diff --git a/WORKSPACE b/WORKSPACE
index 1d27e69..db6dba9 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -247,6 +247,7 @@
     remote = "https://github.com/avventi/Slycot.git",
 )
 
+# TODO(austin): https://github.com/wpilibsuite/roborio-toolchain/releases/tag/v2022-1
 http_archive(
     name = "arm_frc_linux_gnueabi_repo",
     build_file = "@//tools/cpp/arm-frc-linux-gnueabi:arm-frc-linux-gnueabi.BUILD",
@@ -324,13 +325,13 @@
 )
 
 # Generated with:
-# git fetch https://github.com/wpilibsuite/ni-libraries master
-# git archive --output=allwpilib_ni-libraries_c826046.tar.gz --format=tar.gz c826046
+# git fetch https://github.com/wpilibsuite/ni-libraries main
+# git archive --output=allwpilib_ni-libraries_987ab82a80232691d8e21c57c9f13d35d2c31f24.tar.gz --format=tar.gz 987ab82a80232691d8e21c57c9f13d35d2c31f24
 http_archive(
     name = "allwpilib_ni_libraries",
     build_file = "@//debian:ni-libraries.BUILD",
-    sha256 = "59e971854d689b8e60d2b7ede3cc1da911dbc70deeccb9b5306bb7c7aa5102d9",
-    url = "https://www.frc971.org/Build-Dependencies/allwpilib_ni-libraries_c826046.tar.gz",
+    sha256 = "f57dfc76ee3d8680aba8cfa6077e20cd47a2e5b2badd918ec3095d2c6300588f",
+    url = "https://www.frc971.org/Build-Dependencies/allwpilib_ni-libraries_987ab82a80232691d8e21c57c9f13d35d2c31f24.tar.gz",
 )
 
 # Downloaded from:
@@ -518,6 +519,7 @@
     path = "third_party/ceres",
 )
 
+# TODO(austin): Update!
 # Downloaded from http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/api-cpp/5.18.1/.
 http_archive(
     name = "ctre_phoenix_api_cpp_headers",
diff --git a/aos/events/logging/log_stats.cc b/aos/events/logging/log_stats.cc
index 130a11f..cf11fa6 100644
--- a/aos/events/logging/log_stats.cc
+++ b/aos/events/logging/log_stats.cc
@@ -142,6 +142,9 @@
   size_t avg_message_size() const {
     return total_message_size_ / total_num_messages_;
   }
+  size_t avg_message_bandwidth() const {
+    return total_message_size_ / SecondsActive();
+  }
 
   aos::realtime_clock::time_point channel_end_time() const {
     return channel_end_time_;
@@ -298,7 +301,9 @@
                     << "hz max";
         }
         std::cout << " " << channel_stats[i].avg_message_size()
-                  << " bytes avg, " << channel_stats[i].max_message_size()
+                  << " bytes avg, "
+                  << channel_stats[i].avg_message_bandwidth()
+                  << " bytes/sec avg, " << channel_stats[i].max_message_size()
                   << " bytes max / " << channel_stats[i].channel()->max_size()
                   << "bytes " << channel_stats[i].Percentile();
         std::cout << std::endl;
diff --git a/aos/events/logging/logfile_utils_test.cc b/aos/events/logging/logfile_utils_test.cc
index e1e2ebd..9be70c6 100644
--- a/aos/events/logging/logfile_utils_test.cc
+++ b/aos/events/logging/logfile_utils_test.cc
@@ -479,6 +479,7 @@
     ++queue_index_[channel_index];
 
     flatbuffers::FlatBufferBuilder fbb;
+    fbb.ForceDefaults(true);
     fbb.FinishSizePrefixed(
         PackMessage(&fbb, context, channel_index, LogType::kLogMessage));
 
diff --git a/debian/ni-libraries.BUILD b/debian/ni-libraries.BUILD
index 78252a6..571f80f 100644
--- a/debian/ni-libraries.BUILD
+++ b/debian/ni-libraries.BUILD
@@ -1,16 +1,29 @@
+cc_binary(
+    name = "libNiFpgaLv.so.13",
+    srcs = [
+        "src/shims/fpgalv/main.c",
+    ],
+    linkshared = True,
+    linkstatic = False,
+)
+
+cc_binary(
+    name = "libnirio_emb_can.so.21",
+    srcs = [
+        "src/shims/embcan/main.c",
+    ],
+    linkshared = True,
+    linkstatic = False,
+)
+
 cc_library(
     name = "ni-libraries",
     srcs = [
-        "src/lib/chipobject/libRoboRIO_FRC_ChipObject.so.20.0.0",
-        "src/lib/netcomm/libFRC_NetworkCommunication.so.20.0.0",
-        "src/lib/runtime/libNiFpga.so.19.0.0",
-        "src/lib/runtime/libNiFpgaLv.so.19.0.0",
-        "src/lib/runtime/libNiRioSrv.so.19.0.0",
-        "src/lib/runtime/libni_emb.so.12.0.0",
-        "src/lib/runtime/libni_rtlog.so.2.8.0",
-        "src/lib/runtime/libnirio_emb_can.so.16.0.0",
-        "src/lib/runtime/libniriodevenum.so.19.0.0",
-        "src/lib/runtime/libniriosession.so.18.0.0",
+        "libNiFpgaLv.so.13",
+        "libnirio_emb_can.so.21",
+        "src/lib/chipobject/libRoboRIO_FRC_ChipObject.so.22.0.0",
+        "src/lib/netcomm/libFRC_NetworkCommunication.so.22.0.0",
+        "src/lib/visa/libvisa.so.21.0.0",
     ],
     hdrs = glob(["src/include/**"]),
     includes = [
diff --git a/frc971/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
index 0a70e27..f00212f 100755
--- a/frc971/config/setup_roborio.sh
+++ b/frc971/config/setup_roborio.sh
@@ -37,19 +37,11 @@
   ssh "admin@${ROBOT_HOSTNAME}" 'echo "export PATH=\"\${PATH}:/home/admin/robot_code:/home/admin/bin\"" >> /etc/profile'
 fi
 
-if [[ "$(ssh admin@${ROBOT_HOSTNAME} uname -r)" != "4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189" ]]; then
-  echo "Target roboRIO has the wrong kernel"
-  exit 1
-fi
-
 ssh "admin@${ROBOT_HOSTNAME}" "sed -i 's/vm\.overcommit_memory=2/vm\.overcommit_memory=0/' /etc/sysctl.conf"
 
 ssh "admin@${ROBOT_HOSTNAME}" 'echo "vm.min_free_kbytes=4000" >> /etc/sysctl.conf'
 
-ssh "admin@${ROBOT_HOSTNAME}" mkdir "/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/ -p"
-scp frc971/config/sctp.ko "admin@${ROBOT_HOSTNAME}:/lib/modules/4.14.87-rt49-cg-7.0.0f0-xilinx-zynq-189/kernel/net/sctp/sctp.ko"
-ssh "admin@${ROBOT_HOSTNAME}" depmod
-ssh "admin@${ROBOT_HOSTNAME}" sed -i -e 's/^StartupDLLs/;StartupDLLs/' /etc/natinst/share/ni-rt.ini
+ssh "admin@${ROBOT_HOSTNAME}" 'sed -i -e "s/^StartupDLLs/;StartupDLLs/" /etc/natinst/share/ni-rt.ini'
 
 # This fails if the code isn't running.
 ssh "admin@${ROBOT_HOSTNAME}" 'PATH="${PATH}":/usr/local/natinst/bin/ /usr/local/frc/bin/frcKillRobot.sh -r -t' || true
diff --git a/frc971/wpilib/ahal/AnalogInput.cc b/frc971/wpilib/ahal/AnalogInput.cc
index 1694708..8d814ad 100644
--- a/frc971/wpilib/ahal/AnalogInput.cc
+++ b/frc971/wpilib/ahal/AnalogInput.cc
@@ -35,7 +35,7 @@
 
   HAL_PortHandle port = HAL_GetPort(channel);
   int32_t status = 0;
-  m_port = HAL_InitializeAnalogInputPort(port, &status);
+  m_port = HAL_InitializeAnalogInputPort(port, nullptr, &status);
   if (status != 0) {
     wpi_setErrorWithContextRange(status, 0, HAL_GetNumAnalogInputs(), channel,
                                  HAL_GetErrorMessage(status));
diff --git a/frc971/wpilib/ahal/BUILD b/frc971/wpilib/ahal/BUILD
index 510a072..1d02b30 100644
--- a/frc971/wpilib/ahal/BUILD
+++ b/frc971/wpilib/ahal/BUILD
@@ -21,5 +21,6 @@
         "//aos:realtime",
         "//aos/logging",
         "//third_party:wpilib_hal",
+        "@com_google_absl//absl/types:span",
     ],
 )
diff --git a/frc971/wpilib/ahal/Compressor.cc b/frc971/wpilib/ahal/Compressor.cc
index 4a17f76..ba04e52 100644
--- a/frc971/wpilib/ahal/Compressor.cc
+++ b/frc971/wpilib/ahal/Compressor.cc
@@ -6,12 +6,11 @@
 /*----------------------------------------------------------------------------*/
 
 #include "frc971/wpilib/ahal/Compressor.h"
-#include "hal/Compressor.h"
 
+#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "hal/CTREPCM.h"
 #include "hal/HAL.h"
 #include "hal/Ports.h"
-#include "hal/Solenoid.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
 
 using namespace frc;
 
@@ -22,7 +21,7 @@
  */
 Compressor::Compressor(int pcmID) : m_module(pcmID) {
   int32_t status = 0;
-  m_compressorHandle = HAL_InitializeCompressor(m_module, &status);
+  m_compressorHandle = HAL_InitializeCTREPCM(m_module, nullptr, &status);
   if (status != 0) {
     wpi_setErrorWithContextRange(status, 0, HAL_GetNumPCMModules(), pcmID,
                                  HAL_GetErrorMessage(status));
@@ -59,7 +58,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressor(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressor(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -78,7 +77,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorPressureSwitch(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMPressureSwitch(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -97,7 +96,7 @@
   int32_t status = 0;
   double value;
 
-  value = HAL_GetCompressorCurrent(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressorCurrent(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -117,7 +116,7 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
 
-  HAL_SetCompressorClosedLoopControl(m_compressorHandle, on, &status);
+  HAL_SetCTREPCMClosedLoopControl(m_compressorHandle, on, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -136,7 +135,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorClosedLoopControl(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMClosedLoopControl(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -156,7 +155,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorCurrentTooHighFault(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -181,7 +180,7 @@
   bool value;
 
   value =
-      HAL_GetCompressorCurrentTooHighStickyFault(m_compressorHandle, &status);
+      HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -205,7 +204,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorShortedStickyFault(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressorShortedStickyFault(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -225,7 +224,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorShortedFault(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressorShortedFault(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -248,7 +247,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorNotConnectedStickyFault(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -268,7 +267,7 @@
   int32_t status = 0;
   bool value;
 
-  value = HAL_GetCompressorNotConnectedFault(m_compressorHandle, &status);
+  value = HAL_GetCTREPCMCompressorNotConnectedFault(m_compressorHandle, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
@@ -291,7 +290,7 @@
   if (StatusIsFatal()) return;
   int32_t status = 0;
 
-  HAL_ClearAllPCMStickyFaults(m_module, &status);
+  HAL_ClearAllCTREPCMStickyFaults(m_module, &status);
 
   if (status) {
     wpi_setWPIError(Timeout);
diff --git a/frc971/wpilib/ahal/DigitalGlitchFilter.cc b/frc971/wpilib/ahal/DigitalGlitchFilter.cc
index cddfcf0..83e6201 100644
--- a/frc971/wpilib/ahal/DigitalGlitchFilter.cc
+++ b/frc971/wpilib/ahal/DigitalGlitchFilter.cc
@@ -10,13 +10,13 @@
 #include <algorithm>
 #include <array>
 
+#include "frc971/wpilib/ahal/Counter.h"
+#include "frc971/wpilib/ahal/Encoder.h"
+#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "glog/logging.h"
 #include "hal/Constants.h"
 #include "hal/DIO.h"
 #include "hal/HAL.h"
-#include "frc971/wpilib/ahal/Counter.h"
-#include "frc971/wpilib/ahal/Encoder.h"
-#include "frc971/wpilib/ahal/Utility.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
 
 using namespace frc;
 
@@ -26,7 +26,7 @@
 DigitalGlitchFilter::DigitalGlitchFilter() {
   auto index =
       std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false);
-  wpi_assert(index != m_filterAllocated.end());
+  CHECK_NE(index, m_filterAllocated.end());
 
   m_channelIndex = std::distance(m_filterAllocated.begin(), index);
   *index = true;
@@ -67,7 +67,7 @@
     // Validate that we set it correctly.
     int actual_index =
         HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
-    wpi_assertEqual(actual_index, requested_index);
+    CHECK_EQ(actual_index, requested_index);
 
     HAL_Report(HALUsageReporting::kResourceType_DigitalInput,
                input->GetChannel());
diff --git a/frc971/wpilib/ahal/DigitalInput.cc b/frc971/wpilib/ahal/DigitalInput.cc
index e0121ea..3e07208 100644
--- a/frc971/wpilib/ahal/DigitalInput.cc
+++ b/frc971/wpilib/ahal/DigitalInput.cc
@@ -36,7 +36,8 @@
   m_channel = channel;
 
   int32_t status = 0;
-  m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true, &status);
+  m_handle =
+      HAL_InitializeDIOPort(HAL_GetPort(channel), true, nullptr, &status);
   if (status != 0) {
     wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
                                  channel, HAL_GetErrorMessage(status));
@@ -54,9 +55,7 @@
 DigitalInput::~DigitalInput() {
   if (StatusIsFatal()) return;
   if (m_interrupt != HAL_kInvalidHandle) {
-    int32_t status = 0;
-    HAL_CleanInterrupts(m_interrupt, &status);
-    // ignore status, as an invalid handle just needs to be ignored.
+    HAL_CleanInterrupts(m_interrupt);
     m_interrupt = HAL_kInvalidHandle;
   }
 
diff --git a/frc971/wpilib/ahal/DigitalOutput.cc b/frc971/wpilib/ahal/DigitalOutput.cc
index f4b2781..cc1f3d2 100644
--- a/frc971/wpilib/ahal/DigitalOutput.cc
+++ b/frc971/wpilib/ahal/DigitalOutput.cc
@@ -38,7 +38,8 @@
   m_channel = channel;
 
   int32_t status = 0;
-  m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false, &status);
+  m_handle =
+      HAL_InitializeDIOPort(HAL_GetPort(channel), false, nullptr, &status);
   if (status != 0) {
     wpi_setErrorWithContextRange(status, 0, HAL_GetNumDigitalChannels(),
                                  channel, HAL_GetErrorMessage(status));
diff --git a/frc971/wpilib/ahal/DriverStation.cc b/frc971/wpilib/ahal/DriverStation.cc
index 9ea1f7a..2ed0a97 100644
--- a/frc971/wpilib/ahal/DriverStation.cc
+++ b/frc971/wpilib/ahal/DriverStation.cc
@@ -9,10 +9,11 @@
 
 #include <chrono>
 #include <memory>
+#include <string_view>
+#include <functional>
 
 #include "FRC_NetworkCommunication/FRCComm.h"
 #include "frc971/wpilib/ahal/AnalogInput.h"
-#include "frc971/wpilib/ahal/Utility.h"
 #include "frc971/wpilib/ahal/WPIErrors.h"
 #include "hal/HAL.h"
 #include "hal/Power.h"
@@ -43,10 +44,8 @@
  *
  * The error is also printed to the program console.
  */
-void DriverStation::ReportError(const wpi::Twine &error) {
-  wpi::SmallString<128> temp;
-  HAL_SendError(1, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
-                1);
+void DriverStation::ReportError(const std::string_view &error) {
+  HAL_SendError(1, 1, 0, std::string(error).c_str(), "", "", 1);
 }
 
 /**
@@ -54,10 +53,8 @@
  *
  * The warning is also printed to the program console.
  */
-void DriverStation::ReportWarning(const wpi::Twine &error) {
-  wpi::SmallString<128> temp;
-  HAL_SendError(0, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
-                1);
+void DriverStation::ReportWarning(const std::string_view &error) {
+  HAL_SendError(0, 1, 0, std::string(error).c_str(), "", "", 1);
 }
 
 /**
@@ -66,16 +63,11 @@
  * The error is also printed to the program console.
  */
 void DriverStation::ReportError(bool is_error, int32_t code,
-                                const wpi::Twine &error,
-                                const wpi::Twine &location,
-                                const wpi::Twine &stack) {
-  wpi::SmallString<128> errorTemp;
-  wpi::SmallString<128> locationTemp;
-  wpi::SmallString<128> stackTemp;
-  HAL_SendError(is_error, code, 0,
-                error.toNullTerminatedStringRef(errorTemp).data(),
-                location.toNullTerminatedStringRef(locationTemp).data(),
-                stack.toNullTerminatedStringRef(stackTemp).data(), 1);
+                                const std::string_view &error,
+                                const std::string_view &location,
+                                const std::string_view &stack) {
+  HAL_SendError(is_error, code, 0, std::string(error).c_str(),
+                std::string(location).c_str(), std::string(stack).c_str(), 1);
 }
 
 /**
diff --git a/frc971/wpilib/ahal/DriverStation.h b/frc971/wpilib/ahal/DriverStation.h
index 2fbac9c..5150360 100644
--- a/frc971/wpilib/ahal/DriverStation.h
+++ b/frc971/wpilib/ahal/DriverStation.h
@@ -10,12 +10,13 @@
 #include <atomic>
 // #include <condition_variable>
 #include <memory>
+#include <functional>
 #include <string>
+#include <string_view>
 #include <thread>
 
 #include "hal/DriverStation.h"
 #include "frc971/wpilib/ahal/SensorBase.h"
-#include "wpi/Twine.h"
 
 namespace frc {
 
@@ -29,10 +30,10 @@
 
   virtual ~DriverStation();
   static DriverStation &GetInstance();
-  static void ReportError(const wpi::Twine &error);
-  static void ReportWarning(const wpi::Twine &error);
-  static void ReportError(bool is_error, int code, const wpi::Twine &error,
-                          const wpi::Twine &location, const wpi::Twine &stack);
+  static void ReportError(const std::string_view &error);
+  static void ReportWarning(const std::string_view &error);
+  static void ReportError(bool is_error, int code, const std::string_view &error,
+                          const std::string_view &location, const std::string_view &stack);
 
   static const int kJoystickPorts = 6;
 
diff --git a/frc971/wpilib/ahal/ErrorBase.h b/frc971/wpilib/ahal/ErrorBase.h
index c92ca52..f0dfe3e 100644
--- a/frc971/wpilib/ahal/ErrorBase.h
+++ b/frc971/wpilib/ahal/ErrorBase.h
@@ -9,8 +9,6 @@
 
 #include <vector>
 
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
 #include <wpi/mutex.h>
 
 // Forward declared manually to avoid needing to pull in entire HAL header.
diff --git a/frc971/wpilib/ahal/InterruptableSensorBase.cc b/frc971/wpilib/ahal/InterruptableSensorBase.cc
index d17f2dd..59eb962 100644
--- a/frc971/wpilib/ahal/InterruptableSensorBase.cc
+++ b/frc971/wpilib/ahal/InterruptableSensorBase.cc
@@ -7,9 +7,9 @@
 
 #include "frc971/wpilib/ahal/InterruptableSensorBase.h"
 
-#include "hal/HAL.h"
-#include "frc971/wpilib/ahal/Utility.h"
 #include "frc971/wpilib/ahal/WPIErrors.h"
+#include "glog/logging.h"
+#include "hal/HAL.h"
 
 using namespace frc;
 
@@ -18,8 +18,8 @@
 void InterruptableSensorBase::RequestInterrupts() {
   if (StatusIsFatal()) return;
 
-  wpi_assert(m_interrupt == HAL_kInvalidHandle);
-  AllocateInterrupts(true);
+  CHECK_EQ(m_interrupt, HAL_kInvalidHandle);
+  AllocateInterrupts();
   if (StatusIsFatal()) return;  // if allocate failed, out of interrupts
 
   int32_t status = 0;
@@ -31,19 +31,18 @@
   SetUpSourceEdge(true, false);
 }
 
-void InterruptableSensorBase::AllocateInterrupts(bool watcher) {
-  wpi_assert(m_interrupt == HAL_kInvalidHandle);
+void InterruptableSensorBase::AllocateInterrupts() {
+  CHECK_EQ(m_interrupt, HAL_kInvalidHandle);
   // Expects the calling leaf class to allocate an interrupt index.
   int32_t status = 0;
-  m_interrupt = HAL_InitializeInterrupts(watcher, &status);
+  m_interrupt = HAL_InitializeInterrupts(&status);
   wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
 }
 
 void InterruptableSensorBase::CancelInterrupts() {
   if (StatusIsFatal()) return;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  HAL_CleanInterrupts(m_interrupt, &status);
+  CHECK_NE(m_interrupt, HAL_kInvalidHandle);
+  HAL_CleanInterrupts(m_interrupt);
   // ignore status, as an invalid handle just needs to be ignored.
   m_interrupt = HAL_kInvalidHandle;
 }
@@ -51,7 +50,7 @@
 InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(
     double timeout, bool ignorePrevious) {
   if (StatusIsFatal()) return InterruptableSensorBase::kTimeout;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
+  CHECK_NE(m_interrupt, HAL_kInvalidHandle);
   int32_t status = 0;
   int result;
 
@@ -67,17 +66,9 @@
   return static_cast<WaitResult>(falling | rising);
 }
 
-void InterruptableSensorBase::EnableInterrupts() {
-  if (StatusIsFatal()) return;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  HAL_EnableInterrupts(m_interrupt, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
-}
-
 hal::fpga_clock::time_point InterruptableSensorBase::ReadRisingTimestamp() {
   if (StatusIsFatal()) return hal::fpga_clock::min_time;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
+  CHECK_NE(m_interrupt, HAL_kInvalidHandle);
   int32_t status = 0;
   uint64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
   timestamp = HAL_ExpandFPGATime(timestamp, &status);
@@ -87,7 +78,7 @@
 
 hal::fpga_clock::time_point InterruptableSensorBase::ReadFallingTimestamp() {
   if (StatusIsFatal()) return hal::fpga_clock::min_time;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
+  CHECK_NE(m_interrupt, HAL_kInvalidHandle);
   int32_t status = 0;
   uint64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
   timestamp = HAL_ExpandFPGATime(timestamp, &status);
diff --git a/frc971/wpilib/ahal/InterruptableSensorBase.h b/frc971/wpilib/ahal/InterruptableSensorBase.h
index 59ddc9e..c52463b 100644
--- a/frc971/wpilib/ahal/InterruptableSensorBase.h
+++ b/frc971/wpilib/ahal/InterruptableSensorBase.h
@@ -46,9 +46,6 @@
   virtual WaitResult WaitForInterrupt(double timeout,
                                       bool ignorePrevious = true);
 
-  // Enables interrupts to occur based on the current configuration.
-  virtual void EnableInterrupts();
-
   // Returns the timestamp for the most recent rising interrupt.
   virtual hal::fpga_clock::time_point ReadRisingTimestamp();
   // Returns the timestamp for the most recent falling interrupt.
@@ -61,7 +58,7 @@
 
  protected:
   HAL_InterruptHandle m_interrupt = HAL_kInvalidHandle;
-  void AllocateInterrupts(bool watcher);
+  void AllocateInterrupts();
 };
 
 }  // namespace frc
diff --git a/frc971/wpilib/ahal/PWM.cc b/frc971/wpilib/ahal/PWM.cc
index 0bf38a3..31b9863 100644
--- a/frc971/wpilib/ahal/PWM.cc
+++ b/frc971/wpilib/ahal/PWM.cc
@@ -10,10 +10,10 @@
 
 #include <sstream>
 
+#include "frc971/wpilib/ahal/WPIErrors.h"
+#include "glog/logging.h"
 #include "hal/HAL.h"
 #include "hal/Ports.h"
-#include "frc971/wpilib/ahal/Utility.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
 
 using namespace frc;
 
@@ -37,7 +37,7 @@
   }
 
   int32_t status = 0;
-  m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), &status);
+  m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), nullptr, &status);
   if (status != 0) {
     //    wpi_setErrorWithContextRange(status, 0, HAL_GetNumPWMChannels(),
     //    channel,
@@ -271,7 +271,7 @@
       HAL_SetPWMPeriodScale(m_handle, 0, &status);  // Don't squelch any outputs
       break;
     default:
-      wpi_assert(false);
+      LOG(FATAL) << "Invalid multiplier " << mult;
   }
 
   HAL_FATAL_ERROR();
diff --git a/frc971/wpilib/ahal/PowerDistributionPanel.cc b/frc971/wpilib/ahal/PowerDistributionPanel.cc
index 8359cdb..39087fb 100644
--- a/frc971/wpilib/ahal/PowerDistributionPanel.cc
+++ b/frc971/wpilib/ahal/PowerDistributionPanel.cc
@@ -10,7 +10,7 @@
 #include <sstream>
 
 #include "hal/HAL.h"
-#include "hal/PDP.h"
+#include "hal/PowerDistribution.h"
 #include "hal/Ports.h"
 #include "frc971/wpilib/ahal/WPIErrors.h"
 
@@ -24,7 +24,9 @@
  */
 PowerDistributionPanel::PowerDistributionPanel(int module) {
   int32_t status = 0;
-  m_handle = HAL_InitializePDP(module, &status);
+  m_handle = HAL_InitializePowerDistribution(
+      module, HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE,
+      nullptr, &status);
   if (status != 0) {
     return;
   }
@@ -38,7 +40,7 @@
 double PowerDistributionPanel::GetVoltage() const {
   int32_t status = 0;
 
-  double voltage = HAL_GetPDPVoltage(m_handle, &status);
+  double voltage = HAL_GetPowerDistributionVoltage(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -55,7 +57,7 @@
 double PowerDistributionPanel::GetTemperature() const {
   int32_t status = 0;
 
-  double temperature = HAL_GetPDPTemperature(m_handle, &status);
+  double temperature = HAL_GetPowerDistributionTemperature(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -72,13 +74,16 @@
 double PowerDistributionPanel::GetCurrent(int channel) const {
   int32_t status = 0;
 
-  if (!CheckPDPChannel(channel)) {
+  if (!CheckPDPChannel(
+          channel,
+          HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE)) {
     std::stringstream buf;
     buf << "PDP Channel " << channel;
     WPI_LIB_FATAL_ERROR(ChannelIndexOutOfRange, buf.str());
   }
 
-  double current = HAL_GetPDPChannelCurrent(m_handle, channel, &status);
+  double current =
+      HAL_GetPowerDistributionChannelCurrent(m_handle, channel, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -95,7 +100,7 @@
 double PowerDistributionPanel::GetTotalCurrent() const {
   int32_t status = 0;
 
-  double current = HAL_GetPDPTotalCurrent(m_handle, &status);
+  double current = HAL_GetPowerDistributionTotalCurrent(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -112,7 +117,7 @@
 double PowerDistributionPanel::GetTotalPower() const {
   int32_t status = 0;
 
-  double power = HAL_GetPDPTotalPower(m_handle, &status);
+  double power = HAL_GetPowerDistributionTotalPower(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -129,7 +134,7 @@
 double PowerDistributionPanel::GetTotalEnergy() const {
   int32_t status = 0;
 
-  double energy = HAL_GetPDPTotalEnergy(m_handle, &status);
+  double energy = HAL_GetPowerDistributionTotalEnergy(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -146,7 +151,7 @@
 void PowerDistributionPanel::ResetTotalEnergy() {
   int32_t status = 0;
 
-  HAL_ResetPDPTotalEnergy(m_handle, &status);
+  HAL_ResetPowerDistributionTotalEnergy(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
@@ -159,7 +164,7 @@
 void PowerDistributionPanel::ClearStickyFaults() {
   int32_t status = 0;
 
-  HAL_ClearPDPStickyFaults(m_handle, &status);
+  HAL_ClearPowerDistributionStickyFaults(m_handle, &status);
 
   if (status) {
     WPI_LIB_FATAL_ERROR(Timeout, "");
diff --git a/frc971/wpilib/ahal/Relay.cc b/frc971/wpilib/ahal/Relay.cc
index e0cd090..51241be 100644
--- a/frc971/wpilib/ahal/Relay.cc
+++ b/frc971/wpilib/ahal/Relay.cc
@@ -38,7 +38,8 @@
 
   if (m_direction == kBothDirections || m_direction == kForwardOnly) {
     int32_t status = 0;
-    m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, &status);
+    m_forwardHandle =
+        HAL_InitializeRelayPort(portHandle, true, nullptr, &status);
     if (status != 0) {
       wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
                                    channel, HAL_GetErrorMessage(status));
@@ -50,7 +51,7 @@
   }
   if (m_direction == kBothDirections || m_direction == kReverseOnly) {
     int32_t status = 0;
-    m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, &status);
+    m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, nullptr, &status);
     if (status != 0) {
       wpi_setErrorWithContextRange(status, 0, HAL_GetNumRelayChannels(),
                                    channel, HAL_GetErrorMessage(status));
diff --git a/frc971/wpilib/ahal/RobotBase.cc b/frc971/wpilib/ahal/RobotBase.cc
index 1ce1d4e..3cc39df 100644
--- a/frc971/wpilib/ahal/RobotBase.cc
+++ b/frc971/wpilib/ahal/RobotBase.cc
@@ -11,7 +11,6 @@
 
 #include "hal/HAL.h"
 #include "frc971/wpilib/ahal/DriverStation.h"
-#include "frc971/wpilib/ahal/Utility.h"
 #include "frc971/wpilib/ahal/WPILibVersion.h"
 
 using namespace frc;
diff --git a/frc971/wpilib/ahal/SPI.cc b/frc971/wpilib/ahal/SPI.cc
index 01b1626..d51b20f 100644
--- a/frc971/wpilib/ahal/SPI.cc
+++ b/frc971/wpilib/ahal/SPI.cc
@@ -14,6 +14,7 @@
 #include <wpi/SmallVector.h>
 #include <wpi/mutex.h>
 
+#include "absl/types/span.h"
 #include "frc971/wpilib/ahal/DigitalSource.h"
 #include "frc971/wpilib/ahal/WPIErrors.h"
 
@@ -117,7 +118,8 @@
   wpi_setHALError(status);
 }
 
-void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
+void SPI::SetAutoTransmitData(absl::Span<const uint8_t> dataToSend,
+                              int zeroSize) {
   int32_t status = 0;
   HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
                              zeroSize, &status);
diff --git a/frc971/wpilib/ahal/SPI.h b/frc971/wpilib/ahal/SPI.h
index 1d439b2..6421535 100644
--- a/frc971/wpilib/ahal/SPI.h
+++ b/frc971/wpilib/ahal/SPI.h
@@ -8,8 +8,9 @@
 #pragma once
 
 #include <hal/SPITypes.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/deprecated.h>
+#include <wpi/span.h>
+#include "absl/types/span.h"
 
 #include <cstdint>
 #include <memory>
@@ -168,7 +169,7 @@
    * @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);
+  void SetAutoTransmitData(absl::Span<const uint8_t> dataToSend, int zeroSize);
 
   /**
    * Start running the automatic SPI transfer engine at a periodic rate.
diff --git a/frc971/wpilib/ahal/SensorBase.cc b/frc971/wpilib/ahal/SensorBase.cc
index 0b796f1..111e4c6 100644
--- a/frc971/wpilib/ahal/SensorBase.cc
+++ b/frc971/wpilib/ahal/SensorBase.cc
@@ -8,35 +8,24 @@
 #include "frc971/wpilib/ahal/SensorBase.h"
 
 #include "FRC_NetworkCommunication/LoadOut.h"
+#include "frc971/wpilib/ahal/WPIErrors.h"
 #include "hal/AnalogInput.h"
 #include "hal/AnalogOutput.h"
 #include "hal/DIO.h"
 #include "hal/HAL.h"
-#include "hal/PDP.h"
 #include "hal/PWM.h"
 #include "hal/Ports.h"
+#include "hal/PowerDistribution.h"
 #include "hal/Relay.h"
-#include "hal/Solenoid.h"
-#include "frc971/wpilib/ahal/WPIErrors.h"
 
 namespace frc {
 
 const int kDigitalChannels = HAL_GetNumDigitalChannels();
 const int kAnalogInputs = HAL_GetNumAnalogInputs();
-const int kSolenoidChannels = HAL_GetNumSolenoidChannels();
-const int kSolenoidModules = HAL_GetNumPCMModules();
 const int kPwmChannels = HAL_GetNumPWMChannels();
 const int kRelayChannels = HAL_GetNumRelayHeaders();
-const int kPDPChannels = HAL_GetNumPDPChannels();
-
-/**
- * Check that the solenoid module number is valid.
- *
- * @return Solenoid module is valid and present
- */
-bool CheckSolenoidModule(int moduleNumber) {
-  return HAL_CheckSolenoidModule(moduleNumber);
-}
+const int kCTREPDPChannels = HAL_GetNumCTREPDPChannels();
+const int kREVPDPChannels = HAL_GetNumREVPDHChannels();
 
 /**
  * Check that the digital channel number is valid.
@@ -93,19 +82,12 @@
 }
 
 /**
- * Verify that the solenoid channel number is within limits.
- *
- * @return Solenoid channel is valid
- */
-bool CheckSolenoidChannel(int channel) {
-  return HAL_CheckSolenoidChannel(channel);
-}
-
-/**
  * Verify that the power distribution channel number is within limits.
  *
  * @return PDP channel is valid
  */
-bool CheckPDPChannel(int channel) { return HAL_CheckPDPModule(channel); }
+bool CheckPDPChannel(int channel, HAL_PowerDistributionType type) {
+  return HAL_CheckPowerDistributionModule(channel, type);
+}
 
 }  // namespace frc
diff --git a/frc971/wpilib/ahal/SensorBase.h b/frc971/wpilib/ahal/SensorBase.h
index a4de9cc..39c5130 100644
--- a/frc971/wpilib/ahal/SensorBase.h
+++ b/frc971/wpilib/ahal/SensorBase.h
@@ -10,19 +10,18 @@
 #include <cstdint>
 
 #include "frc971/wpilib/ahal/ErrorBase.h"
+#include "hal/PowerDistribution.h"
 
 namespace frc {
 
 inline int GetDefaultSolenoidModule() { return 0; }
 
-bool CheckSolenoidModule(int moduleNumber);
 bool CheckDigitalChannel(int channel);
 bool CheckRelayChannel(int channel);
 bool CheckPWMChannel(int channel);
 bool CheckAnalogInputChannel(int channel);
 bool CheckAnalogOutputChannel(int channel);
-bool CheckSolenoidChannel(int channel);
-bool CheckPDPChannel(int channel);
+bool CheckPDPChannel(int channel, HAL_PowerDistributionType type);
 
 extern const int kDigitalChannels;
 extern const int kAnalogInputs;
diff --git a/frc971/wpilib/ahal/Utility.cc b/frc971/wpilib/ahal/Utility.cc
index f4b9f38..14ed1c5 100644
--- a/frc971/wpilib/ahal/Utility.cc
+++ b/frc971/wpilib/ahal/Utility.cc
@@ -1,146 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) FIRST 2008-2017. All Rights Reserved.                        */
-/* Open Source Software - may be 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 "frc971/wpilib/ahal/Utility.h"
 
-#include <cxxabi.h>
-#include <execinfo.h>
-
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-#include <sstream>
-
 #include "frc971/wpilib/ahal/ErrorBase.h"
-#include "hal/DriverStation.h"
 #include "hal/HAL.h"
-#include "wpi/Path.h"
-#include "wpi/SmallString.h"
-#include "wpi/raw_ostream.h"
-
-using namespace frc;
-
-/**
- * Assert implementation.
- * This allows breakpoints to be set on an assert.
- * The users don't call this, but instead use the wpi_assert macros in
- * Utility.h.
- */
-bool wpi_assert_impl(bool conditionValue, const wpi::Twine &conditionText,
-                     const wpi::Twine &message, wpi::StringRef fileName,
-                     int lineNumber, wpi::StringRef funcName) {
-  if (!conditionValue) {
-    wpi::SmallString<128> locBuf;
-    wpi::raw_svector_ostream locStream(locBuf);
-    locStream << funcName << " [" << wpi::sys::path::filename(fileName) << ":"
-              << lineNumber << "]";
-
-    wpi::SmallString<128> errorBuf;
-    wpi::raw_svector_ostream errorStream(errorBuf);
-
-    errorStream << "Assertion \"" << conditionText << "\" ";
-
-    if (message.isTriviallyEmpty() ||
-        (message.isSingleStringRef() && message.getSingleStringRef().empty())) {
-      errorStream << "failed.\n";
-    } else {
-      errorStream << "failed: " << message << "\n";
-    }
-
-    std::string stack = GetStackTrace(2);
-
-    // Print the error and send it to the DriverStation
-    HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), stack.c_str(), 1);
-  }
-
-  return conditionValue;
-}
-
-/**
- * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
- * This should not be called directly; it should only be used by
- * wpi_assertEqual_impl and wpi_assertNotEqual_impl.
- */
-void wpi_assertEqual_common_impl(const wpi::Twine &valueA,
-                                 const wpi::Twine &valueB,
-                                 const wpi::Twine &equalityType,
-                                 const wpi::Twine &message,
-                                 wpi::StringRef fileName, int lineNumber,
-                                 wpi::StringRef funcName) {
-  wpi::SmallString<128> locBuf;
-  wpi::raw_svector_ostream locStream(locBuf);
-  locStream << funcName << " [" << wpi::sys::path::filename(fileName) << ":"
-            << lineNumber << "]";
-
-  wpi::SmallString<128> errorBuf;
-  wpi::raw_svector_ostream errorStream(errorBuf);
-
-  errorStream << "Assertion \"" << valueA << " " << equalityType << " "
-              << valueB << "\" ";
-
-  if (message.isTriviallyEmpty() ||
-      (message.isSingleStringRef() && message.getSingleStringRef().empty())) {
-    errorStream << "failed.\n";
-  } else {
-    errorStream << "failed: " << message << "\n";
-  }
-
-  std::string trace = GetStackTrace(3);
-
-  // Print the error and send it to the DriverStation
-  HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), trace.c_str(), 1);
-}
-
-/**
- * Assert equal implementation.
- * This determines whether the two given integers are equal. If not,
- * the value of each is printed along with an optional message string.
- * The users don't call this, but instead use the wpi_assertEqual macros in
- * Utility.h.
- */
-bool wpi_assertEqual_impl(int valueA, int valueB,
-                          const wpi::Twine &valueAString,
-                          const wpi::Twine &valueBString,
-                          const wpi::Twine &message, wpi::StringRef fileName,
-                          int lineNumber, wpi::StringRef funcName) {
-  if (!(valueA == valueB)) {
-    wpi_assertEqual_common_impl(valueAString, valueBString, "==", message,
-                                fileName, lineNumber, funcName);
-  }
-  return valueA == valueB;
-}
-
-/**
- * Assert not equal implementation.
- * This determines whether the two given integers are equal. If so,
- * the value of each is printed along with an optional message string.
- * The users don't call this, but instead use the wpi_assertNotEqual macros in
- * Utility.h.
- */
-bool wpi_assertNotEqual_impl(int valueA, int valueB,
-                             const wpi::Twine &valueAString,
-                             const wpi::Twine &valueBString,
-                             const wpi::Twine &message, wpi::StringRef fileName,
-                             int lineNumber, wpi::StringRef funcName) {
-  if (!(valueA != valueB)) {
-    wpi_assertEqual_common_impl(valueAString, valueBString, "!=", message,
-                                fileName, lineNumber, funcName);
-  }
-  return valueA != valueB;
-}
 
 namespace frc {
 
-/**
- * Return the FPGA Version number.
- *
- * For now, expect this to be competition year.
- * @return FPGA Version number.
- */
 int GetFPGAVersion() {
   int32_t status = 0;
   int version = HAL_GetFPGAVersion(&status);
@@ -148,14 +12,6 @@
   return version;
 }
 
-/**
- * Return the FPGA Revision number.
- * The format of the revision is 3 numbers.
- * The 12 most significant bits are the Major Revision.
- * the next 8 bits are the Minor Revision.
- * The 12 least significant bits are the Build Number.
- * @return FPGA Revision number.
- */
 int64_t GetFPGARevision() {
   int32_t status = 0;
   int64_t revision = HAL_GetFPGARevision(&status);
@@ -163,12 +19,6 @@
   return revision;
 }
 
-/**
- * Read the microsecond-resolution timer on the FPGA.
- *
- * @return The current time in microseconds according to the FPGA (since FPGA
- *         reset).
- */
 uint64_t GetFPGATime() {
   int32_t status = 0;
   uint64_t time = HAL_GetFPGATime(&status);
@@ -176,11 +26,6 @@
   return time;
 }
 
-/**
- * Get the state of the "USER" button on the roboRIO.
- *
- * @return True if the button is currently pressed down
- */
 bool GetUserButton() {
   int32_t status = 0;
 
@@ -190,49 +35,4 @@
   return value;
 }
 
-/**
- * Demangle a C++ symbol, used for printing stack traces.
- */
-static std::string demangle(char const *mangledSymbol) {
-  char buffer[256];
-  size_t length;
-  int32_t status;
-
-  if (std::sscanf(mangledSymbol, "%*[^(]%*[(]%255[^)+]", buffer)) {
-    char *symbol = abi::__cxa_demangle(buffer, nullptr, &length, &status);
-    if (status == 0) {
-      return symbol;
-    } else {
-      // If the symbol couldn't be demangled, it's probably a C function,
-      // so just return it as-is.
-      return buffer;
-    }
-  }
-
-  // If everything else failed, just return the mangled symbol
-  return mangledSymbol;
-}
-
-/**
- * Get a stack trace, ignoring the first "offset" symbols.
- * @param offset The number of symbols at the top of the stack to ignore
- */
-std::string GetStackTrace(int offset) {
-  void *stackTrace[128];
-  int stackSize = backtrace(stackTrace, 128);
-  char **mangledSymbols = backtrace_symbols(stackTrace, stackSize);
-  std::stringstream trace;
-
-  for (int i = offset; i < stackSize; i++) {
-    // Only print recursive functions once in a row.
-    if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
-      trace << "\tat " << demangle(mangledSymbols[i]) << std::endl;
-    }
-  }
-
-  std::free(mangledSymbols);
-
-  return trace.str();
-}
-
 }  // namespace frc
diff --git a/frc971/wpilib/ahal/Utility.h b/frc971/wpilib/ahal/Utility.h
index 196dfce..4962cc9 100644
--- a/frc971/wpilib/ahal/Utility.h
+++ b/frc971/wpilib/ahal/Utility.h
@@ -5,57 +5,30 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-#pragma once
+#ifndef FRC971_WPILIB_AHAL_UTILITY_H_
+#define FRC971_WPILIB_AHAL_UTILITY_H_
 
-/** @file
- * Contains global utility functions
- */
-
-#include <cstdint>
 #include <string>
 
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
-
-#define wpi_assert(condition) \
-  wpi_assert_impl(condition, #condition, "", __FILE__, __LINE__, __FUNCTION__)
-#define wpi_assertWithMessage(condition, message)                     \
-  wpi_assert_impl(condition, #condition, message, __FILE__, __LINE__, \
-                  __FUNCTION__)
-
-#define wpi_assertEqual(a, b) \
-  wpi_assertEqual_impl(a, b, #a, #b, "", __FILE__, __LINE__, __FUNCTION__)
-#define wpi_assertEqualWithMessage(a, b, message) \
-  wpi_assertEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, __FUNCTION__)
-
-#define wpi_assertNotEqual(a, b) \
-  wpi_assertNotEqual_impl(a, b, #a, #b, "", __FILE__, __LINE__, __FUNCTION__)
-#define wpi_assertNotEqualWithMessage(a, b, message)                 \
-  wpi_assertNotEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, \
-                          __FUNCTION__)
-
-bool wpi_assert_impl(bool conditionValue, const wpi::Twine &conditionText,
-                     const wpi::Twine &message, wpi::StringRef fileName,
-                     int lineNumber, wpi::StringRef funcName);
-bool wpi_assertEqual_impl(int valueA, int valueB,
-                          const wpi::Twine &valueAString,
-                          const wpi::Twine &valueBString,
-                          const wpi::Twine &message, wpi::StringRef fileName,
-                          int lineNumber, wpi::StringRef funcName);
-bool wpi_assertNotEqual_impl(int valueA, int valueB,
-                             const wpi::Twine &valueAString,
-                             const wpi::Twine &valueBString,
-                             const wpi::Twine &message, wpi::StringRef fileName,
-                             int lineNumber, wpi::StringRef funcName);
-
-void wpi_suspendOnAssertEnabled(bool enabled);
-
 namespace frc {
 
+// Returns the FPGA Version number.  For now, this is the competition year.
 int GetFPGAVersion();
+
+// Returns the FPGA Revision number.
+// The format of the revision is 3 numbers.
+// The 12 most significant bits are the Major Revision.
+// the next 8 bits are the Minor Revision.
+// The 12 least significant bits are the Build Number.
 int64_t GetFPGARevision();
+
+// Reads the microsecond-resolution timer on the FPGA since reset.
 uint64_t GetFPGATime();
+
+// Gets the state of the "USER" button on the roboRIO, returning true if
+// pressed.
 bool GetUserButton();
-std::string GetStackTrace(int offset);
 
 }  // namespace frc
+
+#endif  // FRC971_WPILIB_AHAL_UTILITY_H_
diff --git a/frc971/wpilib/buffered_pcm.cc b/frc971/wpilib/buffered_pcm.cc
index 7f6cb37..8f5a3d9 100644
--- a/frc971/wpilib/buffered_pcm.cc
+++ b/frc971/wpilib/buffered_pcm.cc
@@ -5,7 +5,7 @@
 #include "aos/logging/logging.h"
 #include "hal/HAL.h"
 #include "hal/Ports.h"
-#include "hal/Solenoid.h"
+#include "hal/CTREPCM.h"
 
 namespace frc971 {
 namespace wpilib {
@@ -13,8 +13,8 @@
 BufferedPcm::BufferedPcm(int module) : module_(module) {
   for (int i = 0; i < 8; ++i) {
     int32_t status = 0;
-    solenoid_handles_[i] =
-        HAL_InitializeSolenoidPort(HAL_GetPortWithModule(module_, i), &status);
+    solenoid_handles_[i] = HAL_InitializeCTREPCM(
+        HAL_GetPortWithModule(module_, i), nullptr, &status);
     if (status != 0) {
       AOS_LOG(FATAL, "Status was %d\n", status);
     }
@@ -36,7 +36,7 @@
 
 int32_t BufferedPcm::GetAll() {
   int32_t status = 0;
-  int32_t result = HAL_GetAllSolenoids(module_, &status);
+  int32_t result = HAL_GetCTREPCMSolenoids(module_, &status);
   if (status != 0) {
     AOS_LOG(ERROR, "Failed to flush, %d\n", status);
     return 0;
@@ -47,7 +47,7 @@
 void BufferedPcm::Flush() {
   AOS_LOG(DEBUG, "sending solenoids 0x%" PRIx8 "\n", values_);
   int32_t status = 0;
-  HAL_SetAllSolenoids(module_, static_cast<int>(values_), &status);
+  HAL_SetCTREPCMSolenoids(module_, 0xff, static_cast<int>(values_), &status);
   if (status != 0) {
     AOS_LOG(ERROR, "Failed to flush, %d\n", status);
   }
diff --git a/frc971/wpilib/dma_edge_counting.h b/frc971/wpilib/dma_edge_counting.h
index b645d01..2d49ecb 100644
--- a/frc971/wpilib/dma_edge_counting.h
+++ b/frc971/wpilib/dma_edge_counting.h
@@ -8,12 +8,12 @@
 #include "aos/macros.h"
 
 #include "aos/containers/sized_array.h"
+#include "frc971/wpilib/ahal/Utility.h"
 #include "frc971/wpilib/dma.h"
 
 #include "frc971/wpilib/ahal/AnalogInput.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
 #include "frc971/wpilib/ahal/Encoder.h"
-#include "frc971/wpilib/ahal/Utility.h"
 #undef ERROR
 
 namespace frc971 {
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 16b0e09..2a40128 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -9,7 +9,6 @@
 #include "aos/util/compiler_memory_barrier.h"
 #include "frc971/wpilib/ahal/DigitalInput.h"
 #include "frc971/wpilib/ahal/DriverStation.h"
-#include "frc971/wpilib/ahal/Utility.h"
 #include "frc971/wpilib/fpga_time_conversion.h"
 #include "frc971/wpilib/wpilib_interface.h"
 #include "hal/PWM.h"
diff --git a/third_party/allwpilib/.clang-format b/third_party/allwpilib/.clang-format
index f5a95f7..2643c1b 100644
--- a/third_party/allwpilib/.clang-format
+++ b/third_party/allwpilib/.clang-format
@@ -14,10 +14,10 @@
 AllowAllParametersOfDeclarationOnNextLine: true
 AllowShortBlocksOnASingleLine: Never
 AllowShortCaseLabelsOnASingleLine: false
-AllowShortFunctionsOnASingleLine: All
+AllowShortFunctionsOnASingleLine: Inline
 AllowShortLambdasOnASingleLine: All
-AllowShortIfStatementsOnASingleLine: WithoutElse
-AllowShortLoopsOnASingleLine: true
+AllowShortIfStatementsOnASingleLine: Never
+AllowShortLoopsOnASingleLine: false
 AlwaysBreakAfterDefinitionReturnType: None
 AlwaysBreakAfterReturnType: None
 AlwaysBreakBeforeMultilineStrings: true
@@ -157,7 +157,7 @@
 SpacesInParentheses: false
 SpacesInSquareBrackets: false
 SpaceBeforeSquareBrackets: false
-Standard:        Auto
+Standard:        c++17
 StatementMacros:
   - Q_UNUSED
   - QT_REQUIRE_VERSION
diff --git a/third_party/allwpilib/.clang-tidy b/third_party/allwpilib/.clang-tidy
new file mode 100644
index 0000000..b68e2ed
--- /dev/null
+++ b/third_party/allwpilib/.clang-tidy
@@ -0,0 +1,75 @@
+Checks:
+  'bugprone-assert-side-effect,
+  bugprone-bool-pointer-implicit-conversion,
+  bugprone-copy-constructor-init,
+  bugprone-dangling-handle,
+  bugprone-dynamic-static-initializers,
+  bugprone-forward-declaration-namespace,
+  bugprone-forwarding-reference-overload,
+  bugprone-inaccurate-erase,
+  bugprone-incorrect-roundings,
+  bugprone-integer-division,
+  bugprone-lambda-function-name,
+  bugprone-misplaced-operator-in-strlen-in-alloc,
+  bugprone-misplaced-widening-cast,
+  bugprone-move-forwarding-reference,
+  bugprone-multiple-statement-macro,
+  bugprone-parent-virtual-call,
+  bugprone-posix-return,
+  bugprone-sizeof-container,
+  bugprone-sizeof-expression,
+  bugprone-spuriously-wake-up-functions,
+  bugprone-string-constructor,
+  bugprone-string-integer-assignment,
+  bugprone-string-literal-with-embedded-nul,
+  bugprone-suspicious-enum-usage,
+  bugprone-suspicious-include,
+  bugprone-suspicious-memset-usage,
+  bugprone-suspicious-missing-comma,
+  bugprone-suspicious-semicolon,
+  bugprone-suspicious-string-compare,
+  bugprone-throw-keyword-missing,
+  bugprone-too-small-loop-variable,
+  bugprone-undefined-memory-manipulation,
+  bugprone-undelegated-constructor,
+  bugprone-unhandled-self-assignment,
+  bugprone-unused-raii,
+  bugprone-virtual-near-miss,
+  cert-dcl58-cpp,
+  cert-err52-cpp,
+  cert-err60-cpp,
+  cert-mem57-cpp,
+  cert-oop57-cpp,
+  cert-oop58-cpp,
+  clang-diagnostic-*,
+  -clang-diagnostic-deprecated-declarations,
+  -clang-diagnostic-#warnings,
+  -clang-diagnostic-pedantic,
+  clang-analyzer-*,
+  cppcoreguidelines-slicing,
+  google-build-namespaces,
+  google-explicit-constructor,
+  google-global-names-in-headers,
+  google-readability-avoid-underscore-in-googletest-name,
+  google-readability-casting,
+  google-runtime-operator,
+  llvm-twine-local,
+  misc-definitions-in-headers,
+  misc-misplaced-const,
+  misc-new-delete-overloads,
+  misc-non-copyable-objects,
+  modernize-avoid-bind,
+  modernize-concat-nested-namespaces,
+  modernize-make-shared,
+  modernize-make-unique,
+  modernize-pass-by-value,
+  modernize-use-default-member-init,
+  modernize-use-noexcept,
+  modernize-use-nullptr,
+  modernize-use-override,
+  modernize-use-using,
+  readability-braces-around-statements'
+FormatStyle: file
+CheckOptions:
+  - key: bugprone-dangling-handle
+    value: 'wpi::StringRef;wpi::Twine'
diff --git a/third_party/allwpilib/.gitattributes b/third_party/allwpilib/.gitattributes
new file mode 100644
index 0000000..d8c3d83
--- /dev/null
+++ b/third_party/allwpilib/.gitattributes
@@ -0,0 +1,4 @@
+*.gradle text eol=lf
+*.java text eol=lf
+*.md text eol=lf
+*.xml text eol=lf
diff --git a/third_party/allwpilib/.github/ISSUE_TEMPLATE/bug_report.md b/third_party/allwpilib/.github/ISSUE_TEMPLATE/bug_report.md
index 76b7301..bc502f2 100644
--- a/third_party/allwpilib/.github/ISSUE_TEMPLATE/bug_report.md
+++ b/third_party/allwpilib/.github/ISSUE_TEMPLATE/bug_report.md
@@ -22,7 +22,8 @@
 If applicable, add screenshots to help explain your problem.
 
 **Desktop (please complete the following information):**
- - OS: [e.g. Windows]
+ - WPILib Version: [e.g. 2021.3.1]
+ - OS: [e.g. Windows 11]
  - Java version [e.g. 1.10.2]
  - C++ version [e.g. 17]
 
diff --git a/third_party/allwpilib/.github/workflows/ci.yml b/third_party/allwpilib/.github/workflows/ci.yml
deleted file mode 100644
index 3a8022b..0000000
--- a/third_party/allwpilib/.github/workflows/ci.yml
+++ /dev/null
@@ -1,238 +0,0 @@
-name: CI
-
-on: [pull_request, push]
-
-jobs:
-  build-docker:
-    strategy:
-      fail-fast: false
-      matrix:
-        include:
-          - container: wpilib/roborio-cross-ubuntu:2021-18.04
-            artifact-name: Athena
-            build-options: "-Ponlylinuxathena"
-          - container: wpilib/raspbian-cross-ubuntu:10-18.04
-            artifact-name: Raspbian
-            build-options: "-Ponlylinuxraspbian"
-          - container: wpilib/aarch64-cross-ubuntu:bionic-18.04
-            artifact-name: Aarch64
-            build-options: "-Ponlylinuxaarch64bionic"
-          - container: wpilib/ubuntu-base:18.04
-            artifact-name: Linux
-            build-options: "-Dorg.gradle.jvmargs=-Xmx2g"
-    name: "Build - ${{ matrix.artifact-name }}"
-    runs-on: ubuntu-latest
-    container: ${{ matrix.container }}
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          fetch-depth: 0
-      - name: Set release environment variable
-        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
-        if: startsWith(github.ref, 'refs/tags/v')
-      - name: Build with Gradle
-        run: ./gradlew build -PbuildServer ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
-      - uses: actions/upload-artifact@v2
-        with:
-          name: ${{ matrix.artifact-name }}
-          path: build/allOutputs
-
-  build-host:
-    strategy:
-      fail-fast: false
-      matrix:
-        include:
-          - os: windows-latest
-            artifact-name: Win64
-            architecture: x64
-          - os: windows-latest
-            artifact-name: Win32
-            architecture: x86
-          - os: macos-latest
-            artifact-name: macOS
-            architecture: x64
-    name: "Build - ${{ matrix.artifact-name }}"
-    runs-on: ${{ matrix.os }}
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          fetch-depth: 0
-      - uses: actions/setup-java@v1
-        with:
-          java-version: 11
-          architecture: ${{ matrix.architecture }}
-      - name: Import Developer ID Certificate
-        uses: wpilibsuite/import-signing-certificate@v1
-        with:
-          certificate-data: ${{ secrets.APPLE_CERTIFICATE_DATA }}
-          certificate-passphrase: ${{ secrets.APPLE_CERTIFICATE_PASSWORD }}
-          keychain-password: ${{ secrets.APPLE_KEYCHAIN_PASSWORD }}
-        if: |
-          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
-          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
-      - name: Set Keychain Lock Timeout
-        run: security set-keychain-settings -lut 3600
-        if: |
-          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
-          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
-      - name: Set release environment variable
-        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
-        shell: bash
-        if: startsWith(github.ref, 'refs/tags/v')
-      - name: Build with Gradle
-        run: ./gradlew build -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
-      - name: Sign Libraries with Developer ID
-        run: ./gradlew build -PbuildServer -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ env.EXTRA_GRADLE_ARGS }}
-        if: |
-          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
-          (github.ref == 'refs/heads/master' || startsWith(github.ref, 'refs/tags/v')))
-      - uses: actions/upload-artifact@v2
-        with:
-          name: ${{ matrix.artifact-name }}
-          path: build/allOutputs
-
-  build-documentation:
-    name: "Build - Documentation"
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          fetch-depth: 0
-      - uses: actions/setup-java@v1
-        with:
-          java-version: 13
-      - name: Set release environment variable
-        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
-        if: startsWith(github.ref, 'refs/tags/v')
-      - name: Build with Gradle
-        run: ./gradlew docs:zipDocs -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
-      - uses: actions/upload-artifact@v2
-        with:
-          name: Documentation
-          path: docs/build/outputs
-
-  build-cmake:
-    strategy:
-      fail-fast: false
-      matrix:
-        include:
-          - os: ubuntu-latest
-            name: Linux
-            container: wpilib/roborio-cross-ubuntu:2020-18.04
-            flags: ""
-          - os: macos-latest
-            name: macOS
-            container: ""
-            flags: "-DWITH_JAVA=OFF"
-    name: "Build - CMake ${{ matrix.name }}"
-    runs-on: ${{ matrix.os }}
-    container: ${{ matrix.container }}
-    steps:
-      - uses: actions/checkout@v2
-      - name: Install Dependencies
-        run: |
-          if [ "$RUNNER_OS" == "macOS" ]; then
-            brew install opencv
-          fi
-      - name: configure
-        run: mkdir build && cd build && cmake -DWITH_GUI=OFF ${{ matrix.flags }} ..
-      - name: build
-        working-directory: build
-        run: make -j3
-      - name: test
-        working-directory: build
-        run: make test
-
-  build-cmake-vcpkg:
-    name: "Build - CMake Windows"
-    runs-on: windows-latest
-    steps:
-      - uses: actions/checkout@v2
-      - name: Prepare vcpkg
-        uses: lukka/run-vcpkg@v4
-        with:
-          vcpkgArguments: opencv
-          vcpkgDirectory: ${{ runner.workspace }}/vcpkg/
-          vcpkgGitCommitId: 544f8e4593764f78faa94bac2adb81cca5232943
-          vcpkgTriplet: x64-windows
-      - name: Configure & Build
-        uses: lukka/run-cmake@v3
-        with:
-          buildDirectory: ${{ runner.workspace }}/build/
-          cmakeAppendedArgs: -DWITH_JAVA=OFF -DWITH_GUI=OFF
-          cmakeListsOrSettingsJson: CMakeListsTxtAdvanced
-          useVcpkgToolchainFile: true
-      - name: Run Tests
-        run: ctest -C "Debug"
-        working-directory: ${{ runner.workspace }}/build/
-
-  wpiformat:
-    name: "wpiformat"
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-      - name: Fetch all history and metadata
-        run: |
-          git fetch --prune --unshallow
-          git checkout -b pr
-          git branch -f master origin/master
-      - name: Set up Python 3.8
-        uses: actions/setup-python@v2
-        with:
-          python-version: 3.8
-      - name: Install clang-format
-        run: sudo apt-get update -q && sudo apt-get install clang-format-10
-      - name: Install wpiformat
-        run: pip3 install wpiformat
-      - name: Run
-        run: wpiformat -clang 10
-      - name: Check Output
-        run: git --no-pager diff --exit-code HEAD
-
-  combine:
-    name: Combine
-    needs: [build-docker, build-host, build-documentation]
-    runs-on: ubuntu-latest
-    steps:
-      - uses: actions/checkout@v2
-        with:
-          repository: wpilibsuite/build-tools
-      - uses: actions/download-artifact@v2
-        with:
-          path: combiner/products/build/allOutputs
-      - name: Flatten Artifacts
-        run: rsync -a --delete combiner/products/build/allOutputs/*/* combiner/products/build/allOutputs/
-      - name: Check version number exists
-        run: |
-          cat combiner/products/build/allOutputs/version.txt
-          test -s combiner/products/build/allOutputs/version.txt
-      - uses: actions/setup-java@v1
-        with:
-          java-version: 11
-      - name: Combine
-        if: |
-          !startsWith(github.ref, 'refs/tags/v') &&
-          github.ref != 'refs/heads/master'
-        run: cd combiner && ./gradlew publish -Pallwpilib
-      - name: Combine (Master)
-        if: |
-          github.repository_owner == 'wpilibsuite' &&
-          github.ref == 'refs/heads/master'
-        run: cd combiner && ./gradlew publish -Pallwpilib
-        env:
-          RUN_AZURE_ARTIFACTORY_RELEASE: 'TRUE'
-          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
-          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
-      - name: Combine (Release)
-        if: |
-          github.repository_owner == 'wpilibsuite' &&
-          startsWith(github.ref, 'refs/tags/v')
-        run: cd combiner && ./gradlew publish -Pallwpilib -PreleaseRepoPublish
-        env:
-          RUN_AZURE_ARTIFACTORY_RELEASE: 'TRUE'
-          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
-          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
-      - uses: actions/upload-artifact@v2
-        with:
-          name: Maven
-          path: ~/releases
diff --git a/third_party/allwpilib/.github/workflows/cmake.yml b/third_party/allwpilib/.github/workflows/cmake.yml
new file mode 100644
index 0000000..4d36c91
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/cmake.yml
@@ -0,0 +1,65 @@
+name: CMake
+
+on: [pull_request, push]
+
+jobs:
+  build:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - os: ubuntu-latest
+            name: Linux
+            container: wpilib/roborio-cross-ubuntu:2022-20.04
+            flags: ""
+          - os: macos-latest
+            name: macOS
+            container: ""
+            flags: "-DWITH_JAVA=OFF"
+    name: "Build - ${{ matrix.name }}"
+    runs-on: ${{ matrix.os }}
+    container: ${{ matrix.container }}
+    steps:
+      - uses: actions/checkout@v2
+      - name: Install Dependencies
+        run: |
+          if [ "$RUNNER_OS" == "macOS" ]; then
+            brew install opencv
+          fi
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v2
+        with:
+          python-version: 3.8
+      - name: Install jinja
+        run: python -m pip install jinja2
+      - name: configure
+        run: mkdir build && cd build && cmake ${{ matrix.flags }} ..
+      - name: build
+        working-directory: build
+        run: cmake --build . -j$(nproc)
+      - name: test
+        working-directory: build
+        run: ctest --output-on-failure
+
+  build-vcpkg:
+    name: "Build - Windows"
+    runs-on: windows-latest
+    steps:
+      - uses: actions/checkout@v2
+      - name: Prepare vcpkg
+        uses: lukka/run-vcpkg@v7
+        with:
+          vcpkgArguments: opencv
+          vcpkgDirectory: ${{ runner.workspace }}/vcpkg
+          vcpkgTriplet: x64-windows
+          vcpkgGitCommitId: d781bd9ca77ac3dc2f13d88169021d48459c665f # HEAD on 2021-07-25
+      - name: Configure & Build
+        uses: lukka/run-cmake@v3
+        with:
+          buildDirectory: ${{ runner.workspace }}/build
+          cmakeAppendedArgs: -DWITH_JAVA=OFF
+          cmakeListsOrSettingsJson: CMakeListsTxtAdvanced
+          useVcpkgToolchainFile: true
+      - name: Run Tests
+        run: ctest -C "Debug" --output-on-failure
+        working-directory: ${{ runner.workspace }}/build
diff --git a/third_party/allwpilib/.github/workflows/documentation.yml b/third_party/allwpilib/.github/workflows/documentation.yml
new file mode 100644
index 0000000..ab59b0f
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/documentation.yml
@@ -0,0 +1,60 @@
+name: Documentation
+
+on: [push, workflow_dispatch]
+
+env:
+  BASE_PATH: allwpilib/docs
+
+jobs:
+  publish:
+    name: "Documentation - Publish"
+    runs-on: ubuntu-latest
+    if: github.repository_owner == 'wpilibsuite' && (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v'))
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+          persist-credentials: false
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 13
+      - name: Install libclang-9
+        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
+      - name: Set environment variables (Development)
+        run: |
+          echo "TARGET_FOLDER=$BASE_PATH/development" >> $GITHUB_ENV
+        if: github.ref == 'refs/heads/main'
+      - name: Set environment variables (Tag)
+        run: |
+          echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+          echo "TARGET_FOLDER=$BASE_PATH/beta" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Set environment variables (Release)
+        run: |
+          echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+          echo "TARGET_FOLDER=$BASE_PATH/release" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v') && !contains(github.ref, 'alpha') && !contains(github.ref, 'beta')
+      - name: Build with Gradle
+        run: ./gradlew docs:generateJavaDocs docs:doxygen -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
+      - name: Install SSH Client 🔑
+        uses: webfactory/ssh-agent@v0.4.1
+        with:
+          ssh-private-key: ${{ secrets.GH_DEPLOY_KEY }}
+      - name: Deploy Java 🚀
+        uses: JamesIves/github-pages-deploy-action@3.7.1
+        with:
+          SSH: true
+          REPOSITORY_NAME: wpilibsuite/wpilibsuite.github.io
+          BRANCH: main
+          CLEAN: true
+          FOLDER: docs/build/docs/javadoc
+          TARGET_FOLDER: ${{ env.TARGET_FOLDER }}/java
+      - name: Deploy C++ 🚀
+        uses: JamesIves/github-pages-deploy-action@3.7.1
+        with:
+          SSH: true
+          REPOSITORY_NAME: wpilibsuite/wpilibsuite.github.io
+          BRANCH: main
+          CLEAN: true
+          FOLDER: docs/build/docs/doxygen/html
+          TARGET_FOLDER: ${{ env.TARGET_FOLDER }}/cpp
diff --git a/third_party/allwpilib/.github/workflows/gazebo.yml b/third_party/allwpilib/.github/workflows/gazebo.yml
new file mode 100644
index 0000000..53948fb
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/gazebo.yml
@@ -0,0 +1,15 @@
+name: Gazebo
+
+on: [pull_request, push]
+
+jobs:
+  build:
+    name: "Build"
+    runs-on: ubuntu-latest
+    container: wpilib/gazebo-ubuntu:18.04
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - name: Build with Gradle
+        run: ./gradlew simulation:frc_gazebo_plugins:build simulation:halsim_gazebo:build -PbuildServer -PforceGazebo
diff --git a/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml b/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml
index 405a2b3..c16f58f 100644
--- a/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml
+++ b/third_party/allwpilib/.github/workflows/gradle-wrapper-validation.yml
@@ -1,5 +1,5 @@
 name: "Validate Gradle Wrapper"
-on: [push, pull_request]
+on: [pull_request, push]
 
 jobs:
   validation:
diff --git a/third_party/allwpilib/.github/workflows/gradle.yml b/third_party/allwpilib/.github/workflows/gradle.yml
new file mode 100644
index 0000000..d2c2efb
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/gradle.yml
@@ -0,0 +1,164 @@
+name: Gradle
+
+on: [pull_request, push]
+
+jobs:
+  build-docker:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - container: wpilib/roborio-cross-ubuntu:2022-18.04
+            artifact-name: Athena
+            build-options: "-Ponlylinuxathena"
+          - container: wpilib/raspbian-cross-ubuntu:10-18.04
+            artifact-name: Raspbian
+            build-options: "-Ponlylinuxraspbian"
+          - container: wpilib/aarch64-cross-ubuntu:bionic-18.04
+            artifact-name: Aarch64
+            build-options: "-Ponlylinuxaarch64bionic"
+          - container: wpilib/ubuntu-base:18.04
+            artifact-name: Linux
+            build-options: "-Dorg.gradle.jvmargs=-Xmx2g"
+    name: "Build - ${{ matrix.artifact-name }}"
+    runs-on: ubuntu-latest
+    container: ${{ matrix.container }}
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Build with Gradle
+        run: ./gradlew build -PbuildServer -PskipJavaFormat ${{ matrix.build-options }} ${{ env.EXTRA_GRADLE_ARGS }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: ${{ matrix.artifact-name }}
+          path: build/allOutputs
+
+  build-host:
+    env:
+      MACOSX_DEPLOYMENT_TARGET: 10.14
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - os: windows-latest
+            artifact-name: Win64
+            architecture: x64
+          - os: windows-latest
+            artifact-name: Win32
+            architecture: x86
+          - os: macos-latest
+            artifact-name: macOS
+            architecture: x64
+    name: "Build - ${{ matrix.artifact-name }}"
+    runs-on: ${{ matrix.os }}
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 11
+          architecture: ${{ matrix.architecture }}
+      - name: Import Developer ID Certificate
+        uses: wpilibsuite/import-signing-certificate@v1
+        with:
+          certificate-data: ${{ secrets.APPLE_CERTIFICATE_DATA }}
+          certificate-passphrase: ${{ secrets.APPLE_CERTIFICATE_PASSWORD }}
+          keychain-password: ${{ secrets.APPLE_KEYCHAIN_PASSWORD }}
+        if: |
+          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
+      - name: Set Keychain Lock Timeout
+        run: security set-keychain-settings -lut 3600
+        if: |
+          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        shell: bash
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Build with Gradle
+        run: ./gradlew build -PbuildServer -PskipJavaFormat ${{ env.EXTRA_GRADLE_ARGS }}
+      - name: Sign Libraries with Developer ID
+        run: ./gradlew build -PbuildServer -PskipJavaFormat -PdeveloperID=${{ secrets.APPLE_DEVELOPER_ID }} ${{ env.EXTRA_GRADLE_ARGS }}
+        if: |
+          matrix.artifact-name == 'macOS' && (github.repository_owner == 'wpilibsuite' &&
+          (github.ref == 'refs/heads/main' || startsWith(github.ref, 'refs/tags/v')))
+      - uses: actions/upload-artifact@v2
+        with:
+          name: ${{ matrix.artifact-name }}
+          path: build/allOutputs
+
+  build-documentation:
+    name: "Build - Documentation"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 13
+      - name: Install libclang-9
+        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
+      - name: Set release environment variable
+        run: echo "EXTRA_GRADLE_ARGS=-PreleaseMode" >> $GITHUB_ENV
+        if: startsWith(github.ref, 'refs/tags/v')
+      - name: Build with Gradle
+        run: ./gradlew docs:zipDocs -PbuildServer ${{ env.EXTRA_GRADLE_ARGS }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: Documentation
+          path: docs/build/outputs
+
+  combine:
+    name: Combine
+    needs: [build-docker, build-host, build-documentation]
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          repository: wpilibsuite/build-tools
+      - uses: actions/download-artifact@v2
+        with:
+          path: combiner/products/build/allOutputs
+      - name: Flatten Artifacts
+        run: rsync -a --delete combiner/products/build/allOutputs/*/* combiner/products/build/allOutputs/
+      - name: Check version number exists
+        run: |
+          cat combiner/products/build/allOutputs/version.txt
+          test -s combiner/products/build/allOutputs/version.txt
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 11
+      - name: Combine
+        if: |
+          !startsWith(github.ref, 'refs/tags/v') &&
+          github.ref != 'refs/heads/main'
+        run: cd combiner && ./gradlew publish -Pallwpilib
+      - name: Combine (Master)
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          github.ref == 'refs/heads/main'
+        run: cd combiner && ./gradlew publish -Pallwpilib
+        env:
+          RUN_AZURE_ARTIFACTORY_RELEASE: "TRUE"
+          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
+          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
+      - name: Combine (Release)
+        if: |
+          github.repository_owner == 'wpilibsuite' &&
+          startsWith(github.ref, 'refs/tags/v')
+        run: cd combiner && ./gradlew publish -Pallwpilib -PreleaseRepoPublish
+        env:
+          RUN_AZURE_ARTIFACTORY_RELEASE: "TRUE"
+          ARTIFACTORY_PUBLISH_USERNAME: ${{ secrets.ARTIFACTORY_USERNAME }}
+          ARTIFACTORY_PUBLISH_PASSWORD: ${{ secrets.ARTIFACTORY_PASSWORD }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: Maven
+          path: ~/releases
diff --git a/third_party/allwpilib/.github/workflows/lint-format.yml b/third_party/allwpilib/.github/workflows/lint-format.yml
new file mode 100644
index 0000000..6d806f7
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/lint-format.yml
@@ -0,0 +1,100 @@
+name: Lint and Format
+
+on:
+  pull_request:
+  push:
+    branches-ignore:
+      - main
+
+jobs:
+  wpiformat:
+    name: "wpiformat"
+    runs-on: ubuntu-latest
+    container: wpilib/roborio-cross-ubuntu:2022-20.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Fetch all history and metadata
+        run: |
+          git fetch --prune --unshallow
+          git checkout -b pr
+          git branch -f main origin/main
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v2
+        with:
+          python-version: 3.8
+      - name: Install clang-format
+        run: |
+          sudo sh -c "echo 'deb http://archive.ubuntu.com/ubuntu/ $(lsb_release -cs)-proposed restricted main multiverse universe' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo apt-get update -q
+          sudo apt-get install -y clang-format-12
+      - name: Install wpiformat
+        run: pip3 install wpiformat
+      - name: Run
+        run: wpiformat -clang 12
+      - name: Check output
+        run: git --no-pager diff --exit-code HEAD
+      - name: Generate diff
+        run: git diff HEAD > wpiformat-fixes.patch
+        if: ${{ failure() }}
+      - uses: actions/upload-artifact@v2
+        with:
+          name: wpiformat fixes
+          path: wpiformat-fixes.patch
+        if: ${{ failure() }}
+  tidy:
+    name: "clang-tidy"
+    runs-on: ubuntu-latest
+    container: wpilib/roborio-cross-ubuntu:2022-20.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Fetch all history and metadata
+        run: |
+          git fetch --prune --unshallow
+          git checkout -b pr
+          git branch -f main origin/main
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v2
+        with:
+          python-version: 3.8
+      - name: Install clang-tidy
+        run: |
+          sudo sh -c "echo 'deb http://archive.ubuntu.com/ubuntu/ $(lsb_release -cs)-proposed restricted main multiverse universe' >> /etc/apt/sources.list.d/proposed-repositories.list"
+          sudo apt-get update -q
+          sudo apt-get install -y clang-tidy-12 clang-format-12
+      - name: Install wpiformat
+        run: pip3 install wpiformat
+      - name: Create compile_commands.json
+        run: ./gradlew generateCompileCommands -Ptoolchain-optional-roboRio
+      - name: List changed files
+        run: wpiformat -list-changed-files
+      - name: Run clang-tidy
+        run: wpiformat -clang 12 -no-format -tidy-changed -compile-commands=build/compile_commands/linuxx86-64 -vv
+  javaformat:
+    name: "Java format"
+    runs-on: ubuntu-latest
+    container: wpilib/ubuntu-base:18.04
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - name: Run Java format
+        run: ./gradlew javaFormat spotbugsMain spotbugsTest spotbugsDev
+      - name: Check output
+        run: git --no-pager diff --exit-code HEAD
+      - name: Generate diff
+        run: git diff HEAD > javaformat-fixes.patch
+        if: ${{ failure() }}
+  documentation:
+    name: "Documentation"
+    runs-on: ubuntu-latest
+    steps:
+      - uses: actions/checkout@v2
+        with:
+          fetch-depth: 0
+      - uses: actions/setup-java@v1
+        with:
+          java-version: 13
+      - name: Install libclang-9
+        run: sudo apt update && sudo apt install -y libclang-cpp9 libclang1-9
+      - name: Build with Gradle
+        run: ./gradlew docs:zipDocs -PbuildServer -PdocWarningsAsErrors ${{ env.EXTRA_GRADLE_ARGS }}
diff --git a/third_party/allwpilib/.github/workflows/sanitizers.yml b/third_party/allwpilib/.github/workflows/sanitizers.yml
new file mode 100644
index 0000000..8c0e516
--- /dev/null
+++ b/third_party/allwpilib/.github/workflows/sanitizers.yml
@@ -0,0 +1,49 @@
+name: Sanitizers
+
+on: [pull_request, push]
+
+jobs:
+  build:
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - name: asan
+            cmake-flags: "-DCMAKE_BUILD_TYPE=Asan"
+            ctest-env: ""
+            ctest-flags: "-E 'wpiutil|ntcore|wpilibc'"
+          - name: tsan
+            cmake-flags: "-DCMAKE_BUILD_TYPE=Tsan"
+            ctest-env: "TSAN_OPTIONS=second_deadlock_stack=1"
+            ctest-flags: "-E 'ntcore|cscore|cameraserver|wpilibc|wpilibNewCommands'"
+          - name: ubsan
+            cmake-flags: "-DCMAKE_BUILD_TYPE=Ubsan"
+            ctest-env: ""
+            ctest-flags: ""
+    name: "${{ matrix.name }}"
+    runs-on: ubuntu-latest
+    container: wpilib/roborio-cross-ubuntu:2022-20.04
+    steps:
+      - uses: actions/checkout@v2
+      - name: Install Dependencies
+        run: |
+          sudo add-apt-repository ppa:ubuntu-toolchain-r/test
+          sudo apt install -y gcc-11 g++-11
+          sudo update-alternatives \
+            --install /usr/bin/gcc gcc /usr/bin/gcc-11 11 \
+            --slave /usr/bin/g++ g++ /usr/bin/g++-11
+          sudo update-alternatives --set gcc /usr/bin/gcc-11
+      - name: Set up Python 3.8
+        uses: actions/setup-python@v2
+        with:
+          python-version: 3.8
+      - name: Install jinja
+        run: python -m pip install jinja2
+      - name: configure
+        run: mkdir build && cd build && cmake ${{ matrix.cmake-flags }} ..
+      - name: build
+        working-directory: build
+        run: cmake --build . -j$(nproc)
+      - name: test
+        working-directory: build
+        run: ${{ matrix.ctest-env }} ctest --output-on-failure ${{ matrix.ctest-flags }}
diff --git a/third_party/allwpilib/.gitignore b/third_party/allwpilib/.gitignore
index 524d81d..6a6dde7 100644
--- a/third_party/allwpilib/.gitignore
+++ b/third_party/allwpilib/.gitignore
@@ -222,5 +222,12 @@
 # clang configuration and clangd cache
 .clang
 .clangd/
+.cache/
 
 imgui.ini
+
+# Bazel
+/.ijwb/
+/bazel-*
+user.bazelrc
+coverage_report/
diff --git a/third_party/allwpilib/.styleguide b/third_party/allwpilib/.styleguide
index 485ac26..5cbd3e7 100644
--- a/third_party/allwpilib/.styleguide
+++ b/third_party/allwpilib/.styleguide
@@ -9,6 +9,10 @@
   \.cpp$
 }
 
+modifiableFileExclude {
+  \.patch$
+}
+
 generatedFileExclude {
   FRCNetComm\.java$
   simulation/gz_msgs/src/include/simulation/gz_msgs/msgs\.h$
@@ -23,6 +27,7 @@
   ^cameraserver/
   ^cscore
   ^drake/
+  ^fmt/
   ^hal/
   ^imgui
   ^implot
diff --git a/third_party/allwpilib/.styleguide-license b/third_party/allwpilib/.styleguide-license
index b802370..85c04a3 100644
--- a/third_party/allwpilib/.styleguide-license
+++ b/third_party/allwpilib/.styleguide-license
@@ -1,6 +1,3 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) {year} FIRST. All Rights Reserved.{padding}*/
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
diff --git a/third_party/allwpilib/.wpilib/wpilib_preferences.json b/third_party/allwpilib/.wpilib/wpilib_preferences.json
index 39a096c..683ba3b 100644
--- a/third_party/allwpilib/.wpilib/wpilib_preferences.json
+++ b/third_party/allwpilib/.wpilib/wpilib_preferences.json
@@ -1,6 +1,6 @@
 {
   "enableCppIntellisense": true,
   "currentLanguage": "cpp",
-  "projectYear": "2020",
+  "projectYear": "2021",
   "teamNumber": 0
 }
diff --git a/third_party/allwpilib/BUILD b/third_party/allwpilib/BUILD
index bee2e64..6c70541 100644
--- a/third_party/allwpilib/BUILD
+++ b/third_party/allwpilib/BUILD
@@ -65,6 +65,7 @@
     srcs = glob(
         include = [
             "hal/src/main/native/athena/*.cpp",
+            "hal/src/main/native/athena/rev/*.cpp",
             "hal/src/main/native/cpp/cpp/*.cpp",
             "hal/src/main/native/cpp/*.cpp",
             "hal/src/main/native/athena/ctre/*.cpp",
diff --git a/third_party/allwpilib/CMakeLists.txt b/third_party/allwpilib/CMakeLists.txt
index 399421f..e1cafea 100644
--- a/third_party/allwpilib/CMakeLists.txt
+++ b/third_party/allwpilib/CMakeLists.txt
@@ -1,5 +1,5 @@
 # Disable in-source builds to prevent source tree corruption.
-if(" ${CMAKE_SOURCE_DIR}" STREQUAL " ${CMAKE_BINARY_DIR}")
+if(" ${CMAKE_CURRENT_SOURCE_DIR}" STREQUAL " ${CMAKE_CURRENT_BINARY_DIR}")
   message(FATAL_ERROR "
 FATAL: In-source builds are not allowed.
        You should create a separate directory for build files.
@@ -9,17 +9,19 @@
 
 project(allwpilib)
 cmake_minimum_required(VERSION 3.3.0)
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/modules")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake/modules")
+
+set(WPILIB_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR})
 
 INCLUDE(CPack)
 
 set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 set_property(GLOBAL PROPERTY USE_FOLDERS ON)
 
-set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
-set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
-set(CMAKE_JAVA_TARGET_OUTPUT_DIR ${CMAKE_BINARY_DIR}/jar)
+set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${WPILIB_BINARY_DIR}/lib)
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${WPILIB_BINARY_DIR}/lib)
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${WPILIB_BINARY_DIR}/bin)
+set(CMAKE_JAVA_TARGET_OUTPUT_DIR ${WPILIB_BINARY_DIR}/jar)
 
 # use, i.e. don't skip the full RPATH for the build tree
 SET(CMAKE_SKIP_BUILD_RPATH  FALSE)
@@ -44,7 +46,10 @@
 option(BUILD_SHARED_LIBS "Build with shared libs (needed for JNI)" ON)
 option(WITH_JAVA "Include java and JNI in the build" ON)
 option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
-option(WITH_WPILIB "Build hal, wpilibc/j, wpimath, and myRobot (needs OpenCV)" ON)
+option(WITH_WPIMATH "Build wpimath" ON)
+option(WITH_WPILIB "Build hal, wpilibc/j, and myRobot (needs OpenCV)" ON)
+option(WITH_OLD_COMMANDS "Build old commands" OFF)
+option(WITH_EXAMPLES "Build examples" OFF)
 option(WITH_TESTS "Build unit tests (requires internet connection)" ON)
 option(WITH_GUI "Build GUI items" ON)
 option(WITH_SIMULATION_MODULES "Build simulation modules" ON)
@@ -54,6 +59,7 @@
 set(EXTERNAL_HAL_FILE "" CACHE FILEPATH "Location to look for an external HAL CMake File")
 
 # Options for using a package manager (vcpkg) for certain dependencies.
+option(USE_VCPKG_FMTLIB "Use vcpkg fmtlib" OFF)
 option(USE_VCPKG_LIBUV "Use vcpkg libuv" OFF)
 option(USE_VCPKG_EIGEN "Use vcpkg eigen" OFF)
 
@@ -106,6 +112,13 @@
 ")
 endif()
 
+if (NOT WITH_WPIMATH AND WITH_WPILIB)
+    message(FATAL_ERROR "
+FATAL: Cannot build wpilib without wpimath.
+       Enable wpimath by setting WITH_WPIMATH=ON
+")
+endif()
+
 set( wpilib_dest wpilib)
 set( include_dest wpilib/include )
 set( main_lib_dest wpilib/lib )
@@ -134,6 +147,8 @@
 set(HAL_DEP_REPLACE_IMPL "include(\${SELF_DIR}/hal-config.cmake)")
 set(WPIMATH_DEP_REPLACE "include($\{SELF_DIR\}/wpimath-config.cmake)")
 set(WPILIBC_DEP_REPLACE_IMPL "include(\${SELF_DIR}/wpilibc-config.cmake)")
+set(WPILIBNEWCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibNewcommands-config.cmake)")
+set(WPILIBOLDCOMMANDS_DEP_REPLACE "include(\${SELF_DIR}/wpilibOldcommands-config.cmake)")
 else()
 set(WPIUTIL_DEP_REPLACE "find_dependency(wpiutil)")
 set(NTCORE_DEP_REPLACE "find_dependency(ntcore)")
@@ -142,11 +157,82 @@
 set(HAL_DEP_REPLACE_IMPL "find_dependency(hal)")
 set(WPIMATH_DEP_REPLACE "find_dependency(wpimath)")
 set(WPILIBC_DEP_REPLACE_IMPL "find_dependency(wpilibc)")
+set(WPILIBNEWCOMMANDS_DEP_REPLACE "find_dependency(wpilibNewCommands)")
+set(WPILIBOLDCOMMANDS_DEP_REPLACE "find_dependency(wpilibOldCommands)")
 endif()
 
 set(FILENAME_DEP_REPLACE "get_filename_component(SELF_DIR \"$\{CMAKE_CURRENT_LIST_FILE\}\" PATH)")
 set(SELF_DIR "$\{SELF_DIR\}")
 
+get_property(isMultiConfig GLOBAL PROPERTY GENERATOR_IS_MULTI_CONFIG)
+
+if(isMultiConfig)
+    if(NOT "Asan" IN_LIST CMAKE_CONFIGURATION_TYPES)
+        list(APPEND CMAKE_CONFIGURATION_TYPES Asan)
+    endif()
+    if(NOT "Tsan" IN_LIST CMAKE_CONFIGURATION_TYPES)
+        list(APPEND CMAKE_CONFIGURATION_TYPES Tsan)
+    endif()
+    if(NOT "Ubsan" IN_LIST CMAKE_CONFIGURATION_TYPES)
+        list(APPEND CMAKE_CONFIGURATION_TYPES Ubsan)
+    endif()
+else()
+    set(allowedBuildTypes Asan Tsan Ubsan Debug Release RelWithDebInfo MinSizeRel)
+    set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "${allowedBuildTypes}")
+
+    if(CMAKE_BUILD_TYPE AND NOT CMAKE_BUILD_TYPE IN_LIST allowedBuildTypes)
+        message(FATAL_ERROR "Invalid build type: ${CMAKE_BUILD_TYPE}")
+    endif()
+endif()
+
+set(CMAKE_C_FLAGS_ASAN
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer" CACHE STRING
+    "Flags used by the C compiler for Asan build type or configuration." FORCE)
+
+set(CMAKE_CXX_FLAGS_ASAN
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=address -fno-omit-frame-pointer" CACHE STRING
+    "Flags used by the C++ compiler for Asan build type or configuration." FORCE)
+
+set(CMAKE_EXE_LINKER_FLAGS_ASAN
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address" CACHE STRING
+    "Linker flags to be used to create executables for Asan build type." FORCE)
+
+set(CMAKE_SHARED_LINKER_FLAGS_ASAN
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=address" CACHE STRING
+    "Linker lags to be used to create shared libraries for Asan build type." FORCE)
+
+set(CMAKE_C_FLAGS_TSAN
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer" CACHE STRING
+    "Flags used by the C compiler for Tsan build type or configuration." FORCE)
+
+set(CMAKE_CXX_FLAGS_TSAN
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=thread -fno-omit-frame-pointer" CACHE STRING
+    "Flags used by the C++ compiler for Tsan build type or configuration." FORCE)
+
+set(CMAKE_EXE_LINKER_FLAGS_TSAN
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread" CACHE STRING
+    "Linker flags to be used to create executables for Tsan build type." FORCE)
+
+set(CMAKE_SHARED_LINKER_FLAGS_TSAN
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=thread" CACHE STRING
+    "Linker lags to be used to create shared libraries for Tsan build type." FORCE)
+
+set(CMAKE_C_FLAGS_UBSAN
+    "${CMAKE_C_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer" CACHE STRING
+    "Flags used by the C compiler for Ubsan build type or configuration." FORCE)
+
+set(CMAKE_CXX_FLAGS_UBSAN
+    "${CMAKE_CXX_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all -fno-omit-frame-pointer" CACHE STRING
+    "Flags used by the C++ compiler for Ubsan build type or configuration." FORCE)
+
+set(CMAKE_EXE_LINKER_FLAGS_UBSAN
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined -fno-sanitize-recover=all" CACHE STRING
+    "Linker flags to be used to create executables for Ubsan build type." FORCE)
+
+set(CMAKE_SHARED_LINKER_FLAGS_UBSAN
+    "${CMAKE_SHARED_LINKER_FLAGS_DEBUG} -fsanitize=undefined" CACHE STRING
+    "Linker lags to be used to create shared libraries for Ubsan build type." FORCE)
+
 if (WITH_TESTS)
     enable_testing()
     add_subdirectory(googletest)
@@ -156,9 +242,20 @@
 add_subdirectory(wpiutil)
 add_subdirectory(ntcore)
 
+if (WITH_WPIMATH)
+    add_subdirectory(wpimath)
+endif()
+
 if (WITH_GUI)
     add_subdirectory(imgui)
     add_subdirectory(wpigui)
+    add_subdirectory(glass)
+    add_subdirectory(outlineviewer)
+endif()
+
+if (WITH_WPILIB OR WITH_SIMULATION_MODULES)
+    set(HAL_DEP_REPLACE ${HAL_DEP_REPLACE_IMPL})
+    add_subdirectory(hal)
 endif()
 
 if (WITH_CSCORE)
@@ -166,20 +263,25 @@
     set(CAMERASERVER_DEP_REPLACE ${CAMERASERVER_DEP_REPLACE_IMPL})
     add_subdirectory(cscore)
     add_subdirectory(cameraserver)
-    if (WITH_WPILIB)
-        set(HAL_DEP_REPLACE ${HAL_DEP_REPLACE_IMPL})
-        set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
-        add_subdirectory(hal)
-        add_subdirectory(wpimath)
-        add_subdirectory(wpilibj)
-        add_subdirectory(wpilibc)
-        add_subdirectory(myRobot)
+endif()
+
+if (WITH_WPILIB)
+    set(WPILIBC_DEP_REPLACE ${WPILIBC_DEP_REPLACE_IMPL})
+    add_subdirectory(wpilibj)
+    add_subdirectory(wpilibc)
+    add_subdirectory(wpilibNewCommands)
+    if (WITH_OLD_COMMANDS)
+        add_subdirectory(wpilibOldCommands)
     endif()
+    if (WITH_EXAMPLES)
+        add_subdirectory(wpilibcExamples)
+    endif()
+    add_subdirectory(myRobot)
 endif()
 
 if (WITH_SIMULATION_MODULES AND NOT WITH_EXTERNAL_HAL)
     add_subdirectory(simulation)
 endif()
 
-configure_file(wpilib-config.cmake.in ${CMAKE_BINARY_DIR}/wpilib-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/wpilib-config.cmake DESTINATION ${wpilib_config_dir})
+configure_file(wpilib-config.cmake.in ${WPILIB_BINARY_DIR}/wpilib-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/wpilib-config.cmake DESTINATION ${wpilib_config_dir})
diff --git a/third_party/allwpilib/CONTRIBUTING.md b/third_party/allwpilib/CONTRIBUTING.md
index 8409c5e..61f121d 100644
--- a/third_party/allwpilib/CONTRIBUTING.md
+++ b/third_party/allwpilib/CONTRIBUTING.md
@@ -1,6 +1,6 @@
 # Contributing to WPILib
 
-So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/master/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
+So you want to contribute your changes back to WPILib. Great! We have a few contributing rules that will help you make sure your changes will be accepted into the project. Please remember to follow the rules in the [code of conduct](https://github.com/wpilibsuite/allwpilib/blob/main/CODE_OF_CONDUCT.md), and behave with Gracious Professionalism.
 
 - [General Contribution Rules](#general-contribution-rules)
 - [What to Contribute](#what-to-contribute)
@@ -41,11 +41,13 @@
 
 While the library should be fully formatted according to the styles, additional elements of the style guide were not followed when the library was initially created. All new code should follow the guidelines. If you are looking for some easy ramp-up tasks, finding areas that don't follow the style guide and fixing them is very welcome.
 
+When writing math expressions in documentation, use https://www.unicodeit.net/ to convert LaTeX to a Unicode equivalent that's easier to read. Not all expressions will translate (e.g., superscripts of superscripts) so focus on making it readable by someone who isn't familiar with LaTeX. If content on multiple lines needs to be aligned in Doxygen/Javadoc comments (e.g., integration/summation limits, matrices packed with square brackets and superscripts for them), put them in @verbatim/@endverbatim blocks in Doxygen or `<pre>` tags in Javadoc so they render with monospace font.
+
 ## Submitting Changes
 
 ### Pull Request Format
 
-Changes should be submitted as a Pull Request against the master branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current master. We do this to make sure that the git history isn't too cluttered.
+Changes should be submitted as a Pull Request against the main branch of WPILib. For most changes, commits will be squashed upon merge. For particularly large changes, multiple commits are ok, but assume one commit unless asked otherwise. We may ask you to break a PR into multiple standalone PRs or commits for rebase within one PR to separate unrelated changes. No change will be merged unless it is up to date with the current main branch. We do this to make sure that the git history isn't too cluttered.
 
 ### Merge Process
 
diff --git a/third_party/allwpilib/LICENSE.md b/third_party/allwpilib/LICENSE.md
index a8cd035..3d5a824 100644
--- a/third_party/allwpilib/LICENSE.md
+++ b/third_party/allwpilib/LICENSE.md
@@ -1,4 +1,4 @@
-Copyright (c) 2009-2019 FIRST
+Copyright (c) 2009-2021 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -8,12 +8,12 @@
    * Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.
-   * Neither the name of the FIRST nor the
-     names of its contributors may be used to endorse or promote products
-      derived from this software without specific prior written permission.
+   * Neither the name of FIRST, WPILib, nor the names of other WPILib
+     contributors may be used to endorse or promote products derived from
+     this software without specific prior written permission.
 
-THIS SOFTWARE IS PROVIDED BY FIRST AND CONTRIBUTORS "AS IS" AND ANY
-EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+THIS SOFTWARE IS PROVIDED BY FIRST AND OTHER WPILIB CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY NONINFRINGEMENT AND FITNESS FOR A PARTICULAR
 PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL FIRST OR CONTRIBUTORS BE LIABLE FOR
 ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
diff --git a/third_party/allwpilib/MAINTAINERS.md b/third_party/allwpilib/MAINTAINERS.md
index 63a9a57..d4c3490 100644
--- a/third_party/allwpilib/MAINTAINERS.md
+++ b/third_party/allwpilib/MAINTAINERS.md
@@ -1,27 +1,27 @@
-## Publishing Third Party Dependencies
-Currently the 3rd party deps are imgui, opencv, and google test
-
-For publishing these dependencies, the version needs to be manually updated in the publish.gradle file of their respective repository.
-Then, in the azure build for the dependency you want to build for, manually start a pipeline build (As of current, this is the `Run Pipeline` button).
-A variable needs to be added called `RUN_AZURE_ARTIFACTORY_RELEASE`, with a value of `true`. Then when the pipeline gets started, the final build outputs will be updated to artifactory.
-
-To use newer versions of C++ dependencies, in `shared/config.gradle`, update the version related to the specific dependency.
-For Java dependencies, there is likely a file related to the specific dependency in the shared folder. Update the version in there.
-
-Note, changing artifact locations (This includes changing the artifact year currently, I have an issue open to change this) requires updating the `native-utils` plugin
-
-## Publishing allwpilib
-allwpilib publishes to the development repo on every push to master. To publish a release build, upload a new tag, and a release will automatically be built and published.
-
-## Publishing desktop tools
-Desktop tools publish to the development repo on every push to master. To publish a release build, upload a new tag, and a release will automatically be built and published.
-
-## Publishing VS Code
-Before publishing, make sure to update the gradlerio version in `vscode-wpilib/resources/gradle/version.txt` Also make sure the gradle wrapper version matches the wrapper required by gradlerio.
-Upon pushing a tag, a release will be built, and the files will be uploaded to the releases on GitHub. For publishing to the marketplace, you need a Microsoft account and to be added as a maintainer.
-
-## Publishing GradleRIO
-Before publishing, make sure to update the version in build.gradle. Publishing must happen locally, using the command `./gradlew publishPlugin`. This does require your API key for publishing to be set.
-
-## Building the installer
-Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. Then push, it will build the installer on azure.
+## Publishing Third Party Dependencies
+Currently the 3rd party deps are imgui, opencv, and google test
+
+For publishing these dependencies, the version needs to be manually updated in the publish.gradle file of their respective repository.
+Then, in the azure build for the dependency you want to build for, manually start a pipeline build (As of current, this is the `Run Pipeline` button).
+A variable needs to be added called `RUN_AZURE_ARTIFACTORY_RELEASE`, with a value of `true`. Then when the pipeline gets started, the final build outputs will be updated to artifactory.
+
+To use newer versions of C++ dependencies, in `shared/config.gradle`, update the version related to the specific dependency.
+For Java dependencies, there is likely a file related to the specific dependency in the shared folder. Update the version in there.
+
+Note, changing artifact locations (This includes changing the artifact year currently, I have an issue open to change this) requires updating the `native-utils` plugin
+
+## Publishing allwpilib
+allwpilib publishes to the development repo on every push to main. To publish a release build, upload a new tag, and a release will automatically be built and published.
+
+## Publishing desktop tools
+Desktop tools publish to the development repo on every push to main. To publish a release build, upload a new tag, and a release will automatically be built and published.
+
+## Publishing VS Code
+Before publishing, make sure to update the gradlerio version in `vscode-wpilib/resources/gradle/version.txt` Also make sure the gradle wrapper version matches the wrapper required by gradlerio.
+Upon pushing a tag, a release will be built, and the files will be uploaded to the releases on GitHub. For publishing to the marketplace, you need a Microsoft account and to be added as a maintainer.
+
+## Publishing GradleRIO
+Before publishing, make sure to update the version in build.gradle. Publishing must happen locally, using the command `./gradlew publishPlugin`. This does require your API key for publishing to be set.
+
+## Building the installer
+Update the GradleRIO version in gradle.properties, and in the scripts folder in vscode, update the vscode extension. Then push, it will build the installer on azure.
diff --git a/third_party/allwpilib/MavenArtifacts.md b/third_party/allwpilib/MavenArtifacts.md
index e023abe..be7d1a2 100644
--- a/third_party/allwpilib/MavenArtifacts.md
+++ b/third_party/allwpilib/MavenArtifacts.md
@@ -9,7 +9,7 @@
 * (Development) https://frcmaven.wpi.edu/artifactory/development/
 
 The release repository is where official WPILib releases are pushed.
-The development repository is where development releases of every commit to [master](https://github.com/wpilibsuite/allwpilib/tree/master) is pushed.
+The development repository is where development releases of every commit to [main](https://github.com/wpilibsuite/allwpilib/tree/main) is pushed.
 
 ## Artifact classifiers
 We provide two base types of artifacts.
diff --git a/third_party/allwpilib/OtherVersions.md b/third_party/allwpilib/OtherVersions.md
index a335f55..43ce7d6 100644
--- a/third_party/allwpilib/OtherVersions.md
+++ b/third_party/allwpilib/OtherVersions.md
@@ -12,7 +12,8 @@
 
 ```groovy
 wpi.maven.useDevelopment = true
-wpi.wpilibVersion = 'YEAR.+'
+wpi.versions.wpilibVersion = 'YEAR.+'
+wpi.versions.wpimathVersion = 'YEAR.+
 ```
 
 The top of your ``build.gradle`` file should now look similar to the code below. Ignore any differences in versions.
@@ -21,11 +22,13 @@
 ```groovy
 plugins {
   id "java"
-  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+  id "edu.wpi.first.GradleRIO" version "2022.1.1"
 }
 
+wpi.maven.useLocal = false
 wpi.maven.useDevelopment = true
-wpi.wpilibVersion = '2020.+'
+wpi.versions.wpilibVersion = '2022.+'
+wpi.versions.wpimathVersion = '2022.+'
 ```
 
 C++
@@ -33,11 +36,13 @@
 plugins {
   id "cpp"
   id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+  id "edu.wpi.first.GradleRIO" version "2022.1.1"
 }
 
+wpi.maven.useLocal = false
 wpi.maven.useDevelopment = true
-wpi.wpilibVersion = '2020.+'
+wpi.versions.wpilibVersion = '2022.+'
+wpi.versions.wpimathVersion = '2022.+'
 ```
 
 ## Local Build
@@ -48,11 +53,13 @@
 ```groovy
 plugins {
   id "java"
-  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+  id "edu.wpi.first.GradleRIO" version "2022.1.1"
 }
 
+wpi.maven.useLocal = false
 wpi.maven.useFrcMavenLocalDevelopment = true
-wpi.wpilibVersion = 'YEAR.424242.+'
+wpi.versions.wpilibVersion = 'YEAR.424242.+'
+wpi.versions.wpimathVersion = 'YEAR.424242.+'
 ```
 
 C++
@@ -60,9 +67,26 @@
 plugins {
   id "cpp"
   id "google-test-test-suite"
-  id "edu.wpi.first.GradleRIO" version "2020.3.2"
+  id "edu.wpi.first.GradleRIO" version "2022.1.1"
 }
 
+wpi.maven.useLocal = false
 wpi.maven.useFrcMavenLocalDevelopment = true
-wpi.wpilibVersion = 'YEAR.424242.+'
+wpi.versions.wpilibVersion = 'YEAR.424242.+'
+wpi.versions.wpimathVersion = 'YEAR.424242.+'
 ```
+
+# roboRIO Development
+
+This repo contains a myRobot project built in way to do full project development without needing to do a full publish into GradleRIO. These also only require building the minimum amount of binaries for the roboRIO, so the builds are much faster as well.
+
+The setup only works if the roboRIO is USB connected. If an alternate IP address is preferred, the `address` block in myRobot\build.gradle can be changed to point to another address.
+
+The following 3 tasks can be used for deployment:
+* `:myRobot:deployShared` deploys the C++ project using shared dependencies. Prefer this one for most C++ development.
+* `:myRobot:deployStatic` deploys the C++ project with all dependencies statically linked.
+* `:myRobot:deployJava` deploys the Java project and all required dependencies. Also installs the JRE if not currently installed.
+
+Deploying any of these to the roboRIO will disable the current startup project until it is redeployed.
+
+From here, ssh into the roboRIO using the `admin` account (`lvuser` will fail to run in many cases). In the admin home directory, a file for each deploy type will exist (`myRobotCpp`, `myRobotCppStatic` and `myRobotJavaRun`). These can be run to start up the corresponding project.
diff --git a/third_party/allwpilib/README-CMAKE.md b/third_party/allwpilib/README-CMAKE.md
index 09c9792..361a5b9 100644
--- a/third_party/allwpilib/README-CMAKE.md
+++ b/third_party/allwpilib/README-CMAKE.md
@@ -35,8 +35,10 @@
   * This option will build C++ unit tests. These can be run via `make test`.
 * `WITH_CSCORE` (ON Default)
   * This option will cause cscore to be built. Turning this off will implicitly disable cameraserver, the hal and wpilib as well, irrespective of their specific options. If this is off, the OpenCV build requirement is removed.
+* `WITH_WPIMATH` (ON Default)
+  * This option will build the wpimath library. This option must be on to build wpilib.
 * `WITH_WPILIB` (ON Default)
-  * This option will build the hal, wpilibc/j, and wpimath during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO.
+  * This option will build the hal and wpilibc/j during the build. The HAL is the simulator hal, unless the external hal options are used. The cmake build has no capability to build for the RoboRIO.
 * `WITH_SIMULATION_MODULES` (ON Default)
   * This option will build simulation modules, including wpigui and the HALSim plugins.
 * `WITH_EXTERNAL_HAL` (OFF Default)
@@ -48,7 +50,7 @@
 
 ## Build Setup
 
-The WPILib CMake build does not allow in source builds. Because the `build` directory is used by Gradle, we recommend a `buildcmake` directory in the root. This folder is included in the gitignore.
+The WPILib CMake build does not allow in source builds. Because the `build` directory is used by Gradle, we recommend a `build-cmake` directory in the root. This folder is included in the gitignore.
 
 Once you have a build folder, run CMake configuration in that build directory with the following command.
 
diff --git a/third_party/allwpilib/README.md b/third_party/allwpilib/README.md
index 836924f..1ddac77 100644
--- a/third_party/allwpilib/README.md
+++ b/third_party/allwpilib/README.md
@@ -1,6 +1,8 @@
 # WPILib Project
 
 ![CI](https://github.com/wpilibsuite/allwpilib/workflows/CI/badge.svg)
+[![C++ Documentation](https://img.shields.io/badge/documentation-c%2B%2B-blue)](https://first.wpi.edu/wpilib/allwpilib/docs/development/cpp/)
+[![Java Documentation](https://img.shields.io/badge/documentation-java-orange)](https://first.wpi.edu/wpilib/allwpilib/docs/development/java/)
 
 Welcome to the WPILib project. This repository contains the HAL, WPILibJ, and WPILibC projects. These are the core libraries for creating robot programs for the roboRIO.
 
@@ -31,18 +33,25 @@
 ## Requirements
 
 - [JDK 11](https://adoptopenjdk.net/)
+    - Note that the JRE is insufficient; the full JDK is required
+    - On Ubuntu, run `sudo apt install openjdk-11-jdk`
+    - On Windows, install the JDK 11 .msi from the link above
+    - On macOS, install the JDK 11 .pkg from the link above
 - C++ compiler
-    - On Linux, install GCC
+    - On Linux, install GCC 8 or greater
     - On Windows, install [Visual Studio Community 2019](https://visualstudio.microsoft.com/vs/community/) and select the C++ programming language during installation (Gradle can't use the build tools for Visual Studio 2019)
     - On macOS, install the Xcode command-line build tools via `xcode-select --install`
-- [ARM compiler toolchain](https://github.com/wpilibsuite/roborio-toolchain/releases)
-    - For 2020 and beyond, use GCC version 7 or greater
+- ARM compiler toolchain
+    - Run `./gradlew installRoboRioToolchain` after cloning this repository
+    - If the WPILib installer was used, this toolchain is already installed
+- Raspberry Pi toolchain (optional)
+    - Run `./gradlew installRaspbianToolchain` after cloning this repository
 
 ## Setup
 
-Clone the WPILib repository. If the toolchains are not installed, install them, and make sure they are available on the system PATH.
+Clone the WPILib repository and follow the instructions above for installing any required tooling.
 
-See the [styleguide README](https://github.com/wpilibsuite/styleguide/blob/master/README.md) for wpiformat setup instructions.
+See the [styleguide README](https://github.com/wpilibsuite/styleguide/blob/main/README.md) for wpiformat setup instructions.
 
 ## Building
 
@@ -86,10 +95,10 @@
 
 ### Gazebo simulation
 
-If you also want simulation to be built, add -PmakeSim. This requires gazebo_transport. We have tested on 14.04 and 15.05, but any correct install of Gazebo should work, even on Windows if you build Gazebo from source. Correct means CMake needs to be able to find gazebo-config.cmake. See [The Gazebo website](https://gazebosim.org/) for installation instructions.
+If you also want to force building Gazebo simulation support, add -PforceGazebo. This requires gazebo_transport. We have tested on 14.04 and 15.05, but any correct install of Gazebo should work, even on Windows if you build Gazebo from source. Correct means CMake needs to be able to find gazebo-config.cmake. See [The Gazebo website](https://gazebosim.org/) for installation instructions.
 
 ```bash
-./gradlew build -PmakeSim
+./gradlew build -PforceGazebo
 ```
 
 If you prefer to use CMake directly, the you can still do so.
@@ -111,7 +120,9 @@
 
 #### Java Code Quality Tools
 
-The Java code quality tools (checkstyle, pmd, etc.) can be run with the `./gradlew javaFormat` task.
+The Java code quality tools Checkstyle, PMD, and Spotless can be run via `./gradlew javaFormat`. SpotBugs can be run via the `spotbugsMain`, `spotbugsTest`, and `spotbugsDev` tasks. These tools will all be run automatically by the `build` task. To disable this behavior, pass the `-PskipJavaFormat` flag.
+
+If you only want to run the Java autoformatter, run `./gradlew spotlessApply`.
 
 ### CMake
 
@@ -138,6 +149,8 @@
 
 The hal directory contains more C++ code meant to run on the roboRIO. HAL is an acronym for "Hardware Abstraction Layer", and it interfaces with the NI Libraries. The NI Libraries contain the low-level code for controlling devices on your robot. The NI Libraries are found in the ni-libraries folder.
 
+The upstream_utils directory contains scripts for updating copies of thirdparty code in the repository.
+
 The [styleguide repository](https://github.com/wpilibsuite/styleguide) contains our style guides for C++ and Java code. Anything submitted to the WPILib project needs to follow the code style guides outlined in there. For details about the style, please see the contributors document [here](CONTRIBUTING.md#coding-guidelines).
 
 # Contributing to WPILib
diff --git a/third_party/allwpilib/ThirdPartyNotices.txt b/third_party/allwpilib/ThirdPartyNotices.txt
index 55d21f3..69cbd4a 100644
--- a/third_party/allwpilib/ThirdPartyNotices.txt
+++ b/third_party/allwpilib/ThirdPartyNotices.txt
@@ -27,10 +27,13 @@
 libuv                 wpiutil/src/main/native/include/uv.h
                       wpiutil/src/main/native/include/uv/
                       wpiutil/src/main/native/libuv/
+fmtlib                wpiutil/src/main/native/fmtlib/
 sigslot               wpiutil/src/main/native/include/wpi/Signal.h
                       wpiutil/src/test/native/cpp/sigslot/
 tcpsockets            wpiutil/src/main/native/cpp/TCP{Stream,Connector,Acceptor}.cpp
                       wpiutil/src/main/native/include/wpi/TCP*.h
+MPack                 wpiutil/src/main/native/include/mpack.h
+                      wpiutil/src/main/native/cpp/mpack.cpp
 Bootstrap             wpiutil/src/main/native/resources/bootstrap-*
 CoreUI                wpiutil/src/main/native/resources/coreui-*
 Feather Icons         wpiutil/src/main/native/resources/feather-*
@@ -40,6 +43,9 @@
 Eigen                 wpimath/src/main/native/eigeninclude/
                       wpimath/src/main/native/include/unsupported/
 StackWalker           wpiutil/src/main/native/windows/StackWalker.*
+TCB span              wpiutil/src/main/native/include/wpi/span.h
+                      wpiutil/src/test/native/cpp/span/
+GHC filesystem        wpiutil/src/main/native/include/wpi/ghc/
 Team 254 Library      wpilibj/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
                       wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
                       wpilibc/src/main/native/include/spline/SplineParameterizer.h
@@ -232,6 +238,32 @@
 
 
 ==============================================================================
+MPacks License
+==============================================================================
+The MIT License (MIT)
+
+Copyright (c) 2015-2021 Nicholas Fraser and the MPack authors
+
+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.
+
+
+==============================================================================
 Bootstrap License
 ==============================================================================
 Copyright (c) 2011-2018 Twitter, Inc.
@@ -845,3 +877,95 @@
 THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+=====================
+Portable File Dialogs
+=====================
+Copyright © 2018—2020 Sam Hocevar <sam@hocevar.net>
+
+This library is free software. It comes without any warranty, to
+the extent permitted by applicable law. You can redistribute it
+and/or modify it under the terms of the Do What the **** You Want
+to Public License, Version 2, as published by the WTFPL Task Force.
+See http://www.wtfpl.net/ for more details.
+
+======================
+Boost Software License
+======================
+Boost Software License - Version 1.0 - August 17th, 2003
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+
+======
+fmtlib
+======
+Copyright (c) 2012 - present, Victor Zverovich
+
+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.
+
+--- Optional exception to the license ---
+
+As an exception, if, as a result of your compiling your source code, portions
+of this Software are embedded into a machine-executable object form of such
+source code, you may redistribute such embedded portions in such object form
+without including the above copyright and permission notices.
+
+==============
+GHC filesystem
+==============
+Copyright (c) 2018, Steffen Schümann <s.schuemann@pobox.com>
+
+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.
diff --git a/third_party/allwpilib/azure-pipelines-testbench.yaml b/third_party/allwpilib/azure-pipelines-testbench.yaml
index 674926e..2fc5252 100644
--- a/third_party/allwpilib/azure-pipelines-testbench.yaml
+++ b/third_party/allwpilib/azure-pipelines-testbench.yaml
@@ -4,88 +4,88 @@
   batch: true
   branches:
     include:
-    - master
+      - master
 
 stages:
-- stage: Build
-  jobs:
-  - job: IntegrationTests
-    displayName: Integration Tests
-    pool:
-      vmImage: 'Ubuntu 16.04'
+  - stage: Build
+    jobs:
+      - job: IntegrationTests
+        displayName: Integration Tests
+        pool:
+          vmImage: 'ubuntu-latest'
 
-    container:
-      image:  wpilib/roborio-cross-ubuntu:2021-18.04
+        container:
+          image: wpilib/roborio-cross-ubuntu:2022-18.04
 
-    timeoutInMinutes: 0
+        timeoutInMinutes: 0
 
-    steps:
-    - task: Gradle@2
-      condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
-      inputs:
-        workingDirectory: ''
-        gradleWrapperFile: 'gradlew'
-        gradleOptions: '-Xmx3072m'
-        publishJUnitResults: false
-        testResultsFiles: '**/TEST-*.xml'
-        tasks: 'copyWpilibJIntegrationTestJarToOutput copyWpilibCTestLibrariesToOutput'
-        options: '-Ponlylinuxathena -PbuildServer'
+        steps:
+          - task: Gradle@2
+            condition: and(succeeded(), not(startsWith(variables['Build.SourceBranch'], 'refs/tags/v')))
+            inputs:
+              workingDirectory: ""
+              gradleWrapperFile: "gradlew"
+              gradleOptions: "-Xmx3072m"
+              publishJUnitResults: false
+              testResultsFiles: "**/TEST-*.xml"
+              tasks: "copyWpilibJIntegrationTestJarToOutput copyWpilibCTestLibrariesToOutput"
+              options: "-Ponlylinuxathena -PbuildServer -PskipJavaFormat"
 
-    - task: PublishPipelineArtifact@0
-      inputs:
-        artifactName: 'Integration Tests'
-        targetPath: 'build/integrationTestFiles'
+          - task: PublishPipelineArtifact@0
+            inputs:
+              artifactName: "Integration Tests"
+              targetPath: "build/integrationTestFiles"
 
-- stage: TestBench
-  displayName: Test Bench
-  jobs:
-  - job: Cpp
-    displayName: C++
-    pool: RoboRioConnections
-    timeoutInMinutes: 30
-    workspace:
-      clean: all
-    steps:
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Integration Tests'
-        targetPath: 'build/integrationTestFiles'
+  - stage: TestBench
+    displayName: Test Bench
+    jobs:
+      - job: Cpp
+        displayName: C++
+        pool: RoboRioConnections
+        timeoutInMinutes: 30
+        workspace:
+          clean: all
+        steps:
+          - task: DownloadPipelineArtifact@0
+            inputs:
+              artifactName: "Integration Tests"
+              targetPath: "build/integrationTestFiles"
 
-    - task: ShellScript@2
-      displayName: Run C++ Tests
-      inputs:
-        scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
-        args: 'cpp -A "--gtest_output=xml:/home/admin/testResults/cppreport.xml"'
+          - task: ShellScript@2
+            displayName: Run C++ Tests
+            inputs:
+              scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
+              args: 'cpp -A "--gtest_output=xml:/home/admin/testResults/cppreport.xml"'
 
-    - task: PublishTestResults@2
-      displayName: Publish C++ Test Results
-      inputs:
-        testResultsFormat: 'JUnit'
-        testResultsFiles: '*.xml'
-        testRunTitle: 'C++ Test Report'
-        searchFolder: '$(System.DefaultWorkingDirectory)/test-reports'
+          - task: PublishTestResults@2
+            displayName: Publish C++ Test Results
+            inputs:
+              testResultsFormat: "JUnit"
+              testResultsFiles: "*.xml"
+              testRunTitle: "C++ Test Report"
+              searchFolder: "$(System.DefaultWorkingDirectory)/test-reports"
 
-  - job: Java
-    pool: RoboRioConnections
-    timeoutInMinutes: 30
-    workspace:
-      clean: all
-    steps:
-    - task: DownloadPipelineArtifact@0
-      inputs:
-        artifactName: 'Integration Tests'
-        targetPath: 'build/integrationTestFiles'
+      - job: Java
+        pool: RoboRioConnections
+        timeoutInMinutes: 30
+        workspace:
+          clean: all
+        steps:
+          - task: DownloadPipelineArtifact@0
+            inputs:
+              artifactName: "Integration Tests"
+              targetPath: "build/integrationTestFiles"
 
-    - task: ShellScript@2
-      displayName: Run Java Tests
-      inputs:
-        scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
-        args: 'java'
+          - task: ShellScript@2
+            displayName: Run Java Tests
+            inputs:
+              scriptPath: test-scripts/deploy-and-run-test-on-robot.sh
+              args: "java"
 
-    - task: PublishTestResults@2
-      displayName: Publish Java Test Results
-      inputs:
-        testResultsFormat: 'JUnit'
-        testResultsFiles: '*.xml'
-        testRunTitle: 'Java Test Report'
-        searchFolder: '$(System.DefaultWorkingDirectory)/test-reports'
+          - task: PublishTestResults@2
+            displayName: Publish Java Test Results
+            inputs:
+              testResultsFormat: "JUnit"
+              testResultsFiles: "*.xml"
+              testRunTitle: "Java Test Report"
+              searchFolder: "$(System.DefaultWorkingDirectory)/test-reports"
diff --git a/third_party/allwpilib/build.gradle b/third_party/allwpilib/build.gradle
index a5c0577..19ee6a8 100644
--- a/third_party/allwpilib/build.gradle
+++ b/third_party/allwpilib/build.gradle
@@ -1,27 +1,31 @@
 import edu.wpi.first.toolchain.*
 
+buildscript {
+    repositories {
+        mavenCentral()
+    }
+    dependencies {
+        classpath 'com.hubspot.jinjava:jinjava:2.5.8'
+    }
+}
+
 plugins {
     id 'base'
-    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.0.2'
+    id 'edu.wpi.first.wpilib.versioning.WPILibVersioningPlugin' version '4.1.0'
     id 'edu.wpi.first.wpilib.repositories.WPILibRepositoriesPlugin' version '2020.2'
     id 'edu.wpi.first.NativeUtils' apply false
-    id 'edu.wpi.first.GradleJni' version '0.10.1'
-    id 'edu.wpi.first.GradleVsCode' version '0.11.0'
+    id 'edu.wpi.first.GradleJni' version '1.0.0'
+    id 'edu.wpi.first.GradleVsCode' version '1.0.0'
     id 'idea'
     id 'visual-studio'
     id 'net.ltgt.errorprone' version '1.1.1' apply false
     id 'com.github.johnrengelman.shadow' version '5.2.0' apply false
+    id 'com.diffplug.spotless' version '5.5.0' apply false
+    id 'com.github.spotbugs' version '5.0.0-beta.1' apply false
 }
 
-if (project.hasProperty('buildServer')) {
-    wpilibVersioning.buildServerMode = true
-    wpilibVersioning.useAllTags = true
-}
-
-if (project.hasProperty('releaseMode')) {
-    wpilibVersioning.releaseMode = true
-    wpilibVersioning.useAllTags = true
-}
+wpilibVersioning.buildServerMode = project.hasProperty('buildServer')
+wpilibVersioning.releaseMode = project.hasProperty('releaseMode')
 
 allprojects {
     repositories {
@@ -106,6 +110,13 @@
         }
     }
 
+    // Enables UTF-8 support in Javadoc
+    tasks.withType(Javadoc) {
+        options.addStringOption("charset", "utf-8")
+        options.addStringOption("docencoding", "utf-8")
+        options.addStringOption("encoding", "utf-8")
+    }
+
     // Sign outputs with Developer ID
     if (project.hasProperty("developerID")) {
         tasks.withType(AbstractLinkTask) { task ->
@@ -117,8 +128,12 @@
                     String path = task.getLinkedFile().getAsFile().get().getAbsolutePath()
                     exec {
                         workingDir rootDir
-                        def args = ["sh", "-c", "codesign --force --strict --timestamp --options=runtime " +
-                                    "--verbose -s ${project.findProperty("developerID")} ${path}"]
+                        def args = [
+                            "sh",
+                            "-c",
+                            "codesign --force --strict --timestamp --options=runtime " +
+                            "--verbose -s ${project.findProperty("developerID")} ${path}"
+                        ]
                         commandLine args
                     }
                 }
@@ -132,5 +147,5 @@
 }
 
 wrapper {
-    gradleVersion = '6.0.1'
+    gradleVersion = '7.1.1'
 }
diff --git a/third_party/allwpilib/buildSrc/build.gradle b/third_party/allwpilib/buildSrc/build.gradle
index e4b3a6d..f2afdc7 100644
--- a/third_party/allwpilib/buildSrc/build.gradle
+++ b/third_party/allwpilib/buildSrc/build.gradle
@@ -5,5 +5,5 @@
     }
 }
 dependencies {
-    implementation "edu.wpi.first:native-utils:2021.0.4"
+    implementation "edu.wpi.first:native-utils:2022.3.1"
 }
diff --git a/third_party/allwpilib/buildSrc/src/main/groovy/JREArtifact.groovy b/third_party/allwpilib/buildSrc/src/main/groovy/JREArtifact.groovy
deleted file mode 100644
index e57249a..0000000
--- a/third_party/allwpilib/buildSrc/src/main/groovy/JREArtifact.groovy
+++ /dev/null
@@ -1,48 +0,0 @@
-import groovy.transform.CompileStatic
-import javax.inject.Inject
-import jaci.gradle.deploy.artifact.MavenArtifact
-import jaci.gradle.deploy.context.DeployContext
-import org.gradle.api.Project
-import jaci.gradle.ActionWrapper
-
-import java.util.function.Function
-
-@CompileStatic
-class JREArtifact extends MavenArtifact {
-    Function<DeployContext, Boolean> buildRequiresJre = (Function<DeployContext, Boolean>){ true }
-
-    void setJreDependency(String dep) {
-      dependency = project.dependencies.add(configuration(), dep)
-    }
-
-    @Inject
-    JREArtifact(String name, Project project) {
-        super(name, project)
-        configuration = project.configurations.create(configuration())
-
-        onlyIf = { DeployContext ctx ->
-            (buildRequiresJre.apply(ctx) && jreMissing(ctx)) || project.hasProperty("force-redeploy-jre")
-        }
-
-        predeploy << new ActionWrapper({ DeployContext ctx ->
-            ctx.logger.log('Deploying RoboRIO JRE (this will take a while)...')
-        })
-
-        directory = '/tmp'
-        filename = 'frcjre.ipk'
-
-        postdeploy << new ActionWrapper({ DeployContext ctx ->
-            ctx.logger.log('Installing JRE...')
-            ctx.execute('opkg remove frc2020-openjdk*; opkg install /tmp/frcjre.ipk; rm /tmp/frcjre.ipk')
-            ctx.logger.log('JRE Deployed!')
-        })
-    }
-
-    String configuration() {
-        return name + 'frcjre'
-    }
-
-    boolean jreMissing(DeployContext ctx) {
-        return ctx.execute('if [[ -f "/usr/local/frc/JRE/bin/java" ]]; then echo OK; else echo MISSING; fi').result.contains("MISSING")
-    }
-}
diff --git a/third_party/allwpilib/buildSrc/src/main/groovy/SingleNativeBuild.groovy b/third_party/allwpilib/buildSrc/src/main/groovy/SingleNativeBuild.groovy
index d43afff..f316f13 100644
--- a/third_party/allwpilib/buildSrc/src/main/groovy/SingleNativeBuild.groovy
+++ b/third_party/allwpilib/buildSrc/src/main/groovy/SingleNativeBuild.groovy
@@ -6,6 +6,8 @@
 import java.util.ArrayList;
 import java.util.List;
 
+import org.gradle.api.tasks.Delete
+
 import org.gradle.api.GradleException;
 import org.gradle.api.Plugin;
 import org.gradle.api.Project;
@@ -44,7 +46,7 @@
 import org.gradle.platform.base.TypeBuilder;
 import org.gradle.nativeplatform.tasks.ObjectFilesToBinary;
 import groovy.transform.CompileStatic;
-import edu.wpi.first.nativeutils.tasks.ExportsGenerationTask
+import edu.wpi.first.nativeutils.exports.ExportsGenerationTask
 
 @CompileStatic
 class SingleNativeBuild implements Plugin<Project> {
@@ -87,6 +89,14 @@
                     subs << component
                 }
             }
+            Delete deleteObjects = null
+            if (project.hasProperty('buildServer')) {
+                deleteObjects = project.tasks.create('deleteObjects', Delete)
+                project.tasks.named('build').configure { Task t ->
+                    t.dependsOn deleteObjects
+                    return
+                }
+            }
             subs.each {
                 ((NativeLibrarySpec) it).binaries.each { oBinary ->
                     if (oBinary.buildable == false) {
@@ -136,6 +146,10 @@
                             tree.include '**/*.o'
                             tree.include '**/*.obj'
                             link.source tree
+                            if (project.hasProperty('buildServer')) {
+                                deleteObjects.dependsOn link
+                                deleteObjects.delete tree
+                            }
                         } else if (binary instanceof StaticLibraryBinarySpec) {
                             def sBinary = (StaticLibraryBinarySpec) binary
                             ObjectFilesToBinary assemble = (ObjectFilesToBinary) sBinary.tasks.createStaticLib
@@ -145,6 +159,10 @@
                             tree.include '**/*.o'
                             tree.include '**/*.obj'
                             assemble.source tree
+                            if (project.hasProperty('buildServer')) {
+                                deleteObjects.dependsOn assemble
+                                deleteObjects.delete tree
+                            }
                         }
                     }
                 }
diff --git a/third_party/allwpilib/buildSrc/src/main/groovy/WPIJREArtifact.groovy b/third_party/allwpilib/buildSrc/src/main/groovy/WPIJREArtifact.groovy
new file mode 100644
index 0000000..abe4851
--- /dev/null
+++ b/third_party/allwpilib/buildSrc/src/main/groovy/WPIJREArtifact.groovy
@@ -0,0 +1,48 @@
+import groovy.transform.CompileStatic
+import javax.inject.Inject
+import edu.wpi.first.deployutils.deploy.artifact.MavenArtifact
+import edu.wpi.first.deployutils.deploy.context.DeployContext
+import org.gradle.api.Project
+import edu.wpi.first.deployutils.ActionWrapper
+import edu.wpi.first.deployutils.deploy.target.RemoteTarget
+import edu.wpi.first.deployutils.PredicateWrapper
+
+import java.util.function.Function
+
+@CompileStatic
+public class WPIJREArtifact extends MavenArtifact {
+    private final String configName;
+
+    public String getConfigName() {
+        return configName;
+    }
+
+    @Inject
+    public WPIJREArtifact(String name, RemoteTarget target) {
+        super(name, target);
+        String configName = name + "frcjre";
+        this.configName = configName;
+        Project project = target.getProject();
+        getConfiguration().set(project.getConfigurations().create(configName));
+        getDependency().set(project.getDependencies().add(configName, "edu.wpi.first.jdk:roborio-2022:11.0.12u5-1"));
+
+        setOnlyIf(new PredicateWrapper({ DeployContext ctx ->
+            return jreMissing(ctx) || project.hasProperty("force-redeploy-jre");
+        }));
+
+        getDirectory().set("/tmp");
+        getFilename().set("frcjre.ipk");
+
+        getPostdeploy().add(new ActionWrapper({ DeployContext ctx ->
+            ctx.getLogger().log("Installing JRE...");
+            ctx.execute("opkg remove frc2022-openjdk*; opkg install /tmp/frcjre.ipk; rm /tmp/frcjre.ipk");
+            ctx.getLogger().log("JRE Deployed!");
+        }));
+    }
+
+    private boolean jreMissing(DeployContext ctx) {
+        return ctx.execute("if [[ -f \"/usr/local/frc/JRE/bin/java\" ]]; then echo OK; else echo MISSING; fi").getResult().contains("MISSING");
+    }
+
+
+}
diff --git a/third_party/allwpilib/cameraserver/.styleguide b/third_party/allwpilib/cameraserver/.styleguide
index bea7643..63515d2 100644
--- a/third_party/allwpilib/cameraserver/.styleguide
+++ b/third_party/allwpilib/cameraserver/.styleguide
@@ -12,6 +12,7 @@
 }
 
 includeOtherLibs {
+  ^fmt/
   ^hal/
   ^networktables/
   ^opencv2/
diff --git a/third_party/allwpilib/cameraserver/CMakeLists.txt b/third_party/allwpilib/cameraserver/CMakeLists.txt
index 33e19d0..4916be3 100644
--- a/third_party/allwpilib/cameraserver/CMakeLists.txt
+++ b/third_party/allwpilib/cameraserver/CMakeLists.txt
@@ -9,7 +9,7 @@
 if (WITH_JAVA)
     find_package(Java REQUIRED)
     include(UseJava)
-    set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
     #find java files, copy them locally
 
@@ -53,8 +53,8 @@
     set (cameraserver_config_dir share/cameraserver)
 endif()
 
-configure_file(cameraserver-config.cmake.in ${CMAKE_BINARY_DIR}/cameraserver-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/cameraserver-config.cmake DESTINATION ${cameraserver_config_dir})
+configure_file(cameraserver-config.cmake.in ${WPILIB_BINARY_DIR}/cameraserver-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/cameraserver-config.cmake DESTINATION ${cameraserver_config_dir})
 install(EXPORT cameraserver DESTINATION ${cameraserver_config_dir})
 
 file(GLOB multiCameraServer_src multiCameraServer/src/main/native/cpp/*.cpp)
diff --git a/third_party/allwpilib/cameraserver/build.gradle b/third_party/allwpilib/cameraserver/build.gradle
index 2f864d9..7a611e6 100644
--- a/third_party/allwpilib/cameraserver/build.gradle
+++ b/third_party/allwpilib/cameraserver/build.gradle
@@ -20,9 +20,9 @@
 
 ext {
     sharedCvConfigs = [cameraserver    : [],
-                       cameraserverBase: [],
-                       cameraserverDev : [],
-                       cameraserverTest: []]
+        cameraserverBase: [],
+        cameraserverDev : [],
+        cameraserverTest: []]
     staticCvConfigs = [:]
     useJava = true
     useCpp = true
@@ -32,14 +32,32 @@
 
 nativeUtils.exportsConfigs {
     cameraserver {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
 }
 
diff --git a/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in b/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in
index 301b25b..94d6813 100644
--- a/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in
+++ b/third_party/allwpilib/cameraserver/cameraserver-config.cmake.in
@@ -5,4 +5,5 @@
 @CSCORE_DEP_REPLACE@
 find_dependency(OpenCV)
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cameraserver.cmake)
diff --git a/third_party/allwpilib/cameraserver/multiCameraServer/build.gradle b/third_party/allwpilib/cameraserver/multiCameraServer/build.gradle
index e8565cd..a198cc3 100644
--- a/third_party/allwpilib/cameraserver/multiCameraServer/build.gradle
+++ b/third_party/allwpilib/cameraserver/multiCameraServer/build.gradle
@@ -52,10 +52,10 @@
                 }
             }
             binaries.all { binary ->
-                    lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
-                    lib project: ':ntcore', library: 'ntcore', linkage: 'static'
-                    lib project: ':cscore', library: 'cscore', linkage: 'static'
-                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
+                lib project: ':ntcore', library: 'ntcore', linkage: 'static'
+                lib project: ':cscore', library: 'cscore', linkage: 'static'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
             }
         }
     }
diff --git a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
index 70098cd..f6f91c1 100644
--- a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
+++ b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/java/edu/wpi/Main.java
@@ -1,55 +1,50 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi;
 
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Paths;
-import java.util.ArrayList;
-import java.util.List;
-
 import com.google.gson.Gson;
 import com.google.gson.GsonBuilder;
 import com.google.gson.JsonArray;
 import com.google.gson.JsonElement;
 import com.google.gson.JsonObject;
 import com.google.gson.JsonParser;
-
-import edu.wpi.cscore.VideoSource;
 import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.VideoSource;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.List;
 
 /*
-   JSON format:
-   {
-       "team": <team number>,
-       "ntmode": <"client" or "server", "client" if unspecified>
-       "cameras": [
-           {
-               "name": <camera name>
-               "path": <path, e.g. "/dev/video0">
-               "pixel format": <"MJPEG", "YUYV", etc>   // optional
-               "width": <video mode width>              // optional
-               "height": <video mode height>            // optional
-               "fps": <video mode fps>                  // optional
-               "brightness": <percentage brightness>    // optional
-               "white balance": <"auto", "hold", value> // optional
-               "exposure": <"auto", "hold", value>      // optional
-               "properties": [                          // optional
-                   {
-                       "name": <property name>
-                       "value": <property value>
-                   }
-               ]
-           }
-       ]
-   }
- */
+  JSON format:
+  {
+      "team": <team number>,
+      "ntmode": <"client" or "server", "client" if unspecified>
+      "cameras": [
+          {
+              "name": <camera name>
+              "path": <path, e.g. "/dev/video0">
+              "pixel format": <"MJPEG", "YUYV", etc>   // optional
+              "width": <video mode width>              // optional
+              "height": <video mode height>            // optional
+              "fps": <video mode fps>                  // optional
+              "brightness": <percentage brightness>    // optional
+              "white balance": <"auto", "hold", value> // optional
+              "exposure": <"auto", "hold", value>      // optional
+              "properties": [                          // optional
+                  {
+                      "name": <property name>
+                      "value": <property value>
+                  }
+              ]
+          }
+      ]
+  }
+*/
 
 public final class Main {
   private static String configFile = "/boot/frc.json";
@@ -61,23 +56,18 @@
     public JsonObject config;
   }
 
-  public static int team;
-  public static boolean server;
-  public static List<CameraConfig> cameras = new ArrayList<>();
+  static int team;
+  static boolean server;
+  static List<CameraConfig> cameras = new ArrayList<>();
 
-  private Main() {
-  }
+  private Main() {}
 
-  /**
-   * Report parse error.
-   */
+  /** Report parse error. */
   public static void parseError(String str) {
     System.err.println("config error in '" + configFile + "': " + str);
   }
 
-  /**
-   * Read single camera configuration.
-   */
+  /** Read single camera configuration. */
   public static boolean readCameraConfig(JsonObject config) {
     CameraConfig cam = new CameraConfig();
 
@@ -103,10 +93,7 @@
     return true;
   }
 
-  /**
-   * Read configuration file.
-   */
-  @SuppressWarnings("PMD.CyclomaticComplexity")
+  /** Read configuration file. */
   public static boolean readConfig() {
     // parse file
     JsonElement top;
@@ -160,22 +147,17 @@
     return true;
   }
 
-  /**
-   * Start running the camera.
-   */
+  /** Start running the camera. */
   public static void startCamera(CameraConfig config) {
     System.out.println("Starting camera '" + config.name + "' on " + config.path);
-    VideoSource camera = CameraServer.getInstance().startAutomaticCapture(
-        config.name, config.path);
+    VideoSource camera = CameraServer.startAutomaticCapture(config.name, config.path);
 
     Gson gson = new GsonBuilder().create();
 
     camera.setConfigJson(gson.toJson(config.config));
   }
 
-  /**
-   * Main.
-   */
+  /** Main. */
   public static void main(String... args) {
     if (args.length > 0) {
       configFile = args[0];
@@ -202,7 +184,7 @@
     }
 
     // loop forever
-    for (;;) {
+    for (; ; ) {
       try {
         Thread.sleep(10000);
       } catch (InterruptedException ex) {
diff --git a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
index dbfc7b6..45b56d4 100644
--- a/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/cameraserver/multiCameraServer/src/main/native/cpp/main.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstdio>
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include <networktables/NetworkTableInstance.h>
-#include <wpi/StringRef.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fmt/raw_ostream.h>
 #include <wpi/json.h>
 #include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
@@ -108,7 +107,7 @@
   try {
     j = wpi::json::parse(is);
   } catch (const wpi::json::parse_error& e) {
-    ParseError() << "byte " << e.byte << ": " << e.what() << '\n';
+    fmt::print(ParseError(), "byte {}: {}\n", e.byte, e.what());
     return false;
   }
 
@@ -130,10 +129,9 @@
   if (j.count("ntmode") != 0) {
     try {
       auto str = j.at("ntmode").get<std::string>();
-      wpi::StringRef s(str);
-      if (s.equals_lower("client")) {
+      if (wpi::equals_lower(str, "client")) {
         server = false;
-      } else if (s.equals_lower("server")) {
+      } else if (wpi::equals_lower(str, "server")) {
         server = true;
       } else {
         ParseError() << "could not understand ntmode value '" << str << "'\n";
@@ -146,7 +144,9 @@
   // cameras
   try {
     for (auto&& camera : j.at("cameras")) {
-      if (!ReadCameraConfig(camera)) return false;
+      if (!ReadCameraConfig(camera)) {
+        return false;
+      }
     }
   } catch (const wpi::json::exception& e) {
     ParseError() << "could not read cameras: " << e.what() << '\n';
@@ -157,34 +157,41 @@
 }
 
 void StartCamera(const CameraConfig& config) {
-  wpi::outs() << "Starting camera '" << config.name << "' on " << config.path
-              << '\n';
-  auto camera = frc::CameraServer::GetInstance()->StartAutomaticCapture(
-      config.name, config.path);
+  fmt::print("Starting camera '{}' on {}\n", config.name, config.path);
+  auto camera =
+      frc::CameraServer::StartAutomaticCapture(config.name, config.path);
 
   camera.SetConfigJson(config.config);
 }
 }  // namespace
 
 int main(int argc, char* argv[]) {
-  if (argc >= 2) configFile = argv[1];
+  if (argc >= 2) {
+    configFile = argv[1];
+  }
 
   // read configuration
-  if (!ReadConfig()) return EXIT_FAILURE;
+  if (!ReadConfig()) {
+    return EXIT_FAILURE;
+  }
 
   // start NetworkTables
   auto ntinst = nt::NetworkTableInstance::GetDefault();
   if (server) {
-    wpi::outs() << "Setting up NetworkTables server\n";
+    std::puts("Setting up NetworkTables server");
     ntinst.StartServer();
   } else {
-    wpi::outs() << "Setting up NetworkTables client for team " << team << '\n';
+    fmt::print("Setting up NetworkTables client for team {}\n", team);
     ntinst.StartClientTeam(team);
   }
 
   // start cameras
-  for (auto&& camera : cameras) StartCamera(camera);
+  for (auto&& camera : cameras) {
+    StartCamera(camera);
+  }
 
   // loop forever
-  for (;;) std::this_thread::sleep_for(std::chrono::seconds(10));
+  for (;;) {
+    std::this_thread::sleep_for(std::chrono::seconds(10));
+  }
 }
diff --git a/third_party/allwpilib/cameraserver/src/dev/java/edu/wpi/first/cameraserver/DevMain.java b/third_party/allwpilib/cameraserver/src/dev/java/edu/wpi/first/cameraserver/DevMain.java
index 1182ac4..df4affa 100644
--- a/third_party/allwpilib/cameraserver/src/dev/java/edu/wpi/first/cameraserver/DevMain.java
+++ b/third_party/allwpilib/cameraserver/src/dev/java/edu/wpi/first/cameraserver/DevMain.java
@@ -1,17 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.cameraserver;
 
 public final class DevMain {
-  public static void main(String[] args) {
+  public static void main(String[] args) {}
 
-  }
-
-  private DevMain() {
-  }
+  private DevMain() {}
 }
diff --git a/third_party/allwpilib/cameraserver/src/dev/native/cpp/main.cpp b/third_party/allwpilib/cameraserver/src/dev/native/cpp/main.cpp
index e324b44..a3e363e 100644
--- a/third_party/allwpilib/cameraserver/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/cameraserver/src/dev/native/cpp/main.cpp
@@ -1,8 +1,5 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 int main() {}
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
index 787b944..11de969 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServer.java
@@ -1,12 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.cameraserver;
 
+import edu.wpi.first.cscore.AxisCamera;
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.MjpegServer;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.cscore.VideoEvent;
+import edu.wpi.first.cscore.VideoException;
+import edu.wpi.first.cscore.VideoListener;
+import edu.wpi.first.cscore.VideoMode;
+import edu.wpi.first.cscore.VideoMode.PixelFormat;
+import edu.wpi.first.cscore.VideoProperty;
+import edu.wpi.first.cscore.VideoSink;
+import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.HashMap;
@@ -14,45 +29,28 @@
 import java.util.Objects;
 import java.util.concurrent.atomic.AtomicInteger;
 
-import edu.wpi.cscore.AxisCamera;
-import edu.wpi.cscore.CameraServerJNI;
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.CvSource;
-import edu.wpi.cscore.MjpegServer;
-import edu.wpi.cscore.UsbCamera;
-import edu.wpi.cscore.VideoEvent;
-import edu.wpi.cscore.VideoException;
-import edu.wpi.cscore.VideoListener;
-import edu.wpi.cscore.VideoMode;
-import edu.wpi.cscore.VideoMode.PixelFormat;
-import edu.wpi.cscore.VideoProperty;
-import edu.wpi.cscore.VideoSink;
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.networktables.EntryListenerFlags;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
 /**
- * Singleton class for creating and keeping camera servers.
- * Also publishes camera information to NetworkTables.
+ * Singleton class for creating and keeping camera servers. Also publishes camera information to
+ * NetworkTables.
  */
+@SuppressWarnings("PMD.UnusedPrivateField")
 public final class CameraServer {
   public static final int kBasePort = 1181;
 
-  @Deprecated
-  public static final int kSize640x480 = 0;
-  @Deprecated
-  public static final int kSize320x240 = 1;
-  @Deprecated
-  public static final int kSize160x120 = 2;
+  @Deprecated public static final int kSize640x480 = 0;
+  @Deprecated public static final int kSize320x240 = 1;
+  @Deprecated public static final int kSize160x120 = 2;
 
   private static final String kPublishName = "/CameraPublisher";
   private static CameraServer server;
 
   /**
    * Get the CameraServer instance.
+   *
+   * @return The CameraServer instance.
+   * @deprecated Use the static methods
    */
+  @Deprecated
   public static synchronized CameraServer getInstance() {
     if (server == null) {
       server = new CameraServer();
@@ -60,32 +58,226 @@
     return server;
   }
 
-  private final AtomicInteger m_defaultUsbDevice;
-  private String m_primarySourceName;
-  private final Map<String, VideoSource> m_sources;
-  private final Map<String, VideoSink> m_sinks;
-  private final Map<Integer, NetworkTable> m_tables;  // indexed by source handle
+  private static final AtomicInteger m_defaultUsbDevice = new AtomicInteger();
+  private static String m_primarySourceName;
+  private static final Map<String, VideoSource> m_sources = new HashMap<>();
+  private static final Map<String, VideoSink> m_sinks = new HashMap<>();
+  private static final Map<Integer, NetworkTable> m_tables =
+      new HashMap<>(); // indexed by source handle
   // source handle indexed by sink handle
-  private final Map<Integer, Integer> m_fixedSources;
-  private final NetworkTable m_publishTable;
-  private final VideoListener m_videoListener; //NOPMD
-  private final int m_tableListener; //NOPMD
-  private int m_nextPort;
-  private String[] m_addresses;
+  private static final Map<Integer, Integer> m_fixedSources = new HashMap<>();
+  private static final NetworkTable m_publishTable =
+      NetworkTableInstance.getDefault().getTable(kPublishName);
 
-  @SuppressWarnings("JavadocMethod")
+  // We publish sources to NetworkTables using the following structure:
+  // "/CameraPublisher/{Source.Name}/" - root
+  // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
+  // - "streams" (string array): URLs that can be used to stream data
+  // - "description" (string): Description of the source
+  // - "connected" (boolean): Whether source is connected
+  // - "mode" (string): Current video mode
+  // - "modes" (string array): Available video modes
+  // - "Property/{Property}" - Property values
+  // - "PropertyInfo/{Property}" - Property supporting information
+
+  // Listener for video events
+  private static final VideoListener m_videoListener =
+      new VideoListener(
+          event -> {
+            switch (event.kind) {
+              case kSourceCreated:
+                {
+                  // Create subtable for the camera
+                  NetworkTable table = m_publishTable.getSubTable(event.name);
+                  m_tables.put(event.sourceHandle, table);
+                  table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
+                  table
+                      .getEntry("description")
+                      .setString(CameraServerJNI.getSourceDescription(event.sourceHandle));
+                  table
+                      .getEntry("connected")
+                      .setBoolean(CameraServerJNI.isSourceConnected(event.sourceHandle));
+                  table
+                      .getEntry("streams")
+                      .setStringArray(getSourceStreamValues(event.sourceHandle));
+                  try {
+                    VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
+                    table.getEntry("mode").setDefaultString(videoModeToString(mode));
+                    table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
+                  } catch (VideoException ignored) {
+                    // Do nothing. Let the other event handlers update this if there is an error.
+                  }
+                  break;
+                }
+              case kSourceDestroyed:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    table.getEntry("source").setString("");
+                    table.getEntry("streams").setStringArray(new String[0]);
+                    table.getEntry("modes").setStringArray(new String[0]);
+                  }
+                  break;
+                }
+              case kSourceConnected:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    // update the description too (as it may have changed)
+                    table
+                        .getEntry("description")
+                        .setString(CameraServerJNI.getSourceDescription(event.sourceHandle));
+                    table.getEntry("connected").setBoolean(true);
+                  }
+                  break;
+                }
+              case kSourceDisconnected:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    table.getEntry("connected").setBoolean(false);
+                  }
+                  break;
+                }
+              case kSourceVideoModesUpdated:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
+                  }
+                  break;
+                }
+              case kSourceVideoModeChanged:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    table.getEntry("mode").setString(videoModeToString(event.mode));
+                  }
+                  break;
+                }
+              case kSourcePropertyCreated:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    putSourcePropertyValue(table, event, true);
+                  }
+                  break;
+                }
+              case kSourcePropertyValueUpdated:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    putSourcePropertyValue(table, event, false);
+                  }
+                  break;
+                }
+              case kSourcePropertyChoicesUpdated:
+                {
+                  NetworkTable table = m_tables.get(event.sourceHandle);
+                  if (table != null) {
+                    try {
+                      String[] choices =
+                          CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
+                      table
+                          .getEntry("PropertyInfo/" + event.name + "/choices")
+                          .setStringArray(choices);
+                    } catch (VideoException ignored) {
+                      // ignore
+                    }
+                  }
+                  break;
+                }
+              case kSinkSourceChanged:
+              case kSinkCreated:
+              case kSinkDestroyed:
+              case kNetworkInterfacesChanged:
+                {
+                  m_addresses = CameraServerJNI.getNetworkInterfaces();
+                  updateStreamValues();
+                  break;
+                }
+              default:
+                break;
+            }
+          },
+          0x4fff,
+          true);
+
+  private static final int m_tableListener =
+      NetworkTableInstance.getDefault()
+          .addEntryListener(
+              kPublishName + "/",
+              event -> {
+                String relativeKey = event.name.substring(kPublishName.length() + 1);
+
+                // get source (sourceName/...)
+                int subKeyIndex = relativeKey.indexOf('/');
+                if (subKeyIndex == -1) {
+                  return;
+                }
+                String sourceName = relativeKey.substring(0, subKeyIndex);
+                VideoSource source = m_sources.get(sourceName);
+                if (source == null) {
+                  return;
+                }
+
+                // get subkey
+                relativeKey = relativeKey.substring(subKeyIndex + 1);
+
+                // handle standard names
+                String propName;
+                if ("mode".equals(relativeKey)) {
+                  // reset to current mode
+                  event.getEntry().setString(videoModeToString(source.getVideoMode()));
+                  return;
+                } else if (relativeKey.startsWith("Property/")) {
+                  propName = relativeKey.substring(9);
+                } else if (relativeKey.startsWith("RawProperty/")) {
+                  propName = relativeKey.substring(12);
+                } else {
+                  return; // ignore
+                }
+
+                // everything else is a property
+                VideoProperty property = source.getProperty(propName);
+                switch (property.getKind()) {
+                  case kNone:
+                    return;
+                  case kBoolean:
+                    // reset to current setting
+                    event.getEntry().setBoolean(property.get() != 0);
+                    return;
+                  case kInteger:
+                  case kEnum:
+                    // reset to current setting
+                    event.getEntry().setDouble(property.get());
+                    return;
+                  case kString:
+                    // reset to current setting
+                    event.getEntry().setString(property.getString());
+                    return;
+                  default:
+                    return;
+                }
+              },
+              EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
+  private static int m_nextPort = kBasePort;
+  private static String[] m_addresses = new String[0];
+
+  @SuppressWarnings("MissingJavadocMethod")
   private static String makeSourceValue(int source) {
     switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
       case kUsb:
         return "usb:" + CameraServerJNI.getUsbCameraPath(source);
-      case kHttp: {
-        String[] urls = CameraServerJNI.getHttpCameraUrls(source);
-        if (urls.length > 0) {
-          return "ip:" + urls[0];
-        } else {
-          return "ip:";
+      case kHttp:
+        {
+          String[] urls = CameraServerJNI.getHttpCameraUrls(source);
+          if (urls.length > 0) {
+            return "ip:" + urls[0];
+          } else {
+            return "ip:";
+          }
         }
-      }
       case kCv:
         return "cv:";
       default:
@@ -93,13 +285,13 @@
     }
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   private static String makeStreamValue(String address, int port) {
     return "mjpg:http://" + address + ":" + port + "/?action=stream";
   }
 
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
-  private synchronized String[] getSinkStreamValues(int sink) {
+  @SuppressWarnings("MissingJavadocMethod")
+  private static synchronized String[] getSinkStreamValues(int sink) {
     // Ignore all but MjpegServer
     if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
       return new String[0];
@@ -119,7 +311,7 @@
       values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
       for (String addr : m_addresses) {
         if ("127.0.0.1".equals(addr)) {
-          continue;  // ignore localhost
+          continue; // ignore localhost
         }
         values.add(makeStreamValue(addr, port));
       }
@@ -128,11 +320,11 @@
     return values.toArray(new String[0]);
   }
 
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
-  private synchronized String[] getSourceStreamValues(int source) {
+  @SuppressWarnings("MissingJavadocMethod")
+  private static synchronized String[] getSourceStreamValues(int source) {
     // Ignore all but HttpCamera
     if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
-            != VideoSource.Kind.kHttp) {
+        != VideoSource.Kind.kHttp) {
       return new String[0];
     }
 
@@ -150,7 +342,7 @@
         int sinkSource = CameraServerJNI.getSinkSource(sink);
         if (source == sinkSource
             && VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink))
-            == VideoSink.Kind.kMjpeg) {
+                == VideoSink.Kind.kMjpeg) {
           // Add USB-only passthrough
           String[] finalValues = Arrays.copyOf(values, values.length + 1);
           int port = CameraServerJNI.getMjpegServerPort(sink);
@@ -163,15 +355,16 @@
     return values;
   }
 
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP", "PMD.CyclomaticComplexity"})
-  private synchronized void updateStreamValues() {
+  @SuppressWarnings("MissingJavadocMethod")
+  private static synchronized void updateStreamValues() {
     // Over all the sinks...
     for (VideoSink i : m_sinks.values()) {
       int sink = i.getHandle();
 
       // Get the source's subtable (if none exists, we're done)
-      int source = Objects.requireNonNullElseGet(m_fixedSources.get(sink),
-          () -> CameraServerJNI.getSinkSource(sink));
+      int source =
+          Objects.requireNonNullElseGet(
+              m_fixedSources.get(sink), () -> CameraServerJNI.getSinkSource(sink));
 
       if (source == 0) {
         continue;
@@ -208,7 +401,7 @@
     }
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   private static String pixelFormatToString(PixelFormat pixelFormat) {
     switch (pixelFormat) {
       case kMJPEG:
@@ -228,13 +421,19 @@
 
   /// Provide string description of video mode.
   /// The returned string is "{width}x{height} {format} {fps} fps".
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   private static String videoModeToString(VideoMode mode) {
-    return mode.width + "x" + mode.height + " " + pixelFormatToString(mode.pixelFormat)
-        + " " + mode.fps + " fps";
+    return mode.width
+        + "x"
+        + mode.height
+        + " "
+        + pixelFormatToString(mode.pixelFormat)
+        + " "
+        + mode.fps
+        + " fps";
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   private static String[] getSourceModeValues(int sourceHandle) {
     VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
     String[] modeStrings = new String[modes.length];
@@ -244,7 +443,7 @@
     return modeStrings;
   }
 
-  @SuppressWarnings({"JavadocMethod", "PMD.CyclomaticComplexity"})
+  @SuppressWarnings("MissingJavadocMethod")
   private static void putSourcePropertyValue(NetworkTable table, VideoEvent event, boolean isNew) {
     String name;
     String infoName;
@@ -270,14 +469,18 @@
         case kEnum:
           if (isNew) {
             entry.setDefaultDouble(event.value);
-            table.getEntry(infoName + "/min").setDouble(
-                CameraServerJNI.getPropertyMin(event.propertyHandle));
-            table.getEntry(infoName + "/max").setDouble(
-                CameraServerJNI.getPropertyMax(event.propertyHandle));
-            table.getEntry(infoName + "/step").setDouble(
-                CameraServerJNI.getPropertyStep(event.propertyHandle));
-            table.getEntry(infoName + "/default").setDouble(
-                CameraServerJNI.getPropertyDefault(event.propertyHandle));
+            table
+                .getEntry(infoName + "/min")
+                .setDouble(CameraServerJNI.getPropertyMin(event.propertyHandle));
+            table
+                .getEntry(infoName + "/max")
+                .setDouble(CameraServerJNI.getPropertyMax(event.propertyHandle));
+            table
+                .getEntry(infoName + "/step")
+                .setDouble(CameraServerJNI.getPropertyStep(event.propertyHandle));
+            table
+                .getEntry(infoName + "/default")
+                .setDouble(CameraServerJNI.getPropertyDefault(event.propertyHandle));
           } else {
             entry.setDouble(event.value);
           }
@@ -297,203 +500,21 @@
     }
   }
 
-  @SuppressWarnings({"JavadocMethod", "PMD.UnusedLocalVariable", "PMD.ExcessiveMethodLength",
-      "PMD.NPathComplexity"})
-  private CameraServer() {
-    m_defaultUsbDevice = new AtomicInteger();
-    m_sources = new HashMap<>();
-    m_sinks = new HashMap<>();
-    m_fixedSources = new HashMap<>();
-    m_tables = new HashMap<>();
-    m_publishTable = NetworkTableInstance.getDefault().getTable(kPublishName);
-    m_nextPort = kBasePort;
-    m_addresses = new String[0];
-
-    // We publish sources to NetworkTables using the following structure:
-    // "/CameraPublisher/{Source.Name}/" - root
-    // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
-    // - "streams" (string array): URLs that can be used to stream data
-    // - "description" (string): Description of the source
-    // - "connected" (boolean): Whether source is connected
-    // - "mode" (string): Current video mode
-    // - "modes" (string array): Available video modes
-    // - "Property/{Property}" - Property values
-    // - "PropertyInfo/{Property}" - Property supporting information
-
-    // Listener for video events
-    m_videoListener = new VideoListener(event -> {
-      switch (event.kind) {
-        case kSourceCreated: {
-          // Create subtable for the camera
-          NetworkTable table = m_publishTable.getSubTable(event.name);
-          m_tables.put(event.sourceHandle, table);
-          table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
-          table.getEntry("description").setString(
-              CameraServerJNI.getSourceDescription(event.sourceHandle));
-          table.getEntry("connected").setBoolean(
-              CameraServerJNI.isSourceConnected(event.sourceHandle));
-          table.getEntry("streams").setStringArray(getSourceStreamValues(event.sourceHandle));
-          try {
-            VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
-            table.getEntry("mode").setDefaultString(videoModeToString(mode));
-            table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
-          } catch (VideoException ignored) {
-            // Do nothing. Let the other event handlers update this if there is an error.
-          }
-          break;
-        }
-        case kSourceDestroyed: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("source").setString("");
-            table.getEntry("streams").setStringArray(new String[0]);
-            table.getEntry("modes").setStringArray(new String[0]);
-          }
-          break;
-        }
-        case kSourceConnected: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            // update the description too (as it may have changed)
-            table.getEntry("description").setString(
-                CameraServerJNI.getSourceDescription(event.sourceHandle));
-            table.getEntry("connected").setBoolean(true);
-          }
-          break;
-        }
-        case kSourceDisconnected: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("connected").setBoolean(false);
-          }
-          break;
-        }
-        case kSourceVideoModesUpdated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
-          }
-          break;
-        }
-        case kSourceVideoModeChanged: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("mode").setString(videoModeToString(event.mode));
-          }
-          break;
-        }
-        case kSourcePropertyCreated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            putSourcePropertyValue(table, event, true);
-          }
-          break;
-        }
-        case kSourcePropertyValueUpdated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            putSourcePropertyValue(table, event, false);
-          }
-          break;
-        }
-        case kSourcePropertyChoicesUpdated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            try {
-              String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
-              table.getEntry("PropertyInfo/" + event.name + "/choices").setStringArray(choices);
-            } catch (VideoException ignored) {
-              // ignore
-            }
-          }
-          break;
-        }
-        case kSinkSourceChanged:
-        case kSinkCreated:
-        case kSinkDestroyed:
-        case kNetworkInterfacesChanged: {
-          m_addresses = CameraServerJNI.getNetworkInterfaces();
-          updateStreamValues();
-          break;
-        }
-        default:
-          break;
-      }
-    }, 0x4fff, true);
-
-    // Listener for NetworkTable events
-    // We don't currently support changing settings via NT due to
-    // synchronization issues, so just update to current setting if someone
-    // else tries to change it.
-    m_tableListener = NetworkTableInstance.getDefault().addEntryListener(kPublishName + "/",
-      event -> {
-        String relativeKey = event.name.substring(kPublishName.length() + 1);
-
-        // get source (sourceName/...)
-        int subKeyIndex = relativeKey.indexOf('/');
-        if (subKeyIndex == -1) {
-          return;
-        }
-        String sourceName = relativeKey.substring(0, subKeyIndex);
-        VideoSource source = m_sources.get(sourceName);
-        if (source == null) {
-          return;
-        }
-
-        // get subkey
-        relativeKey = relativeKey.substring(subKeyIndex + 1);
-
-        // handle standard names
-        String propName;
-        if ("mode".equals(relativeKey)) {
-          // reset to current mode
-          event.getEntry().setString(videoModeToString(source.getVideoMode()));
-          return;
-        } else if (relativeKey.startsWith("Property/")) {
-          propName = relativeKey.substring(9);
-        } else if (relativeKey.startsWith("RawProperty/")) {
-          propName = relativeKey.substring(12);
-        } else {
-          return;  // ignore
-        }
-
-        // everything else is a property
-        VideoProperty property = source.getProperty(propName);
-        switch (property.getKind()) {
-          case kNone:
-            return;
-          case kBoolean:
-            // reset to current setting
-            event.getEntry().setBoolean(property.get() != 0);
-            return;
-          case kInteger:
-          case kEnum:
-            // reset to current setting
-            event.getEntry().setDouble(property.get());
-            return;
-          case kString:
-            // reset to current setting
-            event.getEntry().setString(property.getString());
-            return;
-          default:
-            return;
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
-  }
+  private CameraServer() {}
 
   /**
    * Start automatically capturing images to send to the dashboard.
    *
-   * <p>You should call this method to see a camera feed on the dashboard.
-   * If you also want to perform vision processing on the roboRIO, use
-   * getVideo() to get access to the camera images.
+   * <p>You should call this method to see a camera feed on the dashboard. If you also want to
+   * perform vision processing on the roboRIO, use getVideo() to get access to the camera images.
    *
-   * <p>The first time this overload is called, it calls
-   * {@link #startAutomaticCapture(int)} with device 0, creating a camera
-   * named "USB Camera 0".  Subsequent calls increment the device number
+   * <p>The first time this overload is called, it calls {@link #startAutomaticCapture(int)} with
+   * device 0, creating a camera named "USB Camera 0". Subsequent calls increment the device number
    * (e.g. 1, 2, etc).
+   *
+   * @return The USB camera capturing images.
    */
-  public UsbCamera startAutomaticCapture() {
+  public static UsbCamera startAutomaticCapture() {
     UsbCamera camera = startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
     CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
     return camera;
@@ -502,12 +523,13 @@
   /**
    * Start automatically capturing images to send to the dashboard.
    *
-   * <p>This overload calls {@link #startAutomaticCapture(String, int)} with
-   * a name of "USB Camera {dev}".
+   * <p>This overload calls {@link #startAutomaticCapture(String, int)} with a name of "USB Camera
+   * {dev}".
    *
    * @param dev The device number of the camera interface
+   * @return The USB camera capturing images.
    */
-  public UsbCamera startAutomaticCapture(int dev) {
+  public static UsbCamera startAutomaticCapture(int dev) {
     UsbCamera camera = new UsbCamera("USB Camera " + dev, dev);
     startAutomaticCapture(camera);
     CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
@@ -519,8 +541,9 @@
    *
    * @param name The name to give the camera
    * @param dev The device number of the camera interface
+   * @return The USB camera capturing images.
    */
-  public UsbCamera startAutomaticCapture(String name, int dev) {
+  public static UsbCamera startAutomaticCapture(String name, int dev) {
     UsbCamera camera = new UsbCamera(name, dev);
     startAutomaticCapture(camera);
     CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
@@ -532,8 +555,9 @@
    *
    * @param name The name to give the camera
    * @param path The device path (e.g. "/dev/video0") of the camera
+   * @return The USB camera capturing images.
    */
-  public UsbCamera startAutomaticCapture(String name, String path) {
+  public static UsbCamera startAutomaticCapture(String name, String path) {
     UsbCamera camera = new UsbCamera(name, path);
     startAutomaticCapture(camera);
     CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
@@ -541,12 +565,12 @@
   }
 
   /**
-   * Start automatically capturing images to send to the dashboard from
-   * an existing camera.
+   * Start automatically capturing images to send to the dashboard from an existing camera.
    *
    * @param camera Camera
+   * @return The MJPEG server serving images from the given camera.
    */
-  public MjpegServer startAutomaticCapture(VideoSource camera) {
+  public static MjpegServer startAutomaticCapture(VideoSource camera) {
     addCamera(camera);
     MjpegServer server = addServer("serve_" + camera.getName());
     server.setSource(camera);
@@ -556,24 +580,24 @@
   /**
    * Adds an Axis IP camera.
    *
-   * <p>This overload calls {@link #addAxisCamera(String, String)} with
-   * name "Axis Camera".
+   * <p>This overload calls {@link #addAxisCamera(String, String)} with name "Axis Camera".
    *
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+   * @return The Axis camera capturing images.
    */
-  public AxisCamera addAxisCamera(String host) {
+  public static AxisCamera addAxisCamera(String host) {
     return addAxisCamera("Axis Camera", host);
   }
 
   /**
    * Adds an Axis IP camera.
    *
-   * <p>This overload calls {@link #addAxisCamera(String, String[])} with
-   * name "Axis Camera".
+   * <p>This overload calls {@link #addAxisCamera(String, String[])} with name "Axis Camera".
    *
    * @param hosts Array of Camera host IPs/DNS names
+   * @return The Axis camera capturing images.
    */
-  public AxisCamera addAxisCamera(String[] hosts) {
+  public static AxisCamera addAxisCamera(String[] hosts) {
     return addAxisCamera("Axis Camera", hosts);
   }
 
@@ -582,8 +606,9 @@
    *
    * @param name The name to give the camera
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+   * @return The Axis camera capturing images.
    */
-  public AxisCamera addAxisCamera(String name, String host) {
+  public static AxisCamera addAxisCamera(String name, String host) {
     AxisCamera camera = new AxisCamera(name, host);
     // Create a passthrough MJPEG server for USB access
     startAutomaticCapture(camera);
@@ -596,8 +621,9 @@
    *
    * @param name The name to give the camera
    * @param hosts Array of Camera host IPs/DNS names
+   * @return The Axis camera capturing images.
    */
-  public AxisCamera addAxisCamera(String name, String[] hosts) {
+  public static AxisCamera addAxisCamera(String name, String[] hosts) {
     AxisCamera camera = new AxisCamera(name, hosts);
     // Create a passthrough MJPEG server for USB access
     startAutomaticCapture(camera);
@@ -606,16 +632,18 @@
   }
 
   /**
-   * Adds a virtual camera for switching between two streams.  Unlike the
-   * other addCamera methods, this returns a VideoSink rather than a
-   * VideoSource.  Calling setSource() on the returned object can be used
-   * to switch the actual source of the stream.
+   * Adds a virtual camera for switching between two streams. Unlike the other addCamera methods,
+   * this returns a VideoSink rather than a VideoSource. Calling setSource() on the returned object
+   * can be used to switch the actual source of the stream.
+   *
+   * @param name The name to give the camera
+   * @return The MJPEG server serving images from the given camera.
    */
-  public MjpegServer addSwitchedCamera(String name) {
+  public static MjpegServer addSwitchedCamera(String name) {
     // create a dummy CvSource
     CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, 160, 120, 30);
     MjpegServer server = startAutomaticCapture(source);
-    synchronized (this) {
+    synchronized (CameraServer.class) {
       m_fixedSources.put(server.getHandle(), source.getHandle());
     }
 
@@ -623,15 +651,17 @@
   }
 
   /**
-   * Get OpenCV access to the primary camera feed.  This allows you to
-   * get images from the camera for image processing on the roboRIO.
+   * Get OpenCV access to the primary camera feed. This allows you to get images from the camera for
+   * image processing on the roboRIO.
    *
-   * <p>This is only valid to call after a camera feed has been added
-   * with startAutomaticCapture() or addServer().
+   * <p>This is only valid to call after a camera feed has been added with startAutomaticCapture()
+   * or addServer().
+   *
+   * @return OpenCV sink for the primary camera feed
    */
-  public CvSink getVideo() {
+  public static CvSink getVideo() {
     VideoSource source;
-    synchronized (this) {
+    synchronized (CameraServer.class) {
       if (m_primarySourceName == null) {
         throw new VideoException("no camera available");
       }
@@ -644,15 +674,16 @@
   }
 
   /**
-   * Get OpenCV access to the specified camera.  This allows you to get
-   * images from the camera for image processing on the roboRIO.
+   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
+   * image processing on the roboRIO.
    *
    * @param camera Camera (e.g. as returned by startAutomaticCapture).
+   * @return OpenCV sink for the specified camera
    */
-  public CvSink getVideo(VideoSource camera) {
+  public static CvSink getVideo(VideoSource camera) {
     String name = "opencv_" + camera.getName();
 
-    synchronized (this) {
+    synchronized (CameraServer.class) {
       VideoSink sink = m_sinks.get(name);
       if (sink != null) {
         VideoSink.Kind kind = sink.getKind();
@@ -670,14 +701,15 @@
   }
 
   /**
-   * Get OpenCV access to the specified camera.  This allows you to get
-   * images from the camera for image processing on the roboRIO.
+   * Get OpenCV access to the specified camera. This allows you to get images from the camera for
+   * image processing on the roboRIO.
    *
    * @param name Camera name
+   * @return OpenCV sink for the specified camera
    */
-  public CvSink getVideo(String name) {
+  public static CvSink getVideo(String name) {
     VideoSource source;
-    synchronized (this) {
+    synchronized (CameraServer.class) {
       source = m_sources.get(name);
       if (source == null) {
         throw new VideoException("could not find camera " + name);
@@ -687,14 +719,15 @@
   }
 
   /**
-   * Create a MJPEG stream with OpenCV input. This can be called to pass custom
-   * annotated images to the dashboard.
+   * Create a MJPEG stream with OpenCV input. This can be called to pass custom annotated images to
+   * the dashboard.
    *
    * @param name Name to give the stream
    * @param width Width of the image being sent
    * @param height Height of the image being sent
+   * @return OpenCV source for the MJPEG stream
    */
-  public CvSource putVideo(String name, int width, int height) {
+  public static CvSource putVideo(String name, int width, int height) {
     CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
     startAutomaticCapture(source);
     return source;
@@ -704,10 +737,11 @@
    * Adds a MJPEG server at the next available port.
    *
    * @param name Server name
+   * @return The MJPEG server
    */
-  public MjpegServer addServer(String name) {
+  public static MjpegServer addServer(String name) {
     int port;
-    synchronized (this) {
+    synchronized (CameraServer.class) {
       port = m_nextPort;
       m_nextPort++;
     }
@@ -718,8 +752,10 @@
    * Adds a MJPEG server.
    *
    * @param name Server name
+   * @param port Server port
+   * @return The MJPEG server
    */
-  public MjpegServer addServer(String name, int port) {
+  public static MjpegServer addServer(String name, int port) {
     MjpegServer server = new MjpegServer(name, port);
     addServer(server);
     return server;
@@ -730,8 +766,8 @@
    *
    * @param server Server
    */
-  public void addServer(VideoSink server) {
-    synchronized (this) {
+  public static void addServer(VideoSink server) {
+    synchronized (CameraServer.class) {
       m_sinks.put(server.getName(), server);
     }
   }
@@ -741,8 +777,8 @@
    *
    * @param name Server name
    */
-  public void removeServer(String name) {
-    synchronized (this) {
+  public static void removeServer(String name) {
+    synchronized (CameraServer.class) {
       m_sinks.remove(name);
     }
   }
@@ -750,11 +786,13 @@
   /**
    * Get server for the primary camera feed.
    *
-   * <p>This is only valid to call after a camera feed has been added
-   * with startAutomaticCapture() or addServer().
+   * <p>This is only valid to call after a camera feed has been added with startAutomaticCapture()
+   * or addServer().
+   *
+   * @return The server for the primary camera feed
    */
-  public VideoSink getServer() {
-    synchronized (this) {
+  public static VideoSink getServer() {
+    synchronized (CameraServer.class) {
       if (m_primarySourceName == null) {
         throw new VideoException("no camera available");
       }
@@ -766,9 +804,10 @@
    * Gets a server by name.
    *
    * @param name Server name
+   * @return The server
    */
-  public VideoSink getServer(String name) {
-    synchronized (this) {
+  public static VideoSink getServer(String name) {
+    synchronized (CameraServer.class) {
       return m_sinks.get(name);
     }
   }
@@ -778,9 +817,9 @@
    *
    * @param camera Camera
    */
-  public void addCamera(VideoSource camera) {
+  public static void addCamera(VideoSource camera) {
     String name = camera.getName();
-    synchronized (this) {
+    synchronized (CameraServer.class) {
       if (m_primarySourceName == null) {
         m_primarySourceName = name;
       }
@@ -793,8 +832,8 @@
    *
    * @param name Camera name
    */
-  public void removeCamera(String name) {
-    synchronized (this) {
+  public static void removeCamera(String name) {
+    synchronized (CameraServer.class) {
       m_sources.remove(name);
     }
   }
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java
index c9cbb8f..4726de1 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerShared.java
@@ -1,13 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.cameraserver;
 
-
 public interface CameraServerShared {
   /**
    * get the main thread id func.
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java
index c0cf2bb..3d9e119 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/cameraserver/CameraServerSharedStore.java
@@ -1,56 +1,48 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.cameraserver;
 
 public final class CameraServerSharedStore {
   private static CameraServerShared cameraServerShared;
 
-  private CameraServerSharedStore() {
-  }
+  private CameraServerSharedStore() {}
 
   /**
-   * get the CameraServerShared object.
+   * Get the CameraServerShared object.
+   *
+   * @return The CameraServerSharedObject
    */
   public static synchronized CameraServerShared getCameraServerShared() {
     if (cameraServerShared == null) {
-      cameraServerShared = new CameraServerShared() {
+      cameraServerShared =
+          new CameraServerShared() {
+            @Override
+            public void reportVideoServer(int id) {}
 
-        @Override
-        public void reportVideoServer(int id) {
+            @Override
+            public void reportUsbCamera(int id) {}
 
-        }
+            @Override
+            public void reportDriverStationError(String error) {}
 
-        @Override
-        public void reportUsbCamera(int id) {
+            @Override
+            public void reportAxisCamera(int id) {}
 
-        }
-
-        @Override
-        public void reportDriverStationError(String error) {
-
-        }
-
-        @Override
-        public void reportAxisCamera(int id) {
-
-        }
-
-        @Override
-        public Long getRobotMainThreadId() {
-          return null;
-        }
-      };
+            @Override
+            public Long getRobotMainThreadId() {
+              return null;
+            }
+          };
     }
     return cameraServerShared;
   }
 
   /**
-   * set the CameraServerShared object.
+   * Set the CameraServerShared object.
+   *
+   * @param shared The CameraServerShared object.
    */
   public static synchronized void setCameraServerShared(CameraServerShared shared) {
     cameraServerShared = shared;
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionPipeline.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionPipeline.java
index 6df10e7..29c285f 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionPipeline.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionPipeline.java
@@ -1,26 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.vision;
 
 import org.opencv.core.Mat;
 
 /**
- * A vision pipeline is responsible for running a group of
- * OpenCV algorithms to extract data from an image.
+ * A vision pipeline is responsible for running a group of OpenCV algorithms to extract data from an
+ * image.
  *
  * @see VisionRunner
  * @see VisionThread
  */
 public interface VisionPipeline {
   /**
-   * Processes the image input and sets the result objects.
-   * Implementations should make these objects accessible.
+   * Processes the image input and sets the result objects. Implementations should make these
+   * objects accessible.
+   *
+   * @param image The image to process.
    */
   void process(Mat image);
-
 }
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java
index 8d8b12e..ab7072f 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionRunner.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.vision;
 
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.VideoSource;
 import org.opencv.core.Mat;
 
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.cameraserver.CameraServerSharedStore;
-
 /**
- * A vision runner is a convenient wrapper object to make it easy to run vision pipelines
- * from robot code. The easiest  way to use this is to run it in a {@link VisionThread}
- * and use the listener to take snapshots of the pipeline's outputs.
+ * A vision runner is a convenient wrapper object to make it easy to run vision pipelines from robot
+ * code. The easiest way to use this is to run it in a {@link VisionThread} and use the listener to
+ * take snapshots of the pipeline's outputs.
  *
  * @see VisionPipeline
  * @see VisionThread
@@ -45,17 +41,16 @@
      * @param pipeline the vision pipeline that ran
      */
     void copyPipelineOutputs(P pipeline);
-
   }
 
   /**
-   * Creates a new vision runner. It will take images from the {@code videoSource}, send them to
-   * the {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert
-   * user code when it is safe to access the pipeline's outputs.
+   * Creates a new vision runner. It will take images from the {@code videoSource}, send them to the
+   * {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert user
+   * code when it is safe to access the pipeline's outputs.
    *
    * @param videoSource the video source to use to supply images for the pipeline
-   * @param pipeline    the vision pipeline to run
-   * @param listener    a function to call after the pipeline has finished running
+   * @param pipeline the vision pipeline to run
+   * @param listener a function to call after the pipeline has finished running
    */
   public VisionRunner(VideoSource videoSource, P pipeline, Listener<? super P> listener) {
     this.m_pipeline = pipeline;
@@ -64,15 +59,15 @@
   }
 
   /**
-   * Runs the pipeline one time, giving it the next image from the video source specified
-   * in the constructor. This will block until the source either has an image or throws an error.
-   * If the source successfully supplied a frame, the pipeline's image input will be set,
-   * the pipeline will run, and the listener specified in the constructor will be called to notify
-   * it that the pipeline ran.
+   * Runs the pipeline one time, giving it the next image from the video source specified in the
+   * constructor. This will block until the source either has an image or throws an error. If the
+   * source successfully supplied a frame, the pipeline's image input will be set, the pipeline will
+   * run, and the listener specified in the constructor will be called to notify it that the
+   * pipeline ran.
    *
-   * <p>This method is exposed to allow teams to add additional functionality or have their own
-   * ways to run the pipeline. Most teams, however, should just use {@link #runForever} in its own
-   * thread using a {@link VisionThread}.</p>
+   * <p>This method is exposed to allow teams to add additional functionality or have their own ways
+   * to run the pipeline. Most teams, however, should just use {@link #runForever} in its own thread
+   * using a {@link VisionThread}.
    */
   public void runOnce() {
     Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId();
@@ -98,11 +93,11 @@
   }
 
   /**
-   * A convenience method that calls {@link #runOnce()} in an infinite loop. This must
-   * be run in a dedicated thread, and cannot be used in the main robot thread because
-   * it will freeze the robot program.
+   * A convenience method that calls {@link #runOnce()} in an infinite loop. This must be run in a
+   * dedicated thread, and cannot be used in the main robot thread because it will freeze the robot
+   * program.
    *
-   * <p><strong>Do not call this method directly from the main thread.</strong></p>
+   * <p><strong>Do not call this method directly from the main thread.</strong>
    *
    * @throws IllegalStateException if this is called from the main robot thread
    * @see VisionThread
@@ -119,9 +114,7 @@
     }
   }
 
-  /**
-   * Stop a RunForever() loop.
-   */
+  /** Stop a RunForever() loop. */
   public void stop() {
     m_enabled = false;
   }
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java
index 576cb96..6f1a1e3 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/VisionThread.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.vision;
 
-import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cscore.VideoSource;
 
 /**
- * A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread;
- * it does not prevent the program from exiting when all other non-daemon threads
- * have finished running.
+ * A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread; it
+ * does not prevent the program from exiting when all other non-daemon threads have finished
+ * running.
  *
  * @see VisionPipeline
  * @see VisionRunner
@@ -34,14 +31,12 @@
    * equivalent to {@code new VisionThread(new VisionRunner<>(videoSource, pipeline, listener))}.
    *
    * @param videoSource the source for images the pipeline should process
-   * @param pipeline    the pipeline to run
-   * @param listener    the listener to copy outputs from the pipeline after it runs
-   * @param <P>         the type of the pipeline
+   * @param pipeline the pipeline to run
+   * @param listener the listener to copy outputs from the pipeline after it runs
+   * @param <P> the type of the pipeline
    */
-  public <P extends VisionPipeline> VisionThread(VideoSource videoSource,
-                                                 P pipeline,
-                                                 VisionRunner.Listener<? super P> listener) {
+  public <P extends VisionPipeline> VisionThread(
+      VideoSource videoSource, P pipeline, VisionRunner.Listener<? super P> listener) {
     this(new VisionRunner<>(videoSource, pipeline, listener));
   }
-
 }
diff --git a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/package-info.java b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/package-info.java
index e2e5c62..3715570 100644
--- a/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/package-info.java
+++ b/third_party/allwpilib/cameraserver/src/main/java/edu/wpi/first/vision/package-info.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /**
- * Classes in the {@code edu.wpi.first.vision} package are designed to
- * simplify using OpenCV vision processing code from a robot program.
+ * Classes in the {@code edu.wpi.first.vision} package are designed to simplify using OpenCV vision
+ * processing code from a robot program.
  *
- * <p>An example use case for grabbing a yellow tote from 2015 in autonomous:
- * <br>
+ * <p>An example use case for grabbing a yellow tote from 2015 in autonomous: <br>
+ *
  * <pre><code>
  * public class Robot extends IterativeRobot
  *     implements VisionRunner.Listener&lt;MyFindTotePipeline&gt; {
  *
  *      // A USB camera connected to the roboRIO.
- *      private {@link edu.wpi.cscore.VideoSource VideoSource} usbCamera;
+ *      private {@link edu.wpi.first.cscore.VideoSource VideoSource} usbCamera;
  *
  *      // A vision pipeline. This could be handwritten or generated by GRIP.
  *      // This has to implement {@link edu.wpi.first.vision.VisionPipeline}.
@@ -47,7 +44,7 @@
  *
  *     {@literal @}Override
  *      public void robotInit() {
- *          usbCamera = CameraServer.getInstance().startAutomaticCapture(0);
+ *          usbCamera = CameraServer.startAutomaticCapture(0);
  *          findTotePipeline = new MyFindTotePipeline();
  *          findToteThread = new VisionThread(usbCamera, findTotePipeline, this);
  *      }
diff --git a/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp b/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
index f7647de..1a4d42a 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
+++ b/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServer.cpp
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cameraserver/CameraServer.h"
 
 #include <atomic>
 #include <vector>
 
+#include <fmt/format.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/DenseMap.h>
-#include <wpi/ManagedStatic.h>
 #include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
 #include <wpi/StringMap.h>
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 
 #include "cameraserver/CameraServerShared.h"
 #include "ntcore_cpp.h"
@@ -26,8 +23,9 @@
 
 static constexpr char const* kPublishName = "/CameraPublisher";
 
-struct CameraServer::Impl {
-  Impl();
+namespace {
+struct Instance {
+  Instance();
   std::shared_ptr<nt::NetworkTable> GetSourceTable(CS_Source source);
   std::vector<std::string> GetSinkStreamValues(CS_Sink sink);
   std::vector<std::string> GetSourceStreamValues(CS_Source source);
@@ -40,41 +38,45 @@
   wpi::StringMap<cs::VideoSink> m_sinks;
   wpi::DenseMap<CS_Sink, CS_Source> m_fixedSources;
   wpi::DenseMap<CS_Source, std::shared_ptr<nt::NetworkTable>> m_tables;
-  std::shared_ptr<nt::NetworkTable> m_publishTable;
+  std::shared_ptr<nt::NetworkTable> m_publishTable{
+      nt::NetworkTableInstance::GetDefault().GetTable(kPublishName)};
   cs::VideoListener m_videoListener;
   int m_tableListener;
-  int m_nextPort;
+  int m_nextPort{CameraServer::kBasePort};
   std::vector<std::string> m_addresses;
 };
+}  // namespace
 
-CameraServer* CameraServer::GetInstance() {
-  struct Creator {
-    static void* call() { return new CameraServer{}; }
-  };
-  struct Deleter {
-    static void call(void* ptr) { delete static_cast<CameraServer*>(ptr); }
-  };
-  static wpi::ManagedStatic<CameraServer, Creator, Deleter> instance;
-  return &(*instance);
+static Instance& GetInstance() {
+  static Instance instance;
+  return instance;
 }
 
-static wpi::StringRef MakeSourceValue(CS_Source source,
-                                      wpi::SmallVectorImpl<char>& buf) {
+CameraServer* CameraServer::GetInstance() {
+  ::GetInstance();
+  static CameraServer instance;
+  return &instance;
+}
+
+static std::string_view MakeSourceValue(CS_Source source,
+                                        wpi::SmallVectorImpl<char>& buf) {
   CS_Status status = 0;
   buf.clear();
   switch (cs::GetSourceKind(source, &status)) {
     case CS_SOURCE_USB: {
-      wpi::StringRef prefix{"usb:"};
+      std::string_view prefix{"usb:"};
       buf.append(prefix.begin(), prefix.end());
       auto path = cs::GetUsbCameraPath(source, &status);
       buf.append(path.begin(), path.end());
       break;
     }
     case CS_SOURCE_HTTP: {
-      wpi::StringRef prefix{"ip:"};
+      std::string_view prefix{"ip:"};
       buf.append(prefix.begin(), prefix.end());
       auto urls = cs::GetHttpCameraUrls(source, &status);
-      if (!urls.empty()) buf.append(urls[0].begin(), urls[0].end());
+      if (!urls.empty()) {
+        buf.append(urls[0].begin(), urls[0].end());
+      }
       break;
     }
     case CS_SOURCE_CV:
@@ -83,27 +85,25 @@
       return "unknown:";
   }
 
-  return wpi::StringRef{buf.begin(), buf.size()};
+  return {buf.begin(), buf.size()};
 }
 
-static std::string MakeStreamValue(const wpi::Twine& address, int port) {
-  return ("mjpg:http://" + address + wpi::Twine(':') + wpi::Twine(port) +
-          "/?action=stream")
-      .str();
+static std::string MakeStreamValue(std::string_view address, int port) {
+  return fmt::format("mjpg:http://{}:{}/?action=stream", address, port);
 }
 
-std::shared_ptr<nt::NetworkTable> CameraServer::Impl::GetSourceTable(
-    CS_Source source) {
+std::shared_ptr<nt::NetworkTable> Instance::GetSourceTable(CS_Source source) {
   std::scoped_lock lock(m_mutex);
   return m_tables.lookup(source);
 }
 
-std::vector<std::string> CameraServer::Impl::GetSinkStreamValues(CS_Sink sink) {
+std::vector<std::string> Instance::GetSinkStreamValues(CS_Sink sink) {
   CS_Status status = 0;
 
   // Ignore all but MjpegServer
-  if (cs::GetSinkKind(sink, &status) != CS_SINK_MJPEG)
-    return std::vector<std::string>{};
+  if (cs::GetSinkKind(sink, &status) != CS_SINK_MJPEG) {
+    return {};
+  }
 
   // Get port
   int port = cs::GetMjpegServerPort(sink, &status);
@@ -119,7 +119,9 @@
     values.emplace_back(MakeStreamValue(cs::GetHostname() + ".local", port));
 
     for (const auto& addr : m_addresses) {
-      if (addr == "127.0.0.1") continue;  // ignore localhost
+      if (addr == "127.0.0.1") {
+        continue;  // ignore localhost
+      }
       values.emplace_back(MakeStreamValue(addr, port));
     }
   }
@@ -127,17 +129,19 @@
   return values;
 }
 
-std::vector<std::string> CameraServer::Impl::GetSourceStreamValues(
-    CS_Source source) {
+std::vector<std::string> Instance::GetSourceStreamValues(CS_Source source) {
   CS_Status status = 0;
 
   // Ignore all but HttpCamera
-  if (cs::GetSourceKind(source, &status) != CS_SOURCE_HTTP)
-    return std::vector<std::string>{};
+  if (cs::GetSourceKind(source, &status) != CS_SOURCE_HTTP) {
+    return {};
+  }
 
   // Generate values
   auto values = cs::GetHttpCameraUrls(source, &status);
-  for (auto& value : values) value = "mjpg:" + value;
+  for (auto& value : values) {
+    value = "mjpg:" + value;
+  }
 
 #ifdef __FRC_ROBORIO__
   // Look to see if we have a passthrough server for this source
@@ -159,7 +163,7 @@
   return values;
 }
 
-void CameraServer::Impl::UpdateStreamValues() {
+void Instance::UpdateStreamValues() {
   std::scoped_lock lock(m_mutex);
   // Over all the sinks...
   for (const auto& i : m_sinks) {
@@ -168,16 +172,24 @@
 
     // Get the source's subtable (if none exists, we're done)
     CS_Source source = m_fixedSources.lookup(sink);
-    if (source == 0) source = cs::GetSinkSource(sink, &status);
-    if (source == 0) continue;
+    if (source == 0) {
+      source = cs::GetSinkSource(sink, &status);
+    }
+    if (source == 0) {
+      continue;
+    }
     auto table = m_tables.lookup(source);
     if (table) {
       // Don't set stream values if this is a HttpCamera passthrough
-      if (cs::GetSourceKind(source, &status) == CS_SOURCE_HTTP) continue;
+      if (cs::GetSourceKind(source, &status) == CS_SOURCE_HTTP) {
+        continue;
+      }
 
       // Set table value
       auto values = GetSinkStreamValues(sink);
-      if (!values.empty()) table->GetEntry("streams").SetStringArray(values);
+      if (!values.empty()) {
+        table->GetEntry("streams").SetStringArray(values);
+      }
     }
   }
 
@@ -190,7 +202,9 @@
     if (table) {
       // Set table value
       auto values = GetSourceStreamValues(source);
-      if (!values.empty()) table->GetEntry("streams").SetStringArray(values);
+      if (!values.empty()) {
+        table->GetEntry("streams").SetStringArray(values);
+      }
     }
   }
 }
@@ -213,79 +227,72 @@
 }
 
 static std::string VideoModeToString(const cs::VideoMode& mode) {
-  std::string rv;
-  wpi::raw_string_ostream oss{rv};
-  oss << mode.width << "x" << mode.height;
-  oss << " " << PixelFormatToString(mode.pixelFormat) << " ";
-  oss << mode.fps << " fps";
-  return oss.str();
+  return fmt::format("{}x{} {} {} fps", mode.width, mode.height,
+                     PixelFormatToString(mode.pixelFormat), mode.fps);
 }
 
 static std::vector<std::string> GetSourceModeValues(int source) {
   std::vector<std::string> rv;
   CS_Status status = 0;
-  for (const auto& mode : cs::EnumerateSourceVideoModes(source, &status))
+  for (const auto& mode : cs::EnumerateSourceVideoModes(source, &status)) {
     rv.emplace_back(VideoModeToString(mode));
+  }
   return rv;
 }
 
 static void PutSourcePropertyValue(nt::NetworkTable* table,
                                    const cs::VideoEvent& event, bool isNew) {
-  wpi::SmallString<64> name;
-  wpi::SmallString<64> infoName;
-  if (wpi::StringRef{event.name}.startswith("raw_")) {
-    name = "RawProperty/";
-    name += event.name;
-    infoName = "RawPropertyInfo/";
-    infoName += event.name;
+  std::string_view namePrefix;
+  std::string_view infoPrefix;
+  if (wpi::starts_with(event.name, "raw_")) {
+    namePrefix = "RawProperty";
+    infoPrefix = "RawPropertyInfo";
   } else {
-    name = "Property/";
-    name += event.name;
-    infoName = "PropertyInfo/";
-    infoName += event.name;
+    namePrefix = "Property";
+    infoPrefix = "PropertyInfo";
   }
 
   wpi::SmallString<64> buf;
   CS_Status status = 0;
-  nt::NetworkTableEntry entry = table->GetEntry(name);
+  nt::NetworkTableEntry entry =
+      table->GetEntry(fmt::format("{}/{}", namePrefix, event.name));
   switch (event.propertyKind) {
     case CS_PROP_BOOLEAN:
-      if (isNew)
+      if (isNew) {
         entry.SetDefaultBoolean(event.value != 0);
-      else
+      } else {
         entry.SetBoolean(event.value != 0);
+      }
       break;
     case CS_PROP_INTEGER:
     case CS_PROP_ENUM:
       if (isNew) {
         entry.SetDefaultDouble(event.value);
-        table->GetEntry(infoName + "/min")
+        table->GetEntry(fmt::format("{}/{}/min", infoPrefix, event.name))
             .SetDouble(cs::GetPropertyMin(event.propertyHandle, &status));
-        table->GetEntry(infoName + "/max")
+        table->GetEntry(fmt::format("{}/{}/max", infoPrefix, event.name))
             .SetDouble(cs::GetPropertyMax(event.propertyHandle, &status));
-        table->GetEntry(infoName + "/step")
+        table->GetEntry(fmt::format("{}/{}/step", infoPrefix, event.name))
             .SetDouble(cs::GetPropertyStep(event.propertyHandle, &status));
-        table->GetEntry(infoName + "/default")
+        table->GetEntry(fmt::format("{}/{}/default", infoPrefix, event.name))
             .SetDouble(cs::GetPropertyDefault(event.propertyHandle, &status));
       } else {
         entry.SetDouble(event.value);
       }
       break;
     case CS_PROP_STRING:
-      if (isNew)
+      if (isNew) {
         entry.SetDefaultString(event.valueStr);
-      else
+      } else {
         entry.SetString(event.valueStr);
+      }
       break;
     default:
       break;
   }
 }
 
-CameraServer::Impl::Impl()
-    : m_publishTable{nt::NetworkTableInstance::GetDefault().GetTable(
-          kPublishName)},
-      m_nextPort(kBasePort) {
+Instance::Instance() {
   // We publish sources to NetworkTables using the following structure:
   // "/CameraPublisher/{Source.Name}/" - root
   // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
@@ -351,41 +358,48 @@
           }
           case cs::VideoEvent::kSourceDisconnected: {
             auto table = GetSourceTable(event.sourceHandle);
-            if (table) table->GetEntry("connected").SetBoolean(false);
+            if (table) {
+              table->GetEntry("connected").SetBoolean(false);
+            }
             break;
           }
           case cs::VideoEvent::kSourceVideoModesUpdated: {
             auto table = GetSourceTable(event.sourceHandle);
-            if (table)
+            if (table) {
               table->GetEntry("modes").SetStringArray(
                   GetSourceModeValues(event.sourceHandle));
+            }
             break;
           }
           case cs::VideoEvent::kSourceVideoModeChanged: {
             auto table = GetSourceTable(event.sourceHandle);
-            if (table)
+            if (table) {
               table->GetEntry("mode").SetString(VideoModeToString(event.mode));
+            }
             break;
           }
           case cs::VideoEvent::kSourcePropertyCreated: {
             auto table = GetSourceTable(event.sourceHandle);
-            if (table) PutSourcePropertyValue(table.get(), event, true);
+            if (table) {
+              PutSourcePropertyValue(table.get(), event, true);
+            }
             break;
           }
           case cs::VideoEvent::kSourcePropertyValueUpdated: {
             auto table = GetSourceTable(event.sourceHandle);
-            if (table) PutSourcePropertyValue(table.get(), event, false);
+            if (table) {
+              PutSourcePropertyValue(table.get(), event, false);
+            }
             break;
           }
           case cs::VideoEvent::kSourcePropertyChoicesUpdated: {
             auto table = GetSourceTable(event.sourceHandle);
             if (table) {
-              wpi::SmallString<64> name{"PropertyInfo/"};
-              name += event.name;
-              name += "/choices";
               auto choices =
                   cs::GetEnumPropertyChoices(event.propertyHandle, &status);
-              table->GetEntry(name).SetStringArray(choices);
+              table
+                  ->GetEntry(fmt::format("PropertyInfo/{}/choices", event.name))
+                  .SetStringArray(choices);
             }
             break;
           }
@@ -409,31 +423,35 @@
   // else tries to change it.
   wpi::SmallString<64> buf;
   m_tableListener = nt::NetworkTableInstance::GetDefault().AddEntryListener(
-      kPublishName + wpi::Twine('/'),
+      fmt::format("{}/", kPublishName),
       [=](const nt::EntryNotification& event) {
-        wpi::StringRef relativeKey =
-            event.name.substr(wpi::StringRef(kPublishName).size() + 1);
+        auto relativeKey = wpi::drop_front(
+            event.name, std::string_view{kPublishName}.size() + 1);
 
         // get source (sourceName/...)
         auto subKeyIndex = relativeKey.find('/');
-        if (subKeyIndex == wpi::StringRef::npos) return;
-        wpi::StringRef sourceName = relativeKey.slice(0, subKeyIndex);
+        if (subKeyIndex == std::string_view::npos) {
+          return;
+        }
+        auto sourceName = wpi::slice(relativeKey, 0, subKeyIndex);
         auto sourceIt = m_sources.find(sourceName);
-        if (sourceIt == m_sources.end()) return;
+        if (sourceIt == m_sources.end()) {
+          return;
+        }
 
         // get subkey
-        relativeKey = relativeKey.substr(subKeyIndex + 1);
+        relativeKey.remove_prefix(subKeyIndex + 1);
 
         // handle standard names
-        wpi::StringRef propName;
+        std::string_view propName;
         nt::NetworkTableEntry entry{event.entry};
         if (relativeKey == "mode") {
           // reset to current mode
           entry.SetString(VideoModeToString(sourceIt->second.GetVideoMode()));
           return;
-        } else if (relativeKey.startswith("Property/")) {
+        } else if (wpi::starts_with(relativeKey, "Property/")) {
           propName = relativeKey.substr(9);
-        } else if (relativeKey.startswith("RawProperty/")) {
+        } else if (wpi::starts_with(relativeKey, "RawProperty/")) {
           propName = relativeKey.substr(12);
         } else {
           return;  // ignore
@@ -461,26 +479,23 @@
       NT_NOTIFY_IMMEDIATE | NT_NOTIFY_UPDATE);
 }
 
-CameraServer::CameraServer() : m_impl(new Impl) {}
-
-CameraServer::~CameraServer() {}
-
 cs::UsbCamera CameraServer::StartAutomaticCapture() {
-  cs::UsbCamera camera = StartAutomaticCapture(m_impl->m_defaultUsbDevice++);
+  cs::UsbCamera camera =
+      StartAutomaticCapture(::GetInstance().m_defaultUsbDevice++);
   auto csShared = GetCameraServerShared();
   csShared->ReportUsbCamera(camera.GetHandle());
   return camera;
 }
 
 cs::UsbCamera CameraServer::StartAutomaticCapture(int dev) {
-  cs::UsbCamera camera{"USB Camera " + wpi::Twine(dev), dev};
+  cs::UsbCamera camera{fmt::format("USB Camera {}", dev), dev};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
   csShared->ReportUsbCamera(camera.GetHandle());
   return camera;
 }
 
-cs::UsbCamera CameraServer::StartAutomaticCapture(const wpi::Twine& name,
+cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
                                                   int dev) {
   cs::UsbCamera camera{name, dev};
   StartAutomaticCapture(camera);
@@ -489,8 +504,8 @@
   return camera;
 }
 
-cs::UsbCamera CameraServer::StartAutomaticCapture(const wpi::Twine& name,
-                                                  const wpi::Twine& path) {
+cs::UsbCamera CameraServer::StartAutomaticCapture(std::string_view name,
+                                                  std::string_view path) {
   cs::UsbCamera camera{name, path};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -498,7 +513,7 @@
   return camera;
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(const wpi::Twine& host) {
+cs::AxisCamera CameraServer::AddAxisCamera(std::string_view host) {
   return AddAxisCamera("Axis Camera", host);
 }
 
@@ -510,12 +525,12 @@
   return AddAxisCamera("Axis Camera", host);
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(wpi::ArrayRef<std::string> hosts) {
+cs::AxisCamera CameraServer::AddAxisCamera(wpi::span<const std::string> hosts) {
   return AddAxisCamera("Axis Camera", hosts);
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(const wpi::Twine& name,
-                                           const wpi::Twine& host) {
+cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
+                                           std::string_view host) {
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -523,7 +538,7 @@
   return camera;
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(const wpi::Twine& name,
+cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const char* host) {
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
@@ -532,7 +547,7 @@
   return camera;
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(const wpi::Twine& name,
+cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
                                            const std::string& host) {
   cs::AxisCamera camera{name, host};
   StartAutomaticCapture(camera);
@@ -541,8 +556,8 @@
   return camera;
 }
 
-cs::AxisCamera CameraServer::AddAxisCamera(const wpi::Twine& name,
-                                           wpi::ArrayRef<std::string> hosts) {
+cs::AxisCamera CameraServer::AddAxisCamera(std::string_view name,
+                                           wpi::span<const std::string> hosts) {
   cs::AxisCamera camera{name, hosts};
   StartAutomaticCapture(camera);
   auto csShared = GetCameraServerShared();
@@ -550,11 +565,11 @@
   return camera;
 }
 
-cs::MjpegServer CameraServer::AddSwitchedCamera(const wpi::Twine& name) {
+cs::MjpegServer CameraServer::AddSwitchedCamera(std::string_view name) {
   // create a dummy CvSource
   cs::CvSource source{name, cs::VideoMode::PixelFormat::kMJPEG, 160, 120, 30};
   cs::MjpegServer server = StartAutomaticCapture(source);
-  m_impl->m_fixedSources[server.GetHandle()] = source.GetHandle();
+  ::GetInstance().m_fixedSources[server.GetHandle()] = source.GetHandle();
 
   return server;
 }
@@ -562,22 +577,23 @@
 cs::MjpegServer CameraServer::StartAutomaticCapture(
     const cs::VideoSource& camera) {
   AddCamera(camera);
-  auto server = AddServer(wpi::Twine("serve_") + camera.GetName());
+  auto server = AddServer(fmt::format("serve_{}", camera.GetName()));
   server.SetSource(camera);
   return server;
 }
 
 cs::CvSink CameraServer::GetVideo() {
+  auto& inst = ::GetInstance();
   cs::VideoSource source;
   {
     auto csShared = GetCameraServerShared();
-    std::scoped_lock lock(m_impl->m_mutex);
-    if (m_impl->m_primarySourceName.empty()) {
+    std::scoped_lock lock(inst.m_mutex);
+    if (inst.m_primarySourceName.empty()) {
       csShared->SetCameraServerError("no camera available");
       return cs::CvSink{};
     }
-    auto it = m_impl->m_sources.find(m_impl->m_primarySourceName);
-    if (it == m_impl->m_sources.end()) {
+    auto it = inst.m_sources.find(inst.m_primarySourceName);
+    if (it == inst.m_sources.end()) {
       csShared->SetCameraServerError("no camera available");
       return cs::CvSink{};
     }
@@ -587,40 +603,40 @@
 }
 
 cs::CvSink CameraServer::GetVideo(const cs::VideoSource& camera) {
+  auto& inst = ::GetInstance();
   wpi::SmallString<64> name{"opencv_"};
   name += camera.GetName();
 
   {
-    std::scoped_lock lock(m_impl->m_mutex);
-    auto it = m_impl->m_sinks.find(name);
-    if (it != m_impl->m_sinks.end()) {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sinks.find(name);
+    if (it != inst.m_sinks.end()) {
       auto kind = it->second.GetKind();
       if (kind != cs::VideoSink::kCv) {
         auto csShared = GetCameraServerShared();
-        csShared->SetCameraServerError("expected OpenCV sink, but got " +
-                                       wpi::Twine(kind));
+        csShared->SetCameraServerError("expected OpenCV sink, but got {}",
+                                       kind);
         return cs::CvSink{};
       }
       return *static_cast<cs::CvSink*>(&it->second);
     }
   }
 
-  cs::CvSink newsink{name};
+  cs::CvSink newsink{name.str()};
   newsink.SetSource(camera);
   AddServer(newsink);
   return newsink;
 }
 
-cs::CvSink CameraServer::GetVideo(const wpi::Twine& name) {
-  wpi::SmallString<64> nameBuf;
-  wpi::StringRef nameStr = name.toStringRef(nameBuf);
+cs::CvSink CameraServer::GetVideo(std::string_view name) {
+  auto& inst = ::GetInstance();
   cs::VideoSource source;
   {
-    std::scoped_lock lock(m_impl->m_mutex);
-    auto it = m_impl->m_sources.find(nameStr);
-    if (it == m_impl->m_sources.end()) {
+    std::scoped_lock lock(inst.m_mutex);
+    auto it = inst.m_sources.find(name);
+    if (it == inst.m_sources.end()) {
       auto csShared = GetCameraServerShared();
-      csShared->SetCameraServerError("could not find camera " + nameStr);
+      csShared->SetCameraServerError("could not find camera {}", name);
       return cs::CvSink{};
     }
     source = it->second;
@@ -628,89 +644,99 @@
   return GetVideo(source);
 }
 
-cs::CvSource CameraServer::PutVideo(const wpi::Twine& name, int width,
+cs::CvSource CameraServer::PutVideo(std::string_view name, int width,
                                     int height) {
   cs::CvSource source{name, cs::VideoMode::kMJPEG, width, height, 30};
   StartAutomaticCapture(source);
   return source;
 }
 
-cs::MjpegServer CameraServer::AddServer(const wpi::Twine& name) {
+cs::MjpegServer CameraServer::AddServer(std::string_view name) {
+  auto& inst = ::GetInstance();
   int port;
   {
-    std::scoped_lock lock(m_impl->m_mutex);
-    port = m_impl->m_nextPort++;
+    std::scoped_lock lock(inst.m_mutex);
+    port = inst.m_nextPort++;
   }
   return AddServer(name, port);
 }
 
-cs::MjpegServer CameraServer::AddServer(const wpi::Twine& name, int port) {
+cs::MjpegServer CameraServer::AddServer(std::string_view name, int port) {
   cs::MjpegServer server{name, port};
   AddServer(server);
   return server;
 }
 
 void CameraServer::AddServer(const cs::VideoSink& server) {
-  std::scoped_lock lock(m_impl->m_mutex);
-  m_impl->m_sinks.try_emplace(server.GetName(), server);
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.m_mutex);
+  inst.m_sinks.try_emplace(server.GetName(), server);
 }
 
-void CameraServer::RemoveServer(const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->m_mutex);
-  wpi::SmallString<64> nameBuf;
-  m_impl->m_sinks.erase(name.toStringRef(nameBuf));
+void CameraServer::RemoveServer(std::string_view name) {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.m_mutex);
+  inst.m_sinks.erase(name);
 }
 
 cs::VideoSink CameraServer::GetServer() {
-  wpi::SmallString<64> name;
+  auto& inst = ::GetInstance();
+  std::string name;
   {
-    std::scoped_lock lock(m_impl->m_mutex);
-    if (m_impl->m_primarySourceName.empty()) {
+    std::scoped_lock lock(inst.m_mutex);
+    if (inst.m_primarySourceName.empty()) {
       auto csShared = GetCameraServerShared();
       csShared->SetCameraServerError("no camera available");
       return cs::VideoSink{};
     }
-    name = "serve_";
-    name += m_impl->m_primarySourceName;
+    name = fmt::format("serve_{}", inst.m_primarySourceName);
   }
   return GetServer(name);
 }
 
-cs::VideoSink CameraServer::GetServer(const wpi::Twine& name) {
-  wpi::SmallString<64> nameBuf;
-  wpi::StringRef nameStr = name.toStringRef(nameBuf);
-  std::scoped_lock lock(m_impl->m_mutex);
-  auto it = m_impl->m_sinks.find(nameStr);
-  if (it == m_impl->m_sinks.end()) {
+cs::VideoSink CameraServer::GetServer(std::string_view name) {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.m_mutex);
+  auto it = inst.m_sinks.find(name);
+  if (it == inst.m_sinks.end()) {
     auto csShared = GetCameraServerShared();
-    csShared->SetCameraServerError("could not find server " + nameStr);
+    csShared->SetCameraServerError("could not find server {}", name);
     return cs::VideoSink{};
   }
   return it->second;
 }
 
 void CameraServer::AddCamera(const cs::VideoSource& camera) {
+  auto& inst = ::GetInstance();
   std::string name = camera.GetName();
-  std::scoped_lock lock(m_impl->m_mutex);
-  if (m_impl->m_primarySourceName.empty()) m_impl->m_primarySourceName = name;
-  m_impl->m_sources.try_emplace(name, camera);
+  std::scoped_lock lock(inst.m_mutex);
+  if (inst.m_primarySourceName.empty()) {
+    inst.m_primarySourceName = name;
+  }
+  inst.m_sources.try_emplace(name, camera);
 }
 
-void CameraServer::RemoveCamera(const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->m_mutex);
-  wpi::SmallString<64> nameBuf;
-  m_impl->m_sources.erase(name.toStringRef(nameBuf));
+void CameraServer::RemoveCamera(std::string_view name) {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.m_mutex);
+  inst.m_sources.erase(name);
 }
 
 void CameraServer::SetSize(int size) {
-  std::scoped_lock lock(m_impl->m_mutex);
-  if (m_impl->m_primarySourceName.empty()) return;
-  auto it = m_impl->m_sources.find(m_impl->m_primarySourceName);
-  if (it == m_impl->m_sources.end()) return;
-  if (size == kSize160x120)
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.m_mutex);
+  if (inst.m_primarySourceName.empty()) {
+    return;
+  }
+  auto it = inst.m_sources.find(inst.m_primarySourceName);
+  if (it == inst.m_sources.end()) {
+    return;
+  }
+  if (size == kSize160x120) {
     it->second.SetResolution(160, 120);
-  else if (size == kSize320x240)
+  } else if (size == kSize320x240) {
     it->second.SetResolution(320, 240);
-  else if (size == kSize640x480)
+  } else if (size == kSize640x480) {
     it->second.SetResolution(640, 480);
+  }
 }
diff --git a/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServerShared.cpp b/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServerShared.cpp
index 7d30d28..6d1ebc2 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServerShared.cpp
+++ b/third_party/allwpilib/cameraserver/src/main/native/cpp/cameraserver/CameraServerShared.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cameraserver/CameraServerShared.h"
 
@@ -15,9 +12,12 @@
   void ReportUsbCamera(int id) override {}
   void ReportAxisCamera(int id) override {}
   void ReportVideoServer(int id) override {}
-  void SetCameraServerError(const wpi::Twine& error) override {}
-  void SetVisionRunnerError(const wpi::Twine& error) override {}
-  void ReportDriverStationError(const wpi::Twine& error) override {}
+  void SetCameraServerErrorV(fmt::string_view format,
+                             fmt::format_args args) override {}
+  void SetVisionRunnerErrorV(fmt::string_view format,
+                             fmt::format_args args) override {}
+  void ReportDriverStationErrorV(fmt::string_view format,
+                                 fmt::format_args args) override {}
   std::pair<std::thread::id, bool> GetRobotMainThreadId() const override {
     return std::make_pair(std::thread::id(), false);
   }
diff --git a/third_party/allwpilib/cameraserver/src/main/native/cpp/vision/VisionRunner.cpp b/third_party/allwpilib/cameraserver/src/main/native/cpp/vision/VisionRunner.cpp
index 9896bbd..b55325a 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/cpp/vision/VisionRunner.cpp
+++ b/third_party/allwpilib/cameraserver/src/main/native/cpp/vision/VisionRunner.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "vision/VisionRunner.h"
 
@@ -23,7 +20,7 @@
 }
 
 // Located here and not in header due to cv::Mat forward declaration.
-VisionRunnerBase::~VisionRunnerBase() {}
+VisionRunnerBase::~VisionRunnerBase() = default;
 
 void VisionRunnerBase::RunOnce() {
   auto csShared = frc::GetCameraServerShared();
@@ -36,7 +33,7 @@
   auto frameTime = m_cvSink.GrabFrame(*m_image);
   if (frameTime == 0) {
     auto error = m_cvSink.GetError();
-    csShared->ReportDriverStationError(error);
+    csShared->ReportDriverStationError(error.c_str());
   } else {
     DoProcess(*m_image);
   }
@@ -56,4 +53,6 @@
   }
 }
 
-void VisionRunnerBase::Stop() { m_enabled = false; }
+void VisionRunnerBase::Stop() {
+  m_enabled = false;
+}
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h
index 8f384fd..6d087e9 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
-#include <memory>
 #include <string>
+#include <string_view>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/Twine.h>
+#include <wpi/deprecated.h>
+#include <wpi/span.h>
 
 #include "cscore.h"
 #include "cscore_cv.h"
@@ -34,7 +31,9 @@
 
   /**
    * Get the CameraServer instance.
+   * @deprecated Use the static methods
    */
+  WPI_DEPRECATED("Use static methods")
   static CameraServer* GetInstance();
 
   /**
@@ -48,7 +47,7 @@
    * with device 0, creating a camera named "USB Camera 0".  Subsequent calls
    * increment the device number (e.g. 1, 2, etc).
    */
-  cs::UsbCamera StartAutomaticCapture();
+  static cs::UsbCamera StartAutomaticCapture();
 
   /**
    * Start automatically capturing images to send to the dashboard.
@@ -58,7 +57,7 @@
    *
    * @param dev The device number of the camera interface
    */
-  cs::UsbCamera StartAutomaticCapture(int dev);
+  static cs::UsbCamera StartAutomaticCapture(int dev);
 
   /**
    * Start automatically capturing images to send to the dashboard.
@@ -66,7 +65,7 @@
    * @param name The name to give the camera
    * @param dev  The device number of the camera interface
    */
-  cs::UsbCamera StartAutomaticCapture(const wpi::Twine& name, int dev);
+  static cs::UsbCamera StartAutomaticCapture(std::string_view name, int dev);
 
   /**
    * Start automatically capturing images to send to the dashboard.
@@ -74,8 +73,8 @@
    * @param name The name to give the camera
    * @param path The device path (e.g. "/dev/video0") of the camera
    */
-  cs::UsbCamera StartAutomaticCapture(const wpi::Twine& name,
-                                      const wpi::Twine& path);
+  static cs::UsbCamera StartAutomaticCapture(std::string_view name,
+                                             std::string_view path);
 
   /**
    * Start automatically capturing images to send to the dashboard from
@@ -83,7 +82,7 @@
    *
    * @param camera Camera
    */
-  cs::MjpegServer StartAutomaticCapture(const cs::VideoSource& camera);
+  static cs::MjpegServer StartAutomaticCapture(const cs::VideoSource& camera);
 
   /**
    * Adds an Axis IP camera.
@@ -92,7 +91,7 @@
    *
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
    */
-  cs::AxisCamera AddAxisCamera(const wpi::Twine& host);
+  static cs::AxisCamera AddAxisCamera(std::string_view host);
 
   /**
    * Adds an Axis IP camera.
@@ -101,7 +100,7 @@
    *
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
    */
-  cs::AxisCamera AddAxisCamera(const char* host);
+  static cs::AxisCamera AddAxisCamera(const char* host);
 
   /**
    * Adds an Axis IP camera.
@@ -110,7 +109,7 @@
    *
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
    */
-  cs::AxisCamera AddAxisCamera(const std::string& host);
+  static cs::AxisCamera AddAxisCamera(const std::string& host);
 
   /**
    * Adds an Axis IP camera.
@@ -119,7 +118,7 @@
    *
    * @param hosts Array of Camera host IPs/DNS names
    */
-  cs::AxisCamera AddAxisCamera(wpi::ArrayRef<std::string> hosts);
+  static cs::AxisCamera AddAxisCamera(wpi::span<const std::string> hosts);
 
   /**
    * Adds an Axis IP camera.
@@ -129,7 +128,7 @@
    * @param hosts Array of Camera host IPs/DNS names
    */
   template <typename T>
-  cs::AxisCamera AddAxisCamera(std::initializer_list<T> hosts);
+  static cs::AxisCamera AddAxisCamera(std::initializer_list<T> hosts);
 
   /**
    * Adds an Axis IP camera.
@@ -137,7 +136,8 @@
    * @param name The name to give the camera
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
    */
-  cs::AxisCamera AddAxisCamera(const wpi::Twine& name, const wpi::Twine& host);
+  static cs::AxisCamera AddAxisCamera(std::string_view name,
+                                      std::string_view host);
 
   /**
    * Adds an Axis IP camera.
@@ -145,7 +145,7 @@
    * @param name The name to give the camera
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
    */
-  cs::AxisCamera AddAxisCamera(const wpi::Twine& name, const char* host);
+  static cs::AxisCamera AddAxisCamera(std::string_view name, const char* host);
 
   /**
    * Adds an Axis IP camera.
@@ -153,7 +153,8 @@
    * @param name The name to give the camera
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
    */
-  cs::AxisCamera AddAxisCamera(const wpi::Twine& name, const std::string& host);
+  static cs::AxisCamera AddAxisCamera(std::string_view name,
+                                      const std::string& host);
 
   /**
    * Adds an Axis IP camera.
@@ -161,8 +162,8 @@
    * @param name The name to give the camera
    * @param hosts Array of Camera host IPs/DNS names
    */
-  cs::AxisCamera AddAxisCamera(const wpi::Twine& name,
-                               wpi::ArrayRef<std::string> hosts);
+  static cs::AxisCamera AddAxisCamera(std::string_view name,
+                                      wpi::span<const std::string> hosts);
 
   /**
    * Adds an Axis IP camera.
@@ -171,8 +172,8 @@
    * @param hosts Array of Camera host IPs/DNS names
    */
   template <typename T>
-  cs::AxisCamera AddAxisCamera(const wpi::Twine& name,
-                               std::initializer_list<T> hosts);
+  static cs::AxisCamera AddAxisCamera(std::string_view name,
+                                      std::initializer_list<T> hosts);
 
   /**
    * Adds a virtual camera for switching between two streams.  Unlike the
@@ -180,7 +181,7 @@
    * VideoSource.  Calling SetSource() on the returned object can be used
    * to switch the actual source of the stream.
    */
-  cs::MjpegServer AddSwitchedCamera(const wpi::Twine& name);
+  static cs::MjpegServer AddSwitchedCamera(std::string_view name);
 
   /**
    * Get OpenCV access to the primary camera feed.  This allows you to
@@ -189,7 +190,7 @@
    * <p>This is only valid to call after a camera feed has been added
    * with startAutomaticCapture() or addServer().
    */
-  cs::CvSink GetVideo();
+  static cs::CvSink GetVideo();
 
   /**
    * Get OpenCV access to the specified camera.  This allows you to get
@@ -197,7 +198,7 @@
    *
    * @param camera Camera (e.g. as returned by startAutomaticCapture).
    */
-  cs::CvSink GetVideo(const cs::VideoSource& camera);
+  static cs::CvSink GetVideo(const cs::VideoSource& camera);
 
   /**
    * Get OpenCV access to the specified camera.  This allows you to get
@@ -205,7 +206,7 @@
    *
    * @param name Camera name
    */
-  cs::CvSink GetVideo(const wpi::Twine& name);
+  static cs::CvSink GetVideo(std::string_view name);
 
   /**
    * Create a MJPEG stream with OpenCV input. This can be called to pass custom
@@ -215,35 +216,36 @@
    * @param width Width of the image being sent
    * @param height Height of the image being sent
    */
-  cs::CvSource PutVideo(const wpi::Twine& name, int width, int height);
+  static cs::CvSource PutVideo(std::string_view name, int width, int height);
 
   /**
    * Adds a MJPEG server at the next available port.
    *
    * @param name Server name
    */
-  cs::MjpegServer AddServer(const wpi::Twine& name);
+  static cs::MjpegServer AddServer(std::string_view name);
 
   /**
    * Adds a MJPEG server.
    *
    * @param name Server name
+   * @param port Port number
    */
-  cs::MjpegServer AddServer(const wpi::Twine& name, int port);
+  static cs::MjpegServer AddServer(std::string_view name, int port);
 
   /**
    * Adds an already created server.
    *
    * @param server Server
    */
-  void AddServer(const cs::VideoSink& server);
+  static void AddServer(const cs::VideoSink& server);
 
   /**
    * Removes a server by name.
    *
    * @param name Server name
    */
-  void RemoveServer(const wpi::Twine& name);
+  static void RemoveServer(std::string_view name);
 
   /**
    * Get server for the primary camera feed.
@@ -251,28 +253,28 @@
    * This is only valid to call after a camera feed has been added with
    * StartAutomaticCapture() or AddServer().
    */
-  cs::VideoSink GetServer();
+  static cs::VideoSink GetServer();
 
   /**
    * Gets a server by name.
    *
    * @param name Server name
    */
-  cs::VideoSink GetServer(const wpi::Twine& name);
+  static cs::VideoSink GetServer(std::string_view name);
 
   /**
    * Adds an already created camera.
    *
    * @param camera Camera
    */
-  void AddCamera(const cs::VideoSource& camera);
+  static void AddCamera(const cs::VideoSource& camera);
 
   /**
    * Removes a camera by name.
    *
    * @param name Camera name
    */
-  void RemoveCamera(const wpi::Twine& name);
+  static void RemoveCamera(std::string_view name);
 
   /**
    * Sets the size of the image to use. Use the public kSize constants to set
@@ -283,14 +285,10 @@
    *             StartAutomaticCapture() instead.
    * @param size The size to use
    */
-  void SetSize(int size);
+  static void SetSize(int size);
 
  private:
-  CameraServer();
-  ~CameraServer();
-
-  struct Impl;
-  std::unique_ptr<Impl> m_impl;
+  CameraServer() = default;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.inc b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.inc
index 5daf29f..8c8ec21 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.inc
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServer.inc
@@ -1,15 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <string>
 #include <vector>
 
+#include "cameraserver/CameraServer.h"
+
 namespace frc {
 
 template <typename T>
@@ -20,10 +19,12 @@
 
 template <typename T>
 inline cs::AxisCamera CameraServer::AddAxisCamera(
-    const wpi::Twine& name, std::initializer_list<T> hosts) {
+    std::string_view name, std::initializer_list<T> hosts) {
   std::vector<std::string> vec;
   vec.reserve(hosts.size());
-  for (const auto& host : hosts) vec.emplace_back(host);
+  for (const auto& host : hosts) {
+    vec.emplace_back(host);
+  }
   return AddAxisCamera(name, vec);
 }
 
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServerShared.h b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServerShared.h
index cb72c9b..f6c6ae7 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServerShared.h
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/cameraserver/CameraServerShared.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <thread>
 #include <utility>
 
-#include <wpi/Twine.h>
+#include <fmt/format.h>
 
 namespace frc {
 class CameraServerShared {
@@ -20,10 +17,31 @@
   virtual void ReportUsbCamera(int id) = 0;
   virtual void ReportAxisCamera(int id) = 0;
   virtual void ReportVideoServer(int id) = 0;
-  virtual void SetCameraServerError(const wpi::Twine& error) = 0;
-  virtual void SetVisionRunnerError(const wpi::Twine& error) = 0;
-  virtual void ReportDriverStationError(const wpi::Twine& error) = 0;
+  virtual void SetCameraServerErrorV(fmt::string_view format,
+                                     fmt::format_args args) = 0;
+  virtual void SetVisionRunnerErrorV(fmt::string_view format,
+                                     fmt::format_args args) = 0;
+  virtual void ReportDriverStationErrorV(fmt::string_view format,
+                                         fmt::format_args args) = 0;
   virtual std::pair<std::thread::id, bool> GetRobotMainThreadId() const = 0;
+
+  template <typename S, typename... Args>
+  inline void SetCameraServerError(const S& format, Args&&... args) {
+    SetCameraServerErrorV(format,
+                          fmt::make_args_checked<Args...>(format, args...));
+  }
+
+  template <typename S, typename... Args>
+  inline void SetVisionRunnerError(const S& format, Args&&... args) {
+    SetVisionRunnerErrorV(format,
+                          fmt::make_args_checked<Args...>(format, args...));
+  }
+
+  template <typename S, typename... Args>
+  inline void ReportDriverStationError(const S& format, Args&&... args) {
+    ReportDriverStationErrorV(format,
+                              fmt::make_args_checked<Args...>(format, args...));
+  }
 };
 
 CameraServerShared* GetCameraServerShared();
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionPipeline.h b/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionPipeline.h
index de8e54c..954906b 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionPipeline.h
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionPipeline.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.h b/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.h
index 610ac4d..6e6e93a 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.h
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -47,14 +44,14 @@
    *
    * <p>This method is exposed to allow teams to add additional functionality or
    * have their own ways to run the pipeline. Most teams, however, should just
-   * use {@link #runForever} in its own thread using a std::thread.</p>
+   * use RunForever() in its own thread using a std::thread.</p>
    */
   void RunOnce();
 
   /**
-   * A convenience method that calls {@link #runOnce()} in an infinite loop.
-   * This must be run in a dedicated thread, and cannot be used in the main
-   * robot thread because it will freeze the robot program.
+   * A convenience method that calls runOnce() in an infinite loop. This must be
+   * run in a dedicated thread, and cannot be used in the main robot thread
+   * because it will freeze the robot program.
    *
    * <strong>Do not call this method directly from the main thread.</strong>
    */
diff --git a/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.inc b/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.inc
index 1a38048..9a195ff 100644
--- a/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.inc
+++ b/third_party/allwpilib/cameraserver/src/main/native/include/vision/VisionRunner.inc
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include "vision/VisionRunner.h"
+
 namespace frc {
 
 /**
diff --git a/third_party/allwpilib/cameraserver/src/test/native/cpp/main.cpp b/third_party/allwpilib/cameraserver/src/test/native/cpp/main.cpp
index f07ede3..b36f826 100644
--- a/third_party/allwpilib/cameraserver/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/cameraserver/src/test/native/cpp/main.cpp
@@ -1,8 +1,7 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-int main() { return 0; }
+int main() {
+  return 0;
+}
diff --git a/third_party/allwpilib/cmake/modules/AddTest.cmake b/third_party/allwpilib/cmake/modules/AddTest.cmake
index c8ef579..eef4113 100644
--- a/third_party/allwpilib/cmake/modules/AddTest.cmake
+++ b/third_party/allwpilib/cmake/modules/AddTest.cmake
@@ -8,7 +8,7 @@
         target_compile_definitions(${name}_test PRIVATE -DGTEST_LINKED_AS_SHARED_LIBRARY)
     endif()
     if (MSVC)
-        target_compile_options(${name}_test PRIVATE /wd4251 /wd4101)
+        target_compile_options(${name}_test PRIVATE /wd4101 /wd4251)
     endif()
     add_test(NAME ${name} COMMAND ${name}_test)
 endmacro()
diff --git a/third_party/allwpilib/cmake/modules/CompileWarnings.cmake b/third_party/allwpilib/cmake/modules/CompileWarnings.cmake
index 0eb9733..f35f975 100644
--- a/third_party/allwpilib/cmake/modules/CompileWarnings.cmake
+++ b/third_party/allwpilib/cmake/modules/CompileWarnings.cmake
@@ -2,6 +2,6 @@
     if(NOT MSVC)
         target_compile_options(${target} PRIVATE -Wall -pedantic -Wextra -Werror -Wno-unused-parameter -Wno-error=deprecated-declarations)
     else()
-        target_compile_options(${target} PRIVATE /wd4244 /wd4267 /wd4146 /WX /wd4996)
+        target_compile_options(${target} PRIVATE /wd4146 /wd4244 /wd4251 /wd4267 /wd4996 /WX)
     endif()
 endmacro()
diff --git a/third_party/allwpilib/cmake/scripts/GenResource.cmake b/third_party/allwpilib/cmake/scripts/GenResource.cmake
index d28e564..3c6e251 100644
--- a/third_party/allwpilib/cmake/scripts/GenResource.cmake
+++ b/third_party/allwpilib/cmake/scripts/GenResource.cmake
@@ -7,7 +7,7 @@
 STRING(REGEX REPLACE "[^a-zA-Z0-9]" "_" funcName "${inputBase}")
 SET(funcName "GetResource_${funcName}")
 
-FILE(WRITE "${output}" "#include <stddef.h>\n#include <wpi/StringRef.h>\nextern \"C\" {\nstatic const unsigned char contents[] = {")
+FILE(WRITE "${output}" "#include <stddef.h>\n#include <string_view>\nextern \"C\" {\nstatic const unsigned char contents[] = {")
 
 STRING(REGEX MATCHALL ".." outputData "${fileHex}")
 STRING(REGEX REPLACE ";" ", 0x" outputData "${outputData}")
@@ -17,7 +17,7 @@
 IF(NOT namespace STREQUAL "")
   FILE(APPEND "${output}" "namespace ${namespace} {\n")
 ENDIF()
-FILE(APPEND "${output}" "wpi::StringRef ${funcName}() {\n  return wpi::StringRef(reinterpret_cast<const char*>(contents), ${fileSize});\n}\n")
+FILE(APPEND "${output}" "std::string_view ${funcName}() {\n  return std::string_view(reinterpret_cast<const char*>(contents), ${fileSize});\n}\n")
 IF(NOT namespace STREQUAL "")
   FILE(APPEND "${output}" "}\n")
 ENDIF()
diff --git a/third_party/allwpilib/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake b/third_party/allwpilib/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake
index c144e1d..7e344ad 100644
--- a/third_party/allwpilib/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake
+++ b/third_party/allwpilib/cmake/toolchains/arm-frc-gnueabi.toolchain.cmake
@@ -1,4 +1,4 @@
 set(GCC_COMPILER_VERSION "" CACHE STRING "GCC Compiler version")
-set(GNU_MACHINE "arm-frc2021-linux-gnueabi" CACHE STRING "GNU compiler triple")
+set(GNU_MACHINE "arm-frc2022-linux-gnueabi" CACHE STRING "GNU compiler triple")
 set(SOFTFP yes)
 include("${CMAKE_CURRENT_LIST_DIR}/arm.toolchain.cmake")
diff --git a/third_party/allwpilib/cmake/toolchains/gnu.toolchain.cmake b/third_party/allwpilib/cmake/toolchains/gnu.toolchain.cmake
index cba08e7..95156a5 100644
--- a/third_party/allwpilib/cmake/toolchains/gnu.toolchain.cmake
+++ b/third_party/allwpilib/cmake/toolchains/gnu.toolchain.cmake
@@ -1,10 +1,10 @@
 cmake_minimum_required(VERSION 2.8)
 
 # load settings in case of "try compile"
-set(TOOLCHAIN_CONFIG_FILE "${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/toolchain.config.cmake")
+set(TOOLCHAIN_CONFIG_FILE "${WPILIB_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/toolchain.config.cmake")
 get_property(__IN_TRY_COMPILE GLOBAL PROPERTY IN_TRY_COMPILE)
 if(__IN_TRY_COMPILE)
-  include("${CMAKE_CURRENT_SOURCE_DIR}/../toolchain.config.cmake" OPTIONAL) # CMAKE_BINARY_DIR is different
+  include("${CMAKE_CURRENT_SOURCE_DIR}/../toolchain.config.cmake" OPTIONAL) # WPILIB_BINARY_DIR is different
   macro(toolchain_save_config)
     # nothing
   endmacro()
diff --git a/third_party/allwpilib/crossConnIntegrationTests/build.gradle b/third_party/allwpilib/crossConnIntegrationTests/build.gradle
new file mode 100644
index 0000000..8775359
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/build.gradle
@@ -0,0 +1,119 @@
+import org.gradle.language.base.internal.ProjectLayout
+import edu.wpi.first.deployutils.deploy.target.RemoteTarget
+import edu.wpi.first.deployutils.deploy.target.location.SshDeployLocation
+import edu.wpi.first.deployutils.deploy.artifact.*
+import org.gradle.internal.os.OperatingSystem
+
+apply plugin: 'cpp'
+apply plugin: 'visual-studio'
+apply plugin: 'edu.wpi.first.NativeUtils'
+apply plugin: ExtraTasks
+
+apply plugin: 'edu.wpi.first.DeployUtils'
+
+apply from: '../shared/config.gradle'
+
+ext {
+    sharedCvConfigs = [crossConnIntegrationTests: []]
+    staticCvConfigs = [:]
+    useJava = false
+    useCpp = true
+    staticGtestConfigs = [crossConnIntegrationTests: []]
+}
+
+apply from: "${rootDir}/shared/opencv.gradle"
+
+apply from: "${rootDir}/shared/googletest.gradle"
+
+deploy {
+    targets {
+        roborio(RemoteTarget) {
+            directory = '/home/admin'
+            maxChannels = 4
+            locations {
+                ssh(SshDeployLocation) {
+                    address = "172.22.11.2"
+                    user = 'admin'
+                    password = ''
+                    ipv6 = false
+                }
+            }
+
+            artifacts {
+                all {
+                    predeploy << { ctx ->
+                        ctx.execute('/usr/local/frc/bin/frcKillRobot.sh -t')
+                    }
+                    postdeploy << { ctx ->
+                        ctx.execute("sync")
+                        ctx.execute("ldconfig")
+                    }
+                }
+
+                crossConnIntegrationTests(NativeExecutableArtifact) {
+                    libraryDirectory = '/usr/local/frc/third-party/lib'
+                    postdeploy << { ctx ->
+                        ctx.execute('chmod +x crossConnIntegrationTests')
+                    }
+                }
+            }
+        }
+    }
+}
+
+model {
+    components {
+        crossConnIntegrationTests(NativeExecutableSpec) {
+            targetBuildTypes 'debug'
+            nativeUtils.useRequiredLibrary(it, 'googletest_static')
+            binaries.all { binary ->
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    if (binary.buildType.name == 'debug') {
+                        deploy.targets.roborio.artifacts.crossConnIntegrationTests.binary = binary
+                    }
+
+                    binary.sources {
+                        athenaCpp(CppSourceSet) {
+                            source {
+                                srcDirs = ['src/main/native/cpp']
+                                includes = ['**/*.cpp']
+                            }
+                            exportedHeaders {
+                                srcDirs = ['src/main/native/include']
+                                includes = ['**/*.h']
+                            }
+                        }
+                    }
+                    binary.tasks.withType(CppCompile) {
+                        cppCompiler.args "-Wno-missing-field-initializers"
+                        cppCompiler.args "-Wno-unused-variable"
+                        cppCompiler.args "-Wno-error=deprecated-declarations"
+                    }
+                    project(':hal').addHalDependency(binary, 'shared')
+                    project(':hal').addHalJniDependency(binary)
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                    if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                        nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
+                    }
+                } else {
+                    binary.sources {
+                        simCpp(CppSourceSet) {
+                            source {
+                                srcDirs 'src/main/native/dt'
+                                includes = ['**/*.cpp']
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+tasks.register('deployTests') {
+    try {
+        dependsOn tasks.named('deployCrossConnIntegrationTestsLibrariesRoborio')
+        dependsOn tasks.named('deployCrossConnIntegrationTestsRoborio')
+    } catch (ignored) {
+    }
+}
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/AnalogTest.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/AnalogTest.cpp
new file mode 100644
index 0000000..3423ffe
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/AnalogTest.cpp
@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/AnalogInput.h>
+#include <hal/AnalogOutput.h>
+#include <wpi/SmallVector.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class AnalogCrossTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+TEST_P(AnalogCrossTest, AnalogCross) {
+  auto param = GetParam();
+
+  int32_t status = 0;
+  AnalogInputHandle input{param.first, &status};
+  ASSERT_EQ(0, status);
+  AnalogOutputHandle output{param.second, &status};
+  ASSERT_EQ(0, status);
+
+  for (double i = 0; i < 5; i += 0.1) {
+    HAL_SetAnalogOutput(output, i, &status);
+    ASSERT_EQ(0, status);
+    usleep(1000);
+    ASSERT_NEAR(i, HAL_GetAnalogVoltage(input, &status), 0.01);
+    ASSERT_EQ(0, status);
+  }
+
+  for (double i = 5; i > 0; i -= 0.1) {
+    HAL_SetAnalogOutput(output, i, &status);
+    ASSERT_EQ(0, status);
+    usleep(1000);
+    ASSERT_NEAR(i, HAL_GetAnalogVoltage(input, &status), 0.01);
+    ASSERT_EQ(0, status);
+  }
+}
+
+TEST(AnalogInputTest, AllocateAll) {
+  wpi::SmallVector<AnalogInputHandle, 21> analogHandles;
+  for (int i = 0; i < HAL_GetNumAnalogInputs(); i++) {
+    int32_t status = 0;
+    analogHandles.emplace_back(AnalogInputHandle(i, &status));
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(AnalogInputTest, MultipleAllocateFails) {
+  int32_t status = 0;
+  AnalogInputHandle handle(0, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  AnalogInputHandle handle2(0, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(AnalogInputTest, OverAllocateFails) {
+  int32_t status = 0;
+  AnalogInputHandle handle(HAL_GetNumAnalogInputs(), &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(AnalogInputTest, UnderAllocateFails) {
+  int32_t status = 0;
+  AnalogInputHandle handle(-1, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(AnalogOutputTest, AllocateAll) {
+  wpi::SmallVector<AnalogOutputHandle, 21> analogHandles;
+  for (int i = 0; i < HAL_GetNumAnalogOutputs(); i++) {
+    int32_t status = 0;
+    analogHandles.emplace_back(AnalogOutputHandle(i, &status));
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(AnalogOutputTest, MultipleAllocateFails) {
+  int32_t status = 0;
+  AnalogOutputHandle handle(0, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  AnalogOutputHandle handle2(0, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(AnalogOutputTest, OverAllocateFails) {
+  int32_t status = 0;
+  AnalogOutputHandle handle(HAL_GetNumAnalogOutputs(), &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(AnalogOutputTest, UnderAllocateFails) {
+  int32_t status = 0;
+  AnalogOutputHandle handle(-1, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+INSTANTIATE_TEST_SUITE_P(AnalogCrossConnectsTests, AnalogCrossTest,
+                         ::testing::ValuesIn(AnalogCrossConnects));
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/DIOTest.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/DIOTest.cpp
new file mode 100644
index 0000000..84d2b9d
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/DIOTest.cpp
@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/DIO.h>
+#include <wpi/SmallVector.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class DIOTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+TEST_P(DIOTest, DIOCross) {
+  auto param = GetParam();
+  int32_t status = 0;
+  DIOHandle first{param.first, false, &status};
+  ASSERT_EQ(0, status);
+  DIOHandle second{param.second, true, &status};
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(first, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetDIO(first, &status));
+  ASSERT_EQ(0, status);
+  ASSERT_FALSE(HAL_GetDIO(second, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(first, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetDIO(second, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIODirection(first, true, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetDIODirection(second, false, &status);
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(second, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetDIO(first, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetDIO(second, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetDIO(first, &status));
+  ASSERT_EQ(0, status);
+}
+
+TEST(DIOTest, AllocateAll) {
+  wpi::SmallVector<DIOHandle, 32> dioHandles;
+  for (int i = 0; i < HAL_GetNumDigitalChannels(); i++) {
+    int32_t status = 0;
+    dioHandles.emplace_back(i, true, &status);
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(DIOTest, MultipleAllocateFails) {
+  int32_t status = 0;
+  DIOHandle handle(0, true, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  DIOHandle handle2(0, true, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(DIOTest, OverAllocateFails) {
+  int32_t status = 0;
+  DIOHandle handle(HAL_GetNumDigitalChannels(), true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(DIOTest, UnderAllocateFails) {
+  int32_t status = 0;
+  DIOHandle handle(-1, true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(DIOTest, CrossAllocationFails) {
+  int32_t status = 0;
+  PWMHandle pwmHandle(10, &status);
+  ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+  DIOHandle handle(10, true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+INSTANTIATE_TEST_SUITE_P(DIOCrossConnectsTests, DIOTest,
+                         ::testing::ValuesIn(DIOCrossConnects));
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/DutyCycleTest.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/DutyCycleTest.cpp
new file mode 100644
index 0000000..830ddc4
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/DutyCycleTest.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/HAL.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class DutyCycleTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+TEST_P(DutyCycleTest, DutyCycle) {
+  auto param = GetParam();
+
+  int32_t status = 0;
+  PWMHandle pwmHandle(param.first, &status);
+  ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(0, status);
+
+  // Ensure our PWM is disabled, and set up properly
+  HAL_SetPWMRaw(pwmHandle, 0, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
+  HAL_SetPWMConfig(pwmHandle, 5.05, 2.525, 2.525, 2.525, 0, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetPWMPeriodScale(pwmHandle, 0, &status);
+  ASSERT_EQ(0, status);
+
+  DIOHandle dioHandle{param.second, true, &status};
+  ASSERT_EQ(0, status);
+
+  DutyCycleHandle dutyCycle{dioHandle, &status};
+  ASSERT_EQ(0, status);
+
+  HAL_SetPWMSpeed(pwmHandle, 0.5, &status);
+  ASSERT_EQ(0, status);
+
+  // Sleep enough time for the frequency to converge
+  usleep(3500000);
+
+  ASSERT_NEAR(1000 / 5.05,
+              (double)HAL_GetDutyCycleFrequency(dutyCycle, &status), 1);
+
+  // TODO measure output
+}
+
+INSTANTIATE_TEST_SUITE_P(DutyCycleCrossConnTests, DutyCycleTest,
+                         ::testing::ValuesIn(PWMCrossConnects));
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/Main.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/Main.cpp
new file mode 100644
index 0000000..95bc5b5
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/Main.cpp
@@ -0,0 +1,10 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/PWMTest.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/PWMTest.cpp
new file mode 100644
index 0000000..27fcce7
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/PWMTest.cpp
@@ -0,0 +1,357 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <atomic>
+#include <thread>
+
+#include <hal/DMA.h>
+#include <hal/HAL.h>
+#include <wpi/SmallVector.h>
+#include <wpi/condition_variable.h>
+#include <wpi/priority_mutex.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class PWMTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+void TestTimingDMA(int squelch, std::pair<int, int> param) {
+  // Initialize DMA
+  int32_t status = 0;
+  DMAHandle dmaHandle(&status);
+  ASSERT_NE(dmaHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(0, status);
+
+  status = 0;
+  PWMHandle pwmHandle(param.first, &status);
+  ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(0, status);
+
+  // Ensure our PWM is disabled, and set up properly
+  HAL_SetPWMRaw(pwmHandle, 0, &status);
+  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
+  HAL_SetPWMPeriodScale(pwmHandle, squelch, &status);
+
+  unsigned int checkPeriod = 0;
+  switch (squelch) {
+    case (0):
+      checkPeriod = 5050;
+      break;
+    case (1):
+      checkPeriod = 10100;
+      break;
+    case (3):
+      checkPeriod = 20200;
+      break;
+  }
+
+  status = 0;
+  DIOHandle dioHandle(param.second, true, &status);
+  ASSERT_NE(dioHandle, HAL_kInvalidHandle);
+
+  HAL_AddDMADigitalSource(dmaHandle, dioHandle, &status);
+  ASSERT_EQ(0, status);
+
+  HAL_SetDMAExternalTrigger(dmaHandle, dioHandle,
+                            HAL_AnalogTriggerType::HAL_Trigger_kInWindow, true,
+                            true, &status);
+  ASSERT_EQ(0, status);
+
+  // Loop to test 5 speeds
+  for (unsigned int testWidth = 1000; testWidth < 2100; testWidth += 250) {
+    HAL_StartDMA(dmaHandle, 1024, &status);
+    ASSERT_EQ(0, status);
+
+    while (true) {
+      int32_t remaining = 0;
+      HAL_DMASample testSample;
+      HAL_ReadDMA(dmaHandle, &testSample, 0.01, &remaining, &status);
+      if (remaining == 0) {
+        break;
+      }
+    }
+
+    HAL_SetPWMSpeed(pwmHandle, (testWidth - 1000) / 1000.0, &status);
+
+    constexpr const int kSampleCount = 15;
+    HAL_DMASample dmaSamples[kSampleCount];
+    int readCount = 0;
+    while (readCount < kSampleCount) {
+      status = 0;
+      int32_t remaining = 0;
+      HAL_DMAReadStatus readStatus = HAL_ReadDMA(
+          dmaHandle, &dmaSamples[readCount], 1.0, &remaining, &status);
+      ASSERT_EQ(0, status);
+      ASSERT_EQ(HAL_DMAReadStatus::HAL_DMA_OK, readStatus);
+      readCount++;
+    }
+
+    HAL_SetPWMSpeed(pwmHandle, 0, &status);
+    HAL_StopDMA(dmaHandle, &status);
+
+    // Find first rising edge
+    int startIndex = 4;
+    while (startIndex < 6) {
+      status = 0;
+      auto value = HAL_GetDMASampleDigitalSource(&dmaSamples[startIndex],
+                                                 dioHandle, &status);
+      ASSERT_EQ(0, status);
+      if (value)
+        break;
+      startIndex++;
+    }
+    ASSERT_LT(startIndex, 6);
+
+    // Check that samples alternate
+    bool previous = false;
+    int iterationCount = 0;
+    for (int i = startIndex; i < startIndex + 8; i++) {
+      auto value =
+          HAL_GetDMASampleDigitalSource(&dmaSamples[i], dioHandle, &status);
+      ASSERT_EQ(0, status);
+      ASSERT_NE(previous, value);
+      previous = !previous;
+      iterationCount++;
+    }
+    ASSERT_EQ(iterationCount, 8);
+    iterationCount = 0;
+
+    // Check width between samples
+    for (int i = startIndex; i < startIndex + 8; i += 2) {
+      auto width = HAL_GetDMASampleTime(&dmaSamples[i + 1], &status) -
+                   HAL_GetDMASampleTime(&dmaSamples[i], &status);
+      ASSERT_NEAR(testWidth, width, 10);
+      iterationCount++;
+    }
+    ASSERT_EQ(iterationCount, 4);
+    iterationCount = 0;
+
+    // Check period between samples
+    for (int i = startIndex; i < startIndex + 6; i += 2) {
+      auto period = HAL_GetDMASampleTime(&dmaSamples[i + 2], &status) -
+                    HAL_GetDMASampleTime(&dmaSamples[i], &status);
+      ASSERT_NEAR(checkPeriod, period, 10);
+      iterationCount++;
+    }
+    ASSERT_EQ(iterationCount, 3);
+  }
+}
+
+struct InterruptCheckData {
+  wpi::SmallVector<uint64_t, 8> risingStamps;
+  wpi::SmallVector<uint64_t, 8> fallingStamps;
+  wpi::priority_mutex mutex;
+  wpi::condition_variable cond;
+  HAL_InterruptHandle handle;
+};
+
+// TODO switch this to DMA
+void TestTiming(int squelch, std::pair<int, int> param) {
+  // Initialize interrupt
+  int32_t status = 0;
+  InterruptHandle interruptHandle(&status);
+  // Ensure we have a valid interrupt handle
+  ASSERT_NE(interruptHandle, HAL_kInvalidHandle);
+
+  status = 0;
+  PWMHandle pwmHandle(param.first, &status);
+  ASSERT_NE(pwmHandle, HAL_kInvalidHandle);
+
+  // Ensure our PWM is disabled, and set up properly
+  HAL_SetPWMRaw(pwmHandle, 0, &status);
+  HAL_SetPWMConfig(pwmHandle, 2.0, 1.0, 1.0, 0, 0, &status);
+  HAL_SetPWMPeriodScale(pwmHandle, squelch, &status);
+
+  unsigned int checkPeriod = 0;
+  switch (squelch) {
+    case (0):
+      checkPeriod = 5050;
+      break;
+    case (1):
+      checkPeriod = 10100;
+      break;
+    case (3):
+      checkPeriod = 20200;
+      break;
+  }
+
+  status = 0;
+  DIOHandle dioHandle(param.second, true, &status);
+  ASSERT_NE(dioHandle, HAL_kInvalidHandle);
+
+  InterruptCheckData interruptData;
+  interruptData.handle = interruptHandle;
+
+  // Can use any type for the interrupt handle
+  HAL_RequestInterrupts(interruptHandle, dioHandle,
+                        HAL_AnalogTriggerType::HAL_Trigger_kInWindow, &status);
+
+  HAL_SetInterruptUpSourceEdge(interruptHandle, true, true, &status);
+
+  // Loop to test 5 speeds
+  for (unsigned int i = 1000; i < 2100; i += 250) {
+    interruptData.risingStamps.clear();
+    interruptData.fallingStamps.clear();
+
+    std::atomic_bool runThread{true};
+
+    status = 0;
+    std::thread interruptThread([&]() {
+      while (runThread) {
+        int32_t threadStatus = 0;
+        auto mask =
+            HAL_WaitForInterrupt(interruptHandle, 5, true, &threadStatus);
+
+        if ((mask & 0x100) == 0x100 && interruptData.risingStamps.size() == 0 &&
+            interruptData.fallingStamps.size() == 0) {
+          // Falling edge at start of tracking. Skip
+          continue;
+        }
+
+        int32_t status = 0;
+        if ((mask & 0x1) == 0x1) {
+          auto ts = HAL_ReadInterruptRisingTimestamp(interruptHandle, &status);
+          // Rising Edge
+          interruptData.risingStamps.push_back(ts);
+        } else if ((mask & 0x100) == 0x100) {
+          auto ts = HAL_ReadInterruptFallingTimestamp(interruptHandle, &status);
+          // Falling Edge
+          interruptData.fallingStamps.push_back(ts);
+        }
+
+        if (interruptData.risingStamps.size() >= 4 &&
+            interruptData.fallingStamps.size() >= 4) {
+          interruptData.cond.notify_all();
+          runThread = false;
+          break;
+        }
+      }
+    });
+
+    // Ensure our interrupt actually got created correctly.
+    ASSERT_EQ(status, 0);
+    HAL_SetPWMSpeed(pwmHandle, (i - 1000) / 1000.0, &status);
+    ASSERT_EQ(status, 0);
+    {
+      std::unique_lock<wpi::priority_mutex> lock(interruptData.mutex);
+      // Wait for lock
+      // TODO: Add Timeout
+      auto timeout = interruptData.cond.wait_for(lock, std::chrono::seconds(2));
+      if (timeout == std::cv_status::timeout) {
+        runThread = false;
+        if (interruptThread.joinable()) {
+          interruptThread.join();
+        }
+        ASSERT_TRUE(false);  // Exit test as failure on timeout
+      }
+    }
+
+    HAL_SetPWMRaw(pwmHandle, 0, &status);
+
+    // Ensure our interrupts have the proper counts
+    ASSERT_EQ(interruptData.risingStamps.size(),
+              interruptData.fallingStamps.size());
+
+    ASSERT_GT(interruptData.risingStamps.size(), 0u);
+
+    ASSERT_EQ(interruptData.risingStamps.size() % 2, 0u);
+    ASSERT_EQ(interruptData.fallingStamps.size() % 2, 0u);
+
+    for (size_t j = 0; j < interruptData.risingStamps.size(); j++) {
+      uint64_t width =
+          interruptData.fallingStamps[j] - interruptData.risingStamps[j];
+      ASSERT_NEAR(width, i, 10);
+    }
+
+    for (unsigned int j = 0; j < interruptData.risingStamps.size() - 1; j++) {
+      uint64_t period =
+          interruptData.risingStamps[j + 1] - interruptData.risingStamps[j];
+      ASSERT_NEAR(period, checkPeriod, 10);
+    }
+    runThread = false;
+    if (interruptThread.joinable()) {
+      interruptThread.join();
+    }
+  }
+}
+
+TEST_P(PWMTest, Timing4x) {
+  auto param = GetParam();
+  TestTiming(3, param);
+}
+
+TEST_P(PWMTest, Timing2x) {
+  auto param = GetParam();
+  TestTiming(1, param);
+}
+
+TEST_P(PWMTest, Timing1x) {
+  auto param = GetParam();
+  TestTiming(0, param);
+}
+
+TEST_P(PWMTest, TimingDMA4x) {
+  auto param = GetParam();
+  TestTimingDMA(3, param);
+}
+
+TEST_P(PWMTest, TimingDMA2x) {
+  auto param = GetParam();
+  TestTimingDMA(1, param);
+}
+
+TEST_P(PWMTest, TimingDMA1x) {
+  auto param = GetParam();
+  TestTimingDMA(0, param);
+}
+
+TEST(PWMTest, AllocateAll) {
+  wpi::SmallVector<PWMHandle, 21> pwmHandles;
+  for (int i = 0; i < HAL_GetNumPWMChannels(); i++) {
+    int32_t status = 0;
+    pwmHandles.emplace_back(PWMHandle(i, &status));
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(PWMTest, MultipleAllocateFails) {
+  int32_t status = 0;
+  PWMHandle handle(0, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  PWMHandle handle2(0, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(PWMTest, OverAllocateFails) {
+  int32_t status = 0;
+  PWMHandle handle(HAL_GetNumPWMChannels(), &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(PWMTest, UnderAllocateFails) {
+  int32_t status = 0;
+  PWMHandle handle(-1, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(PWMTest, CrossAllocationFails) {
+  int32_t status = 0;
+  DIOHandle dioHandle(10, true, &status);
+  ASSERT_NE(dioHandle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+  PWMHandle handle(10, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+INSTANTIATE_TEST_SUITE_P(PWMCrossConnectTests, PWMTest,
+                         ::testing::ValuesIn(PWMCrossConnects));
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/RelayAnalogTest.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/RelayAnalogTest.cpp
new file mode 100644
index 0000000..fc7da0c
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/RelayAnalogTest.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/AnalogInput.h>
+#include <hal/Relay.h>
+#include <wpi/SmallVector.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class RelayAnalogTest : public ::testing::TestWithParam<std::pair<int, int>> {};
+
+TEST_P(RelayAnalogTest, RelayAnalogCross) {
+  auto param = GetParam();
+
+  int32_t status = 0;
+  RelayHandle relay{param.first, true, &status};
+  ASSERT_EQ(0, status);
+  AnalogInputHandle analog{param.second, &status};
+  ASSERT_EQ(0, status);
+  AnalogTriggerHandle trigger{analog, &status};
+  ASSERT_EQ(0, status);
+  HAL_SetAnalogTriggerLimitsVoltage(trigger, 1.5, 3.0, &status);
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(relay, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetAnalogTriggerTriggerState(trigger, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(relay, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetAnalogTriggerTriggerState(trigger, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(relay, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetAnalogTriggerTriggerState(trigger, &status));
+  ASSERT_EQ(0, status);
+}
+
+INSTANTIATE_TEST_SUITE_P(RelayAnalogCrossConnectsTests, RelayAnalogTest,
+                         ::testing::ValuesIn(RelayAnalogCrossConnects));
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/RelayDigitalTest.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/RelayDigitalTest.cpp
new file mode 100644
index 0000000..c7923bc
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/RelayDigitalTest.cpp
@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/Relay.h>
+#include <wpi/SmallVector.h>
+
+#include "CrossConnects.h"
+#include "LifetimeWrappers.h"
+#include "gtest/gtest.h"
+
+using namespace hlt;
+
+class RelayDigitalTest : public ::testing::TestWithParam<RelayCross> {};
+
+TEST_P(RelayDigitalTest, RelayCross) {
+  auto param = GetParam();
+  int32_t status = 0;
+  RelayHandle fwd{param.Relay, true, &status};
+  ASSERT_EQ(0, status);
+  RelayHandle rev{param.Relay, false, &status};
+  ASSERT_EQ(0, status);
+  DIOHandle fwdInput{param.FwdDio, true, &status};
+  ASSERT_EQ(0, status);
+  DIOHandle revInput{param.RevDio, true, &status};
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(fwd, false, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetRelay(rev, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetDIO(fwdInput, &status));
+  ASSERT_EQ(0, status);
+  ASSERT_FALSE(HAL_GetDIO(revInput, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(fwd, false, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetRelay(rev, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_FALSE(HAL_GetDIO(fwdInput, &status));
+  ASSERT_EQ(0, status);
+  ASSERT_TRUE(HAL_GetDIO(revInput, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(fwd, true, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetRelay(rev, false, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetDIO(fwdInput, &status));
+  ASSERT_EQ(0, status);
+  ASSERT_FALSE(HAL_GetDIO(revInput, &status));
+  ASSERT_EQ(0, status);
+
+  HAL_SetRelay(fwd, true, &status);
+  ASSERT_EQ(0, status);
+  HAL_SetRelay(rev, true, &status);
+  ASSERT_EQ(0, status);
+  usleep(1000);
+  ASSERT_TRUE(HAL_GetDIO(fwdInput, &status));
+  ASSERT_EQ(0, status);
+  ASSERT_TRUE(HAL_GetDIO(revInput, &status));
+  ASSERT_EQ(0, status);
+}
+
+TEST(RelayDigitalTest, AllocateAll) {
+  wpi::SmallVector<RelayHandle, 32> relayHandles;
+  for (int i = 0; i < HAL_GetNumRelayChannels(); i++) {
+    int32_t status = 0;
+    relayHandles.emplace_back(i / 2, i % 2, &status);
+    ASSERT_EQ(status, 0);
+  }
+}
+
+TEST(RelayDigitalTest, MultipleAllocateFails) {
+  int32_t status = 0;
+  RelayHandle handle(0, true, &status);
+  ASSERT_NE(handle, HAL_kInvalidHandle);
+  ASSERT_EQ(status, 0);
+
+  RelayHandle handle2(0, true, &status);
+  ASSERT_EQ(handle2, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_IS_ALLOCATED);
+}
+
+TEST(RelayDigitalTest, OverAllocateFails) {
+  int32_t status = 0;
+  RelayHandle handle(HAL_GetNumRelayChannels(), true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+TEST(RelayDigitalTest, UnderAllocateFails) {
+  int32_t status = 0;
+  RelayHandle handle(-1, true, &status);
+  ASSERT_EQ(handle, HAL_kInvalidHandle);
+  ASSERT_LAST_ERROR_STATUS(status, RESOURCE_OUT_OF_RANGE);
+}
+
+INSTANTIATE_TEST_SUITE_P(RelayDigitalCrossConnectsTests, RelayDigitalTest,
+                         ::testing::ValuesIn(RelayCrossConnects));
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
new file mode 100644
index 0000000..ee8c2b1
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdlib>
+#include <thread>
+
+#include <fmt/core.h>
+#include <hal/HAL.h>
+
+#include "gtest/gtest.h"
+#include "mockds/MockDS.h"
+
+using namespace std::chrono_literals;
+
+class TestEnvironment : public testing::Environment {
+  bool m_alreadySetUp = false;
+  MockDS m_mockDS;
+
+ public:
+  TestEnvironment() {
+    // Only set up once. This allows gtest_repeat to be used to automatically
+    // repeat tests.
+    if (m_alreadySetUp) {
+      return;
+    }
+    m_alreadySetUp = true;
+
+    if (!HAL_Initialize(500, 0)) {
+      fmt::print(stderr, "FATAL ERROR: HAL could not be initialized\n");
+      std::exit(-1);
+    }
+
+    m_mockDS.Start();
+
+    // This sets up the network communications library to enable the driver
+    // station. After starting network coms, it will loop until the driver
+    // station returns that the robot is enabled, to ensure that tests will be
+    // able to run on the hardware.
+    HAL_ObserveUserProgramStarting();
+
+    fmt::print("Started coms\n");
+
+    int enableCounter = 0;
+
+    auto checkEnabled = []() {
+      HAL_ControlWord controlWord;
+      std::memset(&controlWord, 0, sizeof(controlWord));
+      HAL_GetControlWord(&controlWord);
+      return controlWord.enabled && controlWord.dsAttached;
+    };
+    while (!checkEnabled()) {
+      if (enableCounter > 50) {
+        // Robot did not enable properly after 5 seconds.
+        // Force exit
+        fmt::print(stderr, " Failed to enable. Aborting\n");
+        std::terminate();
+      }
+
+      std::this_thread::sleep_for(100ms);
+
+      fmt::print("Waiting for enable: {}\n", enableCounter++);
+    }
+    std::this_thread::sleep_for(500ms);
+  }
+
+  ~TestEnvironment() override { m_mockDS.Stop(); }
+};
+
+testing::Environment* const environment =
+    testing::AddGlobalTestEnvironment(new TestEnvironment);
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp
new file mode 100644
index 0000000..b2e5c2d
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp
@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "MockDS.h"
+
+#include <stdint.h>
+
+#include <string_view>
+
+#include <fmt/core.h>
+#include <hal/cpp/fpga_clock.h>
+#include <wpi/Logger.h>
+#include <wpi/SmallVector.h>
+#include <wpi/UDPClient.h>
+
+static void LoggerFunc(unsigned int level, const char* file, unsigned int line,
+                       const char* msg) {
+  if (level == 20) {
+    fmt::print(stderr, "DS: {}\n", msg);
+    return;
+  }
+
+  std::string_view levelmsg;
+  if (level >= 50) {
+    levelmsg = "CRITICAL";
+  } else if (level >= 40) {
+    levelmsg = "ERROR";
+  } else if (level >= 30) {
+    levelmsg = "WARNING";
+  } else {
+    return;
+  }
+  fmt::print(stderr, "DS: {}: {} ({}:{})\n", levelmsg, msg, file, line);
+}
+
+static void generateEnabledDsPacket(wpi::SmallVectorImpl<uint8_t>& data,
+                                    uint16_t sendCount) {
+  data.clear();
+  data.push_back(sendCount >> 8);
+  data.push_back(sendCount);
+  data.push_back(0x01);  // general data tag
+  data.push_back(0x04);  // teleop enabled
+  data.push_back(0x10);  // normal data request
+  data.push_back(0x00);  // red 1 station
+}
+
+void MockDS::Start() {
+  if (m_active) {
+    return;
+  }
+  m_active = true;
+  m_thread = std::thread([&]() {
+    wpi::Logger logger(LoggerFunc);
+    wpi::UDPClient client(logger);
+    client.start();
+    auto timeout_time = hal::fpga_clock::now();
+    int initCount = 0;
+    uint16_t sendCount = 0;
+    wpi::SmallVector<uint8_t, 8> data;
+    while (m_active) {
+      // Keep 20ms intervals, and increase time to next interval
+      auto current = hal::fpga_clock::now();
+      while (timeout_time <= current) {
+        timeout_time += std::chrono::milliseconds(20);
+      }
+      std::this_thread::sleep_until(timeout_time);
+      generateEnabledDsPacket(data, sendCount++);
+      // ~10 disabled packets are required to make the robot actually enable
+      // 1 is definitely not enough.
+      if (initCount < 10) {
+        initCount++;
+        data[3] = 0;
+      }
+      client.send(data, "127.0.0.1", 1110);
+    }
+    client.shutdown();
+  });
+}
+
+void MockDS::Stop() {
+  m_active = false;
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
+}
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.h b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.h
new file mode 100644
index 0000000..da5fcd9
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/cpp/mockds/MockDS.h
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <atomic>
+#include <thread>
+
+class MockDS {
+ public:
+  MockDS() = default;
+  ~MockDS() { Stop(); }
+  MockDS(const MockDS& other) = delete;
+  MockDS& operator=(const MockDS& other) = delete;
+
+  void Start();
+  void Stop();
+
+ private:
+  std::thread m_thread;
+  std::atomic_bool m_active{false};
+};
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/dt/Main.cpp b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/dt/Main.cpp
new file mode 100644
index 0000000..a3e363e
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/dt/Main.cpp
@@ -0,0 +1,5 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+int main() {}
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/CrossConnects.h b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/CrossConnects.h
new file mode 100644
index 0000000..f77a8f2
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/CrossConnects.h
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <utility>
+
+namespace hlt {
+
+constexpr static std::array<std::pair<int, int>, 22> DIOCrossConnects{
+    std::pair{20, 25},
+    std::pair{19, 24},
+    std::pair{17, 13},
+    std::pair{16, 12},
+    std::pair{15, 11},
+    std::pair{14, 10},
+    std::pair{26, 2},
+    std::pair{27, 1},
+    std::pair{28, 0},
+    std::pair{29, 3},
+    std::pair{30, 4},
+
+    // Opposite direction
+    std::pair{25, 20},
+    std::pair{24, 19},
+    std::pair{13, 17},
+    std::pair{12, 16},
+    std::pair{11, 15},
+    std::pair{10, 14},
+    std::pair{2, 26},
+    std::pair{1, 27},
+    std::pair{0, 28},
+    std::pair{3, 29},
+    std::pair{4, 30},
+};
+
+// PWM on left, DIO on right
+constexpr static std::array<std::pair<int, int>, 2> PWMCrossConnects{
+    std::pair{0, 18},
+    std::pair{16, 25},
+};
+
+// FWD only, relay on left
+constexpr static std::array<std::pair<int, int>, 2> RelayAnalogCrossConnects{
+    std::pair{2, 0}, std::pair{3, 1}};
+
+struct RelayCross {
+  int Relay;
+  int FwdDio;
+  int RevDio;
+};
+
+constexpr static std::array<RelayCross, 1> RelayCrossConnects{
+    RelayCross{0, 23, 22}};
+
+// input on left
+constexpr static std::array<std::pair<int, int>, 2> AnalogCrossConnects{
+    std::pair{2, 0}, std::pair{4, 1}};
+
+}  // namespace hlt
diff --git a/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h
new file mode 100644
index 0000000..9f0eb15
--- /dev/null
+++ b/third_party/allwpilib/crossConnIntegrationTests/src/main/native/include/LifetimeWrappers.h
@@ -0,0 +1,200 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/DMA.h>
+#include <hal/DutyCycle.h>
+#include <hal/HAL.h>
+
+namespace hlt {
+
+struct InterruptHandle {
+ public:
+  explicit InterruptHandle(int32_t* status) {
+    handle = HAL_InitializeInterrupts(status);
+  }
+  InterruptHandle(const InterruptHandle&) = delete;
+  InterruptHandle operator=(const InterruptHandle&) = delete;
+
+  InterruptHandle(InterruptHandle&&) = default;
+  InterruptHandle& operator=(InterruptHandle&&) = default;
+
+  ~InterruptHandle() { HAL_CleanInterrupts(handle); }
+
+  operator HAL_InterruptHandle() const { return handle; }
+
+ private:
+  HAL_InterruptHandle handle = 0;
+};
+
+struct DMAHandle {
+ public:
+  explicit DMAHandle(int32_t* status) { handle = HAL_InitializeDMA(status); }
+  DMAHandle(const DMAHandle&) = delete;
+  DMAHandle operator=(const DMAHandle&) = delete;
+
+  DMAHandle(DMAHandle&&) = default;
+  DMAHandle& operator=(DMAHandle&&) = default;
+
+  ~DMAHandle() { HAL_FreeDMA(handle); }
+
+  operator HAL_DMAHandle() const { return handle; }
+
+ private:
+  HAL_DMAHandle handle = 0;
+};
+
+struct AnalogInputHandle {
+ public:
+  AnalogInputHandle(int32_t port, int32_t* status) {
+    handle = HAL_InitializeAnalogInputPort(HAL_GetPort(port), nullptr, status);
+  }
+  AnalogInputHandle(const AnalogInputHandle&) = delete;
+  AnalogInputHandle operator=(const AnalogInputHandle&) = delete;
+
+  AnalogInputHandle(AnalogInputHandle&&) = default;
+  AnalogInputHandle& operator=(AnalogInputHandle&&) = default;
+
+  ~AnalogInputHandle() { HAL_FreeAnalogInputPort(handle); }
+
+  operator HAL_AnalogInputHandle() const { return handle; }
+
+ private:
+  HAL_AnalogInputHandle handle = 0;
+};
+
+struct AnalogOutputHandle {
+ public:
+  AnalogOutputHandle(int32_t port, int32_t* status) {
+    handle = HAL_InitializeAnalogOutputPort(HAL_GetPort(port), nullptr, status);
+  }
+  AnalogOutputHandle(const AnalogOutputHandle&) = delete;
+  AnalogOutputHandle operator=(const AnalogOutputHandle&) = delete;
+
+  AnalogOutputHandle(AnalogOutputHandle&&) = default;
+  AnalogOutputHandle& operator=(AnalogOutputHandle&&) = default;
+
+  ~AnalogOutputHandle() { HAL_FreeAnalogOutputPort(handle); }
+
+  operator HAL_AnalogOutputHandle() const { return handle; }
+
+ private:
+  HAL_AnalogOutputHandle handle = 0;
+};
+
+struct AnalogTriggerHandle {
+ public:
+  explicit AnalogTriggerHandle(HAL_AnalogInputHandle port, int32_t* status) {
+    handle = HAL_InitializeAnalogTrigger(port, status);
+  }
+  AnalogTriggerHandle(const AnalogTriggerHandle&) = delete;
+  AnalogTriggerHandle operator=(const AnalogTriggerHandle&) = delete;
+
+  AnalogTriggerHandle(AnalogTriggerHandle&&) = default;
+  AnalogTriggerHandle& operator=(AnalogTriggerHandle&&) = default;
+
+  ~AnalogTriggerHandle() {
+    int32_t status = 0;
+    HAL_CleanAnalogTrigger(handle, &status);
+  }
+
+  operator HAL_AnalogTriggerHandle() const { return handle; }
+
+ private:
+  HAL_AnalogTriggerHandle handle = 0;
+};
+
+struct DutyCycleHandle {
+ public:
+  DutyCycleHandle(HAL_DigitalHandle port, int32_t* status) {
+    handle = HAL_InitializeDutyCycle(
+        port, HAL_AnalogTriggerType::HAL_Trigger_kInWindow, status);
+  }
+  DutyCycleHandle(const DutyCycleHandle&) = delete;
+  DutyCycleHandle operator=(const DutyCycleHandle&) = delete;
+
+  DutyCycleHandle(DutyCycleHandle&&) = default;
+  DutyCycleHandle& operator=(DutyCycleHandle&&) = default;
+
+  ~DutyCycleHandle() { HAL_FreeDutyCycle(handle); }
+
+  operator HAL_DutyCycleHandle() const { return handle; }
+
+ private:
+  HAL_DutyCycleHandle handle = 0;
+};
+
+struct DIOHandle {
+ public:
+  DIOHandle() {}
+  DIOHandle(const DIOHandle&) = delete;
+  DIOHandle operator=(const DIOHandle&) = delete;
+
+  DIOHandle(DIOHandle&&) = default;
+  DIOHandle& operator=(DIOHandle&&) = default;
+
+  DIOHandle(int32_t port, HAL_Bool input, int32_t* status) {
+    handle = HAL_InitializeDIOPort(HAL_GetPort(port), input, nullptr, status);
+  }
+
+  ~DIOHandle() { HAL_FreeDIOPort(handle); }
+
+  operator HAL_DigitalHandle() const { return handle; }
+
+ private:
+  HAL_DigitalHandle handle = 0;
+};
+
+struct PWMHandle {
+ public:
+  PWMHandle() {}
+  PWMHandle(const PWMHandle&) = delete;
+  PWMHandle operator=(const PWMHandle&) = delete;
+
+  PWMHandle(PWMHandle&&) = default;
+  PWMHandle& operator=(PWMHandle&&) = default;
+
+  PWMHandle(int32_t port, int32_t* status) {
+    handle = HAL_InitializePWMPort(HAL_GetPort(port), nullptr, status);
+  }
+
+  ~PWMHandle() {
+    int32_t status = 0;
+    HAL_FreePWMPort(handle, &status);
+  }
+
+  operator HAL_DigitalHandle() const { return handle; }
+
+ private:
+  HAL_DigitalHandle handle = 0;
+};
+
+struct RelayHandle {
+ public:
+  RelayHandle(int32_t port, HAL_Bool fwd, int32_t* status) {
+    handle = HAL_InitializeRelayPort(HAL_GetPort(port), fwd, nullptr, status);
+  }
+  RelayHandle(const RelayHandle&) = delete;
+  RelayHandle operator=(const RelayHandle&) = delete;
+
+  RelayHandle(RelayHandle&&) = default;
+  RelayHandle& operator=(RelayHandle&&) = default;
+
+  ~RelayHandle() { HAL_FreeRelayPort(handle); }
+
+  operator HAL_RelayHandle() const { return handle; }
+
+ private:
+  HAL_RelayHandle handle = 0;
+};
+
+#define ASSERT_LAST_ERROR_STATUS(status, x)                          \
+  do {                                                               \
+    ASSERT_EQ(status, HAL_USE_LAST_ERROR);                           \
+    const char* lastErrorMessageInMacro = HAL_GetLastError(&status); \
+    ASSERT_EQ(status, x);                                            \
+  } while (0)
+
+}  // namespace hlt
diff --git a/third_party/allwpilib/cscore/.styleguide b/third_party/allwpilib/cscore/.styleguide
index fada841..f0732e1 100644
--- a/third_party/allwpilib/cscore/.styleguide
+++ b/third_party/allwpilib/cscore/.styleguide
@@ -31,6 +31,7 @@
 }
 
 includeOtherLibs {
+  ^fmt/
   ^opencv2/
   ^support/
   ^tcpsockets/
diff --git a/third_party/allwpilib/cscore/CMakeLists.txt b/third_party/allwpilib/cscore/CMakeLists.txt
index 98f6d7e..9ab9ca7 100644
--- a/third_party/allwpilib/cscore/CMakeLists.txt
+++ b/third_party/allwpilib/cscore/CMakeLists.txt
@@ -46,8 +46,8 @@
     set (cscore_config_dir share/cscore)
 endif()
 
-configure_file(cscore-config.cmake.in ${CMAKE_BINARY_DIR}/cscore-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/cscore-config.cmake DESTINATION ${cscore_config_dir})
+configure_file(cscore-config.cmake.in ${WPILIB_BINARY_DIR}/cscore-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/cscore-config.cmake DESTINATION ${cscore_config_dir})
 install(EXPORT cscore DESTINATION ${cscore_config_dir})
 
 SUBDIR_LIST(cscore_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
@@ -79,7 +79,7 @@
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
-    set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
     #find java files, copy them locally
 
@@ -87,11 +87,11 @@
         set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
     endif()
 
-    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin NO_DEFAULT_PATH)
+    find_file(OPENCV_JAR_FILE NAMES opencv-${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.jar PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/share/java NO_DEFAULT_PATH)
     find_file(OPENCV_JNI_FILE NAMES libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.so
                                     libopencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dylib
                                     opencv_java${OpenCV_VERSION_MAJOR}${OpenCV_VERSION_MINOR}${OpenCV_VERSION_PATCH}.dll
-                                    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/bin/Release ${OpenCV_INSTALL_PATH}/bin/Debug ${OpenCV_INSTALL_PATH}/lib NO_DEFAULT_PATH)
+                                    PATHS ${OPENCV_JAVA_INSTALL_DIR} ${OpenCV_INSTALL_PATH}/bin ${OpenCV_INSTALL_PATH}/bin/Release ${OpenCV_INSTALL_PATH}/bin/Debug ${OpenCV_INSTALL_PATH}/lib ${OpenCV_INSTALL_PATH}/lib/jni NO_DEFAULT_PATH)
 
     file(GLOB
         cscore_jni_src src/main/native/cpp/jni/CameraServerJNI.cpp)
diff --git a/third_party/allwpilib/cscore/build.gradle b/third_party/allwpilib/cscore/build.gradle
index 90e5c20..cd03465 100644
--- a/third_party/allwpilib/cscore/build.gradle
+++ b/third_party/allwpilib/cscore/build.gradle
@@ -60,12 +60,12 @@
 
 ext {
     sharedCvConfigs = [cscore    : [],
-                       cscoreBase: [],
-                       cscoreDev : [],
-                       cscoreTest: [],
-                       cscoreJNIShared: []]
+        cscoreBase: [],
+        cscoreDev : [],
+        cscoreTest: [],
+        cscoreJNIShared: []]
     staticCvConfigs = [cscoreJNI: [],
-                       cscoreJNICvStatic: []]
+        cscoreJNICvStatic: []]
     useJava = true
     useCpp = true
     cvStaticBuild = true
@@ -127,43 +127,57 @@
 
 File examplesTree = file("$projectDir/examples")
 examplesTree.list(new FilenameFilter() {
-    @Override
-    public boolean accept(File current, String name) {
-        return new File(current, name).isDirectory();
-    }
-}).each {
-    sharedCvConfigs.put(it, [])
-    examplesMap.put(it, [])
-}
+            @Override
+            public boolean accept(File current, String name) {
+                return new File(current, name).isDirectory();
+            }
+        }).each {
+            sharedCvConfigs.put(it, [])
+            examplesMap.put(it, [])
+        }
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
+Action<List<String>> symbolFilter = { symbols ->
+    symbols.removeIf({ !it.startsWith('CS_') })
+} as Action<List<String>>;
+
 nativeUtils.exportsConfigs {
     cscore {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
     cscoreJNI {
-        x86SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('CS_') })
-        }
-        x64SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('CS_') })
-        }
+        x86SymbolFilter = symbolFilter
+        x64SymbolFilter = symbolFilter
     }
     cscoreJNICvStatic {
-        x86SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('CS_') })
-        }
-        x64SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('CS_') })
-        }
+        x86SymbolFilter = symbolFilter
+        x64SymbolFilter = symbolFilter
     }
 }
 
diff --git a/third_party/allwpilib/cscore/cscore-config.cmake.in b/third_party/allwpilib/cscore/cscore-config.cmake.in
index da85e8b..33e0af3 100644
--- a/third_party/allwpilib/cscore/cscore-config.cmake.in
+++ b/third_party/allwpilib/cscore/cscore-config.cmake.in
@@ -3,4 +3,5 @@
 @WPIUTIL_DEP_REPLACE@
 find_dependency(OpenCV)
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/cscore.cmake)
diff --git a/third_party/allwpilib/cscore/examples/enum_usb/enum_usb.cpp b/third_party/allwpilib/cscore/examples/enum_usb/enum_usb.cpp
index 866e2f1..e9c1225 100644
--- a/third_party/allwpilib/cscore/examples/enum_usb/enum_usb.cpp
+++ b/third_party/allwpilib/cscore/examples/enum_usb/enum_usb.cpp
@@ -1,84 +1,78 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
+#include <cstdio>
+
+#include <fmt/format.h>
 
 #include "cscore.h"
 
 int main() {
   CS_Status status = 0;
-  wpi::SmallString<64> buf;
 
   for (const auto& caminfo : cs::EnumerateUsbCameras(&status)) {
-    wpi::outs() << caminfo.dev << ": " << caminfo.path << " (" << caminfo.name
-                << ")\n";
+    fmt::print("{}: {} ({})\n", caminfo.dev, caminfo.path, caminfo.name);
     if (!caminfo.otherPaths.empty()) {
-      wpi::outs() << "Other device paths:\n";
-      for (auto&& path : caminfo.otherPaths)
-        wpi::outs() << "  " << path << '\n';
+      std::puts("Other device paths:");
+      for (auto&& path : caminfo.otherPaths) {
+        fmt::print("  {}\n", path);
+      }
     }
 
     cs::UsbCamera camera{"usbcam", caminfo.dev};
 
-    wpi::outs() << "Properties:\n";
+    std::puts("Properties:");
     for (const auto& prop : camera.EnumerateProperties()) {
-      wpi::outs() << "  " << prop.GetName();
+      fmt::print("  {}", prop.GetName());
       switch (prop.GetKind()) {
         case cs::VideoProperty::kBoolean:
-          wpi::outs() << " (bool): "
-                      << "value=" << prop.Get()
-                      << " default=" << prop.GetDefault();
+          fmt::print(" (bool): value={} default={}", prop.Get(),
+                     prop.GetDefault());
           break;
         case cs::VideoProperty::kInteger:
-          wpi::outs() << " (int): "
-                      << "value=" << prop.Get() << " min=" << prop.GetMin()
-                      << " max=" << prop.GetMax() << " step=" << prop.GetStep()
-                      << " default=" << prop.GetDefault();
+          fmt::print(" (int): value={} min={} max={} step={} default={}",
+                     prop.Get(), prop.GetMin(), prop.GetMax(), prop.GetStep(),
+                     prop.GetDefault());
           break;
         case cs::VideoProperty::kString:
-          wpi::outs() << " (string): " << prop.GetString(buf);
+          fmt::print(" (string): {}", prop.GetString());
           break;
         case cs::VideoProperty::kEnum: {
-          wpi::outs() << " (enum): "
-                      << "value=" << prop.Get();
+          fmt::print(" (enum): value={}", prop.Get());
           auto choices = prop.GetChoices();
           for (size_t i = 0; i < choices.size(); ++i) {
-            if (choices[i].empty()) continue;
-            wpi::outs() << "\n    " << i << ": " << choices[i];
+            if (!choices[i].empty()) {
+              fmt::print("\n    {}: {}", i, choices[i]);
+            }
           }
           break;
         }
         default:
           break;
       }
-      wpi::outs() << '\n';
+      std::fputc('\n', stdout);
     }
 
-    wpi::outs() << "Video Modes:\n";
+    std::puts("Video Modes:");
     for (const auto& mode : camera.EnumerateVideoModes()) {
-      wpi::outs() << "  PixelFormat:";
+      const char* pixelFormat;
       switch (mode.pixelFormat) {
         case cs::VideoMode::kMJPEG:
-          wpi::outs() << "MJPEG";
+          pixelFormat = "MJPEG";
           break;
         case cs::VideoMode::kYUYV:
-          wpi::outs() << "YUYV";
+          pixelFormat = "YUYV";
           break;
         case cs::VideoMode::kRGB565:
-          wpi::outs() << "RGB565";
+          pixelFormat = "RGB565";
           break;
         default:
-          wpi::outs() << "Unknown";
+          pixelFormat = "Unknown";
           break;
       }
-      wpi::outs() << " Width:" << mode.width;
-      wpi::outs() << " Height:" << mode.height;
-      wpi::outs() << " FPS:" << mode.fps << '\n';
+      fmt::print("  PixelFormat:{} Width:{} Height:{} FPS:{}\n", pixelFormat,
+                 mode.width, mode.height, mode.fps);
     }
   }
 }
diff --git a/third_party/allwpilib/cscore/examples/httpcvstream/httpcvstream.cpp b/third_party/allwpilib/cscore/examples/httpcvstream/httpcvstream.cpp
index 90d61d5..38a00db 100644
--- a/third_party/allwpilib/cscore/examples/httpcvstream/httpcvstream.cpp
+++ b/third_party/allwpilib/cscore/examples/httpcvstream/httpcvstream.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstdio>
-#include <iostream>
 
+#include <fmt/core.h>
 #include <opencv2/core/core.hpp>
 
 #include "cscore.h"
@@ -27,11 +24,11 @@
   for (;;) {
     uint64_t time = cvsink.GrabFrame(test);
     if (time == 0) {
-      std::cout << "error: " << cvsink.GetError() << std::endl;
+      fmt::print("error: {}\n", cvsink.GetError());
       continue;
     }
-    std::cout << "got frame at time " << time << " size " << test.size()
-              << std::endl;
+    fmt::print("got frame at time {} size ({}, {})\n", time, test.size().width,
+               test.size().height);
     cv::flip(test, flip, 0);
     cvsource.PutFrame(flip);
   }
diff --git a/third_party/allwpilib/cscore/examples/settings/settings.cpp b/third_party/allwpilib/cscore/examples/settings/settings.cpp
index 5aa919f..40961b4 100644
--- a/third_party/allwpilib/cscore/examples/settings/settings.cpp
+++ b/third_party/allwpilib/cscore/examples/settings/settings.cpp
@@ -1,28 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
+#include <cstdio>
 #include <thread>
 
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
 
 #include "cscore.h"
 
 int main(int argc, char** argv) {
   if (argc < 2) {
-    wpi::errs() << "Usage: settings camera [prop val] ... -- [prop val]...\n";
-    wpi::errs() << "  Example: settings 1 brightness 30 raw_contrast 10\n";
+    std::fputs("Usage: settings camera [prop val] ... -- [prop val]...\n",
+               stderr);
+    std::fputs("  Example: settings 1 brightness 30 raw_contrast 10\n", stderr);
     return 1;
   }
 
   int id;
-  if (wpi::StringRef{argv[1]}.getAsInteger(10, id)) {
-    wpi::errs() << "Expected number for camera\n";
+  if (auto v = wpi::parse_integer<int>(argv[1], 10)) {
+    id = v.value();
+  } else {
+    std::fputs("Expected number for camera\n", stderr);
     return 2;
   }
 
@@ -30,75 +31,75 @@
 
   // Set prior to connect
   int arg = 2;
-  wpi::StringRef propName;
-  for (; arg < argc && wpi::StringRef{argv[arg]} != "--"; ++arg) {
+  std::string_view propName;
+  for (; arg < argc && std::string_view{argv[arg]} != "--"; ++arg) {
     if (propName.empty()) {
       propName = argv[arg];
     } else {
-      wpi::StringRef propVal{argv[arg]};
-      int intVal;
-      if (propVal.getAsInteger(10, intVal))
+      std::string_view propVal{argv[arg]};
+      if (auto v = wpi::parse_integer<int>(propVal, 10)) {
+        camera.GetProperty(propName).Set(v.value());
+      } else {
         camera.GetProperty(propName).SetString(propVal);
-      else
-        camera.GetProperty(propName).Set(intVal);
-      propName = wpi::StringRef{};
+      }
+      propName = {};
     }
   }
-  if (arg < argc && wpi::StringRef{argv[arg]} == "--") ++arg;
+  if (arg < argc && std::string_view{argv[arg]} == "--") {
+    ++arg;
+  }
 
   // Wait to connect
-  while (!camera.IsConnected())
+  while (!camera.IsConnected()) {
     std::this_thread::sleep_for(std::chrono::milliseconds(10));
+  }
 
   // Set rest
-  propName = wpi::StringRef{};
+  propName = {};
   for (; arg < argc; ++arg) {
     if (propName.empty()) {
       propName = argv[arg];
     } else {
-      wpi::StringRef propVal{argv[arg]};
-      int intVal;
-      if (propVal.getAsInteger(10, intVal))
+      std::string_view propVal{argv[arg]};
+      if (auto v = wpi::parse_integer<int>(propVal, 10)) {
+        camera.GetProperty(propName).Set(v.value());
+      } else {
         camera.GetProperty(propName).SetString(propVal);
-      else
-        camera.GetProperty(propName).Set(intVal);
-      propName = wpi::StringRef{};
+      }
+      propName = {};
     }
   }
 
   // Print settings
-  wpi::SmallString<64> buf;
-  wpi::outs() << "Properties:\n";
+  std::puts("Properties:");
   for (const auto& prop : camera.EnumerateProperties()) {
-    wpi::outs() << "  " << prop.GetName();
+    fmt::print("  {}", prop.GetName());
     switch (prop.GetKind()) {
       case cs::VideoProperty::kBoolean:
-        wpi::outs() << " (bool): "
-                    << "value=" << prop.Get()
-                    << " default=" << prop.GetDefault();
+        fmt::print(" (bool): value={} default={}", prop.Get(),
+                   prop.GetDefault());
         break;
       case cs::VideoProperty::kInteger:
-        wpi::outs() << " (int): "
-                    << "value=" << prop.Get() << " min=" << prop.GetMin()
-                    << " max=" << prop.GetMax() << " step=" << prop.GetStep()
-                    << " default=" << prop.GetDefault();
+        fmt::print(" (int): value={} min={} max={} step={} default={}",
+                   prop.Get(), prop.GetMin(), prop.GetMax(), prop.GetStep(),
+                   prop.GetDefault());
         break;
       case cs::VideoProperty::kString:
-        wpi::outs() << " (string): " << prop.GetString(buf);
+        fmt::print(" (string): {}", prop.GetString());
         break;
       case cs::VideoProperty::kEnum: {
-        wpi::outs() << " (enum): "
-                    << "value=" << prop.Get();
+        fmt::print(" (enum): value={}", prop.Get());
         auto choices = prop.GetChoices();
         for (size_t i = 0; i < choices.size(); ++i) {
-          if (choices[i].empty()) continue;
-          wpi::outs() << "\n    " << i << ": " << choices[i];
+          if (!choices[i].empty()) {
+            fmt::print("\n    {}: {}", i, choices[i]);
+          }
         }
         break;
       }
       default:
         break;
     }
-    wpi::outs() << '\n';
+    std::fputc('\n', stdout);
   }
 }
diff --git a/third_party/allwpilib/cscore/examples/usbcvstream/usbcvstream.cpp b/third_party/allwpilib/cscore/examples/usbcvstream/usbcvstream.cpp
index 9a4ab06..3635221 100644
--- a/third_party/allwpilib/cscore/examples/usbcvstream/usbcvstream.cpp
+++ b/third_party/allwpilib/cscore/examples/usbcvstream/usbcvstream.cpp
@@ -1,13 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <cstdio>
-#include <iostream>
-
+#include <fmt/core.h>
 #include <opencv2/core/core.hpp>
 
 #include "cscore.h"
@@ -29,11 +24,11 @@
   for (;;) {
     uint64_t time = cvsink.GrabFrame(test);
     if (time == 0) {
-      std::cout << "error: " << cvsink.GetError() << std::endl;
+      fmt::print("error: {}\n", cvsink.GetError());
       continue;
     }
-    std::cout << "got frame at time " << time << " size " << test.size()
-              << std::endl;
+    fmt::print("got frame at time {} size ({}, {})\n", time, test.size().width,
+               test.size().height);
     cv::flip(test, flip, 0);
     cvsource.PutFrame(flip);
   }
diff --git a/third_party/allwpilib/cscore/examples/usbstream/usbstream.cpp b/third_party/allwpilib/cscore/examples/usbstream/usbstream.cpp
index 2f23151..7a57e8d 100644
--- a/third_party/allwpilib/cscore/examples/usbstream/usbstream.cpp
+++ b/third_party/allwpilib/cscore/examples/usbstream/usbstream.cpp
@@ -1,21 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstdio>
 
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 #include "cscore.h"
 
 int main() {
-  wpi::outs() << "hostname: " << cs::GetHostname() << '\n';
-  wpi::outs() << "IPv4 network addresses:\n";
-  for (const auto& addr : cs::GetNetworkInterfaces())
-    wpi::outs() << "  " << addr << '\n';
+  fmt::print("hostname: {}\n", cs::GetHostname());
+  std::puts("IPv4 network addresses:");
+  for (const auto& addr : cs::GetNetworkInterfaces()) {
+    fmt::print("  {}\n", addr);
+  }
   cs::UsbCamera camera{"usbcam", 0};
   camera.SetVideoMode(cs::VideoMode::kMJPEG, 320, 240, 30);
   cs::MjpegServer mjpegServer{"httpserver", 8081};
@@ -24,9 +22,8 @@
   CS_Status status = 0;
   cs::AddListener(
       [&](const cs::RawEvent& event) {
-        wpi::outs() << "FPS=" << camera.GetActualFPS()
-                    << " MBPS=" << (camera.GetActualDataRate() / 1000000.0)
-                    << '\n';
+        fmt::print("FPS={} MBPS={}\n", camera.GetActualFPS(),
+                   (camera.GetActualDataRate() / 1000000.0));
       },
       cs::RawEvent::kTelemetryUpdated, false, &status);
   cs::SetTelemetryPeriod(1.0);
diff --git a/third_party/allwpilib/cscore/examples/usbviewer/usbviewer.cpp b/third_party/allwpilib/cscore/examples/usbviewer/usbviewer.cpp
index 75f961a..5c2e7c9 100644
--- a/third_party/allwpilib/cscore/examples/usbviewer/usbviewer.cpp
+++ b/third_party/allwpilib/cscore/examples/usbviewer/usbviewer.cpp
@@ -1,20 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <atomic>
 #include <thread>
 #include <vector>
 
+#include <fmt/format.h>
+
 #define IMGUI_DEFINE_MATH_OPERATORS
 #include <imgui.h>
 #include <imgui_internal.h>
 #include <opencv2/core/core.hpp>
 #include <opencv2/imgproc.hpp>
-#include <wpi/raw_ostream.h>
 #include <wpi/spinlock.h>
 #include <wpigui.h>
 
@@ -41,7 +39,7 @@
       // get frame from camera
       uint64_t time = cvsink.GrabFrame(frame);
       if (time == 0) {
-        wpi::outs() << "error: " << cvsink.GetError() << '\n';
+        fmt::print("error: {}\n", cvsink.GetError());
         continue;
       }
 
@@ -53,7 +51,9 @@
       } else {
         {
           std::scoped_lock lock(sharedFreeListMutex);
-          for (auto mat : sharedFreeList) sourceFreeList.emplace_back(mat);
+          for (auto mat : sharedFreeList) {
+            sourceFreeList.emplace_back(mat);
+          }
           sharedFreeList.clear();
         }
         if (!sourceFreeList.empty()) {
@@ -71,7 +71,9 @@
       auto prev = latestFrame.exchange(out);
 
       // put prev on free list
-      if (prev) sourceFreeList.emplace_back(prev);
+      if (prev) {
+        sourceFreeList.emplace_back(prev);
+      }
     }
   });
 
diff --git a/third_party/allwpilib/cscore/java-examples/RawCVMatSink.java b/third_party/allwpilib/cscore/java-examples/RawCVMatSink.java
index d9557f7..c0acc28 100644
--- a/third_party/allwpilib/cscore/java-examples/RawCVMatSink.java
+++ b/third_party/allwpilib/cscore/java-examples/RawCVMatSink.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.cscore;
 
-import java.nio.ByteBuffer;
-
-import org.opencv.core.CvType;
-import org.opencv.core.Mat;
-
 import edu.wpi.cscore.VideoMode.PixelFormat;
 import edu.wpi.cscore.raw.RawFrame;
+import java.nio.ByteBuffer;
+import org.opencv.core.CvType;
+import org.opencv.core.Mat;
 
 public class RawCVMatSink extends ImageSink {
   RawFrame frame = new RawFrame();
@@ -27,26 +22,25 @@
   private int getCVFormat(PixelFormat pixelFormat) {
     int type = 0;
     switch (pixelFormat) {
-    case kYUYV:
-    case kRGB565:
-      type = CvType.CV_8UC2;
-      break;
-    case kBGR:
-      type = CvType.CV_8UC3;
-      break;
-    case kGray:
-    case kMJPEG:
-    default:
-      type = CvType.CV_8UC1;
-      break;
+      case kYUYV:
+      case kRGB565:
+        type = CvType.CV_8UC2;
+        break;
+      case kBGR:
+        type = CvType.CV_8UC3;
+        break;
+      case kGray:
+      case kMJPEG:
+      default:
+        type = CvType.CV_8UC1;
+        break;
     }
     return type;
   }
 
   /**
-   * Create a sink for accepting OpenCV images.
-   * WaitForFrame() must be called on the created sink to get each new
-   * image.
+   * Create a sink for accepting OpenCV images. WaitForFrame() must be called on the created sink to
+   * get each new image.
    *
    * @param name Source name (arbitrary unique identifier)
    */
@@ -55,24 +49,21 @@
   }
 
   /**
-   * Wait for the next frame and get the image.
-   * Times out (returning 0) after 0.225 seconds.
-   * The provided image will have three 3-bit channels stored in BGR order.
+   * Wait for the next frame and get the image. Times out (returning 0) after 0.225 seconds. The
+   * provided image will have three 3-bit channels stored in BGR order.
    *
-   * @return Frame time, or 0 on error (call GetError() to obtain the error
-   *         message)
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message)
    */
   public long grabFrame(Mat image) {
     return grabFrame(image, 0.225);
   }
 
   /**
-   * Wait for the next frame and get the image.
-   * Times out (returning 0) after timeout seconds.
-   * The provided image will have three 3-bit channels stored in BGR order.
+   * Wait for the next frame and get the image. Times out (returning 0) after timeout seconds. The
+   * provided image will have three 3-bit channels stored in BGR order.
    *
-   * @return Frame time, or 0 on error (call GetError() to obtain the error
-   *         message); the frame time is in 1 us increments.
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
+   *     is in 1 us increments.
    */
   public long grabFrame(Mat image, double timeout) {
     frame.setWidth(0);
@@ -83,12 +74,20 @@
       return rv;
     }
 
-    if (frame.getDataByteBuffer() != origByteBuffer || width != frame.getWidth() || height != frame.getHeight() || pixelFormat != frame.getPixelFormat()) {
+    if (frame.getDataByteBuffer() != origByteBuffer
+        || width != frame.getWidth()
+        || height != frame.getHeight()
+        || pixelFormat != frame.getPixelFormat()) {
       origByteBuffer = frame.getDataByteBuffer();
       height = frame.getHeight();
       width = frame.getWidth();
       pixelFormat = frame.getPixelFormat();
-      tmpMat = new Mat(frame.getHeight(), frame.getWidth(), getCVFormat(VideoMode.getPixelFormatFromInt(pixelFormat)), origByteBuffer);
+      tmpMat =
+          new Mat(
+              frame.getHeight(),
+              frame.getWidth(),
+              getCVFormat(VideoMode.getPixelFormatFromInt(pixelFormat)),
+              origByteBuffer);
     }
     tmpMat.copyTo(image);
     return rv;
diff --git a/third_party/allwpilib/cscore/java-examples/RawCVMatSource.java b/third_party/allwpilib/cscore/java-examples/RawCVMatSource.java
index 65bd7d2..e000ae3 100644
--- a/third_party/allwpilib/cscore/java-examples/RawCVMatSource.java
+++ b/third_party/allwpilib/cscore/java-examples/RawCVMatSource.java
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.cscore;
 
-import org.opencv.core.Mat;
-
 import edu.wpi.cscore.VideoMode.PixelFormat;
+import org.opencv.core.Mat;
 
 public class RawCVMatSource extends ImageSource {
   /**
@@ -19,11 +15,9 @@
    * @param mode Video mode being generated
    */
   public RawCVMatSource(String name, VideoMode mode) {
-    super(CameraServerJNI.createRawSource(name,
-        mode.pixelFormat.getValue(),
-        mode.width,
-        mode.height,
-        mode.fps));
+    super(
+        CameraServerJNI.createRawSource(
+            name, mode.pixelFormat.getValue(), mode.width, mode.height, mode.fps));
   }
 
   /**
@@ -35,16 +29,17 @@
    * @param height height
    * @param fps fps
    */
-  public RawCVMatSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+  public RawCVMatSource(
+      String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
     super(CameraServerJNI.createRawSource(name, pixelFormat.getValue(), width, height, fps));
   }
 
   /**
    * Put an OpenCV image and notify sinks.
    *
-   * <p>Only 8-bit single-channel or 3-channel (with BGR channel order) images
-   * are supported. If the format, depth or channel order is different, use
-   * Mat.convertTo() and/or cvtColor() to convert it first.
+   * <p>Only 8-bit single-channel or 3-channel (with BGR channel order) images are supported. If the
+   * format, depth or channel order is different, use Mat.convertTo() and/or cvtColor() to convert
+   * it first.
    *
    * @param image OpenCV image
    */
@@ -54,6 +49,12 @@
       throw new VideoException("Unsupported Image Type");
     }
     int imgType = channels == 1 ? PixelFormat.kGray.getValue() : PixelFormat.kBGR.getValue();
-    CameraServerJNI.putRawSourceFrame(m_handle, image.dataAddr(), image.width(), image.height(), imgType, (int)image.total() * channels);
+    CameraServerJNI.putRawSourceFrame(
+        m_handle,
+        image.dataAddr(),
+        image.width(),
+        image.height(),
+        imgType,
+        (int) image.total() * channels);
   }
 }
diff --git a/third_party/allwpilib/cscore/src/dev/java/edu/wpi/cscore/DevMain.java b/third_party/allwpilib/cscore/src/dev/java/edu/wpi/cscore/DevMain.java
deleted file mode 100644
index 51bfd26..0000000
--- a/third_party/allwpilib/cscore/src/dev/java/edu/wpi/cscore/DevMain.java
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import edu.wpi.first.wpiutil.RuntimeDetector;
-
-public final class DevMain {
-  /**
-   * Main method.
-   */
-  public static void main(String[] args) {
-    System.out.println("Hello World!");
-    System.out.println(RuntimeDetector.getPlatformPath());
-    System.out.println(CameraServerJNI.getHostname());
-  }
-
-  private DevMain() {
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/dev/java/edu/wpi/first/cscore/DevMain.java b/third_party/allwpilib/cscore/src/dev/java/edu/wpi/first/cscore/DevMain.java
new file mode 100644
index 0000000..11652f6
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/dev/java/edu/wpi/first/cscore/DevMain.java
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import edu.wpi.first.util.RuntimeDetector;
+
+public final class DevMain {
+  /** Main method. */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    System.out.println(RuntimeDetector.getPlatformPath());
+    System.out.println(CameraServerJNI.getHostname());
+  }
+
+  private DevMain() {}
+}
diff --git a/third_party/allwpilib/cscore/src/dev/native/cpp/main.cpp b/third_party/allwpilib/cscore/src/dev/native/cpp/main.cpp
index f27f61f..279d72a 100644
--- a/third_party/allwpilib/cscore/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/cscore/src/dev/native/cpp/main.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
+#include <fmt/core.h>
 
 #include "cscore.h"
 
-int main() { std::cout << cs::GetHostname() << std::endl; }
+int main() {
+  fmt::print("{}\n", cs::GetHostname());
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/AxisCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/AxisCamera.java
deleted file mode 100644
index 8eef0b3..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/AxisCamera.java
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source that represents an Axis IP camera.
- */
-public class AxisCamera extends HttpCamera {
-  private static String hostToUrl(String host) {
-    return "http://" + host + "/mjpg/video.mjpg";
-  }
-
-  private static String[] hostToUrl(String[] hosts) {
-    String[] urls = new String[hosts.length];
-    for (int i = 0; i < hosts.length; i++) {
-      urls[i] = hostToUrl(hosts[i]);
-    }
-    return urls;
-  }
-
-  /**
-   * Create a source for an Axis IP camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   */
-  public AxisCamera(String name, String host) {
-    super(name, hostToUrl(host), HttpCameraKind.kAxis);
-  }
-
-  /**
-   * Create a source for an Axis IP camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param hosts Array of Camera host IPs/DNS names
-   */
-  public AxisCamera(String name, String[] hosts) {
-    super(name, hostToUrl(hosts), HttpCameraKind.kAxis);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CameraServerCvJNI.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CameraServerCvJNI.java
deleted file mode 100644
index 78b4ff8..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CameraServerCvJNI.java
+++ /dev/null
@@ -1,72 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import java.io.IOException;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-import org.opencv.core.Core;
-
-import edu.wpi.first.wpiutil.RuntimeLoader;
-
-public class CameraServerCvJNI {
-  static boolean libraryLoaded = false;
-
-  static RuntimeLoader<Core> loader = null;
-
-  public static class Helper {
-    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
-
-    public static boolean getExtractOnStaticLoad() {
-      return extractOnStaticLoad.get();
-    }
-
-    public static void setExtractOnStaticLoad(boolean load) {
-      extractOnStaticLoad.set(load);
-    }
-  }
-
-  static {
-    String opencvName = Core.NATIVE_LIBRARY_NAME;
-    if (Helper.getExtractOnStaticLoad()) {
-      try {
-        CameraServerJNI.forceLoad();
-        loader = new RuntimeLoader<>(opencvName, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
-        loader.loadLibraryHashed();
-      } catch (IOException ex) {
-        ex.printStackTrace();
-        System.exit(1);
-      }
-      libraryLoaded = true;
-    }
-  }
-
-  /**
-   * Force load the library.
-   */
-  public static synchronized void forceLoad() throws IOException {
-    if (libraryLoaded) {
-      return;
-    }
-    CameraServerJNI.forceLoad();
-    loader = new RuntimeLoader<>(Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
-    loader.loadLibrary();
-    libraryLoaded = true;
-  }
-
-  public static native int createCvSource(String name, int pixelFormat, int width, int height, int fps);
-
-  public static native void putSourceFrame(int source, long imageNativeObj);
-
-  public static native int createCvSink(String name);
-  //public static native int createCvSinkCallback(String name,
-  //                            void (*processFrame)(long time));
-
-  public static native long grabSinkFrame(int sink, long imageNativeObj);
-  public static native long grabSinkFrameTimeout(int sink, long imageNativeObj, double timeout);
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java
deleted file mode 100644
index b8a548c..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CameraServerJNI.java
+++ /dev/null
@@ -1,256 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import java.io.IOException;
-import java.nio.ByteBuffer;
-import java.util.concurrent.atomic.AtomicBoolean;
-import java.util.function.Consumer;
-
-import edu.wpi.cscore.raw.RawFrame;
-import edu.wpi.first.wpiutil.RuntimeLoader;
-
-public class CameraServerJNI {
-  static boolean libraryLoaded = false;
-
-  static RuntimeLoader<CameraServerJNI> loader = null;
-
-  public static class Helper {
-    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
-
-    public static boolean getExtractOnStaticLoad() {
-      return extractOnStaticLoad.get();
-    }
-
-    public static void setExtractOnStaticLoad(boolean load) {
-      extractOnStaticLoad.set(load);
-    }
-  }
-
-  static {
-    if (Helper.getExtractOnStaticLoad()) {
-      try {
-        loader = new RuntimeLoader<>("cscorejni", RuntimeLoader.getDefaultExtractionRoot(), CameraServerJNI.class);
-        loader.loadLibrary();
-      } catch (IOException ex) {
-        ex.printStackTrace();
-        System.exit(1);
-      }
-      libraryLoaded = true;
-    }
-  }
-
-  /**
-   * Force load the library.
-   */
-  public static synchronized void forceLoad() throws IOException {
-    if (libraryLoaded) {
-      return;
-    }
-    loader = new RuntimeLoader<>("cscorejni", RuntimeLoader.getDefaultExtractionRoot(), CameraServerJNI.class);
-    loader.loadLibrary();
-    libraryLoaded = true;
-  }
-
-  //
-  // Property Functions
-  //
-  public static native int getPropertyKind(int property);
-  public static native String getPropertyName(int property);
-  public static native int getProperty(int property);
-  public static native void setProperty(int property, int value);
-  public static native int getPropertyMin(int property);
-  public static native int getPropertyMax(int property);
-  public static native int getPropertyStep(int property);
-  public static native int getPropertyDefault(int property);
-  public static native String getStringProperty(int property);
-  public static native void setStringProperty(int property, String value);
-  public static native String[] getEnumPropertyChoices(int property);
-
-  //
-  // Source Creation Functions
-  //
-  public static native int createUsbCameraDev(String name, int dev);
-  public static native int createUsbCameraPath(String name, String path);
-  public static native int createHttpCamera(String name, String url, int kind);
-  public static native int createHttpCameraMulti(String name, String[] urls, int kind);
-  public static native int createRawSource(String name, int pixelFormat, int width, int height, int fps);
-
-  //
-  // Source Functions
-  //
-  public static native int getSourceKind(int source);
-  public static native String getSourceName(int source);
-  public static native String getSourceDescription(int source);
-  public static native long getSourceLastFrameTime(int source);
-  public static native void setSourceConnectionStrategy(int source, int strategy);
-  public static native boolean isSourceConnected(int source);
-  public static native boolean isSourceEnabled(int source);
-  public static native int getSourceProperty(int source, String name);
-  public static native int[] enumerateSourceProperties(int source);
-  public static native VideoMode getSourceVideoMode(int source);
-  public static native boolean setSourceVideoMode(int source, int pixelFormat, int width, int height, int fps);
-  public static native boolean setSourcePixelFormat(int source, int pixelFormat);
-  public static native boolean setSourceResolution(int source, int width, int height);
-  public static native boolean setSourceFPS(int source, int fps);
-  public static native boolean setSourceConfigJson(int source, String config);
-  public static native String getSourceConfigJson(int source);
-  public static native VideoMode[] enumerateSourceVideoModes(int source);
-  public static native int[] enumerateSourceSinks(int source);
-  public static native int copySource(int source);
-  public static native void releaseSource(int source);
-
-  //
-  // Camera Source Common Property Fuctions
-  //
-  public static native void setCameraBrightness(int source, int brightness);
-  public static native int getCameraBrightness(int source);
-  public static native void setCameraWhiteBalanceAuto(int source);
-  public static native void setCameraWhiteBalanceHoldCurrent(int source);
-  public static native void setCameraWhiteBalanceManual(int source, int value);
-  public static native void setCameraExposureAuto(int source);
-  public static native void setCameraExposureHoldCurrent(int source);
-  public static native void setCameraExposureManual(int source, int value);
-
-  //
-  // UsbCamera Source Functions
-  //
-  public static native void setUsbCameraPath(int source, String path);
-  public static native String getUsbCameraPath(int source);
-  public static native UsbCameraInfo getUsbCameraInfo(int source);
-
-  //
-  // HttpCamera Source Functions
-  //
-  public static native int getHttpCameraKind(int source);
-  public static native void setHttpCameraUrls(int source, String[] urls);
-  public static native String[] getHttpCameraUrls(int source);
-
-  //
-  // Image Source Functions
-  //
-  public static native void putRawSourceFrameBB(int source, ByteBuffer data, int width, int height, int pixelFormat, int totalData);
-  public static native void putRawSourceFrame(int source, long data, int width, int height, int pixelFormat, int totalData);
-  public static void putRawSourceFrame(int source, RawFrame raw) {
-    putRawSourceFrame(source, raw.getDataPtr(), raw.getWidth(), raw.getHeight(), raw.getPixelFormat(), raw.getTotalData());
-  }
-  public static native void notifySourceError(int source, String msg);
-  public static native void setSourceConnected(int source, boolean connected);
-  public static native void setSourceDescription(int source, String description);
-  public static native int createSourceProperty(int source, String name, int kind, int minimum, int maximum, int step, int defaultValue, int value);
-  public static native void setSourceEnumPropertyChoices(int source, int property, String[] choices);
-
-  //
-  // Sink Creation Functions
-  //
-  public static native int createMjpegServer(String name, String listenAddress, int port);
-
-  public static native int createRawSink(String name);
-
-  //
-  // Sink Functions
-  //
-  public static native int getSinkKind(int sink);
-  public static native String getSinkName(int sink);
-  public static native String getSinkDescription(int sink);
-  public static native int getSinkProperty(int sink, String name);
-  public static native int[] enumerateSinkProperties(int sink);
-  public static native boolean setSinkConfigJson(int sink, String config);
-  public static native String getSinkConfigJson(int sink);
-  public static native void setSinkSource(int sink, int source);
-  public static native int getSinkSourceProperty(int sink, String name);
-  public static native int getSinkSource(int sink);
-  public static native int copySink(int sink);
-  public static native void releaseSink(int sink);
-
-  //
-  // MjpegServer Sink Functions
-  //
-  public static native String getMjpegServerListenAddress(int sink);
-  public static native int getMjpegServerPort(int sink);
-
-  //
-  // Image Sink Functions
-  //
-  public static native void setSinkDescription(int sink, String description);
-
-  private static native long grabRawSinkFrameImpl(int sink, RawFrame rawFrame, long rawFramePtr, ByteBuffer byteBuffer, int width, int height, int pixelFormat);
-  private static native long grabRawSinkFrameTimeoutImpl(int sink, RawFrame rawFrame, long rawFramePtr, ByteBuffer byteBuffer, int width, int height, int pixelFormat, double timeout);
-
-  public static long grabSinkFrame(int sink, RawFrame rawFrame) {
-    return grabRawSinkFrameImpl(sink, rawFrame, rawFrame.getFramePtr(), rawFrame.getDataByteBuffer(), rawFrame.getWidth(), rawFrame.getHeight(), rawFrame.getPixelFormat());
-  }
-  public static long grabSinkFrameTimeout(int sink, RawFrame rawFrame, double timeout) {
-    return grabRawSinkFrameTimeoutImpl(sink, rawFrame, rawFrame.getFramePtr(), rawFrame.getDataByteBuffer(), rawFrame.getWidth(), rawFrame.getHeight(), rawFrame.getPixelFormat(), timeout);
-  }
-  public static native String getSinkError(int sink);
-  public static native void setSinkEnabled(int sink, boolean enabled);
-
-  //
-  // Listener Functions
-  //
-  public static native int addListener(Consumer<VideoEvent> listener,
-                                       int eventMask, boolean immediateNotify);
-
-  public static native void removeListener(int handle);
-
-  //
-  // Telemetry Functions
-  //
-  public enum TelemetryKind {
-    kSourceBytesReceived(1),
-    kSourceFramesReceived(2);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    TelemetryKind(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-  public static native void setTelemetryPeriod(double seconds);
-  public static native double getTelemetryElapsedTime();
-  public static native long getTelemetryValue(int handle, int kind);
-  public static long getTelemetryValue(int handle, TelemetryKind kind) {
-    return getTelemetryValue(handle, kind.getValue());
-  }
-  public static native double getTelemetryAverageValue(int handle, int kind);
-  public static double getTelemetryAverageValue(int handle, TelemetryKind kind) {
-    return getTelemetryAverageValue(handle, kind.getValue());
-  }
-
-  //
-  // Logging Functions
-  //
-  @FunctionalInterface
-  public interface LoggerFunction {
-    void apply(int level, String file, int line, String msg);
-  }
-  public static native void setLogger(LoggerFunction func, int minLevel);
-
-  //
-  // Utility Functions
-  //
-  public static native UsbCameraInfo[] enumerateUsbCameras();
-
-  public static native int[] enumerateSources();
-
-  public static native int[] enumerateSinks();
-
-  public static native String getHostname();
-
-  public static native String[] getNetworkInterfaces();
-
-  public static native long allocateRawFrame();
-
-  public static native void freeRawFrame(long frame);
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CvSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CvSink.java
deleted file mode 100644
index f12dcc7..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CvSink.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import org.opencv.core.Mat;
-
-/**
- * A sink for user code to accept video frames as OpenCV images.
- * These sinks require the WPILib OpenCV builds.
- * For an alternate OpenCV, see the documentation how to build your own
- * with RawSink.
- */
-public class CvSink extends ImageSink {
-  /**
-   * Create a sink for accepting OpenCV images.
-   * WaitForFrame() must be called on the created sink to get each new
-   * image.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   */
-  public CvSink(String name) {
-    super(CameraServerCvJNI.createCvSink(name));
-  }
-
-  /// Create a sink for accepting OpenCV images in a separate thread.
-  /// A thread will be created that calls WaitForFrame() and calls the
-  /// processFrame() callback each time a new frame arrives.
-  /// @param name Source name (arbitrary unique identifier)
-  /// @param processFrame Frame processing function; will be called with a
-  ///        time=0 if an error occurred.  processFrame should call GetImage()
-  ///        or GetError() as needed, but should not call (except in very
-  ///        unusual circumstances) WaitForImage().
-  //public CvSink(wpi::StringRef name,
-  //       std::function<void(uint64_t time)> processFrame) {
-  //  super(CameraServerJNI.createCvSinkCallback(name, processFrame));
-  //}
-
-  /**
-   * Wait for the next frame and get the image.
-   * Times out (returning 0) after 0.225 seconds.
-   * The provided image will have three 3-bit channels stored in BGR order.
-   *
-   * @return Frame time, or 0 on error (call GetError() to obtain the error
-   *         message)
-   */
-  public long grabFrame(Mat image) {
-    return grabFrame(image, 0.225);
-  }
-
-  /**
-   * Wait for the next frame and get the image.
-   * Times out (returning 0) after timeout seconds.
-   * The provided image will have three 3-bit channels stored in BGR order.
-   *
-   * @return Frame time, or 0 on error (call GetError() to obtain the error
-   *         message); the frame time is in 1 us increments.
-   */
-  public long grabFrame(Mat image, double timeout) {
-    return CameraServerCvJNI.grabSinkFrameTimeout(m_handle, image.nativeObj, timeout);
-  }
-
-  /**
-   * Wait for the next frame and get the image.  May block forever.
-   * The provided image will have three 3-bit channels stored in BGR order.
-   *
-   * @return Frame time, or 0 on error (call GetError() to obtain the error
-   *         message); the frame time is in 1 us increments.
-   */
-  public long grabFrameNoTimeout(Mat image) {
-    return CameraServerCvJNI.grabSinkFrame(m_handle, image.nativeObj);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CvSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CvSource.java
deleted file mode 100644
index c04d197..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/CvSource.java
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import org.opencv.core.Mat;
-
-/**
- * A source that represents a video camera.
- * These sources require the WPILib OpenCV builds.
- * For an alternate OpenCV, see the documentation how to build your own
- * with RawSource.
- */
-public class CvSource extends ImageSource {
-  /**
-   * Create an OpenCV source.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param mode Video mode being generated
-   */
-  public CvSource(String name, VideoMode mode) {
-    super(CameraServerCvJNI.createCvSource(name,
-        mode.pixelFormat.getValue(),
-        mode.width,
-        mode.height,
-        mode.fps));
-  }
-
-  /**
-   * Create an OpenCV source.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param pixelFormat Pixel format
-   * @param width width
-   * @param height height
-   * @param fps fps
-   */
-  public CvSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
-    super(CameraServerCvJNI.createCvSource(name, pixelFormat.getValue(), width, height, fps));
-  }
-
-  /**
-   * Put an OpenCV image and notify sinks.
-   *
-   * <p>Only 8-bit single-channel or 3-channel (with BGR channel order) images
-   * are supported. If the format, depth or channel order is different, use
-   * Mat.convertTo() and/or cvtColor() to convert it first.
-   *
-   * @param image OpenCV image
-   */
-  public void putFrame(Mat image) {
-    CameraServerCvJNI.putSourceFrame(m_handle, image.nativeObj);
-  }
-
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/HttpCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/HttpCamera.java
deleted file mode 100644
index 533d00f..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/HttpCamera.java
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source that represents a MJPEG-over-HTTP (IP) camera.
- */
-public class HttpCamera extends VideoCamera {
-  public enum HttpCameraKind {
-    kUnknown(0), kMJPGStreamer(1), kCSCore(2), kAxis(3);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    HttpCameraKind(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /**
-   * Convert from the numerical representation of kind to an enum type.
-   *
-   * @param kind The numerical representation of kind
-   * @return The kind
-   */
-  public static HttpCameraKind getHttpCameraKindFromInt(int kind) {
-    switch (kind) {
-      case 1: return HttpCameraKind.kMJPGStreamer;
-      case 2: return HttpCameraKind.kCSCore;
-      case 3: return HttpCameraKind.kAxis;
-      default: return HttpCameraKind.kUnknown;
-    }
-  }
-
-  /**
-   * Create a source for a MJPEG-over-HTTP (IP) camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
-   */
-  public HttpCamera(String name, String url) {
-    super(CameraServerJNI.createHttpCamera(name, url, HttpCameraKind.kUnknown.getValue()));
-  }
-
-  /**
-   * Create a source for a MJPEG-over-HTTP (IP) camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
-   * @param kind Camera kind (e.g. kAxis)
-   */
-  public HttpCamera(String name, String url, HttpCameraKind kind) {
-    super(CameraServerJNI.createHttpCamera(name, url, kind.getValue()));
-  }
-
-  /**
-   * Create a source for a MJPEG-over-HTTP (IP) camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param urls Array of Camera URLs
-   */
-  public HttpCamera(String name, String[] urls) {
-    super(CameraServerJNI.createHttpCameraMulti(name, urls, HttpCameraKind.kUnknown.getValue()));
-  }
-
-  /**
-   * Create a source for a MJPEG-over-HTTP (IP) camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param urls Array of Camera URLs
-   * @param kind Camera kind (e.g. kAxis)
-   */
-  public HttpCamera(String name, String[] urls, HttpCameraKind kind) {
-    super(CameraServerJNI.createHttpCameraMulti(name, urls, kind.getValue()));
-  }
-
-  /**
-   * Get the kind of HTTP camera.
-   *
-   * <p>Autodetection can result in returning a different value than the camera
-   * was created with.
-   */
-  public HttpCameraKind getHttpCameraKind() {
-    return getHttpCameraKindFromInt(CameraServerJNI.getHttpCameraKind(m_handle));
-  }
-
-  /**
-   * Change the URLs used to connect to the camera.
-   */
-  public void setUrls(String[] urls) {
-    CameraServerJNI.setHttpCameraUrls(m_handle, urls);
-  }
-
-  /**
-   * Get the URLs used to connect to the camera.
-   */
-  public String[] getUrls() {
-    return CameraServerJNI.getHttpCameraUrls(m_handle);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/ImageSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/ImageSink.java
deleted file mode 100644
index f755fb6..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/ImageSink.java
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-public abstract class ImageSink extends VideoSink {
-  protected ImageSink(int handle) {
-    super(handle);
-  }
-
-  /**
-   * Set sink description.
-   *
-   * @param description Description
-   */
-  public void setDescription(String description) {
-    CameraServerJNI.setSinkDescription(m_handle, description);
-  }
-
-  /**
-   * Get error string.  Call this if WaitForFrame() returns 0 to determine
-   * what the error is.
-   */
-  public String getError() {
-    return CameraServerJNI.getSinkError(m_handle);
-  }
-
-  /**
-   * Enable or disable getting new frames.
-   * Disabling will cause processFrame (for callback-based CvSinks) to not
-   * be called and WaitForFrame() to not return.  This can be used to save
-   * processor resources when frames are not needed.
-   */
-  public void setEnabled(boolean enabled) {
-    CameraServerJNI.setSinkEnabled(m_handle, enabled);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/ImageSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/ImageSource.java
deleted file mode 100644
index 3787516..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/ImageSource.java
+++ /dev/null
@@ -1,162 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-public abstract class ImageSource extends VideoSource {
-  protected ImageSource(int handle) {
-    super(handle);
-  }
-
-  /**
-   * Signal sinks that an error has occurred.  This should be called instead
-   * of NotifyFrame when an error occurs.
-   */
-  public void notifyError(String msg) {
-    CameraServerJNI.notifySourceError(m_handle, msg);
-  }
-
-  /**
-   * Set source connection status.  Defaults to true.
-   *
-   * @param connected True for connected, false for disconnected
-   */
-  public void setConnected(boolean connected) {
-    CameraServerJNI.setSourceConnected(m_handle, connected);
-  }
-
-  /**
-   * Set source description.
-   *
-   * @param description Description
-   */
-  public void setDescription(String description) {
-    CameraServerJNI.setSourceDescription(m_handle, description);
-  }
-
-  /**
-   * Create a property.
-   *
-   * @param name Property name
-   * @param kind Property kind
-   * @param minimum Minimum value
-   * @param maximum Maximum value
-   * @param step Step value
-   * @param defaultValue Default value
-   * @param value Current value
-   * @return Property
-   */
-  public VideoProperty createProperty(String name,
-                                      VideoProperty.Kind kind,
-                                      int minimum,
-                                      int maximum,
-                                      int step,
-                                      int defaultValue,
-                                      int value) {
-    return new VideoProperty(
-        CameraServerJNI.createSourceProperty(m_handle,
-            name,
-            kind.getValue(),
-            minimum,
-            maximum,
-            step,
-            defaultValue,
-            value));
-  }
-
-  /**
-   * Create an integer property.
-   *
-   * @param name Property name
-   * @param minimum Minimum value
-   * @param maximum Maximum value
-   * @param step Step value
-   * @param defaultValue Default value
-   * @param value Current value
-   * @return Property
-   */
-  public VideoProperty createIntegerProperty(String name,
-                                             int minimum,
-                                             int maximum,
-                                             int step,
-                                             int defaultValue,
-                                             int value) {
-    return new VideoProperty(
-        CameraServerJNI.createSourceProperty(m_handle,
-            name,
-            VideoProperty.Kind.kInteger.getValue(),
-            minimum,
-            maximum,
-            step,
-            defaultValue,
-            value));
-  }
-
-  /**
-   * Create a boolean property.
-   *
-   * @param name Property name
-   * @param defaultValue Default value
-   * @param value Current value
-   * @return Property
-   */
-  public VideoProperty createBooleanProperty(String name, boolean defaultValue, boolean value) {
-    return new VideoProperty(
-        CameraServerJNI.createSourceProperty(m_handle,
-            name,
-            VideoProperty.Kind.kBoolean.getValue(),
-            0,
-            1,
-            1,
-            defaultValue ? 1 : 0,
-            value ? 1 : 0));
-  }
-
-  /**
-   * Create a string property.
-   *
-   * @param name Property name
-   * @param value Current value
-   * @return Property
-   */
-  public VideoProperty createStringProperty(String name, String value) {
-    VideoProperty prop = new VideoProperty(
-        CameraServerJNI.createSourceProperty(m_handle,
-            name,
-            VideoProperty.Kind.kString.getValue(),
-            0,
-            0,
-            0,
-            0,
-            0));
-    prop.setString(value);
-    return prop;
-  }
-
-  /**
-   * Configure enum property choices.
-   *
-   * @param property Property
-   * @param choices Choices
-   */
-  public void setEnumPropertyChoices(VideoProperty property, String[] choices) {
-    CameraServerJNI.setSourceEnumPropertyChoices(m_handle, property.m_handle, choices);
-  }
-
-  /**
-   * Configure enum property choices.
-   *
-   * @param property Property
-   * @param choices Choices
-   * @deprecated Use {@code setEnumPropertyChoices} instead.
-   */
-  @Deprecated
-  @SuppressWarnings("MethodName")
-  public void SetEnumPropertyChoices(VideoProperty property, String[] choices) {
-    setEnumPropertyChoices(property, choices);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/MjpegServer.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/MjpegServer.java
deleted file mode 100644
index 4c00aed..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/MjpegServer.java
+++ /dev/null
@@ -1,104 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A sink that acts as a MJPEG-over-HTTP network server.
- */
-public class MjpegServer extends VideoSink {
-  /**
-   * Create a MJPEG-over-HTTP server sink.
-   *
-   * @param name Sink name (arbitrary unique identifier)
-   * @param listenAddress TCP listen address (empty string for all addresses)
-   * @param port TCP port number
-   */
-  public MjpegServer(String name, String listenAddress, int port) {
-    super(CameraServerJNI.createMjpegServer(name, listenAddress, port));
-  }
-
-  /**
-   * Create a MJPEG-over-HTTP server sink.
-   *
-   * @param name Sink name (arbitrary unique identifier)
-   * @param port TCP port number
-   */
-  public MjpegServer(String name, int port) {
-    this(name, "", port);
-  }
-
-  /**
-   * Get the listen address of the server.
-   */
-  public String getListenAddress() {
-    return CameraServerJNI.getMjpegServerListenAddress(m_handle);
-  }
-
-  /**
-   * Get the port number of the server.
-   */
-  public int getPort() {
-    return CameraServerJNI.getMjpegServerPort(m_handle);
-  }
-
-  /**
-   * Set the stream resolution for clients that don't specify it.
-   *
-   * <p>It is not necessary to set this if it is the same as the source
-   * resolution.
-   *
-   * <p>Setting this different than the source resolution will result in
-   * increased CPU usage, particularly for MJPEG source cameras, as it will
-   * decompress, resize, and recompress the image, instead of using the
-   * camera's MJPEG image directly.
-   *
-   * @param width width, 0 for unspecified
-   * @param height height, 0 for unspecified
-   */
-  public void setResolution(int width, int height) {
-    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "width"), width);
-    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "height"), height);
-  }
-
-  /**
-   * Set the stream frames per second (FPS) for clients that don't specify it.
-   *
-   * <p>It is not necessary to set this if it is the same as the source FPS.
-   *
-   * @param fps FPS, 0 for unspecified
-   */
-  public void setFPS(int fps) {
-    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "fps"), fps);
-  }
-
-  /**
-   * Set the compression for clients that don't specify it.
-   *
-   * <p>Setting this will result in increased CPU usage for MJPEG source cameras
-   * as it will decompress and recompress the image instead of using the
-   * camera's MJPEG image directly.
-   *
-   * @param quality JPEG compression quality (0-100), -1 for unspecified
-   */
-  public void setCompression(int quality) {
-    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "compression"),
-                                quality);
-  }
-
-  /**
-   * Set the default compression used for non-MJPEG sources.  If not set,
-   * 80 is used.  This function has no effect on MJPEG source cameras; use
-   * SetCompression() instead to force recompression of MJPEG source images.
-   *
-   * @param quality JPEG compression quality (0-100)
-   */
-  public void setDefaultCompression(int quality) {
-    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "default_compression"),
-                                quality);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java
deleted file mode 100644
index 7526335..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/UsbCamera.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source that represents a USB camera.
- */
-public class UsbCamera extends VideoCamera {
-  /**
-   * Create a source for a USB camera based on device number.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param dev Device number (e.g. 0 for /dev/video0)
-   */
-  public UsbCamera(String name, int dev) {
-    super(CameraServerJNI.createUsbCameraDev(name, dev));
-  }
-
-  /**
-   * Create a source for a USB camera based on device path.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param path Path to device (e.g. "/dev/video0" on Linux)
-   */
-  public UsbCamera(String name, String path) {
-    super(CameraServerJNI.createUsbCameraPath(name, path));
-  }
-
-  /**
-   * Enumerate USB cameras on the local system.
-   *
-   * @return Vector of USB camera information (one for each camera)
-   */
-  public static UsbCameraInfo[] enumerateUsbCameras() {
-    return CameraServerJNI.enumerateUsbCameras();
-  }
-
-  /**
-   * Change the path to the device.
-   */
-  void setPath(String path) {
-    CameraServerJNI.setUsbCameraPath(m_handle, path);
-  }
-
-  /**
-   * Get the path to the device.
-   */
-  public String getPath() {
-    return CameraServerJNI.getUsbCameraPath(m_handle);
-  }
-
-  /**
-   * Get the full camera information for the device.
-   */
-  public UsbCameraInfo getInfo() {
-    return CameraServerJNI.getUsbCameraInfo(m_handle);
-  }
-
-  /**
-   * Set how verbose the camera connection messages are.
-   *
-   * @param level 0=don't display Connecting message, 1=do display message
-   */
-  public void setConnectVerbose(int level) {
-    CameraServerJNI.setProperty(CameraServerJNI.getSourceProperty(m_handle, "connect_verbose"),
-                                level);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/UsbCameraInfo.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/UsbCameraInfo.java
deleted file mode 100644
index c3a8309..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/UsbCameraInfo.java
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * USB camera information.
- */
-public class UsbCameraInfo {
-  /**
-   * Create a new set of UsbCameraInfo.
-   *
-   * @param dev Device number (e.g. N in '/dev/videoN' on Linux)
-   * @param path Path to device if available (e.g. '/dev/video0' on Linux)
-   * @param name Vendor/model name of the camera as provided by the USB driver
-   * @param otherPaths Other path aliases to device
-   * @param vendorId USB vendor id
-   * @param productId USB product id
-   */
-  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
-  public UsbCameraInfo(int dev, String path, String name, String[] otherPaths, int vendorId,
-      int productId) {
-    this.dev = dev;
-    this.path = path;
-    this.name = name;
-    this.otherPaths = otherPaths;
-    this.vendorId = vendorId;
-    this.productId = productId;
-  }
-
-  /**
-   * Device number (e.g. N in '/dev/videoN' on Linux).
-   */
-  @SuppressWarnings("MemberName")
-  public int dev;
-
-  /**
-   * Path to device if available (e.g. '/dev/video0' on Linux).
-   */
-  @SuppressWarnings("MemberName")
-  public String path;
-
-  /**
-   * Vendor/model name of the camera as provided by the USB driver.
-   */
-  @SuppressWarnings("MemberName")
-  public String name;
-
-  /**
-   * Other path aliases to device (e.g. '/dev/v4l/by-id/...' etc on Linux).
-   */
-  @SuppressWarnings("MemberName")
-  public String[] otherPaths;
-
-  /**
-   * USB vendor id.
-   */
-  @SuppressWarnings("MemberName")
-  public int vendorId;
-
-  /**
-   * USB product id.
-   */
-  @SuppressWarnings("MemberName")
-  public int productId;
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoCamera.java
deleted file mode 100644
index 0ab95f1..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoCamera.java
+++ /dev/null
@@ -1,81 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source that represents a video camera.
- */
-public class VideoCamera extends VideoSource {
-  public static class WhiteBalance {
-    public static final int kFixedIndoor = 3000;
-    public static final int kFixedOutdoor1 = 4000;
-    public static final int kFixedOutdoor2 = 5000;
-    public static final int kFixedFluorescent1 = 5100;
-    public static final int kFixedFlourescent2 = 5200;
-  }
-
-  protected VideoCamera(int handle) {
-    super(handle);
-  }
-
-  /**
-   * Set the brightness, as a percentage (0-100).
-   */
-  public synchronized void setBrightness(int brightness) {
-    CameraServerJNI.setCameraBrightness(m_handle, brightness);
-  }
-
-  /**
-   * Get the brightness, as a percentage (0-100).
-   */
-  public synchronized int getBrightness() {
-    return CameraServerJNI.getCameraBrightness(m_handle);
-  }
-
-  /**
-   * Set the white balance to auto.
-   */
-  public synchronized void setWhiteBalanceAuto() {
-    CameraServerJNI.setCameraWhiteBalanceAuto(m_handle);
-  }
-
-  /**
-   * Set the white balance to hold current.
-   */
-  public synchronized void setWhiteBalanceHoldCurrent() {
-    CameraServerJNI.setCameraWhiteBalanceHoldCurrent(m_handle);
-  }
-
-  /**
-   * Set the white balance to manual, with specified color temperature.
-   */
-  public synchronized void setWhiteBalanceManual(int value) {
-    CameraServerJNI.setCameraWhiteBalanceManual(m_handle, value);
-  }
-
-  /**
-   * Set the exposure to auto aperture.
-   */
-  public synchronized void setExposureAuto() {
-    CameraServerJNI.setCameraExposureAuto(m_handle);
-  }
-
-  /**
-   * Set the exposure to hold current.
-   */
-  public synchronized void setExposureHoldCurrent() {
-    CameraServerJNI.setCameraExposureHoldCurrent(m_handle);
-  }
-
-  /**
-   * Set the exposure to manual, as a percentage (0-100).
-   */
-  public synchronized void setExposureManual(int value) {
-    CameraServerJNI.setCameraExposureManual(m_handle, value);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoEvent.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoEvent.java
deleted file mode 100644
index 7f4599d..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoEvent.java
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * Video event.
- */
-public class VideoEvent {
-  public enum Kind {
-    kUnknown(0x0000),
-    kSourceCreated(0x0001),
-    kSourceDestroyed(0x0002),
-    kSourceConnected(0x0004),
-    kSourceDisconnected(0x0008),
-    kSourceVideoModesUpdated(0x0010),
-    kSourceVideoModeChanged(0x0020),
-    kSourcePropertyCreated(0x0040),
-    kSourcePropertyValueUpdated(0x0080),
-    kSourcePropertyChoicesUpdated(0x0100),
-    kSinkSourceChanged(0x0200),
-    kSinkCreated(0x0400),
-    kSinkDestroyed(0x0800),
-    kSinkEnabled(0x1000),
-    kSinkDisabled(0x2000),
-    kNetworkInterfacesChanged(0x4000),
-    kTelemetryUpdated(0x8000),
-    kSinkPropertyCreated(0x10000),
-    kSinkPropertyValueUpdated(0x20000),
-    kSinkPropertyChoicesUpdated(0x40000);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    Kind(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /**
-   * Convert from the numerical representation of kind to an enum type.
-   *
-   * @param kind The numerical representation of kind
-   * @return The kind
-   */
-  @SuppressWarnings("PMD.CyclomaticComplexity")
-  public static Kind getKindFromInt(int kind) {
-    switch (kind) {
-      case 0x0001: return Kind.kSourceCreated;
-      case 0x0002: return Kind.kSourceDestroyed;
-      case 0x0004: return Kind.kSourceConnected;
-      case 0x0008: return Kind.kSourceDisconnected;
-      case 0x0010: return Kind.kSourceVideoModesUpdated;
-      case 0x0020: return Kind.kSourceVideoModeChanged;
-      case 0x0040: return Kind.kSourcePropertyCreated;
-      case 0x0080: return Kind.kSourcePropertyValueUpdated;
-      case 0x0100: return Kind.kSourcePropertyChoicesUpdated;
-      case 0x0200: return Kind.kSinkSourceChanged;
-      case 0x0400: return Kind.kSinkCreated;
-      case 0x0800: return Kind.kSinkDestroyed;
-      case 0x1000: return Kind.kSinkEnabled;
-      case 0x2000: return Kind.kSinkDisabled;
-      case 0x4000: return Kind.kNetworkInterfacesChanged;
-      case 0x10000: return Kind.kSinkPropertyCreated;
-      case 0x20000: return Kind.kSinkPropertyValueUpdated;
-      case 0x40000: return Kind.kSinkPropertyChoicesUpdated;
-      default: return Kind.kUnknown;
-    }
-  }
-
-  @SuppressWarnings("PMD.ExcessiveParameterList")
-  VideoEvent(int kind, int source, int sink, String name, int pixelFormat,
-             int width, int height, int fps, int property, int propertyKind,
-             int value, String valueStr) {
-    this.kind = getKindFromInt(kind);
-    this.sourceHandle = source;
-    this.sinkHandle = sink;
-    this.name = name;
-    this.mode = new VideoMode(pixelFormat, width, height, fps);
-    this.propertyHandle = property;
-    this.propertyKind = VideoProperty.getKindFromInt(propertyKind);
-    this.value = value;
-    this.valueStr = valueStr;
-  }
-
-  @SuppressWarnings("MemberName")
-  public Kind kind;
-
-  // Valid for kSource* and kSink* respectively
-  @SuppressWarnings("MemberName")
-  public int sourceHandle;
-  @SuppressWarnings("MemberName")
-  public int sinkHandle;
-
-  // Source/sink/property name
-  @SuppressWarnings("MemberName")
-  public String name;
-
-  // Fields for kSourceVideoModeChanged event
-  @SuppressWarnings("MemberName")
-  public VideoMode mode;
-
-  // Fields for kSourceProperty* events
-  @SuppressWarnings("MemberName")
-  public int propertyHandle;
-  @SuppressWarnings("MemberName")
-  public VideoProperty.Kind propertyKind;
-  @SuppressWarnings("MemberName")
-  public int value;
-  @SuppressWarnings("MemberName")
-  public String valueStr;
-
-  public VideoSource getSource() {
-    return new VideoSource(CameraServerJNI.copySource(sourceHandle));
-  }
-
-  public VideoSink getSink() {
-    return new VideoSink(CameraServerJNI.copySink(sinkHandle));
-  }
-
-  public VideoProperty getProperty() {
-    return new VideoProperty(propertyHandle, propertyKind);
-  }
-
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoException.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoException.java
deleted file mode 100644
index 8c35517..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoException.java
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * An exception raised by the camera server.
- */
-public class VideoException extends RuntimeException {
-  private static final long serialVersionUID = -9155939328084105145L;
-
-  public VideoException(String msg) {
-    super(msg);
-  }
-
-  @Override
-  public String toString() {
-    return "VideoException [" + super.toString() + "]";
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoListener.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoListener.java
deleted file mode 100644
index 2a35505..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoListener.java
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import java.util.function.Consumer;
-
-/**
- * An event listener.  This calls back to a desigated callback function when
- * an event matching the specified mask is generated by the library.
- */
-public class VideoListener implements AutoCloseable {
-  /**
-   * Create an event listener.
-   *
-   * @param listener Listener function
-   * @param eventMask Bitmask of VideoEvent.Type values
-   * @param immediateNotify Whether callback should be immediately called with
-   *        a representative set of events for the current library state.
-   */
-  public VideoListener(Consumer<VideoEvent> listener, int eventMask,
-                       boolean immediateNotify) {
-    m_handle = CameraServerJNI.addListener(listener, eventMask, immediateNotify);
-  }
-
-  @Override
-  public synchronized void close() {
-    if (m_handle != 0) {
-      CameraServerJNI.removeListener(m_handle);
-    }
-    m_handle = 0;
-  }
-
-  public boolean isValid() {
-    return m_handle != 0;
-  }
-
-  private int m_handle;
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoMode.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoMode.java
deleted file mode 100644
index dec4686..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoMode.java
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * Video mode.
- */
-public class VideoMode {
-  public enum PixelFormat {
-    kUnknown(0), kMJPEG(1), kYUYV(2), kRGB565(3), kBGR(4), kGray(5);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    PixelFormat(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  private static final PixelFormat[] m_pixelFormatValues = PixelFormat.values();
-
-  public static PixelFormat getPixelFormatFromInt(int pixelFormat) {
-    return m_pixelFormatValues[pixelFormat];
-  }
-
-  /**
-   * Create a new video mode.
-   */
-  public VideoMode(int pixelFormat, int width, int height, int fps) {
-    this.pixelFormat = getPixelFormatFromInt(pixelFormat);
-    this.width = width;
-    this.height = height;
-    this.fps = fps;
-  }
-
-  /**
-   * Create a new video mode.
-   */
-  public VideoMode(PixelFormat pixelFormat, int width, int height, int fps) {
-    this.pixelFormat = pixelFormat;
-    this.width = width;
-    this.height = height;
-    this.fps = fps;
-  }
-
-  /**
-   * Pixel format.
-   */
-  @SuppressWarnings("MemberName")
-  public PixelFormat pixelFormat;
-
-  /**
-   * Width in pixels.
-   */
-  @SuppressWarnings("MemberName")
-  public int width;
-
-  /**
-   * Height in pixels.
-   */
-  @SuppressWarnings("MemberName")
-  public int height;
-
-  /**
-   * Frames per second.
-   */
-  @SuppressWarnings("MemberName")
-  public int fps;
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoProperty.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoProperty.java
deleted file mode 100644
index 407e107..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoProperty.java
+++ /dev/null
@@ -1,124 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source or sink property.
- */
-public class VideoProperty {
-  public enum Kind {
-    kNone(0), kBoolean(1), kInteger(2), kString(4), kEnum(8);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    Kind(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /**
-   * Convert from the numerical representation of kind to an enum type.
-   *
-   * @param kind The numerical representation of kind
-   * @return The kind
-   */
-  public static Kind getKindFromInt(int kind) {
-    switch (kind) {
-      case 1: return Kind.kBoolean;
-      case 2: return Kind.kInteger;
-      case 4: return Kind.kString;
-      case 8: return Kind.kEnum;
-      default: return Kind.kNone;
-    }
-  }
-
-  public String getName() {
-    return CameraServerJNI.getPropertyName(m_handle);
-  }
-
-  public Kind getKind() {
-    return m_kind;
-  }
-
-  public boolean isValid() {
-    return m_kind != Kind.kNone;
-  }
-
-  // Kind checkers
-  public boolean isBoolean() {
-    return m_kind == Kind.kBoolean;
-  }
-
-  public boolean isInteger() {
-    return m_kind == Kind.kInteger;
-  }
-
-  public boolean isString() {
-    return m_kind == Kind.kString;
-  }
-
-  public boolean isEnum() {
-    return m_kind == Kind.kEnum;
-  }
-
-  public int get() {
-    return CameraServerJNI.getProperty(m_handle);
-  }
-
-  public void set(int value) {
-    CameraServerJNI.setProperty(m_handle, value);
-  }
-
-  public int getMin() {
-    return CameraServerJNI.getPropertyMin(m_handle);
-  }
-
-  public int getMax() {
-    return CameraServerJNI.getPropertyMax(m_handle);
-  }
-
-  public int getStep() {
-    return CameraServerJNI.getPropertyStep(m_handle);
-  }
-
-  public int getDefault() {
-    return CameraServerJNI.getPropertyDefault(m_handle);
-  }
-
-  // String-specific functions
-  public String getString() {
-    return CameraServerJNI.getStringProperty(m_handle);
-  }
-
-  public void setString(String value) {
-    CameraServerJNI.setStringProperty(m_handle, value);
-  }
-
-  // Enum-specific functions
-  public String[] getChoices() {
-    return CameraServerJNI.getEnumPropertyChoices(m_handle);
-  }
-
-  VideoProperty(int handle) {
-    m_handle = handle;
-    m_kind = getKindFromInt(CameraServerJNI.getPropertyKind(handle));
-  }
-
-  VideoProperty(int handle, Kind kind) {
-    m_handle = handle;
-    m_kind = kind;
-  }
-
-  int m_handle;
-  private Kind m_kind;
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoSink.java
deleted file mode 100644
index 107f6d9..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoSink.java
+++ /dev/null
@@ -1,217 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source for video that provides a sequence of frames.  Each frame may
- * consist of multiple images (e.g. from a stereo or depth camera); these
- * are called channels.
- */
-public class VideoSink implements AutoCloseable {
-  public enum Kind {
-    kUnknown(0), kMjpeg(2), kCv(4), kRaw(8);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    Kind(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /**
-   * Convert from the numerical representation of kind to an enum type.
-   *
-   * @param kind The numerical representation of kind
-   * @return The kind
-   */
-  public static Kind getKindFromInt(int kind) {
-    switch (kind) {
-      case 2: return Kind.kMjpeg;
-      case 4: return Kind.kCv;
-      default: return Kind.kUnknown;
-    }
-  }
-
-  protected VideoSink(int handle) {
-    m_handle = handle;
-  }
-
-  @Override
-  public synchronized void close() {
-    if (m_handle != 0) {
-      CameraServerJNI.releaseSink(m_handle);
-    }
-    m_handle = 0;
-  }
-
-  public boolean isValid() {
-    return m_handle != 0;
-  }
-
-  public int getHandle() {
-    return m_handle;
-  }
-
-  @Override
-  public boolean equals(Object other) {
-    if (this == other) {
-      return true;
-    }
-    if (other == null) {
-      return false;
-    }
-    if (getClass() != other.getClass()) {
-      return false;
-    }
-    VideoSink sink = (VideoSink) other;
-    return m_handle == sink.m_handle;
-  }
-
-  @Override
-  public int hashCode() {
-    return m_handle;
-  }
-
-  /**
-   * Get the kind of the sink.
-   */
-  public Kind getKind() {
-    return getKindFromInt(CameraServerJNI.getSinkKind(m_handle));
-  }
-
-  /**
-   * Get the name of the sink.  The name is an arbitrary identifier
-   * provided when the sink is created, and should be unique.
-   */
-  public String getName() {
-    return CameraServerJNI.getSinkName(m_handle);
-  }
-
-  /**
-   * Get the sink description.  This is sink-kind specific.
-   */
-  public String getDescription() {
-    return CameraServerJNI.getSinkDescription(m_handle);
-  }
-
-  /**
-   * Get a property of the sink.
-   *
-   * @param name Property name
-   * @return Property (kind Property::kNone if no property with
-   *         the given name exists)
-   */
-  public VideoProperty getProperty(String name) {
-    return new VideoProperty(CameraServerJNI.getSinkProperty(m_handle, name));
-  }
-
-  /**
-   * Enumerate all properties of this sink.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public VideoProperty[] enumerateProperties() {
-    int[] handles = CameraServerJNI.enumerateSinkProperties(m_handle);
-    VideoProperty[] rv = new VideoProperty[handles.length];
-    for (int i = 0; i < handles.length; i++) {
-      rv[i] = new VideoProperty(handles[i]);
-    }
-    return rv;
-  }
-
-  /**
-   * Set properties from a JSON configuration string.
-   *
-   * <p>The format of the JSON input is:
-   *
-   * <pre>
-   * {
-   *     "properties": [
-   *         {
-   *             "name": property name
-   *             "value": property value
-   *         }
-   *     ]
-   * }
-   * </pre>
-   *
-   * @param config configuration
-   * @return True if set successfully
-   */
-  public boolean setConfigJson(String config) {
-    return CameraServerJNI.setSinkConfigJson(m_handle, config);
-  }
-
-  /**
-   * Get a JSON configuration string.
-   *
-   * @return JSON configuration string
-   */
-  public String getConfigJson() {
-    return CameraServerJNI.getSinkConfigJson(m_handle);
-  }
-
-  /**
-   * Configure which source should provide frames to this sink.  Each sink
-   * can accept frames from only a single source, but a single source can
-   * provide frames to multiple clients.
-   *
-   * @param source Source
-   */
-  public void setSource(VideoSource source) {
-    if (source == null) {
-      CameraServerJNI.setSinkSource(m_handle, 0);
-    } else {
-      CameraServerJNI.setSinkSource(m_handle, source.m_handle);
-    }
-  }
-
-  /**
-   * Get the connected source.
-   *
-   * @return Connected source; nullptr if no source connected.
-   */
-  public VideoSource getSource() {
-    // While VideoSource.close() will call releaseSource(), getSinkSource()
-    // increments the internal reference count so this is okay to do.
-    return new VideoSource(CameraServerJNI.getSinkSource(m_handle));
-  }
-
-  /**
-   * Get a property of the associated source.
-   *
-   * @param name Property name
-   * @return Property (kind Property::kNone if no property with
-   *         the given name exists or no source connected)
-   */
-  public VideoProperty getSourceProperty(String name) {
-    return new VideoProperty(
-        CameraServerJNI.getSinkSourceProperty(m_handle, name));
-  }
-
-  /**
-   * Enumerate all existing sinks.
-   *
-   * @return Vector of sinks.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static VideoSink[] enumerateSinks() {
-    int[] handles = CameraServerJNI.enumerateSinks();
-    VideoSink[] rv = new VideoSink[handles.length];
-    for (int i = 0; i < handles.length; i++) {
-      rv[i] = new VideoSink(handles[i]);
-    }
-    return rv;
-  }
-
-  protected int m_handle;
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoSource.java
deleted file mode 100644
index 51d3821..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/VideoSource.java
+++ /dev/null
@@ -1,371 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-/**
- * A source for video that provides a sequence of frames.  Each frame may
- * consist of multiple images (e.g. from a stereo or depth camera); these
- * are called channels.
- */
-public class VideoSource implements AutoCloseable {
-  public enum Kind {
-    kUnknown(0), kUsb(1), kHttp(2), kCv(4), kRaw(8);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    Kind(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /**
-   * Connection strategy.
-   */
-  public enum ConnectionStrategy {
-    /**
-     * Automatically connect or disconnect based on whether any sinks are
-     * connected to this source.  This is the default behavior.
-     */
-    kAutoManage(0),
-
-    /**
-     * Try to keep the connection open regardless of whether any sinks are
-     * connected.
-     */
-    kKeepOpen(1),
-
-    /**
-     * Never open the connection.  If this is set when the connection is open,
-     * close the connection.
-     */
-    kForceClose(2);
-
-    @SuppressWarnings("MemberName")
-    private final int value;
-
-    ConnectionStrategy(int value) {
-      this.value = value;
-    }
-
-    public int getValue() {
-      return value;
-    }
-  }
-
-  /**
-   * Convert from the numerical representation of kind to an enum type.
-   *
-   * @param kind The numerical representation of kind
-   * @return The kind
-   */
-  public static Kind getKindFromInt(int kind) {
-    switch (kind) {
-      case 1: return Kind.kUsb;
-      case 2: return Kind.kHttp;
-      case 4: return Kind.kCv;
-      default: return Kind.kUnknown;
-    }
-  }
-
-  protected VideoSource(int handle) {
-    m_handle = handle;
-  }
-
-  @Override
-  public synchronized void close() {
-    if (m_handle != 0) {
-      CameraServerJNI.releaseSource(m_handle);
-    }
-    m_handle = 0;
-  }
-
-  public boolean isValid() {
-    return m_handle != 0;
-  }
-
-  public int getHandle() {
-    return m_handle;
-  }
-
-  @Override
-  public boolean equals(Object other) {
-    if (this == other) {
-      return true;
-    }
-    if (other == null) {
-      return false;
-    }
-    if (getClass() != other.getClass()) {
-      return false;
-    }
-    VideoSource source = (VideoSource) other;
-    return m_handle == source.m_handle;
-  }
-
-  @Override
-  public int hashCode() {
-    return m_handle;
-  }
-
-  /**
-   * Get the kind of the source.
-   */
-  public Kind getKind() {
-    return getKindFromInt(CameraServerJNI.getSourceKind(m_handle));
-  }
-
-  /**
-   * Get the name of the source.  The name is an arbitrary identifier
-   * provided when the source is created, and should be unique.
-   */
-  public String getName() {
-    return CameraServerJNI.getSourceName(m_handle);
-  }
-
-  /**
-   * Get the source description.  This is source-kind specific.
-   */
-  public String getDescription() {
-    return CameraServerJNI.getSourceDescription(m_handle);
-  }
-
-  /**
-   * Get the last time a frame was captured.
-   * @return Time in 1 us increments.
-   */
-  public long getLastFrameTime() {
-    return CameraServerJNI.getSourceLastFrameTime(m_handle);
-  }
-
-  /**
-   * Sets the connection strategy.  By default, the source will automatically
-   * connect or disconnect based on whether any sinks are connected.
-   *
-   * <p>This function is non-blocking; look for either a connection open or
-   * close event or call {@link #isConnected()} to determine the connection
-   * state.
-   *
-   * @param strategy connection strategy (auto, keep open, or force close)
-   */
-  public void setConnectionStrategy(ConnectionStrategy strategy) {
-    CameraServerJNI.setSourceConnectionStrategy(m_handle, strategy.getValue());
-  }
-
-  /**
-   * Returns if the source currently connected to whatever is providing the images.
-   */
-  public boolean isConnected() {
-    return CameraServerJNI.isSourceConnected(m_handle);
-  }
-
-  /**
-   * Gets source enable status.  This is determined with a combination of
-   * connection strategy and the number of sinks connected.
-   *
-   * @return True if enabled, false otherwise.
-   */
-  public boolean isEnabled() {
-    return CameraServerJNI.isSourceEnabled(m_handle);
-  }
-
-  /**
-   * Get a property.
-   *
-   * @param name Property name
-   * @return Property contents (of kind Property::kNone if no property with
-   *         the given name exists)
-   */
-  public VideoProperty getProperty(String name) {
-    return new VideoProperty(CameraServerJNI.getSourceProperty(m_handle, name));
-  }
-
-  /**
-   * Enumerate all properties of this source.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public VideoProperty[] enumerateProperties() {
-    int[] handles = CameraServerJNI.enumerateSourceProperties(m_handle);
-    VideoProperty[] rv = new VideoProperty[handles.length];
-    for (int i = 0; i < handles.length; i++) {
-      rv[i] = new VideoProperty(handles[i]);
-    }
-    return rv;
-  }
-
-  /**
-   * Get the current video mode.
-   */
-  public VideoMode getVideoMode() {
-    return CameraServerJNI.getSourceVideoMode(m_handle);
-  }
-
-  /**
-   * Set the video mode.
-   * @param mode Video mode
-   */
-  public boolean setVideoMode(VideoMode mode) {
-    return CameraServerJNI.setSourceVideoMode(m_handle,
-        mode.pixelFormat.getValue(),
-        mode.width,
-        mode.height,
-        mode.fps);
-  }
-
-  /**
-   * Set the video mode.
-   *
-   * @param pixelFormat desired pixel format
-   * @param width desired width
-   * @param height desired height
-   * @param fps desired FPS
-   * @return True if set successfully
-   */
-  public boolean setVideoMode(VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
-    return CameraServerJNI.setSourceVideoMode(m_handle, pixelFormat.getValue(), width, height, fps);
-  }
-
-  /**
-   * Set the pixel format.
-   *
-   * @param pixelFormat desired pixel format
-   * @return True if set successfully
-   */
-  public boolean setPixelFormat(VideoMode.PixelFormat pixelFormat) {
-    return CameraServerJNI.setSourcePixelFormat(m_handle, pixelFormat.getValue());
-  }
-
-  /**
-   * Set the resolution.
-   *
-   * @param width desired width
-   * @param height desired height
-   * @return True if set successfully
-   */
-  public boolean setResolution(int width, int height) {
-    return CameraServerJNI.setSourceResolution(m_handle, width, height);
-  }
-
-  /**
-   * Set the frames per second (FPS).
-   *
-   * @param fps desired FPS
-   * @return True if set successfully
-   */
-  public boolean setFPS(int fps) {
-    return CameraServerJNI.setSourceFPS(m_handle, fps);
-  }
-
-  /**
-   * Set video mode and properties from a JSON configuration string.
-   *
-   * <p>The format of the JSON input is:
-   *
-   * <pre>
-   * {
-   *     "pixel format": "MJPEG", "YUYV", etc
-   *     "width": video mode width
-   *     "height": video mode height
-   *     "fps": video mode fps
-   *     "brightness": percentage brightness
-   *     "white balance": "auto", "hold", or value
-   *     "exposure": "auto", "hold", or value
-   *     "properties": [
-   *         {
-   *             "name": property name
-   *             "value": property value
-   *         }
-   *     ]
-   * }
-   * </pre>
-   *
-   * @param config configuration
-   * @return True if set successfully
-   */
-  public boolean setConfigJson(String config) {
-    return CameraServerJNI.setSourceConfigJson(m_handle, config);
-  }
-
-  /**
-   * Get a JSON configuration string.
-   *
-   * @return JSON configuration string
-   */
-  public String getConfigJson() {
-    return CameraServerJNI.getSourceConfigJson(m_handle);
-  }
-
-  /**
-   * Get the actual FPS.
-   *
-   * <p>CameraServerJNI#setTelemetryPeriod() must be called for this to be valid
-   * (throws VisionException if telemetry is not enabled).
-   *
-   * @return Actual FPS averaged over the telemetry period.
-   */
-  public double getActualFPS() {
-    return CameraServerJNI.getTelemetryAverageValue(m_handle,
-        CameraServerJNI.TelemetryKind.kSourceFramesReceived);
-  }
-
-  /**
-   * Get the data rate (in bytes per second).
-   *
-   * <p>CameraServerJNI#setTelemetryPeriod() must be called for this to be valid
-   * (throws VisionException if telemetry is not enabled).
-   *
-   * @return Data rate averaged over the telemetry period.
-   */
-  public double getActualDataRate() {
-    return CameraServerJNI.getTelemetryAverageValue(m_handle,
-        CameraServerJNI.TelemetryKind.kSourceBytesReceived);
-  }
-
-  /**
-   * Enumerate all known video modes for this source.
-   */
-  public VideoMode[] enumerateVideoModes() {
-    return CameraServerJNI.enumerateSourceVideoModes(m_handle);
-  }
-
-  /**
-   * Enumerate all sinks connected to this source.
-   *
-   * @return Vector of sinks.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public VideoSink[] enumerateSinks() {
-    int[] handles = CameraServerJNI.enumerateSourceSinks(m_handle);
-    VideoSink[] rv = new VideoSink[handles.length];
-    for (int i = 0; i < handles.length; i++) {
-      rv[i] = new VideoSink(handles[i]);
-    }
-    return rv;
-  }
-
-  /**
-   * Enumerate all existing sources.
-   *
-   * @return Vector of sources.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static VideoSource[] enumerateSources() {
-    int[] handles = CameraServerJNI.enumerateSources();
-    VideoSource[] rv = new VideoSource[handles.length];
-    for (int i = 0; i < handles.length; i++) {
-      rv[i] = new VideoSource(handles[i]);
-    }
-    return rv;
-  }
-
-  protected int m_handle;
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawFrame.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawFrame.java
deleted file mode 100644
index 0e7a9ce..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawFrame.java
+++ /dev/null
@@ -1,130 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore.raw;
-
-import java.nio.ByteBuffer;
-
-import edu.wpi.cscore.CameraServerJNI;
-
-/**
- * Class for storing raw frame data between image read call.
- *
- * <p>Data is reused for each frame read, rather then reallocating every frame.
- */
-public class RawFrame implements AutoCloseable {
-  private final long m_framePtr;
-  private ByteBuffer m_dataByteBuffer;
-  private long m_dataPtr;
-  private int m_totalData;
-  private int m_width;
-  private int m_height;
-  private int m_pixelFormat;
-
-  /**
-   * Construct a new RawFrame.
-   */
-  public RawFrame() {
-    m_framePtr = CameraServerJNI.allocateRawFrame();
-  }
-
-  /**
-   * Close the RawFrame, releasing native resources.
-   * Any images currently using the data will be invalidated.
-   */
-  @Override
-  public void close() {
-    CameraServerJNI.freeRawFrame(m_framePtr);
-  }
-
-  /**
-   * Called from JNI to set data in class.
-   */
-  public void setData(ByteBuffer dataByteBuffer, long dataPtr, int totalData,
-                      int width, int height, int pixelFormat) {
-    m_dataByteBuffer = dataByteBuffer;
-    m_dataPtr = dataPtr;
-    m_totalData = totalData;
-    m_width = width;
-    m_height = height;
-    m_pixelFormat = pixelFormat;
-  }
-
-  /**
-   * Get the pointer to native representation of this frame.
-   */
-  public long getFramePtr() {
-    return m_framePtr;
-  }
-
-  /**
-   * Get a ByteBuffer pointing to the frame data.
-   * This ByteBuffer is backed by the frame directly. Its lifetime is controlled by
-   * the frame. If a new frame gets read, it will overwrite the current one.
-   */
-  public ByteBuffer getDataByteBuffer() {
-    return m_dataByteBuffer;
-  }
-
-  /**
-   * Get a long (is a char* in native code) pointing to the frame data.
-   * This pointer is backed by the frame directly. Its lifetime is controlled by
-   * the frame. If a new frame gets read, it will overwrite the current one.
-   */
-  public long getDataPtr() {
-    return m_dataPtr;
-  }
-
-  /**
-   * Get the total length of the data stored in the frame.
-   */
-  public int getTotalData() {
-    return m_totalData;
-  }
-
-  /**
-   * Get the width of the frame.
-   */
-  public int getWidth() {
-    return m_width;
-  }
-
-  /**
-   * Set the width of the frame.
-   */
-  public void setWidth(int width) {
-    this.m_width = width;
-  }
-
-  /**
-   * Get the height of the frame.
-   */
-  public int getHeight() {
-    return m_height;
-  }
-
-  /**
-   * Set the height of the frame.
-   */
-  public void setHeight(int height) {
-    this.m_height = height;
-  }
-
-  /**
-   * Get the PixelFormat of the frame.
-   */
-  public int getPixelFormat() {
-    return m_pixelFormat;
-  }
-
-  /**
-   * Set the PixelFormat of the frame.
-   */
-  public void setPixelFormat(int pixelFormat) {
-    this.m_pixelFormat = pixelFormat;
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawSink.java
deleted file mode 100644
index 535f356..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawSink.java
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore.raw;
-
-import edu.wpi.cscore.CameraServerJNI;
-import edu.wpi.cscore.ImageSink;
-
-/**
- * A sink for user code to accept video frames as raw bytes.
- *
- * <p>This is a complex API, most cases should use CvSink.
- */
-public class RawSink extends ImageSink {
-  /**
-   * Create a sink for accepting raw images.
-   *
-   * <p>grabFrame() must be called on the created sink to get each new
-   * image.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   */
-  public RawSink(String name) {
-    super(CameraServerJNI.createRawSink(name));
-  }
-
-  /**
-   * Wait for the next frame and get the image.
-   * Times out (returning 0) after 0.225 seconds.
-   * The provided image will have three 8-bit channels stored in BGR order.
-   *
-   * @return Frame time, or 0 on error (call getError() to obtain the error
-   *         message); the frame time is in the same time base as wpi::Now(),
-   *         and is in 1 us increments.
-   */
-  protected long grabFrame(RawFrame frame) {
-    return grabFrame(frame, 0.225);
-  }
-
-  /**
-   * Wait for the next frame and get the image.
-   * Times out (returning 0) after timeout seconds.
-   * The provided image will have three 8-bit channels stored in BGR order.
-   *
-   * @return Frame time, or 0 on error (call getError() to obtain the error
-   *         message); the frame time is in the same time base as wpi::Now(),
-   *         and is in 1 us increments.
-   */
-  protected long grabFrame(RawFrame frame, double timeout) {
-    return CameraServerJNI.grabSinkFrameTimeout(m_handle, frame, timeout);
-  }
-
-  /**
-   * Wait for the next frame and get the image.  May block forever.
-   * The provided image will have three 8-bit channels stored in BGR order.
-   *
-   * @return Frame time, or 0 on error (call getError() to obtain the error
-   *         message); the frame time is in the same time base as wpi::Now(),
-   *         and is in 1 us increments.
-   */
-  protected long grabFrameNoTimeout(RawFrame frame) {
-    return CameraServerJNI.grabSinkFrame(m_handle, frame);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawSource.java
deleted file mode 100644
index 9dfb3f3..0000000
--- a/third_party/allwpilib/cscore/src/main/java/edu/wpi/cscore/raw/RawSource.java
+++ /dev/null
@@ -1,85 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore.raw;
-
-import edu.wpi.cscore.CameraServerJNI;
-import edu.wpi.cscore.ImageSource;
-import edu.wpi.cscore.VideoMode;
-
-/**
- * A source for user code to provide video frames as raw bytes.
- *
- * <p>This is a complex API, most cases should use CvSource.
- */
-public class RawSource extends ImageSource {
-  /**
-   * Create a raw frame source.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param mode Video mode being generated
-   */
-  public RawSource(String name, VideoMode mode) {
-    super(CameraServerJNI.createRawSource(name,
-        mode.pixelFormat.getValue(),
-        mode.width, mode.height,
-        mode.fps));
-  }
-
-  /**
-   * Create a raw frame source.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param pixelFormat Pixel format
-   * @param width width
-   * @param height height
-   * @param fps fps
-   */
-  public RawSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
-    super(CameraServerJNI.createRawSource(name,
-        pixelFormat.getValue(),
-        width, height,
-        fps));
-  }
-
-  /**
-   * Put a raw image and notify sinks.
-   *
-   * @param image raw frame image
-   */
-  protected void putFrame(RawFrame image) {
-    CameraServerJNI.putRawSourceFrame(m_handle, image);
-  }
-
-  /**
-   * Put a raw image and notify sinks.
-   *
-   * @param data raw frame data pointer
-   * @param width frame width
-   * @param height frame height
-   * @param pixelFormat pixel format
-   * @param totalData length of data in total
-   */
-  protected void putFrame(long data, int width, int height, int pixelFormat, int totalData) {
-    CameraServerJNI.putRawSourceFrame(m_handle, data, width, height, pixelFormat, totalData);
-  }
-
-  /**
-   * Put a raw image and notify sinks.
-   *
-   * @param data raw frame data pointer
-   * @param width frame width
-   * @param height frame height
-   * @param pixelFormat pixel format
-   * @param totalData length of data in total
-   */
-  protected void putFrame(long data, int width, int height, VideoMode.PixelFormat pixelFormat,
-                          int totalData) {
-    CameraServerJNI.putRawSourceFrame(m_handle, data, width, height, pixelFormat.getValue(),
-                                      totalData);
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/AxisCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/AxisCamera.java
new file mode 100644
index 0000000..296bd6b
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/AxisCamera.java
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** A source that represents an Axis IP camera. */
+public class AxisCamera extends HttpCamera {
+  private static String hostToUrl(String host) {
+    return "http://" + host + "/mjpg/video.mjpg";
+  }
+
+  private static String[] hostToUrl(String[] hosts) {
+    String[] urls = new String[hosts.length];
+    for (int i = 0; i < hosts.length; i++) {
+      urls[i] = hostToUrl(hosts[i]);
+    }
+    return urls;
+  }
+
+  /**
+   * Create a source for an Axis IP camera.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
+   */
+  public AxisCamera(String name, String host) {
+    super(name, hostToUrl(host), HttpCameraKind.kAxis);
+  }
+
+  /**
+   * Create a source for an Axis IP camera.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param hosts Array of Camera host IPs/DNS names
+   */
+  public AxisCamera(String name, String[] hosts) {
+    super(name, hostToUrl(hosts), HttpCameraKind.kAxis);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java
new file mode 100644
index 0000000..3527dae
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerCvJNI.java
@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+import org.opencv.core.Core;
+
+public class CameraServerCvJNI {
+  static boolean libraryLoaded = false;
+
+  static RuntimeLoader<Core> loader = null;
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+
+  static {
+    String opencvName = Core.NATIVE_LIBRARY_NAME;
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        CameraServerJNI.forceLoad();
+        loader =
+            new RuntimeLoader<>(opencvName, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
+        loader.loadLibraryHashed();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException if library load failed
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    CameraServerJNI.forceLoad();
+    loader =
+        new RuntimeLoader<>(
+            Core.NATIVE_LIBRARY_NAME, RuntimeLoader.getDefaultExtractionRoot(), Core.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  public static native int createCvSource(
+      String name, int pixelFormat, int width, int height, int fps);
+
+  public static native void putSourceFrame(int source, long imageNativeObj);
+
+  public static native int createCvSink(String name);
+  // public static native int createCvSinkCallback(String name,
+  //                            void (*processFrame)(long time));
+
+  public static native long grabSinkFrame(int sink, long imageNativeObj);
+
+  public static native long grabSinkFrameTimeout(int sink, long imageNativeObj, double timeout);
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
new file mode 100644
index 0000000..fe46baf
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CameraServerJNI.java
@@ -0,0 +1,393 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import edu.wpi.first.cscore.raw.RawFrame;
+import edu.wpi.first.util.RuntimeLoader;
+import java.io.IOException;
+import java.nio.ByteBuffer;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.function.Consumer;
+
+public class CameraServerJNI {
+  static boolean libraryLoaded = false;
+
+  static RuntimeLoader<CameraServerJNI> loader = null;
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader =
+            new RuntimeLoader<>(
+                "cscorejni", RuntimeLoader.getDefaultExtractionRoot(), CameraServerJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException if library load failed
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    loader =
+        new RuntimeLoader<>(
+            "cscorejni", RuntimeLoader.getDefaultExtractionRoot(), CameraServerJNI.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  //
+  // Property Functions
+  //
+  public static native int getPropertyKind(int property);
+
+  public static native String getPropertyName(int property);
+
+  public static native int getProperty(int property);
+
+  public static native void setProperty(int property, int value);
+
+  public static native int getPropertyMin(int property);
+
+  public static native int getPropertyMax(int property);
+
+  public static native int getPropertyStep(int property);
+
+  public static native int getPropertyDefault(int property);
+
+  public static native String getStringProperty(int property);
+
+  public static native void setStringProperty(int property, String value);
+
+  public static native String[] getEnumPropertyChoices(int property);
+
+  //
+  // Source Creation Functions
+  //
+  public static native int createUsbCameraDev(String name, int dev);
+
+  public static native int createUsbCameraPath(String name, String path);
+
+  public static native int createHttpCamera(String name, String url, int kind);
+
+  public static native int createHttpCameraMulti(String name, String[] urls, int kind);
+
+  public static native int createRawSource(
+      String name, int pixelFormat, int width, int height, int fps);
+
+  //
+  // Source Functions
+  //
+  public static native int getSourceKind(int source);
+
+  public static native String getSourceName(int source);
+
+  public static native String getSourceDescription(int source);
+
+  public static native long getSourceLastFrameTime(int source);
+
+  public static native void setSourceConnectionStrategy(int source, int strategy);
+
+  public static native boolean isSourceConnected(int source);
+
+  public static native boolean isSourceEnabled(int source);
+
+  public static native int getSourceProperty(int source, String name);
+
+  public static native int[] enumerateSourceProperties(int source);
+
+  public static native VideoMode getSourceVideoMode(int source);
+
+  public static native boolean setSourceVideoMode(
+      int source, int pixelFormat, int width, int height, int fps);
+
+  public static native boolean setSourcePixelFormat(int source, int pixelFormat);
+
+  public static native boolean setSourceResolution(int source, int width, int height);
+
+  public static native boolean setSourceFPS(int source, int fps);
+
+  public static native boolean setSourceConfigJson(int source, String config);
+
+  public static native String getSourceConfigJson(int source);
+
+  public static native VideoMode[] enumerateSourceVideoModes(int source);
+
+  public static native int[] enumerateSourceSinks(int source);
+
+  public static native int copySource(int source);
+
+  public static native void releaseSource(int source);
+
+  //
+  // Camera Source Common Property Fuctions
+  //
+  public static native void setCameraBrightness(int source, int brightness);
+
+  public static native int getCameraBrightness(int source);
+
+  public static native void setCameraWhiteBalanceAuto(int source);
+
+  public static native void setCameraWhiteBalanceHoldCurrent(int source);
+
+  public static native void setCameraWhiteBalanceManual(int source, int value);
+
+  public static native void setCameraExposureAuto(int source);
+
+  public static native void setCameraExposureHoldCurrent(int source);
+
+  public static native void setCameraExposureManual(int source, int value);
+
+  //
+  // UsbCamera Source Functions
+  //
+  public static native void setUsbCameraPath(int source, String path);
+
+  public static native String getUsbCameraPath(int source);
+
+  public static native UsbCameraInfo getUsbCameraInfo(int source);
+
+  //
+  // HttpCamera Source Functions
+  //
+  public static native int getHttpCameraKind(int source);
+
+  public static native void setHttpCameraUrls(int source, String[] urls);
+
+  public static native String[] getHttpCameraUrls(int source);
+
+  //
+  // Image Source Functions
+  //
+  public static native void putRawSourceFrameBB(
+      int source, ByteBuffer data, int width, int height, int pixelFormat, int totalData);
+
+  public static native void putRawSourceFrame(
+      int source, long data, int width, int height, int pixelFormat, int totalData);
+
+  public static void putRawSourceFrame(int source, RawFrame raw) {
+    putRawSourceFrame(
+        source,
+        raw.getDataPtr(),
+        raw.getWidth(),
+        raw.getHeight(),
+        raw.getPixelFormat(),
+        raw.getTotalData());
+  }
+
+  public static native void notifySourceError(int source, String msg);
+
+  public static native void setSourceConnected(int source, boolean connected);
+
+  public static native void setSourceDescription(int source, String description);
+
+  public static native int createSourceProperty(
+      int source,
+      String name,
+      int kind,
+      int minimum,
+      int maximum,
+      int step,
+      int defaultValue,
+      int value);
+
+  public static native void setSourceEnumPropertyChoices(
+      int source, int property, String[] choices);
+
+  //
+  // Sink Creation Functions
+  //
+  public static native int createMjpegServer(String name, String listenAddress, int port);
+
+  public static native int createRawSink(String name);
+
+  //
+  // Sink Functions
+  //
+  public static native int getSinkKind(int sink);
+
+  public static native String getSinkName(int sink);
+
+  public static native String getSinkDescription(int sink);
+
+  public static native int getSinkProperty(int sink, String name);
+
+  public static native int[] enumerateSinkProperties(int sink);
+
+  public static native boolean setSinkConfigJson(int sink, String config);
+
+  public static native String getSinkConfigJson(int sink);
+
+  public static native void setSinkSource(int sink, int source);
+
+  public static native int getSinkSourceProperty(int sink, String name);
+
+  public static native int getSinkSource(int sink);
+
+  public static native int copySink(int sink);
+
+  public static native void releaseSink(int sink);
+
+  //
+  // MjpegServer Sink Functions
+  //
+  public static native String getMjpegServerListenAddress(int sink);
+
+  public static native int getMjpegServerPort(int sink);
+
+  //
+  // Image Sink Functions
+  //
+  public static native void setSinkDescription(int sink, String description);
+
+  private static native long grabRawSinkFrameImpl(
+      int sink,
+      RawFrame rawFrame,
+      long rawFramePtr,
+      ByteBuffer byteBuffer,
+      int width,
+      int height,
+      int pixelFormat);
+
+  private static native long grabRawSinkFrameTimeoutImpl(
+      int sink,
+      RawFrame rawFrame,
+      long rawFramePtr,
+      ByteBuffer byteBuffer,
+      int width,
+      int height,
+      int pixelFormat,
+      double timeout);
+
+  public static long grabSinkFrame(int sink, RawFrame rawFrame) {
+    return grabRawSinkFrameImpl(
+        sink,
+        rawFrame,
+        rawFrame.getFramePtr(),
+        rawFrame.getDataByteBuffer(),
+        rawFrame.getWidth(),
+        rawFrame.getHeight(),
+        rawFrame.getPixelFormat());
+  }
+
+  public static long grabSinkFrameTimeout(int sink, RawFrame rawFrame, double timeout) {
+    return grabRawSinkFrameTimeoutImpl(
+        sink,
+        rawFrame,
+        rawFrame.getFramePtr(),
+        rawFrame.getDataByteBuffer(),
+        rawFrame.getWidth(),
+        rawFrame.getHeight(),
+        rawFrame.getPixelFormat(),
+        timeout);
+  }
+
+  public static native String getSinkError(int sink);
+
+  public static native void setSinkEnabled(int sink, boolean enabled);
+
+  //
+  // Listener Functions
+  //
+  public static native int addListener(
+      Consumer<VideoEvent> listener, int eventMask, boolean immediateNotify);
+
+  public static native void removeListener(int handle);
+
+  public static native int createListenerPoller();
+
+  public static native void destroyListenerPoller(int poller);
+
+  public static native int addPolledListener(int poller, int eventMask, boolean immediateNotify);
+
+  public static native VideoEvent[] pollListener(int poller) throws InterruptedException;
+
+  public static native VideoEvent[] pollListenerTimeout(int poller, double timeout)
+      throws InterruptedException;
+
+  public static native void cancelPollListener(int poller);
+
+  //
+  // Telemetry Functions
+  //
+  public enum TelemetryKind {
+    kSourceBytesReceived(1),
+    kSourceFramesReceived(2);
+
+    private final int value;
+
+    TelemetryKind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  public static native void setTelemetryPeriod(double seconds);
+
+  public static native double getTelemetryElapsedTime();
+
+  public static native long getTelemetryValue(int handle, int kind);
+
+  public static long getTelemetryValue(int handle, TelemetryKind kind) {
+    return getTelemetryValue(handle, kind.getValue());
+  }
+
+  public static native double getTelemetryAverageValue(int handle, int kind);
+
+  public static double getTelemetryAverageValue(int handle, TelemetryKind kind) {
+    return getTelemetryAverageValue(handle, kind.getValue());
+  }
+
+  //
+  // Logging Functions
+  //
+  @FunctionalInterface
+  public interface LoggerFunction {
+    void apply(int level, String file, int line, String msg);
+  }
+
+  public static native void setLogger(LoggerFunction func, int minLevel);
+
+  //
+  // Utility Functions
+  //
+  public static native UsbCameraInfo[] enumerateUsbCameras();
+
+  public static native int[] enumerateSources();
+
+  public static native int[] enumerateSinks();
+
+  public static native String getHostname();
+
+  public static native String[] getNetworkInterfaces();
+
+  public static native long allocateRawFrame();
+
+  public static native void freeRawFrame(long frame);
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
new file mode 100644
index 0000000..88ca8b1
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSink.java
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import org.opencv.core.Mat;
+
+/**
+ * A sink for user code to accept video frames as OpenCV images. These sinks require the WPILib
+ * OpenCV builds. For an alternate OpenCV, see the documentation how to build your own with RawSink.
+ */
+public class CvSink extends ImageSink {
+  /**
+   * Create a sink for accepting OpenCV images. WaitForFrame() must be called on the created sink to
+   * get each new image.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   */
+  public CvSink(String name) {
+    super(CameraServerCvJNI.createCvSink(name));
+  }
+
+  /// Create a sink for accepting OpenCV images in a separate thread.
+  /// A thread will be created that calls WaitForFrame() and calls the
+  /// processFrame() callback each time a new frame arrives.
+  /// @param name Source name (arbitrary unique identifier)
+  /// @param processFrame Frame processing function; will be called with a
+  ///        time=0 if an error occurred.  processFrame should call GetImage()
+  ///        or GetError() as needed, but should not call (except in very
+  ///        unusual circumstances) WaitForImage().
+  // public CvSink(wpi::StringRef name,
+  //       std::function<void(uint64_t time)> processFrame) {
+  //  super(CameraServerJNI.createCvSinkCallback(name, processFrame));
+  // }
+
+  /**
+   * Wait for the next frame and get the image. Times out (returning 0) after 0.225 seconds. The
+   * provided image will have three 3-bit channels stored in BGR order.
+   *
+   * @param image Where to store the image.
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message)
+   */
+  public long grabFrame(Mat image) {
+    return grabFrame(image, 0.225);
+  }
+
+  /**
+   * Wait for the next frame and get the image. Times out (returning 0) after timeout seconds. The
+   * provided image will have three 3-bit channels stored in BGR order.
+   *
+   * @param image Where to store the image.
+   * @param timeout Retrieval timeout in seconds.
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
+   *     is in 1 us increments.
+   */
+  public long grabFrame(Mat image, double timeout) {
+    return CameraServerCvJNI.grabSinkFrameTimeout(m_handle, image.nativeObj, timeout);
+  }
+
+  /**
+   * Wait for the next frame and get the image. May block forever. The provided image will have
+   * three 3-bit channels stored in BGR order.
+   *
+   * @param image Where to store the image.
+   * @return Frame time, or 0 on error (call GetError() to obtain the error message); the frame time
+   *     is in 1 us increments.
+   */
+  public long grabFrameNoTimeout(Mat image) {
+    return CameraServerCvJNI.grabSinkFrame(m_handle, image.nativeObj);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java
new file mode 100644
index 0000000..3934a09
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/CvSource.java
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import org.opencv.core.Mat;
+
+/**
+ * A source that represents a video camera. These sources require the WPILib OpenCV builds. For an
+ * alternate OpenCV, see the documentation how to build your own with RawSource.
+ */
+public class CvSource extends ImageSource {
+  /**
+   * Create an OpenCV source.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param mode Video mode being generated
+   */
+  public CvSource(String name, VideoMode mode) {
+    super(
+        CameraServerCvJNI.createCvSource(
+            name, mode.pixelFormat.getValue(), mode.width, mode.height, mode.fps));
+  }
+
+  /**
+   * Create an OpenCV source.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param pixelFormat Pixel format
+   * @param width width
+   * @param height height
+   * @param fps fps
+   */
+  public CvSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+    super(CameraServerCvJNI.createCvSource(name, pixelFormat.getValue(), width, height, fps));
+  }
+
+  /**
+   * Put an OpenCV image and notify sinks.
+   *
+   * <p>Only 8-bit single-channel or 3-channel (with BGR channel order) images are supported. If the
+   * format, depth or channel order is different, use Mat.convertTo() and/or cvtColor() to convert
+   * it first.
+   *
+   * @param image OpenCV image
+   */
+  public void putFrame(Mat image) {
+    CameraServerCvJNI.putSourceFrame(m_handle, image.nativeObj);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java
new file mode 100644
index 0000000..8c72350
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/HttpCamera.java
@@ -0,0 +1,115 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** A source that represents a MJPEG-over-HTTP (IP) camera. */
+public class HttpCamera extends VideoCamera {
+  public enum HttpCameraKind {
+    kUnknown(0),
+    kMJPGStreamer(1),
+    kCSCore(2),
+    kAxis(3);
+
+    private final int value;
+
+    HttpCameraKind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /**
+   * Convert from the numerical representation of kind to an enum type.
+   *
+   * @param kind The numerical representation of kind
+   * @return The kind
+   */
+  public static HttpCameraKind getHttpCameraKindFromInt(int kind) {
+    switch (kind) {
+      case 1:
+        return HttpCameraKind.kMJPGStreamer;
+      case 2:
+        return HttpCameraKind.kCSCore;
+      case 3:
+        return HttpCameraKind.kAxis;
+      default:
+        return HttpCameraKind.kUnknown;
+    }
+  }
+
+  /**
+   * Create a source for a MJPEG-over-HTTP (IP) camera.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
+   */
+  public HttpCamera(String name, String url) {
+    super(CameraServerJNI.createHttpCamera(name, url, HttpCameraKind.kUnknown.getValue()));
+  }
+
+  /**
+   * Create a source for a MJPEG-over-HTTP (IP) camera.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
+   * @param kind Camera kind (e.g. kAxis)
+   */
+  public HttpCamera(String name, String url, HttpCameraKind kind) {
+    super(CameraServerJNI.createHttpCamera(name, url, kind.getValue()));
+  }
+
+  /**
+   * Create a source for a MJPEG-over-HTTP (IP) camera.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param urls Array of Camera URLs
+   */
+  public HttpCamera(String name, String[] urls) {
+    super(CameraServerJNI.createHttpCameraMulti(name, urls, HttpCameraKind.kUnknown.getValue()));
+  }
+
+  /**
+   * Create a source for a MJPEG-over-HTTP (IP) camera.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param urls Array of Camera URLs
+   * @param kind Camera kind (e.g. kAxis)
+   */
+  public HttpCamera(String name, String[] urls, HttpCameraKind kind) {
+    super(CameraServerJNI.createHttpCameraMulti(name, urls, kind.getValue()));
+  }
+
+  /**
+   * Get the kind of HTTP camera.
+   *
+   * <p>Autodetection can result in returning a different value than the camera was created with.
+   *
+   * @return The kind of HTTP camera.
+   */
+  public HttpCameraKind getHttpCameraKind() {
+    return getHttpCameraKindFromInt(CameraServerJNI.getHttpCameraKind(m_handle));
+  }
+
+  /**
+   * Change the URLs used to connect to the camera.
+   *
+   * @param urls Array of Camera URLs
+   */
+  public void setUrls(String[] urls) {
+    CameraServerJNI.setHttpCameraUrls(m_handle, urls);
+  }
+
+  /**
+   * Get the URLs used to connect to the camera.
+   *
+   * @return Array of camera URLs.
+   */
+  public String[] getUrls() {
+    return CameraServerJNI.getHttpCameraUrls(m_handle);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/ImageSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/ImageSink.java
new file mode 100644
index 0000000..1c4cac2
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/ImageSink.java
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+public abstract class ImageSink extends VideoSink {
+  protected ImageSink(int handle) {
+    super(handle);
+  }
+
+  /**
+   * Set sink description.
+   *
+   * @param description Description
+   */
+  public void setDescription(String description) {
+    CameraServerJNI.setSinkDescription(m_handle, description);
+  }
+
+  /**
+   * Get error string. Call this if WaitForFrame() returns 0 to determine what the error is.
+   *
+   * @return Error string.
+   */
+  public String getError() {
+    return CameraServerJNI.getSinkError(m_handle);
+  }
+
+  /**
+   * Enable or disable getting new frames. Disabling will cause processFrame (for callback-based
+   * CvSinks) to not be called and WaitForFrame() to not return. This can be used to save processor
+   * resources when frames are not needed.
+   *
+   * @param enabled Enable to get new frames.
+   */
+  public void setEnabled(boolean enabled) {
+    CameraServerJNI.setSinkEnabled(m_handle, enabled);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/ImageSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/ImageSource.java
new file mode 100644
index 0000000..6bebed8
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/ImageSource.java
@@ -0,0 +1,136 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+public abstract class ImageSource extends VideoSource {
+  protected ImageSource(int handle) {
+    super(handle);
+  }
+
+  /**
+   * Signal sinks that an error has occurred. This should be called instead of NotifyFrame when an
+   * error occurs.
+   *
+   * @param msg Error message.
+   */
+  public void notifyError(String msg) {
+    CameraServerJNI.notifySourceError(m_handle, msg);
+  }
+
+  /**
+   * Set source connection status. Defaults to true.
+   *
+   * @param connected True for connected, false for disconnected
+   */
+  public void setConnected(boolean connected) {
+    CameraServerJNI.setSourceConnected(m_handle, connected);
+  }
+
+  /**
+   * Set source description.
+   *
+   * @param description Description
+   */
+  public void setDescription(String description) {
+    CameraServerJNI.setSourceDescription(m_handle, description);
+  }
+
+  /**
+   * Create a property.
+   *
+   * @param name Property name
+   * @param kind Property kind
+   * @param minimum Minimum value
+   * @param maximum Maximum value
+   * @param step Step value
+   * @param defaultValue Default value
+   * @param value Current value
+   * @return Property
+   */
+  public VideoProperty createProperty(
+      String name,
+      VideoProperty.Kind kind,
+      int minimum,
+      int maximum,
+      int step,
+      int defaultValue,
+      int value) {
+    return new VideoProperty(
+        CameraServerJNI.createSourceProperty(
+            m_handle, name, kind.getValue(), minimum, maximum, step, defaultValue, value));
+  }
+
+  /**
+   * Create an integer property.
+   *
+   * @param name Property name
+   * @param minimum Minimum value
+   * @param maximum Maximum value
+   * @param step Step value
+   * @param defaultValue Default value
+   * @param value Current value
+   * @return Property
+   */
+  public VideoProperty createIntegerProperty(
+      String name, int minimum, int maximum, int step, int defaultValue, int value) {
+    return new VideoProperty(
+        CameraServerJNI.createSourceProperty(
+            m_handle,
+            name,
+            VideoProperty.Kind.kInteger.getValue(),
+            minimum,
+            maximum,
+            step,
+            defaultValue,
+            value));
+  }
+
+  /**
+   * Create a boolean property.
+   *
+   * @param name Property name
+   * @param defaultValue Default value
+   * @param value Current value
+   * @return Property
+   */
+  public VideoProperty createBooleanProperty(String name, boolean defaultValue, boolean value) {
+    return new VideoProperty(
+        CameraServerJNI.createSourceProperty(
+            m_handle,
+            name,
+            VideoProperty.Kind.kBoolean.getValue(),
+            0,
+            1,
+            1,
+            defaultValue ? 1 : 0,
+            value ? 1 : 0));
+  }
+
+  /**
+   * Create a string property.
+   *
+   * @param name Property name
+   * @param value Current value
+   * @return Property
+   */
+  public VideoProperty createStringProperty(String name, String value) {
+    VideoProperty prop =
+        new VideoProperty(
+            CameraServerJNI.createSourceProperty(
+                m_handle, name, VideoProperty.Kind.kString.getValue(), 0, 0, 0, 0, 0));
+    prop.setString(value);
+    return prop;
+  }
+
+  /**
+   * Configure enum property choices.
+   *
+   * @param property Property
+   * @param choices Choices
+   */
+  public void setEnumPropertyChoices(VideoProperty property, String[] choices) {
+    CameraServerJNI.setSourceEnumPropertyChoices(m_handle, property.m_handle, choices);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/MjpegServer.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/MjpegServer.java
new file mode 100644
index 0000000..54c8053
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/MjpegServer.java
@@ -0,0 +1,99 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** A sink that acts as a MJPEG-over-HTTP network server. */
+public class MjpegServer extends VideoSink {
+  /**
+   * Create a MJPEG-over-HTTP server sink.
+   *
+   * @param name Sink name (arbitrary unique identifier)
+   * @param listenAddress TCP listen address (empty string for all addresses)
+   * @param port TCP port number
+   */
+  public MjpegServer(String name, String listenAddress, int port) {
+    super(CameraServerJNI.createMjpegServer(name, listenAddress, port));
+  }
+
+  /**
+   * Create a MJPEG-over-HTTP server sink.
+   *
+   * @param name Sink name (arbitrary unique identifier)
+   * @param port TCP port number
+   */
+  public MjpegServer(String name, int port) {
+    this(name, "", port);
+  }
+
+  /**
+   * Get the listen address of the server.
+   *
+   * @return The listen address.
+   */
+  public String getListenAddress() {
+    return CameraServerJNI.getMjpegServerListenAddress(m_handle);
+  }
+
+  /**
+   * Get the port number of the server.
+   *
+   * @return The port number.
+   */
+  public int getPort() {
+    return CameraServerJNI.getMjpegServerPort(m_handle);
+  }
+
+  /**
+   * Set the stream resolution for clients that don't specify it.
+   *
+   * <p>It is not necessary to set this if it is the same as the source resolution.
+   *
+   * <p>Setting this different than the source resolution will result in increased CPU usage,
+   * particularly for MJPEG source cameras, as it will decompress, resize, and recompress the image,
+   * instead of using the camera's MJPEG image directly.
+   *
+   * @param width width, 0 for unspecified
+   * @param height height, 0 for unspecified
+   */
+  public void setResolution(int width, int height) {
+    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "width"), width);
+    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "height"), height);
+  }
+
+  /**
+   * Set the stream frames per second (FPS) for clients that don't specify it.
+   *
+   * <p>It is not necessary to set this if it is the same as the source FPS.
+   *
+   * @param fps FPS, 0 for unspecified
+   */
+  public void setFPS(int fps) {
+    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "fps"), fps);
+  }
+
+  /**
+   * Set the compression for clients that don't specify it.
+   *
+   * <p>Setting this will result in increased CPU usage for MJPEG source cameras as it will
+   * decompress and recompress the image instead of using the camera's MJPEG image directly.
+   *
+   * @param quality JPEG compression quality (0-100), -1 for unspecified
+   */
+  public void setCompression(int quality) {
+    CameraServerJNI.setProperty(CameraServerJNI.getSinkProperty(m_handle, "compression"), quality);
+  }
+
+  /**
+   * Set the default compression used for non-MJPEG sources. If not set, 80 is used. This function
+   * has no effect on MJPEG source cameras; use SetCompression() instead to force recompression of
+   * MJPEG source images.
+   *
+   * @param quality JPEG compression quality (0-100)
+   */
+  public void setDefaultCompression(int quality) {
+    CameraServerJNI.setProperty(
+        CameraServerJNI.getSinkProperty(m_handle, "default_compression"), quality);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/UsbCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/UsbCamera.java
new file mode 100644
index 0000000..8e0817e
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/UsbCamera.java
@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** A source that represents a USB camera. */
+public class UsbCamera extends VideoCamera {
+  /**
+   * Create a source for a USB camera based on device number.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param dev Device number (e.g. 0 for /dev/video0)
+   */
+  public UsbCamera(String name, int dev) {
+    super(CameraServerJNI.createUsbCameraDev(name, dev));
+  }
+
+  /**
+   * Create a source for a USB camera based on device path.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param path Path to device (e.g. "/dev/video0" on Linux)
+   */
+  public UsbCamera(String name, String path) {
+    super(CameraServerJNI.createUsbCameraPath(name, path));
+  }
+
+  /**
+   * Enumerate USB cameras on the local system.
+   *
+   * @return Vector of USB camera information (one for each camera)
+   */
+  public static UsbCameraInfo[] enumerateUsbCameras() {
+    return CameraServerJNI.enumerateUsbCameras();
+  }
+
+  /**
+   * Change the path to the device.
+   *
+   * @param path New device path.
+   */
+  void setPath(String path) {
+    CameraServerJNI.setUsbCameraPath(m_handle, path);
+  }
+
+  /**
+   * Get the path to the device.
+   *
+   * @return The device path.
+   */
+  public String getPath() {
+    return CameraServerJNI.getUsbCameraPath(m_handle);
+  }
+
+  /**
+   * Get the full camera information for the device.
+   *
+   * @return The camera information.
+   */
+  public UsbCameraInfo getInfo() {
+    return CameraServerJNI.getUsbCameraInfo(m_handle);
+  }
+
+  /**
+   * Set how verbose the camera connection messages are.
+   *
+   * @param level 0=don't display Connecting message, 1=do display message
+   */
+  public void setConnectVerbose(int level) {
+    CameraServerJNI.setProperty(
+        CameraServerJNI.getSourceProperty(m_handle, "connect_verbose"), level);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/UsbCameraInfo.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/UsbCameraInfo.java
new file mode 100644
index 0000000..df856e8
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/UsbCameraInfo.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** USB camera information. */
+public class UsbCameraInfo {
+  /**
+   * Create a new set of UsbCameraInfo.
+   *
+   * @param dev Device number (e.g. N in '/dev/videoN' on Linux)
+   * @param path Path to device if available (e.g. '/dev/video0' on Linux)
+   * @param name Vendor/model name of the camera as provided by the USB driver
+   * @param otherPaths Other path aliases to device
+   * @param vendorId USB vendor id
+   * @param productId USB product id
+   */
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public UsbCameraInfo(
+      int dev, String path, String name, String[] otherPaths, int vendorId, int productId) {
+    this.dev = dev;
+    this.path = path;
+    this.name = name;
+    this.otherPaths = otherPaths;
+    this.vendorId = vendorId;
+    this.productId = productId;
+  }
+
+  /** Device number (e.g. N in '/dev/videoN' on Linux). */
+  @SuppressWarnings("MemberName")
+  public int dev;
+
+  /** Path to device if available (e.g. '/dev/video0' on Linux). */
+  @SuppressWarnings("MemberName")
+  public String path;
+
+  /** Vendor/model name of the camera as provided by the USB driver. */
+  @SuppressWarnings("MemberName")
+  public String name;
+
+  /** Other path aliases to device (e.g. '/dev/v4l/by-id/...' etc on Linux). */
+  @SuppressWarnings("MemberName")
+  public String[] otherPaths;
+
+  /** USB vendor id. */
+  @SuppressWarnings("MemberName")
+  public int vendorId;
+
+  /** USB product id. */
+  @SuppressWarnings("MemberName")
+  public int productId;
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java
new file mode 100644
index 0000000..23cf7d6
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoCamera.java
@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** A source that represents a video camera. */
+public class VideoCamera extends VideoSource {
+  public static class WhiteBalance {
+    public static final int kFixedIndoor = 3000;
+    public static final int kFixedOutdoor1 = 4000;
+    public static final int kFixedOutdoor2 = 5000;
+    public static final int kFixedFluorescent1 = 5100;
+    public static final int kFixedFlourescent2 = 5200;
+  }
+
+  protected VideoCamera(int handle) {
+    super(handle);
+  }
+
+  /**
+   * Set the brightness, as a percentage (0-100).
+   *
+   * @param brightness Brightness as a percentage (0-100).
+   */
+  public synchronized void setBrightness(int brightness) {
+    CameraServerJNI.setCameraBrightness(m_handle, brightness);
+  }
+
+  /**
+   * Get the brightness, as a percentage (0-100).
+   *
+   * @return The brightness as a percentage (0-100).
+   */
+  public synchronized int getBrightness() {
+    return CameraServerJNI.getCameraBrightness(m_handle);
+  }
+
+  /** Set the white balance to auto. */
+  public synchronized void setWhiteBalanceAuto() {
+    CameraServerJNI.setCameraWhiteBalanceAuto(m_handle);
+  }
+
+  /** Set the white balance to hold current. */
+  public synchronized void setWhiteBalanceHoldCurrent() {
+    CameraServerJNI.setCameraWhiteBalanceHoldCurrent(m_handle);
+  }
+
+  /**
+   * Set the white balance to manual, with specified color temperature.
+   *
+   * @param value The specified color temperature.
+   */
+  public synchronized void setWhiteBalanceManual(int value) {
+    CameraServerJNI.setCameraWhiteBalanceManual(m_handle, value);
+  }
+
+  /** Set the exposure to auto aperture. */
+  public synchronized void setExposureAuto() {
+    CameraServerJNI.setCameraExposureAuto(m_handle);
+  }
+
+  /** Set the exposure to hold current. */
+  public synchronized void setExposureHoldCurrent() {
+    CameraServerJNI.setCameraExposureHoldCurrent(m_handle);
+  }
+
+  /**
+   * Set the exposure to manual, as a percentage (0-100).
+   *
+   * @param value The exposure as a percentage (0-100).
+   */
+  public synchronized void setExposureManual(int value) {
+    CameraServerJNI.setCameraExposureManual(m_handle, value);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java
new file mode 100644
index 0000000..50f041c
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoEvent.java
@@ -0,0 +1,166 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** Video event. */
+public class VideoEvent {
+  public enum Kind {
+    kUnknown(0x0000),
+    kSourceCreated(0x0001),
+    kSourceDestroyed(0x0002),
+    kSourceConnected(0x0004),
+    kSourceDisconnected(0x0008),
+    kSourceVideoModesUpdated(0x0010),
+    kSourceVideoModeChanged(0x0020),
+    kSourcePropertyCreated(0x0040),
+    kSourcePropertyValueUpdated(0x0080),
+    kSourcePropertyChoicesUpdated(0x0100),
+    kSinkSourceChanged(0x0200),
+    kSinkCreated(0x0400),
+    kSinkDestroyed(0x0800),
+    kSinkEnabled(0x1000),
+    kSinkDisabled(0x2000),
+    kNetworkInterfacesChanged(0x4000),
+    kTelemetryUpdated(0x8000),
+    kSinkPropertyCreated(0x10000),
+    kSinkPropertyValueUpdated(0x20000),
+    kSinkPropertyChoicesUpdated(0x40000),
+    kUsbCamerasChanged(0x80000);
+
+    private final int value;
+
+    Kind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /**
+   * Convert from the numerical representation of kind to an enum type.
+   *
+   * @param kind The numerical representation of kind
+   * @return The kind
+   */
+  public static Kind getKindFromInt(int kind) {
+    switch (kind) {
+      case 0x0001:
+        return Kind.kSourceCreated;
+      case 0x0002:
+        return Kind.kSourceDestroyed;
+      case 0x0004:
+        return Kind.kSourceConnected;
+      case 0x0008:
+        return Kind.kSourceDisconnected;
+      case 0x0010:
+        return Kind.kSourceVideoModesUpdated;
+      case 0x0020:
+        return Kind.kSourceVideoModeChanged;
+      case 0x0040:
+        return Kind.kSourcePropertyCreated;
+      case 0x0080:
+        return Kind.kSourcePropertyValueUpdated;
+      case 0x0100:
+        return Kind.kSourcePropertyChoicesUpdated;
+      case 0x0200:
+        return Kind.kSinkSourceChanged;
+      case 0x0400:
+        return Kind.kSinkCreated;
+      case 0x0800:
+        return Kind.kSinkDestroyed;
+      case 0x1000:
+        return Kind.kSinkEnabled;
+      case 0x2000:
+        return Kind.kSinkDisabled;
+      case 0x4000:
+        return Kind.kNetworkInterfacesChanged;
+      case 0x10000:
+        return Kind.kSinkPropertyCreated;
+      case 0x20000:
+        return Kind.kSinkPropertyValueUpdated;
+      case 0x40000:
+        return Kind.kSinkPropertyChoicesUpdated;
+      case 0x80000:
+        return Kind.kUsbCamerasChanged;
+      default:
+        return Kind.kUnknown;
+    }
+  }
+
+  VideoEvent(
+      int kind,
+      int source,
+      int sink,
+      String name,
+      int pixelFormat,
+      int width,
+      int height,
+      int fps,
+      int property,
+      int propertyKind,
+      int value,
+      String valueStr,
+      int listener) {
+    this.kind = getKindFromInt(kind);
+    this.sourceHandle = source;
+    this.sinkHandle = sink;
+    this.name = name;
+    this.mode = new VideoMode(pixelFormat, width, height, fps);
+    this.propertyHandle = property;
+    this.propertyKind = VideoProperty.getKindFromInt(propertyKind);
+    this.value = value;
+    this.valueStr = valueStr;
+    this.listener = listener;
+  }
+
+  @SuppressWarnings("MemberName")
+  public Kind kind;
+
+  // Valid for kSource* and kSink* respectively
+  @SuppressWarnings("MemberName")
+  public int sourceHandle;
+
+  @SuppressWarnings("MemberName")
+  public int sinkHandle;
+
+  // Source/sink/property name
+  @SuppressWarnings("MemberName")
+  public String name;
+
+  // Fields for kSourceVideoModeChanged event
+  @SuppressWarnings("MemberName")
+  public VideoMode mode;
+
+  // Fields for kSourceProperty* events
+  @SuppressWarnings("MemberName")
+  public int propertyHandle;
+
+  @SuppressWarnings("MemberName")
+  public VideoProperty.Kind propertyKind;
+
+  @SuppressWarnings("MemberName")
+  public int value;
+
+  @SuppressWarnings("MemberName")
+  public String valueStr;
+
+  // Listener that was triggered
+  @SuppressWarnings("MemberName")
+  public int listener;
+
+  public VideoSource getSource() {
+    return new VideoSource(CameraServerJNI.copySource(sourceHandle));
+  }
+
+  public VideoSink getSink() {
+    return new VideoSink(CameraServerJNI.copySink(sinkHandle));
+  }
+
+  public VideoProperty getProperty() {
+    return new VideoProperty(propertyHandle, propertyKind);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoException.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoException.java
new file mode 100644
index 0000000..1c445c6
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoException.java
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** An exception raised by the camera server. */
+public class VideoException extends RuntimeException {
+  private static final long serialVersionUID = -9155939328084105145L;
+
+  public VideoException(String msg) {
+    super(msg);
+  }
+
+  @Override
+  public String toString() {
+    return "VideoException [" + super.toString() + "]";
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java
new file mode 100644
index 0000000..0d77461
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoListener.java
@@ -0,0 +1,126 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import java.util.HashMap;
+import java.util.Map;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Consumer;
+
+/**
+ * An event listener. This calls back to a desigated callback function when an event matching the
+ * specified mask is generated by the library.
+ */
+public class VideoListener implements AutoCloseable {
+  /**
+   * Create an event listener.
+   *
+   * @param listener Listener function
+   * @param eventMask Bitmask of VideoEvent.Type values
+   * @param immediateNotify Whether callback should be immediately called with a representative set
+   *     of events for the current library state.
+   */
+  public VideoListener(Consumer<VideoEvent> listener, int eventMask, boolean immediateNotify) {
+    s_lock.lock();
+    try {
+      if (s_poller == 0) {
+        s_poller = CameraServerJNI.createListenerPoller();
+        startThread();
+      }
+      m_handle = CameraServerJNI.addPolledListener(s_poller, eventMask, immediateNotify);
+      s_listeners.put(m_handle, listener);
+    } finally {
+      s_lock.unlock();
+    }
+  }
+
+  @Override
+  public synchronized void close() {
+    if (m_handle != 0) {
+      s_lock.lock();
+      try {
+        s_listeners.remove(m_handle);
+      } finally {
+        s_lock.unlock();
+      }
+      CameraServerJNI.removeListener(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  private int m_handle;
+
+  private static final ReentrantLock s_lock = new ReentrantLock();
+  private static final Map<Integer, Consumer<VideoEvent>> s_listeners = new HashMap<>();
+  private static Thread s_thread;
+  private static int s_poller;
+  private static boolean s_waitQueue;
+  private static final Condition s_waitQueueCond = s_lock.newCondition();
+
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
+  private static void startThread() {
+    s_thread =
+        new Thread(
+            () -> {
+              boolean wasInterrupted = false;
+              while (!Thread.interrupted()) {
+                VideoEvent[] events;
+                try {
+                  events = CameraServerJNI.pollListener(s_poller);
+                } catch (InterruptedException ex) {
+                  s_lock.lock();
+                  try {
+                    if (s_waitQueue) {
+                      s_waitQueue = false;
+                      s_waitQueueCond.signalAll();
+                      continue;
+                    }
+                  } finally {
+                    s_lock.unlock();
+                  }
+                  Thread.currentThread().interrupt();
+                  // don't try to destroy poller, as its handle is likely no longer valid
+                  wasInterrupted = true;
+                  break;
+                }
+                for (VideoEvent event : events) {
+                  Consumer<VideoEvent> listener;
+                  s_lock.lock();
+                  try {
+                    listener = s_listeners.get(event.listener);
+                  } finally {
+                    s_lock.unlock();
+                  }
+                  if (listener != null) {
+                    try {
+                      listener.accept(event);
+                    } catch (Throwable throwable) {
+                      System.err.println(
+                          "Unhandled exception during listener callback: " + throwable.toString());
+                      throwable.printStackTrace();
+                    }
+                  }
+                }
+              }
+              s_lock.lock();
+              try {
+                if (!wasInterrupted) {
+                  CameraServerJNI.destroyListenerPoller(s_poller);
+                }
+                s_poller = 0;
+              } finally {
+                s_lock.unlock();
+              }
+            },
+            "VideoListener");
+    s_thread.setDaemon(true);
+    s_thread.start();
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java
new file mode 100644
index 0000000..726b210
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoMode.java
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** Video mode. */
+public class VideoMode {
+  public enum PixelFormat {
+    kUnknown(0),
+    kMJPEG(1),
+    kYUYV(2),
+    kRGB565(3),
+    kBGR(4),
+    kGray(5);
+
+    private final int value;
+
+    PixelFormat(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  private static final PixelFormat[] m_pixelFormatValues = PixelFormat.values();
+
+  public static PixelFormat getPixelFormatFromInt(int pixelFormat) {
+    return m_pixelFormatValues[pixelFormat];
+  }
+
+  /**
+   * Create a new video mode.
+   *
+   * @param pixelFormat The pixel format enum as an integer.
+   * @param width The image width in pixels.
+   * @param height The image height in pixels.
+   * @param fps The camera's frames per second.
+   */
+  public VideoMode(int pixelFormat, int width, int height, int fps) {
+    this.pixelFormat = getPixelFormatFromInt(pixelFormat);
+    this.width = width;
+    this.height = height;
+    this.fps = fps;
+  }
+
+  /**
+   * Create a new video mode.
+   *
+   * @param pixelFormat The pixel format.
+   * @param width The image width in pixels.
+   * @param height The image height in pixels.
+   * @param fps The camera's frames per second.
+   */
+  public VideoMode(PixelFormat pixelFormat, int width, int height, int fps) {
+    this.pixelFormat = pixelFormat;
+    this.width = width;
+    this.height = height;
+    this.fps = fps;
+  }
+
+  /** Pixel format. */
+  @SuppressWarnings("MemberName")
+  public PixelFormat pixelFormat;
+
+  /** Width in pixels. */
+  @SuppressWarnings("MemberName")
+  public int width;
+
+  /** Height in pixels. */
+  @SuppressWarnings("MemberName")
+  public int height;
+
+  /** Frames per second. */
+  @SuppressWarnings("MemberName")
+  public int fps;
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java
new file mode 100644
index 0000000..8179ba3
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoProperty.java
@@ -0,0 +1,127 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/** A source or sink property. */
+public class VideoProperty {
+  public enum Kind {
+    kNone(0),
+    kBoolean(1),
+    kInteger(2),
+    kString(4),
+    kEnum(8);
+
+    private final int value;
+
+    Kind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /**
+   * Convert from the numerical representation of kind to an enum type.
+   *
+   * @param kind The numerical representation of kind
+   * @return The kind
+   */
+  public static Kind getKindFromInt(int kind) {
+    switch (kind) {
+      case 1:
+        return Kind.kBoolean;
+      case 2:
+        return Kind.kInteger;
+      case 4:
+        return Kind.kString;
+      case 8:
+        return Kind.kEnum;
+      default:
+        return Kind.kNone;
+    }
+  }
+
+  public String getName() {
+    return CameraServerJNI.getPropertyName(m_handle);
+  }
+
+  public Kind getKind() {
+    return m_kind;
+  }
+
+  public boolean isValid() {
+    return m_kind != Kind.kNone;
+  }
+
+  // Kind checkers
+  public boolean isBoolean() {
+    return m_kind == Kind.kBoolean;
+  }
+
+  public boolean isInteger() {
+    return m_kind == Kind.kInteger;
+  }
+
+  public boolean isString() {
+    return m_kind == Kind.kString;
+  }
+
+  public boolean isEnum() {
+    return m_kind == Kind.kEnum;
+  }
+
+  public int get() {
+    return CameraServerJNI.getProperty(m_handle);
+  }
+
+  public void set(int value) {
+    CameraServerJNI.setProperty(m_handle, value);
+  }
+
+  public int getMin() {
+    return CameraServerJNI.getPropertyMin(m_handle);
+  }
+
+  public int getMax() {
+    return CameraServerJNI.getPropertyMax(m_handle);
+  }
+
+  public int getStep() {
+    return CameraServerJNI.getPropertyStep(m_handle);
+  }
+
+  public int getDefault() {
+    return CameraServerJNI.getPropertyDefault(m_handle);
+  }
+
+  // String-specific functions
+  public String getString() {
+    return CameraServerJNI.getStringProperty(m_handle);
+  }
+
+  public void setString(String value) {
+    CameraServerJNI.setStringProperty(m_handle, value);
+  }
+
+  // Enum-specific functions
+  public String[] getChoices() {
+    return CameraServerJNI.getEnumPropertyChoices(m_handle);
+  }
+
+  VideoProperty(int handle) {
+    m_handle = handle;
+    m_kind = getKindFromInt(CameraServerJNI.getPropertyKind(handle));
+  }
+
+  VideoProperty(int handle, Kind kind) {
+    m_handle = handle;
+    m_kind = kind;
+  }
+
+  int m_handle;
+  private Kind m_kind;
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java
new file mode 100644
index 0000000..8b07f46
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSink.java
@@ -0,0 +1,221 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/**
+ * A source for video that provides a sequence of frames. Each frame may consist of multiple images
+ * (e.g. from a stereo or depth camera); these are called channels.
+ */
+public class VideoSink implements AutoCloseable {
+  public enum Kind {
+    kUnknown(0),
+    kMjpeg(2),
+    kCv(4),
+    kRaw(8);
+
+    private final int value;
+
+    Kind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /**
+   * Convert from the numerical representation of kind to an enum type.
+   *
+   * @param kind The numerical representation of kind
+   * @return The kind
+   */
+  public static Kind getKindFromInt(int kind) {
+    switch (kind) {
+      case 2:
+        return Kind.kMjpeg;
+      case 4:
+        return Kind.kCv;
+      default:
+        return Kind.kUnknown;
+    }
+  }
+
+  protected VideoSink(int handle) {
+    m_handle = handle;
+  }
+
+  @Override
+  public synchronized void close() {
+    if (m_handle != 0) {
+      CameraServerJNI.releaseSink(m_handle);
+    }
+    m_handle = 0;
+  }
+
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  public int getHandle() {
+    return m_handle;
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null) {
+      return false;
+    }
+    if (getClass() != other.getClass()) {
+      return false;
+    }
+    VideoSink sink = (VideoSink) other;
+    return m_handle == sink.m_handle;
+  }
+
+  @Override
+  public int hashCode() {
+    return m_handle;
+  }
+
+  /**
+   * Get the kind of the sink.
+   *
+   * @return The kind of the sink.
+   */
+  public Kind getKind() {
+    return getKindFromInt(CameraServerJNI.getSinkKind(m_handle));
+  }
+
+  /**
+   * Get the name of the sink. The name is an arbitrary identifier provided when the sink is
+   * created, and should be unique.
+   *
+   * @return The name of the sink.
+   */
+  public String getName() {
+    return CameraServerJNI.getSinkName(m_handle);
+  }
+
+  /**
+   * Get the sink description. This is sink-kind specific.
+   *
+   * @return The sink description.
+   */
+  public String getDescription() {
+    return CameraServerJNI.getSinkDescription(m_handle);
+  }
+
+  /**
+   * Get a property of the sink.
+   *
+   * @param name Property name
+   * @return Property (kind Property::kNone if no property with the given name exists)
+   */
+  public VideoProperty getProperty(String name) {
+    return new VideoProperty(CameraServerJNI.getSinkProperty(m_handle, name));
+  }
+
+  /**
+   * Enumerate all properties of this sink.
+   *
+   * @return List of properties.
+   */
+  public VideoProperty[] enumerateProperties() {
+    int[] handles = CameraServerJNI.enumerateSinkProperties(m_handle);
+    VideoProperty[] rv = new VideoProperty[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      rv[i] = new VideoProperty(handles[i]);
+    }
+    return rv;
+  }
+
+  /**
+   * Set properties from a JSON configuration string.
+   *
+   * <p>The format of the JSON input is:
+   *
+   * <pre>
+   * {
+   *     "properties": [
+   *         {
+   *             "name": property name
+   *             "value": property value
+   *         }
+   *     ]
+   * }
+   * </pre>
+   *
+   * @param config configuration
+   * @return True if set successfully
+   */
+  public boolean setConfigJson(String config) {
+    return CameraServerJNI.setSinkConfigJson(m_handle, config);
+  }
+
+  /**
+   * Get a JSON configuration string.
+   *
+   * @return JSON configuration string
+   */
+  public String getConfigJson() {
+    return CameraServerJNI.getSinkConfigJson(m_handle);
+  }
+
+  /**
+   * Configure which source should provide frames to this sink. Each sink can accept frames from
+   * only a single source, but a single source can provide frames to multiple clients.
+   *
+   * @param source Source
+   */
+  public void setSource(VideoSource source) {
+    if (source == null) {
+      CameraServerJNI.setSinkSource(m_handle, 0);
+    } else {
+      CameraServerJNI.setSinkSource(m_handle, source.m_handle);
+    }
+  }
+
+  /**
+   * Get the connected source.
+   *
+   * @return Connected source; nullptr if no source connected.
+   */
+  public VideoSource getSource() {
+    // While VideoSource.close() will call releaseSource(), getSinkSource()
+    // increments the internal reference count so this is okay to do.
+    return new VideoSource(CameraServerJNI.getSinkSource(m_handle));
+  }
+
+  /**
+   * Get a property of the associated source.
+   *
+   * @param name Property name
+   * @return Property (kind Property::kNone if no property with the given name exists or no source
+   *     connected)
+   */
+  public VideoProperty getSourceProperty(String name) {
+    return new VideoProperty(CameraServerJNI.getSinkSourceProperty(m_handle, name));
+  }
+
+  /**
+   * Enumerate all existing sinks.
+   *
+   * @return Vector of sinks.
+   */
+  public static VideoSink[] enumerateSinks() {
+    int[] handles = CameraServerJNI.enumerateSinks();
+    VideoSink[] rv = new VideoSink[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      rv[i] = new VideoSink(handles[i]);
+    }
+    return rv;
+  }
+
+  protected int m_handle;
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java
new file mode 100644
index 0000000..0ae5add
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/VideoSource.java
@@ -0,0 +1,376 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+/**
+ * A source for video that provides a sequence of frames. Each frame may consist of multiple images
+ * (e.g. from a stereo or depth camera); these are called channels.
+ */
+public class VideoSource implements AutoCloseable {
+  public enum Kind {
+    kUnknown(0),
+    kUsb(1),
+    kHttp(2),
+    kCv(4),
+    kRaw(8);
+
+    private final int value;
+
+    Kind(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /** Connection strategy. */
+  public enum ConnectionStrategy {
+    /**
+     * Automatically connect or disconnect based on whether any sinks are connected to this source.
+     * This is the default behavior.
+     */
+    kAutoManage(0),
+
+    /** Try to keep the connection open regardless of whether any sinks are connected. */
+    kKeepOpen(1),
+
+    /**
+     * Never open the connection. If this is set when the connection is open, close the connection.
+     */
+    kForceClose(2);
+
+    private final int value;
+
+    ConnectionStrategy(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+  }
+
+  /**
+   * Convert from the numerical representation of kind to an enum type.
+   *
+   * @param kind The numerical representation of kind
+   * @return The kind
+   */
+  public static Kind getKindFromInt(int kind) {
+    switch (kind) {
+      case 1:
+        return Kind.kUsb;
+      case 2:
+        return Kind.kHttp;
+      case 4:
+        return Kind.kCv;
+      default:
+        return Kind.kUnknown;
+    }
+  }
+
+  protected VideoSource(int handle) {
+    m_handle = handle;
+  }
+
+  @Override
+  public synchronized void close() {
+    if (m_handle != 0) {
+      CameraServerJNI.releaseSource(m_handle);
+    }
+    m_handle = 0;
+  }
+
+  public boolean isValid() {
+    return m_handle != 0;
+  }
+
+  public int getHandle() {
+    return m_handle;
+  }
+
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (other == null) {
+      return false;
+    }
+    if (getClass() != other.getClass()) {
+      return false;
+    }
+    VideoSource source = (VideoSource) other;
+    return m_handle == source.m_handle;
+  }
+
+  @Override
+  public int hashCode() {
+    return m_handle;
+  }
+
+  /**
+   * Get the kind of the source.
+   *
+   * @return The kind of the source.
+   */
+  public Kind getKind() {
+    return getKindFromInt(CameraServerJNI.getSourceKind(m_handle));
+  }
+
+  /**
+   * Get the name of the source. The name is an arbitrary identifier provided when the source is
+   * created, and should be unique.
+   *
+   * @return The name of the source.
+   */
+  public String getName() {
+    return CameraServerJNI.getSourceName(m_handle);
+  }
+
+  /**
+   * Get the source description. This is source-kind specific.
+   *
+   * @return The source description.
+   */
+  public String getDescription() {
+    return CameraServerJNI.getSourceDescription(m_handle);
+  }
+
+  /**
+   * Get the last time a frame was captured.
+   *
+   * @return Time in 1 us increments.
+   */
+  public long getLastFrameTime() {
+    return CameraServerJNI.getSourceLastFrameTime(m_handle);
+  }
+
+  /**
+   * Sets the connection strategy. By default, the source will automatically connect or disconnect
+   * based on whether any sinks are connected.
+   *
+   * <p>This function is non-blocking; look for either a connection open or close event or call
+   * {@link #isConnected()} to determine the connection state.
+   *
+   * @param strategy connection strategy (auto, keep open, or force close)
+   */
+  public void setConnectionStrategy(ConnectionStrategy strategy) {
+    CameraServerJNI.setSourceConnectionStrategy(m_handle, strategy.getValue());
+  }
+
+  /**
+   * Returns true if the source currently connected to whatever is providing the images.
+   *
+   * @return True if the source currently connected to whatever is providing the images.
+   */
+  public boolean isConnected() {
+    return CameraServerJNI.isSourceConnected(m_handle);
+  }
+
+  /**
+   * Gets source enable status. This is determined with a combination of connection strategy and the
+   * number of sinks connected.
+   *
+   * @return True if enabled, false otherwise.
+   */
+  public boolean isEnabled() {
+    return CameraServerJNI.isSourceEnabled(m_handle);
+  }
+
+  /**
+   * Get a property.
+   *
+   * @param name Property name
+   * @return Property contents (of kind Property::kNone if no property with the given name exists)
+   */
+  public VideoProperty getProperty(String name) {
+    return new VideoProperty(CameraServerJNI.getSourceProperty(m_handle, name));
+  }
+
+  /**
+   * Enumerate all properties of this source.
+   *
+   * @return Array of video properties.
+   */
+  public VideoProperty[] enumerateProperties() {
+    int[] handles = CameraServerJNI.enumerateSourceProperties(m_handle);
+    VideoProperty[] rv = new VideoProperty[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      rv[i] = new VideoProperty(handles[i]);
+    }
+    return rv;
+  }
+
+  /**
+   * Get the current video mode.
+   *
+   * @return The current video mode.
+   */
+  public VideoMode getVideoMode() {
+    return CameraServerJNI.getSourceVideoMode(m_handle);
+  }
+
+  /**
+   * Set the video mode.
+   *
+   * @param mode Video mode
+   * @return True if set successfully.
+   */
+  public boolean setVideoMode(VideoMode mode) {
+    return CameraServerJNI.setSourceVideoMode(
+        m_handle, mode.pixelFormat.getValue(), mode.width, mode.height, mode.fps);
+  }
+
+  /**
+   * Set the video mode.
+   *
+   * @param pixelFormat desired pixel format
+   * @param width desired width
+   * @param height desired height
+   * @param fps desired FPS
+   * @return True if set successfully
+   */
+  public boolean setVideoMode(VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+    return CameraServerJNI.setSourceVideoMode(m_handle, pixelFormat.getValue(), width, height, fps);
+  }
+
+  /**
+   * Set the pixel format.
+   *
+   * @param pixelFormat desired pixel format
+   * @return True if set successfully
+   */
+  public boolean setPixelFormat(VideoMode.PixelFormat pixelFormat) {
+    return CameraServerJNI.setSourcePixelFormat(m_handle, pixelFormat.getValue());
+  }
+
+  /**
+   * Set the resolution.
+   *
+   * @param width desired width
+   * @param height desired height
+   * @return True if set successfully
+   */
+  public boolean setResolution(int width, int height) {
+    return CameraServerJNI.setSourceResolution(m_handle, width, height);
+  }
+
+  /**
+   * Set the frames per second (FPS).
+   *
+   * @param fps desired FPS
+   * @return True if set successfully
+   */
+  public boolean setFPS(int fps) {
+    return CameraServerJNI.setSourceFPS(m_handle, fps);
+  }
+
+  /**
+   * Set video mode and properties from a JSON configuration string.
+   *
+   * <p>The format of the JSON input is:
+   *
+   * <pre>
+   * {
+   *     "pixel format": "MJPEG", "YUYV", etc
+   *     "width": video mode width
+   *     "height": video mode height
+   *     "fps": video mode fps
+   *     "brightness": percentage brightness
+   *     "white balance": "auto", "hold", or value
+   *     "exposure": "auto", "hold", or value
+   *     "properties": [
+   *         {
+   *             "name": property name
+   *             "value": property value
+   *         }
+   *     ]
+   * }
+   * </pre>
+   *
+   * @param config configuration
+   * @return True if set successfully
+   */
+  public boolean setConfigJson(String config) {
+    return CameraServerJNI.setSourceConfigJson(m_handle, config);
+  }
+
+  /**
+   * Get a JSON configuration string.
+   *
+   * @return JSON configuration string
+   */
+  public String getConfigJson() {
+    return CameraServerJNI.getSourceConfigJson(m_handle);
+  }
+
+  /**
+   * Get the actual FPS.
+   *
+   * <p>CameraServerJNI#setTelemetryPeriod() must be called for this to be valid (throws
+   * VisionException if telemetry is not enabled).
+   *
+   * @return Actual FPS averaged over the telemetry period.
+   */
+  public double getActualFPS() {
+    return CameraServerJNI.getTelemetryAverageValue(
+        m_handle, CameraServerJNI.TelemetryKind.kSourceFramesReceived);
+  }
+
+  /**
+   * Get the data rate (in bytes per second).
+   *
+   * <p>CameraServerJNI#setTelemetryPeriod() must be called for this to be valid (throws
+   * VisionException if telemetry is not enabled).
+   *
+   * @return Data rate averaged over the telemetry period.
+   */
+  public double getActualDataRate() {
+    return CameraServerJNI.getTelemetryAverageValue(
+        m_handle, CameraServerJNI.TelemetryKind.kSourceBytesReceived);
+  }
+
+  /**
+   * Enumerate all known video modes for this source.
+   *
+   * @return Vector of video modes.
+   */
+  public VideoMode[] enumerateVideoModes() {
+    return CameraServerJNI.enumerateSourceVideoModes(m_handle);
+  }
+
+  /**
+   * Enumerate all sinks connected to this source.
+   *
+   * @return Vector of sinks.
+   */
+  public VideoSink[] enumerateSinks() {
+    int[] handles = CameraServerJNI.enumerateSourceSinks(m_handle);
+    VideoSink[] rv = new VideoSink[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      rv[i] = new VideoSink(handles[i]);
+    }
+    return rv;
+  }
+
+  /**
+   * Enumerate all existing sources.
+   *
+   * @return Vector of sources.
+   */
+  public static VideoSource[] enumerateSources() {
+    int[] handles = CameraServerJNI.enumerateSources();
+    VideoSource[] rv = new VideoSource[handles.length];
+    for (int i = 0; i < handles.length; i++) {
+      rv[i] = new VideoSource(handles[i]);
+    }
+    return rv;
+  }
+
+  protected int m_handle;
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawFrame.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawFrame.java
new file mode 100644
index 0000000..5f62481
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawFrame.java
@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore.raw;
+
+import edu.wpi.first.cscore.CameraServerJNI;
+import java.nio.ByteBuffer;
+
+/**
+ * Class for storing raw frame data between image read call.
+ *
+ * <p>Data is reused for each frame read, rather then reallocating every frame.
+ */
+public class RawFrame implements AutoCloseable {
+  private final long m_framePtr;
+  private ByteBuffer m_dataByteBuffer;
+  private long m_dataPtr;
+  private int m_totalData;
+  private int m_width;
+  private int m_height;
+  private int m_pixelFormat;
+
+  /** Construct a new RawFrame. */
+  public RawFrame() {
+    m_framePtr = CameraServerJNI.allocateRawFrame();
+  }
+
+  /**
+   * Close the RawFrame, releasing native resources. Any images currently using the data will be
+   * invalidated.
+   */
+  @Override
+  public void close() {
+    CameraServerJNI.freeRawFrame(m_framePtr);
+  }
+
+  /**
+   * Called from JNI to set data in class.
+   *
+   * @param dataByteBuffer A ByteBuffer pointing to the frame data.
+   * @param dataPtr A long (a char* in native code) pointing to the frame data.
+   * @param totalData The total length of the data stored in the frame.
+   * @param width The width of the frame.
+   * @param height The height of the frame.
+   * @param pixelFormat The PixelFormat of the frame.
+   */
+  public void setData(
+      ByteBuffer dataByteBuffer,
+      long dataPtr,
+      int totalData,
+      int width,
+      int height,
+      int pixelFormat) {
+    m_dataByteBuffer = dataByteBuffer;
+    m_dataPtr = dataPtr;
+    m_totalData = totalData;
+    m_width = width;
+    m_height = height;
+    m_pixelFormat = pixelFormat;
+  }
+
+  /**
+   * Get the pointer to native representation of this frame.
+   *
+   * @return The pointer to native representation of this frame.
+   */
+  public long getFramePtr() {
+    return m_framePtr;
+  }
+
+  /**
+   * Get a ByteBuffer pointing to the frame data. This ByteBuffer is backed by the frame directly.
+   * Its lifetime is controlled by the frame. If a new frame gets read, it will overwrite the
+   * current one.
+   *
+   * @return A ByteBuffer pointing to the frame data.
+   */
+  public ByteBuffer getDataByteBuffer() {
+    return m_dataByteBuffer;
+  }
+
+  /**
+   * Get a long (is a char* in native code) pointing to the frame data. This pointer is backed by
+   * the frame directly. Its lifetime is controlled by the frame. If a new frame gets read, it will
+   * overwrite the current one.
+   *
+   * @return A long pointing to the frame data.
+   */
+  public long getDataPtr() {
+    return m_dataPtr;
+  }
+
+  /**
+   * Get the total length of the data stored in the frame.
+   *
+   * @return The total length of the data stored in the frame.
+   */
+  public int getTotalData() {
+    return m_totalData;
+  }
+
+  /**
+   * Get the width of the frame.
+   *
+   * @return The width of the frame.
+   */
+  public int getWidth() {
+    return m_width;
+  }
+
+  /**
+   * Set the width of the frame.
+   *
+   * @param width The width of the frame.
+   */
+  public void setWidth(int width) {
+    this.m_width = width;
+  }
+
+  /**
+   * Get the height of the frame.
+   *
+   * @return The height of the frame.
+   */
+  public int getHeight() {
+    return m_height;
+  }
+
+  /**
+   * Set the height of the frame.
+   *
+   * @param height The height of the frame.
+   */
+  public void setHeight(int height) {
+    this.m_height = height;
+  }
+
+  /**
+   * Get the PixelFormat of the frame.
+   *
+   * @return The PixelFormat of the frame.
+   */
+  public int getPixelFormat() {
+    return m_pixelFormat;
+  }
+
+  /**
+   * Set the PixelFormat of the frame.
+   *
+   * @param pixelFormat The PixelFormat of the frame.
+   */
+  public void setPixelFormat(int pixelFormat) {
+    this.m_pixelFormat = pixelFormat;
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java
new file mode 100644
index 0000000..ae5b0ef
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSink.java
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore.raw;
+
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.cscore.ImageSink;
+
+/**
+ * A sink for user code to accept video frames as raw bytes.
+ *
+ * <p>This is a complex API, most cases should use CvSink.
+ */
+public class RawSink extends ImageSink {
+  /**
+   * Create a sink for accepting raw images.
+   *
+   * <p>grabFrame() must be called on the created sink to get each new image.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   */
+  public RawSink(String name) {
+    super(CameraServerJNI.createRawSink(name));
+  }
+
+  /**
+   * Wait for the next frame and get the image. Times out (returning 0) after 0.225 seconds. The
+   * provided image will have three 8-bit channels stored in BGR order.
+   *
+   * @param frame The frame object in which to store the image.
+   * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
+   *     is in the same time base as wpi::Now(), and is in 1 us increments.
+   */
+  protected long grabFrame(RawFrame frame) {
+    return grabFrame(frame, 0.225);
+  }
+
+  /**
+   * Wait for the next frame and get the image. Times out (returning 0) after timeout seconds. The
+   * provided image will have three 8-bit channels stored in BGR order.
+   *
+   * @param frame The frame object in which to store the image.
+   * @param timeout The frame timeout in seconds.
+   * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
+   *     is in the same time base as wpi::Now(), and is in 1 us increments.
+   */
+  protected long grabFrame(RawFrame frame, double timeout) {
+    return CameraServerJNI.grabSinkFrameTimeout(m_handle, frame, timeout);
+  }
+
+  /**
+   * Wait for the next frame and get the image. May block forever. The provided image will have
+   * three 8-bit channels stored in BGR order.
+   *
+   * @param frame The frame object in which to store the image.
+   * @return Frame time, or 0 on error (call getError() to obtain the error message); the frame time
+   *     is in the same time base as wpi::Now(), and is in 1 us increments.
+   */
+  protected long grabFrameNoTimeout(RawFrame frame) {
+    return CameraServerJNI.grabSinkFrame(m_handle, frame);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java
new file mode 100644
index 0000000..f1be050
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/java/edu/wpi/first/cscore/raw/RawSource.java
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore.raw;
+
+import edu.wpi.first.cscore.CameraServerJNI;
+import edu.wpi.first.cscore.ImageSource;
+import edu.wpi.first.cscore.VideoMode;
+
+/**
+ * A source for user code to provide video frames as raw bytes.
+ *
+ * <p>This is a complex API, most cases should use CvSource.
+ */
+public class RawSource extends ImageSource {
+  /**
+   * Create a raw frame source.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param mode Video mode being generated
+   */
+  public RawSource(String name, VideoMode mode) {
+    super(
+        CameraServerJNI.createRawSource(
+            name, mode.pixelFormat.getValue(), mode.width, mode.height, mode.fps));
+  }
+
+  /**
+   * Create a raw frame source.
+   *
+   * @param name Source name (arbitrary unique identifier)
+   * @param pixelFormat Pixel format
+   * @param width width
+   * @param height height
+   * @param fps fps
+   */
+  public RawSource(String name, VideoMode.PixelFormat pixelFormat, int width, int height, int fps) {
+    super(CameraServerJNI.createRawSource(name, pixelFormat.getValue(), width, height, fps));
+  }
+
+  /**
+   * Put a raw image and notify sinks.
+   *
+   * @param image raw frame image
+   */
+  protected void putFrame(RawFrame image) {
+    CameraServerJNI.putRawSourceFrame(m_handle, image);
+  }
+
+  /**
+   * Put a raw image and notify sinks.
+   *
+   * @param data raw frame data pointer
+   * @param width frame width
+   * @param height frame height
+   * @param pixelFormat pixel format
+   * @param totalData length of data in total
+   */
+  protected void putFrame(long data, int width, int height, int pixelFormat, int totalData) {
+    CameraServerJNI.putRawSourceFrame(m_handle, data, width, height, pixelFormat, totalData);
+  }
+
+  /**
+   * Put a raw image and notify sinks.
+   *
+   * @param data raw frame data pointer
+   * @param width frame width
+   * @param height frame height
+   * @param pixelFormat pixel format
+   * @param totalData length of data in total
+   */
+  protected void putFrame(
+      long data, int width, int height, VideoMode.PixelFormat pixelFormat, int totalData) {
+    CameraServerJNI.putRawSourceFrame(
+        m_handle, data, width, height, pixelFormat.getValue(), totalData);
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp
index 3ba1784..16452f1 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ConfigurableSourceImpl.h"
 
@@ -16,7 +13,7 @@
 
 using namespace cs;
 
-ConfigurableSourceImpl::ConfigurableSourceImpl(const wpi::Twine& name,
+ConfigurableSourceImpl::ConfigurableSourceImpl(std::string_view name,
                                                wpi::Logger& logger,
                                                Notifier& notifier,
                                                Telemetry& telemetry,
@@ -26,7 +23,7 @@
   m_videoModes.push_back(m_mode);
 }
 
-ConfigurableSourceImpl::~ConfigurableSourceImpl() {}
+ConfigurableSourceImpl::~ConfigurableSourceImpl() = default;
 
 void ConfigurableSourceImpl::Start() {
   m_notifier.NotifySource(*this, CS_SOURCE_CONNECTED);
@@ -53,11 +50,11 @@
   // ignore
 }
 
-void ConfigurableSourceImpl::NotifyError(const wpi::Twine& msg) {
+void ConfigurableSourceImpl::NotifyError(std::string_view msg) {
   PutError(msg, wpi::Now());
 }
 
-int ConfigurableSourceImpl::CreateProperty(const wpi::Twine& name,
+int ConfigurableSourceImpl::CreateProperty(std::string_view name,
                                            CS_PropertyKind kind, int minimum,
                                            int maximum, int step,
                                            int defaultValue, int value) {
@@ -78,12 +75,12 @@
         value = prop.value;
       });
   m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CREATED, name, ndx,
-                                  kind, value, wpi::Twine{});
+                                  kind, value, {});
   return ndx;
 }
 
 int ConfigurableSourceImpl::CreateProperty(
-    const wpi::Twine& name, CS_PropertyKind kind, int minimum, int maximum,
+    std::string_view name, CS_PropertyKind kind, int minimum, int maximum,
     int step, int defaultValue, int value,
     std::function<void(CS_Property property)> onChange) {
   // TODO
@@ -91,7 +88,7 @@
 }
 
 void ConfigurableSourceImpl::SetEnumPropertyChoices(
-    int property, wpi::ArrayRef<std::string> choices, CS_Status* status) {
+    int property, wpi::span<const std::string> choices, CS_Status* status) {
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
@@ -102,8 +99,8 @@
     *status = CS_WRONG_PROPERTY_TYPE;
     return;
   }
-  prop->enumChoices = choices;
+  prop->enumChoices.assign(choices.begin(), choices.end());
   m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CHOICES_UPDATED,
                                   prop->name, property, CS_PROP_ENUM,
-                                  prop->value, wpi::Twine{});
+                                  prop->value, {});
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.h
index a10f27e..bb9f3ed 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/ConfigurableSourceImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CONFIGURABLESOURCEIMPL_H_
 #define CSCORE_CONFIGURABLESOURCEIMPL_H_
@@ -12,10 +9,10 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
 #include "SourceImpl.h"
 
@@ -23,7 +20,7 @@
 
 class ConfigurableSourceImpl : public SourceImpl {
  protected:
-  ConfigurableSourceImpl(const wpi::Twine& name, wpi::Logger& logger,
+  ConfigurableSourceImpl(std::string_view name, wpi::Logger& logger,
                          Notifier& notifier, Telemetry& telemetry,
                          const VideoMode& mode);
 
@@ -38,13 +35,14 @@
   void NumSinksEnabledChanged() override;
 
   // OpenCV-specific functions
-  void NotifyError(const wpi::Twine& msg);
-  int CreateProperty(const wpi::Twine& name, CS_PropertyKind kind, int minimum,
+  void NotifyError(std::string_view msg);
+  int CreateProperty(std::string_view name, CS_PropertyKind kind, int minimum,
                      int maximum, int step, int defaultValue, int value);
-  int CreateProperty(const wpi::Twine& name, CS_PropertyKind kind, int minimum,
+  int CreateProperty(std::string_view name, CS_PropertyKind kind, int minimum,
                      int maximum, int step, int defaultValue, int value,
                      std::function<void(CS_Property property)> onChange);
-  void SetEnumPropertyChoices(int property, wpi::ArrayRef<std::string> choices,
+  void SetEnumPropertyChoices(int property,
+                              wpi::span<const std::string> choices,
                               CS_Status* status);
 
  private:
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp
index 7ba505a..dbda6c6 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CvSinkImpl.h"
 
@@ -21,28 +18,34 @@
 
 using namespace cs;
 
-CvSinkImpl::CvSinkImpl(const wpi::Twine& name, wpi::Logger& logger,
+CvSinkImpl::CvSinkImpl(std::string_view name, wpi::Logger& logger,
                        Notifier& notifier, Telemetry& telemetry)
     : SinkImpl{name, logger, notifier, telemetry} {
   m_active = true;
   // m_thread = std::thread(&CvSinkImpl::ThreadMain, this);
 }
 
-CvSinkImpl::CvSinkImpl(const wpi::Twine& name, wpi::Logger& logger,
+CvSinkImpl::CvSinkImpl(std::string_view name, wpi::Logger& logger,
                        Notifier& notifier, Telemetry& telemetry,
                        std::function<void(uint64_t time)> processFrame)
     : SinkImpl{name, logger, notifier, telemetry} {}
 
-CvSinkImpl::~CvSinkImpl() { Stop(); }
+CvSinkImpl::~CvSinkImpl() {
+  Stop();
+}
 
 void CvSinkImpl::Stop() {
   m_active = false;
 
   // wake up any waiters by forcing an empty frame to be sent
-  if (auto source = GetSource()) source->Wakeup();
+  if (auto source = GetSource()) {
+    source->Wakeup();
+  }
 
   // join thread
-  if (m_thread.joinable()) m_thread.join();
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
 }
 
 uint64_t CvSinkImpl::GrabFrame(cv::Mat& image) {
@@ -107,9 +110,11 @@
       std::this_thread::sleep_for(std::chrono::seconds(1));
       continue;
     }
-    SDEBUG4("waiting for frame");
+    SDEBUG4("{}", "waiting for frame");
     Frame frame = source->GetNextFrame();  // blocks
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
     if (!frame) {
       // Bad frame; sleep for 10 ms so we don't consume all processor time.
       std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -122,14 +127,14 @@
 
 namespace cs {
 
-CS_Sink CreateCvSink(const wpi::Twine& name, CS_Status* status) {
+CS_Sink CreateCvSink(std::string_view name, CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSink(
       CS_SINK_CV, std::make_shared<CvSinkImpl>(name, inst.logger, inst.notifier,
                                                inst.telemetry));
 }
 
-CS_Sink CreateCvSinkCallback(const wpi::Twine& name,
+CS_Sink CreateCvSinkCallback(std::string_view name,
                              std::function<void(uint64_t time)> processFrame,
                              CS_Status* status) {
   auto& inst = Instance::GetInstance();
@@ -140,7 +145,7 @@
 
 static constexpr unsigned SinkMask = CS_SINK_CV | CS_SINK_RAW;
 
-void SetSinkDescription(CS_Sink sink, const wpi::Twine& description,
+void SetSinkDescription(CS_Sink sink, std::string_view description,
                         CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data || (data->kind & SinkMask) == 0) {
@@ -178,12 +183,12 @@
   return static_cast<CvSinkImpl&>(*data->sink).GetError();
 }
 
-wpi::StringRef GetSinkError(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
-                            CS_Status* status) {
+std::string_view GetSinkError(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
+                              CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data || (data->kind & SinkMask) == 0) {
     *status = CS_INVALID_HANDLE;
-    return wpi::StringRef{};
+    return {};
   }
   return static_cast<CvSinkImpl&>(*data->sink).GetError(buf);
 }
@@ -243,7 +248,9 @@
 char* CS_GetSinkError(CS_Sink sink, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetSinkError(sink, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h
index 9b7820f..ad63a20 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/CvSinkImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CVSINKIMPL_H_
 #define CSCORE_CVSINKIMPL_H_
@@ -12,10 +9,10 @@
 
 #include <atomic>
 #include <functional>
+#include <string_view>
 #include <thread>
 
 #include <opencv2/core/core.hpp>
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 
 #include "Frame.h"
@@ -27,9 +24,9 @@
 
 class CvSinkImpl : public SinkImpl {
  public:
-  CvSinkImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
              Telemetry& telemetry);
-  CvSinkImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  CvSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
              Telemetry& telemetry,
              std::function<void(uint64_t time)> processFrame);
   ~CvSinkImpl() override;
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.cpp
index 49b9d28..7cd4fe0 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CvSourceImpl.h"
 
 #include <opencv2/core/core.hpp>
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
-#include <wpi/STLExtras.h>
 #include <wpi/timestamp.h>
 
 #include "Handle.h"
@@ -22,20 +18,21 @@
 
 using namespace cs;
 
-CvSourceImpl::CvSourceImpl(const wpi::Twine& name, wpi::Logger& logger,
+CvSourceImpl::CvSourceImpl(std::string_view name, wpi::Logger& logger,
                            Notifier& notifier, Telemetry& telemetry,
                            const VideoMode& mode)
     : ConfigurableSourceImpl{name, logger, notifier, telemetry, mode} {}
 
-CvSourceImpl::~CvSourceImpl() {}
+CvSourceImpl::~CvSourceImpl() = default;
 
 void CvSourceImpl::PutFrame(cv::Mat& image) {
   // We only support 8-bit images; convert if necessary.
   cv::Mat finalImage;
-  if (image.depth() == CV_8U)
+  if (image.depth() == CV_8U) {
     finalImage = image;
-  else
+  } else {
     image.convertTo(finalImage, CV_8U);
+  }
 
   std::unique_ptr<Image> dest;
   switch (image.channels()) {
@@ -55,8 +52,7 @@
       cv::cvtColor(finalImage, dest->AsMat(), cv::COLOR_BGRA2BGR);
       break;
     default:
-      SERROR("PutFrame: " << image.channels()
-                          << "-channel images not supported");
+      SERROR("PutFrame: {}-channel images not supported", image.channels());
       return;
   }
   SourceImpl::PutFrame(std::move(dest), wpi::Now());
@@ -64,7 +60,7 @@
 
 namespace cs {
 
-CS_Source CreateCvSource(const wpi::Twine& name, const VideoMode& mode,
+CS_Source CreateCvSource(std::string_view name, const VideoMode& mode,
                          CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSource(CS_SOURCE_CV, std::make_shared<CvSourceImpl>(
@@ -83,7 +79,7 @@
 
 static constexpr unsigned SourceMask = CS_SINK_CV | CS_SINK_RAW;
 
-void NotifySourceError(CS_Source source, const wpi::Twine& msg,
+void NotifySourceError(CS_Source source, std::string_view msg,
                        CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || (data->kind & SourceMask) == 0) {
@@ -102,7 +98,7 @@
   static_cast<CvSourceImpl&>(*data->source).SetConnected(connected);
 }
 
-void SetSourceDescription(CS_Source source, const wpi::Twine& description,
+void SetSourceDescription(CS_Source source, std::string_view description,
                           CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || (data->kind & SourceMask) == 0) {
@@ -112,7 +108,7 @@
   static_cast<CvSourceImpl&>(*data->source).SetDescription(description);
 }
 
-CS_Property CreateSourceProperty(CS_Source source, const wpi::Twine& name,
+CS_Property CreateSourceProperty(CS_Source source, std::string_view name,
                                  CS_PropertyKind kind, int minimum, int maximum,
                                  int step, int defaultValue, int value,
                                  CS_Status* status) {
@@ -128,7 +124,7 @@
 }
 
 CS_Property CreateSourcePropertyCallback(
-    CS_Source source, const wpi::Twine& name, CS_PropertyKind kind, int minimum,
+    CS_Source source, std::string_view name, CS_PropertyKind kind, int minimum,
     int maximum, int step, int defaultValue, int value,
     std::function<void(CS_Property property)> onChange, CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
@@ -143,7 +139,7 @@
 }
 
 void SetSourceEnumPropertyChoices(CS_Source source, CS_Property property,
-                                  wpi::ArrayRef<std::string> choices,
+                                  wpi::span<const std::string> choices,
                                   CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || (data->kind & SourceMask) == 0) {
@@ -227,7 +223,9 @@
                                      CS_Status* status) {
   wpi::SmallVector<std::string, 8> vec;
   vec.reserve(count);
-  for (int i = 0; i < count; ++i) vec.push_back(choices[i]);
+  for (int i = 0; i < count; ++i) {
+    vec.push_back(choices[i]);
+  }
   return cs::SetSourceEnumPropertyChoices(source, property, vec, status);
 }
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.h
index 978d012..fba7131 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/CvSourceImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CVSOURCEIMPL_H_
 #define CSCORE_CVSOURCEIMPL_H_
@@ -12,11 +9,10 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include <opencv2/core/core.hpp>
-#include <wpi/ArrayRef.h>
-#include <wpi/Twine.h>
 
 #include "ConfigurableSourceImpl.h"
 #include "SourceImpl.h"
@@ -25,7 +21,7 @@
 
 class CvSourceImpl : public ConfigurableSourceImpl {
  public:
-  CvSourceImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  CvSourceImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
                Telemetry& telemetry, const VideoMode& mode);
   ~CvSourceImpl() override;
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp
index 466cb52..0aa1947 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Frame.h"
 
@@ -19,10 +16,10 @@
 
 using namespace cs;
 
-Frame::Frame(SourceImpl& source, const wpi::Twine& error, Time time)
+Frame::Frame(SourceImpl& source, std::string_view error, Time time)
     : m_impl{source.AllocFrameImpl().release()} {
   m_impl->refcount = 1;
-  m_impl->error = error.str();
+  m_impl->error = error;
   m_impl->time = time;
 }
 
@@ -35,22 +32,31 @@
 }
 
 Image* Frame::GetNearestImage(int width, int height) const {
-  if (!m_impl) return nullptr;
+  if (!m_impl) {
+    return nullptr;
+  }
   std::scoped_lock lock(m_impl->mutex);
   Image* found = nullptr;
 
   // Ideally we want the smallest image at least width/height in size
   for (auto i : m_impl->images) {
-    if (i->IsLarger(width, height) && (!found || (i->IsSmaller(*found))))
+    if (i->IsLarger(width, height) && (!found || (i->IsSmaller(*found)))) {
       found = i;
+    }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // Find the largest image (will be less than width/height)
   for (auto i : m_impl->images) {
-    if (!found || (i->IsLarger(*found))) found = i;
+    if (!found || (i->IsLarger(*found))) {
+      found = i;
+    }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // Shouldn't reach this, but just in case...
   return m_impl->images.empty() ? nullptr : m_impl->images[0];
@@ -59,7 +65,9 @@
 Image* Frame::GetNearestImage(int width, int height,
                               VideoMode::PixelFormat pixelFormat,
                               int jpegQuality) const {
-  if (!m_impl) return nullptr;
+  if (!m_impl) {
+    return nullptr;
+  }
   std::scoped_lock lock(m_impl->mutex);
   Image* found = nullptr;
 
@@ -74,19 +82,25 @@
   // 1) Same width, height, pixelFormat, and (possibly) JPEG quality
   //    (e.g. exactly what we want)
   for (auto i : m_impl->images) {
-    if (i->Is(width, height, pixelFormat, jpegQuality)) return i;
+    if (i->Is(width, height, pixelFormat, jpegQuality)) {
+      return i;
+    }
   }
 
   // 2) Same width, height, different (but non-JPEG) pixelFormat (color conv)
   // 2a) If we want JPEG output, prefer BGR over other pixel formats
   if (pixelFormat == VideoMode::kMJPEG) {
     for (auto i : m_impl->images) {
-      if (i->Is(width, height, VideoMode::kBGR)) return i;
+      if (i->Is(width, height, VideoMode::kBGR)) {
+        return i;
+      }
     }
   }
 
   for (auto i : m_impl->images) {
-    if (i->Is(width, height) && i->pixelFormat != VideoMode::kMJPEG) return i;
+    if (i->Is(width, height) && i->pixelFormat != VideoMode::kMJPEG) {
+      return i;
+    }
   }
 
   // 3) Different width, height, same pixelFormat (only if non-JPEG) (resample)
@@ -94,17 +108,23 @@
     // 3a) Smallest image at least width/height in size
     for (auto i : m_impl->images) {
       if (i->IsLarger(width, height) && i->pixelFormat == pixelFormat &&
-          (!found || (i->IsSmaller(*found))))
+          (!found || (i->IsSmaller(*found)))) {
         found = i;
+      }
     }
-    if (found) return found;
+    if (found) {
+      return found;
+    }
 
     // 3b) Largest image (less than width/height)
     for (auto i : m_impl->images) {
-      if (i->pixelFormat == pixelFormat && (!found || (i->IsLarger(*found))))
+      if (i->pixelFormat == pixelFormat && (!found || (i->IsLarger(*found)))) {
         found = i;
+      }
     }
-    if (found) return found;
+    if (found) {
+      return found;
+    }
   }
 
   // 4) Different width, height, different (but non-JPEG) pixelFormat
@@ -112,18 +132,24 @@
   // 4a) Smallest image at least width/height in size
   for (auto i : m_impl->images) {
     if (i->IsLarger(width, height) && i->pixelFormat != VideoMode::kMJPEG &&
-        (!found || (i->IsSmaller(*found))))
+        (!found || (i->IsSmaller(*found)))) {
       found = i;
+    }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // 4b) Largest image (less than width/height)
   for (auto i : m_impl->images) {
     if (i->pixelFormat != VideoMode::kMJPEG &&
-        (!found || (i->IsLarger(*found))))
+        (!found || (i->IsLarger(*found)))) {
       found = i;
+    }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // 5) Same width, height, JPEG pixelFormat (decompression).  As there may be
   //    multiple JPEG images, find the highest quality one.
@@ -133,27 +159,37 @@
       found = i;
       // consider one without a quality setting to be the highest quality
       // (e.g. directly from the camera)
-      if (i->jpegQuality == -1) break;
+      if (i->jpegQuality == -1) {
+        break;
+      }
     }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // 6) Different width, height, JPEG pixelFormat (decompression)
   // 6a) Smallest image at least width/height in size
   for (auto i : m_impl->images) {
     if (i->IsLarger(width, height) && i->pixelFormat == VideoMode::kMJPEG &&
-        (!found || (i->IsSmaller(*found))))
+        (!found || (i->IsSmaller(*found)))) {
       found = i;
+    }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // 6b) Largest image (less than width/height)
   for (auto i : m_impl->images) {
     if (i->pixelFormat != VideoMode::kMJPEG &&
-        (!found || (i->IsLarger(*found))))
+        (!found || (i->IsLarger(*found)))) {
       found = i;
+    }
   }
-  if (found) return found;
+  if (found) {
+    return found;
+  }
 
   // Shouldn't reach this, but just in case...
   return m_impl->images.empty() ? nullptr : m_impl->images[0];
@@ -161,9 +197,10 @@
 
 Image* Frame::ConvertImpl(Image* image, VideoMode::PixelFormat pixelFormat,
                           int requiredJpegQuality, int defaultJpegQuality) {
-  if (!image ||
-      image->Is(image->width, image->height, pixelFormat, requiredJpegQuality))
+  if (!image || image->Is(image->width, image->height, pixelFormat,
+                          requiredJpegQuality)) {
     return image;
+  }
   Image* cur = image;
 
   // If the source image is a JPEG, we need to decode it before we can do
@@ -172,7 +209,9 @@
   // would have returned above).
   if (cur->pixelFormat == VideoMode::kMJPEG) {
     cur = ConvertMJPEGToBGR(cur);
-    if (pixelFormat == VideoMode::kBGR) return cur;
+    if (pixelFormat == VideoMode::kBGR) {
+      return cur;
+    }
   }
 
   // Color convert
@@ -182,17 +221,19 @@
       if (cur->pixelFormat == VideoMode::kYUYV) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
-                GetExistingImage(cur->width, cur->height, VideoMode::kBGR))
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
           cur = newImage;
-        else
+        } else {
           cur = ConvertYUYVToBGR(cur);
+        }
       } else if (cur->pixelFormat == VideoMode::kGray) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
-                GetExistingImage(cur->width, cur->height, VideoMode::kBGR))
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
           cur = newImage;
-        else
+        } else {
           cur = ConvertGrayToBGR(cur);
+        }
       }
       return ConvertBGRToRGB565(cur);
     case VideoMode::kGray:
@@ -200,17 +241,19 @@
       if (cur->pixelFormat == VideoMode::kYUYV) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
-                GetExistingImage(cur->width, cur->height, VideoMode::kBGR))
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
           cur = newImage;
-        else
+        } else {
           cur = ConvertYUYVToBGR(cur);
+        }
       } else if (cur->pixelFormat == VideoMode::kRGB565) {
         // Check to see if BGR version already exists...
         if (Image* newImage =
-                GetExistingImage(cur->width, cur->height, VideoMode::kBGR))
+                GetExistingImage(cur->width, cur->height, VideoMode::kBGR)) {
           cur = newImage;
-        else
+        } else {
           cur = ConvertRGB565ToBGR(cur);
+        }
       }
       return ConvertBGRToGray(cur);
     case VideoMode::kBGR:
@@ -220,10 +263,11 @@
       } else if (cur->pixelFormat == VideoMode::kRGB565) {
         cur = ConvertRGB565ToBGR(cur);
       } else if (cur->pixelFormat == VideoMode::kGray) {
-        if (pixelFormat == VideoMode::kBGR)
+        if (pixelFormat == VideoMode::kBGR) {
           return ConvertGrayToBGR(cur);
-        else
+        } else {
           return ConvertGrayToMJPEG(cur, defaultJpegQuality);
+        }
       }
       break;
     case VideoMode::kYUYV:
@@ -232,14 +276,17 @@
   }
 
   // Compress if destination is JPEG
-  if (pixelFormat == VideoMode::kMJPEG)
+  if (pixelFormat == VideoMode::kMJPEG) {
     cur = ConvertBGRToMJPEG(cur, defaultJpegQuality);
+  }
 
   return cur;
 }
 
 Image* Frame::ConvertMJPEGToBGR(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kMJPEG) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kMJPEG) {
+    return nullptr;
+  }
 
   // Allocate an BGR image
   auto newImage =
@@ -260,7 +307,9 @@
 }
 
 Image* Frame::ConvertMJPEGToGray(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kMJPEG) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kMJPEG) {
+    return nullptr;
+  }
 
   // Allocate an grayscale image
   auto newImage =
@@ -281,7 +330,9 @@
 }
 
 Image* Frame::ConvertYUYVToBGR(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kYUYV) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kYUYV) {
+    return nullptr;
+  }
 
   // Allocate a BGR image
   auto newImage =
@@ -301,7 +352,9 @@
 }
 
 Image* Frame::ConvertBGRToRGB565(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kBGR) {
+    return nullptr;
+  }
 
   // Allocate a RGB565 image
   auto newImage =
@@ -321,7 +374,9 @@
 }
 
 Image* Frame::ConvertRGB565ToBGR(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kRGB565) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kRGB565) {
+    return nullptr;
+  }
 
   // Allocate a BGR image
   auto newImage =
@@ -341,7 +396,9 @@
 }
 
 Image* Frame::ConvertBGRToGray(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kBGR) {
+    return nullptr;
+  }
 
   // Allocate a Grayscale image
   auto newImage =
@@ -361,7 +418,9 @@
 }
 
 Image* Frame::ConvertGrayToBGR(Image* image) {
-  if (!image || image->pixelFormat != VideoMode::kGray) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kGray) {
+    return nullptr;
+  }
 
   // Allocate a BGR image
   auto newImage =
@@ -381,8 +440,12 @@
 }
 
 Image* Frame::ConvertBGRToMJPEG(Image* image, int quality) {
-  if (!image || image->pixelFormat != VideoMode::kBGR) return nullptr;
-  if (!m_impl) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kBGR) {
+    return nullptr;
+  }
+  if (!m_impl) {
+    return nullptr;
+  }
   std::scoped_lock lock(m_impl->mutex);
 
   // Allocate a JPEG image.  We don't actually know what the resulting size
@@ -412,8 +475,12 @@
 }
 
 Image* Frame::ConvertGrayToMJPEG(Image* image, int quality) {
-  if (!image || image->pixelFormat != VideoMode::kGray) return nullptr;
-  if (!m_impl) return nullptr;
+  if (!image || image->pixelFormat != VideoMode::kGray) {
+    return nullptr;
+  }
+  if (!m_impl) {
+    return nullptr;
+  }
   std::scoped_lock lock(m_impl->mutex);
 
   // Allocate a JPEG image.  We don't actually know what the resulting size
@@ -445,23 +512,26 @@
 Image* Frame::GetImageImpl(int width, int height,
                            VideoMode::PixelFormat pixelFormat,
                            int requiredJpegQuality, int defaultJpegQuality) {
-  if (!m_impl) return nullptr;
+  if (!m_impl) {
+    return nullptr;
+  }
   std::scoped_lock lock(m_impl->mutex);
   Image* cur = GetNearestImage(width, height, pixelFormat, requiredJpegQuality);
-  if (!cur || cur->Is(width, height, pixelFormat, requiredJpegQuality))
+  if (!cur || cur->Is(width, height, pixelFormat, requiredJpegQuality)) {
     return cur;
+  }
 
   WPI_DEBUG4(Instance::GetInstance().logger,
-             "converting image from " << cur->width << "x" << cur->height
-                                      << " type " << cur->pixelFormat << " to "
-                                      << width << "x" << height << " type "
-                                      << pixelFormat);
+             "converting image from {}x{} type {} to {}x{} type {}", cur->width,
+             cur->height, cur->pixelFormat, width, height, pixelFormat);
 
   // If the source image is a JPEG, we need to decode it before we can do
   // anything else with it.  Note that if the destination format is JPEG, we
   // still need to do this (unless the width/height/compression were the same,
   // in which case we already returned the existing JPEG above).
-  if (cur->pixelFormat == VideoMode::kMJPEG) cur = ConvertMJPEGToBGR(cur);
+  if (cur->pixelFormat == VideoMode::kMJPEG) {
+    cur = ConvertMJPEGToBGR(cur);
+  }
 
   // Resize
   if (!cur->Is(width, height)) {
@@ -485,14 +555,17 @@
 
 bool Frame::GetCv(cv::Mat& image, int width, int height) {
   Image* rawImage = GetImage(width, height, VideoMode::kBGR);
-  if (!rawImage) return false;
+  if (!rawImage) {
+    return false;
+  }
   rawImage->AsMat().copyTo(image);
   return true;
 }
 
 void Frame::ReleaseFrame() {
-  for (auto image : m_impl->images)
+  for (auto image : m_impl->images) {
     m_impl->source.ReleaseImage(std::unique_ptr<Image>(image));
+  }
   m_impl->images.clear();
   m_impl->source.ReleaseFrameImpl(std::unique_ptr<Impl>(m_impl));
   m_impl = nullptr;
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h
index 07fa24f..9a16824 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Frame.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_FRAME_H_
 #define CSCORE_FRAME_H_
@@ -11,11 +8,11 @@
 #include <atomic>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
 #include <wpi/SmallVector.h>
-#include <wpi/Twine.h>
 #include <wpi/mutex.h>
 
 #include "Image.h"
@@ -45,14 +42,16 @@
   };
 
  public:
-  Frame() noexcept : m_impl{nullptr} {}
+  Frame() noexcept = default;
 
-  Frame(SourceImpl& source, const wpi::Twine& error, Time time);
+  Frame(SourceImpl& source, std::string_view error, Time time);
 
   Frame(SourceImpl& source, std::unique_ptr<Image> image, Time time);
 
   Frame(const Frame& frame) noexcept : m_impl{frame.m_impl} {
-    if (m_impl) ++m_impl->refcount;
+    if (m_impl) {
+      ++m_impl->refcount;
+    }
   }
 
   Frame(Frame&& other) noexcept : Frame() { swap(*this, other); }
@@ -73,61 +72,91 @@
 
   Time GetTime() const { return m_impl ? m_impl->time : 0; }
 
-  wpi::StringRef GetError() const {
-    if (!m_impl) return wpi::StringRef{};
+  std::string_view GetError() const {
+    if (!m_impl) {
+      return {};
+    }
     return m_impl->error;
   }
 
   int GetOriginalWidth() const {
-    if (!m_impl) return 0;
+    if (!m_impl) {
+      return 0;
+    }
     std::scoped_lock lock(m_impl->mutex);
-    if (m_impl->images.empty()) return 0;
+    if (m_impl->images.empty()) {
+      return 0;
+    }
     return m_impl->images[0]->width;
   }
 
   int GetOriginalHeight() const {
-    if (!m_impl) return 0;
+    if (!m_impl) {
+      return 0;
+    }
     std::scoped_lock lock(m_impl->mutex);
-    if (m_impl->images.empty()) return 0;
+    if (m_impl->images.empty()) {
+      return 0;
+    }
     return m_impl->images[0]->height;
   }
 
   int GetOriginalPixelFormat() const {
-    if (!m_impl) return 0;
+    if (!m_impl) {
+      return 0;
+    }
     std::scoped_lock lock(m_impl->mutex);
-    if (m_impl->images.empty()) return 0;
+    if (m_impl->images.empty()) {
+      return 0;
+    }
     return m_impl->images[0]->pixelFormat;
   }
 
   int GetOriginalJpegQuality() const {
-    if (!m_impl) return 0;
+    if (!m_impl) {
+      return 0;
+    }
     std::scoped_lock lock(m_impl->mutex);
-    if (m_impl->images.empty()) return 0;
+    if (m_impl->images.empty()) {
+      return 0;
+    }
     return m_impl->images[0]->jpegQuality;
   }
 
   Image* GetExistingImage(size_t i = 0) const {
-    if (!m_impl) return nullptr;
+    if (!m_impl) {
+      return nullptr;
+    }
     std::scoped_lock lock(m_impl->mutex);
-    if (i >= m_impl->images.size()) return nullptr;
+    if (i >= m_impl->images.size()) {
+      return nullptr;
+    }
     return m_impl->images[i];
   }
 
   Image* GetExistingImage(int width, int height) const {
-    if (!m_impl) return nullptr;
+    if (!m_impl) {
+      return nullptr;
+    }
     std::scoped_lock lock(m_impl->mutex);
     for (auto i : m_impl->images) {
-      if (i->Is(width, height)) return i;
+      if (i->Is(width, height)) {
+        return i;
+      }
     }
     return nullptr;
   }
 
   Image* GetExistingImage(int width, int height,
                           VideoMode::PixelFormat pixelFormat) const {
-    if (!m_impl) return nullptr;
+    if (!m_impl) {
+      return nullptr;
+    }
     std::scoped_lock lock(m_impl->mutex);
     for (auto i : m_impl->images) {
-      if (i->Is(width, height, pixelFormat)) return i;
+      if (i->Is(width, height, pixelFormat)) {
+        return i;
+      }
     }
     return nullptr;
   }
@@ -135,10 +164,14 @@
   Image* GetExistingImage(int width, int height,
                           VideoMode::PixelFormat pixelFormat,
                           int jpegQuality) const {
-    if (!m_impl) return nullptr;
+    if (!m_impl) {
+      return nullptr;
+    }
     std::scoped_lock lock(m_impl->mutex);
     for (auto i : m_impl->images) {
-      if (i->Is(width, height, pixelFormat, jpegQuality)) return i;
+      if (i->Is(width, height, pixelFormat, jpegQuality)) {
+        return i;
+      }
     }
     return nullptr;
   }
@@ -149,7 +182,9 @@
                          int jpegQuality = -1) const;
 
   Image* Convert(Image* image, VideoMode::PixelFormat pixelFormat) {
-    if (pixelFormat == VideoMode::kMJPEG) return nullptr;
+    if (pixelFormat == VideoMode::kMJPEG) {
+      return nullptr;
+    }
     return ConvertImpl(image, pixelFormat, -1, 80);
   }
   Image* ConvertToMJPEG(Image* image, int requiredQuality,
@@ -168,7 +203,9 @@
   Image* ConvertGrayToMJPEG(Image* image, int quality);
 
   Image* GetImage(int width, int height, VideoMode::PixelFormat pixelFormat) {
-    if (pixelFormat == VideoMode::kMJPEG) return nullptr;
+    if (pixelFormat == VideoMode::kMJPEG) {
+      return nullptr;
+    }
     return GetImageImpl(width, height, pixelFormat, -1, 80);
   }
   Image* GetImageMJPEG(int width, int height, int requiredQuality,
@@ -188,11 +225,13 @@
   Image* GetImageImpl(int width, int height, VideoMode::PixelFormat pixelFormat,
                       int requiredJpegQuality, int defaultJpegQuality);
   void DecRef() {
-    if (m_impl && --(m_impl->refcount) == 0) ReleaseFrame();
+    if (m_impl && --(m_impl->refcount) == 0) {
+      ReleaseFrame();
+    }
   }
   void ReleaseFrame();
 
-  Impl* m_impl;
+  Impl* m_impl{nullptr};
 };
 
 }  // namespace cs
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Handle.h b/third_party/allwpilib/cscore/src/main/native/cpp/Handle.h
index ebb3a55..91df0eb 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Handle.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Handle.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_HANDLE_H_
 #define CSCORE_HANDLE_H_
 
+#include <wpi/Synchronization.h>
+
 #include "cscore_c.h"
 
 namespace cs {
@@ -21,11 +20,12 @@
  public:
   enum Type {
     kUndefined = 0,
-    kProperty = 0x40,
+    kProperty = wpi::kHandleTypeCSBase,
     kSource,
     kSink,
     kListener,
-    kSinkProperty
+    kSinkProperty,
+    kListenerPoller
   };
   enum { kIndexMax = 0xffff };
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.cpp
index 57c86a8..068393b 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HttpCameraImpl.h"
 
 #include <wpi/MemAlloc.h>
+#include <wpi/StringExtras.h>
 #include <wpi/TCPConnector.h>
 #include <wpi/timestamp.h>
 
@@ -21,7 +19,7 @@
 
 using namespace cs;
 
-HttpCameraImpl::HttpCameraImpl(const wpi::Twine& name, CS_HttpCameraKind kind,
+HttpCameraImpl::HttpCameraImpl(std::string_view name, CS_HttpCameraKind kind,
                                wpi::Logger& logger, Notifier& notifier,
                                Telemetry& telemetry)
     : SourceImpl{name, logger, notifier, telemetry}, m_kind{kind} {}
@@ -33,26 +31,36 @@
   m_monitorCond.notify_one();
 
   // join monitor thread
-  if (m_monitorThread.joinable()) m_monitorThread.join();
+  if (m_monitorThread.joinable()) {
+    m_monitorThread.join();
+  }
 
   // Close file if it's open
   {
     std::scoped_lock lock(m_mutex);
-    if (m_streamConn) m_streamConn->stream->close();
-    if (m_settingsConn) m_settingsConn->stream->close();
+    if (m_streamConn) {
+      m_streamConn->stream->close();
+    }
+    if (m_settingsConn) {
+      m_settingsConn->stream->close();
+    }
   }
 
   // force wakeup of camera thread in case it's waiting on cv
   m_sinkEnabledCond.notify_one();
 
   // join camera thread
-  if (m_streamThread.joinable()) m_streamThread.join();
+  if (m_streamThread.joinable()) {
+    m_streamThread.join();
+  }
 
   // force wakeup of settings thread
   m_settingsCond.notify_one();
 
   // join settings thread
-  if (m_settingsThread.joinable()) m_settingsThread.join();
+  if (m_settingsThread.joinable()) {
+    m_settingsThread.join();
+  }
 }
 
 void HttpCameraImpl::Start() {
@@ -69,13 +77,15 @@
     m_monitorCond.wait_for(lock, std::chrono::seconds(1),
                            [=] { return !m_active; });
 
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
 
     // check to see if we got any frames, and close the stream if not
     // (this will result in an error at the read point, and ultimately
     // a reconnect attempt)
     if (m_streamConn && m_frameCount == 0) {
-      SWARNING("Monitor detected stream hung, disconnecting");
+      SWARNING("{}", "Monitor detected stream hung, disconnecting");
       m_streamConn->stream->close();
     }
 
@@ -83,7 +93,7 @@
     m_frameCount = 0;
   }
 
-  SDEBUG("Monitor Thread exiting");
+  SDEBUG("{}", "Monitor Thread exiting");
 }
 
 void HttpCameraImpl::StreamThreadMain() {
@@ -96,33 +106,41 @@
     // disconnect if not enabled
     if (!IsEnabled()) {
       std::unique_lock lock(m_mutex);
-      if (m_streamConn) m_streamConn->stream->close();
+      if (m_streamConn) {
+        m_streamConn->stream->close();
+      }
       // Wait for enable
       m_sinkEnabledCond.wait(lock, [=] { return !m_active || IsEnabled(); });
-      if (!m_active) return;
+      if (!m_active) {
+        return;
+      }
     }
 
     // connect
     wpi::SmallString<64> boundary;
     wpi::HttpConnection* conn = DeviceStreamConnect(boundary);
 
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
 
     // keep retrying
-    if (!conn) continue;
+    if (!conn) {
+      continue;
+    }
 
     // update connected since we're actually connected
     SetConnected(true);
 
     // stream
-    DeviceStream(conn->is, boundary);
+    DeviceStream(conn->is, boundary.str());
     {
       std::unique_lock lock(m_mutex);
       m_streamConn = nullptr;
     }
   }
 
-  SDEBUG("Camera Thread exiting");
+  SDEBUG("{}", "Camera Thread exiting");
   SetConnected(false);
 }
 
@@ -133,11 +151,13 @@
   {
     std::scoped_lock lock(m_mutex);
     if (m_locations.empty()) {
-      SERROR("locations array is empty!?");
+      SERROR("{}", "locations array is empty!?");
       std::this_thread::sleep_for(std::chrono::seconds(1));
       return nullptr;
     }
-    if (m_nextLocation >= m_locations.size()) m_nextLocation = 0;
+    if (m_nextLocation >= m_locations.size()) {
+      m_nextLocation = 0;
+    }
     req = wpi::HttpRequest{m_locations[m_nextLocation++], m_streamSettings};
     m_streamSettingsUpdated = false;
   }
@@ -146,7 +166,9 @@
   auto stream =
       wpi::TCPConnector::connect(req.host.c_str(), req.port, m_logger, 1);
 
-  if (!m_active || !stream) return nullptr;
+  if (!m_active || !stream) {
+    return nullptr;
+  }
 
   auto connPtr = std::make_unique<wpi::HttpConnection>(std::move(stream), 1);
   wpi::HttpConnection* conn = connPtr.get();
@@ -160,19 +182,17 @@
 
   std::string warn;
   if (!conn->Handshake(req, &warn)) {
-    SWARNING(GetName() << ": " << warn);
+    SWARNING("{}", warn);
     std::scoped_lock lock(m_mutex);
     m_streamConn = nullptr;
     return nullptr;
   }
 
   // Parse Content-Type header to get the boundary
-  wpi::StringRef mediaType, contentType;
-  std::tie(mediaType, contentType) = conn->contentType.str().split(';');
-  mediaType = mediaType.trim();
+  auto [mediaType, contentType] = wpi::split(conn->contentType.str(), ';');
+  mediaType = wpi::trim(mediaType);
   if (mediaType != "multipart/x-mixed-replace") {
-    SWARNING("\"" << req.host << "\": unrecognized Content-Type \"" << mediaType
-                  << "\"");
+    SWARNING("\"{}\": unrecognized Content-Type \"{}\"", req.host, mediaType);
     std::scoped_lock lock(m_mutex);
     m_streamConn = nullptr;
     return nullptr;
@@ -181,14 +201,13 @@
   // media parameters
   boundary.clear();
   while (!contentType.empty()) {
-    wpi::StringRef keyvalue;
-    std::tie(keyvalue, contentType) = contentType.split(';');
-    contentType = contentType.ltrim();
-    wpi::StringRef key, value;
-    std::tie(key, value) = keyvalue.split('=');
-    if (key.trim() == "boundary") {
-      value = value.trim().trim('"');  // value may be quoted
-      if (value.startswith("--")) {
+    std::string_view keyvalue;
+    std::tie(keyvalue, contentType) = wpi::split(contentType, ';');
+    contentType = wpi::ltrim(contentType);
+    auto [key, value] = wpi::split(keyvalue, '=');
+    if (wpi::trim(key) == "boundary") {
+      value = wpi::trim(wpi::trim(value), '"');  // value may be quoted
+      if (wpi::starts_with(value, "--")) {
         value = value.substr(2);
       }
       boundary.append(value.begin(), value.end());
@@ -196,8 +215,7 @@
   }
 
   if (boundary.empty()) {
-    SWARNING("\"" << req.host
-                  << "\": empty multi-part boundary or no Content-Type");
+    SWARNING("\"{}\": empty multi-part boundary or no Content-Type", req.host);
     std::scoped_lock lock(m_mutex);
     m_streamConn = nullptr;
     return nullptr;
@@ -207,7 +225,7 @@
 }
 
 void HttpCameraImpl::DeviceStream(wpi::raw_istream& is,
-                                  wpi::StringRef boundary) {
+                                  std::string_view boundary) {
   // Stored here so we reuse it from frame to frame
   std::string imageBuf;
 
@@ -218,24 +236,33 @@
   // streaming loop
   while (m_active && !is.has_error() && IsEnabled() && numErrors < 3 &&
          !m_streamSettingsUpdated) {
-    if (!FindMultipartBoundary(is, boundary, nullptr)) break;
+    if (!FindMultipartBoundary(is, boundary, nullptr)) {
+      break;
+    }
 
     // Read the next two characters after the boundary (normally \r\n)
     // Handle just \n for LabVIEW however
     char eol[2];
     is.read(eol, 1);
-    if (!m_active || is.has_error()) break;
+    if (!m_active || is.has_error()) {
+      break;
+    }
     if (eol[0] != '\n') {
       is.read(eol + 1, 1);
-      if (!m_active || is.has_error()) break;
+      if (!m_active || is.has_error()) {
+        break;
+      }
       // End-of-stream is indicated with trailing --
-      if (eol[0] == '-' && eol[1] == '-') break;
+      if (eol[0] == '-' && eol[1] == '-') {
+        break;
+      }
     }
 
-    if (!DeviceStreamFrame(is, imageBuf))
+    if (!DeviceStreamFrame(is, imageBuf)) {
       ++numErrors;
-    else
+    } else {
       numErrors = 0;
+    }
   }
 }
 
@@ -245,28 +272,29 @@
   wpi::SmallString<64> contentTypeBuf;
   wpi::SmallString<64> contentLengthBuf;
   if (!ParseHttpHeaders(is, &contentTypeBuf, &contentLengthBuf)) {
-    SWARNING("disconnected during headers");
+    SWARNING("{}", "disconnected during headers");
     PutError("disconnected during headers", wpi::Now());
     return false;
   }
 
   // Check the content type (if present)
   if (!contentTypeBuf.str().empty() &&
-      !contentTypeBuf.str().startswith("image/jpeg")) {
-    wpi::SmallString<64> errBuf;
-    wpi::raw_svector_ostream errMsg{errBuf};
-    errMsg << "received unknown Content-Type \"" << contentTypeBuf << "\"";
-    SWARNING(errMsg.str());
-    PutError(errMsg.str(), wpi::Now());
+      !wpi::starts_with(contentTypeBuf, "image/jpeg")) {
+    auto errMsg =
+        fmt::format("received unknown Content-Type \"{}\"", contentTypeBuf);
+    SWARNING("{}", errMsg);
+    PutError(errMsg, wpi::Now());
     return false;
   }
 
   unsigned int contentLength = 0;
-  if (contentLengthBuf.str().getAsInteger(10, contentLength)) {
+  if (auto v = wpi::parse_integer<unsigned int>(contentLengthBuf, 10)) {
+    contentLength = v.value();
+  } else {
     // Ugh, no Content-Length?  Read the blocks of the JPEG file.
     int width, height;
     if (!ReadJpeg(is, imageBuf, &width, &height)) {
-      SWARNING("did not receive a JPEG image");
+      SWARNING("{}", "did not receive a JPEG image");
       PutError("did not receive a JPEG image", wpi::Now());
       return false;
     }
@@ -280,10 +308,12 @@
   // the data directly into it.
   auto image = AllocImage(VideoMode::PixelFormat::kMJPEG, 0, 0, contentLength);
   is.read(image->data(), contentLength);
-  if (!m_active || is.has_error()) return false;
+  if (!m_active || is.has_error()) {
+    return false;
+  }
   int width, height;
   if (!GetJpegSize(image->str(), &width, &height)) {
-    SWARNING("did not receive a JPEG image");
+    SWARNING("{}", "did not receive a JPEG image");
     PutError("did not receive a JPEG image", wpi::Now());
     return false;
   }
@@ -302,7 +332,9 @@
       m_settingsCond.wait(lock, [=] {
         return !m_active || (m_prefLocation != -1 && !m_settings.empty());
       });
-      if (!m_active) break;
+      if (!m_active) {
+        break;
+      }
 
       // Build the request
       req = wpi::HttpRequest{m_locations[m_prefLocation], m_settings};
@@ -311,7 +343,7 @@
     DeviceSendSettings(req);
   }
 
-  SDEBUG("Settings Thread exiting");
+  SDEBUG("{}", "Settings Thread exiting");
 }
 
 void HttpCameraImpl::DeviceSendSettings(wpi::HttpRequest& req) {
@@ -319,7 +351,9 @@
   auto stream =
       wpi::TCPConnector::connect(req.host.c_str(), req.port, m_logger, 1);
 
-  if (!m_active || !stream) return;
+  if (!m_active || !stream) {
+    return;
+  }
 
   auto connPtr = std::make_unique<wpi::HttpConnection>(std::move(stream), 1);
   wpi::HttpConnection* conn = connPtr.get();
@@ -332,7 +366,9 @@
 
   // Just need a handshake as settings are sent via GET parameters
   std::string warn;
-  if (!conn->Handshake(req, &warn)) SWARNING(GetName() << ": " << warn);
+  if (!conn->Handshake(req, &warn)) {
+    SWARNING("{}", warn);
+  }
 
   conn->stream->close();
 }
@@ -342,7 +378,7 @@
   return m_kind;
 }
 
-bool HttpCameraImpl::SetUrls(wpi::ArrayRef<std::string> urls,
+bool HttpCameraImpl::SetUrls(wpi::span<const std::string> urls,
                              CS_Status* status) {
   std::vector<wpi::HttpLocation> locations;
   for (const auto& url : urls) {
@@ -350,7 +386,7 @@
     std::string errorMsg;
     locations.emplace_back(url, &error, &errorMsg);
     if (error) {
-      SERROR(GetName() << ": " << errorMsg);
+      SERROR("{}", errorMsg);
       *status = CS_BAD_URL;
       return false;
     }
@@ -366,12 +402,14 @@
 std::vector<std::string> HttpCameraImpl::GetUrls() const {
   std::scoped_lock lock(m_mutex);
   std::vector<std::string> urls;
-  for (const auto& loc : m_locations) urls.push_back(loc.url);
+  for (const auto& loc : m_locations) {
+    urls.push_back(loc.url);
+  }
   return urls;
 }
 
-void HttpCameraImpl::CreateProperty(const wpi::Twine& name,
-                                    const wpi::Twine& httpParam,
+void HttpCameraImpl::CreateProperty(std::string_view name,
+                                    std::string_view httpParam,
                                     bool viaSettings, CS_PropertyKind kind,
                                     int minimum, int maximum, int step,
                                     int defaultValue, int value) const {
@@ -381,13 +419,12 @@
       value));
 
   m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CREATED, name,
-                                  m_propertyData.size() + 1, kind, value,
-                                  wpi::Twine{});
+                                  m_propertyData.size() + 1, kind, value, {});
 }
 
 template <typename T>
 void HttpCameraImpl::CreateEnumProperty(
-    const wpi::Twine& name, const wpi::Twine& httpParam, bool viaSettings,
+    std::string_view name, std::string_view httpParam, bool viaSettings,
     int defaultValue, int value, std::initializer_list<T> choices) const {
   std::scoped_lock lock(m_mutex);
   m_propertyData.emplace_back(std::make_unique<PropertyData>(
@@ -396,18 +433,20 @@
 
   auto& enumChoices = m_propertyData.back()->enumChoices;
   enumChoices.clear();
-  for (const auto& choice : choices) enumChoices.emplace_back(choice);
+  for (const auto& choice : choices) {
+    enumChoices.emplace_back(choice);
+  }
 
   m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CREATED, name,
                                   m_propertyData.size() + 1, CS_PROP_ENUM,
-                                  value, wpi::Twine{});
+                                  value, {});
   m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CHOICES_UPDATED,
                                   name, m_propertyData.size() + 1, CS_PROP_ENUM,
-                                  value, wpi::Twine{});
+                                  value, {});
 }
 
 std::unique_ptr<PropertyImpl> HttpCameraImpl::CreateEmptyProperty(
-    const wpi::Twine& name) const {
+    std::string_view name) const {
   return std::make_unique<PropertyData>(name);
 }
 
@@ -432,7 +471,7 @@
   // TODO
 }
 
-void HttpCameraImpl::SetStringProperty(int property, const wpi::Twine& value,
+void HttpCameraImpl::SetStringProperty(int property, std::string_view value,
                                        CS_Status* status) {
   // TODO
 }
@@ -471,7 +510,9 @@
 }
 
 bool HttpCameraImpl::SetVideoMode(const VideoMode& mode, CS_Status* status) {
-  if (mode.pixelFormat != VideoMode::kMJPEG) return false;
+  if (mode.pixelFormat != VideoMode::kMJPEG) {
+    return false;
+  }
   std::scoped_lock lock(m_mutex);
   m_mode = mode;
   m_streamSettingsUpdated = true;
@@ -516,7 +557,7 @@
 
 namespace cs {
 
-CS_Source CreateHttpCamera(const wpi::Twine& name, const wpi::Twine& url,
+CS_Source CreateHttpCamera(std::string_view name, std::string_view url,
                            CS_HttpCameraKind kind, CS_Status* status) {
   auto& inst = Instance::GetInstance();
   std::shared_ptr<HttpCameraImpl> source;
@@ -530,12 +571,15 @@
                                                 inst.notifier, inst.telemetry);
       break;
   }
-  if (!source->SetUrls(url.str(), status)) return 0;
+  std::string urlStr{url};
+  if (!source->SetUrls(wpi::span{&urlStr, 1}, status)) {
+    return 0;
+  }
   return inst.CreateSource(CS_SOURCE_HTTP, source);
 }
 
-CS_Source CreateHttpCamera(const wpi::Twine& name,
-                           wpi::ArrayRef<std::string> urls,
+CS_Source CreateHttpCamera(std::string_view name,
+                           wpi::span<const std::string> urls,
                            CS_HttpCameraKind kind, CS_Status* status) {
   auto& inst = Instance::GetInstance();
   if (urls.empty()) {
@@ -544,7 +588,9 @@
   }
   auto source = std::make_shared<HttpCameraImpl>(name, kind, inst.logger,
                                                  inst.notifier, inst.telemetry);
-  if (!source->SetUrls(urls, status)) return 0;
+  if (!source->SetUrls(urls, status)) {
+    return 0;
+  }
   return inst.CreateSource(CS_SOURCE_HTTP, source);
 }
 
@@ -557,7 +603,7 @@
   return static_cast<HttpCameraImpl&>(*data->source).GetKind();
 }
 
-void SetHttpCameraUrls(CS_Source source, wpi::ArrayRef<std::string> urls,
+void SetHttpCameraUrls(CS_Source source, wpi::span<const std::string> urls,
                        CS_Status* status) {
   if (urls.empty()) {
     *status = CS_EMPTY_VALUE;
@@ -595,7 +641,9 @@
                                    CS_Status* status) {
   wpi::SmallVector<std::string, 4> vec;
   vec.reserve(count);
-  for (int i = 0; i < count; ++i) vec.push_back(urls[i]);
+  for (int i = 0; i < count; ++i) {
+    vec.push_back(urls[i]);
+  }
   return cs::CreateHttpCamera(name, vec, kind, status);
 }
 
@@ -607,7 +655,9 @@
                           CS_Status* status) {
   wpi::SmallVector<std::string, 4> vec;
   vec.reserve(count);
-  for (int i = 0; i < count; ++i) vec.push_back(urls[i]);
+  for (int i = 0; i < count; ++i) {
+    vec.push_back(urls[i]);
+  }
   cs::SetHttpCameraUrls(source, vec, status);
 }
 
@@ -616,13 +666,19 @@
   char** out =
       static_cast<char**>(wpi::safe_malloc(urls.size() * sizeof(char*)));
   *count = urls.size();
-  for (size_t i = 0; i < urls.size(); ++i) out[i] = cs::ConvertToC(urls[i]);
+  for (size_t i = 0; i < urls.size(); ++i) {
+    out[i] = cs::ConvertToC(urls[i]);
+  }
   return out;
 }
 
 void CS_FreeHttpCameraUrls(char** urls, int count) {
-  if (!urls) return;
-  for (int i = 0; i < count; ++i) std::free(urls[i]);
+  if (!urls) {
+    return;
+  }
+  for (int i = 0; i < count; ++i) {
+    std::free(urls[i]);
+  }
   std::free(urls);
 }
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.h
index 5967015..2c58936 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/HttpCameraImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_HTTPCAMERAIMPL_H_
 #define CSCORE_HTTPCAMERAIMPL_H_
@@ -13,15 +10,16 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <vector>
 
 #include <wpi/HttpUtil.h>
 #include <wpi/SmallString.h>
 #include <wpi/StringMap.h>
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 #include <wpi/raw_istream.h>
+#include <wpi/span.h>
 
 #include "SourceImpl.h"
 #include "cscore_cpp.h"
@@ -30,7 +28,7 @@
 
 class HttpCameraImpl : public SourceImpl {
  public:
-  HttpCameraImpl(const wpi::Twine& name, CS_HttpCameraKind kind,
+  HttpCameraImpl(std::string_view name, CS_HttpCameraKind kind,
                  wpi::Logger& logger, Notifier& notifier, Telemetry& telemetry);
   ~HttpCameraImpl() override;
 
@@ -38,7 +36,7 @@
 
   // Property functions
   void SetProperty(int property, int value, CS_Status* status) override;
-  void SetStringProperty(int property, const wpi::Twine& value,
+  void SetStringProperty(int property, std::string_view value,
                          CS_Status* status) override;
 
   // Standard common camera properties
@@ -57,20 +55,20 @@
   void NumSinksEnabledChanged() override;
 
   CS_HttpCameraKind GetKind() const;
-  bool SetUrls(wpi::ArrayRef<std::string> urls, CS_Status* status);
+  bool SetUrls(wpi::span<const std::string> urls, CS_Status* status);
   std::vector<std::string> GetUrls() const;
 
   // Property data
   class PropertyData : public PropertyImpl {
    public:
     PropertyData() = default;
-    explicit PropertyData(const wpi::Twine& name_) : PropertyImpl{name_} {}
-    PropertyData(const wpi::Twine& name_, const wpi::Twine& httpParam_,
+    explicit PropertyData(std::string_view name_) : PropertyImpl{name_} {}
+    PropertyData(std::string_view name_, std::string_view httpParam_,
                  bool viaSettings_, CS_PropertyKind kind_, int minimum_,
                  int maximum_, int step_, int defaultValue_, int value_)
         : PropertyImpl(name_, kind_, step_, defaultValue_, value_),
           viaSettings(viaSettings_),
-          httpParam(httpParam_.str()) {
+          httpParam(httpParam_) {
       hasMinimum = true;
       minimum = minimum_;
       hasMaximum = true;
@@ -84,16 +82,16 @@
 
  protected:
   std::unique_ptr<PropertyImpl> CreateEmptyProperty(
-      const wpi::Twine& name) const override;
+      std::string_view name) const override;
 
   bool CacheProperties(CS_Status* status) const override;
 
-  void CreateProperty(const wpi::Twine& name, const wpi::Twine& httpParam,
+  void CreateProperty(std::string_view name, std::string_view httpParam,
                       bool viaSettings, CS_PropertyKind kind, int minimum,
                       int maximum, int step, int defaultValue, int value) const;
 
   template <typename T>
-  void CreateEnumProperty(const wpi::Twine& name, const wpi::Twine& httpParam,
+  void CreateEnumProperty(std::string_view name, std::string_view httpParam,
                           bool viaSettings, int defaultValue, int value,
                           std::initializer_list<T> choices) const;
 
@@ -104,7 +102,7 @@
   // Functions used by StreamThreadMain()
   wpi::HttpConnection* DeviceStreamConnect(
       wpi::SmallVectorImpl<char>& boundary);
-  void DeviceStream(wpi::raw_istream& is, wpi::StringRef boundary);
+  void DeviceStream(wpi::raw_istream& is, std::string_view boundary);
   bool DeviceStreamFrame(wpi::raw_istream& is, std::string& imageBuf);
 
   // The camera settings thread
@@ -149,12 +147,12 @@
 
 class AxisCameraImpl : public HttpCameraImpl {
  public:
-  AxisCameraImpl(const wpi::Twine& name, wpi::Logger& logger,
-                 Notifier& notifier, Telemetry& telemetry)
+  AxisCameraImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
+                 Telemetry& telemetry)
       : HttpCameraImpl{name, CS_HTTP_AXIS, logger, notifier, telemetry} {}
 #if 0
   void SetProperty(int property, int value, CS_Status* status) override;
-  void SetStringProperty(int property, const wpi::Twine& value,
+  void SetStringProperty(int property, std::string_view value,
                          CS_Status* status) override;
 #endif
  protected:
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Image.h b/third_party/allwpilib/cscore/src/main/native/cpp/Image.h
index 6305d93..9a1579f 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Image.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Image.h
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_IMAGE_H_
 #define CSCORE_IMAGE_H_
 
+#include <string_view>
 #include <vector>
 
 #include <opencv2/core/core.hpp>
-#include <wpi/StringRef.h>
 
 #include "cscore_cpp.h"
 #include "default_init_allocator.h"
@@ -37,8 +34,8 @@
   Image& operator=(const Image&) = delete;
 
   // Getters
-  operator wpi::StringRef() const { return str(); }
-  wpi::StringRef str() const { return wpi::StringRef(data(), size()); }
+  operator std::string_view() const { return str(); }  // NOLINT
+  std::string_view str() const { return {data(), size()}; }
   size_t capacity() const { return m_data.capacity(); }
   const char* data() const {
     return reinterpret_cast<const char*>(m_data.data());
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Instance.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/Instance.cpp
index 8dd5c26..3b81f01 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Instance.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Instance.cpp
@@ -1,48 +1,45 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Instance.h"
 
-#include <wpi/Path.h>
-#include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
+#include <string_view>
+
+#include <fmt/format.h>
+#include <wpi/fs.h>
 
 using namespace cs;
 
 static void def_log_func(unsigned int level, const char* file,
                          unsigned int line, const char* msg) {
-  wpi::SmallString<128> buf;
-  wpi::raw_svector_ostream oss(buf);
   if (level == 20) {
-    oss << "CS: " << msg << '\n';
-    wpi::errs() << oss.str();
+    fmt::print(stderr, "CS: {}\n", msg);
     return;
   }
 
-  wpi::StringRef levelmsg;
-  if (level >= 50)
-    levelmsg = "CRITICAL: ";
-  else if (level >= 40)
-    levelmsg = "ERROR: ";
-  else if (level >= 30)
-    levelmsg = "WARNING: ";
-  else
+  std::string_view levelmsg;
+  if (level >= 50) {
+    levelmsg = "CRITICAL";
+  } else if (level >= 40) {
+    levelmsg = "ERROR";
+  } else if (level >= 30) {
+    levelmsg = "WARNING";
+  } else {
     return;
-  oss << "CS: " << levelmsg << msg << " (" << wpi::sys::path::filename(file)
-      << ':' << line << ")\n";
-  wpi::errs() << oss.str();
+  }
+  fmt::print(stderr, "CS: {}: {} ({}:{})\n", levelmsg, msg,
+             fs::path{file}.filename().string(), line);
 }
 
-Instance::Instance() : telemetry(notifier), networkListener(logger, notifier) {
+Instance::Instance()
+    : telemetry(notifier),
+      networkListener(logger, notifier),
+      usbCameraListener(logger, notifier) {
   SetDefaultLogger();
 }
 
-Instance::~Instance() {}
+Instance::~Instance() = default;
 
 Instance& Instance::GetInstance() {
   static Instance* inst = new Instance;
@@ -54,11 +51,14 @@
   m_sinks.FreeAll();
   m_sources.FreeAll();
   networkListener.Stop();
+  usbCameraListener.Stop();
   telemetry.Stop();
   notifier.Stop();
 }
 
-void Instance::SetDefaultLogger() { logger.SetLogger(def_log_func); }
+void Instance::SetDefaultLogger() {
+  logger.SetLogger(def_log_func);
+}
 
 std::pair<CS_Source, std::shared_ptr<SourceData>> Instance::FindSource(
     const SourceImpl& source) {
@@ -87,11 +87,13 @@
 }
 
 void Instance::DestroySource(CS_Source handle) {
-  if (auto data = m_sources.Free(handle))
+  if (auto data = m_sources.Free(handle)) {
     notifier.NotifySource(data->source->GetName(), handle, CS_SOURCE_DESTROYED);
+  }
 }
 
 void Instance::DestroySink(CS_Sink handle) {
-  if (auto data = m_sinks.Free(handle))
+  if (auto data = m_sinks.Free(handle)) {
     notifier.NotifySink(data->sink->GetName(), handle, CS_SINK_DESTROYED);
+  }
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Instance.h b/third_party/allwpilib/cscore/src/main/native/cpp/Instance.h
index 0c566f3..f84eacf 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Instance.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Instance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_INSTANCE_H_
 #define CSCORE_INSTANCE_H_
@@ -21,12 +18,13 @@
 #include "SourceImpl.h"
 #include "Telemetry.h"
 #include "UnlimitedHandleResource.h"
+#include "UsbCameraListener.h"
 
 namespace cs {
 
 struct SourceData {
   SourceData(CS_SourceKind kind_, std::shared_ptr<SourceImpl> source_)
-      : kind{kind_}, refCount{0}, source{source_} {}
+      : kind{kind_}, refCount{0}, source{std::move(source_)} {}
 
   CS_SourceKind kind;
   std::atomic_int refCount;
@@ -35,7 +33,7 @@
 
 struct SinkData {
   explicit SinkData(CS_SinkKind kind_, std::shared_ptr<SinkImpl> sink_)
-      : kind{kind_}, refCount{0}, sourceHandle{0}, sink{sink_} {}
+      : kind{kind_}, refCount{0}, sourceHandle{0}, sink{std::move(sink_)} {}
 
   CS_SinkKind kind;
   std::atomic_int refCount;
@@ -57,6 +55,7 @@
   Notifier notifier;
   Telemetry telemetry;
   NetworkListener networkListener;
+  UsbCameraListener usbCameraListener;
 
  private:
   UnlimitedHandleResource<Handle, SourceData, Handle::kSource> m_sources;
@@ -87,21 +86,22 @@
   void DestroySource(CS_Source handle);
   void DestroySink(CS_Sink handle);
 
-  wpi::ArrayRef<CS_Source> EnumerateSourceHandles(
+  wpi::span<CS_Source> EnumerateSourceHandles(
       wpi::SmallVectorImpl<CS_Source>& vec) {
     return m_sources.GetAll(vec);
   }
 
-  wpi::ArrayRef<CS_Sink> EnumerateSinkHandles(
-      wpi::SmallVectorImpl<CS_Sink>& vec) {
+  wpi::span<CS_Sink> EnumerateSinkHandles(wpi::SmallVectorImpl<CS_Sink>& vec) {
     return m_sinks.GetAll(vec);
   }
 
-  wpi::ArrayRef<CS_Sink> EnumerateSourceSinks(
-      CS_Source source, wpi::SmallVectorImpl<CS_Sink>& vec) {
+  wpi::span<CS_Sink> EnumerateSourceSinks(CS_Source source,
+                                          wpi::SmallVectorImpl<CS_Sink>& vec) {
     vec.clear();
     m_sinks.ForEach([&](CS_Sink sinkHandle, const SinkData& data) {
-      if (source == data.sourceHandle.load()) vec.push_back(sinkHandle);
+      if (source == data.sourceHandle.load()) {
+        vec.push_back(sinkHandle);
+      }
     });
     return vec;
   }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.cpp
index 35891e8..2e66338 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "JpegUtil.h"
 
@@ -49,29 +46,44 @@
     0xd5, 0xd6, 0xd7, 0xd8, 0xd9, 0xda, 0xe2, 0xe3, 0xe4, 0xe5, 0xe6, 0xe7,
     0xe8, 0xe9, 0xea, 0xf2, 0xf3, 0xf4, 0xf5, 0xf6, 0xf7, 0xf8, 0xf9, 0xfa};
 
-bool IsJpeg(wpi::StringRef data) {
-  if (data.size() < 11) return false;
+bool IsJpeg(std::string_view data) {
+  if (data.size() < 11) {
+    return false;
+  }
 
   // Check for valid SOI
-  auto bytes = data.bytes_begin();
-  if (bytes[0] != 0xff || bytes[1] != 0xd8) return false;
+  auto bytes = reinterpret_cast<const unsigned char*>(data.data());
+  if (bytes[0] != 0xff || bytes[1] != 0xd8) {
+    return false;
+  }
   return true;
 }
 
-bool GetJpegSize(wpi::StringRef data, int* width, int* height) {
-  if (!IsJpeg(data)) return false;
+bool GetJpegSize(std::string_view data, int* width, int* height) {
+  if (!IsJpeg(data)) {
+    return false;
+  }
 
   data = data.substr(2);  // Get to the first block
-  auto bytes = data.bytes_begin();
   for (;;) {
-    if (data.size() < 4) return false;  // EOF
-    bytes = data.bytes_begin();
-    if (bytes[0] != 0xff) return false;  // not a tag
-    if (bytes[1] == 0xd9) return false;  // EOI without finding SOF?
-    if (bytes[1] == 0xda) return false;  // SOS without finding SOF?
+    if (data.size() < 4) {
+      return false;  // EOF
+    }
+    auto bytes = reinterpret_cast<const unsigned char*>(data.data());
+    if (bytes[0] != 0xff) {
+      return false;  // not a tag
+    }
+    if (bytes[1] == 0xd9) {
+      return false;  // EOI without finding SOF?
+    }
+    if (bytes[1] == 0xda) {
+      return false;  // SOS without finding SOF?
+    }
     if (bytes[1] == 0xc0) {
       // SOF contains the file size
-      if (data.size() < 9) return false;
+      if (data.size() < 9) {
+        return false;
+      }
       *height = bytes[5] * 256 + bytes[6];
       *width = bytes[7] * 256 + bytes[8];
       return true;
@@ -82,21 +94,32 @@
 }
 
 bool JpegNeedsDHT(const char* data, size_t* size, size_t* locSOF) {
-  wpi::StringRef sdata(data, *size);
-  if (!IsJpeg(sdata)) return false;
+  std::string_view sdata(data, *size);
+  if (!IsJpeg(sdata)) {
+    return false;
+  }
 
   *locSOF = *size;
 
   // Search until SOS for DHT tag
   sdata = sdata.substr(2);  // Get to the first block
-  auto bytes = sdata.bytes_begin();
   for (;;) {
-    if (sdata.size() < 4) return false;  // EOF
-    bytes = sdata.bytes_begin();
-    if (bytes[0] != 0xff) return false;                   // not a tag
-    if (bytes[1] == 0xda) break;                          // SOS
-    if (bytes[1] == 0xc4) return false;                   // DHT
-    if (bytes[1] == 0xc0) *locSOF = sdata.data() - data;  // SOF
+    if (sdata.size() < 4) {
+      return false;  // EOF
+    }
+    auto bytes = reinterpret_cast<const unsigned char*>(sdata.data());
+    if (bytes[0] != 0xff) {
+      return false;  // not a tag
+    }
+    if (bytes[1] == 0xda) {
+      break;  // SOS
+    }
+    if (bytes[1] == 0xc4) {
+      return false;  // DHT
+    }
+    if (bytes[1] == 0xc0) {
+      *locSOF = sdata.data() - data;  // SOF
+    }
     // Go to the next block
     sdata = sdata.substr(bytes[2] * 256 + bytes[3] + 2);
   }
@@ -109,9 +132,8 @@
   return false;
 }
 
-wpi::StringRef JpegGetDHT() {
-  return wpi::StringRef(reinterpret_cast<const char*>(dhtData),
-                        sizeof(dhtData));
+std::string_view JpegGetDHT() {
+  return {reinterpret_cast<const char*>(dhtData), sizeof(dhtData)};
 }
 
 static inline void ReadInto(wpi::raw_istream& is, std::string& buf,
@@ -129,18 +151,26 @@
   // read SOI and first marker
   buf.resize(4);
   is.read(&(*buf.begin()), 4);
-  if (is.has_error()) return false;
+  if (is.has_error()) {
+    return false;
+  }
 
   // Check for valid SOI
   auto bytes = reinterpret_cast<const unsigned char*>(buf.data());
-  if (bytes[0] != 0xff || bytes[1] != 0xd8) return false;
+  if (bytes[0] != 0xff || bytes[1] != 0xd8) {
+    return false;
+  }
   size_t pos = 2;  // point to first marker
   for (;;) {
     bytes = reinterpret_cast<const unsigned char*>(buf.data() + pos);
-    if (bytes[0] != 0xff) return false;  // not a marker
+    if (bytes[0] != 0xff) {
+      return false;  // not a marker
+    }
     unsigned char marker = bytes[1];
 
-    if (marker == 0xd9) return true;  // EOI, we're done
+    if (marker == 0xd9) {
+      return true;  // EOI, we're done
+    }
 
     if (marker == 0xda) {
       // SOS: need to keep reading until we reach a normal marker.
@@ -150,12 +180,15 @@
       bool maybeMarker = false;
       for (;;) {
         ReadInto(is, buf, 1);
-        if (is.has_error()) return false;
+        if (is.has_error()) {
+          return false;
+        }
         bytes = reinterpret_cast<const unsigned char*>(buf.data() + pos);
         if (maybeMarker) {
           if (bytes[0] != 0x00 && bytes[0] != 0xff &&
-              (bytes[0] < 0xd0 || bytes[0] > 0xd7))
+              (bytes[0] < 0xd0 || bytes[0] > 0xd7)) {
             break;
+          }
           maybeMarker = false;
         } else if (bytes[0] == 0xff) {
           maybeMarker = true;
@@ -168,7 +201,9 @@
 
     // A normal block. Read the length
     ReadInto(is, buf, 2);  // read length
-    if (is.has_error()) return false;
+    if (is.has_error()) {
+      return false;
+    }
 
     // Point to length
     pos += 2;
@@ -177,7 +212,9 @@
     // Read the block and the next marker
     size_t blockLength = bytes[0] * 256 + bytes[1];
     ReadInto(is, buf, blockLength);
-    if (is.has_error()) return false;
+    if (is.has_error()) {
+      return false;
+    }
     bytes = reinterpret_cast<const unsigned char*>(buf.data() + pos);
 
     // Special block processing
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.h b/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.h
index 082fdc4..4fe0693 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/JpegUtil.h
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_JPEGUTIL_H_
 #define CSCORE_JPEGUTIL_H_
 
 #include <string>
-
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace wpi {
 class raw_istream;
@@ -18,13 +14,13 @@
 
 namespace cs {
 
-bool IsJpeg(wpi::StringRef data);
+bool IsJpeg(std::string_view data);
 
-bool GetJpegSize(wpi::StringRef data, int* width, int* height);
+bool GetJpegSize(std::string_view data, int* width, int* height);
 
 bool JpegNeedsDHT(const char* data, size_t* size, size_t* locSOF);
 
-wpi::StringRef JpegGetDHT();
+std::string_view JpegGetDHT();
 
 bool ReadJpeg(wpi::raw_istream& is, std::string& buf, int* width, int* height);
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Log.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/Log.cpp
new file mode 100644
index 0000000..cf671bb
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Log.cpp
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Log.h"
+
+void cs::NamedLogV(wpi::Logger& logger, unsigned int level, const char* file,
+                   unsigned int line, std::string_view name,
+                   fmt::string_view format, fmt::format_args args) {
+  fmt::memory_buffer out;
+  fmt::format_to(fmt::appender{out}, "{}: ", name);
+  fmt::vformat_to(fmt::appender{out}, format, args);
+  out.push_back('\0');
+  logger.DoLog(level, file, line, out.data());
+}
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Log.h b/third_party/allwpilib/cscore/src/main/native/cpp/Log.h
index b9fd9ab..21becdf 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Log.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Log.h
@@ -1,36 +1,75 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_LOG_H_
 #define CSCORE_LOG_H_
 
+#include <string_view>
+
 #include <wpi/Logger.h>
 
-#define LOG(level, x) WPI_LOG(m_logger, level, x)
+namespace cs {
+
+void NamedLogV(wpi::Logger& logger, unsigned int level, const char* file,
+               unsigned int line, std::string_view name,
+               fmt::string_view format, fmt::format_args args);
+
+template <typename S, typename... Args>
+inline void NamedLog(wpi::Logger& logger, unsigned int level, const char* file,
+                     unsigned int line, std::string_view name, const S& format,
+                     Args&&... args) {
+  if (logger.HasLogger() && level >= logger.min_level()) {
+    NamedLogV(logger, level, file, line, name, format,
+              fmt::make_args_checked<Args...>(format, args...));
+  }
+}
+
+}  // namespace cs
+
+#define LOG(level, format, ...) WPI_LOG(m_logger, level, format, __VA_ARGS__)
 
 #undef ERROR
-#define ERROR(x) WPI_ERROR(m_logger, x)
-#define WARNING(x) WPI_WARNING(m_logger, x)
-#define INFO(x) WPI_INFO(m_logger, x)
+#define ERROR(format, ...) WPI_ERROR(m_logger, format, __VA_ARGS__)
+#define WARNING(format, ...) WPI_WARNING(m_logger, format, __VA_ARGS__)
+#define INFO(format, ...) WPI_INFO(m_logger, format, __VA_ARGS__)
 
-#define DEBUG0(x) WPI_DEBUG(m_logger, x)
-#define DEBUG1(x) WPI_DEBUG1(m_logger, x)
-#define DEBUG2(x) WPI_DEBUG2(m_logger, x)
-#define DEBUG3(x) WPI_DEBUG3(m_logger, x)
-#define DEBUG4(x) WPI_DEBUG4(m_logger, x)
+#define DEBUG0(format, ...) WPI_DEBUG(m_logger, format, __VA_ARGS__)
+#define DEBUG1(format, ...) WPI_DEBUG1(m_logger, format, __VA_ARGS__)
+#define DEBUG2(format, ...) WPI_DEBUG2(m_logger, format, __VA_ARGS__)
+#define DEBUG3(format, ...) WPI_DEBUG3(m_logger, format, __VA_ARGS__)
+#define DEBUG4(format, ...) WPI_DEBUG4(m_logger, format, __VA_ARGS__)
 
-#define SERROR(x) ERROR(GetName() << ": " << x)
-#define SWARNING(x) WARNING(GetName() << ": " << x)
-#define SINFO(x) INFO(GetName() << ": " << x)
+#define SLOG(level, format, ...)                                               \
+  NamedLog(m_logger, level, __FILE__, __LINE__, GetName(), FMT_STRING(format), \
+           __VA_ARGS__)
 
-#define SDEBUG(x) DEBUG0(GetName() << ": " << x)
-#define SDEBUG1(x) DEBUG1(GetName() << ": " << x)
-#define SDEBUG2(x) DEBUG2(GetName() << ": " << x)
-#define SDEBUG3(x) DEBUG3(GetName() << ": " << x)
-#define SDEBUG4(x) DEBUG4(GetName() << ": " << x)
+#define SERROR(format, ...) SLOG(::wpi::WPI_LOG_ERROR, format, __VA_ARGS__)
+#define SWARNING(format, ...) SLOG(::wpi::WPI_LOG_WARNING, format, __VA_ARGS__)
+#define SINFO(format, ...) SLOG(::wpi::WPI_LOG_INFO, format, __VA_ARGS__)
+
+#ifdef NDEBUG
+#define SDEBUG(format, ...) \
+  do {                      \
+  } while (0)
+#define SDEBUG1(format, ...) \
+  do {                       \
+  } while (0)
+#define SDEBUG2(format, ...) \
+  do {                       \
+  } while (0)
+#define SDEBUG3(format, ...) \
+  do {                       \
+  } while (0)
+#define SDEBUG4(format, ...) \
+  do {                       \
+  } while (0)
+#else
+#define SDEBUG(format, ...) SLOG(::wpi::WPI_LOG_DEBUG, format, __VA_ARGS__)
+#define SDEBUG1(format, ...) SLOG(::wpi::WPI_LOG_DEBUG1, format, __VA_ARGS__)
+#define SDEBUG2(format, ...) SLOG(::wpi::WPI_LOG_DEBUG2, format, __VA_ARGS__)
+#define SDEBUG3(format, ...) SLOG(::wpi::WPI_LOG_DEBUG3, format, __VA_ARGS__)
+#define SDEBUG4(format, ...) SLOG(::wpi::WPI_LOG_DEBUG4, format, __VA_ARGS__)
+#endif
 
 #endif  // CSCORE_LOG_H_
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.cpp
index e30b745..92c23ff 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.cpp
@@ -1,17 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "MjpegServerImpl.h"
 
 #include <chrono>
 
+#include <fmt/format.h>
 #include <wpi/HttpUtil.h>
 #include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
 #include <wpi/TCPAcceptor.h>
+#include <wpi/fmt/raw_ostream.h>
 #include <wpi/raw_socket_istream.h>
 #include <wpi/raw_socket_ostream.h>
 
@@ -75,13 +75,13 @@
 
 class MjpegServerImpl::ConnThread : public wpi::SafeThread {
  public:
-  explicit ConnThread(const wpi::Twine& name, wpi::Logger& logger)
-      : m_name(name.str()), m_logger(logger) {}
+  explicit ConnThread(std::string_view name, wpi::Logger& logger)
+      : m_name(name), m_logger(logger) {}
 
-  void Main();
+  void Main() override;
 
   bool ProcessCommand(wpi::raw_ostream& os, SourceImpl& source,
-                      wpi::StringRef parameters, bool respond);
+                      std::string_view parameters, bool respond);
   void SendJSON(wpi::raw_ostream& os, SourceImpl& source, bool header);
   void SendHTMLHeadTitle(wpi::raw_ostream& os) const;
   void SendHTML(wpi::raw_ostream& os, SourceImpl& source, bool header);
@@ -102,7 +102,7 @@
   std::string m_name;
   wpi::Logger& m_logger;
 
-  wpi::StringRef GetName() { return m_name; }
+  std::string_view GetName() { return m_name; }
 
   std::shared_ptr<SourceImpl> GetSource() {
     std::scoped_lock lock(m_mutex);
@@ -111,13 +111,17 @@
 
   void StartStream() {
     std::scoped_lock lock(m_mutex);
-    if (m_source) m_source->EnableSink();
+    if (m_source) {
+      m_source->EnableSink();
+    }
     m_streaming = true;
   }
 
   void StopStream() {
     std::scoped_lock lock(m_mutex);
-    if (m_source) m_source->DisableSink();
+    if (m_source) {
+      m_source->DisableSink();
+    }
     m_streaming = false;
   }
 };
@@ -129,10 +133,9 @@
 // Using cached pictures would lead to showing old/outdated pictures.
 // Many browsers seem to ignore, or at least not always obey, those headers.
 static void SendHeader(wpi::raw_ostream& os, int code,
-                       const wpi::Twine& codeText,
-                       const wpi::Twine& contentType,
-                       const wpi::Twine& extra = wpi::Twine{}) {
-  os << "HTTP/1.0 " << code << ' ' << codeText << "\r\n";
+                       std::string_view codeText, std::string_view contentType,
+                       std::string_view extra = {}) {
+  fmt::print(os, "HTTP/1.0 {} {}\r\n", code, codeText);
   os << "Connection: close\r\n"
         "Server: CameraServer/1.0\r\n"
         "Cache-Control: no-store, no-cache, must-revalidate, pre-check=0, "
@@ -141,9 +144,9 @@
         "Expires: Mon, 3 Jan 2000 12:34:56 GMT\r\n";
   os << "Content-Type: " << contentType << "\r\n";
   os << "Access-Control-Allow-Origin: *\r\nAccess-Control-Allow-Methods: *\r\n";
-  wpi::SmallString<128> extraBuf;
-  wpi::StringRef extraStr = extra.toStringRef(extraBuf);
-  if (!extraStr.empty()) os << extraStr << "\r\n";
+  if (!extra.empty()) {
+    os << extra << "\r\n";
+  }
   os << "\r\n";  // header ends with a blank line
 }
 
@@ -151,8 +154,8 @@
 // @param code HTTP error code (e.g. 404)
 // @param message Additional message text
 static void SendError(wpi::raw_ostream& os, int code,
-                      const wpi::Twine& message) {
-  wpi::StringRef codeText, extra, baseMessage;
+                      std::string_view message) {
+  std::string_view codeText, extra, baseMessage;
   switch (code) {
     case 401:
       codeText = "Unauthorized";
@@ -192,62 +195,61 @@
 // Perform a command specified by HTTP GET parameters.
 bool MjpegServerImpl::ConnThread::ProcessCommand(wpi::raw_ostream& os,
                                                  SourceImpl& source,
-                                                 wpi::StringRef parameters,
+                                                 std::string_view parameters,
                                                  bool respond) {
   wpi::SmallString<256> responseBuf;
   wpi::raw_svector_ostream response{responseBuf};
   // command format: param1=value1&param2=value2...
   while (!parameters.empty()) {
     // split out next param and value
-    wpi::StringRef rawParam, rawValue;
-    std::tie(rawParam, parameters) = parameters.split('&');
-    if (rawParam.empty()) continue;  // ignore "&&"
-    std::tie(rawParam, rawValue) = rawParam.split('=');
-    if (rawParam.empty() || rawValue.empty()) continue;  // ignore "param="
-    SDEBUG4("HTTP parameter \"" << rawParam << "\" value \"" << rawValue
-                                << "\"");
+    std::string_view rawParam, rawValue;
+    std::tie(rawParam, parameters) = wpi::split(parameters, '&');
+    if (rawParam.empty()) {
+      continue;  // ignore "&&"
+    }
+    std::tie(rawParam, rawValue) = wpi::split(rawParam, '=');
+    if (rawParam.empty() || rawValue.empty()) {
+      continue;  // ignore "param="
+    }
+    SDEBUG4("HTTP parameter \"{}\" value \"{}\"", rawParam, rawValue);
 
     // unescape param
     bool error = false;
     wpi::SmallString<64> paramBuf;
-    wpi::StringRef param = wpi::UnescapeURI(rawParam, paramBuf, &error);
+    std::string_view param = wpi::UnescapeURI(rawParam, paramBuf, &error);
     if (error) {
-      wpi::SmallString<128> error;
-      wpi::raw_svector_ostream oss{error};
-      oss << "could not unescape parameter \"" << rawParam << "\"";
-      SendError(os, 500, error.str());
-      SDEBUG(error.str());
+      auto estr = fmt::format("could not unescape parameter \"{}\"", rawParam);
+      SendError(os, 500, estr);
+      SDEBUG("{}", estr);
       return false;
     }
 
     // unescape value
     wpi::SmallString<64> valueBuf;
-    wpi::StringRef value = wpi::UnescapeURI(rawValue, valueBuf, &error);
+    std::string_view value = wpi::UnescapeURI(rawValue, valueBuf, &error);
     if (error) {
-      wpi::SmallString<128> error;
-      wpi::raw_svector_ostream oss{error};
-      oss << "could not unescape value \"" << rawValue << "\"";
-      SendError(os, 500, error.str());
-      SDEBUG(error.str());
+      auto estr = fmt::format("could not unescape value \"{}\"", rawValue);
+      SendError(os, 500, estr);
+      SDEBUG("{}", estr);
       return false;
     }
 
     // Handle resolution, compression, and FPS.  These are handled locally
     // rather than passed to the source.
     if (param == "resolution") {
-      wpi::StringRef widthStr, heightStr;
-      std::tie(widthStr, heightStr) = value.split('x');
-      int width, height;
-      if (widthStr.getAsInteger(10, width)) {
+      auto [widthStr, heightStr] = wpi::split(value, 'x');
+      int width = wpi::parse_integer<int>(widthStr, 10).value_or(-1);
+      int height = wpi::parse_integer<int>(heightStr, 10).value_or(-1);
+      if (width < 0) {
         response << param << ": \"width is not an integer\"\r\n";
-        SWARNING("HTTP parameter \"" << param << "\" width \"" << widthStr
-                                     << "\" is not an integer");
+        SWARNING("HTTP parameter \"{}\" width \"{}\" is not an integer", param,
+                 widthStr);
         continue;
       }
-      if (heightStr.getAsInteger(10, height)) {
+      if (height < 0) {
         response << param << ": \"height is not an integer\"\r\n";
-        SWARNING("HTTP parameter \"" << param << "\" height \"" << heightStr
-                                     << "\" is not an integer");
+        SWARNING("HTTP parameter \"{}\" height \"{}\" is not an integer", param,
+                 heightStr);
         continue;
       }
       m_width = width;
@@ -257,41 +259,39 @@
     }
 
     if (param == "fps") {
-      int fps;
-      if (value.getAsInteger(10, fps)) {
-        response << param << ": \"invalid integer\"\r\n";
-        SWARNING("HTTP parameter \"" << param << "\" value \"" << value
-                                     << "\" is not an integer");
-        continue;
-      } else {
-        m_fps = fps;
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_fps = v.value();
         response << param << ": \"ok\"\r\n";
+      } else {
+        response << param << ": \"invalid integer\"\r\n";
+        SWARNING("HTTP parameter \"{}\" value \"{}\" is not an integer", param,
+                 value);
       }
       continue;
     }
 
     if (param == "compression") {
-      int compression;
-      if (value.getAsInteger(10, compression)) {
-        response << param << ": \"invalid integer\"\r\n";
-        SWARNING("HTTP parameter \"" << param << "\" value \"" << value
-                                     << "\" is not an integer");
-        continue;
-      } else {
-        m_compression = compression;
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_compression = v.value();
         response << param << ": \"ok\"\r\n";
+      } else {
+        response << param << ": \"invalid integer\"\r\n";
+        SWARNING("HTTP parameter \"{}\" value \"{}\" is not an integer", param,
+                 value);
       }
       continue;
     }
 
     // ignore name parameter
-    if (param == "name") continue;
+    if (param == "name") {
+      continue;
+    }
 
     // try to assign parameter
     auto prop = source.GetPropertyIndex(param);
     if (!prop) {
       response << param << ": \"ignored\"\r\n";
-      SWARNING("ignoring HTTP parameter \"" << param << "\"");
+      SWARNING("ignoring HTTP parameter \"{}\"", param);
       continue;
     }
 
@@ -301,21 +301,20 @@
       case CS_PROP_BOOLEAN:
       case CS_PROP_INTEGER:
       case CS_PROP_ENUM: {
-        int val = 0;
-        if (value.getAsInteger(10, val)) {
-          response << param << ": \"invalid integer\"\r\n";
-          SWARNING("HTTP parameter \"" << param << "\" value \"" << value
-                                       << "\" is not an integer");
+        if (auto v = wpi::parse_integer<int>(value, 10)) {
+          fmt::print(response, "{}: {}\r\n", param, v.value());
+          SDEBUG4("HTTP parameter \"{}\" value {}", param, value);
+          source.SetProperty(prop, v.value(), &status);
         } else {
-          response << param << ": " << val << "\r\n";
-          SDEBUG4("HTTP parameter \"" << param << "\" value " << value);
-          source.SetProperty(prop, val, &status);
+          response << param << ": \"invalid integer\"\r\n";
+          SWARNING("HTTP parameter \"{}\" value \"{}\" is not an integer",
+                   param, value);
         }
         break;
       }
       case CS_PROP_STRING: {
         response << param << ": \"ok\"\r\n";
-        SDEBUG4("HTTP parameter \"" << param << "\" value \"" << value << "\"");
+        SDEBUG4("HTTP parameter \"{}\" value \"{}\"", param, value);
         source.SetStringProperty(prop, value, &status);
         break;
       }
@@ -342,7 +341,9 @@
 // Send the root html file with controls for all the settable properties.
 void MjpegServerImpl::ConnThread::SendHTML(wpi::raw_ostream& os,
                                            SourceImpl& source, bool header) {
-  if (header) SendHeader(os, 200, "OK", "text/html");
+  if (header) {
+    SendHeader(os, 200, "OK", "text/html");
+  }
 
   SendHTMLHeadTitle(os);
   os << startRootPage;
@@ -351,31 +352,35 @@
   for (auto prop : source.EnumerateProperties(properties_vec, &status)) {
     wpi::SmallString<128> name_buf;
     auto name = source.GetPropertyName(prop, name_buf, &status);
-    if (name.startswith("raw_")) continue;
+    if (wpi::starts_with(name, "raw_")) {
+      continue;
+    }
     auto kind = source.GetPropertyKind(prop);
-    os << "<p />"
-       << "<label for=\"" << name << "\">" << name << "</label>\n";
+    fmt::print(os, "<p /><label for=\"{0}\">{0}</label>\n", name);
     switch (kind) {
       case CS_PROP_BOOLEAN:
-        os << "<input id=\"" << name
-           << "\" type=\"checkbox\" onclick=\"update('" << name
-           << "', this.checked ? 1 : 0)\" ";
-        if (source.GetProperty(prop, &status) != 0)
+        fmt::print(os,
+                   "<input id=\"{0}\" type=\"checkbox\" "
+                   "onclick=\"update('{0}', this.checked ? 1 : 0)\" ",
+                   name);
+        if (source.GetProperty(prop, &status) != 0) {
           os << "checked />\n";
-        else
+        } else {
           os << " />\n";
+        }
         break;
       case CS_PROP_INTEGER: {
         auto valI = source.GetProperty(prop, &status);
         auto min = source.GetPropertyMin(prop, &status);
         auto max = source.GetPropertyMax(prop, &status);
         auto step = source.GetPropertyStep(prop, &status);
-        os << "<input type=\"range\" min=\"" << min << "\" max=\"" << max
-           << "\" value=\"" << valI << "\" id=\"" << name << "\" step=\""
-           << step << "\" oninput=\"updateInt('#" << name << "op', '" << name
-           << "', value)\" />\n";
-        os << "<output for=\"" << name << "\" id=\"" << name << "op\">" << valI
-           << "</output>\n";
+        fmt::print(os,
+                   "<input type=\"range\" min=\"{1}\" max=\"{2}\" "
+                   "value=\"{3}\" id=\"{0}\" step=\"{4}\" "
+                   "oninput=\"updateInt('#{0}op', '{0}', value)\" />\n",
+                   name, min, max, valI, step);
+        fmt::print(os, "<output for=\"{0}\" id=\"{0}op\">{1}</output>\n", name,
+                   valI);
         break;
       }
       case CS_PROP_ENUM: {
@@ -384,29 +389,36 @@
         int j = 0;
         for (auto choice = choices.begin(), end = choices.end(); choice != end;
              ++j, ++choice) {
-          if (choice->empty()) continue;  // skip empty choices
+          if (choice->empty()) {
+            continue;  // skip empty choices
+          }
           // replace any non-printable characters in name with spaces
           wpi::SmallString<128> ch_name;
-          for (char ch : *choice)
-            ch_name.push_back(std::isprint(ch) ? ch : ' ');
-          os << "<input id=\"" << name << j << "\" type=\"radio\" name=\""
-             << name << "\" value=\"" << ch_name << "\" onclick=\"update('"
-             << name << "', " << j << ")\"";
+          for (char ch : *choice) {
+            ch_name.push_back(wpi::isPrint(ch) ? ch : ' ');
+          }
+          fmt::print(os,
+                     "<input id=\"{0}{1}\" type=\"radio\" name=\"{0}\" "
+                     "value=\"{2}\" onclick=\"update('{0}', {1})\"",
+                     name, j, ch_name);
           if (j == valE) {
             os << " checked";
           }
-          os << " /><label for=\"" << name << j << "\">" << ch_name
-             << "</label>\n";
+          fmt::print(os, " /><label for=\"{}{}\">{}</label>\n", name, j,
+                     ch_name);
         }
         break;
       }
       case CS_PROP_STRING: {
         wpi::SmallString<128> strval_buf;
-        os << "<input type=\"text\" id=\"" << name << "box\" name=\"" << name
-           << "\" value=\""
-           << source.GetStringProperty(prop, strval_buf, &status) << "\" />\n";
-        os << "<input type=\"button\" value =\"Submit\" onclick=\"update('"
-           << name << "', " << name << "box.value)\" />\n";
+        fmt::print(os,
+                   "<input type=\"text\" id=\"{0}box\" name=\"{0}\" "
+                   "value=\"{1}\" />\n",
+                   name, source.GetStringProperty(prop, strval_buf, &status));
+        fmt::print(os,
+                   "<input type=\"button\" value =\"Submit\" "
+                   "onclick=\"update('{0}', {0}box.value)\" />\n",
+                   name);
         break;
       }
       default:
@@ -419,8 +431,9 @@
                                &status);
   if (status == CS_OK) {
     os << "<p>USB device path: " << info.path << '\n';
-    for (auto&& path : info.otherPaths)
+    for (auto&& path : info.otherPaths) {
       os << "<p>Alternate device path: " << path << '\n';
+    }
   }
 
   os << "<p>Supported Video Modes:</p>\n";
@@ -451,10 +464,8 @@
         os << "unknown";
         break;
     }
-    os << "</td><td>" << mode.width;
-    os << "</td><td>" << mode.height;
-    os << "</td><td>" << mode.fps;
-    os << "</td></tr>";
+    fmt::print(os, "</td><td>{}</td><td>{}</td><td>{}</td></tr>", mode.width,
+               mode.height, mode.fps);
   }
   os << "</table>\n";
   os << endRootPage << "\r\n";
@@ -464,35 +475,39 @@
 // Send a JSON file which is contains information about the source parameters.
 void MjpegServerImpl::ConnThread::SendJSON(wpi::raw_ostream& os,
                                            SourceImpl& source, bool header) {
-  if (header) SendHeader(os, 200, "OK", "application/json");
+  if (header) {
+    SendHeader(os, 200, "OK", "application/json");
+  }
 
   os << "{\n\"controls\": [\n";
   wpi::SmallVector<int, 32> properties_vec;
   bool first = true;
   CS_Status status = 0;
   for (auto prop : source.EnumerateProperties(properties_vec, &status)) {
-    if (first)
+    if (first) {
       first = false;
-    else
+    } else {
       os << ",\n";
+    }
     os << '{';
     wpi::SmallString<128> name_buf;
     auto name = source.GetPropertyName(prop, name_buf, &status);
     auto kind = source.GetPropertyKind(prop);
-    os << "\n\"name\": \"" << name << '"';
-    os << ",\n\"id\": \"" << prop << '"';
-    os << ",\n\"type\": \"" << kind << '"';
-    os << ",\n\"min\": \"" << source.GetPropertyMin(prop, &status) << '"';
-    os << ",\n\"max\": \"" << source.GetPropertyMax(prop, &status) << '"';
-    os << ",\n\"step\": \"" << source.GetPropertyStep(prop, &status) << '"';
-    os << ",\n\"default\": \"" << source.GetPropertyDefault(prop, &status)
-       << '"';
+    fmt::print(os, "\n\"name\": \"{}\"", name);
+    fmt::print(os, ",\n\"id\": \"{}\"", prop);
+    fmt::print(os, ",\n\"type\": \"{}\"", kind);
+    fmt::print(os, ",\n\"min\": \"{}\"", source.GetPropertyMin(prop, &status));
+    fmt::print(os, ",\n\"max\": \"{}\"", source.GetPropertyMax(prop, &status));
+    fmt::print(os, ",\n\"step\": \"{}\"",
+               source.GetPropertyStep(prop, &status));
+    fmt::print(os, ",\n\"default\": \"{}\"",
+               source.GetPropertyDefault(prop, &status));
     os << ",\n\"value\": \"";
     switch (kind) {
       case CS_PROP_BOOLEAN:
       case CS_PROP_INTEGER:
       case CS_PROP_ENUM:
-        os << source.GetProperty(prop, &status);
+        fmt::print(os, "{}", source.GetProperty(prop, &status));
         break;
       case CS_PROP_STRING: {
         wpi::SmallString<128> strval_buf;
@@ -514,11 +529,15 @@
       int j = 0;
       for (auto choice = choices.begin(), end = choices.end(); choice != end;
            ++j, ++choice) {
-        if (j != 0) os << ", ";
+        if (j != 0) {
+          os << ", ";
+        }
         // replace any non-printable characters in name with spaces
         wpi::SmallString<128> ch_name;
-        for (char ch : *choice) ch_name.push_back(std::isprint(ch) ? ch : ' ');
-        os << '"' << j << "\": \"" << ch_name << '"';
+        for (char ch : *choice) {
+          ch_name.push_back(std::isprint(ch) ? ch : ' ');
+        }
+        fmt::print(os, "\"{}\": \"{}\"", j, ch_name);
       }
       os << "}\n";
     }
@@ -527,10 +546,11 @@
   os << "\n],\n\"modes\": [\n";
   first = true;
   for (auto mode : source.EnumerateVideoModes(&status)) {
-    if (first)
+    if (first) {
       first = false;
-    else
+    } else {
       os << ",\n";
+    }
     os << '{';
     os << "\n\"pixelFormat\": \"";
     switch (mode.pixelFormat) {
@@ -553,29 +573,26 @@
         os << "unknown";
         break;
     }
-    os << "\",\n\"width\": \"" << mode.width << '"';
-    os << ",\n\"height\": \"" << mode.height << '"';
-    os << ",\n\"fps\": \"" << mode.fps << '"';
+    fmt::print(os, "\",\n\"width\": \"{}\"", mode.width);
+    fmt::print(os, ",\n\"height\": \"{}\"", mode.height);
+    fmt::print(os, ",\n\"fps\": \"{}\"", mode.fps);
     os << '}';
   }
   os << "\n]\n}\n";
   os.flush();
 }
 
-MjpegServerImpl::MjpegServerImpl(const wpi::Twine& name, wpi::Logger& logger,
+MjpegServerImpl::MjpegServerImpl(std::string_view name, wpi::Logger& logger,
                                  Notifier& notifier, Telemetry& telemetry,
-                                 const wpi::Twine& listenAddress, int port,
+                                 std::string_view listenAddress, int port,
                                  std::unique_ptr<wpi::NetworkAcceptor> acceptor)
     : SinkImpl{name, logger, notifier, telemetry},
-      m_listenAddress(listenAddress.str()),
+      m_listenAddress(listenAddress),
       m_port(port),
       m_acceptor{std::move(acceptor)} {
   m_active = true;
 
-  wpi::SmallString<128> descBuf;
-  wpi::raw_svector_ostream desc{descBuf};
-  desc << "HTTP Server on port " << port;
-  SetDescription(desc.str());
+  SetDescription(fmt::format("HTTP Server on port {}", port));
 
   // Create properties
   m_widthProp = CreateProperty("width", [] {
@@ -599,7 +616,9 @@
   m_serverThread = std::thread(&MjpegServerImpl::ServerThreadMain, this);
 }
 
-MjpegServerImpl::~MjpegServerImpl() { Stop(); }
+MjpegServerImpl::~MjpegServerImpl() {
+  Stop();
+}
 
 void MjpegServerImpl::Stop() {
   m_active = false;
@@ -608,24 +627,30 @@
   m_acceptor->shutdown();
 
   // join server thread
-  if (m_serverThread.joinable()) m_serverThread.join();
+  if (m_serverThread.joinable()) {
+    m_serverThread.join();
+  }
 
   // close streams
   for (auto& connThread : m_connThreads) {
     if (auto thr = connThread.GetThread()) {
-      if (thr->m_stream) thr->m_stream->close();
+      if (thr->m_stream) {
+        thr->m_stream->close();
+      }
     }
     connThread.Stop();
   }
 
   // wake up connection threads by forcing an empty frame to be sent
-  if (auto source = GetSource()) source->Wakeup();
+  if (auto source = GetSource()) {
+    source->Wakeup();
+  }
 }
 
 // Send HTTP response and a stream of JPG-frames
 void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
   if (m_noStreaming) {
-    SERROR("Too many simultaneous client streams");
+    SERROR("{}", "Too many simultaneous client streams");
     SendError(os, 503, "Too many simultaneous streams");
     return;
   }
@@ -638,14 +663,18 @@
   SendHeader(oss, 200, "OK", "multipart/x-mixed-replace;boundary=" BOUNDARY);
   os << oss.str();
 
-  SDEBUG("Headers send, sending stream now");
+  SDEBUG("{}", "Headers send, sending stream now");
 
   Frame::Time lastFrameTime = 0;
   Frame::Time timePerFrame = 0;
-  if (m_fps != 0) timePerFrame = 1000000.0 / m_fps;
+  if (m_fps != 0) {
+    timePerFrame = 1000000.0 / m_fps;
+  }
   Frame::Time averageFrameTime = 0;
   Frame::Time averagePeriod = 1000000;  // 1 second window
-  if (averagePeriod < timePerFrame) averagePeriod = timePerFrame * 10;
+  if (averagePeriod < timePerFrame) {
+    averagePeriod = timePerFrame * 10;
+  }
 
   StartStream();
   while (m_active && !os.has_error()) {
@@ -656,9 +685,11 @@
       std::this_thread::sleep_for(std::chrono::milliseconds(200));
       continue;
     }
-    SDEBUG4("waiting for frame");
+    SDEBUG4("{}", "waiting for frame");
     Frame frame = source->GetNextFrame(0.225);  // blocks
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
     if (!frame) {
       // Bad frame; sleep for 20 ms so we don't consume all processor time.
       os << "\r\n";  // Keep connection alive
@@ -717,7 +748,7 @@
         continue;
     }
 
-    SDEBUG4("sending frame size=" << size << " addDHT=" << addDHT);
+    SDEBUG4("sending frame size={} addDHT={}", size, addDHT);
 
     // print the individual mimetype and the length
     // sending the content-length fixes random stream disruption observed
@@ -726,18 +757,18 @@
     double timestamp = lastFrameTime / 1000000.0;
     header.clear();
     oss << "\r\n--" BOUNDARY "\r\n"
-        << "Content-Type: image/jpeg\r\n"
-        << "Content-Length: " << size << "\r\n"
-        << "X-Timestamp: " << timestamp << "\r\n"
-        << "\r\n";
+        << "Content-Type: image/jpeg\r\n";
+    fmt::print(oss, "Content-Length: {}\r\n", size);
+    fmt::print(oss, "X-Timestamp: {}\r\n", timestamp);
+    oss << "\r\n";
     os << oss.str();
     if (addDHT) {
       // Insert DHT data immediately before SOF
-      os << wpi::StringRef(data, locSOF);
+      os << std::string_view(data, locSOF);
       os << JpegGetDHT();
-      os << wpi::StringRef(data + locSOF, image->size() - locSOF);
+      os << std::string_view(data + locSOF, image->size() - locSOF);
     } else {
-      os << wpi::StringRef(data, size);
+      os << std::string_view(data, size);
     }
     // os.flush();
   }
@@ -750,48 +781,50 @@
 
   // Read the request string from the stream
   wpi::SmallString<128> reqBuf;
-  wpi::StringRef req = is.getline(reqBuf, 4096);
+  std::string_view req = is.getline(reqBuf, 4096);
   if (is.has_error()) {
-    SDEBUG("error getting request string");
+    SDEBUG("{}", "error getting request string");
     return;
   }
 
   enum { kCommand, kStream, kGetSettings, kGetSourceConfig, kRootPage } kind;
-  wpi::StringRef parameters;
+  std::string_view parameters;
   size_t pos;
 
-  SDEBUG("HTTP request: '" << req << "'\n");
+  SDEBUG("HTTP request: '{}'\n", req);
 
   // Determine request kind.  Most of these are for mjpgstreamer
   // compatibility, others are for Axis camera compatibility.
-  if ((pos = req.find("POST /stream")) != wpi::StringRef::npos) {
+  if ((pos = req.find("POST /stream")) != std::string_view::npos) {
     kind = kStream;
     parameters = req.substr(req.find('?', pos + 12)).substr(1);
-  } else if ((pos = req.find("GET /?action=stream")) != wpi::StringRef::npos) {
+  } else if ((pos = req.find("GET /?action=stream")) !=
+             std::string_view::npos) {
     kind = kStream;
     parameters = req.substr(req.find('&', pos + 19)).substr(1);
-  } else if ((pos = req.find("GET /stream.mjpg")) != wpi::StringRef::npos) {
+  } else if ((pos = req.find("GET /stream.mjpg")) != std::string_view::npos) {
     kind = kStream;
     parameters = req.substr(req.find('?', pos + 16)).substr(1);
-  } else if (req.find("GET /settings") != wpi::StringRef::npos &&
-             req.find(".json") != wpi::StringRef::npos) {
+  } else if (req.find("GET /settings") != std::string_view::npos &&
+             req.find(".json") != std::string_view::npos) {
     kind = kGetSettings;
-  } else if (req.find("GET /config") != wpi::StringRef::npos &&
-             req.find(".json") != wpi::StringRef::npos) {
+  } else if (req.find("GET /config") != std::string_view::npos &&
+             req.find(".json") != std::string_view::npos) {
     kind = kGetSourceConfig;
-  } else if (req.find("GET /input") != wpi::StringRef::npos &&
-             req.find(".json") != wpi::StringRef::npos) {
+  } else if (req.find("GET /input") != std::string_view::npos &&
+             req.find(".json") != std::string_view::npos) {
     kind = kGetSettings;
-  } else if (req.find("GET /output") != wpi::StringRef::npos &&
-             req.find(".json") != wpi::StringRef::npos) {
+  } else if (req.find("GET /output") != std::string_view::npos &&
+             req.find(".json") != std::string_view::npos) {
     kind = kGetSettings;
-  } else if ((pos = req.find("GET /?action=command")) != wpi::StringRef::npos) {
+  } else if ((pos = req.find("GET /?action=command")) !=
+             std::string_view::npos) {
     kind = kCommand;
     parameters = req.substr(req.find('&', pos + 20)).substr(1);
-  } else if (req.find("GET / ") != wpi::StringRef::npos || req == "GET /\n") {
+  } else if (req.find("GET / ") != std::string_view::npos || req == "GET /\n") {
     kind = kRootPage;
   } else {
-    SDEBUG("HTTP request resource not found");
+    SDEBUG("{}", "HTTP request resource not found");
     SendError(os, 404, "Resource not found");
     return;
   }
@@ -801,22 +834,28 @@
       "abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ_"
       "-=&1234567890%./");
   parameters = parameters.substr(0, pos);
-  SDEBUG("command parameters: \"" << parameters << "\"");
+  SDEBUG("command parameters: \"{}\"", parameters);
 
   // Read the rest of the HTTP request.
   // The end of the request is marked by a single, empty line
   wpi::SmallString<128> lineBuf;
   for (;;) {
-    if (is.getline(lineBuf, 4096).startswith("\n")) break;
-    if (is.has_error()) return;
+    if (wpi::starts_with(is.getline(lineBuf, 4096), "\n")) {
+      break;
+    }
+    if (is.has_error()) {
+      return;
+    }
   }
 
   // Send response
   switch (kind) {
     case kStream:
       if (auto source = GetSource()) {
-        SDEBUG("request for stream " << source->GetName());
-        if (!ProcessCommand(os, *source, parameters, false)) return;
+        SDEBUG("request for stream {}", source->GetName());
+        if (!ProcessCommand(os, *source, parameters, false)) {
+          return;
+        }
       }
       SendStream(os);
       break;
@@ -827,18 +866,19 @@
         SendHeader(os, 200, "OK", "text/plain");
         os << "Ignored due to no connected source."
            << "\r\n";
-        SDEBUG("Ignored due to no connected source.");
+        SDEBUG("{}", "Ignored due to no connected source.");
       }
       break;
     case kGetSettings:
-      SDEBUG("request for JSON file");
-      if (auto source = GetSource())
+      SDEBUG("{}", "request for JSON file");
+      if (auto source = GetSource()) {
         SendJSON(os, *source, true);
-      else
+      } else {
         SendError(os, 404, "Resource not found");
+      }
       break;
     case kGetSourceConfig:
-      SDEBUG("request for JSON file");
+      SDEBUG("{}", "request for JSON file");
       if (auto source = GetSource()) {
         SendHeader(os, 200, "OK", "application/json");
         CS_Status status = CS_OK;
@@ -849,7 +889,7 @@
       }
       break;
     case kRootPage:
-      SDEBUG("request for root page");
+      SDEBUG("{}", "request for root page");
       SendHeader(os, 200, "OK", "text/html");
       if (auto source = GetSource()) {
         SendHTML(os, *source, false);
@@ -860,7 +900,7 @@
       break;
   }
 
-  SDEBUG("leaving HTTP client thread");
+  SDEBUG("{}", "leaving HTTP client thread");
 }
 
 // worker thread for clients that connected to this server
@@ -869,7 +909,9 @@
   while (m_active) {
     while (!m_stream) {
       m_cond.wait(lock);
-      if (!m_active) return;
+      if (!m_active) {
+        return;
+      }
     }
     lock.unlock();
     ProcessRequest();
@@ -885,16 +927,18 @@
     return;
   }
 
-  SDEBUG("waiting for clients to connect");
+  SDEBUG("{}", "waiting for clients to connect");
   while (m_active) {
     auto stream = m_acceptor->accept();
     if (!stream) {
       m_active = false;
       return;
     }
-    if (!m_active) return;
+    if (!m_active) {
+      return;
+    }
 
-    SDEBUG("client connection from " << stream->getPeerIP());
+    SDEBUG("client connection from {}", stream->getPeerIP());
 
     auto source = GetSource();
 
@@ -933,7 +977,7 @@
     thr->m_cond.notify_one();
   }
 
-  SDEBUG("leaving server thread");
+  SDEBUG("{}", "leaving server thread");
 }
 
 void MjpegServerImpl::SetSourceImpl(std::shared_ptr<SourceImpl> source) {
@@ -942,9 +986,13 @@
     if (auto thr = connThread.GetThread()) {
       if (thr->m_source != source) {
         bool streaming = thr->m_streaming;
-        if (thr->m_source && streaming) thr->m_source->DisableSink();
+        if (thr->m_source && streaming) {
+          thr->m_source->DisableSink();
+        }
         thr->m_source = source;
-        if (source && streaming) thr->m_source->EnableSink();
+        if (source && streaming) {
+          thr->m_source->EnableSink();
+        }
       }
     }
   }
@@ -952,19 +1000,15 @@
 
 namespace cs {
 
-CS_Sink CreateMjpegServer(const wpi::Twine& name,
-                          const wpi::Twine& listenAddress, int port,
-                          CS_Status* status) {
+CS_Sink CreateMjpegServer(std::string_view name, std::string_view listenAddress,
+                          int port, CS_Status* status) {
   auto& inst = Instance::GetInstance();
-  wpi::SmallString<128> listenAddressBuf;
   return inst.CreateSink(
       CS_SINK_MJPEG,
       std::make_shared<MjpegServerImpl>(
           name, inst.logger, inst.notifier, inst.telemetry, listenAddress, port,
-          std::unique_ptr<wpi::NetworkAcceptor>(new wpi::TCPAcceptor(
-              port,
-              listenAddress.toNullTerminatedStringRef(listenAddressBuf).data(),
-              inst.logger))));
+          std::unique_ptr<wpi::NetworkAcceptor>(
+              new wpi::TCPAcceptor(port, listenAddress, inst.logger))));
 }
 
 std::string GetMjpegServerListenAddress(CS_Sink sink, CS_Status* status) {
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.h
index db60b86..e323e69 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/MjpegServerImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_MJPEGSERVERIMPL_H_
 #define CSCORE_MJPEGSERVERIMPL_H_
@@ -11,6 +8,7 @@
 #include <atomic>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <vector>
 
@@ -18,7 +16,6 @@
 #include <wpi/NetworkStream.h>
 #include <wpi/SafeThread.h>
 #include <wpi/SmallVector.h>
-#include <wpi/Twine.h>
 #include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 #include <wpi/raw_socket_ostream.h>
@@ -31,9 +28,9 @@
 
 class MjpegServerImpl : public SinkImpl {
  public:
-  MjpegServerImpl(const wpi::Twine& name, wpi::Logger& logger,
+  MjpegServerImpl(std::string_view name, wpi::Logger& logger,
                   Notifier& notifier, Telemetry& telemetry,
-                  const wpi::Twine& listenAddress, int port,
+                  std::string_view listenAddress, int port,
                   std::unique_ptr<wpi::NetworkAcceptor> acceptor);
   ~MjpegServerImpl() override;
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/NetworkListener.h b/third_party/allwpilib/cscore/src/main/native/cpp/NetworkListener.h
index 6d49adc..d48de18 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/NetworkListener.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/NetworkListener.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_NETWORKLISTENER_H_
 #define CSCORE_NETWORKLISTENER_H_
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.cpp
index e4645c9..3711896 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Notifier.h"
 
 #include <queue>
+#include <utility>
 #include <vector>
 
 #include "Handle.h"
@@ -17,144 +15,26 @@
 
 using namespace cs;
 
-bool Notifier::s_destroyed = false;
+Notifier::Notifier() {}
 
-namespace {
-// Vector which provides an integrated freelist for removal and reuse of
-// individual elements.
-template <typename T>
-class UidVector {
- public:
-  using size_type = typename std::vector<T>::size_type;
+Notifier::~Notifier() {}
 
-  size_type size() const { return m_vector.size(); }
-  T& operator[](size_type i) { return m_vector[i]; }
-  const T& operator[](size_type i) const { return m_vector[i]; }
-
-  // Add a new T to the vector.  If there are elements on the freelist,
-  // reuses the last one; otherwise adds to the end of the vector.
-  // Returns the resulting element index (+1).
-  template <class... Args>
-  unsigned int emplace_back(Args&&... args) {
-    unsigned int uid;
-    if (m_free.empty()) {
-      uid = m_vector.size();
-      m_vector.emplace_back(std::forward<Args>(args)...);
-    } else {
-      uid = m_free.back();
-      m_free.pop_back();
-      m_vector[uid] = T(std::forward<Args>(args)...);
-    }
-    return uid + 1;
-  }
-
-  // Removes the identified element by replacing it with a default-constructed
-  // one.  The element is added to the freelist for later reuse.
-  void erase(unsigned int uid) {
-    --uid;
-    if (uid >= m_vector.size() || !m_vector[uid]) return;
-    m_free.push_back(uid);
-    m_vector[uid] = T();
-  }
-
- private:
-  std::vector<T> m_vector;
-  std::vector<unsigned int> m_free;
-};
-
-}  // namespace
-
-class Notifier::Thread : public wpi::SafeThread {
- public:
-  Thread(std::function<void()> on_start, std::function<void()> on_exit)
-      : m_on_start(on_start), m_on_exit(on_exit) {}
-
-  void Main();
-
-  struct Listener {
-    Listener() = default;
-    Listener(std::function<void(const RawEvent& event)> callback_,
-             int eventMask_)
-        : callback(callback_), eventMask(eventMask_) {}
-
-    explicit operator bool() const { return static_cast<bool>(callback); }
-
-    std::string prefix;
-    std::function<void(const RawEvent& event)> callback;
-    int eventMask;
-  };
-  UidVector<Listener> m_listeners;
-
-  std::queue<RawEvent> m_notifications;
-
-  std::function<void()> m_on_start;
-  std::function<void()> m_on_exit;
-};
-
-Notifier::Notifier() { s_destroyed = false; }
-
-Notifier::~Notifier() { s_destroyed = true; }
-
-void Notifier::Start() { m_owner.Start(m_on_start, m_on_exit); }
-
-void Notifier::Stop() { m_owner.Stop(); }
-
-void Notifier::Thread::Main() {
-  if (m_on_start) m_on_start();
-
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    while (m_notifications.empty()) {
-      m_cond.wait(lock);
-      if (!m_active) goto done;
-    }
-
-    while (!m_notifications.empty()) {
-      if (!m_active) goto done;
-      auto item = std::move(m_notifications.front());
-      m_notifications.pop();
-
-      // Use index because iterator might get invalidated.
-      for (size_t i = 0; i < m_listeners.size(); ++i) {
-        if (!m_listeners[i]) continue;  // removed
-
-        // Event type must be within requested set for this listener.
-        if ((item.kind & m_listeners[i].eventMask) == 0) continue;
-
-        // make a copy of the callback so we can safely release the mutex
-        auto callback = m_listeners[i].callback;
-
-        // Don't hold mutex during callback execution!
-        lock.unlock();
-        callback(item);
-        lock.lock();
-      }
-    }
-  }
-
-done:
-  if (m_on_exit) m_on_exit();
+void Notifier::Start() {
+  DoStart();
 }
 
-int Notifier::AddListener(std::function<void(const RawEvent& event)> callback,
-                          int eventMask) {
-  Start();
-  auto thr = m_owner.GetThread();
-  return thr->m_listeners.emplace_back(callback, eventMask);
+unsigned int Notifier::Add(std::function<void(const RawEvent& event)> callback,
+                           int eventMask) {
+  return DoAdd(callback, eventMask);
 }
 
-void Notifier::RemoveListener(int uid) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_listeners.erase(uid);
+unsigned int Notifier::AddPolled(unsigned int pollerUid, int eventMask) {
+  return DoAdd(pollerUid, eventMask);
 }
 
-void Notifier::NotifySource(const wpi::Twine& name, CS_Source source,
+void Notifier::NotifySource(std::string_view name, CS_Source source,
                             CS_EventKind kind) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_notifications.emplace(name, source, static_cast<RawEvent::Kind>(kind));
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, name, source, static_cast<RawEvent::Kind>(kind));
 }
 
 void Notifier::NotifySource(const SourceImpl& source, CS_EventKind kind) {
@@ -164,38 +44,24 @@
 
 void Notifier::NotifySourceVideoMode(const SourceImpl& source,
                                      const VideoMode& mode) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-
   auto handleData = Instance::GetInstance().FindSource(source);
-
-  thr->m_notifications.emplace(source.GetName(), handleData.first, mode);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, source.GetName(), handleData.first, mode);
 }
 
 void Notifier::NotifySourceProperty(const SourceImpl& source, CS_EventKind kind,
-                                    const wpi::Twine& propertyName,
-                                    int property, CS_PropertyKind propertyKind,
-                                    int value, const wpi::Twine& valueStr) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-
+                                    std::string_view propertyName, int property,
+                                    CS_PropertyKind propertyKind, int value,
+                                    std::string_view valueStr) {
   auto handleData = Instance::GetInstance().FindSource(source);
-
-  thr->m_notifications.emplace(
-      propertyName, handleData.first, static_cast<RawEvent::Kind>(kind),
-      Handle{handleData.first, property, Handle::kProperty}, propertyKind,
-      value, valueStr);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, propertyName, handleData.first,
+       static_cast<RawEvent::Kind>(kind),
+       Handle{handleData.first, property, Handle::kProperty}, propertyKind,
+       value, valueStr);
 }
 
-void Notifier::NotifySink(const wpi::Twine& name, CS_Sink sink,
+void Notifier::NotifySink(std::string_view name, CS_Sink sink,
                           CS_EventKind kind) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-
-  thr->m_notifications.emplace(name, sink, static_cast<RawEvent::Kind>(kind));
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, name, sink, static_cast<RawEvent::Kind>(kind));
 }
 
 void Notifier::NotifySink(const SinkImpl& sink, CS_EventKind kind) {
@@ -203,46 +69,32 @@
   NotifySink(sink.GetName(), handleData.first, kind);
 }
 
-void Notifier::NotifySinkSourceChanged(const wpi::Twine& name, CS_Sink sink,
+void Notifier::NotifySinkSourceChanged(std::string_view name, CS_Sink sink,
                                        CS_Source source) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-
   RawEvent event{name, sink, RawEvent::kSinkSourceChanged};
   event.sourceHandle = source;
-
-  thr->m_notifications.emplace(std::move(event));
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, std::move(event));
 }
 
 void Notifier::NotifySinkProperty(const SinkImpl& sink, CS_EventKind kind,
-                                  const wpi::Twine& propertyName, int property,
+                                  std::string_view propertyName, int property,
                                   CS_PropertyKind propertyKind, int value,
-                                  const wpi::Twine& valueStr) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-
+                                  std::string_view valueStr) {
   auto handleData = Instance::GetInstance().FindSink(sink);
-
-  thr->m_notifications.emplace(
-      propertyName, handleData.first, static_cast<RawEvent::Kind>(kind),
-      Handle{handleData.first, property, Handle::kSinkProperty}, propertyKind,
-      value, valueStr);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, propertyName, handleData.first,
+       static_cast<RawEvent::Kind>(kind),
+       Handle{handleData.first, property, Handle::kSinkProperty}, propertyKind,
+       value, valueStr);
 }
 
 void Notifier::NotifyNetworkInterfacesChanged() {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-
-  thr->m_notifications.emplace(RawEvent::kNetworkInterfacesChanged);
-  thr->m_cond.notify_one();
+  Send(UINT_MAX, RawEvent::kNetworkInterfacesChanged);
 }
 
 void Notifier::NotifyTelemetryUpdated() {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
+  Send(UINT_MAX, RawEvent::kTelemetryUpdated);
+}
 
-  thr->m_notifications.emplace(RawEvent::kTelemetryUpdated);
-  thr->m_cond.notify_one();
+void Notifier::NotifyUsbCamerasChanged() {
+  Send(UINT_MAX, RawEvent::kUsbCamerasChanged);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.h b/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.h
index 526cea8..c28e25c 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Notifier.h
@@ -1,17 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_NOTIFIER_H_
 #define CSCORE_NOTIFIER_H_
 
 #include <functional>
+#include <utility>
 
-#include <wpi/SafeThread.h>
+#include <wpi/CallbackManager.h>
 
+#include "Handle.h"
 #include "cscore_cpp.h"
 
 namespace cs {
@@ -19,7 +18,43 @@
 class SinkImpl;
 class SourceImpl;
 
-class Notifier {
+namespace impl {
+
+struct ListenerData : public wpi::CallbackListenerData<
+                          std::function<void(const RawEvent& event)>> {
+  ListenerData() = default;
+  ListenerData(std::function<void(const RawEvent& event)> callback_,
+               int eventMask_)
+      : CallbackListenerData(std::move(callback_)), eventMask(eventMask_) {}
+  ListenerData(unsigned int pollerUid_, int eventMask_)
+      : CallbackListenerData(pollerUid_), eventMask(eventMask_) {}
+
+  int eventMask;
+};
+
+class NotifierThread
+    : public wpi::CallbackThread<NotifierThread, RawEvent, ListenerData> {
+ public:
+  NotifierThread(std::function<void()> on_start, std::function<void()> on_exit)
+      : CallbackThread(std::move(on_start), std::move(on_exit)) {}
+
+  bool Matches(const ListenerData& listener, const RawEvent& data) {
+    return (data.kind & listener.eventMask) != 0;
+  }
+
+  void SetListener(RawEvent* data, unsigned int listener_uid) {
+    data->listener = Handle(listener_uid, Handle::kListener);
+  }
+
+  void DoCallback(std::function<void(const RawEvent& event)> callback,
+                  const RawEvent& data) {
+    callback(data);
+  }
+};
+
+}  // namespace impl
+
+class Notifier : public wpi::CallbackManager<Notifier, impl::NotifierThread> {
   friend class NotifierTest;
 
  public:
@@ -27,44 +62,30 @@
   ~Notifier();
 
   void Start();
-  void Stop();
 
-  static bool destroyed() { return s_destroyed; }
-
-  void SetOnStart(std::function<void()> on_start) { m_on_start = on_start; }
-  void SetOnExit(std::function<void()> on_exit) { m_on_exit = on_exit; }
-
-  int AddListener(std::function<void(const RawEvent& event)> callback,
-                  int eventMask);
-  void RemoveListener(int uid);
+  unsigned int Add(std::function<void(const RawEvent& event)> callback,
+                   int eventMask);
+  unsigned int AddPolled(unsigned int pollerUid, int eventMask);
 
   // Notification events
-  void NotifySource(const wpi::Twine& name, CS_Source source,
-                    CS_EventKind kind);
+  void NotifySource(std::string_view name, CS_Source source, CS_EventKind kind);
   void NotifySource(const SourceImpl& source, CS_EventKind kind);
   void NotifySourceVideoMode(const SourceImpl& source, const VideoMode& mode);
   void NotifySourceProperty(const SourceImpl& source, CS_EventKind kind,
-                            const wpi::Twine& propertyName, int property,
+                            std::string_view propertyName, int property,
                             CS_PropertyKind propertyKind, int value,
-                            const wpi::Twine& valueStr);
-  void NotifySink(const wpi::Twine& name, CS_Sink sink, CS_EventKind kind);
+                            std::string_view valueStr);
+  void NotifySink(std::string_view name, CS_Sink sink, CS_EventKind kind);
   void NotifySink(const SinkImpl& sink, CS_EventKind kind);
-  void NotifySinkSourceChanged(const wpi::Twine& name, CS_Sink sink,
+  void NotifySinkSourceChanged(std::string_view name, CS_Sink sink,
                                CS_Source source);
   void NotifySinkProperty(const SinkImpl& sink, CS_EventKind kind,
-                          const wpi::Twine& propertyName, int property,
+                          std::string_view propertyName, int property,
                           CS_PropertyKind propertyKind, int value,
-                          const wpi::Twine& valueStr);
+                          std::string_view valueStr);
   void NotifyNetworkInterfacesChanged();
   void NotifyTelemetryUpdated();
-
- private:
-  class Thread;
-  wpi::SafeThreadOwner<Thread> m_owner;
-
-  std::function<void()> m_on_start;
-  std::function<void()> m_on_exit;
-  static bool s_destroyed;
+  void NotifyUsbCamerasChanged();
 };
 
 }  // namespace cs
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.cpp
index 17bf94b..b1d3a6f 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "PropertyContainer.h"
 
@@ -14,13 +11,14 @@
 
 using namespace cs;
 
-int PropertyContainer::GetPropertyIndex(const wpi::Twine& name) const {
+int PropertyContainer::GetPropertyIndex(std::string_view name) const {
   // We can't fail, so instead we create a new index if caching fails.
   CS_Status status = 0;
-  if (!m_properties_cached) CacheProperties(&status);
+  if (!m_properties_cached) {
+    CacheProperties(&status);
+  }
   std::scoped_lock lock(m_mutex);
-  wpi::SmallVector<char, 64> nameBuf;
-  int& ndx = m_properties[name.toStringRef(nameBuf)];
+  int& ndx = m_properties[name];
   if (ndx == 0) {
     // create a new index
     ndx = m_propertyData.size() + 1;
@@ -29,41 +27,52 @@
   return ndx;
 }
 
-wpi::ArrayRef<int> PropertyContainer::EnumerateProperties(
+wpi::span<int> PropertyContainer::EnumerateProperties(
     wpi::SmallVectorImpl<int>& vec, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status))
-    return wpi::ArrayRef<int>{};
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return {};
+  }
   std::scoped_lock lock(m_mutex);
   for (int i = 0; i < static_cast<int>(m_propertyData.size()); ++i) {
-    if (m_propertyData[i]) vec.push_back(i + 1);
+    if (m_propertyData[i]) {
+      vec.push_back(i + 1);
+    }
   }
   return vec;
 }
 
 CS_PropertyKind PropertyContainer::GetPropertyKind(int property) const {
   CS_Status status = 0;
-  if (!m_properties_cached && !CacheProperties(&status)) return CS_PROP_NONE;
+  if (!m_properties_cached && !CacheProperties(&status)) {
+    return CS_PROP_NONE;
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
-  if (!prop) return CS_PROP_NONE;
+  if (!prop) {
+    return CS_PROP_NONE;
+  }
   return prop->propKind;
 }
 
-wpi::StringRef PropertyContainer::GetPropertyName(
+std::string_view PropertyContainer::GetPropertyName(
     int property, wpi::SmallVectorImpl<char>& buf, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return wpi::StringRef{};
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return {};
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
     *status = CS_INVALID_PROPERTY;
-    return wpi::StringRef{};
+    return {};
   }
   // safe to not copy because we never modify it after caching
   return prop->name;
 }
 
 int PropertyContainer::GetProperty(int property, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return 0;
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return 0;
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
@@ -88,7 +97,9 @@
   }
 
   // Guess it's integer if we've set before get
-  if (prop->propKind == CS_PROP_NONE) prop->propKind = CS_PROP_INTEGER;
+  if (prop->propKind == CS_PROP_NONE) {
+    prop->propKind = CS_PROP_INTEGER;
+  }
 
   if ((prop->propKind & (CS_PROP_BOOLEAN | CS_PROP_INTEGER | CS_PROP_ENUM)) ==
       0) {
@@ -96,11 +107,13 @@
     return;
   }
 
-  UpdatePropertyValue(property, false, value, wpi::Twine{});
+  UpdatePropertyValue(property, false, value, {});
 }
 
 int PropertyContainer::GetPropertyMin(int property, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return 0;
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return 0;
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
@@ -111,7 +124,9 @@
 }
 
 int PropertyContainer::GetPropertyMax(int property, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return 0;
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return 0;
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
@@ -122,7 +137,9 @@
 }
 
 int PropertyContainer::GetPropertyStep(int property, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return 0;
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return 0;
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
@@ -134,7 +151,9 @@
 
 int PropertyContainer::GetPropertyDefault(int property,
                                           CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return 0;
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return 0;
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
@@ -144,25 +163,27 @@
   return prop->defaultValue;
 }
 
-wpi::StringRef PropertyContainer::GetStringProperty(
+std::string_view PropertyContainer::GetStringProperty(
     int property, wpi::SmallVectorImpl<char>& buf, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return wpi::StringRef{};
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return {};
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
     *status = CS_INVALID_PROPERTY;
-    return wpi::StringRef{};
+    return {};
   }
   if (prop->propKind != CS_PROP_STRING) {
     *status = CS_WRONG_PROPERTY_TYPE;
-    return wpi::StringRef{};
+    return {};
   }
   buf.clear();
   buf.append(prop->valueStr.begin(), prop->valueStr.end());
-  return wpi::StringRef(buf.data(), buf.size());
+  return {buf.data(), buf.size()};
 }
 
-void PropertyContainer::SetStringProperty(int property, const wpi::Twine& value,
+void PropertyContainer::SetStringProperty(int property, std::string_view value,
                                           CS_Status* status) {
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
@@ -172,7 +193,9 @@
   }
 
   // Guess it's string if we've set before get
-  if (prop->propKind == CS_PROP_NONE) prop->propKind = CS_PROP_STRING;
+  if (prop->propKind == CS_PROP_NONE) {
+    prop->propKind = CS_PROP_STRING;
+  }
 
   if (prop->propKind != CS_PROP_STRING) {
     *status = CS_WRONG_PROPERTY_TYPE;
@@ -184,23 +207,24 @@
 
 std::vector<std::string> PropertyContainer::GetEnumPropertyChoices(
     int property, CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status))
-    return std::vector<std::string>{};
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return {};
+  }
   std::scoped_lock lock(m_mutex);
   auto prop = GetProperty(property);
   if (!prop) {
     *status = CS_INVALID_PROPERTY;
-    return std::vector<std::string>{};
+    return {};
   }
   if (prop->propKind != CS_PROP_ENUM) {
     *status = CS_WRONG_PROPERTY_TYPE;
-    return std::vector<std::string>{};
+    return {};
   }
   return prop->enumChoices;
 }
 
 std::unique_ptr<PropertyImpl> PropertyContainer::CreateEmptyProperty(
-    const wpi::Twine& name) const {
+    std::string_view name) const {
   return std::make_unique<PropertyImpl>(name);
 }
 
@@ -212,16 +236,15 @@
 
 bool PropertyContainer::SetPropertiesJson(const wpi::json& config,
                                           wpi::Logger& logger,
-                                          wpi::StringRef logName,
+                                          std::string_view logName,
                                           CS_Status* status) {
   for (auto&& prop : config) {
     std::string name;
     try {
       name = prop.at("name").get<std::string>();
     } catch (const wpi::json::exception& e) {
-      WPI_WARNING(logger,
-                  logName << ": SetConfigJson: could not read property name: "
-                          << e.what());
+      WPI_WARNING(logger, "{}: SetConfigJson: could not read property name: {}",
+                  logName, e.what());
       continue;
     }
     int n = GetPropertyIndex(name);
@@ -229,24 +252,24 @@
       auto& v = prop.at("value");
       if (v.is_string()) {
         std::string val = v.get<std::string>();
-        WPI_INFO(logger, logName << ": SetConfigJson: setting property '"
-                                 << name << "' to '" << val << '\'');
+        WPI_INFO(logger, "{}: SetConfigJson: setting property '{}' to '{}'",
+                 logName, name, val);
         SetStringProperty(n, val, status);
       } else if (v.is_boolean()) {
         bool val = v.get<bool>();
-        WPI_INFO(logger, logName << ": SetConfigJson: setting property '"
-                                 << name << "' to " << val);
+        WPI_INFO(logger, "{}: SetConfigJson: setting property '{}' to {}",
+                 logName, name, val);
         SetProperty(n, val, status);
       } else {
         int val = v.get<int>();
-        WPI_INFO(logger, logName << ": SetConfigJson: setting property '"
-                                 << name << "' to " << val);
+        WPI_INFO(logger, "{}: SetConfigJson: setting property '{}' to {}",
+                 logName, name, val);
         SetProperty(n, val, status);
       }
     } catch (const wpi::json::exception& e) {
       WPI_WARNING(logger,
-                  logName << ": SetConfigJson: could not read property value: "
-                          << e.what());
+                  "{}: SetConfigJson: could not read property value: {}",
+                  logName, e.what());
       continue;
     }
   }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.h b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.h
index 9bbb9c7..a00c675 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_PROPERTYCONTAINER_H_
 #define CSCORE_PROPERTYCONTAINER_H_
@@ -12,20 +9,20 @@
 #include <cstddef>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/SmallVector.h>
 #include <wpi/StringMap.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
 #include <wpi/mutex.h>
+#include <wpi/span.h>
 
 #include "PropertyImpl.h"
 #include "cscore_cpp.h"
 
 namespace wpi {
 class Logger;
+template <typename T>
+class SmallVectorImpl;
 class json;
 }  // namespace wpi
 
@@ -35,50 +32,54 @@
  public:
   virtual ~PropertyContainer() = default;
 
-  int GetPropertyIndex(const wpi::Twine& name) const;
-  wpi::ArrayRef<int> EnumerateProperties(wpi::SmallVectorImpl<int>& vec,
-                                         CS_Status* status) const;
+  int GetPropertyIndex(std::string_view name) const;
+  wpi::span<int> EnumerateProperties(wpi::SmallVectorImpl<int>& vec,
+                                     CS_Status* status) const;
   CS_PropertyKind GetPropertyKind(int property) const;
-  wpi::StringRef GetPropertyName(int property, wpi::SmallVectorImpl<char>& buf,
-                                 CS_Status* status) const;
+  std::string_view GetPropertyName(int property,
+                                   wpi::SmallVectorImpl<char>& buf,
+                                   CS_Status* status) const;
   int GetProperty(int property, CS_Status* status) const;
   virtual void SetProperty(int property, int value, CS_Status* status);
   int GetPropertyMin(int property, CS_Status* status) const;
   int GetPropertyMax(int property, CS_Status* status) const;
   int GetPropertyStep(int property, CS_Status* status) const;
   int GetPropertyDefault(int property, CS_Status* status) const;
-  wpi::StringRef GetStringProperty(int property,
-                                   wpi::SmallVectorImpl<char>& buf,
-                                   CS_Status* status) const;
-  virtual void SetStringProperty(int property, const wpi::Twine& value,
+  std::string_view GetStringProperty(int property,
+                                     wpi::SmallVectorImpl<char>& buf,
+                                     CS_Status* status) const;
+  virtual void SetStringProperty(int property, std::string_view value,
                                  CS_Status* status);
   std::vector<std::string> GetEnumPropertyChoices(int property,
                                                   CS_Status* status) const;
 
   bool SetPropertiesJson(const wpi::json& config, wpi::Logger& logger,
-                         wpi::StringRef logName, CS_Status* status);
+                         std::string_view logName, CS_Status* status);
   wpi::json GetPropertiesJsonObject(CS_Status* status);
 
  protected:
   // Get a property; must be called with m_mutex held.
   PropertyImpl* GetProperty(int property) {
-    if (property <= 0 || static_cast<size_t>(property) > m_propertyData.size())
+    if (property <= 0 ||
+        static_cast<size_t>(property) > m_propertyData.size()) {
       return nullptr;
+    }
     return m_propertyData[property - 1].get();
   }
   const PropertyImpl* GetProperty(int property) const {
-    if (property <= 0 || static_cast<size_t>(property) > m_propertyData.size())
+    if (property <= 0 ||
+        static_cast<size_t>(property) > m_propertyData.size()) {
       return nullptr;
+    }
     return m_propertyData[property - 1].get();
   }
   // Create or update a property; must be called with m_mutex held.
   // @tparam NewFunc functor that returns a std::unique_ptr<PropertyImpl>
   // @tparam UpdateFunc functor that takes a PropertyImpl&.
   template <typename NewFunc, typename UpdateFunc>
-  int CreateOrUpdateProperty(const wpi::Twine& name, NewFunc newFunc,
+  int CreateOrUpdateProperty(std::string_view name, NewFunc newFunc,
                              UpdateFunc updateFunc) {
-    wpi::SmallVector<char, 64> nameBuf;
-    int& ndx = m_properties[name.toStringRef(nameBuf)];
+    int& ndx = m_properties[name];
     if (ndx == 0) {
       // create a new index
       ndx = m_propertyData.size() + 1;
@@ -90,7 +91,7 @@
     return ndx;
   }
   template <typename NewFunc>
-  int CreateProperty(const wpi::Twine& name, NewFunc newFunc) {
+  int CreateProperty(std::string_view name, NewFunc newFunc) {
     return CreateOrUpdateProperty(name, newFunc, [](PropertyImpl&) {});
   }
 
@@ -99,7 +100,7 @@
   // Note: called with m_mutex held.
   // The default implementation simply creates a PropertyImpl object.
   virtual std::unique_ptr<PropertyImpl> CreateEmptyProperty(
-      const wpi::Twine& name) const;
+      std::string_view name) const;
 
   // Cache properties.  Implementations must return false and set status to
   // CS_SOURCE_IS_DISCONNECTED if not possible to cache.
@@ -110,7 +111,7 @@
 
   // Update property value; must be called with m_mutex held.
   virtual void UpdatePropertyValue(int property, bool setString, int value,
-                                   const wpi::Twine& valueStr) = 0;
+                                   std::string_view valueStr) = 0;
 
   // Whether CacheProperties() has been successful at least once (and thus
   // should not be called again)
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.cpp
index 4f1602e..3c36630 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.cpp
@@ -1,26 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "PropertyImpl.h"
 
 using namespace cs;
 
-PropertyImpl::PropertyImpl(const wpi::Twine& name_) : name{name_.str()} {}
-PropertyImpl::PropertyImpl(const wpi::Twine& name_, CS_PropertyKind kind_,
+PropertyImpl::PropertyImpl(std::string_view name_) : name{name_} {}
+PropertyImpl::PropertyImpl(std::string_view name_, CS_PropertyKind kind_,
                            int step_, int defaultValue_, int value_)
-    : name{name_.str()},
+    : name{name_},
       propKind{kind_},
       step{step_},
       defaultValue{defaultValue_},
       value{value_} {}
-PropertyImpl::PropertyImpl(const wpi::Twine& name_, CS_PropertyKind kind_,
+PropertyImpl::PropertyImpl(std::string_view name_, CS_PropertyKind kind_,
                            int minimum_, int maximum_, int step_,
                            int defaultValue_, int value_)
-    : name{name_.str()},
+    : name{name_},
       propKind{kind_},
       hasMinimum{true},
       hasMaximum{true},
@@ -32,34 +29,39 @@
 
 void PropertyImpl::SetValue(int v) {
   int oldValue = value;
-  if (hasMinimum && v < minimum)
+  if (hasMinimum && v < minimum) {
     value = minimum;
-  else if (hasMaximum && v > maximum)
+  } else if (hasMaximum && v > maximum) {
     value = maximum;
-  else
+  } else {
     value = v;
+  }
   bool wasValueSet = valueSet;
   valueSet = true;
-  if (!wasValueSet || value != oldValue) changed();
+  if (!wasValueSet || value != oldValue) {
+    changed();
+  }
 }
 
-void PropertyImpl::SetValue(const wpi::Twine& v) {
+void PropertyImpl::SetValue(std::string_view v) {
   bool valueChanged = false;
-  std::string vStr = v.str();
-  if (valueStr != vStr) {
-    valueStr = vStr;
+  if (valueStr != v) {
+    valueStr = v;
     valueChanged = true;
   }
   bool wasValueSet = valueSet;
   valueSet = true;
-  if (!wasValueSet || valueChanged) changed();
+  if (!wasValueSet || valueChanged) {
+    changed();
+  }
 }
 
 void PropertyImpl::SetDefaultValue(int v) {
-  if (hasMinimum && v < minimum)
+  if (hasMinimum && v < minimum) {
     defaultValue = minimum;
-  else if (hasMaximum && v > maximum)
+  } else if (hasMaximum && v > maximum) {
     defaultValue = maximum;
-  else
+  } else {
     defaultValue = v;
+  }
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.h
index d932132..77e7908 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/PropertyImpl.h
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_PROPERTYIMPL_H_
 #define CSCORE_PROPERTYIMPL_H_
 
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include <wpi/Signal.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
 
 #include "cscore_c.h"
 
@@ -23,17 +19,17 @@
 class PropertyImpl {
  public:
   PropertyImpl() = default;
-  explicit PropertyImpl(const wpi::Twine& name_);
-  PropertyImpl(const wpi::Twine& name_, CS_PropertyKind kind_, int step_,
+  explicit PropertyImpl(std::string_view name_);
+  PropertyImpl(std::string_view name_, CS_PropertyKind kind_, int step_,
                int defaultValue_, int value_);
-  PropertyImpl(const wpi::Twine& name_, CS_PropertyKind kind_, int minimum_,
+  PropertyImpl(std::string_view name_, CS_PropertyKind kind_, int minimum_,
                int maximum_, int step_, int defaultValue_, int value_);
   virtual ~PropertyImpl() = default;
   PropertyImpl(const PropertyImpl& oth) = delete;
   PropertyImpl& operator=(const PropertyImpl& oth) = delete;
 
   void SetValue(int v);
-  void SetValue(const wpi::Twine& v);
+  void SetValue(std::string_view v);
   void SetDefaultValue(int v);
 
   std::string name;
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp
index 986378f..fbc1028 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RawSinkImpl.h"
 
@@ -13,28 +10,34 @@
 
 using namespace cs;
 
-RawSinkImpl::RawSinkImpl(const wpi::Twine& name, wpi::Logger& logger,
+RawSinkImpl::RawSinkImpl(std::string_view name, wpi::Logger& logger,
                          Notifier& notifier, Telemetry& telemetry)
     : SinkImpl{name, logger, notifier, telemetry} {
   m_active = true;
   // m_thread = std::thread(&RawSinkImpl::ThreadMain, this);
 }
 
-RawSinkImpl::RawSinkImpl(const wpi::Twine& name, wpi::Logger& logger,
+RawSinkImpl::RawSinkImpl(std::string_view name, wpi::Logger& logger,
                          Notifier& notifier, Telemetry& telemetry,
                          std::function<void(uint64_t time)> processFrame)
     : SinkImpl{name, logger, notifier, telemetry} {}
 
-RawSinkImpl::~RawSinkImpl() { Stop(); }
+RawSinkImpl::~RawSinkImpl() {
+  Stop();
+}
 
 void RawSinkImpl::Stop() {
   m_active = false;
 
   // wake up any waiters by forcing an empty frame to be sent
-  if (auto source = GetSource()) source->Wakeup();
+  if (auto source = GetSource()) {
+    source->Wakeup();
+  }
 
   // join thread
-  if (m_thread.joinable()) m_thread.join();
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
 }
 
 uint64_t RawSinkImpl::GrabFrame(CS_RawFrame& image) {
@@ -124,9 +127,11 @@
       std::this_thread::sleep_for(std::chrono::seconds(1));
       continue;
     }
-    SDEBUG4("waiting for frame");
+    SDEBUG4("{}", "waiting for frame");
     Frame frame = source->GetNextFrame();  // blocks
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
     if (!frame) {
       // Bad frame; sleep for 10 ms so we don't consume all processor time.
       std::this_thread::sleep_for(std::chrono::milliseconds(10));
@@ -138,14 +143,14 @@
 }
 
 namespace cs {
-CS_Sink CreateRawSink(const wpi::Twine& name, CS_Status* status) {
+CS_Sink CreateRawSink(std::string_view name, CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSink(CS_SINK_RAW,
                          std::make_shared<RawSinkImpl>(
                              name, inst.logger, inst.notifier, inst.telemetry));
 }
 
-CS_Sink CreateRawSinkCallback(const wpi::Twine& name,
+CS_Sink CreateRawSinkCallback(std::string_view name,
                               std::function<void(uint64_t time)> processFrame,
                               CS_Status* status) {
   auto& inst = Instance::GetInstance();
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h
index 3e69485..6e8032a 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSinkImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_RAWSINKIMPL_H_
 #define CSCORE_RAWSINKIMPL_H_
@@ -12,9 +9,9 @@
 
 #include <atomic>
 #include <functional>
+#include <string_view>
 #include <thread>
 
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 
 #include "Frame.h"
@@ -26,9 +23,9 @@
 
 class RawSinkImpl : public SinkImpl {
  public:
-  RawSinkImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  RawSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
               Telemetry& telemetry);
-  RawSinkImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  RawSinkImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
               Telemetry& telemetry,
               std::function<void(uint64_t time)> processFrame);
   ~RawSinkImpl() override;
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp
index e0dba2d..9ce0628 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RawSourceImpl.h"
 
@@ -17,12 +14,12 @@
 
 using namespace cs;
 
-RawSourceImpl::RawSourceImpl(const wpi::Twine& name, wpi::Logger& logger,
+RawSourceImpl::RawSourceImpl(std::string_view name, wpi::Logger& logger,
                              Notifier& notifier, Telemetry& telemetry,
                              const VideoMode& mode)
     : ConfigurableSourceImpl{name, logger, notifier, telemetry, mode} {}
 
-RawSourceImpl::~RawSourceImpl() {}
+RawSourceImpl::~RawSourceImpl() = default;
 
 void RawSourceImpl::PutFrame(const CS_RawFrame& image) {
   int type;
@@ -50,7 +47,7 @@
 }
 
 namespace cs {
-CS_Source CreateRawSource(const wpi::Twine& name, const VideoMode& mode,
+CS_Source CreateRawSource(std::string_view name, const VideoMode& mode,
                           CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSource(CS_SOURCE_RAW, std::make_shared<RawSourceImpl>(
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h
index 1fdc749..5887ed4 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/RawSourceImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_RAWSOURCEIMPL_H_
 #define CSCORE_RAWSOURCEIMPL_H_
@@ -12,11 +9,9 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/Twine.h>
-
 #include "ConfigurableSourceImpl.h"
 #include "SourceImpl.h"
 #include "cscore_raw.h"
@@ -25,7 +20,7 @@
 
 class RawSourceImpl : public ConfigurableSourceImpl {
  public:
-  RawSourceImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  RawSourceImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
                 Telemetry& telemetry, const VideoMode& mode);
   ~RawSourceImpl() override;
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.cpp
index 5d4235a..93a625f 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SinkImpl.h"
 
@@ -15,36 +12,41 @@
 
 using namespace cs;
 
-SinkImpl::SinkImpl(const wpi::Twine& name, wpi::Logger& logger,
+SinkImpl::SinkImpl(std::string_view name, wpi::Logger& logger,
                    Notifier& notifier, Telemetry& telemetry)
     : m_logger(logger),
       m_notifier(notifier),
       m_telemetry(telemetry),
-      m_name{name.str()} {}
+      m_name{name} {}
 
 SinkImpl::~SinkImpl() {
   if (m_source) {
-    if (m_enabledCount > 0) m_source->DisableSink();
+    if (m_enabledCount > 0) {
+      m_source->DisableSink();
+    }
     m_source->RemoveSink();
   }
 }
 
-void SinkImpl::SetDescription(const wpi::Twine& description) {
+void SinkImpl::SetDescription(std::string_view description) {
   std::scoped_lock lock(m_mutex);
-  m_description = description.str();
+  m_description = description;
 }
 
-wpi::StringRef SinkImpl::GetDescription(wpi::SmallVectorImpl<char>& buf) const {
+std::string_view SinkImpl::GetDescription(
+    wpi::SmallVectorImpl<char>& buf) const {
   std::scoped_lock lock(m_mutex);
   buf.append(m_description.begin(), m_description.end());
-  return wpi::StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
 void SinkImpl::Enable() {
   std::scoped_lock lock(m_mutex);
   ++m_enabledCount;
   if (m_enabledCount == 1) {
-    if (m_source) m_source->EnableSink();
+    if (m_source) {
+      m_source->EnableSink();
+    }
     m_notifier.NotifySink(*this, CS_SINK_ENABLED);
   }
 }
@@ -53,7 +55,9 @@
   std::scoped_lock lock(m_mutex);
   --m_enabledCount;
   if (m_enabledCount == 0) {
-    if (m_source) m_source->DisableSink();
+    if (m_source) {
+      m_source->DisableSink();
+    }
     m_notifier.NotifySink(*this, CS_SINK_DISABLED);
   }
 }
@@ -61,11 +65,15 @@
 void SinkImpl::SetEnabled(bool enabled) {
   std::scoped_lock lock(m_mutex);
   if (enabled && m_enabledCount == 0) {
-    if (m_source) m_source->EnableSink();
+    if (m_source) {
+      m_source->EnableSink();
+    }
     m_enabledCount = 1;
     m_notifier.NotifySink(*this, CS_SINK_ENABLED);
   } else if (!enabled && m_enabledCount > 0) {
-    if (m_source) m_source->DisableSink();
+    if (m_source) {
+      m_source->DisableSink();
+    }
     m_enabledCount = 0;
     m_notifier.NotifySink(*this, CS_SINK_DISABLED);
   }
@@ -74,15 +82,21 @@
 void SinkImpl::SetSource(std::shared_ptr<SourceImpl> source) {
   {
     std::scoped_lock lock(m_mutex);
-    if (m_source == source) return;
+    if (m_source == source) {
+      return;
+    }
     if (m_source) {
-      if (m_enabledCount > 0) m_source->DisableSink();
+      if (m_enabledCount > 0) {
+        m_source->DisableSink();
+      }
       m_source->RemoveSink();
     }
     m_source = source;
     if (m_source) {
       m_source->AddSink();
-      if (m_enabledCount > 0) m_source->EnableSink();
+      if (m_enabledCount > 0) {
+        m_source->EnableSink();
+      }
     }
   }
   SetSourceImpl(source);
@@ -90,27 +104,30 @@
 
 std::string SinkImpl::GetError() const {
   std::scoped_lock lock(m_mutex);
-  if (!m_source) return "no source connected";
-  return m_source->GetCurFrame().GetError();
+  if (!m_source) {
+    return "no source connected";
+  }
+  return std::string{m_source->GetCurFrame().GetError()};
 }
 
-wpi::StringRef SinkImpl::GetError(wpi::SmallVectorImpl<char>& buf) const {
+std::string_view SinkImpl::GetError(wpi::SmallVectorImpl<char>& buf) const {
   std::scoped_lock lock(m_mutex);
-  if (!m_source) return "no source connected";
+  if (!m_source) {
+    return "no source connected";
+  }
   // Make a copy as it's shared data
-  wpi::StringRef error = m_source->GetCurFrame().GetError();
+  std::string_view error = m_source->GetCurFrame().GetError();
   buf.clear();
   buf.append(error.data(), error.data() + error.size());
-  return wpi::StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
-bool SinkImpl::SetConfigJson(wpi::StringRef config, CS_Status* status) {
+bool SinkImpl::SetConfigJson(std::string_view config, CS_Status* status) {
   wpi::json j;
   try {
     j = wpi::json::parse(config);
   } catch (const wpi::json::parse_error& e) {
-    SWARNING("SetConfigJson: parse error at byte " << e.byte << ": "
-                                                   << e.what());
+    SWARNING("SetConfigJson: parse error at byte {}: {}", e.byte, e.what());
     *status = CS_PROPERTY_WRITE_FAILED;
     return false;
   }
@@ -118,8 +135,9 @@
 }
 
 bool SinkImpl::SetConfigJson(const wpi::json& config, CS_Status* status) {
-  if (config.count("properties") != 0)
+  if (config.count("properties") != 0) {
     SetPropertiesJson(config.at("properties"), m_logger, GetName(), status);
+  }
 
   return true;
 }
@@ -136,7 +154,9 @@
   wpi::json j;
 
   wpi::json props = GetPropertiesJsonObject(status);
-  if (props.is_array()) j.emplace("properties", props);
+  if (props.is_array()) {
+    j.emplace("properties", props);
+  }
 
   return j;
 }
@@ -146,21 +166,25 @@
                                 propIndex, prop.propKind, prop.value,
                                 prop.valueStr);
   // also notify choices updated event for enum types
-  if (prop.propKind == CS_PROP_ENUM)
+  if (prop.propKind == CS_PROP_ENUM) {
     m_notifier.NotifySinkProperty(*this, CS_SINK_PROPERTY_CHOICES_UPDATED,
                                   prop.name, propIndex, prop.propKind,
-                                  prop.value, wpi::Twine{});
+                                  prop.value, {});
+  }
 }
 
 void SinkImpl::UpdatePropertyValue(int property, bool setString, int value,
-                                   const wpi::Twine& valueStr) {
+                                   std::string_view valueStr) {
   auto prop = GetProperty(property);
-  if (!prop) return;
+  if (!prop) {
+    return;
+  }
 
-  if (setString)
+  if (setString) {
     prop->SetValue(valueStr);
-  else
+  } else {
     prop->SetValue(value);
+  }
 
   // Only notify updates after we've notified created
   if (m_properties_cached) {
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.h
index 7ad831f..aa37d61 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/SinkImpl.h
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_SINKIMPL_H_
 #define CSCORE_SINKIMPL_H_
 
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <wpi/Logger.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
 #include <wpi/mutex.h>
 
 #include "SourceImpl.h"
@@ -30,16 +26,16 @@
 
 class SinkImpl : public PropertyContainer {
  public:
-  explicit SinkImpl(const wpi::Twine& name, wpi::Logger& logger,
+  explicit SinkImpl(std::string_view name, wpi::Logger& logger,
                     Notifier& notifier, Telemetry& telemetry);
-  virtual ~SinkImpl();
+  ~SinkImpl() override;
   SinkImpl(const SinkImpl& queue) = delete;
   SinkImpl& operator=(const SinkImpl& queue) = delete;
 
-  wpi::StringRef GetName() const { return m_name; }
+  std::string_view GetName() const { return m_name; }
 
-  void SetDescription(const wpi::Twine& description);
-  wpi::StringRef GetDescription(wpi::SmallVectorImpl<char>& buf) const;
+  void SetDescription(std::string_view description);
+  std::string_view GetDescription(wpi::SmallVectorImpl<char>& buf) const;
 
   void Enable();
   void Disable();
@@ -53,9 +49,9 @@
   }
 
   std::string GetError() const;
-  wpi::StringRef GetError(wpi::SmallVectorImpl<char>& buf) const;
+  std::string_view GetError(wpi::SmallVectorImpl<char>& buf) const;
 
-  bool SetConfigJson(wpi::StringRef config, CS_Status* status);
+  bool SetConfigJson(std::string_view config, CS_Status* status);
   virtual bool SetConfigJson(const wpi::json& config, CS_Status* status);
   std::string GetConfigJson(CS_Status* status);
   virtual wpi::json GetConfigJsonObject(CS_Status* status);
@@ -64,7 +60,7 @@
   // PropertyContainer implementation
   void NotifyPropertyCreated(int propIndex, PropertyImpl& prop) override;
   void UpdatePropertyValue(int property, bool setString, int value,
-                           const wpi::Twine& valueStr) override;
+                           std::string_view valueStr) override;
 
   virtual void SetSourceImpl(std::shared_ptr<SourceImpl> source);
 
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.cpp
index 455b6cd..da5aa1d 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.cpp
@@ -1,15 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SourceImpl.h"
 
 #include <algorithm>
 #include <cstring>
+#include <memory>
 
+#include <wpi/StringExtras.h>
 #include <wpi/json.h>
 #include <wpi/timestamp.h>
 
@@ -21,13 +20,13 @@
 
 static constexpr size_t kMaxImagesAvail = 32;
 
-SourceImpl::SourceImpl(const wpi::Twine& name, wpi::Logger& logger,
+SourceImpl::SourceImpl(std::string_view name, wpi::Logger& logger,
                        Notifier& notifier, Telemetry& telemetry)
     : m_logger(logger),
       m_notifier(notifier),
       m_telemetry(telemetry),
-      m_name{name.str()} {
-  m_frame = Frame{*this, wpi::StringRef{}, 0};
+      m_name{name} {
+  m_frame = Frame{*this, std::string_view{}, 0};
 }
 
 SourceImpl::~SourceImpl() {
@@ -43,24 +42,25 @@
   // Everything else can clean up itself.
 }
 
-void SourceImpl::SetDescription(const wpi::Twine& description) {
+void SourceImpl::SetDescription(std::string_view description) {
   std::scoped_lock lock(m_mutex);
-  m_description = description.str();
+  m_description = description;
 }
 
-wpi::StringRef SourceImpl::GetDescription(
+std::string_view SourceImpl::GetDescription(
     wpi::SmallVectorImpl<char>& buf) const {
   std::scoped_lock lock(m_mutex);
   buf.append(m_description.begin(), m_description.end());
-  return wpi::StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
 void SourceImpl::SetConnected(bool connected) {
   bool wasConnected = m_connected.exchange(connected);
-  if (wasConnected && !connected)
+  if (wasConnected && !connected) {
     m_notifier.NotifySource(*this, CS_SOURCE_DISCONNECTED);
-  else if (!wasConnected && connected)
+  } else if (!wasConnected && connected) {
     m_notifier.NotifySource(*this, CS_SOURCE_CONNECTED);
+  }
 }
 
 uint64_t SourceImpl::GetCurFrameTime() {
@@ -94,7 +94,7 @@
 void SourceImpl::Wakeup() {
   {
     std::scoped_lock lock{m_frameMutex};
-    m_frame = Frame{*this, wpi::StringRef{}, 0};
+    m_frame = Frame{*this, std::string_view{}, 0};
   }
   m_frameCv.notify_all();
 }
@@ -133,7 +133,9 @@
 }
 
 VideoMode SourceImpl::GetVideoMode(CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status)) return VideoMode{};
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return {};
+  }
   std::scoped_lock lock(m_mutex);
   return m_mode;
 }
@@ -141,14 +143,18 @@
 bool SourceImpl::SetPixelFormat(VideoMode::PixelFormat pixelFormat,
                                 CS_Status* status) {
   auto mode = GetVideoMode(status);
-  if (!mode) return false;
+  if (!mode) {
+    return false;
+  }
   mode.pixelFormat = pixelFormat;
   return SetVideoMode(mode, status);
 }
 
 bool SourceImpl::SetResolution(int width, int height, CS_Status* status) {
   auto mode = GetVideoMode(status);
-  if (!mode) return false;
+  if (!mode) {
+    return false;
+  }
   mode.width = width;
   mode.height = height;
   return SetVideoMode(mode, status);
@@ -156,18 +162,19 @@
 
 bool SourceImpl::SetFPS(int fps, CS_Status* status) {
   auto mode = GetVideoMode(status);
-  if (!mode) return false;
+  if (!mode) {
+    return false;
+  }
   mode.fps = fps;
   return SetVideoMode(mode, status);
 }
 
-bool SourceImpl::SetConfigJson(wpi::StringRef config, CS_Status* status) {
+bool SourceImpl::SetConfigJson(std::string_view config, CS_Status* status) {
   wpi::json j;
   try {
     j = wpi::json::parse(config);
   } catch (const wpi::json::parse_error& e) {
-    SWARNING("SetConfigJson: parse error at byte " << e.byte << ": "
-                                                   << e.what());
+    SWARNING("SetConfigJson: parse error at byte {}: {}", e.byte, e.what());
     *status = CS_PROPERTY_WRITE_FAILED;
     return false;
   }
@@ -181,23 +188,22 @@
   if (config.count("pixel format") != 0) {
     try {
       auto str = config.at("pixel format").get<std::string>();
-      wpi::StringRef s(str);
-      if (s.equals_lower("mjpeg")) {
+      if (wpi::equals_lower(str, "mjpeg")) {
         mode.pixelFormat = cs::VideoMode::kMJPEG;
-      } else if (s.equals_lower("yuyv")) {
+      } else if (wpi::equals_lower(str, "yuyv")) {
         mode.pixelFormat = cs::VideoMode::kYUYV;
-      } else if (s.equals_lower("rgb565")) {
+      } else if (wpi::equals_lower(str, "rgb565")) {
         mode.pixelFormat = cs::VideoMode::kRGB565;
-      } else if (s.equals_lower("bgr")) {
+      } else if (wpi::equals_lower(str, "bgr")) {
         mode.pixelFormat = cs::VideoMode::kBGR;
-      } else if (s.equals_lower("gray")) {
+      } else if (wpi::equals_lower(str, "gray")) {
         mode.pixelFormat = cs::VideoMode::kGray;
       } else {
-        SWARNING("SetConfigJson: could not understand pixel format value '"
-                 << str << '\'');
+        SWARNING("SetConfigJson: could not understand pixel format value '{}'",
+                 str);
       }
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read pixel format: " << e.what());
+      SWARNING("SetConfigJson: could not read pixel format: {}", e.what());
     }
   }
 
@@ -206,7 +212,7 @@
     try {
       mode.width = config.at("width").get<unsigned int>();
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read width: " << e.what());
+      SWARNING("SetConfigJson: could not read width: {}", e.what());
     }
   }
 
@@ -215,7 +221,7 @@
     try {
       mode.height = config.at("height").get<unsigned int>();
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read height: " << e.what());
+      SWARNING("SetConfigJson: could not read height: {}", e.what());
     }
   }
 
@@ -224,30 +230,31 @@
     try {
       mode.fps = config.at("fps").get<unsigned int>();
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read fps: " << e.what());
+      SWARNING("SetConfigJson: could not read fps: {}", e.what());
     }
   }
 
   // if all of video mode is set, use SetVideoMode, otherwise piecemeal it
   if (mode.pixelFormat != VideoMode::kUnknown && mode.width != 0 &&
       mode.height != 0 && mode.fps != 0) {
-    SINFO("SetConfigJson: setting video mode to pixelFormat "
-          << mode.pixelFormat << ", width " << mode.width << ", height "
-          << mode.height << ", fps " << mode.fps);
+    SINFO(
+        "SetConfigJson: setting video mode to pixelFormat {}, width {}, height "
+        "{}, fps {}",
+        mode.pixelFormat, mode.width, mode.height, mode.fps);
     SetVideoMode(mode, status);
   } else {
     if (mode.pixelFormat != cs::VideoMode::kUnknown) {
-      SINFO("SetConfigJson: setting pixelFormat " << mode.pixelFormat);
+      SINFO("SetConfigJson: setting pixelFormat {}", mode.pixelFormat);
       SetPixelFormat(static_cast<cs::VideoMode::PixelFormat>(mode.pixelFormat),
                      status);
     }
     if (mode.width != 0 && mode.height != 0) {
-      SINFO("SetConfigJson: setting width " << mode.width << ", height "
-                                            << mode.height);
+      SINFO("SetConfigJson: setting width {}, height {}", mode.width,
+            mode.height);
       SetResolution(mode.width, mode.height, status);
     }
     if (mode.fps != 0) {
-      SINFO("SetConfigJson: setting fps " << mode.fps);
+      SINFO("SetConfigJson: setting fps {}", mode.fps);
       SetFPS(mode.fps, status);
     }
   }
@@ -256,10 +263,10 @@
   if (config.count("brightness") != 0) {
     try {
       int val = config.at("brightness").get<int>();
-      SINFO("SetConfigJson: setting brightness to " << val);
+      SINFO("SetConfigJson: setting brightness to {}", val);
       SetBrightness(val, status);
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read brightness: " << e.what());
+      SWARNING("SetConfigJson: could not read brightness: {}", e.what());
     }
   }
 
@@ -269,24 +276,24 @@
       auto& setting = config.at("white balance");
       if (setting.is_string()) {
         auto str = setting.get<std::string>();
-        wpi::StringRef s(str);
-        if (s.equals_lower("auto")) {
-          SINFO("SetConfigJson: setting white balance to auto");
+        if (wpi::equals_lower(str, "auto")) {
+          SINFO("SetConfigJson: setting white balance to {}", "auto");
           SetWhiteBalanceAuto(status);
-        } else if (s.equals_lower("hold")) {
-          SINFO("SetConfigJson: setting white balance to hold current");
+        } else if (wpi::equals_lower(str, "hold")) {
+          SINFO("SetConfigJson: setting white balance to {}", "hold current");
           SetWhiteBalanceHoldCurrent(status);
         } else {
-          SWARNING("SetConfigJson: could not understand white balance value '"
-                   << str << '\'');
+          SWARNING(
+              "SetConfigJson: could not understand white balance value '{}'",
+              str);
         }
       } else {
         int val = setting.get<int>();
-        SINFO("SetConfigJson: setting white balance to " << val);
+        SINFO("SetConfigJson: setting white balance to {}", val);
         SetWhiteBalanceManual(val, status);
       }
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read white balance: " << e.what());
+      SWARNING("SetConfigJson: could not read white balance: {}", e.what());
     }
   }
 
@@ -296,30 +303,30 @@
       auto& setting = config.at("exposure");
       if (setting.is_string()) {
         auto str = setting.get<std::string>();
-        wpi::StringRef s(str);
-        if (s.equals_lower("auto")) {
-          SINFO("SetConfigJson: setting exposure to auto");
+        if (wpi::equals_lower(str, "auto")) {
+          SINFO("SetConfigJson: setting exposure to {}", "auto");
           SetExposureAuto(status);
-        } else if (s.equals_lower("hold")) {
-          SINFO("SetConfigJson: setting exposure to hold current");
+        } else if (wpi::equals_lower(str, "hold")) {
+          SINFO("SetConfigJson: setting exposure to {}", "hold current");
           SetExposureHoldCurrent(status);
         } else {
-          SWARNING("SetConfigJson: could not understand exposure value '"
-                   << str << '\'');
+          SWARNING("SetConfigJson: could not understand exposure value '{}'",
+                   str);
         }
       } else {
         int val = setting.get<int>();
-        SINFO("SetConfigJson: setting exposure to " << val);
+        SINFO("SetConfigJson: setting exposure to {}", val);
         SetExposureManual(val, status);
       }
     } catch (const wpi::json::exception& e) {
-      SWARNING("SetConfigJson: could not read exposure: " << e.what());
+      SWARNING("SetConfigJson: could not read exposure: {}", e.what());
     }
   }
 
   // properties
-  if (config.count("properties") != 0)
+  if (config.count("properties") != 0) {
     SetPropertiesJson(config.at("properties"), m_logger, GetName(), status);
+  }
 
   return true;
 }
@@ -336,7 +343,7 @@
   wpi::json j;
 
   // pixel format
-  wpi::StringRef pixelFormat;
+  std::string_view pixelFormat;
   switch (m_mode.pixelFormat) {
     case VideoMode::kMJPEG:
       pixelFormat = "mjpeg";
@@ -356,30 +363,41 @@
     default:
       break;
   }
-  if (!pixelFormat.empty()) j.emplace("pixel format", pixelFormat);
+  if (!pixelFormat.empty()) {
+    j.emplace("pixel format", pixelFormat);
+  }
 
   // width
-  if (m_mode.width != 0) j.emplace("width", m_mode.width);
+  if (m_mode.width != 0) {
+    j.emplace("width", m_mode.width);
+  }
 
   // height
-  if (m_mode.height != 0) j.emplace("height", m_mode.height);
+  if (m_mode.height != 0) {
+    j.emplace("height", m_mode.height);
+  }
 
   // fps
-  if (m_mode.fps != 0) j.emplace("fps", m_mode.fps);
+  if (m_mode.fps != 0) {
+    j.emplace("fps", m_mode.fps);
+  }
 
   // TODO: output brightness, white balance, and exposure?
 
   // properties
   wpi::json props = GetPropertiesJsonObject(status);
-  if (props.is_array()) j.emplace("properties", props);
+  if (props.is_array()) {
+    j.emplace("properties", props);
+  }
 
   return j;
 }
 
 std::vector<VideoMode> SourceImpl::EnumerateVideoModes(
     CS_Status* status) const {
-  if (!m_properties_cached && !CacheProperties(status))
-    return std::vector<VideoMode>{};
+  if (!m_properties_cached && !CacheProperties(status)) {
+    return {};
+  }
   std::scoped_lock lock(m_mutex);
   return m_videoModes;
 }
@@ -404,10 +422,11 @@
     }
 
     // if nothing found, allocate a new buffer
-    if (found < 0)
-      image.reset(new Image{size});
-    else
+    if (found < 0) {
+      image = std::make_unique<Image>(size);
+    } else {
       image = std::move(m_imagesAvail[found]);
+    }
   }
 
   // Initialize image
@@ -420,14 +439,12 @@
 }
 
 void SourceImpl::PutFrame(VideoMode::PixelFormat pixelFormat, int width,
-                          int height, wpi::StringRef data, Frame::Time time) {
+                          int height, std::string_view data, Frame::Time time) {
   auto image = AllocImage(pixelFormat, width, height, data.size());
 
   // Copy in image data
-  SDEBUG4("Copying data to "
-          << reinterpret_cast<const void*>(image->data()) << " from "
-          << reinterpret_cast<const void*>(data.data()) << " (" << data.size()
-          << " bytes)");
+  SDEBUG4("Copying data to {} from {} ({} bytes)", fmt::ptr(image->data()),
+          fmt::ptr(data.data()), data.size());
   std::memcpy(image->data(), data.data(), data.size());
 
   PutFrame(std::move(image), time);
@@ -448,7 +465,7 @@
   m_frameCv.notify_all();
 }
 
-void SourceImpl::PutError(const wpi::Twine& msg, Frame::Time time) {
+void SourceImpl::PutError(std::string_view msg, Frame::Time time) {
   // Update frame
   {
     std::scoped_lock lock{m_frameMutex};
@@ -464,21 +481,25 @@
                                   propIndex, prop.propKind, prop.value,
                                   prop.valueStr);
   // also notify choices updated event for enum types
-  if (prop.propKind == CS_PROP_ENUM)
+  if (prop.propKind == CS_PROP_ENUM) {
     m_notifier.NotifySourceProperty(*this, CS_SOURCE_PROPERTY_CHOICES_UPDATED,
                                     prop.name, propIndex, prop.propKind,
-                                    prop.value, wpi::Twine{});
+                                    prop.value, {});
+  }
 }
 
 void SourceImpl::UpdatePropertyValue(int property, bool setString, int value,
-                                     const wpi::Twine& valueStr) {
+                                     std::string_view valueStr) {
   auto prop = GetProperty(property);
-  if (!prop) return;
+  if (!prop) {
+    return;
+  }
 
-  if (setString)
+  if (setString) {
     prop->SetValue(valueStr);
-  else
+  } else {
     prop->SetValue(value);
+  }
 
   // Only notify updates after we've notified created
   if (m_properties_cached) {
@@ -490,7 +511,9 @@
 
 void SourceImpl::ReleaseImage(std::unique_ptr<Image> image) {
   std::scoped_lock lock{m_poolMutex};
-  if (m_destroyFrames) return;
+  if (m_destroyFrames) {
+    return;
+  }
   // Return the frame to the pool.  First try to find an empty slot, otherwise
   // add it to the end.
   auto it = std::find(m_imagesAvail.begin(), m_imagesAvail.end(), nullptr);
@@ -504,7 +527,9 @@
         [](const std::unique_ptr<Image>& a, const std::unique_ptr<Image>& b) {
           return a->capacity() < b->capacity();
         });
-    if ((*it2)->capacity() < image->capacity()) *it2 = std::move(image);
+    if ((*it2)->capacity() < image->capacity()) {
+      *it2 = std::move(image);
+    }
   } else {
     m_imagesAvail.emplace_back(std::move(image));
   }
@@ -513,7 +538,9 @@
 std::unique_ptr<Frame::Impl> SourceImpl::AllocFrameImpl() {
   std::scoped_lock lock{m_poolMutex};
 
-  if (m_framesAvail.empty()) return std::make_unique<Frame::Impl>(*this);
+  if (m_framesAvail.empty()) {
+    return std::make_unique<Frame::Impl>(*this);
+  }
 
   auto impl = std::move(m_framesAvail.back());
   m_framesAvail.pop_back();
@@ -522,6 +549,8 @@
 
 void SourceImpl::ReleaseFrameImpl(std::unique_ptr<Frame::Impl> impl) {
   std::scoped_lock lock{m_poolMutex};
-  if (m_destroyFrames) return;
+  if (m_destroyFrames) {
+    return;
+  }
   m_framesAvail.push_back(std::move(impl));
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.h b/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.h
index d74d8fa..dd2e574 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/SourceImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_SOURCEIMPL_H_
 #define CSCORE_SOURCEIMPL_H_
@@ -12,12 +9,10 @@
 #include <cstddef>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
 #include <wpi/Logger.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 
@@ -40,18 +35,18 @@
   friend class Frame;
 
  public:
-  SourceImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  SourceImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
              Telemetry& telemetry);
-  virtual ~SourceImpl();
+  ~SourceImpl() override;
   SourceImpl(const SourceImpl& oth) = delete;
   SourceImpl& operator=(const SourceImpl& oth) = delete;
 
   virtual void Start() = 0;
 
-  wpi::StringRef GetName() const { return m_name; }
+  std::string_view GetName() const { return m_name; }
 
-  void SetDescription(const wpi::Twine& description);
-  wpi::StringRef GetDescription(wpi::SmallVectorImpl<char>& buf) const;
+  void SetDescription(std::string_view description);
+  std::string_view GetDescription(wpi::SmallVectorImpl<char>& buf) const;
 
   void SetConnectionStrategy(CS_ConnectionStrategy strategy) {
     m_strategy = static_cast<int>(strategy);
@@ -131,7 +126,7 @@
   virtual bool SetResolution(int width, int height, CS_Status* status);
   virtual bool SetFPS(int fps, CS_Status* status);
 
-  bool SetConfigJson(wpi::StringRef config, CS_Status* status);
+  bool SetConfigJson(std::string_view config, CS_Status* status);
   virtual bool SetConfigJson(const wpi::json& config, CS_Status* status);
   std::string GetConfigJson(CS_Status* status);
   virtual wpi::json GetConfigJsonObject(CS_Status* status);
@@ -144,12 +139,12 @@
  protected:
   void NotifyPropertyCreated(int propIndex, PropertyImpl& prop) override;
   void UpdatePropertyValue(int property, bool setString, int value,
-                           const wpi::Twine& valueStr) override;
+                           std::string_view valueStr) override;
 
   void PutFrame(VideoMode::PixelFormat pixelFormat, int width, int height,
-                wpi::StringRef data, Frame::Time time);
+                std::string_view data, Frame::Time time);
   void PutFrame(std::unique_ptr<Image> image, Frame::Time time);
-  void PutError(const wpi::Twine& msg, Frame::Time time);
+  void PutError(std::string_view msg, Frame::Time time);
 
   // Notification functions for corresponding atomics
   virtual void NumSinksChanged() = 0;
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.cpp
index 77130f6..751ef44 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Telemetry.h"
 
@@ -25,7 +22,7 @@
  public:
   explicit Thread(Notifier& notifier) : m_notifier(notifier) {}
 
-  void Main();
+  void Main() override;
 
   Notifier& m_notifier;
   wpi::DenseMap<std::pair<CS_Handle, int>, int64_t> m_user;
@@ -46,24 +43,33 @@
   return it->getSecond();
 }
 
-Telemetry::~Telemetry() {}
+Telemetry::~Telemetry() = default;
 
-void Telemetry::Start() { m_owner.Start(m_notifier); }
+void Telemetry::Start() {
+  m_owner.Start(m_notifier);
+}
 
-void Telemetry::Stop() { m_owner.Stop(); }
+void Telemetry::Stop() {
+  m_owner.Stop();
+}
 
 void Telemetry::Thread::Main() {
   std::unique_lock lock(m_mutex);
   auto prevTime = std::chrono::steady_clock::now();
   while (m_active) {
     double period = m_period;
-    if (period == 0) period = 1000.0;
+    if (period == 0) {
+      period = 1000.0;
+    }
     auto timeoutTime = prevTime + std::chrono::duration<double>(period);
     while (m_active && !m_updated) {
-      if (m_cond.wait_until(lock, timeoutTime) == std::cv_status::timeout)
+      if (m_cond.wait_until(lock, timeoutTime) == std::cv_status::timeout) {
         break;
+      }
     }
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
     if (m_updated) {
       m_updated = false;
       continue;
@@ -83,8 +89,12 @@
 
 void Telemetry::SetPeriod(double seconds) {
   auto thr = m_owner.GetThread();
-  if (!thr) return;
-  if (thr->m_period == seconds) return;  // no change
+  if (!thr) {
+    return;
+  }
+  if (thr->m_period == seconds) {
+    return;  // no change
+  }
   thr->m_period = seconds;
   thr->m_updated = true;
   thr->m_cond.notify_one();
@@ -92,7 +102,9 @@
 
 double Telemetry::GetElapsedTime() {
   auto thr = m_owner.GetThread();
-  if (!thr) return 0;
+  if (!thr) {
+    return 0;
+  }
   return thr->m_elapsed;
 }
 
@@ -113,13 +125,17 @@
     *status = CS_TELEMETRY_NOT_ENABLED;
     return 0;
   }
-  if (thr->m_elapsed == 0) return 0.0;
+  if (thr->m_elapsed == 0) {
+    return 0.0;
+  }
   return thr->GetValue(handle, kind, status) / thr->m_elapsed;
 }
 
 void Telemetry::RecordSourceBytes(const SourceImpl& source, int quantity) {
   auto thr = m_owner.GetThread();
-  if (!thr) return;
+  if (!thr) {
+    return;
+  }
   auto handleData = Instance::GetInstance().FindSource(source);
   thr->m_current[std::make_pair(Handle{handleData.first, Handle::kSource},
                                 static_cast<int>(CS_SOURCE_BYTES_RECEIVED))] +=
@@ -128,7 +144,9 @@
 
 void Telemetry::RecordSourceFrames(const SourceImpl& source, int quantity) {
   auto thr = m_owner.GetThread();
-  if (!thr) return;
+  if (!thr) {
+    return;
+  }
   auto handleData = Instance::GetInstance().FindSource(source);
   thr->m_current[std::make_pair(Handle{handleData.first, Handle::kSource},
                                 static_cast<int>(CS_SOURCE_FRAMES_RECEIVED))] +=
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.h b/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.h
index 8729704..0a32d21 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/Telemetry.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_TELEMETRY_H_
 #define CSCORE_TELEMETRY_H_
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/UnlimitedHandleResource.h b/third_party/allwpilib/cscore/src/main/native/cpp/UnlimitedHandleResource.h
index 200572c..f7671ae 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/UnlimitedHandleResource.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/UnlimitedHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_UNLIMITEDHANDLERESOURCE_H_
 #define CSCORE_UNLIMITEDHANDLERESOURCE_H_
@@ -12,9 +9,9 @@
 #include <utility>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
 #include <wpi/mutex.h>
+#include <wpi/span.h>
 
 namespace cs {
 
@@ -53,7 +50,7 @@
   std::shared_ptr<TStruct> Free(THandle handle);
 
   template <typename T>
-  wpi::ArrayRef<T> GetAll(wpi::SmallVectorImpl<T>& vec);
+  wpi::span<T> GetAll(wpi::SmallVectorImpl<T>& vec);
 
   std::vector<std::shared_ptr<TStruct>> FreeAll();
 
@@ -90,7 +87,9 @@
       return MakeHandle(i);
     }
   }
-  if (i >= THandle::kIndexMax) return 0;
+  if (i >= THandle::kIndexMax) {
+    return 0;
+  }
 
   m_structures.emplace_back(
       std::make_shared<TStruct>(std::forward<Args>(args)...));
@@ -108,7 +107,9 @@
       return MakeHandle(i);
     }
   }
-  if (i >= THandle::kIndexMax) return 0;
+  if (i >= THandle::kIndexMax) {
+    return 0;
+  }
 
   m_structures.push_back(structure);
   return MakeHandle(i);
@@ -120,9 +121,13 @@
     THandle handle) {
   auto index =
       handle.GetTypedIndex(static_cast<typename THandle::Type>(typeValue));
-  if (index < 0) return nullptr;
+  if (index < 0) {
+    return nullptr;
+  }
   std::scoped_lock sync(m_handleMutex);
-  if (index >= static_cast<int>(m_structures.size())) return nullptr;
+  if (index >= static_cast<int>(m_structures.size())) {
+    return nullptr;
+  }
   return m_structures[index];
 }
 
@@ -132,9 +137,13 @@
     THandle handle) {
   auto index =
       handle.GetTypedIndex(static_cast<typename THandle::Type>(typeValue));
-  if (index < 0) return nullptr;
+  if (index < 0) {
+    return nullptr;
+  }
   std::scoped_lock sync(m_handleMutex);
-  if (index >= static_cast<int>(m_structures.size())) return nullptr;
+  if (index >= static_cast<int>(m_structures.size())) {
+    return nullptr;
+  }
   auto rv = std::move(m_structures[index]);
   m_structures[index].reset();
   return rv;
@@ -142,7 +151,7 @@
 
 template <typename THandle, typename TStruct, int typeValue, typename TMutex>
 template <typename T>
-inline wpi::ArrayRef<T>
+inline wpi::span<T>
 UnlimitedHandleResource<THandle, TStruct, typeValue, TMutex>::GetAll(
     wpi::SmallVectorImpl<T>& vec) {
   ForEach([&](THandle handle, const TStruct& data) { vec.push_back(handle); });
@@ -164,7 +173,9 @@
 UnlimitedHandleResource<THandle, TStruct, typeValue, TMutex>::ForEach(F func) {
   std::scoped_lock sync(m_handleMutex);
   for (size_t i = 0; i < m_structures.size(); i++) {
-    if (m_structures[i] != nullptr) func(MakeHandle(i), *(m_structures[i]));
+    if (m_structures[i] != nullptr) {
+      func(MakeHandle(i), *(m_structures[i]));
+    }
   }
 }
 
@@ -175,8 +186,9 @@
   std::scoped_lock sync(m_handleMutex);
   for (size_t i = 0; i < m_structures.size(); i++) {
     auto& structure = m_structures[i];
-    if (structure != nullptr && func(*structure))
+    if (structure != nullptr && func(*structure)) {
       return std::make_pair(MakeHandle(i), structure);
+    }
   }
   return std::make_pair(0, nullptr);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp
index 41906a7..5731f1f 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraImplCommon.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_c.h"  // NOLINT(build/include_order)
 
@@ -19,8 +16,9 @@
   out->otherPaths = static_cast<char**>(
       wpi::safe_malloc(in.otherPaths.size() * sizeof(char*)));
   out->otherPathsCount = in.otherPaths.size();
-  for (size_t i = 0; i < in.otherPaths.size(); ++i)
+  for (size_t i = 0; i < in.otherPaths.size(); ++i) {
     out->otherPaths[i] = cs::ConvertToC(in.otherPaths[i]);
+  }
   out->vendorId = in.vendorId;
   out->productId = in.productId;
 }
@@ -28,8 +26,9 @@
 static void FreeUsbCameraInfo(CS_UsbCameraInfo* info) {
   std::free(info->path);
   std::free(info->name);
-  for (int i = 0; i < info->otherPathsCount; ++i)
+  for (int i = 0; i < info->otherPathsCount; ++i) {
     std::free(info->otherPaths[i]);
+  }
   std::free(info->otherPaths);
 }
 
@@ -55,7 +54,9 @@
 
 CS_UsbCameraInfo* CS_GetUsbCameraInfo(CS_Source source, CS_Status* status) {
   auto info = cs::GetUsbCameraInfo(source, status);
-  if (*status != CS_OK) return nullptr;
+  if (*status != CS_OK) {
+    return nullptr;
+  }
   CS_UsbCameraInfo* out = static_cast<CS_UsbCameraInfo*>(
       wpi::safe_malloc(sizeof(CS_UsbCameraInfo)));
   ConvertToC(out, info);
@@ -67,18 +68,26 @@
   CS_UsbCameraInfo* out = static_cast<CS_UsbCameraInfo*>(
       wpi::safe_malloc(cameras.size() * sizeof(CS_UsbCameraInfo)));
   *count = cameras.size();
-  for (size_t i = 0; i < cameras.size(); ++i) ConvertToC(&out[i], cameras[i]);
+  for (size_t i = 0; i < cameras.size(); ++i) {
+    ConvertToC(&out[i], cameras[i]);
+  }
   return out;
 }
 
 void CS_FreeEnumeratedUsbCameras(CS_UsbCameraInfo* cameras, int count) {
-  if (!cameras) return;
-  for (int i = 0; i < count; ++i) FreeUsbCameraInfo(&cameras[i]);
+  if (!cameras) {
+    return;
+  }
+  for (int i = 0; i < count; ++i) {
+    FreeUsbCameraInfo(&cameras[i]);
+  }
   std::free(cameras);
 }
 
 void CS_FreeUsbCameraInfo(CS_UsbCameraInfo* info) {
-  if (!info) return;
+  if (!info) {
+    return;
+  }
   FreeUsbCameraInfo(info);
   std::free(info);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraListener.h b/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraListener.h
new file mode 100644
index 0000000..c8e8e15
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/UsbCameraListener.h
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef CSCORE_USBCAMERALISTENER_H_
+#define CSCORE_USBCAMERALISTENER_H_
+
+#include <memory>
+
+#include <wpi/Logger.h>
+
+namespace cs {
+
+class Notifier;
+
+class UsbCameraListener {
+ public:
+  UsbCameraListener(wpi::Logger& logger, Notifier& notifier);
+  ~UsbCameraListener();
+
+  void Start();
+  void Stop();
+
+ private:
+  class Impl;
+  std::unique_ptr<Impl> m_impl;
+};
+
+}  // namespace cs
+
+#endif  // CSCORE_USBCAMERALISTENER_H_
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/c_util.h b/third_party/allwpilib/cscore/src/main/native/cpp/c_util.h
index f985fe2..904d049 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/c_util.h
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/c_util.h
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_C_UTIL_H_
 #define CSCORE_C_UTIL_H_
 
 #include <cstdlib>
 #include <cstring>
+#include <string_view>
 
 #include <wpi/MemAlloc.h>
-#include <wpi/StringRef.h>
 
 namespace cs {
 
-inline char* ConvertToC(wpi::StringRef in) {
+inline char* ConvertToC(std::string_view in) {
   char* out = static_cast<char*>(wpi::safe_malloc(in.size() + 1));
   std::memmove(out, in.data(), in.size());
   out[in.size()] = '\0';
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp
index 321572e..ffbc164 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_c.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_c.h"
 
@@ -18,6 +15,39 @@
 #include "cscore_cpp.h"
 #include "cscore_raw.h"
 
+static CS_Event ConvertToC(const cs::RawEvent& rawEvent) {
+  CS_Event event;
+  event.kind = static_cast<CS_EventKind>(static_cast<int>(rawEvent.kind));
+  event.source = rawEvent.sourceHandle;
+  event.sink = rawEvent.sinkHandle;
+  event.name = rawEvent.name.c_str();
+  event.mode = rawEvent.mode;
+  event.property = rawEvent.propertyHandle;
+  event.propertyKind = rawEvent.propertyKind;
+  event.value = rawEvent.value;
+  event.valueStr = rawEvent.valueStr.c_str();
+  event.listener = rawEvent.listener;
+  return event;
+}
+
+template <typename O, typename I>
+static O* ConvertToC(std::vector<I>&& in, int* count) {
+  using T = std::vector<I>;
+  size_t size = in.size();
+  O* out = static_cast<O*>(wpi::safe_malloc(size * sizeof(O) + sizeof(T)));
+  *count = size;
+  for (size_t i = 0; i < size; ++i) {
+    out[i] = ConvertToC(in[i]);
+  }
+
+  // retain vector at end of returned array
+  alignas(T) unsigned char buf[sizeof(T)];
+  new (buf) T(std::move(in));
+  std::memcpy(out + size * sizeof(O), buf, sizeof(T));
+
+  return out;
+}
+
 extern "C" {
 
 CS_PropertyKind CS_GetPropertyKind(CS_Property property, CS_Status* status) {
@@ -27,7 +57,9 @@
 char* CS_GetPropertyName(CS_Property property, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetPropertyName(property, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
@@ -58,7 +90,9 @@
 char* CS_GetStringProperty(CS_Property property, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetStringProperty(property, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
@@ -73,8 +107,9 @@
   char** out =
       static_cast<char**>(wpi::safe_malloc(choices.size() * sizeof(char*)));
   *count = choices.size();
-  for (size_t i = 0; i < choices.size(); ++i)
+  for (size_t i = 0; i < choices.size(); ++i) {
     out[i] = cs::ConvertToC(choices[i]);
+  }
   return out;
 }
 
@@ -85,14 +120,18 @@
 char* CS_GetSourceName(CS_Source source, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetSourceName(source, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
 char* CS_GetSourceDescription(CS_Source source, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetSourceDescription(source, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
@@ -251,14 +290,18 @@
 char* CS_GetSinkName(CS_Sink sink, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetSinkName(sink, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
 char* CS_GetSinkDescription(CS_Sink sink, CS_Status* status) {
   wpi::SmallString<128> buf;
   auto str = cs::GetSinkDescription(sink, buf, status);
-  if (*status != 0) return nullptr;
+  if (*status != 0) {
+    return nullptr;
+  }
   return cs::ConvertToC(str);
 }
 
@@ -309,11 +352,11 @@
 }
 
 void CS_SetListenerOnStart(void (*onStart)(void* data), void* data) {
-  cs::SetListenerOnStart([=]() { onStart(data); });
+  cs::SetListenerOnStart([=] { onStart(data); });
 }
 
 void CS_SetListenerOnExit(void (*onExit)(void* data), void* data) {
-  cs::SetListenerOnExit([=]() { onExit(data); });
+  cs::SetListenerOnExit([=] { onExit(data); });
 }
 
 CS_Listener CS_AddListener(void* data,
@@ -322,16 +365,7 @@
                            CS_Status* status) {
   return cs::AddListener(
       [=](const cs::RawEvent& rawEvent) {
-        CS_Event event;
-        event.kind = static_cast<CS_EventKind>(static_cast<int>(rawEvent.kind));
-        event.source = rawEvent.sourceHandle;
-        event.sink = rawEvent.sinkHandle;
-        event.name = rawEvent.name.c_str();
-        event.mode = rawEvent.mode;
-        event.property = rawEvent.propertyHandle;
-        event.propertyKind = rawEvent.propertyKind;
-        event.value = rawEvent.value;
-        event.valueStr = rawEvent.valueStr.c_str();
+        CS_Event event = ConvertToC(rawEvent);
         callback(data, &event);
       },
       eventMask, immediateNotify, status);
@@ -341,9 +375,52 @@
   return cs::RemoveListener(handle, status);
 }
 
-int CS_NotifierDestroyed(void) { return cs::NotifierDestroyed(); }
+CS_ListenerPoller CS_CreateListenerPoller(void) {
+  return cs::CreateListenerPoller();
+}
 
-void CS_SetTelemetryPeriod(double seconds) { cs::SetTelemetryPeriod(seconds); }
+void CS_DestroyListenerPoller(CS_ListenerPoller poller) {
+  cs::DestroyListenerPoller(poller);
+}
+
+CS_Listener CS_AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                                 CS_Bool immediateNotify, CS_Status* status) {
+  return cs::AddPolledListener(poller, eventMask, immediateNotify, status);
+}
+
+struct CS_Event* CS_PollListener(CS_ListenerPoller poller, int* count) {
+  return ConvertToC<CS_Event>(cs::PollListener(poller), count);
+}
+
+struct CS_Event* CS_PollListenerTimeout(CS_ListenerPoller poller, int* count,
+                                        double timeout, CS_Bool* timedOut) {
+  bool cppTimedOut = false;
+  auto arrCpp = cs::PollListener(poller, timeout, &cppTimedOut);
+  *timedOut = cppTimedOut;
+  return ConvertToC<CS_Event>(std::move(arrCpp), count);
+}
+
+void CS_CancelPollListener(CS_ListenerPoller poller) {
+  cs::CancelPollListener(poller);
+}
+
+void CS_FreeEvents(CS_Event* arr, int count) {
+  // destroy vector saved at end of array
+  using T = std::vector<cs::RawEvent>;
+  alignas(T) unsigned char buf[sizeof(T)];
+  std::memcpy(buf, arr + count * sizeof(CS_Event), sizeof(T));
+  reinterpret_cast<T*>(buf)->~T();
+
+  std::free(arr);
+}
+
+int CS_NotifierDestroyed(void) {
+  return cs::NotifierDestroyed();
+}
+
+void CS_SetTelemetryPeriod(double seconds) {
+  cs::SetTelemetryPeriod(seconds);
+}
 
 double CS_GetTelemetryElapsedTime(void) {
   return cs::GetTelemetryElapsedTime();
@@ -367,7 +444,9 @@
   cs::SetDefaultLogger(min_level);
 }
 
-void CS_Shutdown(void) { cs::Shutdown(); }
+void CS_Shutdown(void) {
+  cs::Shutdown();
+}
 
 CS_Source* CS_EnumerateSources(int* count, CS_Status* status) {
   wpi::SmallVector<CS_Source, 32> buf;
@@ -380,10 +459,14 @@
 }
 
 void CS_ReleaseEnumeratedSources(CS_Source* sources, int count) {
-  if (!sources) return;
+  if (!sources) {
+    return;
+  }
   for (int i = 0; i < count; ++i) {
     CS_Status status = 0;
-    if (sources[i] != 0) cs::ReleaseSource(sources[i], &status);
+    if (sources[i] != 0) {
+      cs::ReleaseSource(sources[i], &status);
+    }
   }
   std::free(sources);
 }
@@ -399,19 +482,29 @@
 }
 
 void CS_ReleaseEnumeratedSinks(CS_Sink* sinks, int count) {
-  if (!sinks) return;
+  if (!sinks) {
+    return;
+  }
   for (int i = 0; i < count; ++i) {
     CS_Status status = 0;
-    if (sinks[i] != 0) cs::ReleaseSink(sinks[i], &status);
+    if (sinks[i] != 0) {
+      cs::ReleaseSink(sinks[i], &status);
+    }
   }
   std::free(sinks);
 }
 
-void CS_FreeString(char* str) { std::free(str); }
+void CS_FreeString(char* str) {
+  std::free(str);
+}
 
 void CS_FreeEnumPropertyChoices(char** choices, int count) {
-  if (!choices) return;
-  for (int i = 0; i < count; ++i) std::free(choices[i]);
+  if (!choices) {
+    return;
+  }
+  for (int i = 0; i < count; ++i) {
+    std::free(choices[i]);
+  }
   std::free(choices);
 }
 
@@ -423,26 +516,35 @@
   std::free(modes);
 }
 
-char* CS_GetHostname() { return cs::ConvertToC(cs::GetHostname()); }
+char* CS_GetHostname() {
+  return cs::ConvertToC(cs::GetHostname());
+}
 
 char** CS_GetNetworkInterfaces(int* count) {
   auto interfaces = cs::GetNetworkInterfaces();
   char** out =
       static_cast<char**>(wpi::safe_malloc(interfaces.size() * sizeof(char*)));
   *count = interfaces.size();
-  for (size_t i = 0; i < interfaces.size(); ++i)
+  for (size_t i = 0; i < interfaces.size(); ++i) {
     out[i] = cs::ConvertToC(interfaces[i]);
+  }
   return out;
 }
 
 void CS_FreeNetworkInterfaces(char** interfaces, int count) {
-  if (!interfaces) return;
-  for (int i = 0; i < count; ++i) std::free(interfaces[i]);
+  if (!interfaces) {
+    return;
+  }
+  for (int i = 0; i < count; ++i) {
+    std::free(interfaces[i]);
+  }
   std::free(interfaces);
 }
 
 void CS_AllocateRawFrameData(CS_RawFrame* frame, int requestedSize) {
-  if (frame->dataLength >= requestedSize) return;
+  if (frame->dataLength >= requestedSize) {
+    return;
+  }
   if (frame->data) {
     frame->data =
         static_cast<char*>(wpi::safe_realloc(frame->data, requestedSize));
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_cpp.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_cpp.cpp
index d49fb04..3b4e570 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_cpp.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_cpp.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_cpp.h"
 
@@ -60,7 +57,9 @@
 CS_PropertyKind GetPropertyKind(CS_Property property, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return CS_PROP_NONE;
+  if (!container) {
+    return CS_PROP_NONE;
+  }
   return container->GetPropertyKind(propertyIndex);
 }
 
@@ -68,58 +67,74 @@
   wpi::SmallString<128> buf;
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return std::string{};
-  return container->GetPropertyName(propertyIndex, buf, status);
+  if (!container) {
+    return {};
+  }
+  return std::string{container->GetPropertyName(propertyIndex, buf, status)};
 }
 
-wpi::StringRef GetPropertyName(CS_Property property,
-                               wpi::SmallVectorImpl<char>& buf,
-                               CS_Status* status) {
+std::string_view GetPropertyName(CS_Property property,
+                                 wpi::SmallVectorImpl<char>& buf,
+                                 CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return wpi::StringRef{};
+  if (!container) {
+    return {};
+  }
   return container->GetPropertyName(propertyIndex, buf, status);
 }
 
 int GetProperty(CS_Property property, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return false;
+  if (!container) {
+    return false;
+  }
   return container->GetProperty(propertyIndex, status);
 }
 
 void SetProperty(CS_Property property, int value, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return;
+  if (!container) {
+    return;
+  }
   container->SetProperty(propertyIndex, value, status);
 }
 
 int GetPropertyMin(CS_Property property, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return 0.0;
+  if (!container) {
+    return 0.0;
+  }
   return container->GetPropertyMin(propertyIndex, status);
 }
 
 int GetPropertyMax(CS_Property property, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return 0.0;
+  if (!container) {
+    return 0.0;
+  }
   return container->GetPropertyMax(propertyIndex, status);
 }
 
 int GetPropertyStep(CS_Property property, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return 0.0;
+  if (!container) {
+    return 0.0;
+  }
   return container->GetPropertyStep(propertyIndex, status);
 }
 
 int GetPropertyDefault(CS_Property property, CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return 0.0;
+  if (!container) {
+    return 0.0;
+  }
   return container->GetPropertyDefault(propertyIndex, status);
 }
 
@@ -127,24 +142,30 @@
   wpi::SmallString<128> buf;
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return std::string{};
-  return container->GetStringProperty(propertyIndex, buf, status);
+  if (!container) {
+    return {};
+  }
+  return std::string{container->GetStringProperty(propertyIndex, buf, status)};
 }
 
-wpi::StringRef GetStringProperty(CS_Property property,
-                                 wpi::SmallVectorImpl<char>& buf,
-                                 CS_Status* status) {
+std::string_view GetStringProperty(CS_Property property,
+                                   wpi::SmallVectorImpl<char>& buf,
+                                   CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return wpi::StringRef{};
+  if (!container) {
+    return {};
+  }
   return container->GetStringProperty(propertyIndex, buf, status);
 }
 
-void SetStringProperty(CS_Property property, const wpi::Twine& value,
+void SetStringProperty(CS_Property property, std::string_view value,
                        CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return;
+  if (!container) {
+    return;
+  }
   container->SetStringProperty(propertyIndex, value, status);
 }
 
@@ -152,7 +173,9 @@
                                                 CS_Status* status) {
   int propertyIndex;
   auto container = GetPropertyContainer(property, &propertyIndex, status);
-  if (!container) return std::vector<std::string>{};
+  if (!container) {
+    return {};
+  }
   return container->GetEnumPropertyChoices(propertyIndex, status);
 }
 
@@ -173,17 +196,18 @@
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return std::string{};
+    return {};
   }
-  return data->source->GetName();
+  return std::string{data->source->GetName()};
 }
 
-wpi::StringRef GetSourceName(CS_Source source, wpi::SmallVectorImpl<char>& buf,
-                             CS_Status* status) {
+std::string_view GetSourceName(CS_Source source,
+                               wpi::SmallVectorImpl<char>& buf,
+                               CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return wpi::StringRef{};
+    return {};
   }
   return data->source->GetName();
 }
@@ -192,19 +216,19 @@
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return std::string{};
+    return {};
   }
   wpi::SmallString<128> buf;
-  return data->source->GetDescription(buf);
+  return std::string{data->source->GetDescription(buf)};
 }
 
-wpi::StringRef GetSourceDescription(CS_Source source,
-                                    wpi::SmallVectorImpl<char>& buf,
-                                    CS_Status* status) {
+std::string_view GetSourceDescription(CS_Source source,
+                                      wpi::SmallVectorImpl<char>& buf,
+                                      CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return wpi::StringRef{};
+    return {};
   }
   return data->source->GetDescription(buf);
 }
@@ -247,7 +271,7 @@
   return data->source->IsEnabled();
 }
 
-CS_Property GetSourceProperty(CS_Source source, const wpi::Twine& name,
+CS_Property GetSourceProperty(CS_Source source, std::string_view name,
                               CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
@@ -262,18 +286,19 @@
   return Handle{source, property, Handle::kProperty};
 }
 
-wpi::ArrayRef<CS_Property> EnumerateSourceProperties(
+wpi::span<CS_Property> EnumerateSourceProperties(
     CS_Source source, wpi::SmallVectorImpl<CS_Property>& vec,
     CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return 0;
+    return {};
   }
   wpi::SmallVector<int, 32> properties_buf;
   for (auto property :
-       data->source->EnumerateProperties(properties_buf, status))
+       data->source->EnumerateProperties(properties_buf, status)) {
     vec.push_back(Handle{source, property, Handle::kProperty});
+  }
   return vec;
 }
 
@@ -325,7 +350,7 @@
   return data->source->SetFPS(fps, status);
 }
 
-bool SetSourceConfigJson(CS_Source source, wpi::StringRef config,
+bool SetSourceConfigJson(CS_Source source, std::string_view config,
                          CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
@@ -373,20 +398,22 @@
   return data->source->EnumerateVideoModes(status);
 }
 
-wpi::ArrayRef<CS_Sink> EnumerateSourceSinks(CS_Source source,
-                                            wpi::SmallVectorImpl<CS_Sink>& vec,
-                                            CS_Status* status) {
+wpi::span<CS_Sink> EnumerateSourceSinks(CS_Source source,
+                                        wpi::SmallVectorImpl<CS_Sink>& vec,
+                                        CS_Status* status) {
   auto& inst = Instance::GetInstance();
   auto data = inst.GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return wpi::ArrayRef<CS_Sink>{};
+    return {};
   }
   return inst.EnumerateSourceSinks(source, vec);
 }
 
 CS_Source CopySource(CS_Source source, CS_Status* status) {
-  if (source == 0) return 0;
+  if (source == 0) {
+    return 0;
+  }
   auto data = Instance::GetInstance().GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
@@ -397,14 +424,18 @@
 }
 
 void ReleaseSource(CS_Source source, CS_Status* status) {
-  if (source == 0) return;
+  if (source == 0) {
+    return;
+  }
   auto& inst = Instance::GetInstance();
   auto data = inst.GetSource(source);
   if (!data) {
     *status = CS_INVALID_HANDLE;
     return;
   }
-  if (data->refCount-- == 0) inst.DestroySource(source);
+  if (data->refCount-- == 0) {
+    inst.DestroySource(source);
+  }
 }
 
 //
@@ -501,17 +532,17 @@
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return std::string{};
+    return {};
   }
-  return data->sink->GetName();
+  return std::string{data->sink->GetName()};
 }
 
-wpi::StringRef GetSinkName(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
-                           CS_Status* status) {
+std::string_view GetSinkName(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
+                             CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return wpi::StringRef{};
+    return {};
   }
   return data->sink->GetName();
 }
@@ -520,23 +551,24 @@
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return std::string{};
+    return {};
   }
   wpi::SmallString<128> buf;
-  return data->sink->GetDescription(buf);
+  return std::string{data->sink->GetDescription(buf)};
 }
 
-wpi::StringRef GetSinkDescription(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
-                                  CS_Status* status) {
+std::string_view GetSinkDescription(CS_Sink sink,
+                                    wpi::SmallVectorImpl<char>& buf,
+                                    CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return wpi::StringRef{};
+    return {};
   }
   return data->sink->GetDescription(buf);
 }
 
-CS_Property GetSinkProperty(CS_Sink sink, const wpi::Twine& name,
+CS_Property GetSinkProperty(CS_Sink sink, std::string_view name,
                             CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
@@ -551,20 +583,23 @@
   return Handle{sink, property, Handle::kSinkProperty};
 }
 
-wpi::ArrayRef<CS_Property> EnumerateSinkProperties(
+wpi::span<CS_Property> EnumerateSinkProperties(
     CS_Sink sink, wpi::SmallVectorImpl<CS_Property>& vec, CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
-    return 0;
+    return {};
   }
   wpi::SmallVector<int, 32> properties_buf;
-  for (auto property : data->sink->EnumerateProperties(properties_buf, status))
+  for (auto property :
+       data->sink->EnumerateProperties(properties_buf, status)) {
     vec.push_back(Handle{sink, property, Handle::kSinkProperty});
+  }
   return vec;
 }
 
-bool SetSinkConfigJson(CS_Sink sink, wpi::StringRef config, CS_Status* status) {
+bool SetSinkConfigJson(CS_Sink sink, std::string_view config,
+                       CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
@@ -631,7 +666,7 @@
   return data->sourceHandle.load();
 }
 
-CS_Property GetSinkSourceProperty(CS_Sink sink, const wpi::Twine& name,
+CS_Property GetSinkSourceProperty(CS_Sink sink, std::string_view name,
                                   CS_Status* status) {
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
@@ -642,7 +677,9 @@
 }
 
 CS_Sink CopySink(CS_Sink sink, CS_Status* status) {
-  if (sink == 0) return 0;
+  if (sink == 0) {
+    return 0;
+  }
   auto data = Instance::GetInstance().GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
@@ -653,14 +690,18 @@
 }
 
 void ReleaseSink(CS_Sink sink, CS_Status* status) {
-  if (sink == 0) return;
+  if (sink == 0) {
+    return;
+  }
   auto& inst = Instance::GetInstance();
   auto data = inst.GetSink(sink);
   if (!data) {
     *status = CS_INVALID_HANDLE;
     return;
   }
-  if (data->refCount-- == 0) inst.DestroySink(sink);
+  if (data->refCount-- == 0) {
+    inst.DestroySink(sink);
+  }
 }
 
 //
@@ -675,16 +716,30 @@
   Instance::GetInstance().notifier.SetOnExit(onExit);
 }
 
+static void StartBackground(int eventMask, bool immediateNotify) {
+  auto& inst = Instance::GetInstance();
+  if ((eventMask & CS_NETWORK_INTERFACES_CHANGED) != 0) {
+    // start network interface event listener
+    inst.networkListener.Start();
+    if (immediateNotify) {
+      inst.notifier.NotifyNetworkInterfacesChanged();
+    }
+  }
+  if ((eventMask & CS_USB_CAMERAS_CHANGED) != 0) {
+    // start network interface event listener
+    inst.usbCameraListener.Start();
+    if (immediateNotify) {
+      inst.notifier.NotifyUsbCamerasChanged();
+    }
+  }
+}
+
 CS_Listener AddListener(std::function<void(const RawEvent& event)> callback,
                         int eventMask, bool immediateNotify,
                         CS_Status* status) {
   auto& inst = Instance::GetInstance();
-  int uid = inst.notifier.AddListener(callback, eventMask);
-  if ((eventMask & CS_NETWORK_INTERFACES_CHANGED) != 0) {
-    // start network interface event listener
-    inst.networkListener.Start();
-    if (immediateNotify) inst.notifier.NotifyNetworkInterfacesChanged();
-  }
+  int uid = inst.notifier.Add(callback, eventMask);
+  StartBackground(eventMask, immediateNotify);
   if (immediateNotify) {
     // TODO
   }
@@ -697,10 +752,68 @@
     *status = CS_INVALID_HANDLE;
     return;
   }
-  Instance::GetInstance().notifier.RemoveListener(uid);
+  Instance::GetInstance().notifier.Remove(uid);
 }
 
-bool NotifierDestroyed() { return Notifier::destroyed(); }
+CS_ListenerPoller CreateListenerPoller() {
+  auto& inst = Instance::GetInstance();
+  return Handle(inst.notifier.CreatePoller(), Handle::kListenerPoller);
+}
+
+void DestroyListenerPoller(CS_ListenerPoller poller) {
+  int uid = Handle{poller}.GetTypedIndex(Handle::kListenerPoller);
+  if (uid < 0) {
+    return;
+  }
+  Instance::GetInstance().notifier.RemovePoller(uid);
+}
+
+CS_Listener AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                              bool immediateNotify, CS_Status* status) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    *status = CS_INVALID_HANDLE;
+    return 0;
+  }
+
+  auto& inst = Instance::GetInstance();
+  int uid = inst.notifier.AddPolled(id, eventMask);
+  StartBackground(eventMask, immediateNotify);
+  return Handle{uid, Handle::kListener};
+}
+
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    return {};
+  }
+  return Instance::GetInstance().notifier.Poll(id);
+}
+
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller, double timeout,
+                                   bool* timedOut) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    return {};
+  }
+  return Instance::GetInstance().notifier.Poll(id, timeout, timedOut);
+}
+
+void CancelPollListener(CS_ListenerPoller poller) {
+  Handle handle{poller};
+  int id = handle.GetTypedIndex(Handle::kListenerPoller);
+  if (id < 0) {
+    return;
+  }
+  return Instance::GetInstance().notifier.CancelPoll(id);
+}
+
+bool NotifierDestroyed() {
+  return false;
+}
 
 //
 // Telemetry Functions
@@ -744,22 +857,26 @@
 //
 // Shutdown Function
 //
-void Shutdown() { Instance::GetInstance().Shutdown(); }
+void Shutdown() {
+  Instance::GetInstance().Shutdown();
+}
 
 //
 // Utility Functions
 //
 
-wpi::ArrayRef<CS_Source> EnumerateSourceHandles(
+wpi::span<CS_Source> EnumerateSourceHandles(
     wpi::SmallVectorImpl<CS_Source>& vec, CS_Status* status) {
   return Instance::GetInstance().EnumerateSourceHandles(vec);
 }
 
-wpi::ArrayRef<CS_Sink> EnumerateSinkHandles(wpi::SmallVectorImpl<CS_Sink>& vec,
-                                            CS_Status* status) {
+wpi::span<CS_Sink> EnumerateSinkHandles(wpi::SmallVectorImpl<CS_Sink>& vec,
+                                        CS_Status* status) {
   return Instance::GetInstance().EnumerateSinkHandles(vec);
 }
 
-std::string GetHostname() { return wpi::GetHostname(); }
+std::string GetHostname() {
+  return wpi::GetHostname();
+}
 
 }  // namespace cs
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_oo.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_oo.cpp
index f42ed5a..cca5ba7 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/cscore_oo.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/cscore_oo.cpp
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_oo.h"
 
+#include <fmt/format.h>
 #include <wpi/json.h>
 
 using namespace cs;
@@ -28,8 +26,9 @@
 
   std::vector<VideoProperty> properties;
   properties.reserve(handles.size());
-  for (CS_Property handle : handles)
+  for (CS_Property handle : handles) {
     properties.emplace_back(VideoProperty{handle});
+  }
   return properties;
 }
 
@@ -40,7 +39,9 @@
 
   std::vector<VideoSink> sinks;
   sinks.reserve(handles.size());
-  for (int handle : handles) sinks.emplace_back(VideoSink{handle});
+  for (int handle : handles) {
+    sinks.emplace_back(VideoSink{handle});
+  }
   return sinks;
 }
 
@@ -51,7 +52,9 @@
 
   std::vector<VideoSource> sources;
   sources.reserve(handles.size());
-  for (int handle : handles) sources.emplace_back(VideoSource{handle});
+  for (int handle : handles) {
+    sources.emplace_back(VideoSource{handle});
+  }
   return sources;
 }
 
@@ -62,8 +65,9 @@
 
   std::vector<VideoProperty> properties;
   properties.reserve(handles.size());
-  for (CS_Property handle : handles)
+  for (CS_Property handle : handles) {
     properties.emplace_back(VideoProperty{handle});
+  }
   return properties;
 }
 
@@ -74,6 +78,12 @@
 
   std::vector<VideoSink> sinks;
   sinks.reserve(handles.size());
-  for (int handle : handles) sinks.emplace_back(VideoSink{handle});
+  for (int handle : handles) {
+    sinks.emplace_back(VideoSink{handle});
+  }
   return sinks;
 }
+
+std::string AxisCamera::HostToUrl(std::string_view host) {
+  return fmt::format("http://{}/mjpg/video.mjpg", host);
+}
diff --git a/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp b/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
index e9408fd..b99d322 100644
--- a/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/cpp/jni/CameraServerJNI.cpp
@@ -1,21 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <exception>
 
+#include <fmt/format.h>
 #include <opencv2/core/core.hpp>
 #include <wpi/SmallString.h>
 #include <wpi/jni_util.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/span.h>
 
 #include "cscore_cpp.h"
 #include "cscore_cv.h"
 #include "cscore_raw.h"
-#include "edu_wpi_cscore_CameraServerJNI.h"
+#include "edu_wpi_first_cscore_CameraServerJNI.h"
 
 namespace cv {
 class Mat;
@@ -34,6 +32,7 @@
 static JClass videoEventCls;
 static JClass rawFrameCls;
 static JException videoEx;
+static JException interruptedEx;
 static JException nullPointerEx;
 static JException unsupportedEx;
 static JException exceptionEx;
@@ -41,41 +40,49 @@
 static JNIEnv* listenerEnv = nullptr;
 
 static const JClassInit classes[] = {
-    {"edu/wpi/cscore/UsbCameraInfo", &usbCameraInfoCls},
-    {"edu/wpi/cscore/VideoMode", &videoModeCls},
-    {"edu/wpi/cscore/VideoEvent", &videoEventCls},
-    {"edu/wpi/cscore/raw/RawFrame", &rawFrameCls}};
+    {"edu/wpi/first/cscore/UsbCameraInfo", &usbCameraInfoCls},
+    {"edu/wpi/first/cscore/VideoMode", &videoModeCls},
+    {"edu/wpi/first/cscore/VideoEvent", &videoEventCls},
+    {"edu/wpi/first/cscore/raw/RawFrame", &rawFrameCls}};
 
 static const JExceptionInit exceptions[] = {
-    {"edu/wpi/cscore/VideoException", &videoEx},
+    {"edu/wpi/first/cscore/VideoException", &videoEx},
+    {"java/lang/InterruptedException", &interruptedEx},
     {"java/lang/NullPointerException", &nullPointerEx},
     {"java/lang/UnsupportedOperationException", &unsupportedEx},
     {"java/lang/Exception", &exceptionEx}};
 
 static void ListenerOnStart() {
-  if (!jvm) return;
+  if (!jvm) {
+    return;
+  }
   JNIEnv* env;
   JavaVMAttachArgs args;
   args.version = JNI_VERSION_1_2;
   args.name = const_cast<char*>("CSListener");
   args.group = nullptr;
   if (jvm->AttachCurrentThreadAsDaemon(reinterpret_cast<void**>(&env), &args) !=
-      JNI_OK)
+      JNI_OK) {
     return;
-  if (!env || !env->functions) return;
+  }
+  if (!env || !env->functions) {
+    return;
+  }
   listenerEnv = env;
 }
 
 static void ListenerOnExit() {
   listenerEnv = nullptr;
-  if (!jvm) return;
+  if (!jvm) {
+    return;
+  }
   jvm->DetachCurrentThread();
 }
 
 /// throw java exception
 static void ThrowJavaException(JNIEnv* env, const std::exception* e) {
   wpi::SmallString<128> what;
-  jclass je = 0;
+  jclass je = nullptr;
 
   if (e) {
     const char* exception_type = "std::exception";
@@ -92,7 +99,9 @@
     what = "unknown exception";
   }
 
-  if (!je) je = exceptionEx;
+  if (!je) {
+    je = exceptionEx;
+  }
   env->ThrowNew(je, what.c_str());
 }
 
@@ -102,18 +111,23 @@
   jvm = vm;
 
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
+  }
 
   // Cache references to classes
   for (auto& c : classes) {
     *c.cls = JClass(env, c.name);
-    if (!*c.cls) return JNI_ERR;
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
   for (auto& c : exceptions) {
     *c.cls = JException(env, c.name);
-    if (!*c.cls) return JNI_ERR;
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
   // Initial configuration of listener start/exit
@@ -127,8 +141,9 @@
   cs::Shutdown();
 
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return;
+  }
   // Delete global references
   for (auto& c : classes) {
     c.cls->free(env);
@@ -150,22 +165,29 @@
   JCSGlobal(JNIEnv* env, T obj)
       : m_obj(static_cast<T>(env->NewGlobalRef(obj))) {}
   ~JCSGlobal() {
-    if (!jvm || cs::NotifierDestroyed()) return;
+    if (!jvm || cs::NotifierDestroyed()) {
+      return;
+    }
     JNIEnv* env;
     bool attached = false;
     // don't attach and de-attach if already attached to a thread.
     if (jvm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) ==
         JNI_EDETACHED) {
       if (jvm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) !=
-          JNI_OK)
+          JNI_OK) {
         return;
+      }
       attached = true;
     }
-    if (!env || !env->functions) return;
+    if (!env || !env->functions) {
+      return;
+    }
     env->DeleteGlobalRef(m_obj);
-    if (attached) jvm->DetachCurrentThread();
+    if (attached) {
+      jvm->DetachCurrentThread();
+    }
   }
-  operator T() { return m_obj; }
+  operator T() { return m_obj; }  // NOLINT
   T obj() { return m_obj; }
 
  private:
@@ -173,8 +195,11 @@
 };
 
 static void ReportError(JNIEnv* env, CS_Status status) {
-  if (status == CS_OK) return;
-  wpi::SmallString<64> msg;
+  if (status == CS_OK) {
+    return;
+  }
+  std::string_view msg;
+  std::string msgBuf;
   switch (status) {
     case CS_PROPERTY_WRITE_FAILED:
       msg = "property write failed";
@@ -207,8 +232,8 @@
       msg = "telemetry not enabled";
       break;
     default: {
-      wpi::raw_svector_ostream oss{msg};
-      oss << "unknown error code=" << status;
+      msgBuf = fmt::format("unknown error code={}", status);
+      msg = msgBuf;
       break;
     }
   }
@@ -216,7 +241,9 @@
 }
 
 static inline bool CheckStatus(JNIEnv* env, CS_Status status) {
-  if (status != CS_OK) ReportError(env, status);
+  if (status != CS_OK) {
+    ReportError(env, status);
+  }
   return status == CS_OK;
 }
 
@@ -245,7 +272,7 @@
 static jobject MakeJObject(JNIEnv* env, const cs::RawEvent& event) {
   static jmethodID constructor =
       env->GetMethodID(videoEventCls, "<init>",
-                       "(IIILjava/lang/String;IIIIIIILjava/lang/String;)V");
+                       "(IIILjava/lang/String;IIIIIIILjava/lang/String;I)V");
   JLocal<jstring> name(env, MakeJString(env, event.name));
   JLocal<jstring> valueStr(env, MakeJString(env, event.valueStr));
   // clang-format off
@@ -263,19 +290,33 @@
       static_cast<jint>(event.propertyHandle),
       static_cast<jint>(event.propertyKind),
       static_cast<jint>(event.value),
-      valueStr.obj());
+      valueStr.obj(),
+      static_cast<jint>(event.listener));
   // clang-format on
 }
 
+static jobjectArray MakeJObject(JNIEnv* env,
+                                wpi::span<const cs::RawEvent> arr) {
+  jobjectArray jarr = env->NewObjectArray(arr.size(), videoEventCls, nullptr);
+  if (!jarr) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
+    JLocal<jobject> elem{env, MakeJObject(env, arr[i])};
+    env->SetObjectArrayElement(jarr, i, elem.obj());
+  }
+  return jarr;
+}
+
 extern "C" {
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getPropertyKind
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getPropertyKind
+Java_edu_wpi_first_cscore_CameraServerJNI_getPropertyKind
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
@@ -285,28 +326,30 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getPropertyName
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getPropertyName
+Java_edu_wpi_first_cscore_CameraServerJNI_getPropertyName
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetPropertyName(property, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getProperty
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_getProperty
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
@@ -316,12 +359,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setProperty
  * Signature: (II)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_setProperty
   (JNIEnv* env, jclass, jint property, jint value)
 {
   CS_Status status = 0;
@@ -330,12 +373,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getPropertyMin
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getPropertyMin
+Java_edu_wpi_first_cscore_CameraServerJNI_getPropertyMin
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
@@ -345,12 +388,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getPropertyMax
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getPropertyMax
+Java_edu_wpi_first_cscore_CameraServerJNI_getPropertyMax
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
@@ -360,12 +403,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getPropertyStep
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getPropertyStep
+Java_edu_wpi_first_cscore_CameraServerJNI_getPropertyStep
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
@@ -375,12 +418,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getPropertyDefault
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getPropertyDefault
+Java_edu_wpi_first_cscore_CameraServerJNI_getPropertyDefault
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
@@ -390,28 +433,30 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getStringProperty
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getStringProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_getStringProperty
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetStringProperty(property, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setStringProperty
  * Signature: (ILjava/lang/String;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setStringProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_setStringProperty
   (JNIEnv* env, jclass, jint property, jstring value)
 {
   if (!value) {
@@ -424,27 +469,29 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getEnumPropertyChoices
  * Signature: (I)[Ljava/lang/Object;
  */
 JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getEnumPropertyChoices
+Java_edu_wpi_first_cscore_CameraServerJNI_getEnumPropertyChoices
   (JNIEnv* env, jclass, jint property)
 {
   CS_Status status = 0;
   auto arr = cs::GetEnumPropertyChoices(property, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJStringArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createUsbCameraDev
  * Signature: (Ljava/lang/String;I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createUsbCameraDev
+Java_edu_wpi_first_cscore_CameraServerJNI_createUsbCameraDev
   (JNIEnv* env, jclass, jstring name, jint dev)
 {
   if (!name) {
@@ -458,12 +505,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createUsbCameraPath
  * Signature: (Ljava/lang/String;Ljava/lang/String;)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createUsbCameraPath
+Java_edu_wpi_first_cscore_CameraServerJNI_createUsbCameraPath
   (JNIEnv* env, jclass, jstring name, jstring path)
 {
   if (!name) {
@@ -482,12 +529,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createHttpCamera
  * Signature: (Ljava/lang/String;Ljava/lang/String;I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createHttpCamera
+Java_edu_wpi_first_cscore_CameraServerJNI_createHttpCamera
   (JNIEnv* env, jclass, jstring name, jstring url, jint kind)
 {
   if (!name) {
@@ -507,12 +554,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createHttpCameraMulti
  * Signature: (Ljava/lang/String;[Ljava/lang/Object;I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createHttpCameraMulti
+Java_edu_wpi_first_cscore_CameraServerJNI_createHttpCameraMulti
   (JNIEnv* env, jclass, jstring name, jobjectArray urls, jint kind)
 {
   if (!name) {
@@ -533,7 +580,7 @@
       // TODO
       return 0;
     }
-    vec.push_back(JStringRef{env, elem}.str());
+    vec.emplace_back(JStringRef{env, elem}.str());
   }
   CS_Status status = 0;
   auto val =
@@ -544,12 +591,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerCvJNI
+ * Class:     edu_wpi_first_cscore_CameraServerCvJNI
  * Method:    createCvSource
  * Signature: (Ljava/lang/String;IIII)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerCvJNI_createCvSource
+Java_edu_wpi_first_cscore_CameraServerCvJNI_createCvSource
   (JNIEnv* env, jclass, jstring name, jint pixelFormat, jint width, jint height,
    jint fps)
 {
@@ -569,12 +616,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createRawSource
  * Signature: (Ljava/lang/String;IIII)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createRawSource
+Java_edu_wpi_first_cscore_CameraServerJNI_createRawSource
   (JNIEnv* env, jclass, jstring name, jint pixelFormat, jint width, jint height,
    jint fps)
 {
@@ -594,12 +641,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceKind
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceKind
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceKind
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -609,44 +656,48 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceName
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceName
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceName
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetSourceName(source, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceDescription
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceDescription
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceDescription
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetSourceDescription(source, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceLastFrameTime
  * Signature: (I)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceLastFrameTime
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceLastFrameTime
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -656,12 +707,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceConnectionStrategy
  * Signature: (II)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceConnectionStrategy
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceConnectionStrategy
   (JNIEnv* env, jclass, jint source, jint strategy)
 {
   CS_Status status = 0;
@@ -671,12 +722,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    isSourceConnected
  * Signature: (I)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_isSourceConnected
+Java_edu_wpi_first_cscore_CameraServerJNI_isSourceConnected
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -686,12 +737,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    isSourceEnabled
  * Signature: (I)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_isSourceEnabled
+Java_edu_wpi_first_cscore_CameraServerJNI_isSourceEnabled
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -701,12 +752,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceProperty
  * Signature: (ILjava/lang/String;)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceProperty
   (JNIEnv* env, jclass, jint source, jstring name)
 {
   if (!name) {
@@ -721,43 +772,47 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateSourceProperties
  * Signature: (I)[I
  */
 JNIEXPORT jintArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateSourceProperties
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateSourceProperties
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   wpi::SmallVector<CS_Property, 32> buf;
   auto arr = cs::EnumerateSourceProperties(source, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJIntArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceVideoMode
  * Signature: (I)Ljava/lang/Object;
  */
 JNIEXPORT jobject JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceVideoMode
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceVideoMode
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   auto val = cs::GetSourceVideoMode(source, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJObject(env, val);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceVideoMode
  * Signature: (IIIII)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceVideoMode
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceVideoMode
   (JNIEnv* env, jclass, jint source, jint pixelFormat, jint width, jint height,
    jint fps)
 {
@@ -772,12 +827,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourcePixelFormat
  * Signature: (II)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourcePixelFormat
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourcePixelFormat
   (JNIEnv* env, jclass, jint source, jint pixelFormat)
 {
   CS_Status status = 0;
@@ -788,12 +843,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceResolution
  * Signature: (III)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceResolution
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceResolution
   (JNIEnv* env, jclass, jint source, jint width, jint height)
 {
   CS_Status status = 0;
@@ -803,12 +858,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceFPS
  * Signature: (II)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceFPS
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceFPS
   (JNIEnv* env, jclass, jint source, jint fps)
 {
   CS_Status status = 0;
@@ -818,12 +873,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceConfigJson
  * Signature: (ILjava/lang/String;)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceConfigJson
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceConfigJson
   (JNIEnv* env, jclass, jint source, jstring config)
 {
   CS_Status status = 0;
@@ -833,12 +888,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSourceConfigJson
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSourceConfigJson
+Java_edu_wpi_first_cscore_CameraServerJNI_getSourceConfigJson
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -848,19 +903,23 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateSourceVideoModes
  * Signature: (I)[Ljava/lang/Object;
  */
 JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateSourceVideoModes
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateSourceVideoModes
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   auto arr = cs::EnumerateSourceVideoModes(source, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   jobjectArray jarr = env->NewObjectArray(arr.size(), videoModeCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> jelem{env, MakeJObject(env, arr[i])};
     env->SetObjectArrayElement(jarr, i, jelem);
@@ -869,28 +928,30 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateSourceSinks
  * Signature: (I)[I
  */
 JNIEXPORT jintArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateSourceSinks
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateSourceSinks
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   wpi::SmallVector<CS_Sink, 16> buf;
   auto arr = cs::EnumerateSourceSinks(source, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJIntArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    copySource
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_copySource
+Java_edu_wpi_first_cscore_CameraServerJNI_copySource
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -900,12 +961,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    releaseSource
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_releaseSource
+Java_edu_wpi_first_cscore_CameraServerJNI_releaseSource
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -914,12 +975,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraBrightness
  * Signature: (II)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraBrightness
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraBrightness
   (JNIEnv* env, jclass, jint source, jint brightness)
 {
   CS_Status status = 0;
@@ -928,12 +989,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getCameraBrightness
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getCameraBrightness
+Java_edu_wpi_first_cscore_CameraServerJNI_getCameraBrightness
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -943,12 +1004,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraWhiteBalanceAuto
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraWhiteBalanceAuto
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraWhiteBalanceAuto
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -957,12 +1018,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraWhiteBalanceHoldCurrent
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraWhiteBalanceHoldCurrent
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraWhiteBalanceHoldCurrent
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -971,12 +1032,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraWhiteBalanceManual
  * Signature: (II)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraWhiteBalanceManual
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraWhiteBalanceManual
   (JNIEnv* env, jclass, jint source, jint value)
 {
   CS_Status status = 0;
@@ -985,12 +1046,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraExposureAuto
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraExposureAuto
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraExposureAuto
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -999,12 +1060,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraExposureHoldCurrent
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraExposureHoldCurrent
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraExposureHoldCurrent
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -1013,12 +1074,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setCameraExposureManual
  * Signature: (II)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setCameraExposureManual
+Java_edu_wpi_first_cscore_CameraServerJNI_setCameraExposureManual
   (JNIEnv* env, jclass, jint source, jint value)
 {
   CS_Status status = 0;
@@ -1027,12 +1088,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setUsbCameraPath
  * Signature: (ILjava/lang/String;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setUsbCameraPath
+Java_edu_wpi_first_cscore_CameraServerJNI_setUsbCameraPath
   (JNIEnv* env, jclass, jint source, jstring path)
 {
   CS_Status status = 0;
@@ -1041,57 +1102,63 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getUsbCameraPath
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getUsbCameraPath
+Java_edu_wpi_first_cscore_CameraServerJNI_getUsbCameraPath
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   auto str = cs::GetUsbCameraPath(source, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getUsbCameraInfo
  * Signature: (I)Ljava/lang/Object;
  */
 JNIEXPORT jobject JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getUsbCameraInfo
+Java_edu_wpi_first_cscore_CameraServerJNI_getUsbCameraInfo
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   auto info = cs::GetUsbCameraInfo(source, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJObject(env, info);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getHttpCameraKind
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getHttpCameraKind
+Java_edu_wpi_first_cscore_CameraServerJNI_getHttpCameraKind
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   auto kind = cs::GetHttpCameraKind(source, &status);
-  if (!CheckStatus(env, status)) return 0;
+  if (!CheckStatus(env, status)) {
+    return 0;
+  }
   return kind;
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setHttpCameraUrls
  * Signature: (I[Ljava/lang/Object;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setHttpCameraUrls
+Java_edu_wpi_first_cscore_CameraServerJNI_setHttpCameraUrls
   (JNIEnv* env, jclass, jint source, jobjectArray urls)
 {
   if (!urls) {
@@ -1108,7 +1175,7 @@
       // TODO
       return;
     }
-    vec.push_back(JStringRef{env, elem}.str());
+    vec.emplace_back(JStringRef{env, elem}.str());
   }
   CS_Status status = 0;
   cs::SetHttpCameraUrls(source, vec, &status);
@@ -1116,27 +1183,29 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getHttpCameraUrls
  * Signature: (I)[Ljava/lang/Object;
  */
 JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getHttpCameraUrls
+Java_edu_wpi_first_cscore_CameraServerJNI_getHttpCameraUrls
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   auto arr = cs::GetHttpCameraUrls(source, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJStringArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerCvJNI
+ * Class:     edu_wpi_first_cscore_CameraServerCvJNI
  * Method:    putSourceFrame
  * Signature: (IJ)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerCvJNI_putSourceFrame
+Java_edu_wpi_first_cscore_CameraServerCvJNI_putSourceFrame
   (JNIEnv* env, jclass, jint source, jlong imageNativeObj)
 {
   try {
@@ -1147,19 +1216,19 @@
   } catch (const std::exception& e) {
     ThrowJavaException(env, &e);
   } catch (...) {
-    ThrowJavaException(env, 0);
+    ThrowJavaException(env, nullptr);
   }
 }
 
 // int width, int height, int pixelFormat, int totalData
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    putRawSourceFrameBB
  * Signature: (ILjava/lang/Object;IIII)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_putRawSourceFrameBB
+Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrameBB
   (JNIEnv* env, jclass, jint source, jobject byteBuffer, jint width,
    jint height, jint pixelFormat, jint totalData)
 {
@@ -1176,12 +1245,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    putRawSourceFrame
  * Signature: (IJIIII)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_putRawSourceFrame
+Java_edu_wpi_first_cscore_CameraServerJNI_putRawSourceFrame
   (JNIEnv* env, jclass, jint source, jlong ptr, jint width, jint height,
    jint pixelFormat, jint totalData)
 {
@@ -1197,12 +1266,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    notifySourceError
  * Signature: (ILjava/lang/String;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_notifySourceError
+Java_edu_wpi_first_cscore_CameraServerJNI_notifySourceError
   (JNIEnv* env, jclass, jint source, jstring msg)
 {
   if (!msg) {
@@ -1215,12 +1284,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceConnected
  * Signature: (IZ)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceConnected
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceConnected
   (JNIEnv* env, jclass, jint source, jboolean connected)
 {
   CS_Status status = 0;
@@ -1229,12 +1298,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceDescription
  * Signature: (ILjava/lang/String;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceDescription
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceDescription
   (JNIEnv* env, jclass, jint source, jstring description)
 {
   if (!description) {
@@ -1247,12 +1316,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createSourceProperty
  * Signature: (ILjava/lang/String;IIIIII)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createSourceProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_createSourceProperty
   (JNIEnv* env, jclass, jint source, jstring name, jint kind, jint minimum,
    jint maximum, jint step, jint defaultValue, jint value)
 {
@@ -1265,12 +1334,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSourceEnumPropertyChoices
  * Signature: (II[Ljava/lang/Object;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSourceEnumPropertyChoices
+Java_edu_wpi_first_cscore_CameraServerJNI_setSourceEnumPropertyChoices
   (JNIEnv* env, jclass, jint source, jint property, jobjectArray choices)
 {
   if (!choices) {
@@ -1287,7 +1356,7 @@
       // TODO
       return;
     }
-    vec.push_back(JStringRef{env, elem}.str());
+    vec.emplace_back(JStringRef{env, elem}.str());
   }
   CS_Status status = 0;
   cs::SetSourceEnumPropertyChoices(source, property, vec, &status);
@@ -1295,12 +1364,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createMjpegServer
  * Signature: (Ljava/lang/String;Ljava/lang/String;I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createMjpegServer
+Java_edu_wpi_first_cscore_CameraServerJNI_createMjpegServer
   (JNIEnv* env, jclass, jstring name, jstring listenAddress, jint port)
 {
   if (!name) {
@@ -1320,12 +1389,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerCvJNI
+ * Class:     edu_wpi_first_cscore_CameraServerCvJNI
  * Method:    createCvSink
  * Signature: (Ljava/lang/String;)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerCvJNI_createCvSink
+Java_edu_wpi_first_cscore_CameraServerCvJNI_createCvSink
   (JNIEnv* env, jclass, jstring name)
 {
   if (!name) {
@@ -1339,12 +1408,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    createRawSink
  * Signature: (Ljava/lang/String;)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_createRawSink
+Java_edu_wpi_first_cscore_CameraServerJNI_createRawSink
   (JNIEnv* env, jclass, jstring name)
 {
   if (!name) {
@@ -1358,12 +1427,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkKind
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkKind
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkKind
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
@@ -1373,44 +1442,48 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkName
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkName
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkName
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetSinkName(sink, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkDescription
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkDescription
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkDescription
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetSinkDescription(sink, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkProperty
  * Signature: (ILjava/lang/String;)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkProperty
   (JNIEnv* env, jclass, jint sink, jstring name)
 {
   if (!name) {
@@ -1424,28 +1497,30 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateSinkProperties
  * Signature: (I)[I
  */
 JNIEXPORT jintArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateSinkProperties
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateSinkProperties
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
   wpi::SmallVector<CS_Property, 32> buf;
   auto arr = cs::EnumerateSinkProperties(source, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJIntArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSinkConfigJson
  * Signature: (ILjava/lang/String;)Z
  */
 JNIEXPORT jboolean JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSinkConfigJson
+Java_edu_wpi_first_cscore_CameraServerJNI_setSinkConfigJson
   (JNIEnv* env, jclass, jint source, jstring config)
 {
   CS_Status status = 0;
@@ -1455,12 +1530,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkConfigJson
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkConfigJson
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkConfigJson
   (JNIEnv* env, jclass, jint source)
 {
   CS_Status status = 0;
@@ -1470,12 +1545,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSinkSource
  * Signature: (II)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSinkSource
+Java_edu_wpi_first_cscore_CameraServerJNI_setSinkSource
   (JNIEnv* env, jclass, jint sink, jint source)
 {
   CS_Status status = 0;
@@ -1484,12 +1559,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkSourceProperty
  * Signature: (ILjava/lang/String;)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkSourceProperty
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkSourceProperty
   (JNIEnv* env, jclass, jint sink, jstring name)
 {
   if (!name) {
@@ -1504,12 +1579,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkSource
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkSource
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkSource
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
@@ -1519,12 +1594,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    copySink
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_copySink
+Java_edu_wpi_first_cscore_CameraServerJNI_copySink
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
@@ -1534,12 +1609,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    releaseSink
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_releaseSink
+Java_edu_wpi_first_cscore_CameraServerJNI_releaseSink
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
@@ -1548,27 +1623,29 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getMjpegServerListenAddress
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getMjpegServerListenAddress
+Java_edu_wpi_first_cscore_CameraServerJNI_getMjpegServerListenAddress
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
   auto str = cs::GetMjpegServerListenAddress(sink, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getMjpegServerPort
  * Signature: (I)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getMjpegServerPort
+Java_edu_wpi_first_cscore_CameraServerJNI_getMjpegServerPort
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
@@ -1578,12 +1655,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSinkDescription
  * Signature: (ILjava/lang/String;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSinkDescription
+Java_edu_wpi_first_cscore_CameraServerJNI_setSinkDescription
   (JNIEnv* env, jclass, jint sink, jstring description)
 {
   if (!description) {
@@ -1596,12 +1673,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerCvJNI
+ * Class:     edu_wpi_first_cscore_CameraServerCvJNI
  * Method:    grabSinkFrame
  * Signature: (IJ)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerCvJNI_grabSinkFrame
+Java_edu_wpi_first_cscore_CameraServerCvJNI_grabSinkFrame
   (JNIEnv* env, jclass, jint sink, jlong imageNativeObj)
 {
   try {
@@ -1614,18 +1691,18 @@
     ThrowJavaException(env, &e);
     return 0;
   } catch (...) {
-    ThrowJavaException(env, 0);
+    ThrowJavaException(env, nullptr);
     return 0;
   }
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerCvJNI
+ * Class:     edu_wpi_first_cscore_CameraServerCvJNI
  * Method:    grabSinkFrameTimeout
  * Signature: (IJD)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerCvJNI_grabSinkFrameTimeout
+Java_edu_wpi_first_cscore_CameraServerCvJNI_grabSinkFrameTimeout
   (JNIEnv* env, jclass, jint sink, jlong imageNativeObj, jdouble timeout)
 {
   try {
@@ -1638,7 +1715,7 @@
     ThrowJavaException(env, &e);
     return 0;
   } catch (...) {
-    ThrowJavaException(env, 0);
+    ThrowJavaException(env, nullptr);
     return 0;
   }
 }
@@ -1661,12 +1738,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    grabRawSinkFrameImpl
  * Signature: (ILjava/lang/Object;JLjava/lang/Object;III)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_grabRawSinkFrameImpl
+Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameImpl
   (JNIEnv* env, jclass, jint sink, jobject rawFrameObj, jlong rawFramePtr,
    jobject byteBuffer, jint width, jint height, jint pixelFormat)
 {
@@ -1686,12 +1763,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    grabRawSinkFrameTimeoutImpl
  * Signature: (ILjava/lang/Object;JLjava/lang/Object;IIID)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_grabRawSinkFrameTimeoutImpl
+Java_edu_wpi_first_cscore_CameraServerJNI_grabRawSinkFrameTimeoutImpl
   (JNIEnv* env, jclass, jint sink, jobject rawFrameObj, jlong rawFramePtr,
    jobject byteBuffer, jint width, jint height, jint pixelFormat,
    jdouble timeout)
@@ -1713,28 +1790,30 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getSinkError
  * Signature: (I)Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getSinkError
+Java_edu_wpi_first_cscore_CameraServerJNI_getSinkError
   (JNIEnv* env, jclass, jint sink)
 {
   CS_Status status = 0;
   wpi::SmallString<128> buf;
   auto str = cs::GetSinkError(sink, buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJString(env, str);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setSinkEnabled
  * Signature: (IZ)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setSinkEnabled
+Java_edu_wpi_first_cscore_CameraServerJNI_setSinkEnabled
   (JNIEnv* env, jclass, jint sink, jboolean enabled)
 {
   CS_Status status = 0;
@@ -1743,12 +1822,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    addListener
  * Signature: (Ljava/lang/Object;IZ)I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_addListener
+Java_edu_wpi_first_cscore_CameraServerJNI_addListener
   (JNIEnv* envouter, jclass, jobject listener, jint eventMask,
    jboolean immediateNotify)
 {
@@ -1763,17 +1842,23 @@
 
   // cls is a temporary here; cannot be used within callback functor
   jclass cls = envouter->GetObjectClass(listener);
-  if (!cls) return 0;
+  if (!cls) {
+    return 0;
+  }
 
   // method ids, on the other hand, are safe to retain
   jmethodID mid = envouter->GetMethodID(cls, "accept", "(Ljava/lang/Object;)V");
-  if (!mid) return 0;
+  if (!mid) {
+    return 0;
+  }
 
   CS_Status status = 0;
   CS_Listener handle = cs::AddListener(
       [=](const cs::RawEvent& event) {
         JNIEnv* env = listenerEnv;
-        if (!env || !env->functions) return;
+        if (!env || !env->functions) {
+          return;
+        }
 
         // get the handler
         auto handler = listener_global->obj();
@@ -1785,7 +1870,9 @@
           env->ExceptionClear();
           return;
         }
-        if (!jobj) return;
+        if (!jobj) {
+          return;
+        }
 
         env->CallVoidMethod(handler, mid, jobj.obj());
         if (env->ExceptionCheck()) {
@@ -1799,12 +1886,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    removeListener
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_removeListener
+Java_edu_wpi_first_cscore_CameraServerJNI_removeListener
   (JNIEnv* env, jclass, jint handle)
 {
   CS_Status status = 0;
@@ -1813,36 +1900,122 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    createListenerPoller
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_createListenerPoller
+  (JNIEnv*, jclass)
+{
+  return cs::CreateListenerPoller();
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    destroyListenerPoller
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_destroyListenerPoller
+  (JNIEnv*, jclass, jint poller)
+{
+  cs::DestroyListenerPoller(poller);
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    addPolledListener
+ * Signature: (IIZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_addPolledListener
+  (JNIEnv* env, jclass, jint poller, jint eventMask, jboolean immediateNotify)
+{
+  CS_Status status = 0;
+  auto rv = cs::AddPolledListener(poller, eventMask, immediateNotify, &status);
+  CheckStatus(env, status);
+  return rv;
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    pollListener
+ * Signature: (I)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_pollListener
+  (JNIEnv* env, jclass, jint poller)
+{
+  auto events = cs::PollListener(poller);
+  if (events.empty()) {
+    interruptedEx.Throw(env, "PollListener interrupted");
+    return nullptr;
+  }
+  return MakeJObject(env, events);
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    pollListenerTimeout
+ * Signature: (ID)[Ljava/lang/Object;
+ */
+JNIEXPORT jobjectArray JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_pollListenerTimeout
+  (JNIEnv* env, jclass, jint poller, jdouble timeout)
+{
+  bool timed_out = false;
+  auto events = cs::PollListener(poller, timeout, &timed_out);
+  if (events.empty() && !timed_out) {
+    interruptedEx.Throw(env, "PollListener interrupted");
+    return nullptr;
+  }
+  return MakeJObject(env, events);
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
+ * Method:    cancelPollListener
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_cscore_CameraServerJNI_cancelPollListener
+  (JNIEnv*, jclass, jint poller)
+{
+  cs::CancelPollListener(poller);
+}
+
+/*
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setTelemetryPeriod
  * Signature: (D)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setTelemetryPeriod
+Java_edu_wpi_first_cscore_CameraServerJNI_setTelemetryPeriod
   (JNIEnv* env, jclass, jdouble seconds)
 {
   cs::SetTelemetryPeriod(seconds);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getTelemetryElapsedTime
  * Signature: ()D
  */
 JNIEXPORT jdouble JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getTelemetryElapsedTime
+Java_edu_wpi_first_cscore_CameraServerJNI_getTelemetryElapsedTime
   (JNIEnv* env, jclass)
 {
   return cs::GetTelemetryElapsedTime();
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getTelemetryValue
  * Signature: (II)J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getTelemetryValue
+Java_edu_wpi_first_cscore_CameraServerJNI_getTelemetryValue
   (JNIEnv* env, jclass, jint handle, jint kind)
 {
   CS_Status status = 0;
@@ -1853,12 +2026,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getTelemetryAverageValue
  * Signature: (II)D
  */
 JNIEXPORT jdouble JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getTelemetryAverageValue
+Java_edu_wpi_first_cscore_CameraServerJNI_getTelemetryAverageValue
   (JNIEnv* env, jclass, jint handle, jint kind)
 {
   CS_Status status = 0;
@@ -1869,20 +2042,24 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateUsbCameras
  * Signature: ()[Ljava/lang/Object;
  */
 JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateUsbCameras
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateUsbCameras
   (JNIEnv* env, jclass)
 {
   CS_Status status = 0;
   auto arr = cs::EnumerateUsbCameras(&status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   jobjectArray jarr =
       env->NewObjectArray(arr.size(), usbCameraInfoCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> jelem{env, MakeJObject(env, arr[i])};
     env->SetObjectArrayElement(jarr, i, jelem);
@@ -1891,56 +2068,60 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateSources
  * Signature: ()[I
  */
 JNIEXPORT jintArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateSources
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateSources
   (JNIEnv* env, jclass)
 {
   CS_Status status = 0;
   wpi::SmallVector<CS_Source, 16> buf;
   auto arr = cs::EnumerateSourceHandles(buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJIntArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    enumerateSinks
  * Signature: ()[I
  */
 JNIEXPORT jintArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_enumerateSinks
+Java_edu_wpi_first_cscore_CameraServerJNI_enumerateSinks
   (JNIEnv* env, jclass)
 {
   CS_Status status = 0;
   wpi::SmallVector<CS_Sink, 16> buf;
   auto arr = cs::EnumerateSinkHandles(buf, &status);
-  if (!CheckStatus(env, status)) return nullptr;
+  if (!CheckStatus(env, status)) {
+    return nullptr;
+  }
   return MakeJIntArray(env, arr);
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getHostname
  * Signature: ()Ljava/lang/String;
  */
 JNIEXPORT jstring JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getHostname
+Java_edu_wpi_first_cscore_CameraServerJNI_getHostname
   (JNIEnv* env, jclass)
 {
   return MakeJString(env, cs::GetHostname());
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    getNetworkInterfaces
  * Signature: ()[Ljava/lang/Object;
  */
 JNIEXPORT jobjectArray JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_getNetworkInterfaces
+Java_edu_wpi_first_cscore_CameraServerJNI_getNetworkInterfaces
   (JNIEnv* env, jclass)
 {
   return MakeJStringArray(env, cs::GetNetworkInterfaces());
@@ -1959,8 +2140,8 @@
   void CallJava(JNIEnv* env, jobject func, jmethodID mid) {
     JLocal<jstring> file{env, MakeJString(env, m_file)};
     JLocal<jstring> msg{env, MakeJString(env, m_msg)};
-    env->CallVoidMethod(func, mid, (jint)m_level, file.obj(), (jint)m_line,
-                        msg.obj());
+    env->CallVoidMethod(func, mid, static_cast<jint>(m_level), file.obj(),
+                        static_cast<jint>(m_line), msg.obj());
   }
 
   static const char* GetName() { return "CSLogger"; }
@@ -1973,19 +2154,19 @@
   std::string m_msg;
 };
 
-typedef JSingletonCallbackManager<LogMessage> LoggerJNI;
+using LoggerJNI = JSingletonCallbackManager<LogMessage>;
 
 }  // namespace
 
 extern "C" {
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    setLogger
  * Signature: (Ljava/lang/Object;I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_setLogger
+Java_edu_wpi_first_cscore_CameraServerJNI_setLogger
   (JNIEnv* env, jclass, jobject func, jint minLevel)
 {
   if (!func) {
@@ -1994,12 +2175,16 @@
   }
   // cls is a temporary here; cannot be used within callback functor
   jclass cls = env->GetObjectClass(func);
-  if (!cls) return;
+  if (!cls) {
+    return;
+  }
 
   // method ids, on the other hand, are safe to retain
   jmethodID mid = env->GetMethodID(cls, "apply",
                                    "(ILjava/lang/String;ILjava/lang/String;)V");
-  if (!mid) return;
+  if (!mid) {
+    return;
+  }
 
   auto& logger = LoggerJNI::GetInstance();
   logger.Start();
@@ -2014,12 +2199,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    allocateRawFrame
  * Signature: ()J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_allocateRawFrame
+Java_edu_wpi_first_cscore_CameraServerJNI_allocateRawFrame
   (JNIEnv*, jclass)
 {
   cs::RawFrame* rawFrame = new cs::RawFrame{};
@@ -2028,12 +2213,12 @@
 }
 
 /*
- * Class:     edu_wpi_cscore_CameraServerJNI
+ * Class:     edu_wpi_first_cscore_CameraServerJNI
  * Method:    freeRawFrame
  * Signature: (J)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_cscore_CameraServerJNI_freeRawFrame
+Java_edu_wpi_first_cscore_CameraServerJNI_freeRawFrame
   (JNIEnv*, jclass, jlong rawFrame)
 {
   cs::RawFrame* ptr =
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore.h b/third_party/allwpilib/cscore/src/main/native/include/cscore.h
index eff3856..085d6c7 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_H_
 #define CSCORE_CSCORE_H_
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h
index ee5d33a..76bd836 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_c.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_C_H_
 #define CSCORE_CSCORE_C_H_
@@ -49,6 +46,7 @@
 typedef int CS_Handle;
 typedef CS_Handle CS_Property;
 typedef CS_Handle CS_Listener;
+typedef CS_Handle CS_ListenerPoller;
 typedef CS_Handle CS_Sink;
 typedef CS_Handle CS_Source;
 /** @} */
@@ -172,7 +170,8 @@
   CS_TELEMETRY_UPDATED = 0x8000,
   CS_SINK_PROPERTY_CREATED = 0x10000,
   CS_SINK_PROPERTY_VALUE_UPDATED = 0x20000,
-  CS_SINK_PROPERTY_CHOICES_UPDATED = 0x40000
+  CS_SINK_PROPERTY_CHOICES_UPDATED = 0x40000,
+  CS_USB_CAMERAS_CHANGED = 0x80000
 };
 
 /**
@@ -225,6 +224,9 @@
   enum CS_PropertyKind propertyKind;
   int value;
   const char* valueStr;
+
+  // Listener that was triggered
+  CS_Listener listener;
 };
 
 /**
@@ -432,9 +434,19 @@
 void CS_SetListenerOnExit(void (*onExit)(void* data), void* data);
 CS_Listener CS_AddListener(
     void* data, void (*callback)(void* data, const struct CS_Event* event),
-    int eventMask, int immediateNotify, CS_Status* status);
+    int eventMask, CS_Bool immediateNotify, CS_Status* status);
 
 void CS_RemoveListener(CS_Listener handle, CS_Status* status);
+
+CS_ListenerPoller CS_CreateListenerPoller(void);
+void CS_DestroyListenerPoller(CS_ListenerPoller poller);
+CS_Listener CS_AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                                 CS_Bool immediateNotify, CS_Status* status);
+struct CS_Event* CS_PollListener(CS_ListenerPoller poller, int* count);
+struct CS_Event* CS_PollListenerTimeout(CS_ListenerPoller poller, int* count,
+                                        double timeout, CS_Bool* timedOut);
+void CS_FreeEvents(struct CS_Event* arr, int count);
+void CS_CancelPollListener(CS_ListenerPoller poller);
 /** @} */
 
 int CS_NotifierDestroyed(void);
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h
index 1ee1615..8b1eab1 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_cpp.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_CPP_H_
 #define CSCORE_CSCORE_CPP_H_
@@ -12,12 +9,11 @@
 
 #include <functional>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
 #include "cscore_c.h"
 
@@ -119,34 +115,36 @@
     kTelemetryUpdated = CS_TELEMETRY_UPDATED,
     kSinkPropertyCreated = CS_SINK_PROPERTY_CREATED,
     kSinkPropertyValueUpdated = CS_SINK_PROPERTY_VALUE_UPDATED,
-    kSinkPropertyChoicesUpdated = CS_SINK_PROPERTY_CHOICES_UPDATED
+    kSinkPropertyChoicesUpdated = CS_SINK_PROPERTY_CHOICES_UPDATED,
+    kUsbCamerasChanged = CS_USB_CAMERAS_CHANGED
   };
 
   RawEvent() = default;
   explicit RawEvent(RawEvent::Kind kind_) : kind{kind_} {}
-  RawEvent(const wpi::Twine& name_, CS_Handle handle_, RawEvent::Kind kind_)
-      : kind{kind_}, name{name_.str()} {
+  RawEvent(std::string_view name_, CS_Handle handle_, RawEvent::Kind kind_)
+      : kind{kind_}, name{name_} {
     if (kind_ == kSinkCreated || kind_ == kSinkDestroyed ||
-        kind_ == kSinkEnabled || kind_ == kSinkDisabled)
+        kind_ == kSinkEnabled || kind_ == kSinkDisabled) {
       sinkHandle = handle_;
-    else
+    } else {
       sourceHandle = handle_;
+    }
   }
-  RawEvent(const wpi::Twine& name_, CS_Source source_, const VideoMode& mode_)
+  RawEvent(std::string_view name_, CS_Source source_, const VideoMode& mode_)
       : kind{kSourceVideoModeChanged},
         sourceHandle{source_},
-        name{name_.str()},
+        name{name_},
         mode{mode_} {}
-  RawEvent(const wpi::Twine& name_, CS_Source source_, RawEvent::Kind kind_,
+  RawEvent(std::string_view name_, CS_Source source_, RawEvent::Kind kind_,
            CS_Property property_, CS_PropertyKind propertyKind_, int value_,
-           const wpi::Twine& valueStr_)
+           std::string_view valueStr_)
       : kind{kind_},
         sourceHandle{source_},
-        name{name_.str()},
+        name{name_},
         propertyHandle{property_},
         propertyKind{propertyKind_},
         value{value_},
-        valueStr{valueStr_.str()} {}
+        valueStr{valueStr_} {}
 
   Kind kind;
 
@@ -165,6 +163,9 @@
   CS_PropertyKind propertyKind;
   int value;
   std::string valueStr;
+
+  // Listener that was triggered
+  CS_Listener listener{0};
 };
 
 /**
@@ -173,9 +174,9 @@
  */
 CS_PropertyKind GetPropertyKind(CS_Property property, CS_Status* status);
 std::string GetPropertyName(CS_Property property, CS_Status* status);
-wpi::StringRef GetPropertyName(CS_Property property,
-                               wpi::SmallVectorImpl<char>& buf,
-                               CS_Status* status);
+std::string_view GetPropertyName(CS_Property property,
+                                 wpi::SmallVectorImpl<char>& buf,
+                                 CS_Status* status);
 int GetProperty(CS_Property property, CS_Status* status);
 void SetProperty(CS_Property property, int value, CS_Status* status);
 int GetPropertyMin(CS_Property property, CS_Status* status);
@@ -183,10 +184,10 @@
 int GetPropertyStep(CS_Property property, CS_Status* status);
 int GetPropertyDefault(CS_Property property, CS_Status* status);
 std::string GetStringProperty(CS_Property property, CS_Status* status);
-wpi::StringRef GetStringProperty(CS_Property property,
-                                 wpi::SmallVectorImpl<char>& buf,
-                                 CS_Status* status);
-void SetStringProperty(CS_Property property, const wpi::Twine& value,
+std::string_view GetStringProperty(CS_Property property,
+                                   wpi::SmallVectorImpl<char>& buf,
+                                   CS_Status* status);
+void SetStringProperty(CS_Property property, std::string_view value,
                        CS_Status* status);
 std::vector<std::string> GetEnumPropertyChoices(CS_Property property,
                                                 CS_Status* status);
@@ -196,16 +197,15 @@
  * @defgroup cscore_source_create_func Source Creation Functions
  * @{
  */
-CS_Source CreateUsbCameraDev(const wpi::Twine& name, int dev,
-                             CS_Status* status);
-CS_Source CreateUsbCameraPath(const wpi::Twine& name, const wpi::Twine& path,
+CS_Source CreateUsbCameraDev(std::string_view name, int dev, CS_Status* status);
+CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
                               CS_Status* status);
-CS_Source CreateHttpCamera(const wpi::Twine& name, const wpi::Twine& url,
+CS_Source CreateHttpCamera(std::string_view name, std::string_view url,
                            CS_HttpCameraKind kind, CS_Status* status);
-CS_Source CreateHttpCamera(const wpi::Twine& name,
-                           wpi::ArrayRef<std::string> urls,
+CS_Source CreateHttpCamera(std::string_view name,
+                           wpi::span<const std::string> urls,
                            CS_HttpCameraKind kind, CS_Status* status);
-CS_Source CreateCvSource(const wpi::Twine& name, const VideoMode& mode,
+CS_Source CreateCvSource(std::string_view name, const VideoMode& mode,
                          CS_Status* status);
 /** @} */
 
@@ -215,21 +215,22 @@
  */
 CS_SourceKind GetSourceKind(CS_Source source, CS_Status* status);
 std::string GetSourceName(CS_Source source, CS_Status* status);
-wpi::StringRef GetSourceName(CS_Source source, wpi::SmallVectorImpl<char>& buf,
-                             CS_Status* status);
+std::string_view GetSourceName(CS_Source source,
+                               wpi::SmallVectorImpl<char>& buf,
+                               CS_Status* status);
 std::string GetSourceDescription(CS_Source source, CS_Status* status);
-wpi::StringRef GetSourceDescription(CS_Source source,
-                                    wpi::SmallVectorImpl<char>& buf,
-                                    CS_Status* status);
+std::string_view GetSourceDescription(CS_Source source,
+                                      wpi::SmallVectorImpl<char>& buf,
+                                      CS_Status* status);
 uint64_t GetSourceLastFrameTime(CS_Source source, CS_Status* status);
 void SetSourceConnectionStrategy(CS_Source source,
                                  CS_ConnectionStrategy strategy,
                                  CS_Status* status);
 bool IsSourceConnected(CS_Source source, CS_Status* status);
 bool IsSourceEnabled(CS_Source source, CS_Status* status);
-CS_Property GetSourceProperty(CS_Source source, const wpi::Twine& name,
+CS_Property GetSourceProperty(CS_Source source, std::string_view name,
                               CS_Status* status);
-wpi::ArrayRef<CS_Property> EnumerateSourceProperties(
+wpi::span<CS_Property> EnumerateSourceProperties(
     CS_Source source, wpi::SmallVectorImpl<CS_Property>& vec,
     CS_Status* status);
 VideoMode GetSourceVideoMode(CS_Source source, CS_Status* status);
@@ -240,7 +241,7 @@
 bool SetSourceResolution(CS_Source source, int width, int height,
                          CS_Status* status);
 bool SetSourceFPS(CS_Source source, int fps, CS_Status* status);
-bool SetSourceConfigJson(CS_Source source, wpi::StringRef config,
+bool SetSourceConfigJson(CS_Source source, std::string_view config,
                          CS_Status* status);
 bool SetSourceConfigJson(CS_Source source, const wpi::json& config,
                          CS_Status* status);
@@ -248,9 +249,9 @@
 wpi::json GetSourceConfigJsonObject(CS_Source source, CS_Status* status);
 std::vector<VideoMode> EnumerateSourceVideoModes(CS_Source source,
                                                  CS_Status* status);
-wpi::ArrayRef<CS_Sink> EnumerateSourceSinks(CS_Source source,
-                                            wpi::SmallVectorImpl<CS_Sink>& vec,
-                                            CS_Status* status);
+wpi::span<CS_Sink> EnumerateSourceSinks(CS_Source source,
+                                        wpi::SmallVectorImpl<CS_Sink>& vec,
+                                        CS_Status* status);
 CS_Source CopySource(CS_Source source, CS_Status* status);
 void ReleaseSource(CS_Source source, CS_Status* status);
 /** @} */
@@ -274,7 +275,7 @@
  * @defgroup cscore_usbcamera_func UsbCamera Source Functions
  * @{
  */
-void SetUsbCameraPath(CS_Source, const wpi::Twine& path, CS_Status* status);
+void SetUsbCameraPath(CS_Source, std::string_view path, CS_Status* status);
 std::string GetUsbCameraPath(CS_Source source, CS_Status* status);
 UsbCameraInfo GetUsbCameraInfo(CS_Source source, CS_Status* status);
 /** @} */
@@ -284,7 +285,7 @@
  * @{
  */
 CS_HttpCameraKind GetHttpCameraKind(CS_Source source, CS_Status* status);
-void SetHttpCameraUrls(CS_Source source, wpi::ArrayRef<std::string> urls,
+void SetHttpCameraUrls(CS_Source source, wpi::span<const std::string> urls,
                        CS_Status* status);
 std::vector<std::string> GetHttpCameraUrls(CS_Source source, CS_Status* status);
 /** @} */
@@ -293,17 +294,17 @@
  * @defgroup cscore_opencv_source_func OpenCV Source Functions
  * @{
  */
-void NotifySourceError(CS_Source source, const wpi::Twine& msg,
+void NotifySourceError(CS_Source source, std::string_view msg,
                        CS_Status* status);
 void SetSourceConnected(CS_Source source, bool connected, CS_Status* status);
-void SetSourceDescription(CS_Source source, const wpi::Twine& description,
+void SetSourceDescription(CS_Source source, std::string_view description,
                           CS_Status* status);
-CS_Property CreateSourceProperty(CS_Source source, const wpi::Twine& name,
+CS_Property CreateSourceProperty(CS_Source source, std::string_view name,
                                  CS_PropertyKind kind, int minimum, int maximum,
                                  int step, int defaultValue, int value,
                                  CS_Status* status);
 void SetSourceEnumPropertyChoices(CS_Source source, CS_Property property,
-                                  wpi::ArrayRef<std::string> choices,
+                                  wpi::span<const std::string> choices,
                                   CS_Status* status);
 /** @} */
 
@@ -311,11 +312,10 @@
  * @defgroup cscore_sink_create_func Sink Creation Functions
  * @{
  */
-CS_Sink CreateMjpegServer(const wpi::Twine& name,
-                          const wpi::Twine& listenAddress, int port,
-                          CS_Status* status);
-CS_Sink CreateCvSink(const wpi::Twine& name, CS_Status* status);
-CS_Sink CreateCvSinkCallback(const wpi::Twine& name,
+CS_Sink CreateMjpegServer(std::string_view name, std::string_view listenAddress,
+                          int port, CS_Status* status);
+CS_Sink CreateCvSink(std::string_view name, CS_Status* status);
+CS_Sink CreateCvSinkCallback(std::string_view name,
                              std::function<void(uint64_t time)> processFrame,
                              CS_Status* status);
 
@@ -327,19 +327,21 @@
  */
 CS_SinkKind GetSinkKind(CS_Sink sink, CS_Status* status);
 std::string GetSinkName(CS_Sink sink, CS_Status* status);
-wpi::StringRef GetSinkName(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
-                           CS_Status* status);
+std::string_view GetSinkName(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
+                             CS_Status* status);
 std::string GetSinkDescription(CS_Sink sink, CS_Status* status);
-wpi::StringRef GetSinkDescription(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
-                                  CS_Status* status);
-CS_Property GetSinkProperty(CS_Sink sink, const wpi::Twine& name,
+std::string_view GetSinkDescription(CS_Sink sink,
+                                    wpi::SmallVectorImpl<char>& buf,
+                                    CS_Status* status);
+CS_Property GetSinkProperty(CS_Sink sink, std::string_view name,
                             CS_Status* status);
-wpi::ArrayRef<CS_Property> EnumerateSinkProperties(
+wpi::span<CS_Property> EnumerateSinkProperties(
     CS_Sink sink, wpi::SmallVectorImpl<CS_Property>& vec, CS_Status* status);
 void SetSinkSource(CS_Sink sink, CS_Source source, CS_Status* status);
-CS_Property GetSinkSourceProperty(CS_Sink sink, const wpi::Twine& name,
+CS_Property GetSinkSourceProperty(CS_Sink sink, std::string_view name,
                                   CS_Status* status);
-bool SetSinkConfigJson(CS_Sink sink, wpi::StringRef config, CS_Status* status);
+bool SetSinkConfigJson(CS_Sink sink, std::string_view config,
+                       CS_Status* status);
 bool SetSinkConfigJson(CS_Sink sink, const wpi::json& config,
                        CS_Status* status);
 std::string GetSinkConfigJson(CS_Sink sink, CS_Status* status);
@@ -361,11 +363,11 @@
  * @defgroup cscore_opencv_sink_func OpenCV Sink Functions
  * @{
  */
-void SetSinkDescription(CS_Sink sink, const wpi::Twine& description,
+void SetSinkDescription(CS_Sink sink, std::string_view description,
                         CS_Status* status);
 std::string GetSinkError(CS_Sink sink, CS_Status* status);
-wpi::StringRef GetSinkError(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
-                            CS_Status* status);
+std::string_view GetSinkError(CS_Sink sink, wpi::SmallVectorImpl<char>& buf,
+                              CS_Status* status);
 void SetSinkEnabled(CS_Sink sink, bool enabled, CS_Status* status);
 /** @} */
 
@@ -380,6 +382,15 @@
                         int eventMask, bool immediateNotify, CS_Status* status);
 
 void RemoveListener(CS_Listener handle, CS_Status* status);
+
+CS_ListenerPoller CreateListenerPoller();
+void DestroyListenerPoller(CS_ListenerPoller poller);
+CS_Listener AddPolledListener(CS_ListenerPoller poller, int eventMask,
+                              bool immediateNotify, CS_Status* status);
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller);
+std::vector<RawEvent> PollListener(CS_ListenerPoller poller, double timeout,
+                                   bool* timedOut);
+void CancelPollListener(CS_ListenerPoller poller);
 /** @} */
 
 bool NotifierDestroyed();
@@ -419,10 +430,10 @@
  */
 std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status);
 
-wpi::ArrayRef<CS_Source> EnumerateSourceHandles(
+wpi::span<CS_Source> EnumerateSourceHandles(
     wpi::SmallVectorImpl<CS_Source>& vec, CS_Status* status);
-wpi::ArrayRef<CS_Sink> EnumerateSinkHandles(wpi::SmallVectorImpl<CS_Sink>& vec,
-                                            CS_Status* status);
+wpi::span<CS_Sink> EnumerateSinkHandles(wpi::SmallVectorImpl<CS_Sink>& vec,
+                                        CS_Status* status);
 
 std::string GetHostname();
 
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h
index 650399b..30a356b 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_cv.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_CV_H_
 #define CSCORE_CSCORE_CV_H_
@@ -85,7 +82,7 @@
    * @param name Source name (arbitrary unique identifier)
    * @param mode Video mode being generated
    */
-  CvSource(const wpi::Twine& name, const VideoMode& mode);
+  CvSource(std::string_view name, const VideoMode& mode);
 
   /**
    * Create an OpenCV source.
@@ -96,8 +93,8 @@
    * @param height height
    * @param fps fps
    */
-  CvSource(const wpi::Twine& name, VideoMode::PixelFormat pixelFormat,
-           int width, int height, int fps);
+  CvSource(std::string_view name, VideoMode::PixelFormat pixelFormat, int width,
+           int height, int fps);
 
   /**
    * Put an OpenCV image and notify sinks.
@@ -129,7 +126,7 @@
    *
    * @param name Source name (arbitrary unique identifier)
    */
-  explicit CvSink(const wpi::Twine& name);
+  explicit CvSink(std::string_view name);
 
   /**
    * Create a sink for accepting OpenCV images in a separate thread.
@@ -143,7 +140,7 @@
    *        or GetError() as needed, but should not call (except in very
    *        unusual circumstances) WaitForImage().
    */
-  CvSink(const wpi::Twine& name,
+  CvSink(std::string_view name,
          std::function<void(uint64_t time)> processFrame);
 
   /**
@@ -155,7 +152,8 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrame(cv::Mat& image, double timeout = 0.225) const;
+  [[nodiscard]] uint64_t GrabFrame(cv::Mat& image,
+                                   double timeout = 0.225) const;
 
   /**
    * Wait for the next frame and get the image.  May block forever.
@@ -165,14 +163,14 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrameNoTimeout(cv::Mat& image) const;
+  [[nodiscard]] uint64_t GrabFrameNoTimeout(cv::Mat& image) const;
 };
 
-inline CvSource::CvSource(const wpi::Twine& name, const VideoMode& mode) {
+inline CvSource::CvSource(std::string_view name, const VideoMode& mode) {
   m_handle = CreateCvSource(name, mode, &m_status);
 }
 
-inline CvSource::CvSource(const wpi::Twine& name, VideoMode::PixelFormat format,
+inline CvSource::CvSource(std::string_view name, VideoMode::PixelFormat format,
                           int width, int height, int fps) {
   m_handle =
       CreateCvSource(name, VideoMode{format, width, height, fps}, &m_status);
@@ -183,11 +181,11 @@
   PutSourceFrame(m_handle, image, &m_status);
 }
 
-inline CvSink::CvSink(const wpi::Twine& name) {
+inline CvSink::CvSink(std::string_view name) {
   m_handle = CreateCvSink(name, &m_status);
 }
 
-inline CvSink::CvSink(const wpi::Twine& name,
+inline CvSink::CvSink(std::string_view name,
                       std::function<void(uint64_t time)> processFrame) {
   m_handle = CreateCvSinkCallback(name, processFrame, &m_status);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h
index 9e6e290..4fd1bff 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.h
@@ -1,18 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_OO_H_
 #define CSCORE_CSCORE_OO_H_
 
 #include <initializer_list>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
+#include <wpi/span.h>
+
 #include "cscore_cpp.h"
 
 namespace cs {
@@ -51,7 +51,7 @@
     kEnum = CS_PROP_ENUM
   };
 
-  VideoProperty() : m_status(0), m_handle(0), m_kind(kNone) {}
+  VideoProperty() = default;
 
   std::string GetName() const;
 
@@ -74,8 +74,8 @@
 
   // String-specific functions
   std::string GetString() const;
-  wpi::StringRef GetString(wpi::SmallVectorImpl<char>& buf) const;
-  void SetString(const wpi::Twine& value);
+  std::string_view GetString(wpi::SmallVectorImpl<char>& buf) const;
+  void SetString(std::string_view value);
 
   // Enum-specific functions
   std::vector<std::string> GetChoices() const;
@@ -86,9 +86,9 @@
   explicit VideoProperty(CS_Property handle);
   VideoProperty(CS_Property handle, Kind kind);
 
-  mutable CS_Status m_status;
-  CS_Property m_handle;
-  Kind m_kind;
+  mutable CS_Status m_status{0};
+  CS_Property m_handle{0};
+  Kind m_kind{kNone};
 };
 
 /**
@@ -127,7 +127,7 @@
     kConnectionForceClose = CS_CONNECTION_FORCE_CLOSE
   };
 
-  VideoSource() noexcept : m_handle(0) {}
+  VideoSource() noexcept = default;
   VideoSource(const VideoSource& source);
   VideoSource(VideoSource&& other) noexcept;
   VideoSource& operator=(VideoSource other) noexcept;
@@ -197,7 +197,7 @@
    * @return Property contents (of kind Property::kNone if no property with
    *         the given name exists)
    */
-  VideoProperty GetProperty(const wpi::Twine& name);
+  VideoProperty GetProperty(std::string_view name);
 
   /**
    * Enumerate all properties of this source.
@@ -279,7 +279,7 @@
    * @param config configuration
    * @return True if set successfully
    */
-  bool SetConfigJson(wpi::StringRef config);
+  bool SetConfigJson(std::string_view config);
 
   /**
    * Set video mode and properties from a JSON configuration object.
@@ -352,7 +352,7 @@
   explicit VideoSource(CS_Source handle) : m_handle(handle) {}
 
   mutable CS_Status m_status = 0;
-  CS_Source m_handle;
+  CS_Source m_handle{0};
 };
 
 /**
@@ -427,7 +427,7 @@
    * @param name Source name (arbitrary unique identifier)
    * @param dev Device number (e.g. 0 for /dev/video0)
    */
-  UsbCamera(const wpi::Twine& name, int dev);
+  UsbCamera(std::string_view name, int dev);
 
   /**
    * Create a source for a USB camera based on device path.
@@ -435,7 +435,7 @@
    * @param name Source name (arbitrary unique identifier)
    * @param path Path to device (e.g. "/dev/video0" on Linux)
    */
-  UsbCamera(const wpi::Twine& name, const wpi::Twine& path);
+  UsbCamera(std::string_view name, std::string_view path);
 
   /**
    * Enumerate USB cameras on the local system.
@@ -447,7 +447,7 @@
   /**
    * Change the path to the device.
    */
-  void SetPath(const wpi::Twine& path);
+  void SetPath(std::string_view path);
 
   /**
    * Get the path to the device.
@@ -486,7 +486,7 @@
    * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
    * @param kind Camera kind (e.g. kAxis)
    */
-  HttpCamera(const wpi::Twine& name, const wpi::Twine& url,
+  HttpCamera(std::string_view name, std::string_view url,
              HttpCameraKind kind = kUnknown);
 
   /**
@@ -496,7 +496,7 @@
    * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
    * @param kind Camera kind (e.g. kAxis)
    */
-  HttpCamera(const wpi::Twine& name, const char* url,
+  HttpCamera(std::string_view name, const char* url,
              HttpCameraKind kind = kUnknown);
 
   /**
@@ -506,7 +506,7 @@
    * @param url Camera URL (e.g. "http://10.x.y.11/video/stream.mjpg")
    * @param kind Camera kind (e.g. kAxis)
    */
-  HttpCamera(const wpi::Twine& name, const std::string& url,
+  HttpCamera(std::string_view name, const std::string& url,
              HttpCameraKind kind = kUnknown);
 
   /**
@@ -516,7 +516,7 @@
    * @param urls Array of Camera URLs
    * @param kind Camera kind (e.g. kAxis)
    */
-  HttpCamera(const wpi::Twine& name, wpi::ArrayRef<std::string> urls,
+  HttpCamera(std::string_view name, wpi::span<const std::string> urls,
              HttpCameraKind kind = kUnknown);
 
   /**
@@ -527,7 +527,7 @@
    * @param kind Camera kind (e.g. kAxis)
    */
   template <typename T>
-  HttpCamera(const wpi::Twine& name, std::initializer_list<T> urls,
+  HttpCamera(std::string_view name, std::initializer_list<T> urls,
              HttpCameraKind kind = kUnknown);
 
   /**
@@ -541,7 +541,7 @@
   /**
    * Change the URLs used to connect to the camera.
    */
-  void SetUrls(wpi::ArrayRef<std::string> urls);
+  void SetUrls(wpi::span<const std::string> urls);
 
   /**
    * Change the URLs used to connect to the camera.
@@ -559,8 +559,8 @@
  * A source that represents an Axis IP camera.
  */
 class AxisCamera : public HttpCamera {
-  static std::string HostToUrl(const wpi::Twine& host);
-  static std::vector<std::string> HostToUrl(wpi::ArrayRef<std::string> hosts);
+  static std::string HostToUrl(std::string_view host);
+  static std::vector<std::string> HostToUrl(wpi::span<const std::string> hosts);
   template <typename T>
   static std::vector<std::string> HostToUrl(std::initializer_list<T> hosts);
 
@@ -570,55 +570,41 @@
    *
    * @param name Source name (arbitrary unique identifier)
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   * @param kind Camera kind (e.g. kAxis)
    */
-  AxisCamera(const wpi::Twine& name, const wpi::Twine& host);
+  AxisCamera(std::string_view name, std::string_view host);
 
   /**
    * Create a source for an Axis IP camera.
    *
    * @param name Source name (arbitrary unique identifier)
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   * @param kind Camera kind (e.g. kAxis)
    */
-  AxisCamera(const wpi::Twine& name, const char* host);
+  AxisCamera(std::string_view name, const char* host);
 
   /**
    * Create a source for an Axis IP camera.
    *
    * @param name Source name (arbitrary unique identifier)
    * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   * @param kind Camera kind (e.g. kAxis)
    */
-  AxisCamera(const wpi::Twine& name, const std::string& host);
-
-  /**
-   * Create a source for an Axis IP camera.
-   *
-   * @param name Source name (arbitrary unique identifier)
-   * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   * @param kind Camera kind (e.g. kAxis)
-   */
-  AxisCamera(const wpi::Twine& name, wpi::StringRef host);
+  AxisCamera(std::string_view name, const std::string& host);
 
   /**
    * Create a source for an Axis IP camera.
    *
    * @param name Source name (arbitrary unique identifier)
    * @param hosts Array of Camera host IPs/DNS names
-   * @param kind Camera kind (e.g. kAxis)
    */
-  AxisCamera(const wpi::Twine& name, wpi::ArrayRef<std::string> hosts);
+  AxisCamera(std::string_view name, wpi::span<const std::string> hosts);
 
   /**
    * Create a source for an Axis IP camera.
    *
    * @param name Source name (arbitrary unique identifier)
    * @param hosts Array of Camera host IPs/DNS names
-   * @param kind Camera kind (e.g. kAxis)
    */
   template <typename T>
-  AxisCamera(const wpi::Twine& name, std::initializer_list<T> hosts);
+  AxisCamera(std::string_view name, std::initializer_list<T> hosts);
 };
 
 /**
@@ -632,8 +618,10 @@
   /**
    * Signal sinks that an error has occurred.  This should be called instead
    * of NotifyFrame when an error occurs.
+   *
+   * @param msg Notification message.
    */
-  void NotifyError(const wpi::Twine& msg);
+  void NotifyError(std::string_view msg);
 
   /**
    * Set source connection status.  Defaults to true.
@@ -647,7 +635,7 @@
    *
    * @param description Description
    */
-  void SetDescription(const wpi::Twine& description);
+  void SetDescription(std::string_view description);
 
   /**
    * Create a property.
@@ -661,7 +649,7 @@
    * @param value Current value
    * @return Property
    */
-  VideoProperty CreateProperty(const wpi::Twine& name, VideoProperty::Kind kind,
+  VideoProperty CreateProperty(std::string_view name, VideoProperty::Kind kind,
                                int minimum, int maximum, int step,
                                int defaultValue, int value);
 
@@ -676,7 +664,7 @@
    * @param value Current value
    * @return Property
    */
-  VideoProperty CreateIntegerProperty(const wpi::Twine& name, int minimum,
+  VideoProperty CreateIntegerProperty(std::string_view name, int minimum,
                                       int maximum, int step, int defaultValue,
                                       int value);
 
@@ -688,19 +676,18 @@
    * @param value Current value
    * @return Property
    */
-  VideoProperty CreateBooleanProperty(const wpi::Twine& name, bool defaultValue,
+  VideoProperty CreateBooleanProperty(std::string_view name, bool defaultValue,
                                       bool value);
 
   /**
    * Create a string property.
    *
    * @param name Property name
-   * @param defaultValue Default value
    * @param value Current value
    * @return Property
    */
-  VideoProperty CreateStringProperty(const wpi::Twine& name,
-                                     const wpi::Twine& value);
+  VideoProperty CreateStringProperty(std::string_view name,
+                                     std::string_view value);
 
   /**
    * Configure enum property choices.
@@ -709,7 +696,7 @@
    * @param choices Choices
    */
   void SetEnumPropertyChoices(const VideoProperty& property,
-                              wpi::ArrayRef<std::string> choices);
+                              wpi::span<const std::string> choices);
 
   /**
    * Configure enum property choices.
@@ -736,7 +723,7 @@
     kCv = CS_SINK_CV
   };
 
-  VideoSink() noexcept : m_handle(0) {}
+  VideoSink() noexcept = default;
   VideoSink(const VideoSink& sink);
   VideoSink(VideoSink&& sink) noexcept;
   VideoSink& operator=(VideoSink other) noexcept;
@@ -775,7 +762,7 @@
    * @return Property (kind Property::kNone if no property with
    *         the given name exists)
    */
-  VideoProperty GetProperty(const wpi::Twine& name);
+  VideoProperty GetProperty(std::string_view name);
 
   /**
    * Enumerate all properties of this sink.
@@ -801,7 +788,7 @@
    * @param config configuration
    * @return True if set successfully
    */
-  bool SetConfigJson(wpi::StringRef config);
+  bool SetConfigJson(std::string_view config);
 
   /**
    * Set properties from a JSON configuration object.
@@ -848,7 +835,7 @@
    * @return Property (kind Property::kNone if no property with
    *         the given name exists or no source connected)
    */
-  VideoProperty GetSourceProperty(const wpi::Twine& name);
+  VideoProperty GetSourceProperty(std::string_view name);
 
   CS_Status GetLastStatus() const { return m_status; }
 
@@ -869,7 +856,7 @@
   explicit VideoSink(CS_Sink handle) : m_handle(handle) {}
 
   mutable CS_Status m_status = 0;
-  CS_Sink m_handle;
+  CS_Sink m_handle{0};
 };
 
 /**
@@ -886,8 +873,7 @@
    * @param listenAddress TCP listen address (empty string for all addresses)
    * @param port TCP port number
    */
-  MjpegServer(const wpi::Twine& name, const wpi::Twine& listenAddress,
-              int port);
+  MjpegServer(std::string_view name, std::string_view listenAddress, int port);
 
   /**
    * Create a MJPEG-over-HTTP server sink.
@@ -895,7 +881,7 @@
    * @param name Sink name (arbitrary unique identifier)
    * @param port TCP port number
    */
-  MjpegServer(const wpi::Twine& name, int port) : MjpegServer(name, "", port) {}
+  MjpegServer(std::string_view name, int port) : MjpegServer(name, "", port) {}
 
   /**
    * Get the listen address of the server.
@@ -966,7 +952,7 @@
    *
    * @param description Description
    */
-  void SetDescription(const wpi::Twine& description);
+  void SetDescription(std::string_view description);
 
   /**
    * Get error string.  Call this if WaitForFrame() returns 0 to determine
@@ -1011,7 +997,7 @@
  */
 class VideoListener {
  public:
-  VideoListener() : m_handle(0) {}
+  VideoListener() = default;
 
   /**
    * Create an event listener.
@@ -1036,13 +1022,13 @@
   }
 
  private:
-  CS_Listener m_handle;
+  CS_Listener m_handle{0};
 };
 
 /** @} */
 
 }  // namespace cs
 
-#include "cscore_oo.inl"
+#include "cscore_oo.inc"
 
 #endif  // CSCORE_CSCORE_OO_H_
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.inc b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.inc
new file mode 100644
index 0000000..5037d97
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.inc
@@ -0,0 +1,645 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef CSCORE_CSCORE_OO_INC_
+#define CSCORE_CSCORE_OO_INC_
+
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "cscore_oo.h"
+
+namespace cs {
+
+inline std::string VideoProperty::GetName() const {
+  m_status = 0;
+  return GetPropertyName(m_handle, &m_status);
+}
+
+inline int VideoProperty::Get() const {
+  m_status = 0;
+  return GetProperty(m_handle, &m_status);
+}
+
+inline void VideoProperty::Set(int value) {
+  m_status = 0;
+  SetProperty(m_handle, value, &m_status);
+}
+
+inline int VideoProperty::GetMin() const {
+  m_status = 0;
+  return GetPropertyMin(m_handle, &m_status);
+}
+
+inline int VideoProperty::GetMax() const {
+  m_status = 0;
+  return GetPropertyMax(m_handle, &m_status);
+}
+
+inline int VideoProperty::GetStep() const {
+  m_status = 0;
+  return GetPropertyStep(m_handle, &m_status);
+}
+
+inline int VideoProperty::GetDefault() const {
+  m_status = 0;
+  return GetPropertyDefault(m_handle, &m_status);
+}
+
+inline std::string VideoProperty::GetString() const {
+  m_status = 0;
+  return GetStringProperty(m_handle, &m_status);
+}
+
+inline std::string_view VideoProperty::GetString(
+    wpi::SmallVectorImpl<char>& buf) const {
+  m_status = 0;
+  return GetStringProperty(m_handle, buf, &m_status);
+}
+
+inline void VideoProperty::SetString(std::string_view value) {
+  m_status = 0;
+  SetStringProperty(m_handle, value, &m_status);
+}
+
+inline std::vector<std::string> VideoProperty::GetChoices() const {
+  m_status = 0;
+  return GetEnumPropertyChoices(m_handle, &m_status);
+}
+
+inline VideoProperty::VideoProperty(CS_Property handle) : m_handle(handle) {
+  m_status = 0;
+  if (handle == 0) {
+    m_kind = kNone;
+  } else {
+    m_kind =
+        static_cast<Kind>(static_cast<int>(GetPropertyKind(handle, &m_status)));
+  }
+}
+
+inline VideoProperty::VideoProperty(CS_Property handle, Kind kind)
+    : m_handle(handle), m_kind(kind) {}
+
+inline VideoSource::VideoSource(const VideoSource& source)
+    : m_handle(source.m_handle == 0 ? 0
+                                    : CopySource(source.m_handle, &m_status)) {}
+
+inline VideoSource::VideoSource(VideoSource&& other) noexcept : VideoSource() {
+  swap(*this, other);
+}
+
+inline VideoSource& VideoSource::operator=(VideoSource other) noexcept {
+  swap(*this, other);
+  return *this;
+}
+
+inline VideoSource::~VideoSource() {
+  m_status = 0;
+  if (m_handle != 0) {
+    ReleaseSource(m_handle, &m_status);
+  }
+}
+
+inline VideoSource::Kind VideoSource::GetKind() const {
+  m_status = 0;
+  return static_cast<VideoSource::Kind>(GetSourceKind(m_handle, &m_status));
+}
+
+inline std::string VideoSource::GetName() const {
+  m_status = 0;
+  return GetSourceName(m_handle, &m_status);
+}
+
+inline std::string VideoSource::GetDescription() const {
+  m_status = 0;
+  return GetSourceDescription(m_handle, &m_status);
+}
+
+inline uint64_t VideoSource::GetLastFrameTime() const {
+  m_status = 0;
+  return GetSourceLastFrameTime(m_handle, &m_status);
+}
+
+inline void VideoSource::SetConnectionStrategy(ConnectionStrategy strategy) {
+  m_status = 0;
+  SetSourceConnectionStrategy(
+      m_handle, static_cast<CS_ConnectionStrategy>(static_cast<int>(strategy)),
+      &m_status);
+}
+
+inline bool VideoSource::IsConnected() const {
+  m_status = 0;
+  return IsSourceConnected(m_handle, &m_status);
+}
+
+inline bool VideoSource::IsEnabled() const {
+  m_status = 0;
+  return IsSourceEnabled(m_handle, &m_status);
+}
+
+inline VideoProperty VideoSource::GetProperty(std::string_view name) {
+  m_status = 0;
+  return VideoProperty{GetSourceProperty(m_handle, name, &m_status)};
+}
+
+inline VideoMode VideoSource::GetVideoMode() const {
+  m_status = 0;
+  return GetSourceVideoMode(m_handle, &m_status);
+}
+
+inline bool VideoSource::SetVideoMode(const VideoMode& mode) {
+  m_status = 0;
+  return SetSourceVideoMode(m_handle, mode, &m_status);
+}
+
+inline bool VideoSource::SetVideoMode(VideoMode::PixelFormat pixelFormat,
+                                      int width, int height, int fps) {
+  m_status = 0;
+  return SetSourceVideoMode(
+      m_handle, VideoMode{pixelFormat, width, height, fps}, &m_status);
+}
+
+inline bool VideoSource::SetPixelFormat(VideoMode::PixelFormat pixelFormat) {
+  m_status = 0;
+  return SetSourcePixelFormat(m_handle, pixelFormat, &m_status);
+}
+
+inline bool VideoSource::SetResolution(int width, int height) {
+  m_status = 0;
+  return SetSourceResolution(m_handle, width, height, &m_status);
+}
+
+inline bool VideoSource::SetFPS(int fps) {
+  m_status = 0;
+  return SetSourceFPS(m_handle, fps, &m_status);
+}
+
+inline bool VideoSource::SetConfigJson(std::string_view config) {
+  m_status = 0;
+  return SetSourceConfigJson(m_handle, config, &m_status);
+}
+
+inline bool VideoSource::SetConfigJson(const wpi::json& config) {
+  m_status = 0;
+  return SetSourceConfigJson(m_handle, config, &m_status);
+}
+
+inline std::string VideoSource::GetConfigJson() const {
+  m_status = 0;
+  return GetSourceConfigJson(m_handle, &m_status);
+}
+
+inline double VideoSource::GetActualFPS() const {
+  m_status = 0;
+  return cs::GetTelemetryAverageValue(m_handle, CS_SOURCE_FRAMES_RECEIVED,
+                                      &m_status);
+}
+
+inline double VideoSource::GetActualDataRate() const {
+  m_status = 0;
+  return cs::GetTelemetryAverageValue(m_handle, CS_SOURCE_BYTES_RECEIVED,
+                                      &m_status);
+}
+
+inline std::vector<VideoMode> VideoSource::EnumerateVideoModes() const {
+  CS_Status status = 0;
+  return EnumerateSourceVideoModes(m_handle, &status);
+}
+
+inline void VideoCamera::SetBrightness(int brightness) {
+  m_status = 0;
+  SetCameraBrightness(m_handle, brightness, &m_status);
+}
+
+inline int VideoCamera::GetBrightness() {
+  m_status = 0;
+  return GetCameraBrightness(m_handle, &m_status);
+}
+
+inline void VideoCamera::SetWhiteBalanceAuto() {
+  m_status = 0;
+  SetCameraWhiteBalanceAuto(m_handle, &m_status);
+}
+
+inline void VideoCamera::SetWhiteBalanceHoldCurrent() {
+  m_status = 0;
+  SetCameraWhiteBalanceHoldCurrent(m_handle, &m_status);
+}
+
+inline void VideoCamera::SetWhiteBalanceManual(int value) {
+  m_status = 0;
+  SetCameraWhiteBalanceManual(m_handle, value, &m_status);
+}
+
+inline void VideoCamera::SetExposureAuto() {
+  m_status = 0;
+  SetCameraExposureAuto(m_handle, &m_status);
+}
+
+inline void VideoCamera::SetExposureHoldCurrent() {
+  m_status = 0;
+  SetCameraExposureHoldCurrent(m_handle, &m_status);
+}
+
+inline void VideoCamera::SetExposureManual(int value) {
+  m_status = 0;
+  SetCameraExposureManual(m_handle, value, &m_status);
+}
+
+inline UsbCamera::UsbCamera(std::string_view name, int dev) {
+  m_handle = CreateUsbCameraDev(name, dev, &m_status);
+}
+
+inline UsbCamera::UsbCamera(std::string_view name, std::string_view path) {
+  m_handle = CreateUsbCameraPath(name, path, &m_status);
+}
+
+inline std::vector<UsbCameraInfo> UsbCamera::EnumerateUsbCameras() {
+  CS_Status status = 0;
+  return ::cs::EnumerateUsbCameras(&status);
+}
+
+inline void UsbCamera::SetPath(std::string_view path) {
+  m_status = 0;
+  return ::cs::SetUsbCameraPath(m_handle, path, &m_status);
+}
+
+inline std::string UsbCamera::GetPath() const {
+  m_status = 0;
+  return ::cs::GetUsbCameraPath(m_handle, &m_status);
+}
+
+inline UsbCameraInfo UsbCamera::GetInfo() const {
+  m_status = 0;
+  return ::cs::GetUsbCameraInfo(m_handle, &m_status);
+}
+
+inline void UsbCamera::SetConnectVerbose(int level) {
+  m_status = 0;
+  SetProperty(GetSourceProperty(m_handle, "connect_verbose", &m_status), level,
+              &m_status);
+}
+
+inline HttpCamera::HttpCamera(std::string_view name, std::string_view url,
+                              HttpCameraKind kind) {
+  m_handle = CreateHttpCamera(
+      name, url, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
+      &m_status);
+}
+
+inline HttpCamera::HttpCamera(std::string_view name, const char* url,
+                              HttpCameraKind kind) {
+  m_handle = CreateHttpCamera(
+      name, url, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
+      &m_status);
+}
+
+inline HttpCamera::HttpCamera(std::string_view name, const std::string& url,
+                              HttpCameraKind kind)
+    : HttpCamera(name, std::string_view{url}, kind) {}
+
+inline HttpCamera::HttpCamera(std::string_view name,
+                              wpi::span<const std::string> urls,
+                              HttpCameraKind kind) {
+  m_handle = CreateHttpCamera(
+      name, urls, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
+      &m_status);
+}
+
+template <typename T>
+inline HttpCamera::HttpCamera(std::string_view name,
+                              std::initializer_list<T> urls,
+                              HttpCameraKind kind) {
+  std::vector<std::string> vec;
+  vec.reserve(urls.size());
+  for (const auto& url : urls) {
+    vec.emplace_back(url);
+  }
+  m_handle = CreateHttpCamera(
+      name, vec, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
+      &m_status);
+}
+
+inline HttpCamera::HttpCameraKind HttpCamera::GetHttpCameraKind() const {
+  m_status = 0;
+  return static_cast<HttpCameraKind>(
+      static_cast<int>(::cs::GetHttpCameraKind(m_handle, &m_status)));
+}
+
+inline void HttpCamera::SetUrls(wpi::span<const std::string> urls) {
+  m_status = 0;
+  ::cs::SetHttpCameraUrls(m_handle, urls, &m_status);
+}
+
+template <typename T>
+inline void HttpCamera::SetUrls(std::initializer_list<T> urls) {
+  std::vector<std::string> vec;
+  vec.reserve(urls.size());
+  for (const auto& url : urls) {
+    vec.emplace_back(url);
+  }
+  m_status = 0;
+  ::cs::SetHttpCameraUrls(m_handle, vec, &m_status);
+}
+
+inline std::vector<std::string> HttpCamera::GetUrls() const {
+  m_status = 0;
+  return ::cs::GetHttpCameraUrls(m_handle, &m_status);
+}
+
+inline std::vector<std::string> AxisCamera::HostToUrl(
+    wpi::span<const std::string> hosts) {
+  std::vector<std::string> rv;
+  rv.reserve(hosts.size());
+  for (const auto& host : hosts) {
+    rv.emplace_back(HostToUrl(std::string_view{host}));
+  }
+  return rv;
+}
+
+template <typename T>
+inline std::vector<std::string> AxisCamera::HostToUrl(
+    std::initializer_list<T> hosts) {
+  std::vector<std::string> rv;
+  rv.reserve(hosts.size());
+  for (const auto& host : hosts) {
+    rv.emplace_back(HostToUrl(std::string_view{host}));
+  }
+  return rv;
+}
+
+inline AxisCamera::AxisCamera(std::string_view name, std::string_view host)
+    : HttpCamera(name, HostToUrl(host), kAxis) {}
+
+inline AxisCamera::AxisCamera(std::string_view name, const char* host)
+    : HttpCamera(name, HostToUrl(host), kAxis) {}
+
+inline AxisCamera::AxisCamera(std::string_view name, const std::string& host)
+    : HttpCamera(name, HostToUrl(std::string_view{host}), kAxis) {}
+
+inline AxisCamera::AxisCamera(std::string_view name,
+                              wpi::span<const std::string> hosts)
+    : HttpCamera(name, HostToUrl(hosts), kAxis) {}
+
+template <typename T>
+inline AxisCamera::AxisCamera(std::string_view name,
+                              std::initializer_list<T> hosts)
+    : HttpCamera(name, HostToUrl(hosts), kAxis) {}
+
+inline void ImageSource::NotifyError(std::string_view msg) {
+  m_status = 0;
+  NotifySourceError(m_handle, msg, &m_status);
+}
+
+inline void ImageSource::SetConnected(bool connected) {
+  m_status = 0;
+  SetSourceConnected(m_handle, connected, &m_status);
+}
+
+inline void ImageSource::SetDescription(std::string_view description) {
+  m_status = 0;
+  SetSourceDescription(m_handle, description, &m_status);
+}
+
+inline VideoProperty ImageSource::CreateProperty(std::string_view name,
+                                                 VideoProperty::Kind kind,
+                                                 int minimum, int maximum,
+                                                 int step, int defaultValue,
+                                                 int value) {
+  m_status = 0;
+  return VideoProperty{CreateSourceProperty(
+      m_handle, name, static_cast<CS_PropertyKind>(static_cast<int>(kind)),
+      minimum, maximum, step, defaultValue, value, &m_status)};
+}
+
+inline VideoProperty ImageSource::CreateIntegerProperty(std::string_view name,
+                                                        int minimum,
+                                                        int maximum, int step,
+                                                        int defaultValue,
+                                                        int value) {
+  m_status = 0;
+  return VideoProperty{CreateSourceProperty(
+      m_handle, name,
+      static_cast<CS_PropertyKind>(
+          static_cast<int>(VideoProperty::Kind::kInteger)),
+      minimum, maximum, step, defaultValue, value, &m_status)};
+}
+
+inline VideoProperty ImageSource::CreateBooleanProperty(std::string_view name,
+                                                        bool defaultValue,
+                                                        bool value) {
+  m_status = 0;
+  return VideoProperty{CreateSourceProperty(
+      m_handle, name,
+      static_cast<CS_PropertyKind>(
+          static_cast<int>(VideoProperty::Kind::kBoolean)),
+      0, 1, 1, defaultValue ? 1 : 0, value ? 1 : 0, &m_status)};
+}
+
+inline VideoProperty ImageSource::CreateStringProperty(std::string_view name,
+                                                       std::string_view value) {
+  m_status = 0;
+  auto prop = VideoProperty{
+      CreateSourceProperty(m_handle, name,
+                           static_cast<CS_PropertyKind>(
+                               static_cast<int>(VideoProperty::Kind::kString)),
+                           0, 0, 0, 0, 0, &m_status)};
+  prop.SetString(value);
+  return prop;
+}
+
+inline void ImageSource::SetEnumPropertyChoices(
+    const VideoProperty& property, wpi::span<const std::string> choices) {
+  m_status = 0;
+  SetSourceEnumPropertyChoices(m_handle, property.m_handle, choices, &m_status);
+}
+
+template <typename T>
+inline void ImageSource::SetEnumPropertyChoices(
+    const VideoProperty& property, std::initializer_list<T> choices) {
+  std::vector<std::string> vec;
+  vec.reserve(choices.size());
+  for (const auto& choice : choices) {
+    vec.emplace_back(choice);
+  }
+  m_status = 0;
+  SetSourceEnumPropertyChoices(m_handle, property.m_handle, vec, &m_status);
+}
+
+inline VideoSink::VideoSink(const VideoSink& sink)
+    : m_handle(sink.m_handle == 0 ? 0 : CopySink(sink.m_handle, &m_status)) {}
+
+inline VideoSink::VideoSink(VideoSink&& other) noexcept : VideoSink() {
+  swap(*this, other);
+}
+
+inline VideoSink& VideoSink::operator=(VideoSink other) noexcept {
+  swap(*this, other);
+  return *this;
+}
+
+inline VideoSink::~VideoSink() {
+  m_status = 0;
+  if (m_handle != 0) {
+    ReleaseSink(m_handle, &m_status);
+  }
+}
+
+inline VideoSink::Kind VideoSink::GetKind() const {
+  m_status = 0;
+  return static_cast<VideoSink::Kind>(GetSinkKind(m_handle, &m_status));
+}
+
+inline std::string VideoSink::GetName() const {
+  m_status = 0;
+  return GetSinkName(m_handle, &m_status);
+}
+
+inline std::string VideoSink::GetDescription() const {
+  m_status = 0;
+  return GetSinkDescription(m_handle, &m_status);
+}
+
+inline VideoProperty VideoSink::GetProperty(std::string_view name) {
+  m_status = 0;
+  return VideoProperty{GetSinkProperty(m_handle, name, &m_status)};
+}
+
+inline void VideoSink::SetSource(VideoSource source) {
+  m_status = 0;
+  if (!source) {
+    SetSinkSource(m_handle, 0, &m_status);
+  } else {
+    SetSinkSource(m_handle, source.m_handle, &m_status);
+  }
+}
+
+inline VideoSource VideoSink::GetSource() const {
+  m_status = 0;
+  auto handle = GetSinkSource(m_handle, &m_status);
+  return VideoSource{handle == 0 ? 0 : CopySource(handle, &m_status)};
+}
+
+inline VideoProperty VideoSink::GetSourceProperty(std::string_view name) {
+  m_status = 0;
+  return VideoProperty{GetSinkSourceProperty(m_handle, name, &m_status)};
+}
+
+inline bool VideoSink::SetConfigJson(std::string_view config) {
+  m_status = 0;
+  return SetSinkConfigJson(m_handle, config, &m_status);
+}
+
+inline bool VideoSink::SetConfigJson(const wpi::json& config) {
+  m_status = 0;
+  return SetSinkConfigJson(m_handle, config, &m_status);
+}
+
+inline std::string VideoSink::GetConfigJson() const {
+  m_status = 0;
+  return GetSinkConfigJson(m_handle, &m_status);
+}
+
+inline MjpegServer::MjpegServer(std::string_view name,
+                                std::string_view listenAddress, int port) {
+  m_handle = CreateMjpegServer(name, listenAddress, port, &m_status);
+}
+
+inline std::string MjpegServer::GetListenAddress() const {
+  m_status = 0;
+  return cs::GetMjpegServerListenAddress(m_handle, &m_status);
+}
+
+inline int MjpegServer::GetPort() const {
+  m_status = 0;
+  return cs::GetMjpegServerPort(m_handle, &m_status);
+}
+
+inline void MjpegServer::SetResolution(int width, int height) {
+  m_status = 0;
+  SetProperty(GetSinkProperty(m_handle, "width", &m_status), width, &m_status);
+  SetProperty(GetSinkProperty(m_handle, "height", &m_status), height,
+              &m_status);
+}
+
+inline void MjpegServer::SetFPS(int fps) {
+  m_status = 0;
+  SetProperty(GetSinkProperty(m_handle, "fps", &m_status), fps, &m_status);
+}
+
+inline void MjpegServer::SetCompression(int quality) {
+  m_status = 0;
+  SetProperty(GetSinkProperty(m_handle, "compression", &m_status), quality,
+              &m_status);
+}
+
+inline void MjpegServer::SetDefaultCompression(int quality) {
+  m_status = 0;
+  SetProperty(GetSinkProperty(m_handle, "default_compression", &m_status),
+              quality, &m_status);
+}
+
+inline void ImageSink::SetDescription(std::string_view description) {
+  m_status = 0;
+  SetSinkDescription(m_handle, description, &m_status);
+}
+
+inline std::string ImageSink::GetError() const {
+  m_status = 0;
+  return GetSinkError(m_handle, &m_status);
+}
+
+inline void ImageSink::SetEnabled(bool enabled) {
+  m_status = 0;
+  SetSinkEnabled(m_handle, enabled, &m_status);
+}
+
+inline VideoSource VideoEvent::GetSource() const {
+  CS_Status status = 0;
+  return VideoSource{sourceHandle == 0 ? 0 : CopySource(sourceHandle, &status)};
+}
+
+inline VideoSink VideoEvent::GetSink() const {
+  CS_Status status = 0;
+  return VideoSink{sinkHandle == 0 ? 0 : CopySink(sinkHandle, &status)};
+}
+
+inline VideoProperty VideoEvent::GetProperty() const {
+  return VideoProperty{propertyHandle,
+                       static_cast<VideoProperty::Kind>(propertyKind)};
+}
+
+inline VideoListener::VideoListener(
+    std::function<void(const VideoEvent& event)> callback, int eventMask,
+    bool immediateNotify) {
+  CS_Status status = 0;
+  m_handle = AddListener(
+      [=](const RawEvent& event) {
+        callback(static_cast<const VideoEvent&>(event));
+      },
+      eventMask, immediateNotify, &status);
+}
+
+inline VideoListener::VideoListener(VideoListener&& other) noexcept
+    : VideoListener() {
+  swap(*this, other);
+}
+
+inline VideoListener& VideoListener::operator=(VideoListener&& other) noexcept {
+  swap(*this, other);
+  return *this;
+}
+
+inline VideoListener::~VideoListener() {
+  CS_Status status = 0;
+  if (m_handle != 0) {
+    RemoveListener(m_handle, &status);
+  }
+}
+
+}  // namespace cs
+
+#endif  // CSCORE_CSCORE_OO_INC_
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.inl b/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.inl
deleted file mode 100644
index 2d56e1c..0000000
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_oo.inl
+++ /dev/null
@@ -1,636 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef CSCORE_CSCORE_OO_INL_
-#define CSCORE_CSCORE_OO_INL_
-
-#include <string>
-#include <utility>
-#include <vector>
-
-namespace cs {
-
-inline std::string VideoProperty::GetName() const {
-  m_status = 0;
-  return GetPropertyName(m_handle, &m_status);
-}
-
-inline int VideoProperty::Get() const {
-  m_status = 0;
-  return GetProperty(m_handle, &m_status);
-}
-
-inline void VideoProperty::Set(int value) {
-  m_status = 0;
-  SetProperty(m_handle, value, &m_status);
-}
-
-inline int VideoProperty::GetMin() const {
-  m_status = 0;
-  return GetPropertyMin(m_handle, &m_status);
-}
-
-inline int VideoProperty::GetMax() const {
-  m_status = 0;
-  return GetPropertyMax(m_handle, &m_status);
-}
-
-inline int VideoProperty::GetStep() const {
-  m_status = 0;
-  return GetPropertyStep(m_handle, &m_status);
-}
-
-inline int VideoProperty::GetDefault() const {
-  m_status = 0;
-  return GetPropertyDefault(m_handle, &m_status);
-}
-
-inline std::string VideoProperty::GetString() const {
-  m_status = 0;
-  return GetStringProperty(m_handle, &m_status);
-}
-
-inline wpi::StringRef VideoProperty::GetString(
-    wpi::SmallVectorImpl<char>& buf) const {
-  m_status = 0;
-  return GetStringProperty(m_handle, buf, &m_status);
-}
-
-inline void VideoProperty::SetString(const wpi::Twine& value) {
-  m_status = 0;
-  SetStringProperty(m_handle, value, &m_status);
-}
-
-inline std::vector<std::string> VideoProperty::GetChoices() const {
-  m_status = 0;
-  return GetEnumPropertyChoices(m_handle, &m_status);
-}
-
-inline VideoProperty::VideoProperty(CS_Property handle) : m_handle(handle) {
-  m_status = 0;
-  if (handle == 0)
-    m_kind = kNone;
-  else
-    m_kind =
-        static_cast<Kind>(static_cast<int>(GetPropertyKind(handle, &m_status)));
-}
-
-inline VideoProperty::VideoProperty(CS_Property handle, Kind kind)
-    : m_status(0), m_handle(handle), m_kind(kind) {}
-
-inline VideoSource::VideoSource(const VideoSource& source)
-    : m_handle(source.m_handle == 0 ? 0
-                                    : CopySource(source.m_handle, &m_status)) {}
-
-inline VideoSource::VideoSource(VideoSource&& other) noexcept : VideoSource() {
-  swap(*this, other);
-}
-
-inline VideoSource& VideoSource::operator=(VideoSource other) noexcept {
-  swap(*this, other);
-  return *this;
-}
-
-inline VideoSource::~VideoSource() {
-  m_status = 0;
-  if (m_handle != 0) ReleaseSource(m_handle, &m_status);
-}
-
-inline VideoSource::Kind VideoSource::GetKind() const {
-  m_status = 0;
-  return static_cast<VideoSource::Kind>(GetSourceKind(m_handle, &m_status));
-}
-
-inline std::string VideoSource::GetName() const {
-  m_status = 0;
-  return GetSourceName(m_handle, &m_status);
-}
-
-inline std::string VideoSource::GetDescription() const {
-  m_status = 0;
-  return GetSourceDescription(m_handle, &m_status);
-}
-
-inline uint64_t VideoSource::GetLastFrameTime() const {
-  m_status = 0;
-  return GetSourceLastFrameTime(m_handle, &m_status);
-}
-
-inline void VideoSource::SetConnectionStrategy(ConnectionStrategy strategy) {
-  m_status = 0;
-  SetSourceConnectionStrategy(
-      m_handle, static_cast<CS_ConnectionStrategy>(static_cast<int>(strategy)),
-      &m_status);
-}
-
-inline bool VideoSource::IsConnected() const {
-  m_status = 0;
-  return IsSourceConnected(m_handle, &m_status);
-}
-
-inline bool VideoSource::IsEnabled() const {
-  m_status = 0;
-  return IsSourceEnabled(m_handle, &m_status);
-}
-
-inline VideoProperty VideoSource::GetProperty(const wpi::Twine& name) {
-  m_status = 0;
-  return VideoProperty{GetSourceProperty(m_handle, name, &m_status)};
-}
-
-inline VideoMode VideoSource::GetVideoMode() const {
-  m_status = 0;
-  return GetSourceVideoMode(m_handle, &m_status);
-}
-
-inline bool VideoSource::SetVideoMode(const VideoMode& mode) {
-  m_status = 0;
-  return SetSourceVideoMode(m_handle, mode, &m_status);
-}
-
-inline bool VideoSource::SetVideoMode(VideoMode::PixelFormat pixelFormat,
-                                      int width, int height, int fps) {
-  m_status = 0;
-  return SetSourceVideoMode(
-      m_handle, VideoMode{pixelFormat, width, height, fps}, &m_status);
-}
-
-inline bool VideoSource::SetPixelFormat(VideoMode::PixelFormat pixelFormat) {
-  m_status = 0;
-  return SetSourcePixelFormat(m_handle, pixelFormat, &m_status);
-}
-
-inline bool VideoSource::SetResolution(int width, int height) {
-  m_status = 0;
-  return SetSourceResolution(m_handle, width, height, &m_status);
-}
-
-inline bool VideoSource::SetFPS(int fps) {
-  m_status = 0;
-  return SetSourceFPS(m_handle, fps, &m_status);
-}
-
-inline bool VideoSource::SetConfigJson(wpi::StringRef config) {
-  m_status = 0;
-  return SetSourceConfigJson(m_handle, config, &m_status);
-}
-
-inline bool VideoSource::SetConfigJson(const wpi::json& config) {
-  m_status = 0;
-  return SetSourceConfigJson(m_handle, config, &m_status);
-}
-
-inline std::string VideoSource::GetConfigJson() const {
-  m_status = 0;
-  return GetSourceConfigJson(m_handle, &m_status);
-}
-
-inline double VideoSource::GetActualFPS() const {
-  m_status = 0;
-  return cs::GetTelemetryAverageValue(m_handle, CS_SOURCE_FRAMES_RECEIVED,
-                                      &m_status);
-}
-
-inline double VideoSource::GetActualDataRate() const {
-  m_status = 0;
-  return cs::GetTelemetryAverageValue(m_handle, CS_SOURCE_BYTES_RECEIVED,
-                                      &m_status);
-}
-
-inline std::vector<VideoMode> VideoSource::EnumerateVideoModes() const {
-  CS_Status status = 0;
-  return EnumerateSourceVideoModes(m_handle, &status);
-}
-
-inline void VideoCamera::SetBrightness(int brightness) {
-  m_status = 0;
-  SetCameraBrightness(m_handle, brightness, &m_status);
-}
-
-inline int VideoCamera::GetBrightness() {
-  m_status = 0;
-  return GetCameraBrightness(m_handle, &m_status);
-}
-
-inline void VideoCamera::SetWhiteBalanceAuto() {
-  m_status = 0;
-  SetCameraWhiteBalanceAuto(m_handle, &m_status);
-}
-
-inline void VideoCamera::SetWhiteBalanceHoldCurrent() {
-  m_status = 0;
-  SetCameraWhiteBalanceHoldCurrent(m_handle, &m_status);
-}
-
-inline void VideoCamera::SetWhiteBalanceManual(int value) {
-  m_status = 0;
-  SetCameraWhiteBalanceManual(m_handle, value, &m_status);
-}
-
-inline void VideoCamera::SetExposureAuto() {
-  m_status = 0;
-  SetCameraExposureAuto(m_handle, &m_status);
-}
-
-inline void VideoCamera::SetExposureHoldCurrent() {
-  m_status = 0;
-  SetCameraExposureHoldCurrent(m_handle, &m_status);
-}
-
-inline void VideoCamera::SetExposureManual(int value) {
-  m_status = 0;
-  SetCameraExposureManual(m_handle, value, &m_status);
-}
-
-inline UsbCamera::UsbCamera(const wpi::Twine& name, int dev) {
-  m_handle = CreateUsbCameraDev(name, dev, &m_status);
-}
-
-inline UsbCamera::UsbCamera(const wpi::Twine& name, const wpi::Twine& path) {
-  m_handle = CreateUsbCameraPath(name, path, &m_status);
-}
-
-inline std::vector<UsbCameraInfo> UsbCamera::EnumerateUsbCameras() {
-  CS_Status status = 0;
-  return ::cs::EnumerateUsbCameras(&status);
-}
-
-inline void UsbCamera::SetPath(const wpi::Twine& path) {
-  m_status = 0;
-  return ::cs::SetUsbCameraPath(m_handle, path, &m_status);
-}
-
-inline std::string UsbCamera::GetPath() const {
-  m_status = 0;
-  return ::cs::GetUsbCameraPath(m_handle, &m_status);
-}
-
-inline UsbCameraInfo UsbCamera::GetInfo() const {
-  m_status = 0;
-  return ::cs::GetUsbCameraInfo(m_handle, &m_status);
-}
-
-inline void UsbCamera::SetConnectVerbose(int level) {
-  m_status = 0;
-  SetProperty(GetSourceProperty(m_handle, "connect_verbose", &m_status), level,
-              &m_status);
-}
-
-inline HttpCamera::HttpCamera(const wpi::Twine& name, const wpi::Twine& url,
-                              HttpCameraKind kind) {
-  m_handle = CreateHttpCamera(
-      name, url, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
-      &m_status);
-}
-
-inline HttpCamera::HttpCamera(const wpi::Twine& name, const char* url,
-                              HttpCameraKind kind) {
-  m_handle = CreateHttpCamera(
-      name, url, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
-      &m_status);
-}
-
-inline HttpCamera::HttpCamera(const wpi::Twine& name, const std::string& url,
-                              HttpCameraKind kind)
-    : HttpCamera(name, wpi::Twine{url}, kind) {}
-
-inline HttpCamera::HttpCamera(const wpi::Twine& name,
-                              wpi::ArrayRef<std::string> urls,
-                              HttpCameraKind kind) {
-  m_handle = CreateHttpCamera(
-      name, urls, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
-      &m_status);
-}
-
-template <typename T>
-inline HttpCamera::HttpCamera(const wpi::Twine& name,
-                              std::initializer_list<T> urls,
-                              HttpCameraKind kind) {
-  std::vector<std::string> vec;
-  vec.reserve(urls.size());
-  for (const auto& url : urls) vec.emplace_back(url);
-  m_handle = CreateHttpCamera(
-      name, vec, static_cast<CS_HttpCameraKind>(static_cast<int>(kind)),
-      &m_status);
-}
-
-inline HttpCamera::HttpCameraKind HttpCamera::GetHttpCameraKind() const {
-  m_status = 0;
-  return static_cast<HttpCameraKind>(
-      static_cast<int>(::cs::GetHttpCameraKind(m_handle, &m_status)));
-}
-
-inline void HttpCamera::SetUrls(wpi::ArrayRef<std::string> urls) {
-  m_status = 0;
-  ::cs::SetHttpCameraUrls(m_handle, urls, &m_status);
-}
-
-template <typename T>
-inline void HttpCamera::SetUrls(std::initializer_list<T> urls) {
-  std::vector<std::string> vec;
-  vec.reserve(urls.size());
-  for (const auto& url : urls) vec.emplace_back(url);
-  m_status = 0;
-  ::cs::SetHttpCameraUrls(m_handle, vec, &m_status);
-}
-
-inline std::vector<std::string> HttpCamera::GetUrls() const {
-  m_status = 0;
-  return ::cs::GetHttpCameraUrls(m_handle, &m_status);
-}
-
-inline std::string AxisCamera::HostToUrl(const wpi::Twine& host) {
-  return ("http://" + host + "/mjpg/video.mjpg").str();
-}
-
-inline std::vector<std::string> AxisCamera::HostToUrl(
-    wpi::ArrayRef<std::string> hosts) {
-  std::vector<std::string> rv;
-  rv.reserve(hosts.size());
-  for (const auto& host : hosts)
-    rv.emplace_back(HostToUrl(wpi::StringRef{host}));
-  return rv;
-}
-
-template <typename T>
-inline std::vector<std::string> AxisCamera::HostToUrl(
-    std::initializer_list<T> hosts) {
-  std::vector<std::string> rv;
-  rv.reserve(hosts.size());
-  for (const auto& host : hosts)
-    rv.emplace_back(HostToUrl(wpi::StringRef{host}));
-  return rv;
-}
-
-inline AxisCamera::AxisCamera(const wpi::Twine& name, const wpi::Twine& host)
-    : HttpCamera(name, HostToUrl(host), kAxis) {}
-
-inline AxisCamera::AxisCamera(const wpi::Twine& name, const char* host)
-    : HttpCamera(name, HostToUrl(host), kAxis) {}
-
-inline AxisCamera::AxisCamera(const wpi::Twine& name, const std::string& host)
-    : HttpCamera(name, HostToUrl(wpi::Twine{host}), kAxis) {}
-
-inline AxisCamera::AxisCamera(const wpi::Twine& name, wpi::StringRef host)
-    : HttpCamera(name, HostToUrl(host), kAxis) {}
-
-inline AxisCamera::AxisCamera(const wpi::Twine& name,
-                              wpi::ArrayRef<std::string> hosts)
-    : HttpCamera(name, HostToUrl(hosts), kAxis) {}
-
-template <typename T>
-inline AxisCamera::AxisCamera(const wpi::Twine& name,
-                              std::initializer_list<T> hosts)
-    : HttpCamera(name, HostToUrl(hosts), kAxis) {}
-
-inline void ImageSource::NotifyError(const wpi::Twine& msg) {
-  m_status = 0;
-  NotifySourceError(m_handle, msg, &m_status);
-}
-
-inline void ImageSource::SetConnected(bool connected) {
-  m_status = 0;
-  SetSourceConnected(m_handle, connected, &m_status);
-}
-
-inline void ImageSource::SetDescription(const wpi::Twine& description) {
-  m_status = 0;
-  SetSourceDescription(m_handle, description, &m_status);
-}
-
-inline VideoProperty ImageSource::CreateProperty(const wpi::Twine& name,
-                                                 VideoProperty::Kind kind,
-                                                 int minimum, int maximum,
-                                                 int step, int defaultValue,
-                                                 int value) {
-  m_status = 0;
-  return VideoProperty{CreateSourceProperty(
-      m_handle, name, static_cast<CS_PropertyKind>(static_cast<int>(kind)),
-      minimum, maximum, step, defaultValue, value, &m_status)};
-}
-
-inline VideoProperty ImageSource::CreateIntegerProperty(const wpi::Twine& name,
-                                                        int minimum,
-                                                        int maximum, int step,
-                                                        int defaultValue,
-                                                        int value) {
-  m_status = 0;
-  return VideoProperty{CreateSourceProperty(
-      m_handle, name,
-      static_cast<CS_PropertyKind>(
-          static_cast<int>(VideoProperty::Kind::kInteger)),
-      minimum, maximum, step, defaultValue, value, &m_status)};
-}
-
-inline VideoProperty ImageSource::CreateBooleanProperty(const wpi::Twine& name,
-                                                        bool defaultValue,
-                                                        bool value) {
-  m_status = 0;
-  return VideoProperty{CreateSourceProperty(
-      m_handle, name,
-      static_cast<CS_PropertyKind>(
-          static_cast<int>(VideoProperty::Kind::kBoolean)),
-      0, 1, 1, defaultValue ? 1 : 0, value ? 1 : 0, &m_status)};
-}
-
-inline VideoProperty ImageSource::CreateStringProperty(
-    const wpi::Twine& name, const wpi::Twine& value) {
-  m_status = 0;
-  auto prop = VideoProperty{
-      CreateSourceProperty(m_handle, name,
-                           static_cast<CS_PropertyKind>(
-                               static_cast<int>(VideoProperty::Kind::kString)),
-                           0, 0, 0, 0, 0, &m_status)};
-  prop.SetString(value);
-  return prop;
-}
-
-inline void ImageSource::SetEnumPropertyChoices(
-    const VideoProperty& property, wpi::ArrayRef<std::string> choices) {
-  m_status = 0;
-  SetSourceEnumPropertyChoices(m_handle, property.m_handle, choices, &m_status);
-}
-
-template <typename T>
-inline void ImageSource::SetEnumPropertyChoices(
-    const VideoProperty& property, std::initializer_list<T> choices) {
-  std::vector<std::string> vec;
-  vec.reserve(choices.size());
-  for (const auto& choice : choices) vec.emplace_back(choice);
-  m_status = 0;
-  SetSourceEnumPropertyChoices(m_handle, property.m_handle, vec, &m_status);
-}
-
-inline VideoSink::VideoSink(const VideoSink& sink)
-    : m_handle(sink.m_handle == 0 ? 0 : CopySink(sink.m_handle, &m_status)) {}
-
-inline VideoSink::VideoSink(VideoSink&& other) noexcept : VideoSink() {
-  swap(*this, other);
-}
-
-inline VideoSink& VideoSink::operator=(VideoSink other) noexcept {
-  swap(*this, other);
-  return *this;
-}
-
-inline VideoSink::~VideoSink() {
-  m_status = 0;
-  if (m_handle != 0) ReleaseSink(m_handle, &m_status);
-}
-
-inline VideoSink::Kind VideoSink::GetKind() const {
-  m_status = 0;
-  return static_cast<VideoSink::Kind>(GetSinkKind(m_handle, &m_status));
-}
-
-inline std::string VideoSink::GetName() const {
-  m_status = 0;
-  return GetSinkName(m_handle, &m_status);
-}
-
-inline std::string VideoSink::GetDescription() const {
-  m_status = 0;
-  return GetSinkDescription(m_handle, &m_status);
-}
-
-inline VideoProperty VideoSink::GetProperty(const wpi::Twine& name) {
-  m_status = 0;
-  return VideoProperty{GetSinkProperty(m_handle, name, &m_status)};
-}
-
-inline void VideoSink::SetSource(VideoSource source) {
-  m_status = 0;
-  if (!source)
-    SetSinkSource(m_handle, 0, &m_status);
-  else
-    SetSinkSource(m_handle, source.m_handle, &m_status);
-}
-
-inline VideoSource VideoSink::GetSource() const {
-  m_status = 0;
-  auto handle = GetSinkSource(m_handle, &m_status);
-  return VideoSource{handle == 0 ? 0 : CopySource(handle, &m_status)};
-}
-
-inline VideoProperty VideoSink::GetSourceProperty(const wpi::Twine& name) {
-  m_status = 0;
-  return VideoProperty{GetSinkSourceProperty(m_handle, name, &m_status)};
-}
-
-inline bool VideoSink::SetConfigJson(wpi::StringRef config) {
-  m_status = 0;
-  return SetSinkConfigJson(m_handle, config, &m_status);
-}
-
-inline bool VideoSink::SetConfigJson(const wpi::json& config) {
-  m_status = 0;
-  return SetSinkConfigJson(m_handle, config, &m_status);
-}
-
-inline std::string VideoSink::GetConfigJson() const {
-  m_status = 0;
-  return GetSinkConfigJson(m_handle, &m_status);
-}
-
-inline MjpegServer::MjpegServer(const wpi::Twine& name,
-                                const wpi::Twine& listenAddress, int port) {
-  m_handle = CreateMjpegServer(name, listenAddress, port, &m_status);
-}
-
-inline std::string MjpegServer::GetListenAddress() const {
-  m_status = 0;
-  return cs::GetMjpegServerListenAddress(m_handle, &m_status);
-}
-
-inline int MjpegServer::GetPort() const {
-  m_status = 0;
-  return cs::GetMjpegServerPort(m_handle, &m_status);
-}
-
-inline void MjpegServer::SetResolution(int width, int height) {
-  m_status = 0;
-  SetProperty(GetSinkProperty(m_handle, "width", &m_status), width, &m_status);
-  SetProperty(GetSinkProperty(m_handle, "height", &m_status), height,
-              &m_status);
-}
-
-inline void MjpegServer::SetFPS(int fps) {
-  m_status = 0;
-  SetProperty(GetSinkProperty(m_handle, "fps", &m_status), fps, &m_status);
-}
-
-inline void MjpegServer::SetCompression(int quality) {
-  m_status = 0;
-  SetProperty(GetSinkProperty(m_handle, "compression", &m_status), quality,
-              &m_status);
-}
-
-inline void MjpegServer::SetDefaultCompression(int quality) {
-  m_status = 0;
-  SetProperty(GetSinkProperty(m_handle, "default_compression", &m_status),
-              quality, &m_status);
-}
-
-inline void ImageSink::SetDescription(const wpi::Twine& description) {
-  m_status = 0;
-  SetSinkDescription(m_handle, description, &m_status);
-}
-
-inline std::string ImageSink::GetError() const {
-  m_status = 0;
-  return GetSinkError(m_handle, &m_status);
-}
-
-inline void ImageSink::SetEnabled(bool enabled) {
-  m_status = 0;
-  SetSinkEnabled(m_handle, enabled, &m_status);
-}
-
-inline VideoSource VideoEvent::GetSource() const {
-  CS_Status status = 0;
-  return VideoSource{sourceHandle == 0 ? 0 : CopySource(sourceHandle, &status)};
-}
-
-inline VideoSink VideoEvent::GetSink() const {
-  CS_Status status = 0;
-  return VideoSink{sinkHandle == 0 ? 0 : CopySink(sinkHandle, &status)};
-}
-
-inline VideoProperty VideoEvent::GetProperty() const {
-  return VideoProperty{propertyHandle,
-                       static_cast<VideoProperty::Kind>(propertyKind)};
-}
-
-inline VideoListener::VideoListener(
-    std::function<void(const VideoEvent& event)> callback, int eventMask,
-    bool immediateNotify) {
-  CS_Status status = 0;
-  m_handle = AddListener(
-      [=](const RawEvent& event) {
-        callback(static_cast<const VideoEvent&>(event));
-      },
-      eventMask, immediateNotify, &status);
-}
-
-inline VideoListener::VideoListener(VideoListener&& other) noexcept
-    : VideoListener() {
-  swap(*this, other);
-}
-
-inline VideoListener& VideoListener::operator=(VideoListener&& other) noexcept {
-  swap(*this, other);
-  return *this;
-}
-
-inline VideoListener::~VideoListener() {
-  CS_Status status = 0;
-  if (m_handle != 0) RemoveListener(m_handle, &status);
-}
-
-}  // namespace cs
-
-#endif  // CSCORE_CSCORE_OO_INL_
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h
index 902d90e..0aaaeff 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_RAW_H_
 #define CSCORE_CSCORE_RAW_H_
@@ -17,7 +14,7 @@
 /**
  * Raw Frame
  */
-typedef struct CS_RawFrame {
+typedef struct CS_RawFrame {  // NOLINT
   char* data;
   int dataLength;
   int pixelFormat;
@@ -81,11 +78,11 @@
  * @{
  */
 
-CS_Source CreateRawSource(const wpi::Twine& name, const VideoMode& mode,
+CS_Source CreateRawSource(std::string_view name, const VideoMode& mode,
                           CS_Status* status);
 
-CS_Sink CreateRawSink(const wpi::Twine& name, CS_Status* status);
-CS_Sink CreateRawSinkCallback(const wpi::Twine& name,
+CS_Sink CreateRawSink(std::string_view name, CS_Status* status);
+CS_Sink CreateRawSinkCallback(std::string_view name,
                               std::function<void(uint64_t time)> processFrame,
                               CS_Status* status);
 
@@ -110,7 +107,7 @@
    * @param name Source name (arbitrary unique identifier)
    * @param mode Video mode being generated
    */
-  RawSource(const wpi::Twine& name, const VideoMode& mode);
+  RawSource(std::string_view name, const VideoMode& mode);
 
   /**
    * Create a raw frame source.
@@ -121,7 +118,7 @@
    * @param height height
    * @param fps fps
    */
-  RawSource(const wpi::Twine& name, VideoMode::PixelFormat pixelFormat,
+  RawSource(std::string_view name, VideoMode::PixelFormat pixelFormat,
             int width, int height, int fps);
 
  protected:
@@ -150,7 +147,7 @@
    *
    * @param name Source name (arbitrary unique identifier)
    */
-  explicit RawSink(const wpi::Twine& name);
+  explicit RawSink(std::string_view name);
 
   /**
    * Create a sink for accepting raws images in a separate thread.
@@ -164,7 +161,7 @@
    *        or GetError() as needed, but should not call (except in very
    *        unusual circumstances) WaitForImage().
    */
-  RawSink(const wpi::Twine& name,
+  RawSink(std::string_view name,
           std::function<void(uint64_t time)> processFrame);
 
  protected:
@@ -177,7 +174,8 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrame(RawFrame& image, double timeout = 0.225) const;
+  [[nodiscard]] uint64_t GrabFrame(RawFrame& image,
+                                   double timeout = 0.225) const;
 
   /**
    * Wait for the next frame and get the image.  May block forever.
@@ -187,14 +185,14 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrameNoTimeout(RawFrame& image) const;
+  [[nodiscard]] uint64_t GrabFrameNoTimeout(RawFrame& image) const;
 };
 
-inline RawSource::RawSource(const wpi::Twine& name, const VideoMode& mode) {
+inline RawSource::RawSource(std::string_view name, const VideoMode& mode) {
   m_handle = CreateRawSource(name, mode, &m_status);
 }
 
-inline RawSource::RawSource(const wpi::Twine& name,
+inline RawSource::RawSource(std::string_view name,
                             VideoMode::PixelFormat format, int width,
                             int height, int fps) {
   m_handle =
@@ -206,11 +204,11 @@
   PutSourceFrame(m_handle, image, &m_status);
 }
 
-inline RawSink::RawSink(const wpi::Twine& name) {
+inline RawSink::RawSink(std::string_view name) {
   m_handle = CreateRawSink(name, &m_status);
 }
 
-inline RawSink::RawSink(const wpi::Twine& name,
+inline RawSink::RawSink(std::string_view name,
                         std::function<void(uint64_t time)> processFrame) {
   m_handle = CreateRawSinkCallback(name, processFrame, &m_status);
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h
index ed40006..5b9a374 100644
--- a/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h
+++ b/third_party/allwpilib/cscore/src/main/native/include/cscore_raw_cv.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_CSCORE_RAW_CV_H_
 #define CSCORE_CSCORE_RAW_CV_H_
@@ -12,6 +9,8 @@
 #error "Cannot include both cscore_cv.h and cscore_raw_cv.h in the same file"
 #endif
 
+#include <opencv2/core/mat.hpp>
+
 #include "cscore_raw.h"
 
 namespace cs {
@@ -35,7 +34,7 @@
    * @param name Source name (arbitrary unique identifier)
    * @param mode Video mode being generated
    */
-  RawCvSource(const wpi::Twine& name, const VideoMode& mode);
+  RawCvSource(std::string_view name, const VideoMode& mode);
 
   /**
    * Create a Raw OpenCV source.
@@ -46,7 +45,7 @@
    * @param height height
    * @param fps fps
    */
-  RawCvSource(const wpi::Twine& name, VideoMode::PixelFormat pixelFormat,
+  RawCvSource(std::string_view name, VideoMode::PixelFormat pixelFormat,
               int width, int height, int fps);
 
   /**
@@ -86,7 +85,7 @@
    *
    * @param name Source name (arbitrary unique identifier)
    */
-  explicit RawCvSink(const wpi::Twine& name);
+  explicit RawCvSink(std::string_view name);
 
   /**
    * Create a sink for accepting OpenCV images in a separate thread.
@@ -100,7 +99,7 @@
    *        or GetError() as needed, but should not call (except in very
    *        unusual circumstances) WaitForImage().
    */
-  RawCvSink(const wpi::Twine& name,
+  RawCvSink(std::string_view name,
             std::function<void(uint64_t time)> processFrame);
 
   /**
@@ -112,7 +111,7 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrame(cv::Mat& image, double timeout = 0.225);
+  [[nodiscard]] uint64_t GrabFrame(cv::Mat& image, double timeout = 0.225);
 
   /**
    * Wait for the next frame and get the image.  May block forever.
@@ -122,7 +121,7 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrameNoTimeout(cv::Mat& image);
+  [[nodiscard]] uint64_t GrabFrameNoTimeout(cv::Mat& image);
 
   /**
    * Wait for the next frame and get the image.
@@ -133,7 +132,8 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrameDirect(cv::Mat& image, double timeout = 0.225);
+  [[nodiscard]] uint64_t GrabFrameDirect(cv::Mat& image,
+                                         double timeout = 0.225);
 
   /**
    * Wait for the next frame and get the image.  May block forever.
@@ -143,16 +143,16 @@
    *         message); the frame time is in the same time base as wpi::Now(),
    *         and is in 1 us increments.
    */
-  uint64_t GrabFrameNoTimeoutDirect(cv::Mat& image);
+  [[nodiscard]] uint64_t GrabFrameNoTimeoutDirect(cv::Mat& image);
 
  private:
   RawFrame rawFrame;
 };
 
-inline RawCvSource::RawCvSource(const wpi::Twine& name, const VideoMode& mode)
+inline RawCvSource::RawCvSource(std::string_view name, const VideoMode& mode)
     : RawSource{name, mode} {}
 
-inline RawCvSource::RawCvSource(const wpi::Twine& name,
+inline RawCvSource::RawCvSource(std::string_view name,
                                 VideoMode::PixelFormat format, int width,
                                 int height, int fps)
     : RawSource{name, format, width, height, fps} {}
@@ -162,34 +162,34 @@
   rawFrame.data = reinterpret_cast<char*>(image.data);
   rawFrame.width = image.cols;
   rawFrame.height = image.rows;
-  rawFrame.totalData = image.total() * image.channels;
-  rawFrame.pixelFormat = image.channels == 3 ? CS_PIXFMT_BGR : CS_PIXFMT_GRAY;
+  rawFrame.totalData = image.total() * image.channels();
+  rawFrame.pixelFormat = image.channels() == 3 ? CS_PIXFMT_BGR : CS_PIXFMT_GRAY;
   PutSourceFrame(m_handle, rawFrame, &m_status);
 }
 
-inline RawCvSink::RawCvSink(const wpi::Twine& name) : RawSink{name} {}
+inline RawCvSink::RawCvSink(std::string_view name) : RawSink{name} {}
 
-inline RawCvSink::RawCvSink(const wpi::Twine& name,
+inline RawCvSink::RawCvSink(std::string_view name,
                             std::function<void(uint64_t time)> processFrame)
     : RawSink{name, processFrame} {}
 
 inline uint64_t RawCvSink::GrabFrame(cv::Mat& image, double timeout) {
-  cv::Mat tmpMat;
-  auto retVal = GrabFrameDirect(tmpMat);
+  cv::Mat tmpnam;
+  auto retVal = GrabFrameDirect(tmpnam);
   if (retVal <= 0) {
     return retVal;
   }
-  tmpMat.copyTo(image);
+  tmpnam.copyTo(image);
   return retVal;
 }
 
 inline uint64_t RawCvSink::GrabFrameNoTimeout(cv::Mat& image) {
-  cv::Mat tmpMat;
-  auto retVal = GrabFrameNoTimeoutDirect(tmpMat);
+  cv::Mat tmpnam;
+  auto retVal = GrabFrameNoTimeoutDirect(tmpnam);
   if (retVal <= 0) {
     return retVal;
   }
-  tmpMat.copyTo(image);
+  tmpnam.copyTo(image);
   return retVal;
 }
 
@@ -198,7 +198,9 @@
   rawFrame.width = 0;
   rawFrame.pixelFormat = CS_PixelFormat::CS_PIXFMT_BGR;
   m_status = RawSink::GrabFrame(rawFrame, timeout);
-  if (m_status <= 0) return m_status;
+  if (m_status <= 0) {
+    return m_status;
+  }
   image = cv::Mat{rawFrame.height, rawFrame.width, CV_8UC3, rawFrame.data};
   return m_status;
 }
@@ -208,7 +210,9 @@
   rawFrame.width = 0;
   rawFrame.pixelFormat = CS_PixelFormat::CS_PIXFMT_BGR;
   m_status = RawSink::GrabFrameNoTimeout(rawFrame);
-  if (m_status <= 0) return m_status;
+  if (m_status <= 0) {
+    return m_status;
+  }
   image = cv::Mat{rawFrame.height, rawFrame.width, CV_8UC3, rawFrame.data};
   return m_status;
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp b/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp
index 6915b30..1a0ac8b 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/linux/NetworkListener.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "NetworkListener.h"
 
@@ -37,7 +34,7 @@
    public:
     Thread(wpi::Logger& logger, Notifier& notifier)
         : m_logger(logger), m_notifier(notifier) {}
-    void Main();
+    void Main() override;
 
     wpi::Logger& m_logger;
     Notifier& m_notifier;
@@ -50,7 +47,9 @@
 NetworkListener::NetworkListener(wpi::Logger& logger, Notifier& notifier)
     : m_impl(std::make_unique<Impl>(logger, notifier)) {}
 
-NetworkListener::~NetworkListener() { Stop(); }
+NetworkListener::~NetworkListener() {
+  Stop();
+}
 
 void NetworkListener::Start() {
   m_impl->m_owner.Start(m_impl->m_logger, m_impl->m_notifier);
@@ -60,7 +59,9 @@
   // Wake up thread
   if (auto thr = m_impl->m_owner.GetThread()) {
     thr->m_active = false;
-    if (thr->m_command_fd >= 0) eventfd_write(thr->m_command_fd, 1);
+    if (thr->m_command_fd >= 0) {
+      eventfd_write(thr->m_command_fd, 1);
+    }
   }
   m_impl->m_owner.Stop();
 }
@@ -69,15 +70,15 @@
   // Create event socket so we can be shut down
   m_command_fd = ::eventfd(0, 0);
   if (m_command_fd < 0) {
-    ERROR(
-        "NetworkListener: could not create eventfd: " << std::strerror(errno));
+    ERROR("NetworkListener: could not create eventfd: {}",
+          std::strerror(errno));
     return;
   }
 
   // Create netlink socket
   int sd = ::socket(AF_NETLINK, SOCK_RAW, NETLINK_ROUTE);
   if (sd < 0) {
-    ERROR("NetworkListener: could not create socket: " << std::strerror(errno));
+    ERROR("NetworkListener: could not create socket: {}", std::strerror(errno));
     ::close(m_command_fd);
     m_command_fd = -1;
     return;
@@ -89,7 +90,7 @@
   addr.nl_family = AF_NETLINK;
   addr.nl_groups = RTMGRP_LINK | RTMGRP_IPV4_IFADDR;
   if (bind(sd, reinterpret_cast<struct sockaddr*>(&addr), sizeof(addr)) < 0) {
-    ERROR("NetworkListener: could not create socket: " << std::strerror(errno));
+    ERROR("NetworkListener: could not create socket: {}", std::strerror(errno));
     ::close(sd);
     ::close(m_command_fd);
     m_command_fd = -1;
@@ -112,30 +113,40 @@
     int nfds = std::max(m_command_fd, sd) + 1;
 
     if (::select(nfds, &readfds, nullptr, nullptr, &tv) < 0) {
-      ERROR("NetworkListener: select(): " << std::strerror(errno));
+      ERROR("NetworkListener: select(): {}", std::strerror(errno));
       break;  // XXX: is this the right thing to do here?
     }
 
     // Double-check to see if we're shutting down
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
 
-    if (!FD_ISSET(sd, &readfds)) continue;
+    if (!FD_ISSET(sd, &readfds)) {
+      continue;
+    }
 
     std::memset(&addr, 0, sizeof(addr));
     struct iovec iov = {buf, sizeof(buf)};
     struct msghdr msg = {&addr, sizeof(addr), &iov, 1, nullptr, 0, 0};
     int len = ::recvmsg(sd, &msg, 0);
     if (len < 0) {
-      if (errno == EWOULDBLOCK || errno == EAGAIN) continue;
-      ERROR(
-          "NetworkListener: could not read netlink: " << std::strerror(errno));
+      if (errno == EWOULDBLOCK || errno == EAGAIN) {
+        continue;
+      }
+      ERROR("NetworkListener: could not read netlink: {}",
+            std::strerror(errno));
       break;  // XXX: is this the right thing to do here?
     }
-    if (len == 0) continue;  // EOF?
+    if (len == 0) {
+      continue;  // EOF?
+    }
     unsigned int ulen = static_cast<unsigned int>(len);
     for (struct nlmsghdr* nh = reinterpret_cast<struct nlmsghdr*>(buf);
          NLMSG_OK(nh, ulen); nh = NLMSG_NEXT(nh, ulen)) {
-      if (nh->nlmsg_type == NLMSG_DONE) break;
+      if (nh->nlmsg_type == NLMSG_DONE) {
+        break;
+      }
       if (nh->nlmsg_type == RTM_NEWLINK || nh->nlmsg_type == RTM_DELLINK ||
           nh->nlmsg_type == RTM_NEWADDR || nh->nlmsg_type == RTM_DELADDR) {
         m_notifier.NotifyNetworkInterfacesChanged();
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/NetworkUtil.cpp b/third_party/allwpilib/cscore/src/main/native/linux/NetworkUtil.cpp
index 2c1f3cd..43e0fa8 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/NetworkUtil.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/linux/NetworkUtil.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_cpp.h"  // NOLINT(build/include_order)
 
@@ -16,17 +13,25 @@
 
 std::vector<std::string> GetNetworkInterfaces() {
   struct ifaddrs* ifa;
-  if (::getifaddrs(&ifa) != 0) return std::vector<std::string>{};
+  if (::getifaddrs(&ifa) != 0) {
+    return {};
+  }
 
   std::vector<std::string> rv;
   char buf[256];
   for (struct ifaddrs* i = ifa; i; i = i->ifa_next) {
-    if (!i->ifa_addr) continue;                       // no address
-    if (i->ifa_addr->sa_family != AF_INET) continue;  // only return IPv4
+    if (!i->ifa_addr) {
+      continue;  // no address
+    }
+    if (i->ifa_addr->sa_family != AF_INET) {
+      continue;  // only return IPv4
+    }
     struct sockaddr_in* addr_in = reinterpret_cast<sockaddr_in*>(i->ifa_addr);
     const char* addr =
         ::inet_ntop(addr_in->sin_family, &addr_in->sin_addr, buf, sizeof(buf));
-    if (!addr) continue;  // error converting address
+    if (!addr) {
+      continue;  // error converting address
+    }
     rv.emplace_back(addr);
   }
 
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraBuffer.h b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraBuffer.h
index 98ac149..91fd588 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraBuffer.h
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraBuffer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_USBCAMERABUFFER_H_
 #define CSCORE_USBCAMERABUFFER_H_
@@ -16,7 +13,7 @@
 
 class UsbCameraBuffer {
  public:
-  UsbCameraBuffer() noexcept : m_data{nullptr}, m_length{0} {}
+  UsbCameraBuffer() noexcept = default;
   UsbCameraBuffer(UsbCameraBuffer&& other) noexcept : UsbCameraBuffer() {
     swap(*this, other);
   }
@@ -38,7 +35,9 @@
   }
 
   ~UsbCameraBuffer() {
-    if (m_data) munmap(m_data, m_length);
+    if (m_data) {
+      munmap(m_data, m_length);
+    }
   }
 
   friend void swap(UsbCameraBuffer& first, UsbCameraBuffer& second) noexcept {
@@ -47,8 +46,8 @@
     swap(first.m_length, second.m_length);
   }
 
-  void* m_data;
-  size_t m_length;
+  void* m_data{nullptr};
+  size_t m_length{0};
 };
 
 }  // namespace cs
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.cpp b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.cpp
index 1175ca3..a08fbbc 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "UsbCameraImpl.h"
 
@@ -23,11 +20,13 @@
 #include <unistd.h>
 
 #include <algorithm>
+#include <memory>
 
-#include <wpi/FileSystem.h>
+#include <fmt/format.h>
 #include <wpi/MemAlloc.h>
-#include <wpi/Path.h>
 #include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
 #include <wpi/raw_ostream.h>
 #include <wpi/timestamp.h>
 
@@ -42,6 +41,14 @@
 
 using namespace cs;
 
+namespace {
+// Find the length of an array.
+template <class T, size_t N>
+constexpr inline size_t array_lengthof(T (&)[N]) {
+  return N;
+}
+}  // namespace
+
 static constexpr char const* kPropWbAuto = "white_balance_temperature_auto";
 static constexpr char const* kPropWbValue = "white_balance_temperature";
 static constexpr char const* kPropExAuto = "exposure_auto";
@@ -98,8 +105,10 @@
   }
 }
 
-static bool IsPercentageProperty(wpi::StringRef name) {
-  if (name.startswith("raw_")) name = name.substr(4);
+static bool IsPercentageProperty(std::string_view name) {
+  if (wpi::starts_with(name, "raw_")) {
+    name = name.substr(4);
+  }
   return name == "brightness" || name == "contrast" || name == "saturation" ||
          name == "hue" || name == "sharpness" || name == "gain" ||
          name == "exposure_absolute" || name == "exposure_time_absolute";
@@ -123,9 +132,11 @@
   // LifeCam exposure setting quirk
   if (m_lifecam_exposure && rawProp.name == "raw_exposure_absolute" &&
       rawProp.minimum == 5 && rawProp.maximum == 20000) {
-    int nelems = wpi::array_lengthof(quirkLifeCamHd3000);
+    int nelems = array_lengthof(quirkLifeCamHd3000);
     for (int i = 0; i < nelems; ++i) {
-      if (rawValue < quirkLifeCamHd3000[i]) return 100.0 * i / nelems;
+      if (rawValue < quirkLifeCamHd3000[i]) {
+        return 100.0 * i / nelems;
+      }
     }
     return 100;
   }
@@ -138,10 +149,14 @@
   // LifeCam exposure setting quirk
   if (m_lifecam_exposure && rawProp.name == "raw_exposure_absolute" &&
       rawProp.minimum == 5 && rawProp.maximum == 20000) {
-    int nelems = wpi::array_lengthof(quirkLifeCamHd3000);
+    int nelems = array_lengthof(quirkLifeCamHd3000);
     int ndx = nelems * percentValue / 100.0;
-    if (ndx < 0) ndx = 0;
-    if (ndx >= nelems) ndx = nelems - 1;
+    if (ndx < 0) {
+      ndx = 0;
+    }
+    if (ndx >= nelems) {
+      ndx = nelems - 1;
+    }
     return quirkLifeCamHd3000[ndx];
   }
   return rawProp.minimum +
@@ -149,52 +164,64 @@
 }
 
 static bool GetVendorProduct(int dev, int* vendor, int* product) {
-  wpi::SmallString<64> ifpath;
-  {
-    wpi::raw_svector_ostream oss{ifpath};
-    oss << "/sys/class/video4linux/video" << dev << "/device/modalias";
-  }
+  auto ifpath =
+      fmt::format("/sys/class/video4linux/video{}/device/modalias", dev);
 
   int fd = open(ifpath.c_str(), O_RDONLY);
-  if (fd < 0) return false;
+  if (fd < 0) {
+    return false;
+  }
 
   char readBuf[128];
   ssize_t n = read(fd, readBuf, sizeof(readBuf));
   close(fd);
 
-  if (n <= 0) return false;
-  wpi::StringRef readStr{readBuf};
-  if (readStr.substr(readStr.find('v')).substr(1, 4).getAsInteger(16, *vendor))
+  if (n <= 0) {
     return false;
-  if (readStr.substr(readStr.find('p')).substr(1, 4).getAsInteger(16, *product))
+  }
+  std::string_view readStr{readBuf};
+  if (auto v = wpi::parse_integer<int>(
+          readStr.substr(readStr.find('v')).substr(1, 4), 16)) {
+    *vendor = v.value();
+  } else {
     return false;
+  }
+  if (auto v = wpi::parse_integer<int>(
+          readStr.substr(readStr.find('p')).substr(1, 4), 16)) {
+    *product = v.value();
+  } else {
+    return false;
+  }
 
   return true;
 }
 
 static bool GetDescriptionSysV4L(int dev, std::string* desc) {
-  wpi::SmallString<64> ifpath;
-  {
-    wpi::raw_svector_ostream oss{ifpath};
-    oss << "/sys/class/video4linux/video" << dev << "/device/interface";
-  }
+  auto ifpath =
+      fmt::format("/sys/class/video4linux/video{}/device/interface", dev);
 
   int fd = open(ifpath.c_str(), O_RDONLY);
-  if (fd < 0) return false;
+  if (fd < 0) {
+    return false;
+  }
 
   char readBuf[128];
   ssize_t n = read(fd, readBuf, sizeof(readBuf));
   close(fd);
 
-  if (n <= 0) return false;
+  if (n <= 0) {
+    return false;
+  }
 
-  *desc = wpi::StringRef(readBuf, n).rtrim();
+  *desc = wpi::rtrim(std::string_view(readBuf, n));
   return true;
 }
 
 static bool GetDescriptionIoctl(const char* cpath, std::string* desc) {
   int fd = open(cpath, O_RDWR);
-  if (fd < 0) return false;
+  if (fd < 0) {
+    return false;
+  }
 
   struct v4l2_capability vcap;
   std::memset(&vcap, 0, sizeof(vcap));
@@ -204,17 +231,16 @@
   }
   close(fd);
 
-  wpi::StringRef card{reinterpret_cast<const char*>(vcap.card)};
+  std::string_view card{reinterpret_cast<const char*>(vcap.card)};
   // try to convert "UVC Camera (0000:0000)" into a better name
-  int vendor = 0;
-  int product = 0;
-  if (card.startswith("UVC Camera (") &&
-      !card.substr(12, 4).getAsInteger(16, vendor) &&
-      !card.substr(17, 4).getAsInteger(16, product)) {
-    wpi::SmallString<64> card2Buf;
-    wpi::StringRef card2 = GetUsbNameFromId(vendor, product, card2Buf);
+  std::optional<int> vendor;
+  std::optional<int> product;
+  if (wpi::starts_with(card, "UVC Camera (") &&
+      (vendor = wpi::parse_integer<int>(card.substr(12, 4), 16)) &&
+      (product = wpi::parse_integer<int>(card.substr(17, 4), 16))) {
+    std::string card2 = GetUsbNameFromId(vendor.value(), product.value());
     if (!card2.empty()) {
-      *desc = card2;
+      *desc = std::move(card2);
       return true;
     }
   }
@@ -225,7 +251,9 @@
 
 static bool IsVideoCaptureDevice(const char* cpath) {
   int fd = open(cpath, O_RDWR);
-  if (fd < 0) return false;
+  if (fd < 0) {
+    return false;
+  }
 
   struct v4l2_capability vcap;
   std::memset(&vcap, 0, sizeof(vcap));
@@ -243,25 +271,22 @@
 }
 
 static int GetDeviceNum(const char* cpath) {
-  wpi::StringRef path{cpath};
-  std::string pathBuf;
+  fs::path path{cpath};
 
   // it might be a symlink; if so, find the symlink target (e.g. /dev/videoN),
   // add that to the list and make it the keypath
-  if (wpi::sys::fs::is_symlink_file(cpath)) {
-    char* target = ::realpath(cpath, nullptr);
-    if (target) {
-      pathBuf = target;
-      path = pathBuf;
-      std::free(target);
-    }
+  if (fs::is_symlink(path)) {
+    path = fs::canonical(path);
   }
 
-  path = wpi::sys::path::filename(path);
-  if (!path.startswith("video")) return -1;
-  int dev = -1;
-  if (path.substr(5).getAsInteger(10, dev)) return -1;
-  return dev;
+  std::string fn = path.filename();
+  if (!wpi::starts_with(fn, "video")) {
+    return -1;
+  }
+  if (auto dev = wpi::parse_integer<int>(fn.substr(5), 10)) {
+    return dev.value();
+  }
+  return -1;
 }
 
 static std::string GetDescriptionImpl(const char* cpath) {
@@ -270,23 +295,27 @@
   int dev = GetDeviceNum(cpath);
   if (dev >= 0) {
     // Sometimes the /sys tree gives a better name.
-    if (GetDescriptionSysV4L(dev, &rv)) return rv;
+    if (GetDescriptionSysV4L(dev, &rv)) {
+      return rv;
+    }
   }
 
   // Otherwise use an ioctl to query the caps and get the card name
-  if (GetDescriptionIoctl(cpath, &rv)) return rv;
+  if (GetDescriptionIoctl(cpath, &rv)) {
+    return rv;
+  }
 
   return std::string{};
 }
 
-UsbCameraImpl::UsbCameraImpl(const wpi::Twine& name, wpi::Logger& logger,
+UsbCameraImpl::UsbCameraImpl(std::string_view name, wpi::Logger& logger,
                              Notifier& notifier, Telemetry& telemetry,
-                             const wpi::Twine& path)
+                             std::string_view path)
     : SourceImpl{name, logger, notifier, telemetry},
       m_fd{-1},
       m_command_fd{eventfd(0, 0)},
       m_active{true},
-      m_path{path.str()} {
+      m_path{path} {
   SetDescription(GetDescriptionImpl(m_path.c_str()));
   SetQuirks();
 
@@ -308,17 +337,23 @@
   Send(Message{Message::kNone});
 
   // join camera thread
-  if (m_cameraThread.joinable()) m_cameraThread.join();
+  if (m_cameraThread.joinable()) {
+    m_cameraThread.join();
+  }
 
   // close command fd
   int fd = m_command_fd.exchange(-1);
-  if (fd >= 0) close(fd);
+  if (fd >= 0) {
+    close(fd);
+  }
 }
 
 static inline void DoFdSet(int fd, fd_set* set, int* nfds) {
   if (fd >= 0) {
     FD_SET(fd, set);
-    if ((fd + 1) > *nfds) *nfds = fd + 1;
+    if ((fd + 1) > *nfds) {
+      *nfds = fd + 1;
+    }
   }
 }
 
@@ -341,8 +376,8 @@
       close(notify_fd);
       notify_fd = -1;
     } else {
-      notify_is.reset(new wpi::raw_fd_istream{
-          notify_fd, true, sizeof(struct inotify_event) + NAME_MAX + 1});
+      notify_is = std::make_unique<wpi::raw_fd_istream>(
+          notify_fd, true, sizeof(struct inotify_event) + NAME_MAX + 1);
     }
   }
   bool notified = (notify_fd < 0);  // treat as always notified if cannot notify
@@ -360,12 +395,16 @@
 
   while (m_active) {
     // If not connected, try to reconnect
-    if (m_fd < 0) DeviceConnect();
+    if (m_fd < 0) {
+      DeviceConnect();
+    }
 
     // Make copies of fd's in case they go away
     int command_fd = m_command_fd.load();
     int fd = m_fd.load();
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
 
     // Reset notified flag and restart streaming if necessary
     if (fd >= 0) {
@@ -398,20 +437,24 @@
     fd_set readfds;
     FD_ZERO(&readfds);
     DoFdSet(command_fd, &readfds, &nfds);
-    if (m_streaming) DoFdSet(fd, &readfds, &nfds);
+    if (m_streaming) {
+      DoFdSet(fd, &readfds, &nfds);
+    }
     DoFdSet(notify_fd, &readfds, &nfds);
 
     if (select(nfds, &readfds, nullptr, nullptr, &tv) < 0) {
-      SERROR("select(): " << std::strerror(errno));
+      SERROR("select(): {}", std::strerror(errno));
       break;  // XXX: is this the right thing to do here?
     }
 
     // Double-check to see if we're shutting down
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
 
     // Handle notify events
     if (notify_fd >= 0 && FD_ISSET(notify_fd, &readfds)) {
-      SDEBUG4("notify event");
+      SDEBUG4("{}", "notify event");
       struct inotify_event event;
       do {
         // Read the event structure
@@ -421,10 +464,9 @@
         raw_name.resize(event.len);
         notify_is->read(raw_name.data(), event.len);
         // If the name is what we expect...
-        wpi::StringRef name{raw_name.c_str()};
-        SDEBUG4("got event on '" << name << "' (" << name.size()
-                                 << ") compare to '" << base << "' ("
-                                 << base.size() << ") mask " << event.mask);
+        std::string_view name{raw_name.c_str()};
+        SDEBUG4("got event on '{}' ({}) compare to '{}' ({}) mask {}", name,
+                name.size(), base, base.size(), event.mask);
         if (name == base) {
           if ((event.mask & IN_DELETE) != 0) {
             wasStreaming = m_streaming;
@@ -441,7 +483,7 @@
 
     // Handle commands
     if (command_fd >= 0 && FD_ISSET(command_fd, &readfds)) {
-      SDEBUG4("got command");
+      SDEBUG4("{}", "got command");
       // Read it to clear
       eventfd_t val;
       eventfd_read(command_fd, &val);
@@ -451,7 +493,7 @@
 
     // Handle frames
     if (m_streaming && fd >= 0 && FD_ISSET(fd, &readfds)) {
-      SDEBUG4("grabbing image");
+      SDEBUG4("{}", "grabbing image");
 
       // Dequeue buffer
       struct v4l2_buffer buf;
@@ -459,7 +501,7 @@
       buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
       buf.memory = V4L2_MEMORY_MMAP;
       if (DoIoctl(fd, VIDIOC_DQBUF, &buf) != 0) {
-        SWARNING("could not dequeue buffer");
+        SWARNING("{}", "could not dequeue buffer");
         wasStreaming = m_streaming;
         DeviceStreamOff();
         DeviceDisconnect();
@@ -468,14 +510,14 @@
       }
 
       if ((buf.flags & V4L2_BUF_FLAG_ERROR) == 0) {
-        SDEBUG4("got image size=" << buf.bytesused << " index=" << buf.index);
+        SDEBUG4("got image size={} index={}", buf.bytesused, buf.index);
 
         if (buf.index >= kNumBuffers || !m_buffers[buf.index].m_data) {
-          SWARNING("invalid buffer" << buf.index);
+          SWARNING("invalid buffer {}", buf.index);
           continue;
         }
 
-        wpi::StringRef image{
+        std::string_view image{
             static_cast<const char*>(m_buffers[buf.index].m_data),
             static_cast<size_t>(buf.bytesused)};
         int width = m_mode.width;
@@ -483,7 +525,7 @@
         bool good = true;
         if (m_mode.pixelFormat == VideoMode::kMJPEG &&
             !GetJpegSize(image, &width, &height)) {
-          SWARNING("invalid JPEG image received from camera");
+          SWARNING("{}", "invalid JPEG image received from camera");
           good = false;
         }
         if (good) {
@@ -494,7 +536,7 @@
 
       // Requeue buffer
       if (DoIoctl(fd, VIDIOC_QBUF, &buf) != 0) {
-        SWARNING("could not requeue buffer");
+        SWARNING("{}", "could not requeue buffer");
         wasStreaming = m_streaming;
         DeviceStreamOff();
         DeviceDisconnect();
@@ -511,10 +553,14 @@
 
 void UsbCameraImpl::DeviceDisconnect() {
   int fd = m_fd.exchange(-1);
-  if (fd < 0) return;  // already disconnected
+  if (fd < 0) {
+    return;  // already disconnected
+  }
 
   // Unmap buffers
-  for (int i = 0; i < kNumBuffers; ++i) m_buffers[i] = UsbCameraBuffer{};
+  for (int i = 0; i < kNumBuffers; ++i) {
+    m_buffers[i] = UsbCameraBuffer{};
+  }
 
   // Close device
   close(fd);
@@ -524,67 +570,76 @@
 }
 
 void UsbCameraImpl::DeviceConnect() {
-  if (m_fd >= 0) return;
+  if (m_fd >= 0) {
+    return;
+  }
 
-  if (m_connectVerbose) SINFO("Connecting to USB camera on " << m_path);
+  if (m_connectVerbose) {
+    SINFO("Connecting to USB camera on {}", m_path);
+  }
 
   // Try to open the device
-  SDEBUG3("opening device");
+  SDEBUG3("{}", "opening device");
   int fd = open(m_path.c_str(), O_RDWR);
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
   m_fd = fd;
 
   // Get capabilities
-  SDEBUG3("getting capabilities");
+  SDEBUG3("{}", "getting capabilities");
   struct v4l2_capability vcap;
   std::memset(&vcap, 0, sizeof(vcap));
   if (DoIoctl(fd, VIDIOC_QUERYCAP, &vcap) >= 0) {
     m_capabilities = vcap.capabilities;
-    if (m_capabilities & V4L2_CAP_DEVICE_CAPS)
+    if (m_capabilities & V4L2_CAP_DEVICE_CAPS) {
       m_capabilities = vcap.device_caps;
+    }
   }
 
   // Get or restore video mode
   if (!m_properties_cached) {
-    SDEBUG3("caching properties");
+    SDEBUG3("{}", "caching properties");
     DeviceCacheProperties();
     DeviceCacheVideoModes();
     DeviceCacheMode();
     m_properties_cached = true;
   } else {
-    SDEBUG3("restoring video mode");
+    SDEBUG3("{}", "restoring video mode");
     DeviceSetMode();
     DeviceSetFPS();
 
     // Restore settings
-    SDEBUG3("restoring settings");
+    SDEBUG3("{}", "restoring settings");
     std::unique_lock lock2(m_mutex);
     for (size_t i = 0; i < m_propertyData.size(); ++i) {
       const auto prop =
           static_cast<const UsbCameraProperty*>(m_propertyData[i].get());
-      if (!prop || !prop->valueSet || !prop->device || prop->percentage)
+      if (!prop || !prop->valueSet || !prop->device || prop->percentage) {
         continue;
-      if (!prop->DeviceSet(lock2, m_fd))
-        SWARNING("failed to set property " << prop->name);
+      }
+      if (!prop->DeviceSet(lock2, m_fd)) {
+        SWARNING("failed to set property {}", prop->name);
+      }
     }
   }
 
   // Request buffers
-  SDEBUG3("allocating buffers");
+  SDEBUG3("{}", "allocating buffers");
   struct v4l2_requestbuffers rb;
   std::memset(&rb, 0, sizeof(rb));
   rb.count = kNumBuffers;
   rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   rb.memory = V4L2_MEMORY_MMAP;
   if (DoIoctl(fd, VIDIOC_REQBUFS, &rb) != 0) {
-    SWARNING("could not allocate buffers");
+    SWARNING("{}", "could not allocate buffers");
     close(fd);
     m_fd = -1;
     return;
   }
 
   // Map buffers
-  SDEBUG3("mapping buffers");
+  SDEBUG3("{}", "mapping buffers");
   for (int i = 0; i < kNumBuffers; ++i) {
     struct v4l2_buffer buf;
     std::memset(&buf, 0, sizeof(buf));
@@ -592,25 +647,26 @@
     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     buf.memory = V4L2_MEMORY_MMAP;
     if (DoIoctl(fd, VIDIOC_QUERYBUF, &buf) != 0) {
-      SWARNING("could not query buffer " << i);
+      SWARNING("could not query buffer {}", i);
       close(fd);
       m_fd = -1;
       return;
     }
-    SDEBUG4("buf " << i << " length=" << buf.length
-                   << " offset=" << buf.m.offset);
+    SDEBUG4("buf {} length={} offset={}", i, buf.length, buf.m.offset);
 
     m_buffers[i] = UsbCameraBuffer(fd, buf.length, buf.m.offset);
     if (!m_buffers[i].m_data) {
-      SWARNING("could not map buffer " << i);
+      SWARNING("could not map buffer {}", i);
       // release other buffers
-      for (int j = 0; j < i; ++j) m_buffers[j] = UsbCameraBuffer{};
+      for (int j = 0; j < i; ++j) {
+        m_buffers[j] = UsbCameraBuffer{};
+      }
       close(fd);
       m_fd = -1;
       return;
     }
 
-    SDEBUG4("buf " << i << " address=" << m_buffers[i].m_data);
+    SDEBUG4("buf {} address={}", i, m_buffers[i].m_data);
   }
 
   // Update description (as it may have changed)
@@ -624,12 +680,16 @@
 }
 
 bool UsbCameraImpl::DeviceStreamOn() {
-  if (m_streaming) return false;  // ignore if already enabled
+  if (m_streaming) {
+    return false;  // ignore if already enabled
+  }
   int fd = m_fd.load();
-  if (fd < 0) return false;
+  if (fd < 0) {
+    return false;
+  }
 
   // Queue buffers
-  SDEBUG3("queuing buffers");
+  SDEBUG3("{}", "queuing buffers");
   for (int i = 0; i < kNumBuffers; ++i) {
     struct v4l2_buffer buf;
     std::memset(&buf, 0, sizeof(buf));
@@ -637,7 +697,7 @@
     buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
     buf.memory = V4L2_MEMORY_MMAP;
     if (DoIoctl(fd, VIDIOC_QBUF, &buf) != 0) {
-      SWARNING("could not queue buffer " << i);
+      SWARNING("could not queue buffer {}", i);
       return false;
     }
   }
@@ -648,27 +708,34 @@
     if (errno == ENOSPC) {
       // indicates too much USB bandwidth requested
       SERROR(
+          "{}",
           "could not start streaming due to USB bandwidth limitations; try a "
           "lower resolution or a different pixel format (VIDIOC_STREAMON: "
           "No space left on device)");
     } else {
       // some other error
-      SERROR("ioctl VIDIOC_STREAMON failed: " << std::strerror(errno));
+      SERROR("ioctl VIDIOC_STREAMON failed: {}", std::strerror(errno));
     }
     return false;
   }
-  SDEBUG4("enabled streaming");
+  SDEBUG4("{}", "enabled streaming");
   m_streaming = true;
   return true;
 }
 
 bool UsbCameraImpl::DeviceStreamOff() {
-  if (!m_streaming) return false;  // ignore if already disabled
+  if (!m_streaming) {
+    return false;  // ignore if already disabled
+  }
   int fd = m_fd.load();
-  if (fd < 0) return false;
+  if (fd < 0) {
+    return false;
+  }
   int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  if (DoIoctl(fd, VIDIOC_STREAMOFF, &type) != 0) return false;
-  SDEBUG4("disabled streaming");
+  if (DoIoctl(fd, VIDIOC_STREAMOFF, &type) != 0) {
+    return false;
+  }
+  SDEBUG4("{}", "disabled streaming");
   m_streaming = false;
   return true;
 }
@@ -706,12 +773,16 @@
     m_mode = newMode;
     lock.unlock();
     bool wasStreaming = m_streaming;
-    if (wasStreaming) DeviceStreamOff();
+    if (wasStreaming) {
+      DeviceStreamOff();
+    }
     if (m_fd >= 0) {
       DeviceDisconnect();
       DeviceConnect();
     }
-    if (wasStreaming) DeviceStreamOn();
+    if (wasStreaming) {
+      DeviceStreamOn();
+    }
     m_notifier.NotifySourceVideoMode(*this, newMode);
     lock.lock();
   } else if (newMode.fps != m_mode.fps) {
@@ -719,9 +790,13 @@
     lock.unlock();
     // Need to stop streaming to set FPS
     bool wasStreaming = m_streaming;
-    if (wasStreaming) DeviceStreamOff();
+    if (wasStreaming) {
+      DeviceStreamOff();
+    }
     DeviceSetFPS();
-    if (wasStreaming) DeviceStreamOn();
+    if (wasStreaming) {
+      DeviceStreamOn();
+    }
     m_notifier.NotifySourceVideoMode(*this, newMode);
     lock.lock();
   }
@@ -734,25 +809,29 @@
   bool setString = (msg.kind == Message::kCmdSetPropertyStr);
   int property = msg.data[0];
   int value = msg.data[1];
-  wpi::StringRef valueStr = msg.dataStr;
+  std::string_view valueStr = msg.dataStr;
 
   // Look up
   auto prop = static_cast<UsbCameraProperty*>(GetProperty(property));
-  if (!prop) return CS_INVALID_PROPERTY;
+  if (!prop) {
+    return CS_INVALID_PROPERTY;
+  }
 
   // If setting before we get, guess initial type based on set
   if (prop->propKind == CS_PROP_NONE) {
-    if (setString)
+    if (setString) {
       prop->propKind = CS_PROP_STRING;
-    else
+    } else {
       prop->propKind = CS_PROP_INTEGER;
+    }
   }
 
   // Check kind match
   if ((setString && prop->propKind != CS_PROP_STRING) ||
-      (!setString && (prop->propKind &
-                      (CS_PROP_BOOLEAN | CS_PROP_INTEGER | CS_PROP_ENUM)) == 0))
+      (!setString && (prop->propKind & (CS_PROP_BOOLEAN | CS_PROP_INTEGER |
+                                        CS_PROP_ENUM)) == 0)) {
     return CS_WRONG_PROPERTY_TYPE;
+  }
 
   // Handle percentage property
   int percentageProperty = prop->propPair;
@@ -769,17 +848,21 @@
 
   // Actually set the new value on the device (if possible)
   if (!prop->device) {
-    if (prop->id == kPropConnectVerboseId) m_connectVerbose = value;
+    if (prop->id == kPropConnectVerboseId) {
+      m_connectVerbose = value;
+    }
   } else {
-    if (!prop->DeviceSet(lock, m_fd, value, valueStr))
+    if (!prop->DeviceSet(lock, m_fd, value, valueStr)) {
       return CS_PROPERTY_WRITE_FAILED;
+    }
   }
 
   // Cache the set values
   UpdatePropertyValue(property, setString, value, valueStr);
-  if (percentageProperty != 0)
+  if (percentageProperty != 0) {
     UpdatePropertyValue(percentageProperty, setString, percentageValue,
                         valueStr);
+  }
 
   return CS_OK;
 }
@@ -790,12 +873,16 @@
   lock.unlock();
   // disconnect and reconnect
   bool wasStreaming = m_streaming;
-  if (wasStreaming) DeviceStreamOff();
+  if (wasStreaming) {
+    DeviceStreamOff();
+  }
   if (m_fd >= 0) {
     DeviceDisconnect();
     DeviceConnect();
   }
-  if (wasStreaming) DeviceStreamOn();
+  if (wasStreaming) {
+    DeviceStreamOn();
+  }
   lock.lock();
   return CS_OK;
 }
@@ -822,15 +909,18 @@
 
 void UsbCameraImpl::DeviceProcessCommands() {
   std::unique_lock lock(m_mutex);
-  if (m_commands.empty()) return;
+  if (m_commands.empty()) {
+    return;
+  }
   while (!m_commands.empty()) {
     auto msg = std::move(m_commands.back());
     m_commands.pop_back();
 
     CS_StatusValue status = DeviceProcessCommand(lock, msg);
     if (msg.kind != Message::kNumSinksChanged &&
-        msg.kind != Message::kNumSinksEnabledChanged)
+        msg.kind != Message::kNumSinksEnabledChanged) {
       m_responses.emplace_back(msg.from, status);
+    }
   }
   lock.unlock();
   m_responseCv.notify_all();
@@ -838,7 +928,9 @@
 
 void UsbCameraImpl::DeviceSetMode() {
   int fd = m_fd.load();
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
 
   struct v4l2_format vfmt;
   std::memset(&vfmt, 0, sizeof(vfmt));
@@ -851,43 +943,52 @@
   vfmt.fmt.pix.pixelformat =
       FromPixelFormat(static_cast<VideoMode::PixelFormat>(m_mode.pixelFormat));
   if (vfmt.fmt.pix.pixelformat == 0) {
-    SWARNING("could not set format " << m_mode.pixelFormat
-                                     << ", defaulting to MJPEG");
+    SWARNING("could not set format {}, defaulting to MJPEG",
+             m_mode.pixelFormat);
     vfmt.fmt.pix.pixelformat = V4L2_PIX_FMT_MJPEG;
   }
   vfmt.fmt.pix.width = m_mode.width;
   vfmt.fmt.pix.height = m_mode.height;
   vfmt.fmt.pix.field = V4L2_FIELD_ANY;
   if (DoIoctl(fd, VIDIOC_S_FMT, &vfmt) != 0) {
-    SWARNING("could not set format " << m_mode.pixelFormat << " res "
-                                     << m_mode.width << "x" << m_mode.height);
+    SWARNING("could not set format {} res {}x{}", m_mode.pixelFormat,
+             m_mode.width, m_mode.height);
   } else {
-    SINFO("set format " << m_mode.pixelFormat << " res " << m_mode.width << "x"
-                        << m_mode.height);
+    SINFO("set format {} res {}x{}", m_mode.pixelFormat, m_mode.width,
+          m_mode.height);
   }
 }
 
 void UsbCameraImpl::DeviceSetFPS() {
   int fd = m_fd.load();
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
 
   struct v4l2_streamparm parm;
   std::memset(&parm, 0, sizeof(parm));
   parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
-  if (DoIoctl(fd, VIDIOC_G_PARM, &parm) != 0) return;
-  if ((parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) == 0) return;
+  if (DoIoctl(fd, VIDIOC_G_PARM, &parm) != 0) {
+    return;
+  }
+  if ((parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) == 0) {
+    return;
+  }
   std::memset(&parm, 0, sizeof(parm));
   parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   parm.parm.capture.timeperframe = FPSToFract(m_mode.fps);
-  if (DoIoctl(fd, VIDIOC_S_PARM, &parm) != 0)
-    SWARNING("could not set FPS to " << m_mode.fps);
-  else
-    SINFO("set FPS to " << m_mode.fps);
+  if (DoIoctl(fd, VIDIOC_S_PARM, &parm) != 0) {
+    SWARNING("could not set FPS to {}", m_mode.fps);
+  } else {
+    SINFO("set FPS to {}", m_mode.fps);
+  }
 }
 
 void UsbCameraImpl::DeviceCacheMode() {
   int fd = m_fd.load();
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
 
   // Get format
   struct v4l2_format vfmt;
@@ -899,7 +1000,7 @@
 #endif
   vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   if (DoIoctl(fd, VIDIOC_G_FMT, &vfmt) != 0) {
-    SERROR("could not read current video mode");
+    SERROR("{}", "could not read current video mode");
     std::scoped_lock lock(m_mutex);
     m_mode = VideoMode{VideoMode::kMJPEG, 320, 240, 30};
     return;
@@ -914,8 +1015,9 @@
   std::memset(&parm, 0, sizeof(parm));
   parm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   if (TryIoctl(fd, VIDIOC_G_PARM, &parm) == 0) {
-    if (parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME)
+    if (parm.parm.capture.capability & V4L2_CAP_TIMEPERFRAME) {
       fps = FractToFPS(parm.parm.capture.timeperframe);
+    }
   }
 
   // Update format with user changes.
@@ -946,7 +1048,9 @@
     // Default to lowest known resolution (based on number of total pixels)
     int numPixels = width * height;
     for (const auto& mode : m_videoModes) {
-      if (mode.pixelFormat != pixelFormat) continue;
+      if (mode.pixelFormat != pixelFormat) {
+        continue;
+      }
       int numPixelsHere = mode.width * mode.height;
       if (numPixelsHere < numPixels) {
         formatChanged = true;
@@ -973,8 +1077,12 @@
     m_mode.fps = fps;
   }
 
-  if (formatChanged) DeviceSetMode();
-  if (fpsChanged) DeviceSetFPS();
+  if (formatChanged) {
+    DeviceSetMode();
+  }
+  if (fpsChanged) {
+    DeviceSetFPS();
+  }
 
   m_notifier.NotifySourceVideoMode(*this, m_mode);
 }
@@ -1027,8 +1135,9 @@
     rawProp->valueStr = perProp->valueStr;  // copy
   } else {
     // Read current raw value and set percentage from it
-    if (!rawProp->DeviceGet(lock, m_fd))
-      SWARNING("failed to get property " << rawProp->name);
+    if (!rawProp->DeviceGet(lock, m_fd)) {
+      SWARNING("failed to get property {}", rawProp->name);
+    }
 
     if (perProp) {
       perProp->SetValue(RawToPercentage(*rawProp, rawProp->value));
@@ -1038,8 +1147,9 @@
 
   // Set value on device if user-configured
   if (rawProp->valueSet) {
-    if (!rawProp->DeviceSet(lock, m_fd))
-      SWARNING("failed to set property " << rawProp->name);
+    if (!rawProp->DeviceSet(lock, m_fd)) {
+      SWARNING("failed to set property {}", rawProp->name);
+    }
   }
 
   // Update pointers since we released the lock
@@ -1079,12 +1189,16 @@
   }
 
   NotifyPropertyCreated(*rawIndex, *rawPropPtr);
-  if (perPropPtr) NotifyPropertyCreated(*perIndex, *perPropPtr);
+  if (perPropPtr) {
+    NotifyPropertyCreated(*perIndex, *perPropPtr);
+  }
 }
 
 void UsbCameraImpl::DeviceCacheProperties() {
   int fd = m_fd.load();
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
 
 #ifdef V4L2_CTRL_FLAG_NEXT_COMPOUND
   constexpr __u32 nextFlags =
@@ -1102,20 +1216,24 @@
   if (id == nextFlags) {
     // try just enumerating standard...
     for (id = V4L2_CID_BASE; id < V4L2_CID_LASTP1; ++id) {
-      if (auto prop = UsbCameraProperty::DeviceQuery(fd, &id))
+      if (auto prop = UsbCameraProperty::DeviceQuery(fd, &id)) {
         DeviceCacheProperty(std::move(prop));
+      }
     }
     // ... and custom controls
     std::unique_ptr<UsbCameraProperty> prop;
     for (id = V4L2_CID_PRIVATE_BASE;
-         (prop = UsbCameraProperty::DeviceQuery(fd, &id)); ++id)
+         (prop = UsbCameraProperty::DeviceQuery(fd, &id)); ++id) {
       DeviceCacheProperty(std::move(prop));
+    }
   }
 }
 
 void UsbCameraImpl::DeviceCacheVideoModes() {
   int fd = m_fd.load();
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
 
   std::vector<VideoMode> modes;
 
@@ -1125,7 +1243,9 @@
   fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
   for (fmt.index = 0; TryIoctl(fd, VIDIOC_ENUM_FMT, &fmt) >= 0; ++fmt.index) {
     VideoMode::PixelFormat pixelFormat = ToPixelFormat(fmt.pixelformat);
-    if (pixelFormat == VideoMode::kUnknown) continue;
+    if (pixelFormat == VideoMode::kUnknown) {
+      continue;
+    }
 
     // Frame sizes
     struct v4l2_frmsizeenum frmsize;
@@ -1133,7 +1253,9 @@
     frmsize.pixel_format = fmt.pixelformat;
     for (frmsize.index = 0; TryIoctl(fd, VIDIOC_ENUM_FRAMESIZES, &frmsize) >= 0;
          ++frmsize.index) {
-      if (frmsize.type != V4L2_FRMSIZE_TYPE_DISCRETE) continue;
+      if (frmsize.type != V4L2_FRMSIZE_TYPE_DISCRETE) {
+        continue;
+      }
 
       // Frame intervals
       struct v4l2_frmivalenum frmival;
@@ -1144,7 +1266,9 @@
       for (frmival.index = 0;
            TryIoctl(fd, VIDIOC_ENUM_FRAMEINTERVALS, &frmival) >= 0;
            ++frmival.index) {
-        if (frmival.type != V4L2_FRMIVAL_TYPE_DISCRETE) continue;
+        if (frmival.type != V4L2_FRMIVAL_TYPE_DISCRETE) {
+          continue;
+        }
 
         modes.emplace_back(pixelFormat,
                            static_cast<int>(frmsize.discrete.width),
@@ -1183,7 +1307,9 @@
 CS_StatusValue UsbCameraImpl::SendAndWait(Message&& msg) const {
   int fd = m_command_fd.load();
   // exit early if not possible to signal
-  if (fd < 0) return CS_SOURCE_IS_DISCONNECTED;
+  if (fd < 0) {
+    return CS_SOURCE_IS_DISCONNECTED;
+  }
 
   auto from = msg.from;
 
@@ -1194,7 +1320,9 @@
   }
 
   // Signal the camera thread
-  if (eventfd_write(fd, 1) < 0) return CS_SOURCE_IS_DISCONNECTED;
+  if (eventfd_write(fd, 1) < 0) {
+    return CS_SOURCE_IS_DISCONNECTED;
+  }
 
   std::unique_lock lock(m_mutex);
   while (m_active) {
@@ -1220,7 +1348,9 @@
 void UsbCameraImpl::Send(Message&& msg) const {
   int fd = m_command_fd.load();
   // exit early if not possible to signal
-  if (fd < 0) return;
+  if (fd < 0) {
+    return;
+  }
 
   // Add the message to the command queue
   {
@@ -1233,14 +1363,16 @@
 }
 
 std::unique_ptr<PropertyImpl> UsbCameraImpl::CreateEmptyProperty(
-    const wpi::Twine& name) const {
+    std::string_view name) const {
   return std::make_unique<UsbCameraProperty>(name);
 }
 
 bool UsbCameraImpl::CacheProperties(CS_Status* status) const {
   // Wake up camera thread; this will try to reconnect
   *status = SendAndWait(Message{Message::kNone});
-  if (*status != CS_OK) return false;
+  if (*status != CS_OK) {
+    return false;
+  }
   if (!m_properties_cached) {
     *status = CS_SOURCE_IS_DISCONNECTED;
     return false;
@@ -1250,10 +1382,10 @@
 
 void UsbCameraImpl::SetQuirks() {
   wpi::SmallString<128> descbuf;
-  wpi::StringRef desc = GetDescription(descbuf);
-  m_lifecam_exposure =
-      desc.endswith("LifeCam HD-3000") || desc.endswith("LifeCam Cinema (TM)");
-  m_picamera = desc.startswith("mmal service");
+  std::string_view desc = GetDescription(descbuf);
+  m_lifecam_exposure = wpi::ends_with(desc, "LifeCam HD-3000") ||
+                       wpi::ends_with(desc, "LifeCam Cinema (TM)");
+  m_picamera = wpi::ends_with(desc, "mmal service");
 
   int deviceNum = GetDeviceNum(m_path.c_str());
   if (deviceNum >= 0) {
@@ -1271,11 +1403,11 @@
   *status = SendAndWait(std::move(msg));
 }
 
-void UsbCameraImpl::SetStringProperty(int property, const wpi::Twine& value,
+void UsbCameraImpl::SetStringProperty(int property, std::string_view value,
                                       CS_Status* status) {
   Message msg{Message::kCmdSetPropertyStr};
   msg.data[0] = property;
-  msg.dataStr = value.str();
+  msg.dataStr = value;
   *status = SendAndWait(std::move(msg));
 }
 
@@ -1395,9 +1527,9 @@
   Send(Message{Message::kNumSinksEnabledChanged});
 }
 
-void UsbCameraImpl::SetPath(const wpi::Twine& path, CS_Status* status) {
+void UsbCameraImpl::SetPath(std::string_view path, CS_Status* status) {
   Message msg{Message::kCmdSetPath};
-  msg.dataStr = path.str();
+  msg.dataStr = path;
   *status = SendAndWait(std::move(msg));
 }
 
@@ -1408,15 +1540,12 @@
 
 namespace cs {
 
-CS_Source CreateUsbCameraDev(const wpi::Twine& name, int dev,
+CS_Source CreateUsbCameraDev(std::string_view name, int dev,
                              CS_Status* status) {
-  wpi::SmallString<32> path;
-  wpi::raw_svector_ostream oss{path};
-  oss << "/dev/video" << dev;
-  return CreateUsbCameraPath(name, oss.str(), status);
+  return CreateUsbCameraPath(name, fmt::format("/dev/video{}", dev), status);
 }
 
-CS_Source CreateUsbCameraPath(const wpi::Twine& name, const wpi::Twine& path,
+CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
                               CS_Status* status) {
   auto& inst = Instance::GetInstance();
   return inst.CreateSource(CS_SOURCE_USB, std::make_shared<UsbCameraImpl>(
@@ -1424,7 +1553,7 @@
                                               inst.telemetry, path));
 }
 
-void SetUsbCameraPath(CS_Source source, const wpi::Twine& path,
+void SetUsbCameraPath(CS_Source source, std::string_view path,
                       CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || data->kind != CS_SOURCE_USB) {
@@ -1476,7 +1605,9 @@
           path += ep->d_name;
           char* target = ::realpath(path.c_str(), nullptr);
           if (target) {
-            if (keypath == target) info.otherPaths.emplace_back(path.str());
+            if (keypath == target) {
+              info.otherPaths.emplace_back(path.str());
+            }
             std::free(target);
           }
         }
@@ -1498,11 +1629,17 @@
 
   if (DIR* dp = ::opendir("/dev")) {
     while (struct dirent* ep = ::readdir(dp)) {
-      wpi::StringRef fname{ep->d_name};
-      if (!fname.startswith("video")) continue;
+      std::string_view fname{ep->d_name};
+      if (!wpi::starts_with(fname, "video")) {
+        continue;
+      }
 
       unsigned int dev = 0;
-      if (fname.substr(5).getAsInteger(10, dev)) continue;
+      if (auto v = wpi::parse_integer<unsigned int>(fname.substr(5), 10)) {
+        dev = v.value();
+      } else {
+        continue;
+      }
 
       UsbCameraInfo info;
       info.dev = dev;
@@ -1511,20 +1648,26 @@
       path += fname;
       info.path = path.str();
 
-      if (!IsVideoCaptureDevice(path.c_str())) continue;
+      if (!IsVideoCaptureDevice(path.c_str())) {
+        continue;
+      }
 
       info.name = GetDescriptionImpl(path.c_str());
-      if (info.name.empty()) continue;
+      if (info.name.empty()) {
+        continue;
+      }
 
       GetVendorProduct(dev, &info.vendorId, &info.productId);
 
-      if (dev >= retval.size()) retval.resize(info.dev + 1);
+      if (dev >= retval.size()) {
+        retval.resize(info.dev + 1);
+      }
       retval[info.dev] = std::move(info);
     }
     ::closedir(dp);
   } else {
     // *status = ;
-    WPI_ERROR(Instance::GetInstance().logger, "Could not open /dev");
+    WPI_ERROR(Instance::GetInstance().logger, "{}", "Could not open /dev");
     return retval;
   }
 
@@ -1540,11 +1683,12 @@
           path += ep->d_name;
           char* target = ::realpath(path.c_str(), nullptr);
           if (target) {
-            wpi::StringRef fname = wpi::sys::path::filename(target);
-            unsigned int dev = 0;
-            if (fname.startswith("video") &&
-                !fname.substr(5).getAsInteger(10, dev) && dev < retval.size()) {
-              retval[dev].otherPaths.emplace_back(path.str());
+            std::string fname = fs::path{target}.filename();
+            std::optional<unsigned int> dev;
+            if (wpi::starts_with(fname, "video") &&
+                (dev = wpi::parse_integer<unsigned int>(fname.substr(5), 10)) &&
+                dev.value() < retval.size()) {
+              retval[dev.value()].otherPaths.emplace_back(path.str());
             }
             std::free(target);
           }
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.h b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.h
index 62b94a0..a032466 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_USBCAMERAIMPL_H_
 #define CSCORE_USBCAMERAIMPL_H_
@@ -13,13 +10,12 @@
 #include <atomic>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <utility>
 #include <vector>
 
-#include <wpi/STLExtras.h>
 #include <wpi/SmallVector.h>
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 #include <wpi/raw_istream.h>
@@ -36,15 +32,15 @@
 
 class UsbCameraImpl : public SourceImpl {
  public:
-  UsbCameraImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
-                Telemetry& telemetry, const wpi::Twine& path);
+  UsbCameraImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
+                Telemetry& telemetry, std::string_view path);
   ~UsbCameraImpl() override;
 
   void Start() override;
 
   // Property functions
   void SetProperty(int property, int value, CS_Status* status) override;
-  void SetStringProperty(int property, const wpi::Twine& value,
+  void SetStringProperty(int property, std::string_view value,
                          CS_Status* status) override;
 
   // Standard common camera properties
@@ -66,7 +62,7 @@
   void NumSinksChanged() override;
   void NumSinksEnabledChanged() override;
 
-  void SetPath(const wpi::Twine& path, CS_Status* status);
+  void SetPath(std::string_view path, CS_Status* status);
   std::string GetPath() const;
 
   // Messages passed to/from camera thread
@@ -98,7 +94,7 @@
 
  protected:
   std::unique_ptr<PropertyImpl> CreateEmptyProperty(
-      const wpi::Twine& name) const override;
+      std::string_view name) const override;
 
   // Cache properties.  Immediately successful if properties are already cached.
   // If they are not, tries to connect to the camera to do so; returns false and
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraListener.cpp b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraListener.cpp
new file mode 100644
index 0000000..40e84c5
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraListener.cpp
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "UsbCameraListener.h"
+
+#include <wpi/EventLoopRunner.h>
+#include <wpi/StringExtras.h>
+#include <wpi/uv/FsEvent.h>
+#include <wpi/uv/Timer.h>
+
+#include "Notifier.h"
+
+using namespace cs;
+
+class UsbCameraListener::Impl {
+ public:
+  explicit Impl(Notifier& notifier) : m_notifier(notifier) {}
+
+  Notifier& m_notifier;
+
+  std::unique_ptr<wpi::EventLoopRunner> m_runner;
+};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier)
+    : m_impl(std::make_unique<Impl>(notifier)) {}
+
+UsbCameraListener::~UsbCameraListener() = default;
+
+void UsbCameraListener::Start() {
+  if (!m_impl->m_runner) {
+    m_impl->m_runner = std::make_unique<wpi::EventLoopRunner>();
+    m_impl->m_runner->ExecAsync([impl = m_impl.get()](wpi::uv::Loop& loop) {
+      auto refreshTimer = wpi::uv::Timer::Create(loop);
+      refreshTimer->timeout.connect([notifier = &impl->m_notifier] {
+        notifier->NotifyUsbCamerasChanged();
+      });
+      refreshTimer->Unreference();
+
+      auto devEvents = wpi::uv::FsEvent::Create(loop);
+      devEvents->fsEvent.connect([refreshTimer](const char* fn, int flags) {
+        if (wpi::starts_with(fn, "video")) {
+          refreshTimer->Start(wpi::uv::Timer::Time(200));
+        }
+      });
+      devEvents->Start("/dev");
+      devEvents->Unreference();
+    });
+  }
+}
+
+void UsbCameraListener::Stop() {
+  if (m_impl->m_runner) {
+    m_impl->m_runner.reset();
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.cpp b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.cpp
index 4dfa39c..149ca1b 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "UsbCameraProperty.h"
 
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
 
 #include "UsbUtil.h"
 
@@ -29,7 +26,9 @@
     ctrls.count = 1;
     ctrls.controls = &ctrl;
     int rc = DoIoctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls);
-    if (rc < 0) return rc;
+    if (rc < 0) {
+      return rc;
+    }
     *value = ctrl.value;
   } else {
     // Use normal control
@@ -37,7 +36,9 @@
     std::memset(&ctrl, 0, sizeof(ctrl));
     ctrl.id = id;
     int rc = DoIoctl(fd, VIDIOC_G_CTRL, &ctrl);
-    if (rc < 0) return rc;
+    if (rc < 0) {
+      return rc;
+    }
     *value = ctrl.value;
   }
   return 0;
@@ -54,10 +55,11 @@
     std::memset(&ctrl, 0, sizeof(ctrl));
     std::memset(&ctrls, 0, sizeof(ctrls));
     ctrl.id = id;
-    if (type == V4L2_CTRL_TYPE_INTEGER64)
+    if (type == V4L2_CTRL_TYPE_INTEGER64) {
       ctrl.value64 = value;
-    else
+    } else {
       ctrl.value = static_cast<__s32>(value);
+    }
     ctrls.ctrl_class = ctrl_class;
     ctrls.count = 1;
     ctrls.controls = &ctrl;
@@ -90,15 +92,8 @@
 }
 
 static int SetStringCtrlIoctl(int fd, int id, int maximum,
-                              const wpi::Twine& value) {
-  wpi::SmallString<64> strBuf, strBuf2;
-  wpi::StringRef str = value.toNullTerminatedStringRef(strBuf);
-  if (str.size() > static_cast<size_t>(maximum)) {
-    // don't know if strBuf was used, just recopy
-    strBuf2 = str.take_front(maximum);
-    str = strBuf2;
-    strBuf2.push_back('\0');  // null terminate
-  }
+                              std::string_view value) {
+  wpi::SmallString<64> str{value.substr(0, maximum)};
 
   struct v4l2_ext_control ctrl;
   struct v4l2_ext_controls ctrls;
@@ -106,7 +101,7 @@
   std::memset(&ctrls, 0, sizeof(ctrls));
   ctrl.id = id;
   ctrl.size = str.size();
-  ctrl.string = const_cast<char*>(str.data());
+  ctrl.string = const_cast<char*>(str.c_str());
   ctrls.ctrl_class = V4L2_CTRL_ID2CLASS(id);
   ctrls.count = 1;
   ctrls.controls = &ctrl;
@@ -115,25 +110,26 @@
 
 // Removes non-alphanumeric characters and replaces spaces with underscores.
 // e.g. "Zoom, Absolute" -> "zoom_absolute", "Pan (Absolute)" -> "pan_absolute"
-static wpi::StringRef NormalizeName(wpi::StringRef name,
-                                    wpi::SmallVectorImpl<char>& buf) {
+static std::string_view NormalizeName(std::string_view name,
+                                      wpi::SmallVectorImpl<char>& buf) {
   bool newWord = false;
   for (auto ch : name) {
     if (std::isalnum(ch)) {
-      if (newWord) buf.push_back('_');
+      if (newWord) {
+        buf.push_back('_');
+      }
       newWord = false;
       buf.push_back(std::tolower(ch));
     } else if (!buf.empty()) {
       newWord = true;
     }
   }
-  return wpi::StringRef(buf.data(), buf.size());
+  return {buf.data(), buf.size()};
 }
 
 #ifdef VIDIOC_QUERY_EXT_CTRL
 UsbCameraProperty::UsbCameraProperty(const struct v4l2_query_ext_ctrl& ctrl)
-    : PropertyImpl(wpi::StringRef{}, CS_PROP_NONE, ctrl.step,
-                   ctrl.default_value, 0),
+    : PropertyImpl({}, CS_PROP_NONE, ctrl.step, ctrl.default_value, 0),
       id(ctrl.id & V4L2_CTRL_ID_MASK),
       type(ctrl.type) {
   hasMinimum = true;
@@ -166,15 +162,16 @@
 
   // name
   size_t len = 0;
-  while (len < sizeof(ctrl.name) && ctrl.name[len] != '\0') ++len;
+  while (len < sizeof(ctrl.name) && ctrl.name[len] != '\0') {
+    ++len;
+  }
   wpi::SmallString<64> name_buf;
-  name = NormalizeName(wpi::StringRef(ctrl.name, len), name_buf);
+  name = NormalizeName({ctrl.name, len}, name_buf);
 }
 #endif
 
 UsbCameraProperty::UsbCameraProperty(const struct v4l2_queryctrl& ctrl)
-    : PropertyImpl(wpi::StringRef{}, CS_PROP_NONE, ctrl.step,
-                   ctrl.default_value, 0),
+    : PropertyImpl({}, CS_PROP_NONE, ctrl.step, ctrl.default_value, 0),
       id(ctrl.id & V4L2_CTRL_ID_MASK),
       type(ctrl.type) {
   hasMinimum = true;
@@ -204,10 +201,12 @@
 
   // name
   size_t len = 0;
-  while (len < sizeof(ctrl.name) && ctrl.name[len] != '\0') ++len;
+  while (len < sizeof(ctrl.name) && ctrl.name[len] != '\0') {
+    ++len;
+  }
   wpi::SmallString<64> name_buf;
-  name = NormalizeName(
-      wpi::StringRef(reinterpret_cast<const char*>(ctrl.name), len), name_buf);
+  name =
+      NormalizeName({reinterpret_cast<const char*>(ctrl.name), len}, name_buf);
 }
 
 std::unique_ptr<UsbCameraProperty> UsbCameraProperty::DeviceQuery(int fd,
@@ -222,7 +221,9 @@
   if (rc == 0) {
     *id = qc_ext.id;  // copy back
     // We don't support array types
-    if (qc_ext.elems > 1 || qc_ext.nr_of_dims > 0) return nullptr;
+    if (qc_ext.elems > 1 || qc_ext.nr_of_dims > 0) {
+      return nullptr;
+    }
     prop = std::make_unique<UsbCameraProperty>(qc_ext);
   }
 #endif
@@ -233,7 +234,9 @@
     qc.id = *id;
     rc = TryIoctl(fd, VIDIOC_QUERYCTRL, &qc);
     *id = qc.id;  // copy back
-    if (rc != 0) return nullptr;
+    if (rc != 0) {
+      return nullptr;
+    }
     prop = std::make_unique<UsbCameraProperty>(qc);
   }
 
@@ -245,10 +248,11 @@
     qmenu.id = *id;
     for (int i = prop->minimum; i <= prop->maximum; ++i) {
       qmenu.index = static_cast<__u32>(i);
-      if (TryIoctl(fd, VIDIOC_QUERYMENU, &qmenu) != 0) continue;
+      if (TryIoctl(fd, VIDIOC_QUERYMENU, &qmenu) != 0) {
+        continue;
+      }
       if (prop->intMenu) {
-        wpi::raw_string_ostream os(prop->enumChoices[i]);
-        os << qmenu.value;
+        prop->enumChoices[i] = fmt::to_string(qmenu.value);
       } else {
         prop->enumChoices[i] = reinterpret_cast<const char*>(qmenu.name);
       }
@@ -259,7 +263,9 @@
 }
 
 bool UsbCameraProperty::DeviceGet(std::unique_lock<wpi::mutex>& lock, int fd) {
-  if (fd < 0) return true;
+  if (fd < 0) {
+    return true;
+  }
   unsigned idCopy = id;
   int rv = 0;
 
@@ -272,7 +278,9 @@
       lock.unlock();
       rv = GetIntCtrlIoctl(fd, idCopy, typeCopy, &newValue);
       lock.lock();
-      if (rv >= 0) value = newValue;
+      if (rv >= 0) {
+        value = newValue;
+      }
       break;
     }
     case CS_PROP_STRING: {
@@ -281,7 +289,9 @@
       lock.unlock();
       rv = GetStringCtrlIoctl(fd, idCopy, maximumCopy, &newValueStr);
       lock.lock();
-      if (rv >= 0) valueStr = std::move(newValueStr);
+      if (rv >= 0) {
+        valueStr = std::move(newValueStr);
+      }
       break;
     }
     default:
@@ -295,13 +305,15 @@
                                   int fd) const {
   // Make a copy of the string as we're about to release the lock
   wpi::SmallString<128> valueStrCopy{valueStr};
-  return DeviceSet(lock, fd, value, valueStrCopy);
+  return DeviceSet(lock, fd, value, valueStrCopy.str());
 }
 
 bool UsbCameraProperty::DeviceSet(std::unique_lock<wpi::mutex>& lock, int fd,
                                   int newValue,
-                                  const wpi::Twine& newValueStr) const {
-  if (!device || fd < 0) return true;
+                                  std::string_view newValueStr) const {
+  if (!device || fd < 0) {
+    return true;
+  }
   unsigned idCopy = id;
   int rv = 0;
 
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.h b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.h
index 32c5e19..b0291a1 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.h
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbCameraProperty.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_USBCAMERAPROPERTY_H_
 #define CSCORE_USBCAMERAPROPERTY_H_
@@ -11,6 +8,7 @@
 #include <linux/videodev2.h>
 
 #include <memory>
+#include <string_view>
 
 #include <wpi/mutex.h>
 
@@ -22,19 +20,19 @@
 class UsbCameraProperty : public PropertyImpl {
  public:
   UsbCameraProperty() = default;
-  explicit UsbCameraProperty(const wpi::Twine& name_) : PropertyImpl{name_} {}
+  explicit UsbCameraProperty(std::string_view name_) : PropertyImpl{name_} {}
 
   // Software property constructor
-  UsbCameraProperty(const wpi::Twine& name_, unsigned id_,
-                    CS_PropertyKind kind_, int minimum_, int maximum_,
-                    int step_, int defaultValue_, int value_)
+  UsbCameraProperty(std::string_view name_, unsigned id_, CS_PropertyKind kind_,
+                    int minimum_, int maximum_, int step_, int defaultValue_,
+                    int value_)
       : PropertyImpl(name_, kind_, minimum_, maximum_, step_, defaultValue_,
                      value_),
         device{false},
         id{id_} {}
 
   // Normalized property constructor
-  UsbCameraProperty(const wpi::Twine& name_, int rawIndex_,
+  UsbCameraProperty(std::string_view name_, int rawIndex_,
                     const UsbCameraProperty& rawProp, int defaultValue_,
                     int value_)
       : PropertyImpl(name_, rawProp.propKind, 1, defaultValue_, value_),
@@ -58,7 +56,7 @@
   bool DeviceGet(std::unique_lock<wpi::mutex>& lock, int fd);
   bool DeviceSet(std::unique_lock<wpi::mutex>& lock, int fd) const;
   bool DeviceSet(std::unique_lock<wpi::mutex>& lock, int fd, int newValue,
-                 const wpi::Twine& newValueStr) const;
+                 std::string_view newValueStr) const;
 
   // If this is a device (rather than software) property
   bool device{true};
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.cpp b/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.cpp
index ac48178..afe3e42 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "UsbUtil.h"
 
@@ -11,8 +8,10 @@
 #include <libgen.h>
 #include <sys/ioctl.h>
 
-#include <wpi/Format.h>
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
 #include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 
@@ -21,139 +20,142 @@
 
 namespace cs {
 
-static wpi::StringRef GetUsbNameFromFile(int vendor, int product,
-                                         wpi::SmallVectorImpl<char>& buf) {
+static std::string GetUsbNameFromFile(int vendor, int product) {
   int fd = open("/var/lib/usbutils/usb.ids", O_RDONLY);
-  if (fd < 0) return wpi::StringRef{};
+  if (fd < 0) {
+    return {};
+  }
 
-  wpi::raw_svector_ostream os{buf};
+  wpi::SmallString<128> buf;
   wpi::raw_fd_istream is{fd, true};
 
   // build vendor and product 4-char hex strings
-  wpi::SmallString<16> vendorStr, productStr;
-  wpi::raw_svector_ostream vendorOs{vendorStr}, productOs{productStr};
-  vendorOs << wpi::format_hex_no_prefix(vendor, 4);
-  productOs << wpi::format_hex_no_prefix(product, 4);
+  auto vendorStr = fmt::format("{:04x}", vendor);
+  auto productStr = fmt::format("{:04x}", product);
 
   // scan file
   wpi::SmallString<128> lineBuf;
   bool foundVendor = false;
   for (;;) {
     auto line = is.getline(lineBuf, 4096);
-    if (is.has_error()) break;
+    if (is.has_error()) {
+      break;
+    }
 
-    if (line.empty()) continue;
+    if (line.empty()) {
+      continue;
+    }
 
     // look for vendor at start of line
-    if (line.startswith(vendorStr)) {
+    if (wpi::starts_with(line, vendorStr)) {
       foundVendor = true;
-      os << line.substr(5).trim() << ' ';
+      buf += wpi::trim(line.substr(5));
+      buf += ' ';
       continue;
     }
 
     if (foundVendor) {
       // next vendor, but didn't match product?
       if (line[0] != '\t') {
-        os << "Unknown";
-        return os.str();
+        buf += "Unknown";
+        return buf;
       }
 
       // look for product
-      if (line.substr(1).startswith(productStr)) {
-        os << line.substr(6).trim();
-        return os.str();
+      if (wpi::starts_with(line.substr(1), productStr)) {
+        buf += wpi::trim(line.substr(6));
+        return buf;
       }
     }
   }
 
-  return wpi::StringRef{};
+  return {};
 }
 
-wpi::StringRef GetUsbNameFromId(int vendor, int product,
-                                wpi::SmallVectorImpl<char>& buf) {
+std::string GetUsbNameFromId(int vendor, int product) {
   // try reading usb.ids
-  wpi::StringRef rv = GetUsbNameFromFile(vendor, product, buf);
-  if (!rv.empty()) return rv;
-
-  // Fall back to internal database
-  wpi::raw_svector_ostream os{buf};
-  switch (vendor) {
-    case 0x046d:
-      os << "Logitech, Inc. ";
-      switch (product) {
-        case 0x0802:
-          os << "Webcam C200";
-          break;
-        case 0x0804:
-          os << "Webcam C250";
-          break;
-        case 0x0805:
-          os << "Webcam C300";
-          break;
-        case 0x0807:
-          os << "Webcam B500";
-          break;
-        case 0x0808:
-          os << "Webcam C600";
-          break;
-        case 0x0809:
-          os << "Webcam Pro 9000";
-          break;
-        case 0x080a:
-          os << "Portable Webcam C905";
-          break;
-        case 0x080f:
-          os << "Webcam C120";
-          break;
-        case 0x0819:
-          os << "Webcam C210";
-          break;
-        case 0x081b:
-          os << "Webcam C310";
-          break;
-        case 0x081d:
-          os << "HD Webcam C510";
-          break;
-        case 0x0821:
-          os << "HD Webcam C910";
-          break;
-        case 0x0825:
-          os << "Webcam C270";
-          break;
-        case 0x0826:
-          os << "HD Webcam C525";
-          break;
-        case 0x0828:
-          os << "HD Webcam B990";
-          break;
-        case 0x082b:
-          os << "Webcam C170";
-          break;
-        case 0x082d:
-          os << "HD Pro Webcam C920";
-          break;
-        case 0x0836:
-          os << "B525 HD Webcam";
-          break;
-        case 0x0843:
-          os << "Webcam C930e";
-          break;
-      }
-      break;
+  std::string rv = GetUsbNameFromFile(vendor, product);
+  if (!rv.empty()) {
+    return rv;
   }
 
-  return os.str();
+  // Fall back to internal database
+  switch (vendor) {
+    case 0x046d: {
+      std::string_view productStr;
+      switch (product) {
+        case 0x0802:
+          productStr = "Webcam C200";
+          break;
+        case 0x0804:
+          productStr = "Webcam C250";
+          break;
+        case 0x0805:
+          productStr = "Webcam C300";
+          break;
+        case 0x0807:
+          productStr = "Webcam B500";
+          break;
+        case 0x0808:
+          productStr = "Webcam C600";
+          break;
+        case 0x0809:
+          productStr = "Webcam Pro 9000";
+          break;
+        case 0x080a:
+          productStr = "Portable Webcam C905";
+          break;
+        case 0x080f:
+          productStr = "Webcam C120";
+          break;
+        case 0x0819:
+          productStr = "Webcam C210";
+          break;
+        case 0x081b:
+          productStr = "Webcam C310";
+          break;
+        case 0x081d:
+          productStr = "HD Webcam C510";
+          break;
+        case 0x0821:
+          productStr = "HD Webcam C910";
+          break;
+        case 0x0825:
+          productStr = "Webcam C270";
+          break;
+        case 0x0826:
+          productStr = "HD Webcam C525";
+          break;
+        case 0x0828:
+          productStr = "HD Webcam B990";
+          break;
+        case 0x082b:
+          productStr = "Webcam C170";
+          break;
+        case 0x082d:
+          productStr = "HD Pro Webcam C920";
+          break;
+        case 0x0836:
+          productStr = "B525 HD Webcam";
+          break;
+        case 0x0843:
+          productStr = "Webcam C930e";
+          break;
+      }
+      return fmt::format("Logitech, Inc. {}", productStr);
+    }
+  }
+
+  return {};
 }
 
 int CheckedIoctl(int fd, unsigned long req, void* data,  // NOLINT(runtime/int)
                  const char* name, const char* file, int line, bool quiet) {
   int retval = ioctl(fd, req, data);
   if (!quiet && retval < 0) {
-    wpi::SmallString<64> localfile{file};
-    localfile.push_back('\0');
-    WPI_ERROR(Instance::GetInstance().logger,
-              "ioctl " << name << " failed at " << basename(localfile.data())
-                       << ":" << line << ": " << std::strerror(errno));
+    WPI_ERROR(Instance::GetInstance().logger, "ioctl {} failed at {}:{}: {}",
+              name, fs::path{file}.filename().string(), line,
+              std::strerror(errno));
   }
   return retval;
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.h b/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.h
index 4f1d9d8..9f6f8f5 100644
--- a/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.h
+++ b/third_party/allwpilib/cscore/src/main/native/linux/UsbUtil.h
@@ -1,22 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_USBUTIL_H_
 #define CSCORE_USBUTIL_H_
 
-#include <stdint.h>
-
-#include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
+#include <string>
 
 namespace cs {
 
-wpi::StringRef GetUsbNameFromId(int vendor, int product,
-                                wpi::SmallVectorImpl<char>& buf);
+std::string GetUsbNameFromId(int vendor, int product);
 
 int CheckedIoctl(int fd, unsigned long req, void* data,  // NOLINT(runtime/int)
                  const char* name, const char* file, int line, bool quiet);
diff --git a/third_party/allwpilib/cscore/src/main/native/osx/NetworkListener.cpp b/third_party/allwpilib/cscore/src/main/native/osx/NetworkListener.cpp
index 0716789..3d77fba 100644
--- a/third_party/allwpilib/cscore/src/main/native/osx/NetworkListener.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/osx/NetworkListener.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "NetworkListener.h"
 
@@ -13,7 +10,7 @@
 
 NetworkListener::NetworkListener(wpi::Logger& logger, Notifier& notifier) {}
 
-NetworkListener::~NetworkListener() {}
+NetworkListener::~NetworkListener() = default;
 
 void NetworkListener::Start() {}
 
diff --git a/third_party/allwpilib/cscore/src/main/native/osx/NetworkUtil.cpp b/third_party/allwpilib/cscore/src/main/native/osx/NetworkUtil.cpp
index 935c21f..0aaf535 100644
--- a/third_party/allwpilib/cscore/src/main/native/osx/NetworkUtil.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/osx/NetworkUtil.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_cpp.h"
 
diff --git a/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraImpl.cpp b/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraImpl.cpp
index abefcc7..3b5d821 100644
--- a/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraImpl.cpp
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore_cpp.h"
 
 namespace cs {
 
-CS_Source CreateUsbCameraDev(const wpi::Twine& name, int dev,
+CS_Source CreateUsbCameraDev(std::string_view name, int dev,
                              CS_Status* status) {
   *status = CS_INVALID_HANDLE;
   return 0;
 }
 
-CS_Source CreateUsbCameraPath(const wpi::Twine& name, const wpi::Twine& path,
+CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
                               CS_Status* status) {
   *status = CS_INVALID_HANDLE;
   return 0;
 }
 
-void SetUsbCameraPath(CS_Source source, const wpi::Twine& path,
+void SetUsbCameraPath(CS_Source source, std::string_view path,
                       CS_Status* status) {
   *status = CS_INVALID_HANDLE;
 }
diff --git a/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraListener.cpp b/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraListener.cpp
new file mode 100644
index 0000000..b83663d
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/native/osx/UsbCameraListener.cpp
@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "UsbCameraListener.h"
+
+using namespace cs;
+
+class UsbCameraListener::Impl {};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier) {}
+
+UsbCameraListener::~UsbCameraListener() = default;
+
+void UsbCameraListener::Start() {}
+
+void UsbCameraListener::Stop() {}
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.cpp b/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.cpp
index 265f7d3..f9f5cae 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <mfapi.h>
 #include <mfidl.h>
@@ -59,9 +56,13 @@
   return uCount;
 }
 
-STDMETHODIMP SourceReaderCB::OnEvent(DWORD, IMFMediaEvent*) { return S_OK; }
+STDMETHODIMP SourceReaderCB::OnEvent(DWORD, IMFMediaEvent*) {
+  return S_OK;
+}
 
-STDMETHODIMP SourceReaderCB::OnFlush(DWORD) { return S_OK; }
+STDMETHODIMP SourceReaderCB::OnFlush(DWORD) {
+  return S_OK;
+}
 
 void SourceReaderCB::NotifyError(HRESULT hr) {
   wprintf(L"Source Reader error: 0x%X\n", hr);
@@ -73,7 +74,8 @@
                                           IMFSample* pSample  // Can be NULL
 ) {
   auto source = m_source.lock();
-  if (!source) return S_OK;
+  if (!source)
+    return S_OK;
   if (SUCCEEDED(hrStatus)) {
     if (pSample) {
       // Prcoess sample
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.h b/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.h
index f89e6fd..84cb982 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.h
+++ b/third_party/allwpilib/cscore/src/main/native/windows/COMCreators.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -42,7 +39,7 @@
 
  private:
   // Destructor is private. Caller should call Release.
-  virtual ~SourceReaderCB() {}
+  virtual ~SourceReaderCB() = default;
   void NotifyError(HRESULT hr);
 
   ULONG m_nRefCount;
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/ComPtr.h b/third_party/allwpilib/cscore/src/main/native/windows/ComPtr.h
index 22a330d..2d9f241 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/ComPtr.h
+++ b/third_party/allwpilib/cscore/src/main/native/windows/ComPtr.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -27,24 +24,24 @@
   template <typename T>
   friend class ComPtr;
 
-  ComPtr(std::nullptr_t = nullptr) noexcept {}  // NOLINT(runtime/explicit)
+  ComPtr(std::nullptr_t = nullptr) noexcept {}  // NOLINT
   ComPtr(const ComPtr& other) noexcept : m_ptr(other.m_ptr) {
     InternalAddRef();
   }
 
   template <typename T>
-  ComPtr(const ComPtr<T>& other) noexcept : m_ptr(other.m_ptr) {
+  ComPtr(const ComPtr<T>& other) noexcept : m_ptr(other.m_ptr) {  // NOLINT
     InternalAddRef();
   }
 
   template <typename T>
-  ComPtr(ComPtr<T>&& other) noexcept : m_ptr(other.m_ptr) {
+  ComPtr(ComPtr<T>&& other) noexcept : m_ptr(other.m_ptr) {  // NOLINT
     other.m_ptr = nullptr;
   }
 
   ~ComPtr() noexcept { InternalRelease(); }
 
-  ComPtr& operator=(const ComPtr& other) noexcept {
+  ComPtr& operator=(const ComPtr& other) noexcept {  // NOLINT
     InternalCopy(other.m_ptr);
     return *this;
   }
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/NetworkListener.cpp b/third_party/allwpilib/cscore/src/main/native/windows/NetworkListener.cpp
index cd29e44..b4fbb1e 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/NetworkListener.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/NetworkListener.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "NetworkListener.h"
 
@@ -47,7 +44,9 @@
 NetworkListener::NetworkListener(wpi::Logger& logger, Notifier& notifier)
     : m_impl(std::make_unique<Impl>(logger, notifier)) {}
 
-NetworkListener::~NetworkListener() { Stop(); }
+NetworkListener::~NetworkListener() {
+  Stop();
+}
 
 void NetworkListener::Start() {
   NotifyIpInterfaceChange(AF_INET, OnInterfaceChange, &m_impl->m_notifier, true,
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/NetworkUtil.cpp b/third_party/allwpilib/cscore/src/main/native/windows/NetworkUtil.cpp
index 151ba79..86b99bc 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/NetworkUtil.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/NetworkUtil.cpp
@@ -1,11 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <uv.h>
+#include <ws2tcpip.h>
 
 #include "cscore_cpp.h"
 
@@ -24,7 +22,9 @@
   char ip[50];
 
   for (int i = 0; i < counts; i++) {
-    if (adrs[i].is_internal) continue;
+    if (adrs[i].is_internal) {
+      continue;
+    }
     InetNtop(PF_INET, &(adrs[i].netmask.netmask4.sin_addr.s_addr), ip,
              sizeof(ip) - 1);
     ip[49] = '\0';
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.cpp b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.cpp
index 784f5ad..1a3b230 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #define _WINSOCKAPI_
 #include "UsbCameraImpl.h"
@@ -29,8 +26,7 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
 #include <wpi/MemAlloc.h>
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/StringExtras.h>
 #include <wpi/timestamp.h>
 
 #include "COMCreators.h"
@@ -72,24 +68,26 @@
 
 namespace cs {
 
-UsbCameraImpl::UsbCameraImpl(const wpi::Twine& name, wpi::Logger& logger,
+UsbCameraImpl::UsbCameraImpl(std::string_view name, wpi::Logger& logger,
                              Notifier& notifier, Telemetry& telemetry,
-                             const wpi::Twine& path)
-    : SourceImpl{name, logger, notifier, telemetry}, m_path{path.str()} {
+                             std::string_view path)
+    : SourceImpl{name, logger, notifier, telemetry}, m_path{path} {
   std::wstring_convert<std::codecvt_utf8<wchar_t>> utf8_conv;
   m_widePath = utf8_conv.from_bytes(m_path.c_str());
   m_deviceId = -1;
   StartMessagePump();
 }
 
-UsbCameraImpl::UsbCameraImpl(const wpi::Twine& name, wpi::Logger& logger,
+UsbCameraImpl::UsbCameraImpl(std::string_view name, wpi::Logger& logger,
                              Notifier& notifier, Telemetry& telemetry,
                              int deviceId)
     : SourceImpl{name, logger, notifier, telemetry}, m_deviceId(deviceId) {
   StartMessagePump();
 }
 
-UsbCameraImpl::~UsbCameraImpl() { m_messagePump = nullptr; }
+UsbCameraImpl::~UsbCameraImpl() {
+  m_messagePump = nullptr;
+}
 
 void UsbCameraImpl::SetProperty(int property, int value, CS_Status* status) {
   Message msg{Message::kCmdSetProperty};
@@ -100,11 +98,11 @@
           SetCameraMessage, msg.kind, &msg);
   *status = result;
 }
-void UsbCameraImpl::SetStringProperty(int property, const wpi::Twine& value,
+void UsbCameraImpl::SetStringProperty(int property, std::string_view value,
                                       CS_Status* status) {
   Message msg{Message::kCmdSetPropertyStr};
   msg.data[0] = property;
-  msg.dataStr = value.str();
+  msg.dataStr = value;
   auto result =
       m_messagePump->SendWindowMessage<CS_Status, Message::Kind, Message*>(
           SetCameraMessage, msg.kind, &msg);
@@ -198,9 +196,9 @@
       SetCameraMessage, Message::kNumSinksEnabledChanged, nullptr);
 }
 
-void UsbCameraImpl::SetPath(const wpi::Twine& path, CS_Status* status) {
+void UsbCameraImpl::SetPath(std::string_view path, CS_Status* status) {
   Message msg{Message::kCmdSetPath};
-  msg.dataStr = path.str();
+  msg.dataStr = path;
   auto result =
       m_messagePump->SendWindowMessage<CS_Status, Message::Kind, Message*>(
           SetCameraMessage, msg.kind, &msg);
@@ -257,7 +255,8 @@
 }
 
 void UsbCameraImpl::DeviceDisconnect() {
-  if (m_connectVerbose) SINFO("Disconnected from " << m_path);
+  if (m_connectVerbose)
+    SINFO("Disconnected from {}", m_path);
   m_sourceReader.Reset();
   m_mediaSource.Reset();
   if (m_imageCallback) {
@@ -268,8 +267,9 @@
   SetConnected(false);
 }
 
-static bool IsPercentageProperty(wpi::StringRef name) {
-  if (name.startswith("raw_")) name = name.substr(4);
+static bool IsPercentageProperty(std::string_view name) {
+  if (wpi::starts_with(name, "raw_"))
+    name = name.substr(4);
   return name == "Brightness" || name == "Contrast" || name == "Saturation" ||
          name == "Hue" || name == "Sharpness" || name == "Gain" ||
          name == "Exposure";
@@ -277,43 +277,60 @@
 
 void UsbCameraImpl::ProcessFrame(IMFSample* videoSample,
                                  const VideoMode& mode) {
-  if (!videoSample) return;
+  if (!videoSample)
+    return;
 
   ComPtr<IMFMediaBuffer> buf;
 
   if (!SUCCEEDED(videoSample->ConvertToContiguousBuffer(buf.GetAddressOf()))) {
     DWORD bcnt = 0;
-    if (!SUCCEEDED(videoSample->GetBufferCount(&bcnt))) return;
-    if (bcnt == 0) return;
+    if (!SUCCEEDED(videoSample->GetBufferCount(&bcnt)))
+      return;
+    if (bcnt == 0)
+      return;
     if (!SUCCEEDED(videoSample->GetBufferByIndex(0, buf.GetAddressOf())))
       return;
   }
 
-  bool lock2d = false;
   BYTE* ptr = NULL;
   LONG pitch = 0;
-  DWORD maxsize = 0, cursize = 0;
+  DWORD length = 0;
 
-  // "For 2-D buffers, the Lock2D method is more efficient than the Lock
-  // method" see IMFMediaBuffer::Lock method documentation:
-  // https://msdn.microsoft.com/en-us/library/windows/desktop/bb970366(v=vs.85).aspx
-  ComPtr<IMF2DBuffer> buffer2d;
-  DWORD memLength2d = 0;
-  if (true) {
-    buffer2d = buf.As<IMF2DBuffer>();
-    if (buffer2d) {
-      buffer2d->GetContiguousLength(&memLength2d);
-      if (SUCCEEDED(buffer2d->Lock2D(&ptr, &pitch))) {
-        lock2d = true;
+  // First try to access using Lock2DSize, then try Lock2D, then fallback
+  // https://docs.microsoft.com/en-us/windows/win32/api/mfobjects/nf-mfobjects-imfmediabuffer-lock
+
+  ComPtr<IMF2DBuffer> buffer2d = buf.As<IMF2DBuffer>();
+  if (buffer2d) {
+    BYTE* scanline0 = nullptr;
+    HRESULT result;
+    ComPtr<IMF2DBuffer2> buffer2d2 = buf.As<IMF2DBuffer2>();
+    if (buffer2d2) {
+      BYTE* datastart;
+      result = buffer2d2->Lock2DSize(MF2DBuffer_LockFlags_Read, &scanline0,
+                                     &pitch, &datastart, &length);
+    } else {
+      result = buffer2d->Lock2D(&scanline0, &pitch);
+    }
+    if (SUCCEEDED(result)) {
+      BOOL isContiguous;
+      if (pitch > 0 && SUCCEEDED(buffer2d->IsContiguousFormat(&isContiguous)) &&
+          isContiguous &&
+          (length || SUCCEEDED(buffer2d->GetContiguousLength(&length)))) {
+        // Use the buffer pointer.
+        ptr = scanline0;
+      } else {
+        // Release the buffer and fall back to Lock().
+        buffer2d->Unlock2D();
       }
     }
   }
   if (ptr == NULL) {
-    if (!SUCCEEDED(buf->Lock(&ptr, &maxsize, &cursize))) {
+    buffer2d = nullptr;
+    DWORD maxsize = 0;
+    if (!SUCCEEDED(buf->Lock(&ptr, &maxsize, &length))) {
       return;
     }
   }
-  if (!ptr) return;
 
   cv::Mat tmpMat;
   std::unique_ptr<Image> dest;
@@ -323,8 +340,7 @@
     case cs::VideoMode::PixelFormat::kMJPEG: {
       // Special case
       PutFrame(VideoMode::kMJPEG, mode.width, mode.height,
-               wpi::StringRef(reinterpret_cast<char*>(ptr), cursize),
-               wpi::Now());
+               {reinterpret_cast<char*>(ptr), length}, wpi::Now());
       doFinalSet = false;
       break;
     }
@@ -355,10 +371,11 @@
     PutFrame(std::move(dest), wpi::Now());
   }
 
-  if (lock2d)
+  if (buffer2d) {
     buffer2d->Unlock2D();
-  else
+  } else {
     buf->Unlock();
+  }
 }
 
 LRESULT UsbCameraImpl::PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam,
@@ -454,16 +471,19 @@
 }
 
 bool UsbCameraImpl::DeviceConnect() {
-  if (m_mediaSource && m_sourceReader) return true;
+  if (m_mediaSource && m_sourceReader)
+    return true;
 
-  if (m_connectVerbose) SINFO("Connecting to USB camera on " << m_path);
+  if (m_connectVerbose)
+    SINFO("Connecting to USB camera on {}", m_path);
 
-  SDEBUG3("opening device");
+  SDEBUG3("{}", "opening device");
 
   const wchar_t* path = m_widePath.c_str();
   m_mediaSource = CreateVideoCaptureDevice(path);
 
-  if (!m_mediaSource) return false;
+  if (!m_mediaSource)
+    return false;
   m_imageCallback = CreateSourceReaderCB(shared_from_this(), m_mode);
 
   m_sourceReader =
@@ -484,13 +504,13 @@
   }
 
   if (!m_properties_cached) {
-    SDEBUG3("caching properties");
+    SDEBUG3("{}", "caching properties");
     DeviceCacheProperties();
     DeviceCacheVideoModes();
     DeviceCacheMode();
     m_properties_cached = true;
   } else {
-    SDEBUG3("restoring video mode");
+    SDEBUG3("{}", "restoring video mode");
     DeviceSetMode();
   }
 
@@ -504,7 +524,7 @@
 }
 
 std::unique_ptr<PropertyImpl> UsbCameraImpl::CreateEmptyProperty(
-    const wpi::Twine& name) const {
+    std::string_view name) const {
   return nullptr;
 }
 
@@ -513,7 +533,9 @@
   auto result = m_messagePump->SendWindowMessage<CS_Status>(
       WaitForStartupMessage, nullptr, nullptr);
   *status = result;
-  if (*status != CS_OK) return false;
+  if (*status != CS_OK) {
+    return false;
+  }
   if (!m_properties_cached) {
     *status = CS_SOURCE_IS_DISCONNECTED;
     return false;
@@ -522,7 +544,7 @@
 }
 
 template <typename TagProperty, typename IAM>
-void UsbCameraImpl::DeviceAddProperty(const wpi::Twine& name_, TagProperty tag,
+void UsbCameraImpl::DeviceAddProperty(std::string_view name_, TagProperty tag,
                                       IAM* pProcAmp) {
   // First see if properties exist
   bool isValid = false;
@@ -533,11 +555,11 @@
   }
 }
 
-template void UsbCameraImpl::DeviceAddProperty(const wpi::Twine& name_,
+template void UsbCameraImpl::DeviceAddProperty(std::string_view name_,
                                                tagVideoProcAmpProperty tag,
                                                IAMVideoProcAmp* pProcAmp);
 
-template void UsbCameraImpl::DeviceAddProperty(const wpi::Twine& name_,
+template void UsbCameraImpl::DeviceAddProperty(std::string_view name_,
                                                tagCameraControlProperty tag,
                                                IAMCameraControl* pProcAmp);
 
@@ -548,7 +570,8 @@
   DeviceAddProperty(#val, CameraControl_##val, pCamControl);
 
 void UsbCameraImpl::DeviceCacheProperties() {
-  if (!m_sourceReader) return;
+  if (!m_sourceReader)
+    return;
 
   IAMVideoProcAmp* pProcAmp = NULL;
 
@@ -645,7 +668,7 @@
   } else {
     // Read current raw value and set percentage from it
     if (!rawProp->DeviceGet(lock, pProcAmp))
-      SWARNING("failed to get property " << rawProp->name);
+      SWARNING("failed to get property {}", rawProp->name);
 
     if (perProp) {
       perProp->SetValue(RawToPercentage(*rawProp, rawProp->value));
@@ -656,7 +679,7 @@
   // Set value on device if user-configured
   if (rawProp->valueSet) {
     if (!rawProp->DeviceSet(lock, pProcAmp))
-      SWARNING("failed to set property " << rawProp->name);
+      SWARNING("failed to set property {}", rawProp->name);
   }
 
   // Update pointers since we released the lock
@@ -696,7 +719,8 @@
   }
 
   NotifyPropertyCreated(*rawIndex, *rawPropPtr);
-  if (perPropPtr && perIndex) NotifyPropertyCreated(*perIndex, *perPropPtr);
+  if (perPropPtr && perIndex)
+    NotifyPropertyCreated(*perIndex, *perPropPtr);
 }
 
 CS_StatusValue UsbCameraImpl::DeviceProcessCommand(
@@ -739,11 +763,12 @@
   bool setString = (msg.kind == Message::kCmdSetPropertyStr);
   int property = msg.data[0];
   int value = msg.data[1];
-  wpi::StringRef valueStr = msg.dataStr;
+  std::string_view valueStr = msg.dataStr;
 
   // Look up
   auto prop = static_cast<UsbCameraProperty*>(GetProperty(property));
-  if (!prop) return CS_INVALID_PROPERTY;
+  if (!prop)
+    return CS_INVALID_PROPERTY;
 
   // If setting before we get, guess initial type based on set
   if (prop->propKind == CS_PROP_NONE) {
@@ -774,7 +799,8 @@
 
   // Actually set the new value on the device (if possible)
   if (!prop->device) {
-    if (prop->id == kPropConnectVerboseId) m_connectVerbose = value;
+    if (prop->id == kPropConnectVerboseId)
+      m_connectVerbose = value;
   } else {
     if (!prop->DeviceSet(lock, m_sourceReader.Get())) {
       return CS_PROPERTY_WRITE_FAILED;
@@ -858,8 +884,12 @@
 }
 
 bool UsbCameraImpl::DeviceStreamOn() {
-  if (m_streaming) return false;
-  if (!m_deviceValid) return false;
+  if (m_streaming) {
+    return false;
+  }
+  if (!m_deviceValid) {
+    return false;
+  }
   m_streaming = true;
   m_sourceReader->ReadSample(MF_SOURCE_READER_FIRST_VIDEO_STREAM, 0, NULL, NULL,
                              NULL, NULL);
@@ -872,9 +902,11 @@
 }
 
 void UsbCameraImpl::DeviceCacheMode() {
-  if (!m_sourceReader) return;
+  if (!m_sourceReader)
+    return;
 
-  if (m_windowsVideoModes.size() == 0) return;
+  if (m_windowsVideoModes.size() == 0)
+    return;
 
   if (!m_currentMode) {
     // First, see if our set mode is valid
@@ -939,7 +971,8 @@
 }
 
 void UsbCameraImpl::DeviceCacheVideoModes() {
-  if (!m_sourceReader) return;
+  if (!m_sourceReader)
+    return;
 
   std::vector<VideoMode> modes;
   m_windowsVideoModes.clear();
@@ -990,23 +1023,21 @@
   m_notifier.NotifySource(*this, CS_SOURCE_VIDEOMODES_UPDATED);
 }
 
-static void ParseVidAndPid(wpi::StringRef path, int* pid, int* vid) {
-  auto vidIndex = path.find_lower("vid_");
-  auto pidIndex = path.find_lower("pid_");
+static void ParseVidAndPid(std::string_view path, int* pid, int* vid) {
+  auto vidIndex = wpi::find_lower(path, "vid_");
+  auto pidIndex = wpi::find_lower(path, "pid_");
 
-  if (vidIndex != wpi::StringRef::npos) {
-    auto vidSlice = path.slice(vidIndex + 4, vidIndex + 8);
-    uint16_t val = 0;
-    if (!vidSlice.getAsInteger(16, val)) {
-      *vid = val;
+  if (vidIndex != std::string_view::npos) {
+    auto vidSlice = wpi::slice(path, vidIndex + 4, vidIndex + 8);
+    if (auto v = wpi::parse_integer<uint16_t>(vidSlice, 16)) {
+      *vid = v.value();
     }
   }
 
-  if (pidIndex != wpi::StringRef::npos) {
-    auto pidSlice = path.slice(pidIndex + 4, pidIndex + 8);
-    uint16_t val = 0;
-    if (!pidSlice.getAsInteger(16, val)) {
-      *pid = val;
+  if (pidIndex != std::string_view::npos) {
+    auto pidSlice = wpi::slice(path, pidIndex + 4, pidIndex + 8);
+    if (auto v = wpi::parse_integer<uint16_t>(pidSlice, 16)) {
+      *pid = v.value();
     }
   }
 }
@@ -1080,7 +1111,7 @@
   return retval;
 }
 
-CS_Source CreateUsbCameraDev(const wpi::Twine& name, int dev,
+CS_Source CreateUsbCameraDev(std::string_view name, int dev,
                              CS_Status* status) {
   // First check if device exists
   auto devices = cs::EnumerateUsbCameras(status);
@@ -1093,7 +1124,7 @@
   return inst.CreateSource(CS_SOURCE_USB, source);
 }
 
-CS_Source CreateUsbCameraPath(const wpi::Twine& name, const wpi::Twine& path,
+CS_Source CreateUsbCameraPath(std::string_view name, std::string_view path,
                               CS_Status* status) {
   auto& inst = Instance::GetInstance();
   auto source = std::make_shared<UsbCameraImpl>(
@@ -1101,7 +1132,7 @@
   return inst.CreateSource(CS_SOURCE_USB, source);
 }
 
-void SetUsbCameraPath(CS_Source source, const wpi::Twine& path,
+void SetUsbCameraPath(CS_Source source, std::string_view path,
                       CS_Status* status) {
   auto data = Instance::GetInstance().GetSource(source);
   if (!data || data->kind != CS_SOURCE_USB) {
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.h b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.h
index 60a3080..ea6c104 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.h
+++ b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef CSCORE_USBCAMERAIMPL_H_
 #define CSCORE_USBCAMERAIMPL_H_
@@ -19,14 +16,13 @@
 #include <atomic>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <utility>
 #include <vector>
 
 #include <Dbt.h>
-#include <wpi/STLExtras.h>
 #include <wpi/SmallVector.h>
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 #include <wpi/raw_istream.h>
@@ -43,17 +39,17 @@
 class UsbCameraImpl : public SourceImpl,
                       public std::enable_shared_from_this<UsbCameraImpl> {
  public:
-  UsbCameraImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
-                Telemetry& telemetry, const wpi::Twine& path);
-  UsbCameraImpl(const wpi::Twine& name, wpi::Logger& logger, Notifier& notifier,
+  UsbCameraImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
+                Telemetry& telemetry, std::string_view path);
+  UsbCameraImpl(std::string_view name, wpi::Logger& logger, Notifier& notifier,
                 Telemetry& telemetry, int deviceId);
   ~UsbCameraImpl() override;
 
-  void Start();
+  void Start() override;
 
   // Property functions
   void SetProperty(int property, int value, CS_Status* status) override;
-  void SetStringProperty(int property, const wpi::Twine& value,
+  void SetStringProperty(int property, std::string_view value,
                          CS_Status* status) override;
 
   // Standard common camera properties
@@ -78,7 +74,7 @@
   void ProcessFrame(IMFSample* sample, const VideoMode& mode);
   void PostRequestNewFrame();
 
-  void SetPath(const wpi::Twine& path, CS_Status* status);
+  void SetPath(std::string_view path, CS_Status* status);
   std::string GetPath() const;
 
   // Messages passed to/from camera thread
@@ -110,7 +106,7 @@
 
  protected:
   std::unique_ptr<PropertyImpl> CreateEmptyProperty(
-      const wpi::Twine& name) const override;
+      std::string_view name) const override;
 
   // Cache properties.  Immediately successful if properties are already cached.
   // If they are not, tries to connect to the camera to do so; returns false and
@@ -135,7 +131,7 @@
   void DeviceCacheProperties();
   void DeviceCacheVideoModes();
   template <typename TagProperty, typename IAM>
-  void DeviceAddProperty(const wpi::Twine& name_, TagProperty tag,
+  void DeviceAddProperty(std::string_view name_, TagProperty tag,
                          IAM* pProcAmp);
 
   ComPtr<IMFMediaType> DeviceCheckModeValid(const VideoMode& toCheck);
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraListener.cpp b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraListener.cpp
new file mode 100644
index 0000000..be33771
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraListener.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "UsbCameraListener.h"
+
+#include "Notifier.h"
+#include "WindowsMessagePump.h"
+
+#include <dbt.h>  // NOLINT(build/include_order)
+
+#define IDT_TIMER1 1001
+
+using namespace cs;
+
+class UsbCameraListener::Impl {
+ public:
+  explicit Impl(Notifier& notifier) : m_notifier{notifier} {}
+
+  void Start() {
+    m_messagePump = std::make_unique<WindowsMessagePump>(
+        [this](HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam) {
+          return this->PumpMain(hwnd, uiMsg, wParam, lParam);
+        });
+  }
+
+  void Stop() { m_messagePump = nullptr; }
+
+  LRESULT PumpMain(HWND hwnd, UINT uiMsg, WPARAM wParam, LPARAM lParam) {
+    switch (uiMsg) {
+      case WM_CLOSE:
+        KillTimer(hwnd, IDT_TIMER1);
+        break;
+      case WM_TIMER:
+        if (wParam == IDT_TIMER1) {
+          KillTimer(hwnd, IDT_TIMER1);
+          m_notifier.NotifyUsbCamerasChanged();
+        }
+        break;
+      case WM_DEVICECHANGE:
+        PDEV_BROADCAST_HDR parameter =
+            reinterpret_cast<PDEV_BROADCAST_HDR>(lParam);
+        if (wParam == DBT_DEVICEARRIVAL &&
+            parameter->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) {
+          SetTimer(hwnd, IDT_TIMER1, 200, nullptr);
+        } else if (wParam == DBT_DEVICEREMOVECOMPLETE &&
+                   parameter->dbch_devicetype == DBT_DEVTYP_DEVICEINTERFACE) {
+          SetTimer(hwnd, IDT_TIMER1, 200, nullptr);
+        }
+        break;
+    }
+    return 0;
+  }
+
+  Notifier& m_notifier;
+  std::unique_ptr<cs::WindowsMessagePump> m_messagePump;
+};
+
+UsbCameraListener::UsbCameraListener(wpi::Logger& logger, Notifier& notifier)
+    : m_impl{std::make_unique<Impl>(notifier)} {}
+
+UsbCameraListener::~UsbCameraListener() {
+  Stop();
+}
+
+void UsbCameraListener::Start() {
+  m_impl->Start();
+}
+
+void UsbCameraListener::Stop() {
+  m_impl->Stop();
+}
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.cpp b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.cpp
index ae22436..5ddeebf 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.cpp
@@ -1,20 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "UsbCameraProperty.h"
 
+#include <fmt/format.h>
+
 #include "ComPtr.h"
 
 using namespace cs;
 
-UsbCameraProperty::UsbCameraProperty(const wpi::Twine& name_,
+UsbCameraProperty::UsbCameraProperty(std::string_view name_,
                                      tagVideoProcAmpProperty tag, bool autoProp,
                                      IAMVideoProcAmp* pProcAmp, bool* isValid)
-    : PropertyImpl{autoProp ? name_ + "_auto" : name_} {
+    : PropertyImpl{autoProp ? fmt::format("{}_auto", name_) : name_} {
   this->tagVideoProc = tag;
   this->isControlProperty = false;
   this->isAutoProp = autoProp;
@@ -41,7 +40,8 @@
 
 bool UsbCameraProperty::DeviceGet(std::unique_lock<wpi::mutex>& lock,
                                   IAMVideoProcAmp* pProcAmp) {
-  if (!pProcAmp) return true;
+  if (!pProcAmp)
+    return true;
 
   lock.unlock();
   long newValue = 0, paramFlag = 0;  // NOLINT(runtime/int)
@@ -60,7 +60,8 @@
 bool UsbCameraProperty::DeviceSet(std::unique_lock<wpi::mutex>& lock,
                                   IAMVideoProcAmp* pProcAmp,
                                   int newValue) const {
-  if (!pProcAmp) return true;
+  if (!pProcAmp)
+    return true;
 
   lock.unlock();
   if (SUCCEEDED(
@@ -72,11 +73,11 @@
   return false;
 }
 
-UsbCameraProperty::UsbCameraProperty(const wpi::Twine& name_,
+UsbCameraProperty::UsbCameraProperty(std::string_view name_,
                                      tagCameraControlProperty tag,
                                      bool autoProp, IAMCameraControl* pProcAmp,
                                      bool* isValid)
-    : PropertyImpl{autoProp ? name_ + "_auto" : name_} {
+    : PropertyImpl{autoProp ? fmt::format("{}_auto", name_) : name_} {
   this->tagCameraControl = tag;
   this->isControlProperty = true;
   this->isAutoProp = autoProp;
@@ -103,7 +104,8 @@
 
 bool UsbCameraProperty::DeviceGet(std::unique_lock<wpi::mutex>& lock,
                                   IAMCameraControl* pProcAmp) {
-  if (!pProcAmp) return true;
+  if (!pProcAmp)
+    return true;
 
   lock.unlock();
   long newValue = 0, paramFlag = 0;  // NOLINT(runtime/int)
@@ -122,7 +124,8 @@
 bool UsbCameraProperty::DeviceSet(std::unique_lock<wpi::mutex>& lock,
                                   IAMCameraControl* pProcAmp,
                                   int newValue) const {
-  if (!pProcAmp) return true;
+  if (!pProcAmp)
+    return true;
 
   lock.unlock();
   if (SUCCEEDED(pProcAmp->Set(tagCameraControl, newValue,
@@ -136,7 +139,8 @@
 
 bool UsbCameraProperty::DeviceGet(std::unique_lock<wpi::mutex>& lock,
                                   IMFSourceReader* sourceReader) {
-  if (!sourceReader) return true;
+  if (!sourceReader)
+    return true;
 
   if (isControlProperty) {
     ComPtr<IAMCameraControl> pProcAmp;
@@ -165,7 +169,8 @@
 bool UsbCameraProperty::DeviceSet(std::unique_lock<wpi::mutex>& lock,
                                   IMFSourceReader* sourceReader,
                                   int newValue) const {
-  if (!sourceReader) return true;
+  if (!sourceReader)
+    return true;
 
   if (isControlProperty) {
     ComPtr<IAMCameraControl> pProcAmp;
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.h b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.h
index 11888e9..03cd375 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.h
+++ b/third_party/allwpilib/cscore/src/main/native/windows/UsbCameraProperty.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,6 +9,7 @@
 #include <mfreadwrite.h>
 
 #include <memory>
+#include <string_view>
 
 #include <Dshow.h>
 #include <wpi/mutex.h>
@@ -24,19 +22,19 @@
 class UsbCameraProperty : public PropertyImpl {
  public:
   UsbCameraProperty() = default;
-  explicit UsbCameraProperty(const wpi::Twine& name_) : PropertyImpl{name_} {}
+  explicit UsbCameraProperty(std::string_view name_) : PropertyImpl{name_} {}
 
   // Software property constructor
-  UsbCameraProperty(const wpi::Twine& name_, unsigned id_,
-                    CS_PropertyKind kind_, int minimum_, int maximum_,
-                    int step_, int defaultValue_, int value_)
+  UsbCameraProperty(std::string_view name_, unsigned id_, CS_PropertyKind kind_,
+                    int minimum_, int maximum_, int step_, int defaultValue_,
+                    int value_)
       : PropertyImpl(name_, kind_, minimum_, maximum_, step_, defaultValue_,
                      value_),
         device{false},
         id{id_} {}
 
   // Normalized property constructor
-  UsbCameraProperty(const wpi::Twine& name_, int rawIndex_,
+  UsbCameraProperty(std::string_view name_, int rawIndex_,
                     const UsbCameraProperty& rawProp, int defaultValue_,
                     int value_)
       : PropertyImpl(name_, rawProp.propKind, 1, defaultValue_, value_),
@@ -50,10 +48,10 @@
     maximum = 100;
   }
 
-  UsbCameraProperty(const wpi::Twine& name_, tagVideoProcAmpProperty tag,
+  UsbCameraProperty(std::string_view name_, tagVideoProcAmpProperty tag,
                     bool autoProp, IAMVideoProcAmp* pProcAmp, bool* isValid);
 
-  UsbCameraProperty(const wpi::Twine& name_, tagCameraControlProperty tag,
+  UsbCameraProperty(std::string_view name_, tagCameraControlProperty tag,
                     bool autoProp, IAMCameraControl* pProcAmp, bool* isValid);
 
   bool DeviceGet(std::unique_lock<wpi::mutex>& lock,
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp b/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp
index 0206bd0..e0d78d7 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp
+++ b/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WindowsMessagePump.h"
 
@@ -93,7 +90,7 @@
     std::function<LRESULT(HWND, UINT, WPARAM, LPARAM)> callback) {
   m_callback = callback;
   auto handle = CreateEvent(NULL, true, false, NULL);
-  m_mainThread = std::thread([=]() { ThreadMain(handle); });
+  m_mainThread = std::thread([=] { ThreadMain(handle); });
   auto waitResult = WaitForSingleObject(handle, 1000);
   if (waitResult == WAIT_OBJECT_0) {
     CloseHandle(handle);
@@ -102,7 +99,8 @@
 
 WindowsMessagePump::~WindowsMessagePump() {
   auto res = SendMessage(hwnd, WM_CLOSE, NULL, NULL);
-  if (m_mainThread.joinable()) m_mainThread.join();
+  if (m_mainThread.joinable())
+    m_mainThread.join();
 }
 
 void WindowsMessagePump::ThreadMain(HANDLE eventHandle) {
diff --git a/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.h b/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.h
index 4638e4e..97a3f2d 100644
--- a/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.h
+++ b/third_party/allwpilib/cscore/src/main/native/windows/WindowsMessagePump.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,7 +12,7 @@
 namespace cs {
 class WindowsMessagePump {
  public:
-  WindowsMessagePump(
+  explicit WindowsMessagePump(
       std::function<LRESULT(HWND, UINT, WPARAM, LPARAM)> callback);
   ~WindowsMessagePump();
 
diff --git a/third_party/allwpilib/cscore/src/test/java/edu/wpi/cscore/JNITest.java b/third_party/allwpilib/cscore/src/test/java/edu/wpi/cscore/JNITest.java
deleted file mode 100644
index b3c1062..0000000
--- a/third_party/allwpilib/cscore/src/test/java/edu/wpi/cscore/JNITest.java
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import org.junit.jupiter.api.Test;
-
-class JNITest {
-  @Test
-  void jniLinkTest() {
-    // Test to verify that the JNI test link works correctly.
-    CameraServerJNI.getHostname();
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java b/third_party/allwpilib/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java
deleted file mode 100644
index bc7866d..0000000
--- a/third_party/allwpilib/cscore/src/test/java/edu/wpi/cscore/UsbCameraTest.java
+++ /dev/null
@@ -1,61 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.cscore;
-
-import java.time.Duration;
-import java.util.Arrays;
-import java.util.concurrent.CompletableFuture;
-import java.util.concurrent.TimeUnit;
-import java.util.concurrent.TimeoutException;
-
-import org.junit.jupiter.api.Nested;
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.condition.EnabledOnOs;
-import org.junit.jupiter.api.condition.OS;
-
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTimeoutPreemptively;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class UsbCameraTest {
-  @Nested
-  @EnabledOnOs(OS.LINUX)
-  class ConnectVerbose {
-    @Test
-    void setConnectVerboseEnabledTest() {
-      try (UsbCamera camera = new UsbCamera("Nonexistant Camera", getNonexistentCameraDev())) {
-        camera.setConnectVerbose(1);
-
-        CompletableFuture<String> result = new CompletableFuture<>();
-        CameraServerJNI.setLogger((level, file, line, message) -> result.complete(message), 20);
-
-        assertTimeoutPreemptively(Duration.ofSeconds(5),
-            () -> assertTrue(result.get().contains("Connecting to USB camera on ")));
-      }
-    }
-
-    @Test
-    void setConnectVerboseDisabledTest() {
-      try (UsbCamera camera = new UsbCamera("Nonexistant Camera", getNonexistentCameraDev())) {
-        camera.setConnectVerbose(0);
-
-        CompletableFuture<String> result = new CompletableFuture<>();
-        CameraServerJNI.setLogger((level, file, line, message) -> result.complete(message), 20);
-
-        assertThrows(TimeoutException.class,
-            () -> result.get(3, TimeUnit.SECONDS));
-      }
-    }
-  }
-
-  private static int getNonexistentCameraDev() {
-    return Arrays.stream(CameraServerJNI.enumerateUsbCameras())
-        .mapToInt(info -> info.dev)
-        .max().orElse(-1) + 20;
-  }
-}
diff --git a/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/JNITest.java b/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/JNITest.java
new file mode 100644
index 0000000..40e2584
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/JNITest.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import org.junit.jupiter.api.Test;
+
+class JNITest {
+  @Test
+  void jniLinkTest() {
+    // Test to verify that the JNI test link works correctly.
+    CameraServerJNI.getHostname();
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/UsbCameraTest.java b/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/UsbCameraTest.java
new file mode 100644
index 0000000..f0a679f
--- /dev/null
+++ b/third_party/allwpilib/cscore/src/test/java/edu/wpi/first/cscore/UsbCameraTest.java
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.cscore;
+
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTimeoutPreemptively;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.time.Duration;
+import java.util.Arrays;
+import java.util.concurrent.CompletableFuture;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.TimeoutException;
+import org.junit.jupiter.api.Nested;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.condition.EnabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+
+class UsbCameraTest {
+  @Nested
+  @EnabledOnOs(OS.LINUX)
+  static class ConnectVerbose {
+    @Test
+    void setConnectVerboseEnabledTest() {
+      try (UsbCamera camera = new UsbCamera("Nonexistant Camera", getNonexistentCameraDev())) {
+        camera.setConnectVerbose(1);
+
+        CompletableFuture<String> result = new CompletableFuture<>();
+        CameraServerJNI.setLogger((level, file, line, message) -> result.complete(message), 20);
+
+        assertTimeoutPreemptively(
+            Duration.ofSeconds(5),
+            () -> assertTrue(result.get().contains("Connecting to USB camera on ")));
+      }
+    }
+
+    @Test
+    void setConnectVerboseDisabledTest() {
+      try (UsbCamera camera = new UsbCamera("Nonexistant Camera", getNonexistentCameraDev())) {
+        camera.setConnectVerbose(0);
+
+        CompletableFuture<String> result = new CompletableFuture<>();
+        CameraServerJNI.setLogger((level, file, line, message) -> result.complete(message), 20);
+
+        assertThrows(TimeoutException.class, () -> result.get(3, TimeUnit.SECONDS));
+      }
+    }
+  }
+
+  private static int getNonexistentCameraDev() {
+    return Arrays.stream(CameraServerJNI.enumerateUsbCameras())
+            .mapToInt(info -> info.dev)
+            .max()
+            .orElse(-1)
+        + 20;
+  }
+}
diff --git a/third_party/allwpilib/cscore/src/test/native/cpp/CameraSourceTest.cpp b/third_party/allwpilib/cscore/src/test/native/cpp/CameraSourceTest.cpp
index e09f5e6..4c860bf 100644
--- a/third_party/allwpilib/cscore/src/test/native/cpp/CameraSourceTest.cpp
+++ b/third_party/allwpilib/cscore/src/test/native/cpp/CameraSourceTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "cscore.h"
 #include "gtest/gtest.h"
@@ -12,11 +9,12 @@
 
 class CameraSourceTest : public ::testing::Test {
  protected:
-  CameraSourceTest() {}
+  CameraSourceTest() = default;
 };
 
 TEST_F(CameraSourceTest, HTTPCamera) {
   auto source = HttpCamera("axis", "http://localhost:8000");
+  cs::Shutdown();
 }
 
 }  // namespace cs
diff --git a/third_party/allwpilib/cscore/src/test/native/cpp/main.cpp b/third_party/allwpilib/cscore/src/test/native/cpp/main.cpp
index 1e5ecf0..09072ee 100644
--- a/third_party/allwpilib/cscore/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/cscore/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gtest/gtest.h"
 
diff --git a/third_party/allwpilib/docs/build.gradle b/third_party/allwpilib/docs/build.gradle
index 6de7f84..da762ed 100644
--- a/third_party/allwpilib/docs/build.gradle
+++ b/third_party/allwpilib/docs/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id 'java'
-    id "org.ysb33r.doxygen" version "0.5"
+    id "org.ysb33r.doxygen" version "0.7.0"
 }
 
 evaluationDependsOn(':wpiutil')
@@ -25,6 +25,7 @@
 def outputsFolder = file("$project.buildDir/outputs")
 
 def cppProjectZips = []
+def cppIncludeRoots = []
 
 cppProjectZips.add(project(':hal').cppHeadersZip)
 cppProjectZips.add(project(':wpiutil').cppHeadersZip)
@@ -38,43 +39,125 @@
 
 doxygen {
     executables {
-        doxygen version : '1.8.18',
-            baseURI : 'https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen'
+        doxygen version : '1.9.2',
+        baseURI : 'https://frcmaven.wpi.edu/artifactory/generic-release-mirror/doxygen'
     }
 }
 
 doxygen {
     generate_html true
+    html_extra_stylesheet 'theme.css'
 
     cppProjectZips.each {
         dependsOn it
         source it.source
+        it.ext.includeDirs.each {
+            cppIncludeRoots.add(it.absolutePath)
+        }
     }
 
-    exclude 'Eigen/**'
-    exclude 'unsupported/**'
-    exclude 'units/**'
-    exclude 'uv.h'
-    exclude 'uv/**'
+    if (project.hasProperty('docWarningsAsErrors')) {
+        // C++20 shims
+        exclude 'wpi/ghc/filesystem.hpp'
+        exclude 'wpi/span.h'
 
+        // Drake
+        exclude 'drake/common/**'
 
+        // Eigen
+        exclude 'Eigen/**'
+        exclude 'unsupported/**'
+
+        // LLVM
+        exclude 'wpi/AlignOf.h'
+        exclude 'wpi/Chrono.h'
+        exclude 'wpi/Compiler.h'
+        exclude 'wpi/ConvertUTF.h'
+        exclude 'wpi/DenseMap.h'
+        exclude 'wpi/DenseMapInfo.h'
+        exclude 'wpi/Endian.h'
+        exclude 'wpi/EpochTracker.h'
+        exclude 'wpi/Errc.h'
+        exclude 'wpi/Errno.h'
+        exclude 'wpi/ErrorHandling.h'
+        exclude 'wpi/fs.h'
+        exclude 'wpi/FunctionExtras.h'
+        exclude 'wpi/function_ref.h'
+        exclude 'wpi/Hashing.h'
+        exclude 'wpi/iterator.h'
+        exclude 'wpi/iterator_range.h'
+        exclude 'wpi/ManagedStatic.h'
+        exclude 'wpi/MapVector.h'
+        exclude 'wpi/MathExtras.h'
+        exclude 'wpi/MemAlloc.h'
+        exclude 'wpi/PointerIntPair.h'
+        exclude 'wpi/PointerLikeTypeTraits.h'
+        exclude 'wpi/PointerUnion.h'
+        exclude 'wpi/raw_os_ostream.h'
+        exclude 'wpi/raw_ostream.h'
+        exclude 'wpi/SmallPtrSet.h'
+        exclude 'wpi/SmallSet.h'
+        exclude 'wpi/SmallString.h'
+        exclude 'wpi/SmallVector.h'
+        exclude 'wpi/StringExtras.h'
+        exclude 'wpi/StringMap.h'
+        exclude 'wpi/SwapByteOrder.h'
+        exclude 'wpi/type_traits.h'
+        exclude 'wpi/VersionTuple.h'
+        exclude 'wpi/WindowsError.h'
+
+        // fmtlib
+        exclude 'fmt/**'
+
+        // libuv
+        exclude 'uv.h'
+        exclude 'uv/**'
+        exclude 'wpi/uv/**'
+
+        // json
+        exclude 'wpi/json.h'
+
+        // mpack
+        exclude 'wpi/mpack.h'
+
+        // units
+        exclude 'units/**'
+    }
+
+    case_sense_names false
     extension_mapping 'inc=C++'
-    project_name 'WPILibC++'
-    project_number wpilibVersioning.version.get()
-    javadoc_autobrief true
-    recursive true
-    quiet true
-    warnings false
-    warn_if_doc_error false
-    warn_no_paramdoc false
-    warn_format false
-    warn_logfile false
-    warn_if_undocumented false
-    generate_latex false
-    use_mathjax true
-    html_timestamp true
-    generate_treeview true
+    extract_all true
     extract_static true
+    full_path_names true
+    generate_html true
+    generate_latex false
+    generate_treeview true
+    html_extra_stylesheet 'theme.css'
+    html_timestamp true
+    javadoc_autobrief true
+    project_name 'WPILibC++'
+    project_logo '../wpiutil/src/main/native/resources/wpilib-128.png'
+    project_number wpilibVersioning.version.get()
+    quiet true
+    recursive true
+    strip_code_comments false
+    strip_from_inc_path cppIncludeRoots as String[]
+    strip_from_path cppIncludeRoots as String[]
+    use_mathjax true
+    warnings false
+    warn_if_incomplete_doc true
+    warn_if_undocumented false
+    warn_no_paramdoc true
+
+    //enable doxygen preprocessor expansion of WPI_DEPRECATED to fix SpeedController docs
+    enable_preprocessing true
+    macro_expansion true
+    expand_only_predef true
+    predefined "WPI_DEPRECATED(x)=[[deprecated(x)]]"
+
+    if (project.hasProperty('docWarningsAsErrors')) {
+        warn_as_error 'FAIL_ON_WARNINGS'
+    }
 }
 
 tasks.register("zipCppDocs", Zip) {
@@ -106,9 +189,10 @@
 task generateJavaDocs(type: Javadoc) {
     classpath += project(":wpimath").sourceSets.main.compileClasspath
     options.links("https://docs.oracle.com/en/java/javase/11/docs/api/")
-    options.addStringOption "tag", "pre:a:Pre-Condition"
-    options.addBooleanOption "Xdoclint:html,missing,reference,syntax", true
+    options.addStringOption("tag", "pre:a:Pre-Condition")
+    options.addBooleanOption("Xdoclint:html,missing,reference,syntax", true)
     options.addBooleanOption('html5', true)
+    options.linkSource(true)
     dependsOn project(':wpilibj').generateJavaVersion
     dependsOn project(':hal').generateUsageReporting
     dependsOn project(':wpimath').generateNat
@@ -124,11 +208,23 @@
     source configurations.javaSource.collect { zipTree(it) }
     include '**/*.java'
     failOnError = true
-    options.encoding = 'UTF-8'
 
     title = "WPILib API ${wpilibVersioning.version.get()}"
     ext.entryPoint = "$destinationDir/index.html"
 
+    if (JavaVersion.current().isJava8Compatible() && project.hasProperty('docWarningsAsErrors')) {
+        // Treat javadoc warnings as errors.
+        //
+        // The second argument '-quiet' is a hack. The one paramater
+        // addStringOption() doesn't work, so we add '-quiet', which is added
+        // anyway by gradle. See https://github.com/gradle/gradle/issues/2354.
+        //
+        // See JDK-8200363 (https://bugs.openjdk.java.net/browse/JDK-8200363)
+        // for information about the nonstandard -Xwerror option. JDK 15+ has
+        // -Werror.
+        options.addStringOption('Xwerror', '-quiet')
+    }
+
     if (JavaVersion.current().isJava11Compatible()) {
         if (!JavaVersion.current().isJava12Compatible()) {
             options.addBooleanOption('-no-module-directories', true)
diff --git a/third_party/allwpilib/docs/theme.css b/third_party/allwpilib/docs/theme.css
new file mode 100644
index 0000000..a5bb22f
--- /dev/null
+++ b/third_party/allwpilib/docs/theme.css
@@ -0,0 +1,1697 @@
+/**
+
+Doxygen Awesome
+https://github.com/jothepro/doxygen-awesome-css
+
+MIT License
+
+Copyright (c) 2021 jothepro
+
+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.
+
+*/
+
+html {
+    /* primary theme color. This will affect the entire websites color scheme: links, arrows, labels, ... */
+    --primary-color: #1779c4;
+    --primary-dark-color: #00559f;
+    --primary-light-color: #7aabd6;
+    --primary-lighter-color: #cae1f1;
+    --primary-lightest-color: #e9f1f8;
+
+    /* page base colors */
+    --page-background-color: white;
+    --page-foreground-color: #2c3e50;
+    --page-secondary-foreground-color: #67727e;
+
+    /* color for all separators on the website: hr, borders, ... */
+    --separator-color: #dedede;
+
+    /* border radius for all rounded components. Will affect many components, like dropdowns, memitems, codeblocks, ... */
+    --border-radius-large: 8px;
+    --border-radius-small: 4px;
+    --border-radius-medium: 6px;
+
+    /* default spacings. Most compontest reference these values for spacing, to provide uniform spacing on the page. */
+    --spacing-small: 5px;
+    --spacing-medium: 10px;
+    --spacing-large: 16px;
+
+    /* default box shadow used for raising an element above the normal content. Used in dropdowns, Searchresult, ... */
+    --box-shadow: 0 2px 10px 0 rgba(0, 0, 0, 0.1);
+
+    --odd-color: rgba(0, 0, 0, 0.03);
+
+    /* font-families. will affect all text on the website
+     * font-family: the normal font for text, headlines, menus
+     * font-family-monospace: used for preformatted text in memtitle, code, fragments
+     */
+    --font-family: -apple-system, BlinkMacSystemFont, Segoe UI, Roboto, Oxygen,
+        Ubuntu, Cantarell, Fira Sans, Droid Sans, Helvetica Neue, sans-serif;
+    --font-family-monospace: source-code-pro, Menlo, Monaco, Consolas,
+        Courier New, monospace;
+
+    /* font sizes */
+    --page-font-size: 15.6px;
+    --navigation-font-size: 14.4px;
+    --code-font-size: 14.4px; /* affects code, fragment */
+    --title-font-size: 22px;
+
+    /* content text properties. These only affect the page content, not the navigation or any other ui elements */
+    --content-line-height: 27px;
+    /* The content is centered and constraint in it's width. To make the content fill the whole page, set the variable to auto.*/
+    --content-maxwidth: 1000px;
+
+    /* colors for various content boxes: @warning, @note, @deprecated @bug */
+    --warning-color: #fca49b;
+    --warning-color-dark: #b61825;
+    --warning-color-darker: #75070f;
+    --note-color: rgba(255, 229, 100, 0.3);
+    --note-color-dark: #c39900;
+    --note-color-darker: #8d7400;
+    --deprecated-color: rgb(214, 216, 224);
+    --deprecated-color-dark: #5b6269;
+    --deprecated-color-darker: #43454a;
+    --bug-color: rgb(246, 208, 178);
+    --bug-color-dark: #a53a00;
+    --bug-color-darker: #5b1d00;
+    --invariant-color: #b7f8d0;
+    --invariant-color-dark: #00ba44;
+    --invariant-color-darker: #008622;
+
+    /* blockquote colors */
+    --blockquote-background: #f5f5f5;
+    --blockquote-foreground: #727272;
+
+    /* table colors */
+    --tablehead-background: #f1f1f1;
+    --tablehead-foreground: var(--page-foreground-color);
+
+    /* menu-display: block | none
+     * Visibility of the top navigation on screens >= 768px. On smaller screen the menu is always visible.
+     * `GENERATE_TREEVIEW` MUST be enabled!
+     */
+    --menu-display: block;
+
+    --menu-focus-foreground: var(--page-background-color);
+    --menu-focus-background: var(--primary-color);
+    --menu-selected-background: rgba(0, 0, 0, 0.05);
+
+    --header-background: var(--page-background-color);
+    --header-foreground: var(--page-foreground-color);
+
+    /* searchbar colors */
+    --searchbar-background: var(--side-nav-background);
+    --searchbar-foreground: var(--page-foreground-color);
+
+    /* searchbar size
+     * (`searchbar-width` is only applied on screens >= 768px.
+     * on smaller screens the searchbar will always fill the entire screen width) */
+    --searchbar-height: 33px;
+    --searchbar-width: 210px;
+
+    /* code block colors */
+    --code-background: #f5f5f5;
+    --code-foreground: var(--page-foreground-color);
+
+    /* fragment colors */
+    --fragment-background: #282c34;
+    --fragment-foreground: #ffffff;
+    --fragment-keyword: #cc99cd;
+    --fragment-keywordtype: #ab99cd;
+    --fragment-keywordflow: #e08000;
+    --fragment-token: #7ec699;
+    --fragment-comment: #999999;
+    --fragment-link: #98c0e3;
+    --fragment-preprocessor: #65cabe;
+    --fragment-linenumber-color: #cccccc;
+    --fragment-linenumber-background: #35393c;
+    --fragment-linenumber-border: #1f1f1f;
+    --fragment-lineheight: 20px;
+
+    /* sidebar navigation (treeview) colors */
+    --side-nav-background: #fbfbfb;
+    --side-nav-foreground: var(--page-foreground-color);
+    --side-nav-arrow-opacity: 0;
+    --side-nav-arrow-hover-opacity: 0.9;
+
+    /* height of an item in any tree / collapsable table */
+    --tree-item-height: 30px;
+
+    --darkmode-toggle-button-icon: "☀️";
+}
+
+@media screen and (max-width: 767px) {
+    html {
+        --page-font-size: 16px;
+        --navigation-font-size: 16px;
+        --code-font-size: 15px; /* affects code, fragment */
+        --title-font-size: 22px;
+    }
+}
+
+@media (prefers-color-scheme: dark) {
+    html:not(.light-mode) {
+        color-scheme: dark;
+
+        --primary-color: #1982d2;
+        --primary-dark-color: #5ca8e2;
+        --primary-light-color: #4779ac;
+        --primary-lighter-color: #191e21;
+        --primary-lightest-color: #191a1c;
+
+        --box-shadow: 0 2px 10px 0 rgba(0, 0, 0, 0.35);
+
+        --odd-color: rgba(0, 0, 0, 0.1);
+
+        --menu-selected-background: rgba(0, 0, 0, 0.4);
+
+        --page-background-color: #1c1d1f;
+        --page-foreground-color: #d2dbde;
+        --page-secondary-foreground-color: #859399;
+        --separator-color: #000000;
+        --side-nav-background: #252628;
+
+        --code-background: #2a2c2f;
+
+        --tablehead-background: #2a2c2f;
+
+        --blockquote-background: #1f2022;
+        --blockquote-foreground: #77848a;
+
+        --warning-color: #b61825;
+        --warning-color-dark: #510a02;
+        --warning-color-darker: #f5b1aa;
+        --note-color: rgb(255, 183, 0);
+        --note-color-dark: #9f7300;
+        --note-color-darker: #fff6df;
+        --deprecated-color: rgb(88, 90, 96);
+        --deprecated-color-dark: #262e37;
+        --deprecated-color-darker: #a0a5b0;
+        --bug-color: rgb(248, 113, 0);
+        --bug-color-dark: #812a00;
+        --bug-color-darker: #ffd3be;
+
+        --darkmode-toggle-button-icon: "🌛";
+    }
+}
+
+/* dark mode variables are defined twice, to support both the dark-mode without and with doxygen-awesome-darkmode-toggle.js */
+html.dark-mode {
+    color-scheme: dark;
+
+    --primary-color: #1982d2;
+    --primary-dark-color: #5ca8e2;
+    --primary-light-color: #4779ac;
+    --primary-lighter-color: #191e21;
+    --primary-lightest-color: #191a1c;
+
+    --box-shadow: 0 2px 10px 0 rgba(0, 0, 0, 0.35);
+
+    --odd-color: rgba(0, 0, 0, 0.1);
+
+    --menu-selected-background: rgba(0, 0, 0, 0.4);
+
+    --page-background-color: #1c1d1f;
+    --page-foreground-color: #d2dbde;
+    --page-secondary-foreground-color: #859399;
+    --separator-color: #000000;
+    --side-nav-background: #252628;
+
+    --code-background: #2a2c2f;
+
+    --tablehead-background: #2a2c2f;
+
+    --blockquote-background: #1f2022;
+    --blockquote-foreground: #77848a;
+
+    --warning-color: #b61825;
+    --warning-color-dark: #510a02;
+    --warning-color-darker: #f5b1aa;
+    --note-color: rgb(255, 183, 0);
+    --note-color-dark: #9f7300;
+    --note-color-darker: #fff6df;
+    --deprecated-color: rgb(88, 90, 96);
+    --deprecated-color-dark: #262e37;
+    --deprecated-color-darker: #a0a5b0;
+    --bug-color: rgb(248, 113, 0);
+    --bug-color-dark: #812a00;
+    --bug-color-darker: #ffd3be;
+
+    --darkmode-toggle-button-icon: "🌛";
+}
+
+body {
+    color: var(--page-foreground-color);
+    background-color: var(--page-background-color);
+    font-size: var(--page-font-size);
+}
+
+body,
+table,
+div,
+p,
+dl,
+#nav-tree .label,
+.title,
+.sm-dox a,
+.sm-dox a:hover,
+.sm-dox a:focus,
+#projectname,
+.SelectItem,
+#MSearchField,
+.navpath li.navelem a,
+.navpath li.navelem a:hover {
+    font-family: var(--font-family);
+}
+
+h1,
+h2,
+h3,
+h4,
+h5 {
+    margin-top: 0.9em;
+    font-weight: 600;
+    line-height: initial;
+}
+
+p,
+div,
+table,
+dl {
+    font-size: var(--page-font-size);
+}
+
+a:link,
+a:visited,
+a:hover,
+a:focus,
+a:active {
+    color: var(--primary-color) !important;
+    font-weight: 500;
+}
+
+/*
+ Title and top navigation
+ */
+
+#top {
+    background: var(--header-background);
+    border-bottom: 1px solid var(--separator-color);
+}
+
+@media screen and (min-width: 768px) {
+    #top {
+        display: flex;
+        flex-wrap: wrap;
+        justify-content: space-between;
+        align-items: center;
+    }
+}
+
+#main-nav {
+    flex-grow: 5;
+    padding: var(--spacing-small) var(--spacing-medium);
+}
+
+#titlearea {
+    width: auto;
+    padding: var(--spacing-medium) var(--spacing-large);
+    background: none;
+    color: var(--header-foreground);
+    border-bottom: none;
+}
+
+@media screen and (max-width: 767px) {
+    #titlearea {
+        padding-bottom: var(--spacing-small);
+    }
+}
+
+#titlearea table tbody tr {
+    height: auto !important;
+}
+
+#projectname {
+    font-size: var(--title-font-size);
+    font-weight: 600;
+}
+
+#projectnumber {
+    font-family: inherit;
+    font-size: 60%;
+}
+
+#projectbrief {
+    font-family: inherit;
+    font-size: 80%;
+}
+
+#projectlogo {
+    vertical-align: middle;
+}
+
+#projectlogo img {
+    max-height: calc(var(--title-font-size) * 2);
+    margin-right: var(--spacing-small);
+}
+
+.sm-dox,
+.tabs,
+.tabs2,
+.tabs3 {
+    background: none;
+    padding: 0;
+}
+
+.tabs,
+.tabs2,
+.tabs3 {
+    border-bottom: 1px solid var(--separator-color);
+    margin-bottom: -1px;
+}
+
+@media screen and (max-width: 767px) {
+    .sm-dox a span.sub-arrow {
+        background: var(--code-background);
+    }
+}
+
+@media screen and (min-width: 768px) {
+    .sm-dox li,
+    .tablist li {
+        display: var(--menu-display);
+    }
+
+    .sm-dox a span.sub-arrow {
+        border-color: var(--header-foreground) transparent transparent
+            transparent;
+    }
+
+    .sm-dox a:hover span.sub-arrow {
+        border-color: var(--menu-focus-foreground) transparent transparent
+            transparent;
+    }
+
+    .sm-dox ul a span.sub-arrow {
+        border-color: transparent transparent transparent
+            var(--page-foreground-color);
+    }
+
+    .sm-dox ul a:hover span.sub-arrow {
+        border-color: transparent transparent transparent
+            var(--menu-focus-foreground);
+    }
+}
+
+.sm-dox ul {
+    background: var(--page-background-color);
+    box-shadow: var(--box-shadow);
+    border: 1px solid var(--separator-color);
+    border-radius: var(--border-radius-medium) !important;
+    padding: var(--spacing-small);
+    animation: ease-out 150ms slideInMenu;
+}
+
+@keyframes slideInMenu {
+    from {
+        opacity: 0;
+        transform: translate(0px, -2px);
+    }
+
+    to {
+        opacity: 1;
+        transform: translate(0px, 0px);
+    }
+}
+
+.sm-dox ul a {
+    color: var(--page-foreground-color) !important;
+    background: var(--page-background-color);
+    font-size: var(--navigation-font-size);
+}
+
+.sm-dox > li > ul:after {
+    border-bottom-color: var(--page-background-color) !important;
+}
+
+.sm-dox > li > ul:before {
+    border-bottom-color: var(--separator-color) !important;
+}
+
+.sm-dox ul a:hover,
+.sm-dox ul a:active,
+.sm-dox ul a:focus {
+    font-size: var(--navigation-font-size) !important;
+    color: var(--menu-focus-foreground) !important;
+    text-shadow: none;
+    background-color: var(--menu-focus-background);
+    border-radius: var(--border-radius-small) !important;
+}
+
+.sm-dox a,
+.sm-dox a:focus,
+.tablist li,
+.tablist li a,
+.tablist li.current a {
+    text-shadow: none;
+    background: transparent;
+    background-image: none !important;
+    color: var(--header-foreground) !important;
+    font-weight: normal;
+    font-size: var(--navigation-font-size);
+}
+
+.sm-dox a:focus {
+    outline: auto;
+}
+
+.sm-dox a:hover,
+.sm-dox a:active,
+.tablist li a:hover {
+    text-shadow: none;
+    font-weight: normal;
+    background: var(--menu-focus-background);
+    color: var(--menu-focus-foreground) !important;
+    border-radius: var(--border-radius-small) !important;
+    font-size: var(--navigation-font-size);
+}
+
+.tablist li.current {
+    border-radius: var(--border-radius-small);
+    background: var(--menu-selected-background);
+}
+
+.tablist li {
+    margin: var(--spacing-small) 0 var(--spacing-small) var(--spacing-small);
+}
+
+.tablist a {
+    padding: 0 var(--spacing-large);
+}
+
+/*
+ Search box
+ */
+
+#MSearchBox {
+    height: var(--searchbar-height);
+    background: var(--searchbar-background);
+    border-radius: var(--searchbar-height);
+    border: 1px solid var(--separator-color);
+    overflow: hidden;
+    width: var(--searchbar-width);
+    position: relative;
+    box-shadow: none;
+    display: block;
+    margin-top: 0;
+}
+
+.left #MSearchSelect {
+    left: 0;
+}
+
+.tabs .left #MSearchSelect {
+    padding-left: 0;
+}
+
+.tabs #MSearchBox {
+    position: absolute;
+    right: var(--spacing-medium);
+}
+
+@media screen and (max-width: 767px) {
+    .tabs #MSearchBox {
+        position: relative;
+        right: 0;
+        margin-left: var(--spacing-medium);
+        margin-top: 0;
+    }
+}
+
+#MSearchSelectWindow,
+#MSearchResultsWindow {
+    z-index: 9999;
+}
+
+#MSearchBox.MSearchBoxActive {
+    border-color: var(--primary-color);
+    box-shadow: inset 0 0 0 1px var(--primary-color);
+}
+
+#main-menu > li:last-child {
+    margin-right: 0;
+}
+
+@media screen and (max-width: 767px) {
+    #main-menu > li:last-child {
+        height: 50px;
+    }
+}
+
+#MSearchField {
+    font-size: var(--navigation-font-size);
+    height: calc(var(--searchbar-height) - 2px);
+    background: transparent;
+    width: calc(var(--searchbar-width) - 64px);
+}
+
+.MSearchBoxActive #MSearchField {
+    color: var(--searchbar-foreground);
+}
+
+#MSearchSelect {
+    top: calc(calc(var(--searchbar-height) / 2) - 11px);
+}
+
+.left #MSearchSelect {
+    padding-left: 8px;
+}
+
+#MSearchBox span.left,
+#MSearchBox span.right {
+    background: none;
+}
+
+#MSearchBox span.right {
+    padding-top: calc(calc(var(--searchbar-height) / 2) - 12px);
+    position: absolute;
+    right: var(--spacing-small);
+}
+
+.tabs #MSearchBox span.right {
+    top: calc(calc(var(--searchbar-height) / 2) - 12px);
+}
+
+@keyframes slideInSearchResults {
+    from {
+        opacity: 0;
+        transform: translate(0, 15px);
+    }
+
+    to {
+        opacity: 1;
+        transform: translate(0, 20px);
+    }
+}
+
+#MSearchResultsWindow {
+    left: auto !important;
+    right: var(--spacing-medium);
+    border-radius: var(--border-radius-large);
+    border: 1px solid var(--separator-color);
+    transform: translate(0, 20px);
+    box-shadow: var(--box-shadow);
+    animation: ease-out 280ms slideInSearchResults;
+    background: var(--page-background-color);
+}
+
+iframe#MSearchResults {
+    margin: 4px;
+}
+
+@media (prefers-color-scheme: dark) {
+    html:not(.light-mode) iframe#MSearchResults {
+        filter: invert() hue-rotate(180deg);
+    }
+}
+
+html.dark-mode iframe#MSearchResults {
+    filter: invert() hue-rotate(180deg);
+}
+
+#MSearchSelectWindow {
+    border: 1px solid var(--separator-color);
+    border-radius: var(--border-radius-medium);
+    box-shadow: var(--box-shadow);
+    background: var(--page-background-color);
+}
+
+#MSearchSelectWindow a.SelectItem {
+    font-size: var(--navigation-font-size);
+    line-height: var(--content-line-height);
+    margin: 0 var(--spacing-small);
+    border-radius: var(--border-radius-small);
+    color: var(--page-foreground-color) !important;
+    font-weight: normal;
+}
+
+#MSearchSelectWindow a.SelectItem:hover {
+    background: var(--menu-focus-background);
+    color: var(--menu-focus-foreground) !important;
+}
+
+@media screen and (max-width: 767px) {
+    #MSearchBox {
+        margin-top: var(--spacing-medium);
+        margin-bottom: var(--spacing-medium);
+        width: calc(100vw - 30px);
+    }
+
+    #main-menu > li:last-child {
+        float: none !important;
+    }
+
+    #MSearchField {
+        width: calc(100vw - 110px);
+    }
+
+    @keyframes slideInSearchResultsMobile {
+        from {
+            opacity: 0;
+            transform: translate(0, 15px);
+        }
+
+        to {
+            opacity: 1;
+            transform: translate(0, 20px);
+        }
+    }
+
+    #MSearchResultsWindow {
+        left: var(--spacing-medium) !important;
+        right: var(--spacing-medium);
+        overflow: auto;
+        transform: translate(0, 20px);
+        animation: ease-out 280ms slideInSearchResultsMobile;
+    }
+}
+
+/*
+ Tree view
+ */
+
+#side-nav {
+    padding: 0 !important;
+    background: var(--side-nav-background);
+}
+
+@media screen and (max-width: 767px) {
+    #side-nav {
+        display: none;
+    }
+
+    #doc-content {
+        margin-left: 0 !important;
+        height: auto !important;
+        padding-bottom: calc(2 * var(--spacing-large));
+    }
+}
+
+#nav-tree {
+    background: transparent;
+}
+
+#nav-tree .label {
+    font-size: var(--navigation-font-size);
+}
+
+#nav-tree .item {
+    height: var(--tree-item-height);
+    line-height: var(--tree-item-height);
+}
+
+#nav-sync {
+    top: 12px !important;
+    right: 12px;
+}
+
+#nav-tree .selected {
+    text-shadow: none;
+    background-image: none;
+    background-color: transparent;
+    box-shadow: inset 4px 0 0 0 var(--primary-color);
+}
+
+#nav-tree a {
+    color: var(--side-nav-foreground) !important;
+    font-weight: normal;
+}
+
+#nav-tree a:focus {
+    outline-style: auto;
+}
+
+#nav-tree .arrow {
+    opacity: var(--side-nav-arrow-opacity);
+}
+
+.arrow {
+    color: inherit;
+    cursor: pointer;
+    font-size: 45%;
+    vertical-align: middle;
+    margin-right: 2px;
+    font-family: serif;
+    height: auto;
+    text-align: right;
+}
+
+#nav-tree div.item:hover .arrow,
+#nav-tree a:focus .arrow {
+    opacity: var(--side-nav-arrow-hover-opacity);
+}
+
+#nav-tree .selected a {
+    color: var(--primary-color) !important;
+    font-weight: bolder;
+    font-weight: 600;
+}
+
+.ui-resizable-e {
+    background: var(--separator-color);
+    width: 1px;
+}
+
+/*
+ Contents
+ */
+
+div.header {
+    border-bottom: 1px solid var(--separator-color);
+    background-color: var(--page-background-color);
+    background-image: none;
+}
+
+div.contents,
+div.header .title,
+div.header .summary {
+    max-width: var(--content-maxwidth);
+}
+
+div.contents,
+div.header .title {
+    line-height: initial;
+    margin: calc(var(--spacing-medium) + 0.2em) auto var(--spacing-medium) auto;
+}
+
+div.header .summary {
+    margin: var(--spacing-medium) auto 0 auto;
+}
+
+div.headertitle {
+    padding: 0;
+}
+
+div.header .title {
+    font-weight: 600;
+    font-size: 210%;
+    padding: var(--spacing-medium) var(--spacing-large);
+    word-break: break-word;
+}
+
+div.header .summary {
+    width: auto;
+    display: block;
+    float: none;
+    padding: 0 var(--spacing-large);
+}
+
+td.memSeparator {
+    border-color: var(--separator-color);
+}
+
+.mdescLeft,
+.mdescRight,
+.memItemLeft,
+.memItemRight,
+.memTemplItemLeft,
+.memTemplItemRight,
+.memTemplParams {
+    background: var(--code-background);
+}
+
+.mdescRight {
+    color: var(--page-secondary-foreground-color);
+}
+
+span.mlabel {
+    background: var(--primary-color);
+    border: none;
+    padding: 4px 9px;
+    border-radius: 12px;
+    margin-right: var(--spacing-medium);
+}
+
+span.mlabel:last-of-type {
+    margin-right: 2px;
+}
+
+div.contents {
+    padding: 0 var(--spacing-large);
+}
+
+div.contents p,
+div.contents li {
+    line-height: var(--content-line-height);
+}
+
+div.contents div.dyncontent {
+    margin: var(--spacing-medium) 0;
+}
+
+@media (prefers-color-scheme: dark) {
+    html:not(.light-mode) div.contents div.dyncontent img,
+    html:not(.light-mode) div.contents center img,
+    html:not(.light-mode) div.contents table img,
+    html:not(.light-mode) div.contents div.dyncontent iframe,
+    html:not(.light-mode) div.contents center iframe,
+    html:not(.light-mode) div.contents table iframe {
+        filter: hue-rotate(180deg) invert();
+    }
+}
+
+html.dark-mode div.contents div.dyncontent img,
+html.dark-mode div.contents center img,
+html.dark-mode div.contents table img,
+html.dark-mode div.contents div.dyncontent iframe,
+html.dark-mode div.contents center iframe,
+html.dark-mode div.contents table iframe {
+    filter: hue-rotate(180deg) invert();
+}
+
+h2.groupheader {
+    border-bottom: 1px solid var(--separator-color);
+    color: var(--page-foreground-color);
+}
+
+blockquote {
+    padding: var(--spacing-small) var(--spacing-medium);
+    background: var(--blockquote-background);
+    color: var(--blockquote-foreground);
+    border-left: 2px solid var(--blockquote-foreground);
+    margin: 0;
+}
+
+blockquote p {
+    margin: var(--spacing-small) 0 var(--spacing-medium) 0;
+}
+.paramname {
+    font-weight: 600;
+    color: var(--primary-dark-color);
+}
+
+.glow {
+    text-shadow: 0 0 15px var(--primary-light-color) !important;
+}
+
+.alphachar a {
+    color: var(--page-foreground-color);
+}
+
+/*
+ Table of Contents
+ */
+
+div.toc {
+    background-color: var(--side-nav-background);
+    border: 1px solid var(--separator-color);
+    border-radius: var(--border-radius-medium);
+    box-shadow: var(--box-shadow);
+    padding: 0 var(--spacing-large);
+    margin: 0 0 var(--spacing-medium) var(--spacing-medium);
+}
+
+div.toc h3 {
+    color: var(--side-nav-foreground);
+    font-size: var(--navigation-font-size);
+    margin: var(--spacing-large) 0;
+}
+
+div.toc li {
+    font-size: var(--navigation-font-size);
+    padding: 0;
+    background: none;
+}
+
+div.toc li:before {
+    content: "↓";
+    font-weight: 800;
+    font-family: var(--font-family);
+    margin-right: var(--spacing-small);
+    color: var(--side-nav-foreground);
+    opacity: 0.4;
+}
+
+div.toc ul li.level1 {
+    margin: 0;
+}
+
+div.toc ul li.level2,
+div.toc ul li.level3 {
+    margin-top: 0;
+}
+
+@media screen and (max-width: 767px) {
+    div.toc {
+        float: none;
+        width: auto;
+        margin: 0 0 var(--spacing-medium) 0;
+    }
+}
+
+/*
+ Code & Fragments
+ */
+
+code,
+div.fragment,
+pre.fragment {
+    border-radius: var(--border-radius-small);
+    border: none;
+    overflow: hidden;
+}
+
+code {
+    display: inline;
+    background: var(--code-background);
+    color: var(--code-foreground);
+    padding: 2px 6px;
+    word-break: break-word;
+}
+
+div.fragment,
+pre.fragment {
+    margin: var(--spacing-medium) 0;
+    padding: 14px 16px;
+    background: var(--fragment-background);
+    color: var(--fragment-foreground);
+    overflow-x: auto;
+}
+
+@media screen and (max-width: 767px) {
+    div.fragment,
+    pre.fragment {
+        border-top-right-radius: 0;
+        border-bottom-right-radius: 0;
+    }
+
+    .contents > div.fragment,
+    .textblock > div.fragment,
+    .textblock > pre.fragment {
+        margin: var(--spacing-medium) calc(0px - var(--spacing-large));
+        border-radius: 0;
+    }
+
+    .textblock li > .fragment {
+        margin: var(--spacing-medium) calc(0px - var(--spacing-large));
+    }
+
+    .memdoc li > .fragment {
+        margin: var(--spacing-medium) calc(0px - var(--spacing-medium));
+    }
+
+    .memdoc > div.fragment,
+    .memdoc > pre.fragment,
+    dl dd > div.fragment,
+    dl dd pre.fragment {
+        margin: var(--spacing-medium) calc(0px - var(--spacing-medium));
+        border-radius: 0;
+    }
+}
+
+code,
+code a,
+pre.fragment,
+div.fragment,
+div.fragment .line,
+div.fragment span,
+div.fragment .line a,
+div.fragment .line span {
+    font-family: var(--font-family-monospace);
+    font-size: var(--code-font-size) !important;
+}
+
+div.line:after {
+    margin-right: var(--spacing-medium);
+}
+
+div.fragment .line,
+pre.fragment {
+    white-space: pre;
+    word-wrap: initial;
+    line-height: var(--fragment-lineheight);
+}
+
+div.fragment span.keyword {
+    color: var(--fragment-keyword);
+}
+
+div.fragment span.keywordtype {
+    color: var(--fragment-keywordtype);
+}
+
+div.fragment span.keywordflow {
+    color: var(--fragment-keywordflow);
+}
+
+div.fragment span.stringliteral {
+    color: var(--fragment-token);
+}
+
+div.fragment span.comment {
+    color: var(--fragment-comment);
+}
+
+div.fragment a.code {
+    color: var(--fragment-link) !important;
+}
+
+div.fragment span.preprocessor {
+    color: var(--fragment-preprocessor);
+}
+
+div.fragment span.lineno {
+    display: inline-block;
+    width: 27px;
+    border-right: none;
+    background: var(--fragment-linenumber-background);
+    color: var(--fragment-linenumber-color);
+}
+
+div.fragment span.lineno a {
+    background: none;
+    color: var(--fragment-link) !important;
+}
+
+div.fragment .line:first-child .lineno {
+    box-shadow: -999999px 0px 0 999999px var(--fragment-linenumber-background),
+        -999998px 0px 0 999999px var(--fragment-linenumber-border);
+}
+
+/*
+ dl warning, attention, note, deprecated, bug, ...
+ */
+
+dl.warning,
+dl.attention,
+dl.note,
+dl.deprecated,
+dl.bug,
+dl.invariant,
+dl.pre {
+    padding: var(--spacing-medium);
+    margin: var(--spacing-medium) 0;
+    color: var(--page-background-color);
+    overflow: hidden;
+    margin-left: 0;
+    border-radius: var(--border-radius-small);
+}
+
+dl.section dd {
+    margin-bottom: 2px;
+}
+
+dl.warning,
+dl.attention {
+    background: var(--warning-color);
+    border-left: 8px solid var(--warning-color-dark);
+    color: var(--warning-color-darker);
+}
+
+dl.warning dt,
+dl.attention dt {
+    color: var(--warning-color-dark);
+}
+
+dl.note {
+    background: var(--note-color);
+    border-left: 8px solid var(--note-color-dark);
+    color: var(--note-color-darker);
+}
+
+dl.note dt {
+    color: var(--note-color-dark);
+}
+
+dl.bug {
+    background: var(--bug-color);
+    border-left: 8px solid var(--bug-color-dark);
+    color: var(--bug-color-darker);
+}
+
+dl.bug dt a {
+    color: var(--bug-color-dark) !important;
+}
+
+dl.deprecated {
+    background: var(--deprecated-color);
+    border-left: 8px solid var(--deprecated-color-dark);
+    color: var(--deprecated-color-darker);
+}
+
+dl.deprecated dt a {
+    color: var(--deprecated-color-dark) !important;
+}
+
+dl.section dd,
+dl.bug dd,
+dl.deprecated dd {
+    margin-inline-start: 0px;
+}
+
+dl.invariant,
+dl.pre {
+    background: var(--invariant-color);
+    border-left: 8px solid var(--invariant-color-dark);
+    color: var(--invariant-color-darker);
+}
+
+/*
+ memitem
+ */
+
+div.memdoc,
+div.memproto,
+h2.memtitle {
+    box-shadow: none;
+    background-image: none;
+    border: none;
+}
+
+div.memdoc {
+    padding: 0 var(--spacing-medium);
+    background: var(--page-background-color);
+}
+
+h2.memtitle,
+div.memitem {
+    border: 1px solid var(--separator-color);
+}
+
+div.memproto,
+h2.memtitle {
+    background: var(--code-background);
+    text-shadow: none;
+}
+
+h2.memtitle {
+    font-weight: 500;
+    font-family: monospace, fixed;
+    border-bottom: none;
+    border-top-left-radius: var(--border-radius-medium);
+    border-top-right-radius: var(--border-radius-medium);
+    word-break: break-all;
+}
+
+a:target + h2.memtitle,
+a:target + h2.memtitle + div.memitem {
+    border-color: var(--primary-light-color);
+}
+
+a:target + h2.memtitle {
+    box-shadow: -3px -3px 3px 0 var(--primary-lightest-color),
+        3px -3px 3px 0 var(--primary-lightest-color);
+}
+
+a:target + h2.memtitle + div.memitem {
+    box-shadow: 0 0 10px 0 var(--primary-lighter-color);
+}
+
+div.memitem {
+    border-top-right-radius: var(--border-radius-medium);
+    border-bottom-right-radius: var(--border-radius-medium);
+    border-bottom-left-radius: var(--border-radius-medium);
+    overflow: hidden;
+    display: block !important;
+}
+
+div.memdoc {
+    border-radius: 0;
+}
+
+div.memproto {
+    border-radius: 0 var(--border-radius-small) 0 0;
+    overflow: auto;
+    border-bottom: 1px solid var(--separator-color);
+    padding: var(--spacing-medium);
+    margin-bottom: -1px;
+}
+
+div.memtitle {
+    border-top-right-radius: var(--border-radius-medium);
+    border-top-left-radius: var(--border-radius-medium);
+}
+
+div.memproto table.memname {
+    font-family: monospace, fixed;
+    color: var(--page-foreground-color);
+}
+
+table.mlabels,
+table.mlabels > tbody {
+    display: block;
+}
+
+td.mlabels-left {
+    width: auto;
+}
+
+table.mlabels > tbody > tr:first-child {
+    display: flex;
+    justify-content: space-between;
+    flex-wrap: wrap;
+}
+
+.memname,
+.memitem span.mlabels {
+    margin: 0;
+}
+
+/*
+ reflist
+ */
+
+dl.reflist {
+    box-shadow: var(--box-shadow);
+    border-radius: var(--border-radius-medium);
+    border: 1px solid var(--separator-color);
+    overflow: hidden;
+    padding: 0;
+}
+
+dl.reflist dt,
+dl.reflist dd {
+    box-shadow: none;
+    text-shadow: none;
+    background-image: none;
+    border: none;
+    padding: 12px;
+}
+
+dl.reflist dt {
+    font-weight: 500;
+    border-radius: 0;
+    background: var(--code-background);
+    border-bottom: 1px solid var(--separator-color);
+    color: var(--page-foreground-color);
+}
+
+dl.reflist dd {
+    background: none;
+}
+
+/*
+ Table
+ */
+
+table.markdownTable,
+table.fieldtable {
+    width: 100%;
+    border: 1px solid var(--separator-color);
+    margin: var(--spacing-medium) 0;
+}
+
+table.fieldtable {
+    box-shadow: none;
+    border-radius: var(--border-radius-small);
+}
+
+th.markdownTableHeadLeft,
+th.markdownTableHeadRight,
+th.markdownTableHeadCenter,
+th.markdownTableHeadNone {
+    background: var(--tablehead-background);
+    color: var(--tablehead-foreground);
+    font-weight: 600;
+    font-size: var(--page-font-size);
+}
+
+table.markdownTable td,
+table.markdownTable th,
+table.fieldtable dt {
+    border: 1px solid var(--separator-color);
+    padding: var(--spacing-small) var(--spacing-medium);
+}
+
+table.fieldtable th {
+    font-size: var(--page-font-size);
+    font-weight: 600;
+    background-image: none;
+    background-color: var(--tablehead-background);
+    color: var(--tablehead-foreground);
+    border-bottom: 1px solid var(--separator-color);
+}
+
+.fieldtable td.fieldtype,
+.fieldtable td.fieldname {
+    border-bottom: 1px solid var(--separator-color);
+    border-right: 1px solid var(--separator-color);
+}
+
+.fieldtable td.fielddoc {
+    border-bottom: 1px solid var(--separator-color);
+}
+
+.memberdecls td.glow,
+.fieldtable tr.glow {
+    background-color: var(--primary-light-color);
+    box-shadow: 0 0 15px var(--primary-lighter-color);
+}
+
+table.memberdecls {
+    display: block;
+    overflow-x: auto;
+    overflow-y: hidden;
+}
+
+/*
+ Horizontal Rule
+ */
+
+hr {
+    margin-top: var(--spacing-large);
+    margin-bottom: var(--spacing-large);
+    border-top: 1px solid var(--separator-color);
+}
+
+.contents hr {
+    box-shadow: var(--content-maxwidth) 0 0 0 var(--separator-color),
+        calc(0px - var(--content-maxwidth)) 0 0 0 var(--separator-color);
+}
+
+.contents img {
+    max-width: 100%;
+}
+
+/*
+ Directories
+ */
+div.directory {
+    border-top: 1px solid var(--separator-color);
+    border-bottom: 1px solid var(--separator-color);
+    width: auto;
+}
+
+table.directory {
+    font-family: var(--font-family);
+    font-size: var(--page-font-size);
+    font-weight: normal;
+}
+
+.directory td.entry {
+    padding: var(--spacing-small);
+    display: flex;
+    align-items: center;
+}
+
+.directory tr.even {
+    background-color: var(--odd-color);
+}
+
+.icona {
+    width: auto;
+    height: auto;
+    margin: 0 var(--spacing-small);
+}
+
+.icon {
+    background: var(--primary-color);
+    width: 18px;
+    height: 18px;
+    line-height: 18px;
+}
+
+.iconfopen,
+.icondoc,
+.iconfclosed {
+    background-position: center;
+    margin-bottom: 0;
+}
+
+.icondoc {
+    filter: saturate(0.2);
+}
+
+@media screen and (max-width: 767px) {
+    div.directory {
+        margin-left: calc(0px - var(--spacing-medium));
+        margin-right: calc(0px - var(--spacing-medium));
+    }
+}
+
+@media (prefers-color-scheme: dark) {
+    html:not(.light-mode) .iconfopen,
+    html:not(.light-mode) .iconfclosed {
+        filter: hue-rotate(180deg) invert();
+    }
+}
+
+html.dark-mode .iconfopen,
+html.dark-mode .iconfclosed {
+    filter: hue-rotate(180deg) invert();
+}
+
+/*
+ Class list
+ */
+
+.classindex dl.odd {
+    background: var(--odd-color);
+    border-radius: var(--border-radius-small);
+}
+
+@media screen and (max-width: 767px) {
+    .classindex {
+        margin: 0 calc(0px - var(--spacing-small));
+    }
+}
+
+/*
+  Footer and nav-path
+ */
+
+#nav-path {
+    margin-bottom: -1px;
+    width: 100%;
+}
+
+#nav-path ul {
+    background-image: none;
+    background: var(--page-background-color);
+    border: none;
+    border-top: 1px solid var(--separator-color);
+    border-bottom: 1px solid var(--separator-color);
+    font-size: var(--navigation-font-size);
+}
+
+img.footer {
+    width: 60px;
+}
+
+.navpath li.footer {
+    color: var(--page-secondary-foreground-color);
+}
+
+address.footer {
+    margin-bottom: var(--spacing-large);
+}
+
+#nav-path li.navelem {
+    background-image: none;
+    display: flex;
+    align-items: center;
+}
+
+.navpath li.navelem a {
+    text-shadow: none;
+    display: inline-block;
+    color: var(--primary-color) !important;
+}
+
+.navpath li.navelem b {
+    color: var(--primary-dark-color);
+    font-weight: 500;
+}
+
+li.navelem {
+    padding: 0;
+    margin-left: -8px;
+}
+
+li.navelem:first-child {
+    margin-left: var(--spacing-large);
+}
+
+li.navelem:first-child:before {
+    display: none;
+}
+
+#nav-path li.navelem:after {
+    content: "";
+    border: 5px solid var(--page-background-color);
+    border-bottom-color: transparent;
+    border-right-color: transparent;
+    border-top-color: transparent;
+    transform: scaleY(4.2);
+    z-index: 10;
+    margin-left: 6px;
+}
+
+#nav-path li.navelem:before {
+    content: "";
+    border: 5px solid var(--separator-color);
+    border-bottom-color: transparent;
+    border-right-color: transparent;
+    border-top-color: transparent;
+    transform: scaleY(3.2);
+    margin-right: var(--spacing-small);
+}
+
+.navpath li.navelem a:hover {
+    color: var(--primary-color);
+}
+
+/*
+  Optional Dark mode toggle button
+*/
+
+doxygen-awesome-dark-mode-toggle {
+    margin: 0 0 0 var(--spacing-small);
+    padding: 0;
+    width: var(--searchbar-height);
+    height: var(--searchbar-height);
+    background: none;
+    border: none;
+    font-size: 23px;
+    border-radius: var(--border-radius-medium);
+    vertical-align: middle;
+    text-align: center;
+    line-height: var(--searchbar-height);
+}
+
+doxygen-awesome-dark-mode-toggle:hover {
+    background: var(--separator-color);
+}
+
+doxygen-awesome-dark-mode-toggle:after {
+    content: var(--darkmode-toggle-button-icon);
+}
+
+/**
+
+Doxygen Awesome
+https://github.com/jothepro/doxygen-awesome-css
+
+MIT License
+
+Copyright (c) 2021 jothepro
+
+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.
+
+ */
+
+html {
+    /* side nav width. MUST be = `TREEVIEW_WIDTH`.
+     * Make sure it is wide enought to contain the page title (logo + title + version)
+     */
+    --side-nav-fixed-width: 340px;
+    --menu-display: none;
+
+    --top-height: 120px;
+}
+
+@media screen and (min-width: 768px) {
+    html {
+        --searchbar-background: var(--page-background-color);
+    }
+
+    #side-nav {
+        min-width: var(--side-nav-fixed-width);
+        max-width: var(--side-nav-fixed-width);
+        top: var(--top-height);
+    }
+
+    #nav-tree,
+    #side-nav {
+        height: calc(100vh - var(--top-height)) !important;
+    }
+
+    #nav-tree {
+        padding: 0;
+    }
+
+    #top {
+        display: block;
+        border-bottom: none;
+        height: var(--top-height);
+        margin-bottom: calc(0px - var(--top-height));
+        max-width: var(--side-nav-fixed-width);
+        background: var(--side-nav-background);
+    }
+    #main-nav {
+        float: left;
+        padding-right: 0;
+    }
+
+    .ui-resizable-handle {
+        cursor: default;
+        width: 1px !important;
+        box-shadow: 0 calc(-2 * var(--top-height)) 0 0 var(--separator-color);
+    }
+
+    #nav-path {
+        position: fixed;
+        right: 0;
+        left: var(--side-nav-fixed-width);
+        bottom: 0;
+        width: auto;
+    }
+
+    #doc-content {
+        height: calc(100vh - 31px) !important;
+        padding-bottom: calc(3 * var(--spacing-large));
+        padding-top: calc(var(--top-height) - 80px);
+        box-sizing: border-box;
+        margin-left: var(--side-nav-fixed-width) !important;
+    }
+
+    #MSearchBox {
+        width: calc(
+            var(--side-nav-fixed-width) - calc(2 * var(--spacing-medium))
+        );
+    }
+
+    #MSearchField {
+        width: calc(
+            var(--side-nav-fixed-width) - calc(2 * var(--spacing-medium)) - 65px
+        );
+    }
+
+    #MSearchResultsWindow {
+        left: var(--spacing-medium) !important;
+        right: auto;
+    }
+}
diff --git a/third_party/allwpilib/glass/.styleguide b/third_party/allwpilib/glass/.styleguide
new file mode 100644
index 0000000..d555d5a
--- /dev/null
+++ b/third_party/allwpilib/glass/.styleguide
@@ -0,0 +1,31 @@
+cppHeaderFileInclude {
+  \.h$
+  \.inc$
+  \.inl$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+generatedFileExclude {
+  src/app/native/resources/
+  src/app/native/win/glass.ico
+  src/app/native/mac/glass.icns
+}
+
+repoRootNameOverride {
+  glass
+}
+
+includeOtherLibs {
+  ^GLFW
+  ^cscore
+  ^fmt/
+  ^frc/
+  ^imgui
+  ^ntcore
+  ^wpi/
+  ^wpigui
+  ^wpimath/
+}
diff --git a/third_party/allwpilib/glass/CMakeLists.txt b/third_party/allwpilib/glass/CMakeLists.txt
new file mode 100644
index 0000000..41a7819
--- /dev/null
+++ b/third_party/allwpilib/glass/CMakeLists.txt
@@ -0,0 +1,84 @@
+project(glass)
+
+include(CompileWarnings)
+include(GenResources)
+include(LinkMacOSGUI)
+
+#
+# libglass
+#
+file(GLOB_RECURSE libglass_src src/lib/native/cpp/*.cpp)
+
+add_library(libglass STATIC ${libglass_src})
+set_target_properties(libglass PROPERTIES DEBUG_POSTFIX "d" OUTPUT_NAME "glass")
+set_property(TARGET libglass PROPERTY POSITION_INDEPENDENT_CODE ON)
+
+set_property(TARGET libglass PROPERTY FOLDER "libraries")
+
+wpilib_target_warnings(libglass)
+target_link_libraries(libglass PUBLIC wpigui wpimath wpiutil)
+
+target_include_directories(libglass PUBLIC
+                           $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/lib/native/include>
+                           $<INSTALL_INTERFACE:${include_dest}/glass>)
+
+install(TARGETS libglass EXPORT libglass DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/lib/native/include/ DESTINATION "${include_dest}/glass")
+
+#
+# libglassnt
+#
+file(GLOB_RECURSE libglassnt_src src/libnt/native/cpp/*.cpp)
+
+add_library(libglassnt STATIC ${libglassnt_src})
+set_target_properties(libglassnt PROPERTIES DEBUG_POSTFIX "d" OUTPUT_NAME "glassnt")
+set_property(TARGET libglassnt PROPERTY POSITION_INDEPENDENT_CODE ON)
+
+set_property(TARGET libglassnt PROPERTY FOLDER "libraries")
+
+wpilib_target_warnings(libglassnt)
+target_link_libraries(libglassnt PUBLIC ntcore libglass)
+
+target_include_directories(libglassnt PUBLIC
+                           $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/libnt/native/include>
+                           $<INSTALL_INTERFACE:${include_dest}/glass>)
+
+install(TARGETS libglassnt EXPORT libglassnt DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/libnt/native/include/ DESTINATION "${include_dest}/glass")
+
+#
+# glass application
+#
+
+configure_file(src/app/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
+GENERATE_RESOURCES(src/app/native/resources generated/app/cpp GLASS glass glass_resources_src)
+
+file(GLOB glass_src src/app/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
+
+if (WIN32)
+    set(glass_rc src/app/native/win/glass.rc)
+elseif(APPLE)
+    set(MACOSX_BUNDLE_ICON_FILE glass.icns)
+    set(APP_ICON_MACOSX src/app/native/mac/glass.icns)
+    set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
+endif()
+
+add_executable(glass ${glass_src} ${glass_resources_src} ${glass_rc} ${APP_ICON_MACOSX})
+wpilib_link_macos_gui(glass)
+target_link_libraries(glass libglassnt libglass)
+
+if (WIN32)
+    set_target_properties(glass PROPERTIES WIN32_EXECUTABLE YES)
+elseif(APPLE)
+    set_target_properties(glass PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "Glass")
+endif()
+
+#if (MSVC OR FLAT_INSTALL_WPILIB)
+#    set (wpigui_config_dir ${wpilib_dest})
+#else()
+#    set (wpigui_config_dir share/wpigui)
+#endif()
+
+#configure_file(wpigui-config.cmake.in ${CMAKE_BINARY_DIR}/wpigui-config.cmake )
+#install(FILES ${CMAKE_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir})
+#install(EXPORT wpigui DESTINATION ${wpigui_config_dir})
diff --git a/third_party/allwpilib/glass/Info.plist b/third_party/allwpilib/glass/Info.plist
new file mode 100644
index 0000000..4a198fc
--- /dev/null
+++ b/third_party/allwpilib/glass/Info.plist
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleName</key>
+	<string>Glass</string>
+	<key>CFBundleExecutable</key>
+	<string>glass</string>
+	<key>CFBundleDisplayName</key>
+	<string>Glass</string>
+	<key>CFBundleIdentifier</key>
+	<string>edu.wpi.first.tools.Glass</string>
+	<key>CFBundleIconFile</key>
+	<string>glass.icns</string>
+	<key>CFBundlePackageType</key>
+	<string>APPL</string>
+	<key>CFBundleSupportedPlatforms</key>
+	<array>
+		<string>MacOSX</string>
+	</array>
+	<key>CFBundleInfoDictionaryVersion</key>
+	<string>6.0</string>
+	<key>CFBundleShortVersionString</key>
+	<string>2021</string>
+	<key>CFBundleVersion</key>
+	<string>2021</string>
+	<key>LSMinimumSystemVersion</key>
+	<string>10.11</string>
+	<key>NSHighResolutionCapable</key>
+	<true/>
+</dict>
+</plist>
diff --git a/third_party/allwpilib/glass/build.gradle b/third_party/allwpilib/glass/build.gradle
new file mode 100644
index 0000000..ad977ad
--- /dev/null
+++ b/third_party/allwpilib/glass/build.gradle
@@ -0,0 +1,208 @@
+import org.gradle.internal.os.OperatingSystem
+
+if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+
+    description = "A different kind of dashboard"
+
+    apply plugin: 'cpp'
+    apply plugin: 'c'
+    apply plugin: 'google-test-test-suite'
+    apply plugin: 'visual-studio'
+    apply plugin: 'edu.wpi.first.NativeUtils'
+
+    if (OperatingSystem.current().isWindows()) {
+        apply plugin: 'windows-resources'
+    }
+
+    ext {
+        nativeName = 'glass'
+    }
+
+    apply from: "${rootDir}/shared/resources.gradle"
+    apply from: "${rootDir}/shared/config.gradle"
+
+    def wpilibVersionFileInput = file("src/app/generate/WPILibVersion.cpp.in")
+    def wpilibVersionFileOutput = file("$buildDir/generated/app/cpp/WPILibVersion.cpp")
+
+    task generateCppVersion() {
+        description = 'Generates the wpilib version class'
+        group = 'WPILib'
+
+        outputs.file wpilibVersionFileOutput
+        inputs.file wpilibVersionFileInput
+
+        if (wpilibVersioning.releaseMode) {
+            outputs.upToDateWhen { false }
+        }
+
+        // We follow a simple set of checks to determine whether we should generate a new version file:
+        // 1. If the release type is not development, we generate a new version file
+        // 2. If there is no generated version number, we generate a new version file
+        // 3. If there is a generated build number, and the release type is development, then we will
+        //    only generate if the publish task is run.
+        doLast {
+            def version = wpilibVersioning.version.get()
+            println "Writing version ${version} to $wpilibVersionFileOutput"
+
+            if (wpilibVersionFileOutput.exists()) {
+                wpilibVersionFileOutput.delete()
+            }
+            def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
+            wpilibVersionFileOutput.write(read)
+        }
+    }
+
+    gradle.taskGraph.addTaskExecutionGraphListener { graph ->
+        def willPublish = graph.hasTask(publish)
+        if (willPublish) {
+            generateCppVersion.outputs.upToDateWhen { false }
+        }
+    }
+
+    def generateTask = createGenerateResourcesTask('app', 'GLASS', 'glass', project)
+
+    project(':').libraryBuild.dependsOn build
+    tasks.withType(CppCompile) {
+        dependsOn generateTask
+        dependsOn generateCppVersion
+    }
+
+    nativeUtils.exportsConfigs {
+        glass {
+            x86ExcludeSymbols = [
+                '_CT??_R0?AV_System_error',
+                '_CT??_R0?AVexception',
+                '_CT??_R0?AVfailure',
+                '_CT??_R0?AVruntime_error',
+                '_CT??_R0?AVsystem_error',
+                '_CTA5?AVfailure',
+                '_TI5?AVfailure',
+                '_CT??_R0?AVout_of_range',
+                '_CTA3?AVout_of_range',
+                '_TI3?AVout_of_range',
+                '_CT??_R0?AVbad_cast'
+            ]
+            x64ExcludeSymbols = [
+                '_CT??_R0?AV_System_error',
+                '_CT??_R0?AVexception',
+                '_CT??_R0?AVfailure',
+                '_CT??_R0?AVruntime_error',
+                '_CT??_R0?AVsystem_error',
+                '_CTA5?AVfailure',
+                '_TI5?AVfailure',
+                '_CT??_R0?AVout_of_range',
+                '_CTA3?AVout_of_range',
+                '_TI3?AVout_of_range',
+                '_CT??_R0?AVbad_cast'
+            ]
+        }
+    }
+
+    model {
+        components {
+            "${nativeName}"(NativeLibrarySpec) {
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/lib/native/cpp'
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/lib/native/include'
+                        }
+                    }
+                }
+                binaries.all {
+                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                        it.buildable = false
+                        return
+                    }
+                    if (it instanceof SharedLibraryBinarySpec) {
+                        it.buildable = false
+                        return
+                    }
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                    lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                }
+                appendDebugPathToBinaries(binaries)
+            }
+            "${nativeName}nt"(NativeLibrarySpec) {
+                sources {
+                    cpp {
+                        source {
+                            srcDirs = ['src/libnt/native/cpp']
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/libnt/native/include'
+                        }
+                    }
+                }
+                binaries.all {
+                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                        it.buildable = false
+                        return
+                    }
+                    if (it instanceof SharedLibraryBinarySpec) {
+                        it.buildable = false
+                        return
+                    }
+                    lib library: nativeName, linkage: 'static'
+                    lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                    lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                }
+                appendDebugPathToBinaries(binaries)
+            }
+            // By default, a development executable will be generated. This is to help the case of
+            // testing specific functionality of the library.
+            "${nativeName}App"(NativeExecutableSpec) {
+                baseName = 'glass'
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/app/native/cpp', "$buildDir/generated/app/cpp"
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/app/native/include'
+                        }
+                    }
+                    if (OperatingSystem.current().isWindows()) {
+                        rc {
+                            source {
+                                srcDirs 'src/app/native/win'
+                            }
+                        }
+                    }
+                }
+                binaries.all {
+                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                        it.buildable = false
+                        return
+                    }
+                    lib library: 'glassnt', linkage: 'static'
+                    lib library: nativeName, linkage: 'static'
+                    lib project: ':ntcore', library: 'ntcore', linkage: 'static'
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                    lib project: ':wpimath', library: 'wpimath', linkage: 'static'
+                    lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    if (it.targetPlatform.operatingSystem.isWindows()) {
+                        it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+                    } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                        it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+                    } else {
+                        it.linker.args << '-lX11'
+                    }
+                }
+            }
+        }
+    }
+
+    apply from: 'publish.gradle'
+}
diff --git a/third_party/allwpilib/glass/publish.gradle b/third_party/allwpilib/glass/publish.gradle
new file mode 100644
index 0000000..909d4a2
--- /dev/null
+++ b/third_party/allwpilib/glass/publish.gradle
@@ -0,0 +1,184 @@
+apply plugin: 'maven-publish'
+
+def baseArtifactId = 'Glass'
+def artifactGroupId = 'edu.wpi.first.tools'
+def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_Glass_CLS'
+
+def libBaseArtifactId = 'libglass'
+def libArtifactGroupId = 'edu.wpi.first.glass'
+def libZipBaseName = '_GROUP_edu_wpi_first_glass_ID_libglass_CLS'
+
+def libntBaseArtifactId = 'libglassnt'
+def libntArtifactGroupId = 'edu.wpi.first.glass'
+def libntZipBaseName = '_GROUP_edu_wpi_first_glass_ID_libglassnt_CLS'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+task libCppSourcesZip(type: Zip) {
+    destinationDirectory = outputsFolder
+    archiveBaseName = libZipBaseName
+    classifier = "sources"
+
+    from(licenseFile) { into '/' }
+    from('src/lib/native/cpp') { into '/' }
+}
+
+task libCppHeadersZip(type: Zip) {
+    destinationDirectory = outputsFolder
+    archiveBaseName = libZipBaseName
+    classifier = "headers"
+
+    from(licenseFile) { into '/' }
+    from('src/lib/native/include') { into '/' }
+}
+
+task libntCppSourcesZip(type: Zip) {
+    destinationDirectory = outputsFolder
+    archiveBaseName = libntZipBaseName
+    classifier = "sources"
+
+    from(licenseFile) { into '/' }
+    from('src/libnt/native/cpp') { into '/' }
+}
+
+task libntCppHeadersZip(type: Zip) {
+    destinationDirectory = outputsFolder
+    archiveBaseName = libntZipBaseName
+    classifier = "headers"
+
+    from(licenseFile) { into '/' }
+    from('src/libnt/native/include') { into '/' }
+}
+
+build.dependsOn libCppHeadersZip
+build.dependsOn libCppSourcesZip
+build.dependsOn libntCppHeadersZip
+build.dependsOn libntCppSourcesZip
+
+addTaskToCopyAllOutputs(libCppHeadersZip)
+addTaskToCopyAllOutputs(libCppSourcesZip)
+addTaskToCopyAllOutputs(libntCppHeadersZip)
+addTaskToCopyAllOutputs(libntCppSourcesZip)
+
+model {
+    tasks {
+        // Create the run task.
+        $.components.glassApp.binaries.each { bin ->
+            if (bin.buildable && bin.name.toLowerCase().contains("debug")) {
+                Task run = project.tasks.create("run", Exec) {
+                    commandLine bin.tasks.install.runScriptFile.get().asFile.toString()
+                }
+                run.dependsOn bin.tasks.install
+            }
+        }
+    }
+    publishing {
+        def glassAppTaskList = []
+        $.components.each { component ->
+            component.binaries.each { binary ->
+                if (binary in NativeExecutableBinarySpec && binary.component.name.contains("glassApp")) {
+                    if (binary.buildable && binary.name.contains("Release")) {
+                        // We are now in the binary that we want.
+                        // This is the default application path for the ZIP task.
+                        def applicationPath = binary.executable.file
+                        def icon = file("$project.projectDir/src/app/native/mac/glass.icns")
+
+                        // Create the macOS bundle.
+                        def bundleTask = project.tasks.create("bundleGlassOsxApp", Copy) {
+                            description("Creates a macOS application bundle for Glass")
+                            from(file("$project.projectDir/Info.plist"))
+                            into(file("$project.buildDir/outputs/bundles/Glass.app/Contents"))
+                            into("MacOS") { with copySpec { from binary.executable.file } }
+                            into("Resources") { with copySpec { from icon } }
+
+                            doLast {
+                                if (project.hasProperty("developerID")) {
+                                    // Get path to binary.
+                                    exec {
+                                        workingDir rootDir
+                                        def args = [
+                                            "sh",
+                                            "-c",
+                                            "codesign --force --strict --deep " +
+                                            "--timestamp --options=runtime " +
+                                            "--verbose -s ${project.findProperty("developerID")} " +
+                                            "$project.buildDir/outputs/bundles/Glass.app/"
+                                        ]
+                                        commandLine args
+                                    }
+                                }
+                            }
+                        }
+
+                        // Reset the application path if we are creating a bundle.
+                        if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+                            applicationPath = file("$project.buildDir/outputs/bundles")
+                            project.build.dependsOn bundleTask
+                        }
+
+                        // Create the ZIP.
+                        def task = project.tasks.create("copyGlassExecutable", Zip) {
+                            description("Copies the Glass executable to the outputs directory.")
+                            destinationDirectory = outputsFolder
+
+                            archiveBaseName = '_M_' + zipBaseName
+                            duplicatesStrategy = 'exclude'
+                            classifier = nativeUtils.getPublishClassifier(binary)
+
+                            from(licenseFile) {
+                                into '/'
+                            }
+
+                            from(applicationPath)
+                            into(nativeUtils.getPlatformPath(binary))
+                        }
+
+                        if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+                            bundleTask.dependsOn binary.tasks.link
+                            task.dependsOn(bundleTask)
+                        }
+
+                        task.dependsOn binary.tasks.link
+                        glassAppTaskList.add(task)
+                        project.build.dependsOn task
+                        project.artifacts { task }
+                        addTaskToCopyAllOutputs(task)
+                    }
+                }
+            }
+        }
+
+        def libGlassTaskList = createComponentZipTasks($.components, ['glass'], libZipBaseName, Zip, project, includeStandardZipFormat)
+        def libGlassntTaskList = createComponentZipTasks($.components, ['glassnt'], libntZipBaseName, Zip, project, includeStandardZipFormat)
+
+        publications {
+            glassApp(MavenPublication) {
+                glassAppTaskList.each { artifact it }
+
+                artifactId = baseArtifactId
+                groupId = artifactGroupId
+                version wpilibVersioning.version.get()
+            }
+            libglass(MavenPublication) {
+                libGlassTaskList.each { artifact it }
+
+                artifact libCppHeadersZip
+                artifact libCppSourcesZip
+
+                artifactId = libBaseArtifactId
+                groupId = libArtifactGroupId
+                version wpilibVersioning.version.get()
+            }
+            libglassnt(MavenPublication) {
+                libGlassntTaskList.each { artifact it }
+
+                artifact libntCppHeadersZip
+                artifact libntCppSourcesZip
+
+                artifactId = libntBaseArtifactId
+                groupId = libntArtifactGroupId
+                version wpilibVersioning.version.get()
+            }
+        }
+    }
+}
diff --git a/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in b/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in
new file mode 100644
index 0000000..b0a4490
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/generate/WPILibVersion.cpp.in
@@ -0,0 +1,7 @@
+/*
+ * Autogenerated file! Do not manually edit this file. This version is regenerated
+ * any time the publish task is run, or when this file is deleted.
+ */
+const char* GetWPILibVersion() {
+  return "${wpilib_version}";
+}
diff --git a/third_party/allwpilib/glass/src/app/native/cpp/main.cpp b/third_party/allwpilib/glass/src/app/native/cpp/main.cpp
new file mode 100644
index 0000000..3e0adf1
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/cpp/main.cpp
@@ -0,0 +1,218 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+
+#include <GLFW/glfw3.h>
+#include <fmt/format.h>
+#include <imgui.h>
+#include <ntcore_cpp.h>
+#include <wpigui.h>
+
+#include "glass/Context.h"
+#include "glass/Model.h"
+#include "glass/View.h"
+#include "glass/networktables/NetworkTables.h"
+#include "glass/networktables/NetworkTablesProvider.h"
+#include "glass/networktables/NetworkTablesSettings.h"
+#include "glass/other/Log.h"
+#include "glass/other/Plot.h"
+
+namespace gui = wpi::gui;
+
+const char* GetWPILibVersion();
+
+namespace glass {
+std::string_view GetResource_glass_16_png();
+std::string_view GetResource_glass_32_png();
+std::string_view GetResource_glass_48_png();
+std::string_view GetResource_glass_64_png();
+std::string_view GetResource_glass_128_png();
+std::string_view GetResource_glass_256_png();
+std::string_view GetResource_glass_512_png();
+}  // namespace glass
+
+static std::unique_ptr<glass::PlotProvider> gPlotProvider;
+static std::unique_ptr<glass::NetworkTablesProvider> gNtProvider;
+
+static std::unique_ptr<glass::NetworkTablesModel> gNetworkTablesModel;
+static std::unique_ptr<glass::NetworkTablesSettings> gNetworkTablesSettings;
+static glass::LogData gNetworkTablesLog;
+static glass::Window* gNetworkTablesWindow;
+static glass::Window* gNetworkTablesSettingsWindow;
+static glass::Window* gNetworkTablesLogWindow;
+
+static void NtInitialize() {
+  // update window title when connection status changes
+  auto inst = nt::GetDefaultInstance();
+  auto poller = nt::CreateConnectionListenerPoller(inst);
+  nt::AddPolledConnectionListener(poller, true);
+  gui::AddEarlyExecute([poller] {
+    auto win = gui::GetSystemWindow();
+    if (!win) {
+      return;
+    }
+    bool timedOut;
+    for (auto&& event : nt::PollConnectionListener(poller, 0, &timedOut)) {
+      if (event.connected) {
+        glfwSetWindowTitle(
+            win, fmt::format("Glass - Connected ({})", event.conn.remote_ip)
+                     .c_str());
+      } else {
+        glfwSetWindowTitle(win, "Glass - DISCONNECTED");
+      }
+    }
+  });
+
+  // handle NetworkTables log messages
+  auto logPoller = nt::CreateLoggerPoller(inst);
+  nt::AddPolledLogger(logPoller, NT_LOG_INFO, 100);
+  gui::AddEarlyExecute([logPoller] {
+    bool timedOut;
+    for (auto&& msg : nt::PollLogger(logPoller, 0, &timedOut)) {
+      const char* level = "";
+      if (msg.level >= NT_LOG_CRITICAL) {
+        level = "CRITICAL: ";
+      } else if (msg.level >= NT_LOG_ERROR) {
+        level = "ERROR: ";
+      } else if (msg.level >= NT_LOG_WARNING) {
+        level = "WARNING: ";
+      }
+      gNetworkTablesLog.Append(fmt::format("{}{} ({}:{})\n", level, msg.message,
+                                           msg.filename, msg.line));
+    }
+  });
+
+  gNetworkTablesLogWindow = gNtProvider->AddWindow(
+      "NetworkTables Log",
+      std::make_unique<glass::LogView>(&gNetworkTablesLog));
+  if (gNetworkTablesLogWindow) {
+    gNetworkTablesLogWindow->SetDefaultPos(250, 615);
+    gNetworkTablesLogWindow->SetDefaultSize(600, 130);
+    gNetworkTablesLogWindow->SetVisible(false);
+    gNetworkTablesLogWindow->DisableRenamePopup();
+  }
+
+  // NetworkTables table window
+  gNetworkTablesModel = std::make_unique<glass::NetworkTablesModel>();
+  gui::AddEarlyExecute([] { gNetworkTablesModel->Update(); });
+
+  gNetworkTablesWindow = gNtProvider->AddWindow(
+      "NetworkTables",
+      std::make_unique<glass::NetworkTablesView>(gNetworkTablesModel.get()));
+  if (gNetworkTablesWindow) {
+    gNetworkTablesWindow->SetDefaultPos(250, 277);
+    gNetworkTablesWindow->SetDefaultSize(750, 185);
+    gNetworkTablesWindow->DisableRenamePopup();
+  }
+
+  // NetworkTables settings window
+  gNetworkTablesSettings = std::make_unique<glass::NetworkTablesSettings>();
+  gui::AddEarlyExecute([] { gNetworkTablesSettings->Update(); });
+
+  gNetworkTablesSettingsWindow = gNtProvider->AddWindow(
+      "NetworkTables Settings", [] { gNetworkTablesSettings->Display(); });
+  if (gNetworkTablesSettingsWindow) {
+    gNetworkTablesSettingsWindow->SetDefaultPos(30, 30);
+    gNetworkTablesSettingsWindow->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+    gNetworkTablesSettingsWindow->DisableRenamePopup();
+  }
+}
+
+#ifdef _WIN32
+int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
+                      int nCmdShow) {
+#else
+int main() {
+#endif
+  gui::CreateContext();
+  glass::CreateContext();
+
+  gui::AddIcon(glass::GetResource_glass_16_png());
+  gui::AddIcon(glass::GetResource_glass_32_png());
+  gui::AddIcon(glass::GetResource_glass_48_png());
+  gui::AddIcon(glass::GetResource_glass_64_png());
+  gui::AddIcon(glass::GetResource_glass_128_png());
+  gui::AddIcon(glass::GetResource_glass_256_png());
+  gui::AddIcon(glass::GetResource_glass_512_png());
+
+  gPlotProvider = std::make_unique<glass::PlotProvider>("Plot");
+  gNtProvider = std::make_unique<glass::NetworkTablesProvider>("NTProvider");
+
+  gui::ConfigurePlatformSaveFile("glass.ini");
+  gPlotProvider->GlobalInit();
+  gui::AddInit([] { glass::ResetTime(); });
+  gNtProvider->GlobalInit();
+  gui::AddInit(NtInitialize);
+
+  glass::AddStandardNetworkTablesViews(*gNtProvider);
+
+  gui::AddLateExecute([] {
+    ImGui::BeginMainMenuBar();
+    gui::EmitViewMenu();
+    if (ImGui::BeginMenu("View")) {
+      if (ImGui::MenuItem("Reset Time")) {
+        glass::ResetTime();
+      }
+      ImGui::EndMenu();
+    }
+    if (ImGui::BeginMenu("NetworkTables")) {
+      if (gNetworkTablesSettingsWindow) {
+        gNetworkTablesSettingsWindow->DisplayMenuItem("NetworkTables Settings");
+      }
+      if (gNetworkTablesWindow) {
+        gNetworkTablesWindow->DisplayMenuItem("NetworkTables View");
+      }
+      if (gNetworkTablesLogWindow) {
+        gNetworkTablesLogWindow->DisplayMenuItem("NetworkTables Log");
+      }
+      ImGui::Separator();
+      gNtProvider->DisplayMenu();
+      ImGui::EndMenu();
+    }
+    if (ImGui::BeginMenu("Plot")) {
+      bool paused = gPlotProvider->IsPaused();
+      if (ImGui::MenuItem("Pause All Plots", nullptr, &paused)) {
+        gPlotProvider->SetPaused(paused);
+      }
+      ImGui::Separator();
+      gPlotProvider->DisplayMenu();
+      ImGui::EndMenu();
+    }
+
+    bool about = false;
+    if (ImGui::BeginMenu("Info")) {
+      if (ImGui::MenuItem("About")) {
+        about = true;
+      }
+      ImGui::EndMenu();
+    }
+    ImGui::EndMainMenuBar();
+
+    if (about) {
+      ImGui::OpenPopup("About");
+      about = false;
+    }
+    if (ImGui::BeginPopupModal("About")) {
+      ImGui::Text("Glass: A different kind of dashboard");
+      ImGui::Separator();
+      ImGui::Text("v%s", GetWPILibVersion());
+      if (ImGui::Button("Close")) {
+        ImGui::CloseCurrentPopup();
+      }
+      ImGui::EndPopup();
+    }
+  });
+
+  gui::Initialize("Glass - DISCONNECTED", 1024, 768);
+  gui::Main();
+
+  gNetworkTablesModel.reset();
+  gNetworkTablesSettings.reset();
+  gNtProvider.reset();
+  gPlotProvider.reset();
+
+  glass::DestroyContext();
+  gui::DestroyContext();
+}
diff --git a/third_party/allwpilib/glass/src/app/native/mac/glass.icns b/third_party/allwpilib/glass/src/app/native/mac/glass.icns
new file mode 100644
index 0000000..74b6850
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/mac/glass.icns
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-128.png b/third_party/allwpilib/glass/src/app/native/resources/glass-128.png
new file mode 100644
index 0000000..f2ea7a1
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-128.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-16.png b/third_party/allwpilib/glass/src/app/native/resources/glass-16.png
new file mode 100644
index 0000000..dbca52a
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-16.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-256.png b/third_party/allwpilib/glass/src/app/native/resources/glass-256.png
new file mode 100644
index 0000000..db9062f
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-256.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-32.png b/third_party/allwpilib/glass/src/app/native/resources/glass-32.png
new file mode 100644
index 0000000..b6f69d0
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-32.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-48.png b/third_party/allwpilib/glass/src/app/native/resources/glass-48.png
new file mode 100644
index 0000000..14ee934
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-48.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-512.png b/third_party/allwpilib/glass/src/app/native/resources/glass-512.png
new file mode 100644
index 0000000..b022d1e
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-512.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/resources/glass-64.png b/third_party/allwpilib/glass/src/app/native/resources/glass-64.png
new file mode 100644
index 0000000..b488115
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/resources/glass-64.png
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/win/glass.ico b/third_party/allwpilib/glass/src/app/native/win/glass.ico
new file mode 100644
index 0000000..99c5409
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/win/glass.ico
Binary files differ
diff --git a/third_party/allwpilib/glass/src/app/native/win/glass.rc b/third_party/allwpilib/glass/src/app/native/win/glass.rc
new file mode 100644
index 0000000..a9f2951
--- /dev/null
+++ b/third_party/allwpilib/glass/src/app/native/win/glass.rc
@@ -0,0 +1 @@
+IDI_ICON1 ICON "glass.ico"
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp
new file mode 100644
index 0000000..936acb2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/Context.cpp
@@ -0,0 +1,445 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/Context.h"
+
+#include <algorithm>
+#include <cinttypes>
+#include <cstdio>
+
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <wpi/StringExtras.h>
+#include <wpi/timestamp.h>
+#include <wpigui.h>
+
+#include "glass/ContextInternal.h"
+
+using namespace glass;
+
+Context* glass::gContext;
+
+static bool ConvertInt(Storage::Value* value) {
+  value->type = Storage::Value::kInt;
+  if (auto val = wpi::parse_integer<int>(value->stringVal, 10)) {
+    value->intVal = val.value();
+    return true;
+  }
+  return false;
+}
+
+static bool ConvertInt64(Storage::Value* value) {
+  value->type = Storage::Value::kInt64;
+  if (auto val = wpi::parse_integer<int64_t>(value->stringVal, 10)) {
+    value->int64Val = val.value();
+    return true;
+  }
+  return false;
+}
+
+static bool ConvertBool(Storage::Value* value) {
+  value->type = Storage::Value::kBool;
+  if (auto val = wpi::parse_integer<int>(value->stringVal, 10)) {
+    value->intVal = (val.value() != 0);
+    return true;
+  }
+  return false;
+}
+
+static bool ConvertFloat(Storage::Value* value) {
+  value->type = Storage::Value::kFloat;
+  if (auto val = wpi::parse_float<float>(value->stringVal)) {
+    value->floatVal = val.value();
+    return true;
+  }
+  return false;
+}
+
+static bool ConvertDouble(Storage::Value* value) {
+  value->type = Storage::Value::kDouble;
+  if (auto val = wpi::parse_float<double>(value->stringVal)) {
+    value->doubleVal = val.value();
+    return true;
+  }
+  return false;
+}
+
+static void* GlassStorageReadOpen(ImGuiContext*, ImGuiSettingsHandler* handler,
+                                  const char* name) {
+  auto ctx = static_cast<Context*>(handler->UserData);
+  auto& storage = ctx->storage[name];
+  if (!storage) {
+    storage = std::make_unique<Storage>();
+  }
+  return storage.get();
+}
+
+static void GlassStorageReadLine(ImGuiContext*, ImGuiSettingsHandler*,
+                                 void* entry, const char* line) {
+  auto storage = static_cast<Storage*>(entry);
+  auto [key, val] = wpi::split(line, '=');
+  auto& keys = storage->GetKeys();
+  auto& values = storage->GetValues();
+  auto it = std::find(keys.begin(), keys.end(), key);
+  if (it == keys.end()) {
+    keys.emplace_back(key);
+    values.emplace_back(std::make_unique<Storage::Value>(val));
+  } else {
+    auto& value = *values[it - keys.begin()];
+    value.stringVal = val;
+    switch (value.type) {
+      case Storage::Value::kInt:
+        ConvertInt(&value);
+        break;
+      case Storage::Value::kInt64:
+        ConvertInt64(&value);
+        break;
+      case Storage::Value::kBool:
+        ConvertBool(&value);
+        break;
+      case Storage::Value::kFloat:
+        ConvertFloat(&value);
+        break;
+      case Storage::Value::kDouble:
+        ConvertDouble(&value);
+        break;
+      default:
+        break;
+    }
+  }
+}
+
+static void GlassStorageWriteAll(ImGuiContext*, ImGuiSettingsHandler* handler,
+                                 ImGuiTextBuffer* out_buf) {
+  auto ctx = static_cast<Context*>(handler->UserData);
+
+  // sort for output
+  std::vector<wpi::StringMapConstIterator<std::unique_ptr<Storage>>> sorted;
+  for (auto it = ctx->storage.begin(); it != ctx->storage.end(); ++it) {
+    sorted.emplace_back(it);
+  }
+  std::sort(sorted.begin(), sorted.end(), [](const auto& a, const auto& b) {
+    return a->getKey() < b->getKey();
+  });
+
+  for (auto&& entryIt : sorted) {
+    auto& entry = *entryIt;
+    out_buf->append("[GlassStorage][");
+    out_buf->append(entry.first().data(),
+                    entry.first().data() + entry.first().size());
+    out_buf->append("]\n");
+    auto& keys = entry.second->GetKeys();
+    auto& values = entry.second->GetValues();
+    for (size_t i = 0; i < keys.size(); ++i) {
+      out_buf->append(keys[i].data(), keys[i].data() + keys[i].size());
+      out_buf->append("=");
+      auto& value = *values[i];
+      switch (value.type) {
+        case Storage::Value::kInt:
+          out_buf->appendf("%d\n", value.intVal);
+          break;
+        case Storage::Value::kInt64:
+          out_buf->appendf("%" PRId64 "\n", value.int64Val);
+          break;
+        case Storage::Value::kBool:
+          out_buf->appendf("%d\n", value.boolVal ? 1 : 0);
+          break;
+        case Storage::Value::kFloat:
+          out_buf->appendf("%f\n", value.floatVal);
+          break;
+        case Storage::Value::kDouble:
+          out_buf->appendf("%f\n", value.doubleVal);
+          break;
+        case Storage::Value::kNone:
+        case Storage::Value::kString:
+          out_buf->append(value.stringVal.data(),
+                          value.stringVal.data() + value.stringVal.size());
+          out_buf->append("\n");
+          break;
+      }
+    }
+    out_buf->append("\n");
+  }
+}
+
+static void Initialize(Context* ctx) {
+  wpi::gui::AddInit([=] {
+    ImGuiSettingsHandler ini_handler;
+    ini_handler.TypeName = "GlassStorage";
+    ini_handler.TypeHash = ImHashStr("GlassStorage");
+    ini_handler.ReadOpenFn = GlassStorageReadOpen;
+    ini_handler.ReadLineFn = GlassStorageReadLine;
+    ini_handler.WriteAllFn = GlassStorageWriteAll;
+    ini_handler.UserData = ctx;
+    ImGui::GetCurrentContext()->SettingsHandlers.push_back(ini_handler);
+
+    ctx->sources.Initialize();
+  });
+}
+
+static void Shutdown(Context* ctx) {}
+
+Context* glass::CreateContext() {
+  Context* ctx = new Context;
+  if (!gContext) {
+    SetCurrentContext(ctx);
+  }
+  Initialize(ctx);
+  return ctx;
+}
+
+void glass::DestroyContext(Context* ctx) {
+  if (!ctx) {
+    ctx = gContext;
+  }
+  Shutdown(ctx);
+  if (gContext == ctx) {
+    SetCurrentContext(nullptr);
+  }
+  delete ctx;
+}
+
+Context* glass::GetCurrentContext() {
+  return gContext;
+}
+
+void glass::SetCurrentContext(Context* ctx) {
+  gContext = ctx;
+}
+
+void glass::ResetTime() {
+  gContext->zeroTime = wpi::Now();
+}
+
+uint64_t glass::GetZeroTime() {
+  return gContext->zeroTime;
+}
+
+Storage::Value& Storage::GetValue(std::string_view key) {
+  auto it = std::find(m_keys.begin(), m_keys.end(), key);
+  if (it == m_keys.end()) {
+    m_keys.emplace_back(key);
+    m_values.emplace_back(std::make_unique<Value>());
+    return *m_values.back();
+  } else {
+    return *m_values[it - m_keys.begin()];
+  }
+}
+
+#define DEFUN(CapsName, LowerName, CType)                                      \
+  CType Storage::Get##CapsName(std::string_view key, CType defaultVal) const { \
+    auto it = std::find(m_keys.begin(), m_keys.end(), key);                    \
+    if (it == m_keys.end())                                                    \
+      return defaultVal;                                                       \
+    Value& value = *m_values[it - m_keys.begin()];                             \
+    if (value.type != Value::k##CapsName) {                                    \
+      if (!Convert##CapsName(&value))                                          \
+        value.LowerName##Val = defaultVal;                                     \
+    }                                                                          \
+    return value.LowerName##Val;                                               \
+  }                                                                            \
+                                                                               \
+  void Storage::Set##CapsName(std::string_view key, CType val) {               \
+    auto it = std::find(m_keys.begin(), m_keys.end(), key);                    \
+    if (it == m_keys.end()) {                                                  \
+      m_keys.emplace_back(key);                                                \
+      m_values.emplace_back(std::make_unique<Value>());                        \
+      m_values.back()->type = Value::k##CapsName;                              \
+      m_values.back()->LowerName##Val = val;                                   \
+    } else {                                                                   \
+      Value& value = *m_values[it - m_keys.begin()];                           \
+      value.type = Value::k##CapsName;                                         \
+      value.LowerName##Val = val;                                              \
+    }                                                                          \
+  }                                                                            \
+                                                                               \
+  CType* Storage::Get##CapsName##Ref(std::string_view key, CType defaultVal) { \
+    auto it = std::find(m_keys.begin(), m_keys.end(), key);                    \
+    if (it == m_keys.end()) {                                                  \
+      m_keys.emplace_back(key);                                                \
+      m_values.emplace_back(std::make_unique<Value>());                        \
+      m_values.back()->type = Value::k##CapsName;                              \
+      m_values.back()->LowerName##Val = defaultVal;                            \
+      return &m_values.back()->LowerName##Val;                                 \
+    } else {                                                                   \
+      Value& value = *m_values[it - m_keys.begin()];                           \
+      if (value.type != Value::k##CapsName) {                                  \
+        if (!Convert##CapsName(&value))                                        \
+          value.LowerName##Val = defaultVal;                                   \
+      }                                                                        \
+      return &value.LowerName##Val;                                            \
+    }                                                                          \
+  }
+
+DEFUN(Int, int, int)
+DEFUN(Int64, int64, int64_t)
+DEFUN(Bool, bool, bool)
+DEFUN(Float, float, float)
+DEFUN(Double, double, double)
+
+std::string Storage::GetString(std::string_view key,
+                               std::string_view defaultVal) const {
+  auto it = std::find(m_keys.begin(), m_keys.end(), key);
+  if (it == m_keys.end()) {
+    return std::string{defaultVal};
+  }
+  Value& value = *m_values[it - m_keys.begin()];
+  value.type = Value::kString;
+  return value.stringVal;
+}
+
+void Storage::SetString(std::string_view key, std::string_view val) {
+  auto it = std::find(m_keys.begin(), m_keys.end(), key);
+  if (it == m_keys.end()) {
+    m_keys.emplace_back(key);
+    m_values.emplace_back(std::make_unique<Value>(val));
+    m_values.back()->type = Value::kString;
+  } else {
+    Value& value = *m_values[it - m_keys.begin()];
+    value.type = Value::kString;
+    value.stringVal = val;
+  }
+}
+
+std::string* Storage::GetStringRef(std::string_view key,
+                                   std::string_view defaultVal) {
+  auto it = std::find(m_keys.begin(), m_keys.end(), key);
+  if (it == m_keys.end()) {
+    m_keys.emplace_back(key);
+    m_values.emplace_back(std::make_unique<Value>(defaultVal));
+    m_values.back()->type = Value::kString;
+    return &m_values.back()->stringVal;
+  } else {
+    Value& value = *m_values[it - m_keys.begin()];
+    value.type = Value::kString;
+    return &value.stringVal;
+  }
+}
+
+Storage& glass::GetStorage() {
+  auto& storage = gContext->storage[gContext->curId];
+  if (!storage) {
+    storage = std::make_unique<Storage>();
+  }
+  return *storage;
+}
+
+Storage& glass::GetStorage(std::string_view id) {
+  auto& storage = gContext->storage[id];
+  if (!storage) {
+    storage = std::make_unique<Storage>();
+  }
+  return *storage;
+}
+
+static void PushIDStack(std::string_view label_id) {
+  gContext->idStack.emplace_back(gContext->curId.size());
+
+  auto [label, id] = wpi::split(label_id, "###");
+  // if no ###id, use label as id
+  if (id.empty()) {
+    id = label;
+  }
+  if (!gContext->curId.empty()) {
+    gContext->curId += "###";
+  }
+  gContext->curId += id;
+}
+
+static void PopIDStack() {
+  gContext->curId.resize(gContext->idStack.back());
+  gContext->idStack.pop_back();
+}
+
+bool glass::Begin(const char* name, bool* p_open, ImGuiWindowFlags flags) {
+  PushIDStack(name);
+  return ImGui::Begin(name, p_open, flags);
+}
+
+void glass::End() {
+  ImGui::End();
+  PopIDStack();
+}
+
+bool glass::BeginChild(const char* str_id, const ImVec2& size, bool border,
+                       ImGuiWindowFlags flags) {
+  PushIDStack(str_id);
+  return ImGui::BeginChild(str_id, size, border, flags);
+}
+
+void glass::EndChild() {
+  ImGui::EndChild();
+  PopIDStack();
+}
+
+bool glass::CollapsingHeader(const char* label, ImGuiTreeNodeFlags flags) {
+  wpi::SmallString<64> openKey;
+  auto [name, id] = wpi::split(label, "###");
+  // if no ###id, use name as id
+  if (id.empty()) {
+    id = name;
+  }
+  openKey = id;
+  openKey += "###open";
+
+  bool* open = GetStorage().GetBoolRef(openKey.str());
+  *open = ImGui::CollapsingHeader(
+      label, flags | (*open ? ImGuiTreeNodeFlags_DefaultOpen : 0));
+  return *open;
+}
+
+bool glass::TreeNodeEx(const char* label, ImGuiTreeNodeFlags flags) {
+  PushIDStack(label);
+  bool* open = GetStorage().GetBoolRef("open");
+  *open = ImGui::TreeNodeEx(
+      label, flags | (*open ? ImGuiTreeNodeFlags_DefaultOpen : 0));
+  if (!*open) {
+    PopIDStack();
+  }
+  return *open;
+}
+
+void glass::TreePop() {
+  ImGui::TreePop();
+  PopIDStack();
+}
+
+void glass::PushID(const char* str_id) {
+  PushIDStack(str_id);
+  ImGui::PushID(str_id);
+}
+
+void glass::PushID(const char* str_id_begin, const char* str_id_end) {
+  PushIDStack(std::string_view(str_id_begin, str_id_end - str_id_begin));
+  ImGui::PushID(str_id_begin, str_id_end);
+}
+
+void glass::PushID(int int_id) {
+  char buf[16];
+  std::snprintf(buf, sizeof(buf), "%d", int_id);
+  PushIDStack(buf);
+  ImGui::PushID(int_id);
+}
+
+void glass::PopID() {
+  ImGui::PopID();
+  PopIDStack();
+}
+
+bool glass::PopupEditName(const char* label, std::string* name) {
+  bool rv = false;
+  if (ImGui::BeginPopupContextItem(label)) {
+    ImGui::Text("Edit name:");
+    if (ImGui::InputText("##editname", name)) {
+      rv = true;
+    }
+    if (ImGui::Button("Close") || ImGui::IsKeyPressedMap(ImGuiKey_Enter) ||
+        ImGui::IsKeyPressedMap(ImGuiKey_KeyPadEnter)) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+  return rv;
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/DataSource.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/DataSource.cpp
new file mode 100644
index 0000000..adab6e7
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/DataSource.cpp
@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/DataSource.h"
+
+#include <fmt/format.h>
+
+#include "glass/ContextInternal.h"
+
+using namespace glass;
+
+wpi::sig::Signal<const char*, DataSource*> DataSource::sourceCreated;
+
+DataSource::DataSource(std::string_view id) : m_id{id} {
+  auto it = gContext->sources.try_emplace(m_id, this);
+  auto& srcName = it.first->getValue();
+  m_name = srcName.name.get();
+  if (!srcName.source) {
+    srcName.source = this;
+  }
+  sourceCreated(m_id.c_str(), this);
+}
+
+DataSource::DataSource(std::string_view id, int index)
+    : DataSource{fmt::format("{}[{}]", id, index)} {}
+
+DataSource::DataSource(std::string_view id, int index, int index2)
+    : DataSource{fmt::format("{}[{},{}]", id, index, index2)} {}
+
+DataSource::~DataSource() {
+  if (!gContext) {
+    return;
+  }
+  auto it = gContext->sources.find(m_id);
+  if (it == gContext->sources.end()) {
+    return;
+  }
+  auto& srcName = it->getValue();
+  if (srcName.source == this) {
+    srcName.source = nullptr;
+  }
+}
+
+void DataSource::SetName(std::string_view name) {
+  m_name->SetName(name);
+}
+
+const char* DataSource::GetName() const {
+  return m_name->GetName();
+}
+
+void DataSource::PushEditNameId(int index) {
+  m_name->PushEditNameId(index);
+}
+
+void DataSource::PushEditNameId(const char* name) {
+  m_name->PushEditNameId(name);
+}
+
+bool DataSource::PopupEditName(int index) {
+  return m_name->PopupEditName(index);
+}
+
+bool DataSource::PopupEditName(const char* name) {
+  return m_name->PopupEditName(name);
+}
+
+bool DataSource::InputTextName(const char* label_id,
+                               ImGuiInputTextFlags flags) {
+  return m_name->InputTextName(label_id, flags);
+}
+
+void DataSource::LabelText(const char* label, const char* fmt, ...) const {
+  va_list args;
+  va_start(args, fmt);
+  LabelTextV(label, fmt, args);
+  va_end(args);
+}
+
+// Add a label+text combo aligned to other label+value widgets
+void DataSource::LabelTextV(const char* label, const char* fmt,
+                            va_list args) const {
+  ImGui::PushID(label);
+  ImGui::LabelTextV("##input", fmt, args);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  ImGui::PopID();
+  EmitDrag();
+}
+
+bool DataSource::Combo(const char* label, int* current_item,
+                       const char* const items[], int items_count,
+                       int popup_max_height_in_items) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::Combo("##input", current_item, items, items_count,
+                         popup_max_height_in_items);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+bool DataSource::SliderFloat(const char* label, float* v, float v_min,
+                             float v_max, const char* format,
+                             float power) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::SliderFloat("##input", v, v_min, v_max, format, power);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+bool DataSource::InputDouble(const char* label, double* v, double step,
+                             double step_fast, const char* format,
+                             ImGuiInputTextFlags flags) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::InputDouble("##input", v, step, step_fast, format, flags);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+bool DataSource::InputInt(const char* label, int* v, int step, int step_fast,
+                          ImGuiInputTextFlags flags) const {
+  ImGui::PushID(label);
+  bool rv = ImGui::InputInt("##input", v, step, step_fast, flags);
+  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+  ImGui::Selectable(label);
+  EmitDrag();
+  ImGui::PopID();
+  return rv;
+}
+
+void DataSource::EmitDrag(ImGuiDragDropFlags flags) const {
+  if (ImGui::BeginDragDropSource(flags)) {
+    auto self = this;
+    ImGui::SetDragDropPayload("DataSource", &self, sizeof(self));  // NOLINT
+    const char* name = GetName();
+    ImGui::TextUnformatted(name[0] == '\0' ? m_id.c_str() : name);
+    ImGui::EndDragDropSource();
+  }
+}
+
+DataSource* DataSource::Find(std::string_view id) {
+  auto it = gContext->sources.find(id);
+  if (it == gContext->sources.end()) {
+    return nullptr;
+  }
+  return it->getValue().source;
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/MainMenuBar.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/MainMenuBar.cpp
new file mode 100644
index 0000000..2c4d371
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/MainMenuBar.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/MainMenuBar.h"
+
+#include <cstdio>
+
+#include <wpigui.h>
+
+using namespace glass;
+
+void MainMenuBar::AddMainMenu(std::function<void()> menu) {
+  if (menu) {
+    m_menus.emplace_back(std::move(menu));
+  }
+}
+
+void MainMenuBar::AddOptionMenu(std::function<void()> menu) {
+  if (menu) {
+    m_optionMenus.emplace_back(std::move(menu));
+  }
+}
+
+void MainMenuBar::Display() {
+  ImGui::BeginMainMenuBar();
+
+  if (!m_optionMenus.empty()) {
+    if (ImGui::BeginMenu("Options")) {
+      for (auto&& menu : m_optionMenus) {
+        if (menu) {
+          menu();
+        }
+      }
+      ImGui::EndMenu();
+    }
+  }
+
+  wpi::gui::EmitViewMenu();
+
+  for (auto&& menu : m_menus) {
+    if (menu) {
+      menu();
+    }
+  }
+
+#if 0
+  char str[64];
+  std::snprintf(str, sizeof(str), "%.3f ms/frame (%.1f FPS)",
+                1000.0f / ImGui::GetIO().Framerate,
+                ImGui::GetIO().Framerate);
+  ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize(str).x -
+                  10);
+  ImGui::Text("%s", str);
+#endif
+  ImGui::EndMainMenuBar();
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/Model.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/Model.cpp
new file mode 100644
index 0000000..bee9086
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/Model.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/Model.h"
+
+using namespace glass;
+
+bool Model::IsReadOnly() {
+  return false;
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/View.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/View.cpp
new file mode 100644
index 0000000..e01c4df
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/View.cpp
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/View.h"
+
+using namespace glass;
+
+namespace {
+class FunctionView : public View {
+ public:
+  explicit FunctionView(wpi::unique_function<void()> display)
+      : m_display(std::move(display)) {}
+
+  void Display() override { m_display(); }
+
+ private:
+  wpi::unique_function<void()> m_display;
+};
+}  // namespace
+
+std::unique_ptr<View> glass::MakeFunctionView(
+    wpi::unique_function<void()> display) {
+  return std::make_unique<FunctionView>(std::move(display));
+}
+
+void View::Hidden() {}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/Window.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/Window.cpp
new file mode 100644
index 0000000..5c014eb
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/Window.cpp
@@ -0,0 +1,111 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/Window.h"
+
+#include <imgui_internal.h>
+#include <wpi/StringExtras.h>
+
+#include "glass/Context.h"
+
+using namespace glass;
+
+void Window::SetVisibility(Visibility visibility) {
+  switch (visibility) {
+    case kHide:
+      m_visible = false;
+      m_enabled = true;
+      break;
+    case kShow:
+      m_visible = true;
+      m_enabled = true;
+      break;
+    case kDisabled:
+      m_enabled = false;
+      break;
+  }
+}
+
+void Window::Display() {
+  if (!m_view) {
+    return;
+  }
+  if (!m_visible || !m_enabled) {
+    PushID(m_id);
+    m_view->Hidden();
+    PopID();
+    return;
+  }
+
+  if (m_posCond != 0) {
+    ImGui::SetNextWindowPos(m_pos, m_posCond);
+  }
+  if (m_sizeCond != 0) {
+    ImGui::SetNextWindowSize(m_size, m_sizeCond);
+  }
+  if (m_setPadding) {
+    ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, m_padding);
+  }
+
+  char label[128];
+  std::snprintf(label, sizeof(label), "%s###%s",
+                m_name.empty() ? m_defaultName.c_str() : m_name.c_str(),
+                m_id.c_str());
+
+  if (Begin(label, &m_visible, m_flags)) {
+    if (m_renamePopupEnabled) {
+      PopupEditName(nullptr, &m_name);
+    }
+    m_view->Display();
+  } else {
+    m_view->Hidden();
+  }
+  End();
+  if (m_setPadding) {
+    ImGui::PopStyleVar();
+  }
+}
+
+bool Window::DisplayMenuItem(const char* label) {
+  bool wasVisible = m_visible;
+  ImGui::MenuItem(
+      label ? label : (m_name.empty() ? m_id.c_str() : m_name.c_str()), nullptr,
+      &m_visible, m_enabled);
+  return !wasVisible && m_visible;
+}
+
+void Window::ScaleDefault(float scale) {
+  if ((m_posCond & ImGuiCond_FirstUseEver) != 0) {
+    m_pos.x *= scale;
+    m_pos.y *= scale;
+  }
+  if ((m_sizeCond & ImGuiCond_FirstUseEver) != 0) {
+    m_size.x *= scale;
+    m_size.y *= scale;
+  }
+}
+
+void Window::IniReadLine(const char* line) {
+  auto [name, value] = wpi::split(line, '=');
+  name = wpi::trim(name);
+  value = wpi::trim(value);
+
+  if (name == "name") {
+    m_name = value;
+  } else if (name == "visible") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_visible = num.value();
+    }
+  } else if (name == "enabled") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_enabled = num.value();
+    }
+  }
+}
+
+void Window::IniWriteAll(const char* typeName, ImGuiTextBuffer* out_buf) {
+  out_buf->appendf("[%s][%s]\nname=%s\nvisible=%d\nenabled=%d\n\n", typeName,
+                   m_id.c_str(), m_name.c_str(), m_visible ? 1 : 0,
+                   m_enabled ? 1 : 0);
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/WindowManager.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/WindowManager.cpp
new file mode 100644
index 0000000..037b9bd
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/WindowManager.cpp
@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/WindowManager.h"
+
+#include <algorithm>
+#include <cstdio>
+
+#include <fmt/format.h>
+#include <wpigui.h>
+
+using namespace glass;
+
+WindowManager::WindowManager(std::string_view iniName)
+    : m_iniSaver{iniName, this} {}
+
+// read/write open state to ini file
+void* WindowManager::IniSaver::IniReadOpen(const char* name) {
+  return m_manager->GetOrAddWindow(name, true);
+}
+
+void WindowManager::IniSaver::IniReadLine(void* entry, const char* lineStr) {
+  static_cast<Window*>(entry)->IniReadLine(lineStr);
+}
+
+void WindowManager::IniSaver::IniWriteAll(ImGuiTextBuffer* out_buf) {
+  const char* typeName = GetTypeName();
+  for (auto&& window : m_manager->m_windows) {
+    window->IniWriteAll(typeName, out_buf);
+  }
+}
+
+Window* WindowManager::AddWindow(std::string_view id,
+                                 wpi::unique_function<void()> display) {
+  auto win = GetOrAddWindow(id, false);
+  if (!win) {
+    return nullptr;
+  }
+  if (win->HasView()) {
+    fmt::print(stderr, "GUI: ignoring duplicate window '{}'\n", id);
+    return nullptr;
+  }
+  win->SetView(MakeFunctionView(std::move(display)));
+  return win;
+}
+
+Window* WindowManager::AddWindow(std::string_view id,
+                                 std::unique_ptr<View> view) {
+  auto win = GetOrAddWindow(id, false);
+  if (!win) {
+    return nullptr;
+  }
+  if (win->HasView()) {
+    fmt::print(stderr, "GUI: ignoring duplicate window '{}'\n", id);
+    return nullptr;
+  }
+  win->SetView(std::move(view));
+  return win;
+}
+
+Window* WindowManager::GetOrAddWindow(std::string_view id, bool duplicateOk) {
+  // binary search
+  auto it = std::lower_bound(
+      m_windows.begin(), m_windows.end(), id,
+      [](const auto& elem, std::string_view s) { return elem->GetId() < s; });
+  if (it != m_windows.end() && (*it)->GetId() == id) {
+    if (!duplicateOk) {
+      fmt::print(stderr, "GUI: ignoring duplicate window '{}'\n", id);
+      return nullptr;
+    }
+    return it->get();
+  }
+  // insert before (keeps sort)
+  return m_windows.emplace(it, std::make_unique<Window>(id))->get();
+}
+
+Window* WindowManager::GetWindow(std::string_view id) {
+  // binary search
+  auto it = std::lower_bound(
+      m_windows.begin(), m_windows.end(), id,
+      [](const auto& elem, std::string_view s) { return elem->GetId() < s; });
+  if (it == m_windows.end() || (*it)->GetId() != id) {
+    return nullptr;
+  }
+  return it->get();
+}
+
+void WindowManager::GlobalInit() {
+  wpi::gui::AddInit([this] { m_iniSaver.Initialize(); });
+  wpi::gui::AddWindowScaler([this](float scale) {
+    // scale default window positions
+    for (auto&& window : m_windows) {
+      window->ScaleDefault(scale);
+    }
+  });
+  wpi::gui::AddLateExecute([this] { DisplayWindows(); });
+}
+
+void WindowManager::DisplayMenu() {
+  for (auto&& window : m_windows) {
+    window->DisplayMenuItem();
+  }
+}
+
+void WindowManager::DisplayWindows() {
+  for (auto&& window : m_windows) {
+    window->Display();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Accelerometer.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Accelerometer.cpp
new file mode 100644
index 0000000..6a1cc03
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Accelerometer.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/Accelerometer.h"
+
+#include "glass/DataSource.h"
+#include "glass/other/DeviceTree.h"
+
+using namespace glass;
+
+void glass::DisplayAccelerometerDevice(AccelerometerModel* model) {
+  if (!model->Exists()) {
+    return;
+  }
+  if (BeginDevice("BuiltInAccel")) {
+    // Range
+    {
+      int value = model->GetRange();
+      static const char* rangeOptions[] = {"2G", "4G", "8G"};
+      DeviceEnum("Range", true, &value, rangeOptions, 3);
+    }
+
+    // X Accel
+    if (auto xData = model->GetXData()) {
+      double value = xData->GetValue();
+      if (DeviceDouble("X Accel", false, &value, xData)) {
+        model->SetX(value);
+      }
+    }
+
+    // Y Accel
+    if (auto yData = model->GetYData()) {
+      double value = yData->GetValue();
+      if (DeviceDouble("Y Accel", false, &value, yData)) {
+        model->SetY(value);
+      }
+    }
+
+    // Z Accel
+    if (auto zData = model->GetZData()) {
+      double value = zData->GetValue();
+      if (DeviceDouble("Z Accel", false, &value, zData)) {
+        model->SetZ(value);
+      }
+    }
+
+    EndDevice();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogGyro.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogGyro.cpp
new file mode 100644
index 0000000..be06a71
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogGyro.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/AnalogGyro.h"
+
+#include "glass/DataSource.h"
+#include "glass/other/DeviceTree.h"
+
+using namespace glass;
+
+void glass::DisplayAnalogGyroDevice(AnalogGyroModel* model, int index) {
+  char name[32];
+  std::snprintf(name, sizeof(name), "AnalogGyro[%d]", index);
+  if (BeginDevice(name)) {
+    // angle
+    if (auto angleData = model->GetAngleData()) {
+      double value = angleData->GetValue();
+      if (DeviceDouble("Angle", false, &value, angleData)) {
+        model->SetAngle(value);
+      }
+    }
+
+    // rate
+    if (auto rateData = model->GetRateData()) {
+      double value = rateData->GetValue();
+      if (DeviceDouble("Rate", false, &value, rateData)) {
+        model->SetRate(value);
+      }
+    }
+    EndDevice();
+  }
+}
+
+void glass::DisplayAnalogGyrosDevice(AnalogGyrosModel* model) {
+  model->ForEachAnalogGyro(
+      [&](AnalogGyroModel& gyro, int i) { DisplayAnalogGyroDevice(&gyro, i); });
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogInput.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogInput.cpp
new file mode 100644
index 0000000..b9699a4
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogInput.cpp
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/AnalogInput.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayAnalogInput(AnalogInputModel* model, int index) {
+  auto voltageData = model->GetVoltageData();
+  if (!voltageData) {
+    return;
+  }
+
+  // build label
+  std::string* name = GetStorage().GetStringRef("name");
+  char label[128];
+  if (!name->empty()) {
+    std::snprintf(label, sizeof(label), "%s [%d]###name", name->c_str(), index);
+  } else {
+    std::snprintf(label, sizeof(label), "In[%d]###name", index);
+  }
+
+  if (model->IsGyro()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::LabelText(label, "AnalogGyro[%d]", index);
+    ImGui::PopStyleColor();
+  } else if (auto simDevice = model->GetSimDevice()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::LabelText(label, "%s", simDevice);
+    ImGui::PopStyleColor();
+  } else {
+    float val = voltageData->GetValue();
+    if (voltageData->SliderFloat(label, &val, 0.0, 5.0)) {
+      model->SetVoltage(val);
+    }
+  }
+
+  // context menu to change name
+  if (PopupEditName("name", name)) {
+    voltageData->SetName(name->c_str());
+  }
+}
+
+void glass::DisplayAnalogInputs(AnalogInputsModel* model,
+                                std::string_view noneMsg) {
+  ImGui::Text("(Use Ctrl+Click to edit value)");
+  bool hasAny = false;
+  bool first = true;
+  model->ForEachAnalogInput([&](AnalogInputModel& input, int i) {
+    if (!first) {
+      ImGui::Spacing();
+      ImGui::Spacing();
+    } else {
+      first = false;
+    }
+    PushID(i);
+    DisplayAnalogInput(&input, i);
+    PopID();
+    hasAny = true;
+  });
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogOutput.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogOutput.cpp
new file mode 100644
index 0000000..3a9594b
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/AnalogOutput.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/AnalogOutput.h"
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+#include "glass/other/DeviceTree.h"
+
+using namespace glass;
+
+void glass::DisplayAnalogOutputsDevice(AnalogOutputsModel* model) {
+  int count = 0;
+  model->ForEachAnalogOutput([&](auto&, int) { ++count; });
+  if (count == 0) {
+    return;
+  }
+
+  if (BeginDevice("Analog Outputs")) {
+    model->ForEachAnalogOutput([&](auto& analogOut, int i) {
+      auto analogOutData = analogOut.GetVoltageData();
+      if (!analogOutData) {
+        return;
+      }
+      PushID(i);
+
+      // build label
+      std::string* name = GetStorage().GetStringRef("name");
+      char label[128];
+      if (!name->empty()) {
+        std::snprintf(label, sizeof(label), "%s [%d]###name", name->c_str(), i);
+      } else {
+        std::snprintf(label, sizeof(label), "Out[%d]###name", i);
+      }
+
+      double value = analogOutData->GetValue();
+      DeviceDouble(label, true, &value, analogOutData);
+
+      if (PopupEditName("name", name)) {
+        if (analogOutData) {
+          analogOutData->SetName(name->c_str());
+        }
+      }
+      PopID();
+    });
+
+    EndDevice();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/DIO.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/DIO.cpp
new file mode 100644
index 0000000..59d71f8
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/DIO.cpp
@@ -0,0 +1,121 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/DIO.h"
+
+#include <imgui.h>
+
+#include "glass/DataSource.h"
+#include "glass/hardware/Encoder.h"
+#include "glass/support/IniSaverInfo.h"
+
+using namespace glass;
+
+static void LabelSimDevice(const char* name, const char* simDeviceName) {
+  ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+  ImGui::LabelText(name, "%s", simDeviceName);
+  ImGui::PopStyleColor();
+}
+
+void DisplayDIOImpl(DIOModel* model, int index, bool outputsEnabled) {
+  auto dpwm = model->GetDPWM();
+  auto dutyCycle = model->GetDutyCycle();
+  auto encoder = model->GetEncoder();
+
+  auto dioData = model->GetValueData();
+  auto dpwmData = dpwm ? dpwm->GetValueData() : nullptr;
+  auto dutyCycleData = dutyCycle ? dutyCycle->GetValueData() : nullptr;
+
+  bool exists = model->Exists();
+  auto& info = dioData->GetNameInfo();
+  char label[128];
+  if (exists && dpwmData) {
+    dpwmData->GetNameInfo().GetLabel(label, sizeof(label), "PWM", index);
+    if (auto simDevice = dpwm->GetSimDevice()) {
+      LabelSimDevice(label, simDevice);
+    } else {
+      dpwmData->LabelText(label, "%0.3f", dpwmData->GetValue());
+    }
+  } else if (exists && encoder) {
+    info.GetLabel(label, sizeof(label), " In", index);
+    if (auto simDevice = encoder->GetSimDevice()) {
+      LabelSimDevice(label, simDevice);
+    } else {
+      ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+      ImGui::LabelText(label, "Encoder[%d,%d]", encoder->GetChannelA(),
+                       encoder->GetChannelB());
+      ImGui::PopStyleColor();
+    }
+  } else if (exists && dutyCycleData) {
+    dutyCycleData->GetNameInfo().GetLabel(label, sizeof(label), "Dty", index);
+    if (auto simDevice = dutyCycle->GetSimDevice()) {
+      LabelSimDevice(label, simDevice);
+    } else {
+      double val = dutyCycleData->GetValue();
+      if (dutyCycleData->InputDouble(label, &val)) {
+        dutyCycle->SetValue(val);
+      }
+    }
+  } else {
+    const char* name = model->GetName();
+    if (name[0] != '\0') {
+      info.GetLabel(label, sizeof(label), name);
+    } else {
+      info.GetLabel(label, sizeof(label), model->IsInput() ? " In" : "Out",
+                    index);
+    }
+    if (auto simDevice = model->GetSimDevice()) {
+      LabelSimDevice(label, simDevice);
+    } else {
+      if (!exists) {
+        ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+        dioData->LabelText(label, "unknown");
+        ImGui::PopStyleColor();
+      } else if (model->IsReadOnly()) {
+        dioData->LabelText(
+            label, "%s",
+            outputsEnabled ? (dioData->GetValue() != 0 ? "1 (high)" : "0 (low)")
+                           : "1 (disabled)");
+
+      } else {
+        static const char* options[] = {"0 (low)", "1 (high)"};
+        int val = dioData->GetValue() != 0 ? 1 : 0;
+        if (dioData->Combo(label, &val, options, 2)) {
+          model->SetValue(val);
+        }
+      }
+    }
+  }
+  if (info.PopupEditName(index)) {
+    if (dpwmData) {
+      dpwmData->SetName(info.GetName());
+    }
+    if (dutyCycleData) {
+      dutyCycleData->SetName(info.GetName());
+    }
+  }
+}
+
+void glass::DisplayDIO(DIOModel* model, int index, bool outputsEnabled) {
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
+  DisplayDIOImpl(model, index, outputsEnabled);
+  ImGui::PopItemWidth();
+}
+
+void glass::DisplayDIOs(DIOsModel* model, bool outputsEnabled,
+                        std::string_view noneMsg) {
+  bool hasAny = false;
+
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
+  model->ForEachDIO([&](DIOModel& dio, int i) {
+    hasAny = true;
+    ImGui::PushID(i);
+    DisplayDIOImpl(&dio, i, outputsEnabled);
+    ImGui::PopID();
+  });
+  ImGui::PopItemWidth();
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Encoder.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Encoder.cpp
new file mode 100644
index 0000000..599a5b8
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Encoder.cpp
@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/Encoder.h"
+
+#include <fmt/format.h>
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void EncoderModel::SetName(std::string_view name) {
+  if (name.empty()) {
+    if (auto distancePerPulse = GetDistancePerPulseData()) {
+      distancePerPulse->SetName("");
+    }
+    if (auto count = GetCountData()) {
+      count->SetName("");
+    }
+    if (auto period = GetPeriodData()) {
+      period->SetName("");
+    }
+    if (auto direction = GetDirectionData()) {
+      direction->SetName("");
+    }
+    if (auto distance = GetDistanceData()) {
+      distance->SetName("");
+    }
+    if (auto rate = GetRateData()) {
+      rate->SetName("");
+    }
+  } else {
+    if (auto distancePerPulse = GetDistancePerPulseData()) {
+      distancePerPulse->SetName(fmt::format("{} Distance/Count", name));
+    }
+    if (auto count = GetCountData()) {
+      count->SetName(fmt::format("{} Count", name));
+    }
+    if (auto period = GetPeriodData()) {
+      period->SetName(fmt::format("{} Period", name));
+    }
+    if (auto direction = GetDirectionData()) {
+      direction->SetName(fmt::format("{} Direction", name));
+    }
+    if (auto distance = GetDistanceData()) {
+      distance->SetName(fmt::format("{} Distance", name));
+    }
+    if (auto rate = GetRateData()) {
+      rate->SetName(fmt::format("{} Rate", name));
+    }
+  }
+}
+
+void glass::DisplayEncoder(EncoderModel* model) {
+  if (auto simDevice = model->GetSimDevice()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::TextUnformatted(simDevice);
+    ImGui::PopStyleColor();
+    return;
+  }
+
+  int chA = model->GetChannelA();
+  int chB = model->GetChannelB();
+
+  // build header label
+  std::string* name = GetStorage().GetStringRef("name");
+  char label[128];
+  if (!name->empty()) {
+    std::snprintf(label, sizeof(label), "%s [%d,%d]###name", name->c_str(), chA,
+                  chB);
+  } else {
+    std::snprintf(label, sizeof(label), "Encoder[%d,%d]###name", chA, chB);
+  }
+
+  // header
+  bool open = CollapsingHeader(label);
+
+  // context menu to change name
+  if (PopupEditName("name", name)) {
+    model->SetName(name->c_str());
+  }
+
+  if (!open) {
+    return;
+  }
+
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
+  // distance per pulse
+  if (auto distancePerPulseData = model->GetDistancePerPulseData()) {
+    double value = distancePerPulseData->GetValue();
+    distancePerPulseData->LabelText("Dist/Count", "%.6f", value);
+  }
+
+  // count
+  if (auto countData = model->GetCountData()) {
+    int value = countData->GetValue();
+    if (ImGui::InputInt("##input", &value)) {
+      model->SetCount(value);
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Reset")) {
+      model->SetCount(0);
+    }
+    ImGui::SameLine();
+    ImGui::Selectable("Count");
+    countData->EmitDrag();
+  }
+
+  // max period
+  {
+    double maxPeriod = model->GetMaxPeriod();
+    ImGui::LabelText("Max Period", "%.6f", maxPeriod);
+  }
+
+  // period
+  if (auto periodData = model->GetPeriodData()) {
+    double value = periodData->GetValue();
+    if (periodData->InputDouble("Period", &value, 0, 0, "%.6g")) {
+      model->SetPeriod(value);
+    }
+  }
+
+  // reverse direction
+  ImGui::LabelText("Reverse Direction", "%s",
+                   model->GetReverseDirection() ? "true" : "false");
+
+  // direction
+  if (auto directionData = model->GetDirectionData()) {
+    static const char* options[] = {"reverse", "forward"};
+    int value = directionData->GetValue() ? 1 : 0;
+    if (directionData->Combo("Direction", &value, options, 2)) {
+      model->SetDirection(value != 0);
+    }
+  }
+
+  // distance
+  if (auto distanceData = model->GetDistanceData()) {
+    double value = distanceData->GetValue();
+    if (distanceData->InputDouble("Distance", &value, 0, 0, "%.6g")) {
+      model->SetDistance(value);
+    }
+  }
+
+  // rate
+  if (auto rateData = model->GetRateData()) {
+    double value = rateData->GetValue();
+    if (rateData->InputDouble("Rate", &value, 0, 0, "%.6g")) {
+      model->SetRate(value);
+    }
+  }
+  ImGui::PopItemWidth();
+}
+
+void glass::DisplayEncoders(EncodersModel* model, std::string_view noneMsg) {
+  bool hasAny = false;
+  model->ForEachEncoder([&](EncoderModel& encoder, int i) {
+    hasAny = true;
+    PushID(i);
+    DisplayEncoder(&encoder);
+    PopID();
+  });
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Gyro.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Gyro.cpp
new file mode 100644
index 0000000..36a3525
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Gyro.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/Gyro.h"
+
+#include <cmath>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <wpi/numbers>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayGyro(GyroModel* m) {
+  ImColor primaryColor = ImGui::GetStyle().Colors[ImGuiCol_Text];
+  ImColor disabledColor = ImGui::GetStyle().Colors[ImGuiCol_TextDisabled];
+  ImColor secondaryColor = ImGui::GetStyle().Colors[ImGuiCol_Header];
+
+  auto angle = m->GetAngleData();
+  if (!angle || !m->Exists()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown Gyro");
+    ImGui::PopStyleColor();
+    return;
+  }
+
+  // Display the numeric angle value. This can be editable in some cases (i.e.
+  // running from HALSIM).
+  auto flags =
+      m->IsReadOnly() ? ImGuiInputTextFlags_ReadOnly : ImGuiInputTextFlags_None;
+  auto value = angle->GetValue();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+  if (ImGui::InputDouble("Gyro Angle (Deg)", &value, 0.0, 0.0, "%.4f", flags)) {
+    m->SetAngle(value);
+  }
+
+  // Draw the gyro indicator.
+  ImDrawList* draw = ImGui::GetWindowDrawList();
+  ImVec2 window = ImGui::GetWindowPos();
+  float w = ImGui::GetWindowWidth();
+  float h = ImGui::GetWindowHeight();
+
+  float radius = (w < h) ? w * 0.3 : h * 0.3;
+  ImVec2 center = window + ImVec2(w / 2, h / 2 + ImGui::GetFontSize());
+
+  // Add the primary circle.
+  draw->AddCircle(center, radius, primaryColor, 100, 1.5);
+
+  // Draw the spokes at every 5 degrees and a "major" spoke every 45 degrees.
+  for (int i = -175; i <= 180; i += 5) {
+    double radians = i * 2 * wpi::numbers::pi / 360.0;
+    ImVec2 direction(std::sin(radians), -std::cos(radians));
+
+    bool major = i % 45 == 0;
+    auto color = major ? primaryColor : disabledColor;
+
+    draw->AddLine(center + (direction * radius),
+                  center + (direction * radius * (major ? 1.07f : 1.03f)),
+                  color, 1.2f);
+    if (major) {
+      char txt[16];
+      std::snprintf(txt, sizeof(txt), "%d°", i);
+      draw->AddText(
+          center + (direction * radius * 1.25) - ImGui::CalcTextSize(txt) * 0.5,
+          primaryColor, txt, nullptr);
+    }
+  }
+
+  draw->AddCircleFilled(center, radius * 0.075, secondaryColor, 50);
+
+  double radians = value * 2 * wpi::numbers::pi / 360.0;
+  draw->AddLine(
+      center - ImVec2(1, 0),
+      center + ImVec2(std::sin(radians), -std::cos(radians)) * radius * 0.95f,
+      secondaryColor, 3);
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp
new file mode 100644
index 0000000..c1ece3a
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/LEDDisplay.cpp
@@ -0,0 +1,100 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/LEDDisplay.h"
+
+#include <wpi/SmallVector.h>
+
+#include "glass/Context.h"
+#include "glass/support/ExtraGuiWidgets.h"
+
+using namespace glass;
+
+namespace {
+struct IndicatorData {
+  std::vector<int> values;
+  std::vector<ImU32> colors;
+};
+}  // namespace
+
+void glass::DisplayLEDDisplay(LEDDisplayModel* model, int index) {
+  wpi::SmallVector<LEDDisplayModel::Data, 64> dataBuf;
+  auto data = model->GetData(dataBuf);
+  int length = data.size();
+  bool running = model->IsRunning();
+  auto& storage = GetStorage();
+
+  int* numColumns = storage.GetIntRef("columns", 10);
+  bool* serpentine = storage.GetBoolRef("serpentine", false);
+  int* order = storage.GetIntRef("order", LEDConfig::RowMajor);
+  int* start = storage.GetIntRef("start", LEDConfig::UpperLeft);
+
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
+  ImGui::LabelText("Length", "%d", length);
+  ImGui::LabelText("Running", "%s", running ? "Yes" : "No");
+  ImGui::InputInt("Columns", numColumns);
+  {
+    static const char* options[] = {"Row Major", "Column Major"};
+    ImGui::Combo("Order", order, options, 2);
+  }
+  {
+    static const char* options[] = {"Upper Left", "Lower Left", "Upper Right",
+                                    "Lower Right"};
+    ImGui::Combo("Start", start, options, 4);
+  }
+  ImGui::Checkbox("Serpentine", serpentine);
+  if (*numColumns < 1) {
+    *numColumns = 1;
+  }
+  ImGui::PopItemWidth();
+
+  // show as LED indicators
+  auto iData = storage.GetData<IndicatorData>();
+  if (!iData) {
+    storage.SetData(std::make_shared<IndicatorData>());
+    iData = storage.GetData<IndicatorData>();
+  }
+  if (length > static_cast<int>(iData->values.size())) {
+    iData->values.resize(length);
+  }
+  if (length > static_cast<int>(iData->colors.size())) {
+    iData->colors.resize(length);
+  }
+  if (!running) {
+    iData->colors[0] = IM_COL32(128, 128, 128, 255);
+    for (int j = 0; j < length; ++j) {
+      iData->values[j] = -1;
+    }
+  } else {
+    for (int j = 0; j < length; ++j) {
+      iData->values[j] = j + 1;
+      iData->colors[j] = IM_COL32(data[j].r, data[j].g, data[j].b, 255);
+    }
+  }
+
+  LEDConfig config;
+  config.serpentine = *serpentine;
+  config.order = static_cast<LEDConfig::Order>(*order);
+  config.start = static_cast<LEDConfig::Start>(*start);
+
+  DrawLEDs(iData->values.data(), length, *numColumns, iData->colors.data(), 0,
+           0, config);
+}
+
+void glass::DisplayLEDDisplays(LEDDisplaysModel* model) {
+  bool hasAny = false;
+
+  model->ForEachLEDDisplay([&](LEDDisplayModel& display, int i) {
+    hasAny = true;
+    if (model->GetNumLEDDisplays() > 1) {
+      ImGui::Text("LEDs[%d]", i);
+    }
+    PushID(i);
+    DisplayLEDDisplay(&display, i);
+    PopID();
+  });
+  if (!hasAny) {
+    ImGui::Text("No addressable LEDs");
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PCM.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PCM.cpp
new file mode 100644
index 0000000..23746be
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PCM.cpp
@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/PCM.h"
+
+#include <cstdio>
+#include <cstring>
+
+#include <imgui.h>
+#include <wpi/SmallVector.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+#include "glass/other/DeviceTree.h"
+#include "glass/support/ExtraGuiWidgets.h"
+#include "glass/support/IniSaverInfo.h"
+
+using namespace glass;
+
+bool glass::DisplayPCMSolenoids(PCMModel* model, int index,
+                                bool outputsEnabled) {
+  wpi::SmallVector<int, 16> channels;
+  model->ForEachSolenoid([&](SolenoidModel& solenoid, int j) {
+    if (auto data = solenoid.GetOutputData()) {
+      if (j >= static_cast<int>(channels.size())) {
+        channels.resize(j + 1);
+      }
+      channels[j] = (outputsEnabled && data->GetValue()) ? 1 : -1;
+    }
+  });
+
+  if (channels.empty()) {
+    return false;
+  }
+
+  // show nonexistent channels as empty
+  for (auto&& ch : channels) {
+    if (ch == 0) {
+      ch = -2;
+    }
+  }
+
+  // build header label
+  std::string* name = GetStorage().GetStringRef("name");
+  char label[128];
+  if (!name->empty()) {
+    std::snprintf(label, sizeof(label), "%s [%d]###name", name->c_str(), index);
+  } else {
+    std::snprintf(label, sizeof(label), "PCM[%d]###name", index);
+  }
+
+  // header
+  bool open = CollapsingHeader(label);
+
+  PopupEditName("name", name);
+
+  ImGui::SetItemAllowOverlap();
+  ImGui::SameLine();
+
+  // show channels as LED indicators
+  static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
+                                 IM_COL32(128, 128, 128, 255)};
+  DrawLEDs(channels.data(), channels.size(), channels.size(), colors);
+
+  if (open) {
+    ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+    model->ForEachSolenoid([&](SolenoidModel& solenoid, int j) {
+      if (auto data = solenoid.GetOutputData()) {
+        PushID(j);
+        char solenoidName[64];
+        auto& info = data->GetNameInfo();
+        info.GetLabel(solenoidName, sizeof(solenoidName), "Solenoid", j);
+        data->LabelText(solenoidName, "%s", channels[j] == 1 ? "On" : "Off");
+        info.PopupEditName(j);
+        PopID();
+      }
+    });
+    ImGui::PopItemWidth();
+  }
+
+  return true;
+}
+
+void glass::DisplayPCMsSolenoids(PCMsModel* model, bool outputsEnabled,
+                                 std::string_view noneMsg) {
+  bool hasAny = false;
+  model->ForEachPCM([&](PCMModel& pcm, int i) {
+    PushID(i);
+    if (DisplayPCMSolenoids(&pcm, i, outputsEnabled)) {
+      hasAny = true;
+    }
+    PopID();
+  });
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
+
+void glass::DisplayCompressorDevice(PCMModel* model, int index,
+                                    bool outputsEnabled) {
+  auto compressor = model->GetCompressor();
+  if (!compressor || !compressor->Exists()) {
+    return;
+  }
+  DisplayCompressorDevice(compressor, index, outputsEnabled);
+}
+
+void glass::DisplayCompressorDevice(CompressorModel* model, int index,
+                                    bool outputsEnabled) {
+  char name[32];
+  std::snprintf(name, sizeof(name), "Compressor[%d]", index);
+  if (BeginDevice(name)) {
+    // output enabled
+    if (auto runningData = model->GetRunningData()) {
+      bool value = outputsEnabled && runningData->GetValue();
+      if (DeviceBoolean("Running", false, &value, runningData)) {
+        model->SetRunning(value);
+      }
+    }
+
+    // closed loop enabled
+    if (auto enabledData = model->GetEnabledData()) {
+      int value = enabledData->GetValue() ? 1 : 0;
+      static const char* enabledOptions[] = {"disabled", "enabled"};
+      if (DeviceEnum("Closed Loop", true, &value, enabledOptions, 2,
+                     enabledData)) {
+        model->SetEnabled(value != 0);
+      }
+    }
+
+    // pressure switch
+    if (auto pressureSwitchData = model->GetPressureSwitchData()) {
+      int value = pressureSwitchData->GetValue() ? 1 : 0;
+      static const char* switchOptions[] = {"full", "low"};
+      if (DeviceEnum("Pressure", false, &value, switchOptions, 2,
+                     pressureSwitchData)) {
+        model->SetPressureSwitch(value != 0);
+      }
+    }
+
+    // compressor current
+    if (auto currentData = model->GetCurrentData()) {
+      double value = currentData->GetValue();
+      if (DeviceDouble("Current (A)", false, &value, currentData)) {
+        model->SetCurrent(value);
+      }
+    }
+
+    EndDevice();
+  }
+}
+
+void glass::DisplayCompressorsDevice(PCMsModel* model, bool outputsEnabled) {
+  model->ForEachPCM([&](PCMModel& pcm, int i) {
+    DisplayCompressorDevice(&pcm, i, outputsEnabled);
+  });
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PWM.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PWM.cpp
new file mode 100644
index 0000000..3ff8e52
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PWM.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/PWM.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayPWM(PWMModel* model, int index, bool outputsEnabled) {
+  auto data = model->GetSpeedData();
+  if (!data) {
+    return;
+  }
+
+  // build label
+  std::string* name = GetStorage().GetStringRef("name");
+  char label[128];
+  if (!name->empty()) {
+    std::snprintf(label, sizeof(label), "%s [%d]###name", name->c_str(), index);
+  } else {
+    std::snprintf(label, sizeof(label), "PWM[%d]###name", index);
+  }
+
+  int led = model->GetAddressableLED();
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+  if (led >= 0) {
+    ImGui::LabelText(label, "LED[%d]", led);
+  } else {
+    float val = outputsEnabled ? data->GetValue() : 0;
+    data->LabelText(label, "%0.3f", val);
+  }
+  if (PopupEditName("name", name)) {
+    data->SetName(name->c_str());
+  }
+}
+
+void glass::DisplayPWMs(PWMsModel* model, bool outputsEnabled,
+                        std::string_view noneMsg) {
+  bool hasAny = false;
+  bool first = true;
+  model->ForEachPWM([&](PWMModel& pwm, int i) {
+    hasAny = true;
+    PushID(i);
+
+    if (!first) {
+      ImGui::Separator();
+    } else {
+      first = false;
+    }
+
+    DisplayPWM(&pwm, i, outputsEnabled);
+    PopID();
+  });
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PowerDistribution.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PowerDistribution.cpp
new file mode 100644
index 0000000..46e550b
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/PowerDistribution.cpp
@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/PowerDistribution.h"
+
+#include <algorithm>
+#include <cstdio>
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+#include "glass/support/IniSaverInfo.h"
+
+using namespace glass;
+
+static float DisplayChannel(PowerDistributionModel& pdp, int channel) {
+  float width = 0;
+  if (auto currentData = pdp.GetCurrentData(channel)) {
+    ImGui::PushID(channel);
+    auto& leftInfo = currentData->GetNameInfo();
+    char name[64];
+    leftInfo.GetLabel(name, sizeof(name), "", channel);
+    double val = currentData->GetValue();
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+    if (currentData->InputDouble(name, &val, 0, 0, "%.3f")) {
+      pdp.SetCurrent(channel, val);
+    }
+    width = ImGui::GetItemRectSize().x;
+    leftInfo.PopupEditName(channel);
+    ImGui::PopID();
+  }
+  return width;
+}
+
+void glass::DisplayPowerDistribution(PowerDistributionModel* model, int index) {
+  char name[128];
+  std::snprintf(name, sizeof(name), "PowerDistribution[%d]", index);
+  if (CollapsingHeader(name)) {
+    // temperature
+    if (auto tempData = model->GetTemperatureData()) {
+      double value = tempData->GetValue();
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      if (tempData->InputDouble("Temp", &value, 0, 0, "%.3f")) {
+        model->SetTemperature(value);
+      }
+    }
+
+    // voltage
+    if (auto voltageData = model->GetVoltageData()) {
+      double value = voltageData->GetValue();
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      if (voltageData->InputDouble("Voltage", &value, 0, 0, "%.3f")) {
+        model->SetVoltage(value);
+      }
+    }
+
+    // channel currents; show as two columns laid out like PowerDistribution
+    const int numChannels = model->GetNumChannels();
+    ImGui::Text("Channel Current (A)");
+    ImGui::Columns(2, "channels", false);
+    float maxWidth = ImGui::GetFontSize() * 13;
+    for (int left = 0, right = numChannels - 1; left < right; ++left, --right) {
+      float leftWidth = DisplayChannel(*model, left);
+      ImGui::NextColumn();
+
+      float rightWidth = DisplayChannel(*model, right);
+      ImGui::NextColumn();
+
+      float width =
+          (std::max)(leftWidth, rightWidth) * 2 + ImGui::GetFontSize() * 4;
+      if (width > maxWidth) {
+        maxWidth = width;
+      }
+    }
+    ImGui::Columns(1);
+    ImGui::Dummy(ImVec2(maxWidth, 0));
+  }
+}
+
+void glass::DisplayPowerDistributions(PowerDistributionsModel* model,
+                                      std::string_view noneMsg) {
+  bool hasAny = false;
+  model->ForEachPowerDistribution([&](PowerDistributionModel& pdp, int i) {
+    hasAny = true;
+    PushID(i);
+    DisplayPowerDistribution(&pdp, i);
+    PopID();
+  });
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Relay.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Relay.cpp
new file mode 100644
index 0000000..59bbc51
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/Relay.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/Relay.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+#include "glass/support/ExtraGuiWidgets.h"
+
+using namespace glass;
+
+void glass::DisplayRelay(RelayModel* model, int index, bool outputsEnabled) {
+  auto forwardData = model->GetForwardData();
+  auto reverseData = model->GetReverseData();
+
+  if (!forwardData && !reverseData) {
+    return;
+  }
+
+  bool forward = false;
+  bool reverse = false;
+  if (outputsEnabled) {
+    if (forwardData) {
+      forward = forwardData->GetValue();
+    }
+    if (reverseData) {
+      reverse = reverseData->GetValue();
+    }
+  }
+
+  std::string* name = GetStorage().GetStringRef("name");
+  ImGui::PushID("name");
+  if (!name->empty()) {
+    ImGui::Text("%s [%d]", name->c_str(), index);
+  } else {
+    ImGui::Text("Relay[%d]", index);
+  }
+  ImGui::PopID();
+  if (PopupEditName("name", name)) {
+    if (forwardData) {
+      forwardData->SetName(name->c_str());
+    }
+    if (reverseData) {
+      reverseData->SetName(name->c_str());
+    }
+  }
+  ImGui::SameLine();
+
+  // show forward and reverse as LED indicators
+  static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
+                                 IM_COL32(255, 0, 0, 255),
+                                 IM_COL32(128, 128, 128, 255)};
+  int values[2] = {reverseData ? (reverse ? 2 : -2) : -3,
+                   forwardData ? (forward ? 1 : -1) : -3};
+  DataSource* sources[2] = {reverseData, forwardData};
+  DrawLEDSources(values, sources, 2, 2, colors);
+}
+
+void glass::DisplayRelays(RelaysModel* model, bool outputsEnabled,
+                          std::string_view noneMsg) {
+  bool hasAny = false;
+  bool first = true;
+  model->ForEachRelay([&](RelayModel& relay, int i) {
+    hasAny = true;
+
+    if (!first) {
+      ImGui::Separator();
+    } else {
+      first = false;
+    }
+
+    PushID(i);
+    DisplayRelay(&relay, i, outputsEnabled);
+    PopID();
+  });
+  if (!hasAny && !noneMsg.empty()) {
+    ImGui::TextUnformatted(noneMsg.data(), noneMsg.data() + noneMsg.size());
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/RoboRio.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/RoboRio.cpp
new file mode 100644
index 0000000..d0667d9
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/RoboRio.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/RoboRio.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+static void DisplayRail(RoboRioRailModel& rail, const char* name) {
+  if (CollapsingHeader(name)) {
+    ImGui::PushID(name);
+    if (auto data = rail.GetVoltageData()) {
+      double val = data->GetValue();
+      if (data->InputDouble("Voltage (V)", &val)) {
+        rail.SetVoltage(val);
+      }
+    }
+
+    if (auto data = rail.GetCurrentData()) {
+      double val = data->GetValue();
+      if (data->InputDouble("Current (A)", &val)) {
+        rail.SetCurrent(val);
+      }
+    }
+
+    if (auto data = rail.GetActiveData()) {
+      static const char* options[] = {"inactive", "active"};
+      int val = data->GetValue() ? 1 : 0;
+      if (data->Combo("Active", &val, options, 2)) {
+        rail.SetActive(val);
+      }
+    }
+
+    if (auto data = rail.GetFaultsData()) {
+      int val = data->GetValue();
+      if (data->InputInt("Faults", &val)) {
+        rail.SetFaults(val);
+      }
+    }
+    ImGui::PopID();
+  }
+}
+
+void glass::DisplayRoboRio(RoboRioModel* model) {
+  ImGui::Button("User Button");
+  model->SetUserButton(ImGui::IsItemActive());
+
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
+
+  if (CollapsingHeader("RoboRIO Input")) {
+    ImGui::PushID("RoboRIO Input");
+    if (auto data = model->GetVInVoltageData()) {
+      double val = data->GetValue();
+      if (data->InputDouble("Voltage (V)", &val)) {
+        model->SetVInVoltage(val);
+      }
+    }
+
+    if (auto data = model->GetVInCurrentData()) {
+      double val = data->GetValue();
+      if (data->InputDouble("Current (A)", &val)) {
+        model->SetVInCurrent(val);
+      }
+    }
+    ImGui::PopID();
+  }
+
+  if (auto rail = model->GetUser6VRail()) {
+    DisplayRail(*rail, "6V Rail");
+  }
+  if (auto rail = model->GetUser5VRail()) {
+    DisplayRail(*rail, "5V Rail");
+  }
+  if (auto rail = model->GetUser3V3Rail()) {
+    DisplayRail(*rail, "3.3V Rail");
+  }
+
+  ImGui::PopItemWidth();
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/hardware/SpeedController.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/SpeedController.cpp
new file mode 100644
index 0000000..b278401
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/hardware/SpeedController.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/hardware/SpeedController.h"
+
+#include <imgui.h>
+#include <imgui_internal.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplaySpeedController(SpeedControllerModel* m) {
+  // Get duty cycle data from the model and do not display anything if the data
+  // is null.
+  auto dc = m->GetPercentData();
+  if (!dc || !m->Exists()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown SpeedController");
+    ImGui::PopStyleColor();
+    return;
+  }
+
+  // Set the buttons and sliders to read-only if the model is read-only.
+  if (m->IsReadOnly()) {
+    ImGui::PushItemFlag(ImGuiItemFlags_Disabled, true);
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(210, 210, 210, 255));
+  }
+
+  // Add button to zero output.
+  if (ImGui::Button("Zero")) {
+    m->SetPercent(0.0);
+  }
+  ImGui::SameLine();
+
+  // Display a slider for the data.
+  float value = dc->GetValue();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+
+  if (dc->SliderFloat("% Output", &value, -1.0f, 1.0f)) {
+    m->SetPercent(value);
+  }
+
+  if (m->IsReadOnly()) {
+    ImGui::PopStyleColor();
+    ImGui::PopItemFlag();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/CommandScheduler.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/CommandScheduler.cpp
new file mode 100644
index 0000000..83e4118
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/CommandScheduler.cpp
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/CommandScheduler.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayCommandScheduler(CommandSchedulerModel* m) {
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 20);
+  ImGui::Text("Scheduled Commands: ");
+  ImGui::Separator();
+  ImGui::Spacing();
+
+  if (m->Exists()) {
+    float pos = ImGui::GetContentRegionAvail().x * 0.97f -
+                ImGui::CalcTextSize("Cancel").x;
+
+    const auto& commands = m->GetCurrentCommands();
+    for (size_t i = 0; i < commands.size(); ++i) {
+      ImGui::Text("%s", commands[i].c_str());
+      ImGui::SameLine(pos);
+
+      ImGui::PushID(i);
+      if (ImGui::Button("Cancel")) {
+        m->CancelCommand(i);
+      }
+      ImGui::PopID();
+    }
+  } else {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown Scheduler");
+    ImGui::PopStyleColor();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/CommandSelector.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/CommandSelector.cpp
new file mode 100644
index 0000000..d2124c3
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/CommandSelector.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/CommandSelector.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayCommandSelector(CommandSelectorModel* m) {
+  if (auto name = m->GetName()) {
+    ImGui::Text("%s", name);
+  }
+  if (m->Exists()) {
+    if (auto run = m->GetRunningData()) {
+      bool running = run->GetValue();
+      if (ImGui::Button(running ? "Cancel" : "Run")) {
+        running = !running;
+        m->SetRunning(running);
+      }
+      ImGui::SameLine();
+      if (running) {
+        ImGui::Text("Running...");
+      }
+    }
+  } else {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown Command");
+    ImGui::PopStyleColor();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/DeviceTree.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/DeviceTree.cpp
new file mode 100644
index 0000000..cd69eb7
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/DeviceTree.cpp
@@ -0,0 +1,173 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/DeviceTree.h"
+
+#include <cinttypes>
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/ContextInternal.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void DeviceTreeModel::Update() {
+  for (auto&& display : m_displays) {
+    if (display.first) {
+      display.first->Update();
+    }
+  }
+}
+
+bool DeviceTreeModel::Exists() {
+  for (auto&& display : m_displays) {
+    if (display.first && display.first->Exists()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void DeviceTreeModel::Display() {
+  for (auto&& display : m_displays) {
+    if (display.second) {
+      display.second(display.first);
+    }
+  }
+}
+
+void glass::HideDevice(const char* id) {
+  gContext->deviceHidden[id] = true;
+}
+
+bool glass::BeginDevice(const char* id, ImGuiTreeNodeFlags flags) {
+  if (gContext->deviceHidden[id]) {
+    return false;
+  }
+
+  PushID(id);
+
+  // build label
+  std::string* name = GetStorage().GetStringRef("name");
+  char label[128];
+  std::snprintf(label, sizeof(label), "%s###name",
+                name->empty() ? id : name->c_str());
+
+  bool open = CollapsingHeader(label, flags);
+  PopupEditName("name", name);
+
+  if (!open) {
+    PopID();
+  }
+  return open;
+}
+
+void glass::EndDevice() {
+  PopID();
+}
+
+static bool DeviceBooleanImpl(const char* name, bool readonly, bool* value) {
+  if (readonly) {
+    ImGui::LabelText(name, "%s", *value ? "true" : "false");
+  } else {
+    static const char* boolOptions[] = {"false", "true"};
+    int val = *value ? 1 : 0;
+    if (ImGui::Combo(name, &val, boolOptions, 2)) {
+      *value = val;
+      return true;
+    }
+  }
+  return false;
+}
+
+static bool DeviceDoubleImpl(const char* name, bool readonly, double* value) {
+  if (readonly) {
+    ImGui::LabelText(name, "%.6f", *value);
+    return false;
+  } else {
+    return ImGui::InputDouble(name, value, 0, 0, "%.6f",
+                              ImGuiInputTextFlags_EnterReturnsTrue);
+  }
+}
+
+static bool DeviceEnumImpl(const char* name, bool readonly, int* value,
+                           const char** options, int32_t numOptions) {
+  if (readonly) {
+    if (*value < 0 || *value >= numOptions) {
+      ImGui::LabelText(name, "%d (unknown)", *value);
+    } else {
+      ImGui::LabelText(name, "%s", options[*value]);
+    }
+    return false;
+  } else {
+    return ImGui::Combo(name, value, options, numOptions);
+  }
+}
+
+static bool DeviceIntImpl(const char* name, bool readonly, int32_t* value) {
+  if (readonly) {
+    ImGui::LabelText(name, "%" PRId32, *value);
+    return false;
+  } else {
+    return ImGui::InputScalar(name, ImGuiDataType_S32, value, nullptr, nullptr,
+                              nullptr, ImGuiInputTextFlags_EnterReturnsTrue);
+  }
+}
+
+static bool DeviceLongImpl(const char* name, bool readonly, int64_t* value) {
+  if (readonly) {
+    ImGui::LabelText(name, "%" PRId64, *value);
+    return false;
+  } else {
+    return ImGui::InputScalar(name, ImGuiDataType_S64, value, nullptr, nullptr,
+                              nullptr, ImGuiInputTextFlags_EnterReturnsTrue);
+  }
+}
+
+template <typename F, typename... Args>
+static inline bool DeviceValueImpl(const char* name, bool readonly,
+                                   const DataSource* source, F&& func,
+                                   Args... args) {
+  ImGui::SetNextItemWidth(ImGui::GetWindowWidth() * 0.5f);
+  if (!source) {
+    return func(name, readonly, args...);
+  } else {
+    ImGui::PushID(name);
+    bool rv = func("", readonly, args...);
+    ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
+    ImGui::Selectable(name);
+    source->EmitDrag();
+    ImGui::PopID();
+    return rv;
+  }
+}
+
+bool glass::DeviceBoolean(const char* name, bool readonly, bool* value,
+                          const DataSource* source) {
+  return DeviceValueImpl(name, readonly, source, DeviceBooleanImpl, value);
+}
+
+bool glass::DeviceDouble(const char* name, bool readonly, double* value,
+                         const DataSource* source) {
+  return DeviceValueImpl(name, readonly, source, DeviceDoubleImpl, value);
+}
+
+bool glass::DeviceEnum(const char* name, bool readonly, int* value,
+                       const char** options, int32_t numOptions,
+                       const DataSource* source) {
+  return DeviceValueImpl(name, readonly, source, DeviceEnumImpl, value, options,
+                         numOptions);
+}
+
+bool glass::DeviceInt(const char* name, bool readonly, int32_t* value,
+                      const DataSource* source) {
+  return DeviceValueImpl(name, readonly, source, DeviceIntImpl, value);
+}
+
+bool glass::DeviceLong(const char* name, bool readonly, int64_t* value,
+                       const DataSource* source) {
+  return DeviceValueImpl(name, readonly, source, DeviceLongImpl, value);
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/Drive.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/Drive.cpp
new file mode 100644
index 0000000..9dc1675
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/Drive.cpp
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/Drive.h"
+
+#include <array>
+#include <cmath>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <wpi/numbers>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayDrive(DriveModel* m) {
+  // Check if the model exists.
+  if (!m->Exists()) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown Drive");
+    ImGui::PopStyleColor();
+    return;
+  }
+
+  const auto& wheels = m->GetWheels();
+  ImDrawList* draw = ImGui::GetWindowDrawList();
+  ImColor color = ImGui::GetStyle().Colors[ImGuiCol_Text];
+
+  // Get window position and size.
+  ImVec2 pos = ImGui::GetWindowPos();
+  ImVec2 size = ImGui::GetWindowSize();
+
+  // Calculate corners for drivetrain body.
+  float x1 = pos.x + 60.0f;
+  float y1 = pos.y + ImGui::GetFontSize() * 2.0f;
+  float x2 = pos.x + size.x - 60.0f;
+  float y2 = pos.y + size.y - ImGui::GetFontSize() * 2.0f * wheels.size();
+
+  // Draw the primary rectangle.
+  draw->AddRect(ImVec2(x1, y1), ImVec2(x2, y2), color);
+
+  // Display the speed vector.
+  ImVec2 center{(x1 + x2) / 2.0f, (y1 + y2) / 2.0f};
+  ImVec2 speed = m->GetSpeedVector();
+  ImVec2 arrow = center + speed * 50.0f;
+
+  draw->AddLine(center, arrow, color, 2.0f);
+
+  auto drawArrow = [draw, &color](const ImVec2& arrowPos, float angle) {
+    draw->AddTriangleFilled(
+        arrowPos,
+        arrowPos + ImRotate(ImVec2(0.0f, 7.5f),
+                            std::cos(angle + wpi::numbers::pi / 4),
+                            std::sin(angle + wpi::numbers::pi / 4)),
+        arrowPos + ImRotate(ImVec2(0.0f, 7.5f),
+                            std::cos(angle - wpi::numbers::pi / 4),
+                            std::sin(angle - wpi::numbers::pi / 4)),
+        color);
+  };
+
+  // Draw the arrow if there is any translation; draw an X otherwise.
+  if (std::abs(speed.y) > 0 || std::abs(speed.x) > 0) {
+    drawArrow(arrow, std::atan2(speed.x, -speed.y));
+  } else {
+    ImVec2 a{7.5f, +7.5f};
+    ImVec2 b{7.5f, -7.5f};
+    draw->AddLine(center + a, center - a, color);
+    draw->AddLine(center + b, center - b, color);
+  }
+
+  // Calculate the positions of the top-left corner of the wheels.
+  std::array<ImVec2, 4> corners{
+      ImVec2(x1 - 25.0f, y1 + 10.0f), ImVec2(x1 - 25.0f, y2 - 70.0f),
+      ImVec2(x2 + 00.0f, y1 + 10.0f), ImVec2(x2 + 00.0f, y2 - 70.0f)};
+
+  // Draw the wheels.
+  for (auto&& corner : corners) {
+    draw->AddRect(corner, corner + ImVec2(25.0f, 60.0f), color);
+  }
+
+  // Show rotation
+  double rotation = m->GetRotation();
+  if (rotation != 0) {
+    float radius = 60.0f;
+    double a1 = 0.0;
+    double a2 = wpi::numbers::pi / 2 * rotation;
+
+    draw->PathArcTo(center, radius, a1, a2, 20);
+    draw->PathStroke(color, false);
+    draw->PathArcTo(center, radius, a1 + wpi::numbers::pi,
+                    a2 + wpi::numbers::pi, 20);
+    draw->PathStroke(color, false);
+
+    double adder = rotation < 0 ? wpi::numbers::pi : 0;
+
+    auto arrowPos =
+        center + ImVec2(radius * -std::cos(a2), radius * -std::sin(a2));
+    drawArrow(arrowPos, a2 + adder);
+
+    a2 += wpi::numbers::pi;
+    arrowPos = center + ImVec2(radius * -std::cos(a2), radius * -std::sin(a2));
+    drawArrow(arrowPos, a2 + adder);
+  }
+
+  // Set the buttons and sliders to read-only if the model is read-only.
+  if (m->IsReadOnly()) {
+    ImGui::PushItemFlag(ImGuiItemFlags_Disabled, true);
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(210, 210, 210, 255));
+  }
+
+  // Add sliders for the wheel percentages.
+  ImGui::SetCursorPosY(y2 - pos.y + ImGui::GetFontSize() * 0.5);
+  for (auto&& wheel : wheels) {
+    if (wheel.percent) {
+      ImGui::PushID(wheel.name.c_str());
+      if (ImGui::Button("Zero")) {
+        wheel.setter(0.0);
+      }
+      ImGui::PopID();
+
+      ImGui::SameLine();
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8.0f);
+      float value = wheel.percent->GetValue();
+      if (wheel.percent->SliderFloat(wheel.name.c_str(), &value, -1.0f, 1.0f)) {
+        wheel.setter(value);
+      }
+    }
+  }
+
+  if (m->IsReadOnly()) {
+    ImGui::PopStyleColor();
+    ImGui::PopItemFlag();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/FMS.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/FMS.cpp
new file mode 100644
index 0000000..a19cad4
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/FMS.cpp
@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/FMS.h"
+
+#include <imgui.h>
+#include <wpi/SmallString.h>
+
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+static const char* stations[] = {"Red 1",  "Red 2",  "Red 3",
+                                 "Blue 1", "Blue 2", "Blue 3"};
+
+void glass::DisplayFMS(FMSModel* model, bool* matchTimeEnabled) {
+  if (!model->Exists() || model->IsReadOnly()) {
+    return DisplayFMSReadOnly(model);
+  }
+
+  // FMS Attached
+  if (auto data = model->GetFmsAttachedData()) {
+    bool val = data->GetValue();
+    if (ImGui::Checkbox("FMS Attached", &val)) {
+      model->SetFmsAttached(val);
+    }
+    data->EmitDrag();
+  }
+
+  // DS Attached
+  if (auto data = model->GetDsAttachedData()) {
+    bool val = data->GetValue();
+    if (ImGui::Checkbox("DS Attached", &val)) {
+      model->SetDsAttached(val);
+    }
+    data->EmitDrag();
+  }
+
+  // Alliance Station
+  if (auto data = model->GetAllianceStationIdData()) {
+    int val = data->GetValue();
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+    if (ImGui::Combo("Alliance Station", &val, stations, 6)) {
+      model->SetAllianceStationId(val);
+    }
+    data->EmitDrag();
+  }
+
+  // Match Time
+  if (auto data = model->GetMatchTimeData()) {
+    if (matchTimeEnabled) {
+      ImGui::Checkbox("Match Time Enabled", matchTimeEnabled);
+    }
+
+    double val = data->GetValue();
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+    if (ImGui::InputDouble("Match Time", &val, 0, 0, "%.1f",
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+      model->SetMatchTime(val);
+    }
+    data->EmitDrag();
+    ImGui::SameLine();
+    if (ImGui::Button("Reset")) {
+      model->SetMatchTime(0.0);
+    }
+  }
+
+  // Game Specific Message
+  // make buffer full 64 width, null terminated, for editability
+  wpi::SmallString<64> gameSpecificMessage;
+  model->GetGameSpecificMessage(gameSpecificMessage);
+  gameSpecificMessage.resize(63);
+  gameSpecificMessage.push_back('\0');
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+  if (ImGui::InputText("Game Specific", gameSpecificMessage.data(),
+                       gameSpecificMessage.size(),
+                       ImGuiInputTextFlags_EnterReturnsTrue)) {
+    model->SetGameSpecificMessage(gameSpecificMessage.data());
+  }
+}
+
+void glass::DisplayFMSReadOnly(FMSModel* model) {
+  bool exists = model->Exists();
+  if (!exists) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+  }
+
+  if (auto data = model->GetEStopData()) {
+    ImGui::Selectable("E-Stopped: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?");
+  }
+  if (auto data = model->GetEnabledData()) {
+    ImGui::Selectable("Robot Enabled: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?");
+  }
+  if (auto data = model->GetTestData()) {
+    ImGui::Selectable("Test Mode: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?");
+  }
+  if (auto data = model->GetAutonomousData()) {
+    ImGui::Selectable("Autonomous Mode: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?");
+  }
+  if (auto data = model->GetFmsAttachedData()) {
+    ImGui::Selectable("FMS Attached: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?");
+  }
+  if (auto data = model->GetDsAttachedData()) {
+    ImGui::Selectable("DS Attached: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? (data->GetValue() ? "Yes" : "No") : "?");
+  }
+  if (auto data = model->GetAllianceStationIdData()) {
+    ImGui::Selectable("Alliance Station: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    ImGui::TextUnformatted(exists ? stations[static_cast<int>(data->GetValue())]
+                                  : "?");
+  }
+  if (auto data = model->GetMatchTimeData()) {
+    ImGui::Selectable("Match Time: ");
+    data->EmitDrag();
+    ImGui::SameLine();
+    if (exists) {
+      ImGui::Text("%.1f", data->GetValue());
+    } else {
+      ImGui::TextUnformatted("?");
+    }
+  }
+
+  wpi::SmallString<64> gameSpecificMessage;
+  model->GetGameSpecificMessage(gameSpecificMessage);
+  ImGui::Text("Game Specific: %s", exists ? gameSpecificMessage.c_str() : "?");
+
+  if (!exists) {
+    ImGui::PopStyleColor();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/Field2D.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/Field2D.cpp
new file mode 100644
index 0000000..ec0210e
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/Field2D.cpp
@@ -0,0 +1,1227 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/Field2D.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <memory>
+#include <string_view>
+#include <utility>
+
+#include <fmt/format.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <portable-file-dialogs.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
+#include <wpi/StringMap.h>
+#include <wpi/fs.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpigui.h>
+
+#include "glass/Context.h"
+
+using namespace glass;
+
+namespace gui = wpi::gui;
+
+namespace {
+
+enum DisplayUnits { kDisplayMeters = 0, kDisplayFeet, kDisplayInches };
+
+// Per-frame field data (not persistent)
+struct FieldFrameData {
+  frc::Translation2d GetPosFromScreen(const ImVec2& cursor) const {
+    return {
+        units::meter_t{(std::clamp(cursor.x, min.x, max.x) - min.x) / scale},
+        units::meter_t{(max.y - std::clamp(cursor.y, min.y, max.y)) / scale}};
+  }
+  ImVec2 GetScreenFromPos(const frc::Translation2d& pos) const {
+    return {min.x + scale * pos.X().to<float>(),
+            max.y - scale * pos.Y().to<float>()};
+  }
+
+  // in screen coordinates
+  ImVec2 imageMin;
+  ImVec2 imageMax;
+  ImVec2 min;
+  ImVec2 max;
+
+  float scale;  // scaling from meters to screen units
+};
+
+// Pose drag target info
+struct SelectedTargetInfo {
+  FieldObjectModel* objModel = nullptr;
+  std::string name;
+  size_t index;
+  units::radian_t rot;
+  ImVec2 poseCenter;  // center of the pose (screen coordinates)
+  ImVec2 center;      // center of the target (screen coordinates)
+  float radius;       // target radius
+  float dist;         // distance from center to mouse
+  int corner;         // corner (1 = center)
+};
+
+// Pose drag state
+struct PoseDragState {
+  SelectedTargetInfo target;
+  ImVec2 initialOffset;
+  units::radian_t initialAngle = 0_rad;
+};
+
+// Popup edit state
+class PopupState {
+ public:
+  void Open(SelectedTargetInfo* target, const frc::Translation2d& pos);
+  void Close();
+
+  SelectedTargetInfo* GetTarget() { return &m_target; }
+  FieldObjectModel* GetInsertModel() { return m_insertModel; }
+  wpi::span<const frc::Pose2d> GetInsertPoses() const { return m_insertPoses; }
+
+  void Display(Field2DModel* model, const FieldFrameData& ffd);
+
+ private:
+  void DisplayTarget(Field2DModel* model, const FieldFrameData& ffd);
+  void DisplayInsert(Field2DModel* model);
+
+  SelectedTargetInfo m_target;
+
+  // for insert
+  FieldObjectModel* m_insertModel;
+  std::vector<frc::Pose2d> m_insertPoses;
+  std::string m_insertName;
+  int m_insertIndex;
+};
+
+struct DisplayOptions {
+  explicit DisplayOptions(const gui::Texture& texture) : texture{texture} {}
+
+  enum Style { kBoxImage = 0, kLine, kLineClosed, kTrack };
+
+  static constexpr Style kDefaultStyle = kBoxImage;
+  static constexpr float kDefaultWeight = 4.0f;
+  static constexpr ImU32 kDefaultColor = IM_COL32(255, 0, 0, 255);
+  static constexpr auto kDefaultWidth = 0.6858_m;
+  static constexpr auto kDefaultLength = 0.8204_m;
+  static constexpr bool kDefaultArrows = true;
+  static constexpr int kDefaultArrowSize = 50;
+  static constexpr float kDefaultArrowWeight = 4.0f;
+  static constexpr ImU32 kDefaultArrowColor = IM_COL32(0, 255, 0, 255);
+  static constexpr bool kDefaultSelectable = true;
+
+  Style style = kDefaultStyle;
+  float weight = kDefaultWeight;
+  int color = kDefaultColor;
+
+  units::meter_t width = kDefaultWidth;
+  units::meter_t length = kDefaultLength;
+
+  bool arrows = kDefaultArrows;
+  int arrowSize = kDefaultArrowSize;
+  float arrowWeight = kDefaultArrowWeight;
+  int arrowColor = kDefaultArrowColor;
+
+  bool selectable = kDefaultSelectable;
+
+  const gui::Texture& texture;
+};
+
+// Per-frame pose data (not persistent)
+class PoseFrameData {
+ public:
+  explicit PoseFrameData(const frc::Pose2d& pose, FieldObjectModel& model,
+                         size_t index, const FieldFrameData& ffd,
+                         const DisplayOptions& displayOptions);
+  void SetPosition(const frc::Translation2d& pos);
+  void SetRotation(units::radian_t rot);
+  const frc::Rotation2d& GetRotation() const { return m_pose.Rotation(); }
+  const frc::Pose2d& GetPose() const { return m_pose; }
+  float GetHitRadius() const { return m_hitRadius; }
+  void UpdateFrameData();
+  std::pair<int, float> IsHovered(const ImVec2& cursor) const;
+  SelectedTargetInfo GetDragTarget(int corner, float dist) const;
+  void HandleDrag(const ImVec2& cursor);
+  void Draw(ImDrawList* drawList, std::vector<ImVec2>* center,
+            std::vector<ImVec2>* left, std::vector<ImVec2>* right) const;
+
+  // in window coordinates
+  ImVec2 m_center;
+  ImVec2 m_corners[6];  // 5 and 6 are used for track width
+  ImVec2 m_arrow[3];
+
+ private:
+  FieldObjectModel& m_model;
+  size_t m_index;
+  const FieldFrameData& m_ffd;
+  const DisplayOptions& m_displayOptions;
+
+  // scaled width/2 and length/2, in screen units
+  float m_width2;
+  float m_length2;
+
+  float m_hitRadius;
+
+  frc::Pose2d m_pose;
+};
+
+class ObjectInfo {
+ public:
+  ObjectInfo();
+
+  DisplayOptions GetDisplayOptions() const;
+  void DisplaySettings();
+  void DrawLine(ImDrawList* drawList, wpi::span<const ImVec2> points) const;
+
+  void LoadImage();
+  const gui::Texture& GetTexture() const { return m_texture; }
+
+ private:
+  void Reset();
+  bool LoadImageImpl(const char* fn);
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+
+  // in meters
+  float* m_pWidth;
+  float* m_pLength;
+
+  int* m_pStyle;  // DisplayOptions::Style
+  float* m_pWeight;
+  int* m_pColor;
+
+  bool* m_pArrows;
+  int* m_pArrowSize;
+  float* m_pArrowWeight;
+  int* m_pArrowColor;
+
+  bool* m_pSelectable;
+
+  std::string* m_pFilename;
+  gui::Texture m_texture;
+};
+
+class FieldInfo {
+ public:
+  static constexpr auto kDefaultWidth = 15.98_m;
+  static constexpr auto kDefaultHeight = 8.21_m;
+
+  FieldInfo();
+
+  void DisplaySettings();
+
+  void LoadImage();
+  FieldFrameData GetFrameData(ImVec2 min, ImVec2 max) const;
+  void Draw(ImDrawList* drawList, const FieldFrameData& frameData) const;
+
+  wpi::StringMap<std::unique_ptr<ObjectInfo>> m_objects;
+
+ private:
+  void Reset();
+  bool LoadImageImpl(const char* fn);
+  void LoadJson(std::string_view jsonfile);
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+
+  std::string* m_pFilename;
+  gui::Texture m_texture;
+
+  // in meters
+  float* m_pWidth;
+  float* m_pHeight;
+
+  // in image pixels
+  int m_imageWidth;
+  int m_imageHeight;
+  int* m_pTop;
+  int* m_pLeft;
+  int* m_pBottom;
+  int* m_pRight;
+};
+
+}  // namespace
+
+static PoseDragState gDragState;
+static PopupState gPopupState;
+static DisplayUnits gDisplayUnits = kDisplayMeters;
+
+static double ConvertDisplayLength(units::meter_t v) {
+  switch (gDisplayUnits) {
+    case kDisplayFeet:
+      return v.convert<units::feet>().value();
+    case kDisplayInches:
+      return v.convert<units::inches>().value();
+    case kDisplayMeters:
+    default:
+      return v.value();
+  }
+}
+
+static double ConvertDisplayAngle(units::degree_t v) {
+  return v.value();
+}
+
+static bool InputLength(const char* label, units::meter_t* v, double step = 0.0,
+                        double step_fast = 0.0, const char* format = "%.6f",
+                        ImGuiInputTextFlags flags = 0) {
+  double dv = ConvertDisplayLength(*v);
+  if (ImGui::InputDouble(label, &dv, step, step_fast, format, flags)) {
+    switch (gDisplayUnits) {
+      case kDisplayFeet:
+        *v = units::foot_t{dv};
+        break;
+      case kDisplayInches:
+        *v = units::inch_t{dv};
+        break;
+      case kDisplayMeters:
+      default:
+        *v = units::meter_t{dv};
+        break;
+    }
+    return true;
+  }
+  return false;
+}
+
+static bool InputFloatLength(const char* label, float* v, double step = 0.0,
+                             double step_fast = 0.0,
+                             const char* format = "%.3f",
+                             ImGuiInputTextFlags flags = 0) {
+  units::meter_t uv{*v};
+  if (InputLength(label, &uv, step, step_fast, format, flags)) {
+    *v = uv.to<float>();
+    return true;
+  }
+  return false;
+}
+
+static bool InputAngle(const char* label, units::degree_t* v, double step = 0.0,
+                       double step_fast = 0.0, const char* format = "%.6f",
+                       ImGuiInputTextFlags flags = 0) {
+  double dv = ConvertDisplayAngle(*v);
+  if (ImGui::InputDouble(label, &dv, step, step_fast, format, flags)) {
+    *v = units::degree_t{dv};
+    return true;
+  }
+  return false;
+}
+
+static bool InputPose(frc::Pose2d* pose) {
+  auto x = pose->X();
+  auto y = pose->Y();
+  auto rot = pose->Rotation().Degrees();
+
+  bool changed;
+  changed = InputLength("x", &x);
+  changed = InputLength("y", &y) || changed;
+  changed = InputAngle("rot", &rot) || changed;
+  if (changed) {
+    *pose = frc::Pose2d{x, y, rot};
+  }
+  return changed;
+}
+
+FieldInfo::FieldInfo() {
+  auto& storage = GetStorage();
+  m_pFilename = storage.GetStringRef("image");
+  m_pTop = storage.GetIntRef("top", 0);
+  m_pLeft = storage.GetIntRef("left", 0);
+  m_pBottom = storage.GetIntRef("bottom", -1);
+  m_pRight = storage.GetIntRef("right", -1);
+  m_pWidth = storage.GetFloatRef("width", kDefaultWidth.to<float>());
+  m_pHeight = storage.GetFloatRef("height", kDefaultHeight.to<float>());
+}
+
+void FieldInfo::DisplaySettings() {
+  if (ImGui::Button("Choose image...")) {
+    m_fileOpener = std::make_unique<pfd::open_file>(
+        "Choose field image", "",
+        std::vector<std::string>{"Image File",
+                                 "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                                 "*.hdr *.pic *.ppm *.pgm",
+                                 "PathWeaver JSON File", "*.json"});
+  }
+  if (ImGui::Button("Reset image")) {
+    Reset();
+  }
+  InputFloatLength("Field Width", m_pWidth);
+  InputFloatLength("Field Height", m_pHeight);
+  // ImGui::InputInt("Field Top", m_pTop);
+  // ImGui::InputInt("Field Left", m_pLeft);
+  // ImGui::InputInt("Field Right", m_pRight);
+  // ImGui::InputInt("Field Bottom", m_pBottom);
+}
+
+void FieldInfo::Reset() {
+  m_texture = gui::Texture{};
+  m_pFilename->clear();
+  m_imageWidth = 0;
+  m_imageHeight = 0;
+  *m_pTop = 0;
+  *m_pLeft = 0;
+  *m_pBottom = -1;
+  *m_pRight = -1;
+}
+
+void FieldInfo::LoadImage() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) {
+      if (wpi::ends_with(result[0], ".json")) {
+        LoadJson(result[0]);
+      } else {
+        LoadImageImpl(result[0].c_str());
+        *m_pTop = 0;
+        *m_pLeft = 0;
+        *m_pBottom = -1;
+        *m_pRight = -1;
+      }
+    }
+    m_fileOpener.reset();
+  }
+  if (!m_texture && !m_pFilename->empty()) {
+    if (!LoadImageImpl(m_pFilename->c_str())) {
+      m_pFilename->clear();
+    }
+  }
+}
+
+void FieldInfo::LoadJson(std::string_view jsonfile) {
+  std::error_code ec;
+  wpi::raw_fd_istream f(jsonfile, ec);
+  if (ec) {
+    std::fputs("GUI: could not open field JSON file\n", stderr);
+    return;
+  }
+
+  // parse file
+  wpi::json j;
+  try {
+    j = wpi::json::parse(f);
+  } catch (const wpi::json::parse_error& e) {
+    fmt::print(stderr, "GUI: JSON: could not parse: {}\n", e.what());
+  }
+
+  // top level must be an object
+  if (!j.is_object()) {
+    std::fputs("GUI: JSON: does not contain a top object\n", stderr);
+    return;
+  }
+
+  // image filename
+  std::string image;
+  try {
+    image = j.at("field-image").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    fmt::print(stderr, "GUI: JSON: could not read field-image: {}\n", e.what());
+    return;
+  }
+
+  // corners
+  int top, left, bottom, right;
+  try {
+    top = j.at("field-corners").at("top-left").at(1).get<int>();
+    left = j.at("field-corners").at("top-left").at(0).get<int>();
+    bottom = j.at("field-corners").at("bottom-right").at(1).get<int>();
+    right = j.at("field-corners").at("bottom-right").at(0).get<int>();
+  } catch (const wpi::json::exception& e) {
+    fmt::print(stderr, "GUI: JSON: could not read field-corners: {}\n",
+               e.what());
+    return;
+  }
+
+  // size
+  float width;
+  float height;
+  try {
+    width = j.at("field-size").at(0).get<float>();
+    height = j.at("field-size").at(1).get<float>();
+  } catch (const wpi::json::exception& e) {
+    fmt::print(stderr, "GUI: JSON: could not read field-size: {}\n", e.what());
+    return;
+  }
+
+  // units for size
+  std::string unit;
+  try {
+    unit = j.at("field-unit").get<std::string>();
+  } catch (const wpi::json::exception& e) {
+    fmt::print(stderr, "GUI: JSON: could not read field-unit: {}\n", e.what());
+    return;
+  }
+
+  // convert size units to meters
+  if (unit == "foot" || unit == "feet") {
+    width = units::convert<units::feet, units::meters>(width);
+    height = units::convert<units::feet, units::meters>(height);
+  }
+
+  // the image filename is relative to the json file
+  auto pathname = fs::path{jsonfile}.replace_filename(image).string();
+
+  // load field image
+  if (!LoadImageImpl(pathname.c_str())) {
+    return;
+  }
+
+  // save to field info
+  *m_pFilename = pathname;
+  *m_pTop = top;
+  *m_pLeft = left;
+  *m_pBottom = bottom;
+  *m_pRight = right;
+  *m_pWidth = width;
+  *m_pHeight = height;
+}
+
+bool FieldInfo::LoadImageImpl(const char* fn) {
+  fmt::print("GUI: loading field image '{}'\n", fn);
+  auto texture = gui::Texture::CreateFromFile(fn);
+  if (!texture) {
+    std::puts("GUI: could not read field image");
+    return false;
+  }
+  m_texture = std::move(texture);
+  m_imageWidth = m_texture.GetWidth();
+  m_imageHeight = m_texture.GetHeight();
+  *m_pFilename = fn;
+  return true;
+}
+
+FieldFrameData FieldInfo::GetFrameData(ImVec2 min, ImVec2 max) const {
+  // fit the image into the window
+  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
+    gui::MaxFit(&min, &max, m_imageWidth, m_imageHeight);
+  }
+
+  FieldFrameData ffd;
+  ffd.imageMin = min;
+  ffd.imageMax = max;
+
+  // size down the box by the image corners (if any)
+  if (*m_pBottom > 0 && *m_pRight > 0) {
+    min.x += *m_pLeft * (max.x - min.x) / m_imageWidth;
+    min.y += *m_pTop * (max.y - min.y) / m_imageHeight;
+    max.x -= (m_imageWidth - *m_pRight) * (max.x - min.x) / m_imageWidth;
+    max.y -= (m_imageHeight - *m_pBottom) * (max.y - min.y) / m_imageHeight;
+  }
+
+  // draw the field "active area" as a yellow boundary box
+  gui::MaxFit(&min, &max, *m_pWidth, *m_pHeight);
+
+  ffd.min = min;
+  ffd.max = max;
+  ffd.scale = (max.x - min.x) / *m_pWidth;
+  return ffd;
+}
+
+void FieldInfo::Draw(ImDrawList* drawList, const FieldFrameData& ffd) const {
+  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
+    drawList->AddImage(m_texture, ffd.imageMin, ffd.imageMax);
+  }
+
+  // draw the field "active area" as a yellow boundary box
+  drawList->AddRect(ffd.min, ffd.max, IM_COL32(255, 255, 0, 255));
+}
+
+ObjectInfo::ObjectInfo() {
+  auto& storage = GetStorage();
+  m_pFilename = storage.GetStringRef("image");
+  m_pWidth =
+      storage.GetFloatRef("width", DisplayOptions::kDefaultWidth.to<float>());
+  m_pLength =
+      storage.GetFloatRef("length", DisplayOptions::kDefaultLength.to<float>());
+  m_pStyle = storage.GetIntRef("style", DisplayOptions::kDefaultStyle);
+  m_pWeight = storage.GetFloatRef("weight", DisplayOptions::kDefaultWeight);
+  m_pColor = storage.GetIntRef("color", DisplayOptions::kDefaultColor);
+  m_pArrows = storage.GetBoolRef("arrows", DisplayOptions::kDefaultArrows);
+  m_pArrowSize =
+      storage.GetIntRef("arrowSize", DisplayOptions::kDefaultArrowSize);
+  m_pArrowWeight =
+      storage.GetFloatRef("arrowWeight", DisplayOptions::kDefaultArrowWeight);
+  m_pArrowColor =
+      storage.GetIntRef("arrowColor", DisplayOptions::kDefaultArrowColor);
+  m_pSelectable =
+      storage.GetBoolRef("selectable", DisplayOptions::kDefaultSelectable);
+}
+
+DisplayOptions ObjectInfo::GetDisplayOptions() const {
+  DisplayOptions rv{m_texture};
+  rv.style = static_cast<DisplayOptions::Style>(*m_pStyle);
+  rv.weight = *m_pWeight;
+  rv.color = *m_pColor;
+  rv.width = units::meter_t{*m_pWidth};
+  rv.length = units::meter_t{*m_pLength};
+  rv.arrows = *m_pArrows;
+  rv.arrowSize = *m_pArrowSize;
+  rv.arrowWeight = *m_pArrowWeight;
+  rv.arrowColor = *m_pArrowColor;
+  rv.selectable = *m_pSelectable;
+  return rv;
+}
+
+void ObjectInfo::DisplaySettings() {
+  static const char* styleChoices[] = {"Box/Image", "Line", "Line (Closed)",
+                                       "Track"};
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+  ImGui::Combo("Style", m_pStyle, styleChoices, IM_ARRAYSIZE(styleChoices));
+  switch (*m_pStyle) {
+    case DisplayOptions::kBoxImage:
+      if (ImGui::Button("Choose image...")) {
+        m_fileOpener = std::make_unique<pfd::open_file>(
+            "Choose object image", "",
+            std::vector<std::string>{
+                "Image File",
+                "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                "*.hdr *.pic *.ppm *.pgm"});
+      }
+      if (ImGui::Button("Reset image")) {
+        Reset();
+      }
+      InputFloatLength("Width", m_pWidth);
+      InputFloatLength("Length", m_pLength);
+      break;
+    case DisplayOptions::kTrack:
+      InputFloatLength("Width", m_pWidth);
+      break;
+    default:
+      break;
+  }
+
+  ImGui::InputFloat("Line Weight", m_pWeight);
+  ImColor col(*m_pColor);
+  if (ImGui::ColorEdit3("Line Color", &col.Value.x,
+                        ImGuiColorEditFlags_NoInputs)) {
+    *m_pColor = col;
+  }
+  ImGui::Checkbox("Arrows", m_pArrows);
+  if (*m_pArrows) {
+    ImGui::SliderInt("Arrow Size", m_pArrowSize, 0, 100, "%d%%",
+                     ImGuiSliderFlags_AlwaysClamp);
+    ImGui::InputFloat("Arrow Weight", m_pArrowWeight);
+    ImColor col(*m_pArrowColor);
+    if (ImGui::ColorEdit3("Arrow Color", &col.Value.x,
+                          ImGuiColorEditFlags_NoInputs)) {
+      *m_pArrowColor = col;
+    }
+  }
+
+  ImGui::Checkbox("Selectable", m_pSelectable);
+}
+
+void ObjectInfo::DrawLine(ImDrawList* drawList,
+                          wpi::span<const ImVec2> points) const {
+  if (points.empty()) {
+    return;
+  }
+
+  if (points.size() == 1) {
+    drawList->AddCircleFilled(points.front(), *m_pWeight, *m_pWeight);
+    return;
+  }
+
+  // PolyLine doesn't handle acute angles well; workaround from
+  // https://github.com/ocornut/imgui/issues/3366
+  size_t i = 0;
+  while (i + 1 < points.size()) {
+    int nlin = 2;
+    while (i + nlin < points.size()) {
+      auto [x0, y0] = points[i + nlin - 2];
+      auto [x1, y1] = points[i + nlin - 1];
+      auto [x2, y2] = points[i + nlin];
+      auto s0x = x1 - x0, s0y = y1 - y0;
+      auto s1x = x2 - x1, s1y = y2 - y1;
+      auto dotprod = s1x * s0x + s1y * s0y;
+      if (dotprod < 0) {
+        break;
+      }
+      ++nlin;
+    }
+
+    drawList->AddPolyline(&points[i], nlin, *m_pColor, false, *m_pWeight);
+    i += nlin - 1;
+  }
+
+  if (points.size() > 2 && *m_pStyle == DisplayOptions::kLineClosed) {
+    drawList->AddLine(points.back(), points.front(), *m_pColor, *m_pWeight);
+  }
+}
+
+void ObjectInfo::Reset() {
+  m_texture = gui::Texture{};
+  m_pFilename->clear();
+}
+
+void ObjectInfo::LoadImage() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) {
+      LoadImageImpl(result[0].c_str());
+    }
+    m_fileOpener.reset();
+  }
+  if (!m_texture && !m_pFilename->empty()) {
+    if (!LoadImageImpl(m_pFilename->c_str())) {
+      m_pFilename->clear();
+    }
+  }
+}
+
+bool ObjectInfo::LoadImageImpl(const char* fn) {
+  fmt::print("GUI: loading object image '{}'\n", fn);
+  auto texture = gui::Texture::CreateFromFile(fn);
+  if (!texture) {
+    std::fputs("GUI: could not read object image\n", stderr);
+    return false;
+  }
+  m_texture = std::move(texture);
+  *m_pFilename = fn;
+  return true;
+}
+
+PoseFrameData::PoseFrameData(const frc::Pose2d& pose, FieldObjectModel& model,
+                             size_t index, const FieldFrameData& ffd,
+                             const DisplayOptions& displayOptions)
+    : m_model{model},
+      m_index{index},
+      m_ffd{ffd},
+      m_displayOptions{displayOptions},
+      m_width2(ffd.scale * displayOptions.width / 2),
+      m_length2(ffd.scale * displayOptions.length / 2),
+      m_hitRadius((std::min)(m_width2, m_length2) / 2),
+      m_pose{pose} {
+  UpdateFrameData();
+}
+
+void PoseFrameData::SetPosition(const frc::Translation2d& pos) {
+  m_pose = frc::Pose2d{pos, m_pose.Rotation()};
+  m_model.SetPose(m_index, m_pose);
+}
+
+void PoseFrameData::SetRotation(units::radian_t rot) {
+  m_pose = frc::Pose2d{m_pose.Translation(), rot};
+  m_model.SetPose(m_index, m_pose);
+}
+
+void PoseFrameData::UpdateFrameData() {
+  // (0,0) origin is bottom left
+  ImVec2 center = m_ffd.GetScreenFromPos(m_pose.Translation());
+
+  // build rotated points around center
+  float length2 = m_length2;
+  float width2 = m_width2;
+  auto& rot = GetRotation();
+  float cos_a = rot.Cos();
+  float sin_a = -rot.Sin();
+
+  m_corners[0] = center + ImRotate(ImVec2(-length2, -width2), cos_a, sin_a);
+  m_corners[1] = center + ImRotate(ImVec2(length2, -width2), cos_a, sin_a);
+  m_corners[2] = center + ImRotate(ImVec2(length2, width2), cos_a, sin_a);
+  m_corners[3] = center + ImRotate(ImVec2(-length2, width2), cos_a, sin_a);
+  m_corners[4] = center + ImRotate(ImVec2(0, -width2), cos_a, sin_a);
+  m_corners[5] = center + ImRotate(ImVec2(0, width2), cos_a, sin_a);
+
+  float arrowScale = m_displayOptions.arrowSize / 100.0f;
+  m_arrow[0] =
+      center + ImRotate(ImVec2(-length2 * arrowScale, -width2 * arrowScale),
+                        cos_a, sin_a);
+  m_arrow[1] = center + ImRotate(ImVec2(length2 * arrowScale, 0), cos_a, sin_a);
+  m_arrow[2] =
+      center + ImRotate(ImVec2(-length2 * arrowScale, width2 * arrowScale),
+                        cos_a, sin_a);
+
+  m_center = center;
+}
+
+std::pair<int, float> PoseFrameData::IsHovered(const ImVec2& cursor) const {
+  float hitRadiusSquared = m_hitRadius * m_hitRadius;
+  float dist;
+
+  // it's within the hit radius of the center?
+  dist = gui::GetDistSquared(cursor, m_center);
+  if (dist < hitRadiusSquared) {
+    return {1, dist};
+  }
+
+  if (m_displayOptions.style == DisplayOptions::kBoxImage) {
+    dist = gui::GetDistSquared(cursor, m_corners[0]);
+    if (dist < hitRadiusSquared) {
+      return {2, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[1]);
+    if (dist < hitRadiusSquared) {
+      return {3, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[2]);
+    if (dist < hitRadiusSquared) {
+      return {4, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[3]);
+    if (dist < hitRadiusSquared) {
+      return {5, dist};
+    }
+  } else if (m_displayOptions.style == DisplayOptions::kTrack) {
+    dist = gui::GetDistSquared(cursor, m_corners[4]);
+    if (dist < hitRadiusSquared) {
+      return {6, dist};
+    }
+
+    dist = gui::GetDistSquared(cursor, m_corners[5]);
+    if (dist < hitRadiusSquared) {
+      return {7, dist};
+    }
+  }
+
+  return {0, 0.0};
+}
+
+SelectedTargetInfo PoseFrameData::GetDragTarget(int corner, float dist) const {
+  SelectedTargetInfo info;
+  info.objModel = &m_model;
+  info.rot = GetRotation().Radians();
+  info.poseCenter = m_center;
+  if (corner == 1) {
+    info.center = m_center;
+  } else {
+    info.center = m_corners[corner - 2];
+  }
+  info.radius = m_hitRadius;
+  info.dist = dist;
+  info.corner = corner;
+  return info;
+}
+
+void PoseFrameData::HandleDrag(const ImVec2& cursor) {
+  if (gDragState.target.corner == 1) {
+    SetPosition(m_ffd.GetPosFromScreen(cursor - gDragState.initialOffset));
+    UpdateFrameData();
+    gDragState.target.center = m_center;
+    gDragState.target.poseCenter = m_center;
+  } else {
+    ImVec2 off = cursor - m_center;
+    SetRotation(gDragState.initialAngle -
+                units::radian_t{std::atan2(off.y, off.x)});
+    gDragState.target.center = m_corners[gDragState.target.corner - 2];
+    gDragState.target.rot = GetRotation().Radians();
+  }
+}
+
+void PoseFrameData::Draw(ImDrawList* drawList, std::vector<ImVec2>* center,
+                         std::vector<ImVec2>* left,
+                         std::vector<ImVec2>* right) const {
+  switch (m_displayOptions.style) {
+    case DisplayOptions::kBoxImage:
+      if (m_displayOptions.texture) {
+        drawList->AddImageQuad(m_displayOptions.texture, m_corners[0],
+                               m_corners[1], m_corners[2], m_corners[3]);
+        return;
+      }
+      drawList->AddQuad(m_corners[0], m_corners[1], m_corners[2], m_corners[3],
+                        m_displayOptions.color, m_displayOptions.weight);
+      break;
+    case DisplayOptions::kLine:
+    case DisplayOptions::kLineClosed:
+      center->emplace_back(m_center);
+      break;
+    case DisplayOptions::kTrack:
+      center->emplace_back(m_center);
+      left->emplace_back(m_corners[4]);
+      right->emplace_back(m_corners[5]);
+      break;
+  }
+
+  if (m_displayOptions.arrows) {
+    drawList->AddTriangle(m_arrow[0], m_arrow[1], m_arrow[2],
+                          m_displayOptions.arrowColor,
+                          m_displayOptions.arrowWeight);
+  }
+}
+
+void glass::DisplayField2DSettings(Field2DModel* model) {
+  auto& storage = GetStorage();
+  auto field = storage.GetData<FieldInfo>();
+  if (!field) {
+    storage.SetData(std::make_shared<FieldInfo>());
+    field = storage.GetData<FieldInfo>();
+  }
+
+  static const char* unitNames[] = {"meters", "feet", "inches"};
+  int* pDisplayUnits = GetStorage().GetIntRef("units", kDisplayMeters);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+  ImGui::Combo("Units", pDisplayUnits, unitNames, IM_ARRAYSIZE(unitNames));
+  gDisplayUnits = static_cast<DisplayUnits>(*pDisplayUnits);
+
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
+  if (ImGui::CollapsingHeader("Field")) {
+    ImGui::PushID("Field");
+    field->DisplaySettings();
+    ImGui::PopID();
+  }
+
+  model->ForEachFieldObject([&](auto& objModel, auto name) {
+    if (!objModel.Exists()) {
+      return;
+    }
+    PushID(name);
+    auto& objRef = field->m_objects[name];
+    if (!objRef) {
+      objRef = std::make_unique<ObjectInfo>();
+    }
+    auto obj = objRef.get();
+
+    wpi::SmallString<64> nameBuf{name};
+    if (ImGui::CollapsingHeader(nameBuf.c_str())) {
+      obj->DisplaySettings();
+    }
+    PopID();
+  });
+  ImGui::PopItemWidth();
+}
+
+namespace {
+class FieldDisplay {
+ public:
+  void Display(FieldInfo* field, Field2DModel* model,
+               const ImVec2& contentSize);
+
+ private:
+  void DisplayObject(FieldObjectModel& model, std::string_view name);
+
+  FieldInfo* m_field;
+  ImVec2 m_mousePos;
+  ImDrawList* m_drawList;
+
+  // only allow initiation of dragging when invisible button is hovered;
+  // this prevents the window resize handles from simultaneously activating
+  // the drag functionality
+  bool m_isHovered;
+
+  FieldFrameData m_ffd;
+
+  // drag targets
+  std::vector<SelectedTargetInfo> m_targets;
+
+  // splitter so lines are put behind arrows
+  ImDrawListSplitter m_drawSplit;
+
+  // lines; static so buffer gets reused
+  std::vector<ImVec2> m_centerLine, m_leftLine, m_rightLine;
+};
+}  // namespace
+
+void FieldDisplay::Display(FieldInfo* field, Field2DModel* model,
+                           const ImVec2& contentSize) {
+  // screen coords
+  ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos();
+
+  // for dragging to work, there needs to be a button (otherwise the window is
+  // dragged)
+  ImGui::InvisibleButton("field", contentSize);
+
+  m_field = field;
+  m_mousePos = ImGui::GetIO().MousePos;
+  m_drawList = ImGui::GetWindowDrawList();
+  m_isHovered = ImGui::IsItemHovered();
+
+  // field
+  field->LoadImage();
+  m_ffd = field->GetFrameData(cursorPos, cursorPos + contentSize);
+  field->Draw(m_drawList, m_ffd);
+
+  // stop dragging if mouse button not down
+  bool isDown = ImGui::IsMouseDown(0);
+  if (!isDown) {
+    gDragState.target.objModel = nullptr;
+  }
+
+  // clear popup target if popup closed
+  bool isPopupOpen = ImGui::IsPopupOpen("edit");
+  if (!isPopupOpen) {
+    gPopupState.Close();
+  }
+
+  // field objects
+  m_targets.resize(0);
+  model->ForEachFieldObject([this](auto& objModel, auto name) {
+    if (objModel.Exists()) {
+      DisplayObject(objModel, name);
+    }
+  });
+
+  SelectedTargetInfo* target = nullptr;
+
+  if (gDragState.target.objModel) {
+    target = &gDragState.target;
+  } else if (gPopupState.GetTarget()->objModel) {
+    target = gPopupState.GetTarget();
+  } else if (!m_targets.empty()) {
+    // Find the "best" drag target of the available options.  Prefer
+    // center to non-center, and then pick the closest hit.
+    std::sort(m_targets.begin(), m_targets.end(),
+              [](const auto& a, const auto& b) {
+                return a.corner == 0 || a.dist < b.dist;
+              });
+    target = &m_targets.front();
+  }
+
+  if (target) {
+    // draw the target circle; also draw a smaller circle on the pose center
+    m_drawList->AddCircle(target->center, target->radius,
+                          IM_COL32(0, 255, 0, 255));
+    if (target->corner != 1) {
+      m_drawList->AddCircle(target->poseCenter, target->radius / 2.0,
+                            IM_COL32(0, 255, 0, 255));
+    }
+  }
+
+  // right-click popup for editing
+  if (m_isHovered && ImGui::IsMouseClicked(ImGuiMouseButton_Right)) {
+    gPopupState.Open(target, m_ffd.GetPosFromScreen(m_mousePos));
+    ImGui::OpenPopup("edit");
+  }
+  if (ImGui::BeginPopup("edit")) {
+    gPopupState.Display(model, m_ffd);
+    ImGui::EndPopup();
+  } else if (target) {
+    if (m_isHovered && ImGui::IsMouseClicked(0)) {
+      // initialize drag state
+      gDragState.target = *target;
+      gDragState.initialOffset = m_mousePos - target->poseCenter;
+      if (target->corner != 1) {
+        gDragState.initialAngle =
+            units::radian_t{std::atan2(gDragState.initialOffset.y,
+                                       gDragState.initialOffset.x)} +
+            target->rot;
+      }
+    }
+
+    // show tooltip and highlight
+    auto pos = m_ffd.GetPosFromScreen(target->poseCenter);
+    ImGui::SetTooltip(
+        "%s[%d]\nx: %0.3f y: %0.3f rot: %0.3f", target->name.c_str(),
+        static_cast<int>(target->index), ConvertDisplayLength(pos.X()),
+        ConvertDisplayLength(pos.Y()), ConvertDisplayAngle(target->rot));
+  }
+}
+
+void FieldDisplay::DisplayObject(FieldObjectModel& model,
+                                 std::string_view name) {
+  PushID(name);
+  auto& objRef = m_field->m_objects[name];
+  if (!objRef) {
+    objRef = std::make_unique<ObjectInfo>();
+  }
+  auto obj = objRef.get();
+  obj->LoadImage();
+
+  auto displayOptions = obj->GetDisplayOptions();
+
+  m_centerLine.resize(0);
+  m_leftLine.resize(0);
+  m_rightLine.resize(0);
+
+  m_drawSplit.Split(m_drawList, 2);
+  m_drawSplit.SetCurrentChannel(m_drawList, 1);
+  auto poses = gPopupState.GetInsertModel() == &model
+                   ? gPopupState.GetInsertPoses()
+                   : model.GetPoses();
+  size_t i = 0;
+  for (auto&& pose : poses) {
+    PoseFrameData pfd{pose, model, i, m_ffd, displayOptions};
+
+    // check for potential drag targets
+    if (displayOptions.selectable && m_isHovered &&
+        !gDragState.target.objModel) {
+      auto [corner, dist] = pfd.IsHovered(m_mousePos);
+      if (corner > 0) {
+        m_targets.emplace_back(pfd.GetDragTarget(corner, dist));
+        m_targets.back().name = name;
+        m_targets.back().index = i;
+      }
+    }
+
+    // handle active dragging of this object
+    if (gDragState.target.objModel == &model && gDragState.target.index == i) {
+      pfd.HandleDrag(m_mousePos);
+    }
+
+    // draw
+    pfd.Draw(m_drawList, &m_centerLine, &m_leftLine, &m_rightLine);
+    ++i;
+  }
+
+  m_drawSplit.SetCurrentChannel(m_drawList, 0);
+  obj->DrawLine(m_drawList, m_centerLine);
+  obj->DrawLine(m_drawList, m_leftLine);
+  obj->DrawLine(m_drawList, m_rightLine);
+  m_drawSplit.Merge(m_drawList);
+
+  PopID();
+}
+
+void PopupState::Open(SelectedTargetInfo* target,
+                      const frc::Translation2d& pos) {
+  if (target) {
+    m_target = *target;
+  } else {
+    m_target.objModel = nullptr;
+    m_insertModel = nullptr;
+    m_insertPoses.resize(0);
+    m_insertPoses.emplace_back(pos, 0_deg);
+    m_insertName.clear();
+    m_insertIndex = 0;
+  }
+}
+
+void PopupState::Close() {
+  m_target.objModel = nullptr;
+  m_insertModel = nullptr;
+  m_insertPoses.resize(0);
+}
+
+void PopupState::Display(Field2DModel* model, const FieldFrameData& ffd) {
+  if (m_target.objModel) {
+    DisplayTarget(model, ffd);
+  } else {
+    DisplayInsert(model);
+  }
+}
+
+void PopupState::DisplayTarget(Field2DModel* model, const FieldFrameData& ffd) {
+  ImGui::Text("%s[%d]", m_target.name.c_str(),
+              static_cast<int>(m_target.index));
+  frc::Pose2d pose{ffd.GetPosFromScreen(m_target.poseCenter), m_target.rot};
+  if (InputPose(&pose)) {
+    m_target.poseCenter = ffd.GetScreenFromPos(pose.Translation());
+    m_target.rot = pose.Rotation().Radians();
+    m_target.objModel->SetPose(m_target.index, pose);
+  }
+  if (ImGui::Button("Delete Pose")) {
+    auto posesRef = m_target.objModel->GetPoses();
+    std::vector<frc::Pose2d> poses{posesRef.begin(), posesRef.end()};
+    if (m_target.index < poses.size()) {
+      poses.erase(poses.begin() + m_target.index);
+      m_target.objModel->SetPoses(poses);
+    }
+    ImGui::CloseCurrentPopup();
+  }
+  if (ImGui::Button("Delete Object (ALL Poses)")) {
+    model->RemoveFieldObject(m_target.name);
+    ImGui::CloseCurrentPopup();
+  }
+}
+
+void PopupState::DisplayInsert(Field2DModel* model) {
+  ImGui::TextUnformatted("Insert New Pose");
+
+  InputPose(&m_insertPoses[m_insertIndex]);
+
+  const char* insertName = m_insertModel ? m_insertName.c_str() : "<new>";
+  if (ImGui::BeginCombo("Object", insertName)) {
+    bool selected = !m_insertModel;
+    if (ImGui::Selectable("<new>", selected)) {
+      m_insertModel = nullptr;
+      auto pose = m_insertPoses[m_insertIndex];
+      m_insertPoses.resize(0);
+      m_insertPoses.emplace_back(std::move(pose));
+      m_insertName.clear();
+      m_insertIndex = 0;
+    }
+    if (selected) {
+      ImGui::SetItemDefaultFocus();
+    }
+    model->ForEachFieldObject([&](auto& objModel, auto name) {
+      bool selected = m_insertModel == &objModel;
+      if (ImGui::Selectable(name.data(), selected)) {
+        m_insertModel = &objModel;
+        auto pose = m_insertPoses[m_insertIndex];
+        auto posesRef = objModel.GetPoses();
+        m_insertPoses.assign(posesRef.begin(), posesRef.end());
+        m_insertPoses.emplace_back(std::move(pose));
+        m_insertName = name;
+        m_insertIndex = m_insertPoses.size() - 1;
+      }
+      if (selected) {
+        ImGui::SetItemDefaultFocus();
+      }
+    });
+    ImGui::EndCombo();
+  }
+  if (m_insertModel) {
+    int oldIndex = m_insertIndex;
+    if (ImGui::InputInt("Pos", &m_insertIndex, 1, 5)) {
+      if (m_insertIndex < 0) {
+        m_insertIndex = 0;
+      }
+      size_t size = m_insertPoses.size();
+      if (static_cast<size_t>(m_insertIndex) >= size) {
+        m_insertIndex = size - 1;
+      }
+      if (m_insertIndex < oldIndex) {
+        auto begin = m_insertPoses.begin();
+        std::rotate(begin + m_insertIndex, begin + oldIndex,
+                    begin + oldIndex + 1);
+      } else if (m_insertIndex > oldIndex) {
+        auto rbegin = m_insertPoses.rbegin();
+        std::rotate(rbegin + (size - m_insertIndex), rbegin + (size - oldIndex),
+                    rbegin + (size - oldIndex - 1));
+      }
+    }
+  } else {
+    ImGui::InputText("Name", &m_insertName);
+  }
+
+  if (ImGui::Button("Apply")) {
+    if (m_insertModel) {
+      m_insertModel->SetPoses(m_insertPoses);
+    } else if (!m_insertName.empty()) {
+      model->AddFieldObject(m_insertName)->SetPoses(m_insertPoses);
+    }
+    ImGui::CloseCurrentPopup();
+  }
+  ImGui::SameLine();
+  if (ImGui::Button("Cancel")) {
+    ImGui::CloseCurrentPopup();
+  }
+}
+
+void glass::DisplayField2D(Field2DModel* model, const ImVec2& contentSize) {
+  auto& storage = GetStorage();
+  auto field = storage.GetData<FieldInfo>();
+  if (!field) {
+    storage.SetData(std::make_shared<FieldInfo>());
+    field = storage.GetData<FieldInfo>();
+  }
+
+  if (contentSize.x <= 0 || contentSize.y <= 0) {
+    return;
+  }
+
+  static FieldDisplay display;
+  display.Display(field, model, contentSize);
+}
+
+void Field2DView::Display() {
+  if (ImGui::BeginPopupContextItem()) {
+    DisplayField2DSettings(m_model);
+    ImGui::EndPopup();
+  }
+  DisplayField2D(m_model, ImGui::GetWindowContentRegionMax() -
+                              ImGui::GetWindowContentRegionMin());
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/Log.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/Log.cpp
new file mode 100644
index 0000000..9a1d2c5
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/Log.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/Log.h"
+
+#include <imgui.h>
+
+using namespace glass;
+
+LogData::LogData(size_t maxLines) : m_maxLines{maxLines} {}
+
+void LogData::Clear() {
+  m_buf.clear();
+  m_lineOffsets.clear();
+  m_lineOffsets.push_back(0);
+}
+
+void LogData::Append(std::string_view msg) {
+  if (m_lineOffsets.size() >= m_maxLines) {
+    Clear();
+  }
+
+  size_t oldSize = m_buf.size();
+  m_buf.append(msg);
+  for (size_t newSize = m_buf.size(); oldSize < newSize; ++oldSize) {
+    if (m_buf[oldSize] == '\n') {
+      m_lineOffsets.push_back(oldSize + 1);
+    }
+  }
+}
+
+const std::string& LogData::GetBuffer() {
+  return m_buf;
+}
+
+void glass::DisplayLog(LogData* data, bool autoScroll) {
+  if (data->m_buf.empty()) {
+    return;
+  }
+  ImGui::PushStyleVar(ImGuiStyleVar_ItemSpacing, ImVec2(0, 0));
+  const char* buf = data->m_buf.data();
+  const char* bufEnd = buf + data->m_buf.size();
+  ImGuiListClipper clipper;
+  clipper.Begin(data->m_lineOffsets.size());
+  while (clipper.Step()) {
+    for (size_t lineNum = clipper.DisplayStart;
+         lineNum < static_cast<size_t>(clipper.DisplayEnd); lineNum++) {
+      const char* lineStart = buf + data->m_lineOffsets[lineNum];
+      const char* lineEnd = (lineNum + 1 < data->m_lineOffsets.size())
+                                ? (buf + data->m_lineOffsets[lineNum + 1] - 1)
+                                : bufEnd;
+      ImGui::TextUnformatted(lineStart, lineEnd);
+    }
+  }
+  clipper.End();
+  ImGui::PopStyleVar();
+
+  if (autoScroll && ImGui::GetScrollY() >= ImGui::GetScrollMaxY()) {
+    ImGui::SetScrollHereY(1.0f);
+  }
+}
+
+void LogView::Display() {
+  if (ImGui::BeginPopupContextItem()) {
+    ImGui::Checkbox("Auto-scroll", &m_autoScroll);
+    if (ImGui::Selectable("Clear")) {
+      m_data->Clear();
+    }
+    const auto& buf = m_data->GetBuffer();
+    if (ImGui::Selectable("Copy to Clipboard", false,
+                          buf.empty() ? ImGuiSelectableFlags_Disabled : 0)) {
+      ImGui::SetClipboardText(buf.c_str());
+    }
+    ImGui::EndPopup();
+  }
+
+  DisplayLog(m_data, m_autoScroll);
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/Mechanism2D.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/Mechanism2D.cpp
new file mode 100644
index 0000000..07e83e2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/Mechanism2D.cpp
@@ -0,0 +1,259 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/Mechanism2D.h"
+
+#include <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <memory>
+#include <string_view>
+#include <utility>
+
+#include <fmt/format.h>
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Transform2d.h>
+#include <frc/geometry/Translation2d.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <portable-file-dialogs.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpigui.h>
+
+#include "glass/Context.h"
+
+using namespace glass;
+
+namespace gui = wpi::gui;
+
+namespace {
+
+// Per-frame data (not persistent)
+struct FrameData {
+  frc::Translation2d GetPosFromScreen(const ImVec2& cursor) const {
+    return {
+        units::meter_t{(std::clamp(cursor.x, min.x, max.x) - min.x) / scale},
+        units::meter_t{(max.y - std::clamp(cursor.y, min.y, max.y)) / scale}};
+  }
+  ImVec2 GetScreenFromPos(const frc::Translation2d& pos) const {
+    return {min.x + scale * pos.X().to<float>(),
+            max.y - scale * pos.Y().to<float>()};
+  }
+  void DrawObject(ImDrawList* drawList, MechanismObjectModel& objModel,
+                  const frc::Pose2d& pose) const;
+  void DrawGroup(ImDrawList* drawList, MechanismObjectGroup& group,
+                 const frc::Pose2d& pose) const;
+
+  // in screen coordinates
+  ImVec2 imageMin;
+  ImVec2 imageMax;
+  ImVec2 min;
+  ImVec2 max;
+
+  float scale;  // scaling from meters to screen units
+};
+
+class BackgroundInfo {
+ public:
+  BackgroundInfo();
+
+  void DisplaySettings();
+
+  void LoadImage();
+  FrameData GetFrameData(ImVec2 min, ImVec2 max, frc::Translation2d dims) const;
+  void Draw(ImDrawList* drawList, const FrameData& frameData,
+            ImU32 bgColor) const;
+
+ private:
+  void Reset();
+  bool LoadImageImpl(const char* fn);
+
+  std::unique_ptr<pfd::open_file> m_fileOpener;
+
+  std::string* m_pFilename;
+  gui::Texture m_texture;
+
+  // in image pixels
+  int m_imageWidth;
+  int m_imageHeight;
+};
+
+}  // namespace
+
+BackgroundInfo::BackgroundInfo() {
+  auto& storage = GetStorage();
+  m_pFilename = storage.GetStringRef("image");
+}
+
+void BackgroundInfo::DisplaySettings() {
+  if (ImGui::Button("Choose image...")) {
+    m_fileOpener = std::make_unique<pfd::open_file>(
+        "Choose background image", "",
+        std::vector<std::string>{"Image File",
+                                 "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
+                                 "*.hdr *.pic *.ppm *.pgm"});
+  }
+  if (ImGui::Button("Reset background image")) {
+    Reset();
+  }
+}
+
+void BackgroundInfo::Reset() {
+  m_texture = gui::Texture{};
+  m_pFilename->clear();
+  m_imageWidth = 0;
+  m_imageHeight = 0;
+}
+
+void BackgroundInfo::LoadImage() {
+  if (m_fileOpener && m_fileOpener->ready(0)) {
+    auto result = m_fileOpener->result();
+    if (!result.empty()) {
+      LoadImageImpl(result[0].c_str());
+    }
+    m_fileOpener.reset();
+  }
+  if (!m_texture && !m_pFilename->empty()) {
+    if (!LoadImageImpl(m_pFilename->c_str())) {
+      m_pFilename->clear();
+    }
+  }
+}
+
+bool BackgroundInfo::LoadImageImpl(const char* fn) {
+  fmt::print("GUI: loading background image '{}'\n", fn);
+  auto texture = gui::Texture::CreateFromFile(fn);
+  if (!texture) {
+    std::puts("GUI: could not read background image");
+    return false;
+  }
+  m_texture = std::move(texture);
+  m_imageWidth = m_texture.GetWidth();
+  m_imageHeight = m_texture.GetHeight();
+  *m_pFilename = fn;
+  return true;
+}
+
+FrameData BackgroundInfo::GetFrameData(ImVec2 min, ImVec2 max,
+                                       frc::Translation2d dims) const {
+  // fit the image into the window
+  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
+    gui::MaxFit(&min, &max, m_imageWidth, m_imageHeight);
+  }
+
+  FrameData frameData;
+  frameData.imageMin = min;
+  frameData.imageMax = max;
+
+  // determine the "active area"
+  float width = dims.X().to<float>();
+  float height = dims.Y().to<float>();
+  gui::MaxFit(&min, &max, width, height);
+
+  frameData.min = min;
+  frameData.max = max;
+  frameData.scale = (max.x - min.x) / width;
+  return frameData;
+}
+
+void BackgroundInfo::Draw(ImDrawList* drawList, const FrameData& frameData,
+                          ImU32 bgColor) const {
+  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
+    drawList->AddImage(m_texture, frameData.imageMin, frameData.imageMax);
+  } else {
+    drawList->AddRectFilled(frameData.min, frameData.max, bgColor);
+  }
+}
+
+void glass::DisplayMechanism2DSettings(Mechanism2DModel* model) {
+  auto& storage = GetStorage();
+  auto bg = storage.GetData<BackgroundInfo>();
+  if (!bg) {
+    storage.SetData(std::make_shared<BackgroundInfo>());
+    bg = storage.GetData<BackgroundInfo>();
+  }
+  bg->DisplaySettings();
+}
+
+void FrameData::DrawObject(ImDrawList* drawList, MechanismObjectModel& objModel,
+                           const frc::Pose2d& pose) const {
+  const char* type = objModel.GetType();
+  if (std::string_view{type} == "line") {
+    auto startPose =
+        pose + frc::Transform2d{frc::Translation2d{}, objModel.GetAngle()};
+    auto endPose =
+        startPose +
+        frc::Transform2d{frc::Translation2d{objModel.GetLength(), 0_m}, 0_deg};
+    drawList->AddLine(GetScreenFromPos(startPose.Translation()),
+                      GetScreenFromPos(endPose.Translation()),
+                      objModel.GetColor(), objModel.GetWeight());
+    DrawGroup(drawList, objModel, endPose);
+  }
+}
+
+void FrameData::DrawGroup(ImDrawList* drawList, MechanismObjectGroup& group,
+                          const frc::Pose2d& pose) const {
+  group.ForEachObject(
+      [&](auto& objModel) { DrawObject(drawList, objModel, pose); });
+}
+
+void glass::DisplayMechanism2D(Mechanism2DModel* model,
+                               const ImVec2& contentSize) {
+  auto& storage = GetStorage();
+  auto bg = storage.GetData<BackgroundInfo>();
+  if (!bg) {
+    storage.SetData(std::make_shared<BackgroundInfo>());
+    bg = storage.GetData<BackgroundInfo>();
+  }
+
+  if (contentSize.x <= 0 || contentSize.y <= 0) {
+    return;
+  }
+
+  // screen coords
+  ImVec2 cursorPos = ImGui::GetWindowPos() + ImGui::GetCursorPos();
+
+  ImGui::InvisibleButton("background", contentSize);
+
+  // auto mousePos = ImGui::GetIO().MousePos;
+  auto drawList = ImGui::GetWindowDrawList();
+  // bool isHovered = ImGui::IsItemHovered();
+
+  // background
+  bg->LoadImage();
+  auto frameData = bg->GetFrameData(cursorPos, cursorPos + contentSize,
+                                    model->GetDimensions());
+  bg->Draw(drawList, frameData, model->GetBackgroundColor());
+
+  // elements
+  model->ForEachRoot([&](auto& rootModel) {
+    frameData.DrawGroup(drawList, rootModel,
+                        frc::Pose2d{rootModel.GetPosition(), 0_deg});
+  });
+
+#if 0
+  if (target) {
+    // show tooltip and highlight
+    auto pos = frameData.GetPosFromScreen(target->poseCenter);
+    ImGui::SetTooltip(
+        "%s[%d]\nx: %0.3f y: %0.3f rot: %0.3f", target->name.c_str(),
+        static_cast<int>(target->index), ConvertDisplayLength(pos.X()),
+        ConvertDisplayLength(pos.Y()), ConvertDisplayAngle(target->rot));
+  }
+#endif
+}
+
+void Mechanism2DView::Display() {
+  if (ImGui::BeginPopupContextItem()) {
+    DisplayMechanism2DSettings(m_model);
+    ImGui::EndPopup();
+  }
+  DisplayMechanism2D(m_model, ImGui::GetWindowContentRegionMax() -
+                                  ImGui::GetWindowContentRegionMin());
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp
new file mode 100644
index 0000000..3c83b11
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/PIDController.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/PIDController.h"
+
+#include <string>
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplayPIDController(PIDControllerModel* m) {
+  if (auto name = m->GetName()) {
+    ImGui::Text("%s", name);
+    ImGui::Separator();
+  }
+
+  if (m->Exists()) {
+    auto flag = m->IsReadOnly() ? ImGuiInputTextFlags_ReadOnly
+                                : ImGuiInputTextFlags_None;
+    auto createTuningParameter = [flag](const char* name, double* v,
+                                        std::function<void(double)> callback) {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      if (ImGui::InputDouble(name, v, 0.0, 0.0, "%.3f", flag)) {
+        callback(*v);
+      }
+    };
+
+    if (auto p = m->GetPData()) {
+      double value = p->GetValue();
+      createTuningParameter("P", &value, [=](auto v) { m->SetP(v); });
+    }
+    if (auto i = m->GetIData()) {
+      double value = i->GetValue();
+      createTuningParameter("I", &value, [=](auto v) { m->SetI(v); });
+    }
+    if (auto d = m->GetDData()) {
+      double value = d->GetValue();
+      createTuningParameter("D", &value, [=](auto v) { m->SetD(v); });
+    }
+    if (auto s = m->GetSetpointData()) {
+      double value = s->GetValue();
+      createTuningParameter("Setpoint", &value,
+                            [=](auto v) { m->SetSetpoint(v); });
+    }
+  } else {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown PID Controller");
+    ImGui::PopStyleColor();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/Plot.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/Plot.cpp
new file mode 100644
index 0000000..372f8c9
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/Plot.cpp
@@ -0,0 +1,1075 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/Plot.h"
+
+#include <stdint.h>
+
+#include <algorithm>
+#include <atomic>
+#include <cstdio>
+#include <cstring>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <fmt/format.h>
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui.h>
+#include <imgui_internal.h>
+#include <imgui_stdlib.h>
+#include <implot.h>
+#include <wpigui.h>
+#include <wpi/Signal.h>
+#include <wpi/SmallString.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+#include <wpi/timestamp.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+#include "glass/support/ExtraGuiWidgets.h"
+
+using namespace glass;
+
+namespace {
+class PlotView;
+
+struct PlotSeriesRef {
+  PlotView* view;
+  size_t plotIndex;
+  size_t seriesIndex;
+};
+
+class PlotSeries {
+ public:
+  explicit PlotSeries(std::string_view id);
+  explicit PlotSeries(DataSource* source, int yAxis = 0);
+
+  const std::string& GetId() const { return m_id; }
+
+  void CheckSource();
+
+  void SetSource(DataSource* source);
+  DataSource* GetSource() const { return m_source; }
+
+  bool ReadIni(std::string_view name, std::string_view value);
+  void WriteIni(ImGuiTextBuffer* out);
+
+  enum Action { kNone, kMoveUp, kMoveDown, kDelete };
+  Action EmitPlot(PlotView& view, double now, size_t i, size_t plotIndex);
+  void EmitSettings(size_t i);
+  void EmitDragDropPayload(PlotView& view, size_t i, size_t plotIndex);
+
+  const char* GetName() const;
+
+  int GetYAxis() const { return m_yAxis; }
+  void SetYAxis(int yAxis) { m_yAxis = yAxis; }
+
+ private:
+  bool IsDigital() const {
+    return m_digital == kDigital ||
+           (m_digital == kAuto && m_source && m_source->IsDigital());
+  }
+  void AppendValue(double value, uint64_t time);
+
+  // source linkage
+  DataSource* m_source = nullptr;
+  wpi::sig::ScopedConnection m_sourceCreatedConn;
+  wpi::sig::ScopedConnection m_newValueConn;
+  std::string m_id;
+
+  // user settings
+  std::string m_name;
+  int m_yAxis = 0;
+  ImVec4 m_color = IMPLOT_AUTO_COL;
+  int m_marker = 0;
+  float m_weight = IMPLOT_AUTO;
+
+  enum Digital { kAuto, kDigital, kAnalog };
+  int m_digital = 0;
+  int m_digitalBitHeight = 8;
+  int m_digitalBitGap = 4;
+
+  // value storage
+  static constexpr int kMaxSize = 2000;
+  static constexpr double kTimeGap = 0.05;
+  std::atomic<int> m_size = 0;
+  std::atomic<int> m_offset = 0;
+  ImPlotPoint m_data[kMaxSize];
+};
+
+class Plot {
+ public:
+  Plot();
+
+  bool ReadIni(std::string_view name, std::string_view value);
+  void WriteIni(ImGuiTextBuffer* out);
+
+  void DragDropTarget(PlotView& view, size_t i, bool inPlot);
+  void EmitPlot(PlotView& view, double now, bool paused, size_t i);
+  void EmitSettings(size_t i);
+
+  const std::string& GetName() const { return m_name; }
+
+  std::vector<std::unique_ptr<PlotSeries>> m_series;
+
+  // Returns base height; does not include actual plot height if auto-sized.
+  int GetAutoBaseHeight(bool* isAuto, size_t i);
+
+  void SetAutoHeight(int height) {
+    if (m_autoHeight) {
+      m_height = height;
+    }
+  }
+
+ private:
+  void EmitSettingsLimits(int axis);
+
+  std::string m_name;
+  bool m_visible = true;
+  bool m_showPause = true;
+  unsigned int m_plotFlags = ImPlotFlags_None;
+  bool m_lockPrevX = false;
+  bool m_paused = false;
+  float m_viewTime = 10;
+  bool m_autoHeight = true;
+  int m_height = 300;
+  struct PlotRange {
+    double min = 0;
+    double max = 1;
+    bool lockMin = false;
+    bool lockMax = false;
+    bool apply = false;
+  };
+  std::string m_axisLabel[3];
+  PlotRange m_axisRange[3];
+  ImPlotRange m_xaxisRange;  // read from plot, used for lockPrevX
+};
+
+class PlotView : public View {
+ public:
+  explicit PlotView(PlotProvider* provider) : m_provider{provider} {}
+
+  void Display() override;
+
+  void MovePlot(PlotView* fromView, size_t fromIndex, size_t toIndex);
+
+  void MovePlotSeries(PlotView* fromView, size_t fromPlotIndex,
+                      size_t fromSeriesIndex, size_t toPlotIndex,
+                      size_t toSeriesIndex, int yAxis = -1);
+
+  PlotProvider* m_provider;
+  std::vector<std::unique_ptr<Plot>> m_plots;
+};
+
+}  // namespace
+
+PlotSeries::PlotSeries(std::string_view id) : m_id(id) {
+  if (DataSource* source = DataSource::Find(id)) {
+    SetSource(source);
+    return;
+  }
+  CheckSource();
+}
+
+PlotSeries::PlotSeries(DataSource* source, int yAxis) : m_yAxis(yAxis) {
+  SetSource(source);
+  m_id = source->GetId();
+}
+
+void PlotSeries::CheckSource() {
+  if (!m_newValueConn.connected() && !m_sourceCreatedConn.connected()) {
+    m_source = nullptr;
+    m_sourceCreatedConn = DataSource::sourceCreated.connect_connection(
+        [this](const char* id, DataSource* source) {
+          if (m_id == id) {
+            SetSource(source);
+            m_sourceCreatedConn.disconnect();
+          }
+        });
+  }
+}
+
+void PlotSeries::SetSource(DataSource* source) {
+  m_source = source;
+
+  // add initial value
+  m_data[m_size++] = ImPlotPoint{wpi::Now() * 1.0e-6, source->GetValue()};
+
+  m_newValueConn = source->valueChanged.connect_connection(
+      [this](double value, uint64_t time) { AppendValue(value, time); });
+}
+
+void PlotSeries::AppendValue(double value, uint64_t timeUs) {
+  double time = (timeUs != 0 ? timeUs : wpi::Now()) * 1.0e-6;
+  if (IsDigital()) {
+    if (m_size < kMaxSize) {
+      m_data[m_size] = ImPlotPoint{time, value};
+      ++m_size;
+    } else {
+      m_data[m_offset] = ImPlotPoint{time, value};
+      m_offset = (m_offset + 1) % kMaxSize;
+    }
+  } else {
+    // as an analog graph draws linear lines in between each value,
+    // insert duplicate value if "long" time between updates so it
+    // looks appropriately flat
+    if (m_size < kMaxSize) {
+      if (m_size > 0) {
+        if ((time - m_data[m_size - 1].x) > kTimeGap) {
+          m_data[m_size] = ImPlotPoint{time, m_data[m_size - 1].y};
+          ++m_size;
+        }
+      }
+      m_data[m_size] = ImPlotPoint{time, value};
+      ++m_size;
+    } else {
+      if (m_offset == 0) {
+        if ((time - m_data[kMaxSize - 1].x) > kTimeGap) {
+          m_data[m_offset] = ImPlotPoint{time, m_data[kMaxSize - 1].y};
+          ++m_offset;
+        }
+      } else {
+        if ((time - m_data[m_offset - 1].x) > kTimeGap) {
+          m_data[m_offset] = ImPlotPoint{time, m_data[m_offset - 1].y};
+          m_offset = (m_offset + 1) % kMaxSize;
+        }
+      }
+      m_data[m_offset] = ImPlotPoint{time, value};
+      m_offset = (m_offset + 1) % kMaxSize;
+    }
+  }
+}
+
+bool PlotSeries::ReadIni(std::string_view name, std::string_view value) {
+  if (name == "name") {
+    m_name = value;
+    return true;
+  }
+  if (name == "yAxis") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_yAxis = num.value();
+    }
+    return true;
+  } else if (name == "color") {
+    if (auto num = wpi::parse_integer<unsigned int>(value, 10)) {
+      m_color = ImColor(num.value());
+    }
+    return true;
+  } else if (name == "marker") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_marker = num.value();
+    }
+    return true;
+  } else if (name == "weight") {
+    if (auto num = wpi::parse_float<float>(value)) {
+      m_weight = num.value();
+    }
+    return true;
+  } else if (name == "digital") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_digital = num.value();
+    }
+    return true;
+  } else if (name == "digitalBitHeight") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_digitalBitHeight = num.value();
+    }
+    return true;
+  } else if (name == "digitalBitGap") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_digitalBitGap = num.value();
+    }
+    return true;
+  }
+  return false;
+}
+
+void PlotSeries::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf(
+      "name=%s\nyAxis=%d\ncolor=%u\nmarker=%d\nweight=%f\ndigital=%d\n"
+      "digitalBitHeight=%d\ndigitalBitGap=%d\n",
+      m_name.c_str(), m_yAxis, static_cast<ImU32>(ImColor(m_color)), m_marker,
+      m_weight, m_digital, m_digitalBitHeight, m_digitalBitGap);
+}
+
+const char* PlotSeries::GetName() const {
+  if (!m_name.empty()) {
+    return m_name.c_str();
+  }
+  if (m_newValueConn.connected()) {
+    auto sourceName = m_source->GetName();
+    if (sourceName[0] != '\0') {
+      return sourceName;
+    }
+  }
+  return m_id.c_str();
+}
+
+PlotSeries::Action PlotSeries::EmitPlot(PlotView& view, double now, size_t i,
+                                        size_t plotIndex) {
+  CheckSource();
+
+  char label[128];
+  std::snprintf(label, sizeof(label), "%s###name", GetName());
+
+  int size = m_size;
+  int offset = m_offset;
+
+  // need to have last value at current time, so need to create fake last value
+  // we handle the offset logic ourselves to avoid wrap issues with size + 1
+  struct GetterData {
+    double now;
+    double zeroTime;
+    ImPlotPoint* data;
+    int size;
+    int offset;
+  };
+  GetterData getterData = {now, GetZeroTime() * 1.0e-6, m_data, size, offset};
+  auto getter = [](void* data, int idx) {
+    auto d = static_cast<GetterData*>(data);
+    if (idx == d->size) {
+      return ImPlotPoint{
+          d->now - d->zeroTime,
+          d->data[d->offset == 0 ? d->size - 1 : d->offset - 1].y};
+    }
+    ImPlotPoint* point;
+    if (d->offset + idx < d->size) {
+      point = &d->data[d->offset + idx];
+    } else {
+      point = &d->data[d->offset + idx - d->size];
+    }
+    return ImPlotPoint{point->x - d->zeroTime, point->y};
+  };
+
+  if (m_color.w == IMPLOT_AUTO_COL.w) {
+    m_color = ImPlot::GetColormapColor(i);
+  }
+  ImPlot::SetNextLineStyle(m_color, m_weight);
+  if (IsDigital()) {
+    ImPlot::PushStyleVar(ImPlotStyleVar_DigitalBitHeight, m_digitalBitHeight);
+    ImPlot::PushStyleVar(ImPlotStyleVar_DigitalBitGap, m_digitalBitGap);
+    ImPlot::PlotDigitalG(label, getter, &getterData, size + 1);
+    ImPlot::PopStyleVar();
+    ImPlot::PopStyleVar();
+  } else {
+    ImPlot::SetPlotYAxis(m_yAxis);
+    ImPlot::SetNextMarkerStyle(m_marker - 1);
+    ImPlot::PlotLineG(label, getter, &getterData, size + 1);
+  }
+
+  // DND source for PlotSeries
+  if (ImPlot::BeginDragDropSourceItem(label)) {
+    EmitDragDropPayload(view, i, plotIndex);
+    ImPlot::EndDragDropSource();
+  }
+
+  // Show full source name tooltip
+  if (!m_name.empty() && ImPlot::IsLegendEntryHovered(label)) {
+    ImGui::SetTooltip("%s", m_id.c_str());
+  }
+
+  // Edit settings via popup
+  Action rv = kNone;
+  if (ImPlot::BeginLegendPopup(label)) {
+    ImGui::TextUnformatted(m_id.c_str());
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::Text("Edit series name:");
+    ImGui::InputText("##editname", &m_name);
+    if (ImGui::Button("Move Up")) {
+      ImGui::CloseCurrentPopup();
+      rv = kMoveUp;
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Move Down")) {
+      ImGui::CloseCurrentPopup();
+      rv = kMoveDown;
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Delete")) {
+      ImGui::CloseCurrentPopup();
+      rv = kDelete;
+    }
+    EmitSettings(i);
+    ImPlot::EndLegendPopup();
+  }
+
+  return rv;
+}
+
+void PlotSeries::EmitDragDropPayload(PlotView& view, size_t i,
+                                     size_t plotIndex) {
+  PlotSeriesRef ref = {&view, plotIndex, i};
+  ImGui::SetDragDropPayload("PlotSeries", &ref, sizeof(ref));
+  ImGui::TextUnformatted(GetName());
+}
+
+void PlotSeries::EmitSettings(size_t i) {
+  // Line color
+  {
+    ImGui::ColorEdit3("Color", &m_color.x, ImGuiColorEditFlags_NoInputs);
+    ImGui::SameLine();
+    if (ImGui::Button("Default")) {
+      m_color = ImPlot::GetColormapColor(i);
+    }
+  }
+
+  // Line weight
+  {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+    ImGui::InputFloat("Weight", &m_weight, 0.1f, 1.0f, "%.1f");
+  }
+
+  // Digital
+  {
+    static const char* const options[] = {"Auto", "Digital", "Analog"};
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+    ImGui::Combo("Digital", &m_digital, options,
+                 sizeof(options) / sizeof(options[0]));
+  }
+
+  if (IsDigital()) {
+    // Bit Height
+    {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      ImGui::InputInt("Bit Height", &m_digitalBitHeight);
+    }
+
+    // Bit Gap
+    {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      ImGui::InputInt("Bit Gap", &m_digitalBitGap);
+    }
+  } else {
+    // Y-axis
+    {
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
+      static const char* const options[] = {"1", "2", "3"};
+      ImGui::Combo("Y-Axis", &m_yAxis, options, 3);
+    }
+
+    // Marker
+    {
+      static const char* const options[] = {
+          "None", "Circle", "Square", "Diamond", "Up",      "Down",
+          "Left", "Right",  "Cross",  "Plus",    "Asterisk"};
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
+      ImGui::Combo("Marker", &m_marker, options,
+                   sizeof(options) / sizeof(options[0]));
+    }
+  }
+}
+
+Plot::Plot() {
+  for (int i = 0; i < 3; ++i) {
+    m_axisRange[i] = PlotRange{};
+  }
+}
+
+bool Plot::ReadIni(std::string_view name, std::string_view value) {
+  if (name == "name") {
+    m_name = value;
+    return true;
+  } else if (name == "visible") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_visible = num.value() != 0;
+    }
+    return true;
+  } else if (name == "showPause") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_showPause = num.value() != 0;
+    }
+    return true;
+  } else if (name == "lockPrevX") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_lockPrevX = num.value() != 0;
+    }
+    return true;
+  } else if (name == "legend") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      if (num.value() == 0) {
+        m_plotFlags |= ImPlotFlags_NoLegend;
+      } else {
+        m_plotFlags &= ~ImPlotFlags_NoLegend;
+      }
+    }
+    return true;
+  } else if (name == "yaxis2") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      if (num.value() == 0) {
+        m_plotFlags &= ~ImPlotFlags_YAxis2;
+      } else {
+        m_plotFlags |= ImPlotFlags_YAxis2;
+      }
+    }
+    return true;
+  } else if (name == "yaxis3") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      if (num.value() == 0) {
+        m_plotFlags &= ~ImPlotFlags_YAxis3;
+      } else {
+        m_plotFlags |= ImPlotFlags_YAxis3;
+      }
+    }
+    return true;
+  } else if (name == "viewTime") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_viewTime = num.value() / 1000.0;
+    }
+    return true;
+  } else if (name == "autoHeight") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_autoHeight = num.value() != 0;
+    }
+    return true;
+  } else if (name == "height") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      m_height = num.value();
+    }
+    return true;
+  } else if (wpi::starts_with(name, 'y')) {
+    auto [yAxisStr, yName] = wpi::split(name, '_');
+    int yAxis =
+        wpi::parse_integer<int>(wpi::drop_front(yAxisStr), 10).value_or(-1);
+    if (yAxis < 0 || yAxis > 3) {
+      return false;
+    }
+    if (yName == "min") {
+      if (auto num = wpi::parse_integer<int>(value, 10)) {
+        m_axisRange[yAxis].min = num.value() / 1000.0;
+      }
+      return true;
+    } else if (yName == "max") {
+      if (auto num = wpi::parse_integer<int>(value, 10)) {
+        m_axisRange[yAxis].max = num.value() / 1000.0;
+      }
+      return true;
+    } else if (yName == "lockMin") {
+      if (auto num = wpi::parse_integer<int>(value, 10)) {
+        m_axisRange[yAxis].lockMin = num.value() != 0;
+      }
+      return true;
+    } else if (yName == "lockMax") {
+      if (auto num = wpi::parse_integer<int>(value, 10)) {
+        m_axisRange[yAxis].lockMax = num.value() != 0;
+      }
+      return true;
+    } else if (yName == "label") {
+      m_axisLabel[yAxis] = value;
+      return true;
+    }
+  }
+  return false;
+}
+
+void Plot::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf(
+      "name=%s\nvisible=%d\nshowPause=%d\nlockPrevX=%d\nlegend=%d\n"
+      "yaxis2=%d\nyaxis3=%d\nviewTime=%d\nautoHeight=%d\nheight=%d\n",
+      m_name.c_str(), m_visible ? 1 : 0, m_showPause ? 1 : 0,
+      m_lockPrevX ? 1 : 0, (m_plotFlags & ImPlotFlags_NoLegend) ? 0 : 1,
+      (m_plotFlags & ImPlotFlags_YAxis2) ? 1 : 0,
+      (m_plotFlags & ImPlotFlags_YAxis3) ? 1 : 0,
+      static_cast<int>(m_viewTime * 1000), m_autoHeight ? 1 : 0, m_height);
+  for (int i = 0; i < 3; ++i) {
+    out->appendf(
+        "y%d_min=%d\ny%d_max=%d\ny%d_lockMin=%d\ny%d_lockMax=%d\n"
+        "y%d_label=%s\n",
+        i, static_cast<int>(m_axisRange[i].min * 1000), i,
+        static_cast<int>(m_axisRange[i].max * 1000), i,
+        m_axisRange[i].lockMin ? 1 : 0, i, m_axisRange[i].lockMax ? 1 : 0, i,
+        m_axisLabel[i].c_str());
+  }
+}
+
+void Plot::DragDropTarget(PlotView& view, size_t i, bool inPlot) {
+  if (!ImGui::BeginDragDropTarget()) {
+    return;
+  }
+  // handle dragging onto a specific Y axis
+  int yAxis = -1;
+  if (inPlot) {
+    for (int y = 0; y < 3; ++y) {
+      if (ImPlot::IsPlotYAxisHovered(y)) {
+        yAxis = y;
+        break;
+      }
+    }
+  }
+  if (const ImGuiPayload* payload =
+          ImGui::AcceptDragDropPayload("DataSource")) {
+    auto source = *static_cast<DataSource**>(payload->Data);
+    // don't add duplicates unless it's onto a different Y axis
+    auto it =
+        std::find_if(m_series.begin(), m_series.end(), [=](const auto& elem) {
+          return elem->GetId() == source->GetId() &&
+                 (yAxis == -1 || elem->GetYAxis() == yAxis);
+        });
+    if (it == m_series.end()) {
+      m_series.emplace_back(
+          std::make_unique<PlotSeries>(source, yAxis == -1 ? 0 : yAxis));
+    }
+  } else if (const ImGuiPayload* payload =
+                 ImGui::AcceptDragDropPayload("PlotSeries")) {
+    auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
+    view.MovePlotSeries(ref->view, ref->plotIndex, ref->seriesIndex, i,
+                        m_series.size(), yAxis);
+  } else if (const ImGuiPayload* payload =
+                 ImGui::AcceptDragDropPayload("Plot")) {
+    auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
+    view.MovePlot(ref->view, ref->plotIndex, i);
+  }
+}
+
+void Plot::EmitPlot(PlotView& view, double now, bool paused, size_t i) {
+  if (!m_visible) {
+    return;
+  }
+
+  bool lockX = (i != 0 && m_lockPrevX);
+
+  if (!lockX && m_showPause && ImGui::Button(m_paused ? "Resume" : "Pause")) {
+    m_paused = !m_paused;
+  }
+
+  char label[128];
+  std::snprintf(label, sizeof(label), "%s##plot", m_name.c_str());
+
+  if (lockX) {
+    ImPlot::SetNextPlotLimitsX(view.m_plots[i - 1]->m_xaxisRange.Min,
+                               view.m_plots[i - 1]->m_xaxisRange.Max,
+                               ImGuiCond_Always);
+  } else {
+    // also force-pause plots if overall timing is paused
+    double zeroTime = GetZeroTime() * 1.0e-6;
+    ImPlot::SetNextPlotLimitsX(
+        now - zeroTime - m_viewTime, now - zeroTime,
+        (paused || m_paused) ? ImGuiCond_Once : ImGuiCond_Always);
+  }
+
+  ImPlotAxisFlags yFlags[3] = {ImPlotAxisFlags_None,
+                               ImPlotAxisFlags_NoGridLines,
+                               ImPlotAxisFlags_NoGridLines};
+  for (int i = 0; i < 3; ++i) {
+    ImPlot::SetNextPlotLimitsY(
+        m_axisRange[i].min, m_axisRange[i].max,
+        m_axisRange[i].apply ? ImGuiCond_Always : ImGuiCond_Once, i);
+    m_axisRange[i].apply = false;
+    if (m_axisRange[i].lockMin) {
+      yFlags[i] |= ImPlotAxisFlags_LockMin;
+    }
+    if (m_axisRange[i].lockMax) {
+      yFlags[i] |= ImPlotAxisFlags_LockMax;
+    }
+  }
+
+  if (ImPlot::BeginPlot(
+          label, nullptr,
+          m_axisLabel[0].empty() ? nullptr : m_axisLabel[0].c_str(),
+          ImVec2(-1, m_height), m_plotFlags, ImPlotAxisFlags_None, yFlags[0],
+          yFlags[1], yFlags[2],
+          m_axisLabel[1].empty() ? nullptr : m_axisLabel[1].c_str(),
+          m_axisLabel[2].empty() ? nullptr : m_axisLabel[2].c_str())) {
+    for (size_t j = 0; j < m_series.size(); ++j) {
+      ImGui::PushID(j);
+      switch (m_series[j]->EmitPlot(view, now, j, i)) {
+        case PlotSeries::kMoveUp:
+          if (j > 0) {
+            std::swap(m_series[j - 1], m_series[j]);
+          }
+          break;
+        case PlotSeries::kMoveDown:
+          if (j < (m_series.size() - 1)) {
+            std::swap(m_series[j], m_series[j + 1]);
+          }
+          break;
+        case PlotSeries::kDelete:
+          m_series.erase(m_series.begin() + j);
+          break;
+        default:
+          break;
+      }
+      ImGui::PopID();
+    }
+    DragDropTarget(view, i, true);
+    m_xaxisRange = ImPlot::GetPlotLimits().X;
+    ImPlot::EndPlot();
+  }
+}
+
+void Plot::EmitSettingsLimits(int axis) {
+  ImGui::Indent();
+  ImGui::PushID(axis);
+
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 10);
+  ImGui::InputText("Label", &m_axisLabel[axis]);
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 3.5);
+  ImGui::InputDouble("Min", &m_axisRange[axis].min, 0, 0, "%.3f");
+  ImGui::SameLine();
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 3.5);
+  ImGui::InputDouble("Max", &m_axisRange[axis].max, 0, 0, "%.3f");
+  ImGui::SameLine();
+  if (ImGui::Button("Apply")) {
+    m_axisRange[axis].apply = true;
+  }
+
+  ImGui::TextUnformatted("Lock Axis");
+  ImGui::SameLine();
+  ImGui::Checkbox("Min##minlock", &m_axisRange[axis].lockMin);
+  ImGui::SameLine();
+  ImGui::Checkbox("Max##maxlock", &m_axisRange[axis].lockMax);
+
+  ImGui::PopID();
+  ImGui::Unindent();
+}
+
+void Plot::EmitSettings(size_t i) {
+  ImGui::Text("Edit plot name:");
+  ImGui::InputText("##editname", &m_name);
+  ImGui::Checkbox("Visible", &m_visible);
+  ImGui::Checkbox("Show Pause Button", &m_showPause);
+  ImGui::CheckboxFlags("Hide Legend", &m_plotFlags, ImPlotFlags_NoLegend);
+  if (i != 0) {
+    ImGui::Checkbox("Lock X-axis to previous plot", &m_lockPrevX);
+  }
+  ImGui::TextUnformatted("Primary Y-Axis");
+  EmitSettingsLimits(0);
+  ImGui::CheckboxFlags("2nd Y-Axis", &m_plotFlags, ImPlotFlags_YAxis2);
+  if ((m_plotFlags & ImPlotFlags_YAxis2) != 0) {
+    EmitSettingsLimits(1);
+  }
+  ImGui::CheckboxFlags("3rd Y-Axis", &m_plotFlags, ImPlotFlags_YAxis3);
+  if ((m_plotFlags & ImPlotFlags_YAxis3) != 0) {
+    EmitSettingsLimits(2);
+  }
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+  ImGui::InputFloat("View Time (s)", &m_viewTime, 0.1f, 1.0f, "%.1f");
+  ImGui::Checkbox("Auto Height", &m_autoHeight);
+  if (!m_autoHeight) {
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
+    if (ImGui::InputInt("Height", &m_height, 10)) {
+      if (m_height < 0) {
+        m_height = 0;
+      }
+    }
+  }
+}
+
+int Plot::GetAutoBaseHeight(bool* isAuto, size_t i) {
+  *isAuto = m_autoHeight;
+
+  if (!m_visible) {
+    return 0;
+  }
+
+  int height = m_autoHeight ? 0 : m_height;
+
+  // Pause button
+  if ((i == 0 || !m_lockPrevX) && m_showPause) {
+    height += ImGui::GetFrameHeightWithSpacing();
+  }
+
+  return height;
+}
+
+void PlotView::Display() {
+  if (ImGui::BeginPopupContextItem()) {
+    if (ImGui::Button("Add plot")) {
+      m_plots.emplace_back(std::make_unique<Plot>());
+    }
+
+    for (size_t i = 0; i < m_plots.size(); ++i) {
+      auto& plot = m_plots[i];
+      ImGui::PushID(i);
+
+      char name[64];
+      if (!plot->GetName().empty()) {
+        std::snprintf(name, sizeof(name), "%s", plot->GetName().c_str());
+      } else {
+        std::snprintf(name, sizeof(name), "Plot %d", static_cast<int>(i));
+      }
+
+      char label[90];
+      std::snprintf(label, sizeof(label), "%s###header%d", name,
+                    static_cast<int>(i));
+
+      bool open = ImGui::CollapsingHeader(label);
+
+      // DND source and target for Plot
+      if (ImGui::BeginDragDropSource()) {
+        PlotSeriesRef ref = {this, i, 0};
+        ImGui::SetDragDropPayload("Plot", &ref, sizeof(ref));
+        ImGui::TextUnformatted(name);
+        ImGui::EndDragDropSource();
+      }
+      plot->DragDropTarget(*this, i, false);
+
+      if (open) {
+        if (ImGui::Button("Move Up")) {
+          if (i > 0) {
+            std::swap(m_plots[i - 1], plot);
+          }
+        }
+
+        ImGui::SameLine();
+        if (ImGui::Button("Move Down")) {
+          if (i < (m_plots.size() - 1)) {
+            std::swap(plot, m_plots[i + 1]);
+          }
+        }
+
+        ImGui::SameLine();
+        if (ImGui::Button("Delete")) {
+          m_plots.erase(m_plots.begin() + i);
+          ImGui::PopID();
+          continue;
+        }
+
+        plot->EmitSettings(i);
+      }
+
+      ImGui::PopID();
+    }
+
+    ImGui::EndPopup();
+  }
+
+  if (m_plots.empty()) {
+    if (ImGui::Button("Add plot")) {
+      m_plots.emplace_back(std::make_unique<Plot>());
+    }
+
+    // Make "add plot" button a DND target for Plot
+    if (!ImGui::BeginDragDropTarget()) {
+      return;
+    }
+    if (const ImGuiPayload* payload = ImGui::AcceptDragDropPayload("Plot")) {
+      auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
+      MovePlot(ref->view, ref->plotIndex, 0);
+    }
+    return;
+  }
+
+  // Auto-size plots.  This requires two passes: the first pass to get the
+  // total height, the second to actually set the height after averaging it
+  // across all auto-sized heights.
+  int availHeight = ImGui::GetContentRegionAvail().y;
+  int numAuto = 0;
+  for (size_t i = 0; i < m_plots.size(); ++i) {
+    bool isAuto;
+    availHeight -= m_plots[i]->GetAutoBaseHeight(&isAuto, i);
+    availHeight -= ImGui::GetStyle().ItemSpacing.y;
+    if (isAuto) {
+      ++numAuto;
+    }
+  }
+  if (numAuto > 0) {
+    availHeight /= numAuto;
+    for (size_t i = 0; i < m_plots.size(); ++i) {
+      m_plots[i]->SetAutoHeight(availHeight);
+    }
+  }
+
+  double now = wpi::Now() * 1.0e-6;
+  for (size_t i = 0; i < m_plots.size(); ++i) {
+    ImGui::PushID(i);
+    m_plots[i]->EmitPlot(*this, now, m_provider->IsPaused(), i);
+    ImGui::PopID();
+  }
+}
+
+void PlotView::MovePlot(PlotView* fromView, size_t fromIndex, size_t toIndex) {
+  if (fromView == this) {
+    if (fromIndex == toIndex) {
+      return;
+    }
+    auto val = std::move(m_plots[fromIndex]);
+    m_plots.insert(m_plots.begin() + toIndex, std::move(val));
+    m_plots.erase(m_plots.begin() + fromIndex + (fromIndex > toIndex ? 1 : 0));
+  } else {
+    auto val = std::move(fromView->m_plots[fromIndex]);
+    m_plots.insert(m_plots.begin() + toIndex, std::move(val));
+    fromView->m_plots.erase(fromView->m_plots.begin() + fromIndex);
+  }
+}
+
+void PlotView::MovePlotSeries(PlotView* fromView, size_t fromPlotIndex,
+                              size_t fromSeriesIndex, size_t toPlotIndex,
+                              size_t toSeriesIndex, int yAxis) {
+  if (fromView == this && fromPlotIndex == toPlotIndex) {
+    // need to handle this specially as the index of the old location changes
+    if (fromSeriesIndex != toSeriesIndex) {
+      auto& plotSeries = m_plots[fromPlotIndex]->m_series;
+      auto val = std::move(plotSeries[fromSeriesIndex]);
+      // only set Y-axis if actually set
+      if (yAxis != -1) {
+        val->SetYAxis(yAxis);
+      }
+      plotSeries.insert(plotSeries.begin() + toSeriesIndex, std::move(val));
+      plotSeries.erase(plotSeries.begin() + fromSeriesIndex +
+                       (fromSeriesIndex > toSeriesIndex ? 1 : 0));
+    }
+  } else {
+    auto& fromPlot = *fromView->m_plots[fromPlotIndex];
+    auto& toPlot = *m_plots[toPlotIndex];
+    // always set Y-axis if moving plots
+    fromPlot.m_series[fromSeriesIndex]->SetYAxis(yAxis == -1 ? 0 : yAxis);
+    toPlot.m_series.insert(toPlot.m_series.begin() + toSeriesIndex,
+                           std::move(fromPlot.m_series[fromSeriesIndex]));
+    fromPlot.m_series.erase(fromPlot.m_series.begin() + fromSeriesIndex);
+  }
+}
+
+PlotProvider::PlotProvider(std::string_view iniName)
+    : WindowManager{fmt::format("{}Window", iniName)},
+      m_plotSaver{iniName, this, false},
+      m_seriesSaver{fmt::format("{}Series", iniName), this, true} {}
+
+PlotProvider::~PlotProvider() = default;
+
+void PlotProvider::GlobalInit() {
+  WindowManager::GlobalInit();
+  wpi::gui::AddInit([this] {
+    m_plotSaver.Initialize();
+    m_seriesSaver.Initialize();
+  });
+}
+
+void PlotProvider::DisplayMenu() {
+  for (size_t i = 0; i < m_windows.size(); ++i) {
+    m_windows[i]->DisplayMenuItem();
+    // provide method to destroy the plot window
+    if (ImGui::BeginPopupContextItem()) {
+      if (ImGui::Selectable("Destroy Plot Window")) {
+        m_windows.erase(m_windows.begin() + i);
+        ImGui::CloseCurrentPopup();
+      }
+      ImGui::EndPopup();
+    }
+  }
+
+  if (ImGui::MenuItem("New Plot Window")) {
+    // this is an inefficient algorithm, but the number of windows is small
+    char id[32];
+    size_t numWindows = m_windows.size();
+    for (size_t i = 0; i <= numWindows; ++i) {
+      std::snprintf(id, sizeof(id), "Plot <%d>", static_cast<int>(i));
+      bool match = false;
+      for (size_t j = i; j < numWindows; ++j) {
+        if (m_windows[j]->GetId() == id) {
+          match = true;
+          break;
+        }
+      }
+      if (!match) {
+        break;
+      }
+    }
+    if (auto win = AddWindow(id, std::make_unique<PlotView>(this))) {
+      win->SetDefaultSize(700, 400);
+    }
+  }
+}
+
+void PlotProvider::DisplayWindows() {
+  // create views if not already created
+  for (auto&& window : m_windows) {
+    if (!window->HasView()) {
+      window->SetView(std::make_unique<PlotView>(this));
+    }
+  }
+  WindowManager::DisplayWindows();
+}
+
+PlotProvider::IniSaver::IniSaver(std::string_view typeName,
+                                 PlotProvider* provider, bool forSeries)
+    : IniSaverBase{typeName}, m_provider{provider}, m_forSeries{forSeries} {}
+
+void* PlotProvider::IniSaver::IniReadOpen(const char* name) {
+  auto [viewId, plotNumStr] = wpi::split(name, '#');
+  std::string_view seriesId;
+  if (m_forSeries) {
+    std::tie(plotNumStr, seriesId) = wpi::split(plotNumStr, '#');
+    if (seriesId.empty()) {
+      return nullptr;
+    }
+  }
+  unsigned int plotNum;
+  if (auto plotNumOpt = wpi::parse_integer<unsigned int>(plotNumStr, 10)) {
+    plotNum = plotNumOpt.value();
+  } else {
+    return nullptr;
+  }
+
+  // get or create window
+  auto win = m_provider->GetOrAddWindow(viewId, true);
+  if (!win) {
+    return nullptr;
+  }
+
+  // get or create view
+  auto view = static_cast<PlotView*>(win->GetView());
+  if (!view) {
+    win->SetView(std::make_unique<PlotView>(m_provider));
+    view = static_cast<PlotView*>(win->GetView());
+  }
+
+  // get or create plot
+  if (view->m_plots.size() <= plotNum) {
+    view->m_plots.resize(plotNum + 1);
+  }
+  auto& plot = view->m_plots[plotNum];
+  if (!plot) {
+    plot = std::make_unique<Plot>();
+  }
+
+  // early exit for plot data
+  if (!m_forSeries) {
+    return plot.get();
+  }
+
+  // get or create series
+  return plot->m_series.emplace_back(std::make_unique<PlotSeries>(seriesId))
+      .get();
+}
+
+void PlotProvider::IniSaver::IniReadLine(void* entry, const char* line) {
+  auto [name, value] = wpi::split(line, '=');
+  name = wpi::trim(name);
+  value = wpi::trim(value);
+  if (m_forSeries) {
+    static_cast<PlotSeries*>(entry)->ReadIni(name, value);
+  } else {
+    static_cast<Plot*>(entry)->ReadIni(name, value);
+  }
+}
+
+void PlotProvider::IniSaver::IniWriteAll(ImGuiTextBuffer* out_buf) {
+  for (auto&& win : m_provider->m_windows) {
+    auto view = static_cast<PlotView*>(win->GetView());
+    auto id = win->GetId();
+    for (size_t i = 0; i < view->m_plots.size(); ++i) {
+      if (m_forSeries) {
+        // Loop over series
+        for (auto&& series : view->m_plots[i]->m_series) {
+          out_buf->appendf("[%s][%s#%d#%s]\n", GetTypeName(), id.data(),
+                           static_cast<int>(i), series->GetId().c_str());
+          series->WriteIni(out_buf);
+          out_buf->append("\n");
+        }
+      } else {
+        // Just the plot
+        out_buf->appendf("[%s][%s#%d]\n", GetTypeName(), id.data(),
+                         static_cast<int>(i));
+        view->m_plots[i]->WriteIni(out_buf);
+        out_buf->append("\n");
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/StringChooser.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/StringChooser.cpp
new file mode 100644
index 0000000..46fa674
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/StringChooser.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/StringChooser.h"
+
+#include <imgui.h>
+
+using namespace glass;
+
+void glass::DisplayStringChooser(StringChooserModel* model) {
+  auto& defaultValue = model->GetDefault();
+  auto& selected = model->GetSelected();
+  auto& active = model->GetActive();
+  auto& options = model->GetOptions();
+
+  const char* preview =
+      selected.empty() ? defaultValue.c_str() : selected.c_str();
+
+  const char* label;
+  if (active == preview) {
+    label = "GOOD##select";
+  } else {
+    label = "BAD ##select";
+  }
+
+  if (ImGui::BeginCombo(label, preview)) {
+    for (auto&& option : options) {
+      ImGui::PushID(option.c_str());
+      bool isSelected = (option == selected);
+      if (ImGui::Selectable(option.c_str(), isSelected)) {
+        model->SetSelected(option);
+      }
+      if (isSelected) {
+        ImGui::SetItemDefaultFocus();
+      }
+      ImGui::PopID();
+    }
+    ImGui::EndCombo();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/other/Subsystem.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/other/Subsystem.cpp
new file mode 100644
index 0000000..c4ed474
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/other/Subsystem.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/other/Subsystem.h"
+
+#include <imgui.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+void glass::DisplaySubsystem(SubsystemModel* m) {
+  if (auto name = m->GetName()) {
+    ImGui::Text("%s", name);
+    ImGui::Separator();
+  }
+  if (m->Exists()) {
+    std::string defaultCommand = m->GetDefaultCommand();
+    std::string currentCommand = m->GetCurrentCommand();
+    ImGui::Text("%s", ("Default Command: " + defaultCommand).c_str());
+    ImGui::Text("%s", ("Current Command: " + currentCommand).c_str());
+  } else {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
+    ImGui::Text("Unknown Subsystem");
+    ImGui::PopStyleColor();
+  }
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp
new file mode 100644
index 0000000..2af6e5e
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/support/ExtraGuiWidgets.cpp
@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/support/ExtraGuiWidgets.h"
+
+#define IMGUI_DEFINE_MATH_OPERATORS
+#include <imgui_internal.h>
+
+#include "glass/DataSource.h"
+
+namespace glass {
+
+void DrawLEDSources(const int* values, DataSource** sources, int numValues,
+                    int cols, const ImU32* colors, float size, float spacing,
+                    const LEDConfig& config) {
+  if (numValues == 0 || cols < 1) {
+    return;
+  }
+  if (size == 0) {
+    size = ImGui::GetFontSize() / 2.0;
+  }
+  if (spacing == 0) {
+    spacing = ImGui::GetFontSize() / 3.0;
+  }
+
+  int rows = (numValues + cols - 1) / cols;
+  float inc = size + spacing;
+
+  ImDrawList* drawList = ImGui::GetWindowDrawList();
+  const ImVec2 p = ImGui::GetCursorScreenPos();
+
+  float sized2 = size / 2;
+  float ystart, yinc;
+  if (config.start & 1) {
+    // lower
+    ystart = p.y + sized2 + inc * (rows - 1);
+    yinc = -inc;
+  } else {
+    // upper
+    ystart = p.y + sized2;
+    yinc = inc;
+  }
+
+  float xstart, xinc;
+  if (config.start & 2) {
+    // right
+    xstart = p.x + sized2 + inc * (cols - 1);
+    xinc = -inc;
+  } else {
+    // left
+    xstart = p.x + sized2;
+    xinc = inc;
+  }
+
+  float x = xstart, y = ystart;
+  int rowcol = 1;  // row for row-major, column for column-major
+  for (int i = 0; i < numValues; ++i) {
+    if (config.order == LEDConfig::RowMajor) {
+      if (i >= (rowcol * cols)) {
+        ++rowcol;
+        if (config.serpentine) {
+          x -= xinc;
+          xinc = -xinc;
+        } else {
+          x = xstart;
+        }
+        y += yinc;
+      }
+    } else {
+      if (i >= (rowcol * rows)) {
+        ++rowcol;
+        if (config.serpentine) {
+          y -= yinc;
+          yinc = -yinc;
+        } else {
+          y = ystart;
+        }
+        x += xinc;
+      }
+    }
+    if (values[i] > 0) {
+      drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
+                              colors[values[i] - 1]);
+    } else if (values[i] < 0) {
+      drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
+                        colors[-values[i] - 1], 0.0f, 0, 1.0);
+    }
+    if (sources) {
+      ImGui::SetCursorScreenPos(ImVec2(x - sized2, y - sized2));
+      if (sources[i]) {
+        ImGui::PushID(i);
+        ImGui::Selectable("", false, 0, ImVec2(inc, inc));
+        sources[i]->EmitDrag();
+        ImGui::PopID();
+      } else {
+        ImGui::Dummy(ImVec2(inc, inc));
+      }
+    }
+    if (config.order == LEDConfig::RowMajor) {
+      x += xinc;
+    } else {
+      y += yinc;
+    }
+  }
+
+  if (!sources) {
+    ImGui::Dummy(ImVec2(inc * cols, inc * rows));
+  }
+}
+
+void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
+              float size, float spacing, const LEDConfig& config) {
+  DrawLEDSources(values, nullptr, numValues, cols, colors, size, spacing,
+                 config);
+}
+
+bool DeleteButton(ImGuiID id, const ImVec2& pos) {
+  ImGuiContext& g = *GImGui;
+  ImGuiWindow* window = g.CurrentWindow;
+
+  // We intentionally allow interaction when clipped so that a mechanical
+  // Alt,Right,Validate sequence close a window. (this isn't the regular
+  // behavior of buttons, but it doesn't affect the user much because navigation
+  // tends to keep items visible).
+  const ImRect bb(
+      pos, pos + ImVec2(g.FontSize, g.FontSize) + g.Style.FramePadding * 2.0f);
+  bool is_clipped = !ImGui::ItemAdd(bb, id);
+
+  bool hovered, held;
+  bool pressed = ImGui::ButtonBehavior(bb, id, &hovered, &held);
+  if (is_clipped) {
+    return pressed;
+  }
+
+  // Render
+  ImU32 col =
+      ImGui::GetColorU32(held ? ImGuiCol_ButtonActive : ImGuiCol_ButtonHovered);
+  ImVec2 center = bb.GetCenter();
+  if (hovered) {
+    window->DrawList->AddCircleFilled(
+        center, ImMax(2.0f, g.FontSize * 0.5f + 1.0f), col, 12);
+  }
+
+  ImU32 cross_col = ImGui::GetColorU32(ImGuiCol_Text);
+  window->DrawList->AddCircle(center, ImMax(2.0f, g.FontSize * 0.5f + 1.0f),
+                              cross_col, 12);
+  float cross_extent = g.FontSize * 0.5f * 0.5f - 1.0f;
+  center -= ImVec2(0.5f, 0.5f);
+  window->DrawList->AddLine(center + ImVec2(+cross_extent, +cross_extent),
+                            center + ImVec2(-cross_extent, -cross_extent),
+                            cross_col, 1.0f);
+  window->DrawList->AddLine(center + ImVec2(+cross_extent, -cross_extent),
+                            center + ImVec2(-cross_extent, +cross_extent),
+                            cross_col, 1.0f);
+
+  return pressed;
+}
+
+bool HeaderDeleteButton(const char* label) {
+  ImGuiWindow* window = ImGui::GetCurrentWindow();
+  ImGuiContext& g = *GImGui;
+  ImGuiLastItemDataBackup last_item_backup;
+  ImGuiID id = window->GetID(label);
+  float button_size = g.FontSize;
+  float button_x = ImMax(window->DC.LastItemRect.Min.x,
+                         window->DC.LastItemRect.Max.x -
+                             g.Style.FramePadding.x * 2.0f - button_size);
+  float button_y = window->DC.LastItemRect.Min.y;
+  bool rv = DeleteButton(
+      window->GetID(reinterpret_cast<void*>(static_cast<intptr_t>(id) + 1)),
+      ImVec2(button_x, button_y));
+  last_item_backup.Restore();
+  return rv;
+}
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/support/IniSaverBase.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/support/IniSaverBase.cpp
new file mode 100644
index 0000000..ae8d811
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/support/IniSaverBase.cpp
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/support/IniSaverBase.h"
+
+#include <imgui_internal.h>
+
+using namespace glass;
+
+namespace {
+class ImGuiSaver : public IniSaverBackend {
+ public:
+  void Register(IniSaverBase* iniSaver) override;
+  void Unregister(IniSaverBase* iniSaver) override;
+};
+}  // namespace
+
+void ImGuiSaver::Register(IniSaverBase* iniSaver) {
+  // hook ini handler to save settings
+  ImGuiSettingsHandler iniHandler;
+  iniHandler.TypeName = iniSaver->GetTypeName();
+  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+  iniHandler.ReadOpenFn = [](ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                             const char* name) {
+    return static_cast<IniSaverBase*>(handler->UserData)->IniReadOpen(name);
+  };
+  iniHandler.ReadLineFn = [](ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                             void* entry, const char* line) {
+    static_cast<IniSaverBase*>(handler->UserData)->IniReadLine(entry, line);
+  };
+  iniHandler.WriteAllFn = [](ImGuiContext* ctx, ImGuiSettingsHandler* handler,
+                             ImGuiTextBuffer* out_buf) {
+    static_cast<IniSaverBase*>(handler->UserData)->IniWriteAll(out_buf);
+  };
+  iniHandler.UserData = iniSaver;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+}
+
+void ImGuiSaver::Unregister(IniSaverBase* iniSaver) {
+  if (auto ctx = ImGui::GetCurrentContext()) {
+    auto& handlers = ctx->SettingsHandlers;
+    for (auto it = handlers.begin(), end = handlers.end(); it != end; ++it) {
+      if (it->UserData == iniSaver) {
+        handlers.erase(it);
+        return;
+      }
+    }
+  }
+}
+
+static ImGuiSaver* GetSaverInstance() {
+  static ImGuiSaver* inst = new ImGuiSaver;
+  return inst;
+}
+
+IniSaverBase::IniSaverBase(std::string_view typeName, IniSaverBackend* backend)
+    : m_typeName(typeName), m_backend{backend ? backend : GetSaverInstance()} {}
+
+IniSaverBase::~IniSaverBase() {
+  m_backend->Unregister(this);
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/cpp/support/IniSaverInfo.cpp b/third_party/allwpilib/glass/src/lib/native/cpp/support/IniSaverInfo.cpp
new file mode 100644
index 0000000..6525e8e
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/cpp/support/IniSaverInfo.cpp
@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/support/IniSaverInfo.h"
+
+#include <cstdio>
+#include <cstring>
+
+#include <imgui_internal.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+void NameInfo::SetName(std::string_view name) {
+  size_t len = (std::min)(name.size(), sizeof(m_name) - 1);
+  std::memcpy(m_name, name.data(), len);
+  m_name[len] = '\0';
+}
+
+void NameInfo::GetName(char* buf, size_t size, const char* defaultName) const {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s", m_name);
+  } else {
+    std::snprintf(buf, size, "%s", defaultName);
+  }
+}
+
+void NameInfo::GetName(char* buf, size_t size, const char* defaultName,
+                       int index) const {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s [%d]", m_name, index);
+  } else {
+    std::snprintf(buf, size, "%s[%d]", defaultName, index);
+  }
+}
+
+void NameInfo::GetName(char* buf, size_t size, const char* defaultName,
+                       int index, int index2) const {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s [%d,%d]", m_name, index, index2);
+  } else {
+    std::snprintf(buf, size, "%s[%d,%d]", defaultName, index, index2);
+  }
+}
+
+void NameInfo::GetLabel(char* buf, size_t size, const char* defaultName) const {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s###Name%s", m_name, defaultName);
+  } else {
+    std::snprintf(buf, size, "%s###Name%s", defaultName, defaultName);
+  }
+}
+
+void NameInfo::GetLabel(char* buf, size_t size, const char* defaultName,
+                        int index) const {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s [%d]###Name%d", m_name, index, index);
+  } else {
+    std::snprintf(buf, size, "%s[%d]###Name%d", defaultName, index, index);
+  }
+}
+
+void NameInfo::GetLabel(char* buf, size_t size, const char* defaultName,
+                        int index, int index2) const {
+  if (m_name[0] != '\0') {
+    std::snprintf(buf, size, "%s [%d,%d]###Name%d", m_name, index, index2,
+                  index);
+  } else {
+    std::snprintf(buf, size, "%s[%d,%d]###Name%d", defaultName, index, index2,
+                  index);
+  }
+}
+
+bool NameInfo::ReadIni(std::string_view name, std::string_view value) {
+  if (name != "name") {
+    return false;
+  }
+  size_t len = (std::min)(value.size(), sizeof(m_name) - 1);
+  std::memcpy(m_name, value.data(), len);
+  m_name[len] = '\0';
+  return true;
+}
+
+void NameInfo::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf("name=%s\n", m_name);
+}
+
+void NameInfo::PushEditNameId(int index) {
+  char id[64];
+  std::snprintf(id, sizeof(id), "Name%d", index);
+  ImGui::PushID(id);
+}
+
+void NameInfo::PushEditNameId(const char* name) {
+  char id[128];
+  std::snprintf(id, sizeof(id), "Name%s", name);
+  ImGui::PushID(id);
+}
+
+bool NameInfo::PopupEditName(int index) {
+  bool rv = false;
+  char id[64];
+  std::snprintf(id, sizeof(id), "Name%d", index);
+  if (ImGui::BeginPopupContextItem(id)) {
+    ImGui::Text("Edit name:");
+    if (InputTextName("##edit")) {
+      rv = true;
+    }
+    if (ImGui::Button("Close") || ImGui::IsKeyPressedMap(ImGuiKey_Enter) ||
+        ImGui::IsKeyPressedMap(ImGuiKey_KeyPadEnter)) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+  return rv;
+}
+
+bool NameInfo::PopupEditName(const char* name) {
+  bool rv = false;
+  char id[128];
+  std::snprintf(id, sizeof(id), "Name%s", name);
+  if (ImGui::BeginPopupContextItem(id)) {
+    ImGui::Text("Edit name:");
+    if (InputTextName("##edit")) {
+      rv = true;
+    }
+    if (ImGui::Button("Close") || ImGui::IsKeyPressedMap(ImGuiKey_Enter) ||
+        ImGui::IsKeyPressedMap(ImGuiKey_KeyPadEnter)) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+  return rv;
+}
+
+bool NameInfo::InputTextName(const char* label_id, ImGuiInputTextFlags flags) {
+  return ImGui::InputText(label_id, m_name, sizeof(m_name), flags);
+}
+
+bool OpenInfo::ReadIni(std::string_view name, std::string_view value) {
+  if (name != "open") {
+    return false;
+  }
+  if (auto num = wpi::parse_integer<int>(value, 10)) {
+    m_open = num.value();
+  }
+  return true;
+}
+
+void OpenInfo::WriteIni(ImGuiTextBuffer* out) {
+  out->appendf("open=%d\n", m_open ? 1 : 0);
+}
+
+bool NameOpenInfo::ReadIni(std::string_view name, std::string_view value) {
+  if (NameInfo::ReadIni(name, value)) {
+    return true;
+  }
+  if (OpenInfo::ReadIni(name, value)) {
+    return true;
+  }
+  return false;
+}
+
+void NameOpenInfo::WriteIni(ImGuiTextBuffer* out) {
+  NameInfo::WriteIni(out);
+  OpenInfo::WriteIni(out);
+}
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/Context.h b/third_party/allwpilib/glass/src/lib/native/include/glass/Context.h
new file mode 100644
index 0000000..62b0a33
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/Context.h
@@ -0,0 +1,156 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <imgui.h>
+
+namespace glass {
+
+struct Context;
+
+Context* CreateContext();
+void DestroyContext(Context* ctx = nullptr);
+Context* GetCurrentContext();
+void SetCurrentContext(Context* ctx);
+
+/**
+ * Resets zero time to current time.
+ */
+void ResetTime();
+
+/**
+ * Gets the zero time.
+ */
+uint64_t GetZeroTime();
+
+/**
+ * Storage provides both persistent and non-persistent key/value storage for
+ * widgets.
+ *
+ * Keys are always strings.  The storage also provides non-persistent arbitrary
+ * data storage (via std::shared_ptr<void>).
+ *
+ * Storage is automatically indexed internally by the ID stack.  Note it is
+ * necessary to use the glass wrappers for PushID et al to preserve naming in
+ * the save file (unnamed values are still stored, but this is non-ideal for
+ * users trying to hand-edit the save file).
+ */
+class Storage {
+ public:
+  struct Value {
+    Value() = default;
+    explicit Value(std::string_view str) : stringVal{str} {}
+
+    enum Type { kNone, kInt, kInt64, kBool, kFloat, kDouble, kString };
+    Type type = kNone;
+    union {
+      int intVal;
+      int64_t int64Val;
+      bool boolVal;
+      float floatVal;
+      double doubleVal;
+    };
+    std::string stringVal;
+  };
+
+  int GetInt(std::string_view key, int defaultVal = 0) const;
+  int64_t GetInt64(std::string_view key, int64_t defaultVal = 0) const;
+  bool GetBool(std::string_view key, bool defaultVal = false) const;
+  float GetFloat(std::string_view key, float defaultVal = 0.0f) const;
+  double GetDouble(std::string_view key, double defaultVal = 0.0) const;
+  std::string GetString(std::string_view key,
+                        std::string_view defaultVal = {}) const;
+
+  void SetInt(std::string_view key, int val);
+  void SetInt64(std::string_view key, int64_t val);
+  void SetBool(std::string_view key, bool val);
+  void SetFloat(std::string_view key, float val);
+  void SetDouble(std::string_view key, double val);
+  void SetString(std::string_view key, std::string_view val);
+
+  int* GetIntRef(std::string_view key, int defaultVal = 0);
+  int64_t* GetInt64Ref(std::string_view key, int64_t defaultVal = 0);
+  bool* GetBoolRef(std::string_view key, bool defaultVal = false);
+  float* GetFloatRef(std::string_view key, float defaultVal = 0.0f);
+  double* GetDoubleRef(std::string_view key, double defaultVal = 0.0);
+  std::string* GetStringRef(std::string_view key,
+                            std::string_view defaultVal = {});
+
+  Value& GetValue(std::string_view key);
+
+  void SetData(std::shared_ptr<void>&& data) { m_data = std::move(data); }
+
+  template <typename T>
+  T* GetData() const {
+    return static_cast<T*>(m_data.get());
+  }
+
+  Storage() = default;
+  Storage(const Storage&) = delete;
+  Storage& operator=(const Storage&) = delete;
+
+  std::vector<std::string>& GetKeys() { return m_keys; }
+  const std::vector<std::string>& GetKeys() const { return m_keys; }
+  std::vector<std::unique_ptr<Value>>& GetValues() { return m_values; }
+  const std::vector<std::unique_ptr<Value>>& GetValues() const {
+    return m_values;
+  }
+
+ private:
+  mutable std::vector<std::string> m_keys;
+  mutable std::vector<std::unique_ptr<Value>> m_values;
+  std::shared_ptr<void> m_data;
+};
+
+Storage& GetStorage();
+Storage& GetStorage(std::string_view id);
+
+bool Begin(const char* name, bool* p_open = nullptr,
+           ImGuiWindowFlags flags = 0);
+
+void End();
+
+bool BeginChild(const char* str_id, const ImVec2& size = ImVec2(0, 0),
+                bool border = false, ImGuiWindowFlags flags = 0);
+
+void EndChild();
+
+/**
+ * Saves open status to storage "open" key.
+ * If returning 'true' the header is open. doesn't indent nor push on ID stack.
+ * user doesn't have to call TreePop().
+ */
+bool CollapsingHeader(const char* label, ImGuiTreeNodeFlags flags = 0);
+
+bool TreeNodeEx(const char* label, ImGuiTreeNodeFlags flags = 0);
+
+void TreePop();
+
+// push string into the ID stack (will hash string).
+void PushID(const char* str_id);
+
+// push string into the ID stack (will hash string).
+void PushID(const char* str_id_begin, const char* str_id_end);
+
+// push string into the ID stack (will hash string).
+inline void PushID(std::string_view str) {
+  PushID(str.data(), str.data() + str.size());
+}
+
+// push integer into the ID stack (will hash integer).
+void PushID(int int_id);
+
+// pop from the ID stack.
+void PopID();
+
+bool PopupEditName(const char* label, std::string* name);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/ContextInternal.h b/third_party/allwpilib/glass/src/lib/native/include/glass/ContextInternal.h
new file mode 100644
index 0000000..39e54f3
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/ContextInternal.h
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <memory>
+
+#include <imgui.h>
+#include <wpi/SmallString.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringMap.h>
+
+#include "glass/Context.h"
+#include "glass/support/IniSaverInfo.h"
+#include "glass/support/IniSaverString.h"
+
+namespace glass {
+
+class DataSource;
+
+class DataSourceName {
+ public:
+  DataSourceName() = default;
+  explicit DataSourceName(DataSource* source) : source{source} {}
+
+  bool ReadIni(std::string_view name_, std::string_view value) {
+    return name->ReadIni(name_, value);
+  }
+  void WriteIni(ImGuiTextBuffer* out) { name->WriteIni(out); }
+
+  std::unique_ptr<NameInfo> name{new NameInfo};
+  DataSource* source = nullptr;
+};
+
+struct Context {
+  wpi::SmallString<128> curId;
+  wpi::SmallVector<size_t, 32> idStack;
+  wpi::StringMap<std::unique_ptr<Storage>> storage;
+  wpi::StringMap<bool> deviceHidden;
+  IniSaverString<DataSourceName> sources{"Data Sources"};
+  uint64_t zeroTime = 0;
+};
+
+extern Context* gContext;
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/DataSource.h b/third_party/allwpilib/glass/src/lib/native/include/glass/DataSource.h
new file mode 100644
index 0000000..1d5c37b
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/DataSource.h
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <atomic>
+#include <string>
+#include <string_view>
+
+#include <imgui.h>
+#include <wpi/Signal.h>
+#include <wpi/spinlock.h>
+
+namespace glass {
+
+class NameInfo;
+
+/**
+ * A data source for numeric/boolean data.
+ */
+class DataSource {
+ public:
+  explicit DataSource(std::string_view id);
+  DataSource(std::string_view id, int index);
+  DataSource(std::string_view id, int index, int index2);
+  virtual ~DataSource();
+
+  DataSource(const DataSource&) = delete;
+  DataSource& operator=(const DataSource&) = delete;
+
+  const char* GetId() const { return m_id.c_str(); }
+
+  void SetName(std::string_view name);
+  const char* GetName() const;
+  NameInfo& GetNameInfo() { return *m_name; }
+
+  void PushEditNameId(int index);
+  void PushEditNameId(const char* name);
+  bool PopupEditName(int index);
+  bool PopupEditName(const char* name);
+  bool InputTextName(const char* label_id, ImGuiInputTextFlags flags = 0);
+
+  void SetDigital(bool digital) { m_digital = digital; }
+  bool IsDigital() const { return m_digital; }
+
+  void SetValue(double value, uint64_t time = 0) {
+    m_value = value;
+    valueChanged(value, time);
+  }
+  double GetValue() const { return m_value; }
+
+  // drag source helpers
+  void LabelText(const char* label, const char* fmt, ...) const;
+  void LabelTextV(const char* label, const char* fmt, va_list args) const;
+  bool Combo(const char* label, int* current_item, const char* const items[],
+             int items_count, int popup_max_height_in_items = -1) const;
+  bool SliderFloat(const char* label, float* v, float v_min, float v_max,
+                   const char* format = "%.3f", float power = 1.0f) const;
+  bool InputDouble(const char* label, double* v, double step = 0.0,
+                   double step_fast = 0.0, const char* format = "%.6f",
+                   ImGuiInputTextFlags flags = 0) const;
+  bool InputInt(const char* label, int* v, int step = 1, int step_fast = 100,
+                ImGuiInputTextFlags flags = 0) const;
+  void EmitDrag(ImGuiDragDropFlags flags = 0) const;
+
+  wpi::sig::SignalBase<wpi::spinlock, double, uint64_t> valueChanged;
+
+  static DataSource* Find(std::string_view id);
+
+  static wpi::sig::Signal<const char*, DataSource*> sourceCreated;
+
+ private:
+  std::string m_id;
+  NameInfo* m_name;
+  bool m_digital = false;
+  std::atomic<double> m_value = 0;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/MainMenuBar.h b/third_party/allwpilib/glass/src/lib/native/include/glass/MainMenuBar.h
new file mode 100644
index 0000000..7a6a2fc
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/MainMenuBar.h
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+namespace glass {
+
+class WindowManager;
+
+/**
+ * GUI main menu bar.
+ */
+class MainMenuBar {
+ public:
+  /**
+   * Displays the main menu bar.  Should be added to GUI LateExecute.
+   */
+  void Display();
+
+  /**
+   * Adds to GUI's main menu bar.  The menu function is called from within a
+   * ImGui::BeginMainMenuBar()/EndMainMenuBar() block.  Usually it's only
+   * appropriate to create a menu with ImGui::BeginMenu()/EndMenu() inside of
+   * this function.
+   *
+   * @param menu menu display function
+   */
+  void AddMainMenu(std::function<void()> menu);
+
+  /**
+   * Adds to GUI's option menu.  The menu function is called from within a
+   * ImGui::BeginMenu()/EndMenu() block.  Usually it's only appropriate to
+   * create menu items inside of this function.
+   *
+   * @param menu menu display function
+   */
+  void AddOptionMenu(std::function<void()> menu);
+
+ private:
+  std::vector<std::function<void()>> m_optionMenus;
+  std::vector<std::function<void()>> m_menus;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/Model.h b/third_party/allwpilib/glass/src/lib/native/include/glass/Model.h
new file mode 100644
index 0000000..28f546f
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/Model.h
@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace glass {
+
+class Model {
+ public:
+  Model() = default;
+  virtual ~Model() = default;
+
+  Model(const Model&) = delete;
+  Model& operator=(const Model&) = delete;
+
+  virtual void Update() = 0;
+  virtual bool Exists() = 0;
+  virtual bool IsReadOnly();
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/Provider.h b/third_party/allwpilib/glass/src/lib/native/include/glass/Provider.h
new file mode 100644
index 0000000..53b1e75
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/Provider.h
@@ -0,0 +1,167 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <wpigui.h>
+
+#include "glass/Model.h"
+#include "glass/WindowManager.h"
+
+namespace glass {
+
+namespace detail {
+struct ProviderFunctions {
+  using Exists = std::function<bool()>;
+  using CreateModel = std::function<std::unique_ptr<Model>()>;
+  using ViewExists = std::function<bool(Model*)>;
+  using CreateView = std::function<std::unique_ptr<View>(Window*, Model*)>;
+};
+}  // namespace detail
+
+/**
+ * Providers are registries of models and views.  They have ownership over
+ * their created Models, Windows, and Views.
+ *
+ * GlobalInit() configures Update() to be called during EarlyExecute.
+ * Calling Update() calls Update() on all created models (Provider
+ * implementations must ensure this occurs).
+ *
+ * @tparam Functions defines functor interface types
+ */
+template <typename Functions = detail::ProviderFunctions>
+class Provider : public WindowManager {
+ public:
+  using ExistsFunc = typename Functions::Exists;
+  using CreateModelFunc = typename Functions::CreateModel;
+  using ViewExistsFunc = typename Functions::ViewExists;
+  using CreateViewFunc = typename Functions::CreateView;
+
+  /**
+   * Constructor.
+   *
+   * @param iniName Group name to use in ini file
+   */
+  explicit Provider(std::string_view iniName) : WindowManager{iniName} {}
+
+  Provider(const Provider&) = delete;
+  Provider& operator=(const Provider&) = delete;
+
+  /**
+   * Perform global initialization.  This should be called prior to
+   * wpi::gui::Initialize().
+   */
+  void GlobalInit() override;
+
+  /**
+   * Show the specified view by default on first load.  Has no effect if
+   * the user previously hid the window (e.g. in a saved prior execution).
+   *
+   * @param name View name
+   */
+  void ShowDefault(std::string_view name);
+
+  /**
+   * Register a model and view combination.  Equivalent to calling both
+   * RegisterModel() and RegisterView() with no ViewExistsFunc.
+   *
+   * @param name View/model name
+   * @param exists Functor, returns true if model can be created
+   * @param createModel Functor for creating model
+   * @param createView Functor for creating view
+   */
+  void Register(std::string_view name, ExistsFunc exists,
+                CreateModelFunc createModel, CreateViewFunc createView);
+
+  /**
+   * Register a model.
+   *
+   * @param name Model name
+   * @param exists Functor, returns true if model can be created
+   * @param createModel Functor for creating model
+   */
+  void RegisterModel(std::string_view name, ExistsFunc exists,
+                     CreateModelFunc createModel);
+
+  /**
+   * Register a view.
+   *
+   * @param name View name
+   * @param modelName Model name
+   * @param exists Functor, returns true if view can be created
+   * @param createView Functor for creating view
+   */
+  void RegisterView(std::string_view name, std::string_view modelName,
+                    ViewExistsFunc exists, CreateViewFunc createView);
+
+ protected:
+  virtual void Update();
+
+  struct ModelEntry {
+    ModelEntry(std::string_view name, ExistsFunc exists,
+               CreateModelFunc createModel)
+        : name{name},
+          exists{std::move(exists)},
+          createModel{std::move(createModel)} {}
+    virtual ~ModelEntry() = default;
+
+    std::string name;
+    ExistsFunc exists;
+    CreateModelFunc createModel;
+    std::unique_ptr<Model> model;
+  };
+
+  struct ViewEntry {
+    ViewEntry(std::string_view name, ModelEntry* modelEntry,
+              ViewExistsFunc exists, CreateViewFunc createView)
+        : name{name},
+          modelEntry{modelEntry},
+          exists{std::move(exists)},
+          createView{std::move(createView)} {}
+    virtual ~ViewEntry() = default;
+
+    std::string name;
+    ModelEntry* modelEntry;
+    ViewExistsFunc exists;
+    CreateViewFunc createView;
+    Window* window = nullptr;
+  };
+
+  // sorted by name
+  using ModelEntries = std::vector<std::unique_ptr<ModelEntry>>;
+  ModelEntries m_modelEntries;
+  using ViewEntries = std::vector<std::unique_ptr<ViewEntry>>;
+  ViewEntries m_viewEntries;
+
+  typename ModelEntries::iterator FindModelEntry(std::string_view name);
+  typename ViewEntries::iterator FindViewEntry(std::string_view name);
+
+  virtual std::unique_ptr<ModelEntry> MakeModelEntry(
+      std::string_view name, ExistsFunc exists, CreateModelFunc createModel) {
+    return std::make_unique<ModelEntry>(name, std::move(exists),
+                                        std::move(createModel));
+  }
+
+  virtual std::unique_ptr<ViewEntry> MakeViewEntry(std::string_view name,
+                                                   ModelEntry* modelEntry,
+                                                   ViewExistsFunc exists,
+                                                   CreateViewFunc createView) {
+    return std::make_unique<ViewEntry>(name, modelEntry, std::move(exists),
+                                       std::move(createView));
+  }
+
+  virtual void Show(ViewEntry* entry, Window* window) = 0;
+};
+
+}  // namespace glass
+
+#include "Provider.inc"
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/Provider.inc b/third_party/allwpilib/glass/src/lib/native/include/glass/Provider.inc
new file mode 100644
index 0000000..33bb6e0
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/Provider.inc
@@ -0,0 +1,102 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <utility>
+
+#include "glass/Provider.h"
+
+namespace glass {
+
+template <typename Functions>
+void Provider<Functions>::GlobalInit() {
+  WindowManager::GlobalInit();
+  wpi::gui::AddEarlyExecute([this] { Update(); });
+}
+
+template <typename Functions>
+void Provider<Functions>::ShowDefault(std::string_view name) {
+  auto win = GetWindow(name);
+  if (win) {
+    return;
+  }
+  auto it = FindViewEntry(name);
+  if (it == m_viewEntries.end() || (*it)->name != name) {
+    return;
+  }
+  this->Show(it->get(), (*it)->window);
+}
+
+template <typename Functions>
+void Provider<Functions>::Register(std::string_view name, ExistsFunc exists,
+                                   CreateModelFunc createModel,
+                                   CreateViewFunc createView) {
+  RegisterModel(name, std::move(exists), std::move(createModel));
+  RegisterView(name, name, nullptr, std::move(createView));
+}
+
+template <typename Functions>
+void Provider<Functions>::RegisterModel(std::string_view name,
+                                        ExistsFunc exists,
+                                        CreateModelFunc createModel) {
+  auto it = FindModelEntry(name);
+  // ignore if exists
+  if (it != m_modelEntries.end() && (*it)->name == name) {
+    return;
+  }
+  // insert in sorted location
+  m_modelEntries.emplace(
+      it, MakeModelEntry(name, std::move(exists), std::move(createModel)));
+}
+
+template <typename Functions>
+void Provider<Functions>::RegisterView(std::string_view name,
+                                       std::string_view modelName,
+                                       ViewExistsFunc exists,
+                                       CreateViewFunc createView) {
+  // find model; if model doesn't exist, ignore
+  auto modelIt = FindModelEntry(modelName);
+  if (modelIt == m_modelEntries.end() || (*modelIt)->name != modelName) {
+    return;
+  }
+
+  auto viewIt = FindViewEntry(name);
+  // ignore if exists
+  if (viewIt != m_viewEntries.end() && (*viewIt)->name == name) {
+    return;
+  }
+  // insert in sorted location
+  m_viewEntries.emplace(viewIt,
+                        MakeViewEntry(name, modelIt->get(), std::move(exists),
+                                      std::move(createView)));
+}
+
+template <typename Functions>
+void Provider<Functions>::Update() {
+  // update entries
+  for (auto&& entry : m_modelEntries) {
+    if (entry->model) {
+      entry->model->Update();
+    }
+  }
+}
+
+template <typename Functions>
+typename Provider<Functions>::ModelEntries::iterator
+Provider<Functions>::FindModelEntry(std::string_view name) {
+  return std::lower_bound(
+      m_modelEntries.begin(), m_modelEntries.end(), name,
+      [](const auto& elem, std::string_view s) { return elem->name < s; });
+}
+
+template <typename Functions>
+typename Provider<Functions>::ViewEntries::iterator
+Provider<Functions>::FindViewEntry(std::string_view name) {
+  return std::lower_bound(
+      m_viewEntries.begin(), m_viewEntries.end(), name,
+      [](const auto& elem, std::string_view s) { return elem->name < s; });
+}
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/View.h b/third_party/allwpilib/glass/src/lib/native/include/glass/View.h
new file mode 100644
index 0000000..886c29e
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/View.h
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <wpi/FunctionExtras.h>
+
+namespace glass {
+
+/**
+ * A view is the contents of a window (1:1 mapping).
+ * It may reference multiple models.
+ *
+ * Typically a view is constructed by a Provider and the View's constructor
+ * is given the corresponding Model(s).
+ *
+ * A view may retain a reference to its parent window for dynamic
+ * window configuration.
+ */
+class View {
+ public:
+  virtual ~View() = default;
+
+  /**
+   * Displays the window contents.  Called by Window::Display() from within an
+   * ImGui::Begin() / ImGui::End() block.
+   */
+  virtual void Display() = 0;
+
+  /**
+   * Called instead of Display() when the window is hidden (e.g. when
+   * ImGui::Begin() returns false).
+   */
+  virtual void Hidden();
+};
+
+/**
+ * Make a View for a display functor.
+ *
+ * @param display Display function
+ * @return unique_ptr to View
+ */
+std::unique_ptr<View> MakeFunctionView(wpi::unique_function<void()> display);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/Window.h b/third_party/allwpilib/glass/src/lib/native/include/glass/Window.h
new file mode 100644
index 0000000..780479a
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/Window.h
@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+
+#include <imgui.h>
+
+#include "glass/View.h"
+
+namespace glass {
+
+/**
+ * Managed window information.
+ * A Window owns the View that displays the window's contents.
+ */
+class Window {
+ public:
+  Window() = default;
+  explicit Window(std::string_view id) : m_id{id}, m_defaultName{id} {}
+
+  std::string_view GetId() const { return m_id; }
+
+  enum Visibility { kHide = 0, kShow, kDisabled };
+
+  bool HasView() { return static_cast<bool>(m_view); }
+
+  void SetView(std::unique_ptr<View> view) { m_view = std::move(view); }
+
+  View* GetView() { return m_view.get(); }
+  const View* GetView() const { return m_view.get(); }
+
+  bool IsVisible() const { return m_visible; }
+  void SetVisible(bool visible) { m_visible = visible; }
+  bool IsEnabled() const { return m_enabled; }
+  void SetEnabled(bool enabled) { m_enabled = enabled; }
+
+  void SetFlags(ImGuiWindowFlags flags) { m_flags = flags; }
+
+  void SetName(std::string_view name) { m_name = name; }
+  void SetDefaultName(std::string_view name) { m_defaultName = name; }
+
+  /**
+   * Normally windows provide a right-click popup menu on the title bar to
+   * rename the window.  Calling this disables that functionality so the
+   * view can provide its own popup.
+   */
+  void DisableRenamePopup() { m_renamePopupEnabled = false; }
+
+  /**
+   * Sets visibility of window.
+   *
+   * @param visibility 0=hide, 1=show, 2=disabled (force-hide)
+   */
+  void SetVisibility(Visibility visibility);
+
+  /**
+   * Sets default position of window.
+   *
+   * @param x x location of upper left corner
+   * @param y y location of upper left corner
+   */
+  void SetDefaultPos(float x, float y) {
+    m_posCond = ImGuiCond_FirstUseEver;
+    m_pos = ImVec2{x, y};
+  }
+
+  /**
+   * Sets default size of window.
+   *
+   * @param width width
+   * @param height height
+   */
+  void SetDefaultSize(float width, float height) {
+    m_sizeCond = ImGuiCond_FirstUseEver;
+    m_size = ImVec2{width, height};
+  }
+
+  /**
+   * Sets internal padding of window.
+   * @param x horizontal padding
+   * @param y vertical padding
+   */
+  void SetPadding(float x, float y) {
+    m_setPadding = true;
+    m_padding = ImVec2{x, y};
+  }
+
+  /**
+   * Displays window.
+   */
+  void Display();
+
+  /**
+   * Displays menu item for the window.
+   * @param label what to display as the menu item label; defaults to
+   *              window ID if nullptr
+   * @return True if window went from invisible to visible.
+   */
+  bool DisplayMenuItem(const char* label = nullptr);
+
+  /**
+   * Scale default window position and size.
+   */
+  void ScaleDefault(float scale);
+
+  void IniReadLine(const char* lineStr);
+  void IniWriteAll(const char* typeName, ImGuiTextBuffer* out_buf);
+
+ private:
+  std::string m_id;
+  std::string m_name;
+  std::string m_defaultName;
+  std::unique_ptr<View> m_view;
+  ImGuiWindowFlags m_flags = 0;
+  bool m_visible = true;
+  bool m_enabled = true;
+  bool m_renamePopupEnabled = true;
+  ImGuiCond m_posCond = 0;
+  ImGuiCond m_sizeCond = 0;
+  ImVec2 m_pos;
+  ImVec2 m_size;
+  bool m_setPadding = false;
+  ImVec2 m_padding;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/WindowManager.h b/third_party/allwpilib/glass/src/lib/native/include/glass/WindowManager.h
new file mode 100644
index 0000000..4024e15
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/WindowManager.h
@@ -0,0 +1,137 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string_view>
+#include <type_traits>
+#include <vector>
+
+#include <imgui.h>
+#include <wpi/FunctionExtras.h>
+
+#include "glass/Window.h"
+#include "glass/support/IniSaverBase.h"
+
+namespace glass {
+
+/**
+ * Window manager.
+ *
+ * To properly integrate into an application:
+ * - Call GlobalInit() from the application main, after calling
+ *   wpi::gui::CreateContext(), but before calling wpi::gui::Initialize().
+ * - Add DisplayMenu() to the application's MainMenuBar.
+ */
+class WindowManager {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param iniName Group name to use in ini file
+   */
+  explicit WindowManager(std::string_view iniName);
+  virtual ~WindowManager() = default;
+
+  WindowManager(const WindowManager&) = delete;
+  WindowManager& operator=(const WindowManager&) = delete;
+
+  /**
+   * Perform global initialization.  This should be called prior to
+   * wpi::gui::Initialize().
+   */
+  virtual void GlobalInit();
+
+  /**
+   * Displays menu contents, one item for each window.
+   * See Window::DisplayMenuItem().
+   */
+  virtual void DisplayMenu();
+
+  /**
+   * Adds window to GUI.  The display function is called from within a
+   * ImGui::Begin()/End() block.  While windows can be created within the
+   * execute function passed to gui::AddExecute(), using this function ensures
+   * the windows are consistently integrated with the rest of the GUI.
+   *
+   * On each Dear ImGui frame, gui::AddExecute() functions are always called
+   * prior to AddWindow display functions.  Note that windows may be shaded or
+   * completely hidden, in which case this function will not be called.
+   * It's important to perform any processing steps that must be performed
+   * every frame in the gui::AddExecute() function.
+   *
+   * @param id unique identifier of the window (title bar)
+   * @param display window contents display function
+   */
+  Window* AddWindow(std::string_view id, wpi::unique_function<void()> display);
+
+  /**
+   * Adds window to GUI.  The view's display function is called from within a
+   * ImGui::Begin()/End() block.  While windows can be created within the
+   * execute function passed to gui::AddExecute(), using this function ensures
+   * the windows are consistently integrated with the rest of the GUI.
+   *
+   * On each Dear ImGui frame, gui::AddExecute() functions are always called
+   * prior to AddWindow display functions.  Note that windows may be shaded or
+   * completely hidden, in which case this function will not be called.
+   * It's important to perform any processing steps that must be performed
+   * every frame in the gui::AddExecute() function.
+   *
+   * @param id unique identifier of the window (title bar)
+   * @param view view object
+   * @return Window, or nullptr on duplicate window
+   */
+  Window* AddWindow(std::string_view id, std::unique_ptr<View> view);
+
+  /**
+   * Adds window to GUI.  A View must be assigned to the returned Window
+   * to display the window contents.  While windows can be created within the
+   * execute function passed to gui::AddExecute(), using this function ensures
+   * the windows are consistently integrated with the rest of the GUI.
+   *
+   * On each Dear ImGui frame, gui::AddExecute() functions are always called
+   * prior to AddWindow display functions.  Note that windows may be shaded or
+   * completely hidden, in which case this function will not be called.
+   * It's important to perform any processing steps that must be performed
+   * every frame in the gui::AddExecute() function.
+   *
+   * @param id unique identifier of the window (default title bar)
+   * @return Window, or nullptr on duplicate window
+   */
+  Window* GetOrAddWindow(std::string_view id, bool duplicateOk = false);
+
+  /**
+   * Gets existing window.  If none exists, returns nullptr.
+   *
+   * @param id unique identifier of the window (default title bar)
+   * @return Window, or nullptr if window does not exist
+   */
+  Window* GetWindow(std::string_view id);
+
+ protected:
+  virtual void DisplayWindows();
+
+  // kept sorted by id
+  std::vector<std::unique_ptr<Window>> m_windows;
+
+ private:
+  class IniSaver : public IniSaverBase {
+   public:
+    explicit IniSaver(std::string_view typeName, WindowManager* manager)
+        : IniSaverBase{typeName}, m_manager{manager} {}
+
+    void* IniReadOpen(const char* name) override;
+    void IniReadLine(void* entry, const char* lineStr) override;
+    void IniWriteAll(ImGuiTextBuffer* out_buf) override;
+
+   private:
+    WindowManager* m_manager;
+  };
+
+  IniSaver m_iniSaver;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Accelerometer.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Accelerometer.h
new file mode 100644
index 0000000..e997963
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Accelerometer.h
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class AccelerometerModel : public Model {
+ public:
+  virtual DataSource* GetXData() = 0;
+  virtual DataSource* GetYData() = 0;
+  virtual DataSource* GetZData() = 0;
+
+  virtual int GetRange() = 0;
+
+  virtual void SetX(double val) = 0;
+  virtual void SetY(double val) = 0;
+  virtual void SetZ(double val) = 0;
+  virtual void SetRange(int val) = 0;
+};
+
+void DisplayAccelerometerDevice(AccelerometerModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogGyro.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogGyro.h
new file mode 100644
index 0000000..fdfcf4f
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogGyro.h
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class AnalogGyroModel : public Model {
+ public:
+  virtual DataSource* GetAngleData() = 0;
+  virtual DataSource* GetRateData() = 0;
+
+  virtual void SetAngle(double val) = 0;
+  virtual void SetRate(double val) = 0;
+};
+
+class AnalogGyrosModel : public Model {
+ public:
+  virtual void ForEachAnalogGyro(
+      wpi::function_ref<void(AnalogGyroModel& model, int index)> func) = 0;
+};
+
+void DisplayAnalogGyroDevice(AnalogGyroModel* model, int index);
+void DisplayAnalogGyrosDevice(AnalogGyrosModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogInput.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogInput.h
new file mode 100644
index 0000000..2e49b9c
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogInput.h
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class AnalogInputModel : public Model {
+ public:
+  virtual bool IsGyro() const = 0;
+  virtual const char* GetSimDevice() const = 0;
+
+  virtual DataSource* GetVoltageData() = 0;
+
+  virtual void SetVoltage(double val) = 0;
+};
+
+class AnalogInputsModel : public Model {
+ public:
+  virtual void ForEachAnalogInput(
+      wpi::function_ref<void(AnalogInputModel& model, int index)> func) = 0;
+};
+
+void DisplayAnalogInput(AnalogInputModel* model, int index);
+void DisplayAnalogInputs(AnalogInputsModel* model,
+                         std::string_view noneMsg = "No analog inputs");
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogOutput.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogOutput.h
new file mode 100644
index 0000000..7ba8be6
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/AnalogOutput.h
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class AnalogOutputModel : public Model {
+ public:
+  virtual DataSource* GetVoltageData() = 0;
+
+  virtual void SetVoltage(double val) = 0;
+};
+
+class AnalogOutputsModel : public Model {
+ public:
+  virtual void ForEachAnalogOutput(
+      wpi::function_ref<void(AnalogOutputModel& model, int index)> func) = 0;
+};
+
+void DisplayAnalogOutputsDevice(AnalogOutputsModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/DIO.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/DIO.h
new file mode 100644
index 0000000..5593ba6
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/DIO.h
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class EncoderModel;
+class DataSource;
+
+class DPWMModel : public Model {
+ public:
+  virtual const char* GetSimDevice() const = 0;
+
+  virtual DataSource* GetValueData() = 0;
+
+  virtual void SetValue(double val) = 0;
+};
+
+class DutyCycleModel : public Model {
+ public:
+  virtual const char* GetSimDevice() const = 0;
+
+  virtual DataSource* GetValueData() = 0;
+
+  virtual void SetValue(double val) = 0;
+};
+
+class DIOModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+
+  virtual const char* GetSimDevice() const = 0;
+
+  virtual DPWMModel* GetDPWM() = 0;
+  virtual DutyCycleModel* GetDutyCycle() = 0;
+  virtual EncoderModel* GetEncoder() = 0;
+
+  virtual bool IsInput() const = 0;
+
+  virtual DataSource* GetValueData() = 0;
+
+  virtual void SetValue(bool val) = 0;
+};
+
+class DIOsModel : public Model {
+ public:
+  virtual void ForEachDIO(
+      wpi::function_ref<void(DIOModel& model, int index)> func) = 0;
+};
+
+void DisplayDIO(DIOModel* model, int index, bool outputsEnabled);
+void DisplayDIOs(DIOsModel* model, bool outputsEnabled,
+                 std::string_view noneMsg = "No Digital I/O");
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Encoder.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Encoder.h
new file mode 100644
index 0000000..41d781a
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Encoder.h
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class EncoderModel : public Model {
+ public:
+  virtual void SetName(std::string_view name);
+
+  virtual const char* GetSimDevice() const = 0;
+
+  virtual int GetChannelA() const = 0;
+  virtual int GetChannelB() const = 0;
+
+  virtual DataSource* GetDistancePerPulseData() = 0;
+  virtual DataSource* GetCountData() = 0;
+  virtual DataSource* GetPeriodData() = 0;
+  virtual DataSource* GetDirectionData() = 0;
+  virtual DataSource* GetDistanceData() = 0;
+  virtual DataSource* GetRateData() = 0;
+
+  virtual double GetMaxPeriod() = 0;
+  virtual bool GetReverseDirection() = 0;
+
+  virtual void SetDistancePerPulse(double val) = 0;
+  virtual void SetCount(int val) = 0;
+  virtual void SetPeriod(double val) = 0;
+  virtual void SetDirection(bool val) = 0;
+  virtual void SetDistance(double val) = 0;
+  virtual void SetRate(double val) = 0;
+
+  virtual void SetMaxPeriod(double val) = 0;
+  virtual void SetReverseDirection(bool val) = 0;
+};
+
+class EncodersModel : public Model {
+ public:
+  virtual void ForEachEncoder(
+      wpi::function_ref<void(EncoderModel& model, int index)> func) = 0;
+};
+
+void DisplayEncoder(EncoderModel* model);
+void DisplayEncoders(EncodersModel* model,
+                     std::string_view noneMsg = "No encoders");
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Gyro.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Gyro.h
new file mode 100644
index 0000000..d2bb09d
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Gyro.h
@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class GyroModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual const char* GetSimDevice() const = 0;
+
+  virtual DataSource* GetAngleData() = 0;
+  virtual void SetAngle(double angle) = 0;
+};
+void DisplayGyro(GyroModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/LEDDisplay.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/LEDDisplay.h
new file mode 100644
index 0000000..ddd3c27
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/LEDDisplay.h
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/function_ref.h>
+#include <wpi/span.h>
+
+#include "glass/Model.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace glass {
+
+class LEDDisplayModel : public glass::Model {
+ public:
+  struct Data {
+    uint8_t b;
+    uint8_t g;
+    uint8_t r;
+    uint8_t padding;
+  };
+
+  virtual bool IsRunning() = 0;
+
+  virtual wpi::span<const Data> GetData(wpi::SmallVectorImpl<Data>& buf) = 0;
+};
+
+class LEDDisplaysModel : public glass::Model {
+ public:
+  virtual size_t GetNumLEDDisplays() = 0;
+
+  virtual void ForEachLEDDisplay(
+      wpi::function_ref<void(LEDDisplayModel& model, int index)> func) = 0;
+};
+
+void DisplayLEDDisplay(LEDDisplayModel* model, int index);
+void DisplayLEDDisplays(LEDDisplaysModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PCM.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PCM.h
new file mode 100644
index 0000000..107a2a8
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PCM.h
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class CompressorModel : public Model {
+ public:
+  virtual DataSource* GetRunningData() = 0;
+  virtual DataSource* GetEnabledData() = 0;
+  virtual DataSource* GetPressureSwitchData() = 0;
+  virtual DataSource* GetCurrentData() = 0;
+
+  virtual void SetRunning(bool val) = 0;
+  virtual void SetEnabled(bool val) = 0;
+  virtual void SetPressureSwitch(bool val) = 0;
+  virtual void SetCurrent(double val) = 0;
+};
+
+class SolenoidModel : public Model {
+ public:
+  virtual DataSource* GetOutputData() = 0;
+
+  virtual void SetOutput(bool val) = 0;
+};
+
+class PCMModel : public Model {
+ public:
+  virtual CompressorModel* GetCompressor() = 0;
+
+  virtual void ForEachSolenoid(
+      wpi::function_ref<void(SolenoidModel& model, int index)> func) = 0;
+};
+
+class PCMsModel : public Model {
+ public:
+  virtual void ForEachPCM(
+      wpi::function_ref<void(PCMModel& model, int index)> func) = 0;
+};
+
+bool DisplayPCMSolenoids(PCMModel* model, int index, bool outputsEnabled);
+void DisplayPCMsSolenoids(PCMsModel* model, bool outputsEnabled,
+                          std::string_view noneMsg = "No solenoids");
+
+void DisplayCompressorDevice(PCMModel* model, int index, bool outputsEnabled);
+void DisplayCompressorDevice(CompressorModel* model, int index,
+                             bool outputsEnabled);
+void DisplayCompressorsDevice(PCMsModel* model, bool outputsEnabled);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PWM.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PWM.h
new file mode 100644
index 0000000..74c7461
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PWM.h
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class PWMModel : public Model {
+ public:
+  // returns -1 if not an addressable LED
+  virtual int GetAddressableLED() const = 0;
+
+  virtual DataSource* GetSpeedData() = 0;
+
+  virtual void SetSpeed(double val) = 0;
+};
+
+class PWMsModel : public Model {
+ public:
+  virtual void ForEachPWM(
+      wpi::function_ref<void(PWMModel& model, int index)> func) = 0;
+};
+
+void DisplayPWM(PWMModel* model, int index, bool outputsEnabled);
+void DisplayPWMs(PWMsModel* model, bool outputsEnabled,
+                 std::string_view noneMsg = "No PWM outputs");
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PowerDistribution.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PowerDistribution.h
new file mode 100644
index 0000000..0ef6006
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/PowerDistribution.h
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class PowerDistributionModel : public Model {
+ public:
+  virtual int GetNumChannels() const = 0;
+
+  virtual DataSource* GetTemperatureData() = 0;
+  virtual DataSource* GetVoltageData() = 0;
+  virtual DataSource* GetCurrentData(int channel) = 0;
+
+  virtual void SetTemperature(double val) = 0;
+  virtual void SetVoltage(double val) = 0;
+  virtual void SetCurrent(int channel, double val) = 0;
+};
+
+class PowerDistributionsModel : public Model {
+ public:
+  virtual void ForEachPowerDistribution(
+      wpi::function_ref<void(PowerDistributionModel& model, int index)>
+          func) = 0;
+};
+
+void DisplayPowerDistribution(PowerDistributionModel* model, int index);
+void DisplayPowerDistributions(
+    PowerDistributionsModel* model,
+    std::string_view noneMsg = "No Power Distributions");
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Relay.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Relay.h
new file mode 100644
index 0000000..b025119
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/Relay.h
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class RelayModel : public Model {
+ public:
+  virtual DataSource* GetForwardData() = 0;
+  virtual DataSource* GetReverseData() = 0;
+
+  virtual void SetForward(bool val) = 0;
+  virtual void SetReverse(bool val) = 0;
+};
+
+class RelaysModel : public Model {
+ public:
+  virtual void ForEachRelay(
+      wpi::function_ref<void(RelayModel& model, int index)> func) = 0;
+};
+
+void DisplayRelay(RelayModel* model, int index, bool outputsEnabled);
+void DisplayRelays(RelaysModel* model, bool outputsEnabled,
+                   std::string_view noneMsg = "No relays");
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/RoboRio.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/RoboRio.h
new file mode 100644
index 0000000..df9a2a5
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/RoboRio.h
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+class RoboRioRailModel : public Model {
+ public:
+  virtual DataSource* GetVoltageData() = 0;
+  virtual DataSource* GetCurrentData() = 0;
+  virtual DataSource* GetActiveData() = 0;
+  virtual DataSource* GetFaultsData() = 0;
+
+  virtual void SetVoltage(double val) = 0;
+  virtual void SetCurrent(double val) = 0;
+  virtual void SetActive(bool val) = 0;
+  virtual void SetFaults(int val) = 0;
+};
+
+class RoboRioModel : public Model {
+ public:
+  virtual RoboRioRailModel* GetUser6VRail() = 0;
+  virtual RoboRioRailModel* GetUser5VRail() = 0;
+  virtual RoboRioRailModel* GetUser3V3Rail() = 0;
+
+  virtual DataSource* GetUserButton() = 0;
+  virtual DataSource* GetVInVoltageData() = 0;
+  virtual DataSource* GetVInCurrentData() = 0;
+  virtual DataSource* GetBrownoutVoltage() = 0;
+
+  virtual void SetUserButton(bool val) = 0;
+  virtual void SetVInVoltage(double val) = 0;
+  virtual void SetVInCurrent(double val) = 0;
+  virtual void SetBrownoutVoltage(double val) = 0;
+};
+
+void DisplayRoboRio(RoboRioModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/SpeedController.h b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/SpeedController.h
new file mode 100644
index 0000000..033f27d
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/hardware/SpeedController.h
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class SpeedControllerModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual const char* GetSimDevice() const = 0;
+  virtual DataSource* GetPercentData() = 0;
+  virtual void SetPercent(double value) = 0;
+};
+void DisplaySpeedController(SpeedControllerModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/CommandScheduler.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/CommandScheduler.h
new file mode 100644
index 0000000..66b92b2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/CommandScheduler.h
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class CommandSchedulerModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual const std::vector<std::string>& GetCurrentCommands() = 0;
+  virtual void CancelCommand(size_t index) = 0;
+};
+void DisplayCommandScheduler(CommandSchedulerModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/CommandSelector.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/CommandSelector.h
new file mode 100644
index 0000000..e5126f2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/CommandSelector.h
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class CommandSelectorModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual DataSource* GetRunningData() = 0;
+  virtual void SetRunning(bool run) = 0;
+};
+void DisplayCommandSelector(CommandSelectorModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/DeviceTree.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/DeviceTree.h
new file mode 100644
index 0000000..ddd27f2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/DeviceTree.h
@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <imgui.h>
+#include <wpi/FunctionExtras.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class DataSource;
+
+/**
+ * Model for device tree.
+ */
+class DeviceTreeModel : public Model {
+ public:
+  using DisplayFunc = wpi::unique_function<void(Model*)>;
+
+  /**
+   * Add a display to the device tree.
+   *
+   * @param model Model to keep updated (may be nullptr)
+   * @param display Display function
+   */
+  void Add(std::unique_ptr<Model> model, DisplayFunc display) {
+    m_displays.emplace_back(model.get(), std::move(display));
+    m_ownedModels.emplace_back(std::move(model));
+  }
+
+  void Add(Model* model, DisplayFunc display) {
+    m_displays.emplace_back(model, std::move(display));
+  }
+
+  void Update() override;
+
+  bool Exists() override;
+
+  void Display();
+
+ private:
+  std::vector<std::pair<Model*, DisplayFunc>> m_displays;
+  std::vector<std::unique_ptr<Model>> m_ownedModels;
+};
+
+/**
+ * Hides device on tree.
+ *
+ * @param id device name
+ */
+void HideDevice(const char* id);
+
+/**
+ * Wraps CollapsingHeader() to provide both hiding functionality and open
+ * persistence.  As with the ImGui function, returns true if the tree node
+ * is visible and expanded.  If returns true, call EndDevice() to finish
+ * the block.
+ *
+ * @param id label
+ * @param flags ImGuiTreeNodeFlags flags
+ * @return True if expanded
+ */
+bool BeginDevice(const char* id, ImGuiTreeNodeFlags flags = 0);
+
+/**
+ * Finish a device block started with BeginDevice().
+ */
+void EndDevice();
+
+/**
+ * Displays device value.
+ *
+ * @param name value name
+ * @param readonly prevent value from being modified by the user
+ * @param value value contents (modified in place)
+ * @param source data source for drag source (may be nullptr)
+ * @return True if value was modified by the user
+ */
+bool DeviceBoolean(const char* name, bool readonly, bool* value,
+                   const DataSource* source = nullptr);
+
+/**
+ * Displays device value.
+ *
+ * @param name value name
+ * @param readonly prevent value from being modified by the user
+ * @param value value contents (modified in place)
+ * @param source data source for drag source (may be nullptr)
+ * @return True if value was modified by the user
+ */
+bool DeviceDouble(const char* name, bool readonly, double* value,
+                  const DataSource* source = nullptr);
+
+/**
+ * Displays device value.
+ *
+ * @param name value name
+ * @param readonly prevent value from being modified by the user
+ * @param value value contents (modified in place)
+ * @param options options array
+ * @param numOptions size of options array
+ * @param source data source for drag source (may be nullptr)
+ * @return True if value was modified by the user
+ */
+bool DeviceEnum(const char* name, bool readonly, int* value,
+                const char** options, int32_t numOptions,
+                const DataSource* source = nullptr);
+
+/**
+ * Displays device value.
+ *
+ * @param name value name
+ * @param readonly prevent value from being modified by the user
+ * @param value value contents (modified in place)
+ * @param source data source for drag source (may be nullptr)
+ * @return True if value was modified by the user
+ */
+bool DeviceInt(const char* name, bool readonly, int32_t* value,
+               const DataSource* source = nullptr);
+
+/**
+ * Displays device value.
+ *
+ * @param name value name
+ * @param readonly prevent value from being modified by the user
+ * @param value value contents (modified in place)
+ * @param source data source for drag source (may be nullptr)
+ * @return True if value was modified by the user
+ */
+bool DeviceLong(const char* name, bool readonly, int64_t* value,
+                const DataSource* source = nullptr);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/Drive.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Drive.h
new file mode 100644
index 0000000..c969582
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Drive.h
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "glass/Model.h"
+
+struct ImVec2;
+
+namespace glass {
+class DataSource;
+class DriveModel : public Model {
+ public:
+  struct WheelInfo {
+    std::string name;
+    DataSource* percent;
+    std::function<void(double)> setter;
+
+    WheelInfo(std::string_view name, DataSource* percent,
+              std::function<void(double)> setter)
+        : name(name), percent(percent), setter(std::move(setter)) {}
+  };
+
+  virtual const char* GetName() const = 0;
+  virtual const std::vector<WheelInfo>& GetWheels() const = 0;
+
+  virtual ImVec2 GetSpeedVector() const = 0;
+
+  // Clamped between -1 and 1 with -1 being full CCW.
+  virtual double GetRotation() const = 0;
+};
+void DisplayDrive(DriveModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/FMS.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/FMS.h
new file mode 100644
index 0000000..1e0f8ef
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/FMS.h
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include "glass/Model.h"
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace glass {
+
+class DataSource;
+
+class FMSModel : public Model {
+ public:
+  virtual DataSource* GetFmsAttachedData() = 0;
+  virtual DataSource* GetDsAttachedData() = 0;
+  virtual DataSource* GetAllianceStationIdData() = 0;
+  virtual DataSource* GetMatchTimeData() = 0;
+  virtual DataSource* GetEStopData() = 0;
+  virtual DataSource* GetEnabledData() = 0;
+  virtual DataSource* GetTestData() = 0;
+  virtual DataSource* GetAutonomousData() = 0;
+  virtual std::string_view GetGameSpecificMessage(
+      wpi::SmallVectorImpl<char>& buf) = 0;
+
+  virtual void SetFmsAttached(bool val) = 0;
+  virtual void SetDsAttached(bool val) = 0;
+  virtual void SetAllianceStationId(int val) = 0;
+  virtual void SetMatchTime(double val) = 0;
+  virtual void SetEStop(bool val) = 0;
+  virtual void SetEnabled(bool val) = 0;
+  virtual void SetTest(bool val) = 0;
+  virtual void SetAutonomous(bool val) = 0;
+  virtual void SetGameSpecificMessage(const char* val) = 0;
+};
+
+/**
+ * Displays FMS view.
+ *
+ * @param matchTimeEnabled If not null, a checkbox is displayed for
+ *                         "enable match time" linked to this value
+ */
+void DisplayFMS(FMSModel* model, bool* matchTimeEnabled = nullptr);
+void DisplayFMSReadOnly(FMSModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/Field2D.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Field2D.h
new file mode 100644
index 0000000..9399876
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Field2D.h
@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <frc/geometry/Pose2d.h>
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <imgui.h>
+#include <wpi/function_ref.h>
+#include <wpi/span.h>
+
+#include "glass/Model.h"
+#include "glass/View.h"
+
+namespace glass {
+
+class FieldObjectModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+
+  virtual wpi::span<const frc::Pose2d> GetPoses() = 0;
+  virtual void SetPoses(wpi::span<const frc::Pose2d> poses) = 0;
+  virtual void SetPose(size_t i, frc::Pose2d pose) = 0;
+  virtual void SetPosition(size_t i, frc::Translation2d pos) = 0;
+  virtual void SetRotation(size_t i, frc::Rotation2d rot) = 0;
+};
+
+class Field2DModel : public Model {
+ public:
+  virtual FieldObjectModel* AddFieldObject(std::string_view name) = 0;
+  virtual void RemoveFieldObject(std::string_view name) = 0;
+  virtual void ForEachFieldObject(
+      wpi::function_ref<void(FieldObjectModel& model, std::string_view name)>
+          func) = 0;
+};
+
+void DisplayField2D(Field2DModel* model, const ImVec2& contentSize);
+void DisplayField2DSettings(Field2DModel* model);
+
+class Field2DView : public View {
+ public:
+  explicit Field2DView(Field2DModel* model) : m_model{model} {}
+
+  void Display() override;
+
+ private:
+  Field2DModel* m_model;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/Log.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Log.h
new file mode 100644
index 0000000..3d9c59b
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Log.h
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include "glass/View.h"
+
+namespace glass {
+
+class LogData {
+  friend void DisplayLog(LogData*, bool);
+
+ public:
+  explicit LogData(size_t maxLines = 10000);
+
+  void Clear();
+  void Append(std::string_view msg);
+  const std::string& GetBuffer();
+
+ private:
+  size_t m_maxLines;
+  std::string m_buf;
+  std::vector<size_t> m_lineOffsets{0};
+};
+
+void DisplayLog(LogData* data, bool autoScroll);
+
+class LogView : public View {
+ public:
+  explicit LogView(LogData* data) : m_data{data} {}
+
+  void Display() override;
+
+ private:
+  LogData* m_data;
+  bool m_autoScroll{true};
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/Mechanism2D.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Mechanism2D.h
new file mode 100644
index 0000000..7617e6f
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Mechanism2D.h
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/geometry/Rotation2d.h>
+#include <frc/geometry/Translation2d.h>
+#include <imgui.h>
+#include <wpi/function_ref.h>
+
+#include "glass/Model.h"
+#include "glass/View.h"
+
+namespace glass {
+
+class MechanismObjectModel;
+
+class MechanismObjectGroup {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual void ForEachObject(
+      wpi::function_ref<void(MechanismObjectModel& model)> func) = 0;
+};
+
+class MechanismObjectModel : public MechanismObjectGroup {
+ public:
+  virtual const char* GetType() const = 0;
+  virtual ImU32 GetColor() const = 0;
+
+  // line accessors
+  virtual double GetWeight() const = 0;
+  virtual frc::Rotation2d GetAngle() const = 0;
+  virtual units::meter_t GetLength() const = 0;
+};
+
+class MechanismRootModel : public MechanismObjectGroup {
+ public:
+  virtual frc::Translation2d GetPosition() const = 0;
+};
+
+class Mechanism2DModel : public Model {
+ public:
+  virtual frc::Translation2d GetDimensions() const = 0;
+  virtual ImU32 GetBackgroundColor() const = 0;
+  virtual void ForEachRoot(
+      wpi::function_ref<void(MechanismRootModel& model)> func) = 0;
+};
+
+void DisplayMechanism2D(Mechanism2DModel* model, const ImVec2& contentSize);
+void DisplayMechanism2DSettings(Mechanism2DModel* model);
+
+class Mechanism2DView : public View {
+ public:
+  explicit Mechanism2DView(Mechanism2DModel* model) : m_model{model} {}
+
+  void Display() override;
+
+ private:
+  Mechanism2DModel* m_model;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h
new file mode 100644
index 0000000..ab0dcb3
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/PIDController.h
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class PIDControllerModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+
+  virtual DataSource* GetPData() = 0;
+  virtual DataSource* GetIData() = 0;
+  virtual DataSource* GetDData() = 0;
+  virtual DataSource* GetSetpointData() = 0;
+
+  virtual void SetP(double value) = 0;
+  virtual void SetI(double value) = 0;
+  virtual void SetD(double value) = 0;
+  virtual void SetSetpoint(double value) = 0;
+};
+void DisplayPIDController(PIDControllerModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/Plot.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Plot.h
new file mode 100644
index 0000000..f7b196d
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Plot.h
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include "glass/WindowManager.h"
+#include "glass/support/IniSaverBase.h"
+
+namespace glass {
+
+class PlotProvider : private WindowManager {
+ public:
+  explicit PlotProvider(std::string_view iniName);
+  ~PlotProvider() override;
+
+  void GlobalInit() override;
+
+  /**
+   * Pauses or unpauses all plots.
+   *
+   * @param paused true to pause, false to unpause
+   */
+  void SetPaused(bool paused) { m_paused = paused; }
+
+  /**
+   * Returns true if all plots are paused.
+   */
+  bool IsPaused() { return m_paused; }
+
+  void DisplayMenu() override;
+
+ private:
+  void DisplayWindows() override;
+
+  class IniSaver : public IniSaverBase {
+   public:
+    explicit IniSaver(std::string_view typeName, PlotProvider* provider,
+                      bool forSeries);
+
+    void* IniReadOpen(const char* name) override;
+    void IniReadLine(void* entry, const char* lineStr) override;
+    void IniWriteAll(ImGuiTextBuffer* out_buf) override;
+
+   private:
+    PlotProvider* m_provider;
+    bool m_forSeries;
+  };
+
+  IniSaver m_plotSaver;
+  IniSaver m_seriesSaver;
+  bool m_paused = false;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/StringChooser.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/StringChooser.h
new file mode 100644
index 0000000..77f9ac2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/StringChooser.h
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <wpi/span.h>
+
+#include "glass/Model.h"
+
+namespace glass {
+
+class StringChooserModel : public Model {
+ public:
+  virtual const std::string& GetDefault() = 0;
+  virtual const std::string& GetSelected() = 0;
+  virtual const std::string& GetActive() = 0;
+  virtual const std::vector<std::string>& GetOptions() = 0;
+
+  virtual void SetDefault(std::string_view val) = 0;
+  virtual void SetSelected(std::string_view val) = 0;
+  virtual void SetActive(std::string_view val) = 0;
+  virtual void SetOptions(wpi::span<const std::string> val) = 0;
+};
+
+void DisplayStringChooser(StringChooserModel* model);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/other/Subsystem.h b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Subsystem.h
new file mode 100644
index 0000000..db79db5
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/other/Subsystem.h
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "glass/Model.h"
+
+namespace glass {
+class DataSource;
+class SubsystemModel : public Model {
+ public:
+  virtual const char* GetName() const = 0;
+  virtual const char* GetDefaultCommand() const = 0;
+  virtual const char* GetCurrentCommand() const = 0;
+};
+void DisplaySubsystem(SubsystemModel* m);
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h
new file mode 100644
index 0000000..3a35335
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/ExtraGuiWidgets.h
@@ -0,0 +1,96 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <imgui.h>
+
+namespace glass {
+
+class DataSource;
+
+/**
+ * DrawLEDs() configuration for 2D arrays.
+ */
+struct LEDConfig {
+  /**
+   * Whether the major order is serpentined (e.g. the first row goes left to
+   * right, the second row right to left, the third row left to right, and so
+   * on).
+   */
+  bool serpentine = false;
+
+  /**
+   * The input array order (row-major or column-major).
+   */
+  enum Order { RowMajor = 0, ColumnMajor } order = RowMajor;
+
+  /**
+   * The starting location of the array (0 location).
+   */
+  enum Start {
+    UpperLeft = 0,
+    LowerLeft,
+    UpperRight,
+    LowerRight
+  } start = UpperLeft;
+};
+
+/**
+ * Draw a 2D array of LEDs.
+ *
+ * Values are indices into colors array.  Positive values are filled (lit),
+ * negative values are unfilled (dark / border only).  The actual color index
+ * is the absolute value of the value - 1.  0 values are not drawn at all
+ * (an empty space is left).
+ *
+ * @param values values array
+ * @param numValues size of values array
+ * @param cols number of columns
+ * @param colors colors array
+ * @param size size of each LED (both horizontal and vertical);
+ *             if 0, defaults to 1/2 of font size
+ * @param spacing spacing between each LED (both horizontal and vertical);
+ *                if 0, defaults to 1/3 of font size
+ * @param config 2D array configuration
+ */
+void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
+              float size = 0.0f, float spacing = 0.0f,
+              const LEDConfig& config = LEDConfig{});
+
+/**
+ * Draw a 2D array of LEDs.
+ *
+ * Values are indices into colors array.  Positive values are filled (lit),
+ * negative values are unfilled (dark / border only).  The actual color index
+ * is the absolute value of the value - 1.  0 values are not drawn at all
+ * (an empty space is left).
+ *
+ * @param values values array
+ * @param sources sources array
+ * @param numValues size of values and sources arrays
+ * @param cols number of columns
+ * @param colors colors array
+ * @param size size of each LED (both horizontal and vertical);
+ *             if 0, defaults to 1/2 of font size
+ * @param spacing spacing between each LED (both horizontal and vertical);
+ *                if 0, defaults to 1/3 of font size
+ * @param config 2D array configuration
+ */
+void DrawLEDSources(const int* values, DataSource** sources, int numValues,
+                    int cols, const ImU32* colors, float size = 0.0f,
+                    float spacing = 0.0f,
+                    const LEDConfig& config = LEDConfig{});
+
+/**
+ * Delete button (X in circle), based on ImGui::CloseButton().
+ */
+bool DeleteButton(ImGuiID id, const ImVec2& pos);
+
+/**
+ * Create a small overlapping delete button for collapsing headers.
+ */
+bool HeaderDeleteButton(const char* label);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaver.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaver.h
new file mode 100644
index 0000000..a3aa3d0
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaver.h
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <imgui.h>
+#include <wpi/DenseMap.h>
+
+#include "glass/support/IniSaverBase.h"
+
+namespace glass {
+
+template <typename Info>
+class IniSaver : public IniSaverBase {
+ public:
+  explicit IniSaver(std::string_view typeName,
+                    IniSaverBackend* backend = nullptr)
+      : IniSaverBase(typeName, backend) {}
+
+  // pass through useful functions to map
+  Info& operator[](int index) { return m_map[index]; }
+
+  auto begin() { return m_map.begin(); }
+  auto end() { return m_map.end(); }
+  auto find(int index) { return m_map.find(index); }
+
+  auto begin() const { return m_map.begin(); }
+  auto end() const { return m_map.end(); }
+  auto find(int index) const { return m_map.find(index); }
+
+ private:
+  void* IniReadOpen(const char* name) override;
+  void IniReadLine(void* entry, const char* lineStr) override;
+  void IniWriteAll(ImGuiTextBuffer* out_buf) override;
+
+  wpi::DenseMap<int, Info> m_map;
+};
+
+}  // namespace glass
+
+#include "IniSaver.inc"
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaver.inc b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaver.inc
new file mode 100644
index 0000000..42efb85
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaver.inc
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/StringExtras.h>
+
+#include "glass/support/IniSaver.h"
+
+namespace glass {
+
+template <typename Info>
+void* IniSaver<Info>::IniReadOpen(const char* name) {
+  if (auto num = wpi::parse_integer<int>(name, 10)) {
+    return &m_map[num.value()];
+  } else {
+    return nullptr;
+  }
+}
+
+template <typename Info>
+void IniSaver<Info>::IniReadLine(void* entry, const char* line) {
+  auto element = static_cast<Info*>(entry);
+  auto [name, value] = wpi::split(line, '=');
+  element->ReadIni(wpi::trim(name), wpi::trim(value));
+}
+
+template <typename Info>
+void IniSaver<Info>::IniWriteAll(ImGuiTextBuffer* out_buf) {
+  for (auto&& it : m_map) {
+    out_buf->appendf("[%s][%d]\n", GetTypeName(), it.first);
+    it.second.WriteIni(out_buf);
+    out_buf->append("\n");
+  }
+}
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverBase.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverBase.h
new file mode 100644
index 0000000..85ae1e3
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverBase.h
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <imgui.h>
+
+namespace glass {
+
+class IniSaverBase;
+
+class IniSaverBackend {
+ public:
+  virtual ~IniSaverBackend() = default;
+  virtual void Register(IniSaverBase* iniSaver) = 0;
+  virtual void Unregister(IniSaverBase* iniSaver) = 0;
+};
+
+class IniSaverBase {
+ public:
+  explicit IniSaverBase(std::string_view typeName,
+                        IniSaverBackend* backend = nullptr);
+  virtual ~IniSaverBase();
+
+  void Initialize() { m_backend->Register(this); }
+
+  const char* GetTypeName() const { return m_typeName.c_str(); }
+  IniSaverBackend* GetBackend() const { return m_backend; }
+
+  IniSaverBase(const IniSaverBase&) = delete;
+  IniSaverBase& operator=(const IniSaverBase&) = delete;
+
+  virtual void* IniReadOpen(const char* name) = 0;
+  virtual void IniReadLine(void* entry, const char* lineStr) = 0;
+  virtual void IniWriteAll(ImGuiTextBuffer* out_buf) = 0;
+
+ private:
+  std::string m_typeName;
+  IniSaverBackend* m_backend;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverInfo.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverInfo.h
new file mode 100644
index 0000000..2014e9d
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverInfo.h
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <imgui.h>
+
+namespace glass {
+
+class NameInfo {
+ public:
+  NameInfo() { m_name[0] = '\0'; }
+
+  bool HasName() const { return m_name[0] != '\0'; }
+  void SetName(std::string_view name);
+  const char* GetName() const { return m_name; }
+  void GetName(char* buf, size_t size, const char* defaultName) const;
+  void GetName(char* buf, size_t size, const char* defaultName,
+               int index) const;
+  void GetName(char* buf, size_t size, const char* defaultName, int index,
+               int index2) const;
+  void GetLabel(char* buf, size_t size, const char* defaultName) const;
+  void GetLabel(char* buf, size_t size, const char* defaultName,
+                int index) const;
+  void GetLabel(char* buf, size_t size, const char* defaultName, int index,
+                int index2) const;
+
+  bool ReadIni(std::string_view name, std::string_view value);
+  void WriteIni(ImGuiTextBuffer* out);
+  void PushEditNameId(int index);
+  void PushEditNameId(const char* name);
+  bool PopupEditName(int index);
+  bool PopupEditName(const char* name);
+  bool InputTextName(const char* label_id, ImGuiInputTextFlags flags = 0);
+
+ private:
+  char m_name[64];
+};
+
+class OpenInfo {
+ public:
+  OpenInfo() = default;
+  explicit OpenInfo(bool open) : m_open(open) {}
+
+  bool IsOpen() const { return m_open; }
+  void SetOpen(bool open) { m_open = open; }
+  bool ReadIni(std::string_view name, std::string_view value);
+  void WriteIni(ImGuiTextBuffer* out);
+
+ private:
+  bool m_open = false;
+};
+
+class NameOpenInfo : public NameInfo, public OpenInfo {
+ public:
+  bool ReadIni(std::string_view name, std::string_view value);
+  void WriteIni(ImGuiTextBuffer* out);
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverString.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverString.h
new file mode 100644
index 0000000..4134219
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverString.h
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+#include <utility>
+
+#include <imgui.h>
+#include <wpi/StringMap.h>
+
+#include "glass/support/IniSaverBase.h"
+
+namespace glass {
+
+template <typename Info>
+class IniSaverString : public IniSaverBase {
+ public:
+  explicit IniSaverString(std::string_view typeName,
+                          IniSaverBackend* backend = nullptr)
+      : IniSaverBase(typeName, backend) {}
+
+  // pass through useful functions to map
+  Info& operator[](std::string_view key) { return m_map[key]; }
+
+  template <typename... ArgsTy>
+  auto try_emplace(std::string_view key, ArgsTy&&... args) {
+    return m_map.try_emplace(key, std::forward<ArgsTy>(args)...);
+  }
+
+  void erase(typename wpi::StringMap<Info>::iterator it) { m_map.erase(it); }
+  auto erase(std::string_view key) { return m_map.erase(key); }
+
+  auto begin() { return m_map.begin(); }
+  auto end() { return m_map.end(); }
+  auto find(std::string_view key) { return m_map.find(key); }
+
+  auto begin() const { return m_map.begin(); }
+  auto end() const { return m_map.end(); }
+  auto find(std::string_view key) const { return m_map.find(key); }
+
+  bool empty() const { return m_map.empty(); }
+
+ private:
+  void* IniReadOpen(const char* name) override;
+  void IniReadLine(void* entry, const char* lineStr) override;
+  void IniWriteAll(ImGuiTextBuffer* out_buf) override;
+
+  wpi::StringMap<Info> m_map;
+};
+
+}  // namespace glass
+
+#include "IniSaverString.inc"
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverString.inc b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverString.inc
new file mode 100644
index 0000000..0d18d29
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverString.inc
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/StringExtras.h>
+
+#include "glass/support/IniSaverString.h"
+
+namespace glass {
+
+template <typename Info>
+void* IniSaverString<Info>::IniReadOpen(const char* name) {
+  return &m_map[name];
+}
+
+template <typename Info>
+void IniSaverString<Info>::IniReadLine(void* entry, const char* line) {
+  auto element = static_cast<Info*>(entry);
+  auto [name, value] = wpi::split(line, '=');
+  element->ReadIni(wpi::trim(name), wpi::trim(value));
+}
+
+template <typename Info>
+void IniSaverString<Info>::IniWriteAll(ImGuiTextBuffer* out_buf) {
+  for (auto&& it : m_map) {
+    out_buf->appendf("[%s][%s]\n", GetTypeName(), it.getKey().data());
+    it.second.WriteIni(out_buf);
+    out_buf->append("\n");
+  }
+}
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverVector.h b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverVector.h
new file mode 100644
index 0000000..e2e57ce
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverVector.h
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+#include <vector>
+
+#include <imgui.h>
+
+#include "glass/support/IniSaverBase.h"
+
+namespace glass {
+
+template <typename Info>
+class IniSaverVector : public std::vector<Info>, public IniSaverBase {
+ public:
+  explicit IniSaverVector(std::string_view typeName,
+                          IniSaverBackend* backend = nullptr)
+      : IniSaverBase(typeName, backend) {}
+
+ private:
+  void* IniReadOpen(const char* name) override;
+  void IniReadLine(void* entry, const char* lineStr) override;
+  void IniWriteAll(ImGuiTextBuffer* out_buf) override;
+};
+
+}  // namespace glass
+
+#include "IniSaverVector.inc"
diff --git a/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverVector.inc b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverVector.inc
new file mode 100644
index 0000000..a86b116
--- /dev/null
+++ b/third_party/allwpilib/glass/src/lib/native/include/glass/support/IniSaverVector.inc
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <wpi/StringExtras.h>
+
+#include "glass/support/IniSaverVector.h"
+
+namespace glass {
+
+template <typename Info>
+void* IniSaverVector<Info>::IniReadOpen(const char* name) {
+  if (auto num = wpi::parse_integer<unsigned int>(name, 10)) {
+    if (num.value() >= this->size()) {
+      this->resize(num.value() + 1);
+    }
+    return &(*this)[num.value()];
+  } else {
+    return nullptr;
+  }
+}
+
+template <typename Info>
+void IniSaverVector<Info>::IniReadLine(void* entry, const char* line) {
+  auto element = static_cast<Info*>(entry);
+  auto [name, value] = wpi::split(line, '=');
+  element->ReadIni(wpi::trim(name), wpi::trim(value));
+}
+
+template <typename Info>
+void IniSaverVector<Info>::IniWriteAll(ImGuiTextBuffer* out_buf) {
+  for (size_t i = 0; i < this->size(); ++i) {
+    out_buf->appendf("[%s][%d]\n", GetTypeName(), static_cast<int>(i));
+    (*this)[i].WriteIni(out_buf);
+    out_buf->append("\n");
+  }
+}
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTCommandScheduler.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTCommandScheduler.cpp
new file mode 100644
index 0000000..ccc6412
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTCommandScheduler.cpp
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTCommandScheduler.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTCommandSchedulerModel::NTCommandSchedulerModel(std::string_view path)
+    : NTCommandSchedulerModel(nt::GetDefaultInstance(), path) {}
+
+NTCommandSchedulerModel::NTCommandSchedulerModel(NT_Inst instance,
+                                                 std::string_view path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_commands(m_nt.GetEntry(fmt::format("{}/Names", path))),
+      m_ids(m_nt.GetEntry(fmt::format("{}/Ids", path))),
+      m_cancel(m_nt.GetEntry(fmt::format("{}/Cancel", path))),
+      m_nameValue(wpi::rsplit(path, '/').second) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_commands);
+  m_nt.AddListener(m_ids);
+  m_nt.AddListener(m_cancel);
+}
+
+void NTCommandSchedulerModel::CancelCommand(size_t index) {
+  if (index < m_idsValue.size()) {
+    nt::SetEntryValue(
+        m_cancel, nt::NetworkTableValue::MakeDoubleArray({m_idsValue[index]}));
+  }
+}
+
+void NTCommandSchedulerModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    } else if (event.entry == m_commands) {
+      if (event.value && event.value->IsStringArray()) {
+        auto arr = event.value->GetStringArray();
+        m_commandsValue.assign(arr.begin(), arr.end());
+      }
+    } else if (event.entry == m_ids) {
+      if (event.value && event.value->IsDoubleArray()) {
+        auto arr = event.value->GetDoubleArray();
+        m_idsValue.assign(arr.begin(), arr.end());
+      }
+    }
+  }
+}
+
+bool NTCommandSchedulerModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_commands) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTCommandSelector.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTCommandSelector.cpp
new file mode 100644
index 0000000..efcbac2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTCommandSelector.cpp
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTCommandSelector.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTCommandSelectorModel::NTCommandSelectorModel(std::string_view path)
+    : NTCommandSelectorModel(nt::GetDefaultInstance(), path) {}
+
+NTCommandSelectorModel::NTCommandSelectorModel(NT_Inst instance,
+                                               std::string_view path)
+    : m_nt(instance),
+      m_running(m_nt.GetEntry(fmt::format("{}/running", path))),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_runningData(fmt::format("NTCmd:{}", path)),
+      m_nameValue(wpi::rsplit(path, '/').second) {
+  m_runningData.SetDigital(true);
+  m_nt.AddListener(m_running);
+  m_nt.AddListener(m_name);
+}
+
+void NTCommandSelectorModel::SetRunning(bool run) {
+  nt::SetEntryValue(m_running, nt::NetworkTableValue::MakeBoolean(run));
+}
+
+void NTCommandSelectorModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_running) {
+      if (event.value && event.value->IsBoolean()) {
+        m_runningData.SetValue(event.value->GetBoolean());
+      }
+    } else if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    }
+  }
+}
+
+bool NTCommandSelectorModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_running) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTDifferentialDrive.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTDifferentialDrive.cpp
new file mode 100644
index 0000000..b44ea07
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTDifferentialDrive.cpp
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTDifferentialDrive.h"
+
+#include <fmt/format.h>
+#include <imgui.h>
+#include <wpi/MathExtras.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTDifferentialDriveModel::NTDifferentialDriveModel(std::string_view path)
+    : NTDifferentialDriveModel(nt::GetDefaultInstance(), path) {}
+
+NTDifferentialDriveModel::NTDifferentialDriveModel(NT_Inst instance,
+                                                   std::string_view path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_controllable(m_nt.GetEntry(fmt::format("{}/.controllable", path))),
+      m_lPercent(m_nt.GetEntry(fmt::format("{}/Left Motor Speed", path))),
+      m_rPercent(m_nt.GetEntry(fmt::format("{}/Right Motor Speed", path))),
+      m_nameValue(wpi::rsplit(path, '/').second),
+      m_lPercentData(fmt::format("NTDiffDriveL:{}", path)),
+      m_rPercentData(fmt::format("NTDiffDriveR:{}", path)) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_controllable);
+  m_nt.AddListener(m_lPercent);
+  m_nt.AddListener(m_rPercent);
+
+  m_wheels.emplace_back("L % Output", &m_lPercentData, [this](auto value) {
+    nt::SetEntryValue(m_lPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("R % Output", &m_rPercentData, [this](auto value) {
+    nt::SetEntryValue(m_rPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+}
+
+void NTDifferentialDriveModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name && event.value && event.value->IsString()) {
+      m_nameValue = event.value->GetString();
+    } else if (event.entry == m_lPercent && event.value &&
+               event.value->IsDouble()) {
+      m_lPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_rPercent && event.value &&
+               event.value->IsDouble()) {
+      m_rPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_controllable && event.value &&
+               event.value->IsBoolean()) {
+      m_controllableValue = event.value->GetBoolean();
+    }
+  }
+
+  double l = m_lPercentData.GetValue();
+  double r = m_rPercentData.GetValue();
+
+  m_speedVector = ImVec2(0.0, -(l + r) / 2.0);
+  m_rotation = (l - r) / 2.0;
+}
+
+bool NTDifferentialDriveModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_lPercent) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTDigitalInput.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTDigitalInput.cpp
new file mode 100644
index 0000000..5de6c29
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTDigitalInput.cpp
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTDigitalInput.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTDigitalInputModel::NTDigitalInputModel(std::string_view path)
+    : NTDigitalInputModel{nt::GetDefaultInstance(), path} {}
+
+NTDigitalInputModel::NTDigitalInputModel(NT_Inst inst, std::string_view path)
+    : m_nt{inst},
+      m_value{m_nt.GetEntry(fmt::format("{}/Value", path))},
+      m_name{m_nt.GetEntry(fmt::format("{}/.name", path))},
+      m_valueData{fmt::format("NT_DIn:{}", path)},
+      m_nameValue{wpi::rsplit(path, '/').second} {
+  m_nt.AddListener(m_value);
+  m_nt.AddListener(m_name);
+
+  m_valueData.SetDigital(true);
+}
+
+void NTDigitalInputModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_value) {
+      if (event.value && event.value->IsBoolean()) {
+        m_valueData.SetValue(event.value->GetBoolean());
+      }
+    } else if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    }
+  }
+}
+
+bool NTDigitalInputModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_value) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTDigitalOutput.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTDigitalOutput.cpp
new file mode 100644
index 0000000..a09d424
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTDigitalOutput.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTDigitalOutput.h"
+
+#include <fmt/format.h>
+
+using namespace glass;
+
+NTDigitalOutputModel::NTDigitalOutputModel(std::string_view path)
+    : NTDigitalOutputModel{nt::GetDefaultInstance(), path} {}
+
+NTDigitalOutputModel::NTDigitalOutputModel(NT_Inst inst, std::string_view path)
+    : m_nt{inst},
+      m_value{m_nt.GetEntry(fmt::format("{}/Value", path))},
+      m_name{m_nt.GetEntry(fmt::format("{}/.name", path))},
+      m_controllable{m_nt.GetEntry(fmt::format("{}/.controllable", path))},
+      m_valueData{fmt::format("NT_DOut:{}", path)} {
+  m_nt.AddListener(m_value);
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_controllable);
+
+  m_valueData.SetDigital(true);
+}
+
+void NTDigitalOutputModel::SetValue(bool val) {
+  nt::SetEntryValue(m_value, nt::Value::MakeBoolean(val));
+}
+
+void NTDigitalOutputModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_value) {
+      if (event.value && event.value->IsBoolean()) {
+        m_valueData.SetValue(event.value->GetBoolean());
+      }
+    } else if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    } else if (event.entry == m_controllable) {
+      if (event.value && event.value->IsBoolean()) {
+        m_controllableValue = event.value->GetBoolean();
+      }
+    }
+  }
+}
+
+bool NTDigitalOutputModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_value) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTFMS.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTFMS.cpp
new file mode 100644
index 0000000..84c1ce7
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTFMS.cpp
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTFMS.h"
+
+#include <stdint.h>
+
+#include <fmt/format.h>
+#include <wpi/SmallVector.h>
+#include <wpi/timestamp.h>
+
+using namespace glass;
+
+NTFMSModel::NTFMSModel(std::string_view path)
+    : NTFMSModel{nt::GetDefaultInstance(), path} {}
+
+NTFMSModel::NTFMSModel(NT_Inst inst, std::string_view path)
+    : m_nt{inst},
+      m_gameSpecificMessage{
+          m_nt.GetEntry(fmt::format("{}/GameSpecificMessage", path))},
+      m_alliance{m_nt.GetEntry(fmt::format("{}/IsRedAlliance", path))},
+      m_station{m_nt.GetEntry(fmt::format("{}/StationNumber", path))},
+      m_controlWord{m_nt.GetEntry(fmt::format("{}/FMSControlData", path))},
+      m_fmsAttached{fmt::format("NT_FMS:FMSAttached:{}", path)},
+      m_dsAttached{fmt::format("NT_FMS:DSAttached:{}", path)},
+      m_allianceStationId{fmt::format("NT_FMS:AllianceStationID:{}", path)},
+      m_estop{fmt::format("NT_FMS:EStop:{}", path)},
+      m_enabled{fmt::format("NT_FMS:RobotEnabled:{}", path)},
+      m_test{fmt::format("NT_FMS:TestMode:{}", path)},
+      m_autonomous{fmt::format("NT_FMS:AutonomousMode:{}", path)} {
+  m_nt.AddListener(m_alliance);
+  m_nt.AddListener(m_station);
+  m_nt.AddListener(m_controlWord);
+
+  m_fmsAttached.SetDigital(true);
+  m_dsAttached.SetDigital(true);
+  m_estop.SetDigital(true);
+  m_enabled.SetDigital(true);
+  m_test.SetDigital(true);
+  m_autonomous.SetDigital(true);
+}
+
+std::string_view NTFMSModel::GetGameSpecificMessage(
+    wpi::SmallVectorImpl<char>& buf) {
+  buf.clear();
+  auto value = nt::GetEntryValue(m_gameSpecificMessage);
+  if (value && value->IsString()) {
+    auto str = value->GetString();
+    buf.append(str.begin(), str.end());
+  }
+  return std::string_view{buf.data(), buf.size()};
+}
+
+void NTFMSModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_alliance) {
+      if (event.value && event.value->IsBoolean()) {
+        int allianceStationId = m_allianceStationId.GetValue();
+        allianceStationId %= 3;
+        // true if red
+        allianceStationId += 3 * (event.value->GetBoolean() ? 0 : 1);
+        m_allianceStationId.SetValue(allianceStationId);
+      }
+    } else if (event.entry == m_station) {
+      if (event.value && event.value->IsDouble()) {
+        int allianceStationId = m_allianceStationId.GetValue();
+        bool isRed = (allianceStationId < 3);
+        // the NT value is 1-indexed
+        m_allianceStationId.SetValue(event.value->GetDouble() - 1 +
+                                     3 * (isRed ? 0 : 1));
+      }
+    } else if (event.entry == m_controlWord) {
+      if (event.value && event.value->IsDouble()) {
+        uint32_t controlWord = event.value->GetDouble();
+        // See HAL_ControlWord definition
+        auto time = wpi::Now();
+        m_enabled.SetValue(((controlWord & 0x01) != 0) ? 1 : 0, time);
+        m_autonomous.SetValue(((controlWord & 0x02) != 0) ? 1 : 0, time);
+        m_test.SetValue(((controlWord & 0x04) != 0) ? 1 : 0, time);
+        m_estop.SetValue(((controlWord & 0x08) != 0) ? 1 : 0, time);
+        m_fmsAttached.SetValue(((controlWord & 0x10) != 0) ? 1 : 0, time);
+        m_dsAttached.SetValue(((controlWord & 0x20) != 0) ? 1 : 0, time);
+      }
+    }
+  }
+}
+
+bool NTFMSModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_controlWord) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTField2D.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTField2D.cpp
new file mode 100644
index 0000000..47fa9a7
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTField2D.cpp
@@ -0,0 +1,255 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTField2D.h"
+
+#include <algorithm>
+#include <vector>
+
+#include <fmt/format.h>
+#include <ntcore_cpp.h>
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+class NTField2DModel::ObjectModel : public FieldObjectModel {
+ public:
+  ObjectModel(std::string_view name, NT_Entry entry)
+      : m_name{name}, m_entry{entry} {}
+
+  const char* GetName() const override { return m_name.c_str(); }
+  NT_Entry GetEntry() const { return m_entry; }
+
+  void NTUpdate(const nt::Value& value);
+
+  void Update() override {
+    if (auto value = nt::GetEntryValue(m_entry)) {
+      NTUpdate(*value);
+    }
+  }
+  bool Exists() override { return nt::GetEntryType(m_entry) != NT_UNASSIGNED; }
+  bool IsReadOnly() override { return false; }
+
+  wpi::span<const frc::Pose2d> GetPoses() override { return m_poses; }
+  void SetPoses(wpi::span<const frc::Pose2d> poses) override;
+  void SetPose(size_t i, frc::Pose2d pose) override;
+  void SetPosition(size_t i, frc::Translation2d pos) override;
+  void SetRotation(size_t i, frc::Rotation2d rot) override;
+
+ private:
+  void UpdateNT();
+
+  std::string m_name;
+  NT_Entry m_entry;
+
+  std::vector<frc::Pose2d> m_poses;
+};
+
+void NTField2DModel::ObjectModel::NTUpdate(const nt::Value& value) {
+  if (value.IsDoubleArray()) {
+    auto arr = value.GetDoubleArray();
+    auto size = arr.size();
+    if ((size % 3) != 0) {
+      return;
+    }
+    m_poses.resize(size / 3);
+    for (size_t i = 0; i < size / 3; ++i) {
+      m_poses[i] = frc::Pose2d{
+          units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+          frc::Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+    }
+  } else if (value.IsRaw()) {
+    // treat it simply as an array of doubles
+    std::string_view data = value.GetRaw();
+
+    // must be triples of doubles
+    auto size = data.size();
+    if ((size % (3 * 8)) != 0) {
+      return;
+    }
+    m_poses.resize(size / (3 * 8));
+    const char* p = data.data();
+    for (size_t i = 0; i < size / (3 * 8); ++i) {
+      double x = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double y = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double rot = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      m_poses[i] = frc::Pose2d{units::meter_t{x}, units::meter_t{y},
+                               frc::Rotation2d{units::degree_t{rot}}};
+    }
+  }
+}
+
+void NTField2DModel::ObjectModel::UpdateNT() {
+  if (m_poses.size() < (255 / 3)) {
+    wpi::SmallVector<double, 9> arr;
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      arr.push_back(translation.X().value());
+      arr.push_back(translation.Y().value());
+      arr.push_back(pose.Rotation().Degrees().value());
+    }
+    nt::SetEntryTypeValue(m_entry, nt::Value::MakeDoubleArray(arr));
+  } else {
+    // send as raw array of doubles if too big for NT array
+    std::vector<char> arr;
+    arr.resize(m_poses.size() * 3 * 8);
+    char* p = arr.data();
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.X().value()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.Y().value()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(pose.Rotation().Degrees().value()));
+      p += 8;
+    }
+    nt::SetEntryTypeValue(m_entry,
+                          nt::Value::MakeRaw({arr.data(), arr.size()}));
+  }
+}
+
+void NTField2DModel::ObjectModel::SetPoses(wpi::span<const frc::Pose2d> poses) {
+  m_poses.assign(poses.begin(), poses.end());
+  UpdateNT();
+}
+
+void NTField2DModel::ObjectModel::SetPose(size_t i, frc::Pose2d pose) {
+  if (i < m_poses.size()) {
+    m_poses[i] = pose;
+    UpdateNT();
+  }
+}
+
+void NTField2DModel::ObjectModel::SetPosition(size_t i,
+                                              frc::Translation2d pos) {
+  if (i < m_poses.size()) {
+    m_poses[i] = frc::Pose2d{pos, m_poses[i].Rotation()};
+    UpdateNT();
+  }
+}
+
+void NTField2DModel::ObjectModel::SetRotation(size_t i, frc::Rotation2d rot) {
+  if (i < m_poses.size()) {
+    m_poses[i] = frc::Pose2d{m_poses[i].Translation(), rot};
+    UpdateNT();
+  }
+}
+
+NTField2DModel::NTField2DModel(std::string_view path)
+    : NTField2DModel{nt::GetDefaultInstance(), path} {}
+
+NTField2DModel::NTField2DModel(NT_Inst inst, std::string_view path)
+    : m_nt{inst},
+      m_path{fmt::format("{}/", path)},
+      m_name{m_nt.GetEntry(fmt::format("{}/.name", path))} {
+  m_nt.AddListener(m_path, NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_DELETE |
+                               NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE);
+}
+
+NTField2DModel::~NTField2DModel() = default;
+
+void NTField2DModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    // .name
+    if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+      continue;
+    }
+
+    // common case: update of existing entry; search by entry
+    if (event.flags & NT_NOTIFY_UPDATE) {
+      auto it = std::find_if(
+          m_objects.begin(), m_objects.end(),
+          [&](const auto& e) { return e->GetEntry() == event.entry; });
+      if (it != m_objects.end()) {
+        (*it)->NTUpdate(*event.value);
+        continue;
+      }
+    }
+
+    // handle create/delete
+    std::string_view name = event.name;
+    if (wpi::starts_with(name, m_path)) {
+      name.remove_prefix(m_path.size());
+      if (name.empty() || name[0] == '.') {
+        continue;
+      }
+      auto [it, match] = Find(event.name);
+      if (event.flags & NT_NOTIFY_DELETE) {
+        if (match) {
+          m_objects.erase(it);
+        }
+        continue;
+      } else if (event.flags & NT_NOTIFY_NEW) {
+        if (!match) {
+          it = m_objects.emplace(
+              it, std::make_unique<ObjectModel>(event.name, event.entry));
+        }
+      } else if (!match) {
+        continue;
+      }
+      if (event.flags & (NT_NOTIFY_NEW | NT_NOTIFY_UPDATE)) {
+        (*it)->NTUpdate(*event.value);
+      }
+    }
+  }
+}
+
+bool NTField2DModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_name) != NT_UNASSIGNED;
+}
+
+bool NTField2DModel::IsReadOnly() {
+  return false;
+}
+
+FieldObjectModel* NTField2DModel::AddFieldObject(std::string_view name) {
+  auto fullName = fmt::format("{}{}", m_path, name);
+  auto [it, match] = Find(fullName);
+  if (!match) {
+    it = m_objects.emplace(
+        it, std::make_unique<ObjectModel>(fullName, m_nt.GetEntry(fullName)));
+  }
+  return it->get();
+}
+
+void NTField2DModel::RemoveFieldObject(std::string_view name) {
+  auto [it, match] = Find(fmt::format("{}{}", m_path, name));
+  if (match) {
+    nt::DeleteEntry((*it)->GetEntry());
+    m_objects.erase(it);
+  }
+}
+
+void NTField2DModel::ForEachFieldObject(
+    wpi::function_ref<void(FieldObjectModel& model, std::string_view name)>
+        func) {
+  for (auto&& obj : m_objects) {
+    if (obj->Exists()) {
+      func(*obj, wpi::drop_front(obj->GetName(), m_path.size()));
+    }
+  }
+}
+
+std::pair<NTField2DModel::Objects::iterator, bool> NTField2DModel::Find(
+    std::string_view fullName) {
+  auto it = std::lower_bound(
+      m_objects.begin(), m_objects.end(), fullName,
+      [](const auto& e, std::string_view name) { return e->GetName() < name; });
+  return {it, it != m_objects.end() && (*it)->GetName() == fullName};
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTGyro.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTGyro.cpp
new file mode 100644
index 0000000..7651d2c
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTGyro.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTGyro.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTGyroModel::NTGyroModel(std::string_view path)
+    : NTGyroModel(nt::GetDefaultInstance(), path) {}
+
+NTGyroModel::NTGyroModel(NT_Inst instance, std::string_view path)
+    : m_nt(instance),
+      m_angle(m_nt.GetEntry(fmt::format("{}/Value", path))),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_angleData(fmt::format("NT_Gyro:{}", path)),
+      m_nameValue(wpi::rsplit(path, '/').second) {
+  m_nt.AddListener(m_angle);
+  m_nt.AddListener(m_name);
+}
+
+void NTGyroModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_angle) {
+      if (event.value && event.value->IsDouble()) {
+        m_angleData.SetValue(event.value->GetDouble());
+      }
+    } else if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    }
+  }
+}
+
+bool NTGyroModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_angle) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTMecanumDrive.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTMecanumDrive.cpp
new file mode 100644
index 0000000..28c0a67
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTMecanumDrive.cpp
@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTMecanumDrive.h"
+
+#include <fmt/format.h>
+#include <imgui.h>
+#include <wpi/MathExtras.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTMecanumDriveModel::NTMecanumDriveModel(std::string_view path)
+    : NTMecanumDriveModel(nt::GetDefaultInstance(), path) {}
+
+NTMecanumDriveModel::NTMecanumDriveModel(NT_Inst instance,
+                                         std::string_view path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_controllable(m_nt.GetEntry(fmt::format("{}/.controllable", path))),
+      m_flPercent(
+          m_nt.GetEntry(fmt::format("{}/Front Left Motor Speed", path))),
+      m_frPercent(
+          m_nt.GetEntry(fmt::format("{}/Front Right Motor Speed", path))),
+      m_rlPercent(m_nt.GetEntry(fmt::format("{}/Rear Left Motor Speed", path))),
+      m_rrPercent(
+          m_nt.GetEntry(fmt::format("{}/Rear Right Motor Speed", path))),
+      m_nameValue(wpi::rsplit(path, '/').second),
+      m_flPercentData(fmt::format("NTMcnmDriveFL:{}", path)),
+      m_frPercentData(fmt::format("NTMcnmDriveFR:{}", path)),
+      m_rlPercentData(fmt::format("NTMcnmDriveRL:{}", path)),
+      m_rrPercentData(fmt::format("NTMcnmDriveRR:{}", path)) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_controllable);
+  m_nt.AddListener(m_flPercent);
+  m_nt.AddListener(m_frPercent);
+  m_nt.AddListener(m_rlPercent);
+  m_nt.AddListener(m_rrPercent);
+
+  m_wheels.emplace_back("FL % Output", &m_flPercentData, [this](auto value) {
+    nt::SetEntryValue(m_flPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("FR % Output", &m_frPercentData, [this](auto value) {
+    nt::SetEntryValue(m_frPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("RL % Output", &m_rlPercentData, [this](auto value) {
+    nt::SetEntryValue(m_rlPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+
+  m_wheels.emplace_back("RR % Output", &m_rrPercentData, [this](auto value) {
+    nt::SetEntryValue(m_rrPercent, nt::NetworkTableValue::MakeDouble(value));
+  });
+}
+
+void NTMecanumDriveModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name && event.value && event.value->IsString()) {
+      m_nameValue = event.value->GetString();
+    } else if (event.entry == m_flPercent && event.value &&
+               event.value->IsDouble()) {
+      m_flPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_frPercent && event.value &&
+               event.value->IsDouble()) {
+      m_frPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_rlPercent && event.value &&
+               event.value->IsDouble()) {
+      m_rlPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_rrPercent && event.value &&
+               event.value->IsDouble()) {
+      m_rrPercentData.SetValue(event.value->GetDouble());
+    } else if (event.entry == m_controllable && event.value &&
+               event.value->IsBoolean()) {
+      m_controllableValue = event.value->GetBoolean();
+    }
+  }
+
+  double fl = m_flPercentData.GetValue();
+  double fr = m_frPercentData.GetValue();
+  double rl = m_rlPercentData.GetValue();
+  double rr = m_rrPercentData.GetValue();
+
+  m_speedVector =
+      ImVec2((fl - fr - rl + rr) / 4.0f, -(fl + fr + rl + rr) / 4.0f);
+  m_rotation = -(-fl + fr - rl + rr) / 4;
+}
+
+bool NTMecanumDriveModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_flPercent) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTMechanism2D.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTMechanism2D.cpp
new file mode 100644
index 0000000..9c73af2
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTMechanism2D.cpp
@@ -0,0 +1,302 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTMechanism2D.h"
+
+#include <algorithm>
+#include <string_view>
+#include <vector>
+
+#include <fmt/format.h>
+#include <imgui.h>
+#include <ntcore_cpp.h>
+#include <wpi/StringExtras.h>
+
+#include "glass/other/Mechanism2D.h"
+
+using namespace glass;
+
+// Convert "#RRGGBB" hex color to ImU32 color
+static void ConvertColor(std::string_view in, ImU32* out) {
+  if (in.size() != 7 || in[0] != '#') {
+    return;
+  }
+  if (auto v = wpi::parse_integer<ImU32>(wpi::drop_front(in), 16)) {
+    ImU32 val = v.value();
+    *out = IM_COL32((val >> 16) & 0xff, (val >> 8) & 0xff, val & 0xff, 255);
+  }
+}
+
+namespace {
+
+class NTMechanismObjectModel;
+
+class NTMechanismGroupImpl final {
+ public:
+  NTMechanismGroupImpl(NT_Inst inst, std::string_view path,
+                       std::string_view name)
+      : m_inst{inst}, m_path{path}, m_name{name} {}
+
+  const char* GetName() const { return m_name.c_str(); }
+  void ForEachObject(wpi::function_ref<void(MechanismObjectModel& model)> func);
+  void NTUpdate(const nt::EntryNotification& event, std::string_view name);
+
+ protected:
+  NT_Inst m_inst;
+  std::string m_path;
+  std::string m_name;
+  std::vector<std::unique_ptr<NTMechanismObjectModel>> m_objects;
+};
+
+class NTMechanismObjectModel final : public MechanismObjectModel {
+ public:
+  NTMechanismObjectModel(NT_Inst inst, std::string_view path,
+                         std::string_view name)
+      : m_group{inst, path, name},
+        m_type{nt::GetEntry(inst, fmt::format("{}/.type", path))},
+        m_color{nt::GetEntry(inst, fmt::format("{}/color", path))},
+        m_weight{nt::GetEntry(inst, fmt::format("{}/weight", path))},
+        m_angle{nt::GetEntry(inst, fmt::format("{}/angle", path))},
+        m_length{nt::GetEntry(inst, fmt::format("{}/length", path))} {}
+
+  const char* GetName() const final { return m_group.GetName(); }
+  void ForEachObject(
+      wpi::function_ref<void(MechanismObjectModel& model)> func) final {
+    m_group.ForEachObject(func);
+  }
+
+  const char* GetType() const final { return m_typeValue.c_str(); }
+  ImU32 GetColor() const final { return m_colorValue; }
+  double GetWeight() const final { return m_weightValue; }
+  frc::Rotation2d GetAngle() const final { return m_angleValue; }
+  units::meter_t GetLength() const final { return m_lengthValue; }
+
+  bool NTUpdate(const nt::EntryNotification& event, std::string_view childName);
+
+ private:
+  NTMechanismGroupImpl m_group;
+
+  NT_Entry m_type;
+  NT_Entry m_color;
+  NT_Entry m_weight;
+  NT_Entry m_angle;
+  NT_Entry m_length;
+
+  std::string m_typeValue;
+  ImU32 m_colorValue = IM_COL32_WHITE;
+  double m_weightValue = 1.0;
+  frc::Rotation2d m_angleValue;
+  units::meter_t m_lengthValue = 0.0_m;
+};
+
+}  // namespace
+
+void NTMechanismGroupImpl::ForEachObject(
+    wpi::function_ref<void(MechanismObjectModel& model)> func) {
+  for (auto&& obj : m_objects) {
+    func(*obj);
+  }
+}
+
+void NTMechanismGroupImpl::NTUpdate(const nt::EntryNotification& event,
+                                    std::string_view name) {
+  if (name.empty()) {
+    return;
+  }
+  std::string_view childName;
+  std::tie(name, childName) = wpi::split(name, '/');
+  if (childName.empty()) {
+    return;
+  }
+
+  auto it = std::lower_bound(
+      m_objects.begin(), m_objects.end(), name,
+      [](const auto& e, std::string_view name) { return e->GetName() < name; });
+  bool match = it != m_objects.end() && (*it)->GetName() == name;
+
+  if (event.flags & NT_NOTIFY_NEW) {
+    if (!match) {
+      it = m_objects.emplace(
+          it, std::make_unique<NTMechanismObjectModel>(
+                  m_inst, fmt::format("{}/{}", m_path, name), name));
+      match = true;
+    }
+  }
+  if (match) {
+    if ((*it)->NTUpdate(event, childName)) {
+      m_objects.erase(it);
+    }
+  }
+}
+
+bool NTMechanismObjectModel::NTUpdate(const nt::EntryNotification& event,
+                                      std::string_view childName) {
+  if (event.entry == m_type) {
+    if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+      return true;
+    }
+    if (event.value && event.value->IsString()) {
+      m_typeValue = event.value->GetString();
+    }
+  } else if (event.entry == m_color) {
+    if (event.value && event.value->IsString()) {
+      ConvertColor(event.value->GetString(), &m_colorValue);
+    }
+  } else if (event.entry == m_weight) {
+    if (event.value && event.value->IsDouble()) {
+      m_weightValue = event.value->GetDouble();
+    }
+  } else if (event.entry == m_angle) {
+    if (event.value && event.value->IsDouble()) {
+      m_angleValue = units::degree_t{event.value->GetDouble()};
+    }
+  } else if (event.entry == m_length) {
+    if (event.value && event.value->IsDouble()) {
+      m_lengthValue = units::meter_t{event.value->GetDouble()};
+    }
+  } else {
+    m_group.NTUpdate(event, childName);
+  }
+  return false;
+}
+
+class NTMechanism2DModel::RootModel final : public MechanismRootModel {
+ public:
+  RootModel(NT_Inst inst, std::string_view path, std::string_view name)
+      : m_group{inst, path, name},
+        m_x{nt::GetEntry(inst, fmt::format("{}/x", path))},
+        m_y{nt::GetEntry(inst, fmt::format("{}/y", path))} {}
+
+  const char* GetName() const final { return m_group.GetName(); }
+  void ForEachObject(
+      wpi::function_ref<void(MechanismObjectModel& model)> func) final {
+    m_group.ForEachObject(func);
+  }
+
+  bool NTUpdate(const nt::EntryNotification& event, std::string_view childName);
+
+  frc::Translation2d GetPosition() const override { return m_pos; };
+
+ private:
+  NTMechanismGroupImpl m_group;
+  NT_Entry m_x;
+  NT_Entry m_y;
+  frc::Translation2d m_pos;
+};
+
+bool NTMechanism2DModel::RootModel::NTUpdate(const nt::EntryNotification& event,
+                                             std::string_view childName) {
+  if ((event.flags & NT_NOTIFY_DELETE) != 0 &&
+      (event.entry == m_x || event.entry == m_y)) {
+    return true;
+  } else if (event.entry == m_x) {
+    if (event.value && event.value->IsDouble()) {
+      m_pos = frc::Translation2d{units::meter_t{event.value->GetDouble()},
+                                 m_pos.Y()};
+    }
+  } else if (event.entry == m_y) {
+    if (event.value && event.value->IsDouble()) {
+      m_pos = frc::Translation2d{m_pos.X(),
+                                 units::meter_t{event.value->GetDouble()}};
+    }
+  } else {
+    m_group.NTUpdate(event, childName);
+  }
+  return false;
+}
+
+NTMechanism2DModel::NTMechanism2DModel(std::string_view path)
+    : NTMechanism2DModel{nt::GetDefaultInstance(), path} {}
+
+NTMechanism2DModel::NTMechanism2DModel(NT_Inst inst, std::string_view path)
+    : m_nt{inst},
+      m_path{fmt::format("{}/", path)},
+      m_name{m_nt.GetEntry(fmt::format("{}/.name", path))},
+      m_dimensions{m_nt.GetEntry(fmt::format("{}/dims", path))},
+      m_bgColor{m_nt.GetEntry(fmt::format("{}/backgroundColor", path))},
+      m_dimensionsValue{1_m, 1_m} {
+  m_nt.AddListener(m_path, NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_DELETE |
+                               NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE);
+}
+
+NTMechanism2DModel::~NTMechanism2DModel() = default;
+
+void NTMechanism2DModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    // .name
+    if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+      continue;
+    }
+
+    // dims
+    if (event.entry == m_dimensions) {
+      if (event.value && event.value->IsDoubleArray()) {
+        auto arr = event.value->GetDoubleArray();
+        if (arr.size() == 2) {
+          m_dimensionsValue = frc::Translation2d{units::meter_t{arr[0]},
+                                                 units::meter_t{arr[1]}};
+        }
+      }
+    }
+
+    // backgroundColor
+    if (event.entry == m_bgColor) {
+      if (event.value && event.value->IsString()) {
+        ConvertColor(event.value->GetString(), &m_bgColorValue);
+      }
+    }
+
+    std::string_view name = event.name;
+    if (wpi::starts_with(name, m_path)) {
+      name.remove_prefix(m_path.size());
+      if (name.empty() || name[0] == '.') {
+        continue;
+      }
+      std::string_view childName;
+      std::tie(name, childName) = wpi::split(name, '/');
+      if (childName.empty()) {
+        continue;
+      }
+
+      auto it = std::lower_bound(m_roots.begin(), m_roots.end(), name,
+                                 [](const auto& e, std::string_view name) {
+                                   return e->GetName() < name;
+                                 });
+      bool match = it != m_roots.end() && (*it)->GetName() == name;
+
+      if (event.flags & NT_NOTIFY_NEW) {
+        if (!match) {
+          it = m_roots.emplace(
+              it,
+              std::make_unique<RootModel>(
+                  m_nt.GetInstance(), fmt::format("{}{}", m_path, name), name));
+          match = true;
+        }
+      }
+      if (match) {
+        if ((*it)->NTUpdate(event, childName)) {
+          m_roots.erase(it);
+        }
+      }
+    }
+  }
+}
+
+bool NTMechanism2DModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_name) != NT_UNASSIGNED;
+}
+
+bool NTMechanism2DModel::IsReadOnly() {
+  return false;
+}
+
+void NTMechanism2DModel::ForEachRoot(
+    wpi::function_ref<void(MechanismRootModel& model)> func) {
+  for (auto&& obj : m_roots) {
+    func(*obj);
+  }
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp
new file mode 100644
index 0000000..7936057
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTPIDController.cpp
@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTPIDController.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTPIDControllerModel::NTPIDControllerModel(std::string_view path)
+    : NTPIDControllerModel(nt::GetDefaultInstance(), path) {}
+
+NTPIDControllerModel::NTPIDControllerModel(NT_Inst instance,
+                                           std::string_view path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_controllable(m_nt.GetEntry(fmt::format("{}/.controllable", path))),
+      m_p(m_nt.GetEntry(fmt::format("{}/p", path))),
+      m_i(m_nt.GetEntry(fmt::format("{}/i", path))),
+      m_d(m_nt.GetEntry(fmt::format("{}/d", path))),
+      m_setpoint(m_nt.GetEntry(fmt::format("{}/setpoint", path))),
+      m_pData(fmt::format("NTPIDCtrlP:{}", path)),
+      m_iData(fmt::format("NTPIDCtrlI:{}", path)),
+      m_dData(fmt::format("NTPIDCtrlD:{}", path)),
+      m_setpointData(fmt::format("NTPIDCtrlStpt:{}", path)),
+      m_nameValue(wpi::rsplit(path, '/').second) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_controllable);
+  m_nt.AddListener(m_p);
+  m_nt.AddListener(m_i);
+  m_nt.AddListener(m_d);
+  m_nt.AddListener(m_setpoint);
+}
+
+void NTPIDControllerModel::SetP(double value) {
+  nt::SetEntryValue(m_p, nt::NetworkTableValue::MakeDouble(value));
+}
+
+void NTPIDControllerModel::SetI(double value) {
+  nt::SetEntryValue(m_i, nt::NetworkTableValue::MakeDouble(value));
+}
+
+void NTPIDControllerModel::SetD(double value) {
+  nt::SetEntryValue(m_d, nt::NetworkTableValue::MakeDouble(value));
+}
+
+void NTPIDControllerModel::SetSetpoint(double value) {
+  nt::SetEntryValue(m_setpoint, nt::NetworkTableValue::MakeDouble(value));
+}
+
+void NTPIDControllerModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    } else if (event.entry == m_p) {
+      if (event.value && event.value->IsDouble()) {
+        m_pData.SetValue(event.value->GetDouble());
+      }
+    } else if (event.entry == m_i) {
+      if (event.value && event.value->IsDouble()) {
+        m_iData.SetValue(event.value->GetDouble());
+      }
+    } else if (event.entry == m_d) {
+      if (event.value && event.value->IsDouble()) {
+        m_dData.SetValue(event.value->GetDouble());
+      }
+    } else if (event.entry == m_setpoint) {
+      if (event.value && event.value->IsDouble()) {
+        m_setpointData.SetValue(event.value->GetDouble());
+      }
+    } else if (event.entry == m_controllable) {
+      if (event.value && event.value->IsBoolean()) {
+        m_controllableValue = event.value->GetBoolean();
+      }
+    }
+  }
+}
+
+bool NTPIDControllerModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_setpoint) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTSpeedController.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTSpeedController.cpp
new file mode 100644
index 0000000..3dc351a
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTSpeedController.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTSpeedController.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+
+using namespace glass;
+
+NTSpeedControllerModel::NTSpeedControllerModel(std::string_view path)
+    : NTSpeedControllerModel(nt::GetDefaultInstance(), path) {}
+
+NTSpeedControllerModel::NTSpeedControllerModel(NT_Inst instance,
+                                               std::string_view path)
+    : m_nt(instance),
+      m_value(m_nt.GetEntry(fmt::format("{}/Value", path))),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_controllable(m_nt.GetEntry(fmt::format("{}/.controllable", path))),
+      m_valueData(fmt::format("NT_SpdCtrl:{}", path)),
+      m_nameValue(wpi::rsplit(path, '/').second) {
+  m_nt.AddListener(m_value);
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_controllable);
+}
+
+void NTSpeedControllerModel::SetPercent(double value) {
+  nt::SetEntryValue(m_value, nt::NetworkTableValue::MakeDouble(value));
+}
+
+void NTSpeedControllerModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_value) {
+      if (event.value && event.value->IsDouble()) {
+        m_valueData.SetValue(event.value->GetDouble());
+      }
+    } else if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    } else if (event.entry == m_controllable) {
+      if (event.value && event.value->IsBoolean()) {
+        m_controllableValue = event.value->GetBoolean();
+      }
+    }
+  }
+}
+
+bool NTSpeedControllerModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_value) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp
new file mode 100644
index 0000000..e6a97fa
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTStringChooser.cpp
@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTStringChooser.h"
+
+#include <fmt/format.h>
+
+using namespace glass;
+
+NTStringChooserModel::NTStringChooserModel(std::string_view path)
+    : NTStringChooserModel{nt::GetDefaultInstance(), path} {}
+
+NTStringChooserModel::NTStringChooserModel(NT_Inst inst, std::string_view path)
+    : m_nt{inst},
+      m_default{m_nt.GetEntry(fmt::format("{}/default", path))},
+      m_selected{m_nt.GetEntry(fmt::format("{}/selected", path))},
+      m_active{m_nt.GetEntry(fmt::format("{}/active", path))},
+      m_options{m_nt.GetEntry(fmt::format("{}/options", path))} {
+  m_nt.AddListener(m_default);
+  m_nt.AddListener(m_selected);
+  m_nt.AddListener(m_active);
+  m_nt.AddListener(m_options);
+}
+
+void NTStringChooserModel::SetDefault(std::string_view val) {
+  nt::SetEntryValue(m_default, nt::Value::MakeString(val));
+}
+
+void NTStringChooserModel::SetSelected(std::string_view val) {
+  nt::SetEntryValue(m_selected, nt::Value::MakeString(val));
+}
+
+void NTStringChooserModel::SetActive(std::string_view val) {
+  nt::SetEntryValue(m_active, nt::Value::MakeString(val));
+}
+
+void NTStringChooserModel::SetOptions(wpi::span<const std::string> val) {
+  nt::SetEntryValue(m_options, nt::Value::MakeStringArray(val));
+}
+
+void NTStringChooserModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_default) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_defaultValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_defaultValue = event.value->GetString();
+      }
+    } else if (event.entry == m_selected) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_selectedValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_selectedValue = event.value->GetString();
+      }
+    } else if (event.entry == m_active) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_activeValue.clear();
+      } else if (event.value && event.value->IsString()) {
+        m_activeValue = event.value->GetString();
+      }
+    } else if (event.entry == m_options) {
+      if ((event.flags & NT_NOTIFY_DELETE) != 0) {
+        m_optionsValue.clear();
+      } else if (event.value && event.value->IsStringArray()) {
+        auto arr = event.value->GetStringArray();
+        m_optionsValue.assign(arr.begin(), arr.end());
+      }
+    }
+  }
+}
+
+bool NTStringChooserModel::Exists() {
+  return m_nt.IsConnected() && nt::GetEntryType(m_options) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NTSubsystem.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NTSubsystem.cpp
new file mode 100644
index 0000000..b2bdf8c
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NTSubsystem.cpp
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTSubsystem.h"
+
+#include <fmt/format.h>
+
+using namespace glass;
+
+NTSubsystemModel::NTSubsystemModel(std::string_view path)
+    : NTSubsystemModel(nt::GetDefaultInstance(), path) {}
+
+NTSubsystemModel::NTSubsystemModel(NT_Inst instance, std::string_view path)
+    : m_nt(instance),
+      m_name(m_nt.GetEntry(fmt::format("{}/.name", path))),
+      m_defaultCommand(m_nt.GetEntry(fmt::format("{}/.default", path))),
+      m_currentCommand(m_nt.GetEntry(fmt::format("{}/.command", path))) {
+  m_nt.AddListener(m_name);
+  m_nt.AddListener(m_defaultCommand);
+  m_nt.AddListener(m_currentCommand);
+}
+
+void NTSubsystemModel::Update() {
+  for (auto&& event : m_nt.PollListener()) {
+    if (event.entry == m_name) {
+      if (event.value && event.value->IsString()) {
+        m_nameValue = event.value->GetString();
+      }
+    } else if (event.entry == m_defaultCommand) {
+      if (event.value && event.value->IsString()) {
+        m_defaultCommandValue = event.value->GetString();
+      }
+    } else if (event.entry == m_currentCommand) {
+      if (event.value && event.value->IsString()) {
+        m_currentCommandValue = event.value->GetString();
+      }
+    }
+  }
+}
+
+bool NTSubsystemModel::Exists() {
+  return m_nt.IsConnected() &&
+         nt::GetEntryType(m_defaultCommand) != NT_UNASSIGNED;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp
new file mode 100644
index 0000000..596ef0a
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTables.cpp
@@ -0,0 +1,780 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NetworkTables.h"
+
+#include <networktables/NetworkTableValue.h>
+
+#include <cinttypes>
+#include <cstdio>
+#include <cstring>
+#include <initializer_list>
+#include <memory>
+#include <string_view>
+#include <vector>
+
+#include <fmt/format.h>
+#include <imgui.h>
+#include <ntcore_cpp.h>
+#include <wpi/SmallString.h>
+#include <wpi/SpanExtras.h>
+#include <wpi/StringExtras.h>
+#include <wpi/raw_ostream.h>
+#include <wpi/span.h>
+
+#include "glass/Context.h"
+#include "glass/DataSource.h"
+
+using namespace glass;
+
+static std::string BooleanArrayToString(wpi::span<const int> in) {
+  std::string rv;
+  wpi::raw_string_ostream os{rv};
+  os << '[';
+  bool first = true;
+  for (auto v : in) {
+    if (!first) {
+      os << ',';
+    }
+    first = false;
+    if (v) {
+      os << "true";
+    } else {
+      os << "false";
+    }
+  }
+  os << ']';
+  return rv;
+}
+
+static std::string DoubleArrayToString(wpi::span<const double> in) {
+  return fmt::format("[{:.6f}]", fmt::join(in, ","));
+}
+
+static std::string StringArrayToString(wpi::span<const std::string> in) {
+  std::string rv;
+  wpi::raw_string_ostream os{rv};
+  os << '[';
+  bool first = true;
+  for (auto&& v : in) {
+    if (!first) {
+      os << ',';
+    }
+    first = false;
+    os << '"';
+    os.write_escaped(v);
+    os << '"';
+  }
+  os << ']';
+  return rv;
+}
+
+NetworkTablesModel::NetworkTablesModel()
+    : NetworkTablesModel{nt::GetDefaultInstance()} {}
+
+NetworkTablesModel::NetworkTablesModel(NT_Inst inst)
+    : m_inst{inst}, m_poller{nt::CreateEntryListenerPoller(inst)} {
+  nt::AddPolledEntryListener(m_poller, "",
+                             NT_NOTIFY_LOCAL | NT_NOTIFY_NEW |
+                                 NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
+                                 NT_NOTIFY_FLAGS | NT_NOTIFY_IMMEDIATE);
+}
+
+NetworkTablesModel::~NetworkTablesModel() {
+  nt::DestroyEntryListenerPoller(m_poller);
+}
+
+NetworkTablesModel::Entry::Entry(nt::EntryNotification&& event)
+    : entry{event.entry},
+      name{std::move(event.name)},
+      value{std::move(event.value)},
+      flags{nt::GetEntryFlags(event.entry)} {
+  UpdateValue();
+}
+
+void NetworkTablesModel::Entry::UpdateValue() {
+  switch (value->type()) {
+    case NT_BOOLEAN:
+      if (!source) {
+        source = std::make_unique<DataSource>(fmt::format("NT:{}", name));
+      }
+      source->SetValue(value->GetBoolean() ? 1 : 0);
+      source->SetDigital(true);
+      break;
+    case NT_DOUBLE:
+      if (!source) {
+        source = std::make_unique<DataSource>(fmt::format("NT:{}", name));
+      }
+      source->SetValue(value->GetDouble());
+      source->SetDigital(false);
+      break;
+    case NT_BOOLEAN_ARRAY:
+      valueStr = BooleanArrayToString(value->GetBooleanArray());
+      break;
+    case NT_DOUBLE_ARRAY:
+      valueStr = DoubleArrayToString(value->GetDoubleArray());
+      break;
+    case NT_STRING_ARRAY:
+      valueStr = StringArrayToString(value->GetStringArray());
+      break;
+    default:
+      break;
+  }
+}
+
+void NetworkTablesModel::Update() {
+  bool timedOut = false;
+  bool updateTree = false;
+  for (auto&& event : nt::PollEntryListener(m_poller, 0, &timedOut)) {
+    auto& entry = m_entries[event.entry];
+    if (event.flags & NT_NOTIFY_NEW) {
+      if (!entry) {
+        entry = std::make_unique<Entry>(std::move(event));
+        m_sortedEntries.emplace_back(entry.get());
+        updateTree = true;
+        continue;
+      }
+    }
+    if (!entry) {
+      continue;
+    }
+    if (event.flags & NT_NOTIFY_DELETE) {
+      auto it = std::find(m_sortedEntries.begin(), m_sortedEntries.end(),
+                          entry.get());
+      // will be removed completely below
+      if (it != m_sortedEntries.end()) {
+        *it = nullptr;
+      }
+      m_entries.erase(event.entry);
+      updateTree = true;
+      continue;
+    }
+    if (event.flags & NT_NOTIFY_UPDATE) {
+      entry->value = std::move(event.value);
+      entry->UpdateValue();
+    }
+    if (event.flags & NT_NOTIFY_FLAGS) {
+      entry->flags = nt::GetEntryFlags(event.entry);
+    }
+  }
+
+  // shortcut common case (updates)
+  if (!updateTree) {
+    return;
+  }
+
+  // remove deleted entries
+  m_sortedEntries.erase(
+      std::remove(m_sortedEntries.begin(), m_sortedEntries.end(), nullptr),
+      m_sortedEntries.end());
+
+  // sort by name
+  std::sort(m_sortedEntries.begin(), m_sortedEntries.end(),
+            [](const auto& a, const auto& b) { return a->name < b->name; });
+
+  // rebuild tree
+  m_root.clear();
+  wpi::SmallVector<std::string_view, 16> parts;
+  for (auto& entry : m_sortedEntries) {
+    parts.clear();
+    wpi::split(entry->name, parts, '/', -1, false);
+
+    // ignore a raw "/" key
+    if (parts.empty()) {
+      continue;
+    }
+
+    // get to leaf
+    auto nodes = &m_root;
+    for (auto part : wpi::drop_back(wpi::span{parts.begin(), parts.end()})) {
+      auto it =
+          std::find_if(nodes->begin(), nodes->end(),
+                       [&](const auto& node) { return node.name == part; });
+      if (it == nodes->end()) {
+        nodes->emplace_back(part);
+        // path is from the beginning of the string to the end of the current
+        // part; this works because part is a reference to the internals of
+        // entry->name
+        nodes->back().path.assign(
+            entry->name.data(), part.data() + part.size() - entry->name.data());
+        it = nodes->end() - 1;
+      }
+      nodes = &it->children;
+    }
+
+    auto it = std::find_if(nodes->begin(), nodes->end(), [&](const auto& node) {
+      return node.name == parts.back();
+    });
+    if (it == nodes->end()) {
+      nodes->emplace_back(parts.back());
+      // no need to set path, as it's identical to entry->name
+      it = nodes->end() - 1;
+    }
+    it->entry = entry;
+  }
+}
+
+bool NetworkTablesModel::Exists() {
+  return nt::IsConnected(m_inst);
+}
+
+static std::shared_ptr<nt::Value> StringToBooleanArray(std::string_view in) {
+  in = wpi::trim(in);
+  if (in.empty()) {
+    return nt::NetworkTableValue::MakeBooleanArray(
+        std::initializer_list<bool>{});
+  }
+  if (in.front() == '[') {
+    in.remove_prefix(1);
+  }
+  if (in.back() == ']') {
+    in.remove_suffix(1);
+  }
+  in = wpi::trim(in);
+
+  wpi::SmallVector<std::string_view, 16> inSplit;
+  wpi::SmallVector<int, 16> out;
+
+  wpi::split(in, inSplit, ',', -1, false);
+  for (auto val : inSplit) {
+    val = wpi::trim(val);
+    if (wpi::equals_lower(val, "true")) {
+      out.emplace_back(1);
+    } else if (wpi::equals_lower(val, "false")) {
+      out.emplace_back(0);
+    } else {
+      fmt::print(stderr,
+                 "GUI: NetworkTables: Could not understand value '{}'\n", val);
+      return nullptr;
+    }
+  }
+
+  return nt::NetworkTableValue::MakeBooleanArray(out);
+}
+
+static std::shared_ptr<nt::Value> StringToDoubleArray(std::string_view in) {
+  in = wpi::trim(in);
+  if (in.empty()) {
+    return nt::NetworkTableValue::MakeDoubleArray(
+        std::initializer_list<double>{});
+  }
+  if (in.front() == '[') {
+    in.remove_prefix(1);
+  }
+  if (in.back() == ']') {
+    in.remove_suffix(1);
+  }
+  in = wpi::trim(in);
+
+  wpi::SmallVector<std::string_view, 16> inSplit;
+  wpi::SmallVector<double, 16> out;
+
+  wpi::split(in, inSplit, ',', -1, false);
+  for (auto val : inSplit) {
+    if (auto num = wpi::parse_float<double>(wpi::trim(val))) {
+      out.emplace_back(num.value());
+    } else {
+      fmt::print(stderr,
+                 "GUI: NetworkTables: Could not understand value '{}'\n", val);
+      return nullptr;
+    }
+  }
+
+  return nt::NetworkTableValue::MakeDoubleArray(out);
+}
+
+static int fromxdigit(char ch) {
+  if (ch >= 'a' && ch <= 'f') {
+    return (ch - 'a' + 10);
+  } else if (ch >= 'A' && ch <= 'F') {
+    return (ch - 'A' + 10);
+  } else {
+    return ch - '0';
+  }
+}
+
+static std::string_view UnescapeString(std::string_view source,
+                                       wpi::SmallVectorImpl<char>& buf) {
+  assert(source.size() >= 2 && source.front() == '"' && source.back() == '"');
+  buf.clear();
+  buf.reserve(source.size() - 2);
+  for (auto s = source.begin() + 1, end = source.end() - 1; s != end; ++s) {
+    if (*s != '\\') {
+      buf.push_back(*s);
+      continue;
+    }
+    switch (*++s) {
+      case 't':
+        buf.push_back('\t');
+        break;
+      case 'n':
+        buf.push_back('\n');
+        break;
+      case 'x': {
+        if (!isxdigit(*(s + 1))) {
+          buf.push_back('x');  // treat it like a unknown escape
+          break;
+        }
+        int ch = fromxdigit(*++s);
+        if (std::isxdigit(*(s + 1))) {
+          ch <<= 4;
+          ch |= fromxdigit(*++s);
+        }
+        buf.push_back(static_cast<char>(ch));
+        break;
+      }
+      default:
+        buf.push_back(*s);
+        break;
+    }
+  }
+  return {buf.data(), buf.size()};
+}
+
+static std::shared_ptr<nt::Value> StringToStringArray(std::string_view in) {
+  in = wpi::trim(in);
+  if (in.empty()) {
+    return nt::NetworkTableValue::MakeStringArray(
+        std::initializer_list<std::string>{});
+  }
+  if (in.front() == '[') {
+    in.remove_prefix(1);
+  }
+  if (in.back() == ']') {
+    in.remove_suffix(1);
+  }
+  in = wpi::trim(in);
+
+  wpi::SmallVector<std::string_view, 16> inSplit;
+  std::vector<std::string> out;
+  wpi::SmallString<32> buf;
+
+  wpi::split(in, inSplit, ',', -1, false);
+  for (auto val : inSplit) {
+    val = wpi::trim(val);
+    if (val.empty()) {
+      continue;
+    }
+    if (val.front() != '"' || val.back() != '"') {
+      fmt::print(stderr,
+                 "GUI: NetworkTables: Could not understand value '{}'\n", val);
+      return nullptr;
+    }
+    out.emplace_back(UnescapeString(val, buf));
+  }
+
+  return nt::NetworkTableValue::MakeStringArray(std::move(out));
+}
+
+static void EmitEntryValueReadonly(NetworkTablesModel::Entry& entry) {
+  auto& val = entry.value;
+  if (!val) {
+    return;
+  }
+
+  switch (val->type()) {
+    case NT_BOOLEAN:
+      ImGui::LabelText("boolean", "%s", val->GetBoolean() ? "true" : "false");
+      break;
+    case NT_DOUBLE:
+      ImGui::LabelText("double", "%.6f", val->GetDouble());
+      break;
+    case NT_STRING: {
+      // GetString() comes from a std::string, so it's null terminated
+      ImGui::LabelText("string", "%s", val->GetString().data());
+      break;
+    }
+    case NT_BOOLEAN_ARRAY:
+      ImGui::LabelText("boolean[]", "%s", entry.valueStr.c_str());
+      break;
+    case NT_DOUBLE_ARRAY:
+      ImGui::LabelText("double[]", "%s", entry.valueStr.c_str());
+      break;
+    case NT_STRING_ARRAY:
+      ImGui::LabelText("string[]", "%s", entry.valueStr.c_str());
+      break;
+    case NT_RAW:
+      ImGui::LabelText("raw", "[...]");
+      break;
+    case NT_RPC:
+      ImGui::LabelText("rpc", "[...]");
+      break;
+    default:
+      ImGui::LabelText("other", "?");
+      break;
+  }
+}
+
+static constexpr size_t kTextBufferSize = 4096;
+
+static char* GetTextBuffer(std::string_view in) {
+  static char textBuffer[kTextBufferSize];
+  size_t len = (std::min)(in.size(), kTextBufferSize - 1);
+  std::memcpy(textBuffer, in.data(), len);
+  textBuffer[len] = '\0';
+  return textBuffer;
+}
+
+static void EmitEntryValueEditable(NetworkTablesModel::Entry& entry) {
+  auto& val = entry.value;
+  if (!val) {
+    return;
+  }
+
+  ImGui::PushID(entry.name.c_str());
+  switch (val->type()) {
+    case NT_BOOLEAN: {
+      static const char* boolOptions[] = {"false", "true"};
+      int v = val->GetBoolean() ? 1 : 0;
+      if (ImGui::Combo("boolean", &v, boolOptions, 2)) {
+        nt::SetEntryValue(entry.entry, nt::NetworkTableValue::MakeBoolean(v));
+      }
+      break;
+    }
+    case NT_DOUBLE: {
+      double v = val->GetDouble();
+      if (ImGui::InputDouble("double", &v, 0, 0, "%.6f",
+                             ImGuiInputTextFlags_EnterReturnsTrue)) {
+        nt::SetEntryValue(entry.entry, nt::NetworkTableValue::MakeDouble(v));
+      }
+      break;
+    }
+    case NT_STRING: {
+      char* v = GetTextBuffer(val->GetString());
+      if (ImGui::InputText("string", v, kTextBufferSize,
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+        nt::SetEntryValue(entry.entry, nt::NetworkTableValue::MakeString(v));
+      }
+      break;
+    }
+    case NT_BOOLEAN_ARRAY: {
+      char* v = GetTextBuffer(entry.valueStr);
+      if (ImGui::InputText("boolean[]", v, kTextBufferSize,
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+        if (auto outv = StringToBooleanArray(v)) {
+          nt::SetEntryValue(entry.entry, std::move(outv));
+        }
+      }
+      break;
+    }
+    case NT_DOUBLE_ARRAY: {
+      char* v = GetTextBuffer(entry.valueStr);
+      if (ImGui::InputText("double[]", v, kTextBufferSize,
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+        if (auto outv = StringToDoubleArray(v)) {
+          nt::SetEntryValue(entry.entry, std::move(outv));
+        }
+      }
+      break;
+    }
+    case NT_STRING_ARRAY: {
+      char* v = GetTextBuffer(entry.valueStr);
+      if (ImGui::InputText("string[]", v, kTextBufferSize,
+                           ImGuiInputTextFlags_EnterReturnsTrue)) {
+        if (auto outv = StringToStringArray(v)) {
+          nt::SetEntryValue(entry.entry, std::move(outv));
+        }
+      }
+      break;
+    }
+    case NT_RAW:
+      ImGui::LabelText("raw", "[...]");
+      break;
+    case NT_RPC:
+      ImGui::LabelText("rpc", "[...]");
+      break;
+    default:
+      ImGui::LabelText("other", "?");
+      break;
+  }
+  ImGui::PopID();
+}
+
+static void EmitParentContextMenu(const std::string& path,
+                                  NetworkTablesFlags flags) {
+  // Workaround https://github.com/ocornut/imgui/issues/331
+  bool openWarningPopup = false;
+  static char nameBuffer[kTextBufferSize];
+  if (ImGui::BeginPopupContextItem(path.c_str())) {
+    ImGui::Text("%s", path.c_str());
+    ImGui::Separator();
+
+    if (ImGui::BeginMenu("Add new...")) {
+      if (ImGui::IsWindowAppearing()) {
+        nameBuffer[0] = '\0';
+      }
+
+      ImGui::InputTextWithHint("New item name", "example", nameBuffer,
+                               kTextBufferSize);
+      std::string fullNewPath;
+      if (path == "/") {
+        fullNewPath = path + nameBuffer;
+      } else {
+        fullNewPath = fmt::format("{}/{}", path, nameBuffer);
+      }
+
+      ImGui::Text("Adding: %s", fullNewPath.c_str());
+      ImGui::Separator();
+      auto entry = nt::GetEntry(nt::GetDefaultInstance(), fullNewPath);
+      bool enabled = (flags & NetworkTablesFlags_CreateNoncanonicalKeys ||
+                      nameBuffer[0] != '\0') &&
+                     nt::GetEntryType(entry) == NT_Type::NT_UNASSIGNED;
+      if (ImGui::MenuItem("string", nullptr, false, enabled)) {
+        if (!nt::SetEntryValue(entry, nt::Value::MakeString(""))) {
+          openWarningPopup = true;
+        }
+      }
+      if (ImGui::MenuItem("double", nullptr, false, enabled)) {
+        if (!nt::SetEntryValue(entry, nt::Value::MakeDouble(0.0))) {
+          openWarningPopup = true;
+        }
+      }
+      if (ImGui::MenuItem("boolean", nullptr, false, enabled)) {
+        if (!nt::SetEntryValue(entry, nt::Value::MakeBoolean(false))) {
+          openWarningPopup = true;
+        }
+      }
+      if (ImGui::MenuItem("string[]", nullptr, false, enabled)) {
+        if (!nt::SetEntryValue(entry, nt::Value::MakeStringArray({""}))) {
+          openWarningPopup = true;
+        }
+      }
+      if (ImGui::MenuItem("double[]", nullptr, false, enabled)) {
+        if (!nt::SetEntryValue(entry, nt::Value::MakeDoubleArray({0.0}))) {
+          openWarningPopup = true;
+        }
+      }
+      if (ImGui::MenuItem("boolean[]", nullptr, false, enabled)) {
+        if (!nt::SetEntryValue(entry, nt::Value::MakeBooleanArray({false}))) {
+          openWarningPopup = true;
+        }
+      }
+
+      ImGui::EndMenu();
+    }
+
+    ImGui::Separator();
+    if (ImGui::MenuItem("Remove All")) {
+      for (auto&& entry : nt::GetEntries(nt::GetDefaultInstance(), path, 0)) {
+        nt::DeleteEntry(entry);
+      }
+    }
+    ImGui::EndPopup();
+  }
+
+  // Workaround https://github.com/ocornut/imgui/issues/331
+  if (openWarningPopup) {
+    ImGui::OpenPopup("Value exists");
+  }
+  if (ImGui::BeginPopupModal("Value exists", nullptr,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    ImGui::Text("The provided name %s already exists in the tree!", nameBuffer);
+    ImGui::Separator();
+
+    if (ImGui::Button("OK", ImVec2(120, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::SetItemDefaultFocus();
+    ImGui::EndPopup();
+  }
+}
+
+static void EmitEntry(NetworkTablesModel::Entry& entry, const char* name,
+                      NetworkTablesFlags flags) {
+  if (entry.source) {
+    ImGui::Selectable(name);
+    entry.source->EmitDrag();
+  } else {
+    ImGui::Text("%s", name);
+  }
+  if (ImGui::BeginPopupContextItem(entry.name.c_str())) {
+    ImGui::Text("%s", entry.name.c_str());
+    ImGui::Separator();
+    if (ImGui::MenuItem("Remove")) {
+      nt::DeleteEntry(entry.entry);
+    }
+    ImGui::EndPopup();
+  }
+  ImGui::NextColumn();
+
+  if (flags & NetworkTablesFlags_ReadOnly) {
+    EmitEntryValueReadonly(entry);
+  } else {
+    EmitEntryValueEditable(entry);
+  }
+  ImGui::NextColumn();
+
+  if (flags & NetworkTablesFlags_ShowFlags) {
+    if ((entry.flags & NT_PERSISTENT) != 0) {
+      ImGui::Text("Persistent");
+    } else if (entry.flags != 0) {
+      ImGui::Text("%02x", entry.flags);
+    }
+    ImGui::NextColumn();
+  }
+
+  if (flags & NetworkTablesFlags_ShowTimestamp) {
+    if (entry.value) {
+      ImGui::Text("%f", (entry.value->last_change() * 1.0e-6) -
+                            (GetZeroTime() * 1.0e-6));
+    } else {
+      ImGui::TextUnformatted("");
+    }
+    ImGui::NextColumn();
+  }
+  ImGui::Separator();
+}
+
+static void EmitTree(const std::vector<NetworkTablesModel::TreeNode>& tree,
+                     NetworkTablesFlags flags) {
+  for (auto&& node : tree) {
+    if (node.entry) {
+      EmitEntry(*node.entry, node.name.c_str(), flags);
+    }
+
+    if (!node.children.empty()) {
+      bool open =
+          TreeNodeEx(node.name.c_str(), ImGuiTreeNodeFlags_SpanFullWidth);
+      EmitParentContextMenu(node.path, flags);
+      ImGui::NextColumn();
+      ImGui::NextColumn();
+      if (flags & NetworkTablesFlags_ShowFlags) {
+        ImGui::NextColumn();
+      }
+      if (flags & NetworkTablesFlags_ShowTimestamp) {
+        ImGui::NextColumn();
+      }
+      ImGui::Separator();
+      if (open) {
+        EmitTree(node.children, flags);
+        TreePop();
+      }
+    }
+  }
+}
+
+void glass::DisplayNetworkTables(NetworkTablesModel* model,
+                                 NetworkTablesFlags flags) {
+  auto inst = model->GetInstance();
+
+  if (flags & NetworkTablesFlags_ShowConnections) {
+    if (CollapsingHeader("Connections")) {
+      ImGui::Columns(4, "connections");
+      ImGui::Text("Id");
+      ImGui::NextColumn();
+      ImGui::Text("Address");
+      ImGui::NextColumn();
+      ImGui::Text("Updated");
+      ImGui::NextColumn();
+      ImGui::Text("Proto");
+      ImGui::NextColumn();
+      ImGui::Separator();
+      for (auto&& i : nt::GetConnections(inst)) {
+        ImGui::Text("%s", i.remote_id.c_str());
+        ImGui::NextColumn();
+        ImGui::Text("%s", i.remote_ip.c_str());
+        ImGui::NextColumn();
+        ImGui::Text("%llu",
+                    static_cast<unsigned long long>(  // NOLINT(runtime/int)
+                        i.last_update));
+        ImGui::NextColumn();
+        ImGui::Text("%d.%d", i.protocol_version >> 8,
+                    i.protocol_version & 0xff);
+        ImGui::NextColumn();
+      }
+      ImGui::Columns();
+    }
+
+    if (!CollapsingHeader("Values", ImGuiTreeNodeFlags_DefaultOpen)) {
+      return;
+    }
+  }
+
+  const bool showFlags = (flags & NetworkTablesFlags_ShowFlags);
+  const bool showTimestamp = (flags & NetworkTablesFlags_ShowTimestamp);
+
+  static bool first = true;
+  ImGui::Columns(2 + (showFlags ? 1 : 0) + (showTimestamp ? 1 : 0), "values");
+  if (first) {
+    ImGui::SetColumnWidth(-1, 0.5f * ImGui::GetWindowWidth());
+  }
+  ImGui::Text("Name");
+  EmitParentContextMenu("/", flags);
+  ImGui::NextColumn();
+  ImGui::Text("Value");
+  ImGui::NextColumn();
+  if (showFlags) {
+    if (first) {
+      ImGui::SetColumnWidth(-1, 12 * ImGui::GetFontSize());
+    }
+    ImGui::Text("Flags");
+    ImGui::NextColumn();
+  }
+  if (showTimestamp) {
+    ImGui::Text("Changed");
+    ImGui::NextColumn();
+  }
+  ImGui::Separator();
+  first = false;
+
+  if (flags & NetworkTablesFlags_TreeView) {
+    EmitTree(model->GetTreeRoot(), flags);
+  } else {
+    for (auto entry : model->GetEntries()) {
+      EmitEntry(*entry, entry->name.c_str(), flags);
+    }
+  }
+  ImGui::Columns();
+}
+
+void NetworkTablesFlagsSettings::Update() {
+  if (!m_pTreeView) {
+    auto& storage = GetStorage();
+    m_pTreeView = storage.GetBoolRef(
+        "tree", m_defaultFlags & NetworkTablesFlags_TreeView);
+    m_pShowConnections = storage.GetBoolRef(
+        "connections", m_defaultFlags & NetworkTablesFlags_ShowConnections);
+    m_pShowFlags = storage.GetBoolRef(
+        "flags", m_defaultFlags & NetworkTablesFlags_ShowFlags);
+    m_pShowTimestamp = storage.GetBoolRef(
+        "timestamp", m_defaultFlags & NetworkTablesFlags_ShowTimestamp);
+    m_pCreateNoncanonicalKeys = storage.GetBoolRef(
+        "createNonCanonical",
+        m_defaultFlags & NetworkTablesFlags_CreateNoncanonicalKeys);
+  }
+
+  m_flags &=
+      ~(NetworkTablesFlags_TreeView | NetworkTablesFlags_ShowConnections |
+        NetworkTablesFlags_ShowFlags | NetworkTablesFlags_ShowTimestamp |
+        NetworkTablesFlags_CreateNoncanonicalKeys);
+  m_flags |=
+      (*m_pTreeView ? NetworkTablesFlags_TreeView : 0) |
+      (*m_pShowConnections ? NetworkTablesFlags_ShowConnections : 0) |
+      (*m_pShowFlags ? NetworkTablesFlags_ShowFlags : 0) |
+      (*m_pShowTimestamp ? NetworkTablesFlags_ShowTimestamp : 0) |
+      (*m_pCreateNoncanonicalKeys ? NetworkTablesFlags_CreateNoncanonicalKeys
+                                  : 0);
+}
+
+void NetworkTablesFlagsSettings::DisplayMenu() {
+  if (!m_pTreeView) {
+    return;
+  }
+  ImGui::MenuItem("Tree View", "", m_pTreeView);
+  ImGui::MenuItem("Show Connections", "", m_pShowConnections);
+  ImGui::MenuItem("Show Flags", "", m_pShowFlags);
+  ImGui::MenuItem("Show Timestamp", "", m_pShowTimestamp);
+  ImGui::Separator();
+  ImGui::MenuItem("Allow creation of non-canonical keys", "",
+                  m_pCreateNoncanonicalKeys);
+}
+
+void NetworkTablesView::Display() {
+  m_flags.Update();
+  if (ImGui::BeginPopupContextItem()) {
+    m_flags.DisplayMenu();
+    ImGui::EndPopup();
+  }
+  DisplayNetworkTables(m_model, m_flags.GetFlags());
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesHelper.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesHelper.cpp
new file mode 100644
index 0000000..5cb5bbc
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesHelper.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NetworkTablesHelper.h"
+
+using namespace glass;
+
+NetworkTablesHelper::NetworkTablesHelper(NT_Inst inst)
+    : m_inst{inst}, m_poller{nt::CreateEntryListenerPoller(inst)} {}
+
+NetworkTablesHelper::~NetworkTablesHelper() {
+  nt::DestroyEntryListenerPoller(m_poller);
+}
+
+bool NetworkTablesHelper::IsConnected() const {
+  return nt::GetNetworkMode(m_inst) == NT_NET_MODE_SERVER ||
+         nt::IsConnected(m_inst);
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp
new file mode 100644
index 0000000..8d991cb
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesProvider.cpp
@@ -0,0 +1,205 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NetworkTablesProvider.h"
+
+#include <algorithm>
+
+#include <fmt/format.h>
+#include <ntcore_cpp.h>
+#include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
+#include <wpigui.h>
+
+using namespace glass;
+
+NetworkTablesProvider::NetworkTablesProvider(std::string_view iniName)
+    : NetworkTablesProvider{iniName, nt::GetDefaultInstance()} {}
+
+NetworkTablesProvider::NetworkTablesProvider(std::string_view iniName,
+                                             NT_Inst inst)
+    : Provider{fmt::format("{}Window", iniName)},
+      m_nt{inst},
+      m_typeCache{iniName} {
+  m_nt.AddListener("", NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_DELETE |
+                           NT_NOTIFY_IMMEDIATE);
+}
+
+void NetworkTablesProvider::GlobalInit() {
+  Provider::GlobalInit();
+  wpi::gui::AddInit([this] { m_typeCache.Initialize(); });
+}
+
+void NetworkTablesProvider::DisplayMenu() {
+  wpi::SmallVector<std::string_view, 6> path;
+  wpi::SmallString<64> name;
+  for (auto&& entry : m_viewEntries) {
+    path.clear();
+    wpi::split(entry->name, path, '/', -1, false);
+
+    bool fullDepth = true;
+    int depth = 0;
+    for (; depth < (static_cast<int>(path.size()) - 1); ++depth) {
+      name = path[depth];
+      if (!ImGui::BeginMenu(name.c_str())) {
+        fullDepth = false;
+        break;
+      }
+    }
+
+    if (fullDepth) {
+      bool visible = entry->window && entry->window->IsVisible();
+      bool wasVisible = visible;
+      // FIXME: enabled?
+      // data is the last item, so is guaranteed to be null-terminated
+      ImGui::MenuItem(path.back().data(), nullptr, &visible, true);
+      if (!wasVisible && visible) {
+        Show(entry.get(), entry->window);
+      } else if (wasVisible && !visible && entry->window) {
+        entry->window->SetVisible(false);
+      }
+    }
+
+    for (; depth > 0; --depth) {
+      ImGui::EndMenu();
+    }
+  }
+}
+
+void NetworkTablesProvider::Update() {
+  Provider::Update();
+
+  // add/remove entries from NT changes
+  for (auto&& event : m_nt.PollListener()) {
+    // look for .type fields
+    std::string_view eventName{event.name};
+    if (!wpi::ends_with(eventName, "/.type") || !event.value ||
+        !event.value->IsString()) {
+      continue;
+    }
+    auto tableName = wpi::drop_back(eventName, 6);
+
+    // only handle ones where we have a builder
+    auto builderIt = m_typeMap.find(event.value->GetString());
+    if (builderIt == m_typeMap.end()) {
+      continue;
+    }
+
+    if (event.flags & NT_NOTIFY_DELETE) {
+      auto it = std::find_if(
+          m_viewEntries.begin(), m_viewEntries.end(), [&](const auto& elem) {
+            return static_cast<Entry*>(elem->modelEntry)->typeEntry ==
+                   event.entry;
+          });
+      if (it != m_viewEntries.end()) {
+        m_viewEntries.erase(it);
+      }
+    } else if (event.flags & NT_NOTIFY_NEW) {
+      GetOrCreateView(builderIt->second, event.entry, tableName);
+      // cache the type
+      m_typeCache[tableName].SetName(event.value->GetString());
+    }
+  }
+
+  // check for visible windows that need displays (typically this is due to
+  // file loading)
+  for (auto&& window : m_windows) {
+    if (!window->IsVisible() || window->HasView()) {
+      continue;
+    }
+    auto id = window->GetId();
+    auto typeIt = m_typeCache.find(id);
+    if (typeIt == m_typeCache.end()) {
+      continue;
+    }
+
+    // only handle ones where we have a builder
+    auto builderIt = m_typeMap.find(typeIt->second.GetName());
+    if (builderIt == m_typeMap.end()) {
+      continue;
+    }
+
+    auto entry = GetOrCreateView(
+        builderIt->second,
+        nt::GetEntry(m_nt.GetInstance(), fmt::format("{}/.type", id)), id);
+    if (entry) {
+      Show(entry, window.get());
+    }
+  }
+}
+
+void NetworkTablesProvider::Register(std::string_view typeName,
+                                     CreateModelFunc createModel,
+                                     CreateViewFunc createView) {
+  m_typeMap[typeName] = Builder{std::move(createModel), std::move(createView)};
+}
+
+void NetworkTablesProvider::Show(ViewEntry* entry, Window* window) {
+  // if there's already a window, just show it
+  if (entry->window) {
+    entry->window->SetVisible(true);
+    return;
+  }
+
+  // get or create model
+  if (!entry->modelEntry->model) {
+    entry->modelEntry->model =
+        entry->modelEntry->createModel(m_nt.GetInstance(), entry->name.c_str());
+  }
+  if (!entry->modelEntry->model) {
+    return;
+  }
+
+  // the window might exist and we're just not associated to it yet
+  if (!window) {
+    window = GetOrAddWindow(entry->name, true);
+  }
+  if (!window) {
+    return;
+  }
+  if (wpi::starts_with(entry->name, "/SmartDashboard/")) {
+    window->SetDefaultName(
+        fmt::format("{} (SmartDashboard)", wpi::drop_front(entry->name, 16)));
+  }
+  entry->window = window;
+
+  // create view
+  auto view = entry->createView(window, entry->modelEntry->model.get(),
+                                entry->name.c_str());
+  if (!view) {
+    return;
+  }
+  window->SetView(std::move(view));
+
+  entry->window->SetVisible(true);
+}
+
+NetworkTablesProvider::ViewEntry* NetworkTablesProvider::GetOrCreateView(
+    const Builder& builder, NT_Entry typeEntry, std::string_view name) {
+  // get view entry if it already exists
+  auto viewIt = FindViewEntry(name);
+  if (viewIt != m_viewEntries.end() && (*viewIt)->name == name) {
+    // make sure typeEntry is set in model
+    static_cast<Entry*>((*viewIt)->modelEntry)->typeEntry = typeEntry;
+    return viewIt->get();
+  }
+
+  // get or create model entry
+  auto modelIt = FindModelEntry(name);
+  if (modelIt != m_modelEntries.end() && (*modelIt)->name == name) {
+    static_cast<Entry*>(modelIt->get())->typeEntry = typeEntry;
+  } else {
+    modelIt = m_modelEntries.emplace(
+        modelIt, std::make_unique<Entry>(typeEntry, name, builder));
+  }
+
+  // create new view entry
+  viewIt = m_viewEntries.emplace(
+      viewIt,
+      std::make_unique<ViewEntry>(
+          name, modelIt->get(), [](Model*, const char*) { return true; },
+          builder.createView));
+
+  return viewIt->get();
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesSettings.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesSettings.cpp
new file mode 100644
index 0000000..28f4de4
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/NetworkTablesSettings.cpp
@@ -0,0 +1,133 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NetworkTablesSettings.h"
+
+#include <optional>
+#include <string_view>
+#include <utility>
+
+#include <imgui.h>
+#include <imgui_stdlib.h>
+#include <ntcore_cpp.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+
+#include "glass/Context.h"
+
+using namespace glass;
+
+void NetworkTablesSettings::Thread::Main() {
+  while (m_active) {
+    // wait to be woken up
+    std::unique_lock lock(m_mutex);
+    m_cond.wait(lock, [&] { return !m_active || m_restart; });
+    if (!m_active) {
+      break;
+    }
+
+    // clear restart flag
+    m_restart = false;
+
+    int mode;
+    bool dsClient;
+
+    do {
+      mode = m_mode;
+      dsClient = m_dsClient;
+
+      // release lock while stopping to avoid blocking GUI
+      lock.unlock();
+
+      // if just changing servers in client mode, no need to stop and restart
+      unsigned int curMode = nt::GetNetworkMode(m_inst);
+      if (mode != 1 || (curMode & NT_NET_MODE_SERVER) != 0) {
+        nt::StopClient(m_inst);
+        nt::StopServer(m_inst);
+        nt::StopLocal(m_inst);
+      }
+
+      if (m_mode != 1 || !dsClient) {
+        nt::StopDSClient(m_inst);
+      }
+
+      lock.lock();
+    } while (mode != m_mode || dsClient != m_dsClient);
+
+    if (m_mode == 1) {
+      std::string_view serverTeam{m_serverTeam};
+      std::optional<unsigned int> team;
+      if (!wpi::contains(serverTeam, '.') &&
+          (team = wpi::parse_integer<unsigned int>(serverTeam, 10))) {
+        nt::StartClientTeam(m_inst, team.value(), NT_DEFAULT_PORT);
+      } else {
+        wpi::SmallVector<std::string_view, 4> serverNames;
+        wpi::SmallVector<std::pair<std::string_view, unsigned int>, 4> servers;
+        wpi::split(serverTeam, serverNames, ',', -1, false);
+        for (auto&& serverName : serverNames) {
+          servers.emplace_back(serverName, NT_DEFAULT_PORT);
+        }
+        nt::StartClient(m_inst, servers);
+      }
+
+      if (m_dsClient) {
+        nt::StartDSClient(m_inst, NT_DEFAULT_PORT);
+      }
+    } else if (m_mode == 2) {
+      nt::StartServer(m_inst, m_iniName.c_str(), m_listenAddress.c_str(),
+                      NT_DEFAULT_PORT);
+    }
+  }
+}
+
+NetworkTablesSettings::NetworkTablesSettings(NT_Inst inst,
+                                             const char* storageName) {
+  auto& storage = glass::GetStorage(storageName);
+  m_pMode = storage.GetIntRef("mode");
+  m_pIniName = storage.GetStringRef("iniName", "networktables.ini");
+  m_pServerTeam = storage.GetStringRef("serverTeam");
+  m_pListenAddress = storage.GetStringRef("listenAddress");
+  m_pDsClient = storage.GetBoolRef("dsClient", true);
+
+  m_thread.Start(inst);
+}
+
+void NetworkTablesSettings::Update() {
+  if (!m_restart) {
+    return;
+  }
+  m_restart = false;
+
+  // do actual operation on thread
+  auto thr = m_thread.GetThread();
+  thr->m_restart = true;
+  thr->m_mode = *m_pMode;
+  thr->m_iniName = *m_pIniName;
+  thr->m_serverTeam = *m_pServerTeam;
+  thr->m_listenAddress = *m_pListenAddress;
+  thr->m_dsClient = *m_pDsClient;
+  thr->m_cond.notify_one();
+}
+
+bool NetworkTablesSettings::Display() {
+  static const char* modeOptions[] = {"Disabled", "Client", "Server"};
+  ImGui::Combo("Mode", m_pMode, modeOptions, m_serverOption ? 3 : 2);
+  switch (*m_pMode) {
+    case 1:
+      ImGui::InputText("Team/IP", m_pServerTeam);
+      ImGui::Checkbox("Get Address from DS", m_pDsClient);
+      break;
+    case 2:
+      ImGui::InputText("Listen Address", m_pListenAddress);
+      ImGui::InputText("ini Filename", m_pIniName);
+      break;
+    default:
+      break;
+  }
+  if (ImGui::Button("Apply")) {
+    m_restart = true;
+    return true;
+  }
+  return false;
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp
new file mode 100644
index 0000000..0a5f234
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/cpp/StandardNetworkTables.cpp
@@ -0,0 +1,176 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "glass/networktables/NTCommandScheduler.h"
+#include "glass/networktables/NTCommandSelector.h"
+#include "glass/networktables/NTDifferentialDrive.h"
+#include "glass/networktables/NTDigitalInput.h"
+#include "glass/networktables/NTDigitalOutput.h"
+#include "glass/networktables/NTFMS.h"
+#include "glass/networktables/NTField2D.h"
+#include "glass/networktables/NTGyro.h"
+#include "glass/networktables/NTMecanumDrive.h"
+#include "glass/networktables/NTMechanism2D.h"
+#include "glass/networktables/NTPIDController.h"
+#include "glass/networktables/NTSpeedController.h"
+#include "glass/networktables/NTStringChooser.h"
+#include "glass/networktables/NTSubsystem.h"
+#include "glass/networktables/NetworkTablesProvider.h"
+
+using namespace glass;
+
+void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) {
+  provider.Register(
+      NTCommandSchedulerModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTCommandSchedulerModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetDefaultSize(400, 200);
+        return MakeFunctionView([=] {
+          DisplayCommandScheduler(static_cast<NTCommandSchedulerModel*>(model));
+        });
+      });
+  provider.Register(
+      NTCommandSelectorModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTCommandSelectorModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplayCommandSelector(static_cast<NTCommandSelectorModel*>(model));
+        });
+      });
+  provider.Register(
+      NTDifferentialDriveModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTDifferentialDriveModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetDefaultSize(300, 350);
+        return MakeFunctionView([=] {
+          DisplayDrive(static_cast<NTDifferentialDriveModel*>(model));
+        });
+      });
+  provider.Register(
+      NTFMSModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTFMSModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView(
+            [=] { DisplayFMS(static_cast<FMSModel*>(model)); });
+      });
+  provider.Register(
+      NTDigitalInputModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTDigitalInputModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplayDIO(static_cast<NTDigitalInputModel*>(model), 0, true);
+        });
+      });
+  provider.Register(
+      NTDigitalOutputModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTDigitalOutputModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplayDIO(static_cast<NTDigitalOutputModel*>(model), 0, true);
+        });
+      });
+  provider.Register(
+      NTField2DModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTField2DModel>(inst, path);
+      },
+      [=](Window* win, Model* model, const char* path) {
+        win->SetDefaultPos(200, 200);
+        win->SetDefaultSize(400, 200);
+        win->SetPadding(0, 0);
+        return std::make_unique<Field2DView>(
+            static_cast<NTField2DModel*>(model));
+      });
+  provider.Register(
+      NTGyroModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTGyroModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char* path) {
+        win->SetDefaultSize(320, 380);
+        return MakeFunctionView(
+            [=] { DisplayGyro(static_cast<NTGyroModel*>(model)); });
+      });
+  provider.Register(
+      NTMecanumDriveModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTMecanumDriveModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetDefaultSize(300, 350);
+        return MakeFunctionView(
+            [=] { DisplayDrive(static_cast<NTMecanumDriveModel*>(model)); });
+      });
+  provider.Register(
+      NTMechanism2DModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTMechanism2DModel>(inst, path);
+      },
+      [=](Window* win, Model* model, const char* path) {
+        win->SetDefaultPos(400, 400);
+        win->SetDefaultSize(200, 200);
+        win->SetPadding(0, 0);
+        return std::make_unique<Mechanism2DView>(
+            static_cast<NTMechanism2DModel*>(model));
+      });
+  provider.Register(
+      NTPIDControllerModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTPIDControllerModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char* path) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplayPIDController(static_cast<NTPIDControllerModel*>(model));
+        });
+      });
+  provider.Register(
+      NTSpeedControllerModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTSpeedControllerModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char* path) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplaySpeedController(static_cast<NTSpeedControllerModel*>(model));
+        });
+      });
+  provider.Register(
+      NTStringChooserModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTStringChooserModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView([=] {
+          DisplayStringChooser(static_cast<NTStringChooserModel*>(model));
+        });
+      });
+  provider.Register(
+      NTSubsystemModel::kType,
+      [](NT_Inst inst, const char* path) {
+        return std::make_unique<NTSubsystemModel>(inst, path);
+      },
+      [](Window* win, Model* model, const char*) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        return MakeFunctionView(
+            [=] { DisplaySubsystem(static_cast<NTSubsystemModel*>(model)); });
+      });
+}
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTCommandScheduler.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTCommandScheduler.h
new file mode 100644
index 0000000..54dc778
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTCommandScheduler.h
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/CommandScheduler.h"
+
+namespace glass {
+class NTCommandSchedulerModel : public CommandSchedulerModel {
+ public:
+  static constexpr const char* kType = "Scheduler";
+
+  explicit NTCommandSchedulerModel(std::string_view path);
+  NTCommandSchedulerModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const std::vector<std::string>& GetCurrentCommands() override {
+    return m_commandsValue;
+  }
+
+  void CancelCommand(size_t index) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return false; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_commands;
+  NT_Entry m_ids;
+  NT_Entry m_cancel;
+
+  std::string m_nameValue;
+  std::vector<std::string> m_commandsValue;
+  std::vector<double> m_idsValue;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTCommandSelector.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTCommandSelector.h
new file mode 100644
index 0000000..c936665
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTCommandSelector.h
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/CommandSelector.h"
+
+namespace glass {
+class NTCommandSelectorModel : public CommandSelectorModel {
+ public:
+  static constexpr const char* kType = "Command";
+
+  explicit NTCommandSelectorModel(std::string_view path);
+  NTCommandSelectorModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  DataSource* GetRunningData() override { return &m_runningData; }
+  void SetRunning(bool run) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return false; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_running;
+  NT_Entry m_name;
+
+  DataSource m_runningData;
+  std::string m_nameValue;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDifferentialDrive.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDifferentialDrive.h
new file mode 100644
index 0000000..49b3eb0
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDifferentialDrive.h
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Drive.h"
+
+namespace glass {
+class NTDifferentialDriveModel : public DriveModel {
+ public:
+  static constexpr const char* kType = "DifferentialDrive";
+
+  explicit NTDifferentialDriveModel(std::string_view path);
+  NTDifferentialDriveModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const std::vector<DriveModel::WheelInfo>& GetWheels() const override {
+    return m_wheels;
+  }
+
+  ImVec2 GetSpeedVector() const override { return m_speedVector; }
+  double GetRotation() const override { return m_rotation; }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return !m_controllableValue; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_controllable;
+  NT_Entry m_lPercent;
+  NT_Entry m_rPercent;
+
+  std::string m_nameValue;
+  bool m_controllableValue = false;
+  DataSource m_lPercentData;
+  DataSource m_rPercentData;
+
+  std::vector<DriveModel::WheelInfo> m_wheels;
+  ImVec2 m_speedVector;
+  double m_rotation;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDigitalInput.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDigitalInput.h
new file mode 100644
index 0000000..cd3dfeb
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDigitalInput.h
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/hardware/DIO.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+
+namespace glass {
+
+class NTDigitalInputModel : public DIOModel {
+ public:
+  static constexpr const char* kType = "Digital Input";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTDigitalInputModel(std::string_view path);
+  NTDigitalInputModel(NT_Inst inst, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+
+  const char* GetSimDevice() const override { return nullptr; }
+
+  DPWMModel* GetDPWM() override { return nullptr; }
+  DutyCycleModel* GetDutyCycle() override { return nullptr; }
+  EncoderModel* GetEncoder() override { return nullptr; }
+
+  bool IsInput() const override { return true; }
+
+  DataSource* GetValueData() override { return &m_valueData; }
+
+  void SetValue(bool val) override {}
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return true; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_value;
+  NT_Entry m_name;
+
+  DataSource m_valueData;
+  std::string m_nameValue;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDigitalOutput.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDigitalOutput.h
new file mode 100644
index 0000000..8ed1ee7
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTDigitalOutput.h
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/hardware/DIO.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+
+namespace glass {
+
+class NTDigitalOutputModel : public DIOModel {
+ public:
+  static constexpr const char* kType = "Digital Output";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTDigitalOutputModel(std::string_view path);
+  NTDigitalOutputModel(NT_Inst inst, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+
+  const char* GetSimDevice() const override { return nullptr; }
+
+  DPWMModel* GetDPWM() override { return nullptr; }
+  DutyCycleModel* GetDutyCycle() override { return nullptr; }
+  EncoderModel* GetEncoder() override { return nullptr; }
+
+  bool IsInput() const override { return true; }
+
+  DataSource* GetValueData() override { return &m_valueData; }
+
+  void SetValue(bool val) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return !m_controllableValue; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_value;
+  NT_Entry m_name;
+  NT_Entry m_controllable;
+
+  DataSource m_valueData;
+  std::string m_nameValue;
+  bool m_controllableValue = false;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTFMS.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTFMS.h
new file mode 100644
index 0000000..b19a9f0
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTFMS.h
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/FMS.h"
+
+namespace glass {
+
+class NTFMSModel : public FMSModel {
+ public:
+  static constexpr const char* kType = "FMSInfo";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTFMSModel(std::string_view path);
+  NTFMSModel(NT_Inst inst, std::string_view path);
+
+  DataSource* GetFmsAttachedData() override { return &m_fmsAttached; }
+  DataSource* GetDsAttachedData() override { return &m_dsAttached; }
+  DataSource* GetAllianceStationIdData() override {
+    return &m_allianceStationId;
+  }
+  // NT does not provide match time
+  DataSource* GetMatchTimeData() override { return nullptr; }
+  DataSource* GetEStopData() override { return &m_estop; }
+  DataSource* GetEnabledData() override { return &m_enabled; }
+  DataSource* GetTestData() override { return &m_test; }
+  DataSource* GetAutonomousData() override { return &m_autonomous; }
+  std::string_view GetGameSpecificMessage(
+      wpi::SmallVectorImpl<char>& buf) override;
+
+  // NT is read-only (it's continually set by robot code)
+  void SetFmsAttached(bool val) override {}
+  void SetDsAttached(bool val) override {}
+  void SetAllianceStationId(int val) override {}
+  void SetMatchTime(double val) override {}
+  void SetEStop(bool val) override {}
+  void SetEnabled(bool val) override {}
+  void SetTest(bool val) override {}
+  void SetAutonomous(bool val) override {}
+  void SetGameSpecificMessage(const char* val) override {}
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return true; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_gameSpecificMessage;
+  NT_Entry m_alliance;
+  NT_Entry m_station;
+  NT_Entry m_controlWord;
+
+  DataSource m_fmsAttached;
+  DataSource m_dsAttached;
+  DataSource m_allianceStationId;
+  DataSource m_estop;
+  DataSource m_enabled;
+  DataSource m_test;
+  DataSource m_autonomous;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTField2D.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTField2D.h
new file mode 100644
index 0000000..f966e0f
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTField2D.h
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <ntcore_cpp.h>
+
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Field2D.h"
+
+namespace glass {
+
+class NTField2DModel : public Field2DModel {
+ public:
+  static constexpr const char* kType = "Field2d";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTField2DModel(std::string_view path);
+  NTField2DModel(NT_Inst inst, std::string_view path);
+  ~NTField2DModel() override;
+
+  const char* GetPath() const { return m_path.c_str(); }
+  const char* GetName() const { return m_nameValue.c_str(); }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override;
+
+  FieldObjectModel* AddFieldObject(std::string_view name) override;
+  void RemoveFieldObject(std::string_view name) override;
+  void ForEachFieldObject(
+      wpi::function_ref<void(FieldObjectModel& model, std::string_view name)>
+          func) override;
+
+ private:
+  NetworkTablesHelper m_nt;
+  std::string m_path;
+  NT_Entry m_name;
+  std::string m_nameValue;
+
+  class ObjectModel;
+  using Objects = std::vector<std::unique_ptr<ObjectModel>>;
+  Objects m_objects;
+
+  std::pair<Objects::iterator, bool> Find(std::string_view fullName);
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTGyro.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTGyro.h
new file mode 100644
index 0000000..db303a5
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTGyro.h
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/hardware/Gyro.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+
+namespace glass {
+class NTGyroModel : public GyroModel {
+ public:
+  static constexpr const char* kType = "Gyro";
+
+  explicit NTGyroModel(std::string_view path);
+  NTGyroModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const char* GetSimDevice() const override { return nullptr; }
+
+  DataSource* GetAngleData() override { return &m_angleData; }
+  void SetAngle(double value) override {}
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return true; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_angle;
+  NT_Entry m_name;
+
+  DataSource m_angleData;
+  std::string m_nameValue;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTMecanumDrive.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTMecanumDrive.h
new file mode 100644
index 0000000..ce7d234
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTMecanumDrive.h
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Drive.h"
+
+namespace glass {
+class NTMecanumDriveModel : public DriveModel {
+ public:
+  static constexpr const char* kType = "MecanumDrive";
+
+  explicit NTMecanumDriveModel(std::string_view path);
+  NTMecanumDriveModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const std::vector<DriveModel::WheelInfo>& GetWheels() const override {
+    return m_wheels;
+  }
+
+  ImVec2 GetSpeedVector() const override { return m_speedVector; }
+  double GetRotation() const override { return m_rotation; }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return !m_controllableValue; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_controllable;
+  NT_Entry m_flPercent;
+  NT_Entry m_frPercent;
+  NT_Entry m_rlPercent;
+  NT_Entry m_rrPercent;
+
+  std::string m_nameValue;
+  bool m_controllableValue = false;
+  DataSource m_flPercentData;
+  DataSource m_frPercentData;
+  DataSource m_rlPercentData;
+  DataSource m_rrPercentData;
+
+  std::vector<DriveModel::WheelInfo> m_wheels;
+  ImVec2 m_speedVector;
+  double m_rotation;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h
new file mode 100644
index 0000000..81f7df1
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTMechanism2D.h
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <frc/geometry/Translation2d.h>
+#include <ntcore_cpp.h>
+
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Mechanism2D.h"
+
+namespace glass {
+
+class NTMechanism2DModel : public Mechanism2DModel {
+ public:
+  static constexpr const char* kType = "Mechanism2d";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTMechanism2DModel(std::string_view path);
+  NTMechanism2DModel(NT_Inst inst, std::string_view path);
+  ~NTMechanism2DModel() override;
+
+  const char* GetPath() const { return m_path.c_str(); }
+  const char* GetName() const { return m_nameValue.c_str(); }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override;
+
+  frc::Translation2d GetDimensions() const override {
+    return m_dimensionsValue;
+  }
+  ImU32 GetBackgroundColor() const override { return m_bgColorValue; }
+  void ForEachRoot(
+      wpi::function_ref<void(MechanismRootModel& model)> func) override;
+
+ private:
+  NetworkTablesHelper m_nt;
+  std::string m_path;
+
+  NT_Entry m_name;
+  NT_Entry m_dimensions;
+  NT_Entry m_bgColor;
+
+  std::string m_nameValue;
+  frc::Translation2d m_dimensionsValue;
+  ImU32 m_bgColorValue = 0;
+
+  class RootModel;
+  std::vector<std::unique_ptr<RootModel>> m_roots;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h
new file mode 100644
index 0000000..b975641
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTPIDController.h
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/PIDController.h"
+
+namespace glass {
+class NTPIDControllerModel : public PIDControllerModel {
+ public:
+  static constexpr const char* kType = "PIDController";
+
+  explicit NTPIDControllerModel(std::string_view path);
+  NTPIDControllerModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+
+  DataSource* GetPData() override { return &m_pData; }
+  DataSource* GetIData() override { return &m_iData; }
+  DataSource* GetDData() override { return &m_dData; }
+  DataSource* GetSetpointData() override { return &m_setpointData; }
+
+  void SetP(double value) override;
+  void SetI(double value) override;
+  void SetD(double value) override;
+  void SetSetpoint(double value) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return !m_controllableValue; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_controllable;
+  NT_Entry m_p;
+  NT_Entry m_i;
+  NT_Entry m_d;
+  NT_Entry m_setpoint;
+
+  DataSource m_pData;
+  DataSource m_iData;
+  DataSource m_dData;
+  DataSource m_setpointData;
+
+  std::string m_nameValue;
+  bool m_controllableValue = false;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTSpeedController.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTSpeedController.h
new file mode 100644
index 0000000..756d947
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTSpeedController.h
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/hardware/SpeedController.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+
+namespace glass {
+class NTSpeedControllerModel : public SpeedControllerModel {
+ public:
+  static constexpr const char* kType = "Speed Controller";
+
+  explicit NTSpeedControllerModel(std::string_view path);
+  NTSpeedControllerModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const char* GetSimDevice() const override { return nullptr; }
+
+  DataSource* GetPercentData() override { return &m_valueData; }
+  void SetPercent(double value) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return !m_controllableValue; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_value;
+  NT_Entry m_name;
+  NT_Entry m_controllable;
+
+  DataSource m_valueData;
+  std::string m_nameValue;
+  bool m_controllableValue = false;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h
new file mode 100644
index 0000000..2d806c9
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTStringChooser.h
@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include <ntcore_cpp.h>
+
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/StringChooser.h"
+
+namespace glass {
+
+class NTStringChooserModel : public StringChooserModel {
+ public:
+  static constexpr const char* kType = "String Chooser";
+
+  // path is to the table containing ".type", excluding the trailing /
+  explicit NTStringChooserModel(std::string_view path);
+  NTStringChooserModel(NT_Inst inst, std::string_view path);
+
+  const std::string& GetDefault() override { return m_defaultValue; }
+  const std::string& GetSelected() override { return m_selectedValue; }
+  const std::string& GetActive() override { return m_activeValue; }
+  const std::vector<std::string>& GetOptions() override {
+    return m_optionsValue;
+  }
+
+  void SetDefault(std::string_view val) override;
+  void SetSelected(std::string_view val) override;
+  void SetActive(std::string_view val) override;
+  void SetOptions(wpi::span<const std::string> val) override;
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return false; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_default;
+  NT_Entry m_selected;
+  NT_Entry m_active;
+  NT_Entry m_options;
+
+  std::string m_defaultValue;
+  std::string m_selectedValue;
+  std::string m_activeValue;
+  std::vector<std::string> m_optionsValue;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTSubsystem.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTSubsystem.h
new file mode 100644
index 0000000..c5862cf
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NTSubsystem.h
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <ntcore_cpp.h>
+
+#include "glass/DataSource.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/other/Subsystem.h"
+
+namespace glass {
+class NTSubsystemModel : public SubsystemModel {
+ public:
+  static constexpr const char* kType = "Subsystem";
+
+  explicit NTSubsystemModel(std::string_view path);
+  NTSubsystemModel(NT_Inst instance, std::string_view path);
+
+  const char* GetName() const override { return m_nameValue.c_str(); }
+  const char* GetDefaultCommand() const override {
+    return m_defaultCommandValue.c_str();
+  }
+  const char* GetCurrentCommand() const override {
+    return m_currentCommandValue.c_str();
+  }
+
+  void Update() override;
+  bool Exists() override;
+  bool IsReadOnly() override { return true; }
+
+ private:
+  NetworkTablesHelper m_nt;
+  NT_Entry m_name;
+  NT_Entry m_defaultCommand;
+  NT_Entry m_currentCommand;
+
+  std::string m_nameValue;
+  std::string m_defaultCommandValue;
+  std::string m_currentCommandValue;
+};
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTables.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTables.h
new file mode 100644
index 0000000..8538eaa
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTables.h
@@ -0,0 +1,141 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include <ntcore_cpp.h>
+#include <wpi/DenseMap.h>
+
+#include "glass/Model.h"
+#include "glass/View.h"
+
+namespace glass {
+
+class DataSource;
+
+class NetworkTablesModel : public Model {
+ public:
+  struct Entry {
+    explicit Entry(nt::EntryNotification&& event);
+
+    void UpdateValue();
+
+    /** Entry handle. */
+    NT_Entry entry;
+
+    /** Entry name. */
+    std::string name;
+
+    /** The value. */
+    std::shared_ptr<nt::Value> value;
+
+    /** Flags. */
+    unsigned int flags = 0;
+
+    /** String representation of the value (for arrays / complex values). */
+    std::string valueStr;
+
+    /** Data source (for numeric values). */
+    std::unique_ptr<DataSource> source;
+  };
+
+  struct TreeNode {
+    explicit TreeNode(std::string_view name) : name{name} {}
+
+    /** Short name (e.g. of just this node) */
+    std::string name;
+
+    /**
+     * Full path if entry is null (otherwise use entry->name),
+     * excluding trailing /
+     */
+    std::string path;
+
+    /** Null if no value at this node */
+    Entry* entry = nullptr;
+
+    /** Children of node, sorted by name */
+    std::vector<TreeNode> children;
+  };
+
+  NetworkTablesModel();
+  explicit NetworkTablesModel(NT_Inst inst);
+  ~NetworkTablesModel() override;
+
+  void Update() override;
+  bool Exists() override;
+
+  NT_Inst GetInstance() { return m_inst; }
+  const std::vector<Entry*>& GetEntries() { return m_sortedEntries; }
+  const std::vector<TreeNode>& GetTreeRoot() { return m_root; }
+
+ private:
+  NT_Inst m_inst;
+  NT_EntryListenerPoller m_poller;
+  wpi::DenseMap<NT_Entry, std::unique_ptr<Entry>> m_entries;
+
+  // sorted by name
+  std::vector<Entry*> m_sortedEntries;
+
+  std::vector<TreeNode> m_root;
+};
+
+using NetworkTablesFlags = int;
+
+enum NetworkTablesFlags_ {
+  NetworkTablesFlags_TreeView = 1 << 0,
+  NetworkTablesFlags_ReadOnly = 1 << 1,
+  NetworkTablesFlags_ShowConnections = 1 << 2,
+  NetworkTablesFlags_ShowFlags = 1 << 3,
+  NetworkTablesFlags_ShowTimestamp = 1 << 4,
+  NetworkTablesFlags_CreateNoncanonicalKeys = 1 << 5,
+  NetworkTablesFlags_Default = 1 & ~NetworkTablesFlags_ReadOnly &
+                               ~NetworkTablesFlags_CreateNoncanonicalKeys
+};
+
+void DisplayNetworkTables(
+    NetworkTablesModel* model,
+    NetworkTablesFlags flags = NetworkTablesFlags_Default);
+
+class NetworkTablesFlagsSettings {
+ public:
+  explicit NetworkTablesFlagsSettings(
+      NetworkTablesFlags defaultFlags = NetworkTablesFlags_Default)
+      : m_defaultFlags{defaultFlags}, m_flags{defaultFlags} {}
+
+  void Update();
+  void DisplayMenu();
+
+  NetworkTablesFlags GetFlags() const { return m_flags; }
+
+ private:
+  bool* m_pTreeView = nullptr;
+  bool* m_pShowConnections = nullptr;
+  bool* m_pShowFlags = nullptr;
+  bool* m_pShowTimestamp = nullptr;
+  bool* m_pCreateNoncanonicalKeys = nullptr;
+  NetworkTablesFlags m_defaultFlags;  // NOLINT
+  NetworkTablesFlags m_flags;         // NOLINT
+};
+
+class NetworkTablesView : public View {
+ public:
+  explicit NetworkTablesView(
+      NetworkTablesModel* model,
+      NetworkTablesFlags defaultFlags = NetworkTablesFlags_Default)
+      : m_model{model}, m_flags{defaultFlags} {}
+
+  void Display() override;
+
+ private:
+  NetworkTablesModel* m_model;
+  NetworkTablesFlagsSettings m_flags;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h
new file mode 100644
index 0000000..aba3252
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesHelper.h
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string_view>
+#include <vector>
+
+#include <ntcore_cpp.h>
+
+namespace glass {
+
+class NetworkTablesHelper {
+ public:
+  explicit NetworkTablesHelper(NT_Inst inst);
+  ~NetworkTablesHelper();
+
+  NetworkTablesHelper(const NetworkTablesHelper&) = delete;
+  NetworkTablesHelper& operator=(const NetworkTablesHelper&) = delete;
+
+  NT_Inst GetInstance() const { return m_inst; }
+  NT_EntryListenerPoller GetPoller() const { return m_poller; }
+
+  NT_Entry GetEntry(std::string_view name) const {
+    return nt::GetEntry(m_inst, name);
+  }
+
+  static constexpr int kDefaultListenerFlags =
+      NT_NOTIFY_LOCAL | NT_NOTIFY_NEW | NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
+      NT_NOTIFY_IMMEDIATE;
+
+  NT_EntryListener AddListener(NT_Entry entry,
+                               unsigned int flags = kDefaultListenerFlags) {
+    return nt::AddPolledEntryListener(m_poller, entry, flags);
+  }
+
+  NT_EntryListener AddListener(std::string_view prefix,
+                               unsigned int flags = kDefaultListenerFlags) {
+    return nt::AddPolledEntryListener(m_poller, prefix, flags);
+  }
+
+  std::vector<nt::EntryNotification> PollListener() {
+    bool timedOut = false;
+    return nt::PollEntryListener(m_poller, 0, &timedOut);
+  }
+
+  bool IsConnected() const;
+
+ private:
+  NT_Inst m_inst;
+  NT_EntryListenerPoller m_poller;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h
new file mode 100644
index 0000000..17374ca
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesProvider.h
@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string_view>
+#include <vector>
+
+#include <ntcore_cpp.h>
+#include <wpi/StringMap.h>
+
+#include "glass/Model.h"
+#include "glass/Provider.h"
+#include "glass/networktables/NetworkTablesHelper.h"
+#include "glass/support/IniSaverInfo.h"
+#include "glass/support/IniSaverString.h"
+
+namespace glass {
+
+class Window;
+
+namespace detail {
+struct NTProviderFunctions {
+  using Exists = std::function<bool(NT_Inst inst, const char* path)>;
+  using CreateModel =
+      std::function<std::unique_ptr<Model>(NT_Inst inst, const char* path)>;
+  using ViewExists = std::function<bool(Model*, const char* path)>;
+  using CreateView =
+      std::function<std::unique_ptr<View>(Window*, Model*, const char* path)>;
+};
+}  // namespace detail
+
+/**
+ * A provider for NetworkTables (SmartDashboard style) models and views.
+ */
+class NetworkTablesProvider : private Provider<detail::NTProviderFunctions> {
+ public:
+  using Provider::CreateModelFunc;
+  using Provider::CreateViewFunc;
+
+  explicit NetworkTablesProvider(std::string_view iniName);
+  NetworkTablesProvider(std::string_view iniName, NT_Inst inst);
+
+  /**
+   * Get the NetworkTables instance being used for this provider.
+   *
+   * @return NetworkTables instance
+   */
+  NT_Inst GetInstance() const { return m_nt.GetInstance(); }
+
+  /**
+   * Perform global initialization.  This should be called prior to
+   * wpi::gui::Initialize().
+   */
+  void GlobalInit() override;
+
+  /**
+   * Displays menu contents as a tree of available NetworkTables views.
+   */
+  void DisplayMenu() override;
+
+  /**
+   * Registers a NetworkTables model and view.
+   *
+   * @param typeName SmartDashboard .type value to match
+   * @param createModel functor to create model
+   * @param createView functor to create view
+   */
+  void Register(std::string_view typeName, CreateModelFunc createModel,
+                CreateViewFunc createView);
+
+  using WindowManager::AddWindow;
+
+ private:
+  void Update() override;
+
+  NetworkTablesHelper m_nt;
+
+  // cached mapping from table name to type string
+  IniSaverString<NameInfo> m_typeCache;
+
+  struct Builder {
+    CreateModelFunc createModel;
+    CreateViewFunc createView;
+  };
+
+  // mapping from .type string to model/view creators
+  wpi::StringMap<Builder> m_typeMap;
+
+  struct Entry : public ModelEntry {
+    Entry(NT_Entry typeEntry, std::string_view name, const Builder& builder)
+        : ModelEntry{name, [](NT_Inst, const char*) { return true; },
+                     builder.createModel},
+          typeEntry{typeEntry} {}
+    NT_Entry typeEntry;
+  };
+
+  void Show(ViewEntry* entry, Window* window) override;
+
+  ViewEntry* GetOrCreateView(const Builder& builder, NT_Entry typeEntry,
+                             std::string_view name);
+};
+
+/**
+ * Add "standard" set of NetworkTables models/views.
+ *
+ * @param provider NetworkTables provider
+ */
+void AddStandardNetworkTablesViews(NetworkTablesProvider& provider);
+
+}  // namespace glass
diff --git a/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesSettings.h b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesSettings.h
new file mode 100644
index 0000000..7738541
--- /dev/null
+++ b/third_party/allwpilib/glass/src/libnt/native/include/glass/networktables/NetworkTablesSettings.h
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include <ntcore_cpp.h>
+#include <wpi/SafeThread.h>
+
+namespace wpi {
+template <typename T>
+class SmallVectorImpl;
+}  // namespace wpi
+
+namespace glass {
+
+class NetworkTablesSettings {
+ public:
+  explicit NetworkTablesSettings(
+      NT_Inst inst = nt::GetDefaultInstance(),
+      const char* storageName = "NetworkTables Settings");
+
+  /**
+   * Enables or disables the server option.  Default is enabled.
+   */
+  void EnableServerOption(bool enable) { m_serverOption = enable; }
+
+  void Update();
+  bool Display();
+
+ private:
+  bool m_restart = true;
+  bool m_serverOption = true;
+  int* m_pMode;
+  std::string* m_pIniName;
+  std::string* m_pServerTeam;
+  std::string* m_pListenAddress;
+  bool* m_pDsClient;
+
+  class Thread : public wpi::SafeThread {
+   public:
+    explicit Thread(NT_Inst inst) : m_inst{inst} {}
+
+    void Main() override;
+
+    NT_Inst m_inst;
+    bool m_restart = false;
+    int m_mode;
+    std::string m_iniName;
+    std::string m_serverTeam;
+    std::string m_listenAddress;
+    bool m_dsClient;
+  };
+  wpi::SafeThreadOwner<Thread> m_thread;
+};
+
+}  // namespace glass
diff --git a/third_party/allwpilib/googletest/CMakeLists.txt b/third_party/allwpilib/googletest/CMakeLists.txt
index c792212..b06e326 100644
--- a/third_party/allwpilib/googletest/CMakeLists.txt
+++ b/third_party/allwpilib/googletest/CMakeLists.txt
@@ -22,3 +22,6 @@
 add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/googletest-src
                  ${CMAKE_CURRENT_BINARY_DIR}/googletest-build
                  EXCLUDE_FROM_ALL)
+
+target_compile_features(gtest PUBLIC cxx_std_17)
+target_compile_features(gtest_main PUBLIC cxx_std_17)
diff --git a/third_party/allwpilib/googletest/CMakeLists.txt.in b/third_party/allwpilib/googletest/CMakeLists.txt.in
index 9378f44..196b3b6 100644
--- a/third_party/allwpilib/googletest/CMakeLists.txt.in
+++ b/third_party/allwpilib/googletest/CMakeLists.txt.in
@@ -5,7 +5,7 @@
 include(ExternalProject)
 ExternalProject_Add(googletest
   GIT_REPOSITORY    https://github.com/google/googletest.git
-  GIT_TAG           b4676595c03a26bb84f68542c8b74d3d89b38b68
+  GIT_TAG           e2239ee6043f73722e7aa812a459f54a28552929 # 1.11.0
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/googletest-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/googletest-build"
   CONFIGURE_COMMAND ""
diff --git a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar
index cc4fdc2..7454180 100644
--- a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar
+++ b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.jar
Binary files differ
diff --git a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties
index 9492014..05679dc 100644
--- a/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties
+++ b/third_party/allwpilib/gradle/wrapper/gradle-wrapper.properties
@@ -1,5 +1,5 @@
 distributionBase=GRADLE_USER_HOME
 distributionPath=wrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-6.0.1-bin.zip
+distributionUrl=https\://services.gradle.org/distributions/gradle-7.1.1-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=wrapper/dists
diff --git a/third_party/allwpilib/gradlew b/third_party/allwpilib/gradlew
index 2fe81a7..744e882 100755
--- a/third_party/allwpilib/gradlew
+++ b/third_party/allwpilib/gradlew
@@ -72,7 +72,7 @@
   Darwin* )
     darwin=true
     ;;
-  MINGW* )
+  MSYS* | MINGW* )
     msys=true
     ;;
   NONSTOP* )
@@ -82,6 +82,7 @@
 
 CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
 
+
 # Determine the Java command to use to start the JVM.
 if [ -n "$JAVA_HOME" ] ; then
     if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
@@ -129,6 +130,7 @@
 if [ "$cygwin" = "true" -o "$msys" = "true" ] ; then
     APP_HOME=`cygpath --path --mixed "$APP_HOME"`
     CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+
     JAVACMD=`cygpath --unix "$JAVACMD"`
 
     # We build the pattern for arguments to be converted via cygpath
diff --git a/third_party/allwpilib/gradlew.bat b/third_party/allwpilib/gradlew.bat
index 24467a1..ac1b06f 100644
--- a/third_party/allwpilib/gradlew.bat
+++ b/third_party/allwpilib/gradlew.bat
@@ -29,6 +29,9 @@
 set APP_BASE_NAME=%~n0
 set APP_HOME=%DIRNAME%
 
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
 @rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
 set DEFAULT_JVM_OPTS="-Xmx64m" "-Xms64m"
 
@@ -37,7 +40,7 @@
 
 set JAVA_EXE=java.exe
 %JAVA_EXE% -version >NUL 2>&1
-if "%ERRORLEVEL%" == "0" goto init
+if "%ERRORLEVEL%" == "0" goto execute
 
 echo.
 echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
@@ -51,7 +54,7 @@
 set JAVA_HOME=%JAVA_HOME:"=%
 set JAVA_EXE=%JAVA_HOME%/bin/java.exe
 
-if exist "%JAVA_EXE%" goto init
+if exist "%JAVA_EXE%" goto execute
 
 echo.
 echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
@@ -61,28 +64,14 @@
 
 goto fail
 
-:init
-@rem Get command-line arguments, handling Windows variants
-
-if not "%OS%" == "Windows_NT" goto win9xME_args
-
-:win9xME_args
-@rem Slurp the command line arguments.
-set CMD_LINE_ARGS=
-set _SKIP=2
-
-:win9xME_args_slurp
-if "x%~1" == "x" goto execute
-
-set CMD_LINE_ARGS=%*
-
 :execute
 @rem Setup the command line
 
 set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
 
+
 @rem Execute Gradle
-"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %*
 
 :end
 @rem End local scope for the variables with windows NT shell
diff --git a/third_party/allwpilib/hal/.styleguide b/third_party/allwpilib/hal/.styleguide
index 49d4e29..77cd815 100644
--- a/third_party/allwpilib/hal/.styleguide
+++ b/third_party/allwpilib/hal/.styleguide
@@ -10,6 +10,7 @@
 
 generatedFileExclude {
   hal/src/main/native/athena/ctre/
+  hal/src/main/native/athena/rev/
   hal/src/main/native/athena/frccansae/
   hal/src/main/native/athena/visa/
   hal/src/main/native/include/ctre/
@@ -30,6 +31,7 @@
 includeOtherLibs {
   ^FRC_FPGA_ChipObject/
   ^FRC_NetworkCommunication/
+  ^fmt/
   ^i2clib/
   ^llvm/
   ^opencv2/
diff --git a/third_party/allwpilib/hal/CMakeLists.txt b/third_party/allwpilib/hal/CMakeLists.txt
index 2e99064..fae74aa 100644
--- a/third_party/allwpilib/hal/CMakeLists.txt
+++ b/third_party/allwpilib/hal/CMakeLists.txt
@@ -65,8 +65,8 @@
     set (hal_config_dir share/hal)
 endif()
 
-configure_file(hal-config.cmake.in ${CMAKE_BINARY_DIR}/hal-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/hal-config.cmake DESTINATION ${hal_config_dir})
+configure_file(hal-config.cmake.in ${WPILIB_BINARY_DIR}/hal-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/hal-config.cmake DESTINATION ${hal_config_dir})
 install(EXPORT hal DESTINATION ${hal_config_dir})
 
 # Java bindings
@@ -74,7 +74,7 @@
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
-    set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
     configure_file(src/generate/FRCNetComm.java.in FRCNetComm.java)
 
diff --git a/third_party/allwpilib/hal/build.gradle b/third_party/allwpilib/hal/build.gradle
index e5b5f6c..ed67125 100644
--- a/third_party/allwpilib/hal/build.gradle
+++ b/third_party/allwpilib/hal/build.gradle
@@ -43,8 +43,6 @@
     }
 }
 
-apply plugin: DisableBuildingGTest
-
 ext {
     addHalDependency = { binary, shared->
         binary.tasks.withType(AbstractNativeSourceCompileTask) {
@@ -129,34 +127,55 @@
     }
 }
 
+Action<List<String>> symbolFilter = { symbols ->
+    symbols.removeIf({ !it.startsWith('HAL_') && !it.startsWith('HALSIM_') })
+} as Action<List<String>>;
+
 nativeUtils.exportsConfigs {
     hal {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
     halJNI {
-        x86SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('HAL_') && !it.startsWith('HALSIM_') })
-        }
-        x64SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('HAL_') && !it.startsWith('HALSIM_') })
-        }
+        x86SymbolFilter = symbolFilter
+        x64SymbolFilter = symbolFilter
     }
 }
 
 model {
     binaries {
         all {
+            it.tasks.withType(AbstractNativeSourceCompileTask) {
+                it.dependsOn generateUsageReporting
+            }
             if (!(it instanceof NativeBinarySpec)) return
-            if (it.component.name != 'hal' && it.component.name != 'halBase') return
-            if (it.targetPlatform.name != nativeUtils.wpi.platforms.roborio) return
-            nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared')
+                if (it.component.name != 'hal' && it.component.name != 'halBase') return
+                if (it.targetPlatform.name != nativeUtils.wpi.platforms.roborio) return
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries')
         }
     }
 }
diff --git a/third_party/allwpilib/hal/hal-config.cmake.in b/third_party/allwpilib/hal/hal-config.cmake.in
index ae5533c..49eac3d 100644
--- a/third_party/allwpilib/hal/hal-config.cmake.in
+++ b/third_party/allwpilib/hal/hal-config.cmake.in
@@ -2,4 +2,5 @@
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/hal.cmake)
diff --git a/third_party/allwpilib/hal/src/dev/java/edu/wpi/first/hal/DevMain.java b/third_party/allwpilib/hal/src/dev/java/edu/wpi/first/hal/DevMain.java
index 25c6c90..efe8e9b 100644
--- a/third_party/allwpilib/hal/src/dev/java/edu/wpi/first/hal/DevMain.java
+++ b/third_party/allwpilib/hal/src/dev/java/edu/wpi/first/hal/DevMain.java
@@ -1,17 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public final class DevMain {
-  public static void main(String[] args) {
+  public static void main(String[] args) {}
 
-  }
-
-  private DevMain() {
-  }
+  private DevMain() {}
 }
diff --git a/third_party/allwpilib/hal/src/dev/native/cpp/main.cpp b/third_party/allwpilib/hal/src/dev/native/cpp/main.cpp
index 6bfa3df..721c0f6 100644
--- a/third_party/allwpilib/hal/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/hal/src/dev/native/cpp/main.cpp
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
+#include <fmt/core.h>
 
 #include "hal/HAL.h"
 
 int main() {
-  std::cout << "Hello World" << std::endl;
-  std::cout << HAL_GetRuntimeType() << std::endl;
+  fmt::print("Hello World\n");
+  fmt::print("{}\n", HAL_GetRuntimeType());
 }
diff --git a/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in b/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in
index 2c2f924..ce21889 100644
--- a/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in
+++ b/third_party/allwpilib/hal/src/generate/FRCNetComm.java.in
@@ -12,7 +12,7 @@
   /**
    * Resource type from UsageReporting.
    */
-  @SuppressWarnings({"TypeName", "PMD.ConstantsInInterface"})
+  @SuppressWarnings("TypeName")
   public static final class tResourceType {
     private tResourceType() {
     }
@@ -23,7 +23,7 @@
   /**
    * Instances from UsageReporting.
    */
-  @SuppressWarnings({"TypeName", "PMD.ConstantsInInterface"})
+  @SuppressWarnings("TypeName")
   public static final class tInstances {
     private tInstances() {
     }
diff --git a/third_party/allwpilib/hal/src/generate/ResourceType.txt b/third_party/allwpilib/hal/src/generate/ResourceType.txt
index 4ee8eb2..f48cb59 100644
--- a/third_party/allwpilib/hal/src/generate/ResourceType.txt
+++ b/third_party/allwpilib/hal/src/generate/ResourceType.txt
@@ -92,3 +92,4 @@
 kResourceType_DutyCycle = 91
 kResourceType_AddressableLEDs = 92
 kResourceType_FusionVenom = 93
+kResourceType_PS4Controller = 94
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java
index e1e3750..3f22bcb 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccelerometerJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java
index d3aa8c8..9340748 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AccumulatorResult.java
@@ -1,29 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * Structure for holding the values stored in an accumulator.
- */
+/** Structure for holding the values stored in an accumulator. */
 public class AccumulatorResult {
-  /**
-   * The total value accumulated.
-   */
+  /** The total value accumulated. */
   @SuppressWarnings("MemberName")
   public long value;
-  /**
-   * The number of sample value was accumulated over.
-   */
+  /** The number of sample value was accumulated over. */
   @SuppressWarnings("MemberName")
   public long count;
 
   /**
    * Set the value and count.
+   *
+   * @param value The total value accumulated.
+   * @param count The number of samples accumulated.
    */
   public void set(long value, long count) {
     this.value = value;
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
index fb34996..c732ec6 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AddressableLEDJNI.java
@@ -1,23 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 @SuppressWarnings("AbbreviationAsWordInName")
 public class AddressableLEDJNI extends JNIWrapper {
   public static native int initialize(int pwmHandle);
+
   public static native void free(int handle);
 
   public static native void setLength(int handle, int length);
+
   public static native void setData(int handle, byte[] data);
 
-  public static native void setBitTiming(int handle, int lowTime0, int highTime0, int lowTime1, int highTime1);
+  public static native void setBitTiming(
+      int handle, int lowTime0, int highTime0, int lowTime1, int highTime1);
+
   public static native void setSyncTime(int handle, int syncTime);
 
   public static native void start(int handle);
+
   public static native void stop(int handle);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java
index 148dd76..7b601b3 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AllianceStationID.java
@@ -1,12 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public enum AllianceStationID {
-  Red1, Red2, Red3, Blue1, Blue2, Blue3
+  Red1,
+  Red2,
+  Red3,
+  Blue1,
+  Blue2,
+  Blue3
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java
index 332be79..5763f6e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogGyroJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -14,12 +11,11 @@
 
   public static native void freeAnalogGyro(int handle);
 
-  public static native void setAnalogGyroParameters(int handle,
-                                                    double voltsPerDegreePerSecond,
-                                                    double offset, int center);
+  public static native void setAnalogGyroParameters(
+      int handle, double voltsPerDegreePerSecond, double offset, int center);
 
-  public static native void setAnalogGyroVoltsPerDegreePerSecond(int handle,
-                                                                 double voltsPerDegreePerSecond);
+  public static native void setAnalogGyroVoltsPerDegreePerSecond(
+      int handle, double voltsPerDegreePerSecond);
 
   public static native void resetAnalogGyro(int handle);
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
index 449c065..e2deadd 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/AnalogJNI.java
@@ -1,32 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public class AnalogJNI extends JNIWrapper {
   /**
-   * <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:58</i><br> enum values
+   * <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:58</i><br>
+   * enum values
    */
   public interface AnalogTriggerType {
-    /**
-     * <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:54</i>
-     */
+    /** <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:54</i> */
     int kInWindow = 0;
-    /**
-     * <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:55</i>
-     */
+    /** <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:55</i> */
     int kState = 1;
-    /**
-     * <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:56</i>
-     */
+    /** <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:56</i> */
     int kRisingPulse = 2;
-    /**
-     * <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:57</i>
-     */
+    /** <i>native declaration : AthenaJava\target\native\include\HAL\Analog.h:57</i> */
     int kFallingPulse = 3;
   }
 
@@ -68,6 +58,8 @@
 
   public static native int getAnalogVoltsToValue(int analogPortHandle, double voltage);
 
+  public static native double getAnalogValueToVolts(int analogPortHandle, int value);
+
   public static native double getAnalogVoltage(int analogPortHandle);
 
   public static native double getAnalogAverageVoltage(int analogPortHandle);
@@ -98,20 +90,20 @@
 
   public static native void cleanAnalogTrigger(int analogTriggerHandle);
 
-  public static native void setAnalogTriggerLimitsRaw(int analogTriggerHandle, int lower,
-                                                      int upper);
+  public static native void setAnalogTriggerLimitsRaw(
+      int analogTriggerHandle, int lower, int upper);
 
-  public static native void setAnalogTriggerLimitsDutyCycle(int analogTriggerHandle, double lower,
-                                                            double higher);
+  public static native void setAnalogTriggerLimitsDutyCycle(
+      int analogTriggerHandle, double lower, double higher);
 
-  public static native void setAnalogTriggerLimitsVoltage(int analogTriggerHandle,
-                                                          double lower, double upper);
+  public static native void setAnalogTriggerLimitsVoltage(
+      int analogTriggerHandle, double lower, double upper);
 
-  public static native void setAnalogTriggerAveraged(int analogTriggerHandle,
-                                                     boolean useAveragedValue);
+  public static native void setAnalogTriggerAveraged(
+      int analogTriggerHandle, boolean useAveragedValue);
 
-  public static native void setAnalogTriggerFiltered(int analogTriggerHandle,
-                                                     boolean useFilteredValue);
+  public static native void setAnalogTriggerFiltered(
+      int analogTriggerHandle, boolean useFilteredValue);
 
   public static native boolean getAnalogTriggerInWindow(int analogTriggerHandle);
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
index 3507569..31cf973 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANAPIJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -15,15 +12,15 @@
 
   public static native void writeCANPacket(int handle, byte[] data, int apiId);
 
-  public static native void writeCANPacketRepeating(int handle, byte[] data, int apiId,
-                                                    int repeatMs);
+  public static native void writeCANPacketRepeating(
+      int handle, byte[] data, int apiId, int repeatMs);
 
   public static native void writeCANRTRFrame(int handle, int length, int apiId);
 
   public static native int writeCANPacketNoThrow(int handle, byte[] data, int apiId);
 
-  public static native int writeCANPacketRepeatingNoThrow(int handle, byte[] data, int apiId,
-                                                         int repeatMs);
+  public static native int writeCANPacketRepeatingNoThrow(
+      int handle, byte[] data, int apiId, int repeatMs);
 
   public static native int writeCANRTRFrameNoThrow(int handle, int length, int apiId);
 
@@ -33,6 +30,6 @@
 
   public static native boolean readCANPacketLatest(int handle, int apiId, CANData data);
 
-  public static native boolean readCANPacketTimeout(int handle, int apiId, int timeoutMs,
-                                                 CANData data);
+  public static native boolean readCANPacketTimeout(
+      int handle, int apiId, int timeoutMs, CANData data);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java
index 23d37c9..94e9f57 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CANData.java
@@ -1,22 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public class CANData {
   @SuppressWarnings("MemberName")
   public final byte[] data = new byte[8];
+
   @SuppressWarnings("MemberName")
   public int length;
+
   @SuppressWarnings("MemberName")
   public long timestamp;
 
   /**
    * API used from JNI to set the data.
+   *
+   * @param length Length of packet in bytes.
+   * @param timestamp CAN frame timestamp in microseconds.
+   * @return Buffer containing CAN frame.
    */
   @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public byte[] setData(int length, long timestamp) {
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java
new file mode 100644
index 0000000..20d5cb8
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CTREPCMJNI.java
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class CTREPCMJNI extends JNIWrapper {
+  public static native int initialize(int module);
+
+  public static native void free(int handle);
+
+  public static native boolean checkSolenoidChannel(int channel);
+
+  public static native boolean getCompressor(int handle);
+
+  public static native void setClosedLoopControl(int handle, boolean enabled);
+
+  public static native boolean getClosedLoopControl(int handle);
+
+  public static native boolean getPressureSwitch(int handle);
+
+  public static native double getCompressorCurrent(int handle);
+
+  public static native boolean getCompressorCurrentTooHighFault(int handle);
+
+  public static native boolean getCompressorCurrentTooHighStickyFault(int handle);
+
+  public static native boolean getCompressorShortedFault(int handle);
+
+  public static native boolean getCompressorShortedStickyFault(int handle);
+
+  public static native boolean getCompressorNotConnectedFault(int handle);
+
+  public static native boolean getCompressorNotConnectedStickyFault(int handle);
+
+  public static native int getSolenoids(int handle);
+
+  public static native void setSolenoids(int handle, int mask, int values);
+
+  public static native int getSolenoidDisabledList(int handle);
+
+  public static native boolean getSolenoidVoltageFault(int handle);
+
+  public static native boolean getSolenoidVoltageStickyFault(int handle);
+
+  public static native void clearAllStickyFaults(int handle);
+
+  public static native void fireOneShot(int handle, int index);
+
+  public static native void setOneShotDuration(int handle, int index, int durMs);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CompressorJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CompressorJNI.java
deleted file mode 100644
index 64e9e1d..0000000
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CompressorJNI.java
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal;
-
-public class CompressorJNI extends JNIWrapper {
-  public static native int initializeCompressor(byte module);
-
-  public static native boolean checkCompressorModule(byte module);
-
-  public static native boolean getCompressor(int compressorHandle);
-
-  public static native void setCompressorClosedLoopControl(int compressorHandle, boolean value);
-
-  public static native boolean getCompressorClosedLoopControl(int compressorHandle);
-
-  public static native boolean getCompressorPressureSwitch(int compressorHandle);
-
-  public static native double getCompressorCurrent(int compressorHandle);
-
-  public static native boolean getCompressorCurrentTooHighFault(int compressorHandle);
-
-  public static native boolean getCompressorCurrentTooHighStickyFault(int compressorHandle);
-
-  public static native boolean getCompressorShortedStickyFault(int compressorHandle);
-
-  public static native boolean getCompressorShortedFault(int compressorHandle);
-
-  public static native boolean getCompressorNotConnectedStickyFault(int compressorHandle);
-
-  public static native boolean getCompressorNotConnectedFault(int compressorHandle);
-
-  public static native void clearAllPCMStickyFaults(byte compressorModule);
-}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java
index 3db75f5..aba5b33 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ConstantsJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
index 8353606..a568343 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ControlWord.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * A wrapper for the HALControlWord bitfield.
- */
+/** A wrapper for the HALControlWord bitfield. */
 public class ControlWord {
   private boolean m_enabled;
   private boolean m_autonomous;
@@ -18,8 +13,13 @@
   private boolean m_fmsAttached;
   private boolean m_dsAttached;
 
-  void update(boolean enabled, boolean autonomous, boolean test, boolean emergencyStop,
-              boolean fmsAttached, boolean dsAttached) {
+  void update(
+      boolean enabled,
+      boolean autonomous,
+      boolean test,
+      boolean emergencyStop,
+      boolean fmsAttached,
+      boolean dsAttached) {
     m_enabled = enabled;
     m_autonomous = autonomous;
     m_test = test;
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java
index b7935d6..f164dbd 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/CounterJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -16,19 +13,19 @@
 
   public static native void setCounterAverageSize(int counterHandle, int size);
 
-  public static native void setCounterUpSource(int counterHandle, int digitalSourceHandle,
-                                               int analogTriggerType);
+  public static native void setCounterUpSource(
+      int counterHandle, int digitalSourceHandle, int analogTriggerType);
 
-  public static native void setCounterUpSourceEdge(int counterHandle, boolean risingEdge,
-                                                   boolean fallingEdge);
+  public static native void setCounterUpSourceEdge(
+      int counterHandle, boolean risingEdge, boolean fallingEdge);
 
   public static native void clearCounterUpSource(int counterHandle);
 
-  public static native void setCounterDownSource(int counterHandle, int digitalSourceHandle,
-                                                 int analogTriggerType);
+  public static native void setCounterDownSource(
+      int counterHandle, int digitalSourceHandle, int analogTriggerType);
 
-  public static native void setCounterDownSourceEdge(int counterHandle, boolean risingEdge,
-                                                     boolean fallingEdge);
+  public static native void setCounterDownSourceEdge(
+      int counterHandle, boolean risingEdge, boolean fallingEdge);
 
   public static native void clearCounterDownSource(int counterHandle);
 
@@ -36,15 +33,13 @@
 
   public static native void setCounterExternalDirectionMode(int counterHandle);
 
-  public static native void setCounterSemiPeriodMode(int counterHandle,
-                                                     boolean highSemiPeriod);
+  public static native void setCounterSemiPeriodMode(int counterHandle, boolean highSemiPeriod);
 
   public static native void setCounterPulseLengthMode(int counterHandle, double threshold);
 
   public static native int getCounterSamplesToAverage(int counterHandle);
 
-  public static native void setCounterSamplesToAverage(int counterHandle,
-                                                       int samplesToAverage);
+  public static native void setCounterSamplesToAverage(int counterHandle, int samplesToAverage);
 
   public static native void resetCounter(int counterHandle);
 
@@ -60,6 +55,5 @@
 
   public static native boolean getCounterDirection(int counterHandle);
 
-  public static native void setCounterReverseDirection(int counterHandle,
-                                                       boolean reverseDirection);
+  public static native void setCounterReverseDirection(int counterHandle, boolean reverseDirection);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
index ad83c3c..dab1aaf 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DIOJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -17,8 +14,7 @@
 
   public static native void setDIOSimDevice(int handle, int device);
 
-  // TODO(Thad): Switch this to use boolean
-  public static native void setDIO(int dioPortHandle, short value);
+  public static native void setDIO(int dioPortHandle, boolean value);
 
   public static native void setDIODirection(int dioPortHandle, boolean input);
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java
new file mode 100644
index 0000000..21c06f1
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNI.java
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class DMAJNI extends JNIWrapper {
+  public static native int initialize();
+
+  public static native void free(int handle);
+
+  public static native void setPause(int handle, boolean pause);
+
+  public static native void setTimedTrigger(int handle, double periodSeconds);
+
+  public static native void setTimedTriggerCycles(int handle, int cycles);
+
+  public static native void addEncoder(int handle, int encoderHandle);
+
+  public static native void addEncoderPeriod(int handle, int encoderHandle);
+
+  public static native void addCounter(int handle, int counterHandle);
+
+  public static native void addCounterPeriod(int handle, int counterHandle);
+
+  public static native void addDigitalSource(int handle, int digitalSourceHandle);
+
+  public static native void addDutyCycle(int handle, int dutyCycleHandle);
+
+  public static native void addAnalogInput(int handle, int analogInputHandle);
+
+  public static native void addAveragedAnalogInput(int handle, int analogInputHandle);
+
+  public static native void addAnalogAccumulator(int handle, int analogInputHandle);
+
+  public static native int setExternalTrigger(
+      int handle, int digitalSourceHandle, int analogTriggerType, boolean rising, boolean falling);
+
+  public static native void clearSensors(int handle);
+
+  public static native void clearExternalTriggers(int handle);
+
+  public static native void startDMA(int handle, int queueDepth);
+
+  public static native void stopDMA(int handle);
+
+  // 0-21 channelOffsets
+  // 22: capture size
+  // 23: triggerChannels (bitflags)
+  // 24: remaining
+  // 25: read status
+  public static native long readDMA(
+      int handle, double timeoutSeconds, int[] buffer, int[] sampleStore);
+
+  public static native DMAJNISample.BaseStore getSensorReadData(int handle);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java
new file mode 100644
index 0000000..78a0e99
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DMAJNISample.java
@@ -0,0 +1,169 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+import java.util.HashMap;
+import java.util.Map;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class DMAJNISample {
+  private static final int kEnable_Accumulator0 = 8;
+  private static final int kEnable_Accumulator1 = 9;
+
+  static class BaseStore {
+    public final int m_valueType;
+    public final int m_index;
+
+    BaseStore(int valueType, int index) {
+      this.m_valueType = valueType;
+      this.m_index = index;
+    }
+  }
+
+  private final int[] m_dataBuffer = new int[100];
+  private final int[] m_storage = new int[100];
+  private long m_timeStamp;
+  private Map<Integer, BaseStore> m_propertyMap = new HashMap<>();
+
+  public int update(int dmaHandle, double timeoutSeconds) {
+    m_timeStamp = DMAJNI.readDMA(dmaHandle, timeoutSeconds, m_dataBuffer, m_storage);
+    return m_storage[25];
+  }
+
+  public int getCaptureSize() {
+    return m_storage[22];
+  }
+
+  public int getTriggerChannels() {
+    return m_storage[23];
+  }
+
+  public int getRemaining() {
+    return m_storage[24];
+  }
+
+  public long getTime() {
+    return m_timeStamp;
+  }
+
+  private BaseStore addSensorInternal(int handle) {
+    BaseStore sensorData = DMAJNI.getSensorReadData(handle);
+    m_propertyMap.put(handle, sensorData);
+    return sensorData;
+  }
+
+  public void addSensor(int handle) {
+    addSensorInternal(handle);
+  }
+
+  private int readValue(int valueType, int index) {
+    int offset = m_storage[valueType];
+    if (offset == -1) {
+      throw new RuntimeException("Resource not found in DMA capture");
+    }
+    return m_dataBuffer[offset + index];
+  }
+
+  public int getEncoder(int encoderHandle) {
+    BaseStore data = m_propertyMap.get(encoderHandle);
+    if (data == null) {
+      data = addSensorInternal(encoderHandle);
+    }
+    return readValue(data.m_valueType, data.m_index);
+  }
+
+  public int getEncoderPeriod(int encoderHandle) {
+    BaseStore data = m_propertyMap.get(encoderHandle);
+    if (data == null) {
+      data = addSensorInternal(encoderHandle);
+    }
+    // + 2 Hack, but needed to not have to call into JNI
+    return readValue(data.m_valueType + 2, data.m_index);
+  }
+
+  public int getCounter(int counterHandle) {
+    BaseStore data = m_propertyMap.get(counterHandle);
+    if (data == null) {
+      data = addSensorInternal(counterHandle);
+    }
+    return readValue(data.m_valueType, data.m_index);
+  }
+
+  public int getCounterPeriod(int counterHandle) {
+    BaseStore data = m_propertyMap.get(counterHandle);
+    if (data == null) {
+      data = addSensorInternal(counterHandle);
+    }
+    // Hack, but needed to not have to call into JNI
+    return readValue(data.m_valueType + 2, data.m_index);
+  }
+
+  public boolean getDigitalSource(int digitalSourceHandle) {
+    BaseStore data = m_propertyMap.get(digitalSourceHandle);
+    if (data == null) {
+      data = addSensorInternal(digitalSourceHandle);
+    }
+
+    int value = readValue(data.m_valueType, 0);
+
+    return ((value >> data.m_index) & 0x1) != 0;
+  }
+
+  public int getAnalogInput(int analogInputHandle) {
+    BaseStore data = m_propertyMap.get(analogInputHandle);
+    if (data == null) {
+      data = addSensorInternal(analogInputHandle);
+    }
+
+    int value = readValue(data.m_valueType, data.m_index / 2);
+    if ((data.m_index % 2) != 0) {
+      return (value >>> 16) & 0xFFFF;
+    } else {
+      return value & 0xFFFF;
+    }
+  }
+
+  public int getAnalogInputAveraged(int analogInputHandle) {
+    BaseStore data = m_propertyMap.get(analogInputHandle);
+    if (data == null) {
+      data = addSensorInternal(analogInputHandle);
+    }
+
+    // + 2 Hack, but needed to not have to call into JNI
+    int value = readValue(data.m_valueType + 2, data.m_index);
+    return value;
+  }
+
+  public void getAnalogAccumulator(int analogInputHandle, AccumulatorResult result) {
+    BaseStore data = m_propertyMap.get(analogInputHandle);
+    if (data == null) {
+      data = addSensorInternal(analogInputHandle);
+    }
+
+    if (data.m_index == 0) {
+      int val0 = readValue(kEnable_Accumulator0, 0);
+      int val1 = readValue(kEnable_Accumulator0, 1);
+      int val2 = readValue(kEnable_Accumulator0, 2);
+      result.count = val2;
+      result.value = ((long) val1 << 32) | val0;
+    } else if (data.m_index == 1) {
+      int val0 = readValue(kEnable_Accumulator1, 0);
+      int val1 = readValue(kEnable_Accumulator1, 1);
+      int val2 = readValue(kEnable_Accumulator1, 2);
+      result.count = val2;
+      result.value = ((long) val1 << 32) | val0;
+    } else {
+      throw new RuntimeException("Resource not found in DMA capture");
+    }
+  }
+
+  public int getDutyCycleOutput(int dutyCycleHandle) {
+    BaseStore data = m_propertyMap.get(dutyCycleHandle);
+    if (data == null) {
+      data = addSensorInternal(dutyCycleHandle);
+    }
+    return readValue(data.m_valueType, data.m_index);
+  }
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java
index aa818cf..cf0a781 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DigitalGlitchFilterJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
index f0f93e1..a1bba6f 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/DutyCycleJNI.java
@@ -1,19 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public class DutyCycleJNI extends JNIWrapper {
   public static native int initialize(int digitalSourceHandle, int analogTriggerType);
+
   public static native void free(int handle);
 
   public static native int getFrequency(int handle);
+
   public static native double getOutput(int handle);
+
   public static native int getOutputRaw(int handle);
+
   public static native int getOutputScaleFactor(int handle);
 
   @SuppressWarnings("AbbreviationAsWordInName")
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java
index 40fa1c1..f67e3a5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/EncoderJNI.java
@@ -1,16 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public class EncoderJNI extends JNIWrapper {
-  public static native int initializeEncoder(int digitalSourceHandleA, int analogTriggerTypeA,
-                                             int digitalSourceHandleB, int analogTriggerTypeB,
-                                             boolean reverseDirection, int encodingType);
+  public static native int initializeEncoder(
+      int digitalSourceHandleA,
+      int analogTriggerTypeA,
+      int digitalSourceHandleB,
+      int analogTriggerTypeB,
+      boolean reverseDirection,
+      int encodingType);
 
   public static native void freeEncoder(int encoderHandle);
 
@@ -40,16 +41,14 @@
 
   public static native void setEncoderDistancePerPulse(int encoderHandle, double distancePerPulse);
 
-  public static native void setEncoderReverseDirection(int encoderHandle,
-                                                       boolean reverseDirection);
+  public static native void setEncoderReverseDirection(int encoderHandle, boolean reverseDirection);
 
-  public static native void setEncoderSamplesToAverage(int encoderHandle,
-                                                       int samplesToAverage);
+  public static native void setEncoderSamplesToAverage(int encoderHandle, int samplesToAverage);
 
   public static native int getEncoderSamplesToAverage(int encoderHandle);
 
-  public static native void setEncoderIndexSource(int encoderHandle, int digitalSourceHandle,
-                                                  int analogTriggerType, int indexingType);
+  public static native void setEncoderIndexSource(
+      int encoderHandle, int digitalSourceHandle, int analogTriggerType, int indexingType);
 
   @SuppressWarnings("AbbreviationAsWordInName")
   public static native int getEncoderFPGAIndex(int encoderHandle);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HAL.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HAL.java
index 5e07488..af97450 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HAL.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HAL.java
@@ -1,16 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.List;
 
 /**
- * JNI Wrapper for HAL<br>.
+ * JNI Wrapper for HAL<br>
+ * .
  */
 @SuppressWarnings({"AbbreviationAsWordInName", "MethodName"})
 public final class HAL extends JNIWrapper {
@@ -26,6 +26,97 @@
 
   public static native void exitMain();
 
+  private static native void simPeriodicBeforeNative();
+
+  private static final List<Runnable> s_simPeriodicBefore = new ArrayList<>();
+
+  public static class SimPeriodicBeforeCallback implements AutoCloseable {
+    private SimPeriodicBeforeCallback(Runnable r) {
+      m_run = r;
+    }
+
+    @Override
+    public void close() {
+      synchronized (s_simPeriodicBefore) {
+        s_simPeriodicBefore.remove(m_run);
+      }
+    }
+
+    private final Runnable m_run;
+  }
+
+  /**
+   * Registers a callback to be run by IterativeRobotBase prior to the user's simulationPeriodic
+   * code.
+   *
+   * @param r runnable
+   * @return Callback object (must be retained for callback to stay active).
+   */
+  public static SimPeriodicBeforeCallback registerSimPeriodicBeforeCallback(Runnable r) {
+    synchronized (s_simPeriodicBefore) {
+      s_simPeriodicBefore.add(r);
+    }
+    return new SimPeriodicBeforeCallback(r);
+  }
+
+  /**
+   * Runs SimPeriodicBefore callbacks. IterativeRobotBase calls this prior to the user's
+   * simulationPeriodic code.
+   */
+  public static void simPeriodicBefore() {
+    simPeriodicBeforeNative();
+    synchronized (s_simPeriodicBefore) {
+      for (Runnable r : s_simPeriodicBefore) {
+        r.run();
+      }
+    }
+  }
+
+  private static native void simPeriodicAfterNative();
+
+  private static final List<Runnable> s_simPeriodicAfter = new ArrayList<>();
+
+  public static class SimPeriodicAfterCallback implements AutoCloseable {
+    private SimPeriodicAfterCallback(Runnable r) {
+      m_run = r;
+    }
+
+    @Override
+    public void close() {
+      synchronized (s_simPeriodicAfter) {
+        s_simPeriodicAfter.remove(m_run);
+      }
+    }
+
+    private final Runnable m_run;
+  }
+
+  /**
+   * Registers a callback to be run by IterativeRobotBase after the user's simulationPeriodic code.
+   *
+   * @param r runnable
+   * @return Callback object (must be retained for callback to stay active).
+   */
+  public static SimPeriodicAfterCallback registerSimPeriodicAfterCallback(Runnable r) {
+    synchronized (s_simPeriodicAfter) {
+      s_simPeriodicAfter.add(r);
+    }
+    return new SimPeriodicAfterCallback(r);
+  }
+
+  /**
+   * Runs SimPeriodicAfter callbacks. IterativeRobotBase calls this after the user's
+   * simulationPeriodic code.
+   */
+  public static void simPeriodicAfter() {
+    simPeriodicAfterNative();
+    synchronized (s_simPeriodicAfter) {
+      for (Runnable r : s_simPeriodicAfter) {
+        r.run();
+      }
+    }
+  }
+
   public static native void observeUserProgramStarting();
 
   public static native void observeUserProgramDisabled();
@@ -45,33 +136,38 @@
   }
 
   /**
-   * Report the usage of a resource of interest. <br>
+   * Report the usage of a resource of interest.
    *
    * <p>Original signature: <code>uint32_t report(tResourceType, uint8_t, uint8_t, const
    * char*)</code>
    *
-   * @param resource       one of the values in the tResourceType above (max value 51). <br>
-   * @param instanceNumber an index that identifies the resource instance. <br>
-   * @param context        an optional additional context number for some cases (such as module
-   *                       number). Set to 0 to omit. <br>
-   * @param feature        a string to be included describing features in use on a specific
-   *                       resource. Setting the same resource more than once allows you to change
-   *                       the feature string.
+   * @param resource one of the values in the tResourceType above (max value 51).
+   * @param instanceNumber an index that identifies the resource instance.
+   * @param context an optional additional context number for some cases (such as module number).
+   *     Set to 0 to omit.
+   * @param feature a string to be included describing features in use on a specific resource.
+   *     Setting the same resource more than once allows you to change the feature string.
+   * @return TODO
    */
   public static native int report(int resource, int instanceNumber, int context, String feature);
 
   public static native int nativeGetControlWord();
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static void getControlWord(ControlWord controlWord) {
     int word = nativeGetControlWord();
-    controlWord.update((word & 1) != 0, ((word >> 1) & 1) != 0, ((word >> 2) & 1) != 0,
-        ((word >> 3) & 1) != 0, ((word >> 4) & 1) != 0, ((word >> 5) & 1) != 0);
+    controlWord.update(
+        (word & 1) != 0,
+        ((word >> 1) & 1) != 0,
+        ((word >> 2) & 1) != 0,
+        ((word >> 3) & 1) != 0,
+        ((word >> 4) & 1) != 0,
+        ((word >> 5) & 1) != 0);
   }
 
   private static native int nativeGetAllianceStation();
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static AllianceStationID getAllianceStation() {
     switch (nativeGetAllianceStation()) {
       case 0:
@@ -91,13 +187,13 @@
     }
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static native boolean isNewControlData();
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static native void releaseDSMutex();
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static native boolean waitForDSDataTimeout(double timeout);
 
   public static int kMaxJoystickAxes = 12;
@@ -109,8 +205,8 @@
 
   public static native int getJoystickButtons(byte joystickNum, ByteBuffer count);
 
-  public static native int setJoystickOutputs(byte joystickNum, int outputs, short leftRumble,
-                                              short rightRumble);
+  public static native int setJoystickOutputs(
+      byte joystickNum, int outputs, short leftRumble, short rightRumble);
 
   public static native int getJoystickIsXbox(byte joystickNum);
 
@@ -128,9 +224,14 @@
 
   public static native int getMatchInfo(MatchInfoData info);
 
-  public static native int sendError(boolean isError, int errorCode, boolean isLVCode,
-                                     String details, String location, String callStack,
-                                     boolean printMsg);
+  public static native int sendError(
+      boolean isError,
+      int errorCode,
+      boolean isLVCode,
+      String details,
+      String location,
+      String callStack,
+      boolean printMsg);
 
   public static native int sendConsoleLine(String line);
 
@@ -138,7 +239,5 @@
 
   public static native int getPort(byte channel);
 
-  private HAL() {
-
-  }
+  private HAL() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java
index 0e5fe21..4da20a2 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALUtil.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -18,6 +15,10 @@
   public static final int NO_AVAILABLE_RESOURCES = -104;
   public static final int PARAMETER_OUT_OF_RANGE = -1028;
 
+  public static final int RUNTIME_ROBORIO = 0;
+  public static final int RUNTIME_ROBORIO2 = 1;
+  public static final int RUNTIME_SIMULATION = 2;
+
   public static native short getFPGAVersion();
 
   public static native int getFPGARevision();
@@ -38,7 +39,5 @@
     return getHALstrerror(getHALErrno());
   }
 
-  private HALUtil() {
-
-  }
+  private HALUtil() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALValue.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALValue.java
index 636af8f..ded57de 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALValue.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/HALValue.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -30,9 +27,7 @@
     m_long = value;
   }
 
-  private HALValue() {
-
-  }
+  private HALValue() {}
 
   /**
    * Get the type of the value.
@@ -44,7 +39,7 @@
   }
 
   /**
-   * Get the value as a boolean.  Does not perform type checking.
+   * Get the value as a boolean. Does not perform type checking.
    *
    * @return value contents
    */
@@ -53,7 +48,7 @@
   }
 
   /**
-   * Get the value as a long.  Does not perform type checking.
+   * Get the value as a long. Does not perform type checking.
    *
    * @return value contents
    */
@@ -62,7 +57,7 @@
   }
 
   /**
-   * Get the value as a double.  Does not perform type checking.
+   * Get the value as a double. Does not perform type checking.
    *
    * @return value contents
    */
@@ -71,7 +66,7 @@
   }
 
   /**
-   * Get the native long value.  Does not perform type checking.
+   * Get the native long value. Does not perform type checking.
    *
    * @return value contents
    */
@@ -80,7 +75,7 @@
   }
 
   /**
-   * Get the native double value.  Does not perform type checking.
+   * Get the native double value. Does not perform type checking.
    *
    * @return value contents
    */
@@ -152,15 +147,15 @@
    */
   public static HALValue fromNative(int type, long value1, double value2) {
     switch (type) {
-      case 0x01:
+      case kBoolean:
         return makeBoolean(value1 != 0);
-      case 0x02:
+      case kDouble:
         return makeDouble(value2);
-      case 0x16:
+      case kEnum:
         return makeEnum((int) value1);
-      case 0x32:
+      case kInt:
         return makeInt((int) value1);
-      case 0x64:
+      case kLong:
         return makeLong(value1);
       default:
         return makeUnassigned();
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java
index 12a9af0..821c89b 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/I2CJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -13,21 +10,30 @@
 public class I2CJNI extends JNIWrapper {
   public static native void i2CInitialize(int port);
 
-  public static native int i2CTransaction(int port, byte address, ByteBuffer dataToSend,
-                                          byte sendSize, ByteBuffer dataReceived, byte receiveSize);
+  public static native int i2CTransaction(
+      int port,
+      byte address,
+      ByteBuffer dataToSend,
+      byte sendSize,
+      ByteBuffer dataReceived,
+      byte receiveSize);
 
-  public static native int i2CTransactionB(int port, byte address, byte[] dataToSend,
-                                           byte sendSize, byte[] dataReceived, byte receiveSize);
+  public static native int i2CTransactionB(
+      int port,
+      byte address,
+      byte[] dataToSend,
+      byte sendSize,
+      byte[] dataReceived,
+      byte receiveSize);
 
   public static native int i2CWrite(int port, byte address, ByteBuffer dataToSend, byte sendSize);
 
   public static native int i2CWriteB(int port, byte address, byte[] dataToSend, byte sendSize);
 
-  public static native int i2CRead(int port, byte address, ByteBuffer dataReceived,
-                                   byte receiveSize);
+  public static native int i2CRead(
+      int port, byte address, ByteBuffer dataReceived, byte receiveSize);
 
-  public static native int i2CReadB(int port, byte address, byte[] dataReceived,
-                                    byte receiveSize);
+  public static native int i2CReadB(int port, byte address, byte[] dataReceived, byte receiveSize);
 
   public static native void i2CClose(int port);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
index e370d5b..a47a364 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/InterruptJNI.java
@@ -1,43 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public class InterruptJNI extends JNIWrapper {
   public static final int HalInvalidHandle = 0;
 
-  public interface InterruptJNIHandlerFunction {
-    void apply(int interruptAssertedMask, Object param);
-  }
-
-  public static native int initializeInterrupts(boolean watcher);
+  public static native int initializeInterrupts();
 
   public static native void cleanInterrupts(int interruptHandle);
 
-  public static native int waitForInterrupt(int interruptHandle, double timeout,
-                                            boolean ignorePrevious);
-
-  public static native void enableInterrupts(int interruptHandle);
-
-  public static native void disableInterrupts(int interruptHandle);
+  public static native int waitForInterrupt(
+      int interruptHandle, double timeout, boolean ignorePrevious);
 
   public static native long readInterruptRisingTimestamp(int interruptHandle);
 
   public static native long readInterruptFallingTimestamp(int interruptHandle);
 
-  public static native void requestInterrupts(int interruptHandle, int digitalSourceHandle,
-                                              int analogTriggerType);
+  public static native void requestInterrupts(
+      int interruptHandle, int digitalSourceHandle, int analogTriggerType);
 
-  public static native void attachInterruptHandler(int interruptHandle,
-                                                   InterruptJNIHandlerFunction handler,
-                                                   Object param);
-
-  public static native void setInterruptUpSourceEdge(int interruptHandle, boolean risingEdge,
-                                                     boolean fallingEdge);
+  public static native void setInterruptUpSourceEdge(
+      int interruptHandle, boolean risingEdge, boolean fallingEdge);
 
   public static native void releaseWaitingInterrupt(int interruptHandle);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java
index 63ad39f..b2b54da 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/JNIWrapper.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
+import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
 
-import edu.wpi.first.wpiutil.RuntimeLoader;
-
-/**
- * Base class for all JNI wrappers.
- */
+/** Base class for all JNI wrappers. */
 public class JNIWrapper {
   static boolean libraryLoaded = false;
   static RuntimeLoader<JNIWrapper> loader = null;
@@ -34,7 +28,9 @@
   static {
     if (Helper.getExtractOnStaticLoad()) {
       try {
-        loader = new RuntimeLoader<>("wpiHaljni", RuntimeLoader.getDefaultExtractionRoot(), JNIWrapper.class);
+        loader =
+            new RuntimeLoader<>(
+                "wpiHaljni", RuntimeLoader.getDefaultExtractionRoot(), JNIWrapper.class);
         loader.loadLibrary();
       } catch (IOException ex) {
         ex.printStackTrace();
@@ -46,12 +42,16 @@
 
   /**
    * Force load the library.
+   *
+   * @throws IOException if the library load failed
    */
   public static synchronized void forceLoad() throws IOException {
     if (libraryLoaded) {
       return;
     }
-    loader = new RuntimeLoader<>("wpiHaljni", RuntimeLoader.getDefaultExtractionRoot(), JNIWrapper.class);
+    loader =
+        new RuntimeLoader<>(
+            "wpiHaljni", RuntimeLoader.getDefaultExtractionRoot(), JNIWrapper.class);
     loader.loadLibrary();
     libraryLoaded = true;
   }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java
index 2f11515..6737c58 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/MatchInfoData.java
@@ -1,52 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * Structure for holding the match info data request.
- */
+/** Structure for holding the match info data request. */
 public class MatchInfoData {
-  /**
-   * Stores the event name.
-   */
+  /** Stores the event name. */
   @SuppressWarnings("MemberName")
   public String eventName = "";
 
-  /**
-   * Stores the game specific message.
-   */
+  /** Stores the game specific message. */
   @SuppressWarnings("MemberName")
   public String gameSpecificMessage = "";
 
-  /**
-   * Stores the match number.
-   */
+  /** Stores the match number. */
   @SuppressWarnings("MemberName")
   public int matchNumber;
 
-  /**
-   * Stores the replay number.
-   */
+  /** Stores the replay number. */
   @SuppressWarnings("MemberName")
   public int replayNumber;
 
-  /**
-   * Stores the match type.
-   */
+  /** Stores the match type. */
   @SuppressWarnings("MemberName")
   public int matchType;
 
   /**
    * Called from JNI to set the structure data.
+   *
+   * @param eventName Event name.
+   * @param gameSpecificMessage Game-specific message.
+   * @param matchNumber Match number.
+   * @param replayNumber Replay number.
+   * @param matchType Match type.
    */
-  @SuppressWarnings("JavadocMethod")
-  public void setData(String eventName, String gameSpecificMessage,
-                      int matchNumber, int replayNumber, int matchType) {
+  @SuppressWarnings("MissingJavadocMethod")
+  public void setData(
+      String eventName,
+      String gameSpecificMessage,
+      int matchNumber,
+      int replayNumber,
+      int matchType) {
     this.eventName = eventName;
     this.gameSpecificMessage = gameSpecificMessage;
     this.matchNumber = matchNumber;
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
index 000c7d6..c8f4eef 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/NotifierJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -16,38 +13,63 @@
 public class NotifierJNI extends JNIWrapper {
   /**
    * Initializes the notifier.
+   *
+   * @return True on success.
    */
   public static native int initializeNotifier();
 
   /**
+   * Sets the HAL notifier thread priority.
+   *
+   * @param realTime Set to true to set a real-time priority, false for standard priority.
+   * @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being
+   *     highest. For non-real-time, this is forced to 0. See "man 7 sched" for more details.
+   * @return True on success.
+   */
+  public static native boolean setHALThreadPriority(boolean realTime, int priority);
+
+  /**
    * Sets the name of the notifier.
+   *
+   * @param notifierHandle Notifier handle.
+   * @param name Notifier name.
    */
   public static native void setNotifierName(int notifierHandle, String name);
 
   /**
-   * Wakes up the waiter with time=0.  Note: after this function is called, all
-   * calls to waitForNotifierAlarm() will immediately start returning 0.
+   * Wakes up the waiter with time=0. Note: after this function is called, all calls to
+   * waitForNotifierAlarm() will immediately start returning 0.
+   *
+   * @param notifierHandle Notifier handle.
    */
   public static native void stopNotifier(int notifierHandle);
 
   /**
    * Deletes the notifier object when we are done with it.
+   *
+   * @param notifierHandle Notifier handle.
    */
   public static native void cleanNotifier(int notifierHandle);
 
   /**
    * Sets the notifier to wakeup the waiter in another triggerTime microseconds.
+   *
+   * @param notifierHandle Notifier handle.
+   * @param triggerTime Trigger time in microseconds.
    */
   public static native void updateNotifierAlarm(int notifierHandle, long triggerTime);
 
   /**
-   * Cancels any pending wakeups set by updateNotifierAlarm().  Does NOT wake
-   * up any waiters.
+   * Cancels any pending wakeups set by updateNotifierAlarm(). Does NOT wake up any waiters.
+   *
+   * @param notifierHandle Notifier handle.
    */
   public static native void cancelNotifierAlarm(int notifierHandle);
 
   /**
    * Block until woken up by an alarm (or stop).
+   *
+   * @param notifierHandle Notifier handle.
    * @return Time when woken up.
    */
   public static native long waitForNotifierAlarm(int notifierHandle);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java
deleted file mode 100644
index e2d7fcc..0000000
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PDPJNI.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal;
-
-@SuppressWarnings("AbbreviationAsWordInName")
-public class PDPJNI extends JNIWrapper {
-  public static native int initializePDP(int module);
-
-  public static native boolean checkPDPModule(int module);
-
-  public static native boolean checkPDPChannel(int channel);
-
-  public static native double getPDPTemperature(int handle);
-
-  public static native double getPDPVoltage(int handle);
-
-  public static native double getPDPChannelCurrent(byte channel, int handle);
-
-  public static native void getPDPAllCurrents(int handle, double[] currents);
-
-  public static native double getPDPTotalCurrent(int handle);
-
-  public static native double getPDPTotalPower(int handle);
-
-  public static native double getPDPTotalEnergy(int handle);
-
-  public static native void resetPDPTotalEnergy(int handle);
-
-  public static native void clearPDPStickyFaults(int handle);
-}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMConfigDataResult.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMConfigDataResult.java
index d29b0df..e64d6da 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMConfigDataResult.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMConfigDataResult.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * Structure for holding the config data result for PWM.
- */
+/** Structure for holding the config data result for PWM. */
 public class PWMConfigDataResult {
   PWMConfigDataResult(int max, int deadbandMax, int center, int deadbandMin, int min) {
     this.max = max;
@@ -19,33 +14,23 @@
     this.min = min;
   }
 
-  /**
-   * The maximum PWM value.
-   */
+  /** The maximum PWM value. */
   @SuppressWarnings("MemberName")
   public int max;
 
-  /**
-   * The deadband maximum PWM value.
-   */
+  /** The deadband maximum PWM value. */
   @SuppressWarnings("MemberName")
   public int deadbandMax;
 
-  /**
-   * The center PWM value.
-   */
+  /** The center PWM value. */
   @SuppressWarnings("MemberName")
   public int center;
 
-  /**
-   * The deadband minimum PWM value.
-   */
+  /** The deadband minimum PWM value. */
   @SuppressWarnings("MemberName")
   public int deadbandMin;
 
-  /**
-   * The minimum PWM value.
-   */
+  /** The minimum PWM value. */
   @SuppressWarnings("MemberName")
   public int min;
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java
index 3af6f96..946ad07 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PWMJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -15,13 +12,21 @@
 
   public static native void freePWMPort(int pwmPortHandle);
 
-  public static native void setPWMConfigRaw(int pwmPortHandle, int maxPwm,
-                                            int deadbandMaxPwm, int centerPwm,
-                                            int deadbandMinPwm, int minPwm);
+  public static native void setPWMConfigRaw(
+      int pwmPortHandle,
+      int maxPwm,
+      int deadbandMaxPwm,
+      int centerPwm,
+      int deadbandMinPwm,
+      int minPwm);
 
-  public static native void setPWMConfig(int pwmPortHandle, double maxPwm,
-                                         double deadbandMaxPwm, double centerPwm,
-                                         double deadbandMinPwm, double minPwm);
+  public static native void setPWMConfig(
+      int pwmPortHandle,
+      double maxPwm,
+      double deadbandMaxPwm,
+      double centerPwm,
+      double deadbandMinPwm,
+      double minPwm);
 
   public static native PWMConfigDataResult getPWMConfigRaw(int pwmPortHandle);
 
@@ -41,7 +46,7 @@
 
   public static native double getPWMPosition(int pwmPortHandle);
 
-  public static native  void setPWMDisabled(int pwmPortHandle);
+  public static native void setPWMDisabled(int pwmPortHandle);
 
   public static native void latchPWMZero(int pwmPortHandle);
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java
index a2ac60b..6a06ff9 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PortsJNI.java
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
+@SuppressWarnings("AbbreviationAsWordInName")
 public class PortsJNI extends JNIWrapper {
   public static native int getNumAccumulators();
 
@@ -36,11 +34,19 @@
 
   public static native int getNumRelayHeaders();
 
-  public static native int getNumPCMModules();
+  public static native int getNumCTREPCMModules();
 
-  public static native int getNumSolenoidChannels();
+  public static native int getNumCTRESolenoidChannels();
 
-  public static native int getNumPDPModules();
+  public static native int getNumCTREPDPModules();
 
-  public static native int getNumPDPChannels();
+  public static native int getNumCTREPDPChannels();
+
+  public static native int getNumREVPDHModules();
+
+  public static native int getNumREVPDHChannels();
+
+  public static native int getNumREVPHModules();
+
+  public static native int getNumREVPHChannels();
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java
new file mode 100644
index 0000000..b2ac61a
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerDistributionJNI.java
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class PowerDistributionJNI extends JNIWrapper {
+  public static final int AUTOMATIC_TYPE = 0;
+  public static final int CTRE_TYPE = 1;
+  public static final int REV_TYPE = 2;
+  public static final int DEFAULT_MODULE = -1;
+
+  public static native int initialize(int module, int type);
+
+  public static native void free(int handle);
+
+  public static native int getModuleNumber(int handle);
+
+  public static native boolean checkModule(int module, int type);
+
+  public static native boolean checkChannel(int handle, int channel);
+
+  public static native int getType(int handle);
+
+  public static native int getNumChannels(int handle);
+
+  public static native double getTemperature(int handle);
+
+  public static native double getVoltage(int handle);
+
+  public static native double getChannelCurrent(int handle, int channel);
+
+  public static native void getAllCurrents(int handle, double[] currents);
+
+  public static native double getTotalCurrent(int handle);
+
+  public static native double getTotalPower(int handle);
+
+  public static native double getTotalEnergy(int handle);
+
+  public static native void resetTotalEnergy(int handle);
+
+  public static native void clearStickyFaults(int handle);
+
+  public static native boolean getSwitchableChannel(int handle);
+
+  public static native void setSwitchableChannel(int handle, boolean enabled);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java
index 7a0a376..0bea6ed 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/PowerJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -35,4 +32,8 @@
   public static native boolean getUserActive3V3();
 
   public static native int getUserCurrentFaults3V3();
+
+  public static native void setBrownoutVoltage(double voltage);
+
+  public static native double getBrownoutVoltage();
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java
new file mode 100644
index 0000000..a164106
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/REVPHJNI.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class REVPHJNI extends JNIWrapper {
+  public static native int initialize(int module);
+
+  public static native void free(int handle);
+
+  public static native boolean checkSolenoidChannel(int channel);
+
+  public static native boolean getCompressor(int handle);
+
+  public static native void setClosedLoopControl(int handle, boolean enabled);
+
+  public static native boolean getClosedLoopControl(int handle);
+
+  public static native boolean getPressureSwitch(int handle);
+
+  public static native double getAnalogPressure(int handle, int channel);
+
+  public static native double getCompressorCurrent(int handle);
+
+  public static native int getSolenoids(int handle);
+
+  public static native void setSolenoids(int handle, int mask, int values);
+
+  public static native void fireOneShot(int handle, int index, int durMs);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java
index 1b507dc..eee2854 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/RelayJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
index c203213..05ac08a 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
@@ -13,11 +10,11 @@
 public class SPIJNI extends JNIWrapper {
   public static native void spiInitialize(int port);
 
-  public static native int spiTransaction(int port, ByteBuffer dataToSend,
-                                          ByteBuffer dataReceived, byte size);
+  public static native int spiTransaction(
+      int port, ByteBuffer dataToSend, ByteBuffer dataReceived, byte size);
 
-  public static native int spiTransactionB(int port, byte[] dataToSend,
-                                           byte[] dataReceived, byte size);
+  public static native int spiTransactionB(
+      int port, byte[] dataToSend, byte[] dataReceived, byte size);
 
   public static native int spiWrite(int port, ByteBuffer dataToSend, byte sendSize);
 
@@ -31,8 +28,8 @@
 
   public static native void spiSetSpeed(int port, int speed);
 
-  public static native void spiSetOpts(int port, int msbFirst, int sampleOnTrailing,
-                                       int clkIdleHigh);
+  public static native void spiSetOpts(
+      int port, int msbFirst, int sampleOnTrailing, int clkIdleHigh);
 
   public static native void spiSetChipSelectActiveHigh(int port);
 
@@ -44,9 +41,12 @@
 
   public static native void spiStartAutoRate(int port, double period);
 
-  public static native void spiStartAutoTrigger(int port, int digitalSourceHandle,
-                                                int analogTriggerType, boolean triggerRising,
-                                                boolean triggerFalling);
+  public static native void spiStartAutoTrigger(
+      int port,
+      int digitalSourceHandle,
+      int analogTriggerType,
+      boolean triggerRising,
+      boolean triggerFalling);
 
   public static native void spiStopAuto(int port);
 
@@ -54,13 +54,14 @@
 
   public static native void spiForceAutoRead(int port);
 
-  public static native int spiReadAutoReceivedData(int port, ByteBuffer buffer, int numToRead,
-                                                   double timeout);
+  public static native int spiReadAutoReceivedData(
+      int port, ByteBuffer buffer, int numToRead, double timeout);
 
-  public static native int spiReadAutoReceivedData(int port, int[] buffer, int numToRead,
-                                                   double timeout);
+  public static native int spiReadAutoReceivedData(
+      int port, int[] buffer, int numToRead, double timeout);
 
   public static native int spiGetAutoDroppedCount(int port);
 
-  public static native void spiConfigureAutoStall(int port, int csToSclkTicks, int stallTicks, int pow2BytesPerRead);
+  public static native void spiConfigureAutoStall(
+      int port, int csToSclkTicks, int stallTicks, int pow2BytesPerRead);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java
index 267947b..ee9464e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SerialPortJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimBoolean.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimBoolean.java
index 00e8552..3f4c257 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimBoolean.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimBoolean.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * A wrapper around a simulator boolean value handle.
- */
+/** A wrapper around a simulator boolean value handle. */
 public class SimBoolean extends SimValue {
   /**
    * Wraps a simulated value handle as returned by SimDeviceJNI.createSimValueBoolean().
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java
index ecddc2e..67c39fe 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDevice.java
@@ -1,23 +1,39 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 /**
  * A wrapper around a simulator device handle.
+ *
+ * <p>Teams: if you are using this class, you are likely confusing it for {@link
+ * edu.wpi.first.wpilibj.simulation.SimDeviceSim}.
+ *
+ * <p>Vendors: This class should be used from inside the device class to define the
+ * properties/fields of the device. Use {@link #create} to get a SimDevice object, then use {@link
+ * #createDouble(String, Direction, double)} or similar to define the device's fields. See {@link
+ * edu.wpi.first.wpilibj.ADXRS450_Gyro} for an example implementation.
  */
 public class SimDevice implements AutoCloseable {
+  public enum Direction {
+    kInput(SimDeviceJNI.kInput),
+    kOutput(SimDeviceJNI.kOutput),
+    kBidir(SimDeviceJNI.kBidir);
+
+    public final int m_value;
+
+    Direction(int value) {
+      m_value = value;
+    }
+  }
+
   /**
    * Creates a simulated device.
    *
-   * <p>The device name must be unique.  Returns null if the device name
-   * already exists.  If multiple instances of the same device are desired,
-   * recommend appending the instance/unique identifer in brackets to the base
-   * name, e.g. "device[1]".
+   * <p>The device name must be unique. Returns null if the device name already exists. If multiple
+   * instances of the same device are desired, recommend appending the instance/unique identifer in
+   * brackets to the base name, e.g. "device[1]".
    *
    * <p>null is returned if not in simulation.
    *
@@ -35,10 +51,9 @@
   /**
    * Creates a simulated device.
    *
-   * <p>The device name must be unique.  Returns null if the device name
-   * already exists.  This is a convenience method that appends index in
-   * brackets to the device name, e.g. passing index=1 results in "device[1]"
-   * for the device name.
+   * <p>The device name must be unique. Returns null if the device name already exists. This is a
+   * convenience method that appends index in brackets to the device name, e.g. passing index=1
+   * results in "device[1]" for the device name.
    *
    * <p>null is returned if not in simulation.
    *
@@ -53,10 +68,9 @@
   /**
    * Creates a simulated device.
    *
-   * <p>The device name must be unique.  Returns null if the device name
-   * already exists.  This is a convenience method that appends index and
-   * channel in brackets to the device name, e.g. passing index=1 and channel=2
-   * results in "device[1,2]" for the device name.
+   * <p>The device name must be unique. Returns null if the device name already exists. This is a
+   * convenience method that appends index and channel in brackets to the device name, e.g. passing
+   * index=1 and channel=2 results in "device[1,2]" for the device name.
    *
    * <p>null is returned if not in simulation.
    *
@@ -101,9 +115,25 @@
    * @param readonly if the value should not be written from simulation side
    * @param initialValue initial value
    * @return simulated value object
+   * @deprecated Use direction function instead
    */
+  @Deprecated
   public SimValue createValue(String name, boolean readonly, HALValue initialValue) {
-    int handle = SimDeviceJNI.createSimValue(m_handle, name, readonly, initialValue);
+    return createValue(name, readonly ? Direction.kOutput : Direction.kInput, initialValue);
+  }
+
+  /**
+   * Creates a value on the simulated device.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated value object
+   */
+  public SimValue createValue(String name, Direction direction, HALValue initialValue) {
+    int handle = SimDeviceJNI.createSimValue(m_handle, name, direction.m_value, initialValue);
     if (handle <= 0) {
       return null;
     }
@@ -111,6 +141,42 @@
   }
 
   /**
+   * Creates an int value on the simulated device.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated double value object
+   */
+  public SimInt createInt(String name, Direction direction, int initialValue) {
+    int handle = SimDeviceJNI.createSimValueInt(m_handle, name, direction.m_value, initialValue);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimInt(handle);
+  }
+
+  /**
+   * Creates a long value on the simulated device.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated double value object
+   */
+  public SimLong createLong(String name, Direction direction, long initialValue) {
+    int handle = SimDeviceJNI.createSimValueLong(m_handle, name, direction.m_value, initialValue);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimLong(handle);
+  }
+
+  /**
    * Creates a double value on the simulated device.
    *
    * <p>Returns null if not in simulation.
@@ -119,9 +185,25 @@
    * @param readonly if the value should not be written from simulation side
    * @param initialValue initial value
    * @return simulated double value object
+   * @deprecated Use direction function instead
    */
+  @Deprecated
   public SimDouble createDouble(String name, boolean readonly, double initialValue) {
-    int handle = SimDeviceJNI.createSimValueDouble(m_handle, name, readonly, initialValue);
+    return createDouble(name, readonly ? Direction.kOutput : Direction.kInput, initialValue);
+  }
+
+  /**
+   * Creates a double value on the simulated device.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated double value object
+   */
+  public SimDouble createDouble(String name, Direction direction, double initialValue) {
+    int handle = SimDeviceJNI.createSimValueDouble(m_handle, name, direction.m_value, initialValue);
     if (handle <= 0) {
       return null;
     }
@@ -140,9 +222,54 @@
    * @param options array of option descriptions
    * @param initialValue initial value (selection)
    * @return simulated enum value object
+   * @deprecated Use direction function instead
    */
+  @Deprecated
   public SimEnum createEnum(String name, boolean readonly, String[] options, int initialValue) {
-    int handle = SimDeviceJNI.createSimValueEnum(m_handle, name, readonly, options, initialValue);
+    return createEnum(name, readonly ? Direction.kOutput : Direction.kInput, options, initialValue);
+  }
+
+  /**
+   * Creates an enumerated value on the simulated device.
+   *
+   * <p>Enumerated values are always in the range 0 to numOptions-1.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param options array of option descriptions
+   * @param initialValue initial value (selection)
+   * @return simulated enum value object
+   */
+  public SimEnum createEnum(String name, Direction direction, String[] options, int initialValue) {
+    int handle =
+        SimDeviceJNI.createSimValueEnum(m_handle, name, direction.m_value, options, initialValue);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimEnum(handle);
+  }
+
+  /**
+   * Creates an enumerated value on the simulated device with double values.
+   *
+   * <p>Enumerated values are always in the range 0 to numOptions-1.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param options array of option descriptions
+   * @param optionValues array of option values (must be the same size as options)
+   * @param initialValue initial value (selection)
+   * @return simulated enum value object
+   */
+  public SimEnum createEnumDouble(
+      String name, Direction direction, String[] options, double[] optionValues, int initialValue) {
+    int handle =
+        SimDeviceJNI.createSimValueEnumDouble(
+            m_handle, name, direction.m_value, options, optionValues, initialValue);
     if (handle <= 0) {
       return null;
     }
@@ -158,9 +285,26 @@
    * @param readonly if the value should not be written from simulation side
    * @param initialValue initial value
    * @return simulated boolean value object
+   * @deprecated Use direction function instead
    */
+  @Deprecated
   public SimBoolean createBoolean(String name, boolean readonly, boolean initialValue) {
-    int handle = SimDeviceJNI.createSimValueBoolean(m_handle, name, readonly, initialValue);
+    return createBoolean(name, readonly ? Direction.kOutput : Direction.kInput, initialValue);
+  }
+
+  /**
+   * Creates a boolean value on the simulated device.
+   *
+   * <p>Returns null if not in simulation.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated boolean value object
+   */
+  public SimBoolean createBoolean(String name, Direction direction, boolean initialValue) {
+    int handle =
+        SimDeviceJNI.createSimValueBoolean(m_handle, name, direction.m_value, initialValue);
     if (handle <= 0) {
       return null;
     }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java
index d9b8931..4279916 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDeviceJNI.java
@@ -1,20 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
 public class SimDeviceJNI extends JNIWrapper {
+  public static final int kInput = 0;
+  public static final int kOutput = 1;
+  public static final int kBidir = 2;
+
   /**
    * Creates a simulated device.
    *
-   * <p>The device name must be unique.  0 is returned if the device name
-   * already exists.  If multiple instances of the same device are desired,
-   * recommend appending the instance/unique identifer in brackets to the base
-   * name, e.g. "device[1]".
+   * <p>The device name must be unique. 0 is returned if the device name already exists. If multiple
+   * instances of the same device are desired, recommend appending the instance/unique identifer in
+   * brackets to the base name, e.g. "device[1]".
    *
    * <p>0 is returned if not in simulation.
    *
@@ -26,49 +26,124 @@
   /**
    * Frees a simulated device.
    *
-   * <p>This also allows the same device name to be used again.
-   * This also frees all the simulated values created on the device.
+   * <p>This also allows the same device name to be used again. This also frees all the simulated
+   * values created on the device.
    *
    * @param handle simulated device handle
    */
   public static native void freeSimDevice(int handle);
 
-  private static native int createSimValueNative(int device, String name, boolean readonly,
-      int type, long value1, double value2);
+  private static native int createSimValueNative(
+      int device, String name, int direction, int type, long value1, double value2);
 
   /**
    * Creates a value on a simulated device.
    *
-   * <p>Returns 0 if not in simulation; this can be used to avoid calls
-   * to Set/Get functions.
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
    *
    * @param device simulated device handle
    * @param name value name
    * @param readonly if the value should not be written from simulation side
    * @param initialValue initial value
    * @return simulated value handle
+   * @deprecated Use direction-taking function instead
    */
-  public static int createSimValue(int device, String name, boolean readonly,
-      HALValue initialValue) {
-    return createSimValueNative(device, name, readonly, initialValue.getType(),
-        initialValue.getNativeLong(), initialValue.getNativeDouble());
+  @Deprecated
+  public static int createSimValue(
+      int device, String name, boolean readonly, HALValue initialValue) {
+    return createSimValueNative(
+        device,
+        name,
+        readonly ? kOutput : kInput,
+        initialValue.getType(),
+        initialValue.getNativeLong(),
+        initialValue.getNativeDouble());
+  }
+
+  /**
+   * Creates a value on a simulated device.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated value handle
+   */
+  public static int createSimValue(int device, String name, int direction, HALValue initialValue) {
+    return createSimValueNative(
+        device,
+        name,
+        direction,
+        initialValue.getType(),
+        initialValue.getNativeLong(),
+        initialValue.getNativeDouble());
+  }
+
+  /**
+   * Creates an int value on a simulated device.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated value handle
+   */
+  public static int createSimValueInt(int device, String name, int direction, int initialValue) {
+    return createSimValueNative(device, name, direction, HALValue.kInt, initialValue, 0.0);
+  }
+
+  /**
+   * Creates a long value on a simulated device.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated value handle
+   */
+  public static int createSimValueLong(int device, String name, int direction, long initialValue) {
+    return createSimValueNative(device, name, direction, HALValue.kLong, initialValue, 0.0);
   }
 
   /**
    * Creates a double value on a simulated device.
    *
-   * <p>Returns 0 if not in simulation; this can be used to avoid calls
-   * to Set/Get functions.
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
    *
    * @param device simulated device handle
    * @param name value name
    * @param readonly if the value should not be written from simulation side
    * @param initialValue initial value
    * @return simulated value handle
+   * @deprecated Use direction-taking function instead
    */
-  public static int createSimValueDouble(int device, String name, boolean readonly,
-      double initialValue) {
-    return createSimValueNative(device, name, readonly, HALValue.kDouble, 0, initialValue);
+  @Deprecated
+  public static int createSimValueDouble(
+      int device, String name, boolean readonly, double initialValue) {
+    return createSimValueNative(
+        device, name, readonly ? kOutput : kInput, HALValue.kDouble, 0, initialValue);
+  }
+
+  /**
+   * Creates a double value on a simulated device.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated value handle
+   */
+  public static int createSimValueDouble(
+      int device, String name, int direction, double initialValue) {
+    return createSimValueNative(device, name, direction, HALValue.kDouble, 0, initialValue);
   }
 
   /**
@@ -76,8 +151,7 @@
    *
    * <p>Enumerated values are always in the range 0 to numOptions-1.
    *
-   * <p>Returns 0 if not in simulation; this can be used to avoid calls
-   * to Set/Get functions.
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
    *
    * @param device simulated device handle
    * @param name value name
@@ -85,26 +159,88 @@
    * @param options array of option descriptions
    * @param initialValue initial value (selection)
    * @return simulated value handle
+   * @deprecated Use direction-taking function instead
    */
-  public static native int createSimValueEnum(int device, String name, boolean readonly,
-      String[] options, int initialValue);
+  @Deprecated
+  public static int createSimValueEnum(
+      int device, String name, boolean readonly, String[] options, int initialValue) {
+    return createSimValueEnum(device, name, readonly ? kOutput : kInput, options, initialValue);
+  }
+
+  /**
+   * Creates an enumerated value on a simulated device.
+   *
+   * <p>Enumerated values are always in the range 0 to numOptions-1.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param options array of option descriptions
+   * @param initialValue initial value (selection)
+   * @return simulated value handle
+   */
+  public static native int createSimValueEnum(
+      int device, String name, int direction, String[] options, int initialValue);
+
+  /**
+   * Creates an enumerated value on a simulated device with double values.
+   *
+   * <p>Enumerated values are always in the range 0 to numOptions-1.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param options array of option descriptions
+   * @param optionValues array of option values (must be the same size as options)
+   * @param initialValue initial value (selection)
+   * @return simulated value handle
+   */
+  public static native int createSimValueEnumDouble(
+      int device,
+      String name,
+      int direction,
+      String[] options,
+      double[] optionValues,
+      int initialValue);
 
   /**
    * Creates a boolean value on a simulated device.
    *
-   * <p>Returns 0 if not in simulation; this can be used to avoid calls
-   * to Set/Get functions.
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
    *
    * @param device simulated device handle
    * @param name value name
    * @param readonly if the value should not be written from simulation side
    * @param initialValue initial value
    * @return simulated value handle
+   * @deprecated Use direction-taking function instead
    */
-  public static int createSimValueBoolean(int device, String name, boolean readonly,
-      boolean initialValue) {
-    return createSimValueNative(device, name, readonly, HALValue.kBoolean,
-        initialValue ? 1 : 0, 0.0);
+  @Deprecated
+  public static int createSimValueBoolean(
+      int device, String name, boolean readonly, boolean initialValue) {
+    return createSimValueNative(
+        device, name, readonly ? kOutput : kInput, HALValue.kBoolean, initialValue ? 1 : 0, 0.0);
+  }
+
+  /**
+   * Creates a boolean value on a simulated device.
+   *
+   * <p>Returns 0 if not in simulation; this can be used to avoid calls to Set/Get functions.
+   *
+   * @param device simulated device handle
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated value handle
+   */
+  public static int createSimValueBoolean(
+      int device, String name, int direction, boolean initialValue) {
+    return createSimValueNative(
+        device, name, direction, HALValue.kBoolean, initialValue ? 1 : 0, 0.0);
   }
 
   /**
@@ -116,6 +252,22 @@
   public static native HALValue getSimValue(int handle);
 
   /**
+   * Gets a simulated value (int).
+   *
+   * @param handle simulated value handle
+   * @return The current value
+   */
+  public static native int getSimValueInt(int handle);
+
+  /**
+   * Gets a simulated value (long).
+   *
+   * @param handle simulated value handle
+   * @return The current value
+   */
+  public static native long getSimValueLong(int handle);
+
+  /**
    * Gets a simulated value (double).
    *
    * @param handle simulated value handle
@@ -152,6 +304,26 @@
   }
 
   /**
+   * Sets a simulated value (int).
+   *
+   * @param handle simulated value handle
+   * @param value the value to set
+   */
+  public static void setSimValueInt(int handle, int value) {
+    setSimValueNative(handle, HALValue.kInt, value, 0.0);
+  }
+
+  /**
+   * Sets a simulated value (long).
+   *
+   * @param handle simulated value handle
+   * @param value the value to set
+   */
+  public static void setSimValueLong(int handle, long value) {
+    setSimValueNative(handle, HALValue.kLong, value, 0.0);
+  }
+
+  /**
    * Sets a simulated value (double).
    *
    * @param handle simulated value handle
@@ -180,4 +352,13 @@
   public static void setSimValueBoolean(int handle, boolean value) {
     setSimValueNative(handle, HALValue.kBoolean, value ? 1 : 0, 0.0);
   }
+
+  /**
+   * Resets a simulated double or integral value to 0. Has no effect on other value types. Use this
+   * instead of Set(0) for resetting incremental sensor values like encoder counts or gyro
+   * accumulated angle to ensure correct behavior in a distributed system (e.g. WebSockets).
+   *
+   * @param handle simulated value handle
+   */
+  public static native void resetSimValue(int handle);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDouble.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDouble.java
index 51c9789..508208f 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDouble.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimDouble.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * A wrapper around a simulator double value handle.
- */
+/** A wrapper around a simulator double value handle. */
 public class SimDouble extends SimValue {
   /**
    * Wraps a simulated value handle as returned by SimDeviceJNI.createSimValueDouble().
@@ -37,4 +32,13 @@
   public void set(double value) {
     SimDeviceJNI.setSimValueDouble(m_handle, value);
   }
+
+  /**
+   * Resets the simulated value to 0. Use this instead of Set(0) for resetting incremental sensor
+   * values like encoder counts or gyro accumulated angle to ensure correct behavior in a
+   * distributed system (e.g. WebSockets).
+   */
+  public void reset() {
+    SimDeviceJNI.resetSimValue(m_handle);
+  }
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimEnum.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimEnum.java
index d951bed..78f963d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimEnum.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimEnum.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * A wrapper around a simulator enum value handle.
- */
+/** A wrapper around a simulator enum value handle. */
 public class SimEnum extends SimValue {
   /**
    * Wraps a simulated value handle as returned by SimDeviceJNI.createSimValueEnum().
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimInt.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimInt.java
new file mode 100644
index 0000000..f2d30eb
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimInt.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+/** A wrapper around a simulator int value handle. */
+public class SimInt extends SimValue {
+  /**
+   * Wraps a simulated value handle as returned by SimDeviceJNI.createSimValueInt().
+   *
+   * @param handle simulated value handle
+   */
+  public SimInt(int handle) {
+    super(handle);
+  }
+
+  /**
+   * Gets the simulated value.
+   *
+   * @return The current value
+   */
+  public int get() {
+    return SimDeviceJNI.getSimValueInt(m_handle);
+  }
+
+  /**
+   * Sets the simulated value.
+   *
+   * @param value the value to set
+   */
+  public void set(int value) {
+    SimDeviceJNI.setSimValueInt(m_handle, value);
+  }
+
+  /**
+   * Resets the simulated value to 0. Use this instead of Set(0) for resetting incremental sensor
+   * values like encoder counts or gyro accumulated angle to ensure correct behavior in a
+   * distributed system (e.g. WebSockets).
+   */
+  public void reset() {
+    SimDeviceJNI.resetSimValue(m_handle);
+  }
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimLong.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimLong.java
new file mode 100644
index 0000000..80ad963
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimLong.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal;
+
+/** A wrapper around a simulator long value handle. */
+public class SimLong extends SimValue {
+  /**
+   * Wraps a simulated value handle as returned by SimDeviceJNI.createSimValueLong().
+   *
+   * @param handle simulated value handle
+   */
+  public SimLong(int handle) {
+    super(handle);
+  }
+
+  /**
+   * Gets the simulated value.
+   *
+   * @return The current value
+   */
+  public long get() {
+    return SimDeviceJNI.getSimValueLong(m_handle);
+  }
+
+  /**
+   * Sets the simulated value.
+   *
+   * @param value the value to set
+   */
+  public void set(long value) {
+    SimDeviceJNI.setSimValueLong(m_handle, value);
+  }
+
+  /**
+   * Resets the simulated value to 0. Use this instead of Set(0) for resetting incremental sensor
+   * values like encoder counts or gyro accumulated angle to ensure correct behavior in a
+   * distributed system (e.g. WebSockets).
+   */
+  public void reset() {
+    SimDeviceJNI.resetSimValue(m_handle);
+  }
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimValue.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimValue.java
index 05d6b0c..a486981 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimValue.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SimValue.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
-/**
- * A wrapper around a simulator value handle.
- */
+/** A wrapper around a simulator value handle. */
 public class SimValue {
   /**
    * Wraps a simulated value handle as returned by SimDeviceJNI.createSimValue().
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SolenoidJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SolenoidJNI.java
deleted file mode 100644
index 66acbea..0000000
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/SolenoidJNI.java
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal;
-
-public class SolenoidJNI extends JNIWrapper {
-  public static native int initializeSolenoidPort(int halPortHandle);
-
-  public static native boolean checkSolenoidModule(int module);
-
-  public static native boolean checkSolenoidChannel(int channel);
-
-  public static native void freeSolenoidPort(int portHandle);
-
-  public static native void setSolenoid(int portHandle, boolean on);
-
-  public static native boolean getSolenoid(int portHandle);
-
-  public static native int getAllSolenoids(int module);
-
-  public static native int getPCMSolenoidBlackList(int module);
-
-  public static native boolean getPCMSolenoidVoltageStickyFault(int module);
-
-  public static native boolean getPCMSolenoidVoltageFault(int module);
-
-  public static native void clearAllPCMStickyFaults(int module);
-
-  public static native void setOneShotDuration(int portHandle, long durationMS);
-
-  public static native void fireOneShot(int portHandle);
-}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java
index e320eb5..c854e20 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/ThreadsJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANExceptionFactory.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANExceptionFactory.java
index dc089f8..3c0a4b9 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANExceptionFactory.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANExceptionFactory.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
@@ -17,9 +14,10 @@
   static final int ERR_CANSessionMux_NotAllowed = -44088;
   static final int ERR_CANSessionMux_NotInitialized = -44089;
 
-  @SuppressWarnings({"JavadocMethod", "PMD.CyclomaticComplexity"})
-  public static void checkStatus(int status, int messageID) throws CANInvalidBufferException,
-      CANMessageNotAllowedException, CANNotInitializedException, UncleanStatusException {
+  @SuppressWarnings("MissingJavadocMethod")
+  public static void checkStatus(int status, int messageID)
+      throws CANInvalidBufferException, CANMessageNotAllowedException, CANNotInitializedException,
+          UncleanStatusException {
     switch (status) {
       case NIRioStatus.kRioStatusSuccess:
         // Everything is ok... don't throw.
@@ -41,7 +39,5 @@
     }
   }
 
-  private CANExceptionFactory() {
-
-  }
+  private CANExceptionFactory() {}
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANInvalidBufferException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANInvalidBufferException.java
index 8ea718b..ef2fa0d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANInvalidBufferException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANInvalidBufferException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java
index 754157b..b4f344f 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANJNI.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
+import edu.wpi.first.hal.JNIWrapper;
 import java.nio.ByteBuffer;
 import java.nio.IntBuffer;
 
-import edu.wpi.first.hal.JNIWrapper;
-
 @SuppressWarnings("AbbreviationAsWordInName")
 public class CANJNI extends JNIWrapper {
   public static final int CAN_SEND_PERIOD_NO_REPEAT = 0;
@@ -22,15 +18,13 @@
   public static final int CAN_IS_FRAME_11BIT = 0x40000000;
 
   @SuppressWarnings("MethodName")
-  public static native void FRCNetCommCANSessionMuxSendMessage(int messageID,
-                                                               byte[] data,
-                                                               int periodMs);
+  public static native void FRCNetCommCANSessionMuxSendMessage(
+      int messageID, byte[] data, int periodMs);
 
   @SuppressWarnings("MethodName")
   public static native byte[] FRCNetCommCANSessionMuxReceiveMessage(
       IntBuffer messageID, int messageIDMask, ByteBuffer timeStamp);
 
-
   @SuppressWarnings("MethodName")
-  public static native void GetCANStatus(CANStatus status);
+  public static native void getCANStatus(CANStatus status);
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotAllowedException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotAllowedException.java
index f4ba6a8..de7a70a 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotAllowedException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotAllowedException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotFoundException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotFoundException.java
index 0838691..39eab14 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotFoundException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANMessageNotFoundException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANNotInitializedException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANNotInitializedException.java
index 119b59d..30f139d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANNotInitializedException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANNotInitializedException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java
index 492d999..62df8e8 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/can/CANStatus.java
@@ -1,49 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.can;
 
-/**
- * Structure for holding the result of a CAN Status request.
- */
+/** Structure for holding the result of a CAN Status request. */
 public class CANStatus {
-  /**
-   * The utilization of the CAN Bus.
-   */
+  /** The utilization of the CAN Bus. */
   @SuppressWarnings("MemberName")
   public double percentBusUtilization;
 
-  /**
-   * The CAN Bus off count.
-   */
+  /** The CAN Bus off count. */
   @SuppressWarnings("MemberName")
   public int busOffCount;
 
-  /**
-   * The CAN Bus TX full count.
-   */
+  /** The CAN Bus TX full count. */
   @SuppressWarnings("MemberName")
   public int txFullCount;
 
-  /**
-   * The CAN Bus receive error count.
-   */
+  /** The CAN Bus receive error count. */
   @SuppressWarnings("MemberName")
   public int receiveErrorCount;
 
-  /**
-   * The CAN Bus transmit error count.
-   */
+  /** The CAN Bus transmit error count. */
   @SuppressWarnings("MemberName")
   public int transmitErrorCount;
 
-  @SuppressWarnings("JavadocMethod")
-  public void setStatus(double percentBusUtilization, int busOffCount, int txFullCount,
-                        int receiveErrorCount, int transmitErrorCount) {
+  @SuppressWarnings("MissingJavadocMethod")
+  public void setStatus(
+      double percentBusUtilization,
+      int busOffCount,
+      int txFullCount,
+      int receiveErrorCount,
+      int transmitErrorCount) {
     this.percentBusUtilization = percentBusUtilization;
     this.busOffCount = busOffCount;
     this.txFullCount = txFullCount;
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
index f49f34e..ad17a3e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/communication/NIRioStatus.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.communication;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
index cb42c98..5ea3726 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AccelerometerDataJNI.java
@@ -1,38 +1,55 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class AccelerometerDataJNI extends JNIWrapper {
-  public static native int registerActiveCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerActiveCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelActiveCallback(int index, int uid);
+
   public static native boolean getActive(int index);
+
   public static native void setActive(int index, boolean active);
 
-  public static native int registerRangeCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerRangeCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelRangeCallback(int index, int uid);
+
   public static native int getRange(int index);
+
   public static native void setRange(int index, int range);
 
-  public static native int registerXCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerXCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelXCallback(int index, int uid);
+
   public static native double getX(int index);
+
   public static native void setX(int index, double x);
 
-  public static native int registerYCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerYCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelYCallback(int index, int uid);
+
   public static native double getY(int index);
+
   public static native void setY(int index, double y);
 
-  public static native int registerZCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerZCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelZCallback(int index, int uid);
+
   public static native double getZ(int index);
+
   public static native void setZ(int index, double z);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
index 0aa4d47..8ba0e94 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AddressableLEDDataJNI.java
@@ -1,38 +1,54 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class AddressableLEDDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerOutputPortCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerOutputPortCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelOutputPortCallback(int index, int uid);
+
   public static native int getOutputPort(int index);
+
   public static native void setOutputPort(int index, int outputPort);
 
-  public static native int registerLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerLengthCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelLengthCallback(int index, int uid);
+
   public static native int getLength(int index);
+
   public static native void setLength(int index, int length);
 
-  public static native int registerRunningCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerRunningCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelRunningCallback(int index, int uid);
+
   public static native boolean getRunning(int index);
+
   public static native void setRunning(int index, boolean running);
 
   public static native int registerDataCallback(int index, ConstBufferCallback callback);
+
   public static native void cancelDataCallback(int index, int uid);
+
   public static native byte[] getData(int index);
+
   public static native void setData(int index, byte[] data);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
index 42cc508..a6acc9a 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogGyroDataJNI.java
@@ -1,28 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class AnalogGyroDataJNI extends JNIWrapper {
-  public static native int registerAngleCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAngleCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAngleCallback(int index, int uid);
+
   public static native double getAngle(int index);
+
   public static native void setAngle(int index, double angle);
 
-  public static native int registerRateCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerRateCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelRateCallback(int index, int uid);
+
   public static native double getRate(int index);
+
   public static native void setRate(int index, double rate);
 
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
index 3656ffa..4229c09 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogInDataJNI.java
@@ -1,58 +1,91 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class AnalogInDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerAverageBitsCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAverageBitsCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAverageBitsCallback(int index, int uid);
+
   public static native int getAverageBits(int index);
+
   public static native void setAverageBits(int index, int averageBits);
 
-  public static native int registerOversampleBitsCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerOversampleBitsCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelOversampleBitsCallback(int index, int uid);
+
   public static native int getOversampleBits(int index);
+
   public static native void setOversampleBits(int index, int oversampleBits);
 
-  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerVoltageCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelVoltageCallback(int index, int uid);
+
   public static native double getVoltage(int index);
+
   public static native void setVoltage(int index, double voltage);
 
-  public static native int registerAccumulatorInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAccumulatorInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAccumulatorInitializedCallback(int index, int uid);
+
   public static native boolean getAccumulatorInitialized(int index);
+
   public static native void setAccumulatorInitialized(int index, boolean accumulatorInitialized);
 
-  public static native int registerAccumulatorValueCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAccumulatorValueCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAccumulatorValueCallback(int index, int uid);
+
   public static native long getAccumulatorValue(int index);
+
   public static native void setAccumulatorValue(int index, long accumulatorValue);
 
-  public static native int registerAccumulatorCountCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAccumulatorCountCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAccumulatorCountCallback(int index, int uid);
+
   public static native long getAccumulatorCount(int index);
+
   public static native void setAccumulatorCount(int index, long accumulatorCount);
 
-  public static native int registerAccumulatorCenterCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAccumulatorCenterCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAccumulatorCenterCallback(int index, int uid);
+
   public static native int getAccumulatorCenter(int index);
+
   public static native void setAccumulatorCenter(int index, int AccumulatorCenter);
 
-  public static native int registerAccumulatorDeadbandCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerAccumulatorDeadbandCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAccumulatorDeadbandCallback(int index, int uid);
+
   public static native int getAccumulatorDeadband(int index);
+
   public static native void setAccumulatorDeadband(int index, int AccumulatorDeadband);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
index 23bc8ac..6af16a0 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogOutDataJNI.java
@@ -1,23 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class AnalogOutDataJNI extends JNIWrapper {
-  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerVoltageCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelVoltageCallback(int index, int uid);
+
   public static native double getVoltage(int index);
+
   public static native void setVoltage(int index, double voltage);
 
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
index ca89a28..67b65b5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/AnalogTriggerDataJNI.java
@@ -1,28 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class AnalogTriggerDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerTriggerLowerBoundCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerTriggerLowerBoundCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelTriggerLowerBoundCallback(int index, int uid);
+
   public static native double getTriggerLowerBound(int index);
+
   public static native void setTriggerLowerBound(int index, double triggerLowerBound);
 
-  public static native int registerTriggerUpperBoundCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerTriggerUpperBoundCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelTriggerUpperBoundCallback(int index, int uid);
+
   public static native double getTriggerUpperBound(int index);
+
   public static native void setTriggerUpperBound(int index, double triggerUpperBound);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
index a8d8ce1..e93a921 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/BufferCallback.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java
new file mode 100644
index 0000000..9f60e7e
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/CTREPCMDataJNI.java
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class CTREPCMDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelInitializedCallback(int index, int uid);
+
+  public static native boolean getInitialized(int index);
+
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerSolenoidOutputCallback(
+      int index, int channel, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelSolenoidOutputCallback(int index, int channel, int uid);
+
+  public static native boolean getSolenoidOutput(int index, int channel);
+
+  public static native void setSolenoidOutput(int index, int channel, boolean solenoidOutput);
+
+  public static native int registerCompressorOnCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelCompressorOnCallback(int index, int uid);
+
+  public static native boolean getCompressorOn(int index);
+
+  public static native void setCompressorOn(int index, boolean compressorOn);
+
+  public static native int registerClosedLoopEnabledCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelClosedLoopEnabledCallback(int index, int uid);
+
+  public static native boolean getClosedLoopEnabled(int index);
+
+  public static native void setClosedLoopEnabled(int index, boolean closeLoopEnabled);
+
+  public static native int registerPressureSwitchCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelPressureSwitchCallback(int index, int uid);
+
+  public static native boolean getPressureSwitch(int index);
+
+  public static native void setPressureSwitch(int index, boolean pressureSwitch);
+
+  public static native int registerCompressorCurrentCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelCompressorCurrentCallback(int index, int uid);
+
+  public static native double getCompressorCurrent(int index);
+
+  public static native void setCompressorCurrent(int index, double compressorCurrent);
+
+  public static native void registerAllNonSolenoidCallbacks(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void registerAllSolenoidCallbacks(
+      int index, int channel, NotifyCallback callback, boolean initialNotify);
+
+  public static native void resetData(int index);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java
index 6cb2375..9e88ec3 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/ConstBufferCallback.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
index 5d41a2f..0c59415 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DIODataJNI.java
@@ -1,38 +1,55 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class DIODataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerValueCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerValueCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelValueCallback(int index, int uid);
+
   public static native boolean getValue(int index);
+
   public static native void setValue(int index, boolean value);
 
-  public static native int registerPulseLengthCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerPulseLengthCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelPulseLengthCallback(int index, int uid);
+
   public static native double getPulseLength(int index);
+
   public static native void setPulseLength(int index, double pulseLength);
 
-  public static native int registerIsInputCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerIsInputCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelIsInputCallback(int index, int uid);
+
   public static native boolean getIsInput(int index);
+
   public static native void setIsInput(int index, boolean isInput);
 
-  public static native int registerFilterIndexCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerFilterIndexCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelFilterIndexCallback(int index, int uid);
+
   public static native int getFilterIndex(int index);
+
   public static native void setFilterIndex(int index, int filterIndex);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
index 4e2709f..addcffc 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DigitalPWMDataJNI.java
@@ -1,28 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class DigitalPWMDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerDutyCycleCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerDutyCycleCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelDutyCycleCallback(int index, int uid);
+
   public static native double getDutyCycle(int index);
+
   public static native void setDutyCycle(int index, double dutyCycle);
 
-  public static native int registerPinCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerPinCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelPinCallback(int index, int uid);
+
   public static native int getPin(int index);
+
   public static native void setPin(int index, int pin);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
index a1046c5..196f017 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DriverStationDataJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
@@ -11,76 +8,129 @@
 
 public class DriverStationDataJNI extends JNIWrapper {
   public static native int registerEnabledCallback(NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelEnabledCallback(int uid);
+
   public static native boolean getEnabled();
+
   public static native void setEnabled(boolean enabled);
 
-  public static native int registerAutonomousCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerAutonomousCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAutonomousCallback(int uid);
+
   public static native boolean getAutonomous();
+
   public static native void setAutonomous(boolean autonomous);
 
   public static native int registerTestCallback(NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelTestCallback(int uid);
+
   public static native boolean getTest();
+
   public static native void setTest(boolean test);
 
   public static native int registerEStopCallback(NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelEStopCallback(int uid);
+
   public static native boolean getEStop();
+
   public static native void setEStop(boolean eStop);
 
-  public static native int registerFmsAttachedCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerFmsAttachedCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelFmsAttachedCallback(int uid);
+
   public static native boolean getFmsAttached();
+
   public static native void setFmsAttached(boolean fmsAttached);
 
-  public static native int registerDsAttachedCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerDsAttachedCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelDsAttachedCallback(int uid);
+
   public static native boolean getDsAttached();
+
   public static native void setDsAttached(boolean dsAttached);
 
-  public static native int registerAllianceStationIdCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerAllianceStationIdCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelAllianceStationIdCallback(int uid);
+
   public static native int getAllianceStationId();
+
   public static native void setAllianceStationId(int allianceStationId);
 
-  public static native int registerMatchTimeCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerMatchTimeCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelMatchTimeCallback(int uid);
+
   public static native double getMatchTime();
+
   public static native void setMatchTime(double matchTime);
 
   public static native void setJoystickAxes(byte joystickNum, float[] axesArray);
+
   public static native void setJoystickPOVs(byte joystickNum, short[] povsArray);
+
   public static native void setJoystickButtons(byte joystickNum, int buttons, int count);
+
   public static native long getJoystickOutputs(int stick);
+
   public static native int getJoystickRumble(int stick, int rumbleNum);
 
-  public static native void setMatchInfo(String eventName, String gameSpecificMessage, int matchNumber, int replayNumber, int matchType);
+  public static native void setMatchInfo(
+      String eventName,
+      String gameSpecificMessage,
+      int matchNumber,
+      int replayNumber,
+      int matchType);
 
   public static native void registerAllCallbacks(NotifyCallback callback, boolean initialNotify);
+
   public static native void notifyNewData();
 
   public static native void setSendError(boolean shouldSend);
+
   public static native void setSendConsoleLine(boolean shouldSend);
 
   public static native void setJoystickButton(int stick, int button, boolean state);
+
   public static native void setJoystickAxis(int stick, int axis, double value);
+
   public static native void setJoystickPOV(int stick, int pov, int value);
+
   public static native void setJoystickButtonsValue(int stick, int buttons);
+
   public static native void setJoystickAxisCount(int stick, int count);
+
   public static native void setJoystickPOVCount(int stick, int count);
+
   public static native void setJoystickButtonCount(int stick, int count);
 
   public static native void setJoystickIsXbox(int stick, boolean isXbox);
+
   public static native void setJoystickType(int stick, int type);
+
   public static native void setJoystickName(int stick, String name);
+
   public static native void setJoystickAxisType(int stick, int axis, int type);
 
   public static native void setGameSpecificMessage(String message);
+
   public static native void setEventName(String name);
+
   public static native void setMatchType(int type);
+
   public static native void setMatchNumber(int matchNumber);
+
   public static native void setReplayNumber(int replayNumber);
 
   public static native void resetData();
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
index 3228341..8f244ca 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/DutyCycleDataJNI.java
@@ -1,28 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class DutyCycleDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerFrequencyCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerFrequencyCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelFrequencyCallback(int index, int uid);
+
   public static native int getFrequency(int index);
+
   public static native void setFrequency(int index, int frequency);
 
-  public static native int registerOutputCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerOutputCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelOutputCallback(int index, int uid);
+
   public static native double getOutput(int index);
+
   public static native void setOutput(int index, double output);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
index db78d3c..fc9d2e5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/EncoderDataJNI.java
@@ -1,58 +1,90 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class EncoderDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerCountCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerCountCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelCountCallback(int index, int uid);
+
   public static native int getCount(int index);
+
   public static native void setCount(int index, int count);
 
-  public static native int registerPeriodCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerPeriodCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelPeriodCallback(int index, int uid);
+
   public static native double getPeriod(int index);
+
   public static native void setPeriod(int index, double period);
 
-  public static native int registerResetCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerResetCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelResetCallback(int index, int uid);
+
   public static native boolean getReset(int index);
+
   public static native void setReset(int index, boolean reset);
 
-  public static native int registerMaxPeriodCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerMaxPeriodCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelMaxPeriodCallback(int index, int uid);
+
   public static native double getMaxPeriod(int index);
+
   public static native void setMaxPeriod(int index, double maxPeriod);
 
-  public static native int registerDirectionCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerDirectionCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelDirectionCallback(int index, int uid);
+
   public static native boolean getDirection(int index);
+
   public static native void setDirection(int index, boolean direction);
 
-  public static native int registerReverseDirectionCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerReverseDirectionCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelReverseDirectionCallback(int index, int uid);
+
   public static native boolean getReverseDirection(int index);
+
   public static native void setReverseDirection(int index, boolean reverseDirection);
 
-  public static native int registerSamplesToAverageCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerSamplesToAverageCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelSamplesToAverageCallback(int index, int uid);
+
   public static native int getSamplesToAverage(int index);
+
   public static native void setSamplesToAverage(int index, int samplesToAverage);
 
   public static native void setDistance(int index, double distance);
+
   public static native double getDistance(int index);
+
   public static native void setRate(int index, double rate);
+
   public static native double getRate(int index);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
index 8dbc9e6..ebe15eb 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/I2CDataJNI.java
@@ -1,24 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class I2CDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
   public static native int registerReadCallback(int index, BufferCallback callback);
+
   public static native void cancelReadCallback(int index, int uid);
 
   public static native int registerWriteCallback(int index, ConstBufferCallback callback);
+
   public static native void cancelWriteCallback(int index, int uid);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
index 823318c..9c7b0a2 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifierDataJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
@@ -11,5 +8,6 @@
 
 public class NotifierDataJNI extends JNIWrapper {
   public static native long getNextTimeout();
+
   public static native int getNumNotifiers();
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java
index 22ec015..55605b8 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/NotifyCallback.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java
deleted file mode 100644
index 6b72297..0000000
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PCMDataJNI.java
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.simulation;
-
-import edu.wpi.first.hal.JNIWrapper;
-
-public class PCMDataJNI extends JNIWrapper {
-  public static native int registerSolenoidInitializedCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelSolenoidInitializedCallback(int index, int channel, int uid);
-  public static native boolean getSolenoidInitialized(int index, int channel);
-  public static native void setSolenoidInitialized(int index, int channel, boolean solenoidInitialized);
-
-  public static native int registerSolenoidOutputCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelSolenoidOutputCallback(int index, int channel, int uid);
-  public static native boolean getSolenoidOutput(int index, int channel);
-  public static native void setSolenoidOutput(int index, int channel, boolean solenoidOutput);
-
-  public static native int registerCompressorInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCompressorInitializedCallback(int index, int uid);
-  public static native boolean getCompressorInitialized(int index);
-  public static native void setCompressorInitialized(int index, boolean compressorInitialized);
-
-  public static native int registerCompressorOnCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCompressorOnCallback(int index, int uid);
-  public static native boolean getCompressorOn(int index);
-  public static native void setCompressorOn(int index, boolean compressorOn);
-
-  public static native int registerClosedLoopEnabledCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelClosedLoopEnabledCallback(int index, int uid);
-  public static native boolean getClosedLoopEnabled(int index);
-  public static native void setClosedLoopEnabled(int index, boolean closeLoopEnabled);
-
-  public static native int registerPressureSwitchCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelPressureSwitchCallback(int index, int uid);
-  public static native boolean getPressureSwitch(int index);
-  public static native void setPressureSwitch(int index, boolean pressureSwitch);
-
-  public static native int registerCompressorCurrentCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCompressorCurrentCallback(int index, int uid);
-  public static native double getCompressorCurrent(int index);
-  public static native void setCompressorCurrent(int index, double compressorCurrent);
-
-  public static native void registerAllNonSolenoidCallbacks(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void registerAllSolenoidCallbacks(int index, int channel, NotifyCallback callback, boolean initialNotify);
-
-  public static native void resetData(int index);
-}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java
deleted file mode 100644
index 8825f60..0000000
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PDPDataJNI.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.hal.simulation;
-
-import edu.wpi.first.hal.JNIWrapper;
-
-public class PDPDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelInitializedCallback(int index, int uid);
-  public static native boolean getInitialized(int index);
-  public static native void setInitialized(int index, boolean initialized);
-
-  public static native int registerTemperatureCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelTemperatureCallback(int index, int uid);
-  public static native double getTemperature(int index);
-  public static native void setTemperature(int index, double temperature);
-
-  public static native int registerVoltageCallback(int index, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelVoltageCallback(int index, int uid);
-  public static native double getVoltage(int index);
-  public static native void setVoltage(int index, double voltage);
-
-
-  public static native int registerCurrentCallback(int index, int channel, NotifyCallback callback, boolean initialNotify);
-  public static native void cancelCurrentCallback(int index, int channel, int uid);
-  public static native double getCurrent(int index, int channel);
-  public static native void setCurrent(int index, int channel, double current);
-
-  public static native void resetData(int index);
-}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
index f44b56b..bf75398 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PWMDataJNI.java
@@ -1,43 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class PWMDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
-  public static native int registerRawValueCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerRawValueCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelRawValueCallback(int index, int uid);
+
   public static native int getRawValue(int index);
+
   public static native void setRawValue(int index, int rawValue);
 
-  public static native int registerSpeedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerSpeedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelSpeedCallback(int index, int uid);
+
   public static native double getSpeed(int index);
+
   public static native void setSpeed(int index, double speed);
 
-  public static native int registerPositionCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerPositionCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelPositionCallback(int index, int uid);
+
   public static native double getPosition(int index);
+
   public static native void setPosition(int index, double position);
 
-  public static native int registerPeriodScaleCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerPeriodScaleCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelPeriodScaleCallback(int index, int uid);
+
   public static native int getPeriodScale(int index);
+
   public static native void setPeriodScale(int index, int periodScale);
 
-  public static native int registerZeroLatchCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerZeroLatchCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelZeroLatchCallback(int index, int uid);
+
   public static native boolean getZeroLatch(int index);
+
   public static native void setZeroLatch(int index, boolean zeroLatch);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java
new file mode 100644
index 0000000..c17da13
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/PowerDistributionDataJNI.java
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+public class PowerDistributionDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelInitializedCallback(int index, int uid);
+
+  public static native boolean getInitialized(int index);
+
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerTemperatureCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelTemperatureCallback(int index, int uid);
+
+  public static native double getTemperature(int index);
+
+  public static native void setTemperature(int index, double temperature);
+
+  public static native int registerVoltageCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelVoltageCallback(int index, int uid);
+
+  public static native double getVoltage(int index);
+
+  public static native void setVoltage(int index, double voltage);
+
+  public static native int registerCurrentCallback(
+      int index, int channel, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelCurrentCallback(int index, int channel, int uid);
+
+  public static native double getCurrent(int index, int channel);
+
+  public static native void setCurrent(int index, int channel, double current);
+
+  public static native void resetData(int index);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java
new file mode 100644
index 0000000..b12fdcb
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/REVPHDataJNI.java
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.hal.simulation;
+
+import edu.wpi.first.hal.JNIWrapper;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+public class REVPHDataJNI extends JNIWrapper {
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelInitializedCallback(int index, int uid);
+
+  public static native boolean getInitialized(int index);
+
+  public static native void setInitialized(int index, boolean initialized);
+
+  public static native int registerSolenoidOutputCallback(
+      int index, int channel, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelSolenoidOutputCallback(int index, int channel, int uid);
+
+  public static native boolean getSolenoidOutput(int index, int channel);
+
+  public static native void setSolenoidOutput(int index, int channel, boolean solenoidOutput);
+
+  public static native int registerCompressorOnCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelCompressorOnCallback(int index, int uid);
+
+  public static native boolean getCompressorOn(int index);
+
+  public static native void setCompressorOn(int index, boolean compressorOn);
+
+  public static native int registerClosedLoopEnabledCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelClosedLoopEnabledCallback(int index, int uid);
+
+  public static native boolean getClosedLoopEnabled(int index);
+
+  public static native void setClosedLoopEnabled(int index, boolean closeLoopEnabled);
+
+  public static native int registerPressureSwitchCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelPressureSwitchCallback(int index, int uid);
+
+  public static native boolean getPressureSwitch(int index);
+
+  public static native void setPressureSwitch(int index, boolean pressureSwitch);
+
+  public static native int registerCompressorCurrentCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelCompressorCurrentCallback(int index, int uid);
+
+  public static native double getCompressorCurrent(int index);
+
+  public static native void setCompressorCurrent(int index, double compressorCurrent);
+
+  public static native void registerAllNonSolenoidCallbacks(
+      int index, NotifyCallback callback, boolean initialNotify);
+
+  public static native void registerAllSolenoidCallbacks(
+      int index, int channel, NotifyCallback callback, boolean initialNotify);
+
+  public static native void resetData(int index);
+}
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
index e7973a3..3b165e2 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RelayDataJNI.java
@@ -1,33 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class RelayDataJNI extends JNIWrapper {
-  public static native int registerInitializedForwardCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedForwardCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedForwardCallback(int index, int uid);
+
   public static native boolean getInitializedForward(int index);
+
   public static native void setInitializedForward(int index, boolean initializedForward);
 
-  public static native int registerInitializedReverseCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedReverseCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedReverseCallback(int index, int uid);
+
   public static native boolean getInitializedReverse(int index);
+
   public static native void setInitializedReverse(int index, boolean initializedReverse);
 
-  public static native int registerForwardCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerForwardCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelForwardCallback(int index, int uid);
+
   public static native boolean getForward(int index);
+
   public static native void setForward(int index, boolean forward);
 
-  public static native int registerReverseCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerReverseCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelReverseCallback(int index, int uid);
+
   public static native boolean getReverse(int index);
+
   public static native void setReverse(int index, boolean reverse);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
index a95abfe..a822ede 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/RoboRioDataJNI.java
@@ -1,89 +1,159 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class RoboRioDataJNI extends JNIWrapper {
-  public static native int registerFPGAButtonCallback(NotifyCallback callback, boolean initialNotify);
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public static native int registerFPGAButtonCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  @SuppressWarnings("AbbreviationAsWordInName")
   public static native void cancelFPGAButtonCallback(int uid);
+
+  @SuppressWarnings("AbbreviationAsWordInName")
   public static native boolean getFPGAButton();
+
+  @SuppressWarnings("AbbreviationAsWordInName")
   public static native void setFPGAButton(boolean fPGAButton);
 
-  public static native int registerVInVoltageCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerVInVoltageCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelVInVoltageCallback(int uid);
+
   public static native double getVInVoltage();
+
   public static native void setVInVoltage(double vInVoltage);
 
-  public static native int registerVInCurrentCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerVInCurrentCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelVInCurrentCallback(int uid);
+
   public static native double getVInCurrent();
+
   public static native void setVInCurrent(double vInCurrent);
 
-  public static native int registerUserVoltage6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserVoltage6VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserVoltage6VCallback(int uid);
+
   public static native double getUserVoltage6V();
+
   public static native void setUserVoltage6V(double userVoltage6V);
 
-  public static native int registerUserCurrent6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserCurrent6VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserCurrent6VCallback(int uid);
+
   public static native double getUserCurrent6V();
+
   public static native void setUserCurrent6V(double userCurrent6V);
 
-  public static native int registerUserActive6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserActive6VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserActive6VCallback(int uid);
+
   public static native boolean getUserActive6V();
+
   public static native void setUserActive6V(boolean userActive6V);
 
-  public static native int registerUserVoltage5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserVoltage5VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserVoltage5VCallback(int uid);
+
   public static native double getUserVoltage5V();
+
   public static native void setUserVoltage5V(double userVoltage5V);
 
-  public static native int registerUserCurrent5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserCurrent5VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserCurrent5VCallback(int uid);
+
   public static native double getUserCurrent5V();
+
   public static native void setUserCurrent5V(double userCurrent5V);
 
-  public static native int registerUserActive5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserActive5VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserActive5VCallback(int uid);
+
   public static native boolean getUserActive5V();
+
   public static native void setUserActive5V(boolean userActive5V);
 
-  public static native int registerUserVoltage3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserVoltage3V3Callback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserVoltage3V3Callback(int uid);
+
   public static native double getUserVoltage3V3();
+
   public static native void setUserVoltage3V3(double userVoltage3V3);
 
-  public static native int registerUserCurrent3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserCurrent3V3Callback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserCurrent3V3Callback(int uid);
+
   public static native double getUserCurrent3V3();
+
   public static native void setUserCurrent3V3(double userCurrent3V3);
 
-  public static native int registerUserActive3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserActive3V3Callback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserActive3V3Callback(int uid);
+
   public static native boolean getUserActive3V3();
+
   public static native void setUserActive3V3(boolean userActive3V3);
 
-  public static native int registerUserFaults6VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserFaults6VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserFaults6VCallback(int uid);
+
   public static native int getUserFaults6V();
+
   public static native void setUserFaults6V(int userFaults6V);
 
-  public static native int registerUserFaults5VCallback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserFaults5VCallback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserFaults5VCallback(int uid);
+
   public static native int getUserFaults5V();
+
   public static native void setUserFaults5V(int userFaults5V);
 
-  public static native int registerUserFaults3V3Callback(NotifyCallback callback, boolean initialNotify);
+  public static native int registerUserFaults3V3Callback(
+      NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelUserFaults3V3Callback(int uid);
+
   public static native int getUserFaults3V3();
+
   public static native void setUserFaults3V3(int userFaults3V3);
 
+  public static native int registerBrownoutVoltageCallback(
+      NotifyCallback callback, boolean initialNotify);
+
+  public static native void cancelBrownoutVoltageCallback(int uid);
+
+  public static native double getBrownoutVoltage();
+
+  public static native void setBrownoutVoltage(double brownoutVoltage);
+
   public static native void resetData();
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
index 2e8ebae..9ceaad8 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
@@ -1,38 +1,55 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class SPIAccelerometerDataJNI extends JNIWrapper {
-  public static native int registerActiveCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerActiveCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelActiveCallback(int index, int uid);
+
   public static native boolean getActive(int index);
+
   public static native void setActive(int index, boolean active);
 
-  public static native int registerRangeCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerRangeCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelRangeCallback(int index, int uid);
+
   public static native int getRange(int index);
+
   public static native void setRange(int index, int range);
 
-  public static native int registerXCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerXCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelXCallback(int index, int uid);
+
   public static native double getX(int index);
+
   public static native void setX(int index, double x);
 
-  public static native int registerYCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerYCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelYCallback(int index, int uid);
+
   public static native double getY(int index);
+
   public static native void setY(int index, double y);
 
-  public static native int registerZCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerZCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelZCallback(int index, int uid);
+
   public static native double getZ(int index);
+
   public static native void setZ(int index, double z);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
index fd6b854..11b777e 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
@@ -1,27 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
 import edu.wpi.first.hal.JNIWrapper;
 
 public class SPIDataJNI extends JNIWrapper {
-  public static native int registerInitializedCallback(int index, NotifyCallback callback, boolean initialNotify);
+  public static native int registerInitializedCallback(
+      int index, NotifyCallback callback, boolean initialNotify);
+
   public static native void cancelInitializedCallback(int index, int uid);
+
   public static native boolean getInitialized(int index);
+
   public static native void setInitialized(int index, boolean initialized);
 
   public static native int registerReadCallback(int index, BufferCallback callback);
+
   public static native void cancelReadCallback(int index, int uid);
 
   public static native int registerWriteCallback(int index, ConstBufferCallback callback);
+
   public static native void cancelWriteCallback(int index, int uid);
 
-  public static native int registerReadAutoReceiveBufferCallback(int index, SpiReadAutoReceiveBufferCallback callback);
+  public static native int registerReadAutoReceiveBufferCallback(
+      int index, SpiReadAutoReceiveBufferCallback callback);
+
   public static native void cancelReadAutoReceiveBufferCallback(int index, int uid);
 
   public static native void resetData(int index);
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java
index 2390bf4..d9390ce 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceCallback.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
index ea2a9f6..ef6536a 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimDeviceDataJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
@@ -12,19 +9,27 @@
 
 public class SimDeviceDataJNI extends JNIWrapper {
   public static native void setSimDeviceEnabled(String prefix, boolean enabled);
+
   public static native boolean isSimDeviceEnabled(String name);
 
-  public static native int registerSimDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify);
+  public static native int registerSimDeviceCreatedCallback(
+      String prefix, SimDeviceCallback callback, boolean initialNotify);
+
   public static native void cancelSimDeviceCreatedCallback(int uid);
 
-  public static native int registerSimDeviceFreedCallback(String prefix, SimDeviceCallback callback);
+  public static native int registerSimDeviceFreedCallback(
+      String prefix, SimDeviceCallback callback, boolean initialNotify);
+
   public static native void cancelSimDeviceFreedCallback(int uid);
 
   public static native int getSimDeviceHandle(String name);
 
+  public static native String getSimDeviceName(int handle);
+
   public static native int getSimValueDeviceHandle(int handle);
 
   public static class SimDeviceInfo {
+    @SuppressWarnings("JavadocMethod")
     public SimDeviceInfo(String name, int handle) {
       this.name = name;
       this.handle = handle;
@@ -36,21 +41,43 @@
     @SuppressWarnings("MemberName")
     public int handle;
   }
+
   public static native SimDeviceInfo[] enumerateSimDevices(String prefix);
 
-  public static native int registerSimValueCreatedCallback(int device, SimValueCallback callback, boolean initialNotify);
+  public static native int registerSimValueCreatedCallback(
+      int device, SimValueCallback callback, boolean initialNotify);
+
   public static native void cancelSimValueCreatedCallback(int uid);
 
-  public static native int registerSimValueChangedCallback(int handle, SimValueCallback callback, boolean initialNotify);
+  public static native int registerSimValueChangedCallback(
+      int handle, SimValueCallback callback, boolean initialNotify);
+
   public static native void cancelSimValueChangedCallback(int uid);
 
+  /**
+   * Register a callback for SimDeviceJNI.resetSimValue(). The callback is called with the old
+   * value.
+   *
+   * @param handle simulated value handle
+   * @param callback callback
+   * @param initialNotify ignored (present for consistency)
+   * @return TODO
+   */
+  public static native int registerSimValueResetCallback(
+      int handle, SimValueCallback callback, boolean initialNotify);
+
+  public static native void cancelSimValueResetCallback(int uid);
+
   public static native int getSimValueHandle(int device, String name);
 
   public static class SimValueInfo {
-    public SimValueInfo(String name, int handle, boolean readonly, int type, long value1, double value2) {
+    @SuppressWarnings("JavadocMethod")
+    public SimValueInfo(
+        String name, int handle, int direction, int type, long value1, double value2) {
       this.name = name;
       this.handle = handle;
-      this.readonly = readonly;
+      this.readonly = direction == 1;
+      this.direction = direction;
       this.value = HALValue.fromNative(type, value1, value2);
     }
 
@@ -61,14 +88,21 @@
     public int handle;
 
     @SuppressWarnings("MemberName")
+    @Deprecated
     public boolean readonly;
 
     @SuppressWarnings("MemberName")
+    public int direction;
+
+    @SuppressWarnings("MemberName")
     public HALValue value;
   }
+
   public static native SimValueInfo[] enumerateSimValues(int device);
 
   public static native String[] getSimValueEnumOptions(int handle);
 
+  public static native double[] getSimValueEnumDoubleValues(int handle);
+
   public static native void resetSimDeviceData();
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java
index d65ab38..4f9da94 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimValueCallback.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
@@ -11,9 +8,10 @@
 
 @FunctionalInterface
 public interface SimValueCallback {
-  void callback(String name, int handle, boolean readonly, HALValue value);
+  void callback(String name, int handle, int direction, HALValue value);
 
-  default void callbackNative(String name, int handle, boolean readonly, int type, long value1, double value2) {
-    callback(name, handle, readonly, HALValue.fromNative(type, value1, value2));
+  default void callbackNative(
+      String name, int handle, int direction, int type, long value1, double value2) {
+    callback(name, handle, direction, HALValue.fromNative(type, value1, value2));
   }
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
index 8147507..3dfeb06 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SimulatorJNI.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
@@ -11,14 +8,24 @@
 
 public class SimulatorJNI extends JNIWrapper {
   public static native void setRuntimeType(int type);
+
   public static native void waitForProgramStart();
+
   public static native void setProgramStarted();
+
   public static native boolean getProgramStarted();
+
   public static native void restartTiming();
+
   public static native void pauseTiming();
+
   public static native void resumeTiming();
+
   public static native boolean isTimingPaused();
+
   public static native void stepTiming(long delta);
+
   public static native void stepTimingAsync(long delta);
+
   public static native void resetHandles();
 }
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java
index 8061b5b..10dd0d2 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.simulation;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java
index 42e2059..e9f9a91 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/AllocationException.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.util;
 
-/**
- * Exception indicating that the resource is already allocated.
- */
+/** Exception indicating that the resource is already allocated. */
 @SuppressWarnings("serial")
 public class AllocationException extends RuntimeException {
   /**
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java
index 8fef4c9..683980d 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/BoundaryException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.util;
 
@@ -31,8 +28,8 @@
    */
   public static void assertWithinBounds(double value, double lower, double upper) {
     if (value < lower || value > upper) {
-      throw new BoundaryException("Value must be between " + lower + " and " + upper + ", " + value
-          + " given");
+      throw new BoundaryException(
+          "Value must be between " + lower + " and " + upper + ", " + value + " given");
     }
   }
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java
index d9a8a80..6155f17 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/CheckedAllocationException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.util;
 
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java
index a775ec4..65c11f5 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/HalHandleException.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.util;
 
-/**
- * Exception indicating that an error has occurred with a HAL Handle.
- */
+/** Exception indicating that an error has occurred with a HAL Handle. */
 @SuppressWarnings("serial")
 public class HalHandleException extends RuntimeException {
   /**
diff --git a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
index 3d6ab36..90650fc 100644
--- a/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
+++ b/third_party/allwpilib/hal/src/main/java/edu/wpi/first/hal/util/UncleanStatusException.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.hal.util;
 
-/**
- * Exception for bad status codes from the chip object.
- */
+/** Exception for bad status codes from the chip object. */
 @SuppressWarnings("serial")
 public final class UncleanStatusException extends IllegalStateException {
   private final int m_statusCode;
@@ -17,7 +12,7 @@
   /**
    * Create a new UncleanStatusException.
    *
-   * @param status  the status code that caused the exception
+   * @param status the status code that caused the exception
    * @param message A message describing the exception
    */
   public UncleanStatusException(int status, String message) {
@@ -43,9 +38,7 @@
     this(-1, message);
   }
 
-  /**
-   * Create a new UncleanStatusException.
-   */
+  /** Create a new UncleanStatusException. */
   public UncleanStatusException() {
     this(-1, "Status code was non-zero");
   }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Accelerometer.cpp b/third_party/allwpilib/hal/src/main/native/athena/Accelerometer.cpp
index bc4d502..73b1357 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Accelerometer.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Accelerometer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Accelerometer.h"
 
@@ -78,11 +75,9 @@
   kReg_OffZ = 0x31
 };
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAccelerometer() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 namespace hal {
 
@@ -94,7 +89,7 @@
  */
 static void initializeAccelerometer() {
   hal::init::CheckInit();
-  int32_t status;
+  int32_t status = 0;
 
   if (!accel) {
     accel.reset(tAccel::create(&status));
@@ -126,7 +121,8 @@
   // Execute and wait until it's done (up to a millisecond)
   initialTime = HAL_GetFPGATime(&status);
   while (accel->readSTAT(&status) & 1) {
-    if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+    if (HAL_GetFPGATime(&status) > initialTime + 1000)
+      break;
   }
 
   // Send a stop transmit/receive message with the data
@@ -137,7 +133,8 @@
   // Execute and wait until it's done (up to a millisecond)
   initialTime = HAL_GetFPGATime(&status);
   while (accel->readSTAT(&status) & 1) {
-    if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+    if (HAL_GetFPGATime(&status) > initialTime + 1000)
+      break;
   }
 }
 
@@ -154,7 +151,8 @@
   // Execute and wait until it's done (up to a millisecond)
   initialTime = HAL_GetFPGATime(&status);
   while (accel->readSTAT(&status) & 1) {
-    if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+    if (HAL_GetFPGATime(&status) > initialTime + 1000)
+      break;
   }
 
   // Receive a message with the data and stop
@@ -165,7 +163,8 @@
   // Execute and wait until it's done (up to a millisecond)
   initialTime = HAL_GetFPGATime(&status);
   while (accel->readSTAT(&status) & 1) {
-    if (HAL_GetFPGATime(&status) > initialTime + 1000) break;
+    if (HAL_GetFPGATime(&status) > initialTime + 1000)
+      break;
   }
 
   return accel->readDATI(&status);
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AddressableLED.cpp b/third_party/allwpilib/hal/src/main/native/athena/AddressableLED.cpp
index 7334c99..74a323a 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AddressableLED.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AddressableLED.cpp
@@ -1,19 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AddressableLED.h"
 
 #include <cstring>
 
 #include <FRC_FPGA_ChipObject/fpgainterfacecapi/NiFpga_HMB.h>
+#include <fmt/format.h>
 
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AddressableLEDTypes.h"
 #include "hal/ChipObject.h"
@@ -35,8 +34,7 @@
     HAL_AddressableLEDHandle, AddressableLED, kNumAddressableLEDs,
     HAL_HandleEnum::AddressableLED>* addressableLEDHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAddressableLED() {
   static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
                                kNumAddressableLEDs,
@@ -44,8 +42,7 @@
       alH;
   addressableLEDHandles = &alH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -147,8 +144,13 @@
     return;
   }
 
-  if (length > HAL_kAddressableLEDMaxLength) {
+  if (length > HAL_kAddressableLEDMaxLength || length < 0) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format(
+            "LED length must be less than or equal to {}. {} was requested",
+            HAL_kAddressableLEDMaxLength, length));
     return;
   }
 
@@ -178,8 +180,13 @@
     return;
   }
 
-  if (length > led->stringLength) {
+  if (length > led->stringLength || length < 0) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format(
+            "Data length must be less than or equal to {}. {} was requested",
+            led->stringLength, length));
     return;
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogAccumulator.cpp b/third_party/allwpilib/hal/src/main/native/athena/AnalogAccumulator.cpp
index 6664c52..7ff7d47 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogAccumulator.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogAccumulator.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogAccumulator.h"
 
@@ -12,11 +9,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogAccumulator() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -28,7 +23,8 @@
     return false;
   }
   for (int32_t i = 0; i < kNumAccumulators; i++) {
-    if (port->channel == kAccumulatorChannels[i]) return true;
+    if (port->channel == kAccumulatorChannels[i])
+      return true;
   }
   return false;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogGyro.cpp b/third_party/allwpilib/hal/src/main/native/athena/AnalogGyro.cpp
index 12d688d..19f26dd 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogGyro.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogGyro.cpp
@@ -1,18 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogGyro.h"
 
+#include <string>
 #include <thread>
 
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 #include "AnalogInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/AnalogInput.h"
 #include "hal/handles/IndexedHandleResource.h"
@@ -24,6 +23,7 @@
   double voltsPerDegreePerSecond;
   double offset;
   int32_t center;
+  std::string previousAllocation;
 };
 
 }  // namespace
@@ -39,54 +39,62 @@
 static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
                              HAL_HandleEnum::AnalogGyro>* analogGyroHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogGyro() {
   static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
                                HAL_HandleEnum::AnalogGyro>
       agHandles;
   analogGyroHandles = &agHandles;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 static void Wait(double seconds) {
-  if (seconds < 0.0) return;
+  if (seconds < 0.0) {
+    return;
+  }
   std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
 }
 
 extern "C" {
 
 HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
+  // Handle will be type checked by HAL_IsAccumulatorChannel
+  int16_t channel = getHandleIndex(analogHandle);
   if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
     if (*status == 0) {
       *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
+                                       0, kNumAccumulators, channel);
     }
     return HAL_kInvalidHandle;
   }
 
-  // handle known to be correct, so no need to type check
-  int16_t channel = getHandleIndex(analogHandle);
+  HAL_GyroHandle handle;
+  auto gyro = analogGyroHandles->Allocate(channel, &handle, status);
 
-  auto handle = analogGyroHandles->Allocate(channel, status);
-
-  if (*status != 0)
+  if (*status != 0) {
+    if (gyro) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Gyro", channel,
+                                           gyro->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
+                                       0, kNumAccumulators, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
 
   // Initialize port structure
-  auto gyro = analogGyroHandles->Get(handle);
-  if (gyro == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
 
   gyro->handle = analogHandle;
   gyro->voltsPerDegreePerSecond = 0;
   gyro->offset = 0;
   gyro->center = 0;
 
+  gyro->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 
@@ -100,17 +108,25 @@
   gyro->voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
 
   HAL_SetAnalogAverageBits(gyro->handle, kAverageBits, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   HAL_SetAnalogOversampleBits(gyro->handle, kOversampleBits, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   double sampleRate =
       kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
   HAL_SetAnalogSampleRate(sampleRate, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   Wait(0.1);
 
   HAL_SetAnalogGyroDeadband(handle, 0.0, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 }
 
 void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {
@@ -151,14 +167,18 @@
     return;
   }
   HAL_ResetAccumulator(gyro->handle, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 
   const double sampleTime = 1.0 / HAL_GetAnalogSampleRate(status);
   const double overSamples =
       1 << HAL_GetAnalogOversampleBits(gyro->handle, status);
   const double averageSamples =
       1 << HAL_GetAnalogAverageBits(gyro->handle, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   Wait(sampleTime * overSamples * averageSamples);
 }
 
@@ -170,15 +190,19 @@
   }
 
   HAL_InitAccumulator(gyro->handle, status);
-  if (*status != 0) return;
-  wpi::outs() << "Calibrating analog gyro for " << kCalibrationSampleTime
-              << " seconds." << '\n';
+  if (*status != 0) {
+    return;
+  }
+  fmt::print("Calibrating analog gyro for {} seconds.\n",
+             kCalibrationSampleTime);
   Wait(kCalibrationSampleTime);
 
   int64_t value;
   int64_t count;
   HAL_GetAccumulatorOutput(gyro->handle, &value, &count, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 
   gyro->center = static_cast<int32_t>(
       static_cast<double>(value) / static_cast<double>(count) + 0.5);
@@ -186,7 +210,9 @@
   gyro->offset = static_cast<double>(value) / static_cast<double>(count) -
                  static_cast<double>(gyro->center);
   HAL_SetAccumulatorCenter(gyro->handle, gyro->center, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   HAL_ResetAnalogGyro(handle, status);
 }
 
@@ -200,7 +226,9 @@
   int32_t deadband = static_cast<int32_t>(
       volts * 1e9 / HAL_GetAnalogLSBWeight(gyro->handle, status) *
       (1 << HAL_GetAnalogOversampleBits(gyro->handle, status)));
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   HAL_SetAccumulatorDeadband(gyro->handle, deadband, status);
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogInput.cpp b/third_party/allwpilib/hal/src/main/native/athena/AnalogInput.cpp
index 1d502b9..6b3add1 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogInput.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogInput.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogInput.h"
 
@@ -12,44 +9,52 @@
 
 #include "AnalogInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/handles/HandlesInternal.h"
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogInput() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 using namespace hal;
 
 extern "C" {
 
-HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
-                                                    int32_t* status) {
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation,
+    int32_t* status) {
   hal::init::CheckInit();
   initializeAnalog(status);
 
-  if (*status != 0) return HAL_kInvalidHandle;
+  if (*status != 0) {
+    return HAL_kInvalidHandle;
+  }
 
   int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (channel == InvalidHandleIndex || channel >= kNumAnalogInputs) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
+                                     0, kNumAnalogInputs, channel);
     return HAL_kInvalidHandle;
   }
 
-  HAL_AnalogInputHandle handle = analogInputHandles->Allocate(channel, status);
+  HAL_AnalogInputHandle handle;
+  auto analog_port = analogInputHandles->Allocate(channel, &handle, status);
 
-  if (*status != 0)
+  if (*status != 0) {
+    if (analog_port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Input", channel,
+                                           analog_port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
+                                       0, kNumAnalogInputs, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
 
   // Initialize port structure
-  auto analog_port = analogInputHandles->Get(handle);
-  if (analog_port == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
   analog_port->channel = static_cast<uint8_t>(channel);
   if (HAL_IsAccumulatorChannel(handle, status)) {
     analog_port->accumulator.reset(tAccumulator::create(channel, status));
@@ -61,6 +66,8 @@
   analogInputSystem->writeScanList(channel, channel, status);
   HAL_SetAnalogAverageBits(handle, kDefaultAverageBits, status);
   HAL_SetAnalogOversampleBits(handle, kDefaultOversampleBits, status);
+  analog_port->previousAllocation =
+      allocationLocation ? allocationLocation : "";
   return handle;
 }
 
@@ -69,7 +76,9 @@
   analogInputHandles->Free(analogPortHandle);
 }
 
-HAL_Bool HAL_CheckAnalogModule(int32_t module) { return module == 1; }
+HAL_Bool HAL_CheckAnalogModule(int32_t module) {
+  return module == 1;
+}
 
 HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) {
   return channel < kNumAnalogInputs && channel >= 0;
@@ -83,13 +92,17 @@
   // TODO: Need double comparison with epsilon.
   // wpi_assert(!sampleRateSet || GetSampleRate() == samplesPerSecond);
   initializeAnalog(status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   setAnalogSampleRate(samplesPerSecond, status);
 }
 
 double HAL_GetAnalogSampleRate(int32_t* status) {
   initializeAnalog(status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   uint32_t ticksPerConversion = analogInputSystem->readLoopTiming(status);
   uint32_t ticksPerSample =
       ticksPerConversion * getAnalogNumActiveChannels(status);
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.cpp b/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.cpp
index 6ac16b8..7ae6adc 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "AnalogInternal.h"
 
@@ -43,9 +40,13 @@
 
 void initializeAnalog(int32_t* status) {
   hal::init::CheckInit();
-  if (analogSystemInitialized) return;
+  if (analogSystemInitialized) {
+    return;
+  }
   std::scoped_lock lock(analogRegisterWindowMutex);
-  if (analogSystemInitialized) return;
+  if (analogSystemInitialized) {
+    return;
+  }
   analogInputSystem.reset(tAI::create(status));
   analogOutputSystem.reset(tAO::create(status));
   setAnalogNumChannelsToActivate(kNumAnalogInputs);
@@ -55,7 +56,9 @@
 
 int32_t getAnalogNumActiveChannels(int32_t* status) {
   int32_t scanSize = analogInputSystem->readConfig_ScanSize(status);
-  if (scanSize == 0) return 8;
+  if (scanSize == 0) {
+    return 8;
+  }
   return scanSize;
 }
 
@@ -64,8 +67,9 @@
 }
 
 int32_t getAnalogNumChannelsToActivate(int32_t* status) {
-  if (analogNumChannelsToActivate == 0)
+  if (analogNumChannelsToActivate == 0) {
     return getAnalogNumActiveChannels(status);
+  }
   return analogNumChannelsToActivate;
 }
 
@@ -82,7 +86,9 @@
       ticksPerSample / getAnalogNumChannelsToActivate(status);
   // ticksPerConversion must be at least 80
   if (ticksPerConversion < 80) {
-    if ((*status) >= 0) *status = SAMPLE_RATE_TOO_HIGH;
+    if ((*status) >= 0) {
+      *status = SAMPLE_RATE_TOO_HIGH;
+    }
     ticksPerConversion = 80;
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.h b/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.h
index a74562f..431c624 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogInternal.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
 #include <memory>
+#include <string>
 
 #include <wpi/mutex.h>
 
@@ -34,6 +32,7 @@
 struct AnalogPort {
   uint8_t channel;
   std::unique_ptr<tAccumulator> accumulator;
+  std::string previousAllocation;
 };
 
 extern IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogOutput.cpp b/third_party/allwpilib/hal/src/main/native/athena/AnalogOutput.cpp
index 77f841b..eab7d82 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogOutput.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogOutput.cpp
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogOutput.h"
 
+#include <string>
+
 #include "AnalogInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
@@ -20,6 +20,7 @@
 
 struct AnalogOutput {
   uint8_t channel;
+  std::string previousAllocation;
 };
 
 }  // namespace
@@ -28,45 +29,53 @@
                              kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>*
     analogOutputHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogOutput() {
   static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
                                kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>
       aoH;
   analogOutputHandles = &aoH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
-HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
-                                                      int32_t* status) {
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation,
+    int32_t* status) {
   hal::init::CheckInit();
   initializeAnalog(status);
 
-  if (*status != 0) return HAL_kInvalidHandle;
-
-  int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (*status != 0) {
     return HAL_kInvalidHandle;
   }
 
-  HAL_AnalogOutputHandle handle =
-      analogOutputHandles->Allocate(channel, status);
-
-  if (*status != 0)
-    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = analogOutputHandles->Get(handle);
-  if (port == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumAnalogOutputs) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Output",
+                                     0, kNumAnalogOutputs, channel);
     return HAL_kInvalidHandle;
   }
 
+  HAL_AnalogOutputHandle handle;
+  auto port = analogOutputHandles->Allocate(channel, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Output", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status,
+                                       "Invalid Index for Analog Output", 0,
+                                       kNumAnalogOutputs, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
   port->channel = static_cast<uint8_t>(channel);
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 
@@ -85,10 +94,11 @@
 
   uint16_t rawValue = static_cast<uint16_t>(voltage / 5.0 * 0x1000);
 
-  if (voltage < 0.0)
+  if (voltage < 0.0) {
     rawValue = 0;
-  else if (voltage > 5.0)
+  } else if (voltage > 5.0) {
     rawValue = 0x1000;
+  }
 
   analogOutputSystem->writeMXP(port->channel, rawValue, status);
 }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/AnalogTrigger.cpp b/third_party/allwpilib/hal/src/main/native/athena/AnalogTrigger.cpp
index 9ec3f29..d9e2b92 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/AnalogTrigger.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/AnalogTrigger.cpp
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogTrigger.h"
 
 #include "AnalogInternal.h"
 #include "DutyCycleInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogInput.h"
 #include "hal/DutyCycle.h"
@@ -33,8 +31,7 @@
                              kNumAnalogTriggers, HAL_HandleEnum::AnalogTrigger>*
     analogTriggerHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogTrigger() {
   static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
                                kNumAnalogTriggers,
@@ -42,8 +39,7 @@
       atH;
   analogTriggerHandles = &atH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -143,6 +139,11 @@
 
   if (lower < 0.0 || upper > 1.0) {
     *status = PARAMETER_OUT_OF_RANGE;
+    auto lowerStr = std::to_string(lower);
+    auto upperStr = std::to_string(upper);
+    hal::SetLastError(
+        status, "Lower must be >= 0 and upper must be <=1. Requested lower " +
+                    lowerStr + " Requested upper " + upperStr);
     return;
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CAN.cpp b/third_party/allwpilib/hal/src/main/native/athena/CAN.cpp
index 8105358..3436a0b 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/CAN.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/CAN.cpp
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/CAN.h"
 
 #include <FRC_NetworkCommunication/CANSessionMux.h>
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeCAN() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp b/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp
index d460885..a7c5b37 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/CANAPI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/CANAPI.h"
 
@@ -47,15 +44,13 @@
   return ms & 0xFFFFFFFF;
 }
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeCANAPI() {
   static UnlimitedHandleResource<HAL_CANHandle, CANStorage, HAL_HandleEnum::CAN>
       cH;
   canHandles = &cH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 static int32_t CreateCANId(CANStorage* storage, int32_t apiId) {
   int32_t createdId = 0;
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CTREPCM.cpp b/third_party/allwpilib/hal/src/main/native/athena/CTREPCM.cpp
new file mode 100644
index 0000000..b000ace
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/CTREPCM.cpp
@@ -0,0 +1,399 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/CTREPCM.h"
+
+#include <fmt/format.h>
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+    HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
+
+static constexpr HAL_CANDeviceType deviceType =
+    HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics;
+
+static constexpr int32_t Status1 = 0x50;
+static constexpr int32_t StatusSolFaults = 0x51;
+static constexpr int32_t StatusDebug = 0x52;
+
+static constexpr int32_t Control1 = 0x70;
+static constexpr int32_t Control2 = 0x71;
+static constexpr int32_t Control3 = 0x72;
+
+static constexpr int32_t TimeoutMs = 100;
+static constexpr int32_t SendPeriod = 20;
+
+union PcmStatus {
+  uint8_t data[8];
+  struct Bits {
+    /* Byte 0 */
+    unsigned SolenoidBits : 8;
+    /* Byte 1 */
+    unsigned compressorOn : 1;
+    unsigned stickyFaultFuseTripped : 1;
+    unsigned stickyFaultCompCurrentTooHigh : 1;
+    unsigned faultFuseTripped : 1;
+    unsigned faultCompCurrentTooHigh : 1;
+    unsigned faultHardwareFailure : 1;
+    unsigned isCloseloopEnabled : 1;
+    unsigned pressureSwitchEn : 1;
+    /* Byte 2*/
+    unsigned battVoltage : 8;
+    /* Byte 3 */
+    unsigned solenoidVoltageTop8 : 8;
+    /* Byte 4 */
+    unsigned compressorCurrentTop6 : 6;
+    unsigned solenoidVoltageBtm2 : 2;
+    /* Byte 5 */
+    unsigned StickyFault_dItooHigh : 1;
+    unsigned Fault_dItooHigh : 1;
+    unsigned moduleEnabled : 1;
+    unsigned closedLoopOutput : 1;
+    unsigned compressorCurrentBtm4 : 4;
+    /* Byte 6 */
+    unsigned tokenSeedTop8 : 8;
+    /* Byte 7 */
+    unsigned tokenSeedBtm8 : 8;
+  } bits;
+};
+
+union PcmControl {
+  uint8_t data[8];
+  struct Bits {
+    /* Byte 0 */
+    unsigned tokenTop8 : 8;
+    /* Byte 1 */
+    unsigned tokenBtm8 : 8;
+    /* Byte 2 */
+    unsigned solenoidBits : 8;
+    /* Byte 3*/
+    unsigned reserved : 4;
+    unsigned closeLoopOutput : 1;
+    unsigned compressorOn : 1;
+    unsigned closedLoopEnable : 1;
+    unsigned clearStickyFaults : 1;
+    /* Byte 4 */
+    unsigned OneShotField_h8 : 8;
+    /* Byte 5 */
+    unsigned OneShotField_l8 : 8;
+  } bits;
+};
+
+struct PcmControlSetOneShotDur {
+  uint8_t sol10MsPerUnit[8];
+};
+
+union PcmStatusFault {
+  uint8_t data[8];
+  struct Bits {
+    /* Byte 0 */
+    unsigned SolenoidDisabledList : 8;
+    /* Byte 1 */
+    unsigned reserved_bit0 : 1;
+    unsigned reserved_bit1 : 1;
+    unsigned reserved_bit2 : 1;
+    unsigned reserved_bit3 : 1;
+    unsigned StickyFault_CompNoCurrent : 1;
+    unsigned Fault_CompNoCurrent : 1;
+    unsigned StickyFault_SolenoidJumper : 1;
+    unsigned Fault_SolenoidJumper : 1;
+  } bits;
+};
+
+union PcmDebug {
+  uint8_t data[8];
+  struct Bits {
+    unsigned tokFailsTop8 : 8;
+    unsigned tokFailsBtm8 : 8;
+    unsigned lastFailedTokTop8 : 8;
+    unsigned lastFailedTokBtm8 : 8;
+    unsigned tokSuccessTop8 : 8;
+    unsigned tokSuccessBtm8 : 8;
+  } bits;
+};
+
+namespace {
+struct PCM {
+  HAL_CANHandle canHandle;
+  wpi::mutex lock;
+  std::string previousAllocation;
+  PcmControl control;
+  PcmControlSetOneShotDur oneShot;
+};
+}  // namespace
+
+static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
+                             HAL_HandleEnum::CTREPCM>* pcmHandles;
+
+namespace hal::init {
+void InitializeCTREPCM() {
+  static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
+                               HAL_HandleEnum::CTREPCM>
+      pH;
+  pcmHandles = &pH;
+}
+}  // namespace hal::init
+
+#define READ_PACKET(type, frame, failureValue)                             \
+  auto pcm = pcmHandles->Get(handle);                                      \
+  if (pcm == nullptr) {                                                    \
+    *status = HAL_HANDLE_ERROR;                                            \
+    return failureValue;                                                   \
+  }                                                                        \
+  type pcmStatus;                                                          \
+  int32_t length = 0;                                                      \
+  uint64_t receivedTimestamp = 0;                                          \
+  HAL_ReadCANPacketTimeout(pcm->canHandle, frame, pcmStatus.data, &length, \
+                           &receivedTimestamp, TimeoutMs, status);         \
+  if (*status != 0) {                                                      \
+    return failureValue;                                                   \
+  }
+
+#define READ_STATUS(failureValue) READ_PACKET(PcmStatus, Status1, failureValue)
+#define READ_SOL_FAULTS(failureValue) \
+  READ_PACKET(PcmStatusFault, StatusSolFaults, failureValue)
+
+static void SendControl(PCM* pcm, int32_t* status) {
+  HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->control.data, 8, Control1,
+                              SendPeriod, status);
+}
+
+extern "C" {
+
+HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
+                                        const char* allocationLocation,
+                                        int32_t* status) {
+  hal::init::CheckInit();
+
+  HAL_CTREPCMHandle handle;
+  auto pcm = pcmHandles->Allocate(module, &handle, status);
+
+  if (*status != 0) {
+    if (pcm) {
+      hal::SetLastErrorPreviouslyAllocated(status, "CTRE PCM", module,
+                                           pcm->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0,
+                                       kNumCTREPCMModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  pcm->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
+  if (*status != 0) {
+    pcmHandles->Free(handle);
+    return HAL_kInvalidHandle;
+  }
+
+  std::memset(&pcm->oneShot, 0, sizeof(pcm->oneShot));
+  std::memset(&pcm->control, 0, sizeof(pcm->control));
+
+  pcm->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  // Enable closed loop control
+  HAL_SetCTREPCMClosedLoopControl(handle, true, status);
+  if (*status != 0) {
+    HAL_FreeCTREPCM(handle);
+    return HAL_kInvalidHandle;
+  }
+  return handle;
+}
+
+void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm) {
+    HAL_CleanCAN(pcm->canHandle);
+  }
+  pcmHandles->Free(handle);
+}
+
+HAL_Bool HAL_CheckCTREPCMSolenoidChannel(int32_t channel) {
+  return channel < kNumCTRESolenoidChannels && channel >= 0;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressor(HAL_CTREPCMHandle handle, int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.compressorOn;
+}
+
+void HAL_SetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, HAL_Bool enabled,
+                                     int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  std::scoped_lock lock{pcm->lock};
+  pcm->control.bits.closedLoopEnable = enabled ? 1 : 0;
+  SendControl(pcm.get(), status);
+}
+
+HAL_Bool HAL_GetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle,
+                                         int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.isCloseloopEnabled;
+}
+
+HAL_Bool HAL_GetCTREPCMPressureSwitch(HAL_CTREPCMHandle handle,
+                                      int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.pressureSwitchEn;
+}
+
+double HAL_GetCTREPCMCompressorCurrent(HAL_CTREPCMHandle handle,
+                                       int32_t* status) {
+  READ_STATUS(0);
+  uint32_t result = pcmStatus.bits.compressorCurrentTop6;
+  result <<= 4;
+  result |= pcmStatus.bits.compressorCurrentBtm4;
+  return result * 0.03125; /* 5.5 fixed pt value in Amps */
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighFault(HAL_CTREPCMHandle handle,
+                                                     int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.faultCompCurrentTooHigh;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(
+    HAL_CTREPCMHandle handle, int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.stickyFaultCompCurrentTooHigh;
+}
+HAL_Bool HAL_GetCTREPCMCompressorShortedStickyFault(HAL_CTREPCMHandle handle,
+                                                    int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.Fault_dItooHigh;
+}
+HAL_Bool HAL_GetCTREPCMCompressorShortedFault(HAL_CTREPCMHandle handle,
+                                              int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.StickyFault_dItooHigh;
+}
+HAL_Bool HAL_GetCTREPCMCompressorNotConnectedStickyFault(
+    HAL_CTREPCMHandle handle, int32_t* status) {
+  READ_SOL_FAULTS(false);
+  return pcmStatus.bits.StickyFault_CompNoCurrent;
+}
+HAL_Bool HAL_GetCTREPCMCompressorNotConnectedFault(HAL_CTREPCMHandle handle,
+                                                   int32_t* status) {
+  READ_SOL_FAULTS(false);
+  return pcmStatus.bits.Fault_CompNoCurrent;
+}
+
+int32_t HAL_GetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t* status) {
+  READ_STATUS(0);
+  return pcmStatus.bits.SolenoidBits & 0xFF;
+}
+
+void HAL_SetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t mask,
+                             int32_t values, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint8_t smallMask = mask & 0xFF;
+  uint8_t smallValues =
+      (values & 0xFF) & smallMask;  // Enforce only masked values are set
+  uint8_t invertMask = ~smallMask;
+
+  std::scoped_lock lock{pcm->lock};
+  uint8_t existingValue = invertMask & pcm->control.bits.solenoidBits;
+  pcm->control.bits.solenoidBits = existingValue | smallValues;
+  SendControl(pcm.get(), status);
+}
+
+int32_t HAL_GetCTREPCMSolenoidDisabledList(HAL_CTREPCMHandle handle,
+                                           int32_t* status) {
+  READ_SOL_FAULTS(0);
+  return pcmStatus.bits.SolenoidDisabledList;
+}
+
+HAL_Bool HAL_GetCTREPCMSolenoidVoltageStickyFault(HAL_CTREPCMHandle handle,
+                                                  int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.stickyFaultFuseTripped;
+}
+
+HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle,
+                                            int32_t* status) {
+  READ_STATUS(false);
+  return pcmStatus.bits.faultFuseTripped;
+}
+
+void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle,
+                                     int32_t* status) {
+  uint8_t controlData[] = {0, 0, 0, 0x80};
+  HAL_WriteCANPacket(handle, controlData, sizeof(controlData), Control2,
+                     status);
+}
+
+void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index,
+                            int32_t* status) {
+  if (index > 7 || index < 0) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Only [0-7] are valid index values. Requested {}", index));
+    return;
+  }
+
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  std::scoped_lock lock{pcm->lock};
+  uint16_t oneShotField = pcm->control.bits.OneShotField_h8;
+  oneShotField <<= 8;
+  oneShotField |= pcm->control.bits.OneShotField_l8;
+
+  uint16_t shift = 2 * index;
+  uint16_t mask = 3;
+  uint8_t chBits = (oneShotField >> shift) & mask;
+  chBits = (chBits % 3) + 1;
+  oneShotField &= ~(mask << shift);
+  oneShotField |= (chBits << shift);
+  pcm->control.bits.OneShotField_h8 = oneShotField >> 8;
+  pcm->control.bits.OneShotField_l8 = oneShotField;
+  SendControl(pcm.get(), status);
+}
+
+void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
+                                   int32_t durMs, int32_t* status) {
+  if (index > 7 || index < 0) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Only [0-7] are valid index values. Requested {}", index));
+    return;
+  }
+
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  std::scoped_lock lock{pcm->lock};
+  pcm->oneShot.sol10MsPerUnit[index] =
+      (std::min)(static_cast<uint32_t>(durMs) / 10,
+                 static_cast<uint32_t>(0xFF));
+  HAL_WriteCANPacketRepeating(pcm->canHandle, pcm->oneShot.sol10MsPerUnit, 8,
+                              Control2, SendPeriod, status);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp
new file mode 100644
index 0000000..1a25a64
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.cpp
@@ -0,0 +1,526 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "CTREPDP.h"
+
+#include <fmt/format.h>
+#include <wpi/mutex.h>
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/handles/IndexedHandleResource.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+    HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
+
+static constexpr HAL_CANDeviceType deviceType =
+    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
+
+static constexpr int32_t Status1 = 0x50;
+static constexpr int32_t Status2 = 0x51;
+static constexpr int32_t Status3 = 0x52;
+static constexpr int32_t StatusEnergy = 0x5D;
+
+static constexpr int32_t Control1 = 0x70;
+
+static constexpr int32_t TimeoutMs = 100;
+
+/* encoder/decoders */
+union PdpStatus1 {
+  uint8_t data[8];
+  struct Bits {
+    unsigned chan1_h8 : 8;
+    unsigned chan2_h6 : 6;
+    unsigned chan1_l2 : 2;
+    unsigned chan3_h4 : 4;
+    unsigned chan2_l4 : 4;
+    unsigned chan4_h2 : 2;
+    unsigned chan3_l6 : 6;
+    unsigned chan4_l8 : 8;
+    unsigned chan5_h8 : 8;
+    unsigned chan6_h6 : 6;
+    unsigned chan5_l2 : 2;
+    unsigned reserved4 : 4;
+    unsigned chan6_l4 : 4;
+  } bits;
+};
+
+union PdpStatus2 {
+  uint8_t data[8];
+  struct Bits {
+    unsigned chan7_h8 : 8;
+    unsigned chan8_h6 : 6;
+    unsigned chan7_l2 : 2;
+    unsigned chan9_h4 : 4;
+    unsigned chan8_l4 : 4;
+    unsigned chan10_h2 : 2;
+    unsigned chan9_l6 : 6;
+    unsigned chan10_l8 : 8;
+    unsigned chan11_h8 : 8;
+    unsigned chan12_h6 : 6;
+    unsigned chan11_l2 : 2;
+    unsigned reserved4 : 4;
+    unsigned chan12_l4 : 4;
+  } bits;
+};
+
+union PdpStatus3 {
+  uint8_t data[8];
+  struct Bits {
+    unsigned chan13_h8 : 8;
+    unsigned chan14_h6 : 6;
+    unsigned chan13_l2 : 2;
+    unsigned chan15_h4 : 4;
+    unsigned chan14_l4 : 4;
+    unsigned chan16_h2 : 2;
+    unsigned chan15_l6 : 6;
+    unsigned chan16_l8 : 8;
+    unsigned internalResBattery_mOhms : 8;
+    unsigned busVoltage : 8;
+    unsigned temp : 8;
+  } bits;
+};
+
+union PdpStatusEnergy {
+  uint8_t data[8];
+  struct Bits {
+    unsigned TmeasMs_likelywillbe20ms_ : 8;
+    unsigned TotalCurrent_125mAperunit_h8 : 8;
+    unsigned Power_125mWperunit_h4 : 4;
+    unsigned TotalCurrent_125mAperunit_l4 : 4;
+    unsigned Power_125mWperunit_m8 : 8;
+    unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
+    unsigned Power_125mWperunit_l4 : 4;
+    unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
+    unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
+    unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
+  } bits;
+};
+
+namespace {
+struct PDP {
+  HAL_CANHandle canHandle;
+  std::string previousAllocation;
+};
+}  // namespace
+
+static IndexedHandleResource<HAL_PDPHandle, PDP, kNumCTREPDPModules,
+                             HAL_HandleEnum::CTREPDP>* pdpHandles;
+
+namespace hal::init {
+void InitializeCTREPDP() {
+  static IndexedHandleResource<HAL_PDPHandle, PDP, kNumCTREPDPModules,
+                               HAL_HandleEnum::CTREPDP>
+      pH;
+  pdpHandles = &pH;
+}
+}  // namespace hal::init
+
+extern "C" {
+
+HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
+                                int32_t* status) {
+  hal::init::CheckInit();
+  if (!HAL_CheckPDPModule(module)) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, fmt::format("Invalid pdp module {}", module));
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_PDPHandle handle;
+  auto pdp = pdpHandles->Allocate(module, &handle, status);
+
+  if (*status != 0) {
+    if (pdp) {
+      hal::SetLastErrorPreviouslyAllocated(status, "CTRE PDP", module,
+                                           pdp->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PDP", 0,
+                                       kNumCTREPDPModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  pdp->canHandle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
+  if (*status != 0) {
+    pdpHandles->Free(handle);
+    return HAL_kInvalidHandle;
+  }
+
+  pdp->previousAllocation = allocationLocation ? allocationLocation : "";
+
+  return handle;
+}
+
+void HAL_CleanPDP(HAL_PDPHandle handle) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp) {
+    HAL_CleanCAN(pdp->canHandle);
+  }
+  pdpHandles->Free(handle);
+}
+
+int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status) {
+  return hal::getHandleIndex(handle);
+}
+
+HAL_Bool HAL_CheckPDPModule(int32_t module) {
+  return module < kNumCTREPDPModules && module >= 0;
+}
+
+HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
+  return channel < kNumCTREPDPChannels && channel >= 0;
+}
+
+double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PdpStatus3 pdpStatus;
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+
+  HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
+                           &receivedTimestamp, TimeoutMs, status);
+
+  if (*status != 0) {
+    return 0;
+  } else {
+    return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
+  }
+}
+
+double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PdpStatus3 pdpStatus;
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+
+  HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
+                           &receivedTimestamp, TimeoutMs, status);
+
+  if (*status != 0) {
+    return 0;
+  } else {
+    return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
+  }
+}
+
+double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
+                                int32_t* status) {
+  if (!HAL_CheckPDPChannel(channel)) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, fmt::format("Invalid pdp channel {}", channel));
+    return 0;
+  }
+
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+
+  double raw = 0;
+
+  if (channel <= 5) {
+    PdpStatus1 pdpStatus;
+    HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length,
+                             &receivedTimestamp, TimeoutMs, status);
+    if (*status != 0) {
+      return 0;
+    }
+    switch (channel) {
+      case 0:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
+              pdpStatus.bits.chan1_l2;
+        break;
+      case 1:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
+              pdpStatus.bits.chan2_l4;
+        break;
+      case 2:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
+              pdpStatus.bits.chan3_l6;
+        break;
+      case 3:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
+              pdpStatus.bits.chan4_l8;
+        break;
+      case 4:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
+              pdpStatus.bits.chan5_l2;
+        break;
+      case 5:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
+              pdpStatus.bits.chan6_l4;
+        break;
+    }
+  } else if (channel <= 11) {
+    PdpStatus2 pdpStatus;
+    HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus.data, &length,
+                             &receivedTimestamp, TimeoutMs, status);
+    if (*status != 0) {
+      return 0;
+    }
+    switch (channel) {
+      case 6:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
+              pdpStatus.bits.chan7_l2;
+        break;
+      case 7:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
+              pdpStatus.bits.chan8_l4;
+        break;
+      case 8:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
+              pdpStatus.bits.chan9_l6;
+        break;
+      case 9:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
+              pdpStatus.bits.chan10_l8;
+        break;
+      case 10:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
+              pdpStatus.bits.chan11_l2;
+        break;
+      case 11:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
+              pdpStatus.bits.chan12_l4;
+        break;
+    }
+  } else {
+    PdpStatus3 pdpStatus;
+    HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus.data, &length,
+                             &receivedTimestamp, TimeoutMs, status);
+    if (*status != 0) {
+      return 0;
+    }
+    switch (channel) {
+      case 12:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
+              pdpStatus.bits.chan13_l2;
+        break;
+      case 13:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
+              pdpStatus.bits.chan14_l4;
+        break;
+      case 14:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
+              pdpStatus.bits.chan15_l6;
+        break;
+      case 15:
+        raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
+              pdpStatus.bits.chan16_l8;
+        break;
+    }
+  }
+
+  /* convert to amps */
+  return raw * 0.125; /* 7.3 fixed pt value in Amps */
+}
+
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+                                  int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+  PdpStatus1 pdpStatus;
+  HAL_ReadCANPacketTimeout(pdp->canHandle, Status1, pdpStatus.data, &length,
+                           &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return;
+  }
+  PdpStatus2 pdpStatus2;
+  HAL_ReadCANPacketTimeout(pdp->canHandle, Status2, pdpStatus2.data, &length,
+                           &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return;
+  }
+  PdpStatus3 pdpStatus3;
+  HAL_ReadCANPacketTimeout(pdp->canHandle, Status3, pdpStatus3.data, &length,
+                           &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return;
+  }
+
+  currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
+                 pdpStatus.bits.chan1_l2) *
+                0.125;
+  currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
+                 pdpStatus.bits.chan2_l4) *
+                0.125;
+  currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
+                 pdpStatus.bits.chan3_l6) *
+                0.125;
+  currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
+                 pdpStatus.bits.chan4_l8) *
+                0.125;
+  currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
+                 pdpStatus.bits.chan5_l2) *
+                0.125;
+  currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
+                 pdpStatus.bits.chan6_l4) *
+                0.125;
+
+  currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
+                 pdpStatus2.bits.chan7_l2) *
+                0.125;
+  currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
+                 pdpStatus2.bits.chan8_l4) *
+                0.125;
+  currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
+                 pdpStatus2.bits.chan9_l6) *
+                0.125;
+  currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
+                 pdpStatus2.bits.chan10_l8) *
+                0.125;
+  currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
+                  pdpStatus2.bits.chan11_l2) *
+                 0.125;
+  currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
+                  pdpStatus2.bits.chan12_l4) *
+                 0.125;
+
+  currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
+                  pdpStatus3.bits.chan13_l2) *
+                 0.125;
+  currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
+                  pdpStatus3.bits.chan14_l4) *
+                 0.125;
+  currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
+                  pdpStatus3.bits.chan15_l6) *
+                 0.125;
+  currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
+                  pdpStatus3.bits.chan16_l8) *
+                 0.125;
+}
+
+double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PdpStatusEnergy pdpStatus;
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+
+  HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
+                           &length, &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return 0;
+  }
+
+  uint32_t raw;
+  raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
+  raw <<= 4;
+  raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
+  return 0.125 * raw; /* 7.3 fixed pt value in Amps */
+}
+
+double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PdpStatusEnergy pdpStatus;
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+
+  HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
+                           &length, &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return 0;
+  }
+
+  uint32_t raw;
+  raw = pdpStatus.bits.Power_125mWperunit_h4;
+  raw <<= 8;
+  raw |= pdpStatus.bits.Power_125mWperunit_m8;
+  raw <<= 4;
+  raw |= pdpStatus.bits.Power_125mWperunit_l4;
+  return 0.125 * raw; /* 7.3 fixed pt value in Watts */
+}
+
+double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PdpStatusEnergy pdpStatus;
+  int32_t length = 0;
+  uint64_t receivedTimestamp = 0;
+
+  HAL_ReadCANPacketTimeout(pdp->canHandle, StatusEnergy, pdpStatus.data,
+                           &length, &receivedTimestamp, TimeoutMs, status);
+  if (*status != 0) {
+    return 0;
+  }
+
+  uint32_t raw;
+  raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
+  raw <<= 8;
+  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
+  raw <<= 8;
+  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
+  raw <<= 8;
+  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
+
+  double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
+  energyJoules *= 0.001;             /* convert from mW to W */
+  energyJoules *=
+      pdpStatus.bits
+          .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
+  return energyJoules;
+}
+
+void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
+  HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
+}
+
+void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
+  auto pdp = pdpHandles->Get(handle);
+  if (pdp == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
+  HAL_WriteCANPacket(pdp->canHandle, pdpControl, 1, Control1, status);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h
new file mode 100644
index 0000000..dd4b298
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/CTREPDP.h
@@ -0,0 +1,136 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_pdp PDP Functions
+ * @ingroup hal_capi
+ * Functions to control the Power Distribution Panel.
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a Power Distribution Panel.
+ *
+ * @param  module the module number to initialize
+ * @return the created PDP
+ */
+HAL_PDPHandle HAL_InitializePDP(int32_t module, const char* allocationLocation,
+                                int32_t* status);
+
+/**
+ * Cleans a PDP module.
+ *
+ * @param handle the module handle
+ */
+void HAL_CleanPDP(HAL_PDPHandle handle);
+
+/**
+ * Gets the module number for a pdp.
+ */
+int32_t HAL_GetPDPModuleNumber(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Checks if a PDP channel is valid.
+ *
+ * @param channel the channel to check
+ * @return true if the channel is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPDPChannel(int32_t channel);
+
+/**
+ * Checks if a PDP module is valid.
+ *
+ * @param channel the module to check
+ * @return true if the module is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPDPModule(int32_t module);
+
+/**
+ * Gets the temperature of the PDP.
+ *
+ * @param handle the module handle
+ * @return the module temperature (celsius)
+ */
+double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the PDP input voltage.
+ *
+ * @param handle the module handle
+ * @return the input voltage (volts)
+ */
+double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the current of a specific PDP channel.
+ *
+ * @param module  the module
+ * @param channel the channel
+ * @return the channel current (amps)
+ */
+double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
+                                int32_t* status);
+
+/**
+ * Gets the current of all 16 channels on the PDP.
+ *
+ * The array must be large enough to hold all channels.
+ *
+ * @param handle the module handle
+ * @param current the currents (output)
+ */
+void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
+                                  int32_t* status);
+
+/**
+ * Gets the total current of the PDP.
+ *
+ * @param handle the module handle
+ * @return the total current (amps)
+ */
+double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the total power of the PDP.
+ *
+ * @param handle the module handle
+ * @return the total power (watts)
+ */
+double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Gets the total energy of the PDP.
+ *
+ * @param handle the module handle
+ * @return the total energy (joules)
+ */
+double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Resets the PDP accumulated energy.
+ *
+ * @param handle the module handle
+ */
+void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
+
+/**
+ * Clears any PDP sticky faults.
+ *
+ * @param handle the module handle
+ */
+void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status);
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Compressor.cpp b/third_party/allwpilib/hal/src/main/native/athena/Compressor.cpp
deleted file mode 100644
index f381305..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/Compressor.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/Compressor.h"
-
-#include "HALInitializer.h"
-#include "PCMInternal.h"
-#include "PortsInternal.h"
-#include "ctre/PCM.h"
-#include "hal/Errors.h"
-#include "hal/handles/HandlesInternal.h"
-
-using namespace hal;
-
-namespace hal {
-namespace init {
-void InitializeCompressor() {}
-}  // namespace init
-}  // namespace hal
-
-extern "C" {
-
-HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status) {
-  hal::init::CheckInit();
-  // Use status to check for invalid index
-  initializePCM(module, status);
-  if (*status != 0) {
-    return HAL_kInvalidHandle;
-  }
-
-  // As compressors can have unlimited objects, just create a
-  // handle with the module number as the index.
-
-  return (HAL_CompressorHandle)createHandle(static_cast<int16_t>(module),
-                                            HAL_HandleEnum::Compressor, 0);
-}
-
-HAL_Bool HAL_CheckCompressorModule(int32_t module) {
-  return module < kNumPCMModules && module >= 0;
-}
-
-HAL_Bool HAL_GetCompressor(HAL_CompressorHandle compressorHandle,
-                           int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressor(value);
-
-  return value;
-}
-
-void HAL_SetCompressorClosedLoopControl(HAL_CompressorHandle compressorHandle,
-                                        HAL_Bool value, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  *status = PCM_modules[index]->SetClosedLoopControl(value);
-}
-
-HAL_Bool HAL_GetCompressorClosedLoopControl(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetClosedLoopControl(value);
-
-  return value;
-}
-
-HAL_Bool HAL_GetCompressorPressureSwitch(HAL_CompressorHandle compressorHandle,
-                                         int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetPressure(value);
-
-  return value;
-}
-
-double HAL_GetCompressorCurrent(HAL_CompressorHandle compressorHandle,
-                                int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return 0;
-  }
-  float value;
-
-  *status = PCM_modules[index]->GetCompressorCurrent(value);
-
-  return value;
-}
-HAL_Bool HAL_GetCompressorCurrentTooHighFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressorCurrentTooHighFault(value);
-
-  return value;
-}
-HAL_Bool HAL_GetCompressorCurrentTooHighStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressorCurrentTooHighStickyFault(value);
-
-  return value;
-}
-HAL_Bool HAL_GetCompressorShortedStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressorShortedStickyFault(value);
-
-  return value;
-}
-HAL_Bool HAL_GetCompressorShortedFault(HAL_CompressorHandle compressorHandle,
-                                       int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressorShortedFault(value);
-
-  return value;
-}
-HAL_Bool HAL_GetCompressorNotConnectedStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressorNotConnectedStickyFault(value);
-
-  return value;
-}
-HAL_Bool HAL_GetCompressorNotConnectedFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[index]->GetCompressorNotConnectedFault(value);
-
-  return value;
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Constants.cpp b/third_party/allwpilib/hal/src/main/native/athena/Constants.cpp
index 6af443d..a312935 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Constants.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Constants.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Constants.h"
 
@@ -11,11 +8,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeConstants() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/ConstantsInternal.h b/third_party/allwpilib/hal/src/main/native/athena/ConstantsInternal.h
index 55bbdee..21fece4 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/ConstantsInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/ConstantsInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Counter.cpp b/third_party/allwpilib/hal/src/main/native/athena/Counter.cpp
index 6d7e254..7f84df8 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Counter.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Counter.cpp
@@ -1,15 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Counter.h"
 
+#include <fmt/format.h>
+
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/HAL.h"
 #include "hal/handles/LimitedHandleResource.h"
@@ -28,16 +28,14 @@
 static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
                              HAL_HandleEnum::Counter>* counterHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeCounter() {
   static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
                                HAL_HandleEnum::Counter>
       ch;
   counterHandles = &ch;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -151,6 +149,9 @@
     // TODO: wpi_setWPIErrorWithContext(ParameterOutOfRange, "Counter only
     // supports DownSource in TwoPulse and ExternalDirection modes.");
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status,
+                      "Counter only supports DownSource in TwoPulse and "
+                      "ExternalDirection mode.");
     return;
   }
 
@@ -265,6 +266,10 @@
   }
   if (samplesToAverage < 1 || samplesToAverage > 127) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, fmt::format("Samples to average must be between "
+                                          "1 and 127 inclusive. Requested {}",
+                                          samplesToAverage));
+    return;
   }
   counter->counter->writeTimerConfig_AverageSize(samplesToAverage, status);
 }
@@ -362,10 +367,11 @@
   }
   if (counter->counter->readConfig_Mode(status) ==
       HAL_Counter_kExternalDirection) {
-    if (reverseDirection)
+    if (reverseDirection) {
       HAL_SetCounterDownSourceEdge(counterHandle, true, true, status);
-    else
+    } else {
       HAL_SetCounterDownSourceEdge(counterHandle, false, true, status);
+    }
   }
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/DIO.cpp b/third_party/allwpilib/hal/src/main/native/athena/DIO.cpp
index dc4631e..7b26dd1 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/DIO.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/DIO.cpp
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DIO.h"
 
 #include <cmath>
+#include <cstdio>
 #include <thread>
 
-#include <wpi/raw_ostream.h>
-
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/cpp/fpga_clock.h"
 #include "hal/handles/HandlesInternal.h"
@@ -28,8 +25,7 @@
                              kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>*
     digitalPWMHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDIO() {
   static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
                                kNumDigitalPWMOutputs,
@@ -37,36 +33,45 @@
       dpH;
   digitalPWMHandles = &dpH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
-                                        HAL_Bool input, int32_t* status) {
+                                        HAL_Bool input,
+                                        const char* allocationLocation,
+                                        int32_t* status) {
   hal::init::CheckInit();
   initializeDigital(status);
 
-  if (*status != 0) return HAL_kInvalidHandle;
-
-  int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex || channel >= kNumDigitalChannels) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (*status != 0) {
     return HAL_kInvalidHandle;
   }
 
-  auto handle =
-      digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO, status);
-
-  if (*status != 0)
-    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumDigitalChannels) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                     kNumDigitalChannels, channel);
     return HAL_kInvalidHandle;
   }
 
+  HAL_DigitalHandle handle;
+
+  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO,
+                                              &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                       kNumDigitalChannels, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
   port->channel = static_cast<uint8_t>(channel);
 
   std::scoped_lock lock(digitalDIOMutex);
@@ -116,6 +121,7 @@
   }
 
   digitalSystem->writeOutputEnable(outputEnable, status);
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
 
   return handle;
 }
@@ -127,7 +133,8 @@
 void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle) {
   auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
   // no status, so no need to check for a proper free.
-  if (port == nullptr) return;
+  if (port == nullptr)
+    return;
   digitalChannelHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
 
   // Wait for no other object to hold this handle.
@@ -135,8 +142,8 @@
   while (port.use_count() != 1) {
     auto current = hal::fpga_clock::now();
     if (start + std::chrono::seconds(1) < current) {
-      wpi::outs() << "DIO handle free timeout\n";
-      wpi::outs().flush();
+      std::puts("DIO handle free timeout");
+      std::fflush(stdout);
       break;
     }
     std::this_thread::yield();
@@ -189,9 +196,11 @@
   // higher freq.
   // TODO: Round in the linear rate domain.
   initializeDigital(status);
-  if (*status != 0) return;
-  uint16_t pwmPeriodPower = static_cast<uint16_t>(
-      std::log(1.0 / (16 * 1.0E-6 * rate)) / std::log(2.0) + 0.5);
+  if (*status != 0) {
+    return;
+  }
+  uint16_t pwmPeriodPower =
+      std::lround(std::log(1.0 / (16 * 1.0E-6 * rate)) / std::log(2.0));
   digitalSystem->writePWMPeriodPower(pwmPeriodPower, status);
 }
 
@@ -203,10 +212,16 @@
     return;
   }
   int32_t id = *port;
-  if (dutyCycle > 1.0) dutyCycle = 1.0;
-  if (dutyCycle < 0.0) dutyCycle = 0.0;
+  if (dutyCycle > 1.0) {
+    dutyCycle = 1.0;
+  }
+  if (dutyCycle < 0.0) {
+    dutyCycle = 0.0;
+  }
   double rawDutyCycle = 256.0 * dutyCycle;
-  if (rawDutyCycle > 255.5) rawDutyCycle = 255.5;
+  if (rawDutyCycle > 255.5) {
+    rawDutyCycle = 255.5;
+  }
   {
     std::scoped_lock lock(digitalPwmMutex);
     uint16_t pwmPeriodPower = digitalSystem->readPWMPeriodPower(status);
@@ -251,10 +266,35 @@
     return;
   }
   if (value != 0 && value != 1) {
-    if (value != 0) value = 1;
+    if (value != 0) {
+      value = 1;
+    }
   }
   {
     std::scoped_lock lock(digitalDIOMutex);
+
+    tDIO::tOutputEnable currentOutputEnable =
+        digitalSystem->readOutputEnable(status);
+
+    HAL_Bool isInput = false;
+
+    if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
+      isInput =
+          ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
+           1) == 0;
+    } else if (port->channel < kNumDigitalHeaders) {
+      isInput = ((currentOutputEnable.Headers >> port->channel) & 1) == 0;
+    } else {
+      isInput = ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) &
+                 1) == 0;
+    }
+
+    if (isInput) {
+      *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, "Cannot set output of an input channel");
+      return;
+    }
+
     tDIO::tDO currentDIO = digitalSystem->readDO(status);
 
     if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
@@ -358,11 +398,11 @@
 
   if (port->channel >= kNumDigitalHeaders + kNumDigitalMXPChannels) {
     return ((currentOutputEnable.SPIPort >> remapSPIChannel(port->channel)) &
-            1) != 0;
+            1) == 0;
   } else if (port->channel < kNumDigitalHeaders) {
-    return ((currentOutputEnable.Headers >> port->channel) & 1) != 0;
+    return ((currentOutputEnable.Headers >> port->channel) & 1) == 0;
   } else {
-    return ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) & 1) !=
+    return ((currentOutputEnable.MXP >> remapMXPChannel(port->channel)) & 1) ==
            0;
   }
 }
@@ -410,7 +450,9 @@
 
 HAL_Bool HAL_IsAnyPulsing(int32_t* status) {
   initializeDigital(status);
-  if (*status != 0) return false;
+  if (*status != 0) {
+    return false;
+  }
   tDIO::tPulse pulseRegister = digitalSystem->readPulse(status);
   return pulseRegister.Headers != 0 && pulseRegister.MXP != 0 &&
          pulseRegister.SPIPort != 0;
@@ -459,7 +501,9 @@
 
 void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status) {
   initializeDigital(status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   std::scoped_lock lock(digitalDIOMutex);
   digitalSystem->writeFilterPeriodHdr(filterIndex, value, status);
   if (*status == 0) {
@@ -469,7 +513,9 @@
 
 int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status) {
   initializeDigital(status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   uint32_t hdrPeriod = 0;
   uint32_t mxpPeriod = 0;
   {
diff --git a/third_party/allwpilib/hal/src/main/native/athena/DMA.cpp b/third_party/allwpilib/hal/src/main/native/athena/DMA.cpp
index 19c3068..8fa47e7 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/DMA.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/DMA.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DMA.h"
 
@@ -14,8 +11,10 @@
 #include <type_traits>
 
 #include "AnalogInternal.h"
+#include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "EncoderInternal.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
 //#include "hal/AnalogGyro.h"
@@ -73,15 +72,13 @@
 static hal::LimitedHandleResource<HAL_DMAHandle, DMA, 1, HAL_HandleEnum::DMA>*
     dmaHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDMA() {
   static hal::LimitedHandleResource<HAL_DMAHandle, DMA, 1, HAL_HandleEnum::DMA>
       dH;
   dmaHandles = &dH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -107,19 +104,25 @@
     return HAL_kInvalidHandle;
   }
 
-  dma->aDMA->writeConfig_ExternalClock(false, status);
-  if (*status != 0) {
-    dmaHandles->Free(handle);
-    return HAL_kInvalidHandle;
+  std::memset(&dma->captureStore, 0, sizeof(dma->captureStore));
+
+  tDMA::tConfig config;
+  std::memset(&config, 0, sizeof(config));
+  config.Pause = true;
+  dma->aDMA->writeConfig(config, status);
+
+  dma->aDMA->writeRate(1, status);
+
+  tDMA::tExternalTriggers newTrigger;
+  std::memset(&newTrigger, 0, sizeof(newTrigger));
+  for (unsigned char reg = 0; reg < tDMA::kNumExternalTriggersRegisters;
+       reg++) {
+    for (unsigned char bit = 0; bit < tDMA::kNumExternalTriggersElements;
+         bit++) {
+      dma->aDMA->writeExternalTriggers(reg, bit, newTrigger, status);
+    }
   }
 
-  HAL_SetDMARate(handle, 1, status);
-  if (*status != 0) {
-    dmaHandles->Free(handle);
-    return HAL_kInvalidHandle;
-  }
-
-  HAL_SetDMAPause(handle, false, status);
   return handle;
 }
 
@@ -127,7 +130,8 @@
   auto dma = dmaHandles->Get(handle);
   dmaHandles->Free(handle);
 
-  if (!dma) return;
+  if (!dma)
+    return;
 
   int32_t status = 0;
   if (dma->manager) {
@@ -142,20 +146,44 @@
     return;
   }
 
+  if (!dma->manager) {
+    *status = HAL_INVALID_DMA_STATE;
+    return;
+  }
+
   dma->aDMA->writeConfig_Pause(pause, status);
 }
-void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) {
+
+void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double seconds,
+                            int32_t* status) {
+  constexpr double baseMultipler = kSystemClockTicksPerMicrosecond * 1000000;
+  uint32_t cycles = static_cast<uint32_t>(baseMultipler * seconds);
+  HAL_SetDMATimedTriggerCycles(handle, cycles, status);
+}
+
+void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles,
+                                  int32_t* status) {
   auto dma = dmaHandles->Get(handle);
   if (!dma) {
     *status = HAL_HANDLE_ERROR;
     return;
   }
 
+  if (dma->manager) {
+    *status = HAL_INVALID_DMA_ADDITION;
+    return;
+  }
+
   if (cycles < 1) {
     cycles = 1;
   }
 
-  dma->aDMA->writeRate(static_cast<uint32_t>(cycles), status);
+  dma->aDMA->writeConfig_ExternalClock(false, status);
+  if (*status != 0) {
+    return;
+  }
+
+  dma->aDMA->writeRate(cycles, status);
 }
 
 void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
@@ -480,20 +508,20 @@
   }
 }
 
-void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
-                               HAL_Handle digitalSourceHandle,
-                               HAL_AnalogTriggerType analogTriggerType,
-                               HAL_Bool rising, HAL_Bool falling,
-                               int32_t* status) {
+int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+                                  HAL_Handle digitalSourceHandle,
+                                  HAL_AnalogTriggerType analogTriggerType,
+                                  HAL_Bool rising, HAL_Bool falling,
+                                  int32_t* status) {
   auto dma = dmaHandles->Get(handle);
   if (!dma) {
     *status = HAL_HANDLE_ERROR;
-    return;
+    return 0;
   }
 
   if (dma->manager) {
     *status = HAL_INVALID_DMA_ADDITION;
-    return;
+    return 0;
   }
 
   int index = 0;
@@ -507,19 +535,21 @@
 
   if (index == 8) {
     *status = NO_AVAILABLE_RESOURCES;
-    return;
+    return 0;
   }
 
   dma->captureStore.triggerChannels |= (1 << index);
 
   auto channelIndex = index;
 
-  auto isExternalClock = dma->aDMA->readConfig_ExternalClock(status);
-  if (*status == 0 && !isExternalClock) {
-    dma->aDMA->writeConfig_ExternalClock(true, status);
-    if (*status != 0) return;
-  } else if (*status != 0) {
-    return;
+  dma->aDMA->writeConfig_ExternalClock(true, status);
+  if (*status != 0) {
+    return 0;
+  }
+
+  dma->aDMA->writeRate(1, status);
+  if (*status != 0) {
+    return 0;
   }
 
   uint8_t pin = 0;
@@ -530,7 +560,10 @@
 
   if (!success) {
     *status = PARAMETER_OUT_OF_RANGE;
-    return;
+    hal::SetLastError(status,
+                      "Digital Source unabled to be mapped properly. Likely "
+                      "invalid handle passed.");
+    return 0;
   }
 
   tDMA::tExternalTriggers newTrigger;
@@ -542,6 +575,55 @@
 
   dma->aDMA->writeExternalTriggers(channelIndex / 4, channelIndex % 4,
                                    newTrigger, status);
+  return index;
+}
+
+void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status) {
+  auto dma = dmaHandles->Get(handle);
+  if (!dma) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (dma->manager) {
+    *status = HAL_INVALID_DMA_STATE;
+    return;
+  }
+
+  bool existingExternal = dma->aDMA->readConfig_ExternalClock(status);
+  if (*status != 0) {
+    return;
+  }
+
+  tDMA::tConfig config;
+  std::memset(&config, 0, sizeof(config));
+  config.Pause = true;
+  config.ExternalClock = existingExternal;
+  dma->aDMA->writeConfig(config, status);
+}
+
+void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status) {
+  auto dma = dmaHandles->Get(handle);
+  if (!dma) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (dma->manager) {
+    *status = HAL_INVALID_DMA_STATE;
+    return;
+  }
+
+  dma->captureStore.triggerChannels = 0;
+  tDMA::tExternalTriggers newTrigger;
+  std::memset(&newTrigger, 0, sizeof(newTrigger));
+  for (unsigned char reg = 0; reg < tDMA::kNumExternalTriggersRegisters;
+       reg++) {
+    for (unsigned char bit = 0; bit < tDMA::kNumExternalTriggersElements;
+         bit++) {
+      dma->aDMA->writeExternalTriggers(reg, bit, newTrigger, status);
+    }
+  }
 }
 
 void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {
@@ -552,12 +634,14 @@
   }
 
   if (dma->manager) {
-    *status = INCOMPATIBLE_STATE;
+    *status = HAL_INVALID_DMA_STATE;
     return;
   }
 
   tDMA::tConfig config = dma->aDMA->readConfig(status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 
   {
     size_t accum_size = 0;
@@ -594,12 +678,15 @@
     dma->captureStore.captureSize = accum_size + 1;
   }
 
-  dma->manager = std::make_unique<tDMAManager>(
-      g_DMA_index, queueDepth * dma->captureStore.captureSize, status);
+  uint32_t byteDepth = queueDepth * dma->captureStore.captureSize;
+
+  dma->manager = std::make_unique<tDMAManager>(g_DMA_index, byteDepth, status);
   if (*status != 0) {
     return;
   }
 
+  dma->aDMA->writeConfig_Pause(false, status);
+
   dma->manager->start(status);
   dma->manager->stop(status);
   dma->manager->start(status);
@@ -613,6 +700,8 @@
   }
 
   if (dma->manager) {
+    dma->aDMA->writeConfig_Pause(true, status);
+    *status = 0;
     dma->manager->stop(status);
     dma->manager = nullptr;
   }
@@ -625,7 +714,7 @@
 
 enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
                                          HAL_DMASample* dmaSample,
-                                         int32_t timeoutMs,
+                                         double timeoutSeconds,
                                          int32_t* remainingOut,
                                          int32_t* status) {
   DMA* dma = static_cast<DMA*>(dmaPointer);
@@ -633,12 +722,13 @@
   size_t remainingBytes = 0;
 
   if (!dma->manager) {
-    *status = INCOMPATIBLE_STATE;
+    *status = HAL_INVALID_DMA_STATE;
     return HAL_DMA_ERROR;
   }
 
   dma->manager->read(dmaSample->readBuffer, dma->captureStore.captureSize,
-                     timeoutMs, &remainingBytes, status);
+                     static_cast<uint32_t>(timeoutSeconds * 1000),
+                     &remainingBytes, status);
 
   *remainingOut = remainingBytes / dma->captureStore.captureSize;
 
@@ -663,15 +753,16 @@
 }
 
 enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
-                                   HAL_DMASample* dmaSample, int32_t timeoutMs,
-                                   int32_t* remainingOut, int32_t* status) {
+                                   HAL_DMASample* dmaSample,
+                                   double timeoutSeconds, int32_t* remainingOut,
+                                   int32_t* status) {
   auto dma = dmaHandles->Get(handle);
   if (!dma) {
     *status = HAL_HANDLE_ERROR;
     return HAL_DMA_ERROR;
   }
 
-  return HAL_ReadDMADirect(dma.get(), dmaSample, timeoutMs, remainingOut,
+  return HAL_ReadDMADirect(dma.get(), dmaSample, timeoutSeconds, remainingOut,
                            status);
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.cpp b/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.cpp
index 205ce3e..63ea0aa 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "DigitalInternal.h"
 
 #include <atomic>
+#include <cmath>
 #include <thread>
 
 #include <FRC_NetworkCommunication/LoadOut.h>
@@ -46,8 +44,12 @@
 }  // namespace init
 
 namespace detail {
-wpi::mutex& UnsafeGetDIOMutex() { return digitalDIOMutex; }
-tDIO* UnsafeGetDigialSystem() { return digitalSystem.get(); }
+wpi::mutex& UnsafeGetDIOMutex() {
+  return digitalDIOMutex;
+}
+tDIO* UnsafeGetDigialSystem() {
+  return digitalSystem.get();
+}
 int32_t ComputeDigitalMask(HAL_DigitalHandle handle, int32_t* status) {
   auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
   if (port == nullptr) {
@@ -72,11 +74,15 @@
   static std::atomic_bool initialized{false};
   static wpi::mutex initializeMutex;
   // Initial check, as if it's true initialization has finished
-  if (initialized) return;
+  if (initialized) {
+    return;
+  }
 
   std::scoped_lock lock(initializeMutex);
   // Second check in case another thread was waiting
-  if (initialized) return;
+  if (initialized) {
+    return;
+  }
 
   digitalSystem.reset(tDIO::create(status));
 
@@ -92,7 +98,8 @@
 
   // Make sure that the 9403 IONode has had a chance to initialize before
   // continuing.
-  while (pwmSystem->readLoopTiming(status) == 0) std::this_thread::yield();
+  while (pwmSystem->readLoopTiming(status) == 0)
+    std::this_thread::yield();
 
   if (pwmSystem->readLoopTiming(status) != kExpectedLoopTiming) {
     *status = LOOP_TIMING_ERROR;  // NOTE: Doesn't display the error
@@ -102,10 +109,10 @@
   double loopTime = pwmSystem->readLoopTiming(status) /
                     (kSystemClockTicksPerMicrosecond * 1e3);
 
-  pwmSystem->writeConfig_Period(
-      static_cast<uint16_t>(kDefaultPwmPeriod / loopTime + 0.5), status);
-  uint16_t minHigh = static_cast<uint16_t>(
-      (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime + 0.5);
+  pwmSystem->writeConfig_Period(std::lround(kDefaultPwmPeriod / loopTime),
+                                status);
+  uint16_t minHigh = std::lround(
+      (kDefaultPwmCenter - kDefaultPwmStepsDown * loopTime) / loopTime);
   pwmSystem->writeConfig_MinHigh(minHigh, status);
   // Ensure that PWM output values are set to OFF
   for (uint8_t pwmIndex = 0; pwmIndex < kNumPWMChannels; pwmIndex++) {
@@ -158,12 +165,22 @@
     }
     analogTrigger = false;
     return true;
-  } else {
-    return false;
+  } else if (isHandleType(digitalSourceHandle, HAL_HandleEnum::PWM)) {
+    // PWM's on MXP port are supported as a digital source
+    int32_t index = getHandleIndex(digitalSourceHandle);
+    if (index >= kNumPWMHeaders) {
+      channel = remapMXPPWMChannel(index);
+      module = 1;
+      analogTrigger = false;
+      return true;
+    }
   }
+  return false;
 }
 
-int32_t remapMXPChannel(int32_t channel) { return channel - 10; }
+int32_t remapMXPChannel(int32_t channel) {
+  return channel - 10;
+}
 
 int32_t remapMXPPWMChannel(int32_t channel) {
   if (channel < 14) {
@@ -173,7 +190,9 @@
   }
 }
 
-int32_t remapSPIChannel(int32_t channel) { return channel - 26; }
+int32_t remapSPIChannel(int32_t channel) {
+  return channel - 26;
+}
 
 }  // namespace hal
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.h b/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.h
index 2cb9b3c..6b1e909 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/DigitalInternal.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
 #include <memory>
+#include <string>
 
 #include <wpi/mutex.h>
 
@@ -73,6 +71,7 @@
   int32_t centerPwm = 0;
   int32_t deadbandMinPwm = 0;
   int32_t minPwm = 0;
+  std::string previousAllocation;
 };
 
 extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
@@ -101,13 +100,15 @@
  */
 constexpr int32_t remapDigitalChannelToBitfieldChannel(int32_t channel) {
   // First 10 are headers
-  if (channel < kNumDigitalHeaders) return channel;
-  // 2nd group of 16 are mxp. So if mxp port, add 6, since they start at 10
-  else if (channel < kNumDigitalMXPChannels)
+  if (channel < kNumDigitalHeaders) {
+    return channel;
+    // 2nd group of 16 are mxp. So if mxp port, add 6, since they start at 10
+  } else if (channel < kNumDigitalMXPChannels) {
     return channel + 6;
-  // Assume SPI, so remove MXP channels
-  else
+    // Assume SPI, so remove MXP channels
+  } else {
     return channel - kNumDigitalMXPChannels;
+  }
 }
 
 /**
diff --git a/third_party/allwpilib/hal/src/main/native/athena/DutyCycle.cpp b/third_party/allwpilib/hal/src/main/native/athena/DutyCycle.cpp
index 3749a2b..6d70570 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/DutyCycle.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/DutyCycle.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DutyCycle.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/DutyCycleInternal.h b/third_party/allwpilib/hal/src/main/native/athena/DutyCycleInternal.h
index 33a8ff2..ef49e25 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/DutyCycleInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/DutyCycleInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Encoder.cpp b/third_party/allwpilib/hal/src/main/native/athena/Encoder.cpp
index be8c203..6edfea3 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Encoder.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Encoder.cpp
@@ -1,15 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Encoder.h"
 
+#include <fmt/format.h>
+
 #include "EncoderInternal.h"
 #include "FPGAEncoder.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/ChipObject.h"
 #include "hal/Counter.h"
@@ -49,6 +49,8 @@
     }
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, fmt::format("Encoding type {} invalid.",
+                                            static_cast<int>(encodingType)));
       return;
   }
 }
@@ -63,15 +65,23 @@
   m_encodingScale = encodingType == HAL_Encoder_k1X ? 1 : 2;
   m_counter =
       HAL_InitializeCounter(HAL_Counter_kExternalDirection, &m_index, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   HAL_SetCounterMaxPeriod(m_counter, 0.5, status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   HAL_SetCounterUpSource(m_counter, digitalSourceHandleA, analogTriggerTypeA,
                          status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   HAL_SetCounterDownSource(m_counter, digitalSourceHandleB, analogTriggerTypeB,
                            status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   if (encodingType == HAL_Encoder_k1X) {
     HAL_SetCounterUpSourceEdge(m_counter, true, false, status);
     HAL_SetCounterAverageSize(m_counter, 1, status);
@@ -176,6 +186,9 @@
 void Encoder::SetSamplesToAverage(int32_t samplesToAverage, int32_t* status) {
   if (samplesToAverage < 1 || samplesToAverage > 127) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, fmt::format("Samples to average must be between "
+                                          "1 and 127 inclusive. Requested {}",
+                                          samplesToAverage));
     return;
   }
   if (m_counter) {
@@ -226,8 +239,7 @@
                                     kNumEncoders + kNumCounters,
                                     HAL_HandleEnum::Encoder>* encoderHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeEncoder() {
   static LimitedClassedHandleResource<HAL_EncoderHandle, Encoder,
                                       kNumEncoders + kNumCounters,
@@ -235,15 +247,16 @@
       eH;
   encoderHandles = &eH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 namespace hal {
 bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
                           HAL_FPGAEncoderHandle* fpgaHandle,
                           HAL_CounterHandle* counterHandle) {
   auto encoder = encoderHandles->Get(handle);
-  if (!handle) return false;
+  if (!encoder) {
+    return false;
+  }
 
   *fpgaHandle = encoder->m_encoder;
   *counterHandle = encoder->m_counter;
@@ -261,7 +274,9 @@
   auto encoder = std::make_shared<Encoder>(
       digitalSourceHandleA, analogTriggerTypeA, digitalSourceHandleB,
       analogTriggerTypeB, reverseDirection, encodingType, status);
-  if (*status != 0) return HAL_kInvalidHandle;  // return in creation error
+  if (*status != 0) {
+    return HAL_kInvalidHandle;  // return in creation error
+  }
   auto handle = encoderHandles->Allocate(encoder);
   if (handle == HAL_kInvalidHandle) {
     *status = NO_AVAILABLE_RESOURCES;
diff --git a/third_party/allwpilib/hal/src/main/native/athena/EncoderInternal.h b/third_party/allwpilib/hal/src/main/native/athena/EncoderInternal.h
index bed4ee3..5591a6f 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/EncoderInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/EncoderInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.cpp b/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.cpp
index 7f2e107..9965d43 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.cpp
@@ -1,16 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "FPGAEncoder.h"
 
 #include <memory>
 
+#include <fmt/format.h>
+
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
 
@@ -30,16 +30,14 @@
 static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
                              HAL_HandleEnum::FPGAEncoder>* fpgaEncoderHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeFPGAEncoder() {
   static LimitedHandleResource<HAL_FPGAEncoderHandle, Encoder, kNumEncoders,
                                HAL_HandleEnum::FPGAEncoder>
       feH;
   fpgaEncoderHandles = &feH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -198,6 +196,10 @@
   }
   if (samplesToAverage < 1 || samplesToAverage > 127) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, fmt::format("Samples to average must be between "
+                                          "1 and 127 inclusive. Requested {}",
+                                          samplesToAverage));
+    return;
   }
   encoder->encoder->writeTimerConfig_AverageSize(samplesToAverage, status);
 }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.h b/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.h
index f29aeae..b401ccd 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/FPGAEncoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp b/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp
index 51fe327..0f4b69b 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/FRCDriverStation.cpp
@@ -1,22 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <atomic>
 #include <chrono>
 #include <cstdlib>
 #include <cstring>
 #include <limits>
+#include <string>
+#include <string_view>
 
 #include <FRC_NetworkCommunication/FRCComm.h>
 #include <FRC_NetworkCommunication/NetCommRPCProxy_Occur.h>
+#include <fmt/format.h>
 #include <wpi/SafeThread.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 
 #include "hal/DriverStation.h"
 
@@ -70,15 +69,15 @@
       joystickNum, &buttons->buttons, &buttons->count);
 }
 /**
- * Retrieve the Joystick Descriptor for particular slot
- * @param desc [out] descriptor (data transfer object) to fill in.  desc is
- * filled in regardless of success. In other words, if descriptor is not
- * available, desc is filled in with default values matching the init-values in
- * Java and C++ Driverstation for when caller requests a too-large joystick
- * index.
+ * Retrieve the Joystick Descriptor for particular slot.
  *
+ * @param[out] desc descriptor (data transfer object) to fill in. desc is filled
+ *                  in regardless of success. In other words, if descriptor is
+ *                  not available, desc is filled in with default values
+ *                  matching the init-values in Java and C++ Driverstation for
+ *                  when caller requests a too-large joystick index.
  * @return error code reported from Network Comm back-end.  Zero is good,
- * nonzero is bad.
+ *         nonzero is bad.
  */
 static int32_t HAL_GetJoystickDescriptorInternal(int32_t joystickNum,
                                                  HAL_JoystickDescriptor* desc) {
@@ -126,16 +125,14 @@
 static wpi::condition_variable* newDSDataAvailableCond;
 static int newDSDataAvailableCounter{0};
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeFRCDriverStation() {
   static wpi::mutex newMutex;
   newDSDataAvailableMutex = &newMutex;
   static wpi::condition_variable newCond;
   newDSDataAvailableCond = &newCond;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -161,13 +158,15 @@
   auto curTime = std::chrono::steady_clock::now();
   int i;
   for (i = 0; i < KEEP_MSGS; ++i) {
-    if (prevMsg[i] == details) break;
+    if (prevMsg[i] == details) {
+      break;
+    }
   }
   int retval = 0;
   if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
-    wpi::StringRef detailsRef{details};
-    wpi::StringRef locationRef{location};
-    wpi::StringRef callStackRef{callStack};
+    std::string_view detailsRef{details};
+    std::string_view locationRef{location};
+    std::string_view callStackRef{callStack};
 
     // 1 tag, 4 timestamp, 2 seqnum
     // 2 numOccur, 4 error code, 1 flags, 6 strlen
@@ -203,14 +202,16 @@
                                                   newCallStack.c_str());
     }
     if (printMsg) {
+      fmt::memory_buffer buf;
       if (location && location[0] != '\0') {
-        wpi::errs() << (isError ? "Error" : "Warning") << " at " << location
-                    << ": ";
+        fmt::format_to(fmt::appender{buf},
+                       "{} at {}: ", isError ? "Error" : "Warning", location);
       }
-      wpi::errs() << details << "\n";
+      fmt::format_to(fmt::appender{buf}, "{}\n", details);
       if (callStack && callStack[0] != '\0') {
-        wpi::errs() << callStack << "\n";
+        fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
       }
+      std::fwrite(buf.data(), buf.size(), 1, stderr);
     }
     if (i == KEEP_MSGS) {
       // replace the oldest one
@@ -230,7 +231,7 @@
 }
 
 int32_t HAL_SendConsoleLine(const char* line) {
-  wpi::StringRef lineRef{line};
+  std::string_view lineRef{line};
   if (lineRef.size() <= 65535) {
     // Send directly
     return FRC_NetworkCommunication_sendConsoleLine(line);
@@ -299,15 +300,16 @@
     name[0] = '\0';
     return name;
   } else {
-    size_t len = std::strlen(joystickDesc.name);
-    char* name = static_cast<char*>(std::malloc(len + 1));
-    std::strncpy(name, joystickDesc.name, len);
-    name[len] = '\0';
+    const size_t len = std::strlen(joystickDesc.name) + 1;
+    char* name = static_cast<char*>(std::malloc(len));
+    std::memcpy(name, joystickDesc.name, len);
     return name;
   }
 }
 
-void HAL_FreeJoystickName(char* name) { std::free(name); }
+void HAL_FreeJoystickName(char* name) {
+  std::free(name);
+}
 
 int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
   HAL_JoystickDescriptor joystickDesc;
@@ -363,21 +365,17 @@
   std::scoped_lock lock{*newDSDataAvailableMutex};
   int& lastCount = GetThreadLocalLastCount();
   int currentCount = newDSDataAvailableCounter;
-  if (lastCount == currentCount) return false;
+  if (lastCount == currentCount) {
+    return false;
+  }
   lastCount = currentCount;
   return true;
 }
 
-/**
- * Waits for the newest DS packet to arrive. Note that this is a blocking call.
- */
-void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
+void HAL_WaitForDSData(void) {
+  HAL_WaitForDSDataTimeout(0);
+}
 
-/**
- * Waits for the newest DS packet to arrive. If timeout is <= 0, this will wait
- * forever. Otherwise, it will wait until either a new packet, or the timeout
- * time has passed. Returns true on new data, false on timeout.
- */
 HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
   std::unique_lock lock{*newDSDataAvailableMutex};
   int& lastCount = GetThreadLocalLastCount();
@@ -409,7 +407,9 @@
 static void newDataOccur(uint32_t refNum) {
   // Since we could get other values, require our specific handle
   // to signal our threads
-  if (refNum != refNumber) return;
+  if (refNum != refNumber) {
+    return;
+  }
   std::scoped_lock lock{*newDSDataAvailableMutex};
   // Notify all threads
   ++newDSDataAvailableCounter;
@@ -425,11 +425,15 @@
   static std::atomic_bool initialized{false};
   static wpi::mutex initializeMutex;
   // Initial check, as if it's true initialization has finished
-  if (initialized) return;
+  if (initialized) {
+    return;
+  }
 
   std::scoped_lock lock(initializeMutex);
   // Second check in case another thread was waiting
-  if (initialized) return;
+  if (initialized) {
+    return;
+  }
 
   // Set up the occur function internally with NetComm
   NetCommRPCProxy_SetOccurFuncPointer(newDataOccur);
@@ -443,6 +447,8 @@
  * Releases the DS Mutex to allow proper shutdown of any threads that are
  * waiting on it.
  */
-void HAL_ReleaseDSMutex(void) { newDataOccur(refNumber); }
+void HAL_ReleaseDSMutex(void) {
+  newDataOccur(refNumber);
+}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp b/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp
index 71dd048..4cae1e7 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/HAL.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/HAL.h"
 
@@ -12,6 +9,7 @@
 #include <unistd.h>
 
 #include <atomic>
+#include <cstdio>
 #include <cstdlib>
 #include <fstream>
 #include <thread>
@@ -19,13 +17,12 @@
 #include <FRC_NetworkCommunication/FRCComm.h>
 #include <FRC_NetworkCommunication/LoadOut.h>
 #include <FRC_NetworkCommunication/UsageReporting.h>
+#include <fmt/format.h>
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/timestamp.h>
 
 #include "HALInitializer.h"
 #include "HALInternal.h"
-#include "ctre/ctre.h"
 #include "hal/ChipObject.h"
 #include "hal/DriverStation.h"
 #include "hal/Errors.h"
@@ -42,6 +39,8 @@
 namespace hal {
 namespace init {
 void InitializeHAL() {
+  InitializeCTREPCM();
+  InitializeREVPH();
   InitializeAddressableLED();
   InitializeAccelerometer();
   InitializeAnalogAccumulator();
@@ -51,7 +50,6 @@
   InitializeAnalogTrigger();
   InitializeCAN();
   InitializeCANAPI();
-  InitializeCompressor();
   InitializeConstants();
   InitializeCounter();
   InitializeDigitalInternal();
@@ -65,13 +63,12 @@
   InitializeInterrupts();
   InitializeMain();
   InitializeNotifier();
-  InitializePCMInternal();
-  InitializePDP();
+  InitializeCTREPDP();
+  InitializeREVPDH();
   InitializePorts();
   InitializePower();
   InitializePWM();
   InitializeRelay();
-  InitializeSolenoid();
   InitializeSPI();
   InitializeThreads();
 }
@@ -92,14 +89,20 @@
 
 HAL_PortHandle HAL_GetPort(int32_t channel) {
   // Dont allow a number that wouldn't fit in a uint8_t
-  if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
+  if (channel < 0 || channel >= 255) {
+    return HAL_kInvalidHandle;
+  }
   return createPortHandle(channel, 1);
 }
 
 HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) {
   // Dont allow a number that wouldn't fit in a uint8_t
-  if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
-  if (module < 0 || module >= 255) return HAL_kInvalidHandle;
+  if (channel < 0 || channel >= 255) {
+    return HAL_kInvalidHandle;
+  }
+  if (module < 0 || module >= 255) {
+    return HAL_kInvalidHandle;
+  }
   return createPortHandle(channel, module);
 }
 
@@ -107,18 +110,6 @@
   switch (code) {
     case 0:
       return "";
-    case CTR_RxTimeout:
-      return CTR_RxTimeout_MESSAGE;
-    case CTR_TxTimeout:
-      return CTR_TxTimeout_MESSAGE;
-    case CTR_InvalidParamValue:
-      return CTR_InvalidParamValue_MESSAGE;
-    case CTR_UnexpectedArbId:
-      return CTR_UnexpectedArbId_MESSAGE;
-    case CTR_TxFailed:
-      return CTR_TxFailed_MESSAGE;
-    case CTR_SigNotUpdated:
-      return CTR_SigNotUpdated_MESSAGE;
     case NiFpga_Status_FifoTimeout:
       return NiFpga_Status_FifoTimeout_MESSAGE;
     case NiFpga_Status_TransferAborted:
@@ -203,12 +194,26 @@
       return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
     case HAL_LED_CHANNEL_ERROR:
       return HAL_LED_CHANNEL_ERROR_MESSAGE;
+    case HAL_INVALID_DMA_STATE:
+      return HAL_INVALID_DMA_STATE_MESSAGE;
+    case HAL_INVALID_DMA_ADDITION:
+      return HAL_INVALID_DMA_ADDITION_MESSAGE;
+    case HAL_USE_LAST_ERROR:
+      return HAL_USE_LAST_ERROR_MESSAGE;
+    case HAL_CONSOLE_OUT_ENABLED_ERROR:
+      return HAL_CONSOLE_OUT_ENABLED_ERROR_MESSAGE;
     default:
       return "Unknown error status";
   }
 }
 
-HAL_RuntimeType HAL_GetRuntimeType(void) { return HAL_Athena; }
+HAL_RuntimeType HAL_GetRuntimeType(void) {
+  nLoadOut::tTargetClass targetClass = nLoadOut::getTargetClass();
+  if (targetClass == nLoadOut::kTargetClass_RoboRIO2) {
+    return HAL_Runtime_RoboRIO2;
+  }
+  return HAL_Runtime_RoboRIO;
+}
 
 int32_t HAL_GetFPGAVersion(int32_t* status) {
   if (!global) {
@@ -235,35 +240,41 @@
   uint64_t upper1 = global->readLocalTimeUpper(status);
   uint32_t lower = global->readLocalTime(status);
   uint64_t upper2 = global->readLocalTimeUpper(status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   if (upper1 != upper2) {
     // Rolled over between the lower call, reread lower
     lower = global->readLocalTime(status);
-    if (*status != 0) return 0;
+    if (*status != 0) {
+      return 0;
+    }
   }
   return (upper2 << 32) + lower;
 }
 
-uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) {
   // Capture the current FPGA time.  This will give us the upper half of the
   // clock.
-  uint64_t fpga_time = HAL_GetFPGATime(status);
-  if (*status != 0) return 0;
+  uint64_t fpgaTime = HAL_GetFPGATime(status);
+  if (*status != 0) {
+    return 0;
+  }
 
   // Now, we need to detect the case where the lower bits rolled over after we
   // sampled.  In that case, the upper bits will be 1 bigger than they should
   // be.
 
   // Break it into lower and upper portions.
-  uint32_t lower = fpga_time & 0xffffffffull;
-  uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+  uint32_t lower = fpgaTime & 0xffffffffull;
+  uint64_t upper = (fpgaTime >> 32) & 0xffffffff;
 
   // The time was sampled *before* the current time, so roll it back.
-  if (lower < unexpanded_lower) {
+  if (lower < unexpandedLower) {
     --upper;
   }
 
-  return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+  return (upper << 32) + static_cast<uint64_t>(unexpandedLower);
 }
 
 HAL_Bool HAL_GetFPGAButton(int32_t* status) {
@@ -295,7 +306,9 @@
   std::fstream fs;
   // By making this both in/out, it won't give us an error if it doesnt exist
   fs.open("/var/lock/frc.pid", std::fstream::in | std::fstream::out);
-  if (fs.bad()) return false;
+  if (fs.bad()) {
+    return false;
+  }
 
   pid_t pid = 0;
   if (!fs.eof() && !fs.fail()) {
@@ -303,18 +316,19 @@
     // see if the pid is around, but we don't want to mess with init id=1, or
     // ourselves
     if (pid >= 2 && kill(pid, 0) == 0 && pid != getpid()) {
-      wpi::outs() << "Killing previously running FRC program...\n";
+      std::puts("Killing previously running FRC program...");
       kill(pid, SIGTERM);  // try to kill it
       std::this_thread::sleep_for(std::chrono::milliseconds(timeout));
       if (kill(pid, 0) == 0) {
         // still not successful
-        wpi::outs() << "FRC pid " << pid << " did not die within " << timeout
-                    << "ms. Force killing with kill -9\n";
+        fmt::print(
+            "FRC pid {} did not die within {} ms. Force killing with kill -9\n",
+            pid, timeout);
         // Force kill -9
         auto forceKill = kill(pid, SIGKILL);
         if (forceKill != 0) {
           auto errorMsg = std::strerror(forceKill);
-          wpi::outs() << "Kill -9 error: " << errorMsg << "\n";
+          fmt::print("Kill -9 error: {}\n", errorMsg);
         }
         // Give a bit of time for the kill to take place
         std::this_thread::sleep_for(std::chrono::milliseconds(250));
@@ -335,11 +349,15 @@
   static std::atomic_bool initialized{false};
   static wpi::mutex initializeMutex;
   // Initial check, as if it's true initialization has finished
-  if (initialized) return true;
+  if (initialized) {
+    return true;
+  }
 
   std::scoped_lock lock(initializeMutex);
   // Second check in case another thread was waiting
-  if (initialized) return true;
+  if (initialized) {
+    return true;
+  }
 
   hal::init::InitializeHAL();
 
@@ -347,7 +365,6 @@
 
   setlinebuf(stdin);
   setlinebuf(stdout);
-  wpi::outs().SetUnbuffered();
 
   prctl(PR_SET_PDEATHSIG, SIGTERM);
 
@@ -370,7 +387,9 @@
   global.reset(tGlobal::create(&status));
   watchdog.reset(tSysWatchdog::create(&status));
 
-  if (status != 0) return false;
+  if (status != 0) {
+    return false;
+  }
 
   HAL_InitializeDriverStation();
 
@@ -379,11 +398,11 @@
     int32_t status = 0;
     uint64_t rv = HAL_GetFPGATime(&status);
     if (status != 0) {
-      wpi::errs()
-          << "Call to HAL_GetFPGATime failed in wpi::Now() with status "
-          << status
-          << ". Initialization might have failed. Time will not be correct\n";
-      wpi::errs().flush();
+      fmt::print(stderr,
+                 "Call to HAL_GetFPGATime failed in wpi::Now() with status {}. "
+                 "Initialization might have failed. Time will not be correct\n",
+                 status);
+      std::fflush(stderr);
       return 0u;
     }
     return rv;
@@ -395,6 +414,10 @@
 
 void HAL_Shutdown(void) {}
 
+void HAL_SimPeriodicBefore(void) {}
+
+void HAL_SimPeriodicAfter(void) {}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   if (feature == nullptr) {
@@ -405,10 +428,4 @@
       resource, instanceNumber, context, feature);
 }
 
-// TODO: HACKS
-// No need for header definitions, as we should not run from user code.
-void NumericArrayResize(void) {}
-void RTSetCleanupProc(void) {}
-void EDVR_CreateReference(void) {}
-
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.cpp b/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.cpp
index 5c2242b..50cc9ab 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALInitializer.h"
 
 #include "hal/HALBase.h"
 
-namespace hal {
-namespace init {
+namespace hal::init {
 std::atomic_bool HAL_IsInitialized{false};
-void RunInitialize() { HAL_Initialize(500, 0); }
-}  // namespace init
-}  // namespace hal
+void RunInitialize() {
+  HAL_Initialize(500, 0);
+}
+}  // namespace hal::init
diff --git a/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h b/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h
index e27ee31..54e1a92 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/HALInitializer.h
@@ -1,23 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <atomic>
 
-namespace hal {
-namespace init {
+namespace hal::init {
 extern std::atomic_bool HAL_IsInitialized;
 extern void RunInitialize();
-static inline void CheckInit() {
-  if (HAL_IsInitialized.load(std::memory_order_relaxed)) return;
+inline void CheckInit() {
+  if (HAL_IsInitialized.load(std::memory_order_relaxed)) {
+    return;
+  }
   RunInitialize();
 }
 
+extern void InitializeCTREPCM();
+extern void InitializeREVPH();
 extern void InitializeAccelerometer();
 extern void InitializeAddressableLED();
 extern void InitializeAnalogAccumulator();
@@ -27,7 +27,6 @@
 extern void InitializeAnalogTrigger();
 extern void InitializeCAN();
 extern void InitializeCANAPI();
-extern void InitializeCompressor();
 extern void InitializeConstants();
 extern void InitializeCounter();
 extern void InitializeDigitalInternal();
@@ -42,14 +41,13 @@
 extern void InitializeInterrupts();
 extern void InitializeMain();
 extern void InitializeNotifier();
-extern void InitializePCMInternal();
-extern void InitializePDP();
+extern void InitializeCTREPDP();
+extern void InitializeREVPDH();
 extern void InitializePorts();
 extern void InitializePower();
 extern void InitializePWM();
 extern void InitializeRelay();
-extern void InitializeSolenoid();
+extern void InitializeSerialPort();
 extern void InitializeSPI();
 extern void InitializeThreads();
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
diff --git a/third_party/allwpilib/hal/src/main/native/athena/HALInternal.h b/third_party/allwpilib/hal/src/main/native/athena/HALInternal.h
index e3033bf..64a0dca 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/HALInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/HALInternal.h
@@ -1,15 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
+#include <string_view>
+
 namespace hal {
 void ReleaseFPGAInterrupt(int32_t interruptNumber);
-
+void SetLastError(int32_t* status, std::string_view value);
+void SetLastErrorIndexOutOfRange(int32_t* status, std::string_view message,
+                                 int32_t minimum, int32_t maximum,
+                                 int32_t channel);
+void SetLastErrorPreviouslyAllocated(int32_t* status, std::string_view message,
+                                     int32_t channel,
+                                     std::string_view previousAllocation);
 }  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/athena/I2C.cpp b/third_party/allwpilib/hal/src/main/native/athena/I2C.cpp
index b72e25e..93e280e 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/I2C.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/I2C.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/I2C.h"
 
@@ -15,8 +12,11 @@
 
 #include <cstring>
 
+#include <fmt/format.h>
+
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "hal/DIO.h"
 #include "hal/HAL.h"
 
@@ -33,44 +33,50 @@
 static HAL_DigitalHandle i2CMXPDigitalHandle1{HAL_kInvalidHandle};
 static HAL_DigitalHandle i2CMXPDigitalHandle2{HAL_kInvalidHandle};
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeI2C() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) {
   hal::init::CheckInit();
   initializeDigital(status);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 
   if (port < 0 || port > 1) {
-    // Set port out of range error here
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for I2C", 0, 1,
+                                     port);
     return;
   }
 
   if (port == HAL_I2C_kOnboard) {
     std::scoped_lock lock(digitalI2COnBoardMutex);
     i2COnboardObjCount++;
-    if (i2COnboardObjCount > 1) return;
+    if (i2COnboardObjCount > 1) {
+      return;
+    }
     int handle = open("/dev/i2c-2", O_RDWR);
     if (handle < 0) {
-      std::printf("Failed to open onboard i2c bus: %s\n", std::strerror(errno));
+      fmt::print("Failed to open onboard i2c bus: {}\n", std::strerror(errno));
       return;
     }
     i2COnBoardHandle = handle;
   } else {
     std::scoped_lock lock(digitalI2CMXPMutex);
     i2CMXPObjCount++;
-    if (i2CMXPObjCount > 1) return;
+    if (i2CMXPObjCount > 1) {
+      return;
+    }
     if ((i2CMXPDigitalHandle1 = HAL_InitializeDIOPort(
-             HAL_GetPort(24), false, status)) == HAL_kInvalidHandle) {
+             HAL_GetPort(24), false, nullptr, status)) == HAL_kInvalidHandle) {
       return;
     }
     if ((i2CMXPDigitalHandle2 = HAL_InitializeDIOPort(
-             HAL_GetPort(25), false, status)) == HAL_kInvalidHandle) {
+             HAL_GetPort(25), false, nullptr, status)) == HAL_kInvalidHandle) {
       HAL_FreeDIOPort(i2CMXPDigitalHandle1);  // free the first port allocated
       return;
     }
@@ -78,7 +84,7 @@
         digitalSystem->readEnableMXPSpecialFunction(status) | 0xC000, status);
     int handle = open("/dev/i2c-1", O_RDWR);
     if (handle < 0) {
-      std::printf("Failed to open MXP i2c bus: %s\n", std::strerror(errno));
+      fmt::print("Failed to open MXP i2c bus: {}\n", std::strerror(errno));
       return;
     }
     i2CMXPHandle = handle;
@@ -89,7 +95,9 @@
                            const uint8_t* dataToSend, int32_t sendSize,
                            uint8_t* dataReceived, int32_t receiveSize) {
   if (port < 0 || port > 1) {
-    // Set port out of range error here
+    int32_t status = 0;
+    hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
+                                     port);
     return -1;
   }
 
@@ -119,7 +127,9 @@
 int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress,
                      const uint8_t* dataToSend, int32_t sendSize) {
   if (port < 0 || port > 1) {
-    // Set port out of range error here
+    int32_t status = 0;
+    hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
+                                     port);
     return -1;
   }
 
@@ -145,7 +155,9 @@
 int32_t HAL_ReadI2C(HAL_I2CPort port, int32_t deviceAddress, uint8_t* buffer,
                     int32_t count) {
   if (port < 0 || port > 1) {
-    // Set port out of range error here
+    int32_t status = 0;
+    hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
+                                     port);
     return -1;
   }
 
@@ -170,7 +182,9 @@
 
 void HAL_CloseI2C(HAL_I2CPort port) {
   if (port < 0 || port > 1) {
-    // Set port out of range error here
+    int32_t status = 0;
+    hal::SetLastErrorIndexOutOfRange(&status, "Invalid Index for I2C", 0, 1,
+                                     port);
     return;
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Interrupts.cpp b/third_party/allwpilib/hal/src/main/native/athena/Interrupts.cpp
index a330e3c..943a1aa 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Interrupts.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Interrupts.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Interrupts.h"
 
@@ -24,79 +21,29 @@
 using namespace hal;
 
 namespace {
-// Safe thread to allow callbacks to run on their own thread
-class InterruptThread : public wpi::SafeThread {
- public:
-  void Main() {
-    std::unique_lock lock(m_mutex);
-    while (m_active) {
-      m_cond.wait(lock, [&] { return !m_active || m_notify; });
-      if (!m_active) break;
-      m_notify = false;
-      HAL_InterruptHandlerFunction handler = m_handler;
-      uint32_t mask = m_mask;
-      void* param = m_param;
-      lock.unlock();  // don't hold mutex during callback execution
-      handler(mask, param);
-      lock.lock();
-    }
-  }
-
-  bool m_notify = false;
-  HAL_InterruptHandlerFunction m_handler;
-  void* m_param;
-  uint32_t m_mask;
-};
-
-class InterruptThreadOwner : public wpi::SafeThreadOwner<InterruptThread> {
- public:
-  void SetFunc(HAL_InterruptHandlerFunction handler, void* param) {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_handler = handler;
-    thr->m_param = param;
-  }
-
-  void Notify(uint32_t mask) {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_mask = mask;
-    thr->m_notify = true;
-    thr->m_cond.notify_one();
-  }
-};
 
 struct Interrupt {
   std::unique_ptr<tInterrupt> anInterrupt;
   std::unique_ptr<tInterruptManager> manager;
-  std::unique_ptr<InterruptThreadOwner> threadOwner = nullptr;
-  void* param = nullptr;
 };
 
 }  // namespace
 
-static void threadedInterruptHandler(uint32_t mask, void* param) {
-  static_cast<InterruptThreadOwner*>(param)->Notify(mask);
-}
-
 static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
                              HAL_HandleEnum::Interrupt>* interruptHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeInterrupts() {
   static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
                                HAL_HandleEnum::Interrupt>
       iH;
   interruptHandles = &iH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
-HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher,
-                                             int32_t* status) {
+HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status) {
   hal::init::CheckInit();
   HAL_InterruptHandle handle = interruptHandles->Allocate();
   if (handle == HAL_kInvalidHandle) {
@@ -109,24 +56,16 @@
   anInterrupt->anInterrupt.reset(tInterrupt::create(interruptIndex, status));
   anInterrupt->anInterrupt->writeConfig_WaitForAck(false, status);
   anInterrupt->manager = std::make_unique<tInterruptManager>(
-      (1u << interruptIndex) | (1u << (interruptIndex + 8u)), watcher, status);
+      (1u << interruptIndex) | (1u << (interruptIndex + 8u)), true, status);
   return handle;
 }
 
-void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle,
-                          int32_t* status) {
+void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle) {
   auto anInterrupt = interruptHandles->Get(interruptHandle);
   interruptHandles->Free(interruptHandle);
   if (anInterrupt == nullptr) {
-    return nullptr;
+    return;
   }
-
-  if (anInterrupt->manager->isEnabled(status)) {
-    anInterrupt->manager->disable(status);
-  }
-
-  void* param = anInterrupt->param;
-  return param;
 }
 
 int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
@@ -151,31 +90,6 @@
   return result;
 }
 
-void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle,
-                          int32_t* status) {
-  auto anInterrupt = interruptHandles->Get(interruptHandle);
-  if (anInterrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  if (!anInterrupt->manager->isEnabled(status)) {
-    anInterrupt->manager->enable(status);
-  }
-}
-
-void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
-                           int32_t* status) {
-  auto anInterrupt = interruptHandles->Get(interruptHandle);
-  if (anInterrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-  if (anInterrupt->manager->isEnabled(status)) {
-    anInterrupt->manager->disable(status);
-  }
-}
-
 int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
                                          int32_t* status) {
   auto anInterrupt = interruptHandles->Get(interruptHandle);
@@ -224,40 +138,6 @@
   anInterrupt->anInterrupt->writeConfig_Source_Module(routingModule, status);
 }
 
-void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle,
-                                HAL_InterruptHandlerFunction handler,
-                                void* param, int32_t* status) {
-  auto anInterrupt = interruptHandles->Get(interruptHandle);
-  if (anInterrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-  anInterrupt->manager->registerHandler(handler, param, status);
-  anInterrupt->param = param;
-}
-
-void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interrupt_handle,
-                                        HAL_InterruptHandlerFunction handler,
-                                        void* param, int32_t* status) {
-  auto anInterrupt = interruptHandles->Get(interrupt_handle);
-  if (anInterrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  anInterrupt->threadOwner = std::make_unique<InterruptThreadOwner>();
-  anInterrupt->threadOwner->Start();
-  anInterrupt->threadOwner->SetFunc(handler, param);
-
-  HAL_AttachInterruptHandler(interrupt_handle, threadedInterruptHandler,
-                             anInterrupt->threadOwner.get(), status);
-
-  if (*status != 0) {
-    anInterrupt->threadOwner = nullptr;
-  }
-  anInterrupt->param = param;
-}
-
 void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
                                   HAL_Bool risingEdge, HAL_Bool fallingEdge,
                                   int32_t* status) {
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Notifier.cpp b/third_party/allwpilib/hal/src/main/native/athena/Notifier.cpp
index 905c440..d684031 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Notifier.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Notifier.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Notifier.h"
 
@@ -12,6 +9,7 @@
 #include <memory>
 #include <thread>
 
+#include <fmt/core.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
 
@@ -20,6 +18,7 @@
 #include "hal/ChipObject.h"
 #include "hal/Errors.h"
 #include "hal/HAL.h"
+#include "hal/Threads.h"
 #include "hal/handles/UnlimitedHandleResource.h"
 
 using namespace hal;
@@ -29,6 +28,8 @@
 static wpi::mutex notifierMutex;
 static std::unique_ptr<tAlarm> notifierAlarm;
 static std::thread notifierThread;
+static HAL_Bool notifierThreadRealTime = false;
+static int32_t notifierThreadPriority = 0;
 static uint64_t closestTrigger{UINT64_MAX};
 
 namespace {
@@ -78,8 +79,12 @@
 
   // process all notifiers
   notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
-    if (notifier->triggerTime == UINT64_MAX) return;
-    if (currentTime == 0) currentTime = HAL_GetFPGATime(&status);
+    if (notifier->triggerTime == UINT64_MAX) {
+      return;
+    }
+    if (currentTime == 0) {
+      currentTime = HAL_GetFPGATime(&status);
+    }
     std::unique_lock lock(notifier->mutex);
     if (notifier->triggerTime < currentTime) {
       notifier->triggerTime = UINT64_MAX;
@@ -105,41 +110,61 @@
   tInterruptManager manager{1 << kTimerInterruptNumber, true, &status};
   while (notifierRunning) {
     auto triggeredMask = manager.watch(10000, false, &status);
-    if (!notifierRunning) break;
-    if (triggeredMask == 0) continue;
+    if (!notifierRunning) {
+      break;
+    }
+    if (triggeredMask == 0)
+      continue;
     alarmCallback();
   }
 }
 
 static void cleanupNotifierAtExit() {
   int32_t status = 0;
-  if (notifierAlarm) notifierAlarm->writeEnable(false, &status);
+  if (notifierAlarm)
+    notifierAlarm->writeEnable(false, &status);
   notifierAlarm = nullptr;
   notifierRunning = false;
   hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
-  if (notifierThread.joinable()) notifierThread.join();
+  if (notifierThread.joinable()) {
+    notifierThread.join();
+  }
 }
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeNotifier() {
   static NotifierHandleContainer nH;
   notifierHandles = &nH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status) {
   hal::init::CheckInit();
-  if (!notifierAtexitRegistered.test_and_set())
+  if (!notifierAtexitRegistered.test_and_set()) {
     std::atexit(cleanupNotifierAtExit);
+  }
 
   if (notifierRefCount.fetch_add(1) == 0) {
     std::scoped_lock lock(notifierMutex);
     notifierRunning = true;
     notifierThread = std::thread(notifierThreadMain);
+
+    auto native = notifierThread.native_handle();
+    HAL_SetThreadPriority(&native, notifierThreadRealTime,
+                          notifierThreadPriority, status);
+    if (*status == HAL_THREAD_PRIORITY_ERROR) {
+      *status = 0;
+      fmt::print("{}: HAL Notifier thread\n",
+                 HAL_THREAD_PRIORITY_ERROR_MESSAGE);
+    }
+    if (*status == HAL_THREAD_PRIORITY_RANGE_ERROR) {
+      *status = 0;
+      fmt::print("{}: HAL Notifier thread\n",
+                 HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE);
+    }
+
     notifierAlarm.reset(tAlarm::create(status));
   }
 
@@ -152,12 +177,26 @@
   return handle;
 }
 
+HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
+                                       int32_t* status) {
+  std::scoped_lock lock(notifierMutex);
+  notifierThreadRealTime = realTime;
+  notifierThreadPriority = priority;
+  if (notifierThread.joinable()) {
+    auto native = notifierThread.native_handle();
+    return HAL_SetThreadPriority(&native, realTime, priority, status);
+  } else {
+    return true;
+  }
+}
+
 void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
                          int32_t* status) {}
 
 void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier)
+    return;
 
   {
     std::scoped_lock lock(notifier->mutex);
@@ -170,7 +209,8 @@
 
 void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   auto notifier = notifierHandles->Free(notifierHandle);
-  if (!notifier) return;
+  if (!notifier)
+    return;
 
   // Just in case HAL_StopNotifier() wasn't called...
   {
@@ -187,10 +227,13 @@
     // here (the atomic fetch_sub will prevent multiple parallel entries
     // into this function)
 
-    if (notifierAlarm) notifierAlarm->writeEnable(false, status);
+    if (notifierAlarm)
+      notifierAlarm->writeEnable(false, status);
     notifierRunning = false;
     hal::ReleaseFPGAInterrupt(kTimerInterruptNumber);
-    if (notifierThread.joinable()) notifierThread.join();
+    if (notifierThread.joinable()) {
+      notifierThread.join();
+    }
 
     std::scoped_lock lock(notifierMutex);
     notifierAlarm = nullptr;
@@ -201,7 +244,8 @@
 void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              uint64_t triggerTime, int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier)
+    return;
 
   {
     std::scoped_lock lock(notifier->mutex);
@@ -218,14 +262,16 @@
     notifierAlarm->writeTriggerTime(static_cast<uint32_t>(closestTrigger),
                                     status);
     // Enable the alarm.
-    if (!wasActive) notifierAlarm->writeEnable(true, status);
+    if (!wasActive)
+      notifierAlarm->writeEnable(true, status);
   }
 }
 
 void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier)
+    return;
 
   {
     std::scoped_lock lock(notifier->mutex);
@@ -236,7 +282,8 @@
 uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
                                   int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return 0;
+  if (!notifier)
+    return 0;
   std::unique_lock lock(notifier->mutex);
   notifier->cond.wait(lock, [&] {
     return !notifier->active || notifier->triggeredTime != UINT64_MAX;
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PCMInternal.cpp b/third_party/allwpilib/hal/src/main/native/athena/PCMInternal.cpp
deleted file mode 100644
index c81dc1a..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/PCMInternal.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "PCMInternal.h"
-
-#include "HALInitializer.h"
-#include "hal/Errors.h"
-#include "hal/Solenoid.h"
-
-namespace hal {
-
-std::unique_ptr<PCM> PCM_modules[kNumPCMModules];
-
-namespace init {
-void InitializePCMInternal() {
-  for (int i = 0; i < kNumPCMModules; i++) {
-    PCM_modules[i] = nullptr;
-  }
-}
-}  // namespace init
-
-void initializePCM(int32_t module, int32_t* status) {
-  hal::init::CheckInit();
-  if (!HAL_CheckSolenoidModule(module)) {
-    *status = RESOURCE_OUT_OF_RANGE;
-    return;
-  }
-  if (!PCM_modules[module]) {
-    PCM_modules[module] = std::make_unique<PCM>(module);
-  }
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PCMInternal.h b/third_party/allwpilib/hal/src/main/native/athena/PCMInternal.h
deleted file mode 100644
index a9d076c..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/PCMInternal.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <stdint.h>
-
-#include <memory>
-
-#include "PortsInternal.h"
-#include "ctre/PCM.h"
-#include "hal/Errors.h"
-#include "hal/Solenoid.h"
-
-namespace hal {
-
-extern std::unique_ptr<PCM> PCM_modules[kNumPCMModules];
-
-static inline bool checkPCMInit(int32_t module, int32_t* status) {
-  if (!HAL_CheckSolenoidModule(module)) {
-    *status = RESOURCE_OUT_OF_RANGE;
-    return false;
-  }
-  if (!PCM_modules[module]) {
-    *status = INCOMPATIBLE_STATE;
-    return false;
-  }
-  return true;
-}
-
-void initializePCM(int32_t module, int32_t* status);
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PDP.cpp b/third_party/allwpilib/hal/src/main/native/athena/PDP.cpp
deleted file mode 100644
index f60e881..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/PDP.cpp
+++ /dev/null
@@ -1,450 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/PDP.h"
-
-#include <wpi/mutex.h>
-
-#include "HALInitializer.h"
-#include "PortsInternal.h"
-#include "hal/CANAPI.h"
-#include "hal/Errors.h"
-
-using namespace hal;
-
-static constexpr HAL_CANManufacturer manufacturer =
-    HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
-
-static constexpr HAL_CANDeviceType deviceType =
-    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
-
-static constexpr int32_t Status1 = 0x50;
-static constexpr int32_t Status2 = 0x51;
-static constexpr int32_t Status3 = 0x52;
-static constexpr int32_t StatusEnergy = 0x5D;
-
-static constexpr int32_t Control1 = 0x70;
-
-static constexpr int32_t TimeoutMs = 100;
-
-/* encoder/decoders */
-union PdpStatus1 {
-  uint8_t data[8];
-  struct Bits {
-    unsigned chan1_h8 : 8;
-    unsigned chan2_h6 : 6;
-    unsigned chan1_l2 : 2;
-    unsigned chan3_h4 : 4;
-    unsigned chan2_l4 : 4;
-    unsigned chan4_h2 : 2;
-    unsigned chan3_l6 : 6;
-    unsigned chan4_l8 : 8;
-    unsigned chan5_h8 : 8;
-    unsigned chan6_h6 : 6;
-    unsigned chan5_l2 : 2;
-    unsigned reserved4 : 4;
-    unsigned chan6_l4 : 4;
-  } bits;
-};
-
-union PdpStatus2 {
-  uint8_t data[8];
-  struct Bits {
-    unsigned chan7_h8 : 8;
-    unsigned chan8_h6 : 6;
-    unsigned chan7_l2 : 2;
-    unsigned chan9_h4 : 4;
-    unsigned chan8_l4 : 4;
-    unsigned chan10_h2 : 2;
-    unsigned chan9_l6 : 6;
-    unsigned chan10_l8 : 8;
-    unsigned chan11_h8 : 8;
-    unsigned chan12_h6 : 6;
-    unsigned chan11_l2 : 2;
-    unsigned reserved4 : 4;
-    unsigned chan12_l4 : 4;
-  } bits;
-};
-
-union PdpStatus3 {
-  uint8_t data[8];
-  struct Bits {
-    unsigned chan13_h8 : 8;
-    unsigned chan14_h6 : 6;
-    unsigned chan13_l2 : 2;
-    unsigned chan15_h4 : 4;
-    unsigned chan14_l4 : 4;
-    unsigned chan16_h2 : 2;
-    unsigned chan15_l6 : 6;
-    unsigned chan16_l8 : 8;
-    unsigned internalResBattery_mOhms : 8;
-    unsigned busVoltage : 8;
-    unsigned temp : 8;
-  } bits;
-};
-
-union PdpStatusEnergy {
-  uint8_t data[8];
-  struct Bits {
-    unsigned TmeasMs_likelywillbe20ms_ : 8;
-    unsigned TotalCurrent_125mAperunit_h8 : 8;
-    unsigned Power_125mWperunit_h4 : 4;
-    unsigned TotalCurrent_125mAperunit_l4 : 4;
-    unsigned Power_125mWperunit_m8 : 8;
-    unsigned Energy_125mWPerUnitXTmeas_h4 : 4;
-    unsigned Power_125mWperunit_l4 : 4;
-    unsigned Energy_125mWPerUnitXTmeas_mh8 : 8;
-    unsigned Energy_125mWPerUnitXTmeas_ml8 : 8;
-    unsigned Energy_125mWPerUnitXTmeas_l8 : 8;
-  } bits;
-};
-
-static wpi::mutex pdpHandleMutex;
-static HAL_PDPHandle pdpHandles[kNumPDPModules];
-
-namespace hal {
-namespace init {
-void InitializePDP() {
-  for (int i = 0; i < kNumPDPModules; i++) {
-    pdpHandles[i] = HAL_kInvalidHandle;
-  }
-}
-}  // namespace init
-}  // namespace hal
-
-extern "C" {
-
-HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
-  hal::init::CheckInit();
-  if (!HAL_CheckPDPModule(module)) {
-    *status = PARAMETER_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
-  }
-
-  std::scoped_lock lock(pdpHandleMutex);
-
-  if (pdpHandles[module] != HAL_kInvalidHandle) {
-    *status = 0;
-    return pdpHandles[module];
-  }
-
-  auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
-
-  if (*status != 0) {
-    HAL_CleanCAN(handle);
-    return HAL_kInvalidHandle;
-  }
-
-  pdpHandles[module] = handle;
-
-  return handle;
-}
-
-void HAL_CleanPDP(HAL_PDPHandle handle) {
-  HAL_CleanCAN(handle);
-
-  for (int i = 0; i < kNumPDPModules; i++) {
-    if (pdpHandles[i] == handle) {
-      pdpHandles[i] = HAL_kInvalidHandle;
-      return;
-    }
-  }
-}
-
-HAL_Bool HAL_CheckPDPModule(int32_t module) {
-  return module < kNumPDPModules && module >= 0;
-}
-
-HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
-  return channel < kNumPDPChannels && channel >= 0;
-}
-
-double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
-  PdpStatus3 pdpStatus;
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-
-  HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-
-  if (*status != 0) {
-    return 0;
-  } else {
-    return pdpStatus.bits.temp * 1.03250836957542 - 67.8564500484966;
-  }
-}
-
-double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
-  PdpStatus3 pdpStatus;
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-
-  HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-
-  if (*status != 0) {
-    return 0;
-  } else {
-    return pdpStatus.bits.busVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
-  }
-}
-
-double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
-                                int32_t* status) {
-  if (!HAL_CheckPDPChannel(channel)) {
-    *status = PARAMETER_OUT_OF_RANGE;
-    return 0;
-  }
-
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-
-  double raw = 0;
-
-  if (channel <= 5) {
-    PdpStatus1 pdpStatus;
-    HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
-                             &receivedTimestamp, TimeoutMs, status);
-    if (*status != 0) {
-      return 0;
-    }
-    switch (channel) {
-      case 0:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
-              pdpStatus.bits.chan1_l2;
-        break;
-      case 1:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
-              pdpStatus.bits.chan2_l4;
-        break;
-      case 2:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
-              pdpStatus.bits.chan3_l6;
-        break;
-      case 3:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
-              pdpStatus.bits.chan4_l8;
-        break;
-      case 4:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
-              pdpStatus.bits.chan5_l2;
-        break;
-      case 5:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
-              pdpStatus.bits.chan6_l4;
-        break;
-    }
-  } else if (channel <= 11) {
-    PdpStatus2 pdpStatus;
-    HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus.data, &length,
-                             &receivedTimestamp, TimeoutMs, status);
-    if (*status != 0) {
-      return 0;
-    }
-    switch (channel) {
-      case 6:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan7_h8) << 2) |
-              pdpStatus.bits.chan7_l2;
-        break;
-      case 7:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan8_h6) << 4) |
-              pdpStatus.bits.chan8_l4;
-        break;
-      case 8:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan9_h4) << 6) |
-              pdpStatus.bits.chan9_l6;
-        break;
-      case 9:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan10_h2) << 8) |
-              pdpStatus.bits.chan10_l8;
-        break;
-      case 10:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan11_h8) << 2) |
-              pdpStatus.bits.chan11_l2;
-        break;
-      case 11:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan12_h6) << 4) |
-              pdpStatus.bits.chan12_l4;
-        break;
-    }
-  } else {
-    PdpStatus3 pdpStatus;
-    HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus.data, &length,
-                             &receivedTimestamp, TimeoutMs, status);
-    if (*status != 0) {
-      return 0;
-    }
-    switch (channel) {
-      case 12:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan13_h8) << 2) |
-              pdpStatus.bits.chan13_l2;
-        break;
-      case 13:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan14_h6) << 4) |
-              pdpStatus.bits.chan14_l4;
-        break;
-      case 14:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan15_h4) << 6) |
-              pdpStatus.bits.chan15_l6;
-        break;
-      case 15:
-        raw = (static_cast<uint32_t>(pdpStatus.bits.chan16_h2) << 8) |
-              pdpStatus.bits.chan16_l8;
-        break;
-    }
-  }
-
-  /* convert to amps */
-  return raw * 0.125; /* 7.3 fixed pt value in Amps */
-}
-
-void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
-                                  int32_t* status) {
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-  PdpStatus1 pdpStatus;
-  HAL_ReadCANPacketTimeout(handle, Status1, pdpStatus.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-  if (*status != 0) return;
-  PdpStatus2 pdpStatus2;
-  HAL_ReadCANPacketTimeout(handle, Status2, pdpStatus2.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-  if (*status != 0) return;
-  PdpStatus3 pdpStatus3;
-  HAL_ReadCANPacketTimeout(handle, Status3, pdpStatus3.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-  if (*status != 0) return;
-
-  currents[0] = ((static_cast<uint32_t>(pdpStatus.bits.chan1_h8) << 2) |
-                 pdpStatus.bits.chan1_l2) *
-                0.125;
-  currents[1] = ((static_cast<uint32_t>(pdpStatus.bits.chan2_h6) << 4) |
-                 pdpStatus.bits.chan2_l4) *
-                0.125;
-  currents[2] = ((static_cast<uint32_t>(pdpStatus.bits.chan3_h4) << 6) |
-                 pdpStatus.bits.chan3_l6) *
-                0.125;
-  currents[3] = ((static_cast<uint32_t>(pdpStatus.bits.chan4_h2) << 8) |
-                 pdpStatus.bits.chan4_l8) *
-                0.125;
-  currents[4] = ((static_cast<uint32_t>(pdpStatus.bits.chan5_h8) << 2) |
-                 pdpStatus.bits.chan5_l2) *
-                0.125;
-  currents[5] = ((static_cast<uint32_t>(pdpStatus.bits.chan6_h6) << 4) |
-                 pdpStatus.bits.chan6_l4) *
-                0.125;
-
-  currents[6] = ((static_cast<uint32_t>(pdpStatus2.bits.chan7_h8) << 2) |
-                 pdpStatus2.bits.chan7_l2) *
-                0.125;
-  currents[7] = ((static_cast<uint32_t>(pdpStatus2.bits.chan8_h6) << 4) |
-                 pdpStatus2.bits.chan8_l4) *
-                0.125;
-  currents[8] = ((static_cast<uint32_t>(pdpStatus2.bits.chan9_h4) << 6) |
-                 pdpStatus2.bits.chan9_l6) *
-                0.125;
-  currents[9] = ((static_cast<uint32_t>(pdpStatus2.bits.chan10_h2) << 8) |
-                 pdpStatus2.bits.chan10_l8) *
-                0.125;
-  currents[10] = ((static_cast<uint32_t>(pdpStatus2.bits.chan11_h8) << 2) |
-                  pdpStatus2.bits.chan11_l2) *
-                 0.125;
-  currents[11] = ((static_cast<uint32_t>(pdpStatus2.bits.chan12_h6) << 4) |
-                  pdpStatus2.bits.chan12_l4) *
-                 0.125;
-
-  currents[12] = ((static_cast<uint32_t>(pdpStatus3.bits.chan13_h8) << 2) |
-                  pdpStatus3.bits.chan13_l2) *
-                 0.125;
-  currents[13] = ((static_cast<uint32_t>(pdpStatus3.bits.chan14_h6) << 4) |
-                  pdpStatus3.bits.chan14_l4) *
-                 0.125;
-  currents[14] = ((static_cast<uint32_t>(pdpStatus3.bits.chan15_h4) << 6) |
-                  pdpStatus3.bits.chan15_l6) *
-                 0.125;
-  currents[15] = ((static_cast<uint32_t>(pdpStatus3.bits.chan16_h2) << 8) |
-                  pdpStatus3.bits.chan16_l8) *
-                 0.125;
-}
-
-double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
-  PdpStatusEnergy pdpStatus;
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-
-  HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-  if (*status != 0) {
-    return 0;
-  }
-
-  uint32_t raw;
-  raw = pdpStatus.bits.TotalCurrent_125mAperunit_h8;
-  raw <<= 4;
-  raw |= pdpStatus.bits.TotalCurrent_125mAperunit_l4;
-  return 0.125 * raw; /* 7.3 fixed pt value in Amps */
-}
-
-double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
-  PdpStatusEnergy pdpStatus;
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-
-  HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-  if (*status != 0) {
-    return 0;
-  }
-
-  uint32_t raw;
-  raw = pdpStatus.bits.Power_125mWperunit_h4;
-  raw <<= 8;
-  raw |= pdpStatus.bits.Power_125mWperunit_m8;
-  raw <<= 4;
-  raw |= pdpStatus.bits.Power_125mWperunit_l4;
-  return 0.125 * raw; /* 7.3 fixed pt value in Watts */
-}
-
-double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
-  PdpStatusEnergy pdpStatus;
-  int32_t length = 0;
-  uint64_t receivedTimestamp = 0;
-
-  HAL_ReadCANPacketTimeout(handle, StatusEnergy, pdpStatus.data, &length,
-                           &receivedTimestamp, TimeoutMs, status);
-  if (*status != 0) {
-    return 0;
-  }
-
-  uint32_t raw;
-  raw = pdpStatus.bits.Energy_125mWPerUnitXTmeas_h4;
-  raw <<= 8;
-  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_mh8;
-  raw <<= 8;
-  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_ml8;
-  raw <<= 8;
-  raw |= pdpStatus.bits.Energy_125mWPerUnitXTmeas_l8;
-
-  double energyJoules = 0.125 * raw; /* mW integrated every TmeasMs */
-  energyJoules *= 0.001;             /* convert from mW to W */
-  energyJoules *=
-      pdpStatus.bits
-          .TmeasMs_likelywillbe20ms_; /* multiplied by TmeasMs = joules */
-  return energyJoules;
-}
-
-void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
-  uint8_t pdpControl[] = {0x40}; /* only bit set is ResetEnergy */
-  HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
-}
-
-void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {
-  uint8_t pdpControl[] = {0x80}; /* only bit set is ClearStickyFaults */
-  HAL_WriteCANPacket(handle, pdpControl, 1, Control1, status);
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp b/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp
index a3b141c..19a3b83 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/PWM.cpp
@@ -1,20 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/PWM.h"
 
+#include <algorithm>
 #include <cmath>
+#include <cstdio>
 #include <thread>
 
-#include <wpi/raw_ostream.h>
-
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/cpp/fpga_clock.h"
 #include "hal/handles/HandlesInternal.h"
@@ -61,24 +59,27 @@
   return GetMaxPositivePwm(port) - GetMinNegativePwm(port);
 }  ///< The scale for positions.
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePWM() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
   initializeDigital(status);
 
-  if (*status != 0) return HAL_kInvalidHandle;
+  if (*status != 0) {
+    return HAL_kInvalidHandle;
+  }
 
   int16_t channel = getPortHandleChannel(portHandle);
   if (channel == InvalidHandleIndex || channel >= kNumPWMChannels) {
-    *status = PARAMETER_OUT_OF_RANGE;
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                     kNumPWMChannels, channel);
     return HAL_kInvalidHandle;
   }
 
@@ -90,16 +91,20 @@
     channel = remapMXPPWMChannel(channel) + 10;  // remap MXP to proper channel
   }
 
-  auto handle =
-      digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM, status);
+  HAL_DigitalHandle handle;
 
-  if (*status != 0)
+  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM,
+                                              &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", origChannel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                       kNumPWMChannels, origChannel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::PWM);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
   }
 
   port->channel = origChannel;
@@ -115,6 +120,8 @@
   // Defaults to allow an always valid config.
   HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
 
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
@@ -131,8 +138,8 @@
   while (port.use_count() != 1) {
     auto current = hal::fpga_clock::now();
     if (start + std::chrono::seconds(1) < current) {
-      wpi::outs() << "PWM handle free timeout\n";
-      wpi::outs().flush();
+      std::puts("PWM handle free timeout");
+      std::fflush(stdout);
       break;
     }
     std::this_thread::yield();
@@ -163,7 +170,9 @@
   // calculate the loop time in milliseconds
   double loopTime =
       HAL_GetPWMLoopTiming(status) / (kSystemClockTicksPerMicrosecond * 1e3);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 
   int32_t maxPwm = static_cast<int32_t>((max - kDefaultPwmCenter) / loopTime +
                                         kDefaultPwmStepsDown - 1);
@@ -277,13 +286,13 @@
   if (speed == 0.0) {
     rawValue = GetCenterPwm(dPort);
   } else if (speed > 0.0) {
-    rawValue = static_cast<int32_t>(
-        speed * static_cast<double>(GetPositiveScaleFactor(dPort)) +
-        static_cast<double>(GetMinPositivePwm(dPort)) + 0.5);
+    rawValue =
+        std::lround(speed * static_cast<double>(GetPositiveScaleFactor(dPort)) +
+                    static_cast<double>(GetMinPositivePwm(dPort)));
   } else {
-    rawValue = static_cast<int32_t>(
-        speed * static_cast<double>(GetNegativeScaleFactor(dPort)) +
-        static_cast<double>(GetMaxNegativePwm(dPort)) + 0.5);
+    rawValue =
+        std::lround(speed * static_cast<double>(GetNegativeScaleFactor(dPort)) +
+                    static_cast<double>(GetMaxNegativePwm(dPort)));
   }
 
   if (!((rawValue >= GetMinNegativePwm(dPort)) &&
@@ -359,7 +368,9 @@
   }
 
   int32_t value = HAL_GetPWMRaw(pwmPortHandle, status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   DigitalPort* dPort = port.get();
 
   if (value == kPwmDisabled) {
@@ -391,7 +402,9 @@
   }
 
   int32_t value = HAL_GetPWMRaw(pwmPortHandle, status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   DigitalPort* dPort = port.get();
 
   if (value < GetMinNegativePwm(dPort)) {
@@ -433,22 +446,30 @@
 
 int32_t HAL_GetPWMLoopTiming(int32_t* status) {
   initializeDigital(status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   return pwmSystem->readLoopTiming(status);
 }
 
 uint64_t HAL_GetPWMCycleStartTime(int32_t* status) {
   initializeDigital(status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
 
   uint64_t upper1 = pwmSystem->readCycleStartTimeUpper(status);
   uint32_t lower = pwmSystem->readCycleStartTime(status);
   uint64_t upper2 = pwmSystem->readCycleStartTimeUpper(status);
-  if (*status != 0) return 0;
+  if (*status != 0) {
+    return 0;
+  }
   if (upper1 != upper2) {
     // Rolled over between the lower call, reread lower
     lower = pwmSystem->readCycleStartTime(status);
-    if (*status != 0) return 0;
+    if (*status != 0) {
+      return 0;
+    }
   }
   return (upper2 << 32) + lower;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Ports.cpp b/third_party/allwpilib/hal/src/main/native/athena/Ports.cpp
index 47bd400..d27ecbd 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Ports.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Ports.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Ports.h"
 
@@ -11,33 +8,83 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePorts() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
-int32_t HAL_GetNumAccumulators(void) { return kNumAccumulators; }
-int32_t HAL_GetNumAnalogTriggers(void) { return kNumAnalogTriggers; }
-int32_t HAL_GetNumAnalogInputs(void) { return kNumAnalogInputs; }
-int32_t HAL_GetNumAnalogOutputs(void) { return kNumAnalogOutputs; }
-int32_t HAL_GetNumCounters(void) { return kNumCounters; }
-int32_t HAL_GetNumDigitalHeaders(void) { return kNumDigitalHeaders; }
-int32_t HAL_GetNumPWMHeaders(void) { return kNumPWMHeaders; }
-int32_t HAL_GetNumDigitalChannels(void) { return kNumDigitalChannels; }
-int32_t HAL_GetNumPWMChannels(void) { return kNumPWMChannels; }
-int32_t HAL_GetNumDigitalPWMOutputs(void) { return kNumDigitalPWMOutputs; }
-int32_t HAL_GetNumEncoders(void) { return kNumEncoders; }
-int32_t HAL_GetNumInterrupts(void) { return kNumInterrupts; }
-int32_t HAL_GetNumRelayChannels(void) { return kNumRelayChannels; }
-int32_t HAL_GetNumRelayHeaders(void) { return kNumRelayHeaders; }
-int32_t HAL_GetNumPCMModules(void) { return kNumPCMModules; }
-int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
-int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
-int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
-int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
-int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
+int32_t HAL_GetNumAccumulators(void) {
+  return kNumAccumulators;
+}
+int32_t HAL_GetNumAnalogTriggers(void) {
+  return kNumAnalogTriggers;
+}
+int32_t HAL_GetNumAnalogInputs(void) {
+  return kNumAnalogInputs;
+}
+int32_t HAL_GetNumAnalogOutputs(void) {
+  return kNumAnalogOutputs;
+}
+int32_t HAL_GetNumCounters(void) {
+  return kNumCounters;
+}
+int32_t HAL_GetNumDigitalHeaders(void) {
+  return kNumDigitalHeaders;
+}
+int32_t HAL_GetNumPWMHeaders(void) {
+  return kNumPWMHeaders;
+}
+int32_t HAL_GetNumDigitalChannels(void) {
+  return kNumDigitalChannels;
+}
+int32_t HAL_GetNumPWMChannels(void) {
+  return kNumPWMChannels;
+}
+int32_t HAL_GetNumDigitalPWMOutputs(void) {
+  return kNumDigitalPWMOutputs;
+}
+int32_t HAL_GetNumEncoders(void) {
+  return kNumEncoders;
+}
+int32_t HAL_GetNumInterrupts(void) {
+  return kNumInterrupts;
+}
+int32_t HAL_GetNumRelayChannels(void) {
+  return kNumRelayChannels;
+}
+int32_t HAL_GetNumRelayHeaders(void) {
+  return kNumRelayHeaders;
+}
+int32_t HAL_GetNumCTREPCMModules(void) {
+  return kNumCTREPCMModules;
+}
+int32_t HAL_GetNumCTRESolenoidChannels(void) {
+  return kNumCTRESolenoidChannels;
+}
+int32_t HAL_GetNumCTREPDPModules(void) {
+  return kNumCTREPDPModules;
+}
+int32_t HAL_GetNumCTREPDPChannels(void) {
+  return kNumCTREPDPChannels;
+}
+int32_t HAL_GetNumREVPDHModules(void) {
+  return kNumREVPDHModules;
+}
+int32_t HAL_GetNumREVPDHChannels(void) {
+  return kNumREVPDHChannels;
+}
+int32_t HAL_GetNumREVPHModules(void) {
+  return kNumREVPHModules;
+}
+int32_t HAL_GetNumREVPHChannels(void) {
+  return kNumREVPHChannels;
+}
+int32_t HAL_GetNumDutyCycles(void) {
+  return kNumDutyCycles;
+}
+int32_t HAL_GetNumAddressableLEDs(void) {
+  return kNumAddressableLEDs;
+}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PortsInternal.h b/third_party/allwpilib/hal/src/main/native/athena/PortsInternal.h
index 98fc690..18ce569 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/PortsInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/athena/PortsInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -31,11 +28,15 @@
 constexpr int32_t kNumInterrupts = tInterrupt::kNumSystems;
 constexpr int32_t kNumRelayChannels = 8;
 constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2;
-constexpr int32_t kNumPCMModules = 63;
-constexpr int32_t kNumSolenoidChannels = 8;
-constexpr int32_t kNumPDPModules = 63;
-constexpr int32_t kNumPDPChannels = 16;
+constexpr int32_t kNumCTREPCMModules = 63;
+constexpr int32_t kNumCTRESolenoidChannels = 8;
+constexpr int32_t kNumCTREPDPModules = 63;
+constexpr int32_t kNumCTREPDPChannels = 16;
+constexpr int32_t kNumREVPDHModules = 63;
+constexpr int32_t kNumREVPDHChannels = 24;
 constexpr int32_t kNumDutyCycles = tDutyCycle::kNumSystems;
 constexpr int32_t kNumAddressableLEDs = tLED::kNumSystems;
+constexpr int32_t kNumREVPHModules = 63;
+constexpr int32_t kNumREVPHChannels = 16;
 
 }  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Power.cpp b/third_party/allwpilib/hal/src/main/native/athena/Power.cpp
index a13ebb6..895d6e3 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Power.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Power.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Power.h"
 
@@ -27,16 +24,14 @@
 
 }  // namespace hal
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePower() {
   if (power == nullptr) {
     int32_t status = 0;
     power.reset(tPower::create(&status));
   }
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -113,4 +108,22 @@
       power->readFaultCounts_OverCurrentFaultCount3V3(status));
 }
 
+void HAL_SetBrownoutVoltage(double voltage, int32_t* status) {
+  initializePower(status);
+  if (voltage < 0) {
+    voltage = 0;
+  }
+  if (voltage > 50) {
+    voltage = 50;
+  }
+  power->writeBrownoutVoltage250mV(static_cast<unsigned char>(voltage * 4),
+                                   status);
+}
+
+double HAL_GetBrownoutVoltage(int32_t* status) {
+  initializePower(status);
+  auto brownout = power->readBrownoutVoltage250mV(status);
+  return brownout / 4.0;
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp b/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp
new file mode 100644
index 0000000..f7fe88f
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/PowerDistribution.cpp
@@ -0,0 +1,208 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/PowerDistribution.h"
+
+#include "CTREPDP.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "REVPDH.h"
+#include "hal/Errors.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+extern "C" {
+
+HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
+    int32_t moduleNumber, HAL_PowerDistributionType type,
+    const char* allocationLocation, int32_t* status) {
+  if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic) {
+    type = HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE;
+  }
+
+  if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
+    if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
+      moduleNumber = 0;
+    }
+    return static_cast<HAL_PowerDistributionHandle>(
+        HAL_InitializePDP(moduleNumber, allocationLocation, status));  // TODO
+  } else {
+    if (moduleNumber == HAL_DEFAULT_POWER_DISTRIBUTION_MODULE) {
+      moduleNumber = 1;
+    }
+    return static_cast<HAL_PowerDistributionHandle>(
+        HAL_REV_InitializePDH(moduleNumber, allocationLocation, status));
+  }
+}
+
+#define IsCtre(handle) ::hal::isHandleType(handle, HAL_HandleEnum::CTREPDP)
+
+void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) {
+  if (IsCtre(handle)) {
+    HAL_CleanPDP(handle);
+  } else {
+    HAL_REV_FreePDH(handle);
+  }
+}
+
+int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle,
+                                             int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPModuleNumber(handle, status);
+  } else {
+    return HAL_REV_GetPDHModuleNumber(handle, status);
+  }
+}
+
+HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle,
+                                           int32_t channel) {
+  if (IsCtre(handle)) {
+    return HAL_CheckPDPChannel(channel);
+  } else {
+    return HAL_REV_CheckPDHChannelNumber(channel);
+  }
+}
+
+HAL_Bool HAL_CheckPowerDistributionModule(int32_t module,
+                                          HAL_PowerDistributionType type) {
+  if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
+    return HAL_CheckPDPModule(module);
+  } else {
+    return HAL_REV_CheckPDHModuleNumber(module);
+  }
+}
+
+HAL_PowerDistributionType HAL_GetPowerDistributionType(
+    HAL_PowerDistributionHandle handle, int32_t* status) {
+  return IsCtre(handle)
+             ? HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE
+             : HAL_PowerDistributionType::HAL_PowerDistributionType_kRev;
+}
+
+int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {
+  if (IsCtre(handle)) {
+    return kNumCTREPDPChannels;
+  } else {
+    return kNumREVPDHChannels;
+  }
+}
+
+double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle,
+                                           int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPTemperature(handle, status);
+  } else {
+    // Not supported
+    return 0;
+  }
+}
+
+double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle,
+                                       int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPVoltage(handle, status);
+  } else {
+    return HAL_REV_GetPDHSupplyVoltage(handle, status);
+  }
+}
+
+double HAL_GetPowerDistributionChannelCurrent(
+    HAL_PowerDistributionHandle handle, int32_t channel, int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPChannelCurrent(handle, channel, status);
+  } else {
+    return HAL_REV_GetPDHChannelCurrent(handle, channel, status);
+  }
+}
+
+void HAL_GetPowerDistributionAllChannelCurrents(
+    HAL_PowerDistributionHandle handle, double* currents,
+    int32_t currentsLength, int32_t* status) {
+  if (IsCtre(handle)) {
+    if (currentsLength < kNumCTREPDPChannels) {
+      *status = PARAMETER_OUT_OF_RANGE;
+      SetLastError(status, "Output array not large enough");
+      return;
+    }
+    return HAL_GetPDPAllChannelCurrents(handle, currents, status);
+  } else {
+    if (currentsLength < kNumREVPDHChannels) {
+      *status = PARAMETER_OUT_OF_RANGE;
+      SetLastError(status, "Output array not large enough");
+      return;
+    }
+    return HAL_REV_GetPDHAllChannelCurrents(handle, currents, status);
+  }
+}
+
+double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPTotalCurrent(handle, status);
+  } else {
+    return HAL_REV_GetPDHTotalCurrent(handle, status);
+  }
+}
+
+double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle,
+                                          int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPTotalPower(handle, status);
+  } else {
+    // Not currently supported
+    return 0;
+  }
+}
+
+double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
+                                           int32_t* status) {
+  if (IsCtre(handle)) {
+    return HAL_GetPDPTotalEnergy(handle, status);
+  } else {
+    // Not currently supported
+    return 0;
+  }
+}
+
+void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
+                                           int32_t* status) {
+  if (IsCtre(handle)) {
+    HAL_ResetPDPTotalEnergy(handle, status);
+  } else {
+    // Not supported
+  }
+}
+
+void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {
+  if (IsCtre(handle)) {
+    HAL_ClearPDPStickyFaults(handle, status);
+  } else {
+    HAL_REV_ClearPDHFaults(handle, status);
+  }
+}
+
+void HAL_SetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status) {
+  if (IsCtre(handle)) {
+    // No-op on CTRE
+    return;
+  } else {
+    HAL_REV_SetPDHSwitchableChannel(handle, enabled, status);
+  }
+}
+
+HAL_Bool HAL_GetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, int32_t* status) {
+  if (IsCtre(handle)) {
+    // No-op on CTRE
+    return false;
+  } else {
+    return HAL_REV_GetPDHSwitchableChannelState(handle, status);
+  }
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp
new file mode 100644
index 0000000..9ad45b5
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.cpp
@@ -0,0 +1,798 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "REVPDH.h"
+
+#include <hal/CANAPI.h>
+#include <hal/CANAPITypes.h>
+#include <hal/Errors.h>
+#include <hal/handles/HandlesInternal.h>
+#include <hal/handles/IndexedHandleResource.h>
+
+#include <cstring>
+
+#include <fmt/format.h>
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "rev/PDHFrames.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+    HAL_CANManufacturer::HAL_CAN_Man_kREV;
+
+static constexpr HAL_CANDeviceType deviceType =
+    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
+
+static constexpr int32_t kDefaultControlPeriod = 50;
+
+namespace {
+
+struct REV_PDHObj {
+  int32_t controlPeriod;
+  HAL_CANHandle hcan;
+  std::string previousAllocation;
+};
+
+}  // namespace
+
+static constexpr uint32_t APIFromExtId(uint32_t extId) {
+  return (extId >> 6) & 0x3FF;
+}
+
+static constexpr uint32_t PDH_SWITCH_CHANNEL_SET_FRAME_API =
+    APIFromExtId(PDH_SWITCH_CHANNEL_SET_FRAME_ID);
+
+static constexpr uint32_t PDH_STATUS0_FRAME_API =
+    APIFromExtId(PDH_STATUS0_FRAME_ID);
+static constexpr uint32_t PDH_STATUS1_FRAME_API =
+    APIFromExtId(PDH_STATUS1_FRAME_ID);
+static constexpr uint32_t PDH_STATUS2_FRAME_API =
+    APIFromExtId(PDH_STATUS2_FRAME_ID);
+static constexpr uint32_t PDH_STATUS3_FRAME_API =
+    APIFromExtId(PDH_STATUS3_FRAME_ID);
+static constexpr uint32_t PDH_STATUS4_FRAME_API =
+    APIFromExtId(PDH_STATUS4_FRAME_ID);
+
+static constexpr uint32_t PDH_CLEAR_FAULTS_FRAME_API =
+    APIFromExtId(PDH_CLEAR_FAULTS_FRAME_ID);
+
+static constexpr uint32_t PDH_IDENTIFY_FRAME_API =
+    APIFromExtId(PDH_IDENTIFY_FRAME_ID);
+
+static constexpr uint32_t PDH_VERSION_FRAME_API =
+    APIFromExtId(PDH_VERSION_FRAME_ID);
+
+static constexpr uint32_t PDH_CONFIGURE_HR_CHANNEL_FRAME_API =
+    APIFromExtId(PDH_CONFIGURE_HR_CHANNEL_FRAME_ID);
+
+static constexpr int32_t kPDHFrameStatus0Timeout = 20;
+static constexpr int32_t kPDHFrameStatus1Timeout = 20;
+static constexpr int32_t kPDHFrameStatus2Timeout = 20;
+static constexpr int32_t kPDHFrameStatus3Timeout = 20;
+static constexpr int32_t kPDHFrameStatus4Timeout = 20;
+
+static IndexedHandleResource<HAL_REVPDHHandle, REV_PDHObj, kNumREVPDHModules,
+                             HAL_HandleEnum::REVPDH>* REVPDHHandles;
+
+namespace hal::init {
+void InitializeREVPDH() {
+  static IndexedHandleResource<HAL_REVPDHHandle, REV_PDHObj, kNumREVPDHModules,
+                               HAL_HandleEnum::REVPDH>
+      rH;
+  REVPDHHandles = &rH;
+}
+}  // namespace hal::init
+
+extern "C" {
+
+static PDH_status0_t HAL_REV_ReadPDHStatus0(HAL_CANHandle hcan,
+                                            int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PDH_status0_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS0_FRAME_API, packedData, &length,
+                           &timestamp, kPDHFrameStatus0Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PDH_status0_unpack(&result, packedData, PDH_STATUS0_LENGTH);
+
+  return result;
+}
+
+static PDH_status1_t HAL_REV_ReadPDHStatus1(HAL_CANHandle hcan,
+                                            int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PDH_status1_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS1_FRAME_API, packedData, &length,
+                           &timestamp, kPDHFrameStatus1Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PDH_status1_unpack(&result, packedData, PDH_STATUS1_LENGTH);
+
+  return result;
+}
+
+static PDH_status2_t HAL_REV_ReadPDHStatus2(HAL_CANHandle hcan,
+                                            int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PDH_status2_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS2_FRAME_API, packedData, &length,
+                           &timestamp, kPDHFrameStatus2Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PDH_status2_unpack(&result, packedData, PDH_STATUS2_LENGTH);
+
+  return result;
+}
+
+static PDH_status3_t HAL_REV_ReadPDHStatus3(HAL_CANHandle hcan,
+                                            int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PDH_status3_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS3_FRAME_API, packedData, &length,
+                           &timestamp, kPDHFrameStatus3Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PDH_status3_unpack(&result, packedData, PDH_STATUS3_LENGTH);
+
+  return result;
+}
+
+static PDH_status4_t HAL_REV_ReadPDHStatus4(HAL_CANHandle hcan,
+                                            int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PDH_status4_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PDH_STATUS4_FRAME_API, packedData, &length,
+                           &timestamp, kPDHFrameStatus4Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PDH_status4_unpack(&result, packedData, PDH_STATUS4_LENGTH);
+
+  return result;
+}
+
+/**
+ * Helper function for the individual getter functions for status 4
+ */
+PDH_status4_t HAL_REV_GetPDHStatus4(HAL_REVPDHHandle handle, int32_t* status) {
+  PDH_status4_t statusFrame = {};
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return statusFrame;
+  }
+
+  statusFrame = HAL_REV_ReadPDHStatus4(hpdh->hcan, status);
+  return statusFrame;
+}
+
+HAL_REVPDHHandle HAL_REV_InitializePDH(int32_t module,
+                                       const char* allocationLocation,
+                                       int32_t* status) {
+  hal::init::CheckInit();
+  if (!HAL_REV_CheckPDHModuleNumber(module)) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_REVPDHHandle handle;
+  auto hpdh = REVPDHHandles->Allocate(module, &handle, status);
+  if (*status != 0) {
+    if (hpdh) {
+      hal::SetLastErrorPreviouslyAllocated(status, "REV PDH", module,
+                                           hpdh->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PDH", 0,
+                                       kNumREVPDHModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  HAL_CANHandle hcan =
+      HAL_InitializeCAN(manufacturer, module, deviceType, status);
+
+  if (*status != 0) {
+    REVPDHHandles->Free(handle);
+    return HAL_kInvalidHandle;
+  }
+
+  hpdh->previousAllocation = allocationLocation ? allocationLocation : "";
+  hpdh->hcan = hcan;
+  hpdh->controlPeriod = kDefaultControlPeriod;
+
+  return handle;
+}
+
+void HAL_REV_FreePDH(HAL_REVPDHHandle handle) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    return;
+  }
+
+  HAL_CleanCAN(hpdh->hcan);
+
+  REVPDHHandles->Free(handle);
+}
+
+int32_t HAL_REV_GetPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status) {
+  return hal::getHandleIndex(handle);
+}
+
+HAL_Bool HAL_REV_CheckPDHModuleNumber(int32_t module) {
+  return ((module >= 1) && (module < kNumREVPDHModules)) ? 1 : 0;
+}
+
+HAL_Bool HAL_REV_CheckPDHChannelNumber(int32_t channel) {
+  return ((channel >= 0) && (channel < kNumREVPDHChannels)) ? 1 : 0;
+}
+
+double HAL_REV_GetPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel,
+                                    int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  if (!HAL_REV_CheckPDHChannelNumber(channel)) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return 0;
+  }
+
+  // Determine what periodic status the channel is in
+  if (channel < 6) {
+    // Periodic status 0
+    PDH_status0_t statusFrame = HAL_REV_ReadPDHStatus0(hpdh->hcan, status);
+    switch (channel) {
+      case 0:
+        return PDH_status0_channel_0_current_decode(
+            statusFrame.channel_0_current);
+      case 1:
+        return PDH_status0_channel_1_current_decode(
+            statusFrame.channel_1_current);
+      case 2:
+        return PDH_status0_channel_2_current_decode(
+            statusFrame.channel_2_current);
+      case 3:
+        return PDH_status0_channel_3_current_decode(
+            statusFrame.channel_3_current);
+      case 4:
+        return PDH_status0_channel_4_current_decode(
+            statusFrame.channel_4_current);
+      case 5:
+        return PDH_status0_channel_5_current_decode(
+            statusFrame.channel_5_current);
+    }
+  } else if (channel < 12) {
+    // Periodic status 1
+    PDH_status1_t statusFrame = HAL_REV_ReadPDHStatus1(hpdh->hcan, status);
+    switch (channel) {
+      case 6:
+        return PDH_status1_channel_6_current_decode(
+            statusFrame.channel_6_current);
+      case 7:
+        return PDH_status1_channel_7_current_decode(
+            statusFrame.channel_7_current);
+      case 8:
+        return PDH_status1_channel_8_current_decode(
+            statusFrame.channel_8_current);
+      case 9:
+        return PDH_status1_channel_9_current_decode(
+            statusFrame.channel_9_current);
+      case 10:
+        return PDH_status1_channel_10_current_decode(
+            statusFrame.channel_10_current);
+      case 11:
+        return PDH_status1_channel_11_current_decode(
+            statusFrame.channel_11_current);
+    }
+  } else if (channel < 18) {
+    // Periodic status 2
+    PDH_status2_t statusFrame = HAL_REV_ReadPDHStatus2(hpdh->hcan, status);
+    switch (channel) {
+      case 12:
+        return PDH_status2_channel_12_current_decode(
+            statusFrame.channel_12_current);
+      case 13:
+        return PDH_status2_channel_13_current_decode(
+            statusFrame.channel_13_current);
+      case 14:
+        return PDH_status2_channel_14_current_decode(
+            statusFrame.channel_14_current);
+      case 15:
+        return PDH_status2_channel_15_current_decode(
+            statusFrame.channel_15_current);
+      case 16:
+        return PDH_status2_channel_16_current_decode(
+            statusFrame.channel_16_current);
+      case 17:
+        return PDH_status2_channel_17_current_decode(
+            statusFrame.channel_17_current);
+    }
+  } else if (channel < 24) {
+    // Periodic status 3
+    PDH_status3_t statusFrame = HAL_REV_ReadPDHStatus3(hpdh->hcan, status);
+    switch (channel) {
+      case 18:
+        return PDH_status3_channel_18_current_decode(
+            statusFrame.channel_18_current);
+      case 19:
+        return PDH_status3_channel_19_current_decode(
+            statusFrame.channel_19_current);
+      case 20:
+        return PDH_status3_channel_20_current_decode(
+            statusFrame.channel_20_current);
+      case 21:
+        return PDH_status3_channel_21_current_decode(
+            statusFrame.channel_21_current);
+      case 22:
+        return PDH_status3_channel_22_current_decode(
+            statusFrame.channel_22_current);
+      case 23:
+        return PDH_status3_channel_23_current_decode(
+            statusFrame.channel_23_current);
+    }
+  }
+  return 0;
+}
+
+void HAL_REV_GetPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents,
+                                      int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  PDH_status0_t statusFrame0 = HAL_REV_ReadPDHStatus0(hpdh->hcan, status);
+  PDH_status1_t statusFrame1 = HAL_REV_ReadPDHStatus1(hpdh->hcan, status);
+  PDH_status2_t statusFrame2 = HAL_REV_ReadPDHStatus2(hpdh->hcan, status);
+  PDH_status3_t statusFrame3 = HAL_REV_ReadPDHStatus3(hpdh->hcan, status);
+
+  currents[0] =
+      PDH_status0_channel_0_current_decode(statusFrame0.channel_0_current);
+  currents[1] =
+      PDH_status0_channel_1_current_decode(statusFrame0.channel_1_current);
+  currents[2] =
+      PDH_status0_channel_2_current_decode(statusFrame0.channel_2_current);
+  currents[3] =
+      PDH_status0_channel_3_current_decode(statusFrame0.channel_3_current);
+  currents[4] =
+      PDH_status0_channel_4_current_decode(statusFrame0.channel_4_current);
+  currents[5] =
+      PDH_status0_channel_5_current_decode(statusFrame0.channel_5_current);
+  currents[6] =
+      PDH_status1_channel_6_current_decode(statusFrame1.channel_6_current);
+  currents[7] =
+      PDH_status1_channel_7_current_decode(statusFrame1.channel_7_current);
+  currents[8] =
+      PDH_status1_channel_8_current_decode(statusFrame1.channel_8_current);
+  currents[9] =
+      PDH_status1_channel_9_current_decode(statusFrame1.channel_9_current);
+  currents[10] =
+      PDH_status1_channel_10_current_decode(statusFrame1.channel_10_current);
+  currents[11] =
+      PDH_status1_channel_11_current_decode(statusFrame1.channel_11_current);
+  currents[12] =
+      PDH_status2_channel_12_current_decode(statusFrame2.channel_12_current);
+  currents[13] =
+      PDH_status2_channel_13_current_decode(statusFrame2.channel_13_current);
+  currents[14] =
+      PDH_status2_channel_14_current_decode(statusFrame2.channel_14_current);
+  currents[15] =
+      PDH_status2_channel_15_current_decode(statusFrame2.channel_15_current);
+  currents[16] =
+      PDH_status2_channel_16_current_decode(statusFrame2.channel_16_current);
+  currents[17] =
+      PDH_status2_channel_17_current_decode(statusFrame2.channel_17_current);
+  currents[18] =
+      PDH_status3_channel_18_current_decode(statusFrame3.channel_18_current);
+  currents[19] =
+      PDH_status3_channel_19_current_decode(statusFrame3.channel_19_current);
+  currents[20] =
+      PDH_status3_channel_20_current_decode(statusFrame3.channel_20_current);
+  currents[21] =
+      PDH_status3_channel_21_current_decode(statusFrame3.channel_21_current);
+  currents[22] =
+      PDH_status3_channel_22_current_decode(statusFrame3.channel_22_current);
+  currents[23] =
+      PDH_status3_channel_23_current_decode(statusFrame3.channel_23_current);
+}
+
+uint16_t HAL_REV_GetPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0;
+  }
+
+  return PDH_status4_total_current_decode(statusFrame.total_current);
+}
+
+void HAL_REV_SetPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled,
+                                     int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint8_t packedData[8] = {0};
+  PDH_switch_channel_set_t frame;
+  frame.output_set_value = enabled;
+  frame.use_system_enable = false;
+  PDH_switch_channel_set_pack(packedData, &frame, 1);
+
+  HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_SWITCH_CHANNEL_SET_LENGTH,
+                     PDH_SWITCH_CHANNEL_SET_FRAME_API, status);
+}
+
+HAL_Bool HAL_REV_GetPDHSwitchableChannelState(HAL_REVPDHHandle handle,
+                                              int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sw_state_decode(statusFrame.sw_state);
+}
+
+HAL_Bool HAL_REV_CheckPDHChannelBrownout(HAL_REVPDHHandle handle,
+                                         int32_t channel, int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  if (!HAL_REV_CheckPDHChannelNumber(channel)) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return 0;
+  }
+
+  // Determine what periodic status the channel is in
+  if (channel < 4) {
+    // Periodic status 0
+    PDH_status0_t statusFrame = HAL_REV_ReadPDHStatus0(hpdh->hcan, status);
+    switch (channel) {
+      case 0:
+        return PDH_status0_channel_0_brownout_decode(
+            statusFrame.channel_0_brownout);
+      case 1:
+        return PDH_status0_channel_1_brownout_decode(
+            statusFrame.channel_1_brownout);
+      case 2:
+        return PDH_status0_channel_2_brownout_decode(
+            statusFrame.channel_2_brownout);
+      case 3:
+        return PDH_status0_channel_3_brownout_decode(
+            statusFrame.channel_3_brownout);
+    }
+  } else if (channel < 8) {
+    // Periodic status 1
+    PDH_status1_t statusFrame = HAL_REV_ReadPDHStatus1(hpdh->hcan, status);
+    switch (channel) {
+      case 4:
+        return PDH_status1_channel_4_brownout_decode(
+            statusFrame.channel_4_brownout);
+      case 5:
+        return PDH_status1_channel_5_brownout_decode(
+            statusFrame.channel_5_brownout);
+      case 6:
+        return PDH_status1_channel_6_brownout_decode(
+            statusFrame.channel_6_brownout);
+      case 7:
+        return PDH_status1_channel_7_brownout_decode(
+            statusFrame.channel_7_brownout);
+    }
+  } else if (channel < 12) {
+    // Periodic status 2
+    PDH_status2_t statusFrame = HAL_REV_ReadPDHStatus2(hpdh->hcan, status);
+    switch (channel) {
+      case 8:
+        return PDH_status2_channel_8_brownout_decode(
+            statusFrame.channel_8_brownout);
+      case 9:
+        return PDH_status2_channel_9_brownout_decode(
+            statusFrame.channel_9_brownout);
+      case 10:
+        return PDH_status2_channel_10_brownout_decode(
+            statusFrame.channel_10_brownout);
+      case 11:
+        return PDH_status2_channel_11_brownout_decode(
+            statusFrame.channel_11_brownout);
+    }
+  } else if (channel < 24) {
+    // Periodic status 3
+    PDH_status3_t statusFrame = HAL_REV_ReadPDHStatus3(hpdh->hcan, status);
+    switch (channel) {
+      case 12:
+        return PDH_status3_channel_12_brownout_decode(
+            statusFrame.channel_12_brownout);
+      case 13:
+        return PDH_status3_channel_13_brownout_decode(
+            statusFrame.channel_13_brownout);
+      case 14:
+        return PDH_status3_channel_14_brownout_decode(
+            statusFrame.channel_14_brownout);
+      case 15:
+        return PDH_status3_channel_15_brownout_decode(
+            statusFrame.channel_15_brownout);
+      case 16:
+        return PDH_status3_channel_16_brownout_decode(
+            statusFrame.channel_16_brownout);
+      case 17:
+        return PDH_status3_channel_17_brownout_decode(
+            statusFrame.channel_17_brownout);
+      case 18:
+        return PDH_status3_channel_18_brownout_decode(
+            statusFrame.channel_18_brownout);
+      case 19:
+        return PDH_status3_channel_19_brownout_decode(
+            statusFrame.channel_19_brownout);
+      case 20:
+        return PDH_status3_channel_20_brownout_decode(
+            statusFrame.channel_20_brownout);
+      case 21:
+        return PDH_status3_channel_21_brownout_decode(
+            statusFrame.channel_21_brownout);
+      case 22:
+        return PDH_status3_channel_22_brownout_decode(
+            statusFrame.channel_22_brownout);
+      case 23:
+        return PDH_status3_channel_23_brownout_decode(
+            statusFrame.channel_23_brownout);
+    }
+  }
+  return 0;
+}
+
+double HAL_REV_GetPDHSupplyVoltage(HAL_REVPDHHandle handle, int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_v_bus_decode(statusFrame.v_bus);
+}
+
+HAL_Bool HAL_REV_IsPDHEnabled(HAL_REVPDHHandle handle, int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return false;
+  }
+
+  return PDH_status4_system_enable_decode(statusFrame.system_enable);
+}
+
+HAL_Bool HAL_REV_CheckPDHBrownout(HAL_REVPDHHandle handle, int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return false;
+  }
+
+  return PDH_status4_brownout_decode(statusFrame.brownout);
+}
+
+HAL_Bool HAL_REV_CheckPDHCANWarning(HAL_REVPDHHandle handle, int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_can_warning_decode(statusFrame.can_warning);
+}
+
+HAL_Bool HAL_REV_CheckPDHHardwareFault(HAL_REVPDHHandle handle,
+                                       int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_hardware_fault_decode(statusFrame.hardware_fault);
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyBrownout(HAL_REVPDHHandle handle,
+                                        int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sticky_brownout_decode(statusFrame.sticky_brownout);
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyCANWarning(HAL_REVPDHHandle handle,
+                                          int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sticky_can_warning_decode(statusFrame.sticky_can_warning);
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyCANBusOff(HAL_REVPDHHandle handle,
+                                         int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sticky_can_bus_off_decode(statusFrame.sticky_can_bus_off);
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyHardwareFault(HAL_REVPDHHandle handle,
+                                             int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sticky_hardware_fault_decode(
+      statusFrame.sticky_hardware_fault);
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyFirmwareFault(HAL_REVPDHHandle handle,
+                                             int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sticky_firmware_fault_decode(
+      statusFrame.sticky_firmware_fault);
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyChannelBrownout(HAL_REVPDHHandle handle,
+                                               int32_t channel,
+                                               int32_t* status) {
+  if (channel < 20 || channel > 23) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    return 0.0;
+  }
+
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  switch (channel) {
+    case 20:
+      return PDH_status4_sticky_ch20_brownout_decode(
+          statusFrame.sticky_ch20_brownout);
+    case 21:
+      return PDH_status4_sticky_ch21_brownout_decode(
+          statusFrame.sticky_ch21_brownout);
+    case 22:
+      return PDH_status4_sticky_ch22_brownout_decode(
+          statusFrame.sticky_ch22_brownout);
+    case 23:
+      return PDH_status4_sticky_ch23_brownout_decode(
+          statusFrame.sticky_ch23_brownout);
+  }
+  return 0;
+}
+
+HAL_Bool HAL_REV_CheckPDHStickyHasReset(HAL_REVPDHHandle handle,
+                                        int32_t* status) {
+  PDH_status4_t statusFrame = HAL_REV_GetPDHStatus4(handle, status);
+
+  if (*status != 0) {
+    return 0.0;
+  }
+
+  return PDH_status4_sticky_has_reset_decode(statusFrame.sticky_has_reset);
+}
+
+REV_PDH_Version HAL_REV_GetPDHVersion(HAL_REVPDHHandle handle,
+                                      int32_t* status) {
+  REV_PDH_Version version;
+  std::memset(&version, 0, sizeof(version));
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PDH_version_t result = {};
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return version;
+  }
+
+  HAL_WriteCANRTRFrame(hpdh->hcan, PDH_VERSION_LENGTH, PDH_VERSION_FRAME_API,
+                       status);
+
+  if (*status != 0) {
+    return version;
+  }
+
+  HAL_ReadCANPacketTimeout(hpdh->hcan, PDH_VERSION_FRAME_API, packedData,
+                           &length, &timestamp, kDefaultControlPeriod * 2,
+                           status);
+
+  if (*status != 0) {
+    return version;
+  }
+
+  PDH_version_unpack(&result, packedData, PDH_VERSION_LENGTH);
+
+  version.firmwareMajor = result.firmware_year;
+  version.firmwareMinor = result.firmware_minor;
+  version.firmwareFix = result.firmware_fix;
+  version.hardwareRev = result.hardware_code;
+  version.uniqueId = result.unique_id;
+
+  return version;
+}
+
+void HAL_REV_ClearPDHFaults(HAL_REVPDHHandle handle, int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint8_t packedData[8] = {0};
+  HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_CLEAR_FAULTS_LENGTH,
+                     PDH_CLEAR_FAULTS_FRAME_API, status);
+}
+
+void HAL_REV_IdentifyPDH(HAL_REVPDHHandle handle, int32_t* status) {
+  auto hpdh = REVPDHHandles->Get(handle);
+  if (hpdh == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  uint8_t packedData[8] = {0};
+  HAL_WriteCANPacket(hpdh->hcan, packedData, PDH_IDENTIFY_LENGTH,
+                     PDH_IDENTIFY_FRAME_API, status);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h
new file mode 100644
index 0000000..228d05c
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/REVPDH.h
@@ -0,0 +1,314 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_rev_pdh REV Power Distribution Hub API Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+struct REV_PDH_Version {
+  uint32_t firmwareMajor;
+  uint32_t firmwareMinor;
+  uint32_t firmwareFix;
+  uint32_t hardwareRev;
+  uint32_t uniqueId;
+};
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a REV Power Distribution Hub (PDH) device.
+ *
+ * @param module       the device CAN ID (1 .. 63)
+ * @return the created PDH handle
+ */
+HAL_REVPDHHandle HAL_REV_InitializePDH(int32_t module,
+                                       const char* allocationLocation,
+                                       int32_t* status);
+
+/**
+ * Frees a PDH device handle.
+ *
+ * @param handle        the previously created PDH handle
+ */
+void HAL_REV_FreePDH(HAL_REVPDHHandle handle);
+
+/**
+ * Gets the module number for a pdh.
+ */
+int32_t HAL_REV_GetPDHModuleNumber(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Checks if a PDH module number is valid.
+ *
+ * Does not check if a PDH device with this module has been initialized.
+ *
+ * @param module        module number (1 .. 63)
+ * @return 1 if the module number is valid; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHModuleNumber(int32_t module);
+
+/**
+ * Checks if a PDH channel number is valid.
+ *
+ * @param module        channel number (0 .. HAL_REV_PDH_NUM_CHANNELS)
+ * @return 1 if the channel number is valid; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHChannelNumber(int32_t channel);
+
+/**
+ * Gets the current of a PDH channel in Amps.
+ *
+ * @param handle        PDH handle
+ * @param channel       the channel to retrieve the current of (0 ..
+ * HAL_REV_PDH_NUM_CHANNELS)
+ *
+ * @return the current of the PDH channel in Amps
+ */
+double HAL_REV_GetPDHChannelCurrent(HAL_REVPDHHandle handle, int32_t channel,
+                                    int32_t* status);
+
+/**
+ * @param handle        PDH handle
+ * @param currents      array of currents
+ */
+void HAL_REV_GetPDHAllChannelCurrents(HAL_REVPDHHandle handle, double* currents,
+                                      int32_t* status);
+
+/**
+ * Gets the total current of the PDH in Amps, measured to the nearest even
+ * integer.
+ *
+ * @param handle        PDH handle
+ *
+ * @return the total current of the PDH in Amps
+ */
+uint16_t HAL_REV_GetPDHTotalCurrent(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Sets the state of the switchable channel on a PDH device.
+ *
+ * @param handle        PDH handle
+ * @param enabled       1 if the switchable channel should be enabled; 0
+ * otherwise
+ */
+void HAL_REV_SetPDHSwitchableChannel(HAL_REVPDHHandle handle, HAL_Bool enabled,
+                                     int32_t* status);
+
+/**
+ * Gets the current state of the switchable channel on a PDH device.
+ *
+ * This call relies on a periodic status sent by the PDH device and will be as
+ * fresh as the last packet received.
+ *
+ * @param handle        PDH handle
+ * @return 1 if the switchable channel is enabled; 0 otherwise
+ */
+HAL_Bool HAL_REV_GetPDHSwitchableChannelState(HAL_REVPDHHandle handle,
+                                              int32_t* status);
+
+/**
+ * Checks if a PDH channel is currently experiencing a brownout condition.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ * @param channel       the channel to retrieve the brownout status of
+ *
+ * @return 1 if the channel is experiencing a brownout; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHChannelBrownout(HAL_REVPDHHandle handle,
+                                         int32_t channel, int32_t* status);
+
+/**
+ * Gets the voltage being supplied to a PDH device.
+ *
+ * @param handle        PDH handle
+ *
+ * @return the voltage at the input of the PDH in Volts
+ */
+double HAL_REV_GetPDHSupplyVoltage(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Checks if a PDH device is currently enabled.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the PDH is enabled; 0 otherwise
+ */
+HAL_Bool HAL_REV_IsPDHEnabled(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Checks if the input voltage on a PDH device is currently below the minimum
+ * voltage.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the PDH is experiencing a brownout; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHBrownout(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Checks if the CAN RX or TX error levels on a PDH device have exceeded the
+ * warning threshold.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has exceeded the warning threshold; 0
+ * otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHCANWarning(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Checks if a PDH device is currently malfunctioning.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device is in a hardware fault state; 0
+ * otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHHardwareFault(HAL_REVPDHHandle handle,
+                                       int32_t* status);
+
+/**
+ * Checks if the input voltage on a PDH device has gone below the specified
+ * minimum voltage.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has had a brownout; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyBrownout(HAL_REVPDHHandle handle,
+                                        int32_t* status);
+
+/**
+ * Checks if the CAN RX or TX error levels on a PDH device have exceeded the
+ * warning threshold.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has exceeded the CAN warning threshold;
+ * 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyCANWarning(HAL_REVPDHHandle handle,
+                                          int32_t* status);
+
+/**
+ * Checks if the CAN bus on a PDH device has previously experienced a 'Bus Off'
+ * event.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has experienced a 'Bus Off' event; 0
+ * otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyCANBusOff(HAL_REVPDHHandle handle,
+                                         int32_t* status);
+
+/**
+ * Checks if a PDH device has malfunctioned.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has had a malfunction; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyHardwareFault(HAL_REVPDHHandle handle,
+                                             int32_t* status);
+
+/**
+ * Checks if the firmware on a PDH device has malfunctioned and reset during
+ * operation.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has had a malfunction and reset; 0
+ * otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyFirmwareFault(HAL_REVPDHHandle handle,
+                                             int32_t* status);
+
+/**
+ * Checks if a brownout has happened on channels 20-23 of a PDH device while it
+ * was enabled.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ * @param channel       PDH channel to retrieve sticky brownout status (20 ..
+ * 23)
+ *
+ *
+ * @return 1 if the channel has had a brownout; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyChannelBrownout(HAL_REVPDHHandle handle,
+                                               int32_t channel,
+                                               int32_t* status);
+
+/**
+ * Checks if a PDH device has reset.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ *
+ * @return 1 if the device has reset; 0 otherwise
+ */
+HAL_Bool HAL_REV_CheckPDHStickyHasReset(HAL_REVPDHHandle handle,
+                                        int32_t* status);
+
+/**
+ * Gets the firmware and hardware versions of a PDH device.
+ *
+ * @param handle        PDH handle
+ *
+ * @return version information
+ */
+REV_PDH_Version HAL_REV_GetPDHVersion(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Clears the sticky faults on a PDH device.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ */
+void HAL_REV_ClearPDHFaults(HAL_REVPDHHandle handle, int32_t* status);
+
+/**
+ * Identifies a PDH device by blinking its LED.
+ *
+ * NOTE: Not implemented in firmware as of 2021-04-23.
+ *
+ * @param handle        PDH handle
+ */
+void HAL_REV_IdentifyPDH(HAL_REVPDHHandle handle, int32_t* status);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/athena/REVPH.cpp b/third_party/allwpilib/hal/src/main/native/athena/REVPH.cpp
new file mode 100644
index 0000000..155f92c
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/REVPH.cpp
@@ -0,0 +1,480 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/REVPH.h"
+
+#include <fmt/format.h>
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "rev/PHFrames.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+    HAL_CANManufacturer::HAL_CAN_Man_kREV;
+
+static constexpr HAL_CANDeviceType deviceType =
+    HAL_CANDeviceType::HAL_CAN_Dev_kPneumatics;
+
+static constexpr int32_t kDefaultControlPeriod = 20;
+// static constexpr uint8_t kDefaultSensorMask = (1 <<
+// HAL_REV_PHSENSOR_DIGITAL);
+static constexpr uint8_t kDefaultCompressorDuty = 255;
+static constexpr uint8_t kDefaultPressureTarget = 120;
+static constexpr uint8_t kDefaultPressureHysteresis = 60;
+
+#define HAL_REV_MAX_PULSE_TIME 65534
+#define HAL_REV_MAX_PRESSURE_TARGET 120
+#define HAL_REV_MAX_PRESSURE_HYSTERESIS HAL_REV_MAX_PRESSURE_TARGET
+
+static constexpr uint32_t APIFromExtId(uint32_t extId) {
+  return (extId >> 6) & 0x3FF;
+}
+
+static constexpr uint32_t PH_SET_ALL_FRAME_API =
+    APIFromExtId(PH_SET_ALL_FRAME_ID);
+static constexpr uint32_t PH_PULSE_ONCE_FRAME_API =
+    APIFromExtId(PH_PULSE_ONCE_FRAME_ID);
+static constexpr uint32_t PH_STATUS0_FRAME_API =
+    APIFromExtId(PH_STATUS0_FRAME_ID);
+static constexpr uint32_t PH_STATUS1_FRAME_API =
+    APIFromExtId(PH_STATUS1_FRAME_ID);
+
+static constexpr int32_t kPHFrameStatus0Timeout = 50;
+static constexpr int32_t kPHFrameStatus1Timeout = 50;
+
+namespace {
+
+struct REV_PHObj {
+  int32_t controlPeriod;
+  PH_set_all_t desiredSolenoidsState;
+  wpi::mutex solenoidLock;
+  HAL_CANHandle hcan;
+  std::string previousAllocation;
+};
+
+}  // namespace
+
+static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, 63,
+                             HAL_HandleEnum::REVPH>* REVPHHandles;
+
+namespace hal::init {
+void InitializeREVPH() {
+  static IndexedHandleResource<HAL_REVPHHandle, REV_PHObj, kNumREVPHModules,
+                               HAL_HandleEnum::REVPH>
+      rH;
+  REVPHHandles = &rH;
+}
+}  // namespace hal::init
+
+static PH_status0_t HAL_REV_ReadPHStatus0(HAL_CANHandle hcan, int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PH_status0_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PH_STATUS0_FRAME_API, packedData, &length,
+                           &timestamp, kPHFrameStatus0Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PH_status0_unpack(&result, packedData, PH_STATUS0_LENGTH);
+
+  return result;
+}
+
+static PH_status1_t HAL_REV_ReadPHStatus1(HAL_CANHandle hcan, int32_t* status) {
+  uint8_t packedData[8] = {0};
+  int32_t length = 0;
+  uint64_t timestamp = 0;
+  PH_status1_t result = {};
+
+  HAL_ReadCANPacketTimeout(hcan, PH_STATUS1_FRAME_API, packedData, &length,
+                           &timestamp, kPHFrameStatus1Timeout * 2, status);
+
+  if (*status != 0) {
+    return result;
+  }
+
+  PH_status1_unpack(&result, packedData, PH_STATUS1_LENGTH);
+
+  return result;
+}
+
+enum REV_SolenoidState {
+  kSolenoidDisabled = 0,
+  kSolenoidEnabled,
+  kSolenoidControlledViaPulse
+};
+
+static void HAL_REV_UpdateDesiredPHSolenoidState(REV_PHObj* hph,
+                                                 int32_t solenoid,
+                                                 REV_SolenoidState state) {
+  switch (solenoid) {
+    case 0:
+      hph->desiredSolenoidsState.channel_0 = state;
+      break;
+    case 1:
+      hph->desiredSolenoidsState.channel_1 = state;
+      break;
+    case 2:
+      hph->desiredSolenoidsState.channel_2 = state;
+      break;
+    case 3:
+      hph->desiredSolenoidsState.channel_3 = state;
+      break;
+    case 4:
+      hph->desiredSolenoidsState.channel_4 = state;
+      break;
+    case 5:
+      hph->desiredSolenoidsState.channel_5 = state;
+      break;
+    case 6:
+      hph->desiredSolenoidsState.channel_6 = state;
+      break;
+    case 7:
+      hph->desiredSolenoidsState.channel_7 = state;
+      break;
+    case 8:
+      hph->desiredSolenoidsState.channel_8 = state;
+      break;
+    case 9:
+      hph->desiredSolenoidsState.channel_9 = state;
+      break;
+    case 10:
+      hph->desiredSolenoidsState.channel_10 = state;
+      break;
+    case 11:
+      hph->desiredSolenoidsState.channel_11 = state;
+      break;
+    case 12:
+      hph->desiredSolenoidsState.channel_12 = state;
+      break;
+    case 13:
+      hph->desiredSolenoidsState.channel_13 = state;
+      break;
+    case 14:
+      hph->desiredSolenoidsState.channel_14 = state;
+      break;
+    case 15:
+      hph->desiredSolenoidsState.channel_15 = state;
+      break;
+  }
+}
+
+static void HAL_REV_SendSolenoidsState(REV_PHObj* hph, int32_t* status) {
+  uint8_t packedData[PH_SET_ALL_LENGTH] = {0};
+  PH_set_all_pack(packedData, &(hph->desiredSolenoidsState), PH_SET_ALL_LENGTH);
+  HAL_WriteCANPacketRepeating(hph->hcan, packedData, PH_SET_ALL_LENGTH,
+                              PH_SET_ALL_FRAME_API, hph->controlPeriod, status);
+}
+
+static HAL_Bool HAL_REV_CheckPHPulseTime(int32_t time) {
+  return ((time > 0) && (time <= HAL_REV_MAX_PULSE_TIME)) ? 1 : 0;
+}
+
+HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
+                                    const char* allocationLocation,
+                                    int32_t* status) {
+  hal::init::CheckInit();
+  if (!HAL_CheckREVPHModuleNumber(module)) {
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
+                                     kNumREVPHModules, module);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_REVPHHandle handle;
+  auto hph = REVPHHandles->Allocate(module, &handle, status);
+  if (*status != 0) {
+    if (hph) {
+      hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module,
+                                           hph->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
+                                       kNumREVPHModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  HAL_CANHandle hcan =
+      HAL_InitializeCAN(manufacturer, module, deviceType, status);
+
+  if (*status != 0) {
+    REVPHHandles->Free(handle);
+    return HAL_kInvalidHandle;
+  }
+
+  hph->previousAllocation = allocationLocation ? allocationLocation : "";
+  hph->hcan = hcan;
+  hph->controlPeriod = kDefaultControlPeriod;
+
+  // Start closed-loop compressor control by starting solenoid state updates
+  HAL_REV_SendSolenoidsState(hph.get(), status);
+
+  return handle;
+}
+
+void HAL_FreeREVPH(HAL_REVPHHandle handle) {
+  auto hph = REVPHHandles->Get(handle);
+  if (hph == nullptr)
+    return;
+
+  HAL_CleanCAN(hph->hcan);
+
+  REVPHHandles->Free(handle);
+}
+
+HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) {
+  return module >= 1 && module < kNumREVPDHModules;
+}
+
+HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) {
+  return channel >= 0 && channel < kNumREVPHChannels;
+}
+
+HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status);
+
+  if (*status != 0) {
+    return false;
+  }
+
+  return status0.compressor_on;
+}
+
+void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled,
+                                   int32_t* status) {
+  // TODO
+}
+
+HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle,
+                                       int32_t* status) {
+  return false;  // TODO
+}
+
+HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status);
+
+  if (*status != 0) {
+    return false;
+  }
+
+  return status0.digital_sensor;
+}
+
+double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PH_status1_t status1 = HAL_REV_ReadPHStatus1(ph->hcan, status);
+
+  if (*status != 0) {
+    return 0;
+  }
+
+  return PH_status1_compressor_current_decode(status1.compressor_current);
+}
+
+double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel,
+                                  int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  if (channel < 0 || channel > 1) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid REV Analog Index", 0, 2,
+                                     channel);
+    return 0;
+  }
+
+  PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status);
+
+  if (*status != 0) {
+    return 0;
+  }
+
+  if (channel == 1) {
+    return PH_status0_analog_0_decode(status0.analog_0);
+  }
+  return PH_status0_analog_1_decode(status0.analog_1);
+}
+
+int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  PH_status0_t status0 = HAL_REV_ReadPHStatus0(ph->hcan, status);
+
+  if (*status != 0) {
+    return 0;
+  }
+
+  uint32_t result = status0.channel_0;
+  result |= status0.channel_1 << 1;
+  result |= status0.channel_2 << 2;
+  result |= status0.channel_3 << 3;
+  result |= status0.channel_4 << 4;
+  result |= status0.channel_5 << 5;
+  result |= status0.channel_6 << 6;
+  result |= status0.channel_7 << 7;
+  result |= status0.channel_8 << 8;
+  result |= status0.channel_9 << 9;
+  result |= status0.channel_10 << 10;
+  result |= status0.channel_11 << 11;
+  result |= status0.channel_12 << 12;
+  result |= status0.channel_13 << 13;
+  result |= status0.channel_14 << 14;
+  result |= status0.channel_15 << 15;
+
+  return result;
+}
+
+void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
+                           int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  std::scoped_lock lock{ph->solenoidLock};
+  for (int solenoid = 0; solenoid < kNumREVPHChannels; solenoid++) {
+    if (mask & (1 << solenoid)) {
+      // The mask bit for the solenoid is set, so we update the solenoid state
+      REV_SolenoidState desiredSolenoidState =
+          values & (1 << solenoid) ? kSolenoidEnabled : kSolenoidDisabled;
+      HAL_REV_UpdateDesiredPHSolenoidState(ph.get(), solenoid,
+                                           desiredSolenoidState);
+    }
+  }
+  HAL_REV_SendSolenoidsState(ph.get(), status);
+}
+
+void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
+                          int32_t* status) {
+  auto ph = REVPHHandles->Get(handle);
+  if (ph == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  if (index >= kNumREVPHChannels || index < 0) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Only [0-15] are valid index values. Requested {}", index));
+    return;
+  }
+
+  if (!HAL_REV_CheckPHPulseTime(durMs)) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Time not within expected range [0-65534]. Requested {}",
+                    durMs));
+    return;
+  }
+
+  {
+    std::scoped_lock lock{ph->solenoidLock};
+    HAL_REV_UpdateDesiredPHSolenoidState(ph.get(), index,
+                                         kSolenoidControlledViaPulse);
+    HAL_REV_SendSolenoidsState(ph.get(), status);
+  }
+
+  if (*status != 0) {
+    return;
+  }
+
+  PH_pulse_once_t pulse = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+  pulse.pulse_length_ms = durMs;
+
+  // Specify which solenoid should be pulsed
+  // The protocol supports specifying any number of solenoids to be pulsed at
+  // the same time, should that functionality be exposed to users in the future.
+  switch (index) {
+    case 0:
+      pulse.channel_0 = true;
+      break;
+    case 1:
+      pulse.channel_1 = true;
+      break;
+    case 2:
+      pulse.channel_2 = true;
+      break;
+    case 3:
+      pulse.channel_3 = true;
+      break;
+    case 4:
+      pulse.channel_4 = true;
+      break;
+    case 5:
+      pulse.channel_5 = true;
+      break;
+    case 6:
+      pulse.channel_6 = true;
+      break;
+    case 7:
+      pulse.channel_7 = true;
+      break;
+    case 8:
+      pulse.channel_8 = true;
+      break;
+    case 9:
+      pulse.channel_9 = true;
+      break;
+    case 10:
+      pulse.channel_10 = true;
+      break;
+    case 11:
+      pulse.channel_11 = true;
+      break;
+    case 12:
+      pulse.channel_12 = true;
+      break;
+    case 13:
+      pulse.channel_13 = true;
+      break;
+    case 14:
+      pulse.channel_14 = true;
+      break;
+    case 15:
+      pulse.channel_15 = true;
+      break;
+  }
+
+  // Send pulse command
+  uint8_t packedData[PH_PULSE_ONCE_LENGTH] = {0};
+  PH_pulse_once_pack(packedData, &pulse, PH_PULSE_ONCE_LENGTH);
+  HAL_WriteCANPacket(ph->hcan, packedData, PH_PULSE_ONCE_LENGTH,
+                     PH_PULSE_ONCE_FRAME_API, status);
+}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Relay.cpp b/third_party/allwpilib/hal/src/main/native/athena/Relay.cpp
index 4dffa36..a01dd9d 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Relay.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Relay.cpp
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Relay.h"
 
+#include <string>
+
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
 
@@ -19,6 +19,7 @@
 struct Relay {
   uint8_t channel;
   bool fwd;
+  std::string previousAllocation;
 };
 
 }  // namespace
@@ -29,46 +30,54 @@
 // Create a mutex to protect changes to the relay values
 static wpi::mutex digitalRelayMutex;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeRelay() {
   static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
                                HAL_HandleEnum::Relay>
       rH;
   relayHandles = &rH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
   initializeDigital(status);
 
-  if (*status != 0) return HAL_kInvalidHandle;
-
-  int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (*status != 0) {
     return HAL_kInvalidHandle;
   }
 
-  if (!fwd) channel += kNumRelayHeaders;  // add 4 to reverse channels
-
-  auto handle = relayHandles->Allocate(channel, status);
-
-  if (*status != 0)
-    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = relayHandles->Get(handle);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumRelayChannels) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
+                                     kNumRelayChannels, channel);
     return HAL_kInvalidHandle;
   }
 
   if (!fwd) {
+    channel += kNumRelayHeaders;  // add 4 to reverse channels
+  }
+
+  HAL_RelayHandle handle;
+  auto port = relayHandles->Allocate(channel, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Relay", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
+                                       kNumRelayChannels, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  if (!fwd) {
     // Subtract number of headers to put channel in range
     channel -= kNumRelayHeaders;
 
@@ -78,6 +87,7 @@
   }
 
   port->channel = static_cast<uint8_t>(channel);
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
   return handle;
 }
 
@@ -108,7 +118,9 @@
     relays = relaySystem->readValue_Reverse(status);
   }
 
-  if (*status != 0) return;  // bad status read
+  if (*status != 0) {
+    return;  // bad status read
+  }
 
   if (on) {
     relays |= 1 << port->channel;
diff --git a/third_party/allwpilib/hal/src/main/native/athena/SPI.cpp b/third_party/allwpilib/hal/src/main/native/athena/SPI.cpp
index 977d447..86a3f98 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/SPI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/SPI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/SPI.h"
 
@@ -14,13 +11,15 @@
 
 #include <array>
 #include <atomic>
+#include <cstdio>
 #include <cstring>
 
+#include <fmt/format.h>
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "hal/DIO.h"
 #include "hal/HAL.h"
 #include "hal/handles/HandlesInternal.h"
@@ -54,17 +53,17 @@
   // SPI engine conflicts with any other chip selects on the same SPI device.
   // There are two SPI devices: one for ports 0-3 (onboard), the other for port
   // 4 (MXP).
-  if (!spiAutoRunning) return false;
+  if (!spiAutoRunning) {
+    return false;
+  }
   std::scoped_lock lock(spiAutoMutex);
   return (spiAutoPort >= 0 && spiAutoPort <= 3 && port >= 0 && port <= 3) ||
          (spiAutoPort == 4 && port == 4);
 }
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSPI() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -73,19 +72,21 @@
   if (spiPortCount.fetch_add(1) == 0) {
     // Have not been initialized yet
     initializeDigital(status);
-    if (*status != 0) return;
+    if (*status != 0) {
+      return;
+    }
     // MISO
     if ((digitalHandles[3] = HAL_InitializeDIOPort(createPortHandleForSPI(29),
-                                                   false, status)) ==
+                                                   false, nullptr, status)) ==
         HAL_kInvalidHandle) {
-      std::printf("Failed to allocate DIO 29 (MISO)\n");
+      std::puts("Failed to allocate DIO 29 (MISO)");
       return;
     }
     // MOSI
     if ((digitalHandles[4] = HAL_InitializeDIOPort(createPortHandleForSPI(30),
-                                                   false, status)) ==
+                                                   false, nullptr, status)) ==
         HAL_kInvalidHandle) {
-      std::printf("Failed to allocate DIO 30 (MOSI)\n");
+      std::puts("Failed to allocate DIO 30 (MOSI)");
       HAL_FreeDIOPort(digitalHandles[3]);  // free the first port allocated
       return;
     }
@@ -104,20 +105,28 @@
   hal::init::CheckInit();
   if (port < 0 || port >= kSpiMaxHandles) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Serial port must be between 0 and {}. Requested {}",
+                    kSpiMaxHandles, static_cast<int>(port)));
     return;
   }
 
   int handle;
-  if (HAL_GetSPIHandle(port) != 0) return;
+  if (HAL_GetSPIHandle(port) != 0) {
+    return;
+  }
   switch (port) {
     case HAL_SPI_kOnboardCS0:
       CommonSPIPortInit(status);
-      if (*status != 0) return;
+      if (*status != 0) {
+        return;
+      }
       // CS0 is not a DIO port, so nothing to allocate
       handle = open("/dev/spidev0.0", O_RDWR);
       if (handle < 0) {
-        std::printf("Failed to open SPI port %d: %s\n", port,
-                    std::strerror(errno));
+        fmt::print("Failed to open SPI port {}: {}\n", port,
+                   std::strerror(errno));
         CommonSPIPortFree();
         return;
       }
@@ -125,19 +134,21 @@
       break;
     case HAL_SPI_kOnboardCS1:
       CommonSPIPortInit(status);
-      if (*status != 0) return;
+      if (*status != 0) {
+        return;
+      }
       // CS1, Allocate
       if ((digitalHandles[0] = HAL_InitializeDIOPort(createPortHandleForSPI(26),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        std::printf("Failed to allocate DIO 26 (CS1)\n");
+        std::puts("Failed to allocate DIO 26 (CS1)");
         CommonSPIPortFree();
         return;
       }
       handle = open("/dev/spidev0.1", O_RDWR);
       if (handle < 0) {
-        std::printf("Failed to open SPI port %d: %s\n", port,
-                    std::strerror(errno));
+        fmt::print("Failed to open SPI port {}: {}\n", port,
+                   std::strerror(errno));
         CommonSPIPortFree();
         HAL_FreeDIOPort(digitalHandles[0]);
         return;
@@ -146,19 +157,21 @@
       break;
     case HAL_SPI_kOnboardCS2:
       CommonSPIPortInit(status);
-      if (*status != 0) return;
+      if (*status != 0) {
+        return;
+      }
       // CS2, Allocate
       if ((digitalHandles[1] = HAL_InitializeDIOPort(createPortHandleForSPI(27),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        std::printf("Failed to allocate DIO 27 (CS2)\n");
+        std::puts("Failed to allocate DIO 27 (CS2)");
         CommonSPIPortFree();
         return;
       }
       handle = open("/dev/spidev0.2", O_RDWR);
       if (handle < 0) {
-        std::printf("Failed to open SPI port %d: %s\n", port,
-                    std::strerror(errno));
+        fmt::print("Failed to open SPI port {}: {}\n", port,
+                   std::strerror(errno));
         CommonSPIPortFree();
         HAL_FreeDIOPort(digitalHandles[1]);
         return;
@@ -167,19 +180,21 @@
       break;
     case HAL_SPI_kOnboardCS3:
       CommonSPIPortInit(status);
-      if (*status != 0) return;
+      if (*status != 0) {
+        return;
+      }
       // CS3, Allocate
       if ((digitalHandles[2] = HAL_InitializeDIOPort(createPortHandleForSPI(28),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        std::printf("Failed to allocate DIO 28 (CS3)\n");
+        std::puts("Failed to allocate DIO 28 (CS3)");
         CommonSPIPortFree();
         return;
       }
       handle = open("/dev/spidev0.3", O_RDWR);
       if (handle < 0) {
-        std::printf("Failed to open SPI port %d: %s\n", port,
-                    std::strerror(errno));
+        fmt::print("Failed to open SPI port {}: {}\n", port,
+                   std::strerror(errno));
         CommonSPIPortFree();
         HAL_FreeDIOPort(digitalHandles[2]);
         return;
@@ -188,32 +203,34 @@
       break;
     case HAL_SPI_kMXP:
       initializeDigital(status);
-      if (*status != 0) return;
+      if (*status != 0) {
+        return;
+      }
       if ((digitalHandles[5] = HAL_InitializeDIOPort(createPortHandleForSPI(14),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        wpi::outs() << "Failed to allocate DIO 14\n";
+        std::puts("Failed to allocate DIO 14");
         return;
       }
       if ((digitalHandles[6] = HAL_InitializeDIOPort(createPortHandleForSPI(15),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        wpi::outs() << "Failed to allocate DIO 15\n";
+        std::puts("Failed to allocate DIO 15");
         HAL_FreeDIOPort(digitalHandles[5]);  // free the first port allocated
         return;
       }
       if ((digitalHandles[7] = HAL_InitializeDIOPort(createPortHandleForSPI(16),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        wpi::outs() << "Failed to allocate DIO 16\n";
+        std::puts("Failed to allocate DIO 16");
         HAL_FreeDIOPort(digitalHandles[5]);  // free the first port allocated
         HAL_FreeDIOPort(digitalHandles[6]);  // free the second port allocated
         return;
       }
       if ((digitalHandles[8] = HAL_InitializeDIOPort(createPortHandleForSPI(17),
-                                                     false, status)) ==
+                                                     false, nullptr, status)) ==
           HAL_kInvalidHandle) {
-        wpi::outs() << "Failed to allocate DIO 17\n";
+        std::puts("Failed to allocate DIO 17");
         HAL_FreeDIOPort(digitalHandles[5]);  // free the first port allocated
         HAL_FreeDIOPort(digitalHandles[6]);  // free the second port allocated
         HAL_FreeDIOPort(digitalHandles[7]);  // free the third port allocated
@@ -223,8 +240,8 @@
           digitalSystem->readEnableMXPSpecialFunction(status) | 0x00F0, status);
       handle = open("/dev/spidev1.0", O_RDWR);
       if (handle < 0) {
-        std::printf("Failed to open SPI port %d: %s\n", port,
-                    std::strerror(errno));
+        fmt::print("Failed to open SPI port {}: {}\n", port,
+                   std::strerror(errno));
         HAL_FreeDIOPort(digitalHandles[5]);  // free the first port allocated
         HAL_FreeDIOPort(digitalHandles[6]);  // free the second port allocated
         HAL_FreeDIOPort(digitalHandles[7]);  // free the third port allocated
@@ -235,6 +252,8 @@
       break;
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(
+          status, fmt::format("Invalid SPI port {}", static_cast<int>(port)));
       break;
   }
 }
@@ -245,7 +264,9 @@
     return -1;
   }
 
-  if (SPIInUseByAuto(port)) return -1;
+  if (SPIInUseByAuto(port)) {
+    return -1;
+  }
 
   struct spi_ioc_transfer xfer;
   std::memset(&xfer, 0, sizeof(xfer));
@@ -263,7 +284,9 @@
     return -1;
   }
 
-  if (SPIInUseByAuto(port)) return -1;
+  if (SPIInUseByAuto(port)) {
+    return -1;
+  }
 
   struct spi_ioc_transfer xfer;
   std::memset(&xfer, 0, sizeof(xfer));
@@ -279,7 +302,9 @@
     return -1;
   }
 
-  if (SPIInUseByAuto(port)) return -1;
+  if (SPIInUseByAuto(port)) {
+    return -1;
+  }
 
   struct spi_ioc_transfer xfer;
   std::memset(&xfer, 0, sizeof(xfer));
@@ -357,6 +382,10 @@
 void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {
   if (port < 0 || port >= kSpiMaxHandles) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Serial port must be between 0 and {}. Requested {}",
+                    kSpiMaxHandles, static_cast<int>(port)));
     return;
   }
 
@@ -372,6 +401,10 @@
 void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {
   if (port < 0 || port >= kSpiMaxHandles) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Serial port must be between 0 and {}. Requested {}",
+                    kSpiMaxHandles, static_cast<int>(port)));
     return;
   }
 
@@ -436,6 +469,10 @@
 void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {
   if (port < 0 || port >= kSpiMaxHandles) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Serial port must be between 0 and {}. Requested {}",
+                    kSpiMaxHandles, static_cast<int>(port)));
     return;
   }
 
@@ -467,11 +504,17 @@
 void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {
   if (port < 0 || port >= kSpiMaxHandles) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format("Serial port must be between 0 and {}. Requested {}",
+                    kSpiMaxHandles, static_cast<int>(port)));
     return;
   }
 
   std::scoped_lock lock(spiAutoMutex);
-  if (spiAutoPort != port) return;
+  if (spiAutoPort != port) {
+    return;
+  }
   spiAutoPort = kSpiMaxHandles;
 
   // disable by setting to internal clock and setting rate=0
@@ -573,11 +616,21 @@
   // just hard-code it here.
   if (dataSize < 0 || dataSize > 23) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format(
+            "Data size must be between 0 and 32 inclusive. Requested {}",
+            dataSize));
     return;
   }
 
   if (zeroSize < 0 || zeroSize >= 128) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format(
+            "Zero size must be between 0 and 127 inclusive. Requested {}",
+            zeroSize));
     return;
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/SerialPort.cpp b/third_party/allwpilib/hal/src/main/native/athena/SerialPort.cpp
index 7ef9b70..374986b 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/SerialPort.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/SerialPort.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/SerialPort.h"
 
@@ -22,6 +19,9 @@
 #include <string>
 #include <thread>
 
+#include <fmt/format.h>
+
+#include "HALInternal.h"
 #include "hal/cpp/SerialHelper.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
@@ -44,16 +44,14 @@
                       HAL_HandleEnum::SerialPort>* serialPortHandles;
 }  // namespace hal
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSerialPort() {
   static IndexedHandleResource<HAL_SerialPortHandle, SerialPort, 4,
                                HAL_HandleEnum::SerialPort>
       spH;
   serialPortHandles = &spH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 using namespace hal;
 
@@ -75,22 +73,20 @@
 HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port,
                                                     const char* portName,
                                                     int32_t* status) {
-  auto handle = serialPortHandles->Allocate(static_cast<int16_t>(port), status);
+  HAL_SerialPortHandle handle;
+  auto serialPort =
+      serialPortHandles->Allocate(static_cast<int16_t>(port), &handle, status);
 
   if (*status != 0) {
     return HAL_kInvalidHandle;
   }
 
-  auto serialPort = serialPortHandles->Get(handle);
-
-  if (serialPort == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
   serialPort->portId = open(portName, O_RDWR | O_NOCTTY);
   if (serialPort->portId < 0) {
     *status = errno;
+    if (*status == EACCES) {
+      *status = HAL_CONSOLE_OUT_ENABLED_ERROR;
+    }
     serialPortHandles->Free(handle);
     return HAL_kInvalidHandle;
   }
@@ -193,6 +189,7 @@
     BAUDCASE(4000000)
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, fmt::format("Invalid BaudRate: {}", baud));
       return;
   }
   int err = cfsetospeed(&port->tty, static_cast<speed_t>(port->baudRate));
@@ -235,6 +232,7 @@
       break;
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, fmt::format("Invalid data bits: {}", bits));
       return;
   }
 
@@ -282,6 +280,7 @@
       break;
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, fmt::format("Invalid parity bits: {}", parity));
       return;
   }
 
@@ -309,6 +308,7 @@
       break;
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, fmt::format("Invalid stop bits: {}", stopBits));
       return;
   }
 
@@ -344,6 +344,7 @@
       break;
     default:
       *status = PARAMETER_OUT_OF_RANGE;
+      hal::SetLastError(status, fmt::format("Invalid fc bits: {}", flow));
       return;
   }
 
@@ -417,7 +418,9 @@
 int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count,
                        int32_t* status) {
   // Don't do anything if 0 bytes were requested
-  if (count == 0) return 0;
+  if (count == 0) {
+    return 0;
+  }
 
   auto port = serialPortHandles->Get(handle);
   if (!port) {
diff --git a/third_party/allwpilib/hal/src/main/native/athena/SimDevice.cpp b/third_party/allwpilib/hal/src/main/native/athena/SimDevice.cpp
index 94a65d4..3a15f46 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/SimDevice.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/SimDevice.cpp
@@ -1,32 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/SimDevice.h"
 
 extern "C" {
 
-HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) { return 0; }
+HAL_SimDeviceHandle HAL_CreateSimDevice(const char* name) {
+  return 0;
+}
 
 void HAL_FreeSimDevice(HAL_SimDeviceHandle handle) {}
 
 HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
-                                      const char* name, HAL_Bool readonly,
+                                      const char* name, int32_t direction,
                                       const struct HAL_Value* initialValue) {
   return 0;
 }
 
 HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
-                                          const char* name, HAL_Bool readonly,
+                                          const char* name, int32_t direction,
                                           int32_t numOptions,
                                           const char** options,
                                           int32_t initialValue) {
   return 0;
 }
 
+HAL_SimValueHandle HAL_CreateSimValueEnumDouble(
+    HAL_SimDeviceHandle device, const char* name, int32_t direction,
+    int32_t numOptions, const char** options, const double* optionValues,
+    int32_t initialValue) {
+  return 0;
+}
+
 void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value) {
   value->type = HAL_UNASSIGNED;
 }
@@ -34,6 +40,8 @@
 void HAL_SetSimValue(HAL_SimValueHandle handle, const struct HAL_Value* value) {
 }
 
+void HAL_ResetSimValue(HAL_SimValueHandle handle) {}
+
 hal::SimDevice::SimDevice(const char* name, int index) {}
 
 hal::SimDevice::SimDevice(const char* name, int index, int channel) {}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Solenoid.cpp b/third_party/allwpilib/hal/src/main/native/athena/Solenoid.cpp
deleted file mode 100644
index ab563b3..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/Solenoid.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/Solenoid.h"
-
-#include "HALInitializer.h"
-#include "PCMInternal.h"
-#include "PortsInternal.h"
-#include "ctre/PCM.h"
-#include "hal/Errors.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/handles/IndexedHandleResource.h"
-
-namespace {
-
-struct Solenoid {
-  uint8_t module;
-  uint8_t channel;
-};
-
-}  // namespace
-
-using namespace hal;
-
-static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
-                             kNumPCMModules * kNumSolenoidChannels,
-                             HAL_HandleEnum::Solenoid>* solenoidHandles;
-
-namespace hal {
-namespace init {
-void InitializeSolenoid() {
-  static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
-                               kNumPCMModules * kNumSolenoidChannels,
-                               HAL_HandleEnum::Solenoid>
-      sH;
-  solenoidHandles = &sH;
-}
-}  // namespace init
-}  // namespace hal
-
-extern "C" {
-
-HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
-                                              int32_t* status) {
-  hal::init::CheckInit();
-  int16_t channel = getPortHandleChannel(portHandle);
-  int16_t module = getPortHandleModule(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
-  // initializePCM will check the module
-  if (!HAL_CheckSolenoidChannel(channel)) {
-    *status = RESOURCE_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
-  }
-
-  initializePCM(module, status);
-  if (*status != 0) {
-    return HAL_kInvalidHandle;
-  }
-
-  auto handle = solenoidHandles->Allocate(
-      module * kNumSolenoidChannels + channel, status);
-  if (*status != 0) {
-    return HAL_kInvalidHandle;
-  }
-  auto solenoidPort = solenoidHandles->Get(handle);
-  if (solenoidPort == nullptr) {  // would only occur on thread issues
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-  solenoidPort->module = static_cast<uint8_t>(module);
-  solenoidPort->channel = static_cast<uint8_t>(channel);
-
-  return handle;
-}
-
-void HAL_FreeSolenoidPort(HAL_SolenoidHandle solenoidPortHandle) {
-  solenoidHandles->Free(solenoidPortHandle);
-}
-
-HAL_Bool HAL_CheckSolenoidModule(int32_t module) {
-  return module < kNumPCMModules && module >= 0;
-}
-
-HAL_Bool HAL_CheckSolenoidChannel(int32_t channel) {
-  return channel < kNumSolenoidChannels && channel >= 0;
-}
-
-HAL_Bool HAL_GetSolenoid(HAL_SolenoidHandle solenoidPortHandle,
-                         int32_t* status) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-  bool value;
-
-  *status = PCM_modules[port->module]->GetSolenoid(port->channel, value);
-
-  return value;
-}
-
-int32_t HAL_GetAllSolenoids(int32_t module, int32_t* status) {
-  if (!checkPCMInit(module, status)) return 0;
-  uint8_t value;
-
-  *status = PCM_modules[module]->GetAllSolenoids(value);
-
-  return value;
-}
-
-void HAL_SetSolenoid(HAL_SolenoidHandle solenoidPortHandle, HAL_Bool value,
-                     int32_t* status) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  *status = PCM_modules[port->module]->SetSolenoid(port->channel, value);
-}
-
-void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status) {
-  if (!checkPCMInit(module, status)) return;
-
-  *status = PCM_modules[module]->SetAllSolenoids(state);
-}
-
-int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status) {
-  if (!checkPCMInit(module, status)) return 0;
-  uint8_t value;
-
-  *status = PCM_modules[module]->GetSolenoidBlackList(value);
-
-  return value;
-}
-HAL_Bool HAL_GetPCMSolenoidVoltageStickyFault(int32_t module, int32_t* status) {
-  if (!checkPCMInit(module, status)) return 0;
-  bool value;
-
-  *status = PCM_modules[module]->GetSolenoidStickyFault(value);
-
-  return value;
-}
-HAL_Bool HAL_GetPCMSolenoidVoltageFault(int32_t module, int32_t* status) {
-  if (!checkPCMInit(module, status)) return false;
-  bool value;
-
-  *status = PCM_modules[module]->GetSolenoidFault(value);
-
-  return value;
-}
-void HAL_ClearAllPCMStickyFaults(int32_t module, int32_t* status) {
-  if (!checkPCMInit(module, status)) return;
-
-  *status = PCM_modules[module]->ClearStickyFaults();
-}
-
-void HAL_SetOneShotDuration(HAL_SolenoidHandle solenoidPortHandle,
-                            int32_t durMS, int32_t* status) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  *status =
-      PCM_modules[port->module]->SetOneShotDurationMs(port->channel, durMS);
-}
-
-void HAL_FireOneShot(HAL_SolenoidHandle solenoidPortHandle, int32_t* status) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  *status = PCM_modules[port->module]->FireOneShotSolenoid(port->channel);
-}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/Threads.cpp b/third_party/allwpilib/hal/src/main/native/athena/Threads.cpp
index 95d1f59..aa55b56 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/Threads.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/Threads.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Threads.h"
 
@@ -12,11 +9,9 @@
 
 #include "hal/Errors.h"
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeThreads() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -37,8 +32,8 @@
     return sch.sched_priority;
   } else {
     *isRealTime = false;
-    // 0 is the only suppored priority for non-realtime, so scale to 1
-    return 1;
+    // 0 is the only supported priority for non-real-time
+    return 0;
   }
 }
 
@@ -69,11 +64,13 @@
   int policy;
   pthread_getschedparam(*reinterpret_cast<const pthread_t*>(handle), &policy,
                         &sch);
-  if (scheduler == SCHED_FIFO || scheduler == SCHED_RR)
+  if (scheduler == SCHED_FIFO || scheduler == SCHED_RR) {
     sch.sched_priority = priority;
-  else
+  } else {
     // Only need to set 0 priority for non RT thread
     sch.sched_priority = 0;
+  }
+
   if (pthread_setschedparam(*reinterpret_cast<const pthread_t*>(handle),
                             scheduler, &sch)) {
     *status = HAL_THREAD_PRIORITY_ERROR;
diff --git a/third_party/allwpilib/hal/src/main/native/athena/cpp/SerialHelper.cpp b/third_party/allwpilib/hal/src/main/native/athena/cpp/SerialHelper.cpp
index 2156147..3831769 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/cpp/SerialHelper.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/cpp/SerialHelper.cpp
@@ -1,18 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/cpp/SerialHelper.h"
 
 #include <algorithm>
 #include <cstdio>
 #include <cstring>
+#include <string_view>
 
-#include <wpi/FileSystem.h>
-#include <wpi/StringRef.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
 
 #include "hal/Errors.h"
 #include "visa/visa.h"
@@ -56,7 +54,7 @@
     return "";
     // Error
   } else {
-    return m_visaResource[visaIndex].str();
+    return std::string{m_visaResource[visaIndex].str()};
   }
 }
 
@@ -83,7 +81,7 @@
     return "";
     // Error
   } else {
-    return m_osResource[osIndex].str();
+    return std::string{m_osResource[osIndex].str()};
   }
 }
 
@@ -139,8 +137,8 @@
   std::sort(m_sortedHubPath.begin(), m_sortedHubPath.end(),
             [](const wpi::SmallVectorImpl<char>& lhs,
                const wpi::SmallVectorImpl<char>& rhs) -> int {
-              wpi::StringRef lhsRef(lhs.begin(), lhs.size());
-              wpi::StringRef rhsRef(rhs.begin(), rhs.size());
+              std::string_view lhsRef(lhs.begin(), lhs.size());
+              std::string_view rhsRef(rhs.begin(), rhs.size());
               return lhsRef.compare(rhsRef);
             });
 }
@@ -150,15 +148,15 @@
   wpi::SmallVector<wpi::SmallString<16>, 4> sortedVec;
   for (auto& str : m_sortedHubPath) {
     for (size_t i = 0; i < m_unsortedHubPath.size(); i++) {
-      if (wpi::StringRef{m_unsortedHubPath[i].begin(),
-                         m_unsortedHubPath[i].size()}
-              .equals(wpi::StringRef{str.begin(), str.size()})) {
+      if (wpi::equals(std::string_view{m_unsortedHubPath[i].begin(),
+                                       m_unsortedHubPath[i].size()},
+                      std::string_view{str.begin(), str.size()})) {
         sortedVec.push_back(vec[i]);
         break;
       }
     }
   }
-  vec = sortedVec;
+  vec.swap(sortedVec);
 }
 
 void SerialHelper::QueryHubPaths(int32_t* status) {
@@ -194,73 +192,82 @@
     // Open the resource, grab its interface name, and close it.
     ViSession vSession;
     *status = viOpen(m_resourceHandle, desc, VI_NULL, VI_NULL, &vSession);
-    if (*status < 0) goto done;
+    if (*status < 0)
+      goto done;
     *status = 0;
 
     *status = viGetAttribute(vSession, VI_ATTR_INTF_INST_NAME, &osName);
     // Ignore an error here, as we want to close the session on an error
     // Use a separate close variable so we can check
     ViStatus closeStatus = viClose(vSession);
-    if (*status < 0) goto done;
-    if (closeStatus < 0) goto done;
+    if (*status < 0)
+      goto done;
+    if (closeStatus < 0)
+      goto done;
     *status = 0;
 
     // split until (/dev/
-    wpi::StringRef devNameRef = wpi::StringRef{osName}.split("(/dev/").second;
+    std::string_view devNameRef = wpi::split(osName, "(/dev/").second;
     // String not found, continue
-    if (devNameRef.equals("")) continue;
+    if (wpi::equals(devNameRef, ""))
+      continue;
 
     // Split at )
-    wpi::StringRef matchString = devNameRef.split(')').first;
-    if (matchString.equals(devNameRef)) continue;
+    std::string_view matchString = wpi::split(devNameRef, ')').first;
+    if (wpi::equals(matchString, devNameRef))
+      continue;
 
     // Search directories to get a list of system accessors
     // The directories we need are not symbolic, so we can safely
     // disable symbolic links.
     std::error_code ec;
-    for (auto p = wpi::sys::fs::recursive_directory_iterator(
-             "/sys/devices/soc0", ec, false);
-         p != wpi::sys::fs::recursive_directory_iterator(); p.increment(ec)) {
-      if (ec) break;
-      wpi::StringRef path{p->path()};
-      if (path.find("amba") == wpi::StringRef::npos) continue;
-      if (path.find("usb") == wpi::StringRef::npos) continue;
-      if (path.find(matchString) == wpi::StringRef::npos) continue;
+    for (auto& p : fs::recursive_directory_iterator("/sys/devices/soc0", ec)) {
+      if (ec)
+        break;
+      std::string path = p.path();
+      if (path.find("amba") == std::string::npos)
+        continue;
+      if (path.find("usb") == std::string::npos)
+        continue;
+      if (path.find(matchString) == std::string::npos)
+        continue;
 
-      wpi::SmallVector<wpi::StringRef, 16> pathSplitVec;
+      wpi::SmallVector<std::string_view, 16> pathSplitVec;
       // Split path into individual directories
-      path.split(pathSplitVec, '/', -1, false);
+      wpi::split(path, pathSplitVec, '/', -1, false);
 
       // Find each individual item index
       int findusb = -1;
       int findtty = -1;
       int findregex = -1;
       for (size_t i = 0; i < pathSplitVec.size(); i++) {
-        if (findusb == -1 && pathSplitVec[i].equals("usb1")) {
+        if (findusb == -1 && wpi::equals(pathSplitVec[i], "usb1")) {
           findusb = i;
         }
-        if (findtty == -1 && pathSplitVec[i].equals("tty")) {
+        if (findtty == -1 && wpi::equals(pathSplitVec[i], "tty")) {
           findtty = i;
         }
-        if (findregex == -1 && pathSplitVec[i].equals(matchString)) {
+        if (findregex == -1 && wpi::equals(pathSplitVec[i], matchString)) {
           findregex = i;
         }
       }
 
       // Get the index for our device
       int hubIndex = findtty;
-      if (findtty == -1) hubIndex = findregex;
+      if (findtty == -1)
+        hubIndex = findregex;
 
       int devStart = findusb + 1;
 
-      if (hubIndex < devStart) continue;
+      if (hubIndex < devStart)
+        continue;
 
       // Add our devices to our list
       m_unsortedHubPath.emplace_back(
-          wpi::StringRef{pathSplitVec[hubIndex - 2]});
+          std::string_view{pathSplitVec[hubIndex - 2]});
       m_visaResource.emplace_back(desc);
       m_osResource.emplace_back(
-          wpi::StringRef{osName}.split("(").second.split(")").first);
+          wpi::split(wpi::split(osName, "(").second, ")").first);
       break;
     }
   }
@@ -285,8 +292,9 @@
   if (portString.empty()) {
     for (size_t i = 0; i < 2; i++) {
       // Remove all used ports
-      auto idx = std::find(m_sortedHubPath.begin(), m_sortedHubPath.end(),
-                           m_usbNames[i]);
+      auto idx = std::find_if(
+          m_sortedHubPath.begin(), m_sortedHubPath.end(),
+          [&](const auto& s) { return wpi::equals(s, m_usbNames[i]); });
       if (idx != m_sortedHubPath.end()) {
         // found
         m_sortedHubPath.erase(idx);
@@ -321,7 +329,7 @@
   int retIndex = -1;
 
   for (size_t i = 0; i < m_sortedHubPath.size(); i++) {
-    if (m_sortedHubPath[i].equals(portString)) {
+    if (wpi::equals(m_sortedHubPath[i], portString)) {
       retIndex = i;
       break;
     }
diff --git a/third_party/allwpilib/hal/src/main/native/athena/ctre/CtreCanNode.cpp b/third_party/allwpilib/hal/src/main/native/athena/ctre/CtreCanNode.cpp
deleted file mode 100644
index 440bebd..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/ctre/CtreCanNode.cpp
+++ /dev/null
@@ -1,157 +0,0 @@
-#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
-
-#include "CtreCanNode.h"
-#include "FRC_NetworkCommunication/CANSessionMux.h"
-#include <string.h> // memset
-
-static const UINT32 kFullMessageIDMask = 0x1fffffff;
-
-CtreCanNode::CtreCanNode(UINT8 deviceNumber)
-{
-	_deviceNumber = deviceNumber;
-}
-CtreCanNode::~CtreCanNode()
-{
-}
-void CtreCanNode::RegisterRx(uint32_t arbId)
-{
-	/* no need to do anything, we just use new API to poll last received message */
-}
-/**
- * Schedule a CAN Frame for periodic transmit.
- * @param arbId 	CAN Frame Arbitration ID.  Set BIT31 for 11bit ids, otherwise we use 29bit ids.
- * @param periodMs	Period to transmit CAN frame.  Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
- * @param dlc 		Number of bytes to transmit (0 to 8).
- * @param initialFrame	Ptr to the frame data to schedule for transmitting.  Passing null will result
- *						in defaulting to zero data value.
- */
-void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame)
-{
-	int32_t status = 0;
-	if(dlc > 8)
-		dlc = 8;
-	txJob_t job = {0};
-	job.arbId = arbId;
-	job.periodMs = periodMs;
-	job.dlc = dlc;
-	if(initialFrame){
-		/* caller wants to specify original data */
-		memcpy(job.toSend, initialFrame, dlc);
-	}
-	_txJobs[arbId] = job;
-	FRC_NetworkCommunication_CANSessionMux_sendMessage(	job.arbId,
-														job.toSend,
-														job.dlc,
-														job.periodMs,
-														&status);
-}
-/**
- * Schedule a CAN Frame for periodic transmit.  Assume eight byte DLC and zero value for initial transmission.
- * @param arbId 	CAN Frame Arbitration ID.  Set BIT31 for 11bit ids, otherwise we use 29bit ids.
- * @param periodMs	Period to transmit CAN frame.  Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
- */
-void CtreCanNode::RegisterTx(uint32_t arbId, uint32_t periodMs)
-{
-	RegisterTx(arbId,periodMs, 8, 0);
-}
-/**
- * Remove a CAN frame Arbid to stop transmission.
- * @param arbId 	CAN Frame Arbitration ID.  Set BIT31 for 11bit ids, otherwise we use 29bit ids.
- */
-void CtreCanNode::UnregisterTx(uint32_t arbId)
-{
-	/* set period to zero */
-	ChangeTxPeriod(arbId, 0);
-	/* look and remove */
-	txJobs_t::iterator iter = _txJobs.find(arbId);
-	if(iter != _txJobs.end()) {
-		_txJobs.erase(iter);
-	}
-}
-static int64_t GetTimeMs() {
-	std::chrono::time_point < std::chrono::system_clock > now;
-	now = std::chrono::system_clock::now();
-	auto duration = now.time_since_epoch();
-	auto millis = std::chrono::duration_cast < std::chrono::milliseconds
-					> (duration).count();
-	return (int64_t) millis;
-}
-CTR_Code CtreCanNode::GetRx(uint32_t arbId,uint8_t * dataBytes, uint32_t timeoutMs)
-{
-	CTR_Code retval = CTR_OKAY;
-	int32_t status = 0;
-	uint8_t len = 0;
-	uint32_t timeStamp;
-	/* cap timeout at 999ms */
-	if(timeoutMs > 999)
-		timeoutMs = 999;
-	FRC_NetworkCommunication_CANSessionMux_receiveMessage(&arbId,kFullMessageIDMask,dataBytes,&len,&timeStamp,&status);
-	std::scoped_lock lock(_lck);
-	if(status == 0){
-		/* fresh update */
-		rxEvent_t & r = _rxRxEvents[arbId]; /* lookup entry or make a default new one with all zeroes */
-		r.time = GetTimeMs();
-		memcpy(r.bytes,  dataBytes,  8);	/* fill in databytes */
-	}else{
-		/* did not get the message */
-		rxRxEvents_t::iterator i = _rxRxEvents.find(arbId);
-		if(i == _rxRxEvents.end()){
-			/* we've never gotten this mesage */
-			retval = CTR_RxTimeout;
-			/* fill caller's buffer with zeros */
-			memset(dataBytes,0,8);
-		}else{
-			/* we've gotten this message before but not recently */
-			memcpy(dataBytes,i->second.bytes,8);
-			/* get the time now */
-			int64_t now = GetTimeMs(); /* get now */
-			/* how long has it been? */
-			int64_t temp = now - i->second.time; /* temp = now - last */
-			if (temp > ((int64_t) timeoutMs)) {
-					retval = CTR_RxTimeout;
-			} else {
-					/* our last update was recent enough */
-			}
-		}
-	}
-
-	return retval;
-}
-void CtreCanNode::FlushTx(uint32_t arbId)
-{
-	int32_t status = 0;
-	txJobs_t::iterator iter = _txJobs.find(arbId);
-	if(iter != _txJobs.end())
-		FRC_NetworkCommunication_CANSessionMux_sendMessage(	iter->second.arbId,
-															iter->second.toSend,
-															iter->second.dlc,
-															iter->second.periodMs,
-															&status);
-}
-/**
- * Change the transmit period of an already scheduled CAN frame.
- * This keeps the frame payload contents the same without caller having to perform
- * a read-modify-write.
- * @param arbId 	CAN Frame Arbitration ID.  Set BIT31 for 11bit ids, otherwise we use 29bit ids.
- * @param periodMs	Period to transmit CAN frame.  Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
- * @return true if scheduled job was found and updated, false if there was no preceding job for the specified arbID.
- */
-bool CtreCanNode::ChangeTxPeriod(uint32_t arbId, uint32_t periodMs)
-{
-	int32_t status = 0;
-	/* lookup the data bytes and period for this message */
-	txJobs_t::iterator iter = _txJobs.find(arbId);
-	if(iter != _txJobs.end()) {
-		/* modify th periodMs */
-		iter->second.periodMs = periodMs;
-		/* reinsert into scheduler with the same data bytes, only the period changed. */
-		FRC_NetworkCommunication_CANSessionMux_sendMessage(	iter->second.arbId,
-															iter->second.toSend,
-															iter->second.dlc,
-															iter->second.periodMs,
-															&status);
-		return true;
-	}
-	return false;
-}
-
diff --git a/third_party/allwpilib/hal/src/main/native/athena/ctre/CtreCanNode.h b/third_party/allwpilib/hal/src/main/native/athena/ctre/CtreCanNode.h
deleted file mode 100644
index ce2d6ac..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/ctre/CtreCanNode.h
+++ /dev/null
@@ -1,132 +0,0 @@
-#ifndef CtreCanNode_H_
-#define CtreCanNode_H_
-#include "ctre.h"				//BIT Defines + Typedefs
-#include <map>
-#include <wpi/mutex.h>
-class CtreCanNode
-{
-public:
-	CtreCanNode(UINT8 deviceNumber);
-    ~CtreCanNode();
-
-	UINT8 GetDeviceNumber()
-	{
-		return _deviceNumber;
-	}
-protected:
-
-
-	template <typename T> class txTask{
-		public:
-			uint32_t arbId;
-			T * toSend;
-			T * operator -> ()
-			{
-				return toSend;
-			}
-			T & operator*()
-			{
-				return *toSend;
-			}
-			bool IsEmpty()
-			{
-				if(toSend == 0)
-					return true;
-				return false;
-			}
-	};
-	template <typename T> class recMsg{
-		public:
-			uint32_t arbId;
-			uint8_t bytes[8];
-			CTR_Code err;
-			T * operator -> ()
-			{
-				return (T *)bytes;
-			}
-			T & operator*()
-			{
-				return *(T *)bytes;
-			}
-	};
-	UINT8 _deviceNumber;
-	void RegisterRx(uint32_t arbId);
-	/**
-	 * Schedule a CAN Frame for periodic transmit.  Assume eight byte DLC and zero value for initial transmission.
-	 * @param arbId 	CAN Frame Arbitration ID.  Set BIT31 for 11bit ids, otherwise we use 29bit ids.
-	 * @param periodMs	Period to transmit CAN frame.  Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
-	 */
-	void RegisterTx(uint32_t arbId, uint32_t periodMs);
-	/**
-	 * Schedule a CAN Frame for periodic transmit.
-	 * @param arbId 	CAN Frame Arbitration ID.  Set BIT31 for 11bit ids, otherwise we use 29bit ids.
-	 * @param periodMs	Period to transmit CAN frame.  Pass 0 for one-shot, which also disables that ArbID's preceding periodic transmit.
-	 * @param dlc 		Number of bytes to transmit (0 to 8).
-	 * @param initialFrame	Ptr to the frame data to schedule for transmitting.  Passing null will result
-	 *						in defaulting to zero data value.
-	 */
-	void RegisterTx(uint32_t arbId, uint32_t periodMs, uint32_t dlc, const uint8_t * initialFrame);
-	void UnregisterTx(uint32_t arbId);
-
-	CTR_Code GetRx(uint32_t arbId,uint8_t * dataBytes,uint32_t timeoutMs);
-	void FlushTx(uint32_t arbId);
-	bool ChangeTxPeriod(uint32_t arbId, uint32_t periodMs);
-
-	template<typename T> txTask<T> GetTx(uint32_t arbId)
-	{
-		txTask<T> retval = {0, nullptr};
-		txJobs_t::iterator i = _txJobs.find(arbId);
-		if(i != _txJobs.end()){
-			retval.arbId = i->second.arbId;
-			retval.toSend = (T*)i->second.toSend;
-		}
-		return retval;
-	}
-	template<class T> void FlushTx(T & par)
-	{
-		FlushTx(par.arbId);
-	}
-
-	template<class T> recMsg<T> GetRx(uint32_t arbId, uint32_t timeoutMs)
-	{
-		recMsg<T> retval;
-		retval.err = GetRx(arbId,retval.bytes, timeoutMs);
-		return retval;
-	}
-
-private:
-
-	class txJob_t {
-		public:
-			uint32_t arbId;
-			uint8_t toSend[8];
-			uint32_t periodMs;
-			uint8_t dlc;
-	};
-
-	class rxEvent_t{
-		public:
-			uint8_t bytes[8];
-			int64_t time;
-			rxEvent_t()
-			{
-				bytes[0] = 0;
-				bytes[1] = 0;
-				bytes[2] = 0;
-				bytes[3] = 0;
-				bytes[4] = 0;
-				bytes[5] = 0;
-				bytes[6] = 0;
-				bytes[7] = 0;
-			}
-	};
-
-	typedef std::map<uint32_t,txJob_t> txJobs_t;
-	txJobs_t _txJobs;
-
-	typedef std::map<uint32_t,rxEvent_t> rxRxEvents_t;
-	rxRxEvents_t _rxRxEvents;
-
-	wpi::mutex _lck;
-};
-#endif
diff --git a/third_party/allwpilib/hal/src/main/native/athena/ctre/PCM.cpp b/third_party/allwpilib/hal/src/main/native/athena/ctre/PCM.cpp
deleted file mode 100644
index 4fd633f..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/ctre/PCM.cpp
+++ /dev/null
@@ -1,573 +0,0 @@
-#pragma GCC diagnostic ignored "-Wmissing-field-initializers"
-
-#include "PCM.h"
-#include "FRC_NetworkCommunication/CANSessionMux.h"
-/* This can be a constant, as long as nobody needs to update solenoids within
-    1/50 of a second. */
-static const INT32 kCANPeriod = 20;
-
-#define STATUS_1  			0x9041400
-#define STATUS_SOL_FAULTS  	0x9041440
-#define STATUS_DEBUG  		0x9041480
-
-#define EXPECTED_RESPONSE_TIMEOUT_MS	(50)
-#define GET_PCM_STATUS()			CtreCanNode::recMsg<PcmStatus_t> 		rx = GetRx<PcmStatus_t>			(STATUS_1|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_PCM_SOL_FAULTS()		CtreCanNode::recMsg<PcmStatusFault_t> 	rx = GetRx<PcmStatusFault_t>	(STATUS_SOL_FAULTS|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
-#define GET_PCM_DEBUG()				CtreCanNode::recMsg<PcmDebug_t> 		rx = GetRx<PcmDebug_t>			(STATUS_DEBUG|GetDeviceNumber(),EXPECTED_RESPONSE_TIMEOUT_MS)
-
-#define CONTROL_1 			0x09041C00	/* PCM_Control */
-#define CONTROL_2 			0x09041C40	/* PCM_SupplemControl */
-#define CONTROL_3 			0x09041C80	/* PcmControlSetOneShotDur_t */
-
-/* encoder/decoders */
-typedef struct _PcmStatus_t{
-	/* Byte 0 */
-	unsigned SolenoidBits:8;
-	/* Byte 1 */
-	unsigned compressorOn:1;
-	unsigned stickyFaultFuseTripped:1;
-	unsigned stickyFaultCompCurrentTooHigh:1;
-	unsigned faultFuseTripped:1;
-	unsigned faultCompCurrentTooHigh:1;
-	unsigned faultHardwareFailure:1;
-	unsigned isCloseloopEnabled:1;
-	unsigned pressureSwitchEn:1;
-	/* Byte 2*/
-	unsigned battVoltage:8;
-	/* Byte 3 */
-	unsigned solenoidVoltageTop8:8;
-	/* Byte 4 */
-	unsigned compressorCurrentTop6:6;
-	unsigned solenoidVoltageBtm2:2;
-	/* Byte 5 */
-	unsigned StickyFault_dItooHigh :1;
-	unsigned Fault_dItooHigh :1;
-	unsigned moduleEnabled:1;
-	unsigned closedLoopOutput:1;
-	unsigned compressorCurrentBtm4:4;
-	/* Byte 6 */
-	unsigned tokenSeedTop8:8;
-	/* Byte 7 */
-	unsigned tokenSeedBtm8:8;
-}PcmStatus_t;
-
-typedef struct _PcmControl_t{
-	/* Byte 0 */
-	unsigned tokenTop8:8;
-	/* Byte 1 */
-	unsigned tokenBtm8:8;
-	/* Byte 2 */
-	unsigned solenoidBits:8;
-	/* Byte 3*/
-	unsigned reserved:4;
-	unsigned closeLoopOutput:1;
-	unsigned compressorOn:1;
-	unsigned closedLoopEnable:1;
-	unsigned clearStickyFaults:1;
-	/* Byte 4 */
-	unsigned OneShotField_h8:8;
-	/* Byte 5 */
-	unsigned OneShotField_l8:8;
-}PcmControl_t;
-
-typedef struct _PcmControlSetOneShotDur_t{
-	uint8_t sol10MsPerUnit[8];
-}PcmControlSetOneShotDur_t;
-
-typedef struct _PcmStatusFault_t{
-	/* Byte 0 */
-	unsigned SolenoidBlacklist:8;
-	/* Byte 1 */
-	unsigned reserved_bit0 :1;
-	unsigned reserved_bit1 :1;
-	unsigned reserved_bit2 :1;
-	unsigned reserved_bit3 :1;
-	unsigned StickyFault_CompNoCurrent :1;
-	unsigned Fault_CompNoCurrent :1;
-	unsigned StickyFault_SolenoidJumper :1;
-	unsigned Fault_SolenoidJumper :1;
-}PcmStatusFault_t;
-
-typedef struct _PcmDebug_t{
-	unsigned tokFailsTop8:8;
-	unsigned tokFailsBtm8:8;
-	unsigned lastFailedTokTop8:8;
-	unsigned lastFailedTokBtm8:8;
-	unsigned tokSuccessTop8:8;
-	unsigned tokSuccessBtm8:8;
-}PcmDebug_t;
-
-
-/* PCM Constructor - Clears all vars, establishes default settings, starts PCM background process
- *
- * @Return	-	void
- *
- * @Param 	-	deviceNumber	- 	Device ID of PCM to be controlled
- */
-PCM::PCM(UINT8 deviceNumber): CtreCanNode(deviceNumber)
-{
-	RegisterRx(STATUS_1 | deviceNumber );
-	RegisterRx(STATUS_SOL_FAULTS | deviceNumber );
-	RegisterRx(STATUS_DEBUG | deviceNumber );
-	RegisterTx(CONTROL_1 | deviceNumber, kCANPeriod);
-	/* enable close loop */
-	SetClosedLoopControl(1);
-}
-/* PCM D'tor
- */
-PCM::~PCM()
-{
-
-}
-
-/* Set PCM solenoid state
- *
- * @Return	-	CTR_Code	-	Error code (if any) for setting solenoid
- *
- * @Param 	-	idx			- 	ID of solenoid (0-7)
- * @Param 	-	en			- 	Enable / Disable identified solenoid
- */
-CTR_Code PCM::SetSolenoid(unsigned char idx, bool en)
-{
-	CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
-	if(toFill.IsEmpty())return CTR_UnexpectedArbId;
-	if (en)
-		toFill->solenoidBits |= (1ul << (idx));
-	else
-		toFill->solenoidBits &= ~(1ul << (idx));
-	FlushTx(toFill);
-	return CTR_OKAY;
-}
-
-/* Set all PCM solenoid states
- *
- * @Return	-	CTR_Code	-	Error code (if any) for setting solenoids
- * @Param 	-	state			Bitfield to set all solenoids to
- */
-CTR_Code PCM::SetAllSolenoids(UINT8 state) {
-	CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
-	if(toFill.IsEmpty())return CTR_UnexpectedArbId;
-	toFill->solenoidBits = state;
-	FlushTx(toFill);
-	return CTR_OKAY;
-}
-
-/* Clears PCM sticky faults (indicators of past faults
- *
- * @Return	-	CTR_Code	-	Error code (if any) for setting solenoid
- *
- * @Param 	-	clr		- 	Clear / do not clear faults
- */
-CTR_Code PCM::ClearStickyFaults()
-{
-	int32_t status = 0;
-	uint8_t pcmSupplemControl[] = { 0, 0, 0, 0x80 }; /* only bit set is ClearStickyFaults */
-	FRC_NetworkCommunication_CANSessionMux_sendMessage(CONTROL_2  | GetDeviceNumber(), pcmSupplemControl, sizeof(pcmSupplemControl), 0, &status);
-	if(status)
-		return CTR_TxFailed;
-	return CTR_OKAY;
-}
-
-/* Enables PCM Closed Loop Control of Compressor via pressure switch
- *
- * @Return	-	CTR_Code	-	Error code (if any) for setting solenoid
- *
- * @Param 	-	en		- 	Enable / Disable Closed Loop Control
- */
-CTR_Code PCM::SetClosedLoopControl(bool en)
-{
-	CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
-	if(toFill.IsEmpty())return CTR_UnexpectedArbId;
-	toFill->closedLoopEnable = en;
-	FlushTx(toFill);
-	return CTR_OKAY;
-}
-/* Get solenoid Blacklist status
- * @Return	-	CTR_Code	-	Error code (if any)
- * @Param	-	idx			-	ID of solenoid [0,7] to fire one shot pulse.
- */
-CTR_Code PCM::FireOneShotSolenoid(UINT8 idx)
-{
-	CtreCanNode::txTask<PcmControl_t> toFill = GetTx<PcmControl_t>(CONTROL_1 | GetDeviceNumber());
-	if(toFill.IsEmpty())return CTR_UnexpectedArbId;
-	/* grab field as it is now */
-	uint16_t oneShotField;
-	oneShotField = toFill->OneShotField_h8;
-	oneShotField <<= 8;
-	oneShotField |= toFill->OneShotField_l8;
-	/* get the caller's channel */
-	uint16_t shift = 2*idx;
-	uint16_t mask = 3; /* two bits wide */
-	uint8_t chBits = (oneShotField >> shift) & mask;
-	/* flip it */
-	chBits = (chBits)%3 + 1;
-	/* clear out 2bits for this channel*/
-	oneShotField &= ~(mask << shift);
-	/* put new field in */
-	oneShotField |= chBits << shift;
-	/* apply field as it is now */
-	toFill->OneShotField_h8 = oneShotField >> 8;
-	toFill->OneShotField_l8 = oneShotField;
-	FlushTx(toFill);
-	return CTR_OKAY;
-}
-/* Configure the pulse width of a solenoid channel for one-shot pulse.
- * Preprogrammed pulsewidth is 10ms resolution and can be between 10ms and
- * 2.55s.
- *
- * @Return	-	CTR_Code	-	Error code (if any)
- * @Param	-	idx			-	ID of solenoid [0,7] to configure.
- * @Param	-	durMs		-	pulse width in ms.
- */
-CTR_Code PCM::SetOneShotDurationMs(UINT8 idx,uint32_t durMs)
-{
-	/* sanity check caller's param */
-	if(idx > 7)
-		return CTR_InvalidParamValue;
-	/* get latest tx frame */
-	CtreCanNode::txTask<PcmControlSetOneShotDur_t> toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
-	if(toFill.IsEmpty()){
-		/* only send this out if caller wants to do one-shots */
-		RegisterTx(CONTROL_3 | _deviceNumber, kCANPeriod);
-		/* grab it */
-		toFill = GetTx<PcmControlSetOneShotDur_t>(CONTROL_3 | GetDeviceNumber());
-	}
-	toFill->sol10MsPerUnit[idx] = std::min(durMs/10,(uint32_t)0xFF);
-	/* apply the new data bytes */
-	FlushTx(toFill);
-	return CTR_OKAY;
-}
-
-/* Get solenoid state
- *
- * @Return	-	True/False	-	True if solenoid enabled, false otherwise
- *
- * @Param 	-	idx		- 	ID of solenoid (0-7) to return status of
- */
-CTR_Code PCM::GetSolenoid(UINT8 idx, bool &status)
-{
-	GET_PCM_STATUS();
-	status = (rx->SolenoidBits & (1ul<<(idx)) ) ? 1 : 0;
-	return rx.err;
-}
-
-/* Get solenoid state for all solenoids on the PCM
- *
- * @Return	-	Bitfield of solenoid states
- */
-CTR_Code PCM::GetAllSolenoids(UINT8 &status)
-{
-	GET_PCM_STATUS();
-	status = rx->SolenoidBits;
-	return rx.err;
-}
-
-/* Get pressure switch state
- *
- * @Return	-	True/False	-	True if pressure adequate, false if low
- */
-CTR_Code PCM::GetPressure(bool &status)
-{
-	GET_PCM_STATUS();
-	status = (rx->pressureSwitchEn ) ? 1 : 0;
-	return rx.err;
-}
-
-/* Get compressor state
- *
- * @Return	-	True/False	-	True if enabled, false if otherwise
- */
-CTR_Code PCM::GetCompressor(bool &status)
-{
-	GET_PCM_STATUS();
-	status = (rx->compressorOn);
-	return rx.err;
-}
-
-/* Get closed loop control state
- *
- * @Return	-	True/False	-	True if closed loop enabled, false if otherwise
- */
-CTR_Code PCM::GetClosedLoopControl(bool &status)
-{
-	GET_PCM_STATUS();
-	status = (rx->isCloseloopEnabled);
-	return rx.err;
-}
-
-/* Get compressor current draw
- *
- * @Return	-	Amperes	-	Compressor current
- */
-CTR_Code PCM::GetCompressorCurrent(float &status)
-{
-	GET_PCM_STATUS();
-	uint32_t temp =(rx->compressorCurrentTop6);
-	temp <<= 4;
-	temp |=  rx->compressorCurrentBtm4;
-	status = temp * 0.03125; /* 5.5 fixed pt value in Amps */
-	return rx.err;
-}
-
-/* Get voltage across solenoid rail
- *
- * @Return	-	Volts	-	Voltage across solenoid rail
- */
-CTR_Code PCM::GetSolenoidVoltage(float &status)
-{
-	GET_PCM_STATUS();
-	uint32_t raw =(rx->solenoidVoltageTop8);
-	raw <<= 2;
-	raw |=  rx->solenoidVoltageBtm2;
-	status = (double) raw * 0.03125; /* 5.5 fixed pt value in Volts */
-	return rx.err;
-}
-
-/* Get hardware fault value
- *
- * @Return	-	True/False	-	True if hardware failure detected, false if otherwise
- */
-CTR_Code PCM::GetHardwareFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->faultHardwareFailure;
-	return rx.err;
-}
-
-/* Get compressor fault value
- *
- * @Return	-	True/False	-	True if shorted compressor detected, false if otherwise
- */
-CTR_Code PCM::GetCompressorCurrentTooHighFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->faultCompCurrentTooHigh;
-	return rx.err;
-}
-CTR_Code PCM::GetCompressorShortedStickyFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->StickyFault_dItooHigh;
-	return rx.err;
-}
-CTR_Code PCM::GetCompressorShortedFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->Fault_dItooHigh;
-	return rx.err;
-}
-CTR_Code PCM::GetCompressorNotConnectedStickyFault(bool &status)
-{
-	GET_PCM_SOL_FAULTS();
-	status = rx->StickyFault_CompNoCurrent;
-	return rx.err;
-}
-CTR_Code PCM::GetCompressorNotConnectedFault(bool &status)
-{
-	GET_PCM_SOL_FAULTS();
-	status = rx->Fault_CompNoCurrent;
-	return rx.err;
-}
-
-/* Get solenoid fault value
- *
- * @Return	-	True/False	-	True if shorted solenoid detected, false if otherwise
- */
-CTR_Code PCM::GetSolenoidFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->faultFuseTripped;
-	return rx.err;
-}
-
-/* Get compressor sticky fault value
- *
- * @Return	-	True/False	-	True if solenoid had previously been shorted
- * 								(and sticky fault was not cleared), false if otherwise
- */
-CTR_Code PCM::GetCompressorCurrentTooHighStickyFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->stickyFaultCompCurrentTooHigh;
-	return rx.err;
-}
-
-/* Get solenoid sticky fault value
- *
- * @Return	-	True/False	-	True if compressor had previously been shorted
- * 								(and sticky fault was not cleared), false if otherwise
- */
-CTR_Code PCM::GetSolenoidStickyFault(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->stickyFaultFuseTripped;
-	return rx.err;
-}
-/* Get battery voltage
- *
- * @Return	-	Volts	-	Voltage across PCM power ports
- */
-CTR_Code PCM::GetBatteryVoltage(float &status)
-{
-	GET_PCM_STATUS();
-	status = (float)rx->battVoltage * 0.05 + 4.0; /* 50mV per unit plus 4V. */
-	return rx.err;
-}
-/* Return status of module enable/disable
- *
- * @Return	-	bool		-	Returns TRUE if PCM is enabled, FALSE if disabled
- */
-CTR_Code PCM::isModuleEnabled(bool &status)
-{
-	GET_PCM_STATUS();
-	status = rx->moduleEnabled;
-	return rx.err;
-}
-/* Get number of total failed PCM Control Frame
- *
- * @Return	-	Failed Control Frames	-	Number of failed control frames (tokenization fails)
- *
- * @WARNING	-	Return only valid if [SeekDebugFrames] is enabled
- * 				See function SeekDebugFrames
- * 				See function EnableSeekDebugFrames
- */
-CTR_Code PCM::GetNumberOfFailedControlFrames(UINT16 &status)
-{
-	GET_PCM_DEBUG();
-	status = rx->tokFailsTop8;
-	status <<= 8;
-	status |= rx->tokFailsBtm8;
-	return rx.err;
-}
-/* Get raw Solenoid Blacklist
- *
- * @Return	-	BINARY	-	Raw binary breakdown of Solenoid Blacklist
- * 							BIT7 = Solenoid 1, BIT6 = Solenoid 2, etc.
- *
- * @WARNING	-	Return only valid if [SeekStatusFaultFrames] is enabled
- * 				See function SeekStatusFaultFrames
- * 				See function EnableSeekStatusFaultFrames
- */
-CTR_Code PCM::GetSolenoidBlackList(UINT8 &status)
-{
-	GET_PCM_SOL_FAULTS();
-	status = rx->SolenoidBlacklist;
-	return rx.err;
-}
-/* Get solenoid Blacklist status
- * - Blacklisted solenoids cannot be enabled until PCM is power cycled
- *
- * @Return	-	True/False	-	True if Solenoid is blacklisted, false if otherwise
- *
- * @Param	-	idx			-	ID of solenoid [0,7]
- *
- * @WARNING	-	Return only valid if [SeekStatusFaultFrames] is enabled
- * 				See function SeekStatusFaultFrames
- * 				See function EnableSeekStatusFaultFrames
- */
-CTR_Code PCM::IsSolenoidBlacklisted(UINT8 idx, bool &status)
-{
-	GET_PCM_SOL_FAULTS();
-	status = (rx->SolenoidBlacklist & (1ul<<(idx)) )? 1 : 0;
-	return rx.err;
-}
-//------------------ C interface --------------------------------------------//
-extern "C" {
-	void * c_PCM_Init(void) {
-		return new PCM();
-	}
-	CTR_Code c_SetSolenoid(void * handle, unsigned char idx, INT8 param) {
-		return ((PCM*) handle)->SetSolenoid(idx, param);
-	}
-	CTR_Code c_SetAllSolenoids(void * handle, UINT8 state) {
-		return ((PCM*) handle)->SetAllSolenoids(state);
-	}
-	CTR_Code c_SetClosedLoopControl(void * handle, INT8 param) {
-		return ((PCM*) handle)->SetClosedLoopControl(param);
-	}
-	CTR_Code c_ClearStickyFaults(void * handle, INT8 param) {
-		return ((PCM*) handle)->ClearStickyFaults();
-	}
-	CTR_Code c_GetSolenoid(void * handle, UINT8 idx, INT8 * status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetSolenoid(idx, bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetAllSolenoids(void * handle, UINT8 * status) {
-		return ((PCM*) handle)->GetAllSolenoids(*status);
-	}
-	CTR_Code c_GetPressure(void * handle, INT8 * status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetPressure(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetCompressor(void * handle, INT8 * status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetCompressor(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetClosedLoopControl(void * handle, INT8 * status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetClosedLoopControl(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetCompressorCurrent(void * handle, float * status) {
-		CTR_Code retval = ((PCM*) handle)->GetCompressorCurrent(*status);
-		return retval;
-	}
-	CTR_Code c_GetSolenoidVoltage(void * handle, float*status) {
-		return ((PCM*) handle)->GetSolenoidVoltage(*status);
-	}
-	CTR_Code c_GetHardwareFault(void * handle, INT8*status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetHardwareFault(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetCompressorFault(void * handle, INT8*status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighFault(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetSolenoidFault(void * handle, INT8*status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetSolenoidFault(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetCompressorStickyFault(void * handle, INT8*status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetCompressorCurrentTooHighStickyFault(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetSolenoidStickyFault(void * handle, INT8*status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->GetSolenoidStickyFault(bstatus);
-		*status = bstatus;
-		return retval;
-	}
-	CTR_Code c_GetBatteryVoltage(void * handle, float*status) {
-		CTR_Code retval = ((PCM*) handle)->GetBatteryVoltage(*status);
-		return retval;
-	}
-	void c_SetDeviceNumber_PCM(void * handle, UINT8 deviceNumber) {
-	}
-	CTR_Code c_GetNumberOfFailedControlFrames(void * handle, UINT16*status) {
-		return ((PCM*) handle)->GetNumberOfFailedControlFrames(*status);
-	}
-	CTR_Code c_GetSolenoidBlackList(void * handle, UINT8 *status) {
-		return ((PCM*) handle)->GetSolenoidBlackList(*status);
-	}
-	CTR_Code c_IsSolenoidBlacklisted(void * handle, UINT8 idx, INT8*status) {
-		bool bstatus;
-		CTR_Code retval = ((PCM*) handle)->IsSolenoidBlacklisted(idx, bstatus);
-		*status = bstatus;
-		return retval;
-	}
-}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/ctre/PCM.h b/third_party/allwpilib/hal/src/main/native/athena/ctre/PCM.h
deleted file mode 100644
index 4923202..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/ctre/PCM.h
+++ /dev/null
@@ -1,226 +0,0 @@
-#ifndef PCM_H_
-#define PCM_H_
-#include "ctre.h"				//BIT Defines + Typedefs
-#include "CtreCanNode.h"
-class PCM : public CtreCanNode
-{
-public:
-    PCM(UINT8 deviceNumber=0);
-    ~PCM();
-
-    /* Set PCM solenoid state
-     *
-     * @Return	-	CTR_Code	-	Error code (if any) for setting solenoid
-     * @Param 	-	idx			- 	ID of solenoid (0-7)
-     * @Param 	-	en			- 	Enable / Disable identified solenoid
-     */
-    CTR_Code 	SetSolenoid(unsigned char idx, bool en);
-
-    /* Set all PCM solenoid states
-     *
-     * @Return	-	CTR_Code	-	Error code (if any) for setting solenoids
-     * @Param 	-	state			Bitfield to set all solenoids to
-     */
-    CTR_Code 	SetAllSolenoids(UINT8 state);
-
-    /* Enables PCM Closed Loop Control of Compressor via pressure switch
-     * @Return	-	CTR_Code	-	Error code (if any) for setting solenoid
-     * @Param 	-	en		- 	Enable / Disable Closed Loop Control
-     */
-    CTR_Code 	SetClosedLoopControl(bool en);
-
-    /* Clears PCM sticky faults (indicators of past faults
-     * @Return	-	CTR_Code	-	Error code (if any) for setting solenoid
-     */
-    CTR_Code 	ClearStickyFaults();
-
-    /* Get solenoid state
-     *
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param 	-	idx		- 	ID of solenoid (0-7) to return if solenoid is on.
-     * @Param	-	status	-	true if solenoid enabled, false otherwise
-     */
-    CTR_Code 	GetSolenoid(UINT8 idx, bool &status);
-
-    /* Get state of all solenoids
-     *
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status	-	bitfield of solenoid states
-     */
-    CTR_Code 	GetAllSolenoids(UINT8 &status);
-
-    /* Get pressure switch state
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if pressure adequate, false if low
-     */
-    CTR_Code 	GetPressure(bool &status);
-
-    /* Get compressor state
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compress output is on, false if otherwise
-     */
-    CTR_Code	GetCompressor(bool &status);
-
-    /* Get closed loop control state
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status	-	True if closed loop enabled, false if otherwise
-     */
-    CTR_Code 	GetClosedLoopControl(bool &status);
-
-    /* Get compressor current draw
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	Compressor current returned in Amperes (A)
-     */
-    CTR_Code 	GetCompressorCurrent(float &status);
-
-    /* Get voltage across solenoid rail
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	Voltage across solenoid rail in Volts (V)
-     */
-    CTR_Code 	GetSolenoidVoltage(float &status);
-
-    /* Get hardware fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if hardware failure detected, false if otherwise
-     */
-    CTR_Code 	GetHardwareFault(bool &status);
-
-    /* Get compressor fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if abnormally high compressor current detected, false if otherwise
-     */
-    CTR_Code 	GetCompressorCurrentTooHighFault(bool &status);
-
-    /* Get solenoid fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if shorted solenoid detected, false if otherwise
-     */
-    CTR_Code 	GetSolenoidFault(bool &status);
-
-    /* Get compressor sticky fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if solenoid had previously been shorted
-     * 								(and sticky fault was not cleared), false if otherwise
-     */
-    CTR_Code 	GetCompressorCurrentTooHighStickyFault(bool &status);
-    /* Get compressor shorted sticky fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compressor output is shorted, false if otherwise
-     */
-    CTR_Code 	GetCompressorShortedStickyFault(bool &status);
-    /* Get compressor shorted fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compressor output is shorted, false if otherwise
-     */
-    CTR_Code 	GetCompressorShortedFault(bool &status);
-    /* Get compressor is not connected sticky fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compressor current is too low,
-     * 					indicating compressor is not connected, false if otherwise
-     */
-    CTR_Code 	GetCompressorNotConnectedStickyFault(bool &status);
-    /* Get compressor is not connected fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compressor current is too low,
-     * 					indicating compressor is not connected, false if otherwise
-     */
-    CTR_Code 	GetCompressorNotConnectedFault(bool &status);
-
-    /* Get solenoid sticky fault value
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	True if compressor had previously been shorted
-     * 								(and sticky fault was not cleared), false if otherwise
-     */
-    CTR_Code 	GetSolenoidStickyFault(bool &status);
-
-    /* Get battery voltage
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	Voltage across PCM power ports in Volts (V)
-     */
-    CTR_Code 	GetBatteryVoltage(float &status);
-
-    /* Set PCM Device Number and according CAN frame IDs
-     * @Return	-	void
-     * @Param	-	deviceNumber	-	Device number of PCM to control
-     */
-    void	SetDeviceNumber(UINT8 deviceNumber);
-    /* Get number of total failed PCM Control Frame
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	Number of failed control frames (tokenization fails)
-     * @WARNING	-	Return only valid if [SeekDebugFrames] is enabled
-     * 				See function SeekDebugFrames
-     * 				See function EnableSeekDebugFrames
-     */
-	CTR_Code GetNumberOfFailedControlFrames(UINT16 &status);
-
-    /* Get raw Solenoid Blacklist
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	Raw binary breakdown of Solenoid Blacklist
-     * 								BIT7 = Solenoid 1, BIT6 = Solenoid 2, etc.
-     * @WARNING	-	Return only valid if [SeekStatusFaultFrames] is enabled
-     * 				See function SeekStatusFaultFrames
-     * 				See function EnableSeekStatusFaultFrames
-     */
-    CTR_Code 	GetSolenoidBlackList(UINT8 &status);
-
-    /* Get solenoid Blacklist status
-     * - Blacklisted solenoids cannot be enabled until PCM is power cycled
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	idx			-	ID of solenoid [0,7]
-     * @Param	-	status		-	True if Solenoid is blacklisted, false if otherwise
-     * @WARNING	-	Return only valid if [SeekStatusFaultFrames] is enabled
-     * 				See function SeekStatusFaultFrames
-     * 				See function EnableSeekStatusFaultFrames
-     */
-    CTR_Code 	IsSolenoidBlacklisted(UINT8 idx, bool &status);
-
-    /* Return status of module enable/disable
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	status		-	Returns TRUE if PCM is enabled, FALSE if disabled
-     */
-    CTR_Code	isModuleEnabled(bool &status);
-
-    /* Get solenoid Blacklist status
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	idx			-	ID of solenoid [0,7] to fire one shot pulse.
-     */
-    CTR_Code FireOneShotSolenoid(UINT8 idx);
-
-    /* Configure the pulse width of a solenoid channel for one-shot pulse.
-	 * Preprogrammed pulsewidth is 10ms resolute and can be between 20ms and 5.1s.
-     * @Return	-	CTR_Code	-	Error code (if any)
-     * @Param	-	idx			-	ID of solenoid [0,7] to configure.
-     * @Param	-	durMs		-	pulse width in ms.
-     */
-    CTR_Code SetOneShotDurationMs(UINT8 idx,uint32_t durMs);
-
-};
-//------------------ C interface --------------------------------------------//
-extern "C" {
-	void * c_PCM_Init(void);
-	CTR_Code c_SetSolenoid(void * handle,unsigned char idx,INT8 param);
-	CTR_Code c_SetAllSolenoids(void * handle,UINT8 state);
-	CTR_Code c_SetClosedLoopControl(void * handle,INT8 param);
-	CTR_Code c_ClearStickyFaults(void * handle,INT8 param);
-	CTR_Code c_GetSolenoid(void * handle,UINT8 idx,INT8 * status);
-	CTR_Code c_GetAllSolenoids(void * handle,UINT8 * status);
-	CTR_Code c_GetPressure(void * handle,INT8 * status);
-	CTR_Code c_GetCompressor(void * handle,INT8 * status);
-	CTR_Code c_GetClosedLoopControl(void * handle,INT8 * status);
-	CTR_Code c_GetCompressorCurrent(void * handle,float * status);
-	CTR_Code c_GetSolenoidVoltage(void * handle,float*status);
-	CTR_Code c_GetHardwareFault(void * handle,INT8*status);
-	CTR_Code c_GetCompressorFault(void * handle,INT8*status);
-	CTR_Code c_GetSolenoidFault(void * handle,INT8*status);
-	CTR_Code c_GetCompressorStickyFault(void * handle,INT8*status);
-	CTR_Code c_GetSolenoidStickyFault(void * handle,INT8*status);
-	CTR_Code c_GetBatteryVoltage(void * handle,float*status);
-	void c_SetDeviceNumber_PCM(void * handle,UINT8 deviceNumber);
-	void c_EnableSeekStatusFrames(void * handle,INT8 enable);
-	void c_EnableSeekStatusFaultFrames(void * handle,INT8 enable);
-	void c_EnableSeekDebugFrames(void * handle,INT8 enable);
-	CTR_Code c_GetNumberOfFailedControlFrames(void * handle,UINT16*status);
-	CTR_Code c_GetSolenoidBlackList(void * handle,UINT8 *status);
-	CTR_Code c_IsSolenoidBlacklisted(void * handle,UINT8 idx,INT8*status);
-}
-#endif
diff --git a/third_party/allwpilib/hal/src/main/native/athena/ctre/ctre.h b/third_party/allwpilib/hal/src/main/native/athena/ctre/ctre.h
deleted file mode 100644
index 90d33c1..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/ctre/ctre.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/**
- * @file ctre.h
- * Common header for all CTRE HAL modules.
- */
-#ifndef CTRE_H
-#define CTRE_H
-
-//Bit Defines
-#define BIT0 0x01
-#define BIT1 0x02
-#define BIT2 0x04
-#define BIT3 0x08
-#define BIT4 0x10
-#define BIT5 0x20
-#define BIT6 0x40
-#define BIT7 0x80
-#define BIT8  0x0100
-#define BIT9  0x0200
-#define BIT10 0x0400
-#define BIT11 0x0800
-#define BIT12 0x1000
-#define BIT13 0x2000
-#define BIT14 0x4000
-#define BIT15 0x8000
-
-//Signed
-typedef	signed char	INT8;
-typedef	signed short	INT16;
-typedef	signed int	INT32;
-typedef	signed long long INT64;
-
-//Unsigned
-typedef	unsigned char	UINT8;
-typedef	unsigned short	UINT16;
-typedef	unsigned int	UINT32;
-typedef	unsigned long long UINT64;
-
-//Other
-typedef	unsigned char	UCHAR;
-typedef unsigned short	USHORT;
-typedef	unsigned int	UINT;
-typedef unsigned long	ULONG;
-
-typedef enum {
-		CTR_OKAY,				//!< No Error - Function executed as expected
-		CTR_RxTimeout,			//!< CAN frame has not been received within specified period of time.
-		CTR_TxTimeout,			//!< Not used.
-		CTR_InvalidParamValue, 	//!< Caller passed an invalid param
-		CTR_UnexpectedArbId,	//!< Specified CAN Id is invalid.
-		CTR_TxFailed,			//!< Could not transmit the CAN frame.
-		CTR_SigNotUpdated,		//!< Have not received an value response for signal.
-		CTR_BufferFull,			//!< Caller attempted to insert data into a buffer that is full.
-}CTR_Code;
-
-#endif /* CTRE_H */
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AccelerometerData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AccelerometerData.cpp
index 2baaf78..e097a24 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AccelerometerData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AccelerometerData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/AccelerometerData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp
index d718789..a1d7011 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AddressableLEDData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/AddressableLEDData.h"
 
@@ -11,7 +8,9 @@
 
 extern "C" {
 
-int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) { return 0; }
+int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) {
+  return 0;
+}
 
 void HALSIM_ResetAddressableLEDData(int32_t index) {}
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp
index d91ce27..b4ff695 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogGyroData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/AnalogGyroData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogInData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogInData.cpp
index 4288538..30ac795 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogInData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogInData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/AnalogInData.h"
 
@@ -12,7 +9,9 @@
 extern "C" {
 void HALSIM_ResetAnalogInData(int32_t index) {}
 
-HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index) { return 0; }
+HAL_SimDeviceHandle HALSIM_GetAnalogInSimDevice(int32_t index) {
+  return 0;
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
   HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, AnalogIn##CAPINAME, RETURN)
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogOutData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogOutData.cpp
index f05b0be..e6da2aa 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogOutData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogOutData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/AnalogOutData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp
index 7781cf2..af8d7cc 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/AnalogTriggerData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/AnalogTriggerData.h"
 
@@ -11,7 +8,9 @@
 
 extern "C" {
 
-int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) { return 0; }
+int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) {
+  return 0;
+}
 
 void HALSIM_ResetAnalogTriggerData(int32_t index) {}
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/CTREPCMData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/CTREPCMData.cpp
new file mode 100644
index 0000000..fc9edfe
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/CTREPCMData.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/simulation/CTREPCMData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetCTREPCMData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, CTREPCM##CAPINAME, RETURN)
+
+HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, CTREPCMSolenoidOutput,
+                                   false)
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(HAL_Bool, CompressorOn, false)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, false)
+DEFINE_CAPI(double, CompressorCurrent, 0)
+
+void HALSIM_GetCTREPCMAllSolenoids(int32_t index, uint8_t* values) {
+  *values = 0;
+}
+
+void HALSIM_SetCTREPCMAllSolenoids(int32_t index, uint8_t values) {}
+
+void HALSIM_RegisterCTREPCMAllNonSolenoidCallbacks(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify) {}
+
+void HALSIM_RegisterCTREPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify) {}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/CanDataInternal.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/CanDataInternal.cpp
index 69debf0..be2fbc0 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/CanDataInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/CanDataInternal.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/CanData.h"
 #include "hal/simulation/SimDataValue.h"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DIOData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DIOData.cpp
index a06855d..392c31b 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DIOData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DIOData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/DIOData.h"
 
@@ -12,7 +9,9 @@
 extern "C" {
 void HALSIM_ResetDIOData(int32_t index) {}
 
-HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) { return 0; }
+HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) {
+  return 0;
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
   HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DIO##CAPINAME, RETURN)
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp
index bcbe370..c3f5d31 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DigitalPWMData.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/DigitalPWMData.h"
 
 #include "hal/simulation/SimDataValue.h"
 
 extern "C" {
-int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) { return 0; }
+int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) {
+  return 0;
+}
 
 void HALSIM_ResetDigitalPWMData(int32_t index) {}
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DriverStationData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DriverStationData.cpp
index b4695ba..ba519fc 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DriverStationData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DriverStationData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/DriverStationData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DutyCycleData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DutyCycleData.cpp
index 8d3cd61..d63e27e 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/DutyCycleData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/DutyCycleData.cpp
@@ -1,22 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/DutyCycleData.h"
 
 #include "hal/simulation/SimDataValue.h"
 
 extern "C" {
-int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) { return 0; }
+int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) {
+  return 0;
+}
 
 void HALSIM_ResetDutyCycleData(int32_t index) {}
 
-int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) { return 0; }
+int32_t HALSIM_GetDutyCycleDigitalChannel(int32_t index) {
+  return 0;
+}
 
-HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) { return 0; }
+HAL_SimDeviceHandle HALSIM_GetDutyCycleSimDevice(int32_t index) {
+  return 0;
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
   HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, DutyCycle##CAPINAME, RETURN)
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/EncoderData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/EncoderData.cpp
index 87a7385..4b07b31 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/EncoderData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/EncoderData.cpp
@@ -1,24 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/EncoderData.h"
 
 #include "hal/simulation/SimDataValue.h"
 
 extern "C" {
-int32_t HALSIM_FindEncoderForChannel(int32_t channel) { return 0; }
+int32_t HALSIM_FindEncoderForChannel(int32_t channel) {
+  return 0;
+}
 
 void HALSIM_ResetEncoderData(int32_t index) {}
 
-int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index) { return 0; }
+int32_t HALSIM_GetEncoderDigitalChannelA(int32_t index) {
+  return 0;
+}
 
-int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index) { return 0; }
+int32_t HALSIM_GetEncoderDigitalChannelB(int32_t index) {
+  return 0;
+}
 
-HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index) { return 0; }
+HAL_SimDeviceHandle HALSIM_GetEncoderSimDevice(int32_t index) {
+  return 0;
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
   HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, Encoder##CAPINAME, RETURN)
@@ -35,11 +40,15 @@
 
 void HALSIM_SetEncoderDistance(int32_t index, double distance) {}
 
-double HALSIM_GetEncoderDistance(int32_t index) { return 0; }
+double HALSIM_GetEncoderDistance(int32_t index) {
+  return 0;
+}
 
 void HALSIM_SetEncoderRate(int32_t index, double rate) {}
 
-double HALSIM_GetEncoderRate(int32_t index) { return 0; }
+double HALSIM_GetEncoderRate(int32_t index) {
+  return 0;
+}
 
 void HALSIM_RegisterEncoderAllCallbacks(int32_t index,
                                         HAL_NotifyCallback callback,
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/I2CData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/I2CData.cpp
index eb6a6c9..2f91061 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/I2CData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/I2CData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/I2CData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/MockHooks.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/MockHooks.cpp
index 2e7bcfe..0ea05d0 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/MockHooks.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/MockHooks.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/MockHooks.h"
 
@@ -15,7 +12,9 @@
 
 void HALSIM_SetProgramStarted(void) {}
 
-HAL_Bool HALSIM_GetProgramStarted(void) { return false; }
+HAL_Bool HALSIM_GetProgramStarted(void) {
+  return false;
+}
 
 void HALSIM_RestartTiming(void) {}
 
@@ -23,7 +22,9 @@
 
 void HALSIM_ResumeTiming(void) {}
 
-HAL_Bool HALSIM_IsTimingPaused(void) { return false; }
+HAL_Bool HALSIM_IsTimingPaused(void) {
+  return false;
+}
 
 void HALSIM_StepTiming(uint64_t delta) {}
 
@@ -33,4 +34,18 @@
 
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler) {}
 
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return 0;
+}
+
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {}
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return 0;
+}
+
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/NotifierData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/NotifierData.cpp
index 34aa6e7..f75733e 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/NotifierData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/NotifierData.cpp
@@ -1,17 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/NotifierData.h"
 
 extern "C" {
 
-uint64_t HALSIM_GetNextNotifierTimeout(void) { return 0; }
+uint64_t HALSIM_GetNextNotifierTimeout(void) {
+  return 0;
+}
 
-int32_t HALSIM_GetNumNotifiers(void) { return 0; }
+int32_t HALSIM_GetNumNotifiers(void) {
+  return 0;
+}
 
 int32_t HALSIM_GetNotifierInfo(struct HALSIM_NotifierInfo* arr, int32_t size) {
   return 0;
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PCMData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/PCMData.cpp
deleted file mode 100644
index 29302cb..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PCMData.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/simulation/PCMData.h"
-
-#include "hal/simulation/SimDataValue.h"
-
-extern "C" {
-void HALSIM_ResetPCMData(int32_t index) {}
-
-#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
-  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PCM##CAPINAME, RETURN)
-
-HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidInitialized,
-                                   false)
-HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidOutput, false)
-DEFINE_CAPI(HAL_Bool, CompressorInitialized, false)
-DEFINE_CAPI(HAL_Bool, CompressorOn, false)
-DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false)
-DEFINE_CAPI(HAL_Bool, PressureSwitch, false)
-DEFINE_CAPI(double, CompressorCurrent, 0)
-
-void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values) { *values = 0; }
-
-void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values) {}
-
-void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify) {}
-
-void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify) {}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PDPData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/PDPData.cpp
deleted file mode 100644
index a28bb81..0000000
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PDPData.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/simulation/PDPData.h"
-
-#include "../PortsInternal.h"
-#include "hal/simulation/SimDataValue.h"
-
-extern "C" {
-void HALSIM_ResetPDPData(int32_t index) {}
-
-#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
-  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PDP##CAPINAME, RETURN)
-
-DEFINE_CAPI(HAL_Bool, Initialized, false)
-DEFINE_CAPI(double, Temperature, 0)
-DEFINE_CAPI(double, Voltage, 0)
-HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(double, HALSIM, PDPCurrent, 0)
-
-void HALSIM_GetPDPAllCurrents(int32_t index, double* currents) {
-  for (int i = 0; i < hal::kNumPDPChannels; i++) currents[i] = 0;
-}
-
-void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents) {}
-
-void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify) {}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PWMData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/PWMData.cpp
index b9c5691..3e12398 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PWMData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/PWMData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/PWMData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp
new file mode 100644
index 0000000..a44e222
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/PowerDistributionData.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/simulation/PowerDistributionData.h"
+
+#include "../PortsInternal.h"
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetPowerDistributionData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, PowerDistribution##CAPINAME, RETURN)
+
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(double, Temperature, 0)
+DEFINE_CAPI(double, Voltage, 0)
+HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(double, HALSIM, PowerDistributionCurrent, 0)
+
+void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents,
+                                            int length) {
+  for (int i = 0; i < length; i++) {
+    currents[i] = 0;
+  }
+}
+
+void HALSIM_SetPowerDistributionAllCurrents(int32_t index,
+                                            const double* currents,
+                                            int length) {}
+
+void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks(
+    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify) {}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/REVPHData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/REVPHData.cpp
new file mode 100644
index 0000000..d617694
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/REVPHData.cpp
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/simulation/REVPHData.h"
+
+#include "hal/simulation/SimDataValue.h"
+
+extern "C" {
+void HALSIM_ResetREVPHData(int32_t index) {}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
+  HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, REVPH##CAPINAME, RETURN)
+
+HAL_SIMDATAVALUE_STUB_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput, false)
+DEFINE_CAPI(HAL_Bool, Initialized, false)
+DEFINE_CAPI(HAL_Bool, CompressorOn, false)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, false)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, false)
+DEFINE_CAPI(double, CompressorCurrent, 0)
+
+void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) {
+  *values = 0;
+}
+
+void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) {}
+
+void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify) {}
+
+void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/RelayData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/RelayData.cpp
index 734bf89..7d8d439 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/RelayData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/RelayData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/RelayData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp
index f9e9ca7..9f6683a 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/RoboRioData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/RoboRioData.h"
 
@@ -30,6 +27,7 @@
 DEFINE_CAPI(int32_t, UserFaults6V, 0)
 DEFINE_CAPI(int32_t, UserFaults5V, 0)
 DEFINE_CAPI(int32_t, UserFaults3V3, 0)
+DEFINE_CAPI(double, BrownoutVoltage, 6.75)
 
 void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
                                         void* param, HAL_Bool initialNotify) {}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp
index c3e8ede..c938abf 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIAccelerometerData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/SPIAccelerometerData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIData.cpp
index 13cf67c..433ca10 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/SPIData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/SPIData.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/athena/mockdata/SimDeviceData.cpp b/third_party/allwpilib/hal/src/main/native/athena/mockdata/SimDeviceData.cpp
index a31160c..08bd8ce 100644
--- a/third_party/allwpilib/hal/src/main/native/athena/mockdata/SimDeviceData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/athena/mockdata/SimDeviceData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/SimDeviceData.h"
 
@@ -13,7 +10,9 @@
 
 void HALSIM_SetSimDeviceEnabled(const char* prefix, HAL_Bool enabled) {}
 
-HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name) { return false; }
+HAL_Bool HALSIM_IsSimDeviceEnabled(const char* name) {
+  return false;
+}
 
 int32_t HALSIM_RegisterSimDeviceCreatedCallback(
     const char* prefix, void* param, HALSIM_SimDeviceCallback callback,
@@ -23,16 +22,21 @@
 
 void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid) {}
 
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify) {
   return 0;
 }
 
 void HALSIM_CancelSimDeviceFreedCallback(int32_t uid) {}
 
-HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) { return 0; }
+HAL_SimDeviceHandle HALSIM_GetSimDeviceHandle(const char* name) {
+  return 0;
+}
 
-const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) { return ""; }
+const char* HALSIM_GetSimDeviceName(HAL_SimDeviceHandle handle) {
+  return "";
+}
 
 HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
   return 0;
@@ -59,6 +63,15 @@
 
 void HALSIM_CancelSimValueChangedCallback(int32_t uid) {}
 
+int32_t HALSIM_RegisterSimValueResetCallback(HAL_SimValueHandle handle,
+                                             void* param,
+                                             HALSIM_SimValueCallback callback,
+                                             HAL_Bool initialNotify) {
+  return 0;
+}
+
+void HALSIM_CancelSimValueResetCallback(int32_t uid) {}
+
 HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
                                             const char* name) {
   return 0;
@@ -73,6 +86,12 @@
   return nullptr;
 }
 
+const double* HALSIM_GetSimValueEnumDoubleValues(HAL_SimValueHandle handle,
+                                                 int32_t* numOptions) {
+  *numOptions = 0;
+  return nullptr;
+}
+
 void HALSIM_ResetSimDeviceData(void) {}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/athena/rev/PDHFrames.cpp b/third_party/allwpilib/hal/src/main/native/athena/rev/PDHFrames.cpp
new file mode 100644
index 0000000..b50a148
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/rev/PDHFrames.cpp
@@ -0,0 +1,1790 @@
+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018-2019 Erik Moqvist
+ *
+ * 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.
+ */
+
+/**
+ * This file was generated by cantools version
+ */
+
+#include <string.h>
+
+#include "PDHFrames.h"
+
+static inline uint8_t pack_left_shift_u8(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value << shift) & mask);
+}
+
+static inline uint8_t pack_left_shift_u16(
+    uint16_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value << shift) & mask);
+}
+
+static inline uint8_t pack_left_shift_u32(
+    uint32_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value << shift) & mask);
+}
+
+static inline uint8_t pack_right_shift_u16(
+    uint16_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value >> shift) & mask);
+}
+
+static inline uint8_t pack_right_shift_u32(
+    uint32_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value >> shift) & mask);
+}
+
+static inline uint16_t unpack_left_shift_u16(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint16_t)((uint16_t)(value & mask) << shift);
+}
+
+static inline uint32_t unpack_left_shift_u32(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint32_t)((uint32_t)(value & mask) << shift);
+}
+
+static inline uint8_t unpack_right_shift_u8(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value & mask) >> shift);
+}
+
+static inline uint16_t unpack_right_shift_u16(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint16_t)((uint16_t)(value & mask) >> shift);
+}
+
+static inline uint32_t unpack_right_shift_u32(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint32_t)((uint32_t)(value & mask) >> shift);
+}
+
+int PDH_switch_channel_set_pack(
+    uint8_t *dst_p,
+    const struct PDH_switch_channel_set_t *src_p,
+    size_t size)
+{
+    if (size < 1u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 1);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->output_set_value, 0u, 0x01u);
+    dst_p[0] |= pack_left_shift_u8(src_p->use_system_enable, 1u, 0x02u);
+
+    return (1);
+}
+
+int PDH_switch_channel_set_unpack(
+    struct PDH_switch_channel_set_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 1u) {
+        return (-EINVAL);
+    }
+
+    dst_p->output_set_value = unpack_right_shift_u8(src_p[0], 0u, 0x01u);
+    dst_p->use_system_enable = unpack_right_shift_u8(src_p[0], 1u, 0x02u);
+
+    return (0);
+}
+
+uint8_t PDH_switch_channel_set_output_set_value_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_switch_channel_set_output_set_value_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_switch_channel_set_output_set_value_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_switch_channel_set_use_system_enable_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_switch_channel_set_use_system_enable_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_switch_channel_set_use_system_enable_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+int PDH_status0_pack(
+    uint8_t *dst_p,
+    const struct PDH_status0_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u16(src_p->channel_0_current, 0u, 0xffu);
+    dst_p[1] |= pack_right_shift_u16(src_p->channel_0_current, 8u, 0x03u);
+    dst_p[1] |= pack_left_shift_u16(src_p->channel_1_current, 2u, 0xfcu);
+    dst_p[2] |= pack_right_shift_u16(src_p->channel_1_current, 6u, 0x0fu);
+    dst_p[2] |= pack_left_shift_u16(src_p->channel_2_current, 4u, 0xf0u);
+    dst_p[3] |= pack_right_shift_u16(src_p->channel_2_current, 4u, 0x3fu);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_0_brownout, 6u, 0x40u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_1_brownout, 7u, 0x80u);
+    dst_p[4] |= pack_left_shift_u16(src_p->channel_3_current, 0u, 0xffu);
+    dst_p[5] |= pack_right_shift_u16(src_p->channel_3_current, 8u, 0x03u);
+    dst_p[5] |= pack_left_shift_u16(src_p->channel_4_current, 2u, 0xfcu);
+    dst_p[6] |= pack_right_shift_u16(src_p->channel_4_current, 6u, 0x0fu);
+    dst_p[6] |= pack_left_shift_u16(src_p->channel_5_current, 4u, 0xf0u);
+    dst_p[7] |= pack_right_shift_u16(src_p->channel_5_current, 4u, 0x3fu);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_2_brownout, 6u, 0x40u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_3_brownout, 7u, 0x80u);
+
+    return (8);
+}
+
+int PDH_status0_unpack(
+    struct PDH_status0_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_0_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
+    dst_p->channel_0_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u);
+    dst_p->channel_1_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu);
+    dst_p->channel_1_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu);
+    dst_p->channel_2_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u);
+    dst_p->channel_2_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu);
+    dst_p->channel_0_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u);
+    dst_p->channel_1_brownout = unpack_right_shift_u8(src_p[3], 7u, 0x80u);
+    dst_p->channel_3_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
+    dst_p->channel_3_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u);
+    dst_p->channel_4_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu);
+    dst_p->channel_4_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu);
+    dst_p->channel_5_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u);
+    dst_p->channel_5_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu);
+    dst_p->channel_2_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u);
+    dst_p->channel_3_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u);
+
+    return (0);
+}
+
+uint16_t PDH_status0_channel_0_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status0_channel_0_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status0_channel_0_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status0_channel_1_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status0_channel_1_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status0_channel_1_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status0_channel_2_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status0_channel_2_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status0_channel_2_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status0_channel_0_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status0_channel_0_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status0_channel_0_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status0_channel_1_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status0_channel_1_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status0_channel_1_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint16_t PDH_status0_channel_3_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status0_channel_3_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status0_channel_3_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status0_channel_4_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status0_channel_4_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status0_channel_4_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status0_channel_5_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status0_channel_5_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status0_channel_5_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status0_channel_2_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status0_channel_2_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status0_channel_2_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status0_channel_3_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status0_channel_3_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status0_channel_3_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+int PDH_status1_pack(
+    uint8_t *dst_p,
+    const struct PDH_status1_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u16(src_p->channel_6_current, 0u, 0xffu);
+    dst_p[1] |= pack_right_shift_u16(src_p->channel_6_current, 8u, 0x03u);
+    dst_p[1] |= pack_left_shift_u16(src_p->channel_7_current, 2u, 0xfcu);
+    dst_p[2] |= pack_right_shift_u16(src_p->channel_7_current, 6u, 0x0fu);
+    dst_p[2] |= pack_left_shift_u16(src_p->channel_8_current, 4u, 0xf0u);
+    dst_p[3] |= pack_right_shift_u16(src_p->channel_8_current, 4u, 0x3fu);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_4_brownout, 6u, 0x40u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_5_brownout, 7u, 0x80u);
+    dst_p[4] |= pack_left_shift_u16(src_p->channel_9_current, 0u, 0xffu);
+    dst_p[5] |= pack_right_shift_u16(src_p->channel_9_current, 8u, 0x03u);
+    dst_p[5] |= pack_left_shift_u16(src_p->channel_10_current, 2u, 0xfcu);
+    dst_p[6] |= pack_right_shift_u16(src_p->channel_10_current, 6u, 0x0fu);
+    dst_p[6] |= pack_left_shift_u16(src_p->channel_11_current, 4u, 0xf0u);
+    dst_p[7] |= pack_right_shift_u16(src_p->channel_11_current, 4u, 0x3fu);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_6_brownout, 6u, 0x40u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_7_brownout, 7u, 0x80u);
+
+    return (8);
+}
+
+int PDH_status1_unpack(
+    struct PDH_status1_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_6_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
+    dst_p->channel_6_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u);
+    dst_p->channel_7_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu);
+    dst_p->channel_7_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu);
+    dst_p->channel_8_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u);
+    dst_p->channel_8_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu);
+    dst_p->channel_4_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u);
+    dst_p->channel_5_brownout = unpack_right_shift_u8(src_p[3], 7u, 0x80u);
+    dst_p->channel_9_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
+    dst_p->channel_9_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u);
+    dst_p->channel_10_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu);
+    dst_p->channel_10_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu);
+    dst_p->channel_11_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u);
+    dst_p->channel_11_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu);
+    dst_p->channel_6_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u);
+    dst_p->channel_7_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u);
+
+    return (0);
+}
+
+uint16_t PDH_status1_channel_6_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status1_channel_6_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status1_channel_6_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status1_channel_7_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status1_channel_7_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status1_channel_7_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status1_channel_8_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status1_channel_8_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status1_channel_8_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status1_channel_4_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status1_channel_4_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status1_channel_4_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status1_channel_5_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status1_channel_5_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status1_channel_5_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint16_t PDH_status1_channel_9_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status1_channel_9_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status1_channel_9_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status1_channel_10_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status1_channel_10_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status1_channel_10_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status1_channel_11_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status1_channel_11_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status1_channel_11_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status1_channel_6_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status1_channel_6_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status1_channel_6_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status1_channel_7_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status1_channel_7_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status1_channel_7_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+int PDH_status2_pack(
+    uint8_t *dst_p,
+    const struct PDH_status2_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u16(src_p->channel_12_current, 0u, 0xffu);
+    dst_p[1] |= pack_right_shift_u16(src_p->channel_12_current, 8u, 0x03u);
+    dst_p[1] |= pack_left_shift_u16(src_p->channel_13_current, 2u, 0xfcu);
+    dst_p[2] |= pack_right_shift_u16(src_p->channel_13_current, 6u, 0x0fu);
+    dst_p[2] |= pack_left_shift_u16(src_p->channel_14_current, 4u, 0xf0u);
+    dst_p[3] |= pack_right_shift_u16(src_p->channel_14_current, 4u, 0x3fu);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_8_brownout, 6u, 0x40u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_9_brownout, 7u, 0x80u);
+    dst_p[4] |= pack_left_shift_u16(src_p->channel_15_current, 0u, 0xffu);
+    dst_p[5] |= pack_right_shift_u16(src_p->channel_15_current, 8u, 0x03u);
+    dst_p[5] |= pack_left_shift_u16(src_p->channel_16_current, 2u, 0xfcu);
+    dst_p[6] |= pack_right_shift_u16(src_p->channel_16_current, 6u, 0x0fu);
+    dst_p[6] |= pack_left_shift_u16(src_p->channel_17_current, 4u, 0xf0u);
+    dst_p[7] |= pack_right_shift_u16(src_p->channel_17_current, 4u, 0x3fu);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_10_brownout, 6u, 0x40u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_11_brownout, 7u, 0x80u);
+
+    return (8);
+}
+
+int PDH_status2_unpack(
+    struct PDH_status2_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_12_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
+    dst_p->channel_12_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u);
+    dst_p->channel_13_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu);
+    dst_p->channel_13_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu);
+    dst_p->channel_14_current = unpack_right_shift_u16(src_p[2], 4u, 0xf0u);
+    dst_p->channel_14_current |= unpack_left_shift_u16(src_p[3], 4u, 0x3fu);
+    dst_p->channel_8_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u);
+    dst_p->channel_9_brownout = unpack_right_shift_u8(src_p[3], 7u, 0x80u);
+    dst_p->channel_15_current = unpack_right_shift_u16(src_p[4], 0u, 0xffu);
+    dst_p->channel_15_current |= unpack_left_shift_u16(src_p[5], 8u, 0x03u);
+    dst_p->channel_16_current = unpack_right_shift_u16(src_p[5], 2u, 0xfcu);
+    dst_p->channel_16_current |= unpack_left_shift_u16(src_p[6], 6u, 0x0fu);
+    dst_p->channel_17_current = unpack_right_shift_u16(src_p[6], 4u, 0xf0u);
+    dst_p->channel_17_current |= unpack_left_shift_u16(src_p[7], 4u, 0x3fu);
+    dst_p->channel_10_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u);
+    dst_p->channel_11_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u);
+
+    return (0);
+}
+
+uint16_t PDH_status2_channel_12_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status2_channel_12_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status2_channel_12_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status2_channel_13_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status2_channel_13_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status2_channel_13_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status2_channel_14_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status2_channel_14_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status2_channel_14_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status2_channel_8_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status2_channel_8_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status2_channel_8_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status2_channel_9_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status2_channel_9_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status2_channel_9_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint16_t PDH_status2_channel_15_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status2_channel_15_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status2_channel_15_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status2_channel_16_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status2_channel_16_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status2_channel_16_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status2_channel_17_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status2_channel_17_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status2_channel_17_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status2_channel_10_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status2_channel_10_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status2_channel_10_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status2_channel_11_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status2_channel_11_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status2_channel_11_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+int PDH_status3_pack(
+    uint8_t *dst_p,
+    const struct PDH_status3_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u16(src_p->channel_18_current, 0u, 0xffu);
+    dst_p[1] |= pack_right_shift_u16(src_p->channel_18_current, 8u, 0x03u);
+    dst_p[1] |= pack_left_shift_u16(src_p->channel_19_current, 2u, 0xfcu);
+    dst_p[2] |= pack_right_shift_u16(src_p->channel_19_current, 6u, 0x0fu);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_12_brownout, 4u, 0x10u);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_13_brownout, 5u, 0x20u);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_14_brownout, 6u, 0x40u);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_15_brownout, 7u, 0x80u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_20_current, 0u, 0xffu);
+    dst_p[4] |= pack_left_shift_u8(src_p->channel_21_current, 0u, 0xffu);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_22_current, 0u, 0xffu);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_23_current, 0u, 0xffu);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_16_brownout, 0u, 0x01u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_17_brownout, 1u, 0x02u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_18_brownout, 2u, 0x04u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_19_brownout, 3u, 0x08u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_20_brownout, 4u, 0x10u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_21_brownout, 5u, 0x20u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_22_brownout, 6u, 0x40u);
+    dst_p[7] |= pack_left_shift_u8(src_p->channel_23_brownout, 7u, 0x80u);
+
+    return (8);
+}
+
+int PDH_status3_unpack(
+    struct PDH_status3_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_18_current = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
+    dst_p->channel_18_current |= unpack_left_shift_u16(src_p[1], 8u, 0x03u);
+    dst_p->channel_19_current = unpack_right_shift_u16(src_p[1], 2u, 0xfcu);
+    dst_p->channel_19_current |= unpack_left_shift_u16(src_p[2], 6u, 0x0fu);
+    dst_p->channel_12_brownout = unpack_right_shift_u8(src_p[2], 4u, 0x10u);
+    dst_p->channel_13_brownout = unpack_right_shift_u8(src_p[2], 5u, 0x20u);
+    dst_p->channel_14_brownout = unpack_right_shift_u8(src_p[2], 6u, 0x40u);
+    dst_p->channel_15_brownout = unpack_right_shift_u8(src_p[2], 7u, 0x80u);
+    dst_p->channel_20_current = unpack_right_shift_u8(src_p[3], 0u, 0xffu);
+    dst_p->channel_21_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu);
+    dst_p->channel_22_current = unpack_right_shift_u8(src_p[5], 0u, 0xffu);
+    dst_p->channel_23_current = unpack_right_shift_u8(src_p[6], 0u, 0xffu);
+    dst_p->channel_16_brownout = unpack_right_shift_u8(src_p[7], 0u, 0x01u);
+    dst_p->channel_17_brownout = unpack_right_shift_u8(src_p[7], 1u, 0x02u);
+    dst_p->channel_18_brownout = unpack_right_shift_u8(src_p[7], 2u, 0x04u);
+    dst_p->channel_19_brownout = unpack_right_shift_u8(src_p[7], 3u, 0x08u);
+    dst_p->channel_20_brownout = unpack_right_shift_u8(src_p[7], 4u, 0x10u);
+    dst_p->channel_21_brownout = unpack_right_shift_u8(src_p[7], 5u, 0x20u);
+    dst_p->channel_22_brownout = unpack_right_shift_u8(src_p[7], 6u, 0x40u);
+    dst_p->channel_23_brownout = unpack_right_shift_u8(src_p[7], 7u, 0x80u);
+
+    return (0);
+}
+
+uint16_t PDH_status3_channel_18_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status3_channel_18_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status3_channel_18_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint16_t PDH_status3_channel_19_current_encode(double value)
+{
+    return (uint16_t)(value / 0.125);
+}
+
+double PDH_status3_channel_19_current_decode(uint16_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PDH_status3_channel_19_current_is_in_range(uint16_t value)
+{
+    return (value <= 1023u);
+}
+
+uint8_t PDH_status3_channel_12_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_12_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_12_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_13_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_13_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_13_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_14_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_14_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_14_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_15_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_15_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_15_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_20_current_encode(double value)
+{
+    return (uint8_t)(value / 0.0625);
+}
+
+double PDH_status3_channel_20_current_decode(uint8_t value)
+{
+    return ((double)value * 0.0625);
+}
+
+bool PDH_status3_channel_20_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_status3_channel_21_current_encode(double value)
+{
+    return (uint8_t)(value / 0.0625);
+}
+
+double PDH_status3_channel_21_current_decode(uint8_t value)
+{
+    return ((double)value * 0.0625);
+}
+
+bool PDH_status3_channel_21_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_status3_channel_22_current_encode(double value)
+{
+    return (uint8_t)(value / 0.0625);
+}
+
+double PDH_status3_channel_22_current_decode(uint8_t value)
+{
+    return ((double)value * 0.0625);
+}
+
+bool PDH_status3_channel_22_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_status3_channel_23_current_encode(double value)
+{
+    return (uint8_t)(value / 0.0625);
+}
+
+double PDH_status3_channel_23_current_decode(uint8_t value)
+{
+    return ((double)value * 0.0625);
+}
+
+bool PDH_status3_channel_23_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_status3_channel_16_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_16_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_16_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_17_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_17_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_17_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_18_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_18_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_18_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_19_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_19_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_19_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_20_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_20_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_20_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_21_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_21_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_21_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_22_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_22_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_22_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status3_channel_23_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status3_channel_23_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status3_channel_23_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+int PDH_status4_pack(
+    uint8_t *dst_p,
+    const struct PDH_status4_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u16(src_p->v_bus, 0u, 0xffu);
+    dst_p[1] |= pack_right_shift_u16(src_p->v_bus, 8u, 0x0fu);
+    dst_p[1] |= pack_left_shift_u8(src_p->system_enable, 4u, 0x10u);
+    dst_p[1] |= pack_left_shift_u8(src_p->rsvd0, 5u, 0xe0u);
+    dst_p[2] |= pack_left_shift_u8(src_p->brownout, 0u, 0x01u);
+    dst_p[2] |= pack_left_shift_u8(src_p->rsvd1, 1u, 0x02u);
+    dst_p[2] |= pack_left_shift_u8(src_p->can_warning, 2u, 0x04u);
+    dst_p[2] |= pack_left_shift_u8(src_p->hardware_fault, 3u, 0x08u);
+    dst_p[2] |= pack_left_shift_u8(src_p->sw_state, 4u, 0x10u);
+    dst_p[2] |= pack_left_shift_u8(src_p->sticky_brownout, 5u, 0x20u);
+    dst_p[2] |= pack_left_shift_u8(src_p->rsvd2, 6u, 0x40u);
+    dst_p[2] |= pack_left_shift_u8(src_p->sticky_can_warning, 7u, 0x80u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_can_bus_off, 0u, 0x01u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_hardware_fault, 1u, 0x02u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_firmware_fault, 2u, 0x04u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch20_brownout, 3u, 0x08u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch21_brownout, 4u, 0x10u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch22_brownout, 5u, 0x20u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_ch23_brownout, 6u, 0x40u);
+    dst_p[3] |= pack_left_shift_u8(src_p->sticky_has_reset, 7u, 0x80u);
+    dst_p[4] |= pack_left_shift_u8(src_p->total_current, 0u, 0xffu);
+
+    return (8);
+}
+
+int PDH_status4_unpack(
+    struct PDH_status4_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->v_bus = unpack_right_shift_u16(src_p[0], 0u, 0xffu);
+    dst_p->v_bus |= unpack_left_shift_u16(src_p[1], 8u, 0x0fu);
+    dst_p->system_enable = unpack_right_shift_u8(src_p[1], 4u, 0x10u);
+    dst_p->rsvd0 = unpack_right_shift_u8(src_p[1], 5u, 0xe0u);
+    dst_p->brownout = unpack_right_shift_u8(src_p[2], 0u, 0x01u);
+    dst_p->rsvd1 = unpack_right_shift_u8(src_p[2], 1u, 0x02u);
+    dst_p->can_warning = unpack_right_shift_u8(src_p[2], 2u, 0x04u);
+    dst_p->hardware_fault = unpack_right_shift_u8(src_p[2], 3u, 0x08u);
+    dst_p->sw_state = unpack_right_shift_u8(src_p[2], 4u, 0x10u);
+    dst_p->sticky_brownout = unpack_right_shift_u8(src_p[2], 5u, 0x20u);
+    dst_p->rsvd2 = unpack_right_shift_u8(src_p[2], 6u, 0x40u);
+    dst_p->sticky_can_warning = unpack_right_shift_u8(src_p[2], 7u, 0x80u);
+    dst_p->sticky_can_bus_off = unpack_right_shift_u8(src_p[3], 0u, 0x01u);
+    dst_p->sticky_hardware_fault = unpack_right_shift_u8(src_p[3], 1u, 0x02u);
+    dst_p->sticky_firmware_fault = unpack_right_shift_u8(src_p[3], 2u, 0x04u);
+    dst_p->sticky_ch20_brownout = unpack_right_shift_u8(src_p[3], 3u, 0x08u);
+    dst_p->sticky_ch21_brownout = unpack_right_shift_u8(src_p[3], 4u, 0x10u);
+    dst_p->sticky_ch22_brownout = unpack_right_shift_u8(src_p[3], 5u, 0x20u);
+    dst_p->sticky_ch23_brownout = unpack_right_shift_u8(src_p[3], 6u, 0x40u);
+    dst_p->sticky_has_reset = unpack_right_shift_u8(src_p[3], 7u, 0x80u);
+    dst_p->total_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu);
+
+    return (0);
+}
+
+uint16_t PDH_status4_v_bus_encode(double value)
+{
+    return (uint16_t)(value / 0.0078125);
+}
+
+double PDH_status4_v_bus_decode(uint16_t value)
+{
+    return ((double)value * 0.0078125);
+}
+
+bool PDH_status4_v_bus_is_in_range(uint16_t value)
+{
+    return (value <= 4095u);
+}
+
+uint8_t PDH_status4_system_enable_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_system_enable_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_system_enable_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_rsvd0_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_rsvd0_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_rsvd0_is_in_range(uint8_t value)
+{
+    return (value <= 7u);
+}
+
+uint8_t PDH_status4_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_rsvd1_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_rsvd1_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_rsvd1_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_can_warning_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_can_warning_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_can_warning_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_hardware_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_hardware_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_hardware_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sw_state_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sw_state_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sw_state_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_rsvd2_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_rsvd2_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_rsvd2_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_can_warning_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_can_warning_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_can_warning_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_can_bus_off_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_can_bus_off_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_can_bus_off_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_hardware_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_hardware_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_hardware_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_firmware_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_firmware_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_firmware_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_ch20_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_ch20_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_ch20_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_ch21_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_ch21_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_ch21_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_ch22_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_ch22_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_ch22_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_ch23_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_ch23_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_ch23_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_sticky_has_reset_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_status4_sticky_has_reset_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_status4_sticky_has_reset_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PDH_status4_total_current_encode(double value)
+{
+    return (uint8_t)(value / 2.0);
+}
+
+double PDH_status4_total_current_decode(uint8_t value)
+{
+    return ((double)value * 2.0);
+}
+
+bool PDH_status4_total_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+int PDH_clear_faults_pack(
+    uint8_t *dst_p,
+    const struct PDH_clear_faults_t *src_p,
+    size_t size)
+{
+    (void)dst_p;
+    (void)src_p;
+    (void)size;
+
+    return (0);
+}
+
+int PDH_clear_faults_unpack(
+    struct PDH_clear_faults_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    (void)dst_p;
+    (void)src_p;
+    (void)size;
+
+    return (0);
+}
+
+int PDH_identify_pack(
+    uint8_t *dst_p,
+    const struct PDH_identify_t *src_p,
+    size_t size)
+{
+    (void)dst_p;
+    (void)src_p;
+    (void)size;
+
+    return (0);
+}
+
+int PDH_identify_unpack(
+    struct PDH_identify_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    (void)dst_p;
+    (void)src_p;
+    (void)size;
+
+    return (0);
+}
+
+int PDH_version_pack(
+    uint8_t *dst_p,
+    const struct PDH_version_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->firmware_fix, 0u, 0xffu);
+    dst_p[1] |= pack_left_shift_u8(src_p->firmware_minor, 0u, 0xffu);
+    dst_p[2] |= pack_left_shift_u8(src_p->firmware_year, 0u, 0xffu);
+    dst_p[3] |= pack_left_shift_u8(src_p->hardware_code, 0u, 0xffu);
+    dst_p[4] |= pack_left_shift_u32(src_p->unique_id, 0u, 0xffu);
+    dst_p[5] |= pack_right_shift_u32(src_p->unique_id, 8u, 0xffu);
+    dst_p[6] |= pack_right_shift_u32(src_p->unique_id, 16u, 0xffu);
+    dst_p[7] |= pack_right_shift_u32(src_p->unique_id, 24u, 0xffu);
+
+    return (8);
+}
+
+int PDH_version_unpack(
+    struct PDH_version_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->firmware_fix = unpack_right_shift_u8(src_p[0], 0u, 0xffu);
+    dst_p->firmware_minor = unpack_right_shift_u8(src_p[1], 0u, 0xffu);
+    dst_p->firmware_year = unpack_right_shift_u8(src_p[2], 0u, 0xffu);
+    dst_p->hardware_code = unpack_right_shift_u8(src_p[3], 0u, 0xffu);
+    dst_p->unique_id = unpack_right_shift_u32(src_p[4], 0u, 0xffu);
+    dst_p->unique_id |= unpack_left_shift_u32(src_p[5], 8u, 0xffu);
+    dst_p->unique_id |= unpack_left_shift_u32(src_p[6], 16u, 0xffu);
+    dst_p->unique_id |= unpack_left_shift_u32(src_p[7], 24u, 0xffu);
+
+    return (0);
+}
+
+uint8_t PDH_version_firmware_fix_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_version_firmware_fix_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_version_firmware_fix_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_version_firmware_minor_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_version_firmware_minor_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_version_firmware_minor_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_version_firmware_year_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_version_firmware_year_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_version_firmware_year_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PDH_version_hardware_code_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_version_hardware_code_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_version_hardware_code_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint32_t PDH_version_unique_id_encode(double value)
+{
+    return (uint32_t)(value);
+}
+
+double PDH_version_unique_id_decode(uint32_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_version_unique_id_is_in_range(uint32_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+int PDH_configure_hr_channel_pack(
+    uint8_t *dst_p,
+    const struct PDH_configure_hr_channel_t *src_p,
+    size_t size)
+{
+    if (size < 3u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 3);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->channel, 0u, 0xffu);
+    dst_p[1] |= pack_left_shift_u16(src_p->period, 0u, 0xffu);
+    dst_p[2] |= pack_right_shift_u16(src_p->period, 8u, 0xffu);
+
+    return (3);
+}
+
+int PDH_configure_hr_channel_unpack(
+    struct PDH_configure_hr_channel_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 3u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel = unpack_right_shift_u8(src_p[0], 0u, 0xffu);
+    dst_p->period = unpack_right_shift_u16(src_p[1], 0u, 0xffu);
+    dst_p->period |= unpack_left_shift_u16(src_p[2], 8u, 0xffu);
+
+    return (0);
+}
+
+uint8_t PDH_configure_hr_channel_channel_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PDH_configure_hr_channel_channel_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_configure_hr_channel_channel_is_in_range(uint8_t value)
+{
+    return (value <= 23u);
+}
+
+uint16_t PDH_configure_hr_channel_period_encode(double value)
+{
+    return (uint16_t)(value);
+}
+
+double PDH_configure_hr_channel_period_decode(uint16_t value)
+{
+    return ((double)value);
+}
+
+bool PDH_configure_hr_channel_period_is_in_range(uint16_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+int PDH_enter_bootloader_pack(
+    uint8_t *dst_p,
+    const struct PDH_enter_bootloader_t *src_p,
+    size_t size)
+{
+    (void)dst_p;
+    (void)src_p;
+    (void)size;
+
+    return (0);
+}
+
+int PDH_enter_bootloader_unpack(
+    struct PDH_enter_bootloader_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    (void)dst_p;
+    (void)src_p;
+    (void)size;
+
+    return (0);
+}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/rev/PDHFrames.h b/third_party/allwpilib/hal/src/main/native/athena/rev/PDHFrames.h
new file mode 100644
index 0000000..5d2452a
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/rev/PDHFrames.h
@@ -0,0 +1,3136 @@
+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018-2019 Erik Moqvist
+ *
+ * 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.
+ */
+
+/**
+ * This file was generated by cantools version
+ */
+
+#ifndef PDH_H
+#define PDH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+#ifndef EINVAL
+#    define EINVAL 22
+#endif
+
+/* Frame ids. */
+#define PDH_SWITCH_CHANNEL_SET_FRAME_ID (0x8050840u)
+#define PDH_STATUS0_FRAME_ID (0x8051800u)
+#define PDH_STATUS1_FRAME_ID (0x8051840u)
+#define PDH_STATUS2_FRAME_ID (0x8051880u)
+#define PDH_STATUS3_FRAME_ID (0x80518c0u)
+#define PDH_STATUS4_FRAME_ID (0x8051900u)
+#define PDH_CLEAR_FAULTS_FRAME_ID (0x8051b80u)
+#define PDH_IDENTIFY_FRAME_ID (0x8051d80u)
+#define PDH_VERSION_FRAME_ID (0x8052600u)
+#define PDH_CONFIGURE_HR_CHANNEL_FRAME_ID (0x80538c0u)
+#define PDH_ENTER_BOOTLOADER_FRAME_ID (0x8057fc0u)
+
+/* Frame lengths in bytes. */
+#define PDH_SWITCH_CHANNEL_SET_LENGTH (1u)
+#define PDH_STATUS0_LENGTH (8u)
+#define PDH_STATUS1_LENGTH (8u)
+#define PDH_STATUS2_LENGTH (8u)
+#define PDH_STATUS3_LENGTH (8u)
+#define PDH_STATUS4_LENGTH (8u)
+#define PDH_CLEAR_FAULTS_LENGTH (0u)
+#define PDH_IDENTIFY_LENGTH (0u)
+#define PDH_VERSION_LENGTH (8u)
+#define PDH_CONFIGURE_HR_CHANNEL_LENGTH (3u)
+#define PDH_ENTER_BOOTLOADER_LENGTH (0u)
+
+/* Extended or standard frame types. */
+#define PDH_SWITCH_CHANNEL_SET_IS_EXTENDED (1)
+#define PDH_STATUS0_IS_EXTENDED (1)
+#define PDH_STATUS1_IS_EXTENDED (1)
+#define PDH_STATUS2_IS_EXTENDED (1)
+#define PDH_STATUS3_IS_EXTENDED (1)
+#define PDH_STATUS4_IS_EXTENDED (1)
+#define PDH_CLEAR_FAULTS_IS_EXTENDED (1)
+#define PDH_IDENTIFY_IS_EXTENDED (1)
+#define PDH_VERSION_IS_EXTENDED (1)
+#define PDH_CONFIGURE_HR_CHANNEL_IS_EXTENDED (1)
+#define PDH_ENTER_BOOTLOADER_IS_EXTENDED (1)
+
+/* Frame cycle times in milliseconds. */
+
+
+/* Signal choices. */
+
+
+/**
+ * Signals in message SwitchChannelSet.
+ *
+ * Set the switch channel output
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_switch_channel_set_t {
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t output_set_value : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t use_system_enable : 1;
+};
+
+/**
+ * Signals in message Status0.
+ *
+ * Periodic status frame 0
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_status0_t {
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_0_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_1_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_2_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_0_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_1_brownout : 1;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_3_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_4_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_5_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_2_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_3_brownout : 1;
+};
+
+/**
+ * Signals in message Status1.
+ *
+ * Periodic status frame 1
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_status1_t {
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_6_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_7_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_8_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_4_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_5_brownout : 1;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_9_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_10_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_11_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_6_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_7_brownout : 1;
+};
+
+/**
+ * Signals in message Status2.
+ *
+ * Periodic status frame 2
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_status2_t {
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_12_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_13_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_14_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_8_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_9_brownout : 1;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_15_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_16_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_17_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_10_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_11_brownout : 1;
+};
+
+/**
+ * Signals in message Status3.
+ *
+ * Periodic status frame 3
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_status3_t {
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_18_current : 10;
+
+    /**
+     * Range: 0..1023 (0..127.875 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint16_t channel_19_current : 10;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_12_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_13_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_14_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_15_brownout : 1;
+
+    /**
+     * Range: 0..255 (0..15.9375 A)
+     * Scale: 0.0625
+     * Offset: 0
+     */
+    uint8_t channel_20_current : 8;
+
+    /**
+     * Range: 0..255 (0..15.9375 A)
+     * Scale: 0.0625
+     * Offset: 0
+     */
+    uint8_t channel_21_current : 8;
+
+    /**
+     * Range: 0..511 (0..31.9375 A)
+     * Scale: 0.0625
+     * Offset: 0
+     */
+    uint8_t channel_22_current : 8;
+
+    /**
+     * Range: 0..511 (0..31.9375 A)
+     * Scale: 0.0625
+     * Offset: 0
+     */
+    uint8_t channel_23_current : 8;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_16_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_17_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_18_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_19_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_20_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_21_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_22_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_23_brownout : 1;
+};
+
+/**
+ * Signals in message Status4.
+ *
+ * Periodic status frame 4
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_status4_t {
+    /**
+     * Range: 0..4095 (0..31.9921875 V)
+     * Scale: 0.0078125
+     * Offset: 0
+     */
+    uint16_t v_bus : 12;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t system_enable : 1;
+
+    /**
+     * Range: 0..7 (0..7 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t rsvd0 : 3;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t rsvd1 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t can_warning : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t hardware_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sw_state : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t rsvd2 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_can_warning : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_can_bus_off : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_hardware_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_firmware_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_ch20_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_ch21_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_ch22_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_ch23_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_has_reset : 1;
+
+    /**
+     * Range: 0..255 (0..510 A)
+     * Scale: 2
+     * Offset: 0
+     */
+    uint8_t total_current : 8;
+};
+
+/**
+ * Signals in message ClearFaults.
+ *
+ * Clear sticky faults on the device
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_clear_faults_t {
+    /**
+     * Dummy signal in empty message.
+     */
+    uint8_t dummy;
+};
+
+/**
+ * Signals in message Identify.
+ *
+ * Flash the LED on the device to identify this device
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_identify_t {
+    /**
+     * Dummy signal in empty message.
+     */
+    uint8_t dummy;
+};
+
+/**
+ * Signals in message Version.
+ *
+ * Get the version of the PDH device
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_version_t {
+    /**
+     * Range: 0..255 (0..255 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t firmware_fix : 8;
+
+    /**
+     * Range: 0..255 (0..255 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t firmware_minor : 8;
+
+    /**
+     * Range: 0..255 (0..255 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t firmware_year : 8;
+
+    /**
+     * Range: 0..255 (0..255 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t hardware_code : 8;
+
+    /**
+     * Range: 0..4294967295 (0..4294967295 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint32_t unique_id : 32;
+};
+
+/**
+ * Signals in message ConfigureHRChannel.
+ *
+ * Configure a periodic high-resolution channel frame to send back data for a particular channel. This can be useful for more detailed diagnostics, or even for current based control or monitoring.
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_configure_hr_channel_t {
+    /**
+     * Range: 0..23 (0..23 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel : 8;
+
+    /**
+     * Range: 0..65535 (0..65535 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint16_t period : 16;
+};
+
+/**
+ * Signals in message Enter_Bootloader.
+ *
+ * Enter the REV bootloader from user application
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PDH_enter_bootloader_t {
+    /**
+     * Dummy signal in empty message.
+     */
+    uint8_t dummy;
+};
+
+/**
+ * Pack message SwitchChannelSet.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_switch_channel_set_pack(
+    uint8_t *dst_p,
+    const struct PDH_switch_channel_set_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message SwitchChannelSet.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_switch_channel_set_unpack(
+    struct PDH_switch_channel_set_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_switch_channel_set_output_set_value_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_switch_channel_set_output_set_value_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_switch_channel_set_output_set_value_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_switch_channel_set_use_system_enable_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_switch_channel_set_use_system_enable_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_switch_channel_set_use_system_enable_is_in_range(uint8_t value);
+
+/**
+ * Pack message Status0.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_status0_pack(
+    uint8_t *dst_p,
+    const struct PDH_status0_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status0.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_status0_unpack(
+    struct PDH_status0_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status0_channel_0_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_0_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_0_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status0_channel_1_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_1_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_1_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status0_channel_2_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_2_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_2_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status0_channel_0_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_0_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_0_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status0_channel_1_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_1_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_1_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status0_channel_3_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_3_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_3_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status0_channel_4_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_4_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_4_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status0_channel_5_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_5_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_5_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status0_channel_2_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_2_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_2_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status0_channel_3_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status0_channel_3_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status0_channel_3_brownout_is_in_range(uint8_t value);
+
+/**
+ * Pack message Status1.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_status1_pack(
+    uint8_t *dst_p,
+    const struct PDH_status1_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status1.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_status1_unpack(
+    struct PDH_status1_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status1_channel_6_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_6_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_6_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status1_channel_7_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_7_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_7_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status1_channel_8_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_8_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_8_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status1_channel_4_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_4_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_4_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status1_channel_5_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_5_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_5_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status1_channel_9_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_9_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_9_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status1_channel_10_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_10_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_10_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status1_channel_11_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_11_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_11_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status1_channel_6_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_6_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_6_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status1_channel_7_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status1_channel_7_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status1_channel_7_brownout_is_in_range(uint8_t value);
+
+/**
+ * Pack message Status2.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_status2_pack(
+    uint8_t *dst_p,
+    const struct PDH_status2_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status2.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_status2_unpack(
+    struct PDH_status2_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status2_channel_12_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_12_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_12_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status2_channel_13_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_13_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_13_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status2_channel_14_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_14_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_14_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status2_channel_8_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_8_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_8_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status2_channel_9_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_9_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_9_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status2_channel_15_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_15_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_15_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status2_channel_16_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_16_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_16_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status2_channel_17_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_17_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_17_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status2_channel_10_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_10_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_10_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status2_channel_11_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status2_channel_11_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status2_channel_11_brownout_is_in_range(uint8_t value);
+
+/**
+ * Pack message Status3.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_status3_pack(
+    uint8_t *dst_p,
+    const struct PDH_status3_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status3.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_status3_unpack(
+    struct PDH_status3_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status3_channel_18_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_18_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_18_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status3_channel_19_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_19_current_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_19_current_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_12_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_12_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_12_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_13_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_13_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_13_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_14_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_14_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_14_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_15_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_15_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_15_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_20_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_20_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_20_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_21_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_21_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_21_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_22_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_22_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_22_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_23_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_23_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_23_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_16_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_16_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_16_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_17_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_17_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_17_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_18_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_18_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_18_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_19_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_19_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_19_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_20_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_20_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_20_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_21_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_21_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_21_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_22_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_22_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_22_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status3_channel_23_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status3_channel_23_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status3_channel_23_brownout_is_in_range(uint8_t value);
+
+/**
+ * Pack message Status4.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_status4_pack(
+    uint8_t *dst_p,
+    const struct PDH_status4_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status4.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_status4_unpack(
+    struct PDH_status4_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_status4_v_bus_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_v_bus_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_v_bus_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_system_enable_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_system_enable_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_system_enable_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_rsvd0_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_rsvd0_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_rsvd0_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_rsvd1_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_rsvd1_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_rsvd1_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_can_warning_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_can_warning_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_can_warning_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_hardware_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_hardware_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_hardware_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sw_state_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sw_state_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sw_state_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_rsvd2_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_rsvd2_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_rsvd2_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_can_warning_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_can_warning_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_can_warning_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_can_bus_off_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_can_bus_off_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_can_bus_off_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_hardware_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_hardware_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_hardware_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_firmware_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_firmware_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_firmware_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_ch20_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_ch20_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_ch20_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_ch21_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_ch21_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_ch21_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_ch22_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_ch22_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_ch22_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_ch23_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_ch23_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_ch23_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_sticky_has_reset_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_sticky_has_reset_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_sticky_has_reset_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_status4_total_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_status4_total_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_status4_total_current_is_in_range(uint8_t value);
+
+/**
+ * Pack message ClearFaults.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_clear_faults_pack(
+    uint8_t *dst_p,
+    const struct PDH_clear_faults_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message ClearFaults.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_clear_faults_unpack(
+    struct PDH_clear_faults_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Pack message Identify.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_identify_pack(
+    uint8_t *dst_p,
+    const struct PDH_identify_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Identify.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_identify_unpack(
+    struct PDH_identify_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Pack message Version.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_version_pack(
+    uint8_t *dst_p,
+    const struct PDH_version_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Version.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_version_unpack(
+    struct PDH_version_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_version_firmware_fix_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_version_firmware_fix_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_version_firmware_fix_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_version_firmware_minor_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_version_firmware_minor_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_version_firmware_minor_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_version_firmware_year_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_version_firmware_year_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_version_firmware_year_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_version_hardware_code_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_version_hardware_code_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_version_hardware_code_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint32_t PDH_version_unique_id_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_version_unique_id_decode(uint32_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_version_unique_id_is_in_range(uint32_t value);
+
+/**
+ * Pack message ConfigureHRChannel.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_configure_hr_channel_pack(
+    uint8_t *dst_p,
+    const struct PDH_configure_hr_channel_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message ConfigureHRChannel.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_configure_hr_channel_unpack(
+    struct PDH_configure_hr_channel_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PDH_configure_hr_channel_channel_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_configure_hr_channel_channel_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_configure_hr_channel_channel_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PDH_configure_hr_channel_period_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PDH_configure_hr_channel_period_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PDH_configure_hr_channel_period_is_in_range(uint16_t value);
+
+/**
+ * Pack message Enter_Bootloader.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PDH_enter_bootloader_pack(
+    uint8_t *dst_p,
+    const struct PDH_enter_bootloader_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Enter_Bootloader.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PDH_enter_bootloader_unpack(
+    struct PDH_enter_bootloader_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/athena/rev/PHFrames.cpp b/third_party/allwpilib/hal/src/main/native/athena/rev/PHFrames.cpp
new file mode 100644
index 0000000..e7f77e0
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/rev/PHFrames.cpp
@@ -0,0 +1,1720 @@
+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018-2019 Erik Moqvist
+ *
+ * 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.
+ */
+
+/**
+ * This file was generated by cantools version
+ */
+
+#include <string.h>
+
+#include "PHFrames.h"
+
+static inline uint8_t pack_left_shift_u8(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value << shift) & mask);
+}
+
+static inline uint8_t pack_left_shift_u16(
+    uint16_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value << shift) & mask);
+}
+
+static inline uint8_t pack_right_shift_u16(
+    uint16_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value >> shift) & mask);
+}
+
+static inline uint16_t unpack_left_shift_u16(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint16_t)((uint16_t)(value & mask) << shift);
+}
+
+static inline uint8_t unpack_right_shift_u8(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint8_t)((uint8_t)(value & mask) >> shift);
+}
+
+static inline uint16_t unpack_right_shift_u16(
+    uint8_t value,
+    uint8_t shift,
+    uint8_t mask)
+{
+    return (uint16_t)((uint16_t)(value & mask) >> shift);
+}
+
+int PH_set_all_pack(
+    uint8_t *dst_p,
+    const struct PH_set_all_t *src_p,
+    size_t size)
+{
+    if (size < 4u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 4);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x03u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 2u, 0x0cu);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 4u, 0x30u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 6u, 0xc0u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_4, 0u, 0x03u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_5, 2u, 0x0cu);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_6, 4u, 0x30u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_7, 6u, 0xc0u);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x03u);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_9, 2u, 0x0cu);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_10, 4u, 0x30u);
+    dst_p[2] |= pack_left_shift_u8(src_p->channel_11, 6u, 0xc0u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_12, 0u, 0x03u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_13, 2u, 0x0cu);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_14, 4u, 0x30u);
+    dst_p[3] |= pack_left_shift_u8(src_p->channel_15, 6u, 0xc0u);
+
+    return (4);
+}
+
+int PH_set_all_unpack(
+    struct PH_set_all_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 4u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x03u);
+    dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 2u, 0x0cu);
+    dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 4u, 0x30u);
+    dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 6u, 0xc0u);
+    dst_p->channel_4 = unpack_right_shift_u8(src_p[1], 0u, 0x03u);
+    dst_p->channel_5 = unpack_right_shift_u8(src_p[1], 2u, 0x0cu);
+    dst_p->channel_6 = unpack_right_shift_u8(src_p[1], 4u, 0x30u);
+    dst_p->channel_7 = unpack_right_shift_u8(src_p[1], 6u, 0xc0u);
+    dst_p->channel_8 = unpack_right_shift_u8(src_p[2], 0u, 0x03u);
+    dst_p->channel_9 = unpack_right_shift_u8(src_p[2], 2u, 0x0cu);
+    dst_p->channel_10 = unpack_right_shift_u8(src_p[2], 4u, 0x30u);
+    dst_p->channel_11 = unpack_right_shift_u8(src_p[2], 6u, 0xc0u);
+    dst_p->channel_12 = unpack_right_shift_u8(src_p[3], 0u, 0x03u);
+    dst_p->channel_13 = unpack_right_shift_u8(src_p[3], 2u, 0x0cu);
+    dst_p->channel_14 = unpack_right_shift_u8(src_p[3], 4u, 0x30u);
+    dst_p->channel_15 = unpack_right_shift_u8(src_p[3], 6u, 0xc0u);
+
+    return (0);
+}
+
+uint8_t PH_set_all_channel_0_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_0_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_0_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_1_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_1_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_1_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_2_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_2_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_2_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_3_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_3_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_3_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_4_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_4_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_4_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_5_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_5_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_5_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_6_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_6_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_6_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_7_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_7_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_7_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_8_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_8_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_8_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_9_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_9_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_9_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_10_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_10_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_10_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_11_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_11_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_11_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_12_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_12_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_12_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_13_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_13_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_13_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_14_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_14_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_14_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+uint8_t PH_set_all_channel_15_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_set_all_channel_15_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_set_all_channel_15_is_in_range(uint8_t value)
+{
+    return (value <= 3u);
+}
+
+int PH_pulse_once_pack(
+    uint8_t *dst_p,
+    const struct PH_pulse_once_t *src_p,
+    size_t size)
+{
+    if (size < 4u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 4);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x01u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 1u, 0x02u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 2u, 0x04u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 3u, 0x08u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_4, 4u, 0x10u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_5, 5u, 0x20u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_6, 6u, 0x40u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_7, 7u, 0x80u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x01u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_9, 1u, 0x02u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_10, 2u, 0x04u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_11, 3u, 0x08u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_12, 4u, 0x10u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_13, 5u, 0x20u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_14, 6u, 0x40u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_15, 7u, 0x80u);
+    dst_p[2] |= pack_left_shift_u16(src_p->pulse_length_ms, 0u, 0xffu);
+    dst_p[3] |= pack_right_shift_u16(src_p->pulse_length_ms, 8u, 0xffu);
+
+    return (4);
+}
+
+int PH_pulse_once_unpack(
+    struct PH_pulse_once_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 4u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x01u);
+    dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 1u, 0x02u);
+    dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 2u, 0x04u);
+    dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 3u, 0x08u);
+    dst_p->channel_4 = unpack_right_shift_u8(src_p[0], 4u, 0x10u);
+    dst_p->channel_5 = unpack_right_shift_u8(src_p[0], 5u, 0x20u);
+    dst_p->channel_6 = unpack_right_shift_u8(src_p[0], 6u, 0x40u);
+    dst_p->channel_7 = unpack_right_shift_u8(src_p[0], 7u, 0x80u);
+    dst_p->channel_8 = unpack_right_shift_u8(src_p[1], 0u, 0x01u);
+    dst_p->channel_9 = unpack_right_shift_u8(src_p[1], 1u, 0x02u);
+    dst_p->channel_10 = unpack_right_shift_u8(src_p[1], 2u, 0x04u);
+    dst_p->channel_11 = unpack_right_shift_u8(src_p[1], 3u, 0x08u);
+    dst_p->channel_12 = unpack_right_shift_u8(src_p[1], 4u, 0x10u);
+    dst_p->channel_13 = unpack_right_shift_u8(src_p[1], 5u, 0x20u);
+    dst_p->channel_14 = unpack_right_shift_u8(src_p[1], 6u, 0x40u);
+    dst_p->channel_15 = unpack_right_shift_u8(src_p[1], 7u, 0x80u);
+    dst_p->pulse_length_ms = unpack_right_shift_u16(src_p[2], 0u, 0xffu);
+    dst_p->pulse_length_ms |= unpack_left_shift_u16(src_p[3], 8u, 0xffu);
+
+    return (0);
+}
+
+uint8_t PH_pulse_once_channel_0_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_0_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_0_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_1_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_1_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_1_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_2_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_2_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_2_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_3_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_3_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_3_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_4_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_4_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_4_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_5_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_5_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_5_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_6_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_6_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_6_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_7_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_7_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_7_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_8_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_8_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_8_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_9_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_9_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_9_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_10_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_10_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_10_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_11_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_11_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_11_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_12_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_12_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_12_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_13_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_13_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_13_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_14_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_14_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_14_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_pulse_once_channel_15_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_pulse_once_channel_15_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_channel_15_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint16_t PH_pulse_once_pulse_length_ms_encode(double value)
+{
+    return (uint16_t)(value);
+}
+
+double PH_pulse_once_pulse_length_ms_decode(uint16_t value)
+{
+    return ((double)value);
+}
+
+bool PH_pulse_once_pulse_length_ms_is_in_range(uint16_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+int PH_status0_pack(
+    uint8_t *dst_p,
+    const struct PH_status0_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_0, 0u, 0x01u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_1, 1u, 0x02u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_2, 2u, 0x04u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_3, 3u, 0x08u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_4, 4u, 0x10u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_5, 5u, 0x20u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_6, 6u, 0x40u);
+    dst_p[0] |= pack_left_shift_u8(src_p->channel_7, 7u, 0x80u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_8, 0u, 0x01u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_9, 1u, 0x02u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_10, 2u, 0x04u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_11, 3u, 0x08u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_12, 4u, 0x10u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_13, 5u, 0x20u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_14, 6u, 0x40u);
+    dst_p[1] |= pack_left_shift_u8(src_p->channel_15, 7u, 0x80u);
+    dst_p[2] |= pack_left_shift_u8(src_p->analog_0, 0u, 0xffu);
+    dst_p[3] |= pack_left_shift_u8(src_p->analog_1, 0u, 0xffu);
+    dst_p[4] |= pack_left_shift_u8(src_p->digital_sensor, 0u, 0x01u);
+    dst_p[4] |= pack_left_shift_u8(src_p->brownout, 1u, 0x02u);
+    dst_p[4] |= pack_left_shift_u8(src_p->compressor_oc, 2u, 0x04u);
+    dst_p[4] |= pack_left_shift_u8(src_p->compressor_open, 3u, 0x08u);
+    dst_p[4] |= pack_left_shift_u8(src_p->solenoid_oc, 4u, 0x10u);
+    dst_p[4] |= pack_left_shift_u8(src_p->can_warning, 5u, 0x20u);
+    dst_p[4] |= pack_left_shift_u8(src_p->hardware_fault, 6u, 0x40u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_0_fault, 0u, 0x01u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_1_fault, 1u, 0x02u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_2_fault, 2u, 0x04u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_3_fault, 3u, 0x08u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_4_fault, 4u, 0x10u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_5_fault, 5u, 0x20u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_6_fault, 6u, 0x40u);
+    dst_p[5] |= pack_left_shift_u8(src_p->channel_7_fault, 7u, 0x80u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_8_fault, 0u, 0x01u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_9_fault, 1u, 0x02u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_10_fault, 2u, 0x04u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_11_fault, 3u, 0x08u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_12_fault, 4u, 0x10u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_13_fault, 5u, 0x20u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_14_fault, 6u, 0x40u);
+    dst_p[6] |= pack_left_shift_u8(src_p->channel_15_fault, 7u, 0x80u);
+    dst_p[7] |= pack_left_shift_u8(src_p->compressor_on, 0u, 0x01u);
+    dst_p[7] |= pack_left_shift_u8(src_p->system_enabled, 1u, 0x02u);
+
+    return (8);
+}
+
+int PH_status0_unpack(
+    struct PH_status0_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->channel_0 = unpack_right_shift_u8(src_p[0], 0u, 0x01u);
+    dst_p->channel_1 = unpack_right_shift_u8(src_p[0], 1u, 0x02u);
+    dst_p->channel_2 = unpack_right_shift_u8(src_p[0], 2u, 0x04u);
+    dst_p->channel_3 = unpack_right_shift_u8(src_p[0], 3u, 0x08u);
+    dst_p->channel_4 = unpack_right_shift_u8(src_p[0], 4u, 0x10u);
+    dst_p->channel_5 = unpack_right_shift_u8(src_p[0], 5u, 0x20u);
+    dst_p->channel_6 = unpack_right_shift_u8(src_p[0], 6u, 0x40u);
+    dst_p->channel_7 = unpack_right_shift_u8(src_p[0], 7u, 0x80u);
+    dst_p->channel_8 = unpack_right_shift_u8(src_p[1], 0u, 0x01u);
+    dst_p->channel_9 = unpack_right_shift_u8(src_p[1], 1u, 0x02u);
+    dst_p->channel_10 = unpack_right_shift_u8(src_p[1], 2u, 0x04u);
+    dst_p->channel_11 = unpack_right_shift_u8(src_p[1], 3u, 0x08u);
+    dst_p->channel_12 = unpack_right_shift_u8(src_p[1], 4u, 0x10u);
+    dst_p->channel_13 = unpack_right_shift_u8(src_p[1], 5u, 0x20u);
+    dst_p->channel_14 = unpack_right_shift_u8(src_p[1], 6u, 0x40u);
+    dst_p->channel_15 = unpack_right_shift_u8(src_p[1], 7u, 0x80u);
+    dst_p->analog_0 = unpack_right_shift_u8(src_p[2], 0u, 0xffu);
+    dst_p->analog_1 = unpack_right_shift_u8(src_p[3], 0u, 0xffu);
+    dst_p->digital_sensor = unpack_right_shift_u8(src_p[4], 0u, 0x01u);
+    dst_p->brownout = unpack_right_shift_u8(src_p[4], 1u, 0x02u);
+    dst_p->compressor_oc = unpack_right_shift_u8(src_p[4], 2u, 0x04u);
+    dst_p->compressor_open = unpack_right_shift_u8(src_p[4], 3u, 0x08u);
+    dst_p->solenoid_oc = unpack_right_shift_u8(src_p[4], 4u, 0x10u);
+    dst_p->can_warning = unpack_right_shift_u8(src_p[4], 5u, 0x20u);
+    dst_p->hardware_fault = unpack_right_shift_u8(src_p[4], 6u, 0x40u);
+    dst_p->channel_0_fault = unpack_right_shift_u8(src_p[5], 0u, 0x01u);
+    dst_p->channel_1_fault = unpack_right_shift_u8(src_p[5], 1u, 0x02u);
+    dst_p->channel_2_fault = unpack_right_shift_u8(src_p[5], 2u, 0x04u);
+    dst_p->channel_3_fault = unpack_right_shift_u8(src_p[5], 3u, 0x08u);
+    dst_p->channel_4_fault = unpack_right_shift_u8(src_p[5], 4u, 0x10u);
+    dst_p->channel_5_fault = unpack_right_shift_u8(src_p[5], 5u, 0x20u);
+    dst_p->channel_6_fault = unpack_right_shift_u8(src_p[5], 6u, 0x40u);
+    dst_p->channel_7_fault = unpack_right_shift_u8(src_p[5], 7u, 0x80u);
+    dst_p->channel_8_fault = unpack_right_shift_u8(src_p[6], 0u, 0x01u);
+    dst_p->channel_9_fault = unpack_right_shift_u8(src_p[6], 1u, 0x02u);
+    dst_p->channel_10_fault = unpack_right_shift_u8(src_p[6], 2u, 0x04u);
+    dst_p->channel_11_fault = unpack_right_shift_u8(src_p[6], 3u, 0x08u);
+    dst_p->channel_12_fault = unpack_right_shift_u8(src_p[6], 4u, 0x10u);
+    dst_p->channel_13_fault = unpack_right_shift_u8(src_p[6], 5u, 0x20u);
+    dst_p->channel_14_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u);
+    dst_p->channel_15_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u);
+    dst_p->compressor_on = unpack_right_shift_u8(src_p[7], 0u, 0x01u);
+    dst_p->system_enabled = unpack_right_shift_u8(src_p[7], 1u, 0x02u);
+
+    return (0);
+}
+
+uint8_t PH_status0_channel_0_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_0_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_0_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_1_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_1_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_1_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_2_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_2_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_2_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_3_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_3_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_3_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_4_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_4_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_4_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_5_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_5_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_5_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_6_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_6_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_6_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_7_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_7_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_7_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_8_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_8_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_8_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_9_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_9_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_9_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_10_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_10_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_10_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_11_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_11_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_11_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_12_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_12_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_12_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_13_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_13_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_13_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_14_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_14_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_14_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_15_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_15_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_15_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_analog_0_encode(double value)
+{
+    return (uint8_t)(value / 0.01961);
+}
+
+double PH_status0_analog_0_decode(uint8_t value)
+{
+    return ((double)value * 0.01961);
+}
+
+bool PH_status0_analog_0_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PH_status0_analog_1_encode(double value)
+{
+    return (uint8_t)(value / 0.01961);
+}
+
+double PH_status0_analog_1_decode(uint8_t value)
+{
+    return ((double)value * 0.01961);
+}
+
+bool PH_status0_analog_1_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PH_status0_digital_sensor_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_digital_sensor_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_digital_sensor_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_compressor_oc_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_compressor_oc_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_compressor_oc_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_compressor_open_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_compressor_open_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_compressor_open_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_solenoid_oc_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_solenoid_oc_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_solenoid_oc_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_can_warning_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_can_warning_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_can_warning_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_hardware_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_hardware_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_hardware_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_0_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_0_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_0_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_1_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_1_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_1_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_2_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_2_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_2_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_3_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_3_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_3_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_4_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_4_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_4_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_5_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_5_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_5_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_6_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_6_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_6_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_7_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_7_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_7_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_8_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_8_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_8_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_9_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_9_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_9_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_10_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_10_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_10_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_11_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_11_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_11_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_12_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_12_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_12_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_13_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_13_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_13_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_14_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_14_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_14_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_channel_15_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_channel_15_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_channel_15_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_compressor_on_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_compressor_on_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_compressor_on_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status0_system_enabled_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status0_system_enabled_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status0_system_enabled_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+int PH_status1_pack(
+    uint8_t *dst_p,
+    const struct PH_status1_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    memset(&dst_p[0], 0, 8);
+
+    dst_p[0] |= pack_left_shift_u8(src_p->v_bus, 0u, 0xffu);
+    dst_p[1] |= pack_left_shift_u16(src_p->solenoid_voltage, 0u, 0xffu);
+    dst_p[2] |= pack_right_shift_u16(src_p->solenoid_voltage, 8u, 0x0fu);
+    dst_p[4] |= pack_left_shift_u8(src_p->compressor_current, 0u, 0xffu);
+    dst_p[5] |= pack_left_shift_u8(src_p->solenoid_current, 0u, 0xffu);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_brownout, 0u, 0x01u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_compressor_over_current, 1u, 0x02u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_compressor_not_present, 2u, 0x04u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_solenoid_over_current, 3u, 0x08u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_can_warning, 4u, 0x10u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_can_bus_off, 5u, 0x20u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_hardware_fault, 6u, 0x40u);
+    dst_p[6] |= pack_left_shift_u8(src_p->sticky_firmware_fault, 7u, 0x80u);
+    dst_p[7] |= pack_left_shift_u8(src_p->sticky_has_reset, 0u, 0x01u);
+
+    return (8);
+}
+
+int PH_status1_unpack(
+    struct PH_status1_t *dst_p,
+    const uint8_t *src_p,
+    size_t size)
+{
+    if (size < 8u) {
+        return (-EINVAL);
+    }
+
+    dst_p->v_bus = unpack_right_shift_u8(src_p[0], 0u, 0xffu);
+    dst_p->solenoid_voltage = unpack_right_shift_u16(src_p[1], 0u, 0xffu);
+    dst_p->solenoid_voltage |= unpack_left_shift_u16(src_p[2], 8u, 0x0fu);
+    dst_p->compressor_current = unpack_right_shift_u8(src_p[4], 0u, 0xffu);
+    dst_p->solenoid_current = unpack_right_shift_u8(src_p[5], 0u, 0xffu);
+    dst_p->sticky_brownout = unpack_right_shift_u8(src_p[6], 0u, 0x01u);
+    dst_p->sticky_compressor_over_current = unpack_right_shift_u8(src_p[6], 1u, 0x02u);
+    dst_p->sticky_compressor_not_present = unpack_right_shift_u8(src_p[6], 2u, 0x04u);
+    dst_p->sticky_solenoid_over_current = unpack_right_shift_u8(src_p[6], 3u, 0x08u);
+    dst_p->sticky_can_warning = unpack_right_shift_u8(src_p[6], 4u, 0x10u);
+    dst_p->sticky_can_bus_off = unpack_right_shift_u8(src_p[6], 5u, 0x20u);
+    dst_p->sticky_hardware_fault = unpack_right_shift_u8(src_p[6], 6u, 0x40u);
+    dst_p->sticky_firmware_fault = unpack_right_shift_u8(src_p[6], 7u, 0x80u);
+    dst_p->sticky_has_reset = unpack_right_shift_u8(src_p[7], 0u, 0x01u);
+
+    return (0);
+}
+
+uint8_t PH_status1_v_bus_encode(double value)
+{
+    return (uint8_t)((value - 4.0) / 0.0625);
+}
+
+double PH_status1_v_bus_decode(uint8_t value)
+{
+    return (((double)value * 0.0625) + 4.0);
+}
+
+bool PH_status1_v_bus_is_in_range(uint8_t value)
+{
+    return (value <= 192u);
+}
+
+uint16_t PH_status1_solenoid_voltage_encode(double value)
+{
+    return (uint16_t)(value / 0.0078125);
+}
+
+double PH_status1_solenoid_voltage_decode(uint16_t value)
+{
+    return ((double)value * 0.0078125);
+}
+
+bool PH_status1_solenoid_voltage_is_in_range(uint16_t value)
+{
+    return (value <= 4096u);
+}
+
+uint8_t PH_status1_compressor_current_encode(double value)
+{
+    return (uint8_t)(value / 0.125);
+}
+
+double PH_status1_compressor_current_decode(uint8_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PH_status1_compressor_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PH_status1_solenoid_current_encode(double value)
+{
+    return (uint8_t)(value / 0.125);
+}
+
+double PH_status1_solenoid_current_decode(uint8_t value)
+{
+    return ((double)value * 0.125);
+}
+
+bool PH_status1_solenoid_current_is_in_range(uint8_t value)
+{
+    (void)value;
+
+    return (true);
+}
+
+uint8_t PH_status1_sticky_brownout_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_brownout_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_brownout_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_compressor_over_current_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_compressor_over_current_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_compressor_over_current_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_compressor_not_present_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_compressor_not_present_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_compressor_not_present_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_solenoid_over_current_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_solenoid_over_current_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_solenoid_over_current_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_can_warning_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_can_warning_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_can_warning_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_can_bus_off_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_can_bus_off_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_can_bus_off_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_hardware_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_hardware_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_hardware_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_firmware_fault_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_firmware_fault_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_firmware_fault_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
+
+uint8_t PH_status1_sticky_has_reset_encode(double value)
+{
+    return (uint8_t)(value);
+}
+
+double PH_status1_sticky_has_reset_decode(uint8_t value)
+{
+    return ((double)value);
+}
+
+bool PH_status1_sticky_has_reset_is_in_range(uint8_t value)
+{
+    return (value <= 1u);
+}
diff --git a/third_party/allwpilib/hal/src/main/native/athena/rev/PHFrames.h b/third_party/allwpilib/hal/src/main/native/athena/rev/PHFrames.h
new file mode 100644
index 0000000..295be0a
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/athena/rev/PHFrames.h
@@ -0,0 +1,3249 @@
+/**
+ * The MIT License (MIT)
+ *
+ * Copyright (c) 2018-2019 Erik Moqvist
+ *
+ * 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.
+ */
+
+/**
+ * This file was generated by cantools version
+ */
+
+#ifndef PH_H
+#define PH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+
+#ifndef EINVAL
+#    define EINVAL 22
+#endif
+
+/* Frame ids. */
+#define PH_SET_ALL_FRAME_ID (0x9050c00u)
+#define PH_PULSE_ONCE_FRAME_ID (0x9050c40u)
+#define PH_STATUS0_FRAME_ID (0x9051800u)
+#define PH_STATUS1_FRAME_ID (0x9051840u)
+
+/* Frame lengths in bytes. */
+#define PH_SET_ALL_LENGTH (4u)
+#define PH_PULSE_ONCE_LENGTH (4u)
+#define PH_STATUS0_LENGTH (8u)
+#define PH_STATUS1_LENGTH (8u)
+
+/* Extended or standard frame types. */
+#define PH_SET_ALL_IS_EXTENDED (1)
+#define PH_PULSE_ONCE_IS_EXTENDED (1)
+#define PH_STATUS0_IS_EXTENDED (1)
+#define PH_STATUS1_IS_EXTENDED (1)
+
+/* Frame cycle times in milliseconds. */
+
+
+/* Signal choices. */
+
+
+/**
+ * Signals in message SetAll.
+ *
+ * Set state of all channels
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PH_set_all_t {
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_0 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_1 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_2 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_3 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_4 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_5 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_6 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_7 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_8 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_9 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_10 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_11 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_12 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_13 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_14 : 2;
+
+    /**
+     * Range: 0..3 (0..3 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_15 : 2;
+};
+
+/**
+ * Signals in message PulseOnce.
+ *
+ * Pulse selected channels once
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PH_pulse_once_t {
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_0 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_1 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_2 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_3 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_4 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_5 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_6 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_7 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_8 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_9 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_10 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_11 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_12 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_13 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_14 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_15 : 1;
+
+    /**
+     * Range: 0..65535 (0..65535 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint16_t pulse_length_ms : 16;
+};
+
+/**
+ * Signals in message Status0.
+ *
+ * Periodic status frame 0
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PH_status0_t {
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_0 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_1 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_2 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_3 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_4 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_5 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_6 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_7 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_8 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_9 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_10 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_11 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_12 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_13 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_14 : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_15 : 1;
+
+    /**
+     * Range: 0..255 (0..5.00055 V)
+     * Scale: 0.01961
+     * Offset: 0
+     */
+    uint8_t analog_0 : 8;
+
+    /**
+     * Range: 0..255 (0..5.00055 V)
+     * Scale: 0.01961
+     * Offset: 0
+     */
+    uint8_t analog_1 : 8;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t digital_sensor : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t compressor_oc : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t compressor_open : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t solenoid_oc : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t can_warning : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t hardware_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_0_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_1_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_2_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_3_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_4_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_5_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_6_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_7_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_8_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_9_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_10_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_11_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_12_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_13_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_14_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t channel_15_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t compressor_on : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t system_enabled : 1;
+};
+
+/**
+ * Signals in message Status1.
+ *
+ * Periodic status frame 1
+ *
+ * All signal values are as on the CAN bus.
+ */
+struct PH_status1_t {
+    /**
+     * Range: 0..192 (4..16 V)
+     * Scale: 0.0625
+     * Offset: 4
+     */
+    uint8_t v_bus : 8;
+
+    /**
+     * Range: 0..4096 (0..32 V)
+     * Scale: 0.0078125
+     * Offset: 0
+     */
+    uint16_t solenoid_voltage : 12;
+
+    /**
+     * Range: 0..256 (0..32 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint8_t compressor_current : 8;
+
+    /**
+     * Range: 0..256 (0..32 A)
+     * Scale: 0.125
+     * Offset: 0
+     */
+    uint8_t solenoid_current : 8;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_brownout : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_compressor_over_current : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_compressor_not_present : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_solenoid_over_current : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_can_warning : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_can_bus_off : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_hardware_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_firmware_fault : 1;
+
+    /**
+     * Range: 0..1 (0..1 -)
+     * Scale: 1
+     * Offset: 0
+     */
+    uint8_t sticky_has_reset : 1;
+};
+
+/**
+ * Pack message SetAll.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PH_set_all_pack(
+    uint8_t *dst_p,
+    const struct PH_set_all_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message SetAll.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PH_set_all_unpack(
+    struct PH_set_all_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_0_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_0_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_0_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_1_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_1_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_1_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_2_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_2_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_2_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_3_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_3_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_3_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_4_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_4_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_4_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_5_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_5_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_5_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_6_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_6_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_6_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_7_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_7_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_7_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_8_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_8_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_8_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_9_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_9_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_9_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_10_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_10_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_10_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_11_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_11_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_11_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_12_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_12_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_12_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_13_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_13_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_13_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_14_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_14_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_14_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_set_all_channel_15_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_set_all_channel_15_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_set_all_channel_15_is_in_range(uint8_t value);
+
+/**
+ * Pack message PulseOnce.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PH_pulse_once_pack(
+    uint8_t *dst_p,
+    const struct PH_pulse_once_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message PulseOnce.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PH_pulse_once_unpack(
+    struct PH_pulse_once_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_0_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_0_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_0_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_1_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_1_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_1_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_2_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_2_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_2_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_3_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_3_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_3_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_4_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_4_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_4_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_5_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_5_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_5_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_6_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_6_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_6_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_7_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_7_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_7_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_8_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_8_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_8_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_9_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_9_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_9_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_10_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_10_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_10_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_11_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_11_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_11_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_12_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_12_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_12_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_13_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_13_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_13_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_14_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_14_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_14_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_pulse_once_channel_15_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_channel_15_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_channel_15_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PH_pulse_once_pulse_length_ms_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_pulse_once_pulse_length_ms_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_pulse_once_pulse_length_ms_is_in_range(uint16_t value);
+
+/**
+ * Pack message Status0.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PH_status0_pack(
+    uint8_t *dst_p,
+    const struct PH_status0_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status0.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PH_status0_unpack(
+    struct PH_status0_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_0_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_0_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_0_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_1_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_1_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_1_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_2_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_2_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_2_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_3_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_3_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_3_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_4_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_4_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_4_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_5_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_5_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_5_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_6_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_6_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_6_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_7_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_7_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_7_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_8_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_8_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_8_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_9_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_9_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_9_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_10_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_10_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_10_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_11_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_11_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_11_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_12_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_12_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_12_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_13_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_13_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_13_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_14_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_14_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_14_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_15_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_15_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_15_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_analog_0_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_analog_0_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_analog_0_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_analog_1_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_analog_1_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_analog_1_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_digital_sensor_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_digital_sensor_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_digital_sensor_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_compressor_oc_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_compressor_oc_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_compressor_oc_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_compressor_open_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_compressor_open_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_compressor_open_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_solenoid_oc_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_solenoid_oc_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_solenoid_oc_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_can_warning_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_can_warning_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_can_warning_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_hardware_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_hardware_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_hardware_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_0_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_0_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_0_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_1_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_1_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_1_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_2_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_2_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_2_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_3_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_3_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_3_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_4_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_4_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_4_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_5_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_5_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_5_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_6_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_6_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_6_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_7_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_7_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_7_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_8_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_8_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_8_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_9_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_9_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_9_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_10_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_10_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_10_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_11_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_11_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_11_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_12_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_12_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_12_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_13_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_13_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_13_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_14_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_14_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_14_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_channel_15_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_channel_15_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_channel_15_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_compressor_on_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_compressor_on_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_compressor_on_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status0_system_enabled_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status0_system_enabled_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status0_system_enabled_is_in_range(uint8_t value);
+
+/**
+ * Pack message Status1.
+ *
+ * @param[out] dst_p Buffer to pack the message into.
+ * @param[in] src_p Data to pack.
+ * @param[in] size Size of dst_p.
+ *
+ * @return Size of packed data, or negative error code.
+ */
+int PH_status1_pack(
+    uint8_t *dst_p,
+    const struct PH_status1_t *src_p,
+    size_t size);
+
+/**
+ * Unpack message Status1.
+ *
+ * @param[out] dst_p Object to unpack the message into.
+ * @param[in] src_p Message to unpack.
+ * @param[in] size Size of src_p.
+ *
+ * @return zero(0) or negative error code.
+ */
+int PH_status1_unpack(
+    struct PH_status1_t *dst_p,
+    const uint8_t *src_p,
+    size_t size);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_v_bus_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_v_bus_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_v_bus_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint16_t PH_status1_solenoid_voltage_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_solenoid_voltage_decode(uint16_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_solenoid_voltage_is_in_range(uint16_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_compressor_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_compressor_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_compressor_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_solenoid_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_solenoid_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_solenoid_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_brownout_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_brownout_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_brownout_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_compressor_over_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_compressor_over_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_compressor_over_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_compressor_not_present_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_compressor_not_present_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_compressor_not_present_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_solenoid_over_current_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_solenoid_over_current_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_solenoid_over_current_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_can_warning_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_can_warning_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_can_warning_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_can_bus_off_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_can_bus_off_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_can_bus_off_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_hardware_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_hardware_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_hardware_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_firmware_fault_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_firmware_fault_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_firmware_fault_is_in_range(uint8_t value);
+
+/**
+ * Encode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to encode.
+ *
+ * @return Encoded signal.
+ */
+uint8_t PH_status1_sticky_has_reset_encode(double value);
+
+/**
+ * Decode given signal by applying scaling and offset.
+ *
+ * @param[in] value Signal to decode.
+ *
+ * @return Decoded signal.
+ */
+double PH_status1_sticky_has_reset_decode(uint8_t value);
+
+/**
+ * Check that given signal is in allowed range.
+ *
+ * @param[in] value Signal to check.
+ *
+ * @return true if in range, false otherwise.
+ */
+bool PH_status1_sticky_has_reset_is_in_range(uint8_t value);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/ErrorHandling.cpp b/third_party/allwpilib/hal/src/main/native/cpp/ErrorHandling.cpp
new file mode 100644
index 0000000..ad1b40b
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/ErrorHandling.cpp
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <fmt/format.h>
+#include <wpi/SmallString.h>
+
+#include "hal/Errors.h"
+#include "hal/HALBase.h"
+
+namespace {
+struct LastErrorStorage {
+  int32_t status;
+  wpi::SmallString<512> message;
+};
+}  // namespace
+
+static LastErrorStorage& GetThreadLastError() {
+  thread_local LastErrorStorage lastError;
+  return lastError;
+}
+
+namespace hal {
+void SetLastError(int32_t* status, std::string_view value) {
+  LastErrorStorage& lastError = GetThreadLastError();
+  lastError.message = value;
+  lastError.status = *status;
+  *status = HAL_USE_LAST_ERROR;
+}
+
+void SetLastErrorIndexOutOfRange(int32_t* status, std::string_view message,
+                                 int32_t minimum, int32_t maximum,
+                                 int32_t requested) {
+  SetLastError(
+      status,
+      fmt::format("{}\n Status: {}\n  Minimum: {} Maximum: {} Requested: {}",
+                  message, *status, minimum, maximum, requested));
+}
+
+void SetLastErrorPreviouslyAllocated(int32_t* status, std::string_view message,
+                                     int32_t channel,
+                                     std::string_view previousAllocation) {
+  hal::SetLastError(status,
+                    fmt::format("{} {} previously allocated.\n"
+                                "Location of the previous allocation:\n{}\n"
+                                "Location of the current allocation:",
+                                message, channel, previousAllocation));
+}
+}  // namespace hal
+
+extern "C" {
+const char* HAL_GetLastError(int32_t* status) {
+  if (*status == HAL_USE_LAST_ERROR) {
+    LastErrorStorage& lastError = GetThreadLastError();
+    *status = lastError.status;
+    return lastError.message.c_str();
+  }
+  return HAL_GetErrorMessage(*status);
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/Main.cpp b/third_party/allwpilib/hal/src/main/native/cpp/Main.cpp
index a37c2b0..a43a108 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/Main.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/Main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Main.h"
 
@@ -36,14 +33,12 @@
   mainObj->gExitCv.notify_all();
 }
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeMain() {
   static MainObj mO;
   mainObj = &mO;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -55,10 +50,16 @@
   gExitFunc = exitFunc;
 }
 
-HAL_Bool HAL_HasMain(void) { return gHasMain; }
+HAL_Bool HAL_HasMain(void) {
+  return gHasMain;
+}
 
-void HAL_RunMain(void) { gMainFunc(gMainParam); }
+void HAL_RunMain(void) {
+  gMainFunc(gMainParam);
+}
 
-void HAL_ExitMain(void) { gExitFunc(gMainParam); }
+void HAL_ExitMain(void) {
+  gExitFunc(gMainParam);
+}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/cpp/fpga_clock.cpp b/third_party/allwpilib/hal/src/main/native/cpp/cpp/fpga_clock.cpp
index ae65ee7..550596a 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/cpp/fpga_clock.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/cpp/fpga_clock.cpp
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/cpp/fpga_clock.h"
 
+#include <cstdio>
 #include <limits>
 
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 #include "hal/HALBase.h"
 
@@ -22,11 +20,12 @@
   int32_t status = 0;
   uint64_t currentTime = HAL_GetFPGATime(&status);
   if (status != 0) {
-    wpi::errs()
-        << "Call to HAL_GetFPGATime failed in fpga_clock::now() with status "
-        << status
-        << ". Initialization might have failed. Time will not be correct\n";
-    wpi::errs().flush();
+    fmt::print(
+        stderr,
+        "Call to HAL_GetFPGATime failed in fpga_clock::now() with status {}. "
+        "Initialization might have failed. Time will not be correct\n",
+        status);
+    std::fflush(stderr);
     return epoch();
   }
   return time_point(std::chrono::microseconds(currentTime));
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/handles/HandlesInternal.cpp b/third_party/allwpilib/hal/src/main/native/cpp/handles/HandlesInternal.cpp
index 5e66ce1..80dca71 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/handles/HandlesInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/handles/HandlesInternal.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/handles/HandlesInternal.h"
 
@@ -80,9 +77,13 @@
 }
 HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
                         int16_t version) {
-  if (index < 0) return HAL_kInvalidHandle;
+  if (index < 0) {
+    return HAL_kInvalidHandle;
+  }
   uint8_t hType = static_cast<uint8_t>(handleType);
-  if (hType == 0 || hType > 127) return HAL_kInvalidHandle;
+  if (hType == 0 || hType > 127) {
+    return HAL_kInvalidHandle;
+  }
   // set last 8 bits, then shift to first 8 bits
   HAL_Handle handle = hType;
   handle = handle << 8;
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp
index 6d0d256..d2c107e 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/AccelerometerJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
index 3d74b24..75da176 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/AddressableLEDJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
index c06e513..154301f 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogGyroJNI.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
+#include <wpi/jni_util.h>
+
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_AnalogGyroJNI.h"
 #include "hal/AnalogGyro.h"
@@ -27,8 +26,9 @@
   (JNIEnv* env, jclass, jint id)
 {
   int32_t status = 0;
-  HAL_GyroHandle handle =
-      HAL_InitializeAnalogGyro((HAL_AnalogInputHandle)id, &status);
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  HAL_GyroHandle handle = HAL_InitializeAnalogGyro((HAL_AnalogInputHandle)id,
+                                                   stack.c_str(), &status);
   // Analog input does range checking, so we don't need to do so.
   CheckStatusForceThrow(env, status);
   return (jint)handle;
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogJNI.cpp
index 8b920ac..a2cccb6 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/AnalogJNI.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
+#include <wpi/jni_util.h>
+
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_AnalogJNI.h"
 #include "hal/AnalogAccumulator.h"
@@ -32,9 +31,10 @@
   (JNIEnv* env, jclass, jint id)
 {
   int32_t status = 0;
-  auto analog = HAL_InitializeAnalogInputPort((HAL_PortHandle)id, &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumAnalogInputs(),
-                   hal::getPortHandleChannel((HAL_PortHandle)id));
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto analog =
+      HAL_InitializeAnalogInputPort((HAL_PortHandle)id, stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
   return (jint)analog;
 }
 
@@ -60,10 +60,10 @@
   (JNIEnv* env, jclass, jint id)
 {
   int32_t status = 0;
-  HAL_AnalogOutputHandle analog =
-      HAL_InitializeAnalogOutputPort((HAL_PortHandle)id, &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumAnalogOutputs(),
-                   hal::getPortHandleChannel((HAL_PortHandle)id));
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  HAL_AnalogOutputHandle analog = HAL_InitializeAnalogOutputPort(
+      (HAL_PortHandle)id, stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
   return (jlong)analog;
 }
 
@@ -298,6 +298,22 @@
 
 /*
  * Class:     edu_wpi_first_hal_AnalogJNI
+ * Method:    getAnalogValueToVolts
+ * Signature: (II)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_AnalogJNI_getAnalogValueToVolts
+  (JNIEnv* env, jclass, jint id, jint rawValue)
+{
+  int32_t status = 0;
+  jdouble returnValue =
+      HAL_GetAnalogValueToVolts((HAL_AnalogInputHandle)id, rawValue, &status);
+  CheckStatus(env, status);
+  return returnValue;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_AnalogJNI
  * Method:    getAnalogVoltage
  * Signature: (I)D
  */
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/CANAPIJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
index f42cc2f..106167a 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/CANAPIJNI.cpp
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
-#include <wpi/SmallString.h>
 #include <wpi/jni_util.h>
-#include <wpi/raw_ostream.h>
 
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_CANAPIJNI.h"
@@ -197,11 +192,15 @@
   if (!CheckStatus(env, status)) {
     return false;
   }
-  if (dataLength > 8) dataLength = 8;
+  if (dataLength > 8) {
+    dataLength = 8;
+  }
 
   jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
   auto javaLen = env->GetArrayLength(toSetArray);
-  if (javaLen < dataLength) dataLength = javaLen;
+  if (javaLen < dataLength) {
+    dataLength = javaLen;
+  }
   env->SetByteArrayRegion(toSetArray, 0, dataLength,
                           reinterpret_cast<jbyte*>(dataTemp));
   return true;
@@ -229,11 +228,15 @@
   if (!CheckStatus(env, status)) {
     return false;
   }
-  if (dataLength > 8) dataLength = 8;
+  if (dataLength > 8) {
+    dataLength = 8;
+  }
 
   jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
   auto javaLen = env->GetArrayLength(toSetArray);
-  if (javaLen < dataLength) dataLength = javaLen;
+  if (javaLen < dataLength) {
+    dataLength = javaLen;
+  }
   env->SetByteArrayRegion(toSetArray, 0, dataLength,
                           reinterpret_cast<jbyte*>(dataTemp));
   return true;
@@ -262,11 +265,15 @@
   if (!CheckStatus(env, status)) {
     return false;
   }
-  if (dataLength > 8) dataLength = 8;
+  if (dataLength > 8) {
+    dataLength = 8;
+  }
 
   jbyteArray toSetArray = SetCANDataObject(env, data, dataLength, timestamp);
   auto javaLen = env->GetArrayLength(toSetArray);
-  if (javaLen < dataLength) dataLength = javaLen;
+  if (javaLen < dataLength) {
+    dataLength = javaLen;
+  }
   env->SetByteArrayRegion(toSetArray, 0, dataLength,
                           reinterpret_cast<jbyte*>(dataTemp));
   return true;
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp
index 42b484b..968f656 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/CANJNI.cpp
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
-#include <wpi/SmallString.h>
 #include <wpi/jni_util.h>
-#include <wpi/raw_ostream.h>
 
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_can_CANJNI.h"
@@ -64,19 +59,21 @@
   HAL_CAN_ReceiveMessage(messageIDPtr, messageIDMask, buffer, &dataSize,
                          timeStampPtr, &status);
 
-  if (!CheckCANStatus(env, status, *messageIDPtr)) return nullptr;
+  if (!CheckCANStatus(env, status, *messageIDPtr)) {
+    return nullptr;
+  }
   return MakeJByteArray(env,
-                        wpi::StringRef{reinterpret_cast<const char*>(buffer),
-                                       static_cast<size_t>(dataSize)});
+                        std::string_view{reinterpret_cast<const char*>(buffer),
+                                         static_cast<size_t>(dataSize)});
 }
 
 /*
  * Class:     edu_wpi_first_hal_can_CANJNI
- * Method:    GetCANStatus
+ * Method:    getCANStatus
  * Signature: (Ljava/lang/Object;)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_can_CANJNI_GetCANStatus
+Java_edu_wpi_first_hal_can_CANJNI_getCANStatus
   (JNIEnv* env, jclass, jobject canStatus)
 {
   float percentBusUtilization = 0;
@@ -88,7 +85,9 @@
   HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
                        &receiveErrorCount, &transmitErrorCount, &status);
 
-  if (!CheckStatus(env, status)) return;
+  if (!CheckStatus(env, status)) {
+    return;
+  }
 
   SetCanStatusObject(env, canStatus, percentBusUtilization, busOffCount,
                      txFullCount, receiveErrorCount, transmitErrorCount);
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/CTREPCMJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/CTREPCMJNI.cpp
new file mode 100644
index 0000000..4d22f66
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/CTREPCMJNI.cpp
@@ -0,0 +1,340 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_CTREPCMJNI.h"
+#include "hal/CTREPCM.h"
+#include "hal/Ports.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    initialize
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_initialize
+  (JNIEnv* env, jclass, jint module)
+{
+  int32_t status = 0;
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto handle = HAL_InitializeCTREPCM(module, stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
+  return handle;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_free
+  (JNIEnv* env, jclass, jint handle)
+{
+  HAL_FreeCTREPCM(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    checkSolenoidChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_checkSolenoidChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HAL_CheckCTREPCMSolenoidChannel(channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressor
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressor
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressor(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    setClosedLoopControl
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_setClosedLoopControl
+  (JNIEnv* env, jclass, jint handle, jboolean enabled)
+{
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(handle, enabled, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getClosedLoopControl
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getClosedLoopControl
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMClosedLoopControl(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getPressureSwitch
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMPressureSwitch(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorCurrent
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorCurrent(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorCurrentTooHighFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorCurrentTooHighFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorCurrentTooHighStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorCurrentTooHighStickyFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result =
+      HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorShortedFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorShortedFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorShortedFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorShortedStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorShortedStickyFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorShortedStickyFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorNotConnectedFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorNotConnectedFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorNotConnectedFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getCompressorNotConnectedStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getCompressorNotConnectedStickyFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result =
+      HAL_GetCTREPCMCompressorNotConnectedStickyFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getSolenoids
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getSolenoids
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoids(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    setSolenoids
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_setSolenoids
+  (JNIEnv* env, jclass, jint handle, jint mask, jint value)
+{
+  int32_t status = 0;
+  HAL_SetCTREPCMSolenoids(handle, mask, value, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getSolenoidDisabledList
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getSolenoidDisabledList
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidDisabledList(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getSolenoidVoltageFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getSolenoidVoltageFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidVoltageFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    getSolenoidVoltageStickyFault
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_getSolenoidVoltageStickyFault
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    clearAllStickyFaults
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_clearAllStickyFaults
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  HAL_ClearAllCTREPCMStickyFaults(handle, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    fireOneShot
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_fireOneShot
+  (JNIEnv* env, jclass, jint handle, jint index)
+{
+  int32_t status = 0;
+  HAL_FireCTREPCMOneShot(handle, index, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_CTREPCMJNI
+ * Method:    setOneShotDuration
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_CTREPCMJNI_setOneShotDuration
+  (JNIEnv* env, jclass, jint handle, jint index, jint durMs)
+{
+  int32_t status = 0;
+  HAL_SetCTREPCMOneShotDuration(handle, index, durMs, &status);
+  CheckStatus(env, status, false);
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/CompressorJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/CompressorJNI.cpp
deleted file mode 100644
index c214c60..0000000
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/CompressorJNI.cpp
+++ /dev/null
@@ -1,233 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "HALUtil.h"
-#include "edu_wpi_first_hal_CompressorJNI.h"
-#include "hal/Compressor.h"
-#include "hal/Ports.h"
-#include "hal/Solenoid.h"
-
-using namespace hal;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    initializeCompressor
- * Signature: (B)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_initializeCompressor
-  (JNIEnv* env, jclass, jbyte module)
-{
-  int32_t status = 0;
-  auto handle = HAL_InitializeCompressor(module, &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumPCMModules(), module);
-
-  return (jint)handle;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    checkCompressorModule
- * Signature: (B)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_checkCompressorModule
-  (JNIEnv* env, jclass, jbyte module)
-{
-  return HAL_CheckCompressorModule(module);
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressor
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressor
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressor((HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    setCompressorClosedLoopControl
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_setCompressorClosedLoopControl
-  (JNIEnv* env, jclass, jint compressorHandle, jboolean value)
-{
-  int32_t status = 0;
-  HAL_SetCompressorClosedLoopControl((HAL_CompressorHandle)compressorHandle,
-                                     value, &status);
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorClosedLoopControl
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorClosedLoopControl
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorClosedLoopControl(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorPressureSwitch
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorPressureSwitch
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorPressureSwitch(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorCurrent
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorCurrent
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  double val =
-      HAL_GetCompressorCurrent((HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorCurrentTooHighFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorCurrentTooHighFault
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorCurrentTooHighFault(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorCurrentTooHighStickyFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorCurrentTooHighStickyFault
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorCurrentTooHighStickyFault(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorShortedStickyFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorShortedStickyFault
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorShortedStickyFault(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorShortedFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorShortedFault
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorShortedFault(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorNotConnectedStickyFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorNotConnectedStickyFault
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorNotConnectedStickyFault(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    getCompressorNotConnectedFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_getCompressorNotConnectedFault
-  (JNIEnv* env, jclass, jint compressorHandle)
-{
-  int32_t status = 0;
-  bool val = HAL_GetCompressorNotConnectedFault(
-      (HAL_CompressorHandle)compressorHandle, &status);
-  CheckStatus(env, status);
-  return val;
-}
-/*
- * Class:     edu_wpi_first_hal_CompressorJNI
- * Method:    clearAllPCMStickyFaults
- * Signature: (B)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_CompressorJNI_clearAllPCMStickyFaults
-  (JNIEnv* env, jclass, jbyte module)
-{
-  int32_t status = 0;
-  HAL_ClearAllPCMStickyFaults(static_cast<int32_t>(module), &status);
-  CheckStatus(env, status);
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/ConstantsJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
index 9c4aa63..600b7f7 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/ConstantsJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/CounterJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/CounterJNI.cpp
index 06cbceb..0565d5b 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/CounterJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/CounterJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -122,12 +119,6 @@
   HAL_SetCounterDownSource((HAL_CounterHandle)id,
                            (HAL_Handle)digitalSourceHandle,
                            (HAL_AnalogTriggerType)analogTriggerType, &status);
-  if (status == PARAMETER_OUT_OF_RANGE) {
-    ThrowIllegalArgumentException(env,
-                                  "Counter only supports DownSource in "
-                                  "TwoPulse and ExternalDirection modes.");
-    return;
-  }
   CheckStatus(env, status);
 }
 
@@ -243,10 +234,6 @@
 {
   int32_t status = 0;
   HAL_SetCounterSamplesToAverage((HAL_CounterHandle)id, value, &status);
-  if (status == PARAMETER_OUT_OF_RANGE) {
-    ThrowBoundaryException(env, value, 1, 127);
-    return;
-  }
   CheckStatus(env, status);
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/DIOJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/DIOJNI.cpp
index 428dd99..5cd6c2e 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/DIOJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/DIOJNI.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
+#include <wpi/jni_util.h>
+
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_DIOJNI.h"
 #include "hal/DIO.h"
@@ -30,10 +29,10 @@
   (JNIEnv* env, jclass, jint id, jboolean input)
 {
   int32_t status = 0;
-  auto dio = HAL_InitializeDIOPort((HAL_PortHandle)id,
-                                   static_cast<uint8_t>(input), &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumDigitalChannels(),
-                   hal::getPortHandleChannel((HAL_PortHandle)id));
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto dio = HAL_InitializeDIOPort(
+      (HAL_PortHandle)id, static_cast<uint8_t>(input), stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
   return (jint)dio;
 }
 
@@ -76,11 +75,11 @@
 /*
  * Class:     edu_wpi_first_hal_DIOJNI
  * Method:    setDIO
- * Signature: (IS)V
+ * Signature: (IZ)V
  */
 JNIEXPORT void JNICALL
 Java_edu_wpi_first_hal_DIOJNI_setDIO
-  (JNIEnv* env, jclass, jint id, jshort value)
+  (JNIEnv* env, jclass, jint id, jboolean value)
 {
   int32_t status = 0;
   HAL_SetDIO((HAL_DigitalHandle)id, value, &status);
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/DMAJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/DMAJNI.cpp
new file mode 100644
index 0000000..3e95c7b
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/DMAJNI.cpp
@@ -0,0 +1,416 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <algorithm>
+#include <cstring>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_DMAJNI.h"
+#include "hal/DMA.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+namespace hal {
+bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+                          HAL_FPGAEncoderHandle* fpgaEncoderHandle,
+                          HAL_CounterHandle* counterHandle);
+}  // namespace hal
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    initialize
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DMAJNI_initialize
+  (JNIEnv* env, jclass)
+{
+  int32_t status = 0;
+  auto handle = HAL_InitializeDMA(&status);
+  CheckStatusForceThrow(env, status);
+  return handle;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_free
+  (JNIEnv* env, jclass, jint handle)
+{
+  HAL_FreeDMA(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    setPause
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_setPause
+  (JNIEnv* env, jclass, jint handle, jboolean pause)
+{
+  int32_t status = 0;
+  HAL_SetDMAPause(handle, pause, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    setTimedTrigger
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_setTimedTrigger
+  (JNIEnv* env, jclass, jint handle, jdouble seconds)
+{
+  int32_t status = 0;
+  HAL_SetDMATimedTrigger(handle, seconds, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    setTimedTriggerCycles
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_setTimedTriggerCycles
+  (JNIEnv* env, jclass, jint handle, jint cycles)
+{
+  int32_t status = 0;
+  HAL_SetDMATimedTriggerCycles(handle, static_cast<uint32_t>(cycles), &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addEncoder
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addEncoder
+  (JNIEnv* env, jclass, jint handle, jint encoderHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMAEncoder(handle, encoderHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addEncoderPeriod
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addEncoderPeriod
+  (JNIEnv* env, jclass, jint handle, jint encoderHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMAEncoderPeriod(handle, encoderHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addCounter
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addCounter
+  (JNIEnv* env, jclass, jint handle, jint counterHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMACounter(handle, counterHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addCounterPeriod
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addCounterPeriod
+  (JNIEnv* env, jclass, jint handle, jint counterHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMACounterPeriod(handle, counterHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addDutyCycle
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addDutyCycle
+  (JNIEnv* env, jclass, jint handle, jint dutyCycleHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMADutyCycle(handle, dutyCycleHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addDigitalSource
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addDigitalSource
+  (JNIEnv* env, jclass, jint handle, jint digitalSourceHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMADigitalSource(handle, digitalSourceHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addAnalogInput
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addAnalogInput
+  (JNIEnv* env, jclass, jint handle, jint analogInputHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMAAnalogInput(handle, analogInputHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addAveragedAnalogInput
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addAveragedAnalogInput
+  (JNIEnv* env, jclass, jint handle, jint analogInputHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMAAveragedAnalogInput(handle, analogInputHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    addAnalogAccumulator
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_addAnalogAccumulator
+  (JNIEnv* env, jclass, jint handle, jint analogInputHandle)
+{
+  int32_t status = 0;
+  HAL_AddDMAAnalogAccumulator(handle, analogInputHandle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    setExternalTrigger
+ * Signature: (IIIZZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_DMAJNI_setExternalTrigger
+  (JNIEnv* env, jclass, jint handle, jint digitalSourceHandle,
+   jint analogTriggerType, jboolean rising, jboolean falling)
+{
+  int32_t status = 0;
+  int32_t idx = HAL_SetDMAExternalTrigger(
+      handle, digitalSourceHandle,
+      static_cast<HAL_AnalogTriggerType>(analogTriggerType), rising, falling,
+      &status);
+  CheckStatus(env, status);
+  return idx;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    clearSensors
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_clearSensors
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  HAL_ClearDMASensors(handle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    clearExternalTriggers
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_clearExternalTriggers
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  HAL_ClearDMAExternalTriggers(handle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    startDMA
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_startDMA
+  (JNIEnv* env, jclass, jint handle, jint queueDepth)
+{
+  int32_t status = 0;
+  HAL_StartDMA(handle, queueDepth, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    stopDMA
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_DMAJNI_stopDMA
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  HAL_StopDMA(handle, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    readDMA
+ * Signature: (ID[I[I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_DMAJNI_readDMA
+  (JNIEnv* env, jclass, jint handle, jdouble timeoutSeconds, jintArray buf,
+   jintArray store)
+{
+  int32_t status = 0;
+  HAL_DMASample dmaSample;
+  std::memset(&dmaSample, 0, sizeof(dmaSample));
+  int32_t remaining = 0;
+  HAL_DMAReadStatus readStatus =
+      HAL_ReadDMA(handle, &dmaSample, timeoutSeconds, &remaining, &status);
+  CheckStatus(env, status);
+
+  static_assert(sizeof(uint32_t) == sizeof(jint), "Java ints must be 32 bits");
+
+  env->SetIntArrayRegion(buf, 0, dmaSample.captureSize,
+                         reinterpret_cast<jint*>(dmaSample.readBuffer));
+
+  int32_t* nativeArr =
+      static_cast<int32_t*>(env->GetPrimitiveArrayCritical(store, nullptr));
+
+  std::copy_n(
+      dmaSample.channelOffsets,
+      sizeof(dmaSample.channelOffsets) / sizeof(dmaSample.channelOffsets[0]),
+      nativeArr);
+  nativeArr[22] = static_cast<int32_t>(dmaSample.captureSize);
+  nativeArr[23] = static_cast<int32_t>(dmaSample.triggerChannels);
+  nativeArr[24] = remaining;
+  nativeArr[25] = readStatus;
+
+  env->ReleasePrimitiveArrayCritical(store, nativeArr, JNI_ABORT);
+
+  return dmaSample.timeStamp;
+}
+
+// TODO sync these up
+enum DMAOffsetConstants {
+  kEnable_AI0_Low = 0,
+  kEnable_AI0_High = 1,
+  kEnable_AIAveraged0_Low = 2,
+  kEnable_AIAveraged0_High = 3,
+  kEnable_AI1_Low = 4,
+  kEnable_AI1_High = 5,
+  kEnable_AIAveraged1_Low = 6,
+  kEnable_AIAveraged1_High = 7,
+  kEnable_Accumulator0 = 8,
+  kEnable_Accumulator1 = 9,
+  kEnable_DI = 10,
+  kEnable_AnalogTriggers = 11,
+  kEnable_Counters_Low = 12,
+  kEnable_Counters_High = 13,
+  kEnable_CounterTimers_Low = 14,
+  kEnable_CounterTimers_High = 15,
+  kEnable_Encoders_Low = 16,
+  kEnable_Encoders_High = 17,
+  kEnable_EncoderTimers_Low = 18,
+  kEnable_EncoderTimers_High = 19,
+  kEnable_DutyCycle_Low = 20,
+  kEnable_DutyCycle_High = 21,
+};
+
+/*
+ * Class:     edu_wpi_first_hal_DMAJNI
+ * Method:    getSensorReadData
+ * Signature: (I)Ljava/lang/Object;
+ */
+JNIEXPORT jobject JNICALL
+Java_edu_wpi_first_hal_DMAJNI_getSensorReadData
+  (JNIEnv* env, jclass, jint handle)
+{
+  HAL_Handle halHandle = static_cast<HAL_Handle>(handle);
+
+  // Check for encoder/counter handle
+  HAL_FPGAEncoderHandle fpgaEncoderHandle = 0;
+  HAL_CounterHandle counterHandle = 0;
+  bool validEncoderHandle =
+      hal::GetEncoderBaseHandle(halHandle, &fpgaEncoderHandle, &counterHandle);
+
+  if (validEncoderHandle) {
+    if (counterHandle != HAL_kInvalidHandle) {
+      int32_t cindex = getHandleIndex(counterHandle);
+      if (cindex < 4) {
+        return CreateDMABaseStore(env, kEnable_Counters_Low, cindex);
+      } else {
+        return CreateDMABaseStore(env, kEnable_Counters_High, cindex - 4);
+      }
+    } else {
+      int32_t cindex = getHandleIndex(fpgaEncoderHandle);
+      if (cindex < 4) {
+        return CreateDMABaseStore(env, kEnable_Encoders_Low, cindex);
+      } else {
+        return CreateDMABaseStore(env, kEnable_Encoders_High, cindex - 4);
+      }
+    }
+  }
+
+  HAL_HandleEnum handleType = getHandleType(halHandle);
+  int32_t index = getHandleIndex(halHandle);
+  if (handleType == HAL_HandleEnum::DIO) {
+    return CreateDMABaseStore(env, kEnable_DI, index);
+  } else if (handleType == HAL_HandleEnum::AnalogTrigger) {
+    return CreateDMABaseStore(env, kEnable_AnalogTriggers, index);
+  } else if (handleType == HAL_HandleEnum::AnalogInput) {
+    if (index < 4) {
+      return CreateDMABaseStore(env, kEnable_AI0_Low, index);
+    } else {
+      return CreateDMABaseStore(env, kEnable_AI0_High, index - 4);
+    }
+  } else if (handleType == HAL_HandleEnum::DutyCycle) {
+    if (index < 4) {
+      return CreateDMABaseStore(env, kEnable_DutyCycle_Low, index);
+    } else {
+      return CreateDMABaseStore(env, kEnable_DutyCycle_High, index - 4);
+    }
+  } else {
+    return nullptr;
+  }
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
index cfb504d..762f7fb 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/DigitalGlitchFilterJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
index b191374..96cc27b 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/DutyCycleJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/EncoderJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/EncoderJNI.cpp
index 11686a6..f93e064 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/EncoderJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/EncoderJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -270,10 +267,6 @@
 {
   int32_t status = 0;
   HAL_SetEncoderSamplesToAverage((HAL_EncoderHandle)id, value, &status);
-  if (status == PARAMETER_OUT_OF_RANGE) {
-    ThrowBoundaryException(env, value, 1, 127);
-    return;
-  }
   CheckStatus(env, status);
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/HAL.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/HAL.cpp
index 3d736bf..4e26032 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/HAL.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/HAL.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/HAL.h"
 
@@ -12,6 +9,7 @@
 #include <cassert>
 #include <cstring>
 
+#include <fmt/format.h>
 #include <wpi/jni_util.h>
 
 #include "HALUtil.h"
@@ -86,6 +84,30 @@
 
 /*
  * Class:     edu_wpi_first_hal_HAL
+ * Method:    simPeriodicBeforeNative
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_simPeriodicBeforeNative
+  (JNIEnv*, jclass)
+{
+  HAL_SimPeriodicBefore();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_HAL
+ * Method:    simPeriodicAfterNative
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_HAL_simPeriodicAfterNative
+  (JNIEnv*, jclass)
+{
+  HAL_SimPeriodicAfter();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_HAL
  * Method:    observeUserProgramStarting
  * Signature: ()V
  */
@@ -172,7 +194,6 @@
   static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
                 "Java int must match the size of control word");
   HAL_ControlWord controlWord;
-  std::memset(&controlWord, 0, sizeof(HAL_ControlWord));
   HAL_GetControlWord(&controlWord);
   jint retVal = 0;
   std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
@@ -207,13 +228,11 @@
 
   jsize javaSize = env->GetArrayLength(axesArray);
   if (axes.count > javaSize) {
-    wpi::SmallString<128> errStr;
-    wpi::raw_svector_ostream oss{errStr};
-    oss << "Native array size larger then passed in java array size "
-        << "Native Size: " << static_cast<int>(axes.count)
-        << " Java Size: " << static_cast<int>(javaSize);
-
-    ThrowIllegalArgumentException(env, errStr.str());
+    ThrowIllegalArgumentException(
+        env,
+        fmt::format("Native array size larger then passed in java array "
+                    "size\nNative Size: {} Java Size: {}",
+                    static_cast<int>(axes.count), static_cast<int>(javaSize)));
     return 0;
   }
 
@@ -236,13 +255,11 @@
 
   jsize javaSize = env->GetArrayLength(povsArray);
   if (povs.count > javaSize) {
-    wpi::SmallString<128> errStr;
-    wpi::raw_svector_ostream oss{errStr};
-    oss << "Native array size larger then passed in java array size "
-        << "Native Size: " << static_cast<int>(povs.count)
-        << " Java Size: " << static_cast<int>(javaSize);
-
-    ThrowIllegalArgumentException(env, errStr.str());
+    ThrowIllegalArgumentException(
+        env,
+        fmt::format("Native array size larger then passed in java array "
+                    "size\nNative Size: {} Java Size: {}",
+                    static_cast<int>(povs.count), static_cast<int>(javaSize)));
     return 0;
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp
index 2938f45..1070085 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALUtil.h"
 
@@ -15,9 +12,8 @@
 #include <cstring>
 #include <string>
 
-#include <wpi/SmallString.h>
+#include <fmt/format.h>
 #include <wpi/jni_util.h>
-#include <wpi/raw_ostream.h>
 
 #include "edu_wpi_first_hal_HALUtil.h"
 #include "hal/CAN.h"
@@ -34,6 +30,12 @@
 #define kRIOStatusFeatureNotSupported (kRioStatusOffset - 193)
 #define kRIOStatusResourceNotInitialized -52010
 
+static_assert(edu_wpi_first_hal_HALUtil_RUNTIME_ROBORIO == HAL_Runtime_RoboRIO);
+static_assert(edu_wpi_first_hal_HALUtil_RUNTIME_ROBORIO2 ==
+              HAL_Runtime_RoboRIO2);
+static_assert(edu_wpi_first_hal_HALUtil_RUNTIME_SIMULATION ==
+              HAL_Runtime_Simulation);
+
 static JavaVM* jvm = nullptr;
 static JException illegalArgExCls;
 static JException boundaryExCls;
@@ -50,6 +52,7 @@
 static JClass accumulatorResultCls;
 static JClass canDataCls;
 static JClass halValueCls;
+static JClass baseStoreCls;
 
 static const JClassInit classes[] = {
     {"edu/wpi/first/hal/PWMConfigDataResult", &pwmConfigDataResultCls},
@@ -57,7 +60,8 @@
     {"edu/wpi/first/hal/MatchInfoData", &matchInfoDataCls},
     {"edu/wpi/first/hal/AccumulatorResult", &accumulatorResultCls},
     {"edu/wpi/first/hal/CANData", &canDataCls},
-    {"edu/wpi/first/hal/HALValue", &halValueCls}};
+    {"edu/wpi/first/hal/HALValue", &halValueCls},
+    {"edu/wpi/first/hal/DMAJNISample$BaseStore", &baseStoreCls}};
 
 static const JExceptionInit exceptions[] = {
     {"java/lang/IllegalArgumentException", &illegalArgExCls},
@@ -75,7 +79,7 @@
 
 namespace hal {
 
-void ThrowUncleanStatusException(JNIEnv* env, wpi::StringRef msg,
+void ThrowUncleanStatusException(JNIEnv* env, std::string_view msg,
                                  int32_t status) {
   static jmethodID func =
       env->GetMethodID(uncleanStatusExCls, "<init>", "(ILjava/lang/String;)V");
@@ -86,63 +90,64 @@
   env->Throw(static_cast<jthrowable>(exception));
 }
 
-void ThrowAllocationException(JNIEnv* env, int32_t minRange, int32_t maxRange,
-                              int32_t requestedValue, int32_t status) {
-  const char* message = HAL_GetErrorMessage(status);
-  wpi::SmallString<1024> buf;
-  wpi::raw_svector_ostream oss(buf);
-  oss << " Code: " << status << ". " << message
-      << ", Minimum Value: " << minRange << ", Maximum Value: " << maxRange
-      << ", Requested Value: " << requestedValue;
-  env->ThrowNew(allocationExCls, buf.c_str());
-  allocationExCls.Throw(env, buf.c_str());
+void ThrowAllocationException(JNIEnv* env, const char* lastError,
+                              int32_t status) {
+  allocationExCls.Throw(env,
+                        fmt::format("Code: {}\n{}", status, lastError).c_str());
 }
 
 void ThrowHalHandleException(JNIEnv* env, int32_t status) {
-  const char* message = HAL_GetErrorMessage(status);
-  wpi::SmallString<1024> buf;
-  wpi::raw_svector_ostream oss(buf);
-  oss << " Code: " << status << ". " << message;
-  halHandleExCls.Throw(env, buf.c_str());
+  const char* message = HAL_GetLastError(&status);
+  halHandleExCls.Throw(env,
+                       fmt::format(" Code: {}. {}", status, message).c_str());
 }
 
 void ReportError(JNIEnv* env, int32_t status, bool doThrow) {
-  if (status == 0) return;
+  if (status == 0) {
+    return;
+  }
+  const char* message = HAL_GetLastError(&status);
   if (status == HAL_HANDLE_ERROR) {
     ThrowHalHandleException(env, status);
+    return;
   }
-  const char* message = HAL_GetErrorMessage(status);
   if (doThrow && status < 0) {
-    wpi::SmallString<1024> buf;
-    wpi::raw_svector_ostream oss(buf);
-    oss << " Code: " << status << ". " << message;
-    ThrowUncleanStatusException(env, buf.c_str(), status);
+    ThrowUncleanStatusException(
+        env, fmt::format(" Code: {}. {}", status, message).c_str(), status);
   } else {
     std::string func;
     auto stack = GetJavaStackTrace(env, &func, "edu.wpi.first");
-    HAL_SendError(1, status, 0, message, func.c_str(), stack.c_str(), 1);
+    // Make a copy of message for safety, calling back into the HAL might
+    // invalidate the string.
+    std::string lastMessage{message};
+    HAL_SendError(1, status, 0, lastMessage.c_str(), func.c_str(),
+                  stack.c_str(), 1);
   }
 }
 
 void ThrowError(JNIEnv* env, int32_t status, int32_t minRange, int32_t maxRange,
                 int32_t requestedValue) {
-  if (status == 0) return;
+  if (status == 0) {
+    return;
+  }
+  const char* lastError = HAL_GetLastError(&status);
   if (status == NO_AVAILABLE_RESOURCES || status == RESOURCE_IS_ALLOCATED ||
       status == RESOURCE_OUT_OF_RANGE) {
-    ThrowAllocationException(env, minRange, maxRange, requestedValue, status);
+    ThrowAllocationException(env, lastError, status);
+    return;
   }
   if (status == HAL_HANDLE_ERROR) {
     ThrowHalHandleException(env, status);
+    return;
   }
-  const char* message = HAL_GetErrorMessage(status);
-  wpi::SmallString<1024> buf;
-  wpi::raw_svector_ostream oss(buf);
-  oss << " Code: " << status << ". " << message;
-  ThrowUncleanStatusException(env, buf.c_str(), status);
+  ThrowUncleanStatusException(
+      env, fmt::format(" Code: {}. {}", status, lastError).c_str(), status);
 }
 
 void ReportCANError(JNIEnv* env, int32_t status, int message_id) {
-  if (status >= 0) return;
+  if (status >= 0) {
+    return;
+  }
   switch (status) {
     case kRioStatusSuccess:
       // Everything is ok... don't throw.
@@ -150,9 +155,10 @@
     case HAL_ERR_CANSessionMux_InvalidBuffer:
     case kRIOStatusBufferInvalidSize: {
       static jmethodID invalidBufConstruct = nullptr;
-      if (!invalidBufConstruct)
+      if (!invalidBufConstruct) {
         invalidBufConstruct =
             env->GetMethodID(canInvalidBufferExCls, "<init>", "()V");
+      }
       jobject exception =
           env->NewObject(canInvalidBufferExCls, invalidBufConstruct);
       env->Throw(static_cast<jthrowable>(exception));
@@ -161,9 +167,10 @@
     case HAL_ERR_CANSessionMux_MessageNotFound:
     case kRIOStatusOperationTimedOut: {
       static jmethodID messageNotFoundConstruct = nullptr;
-      if (!messageNotFoundConstruct)
+      if (!messageNotFoundConstruct) {
         messageNotFoundConstruct =
             env->GetMethodID(canMessageNotFoundExCls, "<init>", "()V");
+      }
       jobject exception =
           env->NewObject(canMessageNotFoundExCls, messageNotFoundConstruct);
       env->Throw(static_cast<jthrowable>(exception));
@@ -171,48 +178,47 @@
     }
     case HAL_ERR_CANSessionMux_NotAllowed:
     case kRIOStatusFeatureNotSupported: {
-      wpi::SmallString<100> buf;
-      wpi::raw_svector_ostream oss(buf);
-      oss << "MessageID = " << message_id;
-      canMessageNotAllowedExCls.Throw(env, buf.c_str());
+      canMessageNotAllowedExCls.Throw(
+          env, fmt::format("MessageID = {}", message_id).c_str());
       break;
     }
     case HAL_ERR_CANSessionMux_NotInitialized:
     case kRIOStatusResourceNotInitialized: {
       static jmethodID notInitConstruct = nullptr;
-      if (!notInitConstruct)
+      if (!notInitConstruct) {
         notInitConstruct =
             env->GetMethodID(canNotInitializedExCls, "<init>", "()V");
+      }
       jobject exception =
           env->NewObject(canNotInitializedExCls, notInitConstruct);
       env->Throw(static_cast<jthrowable>(exception));
       break;
     }
     default: {
-      wpi::SmallString<100> buf;
-      wpi::raw_svector_ostream oss(buf);
-      oss << "Fatal status code detected: " << status;
-      uncleanStatusExCls.Throw(env, buf.c_str());
+      uncleanStatusExCls.Throw(
+          env, fmt::format("Fatal status code detected: {}", status).c_str());
       break;
     }
   }
 }
 
-void ThrowIllegalArgumentException(JNIEnv* env, wpi::StringRef msg) {
+void ThrowIllegalArgumentException(JNIEnv* env, std::string_view msg) {
   illegalArgExCls.Throw(env, msg);
 }
 
 void ThrowBoundaryException(JNIEnv* env, double value, double lower,
                             double upper) {
   static jmethodID getMessage = nullptr;
-  if (!getMessage)
+  if (!getMessage) {
     getMessage = env->GetStaticMethodID(boundaryExCls, "getMessage",
                                         "(DDD)Ljava/lang/String;");
+  }
 
   static jmethodID constructor = nullptr;
-  if (!constructor)
+  if (!constructor) {
     constructor =
         env->GetMethodID(boundaryExCls, "<init>", "(Ljava/lang/String;)V");
+  }
 
   jobject msg = env->CallStaticObjectMethod(
       boundaryExCls, getMessage, static_cast<jdouble>(value),
@@ -226,9 +232,10 @@
                                   int32_t deadbandMinPwm, int32_t minPwm) {
   static jmethodID constructor =
       env->GetMethodID(pwmConfigDataResultCls, "<init>", "(IIIII)V");
-  return env->NewObject(pwmConfigDataResultCls, constructor, (jint)maxPwm,
-                        (jint)deadbandMaxPwm, (jint)centerPwm,
-                        (jint)deadbandMinPwm, (jint)minPwm);
+  return env->NewObject(
+      pwmConfigDataResultCls, constructor, static_cast<jint>(maxPwm),
+      static_cast<jint>(deadbandMaxPwm), static_cast<jint>(centerPwm),
+      static_cast<jint>(deadbandMinPwm), static_cast<jint>(minPwm));
 }
 
 void SetCanStatusObject(JNIEnv* env, jobject canStatus,
@@ -237,9 +244,11 @@
                         uint32_t transmitErrorCount) {
   static jmethodID func =
       env->GetMethodID(canStatusCls, "setStatus", "(DIIII)V");
-  env->CallVoidMethod(canStatus, func, (jdouble)percentBusUtilization,
-                      (jint)busOffCount, (jint)txFullCount,
-                      (jint)receiveErrorCount, (jint)transmitErrorCount);
+  env->CallVoidMethod(
+      canStatus, func, static_cast<jdouble>(percentBusUtilization),
+      static_cast<jint>(busOffCount), static_cast<jint>(txFullCount),
+      static_cast<jint>(receiveErrorCount),
+      static_cast<jint>(transmitErrorCount));
 }
 
 void SetMatchInfoObject(JNIEnv* env, jobject matchStatus,
@@ -250,11 +259,12 @@
 
   env->CallVoidMethod(
       matchStatus, func, MakeJString(env, matchInfo.eventName),
-      MakeJString(env, wpi::StringRef{reinterpret_cast<const char*>(
-                                          matchInfo.gameSpecificMessage),
-                                      matchInfo.gameSpecificMessageSize}),
-      (jint)matchInfo.matchNumber, (jint)matchInfo.replayNumber,
-      (jint)matchInfo.matchType);
+      MakeJString(env,
+                  {reinterpret_cast<const char*>(matchInfo.gameSpecificMessage),
+                   matchInfo.gameSpecificMessageSize}),
+      static_cast<jint>(matchInfo.matchNumber),
+      static_cast<jint>(matchInfo.replayNumber),
+      static_cast<jint>(matchInfo.matchType));
 }
 
 void SetAccumulatorResultObject(JNIEnv* env, jobject accumulatorResult,
@@ -262,15 +272,16 @@
   static jmethodID func =
       env->GetMethodID(accumulatorResultCls, "set", "(JJ)V");
 
-  env->CallVoidMethod(accumulatorResult, func, (jlong)value, (jlong)count);
+  env->CallVoidMethod(accumulatorResult, func, static_cast<jlong>(value),
+                      static_cast<jlong>(count));
 }
 
 jbyteArray SetCANDataObject(JNIEnv* env, jobject canData, int32_t length,
                             uint64_t timestamp) {
   static jmethodID func = env->GetMethodID(canDataCls, "setData", "(IJ)[B");
 
-  jbyteArray retVal = static_cast<jbyteArray>(
-      env->CallObjectMethod(canData, func, (jint)length, (jlong)timestamp));
+  jbyteArray retVal = static_cast<jbyteArray>(env->CallObjectMethod(
+      canData, func, static_cast<jint>(length), static_cast<jlong>(timestamp)));
   return retVal;
 }
 
@@ -298,11 +309,18 @@
     default:
       break;
   }
-  return env->CallStaticObjectMethod(halValueCls, fromNative, (jint)value.type,
-                                     value1, value2);
+  return env->CallStaticObjectMethod(
+      halValueCls, fromNative, static_cast<jint>(value.type), value1, value2);
 }
 
-JavaVM* GetJVM() { return jvm; }
+jobject CreateDMABaseStore(JNIEnv* env, jint valueType, jint index) {
+  static jmethodID ctor = env->GetMethodID(baseStoreCls, "<init>", "(II)V");
+  return env->NewObject(baseStoreCls, ctor, valueType, index);
+}
+
+JavaVM* GetJVM() {
+  return jvm;
+}
 
 namespace sim {
 jint SimOnLoad(JavaVM* vm, void* reserved);
@@ -322,17 +340,22 @@
   jvm = vm;
 
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
+  }
 
   for (auto& c : classes) {
     *c.cls = JClass(env, c.name);
-    if (!*c.cls) return JNI_ERR;
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
   for (auto& c : exceptions) {
     *c.cls = JException(env, c.name);
-    if (!*c.cls) return JNI_ERR;
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
   return sim::SimOnLoad(vm, reserved);
@@ -342,8 +365,9 @@
   sim::SimOnUnload(vm, reserved);
 
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return;
+  }
   // Delete global references
 
   for (auto& c : classes) {
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h
index 0a192da..2afe595 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/HALUtil.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef HAL_HAL_SRC_MAIN_NATIVE_CPP_JNI_HALUTIL_H_
 #define HAL_HAL_SRC_MAIN_NATIVE_CPP_JNI_HALUTIL_H_
@@ -11,7 +8,7 @@
 #include <jni.h>
 #include <stdint.h>
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
 struct HAL_MatchInfo;
 struct HAL_Value;
@@ -24,29 +21,37 @@
                 int32_t requestedValue);
 
 inline bool CheckStatus(JNIEnv* env, int32_t status, bool doThrow = true) {
-  if (status != 0) ReportError(env, status, doThrow);
+  if (status != 0) {
+    ReportError(env, status, doThrow);
+  }
   return status == 0;
 }
 
 inline bool CheckStatusRange(JNIEnv* env, int32_t status, int32_t minRange,
                              int32_t maxRange, int32_t requestedValue) {
-  if (status != 0) ThrowError(env, status, minRange, maxRange, requestedValue);
+  if (status != 0) {
+    ThrowError(env, status, minRange, maxRange, requestedValue);
+  }
   return status == 0;
 }
 
 inline bool CheckStatusForceThrow(JNIEnv* env, int32_t status) {
-  if (status != 0) ThrowError(env, status, 0, 0, 0);
+  if (status != 0) {
+    ThrowError(env, status, 0, 0, 0);
+  }
   return status == 0;
 }
 
 void ReportCANError(JNIEnv* env, int32_t status, int32_t message_id);
 
 inline bool CheckCANStatus(JNIEnv* env, int32_t status, int32_t message_id) {
-  if (status != 0) ReportCANError(env, status, message_id);
+  if (status != 0) {
+    ReportCANError(env, status, message_id);
+  }
   return status == 0;
 }
 
-void ThrowIllegalArgumentException(JNIEnv* env, wpi::StringRef msg);
+void ThrowIllegalArgumentException(JNIEnv* env, std::string_view msg);
 void ThrowBoundaryException(JNIEnv* env, double value, double lower,
                             double upper);
 
@@ -70,6 +75,8 @@
 
 jobject CreateHALValue(JNIEnv* env, const HAL_Value& value);
 
+jobject CreateDMABaseStore(JNIEnv* env, jint valueType, jint index);
+
 JavaVM* GetJVM();
 
 }  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/I2CJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/I2CJNI.cpp
index de27df6..b605b95 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/I2CJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/I2CJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -45,7 +42,7 @@
    jbyte sendSize, jobject dataReceived, jbyte receiveSize)
 {
   uint8_t* dataToSendPtr = nullptr;
-  if (dataToSend != 0) {
+  if (dataToSend != nullptr) {
     dataToSendPtr =
         reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
   }
@@ -91,7 +88,7 @@
 {
   uint8_t* dataToSendPtr = nullptr;
 
-  if (dataToSend != 0) {
+  if (dataToSend != nullptr) {
     dataToSendPtr =
         reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
   }
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/InterruptJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/InterruptJNI.cpp
index d18ef27..ed56ce5 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/InterruptJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/InterruptJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -20,108 +17,19 @@
 
 using namespace hal;
 
-// Thread where callbacks are actually performed.
-//
-// JNI's AttachCurrentThread() creates a Java Thread object on every
-// invocation, which is both time inefficient and causes issues with Eclipse
-// (which tries to keep a thread list up-to-date and thus gets swamped).
-//
-// Instead, this class attaches just once.  When a hardware notification
-// occurs, a condition variable wakes up this thread and this thread actually
-// makes the call into Java.
-//
-// We don't want to use a FIFO here. If the user code takes too long to
-// process, we will just ignore the redundant wakeup.
-class InterruptThreadJNI : public wpi::SafeThread {
- public:
-  void Main();
-
-  bool m_notify = false;
-  uint32_t m_mask = 0;
-  jobject m_func = nullptr;
-  jmethodID m_mid;
-  jobject m_param = nullptr;
-};
-
-class InterruptJNI : public wpi::SafeThreadOwner<InterruptThreadJNI> {
- public:
-  void SetFunc(JNIEnv* env, jobject func, jmethodID mid, jobject param);
-
-  void Notify(uint32_t mask) {
-    auto thr = GetThread();
-    if (!thr) return;
-    thr->m_notify = true;
-    thr->m_mask = mask;
-    thr->m_cond.notify_one();
-  }
-};
-
-void InterruptJNI::SetFunc(JNIEnv* env, jobject func, jmethodID mid,
-                           jobject param) {
-  auto thr = GetThread();
-  if (!thr) return;
-  // free global references
-  if (thr->m_func) env->DeleteGlobalRef(thr->m_func);
-  if (thr->m_param) env->DeleteGlobalRef(thr->m_param);
-  // create global references
-  thr->m_func = env->NewGlobalRef(func);
-  thr->m_param = param ? env->NewGlobalRef(param) : nullptr;
-  thr->m_mid = mid;
-}
-
-void InterruptThreadJNI::Main() {
-  JNIEnv* env;
-  JavaVMAttachArgs args;
-  args.version = JNI_VERSION_1_2;
-  args.name = const_cast<char*>("Interrupt");
-  args.group = nullptr;
-  jint rs = GetJVM()->AttachCurrentThreadAsDaemon(
-      reinterpret_cast<void**>(&env), &args);
-  if (rs != JNI_OK) return;
-
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    m_cond.wait(lock, [&] { return !m_active || m_notify; });
-    if (!m_active) break;
-    m_notify = false;
-    if (!m_func) continue;
-    jobject func = m_func;
-    jmethodID mid = m_mid;
-    uint32_t mask = m_mask;
-    jobject param = m_param;
-    lock.unlock();  // don't hold mutex during callback execution
-    env->CallVoidMethod(func, mid, static_cast<jint>(mask), param);
-    if (env->ExceptionCheck()) {
-      env->ExceptionDescribe();
-      env->ExceptionClear();
-    }
-    lock.lock();
-  }
-
-  // free global references
-  if (m_func) env->DeleteGlobalRef(m_func);
-  if (m_param) env->DeleteGlobalRef(m_param);
-
-  GetJVM()->DetachCurrentThread();
-}
-
-void interruptHandler(uint32_t mask, void* param) {
-  static_cast<InterruptJNI*>(param)->Notify(mask);
-}
-
 extern "C" {
 
 /*
  * Class:     edu_wpi_first_hal_InterruptJNI
  * Method:    initializeInterrupts
- * Signature: (Z)I
+ * Signature: ()I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_InterruptJNI_initializeInterrupts
-  (JNIEnv* env, jclass, jboolean watcher)
+  (JNIEnv* env, jclass)
 {
   int32_t status = 0;
-  HAL_InterruptHandle interrupt = HAL_InitializeInterrupts(watcher, &status);
+  HAL_InterruptHandle interrupt = HAL_InitializeInterrupts(&status);
 
   CheckStatusForceThrow(env, status);
   return (jint)interrupt;
@@ -136,14 +44,7 @@
 Java_edu_wpi_first_hal_InterruptJNI_cleanInterrupts
   (JNIEnv* env, jclass, jint interruptHandle)
 {
-  int32_t status = 0;
-  auto param =
-      HAL_CleanInterrupts((HAL_InterruptHandle)interruptHandle, &status);
-  if (param) {
-    delete static_cast<InterruptJNI*>(param);
-  }
-
-  // ignore status, as an invalid handle just needs to be ignored.
+  HAL_CleanInterrupts((HAL_InterruptHandle)interruptHandle);
 }
 
 /*
@@ -166,36 +67,6 @@
 
 /*
  * Class:     edu_wpi_first_hal_InterruptJNI
- * Method:    enableInterrupts
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_InterruptJNI_enableInterrupts
-  (JNIEnv* env, jclass, jint interruptHandle)
-{
-  int32_t status = 0;
-  HAL_EnableInterrupts((HAL_InterruptHandle)interruptHandle, &status);
-
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_InterruptJNI
- * Method:    disableInterrupts
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_InterruptJNI_disableInterrupts
-  (JNIEnv* env, jclass, jint interruptHandle)
-{
-  int32_t status = 0;
-  HAL_DisableInterrupts((HAL_InterruptHandle)interruptHandle, &status);
-
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_InterruptJNI
  * Method:    readInterruptRisingTimestamp
  * Signature: (I)J
  */
@@ -248,37 +119,6 @@
 
 /*
  * Class:     edu_wpi_first_hal_InterruptJNI
- * Method:    attachInterruptHandler
- * Signature: (ILjava/lang/Object;Ljava/lang/Object;)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_InterruptJNI_attachInterruptHandler
-  (JNIEnv* env, jclass, jint interruptHandle, jobject handler, jobject param)
-{
-  jclass cls = env->GetObjectClass(handler);
-  if (cls == 0) {
-    assert(false);
-    return;
-  }
-  jmethodID mid = env->GetMethodID(cls, "apply", "(ILjava/lang/Object;)V");
-  if (mid == 0) {
-    assert(false);
-    return;
-  }
-
-  InterruptJNI* intr = new InterruptJNI;
-  intr->Start();
-  intr->SetFunc(env, handler, mid, param);
-
-  int32_t status = 0;
-  HAL_AttachInterruptHandler((HAL_InterruptHandle)interruptHandle,
-                             interruptHandler, intr, &status);
-
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_InterruptJNI
  * Method:    setInterruptUpSourceEdge
  * Signature: (IZZ)V
  */
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/NotifierJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/NotifierJNI.cpp
index ed0b324..4e7069e 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/NotifierJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/NotifierJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -41,6 +38,19 @@
 
 /*
  * Class:     edu_wpi_first_hal_NotifierJNI
+ * Method:    setHALThreadPriority
+ * Signature: (ZI)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_NotifierJNI_setHALThreadPriority
+  (JNIEnv* env, jclass, jboolean realTime, jint priority)
+{
+  int32_t status = 0;
+  return HAL_SetNotifierThreadPriority(realTime, priority, &status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_NotifierJNI
  * Method:    setNotifierName
  * Signature: (ILjava/lang/String;)V
  */
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/PDPJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/PDPJNI.cpp
deleted file mode 100644
index bb28fdf..0000000
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/PDPJNI.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "HALUtil.h"
-#include "edu_wpi_first_hal_PDPJNI.h"
-#include "hal/PDP.h"
-#include "hal/Ports.h"
-
-using namespace hal;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    initializePDP
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_PDPJNI_initializePDP
-  (JNIEnv* env, jclass, jint module)
-{
-  int32_t status = 0;
-  auto handle = HAL_InitializePDP(module, &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumPDPModules(), module);
-  return static_cast<jint>(handle);
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    checkPDPChannel
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_PDPJNI_checkPDPChannel
-  (JNIEnv* env, jclass, jint channel)
-{
-  return HAL_CheckPDPChannel(channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    checkPDPModule
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_PDPJNI_checkPDPModule
-  (JNIEnv* env, jclass, jint module)
-{
-  return HAL_CheckPDPModule(module);
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPTemperature
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPTemperature
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  double temperature = HAL_GetPDPTemperature(handle, &status);
-  CheckStatus(env, status, false);
-  return temperature;
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPVoltage
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPVoltage
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  double voltage = HAL_GetPDPVoltage(handle, &status);
-  CheckStatus(env, status, false);
-  return voltage;
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPChannelCurrent
- * Signature: (BI)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPChannelCurrent
-  (JNIEnv* env, jclass, jbyte channel, jint handle)
-{
-  int32_t status = 0;
-  double current = HAL_GetPDPChannelCurrent(handle, channel, &status);
-  CheckStatus(env, status, false);
-  return current;
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPAllCurrents
- * Signature: (I[D)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPAllCurrents
-  (JNIEnv* env, jclass, jint handle, jdoubleArray jarr)
-{
-  double storage[16];
-  int32_t status = 0;
-  HAL_GetPDPAllChannelCurrents(handle, storage, &status);
-  if (!CheckStatus(env, status, false)) {
-    return;
-  }
-
-  env->SetDoubleArrayRegion(jarr, 0, 16, storage);
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPTotalCurrent
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPTotalCurrent
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  double current = HAL_GetPDPTotalCurrent(handle, &status);
-  CheckStatus(env, status, false);
-  return current;
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPTotalPower
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPTotalPower
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  double power = HAL_GetPDPTotalPower(handle, &status);
-  CheckStatus(env, status, false);
-  return power;
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    getPDPTotalEnergy
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_PDPJNI_getPDPTotalEnergy
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  double energy = HAL_GetPDPTotalEnergy(handle, &status);
-  CheckStatus(env, status, false);
-  return energy;
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    resetPDPTotalEnergy
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_PDPJNI_resetPDPTotalEnergy
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  HAL_ResetPDPTotalEnergy(handle, &status);
-  CheckStatus(env, status, false);
-}
-
-/*
- * Class:     edu_wpi_first_hal_PDPJNI
- * Method:    clearPDPStickyFaults
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_PDPJNI_clearPDPStickyFaults
-  (JNIEnv* env, jclass, jint handle)
-{
-  int32_t status = 0;
-  HAL_ClearPDPStickyFaults(handle, &status);
-  CheckStatus(env, status, false);
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/PWMJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/PWMJNI.cpp
index 085cd0c..e83f11b 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/PWMJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/PWMJNI.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
+#include <wpi/jni_util.h>
+
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_PWMJNI.h"
 #include "hal/DIO.h"
@@ -30,9 +29,9 @@
   (JNIEnv* env, jclass, jint id)
 {
   int32_t status = 0;
-  auto pwm = HAL_InitializePWMPort((HAL_PortHandle)id, &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumPWMChannels(),
-                   hal::getPortHandleChannel((HAL_PortHandle)id));
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto pwm = HAL_InitializePWMPort((HAL_PortHandle)id, stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
   return (jint)pwm;
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/PortsJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/PortsJNI.cpp
index 65fabd4..9757f44 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/PortsJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/PortsJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -200,53 +197,105 @@
 
 /*
  * Class:     edu_wpi_first_hal_PortsJNI
- * Method:    getNumPCMModules
+ * Method:    getNumCTREPCMModules
  * Signature: ()I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_PortsJNI_getNumPCMModules
+Java_edu_wpi_first_hal_PortsJNI_getNumCTREPCMModules
   (JNIEnv* env, jclass)
 {
-  jint value = HAL_GetNumPCMModules();
+  jint value = HAL_GetNumCTREPCMModules();
   return value;
 }
 
 /*
  * Class:     edu_wpi_first_hal_PortsJNI
- * Method:    getNumSolenoidChannels
+ * Method:    getNumCTRESolenoidChannels
  * Signature: ()I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_PortsJNI_getNumSolenoidChannels
+Java_edu_wpi_first_hal_PortsJNI_getNumCTRESolenoidChannels
   (JNIEnv* env, jclass)
 {
-  jint value = HAL_GetNumSolenoidChannels();
+  jint value = HAL_GetNumCTRESolenoidChannels();
   return value;
 }
 
 /*
  * Class:     edu_wpi_first_hal_PortsJNI
- * Method:    getNumPDPModules
+ * Method:    getNumCTREPDPModules
  * Signature: ()I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_PortsJNI_getNumPDPModules
+Java_edu_wpi_first_hal_PortsJNI_getNumCTREPDPModules
   (JNIEnv* env, jclass)
 {
-  jint value = HAL_GetNumPDPModules();
+  jint value = HAL_GetNumCTREPDPModules();
   return value;
 }
 
 /*
  * Class:     edu_wpi_first_hal_PortsJNI
- * Method:    getNumPDPChannels
+ * Method:    getNumCTREPDPChannels
  * Signature: ()I
  */
 JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_PortsJNI_getNumPDPChannels
+Java_edu_wpi_first_hal_PortsJNI_getNumCTREPDPChannels
   (JNIEnv* env, jclass)
 {
-  jint value = HAL_GetNumPDPChannels();
+  jint value = HAL_GetNumCTREPDPChannels();
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PortsJNI
+ * Method:    getNumREVPDHModules
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumREVPDHModules
+  (JNIEnv* env, jclass)
+{
+  jint value = HAL_GetNumREVPDHModules();
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PortsJNI
+ * Method:    getNumREVPDHChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumREVPDHChannels
+  (JNIEnv* env, jclass)
+{
+  jint value = HAL_GetNumREVPDHChannels();
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PortsJNI
+ * Method:    getNumREVPHModules
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumREVPHModules
+  (JNIEnv* env, jclass)
+{
+  jint value = HAL_GetNumREVPHModules();
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PortsJNI
+ * Method:    getNumREVPHChannels
+ * Signature: ()I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PortsJNI_getNumREVPHChannels
+  (JNIEnv* env, jclass)
+{
+  jint value = HAL_GetNumREVPHChannels();
   return value;
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp
new file mode 100644
index 0000000..b608458
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerDistributionJNI.cpp
@@ -0,0 +1,295 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_PowerDistributionJNI.h"
+#include "hal/Ports.h"
+#include "hal/PowerDistribution.h"
+
+using namespace hal;
+
+static_assert(edu_wpi_first_hal_PowerDistributionJNI_AUTOMATIC_TYPE ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic);
+static_assert(edu_wpi_first_hal_PowerDistributionJNI_CTRE_TYPE ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE);
+static_assert(edu_wpi_first_hal_PowerDistributionJNI_REV_TYPE ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kRev);
+static_assert(edu_wpi_first_hal_PowerDistributionJNI_DEFAULT_MODULE ==
+              HAL_DEFAULT_POWER_DISTRIBUTION_MODULE);
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    initialize
+ * Signature: (II)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_initialize
+  (JNIEnv* env, jclass, jint module, jint type)
+{
+  int32_t status = 0;
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto handle = HAL_InitializePowerDistribution(
+      module, static_cast<HAL_PowerDistributionType>(type), stack.c_str(),
+      &status);
+  CheckStatusForceThrow(env, status);
+  return static_cast<jint>(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_free
+  (JNIEnv*, jclass, jint handle)
+{
+  HAL_CleanPowerDistribution(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getModuleNumber
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getModuleNumber
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetPowerDistributionModuleNumber(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    checkChannel
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_checkChannel
+  (JNIEnv* env, jclass, jint handle, jint channel)
+{
+  return HAL_CheckPowerDistributionChannel(handle, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    checkModule
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_checkModule
+  (JNIEnv* env, jclass, jint module, jint type)
+{
+  return HAL_CheckPowerDistributionModule(
+      module, static_cast<HAL_PowerDistributionType>(type));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getType
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getType
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetPowerDistributionType(handle, &status);
+  CheckStatus(env, status);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getNumChannels
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getNumChannels
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetPowerDistributionNumChannels(handle, &status);
+  CheckStatus(env, status);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getTemperature
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getTemperature
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  double temperature = HAL_GetPowerDistributionTemperature(handle, &status);
+  CheckStatus(env, status, false);
+  return temperature;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getVoltage
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  double voltage = HAL_GetPowerDistributionVoltage(handle, &status);
+  CheckStatus(env, status, false);
+  return voltage;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getChannelCurrent
+ * Signature: (II)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getChannelCurrent
+  (JNIEnv* env, jclass, jint handle, jint channel)
+{
+  int32_t status = 0;
+  double current =
+      HAL_GetPowerDistributionChannelCurrent(handle, channel, &status);
+  CheckStatus(env, status, false);
+  return current;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getAllCurrents
+ * Signature: (I[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getAllCurrents
+  (JNIEnv* env, jclass, jint handle, jdoubleArray jarr)
+{
+  double storage[16];
+  int32_t status = 0;
+  // TODO fix me
+  HAL_GetPowerDistributionAllChannelCurrents(handle, storage, 16, &status);
+  if (!CheckStatus(env, status, false)) {
+    return;
+  }
+
+  env->SetDoubleArrayRegion(jarr, 0, 16, storage);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getTotalCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getTotalCurrent
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  double current = HAL_GetPowerDistributionTotalCurrent(handle, &status);
+  CheckStatus(env, status, false);
+  return current;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getTotalPower
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getTotalPower
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  double power = HAL_GetPowerDistributionTotalPower(handle, &status);
+  CheckStatus(env, status, false);
+  return power;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getTotalEnergy
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getTotalEnergy
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  double energy = HAL_GetPowerDistributionTotalEnergy(handle, &status);
+  CheckStatus(env, status, false);
+  return energy;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    resetTotalEnergy
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_resetTotalEnergy
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  HAL_ResetPowerDistributionTotalEnergy(handle, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    clearStickyFaults
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_clearStickyFaults
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  HAL_ClearPowerDistributionStickyFaults(handle, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    setSwitchableChannel
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_setSwitchableChannel
+  (JNIEnv* env, jclass, jint handle, jboolean enabled)
+{
+  int32_t status = 0;
+  HAL_SetPowerDistributionSwitchableChannel(handle, enabled, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerDistributionJNI
+ * Method:    getSwitchableChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_PowerDistributionJNI_getSwitchableChannel
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto state = HAL_GetPowerDistributionSwitchableChannel(handle, &status);
+  CheckStatus(env, status, false);
+  return state;
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerJNI.cpp
index 6fc42b4..92b174d 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/PowerJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -225,4 +222,33 @@
   return val;
 }
 
+/*
+ * Class:     edu_wpi_first_hal_PowerJNI
+ * Method:    setBrownoutVoltage
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_PowerJNI_setBrownoutVoltage
+  (JNIEnv* env, jclass, jdouble brownoutVoltage)
+{
+  int32_t status = 0;
+  HAL_SetBrownoutVoltage(brownoutVoltage, &status);
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_PowerJNI
+ * Method:    getBrownoutVoltage
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_PowerJNI_getBrownoutVoltage
+  (JNIEnv* env, jclass)
+{
+  int32_t status = 0;
+  double val = HAL_GetBrownoutVoltage(&status);
+  CheckStatus(env, status);
+  return val;
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/REVPHJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/REVPHJNI.cpp
new file mode 100644
index 0000000..3e99430
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/REVPHJNI.cpp
@@ -0,0 +1,191 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include <wpi/jni_util.h>
+
+#include "HALUtil.h"
+#include "edu_wpi_first_hal_REVPHJNI.h"
+#include "hal/Ports.h"
+#include "hal/REVPH.h"
+#include "hal/handles/HandlesInternal.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    initialize
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_initialize
+  (JNIEnv* env, jclass, jint module)
+{
+  int32_t status = 0;
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
+  auto handle = HAL_InitializeREVPH(module, stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
+  return handle;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    free
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_free
+  (JNIEnv* env, jclass, jint handle)
+{
+  HAL_FreeREVPH(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    checkSolenoidChannel
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_checkSolenoidChannel
+  (JNIEnv*, jclass, jint channel)
+{
+  return HAL_CheckREVPHSolenoidChannel(channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    getCompressor
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_getCompressor
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressor(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    setClosedLoopControl
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_setClosedLoopControl
+  (JNIEnv* env, jclass, jint handle, jboolean enabled)
+{
+  int32_t status = 0;
+  HAL_SetREVPHClosedLoopControl(handle, enabled, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    getClosedLoopControl
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_getClosedLoopControl
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetREVPHClosedLoopControl(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    getPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_getPressureSwitch
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetREVPHPressureSwitch(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    getAnalogPressure
+ * Signature: (II)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_getAnalogPressure
+  (JNIEnv* env, jclass, jint handle, jint channel)
+{
+  int32_t status = 0;
+  auto result = HAL_GetREVPHAnalogPressure(handle, channel, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_getCompressorCurrent
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressorCurrent(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    getSolenoids
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_getSolenoids
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t status = 0;
+  auto result = HAL_GetREVPHSolenoids(handle, &status);
+  CheckStatus(env, status, false);
+  return result;
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    setSolenoids
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_setSolenoids
+  (JNIEnv* env, jclass, jint handle, jint mask, jint value)
+{
+  int32_t status = 0;
+  HAL_SetREVPHSolenoids(handle, mask, value, &status);
+  CheckStatus(env, status, false);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_REVPHJNI
+ * Method:    fireOneShot
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_REVPHJNI_fireOneShot
+  (JNIEnv* env, jclass, jint handle, jint index, jint durMs)
+{
+  int32_t status = 0;
+  HAL_FireREVPHOneShot(handle, index, durMs, &status);
+  CheckStatus(env, status, false);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/RelayJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/RelayJNI.cpp
index ce43307..9e31a9d 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/RelayJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/RelayJNI.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
+#include <wpi/jni_util.h>
+
 #include "HALUtil.h"
 #include "edu_wpi_first_hal_RelayJNI.h"
 #include "hal/Ports.h"
@@ -29,10 +28,10 @@
   (JNIEnv* env, jclass, jint id, jboolean fwd)
 {
   int32_t status = 0;
+  auto stack = wpi::java::GetJavaStackTrace(env, "edu.wpi.first");
   HAL_RelayHandle handle = HAL_InitializeRelayPort(
-      (HAL_PortHandle)id, static_cast<uint8_t>(fwd), &status);
-  CheckStatusRange(env, status, 0, HAL_GetNumRelayChannels(),
-                   hal::getPortHandleChannel((HAL_PortHandle)id));
+      (HAL_PortHandle)id, static_cast<uint8_t>(fwd), stack.c_str(), &status);
+  CheckStatusForceThrow(env, status);
   return (jint)handle;
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/SPIJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/SPIJNI.cpp
index d7709e2..67ef56d 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/SPIJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/SPIJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -45,7 +42,7 @@
    jbyte size)
 {
   uint8_t* dataToSendPtr = nullptr;
-  if (dataToSend != 0) {
+  if (dataToSend != nullptr) {
     dataToSendPtr =
         reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
   }
@@ -88,7 +85,7 @@
   (JNIEnv* env, jclass, jint port, jobject dataToSend, jbyte size)
 {
   uint8_t* dataToSendPtr = nullptr;
-  if (dataToSend != 0) {
+  if (dataToSend != nullptr) {
     dataToSendPtr =
         reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
   }
@@ -370,7 +367,9 @@
   jint retval =
       HAL_ReadSPIAutoReceivedData(static_cast<HAL_SPIPort>(port),
                                   recvBuf.data(), numToRead, timeout, &status);
-  if (!CheckStatus(env, status)) return retval;
+  if (!CheckStatus(env, status)) {
+    return retval;
+  }
   if (numToRead > 0) {
     env->SetIntArrayRegion(buffer, 0, numToRead,
                            reinterpret_cast<const jint*>(recvBuf.data()));
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/SerialPortJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
index 13f1f99..e7f81aa 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/SerialPortJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
index b69a8bb..f681e71 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/SimDeviceJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -69,25 +66,25 @@
 /*
  * Class:     edu_wpi_first_hal_SimDeviceJNI
  * Method:    createSimValueNative
- * Signature: (ILjava/lang/String;ZIJD)I
+ * Signature: (ILjava/lang/String;IIJD)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SimDeviceJNI_createSimValueNative
-  (JNIEnv* env, jclass, jint device, jstring name, jboolean readonly, jint type,
+  (JNIEnv* env, jclass, jint device, jstring name, jint direction, jint type,
    jlong value1, jdouble value2)
 {
-  return HAL_CreateSimValue(device, JStringRef{env, name}.c_str(), readonly,
+  return HAL_CreateSimValue(device, JStringRef{env, name}.c_str(), direction,
                             ValueFromJava(type, value1, value2));
 }
 
 /*
  * Class:     edu_wpi_first_hal_SimDeviceJNI
  * Method:    createSimValueEnum
- * Signature: (ILjava/lang/String;Z[Ljava/lang/Object;I)I
+ * Signature: (ILjava/lang/String;I[Ljava/lang/Object;I)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_SimDeviceJNI_createSimValueEnum
-  (JNIEnv* env, jclass, jint device, jstring name, jboolean readonly,
+  (JNIEnv* env, jclass, jint device, jstring name, jint direction,
    jobjectArray options, jint initialValue)
 {
   size_t len = env->GetArrayLength(options);
@@ -96,13 +93,52 @@
   for (size_t i = 0; i < len; ++i) {
     JLocal<jstring> elem{
         env, static_cast<jstring>(env->GetObjectArrayElement(options, i))};
-    if (!elem) return 0;
-    arr.push_back(JStringRef{env, elem}.str());
+    if (!elem) {
+      return 0;
+    }
+    arr.emplace_back(JStringRef{env, elem}.str());
   }
   wpi::SmallVector<const char*, 8> carr;
-  for (auto&& val : arr) carr.push_back(val.c_str());
-  return HAL_CreateSimValueEnum(device, JStringRef{env, name}.c_str(), readonly,
-                                len, carr.data(), initialValue);
+  for (auto&& val : arr) {
+    carr.push_back(val.c_str());
+  }
+  return HAL_CreateSimValueEnum(device, JStringRef{env, name}.c_str(),
+                                direction, len, carr.data(), initialValue);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_SimDeviceJNI
+ * Method:    createSimValueEnumDouble
+ * Signature: (ILjava/lang/String;I[Ljava/lang/Object;[DI)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_createSimValueEnumDouble
+  (JNIEnv* env, jclass, jint device, jstring name, jint direction,
+   jobjectArray options, jdoubleArray optionValues, jint initialValue)
+{
+  size_t len = env->GetArrayLength(options);
+  size_t len2 = env->GetArrayLength(optionValues);
+  if (len != len2) {
+    return 0;
+  }
+  std::vector<std::string> arr;
+  arr.reserve(len);
+  for (size_t i = 0; i < len; ++i) {
+    JLocal<jstring> elem{
+        env, static_cast<jstring>(env->GetObjectArrayElement(options, i))};
+    if (!elem) {
+      return 0;
+    }
+    arr.emplace_back(JStringRef{env, elem}.str());
+  }
+
+  wpi::SmallVector<const char*, 8> carr;
+  for (auto&& val : arr) {
+    carr.push_back(val.c_str());
+  }
+  return HAL_CreateSimValueEnumDouble(
+      device, JStringRef{env, name}.c_str(), direction, len, carr.data(),
+      JDoubleArrayRef{env, optionValues}.array().data(), initialValue);
 }
 
 /*
@@ -119,6 +155,30 @@
 
 /*
  * Class:     edu_wpi_first_hal_SimDeviceJNI
+ * Method:    getSimValueInt
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimValueInt
+  (JNIEnv*, jclass, jint handle)
+{
+  return HAL_GetSimValueInt(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_SimDeviceJNI
+ * Method:    getSimValueLong
+ * Signature: (I)J
+ */
+JNIEXPORT jlong JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_getSimValueLong
+  (JNIEnv*, jclass, jint handle)
+{
+  return HAL_GetSimValueLong(handle);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_SimDeviceJNI
  * Method:    getSimValueDouble
  * Signature: (I)D
  */
@@ -165,4 +225,16 @@
   HAL_SetSimValue(handle, ValueFromJava(type, value1, value2));
 }
 
+/*
+ * Class:     edu_wpi_first_hal_SimDeviceJNI
+ * Method:    resetSimValue
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_SimDeviceJNI_resetSimValue
+  (JNIEnv*, jclass, jint handle)
+{
+  HAL_ResetSimValue(handle);
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/SolenoidJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
deleted file mode 100644
index 4877db3..0000000
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/SolenoidJNI.cpp
+++ /dev/null
@@ -1,203 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <jni.h>
-
-#include "HALUtil.h"
-#include "edu_wpi_first_hal_SolenoidJNI.h"
-#include "hal/Ports.h"
-#include "hal/Solenoid.h"
-#include "hal/handles/HandlesInternal.h"
-
-using namespace hal;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    initializeSolenoidPort
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_initializeSolenoidPort
-  (JNIEnv* env, jclass, jint id)
-{
-  int32_t status = 0;
-  HAL_SolenoidHandle handle =
-      HAL_InitializeSolenoidPort((HAL_PortHandle)id, &status);
-
-  // Use solenoid channels, as we have to pick one.
-  CheckStatusRange(env, status, 0, HAL_GetNumSolenoidChannels(),
-                   hal::getPortHandleChannel((HAL_PortHandle)id));
-  return (jint)handle;
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    checkSolenoidChannel
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_checkSolenoidChannel
-  (JNIEnv* env, jclass, jint channel)
-{
-  return HAL_CheckSolenoidChannel(channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    checkSolenoidModule
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_checkSolenoidModule
-  (JNIEnv* env, jclass, jint module)
-{
-  return HAL_CheckSolenoidModule(module);
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    freeSolenoidPort
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_freeSolenoidPort
-  (JNIEnv* env, jclass, jint id)
-{
-  HAL_FreeSolenoidPort((HAL_SolenoidHandle)id);
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    setSolenoid
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_setSolenoid
-  (JNIEnv* env, jclass, jint solenoid_port, jboolean value)
-{
-  int32_t status = 0;
-  HAL_SetSolenoid((HAL_SolenoidHandle)solenoid_port, value, &status);
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    getSolenoid
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_getSolenoid
-  (JNIEnv* env, jclass, jint solenoid_port)
-{
-  int32_t status = 0;
-  jboolean val = HAL_GetSolenoid((HAL_SolenoidHandle)solenoid_port, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    getAllSolenoids
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_getAllSolenoids
-  (JNIEnv* env, jclass, jint module)
-{
-  int32_t status = 0;
-  jint val = HAL_GetAllSolenoids(module, &status);
-  CheckStatus(env, status);
-  return val;
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    getPCMSolenoidBlackList
- * Signature: (I)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_getPCMSolenoidBlackList
-  (JNIEnv* env, jclass, jint module)
-{
-  int32_t status = 0;
-  jint val = HAL_GetPCMSolenoidBlackList(module, &status);
-  CheckStatus(env, status);
-  return val;
-}
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    getPCMSolenoidVoltageStickyFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_getPCMSolenoidVoltageStickyFault
-  (JNIEnv* env, jclass, jint module)
-{
-  int32_t status = 0;
-  bool val = HAL_GetPCMSolenoidVoltageStickyFault(module, &status);
-  CheckStatus(env, status);
-  return val;
-}
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    getPCMSolenoidVoltageFault
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_getPCMSolenoidVoltageFault
-  (JNIEnv* env, jclass, jint module)
-{
-  int32_t status = 0;
-  bool val = HAL_GetPCMSolenoidVoltageFault(module, &status);
-  CheckStatus(env, status);
-  return val;
-}
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    clearAllPCMStickyFaults
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_clearAllPCMStickyFaults
-  (JNIEnv* env, jclass, jint module)
-{
-  int32_t status = 0;
-  HAL_ClearAllPCMStickyFaults(module, &status);
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    setOneShotDuration
- * Signature: (IJ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_setOneShotDuration
-  (JNIEnv* env, jclass, jint solenoid_port, jlong durationMS)
-{
-  int32_t status = 0;
-  HAL_SetOneShotDuration((HAL_SolenoidHandle)solenoid_port, durationMS,
-                         &status);
-  CheckStatus(env, status);
-}
-
-/*
- * Class:     edu_wpi_first_hal_SolenoidJNI
- * Method:    fireOneShot
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_SolenoidJNI_fireOneShot
-  (JNIEnv* env, jclass, jint solenoid_port)
-{
-  int32_t status = 0;
-  HAL_FireOneShot((HAL_SolenoidHandle)solenoid_port, &status);
-  CheckStatus(env, status);
-}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/ThreadsJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
index a26a4bf..94f00c5 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/ThreadsJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -29,7 +26,7 @@
   HAL_Bool isRT = false;
   auto ret = HAL_GetCurrentThreadPriority(&isRT, &status);
   CheckStatus(env, status);
-  return (jint)ret;
+  return static_cast<jint>(ret);
 }
 
 /*
@@ -45,7 +42,7 @@
   HAL_Bool isRT = false;
   HAL_GetCurrentThreadPriority(&isRT, &status);
   CheckStatus(env, status);
-  return (jboolean)isRT;
+  return static_cast<jboolean>(isRT);
 }
 
 /*
@@ -59,9 +56,9 @@
 {
   int32_t status = 0;
   auto ret = HAL_SetCurrentThreadPriority(
-      (HAL_Bool)realTime, static_cast<int32_t>(priority), &status);
-  CheckStatus(env, status);
-  return (jboolean)ret;
+      static_cast<HAL_Bool>(realTime), static_cast<int32_t>(priority), &status);
+  CheckStatus(env, status, false);
+  return static_cast<jboolean>(ret);
 }
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp
index 21d93fe..bc0b499 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AccelerometerDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp
index 8ac59ec..e888bd3 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AddressableLEDDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -260,7 +257,7 @@
       std::make_unique<HAL_AddressableLEDData[]>(HAL_kAddressableLEDMaxLength);
   int32_t length = HALSIM_GetAddressableLEDData(index, data.get());
   return MakeJByteArray(
-      env, wpi::ArrayRef(reinterpret_cast<jbyte*>(data.get()), length * 4));
+      env, wpi::span(reinterpret_cast<jbyte*>(data.get()), length * 4));
 }
 
 /*
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp
index ed867cd..6a916f5 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogGyroDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp
index 0266a76..336c0f4 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogInDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp
index 06f5be6..ce74fad 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogOutDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp
index be92c6d..f7a5147 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/AnalogTriggerDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp
index 6751e03..265b363 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "BufferCallbackStore.h"
 
 #include <jni.h>
 
+#include <cstdio>
+
 #include <wpi/jni_util.h>
 
 #include "SimulatorJNI.h"
@@ -25,16 +24,14 @@
                                     hal::HAL_HandleEnum::SimulationJni>*
     callbackHandles;
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 void InitializeBufferStore() {
   static hal::UnlimitedHandleResource<SIM_JniHandle, BufferCallbackStore,
                                       hal::HAL_HandleEnum::SimulationJni>
       cb;
   callbackHandles = &cb;
 }
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
 
 void BufferCallbackStore::create(JNIEnv* env, jobject obj) {
   m_call = JGlobal<jobject>(env, obj);
@@ -51,21 +48,21 @@
     didAttachThread = true;
     if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
       // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
+      std::puts("Failed to attach");
+      std::fflush(stdout);
       return;
     }
   } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
+    std::puts("Invalid JVM Version requested");
+    std::fflush(stdout);
   }
 
-  auto toCallbackArr =
-      MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
-                                         static_cast<size_t>(length)});
+  auto toCallbackArr = MakeJByteArray(
+      env, std::string_view{reinterpret_cast<const char*>(buffer),
+                            static_cast<size_t>(length)});
 
   env->CallVoidMethod(m_call, sim::GetBufferCallback(), MakeJString(env, name),
-                      toCallbackArr, (jint)length);
+                      toCallbackArr, static_cast<jint>(length));
 
   jbyte* fromCallbackArr = reinterpret_cast<jbyte*>(
       env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
@@ -85,7 +82,9 @@
   }
 }
 
-void BufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
+void BufferCallbackStore::free(JNIEnv* env) {
+  m_call.free(env);
+}
 
 SIM_JniHandle sim::AllocateBufferCallback(
     JNIEnv* env, jint index, jobject callback,
@@ -108,7 +107,9 @@
     uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
     SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
     auto data = callbackHandles->Get(handle);
-    if (!data) return;
+    if (!data) {
+      return;
+    }
 
     data->performCallback(name, buffer, length);
   };
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h
index 8e746d2..73cf957 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/BufferCallbackStore.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,8 +14,7 @@
 #include "hal/handles/UnlimitedHandleResource.h"
 #include "hal/simulation/NotifyListener.h"
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 class BufferCallbackStore {
  public:
   void create(JNIEnv* env, jobject obj);
@@ -34,14 +30,13 @@
 
 void InitializeBufferStore();
 
-typedef int32_t (*RegisterBufferCallbackFunc)(int32_t index,
-                                              HAL_BufferCallback callback,
-                                              void* param);
-typedef void (*FreeBufferCallbackFunc)(int32_t index, int32_t uid);
+using RegisterBufferCallbackFunc = int32_t (*)(int32_t index,
+                                               HAL_BufferCallback callback,
+                                               void* param);
+using FreeBufferCallbackFunc = void (*)(int32_t index, int32_t uid);
 
 SIM_JniHandle AllocateBufferCallback(JNIEnv* env, jint index, jobject callback,
                                      RegisterBufferCallbackFunc createCallback);
 void FreeBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
                         FreeBufferCallbackFunc freeCallback);
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CTREPCMDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CTREPCMDataJNI.cpp
new file mode 100644
index 0000000..416478c
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CTREPCMDataJNI.cpp
@@ -0,0 +1,368 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_CTREPCMDataJNI.h"
+#include "hal/simulation/CTREPCMData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterCTREPCMInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelCTREPCMInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetCTREPCMInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetCTREPCMInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerSolenoidOutputCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerSolenoidOutputCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  return sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      &HALSIM_RegisterCTREPCMSolenoidOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    cancelSolenoidOutputCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_cancelSolenoidOutputCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+  return sim::FreeChannelCallback(env, handle, index, channel,
+                                  &HALSIM_CancelCTREPCMSolenoidOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    getSolenoidOutput
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_getSolenoidOutput
+  (JNIEnv*, jclass, jint index, jint channel)
+{
+  return HALSIM_GetCTREPCMSolenoidOutput(index, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    setSolenoidOutput
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_setSolenoidOutput
+  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
+{
+  HALSIM_SetCTREPCMSolenoidOutput(index, channel, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerCompressorOnCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerCompressorOnCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterCTREPCMCompressorOnCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    cancelCompressorOnCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_cancelCompressorOnCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelCTREPCMCompressorOnCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    getCompressorOn
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_getCompressorOn
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetCTREPCMCompressorOn(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    setCompressorOn
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_setCompressorOn
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetCTREPCMCompressorOn(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerClosedLoopEnabledCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerClosedLoopEnabledCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterCTREPCMClosedLoopEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    cancelClosedLoopEnabledCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_cancelClosedLoopEnabledCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelCTREPCMClosedLoopEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    getClosedLoopEnabled
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_getClosedLoopEnabled
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetCTREPCMClosedLoopEnabled(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    setClosedLoopEnabled
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_setClosedLoopEnabled
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetCTREPCMClosedLoopEnabled(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerPressureSwitchCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerPressureSwitchCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterCTREPCMPressureSwitchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    cancelPressureSwitchCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_cancelPressureSwitchCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelCTREPCMPressureSwitchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    getPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_getPressureSwitch
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetCTREPCMPressureSwitch(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    setPressureSwitch
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_setPressureSwitch
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetCTREPCMPressureSwitch(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerCompressorCurrentCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerCompressorCurrentCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterCTREPCMCompressorCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    cancelCompressorCurrentCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_cancelCompressorCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelCTREPCMCompressorCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_getCompressorCurrent
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetCTREPCMCompressorCurrent(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    setCompressorCurrent
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_setCompressorCurrent
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetCTREPCMCompressorCurrent(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerAllNonSolenoidCallbacks
+ * Signature: (ILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerAllNonSolenoidCallbacks
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) {
+        HALSIM_RegisterCTREPCMAllNonSolenoidCallbacks(index, cb, param, in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    registerAllSolenoidCallbacks
+ * Signature: (IILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_registerAllSolenoidCallbacks
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param,
+         HAL_Bool in) {
+        HALSIM_RegisterCTREPCMAllSolenoidCallbacks(index, channel, cb, param,
+                                                   in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_CTREPCMDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_CTREPCMDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetCTREPCMData(index);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp
index 5ce046d..f57dfb3 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CallbackStore.h"
 
 #include <jni.h>
 
+#include <cstdio>
+
 #include <wpi/jni_util.h>
 
 #include "SimulatorJNI.h"
@@ -25,16 +24,14 @@
                                     hal::HAL_HandleEnum::SimulationJni>*
     callbackHandles;
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 void InitializeStore() {
   static hal::UnlimitedHandleResource<SIM_JniHandle, CallbackStore,
                                       hal::HAL_HandleEnum::SimulationJni>
       cb;
   callbackHandles = &cb;
 }
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
 
 void CallbackStore::create(JNIEnv* env, jobject obj) {
   m_call = JGlobal<jobject>(env, obj);
@@ -50,18 +47,39 @@
     didAttachThread = true;
     if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
       // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
+      std::puts("Failed to attach");
+      std::fflush(stdout);
       return;
     }
   } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
+    std::puts("Invalid JVM Version requested");
+    std::fflush(stdout);
+  }
+
+  int64_t longValue = 0;
+
+  switch (value->type) {
+    case HAL_BOOLEAN:
+      longValue = value->data.v_boolean;
+      break;
+    case HAL_ENUM:
+      longValue = value->data.v_enum;
+      break;
+    case HAL_INT:
+      longValue = value->data.v_int;
+      break;
+    case HAL_LONG:
+      longValue = value->data.v_long;
+      break;
+    case HAL_DOUBLE:
+    case HAL_UNASSIGNED:
+      break;
   }
 
   env->CallVoidMethod(m_call, sim::GetNotifyCallback(), MakeJString(env, name),
-                      (jint)value->type, (jlong)value->data.v_long,
-                      (jdouble)value->data.v_double);
+                      static_cast<jint>(value->type),
+                      static_cast<jlong>(longValue),
+                      static_cast<jdouble>(value->data.v_double));
 
   if (env->ExceptionCheck()) {
     env->ExceptionDescribe();
@@ -72,7 +90,9 @@
   }
 }
 
-void CallbackStore::free(JNIEnv* env) { m_call.free(env); }
+void CallbackStore::free(JNIEnv* env) {
+  m_call.free(env);
+}
 
 SIM_JniHandle sim::AllocateCallback(JNIEnv* env, jint index, jobject callback,
                                     jboolean initialNotify,
@@ -95,7 +115,9 @@
     uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
     SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
     auto data = callbackHandles->Get(handle);
-    if (!data) return;
+    if (!data) {
+      return;
+    }
 
     data->performCallback(name, value);
   };
@@ -135,7 +157,9 @@
     uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
     SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
     auto data = callbackHandles->Get(handle);
-    if (!data) return;
+    if (!data) {
+      return;
+    }
 
     data->performCallback(name, value);
   };
@@ -177,7 +201,9 @@
     uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
     SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
     auto data = callbackHandles->Get(handle);
-    if (!data) return;
+    if (!data) {
+      return;
+    }
 
     data->performCallback(name, value);
   };
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.h
index 0ba44d3..59e1018 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/CallbackStore.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,8 +14,7 @@
 #include "hal/handles/UnlimitedHandleResource.h"
 #include "hal/simulation/NotifyListener.h"
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 class CallbackStore {
  public:
   void create(JNIEnv* env, jobject obj);
@@ -34,20 +30,20 @@
 
 void InitializeStore();
 
-typedef int32_t (*RegisterCallbackFunc)(int32_t index,
-                                        HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
-typedef void (*FreeCallbackFunc)(int32_t index, int32_t uid);
-typedef int32_t (*RegisterChannelCallbackFunc)(int32_t index, int32_t channel,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-typedef void (*FreeChannelCallbackFunc)(int32_t index, int32_t channel,
-                                        int32_t uid);
-typedef int32_t (*RegisterCallbackNoIndexFunc)(HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-typedef void (*FreeCallbackNoIndexFunc)(int32_t uid);
+using RegisterCallbackFunc = int32_t (*)(int32_t index,
+                                         HAL_NotifyCallback callback,
+                                         void* param, HAL_Bool initialNotify);
+using FreeCallbackFunc = void (*)(int32_t index, int32_t uid);
+using RegisterChannelCallbackFunc = int32_t (*)(int32_t index, int32_t channel,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+using FreeChannelCallbackFunc = void (*)(int32_t index, int32_t channel,
+                                         int32_t uid);
+using RegisterCallbackNoIndexFunc = int32_t (*)(HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+using FreeCallbackNoIndexFunc = void (*)(int32_t uid);
 
 SIM_JniHandle AllocateCallback(JNIEnv* env, jint index, jobject callback,
                                jboolean initialNotify,
@@ -64,5 +60,4 @@
                          jint channel, FreeChannelCallbackFunc freeCallback);
 void FreeCallbackNoIndex(JNIEnv* env, SIM_JniHandle handle,
                          FreeCallbackNoIndexFunc freeCallback);
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp
index b9c9808..cba0a5b 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ConstBufferCallbackStore.h"
 
 #include <jni.h>
 
+#include <cstdio>
+
 #include <wpi/jni_util.h>
 
 #include "SimulatorJNI.h"
@@ -25,16 +24,14 @@
                                     hal::HAL_HandleEnum::SimulationJni>*
     callbackHandles;
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 void InitializeConstBufferStore() {
   static hal::UnlimitedHandleResource<SIM_JniHandle, ConstBufferCallbackStore,
                                       hal::HAL_HandleEnum::SimulationJni>
       cb;
   callbackHandles = &cb;
 }
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
 
 void ConstBufferCallbackStore::create(JNIEnv* env, jobject obj) {
   m_call = JGlobal<jobject>(env, obj);
@@ -52,21 +49,22 @@
     didAttachThread = true;
     if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
       // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
+      std::puts("Failed to attach");
+      std::fflush(stdout);
       return;
     }
   } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
+    std::puts("Invalid JVM Version requested");
+    std::fflush(stdout);
   }
 
-  auto toCallbackArr =
-      MakeJByteArray(env, wpi::StringRef{reinterpret_cast<const char*>(buffer),
-                                         static_cast<size_t>(length)});
+  auto toCallbackArr = MakeJByteArray(
+      env, std::string_view{reinterpret_cast<const char*>(buffer),
+                            static_cast<size_t>(length)});
 
   env->CallVoidMethod(m_call, sim::GetConstBufferCallback(),
-                      MakeJString(env, name), toCallbackArr, (jint)length);
+                      MakeJString(env, name), toCallbackArr,
+                      static_cast<jint>(length));
 
   if (env->ExceptionCheck()) {
     env->ExceptionDescribe();
@@ -77,7 +75,9 @@
   }
 }
 
-void ConstBufferCallbackStore::free(JNIEnv* env) { m_call.free(env); }
+void ConstBufferCallbackStore::free(JNIEnv* env) {
+  m_call.free(env);
+}
 
 SIM_JniHandle sim::AllocateConstBufferCallback(
     JNIEnv* env, jint index, jobject callback,
@@ -100,7 +100,9 @@
     uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
     SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
     auto data = callbackHandles->Get(handle);
-    if (!data) return;
+    if (!data) {
+      return;
+    }
 
     data->performCallback(name, buffer, length);
   };
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h
index b69eccf..0998d3c 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/ConstBufferCallbackStore.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,8 +14,7 @@
 #include "hal/handles/UnlimitedHandleResource.h"
 #include "hal/simulation/NotifyListener.h"
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 class ConstBufferCallbackStore {
  public:
   void create(JNIEnv* env, jobject obj);
@@ -35,14 +31,13 @@
 
 void InitializeConstBufferStore();
 
-typedef int32_t (*RegisterConstBufferCallbackFunc)(
-    int32_t index, HAL_ConstBufferCallback callback, void* param);
-typedef void (*FreeConstBufferCallbackFunc)(int32_t index, int32_t uid);
+using RegisterConstBufferCallbackFunc =
+    int32_t (*)(int32_t index, HAL_ConstBufferCallback callback, void* param);
+using FreeConstBufferCallbackFunc = void (*)(int32_t index, int32_t uid);
 
 SIM_JniHandle AllocateConstBufferCallback(
     JNIEnv* env, jint index, jobject callback,
     RegisterConstBufferCallbackFunc createCallback);
 void FreeConstBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
                              FreeConstBufferCallbackFunc freeCallback);
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp
index f757558..e81ad77 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DIODataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp
index ea6bee2..3ba3931 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DigitalPWMDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
index 7868e43..acecacb 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp
index 054fa67..9cda86d 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/DutyCycleDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp
index b97e0fe..69549a1 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/EncoderDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp
index e4c2259..c023754 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/I2CDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp
index 845c164..15b02d7 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/NotifierDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "edu_wpi_first_hal_simulation_NotifierDataJNI.h"
 #include "hal/simulation/NotifierData.h"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp
deleted file mode 100644
index cfcd2c0..0000000
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PCMDataJNI.cpp
+++ /dev/null
@@ -1,421 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_simulation_PCMDataJNI.h"
-#include "hal/simulation/PCMData.h"
-
-using namespace hal;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerSolenoidInitializedCallback
- * Signature: (IILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerSolenoidInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  return sim::AllocateChannelCallback(
-      env, index, channel, callback, initialNotify,
-      &HALSIM_RegisterPCMSolenoidInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelSolenoidInitializedCallback
- * Signature: (III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelSolenoidInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
-{
-  return sim::FreeChannelCallback(env, handle, index, channel,
-                                  &HALSIM_CancelPCMSolenoidInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getSolenoidInitialized
- * Signature: (II)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getSolenoidInitialized
-  (JNIEnv*, jclass, jint index, jint channel)
-{
-  return HALSIM_GetPCMSolenoidInitialized(index, channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setSolenoidInitialized
- * Signature: (IIZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setSolenoidInitialized
-  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
-{
-  HALSIM_SetPCMSolenoidInitialized(index, channel, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerSolenoidOutputCallback
- * Signature: (IILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerSolenoidOutputCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  return sim::AllocateChannelCallback(
-      env, index, channel, callback, initialNotify,
-      &HALSIM_RegisterPCMSolenoidOutputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelSolenoidOutputCallback
- * Signature: (III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelSolenoidOutputCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
-{
-  return sim::FreeChannelCallback(env, handle, index, channel,
-                                  &HALSIM_CancelPCMSolenoidOutputCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getSolenoidOutput
- * Signature: (II)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getSolenoidOutput
-  (JNIEnv*, jclass, jint index, jint channel)
-{
-  return HALSIM_GetPCMSolenoidOutput(index, channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setSolenoidOutput
- * Signature: (IIZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setSolenoidOutput
-  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
-{
-  HALSIM_SetPCMSolenoidOutput(index, channel, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerCompressorInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerCompressorInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      &HALSIM_RegisterPCMCompressorInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelCompressorInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelCompressorInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMCompressorInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getCompressorInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getCompressorInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMCompressorInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setCompressorInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setCompressorInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMCompressorInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerCompressorOnCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerCompressorOnCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMCompressorOnCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelCompressorOnCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelCompressorOnCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMCompressorOnCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getCompressorOn
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getCompressorOn
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMCompressorOn(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setCompressorOn
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setCompressorOn
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMCompressorOn(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerClosedLoopEnabledCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerClosedLoopEnabledCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMClosedLoopEnabledCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelClosedLoopEnabledCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelClosedLoopEnabledCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMClosedLoopEnabledCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getClosedLoopEnabled
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getClosedLoopEnabled
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMClosedLoopEnabled(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setClosedLoopEnabled
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setClosedLoopEnabled
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMClosedLoopEnabled(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerPressureSwitchCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerPressureSwitchCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMPressureSwitchCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelPressureSwitchCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelPressureSwitchCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMPressureSwitchCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getPressureSwitch
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getPressureSwitch
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMPressureSwitch(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setPressureSwitch
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setPressureSwitch
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPCMPressureSwitch(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerCompressorCurrentCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerCompressorCurrentCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPCMCompressorCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    cancelCompressorCurrentCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_cancelCompressorCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPCMCompressorCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    getCompressorCurrent
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_getCompressorCurrent
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPCMCompressorCurrent(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    setCompressorCurrent
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_setCompressorCurrent
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPCMCompressorCurrent(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerAllNonSolenoidCallbacks
- * Signature: (ILjava/lang/Object;Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerAllNonSolenoidCallbacks
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  sim::AllocateCallback(
-      env, index, callback, initialNotify,
-      [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) {
-        HALSIM_RegisterPCMAllNonSolenoidCallbacks(index, cb, param, in);
-        return 0;
-      });
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    registerAllSolenoidCallbacks
- * Signature: (IILjava/lang/Object;Z)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_registerAllSolenoidCallbacks
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  sim::AllocateChannelCallback(
-      env, index, channel, callback, initialNotify,
-      [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param,
-         HAL_Bool in) {
-        HALSIM_RegisterPCMAllSolenoidCallbacks(index, channel, cb, param, in);
-        return 0;
-      });
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PCMDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PCMDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetPCMData(index);
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp
deleted file mode 100644
index 2ce6cbd..0000000
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PDPDataJNI.cpp
+++ /dev/null
@@ -1,232 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <jni.h>
-
-#include "CallbackStore.h"
-#include "edu_wpi_first_hal_simulation_PDPDataJNI.h"
-#include "hal/simulation/PDPData.h"
-
-using namespace hal;
-
-extern "C" {
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    registerInitializedCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerInitializedCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPDPInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    cancelInitializedCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelInitializedCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPDPInitializedCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    getInitialized
- * Signature: (I)Z
- */
-JNIEXPORT jboolean JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_getInitialized
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPDPInitialized(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    setInitialized
- * Signature: (IZ)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_setInitialized
-  (JNIEnv*, jclass, jint index, jboolean value)
-{
-  HALSIM_SetPDPInitialized(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    registerTemperatureCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerTemperatureCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPDPTemperatureCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    cancelTemperatureCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelTemperatureCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPDPTemperatureCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    getTemperature
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_getTemperature
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPDPTemperature(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    setTemperature
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_setTemperature
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPDPTemperature(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    registerVoltageCallback
- * Signature: (ILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerVoltageCallback
-  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
-{
-  return sim::AllocateCallback(env, index, callback, initialNotify,
-                               &HALSIM_RegisterPDPVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    cancelVoltageCallback
- * Signature: (II)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelVoltageCallback
-  (JNIEnv* env, jclass, jint index, jint handle)
-{
-  return sim::FreeCallback(env, handle, index,
-                           &HALSIM_CancelPDPVoltageCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    getVoltage
- * Signature: (I)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_getVoltage
-  (JNIEnv*, jclass, jint index)
-{
-  return HALSIM_GetPDPVoltage(index);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    setVoltage
- * Signature: (ID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_setVoltage
-  (JNIEnv*, jclass, jint index, jdouble value)
-{
-  HALSIM_SetPDPVoltage(index, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    registerCurrentCallback
- * Signature: (IILjava/lang/Object;Z)I
- */
-JNIEXPORT jint JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_registerCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
-   jboolean initialNotify)
-{
-  return sim::AllocateChannelCallback(env, index, channel, callback,
-                                      initialNotify,
-                                      &HALSIM_RegisterPDPCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    cancelCurrentCallback
- * Signature: (III)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_cancelCurrentCallback
-  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
-{
-  return sim::FreeChannelCallback(env, handle, index, channel,
-                                  &HALSIM_CancelPDPCurrentCallback);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    getCurrent
- * Signature: (II)D
- */
-JNIEXPORT jdouble JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_getCurrent
-  (JNIEnv*, jclass, jint index, jint channel)
-{
-  return HALSIM_GetPDPCurrent(index, channel);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    setCurrent
- * Signature: (IID)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_setCurrent
-  (JNIEnv*, jclass, jint index, jint channel, jdouble value)
-{
-  HALSIM_SetPDPCurrent(index, channel, value);
-}
-
-/*
- * Class:     edu_wpi_first_hal_simulation_PDPDataJNI
- * Method:    resetData
- * Signature: (I)V
- */
-JNIEXPORT void JNICALL
-Java_edu_wpi_first_hal_simulation_PDPDataJNI_resetData
-  (JNIEnv*, jclass, jint index)
-{
-  HALSIM_ResetPDPData(index);
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp
index 03e61ef..31c79a7 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PWMDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PowerDistributionDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PowerDistributionDataJNI.cpp
new file mode 100644
index 0000000..517dc09
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/PowerDistributionDataJNI.cpp
@@ -0,0 +1,233 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_PowerDistributionDataJNI.h"
+#include "hal/simulation/PowerDistributionData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterPowerDistributionInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPowerDistributionInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPowerDistributionInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetPowerDistributionInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    registerTemperatureCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_registerTemperatureCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterPowerDistributionTemperatureCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    cancelTemperatureCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_cancelTemperatureCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPowerDistributionTemperatureCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    getTemperature
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_getTemperature
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPowerDistributionTemperature(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    setTemperature
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_setTemperature
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPowerDistributionTemperature(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    registerVoltageCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_registerVoltageCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      &HALSIM_RegisterPowerDistributionVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    cancelVoltageCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_cancelVoltageCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelPowerDistributionVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    getVoltage
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_getVoltage
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetPowerDistributionVoltage(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    setVoltage
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_setVoltage
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetPowerDistributionVoltage(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    registerCurrentCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_registerCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  return sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      &HALSIM_RegisterPowerDistributionCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    cancelCurrentCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_cancelCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+  return sim::FreeChannelCallback(
+      env, handle, index, channel,
+      &HALSIM_CancelPowerDistributionCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    getCurrent
+ * Signature: (II)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_getCurrent
+  (JNIEnv*, jclass, jint index, jint channel)
+{
+  return HALSIM_GetPowerDistributionCurrent(index, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    setCurrent
+ * Signature: (IID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_setCurrent
+  (JNIEnv*, jclass, jint index, jint channel, jdouble value)
+{
+  HALSIM_SetPowerDistributionCurrent(index, channel, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_PowerDistributionDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_PowerDistributionDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetPowerDistributionData(index);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/REVPHDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/REVPHDataJNI.cpp
new file mode 100644
index 0000000..da473de
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/REVPHDataJNI.cpp
@@ -0,0 +1,365 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <jni.h>
+
+#include "CallbackStore.h"
+#include "edu_wpi_first_hal_simulation_REVPHDataJNI.h"
+#include "hal/simulation/REVPHData.h"
+
+using namespace hal;
+
+extern "C" {
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerInitializedCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerInitializedCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterREVPHInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    cancelInitializedCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelInitializedCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelREVPHInitializedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    getInitialized
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getInitialized
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetREVPHInitialized(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    setInitialized
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setInitialized
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetREVPHInitialized(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerSolenoidOutputCallback
+ * Signature: (IILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerSolenoidOutputCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  return sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      &HALSIM_RegisterREVPHSolenoidOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    cancelSolenoidOutputCallback
+ * Signature: (III)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelSolenoidOutputCallback
+  (JNIEnv* env, jclass, jint index, jint channel, jint handle)
+{
+  return sim::FreeChannelCallback(env, handle, index, channel,
+                                  &HALSIM_CancelREVPHSolenoidOutputCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    getSolenoidOutput
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getSolenoidOutput
+  (JNIEnv*, jclass, jint index, jint channel)
+{
+  return HALSIM_GetREVPHSolenoidOutput(index, channel);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    setSolenoidOutput
+ * Signature: (IIZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setSolenoidOutput
+  (JNIEnv*, jclass, jint index, jint channel, jboolean value)
+{
+  HALSIM_SetREVPHSolenoidOutput(index, channel, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerCompressorOnCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerCompressorOnCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterREVPHCompressorOnCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    cancelCompressorOnCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelCompressorOnCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelREVPHCompressorOnCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    getCompressorOn
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getCompressorOn
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetREVPHCompressorOn(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    setCompressorOn
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setCompressorOn
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetREVPHCompressorOn(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerClosedLoopEnabledCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerClosedLoopEnabledCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterREVPHClosedLoopEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    cancelClosedLoopEnabledCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelClosedLoopEnabledCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelREVPHClosedLoopEnabledCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    getClosedLoopEnabled
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getClosedLoopEnabled
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetREVPHClosedLoopEnabled(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    setClosedLoopEnabled
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setClosedLoopEnabled
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetREVPHClosedLoopEnabled(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerPressureSwitchCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerPressureSwitchCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterREVPHPressureSwitchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    cancelPressureSwitchCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelPressureSwitchCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelREVPHPressureSwitchCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    getPressureSwitch
+ * Signature: (I)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getPressureSwitch
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetREVPHPressureSwitch(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    setPressureSwitch
+ * Signature: (IZ)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setPressureSwitch
+  (JNIEnv*, jclass, jint index, jboolean value)
+{
+  HALSIM_SetREVPHPressureSwitch(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerCompressorCurrentCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerCompressorCurrentCallback
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallback(env, index, callback, initialNotify,
+                               &HALSIM_RegisterREVPHCompressorCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    cancelCompressorCurrentCallback
+ * Signature: (II)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_cancelCompressorCurrentCallback
+  (JNIEnv* env, jclass, jint index, jint handle)
+{
+  return sim::FreeCallback(env, handle, index,
+                           &HALSIM_CancelREVPHCompressorCurrentCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    getCompressorCurrent
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_getCompressorCurrent
+  (JNIEnv*, jclass, jint index)
+{
+  return HALSIM_GetREVPHCompressorCurrent(index);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    setCompressorCurrent
+ * Signature: (ID)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_setCompressorCurrent
+  (JNIEnv*, jclass, jint index, jdouble value)
+{
+  HALSIM_SetREVPHCompressorCurrent(index, value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerAllNonSolenoidCallbacks
+ * Signature: (ILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerAllNonSolenoidCallbacks
+  (JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
+{
+  sim::AllocateCallback(
+      env, index, callback, initialNotify,
+      [](int32_t index, HAL_NotifyCallback cb, void* param, HAL_Bool in) {
+        HALSIM_RegisterREVPHAllNonSolenoidCallbacks(index, cb, param, in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    registerAllSolenoidCallbacks
+ * Signature: (IILjava/lang/Object;Z)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_registerAllSolenoidCallbacks
+  (JNIEnv* env, jclass, jint index, jint channel, jobject callback,
+   jboolean initialNotify)
+{
+  sim::AllocateChannelCallback(
+      env, index, channel, callback, initialNotify,
+      [](int32_t index, int32_t channel, HAL_NotifyCallback cb, void* param,
+         HAL_Bool in) {
+        HALSIM_RegisterREVPHAllSolenoidCallbacks(index, channel, cb, param, in);
+        return 0;
+      });
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_REVPHDataJNI
+ * Method:    resetData
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_REVPHDataJNI_resetData
+  (JNIEnv*, jclass, jint index)
+{
+  HALSIM_ResetREVPHData(index);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp
index 7abe7cb..7a59327 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RelayDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
index 1ff6044..03bb0c5 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/RoboRioDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
@@ -779,6 +776,57 @@
 
 /*
  * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    registerBrownoutVoltageCallback
+ * Signature: (Ljava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_registerBrownoutVoltageCallback
+  (JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
+{
+  return sim::AllocateCallbackNoIndex(
+      env, callback, initialNotify,
+      &HALSIM_RegisterRoboRioBrownoutVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    cancelBrownoutVoltageCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_cancelBrownoutVoltageCallback
+  (JNIEnv* env, jclass, jint handle)
+{
+  return sim::FreeCallbackNoIndex(env, handle,
+                                  &HALSIM_CancelRoboRioBrownoutVoltageCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    getBrownoutVoltage
+ * Signature: ()D
+ */
+JNIEXPORT jdouble JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_getBrownoutVoltage
+  (JNIEnv*, jclass)
+{
+  return HALSIM_GetRoboRioBrownoutVoltage();
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
+ * Method:    setBrownoutVoltage
+ * Signature: (D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_RoboRioDataJNI_setBrownoutVoltage
+  (JNIEnv*, jclass, jdouble value)
+{
+  HALSIM_SetRoboRioBrownoutVoltage(value);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_RoboRioDataJNI
  * Method:    resetData
  * Signature: ()V
  */
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp
index fe0c410..51bb320 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp
index 3709984..977f25b 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp
index 0b43d24..60ce0f7 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.cpp
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SimDeviceDataJNI.h"
 
 #include <jni.h>
 
-#include <functional>
-#include <string>
+#include <cstdio>
 #include <utility>
 
-#include <wpi/UidVector.h>
 #include <wpi/jni_util.h>
 
 #include "SimulatorJNI.h"
 #include "edu_wpi_first_hal_simulation_SimDeviceDataJNI.h"
+#include "hal/SimDevice.h"
+#include "hal/handles/UnlimitedHandleResource.h"
 #include "hal/simulation/SimDeviceData.h"
 
 using namespace hal;
@@ -39,23 +36,18 @@
   HAL_SimValueHandle handle;
 
   jobject MakeJava(JNIEnv* env) const;
-  void CallJava(JNIEnv* env, jobject callobj) const;
 };
 
 struct ValueInfo {
-  ValueInfo(const char* name_, HAL_SimValueHandle handle_, bool readonly_,
+  ValueInfo(const char* name_, HAL_SimValueHandle handle_, int32_t direction_,
             const HAL_Value& value_)
-      : name{name_}, handle{handle_}, readonly{readonly_}, value{value_} {}
+      : name{name_}, handle{handle_}, direction{direction_}, value{value_} {}
   std::string name;
   HAL_SimValueHandle handle;
-  bool readonly;
+  int32_t direction;
   HAL_Value value;
 
   jobject MakeJava(JNIEnv* env) const;
-  void CallJava(JNIEnv* env, jobject callobj) const;
-
- private:
-  std::pair<jlong, jdouble> ToValue12() const;
 };
 
 }  // namespace
@@ -64,15 +56,10 @@
   static jmethodID func =
       env->GetMethodID(simDeviceInfoCls, "<init>", "(Ljava/lang/String;I)V");
   return env->NewObject(simDeviceInfoCls, func, MakeJString(env, name),
-                        (jint)handle);
+                        static_cast<jint>(handle));
 }
 
-void DeviceInfo::CallJava(JNIEnv* env, jobject callobj) const {
-  env->CallVoidMethod(callobj, simDeviceCallbackCallback,
-                      MakeJString(env, name), (jint)handle);
-}
-
-std::pair<jlong, jdouble> ValueInfo::ToValue12() const {
+static std::pair<jlong, jdouble> ToValue12(const HAL_Value& value) {
   jlong value1 = 0;
   jdouble value2 = 0.0;
   switch (value.type) {
@@ -99,194 +86,268 @@
 
 jobject ValueInfo::MakeJava(JNIEnv* env) const {
   static jmethodID func =
-      env->GetMethodID(simValueInfoCls, "<init>", "(Ljava/lang/String;IZIJD)V");
-  auto [value1, value2] = ToValue12();
+      env->GetMethodID(simValueInfoCls, "<init>", "(Ljava/lang/String;IIIJD)V");
+  auto [value1, value2] = ToValue12(value);
   return env->NewObject(simValueInfoCls, func, MakeJString(env, name),
-                        (jint)handle, (jboolean)readonly, (jint)value.type,
-                        value1, value2);
-}
-
-void ValueInfo::CallJava(JNIEnv* env, jobject callobj) const {
-  auto [value1, value2] = ToValue12();
-  env->CallVoidMethod(callobj, simValueCallbackCallback, MakeJString(env, name),
-                      (jint)handle, (jboolean)readonly, (jint)value.type,
-                      value1, value2);
+                        static_cast<jint>(handle), static_cast<jint>(direction),
+                        static_cast<jint>(value.type), value1, value2);
 }
 
 namespace {
 
-class CallbackStore {
+class DeviceCallbackStore {
  public:
-  explicit CallbackStore(JNIEnv* env, jobject obj) : m_call{env, obj} {}
-  ~CallbackStore() {
-    if (m_cancelCallback) m_cancelCallback();
-  }
-
-  void SetCancel(std::function<void()> cancelCallback) {
-    m_cancelCallback = std::move(cancelCallback);
-  }
-  void Free(JNIEnv* env) { m_call.free(env); }
-  jobject Get() const { return m_call; }
+  void create(JNIEnv* env, jobject obj) { m_call = JGlobal<jobject>(env, obj); }
+  void performCallback(const char* name, HAL_SimDeviceHandle handle);
+  void free(JNIEnv* env) { m_call.free(env); }
+  void setCallbackId(int32_t id) { callbackId = id; }
+  int32_t getCallbackId() { return callbackId; }
 
  private:
   wpi::java::JGlobal<jobject> m_call;
-  std::function<void()> m_cancelCallback;
+  int32_t callbackId;
 };
 
-class CallbackThreadJNI : public wpi::SafeThread {
+class ValueCallbackStore {
  public:
-  void Main();
-
-  using DeviceCalls =
-      std::vector<std::pair<std::weak_ptr<CallbackStore>, DeviceInfo>>;
-  DeviceCalls m_deviceCalls;
-  using ValueCalls =
-      std::vector<std::pair<std::weak_ptr<CallbackStore>, ValueInfo>>;
-  ValueCalls m_valueCalls;
-
-  wpi::UidVector<std::shared_ptr<CallbackStore>, 4> m_callbacks;
-};
-
-class CallbackJNI {
- public:
-  static CallbackJNI& GetInstance() {
-    static CallbackJNI inst;
-    return inst;
-  }
-  void SendDevice(int32_t callback, DeviceInfo info);
-  void SendValue(int32_t callback, ValueInfo info);
-
-  std::pair<int32_t, std::shared_ptr<CallbackStore>> AllocateCallback(
-      JNIEnv* env, jobject obj);
-
-  void FreeCallback(JNIEnv* env, int32_t uid);
+  void create(JNIEnv* env, jobject obj) { m_call = JGlobal<jobject>(env, obj); }
+  void performCallback(const char* name, HAL_SimValueHandle handle,
+                       int32_t direction, const HAL_Value& value);
+  void free(JNIEnv* env) { m_call.free(env); }
+  void setCallbackId(int32_t id) { m_callbackId = id; }
+  int32_t getCallbackId() { return m_callbackId; }
 
  private:
-  CallbackJNI() { m_owner.Start(); }
-
-  wpi::SafeThreadOwner<CallbackThreadJNI> m_owner;
+  wpi::java::JGlobal<jobject> m_call;
+  int32_t m_callbackId;
 };
 
 }  // namespace
 
-void CallbackThreadJNI::Main() {
+void DeviceCallbackStore::performCallback(const char* name,
+                                          HAL_SimDeviceHandle handle) {
   JNIEnv* env;
-  JavaVMAttachArgs args;
-  args.version = JNI_VERSION_1_2;
-  args.name = const_cast<char*>("SimDeviceCallback");
-  args.group = nullptr;
-  jint rs = sim::GetJVM()->AttachCurrentThreadAsDaemon(
-      reinterpret_cast<void**>(&env), &args);
-  if (rs != JNI_OK) return;
-
-  DeviceCalls deviceCalls;
-  ValueCalls valueCalls;
-
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    m_cond.wait(lock, [&] { return !m_active; });
-    if (!m_active) break;
-
-    deviceCalls.swap(m_deviceCalls);
-    valueCalls.swap(m_valueCalls);
-
-    lock.unlock();  // don't hold mutex during callback execution
-
-    for (auto&& call : deviceCalls) {
-      if (auto store = call.first.lock()) {
-        if (jobject callobj = store->Get()) {
-          call.second.CallJava(env, callobj);
-          if (env->ExceptionCheck()) {
-            env->ExceptionDescribe();
-            env->ExceptionClear();
-          }
-        }
-      }
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      std::puts("Failed to attach");
+      std::fflush(stdout);
+      return;
     }
-
-    for (auto&& call : valueCalls) {
-      if (auto store = call.first.lock()) {
-        if (jobject callobj = store->Get()) {
-          call.second.CallJava(env, callobj);
-          if (env->ExceptionCheck()) {
-            env->ExceptionDescribe();
-            env->ExceptionClear();
-          }
-        }
-      }
-    }
-
-    deviceCalls.clear();
-    valueCalls.clear();
-
-    lock.lock();
+  } else if (tryGetEnv == JNI_EVERSION) {
+    std::puts("Invalid JVM Version requested");
+    std::fflush(stdout);
   }
 
-  // free global references
-  for (auto&& callback : m_callbacks) callback->Free(env);
+  env->CallVoidMethod(m_call, simDeviceCallbackCallback, MakeJString(env, name),
+                      static_cast<jint>(handle));
 
-  sim::GetJVM()->DetachCurrentThread();
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
 }
 
-void CallbackJNI::SendDevice(int32_t callback, DeviceInfo info) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_deviceCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
-  thr->m_cond.notify_one();
+void ValueCallbackStore::performCallback(const char* name,
+                                         HAL_SimValueHandle handle,
+                                         int32_t direction,
+                                         const HAL_Value& value) {
+  JNIEnv* env;
+  JavaVM* vm = sim::GetJVM();
+  bool didAttachThread = false;
+  int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
+  if (tryGetEnv == JNI_EDETACHED) {
+    // Thread not attached
+    didAttachThread = true;
+    if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
+      // Failed to attach, log and return
+      std::puts("Failed to attach");
+      std::fflush(stdout);
+      return;
+    }
+  } else if (tryGetEnv == JNI_EVERSION) {
+    std::puts("Invalid JVM Version requested");
+    std::fflush(stdout);
+  }
+
+  auto [value1, value2] = ToValue12(value);
+  env->CallVoidMethod(m_call, simValueCallbackCallback, MakeJString(env, name),
+                      static_cast<jint>(handle), static_cast<jint>(direction),
+                      static_cast<jint>(value.type), value1, value2);
+
+  if (env->ExceptionCheck()) {
+    env->ExceptionDescribe();
+  }
+
+  if (didAttachThread) {
+    vm->DetachCurrentThread();
+  }
 }
 
-void CallbackJNI::SendValue(int32_t callback, ValueInfo info) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  thr->m_valueCalls.emplace_back(thr->m_callbacks[callback], std::move(info));
-  thr->m_cond.notify_one();
+static hal::UnlimitedHandleResource<SIM_JniHandle, DeviceCallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    deviceCallbackHandles;
+
+namespace {
+using RegisterDeviceCallbackFunc =
+    int32_t (*)(const char* prefix, void* param,
+                HALSIM_SimDeviceCallback callback, HAL_Bool initialNotify);
+using FreeDeviceCallbackFunc = void (*)(int32_t uid);
+}  // namespace
+
+static SIM_JniHandle AllocateDeviceCallback(
+    JNIEnv* env, const char* prefix, jobject callback, jboolean initialNotify,
+    RegisterDeviceCallbackFunc createCallback) {
+  auto callbackStore = std::make_shared<DeviceCallbackStore>();
+
+  auto handle = deviceCallbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         HAL_SimDeviceHandle handle) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle jnihandle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = deviceCallbackHandles->Get(jnihandle);
+    if (!data) {
+      return;
+    }
+
+    data->performCallback(name, handle);
+  };
+
+  auto id =
+      createCallback(prefix, handleAsVoidPtr, callbackFunc, initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
 }
 
-std::pair<int32_t, std::shared_ptr<CallbackStore>>
-CallbackJNI::AllocateCallback(JNIEnv* env, jobject obj) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return std::pair(0, nullptr);
-  auto store = std::make_shared<CallbackStore>(env, obj);
-  return std::pair(thr->m_callbacks.emplace_back(store) + 1, store);
+static void FreeDeviceCallback(JNIEnv* env, SIM_JniHandle handle,
+                               FreeDeviceCallbackFunc freeCallback) {
+  auto callback = deviceCallbackHandles->Free(handle);
+  freeCallback(callback->getCallbackId());
+  callback->free(env);
 }
 
-void CallbackJNI::FreeCallback(JNIEnv* env, int32_t uid) {
-  auto thr = m_owner.GetThread();
-  if (!thr) return;
-  if (uid <= 0 || static_cast<uint32_t>(uid) >= thr->m_callbacks.size()) return;
-  --uid;
-  auto store = std::move(thr->m_callbacks[uid]);
-  thr->m_callbacks.erase(uid);
-  store->Free(env);
+static hal::UnlimitedHandleResource<SIM_JniHandle, ValueCallbackStore,
+                                    hal::HAL_HandleEnum::SimulationJni>*
+    valueCallbackHandles;
+
+namespace {
+using FreeValueCallbackFunc = void (*)(int32_t uid);
+}  // namespace
+
+template <typename THandle>
+static SIM_JniHandle AllocateValueCallback(
+    JNIEnv* env, THandle h, jobject callback, jboolean initialNotify,
+    int32_t (*createCallback)(THandle handle, void* param,
+                              HALSIM_SimValueCallback callback,
+                              HAL_Bool initialNotify)) {
+  auto callbackStore = std::make_shared<ValueCallbackStore>();
+
+  auto handle = valueCallbackHandles->Allocate(callbackStore);
+
+  if (handle == HAL_kInvalidHandle) {
+    return -1;
+  }
+
+  uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
+  void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
+
+  callbackStore->create(env, callback);
+
+  auto callbackFunc = [](const char* name, void* param,
+                         HAL_SimValueHandle handle, int32_t direction,
+                         const HAL_Value* value) {
+    uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
+    SIM_JniHandle jnihandle = static_cast<SIM_JniHandle>(handleTmp);
+    auto data = valueCallbackHandles->Get(jnihandle);
+    if (!data) {
+      return;
+    }
+
+    data->performCallback(name, handle, direction, *value);
+  };
+
+  auto id = createCallback(h, handleAsVoidPtr, callbackFunc, initialNotify);
+
+  callbackStore->setCallbackId(id);
+
+  return handle;
 }
 
-namespace hal {
-namespace sim {
+static void FreeValueCallback(JNIEnv* env, SIM_JniHandle handle,
+                              FreeValueCallbackFunc freeCallback) {
+  auto callback = valueCallbackHandles->Free(handle);
+  freeCallback(callback->getCallbackId());
+  callback->free(env);
+}
+
+namespace hal::sim {
 
 bool InitializeSimDeviceDataJNI(JNIEnv* env) {
   simDeviceInfoCls = JClass(
       env, "edu/wpi/first/hal/simulation/SimDeviceDataJNI$SimDeviceInfo");
-  if (!simDeviceInfoCls) return false;
+  if (!simDeviceInfoCls) {
+    return false;
+  }
 
   simValueInfoCls =
       JClass(env, "edu/wpi/first/hal/simulation/SimDeviceDataJNI$SimValueInfo");
-  if (!simValueInfoCls) return false;
+  if (!simValueInfoCls) {
+    return false;
+  }
 
   simDeviceCallbackCls =
       JClass(env, "edu/wpi/first/hal/simulation/SimDeviceCallback");
-  if (!simDeviceCallbackCls) return false;
+  if (!simDeviceCallbackCls) {
+    return false;
+  }
 
   simDeviceCallbackCallback = env->GetMethodID(simDeviceCallbackCls, "callback",
                                                "(Ljava/lang/String;I)V");
-  if (!simDeviceCallbackCallback) return false;
+  if (!simDeviceCallbackCallback) {
+    return false;
+  }
 
   simValueCallbackCls =
       JClass(env, "edu/wpi/first/hal/simulation/SimValueCallback");
-  if (!simValueCallbackCls) return false;
+  if (!simValueCallbackCls) {
+    return false;
+  }
 
   simValueCallbackCallback = env->GetMethodID(
-      simValueCallbackCls, "callbackNative", "(Ljava/lang/String;IZIJD)V");
-  if (!simValueCallbackCallback) return false;
+      simValueCallbackCls, "callbackNative", "(Ljava/lang/String;IIIJD)V");
+  if (!simValueCallbackCallback) {
+    return false;
+  }
+
+  static hal::UnlimitedHandleResource<SIM_JniHandle, DeviceCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cbDevice;
+  deviceCallbackHandles = &cbDevice;
+
+  static hal::UnlimitedHandleResource<SIM_JniHandle, ValueCallbackStore,
+                                      hal::HAL_HandleEnum::SimulationJni>
+      cbValue;
+  valueCallbackHandles = &cbValue;
 
   return true;
 }
@@ -298,8 +359,7 @@
   simValueCallbackCls.free(env);
 }
 
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
 
 extern "C" {
 
@@ -337,18 +397,9 @@
   (JNIEnv* env, jclass, jstring prefix, jobject callback,
    jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimDeviceCreatedCallback(
-      JStringRef{env, prefix}.c_str(),
-      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimDeviceCreatedCallback(cuid); });
-  return uid;
+  return AllocateDeviceCallback(env, JStringRef{env, prefix}.c_str(), callback,
+                                initialNotify,
+                                &HALSIM_RegisterSimDeviceCreatedCallback);
 }
 
 /*
@@ -360,29 +411,22 @@
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimDeviceCreatedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeDeviceCallback(env, uid, &HALSIM_CancelSimDeviceCreatedCallback);
 }
 
 /*
  * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
  * Method:    registerSimDeviceFreedCallback
- * Signature: (Ljava/lang/String;Ljava/lang/Object;)I
+ * Signature: (Ljava/lang/String;Ljava/lang/Object;Z)I
  */
 JNIEXPORT jint JNICALL
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimDeviceFreedCallback
-  (JNIEnv* env, jclass, jstring prefix, jobject callback)
+  (JNIEnv* env, jclass, jstring prefix, jobject callback,
+   jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimDeviceFreedCallback(
-      JStringRef{env, prefix}.c_str(),
-      reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimDeviceHandle handle) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendDevice(uid, DeviceInfo{name, handle});
-      });
-  store->SetCancel([cuid] { HALSIM_CancelSimDeviceFreedCallback(cuid); });
-  return uid;
+  return AllocateDeviceCallback(env, JStringRef{env, prefix}.c_str(), callback,
+                                initialNotify,
+                                &HALSIM_RegisterSimDeviceFreedCallback);
 }
 
 /*
@@ -394,7 +438,7 @@
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimDeviceFreedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeDeviceCallback(env, uid, &HALSIM_CancelSimDeviceFreedCallback);
 }
 
 /*
@@ -411,6 +455,18 @@
 
 /*
  * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    getSimDeviceName
+ * Signature: (I)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_getSimDeviceName
+  (JNIEnv* env, jclass, jint handle)
+{
+  return MakeJString(env, HALSIM_GetSimDeviceName(handle));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
  * Method:    getSimValueDeviceHandle
  * Signature: (I)I
  */
@@ -443,7 +499,9 @@
   size_t numElems = arr.size();
   jobjectArray jarr =
       env->NewObjectArray(arr.size(), simDeviceInfoCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < numElems; ++i) {
     JLocal<jobject> elem{env, arr[i].MakeJava(env)};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -460,19 +518,9 @@
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueCreatedCallback
   (JNIEnv* env, jclass, jint device, jobject callback, jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimValueCreatedCallback(
-      device, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendValue(
-            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimValueCreatedCallback(cuid); });
-  return uid;
+  return AllocateValueCallback(env, static_cast<HAL_SimDeviceHandle>(device),
+                               callback, initialNotify,
+                               &HALSIM_RegisterSimValueCreatedCallback);
 }
 
 /*
@@ -484,7 +532,7 @@
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueCreatedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeValueCallback(env, uid, &HALSIM_CancelSimValueCreatedCallback);
 }
 
 /*
@@ -496,19 +544,9 @@
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueChangedCallback
   (JNIEnv* env, jclass, jint handle, jobject callback, jboolean initialNotify)
 {
-  auto [uid, store] =
-      CallbackJNI::GetInstance().AllocateCallback(env, callback);
-  int32_t cuid = HALSIM_RegisterSimValueChangedCallback(
-      handle, reinterpret_cast<void*>(static_cast<intptr_t>(uid)),
-      [](const char* name, void* param, HAL_SimValueHandle handle,
-         HAL_Bool readonly, const HAL_Value* value) {
-        int32_t uid = reinterpret_cast<intptr_t>(param);
-        CallbackJNI::GetInstance().SendValue(
-            uid, ValueInfo{name, handle, static_cast<bool>(readonly), *value});
-      },
-      initialNotify);
-  store->SetCancel([cuid] { HALSIM_CancelSimValueChangedCallback(cuid); });
-  return uid;
+  return AllocateValueCallback(env, static_cast<HAL_SimValueHandle>(handle),
+                               callback, initialNotify,
+                               &HALSIM_RegisterSimValueChangedCallback);
 }
 
 /*
@@ -520,7 +558,33 @@
 Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueChangedCallback
   (JNIEnv* env, jclass, jint uid)
 {
-  CallbackJNI::GetInstance().FreeCallback(env, uid);
+  FreeValueCallback(env, uid, &HALSIM_CancelSimValueChangedCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    registerSimValueResetCallback
+ * Signature: (ILjava/lang/Object;Z)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_registerSimValueResetCallback
+  (JNIEnv* env, jclass, jint handle, jobject callback, jboolean initialNotify)
+{
+  return AllocateValueCallback(env, static_cast<HAL_SimValueHandle>(handle),
+                               callback, initialNotify,
+                               &HALSIM_RegisterSimValueResetCallback);
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    cancelSimValueResetCallback
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_cancelSimValueResetCallback
+  (JNIEnv* env, jclass, jint uid)
+{
+  FreeValueCallback(env, uid, &HALSIM_CancelSimValueResetCallback);
 }
 
 /*
@@ -557,7 +621,9 @@
   // convert to java
   size_t numElems = arr.size();
   jobjectArray jarr = env->NewObjectArray(arr.size(), simValueInfoCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < numElems; ++i) {
     JLocal<jobject> elem{env, arr[i].MakeJava(env)};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -575,11 +641,15 @@
   (JNIEnv* env, jclass, jint handle)
 {
   static JClass stringCls{env, "java/lang/String"};
-  if (!stringCls) return nullptr;
+  if (!stringCls) {
+    return nullptr;
+  }
   int32_t numElems = 0;
   const char** elems = HALSIM_GetSimValueEnumOptions(handle, &numElems);
   jobjectArray jarr = env->NewObjectArray(numElems, stringCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (int32_t i = 0; i < numElems; ++i) {
     JLocal<jstring> elem{env, MakeJString(env, elems[i])};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -589,6 +659,20 @@
 
 /*
  * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
+ * Method:    getSimValueEnumDoubleValues
+ * Signature: (I)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_hal_simulation_SimDeviceDataJNI_getSimValueEnumDoubleValues
+  (JNIEnv* env, jclass, jint handle)
+{
+  int32_t numElems = 0;
+  const double* elems = HALSIM_GetSimValueEnumDoubleValues(handle, &numElems);
+  return MakeJDoubleArray(env, wpi::span(elems, numElems));
+}
+
+/*
+ * Class:     edu_wpi_first_hal_simulation_SimDeviceDataJNI
  * Method:    resetSimDeviceData
  * Signature: ()V
  */
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h
index 44fc27f..08c47b9 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimDeviceDataJNI.h
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <jni.h>
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 bool InitializeSimDeviceDataJNI(JNIEnv* env);
 void FreeSimDeviceDataJNI(JNIEnv* env);
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
index b6336e8..859c7f5 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SimulatorJNI.h"
 
@@ -31,61 +28,80 @@
 static jmethodID constBufferCallbackCallback;
 static jmethodID spiReadAutoReceiveBufferCallbackCallback;
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 jint SimOnLoad(JavaVM* vm, void* reserved) {
   jvm = vm;
 
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
+  }
 
   notifyCallbackCls =
       JClass(env, "edu/wpi/first/hal/simulation/NotifyCallback");
-  if (!notifyCallbackCls) return JNI_ERR;
+  if (!notifyCallbackCls) {
+    return JNI_ERR;
+  }
 
   notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
                                             "(Ljava/lang/String;IJD)V");
-  if (!notifyCallbackCallback) return JNI_ERR;
+  if (!notifyCallbackCallback) {
+    return JNI_ERR;
+  }
 
   bufferCallbackCls =
       JClass(env, "edu/wpi/first/hal/simulation/BufferCallback");
-  if (!bufferCallbackCls) return JNI_ERR;
+  if (!bufferCallbackCls) {
+    return JNI_ERR;
+  }
 
   bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
                                             "(Ljava/lang/String;[BI)V");
-  if (!bufferCallbackCallback) return JNI_ERR;
+  if (!bufferCallbackCallback) {
+    return JNI_ERR;
+  }
 
   constBufferCallbackCls =
       JClass(env, "edu/wpi/first/hal/simulation/ConstBufferCallback");
-  if (!constBufferCallbackCls) return JNI_ERR;
+  if (!constBufferCallbackCls) {
+    return JNI_ERR;
+  }
 
   constBufferCallbackCallback = env->GetMethodID(
       constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
-  if (!constBufferCallbackCallback) return JNI_ERR;
+  if (!constBufferCallbackCallback) {
+    return JNI_ERR;
+  }
 
   spiReadAutoReceiveBufferCallbackCls = JClass(
       env, "edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback");
-  if (!spiReadAutoReceiveBufferCallbackCls) return JNI_ERR;
+  if (!spiReadAutoReceiveBufferCallbackCls) {
+    return JNI_ERR;
+  }
 
   spiReadAutoReceiveBufferCallbackCallback =
       env->GetMethodID(spiReadAutoReceiveBufferCallbackCls, "callback",
                        "(Ljava/lang/String;[II)I");
-  if (!spiReadAutoReceiveBufferCallbackCallback) return JNI_ERR;
+  if (!spiReadAutoReceiveBufferCallbackCallback) {
+    return JNI_ERR;
+  }
 
   InitializeStore();
   InitializeBufferStore();
   InitializeConstBufferStore();
   InitializeSpiBufferStore();
-  if (!InitializeSimDeviceDataJNI(env)) return JNI_ERR;
+  if (!InitializeSimDeviceDataJNI(env)) {
+    return JNI_ERR;
+  }
 
   return JNI_VERSION_1_6;
 }
 
 void SimOnUnload(JavaVM* vm, void* reserved) {
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return;
+  }
 
   notifyCallbackCls.free(env);
   bufferCallbackCls.free(env);
@@ -95,19 +111,26 @@
   jvm = nullptr;
 }
 
-JavaVM* GetJVM() { return jvm; }
+JavaVM* GetJVM() {
+  return jvm;
+}
 
-jmethodID GetNotifyCallback() { return notifyCallbackCallback; }
+jmethodID GetNotifyCallback() {
+  return notifyCallbackCallback;
+}
 
-jmethodID GetBufferCallback() { return bufferCallbackCallback; }
+jmethodID GetBufferCallback() {
+  return bufferCallbackCallback;
+}
 
-jmethodID GetConstBufferCallback() { return constBufferCallbackCallback; }
+jmethodID GetConstBufferCallback() {
+  return constBufferCallbackCallback;
+}
 
 jmethodID GetSpiReadAutoReceiveBufferCallback() {
   return spiReadAutoReceiveBufferCallbackCallback;
 }
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
 
 extern "C" {
 /*
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
index d6710e8..3e71acd 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
@@ -1,24 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include "hal/Types.h"
 #include "jni.h"
 
-typedef HAL_Handle SIM_JniHandle;
+using SIM_JniHandle = HAL_Handle;  // NOLINT
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 JavaVM* GetJVM();
 
 jmethodID GetNotifyCallback();
 jmethodID GetBufferCallback();
 jmethodID GetConstBufferCallback();
 jmethodID GetSpiReadAutoReceiveBufferCallback();
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp
index 5432840..c20f607 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SpiReadAutoReceiveBufferCallbackStore.h"
 
 #include <jni.h>
 
+#include <cstdio>
+
 #include <wpi/jni_util.h>
 
 #include "SimulatorJNI.h"
@@ -25,8 +24,7 @@
     SIM_JniHandle, SpiReadAutoReceiveBufferCallbackStore,
     hal::HAL_HandleEnum::SimulationJni>* callbackHandles;
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 void InitializeSpiBufferStore() {
   static hal::UnlimitedHandleResource<SIM_JniHandle,
                                       SpiReadAutoReceiveBufferCallbackStore,
@@ -34,8 +32,7 @@
       cb;
   callbackHandles = &cb;
 }
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
 
 void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) {
   m_call = JGlobal<jobject>(env, obj);
@@ -52,21 +49,21 @@
     didAttachThread = true;
     if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
       // Failed to attach, log and return
-      wpi::outs() << "Failed to attach\n";
-      wpi::outs().flush();
+      std::puts("Failed to attach");
+      std::fflush(stdout);
       return -1;
     }
   } else if (tryGetEnv == JNI_EVERSION) {
-    wpi::outs() << "Invalid JVM Version requested\n";
-    wpi::outs().flush();
+    std::puts("Invalid JVM Version requested");
+    std::fflush(stdout);
   }
 
   auto toCallbackArr = MakeJIntArray(
-      env, wpi::ArrayRef<uint32_t>{buffer, static_cast<size_t>(numToRead)});
+      env, wpi::span<const uint32_t>{buffer, static_cast<size_t>(numToRead)});
 
   jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(),
                                 MakeJString(env, name), toCallbackArr,
-                                (jint)numToRead);
+                                static_cast<jint>(numToRead));
 
   jint* fromCallbackArr = reinterpret_cast<jint*>(
       env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
@@ -113,7 +110,9 @@
     uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
     SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
     auto data = callbackHandles->Get(handle);
-    if (!data) return;
+    if (!data) {
+      return;
+    }
 
     *outputCount = data->performCallback(name, buffer, numToRead);
   };
diff --git a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h
index 6a457de..18e8a44 100644
--- a/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h
+++ b/third_party/allwpilib/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,8 +15,7 @@
 #include "hal/simulation/NotifyListener.h"
 #include "hal/simulation/SPIData.h"
 
-namespace hal {
-namespace sim {
+namespace hal::sim {
 class SpiReadAutoReceiveBufferCallbackStore {
  public:
   void create(JNIEnv* env, jobject obj);
@@ -36,14 +32,13 @@
 
 void InitializeSpiBufferStore();
 
-typedef int32_t (*RegisterSpiBufferCallbackFunc)(
+using RegisterSpiBufferCallbackFunc = int32_t (*)(
     int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
-typedef void (*FreeSpiBufferCallbackFunc)(int32_t index, int32_t uid);
+using FreeSpiBufferCallbackFunc = void (*)(int32_t index, int32_t uid);
 
 SIM_JniHandle AllocateSpiBufferCallback(
     JNIEnv* env, jint index, jobject callback,
     RegisterSpiBufferCallbackFunc createCallback);
 void FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
                            FreeSpiBufferCallbackFunc freeCallback);
-}  // namespace sim
-}  // namespace hal
+}  // namespace hal::sim
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy b/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy
new file mode 100644
index 0000000..4f4edda
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/.clang-tidy
@@ -0,0 +1,75 @@
+Checks:
+  'bugprone-assert-side-effect,
+  bugprone-bool-pointer-implicit-conversion,
+  bugprone-copy-constructor-init,
+  bugprone-dangling-handle,
+  bugprone-dynamic-static-initializers,
+  bugprone-exception-escape,
+  bugprone-forward-declaration-namespace,
+  bugprone-forwarding-reference-overload,
+  bugprone-inaccurate-erase,
+  bugprone-incorrect-roundings,
+  bugprone-integer-division,
+  bugprone-lambda-function-name,
+  bugprone-misplaced-operator-in-strlen-in-alloc,
+  bugprone-misplaced-widening-cast,
+  bugprone-move-forwarding-reference,
+  bugprone-multiple-statement-macro,
+  bugprone-parent-virtual-call,
+  bugprone-posix-return,
+  bugprone-sizeof-container,
+  bugprone-sizeof-expression,
+  bugprone-spuriously-wake-up-functions,
+  bugprone-string-constructor,
+  bugprone-string-integer-assignment,
+  bugprone-string-literal-with-embedded-nul,
+  bugprone-suspicious-enum-usage,
+  bugprone-suspicious-include,
+  bugprone-suspicious-memset-usage,
+  bugprone-suspicious-missing-comma,
+  bugprone-suspicious-semicolon,
+  bugprone-suspicious-string-compare,
+  bugprone-throw-keyword-missing,
+  bugprone-too-small-loop-variable,
+  bugprone-undefined-memory-manipulation,
+  bugprone-undelegated-constructor,
+  bugprone-unhandled-self-assignment,
+  bugprone-unused-raii,
+  bugprone-virtual-near-miss,
+  cert-dcl58-cpp,
+  cert-err52-cpp,
+  cert-err60-cpp,
+  cert-mem57-cpp,
+  cert-oop57-cpp,
+  cert-oop58-cpp,
+  clang-diagnostic-*,
+  -clang-diagnostic-deprecated-declarations,
+  -clang-diagnostic-#warnings,
+  -clang-diagnostic-pedantic,
+  clang-analyzer-*,
+  cppcoreguidelines-slicing,
+  google-build-namespaces,
+  google-explicit-constructor,
+  google-global-names-in-headers,
+  google-readability-avoid-underscore-in-googletest-name,
+  google-readability-casting,
+  google-runtime-operator,
+  llvm-twine-local,
+  misc-definitions-in-headers,
+  misc-misplaced-const,
+  misc-new-delete-overloads,
+  misc-non-copyable-objects,
+  modernize-avoid-bind,
+  modernize-concat-nested-namespaces,
+  modernize-make-shared,
+  modernize-make-unique,
+  modernize-pass-by-value,
+  modernize-use-default-member-init,
+  modernize-use-noexcept,
+  modernize-use-nullptr,
+  modernize-use-override,
+  readability-braces-around-statements'
+FormatStyle: file
+CheckOptions:
+  - key: bugprone-dangling-handle
+    value: 'std::string_view'
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h b/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h
index 9dc488b..9131e83 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Accelerometer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLED.h b/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLED.h
index 68383ef..7674979 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLED.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLED.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLEDTypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLEDTypes.h
index bdcd742..dbaa776 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLEDTypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AddressableLEDTypes.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogAccumulator.h b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogAccumulator.h
index 9e49e90..c0c928d 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogAccumulator.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogAccumulator.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,7 +21,8 @@
 /**
  * Is the channel attached to an accumulator.
  *
- * @param analogPortHandle Handle to the analog port.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[out] status Error status variable. 0 on success.
  * @return The analog channel is attached to an accumulator.
  */
 HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle,
@@ -33,7 +31,8 @@
 /**
  * Initialize the accumulator.
  *
- * @param analogPortHandle Handle to the analog port.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_InitAccumulator(HAL_AnalogInputHandle analogPortHandle,
                          int32_t* status);
@@ -41,7 +40,8 @@
 /**
  * Resets the accumulator to the initial value.
  *
- * @param analogPortHandle Handle to the analog port.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_ResetAccumulator(HAL_AnalogInputHandle analogPortHandle,
                           int32_t* status);
@@ -58,8 +58,9 @@
  * source from channel 1. Because of this, any non-zero oversample bits will
  * affect the size of the value for this field.
  *
- * @param analogPortHandle Handle to the analog port.
- * @param center The center value of the accumulator.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[in] center The center value of the accumulator.
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_SetAccumulatorCenter(HAL_AnalogInputHandle analogPortHandle,
                               int32_t center, int32_t* status);
@@ -67,8 +68,9 @@
 /**
  * Set the accumulator's deadband.
  *
- * @param analogPortHandle Handle to the analog port.
- * @param deadband The deadband of the accumulator.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[in] deadband The deadband of the accumulator.
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_SetAccumulatorDeadband(HAL_AnalogInputHandle analogPortHandle,
                                 int32_t deadband, int32_t* status);
@@ -79,7 +81,8 @@
  * Read the value that has been accumulating on channel 1.
  * The accumulator is attached after the oversample and average engine.
  *
- * @param analogPortHandle Handle to the analog port.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[out] status Error status variable. 0 on success.
  * @return The 64-bit value accumulated since the last Reset().
  */
 int64_t HAL_GetAccumulatorValue(HAL_AnalogInputHandle analogPortHandle,
@@ -91,7 +94,8 @@
  * Read the count of the accumulated values since the accumulator was last
  * Reset().
  *
- * @param analogPortHandle Handle to the analog port.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[out] status Error status variable. 0 on success.
  * @return The number of times samples from the channel were accumulated.
  */
 int64_t HAL_GetAccumulatorCount(HAL_AnalogInputHandle analogPortHandle,
@@ -103,9 +107,10 @@
  * This function reads the value and count from the FPGA atomically.
  * This can be used for averaging.
  *
- * @param analogPortHandle Handle to the analog port.
- * @param value Pointer to the 64-bit accumulated output.
- * @param count Pointer to the number of accumulation cycles.
+ * @param[in] analogPortHandle Handle to the analog port.
+ * @param[in] value Pointer to the 64-bit accumulated output.
+ * @param[in] count Pointer to the number of accumulation cycles.
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_GetAccumulatorOutput(HAL_AnalogInputHandle analogPortHandle,
                               int64_t* value, int64_t* count, int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogGyro.h b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogGyro.h
index 54030bd..6e5a9c2 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogGyro.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogGyro.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,17 +21,22 @@
 /**
  * Initializes an analog gyro.
  *
- * @param handle handle to the analog port
- * @return       the initialized gyro handle
+ * @param[in] handle handle to the analog port
+ * @param[in] allocationLocation the location where the allocation is occuring
+ *                                (can be null)
+ * @param[out] status the error code, or 0 for success
+ * @return the initialized gyro handle
  */
 HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle handle,
+                                        const char* allocationLocation,
                                         int32_t* status);
 
 /**
  * Sets up an analog gyro with the proper offsets and settings for the KOP
  * analog gyro.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status);
 
@@ -51,10 +53,11 @@
  * This is meant to be used if you want to reuse the values from a previous
  * calibration.
  *
- * @param handle                  the gyro handle
- * @param voltsPerDegreePerSecond the gyro volts scaling
- * @param offset                  the gyro offset
- * @param center                  the gyro center
+ * @param[in] handle                  the gyro handle
+ * @param[in] voltsPerDegreePerSecond the gyro volts scaling
+ * @param[in] offset                  the gyro offset
+ * @param[in] center                  the gyro center
+ * @param[out] status                  the error code, or 0 for success
  */
 void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle,
                                  double voltsPerDegreePerSecond, double offset,
@@ -63,8 +66,9 @@
 /**
  * Sets the analog gyro volts per degrees per second scaling.
  *
- * @param handle                  the gyro handle
- * @param voltsPerDegreePerSecond the gyro volts scaling
+ * @param[in] handle                  the gyro handle
+ * @param[in] voltsPerDegreePerSecond the gyro volts scaling
+ * @param[out] status                  the error code, or 0 for success
  */
 void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle,
                                               double voltsPerDegreePerSecond,
@@ -73,7 +77,8 @@
 /**
  * Resets the analog gyro value to 0.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status);
 
@@ -84,15 +89,17 @@
  * setting that as the center. Note that this call blocks for 5 seconds to
  * perform this.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status);
 
 /**
  * Sets the deadband of the analog gyro.
  *
- * @param handle the gyro handle
- * @param volts  the voltage deadband
+ * @param[in] handle the gyro handle
+ * @param[in] volts  the voltage deadband
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts,
                                int32_t* status);
@@ -100,7 +107,8 @@
 /**
  * Gets the gyro angle in degrees.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the gyro angle in degrees
  */
 double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status);
@@ -108,7 +116,8 @@
 /**
  * Gets the gyro rate in degrees/second.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the gyro rate in degrees/second
  */
 double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status);
@@ -118,7 +127,8 @@
  *
  * Can be used to not repeat a calibration but reconstruct the gyro object.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the gryo offset
  */
 double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status);
@@ -128,7 +138,8 @@
  *
  * Can be used to not repeat a calibration but reconstruct the gyro object.
  *
- * @param handle the gyro handle
+ * @param[in] handle the gyro handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the gyro center
  */
 int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogInput.h b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogInput.h
index 95ba4e0..956cd21 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogInput.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogInput.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,11 +21,14 @@
 /**
  * Initializes the analog input port using the given port object.
  *
- * @param portHandle Handle to the port to initialize.
- * @return           the created analog input handle
+ * @param[in] portHandle Handle to the port to initialize.
+ * @param[in] allocationLocation the location where the allocation is occuring
+ *                               (can be null)
+ * @param[out] status the error code, or 0 for success
+ * @return the created analog input handle
  */
-HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
-                                                    int32_t* status);
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation, int32_t* status);
 
 /**
  * Frees an analog input port.
@@ -69,7 +69,8 @@
  *
  * This is a global setting for the Athena and effects all channels.
  *
- * @param samplesPerSecond The number of samples per channel per second.
+ * @param[in] samplesPerSecond The number of samples per channel per second.
+ * @param[out] status          the error code, or 0 for success
  */
 void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status);
 
@@ -79,6 +80,7 @@
  * This assumes one entry in the scan list.
  * This is a global setting for the Athena and effects all channels.
  *
+ * @param[out] status the error code, or 0 for success
  * @return Sample rate.
  */
 double HAL_GetAnalogSampleRate(int32_t* status);
@@ -90,8 +92,9 @@
  * is 2**bits. Use averaging to improve the stability of your measurement at the
  * expense of sampling rate. The averaging is done automatically in the FPGA.
  *
- * @param analogPortHandle Handle to the analog port to configure.
- * @param bits Number of bits to average.
+ * @param[in] analogPortHandle Handle to the analog port to configure.
+ * @param[in] bits Number of bits to average.
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
                               int32_t bits, int32_t* status);
@@ -102,7 +105,8 @@
  * This gets the number of averaging bits from the FPGA. The actual number of
  * averaged samples is 2**bits. The averaging is done automatically in the FPGA.
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status the error code, or 0 for success
  * @return Bits to average.
  */
 int32_t HAL_GetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
@@ -116,8 +120,9 @@
  * measurements at the expense of sampling rate. The oversampling is done
  * automatically in the FPGA.
  *
- * @param analogPortHandle Handle to the analog port to use.
- * @param bits Number of bits to oversample.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[in] bits Number of bits to oversample.
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
                                  int32_t bits, int32_t* status);
@@ -129,7 +134,8 @@
  * oversampled values is 2**bits. The oversampling is done automatically in the
  * FPGA.
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status          the error code, or 0 for success
  * @return Bits to oversample.
  */
 int32_t HAL_GetAnalogOversampleBits(HAL_AnalogInputHandle analogPortHandle,
@@ -142,7 +148,8 @@
  * converter in the module. The units are in A/D converter codes.  Use
  * GetVoltage() to get the analog value in calibrated units.
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status the error code, or 0 for success
  * @return A sample straight from the channel on this module.
  */
 int32_t HAL_GetAnalogValue(HAL_AnalogInputHandle analogPortHandle,
@@ -159,7 +166,8 @@
  * the module on this channel. Use GetAverageVoltage() to get the analog value
  * in calibrated units.
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status the error code, or 0 for success
  * @return A sample from the oversample and average engine for the channel.
  */
 int32_t HAL_GetAnalogAverageValue(HAL_AnalogInputHandle analogPortHandle,
@@ -173,8 +181,9 @@
  *
  * @todo This assumes raw values.  Oversampling not supported as is.
  *
- * @param analogPortHandle Handle to the analog port to use.
- * @param voltage The voltage to convert.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[in] voltage The voltage to convert.
+ * @param[out] status the error code, or 0 for success
  * @return The raw value for the channel.
  */
 int32_t HAL_GetAnalogVoltsToValue(HAL_AnalogInputHandle analogPortHandle,
@@ -186,7 +195,8 @@
  * The value is scaled to units of Volts using the calibrated scaling data from
  * GetLSBWeight() and GetOffset().
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status the error code, or 0 for success
  * @return A scaled sample straight from the channel on this module.
  */
 double HAL_GetAnalogVoltage(HAL_AnalogInputHandle analogPortHandle,
@@ -201,7 +211,8 @@
  * be higher resolution, but it will update more slowly. Using averaging will
  * cause this value to be more stable, but it will update more slowly.
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status the error code, or 0 for success
  * @return A scaled sample from the output of the oversample and average engine
  * for the channel.
  */
@@ -215,7 +226,8 @@
  *
  * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status the error code, or 0 for success
  * @return Least significant bit weight.
  */
 int32_t HAL_GetAnalogLSBWeight(HAL_AnalogInputHandle analogPortHandle,
@@ -228,7 +240,8 @@
  *
  * Volts = ((LSB_Weight * 1e-9) * raw) - (Offset * 1e-9)
  *
- * @param analogPortHandle Handle to the analog port to use.
+ * @param[in] analogPortHandle Handle to the analog port to use.
+ * @param[out] status Error status variable. 0 on success.
  * @return Offset constant.
  */
 int32_t HAL_GetAnalogOffset(HAL_AnalogInputHandle analogPortHandle,
@@ -237,9 +250,11 @@
 /**
  *  Get the analog voltage from a raw value.
  *
- * @param analogPortHandle  Handle to the analog port the values were read from.
- * @param rawValue          The raw analog value
- * @return                  The voltage relating to the value
+ * @param[in] analogPortHandle  Handle to the analog port the values were read
+ *                              from.
+ * @param[in] rawValue          The raw analog value
+ * @param[out] status           Error status variable. 0 on success.
+ * @return The voltage relating to the value
  */
 double HAL_GetAnalogValueToVolts(HAL_AnalogInputHandle analogPortHandle,
                                  int32_t rawValue, int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogOutput.h b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogOutput.h
index ba0d93b..26e5231 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogOutput.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogOutput.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,11 +21,14 @@
 /**
  * Initializes the analog output port using the given port object.
  *
- * @param handle handle to the port
- * @return       the created analog output handle
+ * @param[in] portHandle handle to the port
+ * @param[in] allocationLocation the location where the allocation is occuring
+ *                               (can be null)
+ * @param[out] status Error status variable. 0 on success.
+ * @return the created analog output handle
  */
-HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
-                                                      int32_t* status);
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation, int32_t* status);
 
 /**
  * Frees an analog output port.
@@ -40,8 +40,9 @@
 /**
  * Sets an analog output value.
  *
- * @param analogOutputHandle the analog output handle
- * @param voltage            the voltage (0-5v) to output
+ * @param[in] analogOutputHandle the analog output handle
+ * @param[in] voltage            the voltage (0-5v) to output
+ * @param[out] status            Error status variable. 0 on success.
  */
 void HAL_SetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
                          double voltage, int32_t* status);
@@ -49,8 +50,9 @@
 /**
  * Gets the current analog output value.
  *
- * @param analogOutputHandle the analog output handle
- * @return                   the current output voltage (0-5v)
+ * @param[in] analogOutputHandle the analog output handle
+ * @param[out] status            Error status variable. 0 on success.
+ * @return the current output voltage (0-5v)
  */
 double HAL_GetAnalogOutput(HAL_AnalogOutputHandle analogOutputHandle,
                            int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h
index 85d7680..72727a8 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/AnalogTrigger.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,8 +33,9 @@
 /**
  * Initializes an analog trigger.
  *
- * @param portHandle the analog input to use for triggering
- * @return           the created analog trigger handle
+ * @param[in] portHandle the analog input to use for triggering
+ * @param[out] status     Error status variable. 0 on success.
+ * @return the created analog trigger handle
  */
 HAL_AnalogTriggerHandle HAL_InitializeAnalogTrigger(
     HAL_AnalogInputHandle portHandle, int32_t* status);
@@ -45,6 +43,8 @@
 /**
  * Initializes an analog trigger with a Duty Cycle input
  *
+ * @param[in] dutyCycleHandle the analog input to use for duty cycle
+ * @param[out] status          Error status variable. 0 on success.
  */
 HAL_AnalogTriggerHandle HAL_InitializeAnalogTriggerDutyCycle(
     HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
@@ -52,7 +52,8 @@
 /**
  * Frees an analog trigger.
  *
- * @param analogTriggerHandle the trigger handle
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_CleanAnalogTrigger(HAL_AnalogTriggerHandle analogTriggerHandle,
                             int32_t* status);
@@ -63,9 +64,10 @@
  * HAL_SetAnalogTriggerLimitsVoltage or HAL_SetAnalogTriggerLimitsDutyCycle
  * is likely better in most cases.
  *
- * @param analogTriggerHandle the trigger handle
- * @param lower               the lower ADC value
- * @param upper               the upper ADC value
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[in] lower               the lower ADC value
+ * @param[in] upper               the upper ADC value
+ * @param[out] status              Error status variable. 0 on success.
  */
 void HAL_SetAnalogTriggerLimitsRaw(HAL_AnalogTriggerHandle analogTriggerHandle,
                                    int32_t lower, int32_t upper,
@@ -76,14 +78,25 @@
  *
  * The limits are given as floating point voltage values.
  *
- * @param analogTriggerHandle the trigger handle
- * @param lower               the lower voltage value
- * @param upper               the upper voltage value
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[in] lower               the lower voltage value
+ * @param[in] upper               the upper voltage value
+ * @param[out] status              Error status variable. 0 on success.
  */
 void HAL_SetAnalogTriggerLimitsVoltage(
     HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
     int32_t* status);
 
+/**
+ * Sets the upper and lower limits of the analog trigger.
+ *
+ * The limits are given as floating point duty cycle values.
+ *
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[in] lower               the lower duty cycle value
+ * @param[in] upper               the upper duty cycle value
+ * @param[out] status              Error status variable. 0 on success.
+ */
 void HAL_SetAnalogTriggerLimitsDutyCycle(
     HAL_AnalogTriggerHandle analogTriggerHandle, double lower, double upper,
     int32_t* status);
@@ -97,8 +110,9 @@
  * This is not allowed to be used if filtered mode is set.
  * This is not allowed to be used with Duty Cycle based inputs.
  *
- * @param analogTriggerHandle the trigger handle
- * @param useAveragedValue    true to use averaged values, false for raw
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[in] useAveragedValue    true to use averaged values, false for raw
+ * @param[out] status              Error status variable. 0 on success.
  */
 void HAL_SetAnalogTriggerAveraged(HAL_AnalogTriggerHandle analogTriggerHandle,
                                   HAL_Bool useAveragedValue, int32_t* status);
@@ -112,9 +126,10 @@
  *
  * This is not allowed to be used if averaged mode is set.
  *
- * @param analogTriggerHandle the trigger handle
- * @param useFilteredValue    true to use filtered values, false for average or
- * raw
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[in] useFilteredValue    true to use filtered values, false for average
+ *                                or raw
+ * @param[out] status             Error status variable. 0 on success.
  */
 void HAL_SetAnalogTriggerFiltered(HAL_AnalogTriggerHandle analogTriggerHandle,
                                   HAL_Bool useFilteredValue, int32_t* status);
@@ -124,8 +139,9 @@
  *
  * True if the analog input is between the upper and lower limits.
  *
- * @param analogTriggerHandle the trigger handle
- * @return                    the InWindow output of the analog trigger
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the InWindow output of the analog trigger
  */
 HAL_Bool HAL_GetAnalogTriggerInWindow(
     HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status);
@@ -137,8 +153,9 @@
  * False if below lower limit.
  * If in Hysteresis, maintain previous state.
  *
- * @param analogTriggerHandle the trigger handle
- * @return                    the TriggerState output of the analog trigger
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[out] status              Error status variable. 0 on success.
+ * @return the TriggerState output of the analog trigger
  */
 HAL_Bool HAL_GetAnalogTriggerTriggerState(
     HAL_AnalogTriggerHandle analogTriggerHandle, int32_t* status);
@@ -146,9 +163,10 @@
 /**
  * Gets the state of the analog trigger output.
  *
- * @param analogTriggerHandle the trigger handle
- * @param type                the type of trigger to trigger on
- * @return                    the state of the analog trigger output
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[in] type                the type of trigger to trigger on
+ * @param[out] status              Error status variable. 0 on success.
+ * @return the state of the analog trigger output
  */
 HAL_Bool HAL_GetAnalogTriggerOutput(HAL_AnalogTriggerHandle analogTriggerHandle,
                                     HAL_AnalogTriggerType type,
@@ -157,7 +175,8 @@
 /**
  * Get the FPGA index for the AnlogTrigger.
  *
- * @param analogTriggerHandle the trigger handle
+ * @param[in] analogTriggerHandle the trigger handle
+ * @param[out] status              Error status variable. 0 on success.
  * @return the FPGA index
  */
 int32_t HAL_GetAnalogTriggerFPGAIndex(
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/CAN.h b/third_party/allwpilib/hal/src/main/native/include/hal/CAN.h
index 4f91bc1..77a6239 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/CAN.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/CAN.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -48,11 +45,12 @@
 /**
  * Sends a CAN message.
  *
- * @param messageID the CAN ID to send
- * @param data      the data to send (0-8 bytes)
- * @param dataSize  the size of the data to send (0-8 bytes)
- * @param periodMs  the period to repeat the packet at. Use
- * HAL_CAN_SEND_PERIOD_NO_REPEAT to not repeat.
+ * @param[in] messageID the CAN ID to send
+ * @param[in] data      the data to send (0-8 bytes)
+ * @param[in] dataSize  the size of the data to send (0-8 bytes)
+ * @param[in] periodMs  the period to repeat the packet at. Use
+ *                       HAL_CAN_SEND_PERIOD_NO_REPEAT to not repeat.
+ * @param[out] status    Error status variable. 0 on success.
  */
 void HAL_CAN_SendMessage(uint32_t messageID, const uint8_t* data,
                          uint8_t dataSize, int32_t periodMs, int32_t* status);
@@ -60,12 +58,13 @@
 /**
  * Receives a CAN message.
  *
- * @param messageID     store for the received message ID
- * @param messageIDMask the message ID mask to look for
- * @param data          data output (8 bytes)
- * @param dataSize      data length (0-8 bytes)
- * @param timeStamp     the packet received timestamp (based off of
- * CLOCK_MONOTONIC)
+ * @param[out] messageID     store for the received message ID
+ * @param[in] messageIDMask the message ID mask to look for
+ * @param[out] data          data output (8 bytes)
+ * @param[out] dataSize      data length (0-8 bytes)
+ * @param[out] timeStamp     the packet received timestamp (based off of
+ *                           CLOCK_MONOTONIC)
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_CAN_ReceiveMessage(uint32_t* messageID, uint32_t messageIDMask,
                             uint8_t* data, uint8_t* dataSize,
@@ -74,10 +73,11 @@
 /**
  * Opens a CAN stream.
  *
- * @param sessionHandle output for the session handle
- * @param messageID     the message ID to read
- * @param messageIDMask the mssage ID mask
- * @param maxMessages   the maximum number of messages to stream
+ * @param[out] sessionHandle output for the session handle
+ * @param[in] messageID     the message ID to read
+ * @param[in] messageIDMask the mssage ID mask
+ * @param[in] maxMessages   the maximum number of messages to stream
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_CAN_OpenStreamSession(uint32_t* sessionHandle, uint32_t messageID,
                                uint32_t messageIDMask, uint32_t maxMessages,
@@ -93,10 +93,11 @@
 /**
  * Reads a CAN stream message.
  *
- * @param sessionHandle  the session handle
- * @param messages       array of messages
- * @param messagesToRead the max number of messages to read
- * @param messageRead    the number of messages actually read
+ * @param[in] sessionHandle  the session handle
+ * @param[in] messages       array of messages
+ * @param[in] messagesToRead the max number of messages to read
+ * @param[out] messagesRead   the number of messages actually read
+ * @param[out] status         Error status variable. 0 on success.
  */
 void HAL_CAN_ReadStreamSession(uint32_t sessionHandle,
                                struct HAL_CANStreamMessage* messages,
@@ -106,11 +107,12 @@
 /**
  * Gets CAN status information.
  *
- * @param percentBusUtilization the bus utilization
- * @param busOffCount           the number of bus off errors
- * @param txFullCount           the number of tx full errors
- * @param receiveErrorCount     the number of receive errors
- * @param transmitErrorCount    the number of transmit errors
+ * @param[out] percentBusUtilization the bus utilization
+ * @param[out] busOffCount           the number of bus off errors
+ * @param[out] txFullCount           the number of tx full errors
+ * @param[out] receiveErrorCount     the number of receive errors
+ * @param[out] transmitErrorCount    the number of transmit errors
+ * @param[out] status                Error status variable. 0 on success.
  */
 void HAL_CAN_GetCANStatus(float* percentBusUtilization, uint32_t* busOffCount,
                           uint32_t* txFullCount, uint32_t* receiveErrorCount,
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/CANAPI.h b/third_party/allwpilib/hal/src/main/native/include/hal/CANAPI.h
index 7a6bd88..29859cf 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/CANAPI.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/CANAPI.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -27,10 +24,11 @@
  *
  * These follow the FIRST standard CAN layout. Link TBD
  *
- * @param manufacturer the can manufacturer
- * @param deviceId     the device ID (0-63)
- * @param deviceType   the device type
- * @return             the created CAN handle
+ * @param[in] manufacturer the can manufacturer
+ * @param[in] deviceId     the device ID (0-63)
+ * @param[in] deviceType   the device type
+ * @param[out] status      Error status variable. 0 on success.
+ * @return the created CAN handle
  */
 HAL_CANHandle HAL_InitializeCAN(HAL_CANManufacturer manufacturer,
                                 int32_t deviceId, HAL_CANDeviceType deviceType,
@@ -48,10 +46,11 @@
  *
  * This ID is 10 bits.
  *
- * @param handle the CAN handle
- * @param data   the data to write (0-8 bytes)
- * @param length the length of data (0-8)
- * @param apiId  the ID to write (0-1023 bits)
+ * @param[in] handle  the CAN handle
+ * @param[in] data    the data to write (0-8 bytes)
+ * @param[in] length  the length of data (0-8)
+ * @param[in] apiId   the ID to write (0-1023 bits)
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_WriteCANPacket(HAL_CANHandle handle, const uint8_t* data,
                         int32_t length, int32_t apiId, int32_t* status);
@@ -63,11 +62,12 @@
  *
  * The RoboRIO will automatically repeat the packet at the specified interval
  *
- * @param handle   the CAN handle
- * @param data     the data to write (0-8 bytes)
- * @param length   the length of data (0-8)
- * @param apiId    the ID to write (0-1023)
- * @param repeatMs the period to repeat in ms
+ * @param[in] handle   the CAN handle
+ * @param[in] data     the data to write (0-8 bytes)
+ * @param[in] length   the length of data (0-8)
+ * @param[in] apiId    the ID to write (0-1023)
+ * @param[in] repeatMs the period to repeat in ms
+ * @param[out] status  Error status variable. 0 on success.
  */
 void HAL_WriteCANPacketRepeating(HAL_CANHandle handle, const uint8_t* data,
                                  int32_t length, int32_t apiId,
@@ -80,9 +80,10 @@
  * By spec, the length must be equal to the length sent by the other device,
  * otherwise behavior is unspecified.
  *
- * @param handle   the CAN handle
- * @param length   the length of data to request (0-8)
- * @param apiId    the ID to write (0-1023)
+ * @param[in] handle   the CAN handle
+ * @param[in] length   the length of data to request (0-8)
+ * @param[in] apiId    the ID to write (0-1023)
+ * @param[out] status  Error status variable. 0 on success.
  */
 void HAL_WriteCANRTRFrame(HAL_CANHandle handle, int32_t length, int32_t apiId,
                           int32_t* status);
@@ -92,8 +93,9 @@
  *
  * This ID is 10 bits.
  *
- * @param handle the CAN handle
- * @param apiId  the ID to stop repeating (0-1023)
+ * @param[in] handle  the CAN handle
+ * @param[in] apiId   the ID to stop repeating (0-1023)
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_StopCANPacketRepeating(HAL_CANHandle handle, int32_t apiId,
                                 int32_t* status);
@@ -104,12 +106,13 @@
  * This will only return properly once per packet received. Multiple calls
  * without receiving another packet will return an error code.
  *
- * @param handle            the CAN handle
- * @param apiId             the ID to read (0-1023)
- * @param data              the packet data (8 bytes)
- * @param length            the received length (0-8 bytes)
- * @param receivedTimestamp the packet received timestamp (based off of
- * CLOCK_MONOTONIC)
+ * @param[in] handle             the CAN handle
+ * @param[in] apiId              the ID to read (0-1023)
+ * @param[out] data              the packet data (8 bytes)
+ * @param[out] length            the received length (0-8 bytes)
+ * @param[out] receivedTimestamp the packet received timestamp (based off of
+ *                               CLOCK_MONOTONIC)
+ * @param[out] status            Error status variable. 0 on success.
  */
 void HAL_ReadCANPacketNew(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
                           int32_t* length, uint64_t* receivedTimestamp,
@@ -119,12 +122,13 @@
  * Reads a CAN packet. The will continuously return the last packet received,
  * without accounting for packet age.
  *
- * @param handle            the CAN handle
- * @param apiId             the ID to read (0-1023)
- * @param data              the packet data (8 bytes)
- * @param length            the received length (0-8 bytes)
- * @param receivedTimestamp the packet received timestamp (based off of
- * CLOCK_MONOTONIC)
+ * @param[in] handle             the CAN handle
+ * @param[in] apiId              the ID to read (0-1023)
+ * @param[out] data              the packet data (8 bytes)
+ * @param[out] length            the received length (0-8 bytes)
+ * @param[out] receivedTimestamp the packet received timestamp (based off of
+ *                               CLOCK_MONOTONIC)
+ * @param[out] status            Error status variable. 0 on success.
  */
 void HAL_ReadCANPacketLatest(HAL_CANHandle handle, int32_t apiId, uint8_t* data,
                              int32_t* length, uint64_t* receivedTimestamp,
@@ -135,13 +139,14 @@
  * packet is older then the requested timeout. Then it will return an error
  * code.
  *
- * @param handle            the CAN handle
- * @param apiId             the ID to read (0-1023)
- * @param data              the packet data (8 bytes)
- * @param length            the received length (0-8 bytes)
- * @param receivedTimestamp the packet received timestamp (based off of
- * CLOCK_MONOTONIC)
- * @param timeoutMs         the timeout time for the packet
+ * @param[in] handle             the CAN handle
+ * @param[in] apiId              the ID to read (0-1023)
+ * @param[out] data              the packet data (8 bytes)
+ * @param[out] length            the received length (0-8 bytes)
+ * @param[out] receivedTimestamp the packet received timestamp (based off of
+ *                               CLOCK_MONOTONIC)
+ * @param[out] timeoutMs         the timeout time for the packet
+ * @param[out] status            Error status variable. 0 on success.
  */
 void HAL_ReadCANPacketTimeout(HAL_CANHandle handle, int32_t apiId,
                               uint8_t* data, int32_t* length,
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h
index 5155d93..1be672e 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/CANAPITypes.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/CTREPCM.h b/third_party/allwpilib/hal/src/main/native/include/hal/CTREPCM.h
new file mode 100644
index 0000000..1bed928
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/CTREPCM.h
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_ctre_pcm CTRE PCM Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
+                                        const char* allocationLocation,
+                                        int32_t* status);
+void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle);
+
+HAL_Bool HAL_CheckCTREPCMSolenoidChannel(int32_t channel);
+
+HAL_Bool HAL_GetCTREPCMCompressor(HAL_CTREPCMHandle handle, int32_t* status);
+void HAL_SetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, HAL_Bool enabled,
+                                     int32_t* status);
+HAL_Bool HAL_GetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle,
+                                         int32_t* status);
+HAL_Bool HAL_GetCTREPCMPressureSwitch(HAL_CTREPCMHandle handle,
+                                      int32_t* status);
+double HAL_GetCTREPCMCompressorCurrent(HAL_CTREPCMHandle handle,
+                                       int32_t* status);
+
+HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighFault(HAL_CTREPCMHandle handle,
+                                                     int32_t* status);
+HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(
+    HAL_CTREPCMHandle handle, int32_t* status);
+HAL_Bool HAL_GetCTREPCMCompressorShortedStickyFault(HAL_CTREPCMHandle handle,
+                                                    int32_t* status);
+HAL_Bool HAL_GetCTREPCMCompressorShortedFault(HAL_CTREPCMHandle handle,
+                                              int32_t* status);
+HAL_Bool HAL_GetCTREPCMCompressorNotConnectedStickyFault(
+    HAL_CTREPCMHandle handle, int32_t* status);
+HAL_Bool HAL_GetCTREPCMCompressorNotConnectedFault(HAL_CTREPCMHandle handle,
+                                                   int32_t* status);
+
+int32_t HAL_GetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t* status);
+void HAL_SetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t mask,
+                             int32_t values, int32_t* status);
+
+int32_t HAL_GetCTREPCMSolenoidDisabledList(HAL_CTREPCMHandle handle,
+                                           int32_t* status);
+HAL_Bool HAL_GetCTREPCMSolenoidVoltageStickyFault(HAL_CTREPCMHandle handle,
+                                                  int32_t* status);
+HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle,
+                                            int32_t* status);
+
+void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle, int32_t* status);
+
+void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index,
+                            int32_t* status);
+void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
+                                   int32_t durMs, int32_t* status);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/ChipObject.h b/third_party/allwpilib/hal/src/main/native/include/hal/ChipObject.h
index d891ced..4526a1a 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/ChipObject.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/ChipObject.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 #pragma GCC diagnostic push
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Compressor.h b/third_party/allwpilib/hal/src/main/native/include/hal/Compressor.h
deleted file mode 100644
index cb58e46..0000000
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Compressor.h
+++ /dev/null
@@ -1,143 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <stdint.h>
-
-#include "hal/Types.h"
-
-/**
- * @defgroup hal_compressor Compressor Functions
- * @ingroup hal_capi
- * @{
- */
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/**
- * Initializes a compressor on the given PCM module.
- *
- * @param module the module number
- * @return       the created handle
- */
-HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status);
-
-/**
- * Gets if a compressor module is valid.
- *
- * @param module the module number
- * @return       true if the module is valid, otherwise false
- */
-HAL_Bool HAL_CheckCompressorModule(int32_t module);
-
-/**
- * Gets the compressor state (on or off).
- *
- * @param compressorHandle the compressor handle
- * @return                 true if the compressor is on, otherwise false
- */
-HAL_Bool HAL_GetCompressor(HAL_CompressorHandle compressorHandle,
-                           int32_t* status);
-
-/**
- * Sets the compressor to closed loop mode.
- *
- * @param compressorHandle the compressor handle
- * @param value            true for closed loop mode, false for off
- */
-void HAL_SetCompressorClosedLoopControl(HAL_CompressorHandle compressorHandle,
-                                        HAL_Bool value, int32_t* status);
-
-/**
- * Gets if the compressor is in closed loop mode.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if the compressor is in closed loop mode,
- * otherwise false
- */
-HAL_Bool HAL_GetCompressorClosedLoopControl(
-    HAL_CompressorHandle compressorHandle, int32_t* status);
-
-/**
- * Gets the compressor pressure switch state.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if the pressure switch is triggered, otherwise
- * false
- */
-HAL_Bool HAL_GetCompressorPressureSwitch(HAL_CompressorHandle compressorHandle,
-                                         int32_t* status);
-
-/**
- * Gets the compressor current.
- *
- * @param compressorHandle the compressor handle
- * @return                 the compressor current in amps
- */
-double HAL_GetCompressorCurrent(HAL_CompressorHandle compressorHandle,
-                                int32_t* status);
-
-/**
- * Gets if the compressor is faulted because of too high of current.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if falted, otherwise false
- */
-HAL_Bool HAL_GetCompressorCurrentTooHighFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status);
-
-/**
- * Gets if a sticky fauly is triggered because of too high of current.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if falted, otherwise false
- */
-HAL_Bool HAL_GetCompressorCurrentTooHighStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status);
-
-/**
- * Gets if a sticky fauly is triggered because of a short.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if falted, otherwise false
- */
-HAL_Bool HAL_GetCompressorShortedStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status);
-
-/**
- * Gets if the compressor is faulted because of a short.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if shorted, otherwise false
- */
-HAL_Bool HAL_GetCompressorShortedFault(HAL_CompressorHandle compressorHandle,
-                                       int32_t* status);
-
-/**
- * Gets if a sticky fault is triggered of the compressor not connected.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if falted, otherwise false
- */
-HAL_Bool HAL_GetCompressorNotConnectedStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status);
-
-/**
- * Gets if the compressor is not connected.
- *
- * @param compressorHandle the compressor handle
- * @return                 true if not connected, otherwise false
- */
-HAL_Bool HAL_GetCompressorNotConnectedFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status);
-#ifdef __cplusplus
-}  // extern "C"
-#endif
-/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Constants.h b/third_party/allwpilib/hal/src/main/native/include/hal/Constants.h
index 2a4d409..c8c731c 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Constants.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h b/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h
index c0dd057..cc2d776 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Counter.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -37,9 +34,10 @@
 /**
  * Initializes a counter.
  *
- * @param mode  the counter mode
- * @param index the compressor index (output)
- * @return      the created handle
+ * @param[in] mode    the counter mode
+ * @param[in] index   the compressor index (output)
+ * @param[out] status Error status variable. 0 on success.
+ * @return the created handle
  */
 HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
                                         int32_t* status);
@@ -47,15 +45,17 @@
 /**
  * Frees a counter.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_FreeCounter(HAL_CounterHandle counterHandle, int32_t* status);
 
 /**
  * Sets the average sample size of a counter.
  *
- * @param counterHandle the counter handle
- * @param size          the size of samples to average
+ * @param[in] counterHandle  the counter handle
+ * @param[in] size           the size of samples to average
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_SetCounterAverageSize(HAL_CounterHandle counterHandle, int32_t size,
                                int32_t* status);
@@ -63,11 +63,13 @@
 /**
  * Sets the source object that causes the counter to count up.
  *
- * @param counterHandle       the counter handle
- * @param digitalSourceHandle the digital source handle (either a
- * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
- * @param analogTriggerType   the analog trigger type if the source is an analog
- * trigger
+ * @param[in] counterHandle       the counter handle
+ * @param[in] digitalSourceHandle the digital source handle (either a
+ *                                HAL_AnalogTriggerHandle or a
+ *                                HAL_DigitalHandle)
+ * @param[in] analogTriggerType   the analog trigger type if the source is an
+ *                                analog trigger
+ * @param[out] status             Error status variable. 0 on success.
  */
 void HAL_SetCounterUpSource(HAL_CounterHandle counterHandle,
                             HAL_Handle digitalSourceHandle,
@@ -79,9 +81,10 @@
  *
  * Note that both are allowed to be set true at the same time without issues.
  *
- * @param counterHandle the counter handle
- * @param risingEdge    true to trigger on rising
- * @param fallingEdge   true to trigger on falling
+ * @param[in] counterHandle  the counter handle
+ * @param[in] risingEdge     true to trigger on rising
+ * @param[in] fallingEdge    true to trigger on falling
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_SetCounterUpSourceEdge(HAL_CounterHandle counterHandle,
                                 HAL_Bool risingEdge, HAL_Bool fallingEdge,
@@ -90,18 +93,21 @@
 /**
  * Disables the up counting source to the counter.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_ClearCounterUpSource(HAL_CounterHandle counterHandle, int32_t* status);
 
 /**
  * Sets the source object that causes the counter to count down.
  *
- * @param counterHandle       the counter handle
- * @param digitalSourceHandle the digital source handle (either a
- * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
- * @param analogTriggerType   the analog trigger type if the source is an analog
- * trigger
+ * @param[in] counterHandle       the counter handle
+ * @param[in] digitalSourceHandle the digital source handle (either a
+ *                                HAL_AnalogTriggerHandle or a
+ *                                HAL_DigitalHandle)
+ * @param[in] analogTriggerType   the analog trigger type if the source is an
+ *                                analog trigger
+ * @param[out] status             Error status variable. 0 on success.
  */
 void HAL_SetCounterDownSource(HAL_CounterHandle counterHandle,
                               HAL_Handle digitalSourceHandle,
@@ -112,9 +118,10 @@
  * Sets the down source to either detect rising edges or falling edges.
  * Note that both are allowed to be set true at the same time without issues.
  *
- * @param counterHandle the counter handle
- * @param risingEdge    true to trigger on rising
- * @param fallingEdge   true to trigger on falling
+ * @param[in] counterHandle  the counter handle
+ * @param[in] risingEdge     true to trigger on rising
+ * @param[in] fallingEdge    true to trigger on falling
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_SetCounterDownSourceEdge(HAL_CounterHandle counterHandle,
                                   HAL_Bool risingEdge, HAL_Bool fallingEdge,
@@ -123,7 +130,8 @@
 /**
  * Disables the down counting source to the counter.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_ClearCounterDownSource(HAL_CounterHandle counterHandle,
                                 int32_t* status);
@@ -133,7 +141,8 @@
  *
  * Up and down counts are sourced independently from two inputs.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetCounterUpDownMode(HAL_CounterHandle counterHandle, int32_t* status);
 
@@ -143,7 +152,8 @@
  * The direction is determined by the B input, with counting happening with the
  * A input.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetCounterExternalDirectionMode(HAL_CounterHandle counterHandle,
                                          int32_t* status);
@@ -154,8 +164,10 @@
  * The counter counts up based on the time the input is triggered. High or Low
  * depends on the highSemiPeriod parameter.
  *
- * @param counterHandle  the counter handle
- * @param highSemiPeriod true for counting when the input is high, false for low
+ * @param[in] counterHandle  the counter handle
+ * @param[in] highSemiPeriod true for counting when the input is high, false for
+ *                           low
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_SetCounterSemiPeriodMode(HAL_CounterHandle counterHandle,
                                   HAL_Bool highSemiPeriod, int32_t* status);
@@ -166,9 +178,10 @@
  *
  * This mode is most useful for direction sensitive gear tooth sensors.
  *
- * @param counterHandle the counter handle
- * @param threshold The pulse length beyond which the counter counts the
- * opposite direction (seconds)
+ * @param[in] counterHandle the counter handle
+ * @param[in] threshold     The pulse length beyond which the counter counts the
+ *                          opposite direction (seconds)
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetCounterPulseLengthMode(HAL_CounterHandle counterHandle,
                                    double threshold, int32_t* status);
@@ -178,7 +191,8 @@
  * timer to average when calculating the period. Perform averaging to account
  * for mechanical imperfections or as oversampling to increase resolution.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  * @return SamplesToAverage The number of samples being averaged (from 1 to 127)
  */
 int32_t HAL_GetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
@@ -189,8 +203,9 @@
  * timer to average when calculating the period. Perform averaging to account
  * for mechanical imperfections or as oversampling to increase resolution.
  *
- * @param counterHandle    the counter handle
- * @param samplesToAverage The number of samples to average from 1 to 127
+ * @param[in] counterHandle    the counter handle
+ * @param[in] samplesToAverage The number of samples to average from 1 to 127
+ * @param[out] status          Error status variable. 0 on success.
  */
 void HAL_SetCounterSamplesToAverage(HAL_CounterHandle counterHandle,
                                     int32_t samplesToAverage, int32_t* status);
@@ -201,7 +216,8 @@
  * Sets the counter value to zero. This does not effect the running state of the
  * counter, just sets the current value to zero.
  *
- * @param counterHandle the counter handle
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_ResetCounter(HAL_CounterHandle counterHandle, int32_t* status);
 
@@ -211,8 +227,9 @@
  * Reads the value at this instant. It may still be running, so it reflects the
  * current value. Next time it is read, it might have a different value.
  *
- * @param counterHandle the counter handle
- * @return              the current counter value
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the current counter value
  */
 int32_t HAL_GetCounter(HAL_CounterHandle counterHandle, int32_t* status);
 
@@ -222,8 +239,9 @@
  * Returns the time interval of the most recent count. This can be used for
  * velocity calculations to determine shaft speed.
  *
- * @param counterHandle the counter handle
- * @returns             the period of the last two pulses in units of seconds
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the period of the last two pulses in units of seconds
  */
 double HAL_GetCounterPeriod(HAL_CounterHandle counterHandle, int32_t* status);
 
@@ -234,9 +252,10 @@
  * used to determine the "stopped" state of the counter using the
  * HAL_GetCounterStopped method.
  *
- * @param counterHandle the counter handle
- * @param maxPeriod     the maximum period where the counted device is
- * considered moving in seconds
+ * @param[in] counterHandle the counter handle
+ * @param[in] maxPeriod     the maximum period where the counted device is
+ *                          considered moving in seconds
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetCounterMaxPeriod(HAL_CounterHandle counterHandle, double maxPeriod,
                              int32_t* status);
@@ -261,8 +280,9 @@
  * (since it is updated at the end of an average and there are no samples to
  * average).
  *
- * @param counterHandle the counter handle
- * @param enabled       true to enable counter updating with no samples
+ * @param[in] counterHandle the counter handle
+ * @param[in] enabled       true to enable counter updating with no samples
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetCounterUpdateWhenEmpty(HAL_CounterHandle counterHandle,
                                    HAL_Bool enabled, int32_t* status);
@@ -274,9 +294,10 @@
  * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
  * device (and counter) are assumed to be stopped and it returns true.
  *
- * @param counterHandle the counter handle
- * @return              true if the most recent counter period exceeds the
- * MaxPeriod value set by SetMaxPeriod
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return true if the most recent counter period exceeds the MaxPeriod value
+ *         set by SetMaxPeriod
  */
 HAL_Bool HAL_GetCounterStopped(HAL_CounterHandle counterHandle,
                                int32_t* status);
@@ -284,8 +305,9 @@
 /**
  * Gets the last direction the counter value changed.
  *
- * @param counterHandle the counter handle
- * @return              the last direction the counter value changed
+ * @param[in] counterHandle the counter handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the last direction the counter value changed
  */
 HAL_Bool HAL_GetCounterDirection(HAL_CounterHandle counterHandle,
                                  int32_t* status);
@@ -296,8 +318,9 @@
  * This allows counters to change the direction they are counting in the case of
  * 1X and 2X quadrature encoding only. Any other counter mode isn't supported.
  *
- * @param counterHandle    the counter handle
- * @param reverseDirection true if the value counted should be negated.
+ * @param[in] counterHandle    the counter handle
+ * @param[in] reverseDirection true if the value counted should be negated.
+ * @param[out] status          Error status variable. 0 on success.
  */
 void HAL_SetCounterReverseDirection(HAL_CounterHandle counterHandle,
                                     HAL_Bool reverseDirection, int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DIO.h b/third_party/allwpilib/hal/src/main/native/include/hal/DIO.h
index 6ffc71f..e094a0d 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DIO.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DIO.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,21 +21,31 @@
 /**
  * Creates a new instance of a digital port.
  *
- * @param portHandle the port handle to create from
- * @param input      true for input, false for output
- * @return           the created digital handle
+ * @param[in] portHandle         the port handle to create from
+ * @param[in] input              true for input, false for output
+ * @param[in] allocationLocation the location where the allocation is occuring
+ *                               (can be null)
+ * @param[out] status            Error status variable. 0 on success.
+ * @return the created digital handle
  */
 HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
-                                        HAL_Bool input, int32_t* status);
+                                        HAL_Bool input,
+                                        const char* allocationLocation,
+                                        int32_t* status);
 
 /**
  * Checks if a DIO channel is valid.
  *
  * @param channel the channel number to check
- * @return        true if the channel is correct, otherwise false
+ * @return true if the channel is correct, otherwise false
  */
 HAL_Bool HAL_CheckDIOChannel(int32_t channel);
 
+/**
+ * Frees a DIO port.
+ *
+ * @param dioPortHandle the DIO channel handle
+ */
 void HAL_FreeDIOPort(HAL_DigitalHandle dioPortHandle);
 
 /**
@@ -52,6 +59,7 @@
 /**
  * Allocates a DO PWM Generator.
  *
+ * @param[out] status Error status variable. 0 on success.
  * @return the allocated digital PWM handle
  */
 HAL_DigitalPWMHandle HAL_AllocateDigitalPWM(int32_t* status);
@@ -59,7 +67,8 @@
 /**
  * Frees the resource associated with a DO PWM generator.
  *
- * @param pwmGenerator the digital PWM handle
+ * @param[in] pwmGenerator the digital PWM handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status);
 
@@ -70,15 +79,17 @@
  *
  *  The frequency resolution is logarithmic.
  *
- * @param rate the frequency to output all digital output PWM signals
+ * @param[in] rate the frequency to output all digital output PWM signals
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_SetDigitalPWMRate(double rate, int32_t* status);
 
 /**
  * Configures the duty-cycle of the PWM generator.
  *
- * @param pwmGenerator the digital PWM handle
- * @param dutyCycle    the percent duty cycle to output [0..1]
+ * @param[in] pwmGenerator the digital PWM handle
+ * @param[in] dutyCycle    the percent duty cycle to output [0..1]
+ * @param[out] status      Error status variable. 0 on success.
  */
 void HAL_SetDigitalPWMDutyCycle(HAL_DigitalPWMHandle pwmGenerator,
                                 double dutyCycle, int32_t* status);
@@ -86,8 +97,9 @@
 /**
  * Configures which DO channel the PWM signal is output on.
  *
- * @param pwmGenerator the digital PWM handle
- * @param channel      the channel to output on
+ * @param[in] pwmGenerator the digital PWM handle
+ * @param[in] channel      the channel to output on
+ * @param[out] status      Error status variable. 0 on success.
  */
 void HAL_SetDigitalPWMOutputChannel(HAL_DigitalPWMHandle pwmGenerator,
                                     int32_t channel, int32_t* status);
@@ -95,9 +107,10 @@
 /**
  * Writes a digital value to a DIO channel.
  *
- * @param dioPortHandle the digital port handle
- * @param value         the state to set the digital channel (if it is
- * configured as an output)
+ * @param[in] dioPortHandle the digital port handle
+ * @param[in] value         the state to set the digital channel (if it is
+ *                          configured as an output)
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetDIO(HAL_DigitalHandle dioPortHandle, HAL_Bool value,
                 int32_t* status);
@@ -105,8 +118,9 @@
 /**
  * Sets the direction of a DIO channel.
  *
- * @param dioPortHandle the digital port handle
- * @param input         true to set input, false for output
+ * @param[in] dioPortHandle the digital port handle
+ * @param[in] input         true to set input, false for output
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetDIODirection(HAL_DigitalHandle dioPortHandle, HAL_Bool input,
                          int32_t* status);
@@ -114,16 +128,18 @@
 /**
  * Reads a digital value from a DIO channel.
  *
- * @param dioPortHandle the digital port handle
- * @return              the state of the specified channel
+ * @param[in] dioPortHandle the digital port handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the state of the specified channel
  */
 HAL_Bool HAL_GetDIO(HAL_DigitalHandle dioPortHandle, int32_t* status);
 
 /**
  * Reads the direction of a DIO channel.
  *
- * @param dioPortHandle the digital port handle
- * @return              true for input, false for output
+ * @param[in] dioPortHandle the digital port handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return true for input, false for output
  */
 HAL_Bool HAL_GetDIODirection(HAL_DigitalHandle dioPortHandle, int32_t* status);
 
@@ -133,8 +149,9 @@
  * Write a pulse to the specified digital output channel. There can only be a
  * single pulse going at any time.
  *
- * @param dioPortHandle the digital port handle
- * @param pulseLength   the active length of the pulse (in seconds)
+ * @param[in] dioPortHandle the digital port handle
+ * @param[in] pulseLength   the active length of the pulse (in seconds)
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_Pulse(HAL_DigitalHandle dioPortHandle, double pulseLength,
                int32_t* status);
@@ -142,6 +159,8 @@
 /**
  * Checks a DIO line to see if it is currently generating a pulse.
  *
+ * @param[in] dioPortHandle the digital port handle
+ * @param[out] status Error status variable. 0 on success.
  * @return true if a pulse is in progress, otherwise false
  */
 HAL_Bool HAL_IsPulsing(HAL_DigitalHandle dioPortHandle, int32_t* status);
@@ -149,6 +168,7 @@
 /**
  * Checks if any DIO line is currently generating a pulse.
  *
+ * @param[out] status Error status variable. 0 on success.
  * @return true if a pulse on some line is in progress
  */
 HAL_Bool HAL_IsAnyPulsing(int32_t* status);
@@ -158,9 +178,11 @@
  *
  * Set the filter index used to filter out short pulses.
  *
- * @param dioPortHandle the digital port handle
- * @param filterIndex   the filter index (Must be in the range 0 - 3, where 0
- * means "none" and 1 - 3 means filter # filterIndex - 1)
+ * @param[in] dioPortHandle the digital port handle
+ * @param[in] filterIndex   the filter index (Must be in the range 0 - 3, where
+ *                          0 means "none" and 1 - 3 means filter # filterIndex
+ *                          - 1)
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t filterIndex,
                          int32_t* status);
@@ -170,9 +192,10 @@
  *
  * Gets the filter index used to filter out short pulses.
  *
- * @param dioPortHandle the digital port handle
- * @return filterIndex  the filter index (Must be in the range 0 - 3,
- * where 0 means "none" and 1 - 3 means filter # filterIndex - 1)
+ * @param[in] dioPortHandle the digital port handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return filterIndex  the filter index (Must be in the range 0 - 3, where 0
+ *                      means "none" and 1 - 3 means filter # filterIndex - 1)
  */
 int32_t HAL_GetFilterSelect(HAL_DigitalHandle dioPortHandle, int32_t* status);
 
@@ -183,9 +206,10 @@
  * filter index domains (MXP vs HDR), ignore that distinction for now since it
  * compilicates the interface.  That can be changed later.
  *
- * @param filterIndex the filter index, 0 - 2
- * @param value       the number of cycles that the signal must not transition
- * to be counted as a transition.
+ * @param[in] filterIndex the filter index, 0 - 2
+ * @param[in] value       the number of cycles that the signal must not
+ *                        transition to be counted as a transition.
+ * @param[out] status     Error status variable. 0 on success.
  */
 void HAL_SetFilterPeriod(int32_t filterIndex, int64_t value, int32_t* status);
 
@@ -197,9 +221,8 @@
  * compilicates the interface.  Set status to NiFpga_Status_SoftwareFault if the
  * filter values miss-match.
  *
- * @param filterIndex the filter index, 0 - 2
- * @param value       the number of cycles that the signal must not transition
- * to be counted as a transition.
+ * @param[in] filterIndex the filter index, 0 - 2
+ * @param[out] status     Error status variable. 0 on success.
  */
 int64_t HAL_GetFilterPeriod(int32_t filterIndex, int32_t* status);
 #ifdef __cplusplus
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h b/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h
index 38aaca2..7610d9b 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DMA.h
@@ -1,28 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <stdint.h>
-
 #include "hal/AnalogTrigger.h"
 #include "hal/Types.h"
 
+/**
+ * @defgroup hal_dma DMA Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
 // clang-format off
 /**
  * The DMA Read Status.
  */
-HAL_ENUM(HAL_DMAReadStatus ) {
+HAL_ENUM(HAL_DMAReadStatus) {
   HAL_DMA_OK = 1,
   HAL_DMA_TIMEOUT = 2,
   HAL_DMA_ERROR = 3,
 };
 // clang-format on
 
+/**
+ * Buffer for containing all DMA data for a specific sample.
+ */
 struct HAL_DMASample {
   uint32_t readBuffer[74];
   int32_t channelOffsets[22];
@@ -35,91 +39,415 @@
 extern "C" {
 #endif
 
+/**
+ * Initializes an object for peforming DMA transfers.
+ *
+ * @param[out] status Error status variable. 0 on success.
+ * @return the created dma handle
+ */
 HAL_DMAHandle HAL_InitializeDMA(int32_t* status);
+
+/**
+ * Frees a DMA object.
+ *
+ * @param handle the dma handle
+ */
 void HAL_FreeDMA(HAL_DMAHandle handle);
 
+/**
+ * Pauses or unpauses a DMA transfer.
+ *
+ * This can only be called while DMA is running.
+ *
+ * @param[in] handle  the dma handle
+ * @param[in] pause   true to pause transfers, false to resume.
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status);
-void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status);
 
+/**
+ * Sets DMA transfers to occur at a specific timed interval.
+ *
+ * This will remove any external triggers. Only timed or external is supported.
+ *
+ * Only 1 timed period is supported.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle        the dma handle
+ * @param[in] periodSeconds the period to trigger in seconds
+ * @param[out] status       Error status variable. 0 on success.
+ */
+void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double periodSeconds,
+                            int32_t* status);
+
+/**
+ * Sets DMA transfers to occur at a specific timed interval in FPGA cycles.
+ *
+ * This will remove any external triggers. Only timed or external is supported.
+ *
+ * Only 1 timed period is supported
+ *
+ * The FPGA currently runs at 40 MHz, but this can change.
+ * HAL_GetSystemClockTicksPerMicrosecond can be used to get a computable value
+ * for this.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] cycles the period to trigger in FPGA cycles
+ * @param[out] status Error status variable. 0 on success.
+ */
+void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles,
+                                  int32_t* status);
+
+/**
+ * Adds position data for an encoder to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] encoderHandle the encoder to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
                        int32_t* status);
+
+/**
+ * Adds timer data for an encoder to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] encoderHandle the encoder to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMAEncoderPeriod(HAL_DMAHandle handle,
                              HAL_EncoderHandle encoderHandle, int32_t* status);
+
+/**
+ * Adds position data for an counter to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] counterHandle the counter to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMACounter(HAL_DMAHandle handle, HAL_CounterHandle counterHandle,
                        int32_t* status);
+
+/**
+ * Adds timer data for an counter to be collected by DMA.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] counterHandle the counter to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMACounterPeriod(HAL_DMAHandle handle,
                              HAL_CounterHandle counterHandle, int32_t* status);
+
+/**
+ * Adds a digital source to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] digitalSourceHandle the digital source to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMADigitalSource(HAL_DMAHandle handle,
                              HAL_Handle digitalSourceHandle, int32_t* status);
+
+/**
+ * Adds an analog input to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] aInHandle the analog input to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMAAnalogInput(HAL_DMAHandle handle,
                            HAL_AnalogInputHandle aInHandle, int32_t* status);
 
+/**
+ * Adds averaged data of an analog input to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] aInHandle the analog input to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMAAveragedAnalogInput(HAL_DMAHandle handle,
                                    HAL_AnalogInputHandle aInHandle,
                                    int32_t* status);
 
+/**
+ * Adds acuumulator data of an analog input to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] aInHandle the analog input to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMAAnalogAccumulator(HAL_DMAHandle handle,
                                  HAL_AnalogInputHandle aInHandle,
                                  int32_t* status);
 
+/**
+ * Adds a duty cycle input to be collected by DMA.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] dutyCycleHandle the duty cycle input to add
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_AddDMADutyCycle(HAL_DMAHandle handle,
                          HAL_DutyCycleHandle dutyCycleHandle, int32_t* status);
 
-void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
-                               HAL_Handle digitalSourceHandle,
-                               HAL_AnalogTriggerType analogTriggerType,
-                               HAL_Bool rising, HAL_Bool falling,
-                               int32_t* status);
+/**
+ * Sets DMA transfers to occur on an external trigger.
+ *
+ * This will remove any timed trigger set. Only timed or external is supported.
+ *
+ * Up to 8 external triggers are currently supported.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle              the dma handle
+ * @param[in] digitalSourceHandle the digital source handle (either a
+ *                                HAL_AnalogTriggerHandle or a
+ *                                HAL_DigitalHandle)
+ * @param[in] analogTriggerType   the analog trigger type if the source is an
+ *                                analog trigger
+ * @param[in] rising              true to trigger on rising edge
+ * @param[in] falling             true to trigger on falling edge
+ * @param[out] status             Error status variable. 0 on success.
+ * @return the index of the trigger
+ */
+int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+                                  HAL_Handle digitalSourceHandle,
+                                  HAL_AnalogTriggerType analogTriggerType,
+                                  HAL_Bool rising, HAL_Bool falling,
+                                  int32_t* status);
 
+/**
+ * Clear all sensors from the DMA collection list.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[out] status Error status variable. 0 on success.
+ */
+void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status);
+
+/**
+ * Clear all external triggers from the DMA trigger list.
+ *
+ * This can only be called if DMA is not started.
+ *
+ * @param[in] handle the dma handle
+ * @param[out] status Error status variable. 0 on success.
+ */
+void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status);
+
+/**
+ * Starts DMA Collection.
+ *
+ * @param[in] handle the dma handle
+ * @param[in] queueDepth the number of objects to be able to queue
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status);
+
+/**
+ * Stops DMA Collection.
+ *
+ * @param[in] handle the dma handle
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status);
 
+/**
+ * Gets the direct pointer to the DMA object.
+ *
+ * This is only to be used if absolute maximum performance is required. This
+ * will only be valid until the handle is freed.
+ *
+ * @param handle the dma handle
+ */
 void* HAL_GetDMADirectPointer(HAL_DMAHandle handle);
 
+/**
+ * Reads a DMA sample using a direct DMA pointer.
+ *
+ * See HAL_ReadDMA for full documentation.
+ *
+ * @param[in] dmaPointer     direct DMA pointer
+ * @param[in] dmaSample      the sample object to place data into
+ * @param[in] timeoutSeconds the time to wait for data to be queued before
+ *                           timing out
+ * @param[in] remainingOut   the number of samples remaining in the queue
+ * @param[out] status        Error status variable. 0 on success.
+ */
 enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
                                          HAL_DMASample* dmaSample,
-                                         int32_t timeoutMs,
+                                         double timeoutSeconds,
                                          int32_t* remainingOut,
                                          int32_t* status);
 
+/**
+ * Reads a DMA sample from the queue.
+ *
+ *
+ * @param[in] handle         the dma handle
+ * @param[in] dmaSample      the sample object to place data into
+ * @param[in] timeoutSeconds the time to wait for data to be queued before
+ *                           timing out
+ * @param[in] remainingOut   the number of samples remaining in the queue
+ * @param[out] status        Error status variable. 0 on success.
+ * @return the succes result of the sample read
+ */
 enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
-                                   HAL_DMASample* dmaSample, int32_t timeoutMs,
-                                   int32_t* remainingOut, int32_t* status);
+                                   HAL_DMASample* dmaSample,
+                                   double timeoutSeconds, int32_t* remainingOut,
+                                   int32_t* status);
 
-// Sampling Code
+// The following are helper functions for reading data from samples
+
+/**
+ * Returns the timestamp of the sample.
+ * This is in the same time domain as HAL_GetFPGATime().
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[out] status Error status variable. 0 on success.
+ * @return timestamp in microseconds since FPGA Initialization
+ */
 uint64_t HAL_GetDMASampleTime(const HAL_DMASample* dmaSample, int32_t* status);
 
+/**
+ * Returns the raw distance data for an encoder from the sample.
+ *
+ * This can be scaled with DistancePerPulse and DecodingScaleFactor to match the
+ * result of GetDistance()
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return raw encoder ticks
+ */
 int32_t HAL_GetDMASampleEncoderRaw(const HAL_DMASample* dmaSample,
                                    HAL_EncoderHandle encoderHandle,
                                    int32_t* status);
 
+/**
+ * Returns the distance data for an counter from the sample.
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] counterHandle the counter handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return counter ticks
+ */
 int32_t HAL_GetDMASampleCounter(const HAL_DMASample* dmaSample,
                                 HAL_CounterHandle counterHandle,
                                 int32_t* status);
 
+/**
+ * Returns the raw period data for an encoder from the sample.
+ *
+ * This can be scaled with DistancePerPulse and DecodingScaleFactor to match the
+ * result of GetRate()
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return raw encoder period
+ */
 int32_t HAL_GetDMASampleEncoderPeriodRaw(const HAL_DMASample* dmaSample,
                                          HAL_EncoderHandle encoderHandle,
                                          int32_t* status);
 
+/**
+ * Returns the period data for an counter from the sample.
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] counterHandle the counter handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return counter period
+ */
 int32_t HAL_GetDMASampleCounterPeriod(const HAL_DMASample* dmaSample,
                                       HAL_CounterHandle counterHandle,
                                       int32_t* status);
+
+/**
+ * Returns the state of a digital source from the sample.
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] dSourceHandle the digital source handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return digital source state
+ */
 HAL_Bool HAL_GetDMASampleDigitalSource(const HAL_DMASample* dmaSample,
                                        HAL_Handle dSourceHandle,
                                        int32_t* status);
+
+/**
+ * Returns the raw analog data for an analog input from the sample.
+ *
+ * This can be scaled with HAL_GetAnalogValueToVolts to match GetVoltage().
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] aInHandle the analog input handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return raw analog data
+ */
 int32_t HAL_GetDMASampleAnalogInputRaw(const HAL_DMASample* dmaSample,
                                        HAL_AnalogInputHandle aInHandle,
                                        int32_t* status);
 
+/**
+ * Returns the raw averaged analog data for an analog input from the sample.
+ *
+ * This can be scaled with HAL_GetAnalogValueToVolts to match
+ * GetAveragedVoltage().
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] aInHandle the analog input handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return raw averaged analog data
+ */
 int32_t HAL_GetDMASampleAveragedAnalogInputRaw(const HAL_DMASample* dmaSample,
                                                HAL_AnalogInputHandle aInHandle,
                                                int32_t* status);
 
+/**
+ * Returns the analog accumulator data for an analog input from the sample.
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] aInHandle the analog input handle
+ * @param[in] count the accumulator count
+ * @param[in] value the accumulator value
+ * @param[out] status Error status variable. 0 on success.
+ */
 void HAL_GetDMASampleAnalogAccumulator(const HAL_DMASample* dmaSample,
                                        HAL_AnalogInputHandle aInHandle,
                                        int64_t* count, int64_t* value,
                                        int32_t* status);
 
+/**
+ * Returns the raw duty cycle input ratio data from the sample.
+ *
+ * Use HAL_GetDutyCycleOutputScaleFactor to scale this to a percentage.
+ *
+ * @param[in] dmaSample the sample to read from
+ * @param[in] dutyCycleHandle the duty cycle handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return raw duty cycle input data
+ */
 int32_t HAL_GetDMASampleDutyCycleOutputRaw(const HAL_DMASample* dmaSample,
                                            HAL_DutyCycleHandle dutyCycleHandle,
                                            int32_t* status);
@@ -127,3 +455,4 @@
 #ifdef __cplusplus
 }  // extern "C"
 #endif
+/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h
index 23595db..1839cfc 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStation.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2013-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -30,7 +27,7 @@
  * @param isLVCode  true for a LV error code, false for a standard error code
  * @param details   the details of the error
  * @param location  the file location of the errror
- * @param callstack the callstack of the error
+ * @param callStack the callstack of the error
  * @param printMsg  true to print the error message to stdout as well as to the
  * DS
  */
@@ -50,15 +47,15 @@
  * The control work contains the robot state.
  *
  * @param controlWord the control word (out)
- * @return            the error code, or 0 for success
+ * @return the error code, or 0 for success
  */
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord);
 
 /**
  * Gets the current alliance station ID.
  *
- * @param status the error code, or 0 for success
- * @return       the alliance station ID
+ * @param[out] status the error code, or 0 for success
+ * @return the alliance station ID
  */
 HAL_AllianceStationID HAL_GetAllianceStation(int32_t* status);
 
@@ -67,7 +64,7 @@
  *
  * @param joystickNum the joystick number
  * @param axes        the axes values (output)
- * @return            the error code, or 0 for success
+ * @return the error code, or 0 for success
  */
 int32_t HAL_GetJoystickAxes(int32_t joystickNum, HAL_JoystickAxes* axes);
 
@@ -76,7 +73,7 @@
  *
  * @param joystickNum the joystick number
  * @param povs        the POV values (output)
- * @return            the error code, or 0 for success
+ * @return the error code, or 0 for success
  */
 int32_t HAL_GetJoystickPOVs(int32_t joystickNum, HAL_JoystickPOVs* povs);
 
@@ -85,7 +82,7 @@
  *
  * @param joystickNum the joystick number
  * @param buttons     the button values (output)
- * @return            the error code, or 0 for success
+ * @return the error code, or 0 for success
  */
 int32_t HAL_GetJoystickButtons(int32_t joystickNum,
                                HAL_JoystickButtons* buttons);
@@ -93,14 +90,15 @@
 /**
  * Retrieves the Joystick Descriptor for particular slot.
  *
- * @param desc [out] descriptor (data transfer object) to fill in.  desc is
- * filled in regardless of success. In other words, if descriptor is not
- * available, desc is filled in with default values matching the init-values in
- * Java and C++ Driverstation for when caller requests a too-large joystick
- * index.
- *
+ * @param joystickNum the joystick number
+ * @param[out] desc   descriptor (data transfer object) to fill in. desc is
+ *                    filled in regardless of success. In other words, if
+ *                    descriptor is not available, desc is filled in with
+ *                    default values matching the init-values in Java and C++
+ *                    Driver Station for when caller requests a too-large
+ *                    joystick index.
  * @return error code reported from Network Comm back-end.  Zero is good,
- * nonzero is bad.
+ *         nonzero is bad.
  */
 int32_t HAL_GetJoystickDescriptor(int32_t joystickNum,
                                   HAL_JoystickDescriptor* desc);
@@ -109,7 +107,7 @@
  * Gets is a specific joystick is considered to be an XBox controller.
  *
  * @param joystickNum the joystick number
- * @return            true if xbox, false otherwise
+ * @return true if xbox, false otherwise
  */
 HAL_Bool HAL_GetJoystickIsXbox(int32_t joystickNum);
 
@@ -120,7 +118,7 @@
  * the joystick uses.
  *
  * @param joystickNum the joystick number
- * @return            the enumerated joystick type
+ * @return the enumerated joystick type
  */
 int32_t HAL_GetJoystickType(int32_t joystickNum);
 
@@ -132,7 +130,7 @@
  * Will be null terminated.
  *
  * @param joystickNum the joystick number
- * @return            the joystick name
+ * @return the joystick name
  */
 char* HAL_GetJoystickName(int32_t joystickNum);
 
@@ -151,18 +149,18 @@
  *
  * @param joystickNum the joystick number
  * @param axis        the axis number
- * @return            the enumerated axis type
+ * @return the enumerated axis type
  */
 int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis);
 
 /**
  * Set joystick outputs.
  *
- * @param joystickNum the joystick numer
+ * @param joystickNum the joystick number
  * @param outputs     bitmask of outputs, 1 for on 0 for off
  * @param leftRumble  the left rumble value (0-FFFF)
  * @param rightRumble the right rumble value (0-FFFF)
- * @return            the error code, or 0 for success
+ * @return the error code, or 0 for success
  */
 int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
                                int32_t leftRumble, int32_t rightRumble);
@@ -180,7 +178,7 @@
  * The Practice Match function of the DS approximates the behavior seen on
  * the field.
  *
- * @param status the error code, or 0 for success
+ * @param[out] status the error code, or 0 for success
  * @return time remaining in current match period (auto or teleop)
  */
 double HAL_GetMatchTime(int32_t* status);
@@ -188,8 +186,8 @@
 /**
  * Gets info about a specific match.
  *
- * @param info the match info (output)
- * @return     the error code, or 0 for success
+ * @param[in] info the match info (output)
+ * @return the error code, or 0 for success
  */
 int32_t HAL_GetMatchInfo(HAL_MatchInfo* info);
 
@@ -220,8 +218,8 @@
  * forever. Otherwise, it will wait until either a new packet, or the timeout
  * time has passed.
  *
- * @param timeout timeout in seconds
- * @return        true for new data, false for timeout
+ * @param[in] timeout timeout in seconds
+ * @return true for new data, false for timeout
  */
 HAL_Bool HAL_WaitForDSDataTimeout(double timeout);
 
@@ -243,7 +241,7 @@
  * Sets the disabled flag in the DS.
  *
  * This is used for the DS to ensure the robot is properly responding to its
- * state request. Ensure this get called about every 50ms, or the robot will be
+ * state request. Ensure this gets called about every 50ms, or the robot will be
  * disabled by the DS.
  */
 void HAL_ObserveUserProgramDisabled(void);
@@ -252,7 +250,7 @@
  * Sets the autonomous enabled flag in the DS.
  *
  * This is used for the DS to ensure the robot is properly responding to its
- * state request. Ensure this get called about every 50ms, or the robot will be
+ * state request. Ensure this gets called about every 50ms, or the robot will be
  * disabled by the DS.
  */
 void HAL_ObserveUserProgramAutonomous(void);
@@ -261,7 +259,7 @@
  * Sets the teleoperated enabled flag in the DS.
  *
  * This is used for the DS to ensure the robot is properly responding to its
- * state request. Ensure this get called about every 50ms, or the robot will be
+ * state request. Ensure this gets called about every 50ms, or the robot will be
  * disabled by the DS.
  */
 void HAL_ObserveUserProgramTeleop(void);
@@ -270,7 +268,7 @@
  * Sets the test mode flag in the DS.
  *
  * This is used for the DS to ensure the robot is properly responding to its
- * state request. Ensure this get called about every 50ms, or the robot will be
+ * state request. Ensure this gets called about every 50ms, or the robot will be
  * disabled by the DS.
  */
 void HAL_ObserveUserProgramTest(void);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h
index 7c5d6f6..21f9088 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DriverStationTypes.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/DutyCycle.h b/third_party/allwpilib/hal/src/main/native/include/hal/DutyCycle.h
index 357d8f3..05b654b 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/DutyCycle.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/DutyCycle.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -23,10 +20,12 @@
 /**
  * Initialize a DutyCycle input.
  *
- * @param digitalSourceHandle the digital source to use (either a
- * HAL_DigitalHandle or a HAL_AnalogTriggerHandle)
- * @param triggerType the analog trigger type of the source if it is
- * an analog trigger
+ * @param[in] digitalSourceHandle the digital source to use (either a
+ *                                 HAL_DigitalHandle or a
+ *                                 HAL_AnalogTriggerHandle)
+ * @param[in] triggerType the analog trigger type of the source if it is
+ *                         an analog trigger
+ * @param[out] status Error status variable. 0 on success.
  * @return thre created duty cycle handle
  */
 HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
@@ -52,7 +51,8 @@
 /**
  * Get the frequency of the duty cycle signal.
  *
- * @param dutyCycleHandle the duty cycle handle
+ * @param[in] dutyCycleHandle the duty cycle handle
+ * @param[out] status Error status variable. 0 on success.
  * @return frequency in Hertz
  */
 int32_t HAL_GetDutyCycleFrequency(HAL_DutyCycleHandle dutyCycleHandle,
@@ -63,7 +63,8 @@
  *
  * <p> 0 means always low, 1 means always high.
  *
- * @param dutyCycleHandle the duty cycle handle
+ * @param[in] dutyCycleHandle the duty cycle handle
+ * @param[out] status Error status variable. 0 on success.
  * @return output ratio between 0 and 1
  */
 double HAL_GetDutyCycleOutput(HAL_DutyCycleHandle dutyCycleHandle,
@@ -75,7 +76,8 @@
  * <p> 0 means always low, an output equal to
  * GetOutputScaleFactor() means always high.
  *
- * @param dutyCycleHandle the duty cycle handle
+ * @param[in] dutyCycleHandle the duty cycle handle
+ * @param[out] status Error status variable. 0 on success.
  * @return output ratio in raw units
  */
 int32_t HAL_GetDutyCycleOutputRaw(HAL_DutyCycleHandle dutyCycleHandle,
@@ -88,7 +90,8 @@
  * down to 0. Divide the result of getOutputRaw by this in order to get the
  * percentage between 0 and 1.
  *
- * @param dutyCycleHandle the duty cycle handle
+ * @param[in] dutyCycleHandle the duty cycle handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the output scale factor
  */
 int32_t HAL_GetDutyCycleOutputScaleFactor(HAL_DutyCycleHandle dutyCycleHandle,
@@ -97,7 +100,8 @@
 /**
  * Get the FPGA index for the DutyCycle.
  *
- * @param dutyCycleHandle the duty cycle handle
+ * @param[in] dutyCycleHandle the duty cycle handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the FPGA index
  */
 int32_t HAL_GetDutyCycleFPGAIndex(HAL_DutyCycleHandle dutyCycleHandle,
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h b/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h
index 9d2b5d0..a6bf4af 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Encoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -46,18 +43,19 @@
 /**
  * Initializes an encoder.
  *
- * @param digitalSourceHandleA the A source (either a HAL_DigitalHandle or a
- * HAL_AnalogTriggerHandle)
- * @param analogTriggerTypeA   the analog trigger type of the A source if it is
- * an analog trigger
- * @param digitalSourceHandleB the B source (either a HAL_DigitalHandle or a
- * HAL_AnalogTriggerHandle)
- * @param analogTriggerTypeB   the analog trigger type of the B source if it is
- * an analog trigger
- * @param reverseDirection     true to reverse the counting direction from
- * standard, otherwise false
- * @param encodingType         the encoding type
-   @return                     the created encoder handle
+ * @param[in] digitalSourceHandleA the A source (either a HAL_DigitalHandle or a
+ *                                 HAL_AnalogTriggerHandle)
+ * @param[in] analogTriggerTypeA   the analog trigger type of the A source if it
+ *                                 is an analog trigger
+ * @param[in] digitalSourceHandleB the B source (either a HAL_DigitalHandle or a
+ *                                 HAL_AnalogTriggerHandle)
+ * @param[in] analogTriggerTypeB   the analog trigger type of the B source if it
+ *                                 is an analog trigger
+ * @param[in] reverseDirection     true to reverse the counting direction from
+ *                                 standard, otherwise false
+ * @param[in] encodingType         the encoding type
+ * @param[out] status              Error status variable. 0 on success.
+ * @return the created encoder handle
  */
 HAL_EncoderHandle HAL_InitializeEncoder(
     HAL_Handle digitalSourceHandleA, HAL_AnalogTriggerType analogTriggerTypeA,
@@ -68,7 +66,8 @@
 /**
  * Frees an encoder.
  *
- * @param encoderHandle the encoder handle
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
 
@@ -86,7 +85,8 @@
  *
  * This is scaled by the value passed duing initialization to encodingType.
  *
- * @param encoderHandle the encoder handle
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status Error status variable. 0 on success.
  * @return the current scaled count
  */
 int32_t HAL_GetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
@@ -96,8 +96,9 @@
  *
  * This is not scaled by any values.
  *
- * @param encoderHandle the encoder handle
- * @return              the raw encoder count
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the raw encoder count
  */
 int32_t HAL_GetEncoderRaw(HAL_EncoderHandle encoderHandle, int32_t* status);
 
@@ -106,8 +107,9 @@
  *
  * This is set by the value passed during initialization to encodingType.
  *
- * @param encoderHandle the encoder handle
- * @return              the encoder scale value
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the encoder scale value
  */
 int32_t HAL_GetEncoderEncodingScale(HAL_EncoderHandle encoderHandle,
                                     int32_t* status);
@@ -118,8 +120,8 @@
  * Read the value at this instant. It may still be running, so it reflects the
  * current value. Next time it is read, it might have a different value.
  *
- * @param encoderHandle the encoder handle
- * @return              the current encoder value
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_ResetEncoder(HAL_EncoderHandle encoderHandle, int32_t* status);
 
@@ -129,8 +131,9 @@
  * Returns the time interval of the most recent count. This can be used for
  * velocity calculations to determine shaft speed.
  *
- * @param encoderHandle the encoder handle
- * @returns             the period of the last two pulses in units of seconds
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @returns the period of the last two pulses in units of seconds
  */
 double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status);
 
@@ -141,9 +144,10 @@
  * used to determine the "stopped" state of the encoder using the
  * HAL_GetEncoderStopped method.
  *
- * @param encoderHandle the encoder handle
- * @param maxPeriod     the maximum period where the counted device is
- * considered moving in seconds
+ * @param[in] encoderHandle the encoder handle
+ * @param[in] maxPeriod     the maximum period where the counted device is
+ *                          considered moving in seconds
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetEncoderMaxPeriod(HAL_EncoderHandle encoderHandle, double maxPeriod,
                              int32_t* status);
@@ -155,9 +159,10 @@
  * using the SetMaxPeriod method. If the clock exceeds the MaxPeriod, then the
  * device (and encoder) are assumed to be stopped and it returns true.
  *
- * @param encoderHandle the encoder handle
- * @return              true if the most recent encoder period exceeds the
- * MaxPeriod value set by SetMaxPeriod
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return true if the most recent encoder period exceeds the MaxPeriod value
+ *         set by SetMaxPeriod
  */
 HAL_Bool HAL_GetEncoderStopped(HAL_EncoderHandle encoderHandle,
                                int32_t* status);
@@ -165,8 +170,9 @@
 /**
  * Gets the last direction the encoder value changed.
  *
- * @param encoderHandle the encoder handle
- * @return              the last direction the encoder value changed
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the last direction the encoder value changed
  */
 HAL_Bool HAL_GetEncoderDirection(HAL_EncoderHandle encoderHandle,
                                  int32_t* status);
@@ -177,9 +183,10 @@
  * This is the encoder count scaled by the distance per pulse set for the
  * encoder.
  *
- * @param encoderHandle the encoder handle
- * @return              the encoder distance (units are determined by the units
- * passed to HAL_SetEncoderDistancePerPulse)
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the encoder distance (units are determined by the units
+ *                      passed to HAL_SetEncoderDistancePerPulse)
  */
 double HAL_GetEncoderDistance(HAL_EncoderHandle encoderHandle, int32_t* status);
 
@@ -189,9 +196,10 @@
  * This is the encoder period scaled by the distance per pulse set for the
  * encoder.
  *
- * @param encoderHandle the encoder handle
- * @return              the encoder rate (units are determined by the units
- * passed to HAL_SetEncoderDistancePerPulse, time value is seconds)
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the encoder rate (units are determined by the units passed to
+ *         HAL_SetEncoderDistancePerPulse, time value is seconds)
  */
 double HAL_GetEncoderRate(HAL_EncoderHandle encoderHandle, int32_t* status);
 
@@ -201,10 +209,12 @@
  * Units need to match what is set by HAL_SetEncoderDistancePerPulse, with time
  * as seconds.
  *
- * @param encoderHandle the encoder handle
- * @param minRate       the minimum rate to be considered moving (units are
- * determined by the units passed to HAL_SetEncoderDistancePerPulse, time value
- * is seconds)
+ * @param[in] encoderHandle the encoder handle
+ * @param[in] minRate       the minimum rate to be considered moving (units are
+ *                          determined by the units passed to
+ *                          HAL_SetEncoderDistancePerPulse, time value is
+ *                          seconds)
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetEncoderMinRate(HAL_EncoderHandle encoderHandle, double minRate,
                            int32_t* status);
@@ -213,9 +223,10 @@
  * Sets the distance traveled per encoder pulse. This is used as a scaling
  * factor for the rate and distance calls.
  *
- * @param encoderHandle    the encoder handle
- * @param distancePerPulse the distance traveled per encoder pulse (units user
- * defined)
+ * @param[in] encoderHandle    the encoder handle
+ * @param[in] distancePerPulse the distance traveled per encoder pulse (units
+ *                             user defined)
+ * @param[out] status          Error status variable. 0 on success.
  */
 void HAL_SetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
                                     double distancePerPulse, int32_t* status);
@@ -225,8 +236,9 @@
  *
  * Note that this is not a toggle. It is an absolute set.
  *
- * @param encoderHandle    the encoder handle
- * @param reverseDirection true to reverse the direction, false to not.
+ * @param[in] encoderHandle    the encoder handle
+ * @param[in] reverseDirection true to reverse the direction, false to not.
+ * @param[out] status          Error status variable. 0 on success.
  */
 void HAL_SetEncoderReverseDirection(HAL_EncoderHandle encoderHandle,
                                     HAL_Bool reverseDirection, int32_t* status);
@@ -234,8 +246,9 @@
 /**
  * Sets the number of encoder samples to average when calculating encoder rate.
  *
- * @param encoderHandle    the encoder handle
- * @param samplesToAverage the number of samples to average
+ * @param[in] encoderHandle    the encoder handle
+ * @param[in] samplesToAverage the number of samples to average
+ * @param[out] status          Error status variable. 0 on success.
  */
 void HAL_SetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
                                     int32_t samplesToAverage, int32_t* status);
@@ -243,8 +256,9 @@
 /**
  * Gets the current samples to average value.
  *
- * @param encoderHandle the encoder handle
- * @return              the current samples to average value
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the current samples to average value
  */
 int32_t HAL_GetEncoderSamplesToAverage(HAL_EncoderHandle encoderHandle,
                                        int32_t* status);
@@ -255,12 +269,14 @@
  * The index pulse can be used to cause an encoder to reset based on an external
  * input.
  *
- * @param encoderHandle       the encoder handle
- * @param digitalSourceHandle the index source handle (either a
- * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
- * @param analogTriggerType   the analog trigger type if the source is an analog
- * trigger
- * @param type                the index triggering type
+ * @param[in] encoderHandle       the encoder handle
+ * @param[in] digitalSourceHandle the index source handle (either a
+ *                                HAL_AnalogTriggerHandle or a
+ *                                HAL_DigitalHandle)
+ * @param[in] analogTriggerType   the analog trigger type if the source is an
+ *                                analog trigger
+ * @param[in] type                the index triggering type
+ * @param[out] status             Error status variable. 0 on success.
  */
 void HAL_SetEncoderIndexSource(HAL_EncoderHandle encoderHandle,
                                HAL_Handle digitalSourceHandle,
@@ -270,8 +286,9 @@
 /**
  * Gets the FPGA index of the encoder.
  *
- * @param encoderHandle the encoder handle
- * @return              the FPGA index of the encoder
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the FPGA index of the encoder
  */
 int32_t HAL_GetEncoderFPGAIndex(HAL_EncoderHandle encoderHandle,
                                 int32_t* status);
@@ -281,8 +298,9 @@
  *
  * This is used to perform the scaling from raw to type scaled values.
  *
- * @param encoderHandle the encoder handle
- * @return              the scale value for the encoder
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the scale value for the encoder
  */
 double HAL_GetEncoderDecodingScaleFactor(HAL_EncoderHandle encoderHandle,
                                          int32_t* status);
@@ -290,8 +308,9 @@
 /**
  * Gets the user set distance per pulse of the encoder.
  *
- * @param encoderHandle the encoder handle
- * @return              the set distance per pulse
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the set distance per pulse
  */
 double HAL_GetEncoderDistancePerPulse(HAL_EncoderHandle encoderHandle,
                                       int32_t* status);
@@ -299,8 +318,9 @@
 /**
  * Gets the encoding type of the encoder.
  *
- * @param encoderHandle the encoder handle
- * @return              the encoding type
+ * @param[in] encoderHandle the encoder handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the encoding type
  */
 HAL_EncoderEncodingType HAL_GetEncoderEncodingType(
     HAL_EncoderHandle encoderHandle, int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Errors.h b/third_party/allwpilib/hal/src/main/native/include/hal/Errors.h
index 9f74f8c..b24bfb7 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Errors.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Errors.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -44,6 +41,8 @@
 #define ERR_FRCSystem_NoDSConnection_MESSAGE \
   "FRCSystem: No driver station connected"
 
+#define HAL_SUCCESS 0
+
 #define SAMPLE_RATE_TOO_HIGH 1001
 #define SAMPLE_RATE_TOO_HIGH_MESSAGE \
   "HAL: Analog module sample rate is too high"
@@ -102,6 +101,10 @@
 #define HAL_INVALID_DMA_ADDITION_MESSAGE \
   "HAL_AddDMA() only works before HAL_StartDMA()"
 
+#define HAL_INVALID_DMA_STATE -1103
+#define HAL_INVALID_DMA_STATE_MESSAGE \
+  "HAL_SetPause() only works before HAL_StartDMA()"
+
 #define HAL_SERIAL_PORT_NOT_FOUND -1123
 #define HAL_SERIAL_PORT_NOT_FOUND_MESSAGE \
   "HAL: The specified serial port device was not found"
@@ -116,7 +119,7 @@
 
 #define HAL_THREAD_PRIORITY_ERROR -1152
 #define HAL_THREAD_PRIORITY_ERROR_MESSAGE \
-  "HAL: Getting or setting the priority of a thread has failed";
+  "HAL: Getting or setting the priority of a thread has failed"
 
 #define HAL_THREAD_PRIORITY_RANGE_ERROR -1153
 #define HAL_THREAD_PRIORITY_RANGE_ERROR_MESSAGE \
@@ -128,11 +131,20 @@
 #define HAL_SIM_NOT_SUPPORTED -1155
 #define HAL_SIM_NOT_SUPPORTED_MESSAGE "HAL: Method not supported in sim"
 
+#define HAL_USE_LAST_ERROR -1156
+#define HAL_USE_LAST_ERROR_MESSAGE \
+  "HAL: Use HAL_GetLastError(status) to get last error"
+
+#define HAL_CONSOLE_OUT_ENABLED_ERROR -1157
+#define HAL_CONSOLE_OUT_ENABLED_ERROR_MESSAGE                           \
+  "HAL: Onboard serial port is requested, but Console Out is enabled. " \
+  "Disable Console Out using imaging tool"
+
 #define HAL_CAN_BUFFER_OVERRUN -35007
 #define HAL_CAN_BUFFER_OVERRUN_MESSAGE \
   "HAL: CAN Output Buffer Full. Ensure a device is attached"
 
-#define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error";
+#define VI_ERROR_SYSTEM_ERROR_MESSAGE "HAL - VISA: System Error"
 #define VI_ERROR_INV_OBJECT_MESSAGE "HAL - VISA: Invalid Object"
 #define VI_ERROR_RSRC_LOCKED_MESSAGE "HAL - VISA: Resource Locked"
 #define VI_ERROR_RSRC_NFOUND_MESSAGE "HAL - VISA: Resource Not Found"
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Extensions.h b/third_party/allwpilib/hal/src/main/native/include/hal/Extensions.h
index 13de7f8..ad3f733 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Extensions.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Extensions.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -35,7 +32,7 @@
  * Expected to be called internally, not by users.
  *
  * @param library the library path
- * @return        the succes state of the initialization
+ * @return the succes state of the initialization
  */
 int HAL_LoadOneExtension(const char* library);
 
@@ -43,7 +40,7 @@
  * Loads any extra halsim libraries provided in the HALSIM_EXTENSIONS
  * environment variable.
  *
- * @return        the succes state of the initialization
+ * @return the succes state of the initialization
  */
 int HAL_LoadExtensions(void);
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h b/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h
index cf9b2f8..0e1294b 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/HAL.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2013-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -16,7 +13,7 @@
 #include "hal/AnalogTrigger.h"
 #include "hal/CAN.h"
 #include "hal/CANAPI.h"
-#include "hal/Compressor.h"
+#include "hal/CTREPCM.h"
 #include "hal/Constants.h"
 #include "hal/Counter.h"
 #include "hal/DIO.h"
@@ -29,14 +26,12 @@
 #include "hal/Interrupts.h"
 #include "hal/Main.h"
 #include "hal/Notifier.h"
-#include "hal/PDP.h"
 #include "hal/PWM.h"
 #include "hal/Ports.h"
 #include "hal/Power.h"
 #include "hal/Relay.h"
 #include "hal/SPI.h"
 #include "hal/SimDevice.h"
-#include "hal/Solenoid.h"
 #include "hal/Threads.h"
 #include "hal/Types.h"
 #include "hal/Value.h"
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h b/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h
index ee5b054..b31ec75 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/HALBase.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
  */
 
 // clang-format off
-HAL_ENUM(HAL_RuntimeType) { HAL_Athena, HAL_Mock };
+HAL_ENUM(HAL_RuntimeType) { HAL_Runtime_RoboRIO, HAL_Runtime_RoboRIO2, HAL_Runtime_Simulation };
 // clang-format on
 
 #ifdef __cplusplus
@@ -26,10 +23,23 @@
 #endif
 
 /**
+ * Gets the last error set on this thread, or the message for the status code.
+ *
+ * If passed HAL_USE_LAST_ERROR, the last error set on the thread will be
+ * returned.
+ *
+ * @param[out] status the status code, set to the error status code if input is
+ *               HAL_USE_LAST_ERROR
+ * @return the error message for the code. This does not need to be freed,
+ *               but can be overwritten by another hal call on the same thread.
+ */
+const char* HAL_GetLastError(int32_t* status);
+
+/**
  * Gets the error message for a specific status code.
  *
  * @param code the status code
- * @return     the error message for the code. This does not need to be freed.
+ * @return the error message for the code. This does not need to be freed.
  */
 const char* HAL_GetErrorMessage(int32_t code);
 
@@ -38,6 +48,7 @@
  *
  * For now, expect this to be competition year.
  *
+ * @param[out] status the error code, or 0 for success
  * @return FPGA Version number.
  */
 int32_t HAL_GetFPGAVersion(int32_t* status);
@@ -50,15 +61,22 @@
  * the next 8 bits are the Minor Revision.
  * The 12 least significant bits are the Build Number.
  *
+ * @param[out] status the error code, or 0 for success
  * @return FPGA Revision number.
  */
 int64_t HAL_GetFPGARevision(int32_t* status);
 
+/**
+ * Returns the runtime type of the HAL.
+ *
+ * @return HAL Runtime Type
+ */
 HAL_RuntimeType HAL_GetRuntimeType(void);
 
 /**
  * Gets the state of the "USER" button on the roboRIO.
  *
+ * @param[out] status the error code, or 0 for success
  * @return true if the button is currently pressed down
  */
 HAL_Bool HAL_GetFPGAButton(int32_t* status);
@@ -66,6 +84,7 @@
 /**
  * Gets if the system outputs are currently active
  *
+ * @param[out] status the error code, or 0 for success
  * @return true if the system outputs are active, false if disabled
  */
 HAL_Bool HAL_GetSystemActive(int32_t* status);
@@ -73,6 +92,7 @@
 /**
  * Gets if the system is in a browned out state.
  *
+ * @param[out] status the error code, or 0 for success
  * @return true if the system is in a low voltage brown out, false otherwise
  */
 HAL_Bool HAL_GetBrownedOut(int32_t* status);
@@ -83,7 +103,7 @@
  * The created handle does not need to be freed.
  *
  * @param channel the channel number
- * @return        the created port
+ * @return the created port
  */
 HAL_PortHandle HAL_GetPort(int32_t channel);
 
@@ -97,13 +117,14 @@
  *
  * @param module  the module number
  * @param channel the channel number
- * @return        the created port
+ * @return the created port
  */
 HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel);
 
 /**
  * Reads the microsecond-resolution timer on the FPGA.
  *
+ * @param[out] status the error code, or 0 for success
  * @return The current time in microseconds according to the FPGA (since FPGA
  * reset).
  */
@@ -118,10 +139,12 @@
  * bottom 32 bits of the timestamp and expanding it, you will be off by
  * multiples of 1<<32 microseconds.
  *
+ * @param[in] unexpandedLower 32 bit FPGA time
+ * @param[out] status the error code, or 0 for success
  * @return The current time in microseconds according to the FPGA (since FPGA
- * reset) as a 64 bit number.
+ *         reset) as a 64 bit number.
  */
-uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status);
+uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status);
 
 /**
  * Call this to start up HAL. This is required for robot programs.
@@ -145,7 +168,7 @@
  *
  * @param timeout the initialization timeout (ms)
  * @param mode    the initialization mode (see remarks)
- * @return        true if initialization was successful, otherwise false.
+ * @return true if initialization was successful, otherwise false.
  */
 HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode);
 
@@ -157,6 +180,18 @@
  */
 void HAL_Shutdown(void);
 
+/**
+ * Calls registered SimPeriodic "before" callbacks (only in simulation mode).
+ * This should be called prior to user code periodic simulation functions.
+ */
+void HAL_SimPeriodicBefore(void);
+
+/**
+ * Calls registered SimPeriodic "after" callbacks (only in simulation mode).
+ * This should be called after user code periodic simulation functions.
+ */
+void HAL_SimPeriodicAfter(void);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/I2C.h b/third_party/allwpilib/hal/src/main/native/include/hal/I2C.h
index 4147cbb..4e5c007 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/I2C.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/I2C.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -27,7 +24,8 @@
  * Opens the port if necessary and saves the handle.
  * If opening the MXP port, also sets up the channel functions appropriately.
  *
- * @param port The port to open, 0 for the on-board, 1 for the MXP.
+ * @param[in] port    The port to open, 0 for the on-board, 1 for the MXP.
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status);
 
@@ -38,6 +36,8 @@
  * over each transaction.
  *
  * @param port The I2C port, 0 for the on-board, 1 for the MXP.
+ * @param deviceAddress The address of the register on the device to be
+ *                      read/written.
  * @param dataToSend Buffer of data to send as part of the transaction.
  * @param sendSize Number of bytes to send as part of the transaction.
  * @param dataReceived Buffer to read data into.
@@ -55,9 +55,10 @@
  *   transaction is complete.
  *
  * @param port The I2C port, 0 for the on-board, 1 for the MXP.
- * @param registerAddress The address of the register on the device to be
- * written.
- * @param data The byte to write to the register on the device.
+ * @param deviceAddress The address of the register on the device to be
+ *                      written.
+ * @param dataToSend The byte to write to the register on the device.
+ * @param sendSize Number of bytes to send.
  * @return >= 0 on success or -1 on transfer abort.
  */
 int32_t HAL_WriteI2C(HAL_I2CPort port, int32_t deviceAddress,
@@ -71,7 +72,7 @@
  *   you to read consecutive registers on a device in a single transaction.
  *
  * @param port The I2C port, 0 for the on-board, 1 for the MXP.
- * @param registerAddress The register to read first in the transaction.
+ * @param deviceAddress The register to read first in the transaction.
  * @param count The number of bytes to read in the transaction.
  * @param buffer A pointer to the array of bytes to store the data read from the
  * device.
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h
index b5e8235..6aac0fb 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/I2CTypes.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Interrupts.h b/third_party/allwpilib/hal/src/main/native/include/hal/Interrupts.h
index ff68d48..def800c 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Interrupts.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Interrupts.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -22,67 +19,45 @@
 extern "C" {
 #endif
 
-typedef void (*HAL_InterruptHandlerFunction)(uint32_t interruptAssertedMask,
-                                             void* param);
-
 /**
  * Initializes an interrupt.
  *
- * @param watcher true for synchronous interrupts, false for asynchronous
- * @return        the created interrupt handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the created interrupt handle
  */
-HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher, int32_t* status);
+HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status);
 
 /**
  * Frees an interrupt.
  *
  * @param interruptHandle the interrupt handle
- * @return                the param passed to the interrupt, or nullptr if one
- * wasn't passed.
  */
-void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle, int32_t* status);
+void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle);
 
 /**
  * In synchronous mode, waits for the defined interrupt to occur.
  *
- * @param interruptHandle the interrupt handle
- * @param timeout        timeout in seconds
- * @param ignorePrevious if true, ignore interrupts that happened before
- * waitForInterrupt was called
- * @return               the mask of interrupts that fired
+ * @param[in] interruptHandle the interrupt handle
+ * @param[in] timeout         timeout in seconds
+ * @param[in] ignorePrevious  if true, ignore interrupts that happened before
+ *                            waitForInterrupt was called
+ * @param[out] status         Error status variable. 0 on success.
+ * @return the mask of interrupts that fired
  */
 int64_t HAL_WaitForInterrupt(HAL_InterruptHandle interruptHandle,
                              double timeout, HAL_Bool ignorePrevious,
                              int32_t* status);
 
 /**
- * Enables interrupts to occur on this input.
- *
- * Interrupts are disabled when the RequestInterrupt call is made. This gives
- * time to do the setup of the other options before starting to field
- * interrupts.
- *
- * @param interruptHandle the interrupt handle
- */
-void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle, int32_t* status);
-
-/**
- * Disables interrupts without without deallocating structures.
- *
- * @param interruptHandle the interrupt handle
- */
-void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
-                           int32_t* status);
-
-/**
  * Returns the timestamp for the rising interrupt that occurred most recently.
  *
  * This is in the same time domain as HAL_GetFPGATime().  It only contains the
  * bottom 32 bits of the timestamp.  If your robot has been running for over 1
  * hour, you will need to fill in the upper 32 bits yourself.
  *
- * @param interruptHandle the interrupt handle
- * @return                timestamp in microseconds since FPGA Initialization
+ * @param[in] interruptHandle the interrupt handle
+ * @param[out] status         Error status variable. 0 on success.
+ * @return timestamp in microseconds since FPGA Initialization
  */
 int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
                                          int32_t* status);
@@ -94,8 +69,9 @@
  * bottom 32 bits of the timestamp.  If your robot has been running for over 1
  * hour, you will need to fill in the upper 32 bits yourself.
  *
- * @param interruptHandle the interrupt handle
- * @return                timestamp in microseconds since FPGA Initialization
+ * @param[in] interruptHandle the interrupt handle
+ * @param[out] status         Error status variable. 0 on success.
+ * @return timestamp in microseconds since FPGA Initialization
  */
 int64_t HAL_ReadInterruptFallingTimestamp(HAL_InterruptHandle interruptHandle,
                                           int32_t* status);
@@ -103,10 +79,13 @@
 /**
  * Requests interrupts on a specific digital source.
  *
- * @param interruptHandle     the interrupt handle
- * @param digitalSourceHandle the digital source handle (either a
- * HAL_AnalogTriggerHandle of a HAL_DigitalHandle)
- * @param analogTriggerType   the trigger type if the source is an AnalogTrigger
+ * @param[in] interruptHandle     the interrupt handle
+ * @param[in] digitalSourceHandle the digital source handle (either a
+ *                                HAL_AnalogTriggerHandle or a
+ *                                HAL_DigitalHandle)
+ * @param[in] analogTriggerType   the trigger type if the source is an
+ *                                AnalogTrigger
+ * @param[out] status             Error status variable. 0 on success.
  */
 void HAL_RequestInterrupts(HAL_InterruptHandle interruptHandle,
                            HAL_Handle digitalSourceHandle,
@@ -114,41 +93,14 @@
                            int32_t* status);
 
 /**
- * Attaches an asynchronous interrupt handler to the interrupt.
- *
- * This interrupt gets called directly on the FPGA interrupt thread, so will
- * block other interrupts while running.
- *
- * @param interruptHandle the interrupt handle
- * @param handler         the handler function for the interrupt to call
- * @param param           a parameter to be passed to the handler
- */
-void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle,
-                                HAL_InterruptHandlerFunction handler,
-                                void* param, int32_t* status);
-
-/**
- * Attaches an asynchronous interrupt handler to the interrupt.
- *
- * This interrupt gets called on a thread specific to the interrupt, so will not
- * block other interrupts.
- *
- * @param interruptHandle the interrupt handle
- * @param handler         the handler function for the interrupt to call
- * @param param           a parameter to be passed to the handler
- */
-void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interruptHandle,
-                                        HAL_InterruptHandlerFunction handler,
-                                        void* param, int32_t* status);
-
-/**
  * Sets the edges to trigger the interrupt on.
  *
  * Note that both edges triggered is a valid configuration.
  *
- * @param interruptHandle the interrupt handle
- * @param risingEdge      true for triggering on rising edge
- * @param fallingEdge     true for triggering on falling edge
+ * @param[in] interruptHandle the interrupt handle
+ * @param[in] risingEdge      true for triggering on rising edge
+ * @param[in] fallingEdge     true for triggering on falling edge
+ * @param[out] status         Error status variable. 0 on success.
  */
 void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
                                   HAL_Bool risingEdge, HAL_Bool fallingEdge,
@@ -159,7 +111,8 @@
  *
  * This will release both rising and falling waiters.
  *
- * @param interruptHandle the interrupt handle to release
+ * @param[in] interruptHandle the interrupt handle to release
+ * @param[out] status         Error status variable. 0 on success.
  */
 void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
                                  int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Main.h b/third_party/allwpilib/hal/src/main/native/include/hal/Main.h
index 097f819..712ad01 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Main.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Main.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Notifier.h b/third_party/allwpilib/hal/src/main/native/include/hal/Notifier.h
index 6b8e39f..96452f4 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Notifier.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Notifier.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
+#include <wpi/nodiscard.h>
+
 #include "hal/Types.h"
 
 /**
@@ -27,15 +26,36 @@
  * A notifier is an FPGA controller timer that triggers at requested intervals
  * based on the FPGA time. This can be used to make precise control loops.
  *
+ * @param[out] status Error status variable. 0 on success.
  * @return the created notifier
  */
 HAL_NotifierHandle HAL_InitializeNotifier(int32_t* status);
 
 /**
+ * Sets the HAL notifier thread priority.
+ *
+ * The HAL notifier thread is responsible for managing the FPGA's notifier
+ * interrupt and waking up user's Notifiers when it's their time to run.
+ * Giving the HAL notifier thread real-time priority helps ensure the user's
+ * real-time Notifiers, if any, are notified to run in a timely manner.
+ *
+ * @param[in] realTime Set to true to set a real-time priority, false for
+ *                     standard priority.
+ * @param[in] priority Priority to set the thread to. For real-time, this is
+ *                     1-99 with 99 being highest. For non-real-time, this is
+ *                     forced to 0. See "man 7 sched" for more details.
+ * @param[out] status  Error status variable. 0 on success.
+ * @return True on success.
+ */
+HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
+                                       int32_t* status);
+
+/**
  * Sets the name of a notifier.
  *
- * @param notifierHandle the notifier handle
- * @param name name
+ * @param[in] notifierHandle the notifier handle
+ * @param[in] name name
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
                          int32_t* status);
@@ -45,7 +65,8 @@
  *
  * This will cause any call into HAL_WaitForNotifierAlarm to return.
  *
- * @param notifierHandle the notifier handle
+ * @param[in] notifierHandle the notifier handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status);
 
@@ -54,7 +75,8 @@
  *
  * Note this also stops a notifier if it is already running.
  *
- * @param notifierHandle the notifier handle
+ * @param[in] notifierHandle the notifier handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status);
 
@@ -63,8 +85,9 @@
  *
  * Note that this time is an absolute time relative to HAL_GetFPGATime()
  *
- * @param notifierHandle the notifier handle
- * @param triggerTime    the updated trigger time
+ * @param[in] notifierHandle the notifier handle
+ * @param[in] triggerTime    the updated trigger time
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              uint64_t triggerTime, int32_t* status);
@@ -74,7 +97,8 @@
  *
  * This does not cause HAL_WaitForNotifierAlarm to return.
  *
- * @param notifierHandle the notifier handle
+ * @param[in] notifierHandle the notifier handle
+ * @param[out] status Error status variable. 0 on success.
  */
 void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              int32_t* status);
@@ -83,11 +107,15 @@
  * Waits for the next alarm for the specific notifier.
  *
  * This is a blocking call until either the time elapses or HAL_StopNotifier
- * gets called.
+ * gets called. If the latter occurs, this function will return zero and any
+ * loops using this function should exit. Failing to do so can lead to
+ * use-after-frees.
  *
- * @param notifierHandle the notifier handle
- * @return               the FPGA time the notifier returned
+ * @param[in] notifierHandle the notifier handle
+ * @param[out] status        Error status variable. 0 on success.
+ * @return the FPGA time the notifier returned
  */
+WPI_NODISCARD
 uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
                                   int32_t* status);
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/PDP.h b/third_party/allwpilib/hal/src/main/native/include/hal/PDP.h
deleted file mode 100644
index c80e8b6..0000000
--- a/third_party/allwpilib/hal/src/main/native/include/hal/PDP.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <stdint.h>
-
-#include "hal/Types.h"
-
-/**
- * @defgroup hal_pdp PDP Functions
- * @ingroup hal_capi
- * Functions to control the Power Distribution Panel.
- * @{
- */
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/**
- * Initializes a Power Distribution Panel.
- *
- * @param  module the module number to initialize
- * @return the created PDP
- */
-HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status);
-
-/**
- * Cleans a PDP module.
- *
- * @param handle the module handle
- */
-void HAL_CleanPDP(HAL_PDPHandle handle);
-
-/**
- * Checks if a PDP channel is valid.
- *
- * @param channel the channel to check
- * @return        true if the channel is valid, otherwise false
- */
-HAL_Bool HAL_CheckPDPChannel(int32_t channel);
-
-/**
- * Checks if a PDP module is valid.
- *
- * @param channel the module to check
- * @return        true if the module is valid, otherwise false
- */
-HAL_Bool HAL_CheckPDPModule(int32_t module);
-
-/**
- * Gets the temperature of the PDP.
- *
- * @param handle the module handle
- * @return       the module temperature (celsius)
- */
-double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status);
-
-/**
- * Gets the PDP input voltage.
- *
- * @param handle the module handle
- * @return       the input voltage (volts)
- */
-double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status);
-
-/**
- * Gets the current of a specific PDP channel.
- *
- * @param module  the module
- * @param channel the channel
- * @return        the channel current (amps)
- */
-double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
-                                int32_t* status);
-
-/**
- * Gets the current of all 16 channels on the PDP.
- *
- * The array must be large enough to hold all channels.
- *
- * @param handle the module handle
- * @param current the currents (output)
- */
-void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
-                                  int32_t* status);
-
-/**
- * Gets the total current of the PDP.
- *
- * @param handle the module handle
- * @return       the total current (amps)
- */
-double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status);
-
-/**
- * Gets the total power of the PDP.
- *
- * @param handle the module handle
- * @return       the total power (watts)
- */
-double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status);
-
-/**
- * Gets the total energy of the PDP.
- *
- * @param handle the module handle
- * @return       the total energy (joules)
- */
-double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
-
-/**
- * Resets the PDP accumulated energy.
- *
- * @param handle the module handle
- */
-void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status);
-
-/**
- * Clears any PDP sticky faults.
- *
- * @param handle the module handle
- */
-void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status);
-#ifdef __cplusplus
-}  // extern "C"
-#endif
-/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/PWM.h b/third_party/allwpilib/hal/src/main/native/include/hal/PWM.h
index 7267823..7fd125e 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/PWM.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/PWM.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,16 +21,21 @@
 /**
  * Initializes a PWM port.
  *
- * @param portHandle the port to initialize
- * @return           the created pwm handle
+ * @param[in] portHandle the port to initialize
+ * @param[in] allocationLocation  the location where the allocation is occuring
+ *                                (can be null)
+ * @param[out] status             Error status variable. 0 on success.
+ * @return the created pwm handle
  */
 HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+                                        const char* allocationLocation,
                                         int32_t* status);
 
 /**
  * Frees a PWM port.
  *
- * @param pwmPortHandle the pwm handle
+ * @param[in] pwmPortHandle the pwm handle
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status);
 
@@ -41,7 +43,7 @@
  * Checks if a pwm channel is valid.
  *
  * @param channel the channel to check
- * @return        true if the channel is valid, otherwise false
+ * @return true if the channel is valid, otherwise false
  */
 HAL_Bool HAL_CheckPWMChannel(int32_t channel);
 
@@ -50,12 +52,13 @@
  *
  * All values are in milliseconds.
  *
- * @param pwmPortHandle  the PWM handle
- * @param maxPwm         the maximum PWM value
- * @param deadbandMaxPwm the high range of the center deadband
- * @param centerPwm      the center PWM value
- * @param deadbandMinPwm the low range of the center deadband
- * @param minPwm         the minimum PWM value
+ * @param[in] pwmPortHandle  the PWM handle
+ * @param[in] maxPwm         the maximum PWM value
+ * @param[in] deadbandMaxPwm the high range of the center deadband
+ * @param[in] centerPwm      the center PWM value
+ * @param[in] deadbandMinPwm the low range of the center deadband
+ * @param[in] minPwm         the minimum PWM value
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_SetPWMConfig(HAL_DigitalHandle pwmPortHandle, double maxPwm,
                       double deadbandMaxPwm, double centerPwm,
@@ -69,12 +72,13 @@
  *
  * Values are in raw FPGA units.
  *
- * @param pwmPortHandle  the PWM handle
- * @param maxPwm         the maximum PWM value
- * @param deadbandMaxPwm the high range of the center deadband
- * @param centerPwm      the center PWM value
- * @param deadbandMinPwm the low range of the center deadband
- * @param minPwm         the minimum PWM value
+ * @param[in] pwmPortHandle  the PWM handle
+ * @param[in] maxPwm         the maximum PWM value
+ * @param[in] deadbandMaxPwm the high range of the center deadband
+ * @param[in] centerPwm      the center PWM value
+ * @param[in] deadbandMinPwm the low range of the center deadband
+ * @param[in] minPwm         the minimum PWM value
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_SetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t maxPwm,
                          int32_t deadbandMaxPwm, int32_t centerPwm,
@@ -87,12 +91,13 @@
  * Values are in raw FPGA units. These units have the potential to change for
  * any FPGA release.
  *
- * @param pwmPortHandle  the PWM handle
- * @param maxPwm         the maximum PWM value
- * @param deadbandMaxPwm the high range of the center deadband
- * @param centerPwm      the center PWM value
- * @param deadbandMinPwm the low range of the center deadband
- * @param minPwm         the minimum PWM value
+ * @param[in] pwmPortHandle  the PWM handle
+ * @param[in] maxPwm         the maximum PWM value
+ * @param[in] deadbandMaxPwm the high range of the center deadband
+ * @param[in] centerPwm      the center PWM value
+ * @param[in] deadbandMinPwm the low range of the center deadband
+ * @param[in] minPwm         the minimum PWM value
+ * @param[out] status        Error status variable. 0 on success.
  */
 void HAL_GetPWMConfigRaw(HAL_DigitalHandle pwmPortHandle, int32_t* maxPwm,
                          int32_t* deadbandMaxPwm, int32_t* centerPwm,
@@ -103,8 +108,9 @@
  * Sets if the FPGA should output the center value if the input value is within
  * the deadband.
  *
- * @param pwmPortHandle     the PWM handle
- * @param eliminateDeadband true to eliminate deadband, otherwise false
+ * @param[in] pwmPortHandle     the PWM handle
+ * @param[in] eliminateDeadband true to eliminate deadband, otherwise false
+ * @param[out] status           Error status variable. 0 on success.
  */
 void HAL_SetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
                                  HAL_Bool eliminateDeadband, int32_t* status);
@@ -112,8 +118,9 @@
 /**
  * Gets the current eliminate deadband value.
  *
- * @param pwmPortHandle the PWM handle
- * @return              true if set, otherwise false
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return true if set, otherwise false
  */
 HAL_Bool HAL_GetPWMEliminateDeadband(HAL_DigitalHandle pwmPortHandle,
                                      int32_t* status);
@@ -124,8 +131,9 @@
  * The values are in raw FPGA units, and have the potential to change with any
  * FPGA release.
  *
- * @param pwmPortHandle the PWM handle
- * @param value         the PWM value to set
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[in] value         the PWM value to set
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t value,
                    int32_t* status);
@@ -136,8 +144,9 @@
  * The values range from -1 to 1 and the period is controlled by the PWM Period
  * and MinHigh registers.
  *
- * @param pwmPortHandle the PWM handle
- * @param value         the scaled PWM value to set
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[in] speed         the scaled PWM value to set
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetPWMSpeed(HAL_DigitalHandle pwmPortHandle, double speed,
                      int32_t* status);
@@ -148,8 +157,9 @@
  * The values range from 0 to 1 and the period is controlled by the PWM Period
  * and MinHigh registers.
  *
- * @param pwmPortHandle the PWM handle
- * @param value         the positional PWM value to set
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[in] position      the positional PWM value to set
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetPWMPosition(HAL_DigitalHandle pwmPortHandle, double position,
                         int32_t* status);
@@ -161,7 +171,8 @@
  * from just setting a 0 speed, as this will actively stop all signaling on the
  * channel.
  *
- * @param pwmPortHandle the PWM handle.
+ * @param[in] pwmPortHandle the PWM handle.
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetPWMDisabled(HAL_DigitalHandle pwmPortHandle, int32_t* status);
 
@@ -171,8 +182,9 @@
  * The values are in raw FPGA units, and have the potential to change with any
  * FPGA release.
  *
- * @param pwmPortHandle the PWM handle
- * @return              the current raw PWM value
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the current raw PWM value
  */
 int32_t HAL_GetPWMRaw(HAL_DigitalHandle pwmPortHandle, int32_t* status);
 
@@ -181,8 +193,9 @@
  *
  * The values range from -1 to 1.
  *
- * @param pwmPortHandle the PWM handle
- * @return              the current speed PWM value
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the current speed PWM value
  */
 double HAL_GetPWMSpeed(HAL_DigitalHandle pwmPortHandle, int32_t* status);
 
@@ -191,23 +204,26 @@
  *
  * The values range from 0 to 1.
  *
- * @param pwmPortHandle the PWM handle
- * @return              the current positional PWM value
+ * @param[in] pwmPortHandle the PWM handle
+ * @param[out] status       Error status variable. 0 on success.
+ * @return the current positional PWM value
  */
 double HAL_GetPWMPosition(HAL_DigitalHandle pwmPortHandle, int32_t* status);
 
 /**
  * Forces a PWM signal to go to 0 temporarily.
  *
- * @param pwmPortHandle the PWM handle.
+ * @param[in] pwmPortHandle the PWM handle.
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_LatchPWMZero(HAL_DigitalHandle pwmPortHandle, int32_t* status);
 
 /**
  * Sets how how often the PWM signal is squelched, thus scaling the period.
  *
- * @param pwmPortHandle the PWM handle.
- * @param squelchMask   the 2-bit mask of outputs to squelch
+ * @param[in] pwmPortHandle the PWM handle.
+ * @param[in] squelchMask   the 2-bit mask of outputs to squelch
+ * @param[out] status       Error status variable. 0 on success.
  */
 void HAL_SetPWMPeriodScale(HAL_DigitalHandle pwmPortHandle, int32_t squelchMask,
                            int32_t* status);
@@ -215,6 +231,7 @@
 /**
  * Gets the loop timing of the PWM system.
  *
+ * @param[out] status Error status variable. 0 on success.
  * @return the loop time
  */
 int32_t HAL_GetPWMLoopTiming(int32_t* status);
@@ -224,6 +241,7 @@
  *
  * This time is relative to the FPGA time.
  *
+ * @param[out] status Error status variable. 0 on success.
  * @return the pwm cycle start time
  */
 uint64_t HAL_GetPWMCycleStartTime(int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Ports.h b/third_party/allwpilib/hal/src/main/native/include/hal/Ports.h
index 584bc4f..a212446 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Ports.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Ports.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -122,28 +119,56 @@
  *
  * @return the number of PCM modules
  */
-int32_t HAL_GetNumPCMModules(void);
+int32_t HAL_GetNumCTREPCMModules(void);
 
 /**
  * Gets the number of solenoid channels in the current system.
  *
  * @return the number of solenoid channels
  */
-int32_t HAL_GetNumSolenoidChannels(void);
+int32_t HAL_GetNumCTRESolenoidChannels(void);
 
 /**
  * Gets the number of PDP modules in the current system.
  *
  * @return the number of PDP modules
  */
-int32_t HAL_GetNumPDPModules(void);
+int32_t HAL_GetNumCTREPDPModules(void);
 
 /**
  * Gets the number of PDP channels in the current system.
  *
  * @return the number of PDP channels
  */
-int32_t HAL_GetNumPDPChannels(void);
+int32_t HAL_GetNumCTREPDPChannels(void);
+
+/**
+ * Gets the number of PDH modules in the current system.
+ *
+ * @return the number of PDH modules
+ */
+int32_t HAL_GetNumREVPDHModules(void);
+
+/**
+ * Gets the number of PDH channels in the current system.
+ *
+ * @return the number of PDH channels
+ */
+int32_t HAL_GetNumREVPDHChannels(void);
+
+/**
+ * Gets the number of PH modules in the current system.
+ *
+ * @return the number of PH modules
+ */
+int32_t HAL_GetNumREVPHModules(void);
+
+/**
+ * Gets the number of PH channels in the current system.
+ *
+ * @return the number of PH channels
+ */
+int32_t HAL_GetNumREVPHChannels(void);
 
 /**
  * Gets the number of duty cycle inputs in the current system.
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Power.h b/third_party/allwpilib/hal/src/main/native/include/hal/Power.h
index 7ac7991..2bd983a 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Power.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Power.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,6 +21,7 @@
 /**
  * Gets the roboRIO input voltage.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the input voltage (volts)
  */
 double HAL_GetVinVoltage(int32_t* status);
@@ -31,6 +29,7 @@
 /**
  * Gets the roboRIO input current.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the input current (amps)
  */
 double HAL_GetVinCurrent(int32_t* status);
@@ -38,6 +37,7 @@
 /**
  * Gets the 6V rail voltage.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the 6V rail voltage (volts)
  */
 double HAL_GetUserVoltage6V(int32_t* status);
@@ -45,6 +45,7 @@
 /**
  * Gets the 6V rail current.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the 6V rail current (amps)
  */
 double HAL_GetUserCurrent6V(int32_t* status);
@@ -52,6 +53,7 @@
 /**
  * Gets the active state of the 6V rail.
  *
+ * @param[out] status the error code, or 0 for success
  * @return true if the rail is active, otherwise false
  */
 HAL_Bool HAL_GetUserActive6V(int32_t* status);
@@ -59,6 +61,7 @@
 /**
  * Gets the fault count for the 6V rail.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the number of 6V fault counts
  */
 int32_t HAL_GetUserCurrentFaults6V(int32_t* status);
@@ -66,6 +69,7 @@
 /**
  * Gets the 5V rail voltage.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the 5V rail voltage (volts)
  */
 double HAL_GetUserVoltage5V(int32_t* status);
@@ -73,6 +77,7 @@
 /**
  * Gets the 5V rail current.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the 5V rail current (amps)
  */
 double HAL_GetUserCurrent5V(int32_t* status);
@@ -80,6 +85,7 @@
 /**
  * Gets the active state of the 5V rail.
  *
+ * @param[out] status the error code, or 0 for success
  * @return true if the rail is active, otherwise false
  */
 HAL_Bool HAL_GetUserActive5V(int32_t* status);
@@ -87,6 +93,7 @@
 /**
  * Gets the fault count for the 5V rail.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the number of 5V fault counts
  */
 int32_t HAL_GetUserCurrentFaults5V(int32_t* status);
@@ -94,6 +101,7 @@
 /**
  * Gets the 3V3 rail voltage.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the 3V3 rail voltage (volts)
  */
 double HAL_GetUserVoltage3V3(int32_t* status);
@@ -101,6 +109,7 @@
 /**
  * Gets the 3V3 rail current.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the 3V3 rail current (amps)
  */
 double HAL_GetUserCurrent3V3(int32_t* status);
@@ -108,6 +117,7 @@
 /**
  * Gets the active state of the 3V3 rail.
  *
+ * @param[out] status the error code, or 0 for success
  * @return true if the rail is active, otherwise false
  */
 HAL_Bool HAL_GetUserActive3V3(int32_t* status);
@@ -115,9 +125,30 @@
 /**
  * Gets the fault count for the 3V3 rail.
  *
+ * @param[out] status the error code, or 0 for success
  * @return the number of 3V3 fault counts
  */
 int32_t HAL_GetUserCurrentFaults3V3(int32_t* status);
+
+/**
+ * Get the current brownout voltage setting.
+ *
+ * @param[out] status the error code, or 0 for success
+ * @return The brownout voltage
+ */
+double HAL_GetBrownoutVoltage(int32_t* status);
+
+/**
+ * Set the voltage the roboRIO will brownout and disable all outputs.
+ *
+ * Note that this only does anything on the roboRIO 2.
+ * On the roboRIO it is a no-op.
+ *
+ * @param[in] voltage The brownout voltage
+ * @param[out] status the error code, or 0 for success
+ */
+void HAL_SetBrownoutVoltage(double voltage, int32_t* status);
+
 #ifdef __cplusplus
 }  // extern "C"
 #endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h b/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h
new file mode 100644
index 0000000..fabb8b7
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/PowerDistribution.h
@@ -0,0 +1,224 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_pd Power Distribution Functions
+ * @ingroup hal_capi
+ * Functions to control Power Distribution devices.
+ * @{
+ */
+
+// clang-format off
+/**
+ * The acceptable accelerometer ranges.
+ */
+HAL_ENUM(HAL_PowerDistributionType) {
+  HAL_PowerDistributionType_kAutomatic = 0,
+  HAL_PowerDistributionType_kCTRE = 1,
+  HAL_PowerDistributionType_kRev = 2,
+};
+// clang-format on
+
+#define HAL_DEFAULT_POWER_DISTRIBUTION_MODULE -1
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * Initializes a Power Distribution Panel.
+ *
+ * @param[in] moduleNumber       the module number to initialize
+ * @param[in] type               the type of module to intialize
+ * @param[in] allocationLocation the location where the allocation is occuring
+ * @param[out] status            Error status variable. 0 on success.
+ * @return the created PowerDistribution
+ */
+HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
+    int32_t moduleNumber, HAL_PowerDistributionType type,
+    const char* allocationLocation, int32_t* status);
+
+/**
+ * Gets the module number for a specific handle.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the module number
+ */
+int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle,
+                                             int32_t* status);
+
+/**
+ * Cleans a PowerDistribution module.
+ *
+ * @param handle the module handle
+ */
+void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle);
+
+/**
+ * Checks if a PowerDistribution channel is valid.
+ *
+ * @param handle  the module handle
+ * @param channel the channel to check
+ * @return true if the channel is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle,
+                                           int32_t channel);
+
+/**
+ * Checks if a PowerDistribution module is valid.
+ *
+ * @param module the module to check
+ * @param type   the type of module
+ * @return true if the module is valid, otherwise false
+ */
+HAL_Bool HAL_CheckPowerDistributionModule(int32_t module,
+                                          HAL_PowerDistributionType type);
+
+/**
+ * Gets the type of PowerDistribution module.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the type of module
+ */
+HAL_PowerDistributionType HAL_GetPowerDistributionType(
+    HAL_PowerDistributionHandle handle, int32_t* status);
+
+/**
+ * Gets the number of channels for this handle.
+ *
+ * @param[in] handle the handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return number of channels
+ */
+int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle,
+                                            int32_t* status);
+
+/**
+ * Gets the temperature of the PowerDistribution.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the module temperature (celsius)
+ */
+double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle,
+                                           int32_t* status);
+
+/**
+ * Gets the PowerDistribution input voltage.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the input voltage (volts)
+ */
+double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle,
+                                       int32_t* status);
+
+/**
+ * Gets the current of a specific PowerDistribution channel.
+ *
+ * @param[in] handle   the module handle
+ * @param[in] channel  the channel
+ * @param[out] status  Error status variable. 0 on success.
+ * @return the channel current (amps)
+ */
+double HAL_GetPowerDistributionChannelCurrent(
+    HAL_PowerDistributionHandle handle, int32_t channel, int32_t* status);
+
+/**
+ * Gets the current of all 24 channels on the PowerDistribution.
+ *
+ * The array must be large enough to hold all channels.
+ *
+ * @param[in] handle         the module handle
+ * @param[out] currents      the currents
+ * @param[in] currentsLength the length of the currents array
+ * @param[out] status        Error status variable. 0 on success.
+ */
+void HAL_GetPowerDistributionAllChannelCurrents(
+    HAL_PowerDistributionHandle handle, double* currents,
+    int32_t currentsLength, int32_t* status);
+
+/**
+ * Gets the total current of the PowerDistribution.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the total current (amps)
+ */
+double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle,
+                                            int32_t* status);
+
+/**
+ * Gets the total power of the PowerDistribution.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the total power (watts)
+ */
+double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle,
+                                          int32_t* status);
+
+/**
+ * Gets the total energy of the PowerDistribution.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ * @return the total energy (joules)
+ */
+double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
+                                           int32_t* status);
+
+/**
+ * Resets the PowerDistribution accumulated energy.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ */
+void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
+                                           int32_t* status);
+
+/**
+ * Clears any PowerDistribution sticky faults.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ */
+void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle,
+                                            int32_t* status);
+
+/**
+ * Power on/off switchable channel.
+ *
+ * This is a REV PDH-specific function. This function will no-op on CTRE PDP.
+ *
+ * @param[in] handle the module handle
+ * @param[in] enabled true to turn on switchable channel
+ * @param[out] status Error status variable. 0 on success.
+ */
+void HAL_SetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, HAL_Bool enabled, int32_t* status);
+
+/**
+ * Returns true if switchable channel is powered on.
+ *
+ * This is a REV PDH-specific function. This function will no-op on CTRE PDP.
+ *
+ * @param[in] handle the module handle
+ * @param[out] status Error status variable. 0 on success.
+ */
+HAL_Bool HAL_GetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, int32_t* status);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h b/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h
new file mode 100644
index 0000000..0cab15e
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/REVPH.h
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include "hal/Types.h"
+
+/**
+ * @defgroup hal_rev_ph REV PH Functions
+ * @ingroup hal_capi
+ * @{
+ */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
+                                    const char* allocationLocation,
+                                    int32_t* status);
+
+void HAL_FreeREVPH(HAL_REVPHHandle handle);
+
+HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel);
+HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module);
+
+HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status);
+void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled,
+                                   int32_t* status);
+HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle, int32_t* status);
+HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status);
+double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status);
+double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel,
+                                  int32_t* status);
+
+int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status);
+void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
+                           int32_t* status);
+
+void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
+                          int32_t* status);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Relay.h b/third_party/allwpilib/hal/src/main/native/include/hal/Relay.h
index 281aad6..7d711b2 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Relay.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Relay.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -27,11 +24,16 @@
  * Note this call will only initialize either the forward or reverse port of the
  * relay. If you need both, you will need to initialize 2 relays.
  *
- * @param portHandle the port handle to initialize
- * @param fwd        true for the forward port, false for the reverse port
- * @return           the created relay handle
+ * @param[in] portHandle         the port handle to initialize
+ * @param[in] fwd                true for the forward port, false for the
+ *                               reverse port
+ * @param[in] allocationLocation the location where the allocation is occuring
+ *                               (can be null)
+ * @param[out] status            Error status variable. 0 on success.
+ * @return the created relay handle
  */
 HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+                                        const char* allocationLocation,
                                         int32_t* status);
 
 /**
@@ -45,15 +47,16 @@
  * Checks if a relay channel is valid.
  *
  * @param channel the channel to check
- * @return        true if the channel is valid, otherwise false
+ * @return true if the channel is valid, otherwise false
  */
 HAL_Bool HAL_CheckRelayChannel(int32_t channel);
 
 /**
  * Sets the state of a relay output.
  *
- * @param relayPortHandle the relay handle
- * @param on              true for on, false for off
+ * @param[in] relayPortHandle the relay handle
+ * @param[in] on              true for on, false for off
+ * @param[out] status         Error status variable. 0 on success.
  */
 void HAL_SetRelay(HAL_RelayHandle relayPortHandle, HAL_Bool on,
                   int32_t* status);
@@ -61,8 +64,9 @@
 /**
  * Gets the current state of the relay channel.
  *
- * @param relayPortHandle the relay handle
- * @return                true for on, false for off
+ * @param[in] relayPortHandle the relay handle
+ * @param[out] status         Error status variable. 0 on success.
+ * @return true for on, false for off
  */
 HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status);
 #ifdef __cplusplus
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SPI.h b/third_party/allwpilib/hal/src/main/native/include/hal/SPI.h
index abee379..84cec5d 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SPI.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SPI.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -28,7 +25,9 @@
  *
  * If opening the MXP port, also sets up the channel functions appropriately.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS3, 4 for MXP
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS3, 4
+ *                    for MXP
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status);
 
@@ -39,11 +38,11 @@
  * over each transaction.
  *
  * @param port         The number of the port to use. 0-3 for Onboard CS0-CS2, 4
- * for MXP
+ *                     for MXP
  * @param dataToSend   Buffer of data to send as part of the transaction.
  * @param dataReceived Buffer to read data into.
  * @param size         Number of bytes to transfer. [0..7]
- * @return             Number of bytes transferred, -1 for error
+ * @return Number of bytes transferred, -1 for error
  */
 int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
                            uint8_t* dataReceived, int32_t size);
@@ -53,11 +52,11 @@
  *
  * Writes to a device and wait until the transaction is complete.
  *
- * @param port      The number of the port to use. 0-3 for Onboard CS0-CS2, 4
- * for MXP
- * @param datToSend The data to write to the register on the device.
- * @param sendSize  The number of bytes to be written
- * @return          The number of bytes written. -1 for an error
+ * @param port       The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                   for MXP
+ * @param dataToSend The data to write to the register on the device.
+ * @param sendSize   The number of bytes to be written
+ * @return The number of bytes written. -1 for an error
  */
 int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
                      int32_t sendSize);
@@ -71,11 +70,11 @@
  * begin returning data.
  *
  * @param port   The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP
+ *               MXP
  * @param buffer A pointer to the array of bytes to store the data read from the
- * device.
+ *               device.
  * @param count  The number of bytes to read in the transaction. [1..7]
- * @return       Number of bytes read. -1 for error.
+ * @return Number of bytes read. -1 for error.
  */
 int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count);
 
@@ -90,7 +89,7 @@
  * Sets the clock speed for the SPI bus.
  *
  * @param port  The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP
+ *              MXP
  * @param speed The speed in Hz (0-1MHz)
  */
 void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed);
@@ -99,12 +98,12 @@
  * Sets the SPI options.
  *
  * @param port             The number of the port to use. 0-3 for Onboard
- * CS0-CS2, 4 for MXP
+ *                         CS0-CS2, 4 for MXP
  * @param msbFirst         True to write the MSB first, False for LSB first
  * @param sampleOnTrailing True to sample on the trailing edge, False to sample
- * on the leading edge
+ *                         on the leading edge
  * @param clkIdleHigh      True to set the clock to active low, False to set the
- * clock active high
+ *                         clock active high
  */
 void HAL_SetSPIOpts(HAL_SPIPort port, HAL_Bool msbFirst,
                     HAL_Bool sampleOnTrailing, HAL_Bool clkIdleHigh);
@@ -112,14 +111,18 @@
 /**
  * Sets the CS Active high for a SPI port.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ * @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
+ *                 MXP
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status);
 
 /**
  * Sets the CS Active low for a SPI port.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                    for MXP
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status);
 
@@ -127,8 +130,8 @@
  * Gets the stored handle for a SPI port.
  *
  * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
- * @return     The stored handle for the SPI port. 0 represents no stored
- * handle.
+ * @return The stored handle for the SPI port. 0 represents no stored
+ *         handle.
  */
 int32_t HAL_GetSPIHandle(HAL_SPIPort port);
 
@@ -136,7 +139,7 @@
  * Sets the stored handle for a SPI port.
  *
  * @param port   The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
+ *               MXP.
  * @param handle The value of the handle for the port.
  */
 void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle);
@@ -144,26 +147,29 @@
 /**
  * Initializes the SPI automatic accumulator.
  *
- * @param port       The number of the port to use. 0-3 for Onboard CS0-CS2, 4
- * for MXP.
- * @param bufferSize The accumulator buffer size.
+ * @param[in] port       The number of the port to use. 0-3 for Onboard CS0-CS2,
+ *                       4 for MXP.
+ * @param[in] bufferSize The accumulator buffer size.
+ * @param[out] status    the error code, or 0 for success
  */
 void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status);
 
 /**
  * Frees an SPI automatic accumulator.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                    for MXP.
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status);
 
 /**
  * Sets the period for automatic SPI accumulation.
  *
- * @param port   The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
- * @param period The accumlation period (seconds).
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                    for MXP.
+ * @param[in] period  The accumlation period (seconds).
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status);
 
@@ -173,14 +179,15 @@
  * Note that triggering on both rising and falling edges is a valid
  * configuration.
  *
- * @param port                The number of the port to use. 0-3 for Onboard
- * CS0-CS2, 4 for MXP.
- * @param digitalSourceHandle The trigger source to use (Either
- * HAL_AnalogTriggerHandle or HAL_DigitalHandle).
- * @param analogTriggerType   The analog trigger type, if the source is an
- * analog trigger.
- * @param triggerRising       Trigger on the rising edge if true.
- * @param triggerFalling      Trigger on the falling edge if true.
+ * @param[in] port                The number of the port to use. 0-3 for Onboard
+ *                                CS0-CS2, 4 for MXP.
+ * @param[in] digitalSourceHandle The trigger source to use (Either
+ *                                HAL_AnalogTriggerHandle or HAL_DigitalHandle).
+ * @param[in] analogTriggerType   The analog trigger type, if the source is an
+ *                                analog trigger.
+ * @param[in] triggerRising       Trigger on the rising edge if true.
+ * @param[in] triggerFalling      Trigger on the falling edge if true.
+ * @param[out] status             the error code, or 0 for success
  */
 void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
                              HAL_AnalogTriggerType analogTriggerType,
@@ -190,20 +197,22 @@
 /**
  * Stops an automatic SPI accumlation.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                    for MXP.
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status);
 
 /**
  * Sets the data to be transmitted to the device to initiate a read.
  *
- * @param port       The number of the port to use. 0-3 for Onboard CS0-CS2, 4
- * for MXP.
- * @param dataToSend Pointer to the data to send (Gets copied for continue use,
- * so no need to keep alive).
- * @param dataSize   The length of the data to send.
- * @param zeroSize   The number of zeros to send after the data.
+ * @param[in] port       The number of the port to use. 0-3 for Onboard CS0-CS2,
+ *                       4 for MXP.
+ * @param[in] dataToSend Pointer to the data to send (Gets copied for continue
+ *                       use, so no need to keep alive).
+ * @param[in] dataSize   The length of the data to send.
+ * @param[in] zeroSize   The number of zeros to send after the data.
+ * @param[out] status    the error code, or 0 for success
  */
 void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
                                 int32_t dataSize, int32_t zeroSize,
@@ -212,8 +221,9 @@
 /**
  * Immediately forces an SPI read to happen.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                    for MXP.
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status);
 
@@ -224,12 +234,13 @@
  * sequence is the same as the combined dataSize + zeroSize set in
  * HAL_SetSPIAutoTransmitData.
  *
- * @param port      The number of the port to use. 0-3 for Onboard CS0-CS2, 4
- * for MXP.
- * @param buffer    The buffer to store the data into.
- * @param numToRead The number of words to read.
- * @param timeout   The read timeout (in seconds).
- * @return          The number of words actually read.
+ * @param[in] port      The number of the port to use. 0-3 for Onboard CS0-CS2,
+ *                      4 for MXP.
+ * @param[out] buffer   The buffer to store the data into.
+ * @param[in] numToRead The number of words to read.
+ * @param[in] timeout   The read timeout (in seconds).
+ * @param[out] status   the error code, or 0 for success
+ * @return The number of words actually read.
  */
 int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
                                     int32_t numToRead, double timeout,
@@ -238,20 +249,23 @@
 /**
  * Gets the count of how many SPI accumulations have been missed.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
- * @return     The number of missed accumulations.
+ * @param[in] port    The number of the port to use. 0-3 for Onboard CS0-CS2, 4
+ *                    for MXP.
+ * @param[out] status the error code, or 0 for success
+ * @return The number of missed accumulations.
  */
 int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status);
 
 /**
  * Configure the Auto SPI Stall time between reads.
  *
- * @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
- * MXP.
- * @param csToSclkTicks the number of ticks to wait before asserting the cs pin
- * @param stallTicks the number of ticks to stall for
- * @param pow2BytesPerRead the number of bytes to read before stalling
+ * @param[in] port             The number of the port to use. 0-3 for Onboard
+ *                             CS0-CS2, 4 for MXP.
+ * @param[in] csToSclkTicks    the number of ticks to wait before asserting the
+ *                             cs pin
+ * @param[in] stallTicks       the number of ticks to stall for
+ * @param[in] pow2BytesPerRead the number of bytes to read before stalling
+ * @param[out] status          the error code, or 0 for success
  */
 void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
                                int32_t stallTicks, int32_t pow2BytesPerRead,
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h b/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h
index 170bd27..de66226 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SPITypes.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h b/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h
index 226a2cb..cd6140e 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SerialPort.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,7 +33,8 @@
  * The channels are either the onboard RS232, the mxp uart, or 2 USB ports. The
  * top port is USB1, the bottom port is USB2.
  *
- * @param port the serial port to initialize
+ * @param[in] port the serial port to initialize
+ * @param[out] status the error code, or 0 for success
  */
 HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port,
                                               int32_t* status);
@@ -47,8 +45,9 @@
  * This name is the /dev name for a specific port.
  * Note these are not always consistent between roboRIO reboots.
  *
- * @param port     the serial port to initialize
- * @param portName the dev port name
+ * @param[in] port     the serial port to initialize
+ * @param[in] portName the dev port name
+ * @param[out] status  the error code, or 0 for success
  */
 HAL_SerialPortHandle HAL_InitializeSerialPortDirect(HAL_SerialPort port,
                                                     const char* portName,
@@ -57,8 +56,9 @@
 /**
  * Gets the raw serial port file descriptor from a handle.
  *
- * @param handle the serial port handle
- * @return       the raw port descriptor
+ * @param[in] handle the serial port handle
+ * @param[out] status the error code, or 0 for success
+ * @return the raw port descriptor
  */
 int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status);
 
@@ -67,8 +67,9 @@
  *
  * Any value between 0 and 0xFFFFFFFF may be used. Default is 9600.
  *
- * @param handle the serial port handle
- * @param baud   the baud rate to set
+ * @param[in] handle  the serial port handle
+ * @param[in] baud    the baud rate to set
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud,
                            int32_t* status);
@@ -78,8 +79,9 @@
  *
  * Defaults to 8.
  *
- * @param handle the serial port handle
- * @param bits   the number of data bits (5-8)
+ * @param[in] handle  the serial port handle
+ * @param[in] bits    the number of data bits (5-8)
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialDataBits(HAL_SerialPortHandle handle, int32_t bits,
                            int32_t* status);
@@ -94,8 +96,9 @@
  *   3: Mark - Means exists and always 1
  *   4: Space - Means exists and always 0
  *
- * @param handle the serial port handle
- * @param parity the parity bit mode (see remarks for valid values)
+ * @param[in] handle  the serial port handle
+ * @param[in] parity  the parity bit mode (see remarks for valid values)
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialParity(HAL_SerialPortHandle handle, int32_t parity,
                          int32_t* status);
@@ -108,8 +111,9 @@
  *   15: One and a half stop bits
  *   20: Two stop bits
  *
- * @param handle   the serial port handle
- * @param stopBits the stop bit value (see remarks for valid values)
+ * @param[in] handle    the serial port handle
+ * @param[in] stopBits  the stop bit value (see remarks for valid values)
+ * @param[out] status   the error code, or 0 for success
  */
 void HAL_SetSerialStopBits(HAL_SerialPortHandle handle, int32_t stopBits,
                            int32_t* status);
@@ -121,8 +125,9 @@
  *   1: Flush on access
  *   2: Flush when full (default)
  *
- * @param handle the serial port handle
- * @param mode   the mode to set (see remarks for valid values)
+ * @param[in] handle  the serial port handle
+ * @param[in] mode    the mode to set (see remarks for valid values)
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialWriteMode(HAL_SerialPortHandle handle, int32_t mode,
                             int32_t* status);
@@ -136,8 +141,9 @@
  *   2: RTS-CTS
  *   3: DTR-DSR
  *
- * @param handle the serial port handle
- * @param flow   the mode to set (see remarks for valid values)
+ * @param[in] handle  the serial port handle
+ * @param[in] flow    the mode to set (see remarks for valid values)
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialFlowControl(HAL_SerialPortHandle handle, int32_t flow,
                               int32_t* status);
@@ -145,8 +151,9 @@
 /**
  * Sets the minimum serial read timeout of a port.
  *
- * @param handle  the serial port handle
- * @param timeout the timeout in milliseconds
+ * @param[in] handle   the serial port handle
+ * @param[in] timeout  the timeout in milliseconds
+ * @param[out] status  the error code, or 0 for success
  */
 void HAL_SetSerialTimeout(HAL_SerialPortHandle handle, double timeout,
                           int32_t* status);
@@ -156,8 +163,9 @@
  *
  * By default this is disabled.
  *
- * @param handle     the serial port handle
- * @param terminator the termination character to set
+ * @param[in] handle      the serial port handle
+ * @param[in] terminator  the termination character to set
+ * @param[out] status     the error code, or 0 for success
  */
 void HAL_EnableSerialTermination(HAL_SerialPortHandle handle, char terminator,
                                  int32_t* status);
@@ -165,15 +173,17 @@
 /**
  * Disables a termination character for reads.
  *
- * @param handle the serial port handle
+ * @param[in] handle  the serial port handle
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_DisableSerialTermination(HAL_SerialPortHandle handle, int32_t* status);
 
 /**
  * Sets the size of the read buffer.
  *
- * @param handle the serial port handle
- * @param size   the read buffer size
+ * @param[in] handle  the serial port handle
+ * @param[in] size    the read buffer size
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialReadBufferSize(HAL_SerialPortHandle handle, int32_t size,
                                  int32_t* status);
@@ -181,8 +191,9 @@
 /**
  * Sets the size of the write buffer.
  *
- * @param handle the serial port handle
- * @param size   the write buffer size
+ * @param[in] handle  the serial port handle
+ * @param[in] size    the write buffer size
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_SetSerialWriteBufferSize(HAL_SerialPortHandle handle, int32_t size,
                                   int32_t* status);
@@ -190,8 +201,9 @@
 /**
  * Gets the number of bytes currently in the read buffer.
  *
- * @param handle the serial port handle
- * @return       the number of bytes in the read buffer
+ * @param[in] handle  the serial port handle
+ * @param[out] status the error code, or 0 for success
+ * @return the number of bytes in the read buffer
  */
 int32_t HAL_GetSerialBytesReceived(HAL_SerialPortHandle handle,
                                    int32_t* status);
@@ -202,9 +214,11 @@
  * Will wait for either timeout (if set), the termination char (if set), or the
  * count to be full. Whichever one comes first.
  *
- * @param handle the serial port handle
- * @param count  the number of bytes maximum to read
- * @return       the number of bytes actually read
+ * @param[in] handle  the serial port handle
+ * @param[out] buffer the buffer in which to store bytes read
+ * @param[in] count   the number of bytes maximum to read
+ * @param[out] status the error code, or 0 for success
+ * @return the number of bytes actually read
  */
 int32_t HAL_ReadSerial(HAL_SerialPortHandle handle, char* buffer, int32_t count,
                        int32_t* status);
@@ -212,10 +226,11 @@
 /**
  * Writes data to the serial port.
  *
- * @param handle the serial port handle
- * @param buffer the buffer to write
- * @param count  the number of bytes to write from the buffer
- * @return       the number of bytes actually written
+ * @param[in] handle  the serial port handle
+ * @param[in] buffer  the buffer to write
+ * @param[in] count   the number of bytes to write from the buffer
+ * @param[out] status the error code, or 0 for success
+ * @return the number of bytes actually written
  */
 int32_t HAL_WriteSerial(HAL_SerialPortHandle handle, const char* buffer,
                         int32_t count, int32_t* status);
@@ -223,21 +238,24 @@
 /**
  * Flushes the serial write buffer out to the port.
  *
- * @param handle the serial port handle
+ * @param[in] handle  the serial port handle
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_FlushSerial(HAL_SerialPortHandle handle, int32_t* status);
 
 /**
  * Clears the receive buffer of the serial port.
  *
- * @param handle the serial port handle
+ * @param[in] handle  the serial port handle
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_ClearSerial(HAL_SerialPortHandle handle, int32_t* status);
 
 /**
  * Closes a serial port.
  *
- * @param handle the serial port handle to close
+ * @param[in] handle  the serial port handle to close
+ * @param[out] status the error code, or 0 for success
  */
 void HAL_CloseSerial(HAL_SerialPortHandle handle, int32_t* status);
 #ifdef __cplusplus
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h b/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h
index ff4c12e..7c0cf2d 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/SimDevice.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,7 +9,7 @@
 #ifdef __cplusplus
 #include <initializer_list>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 #endif
 
 #include "hal/Types.h"
@@ -30,6 +27,17 @@
  * @{
  */
 
+/**
+ * Direction of a simulated value (from the perspective of user code).
+ */
+// clang-format off
+HAL_ENUM(HAL_SimValueDirection) {
+  HAL_SimValueInput = 0,  /**< input to user code from the simulator */
+  HAL_SimValueOutput,     /**< output from user code to the simulator */
+  HAL_SimValueBidir       /**< bidirectional between user code and simulator */
+};
+// clang-format on
+
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -67,26 +75,66 @@
  *
  * @param device simulated device handle
  * @param name value name
- * @param readonly if the value should not be written from simulation side
+ * @param direction input/output/bidir (from perspective of user code)
  * @param initialValue initial value
  * @return simulated value handle
  */
 HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
-                                      const char* name, HAL_Bool readonly,
+                                      const char* name, int32_t direction,
                                       const struct HAL_Value* initialValue);
 
 #ifdef __cplusplus
 extern "C++" {
 inline HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
                                              const char* name,
-                                             HAL_Bool readonly,
+                                             int32_t direction,
                                              const HAL_Value& initialValue) {
-  return HAL_CreateSimValue(device, name, readonly, &initialValue);
+  return HAL_CreateSimValue(device, name, direction, &initialValue);
 }
 }  // extern "C++"
 #endif
 
 /**
+ * Creates an int value on a simulated device.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param direction input/output/bidir (from perspective of user code)
+ * @param initialValue initial value
+ * @return simulated value handle
+ */
+inline HAL_SimValueHandle HAL_CreateSimValueInt(HAL_SimDeviceHandle device,
+                                                const char* name,
+                                                int32_t direction,
+                                                int32_t initialValue) {
+  struct HAL_Value v = HAL_MakeInt(initialValue);
+  return HAL_CreateSimValue(device, name, direction, &v);
+}
+
+/**
+ * Creates a long value on a simulated device.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param direction input/output/bidir (from perspective of user code)
+ * @param initialValue initial value
+ * @return simulated value handle
+ */
+inline HAL_SimValueHandle HAL_CreateSimValueLong(HAL_SimDeviceHandle device,
+                                                 const char* name,
+                                                 int32_t direction,
+                                                 int64_t initialValue) {
+  struct HAL_Value v = HAL_MakeLong(initialValue);
+  return HAL_CreateSimValue(device, name, direction, &v);
+}
+
+/**
  * Creates a double value on a simulated device.
  *
  * Returns 0 if not in simulation; this can be used to avoid calls
@@ -94,16 +142,16 @@
  *
  * @param device simulated device handle
  * @param name value name
- * @param readonly if the value should not be written from simulation side
+ * @param direction input/output/bidir (from perspective of user code)
  * @param initialValue initial value
  * @return simulated value handle
  */
 inline HAL_SimValueHandle HAL_CreateSimValueDouble(HAL_SimDeviceHandle device,
                                                    const char* name,
-                                                   HAL_Bool readonly,
+                                                   int32_t direction,
                                                    double initialValue) {
   struct HAL_Value v = HAL_MakeDouble(initialValue);
-  return HAL_CreateSimValue(device, name, readonly, &v);
+  return HAL_CreateSimValue(device, name, direction, &v);
 }
 
 /**
@@ -116,19 +164,41 @@
  *
  * @param device simulated device handle
  * @param name value name
- * @param readonly if the value should not be written from simulation side
+ * @param direction input/output/bidir (from perspective of user code)
  * @param numOptions number of enumerated value options (length of options)
  * @param options array of option descriptions
  * @param initialValue initial value (selection)
  * @return simulated value handle
  */
 HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
-                                          const char* name, HAL_Bool readonly,
+                                          const char* name, int32_t direction,
                                           int32_t numOptions,
                                           const char** options,
                                           int32_t initialValue);
 
 /**
+ * Creates an enumerated value on a simulated device with double values.
+ *
+ * Enumerated values are always in the range 0 to numOptions-1.
+ *
+ * Returns 0 if not in simulation; this can be used to avoid calls
+ * to Set/Get functions.
+ *
+ * @param device simulated device handle
+ * @param name value name
+ * @param direction input/output/bidir (from perspective of user code)
+ * @param numOptions number of enumerated value options (length of options)
+ * @param options array of option descriptions
+ * @param optionValues array of option double values
+ * @param initialValue initial value (selection)
+ * @return simulated value handle
+ */
+HAL_SimValueHandle HAL_CreateSimValueEnumDouble(
+    HAL_SimDeviceHandle device, const char* name, int32_t direction,
+    int32_t numOptions, const char** options, const double* optionValues,
+    int32_t initialValue);
+
+/**
  * Creates a boolean value on a simulated device.
  *
  * Returns 0 if not in simulation; this can be used to avoid calls
@@ -136,16 +206,16 @@
  *
  * @param device simulated device handle
  * @param name value name
- * @param readonly if the value should not be written from simulation side
+ * @param direction input/output/bidir (from perspective of user code)
  * @param initialValue initial value
  * @return simulated value handle
  */
 inline HAL_SimValueHandle HAL_CreateSimValueBoolean(HAL_SimDeviceHandle device,
                                                     const char* name,
-                                                    HAL_Bool readonly,
+                                                    int32_t direction,
                                                     HAL_Bool initialValue) {
   struct HAL_Value v = HAL_MakeBoolean(initialValue);
-  return HAL_CreateSimValue(device, name, readonly, &v);
+  return HAL_CreateSimValue(device, name, direction, &v);
 }
 
 /**
@@ -167,6 +237,30 @@
 #endif
 
 /**
+ * Gets a simulated value (int).
+ *
+ * @param handle simulated value handle
+ * @return The current value
+ */
+inline int32_t HAL_GetSimValueInt(HAL_SimValueHandle handle) {
+  struct HAL_Value v;
+  HAL_GetSimValue(handle, &v);
+  return v.type == HAL_INT ? v.data.v_int : 0;
+}
+
+/**
+ * Gets a simulated value (long).
+ *
+ * @param handle simulated value handle
+ * @return The current value
+ */
+inline int64_t HAL_GetSimValueLong(HAL_SimValueHandle handle) {
+  struct HAL_Value v;
+  HAL_GetSimValue(handle, &v);
+  return v.type == HAL_LONG ? v.data.v_long : 0;
+}
+
+/**
  * Gets a simulated value (double).
  *
  * @param handle simulated value handle
@@ -219,6 +313,28 @@
 #endif
 
 /**
+ * Sets a simulated value (int).
+ *
+ * @param handle simulated value handle
+ * @param value the value to set
+ */
+inline void HAL_SetSimValueInt(HAL_SimValueHandle handle, int value) {
+  struct HAL_Value v = HAL_MakeInt(value);
+  HAL_SetSimValue(handle, &v);
+}
+
+/**
+ * Sets a simulated value (long).
+ *
+ * @param handle simulated value handle
+ * @param value the value to set
+ */
+inline void HAL_SetSimValueLong(HAL_SimValueHandle handle, int64_t value) {
+  struct HAL_Value v = HAL_MakeLong(value);
+  HAL_SetSimValue(handle, &v);
+}
+
+/**
  * Sets a simulated value (double).
  *
  * @param handle simulated value handle
@@ -251,6 +367,17 @@
   HAL_SetSimValue(handle, &v);
 }
 
+/**
+ * Resets a simulated double or integral value to 0.
+ * Has no effect on other value types.
+ * Use this instead of Set(0) for resetting incremental sensor values like
+ * encoder counts or gyro accumulated angle to ensure correct behavior in a
+ * distributed system (e.g. WebSockets).
+ *
+ * @param handle simulated value handle
+ */
+void HAL_ResetSimValue(HAL_SimValueHandle handle);
+
 /** @} */
 
 #ifdef __cplusplus
@@ -276,7 +403,7 @@
    *
    * @param handle simulated value handle
    */
-  /*implicit*/ SimValue(HAL_SimValueHandle val)  // NOLINT(runtime/explicit)
+  /*implicit*/ SimValue(HAL_SimValueHandle val)  // NOLINT
       : m_handle(val) {}
 
   /**
@@ -292,7 +419,7 @@
    *
    * @return internal handle
    */
-  operator HAL_SimValueHandle() const { return m_handle; }
+  operator HAL_SimValueHandle() const { return m_handle; }  // NOLINT
 
   /**
    * Gets the simulated value.
@@ -313,6 +440,88 @@
 };
 
 /**
+ * C++ wrapper around a HAL simulator int value handle.
+ */
+class SimInt : public SimValue {
+ public:
+  /**
+   * Default constructor that results in an "empty" object that is false in
+   * a boolean context.
+   */
+  SimInt() = default;
+
+  /**
+   * Wraps a simulated value handle as returned by HAL_CreateSimValueInt().
+   *
+   * @param handle simulated value handle
+   */
+  /*implicit*/ SimInt(HAL_SimValueHandle val)  // NOLINT
+      : SimValue(val) {}
+
+  /**
+   * Gets the simulated value.
+   *
+   * @return The current value
+   */
+  int32_t Get() const { return HAL_GetSimValueInt(m_handle); }
+
+  /**
+   * Sets the simulated value.
+   *
+   * @param value the value to set
+   */
+  void Set(int32_t value) { HAL_SetSimValueInt(m_handle, value); }
+
+  /**
+   * Resets the simulated value to 0. Use this instead of Set(0) for resetting
+   * incremental sensor values like encoder counts or gyro accumulated angle
+   * to ensure correct behavior in a distributed system (e.g. WebSockets).
+   */
+  void Reset() { HAL_ResetSimValue(m_handle); }
+};
+
+/**
+ * C++ wrapper around a HAL simulator long value handle.
+ */
+class SimLong : public SimValue {
+ public:
+  /**
+   * Default constructor that results in an "empty" object that is false in
+   * a boolean context.
+   */
+  SimLong() = default;
+
+  /**
+   * Wraps a simulated value handle as returned by HAL_CreateSimValueLong().
+   *
+   * @param handle simulated value handle
+   */
+  /*implicit*/ SimLong(HAL_SimValueHandle val)  // NOLINT
+      : SimValue(val) {}
+
+  /**
+   * Gets the simulated value.
+   *
+   * @return The current value
+   */
+  int64_t Get() const { return HAL_GetSimValueLong(m_handle); }
+
+  /**
+   * Sets the simulated value.
+   *
+   * @param value the value to set
+   */
+  void Set(int64_t value) { HAL_SetSimValueLong(m_handle, value); }
+
+  /**
+   * Resets the simulated value to 0. Use this instead of Set(0) for resetting
+   * incremental sensor values like encoder counts or gyro accumulated angle
+   * to ensure correct behavior in a distributed system (e.g. WebSockets).
+   */
+  void Reset() { HAL_ResetSimValue(m_handle); }
+};
+
+/**
  * C++ wrapper around a HAL simulator double value handle.
  */
 class SimDouble : public SimValue {
@@ -328,7 +537,7 @@
    *
    * @param handle simulated value handle
    */
-  /*implicit*/ SimDouble(HAL_SimValueHandle val)  // NOLINT(runtime/explicit)
+  /*implicit*/ SimDouble(HAL_SimValueHandle val)  // NOLINT
       : SimValue(val) {}
 
   /**
@@ -344,6 +553,13 @@
    * @param value the value to set
    */
   void Set(double value) { HAL_SetSimValueDouble(m_handle, value); }
+
+  /**
+   * Resets the simulated value to 0. Use this instead of Set(0) for resetting
+   * incremental sensor values like encoder counts or gyro accumulated angle
+   * to ensure correct behavior in a distributed system (e.g. WebSockets).
+   */
+  void Reset() { HAL_ResetSimValue(m_handle); }
 };
 
 /**
@@ -362,7 +578,7 @@
    *
    * @param handle simulated value handle
    */
-  /*implicit*/ SimEnum(HAL_SimValueHandle val)  // NOLINT(runtime/explicit)
+  /*implicit*/ SimEnum(HAL_SimValueHandle val)  // NOLINT
       : SimValue(val) {}
 
   /**
@@ -396,7 +612,7 @@
    *
    * @param handle simulated value handle
    */
-  /*implicit*/ SimBoolean(HAL_SimValueHandle val)  // NOLINT(runtime/explicit)
+  /*implicit*/ SimBoolean(HAL_SimValueHandle val)  // NOLINT
       : SimValue(val) {}
 
   /**
@@ -420,6 +636,15 @@
 class SimDevice {
  public:
   /**
+   * Direction of a simulated value (from the perspective of user code).
+   */
+  enum Direction {
+    kInput = HAL_SimValueInput,
+    kOutput = HAL_SimValueOutput,
+    kBidir = HAL_SimValueBidir
+  };
+
+  /**
    * Default constructor that results in an "empty" object that is false in
    * a boolean context.
    */
@@ -474,7 +699,9 @@
   SimDevice(const char* name, int index, int channel);
 
   ~SimDevice() {
-    if (m_handle != HAL_kInvalidHandle) HAL_FreeSimDevice(m_handle);
+    if (m_handle != HAL_kInvalidHandle) {
+      HAL_FreeSimDevice(m_handle);
+    }
   }
 
   SimDevice(const SimDevice&) = delete;
@@ -503,7 +730,7 @@
    *
    * @return internal handle
    */
-  operator HAL_SimDeviceHandle() const { return m_handle; }
+  operator HAL_SimDeviceHandle() const { return m_handle; }  // NOLINT
 
   /**
    * Creates a value on the simulated device.
@@ -512,13 +739,44 @@
    * in a boolean context.
    *
    * @param name value name
-   * @param readonly if the value should not be written from simulation side
+   * @param direction input/output/bidir (from perspective of user code)
    * @param initialValue initial value
    * @return simulated value object
    */
-  SimValue CreateValue(const char* name, bool readonly,
+  SimValue CreateValue(const char* name, int32_t direction,
                        const HAL_Value& initialValue) {
-    return HAL_CreateSimValue(m_handle, name, readonly, &initialValue);
+    return HAL_CreateSimValue(m_handle, name, direction, &initialValue);
+  }
+
+  /**
+   * Creates an int value on the simulated device.
+   *
+   * If not in simulation, results in an "empty" object that evaluates to false
+   * in a boolean context.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated double value object
+   */
+  SimInt CreateInt(const char* name, int32_t direction, int32_t initialValue) {
+    return HAL_CreateSimValueInt(m_handle, name, direction, initialValue);
+  }
+
+  /**
+   * Creates a long value on the simulated device.
+   *
+   * If not in simulation, results in an "empty" object that evaluates to false
+   * in a boolean context.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param initialValue initial value
+   * @return simulated double value object
+   */
+  SimLong CreateLong(const char* name, int32_t direction,
+                     int64_t initialValue) {
+    return HAL_CreateSimValueLong(m_handle, name, direction, initialValue);
   }
 
   /**
@@ -528,12 +786,13 @@
    * in a boolean context.
    *
    * @param name value name
-   * @param readonly if the value should not be written from simulation side
+   * @param direction input/output/bidir (from perspective of user code)
    * @param initialValue initial value
    * @return simulated double value object
    */
-  SimDouble CreateDouble(const char* name, bool readonly, double initialValue) {
-    return HAL_CreateSimValueDouble(m_handle, name, readonly, initialValue);
+  SimDouble CreateDouble(const char* name, int32_t direction,
+                         double initialValue) {
+    return HAL_CreateSimValueDouble(m_handle, name, direction, initialValue);
   }
 
   /**
@@ -545,15 +804,15 @@
    * in a boolean context.
    *
    * @param name value name
-   * @param readonly if the value should not be written from simulation side
+   * @param direction input/output/bidir (from perspective of user code)
    * @param options array of option descriptions
    * @param initialValue initial value (selection)
    * @return simulated enum value object
    */
-  SimEnum CreateEnum(const char* name, bool readonly,
+  SimEnum CreateEnum(const char* name, int32_t direction,
                      std::initializer_list<const char*> options,
                      int32_t initialValue) {
-    return HAL_CreateSimValueEnum(m_handle, name, readonly, options.size(),
+    return HAL_CreateSimValueEnum(m_handle, name, direction, options.size(),
                                   const_cast<const char**>(options.begin()),
                                   initialValue);
   }
@@ -567,31 +826,91 @@
    * in a boolean context.
    *
    * @param name value name
-   * @param readonly if the value should not be written from simulation side
+   * @param direction input/output/bidir (from perspective of user code)
    * @param options array of option descriptions
    * @param initialValue initial value (selection)
    * @return simulated enum value object
    */
-  SimEnum CreateEnum(const char* name, bool readonly,
-                     wpi::ArrayRef<const char*> options, int32_t initialValue) {
-    return HAL_CreateSimValueEnum(m_handle, name, readonly, options.size(),
+  SimEnum CreateEnum(const char* name, int32_t direction,
+                     wpi::span<const char* const> options,
+                     int32_t initialValue) {
+    return HAL_CreateSimValueEnum(m_handle, name, direction, options.size(),
                                   const_cast<const char**>(options.data()),
                                   initialValue);
   }
 
   /**
+   * Creates an enumerated value on the simulated device with double values.
+   *
+   * Enumerated values are always in the range 0 to numOptions-1.
+   *
+   * If not in simulation, results in an "empty" object that evaluates to false
+   * in a boolean context.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param options array of option descriptions
+   * @param optionValues array of option values (must be the same size as
+   *                     options)
+   * @param initialValue initial value (selection)
+   * @return simulated enum value object
+   */
+  SimEnum CreateEnumDouble(const char* name, int32_t direction,
+                           std::initializer_list<const char*> options,
+                           std::initializer_list<double> optionValues,
+                           int32_t initialValue) {
+    if (options.size() != optionValues.size()) {
+      return {};
+    }
+    return HAL_CreateSimValueEnumDouble(
+        m_handle, name, direction, options.size(),
+        const_cast<const char**>(options.begin()), optionValues.begin(),
+        initialValue);
+  }
+
+  /**
+   * Creates an enumerated value on the simulated device with double values.
+   *
+   * Enumerated values are always in the range 0 to numOptions-1.
+   *
+   * If not in simulation, results in an "empty" object that evaluates to false
+   * in a boolean context.
+   *
+   * @param name value name
+   * @param direction input/output/bidir (from perspective of user code)
+   * @param options array of option descriptions
+   * @param optionValues array of option values (must be the same size as
+   *                     options)
+   * @param initialValue initial value (selection)
+   * @return simulated enum value object
+   */
+  SimEnum CreateEnumDouble(const char* name, int32_t direction,
+                           wpi::span<const char* const> options,
+                           wpi::span<const double> optionValues,
+                           int32_t initialValue) {
+    if (options.size() != optionValues.size()) {
+      return {};
+    }
+    return HAL_CreateSimValueEnumDouble(
+        m_handle, name, direction, options.size(),
+        const_cast<const char**>(options.data()), optionValues.data(),
+        initialValue);
+  }
+
+  /**
    * Creates a boolean value on the simulated device.
    *
    * If not in simulation, results in an "empty" object that evaluates to false
    * in a boolean context.
    *
    * @param name value name
-   * @param readonly if the value should not be written from simulation side
+   * @param direction input/output/bidir (from perspective of user code)
    * @param initialValue initial value
    * @return simulated boolean value object
    */
-  SimBoolean CreateBoolean(const char* name, bool readonly, bool initialValue) {
-    return HAL_CreateSimValueBoolean(m_handle, name, readonly, initialValue);
+  SimBoolean CreateBoolean(const char* name, int32_t direction,
+                           bool initialValue) {
+    return HAL_CreateSimValueBoolean(m_handle, name, direction, initialValue);
   }
 
  protected:
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Solenoid.h b/third_party/allwpilib/hal/src/main/native/include/hal/Solenoid.h
deleted file mode 100644
index 53257b2..0000000
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Solenoid.h
+++ /dev/null
@@ -1,141 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <stdint.h>
-
-#include "hal/Types.h"
-
-/**
- * @defgroup hal_solenoid Solenoid Output Functions
- * @ingroup hal_capi
- * @{
- */
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/**
- * Initializes a solenoid port.
- *
- * @param portHandle the port handle of the module and channel to initialize
- * @return           the created solenoid handle
- */
-HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
-                                              int32_t* status);
-
-/**
- * Frees a solenoid port.
- *
- * @param solenoidPortHandle the solenoid handle
- */
-void HAL_FreeSolenoidPort(HAL_SolenoidHandle solenoidPortHandle);
-
-/**
- * Checks if a solenoid module is in the valid range.
- *
- * @param module the module number to check
- * @return       true if the module number is valid, otherwise false
- */
-HAL_Bool HAL_CheckSolenoidModule(int32_t module);
-
-/**
- * Checks if a solenoid channel is in the valid range.
- *
- * @param channel the channel number to check
- * @return       true if the channel number is valid, otherwise false
- */
-HAL_Bool HAL_CheckSolenoidChannel(int32_t channel);
-
-/**
- * Gets the current solenoid output value.
- *
- * @param solenoidPortHandle the solenoid handle
- * @return                   true if the solenoid is on, otherwise false
- */
-HAL_Bool HAL_GetSolenoid(HAL_SolenoidHandle solenoidPortHandle,
-                         int32_t* status);
-
-/**
- * Gets the status of all solenoids on a specific module.
- *
- * @param module the module to check
- * @return       bitmask of the channels, 1 for on 0 for off
- */
-int32_t HAL_GetAllSolenoids(int32_t module, int32_t* status);
-
-/**
- * Sets a solenoid output value.
- *
- * @param solenoidPortHandle the solenoid handle
- * @param value              true for on, false for off
- */
-void HAL_SetSolenoid(HAL_SolenoidHandle solenoidPortHandle, HAL_Bool value,
-                     int32_t* status);
-
-/**
- * Sets all channels on a specific module.
- *
- * @param module the module to set the channels on
- * @param state  bitmask of the channels to set, 1 for on 0 for off
- */
-void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status);
-
-/**
- * Gets the channels blacklisted from being enabled on a module.
- *
- * @param module the module to check
- * @retur        bitmask of the blacklisted channels, 1 for true 0 for false
- */
-int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status);
-
-/**
- * Gets if a specific module has an over or under voltage sticky fault.
- *
- * @param module the module to check
- * @return       true if a stick fault is set, otherwise false
- */
-HAL_Bool HAL_GetPCMSolenoidVoltageStickyFault(int32_t module, int32_t* status);
-
-/**
- * Gets if a specific module has an over or under voltage fault.
- *
- * @param module the module to check
- * @return       true if faulted, otherwise false
- */
-HAL_Bool HAL_GetPCMSolenoidVoltageFault(int32_t module, int32_t* status);
-
-/**
- * Clears all faults on a module.
- *
- * @param module the module to clear
- */
-void HAL_ClearAllPCMStickyFaults(int32_t module, int32_t* status);
-
-/**
- * Sets the one shot duration on a solenoid channel.
- *
- * @param solenoidPortHandle the solenoid handle
- * @param durMS              the one shot duration in ms
- */
-void HAL_SetOneShotDuration(HAL_SolenoidHandle solenoidPortHandle,
-                            int32_t durMS, int32_t* status);
-
-/**
- * Fires a single pulse on a solenoid channel.
- *
- * The pulse is the duration set by HAL_SetOneShotDuration().
- *
- * @param solenoidPortHandle the solenoid handle
- */
-void HAL_FireOneShot(HAL_SolenoidHandle solenoidPortHandle, int32_t* status);
-#ifdef __cplusplus
-}  // extern "C"
-#endif
-/** @} */
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Threads.h b/third_party/allwpilib/hal/src/main/native/include/hal/Threads.h
index aea4399..a6db5e0 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Threads.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Threads.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,11 +21,13 @@
 /**
  * Gets the thread priority for the specified thread.
  *
- * @param handle     Native handle pointer to the thread to get the priority for
- * @param isRealTime Set to true if thread is realtime, otherwise false
- * @param status     Error status variable. 0 on success
- * @return           The current thread priority. Scaled 1-99, with 1 being
- * highest.
+ * @param[in] handle      Native handle pointer to the thread to get the
+ *                        priority for.
+ * @param[out] isRealTime Set to true if thread is real-time, otherwise false.
+ * @param[out] status     Error status variable. 0 on success.
+ * @return The current thread priority. For real-time, this is 1-99 with 99
+ *         being highest. For non-real-time, this is 0. See "man 7 sched" for
+ *         details.
  */
 int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
                               int32_t* status);
@@ -36,24 +35,25 @@
 /**
  * Gets the thread priority for the current thread.
  *
- * @param handle     Native handle pointer to the thread to get the priority for
- * @param isRealTime Set to true if thread is realtime, otherwise false
- * @param status     Error status variable. 0 on success
- * @return           The current thread priority. Scaled 1-99, with 1 being
- * highest.
+ * @param[out] isRealTime Set to true if thread is real-time, otherwise false.
+ * @param[out] status     Error status variable. 0 on success.
+ * @return The current thread priority. For real-time, this is 1-99 with 99
+ *         being highest. For non-real-time, this is 0. See "man 7 sched" for
+ *         details.
  */
 int32_t HAL_GetCurrentThreadPriority(HAL_Bool* isRealTime, int32_t* status);
 
 /**
  * Sets the thread priority for the specified thread.
  *
- * @param thread   Reference to the thread to set the priority of
- * @param realTime Set to true to set a realtime priority, false for standard
- * priority
- * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
- * highest
- * @param status   Error status variable. 0 on success
- * @return         The success state of setting the priority
+ * @param[in] handle   Reference to the thread to set the priority of.
+ * @param[in] realTime Set to true to set a real-time priority, false for
+ *                     standard priority.
+ * @param[in] priority Priority to set the thread to. For real-time, this is
+ *                     1-99 with 99 being highest. For non-real-time, this is
+ *                     forced to 0. See "man 7 sched" for more details.
+ * @param[out] status  Error status variable. 0 on success.
+ * @return True on success.
  */
 HAL_Bool HAL_SetThreadPriority(NativeThreadHandle handle, HAL_Bool realTime,
                                int32_t priority, int32_t* status);
@@ -61,13 +61,13 @@
 /**
  * Sets the thread priority for the current thread.
  *
- * @param thread   Reference to the thread to set the priority of
- * @param realTime Set to true to set a realtime priority, false for standard
- * priority
- * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
- * highest
- * @param status   Error status variable. 0 on success
- * @return         The success state of setting the priority
+ * @param[in] realTime Set to true to set a real-time priority, false for
+ *                     standard priority.
+ * @param[in] priority Priority to set the thread to. For real-time, this is
+ *                     1-99 with 99 being highest. For non-real-time, this is
+ *                     forced to 0. See "man 7 sched" for more details.
+ * @param[out] status  Error status variable. 0 on success.
+ * @return True on success.
  */
 HAL_Bool HAL_SetCurrentThreadPriority(HAL_Bool realTime, int32_t priority,
                                       int32_t* status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Types.h b/third_party/allwpilib/hal/src/main/native/include/hal/Types.h
index 1ebbe2d..6b95447 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Types.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Types.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -65,6 +62,14 @@
 
 typedef HAL_CANHandle HAL_PDPHandle;
 
+typedef HAL_Handle HAL_PowerDistributionHandle;
+
+typedef HAL_Handle HAL_CTREPCMHandle;
+
+typedef HAL_Handle HAL_REVPDHHandle;
+
+typedef HAL_Handle HAL_REVPHHandle;
+
 typedef int32_t HAL_Bool;
 
 #ifdef __cplusplus
@@ -86,7 +91,7 @@
 class Handle {
  public:
   Handle() = default;
-  /*implicit*/ Handle(CType val) : m_handle(val) {}  // NOLINT(runtime/explicit)
+  /*implicit*/ Handle(CType val) : m_handle(val) {}  // NOLINT
 
   Handle(const Handle&) = delete;
   Handle& operator=(const Handle&) = delete;
@@ -99,7 +104,7 @@
     return *this;
   }
 
-  operator CType() const { return m_handle; }
+  operator CType() const { return m_handle; }  // NOLINT
 
  private:
   CType m_handle = CInvalid;
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/Value.h b/third_party/allwpilib/hal/src/main/native/include/hal/Value.h
index 578d989..ff72ee7 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/Value.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/Value.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/cpp/SerialHelper.h b/third_party/allwpilib/hal/src/main/native/include/hal/cpp/SerialHelper.h
index 9f4d6a0..13b6857 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/cpp/SerialHelper.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/cpp/SerialHelper.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -29,34 +26,34 @@
   /**
    * Get the VISA name of a serial port.
    *
-   * @param port   the serial port index
-   * @param status status check
-   * @return       the VISA name
+   * @param[in] port    the serial port index
+   * @param[out] status status check
+   * @return the VISA name
    */
   std::string GetVISASerialPortName(HAL_SerialPort port, int32_t* status);
 
   /**
    * Get the OS name of a serial port.
    *
-   * @param port   the serial port index
-   * @param status status check
-   * @return       the OS name
+   * @param[in] port    the serial port index
+   * @param[out] status status check
+   * @return the OS name
    */
   std::string GetOSSerialPortName(HAL_SerialPort port, int32_t* status);
 
   /**
    * Get a vector of all serial port VISA names.
    *
-   * @param status status check
-   * @return       vector of serial port VISA names
+   * @param[out] status status check
+   * @return vector of serial port VISA names
    */
   std::vector<std::string> GetVISASerialPortList(int32_t* status);
 
   /**
    * Get a vector of all serial port OS names.
    *
-   * @param status status check
-   * @return       vector of serial port OS names
+   * @param[out] status status check
+   * @return vector of serial port OS names
    */
   std::vector<std::string> GetOSSerialPortList(int32_t* status);
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/cpp/UnsafeDIO.h b/third_party/allwpilib/hal/src/main/native/include/hal/cpp/UnsafeDIO.h
index dad5eb7..eb7f231 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/cpp/UnsafeDIO.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/cpp/UnsafeDIO.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -61,9 +58,9 @@
  * functions on the Proxy object passed as a parameter can deadlock your
  * program.
  *
- * @param handle the HAL digital handle of the pin to toggle.
- * @param status status check
- * @param func   A functor taking a ref to a DIOSetProxy object.
+ * @param[in] handle the HAL digital handle of the pin to toggle.
+ * @param[out] status status check
+ * @param[in] func   A functor taking a ref to a DIOSetProxy object.
  */
 template <typename Functor>
 void UnsafeManipulateDIO(HAL_DigitalHandle handle, int32_t* status,
@@ -76,7 +73,9 @@
   wpi::mutex& dioMutex = detail::UnsafeGetDIOMutex();
   tDIO* dSys = detail::UnsafeGetDigialSystem();
   auto mask = detail::ComputeDigitalMask(handle, status);
-  if (status != 0) return;
+  if (*status != 0) {
+    return;
+  }
   std::scoped_lock lock(dioMutex);
 
   tDIO::tOutputEnable enableOE = dSys->readOutputEnable(status);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/cpp/fpga_clock.h b/third_party/allwpilib/hal/src/main/native/include/hal/cpp/fpga_clock.h
index f6d5c6c..0f7bb75 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/cpp/fpga_clock.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/cpp/fpga_clock.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/DigitalHandleResource.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
index 5fd8506..5154a43 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/DigitalHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -25,7 +22,7 @@
  * allows a limited number of handles that are allocated by index.
  * The enum value is separate, as 2 enum values are allowed per handle
  * Because they are allocated by index, each individual index holds its own
- * mutex, which reduces contention heavily.]
+ * mutex, which reduces contention heavily.
  *
  * @tparam THandle The Handle Type (Must be typedefed from HAL_Handle)
  * @tparam TStruct The struct type held by this resource
@@ -41,7 +38,8 @@
   DigitalHandleResource(const DigitalHandleResource&) = delete;
   DigitalHandleResource& operator=(const DigitalHandleResource&) = delete;
 
-  THandle Allocate(int16_t index, HAL_HandleEnum enumValue, int32_t* status);
+  std::shared_ptr<TStruct> Allocate(int16_t index, HAL_HandleEnum enumValue,
+                                    THandle* handle, int32_t* status);
   int16_t GetIndex(THandle handle, HAL_HandleEnum enumValue) {
     return getHandleTypedIndex(handle, enumValue, m_version);
   }
@@ -55,21 +53,27 @@
 };
 
 template <typename THandle, typename TStruct, int16_t size>
-THandle DigitalHandleResource<THandle, TStruct, size>::Allocate(
-    int16_t index, HAL_HandleEnum enumValue, int32_t* status) {
+std::shared_ptr<TStruct>
+DigitalHandleResource<THandle, TStruct, size>::Allocate(
+    int16_t index, HAL_HandleEnum enumValue, THandle* handle, int32_t* status) {
   // don't acquire the lock if we can fail early.
   if (index < 0 || index >= size) {
+    *handle = HAL_kInvalidHandle;
     *status = RESOURCE_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
+    return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
   // check for allocation, otherwise allocate and return a valid handle
   if (m_structures[index] != nullptr) {
+    *handle = HAL_kInvalidHandle;
     *status = RESOURCE_IS_ALLOCATED;
-    return HAL_kInvalidHandle;
+    return m_structures[index];
   }
   m_structures[index] = std::make_shared<TStruct>();
-  return static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+  *handle =
+      static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+  *status = HAL_SUCCESS;
+  return m_structures[index];
 }
 
 template <typename THandle, typename TStruct, int16_t size>
@@ -91,7 +95,9 @@
     THandle handle, HAL_HandleEnum enumValue) {
   // get handle index, and fail early if index out of range or wrong handle
   int16_t index = GetIndex(handle, enumValue);
-  if (index < 0 || index >= size) return;
+  if (index < 0 || index >= size) {
+    return;
+  }
   // lock and deallocated handle
   std::scoped_lock lock(m_handleMutexes[index]);
   m_structures[index].reset();
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/HandlesInternal.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/HandlesInternal.h
index 511433e..13a56db 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/HandlesInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/HandlesInternal.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
+#include <wpi/Synchronization.h>
+
 #include "hal/Types.h"
 
 /* General Handle Data Layout
@@ -46,7 +45,7 @@
  */
 enum class HAL_HandleEnum {
   Undefined = 0,
-  DIO = 1,
+  DIO = wpi::kHandleTypeHALBase,
   Port = 2,
   Notifier = 3,
   Interrupt = 4,
@@ -69,13 +68,17 @@
   DutyCycle = 21,
   DMA = 22,
   AddressableLED = 23,
+  CTREPCM = 24,
+  CTREPDP = 25,
+  REVPDH = 26,
+  REVPH = 27,
 };
 
 /**
  * Get the handle index from a handle.
  *
  * @param handle the handle
- * @return       the index
+ * @return the index
  */
 static inline int16_t getHandleIndex(HAL_Handle handle) {
   // mask and return last 16 bits
@@ -86,7 +89,7 @@
  * Get the handle type from a handle.
  *
  * @param handle the handle
- * @return       the type
+ * @return the type
  */
 static inline HAL_HandleEnum getHandleType(HAL_Handle handle) {
   // mask first 8 bits and cast to enum
@@ -98,7 +101,7 @@
  *
  * @param handle     the handle
  * @param handleType the type to check
- * @return           true if the type is correct, otherwise false
+ * @return true if the type is correct, otherwise false
  */
 static inline bool isHandleType(HAL_Handle handle, HAL_HandleEnum handleType) {
   return handleType == getHandleType(handle);
@@ -111,7 +114,7 @@
  *
  * @param handle  the handle
  * @param version the handle version to check
- * @return        true if the handle is the right version, otherwise false
+ * @return true if the handle is the right version, otherwise false
  */
 static inline bool isHandleCorrectVersion(HAL_Handle handle, int16_t version) {
   return (((handle & 0xFF0000) >> 16) & version) == version;
@@ -123,17 +126,20 @@
  * Note the version is not checked on the roboRIO.
  *
  * @param handle     the handle
- * @param handleType the type to check
+ * @param enumType   the type to check
  * @param version    the handle version to check
- * @return           true if the handle is proper version and type, otherwise
- * false.
+ * @return true if the handle is proper version and type, otherwise
+ *         false.
  */
-static inline int16_t getHandleTypedIndex(HAL_Handle handle,
-                                          HAL_HandleEnum enumType,
-                                          int16_t version) {
-  if (!isHandleType(handle, enumType)) return InvalidHandleIndex;
+inline int16_t getHandleTypedIndex(HAL_Handle handle, HAL_HandleEnum enumType,
+                                   int16_t version) {
+  if (!isHandleType(handle, enumType)) {
+    return InvalidHandleIndex;
+  }
 #if !defined(__FRC_ROBORIO__)
-  if (!isHandleCorrectVersion(handle, version)) return InvalidHandleIndex;
+  if (!isHandleCorrectVersion(handle, version)) {
+    return InvalidHandleIndex;
+  }
 #endif
   return getHandleIndex(handle);
 }
@@ -152,10 +158,12 @@
  * Gets the port channel of a port handle.
  *
  * @param handle the port handle
- * @return       the port channel
+ * @return the port channel
  */
-static inline int16_t getPortHandleChannel(HAL_PortHandle handle) {
-  if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex;
+inline int16_t getPortHandleChannel(HAL_PortHandle handle) {
+  if (!isHandleType(handle, HAL_HandleEnum::Port)) {
+    return InvalidHandleIndex;
+  }
   return static_cast<uint8_t>(handle & 0xff);
 }
 
@@ -164,10 +172,12 @@
  * Gets the port module of a port handle.
  *
  * @param handle the port handle
- * @return       the port module
+ * @return the port module
  */
-static inline int16_t getPortHandleModule(HAL_PortHandle handle) {
-  if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex;
+inline int16_t getPortHandleModule(HAL_PortHandle handle) {
+  if (!isHandleType(handle, HAL_HandleEnum::Port)) {
+    return InvalidHandleIndex;
+  }
   return static_cast<uint8_t>((handle >> 8) & 0xff);
 }
 
@@ -176,10 +186,12 @@
  * Gets the SPI channel of a port handle.
  *
  * @param handle the port handle
- * @return       the port SPI channel
+ * @return the port SPI channel
  */
-static inline int16_t getPortHandleSPIEnable(HAL_PortHandle handle) {
-  if (!isHandleType(handle, HAL_HandleEnum::Port)) return InvalidHandleIndex;
+inline int16_t getPortHandleSPIEnable(HAL_PortHandle handle) {
+  if (!isHandleType(handle, HAL_HandleEnum::Port)) {
+    return InvalidHandleIndex;
+  }
   return static_cast<uint8_t>((handle >> 16) & 0xff);
 }
 
@@ -188,7 +200,7 @@
  *
  * @param channel the channel
  * @param module  the module
- * @return        port handle for the module and channel
+ * @return port handle for the module and channel
  */
 HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module);
 
@@ -196,7 +208,7 @@
  * Create a port handle for SPI.
  *
  * @param channel the SPI channel
- * @return        port handle for the channel
+ * @return port handle for the channel
  */
 HAL_PortHandle createPortHandleForSPI(uint8_t channel);
 
@@ -208,7 +220,7 @@
  * @param index      the index
  * @param handleType the handle type
  * @param version    the handle version
- * @return           the created handle
+ * @return the created handle
  */
 HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
                         int16_t version);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
index 902f023..d96de2b 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedClassedHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -52,7 +49,7 @@
   }
   std::shared_ptr<TStruct> Get(THandle handle);
   void Free(THandle handle);
-  void ResetHandles();
+  void ResetHandles() override;
 
  private:
   std::array<std::shared_ptr<TStruct>, size> m_structures;
@@ -101,7 +98,9 @@
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
   int16_t index = GetIndex(handle);
-  if (index < 0 || index >= size) return;
+  if (index < 0 || index >= size) {
+    return;
+  }
   // lock and deallocated handle
   std::scoped_lock lock(m_handleMutexes[index]);
   m_structures[index].reset();
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedHandleResource.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
index 26fda02..2e921ee 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/IndexedHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -42,7 +39,8 @@
   IndexedHandleResource(const IndexedHandleResource&) = delete;
   IndexedHandleResource& operator=(const IndexedHandleResource&) = delete;
 
-  THandle Allocate(int16_t index, int32_t* status);
+  std::shared_ptr<TStruct> Allocate(int16_t index, THandle* handle,
+                                    int32_t* status);
   int16_t GetIndex(THandle handle) {
     return getHandleTypedIndex(handle, enumValue, m_version);
   }
@@ -57,21 +55,27 @@
 
 template <typename THandle, typename TStruct, int16_t size,
           HAL_HandleEnum enumValue>
-THandle IndexedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
-    int16_t index, int32_t* status) {
+std::shared_ptr<TStruct>
+IndexedHandleResource<THandle, TStruct, size, enumValue>::Allocate(
+    int16_t index, THandle* handle, int32_t* status) {
   // don't acquire the lock if we can fail early.
   if (index < 0 || index >= size) {
     *status = RESOURCE_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
+    *handle = HAL_kInvalidHandle;
+    return nullptr;
   }
   std::scoped_lock lock(m_handleMutexes[index]);
   // check for allocation, otherwise allocate and return a valid handle
   if (m_structures[index] != nullptr) {
     *status = RESOURCE_IS_ALLOCATED;
-    return HAL_kInvalidHandle;
+    *handle = HAL_kInvalidHandle;
+    return m_structures[index];
   }
   m_structures[index] = std::make_shared<TStruct>();
-  return static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+  *handle =
+      static_cast<THandle>(hal::createHandle(index, enumValue, m_version));
+  *status = HAL_SUCCESS;
+  return m_structures[index];
 }
 
 template <typename THandle, typename TStruct, int16_t size,
@@ -95,7 +99,9 @@
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
   int16_t index = GetIndex(handle);
-  if (index < 0 || index >= size) return;
+  if (index < 0 || index >= size) {
+    return;
+  }
   // lock and deallocated handle
   std::scoped_lock lock(m_handleMutexes[index]);
   m_structures[index].reset();
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
index 0ab8aac..2650708 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedClassedHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -96,7 +93,9 @@
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
   int16_t index = GetIndex(handle);
-  if (index < 0 || index >= size) return;
+  if (index < 0 || index >= size) {
+    return;
+  }
   // lock and deallocated handle
   std::scoped_lock allocateLock(m_allocateMutex);
   std::scoped_lock handleLock(m_handleMutexes[index]);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedHandleResource.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
index 2f7ed0d..6f76293 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/LimitedHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -91,7 +88,9 @@
     THandle handle) {
   // get handle index, and fail early if index out of range or wrong handle
   int16_t index = GetIndex(handle);
-  if (index < 0 || index >= size) return;
+  if (index < 0 || index >= size) {
+    return;
+  }
   // lock and deallocated handle
   std::scoped_lock allocateLock(m_allocateMutex);
   std::scoped_lock handleLock(m_handleMutexes[index]);
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h b/third_party/allwpilib/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
index 5f74b88..09510ac 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/handles/UnlimitedHandleResource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -74,7 +71,9 @@
       return static_cast<THandle>(createHandle(i, enumValue, m_version));
     }
   }
-  if (i >= INT16_MAX) return HAL_kInvalidHandle;
+  if (i >= INT16_MAX) {
+    return HAL_kInvalidHandle;
+  }
 
   m_structures.push_back(structure);
   return static_cast<THandle>(
@@ -86,8 +85,9 @@
 UnlimitedHandleResource<THandle, TStruct, enumValue>::Get(THandle handle) {
   int16_t index = GetIndex(handle);
   std::scoped_lock lock(m_handleMutex);
-  if (index < 0 || index >= static_cast<int16_t>(m_structures.size()))
+  if (index < 0 || index >= static_cast<int16_t>(m_structures.size())) {
     return nullptr;
+  }
   return m_structures[index];
 }
 
@@ -96,8 +96,9 @@
 UnlimitedHandleResource<THandle, TStruct, enumValue>::Free(THandle handle) {
   int16_t index = GetIndex(handle);
   std::scoped_lock lock(m_handleMutex);
-  if (index < 0 || index >= static_cast<int16_t>(m_structures.size()))
+  if (index < 0 || index >= static_cast<int16_t>(m_structures.size())) {
     return nullptr;
+  }
   return std::move(m_structures[index]);
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AccelerometerData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AccelerometerData.h
index 0a92671..70cee61 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AccelerometerData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AccelerometerData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AddressableLEDData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AddressableLEDData.h
index 91ab30c..d828e5c 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AddressableLEDData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AddressableLEDData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogGyroData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogGyroData.h
index 91e684e..9527140 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogGyroData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogGyroData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogInData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogInData.h
index 9ab2d3b..2e2d8be 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogInData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogInData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogOutData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogOutData.h
index fa1413d..00275c6 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogOutData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogOutData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h
index 74c762c..fb232a2 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/AnalogTriggerData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CTREPCMData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CTREPCMData.h
new file mode 100644
index 0000000..302f7dd
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CTREPCMData.h
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetCTREPCMData(int32_t index);
+int32_t HALSIM_RegisterCTREPCMInitializedCallback(int32_t index,
+                                                  HAL_NotifyCallback callback,
+                                                  void* param,
+                                                  HAL_Bool initialNotify);
+void HALSIM_CancelCTREPCMInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetCTREPCMInitialized(int32_t index);
+void HALSIM_SetCTREPCMInitialized(int32_t index, HAL_Bool solenoidInitialized);
+
+int32_t HALSIM_RegisterCTREPCMSolenoidOutputCallback(
+    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelCTREPCMSolenoidOutputCallback(int32_t index, int32_t channel,
+                                                int32_t uid);
+HAL_Bool HALSIM_GetCTREPCMSolenoidOutput(int32_t index, int32_t channel);
+void HALSIM_SetCTREPCMSolenoidOutput(int32_t index, int32_t channel,
+                                     HAL_Bool solenoidOutput);
+
+int32_t HALSIM_RegisterCTREPCMCompressorOnCallback(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelCTREPCMCompressorOnCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetCTREPCMCompressorOn(int32_t index);
+void HALSIM_SetCTREPCMCompressorOn(int32_t index, HAL_Bool compressorOn);
+
+int32_t HALSIM_RegisterCTREPCMClosedLoopEnabledCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelCTREPCMClosedLoopEnabledCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetCTREPCMClosedLoopEnabled(int32_t index);
+void HALSIM_SetCTREPCMClosedLoopEnabled(int32_t index,
+                                        HAL_Bool closedLoopEnabled);
+
+int32_t HALSIM_RegisterCTREPCMPressureSwitchCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelCTREPCMPressureSwitchCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetCTREPCMPressureSwitch(int32_t index);
+void HALSIM_SetCTREPCMPressureSwitch(int32_t index, HAL_Bool pressureSwitch);
+
+int32_t HALSIM_RegisterCTREPCMCompressorCurrentCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelCTREPCMCompressorCurrentCallback(int32_t index, int32_t uid);
+double HALSIM_GetCTREPCMCompressorCurrent(int32_t index);
+void HALSIM_SetCTREPCMCompressorCurrent(int32_t index,
+                                        double compressorCurrent);
+
+void HALSIM_GetCTREPCMAllSolenoids(int32_t index, uint8_t* values);
+void HALSIM_SetCTREPCMAllSolenoids(int32_t index, uint8_t values);
+
+void HALSIM_RegisterCTREPCMAllNonSolenoidCallbacks(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+
+void HALSIM_RegisterCTREPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CanData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CanData.h
index eb5ea63..2590162 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CanData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/CanData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DIOData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DIOData.h
index ddda655..d2d7181 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DIOData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DIOData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DigitalPWMData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DigitalPWMData.h
index 79428b0..1010811 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DigitalPWMData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DigitalPWMData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DriverStationData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DriverStationData.h
index 536738b..87223ec 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DriverStationData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DriverStationData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DutyCycleData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DutyCycleData.h
index 7b191f28..bbe5bf2 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DutyCycleData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/DutyCycleData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/EncoderData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/EncoderData.h
index 80c142d..df1c12b 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/EncoderData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/EncoderData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/I2CData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/I2CData.h
index 3d72850..d5e13f9 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/I2CData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/I2CData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/MockHooks.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/MockHooks.h
index cef205e..330f72e 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/MockHooks.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/MockHooks.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -30,4 +27,13 @@
 typedef int32_t (*HALSIM_SendConsoleLineHandler)(const char* line);
 void HALSIM_SetSendConsoleLine(HALSIM_SendConsoleLineHandler handler);
 
+typedef void (*HALSIM_SimPeriodicCallback)(void* param);
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param);
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid);
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param);
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid);
+
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifierData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifierData.h
index a5b68b6..13dcf1d 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifierData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifierData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifyListener.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifyListener.h
index a1430c4..46a93c9 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifyListener.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/NotifyListener.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PCMData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PCMData.h
deleted file mode 100644
index ad2188d..0000000
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PCMData.h
+++ /dev/null
@@ -1,92 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "hal/Types.h"
-#include "hal/simulation/NotifyListener.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetPCMData(int32_t index);
-int32_t HALSIM_RegisterPCMSolenoidInitializedCallback(
-    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelPCMSolenoidInitializedCallback(int32_t index, int32_t channel,
-                                                 int32_t uid);
-HAL_Bool HALSIM_GetPCMSolenoidInitialized(int32_t index, int32_t channel);
-void HALSIM_SetPCMSolenoidInitialized(int32_t index, int32_t channel,
-                                      HAL_Bool solenoidInitialized);
-
-int32_t HALSIM_RegisterPCMSolenoidOutputCallback(int32_t index, int32_t channel,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelPCMSolenoidOutputCallback(int32_t index, int32_t channel,
-                                            int32_t uid);
-HAL_Bool HALSIM_GetPCMSolenoidOutput(int32_t index, int32_t channel);
-void HALSIM_SetPCMSolenoidOutput(int32_t index, int32_t channel,
-                                 HAL_Bool solenoidOutput);
-
-int32_t HALSIM_RegisterPCMCompressorInitializedCallback(
-    int32_t index, HAL_NotifyCallback callback, void* param,
-    HAL_Bool initialNotify);
-void HALSIM_CancelPCMCompressorInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMCompressorInitialized(int32_t index);
-void HALSIM_SetPCMCompressorInitialized(int32_t index,
-                                        HAL_Bool compressorInitialized);
-
-int32_t HALSIM_RegisterPCMCompressorOnCallback(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-void HALSIM_CancelPCMCompressorOnCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMCompressorOn(int32_t index);
-void HALSIM_SetPCMCompressorOn(int32_t index, HAL_Bool compressorOn);
-
-int32_t HALSIM_RegisterPCMClosedLoopEnabledCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelPCMClosedLoopEnabledCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMClosedLoopEnabled(int32_t index);
-void HALSIM_SetPCMClosedLoopEnabled(int32_t index, HAL_Bool closedLoopEnabled);
-
-int32_t HALSIM_RegisterPCMPressureSwitchCallback(int32_t index,
-                                                 HAL_NotifyCallback callback,
-                                                 void* param,
-                                                 HAL_Bool initialNotify);
-void HALSIM_CancelPCMPressureSwitchCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPCMPressureSwitch(int32_t index);
-void HALSIM_SetPCMPressureSwitch(int32_t index, HAL_Bool pressureSwitch);
-
-int32_t HALSIM_RegisterPCMCompressorCurrentCallback(int32_t index,
-                                                    HAL_NotifyCallback callback,
-                                                    void* param,
-                                                    HAL_Bool initialNotify);
-void HALSIM_CancelPCMCompressorCurrentCallback(int32_t index, int32_t uid);
-double HALSIM_GetPCMCompressorCurrent(int32_t index);
-void HALSIM_SetPCMCompressorCurrent(int32_t index, double compressorCurrent);
-
-void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values);
-void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values);
-
-void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify);
-
-void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PDPData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PDPData.h
deleted file mode 100644
index 7ec11a4..0000000
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PDPData.h
+++ /dev/null
@@ -1,59 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "hal/Types.h"
-#include "hal/simulation/NotifyListener.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-void HALSIM_ResetPDPData(int32_t index);
-int32_t HALSIM_RegisterPDPInitializedCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelPDPInitializedCallback(int32_t index, int32_t uid);
-HAL_Bool HALSIM_GetPDPInitialized(int32_t index);
-void HALSIM_SetPDPInitialized(int32_t index, HAL_Bool initialized);
-
-int32_t HALSIM_RegisterPDPTemperatureCallback(int32_t index,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-void HALSIM_CancelPDPTemperatureCallback(int32_t index, int32_t uid);
-double HALSIM_GetPDPTemperature(int32_t index);
-void HALSIM_SetPDPTemperature(int32_t index, double temperature);
-
-int32_t HALSIM_RegisterPDPVoltageCallback(int32_t index,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPDPVoltageCallback(int32_t index, int32_t uid);
-double HALSIM_GetPDPVoltage(int32_t index);
-void HALSIM_SetPDPVoltage(int32_t index, double voltage);
-
-int32_t HALSIM_RegisterPDPCurrentCallback(int32_t index, int32_t channel,
-                                          HAL_NotifyCallback callback,
-                                          void* param, HAL_Bool initialNotify);
-void HALSIM_CancelPDPCurrentCallback(int32_t index, int32_t channel,
-                                     int32_t uid);
-double HALSIM_GetPDPCurrent(int32_t index, int32_t channel);
-void HALSIM_SetPDPCurrent(int32_t index, int32_t channel, double current);
-
-void HALSIM_GetPDPAllCurrents(int32_t index, double* currents);
-void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents);
-
-void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify);
-
-#ifdef __cplusplus
-}  // extern "C"
-#endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PWMData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PWMData.h
index a536710..91d22df 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PWMData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PWMData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PowerDistributionData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PowerDistributionData.h
new file mode 100644
index 0000000..0552c49
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/PowerDistributionData.h
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetPowerDistributionData(int32_t index);
+int32_t HALSIM_RegisterPowerDistributionInitializedCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelPowerDistributionInitializedCallback(int32_t index,
+                                                       int32_t uid);
+HAL_Bool HALSIM_GetPowerDistributionInitialized(int32_t index);
+void HALSIM_SetPowerDistributionInitialized(int32_t index,
+                                            HAL_Bool initialized);
+
+int32_t HALSIM_RegisterPowerDistributionTemperatureCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelPowerDistributionTemperatureCallback(int32_t index,
+                                                       int32_t uid);
+double HALSIM_GetPowerDistributionTemperature(int32_t index);
+void HALSIM_SetPowerDistributionTemperature(int32_t index, double temperature);
+
+int32_t HALSIM_RegisterPowerDistributionVoltageCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelPowerDistributionVoltageCallback(int32_t index, int32_t uid);
+double HALSIM_GetPowerDistributionVoltage(int32_t index);
+void HALSIM_SetPowerDistributionVoltage(int32_t index, double voltage);
+
+int32_t HALSIM_RegisterPowerDistributionCurrentCallback(
+    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelPowerDistributionCurrentCallback(int32_t index,
+                                                   int32_t channel,
+                                                   int32_t uid);
+double HALSIM_GetPowerDistributionCurrent(int32_t index, int32_t channel);
+void HALSIM_SetPowerDistributionCurrent(int32_t index, int32_t channel,
+                                        double current);
+
+void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents,
+                                            int length);
+void HALSIM_SetPowerDistributionAllCurrents(int32_t index,
+                                            const double* currents, int length);
+
+void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks(
+    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/REVPHData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/REVPHData.h
new file mode 100644
index 0000000..17e4f2c
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/REVPHData.h
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "hal/Types.h"
+#include "hal/simulation/NotifyListener.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void HALSIM_ResetREVPHData(int32_t index);
+int32_t HALSIM_RegisterREVPHInitializedCallback(int32_t index,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify);
+void HALSIM_CancelREVPHInitializedCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetREVPHInitialized(int32_t index);
+void HALSIM_SetREVPHInitialized(int32_t index, HAL_Bool solenoidInitialized);
+
+int32_t HALSIM_RegisterREVPHSolenoidOutputCallback(int32_t index,
+                                                   int32_t channel,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelREVPHSolenoidOutputCallback(int32_t index, int32_t channel,
+                                              int32_t uid);
+HAL_Bool HALSIM_GetREVPHSolenoidOutput(int32_t index, int32_t channel);
+void HALSIM_SetREVPHSolenoidOutput(int32_t index, int32_t channel,
+                                   HAL_Bool solenoidOutput);
+
+int32_t HALSIM_RegisterREVPHCompressorOnCallback(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+void HALSIM_CancelREVPHCompressorOnCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetREVPHCompressorOn(int32_t index);
+void HALSIM_SetREVPHCompressorOn(int32_t index, HAL_Bool compressorOn);
+
+int32_t HALSIM_RegisterREVPHClosedLoopEnabledCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelREVPHClosedLoopEnabledCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetREVPHClosedLoopEnabled(int32_t index);
+void HALSIM_SetREVPHClosedLoopEnabled(int32_t index,
+                                      HAL_Bool closedLoopEnabled);
+
+int32_t HALSIM_RegisterREVPHPressureSwitchCallback(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify);
+void HALSIM_CancelREVPHPressureSwitchCallback(int32_t index, int32_t uid);
+HAL_Bool HALSIM_GetREVPHPressureSwitch(int32_t index);
+void HALSIM_SetREVPHPressureSwitch(int32_t index, HAL_Bool pressureSwitch);
+
+int32_t HALSIM_RegisterREVPHCompressorCurrentCallback(
+    int32_t index, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify);
+void HALSIM_CancelREVPHCompressorCurrentCallback(int32_t index, int32_t uid);
+double HALSIM_GetREVPHCompressorCurrent(int32_t index);
+void HALSIM_SetREVPHCompressorCurrent(int32_t index, double compressorCurrent);
+
+void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values);
+void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values);
+
+void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify);
+
+void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RelayData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RelayData.h
index 1329d29..d61c9e0 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RelayData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RelayData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h
index 49d32f3..8f33bc5 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/RoboRioData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -118,6 +115,12 @@
 int32_t HALSIM_GetRoboRioUserFaults3V3(void);
 void HALSIM_SetRoboRioUserFaults3V3(int32_t userFaults3V3);
 
+int32_t HALSIM_RegisterRoboRioBrownoutVoltageCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify);
+void HALSIM_CancelRoboRioBrownoutVoltageCallback(int32_t uid);
+double HALSIM_GetRoboRioBrownoutVoltage(void);
+void HALSIM_SetRoboRioBrownoutVoltage(double brownoutVoltage);
+
 void HALSIM_RegisterRoboRioAllCallbacks(HAL_NotifyCallback callback,
                                         void* param, HAL_Bool initialNotify);
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h
index 6515aa8..406798f 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIData.h
index 149a1ef..813b75f 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SPIData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h
index 31f64ef..3d786e0 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimCallbackRegistry.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -30,7 +27,9 @@
  public:
   void Cancel(int32_t uid) {
     std::scoped_lock lock(m_mutex);
-    if (m_callbacks && uid > 0) m_callbacks->erase(uid - 1);
+    if (m_callbacks && uid > 0) {
+      m_callbacks->erase(uid - 1);
+    }
   }
 
   void Reset() {
@@ -43,13 +42,19 @@
  protected:
   int32_t DoRegister(RawFunctor callback, void* param) {
     // Must return -1 on a null callback for error handling
-    if (callback == nullptr) return -1;
-    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+    if (callback == nullptr) {
+      return -1;
+    }
+    if (!m_callbacks) {
+      m_callbacks = std::make_unique<CallbackVector>();
+    }
     return m_callbacks->emplace_back(param, callback) + 1;
   }
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE void DoReset() {
-    if (m_callbacks) m_callbacks->clear();
+    if (m_callbacks) {
+      m_callbacks->clear();
+    }
   }
 
   mutable wpi::recursive_spinlock m_mutex;
@@ -81,9 +86,10 @@
 #endif
     if (m_callbacks) {
       const char* name = GetName();
-      for (auto&& cb : *m_callbacks)
+      for (auto&& cb : *m_callbacks) {
         reinterpret_cast<CallbackFunction>(cb.callback)(name, cb.param,
                                                         std::forward<U>(u)...);
+      }
     }
   }
 
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDataValue.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDataValue.h
index ff333a5..4368a10 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDataValue.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDataValue.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -31,7 +28,7 @@
     return m_value;
   }
 
-  LLVM_ATTRIBUTE_ALWAYS_INLINE operator T() const { return Get(); }
+  LLVM_ATTRIBUTE_ALWAYS_INLINE operator T() const { return Get(); }  // NOLINT
 
   void Reset(T value) {
     std::scoped_lock lock(m_mutex);
@@ -46,7 +43,9 @@
                              HAL_Bool initialNotify, const char* name) {
     std::unique_lock lock(m_mutex);
     int32_t newUid = DoRegister(reinterpret_cast<RawFunctor>(callback), param);
-    if (newUid == -1) return -1;
+    if (newUid == -1) {
+      return -1;
+    }
     if (initialNotify) {
       // We know that the callback is not null because of earlier null check
       HAL_Value value = MakeValue(m_value);
@@ -62,9 +61,10 @@
       m_value = value;
       if (m_callbacks) {
         HAL_Value halValue = MakeValue(value);
-        for (auto&& cb : *m_callbacks)
+        for (auto&& cb : *m_callbacks) {
           reinterpret_cast<HAL_NotifyCallback>(cb.callback)(name, cb.param,
                                                             &halValue);
+        }
       }
     }
   }
diff --git a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDeviceData.h b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDeviceData.h
index e702c5c..221263e 100644
--- a/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDeviceData.h
+++ b/third_party/allwpilib/hal/src/main/native/include/hal/simulation/SimDeviceData.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,7 +12,7 @@
 
 typedef void (*HALSIM_SimValueCallback)(const char* name, void* param,
                                         HAL_SimValueHandle handle,
-                                        HAL_Bool readonly,
+                                        int32_t direction,
                                         const struct HAL_Value* value);
 
 #ifdef __cplusplus
@@ -31,8 +28,9 @@
 
 void HALSIM_CancelSimDeviceCreatedCallback(int32_t uid);
 
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback);
+int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify);
 
 void HALSIM_CancelSimDeviceFreedCallback(int32_t uid);
 
@@ -59,6 +57,22 @@
 
 void HALSIM_CancelSimValueChangedCallback(int32_t uid);
 
+/**
+ * Register a callback for HAL_SimValueReset(). The callback is called with the
+ * old value.
+ *
+ * @param handle simulated value handle
+ * @param param parameter for callback
+ * @param callback callback
+ * @param initialNotify ignored (present for consistency)
+ */
+int32_t HALSIM_RegisterSimValueResetCallback(HAL_SimValueHandle handle,
+                                             void* param,
+                                             HALSIM_SimValueCallback callback,
+                                             HAL_Bool initialNotify);
+
+void HALSIM_CancelSimValueResetCallback(int32_t uid);
+
 HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
                                             const char* name);
 
@@ -68,6 +82,9 @@
 const char** HALSIM_GetSimValueEnumOptions(HAL_SimValueHandle handle,
                                            int32_t* numOptions);
 
+const double* HALSIM_GetSimValueEnumDoubleValues(HAL_SimValueHandle handle,
+                                                 int32_t* numOptions);
+
 void HALSIM_ResetSimDeviceData(void);
 
 #ifdef __cplusplus
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Accelerometer.cpp b/third_party/allwpilib/hal/src/main/native/sim/Accelerometer.cpp
index 1435fd5..2756c99 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Accelerometer.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Accelerometer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Accelerometer.h"
 
@@ -11,11 +8,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAccelerometer() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 void HAL_SetAccelerometerActive(HAL_Bool active) {
@@ -25,7 +20,13 @@
 void HAL_SetAccelerometerRange(HAL_AccelerometerRange range) {
   SimAccelerometerData[0].range = range;
 }
-double HAL_GetAccelerometerX(void) { return SimAccelerometerData[0].x; }
-double HAL_GetAccelerometerY(void) { return SimAccelerometerData[0].y; }
-double HAL_GetAccelerometerZ(void) { return SimAccelerometerData[0].z; }
+double HAL_GetAccelerometerX(void) {
+  return SimAccelerometerData[0].x;
+}
+double HAL_GetAccelerometerY(void) {
+  return SimAccelerometerData[0].y;
+}
+double HAL_GetAccelerometerZ(void) {
+  return SimAccelerometerData[0].z;
+}
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AddressableLED.cpp b/third_party/allwpilib/hal/src/main/native/sim/AddressableLED.cpp
index 70d3f6f..75a09fb 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AddressableLED.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AddressableLED.cpp
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AddressableLED.h"
 
+#include <fmt/format.h>
+
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
@@ -27,8 +27,7 @@
                              kNumAddressableLEDs,
                              HAL_HandleEnum::AddressableLED>* ledHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAddressableLED() {
   static LimitedHandleResource<HAL_AddressableLEDHandle, AddressableLED,
                                kNumAddressableLEDs,
@@ -36,8 +35,7 @@
       dcH;
   ledHandles = &dcH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_AddressableLEDHandle HAL_InitializeAddressableLED(
@@ -86,7 +84,9 @@
 void HAL_FreeAddressableLED(HAL_AddressableLEDHandle handle) {
   auto led = ledHandles->Get(handle);
   ledHandles->Free(handle);
-  if (!led) return;
+  if (!led) {
+    return;
+  }
   SimAddressableLEDData[led->index].running = false;
   SimAddressableLEDData[led->index].initialized = false;
 }
@@ -113,8 +113,13 @@
     *status = HAL_HANDLE_ERROR;
     return;
   }
-  if (length > HAL_kAddressableLEDMaxLength) {
+  if (length > HAL_kAddressableLEDMaxLength || length < 0) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format(
+            "LED length must be less than or equal to {}. {} was requested",
+            HAL_kAddressableLEDMaxLength, length));
     return;
   }
   SimAddressableLEDData[led->index].length = length;
@@ -130,6 +135,11 @@
   }
   if (length > SimAddressableLEDData[led->index].length) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(
+        status,
+        fmt::format(
+            "Data length must be less than or equal to {}. {} was requested",
+            SimAddressableLEDData[led->index].length, length));
     return;
   }
   SimAddressableLEDData[led->index].SetData(data, length);
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogAccumulator.cpp b/third_party/allwpilib/hal/src/main/native/sim/AnalogAccumulator.cpp
index 537aa15..0f65b13 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogAccumulator.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogAccumulator.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogAccumulator.h"
 
@@ -12,11 +9,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogAccumulator() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_Bool HAL_IsAccumulatorChannel(HAL_AnalogInputHandle analogPortHandle,
@@ -27,7 +22,9 @@
     return false;
   }
   for (int32_t i = 0; i < kNumAccumulators; i++) {
-    if (port->channel == kAccumulatorChannels[i]) return true;
+    if (port->channel == kAccumulatorChannels[i]) {
+      return true;
+    }
   }
   return false;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogGyro.cpp b/third_party/allwpilib/hal/src/main/native/sim/AnalogGyro.cpp
index 9666f4a..3efca20 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogGyro.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogGyro.cpp
@@ -1,13 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogGyro.h"
 
+#include <string>
+
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/Errors.h"
@@ -18,6 +18,7 @@
 struct AnalogGyro {
   HAL_AnalogInputHandle handle;
   uint8_t index;
+  std::string previousAllocation;
 };
 }  // namespace
 
@@ -26,41 +27,43 @@
 static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
                              HAL_HandleEnum::AnalogGyro>* analogGyroHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogGyro() {
   static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
                                HAL_HandleEnum::AnalogGyro>
       agH;
   analogGyroHandles = &agH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
+  // Handle will be type checked by HAL_IsAccumulatorChannel
+  int16_t channel = getHandleIndex(analogHandle);
   if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
     if (*status == 0) {
       *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
+                                       0, kNumAccumulators, channel);
     }
     return HAL_kInvalidHandle;
   }
 
-  // handle known to be correct, so no need to type check
-  int16_t channel = getHandleIndex(analogHandle);
+  HAL_GyroHandle handle;
+  auto gyro = analogGyroHandles->Allocate(channel, &handle, status);
 
-  auto handle = analogGyroHandles->Allocate(channel, status);
-
-  if (*status != 0)
+  if (*status != 0) {
+    if (gyro) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Gyro", channel,
+                                           gyro->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Gyro",
+                                       0, kNumAccumulators, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  // Initialize port structure
-  auto gyro = analogGyroHandles->Get(handle);
-  if (gyro == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
   }
 
   gyro->handle = analogHandle;
@@ -68,6 +71,8 @@
 
   SimAnalogGyroData[channel].initialized = true;
 
+  gyro->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 
@@ -78,7 +83,9 @@
 void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {
   auto gyro = analogGyroHandles->Get(handle);
   analogGyroHandles->Free(handle);
-  if (gyro == nullptr) return;
+  if (gyro == nullptr) {
+    return;
+  }
   SimAnalogGyroData[gyro->index].initialized = false;
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogInput.cpp b/third_party/allwpilib/hal/src/main/native/sim/AnalogInput.cpp
index 8ecde74..4697dc2 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogInput.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogInput.cpp
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogInput.h"
 
 #include "AnalogInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/AnalogAccumulator.h"
 #include "hal/handles/HandlesInternal.h"
@@ -16,32 +14,35 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogInput() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
-HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(HAL_PortHandle portHandle,
-                                                    int32_t* status) {
+HAL_AnalogInputHandle HAL_InitializeAnalogInputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation,
+    int32_t* status) {
   hal::init::CheckInit();
   int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (channel == InvalidHandleIndex || channel >= kNumAnalogInputs) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
+                                     0, kNumAnalogInputs, channel);
     return HAL_kInvalidHandle;
   }
 
-  HAL_AnalogInputHandle handle = analogInputHandles->Allocate(channel, status);
+  HAL_AnalogInputHandle handle;
+  auto analog_port = analogInputHandles->Allocate(channel, &handle, status);
 
-  if (*status != 0)
+  if (*status != 0) {
+    if (analog_port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Input", channel,
+                                           analog_port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Input",
+                                       0, kNumAnalogInputs, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  // Initialize port structure
-  auto analog_port = analogInputHandles->Get(handle);
-  if (analog_port == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
   }
 
   analog_port->channel = static_cast<uint8_t>(channel);
@@ -55,18 +56,25 @@
   SimAnalogInData[channel].accumulatorInitialized = false;
   SimAnalogInData[channel].simDevice = 0;
 
+  analog_port->previousAllocation =
+      allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 void HAL_FreeAnalogInputPort(HAL_AnalogInputHandle analogPortHandle) {
   auto port = analogInputHandles->Get(analogPortHandle);
   // no status, so no need to check for a proper free.
   analogInputHandles->Free(analogPortHandle);
-  if (port == nullptr) return;
+  if (port == nullptr) {
+    return;
+  }
   SimAnalogInData[port->channel].initialized = false;
   SimAnalogInData[port->channel].accumulatorInitialized = false;
 }
 
-HAL_Bool HAL_CheckAnalogModule(int32_t module) { return module == 1; }
+HAL_Bool HAL_CheckAnalogModule(int32_t module) {
+  return module == 1;
+}
 
 HAL_Bool HAL_CheckAnalogInputChannel(int32_t channel) {
   return channel < kNumAnalogInputs && channel >= 0;
@@ -75,14 +83,18 @@
 void HAL_SetAnalogInputSimDevice(HAL_AnalogInputHandle handle,
                                  HAL_SimDeviceHandle device) {
   auto port = analogInputHandles->Get(handle);
-  if (port == nullptr) return;
+  if (port == nullptr) {
+    return;
+  }
   SimAnalogInData[port->channel].simDevice = device;
 }
 
 void HAL_SetAnalogSampleRate(double samplesPerSecond, int32_t* status) {
   // No op
 }
-double HAL_GetAnalogSampleRate(int32_t* status) { return kDefaultSampleRate; }
+double HAL_GetAnalogSampleRate(int32_t* status) {
+  return kDefaultSampleRate;
+}
 void HAL_SetAnalogAverageBits(HAL_AnalogInputHandle analogPortHandle,
                               int32_t bits, int32_t* status) {
   auto port = analogInputHandles->Get(analogPortHandle);
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.cpp b/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.cpp
index 820dc97..9b2c225 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "AnalogInternal.h"
 
@@ -15,13 +12,11 @@
                       HAL_HandleEnum::AnalogInput>* analogInputHandles;
 }  // namespace hal
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogInternal() {
   static IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
                                kNumAnalogInputs, HAL_HandleEnum::AnalogInput>
       aiH;
   analogInputHandles = &aiH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.h b/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.h
index f4633f9..89cb359 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogInternal.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
+#include <string>
+
 #include "PortsInternal.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
@@ -23,6 +22,7 @@
 struct AnalogPort {
   uint8_t channel;
   bool isAccumulator;
+  std::string previousAllocation;
 };
 
 extern IndexedHandleResource<HAL_AnalogInputHandle, hal::AnalogPort,
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogOutput.cpp b/third_party/allwpilib/hal/src/main/native/sim/AnalogOutput.cpp
index 2e3a348..d04224a 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogOutput.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogOutput.cpp
@@ -1,13 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogOutput.h"
 
+#include <string>
+
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
@@ -19,6 +19,7 @@
 namespace {
 struct AnalogOutput {
   uint8_t channel;
+  std::string previousAllocation;
 };
 }  // namespace
 
@@ -26,50 +27,58 @@
                              kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>*
     analogOutputHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogOutput() {
   static IndexedHandleResource<HAL_AnalogOutputHandle, AnalogOutput,
                                kNumAnalogOutputs, HAL_HandleEnum::AnalogOutput>
       aoH;
   analogOutputHandles = &aoH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
-HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(HAL_PortHandle portHandle,
-                                                      int32_t* status) {
+HAL_AnalogOutputHandle HAL_InitializeAnalogOutputPort(
+    HAL_PortHandle portHandle, const char* allocationLocation,
+    int32_t* status) {
   hal::init::CheckInit();
   int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (channel == InvalidHandleIndex || channel >= kNumAnalogOutputs) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Analog Output",
+                                     0, kNumAnalogOutputs, channel);
     return HAL_kInvalidHandle;
   }
 
-  HAL_AnalogOutputHandle handle =
-      analogOutputHandles->Allocate(channel, status);
+  HAL_AnalogOutputHandle handle;
+  auto port = analogOutputHandles->Allocate(channel, &handle, status);
 
-  if (*status != 0)
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Analog Output", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status,
+                                       "Invalid Index for Analog Output", 0,
+                                       kNumAnalogOutputs, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = analogOutputHandles->Get(handle);
-  if (port == nullptr) {  // would only error on thread issue
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
   }
 
   port->channel = static_cast<uint8_t>(channel);
 
   // Initialize sim analog input
   SimAnalogOutData[channel].initialized = true;
+
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
   return handle;
 }
 
 void HAL_FreeAnalogOutputPort(HAL_AnalogOutputHandle analogOutputHandle) {
   // no status, so no need to check for a proper free.
   auto port = analogOutputHandles->Get(analogOutputHandle);
-  if (port == nullptr) return;
+  if (port == nullptr) {
+    return;
+  }
   analogOutputHandles->Free(analogOutputHandle);
   SimAnalogOutData[port->channel].initialized = false;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/AnalogTrigger.cpp b/third_party/allwpilib/hal/src/main/native/sim/AnalogTrigger.cpp
index 62bfebc..50c4ab4 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/AnalogTrigger.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/AnalogTrigger.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/AnalogTrigger.h"
 
@@ -31,8 +28,7 @@
                              kNumAnalogTriggers, HAL_HandleEnum::AnalogTrigger>*
     analogTriggerHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogTrigger() {
   static LimitedHandleResource<HAL_AnalogTriggerHandle, AnalogTrigger,
                                kNumAnalogTriggers,
@@ -40,8 +36,7 @@
       atH;
   analogTriggerHandles = &atH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 int32_t hal::GetAnalogTriggerInputIndex(HAL_AnalogTriggerHandle handle,
                                         int32_t* status) {
@@ -102,7 +97,9 @@
                             int32_t* status) {
   auto trigger = analogTriggerHandles->Get(analogTriggerHandle);
   analogTriggerHandles->Free(analogTriggerHandle);
-  if (trigger == nullptr) return;
+  if (trigger == nullptr) {
+    return;
+  }
   SimAnalogTriggerData[trigger->index].initialized = false;
   // caller owns the analog input handle.
 }
@@ -131,10 +128,14 @@
 
   double trigLower =
       GetAnalogValueToVoltage(trigger->analogHandle, lower, status);
-  if (status != 0) return;
+  if (status != nullptr) {
+    return;
+  }
   double trigUpper =
       GetAnalogValueToVoltage(trigger->analogHandle, upper, status);
-  if (status != 0) return;
+  if (status != nullptr) {
+    return;
+  }
 
   SimAnalogTriggerData[trigger->index].triggerUpperBound = trigUpper;
   SimAnalogTriggerData[trigger->index].triggerLowerBound = trigLower;
diff --git a/third_party/allwpilib/hal/src/main/native/sim/CAN.cpp b/third_party/allwpilib/hal/src/main/native/sim/CAN.cpp
index 1e7af73..93a422b 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/CAN.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/CAN.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/CAN.h"
 
@@ -11,11 +8,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeCAN() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/CANAPI.cpp b/third_party/allwpilib/hal/src/main/native/sim/CANAPI.cpp
index c410dea..4d733bb 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/CANAPI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/CANAPI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/CANAPI.h"
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/CANAPIInternal.h b/third_party/allwpilib/hal/src/main/native/sim/CANAPIInternal.h
index 074f682..10e64ef 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/CANAPIInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/CANAPIInternal.h
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include "hal/Types.h"
 
-namespace hal {
-namespace can {
+namespace hal::can {
 int32_t GetCANModuleFromHandle(HAL_CANHandle handle, int32_t* status);
-}  // namespace can
-}  // namespace hal
+}  // namespace hal::can
diff --git a/third_party/allwpilib/hal/src/main/native/sim/CTREPCM.cpp b/third_party/allwpilib/hal/src/main/native/sim/CTREPCM.cpp
new file mode 100644
index 0000000..93f9135
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/CTREPCM.cpp
@@ -0,0 +1,218 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/CTREPCM.h"
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/CTREPCMDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct PCM {
+  int32_t module;
+  wpi::mutex lock;
+  std::string previousAllocation;
+};
+}  // namespace
+
+static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
+                             HAL_HandleEnum::CTREPCM>* pcmHandles;
+
+namespace hal::init {
+void InitializeCTREPCM() {
+  static IndexedHandleResource<HAL_CTREPCMHandle, PCM, kNumCTREPCMModules,
+                               HAL_HandleEnum::CTREPCM>
+      pH;
+  pcmHandles = &pH;
+}
+}  // namespace hal::init
+
+HAL_CTREPCMHandle HAL_InitializeCTREPCM(int32_t module,
+                                        const char* allocationLocation,
+                                        int32_t* status) {
+  hal::init::CheckInit();
+
+  HAL_CTREPCMHandle handle;
+  auto pcm = pcmHandles->Allocate(module, &handle, status);
+
+  if (*status != 0) {
+    if (pcm) {
+      hal::SetLastErrorPreviouslyAllocated(status, "CTRE PCM", module,
+                                           pcm->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for CTRE PCM", 0,
+                                       kNumCTREPCMModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  pcm->previousAllocation = allocationLocation ? allocationLocation : "";
+  pcm->module = module;
+
+  SimCTREPCMData[module].initialized = true;
+  // Enable closed loop
+  SimCTREPCMData[module].closedLoopEnabled = true;
+
+  return handle;
+}
+
+void HAL_FreeCTREPCM(HAL_CTREPCMHandle handle) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    pcmHandles->Free(handle);
+    return;
+  }
+  SimCTREPCMData[pcm->module].initialized = false;
+  pcmHandles->Free(handle);
+}
+
+HAL_Bool HAL_CheckCTREPCMSolenoidChannel(int32_t channel) {
+  return channel < kNumCTRESolenoidChannels && channel >= 0;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressor(HAL_CTREPCMHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimCTREPCMData[pcm->module].compressorOn;
+}
+
+void HAL_SetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle, HAL_Bool enabled,
+                                     int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  SimCTREPCMData[pcm->module].closedLoopEnabled = enabled;
+}
+
+HAL_Bool HAL_GetCTREPCMClosedLoopControl(HAL_CTREPCMHandle handle,
+                                         int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimCTREPCMData[pcm->module].closedLoopEnabled;
+}
+
+HAL_Bool HAL_GetCTREPCMPressureSwitch(HAL_CTREPCMHandle handle,
+                                      int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimCTREPCMData[pcm->module].pressureSwitch;
+}
+
+double HAL_GetCTREPCMCompressorCurrent(HAL_CTREPCMHandle handle,
+                                       int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  return SimCTREPCMData[pcm->module].compressorCurrent;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighFault(HAL_CTREPCMHandle handle,
+                                                     int32_t* status) {
+  return false;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(
+    HAL_CTREPCMHandle handle, int32_t* status) {
+  return false;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorShortedStickyFault(HAL_CTREPCMHandle handle,
+                                                    int32_t* status) {
+  return false;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorShortedFault(HAL_CTREPCMHandle handle,
+                                              int32_t* status) {
+  return false;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorNotConnectedStickyFault(
+    HAL_CTREPCMHandle handle, int32_t* status) {
+  return false;
+}
+
+HAL_Bool HAL_GetCTREPCMCompressorNotConnectedFault(HAL_CTREPCMHandle handle,
+                                                   int32_t* status) {
+  return false;
+}
+
+int32_t HAL_GetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  std::scoped_lock lock{pcm->lock};
+  auto& data = SimCTREPCMData[pcm->module].solenoidOutput;
+  uint8_t ret = 0;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    ret |= (data[i] << i);
+  }
+  return ret;
+}
+void HAL_SetCTREPCMSolenoids(HAL_CTREPCMHandle handle, int32_t mask,
+                             int32_t values, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  auto& data = SimCTREPCMData[pcm->module].solenoidOutput;
+  std::scoped_lock lock{pcm->lock};
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    auto indexMask = (1 << i);
+    if ((mask & indexMask) != 0) {
+      data[i] = (values & indexMask) != 0;
+    }
+  }
+}
+
+int32_t HAL_GetCTREPCMSolenoidDisabledList(HAL_CTREPCMHandle handle,
+                                           int32_t* status) {
+  return 0;
+}
+
+HAL_Bool HAL_GetCTREPCMSolenoidVoltageStickyFault(HAL_CTREPCMHandle handle,
+                                                  int32_t* status) {
+  return false;
+}
+
+HAL_Bool HAL_GetCTREPCMSolenoidVoltageFault(HAL_CTREPCMHandle handle,
+                                            int32_t* status) {
+  return false;
+}
+
+void HAL_ClearAllCTREPCMStickyFaults(HAL_CTREPCMHandle handle,
+                                     int32_t* status) {}
+
+void HAL_FireCTREPCMOneShot(HAL_CTREPCMHandle handle, int32_t index,
+                            int32_t* status) {}
+void HAL_SetCTREPCMOneShotDuration(HAL_CTREPCMHandle handle, int32_t index,
+                                   int32_t durMs, int32_t* status) {}
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Compressor.cpp b/third_party/allwpilib/hal/src/main/native/sim/Compressor.cpp
deleted file mode 100644
index b5c5867..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/Compressor.cpp
+++ /dev/null
@@ -1,123 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/Compressor.h"
-
-#include "HALInitializer.h"
-#include "PortsInternal.h"
-#include "hal/Errors.h"
-#include "hal/handles/HandlesInternal.h"
-#include "mockdata/PCMDataInternal.h"
-
-using namespace hal;
-
-namespace hal {
-namespace init {
-void InitializeCompressor() {}
-}  // namespace init
-}  // namespace hal
-
-extern "C" {
-
-HAL_CompressorHandle HAL_InitializeCompressor(int32_t module, int32_t* status) {
-  hal::init::CheckInit();
-  // As compressors can have unlimited objects, just create a
-  // handle with the module number as the index.
-
-  SimPCMData[module].compressorInitialized = true;
-  return (HAL_CompressorHandle)createHandle(static_cast<int16_t>(module),
-                                            HAL_HandleEnum::Compressor, 0);
-}
-
-HAL_Bool HAL_CheckCompressorModule(int32_t module) {
-  return module < kNumPCMModules && module >= 0;
-}
-
-HAL_Bool HAL_GetCompressor(HAL_CompressorHandle compressorHandle,
-                           int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-
-  return SimPCMData[index].compressorOn;
-}
-
-void HAL_SetCompressorClosedLoopControl(HAL_CompressorHandle compressorHandle,
-                                        HAL_Bool value, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  SimPCMData[index].closedLoopEnabled = value;
-}
-
-HAL_Bool HAL_GetCompressorClosedLoopControl(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-
-  return SimPCMData[index].closedLoopEnabled;
-}
-
-HAL_Bool HAL_GetCompressorPressureSwitch(HAL_CompressorHandle compressorHandle,
-                                         int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-
-  return SimPCMData[index].pressureSwitch;
-}
-
-double HAL_GetCompressorCurrent(HAL_CompressorHandle compressorHandle,
-                                int32_t* status) {
-  int16_t index =
-      getHandleTypedIndex(compressorHandle, HAL_HandleEnum::Compressor, 0);
-  if (index == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return 0;
-  }
-
-  return SimPCMData[index].compressorCurrent;
-}
-HAL_Bool HAL_GetCompressorCurrentTooHighFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  return false;
-}
-HAL_Bool HAL_GetCompressorCurrentTooHighStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  return false;
-}
-HAL_Bool HAL_GetCompressorShortedStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  return false;
-}
-HAL_Bool HAL_GetCompressorShortedFault(HAL_CompressorHandle compressorHandle,
-                                       int32_t* status) {
-  return false;
-}
-HAL_Bool HAL_GetCompressorNotConnectedStickyFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  return false;
-}
-HAL_Bool HAL_GetCompressorNotConnectedFault(
-    HAL_CompressorHandle compressorHandle, int32_t* status) {
-  return false;
-}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Constants.cpp b/third_party/allwpilib/hal/src/main/native/sim/Constants.cpp
index 64cb52b..6c1361e 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Constants.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Constants.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Constants.h"
 
@@ -11,11 +8,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeConstants() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 int32_t HAL_GetSystemClockTicksPerMicrosecond(void) {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/ConstantsInternal.h b/third_party/allwpilib/hal/src/main/native/sim/ConstantsInternal.h
index c3a6e8f..f4af049 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/ConstantsInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/ConstantsInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Counter.cpp b/third_party/allwpilib/hal/src/main/native/sim/Counter.cpp
index f0ea1c4..a56b646 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Counter.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Counter.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Counter.h"
 
@@ -19,16 +16,14 @@
                       HAL_HandleEnum::Counter>* counterHandles;
 }  // namespace hal
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeCounter() {
   static LimitedHandleResource<HAL_CounterHandle, Counter, kNumCounters,
                                HAL_HandleEnum::Counter>
       cH;
   counterHandles = &cH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_CounterHandle HAL_InitializeCounter(HAL_Counter_Mode mode, int32_t* index,
diff --git a/third_party/allwpilib/hal/src/main/native/sim/CounterInternal.h b/third_party/allwpilib/hal/src/main/native/sim/CounterInternal.h
index 70fbe54..81433fa 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/CounterInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/CounterInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/DIO.cpp b/third_party/allwpilib/hal/src/main/native/sim/DIO.cpp
index f6347d9..1b744a1 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/DIO.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/DIO.cpp
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DIO.h"
 
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/handles/LimitedHandleResource.h"
@@ -21,8 +19,7 @@
                              kNumDigitalPWMOutputs, HAL_HandleEnum::DigitalPWM>*
     digitalPWMHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDIO() {
   static LimitedHandleResource<HAL_DigitalPWMHandle, uint8_t,
                                kNumDigitalPWMOutputs,
@@ -30,32 +27,38 @@
       dpH;
   digitalPWMHandles = &dpH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializeDIOPort(HAL_PortHandle portHandle,
-                                        HAL_Bool input, int32_t* status) {
+                                        HAL_Bool input,
+                                        const char* allocationLocation,
+                                        int32_t* status) {
   hal::init::CheckInit();
-  if (*status != 0) return HAL_kInvalidHandle;
 
   int16_t channel = getPortHandleChannel(portHandle);
   if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                     kNumDigitalChannels, channel);
     return HAL_kInvalidHandle;
   }
 
-  auto handle =
-      digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO, status);
+  HAL_DigitalHandle handle;
 
-  if (*status != 0)
+  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::DIO,
+                                              &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for DIO", 0,
+                                       kNumDigitalChannels, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
   }
 
   port->channel = static_cast<uint8_t>(channel);
@@ -63,6 +66,7 @@
   SimDIOData[channel].initialized = true;
   SimDIOData[channel].isInput = input;
   SimDIOData[channel].simDevice = 0;
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
 
   return handle;
 }
@@ -75,13 +79,17 @@
   auto port = digitalChannelHandles->Get(dioPortHandle, HAL_HandleEnum::DIO);
   // no status, so no need to check for a proper free.
   digitalChannelHandles->Free(dioPortHandle, HAL_HandleEnum::DIO);
-  if (port == nullptr) return;
+  if (port == nullptr) {
+    return;
+  }
   SimDIOData[port->channel].initialized = false;
 }
 
 void HAL_SetDIOSimDevice(HAL_DigitalHandle handle, HAL_SimDeviceHandle device) {
   auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::DIO);
-  if (port == nullptr) return;
+  if (port == nullptr) {
+    return;
+  }
   SimDIOData[port->channel].simDevice = device;
 }
 
@@ -107,7 +115,9 @@
 void HAL_FreeDigitalPWM(HAL_DigitalPWMHandle pwmGenerator, int32_t* status) {
   auto port = digitalPWMHandles->Get(pwmGenerator);
   digitalPWMHandles->Free(pwmGenerator);
-  if (port == nullptr) return;
+  if (port == nullptr) {
+    return;
+  }
   int32_t id = *port;
   SimDigitalPWMData[id].initialized = false;
 }
@@ -132,8 +142,12 @@
     return;
   }
   int32_t id = *port;
-  if (dutyCycle > 1.0) dutyCycle = 1.0;
-  if (dutyCycle < 0.0) dutyCycle = 0.0;
+  if (dutyCycle > 1.0) {
+    dutyCycle = 1.0;
+  }
+  if (dutyCycle < 0.0) {
+    dutyCycle = 0.0;
+  }
   SimDigitalPWMData[id].dutyCycle = dutyCycle;
 }
 
@@ -156,7 +170,14 @@
     return;
   }
   if (value != 0 && value != 1) {
-    if (value != 0) value = 1;
+    if (value != 0) {
+      value = 1;
+    }
+  }
+  if (SimDIOData[port->channel].isInput) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, "Cannot set output of an input channel");
+    return;
   }
   SimDIOData[port->channel].value = value;
 }
@@ -179,8 +200,12 @@
     return false;
   }
   HAL_Bool value = SimDIOData[port->channel].value;
-  if (value > 1) value = 1;
-  if (value < 0) value = 0;
+  if (value > 1) {
+    value = 1;
+  }
+  if (value < 0) {
+    value = 0;
+  }
   return value;
 }
 
@@ -191,8 +216,12 @@
     return false;
   }
   HAL_Bool value = SimDIOData[port->channel].isInput;
-  if (value > 1) value = 1;
-  if (value < 0) value = 0;
+  if (value > 1) {
+    value = 1;
+  }
+  if (value < 0) {
+    value = 0;
+  }
   return value;
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/DMA.cpp b/third_party/allwpilib/hal/src/main/native/sim/DMA.cpp
index cea5a29..c000ac5 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/DMA.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/DMA.cpp
@@ -1,18 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DMA.h"
 
 extern "C" {
-HAL_DMAHandle HAL_InitializeDMA(int32_t* status) { return HAL_kInvalidHandle; }
+HAL_DMAHandle HAL_InitializeDMA(int32_t* status) {
+  return HAL_kInvalidHandle;
+}
 void HAL_FreeDMA(HAL_DMAHandle handle) {}
 
 void HAL_SetDMAPause(HAL_DMAHandle handle, HAL_Bool pause, int32_t* status) {}
-void HAL_SetDMARate(HAL_DMAHandle handle, int32_t cycles, int32_t* status) {}
+void HAL_SetDMATimedTrigger(HAL_DMAHandle handle, double periodSeconds,
+                            int32_t* status) {}
+void HAL_SetDMATimedTriggerCycles(HAL_DMAHandle handle, uint32_t cycles,
+                                  int32_t* status) {}
 
 void HAL_AddDMAEncoder(HAL_DMAHandle handle, HAL_EncoderHandle encoderHandle,
                        int32_t* status) {}
@@ -41,28 +43,36 @@
                          HAL_DutyCycleHandle dutyCycleHandle, int32_t* status) {
 }
 
-void HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
-                               HAL_Handle digitalSourceHandle,
-                               HAL_AnalogTriggerType analogTriggerType,
-                               HAL_Bool rising, HAL_Bool falling,
-                               int32_t* status) {}
+int32_t HAL_SetDMAExternalTrigger(HAL_DMAHandle handle,
+                                  HAL_Handle digitalSourceHandle,
+                                  HAL_AnalogTriggerType analogTriggerType,
+                                  HAL_Bool rising, HAL_Bool falling,
+                                  int32_t* status) {
+  return 0;
+}
+
+void HAL_ClearDMASensors(HAL_DMAHandle handle, int32_t* status) {}
+void HAL_ClearDMAExternalTriggers(HAL_DMAHandle handle, int32_t* status) {}
 
 void HAL_StartDMA(HAL_DMAHandle handle, int32_t queueDepth, int32_t* status) {}
 void HAL_StopDMA(HAL_DMAHandle handle, int32_t* status) {}
 
-void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) { return nullptr; }
+void* HAL_GetDMADirectPointer(HAL_DMAHandle handle) {
+  return nullptr;
+}
 
 enum HAL_DMAReadStatus HAL_ReadDMADirect(void* dmaPointer,
                                          HAL_DMASample* dmaSample,
-                                         int32_t timeoutMs,
+                                         double timeoutSeconds,
                                          int32_t* remainingOut,
                                          int32_t* status) {
   return HAL_DMA_ERROR;
 }
 
 enum HAL_DMAReadStatus HAL_ReadDMA(HAL_DMAHandle handle,
-                                   HAL_DMASample* dmaSample, int32_t timeoutMs,
-                                   int32_t* remainingOut, int32_t* status) {
+                                   HAL_DMASample* dmaSample,
+                                   double timeoutSeconds, int32_t* remainingOut,
+                                   int32_t* status) {
   return HAL_DMA_ERROR;
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.cpp b/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.cpp
index db6455a..b7c1d2f 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "DigitalInternal.h"
 
@@ -55,7 +52,9 @@
   }
 }
 
-int32_t remapMXPChannel(int32_t channel) { return channel - 10; }
+int32_t remapMXPChannel(int32_t channel) {
+  return channel - 10;
+}
 
 int32_t remapMXPPWMChannel(int32_t channel) {
   if (channel < 14) {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.h b/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.h
index d169c8a..cd1ac5f 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/DigitalInternal.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
+#include <string>
+
 #include "PortsInternal.h"
 #include "hal/AnalogTrigger.h"
 #include "hal/handles/DigitalHandleResource.h"
@@ -59,6 +58,7 @@
   int32_t centerPwm = 0;
   int32_t deadbandMinPwm = 0;
   int32_t minPwm = 0;
+  std::string previousAllocation;
 };
 
 extern DigitalHandleResource<HAL_DigitalHandle, DigitalPort,
diff --git a/third_party/allwpilib/hal/src/main/native/sim/DriverStation.cpp b/third_party/allwpilib/hal/src/main/native/sim/DriverStation.cpp
index aef8556..723cdac 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/DriverStation.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/DriverStation.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DriverStation.h"
 
@@ -16,9 +13,9 @@
 #include <cstring>
 #include <string>
 
+#include <fmt/format.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 
 #include "HALInitializer.h"
 #include "hal/cpp/fpga_clock.h"
@@ -34,14 +31,12 @@
 static std::atomic<HALSIM_SendConsoleLineHandler> sendConsoleLineHandler{
     nullptr};
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDriverStation() {
   static wpi::condition_variable nddaC;
   newDSDataAvailableCond = &nddaC;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 using namespace hal;
 
@@ -59,9 +54,10 @@
                       const char* details, const char* location,
                       const char* callStack, HAL_Bool printMsg) {
   auto errorHandler = sendErrorHandler.load();
-  if (errorHandler)
+  if (errorHandler) {
     return errorHandler(isError, errorCode, isLVCode, details, location,
                         callStack, printMsg);
+  }
   // Avoid flooding console by keeping track of previous 5 error
   // messages and only printing again if they're longer than 1 second old.
   static constexpr int KEEP_MSGS = 5;
@@ -79,20 +75,24 @@
   auto curTime = fpga_clock::now();
   int i;
   for (i = 0; i < KEEP_MSGS; ++i) {
-    if (prevMsg[i] == details) break;
+    if (prevMsg[i] == details) {
+      break;
+    }
   }
   int retval = 0;
   if (i == KEEP_MSGS || (curTime - prevMsgTime[i]) >= std::chrono::seconds(1)) {
     printMsg = true;
     if (printMsg) {
+      fmt::memory_buffer buf;
       if (location && location[0] != '\0') {
-        std::fprintf(stderr, "%s at %s: ", isError ? "Error" : "Warning",
-                     location);
+        fmt::format_to(fmt::appender{buf},
+                       "{} at {}: ", isError ? "Error" : "Warning", location);
       }
-      std::fprintf(stderr, "%s\n", details);
+      fmt::format_to(fmt::appender{buf}, "{}\n", details);
       if (callStack && callStack[0] != '\0') {
-        std::fprintf(stderr, "%s\n", callStack);
+        fmt::format_to(fmt::appender{buf}, "{}\n", callStack);
       }
+      std::fwrite(buf.data(), buf.size(), 1, stderr);
     }
     if (i == KEEP_MSGS) {
       // replace the oldest one
@@ -116,12 +116,13 @@
   if (handler) {
     return handler(line);
   }
-  wpi::outs() << line << "\n";
-  wpi::outs().flush();
+  std::puts(line);
+  std::fflush(stdout);
   return 0;
 }
 
 int32_t HAL_GetControlWord(HAL_ControlWord* controlWord) {
+  std::memset(controlWord, 0, sizeof(HAL_ControlWord));
   controlWord->enabled = SimDriverStationData->enabled;
   controlWord->autonomous = SimDriverStationData->autonomous;
   controlWord->test = SimDriverStationData->test;
@@ -179,9 +180,13 @@
   return name;
 }
 
-void HAL_FreeJoystickName(char* name) { std::free(name); }
+void HAL_FreeJoystickName(char* name) {
+  std::free(name);
+}
 
-int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) { return 0; }
+int32_t HAL_GetJoystickAxisType(int32_t joystickNum, int32_t axis) {
+  return 0;
+}
 
 int32_t HAL_SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
                                int32_t leftRumble, int32_t rightRumble) {
@@ -199,7 +204,9 @@
   return 0;
 }
 
-void HAL_ObserveUserProgramStarting(void) { HALSIM_SetProgramStarted(); }
+void HAL_ObserveUserProgramStarting(void) {
+  HALSIM_SetProgramStarted();
+}
 
 void HAL_ObserveUserProgramDisabled(void) {
   // TODO
@@ -230,12 +237,16 @@
   std::scoped_lock lock(newDSDataAvailableMutex);
   int& lastCount = GetThreadLocalLastCount();
   int currentCount = newDSDataAvailableCounter;
-  if (lastCount == currentCount) return false;
+  if (lastCount == currentCount) {
+    return false;
+  }
   lastCount = currentCount;
   return true;
 }
 
-void HAL_WaitForDSData(void) { HAL_WaitForDSDataTimeout(0); }
+void HAL_WaitForDSData(void) {
+  HAL_WaitForDSDataTimeout(0);
+}
 
 HAL_Bool HAL_WaitForDSDataTimeout(double timeout) {
   std::unique_lock lock(newDSDataAvailableMutex);
@@ -272,7 +283,9 @@
 static int32_t newDataOccur(uint32_t refNum) {
   // Since we could get other values, require our specific handle
   // to signal our threads
-  if (refNum != refNumber) return 0;
+  if (refNum != refNumber) {
+    return 0;
+  }
   SimDriverStationData->CallNewDataCallbacks();
   std::scoped_lock lock(newDSDataAvailableMutex);
   // Nofify all threads
@@ -286,11 +299,15 @@
   static std::atomic_bool initialized{false};
   static wpi::mutex initializeMutex;
   // Initial check, as if it's true initialization has finished
-  if (initialized) return;
+  if (initialized) {
+    return;
+  }
 
   std::scoped_lock lock(initializeMutex);
   // Second check in case another thread was waiting
-  if (initialized) return;
+  if (initialized) {
+    return;
+  }
 
   SimDriverStationData->ResetData();
 
@@ -302,6 +319,8 @@
   initialized = true;
 }
 
-void HAL_ReleaseDSMutex(void) { newDataOccur(refNumber); }
+void HAL_ReleaseDSMutex(void) {
+  newDataOccur(refNumber);
+}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/DutyCycle.cpp b/third_party/allwpilib/hal/src/main/native/sim/DutyCycle.cpp
index 0e14eeb..51c6506 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/DutyCycle.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/DutyCycle.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/DutyCycle.h"
 
@@ -26,16 +23,14 @@
 static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
                              HAL_HandleEnum::DutyCycle>* dutyCycleHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDutyCycle() {
   static LimitedHandleResource<HAL_DutyCycleHandle, DutyCycle, kNumDutyCycles,
                                HAL_HandleEnum::DutyCycle>
       dcH;
   dutyCycleHandles = &dcH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_DutyCycleHandle HAL_InitializeDutyCycle(HAL_Handle digitalSourceHandle,
@@ -65,14 +60,18 @@
 void HAL_FreeDutyCycle(HAL_DutyCycleHandle dutyCycleHandle) {
   auto dutyCycle = dutyCycleHandles->Get(dutyCycleHandle);
   dutyCycleHandles->Free(dutyCycleHandle);
-  if (dutyCycle == nullptr) return;
+  if (dutyCycle == nullptr) {
+    return;
+  }
   SimDutyCycleData[dutyCycle->index].initialized = false;
 }
 
 void HAL_SetDutyCycleSimDevice(HAL_EncoderHandle handle,
                                HAL_SimDeviceHandle device) {
   auto dutyCycle = dutyCycleHandles->Get(handle);
-  if (dutyCycle == nullptr) return;
+  if (dutyCycle == nullptr) {
+    return;
+  }
   SimDutyCycleData[dutyCycle->index].simDevice = device;
 }
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Encoder.cpp b/third_party/allwpilib/hal/src/main/native/sim/Encoder.cpp
index 32416f0..78aa281 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Encoder.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Encoder.cpp
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Encoder.h"
 
 #include "CounterInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/Errors.h"
 #include "hal/handles/HandlesInternal.h"
@@ -20,6 +18,8 @@
 namespace {
 struct Encoder {
   HAL_Handle nativeHandle;
+  HAL_FPGAEncoderHandle fpgaHandle;
+  HAL_CounterHandle counterHandle;
   HAL_EncoderEncodingType encodingType;
   double distancePerPulse;
   uint8_t index;
@@ -34,8 +34,7 @@
 static LimitedHandleResource<HAL_FPGAEncoderHandle, Empty, kNumEncoders,
                              HAL_HandleEnum::FPGAEncoder>* fpgaEncoderHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeEncoder() {
   static LimitedHandleResource<HAL_FPGAEncoderHandle, Empty, kNumEncoders,
                                HAL_HandleEnum::FPGAEncoder>
@@ -47,7 +46,21 @@
       eH;
   encoderHandles = &eH;
 }
-}  // namespace init
+}  // namespace hal::init
+
+namespace hal {
+bool GetEncoderBaseHandle(HAL_EncoderHandle handle,
+                          HAL_FPGAEncoderHandle* fpgaHandle,
+                          HAL_CounterHandle* counterHandle) {
+  auto encoder = encoderHandles->Get(handle);
+  if (!handle) {
+    return false;
+  }
+
+  *fpgaHandle = encoder->fpgaHandle;
+  *counterHandle = encoder->counterHandle;
+  return true;
+}
 }  // namespace hal
 
 extern "C" {
@@ -90,13 +103,22 @@
   encoder->nativeHandle = nativeHandle;
   encoder->encodingType = encodingType;
   encoder->distancePerPulse = 1.0;
+  if (encodingType == HAL_EncoderEncodingType::HAL_Encoder_k4X) {
+    encoder->fpgaHandle = nativeHandle;
+    encoder->counterHandle = HAL_kInvalidHandle;
+  } else {
+    encoder->fpgaHandle = HAL_kInvalidHandle;
+    encoder->counterHandle = nativeHandle;
+  }
   return handle;
 }
 
 void HAL_FreeEncoder(HAL_EncoderHandle encoderHandle, int32_t* status) {
   auto encoder = encoderHandles->Get(encoderHandle);
   encoderHandles->Free(encoderHandle);
-  if (encoder == nullptr) return;
+  if (encoder == nullptr) {
+    return;
+  }
   if (isHandleType(encoder->nativeHandle, HAL_HandleEnum::FPGAEncoder)) {
     fpgaEncoderHandles->Free(encoder->nativeHandle);
   } else if (isHandleType(encoder->nativeHandle, HAL_HandleEnum::Counter)) {
@@ -108,7 +130,9 @@
 void HAL_SetEncoderSimDevice(HAL_EncoderHandle handle,
                              HAL_SimDeviceHandle device) {
   auto encoder = encoderHandles->Get(handle);
-  if (encoder == nullptr) return;
+  if (encoder == nullptr) {
+    return;
+  }
   SimEncoderData[encoder->index].simDevice = device;
 }
 
@@ -174,9 +198,9 @@
     return;
   }
 
+  SimEncoderData[encoder->index].reset = true;
   SimEncoderData[encoder->index].count = 0;
   SimEncoderData[encoder->index].period = std::numeric_limits<double>::max();
-  SimEncoderData[encoder->index].reset = true;
 }
 double HAL_GetEncoderPeriod(HAL_EncoderHandle encoderHandle, int32_t* status) {
   auto encoder = encoderHandles->Get(encoderHandle);
@@ -247,6 +271,7 @@
 
   if (minRate == 0.0) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, "minRate must not be 0");
     return;
   }
 
@@ -263,6 +288,7 @@
 
   if (distancePerPulse == 0.0) {
     *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, "distancePerPulse must not be 0");
     return;
   }
   encoder->distancePerPulse = distancePerPulse;
diff --git a/third_party/allwpilib/hal/src/main/native/sim/ErrorsInternal.h b/third_party/allwpilib/hal/src/main/native/sim/ErrorsInternal.h
index 55372d8..42d1560 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/ErrorsInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/ErrorsInternal.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-typedef enum {
+#include <stdint.h>
+
+enum CTR_Code {
   CTR_OKAY,       // No Error - Function executed as expected
   CTR_RxTimeout,  // CAN frame has not been received within specified period of
                   // time.
@@ -18,7 +17,7 @@
   CTR_SigNotUpdated,      // Have not received an value response for signal.
   CTR_BufferFull,  // Caller attempted to insert data into a buffer that is
                    // full.
-} CTR_Code;
+};
 
 // VISA Error
 #define _VI_ERROR (-2147483647L - 1)
@@ -109,104 +108,104 @@
  * Represents the resulting status of a function call through its return value.
  * 0 is success, negative values are errors, and positive values are warnings.
  */
-typedef int32_t NiFpga_Status;
+using NiFpga_Status = int32_t;  // NOLINT
 
 /**
  * No errors or warnings.
  */
-static const NiFpga_Status NiFpga_Status_Success = 0;
+constexpr NiFpga_Status NiFpga_Status_Success = 0;
 
 /**
  * The timeout expired before the FIFO operation could complete.
  */
-static const NiFpga_Status NiFpga_Status_FifoTimeout = -50400;
+constexpr NiFpga_Status NiFpga_Status_FifoTimeout = -50400;
 
 /**
  * No transfer is in progress because the transfer was aborted by the client.
  * The operation could not be completed as specified.
  */
-static const NiFpga_Status NiFpga_Status_TransferAborted = -50405;
+constexpr NiFpga_Status NiFpga_Status_TransferAborted = -50405;
 
 /**
  * A memory allocation failed. Try again after rebooting.
  */
-static const NiFpga_Status NiFpga_Status_MemoryFull = -52000;
+constexpr NiFpga_Status NiFpga_Status_MemoryFull = -52000;
 
 /**
  * An unexpected software error occurred.
  */
-static const NiFpga_Status NiFpga_Status_SoftwareFault = -52003;
+constexpr NiFpga_Status NiFpga_Status_SoftwareFault = -52003;
 
 /**
  * A parameter to a function was not valid. This could be a NULL pointer, a bad
  * value, etc.
  */
-static const NiFpga_Status NiFpga_Status_InvalidParameter = -52005;
+constexpr NiFpga_Status NiFpga_Status_InvalidParameter = -52005;
 
 /**
  * A required resource was not found. The NiFpga.* library, the RIO resource, or
  * some other resource may be missing.
  */
-static const NiFpga_Status NiFpga_Status_ResourceNotFound = -52006;
+constexpr NiFpga_Status NiFpga_Status_ResourceNotFound = -52006;
 
 /**
  * A required resource was not properly initialized. This could occur if
  * NiFpga_Initialize was not called or a required NiFpga_IrqContext was not
  * reserved.
  */
-static const NiFpga_Status NiFpga_Status_ResourceNotInitialized = -52010;
+constexpr NiFpga_Status NiFpga_Status_ResourceNotInitialized = -52010;
 
 /**
  * A hardware failure has occurred. The operation could not be completed as
  * specified.
  */
-static const NiFpga_Status NiFpga_Status_HardwareFault = -52018;
+constexpr NiFpga_Status NiFpga_Status_HardwareFault = -52018;
 
 /**
  * The FPGA is already running.
  */
-static const NiFpga_Status NiFpga_Status_FpgaAlreadyRunning = -61003;
+constexpr NiFpga_Status NiFpga_Status_FpgaAlreadyRunning = -61003;
 
 /**
  * An error occurred downloading the VI to the FPGA device. Verify that
  * the target is connected and powered and that the resource of the target
  * is properly configured.
  */
-static const NiFpga_Status NiFpga_Status_DownloadError = -61018;
+constexpr NiFpga_Status NiFpga_Status_DownloadError = -61018;
 
 /**
  * The bitfile was not compiled for the specified resource's device type.
  */
-static const NiFpga_Status NiFpga_Status_DeviceTypeMismatch = -61024;
+constexpr NiFpga_Status NiFpga_Status_DeviceTypeMismatch = -61024;
 
 /**
  * An error was detected in the communication between the host computer and the
  * FPGA target.
  */
-static const NiFpga_Status NiFpga_Status_CommunicationTimeout = -61046;
+constexpr NiFpga_Status NiFpga_Status_CommunicationTimeout = -61046;
 
 /**
  * The timeout expired before any of the IRQs were asserted.
  */
-static const NiFpga_Status NiFpga_Status_IrqTimeout = -61060;
+constexpr NiFpga_Status NiFpga_Status_IrqTimeout = -61060;
 
 /**
  * The specified bitfile is invalid or corrupt.
  */
-static const NiFpga_Status NiFpga_Status_CorruptBitfile = -61070;
+constexpr NiFpga_Status NiFpga_Status_CorruptBitfile = -61070;
 
 /**
  * The requested FIFO depth is invalid. It is either 0 or an amount not
  * supported by the hardware.
  */
-static const NiFpga_Status NiFpga_Status_BadDepth = -61072;
+constexpr NiFpga_Status NiFpga_Status_BadDepth = -61072;
 
 /**
  * The number of FIFO elements is invalid. Either the number is greater than the
  * depth of the host memory DMA FIFO, or more elements were requested for
  * release than had been acquired.
  */
-static const NiFpga_Status NiFpga_Status_BadReadWriteCount = -61073;
+constexpr NiFpga_Status NiFpga_Status_BadReadWriteCount = -61073;
 
 /**
  * A hardware clocking error occurred. A derived clock lost lock with its base
@@ -219,62 +218,62 @@
  * generated from free-running, on-board sources, please contact National
  * Instruments technical support at ni.com/support.
  */
-static const NiFpga_Status NiFpga_Status_ClockLostLock = -61083;
+constexpr NiFpga_Status NiFpga_Status_ClockLostLock = -61083;
 
 /**
  * The operation could not be performed because the FPGA is busy. Stop all
  * activities on the FPGA before requesting this operation. If the target is in
  * Scan Interface programming mode, put it in FPGA Interface programming mode.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusy = -61141;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusy = -61141;
 
 /**
  * The operation could not be performed because the FPGA is busy operating in
  * FPGA Interface C API mode. Stop all activities on the FPGA before requesting
  * this operation.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusyFpgaInterfaceCApi = -61200;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusyFpgaInterfaceCApi = -61200;
 
 /**
  * The chassis is in Scan Interface programming mode. In order to run FPGA VIs,
  * you must go to the chassis properties page, select FPGA programming mode, and
  * deploy settings.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusyScanInterface = -61201;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusyScanInterface = -61201;
 
 /**
  * The operation could not be performed because the FPGA is busy operating in
  * FPGA Interface mode. Stop all activities on the FPGA before requesting this
  * operation.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusyFpgaInterface = -61202;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusyFpgaInterface = -61202;
 
 /**
  * The operation could not be performed because the FPGA is busy operating in
  * Interactive mode. Stop all activities on the FPGA before requesting this
  * operation.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusyInteractive = -61203;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusyInteractive = -61203;
 
 /**
  * The operation could not be performed because the FPGA is busy operating in
  * Emulation mode. Stop all activities on the FPGA before requesting this
  * operation.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusyEmulation = -61204;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusyEmulation = -61204;
 
 /**
  * LabVIEW FPGA does not support the Reset method for bitfiles that allow
  * removal of implicit enable signals in single-cycle Timed Loops.
  */
-static const NiFpga_Status NiFpga_Status_ResetCalledWithImplicitEnableRemoval =
+constexpr NiFpga_Status NiFpga_Status_ResetCalledWithImplicitEnableRemoval =
     -61211;
 
 /**
  * LabVIEW FPGA does not support the Abort method for bitfiles that allow
  * removal of implicit enable signals in single-cycle Timed Loops.
  */
-static const NiFpga_Status NiFpga_Status_AbortCalledWithImplicitEnableRemoval =
+constexpr NiFpga_Status NiFpga_Status_AbortCalledWithImplicitEnableRemoval =
     -61212;
 
 /**
@@ -283,7 +282,7 @@
  * Pass the NiFpga_CloseAttribute_NoResetIfLastSession attribute to NiFpga_Close
  * instead of 0.
  */
-static const NiFpga_Status
+constexpr NiFpga_Status
     NiFpga_Status_CloseAndResetCalledWithImplicitEnableRemoval = -61213;
 
 /**
@@ -291,14 +290,14 @@
  * Timed Loops, LabVIEW FPGA does not support this method prior to running the
  * bitfile.
  */
-static const NiFpga_Status NiFpga_Status_ImplicitEnableRemovalButNotYetRun =
+constexpr NiFpga_Status NiFpga_Status_ImplicitEnableRemovalButNotYetRun =
     -61214;
 
 /**
  * Bitfiles that allow removal of implicit enable signals in single-cycle Timed
  * Loops can run only once. Download the bitfile again before re-running the VI.
  */
-static const NiFpga_Status
+constexpr NiFpga_Status
     NiFpga_Status_RunAfterStoppedCalledWithImplicitEnableRemoval = -61215;
 
 /**
@@ -307,8 +306,7 @@
  * protocol. If you are generating your clocks internally, please contact
  * National Instruments Technical Support.
  */
-static const NiFpga_Status NiFpga_Status_GatedClockHandshakingViolation =
-    -61216;
+constexpr NiFpga_Status NiFpga_Status_GatedClockHandshakingViolation = -61216;
 
 /**
  * The number of elements requested must be less than or equal to the number of
@@ -316,7 +314,7 @@
  * fewer unacquired elements left in the FIFO than are being requested. Release
  * some acquired elements before acquiring more elements.
  */
-static const NiFpga_Status NiFpga_Status_ElementsNotPermissibleToBeAcquired =
+constexpr NiFpga_Status NiFpga_Status_ElementsNotPermissibleToBeAcquired =
     -61219;
 
 /**
@@ -324,12 +322,12 @@
  * discovery mode. Wait for configuration or discovery to complete and retry
  * your operation.
  */
-static const NiFpga_Status NiFpga_Status_FpgaBusyConfiguration = -61252;
+constexpr NiFpga_Status NiFpga_Status_FpgaBusyConfiguration = -61252;
 
 /**
  * An unexpected internal error occurred.
  */
-static const NiFpga_Status NiFpga_Status_InternalError = -61499;
+constexpr NiFpga_Status NiFpga_Status_InternalError = -61499;
 
 /**
  * The NI-RIO driver was unable to allocate memory for a FIFO. This can happen
@@ -338,52 +336,52 @@
  * to reconfigure the controller with a greater maximum FIFO depth. For more
  * information, refer to the NI KnowledgeBase article 65OF2ERQ.
  */
-static const NiFpga_Status NiFpga_Status_TotalDmaFifoDepthExceeded = -63003;
+constexpr NiFpga_Status NiFpga_Status_TotalDmaFifoDepthExceeded = -63003;
 
 /**
  * Access to the remote system was denied. Use MAX to check the Remote Device
  * Access settings under Software>>NI-RIO>>NI-RIO Settings on the remote system.
  */
-static const NiFpga_Status NiFpga_Status_AccessDenied = -63033;
+constexpr NiFpga_Status NiFpga_Status_AccessDenied = -63033;
 
 /**
  * The NI-RIO software on the host is not compatible with the software on the
  * target. Upgrade the NI-RIO software on the host in order to connect to this
  * target.
  */
-static const NiFpga_Status NiFpga_Status_HostVersionMismatch = -63038;
+constexpr NiFpga_Status NiFpga_Status_HostVersionMismatch = -63038;
 
 /**
  * A connection could not be established to the specified remote device. Ensure
  * that the device is on and accessible over the network, that NI-RIO software
  * is installed, and that the RIO server is running and properly configured.
  */
-static const NiFpga_Status NiFpga_Status_RpcConnectionError = -63040;
+constexpr NiFpga_Status NiFpga_Status_RpcConnectionError = -63040;
 
 /**
  * The RPC session is invalid. The target may have reset or been rebooted. Check
  * the network connection and retry the operation.
  */
-static const NiFpga_Status NiFpga_Status_RpcSessionError = -63043;
+constexpr NiFpga_Status NiFpga_Status_RpcSessionError = -63043;
 
 /**
  * The operation could not complete because another session is accessing the
  * FIFO. Close the other session and retry.
  */
-static const NiFpga_Status NiFpga_Status_FifoReserved = -63082;
+constexpr NiFpga_Status NiFpga_Status_FifoReserved = -63082;
 
 /**
  * A Configure FIFO, Stop FIFO, Read FIFO, or Write FIFO function was called
  * while the host had acquired elements of the FIFO. Release all acquired
  * elements before configuring, stopping, reading, or writing.
  */
-static const NiFpga_Status NiFpga_Status_FifoElementsCurrentlyAcquired = -63083;
+constexpr NiFpga_Status NiFpga_Status_FifoElementsCurrentlyAcquired = -63083;
 
 /**
  * A function was called using a misaligned address. The address must be a
  * multiple of the size of the datatype.
  */
-static const NiFpga_Status NiFpga_Status_MisalignedAccess = -63084;
+constexpr NiFpga_Status NiFpga_Status_MisalignedAccess = -63084;
 
 /**
  * The FPGA Read/Write Control Function is accessing a control or indicator
@@ -391,7 +389,7 @@
  * Refer to the hardware documentation for the limitations on data types for
  * this target.
  */
-static const NiFpga_Status NiFpga_Status_ControlOrIndicatorTooLarge = -63085;
+constexpr NiFpga_Status NiFpga_Status_ControlOrIndicatorTooLarge = -63085;
 
 /**
  * A valid .lvbitx bitfile is required. If you are using a valid .lvbitx
@@ -399,14 +397,14 @@
  * Determine which version of LabVIEW was used to make the bitfile, update your
  * software to that version or later, and try again.
  */
-static const NiFpga_Status NiFpga_Status_BitfileReadError = -63101;
+constexpr NiFpga_Status NiFpga_Status_BitfileReadError = -63101;
 
 /**
  * The specified signature does not match the signature of the bitfile. If the
  * bitfile has been recompiled, regenerate the C API and rebuild the
  * application.
  */
-static const NiFpga_Status NiFpga_Status_SignatureMismatch = -63106;
+constexpr NiFpga_Status NiFpga_Status_SignatureMismatch = -63106;
 
 /**
  * The bitfile you are trying to use is incompatible with the version
@@ -416,33 +414,33 @@
  * with the same version of NI-RIO that is currently installed on the
  * target and/or host.
  */
-static const NiFpga_Status NiFpga_Status_IncompatibleBitfile = -63107;
+constexpr NiFpga_Status NiFpga_Status_IncompatibleBitfile = -63107;
 
 /**
  * Either the supplied resource name is invalid as a RIO resource name, or the
  * device was not found. Use MAX to find the proper resource name for the
  * intended device.
  */
-static const NiFpga_Status NiFpga_Status_InvalidResourceName = -63192;
+constexpr NiFpga_Status NiFpga_Status_InvalidResourceName = -63192;
 
 /**
  * The requested feature is not supported.
  */
-static const NiFpga_Status NiFpga_Status_FeatureNotSupported = -63193;
+constexpr NiFpga_Status NiFpga_Status_FeatureNotSupported = -63193;
 
 /**
  * The NI-RIO software on the target system is not compatible with this
  * software. Upgrade the NI-RIO software on the target system.
  */
-static const NiFpga_Status NiFpga_Status_VersionMismatch = -63194;
+constexpr NiFpga_Status NiFpga_Status_VersionMismatch = -63194;
 
 /**
  * The session is invalid or has been closed.
  */
-static const NiFpga_Status NiFpga_Status_InvalidSession = -63195;
+constexpr NiFpga_Status NiFpga_Status_InvalidSession = -63195;
 
 /**
  * The maximum number of open FPGA sessions has been reached. Close some open
  * sessions.
  */
-static const NiFpga_Status NiFpga_Status_OutOfHandles = -63198;
+constexpr NiFpga_Status NiFpga_Status_OutOfHandles = -63198;
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Extensions.cpp b/third_party/allwpilib/hal/src/main/native/sim/Extensions.cpp
index e638d6a..9131bcb 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Extensions.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Extensions.cpp
@@ -1,18 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Extensions.h"
 
+#include <cstdio>
+#include <string_view>
 #include <vector>
 
-#include <wpi/Path.h>
-#include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+#include <wpi/fs.h>
 #include <wpi/spinlock.h>
 
 #if defined(WIN32) || defined(_WIN32)
@@ -27,7 +26,7 @@
 #define DLOPEN(a) LoadLibraryA(a)
 #define DLSYM GetProcAddress
 #define DLCLOSE FreeLibrary
-#define DLERROR "error #" << GetLastError()
+#define DLERROR fmt::format("error #{}", GetLastError())
 #else
 #define DELIM ':'
 #define HTYPE void*
@@ -43,11 +42,9 @@
 static std::vector<std::pair<void*, void (*)(void*, const char*, void*)>>
     gExtensionListeners;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeExtensions() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 static bool& GetShowNotFoundMessage() {
   static bool showMsg = true;
@@ -58,66 +55,64 @@
 
 int HAL_LoadOneExtension(const char* library) {
   int rc = 1;  // It is expected and reasonable not to find an extra simulation
-  wpi::outs() << "HAL Extensions: Attempting to load: "
-              << wpi::sys::path::stem(library) << "\n";
-  wpi::outs().flush();
+  fmt::print("HAL Extensions: Attempting to load: {}\n",
+             fs::path{library}.stem().string());
+  std::fflush(stdout);
   HTYPE handle = DLOPEN(library);
 #if !defined(WIN32) && !defined(_WIN32)
   if (!handle) {
-    wpi::SmallString<128> libraryName("lib");
-    libraryName += library;
 #if defined(__APPLE__)
-    libraryName += ".dylib";
+    auto libraryName = fmt::format("lib{}.dylib", library);
 #else
-    libraryName += ".so";
+    auto libraryName = fmt::format("lib{}.so", library);
 #endif
-    wpi::outs() << "HAL Extensions: Load failed: " << DLERROR
-                << "\nTrying modified name: "
-                << wpi::sys::path::stem(libraryName) << "\n";
-    wpi::outs().flush();
+    fmt::print("HAL Extensions: Load failed: {}\nTrying modified name: {}\n",
+               DLERROR, fs::path{libraryName}.stem().string());
+    std::fflush(stdout);
     handle = DLOPEN(libraryName.c_str());
   }
 #endif
   if (!handle) {
-    wpi::outs() << "HAL Extensions: Failed to load library: " << DLERROR
-                << '\n';
-    wpi::outs().flush();
+    fmt::print("HAL Extensions: Failed to load library: {}\n", DLERROR);
+    std::fflush(stdout);
     return rc;
   }
 
   auto init = reinterpret_cast<halsim_extension_init_func_t*>(
       DLSYM(handle, "HALSIM_InitExtension"));
 
-  if (init) rc = (*init)();
+  if (init) {
+    rc = (*init)();
+  }
 
   if (rc != 0) {
-    wpi::outs() << "HAL Extensions: Failed to load extension\n";
-    wpi::outs().flush();
+    std::puts("HAL Extensions: Failed to load extension");
+    std::fflush(stdout);
     DLCLOSE(handle);
   } else {
-    wpi::outs() << "HAL Extensions: Successfully loaded extension\n";
-    wpi::outs().flush();
+    std::puts("HAL Extensions: Successfully loaded extension");
+    std::fflush(stdout);
   }
   return rc;
 }
 
 int HAL_LoadExtensions(void) {
   int rc = 1;
-  wpi::SmallVector<wpi::StringRef, 2> libraries;
+  wpi::SmallVector<std::string_view, 2> libraries;
   const char* e = std::getenv("HALSIM_EXTENSIONS");
   if (!e) {
     if (GetShowNotFoundMessage()) {
-      wpi::outs() << "HAL Extensions: No extensions found\n";
-      wpi::outs().flush();
+      std::puts("HAL Extensions: No extensions found");
+      std::fflush(stdout);
     }
     return rc;
   }
-  wpi::StringRef env{e};
-  env.split(libraries, DELIM, -1, false);
-  for (auto& libref : libraries) {
-    wpi::SmallString<128> library(libref);
-    rc = HAL_LoadOneExtension(library.c_str());
-    if (rc < 0) break;
+  wpi::split(e, libraries, DELIM, -1, false);
+  for (auto& library : libraries) {
+    rc = HAL_LoadOneExtension(std::string(library).c_str());
+    if (rc < 0) {
+      break;
+    }
   }
   return rc;
 }
@@ -125,8 +120,9 @@
 void HAL_RegisterExtension(const char* name, void* data) {
   std::scoped_lock lock(gExtensionRegistryMutex);
   gExtensionRegistry.emplace_back(name, data);
-  for (auto&& listener : gExtensionListeners)
+  for (auto&& listener : gExtensionListeners) {
     listener.second(listener.first, name, data);
+  }
 }
 
 void HAL_RegisterExtensionListener(void* param,
@@ -134,8 +130,9 @@
                                                 void* data)) {
   std::scoped_lock lock(gExtensionRegistryMutex);
   gExtensionListeners.emplace_back(param, func);
-  for (auto&& extension : gExtensionRegistry)
+  for (auto&& extension : gExtensionRegistry) {
     func(param, extension.first, extension.second);
+  }
 }
 
 void HAL_SetShowExtensionsNotFoundMessages(HAL_Bool showMessage) {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp b/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp
index 64c4a10..8d911df 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/HAL.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/HAL.h"
 
+#include <cstdio>
 #include <vector>
 
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/spinlock.h>
 
 #ifdef _WIN32
@@ -26,16 +23,41 @@
 #include "hal/Extensions.h"
 #include "hal/handles/HandlesInternal.h"
 #include "hal/simulation/DriverStationData.h"
+#include "hal/simulation/SimCallbackRegistry.h"
 #include "mockdata/RoboRioDataInternal.h"
 
 using namespace hal;
 
-static HAL_RuntimeType runtimeType{HAL_Mock};
+namespace {
+class SimPeriodicCallbackRegistry : public impl::SimCallbackRegistryBase {
+ public:
+  int32_t Register(HALSIM_SimPeriodicCallback callback, void* param) {
+    std::scoped_lock lock(m_mutex);
+    return DoRegister(reinterpret_cast<RawFunctor>(callback), param);
+  }
+
+  void operator()() const {
+#ifdef _MSC_VER  // work around VS2019 16.4.0 bug
+    std::scoped_lock<wpi::recursive_spinlock> lock(m_mutex);
+#else
+    std::scoped_lock lock(m_mutex);
+#endif
+    if (m_callbacks) {
+      for (auto&& cb : *m_callbacks) {
+        reinterpret_cast<HALSIM_SimPeriodicCallback>(cb.callback)(cb.param);
+      }
+    }
+  }
+};
+}  // namespace
+
+static HAL_RuntimeType runtimeType{HAL_Runtime_Simulation};
 static wpi::spinlock gOnShutdownMutex;
 static std::vector<std::pair<void*, void (*)(void*)>> gOnShutdown;
+static SimPeriodicCallbackRegistry gSimPeriodicBefore;
+static SimPeriodicCallbackRegistry gSimPeriodicAfter;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeHAL() {
   InitializeAccelerometerData();
   InitializeAddressableLEDData();
@@ -51,8 +73,9 @@
   InitializeDriverStationData();
   InitializeEncoderData();
   InitializeI2CData();
-  InitializePCMData();
-  InitializePDPData();
+  InitializeCTREPCMData();
+  InitializeREVPHData();
+  InitializePowerDistributionData();
   InitializePWMData();
   InitializeRelayData();
   InitializeRoboRioData();
@@ -68,7 +91,6 @@
   InitializeAnalogOutput();
   InitializeAnalogTrigger();
   InitializeCAN();
-  InitializeCompressor();
   InitializeConstants();
   InitializeCounter();
   InitializeDigitalInternal();
@@ -82,32 +104,38 @@
   InitializeMain();
   InitializeMockHooks();
   InitializeNotifier();
-  InitializePDP();
+  InitializePowerDistribution();
   InitializePorts();
   InitializePower();
+  InitializeCTREPCM();
+  InitializeREVPH();
   InitializePWM();
   InitializeRelay();
   InitializeSerialPort();
   InitializeSimDevice();
-  InitializeSolenoid();
   InitializeSPI();
   InitializeThreads();
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_PortHandle HAL_GetPort(int32_t channel) {
   // Dont allow a number that wouldn't fit in a uint8_t
-  if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
+  if (channel < 0 || channel >= 255) {
+    return HAL_kInvalidHandle;
+  }
   return createPortHandle(channel, 1);
 }
 
 HAL_PortHandle HAL_GetPortWithModule(int32_t module, int32_t channel) {
   // Dont allow a number that wouldn't fit in a uint8_t
-  if (channel < 0 || channel >= 255) return HAL_kInvalidHandle;
-  if (module < 0 || module >= 255) return HAL_kInvalidHandle;
+  if (channel < 0 || channel >= 255) {
+    return HAL_kInvalidHandle;
+  }
+  if (module < 0 || module >= 255) {
+    return HAL_kInvalidHandle;
+  }
   return createPortHandle(channel, module);
 }
 
@@ -223,14 +251,22 @@
       return HAL_CAN_BUFFER_OVERRUN_MESSAGE;
     case HAL_LED_CHANNEL_ERROR:
       return HAL_LED_CHANNEL_ERROR_MESSAGE;
+    case HAL_USE_LAST_ERROR:
+      return HAL_USE_LAST_ERROR_MESSAGE;
+    case HAL_CONSOLE_OUT_ENABLED_ERROR:
+      return HAL_CONSOLE_OUT_ENABLED_ERROR_MESSAGE;
     default:
       return "Unknown error status";
   }
 }
 
-HAL_RuntimeType HAL_GetRuntimeType(void) { return runtimeType; }
+HAL_RuntimeType HAL_GetRuntimeType(void) {
+  return runtimeType;
+}
 
-void HALSIM_SetRuntimeType(HAL_RuntimeType type) { runtimeType = type; }
+void HALSIM_SetRuntimeType(HAL_RuntimeType type) {
+  runtimeType = type;
+}
 
 int32_t HAL_GetFPGAVersion(int32_t* status) {
   return 2018;  // Automatically script this at some point
@@ -240,28 +276,32 @@
   return 0;  // TODO: Find a better number to return;
 }
 
-uint64_t HAL_GetFPGATime(int32_t* status) { return hal::GetFPGATime(); }
+uint64_t HAL_GetFPGATime(int32_t* status) {
+  return hal::GetFPGATime();
+}
 
-uint64_t HAL_ExpandFPGATime(uint32_t unexpanded_lower, int32_t* status) {
+uint64_t HAL_ExpandFPGATime(uint32_t unexpandedLower, int32_t* status) {
   // Capture the current FPGA time.  This will give us the upper half of the
   // clock.
-  uint64_t fpga_time = HAL_GetFPGATime(status);
-  if (*status != 0) return 0;
+  uint64_t fpgaTime = HAL_GetFPGATime(status);
+  if (*status != 0) {
+    return 0;
+  }
 
   // Now, we need to detect the case where the lower bits rolled over after we
   // sampled.  In that case, the upper bits will be 1 bigger than they should
   // be.
 
   // Break it into lower and upper portions.
-  uint32_t lower = fpga_time & 0xffffffffull;
-  uint64_t upper = (fpga_time >> 32) & 0xffffffff;
+  uint32_t lower = fpgaTime & 0xffffffffull;
+  uint64_t upper = (fpgaTime >> 32) & 0xffffffff;
 
   // The time was sampled *before* the current time, so roll it back.
-  if (lower < unexpanded_lower) {
+  if (lower < unexpandedLower) {
     --upper;
   }
 
-  return (upper << 32) + static_cast<uint64_t>(unexpanded_lower);
+  return (upper << 32) + static_cast<uint64_t>(unexpandedLower);
 }
 
 HAL_Bool HAL_GetFPGAButton(int32_t* status) {
@@ -280,11 +320,15 @@
   static std::atomic_bool initialized{false};
   static wpi::mutex initializeMutex;
   // Initial check, as if it's true initialization has finished
-  if (initialized) return true;
+  if (initialized) {
+    return true;
+  }
 
   std::scoped_lock lock(initializeMutex);
   // Second check in case another thread was waiting
-  if (initialized) return true;
+  if (initialized) {
+    return true;
+  }
 
   hal::init::InitializeHAL();
 
@@ -311,8 +355,14 @@
   }
 #endif  // _WIN32
 
-  wpi::outs().SetUnbuffered();
-  if (HAL_LoadExtensions() < 0) return false;
+#ifndef _WIN32
+  setlinebuf(stdin);
+  setlinebuf(stdout);
+#endif
+
+  if (HAL_LoadExtensions() < 0) {
+    return false;
+  }
 
   return true;  // Add initialization if we need to at a later point
 }
@@ -333,6 +383,32 @@
   gOnShutdown.emplace_back(param, func);
 }
 
+void HAL_SimPeriodicBefore(void) {
+  gSimPeriodicBefore();
+}
+
+void HAL_SimPeriodicAfter(void) {
+  gSimPeriodicAfter();
+}
+
+int32_t HALSIM_RegisterSimPeriodicBeforeCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return gSimPeriodicBefore.Register(callback, param);
+}
+
+void HALSIM_CancelSimPeriodicBeforeCallback(int32_t uid) {
+  gSimPeriodicBefore.Cancel(uid);
+}
+
+int32_t HALSIM_RegisterSimPeriodicAfterCallback(
+    HALSIM_SimPeriodicCallback callback, void* param) {
+  return gSimPeriodicAfter.Register(callback, param);
+}
+
+void HALSIM_CancelSimPeriodicAfterCallback(int32_t uid) {
+  gSimPeriodicAfter.Cancel(uid);
+}
+
 int64_t HAL_Report(int32_t resource, int32_t instanceNumber, int32_t context,
                    const char* feature) {
   return 0;  // Do nothing for now
diff --git a/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.cpp b/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.cpp
index 5c2242b..50cc9ab 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALInitializer.h"
 
 #include "hal/HALBase.h"
 
-namespace hal {
-namespace init {
+namespace hal::init {
 std::atomic_bool HAL_IsInitialized{false};
-void RunInitialize() { HAL_Initialize(500, 0); }
-}  // namespace init
-}  // namespace hal
+void RunInitialize() {
+  HAL_Initialize(500, 0);
+}
+}  // namespace hal::init
diff --git a/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.h b/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.h
index 29efac5..308e2f4 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/HALInitializer.h
@@ -1,20 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <atomic>
 
-namespace hal {
-namespace init {
+namespace hal::init {
 extern std::atomic_bool HAL_IsInitialized;
 extern void RunInitialize();
-static inline void CheckInit() {
-  if (HAL_IsInitialized.load(std::memory_order_relaxed)) return;
+inline void CheckInit() {
+  if (HAL_IsInitialized.load(std::memory_order_relaxed)) {
+    return;
+  }
   RunInitialize();
 }
 
@@ -33,8 +31,9 @@
 extern void InitializeDriverStationData();
 extern void InitializeEncoderData();
 extern void InitializeI2CData();
-extern void InitializePCMData();
-extern void InitializePDPData();
+extern void InitializeCTREPCMData();
+extern void InitializeREVPHData();
+extern void InitializePowerDistributionData();
 extern void InitializePWMData();
 extern void InitializeRelayData();
 extern void InitializeRoboRioData();
@@ -50,7 +49,6 @@
 extern void InitializeAnalogOutput();
 extern void InitializeAnalogTrigger();
 extern void InitializeCAN();
-extern void InitializeCompressor();
 extern void InitializeConstants();
 extern void InitializeCounter();
 extern void InitializeDigitalInternal();
@@ -64,16 +62,16 @@
 extern void InitializeMain();
 extern void InitializeMockHooks();
 extern void InitializeNotifier();
-extern void InitializePDP();
+extern void InitializePowerDistribution();
 extern void InitializePorts();
 extern void InitializePower();
+extern void InitializeCTREPCM();
+extern void InitializeREVPH();
 extern void InitializePWM();
 extern void InitializeRelay();
 extern void InitializeSerialPort();
 extern void InitializeSimDevice();
-extern void InitializeSolenoid();
 extern void InitializeSPI();
 extern void InitializeThreads();
 
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
diff --git a/third_party/allwpilib/hal/src/main/native/sim/HALInternal.h b/third_party/allwpilib/hal/src/main/native/sim/HALInternal.h
new file mode 100644
index 0000000..7a7863a
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/HALInternal.h
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <string_view>
+
+namespace hal {
+void SetLastError(int32_t* status, std::string_view value);
+void SetLastErrorIndexOutOfRange(int32_t* status, std::string_view message,
+                                 int32_t minimum, int32_t maximum,
+                                 int32_t channel);
+void SetLastErrorPreviouslyAllocated(int32_t* status, std::string_view message,
+                                     int32_t channel,
+                                     std::string_view previousAllocation);
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/I2C.cpp b/third_party/allwpilib/hal/src/main/native/sim/I2C.cpp
index fe4952f..0b50fe6 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/I2C.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/I2C.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/I2C.h"
 
@@ -12,11 +9,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeI2C() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 void HAL_InitializeI2C(HAL_I2CPort port, int32_t* status) {
@@ -40,5 +35,7 @@
   SimI2CData[port].Read(deviceAddress, buffer, count);
   return 0;
 }
-void HAL_CloseI2C(HAL_I2CPort port) { SimI2CData[port].initialized = false; }
+void HAL_CloseI2C(HAL_I2CPort port) {
+  SimI2CData[port].initialized = false;
+}
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Interrupts.cpp b/third_party/allwpilib/hal/src/main/native/sim/Interrupts.cpp
index 7760f06..e55316d 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Interrupts.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Interrupts.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Interrupts.h"
 
@@ -45,16 +42,12 @@
   HAL_Handle portHandle;
   uint8_t index;
   HAL_AnalogTriggerType trigType;
-  bool watcher;
   int64_t risingTimestamp;
   int64_t fallingTimestamp;
   bool previousState;
   bool fireOnUp;
   bool fireOnDown;
   int32_t callbackId;
-
-  void* callbackParam;
-  HAL_InterruptHandlerFunction callbackFunction;
 };
 
 struct SynchronousWaitData {
@@ -67,13 +60,12 @@
 static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
                              HAL_HandleEnum::Interrupt>* interruptHandles;
 
-typedef HAL_Handle SynchronousWaitDataHandle;
+using SynchronousWaitDataHandle = HAL_Handle;
 static UnlimitedHandleResource<SynchronousWaitDataHandle, SynchronousWaitData,
                                HAL_HandleEnum::Vendor>*
     synchronousInterruptHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeInterrupts() {
   static LimitedHandleResource<HAL_InterruptHandle, Interrupt, kNumInterrupts,
                                HAL_HandleEnum::Interrupt>
@@ -84,12 +76,10 @@
       siH;
   synchronousInterruptHandles = &siH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
-HAL_InterruptHandle HAL_InitializeInterrupts(HAL_Bool watcher,
-                                             int32_t* status) {
+HAL_InterruptHandle HAL_InitializeInterrupts(int32_t* status) {
   hal::init::CheckInit();
   HAL_InterruptHandle handle = interruptHandles->Allocate();
   if (handle == HAL_kInvalidHandle) {
@@ -105,19 +95,10 @@
   anInterrupt->index = getHandleIndex(handle);
   anInterrupt->callbackId = -1;
 
-  anInterrupt->watcher = watcher;
-
   return handle;
 }
-void* HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle,
-                          int32_t* status) {
-  HAL_DisableInterrupts(interruptHandle, status);
-  auto anInterrupt = interruptHandles->Get(interruptHandle);
+void HAL_CleanInterrupts(HAL_InterruptHandle interruptHandle) {
   interruptHandles->Free(interruptHandle);
-  if (anInterrupt == nullptr) {
-    return nullptr;
-  }
-  return anInterrupt->callbackParam;
 }
 
 static void ProcessInterruptDigitalSynchronous(const char* name, void* param,
@@ -128,18 +109,32 @@
   SynchronousWaitDataHandle handle =
       static_cast<SynchronousWaitDataHandle>(handleTmp);
   auto interruptData = synchronousInterruptHandles->Get(handle);
-  if (interruptData == nullptr) return;
+  if (interruptData == nullptr) {
+    return;
+  }
   auto interrupt = interruptHandles->Get(interruptData->interruptHandle);
-  if (interrupt == nullptr) return;
+  if (interrupt == nullptr) {
+    return;
+  }
   // Have a valid interrupt
-  if (value->type != HAL_Type::HAL_BOOLEAN) return;
+  if (value->type != HAL_Type::HAL_BOOLEAN) {
+    return;
+  }
   bool retVal = value->data.v_boolean;
+  auto previousState = interrupt->previousState;
+  interrupt->previousState = retVal;
   // If no change in interrupt, return;
-  if (retVal == interrupt->previousState) return;
+  if (retVal == previousState) {
+    return;
+  }
   // If its a falling change, and we dont fire on falling return
-  if (interrupt->previousState && !interrupt->fireOnDown) return;
+  if (previousState && !interrupt->fireOnDown) {
+    return;
+  }
   // If its a rising change, and we dont fire on rising return.
-  if (!interrupt->previousState && !interrupt->fireOnUp) return;
+  if (!previousState && !interrupt->fireOnUp) {
+    return;
+  }
 
   interruptData->waitPredicate = true;
 
@@ -161,11 +156,17 @@
   SynchronousWaitDataHandle handle =
       static_cast<SynchronousWaitDataHandle>(handleTmp);
   auto interruptData = synchronousInterruptHandles->Get(handle);
-  if (interruptData == nullptr) return;
+  if (interruptData == nullptr) {
+    return;
+  }
   auto interrupt = interruptHandles->Get(interruptData->interruptHandle);
-  if (interrupt == nullptr) return;
+  if (interrupt == nullptr) {
+    return;
+  }
   // Have a valid interrupt
-  if (value->type != HAL_Type::HAL_DOUBLE) return;
+  if (value->type != HAL_Type::HAL_DOUBLE) {
+    return;
+  }
   int32_t status = 0;
   bool retVal = GetAnalogTriggerValue(interrupt->portHandle,
                                       interrupt->trigType, &status);
@@ -175,12 +176,20 @@
     // Pulse interrupt
     interruptData->waitCond.notify_all();
   }
+  auto previousState = interrupt->previousState;
+  interrupt->previousState = retVal;
   // If no change in interrupt, return;
-  if (retVal == interrupt->previousState) return;
+  if (retVal == previousState) {
+    return;
+  }
   // If its a falling change, and we dont fire on falling return
-  if (interrupt->previousState && !interrupt->fireOnDown) return;
+  if (previousState && !interrupt->fireOnDown) {
+    return;
+  }
   // If its a rising change, and we dont fire on rising return.
-  if (!interrupt->previousState && !interrupt->fireOnUp) return;
+  if (!previousState && !interrupt->fireOnUp) {
+    return;
+  }
 
   interruptData->waitPredicate = true;
 
@@ -207,7 +216,9 @@
 
   int32_t digitalIndex = GetDigitalInputChannel(interrupt->portHandle, &status);
 
-  if (status != 0) return WaitResult::Timeout;
+  if (status != 0) {
+    return WaitResult::Timeout;
+  }
 
   interrupt->previousState = SimDIOData[digitalIndex].value;
 
@@ -238,7 +249,9 @@
   (void)synchronousInterruptHandles->Free(dataHandle);
 
   // Check for what to return
-  if (timedOut) return WaitResult::Timeout;
+  if (timedOut) {
+    return WaitResult::Timeout;
+  }
   // True => false, Falling
   if (interrupt->previousState) {
     // Set our return value and our timestamps
@@ -268,12 +281,16 @@
   interrupt->previousState = GetAnalogTriggerValue(
       interrupt->portHandle, interrupt->trigType, &status);
 
-  if (status != 0) return WaitResult::Timeout;
+  if (status != 0) {
+    return WaitResult::Timeout;
+  }
 
   int32_t analogIndex =
       GetAnalogTriggerInputIndex(interrupt->portHandle, &status);
 
-  if (status != 0) return WaitResult::Timeout;
+  if (status != 0) {
+    return WaitResult::Timeout;
+  }
 
   int32_t uid = SimAnalogInData[analogIndex].voltage.RegisterCallback(
       &ProcessInterruptAnalogSynchronous,
@@ -302,7 +319,9 @@
   (void)synchronousInterruptHandles->Free(dataHandle);
 
   // Check for what to return
-  if (timedOut) return WaitResult::Timeout;
+  if (timedOut) {
+    return WaitResult::Timeout;
+  }
   // True => false, Falling
   if (interrupt->previousState) {
     // Set our return value and our timestamps
@@ -323,12 +342,6 @@
     return WaitResult::Timeout;
   }
 
-  // Check to make sure we are actually an interrupt in synchronous mode
-  if (!interrupt->watcher) {
-    *status = NiFpga_Status_InvalidParameter;
-    return WaitResult::Timeout;
-  }
-
   if (interrupt->isAnalog) {
     return WaitForInterruptAnalog(interruptHandle, interrupt.get(), timeout,
                                   ignorePrevious);
@@ -338,158 +351,6 @@
   }
 }
 
-static void ProcessInterruptDigitalAsynchronous(const char* name, void* param,
-                                                const struct HAL_Value* value) {
-  // void* is a HAL handle
-  // convert to uintptr_t first, then to handle
-  uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-  HAL_InterruptHandle handle = static_cast<HAL_InterruptHandle>(handleTmp);
-  auto interrupt = interruptHandles->Get(handle);
-  if (interrupt == nullptr) return;
-  // Have a valid interrupt
-  if (value->type != HAL_Type::HAL_BOOLEAN) return;
-  bool retVal = value->data.v_boolean;
-  // If no change in interrupt, return;
-  if (retVal == interrupt->previousState) return;
-  int32_t mask = 0;
-  if (interrupt->previousState) {
-    interrupt->previousState = retVal;
-    interrupt->fallingTimestamp = hal::GetFPGATime();
-    mask = 1 << (8 + interrupt->index);
-    if (!interrupt->fireOnDown) return;
-  } else {
-    interrupt->previousState = retVal;
-    interrupt->risingTimestamp = hal::GetFPGATime();
-    mask = 1 << (interrupt->index);
-    if (!interrupt->fireOnUp) return;
-  }
-
-  // run callback
-  auto callback = interrupt->callbackFunction;
-  if (callback == nullptr) return;
-  callback(mask, interrupt->callbackParam);
-}
-
-static void ProcessInterruptAnalogAsynchronous(const char* name, void* param,
-                                               const struct HAL_Value* value) {
-  // void* is a HAL handle
-  // convert to intptr_t first, then to handle
-  uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
-  HAL_InterruptHandle handle = static_cast<HAL_InterruptHandle>(handleTmp);
-  auto interrupt = interruptHandles->Get(handle);
-  if (interrupt == nullptr) return;
-  // Have a valid interrupt
-  if (value->type != HAL_Type::HAL_DOUBLE) return;
-  int32_t status = 0;
-  bool retVal = GetAnalogTriggerValue(interrupt->portHandle,
-                                      interrupt->trigType, &status);
-  if (status != 0) return;
-  // If no change in interrupt, return;
-  if (retVal == interrupt->previousState) return;
-  int mask = 0;
-  if (interrupt->previousState) {
-    interrupt->previousState = retVal;
-    interrupt->fallingTimestamp = hal::GetFPGATime();
-    if (!interrupt->fireOnDown) return;
-    mask = 1 << (8 + interrupt->index);
-  } else {
-    interrupt->previousState = retVal;
-    interrupt->risingTimestamp = hal::GetFPGATime();
-    if (!interrupt->fireOnUp) return;
-    mask = 1 << (interrupt->index);
-  }
-
-  // run callback
-  auto callback = interrupt->callbackFunction;
-  if (callback == nullptr) return;
-  callback(mask, interrupt->callbackParam);
-}
-
-static void EnableInterruptsDigital(HAL_InterruptHandle handle,
-                                    Interrupt* interrupt) {
-  int32_t status = 0;
-  int32_t digitalIndex = GetDigitalInputChannel(interrupt->portHandle, &status);
-  if (status != 0) return;
-
-  interrupt->previousState = SimDIOData[digitalIndex].value;
-
-  int32_t uid = SimDIOData[digitalIndex].value.RegisterCallback(
-      &ProcessInterruptDigitalAsynchronous,
-      reinterpret_cast<void*>(static_cast<uintptr_t>(handle)), false);
-  interrupt->callbackId = uid;
-}
-
-static void EnableInterruptsAnalog(HAL_InterruptHandle handle,
-                                   Interrupt* interrupt) {
-  int32_t status = 0;
-  int32_t analogIndex =
-      GetAnalogTriggerInputIndex(interrupt->portHandle, &status);
-  if (status != 0) return;
-
-  status = 0;
-  interrupt->previousState = GetAnalogTriggerValue(
-      interrupt->portHandle, interrupt->trigType, &status);
-  if (status != 0) return;
-
-  int32_t uid = SimAnalogInData[analogIndex].voltage.RegisterCallback(
-      &ProcessInterruptAnalogAsynchronous,
-      reinterpret_cast<void*>(static_cast<uintptr_t>(handle)), false);
-  interrupt->callbackId = uid;
-}
-
-void HAL_EnableInterrupts(HAL_InterruptHandle interruptHandle,
-                          int32_t* status) {
-  auto interrupt = interruptHandles->Get(interruptHandle);
-  if (interrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  // If we have not had a callback set, error out
-  if (interrupt->callbackFunction == nullptr) {
-    *status = INCOMPATIBLE_STATE;
-    return;
-  }
-
-  // EnableInterrupts has already been called
-  if (interrupt->callbackId >= 0) {
-    // We can double enable safely.
-    return;
-  }
-
-  if (interrupt->isAnalog) {
-    EnableInterruptsAnalog(interruptHandle, interrupt.get());
-  } else {
-    EnableInterruptsDigital(interruptHandle, interrupt.get());
-  }
-}
-void HAL_DisableInterrupts(HAL_InterruptHandle interruptHandle,
-                           int32_t* status) {
-  auto interrupt = interruptHandles->Get(interruptHandle);
-  if (interrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  // No need to disable if we are already disabled
-  if (interrupt->callbackId < 0) return;
-
-  if (interrupt->isAnalog) {
-    // Do analog
-    int32_t status = 0;
-    int32_t analogIndex =
-        GetAnalogTriggerInputIndex(interrupt->portHandle, &status);
-    if (status != 0) return;
-    SimAnalogInData[analogIndex].voltage.CancelCallback(interrupt->callbackId);
-  } else {
-    int32_t status = 0;
-    int32_t digitalIndex =
-        GetDigitalInputChannel(interrupt->portHandle, &status);
-    if (status != 0) return;
-    SimDIOData[digitalIndex].value.CancelCallback(interrupt->callbackId);
-  }
-  interrupt->callbackId = -1;
-}
 int64_t HAL_ReadInterruptRisingTimestamp(HAL_InterruptHandle interruptHandle,
                                          int32_t* status) {
   auto interrupt = interruptHandles->Get(interruptHandle);
@@ -535,24 +396,6 @@
   interrupt->trigType = analogTriggerType;
   interrupt->portHandle = digitalSourceHandle;
 }
-void HAL_AttachInterruptHandler(HAL_InterruptHandle interruptHandle,
-                                HAL_InterruptHandlerFunction handler,
-                                void* param, int32_t* status) {
-  auto interrupt = interruptHandles->Get(interruptHandle);
-  if (interrupt == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  interrupt->callbackFunction = handler;
-  interrupt->callbackParam = param;
-}
-
-void HAL_AttachInterruptHandlerThreaded(HAL_InterruptHandle interruptHandle,
-                                        HAL_InterruptHandlerFunction handler,
-                                        void* param, int32_t* status) {
-  HAL_AttachInterruptHandler(interruptHandle, handler, param, status);
-}
 
 void HAL_SetInterruptUpSourceEdge(HAL_InterruptHandle interruptHandle,
                                   HAL_Bool risingEdge, HAL_Bool fallingEdge,
@@ -569,6 +412,19 @@
 
 void HAL_ReleaseWaitingInterrupt(HAL_InterruptHandle interruptHandle,
                                  int32_t* status) {
-  // Requires a fairly large rewrite to get working
+  auto interrupt = interruptHandles->Get(interruptHandle);
+  if (interrupt == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  synchronousInterruptHandles->ForEach(
+      [interruptHandle](SynchronousWaitDataHandle handle,
+                        SynchronousWaitData* data) {
+        if (data->interruptHandle == interruptHandle) {
+          data->waitPredicate = true;
+          data->waitCond.notify_all();
+        }
+      });
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/MockHooks.cpp b/third_party/allwpilib/hal/src/main/native/sim/MockHooks.cpp
index 8fe387f..4e20c6a 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/MockHooks.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/MockHooks.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <algorithm>
 #include <atomic>
@@ -11,6 +8,7 @@
 #include <cstdio>
 #include <thread>
 
+#include <fmt/format.h>
 #include <wpi/timestamp.h>
 
 #include "MockHooksInternal.h"
@@ -23,21 +21,25 @@
 static std::atomic<uint64_t> programPauseTime{0};
 static std::atomic<uint64_t> programStepTime{0};
 
-namespace hal {
-namespace init {
-void InitializeMockHooks() { wpi::SetNowImpl(GetFPGATime); }
-}  // namespace init
-}  // namespace hal
+namespace hal::init {
+void InitializeMockHooks() {
+  wpi::SetNowImpl(GetFPGATime);
+}
+}  // namespace hal::init
 
 namespace hal {
 void RestartTiming() {
   programStartTime = wpi::NowDefault();
   programStepTime = 0;
-  if (programPauseTime != 0) programPauseTime = programStartTime.load();
+  if (programPauseTime != 0) {
+    programPauseTime = programStartTime.load();
+  }
 }
 
 void PauseTiming() {
-  if (programPauseTime == 0) programPauseTime = wpi::NowDefault();
+  if (programPauseTime == 0) {
+    programPauseTime = wpi::NowDefault();
+  }
 }
 
 void ResumeTiming() {
@@ -47,20 +49,32 @@
   }
 }
 
-bool IsTimingPaused() { return programPauseTime != 0; }
+bool IsTimingPaused() {
+  return programPauseTime != 0;
+}
 
-void StepTiming(uint64_t delta) { programStepTime += delta; }
+void StepTiming(uint64_t delta) {
+  programStepTime += delta;
+}
 
 uint64_t GetFPGATime() {
   uint64_t curTime = programPauseTime;
-  if (curTime == 0) curTime = wpi::NowDefault();
+  if (curTime == 0) {
+    curTime = wpi::NowDefault();
+  }
   return curTime + programStepTime - programStartTime;
 }
 
-double GetFPGATimestamp() { return GetFPGATime() * 1.0e-6; }
+double GetFPGATimestamp() {
+  return GetFPGATime() * 1.0e-6;
+}
 
-void SetProgramStarted() { programStarted = true; }
-bool GetProgramStarted() { return programStarted; }
+void SetProgramStarted() {
+  programStarted = true;
+}
+bool GetProgramStarted() {
+  return programStarted;
+}
 }  // namespace hal
 
 using namespace hal;
@@ -70,16 +84,22 @@
   int count = 0;
   while (!programStarted) {
     count++;
-    std::printf("Waiting for program start signal: %d\n", count);
+    fmt::print("Waiting for program start signal: {}\n", count);
     std::this_thread::sleep_for(std::chrono::milliseconds(500));
   }
 }
 
-void HALSIM_SetProgramStarted(void) { SetProgramStarted(); }
+void HALSIM_SetProgramStarted(void) {
+  SetProgramStarted();
+}
 
-HAL_Bool HALSIM_GetProgramStarted(void) { return GetProgramStarted(); }
+HAL_Bool HALSIM_GetProgramStarted(void) {
+  return GetProgramStarted();
+}
 
-void HALSIM_RestartTiming(void) { RestartTiming(); }
+void HALSIM_RestartTiming(void) {
+  RestartTiming();
+}
 
 void HALSIM_PauseTiming(void) {
   PauseTiming();
@@ -91,7 +111,9 @@
   ResumeNotifiers();
 }
 
-HAL_Bool HALSIM_IsTimingPaused(void) { return IsTimingPaused(); }
+HAL_Bool HALSIM_IsTimingPaused(void) {
+  return IsTimingPaused();
+}
 
 void HALSIM_StepTiming(uint64_t delta) {
   WaitNotifiers();
diff --git a/third_party/allwpilib/hal/src/main/native/sim/MockHooksInternal.h b/third_party/allwpilib/hal/src/main/native/sim/MockHooksInternal.h
index c53f120..ace09da 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/MockHooksInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/MockHooksInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Notifier.cpp b/third_party/allwpilib/hal/src/main/native/sim/Notifier.cpp
index a697275..49f1c80 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Notifier.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Notifier.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Notifier.h"
 
@@ -71,7 +68,9 @@
 }
 }  // namespace init
 
-void PauseNotifiers() { notifiersPaused = true; }
+void PauseNotifiers() {
+  notifiersPaused = true;
+}
 
 void ResumeNotifiers() {
   notifiersPaused = false;
@@ -110,7 +109,9 @@
       // No longer need to wait for it, put at end so it can be erased
       std::swap(it, waiters[--end]);
     }
-    if (count == 0) break;
+    if (count == 0) {
+      break;
+    }
     waiters.resize(count);
     notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
   }
@@ -151,7 +152,9 @@
       // No longer need to wait for it, put at end so it can be erased
       it.swap(waiters[--end]);
     }
-    if (count == 0) break;
+    if (count == 0) {
+      break;
+    }
     waiters.resize(count);
     notifiersWaiterCond.wait_for(ulock, std::chrono::duration<double>(1));
   }
@@ -171,17 +174,26 @@
   return handle;
 }
 
+HAL_Bool HAL_SetNotifierThreadPriority(HAL_Bool realTime, int32_t priority,
+                                       int32_t* status) {
+  return true;
+}
+
 void HAL_SetNotifierName(HAL_NotifierHandle notifierHandle, const char* name,
                          int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier) {
+    return;
+  }
   std::scoped_lock lock(notifier->mutex);
   notifier->name = name;
 }
 
 void HAL_StopNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier) {
+    return;
+  }
 
   {
     std::scoped_lock lock(notifier->mutex);
@@ -193,7 +205,9 @@
 
 void HAL_CleanNotifier(HAL_NotifierHandle notifierHandle, int32_t* status) {
   auto notifier = notifierHandles->Free(notifierHandle);
-  if (!notifier) return;
+  if (!notifier) {
+    return;
+  }
 
   // Just in case HAL_StopNotifier() wasn't called...
   {
@@ -207,7 +221,9 @@
 void HAL_UpdateNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              uint64_t triggerTime, int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier) {
+    return;
+  }
 
   {
     std::scoped_lock lock(notifier->mutex);
@@ -222,7 +238,9 @@
 void HAL_CancelNotifierAlarm(HAL_NotifierHandle notifierHandle,
                              int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return;
+  if (!notifier) {
+    return;
+  }
 
   {
     std::scoped_lock lock(notifier->mutex);
@@ -233,7 +251,9 @@
 uint64_t HAL_WaitForNotifierAlarm(HAL_NotifierHandle notifierHandle,
                                   int32_t* status) {
   auto notifier = notifierHandles->Get(notifierHandle);
-  if (!notifier) return 0;
+  if (!notifier) {
+    return 0;
+  }
 
   std::unique_lock ulock(notifiersWaiterMutex);
   std::unique_lock lock(notifier->mutex);
@@ -268,8 +288,9 @@
   notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
     std::scoped_lock lock(notifier->mutex);
     if (notifier->active && notifier->waitTimeValid &&
-        timeout > notifier->waitTime)
+        timeout > notifier->waitTime) {
       timeout = notifier->waitTime;
+    }
   });
   return timeout;
 }
@@ -278,7 +299,9 @@
   int32_t count = 0;
   notifierHandles->ForEach([&](HAL_NotifierHandle, Notifier* notifier) {
     std::scoped_lock lock(notifier->mutex);
-    if (notifier->active) ++count;
+    if (notifier->active) {
+      ++count;
+    }
   });
   return count;
 }
@@ -287,7 +310,9 @@
   int32_t num = 0;
   notifierHandles->ForEach([&](HAL_NotifierHandle handle, Notifier* notifier) {
     std::scoped_lock lock(notifier->mutex);
-    if (!notifier->active) return;
+    if (!notifier->active) {
+      return;
+    }
     if (num < size) {
       arr[num].handle = handle;
       if (notifier->name.empty()) {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/NotifierInternal.h b/third_party/allwpilib/hal/src/main/native/sim/NotifierInternal.h
index e6692ca..81d906e 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/NotifierInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/NotifierInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/PDP.cpp b/third_party/allwpilib/hal/src/main/native/sim/PDP.cpp
deleted file mode 100644
index cc095d0..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/PDP.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/PDP.h"
-
-#include "CANAPIInternal.h"
-#include "HALInitializer.h"
-#include "PortsInternal.h"
-#include "hal/CANAPI.h"
-#include "hal/Errors.h"
-#include "mockdata/PDPDataInternal.h"
-
-using namespace hal;
-
-static constexpr HAL_CANManufacturer manufacturer =
-    HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
-
-static constexpr HAL_CANDeviceType deviceType =
-    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
-
-namespace hal {
-namespace init {
-void InitializePDP() {}
-}  // namespace init
-}  // namespace hal
-
-extern "C" {
-HAL_PDPHandle HAL_InitializePDP(int32_t module, int32_t* status) {
-  if (!HAL_CheckPDPModule(module)) {
-    *status = PARAMETER_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
-  }
-  hal::init::CheckInit();
-  SimPDPData[module].initialized = true;
-  auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
-
-  if (*status != 0) {
-    HAL_CleanCAN(handle);
-    return HAL_kInvalidHandle;
-  }
-
-  return handle;
-}
-
-HAL_Bool HAL_CheckPDPModule(int32_t module) {
-  return module < kNumPDPModules && module >= 0;
-}
-
-HAL_Bool HAL_CheckPDPChannel(int32_t channel) {
-  return channel < kNumPDPChannels && channel >= 0;
-}
-
-void HAL_CleanPDP(HAL_PDPHandle handle) { HAL_CleanCAN(handle); }
-
-double HAL_GetPDPTemperature(HAL_PDPHandle handle, int32_t* status) {
-  auto module = hal::can::GetCANModuleFromHandle(handle, status);
-  if (*status != 0) {
-    return 0.0;
-  }
-  return SimPDPData[module].temperature;
-}
-double HAL_GetPDPVoltage(HAL_PDPHandle handle, int32_t* status) {
-  auto module = hal::can::GetCANModuleFromHandle(handle, status);
-  if (*status != 0) {
-    return 0.0;
-  }
-  return SimPDPData[module].voltage;
-}
-double HAL_GetPDPChannelCurrent(HAL_PDPHandle handle, int32_t channel,
-                                int32_t* status) {
-  auto module = hal::can::GetCANModuleFromHandle(handle, status);
-  if (*status != 0) {
-    return 0.0;
-  }
-  return SimPDPData[module].current[channel];
-}
-void HAL_GetPDPAllChannelCurrents(HAL_PDPHandle handle, double* currents,
-                                  int32_t* status) {
-  auto module = hal::can::GetCANModuleFromHandle(handle, status);
-  if (*status != 0) {
-    return;
-  }
-
-  auto& data = SimPDPData[module];
-  for (int i = 0; i < kNumPDPChannels; i++) {
-    currents[i] = data.current[i];
-  }
-}
-double HAL_GetPDPTotalCurrent(HAL_PDPHandle handle, int32_t* status) {
-  return 0.0;
-}
-double HAL_GetPDPTotalPower(HAL_PDPHandle handle, int32_t* status) {
-  return 0.0;
-}
-double HAL_GetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {
-  return 0.0;
-}
-void HAL_ResetPDPTotalEnergy(HAL_PDPHandle handle, int32_t* status) {}
-void HAL_ClearPDPStickyFaults(HAL_PDPHandle handle, int32_t* status) {}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/PWM.cpp b/third_party/allwpilib/hal/src/main/native/sim/PWM.cpp
index 228b540..698769f 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/PWM.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/PWM.cpp
@@ -1,37 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/PWM.h"
 
 #include "ConstantsInternal.h"
 #include "DigitalInternal.h"
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/HandlesInternal.h"
 #include "mockdata/PWMDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePWM() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
 HAL_DigitalHandle HAL_InitializePWMPort(HAL_PortHandle portHandle,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
-  if (*status != 0) return HAL_kInvalidHandle;
 
   int16_t channel = getPortHandleChannel(portHandle);
   if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                     kNumPWMChannels, channel);
     return HAL_kInvalidHandle;
   }
 
@@ -43,16 +41,20 @@
     channel = remapMXPPWMChannel(channel) + 10;  // remap MXP to proper channel
   }
 
-  auto handle =
-      digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM, status);
+  HAL_DigitalHandle handle;
 
-  if (*status != 0)
+  auto port = digitalChannelHandles->Allocate(channel, HAL_HandleEnum::PWM,
+                                              &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "PWM or DIO", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for PWM", 0,
+                                       kNumPWMChannels, channel);
+    }
     return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = digitalChannelHandles->Get(handle, HAL_HandleEnum::PWM);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
   }
 
   port->channel = origChannel;
@@ -62,6 +64,8 @@
   // Defaults to allow an always valid config.
   HAL_SetPWMConfig(handle, 2.0, 1.501, 1.5, 1.499, 1.0, status);
 
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
+
   return handle;
 }
 void HAL_FreePWMPort(HAL_DigitalHandle pwmPortHandle, int32_t* status) {
@@ -92,7 +96,9 @@
   // calculate the loop time in milliseconds
   double loopTime =
       HAL_GetPWMLoopTiming(status) / (kSystemClockTicksPerMicrosecond * 1e3);
-  if (*status != 0) return;
+  if (*status != 0) {
+    return;
+  }
 
   int32_t maxPwm = static_cast<int32_t>((max - kDefaultPwmCenter) / loopTime +
                                         kDefaultPwmStepsDown - 1);
@@ -252,8 +258,12 @@
   }
 
   double speed = SimPWMData[port->channel].speed;
-  if (speed > 1) speed = 1;
-  if (speed < -1) speed = -1;
+  if (speed > 1) {
+    speed = 1;
+  }
+  if (speed < -1) {
+    speed = -1;
+  }
   return speed;
 }
 
@@ -269,8 +279,12 @@
   }
 
   double position = SimPWMData[port->channel].position;
-  if (position > 1) position = 1;
-  if (position < 0) position = 0;
+  if (position > 1) {
+    position = 1;
+  }
+  if (position < 0) {
+    position = 0;
+  }
   return position;
 }
 
@@ -296,7 +310,11 @@
   SimPWMData[port->channel].periodScale = squelchMask;
 }
 
-int32_t HAL_GetPWMLoopTiming(int32_t* status) { return kExpectedLoopTiming; }
+int32_t HAL_GetPWMLoopTiming(int32_t* status) {
+  return kExpectedLoopTiming;
+}
 
-uint64_t HAL_GetPWMCycleStartTime(int32_t* status) { return 0; }
+uint64_t HAL_GetPWMCycleStartTime(int32_t* status) {
+  return 0;
+}
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Ports.cpp b/third_party/allwpilib/hal/src/main/native/sim/Ports.cpp
index 2f670b3..08ca975 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Ports.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Ports.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Ports.h"
 
@@ -11,31 +8,81 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePorts() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
-int32_t HAL_GetNumAccumulators(void) { return kNumAccumulators; }
-int32_t HAL_GetNumAnalogTriggers(void) { return kNumAnalogTriggers; }
-int32_t HAL_GetNumAnalogInputs(void) { return kNumAnalogInputs; }
-int32_t HAL_GetNumAnalogOutputs(void) { return kNumAnalogOutputs; }
-int32_t HAL_GetNumCounters(void) { return kNumCounters; }
-int32_t HAL_GetNumDigitalHeaders(void) { return kNumDigitalHeaders; }
-int32_t HAL_GetNumPWMHeaders(void) { return kNumPWMHeaders; }
-int32_t HAL_GetNumDigitalChannels(void) { return kNumDigitalChannels; }
-int32_t HAL_GetNumPWMChannels(void) { return kNumPWMChannels; }
-int32_t HAL_GetNumDigitalPWMOutputs(void) { return kNumDigitalPWMOutputs; }
-int32_t HAL_GetNumEncoders(void) { return kNumEncoders; }
-int32_t HAL_GetNumInterrupts(void) { return kNumInterrupts; }
-int32_t HAL_GetNumRelayChannels(void) { return kNumRelayChannels; }
-int32_t HAL_GetNumRelayHeaders(void) { return kNumRelayHeaders; }
-int32_t HAL_GetNumPCMModules(void) { return kNumPCMModules; }
-int32_t HAL_GetNumSolenoidChannels(void) { return kNumSolenoidChannels; }
-int32_t HAL_GetNumPDPModules(void) { return kNumPDPModules; }
-int32_t HAL_GetNumPDPChannels(void) { return kNumPDPChannels; }
-int32_t HAL_GetNumDutyCycles(void) { return kNumDutyCycles; }
-int32_t HAL_GetNumAddressableLEDs(void) { return kNumAddressableLEDs; }
+int32_t HAL_GetNumAccumulators(void) {
+  return kNumAccumulators;
+}
+int32_t HAL_GetNumAnalogTriggers(void) {
+  return kNumAnalogTriggers;
+}
+int32_t HAL_GetNumAnalogInputs(void) {
+  return kNumAnalogInputs;
+}
+int32_t HAL_GetNumAnalogOutputs(void) {
+  return kNumAnalogOutputs;
+}
+int32_t HAL_GetNumCounters(void) {
+  return kNumCounters;
+}
+int32_t HAL_GetNumDigitalHeaders(void) {
+  return kNumDigitalHeaders;
+}
+int32_t HAL_GetNumPWMHeaders(void) {
+  return kNumPWMHeaders;
+}
+int32_t HAL_GetNumDigitalChannels(void) {
+  return kNumDigitalChannels;
+}
+int32_t HAL_GetNumPWMChannels(void) {
+  return kNumPWMChannels;
+}
+int32_t HAL_GetNumDigitalPWMOutputs(void) {
+  return kNumDigitalPWMOutputs;
+}
+int32_t HAL_GetNumEncoders(void) {
+  return kNumEncoders;
+}
+int32_t HAL_GetNumInterrupts(void) {
+  return kNumInterrupts;
+}
+int32_t HAL_GetNumRelayChannels(void) {
+  return kNumRelayChannels;
+}
+int32_t HAL_GetNumRelayHeaders(void) {
+  return kNumRelayHeaders;
+}
+int32_t HAL_GetNumCTREPCMModules(void) {
+  return kNumCTREPCMModules;
+}
+int32_t HAL_GetNumCTRESolenoidChannels(void) {
+  return kNumCTRESolenoidChannels;
+}
+int32_t HAL_GetNumCTREPDPModules(void) {
+  return kNumCTREPDPModules;
+}
+int32_t HAL_GetNumCTREPDPChannels(void) {
+  return kNumCTREPDPChannels;
+}
+int32_t HAL_GetNumREVPDHModules(void) {
+  return kNumREVPDHModules;
+}
+int32_t HAL_GetNumREVPDHChannels(void) {
+  return kNumREVPDHChannels;
+}
+int32_t HAL_GetNumREVPHModules(void) {
+  return kNumREVPHModules;
+}
+int32_t HAL_GetNumREVPHChannels(void) {
+  return kNumREVPHChannels;
+}
+int32_t HAL_GetNumDutyCycles(void) {
+  return kNumDutyCycles;
+}
+int32_t HAL_GetNumAddressableLEDs(void) {
+  return kNumAddressableLEDs;
+}
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/PortsInternal.h b/third_party/allwpilib/hal/src/main/native/sim/PortsInternal.h
index d143aca..2d1a205 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/PortsInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/PortsInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,10 +21,14 @@
 constexpr int32_t kNumInterrupts = 8;
 constexpr int32_t kNumRelayChannels = 8;
 constexpr int32_t kNumRelayHeaders = kNumRelayChannels / 2;
-constexpr int32_t kNumPCMModules = 63;
-constexpr int32_t kNumSolenoidChannels = 8;
-constexpr int32_t kNumPDPModules = 63;
-constexpr int32_t kNumPDPChannels = 16;
+constexpr int32_t kNumCTREPCMModules = 63;
+constexpr int32_t kNumCTRESolenoidChannels = 8;
+constexpr int32_t kNumCTREPDPModules = 63;
+constexpr int32_t kNumCTREPDPChannels = 16;
+constexpr int32_t kNumREVPDHModules = 63;
+constexpr int32_t kNumREVPDHChannels = 24;
 constexpr int32_t kNumDutyCycles = 8;
 constexpr int32_t kNumAddressableLEDs = 1;
+constexpr int32_t kNumREVPHModules = 63;
+constexpr int32_t kNumREVPHChannels = 16;
 }  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Power.cpp b/third_party/allwpilib/hal/src/main/native/sim/Power.cpp
index e8ce710..08d5638 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Power.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Power.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Power.h"
 
@@ -11,16 +8,18 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePower() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 // TODO: Fix the naming in here
 extern "C" {
-double HAL_GetVinVoltage(int32_t* status) { return SimRoboRioData->vInVoltage; }
-double HAL_GetVinCurrent(int32_t* status) { return SimRoboRioData->vInCurrent; }
+double HAL_GetVinVoltage(int32_t* status) {
+  return SimRoboRioData->vInVoltage;
+}
+double HAL_GetVinCurrent(int32_t* status) {
+  return SimRoboRioData->vInCurrent;
+}
 double HAL_GetUserVoltage6V(int32_t* status) {
   return SimRoboRioData->userVoltage6V;
 }
@@ -57,4 +56,10 @@
 int32_t HAL_GetUserCurrentFaults3V3(int32_t* status) {
   return SimRoboRioData->userFaults3V3;
 }
+void HAL_SetBrownoutVoltage(double voltage, int32_t* status) {
+  SimRoboRioData->brownoutVoltage = voltage;
+}
+double HAL_GetBrownoutVoltage(int32_t* status) {
+  return SimRoboRioData->brownoutVoltage;
+}
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/PowerDistribution.cpp b/third_party/allwpilib/hal/src/main/native/sim/PowerDistribution.cpp
new file mode 100644
index 0000000..85646cf
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/PowerDistribution.cpp
@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/PowerDistribution.h"
+
+#include <fmt/format.h>
+
+#include "CANAPIInternal.h"
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "mockdata/PowerDistributionDataInternal.h"
+
+using namespace hal;
+
+static constexpr HAL_CANManufacturer manufacturer =
+    HAL_CANManufacturer::HAL_CAN_Man_kCTRE;
+
+static constexpr HAL_CANDeviceType deviceType =
+    HAL_CANDeviceType::HAL_CAN_Dev_kPowerDistribution;
+
+namespace hal::init {
+void InitializePowerDistribution() {}
+}  // namespace hal::init
+
+extern "C" {
+HAL_PowerDistributionHandle HAL_InitializePowerDistribution(
+    int32_t module, HAL_PowerDistributionType type,
+    const char* allocationLocation, int32_t* status) {
+  if (!HAL_CheckPowerDistributionModule(module, type)) {
+    *status = PARAMETER_OUT_OF_RANGE;
+    hal::SetLastError(status, fmt::format("Invalid pdp module {}", module));
+    return HAL_kInvalidHandle;
+  }
+  hal::init::CheckInit();
+  SimPowerDistributionData[module].initialized = true;
+  auto handle = HAL_InitializeCAN(manufacturer, module, deviceType, status);
+
+  if (*status != 0) {
+    HAL_CleanCAN(handle);
+    return HAL_kInvalidHandle;
+  }
+
+  return handle;
+}
+
+int32_t HAL_GetPowerDistributionModuleNumber(HAL_PowerDistributionHandle handle,
+                                             int32_t* status) {
+  auto module = hal::can::GetCANModuleFromHandle(handle, status);
+  if (*status != 0) {
+    return 0;
+  }
+  return module;
+}
+
+HAL_Bool HAL_CheckPowerDistributionModule(int32_t module,
+                                          HAL_PowerDistributionType type) {
+  if (type == HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE) {
+    return module < kNumCTREPDPModules && module >= 0;
+  } else {
+    return module < kNumREVPDHModules && module >= 1;
+  }
+}
+
+HAL_Bool HAL_CheckPowerDistributionChannel(HAL_PowerDistributionHandle handle,
+                                           int32_t channel) {
+  // TODO make this grab from the handle directly
+  if (false) {
+    return channel < kNumCTREPDPChannels && channel >= 0;
+  } else {
+    return channel < kNumREVPDHChannels && channel >= 0;
+  }
+}
+
+HAL_PowerDistributionType HAL_GetPowerDistributionType(
+    HAL_PowerDistributionHandle handle, int32_t* status) {
+  return HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE;
+}
+
+int32_t HAL_GetPowerDistributionNumChannels(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {
+  // TODO make this grab from the handle directly
+  if (false) {
+    return kNumCTREPDPChannels;
+  } else {
+    return kNumREVPDHChannels;
+  }
+}
+
+void HAL_CleanPowerDistribution(HAL_PowerDistributionHandle handle) {
+  HAL_CleanCAN(handle);
+}
+
+double HAL_GetPowerDistributionTemperature(HAL_PowerDistributionHandle handle,
+                                           int32_t* status) {
+  auto module = hal::can::GetCANModuleFromHandle(handle, status);
+  if (*status != 0) {
+    return 0.0;
+  }
+  return SimPowerDistributionData[module].temperature;
+}
+double HAL_GetPowerDistributionVoltage(HAL_PowerDistributionHandle handle,
+                                       int32_t* status) {
+  auto module = hal::can::GetCANModuleFromHandle(handle, status);
+  if (*status != 0) {
+    return 0.0;
+  }
+  return SimPowerDistributionData[module].voltage;
+}
+double HAL_GetPowerDistributionChannelCurrent(
+    HAL_PowerDistributionHandle handle, int32_t channel, int32_t* status) {
+  auto module = hal::can::GetCANModuleFromHandle(handle, status);
+  if (*status != 0) {
+    return 0.0;
+  }
+  return SimPowerDistributionData[module].current[channel];
+}
+void HAL_GetPowerDistributionAllChannelCurrents(
+    HAL_PowerDistributionHandle handle, double* currents,
+    int32_t currentsLength, int32_t* status) {
+  auto module = hal::can::GetCANModuleFromHandle(handle, status);
+  if (*status != 0) {
+    return;
+  }
+
+  auto& data = SimPowerDistributionData[module];
+  int toCopy = (std::min)(currentsLength, kNumPDSimChannels);
+  for (int i = 0; i < toCopy; i++) {
+    currents[i] = data.current[i];
+  }
+}
+double HAL_GetPowerDistributionTotalCurrent(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {
+  return 0.0;
+}
+double HAL_GetPowerDistributionTotalPower(HAL_PowerDistributionHandle handle,
+                                          int32_t* status) {
+  return 0.0;
+}
+double HAL_GetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
+                                           int32_t* status) {
+  return 0.0;
+}
+void HAL_ResetPowerDistributionTotalEnergy(HAL_PowerDistributionHandle handle,
+                                           int32_t* status) {}
+void HAL_ClearPowerDistributionStickyFaults(HAL_PowerDistributionHandle handle,
+                                            int32_t* status) {}
+void HAL_SetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, HAL_Bool state, int32_t* status) {}
+
+HAL_Bool HAL_GetPowerDistributionSwitchableChannel(
+    HAL_PowerDistributionHandle handle, int32_t* status) {
+  return false;
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/REVPH.cpp b/third_party/allwpilib/hal/src/main/native/sim/REVPH.cpp
new file mode 100644
index 0000000..9ddb655
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/REVPH.cpp
@@ -0,0 +1,181 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "hal/REVPH.h"
+
+#include "HALInitializer.h"
+#include "HALInternal.h"
+#include "PortsInternal.h"
+#include "hal/CANAPI.h"
+#include "hal/Errors.h"
+#include "hal/handles/IndexedHandleResource.h"
+#include "mockdata/REVPHDataInternal.h"
+
+using namespace hal;
+
+namespace {
+struct PCM {
+  int32_t module;
+  wpi::mutex lock;
+  std::string previousAllocation;
+};
+}  // namespace
+
+static IndexedHandleResource<HAL_REVPHHandle, PCM, kNumREVPHModules,
+                             HAL_HandleEnum::REVPH>* pcmHandles;
+
+namespace hal::init {
+void InitializeREVPH() {
+  static IndexedHandleResource<HAL_REVPHHandle, PCM, kNumREVPHModules,
+                               HAL_HandleEnum::REVPH>
+      pH;
+  pcmHandles = &pH;
+}
+}  // namespace hal::init
+
+HAL_REVPHHandle HAL_InitializeREVPH(int32_t module,
+                                    const char* allocationLocation,
+                                    int32_t* status) {
+  hal::init::CheckInit();
+
+  if (module == 0) {
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
+                                     kNumREVPHModules, module);
+    return HAL_kInvalidHandle;
+  }
+
+  HAL_REVPHHandle handle;
+  auto pcm = pcmHandles->Allocate(module, &handle, status);
+
+  if (*status != 0) {
+    if (pcm) {
+      hal::SetLastErrorPreviouslyAllocated(status, "REV PH", module,
+                                           pcm->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for REV PH", 1,
+                                       kNumREVPHModules, module);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  pcm->previousAllocation = allocationLocation ? allocationLocation : "";
+  pcm->module = module;
+
+  SimREVPHData[module].initialized = true;
+  // Enable closed loop
+  SimREVPHData[module].closedLoopEnabled = true;
+
+  return handle;
+}
+
+void HAL_FreeREVPH(HAL_REVPHHandle handle) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    pcmHandles->Free(handle);
+    return;
+  }
+  SimREVPHData[pcm->module].initialized = false;
+  pcmHandles->Free(handle);
+}
+
+HAL_Bool HAL_CheckREVPHModuleNumber(int32_t module) {
+  return module >= 1 && module < kNumREVPDHModules;
+}
+
+HAL_Bool HAL_CheckREVPHSolenoidChannel(int32_t channel) {
+  return channel < kNumREVPHChannels && channel >= 0;
+}
+
+HAL_Bool HAL_GetREVPHCompressor(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimREVPHData[pcm->module].compressorOn;
+}
+
+void HAL_SetREVPHClosedLoopControl(HAL_REVPHHandle handle, HAL_Bool enabled,
+                                   int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  SimREVPHData[pcm->module].closedLoopEnabled = enabled;
+}
+
+HAL_Bool HAL_GetREVPHClosedLoopControl(HAL_REVPHHandle handle,
+                                       int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimREVPHData[pcm->module].closedLoopEnabled;
+}
+
+HAL_Bool HAL_GetREVPHPressureSwitch(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return false;
+  }
+
+  return SimREVPHData[pcm->module].pressureSwitch;
+}
+
+double HAL_GetREVPHAnalogPressure(HAL_REVPHHandle handle, int32_t channel,
+                                  int32_t* status) {
+  return 0;
+}
+
+double HAL_GetREVPHCompressorCurrent(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  return SimREVPHData[pcm->module].compressorCurrent;
+}
+
+int32_t HAL_GetREVPHSolenoids(HAL_REVPHHandle handle, int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  std::scoped_lock lock{pcm->lock};
+  auto& data = SimREVPHData[pcm->module].solenoidOutput;
+  uint8_t ret = 0;
+  for (int i = 0; i < kNumREVPHChannels; i++) {
+    ret |= (data[i] << i);
+  }
+  return ret;
+}
+void HAL_SetREVPHSolenoids(HAL_REVPHHandle handle, int32_t mask, int32_t values,
+                           int32_t* status) {
+  auto pcm = pcmHandles->Get(handle);
+  if (pcm == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  auto& data = SimREVPHData[pcm->module].solenoidOutput;
+  std::scoped_lock lock{pcm->lock};
+  for (int i = 0; i < kNumREVPHChannels; i++) {
+    auto indexMask = (1 << i);
+    if ((mask & indexMask) != 0) {
+      data[i] = (values & indexMask) != 0;
+    }
+  }
+}
+
+void HAL_FireREVPHOneShot(HAL_REVPHHandle handle, int32_t index, int32_t durMs,
+                          int32_t* status) {}
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Relay.cpp b/third_party/allwpilib/hal/src/main/native/sim/Relay.cpp
index acecec4..4dfcdaf 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Relay.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Relay.cpp
@@ -1,13 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Relay.h"
 
+#include <string>
+
 #include "HALInitializer.h"
+#include "HALInternal.h"
 #include "PortsInternal.h"
 #include "hal/handles/IndexedHandleResource.h"
 #include "mockdata/RelayDataInternal.h"
@@ -18,49 +18,58 @@
 struct Relay {
   uint8_t channel;
   bool fwd;
+  std::string previousAllocation;
 };
 }  // namespace
 
 static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
                              HAL_HandleEnum::Relay>* relayHandles;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeRelay() {
   static IndexedHandleResource<HAL_RelayHandle, Relay, kNumRelayChannels,
                                HAL_HandleEnum::Relay>
       rH;
   relayHandles = &rH;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_RelayHandle HAL_InitializeRelayPort(HAL_PortHandle portHandle, HAL_Bool fwd,
+                                        const char* allocationLocation,
                                         int32_t* status) {
   hal::init::CheckInit();
-  if (*status != 0) return HAL_kInvalidHandle;
-
-  int16_t channel = getPortHandleChannel(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = PARAMETER_OUT_OF_RANGE;
+  if (*status != 0) {
     return HAL_kInvalidHandle;
   }
 
-  if (!fwd) channel += kNumRelayHeaders;  // add 4 to reverse channels
-
-  auto handle = relayHandles->Allocate(channel, status);
-
-  if (*status != 0)
-    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
-
-  auto port = relayHandles->Get(handle);
-  if (port == nullptr) {  // would only occur on thread issue.
-    *status = HAL_HANDLE_ERROR;
+  int16_t channel = getPortHandleChannel(portHandle);
+  if (channel == InvalidHandleIndex || channel >= kNumRelayChannels) {
+    *status = RESOURCE_OUT_OF_RANGE;
+    hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
+                                     kNumRelayChannels, channel);
     return HAL_kInvalidHandle;
   }
 
   if (!fwd) {
+    channel += kNumRelayHeaders;  // add 4 to reverse channels
+  }
+
+  HAL_RelayHandle handle;
+  auto port = relayHandles->Allocate(channel, &handle, status);
+
+  if (*status != 0) {
+    if (port) {
+      hal::SetLastErrorPreviouslyAllocated(status, "Relay", channel,
+                                           port->previousAllocation);
+    } else {
+      hal::SetLastErrorIndexOutOfRange(status, "Invalid Index for Relay", 0,
+                                       kNumRelayChannels, channel);
+    }
+    return HAL_kInvalidHandle;  // failed to allocate. Pass error back.
+  }
+
+  if (!fwd) {
     // Subtract number of headers to put channel in range
     channel -= kNumRelayHeaders;
 
@@ -73,6 +82,7 @@
   }
 
   port->channel = static_cast<uint8_t>(channel);
+  port->previousAllocation = allocationLocation ? allocationLocation : "";
 
   return handle;
 }
@@ -80,11 +90,14 @@
 void HAL_FreeRelayPort(HAL_RelayHandle relayPortHandle) {
   auto port = relayHandles->Get(relayPortHandle);
   relayHandles->Free(relayPortHandle);
-  if (port == nullptr) return;
-  if (port->fwd)
+  if (port == nullptr) {
+    return;
+  }
+  if (port->fwd) {
     SimRelayData[port->channel].initializedForward = false;
-  else
+  } else {
     SimRelayData[port->channel].initializedReverse = false;
+  }
 }
 
 HAL_Bool HAL_CheckRelayChannel(int32_t channel) {
@@ -101,10 +114,11 @@
     *status = HAL_HANDLE_ERROR;
     return;
   }
-  if (port->fwd)
+  if (port->fwd) {
     SimRelayData[port->channel].forward = on;
-  else
+  } else {
     SimRelayData[port->channel].reverse = on;
+  }
 }
 
 HAL_Bool HAL_GetRelay(HAL_RelayHandle relayPortHandle, int32_t* status) {
@@ -113,9 +127,10 @@
     *status = HAL_HANDLE_ERROR;
     return false;
   }
-  if (port->fwd)
+  if (port->fwd) {
     return SimRelayData[port->channel].forward;
-  else
+  } else {
     return SimRelayData[port->channel].reverse;
+  }
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/SPI.cpp b/third_party/allwpilib/hal/src/main/native/sim/SPI.cpp
index 1c90a98..bf362a8 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/SPI.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/SPI.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/SPI.h"
 
@@ -12,11 +9,9 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSPI() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -35,13 +30,17 @@
 int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) {
   return SimSPIData[port].Read(buffer, count);
 }
-void HAL_CloseSPI(HAL_SPIPort port) { SimSPIData[port].initialized = false; }
+void HAL_CloseSPI(HAL_SPIPort port) {
+  SimSPIData[port].initialized = false;
+}
 void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {}
 void HAL_SetSPIOpts(HAL_SPIPort port, HAL_Bool msbFirst,
                     HAL_Bool sampleOnTrailing, HAL_Bool clkIdleHigh) {}
 void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {}
 void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {}
-int32_t HAL_GetSPIHandle(HAL_SPIPort port) { return 0; }
+int32_t HAL_GetSPIHandle(HAL_SPIPort port) {
+  return 0;
+}
 void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {}
 
 void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {}
diff --git a/third_party/allwpilib/hal/src/main/native/sim/SerialPort.cpp b/third_party/allwpilib/hal/src/main/native/sim/SerialPort.cpp
index 2df2ebe..83ba971 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/SerialPort.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/SerialPort.cpp
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/SerialPort.h"
 
 #include "HALInitializer.h"
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSerialPort() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 HAL_SerialPortHandle HAL_InitializeSerialPort(HAL_SerialPort port,
@@ -29,7 +24,9 @@
   return HAL_kInvalidHandle;
 }
 
-int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status) { return -1; }
+int HAL_GetSerialFD(HAL_SerialPortHandle handle, int32_t* status) {
+  return -1;
+}
 
 void HAL_SetSerialBaudRate(HAL_SerialPortHandle handle, int32_t baud,
                            int32_t* status) {}
diff --git a/third_party/allwpilib/hal/src/main/native/sim/SimDevice.cpp b/third_party/allwpilib/hal/src/main/native/sim/SimDevice.cpp
index a8f8f80..b6c637c 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/SimDevice.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/SimDevice.cpp
@@ -1,25 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/SimDevice.h"
 
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 #include "HALInitializer.h"
 #include "mockdata/SimDeviceDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSimDevice() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 extern "C" {
 
@@ -33,19 +27,29 @@
 }
 
 HAL_SimValueHandle HAL_CreateSimValue(HAL_SimDeviceHandle device,
-                                      const char* name, HAL_Bool readonly,
+                                      const char* name, int32_t direction,
                                       const struct HAL_Value* initialValue) {
-  return SimSimDeviceData->CreateValue(device, name, readonly, 0, nullptr,
-                                       *initialValue);
+  return SimSimDeviceData->CreateValue(device, name, direction, 0, nullptr,
+                                       nullptr, *initialValue);
 }
 
 HAL_SimValueHandle HAL_CreateSimValueEnum(HAL_SimDeviceHandle device,
-                                          const char* name, HAL_Bool readonly,
+                                          const char* name, int32_t direction,
                                           int32_t numOptions,
                                           const char** options,
                                           int32_t initialValue) {
-  return SimSimDeviceData->CreateValue(device, name, readonly, numOptions,
-                                       options, HAL_MakeEnum(initialValue));
+  return SimSimDeviceData->CreateValue(device, name, direction, numOptions,
+                                       options, nullptr,
+                                       HAL_MakeEnum(initialValue));
+}
+
+HAL_SimValueHandle HAL_CreateSimValueEnumDouble(
+    HAL_SimDeviceHandle device, const char* name, int32_t direction,
+    int32_t numOptions, const char** options, const double* optionValues,
+    int32_t initialValue) {
+  return SimSimDeviceData->CreateValue(device, name, direction, numOptions,
+                                       options, optionValues,
+                                       HAL_MakeEnum(initialValue));
 }
 
 void HAL_GetSimValue(HAL_SimValueHandle handle, struct HAL_Value* value) {
@@ -56,20 +60,17 @@
   SimSimDeviceData->SetValue(handle, *value);
 }
 
-hal::SimDevice::SimDevice(const char* name, int index) {
-  wpi::SmallString<128> fullname;
-  wpi::raw_svector_ostream os(fullname);
-  os << name << '[' << index << ']';
+void HAL_ResetSimValue(HAL_SimValueHandle handle) {
+  SimSimDeviceData->ResetValue(handle);
+}
 
-  m_handle = HAL_CreateSimDevice(fullname.c_str());
+hal::SimDevice::SimDevice(const char* name, int index) {
+  m_handle = HAL_CreateSimDevice(fmt::format("{}[{}]", name, index).c_str());
 }
 
 hal::SimDevice::SimDevice(const char* name, int index, int channel) {
-  wpi::SmallString<128> fullname;
-  wpi::raw_svector_ostream os(fullname);
-  os << name << '[' << index << ',' << channel << ']';
-
-  m_handle = HAL_CreateSimDevice(fullname.c_str());
+  m_handle = HAL_CreateSimDevice(
+      fmt::format("{}[{},{}]", name, index, channel).c_str());
 }
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Solenoid.cpp b/third_party/allwpilib/hal/src/main/native/sim/Solenoid.cpp
deleted file mode 100644
index 1881a32..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/Solenoid.cpp
+++ /dev/null
@@ -1,145 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "hal/Solenoid.h"
-
-#include "HALInitializer.h"
-#include "PortsInternal.h"
-#include "hal/Errors.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/handles/IndexedHandleResource.h"
-#include "hal/simulation/PCMData.h"
-
-namespace {
-struct Solenoid {
-  uint8_t module;
-  uint8_t channel;
-};
-}  // namespace
-
-using namespace hal;
-
-static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
-                             kNumPCMModules * kNumSolenoidChannels,
-                             HAL_HandleEnum::Solenoid>* solenoidHandles;
-
-namespace hal {
-namespace init {
-void InitializeSolenoid() {
-  static IndexedHandleResource<HAL_SolenoidHandle, Solenoid,
-                               kNumPCMModules * kNumSolenoidChannels,
-                               HAL_HandleEnum::Solenoid>
-      sH;
-  solenoidHandles = &sH;
-}
-}  // namespace init
-}  // namespace hal
-
-extern "C" {
-HAL_SolenoidHandle HAL_InitializeSolenoidPort(HAL_PortHandle portHandle,
-                                              int32_t* status) {
-  hal::init::CheckInit();
-  int16_t channel = getPortHandleChannel(portHandle);
-  int16_t module = getPortHandleModule(portHandle);
-  if (channel == InvalidHandleIndex) {
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-
-  if (!HAL_CheckSolenoidChannel(channel)) {
-    *status = RESOURCE_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
-  }
-
-  if (!HAL_CheckSolenoidModule(module)) {
-    *status = RESOURCE_OUT_OF_RANGE;
-    return HAL_kInvalidHandle;
-  }
-
-  auto handle = solenoidHandles->Allocate(
-      module * kNumSolenoidChannels + channel, status);
-  if (handle == HAL_kInvalidHandle) {  // out of resources
-    *status = NO_AVAILABLE_RESOURCES;
-    return HAL_kInvalidHandle;
-  }
-  auto solenoidPort = solenoidHandles->Get(handle);
-  if (solenoidPort == nullptr) {  // would only occur on thread issues
-    *status = HAL_HANDLE_ERROR;
-    return HAL_kInvalidHandle;
-  }
-  solenoidPort->module = static_cast<uint8_t>(module);
-  solenoidPort->channel = static_cast<uint8_t>(channel);
-
-  HALSIM_SetPCMSolenoidInitialized(module, channel, true);
-
-  return handle;
-}
-void HAL_FreeSolenoidPort(HAL_SolenoidHandle solenoidPortHandle) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) return;
-  solenoidHandles->Free(solenoidPortHandle);
-  HALSIM_SetPCMSolenoidInitialized(port->module, port->channel, false);
-}
-HAL_Bool HAL_CheckSolenoidModule(int32_t module) {
-  return module < kNumPCMModules && module >= 0;
-}
-
-HAL_Bool HAL_CheckSolenoidChannel(int32_t channel) {
-  return channel < kNumSolenoidChannels && channel >= 0;
-}
-HAL_Bool HAL_GetSolenoid(HAL_SolenoidHandle solenoidPortHandle,
-                         int32_t* status) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return false;
-  }
-
-  return HALSIM_GetPCMSolenoidOutput(port->module, port->channel);
-}
-int32_t HAL_GetAllSolenoids(int32_t module, int32_t* status) {
-  int32_t total = 0;
-  for (int i = 0; i < kNumSolenoidChannels; i++) {
-    int32_t channel = HALSIM_GetPCMSolenoidOutput(module, i) ? 1 : 0;
-    total = total + (channel << i);
-  }
-
-  return total;
-}
-void HAL_SetSolenoid(HAL_SolenoidHandle solenoidPortHandle, HAL_Bool value,
-                     int32_t* status) {
-  auto port = solenoidHandles->Get(solenoidPortHandle);
-  if (port == nullptr) {
-    *status = HAL_HANDLE_ERROR;
-    return;
-  }
-
-  HALSIM_SetPCMSolenoidOutput(port->module, port->channel, value);
-}
-
-void HAL_SetAllSolenoids(int32_t module, int32_t state, int32_t* status) {
-  for (int i = 0; i < kNumSolenoidChannels; i++) {
-    int set = state & 1;
-    HALSIM_SetPCMSolenoidOutput(module, i, set);
-    state >>= 1;
-  }
-}
-
-int32_t HAL_GetPCMSolenoidBlackList(int32_t module, int32_t* status) {
-  return 0;
-}
-HAL_Bool HAL_GetPCMSolenoidVoltageStickyFault(int32_t module, int32_t* status) {
-  return 0;
-}
-HAL_Bool HAL_GetPCMSolenoidVoltageFault(int32_t module, int32_t* status) {
-  return 0;
-}
-void HAL_ClearAllPCMStickyFaults(int32_t module, int32_t* status) {}
-void HAL_SetOneShotDuration(HAL_SolenoidHandle solenoidPortHandle,
-                            int32_t durMS, int32_t* status) {}
-void HAL_FireOneShot(HAL_SolenoidHandle solenoidPortHandle, int32_t* status) {}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/Threads.cpp b/third_party/allwpilib/hal/src/main/native/sim/Threads.cpp
index 2aa2c4b..94b6090 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/Threads.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/Threads.cpp
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/Threads.h"
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeThreads() {}
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 int32_t HAL_GetThreadPriority(NativeThreadHandle handle, HAL_Bool* isRealTime,
                               int32_t* status) {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerData.cpp
index 66e129a..8ed4d78 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "AccelerometerDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAccelerometerData() {
   static AccelerometerData sad[1];
   ::hal::SimAccelerometerData = sad;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 AccelerometerData* hal::SimAccelerometerData;
 void AccelerometerData::ResetData() {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
index e2e0a80..24be83e 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AccelerometerDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
index fe4611e..0945ec9 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <algorithm>
 #include <cstring>
@@ -13,14 +10,12 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAddressableLEDData() {
   static AddressableLEDData sad[kNumAddressableLEDs];
   ::hal::SimAddressableLEDData = sad;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 AddressableLEDData* hal::SimAddressableLEDData;
 
@@ -44,7 +39,9 @@
 int32_t AddressableLEDData::GetData(HAL_AddressableLEDData* d) {
   std::scoped_lock lock(m_dataMutex);
   int32_t len = length;
-  if (d) std::memcpy(d, m_data, len * sizeof(d[0]));
+  if (d) {
+    std::memcpy(d, m_data, len * sizeof(d[0]));
+  }
   return len;
 }
 
@@ -53,8 +50,9 @@
 int32_t HALSIM_FindAddressableLEDForChannel(int32_t channel) {
   for (int i = 0; i < kNumAddressableLEDs; ++i) {
     if (SimAddressableLEDData[i].initialized &&
-        SimAddressableLEDData[i].outputPort == channel)
+        SimAddressableLEDData[i].outputPort == channel) {
       return i;
+    }
   }
   return -1;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
index f0b7998..3382eed 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AddressableLEDDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp
index 2508d9b..bb4e278 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "AnalogGyroDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogGyroData() {
   static AnalogGyroData agd[kNumAccumulators];
   ::hal::SimAnalogGyroData = agd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 AnalogGyroData* hal::SimAnalogGyroData;
 void AnalogGyroData::ResetData() {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
index ff12b60..d88c56d 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogGyroDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInData.cpp
index a2a871c..a93cad0 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "AnalogInDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogInData() {
   static AnalogInData sind[kNumAnalogInputs];
   ::hal::SimAnalogInData = sind;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 AnalogInData* hal::SimAnalogInData;
 void AnalogInData::ResetData() {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
index 90d707c..953c8f8 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogInDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutData.cpp
index d4b9116..0ec39e0 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "AnalogOutDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogOutData() {
   static AnalogOutData siod[kNumAnalogOutputs];
   ::hal::SimAnalogOutData = siod;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 AnalogOutData* hal::SimAnalogOutData;
 void AnalogOutData::ResetData() {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
index 779ee96..d4a61b2 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogOutDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
index ba33522..72184da 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "AnalogTriggerDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeAnalogTriggerData() {
   static AnalogTriggerData satd[kNumAnalogTriggers];
   ::hal::SimAnalogTriggerData = satd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 AnalogTriggerData* hal::SimAnalogTriggerData;
 void AnalogTriggerData::ResetData() {
@@ -32,8 +27,9 @@
 int32_t HALSIM_FindAnalogTriggerForChannel(int32_t channel) {
   for (int i = 0; i < kNumAnalogTriggers; ++i) {
     if (SimAnalogTriggerData[i].initialized &&
-        SimAnalogTriggerData[i].inputPort == channel)
+        SimAnalogTriggerData[i].inputPort == channel) {
       return i;
+    }
   }
   return -1;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
index 74fe5f8..16aab79 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/AnalogTriggerDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/CTREPCMData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CTREPCMData.cpp
new file mode 100644
index 0000000..bfc0b61
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CTREPCMData.cpp
@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "../PortsInternal.h"
+#include "CTREPCMDataInternal.h"
+
+using namespace hal;
+
+namespace hal::init {
+void InitializeCTREPCMData() {
+  static CTREPCMData spd[kNumCTREPCMModules];
+  ::hal::SimCTREPCMData = spd;
+}
+}  // namespace hal::init
+
+CTREPCMData* hal::SimCTREPCMData;
+void CTREPCMData::ResetData() {
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    solenoidOutput[i].Reset(false);
+  }
+  initialized.Reset(false);
+  compressorOn.Reset(false);
+  closedLoopEnabled.Reset(true);
+  pressureSwitch.Reset(false);
+  compressorCurrent.Reset(0.0);
+}
+
+extern "C" {
+void HALSIM_ResetCTREPCMData(int32_t index) {
+  SimCTREPCMData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                  \
+  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, CTREPCM##CAPINAME, \
+                               SimCTREPCMData, LOWERNAME)
+
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, CTREPCMSolenoidOutput,
+                                     SimCTREPCMData, solenoidOutput)
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(HAL_Bool, CompressorOn, compressorOn)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch)
+DEFINE_CAPI(double, CompressorCurrent, compressorCurrent)
+
+void HALSIM_GetCTREPCMAllSolenoids(int32_t index, uint8_t* values) {
+  auto& data = SimCTREPCMData[index].solenoidOutput;
+  uint8_t ret = 0;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    ret |= (data[i] << i);
+  }
+  *values = ret;
+}
+
+void HALSIM_SetCTREPCMAllSolenoids(int32_t index, uint8_t values) {
+  auto& data = SimCTREPCMData[index].solenoidOutput;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    data[i] = (values & 0x1) != 0;
+    values >>= 1;
+  }
+}
+
+#define REGISTER(NAME) \
+  SimCTREPCMData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterCTREPCMAllNonSolenoidCallbacks(int32_t index,
+                                                   HAL_NotifyCallback callback,
+                                                   void* param,
+                                                   HAL_Bool initialNotify) {
+  REGISTER(initialized);
+  REGISTER(compressorOn);
+  REGISTER(closedLoopEnabled);
+  REGISTER(pressureSwitch);
+  REGISTER(compressorCurrent);
+}
+
+void HALSIM_RegisterCTREPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                                HAL_NotifyCallback callback,
+                                                void* param,
+                                                HAL_Bool initialNotify) {
+  REGISTER(solenoidOutput[channel]);
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/CTREPCMDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CTREPCMDataInternal.h
new file mode 100644
index 0000000..56a6a09
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CTREPCMDataInternal.h
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "../PortsInternal.h"
+#include "hal/simulation/CTREPCMData.h"
+#include "hal/simulation/SimDataValue.h"
+
+namespace hal {
+class CTREPCMData {
+  HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+  HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidOutput)
+  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorOn)
+  HAL_SIMDATAVALUE_DEFINE_NAME(ClosedLoopEnabled)
+  HAL_SIMDATAVALUE_DEFINE_NAME(PressureSwitch)
+  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorCurrent)
+
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
+  GetSolenoidOutputDefault() {
+    return false;
+  }
+
+ public:
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+      false};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidOutputName,
+               GetSolenoidOutputDefault>
+      solenoidOutput[kNumCTRESolenoidChannels];
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorOnName> compressorOn{
+      false};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetClosedLoopEnabledName>
+      closedLoopEnabled{true};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetPressureSwitchName> pressureSwitch{
+      false};
+  SimDataValue<double, HAL_MakeDouble, GetCompressorCurrentName>
+      compressorCurrent{0.0};
+
+  virtual void ResetData();
+};
+extern CTREPCMData* SimCTREPCMData;
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.cpp
index e37d865..0ace401 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.cpp
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CanDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeCanData() {
   static CanData scd;
   ::hal::SimCanData = &scd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 CanData* hal::SimCanData;
 
@@ -31,7 +26,9 @@
 
 extern "C" {
 
-void HALSIM_ResetCanData(void) { SimCanData->ResetData(); }
+void HALSIM_ResetCanData(void) {
+  SimCanData->ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                             \
   HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, Can##CAPINAME, \
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.h
index df5b290..c2b7282 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/CanDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIOData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIOData.cpp
index a9c61fd..cdca84b 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIOData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIOData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "DIODataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDIOData() {
   static DIOData sdd[kNumDigitalChannels];
   ::hal::SimDIOData = sdd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 DIOData* hal::SimDIOData;
 void DIOData::ResetData() {
@@ -30,7 +25,9 @@
 }
 
 extern "C" {
-void HALSIM_ResetDIOData(int32_t index) { SimDIOData[index].ResetData(); }
+void HALSIM_ResetDIOData(int32_t index) {
+  SimDIOData[index].ResetData();
+}
 
 HAL_SimDeviceHandle HALSIM_GetDIOSimDevice(int32_t index) {
   return SimDIOData[index].simDevice;
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIODataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIODataInternal.h
index c3266d5..9e6828b 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIODataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DIODataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
index 541d7cf..da4236a 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "DigitalPWMDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDigitalPWMData() {
   static DigitalPWMData sdpd[kNumDigitalPWMOutputs];
   ::hal::SimDigitalPWMData = sdpd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 DigitalPWMData* hal::SimDigitalPWMData;
 void DigitalPWMData::ResetData() {
@@ -29,8 +24,10 @@
 extern "C" {
 int32_t HALSIM_FindDigitalPWMForChannel(int32_t channel) {
   for (int i = 0; i < kNumDigitalPWMOutputs; ++i) {
-    if (SimDigitalPWMData[i].initialized && SimDigitalPWMData[i].pin == channel)
+    if (SimDigitalPWMData[i].initialized &&
+        SimDigitalPWMData[i].pin == channel) {
       return i;
+    }
   }
   return -1;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
index f2c7a5e..fe06389 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DigitalPWMDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationData.cpp
index dae1dd4..c704f2b 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationData.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstring>
 
@@ -12,18 +9,18 @@
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDriverStationData() {
   static DriverStationData dsd;
   ::hal::SimDriverStationData = &dsd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 DriverStationData* hal::SimDriverStationData;
 
-DriverStationData::DriverStationData() { ResetData(); }
+DriverStationData::DriverStationData() {
+  ResetData();
+}
 
 void DriverStationData::ResetData() {
   enabled.Reset(false);
@@ -63,7 +60,8 @@
   int32_t DriverStationData::RegisterJoystick##name##Callback(                 \
       int32_t joystickNum, HAL_Joystick##name##Callback callback, void* param, \
       HAL_Bool initialNotify) {                                                \
-    if (joystickNum < 0 || joystickNum >= kNumJoysticks) return 0;             \
+    if (joystickNum < 0 || joystickNum >= kNumJoysticks)                       \
+      return 0;                                                                \
     std::scoped_lock lock(m_joystickDataMutex);                                \
     int32_t uid = m_joystick##name##Callbacks.Register(callback, param);       \
     if (initialNotify) {                                                       \
@@ -82,14 +80,16 @@
                                                                            \
   void DriverStationData::GetJoystick##name(int32_t joystickNum,           \
                                             HAL_Joystick##name* d) {       \
-    if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;           \
+    if (joystickNum < 0 || joystickNum >= kNumJoysticks)                   \
+      return;                                                              \
     std::scoped_lock lock(m_joystickDataMutex);                            \
     *d = m_joystickData[joystickNum].data##data2;                          \
   }                                                                        \
                                                                            \
   void DriverStationData::SetJoystick##name(int32_t joystickNum,           \
                                             const HAL_Joystick##name* d) { \
-    if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;           \
+    if (joystickNum < 0 || joystickNum >= kNumJoysticks)                   \
+      return;                                                              \
     std::scoped_lock lock(m_joystickDataMutex);                            \
     m_joystickData[joystickNum].data##data2 = *d;                          \
     m_joystick##name##Callbacks(joystickNum, d);                           \
@@ -103,14 +103,18 @@
 
 void DriverStationData::GetJoystickDescriptor(
     int32_t joystickNum, HAL_JoystickDescriptor* descriptor) {
-  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   *descriptor = m_joystickData[joystickNum].descriptor;
 }
 
 void DriverStationData::SetJoystickDescriptor(
     int32_t joystickNum, const HAL_JoystickDescriptor* descriptor) {
-  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[joystickNum].descriptor = *descriptor;
   // Always ensure name is null terminated
@@ -121,8 +125,9 @@
 int32_t DriverStationData::RegisterJoystickOutputsCallback(
     int32_t joystickNum, HAL_JoystickOutputsCallback callback, void* param,
     HAL_Bool initialNotify) {
-  if (joystickNum < 0 || joystickNum >= DriverStationData::kNumJoysticks)
+  if (joystickNum < 0 || joystickNum >= DriverStationData::kNumJoysticks) {
     return 0;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   int32_t uid = m_joystickOutputsCallbacks.Register(callback, param);
   if (initialNotify) {
@@ -141,7 +146,9 @@
                                            int64_t* outputs,
                                            int32_t* leftRumble,
                                            int32_t* rightRumble) {
-  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   *leftRumble = m_joystickData[joystickNum].outputs.leftRumble;
   *outputs = m_joystickData[joystickNum].outputs.outputs;
@@ -151,7 +158,9 @@
 void DriverStationData::SetJoystickOutputs(int32_t joystickNum, int64_t outputs,
                                            int32_t leftRumble,
                                            int32_t rightRumble) {
-  if (joystickNum < 0 || joystickNum >= kNumJoysticks) return;
+  if (joystickNum < 0 || joystickNum >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[joystickNum].outputs.leftRumble = leftRumble;
   m_joystickData[joystickNum].outputs.outputs = outputs;
@@ -207,23 +216,32 @@
   m_newDataCallbacks(&empty);
 }
 
-void DriverStationData::NotifyNewData() { HAL_ReleaseDSMutex(); }
+void DriverStationData::NotifyNewData() {
+  HAL_ReleaseDSMutex();
+}
 
 void DriverStationData::SetJoystickButton(int32_t stick, int32_t button,
                                           HAL_Bool state) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
-  if (state)
+  if (state) {
     m_joystickData[stick].buttons.buttons |= 1 << (button - 1);
-  else
+  } else {
     m_joystickData[stick].buttons.buttons &= ~(1 << (button - 1));
+  }
   m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
 }
 
 void DriverStationData::SetJoystickAxis(int32_t stick, int32_t axis,
                                         double value) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
-  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
+  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].axes.axes[axis] = value;
   m_joystickAxesCallbacks(stick, &m_joystickData[stick].axes);
@@ -231,22 +249,30 @@
 
 void DriverStationData::SetJoystickPOV(int32_t stick, int32_t pov,
                                        int32_t value) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
-  if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
+  if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].povs.povs[pov] = value;
   m_joystickPOVsCallbacks(stick, &m_joystickData[stick].povs);
 }
 
 void DriverStationData::SetJoystickButtons(int32_t stick, uint32_t buttons) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].buttons.buttons = buttons;
   m_joystickButtonsCallbacks(stick, &m_joystickData[stick].buttons);
 }
 
 void DriverStationData::SetJoystickAxisCount(int32_t stick, int32_t count) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].axes.count = count;
   m_joystickData[stick].descriptor.axisCount = count;
@@ -255,7 +281,9 @@
 }
 
 void DriverStationData::SetJoystickPOVCount(int32_t stick, int32_t count) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].povs.count = count;
   m_joystickData[stick].descriptor.povCount = count;
@@ -264,7 +292,9 @@
 }
 
 void DriverStationData::SetJoystickButtonCount(int32_t stick, int32_t count) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].buttons.count = count;
   m_joystickData[stick].descriptor.buttonCount = count;
@@ -288,21 +318,27 @@
 }
 
 void DriverStationData::SetJoystickIsXbox(int32_t stick, HAL_Bool isXbox) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].descriptor.isXbox = isXbox;
   m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
 }
 
 void DriverStationData::SetJoystickType(int32_t stick, int32_t type) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].descriptor.type = type;
   m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
 }
 
 void DriverStationData::SetJoystickName(int32_t stick, const char* name) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   std::strncpy(m_joystickData[stick].descriptor.name, name,
                sizeof(m_joystickData[stick].descriptor.name) - 1);
@@ -312,8 +348,12 @@
 
 void DriverStationData::SetJoystickAxisType(int32_t stick, int32_t axis,
                                             int32_t type) {
-  if (stick < 0 || stick >= kNumJoysticks) return;
-  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) return;
+  if (stick < 0 || stick >= kNumJoysticks) {
+    return;
+  }
+  if (axis < 0 || axis >= HAL_kMaxJoystickAxes) {
+    return;
+  }
   std::scoped_lock lock(m_joystickDataMutex);
   m_joystickData[stick].descriptor.axisTypes[axis] = type;
   m_joystickDescriptorCallbacks(stick, &m_joystickData[stick].descriptor);
@@ -354,7 +394,9 @@
 }
 
 extern "C" {
-void HALSIM_ResetDriverStationData(void) { SimDriverStationData->ResetData(); }
+void HALSIM_ResetDriverStationData(void) {
+  SimDriverStationData->ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                                \
   HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, DriverStation##CAPINAME, \
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
index 2a50742..e0f545e 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
index 660522d..de677a5 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "DutyCycleDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeDutyCycleData() {
   static DutyCycleData sed[kNumDutyCycles];
   ::hal::SimDutyCycleData = sed;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 DutyCycleData* hal::SimDutyCycleData;
 
@@ -33,8 +28,9 @@
 int32_t HALSIM_FindDutyCycleForChannel(int32_t channel) {
   for (int i = 0; i < kNumDutyCycles; ++i) {
     if (SimDutyCycleData[i].initialized &&
-        SimDutyCycleData[i].digitalChannel == channel)
+        SimDutyCycleData[i].digitalChannel == channel) {
       return i;
+    }
   }
   return -1;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
index e69f9aa..0eb8aff 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/DutyCycleDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderData.cpp
index 0b17ac3..62ff744 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "EncoderDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeEncoderData() {
   static EncoderData sed[kNumEncoders];
   ::hal::SimEncoderData = sed;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 EncoderData* hal::SimEncoderData;
 void EncoderData::ResetData() {
@@ -38,10 +33,13 @@
 extern "C" {
 int32_t HALSIM_FindEncoderForChannel(int32_t channel) {
   for (int i = 0; i < kNumEncoders; ++i) {
-    if (!SimEncoderData[i].initialized) continue;
+    if (!SimEncoderData[i].initialized) {
+      continue;
+    }
     if (SimEncoderData[i].digitalChannelA == channel ||
-        SimEncoderData[i].digitalChannelB == channel)
+        SimEncoderData[i].digitalChannelB == channel) {
       return i;
+    }
   }
   return -1;
 }
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
index 3848f1d..bd0db11 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/EncoderDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CData.cpp
index 713064e..0ac813d 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "I2CDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeI2CData() {
   static I2CData sid[2];
   ::hal::SimI2CData = sid;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 I2CData* hal::SimI2CData;
 
@@ -37,7 +32,9 @@
 }
 
 extern "C" {
-void HALSIM_ResetI2CData(int32_t index) { SimI2CData[index].ResetData(); }
+void HALSIM_ResetI2CData(int32_t index) {
+  SimI2CData[index].ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
   HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, I2C##CAPINAME, SimI2CData, \
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CDataInternal.h
index 4822222..6dd4739 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/I2CDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PCMData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PCMData.cpp
deleted file mode 100644
index 6193b05..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PCMData.cpp
+++ /dev/null
@@ -1,90 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "../PortsInternal.h"
-#include "PCMDataInternal.h"
-
-using namespace hal;
-
-namespace hal {
-namespace init {
-void InitializePCMData() {
-  static PCMData spd[kNumPCMModules];
-  ::hal::SimPCMData = spd;
-}
-}  // namespace init
-}  // namespace hal
-
-PCMData* hal::SimPCMData;
-void PCMData::ResetData() {
-  for (int i = 0; i < kNumSolenoidChannels; i++) {
-    solenoidInitialized[i].Reset(false);
-    solenoidOutput[i].Reset(false);
-  }
-  compressorInitialized.Reset(false);
-  compressorOn.Reset(false);
-  closedLoopEnabled.Reset(true);
-  pressureSwitch.Reset(false);
-  compressorCurrent.Reset(0.0);
-}
-
-extern "C" {
-void HALSIM_ResetPCMData(int32_t index) { SimPCMData[index].ResetData(); }
-
-#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
-  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PCM##CAPINAME, SimPCMData, \
-                               LOWERNAME)
-
-HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidInitialized,
-                                     SimPCMData, solenoidInitialized)
-HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, PCMSolenoidOutput,
-                                     SimPCMData, solenoidOutput)
-DEFINE_CAPI(HAL_Bool, CompressorInitialized, compressorInitialized)
-DEFINE_CAPI(HAL_Bool, CompressorOn, compressorOn)
-DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled)
-DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch)
-DEFINE_CAPI(double, CompressorCurrent, compressorCurrent)
-
-void HALSIM_GetPCMAllSolenoids(int32_t index, uint8_t* values) {
-  auto& data = SimPCMData[index].solenoidOutput;
-  uint8_t ret = 0;
-  for (int i = 0; i < kNumSolenoidChannels; i++) {
-    ret |= (data[i] << i);
-  }
-  *values = ret;
-}
-
-void HALSIM_SetPCMAllSolenoids(int32_t index, uint8_t values) {
-  auto& data = SimPCMData[index].solenoidOutput;
-  for (int i = 0; i < kNumSolenoidChannels; i++) {
-    data[i] = (values & 0x1) != 0;
-    values >>= 1;
-  }
-}
-
-#define REGISTER(NAME) \
-  SimPCMData[index].NAME.RegisterCallback(callback, param, initialNotify)
-
-void HALSIM_RegisterPCMAllNonSolenoidCallbacks(int32_t index,
-                                               HAL_NotifyCallback callback,
-                                               void* param,
-                                               HAL_Bool initialNotify) {
-  REGISTER(compressorInitialized);
-  REGISTER(compressorOn);
-  REGISTER(closedLoopEnabled);
-  REGISTER(pressureSwitch);
-  REGISTER(compressorCurrent);
-}
-
-void HALSIM_RegisterPCMAllSolenoidCallbacks(int32_t index, int32_t channel,
-                                            HAL_NotifyCallback callback,
-                                            void* param,
-                                            HAL_Bool initialNotify) {
-  REGISTER(solenoidInitialized[channel]);
-  REGISTER(solenoidOutput[channel]);
-}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PCMDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PCMDataInternal.h
deleted file mode 100644
index 0d3752e..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PCMDataInternal.h
+++ /dev/null
@@ -1,54 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "../PortsInternal.h"
-#include "hal/simulation/PCMData.h"
-#include "hal/simulation/SimDataValue.h"
-
-namespace hal {
-class PCMData {
-  HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidInitialized)
-  HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidOutput)
-  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorInitialized)
-  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorOn)
-  HAL_SIMDATAVALUE_DEFINE_NAME(ClosedLoopEnabled)
-  HAL_SIMDATAVALUE_DEFINE_NAME(PressureSwitch)
-  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorCurrent)
-
-  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
-  GetSolenoidInitializedDefault() {
-    return false;
-  }
-  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
-  GetSolenoidOutputDefault() {
-    return false;
-  }
-
- public:
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidInitializedName,
-               GetSolenoidInitializedDefault>
-      solenoidInitialized[kNumSolenoidChannels];
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidOutputName,
-               GetSolenoidOutputDefault>
-      solenoidOutput[kNumSolenoidChannels];
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorInitializedName>
-      compressorInitialized{false};
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorOnName> compressorOn{
-      false};
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetClosedLoopEnabledName>
-      closedLoopEnabled{true};
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetPressureSwitchName> pressureSwitch{
-      false};
-  SimDataValue<double, HAL_MakeDouble, GetCompressorCurrentName>
-      compressorCurrent{0.0};
-
-  virtual void ResetData();
-};
-extern PCMData* SimPCMData;
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PDPData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PDPData.cpp
deleted file mode 100644
index 1c150bb..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PDPData.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "../PortsInternal.h"
-#include "PDPDataInternal.h"
-
-using namespace hal;
-
-namespace hal {
-namespace init {
-void InitializePDPData() {
-  static PDPData spd[kNumPDPModules];
-  ::hal::SimPDPData = spd;
-}
-}  // namespace init
-}  // namespace hal
-
-PDPData* hal::SimPDPData;
-void PDPData::ResetData() {
-  initialized.Reset(false);
-  temperature.Reset(0.0);
-  voltage.Reset(12.0);
-  for (int i = 0; i < kNumPDPChannels; i++) {
-    current[i].Reset(0.0);
-  }
-}
-
-extern "C" {
-void HALSIM_ResetPDPData(int32_t index) { SimPDPData[index].ResetData(); }
-
-#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
-  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PDP##CAPINAME, SimPDPData, \
-                               LOWERNAME)
-
-DEFINE_CAPI(HAL_Bool, Initialized, initialized)
-DEFINE_CAPI(double, Temperature, temperature)
-DEFINE_CAPI(double, Voltage, voltage)
-HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(double, HALSIM, PDPCurrent, SimPDPData,
-                                     current)
-
-void HALSIM_GetPDPAllCurrents(int32_t index, double* currents) {
-  auto& data = SimPDPData[index].current;
-  for (int i = 0; i < kNumPDPChannels; i++) {
-    currents[i] = data[i];
-  }
-}
-
-void HALSIM_SetPDPAllCurrents(int32_t index, const double* currents) {
-  auto& data = SimPDPData[index].current;
-  for (int i = 0; i < kNumPDPChannels; i++) {
-    data[i] = currents[i];
-  }
-}
-
-#define REGISTER(NAME) \
-  SimPDPData[index].NAME.RegisterCallback(callback, param, initialNotify)
-
-void HALSIM_RegisterPDPAllNonCurrentCallbacks(int32_t index, int32_t channel,
-                                              HAL_NotifyCallback callback,
-                                              void* param,
-                                              HAL_Bool initialNotify) {
-  REGISTER(initialized);
-  REGISTER(temperature);
-  REGISTER(voltage);
-}
-}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PDPDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PDPDataInternal.h
deleted file mode 100644
index 3392eaa..0000000
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PDPDataInternal.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "../PortsInternal.h"
-#include "hal/simulation/PDPData.h"
-#include "hal/simulation/SimDataValue.h"
-
-namespace hal {
-class PDPData {
-  HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
-  HAL_SIMDATAVALUE_DEFINE_NAME(Temperature)
-  HAL_SIMDATAVALUE_DEFINE_NAME(Voltage)
-  HAL_SIMDATAVALUE_DEFINE_NAME(Current)
-
-  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr double GetCurrentDefault() {
-    return 0.0;
-  }
-
- public:
-  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
-      false};
-  SimDataValue<double, HAL_MakeDouble, GetTemperatureName> temperature{0.0};
-  SimDataValue<double, HAL_MakeDouble, GetVoltageName> voltage{12.0};
-  SimDataValue<double, HAL_MakeDouble, GetCurrentName, GetCurrentDefault>
-      current[kNumPDPChannels];
-
-  virtual void ResetData();
-};
-extern PDPData* SimPDPData;
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMData.cpp
index 4d2121d..a221d13 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "PWMDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializePWMData() {
   static PWMData spd[kNumPWMChannels];
   ::hal::SimPWMData = spd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 PWMData* hal::SimPWMData;
 void PWMData::ResetData() {
@@ -30,7 +25,9 @@
 }
 
 extern "C" {
-void HALSIM_ResetPWMData(int32_t index) { SimPWMData[index].ResetData(); }
+void HALSIM_ResetPWMData(int32_t index) {
+  SimPWMData[index].ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
   HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PWM##CAPINAME, SimPWMData, \
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMDataInternal.h
index 028e25a..737ced6 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PWMDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp
new file mode 100644
index 0000000..9c5b826
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PowerDistributionData.cpp
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "../PortsInternal.h"
+#include "PowerDistributionDataInternal.h"
+
+using namespace hal;
+
+namespace hal::init {
+void InitializePowerDistributionData() {
+  static PowerDistributionData spd[kNumPDSimModules];
+  ::hal::SimPowerDistributionData = spd;
+}
+}  // namespace hal::init
+
+PowerDistributionData* hal::SimPowerDistributionData;
+void PowerDistributionData::ResetData() {
+  initialized.Reset(false);
+  temperature.Reset(0.0);
+  voltage.Reset(12.0);
+  for (int i = 0; i < kNumPDSimChannels; i++) {
+    current[i].Reset(0.0);
+  }
+}
+
+extern "C" {
+void HALSIM_ResetPowerDistributionData(int32_t index) {
+  SimPowerDistributionData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                            \
+  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, PowerDistribution##CAPINAME, \
+                               SimPowerDistributionData, LOWERNAME)
+
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(double, Temperature, temperature)
+DEFINE_CAPI(double, Voltage, voltage)
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(double, HALSIM, PowerDistributionCurrent,
+                                     SimPowerDistributionData, current)
+
+void HALSIM_GetPowerDistributionAllCurrents(int32_t index, double* currents,
+                                            int length) {
+  auto& data = SimPowerDistributionData[index].current;
+  int toCopy = (std::min)(length, kNumPDSimChannels);
+  for (int i = 0; i < toCopy; i++) {
+    currents[i] = data[i];
+  }
+}
+
+void HALSIM_SetPowerDistributionAllCurrents(int32_t index,
+                                            const double* currents,
+                                            int length) {
+  auto& data = SimPowerDistributionData[index].current;
+  int toCopy = (std::min)(length, kNumPDSimChannels);
+  for (int i = 0; i < toCopy; i++) {
+    data[i] = currents[i];
+  }
+}
+
+#define REGISTER(NAME)                                                   \
+  SimPowerDistributionData[index].NAME.RegisterCallback(callback, param, \
+                                                        initialNotify)
+
+void HALSIM_RegisterPowerDistributionAllNonCurrentCallbacks(
+    int32_t index, int32_t channel, HAL_NotifyCallback callback, void* param,
+    HAL_Bool initialNotify) {
+  REGISTER(initialized);
+  REGISTER(temperature);
+  REGISTER(voltage);
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h
new file mode 100644
index 0000000..4876dbd
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/PowerDistributionDataInternal.h
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "../PortsInternal.h"
+#include "hal/simulation/PowerDistributionData.h"
+#include "hal/simulation/SimDataValue.h"
+
+namespace hal {
+constexpr int32_t kNumPDSimModules = hal::kNumREVPDHModules;
+constexpr int32_t kNumPDSimChannels = hal::kNumREVPDHChannels;
+
+class PowerDistributionData {
+  HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+  HAL_SIMDATAVALUE_DEFINE_NAME(Temperature)
+  HAL_SIMDATAVALUE_DEFINE_NAME(Voltage)
+  HAL_SIMDATAVALUE_DEFINE_NAME(Current)
+
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr double GetCurrentDefault() {
+    return 0.0;
+  }
+
+ public:
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+      false};
+  SimDataValue<double, HAL_MakeDouble, GetTemperatureName> temperature{0.0};
+  SimDataValue<double, HAL_MakeDouble, GetVoltageName> voltage{12.0};
+  SimDataValue<double, HAL_MakeDouble, GetCurrentName, GetCurrentDefault>
+      current[kNumPDSimChannels];
+
+  virtual void ResetData();
+};
+extern PowerDistributionData* SimPowerDistributionData;
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/REVPHData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/REVPHData.cpp
new file mode 100644
index 0000000..6c0ed5a
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/REVPHData.cpp
@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "../PortsInternal.h"
+#include "REVPHDataInternal.h"
+
+using namespace hal;
+
+namespace hal::init {
+void InitializeREVPHData() {
+  static REVPHData spd[kNumREVPHModules];
+  ::hal::SimREVPHData = spd;
+}
+}  // namespace hal::init
+
+REVPHData* hal::SimREVPHData;
+void REVPHData::ResetData() {
+  for (int i = 0; i < kNumREVPHChannels; i++) {
+    solenoidOutput[i].Reset(false);
+  }
+  initialized.Reset(false);
+  compressorOn.Reset(false);
+  closedLoopEnabled.Reset(true);
+  pressureSwitch.Reset(false);
+  compressorCurrent.Reset(0.0);
+}
+
+extern "C" {
+void HALSIM_ResetREVPHData(int32_t index) {
+  SimREVPHData[index].ResetData();
+}
+
+#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                              \
+  HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, REVPH##CAPINAME, SimREVPHData, \
+                               LOWERNAME)
+
+HAL_SIMDATAVALUE_DEFINE_CAPI_CHANNEL(HAL_Bool, HALSIM, REVPHSolenoidOutput,
+                                     SimREVPHData, solenoidOutput)
+DEFINE_CAPI(HAL_Bool, Initialized, initialized)
+DEFINE_CAPI(HAL_Bool, CompressorOn, compressorOn)
+DEFINE_CAPI(HAL_Bool, ClosedLoopEnabled, closedLoopEnabled)
+DEFINE_CAPI(HAL_Bool, PressureSwitch, pressureSwitch)
+DEFINE_CAPI(double, CompressorCurrent, compressorCurrent)
+
+void HALSIM_GetREVPHAllSolenoids(int32_t index, uint8_t* values) {
+  auto& data = SimREVPHData[index].solenoidOutput;
+  uint8_t ret = 0;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    ret |= (data[i] << i);
+  }
+  *values = ret;
+}
+
+void HALSIM_SetREVPHAllSolenoids(int32_t index, uint8_t values) {
+  auto& data = SimREVPHData[index].solenoidOutput;
+  for (int i = 0; i < kNumCTRESolenoidChannels; i++) {
+    data[i] = (values & 0x1) != 0;
+    values >>= 1;
+  }
+}
+
+#define REGISTER(NAME) \
+  SimREVPHData[index].NAME.RegisterCallback(callback, param, initialNotify)
+
+void HALSIM_RegisterREVPHAllNonSolenoidCallbacks(int32_t index,
+                                                 HAL_NotifyCallback callback,
+                                                 void* param,
+                                                 HAL_Bool initialNotify) {
+  REGISTER(initialized);
+  REGISTER(compressorOn);
+  REGISTER(closedLoopEnabled);
+  REGISTER(pressureSwitch);
+  REGISTER(compressorCurrent);
+}
+
+void HALSIM_RegisterREVPHAllSolenoidCallbacks(int32_t index, int32_t channel,
+                                              HAL_NotifyCallback callback,
+                                              void* param,
+                                              HAL_Bool initialNotify) {
+  REGISTER(solenoidOutput[channel]);
+}
+}  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/REVPHDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/REVPHDataInternal.h
new file mode 100644
index 0000000..ebf4964
--- /dev/null
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/REVPHDataInternal.h
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "../PortsInternal.h"
+#include "hal/simulation/REVPHData.h"
+#include "hal/simulation/SimDataValue.h"
+
+namespace hal {
+class REVPHData {
+  HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
+  HAL_SIMDATAVALUE_DEFINE_NAME(SolenoidOutput)
+  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorOn)
+  HAL_SIMDATAVALUE_DEFINE_NAME(ClosedLoopEnabled)
+  HAL_SIMDATAVALUE_DEFINE_NAME(PressureSwitch)
+  HAL_SIMDATAVALUE_DEFINE_NAME(CompressorCurrent)
+
+  static LLVM_ATTRIBUTE_ALWAYS_INLINE constexpr HAL_Bool
+  GetSolenoidOutputDefault() {
+    return false;
+  }
+
+ public:
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
+      false};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetSolenoidOutputName,
+               GetSolenoidOutputDefault>
+      solenoidOutput[kNumREVPHChannels];
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetCompressorOnName> compressorOn{
+      false};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetClosedLoopEnabledName>
+      closedLoopEnabled{true};
+  SimDataValue<HAL_Bool, HAL_MakeBoolean, GetPressureSwitchName> pressureSwitch{
+      false};
+  SimDataValue<double, HAL_MakeDouble, GetCompressorCurrentName>
+      compressorCurrent{0.0};
+
+  virtual void ResetData();
+};
+extern REVPHData* SimREVPHData;
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayData.cpp
index 4623203..ffe3bd5 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "RelayDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeRelayData() {
   static RelayData srd[kNumRelayHeaders];
   ::hal::SimRelayData = srd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 RelayData* hal::SimRelayData;
 void RelayData::ResetData() {
@@ -28,7 +23,9 @@
 }
 
 extern "C" {
-void HALSIM_ResetRelayData(int32_t index) { SimRelayData[index].ResetData(); }
+void HALSIM_ResetRelayData(int32_t index) {
+  SimRelayData[index].ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                              \
   HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, Relay##CAPINAME, SimRelayData, \
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayDataInternal.h
index d62ea2f..b6fec87 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RelayDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp
index 76406f5..6932620 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "RoboRioDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeRoboRioData() {
   static RoboRioData srrd;
   ::hal::SimRoboRioData = &srrd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 RoboRioData* hal::SimRoboRioData;
 void RoboRioData::ResetData() {
@@ -36,10 +31,13 @@
   userFaults6V.Reset(0);
   userFaults5V.Reset(0);
   userFaults3V3.Reset(0);
+  brownoutVoltage.Reset(6.75);
 }
 
 extern "C" {
-void HALSIM_ResetRoboRioData(void) { SimRoboRioData->ResetData(); }
+void HALSIM_ResetRoboRioData(void) {
+  SimRoboRioData->ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
   HAL_SIMDATAVALUE_DEFINE_CAPI_NOINDEX(TYPE, HALSIM, RoboRio##CAPINAME, \
@@ -60,6 +58,7 @@
 DEFINE_CAPI(int32_t, UserFaults6V, userFaults6V)
 DEFINE_CAPI(int32_t, UserFaults5V, userFaults5V)
 DEFINE_CAPI(int32_t, UserFaults3V3, userFaults3V3)
+DEFINE_CAPI(double, BrownoutVoltage, brownoutVoltage)
 
 #define REGISTER(NAME) \
   SimRoboRioData->NAME.RegisterCallback(callback, param, initialNotify)
@@ -81,5 +80,6 @@
   REGISTER(userFaults6V);
   REGISTER(userFaults5V);
   REGISTER(userFaults3V3);
+  REGISTER(brownoutVoltage);
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
index 2fb3456..99e61ea 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/RoboRioDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -27,6 +24,7 @@
   HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults6V)
   HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults5V)
   HAL_SIMDATAVALUE_DEFINE_NAME(UserFaults3V3)
+  HAL_SIMDATAVALUE_DEFINE_NAME(BrownoutVoltage)
 
  public:
   SimDataValue<HAL_Bool, HAL_MakeBoolean, GetFPGAButtonName> fpgaButton{false};
@@ -49,6 +47,8 @@
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults6VName> userFaults6V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults5VName> userFaults5V{0};
   SimDataValue<int32_t, HAL_MakeInt, GetUserFaults3V3Name> userFaults3V3{0};
+  SimDataValue<double, HAL_MakeDouble, GetBrownoutVoltageName> brownoutVoltage{
+      6.75};
 
   virtual void ResetData();
 };
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp
index db7dc1d..6c5f341 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "SPIAccelerometerDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSPIAccelerometerData() {
   static SPIAccelerometerData ssad[5];
   ::hal::SimSPIAccelerometerData = ssad;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 SPIAccelerometerData* hal::SimSPIAccelerometerData;
 void SPIAccelerometerData::ResetData() {
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
index 59c6685..db405f8 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIData.cpp
index 106ab7f..9499b9b 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIData.cpp
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "../PortsInternal.h"
 #include "SPIDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSPIData() {
   static SPIData ssd[5];
   ::hal::SimSPIData = ssd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 SPIData* hal::SimSPIData;
 void SPIData::ResetData() {
@@ -52,7 +47,9 @@
 }
 
 extern "C" {
-void HALSIM_ResetSPIData(int32_t index) { SimSPIData[index].ResetData(); }
+void HALSIM_ResetSPIData(int32_t index) {
+  SimSPIData[index].ResetData();
+}
 
 #define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME)                          \
   HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, SPI##CAPINAME, SimSPIData, \
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIDataInternal.h
index ff0a6c9..eb5a5a7 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SPIDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceData.cpp b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
index 6c0a7f2..9c7fdbc 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceData.cpp
@@ -1,50 +1,55 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "hal/simulation/SimDeviceData.h"  // NOLINT(build/include_order)
 
 #include <algorithm>
 
+#include <wpi/StringExtras.h>
+
 #include "SimDeviceDataInternal.h"
 
 using namespace hal;
 
-namespace hal {
-namespace init {
+namespace hal::init {
 void InitializeSimDeviceData() {
   static SimDeviceData sdd;
   ::hal::SimSimDeviceData = &sdd;
 }
-}  // namespace init
-}  // namespace hal
+}  // namespace hal::init
 
 SimDeviceData* hal::SimSimDeviceData;
 
 SimDeviceData::Device* SimDeviceData::LookupDevice(HAL_SimDeviceHandle handle) {
-  if (handle <= 0) return nullptr;
-  --handle;
-  if (static_cast<uint32_t>(handle) >= m_devices.size() || !m_devices[handle])
+  if (handle <= 0) {
     return nullptr;
+  }
+  --handle;
+  if (static_cast<uint32_t>(handle) >= m_devices.size() || !m_devices[handle]) {
+    return nullptr;
+  }
   return m_devices[handle].get();
 }
 
 SimDeviceData::Value* SimDeviceData::LookupValue(HAL_SimValueHandle handle) {
-  if (handle <= 0) return nullptr;
+  if (handle <= 0) {
+    return nullptr;
+  }
 
   // look up device
   Device* deviceImpl = LookupDevice(handle >> 16);
-  if (!deviceImpl) return nullptr;
+  if (!deviceImpl) {
+    return nullptr;
+  }
 
   // look up value
   handle &= 0xffff;
   --handle;
   if (static_cast<uint32_t>(handle) >= deviceImpl->values.size() ||
-      !deviceImpl->values[handle])
+      !deviceImpl->values[handle]) {
     return nullptr;
+  }
 
   return deviceImpl->values[handle].get();
 }
@@ -68,7 +73,9 @@
 bool SimDeviceData::IsDeviceEnabled(const char* name) {
   std::scoped_lock lock(m_mutex);
   for (const auto& elem : m_prefixEnabled) {
-    if (wpi::StringRef{name}.startswith(elem.first)) return elem.second;
+    if (wpi::starts_with(name, elem.first)) {
+      return elem.second;
+    }
   }
   return true;
 }
@@ -78,18 +85,24 @@
 
   // don't create if disabled
   for (const auto& elem : m_prefixEnabled) {
-    if (wpi::StringRef{name}.startswith(elem.first)) {
-      if (elem.second) break;  // enabled
-      return 0;                // disabled
+    if (wpi::starts_with(name, elem.first)) {
+      if (elem.second) {
+        break;  // enabled
+      }
+      return 0;  // disabled
     }
   }
 
   // check for duplicates and don't overwrite them
-  if (m_deviceMap.count(name) > 0) return 0;
+  if (m_deviceMap.count(name) > 0) {
+    return 0;
+  }
 
   // don't allow more than 4096 devices (limit driven by 12-bit allocation in
   // value changed callback uid)
-  if (m_devices.size() >= 4095) return 0;
+  if (m_devices.size() >= 4095) {
+    return 0;
+  }
 
   // create and save
   auto deviceImpl = std::make_shared<Device>(name);
@@ -108,9 +121,13 @@
   --handle;
 
   // see if it exists
-  if (handle < 0 || static_cast<uint32_t>(handle) >= m_devices.size()) return;
+  if (handle < 0 || static_cast<uint32_t>(handle) >= m_devices.size()) {
+    return;
+  }
   auto deviceImpl = std::move(m_devices[handle]);
-  if (!deviceImpl) return;
+  if (!deviceImpl) {
+    return;
+  }
 
   // remove from map
   m_deviceMap.erase(deviceImpl->name);
@@ -122,27 +139,32 @@
   m_deviceFreed(deviceImpl->name.c_str(), handle + 1);
 }
 
-HAL_SimValueHandle SimDeviceData::CreateValue(HAL_SimDeviceHandle device,
-                                              const char* name, bool readonly,
-                                              int32_t numOptions,
-                                              const char** options,
-                                              const HAL_Value& initialValue) {
+HAL_SimValueHandle SimDeviceData::CreateValue(
+    HAL_SimDeviceHandle device, const char* name, int32_t direction,
+    int32_t numOptions, const char** options, const double* optionValues,
+    const HAL_Value& initialValue) {
   std::scoped_lock lock(m_mutex);
 
   // look up device
   Device* deviceImpl = LookupDevice(device);
-  if (!deviceImpl) return 0;
+  if (!deviceImpl) {
+    return 0;
+  }
 
   // check for duplicates and don't overwrite them
   auto it = deviceImpl->valueMap.find(name);
-  if (it != deviceImpl->valueMap.end()) return 0;
+  if (it != deviceImpl->valueMap.end()) {
+    return 0;
+  }
 
   // don't allow more than 4096 values per device (limit driven by 12-bit
   // allocation in value changed callback uid)
-  if (deviceImpl->values.size() >= 4095) return 0;
+  if (deviceImpl->values.size() >= 4095) {
+    return 0;
+  }
 
   // create and save; encode device into handle
-  auto valueImplPtr = std::make_unique<Value>(name, readonly, initialValue);
+  auto valueImplPtr = std::make_unique<Value>(name, direction, initialValue);
   Value* valueImpl = valueImplPtr.get();
   HAL_SimValueHandle valueHandle =
       (device << 16) |
@@ -159,10 +181,14 @@
           valueImpl->enumOptions.back().c_str());
     }
   }
+  // copy option values (if any provided)
+  if (numOptions > 0 && optionValues) {
+    valueImpl->enumOptionValues.assign(optionValues, optionValues + numOptions);
+  }
   deviceImpl->valueMap[name] = valueImpl;
 
   // notify callbacks
-  deviceImpl->valueCreated(name, valueHandle, readonly, &initialValue);
+  deviceImpl->valueCreated(name, valueHandle, direction, &initialValue);
 
   return valueHandle;
 }
@@ -185,13 +211,56 @@
                              const HAL_Value& value) {
   std::scoped_lock lock(m_mutex);
   Value* valueImpl = LookupValue(handle);
-  if (!valueImpl) return;
+  if (!valueImpl) {
+    return;
+  }
 
   valueImpl->value = value;
 
   // notify callbacks
   valueImpl->changed(valueImpl->name.c_str(), valueImpl->handle,
-                     valueImpl->readonly, &value);
+                     valueImpl->direction, &value);
+}
+
+void SimDeviceData::ResetValue(HAL_SimValueHandle handle) {
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+  if (!valueImpl) {
+    return;
+  }
+
+  // don't notify reset if we aren't going to actually reset anything
+  switch (valueImpl->value.type) {
+    case HAL_INT:
+    case HAL_LONG:
+    case HAL_DOUBLE:
+      break;
+    default:
+      return;
+  }
+
+  // notify reset callbacks (done here so they're called with the old value)
+  valueImpl->reset(valueImpl->name.c_str(), valueImpl->handle,
+                   valueImpl->direction, &valueImpl->value);
+
+  // set user-facing value to 0
+  switch (valueImpl->value.type) {
+    case HAL_INT:
+      valueImpl->value.data.v_int = 0;
+      break;
+    case HAL_LONG:
+      valueImpl->value.data.v_long = 0;
+      break;
+    case HAL_DOUBLE:
+      valueImpl->value.data.v_double = 0;
+      break;
+    default:
+      return;
+  }
+
+  // notify changed callbacks
+  valueImpl->changed(valueImpl->name.c_str(), valueImpl->handle,
+                     valueImpl->direction, &valueImpl->value);
 }
 
 int32_t SimDeviceData::RegisterDeviceCreatedCallback(
@@ -204,15 +273,20 @@
 
   // initial notifications
   if (initialNotify) {
-    for (auto&& device : m_devices)
-      callback(device->name.c_str(), param, device->handle);
+    for (auto&& device : m_devices) {
+      if (wpi::starts_with(device->name, prefix)) {
+        callback(device->name.c_str(), param, device->handle);
+      }
+    }
   }
 
   return index;
 }
 
 void SimDeviceData::CancelDeviceCreatedCallback(int32_t uid) {
-  if (uid <= 0) return;
+  if (uid <= 0) {
+    return;
+  }
   std::scoped_lock lock(m_mutex);
   m_deviceCreated.Cancel(uid);
 }
@@ -224,7 +298,9 @@
 }
 
 void SimDeviceData::CancelDeviceFreedCallback(int32_t uid) {
-  if (uid <= 0) return;
+  if (uid <= 0) {
+    return;
+  }
   std::scoped_lock lock(m_mutex);
   m_deviceFreed.Cancel(uid);
 }
@@ -232,11 +308,14 @@
 HAL_SimDeviceHandle SimDeviceData::GetDeviceHandle(const char* name) {
   std::scoped_lock lock(m_mutex);
   auto it = m_deviceMap.find(name);
-  if (it == m_deviceMap.end()) return 0;
-  if (auto deviceImpl = it->getValue().lock())
-    return deviceImpl->handle;
-  else
+  if (it == m_deviceMap.end()) {
     return 0;
+  }
+  if (auto deviceImpl = it->getValue().lock()) {
+    return deviceImpl->handle;
+  } else {
+    return 0;
+  }
 }
 
 const char* SimDeviceData::GetDeviceName(HAL_SimDeviceHandle handle) {
@@ -244,7 +323,9 @@
 
   // look up device
   Device* deviceImpl = LookupDevice(handle);
-  if (!deviceImpl) return nullptr;
+  if (!deviceImpl) {
+    return nullptr;
+  }
 
   return deviceImpl->name.c_str();
 }
@@ -253,8 +334,9 @@
                                      HALSIM_SimDeviceCallback callback) {
   std::scoped_lock lock(m_mutex);
   for (auto&& device : m_devices) {
-    if (wpi::StringRef{device->name}.startswith(prefix))
+    if (wpi::starts_with(device->name, prefix)) {
       callback(device->name.c_str(), param, device->handle);
+    }
   }
 }
 
@@ -263,16 +345,19 @@
     bool initialNotify) {
   std::scoped_lock lock(m_mutex);
   Device* deviceImpl = LookupDevice(device);
-  if (!deviceImpl) return -1;
+  if (!deviceImpl) {
+    return -1;
+  }
 
   // register callback
   int32_t index = deviceImpl->valueCreated.Register(callback, param);
 
   // initial notifications
   if (initialNotify) {
-    for (auto&& value : deviceImpl->values)
-      callback(value->name.c_str(), param, value->handle, value->readonly,
+    for (auto&& value : deviceImpl->values) {
+      callback(value->name.c_str(), param, value->handle, value->direction,
                &value->value);
+    }
   }
 
   // encode device into uid
@@ -280,10 +365,14 @@
 }
 
 void SimDeviceData::CancelValueCreatedCallback(int32_t uid) {
-  if (uid <= 0) return;
+  if (uid <= 0) {
+    return;
+  }
   std::scoped_lock lock(m_mutex);
   Device* deviceImpl = LookupDevice(uid >> 16);
-  if (!deviceImpl) return;
+  if (!deviceImpl) {
+    return;
+  }
   deviceImpl->valueCreated.Cancel(uid & 0xffff);
 }
 
@@ -292,15 +381,18 @@
     bool initialNotify) {
   std::scoped_lock lock(m_mutex);
   Value* valueImpl = LookupValue(handle);
-  if (!valueImpl) return -1;
+  if (!valueImpl) {
+    return -1;
+  }
 
   // register callback
   int32_t index = valueImpl->changed.Register(callback, param);
 
   // initial notification
-  if (initialNotify)
+  if (initialNotify) {
     callback(valueImpl->name.c_str(), param, valueImpl->handle,
-             valueImpl->readonly, &valueImpl->value);
+             valueImpl->direction, &valueImpl->value);
+  }
 
   // encode device and value into uid
   return (((handle >> 16) & 0xfff) << 19) | ((handle & 0xfff) << 7) |
@@ -308,23 +400,62 @@
 }
 
 void SimDeviceData::CancelValueChangedCallback(int32_t uid) {
-  if (uid <= 0) return;
+  if (uid <= 0) {
+    return;
+  }
   std::scoped_lock lock(m_mutex);
   Value* valueImpl = LookupValue(((uid >> 19) << 16) | ((uid >> 7) & 0xfff));
-  if (!valueImpl) return;
+  if (!valueImpl) {
+    return;
+  }
   valueImpl->changed.Cancel(uid & 0x7f);
 }
 
+int32_t SimDeviceData::RegisterValueResetCallback(
+    HAL_SimValueHandle handle, void* param, HALSIM_SimValueCallback callback,
+    bool initialNotify) {
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+  if (!valueImpl) {
+    return -1;
+  }
+
+  // register callback
+  int32_t index = valueImpl->reset.Register(callback, param);
+
+  // encode device and value into uid
+  return (((handle >> 16) & 0xfff) << 19) | ((handle & 0xfff) << 7) |
+         (index & 0x7f);
+}
+
+void SimDeviceData::CancelValueResetCallback(int32_t uid) {
+  if (uid <= 0) {
+    return;
+  }
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(((uid >> 19) << 16) | ((uid >> 7) & 0xfff));
+  if (!valueImpl) {
+    return;
+  }
+  valueImpl->reset.Cancel(uid & 0x7f);
+}
+
 HAL_SimValueHandle SimDeviceData::GetValueHandle(HAL_SimDeviceHandle device,
                                                  const char* name) {
   std::scoped_lock lock(m_mutex);
   Device* deviceImpl = LookupDevice(device);
-  if (!deviceImpl) return 0;
+  if (!deviceImpl) {
+    return 0;
+  }
 
   // lookup value
   auto it = deviceImpl->valueMap.find(name);
-  if (it == deviceImpl->valueMap.end()) return 0;
-  if (!it->getValue()) return 0;
+  if (it == deviceImpl->valueMap.end()) {
+    return 0;
+  }
+  if (!it->getValue()) {
+    return 0;
+  }
   return it->getValue()->handle;
 }
 
@@ -332,11 +463,14 @@
                                     HALSIM_SimValueCallback callback) {
   std::scoped_lock lock(m_mutex);
   Device* deviceImpl = LookupDevice(device);
-  if (!deviceImpl) return;
+  if (!deviceImpl) {
+    return;
+  }
 
-  for (auto&& value : deviceImpl->values)
-    callback(value->name.c_str(), param, value->handle, value->readonly,
+  for (auto&& value : deviceImpl->values) {
+    callback(value->name.c_str(), param, value->handle, value->direction,
              &value->value);
+  }
 }
 
 const char** SimDeviceData::GetValueEnumOptions(HAL_SimValueHandle handle,
@@ -345,7 +479,9 @@
 
   std::scoped_lock lock(m_mutex);
   Value* valueImpl = LookupValue(handle);
-  if (!valueImpl) return nullptr;
+  if (!valueImpl) {
+    return nullptr;
+  }
 
   // get list of options (safe to return as they never change)
   auto& options = valueImpl->cstrEnumOptions;
@@ -353,6 +489,22 @@
   return options.data();
 }
 
+const double* SimDeviceData::GetValueEnumDoubleValues(HAL_SimValueHandle handle,
+                                                      int32_t* numOptions) {
+  *numOptions = 0;
+
+  std::scoped_lock lock(m_mutex);
+  Value* valueImpl = LookupValue(handle);
+  if (!valueImpl) {
+    return nullptr;
+  }
+
+  // get list of option values (safe to return as they never change)
+  auto& optionValues = valueImpl->enumOptionValues;
+  *numOptions = optionValues.size();
+  return optionValues.data();
+}
+
 void SimDeviceData::ResetData() {
   std::scoped_lock lock(m_mutex);
   m_devices.clear();
@@ -383,8 +535,9 @@
   SimSimDeviceData->CancelDeviceCreatedCallback(uid);
 }
 
-int32_t HALSIM_RegisterSimDeviceFreedCallback(
-    const char* prefix, void* param, HALSIM_SimDeviceCallback callback) {
+int32_t HALSIM_RegisterSimDeviceFreedCallback(const char* prefix, void* param,
+                                              HALSIM_SimDeviceCallback callback,
+                                              HAL_Bool initialNotify) {
   return SimSimDeviceData->RegisterDeviceFreedCallback(prefix, param, callback);
 }
 
@@ -401,7 +554,9 @@
 }
 
 HAL_SimDeviceHandle HALSIM_GetSimValueDeviceHandle(HAL_SimValueHandle handle) {
-  if (handle <= 0) return 0;
+  if (handle <= 0) {
+    return 0;
+  }
   return handle >> 16;
 }
 
@@ -434,6 +589,18 @@
   SimSimDeviceData->CancelValueChangedCallback(uid);
 }
 
+int32_t HALSIM_RegisterSimValueResetCallback(HAL_SimValueHandle handle,
+                                             void* param,
+                                             HALSIM_SimValueCallback callback,
+                                             HAL_Bool initialNotify) {
+  return SimSimDeviceData->RegisterValueResetCallback(handle, param, callback,
+                                                      initialNotify);
+}
+
+void HALSIM_CancelSimValueResetCallback(int32_t uid) {
+  SimSimDeviceData->CancelValueResetCallback(uid);
+}
+
 HAL_SimValueHandle HALSIM_GetSimValueHandle(HAL_SimDeviceHandle device,
                                             const char* name) {
   return SimSimDeviceData->GetValueHandle(device, name);
@@ -449,6 +616,13 @@
   return SimSimDeviceData->GetValueEnumOptions(handle, numOptions);
 }
 
-void HALSIM_ResetSimDeviceData(void) { SimSimDeviceData->ResetData(); }
+const double* HALSIM_GetSimValueEnumDoubleValues(HAL_SimValueHandle handle,
+                                                 int32_t* numOptions) {
+  return SimSimDeviceData->GetValueEnumDoubleValues(handle, numOptions);
+}
+
+void HALSIM_ResetSimDeviceData(void) {
+  SimSimDeviceData->ResetData();
+}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
index 63c2288..deff6ed 100644
--- a/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
+++ b/third_party/allwpilib/hal/src/main/native/sim/mockdata/SimDeviceDataInternal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,6 +11,7 @@
 #include <utility>
 #include <vector>
 
+#include <wpi/StringExtras.h>
 #include <wpi/StringMap.h>
 #include <wpi/UidVector.h>
 #include <wpi/spinlock.h>
@@ -36,17 +34,25 @@
 
  public:
   void Cancel(int32_t uid) {
-    if (m_callbacks) m_callbacks->erase(uid - 1);
+    if (m_callbacks) {
+      m_callbacks->erase(uid - 1);
+    }
   }
 
   void Reset() {
-    if (m_callbacks) m_callbacks->clear();
+    if (m_callbacks) {
+      m_callbacks->clear();
+    }
   }
 
   int32_t Register(CallbackFunction callback, void* param) {
     // Must return -1 on a null callback for error handling
-    if (callback == nullptr) return -1;
-    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+    if (callback == nullptr) {
+      return -1;
+    }
+    if (!m_callbacks) {
+      m_callbacks = std::make_unique<CallbackVector>();
+    }
     return m_callbacks->emplace_back(param,
                                      reinterpret_cast<RawFunctor>(callback)) +
            1;
@@ -87,17 +93,25 @@
 
  public:
   void Cancel(int32_t uid) {
-    if (m_callbacks) m_callbacks->erase(uid - 1);
+    if (m_callbacks) {
+      m_callbacks->erase(uid - 1);
+    }
   }
 
   void Reset() {
-    if (m_callbacks) m_callbacks->clear();
+    if (m_callbacks) {
+      m_callbacks->clear();
+    }
   }
 
   int32_t Register(const char* prefix, void* param, CallbackFunction callback) {
     // Must return -1 on a null callback for error handling
-    if (callback == nullptr) return -1;
-    if (!m_callbacks) m_callbacks = std::make_unique<CallbackVector>();
+    if (callback == nullptr) {
+      return -1;
+    }
+    if (!m_callbacks) {
+      m_callbacks = std::make_unique<CallbackVector>();
+    }
     return m_callbacks->emplace_back(prefix, param, callback) + 1;
   }
 
@@ -105,7 +119,7 @@
   void Invoke(const char* name, U&&... u) const {
     if (m_callbacks) {
       for (auto&& cb : *m_callbacks) {
-        if (wpi::StringRef{name}.startswith(cb.prefix)) {
+        if (wpi::starts_with(name, cb.prefix)) {
           cb.callback(name, cb.param, std::forward<U>(u)...);
         }
       }
@@ -126,16 +140,18 @@
 class SimDeviceData {
  private:
   struct Value {
-    Value(const char* name_, bool readonly_, const HAL_Value& value_)
-        : name{name_}, readonly{readonly_}, value{value_} {}
+    Value(const char* name_, int32_t direction_, const HAL_Value& value_)
+        : name{name_}, direction{direction_}, value{value_} {}
 
     HAL_SimValueHandle handle{0};
     std::string name;
-    bool readonly;
+    int32_t direction;
     HAL_Value value;
     std::vector<std::string> enumOptions;
     std::vector<const char*> cstrEnumOptions;
+    std::vector<double> enumOptionValues;
     impl::SimUnnamedCallbackRegistry<HALSIM_SimValueCallback> changed;
+    impl::SimUnnamedCallbackRegistry<HALSIM_SimValueCallback> reset;
   };
 
   struct Device {
@@ -168,11 +184,13 @@
   HAL_SimDeviceHandle CreateDevice(const char* name);
   void FreeDevice(HAL_SimDeviceHandle handle);
   HAL_SimValueHandle CreateValue(HAL_SimDeviceHandle device, const char* name,
-                                 bool readonly, int32_t numOptions,
+                                 int32_t direction, int32_t numOptions,
                                  const char** options,
+                                 const double* optionValues,
                                  const HAL_Value& initialValue);
   HAL_Value GetValue(HAL_SimValueHandle handle);
   void SetValue(HAL_SimValueHandle handle, const HAL_Value& value);
+  void ResetValue(HAL_SimValueHandle handle);
 
   int32_t RegisterDeviceCreatedCallback(const char* prefix, void* param,
                                         HALSIM_SimDeviceCallback callback,
@@ -203,6 +221,12 @@
 
   void CancelValueChangedCallback(int32_t uid);
 
+  int32_t RegisterValueResetCallback(HAL_SimValueHandle handle, void* param,
+                                     HALSIM_SimValueCallback callback,
+                                     bool initialNotify);
+
+  void CancelValueResetCallback(int32_t uid);
+
   HAL_SimValueHandle GetValueHandle(HAL_SimDeviceHandle device,
                                     const char* name);
 
@@ -212,6 +236,9 @@
   const char** GetValueEnumOptions(HAL_SimValueHandle handle,
                                    int32_t* numOptions);
 
+  const double* GetValueEnumDoubleValues(HAL_SimValueHandle handle,
+                                         int32_t* numOptions);
+
   void ResetData();
 };
 extern SimDeviceData* SimSimDeviceData;
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/HALTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/HALTest.cpp
new file mode 100644
index 0000000..62a4f85
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/HALTest.cpp
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+
+namespace hal {
+TEST(HALTest, RuntimeType) {
+  EXPECT_EQ(HAL_RuntimeType::HAL_Runtime_Simulation, HAL_GetRuntimeType());
+}
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/HALTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/HALTests.cpp
deleted file mode 100644
index b387c14..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/HALTests.cpp
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/Solenoid.h"
-
-namespace hal {
-TEST(HALTests, RuntimeType) {
-  EXPECT_EQ(HAL_RuntimeType::HAL_Mock, HAL_GetRuntimeType());
-}
-
-TEST(HALSOLENOID, SolenoidTest) {
-  int32_t status = 0;
-  HAL_InitializeSolenoidPort(0, &status);
-  EXPECT_NE(status, 0);
-}
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/can/CANTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/can/CANTest.cpp
index 427bb54..db9dbf5 100644
--- a/third_party/allwpilib/hal/src/test/native/cpp/can/CANTest.cpp
+++ b/third_party/allwpilib/hal/src/test/native/cpp/can/CANTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gtest/gtest.h"
 #include "hal/CANAPI.h"
@@ -41,7 +38,7 @@
   int32_t handle;
 };
 
-TEST(HALCanTests, CanIdPackingTest) {
+TEST(CANTest, CanIdPacking) {
   int32_t status = 0;
   int32_t deviceId = 12;
   CANTestStore testStore(deviceId, &status);
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/handles/HandleTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/handles/HandleTest.cpp
new file mode 100644
index 0000000..5c98f7f
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/handles/HandleTest.cpp
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/handles/IndexedClassedHandleResource.h"
+
+#define HAL_TestHandle HAL_Handle
+
+namespace {
+class MyTestClass {};
+}  // namespace
+
+namespace hal {
+TEST(HandleTest, ClassedHandle) {
+  hal::IndexedClassedHandleResource<HAL_TestHandle, MyTestClass, 8,
+                                    HAL_HandleEnum::Vendor>
+      testClass;
+  int32_t status = 0;
+  testClass.Allocate(0, std::make_unique<MyTestClass>(), &status);
+  EXPECT_EQ(0, status);
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/handles/HandleTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/handles/HandleTests.cpp
deleted file mode 100644
index e893c78..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/handles/HandleTests.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/handles/IndexedClassedHandleResource.h"
-
-#define HAL_TestHandle HAL_Handle
-
-namespace {
-class MyTestClass {};
-}  // namespace
-
-namespace hal {
-TEST(HandleTests, ClassedHandleTest) {
-  hal::IndexedClassedHandleResource<HAL_TestHandle, MyTestClass, 8,
-                                    HAL_HandleEnum::Vendor>
-      testClass;
-  int32_t status = 0;
-  testClass.Allocate(0, std::make_unique<MyTestClass>(), &status);
-  EXPECT_EQ(0, status);
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/main.cpp b/third_party/allwpilib/hal/src/test/native/cpp/main.cpp
index 33990f0..7981c04 100644
--- a/third_party/allwpilib/hal/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/hal/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gtest/gtest.h"
 #include "hal/HAL.h"
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogInDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogInDataTest.cpp
new file mode 100644
index 0000000..284ebd6
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogInDataTest.cpp
@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/AnalogInput.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/AnalogInData.h"
+
+namespace hal {
+
+std::string gTestAnalogInCallbackName;
+HAL_Value gTestAnalogInCallbackValue;
+
+void TestAnalogInInitializationCallback(const char* name, void* param,
+                                        const struct HAL_Value* value) {
+  gTestAnalogInCallbackName = name;
+  gTestAnalogInCallbackValue = *value;
+}
+
+TEST(AnalogInSimTest, AnalogInInitialization) {
+  const int INDEX_TO_TEST = 1;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterAnalogInInitializedCallback(
+      INDEX_TO_TEST, &TestAnalogInInitializationCallback, &callbackParam,
+      false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+  HAL_PortHandle portHandle;
+  HAL_DigitalHandle analogInHandle;
+
+  // Use out of range index
+  portHandle = 8000;
+  gTestAnalogInCallbackName = "Unset";
+  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, analogInHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_OUT_OF_RANGE, status);
+  EXPECT_STREQ("Unset", gTestAnalogInCallbackName.c_str());
+
+  // Successful setup
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestAnalogInCallbackName = "Unset";
+  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != analogInHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestAnalogInCallbackName.c_str());
+
+  // Double initialize... should fail
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestAnalogInCallbackName = "Unset";
+  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, analogInHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
+  EXPECT_STREQ("Unset", gTestAnalogInCallbackName.c_str());
+
+  // Reset, should allow you to re-register
+  hal::HandleBase::ResetGlobalHandles();
+  HALSIM_ResetAnalogInData(INDEX_TO_TEST);
+  callbackId = HALSIM_RegisterAnalogInInitializedCallback(
+      INDEX_TO_TEST, &TestAnalogInInitializationCallback, &callbackParam,
+      false);
+
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestAnalogInCallbackName = "Unset";
+  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != analogInHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestAnalogInCallbackName.c_str());
+  HALSIM_CancelAnalogInInitializedCallback(INDEX_TO_TEST, callbackId);
+}
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp
deleted file mode 100644
index c102480..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogInDataTests.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/AnalogInput.h"
-#include "hal/HAL.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/AnalogInData.h"
-
-namespace hal {
-
-std::string gTestAnalogInCallbackName;
-HAL_Value gTestAnalogInCallbackValue;
-
-void TestAnalogInInitializationCallback(const char* name, void* param,
-                                        const struct HAL_Value* value) {
-  gTestAnalogInCallbackName = name;
-  gTestAnalogInCallbackValue = *value;
-}
-
-TEST(AnalogInSimTests, TestAnalogInInitialization) {
-  const int INDEX_TO_TEST = 1;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterAnalogInInitializedCallback(
-      INDEX_TO_TEST, &TestAnalogInInitializationCallback, &callbackParam,
-      false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-  HAL_PortHandle portHandle;
-  HAL_DigitalHandle analogInHandle;
-
-  // Use out of range index
-  status = 0;
-  portHandle = 8000;
-  gTestAnalogInCallbackName = "Unset";
-  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, analogInHandle);
-  EXPECT_EQ(PARAMETER_OUT_OF_RANGE, status);
-  EXPECT_STREQ("Unset", gTestAnalogInCallbackName.c_str());
-
-  // Successful setup
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestAnalogInCallbackName = "Unset";
-  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != analogInHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestAnalogInCallbackName.c_str());
-
-  // Double initialize... should fail
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestAnalogInCallbackName = "Unset";
-  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, analogInHandle);
-  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
-  EXPECT_STREQ("Unset", gTestAnalogInCallbackName.c_str());
-
-  // Reset, should allow you to re-register
-  hal::HandleBase::ResetGlobalHandles();
-  HALSIM_ResetAnalogInData(INDEX_TO_TEST);
-  callbackId = HALSIM_RegisterAnalogInInitializedCallback(
-      INDEX_TO_TEST, &TestAnalogInInitializationCallback, &callbackParam,
-      false);
-
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestAnalogInCallbackName = "Unset";
-  analogInHandle = HAL_InitializeAnalogInputPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != analogInHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestAnalogInCallbackName.c_str());
-}
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogOutDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogOutDataTest.cpp
new file mode 100644
index 0000000..15a5fa6
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogOutDataTest.cpp
@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/AnalogOutput.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/AnalogOutData.h"
+
+namespace hal {
+
+std::string gTestAnalogOutCallbackName;
+HAL_Value gTestAnalogOutCallbackValue;
+
+void TestAnalogOutInitializationCallback(const char* name, void* param,
+                                         const struct HAL_Value* value) {
+  gTestAnalogOutCallbackName = name;
+  gTestAnalogOutCallbackValue = *value;
+}
+
+TEST(AnalogOutSimTest, AnalogOutInitialization) {
+  const int INDEX_TO_TEST = 1;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterAnalogOutInitializedCallback(
+      INDEX_TO_TEST, &TestAnalogOutInitializationCallback, &callbackParam,
+      false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+  HAL_PortHandle portHandle;
+  HAL_DigitalHandle analogOutHandle;
+
+  // Use out of range index
+  portHandle = 8000;
+  gTestAnalogOutCallbackName = "Unset";
+  analogOutHandle =
+      HAL_InitializeAnalogOutputPort(portHandle, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, analogOutHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_OUT_OF_RANGE, status);
+  EXPECT_STREQ("Unset", gTestAnalogOutCallbackName.c_str());
+
+  // Successful setup
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestAnalogOutCallbackName = "Unset";
+  analogOutHandle =
+      HAL_InitializeAnalogOutputPort(portHandle, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != analogOutHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestAnalogOutCallbackName.c_str());
+
+  // Double initialize... should fail
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestAnalogOutCallbackName = "Unset";
+  analogOutHandle =
+      HAL_InitializeAnalogOutputPort(portHandle, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, analogOutHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
+  EXPECT_STREQ("Unset", gTestAnalogOutCallbackName.c_str());
+
+  // Reset, should allow you to re-register
+  hal::HandleBase::ResetGlobalHandles();
+  HALSIM_ResetAnalogOutData(INDEX_TO_TEST);
+  callbackId = HALSIM_RegisterAnalogOutInitializedCallback(
+      INDEX_TO_TEST, &TestAnalogOutInitializationCallback, &callbackParam,
+      false);
+
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestAnalogOutCallbackName = "Unset";
+  analogOutHandle =
+      HAL_InitializeAnalogOutputPort(portHandle, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != analogOutHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestAnalogOutCallbackName.c_str());
+  HALSIM_CancelAnalogOutInitializedCallback(INDEX_TO_TEST, callbackId);
+}
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp
deleted file mode 100644
index d6c9f70..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/AnalogOutDataTests.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/AnalogOutput.h"
-#include "hal/HAL.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/AnalogOutData.h"
-
-namespace hal {
-
-std::string gTestAnalogOutCallbackName;
-HAL_Value gTestAnalogOutCallbackValue;
-
-void TestAnalogOutInitializationCallback(const char* name, void* param,
-                                         const struct HAL_Value* value) {
-  gTestAnalogOutCallbackName = name;
-  gTestAnalogOutCallbackValue = *value;
-}
-
-TEST(AnalogOutSimTests, TestAnalogOutInitialization) {
-  const int INDEX_TO_TEST = 1;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterAnalogOutInitializedCallback(
-      INDEX_TO_TEST, &TestAnalogOutInitializationCallback, &callbackParam,
-      false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-  HAL_PortHandle portHandle;
-  HAL_DigitalHandle analogOutHandle;
-
-  // Use out of range index
-  status = 0;
-  portHandle = 8000;
-  gTestAnalogOutCallbackName = "Unset";
-  analogOutHandle = HAL_InitializeAnalogOutputPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, analogOutHandle);
-  EXPECT_EQ(PARAMETER_OUT_OF_RANGE, status);
-  EXPECT_STREQ("Unset", gTestAnalogOutCallbackName.c_str());
-
-  // Successful setup
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestAnalogOutCallbackName = "Unset";
-  analogOutHandle = HAL_InitializeAnalogOutputPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != analogOutHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestAnalogOutCallbackName.c_str());
-
-  // Double initialize... should fail
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestAnalogOutCallbackName = "Unset";
-  analogOutHandle = HAL_InitializeAnalogOutputPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, analogOutHandle);
-  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
-  EXPECT_STREQ("Unset", gTestAnalogOutCallbackName.c_str());
-
-  // Reset, should allow you to re-register
-  hal::HandleBase::ResetGlobalHandles();
-  HALSIM_ResetAnalogOutData(INDEX_TO_TEST);
-  callbackId = HALSIM_RegisterAnalogOutInitializedCallback(
-      INDEX_TO_TEST, &TestAnalogOutInitializationCallback, &callbackParam,
-      false);
-
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestAnalogOutCallbackName = "Unset";
-  analogOutHandle = HAL_InitializeAnalogOutputPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != analogOutHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestAnalogOutCallbackName.c_str());
-}
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DIODataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DIODataTest.cpp
new file mode 100644
index 0000000..e8ab350
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DIODataTest.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/DIO.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/DIOData.h"
+
+namespace hal {
+
+std::string gTestDigitalIoCallbackName;
+HAL_Value gTestDigitalIoCallbackValue;
+
+void TestDigitalIoInitializationCallback(const char* name, void* param,
+                                         const struct HAL_Value* value) {
+  gTestDigitalIoCallbackName = name;
+  gTestDigitalIoCallbackValue = *value;
+}
+
+TEST(DigitalIoSimTest, DigitalIoInitialization) {
+  const int INDEX_TO_TEST = 3;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterDIOInitializedCallback(
+      INDEX_TO_TEST, &TestDigitalIoInitializationCallback, &callbackParam,
+      false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+  HAL_PortHandle portHandle;
+  HAL_DigitalHandle digitalIoHandle;
+
+  // Use out of range index
+  portHandle = 8000;
+  gTestDigitalIoCallbackName = "Unset";
+  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, digitalIoHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_OUT_OF_RANGE, status);
+  EXPECT_STREQ("Unset", gTestDigitalIoCallbackName.c_str());
+
+  // Successful setup
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestDigitalIoCallbackName = "Unset";
+  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != digitalIoHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestDigitalIoCallbackName.c_str());
+
+  // Double initialize... should fail
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestDigitalIoCallbackName = "Unset";
+  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, digitalIoHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
+  EXPECT_STREQ("Unset", gTestDigitalIoCallbackName.c_str());
+
+  // Reset, should allow you to re-register
+  hal::HandleBase::ResetGlobalHandles();
+  HALSIM_ResetDIOData(INDEX_TO_TEST);
+  callbackId = HALSIM_RegisterDIOInitializedCallback(
+      INDEX_TO_TEST, &TestDigitalIoInitializationCallback, &callbackParam,
+      false);
+
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestDigitalIoCallbackName = "Unset";
+  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != digitalIoHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestDigitalIoCallbackName.c_str());
+  HALSIM_CancelDIOInitializedCallback(INDEX_TO_TEST, callbackId);
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DIODataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DIODataTests.cpp
deleted file mode 100644
index 248f841..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DIODataTests.cpp
+++ /dev/null
@@ -1,81 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/DIO.h"
-#include "hal/HAL.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/DIOData.h"
-
-namespace hal {
-
-std::string gTestDigitalIoCallbackName;
-HAL_Value gTestDigitalIoCallbackValue;
-
-void TestDigitalIoInitializationCallback(const char* name, void* param,
-                                         const struct HAL_Value* value) {
-  gTestDigitalIoCallbackName = name;
-  gTestDigitalIoCallbackValue = *value;
-}
-
-TEST(DigitalIoSimTests, TestDigitalIoInitialization) {
-  const int INDEX_TO_TEST = 3;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterDIOInitializedCallback(
-      INDEX_TO_TEST, &TestDigitalIoInitializationCallback, &callbackParam,
-      false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-  HAL_PortHandle portHandle;
-  HAL_DigitalHandle digitalIoHandle;
-
-  // Use out of range index
-  status = 0;
-  portHandle = 8000;
-  gTestDigitalIoCallbackName = "Unset";
-  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, digitalIoHandle);
-  EXPECT_EQ(PARAMETER_OUT_OF_RANGE, status);
-  EXPECT_STREQ("Unset", gTestDigitalIoCallbackName.c_str());
-
-  // Successful setup
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestDigitalIoCallbackName = "Unset";
-  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != digitalIoHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestDigitalIoCallbackName.c_str());
-
-  // Double initialize... should fail
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestDigitalIoCallbackName = "Unset";
-  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, digitalIoHandle);
-  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
-  EXPECT_STREQ("Unset", gTestDigitalIoCallbackName.c_str());
-
-  // Reset, should allow you to re-register
-  hal::HandleBase::ResetGlobalHandles();
-  HALSIM_ResetDIOData(INDEX_TO_TEST);
-  callbackId = HALSIM_RegisterDIOInitializedCallback(
-      INDEX_TO_TEST, &TestDigitalIoInitializationCallback, &callbackParam,
-      false);
-
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestDigitalIoCallbackName = "Unset";
-  digitalIoHandle = HAL_InitializeDIOPort(portHandle, true, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != digitalIoHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestDigitalIoCallbackName.c_str());
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp
new file mode 100644
index 0000000..4925e16
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DriverStationDataTest.cpp
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstring>
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/simulation/DriverStationData.h"
+
+namespace hal {
+
+TEST(DriverStationTest, Joystick) {
+  HAL_JoystickAxes axes;
+  HAL_JoystickPOVs povs;
+  HAL_JoystickButtons buttons;
+
+  // Check default values before anything has been set
+  for (int joystickNum = 0; joystickNum < 6; ++joystickNum) {
+    HAL_GetJoystickAxes(joystickNum, &axes);
+    HAL_GetJoystickPOVs(joystickNum, &povs);
+    HAL_GetJoystickButtons(joystickNum, &buttons);
+
+    EXPECT_EQ(0, axes.count);
+    for (int i = 0; i < HAL_kMaxJoystickAxes; ++i) {
+      EXPECT_EQ(0, axes.axes[i]);
+    }
+
+    EXPECT_EQ(0, povs.count);
+    for (int i = 0; i < HAL_kMaxJoystickPOVs; ++i) {
+      EXPECT_EQ(0, povs.povs[i]);
+    }
+
+    EXPECT_EQ(0, buttons.count);
+    EXPECT_EQ(0u, buttons.buttons);
+  }
+
+  HAL_JoystickAxes set_axes;
+  std::memset(&set_axes, 0, sizeof(HAL_JoystickAxes));
+  HAL_JoystickPOVs set_povs;
+  std::memset(&set_povs, 0, sizeof(HAL_JoystickPOVs));
+  HAL_JoystickButtons set_buttons;
+  std::memset(&set_buttons, 0, sizeof(HAL_JoystickButtons));
+
+  // Set values
+  int joystickUnderTest = 4;
+  set_axes.count = 5;
+  for (int i = 0; i < set_axes.count; ++i) {
+    set_axes.axes[i] = i * 0.125;
+  }
+
+  set_povs.count = 3;
+  for (int i = 0; i < set_povs.count; ++i) {
+    set_povs.povs[i] = i * 15 + 12;
+  }
+
+  set_buttons.count = 8;
+  set_buttons.buttons = 0xDEADBEEF;
+
+  HALSIM_SetJoystickAxes(joystickUnderTest, &set_axes);
+  HALSIM_SetJoystickPOVs(joystickUnderTest, &set_povs);
+  HALSIM_SetJoystickButtons(joystickUnderTest, &set_buttons);
+
+  // Check the set values
+  HAL_GetJoystickAxes(joystickUnderTest, &axes);
+  HAL_GetJoystickPOVs(joystickUnderTest, &povs);
+  HAL_GetJoystickButtons(joystickUnderTest, &buttons);
+
+  EXPECT_EQ(5, axes.count);
+  EXPECT_NEAR(0.000, axes.axes[0], 0.000001);
+  EXPECT_NEAR(0.125, axes.axes[1], 0.000001);
+  EXPECT_NEAR(0.250, axes.axes[2], 0.000001);
+  EXPECT_NEAR(0.375, axes.axes[3], 0.000001);
+  EXPECT_NEAR(0.500, axes.axes[4], 0.000001);
+  EXPECT_NEAR(0, axes.axes[5], 0.000001);  // Should not have been set, still 0
+  EXPECT_NEAR(0, axes.axes[6], 0.000001);  // Should not have been set, still 0
+
+  EXPECT_EQ(3, povs.count);
+  EXPECT_EQ(12, povs.povs[0]);
+  EXPECT_EQ(27, povs.povs[1]);
+  EXPECT_EQ(42, povs.povs[2]);
+  EXPECT_EQ(0, povs.povs[3]);  // Should not have been set, still 0
+  EXPECT_EQ(0, povs.povs[4]);  // Should not have been set, still 0
+  EXPECT_EQ(0, povs.povs[5]);  // Should not have been set, still 0
+  EXPECT_EQ(0, povs.povs[6]);  // Should not have been set, still 0
+
+  EXPECT_EQ(8, buttons.count);
+  EXPECT_EQ(0xDEADBEEFu, buttons.buttons);
+
+  // Reset
+  HALSIM_ResetDriverStationData();
+  for (int joystickNum = 0; joystickNum < 6; ++joystickNum) {
+    HAL_GetJoystickAxes(joystickNum, &axes);
+    HAL_GetJoystickPOVs(joystickNum, &povs);
+    HAL_GetJoystickButtons(joystickNum, &buttons);
+
+    EXPECT_EQ(0, axes.count);
+    for (int i = 0; i < HAL_kMaxJoystickAxes; ++i) {
+      EXPECT_EQ(0, axes.axes[i]);
+    }
+
+    EXPECT_EQ(0, povs.count);
+    for (int i = 0; i < HAL_kMaxJoystickPOVs; ++i) {
+      EXPECT_EQ(0, povs.povs[i]);
+    }
+
+    EXPECT_EQ(0, buttons.count);
+    EXPECT_EQ(0u, buttons.buttons);
+  }
+}
+
+TEST(DriverStationTest, EventInfo) {
+  std::string eventName = "UnitTest";
+  std::string gameData = "Insert game specific info here :D";
+  HAL_MatchInfo info;
+  std::snprintf(info.eventName, sizeof(info.eventName), "%s",
+                eventName.c_str());
+  std::snprintf(reinterpret_cast<char*>(info.gameSpecificMessage),
+                sizeof(info.gameSpecificMessage), "%s", gameData.c_str());
+  info.gameSpecificMessageSize = gameData.size();
+  info.matchNumber = 5;
+  info.matchType = HAL_MatchType::HAL_kMatchType_qualification;
+  info.replayNumber = 42;
+  HALSIM_SetMatchInfo(&info);
+
+  HAL_MatchInfo dataBack;
+  HAL_GetMatchInfo(&dataBack);
+
+  std::string gsm{reinterpret_cast<char*>(dataBack.gameSpecificMessage),
+                  dataBack.gameSpecificMessageSize};
+
+  EXPECT_STREQ(eventName.c_str(), dataBack.eventName);
+  EXPECT_EQ(gameData, gsm);
+  EXPECT_EQ(5, dataBack.matchNumber);
+  EXPECT_EQ(HAL_MatchType::HAL_kMatchType_qualification, dataBack.matchType);
+  EXPECT_EQ(42, dataBack.replayNumber);
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
deleted file mode 100644
index 5cb28c4..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/DriverStationDataTests.cpp
+++ /dev/null
@@ -1,143 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <cstring>
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/Solenoid.h"
-#include "hal/simulation/DriverStationData.h"
-
-namespace hal {
-
-TEST(DriverStationTests, JoystickTests) {
-  HAL_JoystickAxes axes;
-  HAL_JoystickPOVs povs;
-  HAL_JoystickButtons buttons;
-
-  // Check default values before anything has been set
-  for (int joystickNum = 0; joystickNum < 6; ++joystickNum) {
-    HAL_GetJoystickAxes(joystickNum, &axes);
-    HAL_GetJoystickPOVs(joystickNum, &povs);
-    HAL_GetJoystickButtons(joystickNum, &buttons);
-
-    EXPECT_EQ(0, axes.count);
-    for (int i = 0; i < HAL_kMaxJoystickAxes; ++i) {
-      EXPECT_EQ(0, axes.axes[i]);
-    }
-
-    EXPECT_EQ(0, povs.count);
-    for (int i = 0; i < HAL_kMaxJoystickPOVs; ++i) {
-      EXPECT_EQ(0, povs.povs[i]);
-    }
-
-    EXPECT_EQ(0, buttons.count);
-    EXPECT_EQ(0u, buttons.buttons);
-  }
-
-  HAL_JoystickAxes set_axes;
-  std::memset(&set_axes, 0, sizeof(HAL_JoystickAxes));
-  HAL_JoystickPOVs set_povs;
-  std::memset(&set_povs, 0, sizeof(HAL_JoystickPOVs));
-  HAL_JoystickButtons set_buttons;
-  std::memset(&set_buttons, 0, sizeof(HAL_JoystickButtons));
-
-  // Set values
-  int joystickUnderTest = 4;
-  set_axes.count = 5;
-  for (int i = 0; i < set_axes.count; ++i) {
-    set_axes.axes[i] = i * 0.125;
-  }
-
-  set_povs.count = 3;
-  for (int i = 0; i < set_povs.count; ++i) {
-    set_povs.povs[i] = i * 15 + 12;
-  }
-
-  set_buttons.count = 8;
-  set_buttons.buttons = 0xDEADBEEF;
-
-  HALSIM_SetJoystickAxes(joystickUnderTest, &set_axes);
-  HALSIM_SetJoystickPOVs(joystickUnderTest, &set_povs);
-  HALSIM_SetJoystickButtons(joystickUnderTest, &set_buttons);
-
-  // Check the set values
-  HAL_GetJoystickAxes(joystickUnderTest, &axes);
-  HAL_GetJoystickPOVs(joystickUnderTest, &povs);
-  HAL_GetJoystickButtons(joystickUnderTest, &buttons);
-
-  EXPECT_EQ(5, axes.count);
-  EXPECT_NEAR(0.000, axes.axes[0], 0.000001);
-  EXPECT_NEAR(0.125, axes.axes[1], 0.000001);
-  EXPECT_NEAR(0.250, axes.axes[2], 0.000001);
-  EXPECT_NEAR(0.375, axes.axes[3], 0.000001);
-  EXPECT_NEAR(0.500, axes.axes[4], 0.000001);
-  EXPECT_NEAR(0, axes.axes[5], 0.000001);  // Should not have been set, still 0
-  EXPECT_NEAR(0, axes.axes[6], 0.000001);  // Should not have been set, still 0
-
-  EXPECT_EQ(3, povs.count);
-  EXPECT_EQ(12, povs.povs[0]);
-  EXPECT_EQ(27, povs.povs[1]);
-  EXPECT_EQ(42, povs.povs[2]);
-  EXPECT_EQ(0, povs.povs[3]);  // Should not have been set, still 0
-  EXPECT_EQ(0, povs.povs[4]);  // Should not have been set, still 0
-  EXPECT_EQ(0, povs.povs[5]);  // Should not have been set, still 0
-  EXPECT_EQ(0, povs.povs[6]);  // Should not have been set, still 0
-
-  EXPECT_EQ(8, buttons.count);
-  EXPECT_EQ(0xDEADBEEFu, buttons.buttons);
-
-  // Reset
-  HALSIM_ResetDriverStationData();
-  for (int joystickNum = 0; joystickNum < 6; ++joystickNum) {
-    HAL_GetJoystickAxes(joystickNum, &axes);
-    HAL_GetJoystickPOVs(joystickNum, &povs);
-    HAL_GetJoystickButtons(joystickNum, &buttons);
-
-    EXPECT_EQ(0, axes.count);
-    for (int i = 0; i < HAL_kMaxJoystickAxes; ++i) {
-      EXPECT_EQ(0, axes.axes[i]);
-    }
-
-    EXPECT_EQ(0, povs.count);
-    for (int i = 0; i < HAL_kMaxJoystickPOVs; ++i) {
-      EXPECT_EQ(0, povs.povs[i]);
-    }
-
-    EXPECT_EQ(0, buttons.count);
-    EXPECT_EQ(0u, buttons.buttons);
-  }
-}
-
-TEST(DriverStationTests, EventInfoTest) {
-  std::string eventName = "UnitTest";
-  std::string gameData = "Insert game specific info here :D";
-  HAL_MatchInfo info;
-  std::snprintf(info.eventName, sizeof(info.eventName), "%s",
-                eventName.c_str());
-  std::snprintf(reinterpret_cast<char*>(info.gameSpecificMessage),
-                sizeof(info.gameSpecificMessage), "%s", gameData.c_str());
-  info.gameSpecificMessageSize = gameData.size();
-  info.matchNumber = 5;
-  info.matchType = HAL_MatchType::HAL_kMatchType_qualification;
-  info.replayNumber = 42;
-  HALSIM_SetMatchInfo(&info);
-
-  HAL_MatchInfo dataBack;
-  HAL_GetMatchInfo(&dataBack);
-
-  std::string gsm{reinterpret_cast<char*>(dataBack.gameSpecificMessage),
-                  dataBack.gameSpecificMessageSize};
-
-  EXPECT_STREQ(eventName.c_str(), dataBack.eventName);
-  EXPECT_EQ(gameData, gsm);
-  EXPECT_EQ(5, dataBack.matchNumber);
-  EXPECT_EQ(HAL_MatchType::HAL_kMatchType_qualification, dataBack.matchType);
-  EXPECT_EQ(42, dataBack.replayNumber);
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/I2CDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/I2CDataTest.cpp
new file mode 100644
index 0000000..7678a4a
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/I2CDataTest.cpp
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/I2C.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/I2CData.h"
+
+namespace hal {
+
+std::string gTestI2CCallbackName;
+HAL_Value gTestI2CCallbackValue;
+
+void TestI2CInitializationCallback(const char* name, void* param,
+                                   const struct HAL_Value* value) {
+  gTestI2CCallbackName = name;
+  gTestI2CCallbackValue = *value;
+}
+
+TEST(I2CSimTest, I2CInitialization) {
+  const int INDEX_TO_TEST = 1;
+
+  int32_t status = 0;
+  HAL_I2CPort port;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterI2CInitializedCallback(
+      INDEX_TO_TEST, &TestI2CInitializationCallback, &callbackParam, false);
+  ASSERT_TRUE(0 != callbackId);
+
+  port = HAL_I2C_kMXP;
+  gTestI2CCallbackName = "Unset";
+  HAL_InitializeI2C(port, &status);
+  EXPECT_STREQ("Initialized", gTestI2CCallbackName.c_str());
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp
deleted file mode 100644
index b78564b..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/I2CDataTests.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/I2C.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/I2CData.h"
-
-namespace hal {
-
-std::string gTestI2CCallbackName;
-HAL_Value gTestI2CCallbackValue;
-
-void TestI2CInitializationCallback(const char* name, void* param,
-                                   const struct HAL_Value* value) {
-  gTestI2CCallbackName = name;
-  gTestI2CCallbackValue = *value;
-}
-
-TEST(I2CSimTests, TestI2CInitialization) {
-  const int INDEX_TO_TEST = 1;
-
-  int32_t status;
-  HAL_I2CPort port;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterI2CInitializedCallback(
-      INDEX_TO_TEST, &TestI2CInitializationCallback, &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  status = 0;
-  port = HAL_I2C_kMXP;
-  gTestI2CCallbackName = "Unset";
-  HAL_InitializeI2C(port, &status);
-  EXPECT_STREQ("Initialized", gTestI2CCallbackName.c_str());
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PCMDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PCMDataTest.cpp
new file mode 100644
index 0000000..58a94c0
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PCMDataTest.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/CTREPCM.h"
+#include "hal/HAL.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/CTREPCMData.h"
+
+namespace hal {
+
+std::string gTestSolenoidCallbackName;
+HAL_Value gTestSolenoidCallbackValue;
+
+void TestSolenoidInitializationCallback(const char* name, void* param,
+                                        const struct HAL_Value* value) {
+  gTestSolenoidCallbackName = name;
+  gTestSolenoidCallbackValue = *value;
+}
+
+TEST(PCMDataTest, PCMInitialization) {
+  const int MODULE_TO_TEST = 2;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterCTREPCMInitializedCallback(
+      MODULE_TO_TEST, &TestSolenoidInitializationCallback, &callbackParam,
+      false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+  int32_t module;
+  HAL_CTREPCMHandle pcmHandle;
+
+  // Use out of range index
+  module = 8000;
+  gTestSolenoidCallbackName = "Unset";
+  pcmHandle = HAL_InitializeCTREPCM(module, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, pcmHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_OUT_OF_RANGE, status);
+  EXPECT_STREQ("Unset", gTestSolenoidCallbackName.c_str());
+
+  // Successful setup
+  status = 0;
+  module = MODULE_TO_TEST;
+  gTestSolenoidCallbackName = "Unset";
+  pcmHandle = HAL_InitializeCTREPCM(module, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != pcmHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestSolenoidCallbackName.c_str());
+
+  // Double initialize... should fail
+  status = 0;
+  module = MODULE_TO_TEST;
+  gTestSolenoidCallbackName = "Unset";
+  pcmHandle = HAL_InitializeCTREPCM(module, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, pcmHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
+  EXPECT_STREQ("Unset", gTestSolenoidCallbackName.c_str());
+
+  // Reset, should allow you to re-register
+  hal::HandleBase::ResetGlobalHandles();
+  HALSIM_ResetCTREPCMData(MODULE_TO_TEST);
+  callbackId = HALSIM_RegisterCTREPCMInitializedCallback(
+      MODULE_TO_TEST, &TestSolenoidInitializationCallback, &callbackParam,
+      false);
+  ASSERT_TRUE(0 != callbackId);
+
+  status = 0;
+  module = MODULE_TO_TEST;
+  gTestSolenoidCallbackName = "Unset";
+  pcmHandle = HAL_InitializeCTREPCM(module, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != pcmHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestSolenoidCallbackName.c_str());
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp
deleted file mode 100644
index 5fc19aa..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PCMDataTests.cpp
+++ /dev/null
@@ -1,83 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/Solenoid.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/PCMData.h"
-
-namespace hal {
-
-std::string gTestSolenoidCallbackName;
-HAL_Value gTestSolenoidCallbackValue;
-
-void TestSolenoidInitializationCallback(const char* name, void* param,
-                                        const struct HAL_Value* value) {
-  gTestSolenoidCallbackName = name;
-  gTestSolenoidCallbackValue = *value;
-}
-
-TEST(SolenoidSimTests, TestSolenoidInitialization) {
-  const int MODULE_TO_TEST = 2;
-  const int CHANNEL_TO_TEST = 3;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterPCMSolenoidInitializedCallback(
-      MODULE_TO_TEST, CHANNEL_TO_TEST, &TestSolenoidInitializationCallback,
-      &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-  HAL_PortHandle portHandle;
-  HAL_DigitalHandle solenoidHandle;
-
-  // Use out of range index
-  status = 0;
-  portHandle = 8000;
-  gTestSolenoidCallbackName = "Unset";
-  solenoidHandle = HAL_InitializeSolenoidPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, solenoidHandle);
-  EXPECT_EQ(HAL_HANDLE_ERROR, status);
-  EXPECT_STREQ("Unset", gTestSolenoidCallbackName.c_str());
-
-  // Successful setup
-  status = 0;
-  portHandle = HAL_GetPortWithModule(MODULE_TO_TEST, CHANNEL_TO_TEST);
-  gTestSolenoidCallbackName = "Unset";
-  solenoidHandle = HAL_InitializeSolenoidPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != solenoidHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("SolenoidInitialized", gTestSolenoidCallbackName.c_str());
-
-  // Double initialize... should fail
-  status = 0;
-  portHandle = HAL_GetPortWithModule(MODULE_TO_TEST, CHANNEL_TO_TEST);
-  gTestSolenoidCallbackName = "Unset";
-  solenoidHandle = HAL_InitializeSolenoidPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, solenoidHandle);
-  EXPECT_EQ(NO_AVAILABLE_RESOURCES, status);
-  EXPECT_STREQ("Unset", gTestSolenoidCallbackName.c_str());
-
-  // Reset, should allow you to re-register
-  hal::HandleBase::ResetGlobalHandles();
-  HALSIM_ResetPCMData(MODULE_TO_TEST);
-  callbackId = HALSIM_RegisterPCMSolenoidInitializedCallback(
-      MODULE_TO_TEST, CHANNEL_TO_TEST, &TestSolenoidInitializationCallback,
-      &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  status = 0;
-  portHandle = HAL_GetPortWithModule(MODULE_TO_TEST, CHANNEL_TO_TEST);
-  gTestSolenoidCallbackName = "Unset";
-  solenoidHandle = HAL_InitializeSolenoidPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != solenoidHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("SolenoidInitialized", gTestSolenoidCallbackName.c_str());
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PDPDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PDPDataTest.cpp
new file mode 100644
index 0000000..59b3ea4
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PDPDataTest.cpp
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/PowerDistribution.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/PowerDistributionData.h"
+
+namespace hal {
+
+std::string gTestPdpCallbackName;
+HAL_Value gTestPdpCallbackValue;
+
+void TestPdpInitializationCallback(const char* name, void* param,
+                                   const struct HAL_Value* value) {
+  gTestPdpCallbackName = name;
+  gTestPdpCallbackValue = *value;
+}
+
+TEST(PdpSimTest, PdpInitialization) {
+  const int INDEX_TO_TEST = 1;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterPowerDistributionInitializedCallback(
+      INDEX_TO_TEST, &TestPdpInitializationCallback, &callbackParam, false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+
+  // Use out of range index
+  gTestPdpCallbackName = "Unset";
+  HAL_InitializePowerDistribution(
+      INDEX_TO_TEST, HAL_PowerDistributionType_kCTRE, nullptr, &status);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestPdpCallbackName.c_str());
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp
deleted file mode 100644
index f980c7c..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PDPDataTests.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/PDP.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/PDPData.h"
-
-namespace hal {
-
-std::string gTestPdpCallbackName;
-HAL_Value gTestPdpCallbackValue;
-
-void TestPdpInitializationCallback(const char* name, void* param,
-                                   const struct HAL_Value* value) {
-  gTestPdpCallbackName = name;
-  gTestPdpCallbackValue = *value;
-}
-
-TEST(PdpSimTests, TestPdpInitialization) {
-  const int INDEX_TO_TEST = 1;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterPDPInitializedCallback(
-      INDEX_TO_TEST, &TestPdpInitializationCallback, &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-
-  // Use out of range index
-  status = 0;
-  gTestPdpCallbackName = "Unset";
-  HAL_InitializePDP(INDEX_TO_TEST, &status);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestPdpCallbackName.c_str());
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PWMDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PWMDataTest.cpp
new file mode 100644
index 0000000..ab14704
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PWMDataTest.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/PWM.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/PWMData.h"
+
+namespace hal {
+
+std::string gTestPwmCallbackName;
+HAL_Value gTestPwmCallbackValue;
+
+void TestPwmInitializationCallback(const char* name, void* param,
+                                   const struct HAL_Value* value) {
+  gTestPwmCallbackName = name;
+  gTestPwmCallbackValue = *value;
+}
+
+TEST(PWMSimTest, PwmInitialization) {
+  const int INDEX_TO_TEST = 7;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterPWMInitializedCallback(
+      INDEX_TO_TEST, &TestPwmInitializationCallback, &callbackParam, false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+  HAL_PortHandle portHandle;
+  HAL_DigitalHandle pwmHandle;
+
+  // Use out of range index
+  portHandle = 8000;
+  gTestPwmCallbackName = "Unset";
+  pwmHandle = HAL_InitializePWMPort(portHandle, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, pwmHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_OUT_OF_RANGE, status);
+  EXPECT_STREQ("Unset", gTestPwmCallbackName.c_str());
+
+  // Successful setup
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestPwmCallbackName = "Unset";
+  pwmHandle = HAL_InitializePWMPort(portHandle, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != pwmHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestPwmCallbackName.c_str());
+
+  // Double initialize... should fail
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestPwmCallbackName = "Unset";
+  pwmHandle = HAL_InitializePWMPort(portHandle, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, pwmHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
+  EXPECT_STREQ("Unset", gTestPwmCallbackName.c_str());
+
+  // Reset, should allow you to re-register
+  hal::HandleBase::ResetGlobalHandles();
+  HALSIM_ResetPWMData(INDEX_TO_TEST);
+  callbackId = HALSIM_RegisterPWMInitializedCallback(
+      INDEX_TO_TEST, &TestPwmInitializationCallback, &callbackParam, false);
+
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestPwmCallbackName = "Unset";
+  pwmHandle = HAL_InitializePWMPort(portHandle, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != pwmHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("Initialized", gTestPwmCallbackName.c_str());
+  HALSIM_CancelPWMInitializedCallback(INDEX_TO_TEST, callbackId);
+}
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp
deleted file mode 100644
index daca364..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/PWMDataTests.cpp
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/PWM.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/PWMData.h"
-
-namespace hal {
-
-std::string gTestPwmCallbackName;
-HAL_Value gTestPwmCallbackValue;
-
-void TestPwmInitializationCallback(const char* name, void* param,
-                                   const struct HAL_Value* value) {
-  gTestPwmCallbackName = name;
-  gTestPwmCallbackValue = *value;
-}
-
-TEST(PWMSimTests, TestPwmInitialization) {
-  const int INDEX_TO_TEST = 7;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterPWMInitializedCallback(
-      INDEX_TO_TEST, &TestPwmInitializationCallback, &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-  HAL_PortHandle portHandle;
-  HAL_DigitalHandle pwmHandle;
-
-  // Use out of range index
-  status = 0;
-  portHandle = 8000;
-  gTestPwmCallbackName = "Unset";
-  pwmHandle = HAL_InitializePWMPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, pwmHandle);
-  EXPECT_EQ(PARAMETER_OUT_OF_RANGE, status);
-  EXPECT_STREQ("Unset", gTestPwmCallbackName.c_str());
-
-  // Successful setup
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestPwmCallbackName = "Unset";
-  pwmHandle = HAL_InitializePWMPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != pwmHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestPwmCallbackName.c_str());
-
-  // Double initialize... should fail
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestPwmCallbackName = "Unset";
-  pwmHandle = HAL_InitializePWMPort(portHandle, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, pwmHandle);
-  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
-  EXPECT_STREQ("Unset", gTestPwmCallbackName.c_str());
-
-  // Reset, should allow you to re-register
-  hal::HandleBase::ResetGlobalHandles();
-  HALSIM_ResetPWMData(INDEX_TO_TEST);
-  callbackId = HALSIM_RegisterPWMInitializedCallback(
-      INDEX_TO_TEST, &TestPwmInitializationCallback, &callbackParam, false);
-
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestPwmCallbackName = "Unset";
-  pwmHandle = HAL_InitializePWMPort(portHandle, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != pwmHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("Initialized", gTestPwmCallbackName.c_str());
-}
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/RelayDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/RelayDataTest.cpp
new file mode 100644
index 0000000..eef8631
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/RelayDataTest.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/Relay.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/RelayData.h"
+
+namespace hal {
+
+std::string gTestRelayCallbackName;
+HAL_Value gTestRelayCallbackValue;
+
+void TestRelayInitializationCallback(const char* name, void* param,
+                                     const struct HAL_Value* value) {
+  gTestRelayCallbackName = name;
+  gTestRelayCallbackValue = *value;
+}
+
+TEST(RelaySimTest, RelayInitialization) {
+  const int INDEX_TO_TEST = 3;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterRelayInitializedForwardCallback(
+      INDEX_TO_TEST, &TestRelayInitializationCallback, &callbackParam, false);
+  ASSERT_TRUE(0 != callbackId);
+
+  int32_t status = 0;
+  HAL_PortHandle portHandle;
+  HAL_DigitalHandle pdpHandle;
+
+  // Use out of range index
+  portHandle = 8000;
+  gTestRelayCallbackName = "Unset";
+  pdpHandle = HAL_InitializeRelayPort(portHandle, true, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, pdpHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_OUT_OF_RANGE, status);
+  EXPECT_STREQ("Unset", gTestRelayCallbackName.c_str());
+
+  // Successful setup
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestRelayCallbackName = "Unset";
+  pdpHandle = HAL_InitializeRelayPort(portHandle, true, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != pdpHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("InitializedForward", gTestRelayCallbackName.c_str());
+
+  // Double initialize... should fail
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestRelayCallbackName = "Unset";
+  pdpHandle = HAL_InitializeRelayPort(portHandle, true, nullptr, &status);
+  EXPECT_EQ(HAL_kInvalidHandle, pdpHandle);
+  EXPECT_EQ(HAL_USE_LAST_ERROR, status);
+  HAL_GetLastError(&status);
+  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
+  EXPECT_STREQ("Unset", gTestRelayCallbackName.c_str());
+
+  // Reset, should allow you to re-register
+  hal::HandleBase::ResetGlobalHandles();
+  HALSIM_ResetRelayData(INDEX_TO_TEST);
+  callbackId = HALSIM_RegisterRelayInitializedForwardCallback(
+      INDEX_TO_TEST, &TestRelayInitializationCallback, &callbackParam, false);
+
+  status = 0;
+  portHandle = HAL_GetPort(INDEX_TO_TEST);
+  gTestRelayCallbackName = "Unset";
+  pdpHandle = HAL_InitializeRelayPort(portHandle, true, nullptr, &status);
+  EXPECT_TRUE(HAL_kInvalidHandle != pdpHandle);
+  EXPECT_EQ(0, status);
+  EXPECT_STREQ("InitializedForward", gTestRelayCallbackName.c_str());
+  HALSIM_CancelRelayInitializedForwardCallback(INDEX_TO_TEST, callbackId);
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp
deleted file mode 100644
index edc0fb4..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/RelayDataTests.cpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/Relay.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/RelayData.h"
-
-namespace hal {
-
-std::string gTestRelayCallbackName;
-HAL_Value gTestRelayCallbackValue;
-
-void TestRelayInitializationCallback(const char* name, void* param,
-                                     const struct HAL_Value* value) {
-  gTestRelayCallbackName = name;
-  gTestRelayCallbackValue = *value;
-}
-
-TEST(RelaySimTests, TestRelayInitialization) {
-  const int INDEX_TO_TEST = 3;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterRelayInitializedForwardCallback(
-      INDEX_TO_TEST, &TestRelayInitializationCallback, &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  int32_t status;
-  HAL_PortHandle portHandle;
-  HAL_DigitalHandle pdpHandle;
-
-  // Use out of range index
-  status = 0;
-  portHandle = 8000;
-  gTestRelayCallbackName = "Unset";
-  pdpHandle = HAL_InitializeRelayPort(portHandle, true, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, pdpHandle);
-  EXPECT_EQ(PARAMETER_OUT_OF_RANGE, status);
-  EXPECT_STREQ("Unset", gTestRelayCallbackName.c_str());
-
-  // Successful setup
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestRelayCallbackName = "Unset";
-  pdpHandle = HAL_InitializeRelayPort(portHandle, true, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != pdpHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("InitializedForward", gTestRelayCallbackName.c_str());
-
-  // Double initialize... should fail
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestRelayCallbackName = "Unset";
-  pdpHandle = HAL_InitializeRelayPort(portHandle, true, &status);
-  EXPECT_EQ(HAL_kInvalidHandle, pdpHandle);
-  EXPECT_EQ(RESOURCE_IS_ALLOCATED, status);
-  EXPECT_STREQ("Unset", gTestRelayCallbackName.c_str());
-
-  // Reset, should allow you to re-register
-  hal::HandleBase::ResetGlobalHandles();
-  HALSIM_ResetRelayData(INDEX_TO_TEST);
-  callbackId = HALSIM_RegisterRelayInitializedForwardCallback(
-      INDEX_TO_TEST, &TestRelayInitializationCallback, &callbackParam, false);
-
-  status = 0;
-  portHandle = HAL_GetPort(INDEX_TO_TEST);
-  gTestRelayCallbackName = "Unset";
-  pdpHandle = HAL_InitializeRelayPort(portHandle, true, &status);
-  EXPECT_TRUE(HAL_kInvalidHandle != pdpHandle);
-  EXPECT_EQ(0, status);
-  EXPECT_STREQ("InitializedForward", gTestRelayCallbackName.c_str());
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SPIDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SPIDataTest.cpp
new file mode 100644
index 0000000..c427eaf
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SPIDataTest.cpp
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/HAL.h"
+#include "hal/SPI.h"
+#include "hal/handles/HandlesInternal.h"
+#include "hal/simulation/SPIData.h"
+
+namespace hal {
+
+std::string gTestSpiCallbackName;
+HAL_Value gTestSpiCallbackValue;
+
+void TestSpiInitializationCallback(const char* name, void* param,
+                                   const struct HAL_Value* value) {
+  gTestSpiCallbackName = name;
+  gTestSpiCallbackValue = *value;
+}
+
+TEST(SpiSimTest, SpiInitialization) {
+  const int INDEX_TO_TEST = 2;
+
+  int32_t status = 0;
+  HAL_SPIPort port;
+
+  int callbackParam = 0;
+  int callbackId = HALSIM_RegisterSPIInitializedCallback(
+      INDEX_TO_TEST, &TestSpiInitializationCallback, &callbackParam, false);
+  ASSERT_TRUE(0 != callbackId);
+
+  port = HAL_SPI_kOnboardCS2;
+  gTestSpiCallbackName = "Unset";
+  HAL_InitializeSPI(port, &status);
+  EXPECT_STREQ("Initialized", gTestSpiCallbackName.c_str());
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp
deleted file mode 100644
index 423db6d..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SPIDataTests.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-#include "hal/HAL.h"
-#include "hal/SPI.h"
-#include "hal/handles/HandlesInternal.h"
-#include "hal/simulation/SPIData.h"
-
-namespace hal {
-
-std::string gTestSpiCallbackName;
-HAL_Value gTestSpiCallbackValue;
-
-void TestSpiInitializationCallback(const char* name, void* param,
-                                   const struct HAL_Value* value) {
-  gTestSpiCallbackName = name;
-  gTestSpiCallbackValue = *value;
-}
-
-TEST(SpiSimTests, TestSpiInitialization) {
-  const int INDEX_TO_TEST = 2;
-
-  int32_t status;
-  HAL_SPIPort port;
-
-  int callbackParam = 0;
-  int callbackId = HALSIM_RegisterSPIInitializedCallback(
-      INDEX_TO_TEST, &TestSpiInitializationCallback, &callbackParam, false);
-  ASSERT_TRUE(0 != callbackId);
-
-  status = 0;
-  port = HAL_SPI_kOnboardCS2;
-  gTestSpiCallbackName = "Unset";
-  HAL_InitializeSPI(port, &status);
-  EXPECT_STREQ("Initialized", gTestSpiCallbackName.c_str());
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SimDeviceDataTest.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SimDeviceDataTest.cpp
new file mode 100644
index 0000000..1203fd7
--- /dev/null
+++ b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SimDeviceDataTest.cpp
@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+#include "hal/SimDevice.h"
+#include "hal/simulation/SimDeviceData.h"
+
+namespace hal {
+
+TEST(SimDeviceSimTest, Enabled) {
+  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("foo"));
+  HALSIM_SetSimDeviceEnabled("f", false);
+  HALSIM_SetSimDeviceEnabled("foob", true);
+  ASSERT_FALSE(HALSIM_IsSimDeviceEnabled("foo"));
+  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("foobar"));
+  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("bar"));
+
+  ASSERT_EQ(HAL_CreateSimDevice("foo"), 0);
+}
+
+}  // namespace hal
diff --git a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp b/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp
deleted file mode 100644
index 8f65f5c..0000000
--- a/third_party/allwpilib/hal/src/test/native/cpp/mockdata/SimDeviceDataTests.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "gtest/gtest.h"
-#include "hal/SimDevice.h"
-#include "hal/simulation/SimDeviceData.h"
-
-namespace hal {
-
-TEST(SimDeviceSimTests, TestEnabled) {
-  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("foo"));
-  HALSIM_SetSimDeviceEnabled("f", false);
-  HALSIM_SetSimDeviceEnabled("foob", true);
-  ASSERT_FALSE(HALSIM_IsSimDeviceEnabled("foo"));
-  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("foobar"));
-  ASSERT_TRUE(HALSIM_IsSimDeviceEnabled("bar"));
-
-  ASSERT_EQ(HAL_CreateSimDevice("foo"), 0);
-}
-
-}  // namespace hal
diff --git a/third_party/allwpilib/imgui/CMakeLists.txt b/third_party/allwpilib/imgui/CMakeLists.txt
index 82b1592..9f2ac74 100644
--- a/third_party/allwpilib/imgui/CMakeLists.txt
+++ b/third_party/allwpilib/imgui/CMakeLists.txt
@@ -40,6 +40,7 @@
 # Add imgui directly to our build.
 set(SAVE_BUILD_SHARED_LIBS ${BUILD_SHARED_LIBS})
 set(BUILD_SHARED_LIBS OFF)
+set(GLFW_INSTALL OFF)
 add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/glfw-src
                  ${CMAKE_CURRENT_BINARY_DIR}/glfw-build
                  EXCLUDE_FROM_ALL)
@@ -50,24 +51,24 @@
                  EXCLUDE_FROM_ALL)
 
 set(imgui_srcdir ${CMAKE_CURRENT_BINARY_DIR}/imgui-src)
-file(GLOB imgui_sources ${imgui_srcdir}/*.cpp)
+file(GLOB imgui_sources ${imgui_srcdir}/*.cpp ${imgui_srcdir}/misc/cpp/*.cpp)
 set(implot_srcdir ${CMAKE_CURRENT_BINARY_DIR}/implot-src)
 file(GLOB implot_sources ${implot_srcdir}/*.cpp)
-add_library(imgui STATIC ${imgui_sources} ${implot_sources} ${imgui_srcdir}/examples/imgui_impl_glfw.cpp ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp)
+add_library(imgui STATIC ${imgui_sources} ${implot_sources} ${imgui_srcdir}/backends/imgui_impl_glfw.cpp ${imgui_srcdir}/backends/imgui_impl_opengl3.cpp ${CMAKE_CURRENT_BINARY_DIR}/imgui_ProggyDotted.cpp ${CMAKE_CURRENT_BINARY_DIR}/stb_image.cpp)
 target_compile_definitions(imgui PUBLIC IMGUI_IMPL_OPENGL_LOADER_GL3W)
 if (MSVC)
-    target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_directx11.cpp)
+    target_sources(imgui PRIVATE ${imgui_srcdir}/backends/imgui_impl_dx11.cpp)
 else()
     if (APPLE)
         target_compile_options(imgui PRIVATE -fobjc-arc)
         set_target_properties(imgui PROPERTIES LINK_FLAGS "-framework Metal -framework QuartzCore")
-        target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_metal.mm)
+        target_sources(imgui PRIVATE ${imgui_srcdir}/backends/imgui_impl_metal.mm)
     else()
-        #target_sources(imgui PRIVATE ${imgui_srcdir}/examples/imgui_impl_opengl3.cpp)
+        #target_sources(imgui PRIVATE ${imgui_srcdir}/backends/imgui_impl_opengl3.cpp)
     endif()
 endif()
 target_link_libraries(imgui PUBLIC gl3w glfw)
-target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${implot_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/examples>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/stb-src>")
+target_include_directories(imgui PUBLIC "$<BUILD_INTERFACE:${imgui_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/misc/cpp>" "$<BUILD_INTERFACE:${implot_srcdir}>" "$<BUILD_INTERFACE:${imgui_srcdir}/backends>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_BINARY_DIR}/stb-src>")
 
 set_property(TARGET imgui PROPERTY POSITION_INDEPENDENT_CODE ON)
 target_compile_features(imgui PUBLIC cxx_std_17)
diff --git a/third_party/allwpilib/imgui/CMakeLists.txt.in b/third_party/allwpilib/imgui/CMakeLists.txt.in
index 998837d..be649d0 100644
--- a/third_party/allwpilib/imgui/CMakeLists.txt.in
+++ b/third_party/allwpilib/imgui/CMakeLists.txt.in
@@ -5,7 +5,7 @@
 include(ExternalProject)
 ExternalProject_Add(glfw3
   GIT_REPOSITORY    https://github.com/glfw/glfw.git
-  GIT_TAG           63af05c41961c238c2f891e2c923e1b89c1271b6
+  GIT_TAG           3.3.4
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/glfw-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/glfw-build"
   CONFIGURE_COMMAND ""
@@ -15,7 +15,7 @@
 )
 ExternalProject_Add(gl3w
   GIT_REPOSITORY    https://github.com/skaslev/gl3w
-  GIT_TAG           7729692af8a2322cddb636b90393a42c130b1c85
+  GIT_TAG           8418c1b38d195edbd3b20a8f13ec91de6c8c570c
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/gl3w-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/gl3w-build"
   INSTALL_COMMAND   ""
@@ -23,7 +23,7 @@
 )
 ExternalProject_Add(imgui
   GIT_REPOSITORY    https://github.com/ocornut/imgui.git
-  GIT_TAG           v1.76
+  GIT_TAG           v1.82
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/imgui-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/imgui-build"
   CONFIGURE_COMMAND ""
@@ -33,7 +33,7 @@
 )
 ExternalProject_Add(implot
   GIT_REPOSITORY    https://github.com/epezent/implot.git
-  GIT_TAG           90693cca1bd0ca5f0d49bc9cb8187d56b0b8f289
+  GIT_TAG           555ff688a8134bc0c602123149abe9c17d577475
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/implot-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/implot-build"
   CONFIGURE_COMMAND ""
@@ -53,7 +53,7 @@
 )
 ExternalProject_Add(stb
   GIT_REPOSITORY    https://github.com/nothings/stb.git
-  GIT_TAG           f54acd4e13430c5122cab4ca657705c84aa61b08
+  GIT_TAG           c9064e317699d2e495f36ba4f9ac037e88ee371a
   SOURCE_DIR        "${CMAKE_CURRENT_BINARY_DIR}/stb-src"
   BINARY_DIR        "${CMAKE_CURRENT_BINARY_DIR}/stb-build"
   CONFIGURE_COMMAND ""
diff --git a/third_party/allwpilib/msvcruntime/build.gradle b/third_party/allwpilib/msvcruntime/build.gradle
new file mode 100644
index 0000000..80dafa7
--- /dev/null
+++ b/third_party/allwpilib/msvcruntime/build.gradle
@@ -0,0 +1,91 @@
+import org.gradle.nativeplatform.toolchain.internal.msvcpp.VisualStudioLocator
+import org.gradle.internal.os.OperatingSystem
+import org.gradle.util.VersionNumber
+
+plugins {
+    id 'cpp'
+    id 'maven-publish'
+}
+
+if (OperatingSystem.current().isWindows()) {
+    def outputsFolder = file("$buildDir/outputs")
+
+    def baseArtifactId = 'runtime'
+    def artifactGroupId = "edu.wpi.first.msvc"
+    def zipBaseName = "_GROUP_edu_wpi_first_msvc_ID_runtime_CLS"
+
+    def vsLocator = gradle.services.get(VisualStudioLocator)
+
+    def vsLocation = vsLocator.locateAllComponents().first()
+
+    def visualCppVersion = vsLocation.visualCpp.version
+
+    def vsDirectory = vsLocation.visualStudioDir
+
+    def runtimeLocation = file("$vsDirectory\\VC\\Redist\\MSVC")
+
+    def runtimeVerNumber = null
+
+    runtimeLocation.eachFile {
+        def verNumber =  VersionNumber.parse(it.name)
+        if (verNumber.major == visualCppVersion.major && verNumber.minor == visualCppVersion.minor) {
+            runtimeVerNumber = verNumber
+        }
+    }
+
+    if (runtimeVerNumber != null) {
+        runtimeLocation = file("$runtimeLocation\\$runtimeVerNumber")
+
+        def x86Folder = null
+
+        file("$runtimeLocation\\x86").eachFile {
+            if (it.name.endsWith('.CRT')) {
+                x86Folder = it
+            }
+        }
+
+        def x64Folder = null
+
+        file("$runtimeLocation\\x64").eachFile {
+            if (it.name.endsWith('.CRT')) {
+                x64Folder = it
+            }
+        }
+
+        def x86ZipTask = tasks.create('x86RuntimeZip', Zip) {
+            destinationDirectory = outputsFolder
+            archiveBaseName = zipBaseName
+            classifier = 'x86'
+
+            from x86Folder
+        }
+
+        def x64ZipTask = tasks.create('x64RuntimeZip', Zip) {
+            destinationDirectory = outputsFolder
+            archiveBaseName = zipBaseName
+            classifier = 'x64'
+
+            from x64Folder
+        }
+
+        addTaskToCopyAllOutputs(x86ZipTask)
+        addTaskToCopyAllOutputs(x64ZipTask)
+
+        build.dependsOn x86ZipTask
+        build.dependsOn x64ZipTask
+
+        publishing {
+            publications {
+
+                runtime(MavenPublication) {
+                    artifact x86ZipTask
+                    artifact x64ZipTask
+
+                    artifactId = "${baseArtifactId}"
+                    groupId artifactGroupId
+                    version wpilibVersioning.version.get()
+                }
+            }
+        }
+    }
+}
diff --git a/third_party/allwpilib/myRobot/build.gradle b/third_party/allwpilib/myRobot/build.gradle
index 4d6bb26..045f827 100644
--- a/third_party/allwpilib/myRobot/build.gradle
+++ b/third_party/allwpilib/myRobot/build.gradle
@@ -1,5 +1,6 @@
-import jaci.gradle.toolchains.*
-import jaci.gradle.nativedeps.*
+import edu.wpi.first.deployutils.deploy.target.RemoteTarget
+import edu.wpi.first.deployutils.deploy.target.location.SshDeployLocation
+import edu.wpi.first.deployutils.deploy.artifact.*
 import org.gradle.internal.os.OperatingSystem
 
 plugins {
@@ -10,7 +11,7 @@
 }
 
 apply plugin: 'edu.wpi.first.NativeUtils'
-apply plugin: 'jaci.gradle.EmbeddedTools'
+apply plugin: 'edu.wpi.first.DeployUtils'
 
 apply from: '../shared/config.gradle'
 
@@ -30,92 +31,6 @@
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
-deploy {
-    targets {
-        target('roborio') {
-            directory = '/home/admin'
-            maxChannels = 4
-            locations {
-                ssh {
-                    address = "172.22.11.2"
-                    user = 'admin'
-                    password = ''
-                    ipv6 = false
-                }
-            }
-        }
-    }
-    artifacts {
-        all {
-            targets << 'roborio'
-            predeploy << { ctx ->
-                ctx.execute('/usr/local/frc/bin/frcKillRobot.sh -t')
-            }
-            postdeploy << { ctx ->
-                ctx.execute("sync")
-                ctx.execute("ldconfig")
-            }
-        }
-
-        artifact('jre', JREArtifact) {
-            jreDependency = 'edu.wpi.first.jdk:roborio-2020:11.0.4u10-2'
-        }
-
-        javaArtifact('myRobotJava') {
-            jar = 'shadowJar'
-            postdeploy << { ctx ->
-                ctx.execute("echo '/usr/local/frc/JRE/bin/java -XX:+UseConcMarkSweepGC -Djava.library.path=/usr/local/frc/third-party/lib -Djava.lang.invoke.stringConcat=BC_SB -jar /home/admin/myRobot-all.jar' > /home/admin/myRobotJavaRun")
-                ctx.execute("chmod +x /home/admin/myRobotJavaRun; chown lvuser /home/admin/myRobotJavaRun")
-            }
-        }
-
-        nativeArtifact('myRobotCpp') {
-            component = 'myRobotCpp'
-            targetPlatform = nativeUtils.wpi.platforms.roborio
-            libraryDirectory = '/usr/local/frc/third-party/lib'
-            buildType = 'debug'
-            postdeploy << { ctx ->
-                ctx.execute('chmod +x myRobotCpp')
-            }
-
-        }
-
-        nativeArtifact('myRobotCppStatic') {
-            component = 'myRobotCppStatic'
-            targetPlatform = nativeUtils.wpi.platforms.roborio
-            buildType = 'debug'
-
-            postdeploy << { ctx ->
-                ctx.execute('chmod +x myRobotCppStatic')
-            }
-        }
-    }
-}
-
-tasks.register('deployJava') {
-    try {
-        dependsOn tasks.named('deployJreRoborio')
-        dependsOn tasks.named('deployMyRobotJavaRoborio')
-        dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
-    } catch (ignored) {
-    }
-}
-
-tasks.register('deployShared') {
-    try {
-        dependsOn tasks.named('deployMyRobotCppLibrariesRoborio')
-        dependsOn tasks.named('deployMyRobotCppRoborio')
-    } catch (ignored) {
-    }
-}
-
-tasks.register('deployStatic') {
-    try {
-        dependsOn tasks.named('deployMyRobotCppStaticRoborio')
-    } catch (ignored) {
-    }
-}
-
 mainClassName = 'frc.robot.Main'
 
 apply plugin: 'com.github.johnrengelman.shadow'
@@ -136,9 +51,90 @@
     implementation project(':wpilibNewCommands')
 }
 
-def simProjects = [
-    'halsim_gui'
-]
+def simProjects = ['halsim_gui']
+
+deploy {
+    targets {
+        roborio(RemoteTarget) {
+            directory = '/home/admin'
+            maxChannels = 4
+            locations {
+                ssh(SshDeployLocation) {
+                    address = "172.22.11.2"
+                    user = 'admin'
+                    password = ''
+                    ipv6 = false
+                }
+            }
+
+            def remote = it
+
+            artifacts.registerFactory(WPIJREArtifact) {
+                return objects.newInstance(WPIJREArtifact, it, remote)
+            }
+
+            artifacts {
+                all {
+                    predeploy << { ctx ->
+                        ctx.execute('/usr/local/frc/bin/frcKillRobot.sh -t')
+                    }
+                    postdeploy << { ctx ->
+                        ctx.execute("sync")
+                        ctx.execute("ldconfig")
+                    }
+                }
+
+                myRobotCpp(NativeExecutableArtifact) {
+                    libraryDirectory = '/usr/local/frc/third-party/lib'
+                    postdeploy << { ctx ->
+                        ctx.execute('chmod +x myRobotCpp')
+                    }
+                }
+
+                myRobotCppStatic(NativeExecutableArtifact) {
+                    libraryDirectory = '/usr/local/frc/third-party/lib'
+                    postdeploy << { ctx ->
+                        ctx.execute('chmod +x myRobotCppStatic')
+                    }
+                }
+
+                jre(WPIJREArtifact) {
+                }
+
+                myRobotJava(JavaArtifact) {
+                    jarTask = shadowJar
+                    postdeploy << { ctx ->
+                        ctx.execute("echo '/usr/local/frc/JRE/bin/java -XX:+UseConcMarkSweepGC -Djava.library.path=/usr/local/frc/third-party/lib -Djava.lang.invoke.stringConcat=BC_SB -jar /home/admin/myRobot-all.jar' > /home/admin/myRobotJavaRun")
+                        ctx.execute("chmod +x /home/admin/myRobotJavaRun; chown lvuser /home/admin/myRobotJavaRun")
+                    }
+                }
+            }
+        }
+    }
+}
+
+tasks.register('deployJava') {
+    try {
+        dependsOn tasks.named('deployjreroborio')
+        dependsOn tasks.named('deploymyRobotJavaroborio')
+        dependsOn tasks.named('deploymyRobotCpproborio') // Deploying shared C++ is how to get the Java shared libraries.
+    } catch (ignored) {
+    }
+}
+
+tasks.register('deployShared') {
+    try {
+        dependsOn tasks.named('deploymyRobotCpproborio')
+    } catch (ignored) {
+    }
+}
+
+tasks.register('deployStatic') {
+    try {
+        dependsOn tasks.named('deploymyRobotCppStaticroborio')
+    } catch (ignored) {
+    }
+}
 
 model {
     components {
@@ -157,6 +153,11 @@
                 }
             }
             binaries.all { binary ->
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    if (binary.buildType.name == 'debug') {
+                        deploy.targets.roborio.artifacts.myRobotCpp.binary = binary
+                    }
+                }
                 lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                 lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                 lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
@@ -170,7 +171,7 @@
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                 } else {
                     def systemArch = getCurrentArch()
                     if (binary.targetPlatform.name == systemArch) {
@@ -197,6 +198,11 @@
                 }
             }
             binaries.all { binary ->
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    if (binary.buildType.name == 'debug') {
+                        deploy.targets.roborio.artifacts.myRobotCppStatic.binary = binary
+                    }
+                }
                 lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'static'
                 lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'static'
                 lib project: ':wpilibc', library: 'wpilibc', linkage: 'static'
@@ -207,7 +213,7 @@
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'static'
                 if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                 }
             }
         }
diff --git a/third_party/allwpilib/myRobot/src/main/java/frc/robot/Main.java b/third_party/allwpilib/myRobot/src/main/java/frc/robot/Main.java
index 070dd31..428a73d 100644
--- a/third_party/allwpilib/myRobot/src/main/java/frc/robot/Main.java
+++ b/third_party/allwpilib/myRobot/src/main/java/frc/robot/Main.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/myRobot/src/main/java/frc/robot/MyRobot.java b/third_party/allwpilib/myRobot/src/main/java/frc/robot/MyRobot.java
index 3166944..e62c345 100644
--- a/third_party/allwpilib/myRobot/src/main/java/frc/robot/MyRobot.java
+++ b/third_party/allwpilib/myRobot/src/main/java/frc/robot/MyRobot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package frc.robot;
 
@@ -12,45 +9,33 @@
 @SuppressWarnings("all")
 public class MyRobot extends TimedRobot {
   /**
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
    */
   @Override
   public void robotInit() {}
 
-  /**
-   * This function is run once each time the robot enters autonomous mode
-   */
+  /** This function is run once each time the robot enters autonomous mode */
   @Override
   public void autonomousInit() {}
 
-  /**
-   * This function is called periodically during autonomous
-   */
+  /** This function is called periodically during autonomous */
   @Override
   public void autonomousPeriodic() {}
 
-  /**
-   * This function is called once each time the robot enters tele-operated mode
-   */
+  /** This function is called once each time the robot enters tele-operated mode */
   @Override
   public void teleopInit() {}
 
-  /**
-   * This function is called periodically during operator control
-   */
+  /** This function is called periodically during operator control */
   @Override
   public void teleopPeriodic() {}
 
-  /**
-   * This function is called periodically during test mode
-   */
+  /** This function is called periodically during test mode */
   @Override
   public void testPeriodic() {}
 
-  /**
-   * This function is called periodically during all modes
-   */
+  /** This function is called periodically during all modes */
   @Override
   public void robotPeriodic() {}
 }
diff --git a/third_party/allwpilib/myRobot/src/main/native/cpp/MyRobot.cpp b/third_party/allwpilib/myRobot/src/main/native/cpp/MyRobot.cpp
index 79c6e9f..f339b95 100644
--- a/third_party/allwpilib/myRobot/src/main/native/cpp/MyRobot.cpp
+++ b/third_party/allwpilib/myRobot/src/main/native/cpp/MyRobot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/TimedRobot.h>
 
@@ -45,4 +42,6 @@
   void RobotPeriodic() override {}
 };
 
-int main() { return frc::StartRobot<MyRobot>(); }
+int main() {
+  return frc::StartRobot<MyRobot>();
+}
diff --git a/third_party/allwpilib/ntcore/.styleguide b/third_party/allwpilib/ntcore/.styleguide
index 67befdb..8cf3a51 100644
--- a/third_party/allwpilib/ntcore/.styleguide
+++ b/third_party/allwpilib/ntcore/.styleguide
@@ -27,6 +27,7 @@
 }
 
 includeOtherLibs {
+  ^fmt/
   ^support/
   ^wpi/
 }
diff --git a/third_party/allwpilib/ntcore/CMakeLists.txt b/third_party/allwpilib/ntcore/CMakeLists.txt
index e095fa9..44992ef 100644
--- a/third_party/allwpilib/ntcore/CMakeLists.txt
+++ b/third_party/allwpilib/ntcore/CMakeLists.txt
@@ -13,6 +13,7 @@
                 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
                             $<INSTALL_INTERFACE:${include_dest}/ntcore>)
 wpilib_target_warnings(ntcore)
+target_compile_features(ntcore PUBLIC cxx_std_17)
 target_link_libraries(ntcore PUBLIC wpiutil)
 
 set_property(TARGET ntcore PROPERTY FOLDER "libraries")
@@ -26,8 +27,8 @@
     set (ntcore_config_dir share/ntcore)
 endif()
 
-configure_file(ntcore-config.cmake.in ${CMAKE_BINARY_DIR}/ntcore-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/ntcore-config.cmake DESTINATION ${ntcore_config_dir})
+configure_file(ntcore-config.cmake.in ${WPILIB_BINARY_DIR}/ntcore-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/ntcore-config.cmake DESTINATION ${ntcore_config_dir})
 install(EXPORT ntcore DESTINATION ${ntcore_config_dir})
 
 # Java bindings
@@ -35,7 +36,7 @@
     find_package(Java REQUIRED)
     find_package(JNI REQUIRED)
     include(UseJava)
-    set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
     file(GLOB
         ntcore_jni_src src/main/native/cpp/jni/NetworkTablesJNI.cpp)
diff --git a/third_party/allwpilib/ntcore/build.gradle b/third_party/allwpilib/ntcore/build.gradle
index 5739b71..67674c6 100644
--- a/third_party/allwpilib/ntcore/build.gradle
+++ b/third_party/allwpilib/ntcore/build.gradle
@@ -5,29 +5,41 @@
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
+Action<List<String>> symbolFilter = { symbols ->
+    symbols.removeIf({ !it.startsWith('NT_') })
+} as Action<List<String>>;
+
 nativeUtils.exportsConfigs {
     ntcore {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
     ntcoreJNI {
-        x86SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('NT_') })
-        }
-        x64SymbolFilter = { symbols ->
-            symbols.removeIf({ !it.startsWith('NT_') })
-        }
-    }
-}
-
-if (!project.hasProperty('skipPMD')) {
-    pmdMain {
-        pmdMain.enabled = false
+        x86SymbolFilter = symbolFilter
+        x64SymbolFilter = symbolFilter
     }
 }
diff --git a/third_party/allwpilib/ntcore/doc/networktables2.adoc b/third_party/allwpilib/ntcore/doc/networktables2.adoc
index 8471f86..e090801 100644
--- a/third_party/allwpilib/ntcore/doc/networktables2.adoc
+++ b/third_party/allwpilib/ntcore/doc/networktables2.adoc
@@ -1,4 +1,4 @@
-= Network Tables Protocol Specification, Version 2.0
+= NetworkTables Protocol Specification, Version 2.0
 WPILib Developers <wpilib@wpi.edu>
 Protocol Revision 2.0 (0x0200), 1/8/2013
 :toc:
@@ -19,9 +19,9 @@
 a client must have received a server's most recent state before it can replace
 it with a new value.
 
-This is a backwards-incompatible rework of the Network Tables network protocol
+This is a backwards-incompatible rework of the NetworkTables network protocol
 originally introduced for the 2012 FIRST Robotics Competition. Note that this
-revision of the Network Tables protocol no longer includes the concept of
+revision of the NetworkTables protocol no longer includes the concept of
 sub-tables. We suggest that instead of representing sub-tables as first-class
 data types in the network protocol, it would be easy for an implementation to
 provide a similar API abstraction by adding prefixes to keys. For example, we
diff --git a/third_party/allwpilib/ntcore/doc/networktables3.adoc b/third_party/allwpilib/ntcore/doc/networktables3.adoc
index af8b953..2850168 100644
--- a/third_party/allwpilib/ntcore/doc/networktables3.adoc
+++ b/third_party/allwpilib/ntcore/doc/networktables3.adoc
@@ -1,4 +1,4 @@
-= Network Tables Protocol Specification, Version 3.0
+= NetworkTables Protocol Specification, Version 3.0
 WPILib Developers <wpilib@wpi.edu>
 Protocol Revision 3.0 (0x0300), 6/12/2015
 :toc:
@@ -20,7 +20,7 @@
 it with a new value.
 
 This is a backwards-compatible update of <<networktables2,version 2.0>> of the
-Network Tables network protocol. The protocol is designed such that 3.0 clients
+NetworkTables network protocol. The protocol is designed such that 3.0 clients
 and servers can interoperate with 2.0 clients and servers with the only loss of
 functionality being the extended features introduced in 3.0.
 
@@ -50,7 +50,7 @@
 
 Remote procedure call:: The Server may create specially-typed entries that
 inform Clients of remotely callable functions on the Server. Clients can then
-execute these functions via the Network Tables protocol. See <<rpc-operation>>.
+execute these functions via the NetworkTables protocol. See <<rpc-operation>>.
 
 Raw data type:: An arbitrary data type has been added. While string could be
 used to encode raw data, the reason for a different data type is so that
@@ -108,7 +108,7 @@
 == References
 
 [[networktables2]]
-* <<networktables2.adoc#,Network Tables Protocol Specification, Protocol
+* <<networktables2.adoc#,NetworkTables Protocol Specification, Protocol
 Revision 2.0 (0x0200)>>, dated 1/8/2013.
 
 [[leb128,LEB128]]
diff --git a/third_party/allwpilib/ntcore/manualTests/java/Client.java b/third_party/allwpilib/ntcore/manualTests/java/Client.java
index 1d5f181..a329c83 100644
--- a/third_party/allwpilib/ntcore/manualTests/java/Client.java
+++ b/third_party/allwpilib/ntcore/manualTests/java/Client.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 import edu.wpi.first.wpilibj.networktables.*;
 import edu.wpi.first.wpilibj.tables.*;
@@ -21,10 +18,13 @@
     NetworkTable.setPort(10000);
     NetworkTable.setClientMode();
     NetworkTable nt = NetworkTable.getTable("");
-    try { Thread.sleep(2000); } catch (InterruptedException e) {}
+    try {
+      Thread.sleep(2000);
+    } catch (InterruptedException e) {
+    }
     try {
       System.out.println("Got foo: " + nt.getNumber("foo"));
-    } catch(TableKeyNotDefinedException ex) {
+    } catch (TableKeyNotDefinedException ex) {
     }
     nt.putBoolean("bar", false);
     nt.setFlags("bar", NetworkTable.PERSISTENT);
@@ -41,6 +41,9 @@
     strs[0] = "Hello";
     strs[1] = "World";
     nt.putStringArray("strarray", strs);
-    try { Thread.sleep(10000); } catch (InterruptedException e) {}
+    try {
+      Thread.sleep(10000);
+    } catch (InterruptedException e) {
+    }
   }
 }
diff --git a/third_party/allwpilib/ntcore/manualTests/java/Server.java b/third_party/allwpilib/ntcore/manualTests/java/Server.java
index 7f4dbc6..9527c96 100644
--- a/third_party/allwpilib/ntcore/manualTests/java/Server.java
+++ b/third_party/allwpilib/ntcore/manualTests/java/Server.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 import edu.wpi.first.wpilibj.networktables.*;
 import edu.wpi.first.wpilibj.tables.*;
@@ -21,13 +18,19 @@
     NetworkTable.setPort(10000);
     NetworkTable.setServerMode();
     NetworkTable nt = NetworkTable.getTable("");
-    try { Thread.sleep(1000); } catch (InterruptedException e) {}
+    try {
+      Thread.sleep(1000);
+    } catch (InterruptedException e) {
+    }
     nt.putNumber("foo", 0.5);
     nt.setFlags("foo", NetworkTable.PERSISTENT);
     nt.putNumber("foo2", 0.5);
     nt.putNumber("foo2", 0.7);
     nt.putNumber("foo2", 0.6);
     nt.putNumber("foo2", 0.5);
-    try { Thread.sleep(10000); } catch (InterruptedException e) {}
+    try {
+      Thread.sleep(10000);
+    } catch (InterruptedException e) {
+    }
   }
 }
diff --git a/third_party/allwpilib/ntcore/manualTests/native/client.cpp b/third_party/allwpilib/ntcore/manualTests/native/client.cpp
index bb16a9c..f946673 100644
--- a/third_party/allwpilib/ntcore/manualTests/native/client.cpp
+++ b/third_party/allwpilib/ntcore/manualTests/native/client.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <climits>
@@ -26,8 +23,9 @@
 
   auto foo = nt::GetEntry(inst, "/foo");
   auto foo_val = nt::GetEntryValue(foo);
-  if (foo_val && foo_val->IsDouble())
+  if (foo_val && foo_val->IsDouble()) {
     std::printf("Got foo: %g\n", foo_val->GetDouble());
+  }
 
   auto bar = nt::GetEntry(inst, "/bar");
   nt::SetEntryValue(bar, nt::Value::MakeBoolean(false));
diff --git a/third_party/allwpilib/ntcore/manualTests/native/rpc_local.cpp b/third_party/allwpilib/ntcore/manualTests/native/rpc_local.cpp
index 3311463..2879c33 100644
--- a/third_party/allwpilib/ntcore/manualTests/native/rpc_local.cpp
+++ b/third_party/allwpilib/ntcore/manualTests/native/rpc_local.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <climits>
diff --git a/third_party/allwpilib/ntcore/manualTests/native/rpc_speed.cpp b/third_party/allwpilib/ntcore/manualTests/native/rpc_speed.cpp
index 70558ef..6f9dbbf 100644
--- a/third_party/allwpilib/ntcore/manualTests/native/rpc_speed.cpp
+++ b/third_party/allwpilib/ntcore/manualTests/native/rpc_speed.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <climits>
-#include <cstdio>
-#include <iostream>
 #include <thread>
 
+#include <fmt/core.h>
 #include <support/json.h>
 
 #include "ntcore.h"
@@ -20,11 +16,11 @@
   try {
     params = wpi::json::from_cbor(answer.params);
   } catch (wpi::json::parse_error err) {
-    std::fputs("could not decode params?\n", stderr);
+    fmt::print(stderr, "could not decode params?\n");
     return;
   }
   if (!params.is_number()) {
-    std::fputs("did not get number\n", stderr);
+    fmt::print(stderr, "did not get number\n");
     return;
   }
   double val = params.get<double>();
@@ -47,29 +43,25 @@
     try {
       call1_result = wpi::json::from_cbor(call1_result_str);
     } catch (wpi::json::parse_error err) {
-      std::fputs("could not decode result?\n", stderr);
+      fmt::print(stderr, "could not decode result?\n");
       return 1;
     }
     if (!call1_result.is_number()) {
-      std::fputs("result is not number?\n", stderr);
+      fmt::print(stderr, "result is not number?\n");
       return 1;
     }
   }
   auto end2 = std::chrono::high_resolution_clock::now();
   auto end = nt::Now();
-  std::cerr << "nt::Now start=" << start << " end=" << end << '\n';
-  std::cerr << "std::chrono start="
-            << std::chrono::duration_cast<std::chrono::nanoseconds>(
-                   start2.time_since_epoch())
-                   .count()
-            << " end="
-            << std::chrono::duration_cast<std::chrono::nanoseconds>(
-                   end2.time_since_epoch())
-                   .count()
-            << '\n';
-  std::fprintf(stderr, "time/call = %g us\n", (end - start) / 10.0 / 10000.0);
+  fmt::print(stderr, "nt::Now start={} end={}\n", start, end);
+  fmt::print(stderr, "std::chrono start={} end={}\n",
+             std::chrono::duration_cast<std::chrono::nanoseconds>(
+                 start2.time_since_epoch())
+                 .count(),
+             std::chrono::duration_cast<std::chrono::nanoseconds>(
+                 end2.time_since_epoch())
+                 .count());
+  fmt::print(stderr, "time/call = %g us\n", (end - start) / 10.0 / 10000.0);
   std::chrono::duration<double, std::micro> diff = end2 - start2;
-  std::cerr << "time/call = " << (diff.count() / 10000.0) << " us\n";
-
-  return 0;
+  fmt::print(stderr, "time/call = {} us\n", diff.count() / 10000.0);
 }
diff --git a/third_party/allwpilib/ntcore/manualTests/native/server.cpp b/third_party/allwpilib/ntcore/manualTests/native/server.cpp
index 087ddb4..0825960 100644
--- a/third_party/allwpilib/ntcore/manualTests/native/server.cpp
+++ b/third_party/allwpilib/ntcore/manualTests/native/server.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <climits>
diff --git a/third_party/allwpilib/ntcore/ntcore-config.cmake.in b/third_party/allwpilib/ntcore/ntcore-config.cmake.in
index fb677b3..17006a5 100644
--- a/third_party/allwpilib/ntcore/ntcore-config.cmake.in
+++ b/third_party/allwpilib/ntcore/ntcore-config.cmake.in
@@ -2,4 +2,5 @@
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/ntcore.cmake)
diff --git a/third_party/allwpilib/ntcore/src/dev/java/edu/wpi/first/ntcore/DevMain.java b/third_party/allwpilib/ntcore/src/dev/java/edu/wpi/first/ntcore/DevMain.java
index 547af0a..466731c 100644
--- a/third_party/allwpilib/ntcore/src/dev/java/edu/wpi/first/ntcore/DevMain.java
+++ b/third_party/allwpilib/ntcore/src/dev/java/edu/wpi/first/ntcore/DevMain.java
@@ -1,25 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.ntcore;
 
 import edu.wpi.first.networktables.NetworkTablesJNI;
-import edu.wpi.first.wpiutil.RuntimeDetector;
+import edu.wpi.first.util.RuntimeDetector;
 
 public final class DevMain {
-  /**
-   * Main method.
-   */
+  /** Main method. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
     System.out.println(RuntimeDetector.getPlatformPath());
     NetworkTablesJNI.flush(NetworkTablesJNI.getDefaultInstance());
   }
 
-  private DevMain() {
-  }
+  private DevMain() {}
 }
diff --git a/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp b/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp
index 1153347..f863018 100644
--- a/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/ntcore/src/dev/native/cpp/main.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
+#include <fmt/core.h>
 
 #include "ntcore.h"
 
@@ -14,5 +11,5 @@
 
   nt::SetEntryValue(myValue, nt::Value::MakeString("Hello World"));
 
-  std::cout << nt::GetEntryValue(myValue)->GetString() << std::endl;
+  fmt::print("{}\n", nt::GetEntryValue(myValue)->GetString());
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionInfo.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionInfo.java
index b060f2f..477d53b 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionInfo.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionInfo.java
@@ -1,51 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables Connection information.
- */
+/** NetworkTables Connection information. */
 public final class ConnectionInfo {
   /**
-   * The remote identifier (as set on the remote node by
-   * {@link NetworkTableInstance#setNetworkIdentity(String)}).
+   * The remote identifier (as set on the remote node by {@link
+   * NetworkTableInstance#setNetworkIdentity(String)}).
    */
   @SuppressWarnings("MemberName")
   public final String remote_id;
 
-  /**
-   * The IP address of the remote node.
-   */
+  /** The IP address of the remote node. */
   @SuppressWarnings("MemberName")
   public final String remote_ip;
 
-  /**
-   * The port number of the remote node.
-   */
+  /** The port number of the remote node. */
   @SuppressWarnings("MemberName")
   public final int remote_port;
 
   /**
-   * The last time any update was received from the remote node (same scale as
-   * returned by {@link NetworkTablesJNI#now()}).
+   * The last time any update was received from the remote node (same scale as returned by {@link
+   * NetworkTablesJNI#now()}).
    */
   @SuppressWarnings("MemberName")
   public final long last_update;
 
   /**
-   * The protocol version being used for this connection.  This is in protocol
-   * layer format, so 0x0200 = 2.0, 0x0300 = 3.0).
+   * The protocol version being used for this connection. This is in protocol layer format, so
+   * 0x0200 = 2.0, 0x0300 = 3.0).
    */
   @SuppressWarnings("MemberName")
   public final int protocol_version;
 
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param remoteId Remote identifier
    * @param remoteIp Remote IP address
@@ -53,8 +44,8 @@
    * @param lastUpdate Last time an update was received
    * @param protocolVersion The protocol version used for the connection
    */
-  public ConnectionInfo(String remoteId, String remoteIp, int remotePort, long lastUpdate,
-                        int protocolVersion) {
+  public ConnectionInfo(
+      String remoteId, String remoteIp, int remotePort, long lastUpdate, int protocolVersion) {
     remote_id = remoteId;
     remote_ip = remoteIp;
     remote_port = remotePort;
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionNotification.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionNotification.java
index 129bf1a..544e075 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionNotification.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/ConnectionNotification.java
@@ -1,44 +1,33 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables Connection notification.
- */
+/** NetworkTables Connection notification. */
 public final class ConnectionNotification {
-  /**
-   *  Listener that was triggered.
-   */
+  /** Listener that was triggered. */
   @SuppressWarnings("MemberName")
   public final int listener;
 
-  /**
-   * True if event is due to connection being established.
-   */
+  /** True if event is due to connection being established. */
   @SuppressWarnings("MemberName")
   public final boolean connected;
 
-  /**
-   * Connection information.
-   */
+  /** Connection information. */
   @SuppressWarnings("MemberName")
   public final ConnectionInfo conn;
 
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param inst Instance
    * @param listener Listener that was triggered
    * @param connected Connected if true
    * @param conn Connection information
    */
-  public ConnectionNotification(NetworkTableInstance inst, int listener, boolean connected,
-                                ConnectionInfo conn) {
+  public ConnectionNotification(
+      NetworkTableInstance inst, int listener, boolean connected, ConnectionInfo conn) {
     this.m_inst = inst;
     this.listener = listener;
     this.connected = connected;
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryInfo.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryInfo.java
index b516cc7..471edc8 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryInfo.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryInfo.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables Entry information.
- */
+/** NetworkTables Entry information. */
 public final class EntryInfo {
   /** Entry handle. */
   @SuppressWarnings("MemberName")
@@ -31,8 +26,8 @@
   @SuppressWarnings("MemberName")
   public final long last_change;
 
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param inst Instance
    * @param entry Entry handle
@@ -41,8 +36,8 @@
    * @param flags Flags
    * @param lastChange Timestamp of last change
    */
-  public EntryInfo(NetworkTableInstance inst, int entry, String name, int type, int flags,
-                   long lastChange) {
+  public EntryInfo(
+      NetworkTableInstance inst, int entry, String name, int type, int flags, long lastChange) {
     this.m_inst = inst;
     this.entry = entry;
     this.name = name;
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryListenerFlags.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryListenerFlags.java
index 9cdc0f0..856c962 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryListenerFlags.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryListenerFlags.java
@@ -1,42 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
 /**
  * Flag values for use with entry listeners.
  *
- * <p>The flags are a bitmask and must be OR'ed together to indicate the
- * combination of events desired to be received.
+ * <p>The flags are a bitmask and must be OR'ed together to indicate the combination of events
+ * desired to be received.
  *
- * <p>The constants kNew, kDelete, kUpdate, and kFlags represent different events
- * that can occur to entries.
+ * <p>The constants kNew, kDelete, kUpdate, and kFlags represent different events that can occur to
+ * entries.
  *
- * <p>By default, notifications are only generated for remote changes occurring
- * after the listener is created.  The constants kImmediate and kLocal are
- * modifiers that cause notifications to be generated at other times.
+ * <p>By default, notifications are only generated for remote changes occurring after the listener
+ * is created. The constants kImmediate and kLocal are modifiers that cause notifications to be
+ * generated at other times.
  */
 public interface EntryListenerFlags {
   /**
    * Initial listener addition.
    *
-   * <p>Set this flag to receive immediate notification of entries matching the
-   * flag criteria (generally only useful when combined with kNew).
+   * <p>Set this flag to receive immediate notification of entries matching the flag criteria
+   * (generally only useful when combined with kNew).
    */
   int kImmediate = 0x01;
 
   /**
    * Changed locally.
    *
-   * <p>Set this flag to receive notification of both local changes and changes
-   * coming from remote nodes.  By default, notifications are only generated
-   * for remote changes.  Must be combined with some combination of kNew,
-   * kDelete, kUpdate, and kFlags to receive notifications of those respective
-   * events.
+   * <p>Set this flag to receive notification of both local changes and changes coming from remote
+   * nodes. By default, notifications are only generated for remote changes. Must be combined with
+   * some combination of kNew, kDelete, kUpdate, and kFlags to receive notifications of those
+   * respective events.
    */
   int kLocal = 0x02;
 
@@ -57,16 +53,14 @@
   /**
    * Entry's value changed.
    *
-   * <p>Set this flag to receive a notification when an entry's value (or type)
-   * changes.
+   * <p>Set this flag to receive a notification when an entry's value (or type) changes.
    */
   int kUpdate = 0x10;
 
   /**
    * Entry's flags changed.
    *
-   * <p>Set this flag to receive a notification when an entry's flags value
-   * changes.
+   * <p>Set this flag to receive a notification when an entry's flags value changes.
    */
   int kFlags = 0x20;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryNotification.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryNotification.java
index 159b968..0f5cb51 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryNotification.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/EntryNotification.java
@@ -1,49 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables Entry notification.
- */
+/** NetworkTables Entry notification. */
 public final class EntryNotification {
-  /**
-   * Listener that was triggered.
-   */
+  /** Listener that was triggered. */
   @SuppressWarnings("MemberName")
   public final int listener;
 
-  /**
-   * Entry handle.
-   */
+  /** Entry handle. */
   @SuppressWarnings("MemberName")
   public final int entry;
 
-  /**
-   * Entry name.
-   */
+  /** Entry name. */
   @SuppressWarnings("MemberName")
   public final String name;
 
-  /**
-   * The new value.
-   */
+  /** The new value. */
   @SuppressWarnings("MemberName")
   public final NetworkTableValue value;
 
   /**
-   * Update flags.  For example, {@link EntryListenerFlags#kNew} if the key did
-   * not previously exist.
+   * Update flags. For example, {@link EntryListenerFlags#kNew} if the key did not previously exist.
    */
   @SuppressWarnings("MemberName")
   public final int flags;
 
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param inst Instance
    * @param listener Listener that was triggered
@@ -52,8 +38,13 @@
    * @param value The new value
    * @param flags Update flags
    */
-  public EntryNotification(NetworkTableInstance inst, int listener, int entry, String name,
-                           NetworkTableValue value, int flags) {
+  public EntryNotification(
+      NetworkTableInstance inst,
+      int listener,
+      int entry,
+      String name,
+      NetworkTableValue value,
+      int flags) {
     this.m_inst = inst;
     this.listener = listener;
     this.entry = entry;
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/LogMessage.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/LogMessage.java
index 2cf22ec..5d77d8b 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/LogMessage.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/LogMessage.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables log message.
- */
+/** NetworkTables log message. */
 public final class LogMessage {
-  /**
-   * Logging levels.
-   */
+  /** Logging levels. */
   public static final int kCritical = 50;
+
   public static final int kError = 40;
   public static final int kWarning = 30;
   public static final int kInfo = 20;
@@ -24,38 +18,28 @@
   public static final int kDebug3 = 7;
   public static final int kDebug4 = 6;
 
-  /**
-   * The logger that generated the message.
-   */
+  /** The logger that generated the message. */
   @SuppressWarnings("MemberName")
   public final int logger;
 
-  /**
-   * Log level of the message.
-   */
+  /** Log level of the message. */
   @SuppressWarnings("MemberName")
   public final int level;
 
-  /**
-   * The filename of the source file that generated the message.
-   */
+  /** The filename of the source file that generated the message. */
   @SuppressWarnings("MemberName")
   public final String filename;
 
-  /**
-   * The line number in the source file that generated the message.
-   */
+  /** The line number in the source file that generated the message. */
   @SuppressWarnings("MemberName")
   public final int line;
 
-  /**
-   * The message.
-   */
+  /** The message. */
   @SuppressWarnings("MemberName")
   public final String message;
 
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param inst Instance
    * @param logger Logger
@@ -64,8 +48,8 @@
    * @param line Line number
    * @param message Message
    */
-  public LogMessage(NetworkTableInstance inst, int logger, int level, String filename, int line,
-                    String message) {
+  public LogMessage(
+      NetworkTableInstance inst, int logger, int level, String filename, int line, String message) {
     this.m_inst = inst;
     this.logger = logger;
     this.level = level;
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NTSendable.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NTSendable.java
new file mode 100644
index 0000000..641224d
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NTSendable.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+/** Interface for NetworkTable Sendable objects. */
+public interface NTSendable extends Sendable {
+  /**
+   * Initializes this {@link Sendable} object.
+   *
+   * @param builder sendable builder
+   */
+  void initSendable(NTSendableBuilder builder);
+
+  @Override
+  default void initSendable(SendableBuilder builder) {
+    if (builder.getBackendKind() == SendableBuilder.BackendKind.kNetworkTables) {
+      initSendable((NTSendableBuilder) builder);
+    }
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NTSendableBuilder.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NTSendableBuilder.java
new file mode 100644
index 0000000..65bfbca
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NTSendableBuilder.java
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.networktables;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+public interface NTSendableBuilder extends SendableBuilder {
+  /**
+   * Set the function that should be called to update the network table for things other than
+   * properties. Note this function is not passed the network table object; instead it should use
+   * the entry handles returned by getEntry().
+   *
+   * @param func function
+   */
+  void setUpdateTable(Runnable func);
+
+  /**
+   * Add a property without getters or setters. This can be used to get entry handles for the
+   * function called by setUpdateTable().
+   *
+   * @param key property name
+   * @return Network table entry
+   */
+  NetworkTableEntry getEntry(String key);
+
+  /**
+   * Add a NetworkTableValue property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addValueProperty(
+      String key, Supplier<NetworkTableValue> getter, Consumer<NetworkTableValue> setter);
+
+  /**
+   * Get the network table.
+   *
+   * @return The network table
+   */
+  NetworkTable getTable();
+
+  @Override
+  default BackendKind getBackendKind() {
+    return BackendKind.kNetworkTables;
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java
index ad25f23..7837a21 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTable.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
@@ -16,13 +13,9 @@
 import java.util.concurrent.ConcurrentMap;
 import java.util.function.Consumer;
 
-/**
- * A network table that knows its subtable path.
- */
+/** A network table that knows its subtable path. */
 public final class NetworkTable {
-  /**
-   * The path separator for sub-tables and keys.
-   */
+  /** The path separator for sub-tables and keys. */
   public static final char PATH_SEPARATOR = '/';
 
   private final String m_path;
@@ -30,8 +23,8 @@
   private final NetworkTableInstance m_inst;
 
   /**
-   * Gets the "base name" of a key. For example, "/foo/bar" becomes "bar".
-   * If the key has a trailing slash, returns an empty string.
+   * Gets the "base name" of a key. For example, "/foo/bar" becomes "bar". If the key has a trailing
+   * slash, returns an empty string.
    *
    * @param key key
    * @return base name
@@ -45,8 +38,8 @@
   }
 
   /**
-   * Normalizes an network table key to contain no consecutive slashes and
-   * optionally start with a leading slash. For example:
+   * Normalizes an network table key to contain no consecutive slashes and optionally start with a
+   * leading slash. For example:
    *
    * <pre><code>
    * normalizeKey("/foo/bar", true)  == "/foo/bar"
@@ -55,9 +48,8 @@
    * normalizeKey("foo//bar", false) == "foo/bar"
    * </code></pre>
    *
-   * @param key              the key to normalize
-   * @param withLeadingSlash whether or not the normalized key should begin
-   *                         with a leading slash
+   * @param key the key to normalize
+   * @param withLeadingSlash whether or not the normalized key should begin with a leading slash
    * @return normalized key
    */
   public static String normalizeKey(String key, boolean withLeadingSlash) {
@@ -77,10 +69,9 @@
   }
 
   /**
-   * Normalizes a network table key to start with exactly one leading slash
-   * ("/") and contain no consecutive slashes. For example,
-   * {@code "//foo/bar/"} becomes {@code "/foo/bar/"} and
-   * {@code "///a/b/c"} becomes {@code "/a/b/c"}.
+   * Normalizes a network table key to start with exactly one leading slash ("/") and contain no
+   * consecutive slashes. For example, {@code "//foo/bar/"} becomes {@code "/foo/bar/"} and {@code
+   * "///a/b/c"} becomes {@code "/a/b/c"}.
    *
    * <p>This is equivalent to {@code normalizeKey(key, true)}
    *
@@ -92,9 +83,8 @@
   }
 
   /**
-   * Gets a list of the names of all the super tables of a given key. For
-   * example, the key "/foo/bar/baz" has a hierarchy of "/", "/foo",
-   * "/foo/bar", and "/foo/bar/baz".
+   * Gets a list of the names of all the super tables of a given key. For example, the key
+   * "/foo/bar/baz" has a hierarchy of "/", "/foo", "/foo/bar", and "/foo/bar/baz".
    *
    * @param key the key
    * @return List of super tables
@@ -118,9 +108,7 @@
     return hierarchy;
   }
 
-  /**
-   * Constructor.  Use NetworkTableInstance.getTable() or getSubTable() instead.
-   */
+  /** Constructor. Use NetworkTableInstance.getTable() or getSubTable() instead. */
   NetworkTable(NetworkTableInstance inst, String path) {
     m_path = path;
     m_pathWithSep = path + PATH_SEPARATOR;
@@ -161,52 +149,54 @@
   /**
    * Listen to keys only within this table.
    *
-   * @param listener    listener to add
-   * @param flags       {@link EntryListenerFlags} bitmask
+   * @param listener listener to add
+   * @param flags {@link EntryListenerFlags} bitmask
    * @return Listener handle
    */
   public int addEntryListener(TableEntryListener listener, int flags) {
     final int prefixLen = m_path.length() + 1;
-    return m_inst.addEntryListener(m_pathWithSep, event -> {
-      String relativeKey = event.name.substring(prefixLen);
-      if (relativeKey.indexOf(PATH_SEPARATOR) != -1) {
-        // part of a sub table
-        return;
-      }
-      listener.valueChanged(this, relativeKey, event.getEntry(), event.value, event.flags);
-    }, flags);
+    return m_inst.addEntryListener(
+        m_pathWithSep,
+        event -> {
+          String relativeKey = event.name.substring(prefixLen);
+          if (relativeKey.indexOf(PATH_SEPARATOR) != -1) {
+            // part of a sub table
+            return;
+          }
+          listener.valueChanged(this, relativeKey, event.getEntry(), event.value, event.flags);
+        },
+        flags);
   }
 
   /**
    * Listen to a single key.
    *
-   * @param key         the key name
-   * @param listener    listener to add
-   * @param flags       {@link EntryListenerFlags} bitmask
+   * @param key the key name
+   * @param listener listener to add
+   * @param flags {@link EntryListenerFlags} bitmask
    * @return Listener handle
    */
   public int addEntryListener(String key, TableEntryListener listener, int flags) {
     final NetworkTableEntry entry = getEntry(key);
-    return m_inst.addEntryListener(entry,
-        event -> listener.valueChanged(this, key, entry, event.value, event.flags), flags);
+    return m_inst.addEntryListener(
+        entry, event -> listener.valueChanged(this, key, entry, event.value, event.flags), flags);
   }
 
   /**
    * Remove an entry listener.
    *
-   * @param listener    listener handle
+   * @param listener listener handle
    */
   public void removeEntryListener(int listener) {
     m_inst.removeEntryListener(listener);
   }
 
   /**
-   * Listen for sub-table creation.
-   * This calls the listener once for each newly created sub-table.
+   * Listen for sub-table creation. This calls the listener once for each newly created sub-table.
    * It immediately calls the listener for any existing sub-tables.
    *
-   * @param listener        listener to add
-   * @param localNotify     notify local changes as well as remote
+   * @param listener listener to add
+   * @param localNotify notify local changes as well as remote
    * @return Listener handle
    */
   public int addSubTableListener(TableListener listener, boolean localNotify) {
@@ -218,38 +208,41 @@
     final int prefixLen = m_path.length() + 1;
     final NetworkTable parent = this;
 
-    return m_inst.addEntryListener(m_pathWithSep, new Consumer<>() {
-      final Set<String> m_notifiedTables = new HashSet<>();
+    return m_inst.addEntryListener(
+        m_pathWithSep,
+        new Consumer<>() {
+          final Set<String> m_notifiedTables = new HashSet<>();
 
-      @Override
-      public void accept(EntryNotification event) {
-        String relativeKey = event.name.substring(prefixLen);
-        int endSubTable = relativeKey.indexOf(PATH_SEPARATOR);
-        if (endSubTable == -1) {
-          return;
-        }
-        String subTableKey = relativeKey.substring(0, endSubTable);
-        if (m_notifiedTables.contains(subTableKey)) {
-          return;
-        }
-        m_notifiedTables.add(subTableKey);
-        listener.tableCreated(parent, subTableKey, parent.getSubTable(subTableKey));
-      }
-    }, flags);
+          @Override
+          public void accept(EntryNotification event) {
+            String relativeKey = event.name.substring(prefixLen);
+            int endSubTable = relativeKey.indexOf(PATH_SEPARATOR);
+            if (endSubTable == -1) {
+              return;
+            }
+            String subTableKey = relativeKey.substring(0, endSubTable);
+            if (m_notifiedTables.contains(subTableKey)) {
+              return;
+            }
+            m_notifiedTables.add(subTableKey);
+            listener.tableCreated(parent, subTableKey, parent.getSubTable(subTableKey));
+          }
+        },
+        flags);
   }
 
   /**
    * Remove a sub-table listener.
    *
-   * @param listener    listener handle
+   * @param listener listener handle
    */
   public void removeTableListener(int listener) {
     m_inst.removeEntryListener(listener);
   }
 
   /**
-   * Returns the table at the specified key. If there is no table at the
-   * specified key, it will create a new table
+   * Returns the table at the specified key. If there is no table at the specified key, it will
+   * create a new table
    *
    * @param key the name of the table relative to this one
    * @return a sub table relative to this one
@@ -276,8 +269,8 @@
    *     its own
    */
   public boolean containsSubTable(String key) {
-    int[] handles = NetworkTablesJNI.getEntries(m_inst.getHandle(),
-        m_pathWithSep + key + PATH_SEPARATOR, 0);
+    int[] handles =
+        NetworkTablesJNI.getEntries(m_inst.getHandle(), m_pathWithSep + key + PATH_SEPARATOR, 0);
     return handles.length != 0;
   }
 
@@ -333,8 +326,7 @@
   }
 
   /**
-   * Deletes the specified key in this table. The key can
-   * not be null.
+   * Deletes the specified key in this table. The key can not be null.
    *
    * @param key the key name
    */
@@ -376,16 +368,17 @@
 
   /**
    * Get the path of the NetworkTable.
+   *
+   * @return The path of the NetworkTable.
    */
   public String getPath() {
     return m_path;
   }
 
   /**
-   * Save table values to a file.  The file format used is identical to
-   * that used for SavePersistent.
+   * Save table values to a file. The file format used is identical to that used for SavePersistent.
    *
-   * @param filename  filename
+   * @param filename filename
    * @throws PersistentException if error saving file
    */
   public void saveEntries(String filename) throws PersistentException {
@@ -393,10 +386,10 @@
   }
 
   /**
-   * Load table values from a file.  The file format used is identical to
-   * that used for SavePersistent / LoadPersistent.
+   * Load table values from a file. The file format used is identical to that used for
+   * SavePersistent / LoadPersistent.
    *
-   * @param filename  filename
+   * @param filename filename
    * @return List of warnings (errors result in an exception instead)
    * @throws PersistentException if error saving file
    */
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableEntry.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableEntry.java
index 6eea5ca..872bc89 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableEntry.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableEntry.java
@@ -1,22 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
 import java.nio.ByteBuffer;
 import java.util.function.Consumer;
 
-/**
- * NetworkTables Entry.
- */
+/** NetworkTables Entry. */
 public final class NetworkTableEntry {
-  /**
-   * Flag values (as returned by {@link #getFlags()}).
-   */
+  /** Flag values (as returned by {@link #getFlags()}). */
   public static final int kPersistent = 0x01;
 
   /**
@@ -112,8 +105,7 @@
   }
 
   /**
-   * Gets the entry's value.
-   * Returns a value with type NetworkTableType.kUnassigned if the value
+   * Gets the entry's value. Returns a value with type NetworkTableType.kUnassigned if the value
    * does not exist.
    *
    * @return the entry's value
@@ -123,8 +115,8 @@
   }
 
   /**
-   * Gets the entry's value as a boolean. If the entry does not exist or is of
-   * different type, it will return the default value.
+   * Gets the entry's value as a boolean. If the entry does not exist or is of different type, it
+   * will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -134,8 +126,8 @@
   }
 
   /**
-   * Gets the entry's value as a double. If the entry does not exist or is of
-   * different type, it will return the default value.
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -145,8 +137,8 @@
   }
 
   /**
-   * Gets the entry's value as a double. If the entry does not exist or is of
-   * different type, it will return the default value.
+   * Gets the entry's value as a double. If the entry does not exist or is of different type, it
+   * will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -156,8 +148,8 @@
   }
 
   /**
-   * Gets the entry's value as a string. If the entry does not exist or is of
-   * different type, it will return the default value.
+   * Gets the entry's value as a string. If the entry does not exist or is of different type, it
+   * will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -167,8 +159,8 @@
   }
 
   /**
-   * Gets the entry's value as a raw value (byte array). If the entry does not
-   * exist or is of different type, it will return the default value.
+   * Gets the entry's value as a raw value (byte array). If the entry does not exist or is of
+   * different type, it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -178,8 +170,8 @@
   }
 
   /**
-   * Gets the entry's value as a boolean array. If the entry does not exist
-   * or is of different type, it will return the default value.
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -189,20 +181,20 @@
   }
 
   /**
-   * Gets the entry's value as a boolean array. If the entry does not exist
-   * or is of different type, it will return the default value.
+   * Gets the entry's value as a boolean array. If the entry does not exist or is of different type,
+   * it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
    */
   public Boolean[] getBooleanArray(Boolean[] defaultValue) {
-    return NetworkTableValue.fromNative(NetworkTablesJNI.getBooleanArray(m_handle,
-        NetworkTableValue.toNative(defaultValue)));
+    return NetworkTableValue.fromNative(
+        NetworkTablesJNI.getBooleanArray(m_handle, NetworkTableValue.toNative(defaultValue)));
   }
 
   /**
-   * Gets the entry's value as a double array. If the entry does not exist
-   * or is of different type, it will return the default value.
+   * Gets the entry's value as a double array. If the entry does not exist or is of different type,
+   * it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -212,32 +204,32 @@
   }
 
   /**
-   * Gets the entry's value as a double array. If the entry does not exist
-   * or is of different type, it will return the default value.
+   * Gets the entry's value as a double array. If the entry does not exist or is of different type,
+   * it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
    */
   public Double[] getDoubleArray(Double[] defaultValue) {
-    return NetworkTableValue.fromNative(NetworkTablesJNI.getDoubleArray(m_handle,
-        NetworkTableValue.toNative(defaultValue)));
+    return NetworkTableValue.fromNative(
+        NetworkTablesJNI.getDoubleArray(m_handle, NetworkTableValue.toNative(defaultValue)));
   }
 
   /**
-   * Gets the entry's value as a double array. If the entry does not exist
-   * or is of different type, it will return the default value.
+   * Gets the entry's value as a double array. If the entry does not exist or is of different type,
+   * it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
    */
   public Number[] getNumberArray(Number[] defaultValue) {
-    return NetworkTableValue.fromNative(NetworkTablesJNI.getDoubleArray(m_handle,
-        NetworkTableValue.toNative(defaultValue)));
+    return NetworkTableValue.fromNative(
+        NetworkTablesJNI.getDoubleArray(m_handle, NetworkTableValue.toNative(defaultValue)));
   }
 
   /**
-   * Gets the entry's value as a string array. If the entry does not exist
-   * or is of different type, it will return the default value.
+   * Gets the entry's value as a string array. If the entry does not exist or is of different type,
+   * it will return the default value.
    *
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
@@ -279,11 +271,10 @@
       Object otherValue = ((NetworkTableValue) defaultValue).getValue();
       switch (((NetworkTableValue) defaultValue).getType()) {
         case kBoolean:
-          return NetworkTablesJNI.setDefaultBoolean(m_handle, time,
-              (Boolean) otherValue);
+          return NetworkTablesJNI.setDefaultBoolean(m_handle, time, (Boolean) otherValue);
         case kDouble:
-          return NetworkTablesJNI.setDefaultDouble(m_handle, time,
-              ((Number) otherValue).doubleValue());
+          return NetworkTablesJNI.setDefaultDouble(
+              m_handle, time, ((Number) otherValue).doubleValue());
         case kString:
           return NetworkTablesJNI.setDefaultString(m_handle, time, (String) otherValue);
         case kRaw:
@@ -305,21 +296,21 @@
       return setDefaultNumber((Number) defaultValue);
     } else if (defaultValue instanceof String) {
       return setDefaultString((String) defaultValue);
-    } else if (defaultValue instanceof byte[])  {
+    } else if (defaultValue instanceof byte[]) {
       return setDefaultRaw((byte[]) defaultValue);
-    } else if (defaultValue instanceof boolean[])  {
+    } else if (defaultValue instanceof boolean[]) {
       return setDefaultBooleanArray((boolean[]) defaultValue);
-    } else if (defaultValue instanceof double[])  {
+    } else if (defaultValue instanceof double[]) {
       return setDefaultDoubleArray((double[]) defaultValue);
-    } else if (defaultValue instanceof Boolean[])  {
+    } else if (defaultValue instanceof Boolean[]) {
       return setDefaultBooleanArray((Boolean[]) defaultValue);
-    } else if (defaultValue instanceof Number[])  {
+    } else if (defaultValue instanceof Number[]) {
       return setDefaultNumberArray((Number[]) defaultValue);
-    } else if (defaultValue instanceof String[])  {
+    } else if (defaultValue instanceof String[]) {
       return setDefaultStringArray((String[]) defaultValue);
     } else {
-      throw new IllegalArgumentException("Value of type " + defaultValue.getClass().getName()
-        + " cannot be put into a table");
+      throw new IllegalArgumentException(
+          "Value of type " + defaultValue.getClass().getName() + " cannot be put into a table");
     }
   }
 
@@ -390,8 +381,8 @@
    * @return False if the entry exists with a different type
    */
   public boolean setDefaultBooleanArray(Boolean[] defaultValue) {
-    return NetworkTablesJNI.setDefaultBooleanArray(m_handle,
-        0, NetworkTableValue.toNative(defaultValue));
+    return NetworkTablesJNI.setDefaultBooleanArray(
+        m_handle, 0, NetworkTableValue.toNative(defaultValue));
   }
 
   /**
@@ -411,8 +402,8 @@
    * @return False if the entry exists with a different type
    */
   public boolean setDefaultNumberArray(Number[] defaultValue) {
-    return NetworkTablesJNI.setDefaultDoubleArray(m_handle,
-        0, NetworkTableValue.toNative(defaultValue));
+    return NetworkTablesJNI.setDefaultDoubleArray(
+        m_handle, 0, NetworkTableValue.toNative(defaultValue));
   }
 
   /**
@@ -438,11 +429,10 @@
       Object otherValue = ((NetworkTableValue) value).getValue();
       switch (((NetworkTableValue) value).getType()) {
         case kBoolean:
-          return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue,
-              false);
+          return NetworkTablesJNI.setBoolean(m_handle, time, (Boolean) otherValue, false);
         case kDouble:
-          return NetworkTablesJNI.setDouble(m_handle, time, ((Number) otherValue).doubleValue(),
-              false);
+          return NetworkTablesJNI.setDouble(
+              m_handle, time, ((Number) otherValue).doubleValue(), false);
         case kString:
           return NetworkTablesJNI.setString(m_handle, time, (String) otherValue, false);
         case kRaw:
@@ -477,8 +467,8 @@
     } else if (value instanceof String[]) {
       return setStringArray((String[]) value);
     } else {
-      throw new IllegalArgumentException("Value of type " + value.getClass().getName()
-        + " cannot be put into a table");
+      throw new IllegalArgumentException(
+          "Value of type " + value.getClass().getName() + " cannot be put into a table");
     }
   }
 
@@ -600,8 +590,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    * @throws IllegalArgumentException if the value is not a known type
@@ -656,14 +646,14 @@
     } else if (value instanceof String[]) {
       forceSetStringArray((String[]) value);
     } else {
-      throw new IllegalArgumentException("Value of type " + value.getClass().getName()
-        + " cannot be put into a table");
+      throw new IllegalArgumentException(
+          "Value of type " + value.getClass().getName() + " cannot be put into a table");
     }
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -672,8 +662,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -682,8 +672,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -692,8 +682,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -702,8 +692,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -712,8 +702,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -722,8 +712,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -732,8 +722,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -742,8 +732,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -752,8 +742,8 @@
   }
 
   /**
-   * Sets the entry's value.  If the value is of different type, the type is
-   * changed to match the new value.
+   * Sets the entry's value. If the value is of different type, the type is changed to match the new
+   * value.
    *
    * @param value the value to set
    */
@@ -779,16 +769,12 @@
     NetworkTablesJNI.setEntryFlags(m_handle, getFlags() & ~flags);
   }
 
-  /**
-   * Make value persistent through program restarts.
-   */
+  /** Make value persistent through program restarts. */
   public void setPersistent() {
     setFlags(kPersistent);
   }
 
-  /**
-   * Stop making value persistent through program restarts.
-   */
+  /** Stop making value persistent through program restarts. */
   public void clearPersistent() {
     clearFlags(kPersistent);
   }
@@ -802,31 +788,28 @@
     return (getFlags() & kPersistent) != 0;
   }
 
-  /**
-   * Deletes the entry.
-   */
+  /** Deletes the entry. */
   public void delete() {
     NetworkTablesJNI.deleteEntry(m_handle);
   }
 
   /**
-   * Create a callback-based RPC entry point.  Only valid to use on the server.
-   * The callback function will be called when the RPC is called.
-   * This function creates RPC version 0 definitions (raw data in and out).
+   * Create a callback-based RPC entry point. Only valid to use on the server. The callback function
+   * will be called when the RPC is called. This function creates RPC version 0 definitions (raw
+   * data in and out).
    *
-   * @param callback  callback function
+   * @param callback callback function
    */
   public void createRpc(Consumer<RpcAnswer> callback) {
     m_inst.createRpc(this, callback);
   }
 
   /**
-   * Call a RPC function.  May be used on either the client or server.
-   * This function is non-blocking.  Either {@link RpcCall#getResult()} or
-   * {@link RpcCall#cancelResult()} must be called on the return value to either
-   * get or ignore the result of the call.
+   * Call a RPC function. May be used on either the client or server. This function is non-blocking.
+   * Either {@link RpcCall#getResult()} or {@link RpcCall#cancelResult()} must be called on the
+   * return value to either get or ignore the result of the call.
    *
-   * @param params      parameter
+   * @param params parameter
    * @return RPC call object.
    */
   public RpcCall callRpc(byte[] params) {
@@ -870,6 +853,6 @@
     return m_handle;
   }
 
-  private NetworkTableInstance m_inst;
-  private int m_handle;
+  private final NetworkTableInstance m_inst;
+  private final int m_handle;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
index 06dad09..162327e 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableInstance.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
@@ -19,36 +16,33 @@
 /**
  * NetworkTables Instance.
  *
- * <p>Instances are completely independent from each other.  Table operations on
- * one instance will not be visible to other instances unless the instances are
- * connected via the network.  The main limitation on instances is that you
- * cannot have two servers on the same network port.  The main utility of
- * instances is for unit testing, but they can also enable one program to
- * connect to two different NetworkTables networks.
+ * <p>Instances are completely independent from each other. Table operations on one instance will
+ * not be visible to other instances unless the instances are connected via the network. The main
+ * limitation on instances is that you cannot have two servers on the same network port. The main
+ * utility of instances is for unit testing, but they can also enable one program to connect to two
+ * different NetworkTables networks.
  *
- * <p>The global "default" instance (as returned by {@link #getDefault()}) is
- * always available, and is intended for the common case when there is only
- * a single NetworkTables instance being used in the program.
+ * <p>The global "default" instance (as returned by {@link #getDefault()}) is always available, and
+ * is intended for the common case when there is only a single NetworkTables instance being used in
+ * the program.
  *
- * <p>Additional instances can be created with the {@link #create()} function.
- * A reference must be kept to the NetworkTableInstance returned by this
- * function to keep it from being garbage collected.
+ * <p>Additional instances can be created with the {@link #create()} function. A reference must be
+ * kept to the NetworkTableInstance returned by this function to keep it from being garbage
+ * collected.
  */
 public final class NetworkTableInstance implements AutoCloseable {
   /**
-   * Client/server mode flag values (as returned by {@link #getNetworkMode()}).
-   * This is a bitmask.
+   * Client/server mode flag values (as returned by {@link #getNetworkMode()}). This is a bitmask.
    */
   public static final int kNetModeNone = 0x00;
+
   public static final int kNetModeServer = 0x01;
   public static final int kNetModeClient = 0x02;
   public static final int kNetModeStarting = 0x04;
   public static final int kNetModeFailure = 0x08;
   public static final int kNetModeLocal = 0x10;
 
-  /**
-   * The default port that network tables operates on.
-   */
+  /** The default port that network tables operates on. */
   public static final int kDefaultPort = 1735;
 
   /**
@@ -61,9 +55,7 @@
     m_handle = handle;
   }
 
-  /**
-   * Destroys the instance (if created by {@link #create()}).
-   */
+  /** Destroys the instance (if created by {@link #create()}). */
   @Override
   public synchronized void close() {
     if (m_owned && m_handle != 0) {
@@ -96,8 +88,7 @@
   }
 
   /**
-   * Create an instance.
-   * Note: A reference to the returned instance must be retained to ensure the
+   * Create an instance. Note: A reference to the returned instance must be retained to ensure the
    * instance is not garbage collected.
    *
    * @return Newly created instance
@@ -128,12 +119,11 @@
   }
 
   /**
-   * Get entries starting with the given prefix.
-   * The results are optionally filtered by string prefix and entry type to
-   * only return a subset of all entries.
+   * Get entries starting with the given prefix. The results are optionally filtered by string
+   * prefix and entry type to only return a subset of all entries.
    *
-   * @param prefix entry name required prefix; only entries whose name
-   *     starts with this string are returned
+   * @param prefix entry name required prefix; only entries whose name starts with this string are
+   *     returned
    * @param types bitmask of types; 0 is treated as a "don't care"
    * @return Array of entries.
    */
@@ -147,12 +137,11 @@
   }
 
   /**
-   * Get information about entries starting with the given prefix.
-   * The results are optionally filtered by string prefix and entry type to
-   * only return a subset of all entries.
+   * Get information about entries starting with the given prefix. The results are optionally
+   * filtered by string prefix and entry type to only return a subset of all entries.
    *
-   * @param prefix entry name required prefix; only entries whose name
-   *     starts with this string are returned
+   * @param prefix entry name required prefix; only entries whose name starts with this string are
+   *     returned
    * @param types bitmask of types; 0 is treated as a "don't care"
    * @return Array of entry information.
    */
@@ -172,7 +161,7 @@
   public NetworkTable getTable(String key) {
     // prepend leading / if not present
     String theKey;
-    if (key.isEmpty() || key.equals("/")) {
+    if (key.isEmpty() || "/".equals(key)) {
       theKey = "";
     } else if (key.charAt(0) == NetworkTable.PATH_SEPARATOR) {
       theKey = key;
@@ -192,10 +181,7 @@
     return table;
   }
 
-  /**
-   * Deletes ALL keys in ALL subtables (except persistent values).
-   * Use with caution!
-   */
+  /** Deletes ALL keys in ALL subtables (except persistent values). Use with caution! */
   public void deleteAllEntries() {
     NetworkTablesJNI.deleteAllEntries(m_handle);
   }
@@ -216,74 +202,78 @@
 
   private final ReentrantLock m_entryListenerLock = new ReentrantLock();
   private final Map<Integer, EntryConsumer<EntryNotification>> m_entryListeners = new HashMap<>();
-  private Thread m_entryListenerThread;
   private int m_entryListenerPoller;
   private boolean m_entryListenerWaitQueue;
   private final Condition m_entryListenerWaitQueueCond = m_entryListenerLock.newCondition();
 
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
   private void startEntryListenerThread() {
-    m_entryListenerThread = new Thread(() -> {
-      boolean wasInterrupted = false;
-      while (!Thread.interrupted()) {
-        EntryNotification[] events;
-        try {
-          events = NetworkTablesJNI.pollEntryListener(this, m_entryListenerPoller);
-        } catch (InterruptedException ex) {
-          m_entryListenerLock.lock();
-          try {
-            if (m_entryListenerWaitQueue) {
-              m_entryListenerWaitQueue = false;
-              m_entryListenerWaitQueueCond.signalAll();
-              continue;
-            }
-          } finally {
-            m_entryListenerLock.unlock();
-          }
-          Thread.currentThread().interrupt();
-          // don't try to destroy poller, as its handle is likely no longer valid
-          wasInterrupted = true;
-          break;
-        }
-        for (EntryNotification event : events) {
-          EntryConsumer<EntryNotification> listener;
-          m_entryListenerLock.lock();
-          try {
-            listener = m_entryListeners.get(event.listener);
-          } finally {
-            m_entryListenerLock.unlock();
-          }
-          if (listener != null) {
-            event.m_entryObject = listener.m_entry;
-            try {
-              listener.m_consumer.accept(event);
-            } catch (Throwable throwable) {
-              System.err.println("Unhandled exception during entry listener callback: "
-                  + throwable.toString());
-              throwable.printStackTrace();
-            }
-          }
-        }
-      }
-      m_entryListenerLock.lock();
-      try {
-        if (!wasInterrupted) {
-          NetworkTablesJNI.destroyEntryListenerPoller(m_entryListenerPoller);
-        }
-        m_entryListenerPoller = 0;
-      } finally {
-        m_entryListenerLock.unlock();
-      }
-    }, "NTEntryListener");
-    m_entryListenerThread.setDaemon(true);
-    m_entryListenerThread.start();
+    var entryListenerThread =
+        new Thread(
+            () -> {
+              boolean wasInterrupted = false;
+              while (!Thread.interrupted()) {
+                EntryNotification[] events;
+                try {
+                  events = NetworkTablesJNI.pollEntryListener(this, m_entryListenerPoller);
+                } catch (InterruptedException ex) {
+                  m_entryListenerLock.lock();
+                  try {
+                    if (m_entryListenerWaitQueue) {
+                      m_entryListenerWaitQueue = false;
+                      m_entryListenerWaitQueueCond.signalAll();
+                      continue;
+                    }
+                  } finally {
+                    m_entryListenerLock.unlock();
+                  }
+                  Thread.currentThread().interrupt();
+                  // don't try to destroy poller, as its handle is likely no longer valid
+                  wasInterrupted = true;
+                  break;
+                }
+                for (EntryNotification event : events) {
+                  EntryConsumer<EntryNotification> listener;
+                  m_entryListenerLock.lock();
+                  try {
+                    listener = m_entryListeners.get(event.listener);
+                  } finally {
+                    m_entryListenerLock.unlock();
+                  }
+                  if (listener != null) {
+                    event.m_entryObject = listener.m_entry;
+                    try {
+                      listener.m_consumer.accept(event);
+                    } catch (Throwable throwable) {
+                      System.err.println(
+                          "Unhandled exception during entry listener callback: "
+                              + throwable.toString());
+                      throwable.printStackTrace();
+                    }
+                  }
+                }
+              }
+              m_entryListenerLock.lock();
+              try {
+                if (!wasInterrupted) {
+                  NetworkTablesJNI.destroyEntryListenerPoller(m_entryListenerPoller);
+                }
+                m_entryListenerPoller = 0;
+              } finally {
+                m_entryListenerLock.unlock();
+              }
+            },
+            "NTEntryListener");
+    entryListenerThread.setDaemon(true);
+    entryListenerThread.start();
   }
 
   /**
    * Add a listener for all entries starting with a certain prefix.
    *
-   * @param prefix            UTF-8 string prefix
-   * @param listener          listener to add
-   * @param flags             {@link EntryListenerFlags} bitmask
+   * @param prefix UTF-8 string prefix
+   * @param listener listener to add
+   * @param flags {@link EntryListenerFlags} bitmask
    * @return Listener handle
    */
   public int addEntryListener(String prefix, Consumer<EntryNotification> listener, int flags) {
@@ -304,14 +294,13 @@
   /**
    * Add a listener for a particular entry.
    *
-   * @param entry             the entry
-   * @param listener          listener to add
-   * @param flags             {@link EntryListenerFlags} bitmask
+   * @param entry the entry
+   * @param listener listener to add
+   * @param flags {@link EntryListenerFlags} bitmask
    * @return Listener handle
    */
-  public int addEntryListener(NetworkTableEntry entry,
-                              Consumer<EntryNotification> listener,
-                              int flags) {
+  public int addEntryListener(
+      NetworkTableEntry entry, Consumer<EntryNotification> listener, int flags) {
     if (!equals(entry.getInstance())) {
       throw new IllegalArgumentException("entry does not belong to this instance");
     }
@@ -321,8 +310,8 @@
         m_entryListenerPoller = NetworkTablesJNI.createEntryListenerPoller(m_handle);
         startEntryListenerThread();
       }
-      int handle = NetworkTablesJNI.addPolledEntryListener(m_entryListenerPoller, entry.getHandle(),
-          flags);
+      int handle =
+          NetworkTablesJNI.addPolledEntryListener(m_entryListenerPoller, entry.getHandle(), flags);
       m_entryListeners.put(handle, new EntryConsumer<>(entry, listener));
       return handle;
     } finally {
@@ -340,15 +329,14 @@
   }
 
   /**
-   * Wait for the entry listener queue to be empty.  This is primarily useful
-   * for deterministic testing.  This blocks until either the entry listener
-   * queue is empty (e.g. there are no more events that need to be passed along
-   * to callbacks or poll queues) or the timeout expires.
+   * Wait for the entry listener queue to be empty. This is primarily useful for deterministic
+   * testing. This blocks until either the entry listener queue is empty (e.g. there are no more
+   * events that need to be passed along to callbacks or poll queues) or the timeout expires.
    *
-   * @param timeout   timeout, in seconds.  Set to 0 for non-blocking behavior,
-   *                  or a negative value to block indefinitely
+   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
+   *     block indefinitely
    * @return False if timed out, otherwise true.
-  */
+   */
   public boolean waitForEntryListenerQueue(double timeout) {
     if (!NetworkTablesJNI.waitForEntryListenerQueue(m_handle, timeout)) {
       return false;
@@ -363,8 +351,8 @@
             if (timeout < 0) {
               m_entryListenerWaitQueueCond.await();
             } else {
-              return m_entryListenerWaitQueueCond.await((long) (timeout * 1e9),
-                  TimeUnit.NANOSECONDS);
+              return m_entryListenerWaitQueueCond.await(
+                  (long) (timeout * 1e9), TimeUnit.NANOSECONDS);
             }
           } catch (InterruptedException ex) {
             Thread.currentThread().interrupt();
@@ -379,68 +367,73 @@
   }
 
   private final ReentrantLock m_connectionListenerLock = new ReentrantLock();
-  private final Map<Integer, Consumer<ConnectionNotification>> m_connectionListeners
-      = new HashMap<>();
-  private Thread m_connectionListenerThread;
+  private final Map<Integer, Consumer<ConnectionNotification>> m_connectionListeners =
+      new HashMap<>();
   private int m_connectionListenerPoller;
   private boolean m_connectionListenerWaitQueue;
-  private final Condition m_connectionListenerWaitQueueCond
-      = m_connectionListenerLock.newCondition();
+  private final Condition m_connectionListenerWaitQueueCond =
+      m_connectionListenerLock.newCondition();
 
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
   private void startConnectionListenerThread() {
-    m_connectionListenerThread = new Thread(() -> {
-      boolean wasInterrupted = false;
-      while (!Thread.interrupted()) {
-        ConnectionNotification[] events;
-        try {
-          events = NetworkTablesJNI.pollConnectionListener(this, m_connectionListenerPoller);
-        } catch (InterruptedException ex) {
-          m_connectionListenerLock.lock();
-          try {
-            if (m_connectionListenerWaitQueue) {
-              m_connectionListenerWaitQueue = false;
-              m_connectionListenerWaitQueueCond.signalAll();
-              continue;
-            }
-          } finally {
-            m_connectionListenerLock.unlock();
-          }
-          Thread.currentThread().interrupt();
-          // don't try to destroy poller, as its handle is likely no longer valid
-          wasInterrupted = true;
-          break;
-        }
-        for (ConnectionNotification event : events) {
-          Consumer<ConnectionNotification> listener;
-          m_connectionListenerLock.lock();
-          try {
-            listener = m_connectionListeners.get(event.listener);
-          } finally {
-            m_connectionListenerLock.unlock();
-          }
-          if (listener != null) {
-            try {
-              listener.accept(event);
-            } catch (Throwable throwable) {
-              System.err.println("Unhandled exception during connection listener callback: "
-                  + throwable.toString());
-              throwable.printStackTrace();
-            }
-          }
-        }
-      }
-      m_connectionListenerLock.lock();
-      try {
-        if (!wasInterrupted) {
-          NetworkTablesJNI.destroyConnectionListenerPoller(m_connectionListenerPoller);
-        }
-        m_connectionListenerPoller = 0;
-      } finally {
-        m_connectionListenerLock.unlock();
-      }
-    }, "NTConnectionListener");
-    m_connectionListenerThread.setDaemon(true);
-    m_connectionListenerThread.start();
+    var connectionListenerThread =
+        new Thread(
+            () -> {
+              boolean wasInterrupted = false;
+              while (!Thread.interrupted()) {
+                ConnectionNotification[] events;
+                try {
+                  events =
+                      NetworkTablesJNI.pollConnectionListener(this, m_connectionListenerPoller);
+                } catch (InterruptedException ex) {
+                  m_connectionListenerLock.lock();
+                  try {
+                    if (m_connectionListenerWaitQueue) {
+                      m_connectionListenerWaitQueue = false;
+                      m_connectionListenerWaitQueueCond.signalAll();
+                      continue;
+                    }
+                  } finally {
+                    m_connectionListenerLock.unlock();
+                  }
+                  Thread.currentThread().interrupt();
+                  // don't try to destroy poller, as its handle is likely no longer valid
+                  wasInterrupted = true;
+                  break;
+                }
+                for (ConnectionNotification event : events) {
+                  Consumer<ConnectionNotification> listener;
+                  m_connectionListenerLock.lock();
+                  try {
+                    listener = m_connectionListeners.get(event.listener);
+                  } finally {
+                    m_connectionListenerLock.unlock();
+                  }
+                  if (listener != null) {
+                    try {
+                      listener.accept(event);
+                    } catch (Throwable throwable) {
+                      System.err.println(
+                          "Unhandled exception during connection listener callback: "
+                              + throwable.toString());
+                      throwable.printStackTrace();
+                    }
+                  }
+                }
+              }
+              m_connectionListenerLock.lock();
+              try {
+                if (!wasInterrupted) {
+                  NetworkTablesJNI.destroyConnectionListenerPoller(m_connectionListenerPoller);
+                }
+                m_connectionListenerPoller = 0;
+              } finally {
+                m_connectionListenerLock.unlock();
+              }
+            },
+            "NTConnectionListener");
+    connectionListenerThread.setDaemon(true);
+    connectionListenerThread.start();
   }
 
   /**
@@ -450,16 +443,16 @@
    * @param immediateNotify Notify listener of all existing connections
    * @return Listener handle
    */
-  public int addConnectionListener(Consumer<ConnectionNotification> listener,
-                                   boolean immediateNotify) {
+  public int addConnectionListener(
+      Consumer<ConnectionNotification> listener, boolean immediateNotify) {
     m_connectionListenerLock.lock();
     try {
       if (m_connectionListenerPoller == 0) {
         m_connectionListenerPoller = NetworkTablesJNI.createConnectionListenerPoller(m_handle);
         startConnectionListenerThread();
       }
-      int handle = NetworkTablesJNI.addPolledConnectionListener(m_connectionListenerPoller,
-          immediateNotify);
+      int handle =
+          NetworkTablesJNI.addPolledConnectionListener(m_connectionListenerPoller, immediateNotify);
       m_connectionListeners.put(handle, listener);
       return handle;
     } finally {
@@ -483,13 +476,12 @@
   }
 
   /**
-   * Wait for the connection listener queue to be empty.  This is primarily useful
-   * for deterministic testing.  This blocks until either the connection listener
-   * queue is empty (e.g. there are no more events that need to be passed along
-   * to callbacks or poll queues) or the timeout expires.
+   * Wait for the connection listener queue to be empty. This is primarily useful for deterministic
+   * testing. This blocks until either the connection listener queue is empty (e.g. there are no
+   * more events that need to be passed along to callbacks or poll queues) or the timeout expires.
    *
-   * @param timeout   timeout, in seconds.  Set to 0 for non-blocking behavior,
-   *                  or a negative value to block indefinitely
+   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
+   *     block indefinitely
    * @return False if timed out, otherwise true.
    */
   public boolean waitForConnectionListenerQueue(double timeout) {
@@ -506,8 +498,8 @@
             if (timeout < 0) {
               m_connectionListenerWaitQueueCond.await();
             } else {
-              return m_connectionListenerWaitQueueCond.await((long) (timeout * 1e9),
-                  TimeUnit.NANOSECONDS);
+              return m_connectionListenerWaitQueueCond.await(
+                  (long) (timeout * 1e9), TimeUnit.NANOSECONDS);
             }
           } catch (InterruptedException ex) {
             Thread.currentThread().interrupt();
@@ -527,78 +519,81 @@
 
   private final ReentrantLock m_rpcCallLock = new ReentrantLock();
   private final Map<Integer, EntryConsumer<RpcAnswer>> m_rpcCalls = new HashMap<>();
-  private Thread m_rpcCallThread;
   private int m_rpcCallPoller;
   private boolean m_rpcCallWaitQueue;
   private final Condition m_rpcCallWaitQueueCond = m_rpcCallLock.newCondition();
 
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
   private void startRpcCallThread() {
-    m_rpcCallThread = new Thread(() -> {
-      boolean wasInterrupted = false;
-      while (!Thread.interrupted()) {
-        RpcAnswer[] events;
-        try {
-          events = NetworkTablesJNI.pollRpc(this, m_rpcCallPoller);
-        } catch (InterruptedException ex) {
-          m_rpcCallLock.lock();
-          try {
-            if (m_rpcCallWaitQueue) {
-              m_rpcCallWaitQueue = false;
-              m_rpcCallWaitQueueCond.signalAll();
-              continue;
-            }
-          } finally {
-            m_rpcCallLock.unlock();
-          }
-          Thread.currentThread().interrupt();
-          // don't try to destroy poller, as its handle is likely no longer valid
-          wasInterrupted = true;
-          break;
-        }
-        for (RpcAnswer event : events) {
-          EntryConsumer<RpcAnswer> listener;
-          m_rpcCallLock.lock();
-          try {
-            listener = m_rpcCalls.get(event.entry);
-          } finally {
-            m_rpcCallLock.unlock();
-          }
-          if (listener != null) {
-            event.m_entryObject = listener.m_entry;
-            try {
-              listener.m_consumer.accept(event);
-            } catch (Throwable throwable) {
-              System.err.println("Unhandled exception during RPC callback: "
-                  + throwable.toString());
-              throwable.printStackTrace();
-            }
-            event.finish();
-          }
-        }
-      }
-      m_rpcCallLock.lock();
-      try {
-        if (!wasInterrupted) {
-          NetworkTablesJNI.destroyRpcCallPoller(m_rpcCallPoller);
-        }
-        m_rpcCallPoller = 0;
-      } finally {
-        m_rpcCallLock.unlock();
-      }
-    }, "NTRpcCall");
-    m_rpcCallThread.setDaemon(true);
-    m_rpcCallThread.start();
+    var rpcCallThread =
+        new Thread(
+            () -> {
+              boolean wasInterrupted = false;
+              while (!Thread.interrupted()) {
+                RpcAnswer[] events;
+                try {
+                  events = NetworkTablesJNI.pollRpc(this, m_rpcCallPoller);
+                } catch (InterruptedException ex) {
+                  m_rpcCallLock.lock();
+                  try {
+                    if (m_rpcCallWaitQueue) {
+                      m_rpcCallWaitQueue = false;
+                      m_rpcCallWaitQueueCond.signalAll();
+                      continue;
+                    }
+                  } finally {
+                    m_rpcCallLock.unlock();
+                  }
+                  Thread.currentThread().interrupt();
+                  // don't try to destroy poller, as its handle is likely no longer valid
+                  wasInterrupted = true;
+                  break;
+                }
+                for (RpcAnswer event : events) {
+                  EntryConsumer<RpcAnswer> listener;
+                  m_rpcCallLock.lock();
+                  try {
+                    listener = m_rpcCalls.get(event.entry);
+                  } finally {
+                    m_rpcCallLock.unlock();
+                  }
+                  if (listener != null) {
+                    event.m_entryObject = listener.m_entry;
+                    try {
+                      listener.m_consumer.accept(event);
+                    } catch (Throwable throwable) {
+                      System.err.println(
+                          "Unhandled exception during RPC callback: " + throwable.toString());
+                      throwable.printStackTrace();
+                    }
+                    event.finish();
+                  }
+                }
+              }
+              m_rpcCallLock.lock();
+              try {
+                if (!wasInterrupted) {
+                  NetworkTablesJNI.destroyRpcCallPoller(m_rpcCallPoller);
+                }
+                m_rpcCallPoller = 0;
+              } finally {
+                m_rpcCallLock.unlock();
+              }
+            },
+            "NTRpcCall");
+    rpcCallThread.setDaemon(true);
+    rpcCallThread.start();
   }
 
   private static final byte[] rev0def = new byte[] {0};
 
   /**
-   * Create a callback-based RPC entry point.  Only valid to use on the server.
-   * The callback function will be called when the RPC is called.
-   * This function creates RPC version 0 definitions (raw data in and out).
+   * Create a callback-based RPC entry point. Only valid to use on the server. The callback function
+   * will be called when the RPC is called. This function creates RPC version 0 definitions (raw
+   * data in and out).
    *
-   * @param entry     the entry
-   * @param callback  callback function
+   * @param entry the entry
+   * @param callback callback function
    */
   public void createRpc(NetworkTableEntry entry, Consumer<RpcAnswer> callback) {
     m_rpcCallLock.lock();
@@ -615,13 +610,12 @@
   }
 
   /**
-   * Wait for the incoming RPC call queue to be empty.  This is primarily useful
-   * for deterministic testing.  This blocks until either the RPC call
-   * queue is empty (e.g. there are no more events that need to be passed along
-   * to callbacks or poll queues) or the timeout expires.
+   * Wait for the incoming RPC call queue to be empty. This is primarily useful for deterministic
+   * testing. This blocks until either the RPC call queue is empty (e.g. there are no more events
+   * that need to be passed along to callbacks or poll queues) or the timeout expires.
    *
-   * @param timeout   timeout, in seconds.  Set to 0 for non-blocking behavior,
-   *                  or a negative value to block indefinitely
+   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
+   *     block indefinitely
    * @return False if timed out, otherwise true.
    */
   public boolean waitForRpcCallQueue(double timeout) {
@@ -657,11 +651,10 @@
    */
 
   /**
-   * Set the network identity of this node.
-   * This is the name used during the initial connection handshake, and is
-   * visible through ConnectionInfo on the remote node.
+   * Set the network identity of this node. This is the name used during the initial connection
+   * handshake, and is visible through ConnectionInfo on the remote node.
    *
-   * @param name      identity to advertise
+   * @param name identity to advertise
    */
   public void setNetworkIdentity(String name) {
     NetworkTablesJNI.setNetworkIdentity(m_handle, name);
@@ -677,47 +670,44 @@
   }
 
   /**
-   * Starts local-only operation.  Prevents calls to startServer or startClient
-   * from taking effect.  Has no effect if startServer or startClient
-   * has already been called.
+   * Starts local-only operation. Prevents calls to startServer or startClient from taking effect.
+   * Has no effect if startServer or startClient has already been called.
    */
   public void startLocal() {
     NetworkTablesJNI.startLocal(m_handle);
   }
 
   /**
-   * Stops local-only operation.  startServer or startClient can be called after
-   * this call to start a server or client.
+   * Stops local-only operation. startServer or startClient can be called after this call to start a
+   * server or client.
    */
   public void stopLocal() {
     NetworkTablesJNI.stopLocal(m_handle);
   }
 
   /**
-   * Starts a server using the networktables.ini as the persistent file,
-   * using the default listening address and port.
+   * Starts a server using the networktables.ini as the persistent file, using the default listening
+   * address and port.
    */
   public void startServer() {
     startServer("networktables.ini");
   }
 
   /**
-   * Starts a server using the specified persistent filename, using the default
-   * listening address and port.
+   * Starts a server using the specified persistent filename, using the default listening address
+   * and port.
    *
-   * @param persistFilename  the name of the persist file to use
+   * @param persistFilename the name of the persist file to use
    */
   public void startServer(String persistFilename) {
     startServer(persistFilename, "");
   }
 
   /**
-   * Starts a server using the specified filename and listening address,
-   * using the default port.
+   * Starts a server using the specified filename and listening address, using the default port.
    *
-   * @param persistFilename  the name of the persist file to use
-   * @param listenAddress    the address to listen on, or empty to listen on any
-   *                         address
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
    */
   public void startServer(String persistFilename, String listenAddress) {
     startServer(persistFilename, listenAddress, kDefaultPort);
@@ -726,25 +716,20 @@
   /**
    * Starts a server using the specified filename, listening address, and port.
    *
-   * @param persistFilename  the name of the persist file to use
-   * @param listenAddress    the address to listen on, or empty to listen on any
-   *                         address
-   * @param port             port to communicate over
+   * @param persistFilename the name of the persist file to use
+   * @param listenAddress the address to listen on, or empty to listen on any address
+   * @param port port to communicate over
    */
   public void startServer(String persistFilename, String listenAddress, int port) {
     NetworkTablesJNI.startServer(m_handle, persistFilename, listenAddress, port);
   }
 
-  /**
-   * Stops the server if it is running.
-   */
+  /** Stops the server if it is running. */
   public void stopServer() {
     NetworkTablesJNI.stopServer(m_handle);
   }
 
-  /**
-   * Starts a client.  Use SetServer to set the server name and port.
-   */
+  /** Starts a client. Use SetServer to set the server name and port. */
   public void startClient() {
     NetworkTablesJNI.startClient(m_handle);
   }
@@ -752,7 +737,7 @@
   /**
    * Starts a client using the specified server and the default port.
    *
-   * @param serverName  server name
+   * @param serverName server name
    */
   public void startClient(String serverName) {
     startClient(serverName, kDefaultPort);
@@ -761,29 +746,29 @@
   /**
    * Starts a client using the specified server and port.
    *
-   * @param serverName  server name
-   * @param port        port to communicate over
+   * @param serverName server name
+   * @param port port to communicate over
    */
   public void startClient(String serverName, int port) {
     NetworkTablesJNI.startClient(m_handle, serverName, port);
   }
 
   /**
-   * Starts a client using the specified servers and default port.  The
-   * client will attempt to connect to each server in round robin fashion.
+   * Starts a client using the specified servers and default port. The client will attempt to
+   * connect to each server in round robin fashion.
    *
-   * @param serverNames   array of server names
+   * @param serverNames array of server names
    */
   public void startClient(String[] serverNames) {
     startClient(serverNames, kDefaultPort);
   }
 
   /**
-   * Starts a client using the specified servers and port number.  The
-   * client will attempt to connect to each server in round robin fashion.
+   * Starts a client using the specified servers and port number. The client will attempt to connect
+   * to each server in round robin fashion.
    *
-   * @param serverNames   array of server names
-   * @param port          port to communicate over
+   * @param serverNames array of server names
+   * @param port port to communicate over
    */
   public void startClient(String[] serverNames, int port) {
     int[] ports = new int[serverNames.length];
@@ -794,49 +779,46 @@
   }
 
   /**
-   * Starts a client using the specified (server, port) combinations.  The
-   * client will attempt to connect to each server in round robin fashion.
+   * Starts a client using the specified (server, port) combinations. The client will attempt to
+   * connect to each server in round robin fashion.
    *
-   * @param serverNames   array of server names
-   * @param ports         array of port numbers
+   * @param serverNames array of server names
+   * @param ports array of port numbers
    */
   public void startClient(String[] serverNames, int[] ports) {
     NetworkTablesJNI.startClient(m_handle, serverNames, ports);
   }
 
   /**
-   * Starts a client using commonly known robot addresses for the specified
-   * team using the default port number.
+   * Starts a client using commonly known robot addresses for the specified team using the default
+   * port number.
    *
-   * @param team        team number
+   * @param team team number
    */
   public void startClientTeam(int team) {
     startClientTeam(team, kDefaultPort);
   }
 
   /**
-   * Starts a client using commonly known robot addresses for the specified
-   * team.
+   * Starts a client using commonly known robot addresses for the specified team.
    *
-   * @param team        team number
-   * @param port        port to communicate over
+   * @param team team number
+   * @param port port to communicate over
    */
   public void startClientTeam(int team, int port) {
     NetworkTablesJNI.startClientTeam(m_handle, team, port);
   }
 
-  /**
-   * Stops the client if it is running.
-   */
+  /** Stops the client if it is running. */
   public void stopClient() {
     NetworkTablesJNI.stopClient(m_handle);
   }
 
   /**
-   * Sets server address and port for client (without restarting client).
-   * Changes the port to the default port.
+   * Sets server address and port for client (without restarting client). Changes the port to the
+   * default port.
    *
-   * @param serverName  server name
+   * @param serverName server name
    */
   public void setServer(String serverName) {
     setServer(serverName, kDefaultPort);
@@ -845,30 +827,29 @@
   /**
    * Sets server address and port for client (without restarting client).
    *
-   * @param serverName  server name
-   * @param port        port to communicate over
+   * @param serverName server name
+   * @param port port to communicate over
    */
   public void setServer(String serverName, int port) {
     NetworkTablesJNI.setServer(m_handle, serverName, port);
   }
 
   /**
-   * Sets server addresses and port for client (without restarting client).
-   * Changes the port to the default port.  The client will attempt to connect
-   * to each server in round robin fashion.
+   * Sets server addresses and port for client (without restarting client). Changes the port to the
+   * default port. The client will attempt to connect to each server in round robin fashion.
    *
-   * @param serverNames   array of server names
+   * @param serverNames array of server names
    */
   public void setServer(String[] serverNames) {
     setServer(serverNames, kDefaultPort);
   }
 
   /**
-   * Sets server addresses and port for client (without restarting client).
-   * The client will attempt to connect to each server in round robin fashion.
+   * Sets server addresses and port for client (without restarting client). The client will attempt
+   * to connect to each server in round robin fashion.
    *
-   * @param serverNames   array of server names
-   * @param port          port to communicate over
+   * @param serverNames array of server names
+   * @param port port to communicate over
    */
   public void setServer(String[] serverNames, int port) {
     int[] ports = new int[serverNames.length];
@@ -879,51 +860,48 @@
   }
 
   /**
-   * Sets server addresses and ports for client (without restarting client).
-   * The client will attempt to connect to each server in round robin fashion.
+   * Sets server addresses and ports for client (without restarting client). The client will attempt
+   * to connect to each server in round robin fashion.
    *
-   * @param serverNames   array of server names
-   * @param ports         array of port numbers
+   * @param serverNames array of server names
+   * @param ports array of port numbers
    */
   public void setServer(String[] serverNames, int[] ports) {
     NetworkTablesJNI.setServer(m_handle, serverNames, ports);
   }
 
   /**
-   * Sets server addresses and port for client (without restarting client).
-   * Changes the port to the default port.  The client will attempt to connect
-   * to each server in round robin fashion.
+   * Sets server addresses and port for client (without restarting client). Changes the port to the
+   * default port. The client will attempt to connect to each server in round robin fashion.
    *
-   * @param team        team number
+   * @param team team number
    */
   public void setServerTeam(int team) {
     setServerTeam(team, kDefaultPort);
   }
 
   /**
-   * Sets server addresses and port for client (without restarting client).
-   * Connects using commonly known robot addresses for the specified team.
+   * Sets server addresses and port for client (without restarting client). Connects using commonly
+   * known robot addresses for the specified team.
    *
-   * @param team        team number
-   * @param port        port to communicate over
+   * @param team team number
+   * @param port port to communicate over
    */
   public void setServerTeam(int team, int port) {
     NetworkTablesJNI.setServerTeam(m_handle, team, port);
   }
 
   /**
-   * Starts requesting server address from Driver Station.
-   * This connects to the Driver Station running on localhost to obtain the
-   * server IP address, and connects with the default port.
+   * Starts requesting server address from Driver Station. This connects to the Driver Station
+   * running on localhost to obtain the server IP address, and connects with the default port.
    */
   public void startDSClient() {
     startDSClient(kDefaultPort);
   }
 
   /**
-   * Starts requesting server address from Driver Station.
-   * This connects to the Driver Station running on localhost to obtain the
-   * server IP address.
+   * Starts requesting server address from Driver Station. This connects to the Driver Station
+   * running on localhost to obtain the server IP address.
    *
    * @param port server port to use in combination with IP from DS
    */
@@ -931,16 +909,14 @@
     NetworkTablesJNI.startDSClient(m_handle, port);
   }
 
-  /**
-   * Stops requesting server address from Driver Station.
-   */
+  /** Stops requesting server address from Driver Station. */
   public void stopDSClient() {
     NetworkTablesJNI.stopDSClient(m_handle);
   }
 
   /**
-   * Set the periodic update rate.
-   * Sets how frequently updates are sent to other nodes over the network.
+   * Set the periodic update rate. Sets how frequently updates are sent to other nodes over the
+   * network.
    *
    * @param interval update interval in seconds (range 0.01 to 1.0)
    */
@@ -949,18 +925,17 @@
   }
 
   /**
-   * Flushes all updated values immediately to the network.
-   * Note: This is rate-limited to protect the network from flooding.
-   * This is primarily useful for synchronizing network updates with
-   * user code.
+   * Flushes all updated values immediately to the network. Note: This is rate-limited to protect
+   * the network from flooding. This is primarily useful for synchronizing network updates with user
+   * code.
    */
   public void flush() {
     NetworkTablesJNI.flush(m_handle);
   }
 
   /**
-   * Gets information on the currently established network connections.
-   * If operating as a client, this will return either zero or one values.
+   * Gets information on the currently established network connections. If operating as a client,
+   * this will return either zero or one values.
    *
    * @return array of connection information
    */
@@ -978,7 +953,7 @@
   }
 
   /**
-   * Saves persistent keys to a file.  The server does this automatically.
+   * Saves persistent keys to a file. The server does this automatically.
    *
    * @param filename file name
    * @throws PersistentException if error saving file
@@ -988,7 +963,7 @@
   }
 
   /**
-   * Loads persistent keys from a file.  The server does this automatically.
+   * Loads persistent keys from a file. The server does this automatically.
    *
    * @param filename file name
    * @return List of warnings (errors result in an exception instead)
@@ -999,11 +974,10 @@
   }
 
   /**
-   * Save table values to a file.  The file format used is identical to
-   * that used for SavePersistent.
+   * Save table values to a file. The file format used is identical to that used for SavePersistent.
    *
-   * @param filename  filename
-   * @param prefix    save only keys starting with this prefix
+   * @param filename filename
+   * @param prefix save only keys starting with this prefix
    * @throws PersistentException if error saving file
    */
   public void saveEntries(String filename, String prefix) throws PersistentException {
@@ -1011,11 +985,11 @@
   }
 
   /**
-   * Load table values from a file.  The file format used is identical to
-   * that used for SavePersistent / LoadPersistent.
+   * Load table values from a file. The file format used is identical to that used for
+   * SavePersistent / LoadPersistent.
    *
-   * @param filename  filename
-   * @param prefix    load only keys starting with this prefix
+   * @param filename filename
+   * @param prefix load only keys starting with this prefix
    * @return List of warnings (errors result in an exception instead)
    * @throws PersistentException if error saving file
    */
@@ -1025,67 +999,69 @@
 
   private final ReentrantLock m_loggerLock = new ReentrantLock();
   private final Map<Integer, Consumer<LogMessage>> m_loggers = new HashMap<>();
-  private Thread m_loggerThread;
   private int m_loggerPoller;
   private boolean m_loggerWaitQueue;
   private final Condition m_loggerWaitQueueCond = m_loggerLock.newCondition();
 
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
   private void startLogThread() {
-    m_loggerThread = new Thread(() -> {
-      boolean wasInterrupted = false;
-      while (!Thread.interrupted()) {
-        LogMessage[] events;
-        try {
-          events = NetworkTablesJNI.pollLogger(this, m_loggerPoller);
-        } catch (InterruptedException ex) {
-          Thread.currentThread().interrupt();
-          // don't try to destroy poller, as its handle is likely no longer valid
-          wasInterrupted = true;
-          break;
-        }
-        for (LogMessage event : events) {
-          Consumer<LogMessage> logger;
-          m_loggerLock.lock();
-          try {
-            logger = m_loggers.get(event.logger);
-          } finally {
-            m_loggerLock.unlock();
-          }
-          if (logger != null) {
-            try {
-              logger.accept(event);
-            } catch (Throwable throwable) {
-              System.err.println("Unhandled exception during logger callback: "
-                  + throwable.toString());
-              throwable.printStackTrace();
-            }
-          }
-        }
-      }
-      m_loggerLock.lock();
-      try {
-        if (!wasInterrupted) {
-          NetworkTablesJNI.destroyLoggerPoller(m_loggerPoller);
-        }
-        m_rpcCallPoller = 0;
-      } finally {
-        m_loggerLock.unlock();
-      }
-    }, "NTLogger");
-    m_loggerThread.setDaemon(true);
-    m_loggerThread.start();
+    var loggerThread =
+        new Thread(
+            () -> {
+              boolean wasInterrupted = false;
+              while (!Thread.interrupted()) {
+                LogMessage[] events;
+                try {
+                  events = NetworkTablesJNI.pollLogger(this, m_loggerPoller);
+                } catch (InterruptedException ex) {
+                  Thread.currentThread().interrupt();
+                  // don't try to destroy poller, as its handle is likely no longer valid
+                  wasInterrupted = true;
+                  break;
+                }
+                for (LogMessage event : events) {
+                  Consumer<LogMessage> logger;
+                  m_loggerLock.lock();
+                  try {
+                    logger = m_loggers.get(event.logger);
+                  } finally {
+                    m_loggerLock.unlock();
+                  }
+                  if (logger != null) {
+                    try {
+                      logger.accept(event);
+                    } catch (Throwable throwable) {
+                      System.err.println(
+                          "Unhandled exception during logger callback: " + throwable.toString());
+                      throwable.printStackTrace();
+                    }
+                  }
+                }
+              }
+              m_loggerLock.lock();
+              try {
+                if (!wasInterrupted) {
+                  NetworkTablesJNI.destroyLoggerPoller(m_loggerPoller);
+                }
+                m_rpcCallPoller = 0;
+              } finally {
+                m_loggerLock.unlock();
+              }
+            },
+            "NTLogger");
+    loggerThread.setDaemon(true);
+    loggerThread.start();
   }
 
   /**
-   * Add logger callback function.  By default, log messages are sent to stderr;
-   * this function sends log messages with the specified levels to the provided
-   * callback function instead.  The callback function will only be called for
-   * log messages with level greater than or equal to minLevel and less than or
-   * equal to maxLevel; messages outside this range will be silently ignored.
+   * Add logger callback function. By default, log messages are sent to stderr; this function sends
+   * log messages with the specified levels to the provided callback function instead. The callback
+   * function will only be called for log messages with level greater than or equal to minLevel and
+   * less than or equal to maxLevel; messages outside this range will be silently ignored.
    *
-   * @param func        log callback function
-   * @param minLevel    minimum log level
-   * @param maxLevel    maximum log level
+   * @param func log callback function
+   * @param minLevel minimum log level
+   * @param maxLevel maximum log level
    * @return Logger handle
    */
   public int addLogger(Consumer<LogMessage> func, int minLevel, int maxLevel) {
@@ -1119,13 +1095,12 @@
   }
 
   /**
-   * Wait for the incoming log event queue to be empty.  This is primarily useful
-   * for deterministic testing.  This blocks until either the log event
-   * queue is empty (e.g. there are no more events that need to be passed along
-   * to callbacks or poll queues) or the timeout expires.
+   * Wait for the incoming log event queue to be empty. This is primarily useful for deterministic
+   * testing. This blocks until either the log event queue is empty (e.g. there are no more events
+   * that need to be passed along to callbacks or poll queues) or the timeout expires.
    *
-   * @param timeout   timeout, in seconds.  Set to 0 for non-blocking behavior,
-   *                  or a negative value to block indefinitely
+   * @param timeout timeout, in seconds. Set to 0 for non-blocking behavior, or a negative value to
+   *     block indefinitely
    * @return False if timed out, otherwise true.
    */
   public boolean waitForLoggerQueue(double timeout) {
@@ -1174,5 +1149,5 @@
   }
 
   private boolean m_owned;
-  private int m_handle;
+  private final int m_handle;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java
index 54a9f55..a173bb2 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableType.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * Network table data types.
- */
+/** Network table data types. */
 public enum NetworkTableType {
   kUnassigned(0),
   kBoolean(0x01),
@@ -21,7 +16,6 @@
   kStringArray(0x40),
   kRpc(0x80);
 
-  @SuppressWarnings("MemberName")
   private final int value;
 
   NetworkTableType(int value) {
@@ -40,15 +34,24 @@
    */
   public static NetworkTableType getFromInt(int value) {
     switch (value) {
-      case 0x01: return kBoolean;
-      case 0x02: return kDouble;
-      case 0x04: return kString;
-      case 0x08: return kRaw;
-      case 0x10: return kBooleanArray;
-      case 0x20: return kDoubleArray;
-      case 0x40: return kStringArray;
-      case 0x80: return kRpc;
-      default: return kUnassigned;
+      case 0x01:
+        return kBoolean;
+      case 0x02:
+        return kDouble;
+      case 0x04:
+        return kString;
+      case 0x08:
+        return kRaw;
+      case 0x10:
+        return kBooleanArray;
+      case 0x20:
+        return kDoubleArray;
+      case 0x40:
+        return kStringArray;
+      case 0x80:
+        return kRpc;
+      default:
+        return kUnassigned;
     }
   }
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableValue.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableValue.java
index eb7e2c2..f4b9290 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableValue.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTableValue.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
 import java.util.Objects;
 
-/**
- * A network table entry value.
- */
+/** A network table entry value. */
 public final class NetworkTableValue {
   NetworkTableValue(NetworkTableType type, Object value, long time) {
     m_type = type;
@@ -188,6 +183,7 @@
    * @return The raw value.
    * @throws ClassCastException if the entry value is not of raw type.
    */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public byte[] getRaw() {
     if (m_type != NetworkTableType.kRaw) {
       throw new ClassCastException("cannot convert " + m_type + " to raw");
@@ -201,6 +197,7 @@
    * @return The rpc definition value.
    * @throws ClassCastException if the entry value is not of rpc definition type.
    */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public byte[] getRpc() {
     if (m_type != NetworkTableType.kRpc) {
       throw new ClassCastException("cannot convert " + m_type + " to rpc");
@@ -214,6 +211,7 @@
    * @return The boolean array value.
    * @throws ClassCastException if the entry value is not of boolean array type.
    */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public boolean[] getBooleanArray() {
     if (m_type != NetworkTableType.kBooleanArray) {
       throw new ClassCastException("cannot convert " + m_type + " to boolean array");
@@ -227,6 +225,7 @@
    * @return The double array value.
    * @throws ClassCastException if the entry value is not of double array type.
    */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public double[] getDoubleArray() {
     if (m_type != NetworkTableType.kDoubleArray) {
       throw new ClassCastException("cannot convert " + m_type + " to double array");
@@ -240,6 +239,7 @@
    * @return The string array value.
    * @throws ClassCastException if the entry value is not of string array type.
    */
+  @SuppressWarnings("PMD.MethodReturnsInternalArray")
   public String[] getStringArray() {
     if (m_type != NetworkTableType.kStringArray) {
       throw new ClassCastException("cannot convert " + m_type + " to string array");
@@ -478,6 +478,9 @@
     return Objects.hash(m_type, m_value);
   }
 
+  // arraycopy() doesn't know how to unwrap boxed values; this is a false positive in PMD
+  // (see https://sourceforge.net/p/pmd/bugs/804/)
+  @SuppressWarnings("PMD.AvoidArrayLoops")
   static boolean[] toNative(Boolean[] arr) {
     boolean[] out = new boolean[arr.length];
     for (int i = 0; i < arr.length; i++) {
@@ -486,6 +489,7 @@
     return out;
   }
 
+  @SuppressWarnings("PMD.AvoidArrayLoops")
   static double[] toNative(Number[] arr) {
     double[] out = new double[arr.length];
     for (int i = 0; i < arr.length; i++) {
@@ -494,6 +498,7 @@
     return out;
   }
 
+  @SuppressWarnings("PMD.AvoidArrayLoops")
   static Boolean[] fromNative(boolean[] arr) {
     Boolean[] out = new Boolean[arr.length];
     for (int i = 0; i < arr.length; i++) {
@@ -502,6 +507,7 @@
     return out;
   }
 
+  @SuppressWarnings("PMD.AvoidArrayLoops")
   static Double[] fromNative(double[] arr) {
     Double[] out = new Double[arr.length];
     for (int i = 0; i < arr.length; i++) {
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
index 742c0ca..8c4916f 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/NetworkTablesJNI.java
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
+import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import java.nio.ByteBuffer;
 import java.util.concurrent.atomic.AtomicBoolean;
 
-import edu.wpi.first.wpiutil.RuntimeLoader;
-
 public final class NetworkTablesJNI {
   static boolean libraryLoaded = false;
   static RuntimeLoader<NetworkTablesJNI> loader = null;
@@ -32,7 +28,9 @@
   static {
     if (Helper.getExtractOnStaticLoad()) {
       try {
-        loader = new RuntimeLoader<>("ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
+        loader =
+            new RuntimeLoader<>(
+                "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
         loader.loadLibrary();
       } catch (IOException ex) {
         ex.printStackTrace();
@@ -44,56 +42,88 @@
 
   /**
    * Force load the library.
+   *
+   * @throws IOException if the library fails to load
    */
   public static synchronized void forceLoad() throws IOException {
     if (libraryLoaded) {
       return;
     }
-    loader = new RuntimeLoader<>("ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
+    loader =
+        new RuntimeLoader<>(
+            "ntcorejni", RuntimeLoader.getDefaultExtractionRoot(), NetworkTablesJNI.class);
     loader.loadLibrary();
     libraryLoaded = true;
   }
 
   public static native int getDefaultInstance();
+
   public static native int createInstance();
+
   public static native void destroyInstance(int inst);
+
   public static native int getInstanceFromHandle(int handle);
 
   public static native int getEntry(int inst, String key);
+
   public static native int[] getEntries(int inst, String prefix, int types);
+
   public static native String getEntryName(int entry);
+
   public static native long getEntryLastChange(int entry);
 
   public static native int getType(int entry);
 
   public static native boolean setBoolean(int entry, long time, boolean value, boolean force);
+
   public static native boolean setDouble(int entry, long time, double value, boolean force);
+
   public static native boolean setString(int entry, long time, String value, boolean force);
+
   public static native boolean setRaw(int entry, long time, byte[] value, boolean force);
-  public static native boolean setRaw(int entry, long time, ByteBuffer value, int len, boolean force);
-  public static native boolean setBooleanArray(int entry, long time, boolean[] value, boolean force);
+
+  public static native boolean setRaw(
+      int entry, long time, ByteBuffer value, int len, boolean force);
+
+  public static native boolean setBooleanArray(
+      int entry, long time, boolean[] value, boolean force);
+
   public static native boolean setDoubleArray(int entry, long time, double[] value, boolean force);
+
   public static native boolean setStringArray(int entry, long time, String[] value, boolean force);
 
   public static native NetworkTableValue getValue(int entry);
 
   public static native boolean getBoolean(int entry, boolean defaultValue);
+
   public static native double getDouble(int entry, double defaultValue);
+
   public static native String getString(int entry, String defaultValue);
+
   public static native byte[] getRaw(int entry, byte[] defaultValue);
+
   public static native boolean[] getBooleanArray(int entry, boolean[] defaultValue);
+
   public static native double[] getDoubleArray(int entry, double[] defaultValue);
+
   public static native String[] getStringArray(int entry, String[] defaultValue);
+
   public static native boolean setDefaultBoolean(int entry, long time, boolean defaultValue);
 
   public static native boolean setDefaultDouble(int entry, long time, double defaultValue);
+
   public static native boolean setDefaultString(int entry, long time, String defaultValue);
+
   public static native boolean setDefaultRaw(int entry, long time, byte[] defaultValue);
+
   public static native boolean setDefaultBooleanArray(int entry, long time, boolean[] defaultValue);
+
   public static native boolean setDefaultDoubleArray(int entry, long time, double[] defaultValue);
+
   public static native boolean setDefaultStringArray(int entry, long time, String[] defaultValue);
 
   public static native void setEntryFlags(int entry, int flags);
+
   public static native int getEntryFlags(int entry);
 
   public static native void deleteEntry(int entry);
@@ -101,58 +131,109 @@
   public static native void deleteAllEntries(int inst);
 
   public static native EntryInfo getEntryInfoHandle(NetworkTableInstance inst, int entry);
-  public static native EntryInfo[] getEntryInfo(NetworkTableInstance instObject, int inst, String prefix, int types);
+
+  public static native EntryInfo[] getEntryInfo(
+      NetworkTableInstance instObject, int inst, String prefix, int types);
 
   public static native int createEntryListenerPoller(int inst);
+
   public static native void destroyEntryListenerPoller(int poller);
+
   public static native int addPolledEntryListener(int poller, String prefix, int flags);
+
   public static native int addPolledEntryListener(int poller, int entry, int flags);
-  public static native EntryNotification[] pollEntryListener(NetworkTableInstance inst, int poller) throws InterruptedException;
-  public static native EntryNotification[] pollEntryListenerTimeout(NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
+  public static native EntryNotification[] pollEntryListener(NetworkTableInstance inst, int poller)
+      throws InterruptedException;
+
+  public static native EntryNotification[] pollEntryListenerTimeout(
+      NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
   public static native void cancelPollEntryListener(int poller);
+
   public static native void removeEntryListener(int entryListener);
+
   public static native boolean waitForEntryListenerQueue(int inst, double timeout);
 
   public static native int createConnectionListenerPoller(int inst);
+
   public static native void destroyConnectionListenerPoller(int poller);
+
   public static native int addPolledConnectionListener(int poller, boolean immediateNotify);
-  public static native ConnectionNotification[] pollConnectionListener(NetworkTableInstance inst, int poller) throws InterruptedException;
-  public static native ConnectionNotification[] pollConnectionListenerTimeout(NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
+  public static native ConnectionNotification[] pollConnectionListener(
+      NetworkTableInstance inst, int poller) throws InterruptedException;
+
+  public static native ConnectionNotification[] pollConnectionListenerTimeout(
+      NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
   public static native void cancelPollConnectionListener(int poller);
+
   public static native void removeConnectionListener(int connListener);
+
   public static native boolean waitForConnectionListenerQueue(int inst, double timeout);
 
   public static native int createRpcCallPoller(int inst);
+
   public static native void destroyRpcCallPoller(int poller);
+
   public static native void createPolledRpc(int entry, byte[] def, int poller);
-  public static native RpcAnswer[] pollRpc(NetworkTableInstance inst, int poller) throws InterruptedException;
-  public static native RpcAnswer[] pollRpcTimeout(NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
+  public static native RpcAnswer[] pollRpc(NetworkTableInstance inst, int poller)
+      throws InterruptedException;
+
+  public static native RpcAnswer[] pollRpcTimeout(
+      NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
   public static native void cancelPollRpc(int poller);
+
   public static native boolean waitForRpcCallQueue(int inst, double timeout);
+
   public static native boolean postRpcResponse(int entry, int call, byte[] result);
+
   public static native int callRpc(int entry, byte[] params);
+
   public static native byte[] getRpcResult(int entry, int call);
+
   public static native byte[] getRpcResult(int entry, int call, double timeout);
+
   public static native void cancelRpcResult(int entry, int call);
 
   public static native byte[] getRpc(int entry, byte[] defaultValue);
 
   public static native void setNetworkIdentity(int inst, String name);
+
   public static native int getNetworkMode(int inst);
+
   public static native void startLocal(int inst);
+
   public static native void stopLocal(int inst);
-  public static native void startServer(int inst, String persistFilename, String listenAddress, int port);
+
+  public static native void startServer(
+      int inst, String persistFilename, String listenAddress, int port);
+
   public static native void stopServer(int inst);
+
   public static native void startClient(int inst);
+
   public static native void startClient(int inst, String serverName, int port);
+
   public static native void startClient(int inst, String[] serverNames, int[] ports);
+
   public static native void startClientTeam(int inst, int team, int port);
+
   public static native void stopClient(int inst);
+
   public static native void setServer(int inst, String serverName, int port);
+
   public static native void setServer(int inst, String[] serverNames, int[] ports);
+
   public static native void setServerTeam(int inst, int team, int port);
+
   public static native void startDSClient(int inst, int port);
+
   public static native void stopDSClient(int inst);
+
   public static native void setUpdateRate(int inst, double interval);
 
   public static native void flush(int inst);
@@ -162,19 +243,33 @@
   public static native boolean isConnected(int inst);
 
   public static native void savePersistent(int inst, String filename) throws PersistentException;
-  public static native String[] loadPersistent(int inst, String filename) throws PersistentException;  // returns warnings
 
-  public static native void saveEntries(int inst, String filename, String prefix) throws PersistentException;
-  public static native String[] loadEntries(int inst, String filename, String prefix) throws PersistentException;  // returns warnings
+  public static native String[] loadPersistent(int inst, String filename)
+      throws PersistentException; // returns warnings
+
+  public static native void saveEntries(int inst, String filename, String prefix)
+      throws PersistentException;
+
+  public static native String[] loadEntries(int inst, String filename, String prefix)
+      throws PersistentException; // returns warnings
 
   public static native long now();
 
   public static native int createLoggerPoller(int inst);
+
   public static native void destroyLoggerPoller(int poller);
+
   public static native int addPolledLogger(int poller, int minLevel, int maxLevel);
-  public static native LogMessage[] pollLogger(NetworkTableInstance inst, int poller) throws InterruptedException;
-  public static native LogMessage[] pollLoggerTimeout(NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
+  public static native LogMessage[] pollLogger(NetworkTableInstance inst, int poller)
+      throws InterruptedException;
+
+  public static native LogMessage[] pollLoggerTimeout(
+      NetworkTableInstance inst, int poller, double timeout) throws InterruptedException;
+
   public static native void cancelPollLogger(int poller);
+
   public static native void removeLogger(int logger);
+
   public static native boolean waitForLoggerQueue(int inst, double timeout);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/PersistentException.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/PersistentException.java
index 205b015..1c61595 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/PersistentException.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/PersistentException.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
 import java.io.IOException;
 
-/**
- * An exception thrown when persistent load/save fails in a {@link NetworkTable}.
- */
+/** An exception thrown when persistent load/save fails in a {@link NetworkTable}. */
 public final class PersistentException extends IOException {
   public static final long serialVersionUID = 0;
 
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcAnswer.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcAnswer.java
index 91e1aa4..a244bde 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcAnswer.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcAnswer.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables Remote Procedure Call (Server Side).
- */
+/** NetworkTables Remote Procedure Call (Server Side). */
 public final class RpcAnswer {
   /** Entry handle. */
   @SuppressWarnings("MemberName")
@@ -31,8 +26,8 @@
   @SuppressWarnings("MemberName")
   public final ConnectionInfo conn;
 
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param inst Instance
    * @param entry Entry handle
@@ -41,8 +36,14 @@
    * @param params Call raw parameters
    * @param conn Connection info
    */
-  public RpcAnswer(NetworkTableInstance inst, int entry, int call, String name, byte[] params,
-                   ConnectionInfo conn) {
+  @SuppressWarnings("PMD.ArrayIsStoredDirectly")
+  public RpcAnswer(
+      NetworkTableInstance inst,
+      int entry,
+      int call,
+      String name,
+      byte[] params,
+      ConnectionInfo conn) {
     this.m_inst = inst;
     this.entry = entry;
     this.call = call;
@@ -76,8 +77,8 @@
   /**
    * Post RPC response (return value) for a polled RPC.
    *
-   * @param result  result raw data that will be provided to remote caller
-   * @return        true if the response was posted, otherwise false
+   * @param result result raw data that will be provided to remote caller
+   * @return true if the response was posted, otherwise false
    */
   public boolean postResponse(byte[] result) {
     boolean ret = NetworkTablesJNI.postRpcResponse(entry, call, result);
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcCall.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcCall.java
index b9148c9..790ff5f 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcCall.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/RpcCall.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * NetworkTables Remote Procedure Call.
- */
+/** NetworkTables Remote Procedure Call. */
 public final class RpcCall implements AutoCloseable {
-  /** Constructor.
-   * This should generally only be used internally to NetworkTables.
+  /**
+   * Constructor. This should generally only be used internally to NetworkTables.
    *
    * @param entry Entry
    * @param call Call handle
@@ -22,9 +17,7 @@
     m_call = call;
   }
 
-  /**
-   * Cancels the result if no other action taken.
-   */
+  /** Cancels the result if no other action taken. */
   @Override
   public synchronized void close() {
     if (m_call != 0) {
@@ -60,8 +53,7 @@
   }
 
   /**
-   * Get the result (return value).  This function blocks until
-   * the result is received.
+   * Get the result (return value). This function blocks until the result is received.
    *
    * @return Received result (output)
    */
@@ -74,10 +66,10 @@
   }
 
   /**
-   * Get the result (return value).  This function blocks until
-   * the result is received or it times out.
+   * Get the result (return value). This function blocks until the result is received or it times
+   * out.
    *
-   * @param timeout     timeout, in seconds
+   * @param timeout timeout, in seconds
    * @return Received result (output)
    */
   public byte[] getResult(double timeout) {
@@ -88,9 +80,7 @@
     return result;
   }
 
-  /**
-   * Ignore the result.  This function is non-blocking.
-   */
+  /** Ignore the result. This function is non-blocking. */
   public void cancelResult() {
     NetworkTablesJNI.cancelRpcResult(m_entry.getHandle(), m_call);
   }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableEntryListener.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableEntryListener.java
index 676e57e..e0781e1 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableEntryListener.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableEntryListener.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * A listener that listens to changes in values in a {@link NetworkTable}.
- */
+/** A listener that listens to changes in values in a {@link NetworkTable}. */
 @FunctionalInterface
 public interface TableEntryListener extends EntryListenerFlags {
   /**
@@ -19,9 +14,9 @@
    * @param key the key associated with the value that changed
    * @param entry the entry associated with the value that changed
    * @param value the new value
-   * @param flags update flags; for example, EntryListenerFlags.kNew if the key
-   *     did not previously exist in the table
+   * @param flags update flags; for example, EntryListenerFlags.kNew if the key did not previously
+   *     exist in the table
    */
-  void valueChanged(NetworkTable table, String key, NetworkTableEntry entry,
-                    NetworkTableValue value, int flags);
+  void valueChanged(
+      NetworkTable table, String key, NetworkTableEntry entry, NetworkTableValue value, int flags);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableListener.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableListener.java
index 3c686e5..24fae2d 100644
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableListener.java
+++ b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/networktables/TableListener.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-/**
- * A listener that listens to new tables in a {@link NetworkTable}.
- */
+/** A listener that listens to new tables in a {@link NetworkTable}. */
 @FunctionalInterface
 public interface TableListener {
   /**
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/networktables/NetworkTable.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/networktables/NetworkTable.java
deleted file mode 100644
index f360a19..0000000
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/networktables/NetworkTable.java
+++ /dev/null
@@ -1,1189 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.networktables;
-
-import java.nio.ByteBuffer;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.HashSet;
-import java.util.List;
-import java.util.Objects;
-import java.util.Set;
-import java.util.concurrent.ConcurrentHashMap;
-import java.util.concurrent.ConcurrentMap;
-import java.util.function.Consumer;
-
-import edu.wpi.first.networktables.ConnectionInfo;
-import edu.wpi.first.networktables.ConnectionNotification;
-import edu.wpi.first.networktables.EntryInfo;
-import edu.wpi.first.networktables.EntryNotification;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.networktables.NetworkTableType;
-import edu.wpi.first.networktables.NetworkTableValue;
-import edu.wpi.first.networktables.NetworkTablesJNI;
-import edu.wpi.first.networktables.PersistentException;
-import edu.wpi.first.wpilibj.tables.IRemote;
-import edu.wpi.first.wpilibj.tables.IRemoteConnectionListener;
-import edu.wpi.first.wpilibj.tables.ITable;
-import edu.wpi.first.wpilibj.tables.ITableListener;
-
-/**
- * A network table that knows its subtable path.
- * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable} instead.
- */
-@Deprecated
-@SuppressWarnings("checkstyle:all")
-public class NetworkTable implements ITable, IRemote {
-  /**
-   * The path separator for sub-tables and keys
-   *
-   */
-  public static final char PATH_SEPARATOR = '/';
-  /**
-   * The default port that network tables operates on
-   */
-  public static final int DEFAULT_PORT = 1735;
-
-  private static boolean client = false;
-  private static boolean enableDS = true;
-  private static boolean running = false;
-  private static int port = DEFAULT_PORT;
-  private static String persistentFilename = "networktables.ini";
-
-  private synchronized static void checkInit() {
-    if (running)
-      throw new IllegalStateException(
-          "Network tables has already been initialized");
-  }
-
-  /**
-   * initializes network tables
-   * @deprecated Use {@link NetworkTableInstance#startServer()} or
-   * {@link NetworkTableInstance#startClient()} instead.
-   */
-  @Deprecated
-  public synchronized static void initialize() {
-    if (running)
-      shutdown();
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
-    if (client) {
-      inst.startClient();
-      if (enableDS)
-        inst.startDSClient(port);
-    } else
-      inst.startServer(persistentFilename, "", port);
-    running = true;
-  }
-
-  /**
-   * shuts down network tables
-   * @deprecated Use {@link NetworkTableInstance#stopServer()} or
-   * {@link NetworkTableInstance#stopClient()} instead.
-   */
-  @Deprecated
-  public synchronized static void shutdown() {
-    if (!running)
-      return;
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
-    if (client) {
-      inst.stopDSClient();
-      inst.stopClient();
-    } else
-      inst.stopServer();
-    running = false;
-  }
-
-  /**
-   * set that network tables should be a server
-   * This must be called before initialize or getTable
-   * @deprecated Use {@link NetworkTableInstance#startServer()} instead.
-   */
-  @Deprecated
-  public synchronized static void setServerMode() {
-    if (!client)
-      return;
-    checkInit();
-    client = false;
-  }
-
-  /**
-   * set that network tables should be a client
-   * This must be called before initialize or getTable
-   * @deprecated Use {@link NetworkTableInstance#startClient()} instead.
-   */
-  @Deprecated
-  public synchronized static void setClientMode() {
-    if (client)
-      return;
-    checkInit();
-    client = true;
-  }
-
-  /**
-   * set the team the robot is configured for (this will set the mdns address that
-   * network tables will connect to in client mode)
-   * This must be called before initialize or getTable
-   * @param team the team number
-   * @deprecated Use {@link NetworkTableInstance#setServerTeam(int)} or
-   * {@link NetworkTableInstance#startClientTeam(int)} instead.
-   */
-  @Deprecated
-  public synchronized static void setTeam(int team) {
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
-    inst.setServerTeam(team, port);
-    if (enableDS)
-      inst.startDSClient(port);
-  }
-
-  /**
-   * @param address the address that network tables will connect to in client
-   * mode
-   * @deprecated Use {@link NetworkTableInstance#setServer(String)} or
-   * {@link NetworkTableInstance#startClient(String)} instead.
-   */
-  @Deprecated
-  public synchronized static void setIPAddress(final String address) {
-    String[] addresses = new String[1];
-    addresses[0] = address;
-    setIPAddress(addresses);
-  }
-
-  /**
-   * @param addresses the adresses that network tables will connect to in
-   * client mode (in round robin order)
-   * @deprecated Use {@link NetworkTableInstance#setServer(String[])} or
-   * {@link NetworkTableInstance#startClient(String[])} instead.
-   */
-  @Deprecated
-  public synchronized static void setIPAddress(final String[] addresses) {
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
-    inst.setServer(addresses, port);
-
-    // Stop the DS client if we're explicitly connecting to localhost
-    if (addresses.length > 0 &&
-        (addresses[0].equals("localhost") || addresses[0].equals("127.0.0.1")))
-      inst.stopDSClient();
-    else if (enableDS)
-      inst.startDSClient(port);
-  }
-
-  /**
-   * Set the port number that network tables will connect to in client
-   * mode or listen to in server mode.
-   * @param aport the port number
-   * @deprecated Use the appropriate parameters to
-   * {@link NetworkTableInstance#setServer(String, int)},
-   * {@link NetworkTableInstance#startClient(String, int)},
-   * {@link NetworkTableInstance#startServer(String, String, int)}, and
-   * {@link NetworkTableInstance#startDSClient(int)} instead.
-   */
-  @Deprecated
-  public synchronized static void setPort(int aport) {
-    if (port == aport)
-      return;
-    checkInit();
-    port = aport;
-  }
-
-  /**
-   * Enable requesting the server address from the Driver Station.
-   * @param enabled whether to enable the connection to the local DS
-   * @deprecated Use {@link NetworkTableInstance#startDSClient()} and
-   * {@link NetworkTableInstance#stopDSClient()} instead.
-   */
-  @Deprecated
-  public synchronized static void setDSClientEnabled(boolean enabled) {
-    NetworkTableInstance inst = NetworkTableInstance.getDefault();
-    enableDS = enabled;
-    if (enableDS)
-      inst.startDSClient(port);
-    else
-      inst.stopDSClient();
-  }
-
-  /**
-   * Sets the persistent filename.
-   * @param filename the filename that the network tables server uses for
-   * automatic loading and saving of persistent values
-   * @deprecated Use the appropriate parameter to
-   * {@link NetworkTableInstance#startServer()} instead.
-   */
-  @Deprecated
-  public synchronized static void setPersistentFilename(final String filename) {
-    if (persistentFilename.equals(filename))
-      return;
-    checkInit();
-    persistentFilename = filename;
-  }
-
-  /**
-   * Sets the network identity.
-   * This is provided in the connection info on the remote end.
-   * @param name identity
-   * @deprecated Use {@link NetworkTableInstance#setNetworkIdentity(String)}
-   * instead.
-   */
-  @Deprecated
-  public static void setNetworkIdentity(String name) {
-    NetworkTableInstance.getDefault().setNetworkIdentity(name);
-  }
-
-  public static boolean[] toNative(Boolean[] arr) {
-    boolean[] out = new boolean[arr.length];
-    for (int i = 0; i < arr.length; i++)
-      out[i] = arr[i];
-    return out;
-  }
-
-  public static double[] toNative(Number[] arr) {
-    double[] out = new double[arr.length];
-    for (int i = 0; i < arr.length; i++)
-      out[i] = arr[i].doubleValue();
-    return out;
-  }
-
-  public static Boolean[] fromNative(boolean[] arr) {
-    Boolean[] out = new Boolean[arr.length];
-    for (int i = 0; i < arr.length; i++)
-      out[i] = arr[i];
-    return out;
-  }
-
-  public static Double[] fromNative(double[] arr) {
-    Double[] out = new Double[arr.length];
-    for (int i = 0; i < arr.length; i++)
-      out[i] = arr[i];
-    return out;
-  }
-
-  /**
-   * Gets the table with the specified key. If the table does not exist, a new
-   * table will be created.<br>
-   * This will automatically initialize network tables if it has not been
-   * already
-   *
-   * @deprecated Use {@link NetworkTableInstance#getTable(String)} instead.
-   *
-   * @param key   the key name
-   * @return the network table requested
-   */
-  @Deprecated
-  public synchronized static NetworkTable getTable(String key) {
-    if (!running)
-      initialize();
-    String theKey;
-    if (key.isEmpty() || key.equals("/")) {
-      theKey = "";
-    } else if (key.charAt(0) == NetworkTable.PATH_SEPARATOR) {
-      theKey = key;
-    } else {
-      theKey = NetworkTable.PATH_SEPARATOR + key;
-    }
-    return new NetworkTable(NetworkTableInstance.getDefault(), theKey);
-  }
-
-  private final String path;
-  private final String pathWithSep;
-  private final NetworkTableInstance inst;
-
-  NetworkTable(NetworkTableInstance inst, String path) {
-    this.path = path;
-    this.pathWithSep = path + PATH_SEPARATOR;
-    this.inst = inst;
-  }
-
-  @Override
-  public String toString() { return "NetworkTable: " + path; }
-
-  private final ConcurrentMap<String, NetworkTableEntry> entries = new ConcurrentHashMap<String, NetworkTableEntry>();
-
-  /**
-   * Gets the entry for a subkey.
-   * @param key the key name
-   * @return Network table entry.
-   */
-  private NetworkTableEntry getEntry(String key) {
-    NetworkTableEntry entry = entries.get(key);
-    if (entry == null) {
-      entry = inst.getEntry(pathWithSep + key);
-      entries.putIfAbsent(key, entry);
-    }
-    return entry;
-  }
-
-  /**
-   * Gets the current network connections.
-   * @return An array of connection information.
-   * @deprecated Use {@link NetworkTableInstance#getConnections()} instead.
-   */
-  @Deprecated
-  public static ConnectionInfo[] connections() {
-    return NetworkTableInstance.getDefault().getConnections();
-  }
-
-  /**
-   * Determine whether or not a network connection is active.
-   * @return True if connected, false if not connected.
-   * @deprecated Use {@link NetworkTableInstance#isConnected()} instead.
-   */
-  @Override
-  @Deprecated
-  public boolean isConnected() {
-    return inst.isConnected();
-  }
-
-  /**
-   * Determine whether NetworkTables is operating as a server or as a client.
-   * @return True if operating as a server, false otherwise.
-   * @deprecated Use {@link NetworkTableInstance#getNetworkMode()} instead.
-   */
-  @Override
-  @Deprecated
-  public boolean isServer() {
-    return (inst.getNetworkMode() & NetworkTableInstance.kNetModeServer) != 0;
-  }
-
-  /* Backwards compatibility shims for IRemoteConnectionListener */
-  private static class ConnectionListenerAdapter implements Consumer<ConnectionNotification> {
-    public int uid;
-    private final IRemote targetSource;
-    private final IRemoteConnectionListener targetListener;
-
-    public ConnectionListenerAdapter(IRemote targetSource, IRemoteConnectionListener targetListener) {
-      this.targetSource = targetSource;
-      this.targetListener = targetListener;
-    }
-
-    @Override
-    public void accept(ConnectionNotification event) {
-      if (event.connected)
-        targetListener.connectedEx(targetSource, event.conn);
-      else
-        targetListener.disconnectedEx(targetSource, event.conn);
-    }
-  }
-
-  private static final HashMap<IRemoteConnectionListener,ConnectionListenerAdapter> globalConnectionListenerMap = new HashMap<IRemoteConnectionListener,ConnectionListenerAdapter>();
-
-  private static IRemote staticRemote = new IRemote() {
-    @Override
-    public void addConnectionListener(IRemoteConnectionListener listener, boolean immediateNotify) {
-      NetworkTable.addGlobalConnectionListener(listener, immediateNotify);
-    }
-    @Override
-    public void removeConnectionListener(IRemoteConnectionListener listener) {
-      NetworkTable.removeGlobalConnectionListener(listener);
-    }
-    @Override
-    public boolean isConnected() {
-      ConnectionInfo[] conns = NetworkTableInstance.getDefault().getConnections();
-      return conns.length > 0;
-    }
-    @Override
-    public boolean isServer() {
-      return (NetworkTableInstance.getDefault().getNetworkMode() & NetworkTableInstance.kNetModeServer) != 0;
-    }
-  };
-
-  private final HashMap<IRemoteConnectionListener,ConnectionListenerAdapter> connectionListenerMap = new HashMap<IRemoteConnectionListener,ConnectionListenerAdapter>();
-
-  /**
-   * Add a connection listener.
-   * @param listener connection listener
-   * @param immediateNotify call listener immediately for all existing connections
-   * @deprecated Use {@link NetworkTableInstance#addConnectionListener(Consumer, boolean)} instead.
-   */
-  @Deprecated
-  public static synchronized void addGlobalConnectionListener(IRemoteConnectionListener listener, boolean immediateNotify) {
-    ConnectionListenerAdapter adapter = new ConnectionListenerAdapter(staticRemote, listener);
-    if (globalConnectionListenerMap.putIfAbsent(listener, adapter) != null) {
-      throw new IllegalStateException("Cannot add the same listener twice");
-    }
-    adapter.uid = NetworkTableInstance.getDefault().addConnectionListener(adapter, immediateNotify);
-  }
-
-  /**
-   * Remove a connection listener.
-   * @param listener connection listener
-   * @deprecated Use {@link NetworkTableInstance#removeConnectionListener(int)} instead.
-   */
-  @Deprecated
-  public static synchronized void removeGlobalConnectionListener(IRemoteConnectionListener listener) {
-    ConnectionListenerAdapter adapter = globalConnectionListenerMap.remove(listener);
-    if (adapter != null) {
-      NetworkTableInstance.getDefault().removeConnectionListener(adapter.uid);
-    }
-  }
-
-  /**
-   * Add a connection listener.
-   * @param listener connection listener
-   * @param immediateNotify call listener immediately for all existing connections
-   * @deprecated Use {@link NetworkTableInstance#addConnectionListener(Consumer, boolean)} instead.
-   */
-  @Override
-  @Deprecated
-  public synchronized void addConnectionListener(IRemoteConnectionListener listener,
-                                                 boolean immediateNotify) {
-    ConnectionListenerAdapter adapter = new ConnectionListenerAdapter(this, listener);
-    if (connectionListenerMap.putIfAbsent(listener, adapter) != null) {
-      throw new IllegalStateException("Cannot add the same listener twice");
-    }
-    adapter.uid = inst.addConnectionListener(adapter, immediateNotify);
-  }
-
-  /**
-   * Remove a connection listener.
-   * @param listener connection listener
-   * @deprecated Use {@link NetworkTableInstance#removeConnectionListener(int)} instead.
-   */
-  @Override
-  @Deprecated
-  public synchronized void removeConnectionListener(IRemoteConnectionListener listener) {
-    ConnectionListenerAdapter adapter = connectionListenerMap.get(listener);
-    if (adapter != null && connectionListenerMap.remove(listener, adapter)) {
-      inst.removeConnectionListener(adapter.uid);
-    }
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addEntryListener(TableEntryListener, int)} instead
-   * (with flags value of NOTIFY_NEW | NOTIFY_UPDATE).
-   */
-  @Override
-  @Deprecated
-  public void addTableListener(ITableListener listener) {
-    addTableListenerEx(listener, NOTIFY_NEW | NOTIFY_UPDATE);
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addEntryListener(TableEntryListener, int)} instead
-   * (with flags value of NOTIFY_NEW | NOTIFY_UPDATE | NOTIFY_IMMEDIATE).
-   */
-  @Override
-  @Deprecated
-  public void addTableListener(ITableListener listener,
-                               boolean immediateNotify) {
-    int flags = NOTIFY_NEW | NOTIFY_UPDATE;
-    if (immediateNotify)
-      flags |= NOTIFY_IMMEDIATE;
-    addTableListenerEx(listener, flags);
-  }
-
-  /* Base class for listeners; stores uid to implement remove functions */
-  private static class ListenerBase {
-    public int uid;
-  }
-
-  private static class OldTableListenerAdapter extends ListenerBase implements Consumer<EntryNotification> {
-    private final int prefixLen;
-    private final ITable targetSource;
-    private final ITableListener targetListener;
-
-    public OldTableListenerAdapter(int prefixLen, ITable targetSource, ITableListener targetListener) {
-      this.prefixLen = prefixLen;
-      this.targetSource = targetSource;
-      this.targetListener = targetListener;
-    }
-
-    @Override
-    public void accept(EntryNotification event) {
-      String relativeKey = event.name.substring(prefixLen);
-      if (relativeKey.indexOf(PATH_SEPARATOR) != -1)
-        return;
-      targetListener.valueChangedEx(targetSource, relativeKey, event.value.getValue(), event.flags);
-    }
-  }
-
-  private final HashMap<ITableListener,List<ListenerBase>> oldListenerMap = new HashMap<ITableListener,List<ListenerBase>>();
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addEntryListener(TableEntryListener, int)} instead.
-   */
-  @Override
-  @Deprecated
-  public synchronized void addTableListenerEx(ITableListener listener,
-                                              int flags) {
-    List<ListenerBase> adapters = oldListenerMap.get(listener);
-    if (adapters == null) {
-      adapters = new ArrayList<ListenerBase>();
-      oldListenerMap.put(listener, adapters);
-    }
-    OldTableListenerAdapter adapter =
-        new OldTableListenerAdapter(path.length() + 1, this, listener);
-    adapter.uid = inst.addEntryListener(pathWithSep, adapter, flags);
-    adapters.add(adapter);
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addEntryListener(String, TableEntryListener, int)}
-   * or {@link NetworkTableEntry#addListener(Consumer, int)} instead.
-   */
-  @Override
-  @Deprecated
-  public void addTableListener(String key, ITableListener listener,
-                               boolean immediateNotify) {
-    int flags = NOTIFY_NEW | NOTIFY_UPDATE;
-    if (immediateNotify)
-      flags |= NOTIFY_IMMEDIATE;
-    addTableListenerEx(key, listener, flags);
-  }
-
-  private static class OldKeyListenerAdapter extends ListenerBase implements Consumer<EntryNotification> {
-    private final String relativeKey;
-    private final ITable targetSource;
-    private final ITableListener targetListener;
-
-    public OldKeyListenerAdapter(String relativeKey, ITable targetSource, ITableListener targetListener) {
-      this.relativeKey = relativeKey;
-      this.targetSource = targetSource;
-      this.targetListener = targetListener;
-    }
-
-    @Override
-    public void accept(EntryNotification event) {
-      targetListener.valueChangedEx(targetSource, relativeKey, event.value.getValue(), event.flags);
-    }
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addEntryListener(String, TableEntryListener, int)}
-   * or {@link NetworkTableEntry#addListener(Consumer, int)} instead.
-   */
-  @Override
-  @Deprecated
-  public synchronized void addTableListenerEx(String key,
-                                              ITableListener listener,
-                                              int flags) {
-    List<ListenerBase> adapters = oldListenerMap.get(listener);
-    if (adapters == null) {
-      adapters = new ArrayList<ListenerBase>();
-      oldListenerMap.put(listener, adapters);
-    }
-    OldKeyListenerAdapter adapter = new OldKeyListenerAdapter(key, this, listener);
-    adapter.uid = inst.addEntryListener(getEntry(key), adapter, flags);
-    adapters.add(adapter);
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addSubTableListener(TableListener, boolean)}
-   * instead.
-   */
-  @Override
-  @Deprecated
-  public void addSubTableListener(final ITableListener listener) {
-    addSubTableListener(listener, false);
-  }
-
-  private static class OldSubListenerAdapter extends ListenerBase implements Consumer<EntryNotification> {
-    private final int prefixLen;
-    private final ITable targetSource;
-    private final ITableListener targetListener;
-    private final Set<String> notifiedTables = new HashSet<String>();
-
-    public OldSubListenerAdapter(int prefixLen, ITable targetSource, ITableListener targetListener) {
-      this.prefixLen = prefixLen;
-      this.targetSource = targetSource;
-      this.targetListener = targetListener;
-    }
-
-    @Override
-    public void accept(EntryNotification event) {
-      String relativeKey = event.name.substring(prefixLen);
-      int endSubTable = relativeKey.indexOf(PATH_SEPARATOR);
-      if (endSubTable == -1)
-        return;
-      String subTableKey = relativeKey.substring(0, endSubTable);
-      if (notifiedTables.contains(subTableKey))
-        return;
-      notifiedTables.add(subTableKey);
-      targetListener.valueChangedEx(targetSource, subTableKey, targetSource.getSubTable(subTableKey), event.flags);
-    }
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#addSubTableListener(TableListener, boolean)}
-   * instead.
-   */
-  @Override
-  @Deprecated
-  public synchronized void addSubTableListener(final ITableListener listener,
-                                               boolean localNotify) {
-    List<ListenerBase> adapters = oldListenerMap.get(listener);
-    if (adapters == null) {
-      adapters = new ArrayList<ListenerBase>();
-      oldListenerMap.put(listener, adapters);
-    }
-    OldSubListenerAdapter adapter =
-        new OldSubListenerAdapter(path.length() + 1, this, listener);
-    int flags = NOTIFY_NEW | NOTIFY_IMMEDIATE;
-    if (localNotify)
-      flags |= NOTIFY_LOCAL;
-    adapter.uid = inst.addEntryListener(pathWithSep, adapter, flags);
-    adapters.add(adapter);
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable#removeTableListener(int)} instead.
-   */
-  @Override
-  @Deprecated
-  public synchronized void removeTableListener(ITableListener listener) {
-    List<ListenerBase> adapters = oldListenerMap.remove(listener);
-    if (adapters != null) {
-      for (ListenerBase adapter : adapters)
-        inst.removeEntryListener(adapter.uid);
-    }
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public ITable getSubTable(String key) {
-    return new NetworkTable(inst, pathWithSep + key);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean containsKey(String key) {
-    return getEntry(key).exists();
-  }
-
-  @Override
-  public boolean containsSubTable(String key) {
-    int[] handles = NetworkTablesJNI.getEntries(inst.getHandle(), pathWithSep + key + PATH_SEPARATOR, 0);
-    return handles.length != 0;
-  }
-
-  /**
-   * @param types bitmask of types; 0 is treated as a "don't care".
-   * @return keys currently in the table
-   */
-  @Override
-  public Set<String> getKeys(int types) {
-    Set<String> keys = new HashSet<String>();
-    int prefixLen = path.length() + 1;
-    for (EntryInfo info : inst.getEntryInfo(pathWithSep, types)) {
-      String relativeKey = info.name.substring(prefixLen);
-      if (relativeKey.indexOf(PATH_SEPARATOR) != -1)
-        continue;
-      keys.add(relativeKey);
-      // populate entries as we go
-      if (entries.get(relativeKey) == null) {
-        entries.putIfAbsent(relativeKey, new NetworkTableEntry(inst, info.entry));
-      }
-    }
-    return keys;
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public Set<String> getKeys() {
-    return getKeys(0);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public Set<String> getSubTables() {
-    Set<String> keys = new HashSet<String>();
-    int prefixLen = path.length() + 1;
-    for (EntryInfo info : inst.getEntryInfo(pathWithSep, 0)) {
-      String relativeKey = info.name.substring(prefixLen);
-      int endSubTable = relativeKey.indexOf(PATH_SEPARATOR);
-      if (endSubTable == -1)
-        continue;
-      keys.add(relativeKey.substring(0, endSubTable));
-    }
-    return keys;
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putNumber(String key, double value) {
-    return getEntry(key).setNumber(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultNumber(String key, double defaultValue) {
-    return getEntry(key).setDefaultDouble(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public double getNumber(String key, double defaultValue) {
-    return getEntry(key).getDouble(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putString(String key, String value) {
-    return getEntry(key).setString(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultString(String key, String defaultValue) {
-    return getEntry(key).setDefaultString(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public String getString(String key, String defaultValue) {
-    return getEntry(key).getString(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putBoolean(String key, boolean value) {
-    return getEntry(key).setBoolean(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultBoolean(String key, boolean defaultValue) {
-    return getEntry(key).setDefaultBoolean(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean getBoolean(String key, boolean defaultValue) {
-    return getEntry(key).getBoolean(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putBooleanArray(String key, boolean[] value) {
-    return getEntry(key).setBooleanArray(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putBooleanArray(String key, Boolean[] value) {
-    return getEntry(key).setBooleanArray(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultBooleanArray(String key, boolean[] defaultValue) {
-    return getEntry(key).setDefaultBooleanArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultBooleanArray(String key, Boolean[] defaultValue) {
-    return getEntry(key).setDefaultBooleanArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean[] getBooleanArray(String key, boolean[] defaultValue) {
-    return getEntry(key).getBooleanArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public Boolean[] getBooleanArray(String key, Boolean[] defaultValue) {
-    return getEntry(key).getBooleanArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putNumberArray(String key, double[] value) {
-    return getEntry(key).setDoubleArray(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putNumberArray(String key, Double[] value) {
-    return getEntry(key).setNumberArray(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultNumberArray(String key, double[] defaultValue) {
-    return getEntry(key).setDefaultDoubleArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultNumberArray(String key, Double[] defaultValue) {
-    return getEntry(key).setDefaultNumberArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public double[] getNumberArray(String key, double[] defaultValue) {
-    return getEntry(key).getDoubleArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public Double[] getNumberArray(String key, Double[] defaultValue) {
-    return getEntry(key).getDoubleArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putStringArray(String key, String[] value) {
-    return getEntry(key).setStringArray(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultStringArray(String key, String[] defaultValue) {
-    return getEntry(key).setDefaultStringArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public String[] getStringArray(String key, String[] defaultValue) {
-    return getEntry(key).getStringArray(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putRaw(String key, byte[] value) {
-    return getEntry(key).setRaw(value);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean setDefaultRaw(String key, byte[] defaultValue) {
-    return getEntry(key).setDefaultRaw(defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean putRaw(String key, ByteBuffer value, int len) {
-    return getEntry(key).setRaw(value, len);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public byte[] getRaw(String key, byte[] defaultValue) {
-    return getEntry(key).getRaw(defaultValue);
-  }
-
-  /**
-   * Put a value in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putValue(String key, NetworkTableValue value) {
-    return getEntry(key).setValue(value);
-  }
-
-  /**
-   * Sets the current value in the table if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultValue(String key, NetworkTableValue defaultValue) {
-    return getEntry(key).setDefaultValue(defaultValue);
-  }
-
-  /**
-   * Gets the value associated with a key as a NetworkTableValue object.
-   * @param key the key of the value to look up
-   * @return the value associated with the given key
-   */
-  public NetworkTableValue getValue(String key) {
-    return getEntry(key).getValue();
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTableEntry#setValue(Object)}
-   * instead, e.g. `NetworkTable.getEntry(key).setValue(NetworkTableEntry.makeBoolean(false));`
-   * or `NetworkTable.getEntry(key).setValue(new Boolean(false));`
-   */
-  @Override
-  @Deprecated
-  public boolean putValue(String key, Object value) throws IllegalArgumentException {
-    if (value instanceof Boolean)
-      return putBoolean(key, ((Boolean)value).booleanValue());
-    else if (value instanceof Number)
-      return putDouble(key, ((Number)value).doubleValue());
-    else if (value instanceof String)
-      return putString(key, (String)value);
-    else if (value instanceof byte[])
-      return putRaw(key, (byte[])value);
-    else if (value instanceof boolean[])
-      return putBooleanArray(key, (boolean[])value);
-    else if (value instanceof double[])
-      return putNumberArray(key, (double[])value);
-    else if (value instanceof Boolean[])
-      return putBooleanArray(key, toNative((Boolean[])value));
-    else if (value instanceof Number[])
-      return putNumberArray(key, toNative((Number[])value));
-    else if (value instanceof String[])
-      return putStringArray(key, (String[])value);
-    else if (value instanceof NetworkTableValue)
-      return getEntry(key).setValue((NetworkTableValue)value);
-    else
-      throw new IllegalArgumentException("Value of type " + value.getClass().getName() + " cannot be put into a table");
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link edu.wpi.first.networktables.NetworkTableEntry#getValue()}
-   * instead, e.g. `NetworkTable.getEntry(key).getValue();`
-   */
-  @Override
-  @Deprecated
-  public Object getValue(String key, Object defaultValue) {
-    NetworkTableValue value = getValue(key);
-    if (value.getType() == NetworkTableType.kUnassigned) {
-      return defaultValue;
-    }
-    return value.getValue();
-  }
-
-  /** The persistent flag value. */
-  public static final int PERSISTENT = 1;
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public void setPersistent(String key) {
-    getEntry(key).setPersistent();
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public void clearPersistent(String key) {
-    getEntry(key).clearPersistent();
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public boolean isPersistent(String key) {
-    return getEntry(key).isPersistent();
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public void setFlags(String key, int flags) {
-    getEntry(key).setFlags(flags);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public void clearFlags(String key, int flags) {
-    getEntry(key).clearFlags(flags);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public int getFlags(String key) {
-    return getEntry(key).getFlags();
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public void delete(String key) {
-    getEntry(key).delete();
-  }
-
-  /**
-   * Deletes ALL keys in ALL subtables.  Use with caution!
-   * @deprecated Use {@link NetworkTableInstance#deleteAllEntries()} instead.
-   */
-  @Deprecated
-  public static void globalDeleteAll() {
-    NetworkTableInstance.getDefault().deleteAllEntries();
-  }
-
-  /**
-   * Flushes all updated values immediately to the network.
-   * Note: This is rate-limited to protect the network from flooding.
-   * This is primarily useful for synchronizing network updates with
-   * user code.
-   * @deprecated Use {@link NetworkTableInstance#flush()} instead.
-   */
-  @Deprecated
-  public static void flush() {
-    NetworkTableInstance.getDefault().flush();
-  }
-
-  /**
-   * Set the periodic update rate.
-   *
-   * @param interval update interval in seconds (range 0.01 to 1.0)
-   * @deprecated Use {@link NetworkTableInstance#setUpdateRate(double)}
-   * instead.
-   */
-  @Deprecated
-  public static void setUpdateRate(double interval) {
-    NetworkTableInstance.getDefault().setUpdateRate(interval);
-  }
-
-  /**
-   * Saves persistent keys to a file.  The server does this automatically.
-   *
-   * @param filename file name
-   * @throws PersistentException if error saving file
-   * @deprecated Use {@link NetworkTableInstance#savePersistent(String)}
-   * instead.
-   */
-  @Deprecated
-  public static void savePersistent(String filename) throws PersistentException {
-    NetworkTableInstance.getDefault().savePersistent(filename);
-  }
-
-  /**
-   * Loads persistent keys from a file.  The server does this automatically.
-   *
-   * @param filename file name
-   * @return List of warnings (errors result in an exception instead)
-   * @throws PersistentException if error reading file
-   * @deprecated Use {@link NetworkTableInstance#loadPersistent(String)}
-   * instead.
-   */
-  @Deprecated
-  public static String[] loadPersistent(String filename) throws PersistentException {
-    return NetworkTableInstance.getDefault().loadPersistent(filename);
-  }
-
-  /*
-   * Deprecated Methods
-   */
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link #putNumber(String, double)} instead.
-   */
-  @Override
-  @Deprecated
-  public boolean putDouble(String key, double value) {
-    return putNumber(key, value);
-  }
-
-  /**
-   * {@inheritDoc}
-   * @deprecated Use {@link #getNumber(String, double)} instead.
-   */
-  @Override
-  @Deprecated
-  public double getDouble(String key, double defaultValue) {
-    return getNumber(key, defaultValue);
-  }
-
-  /**
-   * {@inheritDoc}
-   */
-  @Override
-  public String getPath() {
-    return path;
-  }
-
-  @Override
-  public boolean equals(Object o) {
-    if (o == this) {
-      return true;
-    }
-    if (!(o instanceof NetworkTable)) {
-      return false;
-    }
-    NetworkTable other = (NetworkTable) o;
-    return inst.equals(other.inst) && path.equals(other.path);
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(inst, path);
-  }
-}
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/IRemote.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/IRemote.java
deleted file mode 100644
index 22b6f96..0000000
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/IRemote.java
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.tables;
-
-
-/**
- * Represents an object that has a remote connection.
- * @deprecated Use {@link edu.wpi.first.networktables.NetworkTableInstance}.
- */
-@Deprecated
-@SuppressWarnings("checkstyle:all")
-public interface IRemote {
-  /**
-   * Register an object to listen for connection and disconnection events
-   *
-   * @param listener the listener to be register
-   * @param immediateNotify if the listener object should be notified of the current connection state
-   */
-  public void addConnectionListener(IRemoteConnectionListener listener, boolean immediateNotify);
-
-  /**
-   * Unregister a listener from connection events
-   *
-   * @param listener the listener to be unregistered
-   */
-  public void removeConnectionListener(IRemoteConnectionListener listener);
-
-  /**
-   * Get the current state of the objects connection
-   * @return the current connection state
-   */
-    public boolean isConnected();
-
-  /**
-   * If the object is acting as a server
-   * @return if the object is a server
-   */
-    public boolean isServer();
-}
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/IRemoteConnectionListener.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/IRemoteConnectionListener.java
deleted file mode 100644
index a3ff118..0000000
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/IRemoteConnectionListener.java
+++ /dev/null
@@ -1,47 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.tables;
-
-import edu.wpi.first.networktables.ConnectionInfo;
-
-/**
- * A listener that listens for connection changes in a {@link IRemote} object.
- * @deprecated Use Consumer&lt;{@link edu.wpi.first.networktables.ConnectionNotification}&gt;.
- */
-@Deprecated
-@SuppressWarnings("checkstyle:all")
-public interface IRemoteConnectionListener {
-  /**
-   * Called when an IRemote is connected
-   * @param remote the object that connected
-   */
-  public void connected(IRemote remote);
-  /**
-   * Called when an IRemote is disconnected
-   * @param remote the object that disconnected
-   */
-  public void disconnected(IRemote remote);
-  /**
-   * Extended version of connected called when an IRemote is connected.
-    * Contains the connection info of the connected remote
-   * @param remote the object that connected
-   * @param info the connection info for the connected remote
-   */
-  default public void connectedEx(IRemote remote, ConnectionInfo info) {
-    connected(remote);
-  }
-  /**
-   * Extended version of connected called when an IRemote is disconnected.
-   * Contains the connection info of the disconnected remote
-   * @param remote the object that disconnected
-   * @param info the connection info for the disconnected remote
-   */
-  default public void disconnectedEx(IRemote remote, ConnectionInfo info) {
-    disconnected(remote);
-  }
-}
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/ITable.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/ITable.java
deleted file mode 100644
index 2d0d5d6..0000000
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/ITable.java
+++ /dev/null
@@ -1,488 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.tables;
-
-import java.nio.ByteBuffer;
-import java.util.Set;
-
-
-/**
- * A table whose values can be read and written to.
- * @deprecated Use {@link edu.wpi.first.networktables.NetworkTable}.
- */
-@Deprecated
-@SuppressWarnings("checkstyle:all")
-public interface ITable {
-
-  /**
-   * Checks the table and tells if it contains the specified key
-   *
-   * @param key the key to search for
-   * @return true if the table as a value assigned to the given key
-   */
-  public boolean containsKey(String key);
-
-  /**
-   * @param key the key to search for
-   * @return true if there is a subtable with the key which contains at least
-   * one key/subtable of its own
-   */
-  public boolean containsSubTable(String key);
-
-  /**
-   * Returns the table at the specified key. If there is no table at the
-   * specified key, it will create a new table
-   *
-   * @param key the name of the table relative to this one
-   * @return a sub table relative to this one
-   */
-  public ITable getSubTable(String key);
-
-  /**
-   * Gets all keys in the table (not including sub-tables).
-   * @param types bitmask of types; 0 is treated as a "don't care".
-   * @return keys currently in the table
-   */
-  public Set<String> getKeys(int types);
-
-  /**
-   * Gets all keys in the table (not including sub-tables).
-   * @return keys currently in the table
-   */
-  public Set<String> getKeys();
-
-  /**
-   * Gets the names of all subtables in the table.
-   * @return subtables currently in the table
-   */
-  public Set<String> getSubTables();
-
-  /**
-   * Makes a key's value persistent through program restarts.
-   * The key cannot be null.
-   *
-   * @param key the key name
-   */
-  public void setPersistent(String key);
-
-  /**
-   * Stop making a key's value persistent through program restarts.
-   * The key cannot be null.
-   *
-   * @param key the key name
-   */
-  public void clearPersistent(String key);
-
-  /**
-   * Returns whether the value is persistent through program restarts.
-   * The key cannot be null.
-   *
-   * @param key the key name
-   * @return True if the value is persistent.
-   */
-  public boolean isPersistent(String key);
-
-  /**
-   * Sets flags on the specified key in this table. The key can
-   * not be null.
-   *
-   * @param key the key name
-   * @param flags the flags to set (bitmask)
-   */
-  public void setFlags(String key, int flags);
-
-  /**
-   * Clears flags on the specified key in this table. The key can
-   * not be null.
-   *
-   * @param key the key name
-   * @param flags the flags to clear (bitmask)
-   */
-  public void clearFlags(String key, int flags);
-
-  /**
-   * Returns the flags for the specified key.
-   *
-   * @param key the key name
-   * @return the flags, or 0 if the key is not defined
-   */
-  public int getFlags(String key);
-
-  /**
-   * Deletes the specified key in this table. The key can
-   * not be null.
-   *
-   * @param key the key name
-   */
-  public void delete(String key);
-
-  /**
-   * Gets the value associated with a key as an object.
-   * NOTE: If the value is a double, it will return a Double object,
-   * not a primitive.  To get the primitive, use
-   * {@link #getDouble(String, double)}.
-   * @param key the key of the value to look up
-   * @param defaultValue the default value if the key is null
-   * @return the value associated with the given key
-   */
-  public Object getValue(String key, Object defaultValue);
-
-  /**
-   * Put a value in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   * @throws IllegalArgumentException when the value is not supported by the
-   * table
-   */
-  public boolean putValue(String key, Object value)
-      throws IllegalArgumentException;
-
-  /**
-   * Put a number in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putNumber(String key, double value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultNumber(String key, double defaultValue);
-
-  /**
-   * Returns the number the key maps to. If the key does not exist or is of
-   * different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public double getNumber(String key, double defaultValue);
-
-  /**
-   * Put a string in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putString(String key, String value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultString(String key, String defaultValue);
-
-  /**
-   * Returns the string the key maps to. If the key does not exist or is of
-   * different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public String getString(String key, String defaultValue);
-
-  /**
-   * Put a boolean in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putBoolean(String key, boolean value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultBoolean(String key, boolean defaultValue);
-
-  /**
-   * Returns the boolean the key maps to. If the key does not exist or is of
-   * different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public boolean getBoolean(String key, boolean defaultValue);
-
-  /**
-   * Put a boolean array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putBooleanArray(String key, boolean[] value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultBooleanArray(String key, boolean[] defaultValue);
-
-  /**
-   * Put a boolean array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putBooleanArray(String key, Boolean[] value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultBooleanArray(String key, Boolean[] defaultValue);
-
-  /**
-   * Returns the boolean array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public boolean[] getBooleanArray(String key, boolean[] defaultValue);
-  /**
-   * Returns the boolean array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public Boolean[] getBooleanArray(String key, Boolean[] defaultValue);
-
-  /**
-   * Put a number array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putNumberArray(String key, double[] value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultNumberArray(String key, double[] defaultValue);
-
-  /**
-   * Put a number array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putNumberArray(String key, Double[] value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultNumberArray(String key, Double[] defaultValue);
-
-  /**
-   * Returns the number array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public double[] getNumberArray(String key, double[] defaultValue);
-  /**
-   * Returns the number array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public Double[] getNumberArray(String key, Double[] defaultValue);
-
-  /**
-   * Put a string array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putStringArray(String key, String[] value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultStringArray(String key, String[] defaultValue);
-
-  /**
-   * Returns the string array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public String[] getStringArray(String key, String[] defaultValue);
-
-  /**
-   * Put a raw value (byte array) in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putRaw(String key, byte[] value);
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doens't exist.
-   * @return False if the table key exists with a different type
-   */
-  public boolean setDefaultRaw(String key, byte[] defaultValue);
-
-  /**
-   * Put a raw value (bytes from a byte buffer) in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @param len the length of the value
-   * @return False if the table key already exists with a different type
-   */
-  public boolean putRaw(String key, ByteBuffer value, int len);
-
-  /**
-   * Returns the raw value (byte array) the key maps to. If the key does not
-   * exist or is of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  public byte[] getRaw(String key, byte[] defaultValue);
-
-  /** Notifier flag values. */
-  public static final int NOTIFY_IMMEDIATE = 0x01;
-  public static final int NOTIFY_LOCAL = 0x02;
-  public static final int NOTIFY_NEW = 0x04;
-  public static final int NOTIFY_DELETE = 0x08;
-  public static final int NOTIFY_UPDATE = 0x10;
-  public static final int NOTIFY_FLAGS = 0x20;
-
-  /**
-   * Add a listener for changes to the table
-   * @param listener the listener to add
-   */
-  public void addTableListener(ITableListener listener);
-  /**
-   * Add a listener for changes to the table
-   * @param listener the listener to add
-   * @param immediateNotify if true then this listener will be notified of all
-   * current entries (marked as new)
-   */
-  public void addTableListener(ITableListener listener,
-                               boolean immediateNotify);
-  /**
-   * Add a listener for changes to the table
-   * @param listener the listener to add
-   * @param flags bitmask specifying desired notifications
-   */
-  public void addTableListenerEx(ITableListener listener, int flags);
-
-  /**
-   * Add a listener for changes to a specific key the table
-   * @param key the key to listen for
-   * @param listener the listener to add
-   * @param immediateNotify if true then this listener will be notified of all
-   * current entries (marked as new)
-   */
-  public void addTableListener(String key, ITableListener listener,
-                               boolean immediateNotify);
-  /**
-   * Add a listener for changes to a specific key the table
-   * @param key the key to listen for
-   * @param listener the listener to add
-   * @param flags bitmask specifying desired notifications
-   */
-  public void addTableListenerEx(String key, ITableListener listener,
-                                 int flags);
-  /**
-   * This will immediately notify the listener of all current sub tables
-   * @param listener the listener to notify
-   */
-  public void addSubTableListener(final ITableListener listener);
-  /**
-   * This will immediately notify the listener of all current sub tables
-   * @param listener the listener to notify
-   * @param localNotify if true then this listener will be notified of all
-   * local changes in addition to all remote changes
-   */
-  public void addSubTableListener(final ITableListener listener,
-                                  boolean localNotify);
-  /**
-   * Remove a listener from receiving table events
-   * @param listener the listener to be removed
-   */
-  public void removeTableListener(ITableListener listener);
-
-  /*
-   * Deprecated Methods
-   */
-
-  /**
-   * Maps the specified key to the specified value in this table.
-   * The key can not be null.
-   * The value can be retrieved by calling the get method with a key that is
-   * equal to the original key.
-   * @param key the key
-   * @param value the value
-   * @return False if the table key already exists with a different type
-   * @throws IllegalArgumentException if key is null
-   * @deprecated Use {@link #putNumber(String, double)} instead.
-   */
-  @Deprecated
-  public boolean putDouble(String key, double value);
-
-  /**
-   * Returns the value at the specified key.
-   * @param key the key
-   * @param defaultValue the value returned if the key is undefined
-   * @return the value
-   * @throws IllegalArgumentException if the value mapped to by the key is not a
-   * double
-   * @throws IllegalArgumentException if the key is null
-   * @deprecated Use {@link #getNumber(String, double)} instead.
-   */
-  @Deprecated
-  public double getDouble(String key, double defaultValue);
-
-  /**
-   * Gets the full path of this table.  Does not include the trailing "/".
-   * @return The path to this table (e.g. "", "/foo").
-   */
-  public String getPath();
-
-}
diff --git a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/ITableListener.java b/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/ITableListener.java
deleted file mode 100644
index b08312b..0000000
--- a/third_party/allwpilib/ntcore/src/main/java/edu/wpi/first/wpilibj/tables/ITableListener.java
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.tables;
-
-/**
- * A listener that listens to changes in values in a {@link ITable}.
- * @deprecated Use Consumer&lt;{@link edu.wpi.first.networktables.EntryNotification}&gt;,
- * {@link edu.wpi.first.networktables.TableEntryListener}, or
- * {@link edu.wpi.first.networktables.TableListener} as appropriate.
- */
-@FunctionalInterface
-@Deprecated
-@SuppressWarnings("checkstyle:all")
-public interface ITableListener {
-    /**
-     * Called when a key-value pair is changed in a {@link ITable}
-     * @param source the table the key-value pair exists in
-     * @param key the key associated with the value that changed
-     * @param value the new value
-     * @param isNew true if the key did not previously exist in the table, otherwise it is false
-     */
-    public void valueChanged(ITable source, String key, Object value, boolean isNew);
-
-    /**
-     * Extended version of valueChanged.  Called when a key-value pair is
-     * changed in a {@link ITable}.  The default implementation simply calls
-     * valueChanged().  If this is overridden, valueChanged() will not be
-     * called.
-     * @param source the table the key-value pair exists in
-     * @param key the key associated with the value that changed
-     * @param value the new value
-     * @param flags update flags; for example, NOTIFY_NEW if the key did not
-     * previously exist in the table
-     */
-    default public void valueChangedEx(ITable source, String key, Object value, int flags) {
-        // NOTIFY_NEW = 0x04
-        valueChanged(source, key, value, (flags & 0x04) != 0);
-    }
-}
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/CallbackManager.h b/third_party/allwpilib/ntcore/src/main/native/cpp/CallbackManager.h
deleted file mode 100644
index bf9b000..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/CallbackManager.h
+++ /dev/null
@@ -1,328 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef NTCORE_CALLBACKMANAGER_H_
-#define NTCORE_CALLBACKMANAGER_H_
-
-#include <atomic>
-#include <climits>
-#include <functional>
-#include <memory>
-#include <queue>
-#include <utility>
-#include <vector>
-
-#include <wpi/SafeThread.h>
-#include <wpi/UidVector.h>
-#include <wpi/condition_variable.h>
-#include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
-
-namespace nt {
-
-namespace impl {
-
-template <typename Callback>
-class ListenerData {
- public:
-  ListenerData() = default;
-  explicit ListenerData(Callback callback_) : callback(callback_) {}
-  explicit ListenerData(unsigned int poller_uid_) : poller_uid(poller_uid_) {}
-
-  explicit operator bool() const { return callback || poller_uid != UINT_MAX; }
-
-  Callback callback;
-  unsigned int poller_uid = UINT_MAX;
-};
-
-// CRTP callback manager thread
-// @tparam Derived        derived class
-// @tparam NotifierData   data buffered for each callback
-// @tparam ListenerData   data stored for each listener
-// Derived must define the following functions:
-//   bool Matches(const ListenerData& listener, const NotifierData& data);
-//   void SetListener(NotifierData* data, unsigned int listener_uid);
-//   void DoCallback(Callback callback, const NotifierData& data);
-template <typename Derived, typename TUserInfo,
-          typename TListenerData =
-              ListenerData<std::function<void(const TUserInfo& info)>>,
-          typename TNotifierData = TUserInfo>
-class CallbackThread : public wpi::SafeThread {
- public:
-  typedef TUserInfo UserInfo;
-  typedef TNotifierData NotifierData;
-  typedef TListenerData ListenerData;
-
-  ~CallbackThread() {
-    // Wake up any blocked pollers
-    for (size_t i = 0; i < m_pollers.size(); ++i) {
-      if (auto poller = m_pollers[i]) poller->Terminate();
-    }
-  }
-
-  void Main() override;
-
-  wpi::UidVector<ListenerData, 64> m_listeners;
-
-  std::queue<std::pair<unsigned int, NotifierData>> m_queue;
-  wpi::condition_variable m_queue_empty;
-
-  struct Poller {
-    void Terminate() {
-      {
-        std::scoped_lock lock(poll_mutex);
-        terminating = true;
-      }
-      poll_cond.notify_all();
-    }
-    std::queue<NotifierData> poll_queue;
-    wpi::mutex poll_mutex;
-    wpi::condition_variable poll_cond;
-    bool terminating = false;
-    bool canceling = false;
-  };
-  wpi::UidVector<std::shared_ptr<Poller>, 64> m_pollers;
-
-  // Must be called with m_mutex held
-  template <typename... Args>
-  void SendPoller(unsigned int poller_uid, Args&&... args) {
-    if (poller_uid > m_pollers.size()) return;
-    auto poller = m_pollers[poller_uid];
-    if (!poller) return;
-    {
-      std::scoped_lock lock(poller->poll_mutex);
-      poller->poll_queue.emplace(std::forward<Args>(args)...);
-    }
-    poller->poll_cond.notify_one();
-  }
-};
-
-template <typename Derived, typename TUserInfo, typename TListenerData,
-          typename TNotifierData>
-void CallbackThread<Derived, TUserInfo, TListenerData, TNotifierData>::Main() {
-  std::unique_lock lock(m_mutex);
-  while (m_active) {
-    while (m_queue.empty()) {
-      m_cond.wait(lock);
-      if (!m_active) return;
-    }
-
-    while (!m_queue.empty()) {
-      if (!m_active) return;
-      auto item = std::move(m_queue.front());
-
-      if (item.first != UINT_MAX) {
-        if (item.first < m_listeners.size()) {
-          auto& listener = m_listeners[item.first];
-          if (listener &&
-              static_cast<Derived*>(this)->Matches(listener, item.second)) {
-            static_cast<Derived*>(this)->SetListener(&item.second, item.first);
-            if (listener.callback) {
-              lock.unlock();
-              static_cast<Derived*>(this)->DoCallback(listener.callback,
-                                                      item.second);
-              lock.lock();
-            } else if (listener.poller_uid != UINT_MAX) {
-              SendPoller(listener.poller_uid, std::move(item.second));
-            }
-          }
-        }
-      } else {
-        // Use index because iterator might get invalidated.
-        for (size_t i = 0; i < m_listeners.size(); ++i) {
-          auto& listener = m_listeners[i];
-          if (!listener) continue;
-          if (!static_cast<Derived*>(this)->Matches(listener, item.second))
-            continue;
-          static_cast<Derived*>(this)->SetListener(&item.second,
-                                                   static_cast<unsigned>(i));
-          if (listener.callback) {
-            lock.unlock();
-            static_cast<Derived*>(this)->DoCallback(listener.callback,
-                                                    item.second);
-            lock.lock();
-          } else if (listener.poller_uid != UINT_MAX) {
-            SendPoller(listener.poller_uid, item.second);
-          }
-        }
-      }
-      m_queue.pop();
-    }
-
-    m_queue_empty.notify_all();
-  }
-}
-
-}  // namespace impl
-
-// CRTP callback manager
-// @tparam Derived  derived class
-// @tparam Thread   custom thread (must be derived from impl::CallbackThread)
-//
-// Derived must define the following functions:
-//   void Start();
-template <typename Derived, typename Thread>
-class CallbackManager {
-  friend class RpcServerTest;
-
- public:
-  void Stop() { m_owner.Stop(); }
-
-  void Remove(unsigned int listener_uid) {
-    auto thr = m_owner.GetThread();
-    if (!thr) return;
-    thr->m_listeners.erase(listener_uid);
-  }
-
-  unsigned int CreatePoller() {
-    static_cast<Derived*>(this)->Start();
-    auto thr = m_owner.GetThread();
-    return thr->m_pollers.emplace_back(
-        std::make_shared<typename Thread::Poller>());
-  }
-
-  void RemovePoller(unsigned int poller_uid) {
-    auto thr = m_owner.GetThread();
-    if (!thr) return;
-
-    // Remove any listeners that are associated with this poller
-    for (size_t i = 0; i < thr->m_listeners.size(); ++i) {
-      if (thr->m_listeners[i].poller_uid == poller_uid)
-        thr->m_listeners.erase(i);
-    }
-
-    // Wake up any blocked pollers
-    if (poller_uid >= thr->m_pollers.size()) return;
-    auto poller = thr->m_pollers[poller_uid];
-    if (!poller) return;
-    poller->Terminate();
-    return thr->m_pollers.erase(poller_uid);
-  }
-
-  bool WaitForQueue(double timeout) {
-    auto thr = m_owner.GetThread();
-    if (!thr) return true;
-
-    auto& lock = thr.GetLock();
-    auto timeout_time = std::chrono::steady_clock::now() +
-                        std::chrono::duration<double>(timeout);
-    while (!thr->m_queue.empty()) {
-      if (!thr->m_active) return true;
-      if (timeout == 0) return false;
-      if (timeout < 0) {
-        thr->m_queue_empty.wait(lock);
-      } else {
-        auto cond_timed_out = thr->m_queue_empty.wait_until(lock, timeout_time);
-        if (cond_timed_out == std::cv_status::timeout) return false;
-      }
-    }
-
-    return true;
-  }
-
-  std::vector<typename Thread::UserInfo> Poll(unsigned int poller_uid) {
-    bool timed_out = false;
-    return Poll(poller_uid, -1, &timed_out);
-  }
-
-  std::vector<typename Thread::UserInfo> Poll(unsigned int poller_uid,
-                                              double timeout, bool* timed_out) {
-    std::vector<typename Thread::UserInfo> infos;
-    std::shared_ptr<typename Thread::Poller> poller;
-    {
-      auto thr = m_owner.GetThread();
-      if (!thr) return infos;
-      if (poller_uid > thr->m_pollers.size()) return infos;
-      poller = thr->m_pollers[poller_uid];
-      if (!poller) return infos;
-    }
-
-    std::unique_lock lock(poller->poll_mutex);
-    auto timeout_time = std::chrono::steady_clock::now() +
-                        std::chrono::duration<double>(timeout);
-    *timed_out = false;
-    while (poller->poll_queue.empty()) {
-      if (poller->terminating) return infos;
-      if (poller->canceling) {
-        // Note: this only works if there's a single thread calling this
-        // function for any particular poller, but that's the intended use.
-        poller->canceling = false;
-        return infos;
-      }
-      if (timeout == 0) {
-        *timed_out = true;
-        return infos;
-      }
-      if (timeout < 0) {
-        poller->poll_cond.wait(lock);
-      } else {
-        auto cond_timed_out = poller->poll_cond.wait_until(lock, timeout_time);
-        if (cond_timed_out == std::cv_status::timeout) {
-          *timed_out = true;
-          return infos;
-        }
-      }
-    }
-
-    while (!poller->poll_queue.empty()) {
-      infos.emplace_back(std::move(poller->poll_queue.front()));
-      poller->poll_queue.pop();
-    }
-    return infos;
-  }
-
-  void CancelPoll(unsigned int poller_uid) {
-    std::shared_ptr<typename Thread::Poller> poller;
-    {
-      auto thr = m_owner.GetThread();
-      if (!thr) return;
-      if (poller_uid > thr->m_pollers.size()) return;
-      poller = thr->m_pollers[poller_uid];
-      if (!poller) return;
-    }
-
-    {
-      std::scoped_lock lock(poller->poll_mutex);
-      poller->canceling = true;
-    }
-    poller->poll_cond.notify_one();
-  }
-
- protected:
-  template <typename... Args>
-  void DoStart(Args&&... args) {
-    m_owner.Start(std::forward<Args>(args)...);
-  }
-
-  template <typename... Args>
-  unsigned int DoAdd(Args&&... args) {
-    static_cast<Derived*>(this)->Start();
-    auto thr = m_owner.GetThread();
-    return thr->m_listeners.emplace_back(std::forward<Args>(args)...);
-  }
-
-  template <typename... Args>
-  void Send(unsigned int only_listener, Args&&... args) {
-    auto thr = m_owner.GetThread();
-    if (!thr || thr->m_listeners.empty()) return;
-    thr->m_queue.emplace(std::piecewise_construct,
-                         std::make_tuple(only_listener),
-                         std::forward_as_tuple(std::forward<Args>(args)...));
-    thr->m_cond.notify_one();
-  }
-
-  typename wpi::SafeThreadOwner<Thread>::Proxy GetThread() const {
-    return m_owner.GetThread();
-  }
-
- private:
-  wpi::SafeThreadOwner<Thread> m_owner;
-};
-
-}  // namespace nt
-
-#endif  // NTCORE_CALLBACKMANAGER_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.cpp
index 340aad0..60e7303 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ConnectionNotifier.h"
 
@@ -11,7 +8,9 @@
 
 ConnectionNotifier::ConnectionNotifier(int inst) : m_inst(inst) {}
 
-void ConnectionNotifier::Start() { DoStart(m_inst); }
+void ConnectionNotifier::Start() {
+  DoStart(m_inst);
+}
 
 unsigned int ConnectionNotifier::Add(
     std::function<void(const ConnectionNotification& event)> callback) {
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.h b/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.h
index 65eec06..fab4733 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ConnectionNotifier.h
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_CONNECTIONNOTIFIER_H_
 #define NTCORE_CONNECTIONNOTIFIER_H_
 
-#include "CallbackManager.h"
+#include <utility>
+
+#include <wpi/CallbackManager.h>
+
 #include "Handle.h"
 #include "IConnectionNotifier.h"
 #include "ntcore_cpp.h"
@@ -18,9 +18,12 @@
 namespace impl {
 
 class ConnectionNotifierThread
-    : public CallbackThread<ConnectionNotifierThread, ConnectionNotification> {
+    : public wpi::CallbackThread<ConnectionNotifierThread,
+                                 ConnectionNotification> {
  public:
-  explicit ConnectionNotifierThread(int inst) : m_inst(inst) {}
+  ConnectionNotifierThread(std::function<void()> on_start,
+                           std::function<void()> on_exit, int inst)
+      : CallbackThread(std::move(on_start), std::move(on_exit)), m_inst(inst) {}
 
   bool Matches(const ListenerData& /*listener*/,
                const ConnectionNotification& /*data*/) {
@@ -45,11 +48,11 @@
 
 class ConnectionNotifier
     : public IConnectionNotifier,
-      public CallbackManager<ConnectionNotifier,
-                             impl::ConnectionNotifierThread> {
+      public wpi::CallbackManager<ConnectionNotifier,
+                                  impl::ConnectionNotifierThread> {
   friend class ConnectionNotifierTest;
-  friend class CallbackManager<ConnectionNotifier,
-                               impl::ConnectionNotifierThread>;
+  friend class wpi::CallbackManager<ConnectionNotifier,
+                                    impl::ConnectionNotifierThread>;
 
  public:
   explicit ConnectionNotifier(int inst);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.cpp
index f3bff4d..84d9698 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.cpp
@@ -1,17 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Dispatcher.h"
 
 #include <algorithm>
 #include <iterator>
 
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
 #include <wpi/TCPAcceptor.h>
 #include <wpi/TCPConnector.h>
+#include <wpi/timestamp.h>
 
 #include "IConnectionNotifier.h"
 #include "IStorage.h"
@@ -20,9 +20,9 @@
 
 using namespace nt;
 
-void Dispatcher::StartServer(const Twine& persist_filename,
+void Dispatcher::StartServer(std::string_view persist_filename,
                              const char* listen_address, unsigned int port) {
-  std::string listen_address_copy(StringRef(listen_address).trim());
+  std::string listen_address_copy(wpi::trim(listen_address));
   DispatcherBase::StartServer(
       persist_filename,
       std::unique_ptr<wpi::NetworkAcceptor>(new wpi::TCPAcceptor(
@@ -30,7 +30,7 @@
 }
 
 void Dispatcher::SetServer(const char* server_name, unsigned int port) {
-  std::string server_name_copy(StringRef(server_name).trim());
+  std::string server_name_copy(wpi::trim(server_name));
   SetConnector([=]() -> std::unique_ptr<wpi::NetworkStream> {
     return wpi::TCPConnector::connect(server_name_copy.c_str(),
                                       static_cast<int>(port), m_logger, 1);
@@ -38,71 +38,59 @@
 }
 
 void Dispatcher::SetServer(
-    ArrayRef<std::pair<StringRef, unsigned int>> servers) {
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers) {
   wpi::SmallVector<std::pair<std::string, int>, 16> servers_copy;
-  for (const auto& server : servers)
-    servers_copy.emplace_back(std::string{server.first.trim()},
+  for (const auto& server : servers) {
+    servers_copy.emplace_back(std::string{wpi::trim(server.first)},
                               static_cast<int>(server.second));
+  }
 
   SetConnector([=]() -> std::unique_ptr<wpi::NetworkStream> {
     wpi::SmallVector<std::pair<const char*, int>, 16> servers_copy2;
-    for (const auto& server : servers_copy)
+    for (const auto& server : servers_copy) {
       servers_copy2.emplace_back(server.first.c_str(), server.second);
+    }
     return wpi::TCPConnector::connect_parallel(servers_copy2, m_logger, 1);
   });
 }
 
 void Dispatcher::SetServerTeam(unsigned int team, unsigned int port) {
-  std::pair<StringRef, unsigned int> servers[5];
+  std::pair<std::string_view, unsigned int> servers[5];
 
   // 10.te.am.2
-  wpi::SmallString<32> fixed;
-  {
-    wpi::raw_svector_ostream oss{fixed};
-    oss << "10." << static_cast<int>(team / 100) << '.'
-        << static_cast<int>(team % 100) << ".2";
-    servers[0] = std::make_pair(oss.str(), port);
-  }
+  auto fixed = fmt::format("10.{}.{}.2", static_cast<int>(team / 100),
+                           static_cast<int>(team % 100));
+  servers[0] = {fixed, port};
 
   // 172.22.11.2
-  servers[1] = std::make_pair("172.22.11.2", port);
+  servers[1] = {"172.22.11.2", port};
 
   // roboRIO-<team>-FRC.local
-  wpi::SmallString<32> mdns;
-  {
-    wpi::raw_svector_ostream oss{mdns};
-    oss << "roboRIO-" << team << "-FRC.local";
-    servers[2] = std::make_pair(oss.str(), port);
-  }
+  auto mdns = fmt::format("roboRIO-{}-FRC.local", team);
+  servers[2] = {mdns, port};
 
   // roboRIO-<team>-FRC.lan
-  wpi::SmallString<32> mdns_lan;
-  {
-    wpi::raw_svector_ostream oss{mdns_lan};
-    oss << "roboRIO-" << team << "-FRC.lan";
-    servers[3] = std::make_pair(oss.str(), port);
-  }
+  auto mdns_lan = fmt::format("roboRIO-{}-FRC.lan", team);
+  servers[3] = {mdns_lan, port};
 
   // roboRIO-<team>-FRC.frc-field.local
-  wpi::SmallString<64> field_local;
-  {
-    wpi::raw_svector_ostream oss{field_local};
-    oss << "roboRIO-" << team << "-FRC.frc-field.local";
-    servers[4] = std::make_pair(oss.str(), port);
-  }
+  auto field_local = fmt::format("roboRIO-{}-FRC.frc-field.local", team);
+  servers[4] = {field_local, port};
 
   SetServer(servers);
 }
 
 void Dispatcher::SetServerOverride(const char* server_name, unsigned int port) {
-  std::string server_name_copy(StringRef(server_name).trim());
+  std::string server_name_copy(wpi::trim(server_name));
   SetConnectorOverride([=]() -> std::unique_ptr<wpi::NetworkStream> {
     return wpi::TCPConnector::connect(server_name_copy.c_str(),
                                       static_cast<int>(port), m_logger, 1);
   });
 }
 
-void Dispatcher::ClearServerOverride() { ClearConnectorOverride(); }
+void Dispatcher::ClearServerOverride() {
+  ClearConnectorOverride();
+}
 
 DispatcherBase::DispatcherBase(IStorage& storage, IConnectionNotifier& notifier,
                                wpi::Logger& logger)
@@ -111,14 +99,20 @@
   m_update_rate = 100;
 }
 
-DispatcherBase::~DispatcherBase() { Stop(); }
+DispatcherBase::~DispatcherBase() {
+  Stop();
+}
 
-unsigned int DispatcherBase::GetNetworkMode() const { return m_networkMode; }
+unsigned int DispatcherBase::GetNetworkMode() const {
+  return m_networkMode;
+}
 
 void DispatcherBase::StartLocal() {
   {
     std::scoped_lock lock(m_user_mutex);
-    if (m_active) return;
+    if (m_active) {
+      return;
+    }
     m_active = true;
   }
   m_networkMode = NT_NET_MODE_LOCAL;
@@ -126,30 +120,30 @@
 }
 
 void DispatcherBase::StartServer(
-    const Twine& persist_filename,
+    std::string_view persist_filename,
     std::unique_ptr<wpi::NetworkAcceptor> acceptor) {
   {
     std::scoped_lock lock(m_user_mutex);
-    if (m_active) return;
+    if (m_active) {
+      return;
+    }
     m_active = true;
   }
   m_networkMode = NT_NET_MODE_SERVER | NT_NET_MODE_STARTING;
-  m_persist_filename = persist_filename.str();
+  m_persist_filename = persist_filename;
   m_server_acceptor = std::move(acceptor);
 
   // Load persistent file.  Ignore errors, but pass along warnings.
-  if (!persist_filename.isTriviallyEmpty() &&
-      (!persist_filename.isSingleStringRef() ||
-       !persist_filename.getSingleStringRef().empty())) {
+  if (!persist_filename.empty()) {
     bool first = true;
     m_storage.LoadPersistent(
         persist_filename, [&](size_t line, const char* msg) {
           if (first) {
             first = false;
-            WARNING("When reading initial persistent values from '"
-                    << persist_filename << "':");
+            WARNING("When reading initial persistent values from '{}':",
+                    persist_filename);
           }
-          WARNING(persist_filename << ":" << line << ": " << msg);
+          WARNING("{}:{}: {}", persist_filename, line, msg);
         });
   }
 
@@ -162,7 +156,9 @@
 void DispatcherBase::StartClient() {
   {
     std::scoped_lock lock(m_user_mutex);
-    if (m_active) return;
+    if (m_active) {
+      return;
+    }
     m_active = true;
   }
   m_networkMode = NT_NET_MODE_CLIENT | NT_NET_MODE_STARTING;
@@ -186,11 +182,17 @@
   ClientReconnect();
 
   // wake up server thread by shutting down the socket
-  if (m_server_acceptor) m_server_acceptor->shutdown();
+  if (m_server_acceptor) {
+    m_server_acceptor->shutdown();
+  }
 
   // join threads, with timeout
-  if (m_dispatch_thread.joinable()) m_dispatch_thread.join();
-  if (m_clientserver_thread.joinable()) m_clientserver_thread.join();
+  if (m_dispatch_thread.joinable()) {
+    m_dispatch_thread.join();
+  }
+  if (m_clientserver_thread.joinable()) {
+    m_clientserver_thread.join();
+  }
 
   std::vector<std::shared_ptr<INetworkConnection>> conns;
   {
@@ -203,25 +205,28 @@
 }
 
 void DispatcherBase::SetUpdateRate(double interval) {
-  // don't allow update rates faster than 10 ms or slower than 1 second
-  if (interval < 0.01)
-    interval = 0.01;
-  else if (interval > 1.0)
+  // don't allow update rates faster than 5 ms or slower than 1 second
+  if (interval < 0.005) {
+    interval = 0.005;
+  } else if (interval > 1.0) {
     interval = 1.0;
+  }
   m_update_rate = static_cast<unsigned int>(interval * 1000);
 }
 
-void DispatcherBase::SetIdentity(const Twine& name) {
+void DispatcherBase::SetIdentity(std::string_view name) {
   std::scoped_lock lock(m_user_mutex);
-  m_identity = name.str();
+  m_identity = name;
 }
 
 void DispatcherBase::Flush() {
-  auto now = std::chrono::steady_clock::now();
+  auto now = wpi::Now();
   {
     std::scoped_lock lock(m_flush_mutex);
-    // don't allow flushes more often than every 10 ms
-    if ((now - m_last_flush) < std::chrono::milliseconds(10)) return;
+    // don't allow flushes more often than every 5 ms
+    if ((now - m_last_flush) < 5000) {
+      return;
+    }
     m_last_flush = now;
     m_do_flush = true;
   }
@@ -230,11 +235,15 @@
 
 std::vector<ConnectionInfo> DispatcherBase::GetConnections() const {
   std::vector<ConnectionInfo> conns;
-  if (!m_active) return conns;
+  if (!m_active) {
+    return conns;
+  }
 
   std::scoped_lock lock(m_user_mutex);
   for (auto& conn : m_connections) {
-    if (conn->state() != NetworkConnection::kActive) continue;
+    if (conn->state() != NetworkConnection::kActive) {
+      continue;
+    }
     conns.emplace_back(conn->info());
   }
 
@@ -242,13 +251,19 @@
 }
 
 bool DispatcherBase::IsConnected() const {
-  if (!m_active) return false;
+  if (!m_active) {
+    return false;
+  }
 
-  if (m_networkMode == NT_NET_MODE_LOCAL) return true;
+  if (m_networkMode == NT_NET_MODE_LOCAL) {
+    return true;
+  }
 
   std::scoped_lock lock(m_user_mutex);
   for (auto& conn : m_connections) {
-    if (conn->state() == NetworkConnection::kActive) return true;
+    if (conn->state() == NetworkConnection::kActive) {
+      return true;
+    }
   }
 
   return false;
@@ -262,7 +277,9 @@
   // perform immediate notifications
   if (immediate_notify) {
     for (auto& conn : m_connections) {
-      if (conn->state() != NetworkConnection::kActive) continue;
+      if (conn->state() != NetworkConnection::kActive) {
+        continue;
+      }
       m_notifier.NotifyConnection(true, conn->info(), uid);
     }
   }
@@ -276,7 +293,9 @@
   // perform immediate notifications
   if (immediate_notify) {
     for (auto& conn : m_connections) {
-      if (conn->state() != NetworkConnection::kActive) continue;
+      if (conn->state() != NetworkConnection::kActive) {
+        continue;
+      }
       m_notifier.NotifyConnection(true, conn->info(), uid);
     }
   }
@@ -309,7 +328,9 @@
   while (m_active) {
     // handle loop taking too long
     auto start = std::chrono::steady_clock::now();
-    if (start > timeout_time) timeout_time = start;
+    if (start > timeout_time) {
+      timeout_time = start;
+    }
 
     // wait for periodic or when flushed
     timeout_time += std::chrono::milliseconds(m_update_rate);
@@ -318,16 +339,22 @@
                           [&] { return !m_active || m_do_flush; });
     m_do_flush = false;
     flush_lock.unlock();
-    if (!m_active) break;  // in case we were woken up to terminate
+    if (!m_active) {
+      break;  // in case we were woken up to terminate
+    }
 
     // perform periodic persistent save
     if ((m_networkMode & NT_NET_MODE_SERVER) != 0 &&
         !m_persist_filename.empty() && start > next_save_time) {
       next_save_time += save_delta_time;
       // handle loop taking too long
-      if (start > next_save_time) next_save_time = start + save_delta_time;
+      if (start > next_save_time) {
+        next_save_time = start + save_delta_time;
+      }
       const char* err = m_storage.SavePersistent(m_persist_filename, true);
-      if (err) WARNING("periodic persistent save: " << err);
+      if (err) {
+        WARNING("periodic persistent save: {}", err);
+      }
     }
 
     {
@@ -335,20 +362,22 @@
       bool reconnect = false;
 
       if (++count > 10) {
-        DEBUG0("dispatch running " << m_connections.size() << " connections");
+        DEBUG0("dispatch running {} connections", m_connections.size());
         count = 0;
       }
 
       for (auto& conn : m_connections) {
         // post outgoing messages if connection is active
         // only send keep-alives on client
-        if (conn->state() == NetworkConnection::kActive)
+        if (conn->state() == NetworkConnection::kActive) {
           conn->PostOutgoing((m_networkMode & NT_NET_MODE_CLIENT) != 0);
+        }
 
         // if client, reconnect if connection died
         if ((m_networkMode & NT_NET_MODE_CLIENT) != 0 &&
-            conn->state() == NetworkConnection::kDead)
+            conn->state() == NetworkConnection::kDead) {
           reconnect = true;
+        }
       }
       // reconnect if we disconnected (and a reconnect is not in progress)
       if (reconnect && !m_do_reconnect) {
@@ -364,12 +393,17 @@
                                    INetworkConnection* except) {
   std::scoped_lock user_lock(m_user_mutex);
   for (auto& conn : m_connections) {
-    if (conn.get() == except) continue;
-    if (only && conn.get() != only) continue;
+    if (conn.get() == except) {
+      continue;
+    }
+    if (only && conn.get() != only) {
+      continue;
+    }
     auto state = conn->state();
     if (state != NetworkConnection::kSynchronized &&
-        state != NetworkConnection::kActive)
+        state != NetworkConnection::kActive) {
       continue;
+    }
     conn->QueueOutgoing(msg);
   }
 }
@@ -391,17 +425,17 @@
       m_networkMode = NT_NET_MODE_NONE;
       return;
     }
-    DEBUG0("server: client connection from " << stream->getPeerIP() << " port "
-                                             << stream->getPeerPort());
+    DEBUG0("server: client connection from {} port {}", stream->getPeerIP(),
+           stream->getPeerPort());
 
     // add to connections list
     using namespace std::placeholders;
     auto conn = std::make_shared<NetworkConnection>(
         ++m_connections_uid, std::move(stream), m_notifier, m_logger,
-        std::bind(&Dispatcher::ServerHandshake, this, _1, _2, _3),
-        std::bind(&IStorage::GetMessageEntryType, &m_storage, _1));
+        std::bind(&Dispatcher::ServerHandshake, this, _1, _2, _3),   // NOLINT
+        std::bind(&IStorage::GetMessageEntryType, &m_storage, _1));  // NOLINT
     conn->set_process_incoming(
-        std::bind(&IStorage::ProcessIncoming, &m_storage, _1, _2,
+        std::bind(&IStorage::ProcessIncoming, &m_storage, _1, _2,  // NOLINT
                   std::weak_ptr<NetworkConnection>(conn)));
     {
       std::scoped_lock lock(m_user_mutex);
@@ -414,7 +448,9 @@
           break;
         }
       }
-      if (!placed) m_connections.emplace_back(conn);
+      if (!placed) {
+        m_connections.emplace_back(conn);
+      }
       conn->Start();
     }
   }
@@ -442,23 +478,23 @@
     }
 
     // try to connect (with timeout)
-    DEBUG0("client trying to connect");
+    DEBUG0("{}", "client trying to connect");
     auto stream = connect();
     if (!stream) {
       m_networkMode = NT_NET_MODE_CLIENT | NT_NET_MODE_FAILURE;
       continue;  // keep retrying
     }
-    DEBUG0("client connected");
+    DEBUG0("{}", "client connected");
     m_networkMode = NT_NET_MODE_CLIENT;
 
     std::unique_lock lock(m_user_mutex);
     using namespace std::placeholders;
     auto conn = std::make_shared<NetworkConnection>(
         ++m_connections_uid, std::move(stream), m_notifier, m_logger,
-        std::bind(&Dispatcher::ClientHandshake, this, _1, _2, _3),
-        std::bind(&IStorage::GetMessageEntryType, &m_storage, _1));
+        std::bind(&Dispatcher::ClientHandshake, this, _1, _2, _3),   // NOLINT
+        std::bind(&IStorage::GetMessageEntryType, &m_storage, _1));  // NOLINT
     conn->set_process_incoming(
-        std::bind(&IStorage::ProcessIncoming, &m_storage, _1, _2,
+        std::bind(&IStorage::ProcessIncoming, &m_storage, _1, _2,  // NOLINT
                   std::weak_ptr<NetworkConnection>(conn)));
     m_connections.resize(0);  // disconnect any current
     m_connections.emplace_back(conn);
@@ -477,7 +513,7 @@
 
 bool DispatcherBase::ClientHandshake(
     NetworkConnection& conn, std::function<std::shared_ptr<Message>()> get_msg,
-    std::function<void(wpi::ArrayRef<std::shared_ptr<Message>>)> send_msgs) {
+    std::function<void(wpi::span<std::shared_ptr<Message>>)> send_msgs) {
   // get identity
   std::string self_id;
   {
@@ -486,28 +522,35 @@
   }
 
   // send client hello
-  DEBUG0("client: sending hello");
-  send_msgs(Message::ClientHello(self_id));
+  DEBUG0("{}", "client: sending hello");
+  auto msg = Message::ClientHello(self_id);
+  send_msgs(wpi::span(&msg, 1));
 
   // wait for response
-  auto msg = get_msg();
+  msg = get_msg();
   if (!msg) {
     // disconnected, retry
-    DEBUG0("client: server disconnected before first response");
+    DEBUG0("{}", "client: server disconnected before first response");
     return false;
   }
 
   if (msg->Is(Message::kProtoUnsup)) {
-    if (msg->id() == 0x0200) ClientReconnect(0x0200);
+    if (msg->id() == 0x0200) {
+      ClientReconnect(0x0200);
+    }
     return false;
   }
 
   bool new_server = true;
   if (conn.proto_rev() >= 0x0300) {
     // should be server hello; if not, disconnect.
-    if (!msg->Is(Message::kServerHello)) return false;
+    if (!msg->Is(Message::kServerHello)) {
+      return false;
+    }
     conn.set_remote_id(msg->str());
-    if ((msg->flags() & 1) != 0) new_server = false;
+    if ((msg->flags() & 1) != 0) {
+      new_server = false;
+    }
     // get the next message
     msg = get_msg();
   }
@@ -517,12 +560,14 @@
   for (;;) {
     if (!msg) {
       // disconnected, retry
-      DEBUG0("client: server disconnected during initial entries");
+      DEBUG0("{}", "client: server disconnected during initial entries");
       return false;
     }
-    DEBUG4("received init str=" << msg->str() << " id=" << msg->id()
-                                << " seq_num=" << msg->seq_num_uid());
-    if (msg->Is(Message::kServerHelloDone)) break;
+    DEBUG4("received init str={} id={} seq_num={}", msg->str(), msg->id(),
+           msg->seq_num_uid());
+    if (msg->Is(Message::kServerHelloDone)) {
+      break;
+    }
     // shouldn't receive a keep alive, but handle gracefully
     if (msg->Is(Message::kKeepAlive)) {
       msg = get_msg();
@@ -530,9 +575,10 @@
     }
     if (!msg->Is(Message::kEntryAssign)) {
       // unexpected message
-      DEBUG0("client: received message ("
-             << msg->type()
-             << ") other than entry assignment during initial handshake");
+      DEBUG0(
+          "client: received message ({}) other than entry assignment during "
+          "initial handshake",
+          msg->type());
       return false;
     }
     incoming.emplace_back(std::move(msg));
@@ -545,42 +591,48 @@
 
   m_storage.ApplyInitialAssignments(conn, incoming, new_server, &outgoing);
 
-  if (conn.proto_rev() >= 0x0300)
+  if (conn.proto_rev() >= 0x0300) {
     outgoing.emplace_back(Message::ClientHelloDone());
+  }
 
-  if (!outgoing.empty()) send_msgs(outgoing);
+  if (!outgoing.empty()) {
+    send_msgs(outgoing);
+  }
 
-  INFO("client: CONNECTED to server " << conn.stream().getPeerIP() << " port "
-                                      << conn.stream().getPeerPort());
+  INFO("client: CONNECTED to server {} port {}", conn.stream().getPeerIP(),
+       conn.stream().getPeerPort());
   return true;
 }
 
 bool DispatcherBase::ServerHandshake(
     NetworkConnection& conn, std::function<std::shared_ptr<Message>()> get_msg,
-    std::function<void(wpi::ArrayRef<std::shared_ptr<Message>>)> send_msgs) {
+    std::function<void(wpi::span<std::shared_ptr<Message>>)> send_msgs) {
   // Wait for the client to send us a hello.
   auto msg = get_msg();
   if (!msg) {
-    DEBUG0("server: client disconnected before sending hello");
+    DEBUG0("{}", "server: client disconnected before sending hello");
     return false;
   }
   if (!msg->Is(Message::kClientHello)) {
-    DEBUG0("server: client initial message was not client hello");
+    DEBUG0("{}", "server: client initial message was not client hello");
     return false;
   }
 
   // Check that the client requested version is not too high.
   unsigned int proto_rev = msg->id();
   if (proto_rev > 0x0300) {
-    DEBUG0("server: client requested proto > 0x0300");
-    send_msgs(Message::ProtoUnsup());
+    DEBUG0("{}", "server: client requested proto > 0x0300");
+    auto toSend = Message::ProtoUnsup();
+    send_msgs(wpi::span(&toSend, 1));
     return false;
   }
 
-  if (proto_rev >= 0x0300) conn.set_remote_id(msg->str());
+  if (proto_rev >= 0x0300) {
+    conn.set_remote_id(msg->str());
+  }
 
   // Set the proto version to the client requested version
-  DEBUG0("server: client protocol " << proto_rev);
+  DEBUG0("server: client protocol {}", proto_rev);
   conn.set_proto_rev(proto_rev);
 
   // Send initial set of assignments
@@ -599,7 +651,7 @@
   outgoing.emplace_back(Message::ServerHelloDone());
 
   // Batch transmit
-  DEBUG0("server: sending initial assignments");
+  DEBUG0("{}", "server: sending initial assignments");
   send_msgs(outgoing);
 
   // In proto rev 3.0 and later, the handshake concludes with a client hello
@@ -613,10 +665,12 @@
     for (;;) {
       if (!msg) {
         // disconnected, retry
-        DEBUG0("server: disconnected waiting for initial entries");
+        DEBUG0("{}", "server: disconnected waiting for initial entries");
         return false;
       }
-      if (msg->Is(Message::kClientHelloDone)) break;
+      if (msg->Is(Message::kClientHelloDone)) {
+        break;
+      }
       // shouldn't receive a keep alive, but handle gracefully
       if (msg->Is(Message::kKeepAlive)) {
         msg = get_msg();
@@ -624,26 +678,30 @@
       }
       if (!msg->Is(Message::kEntryAssign)) {
         // unexpected message
-        DEBUG0("server: received message ("
-               << msg->type()
-               << ") other than entry assignment during initial handshake");
+        DEBUG0(
+            "server: received message ({}) other than entry assignment during "
+            "initial handshake",
+            msg->type());
         return false;
       }
       incoming.push_back(msg);
       // get the next message (blocks)
       msg = get_msg();
     }
-    for (auto& msg : incoming)
+    for (auto& msg : incoming) {
       m_storage.ProcessIncoming(msg, &conn, std::weak_ptr<NetworkConnection>());
+    }
   }
 
-  INFO("server: client CONNECTED: " << conn.stream().getPeerIP() << " port "
-                                    << conn.stream().getPeerPort());
+  INFO("server: client CONNECTED: {} port {}", conn.stream().getPeerIP(),
+       conn.stream().getPeerPort());
   return true;
 }
 
 void DispatcherBase::ClientReconnect(unsigned int proto_rev) {
-  if ((m_networkMode & NT_NET_MODE_SERVER) != 0) return;
+  if ((m_networkMode & NT_NET_MODE_SERVER) != 0) {
+    return;
+  }
   {
     std::scoped_lock lock(m_user_mutex);
     m_reconnect_proto_rev = proto_rev;
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.h b/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.h
index bfddae7..51b8ec7 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Dispatcher.h
@@ -1,26 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_DISPATCHER_H_
 #define NTCORE_DISPATCHER_H_
 
 #include <atomic>
-#include <chrono>
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <utility>
 #include <vector>
 
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
+#include <wpi/span.h>
 
 #include "IDispatcher.h"
 #include "INetworkConnection.h"
@@ -41,20 +37,20 @@
   friend class DispatcherTest;
 
  public:
-  typedef std::function<std::unique_ptr<wpi::NetworkStream>()> Connector;
+  using Connector = std::function<std::unique_ptr<wpi::NetworkStream>()>;
 
   DispatcherBase(IStorage& storage, IConnectionNotifier& notifier,
                  wpi::Logger& logger);
-  virtual ~DispatcherBase();
+  ~DispatcherBase() override;
 
   unsigned int GetNetworkMode() const;
   void StartLocal();
-  void StartServer(const Twine& persist_filename,
+  void StartServer(std::string_view persist_filename,
                    std::unique_ptr<wpi::NetworkAcceptor> acceptor);
   void StartClient();
   void Stop();
   void SetUpdateRate(double interval);
-  void SetIdentity(const Twine& name);
+  void SetIdentity(std::string_view name);
   void Flush();
   std::vector<ConnectionInfo> GetConnections() const;
   bool IsConnected() const;
@@ -82,11 +78,11 @@
   bool ClientHandshake(
       NetworkConnection& conn,
       std::function<std::shared_ptr<Message>()> get_msg,
-      std::function<void(wpi::ArrayRef<std::shared_ptr<Message>>)> send_msgs);
+      std::function<void(wpi::span<std::shared_ptr<Message>>)> send_msgs);
   bool ServerHandshake(
       NetworkConnection& conn,
       std::function<std::shared_ptr<Message>()> get_msg,
-      std::function<void(wpi::ArrayRef<std::shared_ptr<Message>>)> send_msgs);
+      std::function<void(wpi::span<std::shared_ptr<Message>>)> send_msgs);
 
   void ClientReconnect(unsigned int proto_rev = 0x0300);
 
@@ -116,7 +112,7 @@
   // Condition variable for forced dispatch wakeup (flush)
   wpi::mutex m_flush_mutex;
   wpi::condition_variable m_flush_cv;
-  std::chrono::steady_clock::time_point m_last_flush;
+  uint64_t m_last_flush = 0;
   bool m_do_flush = false;
 
   // Condition variable for client reconnect (uses user mutex)
@@ -136,11 +132,12 @@
              wpi::Logger& logger)
       : DispatcherBase(storage, notifier, logger) {}
 
-  void StartServer(const Twine& persist_filename, const char* listen_address,
-                   unsigned int port);
+  void StartServer(std::string_view persist_filename,
+                   const char* listen_address, unsigned int port);
 
   void SetServer(const char* server_name, unsigned int port);
-  void SetServer(ArrayRef<std::pair<StringRef, unsigned int>> servers);
+  void SetServer(
+      wpi::span<const std::pair<std::string_view, unsigned int>> servers);
   void SetServerTeam(unsigned int team, unsigned int port);
 
   void SetServerOverride(const char* server_name, unsigned int port);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.cpp
index 32a756f..9ed1850 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "DsClient.h"
 
 #include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
 #include <wpi/TCPConnector.h>
 #include <wpi/raw_ostream.h>
 #include <wpi/raw_socket_istream.h>
@@ -22,7 +20,7 @@
   Thread(Dispatcher& dispatcher, wpi::Logger& logger, unsigned int port)
       : m_dispatcher(dispatcher), m_logger(logger), m_port(port) {}
 
-  void Main();
+  void Main() override;
 
   Dispatcher& m_dispatcher;
   wpi::Logger& m_logger;
@@ -35,10 +33,11 @@
 
 void DsClient::Start(unsigned int port) {
   auto thr = m_owner.GetThread();
-  if (!thr)
+  if (!thr) {
     m_owner.Start(m_dispatcher, m_logger, port);
-  else
+  } else {
     thr->m_port = port;
+  }
 }
 
 void DsClient::Stop() {
@@ -47,7 +46,9 @@
     auto thr = m_owner.GetThread();
     if (thr) {
       thr->m_active = false;
-      if (thr->m_stream) thr->m_stream->close();
+      if (thr->m_stream) {
+        thr->m_stream->close();
+      }
     }
   }
   m_owner.Stop();
@@ -67,14 +68,20 @@
       m_cond.wait_until(lock, timeout_time, [&] { return !m_active; });
       port = m_port;
     }
-    if (!m_active) goto done;
+    if (!m_active) {
+      goto done;
+    }
 
     // Try to connect to DS on the local machine
     m_stream = wpi::TCPConnector::connect("127.0.0.1", 1742, nolog, 1);
-    if (!m_active) goto done;
-    if (!m_stream) continue;
+    if (!m_active) {
+      goto done;
+    }
+    if (!m_stream) {
+      continue;
+    }
 
-    DEBUG3("connected to DS");
+    DEBUG3("{}", "connected to DS");
     wpi::raw_socket_istream is(*m_stream);
 
     while (m_active && !is.has_error()) {
@@ -86,8 +93,12 @@
       // Throw away characters until {
       do {
         is.read(ch);
-        if (is.has_error()) break;
-        if (!m_active) goto done;
+        if (is.has_error()) {
+          break;
+        }
+        if (!m_active) {
+          goto done;
+        }
       } while (ch != '{');
       json += '{';
 
@@ -99,8 +110,12 @@
       // Read characters until }
       do {
         is.read(ch);
-        if (is.has_error()) break;
-        if (!m_active) goto done;
+        if (is.has_error()) {
+          break;
+        }
+        if (!m_active) {
+          goto done;
+        }
         json += ch;
       } while (ch != '}');
 
@@ -108,20 +123,29 @@
         m_stream = nullptr;
         break;
       }
-      DEBUG3("json=" << json);
+      DEBUG3("json={}", json);
 
       // Look for "robotIP":12345, and get 12345 portion
       size_t pos = json.find("\"robotIP\"");
-      if (pos == wpi::StringRef::npos) continue;  // could not find?
+      if (pos == std::string_view::npos) {
+        continue;  // could not find?
+      }
       pos += 9;
       pos = json.find(':', pos);
-      if (pos == wpi::StringRef::npos) continue;  // could not find?
+      if (pos == std::string_view::npos) {
+        continue;  // could not find?
+      }
       size_t endpos = json.find_first_not_of("0123456789", pos + 1);
-      DEBUG3("found robotIP=" << json.slice(pos + 1, endpos));
+      DEBUG3("found robotIP={}", wpi::slice(json, pos + 1, endpos));
 
       // Parse into number
       unsigned int ip = 0;
-      if (json.slice(pos + 1, endpos).getAsInteger(10, ip)) continue;  // error
+      if (auto v = wpi::parse_integer<unsigned int>(
+              wpi::slice(json, pos + 1, endpos), 10)) {
+        ip = v.value();
+      } else {
+        continue;  // error
+      }
 
       // If zero, clear the server override
       if (ip == 0) {
@@ -131,16 +155,16 @@
       }
 
       // If unchanged, don't reconnect
-      if (ip == oldip) continue;
+      if (ip == oldip) {
+        continue;
+      }
       oldip = ip;
 
       // Convert number into dotted quad
-      json.clear();
-      wpi::raw_svector_ostream os{json};
-      os << ((ip >> 24) & 0xff) << "." << ((ip >> 16) & 0xff) << "."
-         << ((ip >> 8) & 0xff) << "." << (ip & 0xff);
-      INFO("client: DS overriding server IP to " << os.str());
-      m_dispatcher.SetServerOverride(json.c_str(), port);
+      auto newip = fmt::format("{}.{}.{}.{}", (ip >> 24) & 0xff,
+                               (ip >> 16) & 0xff, (ip >> 8) & 0xff, ip & 0xff);
+      INFO("client: DS overriding server IP to {}", newip);
+      m_dispatcher.SetServerOverride(newip.c_str(), port);
     }
 
     // We disconnected from the DS, clear the server override
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.h b/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.h
index ad13d25..73fc3d3 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/DsClient.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_DSCLIENT_H_
 #define NTCORE_DSCLIENT_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.cpp
index c8f8d76..6f251a3 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "EntryNotifier.h"
 
+#include <wpi/StringExtras.h>
+
 #include "Log.h"
 
 using namespace nt;
@@ -16,13 +15,19 @@
   m_local_notifiers = false;
 }
 
-void EntryNotifier::Start() { DoStart(m_inst); }
+void EntryNotifier::Start() {
+  DoStart(m_inst);
+}
 
-bool EntryNotifier::local_notifiers() const { return m_local_notifiers; }
+bool EntryNotifier::local_notifiers() const {
+  return m_local_notifiers;
+}
 
 bool impl::EntryNotifierThread::Matches(const EntryListenerData& listener,
                                         const EntryNotification& data) {
-  if (!data.value) return false;
+  if (!data.value) {
+    return false;
+  }
 
   // Flags must be within requested flag set for this listener.
   // Because assign messages can result in both a value and flags update,
@@ -32,58 +37,73 @@
   unsigned int flags = data.flags & ~(NT_NOTIFY_IMMEDIATE | NT_NOTIFY_LOCAL);
   unsigned int assign_both = NT_NOTIFY_UPDATE | NT_NOTIFY_FLAGS;
   if ((flags & assign_both) == assign_both) {
-    if ((listen_flags & assign_both) == 0) return false;
+    if ((listen_flags & assign_both) == 0) {
+      return false;
+    }
     listen_flags &= ~assign_both;
     flags &= ~assign_both;
   }
-  if ((flags & ~listen_flags) != 0) return false;
+  if ((flags & ~listen_flags) != 0) {
+    return false;
+  }
 
   // must match local id or prefix
-  if (listener.entry != 0 && data.entry != listener.entry) return false;
-  if (listener.entry == 0 &&
-      !wpi::StringRef(data.name).startswith(listener.prefix))
+  if (listener.entry != 0 && data.entry != listener.entry) {
     return false;
+  }
+  if (listener.entry == 0 && !wpi::starts_with(data.name, listener.prefix)) {
+    return false;
+  }
 
   return true;
 }
 
 unsigned int EntryNotifier::Add(
     std::function<void(const EntryNotification& event)> callback,
-    StringRef prefix, unsigned int flags) {
-  if ((flags & NT_NOTIFY_LOCAL) != 0) m_local_notifiers = true;
+    std::string_view prefix, unsigned int flags) {
+  if ((flags & NT_NOTIFY_LOCAL) != 0) {
+    m_local_notifiers = true;
+  }
   return DoAdd(callback, prefix, flags);
 }
 
 unsigned int EntryNotifier::Add(
     std::function<void(const EntryNotification& event)> callback,
     unsigned int local_id, unsigned int flags) {
-  if ((flags & NT_NOTIFY_LOCAL) != 0) m_local_notifiers = true;
+  if ((flags & NT_NOTIFY_LOCAL) != 0) {
+    m_local_notifiers = true;
+  }
   return DoAdd(callback, Handle(m_inst, local_id, Handle::kEntry), flags);
 }
 
 unsigned int EntryNotifier::AddPolled(unsigned int poller_uid,
-                                      wpi::StringRef prefix,
+                                      std::string_view prefix,
                                       unsigned int flags) {
-  if ((flags & NT_NOTIFY_LOCAL) != 0) m_local_notifiers = true;
+  if ((flags & NT_NOTIFY_LOCAL) != 0) {
+    m_local_notifiers = true;
+  }
   return DoAdd(poller_uid, prefix, flags);
 }
 
 unsigned int EntryNotifier::AddPolled(unsigned int poller_uid,
                                       unsigned int local_id,
                                       unsigned int flags) {
-  if ((flags & NT_NOTIFY_LOCAL) != 0) m_local_notifiers = true;
+  if ((flags & NT_NOTIFY_LOCAL) != 0) {
+    m_local_notifiers = true;
+  }
   return DoAdd(poller_uid, Handle(m_inst, local_id, Handle::kEntry), flags);
 }
 
-void EntryNotifier::NotifyEntry(unsigned int local_id, StringRef name,
+void EntryNotifier::NotifyEntry(unsigned int local_id, std::string_view name,
                                 std::shared_ptr<Value> value,
                                 unsigned int flags,
                                 unsigned int only_listener) {
   // optimization: don't generate needless local queue entries if we have
   // no local listeners (as this is a common case on the server side)
-  if ((flags & NT_NOTIFY_LOCAL) != 0 && !m_local_notifiers) return;
-  DEBUG0("notifying '" << name << "' (local=" << local_id
-                       << "), flags=" << flags);
+  if ((flags & NT_NOTIFY_LOCAL) != 0 && !m_local_notifiers) {
+    return;
+  }
+  DEBUG0("notifying '{}' (local={}), flags={}", name, local_id, flags);
   Send(only_listener, 0, Handle(m_inst, local_id, Handle::kEntry).handle(),
        name, value, flags);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.h b/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.h
index 3ccf9ff..bbe2172 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/EntryNotifier.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_ENTRYNOTIFIER_H_
 #define NTCORE_ENTRYNOTIFIER_H_
@@ -11,8 +8,11 @@
 #include <atomic>
 #include <memory>
 #include <string>
+#include <string_view>
+#include <utility>
 
-#include "CallbackManager.h"
+#include <wpi/CallbackManager.h>
+
 #include "Handle.h"
 #include "IEntryNotifier.h"
 #include "ntcore_cpp.h"
@@ -26,22 +26,23 @@
 namespace impl {
 
 struct EntryListenerData
-    : public ListenerData<std::function<void(const EntryNotification& event)>> {
+    : public wpi::CallbackListenerData<
+          std::function<void(const EntryNotification& event)>> {
   EntryListenerData() = default;
   EntryListenerData(
       std::function<void(const EntryNotification& event)> callback_,
-      StringRef prefix_, unsigned int flags_)
-      : ListenerData(callback_), prefix(prefix_), flags(flags_) {}
+      std::string_view prefix_, unsigned int flags_)
+      : CallbackListenerData(callback_), prefix(prefix_), flags(flags_) {}
   EntryListenerData(
       std::function<void(const EntryNotification& event)> callback_,
       NT_Entry entry_, unsigned int flags_)
-      : ListenerData(callback_), entry(entry_), flags(flags_) {}
-  EntryListenerData(unsigned int poller_uid_, StringRef prefix_,
+      : CallbackListenerData(callback_), entry(entry_), flags(flags_) {}
+  EntryListenerData(unsigned int poller_uid_, std::string_view prefix_,
                     unsigned int flags_)
-      : ListenerData(poller_uid_), prefix(prefix_), flags(flags_) {}
+      : CallbackListenerData(poller_uid_), prefix(prefix_), flags(flags_) {}
   EntryListenerData(unsigned int poller_uid_, NT_Entry entry_,
                     unsigned int flags_)
-      : ListenerData(poller_uid_), entry(entry_), flags(flags_) {}
+      : CallbackListenerData(poller_uid_), entry(entry_), flags(flags_) {}
 
   std::string prefix;
   NT_Entry entry = 0;
@@ -49,10 +50,12 @@
 };
 
 class EntryNotifierThread
-    : public CallbackThread<EntryNotifierThread, EntryNotification,
-                            EntryListenerData> {
+    : public wpi::CallbackThread<EntryNotifierThread, EntryNotification,
+                                 EntryListenerData> {
  public:
-  explicit EntryNotifierThread(int inst) : m_inst(inst) {}
+  EntryNotifierThread(std::function<void()> on_start,
+                      std::function<void()> on_exit, int inst)
+      : CallbackThread(std::move(on_start), std::move(on_exit)), m_inst(inst) {}
 
   bool Matches(const EntryListenerData& listener,
                const EntryNotification& data);
@@ -74,9 +77,9 @@
 
 class EntryNotifier
     : public IEntryNotifier,
-      public CallbackManager<EntryNotifier, impl::EntryNotifierThread> {
+      public wpi::CallbackManager<EntryNotifier, impl::EntryNotifierThread> {
   friend class EntryNotifierTest;
-  friend class CallbackManager<EntryNotifier, impl::EntryNotifierThread>;
+  friend class wpi::CallbackManager<EntryNotifier, impl::EntryNotifierThread>;
 
  public:
   explicit EntryNotifier(int inst, wpi::Logger& logger);
@@ -86,15 +89,15 @@
   bool local_notifiers() const override;
 
   unsigned int Add(std::function<void(const EntryNotification& event)> callback,
-                   wpi::StringRef prefix, unsigned int flags) override;
+                   std::string_view prefix, unsigned int flags) override;
   unsigned int Add(std::function<void(const EntryNotification& event)> callback,
                    unsigned int local_id, unsigned int flags) override;
-  unsigned int AddPolled(unsigned int poller_uid, wpi::StringRef prefix,
+  unsigned int AddPolled(unsigned int poller_uid, std::string_view prefix,
                          unsigned int flags) override;
   unsigned int AddPolled(unsigned int poller_uid, unsigned int local_id,
                          unsigned int flags) override;
 
-  void NotifyEntry(unsigned int local_id, StringRef name,
+  void NotifyEntry(unsigned int local_id, std::string_view name,
                    std::shared_ptr<Value> value, unsigned int flags,
                    unsigned int only_listener = UINT_MAX) override;
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Handle.h b/third_party/allwpilib/ntcore/src/main/native/cpp/Handle.h
index 47b5edf..df12381 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Handle.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Handle.h
@@ -1,26 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_HANDLE_H_
 #define NTCORE_HANDLE_H_
 
+#include <wpi/Synchronization.h>
+
 #include "ntcore_c.h"
 
 namespace nt {
 
 // Handle data layout:
-// Bits 30-28: Type
-// Bits 27-20: Instance index
+// Bits 30-24: Type
+// Bits 23-20: Instance index
 // Bits 19-0:  Handle index (0/unused for instance handles)
 
 class Handle {
  public:
   enum Type {
-    kConnectionListener = 1,
+    kConnectionListener = wpi::kHandleTypeNTBase,
     kConnectionListenerPoller,
     kEntry,
     kEntryListener,
@@ -43,15 +42,15 @@
       m_handle = 0;
       return;
     }
-    m_handle = ((static_cast<int>(type) & 0xf) << 27) | ((inst & 0x7f) << 20) |
+    m_handle = ((static_cast<int>(type) & 0x7f) << 24) | ((inst & 0xf) << 20) |
                (index & 0xfffff);
   }
 
   int GetIndex() const { return static_cast<int>(m_handle) & 0xfffff; }
   Type GetType() const {
-    return static_cast<Type>((static_cast<int>(m_handle) >> 27) & 0xf);
+    return static_cast<Type>((static_cast<int>(m_handle) >> 24) & 0x7f);
   }
-  int GetInst() const { return (static_cast<int>(m_handle) >> 20) & 0x7f; }
+  int GetInst() const { return (static_cast<int>(m_handle) >> 20) & 0xf; }
   bool IsType(Type type) const { return type == GetType(); }
   int GetTypedIndex(Type type) const { return IsType(type) ? GetIndex() : -1; }
   int GetTypedInst(Type type) const { return IsType(type) ? GetInst() : -1; }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/IConnectionNotifier.h b/third_party/allwpilib/ntcore/src/main/native/cpp/IConnectionNotifier.h
index 7a165f2..5983866 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/IConnectionNotifier.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/IConnectionNotifier.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_ICONNECTIONNOTIFIER_H_
 #define NTCORE_ICONNECTIONNOTIFIER_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/IDispatcher.h b/third_party/allwpilib/ntcore/src/main/native/cpp/IDispatcher.h
index aace766..4cf8a5f 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/IDispatcher.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/IDispatcher.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_IDISPATCHER_H_
 #define NTCORE_IDISPATCHER_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/IEntryNotifier.h b/third_party/allwpilib/ntcore/src/main/native/cpp/IEntryNotifier.h
index 80ed97e..b3f91b2 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/IEntryNotifier.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/IEntryNotifier.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_IENTRYNOTIFIER_H_
 #define NTCORE_IENTRYNOTIFIER_H_
 
 #include <climits>
 #include <memory>
+#include <string_view>
 
 #include "ntcore_cpp.h"
 
@@ -25,16 +23,17 @@
 
   virtual unsigned int Add(
       std::function<void(const EntryNotification& event)> callback,
-      wpi::StringRef prefix, unsigned int flags) = 0;
+      std::string_view prefix, unsigned int flags) = 0;
   virtual unsigned int Add(
       std::function<void(const EntryNotification& event)> callback,
       unsigned int local_id, unsigned int flags) = 0;
-  virtual unsigned int AddPolled(unsigned int poller_uid, wpi::StringRef prefix,
+  virtual unsigned int AddPolled(unsigned int poller_uid,
+                                 std::string_view prefix,
                                  unsigned int flags) = 0;
   virtual unsigned int AddPolled(unsigned int poller_uid, unsigned int local_id,
                                  unsigned int flags) = 0;
 
-  virtual void NotifyEntry(unsigned int local_id, StringRef name,
+  virtual void NotifyEntry(unsigned int local_id, std::string_view name,
                            std::shared_ptr<Value> value, unsigned int flags,
                            unsigned int only_listener = UINT_MAX) = 0;
 };
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/INetworkConnection.h b/third_party/allwpilib/ntcore/src/main/native/cpp/INetworkConnection.h
index 0387cc9..94e9bb1 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/INetworkConnection.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/INetworkConnection.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_INETWORKCONNECTION_H_
 #define NTCORE_INETWORKCONNECTION_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/IRpcServer.h b/third_party/allwpilib/ntcore/src/main/native/cpp/IRpcServer.h
index dc8b0a6..aa16084 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/IRpcServer.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/IRpcServer.h
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_IRPCSERVER_H_
 #define NTCORE_IRPCSERVER_H_
 
 #include <memory>
+#include <string_view>
 
 #include "Message.h"
 #include "ntcore_cpp.h"
@@ -17,7 +15,7 @@
 
 class IRpcServer {
  public:
-  typedef std::function<void(StringRef result)> SendResponseFunc;
+  using SendResponseFunc = std::function<void(std::string_view result)>;
 
   IRpcServer() = default;
   IRpcServer(const IRpcServer&) = delete;
@@ -27,7 +25,7 @@
   virtual void RemoveRpc(unsigned int rpc_uid) = 0;
 
   virtual void ProcessRpc(unsigned int local_id, unsigned int call_uid,
-                          StringRef name, StringRef params,
+                          std::string_view name, std::string_view params,
                           const ConnectionInfo& conn,
                           SendResponseFunc send_response,
                           unsigned int rpc_uid) = 0;
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/IStorage.h b/third_party/allwpilib/ntcore/src/main/native/cpp/IStorage.h
index 0fb3a0b..795d032 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/IStorage.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/IStorage.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_ISTORAGE_H_
 #define NTCORE_ISTORAGE_H_
 
 #include <functional>
 #include <memory>
+#include <string_view>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
 #include "Message.h"
 #include "ntcore_cpp.h"
@@ -48,15 +45,15 @@
       INetworkConnection& conn,
       std::vector<std::shared_ptr<Message>>* msgs) = 0;
   virtual void ApplyInitialAssignments(
-      INetworkConnection& conn, wpi::ArrayRef<std::shared_ptr<Message>> msgs,
+      INetworkConnection& conn, wpi::span<std::shared_ptr<Message>> msgs,
       bool new_server, std::vector<std::shared_ptr<Message>>* out_msgs) = 0;
 
   // Filename-based save/load functions.  Used both by periodic saves and
   // accessible directly via the user API.
-  virtual const char* SavePersistent(const Twine& filename,
+  virtual const char* SavePersistent(std::string_view filename,
                                      bool periodic) const = 0;
   virtual const char* LoadPersistent(
-      const Twine& filename,
+      std::string_view filename,
       std::function<void(size_t line, const char* msg)> warn) = 0;
 };
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.cpp
index cd35fb0..8d59a0b 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.cpp
@@ -1,24 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "InstanceImpl.h"
 
 using namespace nt;
 
 std::atomic<int> InstanceImpl::s_default{-1};
-std::atomic<InstanceImpl*> InstanceImpl::s_fast_instances[10];
-wpi::UidVector<InstanceImpl*, 10> InstanceImpl::s_instances;
+std::atomic<InstanceImpl*> InstanceImpl::s_instances[kNumInstances];
 wpi::mutex InstanceImpl::s_mutex;
 
 using namespace std::placeholders;
 
 InstanceImpl::InstanceImpl(int inst)
     : logger_impl(inst),
-      logger(std::bind(&LoggerImpl::Log, &logger_impl, _1, _2, _3, _4)),
+      logger(
+          std::bind(&LoggerImpl::Log, &logger_impl, _1, _2, _3, _4)),  // NOLINT
       connection_notifier(inst),
       entry_notifier(inst, logger),
       rpc_server(inst, logger),
@@ -28,49 +25,35 @@
   logger.set_min_level(logger_impl.GetMinLevel());
 }
 
-InstanceImpl::~InstanceImpl() { logger.SetLogger(nullptr); }
+InstanceImpl::~InstanceImpl() {
+  logger.SetLogger(nullptr);
+}
 
-InstanceImpl* InstanceImpl::GetDefault() { return Get(GetDefaultIndex()); }
+InstanceImpl* InstanceImpl::GetDefault() {
+  return Get(GetDefaultIndex());
+}
 
 InstanceImpl* InstanceImpl::Get(int inst) {
-  if (inst < 0) return nullptr;
-
-  // fast path, just an atomic read
-  if (static_cast<unsigned int>(inst) <
-      (sizeof(s_fast_instances) / sizeof(s_fast_instances[0]))) {
-    InstanceImpl* ptr = s_fast_instances[inst];
-    if (ptr) return ptr;
+  if (inst < 0 || inst >= kNumInstances) {
+    return nullptr;
   }
-
-  // slow path
-  std::scoped_lock lock(s_mutex);
-
-  // static fast-path block
-  if (static_cast<unsigned int>(inst) <
-      (sizeof(s_fast_instances) / sizeof(s_fast_instances[0]))) {
-    // double-check
-    return s_fast_instances[inst];
-  }
-
-  // vector
-  if (static_cast<unsigned int>(inst) < s_instances.size()) {
-    return s_instances[inst];
-  }
-
-  // doesn't exist
-  return nullptr;
+  return s_instances[inst];
 }
 
 int InstanceImpl::GetDefaultIndex() {
   int inst = s_default;
-  if (inst >= 0) return inst;
+  if (inst >= 0) {
+    return inst;
+  }
 
   // slow path
   std::scoped_lock lock(s_mutex);
 
   // double-check
   inst = s_default;
-  if (inst >= 0) return inst;
+  if (inst >= 0) {
+    return inst;
+  }
 
   // alloc and save
   inst = AllocImpl();
@@ -84,26 +67,23 @@
 }
 
 int InstanceImpl::AllocImpl() {
-  unsigned int inst = s_instances.emplace_back(nullptr);
-  InstanceImpl* ptr = new InstanceImpl(inst);
-  s_instances[inst] = ptr;
-
-  if (inst < (sizeof(s_fast_instances) / sizeof(s_fast_instances[0]))) {
-    s_fast_instances[inst] = ptr;
+  int inst = 0;
+  for (; inst < kNumInstances; ++inst) {
+    if (!s_instances[inst]) {
+      s_instances[inst] = new InstanceImpl(inst);
+      return inst;
+    }
   }
-
-  return static_cast<int>(inst);
+  return -1;
 }
 
 void InstanceImpl::Destroy(int inst) {
   std::scoped_lock lock(s_mutex);
-  if (inst < 0 || static_cast<unsigned int>(inst) >= s_instances.size()) return;
-
-  if (static_cast<unsigned int>(inst) <
-      (sizeof(s_fast_instances) / sizeof(s_fast_instances[0]))) {
-    s_fast_instances[inst] = nullptr;
+  if (inst < 0 || inst >= kNumInstances) {
+    return;
   }
 
-  delete s_instances[inst];
-  s_instances.erase(inst);
+  InstanceImpl* ptr = nullptr;
+  s_instances[inst].exchange(ptr);
+  delete ptr;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.h b/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.h
index 6e732d8..22d94f5 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/InstanceImpl.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_INSTANCEIMPL_H_
 #define NTCORE_INSTANCEIMPL_H_
@@ -11,7 +8,6 @@
 #include <atomic>
 #include <memory>
 
-#include <wpi/UidVector.h>
 #include <wpi/mutex.h>
 
 #include "ConnectionNotifier.h"
@@ -50,8 +46,8 @@
   static int AllocImpl();
 
   static std::atomic<int> s_default;
-  static std::atomic<InstanceImpl*> s_fast_instances[10];
-  static wpi::UidVector<InstanceImpl*, 10> s_instances;
+  static constexpr int kNumInstances = 16;
+  static std::atomic<InstanceImpl*> s_instances[kNumInstances];
   static wpi::mutex s_mutex;
 };
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Log.h b/third_party/allwpilib/ntcore/src/main/native/cpp/Log.h
index eba8f04..d3066d7 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Log.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Log.h
@@ -1,26 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_LOG_H_
 #define NTCORE_LOG_H_
 
 #include <wpi/Logger.h>
 
-#define LOG(level, x) WPI_LOG(m_logger, level, x)
+#define LOG(level, format, ...) WPI_LOG(m_logger, level, format, __VA_ARGS__)
 
 #undef ERROR
-#define ERROR(x) WPI_ERROR(m_logger, x)
-#define WARNING(x) WPI_WARNING(m_logger, x)
-#define INFO(x) WPI_INFO(m_logger, x)
+#define ERROR(format, ...) WPI_ERROR(m_logger, format, __VA_ARGS__)
+#define WARNING(format, ...) WPI_WARNING(m_logger, format, __VA_ARGS__)
+#define INFO(format, ...) WPI_INFO(m_logger, format, __VA_ARGS__)
 
-#define DEBUG0(x) WPI_DEBUG(m_logger, x)
-#define DEBUG1(x) WPI_DEBUG1(m_logger, x)
-#define DEBUG2(x) WPI_DEBUG2(m_logger, x)
-#define DEBUG3(x) WPI_DEBUG3(m_logger, x)
-#define DEBUG4(x) WPI_DEBUG4(m_logger, x)
+#define DEBUG0(format, ...) WPI_DEBUG(m_logger, format, __VA_ARGS__)
+#define DEBUG1(format, ...) WPI_DEBUG1(m_logger, format, __VA_ARGS__)
+#define DEBUG2(format, ...) WPI_DEBUG2(m_logger, format, __VA_ARGS__)
+#define DEBUG3(format, ...) WPI_DEBUG3(m_logger, format, __VA_ARGS__)
+#define DEBUG4(format, ...) WPI_DEBUG4(m_logger, format, __VA_ARGS__)
 
 #endif  // NTCORE_LOG_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.cpp
index b2c6786..fcb59d6 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.cpp
@@ -1,45 +1,39 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "LoggerImpl.h"
 
-#include <wpi/Path.h>
-#include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
+#include <wpi/fs.h>
 
 using namespace nt;
 
 static void DefaultLogger(unsigned int level, const char* file,
                           unsigned int line, const char* msg) {
-  wpi::SmallString<128> buf;
-  wpi::raw_svector_ostream oss(buf);
   if (level == 20) {
-    oss << "NT: " << msg << '\n';
-    wpi::errs() << oss.str();
+    fmt::print(stderr, "NT: {}\n", msg);
     return;
   }
 
-  wpi::StringRef levelmsg;
-  if (level >= 50)
-    levelmsg = "CRITICAL: ";
-  else if (level >= 40)
-    levelmsg = "ERROR: ";
-  else if (level >= 30)
-    levelmsg = "WARNING: ";
-  else
+  std::string_view levelmsg;
+  if (level >= 50) {
+    levelmsg = "CRITICAL";
+  } else if (level >= 40) {
+    levelmsg = "ERROR";
+  } else if (level >= 30) {
+    levelmsg = "WARNING";
+  } else {
     return;
-  oss << "NT: " << levelmsg << msg << " (" << file << ':' << line << ")\n";
-  wpi::errs() << oss.str();
+  }
+  fmt::print(stderr, "NT: {}: {} ({}:{})\n", levelmsg, msg, file, line);
 }
 
 LoggerImpl::LoggerImpl(int inst) : m_inst(inst) {}
 
-void LoggerImpl::Start() { DoStart(m_inst); }
+void LoggerImpl::Start() {
+  DoStart(m_inst);
+}
 
 unsigned int LoggerImpl::Add(
     std::function<void(const LogMessage& msg)> callback, unsigned int min_level,
@@ -55,23 +49,27 @@
 
 unsigned int LoggerImpl::GetMinLevel() {
   auto thr = GetThread();
-  if (!thr) return NT_LOG_INFO;
+  if (!thr) {
+    return NT_LOG_INFO;
+  }
   unsigned int level = NT_LOG_INFO;
   for (size_t i = 0; i < thr->m_listeners.size(); ++i) {
     const auto& listener = thr->m_listeners[i];
-    if (listener && listener.min_level < level) level = listener.min_level;
+    if (listener && listener.min_level < level) {
+      level = listener.min_level;
+    }
   }
   return level;
 }
 
 void LoggerImpl::Log(unsigned int level, const char* file, unsigned int line,
                      const char* msg) {
-  // this is safe because it's null terminated and always the end
-  const char* filename = wpi::sys::path::filename(file).data();
+  auto filename = fs::path{file}.filename();
   {
     auto thr = GetThread();
-    if (!thr || thr->m_listeners.empty())
-      DefaultLogger(level, filename, line, msg);
+    if (!thr || thr->m_listeners.empty()) {
+      DefaultLogger(level, filename.string().c_str(), line, msg);
+    }
   }
-  Send(UINT_MAX, 0, level, filename, line, msg);
+  Send(UINT_MAX, 0, level, filename.string(), line, msg);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.h b/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.h
index 3ac0295..2b577c1 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/LoggerImpl.h
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_LOGGERIMPL_H_
 #define NTCORE_LOGGERIMPL_H_
 
-#include "CallbackManager.h"
+#include <utility>
+
+#include <wpi/CallbackManager.h>
+
 #include "Handle.h"
 #include "ntcore_cpp.h"
 
@@ -16,15 +16,17 @@
 
 namespace impl {
 
-struct LoggerListenerData
-    : public ListenerData<std::function<void(const LogMessage& msg)>> {
+struct LoggerListenerData : public wpi::CallbackListenerData<
+                                std::function<void(const LogMessage& msg)>> {
   LoggerListenerData() = default;
   LoggerListenerData(std::function<void(const LogMessage& msg)> callback_,
                      unsigned int min_level_, unsigned int max_level_)
-      : ListenerData(callback_), min_level(min_level_), max_level(max_level_) {}
+      : CallbackListenerData(callback_),
+        min_level(min_level_),
+        max_level(max_level_) {}
   LoggerListenerData(unsigned int poller_uid_, unsigned int min_level_,
                      unsigned int max_level_)
-      : ListenerData(poller_uid_),
+      : CallbackListenerData(poller_uid_),
         min_level(min_level_),
         max_level(max_level_) {}
 
@@ -33,9 +35,11 @@
 };
 
 class LoggerThread
-    : public CallbackThread<LoggerThread, LogMessage, LoggerListenerData> {
+    : public wpi::CallbackThread<LoggerThread, LogMessage, LoggerListenerData> {
  public:
-  explicit LoggerThread(int inst) : m_inst(inst) {}
+  LoggerThread(std::function<void()> on_start, std::function<void()> on_exit,
+               int inst)
+      : CallbackThread(std::move(on_start), std::move(on_exit)), m_inst(inst) {}
 
   bool Matches(const LoggerListenerData& listener, const LogMessage& data) {
     return data.level >= listener.min_level && data.level <= listener.max_level;
@@ -55,9 +59,9 @@
 
 }  // namespace impl
 
-class LoggerImpl : public CallbackManager<LoggerImpl, impl::LoggerThread> {
+class LoggerImpl : public wpi::CallbackManager<LoggerImpl, impl::LoggerThread> {
   friend class LoggerTest;
-  friend class CallbackManager<LoggerImpl, impl::LoggerThread>;
+  friend class wpi::CallbackManager<LoggerImpl, impl::LoggerThread>;
 
  public:
   explicit LoggerImpl(int inst);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Message.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/Message.cpp
index 576e444..61efed1 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Message.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Message.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Message.h"
 
@@ -20,7 +17,9 @@
 std::shared_ptr<Message> Message::Read(WireDecoder& decoder,
                                        GetEntryTypeFunc get_entry_type) {
   unsigned int msg_type = 0;
-  if (!decoder.Read8(&msg_type)) return nullptr;
+  if (!decoder.Read8(&msg_type)) {
+    return nullptr;
+  }
   auto msg =
       std::make_shared<Message>(static_cast<MsgType>(msg_type), private_init());
   switch (msg_type) {
@@ -28,17 +27,23 @@
       break;
     case kClientHello: {
       unsigned int proto_rev;
-      if (!decoder.Read16(&proto_rev)) return nullptr;
+      if (!decoder.Read16(&proto_rev)) {
+        return nullptr;
+      }
       msg->m_id = proto_rev;
       // This intentionally uses the provided proto_rev instead of
       // decoder.proto_rev().
       if (proto_rev >= 0x0300u) {
-        if (!decoder.ReadString(&msg->m_str)) return nullptr;
+        if (!decoder.ReadString(&msg->m_str)) {
+          return nullptr;
+        }
       }
       break;
     }
     case kProtoUnsup: {
-      if (!decoder.Read16(&msg->m_id)) return nullptr;  // proto rev
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;  // proto rev
+      }
       break;
     }
     case kServerHelloDone:
@@ -48,8 +53,12 @@
         decoder.set_error("received SERVER_HELLO in protocol < 3.0");
         return nullptr;
       }
-      if (!decoder.Read8(&msg->m_flags)) return nullptr;
-      if (!decoder.ReadString(&msg->m_str)) return nullptr;
+      if (!decoder.Read8(&msg->m_flags)) {
+        return nullptr;
+      }
+      if (!decoder.ReadString(&msg->m_str)) {
+        return nullptr;
+      }
       break;
     case kClientHelloDone:
       if (decoder.proto_rev() < 0x0300u) {
@@ -58,30 +67,50 @@
       }
       break;
     case kEntryAssign: {
-      if (!decoder.ReadString(&msg->m_str)) return nullptr;  // name
+      if (!decoder.ReadString(&msg->m_str)) {
+        return nullptr;  // name
+      }
       NT_Type type;
-      if (!decoder.ReadType(&type)) return nullptr;              // entry type
-      if (!decoder.Read16(&msg->m_id)) return nullptr;           // id
-      if (!decoder.Read16(&msg->m_seq_num_uid)) return nullptr;  // seq num
+      if (!decoder.ReadType(&type)) {
+        return nullptr;  // entry type
+      }
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;  // id
+      }
+      if (!decoder.Read16(&msg->m_seq_num_uid)) {
+        return nullptr;  // seq num
+      }
       if (decoder.proto_rev() >= 0x0300u) {
-        if (!decoder.Read8(&msg->m_flags)) return nullptr;  // flags
+        if (!decoder.Read8(&msg->m_flags)) {
+          return nullptr;  // flags
+        }
       }
       msg->m_value = decoder.ReadValue(type);
-      if (!msg->m_value) return nullptr;
+      if (!msg->m_value) {
+        return nullptr;
+      }
       break;
     }
     case kEntryUpdate: {
-      if (!decoder.Read16(&msg->m_id)) return nullptr;           // id
-      if (!decoder.Read16(&msg->m_seq_num_uid)) return nullptr;  // seq num
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;  // id
+      }
+      if (!decoder.Read16(&msg->m_seq_num_uid)) {
+        return nullptr;  // seq num
+      }
       NT_Type type;
       if (decoder.proto_rev() >= 0x0300u) {
-        if (!decoder.ReadType(&type)) return nullptr;
+        if (!decoder.ReadType(&type)) {
+          return nullptr;
+        }
       } else {
         type = get_entry_type(msg->m_id);
       }
-      WPI_DEBUG4(decoder.logger(), "update message data type: " << type);
+      WPI_DEBUG4(decoder.logger(), "update message data type: {}", type);
       msg->m_value = decoder.ReadValue(type);
-      if (!msg->m_value) return nullptr;
+      if (!msg->m_value) {
+        return nullptr;
+      }
       break;
     }
     case kFlagsUpdate: {
@@ -89,8 +118,12 @@
         decoder.set_error("received FLAGS_UPDATE in protocol < 3.0");
         return nullptr;
       }
-      if (!decoder.Read16(&msg->m_id)) return nullptr;
-      if (!decoder.Read8(&msg->m_flags)) return nullptr;
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;
+      }
+      if (!decoder.Read8(&msg->m_flags)) {
+        return nullptr;
+      }
       break;
     }
     case kEntryDelete: {
@@ -98,7 +131,9 @@
         decoder.set_error("received ENTRY_DELETE in protocol < 3.0");
         return nullptr;
       }
-      if (!decoder.Read16(&msg->m_id)) return nullptr;
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;
+      }
       break;
     }
     case kClearEntries: {
@@ -107,7 +142,9 @@
         return nullptr;
       }
       uint32_t magic;
-      if (!decoder.Read32(&magic)) return nullptr;
+      if (!decoder.Read32(&magic)) {
+        return nullptr;
+      }
       if (magic != kClearAllMagic) {
         decoder.set_error(
             "received incorrect CLEAR_ENTRIES magic value, ignoring");
@@ -120,13 +157,21 @@
         decoder.set_error("received EXECUTE_RPC in protocol < 3.0");
         return nullptr;
       }
-      if (!decoder.Read16(&msg->m_id)) return nullptr;
-      if (!decoder.Read16(&msg->m_seq_num_uid)) return nullptr;  // uid
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;
+      }
+      if (!decoder.Read16(&msg->m_seq_num_uid)) {
+        return nullptr;  // uid
+      }
       uint64_t size;
-      if (!decoder.ReadUleb128(&size)) return nullptr;
+      if (!decoder.ReadUleb128(&size)) {
+        return nullptr;
+      }
       const char* params;
-      if (!decoder.Read(&params, size)) return nullptr;
-      msg->m_str = wpi::StringRef(params, size);
+      if (!decoder.Read(&params, size)) {
+        return nullptr;
+      }
+      msg->m_str.assign(params, size);
       break;
     }
     case kRpcResponse: {
@@ -134,38 +179,46 @@
         decoder.set_error("received RPC_RESPONSE in protocol < 3.0");
         return nullptr;
       }
-      if (!decoder.Read16(&msg->m_id)) return nullptr;
-      if (!decoder.Read16(&msg->m_seq_num_uid)) return nullptr;  // uid
+      if (!decoder.Read16(&msg->m_id)) {
+        return nullptr;
+      }
+      if (!decoder.Read16(&msg->m_seq_num_uid)) {
+        return nullptr;  // uid
+      }
       uint64_t size;
-      if (!decoder.ReadUleb128(&size)) return nullptr;
+      if (!decoder.ReadUleb128(&size)) {
+        return nullptr;
+      }
       const char* results;
-      if (!decoder.Read(&results, size)) return nullptr;
-      msg->m_str = wpi::StringRef(results, size);
+      if (!decoder.Read(&results, size)) {
+        return nullptr;
+      }
+      msg->m_str.assign(results, size);
       break;
     }
     default:
       decoder.set_error("unrecognized message type");
-      WPI_INFO(decoder.logger(), "unrecognized message type: " << msg_type);
+      WPI_INFO(decoder.logger(), "unrecognized message type: {}", msg_type);
       return nullptr;
   }
   return msg;
 }
 
-std::shared_ptr<Message> Message::ClientHello(wpi::StringRef self_id) {
+std::shared_ptr<Message> Message::ClientHello(std::string_view self_id) {
   auto msg = std::make_shared<Message>(kClientHello, private_init());
   msg->m_str = self_id;
   return msg;
 }
 
 std::shared_ptr<Message> Message::ServerHello(unsigned int flags,
-                                              wpi::StringRef self_id) {
+                                              std::string_view self_id) {
   auto msg = std::make_shared<Message>(kServerHello, private_init());
   msg->m_str = self_id;
   msg->m_flags = flags;
   return msg;
 }
 
-std::shared_ptr<Message> Message::EntryAssign(wpi::StringRef name,
+std::shared_ptr<Message> Message::EntryAssign(std::string_view name,
                                               unsigned int id,
                                               unsigned int seq_num,
                                               std::shared_ptr<Value> value,
@@ -204,7 +257,7 @@
 }
 
 std::shared_ptr<Message> Message::ExecuteRpc(unsigned int id, unsigned int uid,
-                                             wpi::StringRef params) {
+                                             std::string_view params) {
   auto msg = std::make_shared<Message>(kExecuteRpc, private_init());
   msg->m_str = params;
   msg->m_id = id;
@@ -213,7 +266,7 @@
 }
 
 std::shared_ptr<Message> Message::RpcResponse(unsigned int id, unsigned int uid,
-                                              wpi::StringRef result) {
+                                              std::string_view result) {
   auto msg = std::make_shared<Message>(kRpcResponse, private_init());
   msg->m_str = result;
   msg->m_id = id;
@@ -229,7 +282,9 @@
     case kClientHello:
       encoder.Write8(kClientHello);
       encoder.Write16(encoder.proto_rev());
-      if (encoder.proto_rev() < 0x0300u) return;
+      if (encoder.proto_rev() < 0x0300u) {
+        return;
+      }
       encoder.WriteString(m_str);
       break;
     case kProtoUnsup:
@@ -240,13 +295,17 @@
       encoder.Write8(kServerHelloDone);
       break;
     case kServerHello:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kServerHello);
       encoder.Write8(m_flags);
       encoder.WriteString(m_str);
       break;
     case kClientHelloDone:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kClientHelloDone);
       break;
     case kEntryAssign:
@@ -255,41 +314,55 @@
       encoder.WriteType(m_value->type());
       encoder.Write16(m_id);
       encoder.Write16(m_seq_num_uid);
-      if (encoder.proto_rev() >= 0x0300u) encoder.Write8(m_flags);
+      if (encoder.proto_rev() >= 0x0300u) {
+        encoder.Write8(m_flags);
+      }
       encoder.WriteValue(*m_value);
       break;
     case kEntryUpdate:
       encoder.Write8(kEntryUpdate);
       encoder.Write16(m_id);
       encoder.Write16(m_seq_num_uid);
-      if (encoder.proto_rev() >= 0x0300u) encoder.WriteType(m_value->type());
+      if (encoder.proto_rev() >= 0x0300u) {
+        encoder.WriteType(m_value->type());
+      }
       encoder.WriteValue(*m_value);
       break;
     case kFlagsUpdate:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kFlagsUpdate);
       encoder.Write16(m_id);
       encoder.Write8(m_flags);
       break;
     case kEntryDelete:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kEntryDelete);
       encoder.Write16(m_id);
       break;
     case kClearEntries:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kClearEntries);
       encoder.Write32(kClearAllMagic);
       break;
     case kExecuteRpc:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kExecuteRpc);
       encoder.Write16(m_id);
       encoder.Write16(m_seq_num_uid);
       encoder.WriteString(m_str);
       break;
     case kRpcResponse:
-      if (encoder.proto_rev() < 0x0300u) return;  // new message in version 3.0
+      if (encoder.proto_rev() < 0x0300u) {
+        return;  // new message in version 3.0
+      }
       encoder.Write8(kRpcResponse);
       encoder.Write16(m_id);
       encoder.Write16(m_seq_num_uid);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Message.h b/third_party/allwpilib/ntcore/src/main/native/cpp/Message.h
index 9b25abf..ec34a75 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Message.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Message.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MESSAGE_H_
 #define NTCORE_MESSAGE_H_
@@ -11,6 +8,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include "networktables/NetworkTableValue.h"
 
@@ -39,18 +37,17 @@
     kExecuteRpc = 0x20,
     kRpcResponse = 0x21
   };
-  typedef std::function<NT_Type(unsigned int id)> GetEntryTypeFunc;
+  using GetEntryTypeFunc = std::function<NT_Type(unsigned int id)>;
 
-  Message() : m_type(kUnknown), m_id(0), m_flags(0), m_seq_num_uid(0) {}
-  Message(MsgType type, const private_init&)
-      : m_type(type), m_id(0), m_flags(0), m_seq_num_uid(0) {}
+  Message() = default;
+  Message(MsgType type, const private_init&) : m_type(type) {}
 
   MsgType type() const { return m_type; }
   bool Is(MsgType type) const { return type == m_type; }
 
   // Message data accessors.  Callers are responsible for knowing what data is
   // actually provided for a particular message.
-  wpi::StringRef str() const { return m_str; }
+  std::string_view str() const { return m_str; }
   std::shared_ptr<Value> value() const { return m_value; }
   unsigned int id() const { return m_id; }
   unsigned int flags() const { return m_flags; }
@@ -79,10 +76,10 @@
   }
 
   // Create messages with data
-  static std::shared_ptr<Message> ClientHello(wpi::StringRef self_id);
+  static std::shared_ptr<Message> ClientHello(std::string_view self_id);
   static std::shared_ptr<Message> ServerHello(unsigned int flags,
-                                              wpi::StringRef self_id);
-  static std::shared_ptr<Message> EntryAssign(wpi::StringRef name,
+                                              std::string_view self_id);
+  static std::shared_ptr<Message> EntryAssign(std::string_view name,
                                               unsigned int id,
                                               unsigned int seq_num,
                                               std::shared_ptr<Value> value,
@@ -94,22 +91,22 @@
                                               unsigned int flags);
   static std::shared_ptr<Message> EntryDelete(unsigned int id);
   static std::shared_ptr<Message> ExecuteRpc(unsigned int id, unsigned int uid,
-                                             wpi::StringRef params);
+                                             std::string_view params);
   static std::shared_ptr<Message> RpcResponse(unsigned int id, unsigned int uid,
-                                              wpi::StringRef result);
+                                              std::string_view result);
 
   Message(const Message&) = delete;
   Message& operator=(const Message&) = delete;
 
  private:
-  MsgType m_type;
+  MsgType m_type{kUnknown};
 
   // Message data.  Use varies by message type.
   std::string m_str;
   std::shared_ptr<Value> m_value;
-  unsigned int m_id;  // also used for proto_rev
-  unsigned int m_flags;
-  unsigned int m_seq_num_uid;
+  unsigned int m_id{0};  // also used for proto_rev
+  unsigned int m_flags{0};
+  unsigned int m_seq_num_uid{0};
 };
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.cpp
index 281b638..838eecb 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "NetworkConnection.h"
 
+#include <utility>
+
 #include <wpi/NetworkStream.h>
 #include <wpi/raw_socket_istream.h>
 #include <wpi/timestamp.h>
@@ -28,8 +27,8 @@
       m_stream(std::move(stream)),
       m_notifier(notifier),
       m_logger(logger),
-      m_handshake(handshake),
-      m_get_entry_type(get_entry_type),
+      m_handshake(std::move(handshake)),
+      m_get_entry_type(std::move(get_entry_type)),
       m_state(kCreated) {
   m_active = false;
   m_proto_rev = 0x0300;
@@ -39,14 +38,20 @@
   m_stream->setNoDelay();
 }
 
-NetworkConnection::~NetworkConnection() { Stop(); }
+NetworkConnection::~NetworkConnection() {
+  Stop();
+}
 
 void NetworkConnection::Start() {
-  if (m_active) return;
+  if (m_active) {
+    return;
+  }
   m_active = true;
   set_state(kInit);
   // clear queue
-  while (!m_outgoing.empty()) m_outgoing.pop();
+  while (!m_outgoing.empty()) {
+    m_outgoing.pop();
+  }
   // reset shutdown flags
   {
     std::scoped_lock lock(m_shutdown_mutex);
@@ -59,11 +64,13 @@
 }
 
 void NetworkConnection::Stop() {
-  DEBUG2("NetworkConnection stopping (" << this << ")");
+  DEBUG2("NetworkConnection stopping ({})", fmt::ptr(this));
   set_state(kDead);
   m_active = false;
   // closing the stream so the read thread terminates
-  if (m_stream) m_stream->close();
+  if (m_stream) {
+    m_stream->close();
+  }
   // send an empty outgoing message set so the write thread terminates
   m_outgoing.push(Outgoing());
   // wait for threads to terminate, with timeout
@@ -72,32 +79,38 @@
     auto timeout_time =
         std::chrono::steady_clock::now() + std::chrono::milliseconds(200);
     if (m_write_shutdown_cv.wait_until(lock, timeout_time,
-                                       [&] { return m_write_shutdown; }))
+                                       [&] { return m_write_shutdown; })) {
       m_write_thread.join();
-    else
+    } else {
       m_write_thread.detach();  // timed out, detach it
+    }
   }
   if (m_read_thread.joinable()) {
     std::unique_lock lock(m_shutdown_mutex);
     auto timeout_time =
         std::chrono::steady_clock::now() + std::chrono::milliseconds(200);
     if (m_read_shutdown_cv.wait_until(lock, timeout_time,
-                                      [&] { return m_read_shutdown; }))
+                                      [&] { return m_read_shutdown; })) {
       m_read_thread.join();
-    else
+    } else {
       m_read_thread.detach();  // timed out, detach it
+    }
   }
   // clear queue
-  while (!m_outgoing.empty()) m_outgoing.pop();
+  while (!m_outgoing.empty()) {
+    m_outgoing.pop();
+  }
 }
 
 ConnectionInfo NetworkConnection::info() const {
-  return ConnectionInfo{remote_id(), m_stream->getPeerIP(),
+  return ConnectionInfo{remote_id(), std::string{m_stream->getPeerIP()},
                         static_cast<unsigned int>(m_stream->getPeerPort()),
                         m_last_update, m_proto_rev};
 }
 
-unsigned int NetworkConnection::proto_rev() const { return m_proto_rev; }
+unsigned int NetworkConnection::proto_rev() const {
+  return m_proto_rev;
+}
 
 void NetworkConnection::set_proto_rev(unsigned int proto_rev) {
   m_proto_rev = proto_rev;
@@ -111,12 +124,16 @@
 void NetworkConnection::set_state(State state) {
   std::scoped_lock lock(m_state_mutex);
   // Don't update state any more once we've died
-  if (m_state == kDead) return;
+  if (m_state == kDead) {
+    return;
+  }
   // One-shot notify state changes
-  if (m_state != kActive && state == kActive)
+  if (m_state != kActive && state == kActive) {
     m_notifier.NotifyConnection(true, info());
-  if (m_state != kDead && state == kDead)
+  }
+  if (m_state != kDead && state == kDead) {
     m_notifier.NotifyConnection(false, info());
+  }
   m_state = state;
 }
 
@@ -125,7 +142,7 @@
   return m_remote_id;
 }
 
-void NetworkConnection::set_remote_id(StringRef remote_id) {
+void NetworkConnection::set_remote_id(std::string_view remote_id) {
   std::scoped_lock lock(m_remote_id_mutex);
   m_remote_id = remote_id;
 }
@@ -140,12 +157,14 @@
           [&] {
             decoder.set_proto_rev(m_proto_rev);
             auto msg = Message::Read(decoder, m_get_entry_type);
-            if (!msg && decoder.error())
-              DEBUG0("error reading in handshake: " << decoder.error());
+            if (!msg && decoder.error()) {
+              DEBUG0("error reading in handshake: {}", decoder.error());
+            }
             return msg;
           },
-          [&](wpi::ArrayRef<std::shared_ptr<Message>> msgs) {
-            m_outgoing.emplace(msgs);
+          [&](auto msgs) {
+            m_outgoing.emplace(std::vector<std::shared_ptr<Message>>(
+                msgs.begin(), msgs.end()));
           })) {
     set_state(kDead);
     m_active = false;
@@ -154,23 +173,28 @@
 
   set_state(kActive);
   while (m_active) {
-    if (!m_stream) break;
+    if (!m_stream) {
+      break;
+    }
     decoder.set_proto_rev(m_proto_rev);
     decoder.Reset();
     auto msg = Message::Read(decoder, m_get_entry_type);
     if (!msg) {
-      if (decoder.error()) INFO("read error: " << decoder.error());
+      if (decoder.error()) {
+        INFO("read error: {}", decoder.error());
+      }
       // terminate connection on bad message
-      if (m_stream) m_stream->close();
+      if (m_stream) {
+        m_stream->close();
+      }
       break;
     }
-    DEBUG3("received type=" << msg->type() << " with str=" << msg->str()
-                            << " id=" << msg->id()
-                            << " seq_num=" << msg->seq_num_uid());
+    DEBUG3("received type={} with str={} id={} seq_num={}", msg->type(),
+           msg->str(), msg->id(), msg->seq_num_uid());
     m_last_update = Now();
     m_process_incoming(std::move(msg), this);
   }
-  DEBUG2("read thread died (" << this << ")");
+  DEBUG2("read thread died ({})", fmt::ptr(this));
   set_state(kDead);
   m_active = false;
   m_outgoing.push(Outgoing());  // also kill write thread
@@ -189,29 +213,38 @@
 
   while (m_active) {
     auto msgs = m_outgoing.pop();
-    DEBUG4("write thread woke up");
-    if (msgs.empty()) continue;
+    DEBUG4("{}", "write thread woke up");
+    if (msgs.empty()) {
+      continue;
+    }
     encoder.set_proto_rev(m_proto_rev);
     encoder.Reset();
-    DEBUG3("sending " << msgs.size() << " messages");
+    DEBUG3("sending {} messages", msgs.size());
     for (auto& msg : msgs) {
       if (msg) {
-        DEBUG3("sending type=" << msg->type() << " with str=" << msg->str()
-                               << " id=" << msg->id()
-                               << " seq_num=" << msg->seq_num_uid());
+        DEBUG3("sending type={} with str={} id={} seq_num={}", msg->type(),
+               msg->str(), msg->id(), msg->seq_num_uid());
         msg->Write(encoder);
       }
     }
     wpi::NetworkStream::Error err;
-    if (!m_stream) break;
-    if (encoder.size() == 0) continue;
-    if (m_stream->send(encoder.data(), encoder.size(), &err) == 0) break;
-    DEBUG4("sent " << encoder.size() << " bytes");
+    if (!m_stream) {
+      break;
+    }
+    if (encoder.size() == 0) {
+      continue;
+    }
+    if (m_stream->send(encoder.data(), encoder.size(), &err) == 0) {
+      break;
+    }
+    DEBUG4("sent {} bytes", encoder.size());
   }
-  DEBUG2("write thread died (" << this << ")");
+  DEBUG2("write thread died ({})", fmt::ptr(this));
   set_state(kDead);
   m_active = false;
-  if (m_stream) m_stream->close();  // also kill read thread
+  if (m_stream) {
+    m_stream->close();  // also kill read thread
+  }
 
   // use condition variable to signal thread shutdown
   {
@@ -249,7 +282,9 @@
         // new, but remember it
         size_t pos = m_pending_outgoing.size();
         m_pending_outgoing.push_back(msg);
-        if (id >= m_pending_update.size()) m_pending_update.resize(id + 1);
+        if (id >= m_pending_update.size()) {
+          m_pending_update.resize(id + 1);
+        }
         m_pending_update[id].first = pos + 1;
       }
       break;
@@ -292,7 +327,9 @@
         // new, but remember it
         size_t pos = m_pending_outgoing.size();
         m_pending_outgoing.push_back(msg);
-        if (id >= m_pending_update.size()) m_pending_update.resize(id + 1);
+        if (id >= m_pending_update.size()) {
+          m_pending_update.resize(id + 1);
+        }
         m_pending_update[id].second = pos + 1;
       }
       break;
@@ -300,12 +337,15 @@
     case Message::kClearEntries: {
       // knock out all previous assigns/updates!
       for (auto& i : m_pending_outgoing) {
-        if (!i) continue;
+        if (!i) {
+          continue;
+        }
         auto t = i->type();
         if (t == Message::kEntryAssign || t == Message::kEntryUpdate ||
             t == Message::kFlagsUpdate || t == Message::kEntryDelete ||
-            t == Message::kClearEntries)
+            t == Message::kClearEntries) {
           i.reset();
+        }
       }
       m_pending_update.resize(0);
       m_pending_outgoing.push_back(msg);
@@ -321,9 +361,13 @@
   std::scoped_lock lock(m_pending_mutex);
   auto now = std::chrono::steady_clock::now();
   if (m_pending_outgoing.empty()) {
-    if (!keep_alive) return;
+    if (!keep_alive) {
+      return;
+    }
     // send keep-alives once a second (if no other messages have been sent)
-    if ((now - m_last_post) < std::chrono::seconds(1)) return;
+    if ((now - m_last_post) < std::chrono::seconds(1)) {
+      return;
+    }
     m_outgoing.emplace(Outgoing{Message::KeepAlive()});
   } else {
     m_outgoing.emplace(std::move(m_pending_outgoing));
@@ -331,4 +375,4 @@
     m_pending_update.resize(0);
   }
   m_last_post = now;
-}
+}  // NOLINT
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.h b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.h
index 91ad64e..59f18ff 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/NetworkConnection.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKCONNECTION_H_
 #define NTCORE_NETWORKCONNECTION_H_
@@ -14,6 +11,7 @@
 #include <chrono>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <utility>
 #include <vector>
@@ -21,6 +19,7 @@
 #include <wpi/ConcurrentQueue.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
+#include <wpi/span.h>
 
 #include "INetworkConnection.h"
 #include "Message.h"
@@ -37,23 +36,21 @@
 
 class NetworkConnection : public INetworkConnection {
  public:
-  typedef std::function<bool(
+  using HandshakeFunc = std::function<bool(
       NetworkConnection& conn,
       std::function<std::shared_ptr<Message>()> get_msg,
-      std::function<void(wpi::ArrayRef<std::shared_ptr<Message>>)> send_msgs)>
-      HandshakeFunc;
-  typedef std::function<void(std::shared_ptr<Message> msg,
-                             NetworkConnection* conn)>
-      ProcessIncomingFunc;
-  typedef std::vector<std::shared_ptr<Message>> Outgoing;
-  typedef wpi::ConcurrentQueue<Outgoing> OutgoingQueue;
+      std::function<void(wpi::span<std::shared_ptr<Message>>)> send_msgs)>;
+  using ProcessIncomingFunc =
+      std::function<void(std::shared_ptr<Message>, NetworkConnection*)>;
+  using Outgoing = std::vector<std::shared_ptr<Message>>;
+  using OutgoingQueue = wpi::ConcurrentQueue<Outgoing>;
 
   NetworkConnection(unsigned int uid,
                     std::unique_ptr<wpi::NetworkStream> stream,
                     IConnectionNotifier& notifier, wpi::Logger& logger,
                     HandshakeFunc handshake,
                     Message::GetEntryTypeFunc get_entry_type);
-  ~NetworkConnection();
+  ~NetworkConnection() override;
 
   // Set the input processor function.  This must be called before Start().
   void set_process_incoming(ProcessIncomingFunc func) {
@@ -63,24 +60,24 @@
   void Start();
   void Stop();
 
-  ConnectionInfo info() const override;
+  ConnectionInfo info() const final;
 
   bool active() const { return m_active; }
   wpi::NetworkStream& stream() { return *m_stream; }
 
-  void QueueOutgoing(std::shared_ptr<Message> msg) override;
+  void QueueOutgoing(std::shared_ptr<Message> msg) final;
   void PostOutgoing(bool keep_alive) override;
 
   unsigned int uid() const { return m_uid; }
 
-  unsigned int proto_rev() const override;
-  void set_proto_rev(unsigned int proto_rev) override;
+  unsigned int proto_rev() const final;
+  void set_proto_rev(unsigned int proto_rev) final;
 
-  State state() const override;
-  void set_state(State state) override;
+  State state() const final;
+  void set_state(State state) final;
 
   std::string remote_id() const;
-  void set_remote_id(StringRef remote_id);
+  void set_remote_id(std::string_view remote_id);
 
   uint64_t last_update() const { return m_last_update; }
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.cpp
index 0c6811c..b4bf96a 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RpcServer.h"
 
@@ -12,7 +9,9 @@
 RpcServer::RpcServer(int inst, wpi::Logger& logger)
     : m_inst(inst), m_logger(logger) {}
 
-void RpcServer::Start() { DoStart(m_inst, m_logger); }
+void RpcServer::Start() {
+  DoStart(m_inst, m_logger);
+}
 
 unsigned int RpcServer::Add(
     std::function<void(const RpcAnswer& answer)> callback) {
@@ -23,10 +22,12 @@
   return DoAdd(poller_uid);
 }
 
-void RpcServer::RemoveRpc(unsigned int rpc_uid) { Remove(rpc_uid); }
+void RpcServer::RemoveRpc(unsigned int rpc_uid) {
+  Remove(rpc_uid);
+}
 
 void RpcServer::ProcessRpc(unsigned int local_id, unsigned int call_uid,
-                           StringRef name, StringRef params,
+                           std::string_view name, std::string_view params,
                            const ConnectionInfo& conn,
                            SendResponseFunc send_response,
                            unsigned int rpc_uid) {
@@ -36,11 +37,12 @@
 }
 
 bool RpcServer::PostRpcResponse(unsigned int local_id, unsigned int call_uid,
-                                wpi::StringRef result) {
+                                std::string_view result) {
   auto thr = GetThread();
   auto i = thr->m_response_map.find(impl::RpcIdPair{local_id, call_uid});
   if (i == thr->m_response_map.end()) {
-    WARNING("posting RPC response to nonexistent call (or duplicate response)");
+    WARNING("{}",
+            "posting RPC response to nonexistent call (or duplicate response)");
     return false;
   }
   (i->getSecond())(result);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.h b/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.h
index cca490f..b8ae6b7 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/RpcServer.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_RPCSERVER_H_
 #define NTCORE_RPCSERVER_H_
 
 #include <utility>
 
+#include <wpi/CallbackManager.h>
 #include <wpi/DenseMap.h>
 #include <wpi/mutex.h>
 
-#include "CallbackManager.h"
 #include "Handle.h"
 #include "IRpcServer.h"
 #include "Log.h"
@@ -22,27 +19,30 @@
 
 namespace impl {
 
-typedef std::pair<unsigned int, unsigned int> RpcIdPair;
+using RpcIdPair = std::pair<unsigned int, unsigned int>;
 
 struct RpcNotifierData : public RpcAnswer {
-  RpcNotifierData(NT_Entry entry_, NT_RpcCall call_, StringRef name_,
-                  StringRef params_, const ConnectionInfo& conn_,
+  RpcNotifierData(NT_Entry entry_, NT_RpcCall call_, std::string_view name_,
+                  std::string_view params_, const ConnectionInfo& conn_,
                   IRpcServer::SendResponseFunc send_response_)
       : RpcAnswer{entry_, call_, name_, params_, conn_},
-        send_response{send_response_} {}
+        send_response{std::move(send_response_)} {}
 
   IRpcServer::SendResponseFunc send_response;
 };
 
 using RpcListenerData =
-    ListenerData<std::function<void(const RpcAnswer& answer)>>;
+    wpi::CallbackListenerData<std::function<void(const RpcAnswer& answer)>>;
 
 class RpcServerThread
-    : public CallbackThread<RpcServerThread, RpcAnswer, RpcListenerData,
-                            RpcNotifierData> {
+    : public wpi::CallbackThread<RpcServerThread, RpcAnswer, RpcListenerData,
+                                 RpcNotifierData> {
  public:
-  RpcServerThread(int inst, wpi::Logger& logger)
-      : m_inst(inst), m_logger(logger) {}
+  RpcServerThread(std::function<void()> on_start, std::function<void()> on_exit,
+                  int inst, wpi::Logger& logger)
+      : CallbackThread(std::move(on_start), std::move(on_exit)),
+        m_inst(inst),
+        m_logger(logger) {}
 
   bool Matches(const RpcListenerData& /*listener*/,
                const RpcNotifierData& data) {
@@ -58,7 +58,7 @@
 
   void DoCallback(std::function<void(const RpcAnswer& call)> callback,
                   const RpcNotifierData& data) {
-    DEBUG4("rpc calling " << data.name);
+    DEBUG4("rpc calling {}", data.name);
     unsigned int local_id = Handle{data.entry}.GetIndex();
     unsigned int call_uid = Handle{data.call}.GetIndex();
     RpcIdPair lookup_uid{local_id, call_uid};
@@ -81,10 +81,11 @@
 
 }  // namespace impl
 
-class RpcServer : public IRpcServer,
-                  public CallbackManager<RpcServer, impl::RpcServerThread> {
+class RpcServer
+    : public IRpcServer,
+      public wpi::CallbackManager<RpcServer, impl::RpcServerThread> {
   friend class RpcServerTest;
-  friend class CallbackManager<RpcServer, impl::RpcServerThread>;
+  friend class wpi::CallbackManager<RpcServer, impl::RpcServerThread>;
 
  public:
   RpcServer(int inst, wpi::Logger& logger);
@@ -95,13 +96,13 @@
   unsigned int AddPolled(unsigned int poller_uid);
   void RemoveRpc(unsigned int rpc_uid) override;
 
-  void ProcessRpc(unsigned int local_id, unsigned int call_uid, StringRef name,
-                  StringRef params, const ConnectionInfo& conn,
-                  SendResponseFunc send_response,
+  void ProcessRpc(unsigned int local_id, unsigned int call_uid,
+                  std::string_view name, std::string_view params,
+                  const ConnectionInfo& conn, SendResponseFunc send_response,
                   unsigned int rpc_uid) override;
 
   bool PostRpcResponse(unsigned int local_id, unsigned int call_uid,
-                       wpi::StringRef result);
+                       std::string_view result);
 
  private:
   int m_inst;
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.cpp
index 6d61331..e25e636 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.cpp
@@ -1,30 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SequenceNumber.h"
 
 namespace nt {
 
 bool operator<(const SequenceNumber& lhs, const SequenceNumber& rhs) {
-  if (lhs.m_value < rhs.m_value)
+  if (lhs.m_value < rhs.m_value) {
     return (rhs.m_value - lhs.m_value) < (1u << 15);
-  else if (lhs.m_value > rhs.m_value)
+  } else if (lhs.m_value > rhs.m_value) {
     return (lhs.m_value - rhs.m_value) > (1u << 15);
-  else
+  } else {
     return false;
+  }
 }
 
 bool operator>(const SequenceNumber& lhs, const SequenceNumber& rhs) {
-  if (lhs.m_value < rhs.m_value)
+  if (lhs.m_value < rhs.m_value) {
     return (rhs.m_value - lhs.m_value) > (1u << 15);
-  else if (lhs.m_value > rhs.m_value)
+  } else if (lhs.m_value > rhs.m_value) {
     return (lhs.m_value - rhs.m_value) < (1u << 15);
-  else
+  } else {
     return false;
+  }
 }
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.h b/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.h
index 11d9953..719d85c 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/SequenceNumber.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_SEQUENCENUMBER_H_
 #define NTCORE_SEQUENCENUMBER_H_
@@ -13,13 +10,15 @@
 /* A sequence number per RFC 1982 */
 class SequenceNumber {
  public:
-  SequenceNumber() : m_value(0) {}
+  SequenceNumber() = default;
   explicit SequenceNumber(unsigned int value) : m_value(value) {}
   unsigned int value() const { return m_value; }
 
   SequenceNumber& operator++() {
     ++m_value;
-    if (m_value > 0xffff) m_value = 0;
+    if (m_value > 0xffff) {
+      m_value = 0;
+    }
     return *this;
   }
   SequenceNumber operator++(int) {
@@ -36,7 +35,7 @@
   friend bool operator!=(const SequenceNumber& lhs, const SequenceNumber& rhs);
 
  private:
-  unsigned int m_value;
+  unsigned int m_value{0};
 };
 
 bool operator<(const SequenceNumber& lhs, const SequenceNumber& rhs);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.cpp
index dadb8e7..dadc3f6 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.cpp
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Storage.h"
 
+#include <wpi/StringExtras.h>
 #include <wpi/timestamp.h>
 
 #include "Handle.h"
@@ -35,13 +33,19 @@
   m_server = server;
 }
 
-void Storage::ClearDispatcher() { m_dispatcher = nullptr; }
+void Storage::ClearDispatcher() {
+  m_dispatcher = nullptr;
+}
 
 NT_Type Storage::GetMessageEntryType(unsigned int id) const {
   std::scoped_lock lock(m_mutex);
-  if (id >= m_idmap.size()) return NT_UNASSIGNED;
+  if (id >= m_idmap.size()) {
+    return NT_UNASSIGNED;
+  }
   Entry* entry = m_idmap[id];
-  if (!entry || !entry->value) return NT_UNASSIGNED;
+  if (!entry || !entry->value) {
+    return NT_UNASSIGNED;
+  }
   return entry->value->type();
 }
 
@@ -88,7 +92,7 @@
                                          INetworkConnection* conn) {
   std::unique_lock lock(m_mutex);
   unsigned int id = msg->id();
-  StringRef name = msg->str();
+  std::string_view name = msg->str();
   Entry* entry;
   bool may_need_update = false;
   SequenceNumber seq_num(msg->seq_num_uid());
@@ -99,7 +103,9 @@
     if (id == 0xffff) {
       entry = GetOrNew(name);
       // see if it was already assigned; ignore if so.
-      if (entry->id != 0xffff) return;
+      if (entry->id != 0xffff) {
+        return;
+      }
 
       entry->flags = msg->flags();
       entry->seq_num = seq_num;
@@ -110,7 +116,7 @@
       // ignore arbitrary entry assignments
       // this can happen due to e.g. assignment to deleted entry
       lock.unlock();
-      DEBUG0("server: received assignment to unknown entry");
+      DEBUG0("{}", "server: received assignment to unknown entry");
       return;
     }
     entry = m_idmap[id];
@@ -118,10 +124,12 @@
     // clients simply accept new assignments
     if (id == 0xffff) {
       lock.unlock();
-      DEBUG0("client: received entry assignment request?");
+      DEBUG0("{}", "client: received entry assignment request?");
       return;
     }
-    if (id >= m_idmap.size()) m_idmap.resize(id + 1);
+    if (id >= m_idmap.size()) {
+      m_idmap.resize(id + 1);
+    }
     entry = m_idmap[id];
     if (!entry) {
       // create local
@@ -171,7 +179,7 @@
   // sanity check: name should match id
   if (msg->str() != entry->name) {
     lock.unlock();
-    DEBUG0("entry assignment for same id with different name?");
+    DEBUG0("{}", "entry assignment for same id with different name?");
     return;
   }
 
@@ -181,15 +189,19 @@
   // don't update flags if this is a server response to a client id request
   if (!may_need_update && conn->proto_rev() >= 0x0300) {
     // update persistent dirty flag if persistent flag changed
-    if ((entry->flags & NT_PERSISTENT) != (msg->flags() & NT_PERSISTENT))
+    if ((entry->flags & NT_PERSISTENT) != (msg->flags() & NT_PERSISTENT)) {
       m_persistent_dirty = true;
-    if (entry->flags != msg->flags()) notify_flags |= NT_NOTIFY_FLAGS;
+    }
+    if (entry->flags != msg->flags()) {
+      notify_flags |= NT_NOTIFY_FLAGS;
+    }
     entry->flags = msg->flags();
   }
 
   // update persistent dirty flag if the value changed and it's persistent
-  if (entry->IsPersistent() && *entry->value != *msg->value())
+  if (entry->IsPersistent() && *entry->value != *msg->value()) {
     m_persistent_dirty = true;
+  }
 
   // update local
   entry->value = msg->value();
@@ -217,21 +229,25 @@
     // ignore arbitrary entry updates;
     // this can happen due to deleted entries
     lock.unlock();
-    DEBUG0("received update to unknown entry");
+    DEBUG0("{}", "received update to unknown entry");
     return;
   }
   Entry* entry = m_idmap[id];
 
   // ignore if sequence number not higher than local
   SequenceNumber seq_num(msg->seq_num_uid());
-  if (seq_num <= entry->seq_num) return;
+  if (seq_num <= entry->seq_num) {
+    return;
+  }
 
   // update local
   entry->value = msg->value();
   entry->seq_num = seq_num;
 
   // update persistent dirty flag if it's a persistent value
-  if (entry->IsPersistent()) m_persistent_dirty = true;
+  if (entry->IsPersistent()) {
+    m_persistent_dirty = true;
+  }
 
   // notify
   m_notifier.NotifyEntry(entry->local_id, entry->name, entry->value,
@@ -254,7 +270,7 @@
     // ignore arbitrary entry updates;
     // this can happen due to deleted entries
     lock.unlock();
-    DEBUG0("received flags update to unknown entry");
+    DEBUG0("{}", "received flags update to unknown entry");
     return;
   }
 
@@ -278,7 +294,7 @@
     // ignore arbitrary entry updates;
     // this can happen due to deleted entries
     lock.unlock();
-    DEBUG0("received delete to unknown entry");
+    DEBUG0("{}", "received delete to unknown entry");
     return;
   }
 
@@ -313,19 +329,21 @@
     std::shared_ptr<Message> msg, INetworkConnection* /*conn*/,
     std::weak_ptr<INetworkConnection> conn_weak) {
   std::unique_lock lock(m_mutex);
-  if (!m_server) return;  // only process on server
+  if (!m_server) {
+    return;  // only process on server
+  }
   unsigned int id = msg->id();
   if (id >= m_idmap.size() || !m_idmap[id]) {
     // ignore call to non-existent RPC
     // this can happen due to deleted entries
     lock.unlock();
-    DEBUG0("received RPC call to unknown entry");
+    DEBUG0("{}", "received RPC call to unknown entry");
     return;
   }
   Entry* entry = m_idmap[id];
   if (!entry->value || !entry->value->IsRpc()) {
     lock.unlock();
-    DEBUG0("received RPC call to non-RPC entry");
+    DEBUG0("{}", "received RPC call to non-RPC entry");
     return;
   }
   ConnectionInfo conn_info;
@@ -342,9 +360,11 @@
   unsigned int call_uid = msg->seq_num_uid();
   m_rpc_server.ProcessRpc(
       entry->local_id, call_uid, entry->name, msg->str(), conn_info,
-      [=](StringRef result) {
+      [=](std::string_view result) {
         auto c = conn_weak.lock();
-        if (c) c->QueueOutgoing(Message::RpcResponse(id, call_uid, result));
+        if (c) {
+          c->QueueOutgoing(Message::RpcResponse(id, call_uid, result));
+        }
       },
       entry->rpc_uid);
 }
@@ -352,23 +372,25 @@
 void Storage::ProcessIncomingRpcResponse(std::shared_ptr<Message> msg,
                                          INetworkConnection* /*conn*/) {
   std::unique_lock lock(m_mutex);
-  if (m_server) return;  // only process on client
+  if (m_server) {
+    return;  // only process on client
+  }
   unsigned int id = msg->id();
   if (id >= m_idmap.size() || !m_idmap[id]) {
     // ignore response to non-existent RPC
     // this can happen due to deleted entries
     lock.unlock();
-    DEBUG0("received rpc response to unknown entry");
+    DEBUG0("{}", "received rpc response to unknown entry");
     return;
   }
   Entry* entry = m_idmap[id];
   if (!entry->value || !entry->value->IsRpc()) {
     lock.unlock();
-    DEBUG0("received RPC response to non-RPC entry");
+    DEBUG0("{}", "received RPC response to non-RPC entry");
     return;
   }
-  m_rpc_results.insert(std::make_pair(
-      RpcIdPair{entry->local_id, msg->seq_num_uid()}, msg->str()));
+  m_rpc_results.insert({RpcIdPair{entry->local_id, msg->seq_num_uid()},
+                        std::string{msg->str()}});
   m_rpc_results_cond.notify_all();
 }
 
@@ -378,7 +400,9 @@
   conn.set_state(INetworkConnection::kSynchronized);
   for (auto& i : m_entries) {
     Entry* entry = i.getValue();
-    if (!entry->value) continue;
+    if (!entry->value) {
+      continue;
+    }
     msgs->emplace_back(Message::EntryAssign(i.getKey(), entry->id,
                                             entry->seq_num.value(),
                                             entry->value, entry->flags));
@@ -386,17 +410,21 @@
 }
 
 void Storage::ApplyInitialAssignments(
-    INetworkConnection& conn, wpi::ArrayRef<std::shared_ptr<Message>> msgs,
+    INetworkConnection& conn, wpi::span<std::shared_ptr<Message>> msgs,
     bool /*new_server*/, std::vector<std::shared_ptr<Message>>* out_msgs) {
   std::unique_lock lock(m_mutex);
-  if (m_server) return;  // should not do this on server
+  if (m_server) {
+    return;  // should not do this on server
+  }
 
   conn.set_state(INetworkConnection::kSynchronized);
 
   std::vector<std::shared_ptr<Message>> update_msgs;
 
   // clear existing id's
-  for (auto& i : m_entries) i.getValue()->id = 0xffff;
+  for (auto& i : m_entries) {
+    i.getValue()->id = 0xffff;
+  }
 
   // clear existing idmap
   m_idmap.resize(0);
@@ -404,18 +432,18 @@
   // apply assignments
   for (auto& msg : msgs) {
     if (!msg->Is(Message::kEntryAssign)) {
-      DEBUG0("client: received non-entry assignment request?");
+      DEBUG0("{}", "client: received non-entry assignment request?");
       continue;
     }
 
     unsigned int id = msg->id();
     if (id == 0xffff) {
-      DEBUG0("client: received entry assignment request?");
+      DEBUG0("{}", "client: received entry assignment request?");
       continue;
     }
 
     SequenceNumber seq_num(msg->seq_num_uid());
-    StringRef name = msg->str();
+    std::string_view name = msg->str();
 
     Entry* entry = GetOrNew(name);
     entry->seq_num = seq_num;
@@ -440,7 +468,9 @@
         unsigned int notify_flags = NT_NOTIFY_UPDATE;
         // don't update flags from a <3.0 remote (not part of message)
         if (conn.proto_rev() >= 0x0300) {
-          if (entry->flags != msg->flags()) notify_flags |= NT_NOTIFY_FLAGS;
+          if (entry->flags != msg->flags()) {
+            notify_flags |= NT_NOTIFY_FLAGS;
+          }
           entry->flags = msg->flags();
         }
         // notify
@@ -450,14 +480,18 @@
     }
 
     // save to idmap
-    if (id >= m_idmap.size()) m_idmap.resize(id + 1);
+    if (id >= m_idmap.size()) {
+      m_idmap.resize(id + 1);
+    }
     m_idmap[id] = entry;
   }
 
   // delete or generate assign messages for unassigned local entries
   DeleteAllEntriesImpl(false, [&](Entry* entry) -> bool {
     // was assigned by the server, don't delete
-    if (entry->id != 0xffff) return false;
+    if (entry->id != 0xffff) {
+      return false;
+    }
     // if we have written the value locally, we send an assign message to the
     // server instead of deleting
     if (entry->local_write) {
@@ -471,32 +505,43 @@
   });
   auto dispatcher = m_dispatcher;
   lock.unlock();
-  for (auto& msg : update_msgs)
+  for (auto& msg : update_msgs) {
     dispatcher->QueueOutgoing(msg, nullptr, nullptr);
+  }
 }
 
-std::shared_ptr<Value> Storage::GetEntryValue(StringRef name) const {
+std::shared_ptr<Value> Storage::GetEntryValue(std::string_view name) const {
   std::scoped_lock lock(m_mutex);
   auto i = m_entries.find(name);
-  if (i == m_entries.end()) return nullptr;
+  if (i == m_entries.end()) {
+    return nullptr;
+  }
   return i->getValue()->value;
 }
 
 std::shared_ptr<Value> Storage::GetEntryValue(unsigned int local_id) const {
   std::scoped_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return nullptr;
+  if (local_id >= m_localmap.size()) {
+    return nullptr;
+  }
   return m_localmap[local_id]->value;
 }
 
-bool Storage::SetDefaultEntryValue(StringRef name,
+bool Storage::SetDefaultEntryValue(std::string_view name,
                                    std::shared_ptr<Value> value) {
-  if (name.empty()) return false;
-  if (!value) return false;
+  if (name.empty()) {
+    return false;
+  }
+  if (!value) {
+    return false;
+  }
   std::unique_lock lock(m_mutex);
   Entry* entry = GetOrNew(name);
 
   // we return early if value already exists; if types match return true
-  if (entry->value) return entry->value->type() == value->type();
+  if (entry->value) {
+    return entry->value->type() == value->type();
+  }
 
   SetEntryValueImpl(entry, value, lock, true);
   return true;
@@ -504,26 +549,38 @@
 
 bool Storage::SetDefaultEntryValue(unsigned int local_id,
                                    std::shared_ptr<Value> value) {
-  if (!value) return false;
+  if (!value) {
+    return false;
+  }
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return false;
+  if (local_id >= m_localmap.size()) {
+    return false;
+  }
   Entry* entry = m_localmap[local_id].get();
 
   // we return early if value already exists; if types match return true
-  if (entry->value) return entry->value->type() == value->type();
+  if (entry->value) {
+    return entry->value->type() == value->type();
+  }
 
   SetEntryValueImpl(entry, value, lock, true);
   return true;
 }
 
-bool Storage::SetEntryValue(StringRef name, std::shared_ptr<Value> value) {
-  if (name.empty()) return true;
-  if (!value) return true;
+bool Storage::SetEntryValue(std::string_view name,
+                            std::shared_ptr<Value> value) {
+  if (name.empty()) {
+    return true;
+  }
+  if (!value) {
+    return true;
+  }
   std::unique_lock lock(m_mutex);
   Entry* entry = GetOrNew(name);
 
-  if (entry->value && entry->value->type() != value->type())
+  if (entry->value && entry->value->type() != value->type()) {
     return false;  // error on type mismatch
+  }
 
   SetEntryValueImpl(entry, value, lock, true);
   return true;
@@ -531,13 +588,18 @@
 
 bool Storage::SetEntryValue(unsigned int local_id,
                             std::shared_ptr<Value> value) {
-  if (!value) return true;
+  if (!value) {
+    return true;
+  }
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return true;
+  if (local_id >= m_localmap.size()) {
+    return true;
+  }
   Entry* entry = m_localmap[local_id].get();
 
-  if (entry->value && entry->value->type() != value->type())
+  if (entry->value && entry->value->type() != value->type()) {
     return false;  // error on type mismatch
+  }
 
   SetEntryValueImpl(entry, value, lock, true);
   return true;
@@ -546,7 +608,9 @@
 void Storage::SetEntryValueImpl(Entry* entry, std::shared_ptr<Value> value,
                                 std::unique_lock<wpi::mutex>& lock,
                                 bool local) {
-  if (!value) return;
+  if (!value) {
+    return;
+  }
   auto old_value = entry->value;
   entry->value = value;
 
@@ -558,31 +622,41 @@
   }
 
   // update persistent dirty flag if value changed and it's persistent
-  if (entry->IsPersistent() && (!old_value || *old_value != *value))
+  if (entry->IsPersistent() && (!old_value || *old_value != *value)) {
     m_persistent_dirty = true;
+  }
 
   // notify
-  if (!old_value)
+  if (!old_value) {
     m_notifier.NotifyEntry(entry->local_id, entry->name, value,
                            NT_NOTIFY_NEW | (local ? NT_NOTIFY_LOCAL : 0));
-  else if (*old_value != *value)
+  } else if (*old_value != *value) {
     m_notifier.NotifyEntry(entry->local_id, entry->name, value,
                            NT_NOTIFY_UPDATE | (local ? NT_NOTIFY_LOCAL : 0));
+  }
 
   // remember local changes
-  if (local) entry->local_write = true;
+  if (local) {
+    entry->local_write = true;
+  }
 
   // generate message
-  if (!m_dispatcher || (!local && !m_server)) return;
+  if (!m_dispatcher || (!local && !m_server)) {
+    return;
+  }
   auto dispatcher = m_dispatcher;
   if (!old_value || old_value->type() != value->type()) {
-    if (local) ++entry->seq_num;
+    if (local) {
+      ++entry->seq_num;
+    }
     auto msg = Message::EntryAssign(
         entry->name, entry->id, entry->seq_num.value(), value, entry->flags);
     lock.unlock();
     dispatcher->QueueOutgoing(msg, nullptr, nullptr);
   } else if (*old_value != *value) {
-    if (local) ++entry->seq_num;
+    if (local) {
+      ++entry->seq_num;
+    }
     // don't send an update if we don't have an assigned id yet
     if (entry->id != 0xffff) {
       auto msg = Message::EntryUpdate(entry->id, entry->seq_num.value(), value);
@@ -592,9 +666,14 @@
   }
 }
 
-void Storage::SetEntryTypeValue(StringRef name, std::shared_ptr<Value> value) {
-  if (name.empty()) return;
-  if (!value) return;
+void Storage::SetEntryTypeValue(std::string_view name,
+                                std::shared_ptr<Value> value) {
+  if (name.empty()) {
+    return;
+  }
+  if (!value) {
+    return;
+  }
   std::unique_lock lock(m_mutex);
   Entry* entry = GetOrNew(name);
 
@@ -603,37 +682,52 @@
 
 void Storage::SetEntryTypeValue(unsigned int local_id,
                                 std::shared_ptr<Value> value) {
-  if (!value) return;
+  if (!value) {
+    return;
+  }
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return;
+  if (local_id >= m_localmap.size()) {
+    return;
+  }
   Entry* entry = m_localmap[local_id].get();
-  if (!entry) return;
+  if (!entry) {
+    return;
+  }
 
   SetEntryValueImpl(entry, value, lock, true);
 }
 
-void Storage::SetEntryFlags(StringRef name, unsigned int flags) {
-  if (name.empty()) return;
+void Storage::SetEntryFlags(std::string_view name, unsigned int flags) {
+  if (name.empty()) {
+    return;
+  }
   std::unique_lock lock(m_mutex);
   auto i = m_entries.find(name);
-  if (i == m_entries.end()) return;
+  if (i == m_entries.end()) {
+    return;
+  }
   SetEntryFlagsImpl(i->getValue(), flags, lock, true);
 }
 
 void Storage::SetEntryFlags(unsigned int id_local, unsigned int flags) {
   std::unique_lock lock(m_mutex);
-  if (id_local >= m_localmap.size()) return;
+  if (id_local >= m_localmap.size()) {
+    return;
+  }
   SetEntryFlagsImpl(m_localmap[id_local].get(), flags, lock, true);
 }
 
 void Storage::SetEntryFlagsImpl(Entry* entry, unsigned int flags,
                                 std::unique_lock<wpi::mutex>& lock,
                                 bool local) {
-  if (!entry->value || entry->flags == flags) return;
+  if (!entry->value || entry->flags == flags) {
+    return;
+  }
 
   // update persistent dirty flag if persistent flag changed
-  if ((entry->flags & NT_PERSISTENT) != (flags & NT_PERSISTENT))
+  if ((entry->flags & NT_PERSISTENT) != (flags & NT_PERSISTENT)) {
     m_persistent_dirty = true;
+  }
 
   entry->flags = flags;
 
@@ -642,7 +736,9 @@
                          NT_NOTIFY_FLAGS | (local ? NT_NOTIFY_LOCAL : 0));
 
   // generate message
-  if (!local || !m_dispatcher) return;
+  if (!local || !m_dispatcher) {
+    return;
+  }
   auto dispatcher = m_dispatcher;
   unsigned int id = entry->id;
   // don't send an update if we don't have an assigned id yet
@@ -653,29 +749,37 @@
   }
 }
 
-unsigned int Storage::GetEntryFlags(StringRef name) const {
+unsigned int Storage::GetEntryFlags(std::string_view name) const {
   std::scoped_lock lock(m_mutex);
   auto i = m_entries.find(name);
-  if (i == m_entries.end()) return 0;
+  if (i == m_entries.end()) {
+    return 0;
+  }
   return i->getValue()->flags;
 }
 
 unsigned int Storage::GetEntryFlags(unsigned int local_id) const {
   std::scoped_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return 0;
+  if (local_id >= m_localmap.size()) {
+    return 0;
+  }
   return m_localmap[local_id]->flags;
 }
 
-void Storage::DeleteEntry(StringRef name) {
+void Storage::DeleteEntry(std::string_view name) {
   std::unique_lock lock(m_mutex);
   auto i = m_entries.find(name);
-  if (i == m_entries.end()) return;
+  if (i == m_entries.end()) {
+    return;
+  }
   DeleteEntryImpl(i->getValue(), lock, true);
 }
 
 void Storage::DeleteEntry(unsigned int local_id) {
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return;
+  if (local_id >= m_localmap.size()) {
+    return;
+  }
   DeleteEntryImpl(m_localmap[local_id].get(), lock, true);
 }
 
@@ -684,7 +788,9 @@
   unsigned int id = entry->id;
 
   // Erase entry from id mapping.
-  if (id < m_idmap.size()) m_idmap[id] = nullptr;
+  if (id < m_idmap.size()) {
+    m_idmap[id] = nullptr;
+  }
 
   // empty the value and reset id and local_write flag
   std::shared_ptr<Value> old_value;
@@ -699,12 +805,16 @@
   }
 
   // update persistent dirty flag if it's a persistent value
-  if (entry->IsPersistent()) m_persistent_dirty = true;
+  if (entry->IsPersistent()) {
+    m_persistent_dirty = true;
+  }
 
   // reset flags
   entry->flags = 0;
 
-  if (!old_value) return;  // was not previously assigned
+  if (!old_value) {
+    return;  // was not previously assigned
+  }
 
   // notify
   m_notifier.NotifyEntry(entry->local_id, entry->name, old_value,
@@ -713,7 +823,9 @@
   // if it had a value, generate message
   // don't send an update if we don't have an assigned id yet
   if (local && id != 0xffff) {
-    if (!m_dispatcher) return;
+    if (!m_dispatcher) {
+      return;
+    }
     auto dispatcher = m_dispatcher;
     lock.unlock();
     dispatcher->QueueOutgoing(Message::EntryDelete(id), nullptr, nullptr);
@@ -729,7 +841,9 @@
       m_notifier.NotifyEntry(entry->local_id, i.getKey(), entry->value,
                              NT_NOTIFY_DELETE | (local ? NT_NOTIFY_LOCAL : 0));
       // remove it from idmap
-      if (entry->id < m_idmap.size()) m_idmap[entry->id] = nullptr;
+      if (entry->id < m_idmap.size()) {
+        m_idmap[entry->id] = nullptr;
+      }
       entry->id = 0xffff;
       entry->local_write = false;
       entry->value.reset();
@@ -746,48 +860,52 @@
 
 void Storage::DeleteAllEntries() {
   std::unique_lock lock(m_mutex);
-  if (m_entries.empty()) return;
+  if (m_entries.empty()) {
+    return;
+  }
 
   DeleteAllEntriesImpl(true);
 
   // generate message
-  if (!m_dispatcher) return;
+  if (!m_dispatcher) {
+    return;
+  }
   auto dispatcher = m_dispatcher;
   lock.unlock();
   dispatcher->QueueOutgoing(Message::ClearEntries(), nullptr, nullptr);
 }
 
-Storage::Entry* Storage::GetOrNew(const Twine& name) {
-  wpi::SmallString<128> nameBuf;
-  StringRef nameStr = name.toStringRef(nameBuf);
-  auto& entry = m_entries[nameStr];
+Storage::Entry* Storage::GetOrNew(std::string_view name) {
+  auto& entry = m_entries[name];
   if (!entry) {
-    m_localmap.emplace_back(new Entry(nameStr));
+    m_localmap.emplace_back(new Entry(name));
     entry = m_localmap.back().get();
     entry->local_id = m_localmap.size() - 1;
   }
   return entry;
 }
 
-unsigned int Storage::GetEntry(const Twine& name) {
-  if (name.isTriviallyEmpty() ||
-      (name.isSingleStringRef() && name.getSingleStringRef().empty()))
+unsigned int Storage::GetEntry(std::string_view name) {
+  if (name.empty()) {
     return UINT_MAX;
+  }
   std::unique_lock lock(m_mutex);
   return GetOrNew(name)->local_id;
 }
 
-std::vector<unsigned int> Storage::GetEntries(const Twine& prefix,
+std::vector<unsigned int> Storage::GetEntries(std::string_view prefix,
                                               unsigned int types) {
-  wpi::SmallString<128> prefixBuf;
-  StringRef prefixStr = prefix.toStringRef(prefixBuf);
   std::scoped_lock lock(m_mutex);
   std::vector<unsigned int> ids;
   for (auto& i : m_entries) {
     Entry* entry = i.getValue();
     auto value = entry->value.get();
-    if (!value || !i.getKey().startswith(prefixStr)) continue;
-    if (types != 0 && (types & value->type()) == 0) continue;
+    if (!value || !wpi::starts_with(i.getKey(), prefix)) {
+      continue;
+    }
+    if (types != 0 && (types & value->type()) == 0) {
+      continue;
+    }
     ids.push_back(entry->local_id);
   }
   return ids;
@@ -801,9 +919,13 @@
   info.last_change = 0;
 
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return info;
+  if (local_id >= m_localmap.size()) {
+    return info;
+  }
   Entry* entry = m_localmap[local_id].get();
-  if (!entry->value) return info;
+  if (!entry->value) {
+    return info;
+  }
 
   info.entry = Handle(inst, local_id, Handle::kEntry);
   info.name = entry->name;
@@ -815,37 +937,49 @@
 
 std::string Storage::GetEntryName(unsigned int local_id) const {
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return std::string{};
+  if (local_id >= m_localmap.size()) {
+    return {};
+  }
   return m_localmap[local_id]->name;
 }
 
 NT_Type Storage::GetEntryType(unsigned int local_id) const {
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return NT_UNASSIGNED;
+  if (local_id >= m_localmap.size()) {
+    return NT_UNASSIGNED;
+  }
   Entry* entry = m_localmap[local_id].get();
-  if (!entry->value) return NT_UNASSIGNED;
+  if (!entry->value) {
+    return NT_UNASSIGNED;
+  }
   return entry->value->type();
 }
 
 uint64_t Storage::GetEntryLastChange(unsigned int local_id) const {
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return 0;
+  if (local_id >= m_localmap.size()) {
+    return 0;
+  }
   Entry* entry = m_localmap[local_id].get();
-  if (!entry->value) return 0;
+  if (!entry->value) {
+    return 0;
+  }
   return entry->value->last_change();
 }
 
-std::vector<EntryInfo> Storage::GetEntryInfo(int inst, const Twine& prefix,
+std::vector<EntryInfo> Storage::GetEntryInfo(int inst, std::string_view prefix,
                                              unsigned int types) {
-  wpi::SmallString<128> prefixBuf;
-  StringRef prefixStr = prefix.toStringRef(prefixBuf);
   std::scoped_lock lock(m_mutex);
   std::vector<EntryInfo> infos;
   for (auto& i : m_entries) {
     Entry* entry = i.getValue();
     auto value = entry->value.get();
-    if (!value || !i.getKey().startswith(prefixStr)) continue;
-    if (types != 0 && (types & value->type()) == 0) continue;
+    if (!value || !wpi::starts_with(i.getKey(), prefix)) {
+      continue;
+    }
+    if (types != 0 && (types & value->type()) == 0) {
+      continue;
+    }
     EntryInfo info;
     info.entry = Handle(inst, entry->local_id, Handle::kEntry);
     info.name = i.getKey();
@@ -858,18 +992,18 @@
 }
 
 unsigned int Storage::AddListener(
-    const Twine& prefix,
+    std::string_view prefix,
     std::function<void(const EntryNotification& event)> callback,
     unsigned int flags) const {
-  wpi::SmallString<128> prefixBuf;
-  StringRef prefixStr = prefix.toStringRef(prefixBuf);
   std::scoped_lock lock(m_mutex);
-  unsigned int uid = m_notifier.Add(callback, prefixStr, flags);
+  unsigned int uid = m_notifier.Add(callback, prefix, flags);
   // perform immediate notifications
   if ((flags & NT_NOTIFY_IMMEDIATE) != 0 && (flags & NT_NOTIFY_NEW) != 0) {
     for (auto& i : m_entries) {
       Entry* entry = i.getValue();
-      if (!entry->value || !i.getKey().startswith(prefixStr)) continue;
+      if (!entry->value || !wpi::starts_with(i.getKey(), prefix)) {
+        continue;
+      }
       m_notifier.NotifyEntry(entry->local_id, i.getKey(), entry->value,
                              NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW, uid);
     }
@@ -896,18 +1030,20 @@
 }
 
 unsigned int Storage::AddPolledListener(unsigned int poller,
-                                        const Twine& prefix,
+                                        std::string_view prefix,
                                         unsigned int flags) const {
-  wpi::SmallString<128> prefixBuf;
-  StringRef prefixStr = prefix.toStringRef(prefixBuf);
   std::scoped_lock lock(m_mutex);
-  unsigned int uid = m_notifier.AddPolled(poller, prefixStr, flags);
+  unsigned int uid = m_notifier.AddPolled(poller, prefix, flags);
   // perform immediate notifications
   if ((flags & NT_NOTIFY_IMMEDIATE) != 0 && (flags & NT_NOTIFY_NEW) != 0) {
     for (auto& i : m_entries) {
-      if (!i.getKey().startswith(prefixStr)) continue;
+      if (!wpi::starts_with(i.getKey(), prefix)) {
+        continue;
+      }
       Entry* entry = i.getValue();
-      if (!entry->value) continue;
+      if (!entry->value) {
+        continue;
+      }
       m_notifier.NotifyEntry(entry->local_id, i.getKey(), entry->value,
                              NT_NOTIFY_IMMEDIATE | NT_NOTIFY_NEW, uid);
     }
@@ -941,13 +1077,17 @@
   {
     std::scoped_lock lock(m_mutex);
     // for periodic, don't re-save unless something has changed
-    if (periodic && !m_persistent_dirty) return false;
+    if (periodic && !m_persistent_dirty) {
+      return false;
+    }
     m_persistent_dirty = false;
     entries->reserve(m_entries.size());
     for (auto& i : m_entries) {
       Entry* entry = i.getValue();
       // only write persistent-flagged values
-      if (!entry->value || !entry->IsPersistent()) continue;
+      if (!entry->value || !entry->IsPersistent()) {
+        continue;
+      }
       entries->emplace_back(i.getKey(), entry->value);
     }
   }
@@ -962,11 +1102,9 @@
 }
 
 bool Storage::GetEntries(
-    const Twine& prefix,
+    std::string_view prefix,
     std::vector<std::pair<std::string, std::shared_ptr<Value>>>* entries)
     const {
-  wpi::SmallString<128> prefixBuf;
-  StringRef prefixStr = prefix.toStringRef(prefixBuf);
   // copy values out of storage as quickly as possible so lock isn't held
   {
     std::scoped_lock lock(m_mutex);
@@ -974,7 +1112,9 @@
     for (auto& i : m_entries) {
       Entry* entry = i.getValue();
       // only write values with given prefix
-      if (!entry->value || !i.getKey().startswith(prefixStr)) continue;
+      if (!entry->value || !wpi::starts_with(i.getKey(), prefix)) {
+        continue;
+      }
       entries->emplace_back(i.getKey(), entry->value);
     }
   }
@@ -988,10 +1128,12 @@
   return true;
 }
 
-void Storage::CreateRpc(unsigned int local_id, StringRef def,
+void Storage::CreateRpc(unsigned int local_id, std::string_view def,
                         unsigned int rpc_uid) {
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return;
+  if (local_id >= m_localmap.size()) {
+    return;
+  }
   Entry* entry = m_localmap[local_id].get();
 
   auto old_value = entry->value;
@@ -1001,7 +1143,9 @@
   // set up the RPC info
   entry->rpc_uid = rpc_uid;
 
-  if (old_value && *old_value == *value) return;
+  if (old_value && *old_value == *value) {
+    return;
+  }
 
   // assign an id if it doesn't have one
   if (entry->id == 0xffff) {
@@ -1011,7 +1155,9 @@
   }
 
   // generate message
-  if (!m_dispatcher) return;
+  if (!m_dispatcher) {
+    return;
+  }
   auto dispatcher = m_dispatcher;
   if (!old_value || old_value->type() != value->type()) {
     ++entry->seq_num;
@@ -1027,19 +1173,25 @@
   }
 }
 
-unsigned int Storage::CallRpc(unsigned int local_id, StringRef params) {
+unsigned int Storage::CallRpc(unsigned int local_id, std::string_view params) {
   std::unique_lock lock(m_mutex);
-  if (local_id >= m_localmap.size()) return 0;
+  if (local_id >= m_localmap.size()) {
+    return 0;
+  }
   Entry* entry = m_localmap[local_id].get();
 
-  if (!entry->value || !entry->value->IsRpc()) return 0;
+  if (!entry->value || !entry->value->IsRpc()) {
+    return 0;
+  }
 
   ++entry->rpc_call_uid;
-  if (entry->rpc_call_uid > 0xffff) entry->rpc_call_uid = 0;
+  if (entry->rpc_call_uid > 0xffff) {
+    entry->rpc_call_uid = 0;
+  }
   unsigned int call_uid = entry->rpc_call_uid;
 
   auto msg = Message::ExecuteRpc(entry->id, call_uid, params);
-  StringRef name{entry->name};
+  std::string_view name{entry->name};
 
   if (m_server) {
     // RPCs are unlikely to be used locally on the server, but handle it
@@ -1055,10 +1207,10 @@
     unsigned int call_uid = msg->seq_num_uid();
     m_rpc_server.ProcessRpc(
         local_id, call_uid, name, msg->str(), conn_info,
-        [=](StringRef result) {
+        [=](std::string_view result) {
           std::scoped_lock lock(m_mutex);
-          m_rpc_results.insert(
-              std::make_pair(RpcIdPair{local_id, call_uid}, result));
+          m_rpc_results.insert(std::make_pair(RpcIdPair{local_id, call_uid},
+                                              std::string{result}));
           m_rpc_results_cond.notify_all();
         },
         rpc_uid);
@@ -1084,7 +1236,9 @@
   RpcIdPair call_pair{local_id, call_uid};
 
   // only allow one blocking call per rpc call uid
-  if (!m_rpc_blocking_calls.insert(call_pair).second) return false;
+  if (!m_rpc_blocking_calls.insert(call_pair).second) {
+    return false;
+  }
 
   auto timeout_time =
       std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.h b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.h
index fa9b2bf..f49c071 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_STORAGE_H_
 #define NTCORE_STORAGE_H_
@@ -15,6 +12,7 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
@@ -23,6 +21,7 @@
 #include <wpi/StringMap.h>
 #include <wpi/condition_variable.h>
 #include <wpi/mutex.h>
+#include <wpi/span.h>
 
 #include "IStorage.h"
 #include "Message.h"
@@ -50,7 +49,7 @@
   Storage(const Storage&) = delete;
   Storage& operator=(const Storage&) = delete;
 
-  ~Storage();
+  ~Storage() override;
 
   // Accessors required by Dispatcher.  An interface is used for
   // generation of outgoing messages to break a dependency loop between
@@ -69,41 +68,42 @@
       INetworkConnection& conn,
       std::vector<std::shared_ptr<Message>>* msgs) override;
   void ApplyInitialAssignments(
-      INetworkConnection& conn, wpi::ArrayRef<std::shared_ptr<Message>> msgs,
+      INetworkConnection& conn, wpi::span<std::shared_ptr<Message>> msgs,
       bool new_server,
       std::vector<std::shared_ptr<Message>>* out_msgs) override;
 
   // User functions.  These are the actual implementations of the corresponding
   // user API functions in ntcore_cpp.
-  std::shared_ptr<Value> GetEntryValue(StringRef name) const;
+  std::shared_ptr<Value> GetEntryValue(std::string_view name) const;
   std::shared_ptr<Value> GetEntryValue(unsigned int local_id) const;
 
-  bool SetDefaultEntryValue(StringRef name, std::shared_ptr<Value> value);
+  bool SetDefaultEntryValue(std::string_view name,
+                            std::shared_ptr<Value> value);
   bool SetDefaultEntryValue(unsigned int local_id,
                             std::shared_ptr<Value> value);
 
-  bool SetEntryValue(StringRef name, std::shared_ptr<Value> value);
+  bool SetEntryValue(std::string_view name, std::shared_ptr<Value> value);
   bool SetEntryValue(unsigned int local_id, std::shared_ptr<Value> value);
 
-  void SetEntryTypeValue(StringRef name, std::shared_ptr<Value> value);
+  void SetEntryTypeValue(std::string_view name, std::shared_ptr<Value> value);
   void SetEntryTypeValue(unsigned int local_id, std::shared_ptr<Value> value);
 
-  void SetEntryFlags(StringRef name, unsigned int flags);
+  void SetEntryFlags(std::string_view name, unsigned int flags);
   void SetEntryFlags(unsigned int local_id, unsigned int flags);
 
-  unsigned int GetEntryFlags(StringRef name) const;
+  unsigned int GetEntryFlags(std::string_view name) const;
   unsigned int GetEntryFlags(unsigned int local_id) const;
 
-  void DeleteEntry(StringRef name);
+  void DeleteEntry(std::string_view name);
   void DeleteEntry(unsigned int local_id);
 
   void DeleteAllEntries();
 
-  std::vector<EntryInfo> GetEntryInfo(int inst, const Twine& prefix,
+  std::vector<EntryInfo> GetEntryInfo(int inst, std::string_view prefix,
                                       unsigned int types);
 
   unsigned int AddListener(
-      const Twine& prefix,
+      std::string_view prefix,
       std::function<void(const EntryNotification& event)> callback,
       unsigned int flags) const;
   unsigned int AddListener(
@@ -111,14 +111,16 @@
       std::function<void(const EntryNotification& event)> callback,
       unsigned int flags) const;
 
-  unsigned int AddPolledListener(unsigned int poller_uid, const Twine& prefix,
+  unsigned int AddPolledListener(unsigned int poller_uid,
+                                 std::string_view prefix,
                                  unsigned int flags) const;
   unsigned int AddPolledListener(unsigned int poller_uid, unsigned int local_id,
                                  unsigned int flags) const;
 
   // Index-only
-  unsigned int GetEntry(const Twine& name);
-  std::vector<unsigned int> GetEntries(const Twine& prefix, unsigned int types);
+  unsigned int GetEntry(std::string_view name);
+  std::vector<unsigned int> GetEntries(std::string_view prefix,
+                                       unsigned int types);
   EntryInfo GetEntryInfo(int inst, unsigned int local_id) const;
   std::string GetEntryName(unsigned int local_id) const;
   NT_Type GetEntryType(unsigned int local_id) const;
@@ -126,29 +128,32 @@
 
   // Filename-based save/load functions.  Used both by periodic saves and
   // accessible directly via the user API.
-  const char* SavePersistent(const Twine& filename,
+  const char* SavePersistent(std::string_view filename,
                              bool periodic) const override;
   const char* LoadPersistent(
-      const Twine& filename,
+      std::string_view filename,
       std::function<void(size_t line, const char* msg)> warn) override;
 
-  const char* SaveEntries(const Twine& filename, const Twine& prefix) const;
+  const char* SaveEntries(std::string_view filename,
+                          std::string_view prefix) const;
   const char* LoadEntries(
-      const Twine& filename, const Twine& prefix,
+      std::string_view filename, std::string_view prefix,
       std::function<void(size_t line, const char* msg)> warn);
 
   // Stream-based save/load functions (exposed for testing purposes).  These
   // implement the guts of the filename-based functions.
   void SavePersistent(wpi::raw_ostream& os, bool periodic) const;
-  bool LoadEntries(wpi::raw_istream& is, const Twine& prefix, bool persistent,
+  bool LoadEntries(wpi::raw_istream& is, std::string_view prefix,
+                   bool persistent,
                    std::function<void(size_t line, const char* msg)> warn);
 
-  void SaveEntries(wpi::raw_ostream& os, const Twine& prefix) const;
+  void SaveEntries(wpi::raw_ostream& os, std::string_view prefix) const;
 
   // RPC configuration needs to come through here as RPC definitions are
   // actually special Storage value types.
-  void CreateRpc(unsigned int local_id, StringRef def, unsigned int rpc_uid);
-  unsigned int CallRpc(unsigned int local_id, StringRef params);
+  void CreateRpc(unsigned int local_id, std::string_view def,
+                 unsigned int rpc_uid);
+  unsigned int CallRpc(unsigned int local_id, std::string_view params);
   bool GetRpcResult(unsigned int local_id, unsigned int call_uid,
                     std::string* result);
   bool GetRpcResult(unsigned int local_id, unsigned int call_uid,
@@ -158,7 +163,7 @@
  private:
   // Data for each table entry.
   struct Entry {
-    explicit Entry(wpi::StringRef name_) : name(name_) {}
+    explicit Entry(std::string_view name_) : name(name_) {}
     bool IsPersistent() const { return (flags & NT_PERSISTENT) != 0; }
 
     // We redundantly store the name so that it's available when accessing the
@@ -192,12 +197,12 @@
     unsigned int rpc_call_uid{0};
   };
 
-  typedef wpi::StringMap<Entry*> EntriesMap;
-  typedef std::vector<Entry*> IdMap;
-  typedef std::vector<std::unique_ptr<Entry>> LocalMap;
-  typedef std::pair<unsigned int, unsigned int> RpcIdPair;
-  typedef wpi::DenseMap<RpcIdPair, std::string> RpcResultMap;
-  typedef wpi::SmallSet<RpcIdPair, 12> RpcBlockingCallSet;
+  using EntriesMap = wpi::StringMap<Entry*>;
+  using IdMap = std::vector<Entry*>;
+  using LocalMap = std::vector<std::unique_ptr<Entry>>;
+  using RpcIdPair = std::pair<unsigned int, unsigned int>;
+  using RpcResultMap = wpi::DenseMap<RpcIdPair, std::string>;
+  using RpcBlockingCallSet = wpi::SmallSet<RpcIdPair, 12>;
 
   mutable wpi::mutex m_mutex;
   EntriesMap m_entries;
@@ -240,7 +245,7 @@
       bool periodic,
       std::vector<std::pair<std::string, std::shared_ptr<Value>>>* entries)
       const;
-  bool GetEntries(const Twine& prefix,
+  bool GetEntries(std::string_view prefix,
                   std::vector<std::pair<std::string, std::shared_ptr<Value>>>*
                       entries) const;
   void SetEntryValueImpl(Entry* entry, std::shared_ptr<Value> value,
@@ -254,7 +259,7 @@
   template <typename F>
   void DeleteAllEntriesImpl(bool local, F should_delete);
   void DeleteAllEntriesImpl(bool local);
-  Entry* GetOrNew(const Twine& name);
+  Entry* GetOrNew(std::string_view name);
 };
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_load.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_load.cpp
index 69eb173..11bbb0e 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_load.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_load.cpp
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cctype>
 #include <string>
+#include <utility>
 
 #include <wpi/Base64.h>
 #include <wpi/SmallString.h>
@@ -23,19 +21,19 @@
 
 class LoadPersistentImpl {
  public:
-  typedef std::pair<std::string, std::shared_ptr<Value>> Entry;
-  typedef std::function<void(size_t line, const char* msg)> WarnFunc;
+  using Entry = std::pair<std::string, std::shared_ptr<Value>>;
+  using WarnFunc = std::function<void(size_t, const char*)>;
 
   LoadPersistentImpl(wpi::raw_istream& is, WarnFunc warn)
-      : m_is(is), m_warn(warn) {}
+      : m_is(is), m_warn(std::move(warn)) {}
 
-  bool Load(StringRef prefix, std::vector<Entry>* entries);
+  bool Load(std::string_view prefix, std::vector<Entry>* entries);
 
  private:
   bool ReadLine();
   bool ReadHeader();
   NT_Type ReadType();
-  wpi::StringRef ReadName(wpi::SmallVectorImpl<char>& buf);
+  std::string_view ReadName(wpi::SmallVectorImpl<char>& buf);
   std::shared_ptr<Value> ReadValue(NT_Type type);
   std::shared_ptr<Value> ReadBooleanValue();
   std::shared_ptr<Value> ReadDoubleValue();
@@ -46,13 +44,15 @@
   std::shared_ptr<Value> ReadStringArrayValue();
 
   void Warn(const char* msg) {
-    if (m_warn) m_warn(m_line_num, msg);
+    if (m_warn) {
+      m_warn(m_line_num, msg);
+    }
   }
 
   wpi::raw_istream& m_is;
   WarnFunc m_warn;
 
-  wpi::StringRef m_line;
+  std::string_view m_line;
   wpi::SmallString<128> m_line_buf;
   size_t m_line_num = 0;
 
@@ -71,11 +71,12 @@
  * Returns a pair containing the extracted token (if any) and the remaining
  * tail string.
  */
-static std::pair<wpi::StringRef, wpi::StringRef> ReadStringToken(
-    wpi::StringRef source) {
+static std::pair<std::string_view, std::string_view> ReadStringToken(
+    std::string_view source) {
   // Match opening quote
-  if (source.empty() || source.front() != '"')
-    return std::make_pair(wpi::StringRef(), source);
+  if (source.empty() || source.front() != '"') {
+    return {{}, source};
+  }
 
   // Scan for ending double quote, checking for escaped as we go.
   size_t size = source.size();
@@ -86,20 +87,21 @@
       break;
     }
   }
-  return std::make_pair(source.slice(0, pos), source.substr(pos));
+  return {wpi::slice(source, 0, pos), source.substr(pos)};
 }
 
 static int fromxdigit(char ch) {
-  if (ch >= 'a' && ch <= 'f')
+  if (ch >= 'a' && ch <= 'f') {
     return (ch - 'a' + 10);
-  else if (ch >= 'A' && ch <= 'F')
+  } else if (ch >= 'A' && ch <= 'F') {
     return (ch - 'A' + 10);
-  else
+  } else {
     return ch - '0';
+  }
 }
 
-static wpi::StringRef UnescapeString(wpi::StringRef source,
-                                     wpi::SmallVectorImpl<char>& buf) {
+static std::string_view UnescapeString(std::string_view source,
+                                       wpi::SmallVectorImpl<char>& buf) {
   assert(source.size() >= 2 && source.front() == '"' && source.back() == '"');
   buf.clear();
   buf.reserve(source.size() - 2);
@@ -133,11 +135,14 @@
         break;
     }
   }
-  return wpi::StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
-bool LoadPersistentImpl::Load(StringRef prefix, std::vector<Entry>* entries) {
-  if (!ReadHeader()) return false;  // header
+bool LoadPersistentImpl::Load(std::string_view prefix,
+                              std::vector<Entry>* entries) {
+  if (!ReadHeader()) {
+    return false;  // header
+  }
 
   while (ReadLine()) {
     // type
@@ -149,22 +154,27 @@
 
     // name
     wpi::SmallString<128> buf;
-    wpi::StringRef name = ReadName(buf);
-    if (name.empty() || !name.startswith(prefix)) continue;
+    std::string_view name = ReadName(buf);
+    if (name.empty() || !wpi::starts_with(name, prefix)) {
+      continue;
+    }
 
     // =
-    m_line = m_line.ltrim(" \t");
+    m_line = wpi::ltrim(m_line, " \t");
     if (m_line.empty() || m_line.front() != '=') {
       Warn("expected = after name");
       continue;
     }
-    m_line = m_line.drop_front().ltrim(" \t");
+    m_line.remove_prefix(1);
+    m_line = wpi::ltrim(m_line, " \t");
 
     // value
     auto value = ReadValue(type);
 
     // move to entries
-    if (value) entries->emplace_back(name, std::move(value));
+    if (value) {
+      entries->emplace_back(name, std::move(value));
+    }
   }
   return true;
 }
@@ -173,9 +183,10 @@
   // ignore blank lines and lines that start with ; or # (comments)
   while (!m_is.has_error()) {
     ++m_line_num;
-    m_line = m_is.getline(m_line_buf, INT_MAX).trim();
-    if (!m_line.empty() && m_line.front() != ';' && m_line.front() != '#')
+    m_line = wpi::trim(m_is.getline(m_line_buf, INT_MAX));
+    if (!m_line.empty() && m_line.front() != ';' && m_line.front() != '#') {
       return true;
+    }
   }
   return false;
 }
@@ -190,8 +201,8 @@
 }
 
 NT_Type LoadPersistentImpl::ReadType() {
-  wpi::StringRef tok;
-  std::tie(tok, m_line) = m_line.split(' ');
+  std::string_view tok;
+  std::tie(tok, m_line) = wpi::split(m_line, ' ');
   if (tok == "boolean") {
     return NT_BOOLEAN;
   } else if (tok == "double") {
@@ -201,28 +212,29 @@
   } else if (tok == "raw") {
     return NT_RAW;
   } else if (tok == "array") {
-    wpi::StringRef array_tok;
-    std::tie(array_tok, m_line) = m_line.split(' ');
-    if (array_tok == "boolean")
+    std::string_view array_tok;
+    std::tie(array_tok, m_line) = wpi::split(m_line, ' ');
+    if (array_tok == "boolean") {
       return NT_BOOLEAN_ARRAY;
-    else if (array_tok == "double")
+    } else if (array_tok == "double") {
       return NT_DOUBLE_ARRAY;
-    else if (array_tok == "string")
+    } else if (array_tok == "string") {
       return NT_STRING_ARRAY;
+    }
   }
   return NT_UNASSIGNED;
 }
 
-wpi::StringRef LoadPersistentImpl::ReadName(wpi::SmallVectorImpl<char>& buf) {
-  wpi::StringRef tok;
+std::string_view LoadPersistentImpl::ReadName(wpi::SmallVectorImpl<char>& buf) {
+  std::string_view tok;
   std::tie(tok, m_line) = ReadStringToken(m_line);
   if (tok.empty()) {
     Warn("missing name");
-    return wpi::StringRef{};
+    return {};
   }
   if (tok.back() != '"') {
     Warn("unterminated name string");
-    return wpi::StringRef{};
+    return {};
   }
   return UnescapeString(tok, buf);
 }
@@ -250,15 +262,19 @@
 
 std::shared_ptr<Value> LoadPersistentImpl::ReadBooleanValue() {
   // only true or false is accepted
-  if (m_line == "true") return Value::MakeBoolean(true);
-  if (m_line == "false") return Value::MakeBoolean(false);
+  if (m_line == "true") {
+    return Value::MakeBoolean(true);
+  }
+  if (m_line == "false") {
+    return Value::MakeBoolean(false);
+  }
   Warn("unrecognized boolean value, not 'true' or 'false'");
   return nullptr;
 }
 
 std::shared_ptr<Value> LoadPersistentImpl::ReadDoubleValue() {
   // need to convert to null-terminated string for std::strtod()
-  wpi::SmallString<64> buf = m_line;
+  wpi::SmallString<64> buf{m_line};
   char* end;
   double v = std::strtod(buf.c_str(), &end);
   if (*end != '\0') {
@@ -269,7 +285,7 @@
 }
 
 std::shared_ptr<Value> LoadPersistentImpl::ReadStringValue() {
-  wpi::StringRef tok;
+  std::string_view tok;
   std::tie(tok, m_line) = ReadStringToken(m_line);
   if (tok.empty()) {
     Warn("missing string value");
@@ -292,9 +308,9 @@
 std::shared_ptr<Value> LoadPersistentImpl::ReadBooleanArrayValue() {
   m_buf_boolean_array.clear();
   while (!m_line.empty()) {
-    wpi::StringRef tok;
-    std::tie(tok, m_line) = m_line.split(',');
-    tok = tok.trim(" \t");
+    std::string_view tok;
+    std::tie(tok, m_line) = wpi::split(m_line, ',');
+    tok = wpi::trim(tok, " \t");
     if (tok == "true") {
       m_buf_boolean_array.push_back(1);
     } else if (tok == "false") {
@@ -310,11 +326,11 @@
 std::shared_ptr<Value> LoadPersistentImpl::ReadDoubleArrayValue() {
   m_buf_double_array.clear();
   while (!m_line.empty()) {
-    wpi::StringRef tok;
-    std::tie(tok, m_line) = m_line.split(',');
-    tok = tok.trim(" \t");
+    std::string_view tok;
+    std::tie(tok, m_line) = wpi::split(m_line, ',');
+    tok = wpi::trim(tok, " \t");
     // need to convert to null-terminated string for std::strtod()
-    wpi::SmallString<64> buf = tok;
+    wpi::SmallString<64> buf{tok};
     char* end;
     double v = std::strtod(buf.c_str(), &end);
     if (*end != '\0') {
@@ -330,7 +346,7 @@
 std::shared_ptr<Value> LoadPersistentImpl::ReadStringArrayValue() {
   m_buf_string_array.clear();
   while (!m_line.empty()) {
-    wpi::StringRef tok;
+    std::string_view tok;
     std::tie(tok, m_line) = ReadStringToken(m_line);
     if (tok.empty()) {
       Warn("missing string value");
@@ -342,31 +358,33 @@
     }
 
     wpi::SmallString<128> buf;
-    m_buf_string_array.push_back(UnescapeString(tok, buf));
+    m_buf_string_array.emplace_back(UnescapeString(tok, buf));
 
-    m_line = m_line.ltrim(" \t");
-    if (m_line.empty()) break;
+    m_line = wpi::ltrim(m_line, " \t");
+    if (m_line.empty()) {
+      break;
+    }
     if (m_line.front() != ',') {
       Warn("expected comma between strings");
       return nullptr;
     }
-    m_line = m_line.drop_front().ltrim(" \t");
+    m_line.remove_prefix(1);
+    m_line = wpi::ltrim(m_line, " \t");
   }
 
   return Value::MakeStringArray(std::move(m_buf_string_array));
 }
 
 bool Storage::LoadEntries(
-    wpi::raw_istream& is, const Twine& prefix, bool persistent,
+    wpi::raw_istream& is, std::string_view prefix, bool persistent,
     std::function<void(size_t line, const char* msg)> warn) {
-  wpi::SmallString<128> prefixBuf;
-  StringRef prefixStr = prefix.toStringRef(prefixBuf);
-
   // entries to add
   std::vector<LoadPersistentImpl::Entry> entries;
 
   // load file
-  if (!LoadPersistentImpl(is, warn).Load(prefixStr, &entries)) return false;
+  if (!LoadPersistentImpl(is, warn).Load(prefix, &entries)) {
+    return false;
+  }
 
   // copy values into storage as quickly as possible so lock isn't held
   std::vector<std::shared_ptr<Message>> msgs;
@@ -376,7 +394,9 @@
     auto old_value = entry->value;
     entry->value = i.second;
     bool was_persist = entry->IsPersistent();
-    if (!was_persist && persistent) entry->flags |= NT_PERSISTENT;
+    if (!was_persist && persistent) {
+      entry->flags |= NT_PERSISTENT;
+    }
 
     // if we're the server, assign an id if it doesn't have one
     if (m_server && entry->id == 0xffff) {
@@ -392,7 +412,9 @@
                                NT_NOTIFY_NEW | NT_NOTIFY_LOCAL);
       } else if (*old_value != *i.second) {
         unsigned int notify_flags = NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL;
-        if (!was_persist && persistent) notify_flags |= NT_NOTIFY_FLAGS;
+        if (!was_persist && persistent) {
+          notify_flags |= NT_NOTIFY_FLAGS;
+        }
         m_notifier.NotifyEntry(entry->local_id, i.first, i.second,
                                notify_flags);
       } else if (!was_persist && persistent) {
@@ -401,7 +423,9 @@
       }
     }
 
-    if (!m_dispatcher) continue;  // shortcut
+    if (!m_dispatcher) {
+      continue;  // shortcut
+    }
     ++entry->seq_num;
 
     // put on update queue
@@ -410,40 +434,51 @@
           i.first, entry->id, entry->seq_num.value(), i.second, entry->flags));
     } else if (entry->id != 0xffff) {
       // don't send an update if we don't have an assigned id yet
-      if (*old_value != *i.second)
+      if (*old_value != *i.second) {
         msgs.emplace_back(
             Message::EntryUpdate(entry->id, entry->seq_num.value(), i.second));
-      if (!was_persist)
+      }
+      if (!was_persist) {
         msgs.emplace_back(Message::FlagsUpdate(entry->id, entry->flags));
+      }
     }
   }
 
   if (m_dispatcher) {
     auto dispatcher = m_dispatcher;
     lock.unlock();
-    for (auto& msg : msgs)
+    for (auto& msg : msgs) {
       dispatcher->QueueOutgoing(std::move(msg), nullptr, nullptr);
+    }
   }
 
   return true;
 }
 
 const char* Storage::LoadPersistent(
-    const Twine& filename,
+    std::string_view filename,
     std::function<void(size_t line, const char* msg)> warn) {
   std::error_code ec;
   wpi::raw_fd_istream is(filename, ec);
-  if (ec.value() != 0) return "could not open file";
-  if (!LoadEntries(is, "", true, warn)) return "error reading file";
+  if (ec.value() != 0) {
+    return "could not open file";
+  }
+  if (!LoadEntries(is, "", true, warn)) {
+    return "error reading file";
+  }
   return nullptr;
 }
 
 const char* Storage::LoadEntries(
-    const Twine& filename, const Twine& prefix,
+    std::string_view filename, std::string_view prefix,
     std::function<void(size_t line, const char* msg)> warn) {
   std::error_code ec;
   wpi::raw_fd_istream is(filename, ec);
-  if (ec.value() != 0) return "could not open file";
-  if (!LoadEntries(is, prefix, false, warn)) return "error reading file";
+  if (ec.value() != 0) {
+    return "could not open file";
+  }
+  if (!LoadEntries(is, prefix, false, warn)) {
+    return "error reading file";
+  }
   return nullptr;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_save.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_save.cpp
index 797eb8b..6d5db9f 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_save.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Storage_save.cpp
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cctype>
 #include <string>
 
+#include <fmt/format.h>
 #include <wpi/Base64.h>
-#include <wpi/FileSystem.h>
-#include <wpi/Format.h>
 #include <wpi/SmallString.h>
 #include <wpi/StringExtras.h>
+#include <wpi/fs.h>
 #include <wpi/raw_ostream.h>
 
 #include "Log.h"
@@ -24,17 +21,17 @@
 
 class SavePersistentImpl {
  public:
-  typedef std::pair<std::string, std::shared_ptr<Value>> Entry;
+  using Entry = std::pair<std::string, std::shared_ptr<Value>>;
 
   explicit SavePersistentImpl(wpi::raw_ostream& os) : m_os(os) {}
 
-  void Save(wpi::ArrayRef<Entry> entries);
+  void Save(wpi::span<const Entry> entries);
 
  private:
-  void WriteString(wpi::StringRef str);
+  void WriteString(std::string_view str);
   void WriteHeader();
-  void WriteEntries(wpi::ArrayRef<Entry> entries);
-  void WriteEntry(wpi::StringRef name, const Value& value);
+  void WriteEntries(wpi::span<const Entry> entries);
+  void WriteEntry(std::string_view name, const Value& value);
   bool WriteType(NT_Type type);
   void WriteValue(const Value& value);
 
@@ -44,7 +41,7 @@
 }  // namespace
 
 /* Escapes and writes a string, including start and end double quotes */
-void SavePersistentImpl::WriteString(wpi::StringRef str) {
+void SavePersistentImpl::WriteString(std::string_view str) {
   m_os << '"';
   for (auto c : str) {
     switch (c) {
@@ -75,7 +72,7 @@
   m_os << '"';
 }
 
-void SavePersistentImpl::Save(wpi::ArrayRef<Entry> entries) {
+void SavePersistentImpl::Save(wpi::span<const Entry> entries) {
   WriteHeader();
   WriteEntries(entries);
 }
@@ -84,19 +81,23 @@
   m_os << "[NetworkTables Storage 3.0]\n";
 }
 
-void SavePersistentImpl::WriteEntries(wpi::ArrayRef<Entry> entries) {
+void SavePersistentImpl::WriteEntries(wpi::span<const Entry> entries) {
   for (auto& i : entries) {
-    if (!i.second) continue;
+    if (!i.second) {
+      continue;
+    }
     WriteEntry(i.first, *i.second);
   }
 }
 
-void SavePersistentImpl::WriteEntry(wpi::StringRef name, const Value& value) {
-  if (!WriteType(value.type())) return;  // type
-  WriteString(name);                     // name
-  m_os << '=';                           // '='
-  WriteValue(value);                     // value
-  m_os << '\n';                          // eol
+void SavePersistentImpl::WriteEntry(std::string_view name, const Value& value) {
+  if (!WriteType(value.type())) {
+    return;  // type
+  }
+  WriteString(name);  // name
+  m_os << '=';        // '='
+  WriteValue(value);  // value
+  m_os << '\n';       // eol
 }
 
 bool SavePersistentImpl::WriteType(NT_Type type) {
@@ -134,7 +135,7 @@
       m_os << (value.GetBoolean() ? "true" : "false");
       break;
     case NT_DOUBLE:
-      m_os << wpi::format("%g", value.GetDouble());
+      m_os << fmt::format("{:g}", value.GetDouble());
       break;
     case NT_STRING:
       WriteString(value.GetString());
@@ -146,7 +147,9 @@
     case NT_BOOLEAN_ARRAY: {
       bool first = true;
       for (auto elem : value.GetBooleanArray()) {
-        if (!first) m_os << ',';
+        if (!first) {
+          m_os << ',';
+        }
         first = false;
         m_os << (elem ? "true" : "false");
       }
@@ -155,16 +158,20 @@
     case NT_DOUBLE_ARRAY: {
       bool first = true;
       for (auto elem : value.GetDoubleArray()) {
-        if (!first) m_os << ',';
+        if (!first) {
+          m_os << ',';
+        }
         first = false;
-        m_os << wpi::format("%g", elem);
+        m_os << fmt::format("{:g}", elem);
       }
       break;
     }
     case NT_STRING_ARRAY: {
       bool first = true;
       for (auto& elem : value.GetStringArray()) {
-        if (!first) m_os << ',';
+        if (!first) {
+          m_os << ',';
+        }
         first = false;
         WriteString(elem);
       }
@@ -177,33 +184,34 @@
 
 void Storage::SavePersistent(wpi::raw_ostream& os, bool periodic) const {
   std::vector<SavePersistentImpl::Entry> entries;
-  if (!GetPersistentEntries(periodic, &entries)) return;
+  if (!GetPersistentEntries(periodic, &entries)) {
+    return;
+  }
   SavePersistentImpl(os).Save(entries);
 }
 
-const char* Storage::SavePersistent(const Twine& filename,
+const char* Storage::SavePersistent(std::string_view filename,
                                     bool periodic) const {
-  wpi::SmallString<128> fn;
-  filename.toVector(fn);
-  wpi::SmallString<128> tmp = fn;
-  tmp += ".tmp";
-  wpi::SmallString<128> bak = fn;
-  bak += ".bak";
+  std::string fn{filename};
+  auto tmp = fmt::format("{}.tmp", filename);
+  auto bak = fmt::format("{}.bak", filename);
 
   // Get entries before creating file
   std::vector<SavePersistentImpl::Entry> entries;
-  if (!GetPersistentEntries(periodic, &entries)) return nullptr;
+  if (!GetPersistentEntries(periodic, &entries)) {
+    return nullptr;
+  }
 
   const char* err = nullptr;
 
   // start by writing to temporary file
   std::error_code ec;
-  wpi::raw_fd_ostream os(tmp, ec, wpi::sys::fs::F_Text);
+  wpi::raw_fd_ostream os(tmp, ec, fs::F_Text);
   if (ec.value() != 0) {
     err = "could not open file";
     goto done;
   }
-  DEBUG0("saving persistent file '" << filename << "'");
+  DEBUG0("saving persistent file '{}'", filename);
   SavePersistentImpl(os).Save(entries);
   os.close();
   if (os.has_error()) {
@@ -223,36 +231,39 @@
 
 done:
   // try again if there was an error
-  if (err && periodic) m_persistent_dirty = true;
+  if (err && periodic) {
+    m_persistent_dirty = true;
+  }
   return err;
 }
 
-void Storage::SaveEntries(wpi::raw_ostream& os, const Twine& prefix) const {
+void Storage::SaveEntries(wpi::raw_ostream& os, std::string_view prefix) const {
   std::vector<SavePersistentImpl::Entry> entries;
-  if (!GetEntries(prefix, &entries)) return;
+  if (!GetEntries(prefix, &entries)) {
+    return;
+  }
   SavePersistentImpl(os).Save(entries);
 }
 
-const char* Storage::SaveEntries(const Twine& filename,
-                                 const Twine& prefix) const {
-  wpi::SmallString<128> fn;
-  filename.toVector(fn);
-  wpi::SmallString<128> tmp = fn;
-  tmp += ".tmp";
-  wpi::SmallString<128> bak = fn;
-  bak += ".bak";
+const char* Storage::SaveEntries(std::string_view filename,
+                                 std::string_view prefix) const {
+  std::string fn{filename};
+  auto tmp = fmt::format("{}.tmp", filename);
+  auto bak = fmt::format("{}.bak", filename);
 
   // Get entries before creating file
   std::vector<SavePersistentImpl::Entry> entries;
-  if (!GetEntries(prefix, &entries)) return nullptr;
+  if (!GetEntries(prefix, &entries)) {
+    return nullptr;
+  }
 
   // start by writing to temporary file
   std::error_code ec;
-  wpi::raw_fd_ostream os(tmp, ec, wpi::sys::fs::F_Text);
+  wpi::raw_fd_ostream os(tmp, ec, fs::F_Text);
   if (ec.value() != 0) {
     return "could not open file";
   }
-  DEBUG0("saving file '" << filename << "'");
+  DEBUG0("saving file '{}'", filename);
   SavePersistentImpl(os).Save(entries);
   os.close();
   if (os.has_error()) {
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Value.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/Value.cpp
index 3f58b73..e0d65c9 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Value.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Value.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <stdint.h>
 
+#include <cstring>
+
 #include <wpi/MemAlloc.h>
 #include <wpi/timestamp.h>
 
@@ -22,28 +21,31 @@
 
 Value::Value(NT_Type type, uint64_t time, const private_init&) {
   m_val.type = type;
-  if (time == 0)
+  if (time == 0) {
     m_val.last_change = wpi::Now();
-  else
+  } else {
     m_val.last_change = time;
-  if (m_val.type == NT_BOOLEAN_ARRAY)
+  }
+  if (m_val.type == NT_BOOLEAN_ARRAY) {
     m_val.data.arr_boolean.arr = nullptr;
-  else if (m_val.type == NT_DOUBLE_ARRAY)
+  } else if (m_val.type == NT_DOUBLE_ARRAY) {
     m_val.data.arr_double.arr = nullptr;
-  else if (m_val.type == NT_STRING_ARRAY)
+  } else if (m_val.type == NT_STRING_ARRAY) {
     m_val.data.arr_string.arr = nullptr;
+  }
 }
 
 Value::~Value() {
-  if (m_val.type == NT_BOOLEAN_ARRAY)
+  if (m_val.type == NT_BOOLEAN_ARRAY) {
     delete[] m_val.data.arr_boolean.arr;
-  else if (m_val.type == NT_DOUBLE_ARRAY)
+  } else if (m_val.type == NT_DOUBLE_ARRAY) {
     delete[] m_val.data.arr_double.arr;
-  else if (m_val.type == NT_STRING_ARRAY)
+  } else if (m_val.type == NT_STRING_ARRAY) {
     delete[] m_val.data.arr_string.arr;
+  }
 }
 
-std::shared_ptr<Value> Value::MakeBooleanArray(wpi::ArrayRef<bool> value,
+std::shared_ptr<Value> Value::MakeBooleanArray(wpi::span<const bool> value,
                                                uint64_t time) {
   auto val = std::make_shared<Value>(NT_BOOLEAN_ARRAY, time, private_init());
   val->m_val.data.arr_boolean.arr = new int[value.size()];
@@ -52,7 +54,7 @@
   return val;
 }
 
-std::shared_ptr<Value> Value::MakeBooleanArray(wpi::ArrayRef<int> value,
+std::shared_ptr<Value> Value::MakeBooleanArray(wpi::span<const int> value,
                                                uint64_t time) {
   auto val = std::make_shared<Value>(NT_BOOLEAN_ARRAY, time, private_init());
   val->m_val.data.arr_boolean.arr = new int[value.size()];
@@ -61,7 +63,7 @@
   return val;
 }
 
-std::shared_ptr<Value> Value::MakeDoubleArray(wpi::ArrayRef<double> value,
+std::shared_ptr<Value> Value::MakeDoubleArray(wpi::span<const double> value,
                                               uint64_t time) {
   auto val = std::make_shared<Value>(NT_DOUBLE_ARRAY, time, private_init());
   val->m_val.data.arr_double.arr = new double[value.size()];
@@ -70,10 +72,10 @@
   return val;
 }
 
-std::shared_ptr<Value> Value::MakeStringArray(wpi::ArrayRef<std::string> value,
-                                              uint64_t time) {
+std::shared_ptr<Value> Value::MakeStringArray(
+    wpi::span<const std::string> value, uint64_t time) {
   auto val = std::make_shared<Value>(NT_STRING_ARRAY, time, private_init());
-  val->m_string_array = value;
+  val->m_string_array.assign(value.begin(), value.end());
   // point NT_Value to the contents in the vector.
   val->m_val.data.arr_string.arr = new NT_String[value.size()];
   val->m_val.data.arr_string.size = val->m_string_array.size();
@@ -140,8 +142,9 @@
       auto v = in.GetStringArray();
       out->data.arr_string.arr = static_cast<NT_String*>(
           wpi::safe_malloc(v.size() * sizeof(NT_String)));
-      for (size_t i = 0; i < v.size(); ++i)
+      for (size_t i = 0; i < v.size(); ++i) {
         ConvertToC(v[i], &out->data.arr_string.arr[i]);
+      }
       out->data.arr_string.size = v.size();
       break;
     }
@@ -152,7 +155,7 @@
   out->type = in.type();
 }
 
-void nt::ConvertToC(wpi::StringRef in, NT_String* out) {
+void nt::ConvertToC(std::string_view in, NT_String* out) {
   out->len = in.size();
   out->str = static_cast<char*>(wpi::safe_malloc(in.size() + 1));
   std::memcpy(out->str, in.data(), in.size());
@@ -174,16 +177,17 @@
     case NT_RPC:
       return Value::MakeRpc(ConvertFromC(value.data.v_raw));
     case NT_BOOLEAN_ARRAY:
-      return Value::MakeBooleanArray(wpi::ArrayRef<int>(
-          value.data.arr_boolean.arr, value.data.arr_boolean.size));
+      return Value::MakeBooleanArray(
+          wpi::span(value.data.arr_boolean.arr, value.data.arr_boolean.size));
     case NT_DOUBLE_ARRAY:
-      return Value::MakeDoubleArray(wpi::ArrayRef<double>(
-          value.data.arr_double.arr, value.data.arr_double.size));
+      return Value::MakeDoubleArray(
+          wpi::span(value.data.arr_double.arr, value.data.arr_double.size));
     case NT_STRING_ARRAY: {
       std::vector<std::string> v;
       v.reserve(value.data.arr_string.size);
-      for (size_t i = 0; i < value.data.arr_string.size; ++i)
-        v.push_back(ConvertFromC(value.data.arr_string.arr[i]));
+      for (size_t i = 0; i < value.data.arr_string.size; ++i) {
+        v.emplace_back(ConvertFromC(value.data.arr_string.arr[i]));
+      }
       return Value::MakeStringArray(std::move(v));
     }
     default:
@@ -193,7 +197,9 @@
 }
 
 bool nt::operator==(const Value& lhs, const Value& rhs) {
-  if (lhs.type() != rhs.type()) return false;
+  if (lhs.type() != rhs.type()) {
+    return false;
+  }
   switch (lhs.type()) {
     case NT_UNASSIGNED:
       return true;  // XXX: is this better being false instead?
@@ -206,15 +212,17 @@
     case NT_RPC:
       return lhs.m_string == rhs.m_string;
     case NT_BOOLEAN_ARRAY:
-      if (lhs.m_val.data.arr_boolean.size != rhs.m_val.data.arr_boolean.size)
+      if (lhs.m_val.data.arr_boolean.size != rhs.m_val.data.arr_boolean.size) {
         return false;
+      }
       return std::memcmp(lhs.m_val.data.arr_boolean.arr,
                          rhs.m_val.data.arr_boolean.arr,
                          lhs.m_val.data.arr_boolean.size *
                              sizeof(lhs.m_val.data.arr_boolean.arr[0])) == 0;
     case NT_DOUBLE_ARRAY:
-      if (lhs.m_val.data.arr_double.size != rhs.m_val.data.arr_double.size)
+      if (lhs.m_val.data.arr_double.size != rhs.m_val.data.arr_double.size) {
         return false;
+      }
       return std::memcmp(lhs.m_val.data.arr_double.arr,
                          rhs.m_val.data.arr_double.arr,
                          lhs.m_val.data.arr_double.size *
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/Value_internal.h b/third_party/allwpilib/ntcore/src/main/native/cpp/Value_internal.h
index ea25777..54850ab 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/Value_internal.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/Value_internal.h
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_VALUE_INTERNAL_H_
 #define NTCORE_VALUE_INTERNAL_H_
 
 #include <memory>
 #include <string>
-
-#include <wpi/StringRef.h>
+#include <string_view>
 
 #include "ntcore_c.h"
 
@@ -21,9 +17,9 @@
 
 void ConvertToC(const Value& in, NT_Value* out);
 std::shared_ptr<Value> ConvertFromC(const NT_Value& value);
-void ConvertToC(wpi::StringRef in, NT_String* out);
-inline wpi::StringRef ConvertFromC(const NT_String& str) {
-  return wpi::StringRef(str.str, str.len);
+void ConvertToC(std::string_view in, NT_String* out);
+inline std::string_view ConvertFromC(const NT_String& str) {
+  return {str.str, str.len};
 }
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.cpp
index 07c85d2..c0647dc 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WireDecoder.h"
 
@@ -58,27 +55,37 @@
   m_error = nullptr;
 }
 
-WireDecoder::~WireDecoder() { std::free(m_buf); }
+WireDecoder::~WireDecoder() {
+  std::free(m_buf);
+}
 
 bool WireDecoder::ReadDouble(double* val) {
   const char* buf;
-  if (!Read(&buf, 8)) return false;
+  if (!Read(&buf, 8)) {
+    return false;
+  }
   *val = ::ReadDouble(buf);
   return true;
 }
 
 void WireDecoder::Realloc(size_t len) {
   // Double current buffer size until we have enough space.
-  if (m_allocated >= len) return;
+  if (m_allocated >= len) {
+    return;
+  }
   size_t newlen = m_allocated * 2;
-  while (newlen < len) newlen *= 2;
+  while (newlen < len) {
+    newlen *= 2;
+  }
   m_buf = static_cast<char*>(wpi::safe_realloc(m_buf, newlen));
   m_allocated = newlen;
 }
 
 bool WireDecoder::ReadType(NT_Type* type) {
   unsigned int itype;
-  if (!Read8(&itype)) return false;
+  if (!Read8(&itype)) {
+    return false;
+  }
   // Convert from byte value to enum
   switch (itype) {
     case 0x00:
@@ -117,17 +124,23 @@
   switch (type) {
     case NT_BOOLEAN: {
       unsigned int v;
-      if (!Read8(&v)) return nullptr;
+      if (!Read8(&v)) {
+        return nullptr;
+      }
       return Value::MakeBoolean(v != 0);
     }
     case NT_DOUBLE: {
       double v;
-      if (!ReadDouble(&v)) return nullptr;
+      if (!ReadDouble(&v)) {
+        return nullptr;
+      }
       return Value::MakeDouble(v);
     }
     case NT_STRING: {
       std::string v;
-      if (!ReadString(&v)) return nullptr;
+      if (!ReadString(&v)) {
+        return nullptr;
+      }
       return Value::MakeString(std::move(v));
     }
     case NT_RAW: {
@@ -136,7 +149,9 @@
         return nullptr;
       }
       std::string v;
-      if (!ReadString(&v)) return nullptr;
+      if (!ReadString(&v)) {
+        return nullptr;
+      }
       return Value::MakeRaw(std::move(v));
     }
     case NT_RPC: {
@@ -145,42 +160,60 @@
         return nullptr;
       }
       std::string v;
-      if (!ReadString(&v)) return nullptr;
+      if (!ReadString(&v)) {
+        return nullptr;
+      }
       return Value::MakeRpc(std::move(v));
     }
     case NT_BOOLEAN_ARRAY: {
       // size
       unsigned int size;
-      if (!Read8(&size)) return nullptr;
+      if (!Read8(&size)) {
+        return nullptr;
+      }
 
       // array values
       const char* buf;
-      if (!Read(&buf, size)) return nullptr;
+      if (!Read(&buf, size)) {
+        return nullptr;
+      }
       std::vector<int> v(size);
-      for (unsigned int i = 0; i < size; ++i) v[i] = buf[i] ? 1 : 0;
+      for (unsigned int i = 0; i < size; ++i) {
+        v[i] = buf[i] ? 1 : 0;
+      }
       return Value::MakeBooleanArray(std::move(v));
     }
     case NT_DOUBLE_ARRAY: {
       // size
       unsigned int size;
-      if (!Read8(&size)) return nullptr;
+      if (!Read8(&size)) {
+        return nullptr;
+      }
 
       // array values
       const char* buf;
-      if (!Read(&buf, size * 8)) return nullptr;
+      if (!Read(&buf, size * 8)) {
+        return nullptr;
+      }
       std::vector<double> v(size);
-      for (unsigned int i = 0; i < size; ++i) v[i] = ::ReadDouble(buf);
+      for (unsigned int i = 0; i < size; ++i) {
+        v[i] = ::ReadDouble(buf);
+      }
       return Value::MakeDoubleArray(std::move(v));
     }
     case NT_STRING_ARRAY: {
       // size
       unsigned int size;
-      if (!Read8(&size)) return nullptr;
+      if (!Read8(&size)) {
+        return nullptr;
+      }
 
       // array values
       std::vector<std::string> v(size);
       for (unsigned int i = 0; i < size; ++i) {
-        if (!ReadString(&v[i])) return nullptr;
+        if (!ReadString(&v[i])) {
+          return nullptr;
+        }
       }
       return Value::MakeStringArray(std::move(v));
     }
@@ -194,15 +227,21 @@
   size_t len;
   if (m_proto_rev < 0x0300u) {
     unsigned int v;
-    if (!Read16(&v)) return false;
+    if (!Read16(&v)) {
+      return false;
+    }
     len = v;
   } else {
     uint64_t v;
-    if (!ReadUleb128(&v)) return false;
+    if (!ReadUleb128(&v)) {
+      return false;
+    }
     len = v;
   }
   const char* buf;
-  if (!Read(&buf, len)) return false;
-  *str = wpi::StringRef(buf, len);
+  if (!Read(&buf, len)) {
+    return false;
+  }
+  str->assign(buf, len);
   return true;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.h b/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.h
index 6b4483b..972be57 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/WireDecoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_WIREDECODER_H_
 #define NTCORE_WIREDECODER_H_
@@ -59,7 +56,9 @@
    * Caution: the buffer is only temporarily valid.
    */
   bool Read(const char** buf, size_t len) {
-    if (len > m_allocated) Realloc(len);
+    if (len > m_allocated) {
+      Realloc(len);
+    }
     *buf = m_buf;
     m_is.read(m_buf, len);
 #if 0
@@ -81,7 +80,9 @@
   /* Reads a single byte. */
   bool Read8(unsigned int* val) {
     const char* buf;
-    if (!Read(&buf, 1)) return false;
+    if (!Read(&buf, 1)) {
+      return false;
+    }
     *val = (*reinterpret_cast<const unsigned char*>(buf)) & 0xff;
     return true;
   }
@@ -89,7 +90,9 @@
   /* Reads a 16-bit word. */
   bool Read16(unsigned int* val) {
     const char* buf;
-    if (!Read(&buf, 2)) return false;
+    if (!Read(&buf, 2)) {
+      return false;
+    }
     unsigned int v = (*reinterpret_cast<const unsigned char*>(buf)) & 0xff;
     ++buf;
     v <<= 8;
@@ -101,7 +104,9 @@
   /* Reads a 32-bit word. */
   bool Read32(uint32_t* val) {
     const char* buf;
-    if (!Read(&buf, 4)) return false;
+    if (!Read(&buf, 4)) {
+      return false;
+    }
     unsigned int v = (*reinterpret_cast<const unsigned char*>(buf)) & 0xff;
     ++buf;
     v <<= 8;
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.cpp
index 6538349..b35b780 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WireEncoder.h"
 
@@ -33,7 +30,9 @@
        static_cast<char>((v >> 8) & 0xff), static_cast<char>(v & 0xff)});
 }
 
-void WireEncoder::WriteUleb128(uint32_t val) { wpi::WriteUleb128(m_data, val); }
+void WireEncoder::WriteUleb128(uint32_t val) {
+  wpi::WriteUleb128(m_data, val);
+}
 
 void WireEncoder::WriteType(NT_Type type) {
   char ch;
@@ -87,29 +86,41 @@
     case NT_STRING:
       return GetStringSize(value.GetString());
     case NT_RAW:
-      if (m_proto_rev < 0x0300u) return 0;
+      if (m_proto_rev < 0x0300u) {
+        return 0;
+      }
       return GetStringSize(value.GetRaw());
     case NT_RPC:
-      if (m_proto_rev < 0x0300u) return 0;
+      if (m_proto_rev < 0x0300u) {
+        return 0;
+      }
       return GetStringSize(value.GetRpc());
     case NT_BOOLEAN_ARRAY: {
       // 1-byte size, 1 byte per element
       size_t size = value.GetBooleanArray().size();
-      if (size > 0xff) size = 0xff;  // size is only 1 byte, truncate
+      if (size > 0xff) {
+        size = 0xff;  // size is only 1 byte, truncate
+      }
       return 1 + size;
     }
     case NT_DOUBLE_ARRAY: {
       // 1-byte size, 8 bytes per element
       size_t size = value.GetDoubleArray().size();
-      if (size > 0xff) size = 0xff;  // size is only 1 byte, truncate
+      if (size > 0xff) {
+        size = 0xff;  // size is only 1 byte, truncate
+      }
       return 1 + size * 8;
     }
     case NT_STRING_ARRAY: {
       auto v = value.GetStringArray();
       size_t size = v.size();
-      if (size > 0xff) size = 0xff;  // size is only 1 byte, truncate
-      size_t len = 1;                // 1-byte size
-      for (size_t i = 0; i < size; ++i) len += GetStringSize(v[i]);
+      if (size > 0xff) {
+        size = 0xff;  // size is only 1 byte, truncate
+      }
+      size_t len = 1;  // 1-byte size
+      for (size_t i = 0; i < size; ++i) {
+        len += GetStringSize(v[i]);
+      }
       return len;
     }
     default:
@@ -145,28 +156,40 @@
     case NT_BOOLEAN_ARRAY: {
       auto v = value.GetBooleanArray();
       size_t size = v.size();
-      if (size > 0xff) size = 0xff;  // size is only 1 byte, truncate
+      if (size > 0xff) {
+        size = 0xff;  // size is only 1 byte, truncate
+      }
       Write8(size);
 
-      for (size_t i = 0; i < size; ++i) Write8(v[i] ? 1 : 0);
+      for (size_t i = 0; i < size; ++i) {
+        Write8(v[i] ? 1 : 0);
+      }
       break;
     }
     case NT_DOUBLE_ARRAY: {
       auto v = value.GetDoubleArray();
       size_t size = v.size();
-      if (size > 0xff) size = 0xff;  // size is only 1 byte, truncate
+      if (size > 0xff) {
+        size = 0xff;  // size is only 1 byte, truncate
+      }
       Write8(size);
 
-      for (size_t i = 0; i < size; ++i) WriteDouble(v[i]);
+      for (size_t i = 0; i < size; ++i) {
+        WriteDouble(v[i]);
+      }
       break;
     }
     case NT_STRING_ARRAY: {
       auto v = value.GetStringArray();
       size_t size = v.size();
-      if (size > 0xff) size = 0xff;  // size is only 1 byte, truncate
+      if (size > 0xff) {
+        size = 0xff;  // size is only 1 byte, truncate
+      }
       Write8(size);
 
-      for (size_t i = 0; i < size; ++i) WriteString(v[i]);
+      for (size_t i = 0; i < size; ++i) {
+        WriteString(v[i]);
+      }
       break;
     }
     default:
@@ -175,20 +198,24 @@
   }
 }
 
-size_t WireEncoder::GetStringSize(wpi::StringRef str) const {
+size_t WireEncoder::GetStringSize(std::string_view str) const {
   if (m_proto_rev < 0x0300u) {
     size_t len = str.size();
-    if (len > 0xffff) len = 0xffff;  // Limited to 64K length; truncate
+    if (len > 0xffff) {
+      len = 0xffff;  // Limited to 64K length; truncate
+    }
     return 2 + len;
   }
   return wpi::SizeUleb128(str.size()) + str.size();
 }
 
-void WireEncoder::WriteString(wpi::StringRef str) {
+void WireEncoder::WriteString(std::string_view str) {
   // length
   size_t len = str.size();
   if (m_proto_rev < 0x0300u) {
-    if (len > 0xffff) len = 0xffff;  // Limited to 64K length; truncate
+    if (len > 0xffff) {
+      len = 0xffff;  // Limited to 64K length; truncate
+    }
     Write16(len);
   } else {
     WriteUleb128(len);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.h b/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.h
index c4f769c..84edc39 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.h
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/WireEncoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_WIREENCODER_H_
 #define NTCORE_WIREENCODER_H_
@@ -12,9 +9,9 @@
 
 #include <cassert>
 #include <cstddef>
+#include <string_view>
 
 #include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
 
 #include "networktables/NetworkTableValue.h"
 
@@ -52,8 +49,8 @@
   /* Returns number of bytes written to memory buffer. */
   size_t size() const { return m_data.size(); }
 
-  wpi::StringRef ToStringRef() const {
-    return wpi::StringRef(m_data.data(), m_data.size());
+  std::string_view ToStringView() const {
+    return {m_data.data(), m_data.size()};
   }
 
   /* Writes a single byte. */
@@ -83,7 +80,7 @@
 
   void WriteType(NT_Type type);
   void WriteValue(const Value& value);
-  void WriteString(wpi::StringRef str);
+  void WriteString(std::string_view str);
 
   /* Utility function to get the written size of a value (without actually
    * writing it).
@@ -93,7 +90,7 @@
   /* Utility function to get the written size of a string (without actually
    * writing it).
    */
-  size_t GetStringSize(wpi::StringRef str) const;
+  size_t GetStringSize(std::string_view str) const;
 
  protected:
   /* The protocol revision.  E.g. 0x0200 for version 2.0. */
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
index ac00a3c..6c7ed82 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/jni/NetworkTablesJNI.cpp
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
 #include <cassert>
 
+#include <fmt/format.h>
 #include <wpi/ConvertUTF.h>
-#include <wpi/SmallString.h>
 #include <wpi/jni_util.h>
-#include <wpi/raw_ostream.h>
 
 #include "edu_wpi_first_networktables_NetworkTablesJNI.h"
 #include "ntcore.h"
@@ -67,18 +63,23 @@
   jvm = vm;
 
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
+  }
 
   // Cache references to classes
   for (auto& c : classes) {
     *c.cls = JClass(env, c.name);
-    if (!*c.cls) return JNI_ERR;
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
   for (auto& c : exceptions) {
     *c.cls = JException(env, c.name);
-    if (!*c.cls) return JNI_ERR;
+    if (!*c.cls) {
+      return JNI_ERR;
+    }
   }
 
   return JNI_VERSION_1_6;
@@ -86,8 +87,9 @@
 
 JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return;
+  }
   // Delete global references
   for (auto& c : classes) {
     c.cls->free(env);
@@ -107,40 +109,52 @@
 inline std::shared_ptr<nt::Value> FromJavaRaw(JNIEnv* env, jbyteArray jarr,
                                               jlong time) {
   CriticalJByteArrayRef ref{env, jarr};
-  if (!ref) return nullptr;
-  return nt::Value::MakeRaw(ref, time);
+  if (!ref) {
+    return nullptr;
+  }
+  return nt::Value::MakeRaw(ref.str(), time);
 }
 
 inline std::shared_ptr<nt::Value> FromJavaRawBB(JNIEnv* env, jobject jbb,
                                                 int len, jlong time) {
   JByteArrayRef ref{env, jbb, len};
-  if (!ref) return nullptr;
+  if (!ref) {
+    return nullptr;
+  }
   return nt::Value::MakeRaw(ref.str(), time);
 }
 
 inline std::shared_ptr<nt::Value> FromJavaRpc(JNIEnv* env, jbyteArray jarr,
                                               jlong time) {
   CriticalJByteArrayRef ref{env, jarr};
-  if (!ref) return nullptr;
+  if (!ref) {
+    return nullptr;
+  }
   return nt::Value::MakeRpc(ref.str(), time);
 }
 
 std::shared_ptr<nt::Value> FromJavaBooleanArray(JNIEnv* env, jbooleanArray jarr,
                                                 jlong time) {
   CriticalJBooleanArrayRef ref{env, jarr};
-  if (!ref) return nullptr;
-  wpi::ArrayRef<jboolean> elements{ref};
+  if (!ref) {
+    return nullptr;
+  }
+  wpi::span<const jboolean> elements{ref};
   size_t len = elements.size();
   std::vector<int> arr;
   arr.reserve(len);
-  for (size_t i = 0; i < len; ++i) arr.push_back(elements[i]);
+  for (size_t i = 0; i < len; ++i) {
+    arr.push_back(elements[i]);
+  }
   return nt::Value::MakeBooleanArray(arr, time);
 }
 
 std::shared_ptr<nt::Value> FromJavaDoubleArray(JNIEnv* env, jdoubleArray jarr,
                                                jlong time) {
   CriticalJDoubleArrayRef ref{env, jarr};
-  if (!ref) return nullptr;
+  if (!ref) {
+    return nullptr;
+  }
   return nt::Value::MakeDoubleArray(ref, time);
 }
 
@@ -152,8 +166,10 @@
   for (size_t i = 0; i < len; ++i) {
     JLocal<jstring> elem{
         env, static_cast<jstring>(env->GetObjectArrayElement(jarr, i))};
-    if (!elem) return nullptr;
-    arr.push_back(JStringRef{env, elem}.str());
+    if (!elem) {
+      return nullptr;
+    }
+    arr.emplace_back(JStringRef{env, elem}.str());
   }
   return nt::Value::MakeStringArray(std::move(arr), time);
 }
@@ -165,18 +181,20 @@
 static jobject MakeJObject(JNIEnv* env, const nt::Value& value) {
   static jmethodID booleanConstructor = nullptr;
   static jmethodID doubleConstructor = nullptr;
-  if (!booleanConstructor)
+  if (!booleanConstructor) {
     booleanConstructor = env->GetMethodID(booleanCls, "<init>", "(Z)V");
-  if (!doubleConstructor)
+  }
+  if (!doubleConstructor) {
     doubleConstructor = env->GetMethodID(doubleCls, "<init>", "(D)V");
+  }
 
   switch (value.type()) {
     case NT_BOOLEAN:
       return env->NewObject(booleanCls, booleanConstructor,
-                            (jboolean)(value.GetBoolean() ? 1 : 0));
+                            static_cast<jboolean>(value.GetBoolean() ? 1 : 0));
     case NT_DOUBLE:
       return env->NewObject(doubleCls, doubleConstructor,
-                            (jdouble)value.GetDouble());
+                            static_cast<jdouble>(value.GetDouble()));
     case NT_STRING:
       return MakeJString(env, value.GetString());
     case NT_RAW:
@@ -197,11 +215,14 @@
 static jobject MakeJValue(JNIEnv* env, const nt::Value* value) {
   static jmethodID constructor =
       env->GetMethodID(valueCls, "<init>", "(ILjava/lang/Object;J)V");
-  if (!value)
-    return env->NewObject(valueCls, constructor, (jint)NT_UNASSIGNED, nullptr,
-                          (jlong)0);
-  return env->NewObject(valueCls, constructor, (jint)value->type(),
-                        MakeJObject(env, *value), (jlong)value->time());
+  if (!value) {
+    return env->NewObject(valueCls, constructor,
+                          static_cast<jint>(NT_UNASSIGNED), nullptr,
+                          static_cast<jlong>(0));
+  }
+  return env->NewObject(valueCls, constructor, static_cast<jint>(value->type()),
+                        MakeJObject(env, *value),
+                        static_cast<jlong>(value->time()));
 }
 
 static jobject MakeJObject(JNIEnv* env, const nt::ConnectionInfo& info) {
@@ -211,8 +232,9 @@
   JLocal<jstring> remote_id{env, MakeJString(env, info.remote_id)};
   JLocal<jstring> remote_ip{env, MakeJString(env, info.remote_ip)};
   return env->NewObject(connectionInfoCls, constructor, remote_id.obj(),
-                        remote_ip.obj(), (jint)info.remote_port,
-                        (jlong)info.last_update, (jint)info.protocol_version);
+                        remote_ip.obj(), static_cast<jint>(info.remote_port),
+                        static_cast<jlong>(info.last_update),
+                        static_cast<jint>(info.protocol_version));
 }
 
 static jobject MakeJObject(JNIEnv* env, jobject inst,
@@ -223,8 +245,9 @@
       "networktables/ConnectionInfo;)V");
   JLocal<jobject> conn{env, MakeJObject(env, notification.conn)};
   return env->NewObject(connectionNotificationCls, constructor, inst,
-                        (jint)notification.listener,
-                        (jboolean)notification.connected, conn.obj());
+                        static_cast<jint>(notification.listener),
+                        static_cast<jboolean>(notification.connected),
+                        conn.obj());
 }
 
 static jobject MakeJObject(JNIEnv* env, jobject inst,
@@ -234,9 +257,10 @@
                        "(Ledu/wpi/first/networktables/"
                        "NetworkTableInstance;ILjava/lang/String;IIJ)V");
   JLocal<jstring> name{env, MakeJString(env, info.name)};
-  return env->NewObject(entryInfoCls, constructor, inst, (jint)info.entry,
-                        name.obj(), (jint)info.type, (jint)info.flags,
-                        (jlong)info.last_change);
+  return env->NewObject(
+      entryInfoCls, constructor, inst, static_cast<jint>(info.entry),
+      name.obj(), static_cast<jint>(info.type), static_cast<jint>(info.flags),
+      static_cast<jlong>(info.last_change));
 }
 
 static jobject MakeJObject(JNIEnv* env, jobject inst,
@@ -248,8 +272,9 @@
   JLocal<jstring> name{env, MakeJString(env, notification.name)};
   JLocal<jobject> value{env, MakeJValue(env, notification.value.get())};
   return env->NewObject(entryNotificationCls, constructor, inst,
-                        (jint)notification.listener, (jint)notification.entry,
-                        name.obj(), value.obj(), (jint)notification.flags);
+                        static_cast<jint>(notification.listener),
+                        static_cast<jint>(notification.entry), name.obj(),
+                        value.obj(), static_cast<jint>(notification.flags));
 }
 
 static jobject MakeJObject(JNIEnv* env, jobject inst,
@@ -260,9 +285,10 @@
       "String;ILjava/lang/String;)V");
   JLocal<jstring> filename{env, MakeJString(env, msg.filename)};
   JLocal<jstring> message{env, MakeJString(env, msg.message)};
-  return env->NewObject(logMessageCls, constructor, inst, (jint)msg.logger,
-                        (jint)msg.level, filename.obj(), (jint)msg.line,
-                        message.obj());
+  return env->NewObject(logMessageCls, constructor, inst,
+                        static_cast<jint>(msg.logger),
+                        static_cast<jint>(msg.level), filename.obj(),
+                        static_cast<jint>(msg.line), message.obj());
 }
 
 static jobject MakeJObject(JNIEnv* env, jobject inst,
@@ -275,16 +301,19 @@
   JLocal<jstring> name{env, MakeJString(env, answer.name)};
   JLocal<jbyteArray> params{env, MakeJByteArray(env, answer.params)};
   JLocal<jobject> conn{env, MakeJObject(env, answer.conn)};
-  return env->NewObject(rpcAnswerCls, constructor, inst, (jint)answer.entry,
-                        (jint)answer.call, name.obj(), params.obj(),
-                        conn.obj());
+  return env->NewObject(
+      rpcAnswerCls, constructor, inst, static_cast<jint>(answer.entry),
+      static_cast<jint>(answer.call), name.obj(), params.obj(), conn.obj());
 }
 
-static jobjectArray MakeJObject(JNIEnv* env, jobject inst,
-                                wpi::ArrayRef<nt::ConnectionNotification> arr) {
+static jobjectArray MakeJObject(
+    JNIEnv* env, jobject inst,
+    wpi::span<const nt::ConnectionNotification> arr) {
   jobjectArray jarr =
       env->NewObjectArray(arr.size(), connectionNotificationCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> elem{env, MakeJObject(env, inst, arr[i])};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -293,10 +322,12 @@
 }
 
 static jobjectArray MakeJObject(JNIEnv* env, jobject inst,
-                                wpi::ArrayRef<nt::EntryNotification> arr) {
+                                wpi::span<const nt::EntryNotification> arr) {
   jobjectArray jarr =
       env->NewObjectArray(arr.size(), entryNotificationCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> elem{env, MakeJObject(env, inst, arr[i])};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -305,9 +336,11 @@
 }
 
 static jobjectArray MakeJObject(JNIEnv* env, jobject inst,
-                                wpi::ArrayRef<nt::LogMessage> arr) {
+                                wpi::span<const nt::LogMessage> arr) {
   jobjectArray jarr = env->NewObjectArray(arr.size(), logMessageCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> elem{env, MakeJObject(env, inst, arr[i])};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -316,9 +349,11 @@
 }
 
 static jobjectArray MakeJObject(JNIEnv* env, jobject inst,
-                                wpi::ArrayRef<nt::RpcAnswer> arr) {
+                                wpi::span<const nt::RpcAnswer> arr) {
   jobjectArray jarr = env->NewObjectArray(arr.size(), rpcAnswerCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> elem{env, MakeJObject(env, inst, arr[i])};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -516,7 +551,9 @@
     return false;
   }
   auto v = FromJavaRaw(env, value, time);
-  if (!v) return false;
+  if (!v) {
+    return false;
+  }
   if (force) {
     nt::SetEntryTypeValue(entry, v);
     return JNI_TRUE;
@@ -539,7 +576,9 @@
     return false;
   }
   auto v = FromJavaRawBB(env, value, len, time);
-  if (!v) return false;
+  if (!v) {
+    return false;
+  }
   if (force) {
     nt::SetEntryTypeValue(entry, v);
     return JNI_TRUE;
@@ -562,7 +601,9 @@
     return false;
   }
   auto v = FromJavaBooleanArray(env, value, time);
-  if (!v) return false;
+  if (!v) {
+    return false;
+  }
   if (force) {
     nt::SetEntryTypeValue(entry, v);
     return JNI_TRUE;
@@ -585,7 +626,9 @@
     return false;
   }
   auto v = FromJavaDoubleArray(env, value, time);
-  if (!v) return false;
+  if (!v) {
+    return false;
+  }
   if (force) {
     nt::SetEntryTypeValue(entry, v);
     return JNI_TRUE;
@@ -608,7 +651,9 @@
     return false;
   }
   auto v = FromJavaStringArray(env, value, time);
-  if (!v) return false;
+  if (!v) {
+    return false;
+  }
   if (force) {
     nt::SetEntryTypeValue(entry, v);
     return JNI_TRUE;
@@ -639,7 +684,9 @@
   (JNIEnv*, jclass, jint entry, jboolean defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsBoolean()) return defaultValue;
+  if (!val || !val->IsBoolean()) {
+    return defaultValue;
+  }
   return val->GetBoolean();
 }
 
@@ -653,7 +700,9 @@
   (JNIEnv*, jclass, jint entry, jdouble defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsDouble()) return defaultValue;
+  if (!val || !val->IsDouble()) {
+    return defaultValue;
+  }
   return val->GetDouble();
 }
 
@@ -667,7 +716,9 @@
   (JNIEnv* env, jclass, jint entry, jstring defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsString()) return defaultValue;
+  if (!val || !val->IsString()) {
+    return defaultValue;
+  }
   return MakeJString(env, val->GetString());
 }
 
@@ -681,7 +732,9 @@
   (JNIEnv* env, jclass, jint entry, jbyteArray defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsRaw()) return defaultValue;
+  if (!val || !val->IsRaw()) {
+    return defaultValue;
+  }
   return MakeJByteArray(env, val->GetRaw());
 }
 
@@ -695,7 +748,9 @@
   (JNIEnv* env, jclass, jint entry, jbooleanArray defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsBooleanArray()) return defaultValue;
+  if (!val || !val->IsBooleanArray()) {
+    return defaultValue;
+  }
   return MakeJBooleanArray(env, val->GetBooleanArray());
 }
 
@@ -709,7 +764,9 @@
   (JNIEnv* env, jclass, jint entry, jdoubleArray defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsDoubleArray()) return defaultValue;
+  if (!val || !val->IsDoubleArray()) {
+    return defaultValue;
+  }
   return MakeJDoubleArray(env, val->GetDoubleArray());
 }
 
@@ -723,7 +780,9 @@
   (JNIEnv* env, jclass, jint entry, jobjectArray defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsStringArray()) return defaultValue;
+  if (!val || !val->IsStringArray()) {
+    return defaultValue;
+  }
   return MakeJStringArray(env, val->GetStringArray());
 }
 
@@ -914,7 +973,9 @@
   }
   auto arr = nt::GetEntryInfo(inst, JStringRef{env, prefix}.str(), types);
   jobjectArray jarr = env->NewObjectArray(arr.size(), entryInfoCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> jelem{env, MakeJObject(env, instObject, arr[i])};
     env->SetObjectArrayElement(jarr, i, jelem);
@@ -1190,7 +1251,7 @@
     nullPointerEx.Throw(env, "def cannot be null");
     return;
   }
-  nt::CreatePolledRpc(entry, JByteArrayRef{env, def}, poller);
+  nt::CreatePolledRpc(entry, JByteArrayRef{env, def}.str(), poller);
 }
 
 /*
@@ -1265,7 +1326,7 @@
     nullPointerEx.Throw(env, "result cannot be null");
     return false;
   }
-  return nt::PostRpcResponse(entry, call, JByteArrayRef{env, result});
+  return nt::PostRpcResponse(entry, call, JByteArrayRef{env, result}.str());
 }
 
 /*
@@ -1281,7 +1342,7 @@
     nullPointerEx.Throw(env, "params cannot be null");
     return 0;
   }
-  return nt::CallRpc(entry, JByteArrayRef{env, params});
+  return nt::CallRpc(entry, JByteArrayRef{env, params}.str());
 }
 
 /*
@@ -1294,7 +1355,9 @@
   (JNIEnv* env, jclass, jint entry, jint call)
 {
   std::string result;
-  if (!nt::GetRpcResult(entry, call, &result)) return nullptr;
+  if (!nt::GetRpcResult(entry, call, &result)) {
+    return nullptr;
+  }
   return MakeJByteArray(env, result);
 }
 
@@ -1309,8 +1372,9 @@
 {
   std::string result;
   bool timed_out = false;
-  if (!nt::GetRpcResult(entry, call, &result, timeout, &timed_out))
+  if (!nt::GetRpcResult(entry, call, &result, timeout, &timed_out)) {
     return nullptr;
+  }
   return MakeJByteArray(env, result);
 }
 
@@ -1336,7 +1400,9 @@
   (JNIEnv* env, jclass, jint entry, jbyteArray defaultValue)
 {
   auto val = nt::GetEntryValue(entry);
-  if (!val || !val->IsRpc()) return defaultValue;
+  if (!val || !val->IsRpc()) {
+    return defaultValue;
+  }
   return MakeJByteArray(env, val->GetRpc());
 }
 
@@ -1478,10 +1544,12 @@
     return;
   }
   jint* portInts = env->GetIntArrayElements(ports, nullptr);
-  if (!portInts) return;
+  if (!portInts) {
+    return;
+  }
 
   std::vector<std::string> names;
-  std::vector<std::pair<nt::StringRef, unsigned int>> servers;
+  std::vector<std::pair<std::string_view, unsigned int>> servers;
   names.reserve(len);
   servers.reserve(len);
   for (int i = 0; i < len; ++i) {
@@ -1493,7 +1561,7 @@
     }
     names.emplace_back(JStringRef{env, elem}.str());
     servers.emplace_back(
-        std::make_pair(nt::StringRef(names.back()), portInts[i]));
+        std::make_pair(std::string_view{names.back()}, portInts[i]));
   }
   env->ReleaseIntArrayElements(ports, portInts, JNI_ABORT);
   nt::StartClient(inst, servers);
@@ -1563,10 +1631,12 @@
     return;
   }
   jint* portInts = env->GetIntArrayElements(ports, nullptr);
-  if (!portInts) return;
+  if (!portInts) {
+    return;
+  }
 
   std::vector<std::string> names;
-  std::vector<std::pair<nt::StringRef, unsigned int>> servers;
+  std::vector<std::pair<std::string_view, unsigned int>> servers;
   names.reserve(len);
   servers.reserve(len);
   for (int i = 0; i < len; ++i) {
@@ -1578,7 +1648,7 @@
     }
     names.emplace_back(JStringRef{env, elem}.str());
     servers.emplace_back(
-        std::make_pair(nt::StringRef(names.back()), portInts[i]));
+        std::make_pair(std::string_view{names.back()}, portInts[i]));
   }
   env->ReleaseIntArrayElements(ports, portInts, JNI_ABORT);
   nt::SetServer(inst, servers);
@@ -1656,7 +1726,9 @@
   auto arr = nt::GetConnections(inst);
   jobjectArray jarr =
       env->NewObjectArray(arr.size(), connectionInfoCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jobject> jelem{env, MakeJObject(env, arr[i])};
     env->SetObjectArrayElement(jarr, i, jelem);
@@ -1690,7 +1762,9 @@
     return;
   }
   const char* err = nt::SavePersistent(inst, JStringRef{env, filename}.str());
-  if (err) persistentEx.Throw(env, err);
+  if (err) {
+    persistentEx.Throw(env, err);
+  }
 }
 
 /*
@@ -1707,13 +1781,10 @@
     return nullptr;
   }
   std::vector<std::string> warns;
-  const char* err = nt::LoadPersistent(inst, JStringRef{env, filename}.str(),
-                                       [&](size_t line, const char* msg) {
-                                         wpi::SmallString<128> warn;
-                                         wpi::raw_svector_ostream oss(warn);
-                                         oss << line << ": " << msg;
-                                         warns.emplace_back(oss.str());
-                                       });
+  const char* err = nt::LoadPersistent(
+      inst, JStringRef{env, filename}.str(), [&](size_t line, const char* msg) {
+        warns.emplace_back(fmt::format("{}: {}", line, msg));
+      });
   if (err) {
     persistentEx.Throw(env, err);
     return nullptr;
@@ -1740,7 +1811,9 @@
   }
   const char* err = nt::SaveEntries(inst, JStringRef{env, filename}.str(),
                                     JStringRef{env, prefix}.str());
-  if (err) persistentEx.Throw(env, err);
+  if (err) {
+    persistentEx.Throw(env, err);
+  }
 }
 
 /*
@@ -1761,14 +1834,11 @@
     return nullptr;
   }
   std::vector<std::string> warns;
-  const char* err = nt::LoadEntries(inst, JStringRef{env, filename}.str(),
-                                    JStringRef{env, prefix}.str(),
-                                    [&](size_t line, const char* msg) {
-                                      wpi::SmallString<128> warn;
-                                      wpi::raw_svector_ostream oss(warn);
-                                      oss << line << ": " << msg;
-                                      warns.emplace_back(oss.str());
-                                    });
+  const char* err = nt::LoadEntries(
+      inst, JStringRef{env, filename}.str(), JStringRef{env, prefix}.str(),
+      [&](size_t line, const char* msg) {
+        warns.emplace_back(fmt::format("{}: {}", line, msg));
+      });
   if (err) {
     persistentEx.Throw(env, err);
     return nullptr;
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NTSendable.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NTSendable.cpp
new file mode 100644
index 0000000..211fcc5
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NTSendable.cpp
@@ -0,0 +1,17 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "networktables/NTSendable.h"
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "networktables/NTSendableBuilder.h"
+
+using namespace nt;
+
+void NTSendable::InitSendable(wpi::SendableBuilder& builder) {
+  if (builder.GetBackendKind() == wpi::SendableBuilder::kNetworkTables) {
+    InitSendable(static_cast<NTSendableBuilder&>(builder));
+  }
+}
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NTSendableBuilder.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NTSendableBuilder.cpp
new file mode 100644
index 0000000..df9fc47
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NTSendableBuilder.cpp
@@ -0,0 +1,11 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "networktables/NTSendableBuilder.h"
+
+using namespace nt;
+
+NTSendableBuilder::BackendKind NTSendableBuilder::GetBackendKind() const {
+  return kNetworkTables;
+}
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp
index ca6e8a6..cc519f5 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTable.cpp
@@ -1,77 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "networktables/NetworkTable.h"
 
 #include <algorithm>
 
+#include <fmt/core.h>
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
 #include <wpi/StringMap.h>
-#include <wpi/raw_ostream.h>
 
 #include "networktables/NetworkTableInstance.h"
 #include "ntcore.h"
-#include "tables/ITableListener.h"
 
 using namespace nt;
 
-#ifdef __GNUC__
-#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#elif _WIN32
-#pragma warning(disable : 4996)
-#endif
-
-const char NetworkTable::PATH_SEPARATOR_CHAR = '/';
-std::string NetworkTable::s_persistent_filename = "networktables.ini";
-bool NetworkTable::s_client = false;
-bool NetworkTable::s_enable_ds = true;
-bool NetworkTable::s_running = false;
-unsigned int NetworkTable::s_port = NT_DEFAULT_PORT;
-
-StringRef NetworkTable::BasenameKey(StringRef key) {
+std::string_view NetworkTable::BasenameKey(std::string_view key) {
   size_t slash = key.rfind(PATH_SEPARATOR_CHAR);
-  if (slash == StringRef::npos) return key;
+  if (slash == std::string_view::npos) {
+    return key;
+  }
   return key.substr(slash + 1);
 }
 
-std::string NetworkTable::NormalizeKey(const Twine& key,
+std::string NetworkTable::NormalizeKey(std::string_view key,
                                        bool withLeadingSlash) {
   wpi::SmallString<128> buf;
-  return NormalizeKey(key, buf, withLeadingSlash);
+  return std::string{NormalizeKey(key, buf, withLeadingSlash)};
 }
 
-StringRef NetworkTable::NormalizeKey(const Twine& key,
-                                     wpi::SmallVectorImpl<char>& buf,
-                                     bool withLeadingSlash) {
+std::string_view NetworkTable::NormalizeKey(std::string_view key,
+                                            wpi::SmallVectorImpl<char>& buf,
+                                            bool withLeadingSlash) {
   buf.clear();
-  if (withLeadingSlash) buf.push_back(PATH_SEPARATOR_CHAR);
+  if (withLeadingSlash) {
+    buf.push_back(PATH_SEPARATOR_CHAR);
+  }
   // for each path element, add it with a slash following
-  wpi::SmallString<128> keyBuf;
-  StringRef keyStr = key.toStringRef(keyBuf);
-  wpi::SmallVector<StringRef, 16> parts;
-  keyStr.split(parts, PATH_SEPARATOR_CHAR, -1, false);
+  wpi::SmallVector<std::string_view, 16> parts;
+  wpi::split(key, parts, PATH_SEPARATOR_CHAR, -1, false);
   for (auto i = parts.begin(); i != parts.end(); ++i) {
     buf.append(i->begin(), i->end());
     buf.push_back(PATH_SEPARATOR_CHAR);
   }
   // remove trailing slash if the input key didn't have one
-  if (!keyStr.empty() && keyStr.back() != PATH_SEPARATOR_CHAR) buf.pop_back();
-  return StringRef(buf.data(), buf.size());
+  if (!key.empty() && key.back() != PATH_SEPARATOR_CHAR) {
+    buf.pop_back();
+  }
+  return {buf.data(), buf.size()};
 }
 
-std::vector<std::string> NetworkTable::GetHierarchy(const Twine& key) {
+std::vector<std::string> NetworkTable::GetHierarchy(std::string_view key) {
   std::vector<std::string> hierarchy;
   hierarchy.emplace_back(1, PATH_SEPARATOR_CHAR);
   // for each path element, add it to the end of what we built previously
-  wpi::SmallString<128> keyBuf;
-  StringRef keyStr = key.toStringRef(keyBuf);
   wpi::SmallString<128> path;
-  wpi::SmallVector<StringRef, 16> parts;
-  keyStr.split(parts, PATH_SEPARATOR_CHAR, -1, false);
+  wpi::SmallVector<std::string_view, 16> parts;
+  wpi::split(key, parts, PATH_SEPARATOR_CHAR, -1, false);
   if (!parts.empty()) {
     for (auto i = parts.begin(); i != parts.end(); ++i) {
       path += PATH_SEPARATOR_CHAR;
@@ -79,7 +66,7 @@
       hierarchy.emplace_back(path.str());
     }
     // handle trailing slash
-    if (keyStr.back() == PATH_SEPARATOR_CHAR) {
+    if (key.back() == PATH_SEPARATOR_CHAR) {
       path += PATH_SEPARATOR_CHAR;
       hierarchy.emplace_back(path.str());
     }
@@ -87,129 +74,27 @@
   return hierarchy;
 }
 
-void NetworkTable::Initialize() {
-  if (s_running) Shutdown();
-  auto inst = NetworkTableInstance::GetDefault();
-  if (s_client) {
-    inst.StartClient();
-    if (s_enable_ds) inst.StartDSClient(s_port);
-  } else {
-    inst.StartServer(s_persistent_filename, "", s_port);
-  }
-  s_running = true;
-}
-
-void NetworkTable::Shutdown() {
-  if (!s_running) return;
-  auto inst = NetworkTableInstance::GetDefault();
-  if (s_client) {
-    inst.StopDSClient();
-    inst.StopClient();
-  } else {
-    inst.StopServer();
-  }
-  s_running = false;
-}
-
-void NetworkTable::SetClientMode() { s_client = true; }
-
-void NetworkTable::SetServerMode() { s_client = false; }
-
-void NetworkTable::SetTeam(int team) {
-  auto inst = NetworkTableInstance::GetDefault();
-  inst.SetServerTeam(team, s_port);
-  if (s_enable_ds) inst.StartDSClient(s_port);
-}
-
-void NetworkTable::SetIPAddress(StringRef address) {
-  auto inst = NetworkTableInstance::GetDefault();
-  wpi::SmallString<32> addr_copy{address};
-  inst.SetServer(addr_copy.c_str(), s_port);
-
-  // Stop the DS client if we're explicitly connecting to localhost
-  if (address == "localhost" || address == "127.0.0.1")
-    inst.StopDSClient();
-  else if (s_enable_ds)
-    inst.StartDSClient(s_port);
-}
-
-void NetworkTable::SetIPAddress(ArrayRef<std::string> addresses) {
-  auto inst = NetworkTableInstance::GetDefault();
-  wpi::SmallVector<StringRef, 8> servers;
-  for (const auto& ip_address : addresses) servers.emplace_back(ip_address);
-  inst.SetServer(servers, s_port);
-
-  // Stop the DS client if we're explicitly connecting to localhost
-  if (!addresses.empty() &&
-      (addresses[0] == "localhost" || addresses[0] == "127.0.0.1"))
-    inst.StopDSClient();
-  else if (s_enable_ds)
-    inst.StartDSClient(s_port);
-}
-
-void NetworkTable::SetPort(unsigned int port) { s_port = port; }
-
-void NetworkTable::SetDSClientEnabled(bool enabled) {
-  auto inst = NetworkTableInstance::GetDefault();
-  s_enable_ds = enabled;
-  if (s_enable_ds)
-    inst.StartDSClient(s_port);
-  else
-    inst.StopDSClient();
-}
-
-void NetworkTable::SetPersistentFilename(StringRef filename) {
-  s_persistent_filename = filename;
-}
-
-void NetworkTable::SetNetworkIdentity(StringRef name) {
-  NetworkTableInstance::GetDefault().SetNetworkIdentity(name);
-}
-
-void NetworkTable::GlobalDeleteAll() {
-  NetworkTableInstance::GetDefault().DeleteAllEntries();
-}
-
-void NetworkTable::Flush() { NetworkTableInstance::GetDefault().Flush(); }
-
-void NetworkTable::SetUpdateRate(double interval) {
-  NetworkTableInstance::GetDefault().SetUpdateRate(interval);
-}
-
-const char* NetworkTable::SavePersistent(StringRef filename) {
-  return NetworkTableInstance::GetDefault().SavePersistent(filename);
-}
-
-const char* NetworkTable::LoadPersistent(
-    StringRef filename,
-    std::function<void(size_t line, const char* msg)> warn) {
-  return NetworkTableInstance::GetDefault().LoadPersistent(filename, warn);
-}
-
-std::shared_ptr<NetworkTable> NetworkTable::GetTable(StringRef key) {
-  if (!s_running) Initialize();
-  return NetworkTableInstance::GetDefault().GetTable(key);
-}
-
-NetworkTable::NetworkTable(NT_Inst inst, const Twine& path, const private_init&)
-    : m_inst(inst), m_path(path.str()) {}
+NetworkTable::NetworkTable(NT_Inst inst, std::string_view path,
+                           const private_init&)
+    : m_inst(inst), m_path(path) {}
 
 NetworkTable::~NetworkTable() {
-  for (auto& i : m_listeners) RemoveEntryListener(i.second);
-  for (auto i : m_lambdaListeners) RemoveEntryListener(i);
+  for (auto i : m_listeners) {
+    RemoveEntryListener(i);
+  }
 }
 
 NetworkTableInstance NetworkTable::GetInstance() const {
   return NetworkTableInstance{m_inst};
 }
 
-NetworkTableEntry NetworkTable::GetEntry(const Twine& key) const {
-  wpi::SmallString<128> keyBuf;
-  StringRef keyStr = key.toStringRef(keyBuf);
+NetworkTableEntry NetworkTable::GetEntry(std::string_view key) const {
   std::scoped_lock lock(m_mutex);
-  NT_Entry& entry = m_entries[keyStr];
+  NT_Entry& entry = m_entries[key];
   if (entry == 0) {
-    entry = nt::GetEntry(m_inst, m_path + Twine(PATH_SEPARATOR_CHAR) + keyStr);
+    fmt::memory_buffer buf;
+    fmt::format_to(fmt::appender{buf}, "{}/{}", m_path, key);
+    entry = nt::GetEntry(m_inst, {buf.data(), buf.size()});
   }
   return NetworkTableEntry{entry};
 }
@@ -218,17 +103,19 @@
                                                 unsigned int flags) const {
   size_t prefix_len = m_path.size() + 1;
   return nt::AddEntryListener(
-      m_inst, m_path + Twine(PATH_SEPARATOR_CHAR),
+      m_inst, fmt::format("{}/", m_path),
       [=](const EntryNotification& event) {
-        StringRef relative_key = event.name.substr(prefix_len);
-        if (relative_key.find(PATH_SEPARATOR_CHAR) != StringRef::npos) return;
+        auto relative_key = std::string_view{event.name}.substr(prefix_len);
+        if (relative_key.find(PATH_SEPARATOR_CHAR) != std::string_view::npos) {
+          return;
+        }
         listener(const_cast<NetworkTable*>(this), relative_key,
                  NetworkTableEntry{event.entry}, event.value, event.flags);
       },
       flags);
 }
 
-NT_EntryListener NetworkTable::AddEntryListener(const Twine& key,
+NT_EntryListener NetworkTable::AddEntryListener(std::string_view key,
                                                 TableEntryListener listener,
                                                 unsigned int flags) const {
   size_t prefix_len = m_path.size() + 1;
@@ -236,8 +123,9 @@
   return nt::AddEntryListener(
       entry.GetHandle(),
       [=](const EntryNotification& event) {
-        listener(const_cast<NetworkTable*>(this), event.name.substr(prefix_len),
-                 entry, event.value, event.flags);
+        listener(const_cast<NetworkTable*>(this),
+                 std::string_view{event.name}.substr(prefix_len), entry,
+                 event.value, event.flags);
       },
       flags);
 }
@@ -246,60 +134,6 @@
   nt::RemoveEntryListener(listener);
 }
 
-void NetworkTable::AddTableListener(ITableListener* listener) {
-  AddTableListenerEx(listener, NT_NOTIFY_NEW | NT_NOTIFY_UPDATE);
-}
-
-void NetworkTable::AddTableListener(ITableListener* listener,
-                                    bool immediateNotify) {
-  unsigned int flags = NT_NOTIFY_NEW | NT_NOTIFY_UPDATE;
-  if (immediateNotify) flags |= NT_NOTIFY_IMMEDIATE;
-  AddTableListenerEx(listener, flags);
-}
-
-void NetworkTable::AddTableListenerEx(ITableListener* listener,
-                                      unsigned int flags) {
-  std::scoped_lock lock(m_mutex);
-  wpi::SmallString<128> path(m_path);
-  path += PATH_SEPARATOR_CHAR;
-  size_t prefix_len = path.size();
-  NT_EntryListener id = nt::AddEntryListener(
-      m_inst, path,
-      [=](const EntryNotification& event) {
-        StringRef relative_key = event.name.substr(prefix_len);
-        if (relative_key.find(PATH_SEPARATOR_CHAR) != StringRef::npos) return;
-        listener->ValueChangedEx(this, relative_key, event.value, event.flags);
-      },
-      flags);
-  m_listeners.emplace_back(listener, id);
-}
-
-void NetworkTable::AddTableListener(StringRef key, ITableListener* listener,
-                                    bool immediateNotify) {
-  unsigned int flags = NT_NOTIFY_NEW | NT_NOTIFY_UPDATE;
-  if (immediateNotify) flags |= NT_NOTIFY_IMMEDIATE;
-  AddTableListenerEx(key, listener, flags);
-}
-
-void NetworkTable::AddTableListenerEx(StringRef key, ITableListener* listener,
-                                      unsigned int flags) {
-  std::scoped_lock lock(m_mutex);
-  size_t prefix_len = m_path.size() + 1;
-  auto entry = GetEntry(key);
-  NT_EntryListener id = nt::AddEntryListener(
-      entry.GetHandle(),
-      [=](const EntryNotification& event) {
-        listener->ValueChangedEx(this, event.name.substr(prefix_len),
-                                 event.value, event.flags);
-      },
-      flags);
-  m_listeners.emplace_back(listener, id);
-}
-
-void NetworkTable::AddSubTableListener(ITableListener* listener) {
-  AddSubTableListener(listener, false);
-}
-
 NT_EntryListener NetworkTable::AddSubTableListener(TableListener listener,
                                                    bool localNotify) {
   size_t prefix_len = m_path.size() + 1;
@@ -309,99 +143,64 @@
   auto notified_tables = std::make_shared<wpi::StringMap<char>>();
 
   unsigned int flags = NT_NOTIFY_NEW | NT_NOTIFY_IMMEDIATE;
-  if (localNotify) flags |= NT_NOTIFY_LOCAL;
+  if (localNotify) {
+    flags |= NT_NOTIFY_LOCAL;
+  }
   NT_EntryListener id = nt::AddEntryListener(
-      m_inst, m_path + Twine(PATH_SEPARATOR_CHAR),
+      m_inst, fmt::format("{}/", m_path),
       [=](const EntryNotification& event) {
-        StringRef relative_key = event.name.substr(prefix_len);
+        auto relative_key = std::string_view{event.name}.substr(prefix_len);
         auto end_sub_table = relative_key.find(PATH_SEPARATOR_CHAR);
-        if (end_sub_table == StringRef::npos) return;
-        StringRef sub_table_key = relative_key.substr(0, end_sub_table);
-        if (notified_tables->find(sub_table_key) == notified_tables->end())
+        if (end_sub_table == std::string_view::npos) {
           return;
+        }
+        auto sub_table_key = relative_key.substr(0, end_sub_table);
+        if (notified_tables->find(sub_table_key) == notified_tables->end()) {
+          return;
+        }
         notified_tables->insert(std::make_pair(sub_table_key, '\0'));
         listener(this, sub_table_key, this->GetSubTable(sub_table_key));
       },
       flags);
-  m_lambdaListeners.emplace_back(id);
+  m_listeners.emplace_back(id);
   return id;
 }
 
 void NetworkTable::RemoveTableListener(NT_EntryListener listener) {
   nt::RemoveEntryListener(listener);
   auto matches_begin =
-      std::remove(m_lambdaListeners.begin(), m_lambdaListeners.end(), listener);
-  m_lambdaListeners.erase(matches_begin, m_lambdaListeners.end());
-}
-
-void NetworkTable::AddSubTableListener(ITableListener* listener,
-                                       bool localNotify) {
-  std::scoped_lock lock(m_mutex);
-  size_t prefix_len = m_path.size() + 1;
-
-  // The lambda needs to be copyable, but StringMap is not, so use
-  // a shared_ptr to it.
-  auto notified_tables = std::make_shared<wpi::StringMap<char>>();
-
-  unsigned int flags = NT_NOTIFY_NEW | NT_NOTIFY_IMMEDIATE;
-  if (localNotify) flags |= NT_NOTIFY_LOCAL;
-  NT_EntryListener id = nt::AddEntryListener(
-      m_inst, m_path + Twine(PATH_SEPARATOR_CHAR),
-      [=](const EntryNotification& event) {
-        StringRef relative_key = event.name.substr(prefix_len);
-        auto end_sub_table = relative_key.find(PATH_SEPARATOR_CHAR);
-        if (end_sub_table == StringRef::npos) return;
-        StringRef sub_table_key = relative_key.substr(0, end_sub_table);
-        if (notified_tables->find(sub_table_key) == notified_tables->end())
-          return;
-        notified_tables->insert(std::make_pair(sub_table_key, '\0'));
-        listener->ValueChangedEx(this, sub_table_key, nullptr, event.flags);
-      },
-      flags);
-  m_listeners.emplace_back(listener, id);
-}
-
-void NetworkTable::RemoveTableListener(ITableListener* listener) {
-  std::scoped_lock lock(m_mutex);
-  auto matches_begin =
-      std::remove_if(m_listeners.begin(), m_listeners.end(),
-                     [=](const Listener& x) { return x.first == listener; });
-
-  for (auto i = matches_begin; i != m_listeners.end(); ++i)
-    RemoveEntryListener(i->second);
+      std::remove(m_listeners.begin(), m_listeners.end(), listener);
   m_listeners.erase(matches_begin, m_listeners.end());
 }
 
 std::shared_ptr<NetworkTable> NetworkTable::GetSubTable(
-    const Twine& key) const {
+    std::string_view key) const {
   return std::make_shared<NetworkTable>(
-      m_inst, m_path + Twine(PATH_SEPARATOR_CHAR) + key, private_init{});
+      m_inst, fmt::format("{}/{}", m_path, key), private_init{});
 }
 
-bool NetworkTable::ContainsKey(const Twine& key) const {
-  if (key.isTriviallyEmpty() ||
-      (key.isSingleStringRef() && key.getSingleStringRef().empty()))
+bool NetworkTable::ContainsKey(std::string_view key) const {
+  if (key.empty()) {
     return false;
+  }
   return GetEntry(key).Exists();
 }
 
-bool NetworkTable::ContainsSubTable(const Twine& key) const {
-  return !GetEntryInfo(m_inst,
-                       m_path + Twine(PATH_SEPARATOR_CHAR) + key +
-                           Twine(PATH_SEPARATOR_CHAR),
-                       0)
-              .empty();
+bool NetworkTable::ContainsSubTable(std::string_view key) const {
+  return !GetEntryInfo(m_inst, fmt::format("{}/{}/", m_path, key), 0).empty();
 }
 
 std::vector<std::string> NetworkTable::GetKeys(int types) const {
   std::vector<std::string> keys;
   size_t prefix_len = m_path.size() + 1;
-  auto infos = GetEntryInfo(m_inst, m_path + Twine(PATH_SEPARATOR_CHAR), types);
+  auto infos = GetEntryInfo(m_inst, fmt::format("{}/", m_path), types);
   std::scoped_lock lock(m_mutex);
   for (auto& info : infos) {
-    auto relative_key = StringRef(info.name).substr(prefix_len);
-    if (relative_key.find(PATH_SEPARATOR_CHAR) != StringRef::npos) continue;
-    keys.push_back(relative_key);
+    auto relative_key = std::string_view{info.name}.substr(prefix_len);
+    if (relative_key.find(PATH_SEPARATOR_CHAR) != std::string_view::npos) {
+      continue;
+    }
+    keys.emplace_back(relative_key);
     m_entries[relative_key] = info.entry;
   }
   return keys;
@@ -410,155 +209,167 @@
 std::vector<std::string> NetworkTable::GetSubTables() const {
   std::vector<std::string> keys;
   size_t prefix_len = m_path.size() + 1;
-  for (auto& entry :
-       GetEntryInfo(m_inst, m_path + Twine(PATH_SEPARATOR_CHAR), 0)) {
-    auto relative_key = StringRef(entry.name).substr(prefix_len);
+  for (auto& entry : GetEntryInfo(m_inst, fmt::format("{}/", m_path), 0)) {
+    auto relative_key = std::string_view{entry.name}.substr(prefix_len);
     size_t end_subtable = relative_key.find(PATH_SEPARATOR_CHAR);
-    if (end_subtable == StringRef::npos) continue;
-    keys.push_back(relative_key.substr(0, end_subtable));
+    if (end_subtable == std::string_view::npos) {
+      continue;
+    }
+    keys.emplace_back(relative_key.substr(0, end_subtable));
   }
   return keys;
 }
 
-void NetworkTable::SetPersistent(StringRef key) {
+void NetworkTable::SetPersistent(std::string_view key) {
   GetEntry(key).SetPersistent();
 }
 
-void NetworkTable::ClearPersistent(StringRef key) {
+void NetworkTable::ClearPersistent(std::string_view key) {
   GetEntry(key).ClearPersistent();
 }
 
-bool NetworkTable::IsPersistent(StringRef key) const {
+bool NetworkTable::IsPersistent(std::string_view key) const {
   return GetEntry(key).IsPersistent();
 }
 
-void NetworkTable::SetFlags(StringRef key, unsigned int flags) {
+void NetworkTable::SetFlags(std::string_view key, unsigned int flags) {
   GetEntry(key).SetFlags(flags);
 }
 
-void NetworkTable::ClearFlags(StringRef key, unsigned int flags) {
+void NetworkTable::ClearFlags(std::string_view key, unsigned int flags) {
   GetEntry(key).ClearFlags(flags);
 }
 
-unsigned int NetworkTable::GetFlags(StringRef key) const {
+unsigned int NetworkTable::GetFlags(std::string_view key) const {
   return GetEntry(key).GetFlags();
 }
 
-void NetworkTable::Delete(const Twine& key) { GetEntry(key).Delete(); }
+void NetworkTable::Delete(std::string_view key) {
+  GetEntry(key).Delete();
+}
 
-bool NetworkTable::PutNumber(StringRef key, double value) {
+bool NetworkTable::PutNumber(std::string_view key, double value) {
   return GetEntry(key).SetDouble(value);
 }
 
-bool NetworkTable::SetDefaultNumber(StringRef key, double defaultValue) {
+bool NetworkTable::SetDefaultNumber(std::string_view key, double defaultValue) {
   return GetEntry(key).SetDefaultDouble(defaultValue);
 }
 
-double NetworkTable::GetNumber(StringRef key, double defaultValue) const {
+double NetworkTable::GetNumber(std::string_view key,
+                               double defaultValue) const {
   return GetEntry(key).GetDouble(defaultValue);
 }
 
-bool NetworkTable::PutString(StringRef key, StringRef value) {
+bool NetworkTable::PutString(std::string_view key, std::string_view value) {
   return GetEntry(key).SetString(value);
 }
 
-bool NetworkTable::SetDefaultString(StringRef key, StringRef defaultValue) {
+bool NetworkTable::SetDefaultString(std::string_view key,
+                                    std::string_view defaultValue) {
   return GetEntry(key).SetDefaultString(defaultValue);
 }
 
-std::string NetworkTable::GetString(StringRef key,
-                                    StringRef defaultValue) const {
+std::string NetworkTable::GetString(std::string_view key,
+                                    std::string_view defaultValue) const {
   return GetEntry(key).GetString(defaultValue);
 }
 
-bool NetworkTable::PutBoolean(StringRef key, bool value) {
+bool NetworkTable::PutBoolean(std::string_view key, bool value) {
   return GetEntry(key).SetBoolean(value);
 }
 
-bool NetworkTable::SetDefaultBoolean(StringRef key, bool defaultValue) {
+bool NetworkTable::SetDefaultBoolean(std::string_view key, bool defaultValue) {
   return GetEntry(key).SetDefaultBoolean(defaultValue);
 }
 
-bool NetworkTable::GetBoolean(StringRef key, bool defaultValue) const {
+bool NetworkTable::GetBoolean(std::string_view key, bool defaultValue) const {
   return GetEntry(key).GetBoolean(defaultValue);
 }
 
-bool NetworkTable::PutBooleanArray(StringRef key, ArrayRef<int> value) {
+bool NetworkTable::PutBooleanArray(std::string_view key,
+                                   wpi::span<const int> value) {
   return GetEntry(key).SetBooleanArray(value);
 }
 
-bool NetworkTable::SetDefaultBooleanArray(StringRef key,
-                                          ArrayRef<int> defaultValue) {
+bool NetworkTable::SetDefaultBooleanArray(std::string_view key,
+                                          wpi::span<const int> defaultValue) {
   return GetEntry(key).SetDefaultBooleanArray(defaultValue);
 }
 
 std::vector<int> NetworkTable::GetBooleanArray(
-    StringRef key, ArrayRef<int> defaultValue) const {
+    std::string_view key, wpi::span<const int> defaultValue) const {
   return GetEntry(key).GetBooleanArray(defaultValue);
 }
 
-bool NetworkTable::PutNumberArray(StringRef key, ArrayRef<double> value) {
+bool NetworkTable::PutNumberArray(std::string_view key,
+                                  wpi::span<const double> value) {
   return GetEntry(key).SetDoubleArray(value);
 }
 
-bool NetworkTable::SetDefaultNumberArray(StringRef key,
-                                         ArrayRef<double> defaultValue) {
+bool NetworkTable::SetDefaultNumberArray(std::string_view key,
+                                         wpi::span<const double> defaultValue) {
   return GetEntry(key).SetDefaultDoubleArray(defaultValue);
 }
 
 std::vector<double> NetworkTable::GetNumberArray(
-    StringRef key, ArrayRef<double> defaultValue) const {
+    std::string_view key, wpi::span<const double> defaultValue) const {
   return GetEntry(key).GetDoubleArray(defaultValue);
 }
 
-bool NetworkTable::PutStringArray(StringRef key, ArrayRef<std::string> value) {
+bool NetworkTable::PutStringArray(std::string_view key,
+                                  wpi::span<const std::string> value) {
   return GetEntry(key).SetStringArray(value);
 }
 
-bool NetworkTable::SetDefaultStringArray(StringRef key,
-                                         ArrayRef<std::string> defaultValue) {
+bool NetworkTable::SetDefaultStringArray(
+    std::string_view key, wpi::span<const std::string> defaultValue) {
   return GetEntry(key).SetDefaultStringArray(defaultValue);
 }
 
 std::vector<std::string> NetworkTable::GetStringArray(
-    StringRef key, ArrayRef<std::string> defaultValue) const {
+    std::string_view key, wpi::span<const std::string> defaultValue) const {
   return GetEntry(key).GetStringArray(defaultValue);
 }
 
-bool NetworkTable::PutRaw(StringRef key, StringRef value) {
+bool NetworkTable::PutRaw(std::string_view key, std::string_view value) {
   return GetEntry(key).SetRaw(value);
 }
 
-bool NetworkTable::SetDefaultRaw(StringRef key, StringRef defaultValue) {
+bool NetworkTable::SetDefaultRaw(std::string_view key,
+                                 std::string_view defaultValue) {
   return GetEntry(key).SetDefaultRaw(defaultValue);
 }
 
-std::string NetworkTable::GetRaw(StringRef key, StringRef defaultValue) const {
+std::string NetworkTable::GetRaw(std::string_view key,
+                                 std::string_view defaultValue) const {
   return GetEntry(key).GetRaw(defaultValue);
 }
 
-bool NetworkTable::PutValue(const Twine& key, std::shared_ptr<Value> value) {
+bool NetworkTable::PutValue(std::string_view key,
+                            std::shared_ptr<Value> value) {
   return GetEntry(key).SetValue(value);
 }
 
-bool NetworkTable::SetDefaultValue(const Twine& key,
+bool NetworkTable::SetDefaultValue(std::string_view key,
                                    std::shared_ptr<Value> defaultValue) {
   return GetEntry(key).SetDefaultValue(defaultValue);
 }
 
-std::shared_ptr<Value> NetworkTable::GetValue(const Twine& key) const {
+std::shared_ptr<Value> NetworkTable::GetValue(std::string_view key) const {
   return GetEntry(key).GetValue();
 }
 
-StringRef NetworkTable::GetPath() const { return m_path; }
+std::string_view NetworkTable::GetPath() const {
+  return m_path;
+}
 
-const char* NetworkTable::SaveEntries(const Twine& filename) const {
-  return nt::SaveEntries(m_inst, filename, m_path + Twine(PATH_SEPARATOR_CHAR));
+const char* NetworkTable::SaveEntries(std::string_view filename) const {
+  return nt::SaveEntries(m_inst, filename, fmt::format("{}/", m_path));
 }
 
 const char* NetworkTable::LoadEntries(
-    const Twine& filename,
+    std::string_view filename,
     std::function<void(size_t line, const char* msg)> warn) {
-  return nt::LoadEntries(m_inst, filename, m_path + Twine(PATH_SEPARATOR_CHAR),
-                         warn);
+  return nt::LoadEntries(m_inst, filename, fmt::format("{}/", m_path), warn);
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableEntry.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableEntry.cpp
index 5507ac0..abe64e5 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableEntry.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableEntry.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "networktables/NetworkTableEntry.h"
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableInstance.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableInstance.cpp
index 018572e..4566b30 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableInstance.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/NetworkTableInstance.cpp
@@ -1,52 +1,48 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "networktables/NetworkTableInstance.h"
 
-#include <wpi/SmallString.h>
+#include <fmt/format.h>
+#include <wpi/SmallVector.h>
 
 using namespace nt;
 
 std::shared_ptr<NetworkTable> NetworkTableInstance::GetTable(
-    const Twine& key) const {
-  StringRef simple;
-  bool isSimple = key.isSingleStringRef();
-  if (isSimple) simple = key.getSingleStringRef();
-  if (isSimple && (simple.empty() || simple == "/")) {
+    std::string_view key) const {
+  if (key.empty() || key == "/") {
     return std::make_shared<NetworkTable>(m_handle, "",
                                           NetworkTable::private_init{});
-  } else if (isSimple && simple[0] == NetworkTable::PATH_SEPARATOR_CHAR) {
+  } else if (key.front() == NetworkTable::PATH_SEPARATOR_CHAR) {
     return std::make_shared<NetworkTable>(m_handle, key,
                                           NetworkTable::private_init{});
   } else {
-    return std::make_shared<NetworkTable>(
-        m_handle, Twine(NetworkTable::PATH_SEPARATOR_CHAR) + key,
-        NetworkTable::private_init{});
+    return std::make_shared<NetworkTable>(m_handle, fmt::format("/{}", key),
+                                          NetworkTable::private_init{});
   }
 }
 
-void NetworkTableInstance::StartClient(ArrayRef<StringRef> servers,
-                                       unsigned int port) {
-  wpi::SmallVector<std::pair<StringRef, unsigned int>, 8> server_ports;
-  for (const auto& server : servers)
+void NetworkTableInstance::StartClient(
+    wpi::span<const std::string_view> servers, unsigned int port) {
+  wpi::SmallVector<std::pair<std::string_view, unsigned int>, 8> server_ports;
+  for (const auto& server : servers) {
     server_ports.emplace_back(std::make_pair(server, port));
+  }
   StartClient(server_ports);
 }
 
-void NetworkTableInstance::SetServer(ArrayRef<StringRef> servers,
+void NetworkTableInstance::SetServer(wpi::span<const std::string_view> servers,
                                      unsigned int port) {
-  wpi::SmallVector<std::pair<StringRef, unsigned int>, 8> server_ports;
-  for (const auto& server : servers)
+  wpi::SmallVector<std::pair<std::string_view, unsigned int>, 8> server_ports;
+  for (const auto& server : servers) {
     server_ports.emplace_back(std::make_pair(server, port));
+  }
   SetServer(server_ports);
 }
 
 NT_EntryListener NetworkTableInstance::AddEntryListener(
-    const Twine& prefix,
+    std::string_view prefix,
     std::function<void(const EntryNotification& event)> callback,
     unsigned int flags) const {
   return ::nt::AddEntryListener(m_handle, prefix, callback, flags);
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/RpcCall.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/RpcCall.cpp
index 1149681..2192a82 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/RpcCall.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/networktables/RpcCall.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "networktables/RpcCall.h"
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp
index ee8eabc..d910892 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_c.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <stdint.h>
 
 #include <cassert>
 #include <cstdlib>
+#include <cstring>
+#include <string_view>
 
 #include <wpi/MemAlloc.h>
 #include <wpi/timestamp.h>
@@ -20,9 +19,9 @@
 
 // Conversion helpers
 
-static void ConvertToC(wpi::StringRef in, char** out) {
+static void ConvertToC(std::string_view in, char** out) {
   *out = static_cast<char*>(wpi::safe_malloc(in.size() + 1));
-  std::memmove(*out, in.data(), in.size());
+  std::memmove(*out, in.data(), in.size());  // NOLINT
   (*out)[in.size()] = '\0';
 }
 
@@ -59,14 +58,16 @@
   out->num_params = in.params.size();
   out->params = static_cast<NT_RpcParamDef*>(
       wpi::safe_malloc(in.params.size() * sizeof(NT_RpcParamDef)));
-  for (size_t i = 0; i < in.params.size(); ++i)
+  for (size_t i = 0; i < in.params.size(); ++i) {
     ConvertToC(in.params[i], &out->params[i]);
+  }
 
   out->num_results = in.results.size();
   out->results = static_cast<NT_RpcResultDef*>(
       wpi::safe_malloc(in.results.size() * sizeof(NT_RpcResultDef)));
-  for (size_t i = 0; i < in.results.size(); ++i)
+  for (size_t i = 0; i < in.results.size(); ++i) {
     ConvertToC(in.results[i], &out->results[i]);
+  }
 }
 
 static void ConvertToC(const RpcAnswer& in, NT_RpcAnswer* out) {
@@ -95,18 +96,24 @@
 static void ConvertToC(const LogMessage& in, NT_LogMessage* out) {
   out->logger = in.logger;
   out->level = in.level;
-  out->filename = in.filename;
+  ConvertToC(in.filename, &out->filename);
   out->line = in.line;
   ConvertToC(in.message, &out->message);
 }
 
 template <typename O, typename I>
 static O* ConvertToC(const std::vector<I>& in, size_t* out_len) {
-  if (!out_len) return nullptr;
+  if (!out_len) {
+    return nullptr;
+  }
   *out_len = in.size();
-  if (in.empty()) return nullptr;
+  if (in.empty()) {
+    return nullptr;
+  }
   O* out = static_cast<O*>(wpi::safe_malloc(sizeof(O) * in.size()));
-  for (size_t i = 0; i < in.size(); ++i) ConvertToC(in[i], &out[i]);
+  for (size_t i = 0; i < in.size(); ++i) {
+    ConvertToC(in[i], &out[i]);
+  }
   return out;
 }
 
@@ -115,7 +122,9 @@
   std::free(info->remote_ip.str);
 }
 
-static void DisposeEntryInfo(NT_EntryInfo* info) { std::free(info->name.str); }
+static void DisposeEntryInfo(NT_EntryInfo* info) {
+  std::free(info->name.str);
+}
 
 static void DisposeEntryNotification(NT_EntryNotification* info) {
   std::free(info->name.str);
@@ -146,12 +155,14 @@
   out.name = ConvertFromC(in.name);
 
   out.params.reserve(in.num_params);
-  for (size_t i = 0; i < in.num_params; ++i)
+  for (size_t i = 0; i < in.num_params; ++i) {
     out.params.push_back(ConvertFromC(in.params[i]));
+  }
 
   out.results.reserve(in.num_results);
-  for (size_t i = 0; i < in.num_results; ++i)
+  for (size_t i = 0; i < in.num_results; ++i) {
     out.results.push_back(ConvertFromC(in.results[i]));
+  }
 
   return out;
 }
@@ -162,11 +173,17 @@
  * Instance Functions
  */
 
-NT_Inst NT_GetDefaultInstance(void) { return nt::GetDefaultInstance(); }
+NT_Inst NT_GetDefaultInstance(void) {
+  return nt::GetDefaultInstance();
+}
 
-NT_Inst NT_CreateInstance(void) { return nt::CreateInstance(); }
+NT_Inst NT_CreateInstance(void) {
+  return nt::CreateInstance();
+}
 
-void NT_DestroyInstance(NT_Inst inst) { return nt::DestroyInstance(inst); }
+void NT_DestroyInstance(NT_Inst inst) {
+  return nt::DestroyInstance(inst);
+}
 
 NT_Inst NT_GetInstanceFromHandle(NT_Handle handle) {
   return nt::GetInstanceFromHandle(handle);
@@ -177,14 +194,16 @@
  */
 
 NT_Entry NT_GetEntry(NT_Inst inst, const char* name, size_t name_len) {
-  return nt::GetEntry(inst, StringRef(name, name_len));
+  return nt::GetEntry(inst, {name, name_len});
 }
 
 NT_Entry* NT_GetEntries(NT_Inst inst, const char* prefix, size_t prefix_len,
                         unsigned int types, size_t* count) {
-  auto info_v = nt::GetEntries(inst, StringRef(prefix, prefix_len), types);
+  auto info_v = nt::GetEntries(inst, {prefix, prefix_len}, types);
   *count = info_v.size();
-  if (info_v.size() == 0) return nullptr;
+  if (info_v.size() == 0) {
+    return nullptr;
+  }
 
   // create array and copy into it
   NT_Entry* info = static_cast<NT_Entry*>(
@@ -200,7 +219,9 @@
   return v_name.str;
 }
 
-enum NT_Type NT_GetEntryType(NT_Entry entry) { return nt::GetEntryType(entry); }
+enum NT_Type NT_GetEntryType(NT_Entry entry) {
+  return nt::GetEntryType(entry);
+}
 
 uint64_t NT_GetEntryLastChange(NT_Entry entry) {
   return nt::GetEntryLastChange(entry);
@@ -209,7 +230,9 @@
 void NT_GetEntryValue(NT_Entry entry, struct NT_Value* value) {
   NT_InitValue(value);
   auto v = nt::GetEntryValue(entry);
-  if (!v) return;
+  if (!v) {
+    return;
+  }
   ConvertToC(*v, value);
 }
 
@@ -234,20 +257,26 @@
   return nt::GetEntryFlags(entry);
 }
 
-void NT_DeleteEntry(NT_Entry entry) { nt::DeleteEntry(entry); }
+void NT_DeleteEntry(NT_Entry entry) {
+  nt::DeleteEntry(entry);
+}
 
-void NT_DeleteAllEntries(NT_Inst inst) { nt::DeleteAllEntries(inst); }
+void NT_DeleteAllEntries(NT_Inst inst) {
+  nt::DeleteAllEntries(inst);
+}
 
 struct NT_EntryInfo* NT_GetEntryInfo(NT_Inst inst, const char* prefix,
                                      size_t prefix_len, unsigned int types,
                                      size_t* count) {
-  auto info_v = nt::GetEntryInfo(inst, StringRef(prefix, prefix_len), types);
+  auto info_v = nt::GetEntryInfo(inst, {prefix, prefix_len}, types);
   return ConvertToC<NT_EntryInfo>(info_v, count);
 }
 
 NT_Bool NT_GetEntryInfoHandle(NT_Entry entry, struct NT_EntryInfo* info) {
   auto info_v = nt::GetEntryInfo(entry);
-  if (info_v.name.empty()) return false;
+  if (info_v.name.empty()) {
+    return false;
+  }
   ConvertToC(info_v, info);
   return true;
 }
@@ -261,7 +290,7 @@
                                      NT_EntryListenerCallback callback,
                                      unsigned int flags) {
   return nt::AddEntryListener(
-      inst, StringRef(prefix, prefix_len),
+      inst, {prefix, prefix_len},
       [=](const EntryNotification& event) {
         NT_EntryNotification c_event;
         ConvertToC(event, &c_event);
@@ -297,8 +326,7 @@
                                            const char* prefix,
                                            size_t prefix_len,
                                            unsigned int flags) {
-  return nt::AddPolledEntryListener(poller, StringRef(prefix, prefix_len),
-                                    flags);
+  return nt::AddPolledEntryListener(poller, {prefix, prefix_len}, flags);
 }
 
 NT_EntryListener NT_AddPolledEntryListenerSingle(NT_EntryListenerPoller poller,
@@ -394,7 +422,7 @@
 
 void NT_CreateRpc(NT_Entry entry, const char* def, size_t def_len, void* data,
                   NT_RpcCallback callback) {
-  nt::CreateRpc(entry, StringRef(def, def_len), [=](const RpcAnswer& answer) {
+  nt::CreateRpc(entry, {def, def_len}, [=](const RpcAnswer& answer) {
     NT_RpcAnswer answer_c;
     ConvertToC(answer, &answer_c);
     callback(data, &answer_c);
@@ -412,7 +440,7 @@
 
 void NT_CreatePolledRpc(NT_Entry entry, const char* def, size_t def_len,
                         NT_RpcCallPoller poller) {
-  nt::CreatePolledRpc(entry, StringRef(def, def_len), poller);
+  nt::CreatePolledRpc(entry, {def, def_len}, poller);
 }
 
 NT_RpcAnswer* NT_PollRpc(NT_RpcCallPoller poller, size_t* len) {
@@ -428,7 +456,9 @@
   return ConvertToC<NT_RpcAnswer>(arr_cpp, len);
 }
 
-void NT_CancelPollRpc(NT_RpcCallPoller poller) { nt::CancelPollRpc(poller); }
+void NT_CancelPollRpc(NT_RpcCallPoller poller) {
+  nt::CancelPollRpc(poller);
+}
 
 NT_Bool NT_WaitForRpcCallQueue(NT_Inst inst, double timeout) {
   return nt::WaitForRpcCallQueue(inst, timeout);
@@ -436,16 +466,18 @@
 
 NT_Bool NT_PostRpcResponse(NT_Entry entry, NT_RpcCall call, const char* result,
                            size_t result_len) {
-  return nt::PostRpcResponse(entry, call, StringRef(result, result_len));
+  return nt::PostRpcResponse(entry, call, {result, result_len});
 }
 
 NT_RpcCall NT_CallRpc(NT_Entry entry, const char* params, size_t params_len) {
-  return nt::CallRpc(entry, StringRef(params, params_len));
+  return nt::CallRpc(entry, {params, params_len});
 }
 
 char* NT_GetRpcResult(NT_Entry entry, NT_RpcCall call, size_t* result_len) {
   std::string result;
-  if (!nt::GetRpcResult(entry, call, &result)) return nullptr;
+  if (!nt::GetRpcResult(entry, call, &result)) {
+    return nullptr;
+  }
 
   // convert result
   *result_len = result.size();
@@ -489,7 +521,9 @@
 NT_Bool NT_UnpackRpcDefinition(const char* packed, size_t packed_len,
                                NT_RpcDefinition* def) {
   nt::RpcDefinition def_v;
-  if (!nt::UnpackRpcDefinition(StringRef(packed, packed_len), &def_v)) return 0;
+  if (!nt::UnpackRpcDefinition({packed, packed_len}, &def_v)) {
+    return 0;
+  }
 
   // convert result
   ConvertToC(def_v, def);
@@ -501,8 +535,9 @@
   // create input vector
   std::vector<std::shared_ptr<Value>> values_v;
   values_v.reserve(values_len);
-  for (size_t i = 0; i < values_len; ++i)
+  for (size_t i = 0; i < values_len; ++i) {
     values_v.push_back(ConvertFromC(*values[i]));
+  }
 
   // make the call
   auto packed = nt::PackRpcValues(values_v);
@@ -516,13 +551,14 @@
 
 NT_Value** NT_UnpackRpcValues(const char* packed, size_t packed_len,
                               const NT_Type* types, size_t types_len) {
-  auto values_v = nt::UnpackRpcValues(StringRef(packed, packed_len),
-                                      ArrayRef<NT_Type>(types, types_len));
-  if (values_v.size() == 0) return nullptr;
+  auto values_v = nt::UnpackRpcValues({packed, packed_len}, {types, types_len});
+  if (values_v.size() == 0) {
+    return nullptr;
+  }
 
   // create array and copy into it
   NT_Value** values = static_cast<NT_Value**>(
-      wpi::safe_malloc(values_v.size() * sizeof(NT_Value*)));
+      wpi::safe_malloc(values_v.size() * sizeof(NT_Value*)));  // NOLINT
   for (size_t i = 0; i < values_v.size(); ++i) {
     values[i] = static_cast<NT_Value*>(wpi::safe_malloc(sizeof(NT_Value)));
     ConvertToC(*values_v[i], values[i]);
@@ -535,25 +571,33 @@
  */
 
 void NT_SetNetworkIdentity(NT_Inst inst, const char* name, size_t name_len) {
-  nt::SetNetworkIdentity(inst, StringRef(name, name_len));
+  nt::SetNetworkIdentity(inst, {name, name_len});
 }
 
 unsigned int NT_GetNetworkMode(NT_Inst inst) {
   return nt::GetNetworkMode(inst);
 }
 
-void NT_StartLocal(NT_Inst inst) { nt::StartLocal(inst); }
+void NT_StartLocal(NT_Inst inst) {
+  nt::StartLocal(inst);
+}
 
-void NT_StopLocal(NT_Inst inst) { nt::StopLocal(inst); }
+void NT_StopLocal(NT_Inst inst) {
+  nt::StopLocal(inst);
+}
 
 void NT_StartServer(NT_Inst inst, const char* persist_filename,
                     const char* listen_address, unsigned int port) {
   nt::StartServer(inst, persist_filename, listen_address, port);
 }
 
-void NT_StopServer(NT_Inst inst) { nt::StopServer(inst); }
+void NT_StopServer(NT_Inst inst) {
+  nt::StopServer(inst);
+}
 
-void NT_StartClientNone(NT_Inst inst) { nt::StartClient(inst); }
+void NT_StartClientNone(NT_Inst inst) {
+  nt::StartClient(inst);
+}
 
 void NT_StartClient(NT_Inst inst, const char* server_name, unsigned int port) {
   nt::StartClient(inst, server_name, port);
@@ -561,10 +605,11 @@
 
 void NT_StartClientMulti(NT_Inst inst, size_t count, const char** server_names,
                          const unsigned int* ports) {
-  std::vector<std::pair<StringRef, unsigned int>> servers;
+  std::vector<std::pair<std::string_view, unsigned int>> servers;
   servers.reserve(count);
-  for (size_t i = 0; i < count; ++i)
+  for (size_t i = 0; i < count; ++i) {
     servers.emplace_back(std::make_pair(server_names[i], ports[i]));
+  }
   nt::StartClient(inst, servers);
 }
 
@@ -572,7 +617,9 @@
   nt::StartClientTeam(inst, team, port);
 }
 
-void NT_StopClient(NT_Inst inst) { nt::StopClient(inst); }
+void NT_StopClient(NT_Inst inst) {
+  nt::StopClient(inst);
+}
 
 void NT_SetServer(NT_Inst inst, const char* server_name, unsigned int port) {
   nt::SetServer(inst, server_name, port);
@@ -580,10 +627,11 @@
 
 void NT_SetServerMulti(NT_Inst inst, size_t count, const char** server_names,
                        const unsigned int* ports) {
-  std::vector<std::pair<StringRef, unsigned int>> servers;
+  std::vector<std::pair<std::string_view, unsigned int>> servers;
   servers.reserve(count);
-  for (size_t i = 0; i < count; ++i)
+  for (size_t i = 0; i < count; ++i) {
     servers.emplace_back(std::make_pair(server_names[i], ports[i]));
+  }
   nt::SetServer(inst, servers);
 }
 
@@ -595,15 +643,21 @@
   nt::StartDSClient(inst, port);
 }
 
-void NT_StopDSClient(NT_Inst inst) { nt::StopDSClient(inst); }
+void NT_StopDSClient(NT_Inst inst) {
+  nt::StopDSClient(inst);
+}
 
 void NT_SetUpdateRate(NT_Inst inst, double interval) {
   nt::SetUpdateRate(inst, interval);
 }
 
-void NT_Flush(NT_Inst inst) { nt::Flush(inst); }
+void NT_Flush(NT_Inst inst) {
+  nt::Flush(inst);
+}
 
-NT_Bool NT_IsConnected(NT_Inst inst) { return nt::IsConnected(inst); }
+NT_Bool NT_IsConnected(NT_Inst inst) {
+  return nt::IsConnected(inst);
+}
 
 struct NT_ConnectionInfo* NT_GetConnections(NT_Inst inst, size_t* count) {
   auto conn_v = nt::GetConnections(inst);
@@ -625,20 +679,22 @@
 
 const char* NT_SaveEntries(NT_Inst inst, const char* filename,
                            const char* prefix, size_t prefix_len) {
-  return nt::SaveEntries(inst, filename, StringRef(prefix, prefix_len));
+  return nt::SaveEntries(inst, filename, {prefix, prefix_len});
 }
 
 const char* NT_LoadEntries(NT_Inst inst, const char* filename,
                            const char* prefix, size_t prefix_len,
                            void (*warn)(size_t line, const char* msg)) {
-  return nt::LoadEntries(inst, filename, StringRef(prefix, prefix_len), warn);
+  return nt::LoadEntries(inst, filename, {prefix, prefix_len}, warn);
 }
 
 /*
  * Utility Functions
  */
 
-uint64_t NT_Now(void) { return wpi::Now(); }
+uint64_t NT_Now(void) {
+  return wpi::Now();
+}
 
 NT_Logger NT_AddLogger(NT_Inst inst, void* data, NT_LogFunc func,
                        unsigned int min_level, unsigned int max_level) {
@@ -683,7 +739,9 @@
   nt::CancelPollLogger(poller);
 }
 
-void NT_RemoveLogger(NT_Logger logger) { nt::RemoveLogger(logger); }
+void NT_RemoveLogger(NT_Logger logger) {
+  nt::RemoveLogger(logger);
+}
 
 NT_Bool NT_WaitForLoggerQueue(NT_Inst inst, double timeout) {
   return nt::WaitForLoggerQueue(inst, timeout);
@@ -707,8 +765,9 @@
       std::free(value->data.arr_double.arr);
       break;
     case NT_STRING_ARRAY: {
-      for (size_t i = 0; i < value->data.arr_string.size; i++)
+      for (size_t i = 0; i < value->data.arr_string.size; i++) {
         std::free(value->data.arr_string.arr[i].str);
+      }
       std::free(value->data.arr_string.arr);
       break;
     }
@@ -735,22 +794,32 @@
   str->len = 0;
 }
 
-void NT_DisposeEntryArray(NT_Entry* arr, size_t /*count*/) { std::free(arr); }
+void NT_DisposeEntryArray(NT_Entry* arr, size_t /*count*/) {
+  std::free(arr);
+}
 
 void NT_DisposeConnectionInfoArray(NT_ConnectionInfo* arr, size_t count) {
-  for (size_t i = 0; i < count; i++) DisposeConnectionInfo(&arr[i]);
+  for (size_t i = 0; i < count; i++) {
+    DisposeConnectionInfo(&arr[i]);
+  }
   std::free(arr);
 }
 
 void NT_DisposeEntryInfoArray(NT_EntryInfo* arr, size_t count) {
-  for (size_t i = 0; i < count; i++) DisposeEntryInfo(&arr[i]);
+  for (size_t i = 0; i < count; i++) {
+    DisposeEntryInfo(&arr[i]);
+  }
   std::free(arr);
 }
 
-void NT_DisposeEntryInfo(NT_EntryInfo* info) { DisposeEntryInfo(info); }
+void NT_DisposeEntryInfo(NT_EntryInfo* info) {
+  DisposeEntryInfo(info);
+}
 
 void NT_DisposeEntryNotificationArray(NT_EntryNotification* arr, size_t count) {
-  for (size_t i = 0; i < count; i++) DisposeEntryNotification(&arr[i]);
+  for (size_t i = 0; i < count; i++) {
+    DisposeEntryNotification(&arr[i]);
+  }
   std::free(arr);
 }
 
@@ -760,7 +829,9 @@
 
 void NT_DisposeConnectionNotificationArray(NT_ConnectionNotification* arr,
                                            size_t count) {
-  for (size_t i = 0; i < count; i++) DisposeConnectionNotification(&arr[i]);
+  for (size_t i = 0; i < count; i++) {
+    DisposeConnectionNotification(&arr[i]);
+  }
   std::free(arr);
 }
 
@@ -769,11 +840,16 @@
 }
 
 void NT_DisposeLogMessageArray(NT_LogMessage* arr, size_t count) {
-  for (size_t i = 0; i < count; i++) NT_DisposeLogMessage(&arr[i]);
+  for (size_t i = 0; i < count; i++) {
+    NT_DisposeLogMessage(&arr[i]);
+  }
   std::free(arr);
 }
 
-void NT_DisposeLogMessage(NT_LogMessage* info) { std::free(info->message); }
+void NT_DisposeLogMessage(NT_LogMessage* info) {
+  std::free(info->filename);
+  std::free(info->message);
+}
 
 void NT_DisposeRpcDefinition(NT_RpcDefinition* def) {
   NT_DisposeString(&def->name);
@@ -786,15 +862,18 @@
   def->params = nullptr;
   def->num_params = 0;
 
-  for (size_t i = 0; i < def->num_results; ++i)
+  for (size_t i = 0; i < def->num_results; ++i) {
     NT_DisposeString(&def->results[i].name);
+  }
   std::free(def->results);
   def->results = nullptr;
   def->num_results = 0;
 }
 
 void NT_DisposeRpcAnswerArray(NT_RpcAnswer* arr, size_t count) {
-  for (size_t i = 0; i < count; i++) NT_DisposeRpcAnswer(&arr[i]);
+  for (size_t i = 0; i < count; i++) {
+    NT_DisposeRpcAnswer(&arr[i]);
+  }
   std::free(arr);
 }
 
@@ -834,11 +913,19 @@
   return retVal;
 }
 
-void NT_FreeCharArray(char* v_char) { std::free(v_char); }
-void NT_FreeDoubleArray(double* v_double) { std::free(v_double); }
-void NT_FreeBooleanArray(int* v_boolean) { std::free(v_boolean); }
+void NT_FreeCharArray(char* v_char) {
+  std::free(v_char);
+}
+void NT_FreeDoubleArray(double* v_double) {
+  std::free(v_double);
+}
+void NT_FreeBooleanArray(int* v_boolean) {
+  std::free(v_boolean);
+}
 void NT_FreeStringArray(struct NT_String* v_string, size_t arr_size) {
-  for (size_t i = 0; i < arr_size; i++) std::free(v_string[i].str);
+  for (size_t i = 0; i < arr_size; i++) {
+    std::free(v_string[i].str);
+  }
   std::free(v_string);
 }
 
@@ -865,23 +952,20 @@
 NT_Bool NT_SetEntryString(NT_Entry entry, uint64_t time, const char* str,
                           size_t str_len, NT_Bool force) {
   if (force != 0) {
-    nt::SetEntryTypeValue(entry,
-                          Value::MakeString(StringRef(str, str_len), time));
+    nt::SetEntryTypeValue(entry, Value::MakeString({str, str_len}, time));
     return 1;
   } else {
-    return nt::SetEntryValue(entry,
-                             Value::MakeString(StringRef(str, str_len), time));
+    return nt::SetEntryValue(entry, Value::MakeString({str, str_len}, time));
   }
 }
 
 NT_Bool NT_SetEntryRaw(NT_Entry entry, uint64_t time, const char* raw,
                        size_t raw_len, NT_Bool force) {
   if (force != 0) {
-    nt::SetEntryTypeValue(entry, Value::MakeRaw(StringRef(raw, raw_len), time));
+    nt::SetEntryTypeValue(entry, Value::MakeRaw({raw, raw_len}, time));
     return 1;
   } else {
-    return nt::SetEntryValue(entry,
-                             Value::MakeRaw(StringRef(raw, raw_len), time));
+    return nt::SetEntryValue(entry, Value::MakeRaw({raw, raw_len}, time));
   }
 }
 
@@ -889,24 +973,22 @@
                                 const NT_Bool* arr, size_t size,
                                 NT_Bool force) {
   if (force != 0) {
-    nt::SetEntryTypeValue(
-        entry, Value::MakeBooleanArray(wpi::makeArrayRef(arr, size), time));
+    nt::SetEntryTypeValue(entry,
+                          Value::MakeBooleanArray(wpi::span(arr, size), time));
     return 1;
   } else {
     return nt::SetEntryValue(
-        entry, Value::MakeBooleanArray(wpi::makeArrayRef(arr, size), time));
+        entry, Value::MakeBooleanArray(wpi::span(arr, size), time));
   }
 }
 
 NT_Bool NT_SetEntryDoubleArray(NT_Entry entry, uint64_t time, const double* arr,
                                size_t size, NT_Bool force) {
   if (force != 0) {
-    nt::SetEntryTypeValue(
-        entry, Value::MakeDoubleArray(wpi::makeArrayRef(arr, size), time));
+    nt::SetEntryTypeValue(entry, Value::MakeDoubleArray({arr, size}, time));
     return 1;
   } else {
-    return nt::SetEntryValue(
-        entry, Value::MakeDoubleArray(wpi::makeArrayRef(arr, size), time));
+    return nt::SetEntryValue(entry, Value::MakeDoubleArray({arr, size}, time));
   }
 }
 
@@ -915,7 +997,9 @@
                                NT_Bool force) {
   std::vector<std::string> v;
   v.reserve(size);
-  for (size_t i = 0; i < size; ++i) v.push_back(ConvertFromC(arr[i]));
+  for (size_t i = 0; i < size; ++i) {
+    v.emplace_back(ConvertFromC(arr[i]));
+  }
 
   if (force != 0) {
     nt::SetEntryTypeValue(entry, Value::MakeStringArray(std::move(v), time));
@@ -926,13 +1010,17 @@
 }
 
 enum NT_Type NT_GetValueType(const struct NT_Value* value) {
-  if (!value) return NT_Type::NT_UNASSIGNED;
+  if (!value) {
+    return NT_Type::NT_UNASSIGNED;
+  }
   return value->type;
 }
 
 NT_Bool NT_GetValueBoolean(const struct NT_Value* value, uint64_t* last_change,
                            NT_Bool* v_boolean) {
-  if (!value || value->type != NT_Type::NT_BOOLEAN) return 0;
+  if (!value || value->type != NT_Type::NT_BOOLEAN) {
+    return 0;
+  }
   *v_boolean = value->data.v_boolean;
   *last_change = value->last_change;
   return 1;
@@ -940,7 +1028,9 @@
 
 NT_Bool NT_GetValueDouble(const struct NT_Value* value, uint64_t* last_change,
                           double* v_double) {
-  if (!value || value->type != NT_Type::NT_DOUBLE) return 0;
+  if (!value || value->type != NT_Type::NT_DOUBLE) {
+    return 0;
+  }
   *last_change = value->last_change;
   *v_double = value->data.v_double;
   return 1;
@@ -948,7 +1038,9 @@
 
 char* NT_GetValueString(const struct NT_Value* value, uint64_t* last_change,
                         size_t* str_len) {
-  if (!value || value->type != NT_Type::NT_STRING) return nullptr;
+  if (!value || value->type != NT_Type::NT_STRING) {
+    return nullptr;
+  }
   *last_change = value->last_change;
   *str_len = value->data.v_string.len;
   char* str =
@@ -959,7 +1051,9 @@
 
 char* NT_GetValueRaw(const struct NT_Value* value, uint64_t* last_change,
                      size_t* raw_len) {
-  if (!value || value->type != NT_Type::NT_RAW) return nullptr;
+  if (!value || value->type != NT_Type::NT_RAW) {
+    return nullptr;
+  }
   *last_change = value->last_change;
   *raw_len = value->data.v_string.len;
   char* raw =
@@ -970,7 +1064,9 @@
 
 NT_Bool* NT_GetValueBooleanArray(const struct NT_Value* value,
                                  uint64_t* last_change, size_t* arr_size) {
-  if (!value || value->type != NT_Type::NT_BOOLEAN_ARRAY) return nullptr;
+  if (!value || value->type != NT_Type::NT_BOOLEAN_ARRAY) {
+    return nullptr;
+  }
   *last_change = value->last_change;
   *arr_size = value->data.arr_boolean.size;
   NT_Bool* arr = static_cast<int*>(
@@ -982,7 +1078,9 @@
 
 double* NT_GetValueDoubleArray(const struct NT_Value* value,
                                uint64_t* last_change, size_t* arr_size) {
-  if (!value || value->type != NT_Type::NT_DOUBLE_ARRAY) return nullptr;
+  if (!value || value->type != NT_Type::NT_DOUBLE_ARRAY) {
+    return nullptr;
+  }
   *last_change = value->last_change;
   *arr_size = value->data.arr_double.size;
   double* arr = static_cast<double*>(
@@ -994,7 +1092,9 @@
 
 NT_String* NT_GetValueStringArray(const struct NT_Value* value,
                                   uint64_t* last_change, size_t* arr_size) {
-  if (!value || value->type != NT_Type::NT_STRING_ARRAY) return nullptr;
+  if (!value || value->type != NT_Type::NT_STRING_ARRAY) {
+    return nullptr;
+  }
   *last_change = value->last_change;
   *arr_size = value->data.arr_string.size;
   NT_String* arr = static_cast<NT_String*>(
@@ -1024,29 +1124,28 @@
                                  const char* default_value,
                                  size_t default_len) {
   return nt::SetDefaultEntryValue(
-      entry, Value::MakeString(StringRef(default_value, default_len), time));
+      entry, Value::MakeString({default_value, default_len}, time));
 }
 
 NT_Bool NT_SetDefaultEntryRaw(NT_Entry entry, uint64_t time,
                               const char* default_value, size_t default_len) {
   return nt::SetDefaultEntryValue(
-      entry, Value::MakeRaw(StringRef(default_value, default_len), time));
+      entry, Value::MakeRaw({default_value, default_len}, time));
 }
 
 NT_Bool NT_SetDefaultEntryBooleanArray(NT_Entry entry, uint64_t time,
                                        const NT_Bool* default_value,
                                        size_t default_size) {
   return nt::SetDefaultEntryValue(
-      entry, Value::MakeBooleanArray(
-                 wpi::makeArrayRef(default_value, default_size), time));
+      entry,
+      Value::MakeBooleanArray(wpi::span(default_value, default_size), time));
 }
 
 NT_Bool NT_SetDefaultEntryDoubleArray(NT_Entry entry, uint64_t time,
                                       const double* default_value,
                                       size_t default_size) {
   return nt::SetDefaultEntryValue(
-      entry, Value::MakeDoubleArray(
-                 wpi::makeArrayRef(default_value, default_size), time));
+      entry, Value::MakeDoubleArray({default_value, default_size}, time));
 }
 
 NT_Bool NT_SetDefaultEntryStringArray(NT_Entry entry, uint64_t time,
@@ -1054,8 +1153,9 @@
                                       size_t default_size) {
   std::vector<std::string> vec;
   vec.reserve(default_size);
-  for (size_t i = 0; i < default_size; ++i)
-    vec.push_back(ConvertFromC(default_value[i]));
+  for (size_t i = 0; i < default_size; ++i) {
+    vec.emplace_back(ConvertFromC(default_value[i]));
+  }
 
   return nt::SetDefaultEntryValue(entry,
                                   Value::MakeStringArray(std::move(vec), time));
@@ -1064,7 +1164,9 @@
 NT_Bool NT_GetEntryBoolean(NT_Entry entry, uint64_t* last_change,
                            NT_Bool* v_boolean) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsBoolean()) return 0;
+  if (!v || !v->IsBoolean()) {
+    return 0;
+  }
   *v_boolean = v->GetBoolean();
   *last_change = v->last_change();
   return 1;
@@ -1073,7 +1175,9 @@
 NT_Bool NT_GetEntryDouble(NT_Entry entry, uint64_t* last_change,
                           double* v_double) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsDouble()) return 0;
+  if (!v || !v->IsDouble()) {
+    return 0;
+  }
   *last_change = v->last_change();
   *v_double = v->GetDouble();
   return 1;
@@ -1082,7 +1186,9 @@
 char* NT_GetEntryString(NT_Entry entry, uint64_t* last_change,
                         size_t* str_len) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsString()) return nullptr;
+  if (!v || !v->IsString()) {
+    return nullptr;
+  }
   *last_change = v->last_change();
   struct NT_String v_string;
   nt::ConvertToC(v->GetString(), &v_string);
@@ -1092,7 +1198,9 @@
 
 char* NT_GetEntryRaw(NT_Entry entry, uint64_t* last_change, size_t* raw_len) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsRaw()) return nullptr;
+  if (!v || !v->IsRaw()) {
+    return nullptr;
+  }
   *last_change = v->last_change();
   struct NT_String v_raw;
   nt::ConvertToC(v->GetRaw(), &v_raw);
@@ -1103,7 +1211,9 @@
 NT_Bool* NT_GetEntryBooleanArray(NT_Entry entry, uint64_t* last_change,
                                  size_t* arr_size) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsBooleanArray()) return nullptr;
+  if (!v || !v->IsBooleanArray()) {
+    return nullptr;
+  }
   *last_change = v->last_change();
   auto vArr = v->GetBooleanArray();
   NT_Bool* arr =
@@ -1116,7 +1226,9 @@
 double* NT_GetEntryDoubleArray(NT_Entry entry, uint64_t* last_change,
                                size_t* arr_size) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsDoubleArray()) return nullptr;
+  if (!v || !v->IsDoubleArray()) {
+    return nullptr;
+  }
   *last_change = v->last_change();
   auto vArr = v->GetDoubleArray();
   double* arr =
@@ -1129,7 +1241,9 @@
 NT_String* NT_GetEntryStringArray(NT_Entry entry, uint64_t* last_change,
                                   size_t* arr_size) {
   auto v = nt::GetEntryValue(entry);
-  if (!v || !v->IsStringArray()) return nullptr;
+  if (!v || !v->IsStringArray()) {
+    return nullptr;
+  }
   *last_change = v->last_change();
   auto vArr = v->GetStringArray();
   NT_String* arr = static_cast<NT_String*>(
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp
index ad8f68b..8616aaf 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_cpp.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <stdint.h>
 
@@ -36,15 +33,18 @@
 
 void DestroyInstance(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
-  if (i < 0) return;
+  if (i < 0) {
+    return;
+  }
   InstanceImpl::Destroy(i);
 }
 
 NT_Inst GetInstanceFromHandle(NT_Handle handle) {
   Handle h{handle};
   auto type = h.GetType();
-  if (type >= Handle::kConnectionListener && type <= Handle::kRpcCallPoller)
+  if (type >= Handle::kConnectionListener && type <= Handle::kRpcCallPoller) {
     return Handle(h.GetInst(), 0, Handle::kInstance);
+  }
 
   return 0;
 }
@@ -53,25 +53,33 @@
  * Table Functions
  */
 
-NT_Entry GetEntry(NT_Inst inst, const Twine& name) {
+NT_Entry GetEntry(NT_Inst inst, std::string_view name) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   unsigned int id = ii->storage.GetEntry(name);
-  if (id == UINT_MAX) return 0;
+  if (id == UINT_MAX) {
+    return 0;
+  }
   return Handle(i, id, Handle::kEntry);
 }
 
-std::vector<NT_Entry> GetEntries(NT_Inst inst, const Twine& prefix,
+std::vector<NT_Entry> GetEntries(NT_Inst inst, std::string_view prefix,
                                  unsigned int types) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return std::vector<NT_Entry>{};
+  if (!ii) {
+    return {};
+  }
 
   auto arr = ii->storage.GetEntries(prefix, types);
   // convert indices to handles
-  for (auto& val : arr) val = Handle(i, val, Handle::kEntry);
+  for (auto& val : arr) {
+    val = Handle(i, val, Handle::kEntry);
+  }
   return arr;
 }
 
@@ -79,7 +87,9 @@
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::string{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->storage.GetEntryName(id);
 }
@@ -88,7 +98,9 @@
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return NT_UNASSIGNED;
+  if (id < 0 || !ii) {
+    return NT_UNASSIGNED;
+  }
 
   return ii->storage.GetEntryType(id);
 }
@@ -97,123 +109,107 @@
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   return ii->storage.GetEntryLastChange(id);
 }
 
-std::shared_ptr<Value> GetEntryValue(StringRef name) {
-  return InstanceImpl::GetDefault()->storage.GetEntryValue(name);
-}
-
 std::shared_ptr<Value> GetEntryValue(NT_Entry entry) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return nullptr;
+  if (id < 0 || !ii) {
+    return nullptr;
+  }
 
   return ii->storage.GetEntryValue(id);
 }
 
-bool SetDefaultEntryValue(StringRef name, std::shared_ptr<Value> value) {
-  return InstanceImpl::GetDefault()->storage.SetDefaultEntryValue(name, value);
-}
-
 bool SetDefaultEntryValue(NT_Entry entry, std::shared_ptr<Value> value) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return false;
+  if (id < 0 || !ii) {
+    return false;
+  }
 
   return ii->storage.SetDefaultEntryValue(id, value);
 }
 
-bool SetEntryValue(StringRef name, std::shared_ptr<Value> value) {
-  return InstanceImpl::GetDefault()->storage.SetEntryValue(name, value);
-}
-
 bool SetEntryValue(NT_Entry entry, std::shared_ptr<Value> value) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return false;
+  if (id < 0 || !ii) {
+    return false;
+  }
 
   return ii->storage.SetEntryValue(id, value);
 }
 
-void SetEntryTypeValue(StringRef name, std::shared_ptr<Value> value) {
-  InstanceImpl::GetDefault()->storage.SetEntryTypeValue(name, value);
-}
-
 void SetEntryTypeValue(NT_Entry entry, std::shared_ptr<Value> value) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->storage.SetEntryTypeValue(id, value);
 }
 
-void SetEntryFlags(StringRef name, unsigned int flags) {
-  InstanceImpl::GetDefault()->storage.SetEntryFlags(name, flags);
-}
-
 void SetEntryFlags(NT_Entry entry, unsigned int flags) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->storage.SetEntryFlags(id, flags);
 }
 
-unsigned int GetEntryFlags(StringRef name) {
-  return InstanceImpl::GetDefault()->storage.GetEntryFlags(name);
-}
-
 unsigned int GetEntryFlags(NT_Entry entry) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   return ii->storage.GetEntryFlags(id);
 }
 
-void DeleteEntry(StringRef name) {
-  InstanceImpl::GetDefault()->storage.DeleteEntry(name);
-}
-
 void DeleteEntry(NT_Entry entry) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->storage.DeleteEntry(id);
 }
 
-void DeleteAllEntries() {
-  InstanceImpl::GetDefault()->storage.DeleteAllEntries();
-}
-
 void DeleteAllEntries(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (i < 0 || !ii) return;
+  if (i < 0 || !ii) {
+    return;
+  }
 
   ii->storage.DeleteAllEntries();
 }
 
-std::vector<EntryInfo> GetEntryInfo(StringRef prefix, unsigned int types) {
-  return InstanceImpl::GetDefault()->storage.GetEntryInfo(0, prefix, types);
-}
-
-std::vector<EntryInfo> GetEntryInfo(NT_Inst inst, const Twine& prefix,
+std::vector<EntryInfo> GetEntryInfo(NT_Inst inst, std::string_view prefix,
                                     unsigned int types) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return std::vector<EntryInfo>{};
+  if (!ii) {
+    return {};
+  }
 
   return ii->storage.GetEntryInfo(i, prefix, types);
 }
@@ -239,24 +235,15 @@
  * Callback Creation Functions
  */
 
-NT_EntryListener AddEntryListener(StringRef prefix,
-                                  EntryListenerCallback callback,
-                                  unsigned int flags) {
-  return AddEntryListener(
-      Handle(InstanceImpl::GetDefaultIndex(), 0, Handle::kInstance), prefix,
-      [=](const EntryNotification& event) {
-        callback(event.listener, event.name, event.value, event.flags);
-      },
-      flags);
-}
-
 NT_EntryListener AddEntryListener(
-    NT_Inst inst, const Twine& prefix,
+    NT_Inst inst, std::string_view prefix,
     std::function<void(const EntryNotification& event)> callback,
     unsigned int flags) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (i < 0 || !ii) return 0;
+  if (i < 0 || !ii) {
+    return 0;
+  }
 
   unsigned int uid = ii->storage.AddListener(prefix, callback, flags);
   return Handle(i, uid, Handle::kEntryListener);
@@ -270,7 +257,9 @@
   int id = handle.GetTypedIndex(Handle::kEntry);
   int i = handle.GetInst();
   auto ii = InstanceImpl::Get(i);
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   unsigned int uid = ii->storage.AddListener(id, callback, flags);
   return Handle(i, uid, Handle::kEntryListener);
@@ -279,7 +268,9 @@
 NT_EntryListenerPoller CreateEntryListenerPoller(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   return Handle(i, ii->entry_notifier.CreatePoller(),
                 Handle::kEntryListenerPoller);
@@ -289,19 +280,23 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kEntryListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->entry_notifier.RemovePoller(id);
 }
 
 NT_EntryListener AddPolledEntryListener(NT_EntryListenerPoller poller,
-                                        const Twine& prefix,
+                                        std::string_view prefix,
                                         unsigned int flags) {
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kEntryListenerPoller);
   int i = handle.GetInst();
   auto ii = InstanceImpl::Get(i);
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   unsigned int uid = ii->storage.AddPolledListener(id, prefix, flags);
   return Handle(i, uid, Handle::kEntryListener);
@@ -313,12 +308,18 @@
   int id = handle.GetTypedIndex(Handle::kEntry);
   int i = handle.GetInst();
   auto ii = InstanceImpl::Get(i);
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   Handle phandle{poller};
   int p_id = phandle.GetTypedIndex(Handle::kEntryListenerPoller);
-  if (p_id < 0) return 0;
-  if (handle.GetInst() != phandle.GetInst()) return 0;
+  if (p_id < 0) {
+    return 0;
+  }
+  if (handle.GetInst() != phandle.GetInst()) {
+    return 0;
+  }
 
   unsigned int uid = ii->storage.AddPolledListener(p_id, id, flags);
   return Handle(i, uid, Handle::kEntryListener);
@@ -329,7 +330,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kEntryListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<EntryNotification>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->entry_notifier.Poll(static_cast<unsigned int>(id));
 }
@@ -341,7 +344,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kEntryListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<EntryNotification>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->entry_notifier.Poll(static_cast<unsigned int>(id), timeout,
                                  timed_out);
@@ -351,7 +356,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kEntryListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->entry_notifier.CancelPoll(id);
 }
@@ -360,7 +367,9 @@
   Handle handle{entry_listener};
   int uid = handle.GetTypedIndex(Handle::kEntryListener);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (uid < 0 || !ii) return;
+  if (uid < 0 || !ii) {
+    return;
+  }
 
   ii->entry_notifier.Remove(uid);
 }
@@ -368,27 +377,21 @@
 bool WaitForEntryListenerQueue(NT_Inst inst, double timeout) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return true;
+  if (!ii) {
+    return true;
+  }
   return ii->entry_notifier.WaitForQueue(timeout);
 }
 
-NT_ConnectionListener AddConnectionListener(ConnectionListenerCallback callback,
-                                            bool immediate_notify) {
-  return AddConnectionListener(
-      Handle(InstanceImpl::GetDefaultIndex(), 0, Handle::kInstance),
-      [=](const ConnectionNotification& event) {
-        callback(event.listener, event.connected, event.conn);
-      },
-      immediate_notify);
-}
-
 NT_ConnectionListener AddConnectionListener(
     NT_Inst inst,
     std::function<void(const ConnectionNotification& event)> callback,
     bool immediate_notify) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   unsigned int uid = ii->dispatcher.AddListener(callback, immediate_notify);
   return Handle(i, uid, Handle::kConnectionListener);
@@ -397,7 +400,9 @@
 NT_ConnectionListenerPoller CreateConnectionListenerPoller(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   return Handle(i, ii->connection_notifier.CreatePoller(),
                 Handle::kConnectionListenerPoller);
@@ -407,7 +412,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kConnectionListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->connection_notifier.RemovePoller(id);
 }
@@ -418,7 +425,9 @@
   int id = handle.GetTypedIndex(Handle::kConnectionListenerPoller);
   int i = handle.GetInst();
   auto ii = InstanceImpl::Get(i);
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   unsigned int uid = ii->dispatcher.AddPolledListener(id, immediate_notify);
   return Handle(i, uid, Handle::kConnectionListener);
@@ -429,7 +438,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kConnectionListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<ConnectionNotification>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->connection_notifier.Poll(static_cast<unsigned int>(id));
 }
@@ -440,7 +451,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kConnectionListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<ConnectionNotification>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->connection_notifier.Poll(static_cast<unsigned int>(id), timeout,
                                       timed_out);
@@ -450,7 +463,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kConnectionListenerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->connection_notifier.CancelPoll(id);
 }
@@ -459,7 +474,9 @@
   Handle handle{conn_listener};
   int uid = handle.GetTypedIndex(Handle::kConnectionListener);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (uid < 0 || !ii) return;
+  if (uid < 0 || !ii) {
+    return;
+  }
 
   ii->connection_notifier.Remove(uid);
 }
@@ -467,7 +484,9 @@
 bool WaitForConnectionListenerQueue(NT_Inst inst, double timeout) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return true;
+  if (!ii) {
+    return true;
+  }
   return ii->connection_notifier.WaitForQueue(timeout);
 }
 
@@ -475,16 +494,22 @@
  * Remote Procedure Call Functions
  */
 
-void CreateRpc(NT_Entry entry, StringRef def,
+void CreateRpc(NT_Entry entry, std::string_view def,
                std::function<void(const RpcAnswer& answer)> callback) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   // only server can create RPCs
-  if ((ii->dispatcher.GetNetworkMode() & NT_NET_MODE_SERVER) == 0) return;
-  if (def.empty() || !callback) return;
+  if ((ii->dispatcher.GetNetworkMode() & NT_NET_MODE_SERVER) == 0) {
+    return;
+  }
+  if (def.empty() || !callback) {
+    return;
+  }
 
   ii->storage.CreateRpc(id, def, ii->rpc_server.Add(callback));
 }
@@ -492,7 +517,9 @@
 NT_RpcCallPoller CreateRpcCallPoller(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   return Handle(i, ii->rpc_server.CreatePoller(), Handle::kRpcCallPoller);
 }
@@ -501,25 +528,38 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kRpcCallPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->rpc_server.RemovePoller(id);
 }
 
-void CreatePolledRpc(NT_Entry entry, StringRef def, NT_RpcCallPoller poller) {
+void CreatePolledRpc(NT_Entry entry, std::string_view def,
+                     NT_RpcCallPoller poller) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   Handle phandle{poller};
   int p_id = phandle.GetTypedIndex(Handle::kRpcCallPoller);
-  if (p_id < 0) return;
-  if (handle.GetInst() != phandle.GetInst()) return;
+  if (p_id < 0) {
+    return;
+  }
+  if (handle.GetInst() != phandle.GetInst()) {
+    return;
+  }
 
   // only server can create RPCs
-  if ((ii->dispatcher.GetNetworkMode() & NT_NET_MODE_SERVER) == 0) return;
-  if (def.empty()) return;
+  if ((ii->dispatcher.GetNetworkMode() & NT_NET_MODE_SERVER) == 0) {
+    return;
+  }
+  if (def.empty()) {
+    return;
+  }
 
   ii->storage.CreateRpc(id, def, ii->rpc_server.AddPolled(p_id));
 }
@@ -528,7 +568,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kRpcCallPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<RpcAnswer>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->rpc_server.Poll(static_cast<unsigned int>(id));
 }
@@ -539,7 +581,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kRpcCallPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<RpcAnswer>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->rpc_server.Poll(static_cast<unsigned int>(id), timeout, timed_out);
 }
@@ -548,7 +592,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kRpcCallPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->rpc_server.CancelPoll(id);
 }
@@ -556,33 +602,45 @@
 bool WaitForRpcCallQueue(NT_Inst inst, double timeout) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return true;
+  if (!ii) {
+    return true;
+  }
   return ii->rpc_server.WaitForQueue(timeout);
 }
 
-bool PostRpcResponse(NT_Entry entry, NT_RpcCall call, StringRef result) {
+bool PostRpcResponse(NT_Entry entry, NT_RpcCall call, std::string_view result) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return false;
+  if (id < 0 || !ii) {
+    return false;
+  }
 
   Handle chandle{call};
   int call_uid = chandle.GetTypedIndex(Handle::kRpcCall);
-  if (call_uid < 0) return false;
-  if (handle.GetInst() != chandle.GetInst()) return false;
+  if (call_uid < 0) {
+    return false;
+  }
+  if (handle.GetInst() != chandle.GetInst()) {
+    return false;
+  }
 
   return ii->rpc_server.PostRpcResponse(id, call_uid, result);
 }
 
-NT_RpcCall CallRpc(NT_Entry entry, StringRef params) {
+NT_RpcCall CallRpc(NT_Entry entry, std::string_view params) {
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   int i = handle.GetInst();
   auto ii = InstanceImpl::Get(i);
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
   unsigned int call_uid = ii->storage.CallRpc(id, params);
-  if (call_uid == 0) return 0;
+  if (call_uid == 0) {
+    return 0;
+  }
   return Handle(i, call_uid, Handle::kRpcCall);
 }
 
@@ -590,12 +648,18 @@
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return false;
+  if (id < 0 || !ii) {
+    return false;
+  }
 
   Handle chandle{call};
   int call_uid = chandle.GetTypedIndex(Handle::kRpcCall);
-  if (call_uid < 0) return false;
-  if (handle.GetInst() != chandle.GetInst()) return false;
+  if (call_uid < 0) {
+    return false;
+  }
+  if (handle.GetInst() != chandle.GetInst()) {
+    return false;
+  }
 
   return ii->storage.GetRpcResult(id, call_uid, result);
 }
@@ -606,12 +670,18 @@
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return false;
+  if (id < 0 || !ii) {
+    return false;
+  }
 
   Handle chandle{call};
   int call_uid = chandle.GetTypedIndex(Handle::kRpcCall);
-  if (call_uid < 0) return false;
-  if (handle.GetInst() != chandle.GetInst()) return false;
+  if (call_uid < 0) {
+    return false;
+  }
+  if (handle.GetInst() != chandle.GetInst()) {
+    return false;
+  }
 
   return ii->storage.GetRpcResult(id, call_uid, result, timeout, timed_out);
 }
@@ -620,12 +690,18 @@
   Handle handle{entry};
   int id = handle.GetTypedIndex(Handle::kEntry);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   Handle chandle{call};
   int call_uid = chandle.GetTypedIndex(Handle::kRpcCall);
-  if (call_uid < 0) return;
-  if (handle.GetInst() != chandle.GetInst()) return;
+  if (call_uid < 0) {
+    return;
+  }
+  if (handle.GetInst() != chandle.GetInst()) {
+    return;
+  }
 
   ii->storage.CancelRpcResult(id, call_uid);
 }
@@ -637,7 +713,9 @@
 
   // parameters
   unsigned int params_size = def.params.size();
-  if (params_size > 0xff) params_size = 0xff;
+  if (params_size > 0xff) {
+    params_size = 0xff;
+  }
   enc.Write8(params_size);
   for (size_t i = 0; i < params_size; ++i) {
     enc.WriteType(def.params[i].def_value->type());
@@ -647,171 +725,186 @@
 
   // results
   unsigned int results_size = def.results.size();
-  if (results_size > 0xff) results_size = 0xff;
+  if (results_size > 0xff) {
+    results_size = 0xff;
+  }
   enc.Write8(results_size);
   for (size_t i = 0; i < results_size; ++i) {
     enc.WriteType(def.results[i].type);
     enc.WriteString(def.results[i].name);
   }
 
-  return enc.ToStringRef();
+  return std::string{enc.ToStringView()};
 }
 
-bool UnpackRpcDefinition(StringRef packed, RpcDefinition* def) {
+bool UnpackRpcDefinition(std::string_view packed, RpcDefinition* def) {
   wpi::raw_mem_istream is(packed.data(), packed.size());
   wpi::Logger logger;
   WireDecoder dec(is, 0x0300, logger);
-  if (!dec.Read8(&def->version)) return false;
-  if (!dec.ReadString(&def->name)) return false;
+  if (!dec.Read8(&def->version)) {
+    return false;
+  }
+  if (!dec.ReadString(&def->name)) {
+    return false;
+  }
 
   // parameters
   unsigned int params_size;
-  if (!dec.Read8(&params_size)) return false;
+  if (!dec.Read8(&params_size)) {
+    return false;
+  }
   def->params.resize(0);
   def->params.reserve(params_size);
   for (size_t i = 0; i < params_size; ++i) {
     RpcParamDef pdef;
     NT_Type type;
-    if (!dec.ReadType(&type)) return false;
-    if (!dec.ReadString(&pdef.name)) return false;
+    if (!dec.ReadType(&type)) {
+      return false;
+    }
+    if (!dec.ReadString(&pdef.name)) {
+      return false;
+    }
     pdef.def_value = dec.ReadValue(type);
-    if (!pdef.def_value) return false;
+    if (!pdef.def_value) {
+      return false;
+    }
     def->params.emplace_back(std::move(pdef));
   }
 
   // results
   unsigned int results_size;
-  if (!dec.Read8(&results_size)) return false;
+  if (!dec.Read8(&results_size)) {
+    return false;
+  }
   def->results.resize(0);
   def->results.reserve(results_size);
   for (size_t i = 0; i < results_size; ++i) {
     RpcResultDef rdef;
-    if (!dec.ReadType(&rdef.type)) return false;
-    if (!dec.ReadString(&rdef.name)) return false;
+    if (!dec.ReadType(&rdef.type)) {
+      return false;
+    }
+    if (!dec.ReadString(&rdef.name)) {
+      return false;
+    }
     def->results.emplace_back(std::move(rdef));
   }
 
   return true;
 }
 
-std::string PackRpcValues(ArrayRef<std::shared_ptr<Value>> values) {
+std::string PackRpcValues(wpi::span<const std::shared_ptr<Value>> values) {
   WireEncoder enc(0x0300);
-  for (auto& value : values) enc.WriteValue(*value);
-  return enc.ToStringRef();
+  for (auto& value : values) {
+    enc.WriteValue(*value);
+  }
+  return std::string{enc.ToStringView()};
 }
 
-std::vector<std::shared_ptr<Value>> UnpackRpcValues(StringRef packed,
-                                                    ArrayRef<NT_Type> types) {
+std::vector<std::shared_ptr<Value>> UnpackRpcValues(
+    std::string_view packed, wpi::span<const NT_Type> types) {
   wpi::raw_mem_istream is(packed.data(), packed.size());
   wpi::Logger logger;
   WireDecoder dec(is, 0x0300, logger);
   std::vector<std::shared_ptr<Value>> vec;
   for (auto type : types) {
     auto item = dec.ReadValue(type);
-    if (!item) return std::vector<std::shared_ptr<Value>>();
+    if (!item) {
+      return std::vector<std::shared_ptr<Value>>();
+    }
     vec.emplace_back(std::move(item));
   }
   return vec;
 }
 
-uint64_t Now() { return wpi::Now(); }
+uint64_t Now() {
+  return wpi::Now();
+}
 
 /*
  * Client/Server Functions
  */
 
-void SetNetworkIdentity(StringRef name) {
-  InstanceImpl::GetDefault()->dispatcher.SetIdentity(name);
-}
-
-void SetNetworkIdentity(NT_Inst inst, const Twine& name) {
+void SetNetworkIdentity(NT_Inst inst, std::string_view name) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetIdentity(name);
 }
 
-unsigned int GetNetworkMode() {
-  return InstanceImpl::GetDefault()->dispatcher.GetNetworkMode();
-}
-
 unsigned int GetNetworkMode(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   return ii->dispatcher.GetNetworkMode();
 }
 
 void StartLocal(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.StartLocal();
 }
 
 void StopLocal(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.Stop();
 }
 
-void StartServer(StringRef persist_filename, const char* listen_address,
-                 unsigned int port) {
-  auto ii = InstanceImpl::GetDefault();
-  ii->dispatcher.StartServer(persist_filename, listen_address, port);
-}
-
-void StartServer(NT_Inst inst, const Twine& persist_filename,
+void StartServer(NT_Inst inst, std::string_view persist_filename,
                  const char* listen_address, unsigned int port) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.StartServer(persist_filename, listen_address, port);
 }
 
-void StopServer() { InstanceImpl::GetDefault()->dispatcher.Stop(); }
-
 void StopServer(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.Stop();
 }
 
-void StartClient() { InstanceImpl::GetDefault()->dispatcher.StartClient(); }
-
 void StartClient(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.StartClient();
 }
 
-void StartClient(const char* server_name, unsigned int port) {
-  auto ii = InstanceImpl::GetDefault();
-  ii->dispatcher.SetServer(server_name, port);
-  ii->dispatcher.StartClient();
-}
-
 void StartClient(NT_Inst inst, const char* server_name, unsigned int port) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetServer(server_name, port);
   ii->dispatcher.StartClient();
 }
 
-void StartClient(ArrayRef<std::pair<StringRef, unsigned int>> servers) {
-  auto ii = InstanceImpl::GetDefault();
-  ii->dispatcher.SetServer(servers);
-  ii->dispatcher.StartClient();
-}
-
-void StartClient(NT_Inst inst,
-                 ArrayRef<std::pair<StringRef, unsigned int>> servers) {
+void StartClient(
+    NT_Inst inst,
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetServer(servers);
   ii->dispatcher.StartClient();
@@ -819,105 +912,102 @@
 
 void StartClientTeam(NT_Inst inst, unsigned int team, unsigned int port) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetServerTeam(team, port);
   ii->dispatcher.StartClient();
 }
 
-void StopClient() { InstanceImpl::GetDefault()->dispatcher.Stop(); }
-
 void StopClient(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.Stop();
 }
 
-void SetServer(const char* server_name, unsigned int port) {
-  InstanceImpl::GetDefault()->dispatcher.SetServer(server_name, port);
-}
-
 void SetServer(NT_Inst inst, const char* server_name, unsigned int port) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetServer(server_name, port);
 }
 
-void SetServer(ArrayRef<std::pair<StringRef, unsigned int>> servers) {
-  InstanceImpl::GetDefault()->dispatcher.SetServer(servers);
-}
-
-void SetServer(NT_Inst inst,
-               ArrayRef<std::pair<StringRef, unsigned int>> servers) {
+void SetServer(
+    NT_Inst inst,
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetServer(servers);
 }
 
 void SetServerTeam(NT_Inst inst, unsigned int team, unsigned int port) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetServerTeam(team, port);
 }
 
-void StartDSClient(unsigned int port) {
-  InstanceImpl::GetDefault()->ds_client.Start(port);
-}
-
 void StartDSClient(NT_Inst inst, unsigned int port) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->ds_client.Start(port);
 }
 
-void StopDSClient() { InstanceImpl::GetDefault()->ds_client.Stop(); }
-
 void StopDSClient(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->ds_client.Stop();
 }
 
-void SetUpdateRate(double interval) {
-  InstanceImpl::GetDefault()->dispatcher.SetUpdateRate(interval);
-}
-
 void SetUpdateRate(NT_Inst inst, double interval) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.SetUpdateRate(interval);
 }
 
-void Flush() { InstanceImpl::GetDefault()->dispatcher.Flush(); }
-
 void Flush(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return;
+  if (!ii) {
+    return;
+  }
 
   ii->dispatcher.Flush();
 }
 
-std::vector<ConnectionInfo> GetConnections() {
-  return InstanceImpl::GetDefault()->dispatcher.GetConnections();
-}
-
 std::vector<ConnectionInfo> GetConnections(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return std::vector<ConnectionInfo>{};
+  if (!ii) {
+    return {};
+  }
 
   return ii->dispatcher.GetConnections();
 }
 
 bool IsConnected(NT_Inst inst) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return false;
+  if (!ii) {
+    return false;
+  }
 
   return ii->dispatcher.IsConnected();
 }
@@ -926,79 +1016,70 @@
  * Persistent Functions
  */
 
-const char* SavePersistent(StringRef filename) {
-  return InstanceImpl::GetDefault()->storage.SavePersistent(filename, false);
-}
-
-const char* SavePersistent(NT_Inst inst, const Twine& filename) {
+const char* SavePersistent(NT_Inst inst, std::string_view filename) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return "invalid instance handle";
+  if (!ii) {
+    return "invalid instance handle";
+  }
 
   return ii->storage.SavePersistent(filename, false);
 }
 
 const char* LoadPersistent(
-    StringRef filename,
-    std::function<void(size_t line, const char* msg)> warn) {
-  return InstanceImpl::GetDefault()->storage.LoadPersistent(filename, warn);
-}
-
-const char* LoadPersistent(
-    NT_Inst inst, const Twine& filename,
+    NT_Inst inst, std::string_view filename,
     std::function<void(size_t line, const char* msg)> warn) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return "invalid instance handle";
+  if (!ii) {
+    return "invalid instance handle";
+  }
 
   return ii->storage.LoadPersistent(filename, warn);
 }
 
-const char* SaveEntries(NT_Inst inst, const Twine& filename,
-                        const Twine& prefix) {
+const char* SaveEntries(NT_Inst inst, std::string_view filename,
+                        std::string_view prefix) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return "invalid instance handle";
+  if (!ii) {
+    return "invalid instance handle";
+  }
 
   return ii->storage.SaveEntries(filename, prefix);
 }
 
 const char* LoadEntries(
-    NT_Inst inst, const Twine& filename, const Twine& prefix,
+    NT_Inst inst, std::string_view filename, std::string_view prefix,
     std::function<void(size_t line, const char* msg)> warn) {
   auto ii = InstanceImpl::Get(Handle{inst}.GetTypedInst(Handle::kInstance));
-  if (!ii) return "invalid instance handle";
+  if (!ii) {
+    return "invalid instance handle";
+  }
 
   return ii->storage.LoadEntries(filename, prefix, warn);
 }
 
-void SetLogger(LogFunc func, unsigned int min_level) {
-  auto ii = InstanceImpl::GetDefault();
-  static wpi::mutex mutex;
-  static unsigned int logger = 0;
-  std::scoped_lock lock(mutex);
-  if (logger != 0) ii->logger_impl.Remove(logger);
-  logger = ii->logger_impl.Add(
-      [=](const LogMessage& msg) {
-        func(msg.level, msg.filename, msg.line, msg.message.c_str());
-      },
-      min_level, UINT_MAX);
-}
-
 NT_Logger AddLogger(NT_Inst inst,
                     std::function<void(const LogMessage& msg)> func,
-                    unsigned int min_level, unsigned int max_level) {
+                    unsigned int minLevel, unsigned int maxLevel) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
-  if (min_level < ii->logger.min_level()) ii->logger.set_min_level(min_level);
+  if (minLevel < ii->logger.min_level()) {
+    ii->logger.set_min_level(minLevel);
+  }
 
-  return Handle(i, ii->logger_impl.Add(func, min_level, max_level),
+  return Handle(i, ii->logger_impl.Add(func, minLevel, maxLevel),
                 Handle::kLogger);
 }
 
 NT_LoggerPoller CreateLoggerPoller(NT_Inst inst) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return 0;
+  if (!ii) {
+    return 0;
+  }
 
   return Handle(i, ii->logger_impl.CreatePoller(), Handle::kLoggerPoller);
 }
@@ -1007,7 +1088,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kLoggerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->logger_impl.RemovePoller(id);
 }
@@ -1018,9 +1101,13 @@
   int id = handle.GetTypedIndex(Handle::kLoggerPoller);
   int i = handle.GetInst();
   auto ii = InstanceImpl::Get(i);
-  if (id < 0 || !ii) return 0;
+  if (id < 0 || !ii) {
+    return 0;
+  }
 
-  if (min_level < ii->logger.min_level()) ii->logger.set_min_level(min_level);
+  if (min_level < ii->logger.min_level()) {
+    ii->logger.set_min_level(min_level);
+  }
 
   return Handle(i, ii->logger_impl.AddPolled(id, min_level, max_level),
                 Handle::kLogger);
@@ -1030,7 +1117,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kLoggerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<LogMessage>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->logger_impl.Poll(static_cast<unsigned int>(id));
 }
@@ -1041,7 +1130,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kLoggerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return std::vector<LogMessage>{};
+  if (id < 0 || !ii) {
+    return {};
+  }
 
   return ii->logger_impl.Poll(static_cast<unsigned int>(id), timeout,
                               timed_out);
@@ -1051,7 +1142,9 @@
   Handle handle{poller};
   int id = handle.GetTypedIndex(Handle::kLoggerPoller);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (id < 0 || !ii) return;
+  if (id < 0 || !ii) {
+    return;
+  }
 
   ii->logger_impl.CancelPoll(id);
 }
@@ -1060,7 +1153,9 @@
   Handle handle{logger};
   int uid = handle.GetTypedIndex(Handle::kLogger);
   auto ii = InstanceImpl::Get(handle.GetInst());
-  if (uid < 0 || !ii) return;
+  if (uid < 0 || !ii) {
+    return;
+  }
 
   ii->logger_impl.Remove(uid);
   ii->logger.set_min_level(ii->logger_impl.GetMinLevel());
@@ -1069,7 +1164,9 @@
 bool WaitForLoggerQueue(NT_Inst inst, double timeout) {
   int i = Handle{inst}.GetTypedInst(Handle::kInstance);
   auto ii = InstanceImpl::Get(i);
-  if (!ii) return true;
+  if (!ii) {
+    return true;
+  }
   return ii->logger_impl.WaitForQueue(timeout);
 }
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_test.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_test.cpp
index d633fc5..64bde68 100644
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_test.cpp
+++ b/third_party/allwpilib/ntcore/src/main/native/cpp/ntcore_test.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ntcore_test.h"
 
+#include <cstring>
+
 #include <wpi/MemAlloc.h>
 
 #include "Value_internal.h"
@@ -15,7 +14,7 @@
 struct NT_String* NT_GetStringForTesting(const char* string, int* struct_size) {
   struct NT_String* str =
       static_cast<NT_String*>(wpi::safe_calloc(1, sizeof(NT_String)));
-  nt::ConvertToC(wpi::StringRef(string), str);
+  nt::ConvertToC(string, str);
   *struct_size = sizeof(NT_String);
   return str;
 }
@@ -27,7 +26,7 @@
                                                int* struct_size) {
   struct NT_EntryInfo* entry_info =
       static_cast<NT_EntryInfo*>(wpi::safe_calloc(1, sizeof(NT_EntryInfo)));
-  nt::ConvertToC(wpi::StringRef(name), &entry_info->name);
+  nt::ConvertToC(name, &entry_info->name);
   entry_info->type = type;
   entry_info->flags = flags;
   entry_info->last_change = last_change;
@@ -45,8 +44,8 @@
     uint64_t last_update, unsigned int protocol_version, int* struct_size) {
   struct NT_ConnectionInfo* conn_info = static_cast<NT_ConnectionInfo*>(
       wpi::safe_calloc(1, sizeof(NT_ConnectionInfo)));
-  nt::ConvertToC(wpi::StringRef(remote_id), &conn_info->remote_id);
-  nt::ConvertToC(wpi::StringRef(remote_ip), &conn_info->remote_ip);
+  nt::ConvertToC(remote_id, &conn_info->remote_id);
+  nt::ConvertToC(remote_ip, &conn_info->remote_ip);
   conn_info->remote_port = remote_port;
   conn_info->last_update = last_update;
   conn_info->protocol_version = protocol_version;
@@ -89,7 +88,7 @@
       static_cast<NT_Value*>(wpi::safe_calloc(1, sizeof(NT_Value)));
   value->type = NT_STRING;
   value->last_change = last_change;
-  nt::ConvertToC(wpi::StringRef(str), &value->data.v_string);
+  nt::ConvertToC(str, &value->data.v_string);
   *struct_size = sizeof(NT_Value);
   return value;
 }
@@ -100,7 +99,7 @@
       static_cast<NT_Value*>(wpi::safe_calloc(1, sizeof(NT_Value)));
   value->type = NT_RAW;
   value->last_change = last_change;
-  nt::ConvertToC(wpi::StringRef(raw, raw_len), &value->data.v_string);
+  nt::ConvertToC(std::string_view(raw, raw_len), &value->data.v_string);
   *struct_size = sizeof(NT_Value);
   return value;
 }
@@ -167,7 +166,7 @@
 
 static void CopyNtString(const struct NT_String* copy_from,
                          struct NT_String* copy_to) {
-  nt::ConvertToC(wpi::StringRef(copy_from->str, copy_from->len), copy_to);
+  nt::ConvertToC({copy_from->str, copy_from->len}, copy_to);
 }
 
 struct NT_RpcParamDef* NT_GetRpcParamDefForTesting(const char* name,
@@ -175,7 +174,7 @@
                                                    int* struct_size) {
   struct NT_RpcParamDef* def =
       static_cast<NT_RpcParamDef*>(wpi::safe_calloc(1, sizeof(NT_RpcParamDef)));
-  nt::ConvertToC(wpi::StringRef(name), &def->name);
+  nt::ConvertToC(name, &def->name);
   CopyNtValue(val, &def->def_value);
   *struct_size = sizeof(NT_RpcParamDef);
   return def;
@@ -192,7 +191,7 @@
                                                       int* struct_size) {
   struct NT_RpcResultDef* def = static_cast<NT_RpcResultDef*>(
       wpi::safe_calloc(1, sizeof(NT_RpcResultDef)));
-  nt::ConvertToC(wpi::StringRef(name), &def->name);
+  nt::ConvertToC(name, &def->name);
   def->type = type;
   *struct_size = sizeof(NT_RpcResultDef);
   return def;
@@ -210,7 +209,7 @@
   struct NT_RpcDefinition* def = static_cast<NT_RpcDefinition*>(
       wpi::safe_calloc(1, sizeof(NT_RpcDefinition)));
   def->version = version;
-  nt::ConvertToC(wpi::StringRef(name), &def->name);
+  nt::ConvertToC(name, &def->name);
   def->num_params = num_params;
   def->params = static_cast<NT_RpcParamDef*>(
       wpi::safe_malloc(num_params * sizeof(NT_RpcParamDef)));
@@ -237,8 +236,8 @@
       static_cast<NT_RpcAnswer*>(wpi::safe_calloc(1, sizeof(NT_RpcAnswer)));
   info->entry = rpc_id;
   info->call = call_uid;
-  nt::ConvertToC(wpi::StringRef(name), &info->name);
-  nt::ConvertToC(wpi::StringRef(params, params_len), &info->params);
+  nt::ConvertToC(name, &info->name);
+  nt::ConvertToC({params, params_len}, &info->params);
   *struct_size = sizeof(NT_RpcAnswer);
   return info;
 }
diff --git a/third_party/allwpilib/ntcore/src/main/native/cpp/tables/ITableListener.cpp b/third_party/allwpilib/ntcore/src/main/native/cpp/tables/ITableListener.cpp
deleted file mode 100644
index 6abd3bb..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/cpp/tables/ITableListener.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "tables/ITableListener.h"
-
-#include "ntcore_c.h"
-
-void ITableListener::ValueChangedEx(ITable* source, wpi::StringRef key,
-                                    std::shared_ptr<nt::Value> value,
-                                    unsigned int flags) {
-  ValueChanged(source, key, value, (flags & NT_NOTIFY_NEW) != 0);
-}
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/EntryListenerFlags.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/EntryListenerFlags.h
index c5cc620..bbf5e42 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/EntryListenerFlags.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/EntryListenerFlags.h
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_ENTRYLISTENERFLAGS_H_
 #define NTCORE_NETWORKTABLES_ENTRYLISTENERFLAGS_H_
 
 #include "ntcore_c.h"
 
-namespace nt {
-
 /** Entry listener flags */
-namespace EntryListenerFlags {
+namespace nt::EntryListenerFlags {
 
 /**
  * Flag values for use with entry listeners.
@@ -75,8 +70,6 @@
   kFlags = NT_NOTIFY_FLAGS
 };
 
-}  // namespace EntryListenerFlags
-
-}  // namespace nt
+}  // namespace nt::EntryListenerFlags
 
 #endif  // NTCORE_NETWORKTABLES_ENTRYLISTENERFLAGS_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NTSendable.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NTSendable.h
new file mode 100644
index 0000000..88924f8
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NTSendable.h
@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/sendable/Sendable.h>
+
+namespace nt {
+
+class NTSendableBuilder;
+
+/**
+ * Interface for NetworkTable Sendable objects.
+ */
+class NTSendable : public wpi::Sendable {
+ public:
+  /**
+   * Initializes this Sendable object.
+   *
+   * @param builder sendable builder
+   */
+  virtual void InitSendable(NTSendableBuilder& builder) = 0;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+};
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NTSendableBuilder.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NTSendableBuilder.h
new file mode 100644
index 0000000..8ff265b
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NTSendableBuilder.h
@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string_view>
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "networktables/NetworkTable.h"
+#include "networktables/NetworkTableEntry.h"
+#include "networktables/NetworkTableValue.h"
+
+namespace nt {
+
+class NTSendableBuilder : public wpi::SendableBuilder {
+ public:
+  /**
+   * Set the function that should be called to update the network table
+   * for things other than properties.  Note this function is not passed
+   * the network table object; instead it should use the entry handles
+   * returned by GetEntry().
+   *
+   * @param func    function
+   */
+  virtual void SetUpdateTable(std::function<void()> func) = 0;
+
+  /**
+   * Add a property without getters or setters.  This can be used to get
+   * entry handles for the function called by SetUpdateTable().
+   *
+   * @param key   property name
+   * @return Network table entry
+   */
+  virtual NetworkTableEntry GetEntry(std::string_view key) = 0;
+
+  /**
+   * Add a NetworkTableValue property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddValueProperty(
+      std::string_view key, std::function<std::shared_ptr<Value>()> getter,
+      std::function<void(std::shared_ptr<Value>)> setter) = 0;
+
+  /**
+   * Get the network table.
+   * @return The network table
+   */
+  virtual std::shared_ptr<NetworkTable> GetTable() = 0;
+
+  /**
+   * Gets the kind of backend being used.
+   *
+   * @return Backend kind
+   */
+  BackendKind GetBackendKind() const override;
+};
+
+}  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h
index 3bfadbd..3a04e45 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTable.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_NETWORKTABLE_H_
 #define NTCORE_NETWORKTABLES_NETWORKTABLE_H_
@@ -11,36 +8,23 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
 #include <wpi/StringMap.h>
-#include <wpi/Twine.h>
 #include <wpi/mutex.h>
+#include <wpi/span.h>
 
 #include "networktables/NetworkTableEntry.h"
 #include "networktables/TableEntryListener.h"
 #include "networktables/TableListener.h"
 #include "ntcore_c.h"
-#include "tables/ITable.h"
 
 namespace nt {
 
-using wpi::ArrayRef;
-using wpi::StringRef;
-using wpi::Twine;
-
 class NetworkTableInstance;
 
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#elif _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4996)
-#endif
-
 /**
  * @defgroup ntcore_cpp_api ntcore C++ object-oriented API
  *
@@ -51,22 +35,13 @@
  * A network table that knows its subtable path.
  * @ingroup ntcore_cpp_api
  */
-class NetworkTable final : public ITable {
+class NetworkTable final {
  private:
   NT_Inst m_inst;
   std::string m_path;
   mutable wpi::mutex m_mutex;
   mutable wpi::StringMap<NT_Entry> m_entries;
-  typedef std::pair<ITableListener*, NT_EntryListener> Listener;
-  std::vector<Listener> m_listeners;
-  std::vector<NT_EntryListener> m_lambdaListeners;
-
-  static std::vector<std::string> s_ip_addresses;
-  static std::string s_persistent_filename;
-  static bool s_client;
-  static bool s_enable_ds;
-  static bool s_running;
-  static unsigned int s_port;
+  std::vector<NT_EntryListener> m_listeners;
 
   struct private_init {};
   friend class NetworkTableInstance;
@@ -79,7 +54,7 @@
    * @param key key
    * @return base name
    */
-  static StringRef BasenameKey(StringRef key);
+  static std::string_view BasenameKey(std::string_view key);
 
   /**
    * Normalizes an network table key to contain no consecutive slashes and
@@ -97,12 +72,12 @@
    *                         with a leading slash
    * @return normalized key
    */
-  static std::string NormalizeKey(const Twine& key,
+  static std::string NormalizeKey(std::string_view key,
                                   bool withLeadingSlash = true);
 
-  static StringRef NormalizeKey(const Twine& key,
-                                wpi::SmallVectorImpl<char>& buf,
-                                bool withLeadingSlash = true);
+  static std::string_view NormalizeKey(std::string_view key,
+                                       wpi::SmallVectorImpl<char>& buf,
+                                       bool withLeadingSlash = true);
 
   /**
    * Gets a list of the names of all the super tables of a given key. For
@@ -112,13 +87,13 @@
    * @param key the key
    * @return List of super tables
    */
-  static std::vector<std::string> GetHierarchy(const Twine& key);
+  static std::vector<std::string> GetHierarchy(std::string_view key);
 
   /**
    * Constructor.  Use NetworkTableInstance::GetTable() or GetSubTable()
    * instead.
    */
-  NetworkTable(NT_Inst inst, const Twine& path, const private_init&);
+  NetworkTable(NT_Inst inst, std::string_view path, const private_init&);
   virtual ~NetworkTable();
 
   /**
@@ -131,169 +106,7 @@
   /**
    * The path separator for sub-tables and keys
    */
-  static const char PATH_SEPARATOR_CHAR;
-
-  /**
-   * Initializes network tables
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::StartServer() or "
-      "NetworkTableInstance::StartClient() instead")
-  static void Initialize();
-
-  /**
-   * Shuts down network tables
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::StopServer() or "
-      "NetworkTableInstance::StopClient() instead")
-  static void Shutdown();
-
-  /**
-   * set that network tables should be a client
-   * This must be called before initialize or GetTable
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::StartClient() instead")
-  static void SetClientMode();
-
-  /**
-   * set that network tables should be a server
-   * This must be called before initialize or GetTable
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::StartServer() instead")
-  static void SetServerMode();
-
-  /**
-   * set the team the robot is configured for (this will set the mdns address
-   * that network tables will connect to in client mode)
-   * This must be called before initialize or GetTable
-   *
-   * @param team the team number
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::SetServerTeam() or "
-      "NetworkTableInstance::StartClientTeam() instead")
-  static void SetTeam(int team);
-
-  /**
-   * @param address the address that network tables will connect to in client
-   * mode
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::SetServer() or "
-      "NetworkTableInstance::StartClient() instead")
-  static void SetIPAddress(StringRef address);
-
-  /**
-   * @param addresses the addresses that network tables will connect to in
-   * client mode (in round robin order)
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::SetServer() or "
-      "NetworkTableInstance::StartClient() instead")
-  static void SetIPAddress(ArrayRef<std::string> addresses);
-
-  /**
-   * Set the port number that network tables will connect to in client
-   * mode or listen to in server mode.
-   *
-   * @param port the port number
-   */
-  WPI_DEPRECATED(
-      "use the appropriate parameters to NetworkTableInstance::SetServer(), "
-      "NetworkTableInstance::StartClient(), "
-      "NetworkTableInstance::StartServer(), and "
-      "NetworkTableInstance::StartDSClient() instead")
-  static void SetPort(unsigned int port);
-
-  /**
-   * Enable requesting the server address from the Driver Station.
-   *
-   * @param enabled whether to enable the connection to the local DS
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::StartDSClient() and "
-      "NetworkTableInstance::StopDSClient() instead")
-  static void SetDSClientEnabled(bool enabled);
-
-  /**
-   * Sets the persistent filename.
-   *
-   * @param filename the filename that the network tables server uses for
-   * automatic loading and saving of persistent values
-   */
-  WPI_DEPRECATED(
-      "use the appropriate parameter to NetworkTableInstance::StartServer() "
-      "instead")
-  static void SetPersistentFilename(StringRef filename);
-
-  /**
-   * Sets the network identity.
-   * This is provided in the connection info on the remote end.
-   *
-   * @param name identity
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::SetNetworkIdentity() instead")
-  static void SetNetworkIdentity(StringRef name);
-
-  /**
-   * Deletes ALL keys in ALL subtables.  Use with caution!
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::DeleteAllEntries() instead")
-  static void GlobalDeleteAll();
-
-  /**
-   * Flushes all updated values immediately to the network.
-   * Note: This is rate-limited to protect the network from flooding.
-   * This is primarily useful for synchronizing network updates with
-   * user code.
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::Flush() instead")
-  static void Flush();
-
-  /**
-   * Set the periodic update rate.
-   * Sets how frequently updates are sent to other nodes over the network.
-   *
-   * @param interval update interval in seconds (range 0.01 to 1.0)
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::SetUpdateRate() instead")
-  static void SetUpdateRate(double interval);
-
-  /**
-   * Saves persistent keys to a file.  The server does this automatically.
-   *
-   * @param filename file name
-   * @return Error (or nullptr).
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::SavePersistent() instead")
-  static const char* SavePersistent(StringRef filename);
-
-  /**
-   * Loads persistent keys from a file.  The server does this automatically.
-   *
-   * @param filename file name
-   * @param warn callback function called for warnings
-   * @return Error (or nullptr).
-   */
-  WPI_DEPRECATED("use NetworkTableInstance::LoadPersistent() instead")
-  static const char* LoadPersistent(
-      StringRef filename,
-      std::function<void(size_t line, const char* msg)> warn);
-
-  /**
-   * Gets the table with the specified key. If the table does not exist, a new
-   * table will be created.<br>
-   * This will automatically initialize network tables if it has not been
-   * already.
-   *
-   * @param key  the key name
-   * @return the network table requested
-   */
-  WPI_DEPRECATED(
-      "use NetworkTableInstance::GetTable() or "
-      "NetworkTableInstance::GetEntry() instead")
-  static std::shared_ptr<NetworkTable> GetTable(StringRef key);
+  static constexpr char PATH_SEPARATOR_CHAR = '/';
 
   /**
    * Gets the entry for a subkey.
@@ -301,7 +114,7 @@
    * @param key the key name
    * @return Network table entry.
    */
-  NetworkTableEntry GetEntry(const Twine& key) const;
+  NetworkTableEntry GetEntry(std::string_view key) const;
 
   /**
    * Listen to keys only within this table.
@@ -321,7 +134,7 @@
    * @param flags       EntryListenerFlags bitmask
    * @return Listener handle
    */
-  NT_EntryListener AddEntryListener(const Twine& key,
+  NT_EntryListener AddEntryListener(std::string_view key,
                                     TableEntryListener listener,
                                     unsigned int flags) const;
 
@@ -351,38 +164,6 @@
    */
   void RemoveTableListener(NT_EntryListener listener);
 
-  WPI_DEPRECATED(
-      "use AddEntryListener() instead with flags value of NT_NOTIFY_NEW | "
-      "NT_NOTIFY_UPDATE")
-  void AddTableListener(ITableListener* listener) override;
-
-  WPI_DEPRECATED(
-      "use AddEntryListener() instead with flags value of NT_NOTIFY_NEW | "
-      "NT_NOTIFY_UPDATE | NT_NOTIFY_IMMEDIATE")
-  void AddTableListener(ITableListener* listener,
-                        bool immediateNotify) override;
-
-  WPI_DEPRECATED("use AddEntryListener() instead")
-  void AddTableListenerEx(ITableListener* listener,
-                          unsigned int flags) override;
-
-  WPI_DEPRECATED("use AddEntryListener() instead")
-  void AddTableListener(StringRef key, ITableListener* listener,
-                        bool immediateNotify) override;
-
-  WPI_DEPRECATED("use AddEntryListener() instead")
-  void AddTableListenerEx(StringRef key, ITableListener* listener,
-                          unsigned int flags) override;
-
-  WPI_DEPRECATED("use AddSubTableListener(TableListener, bool) instead")
-  void AddSubTableListener(ITableListener* listener) override;
-
-  WPI_DEPRECATED("use AddSubTableListener(TableListener, bool) instead")
-  void AddSubTableListener(ITableListener* listener, bool localNotify) override;
-
-  WPI_DEPRECATED("use RemoveTableListener(NT_EntryListener) instead")
-  void RemoveTableListener(ITableListener* listener) override;
-
   /**
    * Returns the table at the specified key. If there is no table at the
    * specified key, it will create a new table
@@ -390,7 +171,7 @@
    * @param key the key name
    * @return the networktable to be returned
    */
-  std::shared_ptr<NetworkTable> GetSubTable(const Twine& key) const override;
+  std::shared_ptr<NetworkTable> GetSubTable(std::string_view key) const;
 
   /**
    * Determines whether the given key is in this table.
@@ -398,7 +179,7 @@
    * @param key the key to search for
    * @return true if the table as a value assigned to the given key
    */
-  bool ContainsKey(const Twine& key) const override;
+  bool ContainsKey(std::string_view key) const;
 
   /**
    * Determines whether there exists a non-empty subtable for this key
@@ -408,7 +189,7 @@
    * @return true if there is a subtable with the key which contains at least
    * one key/subtable of its own
    */
-  bool ContainsSubTable(const Twine& key) const override;
+  bool ContainsSubTable(std::string_view key) const;
 
   /**
    * Gets all keys in the table (not including sub-tables).
@@ -416,21 +197,21 @@
    * @param types bitmask of types; 0 is treated as a "don't care".
    * @return keys currently in the table
    */
-  std::vector<std::string> GetKeys(int types = 0) const override;
+  std::vector<std::string> GetKeys(int types = 0) const;
 
   /**
    * Gets the names of all subtables in the table.
    *
    * @return subtables currently in the table
    */
-  std::vector<std::string> GetSubTables() const override;
+  std::vector<std::string> GetSubTables() const;
 
   /**
    * Makes a key's value persistent through program restarts.
    *
    * @param key the key to make persistent
    */
-  void SetPersistent(StringRef key) override;
+  void SetPersistent(std::string_view key);
 
   /**
    * Stop making a key's value persistent through program restarts.
@@ -438,7 +219,7 @@
    *
    * @param key the key name
    */
-  void ClearPersistent(StringRef key) override;
+  void ClearPersistent(std::string_view key);
 
   /**
    * Returns whether the value is persistent through program restarts.
@@ -446,7 +227,7 @@
    *
    * @param key the key name
    */
-  bool IsPersistent(StringRef key) const override;
+  bool IsPersistent(std::string_view key) const;
 
   /**
    * Sets flags on the specified key in this table. The key can
@@ -455,7 +236,7 @@
    * @param key the key name
    * @param flags the flags to set (bitmask)
    */
-  void SetFlags(StringRef key, unsigned int flags) override;
+  void SetFlags(std::string_view key, unsigned int flags);
 
   /**
    * Clears flags on the specified key in this table. The key can
@@ -464,7 +245,7 @@
    * @param key the key name
    * @param flags the flags to clear (bitmask)
    */
-  void ClearFlags(StringRef key, unsigned int flags) override;
+  void ClearFlags(std::string_view key, unsigned int flags);
 
   /**
    * Returns the flags for the specified key.
@@ -472,14 +253,14 @@
    * @param key the key name
    * @return the flags, or 0 if the key is not defined
    */
-  unsigned int GetFlags(StringRef key) const override;
+  unsigned int GetFlags(std::string_view key) const;
 
   /**
    * Deletes the specified key in this table.
    *
    * @param key the key name
    */
-  void Delete(const Twine& key) override;
+  void Delete(std::string_view key);
 
   /**
    * Put a number in the table
@@ -488,7 +269,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutNumber(StringRef key, double value) override;
+  bool PutNumber(std::string_view key, double value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -497,7 +278,7 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  bool SetDefaultNumber(StringRef key, double defaultValue) override;
+  bool SetDefaultNumber(std::string_view key, double defaultValue);
 
   /**
    * Gets the number associated with the given name.
@@ -507,7 +288,7 @@
    * @return the value associated with the given key or the given default value
    * if there is no value associated with the key
    */
-  double GetNumber(StringRef key, double defaultValue) const override;
+  double GetNumber(std::string_view key, double defaultValue) const;
 
   /**
    * Put a string in the table
@@ -516,7 +297,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutString(StringRef key, StringRef value) override;
+  bool PutString(std::string_view key, std::string_view value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -525,7 +306,7 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  bool SetDefaultString(StringRef key, StringRef defaultValue) override;
+  bool SetDefaultString(std::string_view key, std::string_view defaultValue);
 
   /**
    * Gets the string associated with the given name. If the key does not
@@ -536,7 +317,8 @@
    * @return the value associated with the given key or the given default value
    * if there is no value associated with the key
    */
-  std::string GetString(StringRef key, StringRef defaultValue) const override;
+  std::string GetString(std::string_view key,
+                        std::string_view defaultValue) const;
 
   /**
    * Put a boolean in the table
@@ -545,7 +327,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutBoolean(StringRef key, bool value) override;
+  bool PutBoolean(std::string_view key, bool value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -554,7 +336,7 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  bool SetDefaultBoolean(StringRef key, bool defaultValue) override;
+  bool SetDefaultBoolean(std::string_view key, bool defaultValue);
 
   /**
    * Gets the boolean associated with the given name. If the key does not
@@ -565,7 +347,7 @@
    * @return the value associated with the given key or the given default value
    * if there is no value associated with the key
    */
-  bool GetBoolean(StringRef key, bool defaultValue) const override;
+  bool GetBoolean(std::string_view key, bool defaultValue) const;
 
   /**
    * Put a boolean array in the table
@@ -578,7 +360,7 @@
    *       std::vector<bool> is special-cased in C++.  0 is false, any
    *       non-zero value is true.
    */
-  bool PutBooleanArray(StringRef key, ArrayRef<int> value) override;
+  bool PutBooleanArray(std::string_view key, wpi::span<const int> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -587,8 +369,8 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @return False if the table key exists with a different type
    */
-  bool SetDefaultBooleanArray(StringRef key,
-                              ArrayRef<int> defaultValue) override;
+  bool SetDefaultBooleanArray(std::string_view key,
+                              wpi::span<const int> defaultValue);
 
   /**
    * Returns the boolean array the key maps to. If the key does not exist or is
@@ -606,8 +388,8 @@
    *       because std::vector<bool> is special-cased in C++.  0 is false, any
    *       non-zero value is true.
    */
-  std::vector<int> GetBooleanArray(StringRef key,
-                                   ArrayRef<int> defaultValue) const override;
+  std::vector<int> GetBooleanArray(std::string_view key,
+                                   wpi::span<const int> defaultValue) const;
 
   /**
    * Put a number array in the table
@@ -616,7 +398,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutNumberArray(StringRef key, ArrayRef<double> value) override;
+  bool PutNumberArray(std::string_view key, wpi::span<const double> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -625,8 +407,8 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  bool SetDefaultNumberArray(StringRef key,
-                             ArrayRef<double> defaultValue) override;
+  bool SetDefaultNumberArray(std::string_view key,
+                             wpi::span<const double> defaultValue);
 
   /**
    * Returns the number array the key maps to. If the key does not exist or is
@@ -641,7 +423,7 @@
    *       concern, use GetValue() instead.
    */
   std::vector<double> GetNumberArray(
-      StringRef key, ArrayRef<double> defaultValue) const override;
+      std::string_view key, wpi::span<const double> defaultValue) const;
 
   /**
    * Put a string array in the table
@@ -650,7 +432,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutStringArray(StringRef key, ArrayRef<std::string> value) override;
+  bool PutStringArray(std::string_view key, wpi::span<const std::string> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -659,8 +441,8 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  bool SetDefaultStringArray(StringRef key,
-                             ArrayRef<std::string> defaultValue) override;
+  bool SetDefaultStringArray(std::string_view key,
+                             wpi::span<const std::string> defaultValue);
 
   /**
    * Returns the string array the key maps to. If the key does not exist or is
@@ -675,7 +457,7 @@
    *       concern, use GetValue() instead.
    */
   std::vector<std::string> GetStringArray(
-      StringRef key, ArrayRef<std::string> defaultValue) const override;
+      std::string_view key, wpi::span<const std::string> defaultValue) const;
 
   /**
    * Put a raw value (byte array) in the table
@@ -684,7 +466,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutRaw(StringRef key, StringRef value) override;
+  bool PutRaw(std::string_view key, std::string_view value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -693,7 +475,7 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @return False if the table key exists with a different type
    */
-  bool SetDefaultRaw(StringRef key, StringRef defaultValue) override;
+  bool SetDefaultRaw(std::string_view key, std::string_view defaultValue);
 
   /**
    * Returns the raw value (byte array) the key maps to. If the key does not
@@ -707,7 +489,7 @@
    * @note This makes a copy of the raw contents.  If the overhead of this is a
    *       concern, use GetValue() instead.
    */
-  std::string GetRaw(StringRef key, StringRef defaultValue) const override;
+  std::string GetRaw(std::string_view key, std::string_view defaultValue) const;
 
   /**
    * Put a value in the table
@@ -716,7 +498,7 @@
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
    */
-  bool PutValue(const Twine& key, std::shared_ptr<Value> value) override;
+  bool PutValue(std::string_view key, std::shared_ptr<Value> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -725,8 +507,8 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @return False if the table key exists with a different type
    */
-  bool SetDefaultValue(const Twine& key,
-                       std::shared_ptr<Value> defaultValue) override;
+  bool SetDefaultValue(std::string_view key,
+                       std::shared_ptr<Value> defaultValue);
 
   /**
    * Gets the value associated with a key as an object
@@ -735,14 +517,14 @@
    * @return the value associated with the given key, or nullptr if the key
    * does not exist
    */
-  std::shared_ptr<Value> GetValue(const Twine& key) const override;
+  std::shared_ptr<Value> GetValue(std::string_view key) const;
 
   /**
    * Gets the full path of this table.  Does not include the trailing "/".
    *
    * @return The path (e.g "", "/foo").
    */
-  StringRef GetPath() const override;
+  std::string_view GetPath() const;
 
   /**
    * Save table values to a file.  The file format used is identical to
@@ -751,7 +533,7 @@
    * @param filename  filename
    * @return error string, or nullptr if successful
    */
-  const char* SaveEntries(const Twine& filename) const;
+  const char* SaveEntries(std::string_view filename) const;
 
   /**
    * Load table values from a file.  The file format used is identical to
@@ -762,21 +544,10 @@
    * @return error string, or nullptr if successful
    */
   const char* LoadEntries(
-      const Twine& filename,
+      std::string_view filename,
       std::function<void(size_t line, const char* msg)> warn);
 };
 
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#elif _WIN32
-#pragma warning(pop)
-#endif
-
 }  // namespace nt
 
-// For backwards compatibility
-#ifndef NAMESPACED_NT
-using nt::NetworkTable;  // NOLINT
-#endif
-
 #endif  // NTCORE_NETWORKTABLES_NETWORKTABLE_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.h
index 8fdedc6..c6c9c86 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_H_
 #define NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_H_
@@ -13,10 +10,10 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
 #include "networktables/NetworkTableType.h"
 #include "networktables/NetworkTableValue.h"
@@ -26,10 +23,6 @@
 
 namespace nt {
 
-using wpi::ArrayRef;
-using wpi::StringRef;
-using wpi::Twine;
-
 class NetworkTableInstance;
 
 /**
@@ -150,7 +143,7 @@
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
    */
-  std::string GetString(StringRef defaultValue) const;
+  std::string GetString(std::string_view defaultValue) const;
 
   /**
    * Gets the entry's value as a raw. If the entry does not exist or is of
@@ -159,7 +152,7 @@
    * @param defaultValue the value to be returned if no value is found
    * @return the entry's value or the given default value
    */
-  std::string GetRaw(StringRef defaultValue) const;
+  std::string GetRaw(std::string_view defaultValue) const;
 
   /**
    * Gets the entry's value as a boolean array. If the entry does not exist
@@ -175,7 +168,7 @@
    *       because std::vector<bool> is special-cased in C++.  0 is false, any
    *       non-zero value is true.
    */
-  std::vector<int> GetBooleanArray(ArrayRef<int> defaultValue) const;
+  std::vector<int> GetBooleanArray(wpi::span<const int> defaultValue) const;
 
   /**
    * Gets the entry's value as a boolean array. If the entry does not exist
@@ -204,7 +197,8 @@
    * @note This makes a copy of the array.  If the overhead of this is a
    *       concern, use GetValue() instead.
    */
-  std::vector<double> GetDoubleArray(ArrayRef<double> defaultValue) const;
+  std::vector<double> GetDoubleArray(
+      wpi::span<const double> defaultValue) const;
 
   /**
    * Gets the entry's value as a double array. If the entry does not exist
@@ -230,7 +224,7 @@
    *       concern, use GetValue() instead.
    */
   std::vector<std::string> GetStringArray(
-      ArrayRef<std::string> defaultValue) const;
+      wpi::span<const std::string> defaultValue) const;
 
   /**
    * Gets the entry's value as a string array. If the entry does not exist
@@ -248,7 +242,7 @@
   /**
    * Sets the entry's value if it does not exist.
    *
-   * @param defaultValue the default value to set
+   * @param value the default value to set
    * @return False if the entry exists with a different type
    */
   bool SetDefaultValue(std::shared_ptr<Value> value);
@@ -275,7 +269,7 @@
    * @param defaultValue the default value to set
    * @return False if the entry exists with a different type
    */
-  bool SetDefaultString(const Twine& defaultValue);
+  bool SetDefaultString(std::string_view defaultValue);
 
   /**
    * Sets the entry's value if it does not exist.
@@ -283,7 +277,7 @@
    * @param defaultValue the default value to set
    * @return False if the entry exists with a different type
    */
-  bool SetDefaultRaw(StringRef defaultValue);
+  bool SetDefaultRaw(std::string_view defaultValue);
 
   /**
    * Sets the entry's value if it does not exist.
@@ -291,7 +285,7 @@
    * @param defaultValue the default value to set
    * @return False if the entry exists with a different type
    */
-  bool SetDefaultBooleanArray(ArrayRef<int> defaultValue);
+  bool SetDefaultBooleanArray(wpi::span<const int> defaultValue);
 
   /**
    * Sets the entry's value if it does not exist.
@@ -307,7 +301,7 @@
    * @param defaultValue the default value to set
    * @return False if the entry exists with a different type
    */
-  bool SetDefaultDoubleArray(ArrayRef<double> defaultValue);
+  bool SetDefaultDoubleArray(wpi::span<const double> defaultValue);
 
   /**
    * Sets the entry's value if it does not exist.
@@ -323,7 +317,7 @@
    * @param defaultValue the default value to set
    * @return False if the entry exists with a different type
    */
-  bool SetDefaultStringArray(ArrayRef<std::string> defaultValue);
+  bool SetDefaultStringArray(wpi::span<const std::string> defaultValue);
 
   /**
    * Sets the entry's value if it does not exist.
@@ -363,7 +357,7 @@
    * @param value the value to set
    * @return False if the entry exists with a different type
    */
-  bool SetString(const Twine& value);
+  bool SetString(std::string_view value);
 
   /**
    * Sets the entry's value.
@@ -371,7 +365,7 @@
    * @param value the value to set
    * @return False if the entry exists with a different type
    */
-  bool SetRaw(StringRef value);
+  bool SetRaw(std::string_view value);
 
   /**
    * Sets the entry's value.
@@ -379,7 +373,7 @@
    * @param value the value to set
    * @return False if the entry exists with a different type
    */
-  bool SetBooleanArray(ArrayRef<bool> value);
+  bool SetBooleanArray(wpi::span<const bool> value);
 
   /**
    * Sets the entry's value.
@@ -395,7 +389,7 @@
    * @param value the value to set
    * @return False if the entry exists with a different type
    */
-  bool SetBooleanArray(ArrayRef<int> value);
+  bool SetBooleanArray(wpi::span<const int> value);
 
   /**
    * Sets the entry's value.
@@ -411,7 +405,7 @@
    * @param value the value to set
    * @return False if the entry exists with a different type
    */
-  bool SetDoubleArray(ArrayRef<double> value);
+  bool SetDoubleArray(wpi::span<const double> value);
 
   /**
    * Sets the entry's value.
@@ -427,7 +421,7 @@
    * @param value the value to set
    * @return False if the entry exists with a different type
    */
-  bool SetStringArray(ArrayRef<std::string> value);
+  bool SetStringArray(wpi::span<const std::string> value);
 
   /**
    * Sets the entry's value.
@@ -467,7 +461,7 @@
    *
    * @param value the value to set
    */
-  void ForceSetString(const Twine& value);
+  void ForceSetString(std::string_view value);
 
   /**
    * Sets the entry's value.  If the value is of different type, the type is
@@ -475,7 +469,7 @@
    *
    * @param value the value to set
    */
-  void ForceSetRaw(StringRef value);
+  void ForceSetRaw(std::string_view value);
 
   /**
    * Sets the entry's value.  If the value is of different type, the type is
@@ -483,7 +477,7 @@
    *
    * @param value the value to set
    */
-  void ForceSetBooleanArray(ArrayRef<bool> value);
+  void ForceSetBooleanArray(wpi::span<const bool> value);
 
   /**
    * Sets the entry's value.  If the value is of different type, the type is
@@ -499,7 +493,7 @@
    *
    * @param value the value to set
    */
-  void ForceSetBooleanArray(ArrayRef<int> value);
+  void ForceSetBooleanArray(wpi::span<const int> value);
 
   /**
    * Sets the entry's value.  If the value is of different type, the type is
@@ -515,7 +509,7 @@
    *
    * @param value the value to set
    */
-  void ForceSetDoubleArray(ArrayRef<double> value);
+  void ForceSetDoubleArray(wpi::span<const double> value);
 
   /**
    * Sets the entry's value.  If the value is of different type, the type is
@@ -531,7 +525,7 @@
    *
    * @param value the value to set
    */
-  void ForceSetStringArray(ArrayRef<std::string> value);
+  void ForceSetStringArray(wpi::span<const std::string> value);
 
   /**
    * Sets the entry's value.  If the value is of different type, the type is
@@ -603,7 +597,7 @@
    * @param params      parameter
    * @return RPC call object.
    */
-  RpcCall CallRpc(StringRef params);
+  RpcCall CallRpc(std::string_view params);
 
   /**
    * Add a listener for changes to this entry.
@@ -638,11 +632,11 @@
 
  protected:
   /* Native handle */
-  NT_Entry m_handle;
+  NT_Entry m_handle{0};
 };
 
 }  // namespace nt
 
-#include "networktables/NetworkTableEntry.inl"
+#include "networktables/NetworkTableEntry.inc"
 
 #endif  // NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.inc b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.inc
new file mode 100644
index 0000000..e7deb51
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.inc
@@ -0,0 +1,345 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INC_
+#define NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INC_
+
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include "networktables/NetworkTableEntry.h"
+
+namespace nt {
+
+inline NetworkTableEntry::NetworkTableEntry() {}
+
+inline NetworkTableEntry::NetworkTableEntry(NT_Entry handle)
+    : m_handle{handle} {}
+
+inline NT_Entry NetworkTableEntry::GetHandle() const {
+  return m_handle;
+}
+
+inline bool NetworkTableEntry::Exists() const {
+  return GetEntryType(m_handle) != NT_UNASSIGNED;
+}
+
+inline std::string NetworkTableEntry::GetName() const {
+  return GetEntryName(m_handle);
+}
+
+inline NetworkTableType NetworkTableEntry::GetType() const {
+  return static_cast<NetworkTableType>(GetEntryType(m_handle));
+}
+
+inline unsigned int NetworkTableEntry::GetFlags() const {
+  return GetEntryFlags(m_handle);
+}
+
+inline uint64_t NetworkTableEntry::GetLastChange() const {
+  return GetEntryLastChange(m_handle);
+}
+
+inline EntryInfo NetworkTableEntry::GetInfo() const {
+  return GetEntryInfo(m_handle);
+}
+
+inline std::shared_ptr<Value> NetworkTableEntry::GetValue() const {
+  return GetEntryValue(m_handle);
+}
+
+inline bool NetworkTableEntry::GetBoolean(bool defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_BOOLEAN) {
+    return defaultValue;
+  }
+  return value->GetBoolean();
+}
+
+inline double NetworkTableEntry::GetDouble(double defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_DOUBLE) {
+    return defaultValue;
+  }
+  return value->GetDouble();
+}
+
+inline std::string NetworkTableEntry::GetString(
+    std::string_view defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_STRING) {
+    return std::string{defaultValue};
+  }
+  return std::string{value->GetString()};
+}
+
+inline std::string NetworkTableEntry::GetRaw(
+    std::string_view defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_RAW) {
+    return std::string{defaultValue};
+  }
+  return std::string{value->GetRaw()};
+}
+
+inline std::vector<int> NetworkTableEntry::GetBooleanArray(
+    wpi::span<const int> defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_BOOLEAN_ARRAY) {
+    return {defaultValue.begin(), defaultValue.end()};
+  }
+  auto arr = value->GetBooleanArray();
+  return {arr.begin(), arr.end()};
+}
+
+inline std::vector<int> NetworkTableEntry::GetBooleanArray(
+    std::initializer_list<int> defaultValue) const {
+  return GetBooleanArray({defaultValue.begin(), defaultValue.end()});
+}
+
+inline std::vector<double> NetworkTableEntry::GetDoubleArray(
+    wpi::span<const double> defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_DOUBLE_ARRAY) {
+    return {defaultValue.begin(), defaultValue.end()};
+  }
+  auto arr = value->GetDoubleArray();
+  return {arr.begin(), arr.end()};
+}
+
+inline std::vector<double> NetworkTableEntry::GetDoubleArray(
+    std::initializer_list<double> defaultValue) const {
+  return GetDoubleArray({defaultValue.begin(), defaultValue.end()});
+}
+
+inline std::vector<std::string> NetworkTableEntry::GetStringArray(
+    wpi::span<const std::string> defaultValue) const {
+  auto value = GetEntryValue(m_handle);
+  if (!value || value->type() != NT_STRING_ARRAY) {
+    return {defaultValue.begin(), defaultValue.end()};
+  }
+  auto arr = value->GetStringArray();
+  return {arr.begin(), arr.end()};
+}
+
+inline std::vector<std::string> NetworkTableEntry::GetStringArray(
+    std::initializer_list<std::string> defaultValue) const {
+  return GetStringArray({defaultValue.begin(), defaultValue.end()});
+}
+
+inline bool NetworkTableEntry::SetDefaultValue(std::shared_ptr<Value> value) {
+  return SetDefaultEntryValue(m_handle, value);
+}
+
+inline bool NetworkTableEntry::SetDefaultBoolean(bool defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeBoolean(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultDouble(double defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeDouble(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultString(std::string_view defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeString(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultRaw(std::string_view defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeRaw(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultBooleanArray(
+    wpi::span<const int> defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeBooleanArray(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultBooleanArray(
+    std::initializer_list<int> defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeBooleanArray(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultDoubleArray(
+    wpi::span<const double> defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeDoubleArray(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultDoubleArray(
+    std::initializer_list<double> value) {
+  return SetDefaultEntryValue(m_handle, Value::MakeDoubleArray(value));
+}
+
+inline bool NetworkTableEntry::SetDefaultStringArray(
+    wpi::span<const std::string> defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeStringArray(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetDefaultStringArray(
+    std::initializer_list<std::string> defaultValue) {
+  return SetDefaultEntryValue(m_handle, Value::MakeStringArray(defaultValue));
+}
+
+inline bool NetworkTableEntry::SetValue(std::shared_ptr<Value> value) {
+  return SetEntryValue(m_handle, value);
+}
+
+inline bool NetworkTableEntry::SetBoolean(bool value) {
+  return SetEntryValue(m_handle, Value::MakeBoolean(value));
+}
+
+inline bool NetworkTableEntry::SetDouble(double value) {
+  return SetEntryValue(m_handle, Value::MakeDouble(value));
+}
+
+inline bool NetworkTableEntry::SetString(std::string_view value) {
+  return SetEntryValue(m_handle, Value::MakeString(value));
+}
+
+inline bool NetworkTableEntry::SetRaw(std::string_view value) {
+  return SetEntryValue(m_handle, Value::MakeRaw(value));
+}
+
+inline bool NetworkTableEntry::SetBooleanArray(wpi::span<const bool> value) {
+  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline bool NetworkTableEntry::SetBooleanArray(
+    std::initializer_list<bool> value) {
+  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline bool NetworkTableEntry::SetBooleanArray(wpi::span<const int> value) {
+  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline bool NetworkTableEntry::SetBooleanArray(
+    std::initializer_list<int> value) {
+  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline bool NetworkTableEntry::SetDoubleArray(wpi::span<const double> value) {
+  return SetEntryValue(m_handle, Value::MakeDoubleArray(value));
+}
+
+inline bool NetworkTableEntry::SetDoubleArray(
+    std::initializer_list<double> value) {
+  return SetEntryValue(m_handle, Value::MakeDoubleArray(value));
+}
+
+inline bool NetworkTableEntry::SetStringArray(
+    wpi::span<const std::string> value) {
+  return SetEntryValue(m_handle, Value::MakeStringArray(value));
+}
+
+inline bool NetworkTableEntry::SetStringArray(
+    std::initializer_list<std::string> value) {
+  return SetEntryValue(m_handle, Value::MakeStringArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetValue(std::shared_ptr<Value> value) {
+  SetEntryTypeValue(m_handle, value);
+}
+
+inline void NetworkTableEntry::ForceSetBoolean(bool value) {
+  SetEntryTypeValue(m_handle, Value::MakeBoolean(value));
+}
+
+inline void NetworkTableEntry::ForceSetDouble(double value) {
+  SetEntryTypeValue(m_handle, Value::MakeDouble(value));
+}
+
+inline void NetworkTableEntry::ForceSetString(std::string_view value) {
+  SetEntryTypeValue(m_handle, Value::MakeString(value));
+}
+
+inline void NetworkTableEntry::ForceSetRaw(std::string_view value) {
+  SetEntryTypeValue(m_handle, Value::MakeRaw(value));
+}
+
+inline void NetworkTableEntry::ForceSetBooleanArray(
+    wpi::span<const bool> value) {
+  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetBooleanArray(
+    std::initializer_list<bool> value) {
+  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetBooleanArray(
+    wpi::span<const int> value) {
+  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetBooleanArray(
+    std::initializer_list<int> value) {
+  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetDoubleArray(
+    wpi::span<const double> value) {
+  SetEntryTypeValue(m_handle, Value::MakeDoubleArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetDoubleArray(
+    std::initializer_list<double> value) {
+  SetEntryTypeValue(m_handle, Value::MakeDoubleArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetStringArray(
+    wpi::span<const std::string> value) {
+  SetEntryTypeValue(m_handle, Value::MakeStringArray(value));
+}
+
+inline void NetworkTableEntry::ForceSetStringArray(
+    std::initializer_list<std::string> value) {
+  SetEntryTypeValue(m_handle, Value::MakeStringArray(value));
+}
+
+inline void NetworkTableEntry::SetFlags(unsigned int flags) {
+  SetEntryFlags(m_handle, GetFlags() | flags);
+}
+
+inline void NetworkTableEntry::ClearFlags(unsigned int flags) {
+  SetEntryFlags(m_handle, GetFlags() & ~flags);
+}
+
+inline void NetworkTableEntry::SetPersistent() {
+  SetFlags(kPersistent);
+}
+
+inline void NetworkTableEntry::ClearPersistent() {
+  ClearFlags(kPersistent);
+}
+
+inline bool NetworkTableEntry::IsPersistent() const {
+  return (GetFlags() & kPersistent) != 0;
+}
+
+inline void NetworkTableEntry::Delete() {
+  DeleteEntry(m_handle);
+}
+
+inline void NetworkTableEntry::CreateRpc(
+    std::function<void(const RpcAnswer& answer)> callback) {
+  ::nt::CreateRpc(m_handle, std::string_view("\0", 1), callback);
+}
+
+inline RpcCall NetworkTableEntry::CallRpc(std::string_view params) {
+  return RpcCall{m_handle, ::nt::CallRpc(m_handle, params)};
+}
+
+inline NT_EntryListener NetworkTableEntry::AddListener(
+    std::function<void(const EntryNotification& event)> callback,
+    unsigned int flags) const {
+  return AddEntryListener(m_handle, callback, flags);
+}
+
+inline void NetworkTableEntry::RemoveListener(NT_EntryListener entry_listener) {
+  RemoveEntryListener(entry_listener);
+}
+
+}  // namespace nt
+
+#endif  // NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INC_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl
deleted file mode 100644
index 5bf1dbf..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableEntry.inl
+++ /dev/null
@@ -1,302 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INL_
-#define NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INL_
-
-#include <memory>
-#include <string>
-#include <vector>
-
-namespace nt {
-
-inline NetworkTableEntry::NetworkTableEntry() : m_handle{0} {}
-
-inline NetworkTableEntry::NetworkTableEntry(NT_Entry handle)
-    : m_handle{handle} {}
-
-inline NT_Entry NetworkTableEntry::GetHandle() const { return m_handle; }
-
-inline bool NetworkTableEntry::Exists() const {
-  return GetEntryType(m_handle) != NT_UNASSIGNED;
-}
-
-inline std::string NetworkTableEntry::GetName() const {
-  return GetEntryName(m_handle);
-}
-
-inline NetworkTableType NetworkTableEntry::GetType() const {
-  return static_cast<NetworkTableType>(GetEntryType(m_handle));
-}
-
-inline unsigned int NetworkTableEntry::GetFlags() const {
-  return GetEntryFlags(m_handle);
-}
-
-inline uint64_t NetworkTableEntry::GetLastChange() const {
-  return GetEntryLastChange(m_handle);
-}
-
-inline EntryInfo NetworkTableEntry::GetInfo() const {
-  return GetEntryInfo(m_handle);
-}
-
-inline std::shared_ptr<Value> NetworkTableEntry::GetValue() const {
-  return GetEntryValue(m_handle);
-}
-
-inline bool NetworkTableEntry::GetBoolean(bool defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_BOOLEAN) return defaultValue;
-  return value->GetBoolean();
-}
-
-inline double NetworkTableEntry::GetDouble(double defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_DOUBLE) return defaultValue;
-  return value->GetDouble();
-}
-
-inline std::string NetworkTableEntry::GetString(StringRef defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_STRING) return defaultValue;
-  return value->GetString();
-}
-
-inline std::string NetworkTableEntry::GetRaw(StringRef defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_RAW) return defaultValue;
-  return value->GetString();
-}
-
-inline std::vector<int> NetworkTableEntry::GetBooleanArray(
-    ArrayRef<int> defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_BOOLEAN_ARRAY) return defaultValue;
-  return value->GetBooleanArray();
-}
-
-inline std::vector<int> NetworkTableEntry::GetBooleanArray(
-    std::initializer_list<int> defaultValue) const {
-  return GetBooleanArray(
-      wpi::makeArrayRef(defaultValue.begin(), defaultValue.end()));
-}
-
-inline std::vector<double> NetworkTableEntry::GetDoubleArray(
-    ArrayRef<double> defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_DOUBLE_ARRAY) return defaultValue;
-  return value->GetDoubleArray();
-}
-
-inline std::vector<double> NetworkTableEntry::GetDoubleArray(
-    std::initializer_list<double> defaultValue) const {
-  return GetDoubleArray(
-      wpi::makeArrayRef(defaultValue.begin(), defaultValue.end()));
-}
-
-inline std::vector<std::string> NetworkTableEntry::GetStringArray(
-    ArrayRef<std::string> defaultValue) const {
-  auto value = GetEntryValue(m_handle);
-  if (!value || value->type() != NT_STRING_ARRAY) return defaultValue;
-  return value->GetStringArray();
-}
-
-inline std::vector<std::string> NetworkTableEntry::GetStringArray(
-    std::initializer_list<std::string> defaultValue) const {
-  return GetStringArray(
-      wpi::makeArrayRef(defaultValue.begin(), defaultValue.end()));
-}
-
-inline bool NetworkTableEntry::SetDefaultValue(std::shared_ptr<Value> value) {
-  return SetDefaultEntryValue(m_handle, value);
-}
-
-inline bool NetworkTableEntry::SetDefaultBoolean(bool defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeBoolean(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetDefaultDouble(double defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeDouble(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetDefaultString(const Twine& defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeString(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetDefaultRaw(StringRef defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeRaw(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetDefaultBooleanArray(
-    ArrayRef<int> defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeBooleanArray(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetDefaultDoubleArray(
-    ArrayRef<double> defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeDoubleArray(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetDefaultStringArray(
-    ArrayRef<std::string> defaultValue) {
-  return SetDefaultEntryValue(m_handle, Value::MakeStringArray(defaultValue));
-}
-
-inline bool NetworkTableEntry::SetValue(std::shared_ptr<Value> value) {
-  return SetEntryValue(m_handle, value);
-}
-
-inline bool NetworkTableEntry::SetBoolean(bool value) {
-  return SetEntryValue(m_handle, Value::MakeBoolean(value));
-}
-
-inline bool NetworkTableEntry::SetDouble(double value) {
-  return SetEntryValue(m_handle, Value::MakeDouble(value));
-}
-
-inline bool NetworkTableEntry::SetString(const Twine& value) {
-  return SetEntryValue(m_handle, Value::MakeString(value));
-}
-
-inline bool NetworkTableEntry::SetRaw(StringRef value) {
-  return SetEntryValue(m_handle, Value::MakeRaw(value));
-}
-
-inline bool NetworkTableEntry::SetBooleanArray(ArrayRef<bool> value) {
-  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline bool NetworkTableEntry::SetBooleanArray(
-    std::initializer_list<bool> value) {
-  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline bool NetworkTableEntry::SetBooleanArray(ArrayRef<int> value) {
-  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline bool NetworkTableEntry::SetBooleanArray(
-    std::initializer_list<int> value) {
-  return SetEntryValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline bool NetworkTableEntry::SetDoubleArray(ArrayRef<double> value) {
-  return SetEntryValue(m_handle, Value::MakeDoubleArray(value));
-}
-
-inline bool NetworkTableEntry::SetDoubleArray(
-    std::initializer_list<double> value) {
-  return SetEntryValue(m_handle, Value::MakeDoubleArray(value));
-}
-
-inline bool NetworkTableEntry::SetStringArray(ArrayRef<std::string> value) {
-  return SetEntryValue(m_handle, Value::MakeStringArray(value));
-}
-
-inline bool NetworkTableEntry::SetStringArray(
-    std::initializer_list<std::string> value) {
-  return SetEntryValue(m_handle, Value::MakeStringArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetValue(std::shared_ptr<Value> value) {
-  SetEntryTypeValue(m_handle, value);
-}
-
-inline void NetworkTableEntry::ForceSetBoolean(bool value) {
-  SetEntryTypeValue(m_handle, Value::MakeBoolean(value));
-}
-
-inline void NetworkTableEntry::ForceSetDouble(double value) {
-  SetEntryTypeValue(m_handle, Value::MakeDouble(value));
-}
-
-inline void NetworkTableEntry::ForceSetString(const Twine& value) {
-  SetEntryTypeValue(m_handle, Value::MakeString(value));
-}
-
-inline void NetworkTableEntry::ForceSetRaw(StringRef value) {
-  SetEntryTypeValue(m_handle, Value::MakeRaw(value));
-}
-
-inline void NetworkTableEntry::ForceSetBooleanArray(ArrayRef<bool> value) {
-  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetBooleanArray(
-    std::initializer_list<bool> value) {
-  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetBooleanArray(ArrayRef<int> value) {
-  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetBooleanArray(
-    std::initializer_list<int> value) {
-  SetEntryTypeValue(m_handle, Value::MakeBooleanArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetDoubleArray(ArrayRef<double> value) {
-  SetEntryTypeValue(m_handle, Value::MakeDoubleArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetDoubleArray(
-    std::initializer_list<double> value) {
-  SetEntryTypeValue(m_handle, Value::MakeDoubleArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetStringArray(
-    ArrayRef<std::string> value) {
-  SetEntryTypeValue(m_handle, Value::MakeStringArray(value));
-}
-
-inline void NetworkTableEntry::ForceSetStringArray(
-    std::initializer_list<std::string> value) {
-  SetEntryTypeValue(m_handle, Value::MakeStringArray(value));
-}
-
-inline void NetworkTableEntry::SetFlags(unsigned int flags) {
-  SetEntryFlags(m_handle, GetFlags() | flags);
-}
-
-inline void NetworkTableEntry::ClearFlags(unsigned int flags) {
-  SetEntryFlags(m_handle, GetFlags() & ~flags);
-}
-
-inline void NetworkTableEntry::SetPersistent() { SetFlags(kPersistent); }
-
-inline void NetworkTableEntry::ClearPersistent() { ClearFlags(kPersistent); }
-
-inline bool NetworkTableEntry::IsPersistent() const {
-  return (GetFlags() & kPersistent) != 0;
-}
-
-inline void NetworkTableEntry::Delete() { DeleteEntry(m_handle); }
-
-inline void NetworkTableEntry::CreateRpc(
-    std::function<void(const RpcAnswer& answer)> callback) {
-  ::nt::CreateRpc(m_handle, StringRef("\0", 1), callback);
-}
-
-inline RpcCall NetworkTableEntry::CallRpc(StringRef params) {
-  return RpcCall{m_handle, ::nt::CallRpc(m_handle, params)};
-}
-
-inline NT_EntryListener NetworkTableEntry::AddListener(
-    std::function<void(const EntryNotification& event)> callback,
-    unsigned int flags) const {
-  return AddEntryListener(m_handle, callback, flags);
-}
-
-inline void NetworkTableEntry::RemoveListener(NT_EntryListener entry_listener) {
-  RemoveEntryListener(entry_listener);
-}
-
-}  // namespace nt
-
-#endif  // NTCORE_NETWORKTABLES_NETWORKTABLEENTRY_INL_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
index 16fc5b2..54b5324 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_H_
 #define NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_H_
@@ -11,12 +8,11 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
 #include "networktables/NetworkTable.h"
 #include "networktables/NetworkTableEntry.h"
@@ -25,10 +21,6 @@
 
 namespace nt {
 
-using wpi::ArrayRef;
-using wpi::StringRef;
-using wpi::Twine;
-
 /**
  * NetworkTables Instance.
  *
@@ -93,7 +85,7 @@
   /**
    * Construct from native handle.
    *
-   * @param handle Native handle
+   * @param inst Native handle
    */
   explicit NetworkTableInstance(NT_Inst inst) noexcept;
 
@@ -138,7 +130,7 @@
    * @param name Key
    * @return Network table entry.
    */
-  NetworkTableEntry GetEntry(const Twine& name);
+  NetworkTableEntry GetEntry(std::string_view name);
 
   /**
    * Get entries starting with the given prefix.
@@ -151,7 +143,7 @@
    * @param types bitmask of types; 0 is treated as a "don't care"
    * @return Array of entries.
    */
-  std::vector<NetworkTableEntry> GetEntries(const Twine& prefix,
+  std::vector<NetworkTableEntry> GetEntries(std::string_view prefix,
                                             unsigned int types);
 
   /**
@@ -165,7 +157,7 @@
    * @param types bitmask of types; 0 is treated as a "don't care"
    * @return Array of entry information.
    */
-  std::vector<EntryInfo> GetEntryInfo(const Twine& prefix,
+  std::vector<EntryInfo> GetEntryInfo(std::string_view prefix,
                                       unsigned int types) const;
 
   /**
@@ -174,7 +166,7 @@
    * @param key the key name
    * @return The network table
    */
-  std::shared_ptr<NetworkTable> GetTable(const Twine& key) const;
+  std::shared_ptr<NetworkTable> GetTable(std::string_view key) const;
 
   /**
    * Deletes ALL keys in ALL subtables (except persistent values).
@@ -196,7 +188,7 @@
    * @return Listener handle
    */
   NT_EntryListener AddEntryListener(
-      const Twine& prefix,
+      std::string_view prefix,
       std::function<void(const EntryNotification& event)> callback,
       unsigned int flags) const;
 
@@ -290,7 +282,7 @@
    *
    * @param name      identity to advertise
    */
-  void SetNetworkIdentity(const Twine& name);
+  void SetNetworkIdentity(std::string_view name);
 
   /**
    * Get the current network mode.
@@ -321,7 +313,7 @@
    *                          address (UTF-8 string, null terminated)
    * @param port              port to communicate over
    */
-  void StartServer(const Twine& persist_filename = "networktables.ini",
+  void StartServer(std::string_view persist_filename = "networktables.ini",
                    const char* listen_address = "",
                    unsigned int port = kDefaultPort);
 
@@ -349,7 +341,8 @@
    *
    * @param servers   array of server name and port pairs
    */
-  void StartClient(ArrayRef<std::pair<StringRef, unsigned int>> servers);
+  void StartClient(
+      wpi::span<const std::pair<std::string_view, unsigned int>> servers);
 
   /**
    * Starts a client using the specified servers and port.  The
@@ -358,7 +351,7 @@
    * @param servers   array of server names
    * @param port      port to communicate over
    */
-  void StartClient(ArrayRef<StringRef> servers,
+  void StartClient(wpi::span<const std::string_view> servers,
                    unsigned int port = kDefaultPort);
 
   /**
@@ -389,7 +382,8 @@
    *
    * @param servers   array of server name and port pairs
    */
-  void SetServer(ArrayRef<std::pair<StringRef, unsigned int>> servers);
+  void SetServer(
+      wpi::span<const std::pair<std::string_view, unsigned int>> servers);
 
   /**
    * Sets server addresses and port for client (without restarting client).
@@ -398,7 +392,8 @@
    * @param servers   array of server names
    * @param port      port to communicate over
    */
-  void SetServer(ArrayRef<StringRef> servers, unsigned int port = kDefaultPort);
+  void SetServer(wpi::span<const std::string_view> servers,
+                 unsigned int port = kDefaultPort);
 
   /**
    * Sets server addresses and port for client (without restarting client).
@@ -469,7 +464,7 @@
    * @param filename  filename
    * @return error string, or nullptr if successful
    */
-  const char* SavePersistent(const Twine& filename) const;
+  const char* SavePersistent(std::string_view filename) const;
 
   /**
    * Load persistent values from a file.  The server automatically does this
@@ -481,7 +476,7 @@
    * @return error string, or nullptr if successful
    */
   const char* LoadPersistent(
-      const Twine& filename,
+      std::string_view filename,
       std::function<void(size_t line, const char* msg)> warn);
 
   /**
@@ -492,7 +487,8 @@
    * @param prefix    save only keys starting with this prefix
    * @return error string, or nullptr if successful
    */
-  const char* SaveEntries(const Twine& filename, const Twine& prefix) const;
+  const char* SaveEntries(std::string_view filename,
+                          std::string_view prefix) const;
 
   /**
    * Load table values from a file.  The file format used is identical to
@@ -504,7 +500,7 @@
    * @return error string, or nullptr if successful
    */
   const char* LoadEntries(
-      const Twine& filename, const Twine& prefix,
+      std::string_view filename, std::string_view prefix,
       std::function<void(size_t line, const char* msg)> warn);
 
   /** @} */
@@ -527,7 +523,7 @@
    * @return Logger handle
    */
   NT_Logger AddLogger(std::function<void(const LogMessage& msg)> func,
-                      unsigned int min_level, unsigned int max_level);
+                      unsigned int minLevel, unsigned int maxLevel);
 
   /**
    * Remove a logger.
@@ -565,11 +561,11 @@
 
  private:
   /* Native handle */
-  NT_Inst m_handle;
+  NT_Inst m_handle{0};
 };
 
 }  // namespace nt
 
-#include "networktables/NetworkTableInstance.inl"
+#include "networktables/NetworkTableInstance.inc"
 
 #endif  // NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc
new file mode 100644
index 0000000..5cb7be0
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inc
@@ -0,0 +1,211 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INC_
+#define NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INC_
+
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include "networktables/NetworkTableInstance.h"
+
+namespace nt {
+
+inline NetworkTableInstance::NetworkTableInstance() noexcept {}
+
+inline NetworkTableInstance::NetworkTableInstance(NT_Inst handle) noexcept
+    : m_handle{handle} {}
+
+inline NetworkTableInstance NetworkTableInstance::GetDefault() {
+  return NetworkTableInstance{GetDefaultInstance()};
+}
+
+inline NetworkTableInstance NetworkTableInstance::Create() {
+  return NetworkTableInstance{CreateInstance()};
+}
+
+inline void NetworkTableInstance::Destroy(NetworkTableInstance inst) {
+  if (inst.m_handle != 0) {
+    DestroyInstance(inst.m_handle);
+  }
+}
+
+inline NT_Inst NetworkTableInstance::GetHandle() const {
+  return m_handle;
+}
+
+inline NetworkTableEntry NetworkTableInstance::GetEntry(std::string_view name) {
+  return NetworkTableEntry{::nt::GetEntry(m_handle, name)};
+}
+
+inline std::vector<NetworkTableEntry> NetworkTableInstance::GetEntries(
+    std::string_view prefix, unsigned int types) {
+  std::vector<NetworkTableEntry> entries;
+  for (auto entry : ::nt::GetEntries(m_handle, prefix, types)) {
+    entries.emplace_back(entry);
+  }
+  return entries;
+}
+
+inline std::vector<EntryInfo> NetworkTableInstance::GetEntryInfo(
+    std::string_view prefix, unsigned int types) const {
+  return ::nt::GetEntryInfo(m_handle, prefix, types);
+}
+
+inline void NetworkTableInstance::DeleteAllEntries() {
+  ::nt::DeleteAllEntries(m_handle);
+}
+
+inline void NetworkTableInstance::RemoveEntryListener(
+    NT_EntryListener entry_listener) {
+  ::nt::RemoveEntryListener(entry_listener);
+}
+
+inline bool NetworkTableInstance::WaitForEntryListenerQueue(double timeout) {
+  return ::nt::WaitForEntryListenerQueue(m_handle, timeout);
+}
+
+inline void NetworkTableInstance::RemoveConnectionListener(
+    NT_ConnectionListener conn_listener) {
+  ::nt::RemoveConnectionListener(conn_listener);
+}
+
+inline bool NetworkTableInstance::WaitForConnectionListenerQueue(
+    double timeout) {
+  return ::nt::WaitForConnectionListenerQueue(m_handle, timeout);
+}
+
+inline bool NetworkTableInstance::WaitForRpcCallQueue(double timeout) {
+  return ::nt::WaitForRpcCallQueue(m_handle, timeout);
+}
+
+inline void NetworkTableInstance::SetNetworkIdentity(std::string_view name) {
+  ::nt::SetNetworkIdentity(m_handle, name);
+}
+
+inline unsigned int NetworkTableInstance::GetNetworkMode() const {
+  return ::nt::GetNetworkMode(m_handle);
+}
+
+inline void NetworkTableInstance::StartLocal() {
+  ::nt::StartLocal(m_handle);
+}
+
+inline void NetworkTableInstance::StopLocal() {
+  ::nt::StopLocal(m_handle);
+}
+
+inline void NetworkTableInstance::StartServer(std::string_view persist_filename,
+                                              const char* listen_address,
+                                              unsigned int port) {
+  ::nt::StartServer(m_handle, persist_filename, listen_address, port);
+}
+
+inline void NetworkTableInstance::StopServer() {
+  ::nt::StopServer(m_handle);
+}
+
+inline void NetworkTableInstance::StartClient() {
+  ::nt::StartClient(m_handle);
+}
+
+inline void NetworkTableInstance::StartClient(const char* server_name,
+                                              unsigned int port) {
+  ::nt::StartClient(m_handle, server_name, port);
+}
+
+inline void NetworkTableInstance::StartClient(
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers) {
+  ::nt::StartClient(m_handle, servers);
+}
+
+inline void NetworkTableInstance::StartClientTeam(unsigned int team,
+                                                  unsigned int port) {
+  ::nt::StartClientTeam(m_handle, team, port);
+}
+
+inline void NetworkTableInstance::StopClient() {
+  ::nt::StopClient(m_handle);
+}
+
+inline void NetworkTableInstance::SetServer(const char* server_name,
+                                            unsigned int port) {
+  ::nt::SetServer(m_handle, server_name, port);
+}
+
+inline void NetworkTableInstance::SetServer(
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers) {
+  ::nt::SetServer(m_handle, servers);
+}
+
+inline void NetworkTableInstance::SetServerTeam(unsigned int team,
+                                                unsigned int port) {
+  ::nt::SetServerTeam(m_handle, team, port);
+}
+
+inline void NetworkTableInstance::StartDSClient(unsigned int port) {
+  ::nt::StartDSClient(m_handle, port);
+}
+
+inline void NetworkTableInstance::StopDSClient() {
+  ::nt::StopDSClient(m_handle);
+}
+
+inline void NetworkTableInstance::SetUpdateRate(double interval) {
+  ::nt::SetUpdateRate(m_handle, interval);
+}
+
+inline void NetworkTableInstance::Flush() const {
+  ::nt::Flush(m_handle);
+}
+
+inline std::vector<ConnectionInfo> NetworkTableInstance::GetConnections()
+    const {
+  return ::nt::GetConnections(m_handle);
+}
+
+inline bool NetworkTableInstance::IsConnected() const {
+  return ::nt::IsConnected(m_handle);
+}
+
+inline const char* NetworkTableInstance::SavePersistent(
+    std::string_view filename) const {
+  return ::nt::SavePersistent(m_handle, filename);
+}
+
+inline const char* NetworkTableInstance::LoadPersistent(
+    std::string_view filename,
+    std::function<void(size_t line, const char* msg)> warn) {
+  return ::nt::LoadPersistent(m_handle, filename, warn);
+}
+
+inline const char* NetworkTableInstance::SaveEntries(
+    std::string_view filename, std::string_view prefix) const {
+  return ::nt::SaveEntries(m_handle, filename, prefix);
+}
+
+inline const char* NetworkTableInstance::LoadEntries(
+    std::string_view filename, std::string_view prefix,
+    std::function<void(size_t line, const char* msg)> warn) {
+  return ::nt::LoadEntries(m_handle, filename, prefix, warn);
+}
+
+inline NT_Logger NetworkTableInstance::AddLogger(
+    std::function<void(const LogMessage& msg)> func, unsigned int min_level,
+    unsigned int max_level) {
+  return ::nt::AddLogger(m_handle, func, min_level, max_level);
+}
+
+inline void NetworkTableInstance::RemoveLogger(NT_Logger logger) {
+  ::nt::RemoveLogger(logger);
+}
+
+inline bool NetworkTableInstance::WaitForLoggerQueue(double timeout) {
+  return ::nt::WaitForLoggerQueue(m_handle, timeout);
+}
+
+}  // namespace nt
+
+#endif  // NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INC_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
deleted file mode 100644
index 5321e83..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableInstance.inl
+++ /dev/null
@@ -1,194 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INL_
-#define NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INL_
-
-#include <utility>
-#include <vector>
-
-namespace nt {
-
-inline NetworkTableInstance::NetworkTableInstance() noexcept : m_handle{0} {}
-
-inline NetworkTableInstance::NetworkTableInstance(NT_Inst handle) noexcept
-    : m_handle{handle} {}
-
-inline NetworkTableInstance NetworkTableInstance::GetDefault() {
-  return NetworkTableInstance{GetDefaultInstance()};
-}
-
-inline NetworkTableInstance NetworkTableInstance::Create() {
-  return NetworkTableInstance{CreateInstance()};
-}
-
-inline void NetworkTableInstance::Destroy(NetworkTableInstance inst) {
-  if (inst.m_handle != 0) DestroyInstance(inst.m_handle);
-}
-
-inline NT_Inst NetworkTableInstance::GetHandle() const { return m_handle; }
-
-inline NetworkTableEntry NetworkTableInstance::GetEntry(const Twine& name) {
-  return NetworkTableEntry{::nt::GetEntry(m_handle, name)};
-}
-
-inline std::vector<NetworkTableEntry> NetworkTableInstance::GetEntries(
-    const Twine& prefix, unsigned int types) {
-  std::vector<NetworkTableEntry> entries;
-  for (auto entry : ::nt::GetEntries(m_handle, prefix, types))
-    entries.emplace_back(entry);
-  return entries;
-}
-
-inline std::vector<EntryInfo> NetworkTableInstance::GetEntryInfo(
-    const Twine& prefix, unsigned int types) const {
-  return ::nt::GetEntryInfo(m_handle, prefix, types);
-}
-
-inline void NetworkTableInstance::DeleteAllEntries() {
-  ::nt::DeleteAllEntries(m_handle);
-}
-
-inline void NetworkTableInstance::RemoveEntryListener(
-    NT_EntryListener entry_listener) {
-  ::nt::RemoveEntryListener(entry_listener);
-}
-
-inline bool NetworkTableInstance::WaitForEntryListenerQueue(double timeout) {
-  return ::nt::WaitForEntryListenerQueue(m_handle, timeout);
-}
-
-inline void NetworkTableInstance::RemoveConnectionListener(
-    NT_ConnectionListener conn_listener) {
-  ::nt::RemoveConnectionListener(conn_listener);
-}
-
-inline bool NetworkTableInstance::WaitForConnectionListenerQueue(
-    double timeout) {
-  return ::nt::WaitForConnectionListenerQueue(m_handle, timeout);
-}
-
-inline bool NetworkTableInstance::WaitForRpcCallQueue(double timeout) {
-  return ::nt::WaitForRpcCallQueue(m_handle, timeout);
-}
-
-inline void NetworkTableInstance::SetNetworkIdentity(const Twine& name) {
-  ::nt::SetNetworkIdentity(m_handle, name);
-}
-
-inline unsigned int NetworkTableInstance::GetNetworkMode() const {
-  return ::nt::GetNetworkMode(m_handle);
-}
-
-inline void NetworkTableInstance::StartLocal() { ::nt::StartLocal(m_handle); }
-
-inline void NetworkTableInstance::StopLocal() { ::nt::StopLocal(m_handle); }
-
-inline void NetworkTableInstance::StartServer(const Twine& persist_filename,
-                                              const char* listen_address,
-                                              unsigned int port) {
-  ::nt::StartServer(m_handle, persist_filename, listen_address, port);
-}
-
-inline void NetworkTableInstance::StopServer() { ::nt::StopServer(m_handle); }
-
-inline void NetworkTableInstance::StartClient() { ::nt::StartClient(m_handle); }
-
-inline void NetworkTableInstance::StartClient(const char* server_name,
-                                              unsigned int port) {
-  ::nt::StartClient(m_handle, server_name, port);
-}
-
-inline void NetworkTableInstance::StartClient(
-    ArrayRef<std::pair<StringRef, unsigned int>> servers) {
-  ::nt::StartClient(m_handle, servers);
-}
-
-inline void NetworkTableInstance::StartClientTeam(unsigned int team,
-                                                  unsigned int port) {
-  ::nt::StartClientTeam(m_handle, team, port);
-}
-
-inline void NetworkTableInstance::StopClient() { ::nt::StopClient(m_handle); }
-
-inline void NetworkTableInstance::SetServer(const char* server_name,
-                                            unsigned int port) {
-  ::nt::SetServer(m_handle, server_name, port);
-}
-
-inline void NetworkTableInstance::SetServer(
-    ArrayRef<std::pair<StringRef, unsigned int>> servers) {
-  ::nt::SetServer(m_handle, servers);
-}
-
-inline void NetworkTableInstance::SetServerTeam(unsigned int team,
-                                                unsigned int port) {
-  ::nt::SetServerTeam(m_handle, team, port);
-}
-
-inline void NetworkTableInstance::StartDSClient(unsigned int port) {
-  ::nt::StartDSClient(m_handle, port);
-}
-
-inline void NetworkTableInstance::StopDSClient() {
-  ::nt::StopDSClient(m_handle);
-}
-
-inline void NetworkTableInstance::SetUpdateRate(double interval) {
-  ::nt::SetUpdateRate(m_handle, interval);
-}
-
-inline void NetworkTableInstance::Flush() const { ::nt::Flush(m_handle); }
-
-inline std::vector<ConnectionInfo> NetworkTableInstance::GetConnections()
-    const {
-  return ::nt::GetConnections(m_handle);
-}
-
-inline bool NetworkTableInstance::IsConnected() const {
-  return ::nt::IsConnected(m_handle);
-}
-
-inline const char* NetworkTableInstance::SavePersistent(
-    const Twine& filename) const {
-  return ::nt::SavePersistent(m_handle, filename);
-}
-
-inline const char* NetworkTableInstance::LoadPersistent(
-    const Twine& filename,
-    std::function<void(size_t line, const char* msg)> warn) {
-  return ::nt::LoadPersistent(m_handle, filename, warn);
-}
-
-inline const char* NetworkTableInstance::SaveEntries(
-    const Twine& filename, const Twine& prefix) const {
-  return ::nt::SaveEntries(m_handle, filename, prefix);
-}
-
-inline const char* NetworkTableInstance::LoadEntries(
-    const Twine& filename, const Twine& prefix,
-    std::function<void(size_t line, const char* msg)> warn) {
-  return ::nt::LoadEntries(m_handle, filename, prefix, warn);
-}
-
-inline NT_Logger NetworkTableInstance::AddLogger(
-    std::function<void(const LogMessage& msg)> func, unsigned int min_level,
-    unsigned int max_level) {
-  return ::nt::AddLogger(m_handle, func, min_level, max_level);
-}
-
-inline void NetworkTableInstance::RemoveLogger(NT_Logger logger) {
-  ::nt::RemoveLogger(logger);
-}
-
-inline bool NetworkTableInstance::WaitForLoggerQueue(double timeout) {
-  return ::nt::WaitForLoggerQueue(m_handle, timeout);
-}
-
-}  // namespace nt
-
-#endif  // NTCORE_NETWORKTABLES_NETWORKTABLEINSTANCE_INL_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h
index 7ac3d9a..d7681f2 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableType.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_NETWORKTABLETYPE_H_
 #define NTCORE_NETWORKTABLES_NETWORKTABLETYPE_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableValue.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableValue.h
index 1b8aabe..fcb9cb5 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableValue.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/NetworkTableValue.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_NETWORKTABLEVALUE_H_
 #define NTCORE_NETWORKTABLES_NETWORKTABLEVALUE_H_
@@ -14,22 +11,17 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <type_traits>
 #include <utility>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
 #include "ntcore_c.h"
 
 namespace nt {
 
-using wpi::ArrayRef;
-using wpi::StringRef;
-using wpi::Twine;
-
 /**
  * A network table entry value.
  * @ingroup ntcore_cpp_api
@@ -170,7 +162,7 @@
    *
    * @return The string value.
    */
-  StringRef GetString() const {
+  std::string_view GetString() const {
     assert(m_val.type == NT_STRING);
     return m_string;
   }
@@ -180,7 +172,7 @@
    *
    * @return The raw value.
    */
-  StringRef GetRaw() const {
+  std::string_view GetRaw() const {
     assert(m_val.type == NT_RAW);
     return m_string;
   }
@@ -190,7 +182,7 @@
    *
    * @return The rpc definition value.
    */
-  StringRef GetRpc() const {
+  std::string_view GetRpc() const {
     assert(m_val.type == NT_RPC);
     return m_string;
   }
@@ -200,10 +192,9 @@
    *
    * @return The boolean array value.
    */
-  ArrayRef<int> GetBooleanArray() const {
+  wpi::span<const int> GetBooleanArray() const {
     assert(m_val.type == NT_BOOLEAN_ARRAY);
-    return ArrayRef<int>(m_val.data.arr_boolean.arr,
-                         m_val.data.arr_boolean.size);
+    return {m_val.data.arr_boolean.arr, m_val.data.arr_boolean.size};
   }
 
   /**
@@ -211,10 +202,9 @@
    *
    * @return The double array value.
    */
-  ArrayRef<double> GetDoubleArray() const {
+  wpi::span<const double> GetDoubleArray() const {
     assert(m_val.type == NT_DOUBLE_ARRAY);
-    return ArrayRef<double>(m_val.data.arr_double.arr,
-                            m_val.data.arr_double.size);
+    return {m_val.data.arr_double.arr, m_val.data.arr_double.size};
   }
 
   /**
@@ -222,7 +212,7 @@
    *
    * @return The string array value.
    */
-  ArrayRef<std::string> GetStringArray() const {
+  wpi::span<const std::string> GetStringArray() const {
     assert(m_val.type == NT_STRING_ARRAY);
     return m_string_array;
   }
@@ -270,10 +260,10 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeString(const Twine& value,
+  static std::shared_ptr<Value> MakeString(std::string_view value,
                                            uint64_t time = 0) {
     auto val = std::make_shared<Value>(NT_STRING, time, private_init());
-    val->m_string = value.str();
+    val->m_string = value;
     val->m_val.data.v_string.str = const_cast<char*>(val->m_string.c_str());
     val->m_val.data.v_string.len = val->m_string.size();
     return val;
@@ -291,7 +281,7 @@
             typename std::enable_if<std::is_same<T, std::string>::value>::type>
   static std::shared_ptr<Value> MakeString(T&& value, uint64_t time = 0) {
     auto val = std::make_shared<Value>(NT_STRING, time, private_init());
-    val->m_string = std::move(value);
+    val->m_string = std::forward<T>(value);
     val->m_val.data.v_string.str = const_cast<char*>(val->m_string.c_str());
     val->m_val.data.v_string.len = val->m_string.size();
     return val;
@@ -305,7 +295,8 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeRaw(StringRef value, uint64_t time = 0) {
+  static std::shared_ptr<Value> MakeRaw(std::string_view value,
+                                        uint64_t time = 0) {
     auto val = std::make_shared<Value>(NT_RAW, time, private_init());
     val->m_string = value;
     val->m_val.data.v_raw.str = const_cast<char*>(val->m_string.c_str());
@@ -325,7 +316,7 @@
             typename std::enable_if<std::is_same<T, std::string>::value>::type>
   static std::shared_ptr<Value> MakeRaw(T&& value, uint64_t time = 0) {
     auto val = std::make_shared<Value>(NT_RAW, time, private_init());
-    val->m_string = std::move(value);
+    val->m_string = std::forward<T>(value);
     val->m_val.data.v_raw.str = const_cast<char*>(val->m_string.c_str());
     val->m_val.data.v_raw.len = val->m_string.size();
     return val;
@@ -339,7 +330,8 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeRpc(StringRef value, uint64_t time = 0) {
+  static std::shared_ptr<Value> MakeRpc(std::string_view value,
+                                        uint64_t time = 0) {
     auto val = std::make_shared<Value>(NT_RPC, time, private_init());
     val->m_string = value;
     val->m_val.data.v_raw.str = const_cast<char*>(val->m_string.c_str());
@@ -358,7 +350,7 @@
   template <typename T>
   static std::shared_ptr<Value> MakeRpc(T&& value, uint64_t time = 0) {
     auto val = std::make_shared<Value>(NT_RPC, time, private_init());
-    val->m_string = std::move(value);
+    val->m_string = std::forward<T>(value);
     val->m_val.data.v_raw.str = const_cast<char*>(val->m_string.c_str());
     val->m_val.data.v_raw.len = val->m_string.size();
     return val;
@@ -372,7 +364,7 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeBooleanArray(ArrayRef<bool> value,
+  static std::shared_ptr<Value> MakeBooleanArray(wpi::span<const bool> value,
                                                  uint64_t time = 0);
 
   /**
@@ -385,8 +377,7 @@
    */
   static std::shared_ptr<Value> MakeBooleanArray(
       std::initializer_list<bool> value, uint64_t time = 0) {
-    return MakeBooleanArray(wpi::makeArrayRef(value.begin(), value.end()),
-                            time);
+    return MakeBooleanArray(wpi::span(value.begin(), value.end()), time);
   }
 
   /**
@@ -397,7 +388,7 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeBooleanArray(ArrayRef<int> value,
+  static std::shared_ptr<Value> MakeBooleanArray(wpi::span<const int> value,
                                                  uint64_t time = 0);
 
   /**
@@ -410,8 +401,7 @@
    */
   static std::shared_ptr<Value> MakeBooleanArray(
       std::initializer_list<int> value, uint64_t time = 0) {
-    return MakeBooleanArray(wpi::makeArrayRef(value.begin(), value.end()),
-                            time);
+    return MakeBooleanArray(wpi::span(value.begin(), value.end()), time);
   }
 
   /**
@@ -422,7 +412,7 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeDoubleArray(ArrayRef<double> value,
+  static std::shared_ptr<Value> MakeDoubleArray(wpi::span<const double> value,
                                                 uint64_t time = 0);
 
   /**
@@ -435,7 +425,7 @@
    */
   static std::shared_ptr<Value> MakeDoubleArray(
       std::initializer_list<double> value, uint64_t time = 0) {
-    return MakeDoubleArray(wpi::makeArrayRef(value.begin(), value.end()), time);
+    return MakeDoubleArray(wpi::span(value.begin(), value.end()), time);
   }
 
   /**
@@ -446,8 +436,8 @@
    *             time)
    * @return The entry value
    */
-  static std::shared_ptr<Value> MakeStringArray(ArrayRef<std::string> value,
-                                                uint64_t time = 0);
+  static std::shared_ptr<Value> MakeStringArray(
+      wpi::span<const std::string> value, uint64_t time = 0);
 
   /**
    * Creates a string array entry value.
@@ -459,7 +449,7 @@
    */
   static std::shared_ptr<Value> MakeStringArray(
       std::initializer_list<std::string> value, uint64_t time = 0) {
-    return MakeStringArray(wpi::makeArrayRef(value.begin(), value.end()), time);
+    return MakeStringArray(wpi::span(value.begin(), value.end()), time);
   }
 
   /**
@@ -496,7 +486,7 @@
  * NetworkTable Value alias for similarity with Java.
  * @ingroup ntcore_cpp_api
  */
-typedef Value NetworkTableValue;
+using NetworkTableValue = Value;
 
 }  // namespace nt
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.h
index fc2e0bf..9c6e9f9 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_RPCCALL_H_
 #define NTCORE_NETWORKTABLES_RPCCALL_H_
@@ -26,7 +23,7 @@
   /**
    * Construct invalid instance.
    */
-  RpcCall() : m_entry(0), m_call(0) {}
+  RpcCall() = default;
 
   /**
    * Construct from native handles.
@@ -98,12 +95,12 @@
   }
 
  private:
-  NT_Entry m_entry;
-  NT_RpcCall m_call;
+  NT_Entry m_entry{0};
+  NT_RpcCall m_call{0};
 };
 
 }  // namespace nt
 
-#include "networktables/RpcCall.inl"
+#include "networktables/RpcCall.inc"
 
 #endif  // NTCORE_NETWORKTABLES_RPCCALL_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.inc b/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.inc
new file mode 100644
index 0000000..5e25b04
--- /dev/null
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.inc
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef NTCORE_NETWORKTABLES_RPCCALL_INC_
+#define NTCORE_NETWORKTABLES_RPCCALL_INC_
+
+#include <string>
+#include <utility>
+
+#include "networktables/RpcCall.h"
+#include "ntcore_cpp.h"
+
+namespace nt {
+
+inline RpcCall::RpcCall(RpcCall&& other) noexcept : RpcCall() {
+  swap(*this, other);
+}
+
+inline RpcCall::~RpcCall() {
+  // automatically cancel result if user didn't request it
+  if (m_call != 0) {
+    CancelResult();
+  }
+}
+
+inline bool RpcCall::GetResult(std::string* result) {
+  if (GetRpcResult(m_entry, m_call, result)) {
+    m_call = 0;
+    return true;
+  }
+  return false;
+}
+
+inline bool RpcCall::GetResult(std::string* result, double timeout,
+                               bool* timed_out) {
+  if (GetRpcResult(m_entry, m_call, result, timeout, timed_out)) {
+    m_call = 0;
+    return true;
+  }
+  return false;
+}
+
+inline void RpcCall::CancelResult() {
+  CancelRpcResult(m_entry, m_call);
+  m_call = 0;
+}
+
+}  // namespace nt
+
+#endif  // NTCORE_NETWORKTABLES_RPCCALL_INC_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.inl b/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.inl
deleted file mode 100644
index faf305d..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/RpcCall.inl
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef NTCORE_NETWORKTABLES_RPCCALL_INL_
-#define NTCORE_NETWORKTABLES_RPCCALL_INL_
-
-#include <string>
-#include <utility>
-
-#include "ntcore_cpp.h"
-
-namespace nt {
-
-inline RpcCall::RpcCall(RpcCall&& other) noexcept : RpcCall() {
-  swap(*this, other);
-}
-
-inline RpcCall::~RpcCall() {
-  // automatically cancel result if user didn't request it
-  if (m_call != 0) CancelResult();
-}
-
-inline bool RpcCall::GetResult(std::string* result) {
-  if (GetRpcResult(m_entry, m_call, result)) {
-    m_call = 0;
-    return true;
-  }
-  return false;
-}
-
-inline bool RpcCall::GetResult(std::string* result, double timeout,
-                               bool* timed_out) {
-  if (GetRpcResult(m_entry, m_call, result, timeout, timed_out)) {
-    m_call = 0;
-    return true;
-  }
-  return false;
-}
-
-inline void RpcCall::CancelResult() {
-  CancelRpcResult(m_entry, m_call);
-  m_call = 0;
-}
-
-}  // namespace nt
-
-#endif  // NTCORE_NETWORKTABLES_RPCCALL_INL_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableEntryListener.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableEntryListener.h
index c4552678..180234f 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableEntryListener.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableEntryListener.h
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_TABLEENTRYLISTENER_H_
 #define NTCORE_NETWORKTABLES_TABLEENTRYLISTENER_H_
 
 #include <functional>
 #include <memory>
-
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace nt {
 
@@ -19,8 +15,6 @@
 class NetworkTableEntry;
 class Value;
 
-using wpi::StringRef;
-
 /**
  * A listener that listens to changes in values in a NetworkTable.
  *
@@ -35,10 +29,9 @@
  *
  * @ingroup ntcore_cpp_api
  */
-typedef std::function<void(NetworkTable* table, StringRef name,
-                           NetworkTableEntry entry,
-                           std::shared_ptr<Value> value, int flags)>
-    TableEntryListener;
+using TableEntryListener = std::function<void(
+    NetworkTable* table, std::string_view name, NetworkTableEntry entry,
+    std::shared_ptr<Value> value, int flags)>;
 
 }  // namespace nt
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableListener.h b/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableListener.h
index 9940bad..cc1113e 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableListener.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/networktables/TableListener.h
@@ -1,24 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NETWORKTABLES_TABLELISTENER_H_
 #define NTCORE_NETWORKTABLES_TABLELISTENER_H_
 
 #include <functional>
 #include <memory>
-
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace nt {
 
 class NetworkTable;
 
-using wpi::StringRef;
-
 /**
  * A listener that listens to new sub-tables in a NetworkTable.
  *
@@ -30,9 +24,9 @@
  *
  * @ingroup ntcore_cpp_api
  */
-typedef std::function<void(NetworkTable* parent, StringRef name,
-                           std::shared_ptr<NetworkTable> table)>
-    TableListener;
+using TableListener =
+    std::function<void(NetworkTable* parent, std::string_view name,
+                       std::shared_ptr<NetworkTable> table)>;
 
 }  // namespace nt
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/ntcore.h b/third_party/allwpilib/ntcore/src/main/native/include/ntcore.h
index ff0511a..5cdd473 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/ntcore.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/ntcore.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NTCORE_H_
 #define NTCORE_NTCORE_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h
index 980752f..3ee6d51 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_c.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NTCORE_C_H_
 #define NTCORE_NTCORE_C_H_
@@ -16,8 +13,6 @@
 #include <stddef.h>
 #endif
 
-#include <wpi/deprecated.h>
-
 #ifdef __cplusplus
 extern "C" {
 #endif
@@ -261,7 +256,7 @@
   unsigned int level;
 
   /** The filename of the source file that generated the message. */
-  const char* filename;
+  char* filename;
 
   /** The line number in the source file that generated the message. */
   unsigned int line;
@@ -330,11 +325,13 @@
  * filtered by string prefix and entry type to only return a subset of all
  * entries.
  *
- * @param prefix        entry name required prefix; only entries whose name
- *                      starts with this string are returned
- * @param prefix_len    length of prefix in bytes
- * @param types         bitmask of NT_Type values; 0 is treated specially
- *                      as a "don't care"
+ * @param inst       NetworkTable instance
+ * @param prefix     entry name required prefix; only entries whose name starts
+ *                   with this string are returned
+ * @param prefix_len length of prefix in bytes
+ * @param types      bitmask of NT_Type values; 0 is treated specially as a
+ *                   "don't care"
+ * @param count      stores number of entry handles returned
  * @return Array of entry handles.
  */
 NT_Entry* NT_GetEntries(NT_Inst inst, const char* prefix, size_t prefix_len,
@@ -575,6 +572,7 @@
  *
  * @param poller            poller handle
  * @param prefix            UTF-8 string prefix
+ * @param prefix_len        Length of UTF-8 string prefix
  * @param flags             NT_NotifyKind bitmask
  * @return Listener handle
  */
@@ -588,7 +586,7 @@
  * The caller is responsible for calling NT_PollEntryListener() to poll.
  *
  * @param poller            poller handle
- * @param prefix            UTF-8 string prefix
+ * @param entry             entry handle
  * @param flags             NT_NotifyKind bitmask
  * @return Listener handle
  */
@@ -1597,21 +1595,21 @@
 /**
  * Frees an array of chars.
  *
- * @param v_boolean  pointer to the char array to free
+ * @param v_char pointer to the char array to free
  */
 void NT_FreeCharArray(char* v_char);
 
 /**
  * Frees an array of doubles.
  *
- * @param v_boolean  pointer to the double array to free
+ * @param v_double pointer to the double array to free
  */
 void NT_FreeDoubleArray(double* v_double);
 
 /**
  * Frees an array of booleans.
  *
- * @param v_boolean  pointer to the boolean array to free
+ * @param v_boolean pointer to the boolean array to free
  */
 void NT_FreeBooleanArray(NT_Bool* v_boolean);
 
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h
index af1ea12..2d91fe5 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_cpp.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NTCORE_CPP_H_
 #define NTCORE_NTCORE_CPP_H_
@@ -14,14 +11,12 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <thread>
 #include <utility>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-#include <wpi/deprecated.h>
+#include <wpi/span.h>
 
 #include "networktables/NetworkTableValue.h"
 
@@ -36,10 +31,6 @@
  * @{
  */
 
-using wpi::ArrayRef;
-using wpi::StringRef;
-using wpi::Twine;
-
 /** NetworkTables Entry Information */
 struct EntryInfo {
   /** Entry handle */
@@ -106,8 +97,8 @@
 /** NetworkTables RPC Version 1 Definition Parameter */
 struct RpcParamDef {
   RpcParamDef() = default;
-  RpcParamDef(StringRef name_, std::shared_ptr<Value> def_value_)
-      : name(name_), def_value(def_value_) {}
+  RpcParamDef(std::string_view name_, std::shared_ptr<Value> def_value_)
+      : name(name_), def_value(std::move(def_value_)) {}
 
   std::string name;
   std::shared_ptr<Value> def_value;
@@ -116,7 +107,8 @@
 /** NetworkTables RPC Version 1 Definition Result */
 struct RpcResultDef {
   RpcResultDef() = default;
-  RpcResultDef(StringRef name_, NT_Type type_) : name(name_), type(type_) {}
+  RpcResultDef(std::string_view name_, NT_Type type_)
+      : name(name_), type(type_) {}
 
   std::string name;
   NT_Type type;
@@ -133,16 +125,20 @@
 /** NetworkTables Remote Procedure Call (Server Side) */
 class RpcAnswer {
  public:
-  RpcAnswer() : entry(0), call(0) {}
-  RpcAnswer(NT_Entry entry_, NT_RpcCall call_, StringRef name_,
-            StringRef params_, const ConnectionInfo& conn_)
-      : entry(entry_), call(call_), name(name_), params(params_), conn(conn_) {}
+  RpcAnswer() = default;
+  RpcAnswer(NT_Entry entry_, NT_RpcCall call_, std::string_view name_,
+            std::string_view params_, ConnectionInfo conn_)
+      : entry(entry_),
+        call(call_),
+        name(name_),
+        params(params_),
+        conn(std::move(conn_)) {}
 
   /** Entry handle. */
-  NT_Entry entry;
+  NT_Entry entry{0};
 
   /** Call handle. */
-  mutable NT_RpcCall call;
+  mutable NT_RpcCall call{0};
 
   /** Entry name. */
   std::string name;
@@ -164,7 +160,7 @@
    * @param result  result raw data that will be provided to remote caller
    * @return True if posting the response is valid, otherwise false
    */
-  bool PostResponse(StringRef result) const;
+  bool PostResponse(std::string_view result) const;
 
   friend void swap(RpcAnswer& first, RpcAnswer& second) {
     using std::swap;
@@ -179,21 +175,21 @@
 /** NetworkTables Entry Notification */
 class EntryNotification {
  public:
-  EntryNotification() : listener(0), entry(0), flags(0) {}
+  EntryNotification() = default;
   EntryNotification(NT_EntryListener listener_, NT_Entry entry_,
-                    StringRef name_, std::shared_ptr<Value> value_,
+                    std::string_view name_, std::shared_ptr<Value> value_,
                     unsigned int flags_)
       : listener(listener_),
         entry(entry_),
         name(name_),
-        value(value_),
+        value(std::move(value_)),
         flags(flags_) {}
 
   /** Listener that was triggered. */
-  NT_EntryListener listener;
+  NT_EntryListener listener{0};
 
   /** Entry handle. */
-  NT_Entry entry;
+  NT_Entry entry{0};
 
   /** Entry name. */
   std::string name;
@@ -205,7 +201,7 @@
    * Update flags.  For example, NT_NOTIFY_NEW if the key did not previously
    * exist.
    */
-  unsigned int flags;
+  unsigned int flags{0};
 
   friend void swap(EntryNotification& first, EntryNotification& second) {
     using std::swap;
@@ -220,13 +216,13 @@
 /** NetworkTables Connection Notification */
 class ConnectionNotification {
  public:
-  ConnectionNotification() : listener(0), connected(false) {}
+  ConnectionNotification() = default;
   ConnectionNotification(NT_ConnectionListener listener_, bool connected_,
-                         const ConnectionInfo& conn_)
-      : listener(listener_), connected(connected_), conn(conn_) {}
+                         ConnectionInfo conn_)
+      : listener(listener_), connected(connected_), conn(std::move(conn_)) {}
 
   /** Listener that was triggered. */
-  NT_ConnectionListener listener;
+  NT_ConnectionListener listener{0};
 
   /** True if event is due to connection being established. */
   bool connected = false;
@@ -246,9 +242,9 @@
 /** NetworkTables log message. */
 class LogMessage {
  public:
-  LogMessage() : logger(0), level(0), filename(""), line(0) {}
-  LogMessage(NT_Logger logger_, unsigned int level_, const char* filename_,
-             unsigned int line_, StringRef message_)
+  LogMessage() = default;
+  LogMessage(NT_Logger logger_, unsigned int level_, std::string_view filename_,
+             unsigned int line_, std::string_view message_)
       : logger(logger_),
         level(level_),
         filename(filename_),
@@ -256,16 +252,16 @@
         message(message_) {}
 
   /** The logger that generated the message. */
-  NT_Logger logger;
+  NT_Logger logger{0};
 
   /** Log level of the message.  See NT_LogLevel. */
-  unsigned int level;
+  unsigned int level{0};
 
   /** The filename of the source file that generated the message. */
-  const char* filename;
+  std::string filename;
 
   /** The line number in the source file that generated the message. */
-  unsigned int line;
+  unsigned int line{0};
 
   /** The message. */
   std::string message;
@@ -330,7 +326,7 @@
  * @param name      entry name (UTF-8 string)
  * @return entry handle
  */
-NT_Entry GetEntry(NT_Inst inst, const Twine& name);
+NT_Entry GetEntry(NT_Inst inst, std::string_view name);
 
 /**
  * Get Entry Handles.
@@ -346,7 +342,7 @@
  *                      as a "don't care"
  * @return Array of entry handles.
  */
-std::vector<NT_Entry> GetEntries(NT_Inst inst, const Twine& prefix,
+std::vector<NT_Entry> GetEntries(NT_Inst inst, std::string_view prefix,
                                  unsigned int types);
 
 /**
@@ -381,18 +377,6 @@
  * Returns copy of current entry value.
  * Note that one of the type options is "unassigned".
  *
- * @param name      entry name (UTF-8 string)
- * @return entry value
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-std::shared_ptr<Value> GetEntryValue(StringRef name);
-
-/**
- * Get Entry Value.
- *
- * Returns copy of current entry value.
- * Note that one of the type options is "unassigned".
- *
  * @param entry     entry handle
  * @return entry value
  */
@@ -405,20 +389,6 @@
  * Otherwise, sets passed in value, and returns set value.
  * Note that one of the type options is "unassigned".
  *
- * @param name      entry name (UTF-8 string)
- * @param value     value to be set if name does not exist
- * @return False on error (value not set), True on success
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-bool SetDefaultEntryValue(StringRef name, std::shared_ptr<Value> value);
-
-/**
- * Set Default Entry Value
- *
- * Returns copy of current entry value if it exists.
- * Otherwise, sets passed in value, and returns set value.
- * Note that one of the type options is "unassigned".
- *
  * @param entry     entry handle
  * @param value     value to be set if name does not exist
  * @return False on error (value not set), True on success
@@ -431,19 +401,6 @@
  * Sets new entry value.  If type of new value differs from the type of the
  * currently stored entry, returns error and does not update value.
  *
- * @param name      entry name (UTF-8 string)
- * @param value     new entry value
- * @return False on error (type mismatch), True on success
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-bool SetEntryValue(StringRef name, std::shared_ptr<Value> value);
-
-/**
- * Set Entry Value.
- *
- * Sets new entry value.  If type of new value differs from the type of the
- * currently stored entry, returns error and does not update value.
- *
  * @param entry     entry handle
  * @param value     new entry value
  * @return False on error (type mismatch), True on success
@@ -460,22 +417,6 @@
  * This is NOT the preferred method to update a value; generally
  * SetEntryValue() should be used instead, with appropriate error handling.
  *
- * @param name      entry name (UTF-8 string)
- * @param value     new entry value
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-void SetEntryTypeValue(StringRef name, std::shared_ptr<Value> value);
-
-/**
- * Set Entry Type and Value.
- *
- * Sets new entry value.  If type of new value differs from the type of the
- * currently stored entry, the currently stored entry type is overridden
- * (generally this will generate an Entry Assignment message).
- *
- * This is NOT the preferred method to update a value; generally
- * SetEntryValue() should be used instead, with appropriate error handling.
- *
  * @param entry     entry handle
  * @param value     new entry value
  */
@@ -484,15 +425,6 @@
 /**
  * Set Entry Flags.
  *
- * @param name      entry name (UTF-8 string)
- * @param flags     flags value (bitmask of NT_EntryFlags)
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-void SetEntryFlags(StringRef name, unsigned int flags);
-
-/**
- * Set Entry Flags.
- *
  * @param entry     entry handle
  * @param flags     flags value (bitmask of NT_EntryFlags)
  */
@@ -501,15 +433,6 @@
 /**
  * Get Entry Flags.
  *
- * @param name      entry name (UTF-8 string)
- * @return Flags value (bitmask of NT_EntryFlags)
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-unsigned int GetEntryFlags(StringRef name);
-
-/**
- * Get Entry Flags.
- *
  * @param entry     entry handle
  * @return Flags value (bitmask of NT_EntryFlags)
  */
@@ -526,22 +449,6 @@
  * of direct remote connection(s), but this is not sufficient to determine
  * if all nodes in the network are version 3.0 or newer.
  *
- * @param name      entry name (UTF-8 string)
- */
-WPI_DEPRECATED("use NT_Entry function instead")
-void DeleteEntry(StringRef name);
-
-/**
- * Delete Entry.
- *
- * Deletes an entry.  This is a new feature in version 3.0 of the protocol,
- * so this may not have an effect if any other node in the network is not
- * version 3.0 or newer.
- *
- * Note: GetConnections() can be used to determine the protocol version
- * of direct remote connection(s), but this is not sufficient to determine
- * if all nodes in the network are version 3.0 or newer.
- *
  * @param entry     entry handle
  */
 void DeleteEntry(NT_Entry entry);
@@ -556,12 +463,6 @@
  * Note: GetConnections() can be used to determine the protocol version
  * of direct remote connection(s), but this is not sufficient to determine
  * if all nodes in the network are version 3.0 or newer.
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void DeleteAllEntries();
-
-/**
- * @copydoc DeleteAllEntries()
  *
  * @param inst      instance handle
  */
@@ -575,21 +476,14 @@
  * filtered by string prefix and entry type to only return a subset of all
  * entries.
  *
+ * @param inst    instance handle
  * @param prefix        entry name required prefix; only entries whose name
  *                      starts with this string are returned
  * @param types         bitmask of NT_Type values; 0 is treated specially
  *                      as a "don't care"
  * @return Array of entry information.
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-std::vector<EntryInfo> GetEntryInfo(StringRef prefix, unsigned int types);
-
-/**
- * @copydoc GetEntryInfo(StringRef, unsigned int)
- *
- * @param inst    instance handle
- */
-std::vector<EntryInfo> GetEntryInfo(NT_Inst inst, const Twine& prefix,
+std::vector<EntryInfo> GetEntryInfo(NT_Inst inst, std::string_view prefix,
                                     unsigned int types);
 
 /**
@@ -621,30 +515,21 @@
  * @param flags           update flags; for example, NT_NOTIFY_NEW if the key
  *                        did not previously exist
  */
-typedef std::function<void(NT_EntryListener entry_listener, StringRef name,
-                           std::shared_ptr<Value> value, unsigned int flags)>
-    EntryListenerCallback;
+using EntryListenerCallback =
+    std::function<void(NT_EntryListener entry_listener, std::string_view name,
+                       std::shared_ptr<Value> value, unsigned int flags)>;
 
 /**
  * Add a listener for all entries starting with a certain prefix.
  *
+ * @param inst              instance handle
  * @param prefix            UTF-8 string prefix
  * @param callback          listener to add
  * @param flags             NotifyKind bitmask
  * @return Listener handle
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-NT_EntryListener AddEntryListener(StringRef prefix,
-                                  EntryListenerCallback callback,
-                                  unsigned int flags);
-
-/**
- * @copydoc AddEntryListener(StringRef, EntryListenerCallback, unsigned int)
- *
- * @param inst              instance handle
- */
 NT_EntryListener AddEntryListener(
-    NT_Inst inst, const Twine& prefix,
+    NT_Inst inst, std::string_view prefix,
     std::function<void(const EntryNotification& event)> callback,
     unsigned int flags);
 
@@ -692,7 +577,7 @@
  * @return Listener handle
  */
 NT_EntryListener AddPolledEntryListener(NT_EntryListenerPoller poller,
-                                        const Twine& prefix,
+                                        std::string_view prefix,
                                         unsigned int flags);
 
 /**
@@ -700,7 +585,7 @@
  * The caller is responsible for calling PollEntryListener() to poll.
  *
  * @param poller            poller handle
- * @param prefix            UTF-8 string prefix
+ * @param entry             entry handle
  * @param flags             NotifyKind bitmask
  * @return Listener handle
  */
@@ -780,26 +665,17 @@
  * @param connected       true if event is due to connection being established
  * @param conn            connection info
  */
-typedef std::function<void(NT_ConnectionListener conn_listener, bool connected,
-                           const ConnectionInfo& conn)>
-    ConnectionListenerCallback;
+using ConnectionListenerCallback =
+    std::function<void(NT_ConnectionListener, bool, const ConnectionInfo&)>;
 
 /**
  * Add a connection listener.
  *
+ * @param inst              instance handle
  * @param callback          listener to add
  * @param immediate_notify  notify listener of all existing connections
  * @return Listener handle
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-NT_ConnectionListener AddConnectionListener(ConnectionListenerCallback callback,
-                                            bool immediate_notify);
-
-/**
- * @copydoc AddConnectionListener(ConnectionListenerCallback, bool)
- *
- * @param inst              instance handle
- */
 NT_ConnectionListener AddConnectionListener(
     NT_Inst inst,
     std::function<void(const ConnectionNotification& event)> callback,
@@ -910,7 +786,7 @@
  * @param callback  callback function; note the callback function must call
  *                  PostRpcResponse() to provide a response to the call
  */
-void CreateRpc(NT_Entry entry, StringRef def,
+void CreateRpc(NT_Entry entry, std::string_view def,
                std::function<void(const RpcAnswer& answer)> callback);
 
 /**
@@ -943,7 +819,8 @@
  * @param def       RPC definition
  * @param poller    poller handle
  */
-void CreatePolledRpc(NT_Entry entry, StringRef def, NT_RpcCallPoller poller);
+void CreatePolledRpc(NT_Entry entry, std::string_view def,
+                     NT_RpcCallPoller poller);
 
 /**
  * Get the next incoming RPC call.  This blocks until the next incoming RPC
@@ -1006,7 +883,7 @@
  * @param result      result raw data that will be provided to remote caller
  * @return            true if the response was posted, otherwise false
  */
-bool PostRpcResponse(NT_Entry entry, NT_RpcCall call, StringRef result);
+bool PostRpcResponse(NT_Entry entry, NT_RpcCall call, std::string_view result);
 
 /**
  * Call a RPC function.  May be used on either the client or server.
@@ -1019,7 +896,7 @@
  * @return RPC call handle (for use with GetRpcResult() or
  *         CancelRpcResult()).
  */
-NT_RpcCall CallRpc(NT_Entry entry, StringRef params);
+NT_RpcCall CallRpc(NT_Entry entry, std::string_view params);
 
 /**
  * Get the result (return value) of a RPC call.  This function blocks until
@@ -1070,7 +947,7 @@
  * @param def         RPC version 1 definition (output)
  * @return True if successfully unpacked, false otherwise.
  */
-bool UnpackRpcDefinition(StringRef packed, RpcDefinition* def);
+bool UnpackRpcDefinition(std::string_view packed, RpcDefinition* def);
 
 /**
  * Pack RPC values as required for RPC version 1 definition messages.
@@ -1078,7 +955,7 @@
  * @param values      array of values to pack
  * @return Raw packed bytes.
  */
-std::string PackRpcValues(ArrayRef<std::shared_ptr<Value>> values);
+std::string PackRpcValues(wpi::span<const std::shared_ptr<Value>> values);
 
 /**
  * Unpack RPC values as required for RPC version 1 definition messages.
@@ -1087,8 +964,8 @@
  * @param types       array of data types (as provided in the RPC definition)
  * @return Array of values.
  */
-std::vector<std::shared_ptr<Value>> UnpackRpcValues(StringRef packed,
-                                                    ArrayRef<NT_Type> types);
+std::vector<std::shared_ptr<Value>> UnpackRpcValues(
+    std::string_view packed, wpi::span<const NT_Type> types);
 
 /** @} */
 
@@ -1102,25 +979,10 @@
  * This is the name used during the initial connection handshake, and is
  * visible through ConnectionInfo on the remote node.
  *
+ * @param inst      instance handle
  * @param name      identity to advertise
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-void SetNetworkIdentity(StringRef name);
-
-/**
- * @copydoc SetNetworkIdentity(StringRef)
- *
- * @param inst      instance handle
- */
-void SetNetworkIdentity(NT_Inst inst, const Twine& name);
-
-/**
- * Get the current network mode.
- *
- * @return Bitmask of NT_NetworkMode.
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-unsigned int GetNetworkMode();
+void SetNetworkIdentity(NT_Inst inst, std::string_view name);
 
 /**
  * Get the current network mode.
@@ -1146,32 +1008,18 @@
 /**
  * Starts a server using the specified filename, listening address, and port.
  *
+ * @param inst              instance handle
  * @param persist_filename  the name of the persist file to use (UTF-8 string,
  *                          null terminated)
  * @param listen_address    the address to listen on, or null to listen on any
  *                          address. (UTF-8 string, null terminated)
  * @param port              port to communicate over.
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StartServer(StringRef persist_filename, const char* listen_address,
-                 unsigned int port);
-
-/**
- * @copydoc StartServer(StringRef, const char*, unsigned int)
- *
- * @param inst              instance handle
- */
-void StartServer(NT_Inst inst, const Twine& persist_filename,
+void StartServer(NT_Inst inst, std::string_view persist_filename,
                  const char* listen_address, unsigned int port);
 
 /**
  * Stops the server if it is running.
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StopServer();
-
-/**
- * @copydoc StopServer()
  *
  * @param inst  instance handle
  */
@@ -1179,49 +1027,30 @@
 
 /**
  * Starts a client.  Use SetServer to set the server name and port.
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StartClient();
-
-/**
- * Starts a client using the specified server and port
- *
- * @param server_name server name (UTF-8 string, null terminated)
- * @param port        port to communicate over
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StartClient(const char* server_name, unsigned int port);
-
-/**
- * Starts a client using the specified (server, port) combinations.  The
- * client will attempt to connect to each server in round robin fashion.
- *
- * @param servers   array of server name and port pairs
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StartClient(ArrayRef<std::pair<StringRef, unsigned int>> servers);
-
-/**
- * @copydoc StartClient()
  *
  * @param inst  instance handle
  */
 void StartClient(NT_Inst inst);
 
 /**
- * @copydoc StartClient(const char*, unsigned int)
+ * Starts a client using the specified server and port
  *
  * @param inst        instance handle
+ * @param server_name server name (UTF-8 string, null terminated)
+ * @param port        port to communicate over
  */
 void StartClient(NT_Inst inst, const char* server_name, unsigned int port);
 
 /**
- * @copydoc StartClient(ArrayRef<std::pair<StringRef, unsigned int>>)
+ * Starts a client using the specified (server, port) combinations.  The
+ * client will attempt to connect to each server in round robin fashion.
  *
  * @param inst      instance handle
+ * @param servers   array of server name and port pairs
  */
-void StartClient(NT_Inst inst,
-                 ArrayRef<std::pair<StringRef, unsigned int>> servers);
+void StartClient(
+    NT_Inst inst,
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers);
 
 /**
  * Starts a client using commonly known robot addresses for the specified
@@ -1235,12 +1064,7 @@
 
 /**
  * Stops the client if it is running.
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StopClient();
-
-/**
- * @copydoc StopClient()
+ *
  * @param inst  instance handle
  */
 void StopClient(NT_Inst inst);
@@ -1248,35 +1072,22 @@
 /**
  * Sets server address and port for client (without restarting client).
  *
+ * @param inst        instance handle
  * @param server_name server name (UTF-8 string, null terminated)
  * @param port        port to communicate over
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-void SetServer(const char* server_name, unsigned int port);
+void SetServer(NT_Inst inst, const char* server_name, unsigned int port);
 
 /**
  * Sets server addresses for client (without restarting client).
  * The client will attempt to connect to each server in round robin fashion.
  *
+ * @param inst      instance handle
  * @param servers   array of server name and port pairs
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-void SetServer(ArrayRef<std::pair<StringRef, unsigned int>> servers);
-
-/**
- * @copydoc SetServer(const char*, unsigned int)
- *
- * @param inst        instance handle
- */
-void SetServer(NT_Inst inst, const char* server_name, unsigned int port);
-
-/**
- * @copydoc SetServer(ArrayRef<std::pair<StringRef, unsigned int>>)
- *
- * @param inst      instance handle
- */
-void SetServer(NT_Inst inst,
-               ArrayRef<std::pair<StringRef, unsigned int>> servers);
+void SetServer(
+    NT_Inst inst,
+    wpi::span<const std::pair<std::string_view, unsigned int>> servers);
 
 /**
  * Sets server addresses and port for client (without restarting client).
@@ -1293,45 +1104,24 @@
  * This connects to the Driver Station running on localhost to obtain the
  * server IP address.
  *
- * @param port server port to use in combination with IP from DS
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StartDSClient(unsigned int port);
-
-/**
- * @copydoc StartDSClient(unsigned int)
  * @param inst  instance handle
+ * @param port server port to use in combination with IP from DS
  */
 void StartDSClient(NT_Inst inst, unsigned int port);
 
-/** Stops requesting server address from Driver Station. */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StopDSClient();
-
 /**
- * @copydoc StopDSClient()
+ * Stops requesting server address from Driver Station.
  *
  * @param inst  instance handle
  */
 void StopDSClient(NT_Inst inst);
 
-/** Stops the RPC server if it is running. */
-WPI_DEPRECATED("use NT_Inst function instead")
-void StopRpcServer();
-
 /**
  * Set the periodic update rate.
  * Sets how frequently updates are sent to other nodes over the network.
  *
- * @param interval  update interval in seconds (range 0.01 to 1.0)
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void SetUpdateRate(double interval);
-
-/**
- * @copydoc SetUpdateRate(double)
- *
  * @param inst      instance handle
+ * @param interval  update interval in seconds (range 0.01 to 1.0)
  */
 void SetUpdateRate(NT_Inst inst, double interval);
 
@@ -1345,12 +1135,6 @@
  * Note: flushes are rate limited to avoid excessive network traffic.  If
  * the time between calls is too short, the flush will occur after the minimum
  * time elapses (rather than immediately).
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void Flush();
-
-/**
- * @copydoc Flush()
  *
  * @param inst      instance handle
  */
@@ -1360,15 +1144,8 @@
  * Get information on the currently established network connections.
  * If operating as a client, this will return either zero or one values.
  *
- * @return      array of connection information
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-std::vector<ConnectionInfo> GetConnections();
-
-/**
- * @copydoc GetConnections()
- *
  * @param inst  instance handle
+ * @return      array of connection information
  */
 std::vector<ConnectionInfo> GetConnections(NT_Inst inst);
 
@@ -1392,39 +1169,24 @@
  * but this function provides a way to save persistent values in the same
  * format to a file on either a client or a server.
  *
+ * @param inst      instance handle
  * @param filename  filename
  * @return error string, or nullptr if successful
  */
-WPI_DEPRECATED("use NT_Inst function instead")
-const char* SavePersistent(StringRef filename);
-
-/**
- * @copydoc SavePersistent(StringRef)
- * @param inst      instance handle
- */
-const char* SavePersistent(NT_Inst inst, const Twine& filename);
+const char* SavePersistent(NT_Inst inst, std::string_view filename);
 
 /**
  * Load persistent values from a file.  The server automatically does this
  * at startup, but this function provides a way to restore persistent values
  * in the same format from a file at any time on either a client or a server.
  *
+ * @param inst      instance handle
  * @param filename  filename
  * @param warn      callback function for warnings
  * @return error string, or nullptr if successful
  */
-WPI_DEPRECATED("use NT_Inst function instead")
 const char* LoadPersistent(
-    StringRef filename, std::function<void(size_t line, const char* msg)> warn);
-
-/**
- * @copydoc LoadPersistent(StringRef, std::function<void(size_t, const
- * char*)>)
- *
- * @param inst      instance handle
- */
-const char* LoadPersistent(
-    NT_Inst inst, const Twine& filename,
+    NT_Inst inst, std::string_view filename,
     std::function<void(size_t line, const char* msg)> warn);
 
 /**
@@ -1436,8 +1198,8 @@
  * @param prefix    save only keys starting with this prefix
  * @return error string, or nullptr if successful
  */
-const char* SaveEntries(NT_Inst inst, const Twine& filename,
-                        const Twine& prefix);
+const char* SaveEntries(NT_Inst inst, std::string_view filename,
+                        std::string_view prefix);
 
 /**
  * Load table values from a file.  The file format used is identical to
@@ -1449,8 +1211,8 @@
  * @param warn      callback function for warnings
  * @return error string, or nullptr if successful
  */
-const char* LoadEntries(NT_Inst inst, const Twine& filename,
-                        const Twine& prefix,
+const char* LoadEntries(NT_Inst inst, std::string_view filename,
+                        std::string_view prefix,
                         std::function<void(size_t line, const char* msg)> warn);
 
 /** @} */
@@ -1477,31 +1239,6 @@
  */
 
 /**
- * Log function.
- *
- * @param level   log level of the message (see NT_LogLevel)
- * @param file    origin source filename
- * @param line    origin source line number
- * @param msg     message
- */
-typedef std::function<void(unsigned int level, const char* file,
-                           unsigned int line, const char* msg)>
-    LogFunc;
-
-/**
- * Set logger callback function.  By default, log messages are sent to stderr;
- * this function changes the log level and sends log messages to the provided
- * callback function instead.  The callback function will only be called for
- * log messages with level greater than or equal to min_level; messages lower
- * than this level will be silently ignored.
- *
- * @param func        log callback function
- * @param min_level   minimum log level
- */
-WPI_DEPRECATED("use NT_Inst function instead")
-void SetLogger(LogFunc func, unsigned int min_level);
-
-/**
  * Add logger callback function.  By default, log messages are sent to stderr;
  * this function sends log messages to the provided callback function instead.
  * The callback function will only be called for log messages with level
@@ -1602,7 +1339,7 @@
 /** @} */
 /** @} */
 
-inline bool RpcAnswer::PostResponse(StringRef result) const {
+inline bool RpcAnswer::PostResponse(std::string_view result) const {
   auto ret = PostRpcResponse(entry, call, result);
   call = 0;
   return ret;
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_test.h b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_test.h
index 920fd68..65d1243 100644
--- a/third_party/allwpilib/ntcore/src/main/native/include/ntcore_test.h
+++ b/third_party/allwpilib/ntcore/src/main/native/include/ntcore_test.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_NTCORE_TEST_H_
 #define NTCORE_NTCORE_TEST_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/tables/ITable.h b/third_party/allwpilib/ntcore/src/main/native/include/tables/ITable.h
deleted file mode 100644
index d03aaa7..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/include/tables/ITable.h
+++ /dev/null
@@ -1,456 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef NTCORE_TABLES_ITABLE_H_
-#define NTCORE_TABLES_ITABLE_H_
-
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-#include <wpi/deprecated.h>
-
-#include "networktables/NetworkTableValue.h"
-
-namespace nt {
-class NetworkTable;
-}  // namespace nt
-
-class ITableListener;
-
-/**
- * A table whose values can be read and written to
- */
-class WPI_DEPRECATED("Use NetworkTable directly") ITable {
- public:
-  /**
-   * Determines whether the given key is in this table.
-   *
-   * @param key the key to search for
-   * @return true if the table as a value assigned to the given key
-   */
-  virtual bool ContainsKey(const wpi::Twine& key) const = 0;
-
-  /**
-   * Determines whether there exists a non-empty subtable for this key
-   * in this table.
-   *
-   * @param key the key to search for
-   * @return true if there is a subtable with the key which contains at least
-   * one key/subtable of its own
-   */
-  virtual bool ContainsSubTable(const wpi::Twine& key) const = 0;
-
-  /**
-   * Gets the subtable in this table for the given name.
-   *
-   * @param key the name of the table relative to this one
-   * @return a sub table relative to this one
-   */
-  virtual std::shared_ptr<nt::NetworkTable> GetSubTable(
-      const wpi::Twine& key) const = 0;
-
-  /**
-   * @param types bitmask of types; 0 is treated as a "don't care".
-   * @return keys currently in the table
-   */
-  virtual std::vector<std::string> GetKeys(int types = 0) const = 0;
-
-  /**
-   * @return subtables currently in the table
-   */
-  virtual std::vector<std::string> GetSubTables() const = 0;
-
-  /**
-   * Makes a key's value persistent through program restarts.
-   *
-   * @param key the key to make persistent
-   */
-  virtual void SetPersistent(wpi::StringRef key) = 0;
-
-  /**
-   * Stop making a key's value persistent through program restarts.
-   * The key cannot be null.
-   *
-   * @param key the key name
-   */
-  virtual void ClearPersistent(wpi::StringRef key) = 0;
-
-  /**
-   * Returns whether the value is persistent through program restarts.
-   * The key cannot be null.
-   *
-   * @param key the key name
-   */
-  virtual bool IsPersistent(wpi::StringRef key) const = 0;
-
-  /**
-   * Sets flags on the specified key in this table. The key can
-   * not be null.
-   *
-   * @param key the key name
-   * @param flags the flags to set (bitmask)
-   */
-  virtual void SetFlags(wpi::StringRef key, unsigned int flags) = 0;
-
-  /**
-   * Clears flags on the specified key in this table. The key can
-   * not be null.
-   *
-   * @param key the key name
-   * @param flags the flags to clear (bitmask)
-   */
-  virtual void ClearFlags(wpi::StringRef key, unsigned int flags) = 0;
-
-  /**
-   * Returns the flags for the specified key.
-   *
-   * @param key the key name
-   * @return the flags, or 0 if the key is not defined
-   */
-  virtual unsigned int GetFlags(wpi::StringRef key) const = 0;
-
-  /**
-   * Deletes the specified key in this table.
-   *
-   * @param key the key name
-   */
-  virtual void Delete(const wpi::Twine& key) = 0;
-
-  /**
-   * Gets the value associated with a key as an object
-   *
-   * @param key the key of the value to look up
-   * @return the value associated with the given key, or nullptr if the key
-   * does not exist
-   */
-  virtual std::shared_ptr<nt::Value> GetValue(const wpi::Twine& key) const = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultValue(const wpi::Twine& key,
-                               std::shared_ptr<nt::Value> defaultValue) = 0;
-
-  /**
-   * Put a value in the table
-   *
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutValue(const wpi::Twine& key,
-                        std::shared_ptr<nt::Value> value) = 0;
-
-  /**
-   * Put a number in the table
-   *
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutNumber(wpi::StringRef key, double value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultNumber(wpi::StringRef key, double defaultValue) = 0;
-
-  /**
-   * Gets the number associated with the given name.
-   *
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  virtual double GetNumber(wpi::StringRef key, double defaultValue) const = 0;
-
-  /**
-   * Put a string in the table
-   *
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutString(wpi::StringRef key, wpi::StringRef value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultString(wpi::StringRef key,
-                                wpi::StringRef defaultValue) = 0;
-
-  /**
-   * Gets the string associated with the given name. If the key does not
-   * exist or is of different type, it will return the default value.
-   *
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   *
-   * @note This makes a copy of the string.  If the overhead of this is a
-   *       concern, use GetValue() instead.
-   */
-  virtual std::string GetString(wpi::StringRef key,
-                                wpi::StringRef defaultValue) const = 0;
-
-  /**
-   * Put a boolean in the table
-   *
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutBoolean(wpi::StringRef key, bool value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultBoolean(wpi::StringRef key, bool defaultValue) = 0;
-
-  /**
-   * Gets the boolean associated with the given name. If the key does not
-   * exist or is of different type, it will return the default value.
-   *
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   */
-  virtual bool GetBoolean(wpi::StringRef key, bool defaultValue) const = 0;
-
-  /**
-   * Put a boolean array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   *
-   * @note The array must be of int's rather than of bool's because
-   *       std::vector<bool> is special-cased in C++.  0 is false, any
-   *       non-zero value is true.
-   */
-  virtual bool PutBooleanArray(wpi::StringRef key,
-                               wpi::ArrayRef<int> value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultBooleanArray(wpi::StringRef key,
-                                      wpi::ArrayRef<int> defaultValue) = 0;
-
-  /**
-   * Returns the boolean array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   *
-   * @note This makes a copy of the array.  If the overhead of this is a
-   *       concern, use GetValue() instead.
-   *
-   * @note The returned array is std::vector<int> instead of std::vector<bool>
-   *       because std::vector<bool> is special-cased in C++.  0 is false, any
-   *       non-zero value is true.
-   */
-  virtual std::vector<int> GetBooleanArray(
-      wpi::StringRef key, wpi::ArrayRef<int> defaultValue) const = 0;
-
-  /**
-   * Put a number array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutNumberArray(wpi::StringRef key,
-                              wpi::ArrayRef<double> value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultNumberArray(wpi::StringRef key,
-                                     wpi::ArrayRef<double> defaultValue) = 0;
-
-  /**
-   * Returns the number array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   *
-   * @note This makes a copy of the array.  If the overhead of this is a
-   *       concern, use GetValue() instead.
-   */
-  virtual std::vector<double> GetNumberArray(
-      wpi::StringRef key, wpi::ArrayRef<double> defaultValue) const = 0;
-
-  /**
-   * Put a string array in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutStringArray(wpi::StringRef key,
-                              wpi::ArrayRef<std::string> value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultStringArray(
-      wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue) = 0;
-
-  /**
-   * Returns the string array the key maps to. If the key does not exist or is
-   * of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   *
-   * @note This makes a copy of the array.  If the overhead of this is a
-   *       concern, use GetValue() instead.
-   */
-  virtual std::vector<std::string> GetStringArray(
-      wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue) const = 0;
-
-  /**
-   * Put a raw value (byte array) in the table
-   * @param key the key to be assigned to
-   * @param value the value that will be assigned
-   * @return False if the table key already exists with a different type
-   */
-  virtual bool PutRaw(wpi::StringRef key, wpi::StringRef value) = 0;
-
-  /**
-   * Gets the current value in the table, setting it if it does not exist.
-   * @param key the key
-   * @param defaultValue the default value to set if key doesn't exist.
-   * @returns False if the table key exists with a different type
-   */
-  virtual bool SetDefaultRaw(wpi::StringRef key,
-                             wpi::StringRef defaultValue) = 0;
-
-  /**
-   * Returns the raw value (byte array) the key maps to. If the key does not
-   * exist or is of different type, it will return the default value.
-   * @param key the key to look up
-   * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   * if there is no value associated with the key
-   *
-   * @note This makes a copy of the raw contents.  If the overhead of this is a
-   *       concern, use GetValue() instead.
-   */
-  virtual std::string GetRaw(wpi::StringRef key,
-                             wpi::StringRef defaultValue) const = 0;
-
-  /**
-   * Add a listener for changes to the table
-   *
-   * @param listener the listener to add
-   */
-  virtual void AddTableListener(ITableListener* listener) = 0;
-
-  /**
-   * Add a listener for changes to the table
-   *
-   * @param listener the listener to add
-   * @param immediateNotify if true then this listener will be notified of all
-   * current entries (marked as new)
-   */
-  virtual void AddTableListener(ITableListener* listener,
-                                bool immediateNotify) = 0;
-
-  /**
-   * Add a listener for changes to the table
-   *
-   * @param listener the listener to add
-   * @param immediateNotify if true then this listener will be notified of all
-   * current entries (marked as new)
-   * @param flags bitmask of NT_NotifyKind specifying desired notifications
-   */
-  virtual void AddTableListenerEx(ITableListener* listener,
-                                  unsigned int flags) = 0;
-
-  /**
-   * Add a listener for changes to a specific key the table
-   *
-   * @param key the key to listen for
-   * @param listener the listener to add
-   * @param immediateNotify if true then this listener will be notified of all
-   * current entries (marked as new)
-   */
-  virtual void AddTableListener(wpi::StringRef key, ITableListener* listener,
-                                bool immediateNotify) = 0;
-
-  /**
-   * Add a listener for changes to a specific key the table
-   *
-   * @param key the key to listen for
-   * @param listener the listener to add
-   * @param immediateNotify if true then this listener will be notified of all
-   * current entries (marked as new)
-   * @param flags bitmask of NT_NotifyKind specifying desired notifications
-   */
-  virtual void AddTableListenerEx(wpi::StringRef key, ITableListener* listener,
-                                  unsigned int flags) = 0;
-
-  /**
-   * This will immediately notify the listener of all current sub tables
-   * @param listener the listener to add
-   */
-  virtual void AddSubTableListener(ITableListener* listener) = 0;
-
-  /**
-   * This will immediately notify the listener of all current sub tables
-   * @param listener the listener to add
-   * @param localNotify if true then this listener will be notified of all
-   * local changes in addition to all remote changes
-   */
-  virtual void AddSubTableListener(ITableListener* listener,
-                                   bool localNotify) = 0;
-
-  /**
-   * Remove a listener from receiving table events
-   *
-   * @param listener the listener to be removed
-   */
-  virtual void RemoveTableListener(ITableListener* listener) = 0;
-
-  /**
-   * Gets the full path of this table.
-   */
-  virtual wpi::StringRef GetPath() const = 0;
-};
-
-#endif  // NTCORE_TABLES_ITABLE_H_
diff --git a/third_party/allwpilib/ntcore/src/main/native/include/tables/ITableListener.h b/third_party/allwpilib/ntcore/src/main/native/include/tables/ITableListener.h
deleted file mode 100644
index dae6f85..0000000
--- a/third_party/allwpilib/ntcore/src/main/native/include/tables/ITableListener.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef NTCORE_TABLES_ITABLELISTENER_H_
-#define NTCORE_TABLES_ITABLELISTENER_H_
-
-#include <memory>
-
-#include <wpi/StringRef.h>
-#include <wpi/deprecated.h>
-
-#include "networktables/NetworkTableValue.h"
-
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
-#elif _WIN32
-#pragma warning(push)
-#pragma warning(disable : 4996)
-#endif
-
-class ITable;
-
-/**
- * A listener that listens to changes in values in a {@link ITable}
- */
-class WPI_DEPRECATED(
-    "Use EntryListener, TableEntryListener, or TableListener as appropriate")
-    ITableListener {
- public:
-  virtual ~ITableListener() = default;
-  /**
-   * Called when a key-value pair is changed in a {@link ITable}
-   * @param source the table the key-value pair exists in
-   * @param key the key associated with the value that changed
-   * @param value the new value
-   * @param isNew true if the key did not previously exist in the table,
-   * otherwise it is false
-   */
-  virtual void ValueChanged(ITable* source, wpi::StringRef key,
-                            std::shared_ptr<nt::Value> value, bool isNew) = 0;
-
-  /**
-   * Extended version of ValueChanged.  Called when a key-value pair is
-   * changed in a {@link ITable}.  The default implementation simply calls
-   * ValueChanged().  If this is overridden, ValueChanged() will not be called.
-   * @param source the table the key-value pair exists in
-   * @param key the key associated with the value that changed
-   * @param value the new value
-   * @param flags update flags; for example, NT_NOTIFY_NEW if the key did not
-   * previously exist in the table
-   */
-  virtual void ValueChangedEx(ITable* source, wpi::StringRef key,
-                              std::shared_ptr<nt::Value> value,
-                              unsigned int flags);
-};
-
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#elif _WIN32
-#pragma warning(pop)
-#endif
-
-#endif  // NTCORE_TABLES_ITABLELISTENER_H_
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/ConnectionListenerTest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/ConnectionListenerTest.java
index 9628a0e..3c38853 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/ConnectionListenerTest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/ConnectionListenerTest.java
@@ -1,23 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-import java.util.ArrayList;
-import java.util.List;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.condition.DisabledOnOs;
-import org.junit.jupiter.api.condition.OS;
-import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.ValueSource;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
@@ -25,6 +11,16 @@
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.condition.DisabledOnOs;
+import org.junit.jupiter.api.condition.OS;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.ValueSource;
+
 class ConnectionListenerTest {
   private NetworkTableInstance m_serverInst;
   private NetworkTableInstance m_clientInst;
@@ -44,13 +40,10 @@
     m_serverInst.close();
   }
 
-  /**
-   * Connect to the server.
-   */
-  @SuppressWarnings("PMD.AvoidUsingHardCodedIP")
-  private void connect() {
-    m_serverInst.startServer("connectionlistenertest.ini", "127.0.0.1", 10000);
-    m_clientInst.startClient("127.0.0.1", 10000);
+  /** Connect to the server. */
+  private void connect(int port) {
+    m_serverInst.startServer("connectionlistenertest.ini", "127.0.0.1", port);
+    m_clientInst.startClient("127.0.0.1", port);
 
     // wait for client to report it's started, then wait another 0.1 sec
     try {
@@ -73,7 +66,7 @@
     assertNotSame(handle, 0, "bad listener handle");
 
     // trigger a connect event
-    connect();
+    connect(10020);
 
     // get the event
     assertTrue(m_serverInst.waitForConnectionListenerQueue(1.0));
@@ -111,20 +104,21 @@
     assertEquals(1, events.length);
     assertEquals(handle, events[0].listener);
     assertFalse(events[0].connected);
-
   }
 
+  private static int threadedPort = 10001;
+
   @ParameterizedTest
   @DisabledOnOs(OS.WINDOWS)
-  @SuppressWarnings("PMD.AvoidUsingHardCodedIP")
-  @ValueSource(strings = { "127.0.0.1", "127.0.0.1 ", " 127.0.0.1 " })
+  @ValueSource(strings = {"127.0.0.1", "127.0.0.1 ", " 127.0.0.1 "})
   void testThreaded(String address) {
-    m_serverInst.startServer("connectionlistenertest.ini", address, 10000);
+    m_serverInst.startServer("connectionlistenertest.ini", address, threadedPort);
     List<ConnectionNotification> events = new ArrayList<>();
     final int handle = m_serverInst.addConnectionListener(events::add, false);
 
     // trigger a connect event
-    m_clientInst.startClient(address, 10000);
+    m_clientInst.startClient(address, threadedPort);
+    threadedPort++;
 
     // wait for client to report it's started, then wait another 0.1 sec
     try {
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/EntryListenerTest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/EntryListenerTest.java
index 80627a4..7b31c26 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/EntryListenerTest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/EntryListenerTest.java
@@ -1,24 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
-import java.util.ArrayList;
-import java.util.List;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.Assertions.fail;
 
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
 class EntryListenerTest {
   private NetworkTableInstance m_serverInst;
   private NetworkTableInstance m_clientInst;
@@ -38,10 +34,9 @@
     m_serverInst.close();
   }
 
-  @SuppressWarnings("PMD.AvoidUsingHardCodedIP")
   private void connect() {
-    m_serverInst.startServer("connectionlistenertest.ini", "127.0.0.1", 10000);
-    m_clientInst.startClient("127.0.0.1", 10000);
+    m_serverInst.startServer("connectionlistenertest.ini", "127.0.0.1", 10010);
+    m_clientInst.startClient("127.0.0.1", 10010);
 
     // Use connection listener to ensure we've connected
     int poller = NetworkTablesJNI.createConnectionListenerPoller(m_clientInst.getHandle());
@@ -56,15 +51,12 @@
     }
   }
 
-  /**
-   * Test prefix with a new remote.
-   */
+  /** Test prefix with a new remote. */
   @Test
   void testPrefixNewRemote() {
     connect();
     List<EntryNotification> events = new ArrayList<>();
-    final int handle = m_serverInst.addEntryListener("/foo", events::add,
-        EntryListenerFlags.kNew);
+    final int handle = m_serverInst.addEntryListener("/foo", events::add, EntryListenerFlags.kNew);
 
     // Trigger an event
     m_clientInst.getEntry("/foo/bar").setDouble(1.0);
@@ -79,13 +71,13 @@
     assertTrue(m_serverInst.waitForEntryListenerQueue(1.0));
 
     // Check the event
-    assertAll("Event",
+    assertAll(
+        "Event",
         () -> assertEquals(1, events.size()),
         () -> assertEquals(handle, events.get(0).listener),
         () -> assertEquals(m_serverInst.getEntry("/foo/bar"), events.get(0).getEntry()),
         () -> assertEquals("/foo/bar", events.get(0).name),
         () -> assertEquals(NetworkTableValue.makeDouble(1.0), events.get(0).value),
-        () -> assertEquals(EntryListenerFlags.kNew, events.get(0).flags)
-    );
+        () -> assertEquals(EntryListenerFlags.kNew, events.get(0).flags));
   }
 }
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/JNITest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/JNITest.java
index ef2b42b..3eb522b 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/JNITest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/JNITest.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/LoggerTest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/LoggerTest.java
index 420f9dc..1dc26ab 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/LoggerTest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/LoggerTest.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.fail;
+
 import java.util.ArrayList;
 import java.util.List;
-
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.fail;
-
 class LoggerTest {
   private NetworkTableInstance m_clientInst;
 
@@ -31,7 +27,6 @@
   }
 
   @Test
-  @SuppressWarnings("PMD.AvoidUsingHardCodedIP")
   void addMessageTest() {
     List<LogMessage> msgs = new ArrayList<>();
     m_clientInst.addLogger(msgs::add, LogMessage.kInfo, 100);
diff --git a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java
index 71ce0ae..4c431af 100644
--- a/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java
+++ b/third_party/allwpilib/ntcore/src/test/java/edu/wpi/first/networktables/NetworkTableTest.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.networktables;
 
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
 import java.util.Arrays;
 import java.util.Collections;
 import java.util.List;
 import java.util.stream.Stream;
-
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
 
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 class NetworkTableTest {
   private static Stream<Arguments> basenameKeyArguments() {
     return Stream.of(
         Arguments.of("simple", "simple"),
         Arguments.of("simple", "one/two/many/simple"),
-        Arguments.of("simple", "//////an/////awful/key////simple")
-    );
+        Arguments.of("simple", "//////an/////awful/key////simple"));
   }
 
   @ParameterizedTest
@@ -38,8 +33,7 @@
         Arguments.of("/", "///"),
         Arguments.of("/no/normal/req", "/no/normal/req"),
         Arguments.of("/no/leading/slash", "no/leading/slash"),
-        Arguments.of("/what/an/awful/key/", "//////what////an/awful/////key///")
-    );
+        Arguments.of("/what/an/awful/key/", "//////what////an/awful/////key///"));
   }
 
   @ParameterizedTest
@@ -54,8 +48,7 @@
         Arguments.of("a", "///a"),
         Arguments.of("leading/slash", "/leading/slash"),
         Arguments.of("no/leading/slash", "no/leading/slash"),
-        Arguments.of("what/an/awful/key/", "//////what////an/awful/////key///")
-    );
+        Arguments.of("what/an/awful/key/", "//////what////an/awful/////key///"));
   }
 
   @ParameterizedTest
@@ -69,8 +62,7 @@
         Arguments.of(Collections.singletonList("/"), ""),
         Arguments.of(Collections.singletonList("/"), "/"),
         Arguments.of(Arrays.asList("/", "/foo", "/foo/bar", "/foo/bar/baz"), "/foo/bar/baz"),
-        Arguments.of(Arrays.asList("/", "/foo", "/foo/bar", "/foo/bar/"), "/foo/bar/")
-    );
+        Arguments.of(Arrays.asList("/", "/foo", "/foo/bar", "/foo/bar/"), "/foo/bar/"));
   }
 
   @ParameterizedTest
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/ConnectionListenerTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/ConnectionListenerTest.cpp
index a56e45c..14c327c 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/ConnectionListenerTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/ConnectionListenerTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <thread>
@@ -25,21 +22,21 @@
     nt::DestroyInstance(client_inst);
   }
 
-  void Connect();
+  void Connect(unsigned int port);
 
  protected:
   NT_Inst server_inst;
   NT_Inst client_inst;
 };
 
-void ConnectionListenerTest::Connect() {
-  nt::StartServer(server_inst, "connectionlistenertest.ini", "127.0.0.1",
-                  10000);
-  nt::StartClient(client_inst, "127.0.0.1", 10000);
+void ConnectionListenerTest::Connect(unsigned int port) {
+  nt::StartServer(server_inst, "connectionlistenertest.ini", "127.0.0.1", port);
+  nt::StartClient(client_inst, "127.0.0.1", port);
 
   // wait for client to report it's started, then wait another 0.1 sec
-  while ((nt::GetNetworkMode(client_inst) & NT_NET_MODE_STARTING) != 0)
+  while ((nt::GetNetworkMode(client_inst) & NT_NET_MODE_STARTING) != 0) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
+  }
   std::this_thread::sleep_for(std::chrono::milliseconds(100));
 }
 
@@ -52,7 +49,7 @@
   ASSERT_NE(handle, 0u);
 
   // trigger a connect event
-  Connect();
+  Connect(10000);
 
   // get the event
   ASSERT_TRUE(nt::WaitForConnectionListenerQueue(server_inst, 1.0));
@@ -87,7 +84,7 @@
       false);
 
   // trigger a connect event
-  Connect();
+  Connect(10001);
 
   ASSERT_TRUE(nt::WaitForConnectionListenerQueue(server_inst, 1.0));
 
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/EntryListenerTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/EntryListenerTest.cpp
index b7bf2f6..8349484 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/EntryListenerTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/EntryListenerTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <thread>
@@ -38,16 +35,16 @@
     nt::DestroyInstance(client_inst);
   }
 
-  void Connect();
+  void Connect(unsigned int port);
 
  protected:
   NT_Inst server_inst;
   NT_Inst client_inst;
 };
 
-void EntryListenerTest::Connect() {
-  nt::StartServer(server_inst, "entrylistenertest.ini", "127.0.0.1", 10000);
-  nt::StartClient(client_inst, "127.0.0.1", 10000);
+void EntryListenerTest::Connect(unsigned int port) {
+  nt::StartServer(server_inst, "entrylistenertest.ini", "127.0.0.1", port);
+  nt::StartClient(client_inst, "127.0.0.1", port);
 
   // Use connection listener to ensure we've connected
   NT_ConnectionListenerPoller poller =
@@ -84,8 +81,10 @@
 }
 
 TEST_F(EntryListenerTest, DISABLED_EntryNewRemote) {
-  Connect();
-  if (HasFatalFailure()) return;
+  Connect(10010);
+  if (HasFatalFailure()) {
+    return;
+  }
   std::vector<nt::EntryNotification> events;
   auto handle = nt::AddEntryListener(
       nt::GetEntry(server_inst, "/foo"),
@@ -136,8 +135,10 @@
 }
 
 TEST_F(EntryListenerTest, DISABLED_PrefixNewRemote) {
-  Connect();
-  if (HasFatalFailure()) return;
+  Connect(10011);
+  if (HasFatalFailure()) {
+    return;
+  }
   std::vector<nt::EntryNotification> events;
   auto handle = nt::AddEntryListener(
       server_inst, "/foo",
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/EntryNotifierTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/EntryNotifierTest.cpp
index 604db3d..e781b49 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/EntryNotifierTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/EntryNotifierTest.cpp
@@ -1,11 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <wpi/Logger.h>
+#include <wpi/StringExtras.h>
 
 #include "EntryNotifier.h"
 #include "TestPrinters.h"
@@ -248,7 +246,7 @@
   int g4count = 0;
   for (const auto& result : results) {
     SCOPED_TRACE(::testing::PrintToString(result));
-    EXPECT_TRUE(StringRef(result.name).startswith("/foo"));
+    EXPECT_TRUE(wpi::starts_with(result.name, "/foo"));
     EXPECT_THAT(result.value, ValueEq(Value::MakeDouble(1)));
     EXPECT_EQ(Handle{result.entry}.GetType(), Handle::kEntry);
     EXPECT_EQ(Handle{result.entry}.GetInst(), 1);
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.cpp
index 35d4f8b..69c87a0 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "MessageMatcher.h"
 
@@ -13,7 +10,9 @@
     std::shared_ptr<Message> msg,
     ::testing::MatchResultListener* listener) const {
   bool match = true;
-  if (!msg) return false;
+  if (!msg) {
+    return false;
+  }
   if (msg->str() != goodmsg->str()) {
     *listener << "str mismatch ";
     match = false;
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.h b/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.h
index 5b14334..7afeeef 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MessageMatcher.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MESSAGEMATCHER_H_
 #define NTCORE_MESSAGEMATCHER_H_
 
 #include <memory>
 #include <ostream>
+#include <utility>
 
 #include "Message.h"
 #include "TestPrinters.h"
@@ -21,7 +19,7 @@
     : public ::testing::MatcherInterface<std::shared_ptr<Message>> {
  public:
   explicit MessageMatcher(std::shared_ptr<Message> goodmsg_)
-      : goodmsg(goodmsg_) {}
+      : goodmsg(std::move(goodmsg_)) {}
 
   bool MatchAndExplain(std::shared_ptr<Message> msg,
                        ::testing::MatchResultListener* listener) const override;
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MockConnectionNotifier.h b/third_party/allwpilib/ntcore/src/test/native/cpp/MockConnectionNotifier.h
index ddf8b2e..d632d5c 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MockConnectionNotifier.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MockConnectionNotifier.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MOCKCONNECTIONNOTIFIER_H_
 #define NTCORE_MOCKCONNECTIONNOTIFIER_H_
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MockDispatcher.h b/third_party/allwpilib/ntcore/src/test/native/cpp/MockDispatcher.h
index 10af839..22b0fba 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MockDispatcher.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MockDispatcher.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MOCKDISPATCHER_H_
 #define NTCORE_MOCKDISPATCHER_H_
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MockEntryNotifier.h b/third_party/allwpilib/ntcore/src/test/native/cpp/MockEntryNotifier.h
index 2f078cb..58518c6 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MockEntryNotifier.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MockEntryNotifier.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MOCKENTRYNOTIFIER_H_
 #define NTCORE_MOCKENTRYNOTIFIER_H_
@@ -21,19 +18,19 @@
   MOCK_METHOD3(
       Add,
       unsigned int(std::function<void(const EntryNotification& event)> callback,
-                   wpi::StringRef prefix, unsigned int flags));
+                   std::string_view prefix, unsigned int flags));
   MOCK_METHOD3(
       Add,
       unsigned int(std::function<void(const EntryNotification& event)> callback,
                    unsigned int local_id, unsigned int flags));
   MOCK_METHOD3(AddPolled,
-               unsigned int(unsigned int poller_uid, wpi::StringRef prefix,
+               unsigned int(unsigned int poller_uid, std::string_view prefix,
                             unsigned int flags));
   MOCK_METHOD3(AddPolled,
                unsigned int(unsigned int poller_uid, unsigned int local_id,
                             unsigned int flags));
   MOCK_METHOD5(NotifyEntry,
-               void(unsigned int local_id, StringRef name,
+               void(unsigned int local_id, std::string_view name,
                     std::shared_ptr<Value> value, unsigned int flags,
                     unsigned int only_listener));
 };
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MockNetworkConnection.h b/third_party/allwpilib/ntcore/src/test/native/cpp/MockNetworkConnection.h
index 52c917d..be9c2c6 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MockNetworkConnection.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MockNetworkConnection.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MOCKNETWORKCONNECTION_H_
 #define NTCORE_MOCKNETWORKCONNECTION_H_
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/MockRpcServer.h b/third_party/allwpilib/ntcore/src/test/native/cpp/MockRpcServer.h
index 6e9d970..be9e512 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/MockRpcServer.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/MockRpcServer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_MOCKRPCSERVER_H_
 #define NTCORE_MOCKRPCSERVER_H_
@@ -19,7 +16,7 @@
   MOCK_METHOD1(RemoveRpc, void(unsigned int rpc_uid));
   MOCK_METHOD7(ProcessRpc,
                void(unsigned int local_id, unsigned int call_uid,
-                    StringRef name, StringRef params,
+                    std::string_view name, std::string_view params,
                     const ConnectionInfo& conn, SendResponseFunc send_response,
                     unsigned int rpc_uid));
 };
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/NetworkTableTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/NetworkTableTest.cpp
index d9a2743..73c4786 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/NetworkTableTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/NetworkTableTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "TestPrinters.h"
 #include "gtest/gtest.h"
@@ -13,51 +10,51 @@
 class NetworkTableTest : public ::testing::Test {};
 
 TEST_F(NetworkTableTest, BasenameKey) {
-  EXPECT_EQ("simple", NetworkTable::BasenameKey("simple"));
-  EXPECT_EQ("simple", NetworkTable::BasenameKey("one/two/many/simple"));
+  EXPECT_EQ("simple", nt::NetworkTable::BasenameKey("simple"));
+  EXPECT_EQ("simple", nt::NetworkTable::BasenameKey("one/two/many/simple"));
   EXPECT_EQ("simple",
-            NetworkTable::BasenameKey("//////an/////awful/key////simple"));
+            nt::NetworkTable::BasenameKey("//////an/////awful/key////simple"));
 }
 
 TEST_F(NetworkTableTest, NormalizeKeySlash) {
-  EXPECT_EQ("/", NetworkTable::NormalizeKey("///"));
-  EXPECT_EQ("/no/normal/req", NetworkTable::NormalizeKey("/no/normal/req"));
+  EXPECT_EQ("/", nt::NetworkTable::NormalizeKey("///"));
+  EXPECT_EQ("/no/normal/req", nt::NetworkTable::NormalizeKey("/no/normal/req"));
   EXPECT_EQ("/no/leading/slash",
-            NetworkTable::NormalizeKey("no/leading/slash"));
-  EXPECT_EQ("/what/an/awful/key/",
-            NetworkTable::NormalizeKey("//////what////an/awful/////key///"));
+            nt::NetworkTable::NormalizeKey("no/leading/slash"));
+  EXPECT_EQ("/what/an/awful/key/", nt::NetworkTable::NormalizeKey(
+                                       "//////what////an/awful/////key///"));
 }
 
 TEST_F(NetworkTableTest, NormalizeKeyNoSlash) {
-  EXPECT_EQ("a", NetworkTable::NormalizeKey("a", false));
-  EXPECT_EQ("a", NetworkTable::NormalizeKey("///a", false));
+  EXPECT_EQ("a", nt::NetworkTable::NormalizeKey("a", false));
+  EXPECT_EQ("a", nt::NetworkTable::NormalizeKey("///a", false));
   EXPECT_EQ("leading/slash",
-            NetworkTable::NormalizeKey("/leading/slash", false));
+            nt::NetworkTable::NormalizeKey("/leading/slash", false));
   EXPECT_EQ("no/leading/slash",
-            NetworkTable::NormalizeKey("no/leading/slash", false));
-  EXPECT_EQ(
-      "what/an/awful/key/",
-      NetworkTable::NormalizeKey("//////what////an/awful/////key///", false));
+            nt::NetworkTable::NormalizeKey("no/leading/slash", false));
+  EXPECT_EQ("what/an/awful/key/",
+            nt::NetworkTable::NormalizeKey("//////what////an/awful/////key///",
+                                           false));
 }
 
 TEST_F(NetworkTableTest, GetHierarchyEmpty) {
   std::vector<std::string> expected{"/"};
-  ASSERT_EQ(expected, NetworkTable::GetHierarchy(""));
+  ASSERT_EQ(expected, nt::NetworkTable::GetHierarchy(""));
 }
 
 TEST_F(NetworkTableTest, GetHierarchyRoot) {
   std::vector<std::string> expected{"/"};
-  ASSERT_EQ(expected, NetworkTable::GetHierarchy("/"));
+  ASSERT_EQ(expected, nt::NetworkTable::GetHierarchy("/"));
 }
 
 TEST_F(NetworkTableTest, GetHierarchyNormal) {
   std::vector<std::string> expected{"/", "/foo", "/foo/bar", "/foo/bar/baz"};
-  ASSERT_EQ(expected, NetworkTable::GetHierarchy("/foo/bar/baz"));
+  ASSERT_EQ(expected, nt::NetworkTable::GetHierarchy("/foo/bar/baz"));
 }
 
 TEST_F(NetworkTableTest, GetHierarchyTrailingSlash) {
   std::vector<std::string> expected{"/", "/foo", "/foo/bar", "/foo/bar/"};
-  ASSERT_EQ(expected, NetworkTable::GetHierarchy("/foo/bar/"));
+  ASSERT_EQ(expected, nt::NetworkTable::GetHierarchy("/foo/bar/"));
 }
 
 TEST_F(NetworkTableTest, ContainsKey) {
@@ -68,6 +65,7 @@
   ASSERT_TRUE(nt->ContainsKey("testkey"));
   ASSERT_TRUE(inst.GetEntry("/containskey/testkey").Exists());
   ASSERT_FALSE(inst.GetEntry("containskey/testkey").Exists());
+  nt::NetworkTableInstance::Destroy(inst);
 }
 
 TEST_F(NetworkTableTest, LeadingSlash) {
@@ -78,6 +76,7 @@
   nt2->PutNumber("testkey", 5);
   ASSERT_TRUE(nt->ContainsKey("testkey"));
   ASSERT_TRUE(inst.GetEntry("/leadingslash/testkey").Exists());
+  nt::NetworkTableInstance::Destroy(inst);
 }
 
 TEST_F(NetworkTableTest, EmptyOrNoSlash) {
@@ -88,4 +87,5 @@
   nt2->PutNumber("testkey", 5);
   ASSERT_TRUE(nt->ContainsKey("testkey"));
   ASSERT_TRUE(inst.GetEntry("/testkey").Exists());
+  nt::NetworkTableInstance::Destroy(inst);
 }
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.cpp
index f271123..e1ee1c7 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "StorageTest.h"
 
+#include <wpi/SmallString.h>
+#include <wpi/StringExtras.h>
 #include <wpi/raw_istream.h>
 #include <wpi/raw_ostream.h>
 
@@ -25,10 +24,10 @@
 
 namespace nt {
 
-class StorageTestEmpty : public StorageTest,
+class StorageEmptyTest : public StorageTest,
                          public ::testing::TestWithParam<bool> {
  public:
-  StorageTestEmpty() {
+  StorageEmptyTest() {
     HookOutgoing(GetParam());
     EXPECT_CALL(notifier, local_notifiers())
         .Times(AnyNumber())
@@ -36,9 +35,9 @@
   }
 };
 
-class StorageTestPopulateOne : public StorageTestEmpty {
+class StoragePopulateOneTest : public StorageEmptyTest {
  public:
-  StorageTestPopulateOne() {
+  StoragePopulateOneTest() {
     EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
     EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
     EXPECT_CALL(notifier, local_notifiers())
@@ -53,9 +52,9 @@
   }
 };
 
-class StorageTestPopulated : public StorageTestEmpty {
+class StoragePopulatedTest : public StorageEmptyTest {
  public:
-  StorageTestPopulated() {
+  StoragePopulatedTest() {
     EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
     EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
     EXPECT_CALL(notifier, local_notifiers())
@@ -73,9 +72,9 @@
   }
 };
 
-class StorageTestPersistent : public StorageTestEmpty {
+class StoragePersistentTest : public StorageEmptyTest {
  public:
-  StorageTestPersistent() {
+  StoragePersistentTest() {
     EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
     EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
     EXPECT_CALL(notifier, local_notifiers())
@@ -88,13 +87,13 @@
     storage.SetEntryTypeValue("double/big", Value::MakeDouble(1.3e8));
     storage.SetEntryTypeValue("string/empty", Value::MakeString(""));
     storage.SetEntryTypeValue("string/normal", Value::MakeString("hello"));
-    storage.SetEntryTypeValue("string/special",
-                              Value::MakeString(StringRef("\0\3\5\n", 4)));
+    storage.SetEntryTypeValue(
+        "string/special", Value::MakeString(std::string_view("\0\3\5\n", 4)));
     storage.SetEntryTypeValue("string/quoted", Value::MakeString("\"a\""));
     storage.SetEntryTypeValue("raw/empty", Value::MakeRaw(""));
     storage.SetEntryTypeValue("raw/normal", Value::MakeRaw("hello"));
     storage.SetEntryTypeValue("raw/special",
-                              Value::MakeRaw(StringRef("\0\3\5\n", 4)));
+                              Value::MakeRaw(std::string_view("\0\3\5\n", 4)));
     storage.SetEntryTypeValue("booleanarr/empty",
                               Value::MakeBooleanArray(std::vector<int>{}));
     storage.SetEntryTypeValue("booleanarr/one",
@@ -116,7 +115,7 @@
     storage.SetEntryTypeValue(
         "stringarr/two",
         Value::MakeStringArray(std::vector<std::string>{"hello", "world\n"}));
-    storage.SetEntryTypeValue(StringRef("\0\3\5\n", 4),
+    storage.SetEntryTypeValue(std::string_view("\0\3\5\n", 4),
                               Value::MakeBoolean(true));
     storage.SetEntryTypeValue("=", Value::MakeBoolean(true));
     ::testing::Mock::VerifyAndClearExpectations(&dispatcher);
@@ -129,15 +128,15 @@
 
 class MockLoadWarn {
  public:
-  MOCK_METHOD2(Warn, void(size_t line, wpi::StringRef msg));
+  MOCK_METHOD2(Warn, void(size_t line, std::string_view msg));
 };
 
-TEST_P(StorageTestEmpty, Construct) {
+TEST_P(StorageEmptyTest, Construct) {
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, StorageEntryInit) {
+TEST_P(StorageEmptyTest, StorageEntryInit) {
   auto entry = GetEntry("foo");
   EXPECT_FALSE(entry->value);
   EXPECT_EQ(0u, entry->flags);
@@ -146,13 +145,13 @@
   EXPECT_EQ(SequenceNumber(), entry->seq_num);
 }
 
-TEST_P(StorageTestEmpty, GetEntryValueNotExist) {
+TEST_P(StorageEmptyTest, GetEntryValueNotExist) {
   EXPECT_FALSE(storage.GetEntryValue("foo"));
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, GetEntryValueExist) {
+TEST_P(StorageEmptyTest, GetEntryValueExist) {
   auto value = Value::MakeBoolean(true);
   EXPECT_CALL(dispatcher, QueueOutgoing(_, IsNull(), IsNull()));
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _));
@@ -160,7 +159,7 @@
   EXPECT_EQ(value, storage.GetEntryValue("foo"));
 }
 
-TEST_P(StorageTestEmpty, SetEntryTypeValueAssignNew) {
+TEST_P(StorageEmptyTest, SetEntryTypeValueAssignNew) {
   // brand new entry
   auto value = Value::MakeBoolean(true);
   // id assigned if server
@@ -168,7 +167,7 @@
               QueueOutgoing(MessageEq(Message::EntryAssign(
                                 "foo", GetParam() ? 0 : 0xffff, 1, value, 0)),
                             IsNull(), IsNull()));
-  EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"), value,
+  EXPECT_CALL(notifier, NotifyEntry(0, std::string_view("foo"), value,
                                     NT_NOTIFY_NEW | NT_NOTIFY_LOCAL, UINT_MAX));
 
   storage.SetEntryTypeValue("foo", value);
@@ -181,7 +180,7 @@
   }
 }
 
-TEST_P(StorageTestPopulateOne, SetEntryTypeValueAssignTypeChange) {
+TEST_P(StoragePopulateOneTest, SetEntryTypeValueAssignTypeChange) {
   // update with different type results in assignment message
   auto value = Value::MakeDouble(0.0);
 
@@ -191,14 +190,14 @@
                                 "foo", GetParam() ? 0 : 0xffff, 2, value, 0)),
                             IsNull(), IsNull()));
   EXPECT_CALL(notifier,
-              NotifyEntry(0, StringRef("foo"), value,
+              NotifyEntry(0, std::string_view("foo"), value,
                           NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL, UINT_MAX));
 
   storage.SetEntryTypeValue("foo", value);
   EXPECT_EQ(value, GetEntry("foo")->value);
 }
 
-TEST_P(StorageTestPopulateOne, SetEntryTypeValueEqualValue) {
+TEST_P(StoragePopulateOneTest, SetEntryTypeValueEqualValue) {
   // update with same type and same value: change value contents but no update
   // message is issued (minimizing bandwidth usage)
   auto value = Value::MakeBoolean(true);
@@ -206,7 +205,7 @@
   EXPECT_EQ(value, GetEntry("foo")->value);
 }
 
-TEST_P(StorageTestPopulated, SetEntryTypeValueDifferentValue) {
+TEST_P(StoragePopulatedTest, SetEntryTypeValueDifferentValue) {
   // update with same type and different value results in value update message
   auto value = Value::MakeDouble(1.0);
 
@@ -218,7 +217,7 @@
                               IsNull(), IsNull()));
   }
   EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), value,
+              NotifyEntry(1, std::string_view("foo2"), value,
                           NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL, UINT_MAX));
   storage.SetEntryTypeValue("foo2", value);
   EXPECT_EQ(value, GetEntry("foo2")->value);
@@ -229,20 +228,20 @@
   }
 }
 
-TEST_P(StorageTestEmpty, SetEntryTypeValueEmptyName) {
+TEST_P(StorageEmptyTest, SetEntryTypeValueEmptyName) {
   auto value = Value::MakeBoolean(true);
   storage.SetEntryTypeValue("", value);
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, SetEntryTypeValueEmptyValue) {
+TEST_P(StorageEmptyTest, SetEntryTypeValueEmptyValue) {
   storage.SetEntryTypeValue("foo", nullptr);
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, SetEntryValueAssignNew) {
+TEST_P(StorageEmptyTest, SetEntryValueAssignNew) {
   // brand new entry
   auto value = Value::MakeBoolean(true);
 
@@ -251,14 +250,14 @@
               QueueOutgoing(MessageEq(Message::EntryAssign(
                                 "foo", GetParam() ? 0 : 0xffff, 1, value, 0)),
                             IsNull(), IsNull()));
-  EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"), value,
+  EXPECT_CALL(notifier, NotifyEntry(0, std::string_view("foo"), value,
                                     NT_NOTIFY_NEW | NT_NOTIFY_LOCAL, UINT_MAX));
 
   EXPECT_TRUE(storage.SetEntryValue("foo", value));
   EXPECT_EQ(value, GetEntry("foo")->value);
 }
 
-TEST_P(StorageTestPopulateOne, SetEntryValueAssignTypeChange) {
+TEST_P(StoragePopulateOneTest, SetEntryValueAssignTypeChange) {
   // update with different type results in error and no message or notification
   auto value = Value::MakeDouble(0.0);
   EXPECT_FALSE(storage.SetEntryValue("foo", value));
@@ -266,7 +265,7 @@
   EXPECT_NE(value, entry->value);
 }
 
-TEST_P(StorageTestPopulateOne, SetEntryValueEqualValue) {
+TEST_P(StoragePopulateOneTest, SetEntryValueEqualValue) {
   // update with same type and same value: change value contents but no update
   // message is issued (minimizing bandwidth usage)
   auto value = Value::MakeBoolean(true);
@@ -275,7 +274,7 @@
   EXPECT_EQ(value, entry->value);
 }
 
-TEST_P(StorageTestPopulated, SetEntryValueDifferentValue) {
+TEST_P(StoragePopulatedTest, SetEntryValueDifferentValue) {
   // update with same type and different value results in value update message
   auto value = Value::MakeDouble(1.0);
 
@@ -287,7 +286,7 @@
                               IsNull(), IsNull()));
   }
   EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), value,
+              NotifyEntry(1, std::string_view("foo2"), value,
                           NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL, UINT_MAX));
 
   EXPECT_TRUE(storage.SetEntryValue("foo2", value));
@@ -300,20 +299,20 @@
   }
 }
 
-TEST_P(StorageTestEmpty, SetEntryValueEmptyName) {
+TEST_P(StorageEmptyTest, SetEntryValueEmptyName) {
   auto value = Value::MakeBoolean(true);
   EXPECT_TRUE(storage.SetEntryValue("", value));
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, SetEntryValueEmptyValue) {
+TEST_P(StorageEmptyTest, SetEntryValueEmptyValue) {
   EXPECT_TRUE(storage.SetEntryValue("foo", nullptr));
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, SetDefaultEntryAssignNew) {
+TEST_P(StorageEmptyTest, SetDefaultEntryAssignNew) {
   // brand new entry
   auto value = Value::MakeBoolean(true);
 
@@ -322,7 +321,7 @@
               QueueOutgoing(MessageEq(Message::EntryAssign(
                                 "foo", GetParam() ? 0 : 0xffff, 1, value, 0)),
                             IsNull(), IsNull()));
-  EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"), value,
+  EXPECT_CALL(notifier, NotifyEntry(0, std::string_view("foo"), value,
                                     NT_NOTIFY_NEW | NT_NOTIFY_LOCAL, UINT_MAX));
 
   auto ret_val = storage.SetDefaultEntryValue("foo", value);
@@ -330,7 +329,7 @@
   EXPECT_EQ(value, GetEntry("foo")->value);
 }
 
-TEST_P(StorageTestPopulateOne, SetDefaultEntryExistsSameType) {
+TEST_P(StoragePopulateOneTest, SetDefaultEntryExistsSameType) {
   // existing entry
   auto value = Value::MakeBoolean(true);
   auto ret_val = storage.SetDefaultEntryValue("foo", value);
@@ -338,7 +337,7 @@
   EXPECT_NE(value, GetEntry("foo")->value);
 }
 
-TEST_P(StorageTestPopulateOne, SetDefaultEntryExistsDifferentType) {
+TEST_P(StoragePopulateOneTest, SetDefaultEntryExistsDifferentType) {
   // existing entry is boolean
   auto value = Value::MakeDouble(2.0);
   auto ret_val = storage.SetDefaultEntryValue("foo", value);
@@ -347,7 +346,7 @@
   EXPECT_NE(value, GetEntry("foo")->value);
 }
 
-TEST_P(StorageTestEmpty, SetDefaultEntryEmptyName) {
+TEST_P(StorageEmptyTest, SetDefaultEntryEmptyName) {
   auto value = Value::MakeBoolean(true);
   auto ret_val = storage.SetDefaultEntryValue("", value);
   EXPECT_FALSE(ret_val);
@@ -361,7 +360,7 @@
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, SetDefaultEntryEmptyValue) {
+TEST_P(StorageEmptyTest, SetDefaultEntryEmptyValue) {
   auto value = Value::MakeBoolean(true);
   auto ret_val = storage.SetDefaultEntryValue("", nullptr);
   EXPECT_FALSE(ret_val);
@@ -375,38 +374,40 @@
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestPopulated, SetDefaultEntryEmptyName) {
+TEST_P(StoragePopulatedTest, SetDefaultEntryEmptyName) {
   auto value = Value::MakeBoolean(true);
   auto ret_val = storage.SetDefaultEntryValue("", value);
   EXPECT_FALSE(ret_val);
   // assert that no entries get added
   EXPECT_EQ(4u, entries().size());
-  if (GetParam())
+  if (GetParam()) {
     EXPECT_EQ(4u, idmap().size());
-  else
+  } else {
     EXPECT_EQ(0u, idmap().size());
+  }
 }
 
-TEST_P(StorageTestPopulated, SetDefaultEntryEmptyValue) {
+TEST_P(StoragePopulatedTest, SetDefaultEntryEmptyValue) {
   auto value = Value::MakeBoolean(true);
   auto ret_val = storage.SetDefaultEntryValue("", nullptr);
   EXPECT_FALSE(ret_val);
   // assert that no entries get added
   EXPECT_EQ(4u, entries().size());
-  if (GetParam())
+  if (GetParam()) {
     EXPECT_EQ(4u, idmap().size());
-  else
+  } else {
     EXPECT_EQ(0u, idmap().size());
+  }
 }
 
-TEST_P(StorageTestEmpty, SetEntryFlagsNew) {
+TEST_P(StorageEmptyTest, SetEntryFlagsNew) {
   // flags setting doesn't create an entry
   storage.SetEntryFlags("foo", 0u);
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestPopulateOne, SetEntryFlagsEqualValue) {
+TEST_P(StoragePopulateOneTest, SetEntryFlagsEqualValue) {
   // update with same value: no update message is issued (minimizing bandwidth
   // usage)
   storage.SetEntryFlags("foo", 0u);
@@ -414,7 +415,7 @@
   EXPECT_EQ(0u, entry->flags);
 }
 
-TEST_P(StorageTestPopulated, SetEntryFlagsDifferentValue) {
+TEST_P(StoragePopulatedTest, SetEntryFlagsDifferentValue) {
   // update with different value results in flags update message
   // client shouldn't send an update as id not assigned yet
   if (GetParam()) {
@@ -423,25 +424,25 @@
                                           IsNull(), IsNull()));
   }
   EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), _,
+              NotifyEntry(1, std::string_view("foo2"), _,
                           NT_NOTIFY_FLAGS | NT_NOTIFY_LOCAL, UINT_MAX));
   storage.SetEntryFlags("foo2", 1u);
   EXPECT_EQ(1u, GetEntry("foo2")->flags);
 }
 
-TEST_P(StorageTestEmpty, SetEntryFlagsEmptyName) {
+TEST_P(StorageEmptyTest, SetEntryFlagsEmptyName) {
   storage.SetEntryFlags("", 0u);
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, GetEntryFlagsNotExist) {
+TEST_P(StorageEmptyTest, GetEntryFlagsNotExist) {
   EXPECT_EQ(0u, storage.GetEntryFlags("foo"));
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestPopulateOne, GetEntryFlagsExist) {
+TEST_P(StoragePopulateOneTest, GetEntryFlagsExist) {
   EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _));
   storage.SetEntryFlags("foo", 1u);
@@ -449,18 +450,21 @@
   EXPECT_EQ(1u, storage.GetEntryFlags("foo"));
 }
 
-TEST_P(StorageTestEmpty, DeleteEntryNotExist) { storage.DeleteEntry("foo"); }
+TEST_P(StorageEmptyTest, DeleteEntryNotExist) {
+  storage.DeleteEntry("foo");
+}
 
-TEST_P(StorageTestPopulated, DeleteEntryExist) {
+TEST_P(StoragePopulatedTest, DeleteEntryExist) {
   // client shouldn't send an update as id not assigned yet
   if (GetParam()) {
     // id assigned as this is the server
     EXPECT_CALL(dispatcher, QueueOutgoing(MessageEq(Message::EntryDelete(1)),
                                           IsNull(), IsNull()));
   }
-  EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), ValueEq(Value::MakeDouble(0)),
-                          NT_NOTIFY_DELETE | NT_NOTIFY_LOCAL, UINT_MAX));
+  EXPECT_CALL(
+      notifier,
+      NotifyEntry(1, std::string_view("foo2"), ValueEq(Value::MakeDouble(0)),
+                  NT_NOTIFY_DELETE | NT_NOTIFY_LOCAL, UINT_MAX));
 
   storage.DeleteEntry("foo2");
   ASSERT_EQ(1u, entries().count("foo2"));
@@ -473,12 +477,12 @@
   }
 }
 
-TEST_P(StorageTestEmpty, DeleteAllEntriesEmpty) {
+TEST_P(StorageEmptyTest, DeleteAllEntriesEmpty) {
   storage.DeleteAllEntries();
   ASSERT_TRUE(entries().empty());
 }
 
-TEST_P(StorageTestPopulated, DeleteAllEntries) {
+TEST_P(StoragePopulatedTest, DeleteAllEntries) {
   EXPECT_CALL(dispatcher, QueueOutgoing(MessageEq(Message::ClearEntries()),
                                         IsNull(), IsNull()));
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, NT_NOTIFY_DELETE | NT_NOTIFY_LOCAL,
@@ -490,7 +494,7 @@
   EXPECT_EQ(nullptr, entries()["foo2"]->value);
 }
 
-TEST_P(StorageTestPopulated, DeleteAllEntriesPersistent) {
+TEST_P(StoragePopulatedTest, DeleteAllEntriesPersistent) {
   GetEntry("foo2")->flags = NT_PERSISTENT;
 
   EXPECT_CALL(dispatcher, QueueOutgoing(MessageEq(Message::ClearEntries()),
@@ -504,12 +508,12 @@
   EXPECT_NE(nullptr, entries()["foo2"]->value);
 }
 
-TEST_P(StorageTestPopulated, GetEntryInfoAll) {
+TEST_P(StoragePopulatedTest, GetEntryInfoAll) {
   auto info = storage.GetEntryInfo(0, "", 0u);
   ASSERT_EQ(4u, info.size());
 }
 
-TEST_P(StorageTestPopulated, GetEntryInfoPrefix) {
+TEST_P(StoragePopulatedTest, GetEntryInfoPrefix) {
   auto info = storage.GetEntryInfo(0, "foo", 0u);
   ASSERT_EQ(2u, info.size());
   if (info[0].name == "foo") {
@@ -525,7 +529,7 @@
   }
 }
 
-TEST_P(StorageTestPopulated, GetEntryInfoTypes) {
+TEST_P(StoragePopulatedTest, GetEntryInfoTypes) {
   auto info = storage.GetEntryInfo(0, "", NT_DOUBLE);
   ASSERT_EQ(2u, info.size());
   EXPECT_EQ(NT_DOUBLE, info[0].type);
@@ -539,101 +543,103 @@
   }
 }
 
-TEST_P(StorageTestPopulated, GetEntryInfoPrefixTypes) {
+TEST_P(StoragePopulatedTest, GetEntryInfoPrefixTypes) {
   auto info = storage.GetEntryInfo(0, "bar", NT_BOOLEAN);
   ASSERT_EQ(1u, info.size());
   EXPECT_EQ("bar2", info[0].name);
   EXPECT_EQ(NT_BOOLEAN, info[0].type);
 }
 
-TEST_P(StorageTestPersistent, SavePersistentEmpty) {
+TEST_P(StoragePersistentTest, SavePersistentEmpty) {
   wpi::SmallString<256> buf;
   wpi::raw_svector_ostream oss(buf);
   storage.SavePersistent(oss, false);
   ASSERT_EQ("[NetworkTables Storage 3.0]\n", oss.str());
 }
 
-TEST_P(StorageTestPersistent, SavePersistent) {
-  for (auto& i : entries()) i.getValue()->flags = NT_PERSISTENT;
+TEST_P(StoragePersistentTest, SavePersistent) {
+  for (auto& i : entries()) {
+    i.getValue()->flags = NT_PERSISTENT;
+  }
   wpi::SmallString<256> buf;
   wpi::raw_svector_ostream oss(buf);
   storage.SavePersistent(oss, false);
-  wpi::StringRef out = oss.str();
+  std::string_view out = oss.str();
   // std::fputs(out.c_str(), stderr);
-  wpi::StringRef line, rem = out;
-  std::tie(line, rem) = rem.split('\n');
+  std::string_view line, rem = out;
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("[NetworkTables Storage 3.0]", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("boolean \"\\x00\\x03\\x05\\n\"=true", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("boolean \"\\x3D\"=true", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("boolean \"boolean/false\"=false", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("boolean \"boolean/true\"=true", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array boolean \"booleanarr/empty\"=", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array boolean \"booleanarr/one\"=true", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array boolean \"booleanarr/two\"=true,false", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("double \"double/big\"=1.3e+08", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("double \"double/neg\"=-1.5", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("double \"double/zero\"=0", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array double \"doublearr/empty\"=", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array double \"doublearr/one\"=0.5", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array double \"doublearr/two\"=0.5,-0.25", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("raw \"raw/empty\"=", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("raw \"raw/normal\"=aGVsbG8=", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("raw \"raw/special\"=AAMFCg==", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("string \"string/empty\"=\"\"", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("string \"string/normal\"=\"hello\"", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("string \"string/quoted\"=\"\\\"a\\\"\"", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("string \"string/special\"=\"\\x00\\x03\\x05\\n\"", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array string \"stringarr/empty\"=", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array string \"stringarr/one\"=\"hello\"", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("array string \"stringarr/two\"=\"hello\",\"world\\n\"", line);
-  std::tie(line, rem) = rem.split('\n');
+  std::tie(line, rem) = wpi::split(rem, '\n');
   ASSERT_EQ("", line);
 }
 
-TEST_P(StorageTestEmpty, LoadPersistentBadHeader) {
+TEST_P(StorageEmptyTest, LoadPersistentBadHeader) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
   wpi::raw_mem_istream iss("");
   EXPECT_CALL(
       warn,
-      Warn(1, wpi::StringRef("header line mismatch, ignoring rest of file")));
+      Warn(1, std::string_view("header line mismatch, ignoring rest of file")));
   EXPECT_FALSE(storage.LoadEntries(iss, "", true, warn_func));
 
   wpi::raw_mem_istream iss2("[NetworkTables");
   EXPECT_CALL(
       warn,
-      Warn(1, wpi::StringRef("header line mismatch, ignoring rest of file")));
+      Warn(1, std::string_view("header line mismatch, ignoring rest of file")));
 
   EXPECT_FALSE(storage.LoadEntries(iss2, "", true, warn_func));
   EXPECT_TRUE(entries().empty());
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, LoadPersistentCommentHeader) {
+TEST_P(StorageEmptyTest, LoadPersistentCommentHeader) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -644,7 +650,7 @@
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, LoadPersistentEmptyName) {
+TEST_P(StorageEmptyTest, LoadPersistentEmptyName) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -654,7 +660,7 @@
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, LoadPersistentAssign) {
+TEST_P(StorageEmptyTest, LoadPersistentAssign) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -665,7 +671,7 @@
                                             "foo", GetParam() ? 0 : 0xffff, 1,
                                             value, NT_PERSISTENT)),
                                         IsNull(), IsNull()));
-  EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"),
+  EXPECT_CALL(notifier, NotifyEntry(0, std::string_view("foo"),
                                     ValueEq(Value::MakeBoolean(true)),
                                     NT_NOTIFY_NEW | NT_NOTIFY_LOCAL, UINT_MAX));
 
@@ -677,7 +683,7 @@
   EXPECT_EQ(NT_PERSISTENT, entry->flags);
 }
 
-TEST_P(StorageTestPopulated, LoadPersistentUpdateFlags) {
+TEST_P(StoragePopulatedTest, LoadPersistentUpdateFlags) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -688,9 +694,10 @@
                 QueueOutgoing(MessageEq(Message::FlagsUpdate(1, NT_PERSISTENT)),
                               IsNull(), IsNull()));
   }
-  EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), ValueEq(Value::MakeDouble(0)),
-                          NT_NOTIFY_FLAGS | NT_NOTIFY_LOCAL, UINT_MAX));
+  EXPECT_CALL(
+      notifier,
+      NotifyEntry(1, std::string_view("foo2"), ValueEq(Value::MakeDouble(0)),
+                  NT_NOTIFY_FLAGS | NT_NOTIFY_LOCAL, UINT_MAX));
 
   wpi::raw_mem_istream iss(
       "[NetworkTables Storage 3.0]\ndouble \"foo2\"=0.0\n");
@@ -700,7 +707,7 @@
   EXPECT_EQ(NT_PERSISTENT, entry->flags);
 }
 
-TEST_P(StorageTestPopulated, LoadPersistentUpdateValue) {
+TEST_P(StoragePopulatedTest, LoadPersistentUpdateValue) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -715,9 +722,10 @@
                 QueueOutgoing(MessageEq(Message::EntryUpdate(1, 2, value)),
                               IsNull(), IsNull()));
   }
-  EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), ValueEq(Value::MakeDouble(1)),
-                          NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL, UINT_MAX));
+  EXPECT_CALL(
+      notifier,
+      NotifyEntry(1, std::string_view("foo2"), ValueEq(Value::MakeDouble(1)),
+                  NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL, UINT_MAX));
 
   wpi::raw_mem_istream iss(
       "[NetworkTables Storage 3.0]\ndouble \"foo2\"=1.0\n");
@@ -732,7 +740,7 @@
   }
 }
 
-TEST_P(StorageTestPopulated, LoadPersistentUpdateValueFlags) {
+TEST_P(StoragePopulatedTest, LoadPersistentUpdateValueFlags) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -748,10 +756,11 @@
                 QueueOutgoing(MessageEq(Message::FlagsUpdate(1, NT_PERSISTENT)),
                               IsNull(), IsNull()));
   }
-  EXPECT_CALL(notifier,
-              NotifyEntry(1, StringRef("foo2"), ValueEq(Value::MakeDouble(1)),
-                          NT_NOTIFY_FLAGS | NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL,
-                          UINT_MAX));
+  EXPECT_CALL(
+      notifier,
+      NotifyEntry(1, std::string_view("foo2"), ValueEq(Value::MakeDouble(1)),
+                  NT_NOTIFY_FLAGS | NT_NOTIFY_UPDATE | NT_NOTIFY_LOCAL,
+                  UINT_MAX));
 
   wpi::raw_mem_istream iss(
       "[NetworkTables Storage 3.0]\ndouble \"foo2\"=1.0\n");
@@ -766,7 +775,7 @@
   }
 }
 
-TEST_P(StorageTestEmpty, LoadPersistent) {
+TEST_P(StorageEmptyTest, LoadPersistent) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
@@ -813,13 +822,13 @@
   EXPECT_EQ(*Value::MakeString(""), *storage.GetEntryValue("string/empty"));
   EXPECT_EQ(*Value::MakeString("hello"),
             *storage.GetEntryValue("string/normal"));
-  EXPECT_EQ(*Value::MakeString(StringRef("\0\3\5\n", 4)),
+  EXPECT_EQ(*Value::MakeString(std::string_view("\0\3\5\n", 4)),
             *storage.GetEntryValue("string/special"));
   EXPECT_EQ(*Value::MakeString("\"a\""),
             *storage.GetEntryValue("string/quoted"));
   EXPECT_EQ(*Value::MakeRaw(""), *storage.GetEntryValue("raw/empty"));
   EXPECT_EQ(*Value::MakeRaw("hello"), *storage.GetEntryValue("raw/normal"));
-  EXPECT_EQ(*Value::MakeRaw(StringRef("\0\3\5\n", 4)),
+  EXPECT_EQ(*Value::MakeRaw(std::string_view("\0\3\5\n", 4)),
             *storage.GetEntryValue("raw/special"));
   EXPECT_EQ(*Value::MakeBooleanArray(std::vector<int>{}),
             *storage.GetEntryValue("booleanarr/empty"));
@@ -841,18 +850,18 @@
       *Value::MakeStringArray(std::vector<std::string>{"hello", "world\n"}),
       *storage.GetEntryValue("stringarr/two"));
   EXPECT_EQ(*Value::MakeBoolean(true),
-            *storage.GetEntryValue(StringRef("\0\3\5\n", 4)));
+            *storage.GetEntryValue(std::string_view("\0\3\5\n", 4)));
   EXPECT_EQ(*Value::MakeBoolean(true), *storage.GetEntryValue("="));
 }
 
-TEST_P(StorageTestEmpty, LoadPersistentWarn) {
+TEST_P(StorageEmptyTest, LoadPersistentWarn) {
   MockLoadWarn warn;
   auto warn_func = [&](size_t line, const char* msg) { warn.Warn(line, msg); };
 
   wpi::raw_mem_istream iss(
       "[NetworkTables Storage 3.0]\nboolean \"foo\"=foo\n");
   EXPECT_CALL(
-      warn, Warn(2, wpi::StringRef(
+      warn, Warn(2, std::string_view(
                         "unrecognized boolean value, not 'true' or 'false'")));
   EXPECT_TRUE(storage.LoadEntries(iss, "", true, warn_func));
 
@@ -860,7 +869,7 @@
   EXPECT_TRUE(idmap().empty());
 }
 
-TEST_P(StorageTestEmpty, ProcessIncomingEntryAssign) {
+TEST_P(StorageEmptyTest, ProcessIncomingEntryAssign) {
   auto conn = std::make_shared<MockNetworkConnection>();
   auto value = Value::MakeDouble(1.0);
   if (GetParam()) {
@@ -870,7 +879,7 @@
         QueueOutgoing(MessageEq(Message::EntryAssign("foo", 0, 0, value, 0)),
                       IsNull(), IsNull()));
   }
-  EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"), ValueEq(value),
+  EXPECT_CALL(notifier, NotifyEntry(0, std::string_view("foo"), ValueEq(value),
                                     NT_NOTIFY_NEW, UINT_MAX));
 
   storage.ProcessIncoming(
@@ -878,7 +887,7 @@
       conn.get(), conn);
 }
 
-TEST_P(StorageTestPopulateOne, ProcessIncomingEntryAssign) {
+TEST_P(StoragePopulateOneTest, ProcessIncomingEntryAssign) {
   auto conn = std::make_shared<MockNetworkConnection>();
   auto value = Value::MakeDouble(1.0);
   EXPECT_CALL(*conn, proto_rev()).WillRepeatedly(Return(0x0300u));
@@ -889,21 +898,21 @@
         QueueOutgoing(MessageEq(Message::EntryAssign("foo", 0, 1, value, 0)),
                       IsNull(), conn.get()));
   }
-  EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"), ValueEq(value),
+  EXPECT_CALL(notifier, NotifyEntry(0, std::string_view("foo"), ValueEq(value),
                                     NT_NOTIFY_UPDATE, UINT_MAX));
 
   storage.ProcessIncoming(Message::EntryAssign("foo", 0, 1, value, 0),
                           conn.get(), conn);
 }
 
-TEST_P(StorageTestPopulateOne, ProcessIncomingEntryAssignIgnore) {
+TEST_P(StoragePopulateOneTest, ProcessIncomingEntryAssignIgnore) {
   auto conn = std::make_shared<MockNetworkConnection>();
   auto value = Value::MakeDouble(1.0);
   storage.ProcessIncoming(Message::EntryAssign("foo", 0xffff, 1, value, 0),
                           conn.get(), conn);
 }
 
-TEST_P(StorageTestPopulateOne, ProcessIncomingEntryAssignWithFlags) {
+TEST_P(StoragePopulateOneTest, ProcessIncomingEntryAssignWithFlags) {
   auto conn = std::make_shared<MockNetworkConnection>();
   auto value = Value::MakeDouble(1.0);
   EXPECT_CALL(*conn, proto_rev()).WillRepeatedly(Return(0x0300u));
@@ -914,22 +923,23 @@
         QueueOutgoing(MessageEq(Message::EntryAssign("foo", 0, 1, value, 0x2)),
                       IsNull(), conn.get()));
     EXPECT_CALL(notifier,
-                NotifyEntry(0, StringRef("foo"), ValueEq(value),
+                NotifyEntry(0, std::string_view("foo"), ValueEq(value),
                             NT_NOTIFY_UPDATE | NT_NOTIFY_FLAGS, UINT_MAX));
   } else {
     // client forces flags back when an assign message is received for an
     // existing entry with different flags
     EXPECT_CALL(dispatcher, QueueOutgoing(MessageEq(Message::FlagsUpdate(0, 0)),
                                           IsNull(), IsNull()));
-    EXPECT_CALL(notifier, NotifyEntry(0, StringRef("foo"), ValueEq(value),
-                                      NT_NOTIFY_UPDATE, UINT_MAX));
+    EXPECT_CALL(notifier,
+                NotifyEntry(0, std::string_view("foo"), ValueEq(value),
+                            NT_NOTIFY_UPDATE, UINT_MAX));
   }
 
   storage.ProcessIncoming(Message::EntryAssign("foo", 0, 1, value, 0x2),
                           conn.get(), conn);
 }
 
-TEST_P(StorageTestPopulateOne, DeleteCheckHandle) {
+TEST_P(StoragePopulateOneTest, DeleteCheckHandle) {
   EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
   auto handle = storage.GetEntry("foo");
@@ -942,7 +952,7 @@
   ASSERT_EQ(handle, handle2);
 }
 
-TEST_P(StorageTestPopulateOne, DeletedEntryFlags) {
+TEST_P(StoragePopulateOneTest, DeletedEntryFlags) {
   EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
   auto handle = storage.GetEntry("foo");
@@ -959,7 +969,7 @@
   EXPECT_EQ(storage.GetEntryFlags(handle), 0u);
 }
 
-TEST_P(StorageTestPopulateOne, DeletedDeleteAllEntries) {
+TEST_P(StoragePopulateOneTest, DeletedDeleteAllEntries) {
   EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
   storage.DeleteEntry("foo");
@@ -971,7 +981,7 @@
   storage.DeleteAllEntries();
 }
 
-TEST_P(StorageTestPopulateOne, DeletedGetEntries) {
+TEST_P(StoragePopulateOneTest, DeletedGetEntries) {
   EXPECT_CALL(dispatcher, QueueOutgoing(_, _, _)).Times(AnyNumber());
   EXPECT_CALL(notifier, NotifyEntry(_, _, _, _, _)).Times(AnyNumber());
   storage.DeleteEntry("foo");
@@ -981,13 +991,13 @@
   EXPECT_TRUE(storage.GetEntries("", 0).empty());
 }
 
-INSTANTIATE_TEST_SUITE_P(StorageTestsEmpty, StorageTestEmpty,
+INSTANTIATE_TEST_SUITE_P(StorageEmptyTests, StorageEmptyTest,
                          ::testing::Bool());
-INSTANTIATE_TEST_SUITE_P(StorageTestsPopulateOne, StorageTestPopulateOne,
+INSTANTIATE_TEST_SUITE_P(StoragePopulateOneTests, StoragePopulateOneTest,
                          ::testing::Bool());
-INSTANTIATE_TEST_SUITE_P(StorageTestsPopulated, StorageTestPopulated,
+INSTANTIATE_TEST_SUITE_P(StoragePopulatedTests, StoragePopulatedTest,
                          ::testing::Bool());
-INSTANTIATE_TEST_SUITE_P(StorageTestsPersistent, StorageTestPersistent,
+INSTANTIATE_TEST_SUITE_P(StoragePersistentTests, StoragePersistentTest,
                          ::testing::Bool());
 
 }  // namespace nt
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.h b/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.h
index 1bb8a8c..3a1755f 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/StorageTest.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_STORAGETEST_H_
 #define NTCORE_STORAGETEST_H_
@@ -27,7 +24,7 @@
   Storage::EntriesMap& entries() { return storage.m_entries; }
   Storage::IdMap& idmap() { return storage.m_idmap; }
 
-  Storage::Entry* GetEntry(StringRef name) {
+  Storage::Entry* GetEntry(std::string_view name) {
     auto i = storage.m_entries.find(name);
     return i == storage.m_entries.end() ? &tmp_entry : i->getValue();
   }
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.cpp
index 3368b8c..49b407b 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "TestPrinters.h"
 
@@ -129,7 +126,7 @@
       *os << value.GetDouble();
       break;
     case NT_STRING:
-      *os << '"' << value.GetString().str() << '"';
+      *os << '"' << value.GetString() << '"';
       break;
     case NT_RAW:
       *os << ::testing::PrintToString(value.GetRaw());
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.h b/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.h
index 2976c1c..47c9eb4 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/TestPrinters.h
@@ -1,24 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_TESTPRINTERS_H_
 #define NTCORE_TESTPRINTERS_H_
 
 #include <memory>
 #include <ostream>
-
-#include <wpi/StringRef.h>
+#include <string>
+#include <string_view>
 
 #include "gtest/gtest.h"
 
 namespace wpi {
 
-inline void PrintTo(StringRef str, ::std::ostream* os) {
-  ::testing::internal::PrintStringTo(str.str(), os);
+inline void PrintTo(std::string_view str, ::std::ostream* os) {
+  ::testing::internal::PrintStringTo(std::string{str}, os);
 }
 
 }  // namespace wpi
@@ -37,7 +34,9 @@
 
 inline void PrintTo(std::shared_ptr<Message> msg, std::ostream* os) {
   *os << "shared_ptr{";
-  if (msg) PrintTo(*msg, os);
+  if (msg) {
+    PrintTo(*msg, os);
+  }
   *os << '}';
 }
 
@@ -45,7 +44,9 @@
 
 inline void PrintTo(std::shared_ptr<Value> value, std::ostream* os) {
   *os << "shared_ptr{";
-  if (value) PrintTo(*value, os);
+  if (value) {
+    PrintTo(*value, os);
+  }
   *os << '}';
 }
 
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.cpp
index 45b09ed..1f55c01 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ValueMatcher.h"
 
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.h b/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.h
index 5c417d7..ab68448 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.h
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/ValueMatcher.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef NTCORE_VALUEMATCHER_H_
 #define NTCORE_VALUEMATCHER_H_
 
 #include <memory>
 #include <ostream>
+#include <utility>
 
 #include "gmock/gmock.h"
 #include "networktables/NetworkTableValue.h"
@@ -19,7 +17,8 @@
 class ValueMatcher
     : public ::testing::MatcherInterface<std::shared_ptr<Value>> {
  public:
-  explicit ValueMatcher(std::shared_ptr<Value> goodval_) : goodval(goodval_) {}
+  explicit ValueMatcher(std::shared_ptr<Value> goodval_)
+      : goodval(std::move(goodval_)) {}
 
   bool MatchAndExplain(std::shared_ptr<Value> msg,
                        ::testing::MatchResultListener* listener) const override;
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/ValueTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/ValueTest.cpp
index 57d1c38..a9f2218 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/ValueTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/ValueTest.cpp
@@ -1,20 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <algorithm>
+#include <string_view>
 
 #include "TestPrinters.h"
 #include "Value_internal.h"
 #include "gtest/gtest.h"
 #include "networktables/NetworkTableValue.h"
 
+using namespace std::string_view_literals;
+
+namespace wpi {
+template <typename T>
+inline bool operator==(span<T> lhs, span<T> rhs) {
+  if (lhs.size() != rhs.size()) {
+    return false;
+  }
+  return std::equal(lhs.begin(), lhs.end(), rhs.begin());
+}
+}  // namespace wpi
+
 namespace nt {
 
 class ValueTest : public ::testing::Test {};
 
-typedef ValueTest ValueDeathTest;
+using ValueDeathTest = ValueTest;
 
 TEST_F(ValueTest, ConstructEmpty) {
   Value v;
@@ -34,6 +46,7 @@
   v = Value::MakeBoolean(true);
   ASSERT_EQ(NT_BOOLEAN, v->type());
   ASSERT_TRUE(v->GetBoolean());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_BOOLEAN, cv.type);
   ASSERT_EQ(1, cv.data.v_boolean);
@@ -54,6 +67,7 @@
   v = Value::MakeDouble(0.25);
   ASSERT_EQ(NT_DOUBLE, v->type());
   ASSERT_EQ(0.25, v->GetDouble());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_DOUBLE, cv.type);
   ASSERT_EQ(0.25, cv.data.v_double);
@@ -69,15 +83,16 @@
   NT_InitValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_STRING, cv.type);
-  ASSERT_EQ(wpi::StringRef("hello"), cv.data.v_string.str);
+  ASSERT_EQ("hello"sv, cv.data.v_string.str);
   ASSERT_EQ(5u, cv.data.v_string.len);
 
   v = Value::MakeString("goodbye");
   ASSERT_EQ(NT_STRING, v->type());
   ASSERT_EQ("goodbye", v->GetString());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_STRING, cv.type);
-  ASSERT_EQ(wpi::StringRef("goodbye"), cv.data.v_string.str);
+  ASSERT_EQ("goodbye"sv, cv.data.v_string.str);
   ASSERT_EQ(7u, cv.data.v_string.len);
 
   NT_DisposeValue(&cv);
@@ -91,15 +106,16 @@
   NT_InitValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_RAW, cv.type);
-  ASSERT_EQ(wpi::StringRef("hello"), cv.data.v_string.str);
+  ASSERT_EQ("hello"sv, cv.data.v_string.str);
   ASSERT_EQ(5u, cv.data.v_string.len);
 
   v = Value::MakeRaw("goodbye");
   ASSERT_EQ(NT_RAW, v->type());
   ASSERT_EQ("goodbye", v->GetRaw());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_RAW, cv.type);
-  ASSERT_EQ(wpi::StringRef("goodbye"), cv.data.v_string.str);
+  ASSERT_EQ("goodbye"sv, cv.data.v_string.str);
   ASSERT_EQ(7u, cv.data.v_string.len);
 
   NT_DisposeValue(&cv);
@@ -109,7 +125,7 @@
   std::vector<int> vec{1, 0, 1};
   auto v = Value::MakeBooleanArray(vec);
   ASSERT_EQ(NT_BOOLEAN_ARRAY, v->type());
-  ASSERT_EQ(wpi::ArrayRef<int>(vec), v->GetBooleanArray());
+  ASSERT_EQ(wpi::span<const int>(vec), v->GetBooleanArray());
   NT_Value cv;
   NT_InitValue(&cv);
   ConvertToC(*v, &cv);
@@ -123,7 +139,8 @@
   vec = {0, 1, 0};
   v = Value::MakeBooleanArray(vec);
   ASSERT_EQ(NT_BOOLEAN_ARRAY, v->type());
-  ASSERT_EQ(wpi::ArrayRef<int>(vec), v->GetBooleanArray());
+  ASSERT_EQ(wpi::span<const int>(vec), v->GetBooleanArray());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_BOOLEAN_ARRAY, cv.type);
   ASSERT_EQ(3u, cv.data.arr_boolean.size);
@@ -135,7 +152,8 @@
   vec = {1, 0};
   v = Value::MakeBooleanArray(vec);
   ASSERT_EQ(NT_BOOLEAN_ARRAY, v->type());
-  ASSERT_EQ(wpi::ArrayRef<int>(vec), v->GetBooleanArray());
+  ASSERT_EQ(wpi::span<const int>(vec), v->GetBooleanArray());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_BOOLEAN_ARRAY, cv.type);
   ASSERT_EQ(2u, cv.data.arr_boolean.size);
@@ -149,7 +167,7 @@
   std::vector<double> vec{0.5, 0.25, 0.5};
   auto v = Value::MakeDoubleArray(vec);
   ASSERT_EQ(NT_DOUBLE_ARRAY, v->type());
-  ASSERT_EQ(wpi::ArrayRef<double>(vec), v->GetDoubleArray());
+  ASSERT_EQ(wpi::span<const double>(vec), v->GetDoubleArray());
   NT_Value cv;
   NT_InitValue(&cv);
   ConvertToC(*v, &cv);
@@ -163,7 +181,8 @@
   vec = {0.25, 0.5, 0.25};
   v = Value::MakeDoubleArray(vec);
   ASSERT_EQ(NT_DOUBLE_ARRAY, v->type());
-  ASSERT_EQ(wpi::ArrayRef<double>(vec), v->GetDoubleArray());
+  ASSERT_EQ(wpi::span<const double>(vec), v->GetDoubleArray());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_DOUBLE_ARRAY, cv.type);
   ASSERT_EQ(3u, cv.data.arr_double.size);
@@ -175,7 +194,8 @@
   vec = {0.5, 0.25};
   v = Value::MakeDoubleArray(vec);
   ASSERT_EQ(NT_DOUBLE_ARRAY, v->type());
-  ASSERT_EQ(wpi::ArrayRef<double>(vec), v->GetDoubleArray());
+  ASSERT_EQ(wpi::span<const double>(vec), v->GetDoubleArray());
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_DOUBLE_ARRAY, cv.type);
   ASSERT_EQ(2u, cv.data.arr_double.size);
@@ -193,17 +213,17 @@
   auto v = Value::MakeStringArray(std::move(vec));
   ASSERT_EQ(NT_STRING_ARRAY, v->type());
   ASSERT_EQ(3u, v->GetStringArray().size());
-  ASSERT_EQ(wpi::StringRef("hello"), v->GetStringArray()[0]);
-  ASSERT_EQ(wpi::StringRef("goodbye"), v->GetStringArray()[1]);
-  ASSERT_EQ(wpi::StringRef("string"), v->GetStringArray()[2]);
+  ASSERT_EQ("hello"sv, v->GetStringArray()[0]);
+  ASSERT_EQ("goodbye"sv, v->GetStringArray()[1]);
+  ASSERT_EQ("string"sv, v->GetStringArray()[2]);
   NT_Value cv;
   NT_InitValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_STRING_ARRAY, cv.type);
   ASSERT_EQ(3u, cv.data.arr_string.size);
-  ASSERT_EQ(wpi::StringRef("hello"), cv.data.arr_string.arr[0].str);
-  ASSERT_EQ(wpi::StringRef("goodbye"), cv.data.arr_string.arr[1].str);
-  ASSERT_EQ(wpi::StringRef("string"), cv.data.arr_string.arr[2].str);
+  ASSERT_EQ("hello"sv, cv.data.arr_string.arr[0].str);
+  ASSERT_EQ("goodbye"sv, cv.data.arr_string.arr[1].str);
+  ASSERT_EQ("string"sv, cv.data.arr_string.arr[2].str);
 
   // assign with same size
   vec.clear();
@@ -213,15 +233,16 @@
   v = Value::MakeStringArray(vec);
   ASSERT_EQ(NT_STRING_ARRAY, v->type());
   ASSERT_EQ(3u, v->GetStringArray().size());
-  ASSERT_EQ(wpi::StringRef("s1"), v->GetStringArray()[0]);
-  ASSERT_EQ(wpi::StringRef("str2"), v->GetStringArray()[1]);
-  ASSERT_EQ(wpi::StringRef("string3"), v->GetStringArray()[2]);
+  ASSERT_EQ("s1"sv, v->GetStringArray()[0]);
+  ASSERT_EQ("str2"sv, v->GetStringArray()[1]);
+  ASSERT_EQ("string3"sv, v->GetStringArray()[2]);
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_STRING_ARRAY, cv.type);
   ASSERT_EQ(3u, cv.data.arr_string.size);
-  ASSERT_EQ(wpi::StringRef("s1"), cv.data.arr_string.arr[0].str);
-  ASSERT_EQ(wpi::StringRef("str2"), cv.data.arr_string.arr[1].str);
-  ASSERT_EQ(wpi::StringRef("string3"), cv.data.arr_string.arr[2].str);
+  ASSERT_EQ("s1"sv, cv.data.arr_string.arr[0].str);
+  ASSERT_EQ("str2"sv, cv.data.arr_string.arr[1].str);
+  ASSERT_EQ("string3"sv, cv.data.arr_string.arr[2].str);
 
   // assign with different size
   vec.clear();
@@ -230,13 +251,14 @@
   v = Value::MakeStringArray(std::move(vec));
   ASSERT_EQ(NT_STRING_ARRAY, v->type());
   ASSERT_EQ(2u, v->GetStringArray().size());
-  ASSERT_EQ(wpi::StringRef("short"), v->GetStringArray()[0]);
-  ASSERT_EQ(wpi::StringRef("er"), v->GetStringArray()[1]);
+  ASSERT_EQ("short"sv, v->GetStringArray()[0]);
+  ASSERT_EQ("er"sv, v->GetStringArray()[1]);
+  NT_DisposeValue(&cv);
   ConvertToC(*v, &cv);
   ASSERT_EQ(NT_STRING_ARRAY, cv.type);
   ASSERT_EQ(2u, cv.data.arr_string.size);
-  ASSERT_EQ(wpi::StringRef("short"), cv.data.arr_string.arr[0].str);
-  ASSERT_EQ(wpi::StringRef("er"), cv.data.arr_string.arr[1].str);
+  ASSERT_EQ("short"sv, cv.data.arr_string.arr[0].str);
+  ASSERT_EQ("er"sv, cv.data.arr_string.arr[1].str);
 
   NT_DisposeValue(&cv);
 }
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/WireDecoderTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/WireDecoderTest.cpp
index e32f909..313fd98 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/WireDecoderTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/WireDecoderTest.cpp
@@ -1,22 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <stdint.h>
 
 #include <cfloat>
 #include <climits>
 #include <string>
-
-#include <wpi/StringRef.h>
+#include <string_view>
 
 #include "TestPrinters.h"
 #include "WireDecoder.h"
 #include "gtest/gtest.h"
 
+using namespace std::string_view_literals;
+
 namespace nt {
 
 class WireDecoderTest : public ::testing::Test {
@@ -24,8 +22,8 @@
   WireDecoderTest() {
     v_boolean = Value::MakeBoolean(true);
     v_double = Value::MakeDouble(1.0);
-    v_string = Value::MakeString(wpi::StringRef("hello"));
-    v_raw = Value::MakeRaw(wpi::StringRef("hello"));
+    v_string = Value::MakeString("hello"sv);
+    v_raw = Value::MakeRaw("hello"sv);
     v_boolean_array = Value::MakeBooleanArray(std::vector<int>{0, 1, 0});
     v_boolean_array_big = Value::MakeBooleanArray(std::vector<int>(255));
     v_double_array = Value::MakeDoubleArray(std::vector<double>{0.5, 0.25});
@@ -37,7 +35,9 @@
     v_string_array = Value::MakeStringArray(std::move(sa));
 
     sa.clear();
-    for (int i = 0; i < 255; ++i) sa.push_back("h");
+    for (int i = 0; i < 255; ++i) {
+      sa.push_back("h");
+    }
     v_string_array_big = Value::MakeStringArray(std::move(sa));
 
     s_normal = std::string("hello");
@@ -269,7 +269,7 @@
   ASSERT_TRUE(static_cast<bool>(val));
   EXPECT_EQ(*v_string, *val);
 
-  auto v_bye = Value::MakeString(wpi::StringRef("bye"));
+  auto v_bye = Value::MakeString("bye"sv);
   val = d.ReadValue(NT_STRING);
   ASSERT_TRUE(static_cast<bool>(val));
   EXPECT_EQ(*v_bye, *val);
@@ -367,7 +367,9 @@
 TEST_F(WireDecoderTest, ReadStringArrayBigValue2) {
   std::string s;
   s.push_back('\xff');
-  for (int i = 0; i < 255; ++i) s.append("\x00\x01h", 3);
+  for (int i = 0; i < 255; ++i) {
+    s.append("\x00\x01h", 3);
+  }
   wpi::raw_mem_istream is(s.data(), s.size());
   wpi::Logger logger;
   WireDecoder d(is, 0x0200u, logger);
@@ -442,7 +444,7 @@
   ASSERT_TRUE(static_cast<bool>(val));
   EXPECT_EQ(*v_string, *val);
 
-  auto v_bye = Value::MakeString(wpi::StringRef("bye"));
+  auto v_bye = Value::MakeString("bye"sv);
   val = d.ReadValue(NT_STRING);
   ASSERT_TRUE(static_cast<bool>(val));
   EXPECT_EQ(*v_bye, *val);
@@ -466,7 +468,7 @@
   ASSERT_TRUE(static_cast<bool>(val));
   EXPECT_EQ(*v_raw, *val);
 
-  auto v_bye = Value::MakeRaw(wpi::StringRef("bye"));
+  auto v_bye = Value::MakeRaw("bye"sv);
   val = d.ReadValue(NT_RAW);
   ASSERT_TRUE(static_cast<bool>(val));
   EXPECT_EQ(*v_bye, *val);
@@ -564,7 +566,9 @@
 TEST_F(WireDecoderTest, ReadStringArrayBigValue3) {
   std::string s;
   s.push_back('\xff');
-  for (int i = 0; i < 255; ++i) s.append("\x01h", 2);
+  for (int i = 0; i < 255; ++i) {
+    s.append("\x01h", 2);
+  }
   wpi::raw_mem_istream is(s.data(), s.size());
   wpi::Logger logger;
   WireDecoder d(is, 0x0300u, logger);
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/WireEncoderTest.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/WireEncoderTest.cpp
index fab5a22..b714112 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/WireEncoderTest.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/WireEncoderTest.cpp
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cfloat>
 #include <climits>
 #include <string>
-
-#include <wpi/StringRef.h>
+#include <string_view>
 
 #include "TestPrinters.h"
 #include "WireEncoder.h"
@@ -17,6 +13,8 @@
 
 #define BUFSIZE 1024
 
+using namespace std::string_view_literals;
+
 namespace nt {
 
 class WireEncoderTest : public ::testing::Test {
@@ -25,8 +23,8 @@
     v_empty = std::make_shared<Value>();
     v_boolean = Value::MakeBoolean(true);
     v_double = Value::MakeDouble(1.0);
-    v_string = Value::MakeString(wpi::StringRef("hello"));
-    v_raw = Value::MakeRaw(wpi::StringRef("hello"));
+    v_string = Value::MakeString("hello"sv);
+    v_raw = Value::MakeRaw("hello"sv);
     v_boolean_array = Value::MakeBooleanArray(std::vector<int>{0, 1, 0});
     v_boolean_array_big = Value::MakeBooleanArray(std::vector<int>(256));
     v_double_array = Value::MakeDoubleArray(std::vector<double>{0.5, 0.25});
@@ -38,7 +36,9 @@
     v_string_array = Value::MakeStringArray(std::move(sa));
 
     sa.clear();
-    for (int i = 0; i < 256; ++i) sa.push_back("h");
+    for (int i = 0; i < 256; ++i) {
+      sa.push_back("h");
+    }
     v_string_array_big = Value::MakeStringArray(std::move(sa));
 
     s_normal = "hello";
@@ -77,48 +77,55 @@
 TEST_F(WireEncoderTest, Write8) {
   size_t off = BUFSIZE - 1;
   WireEncoder e(0x0300u);
-  for (size_t i = 0; i < off; ++i) e.Write8(0u);  // test across Reserve()
+  for (size_t i = 0; i < off; ++i) {
+    e.Write8(0u);  // test across Reserve()
+  }
   e.Write8(5u);
   e.Write8(0x101u);  // should be truncated
   e.Write8(0u);
   ASSERT_EQ(3u, e.size() - off);
-  ASSERT_EQ(wpi::StringRef("\x05\x01\x00", 3),
-            wpi::StringRef(e.data(), e.size()).substr(off));
+  ASSERT_EQ("\x05\x01\x00"sv, std::string_view(e.data(), e.size()).substr(off));
 }
 
 TEST_F(WireEncoderTest, Write16) {
   size_t off = BUFSIZE - 2;
   WireEncoder e(0x0300u);
-  for (size_t i = 0; i < off; ++i) e.Write8(0u);  // test across Reserve()
+  for (size_t i = 0; i < off; ++i) {
+    e.Write8(0u);  // test across Reserve()
+  }
   e.Write16(5u);
   e.Write16(0x10001u);  // should be truncated
   e.Write16(0x4567u);
   e.Write16(0u);
   ASSERT_EQ(8u, e.size() - off);
-  ASSERT_EQ(wpi::StringRef("\x00\x05\x00\x01\x45\x67\x00\x00", 8),
-            wpi::StringRef(e.data(), e.size()).substr(off));
+  ASSERT_EQ("\x00\x05\x00\x01\x45\x67\x00\x00"sv,
+            std::string_view(e.data(), e.size()).substr(off));
 }
 
 TEST_F(WireEncoderTest, Write32) {
   size_t off = BUFSIZE - 4;
   WireEncoder e(0x0300u);
-  for (size_t i = 0; i < off; ++i) e.Write8(0u);  // test across Reserve()
+  for (size_t i = 0; i < off; ++i) {
+    e.Write8(0u);  // test across Reserve()
+  }
   e.Write32(5ul);
   e.Write32(1ul);
   e.Write32(0xabcdul);
   e.Write32(0x12345678ul);
   e.Write32(0ul);
   ASSERT_EQ(20u, e.size() - off);
-  ASSERT_EQ(wpi::StringRef("\x00\x00\x00\x05\x00\x00\x00\x01\x00\x00\xab\xcd"
-                           "\x12\x34\x56\x78\x00\x00\x00\x00",
-                           20),
-            wpi::StringRef(e.data(), e.size()).substr(off));
+  ASSERT_EQ(std::string_view("\x00\x00\x00\x05\x00\x00\x00\x01\x00\x00\xab\xcd"
+                             "\x12\x34\x56\x78\x00\x00\x00\x00",
+                             20),
+            std::string_view(e.data(), e.size()).substr(off));
 }
 
 TEST_F(WireEncoderTest, WriteDouble) {
   size_t off = BUFSIZE - 8;
   WireEncoder e(0x0300u);
-  for (size_t i = 0; i < off; ++i) e.Write8(0u);  // test across Reserve()
+  for (size_t i = 0; i < off; ++i) {
+    e.Write8(0u);  // test across Reserve()
+  }
   e.WriteDouble(0.0);
   e.WriteDouble(2.3e5);
   e.WriteDouble(std::numeric_limits<double>::infinity());
@@ -127,31 +134,35 @@
   ASSERT_EQ(40u, e.size() - off);
   // golden values except min and max from
   // http://www.binaryconvert.com/result_double.html
-  ASSERT_EQ(wpi::StringRef("\x00\x00\x00\x00\x00\x00\x00\x00"
-                           "\x41\x0c\x13\x80\x00\x00\x00\x00"
-                           "\x7f\xf0\x00\x00\x00\x00\x00\x00"
-                           "\x00\x10\x00\x00\x00\x00\x00\x00"
-                           "\x7f\xef\xff\xff\xff\xff\xff\xff",
-                           40),
-            wpi::StringRef(e.data(), e.size()).substr(off));
+  ASSERT_EQ(std::string_view("\x00\x00\x00\x00\x00\x00\x00\x00"
+                             "\x41\x0c\x13\x80\x00\x00\x00\x00"
+                             "\x7f\xf0\x00\x00\x00\x00\x00\x00"
+                             "\x00\x10\x00\x00\x00\x00\x00\x00"
+                             "\x7f\xef\xff\xff\xff\xff\xff\xff",
+                             40),
+            std::string_view(e.data(), e.size()).substr(off));
 }
 
 TEST_F(WireEncoderTest, WriteUleb128) {
   size_t off = BUFSIZE - 2;
   WireEncoder e(0x0300u);
-  for (size_t i = 0; i < off; ++i) e.Write8(0u);  // test across Reserve()
+  for (size_t i = 0; i < off; ++i) {
+    e.Write8(0u);  // test across Reserve()
+  }
   e.WriteUleb128(0ul);
   e.WriteUleb128(0x7ful);
   e.WriteUleb128(0x80ul);
   ASSERT_EQ(4u, e.size() - off);
-  ASSERT_EQ(wpi::StringRef("\x00\x7f\x80\x01", 4),
-            wpi::StringRef(e.data(), e.size()).substr(off));
+  ASSERT_EQ("\x00\x7f\x80\x01"sv,
+            std::string_view(e.data(), e.size()).substr(off));
 }
 
 TEST_F(WireEncoderTest, WriteType) {
   size_t off = BUFSIZE - 1;
   WireEncoder e(0x0300u);
-  for (size_t i = 0; i < off; ++i) e.Write8(0u);  // test across Reserve()
+  for (size_t i = 0; i < off; ++i) {
+    e.Write8(0u);  // test across Reserve()
+  }
   e.WriteType(NT_BOOLEAN);
   e.WriteType(NT_DOUBLE);
   e.WriteType(NT_STRING);
@@ -162,8 +173,8 @@
   e.WriteType(NT_RPC);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(8u, e.size() - off);
-  ASSERT_EQ(wpi::StringRef("\x00\x01\x02\x03\x10\x11\x12\x20", 8),
-            wpi::StringRef(e.data(), e.size()).substr(off));
+  ASSERT_EQ("\x00\x01\x02\x03\x10\x11\x12\x20"sv,
+            std::string_view(e.data(), e.size()).substr(off));
 }
 
 TEST_F(WireEncoderTest, WriteTypeError) {
@@ -224,7 +235,7 @@
   e.WriteValue(*v_false);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(2u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x01\x00", 2), wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x01\x00"sv, std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteDoubleValue2) {
@@ -232,8 +243,8 @@
   e.WriteValue(*v_double);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x3f\xf0\x00\x00\x00\x00\x00\x00", 8),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x3f\xf0\x00\x00\x00\x00\x00\x00"sv,
+            std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteStringValue2) {
@@ -241,8 +252,7 @@
   e.WriteValue(*v_string);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(7u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x00\x05hello", 7),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x00\x05hello"sv, std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteBooleanArrayValue2) {
@@ -250,15 +260,14 @@
   e.WriteValue(*v_boolean_array);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 3u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x03\x00\x01\x00", 4),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x03\x00\x01\x00"sv, std::string_view(e.data(), e.size()));
 
   // truncated
   e.Reset();
   e.WriteValue(*v_boolean_array_big);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 255u, e.size());
-  ASSERT_EQ(wpi::StringRef("\xff\x00", 2), wpi::StringRef(e.data(), 2));
+  ASSERT_EQ("\xff\x00"sv, std::string_view(e.data(), 2));
 }
 
 TEST_F(WireEncoderTest, WriteDoubleArrayValue2) {
@@ -266,17 +275,17 @@
   e.WriteValue(*v_double_array);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 2u * 8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x02\x3f\xe0\x00\x00\x00\x00\x00\x00"
-                           "\x3f\xd0\x00\x00\x00\x00\x00\x00",
-                           17),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ(std::string_view("\x02\x3f\xe0\x00\x00\x00\x00\x00\x00"
+                             "\x3f\xd0\x00\x00\x00\x00\x00\x00",
+                             17),
+            std::string_view(e.data(), e.size()));
 
   // truncated
   e.Reset();
   e.WriteValue(*v_double_array_big);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 255u * 8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\xff\x00", 2), wpi::StringRef(e.data(), 2));
+  ASSERT_EQ("\xff\x00"sv, std::string_view(e.data(), 2));
 }
 
 TEST_F(WireEncoderTest, WriteStringArrayValue2) {
@@ -284,15 +293,15 @@
   e.WriteValue(*v_string_array);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 7u + 9u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x02\x00\x05hello\x00\x07goodbye", 17),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x02\x00\x05hello\x00\x07goodbye"sv,
+            std::string_view(e.data(), e.size()));
 
   // truncated
   e.Reset();
   e.WriteValue(*v_string_array_big);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 255u * 3u, e.size());
-  ASSERT_EQ(wpi::StringRef("\xff\x00\x01", 3), wpi::StringRef(e.data(), 3));
+  ASSERT_EQ("\xff\x00\x01"sv, std::string_view(e.data(), 3));
 }
 
 TEST_F(WireEncoderTest, WriteValueError2) {
@@ -335,7 +344,7 @@
   e.WriteValue(*v_false);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(2u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x01\x00", 2), wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x01\x00"sv, std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteDoubleValue3) {
@@ -343,8 +352,8 @@
   e.WriteValue(*v_double);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x3f\xf0\x00\x00\x00\x00\x00\x00", 8),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x3f\xf0\x00\x00\x00\x00\x00\x00"sv,
+            std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteStringValue3) {
@@ -352,7 +361,7 @@
   e.WriteValue(*v_string);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(6u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x05hello", 6), wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x05hello"sv, std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteRawValue3) {
@@ -360,7 +369,7 @@
   e.WriteValue(*v_raw);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(6u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x05hello", 6), wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x05hello"sv, std::string_view(e.data(), e.size()));
 }
 
 TEST_F(WireEncoderTest, WriteBooleanArrayValue3) {
@@ -368,15 +377,14 @@
   e.WriteValue(*v_boolean_array);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 3u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x03\x00\x01\x00", 4),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x03\x00\x01\x00"sv, std::string_view(e.data(), e.size()));
 
   // truncated
   e.Reset();
   e.WriteValue(*v_boolean_array_big);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 255u, e.size());
-  ASSERT_EQ(wpi::StringRef("\xff\x00", 2), wpi::StringRef(e.data(), 2));
+  ASSERT_EQ("\xff\x00"sv, std::string_view(e.data(), 2));
 }
 
 TEST_F(WireEncoderTest, WriteDoubleArrayValue3) {
@@ -384,17 +392,17 @@
   e.WriteValue(*v_double_array);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 2u * 8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x02\x3f\xe0\x00\x00\x00\x00\x00\x00"
-                           "\x3f\xd0\x00\x00\x00\x00\x00\x00",
-                           17),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ(std::string_view("\x02\x3f\xe0\x00\x00\x00\x00\x00\x00"
+                             "\x3f\xd0\x00\x00\x00\x00\x00\x00",
+                             17),
+            std::string_view(e.data(), e.size()));
 
   // truncated
   e.Reset();
   e.WriteValue(*v_double_array_big);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 255u * 8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\xff\x00", 2), wpi::StringRef(e.data(), 2));
+  ASSERT_EQ("\xff\x00"sv, std::string_view(e.data(), 2));
 }
 
 TEST_F(WireEncoderTest, WriteStringArrayValue3) {
@@ -402,15 +410,14 @@
   e.WriteValue(*v_string_array);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 6u + 8u, e.size());
-  ASSERT_EQ(wpi::StringRef("\x02\x05hello\x07goodbye", 15),
-            wpi::StringRef(e.data(), e.size()));
+  ASSERT_EQ("\x02\x05hello\x07goodbye"sv, std::string_view(e.data(), e.size()));
 
   // truncated
   e.Reset();
   e.WriteValue(*v_string_array_big);
   ASSERT_EQ(nullptr, e.error());
   ASSERT_EQ(1u + 255u * 2u, e.size());
-  ASSERT_EQ(wpi::StringRef("\xff\x01", 2), wpi::StringRef(e.data(), 2));
+  ASSERT_EQ("\xff\x01"sv, std::string_view(e.data(), 2));
 }
 
 TEST_F(WireEncoderTest, WriteValueError3) {
@@ -434,14 +441,13 @@
   e.WriteString(s_normal);
   EXPECT_EQ(nullptr, e.error());
   EXPECT_EQ(7u, e.size());
-  EXPECT_EQ(wpi::StringRef("\x00\x05hello", 7),
-            wpi::StringRef(e.data(), e.size()));
+  EXPECT_EQ("\x00\x05hello"sv, std::string_view(e.data(), e.size()));
 
   e.Reset();
   e.WriteString(s_long);
   EXPECT_EQ(nullptr, e.error());
   ASSERT_EQ(130u, e.size());
-  EXPECT_EQ(wpi::StringRef("\x00\x80**", 4), wpi::StringRef(e.data(), 4));
+  EXPECT_EQ("\x00\x80**"sv, std::string_view(e.data(), 4));
   EXPECT_EQ('*', e.data()[128]);
   EXPECT_EQ('x', e.data()[129]);
 
@@ -450,7 +456,7 @@
   e.WriteString(s_big);
   EXPECT_EQ(nullptr, e.error());
   ASSERT_EQ(65537u, e.size());
-  EXPECT_EQ(wpi::StringRef("\xff\xff**", 4), wpi::StringRef(e.data(), 4));
+  EXPECT_EQ("\xff\xff**"sv, std::string_view(e.data(), 4));
   EXPECT_EQ('*', e.data()[65535]);
   EXPECT_EQ('x', e.data()[65536]);
 }
@@ -468,13 +474,13 @@
   e.WriteString(s_normal);
   EXPECT_EQ(nullptr, e.error());
   EXPECT_EQ(6u, e.size());
-  EXPECT_EQ(wpi::StringRef("\x05hello", 6), wpi::StringRef(e.data(), e.size()));
+  EXPECT_EQ("\x05hello"sv, std::string_view(e.data(), e.size()));
 
   e.Reset();
   e.WriteString(s_long);
   EXPECT_EQ(nullptr, e.error());
   ASSERT_EQ(130u, e.size());
-  EXPECT_EQ(wpi::StringRef("\x80\x01**", 4), wpi::StringRef(e.data(), 4));
+  EXPECT_EQ("\x80\x01**"sv, std::string_view(e.data(), 4));
   EXPECT_EQ('*', e.data()[128]);
   EXPECT_EQ('x', e.data()[129]);
 
@@ -483,7 +489,7 @@
   e.WriteString(s_big);
   EXPECT_EQ(nullptr, e.error());
   ASSERT_EQ(65540u, e.size());
-  EXPECT_EQ(wpi::StringRef("\x81\x80\x04*", 4), wpi::StringRef(e.data(), 4));
+  EXPECT_EQ("\x81\x80\x04*"sv, std::string_view(e.data(), 4));
   EXPECT_EQ('*', e.data()[65536]);
   EXPECT_EQ('x', e.data()[65537]);
   EXPECT_EQ('x', e.data()[65538]);
diff --git a/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp b/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp
index 112289f..a3fec50 100644
--- a/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/ntcore/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <climits>
 
diff --git a/third_party/allwpilib/outlineviewer/.styleguide b/third_party/allwpilib/outlineviewer/.styleguide
new file mode 100644
index 0000000..d4917bf
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/.styleguide
@@ -0,0 +1,28 @@
+cppHeaderFileInclude {
+  \.h$
+  \.inc$
+  \.inl$
+}
+
+cppSrcFileInclude {
+  \.cpp$
+}
+
+generatedFileExclude {
+  src/main/native/resources/
+  src/main/native/win/outlineviewer.ico
+  src/main/native/mac/ov.icns
+}
+
+repoRootNameOverride {
+  outlineviewer
+}
+
+includeOtherLibs {
+  ^GLFW
+  ^fmt/
+  ^imgui
+  ^ntcore
+  ^wpi/
+  ^wpigui
+}
diff --git a/third_party/allwpilib/outlineviewer/CMakeLists.txt b/third_party/allwpilib/outlineviewer/CMakeLists.txt
new file mode 100644
index 0000000..537134d
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/CMakeLists.txt
@@ -0,0 +1,28 @@
+project(outlineviewer)
+
+include(CompileWarnings)
+include(GenResources)
+include(LinkMacOSGUI)
+
+configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
+GENERATE_RESOURCES(src/main/native/resources generated/main/cpp OV ov outlineviewer_resources_src)
+
+file(GLOB outlineviewer_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
+
+if (WIN32)
+    set(outlineviewer_rc src/main/native/win/outlineviewer.rc)
+elseif(APPLE)
+    set(MACOSX_BUNDLE_ICON_FILE ov.icns)
+    set(APP_ICON_MACOSX src/main/native/mac/ov.icns)
+    set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
+endif()
+
+add_executable(outlineviewer ${outlineviewer_src} ${outlineviewer_resources_src} ${outlineviewer_rc} ${APP_ICON_MACOSX})
+wpilib_link_macos_gui(outlineviewer)
+target_link_libraries(outlineviewer libglassnt libglass)
+
+if (WIN32)
+    set_target_properties(outlineviewer PROPERTIES WIN32_EXECUTABLE YES)
+elseif(APPLE)
+    set_target_properties(outlineviewer PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "OutlineViewer")
+endif()
diff --git a/third_party/allwpilib/outlineviewer/Info.plist b/third_party/allwpilib/outlineviewer/Info.plist
new file mode 100644
index 0000000..a3e8a85
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/Info.plist
@@ -0,0 +1,32 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleName</key>
+	<string>OutlineViewer</string>
+	<key>CFBundleExecutable</key>
+	<string>outlineviewer</string>
+	<key>CFBundleDisplayName</key>
+	<string>OutlineViewer</string>
+	<key>CFBundleIdentifier</key>
+	<string>edu.wpi.first.tools.OutlineViewer</string>
+	<key>CFBundleIconFile</key>
+	<string>ov.icns</string>
+	<key>CFBundlePackageType</key>
+	<string>APPL</string>
+	<key>CFBundleSupportedPlatforms</key>
+	<array>
+		<string>MacOSX</string>
+	</array>
+	<key>CFBundleInfoDictionaryVersion</key>
+	<string>6.0</string>
+	<key>CFBundleShortVersionString</key>
+	<string>2021</string>
+	<key>CFBundleVersion</key>
+	<string>2021</string>
+	<key>LSMinimumSystemVersion</key>
+	<string>10.11</string>
+	<key>NSHighResolutionCapable</key>
+	<true/>
+</dict>
+</plist>
diff --git a/third_party/allwpilib/outlineviewer/build.gradle b/third_party/allwpilib/outlineviewer/build.gradle
new file mode 100644
index 0000000..5b4ee6e
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/build.gradle
@@ -0,0 +1,119 @@
+import org.gradle.internal.os.OperatingSystem
+
+if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
+
+    description = "NetworkTables Viewer"
+
+    apply plugin: 'cpp'
+    apply plugin: 'c'
+    apply plugin: 'google-test-test-suite'
+    apply plugin: 'visual-studio'
+    apply plugin: 'edu.wpi.first.NativeUtils'
+
+    if (OperatingSystem.current().isWindows()) {
+        apply plugin: 'windows-resources'
+    }
+
+    ext {
+        nativeName = 'outlineviewer'
+    }
+
+    apply from: "${rootDir}/shared/resources.gradle"
+    apply from: "${rootDir}/shared/config.gradle"
+
+    def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in")
+    def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
+
+    task generateCppVersion() {
+        description = 'Generates the wpilib version class'
+        group = 'WPILib'
+
+        outputs.file wpilibVersionFileOutput
+        inputs.file wpilibVersionFileInput
+
+        if (wpilibVersioning.releaseMode) {
+            outputs.upToDateWhen { false }
+        }
+
+        // We follow a simple set of checks to determine whether we should generate a new version file:
+        // 1. If the release type is not development, we generate a new version file
+        // 2. If there is no generated version number, we generate a new version file
+        // 3. If there is a generated build number, and the release type is development, then we will
+        //    only generate if the publish task is run.
+        doLast {
+            def version = wpilibVersioning.version.get()
+            println "Writing version ${version} to $wpilibVersionFileOutput"
+
+            if (wpilibVersionFileOutput.exists()) {
+                wpilibVersionFileOutput.delete()
+            }
+            def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
+            wpilibVersionFileOutput.write(read)
+        }
+    }
+
+    gradle.taskGraph.addTaskExecutionGraphListener { graph ->
+        def willPublish = graph.hasTask(publish)
+        if (willPublish) {
+            generateCppVersion.outputs.upToDateWhen { false }
+        }
+    }
+
+    def generateTask = createGenerateResourcesTask('main', 'OV', 'ov', project)
+
+    project(':').libraryBuild.dependsOn build
+    tasks.withType(CppCompile) {
+        dependsOn generateTask
+        dependsOn generateCppVersion
+    }
+
+    model {
+        components {
+            // By default, a development executable will be generated. This is to help the case of
+            // testing specific functionality of the library.
+            "${nativeName}"(NativeExecutableSpec) {
+                baseName = 'outlineviewer'
+                sources {
+                    cpp {
+                        source {
+                            srcDirs 'src/main/native/cpp', "$buildDir/generated/main/cpp"
+                            include '**/*.cpp'
+                        }
+                        exportedHeaders {
+                            srcDirs 'src/main/native/include'
+                        }
+                    }
+                    if (OperatingSystem.current().isWindows()) {
+                        rc {
+                            source {
+                                srcDirs 'src/main/native/win'
+                                include '*.rc'
+                            }
+                        }
+                    }
+                }
+                binaries.all {
+                    if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
+                        it.buildable = false
+                        return
+                    }
+                    lib project: ':glass', library: 'glassnt', linkage: 'static'
+                    lib project: ':glass', library: 'glass', linkage: 'static'
+                    lib project: ':ntcore', library: 'ntcore', linkage: 'static'
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
+                    lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                    nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    if (it.targetPlatform.operatingSystem.isWindows()) {
+                        it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
+                    } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
+                        it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore'
+                    } else {
+                        it.linker.args << '-lX11'
+                    }
+                }
+            }
+        }
+    }
+
+    apply from: 'publish.gradle'
+}
diff --git a/third_party/allwpilib/outlineviewer/publish.gradle b/third_party/allwpilib/outlineviewer/publish.gradle
new file mode 100644
index 0000000..b0fa202
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/publish.gradle
@@ -0,0 +1,107 @@
+apply plugin: 'maven-publish'
+
+def baseArtifactId = 'OutlineViewer'
+def artifactGroupId = 'edu.wpi.first.tools'
+def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_OutlineViewer_CLS'
+
+def outputsFolder = file("$project.buildDir/outputs")
+
+model {
+    tasks {
+        // Create the run task.
+        $.components.outlineviewer.binaries.each { bin ->
+            if (bin.buildable && bin.name.toLowerCase().contains("debug")) {
+                Task run = project.tasks.create("run", Exec) {
+                    commandLine bin.tasks.install.runScriptFile.get().asFile.toString()
+                }
+                run.dependsOn bin.tasks.install
+            }
+        }
+    }
+    publishing {
+        def outlineViewerTaskList = []
+        $.components.each { component ->
+            component.binaries.each { binary ->
+                if (binary in NativeExecutableBinarySpec && binary.component.name.contains("outlineviewer")) {
+                    if (binary.buildable && binary.name.contains("Release")) {
+                        // We are now in the binary that we want.
+                        // This is the default application path for the ZIP task.
+                        def applicationPath = binary.executable.file
+                        def icon = file("$project.projectDir/src/main/native/mac/ov.icns")
+
+                        // Create the macOS bundle.
+                        def bundleTask = project.tasks.create("bundleOutlineViewerOsxApp", Copy) {
+                            description("Creates a macOS application bundle for OutlineViewer")
+                            from(file("$project.projectDir/Info.plist"))
+                            into(file("$project.buildDir/outputs/bundles/OutlineViewer.app/Contents"))
+                            into("MacOS") { with copySpec { from binary.executable.file } }
+                            into("Resources") { with copySpec { from icon } }
+
+                            doLast {
+                                if (project.hasProperty("developerID")) {
+                                    // Get path to binary.
+                                    exec {
+                                        workingDir rootDir
+                                        def args = [
+                                            "sh",
+                                            "-c",
+                                            "codesign --force --strict --deep " +
+                                            "--timestamp --options=runtime " +
+                                            "--verbose -s ${project.findProperty("developerID")} " +
+                                            "$project.buildDir/outputs/bundles/OutlineViewer.app/"
+                                        ]
+                                        commandLine args
+                                    }
+                                }
+                            }
+                        }
+
+                        // Reset the application path if we are creating a bundle.
+                        if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+                            applicationPath = file("$project.buildDir/outputs/bundles")
+                            project.build.dependsOn bundleTask
+                        }
+
+                        // Create the ZIP.
+                        def task = project.tasks.create("copyOutlineViewerExecutable", Zip) {
+                            description("Copies the OutlineViewer executable to the outputs directory.")
+                            destinationDirectory = outputsFolder
+
+                            archiveBaseName = '_M_' + zipBaseName
+                            duplicatesStrategy = 'exclude'
+                            classifier = nativeUtils.getPublishClassifier(binary)
+
+                            from(licenseFile) {
+                                into '/'
+                            }
+
+                            from(applicationPath)
+                            into(nativeUtils.getPlatformPath(binary))
+                        }
+
+                        if (binary.targetPlatform.operatingSystem.isMacOsX()) {
+                            bundleTask.dependsOn binary.tasks.link
+                            task.dependsOn(bundleTask)
+                        }
+
+                        task.dependsOn binary.tasks.link
+                        outlineViewerTaskList.add(task)
+                        project.build.dependsOn task
+                        project.artifacts { task }
+                        addTaskToCopyAllOutputs(task)
+                    }
+                }
+            }
+        }
+
+        publications {
+            outlineViewer(MavenPublication) {
+                outlineViewerTaskList.each { artifact it }
+
+                artifactId = baseArtifactId
+                groupId = artifactGroupId
+                version wpilibVersioning.version.get()
+            }
+        }
+    }
+}
diff --git a/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in b/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in
new file mode 100644
index 0000000..b0a4490
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/generate/WPILibVersion.cpp.in
@@ -0,0 +1,7 @@
+/*
+ * Autogenerated file! Do not manually edit this file. This version is regenerated
+ * any time the publish task is run, or when this file is deleted.
+ */
+const char* GetWPILibVersion() {
+  return "${wpilib_version}";
+}
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/cpp/main.cpp b/third_party/allwpilib/outlineviewer/src/main/native/cpp/main.cpp
new file mode 100644
index 0000000..11428f5
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/cpp/main.cpp
@@ -0,0 +1,224 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <memory>
+
+#include <GLFW/glfw3.h>
+#include <fmt/format.h>
+#include <imgui.h>
+#include <ntcore_cpp.h>
+#include <wpigui.h>
+
+#include "glass/Context.h"
+#include "glass/Model.h"
+#include "glass/networktables/NetworkTables.h"
+#include "glass/networktables/NetworkTablesSettings.h"
+#include "glass/other/Log.h"
+
+namespace gui = wpi::gui;
+
+const char* GetWPILibVersion();
+
+namespace ov {
+std::string_view GetResource_ov_16_png();
+std::string_view GetResource_ov_32_png();
+std::string_view GetResource_ov_48_png();
+std::string_view GetResource_ov_64_png();
+std::string_view GetResource_ov_128_png();
+std::string_view GetResource_ov_256_png();
+std::string_view GetResource_ov_512_png();
+}  // namespace ov
+
+static std::unique_ptr<glass::NetworkTablesModel> gModel;
+static std::unique_ptr<glass::NetworkTablesSettings> gSettings;
+static glass::LogData gLog;
+static glass::NetworkTablesFlagsSettings gFlagsSettings;
+
+static void NtInitialize() {
+  // update window title when connection status changes
+  auto inst = nt::GetDefaultInstance();
+  auto poller = nt::CreateConnectionListenerPoller(inst);
+  nt::AddPolledConnectionListener(poller, true);
+  gui::AddEarlyExecute([inst, poller] {
+    auto win = gui::GetSystemWindow();
+    if (!win) {
+      return;
+    }
+    bool timedOut;
+    for (auto&& event : nt::PollConnectionListener(poller, 0, &timedOut)) {
+      if ((nt::GetNetworkMode(inst) & NT_NET_MODE_SERVER) != 0) {
+        // for server mode, just print number of clients connected
+        glfwSetWindowTitle(win,
+                           fmt::format("OutlineViewer - {} Clients Connected",
+                                       nt::GetConnections(inst).size())
+                               .c_str());
+      } else if (event.connected) {
+        glfwSetWindowTitle(win, fmt::format("OutlineViewer - Connected ({})",
+                                            event.conn.remote_ip)
+                                    .c_str());
+      } else {
+        glfwSetWindowTitle(win, "OutlineViewer - DISCONNECTED");
+      }
+    }
+  });
+
+  // handle NetworkTables log messages
+  auto logPoller = nt::CreateLoggerPoller(inst);
+  nt::AddPolledLogger(logPoller, NT_LOG_INFO, 100);
+  gui::AddEarlyExecute([logPoller] {
+    bool timedOut;
+    for (auto&& msg : nt::PollLogger(logPoller, 0, &timedOut)) {
+      const char* level = "";
+      if (msg.level >= NT_LOG_CRITICAL) {
+        level = "CRITICAL: ";
+      } else if (msg.level >= NT_LOG_ERROR) {
+        level = "ERROR: ";
+      } else if (msg.level >= NT_LOG_WARNING) {
+        level = "WARNING: ";
+      }
+      gLog.Append(fmt::format("{}{} ({}:{})\n", level, msg.message,
+                              msg.filename, msg.line));
+    }
+  });
+
+  // NetworkTables table window
+  gModel = std::make_unique<glass::NetworkTablesModel>();
+  gui::AddEarlyExecute([] { gModel->Update(); });
+
+  // NetworkTables settings window
+  gSettings = std::make_unique<glass::NetworkTablesSettings>();
+  gui::AddEarlyExecute([] { gSettings->Update(); });
+}
+
+static void DisplayGui() {
+  ImGui::GetStyle().WindowRounding = 0;
+
+  // fill entire OS window with this window
+  ImGui::SetNextWindowPos(ImVec2(0, 0));
+  int width, height;
+  glfwGetWindowSize(gui::GetSystemWindow(), &width, &height);
+  ImGui::SetNextWindowSize(ImVec2(width, height));
+
+  ImGui::Begin("Entries", nullptr,
+               ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_MenuBar |
+                   ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove |
+                   ImGuiWindowFlags_NoCollapse);
+
+  gFlagsSettings.Update();
+
+  // can't create popups from within menu, so use flags
+  bool settings = false;
+  bool log = false;
+  bool about = false;
+
+  // main menu
+  ImGui::BeginMenuBar();
+  gui::EmitViewMenu();
+  if (ImGui::BeginMenu("View")) {
+    gFlagsSettings.DisplayMenu();
+    ImGui::EndMenu();
+  }
+
+  if (ImGui::BeginMenu("Options")) {
+    if (ImGui::MenuItem("Settings")) {
+      settings = true;
+    }
+    ImGui::EndMenu();
+  }
+
+  if (ImGui::BeginMenu("Info")) {
+    if (ImGui::MenuItem("Log")) {
+      log = true;
+    }
+    ImGui::Separator();
+    if (ImGui::MenuItem("About")) {
+      about = true;
+    }
+    ImGui::EndMenu();
+  }
+  ImGui::EndMenuBar();
+
+  // settings popup
+  if (settings) {
+    ImGui::OpenPopup("Settings");
+  }
+  if (ImGui::BeginPopupModal("Settings", nullptr,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    if (gSettings->Display()) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Cancel")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  // log popup
+  if (log) {
+    ImGui::OpenPopup("Log");
+  }
+  if (ImGui::BeginPopupModal("Log", nullptr,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::BeginChild("Lines", ImVec2(width * 0.75f, height * 0.75f));
+    glass::DisplayLog(&gLog, true);
+    ImGui::EndChild();
+    ImGui::EndPopup();
+  }
+
+  // about popup
+  if (about) {
+    ImGui::OpenPopup("About");
+  }
+  if (ImGui::BeginPopupModal("About", nullptr,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    ImGui::Text("OutlineViewer");
+    ImGui::Separator();
+    ImGui::Text("v%s", GetWPILibVersion());
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  // display table view
+  glass::DisplayNetworkTables(gModel.get(), gFlagsSettings.GetFlags());
+
+  ImGui::End();
+}
+
+#ifdef _WIN32
+int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
+                      int nCmdShow) {
+#else
+int main() {
+#endif
+  gui::CreateContext();
+  glass::CreateContext();
+
+  gui::AddIcon(ov::GetResource_ov_16_png());
+  gui::AddIcon(ov::GetResource_ov_32_png());
+  gui::AddIcon(ov::GetResource_ov_48_png());
+  gui::AddIcon(ov::GetResource_ov_64_png());
+  gui::AddIcon(ov::GetResource_ov_128_png());
+  gui::AddIcon(ov::GetResource_ov_256_png());
+  gui::AddIcon(ov::GetResource_ov_512_png());
+
+  gui::ConfigurePlatformSaveFile("outlineviewer.ini");
+  gui::AddInit(NtInitialize);
+
+  gui::AddLateExecute(DisplayGui);
+
+  gui::Initialize("OutlineViewer - DISCONNECTED", 600, 400);
+  gui::Main();
+
+  gModel.reset();
+  gSettings.reset();
+
+  glass::DestroyContext();
+  gui::DestroyContext();
+}
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/mac/ov.icns b/third_party/allwpilib/outlineviewer/src/main/native/mac/ov.icns
new file mode 100644
index 0000000..17f9770
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/mac/ov.icns
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-128.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-128.png
new file mode 100644
index 0000000..950d0ca
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-128.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-16.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-16.png
new file mode 100644
index 0000000..db84366
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-16.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-256.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-256.png
new file mode 100644
index 0000000..7b3b2b7
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-256.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-32.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-32.png
new file mode 100644
index 0000000..4db8e8b
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-32.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-48.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-48.png
new file mode 100644
index 0000000..026491c
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-48.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-512.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-512.png
new file mode 100644
index 0000000..d4fd60c
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-512.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-64.png b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-64.png
new file mode 100644
index 0000000..9cd5d96
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/resources/ov-64.png
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/win/outlineviewer.ico b/third_party/allwpilib/outlineviewer/src/main/native/win/outlineviewer.ico
new file mode 100644
index 0000000..7156af8
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/win/outlineviewer.ico
Binary files differ
diff --git a/third_party/allwpilib/outlineviewer/src/main/native/win/outlineviewer.rc b/third_party/allwpilib/outlineviewer/src/main/native/win/outlineviewer.rc
new file mode 100644
index 0000000..85bdcae
--- /dev/null
+++ b/third_party/allwpilib/outlineviewer/src/main/native/win/outlineviewer.rc
@@ -0,0 +1 @@
+IDI_ICON1 ICON "outlineviewer.ico"
diff --git a/third_party/allwpilib/settings.gradle b/third_party/allwpilib/settings.gradle
index 2d59b6e..ac22bc4 100644
--- a/third_party/allwpilib/settings.gradle
+++ b/third_party/allwpilib/settings.gradle
@@ -28,6 +28,9 @@
 include 'wpilibjExamples'
 include 'wpilibjIntegrationTests'
 include 'wpilibj'
+include 'crossConnIntegrationTests'
+include 'glass'
+include 'outlineviewer'
 include 'simulation:gz_msgs'
 include 'simulation:frc_gazebo_plugins'
 include 'simulation:halsim_gazebo'
@@ -42,3 +45,4 @@
 include 'wpilibNewCommands'
 include 'myRobot'
 include 'docs'
+include 'msvcruntime'
diff --git a/third_party/allwpilib/shared/config.gradle b/third_party/allwpilib/shared/config.gradle
index 0f8b8a9..9f9a221 100644
--- a/third_party/allwpilib/shared/config.gradle
+++ b/third_party/allwpilib/shared/config.gradle
@@ -1,20 +1,22 @@
 import org.gradle.internal.os.OperatingSystem
 
+nativeUtils.skipInstallPdb = project.hasProperty('buildServer')
+
 nativeUtils.addWpiNativeUtils()
 nativeUtils.withRoboRIO()
 nativeUtils.withRaspbian()
 nativeUtils.withBionic()
 nativeUtils {
-  wpi {
-    configureDependencies {
-      wpiVersion = "-1"
-      niLibVersion = "2020.10.1"
-      opencvVersion = "3.4.7-5"
-      googleTestVersion = "1.9.0-5-437e100-1"
-      imguiVersion = "1.76-9"
-      wpimathVersion = "-1"
+    wpi {
+        configureDependencies {
+            wpiVersion = "-1"
+            niLibVersion = "2022.2.2"
+            opencvVersion = "4.5.2-1"
+            googleTestVersion = "1.9.0-5-437e100-1"
+            imguiVersion = "1.82-1"
+            wpimathVersion = "-1"
+        }
     }
-  }
 }
 
 nativeUtils.wpi.addWarnings()
@@ -25,13 +27,17 @@
 nativeUtils.enableSourceLink()
 
 nativeUtils.platformConfigs.each {
-    if (it.name.contains('windows')) return
+    if (it.name.contains('windows')) {
+        return
+    }
     it.linker.args << '-Wl,-rpath,\'$ORIGIN\''
     if (it.name == 'osxx86-64') {
         it.linker.args << "-headerpad_max_install_names"
     }
 }
 
+nativeUtils.platformConfigs.linuxathena.linker.args.add("-Wl,--fatal-warnings")
+
 model {
     components {
         all {
@@ -45,6 +51,19 @@
     }
 }
 
+apply plugin: DisableBuildingGTest
+
+if (project.hasProperty('buildServer')) {
+    tasks.withType(org.gradle.nativeplatform.test.tasks.RunTestExecutable) {
+        def exeFile = file(it.executable)
+        def folder = exeFile.parentFile
+
+        it.doLast {
+            folder.deleteDir()
+        }
+    }
+}
+
 ext.appendDebugPathToBinaries = { binaries->
     binaries.withType(StaticLibraryBinarySpec) {
         if (it.buildType.name.contains('debug')) {
@@ -86,7 +105,7 @@
         if (it in NativeLibrarySpec && stringNames.contains(it.name)) {
             it.binaries.each {
                 if (!it.buildable) return
-                def target = nativeUtils.getPublishClassifier(it)
+                    def target = nativeUtils.getPublishClassifier(it)
                 if (configMap.containsKey(target)) {
                     configMap.get(target).add(it)
                 } else {
diff --git a/third_party/allwpilib/shared/cppDesktopTestTask.gradle b/third_party/allwpilib/shared/cppDesktopTestTask.gradle
index 04b7224..928cc33 100644
--- a/third_party/allwpilib/shared/cppDesktopTestTask.gradle
+++ b/third_party/allwpilib/shared/cppDesktopTestTask.gradle
@@ -1,21 +1,21 @@
-model {
-    tasks {
-        def ts = $.testSuites
-        project.tasks.register('testDesktopCpp') { testTask->
-            def systemArch = getCurrentArch()
-            def found = false
-            ts.each {
-                if (it in GoogleTestTestSuiteSpec && it.name == "${nativeName}Test") {
-                    it.binaries.each {
-                        if (found) return
-                        def arch = it.targetPlatform.name
-                        if (arch == systemArch && it.buildType.name == 'debug') {
-                            testTask.dependsOn it.tasks.run
-                            found = true
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
+model {
+    tasks {
+        def ts = $.testSuites
+        project.tasks.register('testDesktopCpp') { testTask->
+            def systemArch = getCurrentArch()
+            def found = false
+            ts.each {
+                if (it in GoogleTestTestSuiteSpec && it.name == "${nativeName}Test") {
+                    it.binaries.each {
+                        if (found) return
+                            def arch = it.targetPlatform.name
+                        if (arch == systemArch && it.buildType.name == 'debug') {
+                            testTask.dependsOn it.tasks.run
+                            found = true
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
diff --git a/third_party/allwpilib/shared/java/javacommon.gradle b/third_party/allwpilib/shared/java/javacommon.gradle
index d83ead2..2eb1438 100644
--- a/third_party/allwpilib/shared/java/javacommon.gradle
+++ b/third_party/allwpilib/shared/java/javacommon.gradle
@@ -1,6 +1,5 @@
 apply plugin: 'maven-publish'
 apply plugin: 'java-library'
-//apply plugin: 'net.ltgt.errorprone'
 apply plugin: 'jacoco'
 
 def baseArtifactId = project.baseId
@@ -96,7 +95,12 @@
 }
 
 tasks.withType(JavaCompile).configureEach {
-    options.compilerArgs = ['--release', '11']
+    options.compilerArgs = [
+        '--release',
+        '11',
+        '-encoding',
+        'UTF8'
+    ]
 }
 
 dependencies {
@@ -105,15 +109,12 @@
     testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.4.2'
 
     devImplementation sourceSets.main.output
-
-    //errorprone 'com.google.errorprone:error_prone_core:2.3.2-SNAPSHOT'
-    //errorproneJavac 'com.google.errorprone:error_prone_core:2.3.1'
 }
 
 task run(type: JavaExec) {
     classpath = sourceSets.dev.runtimeClasspath
 
-    main = project.devMain
+    mainClass = project.devMain
 }
 
 build.dependsOn devClasses
@@ -124,7 +125,7 @@
 
 jacocoTestReport {
     reports {
-        xml.enabled true
-        html.enabled true
+        xml.required = true
+        html.required = true
     }
 }
diff --git a/third_party/allwpilib/shared/java/javastyle.gradle b/third_party/allwpilib/shared/java/javastyle.gradle
index ecd551b..281eed3 100644
--- a/third_party/allwpilib/shared/java/javastyle.gradle
+++ b/third_party/allwpilib/shared/java/javastyle.gradle
@@ -1,13 +1,12 @@
+if (!project.hasProperty('skipJavaFormat')) {
+    apply plugin: 'checkstyle'
 
-apply plugin: 'checkstyle'
+    checkstyle {
+        toolVersion = "8.38"
+        configDirectory = file("${project.rootDir}/styleguide")
+        config = resources.text.fromFile(new File(configDirectory.get().getAsFile(), "checkstyle.xml"))
+    }
 
-checkstyle {
-    toolVersion = "8.12"
-    configDirectory = file("${project.rootDir}/styleguide")
-    config = resources.text.fromFile(new File(configDirectory.get().getAsFile(), "checkstyle.xml"))
-}
-
-if (!project.hasProperty('skipPMD')) {
     apply plugin: 'pmd'
 
     pmd {
@@ -17,9 +16,63 @@
         ruleSetFiles = files(new File(rootDir, "styleguide/pmd-ruleset.xml"))
         ruleSets = []
     }
+
+    apply plugin: 'com.diffplug.spotless'
+
+    spotless {
+        java {
+            target fileTree('.') {
+                include '**/*.java'
+                exclude '**/build/**', '**/build-*/**'
+            }
+            toggleOffOn()
+            googleJavaFormat()
+            removeUnusedImports()
+            trimTrailingWhitespace()
+            endWithNewline()
+        }
+        groovyGradle {
+            target fileTree('.') {
+                include '**/*.gradle'
+                exclude '**/build/**', '**/build-*/**'
+            }
+            greclipse()
+            indentWithSpaces(4)
+            trimTrailingWhitespace()
+            endWithNewline()
+        }
+        format 'xml', {
+            target fileTree('.') {
+                include '**/*.xml'
+                exclude '**/build/**', '**/build-*/**'
+            }
+            eclipseWtp('xml')
+            trimTrailingWhitespace()
+            indentWithSpaces(2)
+            endWithNewline()
+        }
+        format 'misc', {
+            target fileTree('.') {
+                include '**/*.md', '**/.gitignore'
+                exclude '**/build/**', '**/build-*/**'
+            }
+            trimTrailingWhitespace()
+            indentWithSpaces(2)
+            endWithNewline()
+        }
+    }
+
+    apply plugin: 'com.github.spotbugs'
+
+    spotbugs {
+        ignoreFailures = false
+        effort = 'max'
+        excludeFilter = file("${project.rootDir}/styleguide/spotbugs-exclude.xml")
+    }
 }
 
 task javaFormat {
     dependsOn(tasks.withType(Checkstyle))
     dependsOn(tasks.withType(Pmd))
 }
+javaFormat.dependsOn 'spotlessApply'
diff --git a/third_party/allwpilib/shared/javaDesktopTestTask.gradle b/third_party/allwpilib/shared/javaDesktopTestTask.gradle
index 766fa50..82a6a0e 100644
--- a/third_party/allwpilib/shared/javaDesktopTestTask.gradle
+++ b/third_party/allwpilib/shared/javaDesktopTestTask.gradle
@@ -1,3 +1,3 @@
 tasks.register('testDesktopJava') {
-  dependsOn test
+    dependsOn test
 }
diff --git a/third_party/allwpilib/shared/javacpp/publish.gradle b/third_party/allwpilib/shared/javacpp/publish.gradle
index 140ae20..201c023 100644
--- a/third_party/allwpilib/shared/javacpp/publish.gradle
+++ b/third_party/allwpilib/shared/javacpp/publish.gradle
@@ -31,8 +31,14 @@
         into '/'
     }
 
-    from('src/main/native/include') {
-        into '/'
+    ext.includeDirs = [
+        project.file('src/main/native/include')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/'
+        }
     }
 }
 
diff --git a/third_party/allwpilib/shared/jni/publish.gradle b/third_party/allwpilib/shared/jni/publish.gradle
index f369631..8745caf 100644
--- a/third_party/allwpilib/shared/jni/publish.gradle
+++ b/third_party/allwpilib/shared/jni/publish.gradle
@@ -50,8 +50,14 @@
         into '/'
     }
 
-    from('src/main/native/include') {
-        into '/'
+    ext.includeDirs = [
+        project.file('src/main/native/include')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/'
+        }
     }
 }
 
@@ -65,7 +71,10 @@
 
 model {
     publishing {
-        def taskList = createComponentZipTasks($.components, [nativeName, "${nativeName}JNIShared"], zipBaseName, Zip, project, includeStandardZipFormat)
+        def taskList = createComponentZipTasks($.components, [
+            nativeName,
+            "${nativeName}JNIShared"
+        ], zipBaseName, Zip, project, includeStandardZipFormat)
 
         def jniTaskList = createComponentZipTasks($.components, ["${nativeName}JNI"], jniBaseName, Jar, project, { task, value ->
             value.each { binary ->
diff --git a/third_party/allwpilib/shared/jni/setupBuild.gradle b/third_party/allwpilib/shared/jni/setupBuild.gradle
index 369422d..f897806 100644
--- a/third_party/allwpilib/shared/jni/setupBuild.gradle
+++ b/third_party/allwpilib/shared/jni/setupBuild.gradle
@@ -59,6 +59,8 @@
                     it.buildable = false
                     return
                 }
+                it.cppCompiler.define 'WPILIB_EXPORTS'
+                it.cCompiler.define 'WPILIB_EXPORTS'
                 if (!project.hasProperty('noWpiutil')) {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 }
@@ -117,7 +119,6 @@
                         }
                         include '**/*.h'
                     }
-
                 }
             }
             binaries.all {
@@ -198,9 +199,10 @@
                 lib library: "${nativeName}JNIShared", linkage: 'shared'
                 if (!project.hasProperty('noWpiutil')) {
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                    lib project: ':wpiutil', library: 'wpiutilJNIShared', linkage: 'shared'
                 }
                 if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                 }
             }
         }
@@ -235,7 +237,7 @@
             if (!project.hasProperty('noWpiutil')) {
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 if (nativeName == 'hal' && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                 }
             }
         }
@@ -257,6 +259,15 @@
                                 commandLine it.tasks.install.runScriptFile.get().asFile.toString()
                                 def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
                                 test.dependsOn it.tasks.install
+
+                                if (project.hasProperty('buildServer')) {
+                                    def folderDir = it.tasks.install.installDirectory.get().toString()
+                                    test.doLast {
+                                        def folder = file(folderDir)
+                                        folder.deleteDir()
+                                    }
+                                }
+
                                 test.systemProperty 'java.library.path', filePath
                                 test.environment 'LD_LIBRARY_PATH', filePath
                                 test.workingDir filePath
diff --git a/third_party/allwpilib/shared/opencv.gradle b/third_party/allwpilib/shared/opencv.gradle
index 86151c1..ef7715d 100644
--- a/third_party/allwpilib/shared/opencv.gradle
+++ b/third_party/allwpilib/shared/opencv.gradle
@@ -1,4 +1,4 @@
-def opencvVersion = '3.4.7-5'
+def opencvVersion = '4.5.2-1'
 
 if (project.hasProperty('useCpp') && project.useCpp) {
     model {
@@ -22,12 +22,12 @@
 
 if (project.hasProperty('useJava') && project.useJava) {
     dependencies {
-        implementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
+        implementation "edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:${opencvVersion}"
         if (!project.hasProperty('skipDev') || !project.skipDev) {
-            devImplementation "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}"
+            devImplementation "edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:${opencvVersion}"
         }
         if (project.hasProperty('useDocumentation') && project.useDocumentation) {
-            javaSource "edu.wpi.first.thirdparty.frc2021.opencv:opencv-java:${opencvVersion}:sources"
+            javaSource "edu.wpi.first.thirdparty.frc2022.opencv:opencv-java:${opencvVersion}:sources"
         }
     }
 }
diff --git a/third_party/allwpilib/shared/plugins/publish.gradle b/third_party/allwpilib/shared/plugins/publish.gradle
index ec160ed..26caad9 100644
--- a/third_party/allwpilib/shared/plugins/publish.gradle
+++ b/third_party/allwpilib/shared/plugins/publish.gradle
@@ -43,18 +43,7 @@
 
 model {
     publishing {
-        def pluginTaskList = createComponentZipTasks($.components, [pluginName], zipBaseName, Zip, project, { task, value ->
-            value.each { binary ->
-                if (binary.buildable) {
-                    if (binary instanceof SharedLibraryBinarySpec) {
-                        task.dependsOn binary.buildTask
-                        task.from(binary.sharedLibraryFile) {
-                            into nativeUtils.getPlatformPath(binary) + '/shared'
-                        }
-                    }
-                }
-            }
-        })
+        def pluginTaskList = createComponentZipTasks($.components, [pluginName], zipBaseName, Zip, project, includeStandardZipFormat)
 
         publications {
             cpp(MavenPublication) {
diff --git a/third_party/allwpilib/shared/plugins/setupBuild.gradle b/third_party/allwpilib/shared/plugins/setupBuild.gradle
index af9b4c4..2097ace 100644
--- a/third_party/allwpilib/shared/plugins/setupBuild.gradle
+++ b/third_party/allwpilib/shared/plugins/setupBuild.gradle
@@ -21,10 +21,15 @@
                     }
                 }
                 binaries.all {
+                    // Define a custom entry point if we are building statically to avoid symbol collision.
                     if (it instanceof StaticLibraryBinarySpec) {
-                        it.buildable = false
-                        return
+                        // These scenario is seldom used, except in sysid to build a fully static exe
+                        // with simulation modules. When building static, it is important that no two
+                        // modules have the same entry point symbolically.
+                        def name = project.name.replace("halsim_", "").toUpperCase()
+                        it.cppCompiler.define("HALSIM_InitExtension", "HALSIM_InitExtension_$name")
                     }
+
                     project(':hal').addHalDependency(it, 'shared')
                     if (project.hasProperty('includeNtCore')) {
                         lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
@@ -57,7 +62,7 @@
                         }
                         lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                         if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                            nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                            nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                         }
                     } else {
                         it.buildable = false
diff --git a/third_party/allwpilib/shared/resources.gradle b/third_party/allwpilib/shared/resources.gradle
index 21da736..f5aa19f 100644
--- a/third_party/allwpilib/shared/resources.gradle
+++ b/third_party/allwpilib/shared/resources.gradle
@@ -13,13 +13,13 @@
             generatedOutputDir.mkdirs()
             inputDir.eachFile { inputFile ->
                 if (inputFile.name.startsWith('.')) return
-                def fileBytes = inputFile.bytes
+                    def fileBytes = inputFile.bytes
                 def outputFile = file("$generatedOutputDir/${inputFile.name}.cpp")
                 def funcName = "GetResource_" + inputFile.name.replaceAll('[^a-zA-Z0-9]', '_')
                 outputFile.withWriter { out ->
                     def inputBytes = inputFile.bytes
                     out.print '''#include <stddef.h>
-#include <wpi/StringRef.h>
+#include <string_view>
 extern "C" {
 static const unsigned char contents[] = { '''
 
@@ -36,8 +36,8 @@
                     if (!namespace.isEmpty()) {
                         out.println "namespace ${namespace} {"
                     }
-                    out.println """wpi::StringRef ${funcName}() {
-  return wpi::StringRef(reinterpret_cast<const char*>(contents), ${fileBytes.size()});
+                    out.println """std::string_view ${funcName}() {
+  return std::string_view(reinterpret_cast<const char*>(contents), ${fileBytes.size()});
 }"""
                     if (!namespace.isEmpty()) {
                         out.println '}'
diff --git a/third_party/allwpilib/shared/singlelib/singlelib.cpp b/third_party/allwpilib/shared/singlelib/singlelib.cpp
index 6c9476a..85c04a3 100644
--- a/third_party/allwpilib/shared/singlelib/singlelib.cpp
+++ b/third_party/allwpilib/shared/singlelib/singlelib.cpp
@@ -1,6 +1,3 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/README.md b/third_party/allwpilib/simulation/frc_gazebo_plugins/README.md
index 4a5a59a..b89bbc5 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/README.md
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/README.md
@@ -13,7 +13,7 @@
 
 This command:
 
-    `./gradlew build -PmakeSim`
+    `./gradlew build -PforceGazebo`
 
 will force it to attempt to build.
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/build.gradle b/third_party/allwpilib/simulation/frc_gazebo_plugins/build.gradle
index d9db2b9..f42b70b 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/build.gradle
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/build.gradle
@@ -21,10 +21,10 @@
     gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
 } catch(Exception ex) { }
 
-if (project.hasProperty("makeSim")) {
+if (project.hasProperty("forceGazebo")) {
     if (!gazebo_version?.trim()) {
         println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
-        println "makeSim set. Forcing build - failure likely."
+        println "forceGazebo set. Forcing build - failure likely."
     }
 } else {
     ext.skip_frc_plugins = true
@@ -56,7 +56,6 @@
                 cpp.lib library:  "${component.name}", linkage: "static"
             }
         }
-
     }
 
     /* TODO:  Finish writing the test case */
@@ -97,7 +96,7 @@
             //    project(':gmock').addGmockToLinker(it)
             //}
             //else {
-                buildable = false
+            buildable = false
             //}
         }
     }
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/cpp/clock.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/cpp/clock.cpp
index c7440eb..d588708 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/cpp/clock.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/cpp/clock.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "clock.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/headers/clock.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/headers/clock.h
index fcdc2bd..3ff7d12 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/headers/clock.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clock/headers/clock.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clockTest/cpp/ClockTest.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clockTest/cpp/ClockTest.cpp
new file mode 100644
index 0000000..84bde78
--- /dev/null
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clockTest/cpp/ClockTest.cpp
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <simulation/gz_msgs/msgs.h>
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/gazebo_client.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/sensors/sensors.hh>
+#include <gazebo/transport/transport.hh>
+
+#include "gtest/gtest.h"
+
+using namespace testing;
+
+static char* library;
+static char* world_sdf;
+static double latest_time;
+
+void cb(gazebo::msgs::ConstFloat64Ptr& msg) {
+  latest_time = msg->data();
+}
+
+TEST(ClockTest, Clock) {
+  gazebo::physics::WorldPtr world;
+
+  ASSERT_TRUE(library);
+  ASSERT_TRUE(!access(library, X_OK | R_OK));
+  ASSERT_TRUE(world_sdf);
+  ASSERT_TRUE(!access(world_sdf, R_OK));
+
+  gazebo::addPlugin(library);
+  ASSERT_TRUE(gazebo::setupServer());
+
+  world = gazebo::loadWorld(world_sdf);
+  ASSERT_TRUE(world != NULL);
+
+  ASSERT_TRUE(gazebo::client::setup());
+
+  gazebo::transport::NodePtr node(new gazebo::transport::Node());
+  node->Init();
+
+  gazebo::transport::SubscriberPtr sub =
+      node->Subscribe("/gazebo/frc/time", cb);
+
+  gazebo::sensors::run_once(true);
+  gazebo::sensors::run_threads();
+  gazebo::common::Time::MSleep(1);
+
+  double start = latest_time;
+
+  for (unsigned int i = 0; i < 1000; ++i) {
+    gazebo::runWorld(world, 1);
+    gazebo::sensors::run_once();
+    gazebo::common::Time::MSleep(1);
+  }
+
+  double end = latest_time;
+
+  ASSERT_TRUE(end > start);
+  ASSERT_TRUE(gazebo::client::shutdown());
+  ASSERT_TRUE(gazebo::shutdown());
+}
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+
+  if (argc >= 1) {
+    library = argv[1];
+  }
+
+  if (argc >= 2) {
+    world_sdf = argv[2];
+  }
+
+  return RUN_ALL_TESTS();
+}
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clockTest/cpp/clockTest.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clockTest/cpp/clockTest.cpp
deleted file mode 100644
index 275425c..0000000
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/clockTest/cpp/clockTest.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <simulation/gz_msgs/msgs.h>
-
-#include <gazebo/gazebo.hh>
-#include <gazebo/gazebo_client.hh>
-#include <gazebo/msgs/msgs.hh>
-#include <gazebo/sensors/sensors.hh>
-#include <gazebo/transport/transport.hh>
-
-#include "gtest/gtest.h"
-
-using namespace testing;
-
-static char* library;
-static char* world_sdf;
-static double latest_time;
-
-void cb(gazebo::msgs::ConstFloat64Ptr& msg) { latest_time = msg->data(); }
-
-TEST(ClockTests, test_clock) {
-  gazebo::physics::WorldPtr world;
-
-  ASSERT_TRUE(library);
-  ASSERT_TRUE(!access(library, X_OK | R_OK));
-  ASSERT_TRUE(world_sdf);
-  ASSERT_TRUE(!access(world_sdf, R_OK));
-
-  gazebo::addPlugin(library);
-  ASSERT_TRUE(gazebo::setupServer());
-
-  world = gazebo::loadWorld(world_sdf);
-  ASSERT_TRUE(world != NULL);
-
-  ASSERT_TRUE(gazebo::client::setup());
-
-  gazebo::transport::NodePtr node(new gazebo::transport::Node());
-  node->Init();
-
-  gazebo::transport::SubscriberPtr sub =
-      node->Subscribe("/gazebo/frc/time", cb);
-
-  gazebo::sensors::run_once(true);
-  gazebo::sensors::run_threads();
-  gazebo::common::Time::MSleep(1);
-
-  double start = latest_time;
-
-  for (unsigned int i = 0; i < 1000; ++i) {
-    gazebo::runWorld(world, 1);
-    gazebo::sensors::run_once();
-    gazebo::common::Time::MSleep(1);
-  }
-
-  double end = latest_time;
-
-  ASSERT_TRUE(end > start);
-  ASSERT_TRUE(gazebo::client::shutdown());
-  ASSERT_TRUE(gazebo::shutdown());
-}
-
-int main(int argc, char** argv) {
-  testing::InitGoogleTest(&argc, argv);
-
-  if (argc >= 1) library = argv[1];
-
-  if (argc >= 2) world_sdf = argv[2];
-
-  return RUN_ALL_TESTS();
-}
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/cpp/dc_motor.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/cpp/dc_motor.cpp
index 8cc3039..7951dfc 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/cpp/dc_motor.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/cpp/dc_motor.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "dc_motor.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/headers/dc_motor.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/headers/dc_motor.h
index f9c2523..44440ac 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/headers/dc_motor.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/dc_motor/headers/dc_motor.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/cpp/drive_motor.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/cpp/drive_motor.cpp
index e6511bb..01679e9 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/cpp/drive_motor.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/cpp/drive_motor.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "drive_motor.h"
 
@@ -90,7 +87,9 @@
 
 static double computeForce(double input, double velocity, double max) {
   double output = input;
-  if (max == 0.0) return output;
+  if (max == 0.0) {
+    return output;
+  }
   if (std::fabs(velocity) >= max) {
     output = 0;
   } else {
@@ -107,7 +106,8 @@
   ignition::math::Vector3d velocity = parent->GetRelativeLinearVel().Ign();
 #endif
 
-  if (signal == 0) return;
+  if (signal == 0)
+    return;
 
   double x = computeForce(signal * dx * multiplier, velocity.X(),
                           std::fabs(maxSpeed * dx));
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/headers/drive_motor.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/headers/drive_motor.h
index 2033223..8516961 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/headers/drive_motor.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/drive_motor/headers/drive_motor.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/cpp/encoder.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/cpp/encoder.cpp
index 37551f2..c204701 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/cpp/encoder.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/cpp/encoder.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "encoder.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h
index 4ecc17a..72f3542 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/encoder/headers/encoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/cpp/gyro.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/cpp/gyro.cpp
index 65d42b7..b30d9b7 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/cpp/gyro.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/cpp/gyro.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gyro.h"
 
@@ -23,9 +20,12 @@
   }
 
   std::string axisString = sdf->Get<std::string>("axis");
-  if (axisString == "roll") axis = Roll;
-  if (axisString == "pitch") axis = Pitch;
-  if (axisString == "yaw") axis = Yaw;
+  if (axisString == "roll")
+    axis = Roll;
+  if (axisString == "pitch")
+    axis = Pitch;
+  if (axisString == "yaw")
+    axis = Yaw;
 
   if (sdf->HasElement("units")) {
     radians = sdf->Get<std::string>("units") != "degrees";
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/headers/gyro.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/headers/gyro.h
index 3faa144..adc332b 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/headers/gyro.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/gyro/headers/gyro.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -16,7 +13,7 @@
 #include "simulation/gz_msgs/msgs.h"
 
 /// \brief The axis about which to measure rotation.
-typedef enum { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ } ROTATION;
+enum ROTATION { Roll /*X*/, Pitch /*Y*/, Yaw /*Z*/ };
 
 /**
  * \brief Plugin for reading the speed and relative angle of a link.
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/external_limit_switch.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/external_limit_switch.cpp
index c5780ae..b15ab71 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/external_limit_switch.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/external_limit_switch.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "external_limit_switch.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/internal_limit_switch.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/internal_limit_switch.cpp
index adfd860..f01ceb6 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/internal_limit_switch.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/internal_limit_switch.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "internal_limit_switch.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/limit_switch.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/limit_switch.cpp
index 34b7ae3..c380bae 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/limit_switch.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/cpp/limit_switch.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "limit_switch.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/external_limit_switch.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/external_limit_switch.h
index 2114b71..8a39f61 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/external_limit_switch.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/external_limit_switch.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -22,7 +19,7 @@
   explicit ExternalLimitSwitch(sdf::ElementPtr sdf);
 
   /// \brief Returns true when the switch is triggered.
-  virtual bool Get();
+  bool Get() override;
 
  private:
   gazebo::sensors::ContactSensorPtr sensor;
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/internal_limit_switch.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/internal_limit_switch.h
index 870e8e3..11159c7 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/internal_limit_switch.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/internal_limit_switch.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -21,7 +18,7 @@
   InternalLimitSwitch(gazebo::physics::ModelPtr model, sdf::ElementPtr sdf);
 
   /// \brief Returns true when the switch is triggered.
-  virtual bool Get();
+  bool Get() override;
 
  private:
   gazebo::physics::JointPtr joint;
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/limit_switch.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/limit_switch.h
index 23ae2cc..089cfa1 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/limit_switch.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/limit_switch.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/switch.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/switch.h
index 0387647..cc7018e 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/switch.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/limit_switch/headers/switch.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/cpp/pneumatic_piston.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/cpp/pneumatic_piston.cpp
index f0e4c7e..a1f8d8d 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/cpp/pneumatic_piston.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/cpp/pneumatic_piston.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "pneumatic_piston.h"
 
@@ -81,7 +78,8 @@
        the lack of the forward signal suffices.
        Note that a true simulation would not allow a SingleSolenoid to
        have reverse force, but we put that in the hands of the model builder.*/
-    if (reverse_topic.empty() || reverse_signal) force = reverse_force;
+    if (reverse_topic.empty() || reverse_signal)
+      force = reverse_force;
   }
   joint->SetForce(0, force);
 }
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/headers/pneumatic_piston.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/headers/pneumatic_piston.h
index ba8becf..bcc02b4 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/headers/pneumatic_piston.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/pneumatic_piston/headers/pneumatic_piston.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/cpp/potentiometer.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/cpp/potentiometer.cpp
index 140afd1..9691a4d 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/cpp/potentiometer.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/cpp/potentiometer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "potentiometer.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/headers/potentiometer.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/headers/potentiometer.h
index f9111cb..5fe6d66 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/headers/potentiometer.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/potentiometer/headers/potentiometer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/cpp/rangefinder.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/cpp/rangefinder.cpp
index a01f10f..be8bbe6 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/cpp/rangefinder.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/cpp/rangefinder.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "rangefinder.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/headers/rangefinder.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/headers/rangefinder.h
index 6a395b6..6fee59b 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/headers/rangefinder.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/rangefinder/headers/rangefinder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/cpp/servo.cpp b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/cpp/servo.cpp
index ca9d361..e090d0c 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/cpp/servo.cpp
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/cpp/servo.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "servo.h"
 
diff --git a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/headers/servo.h b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/headers/servo.h
index 6d8ab08..d934385 100644
--- a/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/headers/servo.h
+++ b/third_party/allwpilib/simulation/frc_gazebo_plugins/src/servo/headers/servo.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/gz_msgs/README.md b/third_party/allwpilib/simulation/gz_msgs/README.md
index 7df7a79..6850be7 100644
--- a/third_party/allwpilib/simulation/gz_msgs/README.md
+++ b/third_party/allwpilib/simulation/gz_msgs/README.md
@@ -7,6 +7,6 @@
 
 If it's not found via pkg-config, then it's build is diabled.
 
-You can force it by specifying -PmakeSim on the gradle command line.
+You can force it by specifying -PforceGazebo on the gradle command line.
 
 If you are installing FRCSim with the script, then this *should* have be done for you.
diff --git a/third_party/allwpilib/simulation/gz_msgs/build.gradle b/third_party/allwpilib/simulation/gz_msgs/build.gradle
index d07dc09..ccc933e 100644
--- a/third_party/allwpilib/simulation/gz_msgs/build.gradle
+++ b/third_party/allwpilib/simulation/gz_msgs/build.gradle
@@ -1,7 +1,7 @@
 plugins {
     id 'cpp'
     id 'java'
-    id 'com.google.protobuf' version '0.8.8'
+    id 'com.google.protobuf' version '0.8.17'
     id 'edu.wpi.first.NativeUtils'
 }
 
@@ -13,11 +13,9 @@
 apply from: "${rootDir}/shared/config.gradle"
 
 /* Use a sort of poor man's autoconf to find the protobuf development
-   files; on Debian, those are supplied by libprotobuf-dev.
-
-   This should get skipped on Windows.
-
-   TODO:  Add Windows support for the simulation code */
+ files; on Debian, those are supplied by libprotobuf-dev.
+ This should get skipped on Windows.
+ TODO:  Add Windows support for the simulation code */
 
 def protobuf_version = ""
 try {
@@ -26,10 +24,10 @@
 } catch(Exception ex) {
 }
 
-if (project.hasProperty("makeSim")) {
+if (project.hasProperty("forceGazebo")) {
     if (!protobuf_version?.trim()) {
         println "Protobuf is not available. (pkg-config --modversion protobuf failed)"
-        println "makeSim set. Forcing build - failure likely."
+        println "forceGazebo set. Forcing build - failure likely."
     }
 } else {
     ext.skip_gz_msgs = true
@@ -41,14 +39,14 @@
 }
 
 dependencies {
-      implementation "com.google.protobuf:protobuf-java:${protobuf_version}"
-      implementation "com.google.protobuf:protoc:${protobuf_version}"
+    implementation "com.google.protobuf:protobuf-java:${protobuf_version}"
+    implementation "com.google.protobuf:protoc:${protobuf_version}"
 }
 
 /* There is a nice gradle plugin for protobuf, and the protoc tool
-   is included; using it simplifies our build process.
-   The trick is that we have to use the same version as the system
-   copy of libprotobuf-dev */
+ is included; using it simplifies our build process.
+ The trick is that we have to use the same version as the system
+ copy of libprotobuf-dev */
 protobuf {
     protoc {
         artifact = "com.google.protobuf:protoc:${protobuf_version}"
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/CMakeLists.txt b/third_party/allwpilib/simulation/halsim_ds_socket/CMakeLists.txt
index 3e2518c..bc9adb1 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/CMakeLists.txt
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/CMakeLists.txt
@@ -4,7 +4,7 @@
 
 file(GLOB halsim_ds_socket_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_ds_socket MODULE ${halsim_ds_socket_src})
+add_library(halsim_ds_socket SHARED ${halsim_ds_socket_src})
 wpilib_target_warnings(halsim_ds_socket)
 set_target_properties(halsim_ds_socket PROPERTIES DEBUG_POSTFIX "d")
 target_link_libraries(halsim_ds_socket PUBLIC hal)
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/build.gradle b/third_party/allwpilib/simulation/halsim_ds_socket/build.gradle
index 440e5d9..63c5576 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/build.gradle
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/build.gradle
@@ -49,7 +49,7 @@
             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
             lib library: pluginName, linkage: 'shared'
             if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
             }
         }
     }
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
index 2c1e83a..cf3adc6 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/dev/native/cpp/main.cpp
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <thread>
 
 #include <hal/DriverStation.h>
 #include <hal/HALBase.h>
-#include <wpi/Format.h>
-#include <wpi/raw_ostream.h>
 
 extern "C" int HALSIM_InitExtension(void);
 
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
index 4a87f13..1aff988 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/DSCommPacket.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "DSCommPacket.h"
 
@@ -15,8 +12,7 @@
 
 #include <hal/simulation/DriverStationData.h>
 #include <hal/simulation/MockHooks.h>
-#include <wpi/ArrayRef.h>
-#include <wpi/Format.h>
+#include <wpi/span.h>
 
 using namespace halsim;
 
@@ -49,8 +45,10 @@
   m_alliance_station = static_cast<HAL_AllianceStationID>(station_code);
 }
 
-void DSCommPacket::ReadMatchtimeTag(wpi::ArrayRef<uint8_t> tagData) {
-  if (tagData.size() < 6) return;
+void DSCommPacket::ReadMatchtimeTag(wpi::span<const uint8_t> tagData) {
+  if (tagData.size() < 6) {
+    return;
+  }
 
   uint32_t store = tagData[2] << 24;
   store |= tagData[3] << 16;
@@ -65,7 +63,7 @@
   m_match_time = matchTime;
 }
 
-void DSCommPacket::ReadJoystickTag(wpi::ArrayRef<uint8_t> dataInput,
+void DSCommPacket::ReadJoystickTag(wpi::span<const uint8_t> dataInput,
                                    int index) {
   DSCommJoystickPacket& stick = m_joystick_packets[index];
   stick.ResetUdp();
@@ -74,7 +72,7 @@
     return;
   }
 
-  dataInput = dataInput.slice(2);
+  dataInput = dataInput.subspan(2);
 
   // Read axes
   int axesLength = dataInput[0];
@@ -88,7 +86,7 @@
   }
   stick.axes.count = axesLength;
 
-  dataInput = dataInput.slice(1 + axesLength);
+  dataInput = dataInput.subspan(1 + axesLength);
 
   // Read Buttons
   int buttonCount = dataInput[0];
@@ -99,7 +97,7 @@
   }
   stick.buttons.count = buttonCount;
 
-  dataInput = dataInput.slice(1 + numBytes);
+  dataInput = dataInput.subspan(1 + numBytes);
 
   int povsLength = dataInput[0];
   for (int i = 0; i < povsLength * 2; i += 2) {
@@ -114,11 +112,11 @@
 /*----------------------------------------------------------------------------
 **  Communication methods
 **--------------------------------------------------------------------------*/
-void DSCommPacket::DecodeTCP(wpi::ArrayRef<uint8_t> packet) {
+void DSCommPacket::DecodeTCP(wpi::span<const uint8_t> packet) {
   // No header
   while (!packet.empty()) {
     int tagLength = packet[0] << 8 | packet[1];
-    auto tagPacket = packet.slice(0, tagLength + 2);
+    auto tagPacket = packet.subspan(0, tagLength + 2);
 
     if (tagLength == 0) {
       return;
@@ -135,12 +133,14 @@
         ReadNewMatchInfoTag(tagPacket);
         break;
     }
-    packet = packet.slice(tagLength + 2);
+    packet = packet.subspan(tagLength + 2);
   }
 }
 
-void DSCommPacket::DecodeUDP(wpi::ArrayRef<uint8_t> packet) {
-  if (packet.size() < 6) return;
+void DSCommPacket::DecodeUDP(wpi::span<const uint8_t> packet) {
+  if (packet.size() < 6) {
+    return;
+  }
   // Decode fixed header
   m_hi = packet[0];
   m_lo = packet[1];
@@ -149,17 +149,19 @@
   SetAlliance(packet[5]);
 
   // Return if packet finished
-  if (packet.size() == 6) return;
+  if (packet.size() == 6) {
+    return;
+  }
 
   // Else, handle tagged data
-  packet = packet.slice(6);
+  packet = packet.subspan(6);
 
   int joystickNum = 0;
 
   // Loop to handle multiple tags
   while (!packet.empty()) {
     auto tagLength = packet[0];
-    auto tagPacket = packet.slice(0, tagLength + 1);
+    auto tagPacket = packet.subspan(0, tagLength + 1);
 
     switch (packet[1]) {
       case kJoystickDataTag:
@@ -170,13 +172,15 @@
         ReadMatchtimeTag(tagPacket);
         break;
     }
-    packet = packet.slice(tagLength + 1);
+    packet = packet.subspan(tagLength + 1);
   }
 }
 
-void DSCommPacket::ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data) {
+void DSCommPacket::ReadNewMatchInfoTag(wpi::span<const uint8_t> data) {
   // Size 2 bytes, tag 1 byte
-  if (data.size() <= 3) return;
+  if (data.size() <= 3) {
+    return;
+  }
 
   int nameLength = std::min<size_t>(data[3], sizeof(matchInfo.eventName) - 1);
 
@@ -186,9 +190,11 @@
 
   matchInfo.eventName[nameLength] = '\0';
 
-  data = data.slice(4 + nameLength);
+  data = data.subspan(4 + nameLength);
 
-  if (data.size() < 4) return;
+  if (data.size() < 4) {
+    return;
+  }
 
   matchInfo.matchType = static_cast<HAL_MatchType>(
       data[0]);  // None, Practice, Qualification, Elimination, Test
@@ -198,9 +204,11 @@
   HALSIM_SetMatchInfo(&matchInfo);
 }
 
-void DSCommPacket::ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data) {
+void DSCommPacket::ReadGameSpecificMessageTag(wpi::span<const uint8_t> data) {
   // Size 2 bytes, tag 1 byte
-  if (data.size() <= 3) return;
+  if (data.size() <= 3) {
+    return;
+  }
 
   int length = std::min<size_t>(((data[0] << 8) | data[1]) - 1,
                                 sizeof(matchInfo.gameSpecificMessage));
@@ -212,9 +220,11 @@
 
   HALSIM_SetMatchInfo(&matchInfo);
 }
-void DSCommPacket::ReadJoystickDescriptionTag(wpi::ArrayRef<uint8_t> data) {
-  if (data.size() < 3) return;
-  data = data.slice(3);
+void DSCommPacket::ReadJoystickDescriptionTag(wpi::span<const uint8_t> data) {
+  if (data.size() < 3) {
+    return;
+  }
+  data = data.subspan(3);
   int joystickNum = data[0];
   DSCommJoystickPacket& packet = m_joystick_packets[joystickNum];
   packet.ResetTcp();
@@ -225,14 +235,16 @@
   for (int i = 0; i < nameLength; i++) {
     packet.descriptor.name[i] = data[4 + i];
   }
-  data = data.slice(4 + nameLength);
+  data = data.subspan(4 + nameLength);
   packet.descriptor.name[nameLength] = '\0';
   int axesCount = data[0];
   packet.descriptor.axisCount = axesCount;
-  for (int i = 0; i < axesCount; i++) {
+  for (int i = 0,
+           len = std::min<int>(axesCount, sizeof(packet.descriptor.axisTypes));
+       i < len; i++) {
     packet.descriptor.axisTypes[i] = data[1 + i];
   }
-  data = data.slice(1 + axesCount);
+  data = data.subspan(1 + axesCount);
 
   packet.descriptor.buttonCount = data[0];
   packet.descriptor.povCount = data[1];
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
index bafe8b3..799cdd9 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*----------------------------------------------------------------------------
 **  This extension reimplements enough of the FRC_Network layer to enable the
@@ -17,13 +14,14 @@
 #include <sys/types.h>
 
 #include <atomic>
+#include <cstdio>
 #include <cstring>
+#include <string_view>
 
 #include <DSCommPacket.h>
+#include <fmt/format.h>
 #include <hal/Extensions.h>
 #include <wpi/EventLoopRunner.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/raw_uv_ostream.h>
 #include <wpi/uv/Tcp.h>
 #include <wpi/uv/Timer.h>
@@ -53,14 +51,16 @@
 }
 
 static void HandleTcpDataStream(Buffer& buf, size_t size, DataStore& store) {
-  wpi::StringRef data{buf.base, size};
+  std::string_view data{buf.base, size};
   while (!data.empty()) {
     if (store.m_frameSize == (std::numeric_limits<size_t>::max)()) {
       if (store.m_frame.size() < 2u) {
         size_t toCopy = (std::min)(2u - store.m_frame.size(), data.size());
-        store.m_frame.append(data.bytes_begin(), data.bytes_begin() + toCopy);
-        data = data.drop_front(toCopy);
-        if (store.m_frame.size() < 2u) return;  // need more data
+        store.m_frame.append(data.data(), data.data() + toCopy);
+        data.remove_prefix(toCopy);
+        if (store.m_frame.size() < 2u) {
+          return;  // need more data
+        }
       }
       store.m_frameSize = (static_cast<uint16_t>(store.m_frame[0]) << 8) |
                           static_cast<uint16_t>(store.m_frame[1]);
@@ -68,8 +68,8 @@
     if (store.m_frameSize != (std::numeric_limits<size_t>::max)()) {
       size_t need = store.m_frameSize - (store.m_frame.size() - 2);
       size_t toCopy = (std::min)(need, data.size());
-      store.m_frame.append(data.bytes_begin(), data.bytes_begin() + toCopy);
-      data = data.drop_front(toCopy);
+      store.m_frame.append(data.data(), data.data() + toCopy);
+      data.remove_prefix(toCopy);
       need -= toCopy;
       if (need == 0) {
         auto ds = store.dsPacket;
@@ -116,13 +116,12 @@
   struct sockaddr_in simAddr;
   NameToAddr("127.0.0.1", 1135, &simAddr);
   simLoopTimer->timeout.connect([udpLocal = udp.get(), simAddr] {
-    udpLocal->Send(simAddr, wpi::ArrayRef<Buffer>{singleByte.get(), 1},
-                   [](auto buf, Error err) {
-                     if (err) {
-                       wpi::errs() << err.str() << "\n";
-                       wpi::errs().flush();
-                     }
-                   });
+    udpLocal->Send(simAddr, {singleByte.get(), 1}, [](auto buf, Error err) {
+      if (err) {
+        fmt::print(stderr, "{}\n", err.str());
+        std::fflush(stderr);
+      }
+    });
   });
   simLoopTimer->Start(Timer::Time{100}, Timer::Time{100});
 
@@ -131,8 +130,7 @@
                                                const sockaddr& recSock,
                                                unsigned int port) {
     auto ds = udpLocal->GetLoop()->GetData<halsim::DSCommPacket>();
-    ds->DecodeUDP(
-        wpi::ArrayRef<uint8_t>{reinterpret_cast<uint8_t*>(buf.base), len});
+    ds->DecodeUDP({reinterpret_cast<uint8_t*>(buf.base), len});
 
     struct sockaddr_in outAddr;
     std::memcpy(&outAddr, &recSock, sizeof(sockaddr_in));
@@ -147,8 +145,8 @@
     udpLocal->Send(outAddr, sendBufs, [](auto bufs, Error err) {
       GetBufferPool().Release(bufs);
       if (err) {
-        wpi::errs() << err.str() << "\n";
-        wpi::errs().flush();
+        fmt::print(stderr, "{}\n", err.str());
+        std::fflush(stderr);
       }
     });
     ds->SendUDPToHALSim();
@@ -178,12 +176,12 @@
   static bool once = false;
 
   if (once) {
-    wpi::errs() << "Error: cannot invoke HALSIM_InitExtension twice.\n";
+    std::fputs("Error: cannot invoke HALSIM_InitExtension twice.\n", stderr);
     return -1;
   }
   once = true;
 
-  wpi::outs() << "DriverStationSocket Initializing.\n";
+  std::puts("DriverStationSocket Initializing.");
 
   HAL_RegisterExtension("ds_socket", &gDSConnected);
 
@@ -193,7 +191,7 @@
 
   eventLoopRunner->ExecAsync(SetupEventLoop);
 
-  wpi::outs() << "DriverStationSocket Initialized!\n";
+  std::puts("DriverStationSocket Initialized!");
   return 0;
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
index 99851f2..db430a1 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommJoystickPacket.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
index f189d6a..1285711 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/main/native/include/DSCommPacket.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,8 +8,8 @@
 
 #include <DSCommJoystickPacket.h>
 #include <hal/simulation/DriverStationData.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/raw_uv_ostream.h>
+#include <wpi/span.h>
 
 class DSCommPacketTest;
 
@@ -23,8 +20,8 @@
 
  public:
   DSCommPacket(void);
-  void DecodeTCP(wpi::ArrayRef<uint8_t> packet);
-  void DecodeUDP(wpi::ArrayRef<uint8_t> packet);
+  void DecodeTCP(wpi::span<const uint8_t> packet);
+  void DecodeUDP(wpi::span<const uint8_t> packet);
   void SendUDPToHALSim(void);
   void SetupSendBuffer(wpi::raw_uv_ostream& buf);
 
@@ -56,11 +53,11 @@
   void SetAlliance(uint8_t station_code);
   void SetupSendHeader(wpi::raw_uv_ostream& buf);
   void SetupJoystickTag(wpi::raw_uv_ostream& buf);
-  void ReadMatchtimeTag(wpi::ArrayRef<uint8_t> tagData);
-  void ReadJoystickTag(wpi::ArrayRef<uint8_t> data, int index);
-  void ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data);
-  void ReadGameSpecificMessageTag(wpi::ArrayRef<uint8_t> data);
-  void ReadJoystickDescriptionTag(wpi::ArrayRef<uint8_t> data);
+  void ReadMatchtimeTag(wpi::span<const uint8_t> tagData);
+  void ReadJoystickTag(wpi::span<const uint8_t> data, int index);
+  void ReadNewMatchInfoTag(wpi::span<const uint8_t> data);
+  void ReadGameSpecificMessageTag(wpi::span<const uint8_t> data);
+  void ReadJoystickDescriptionTag(wpi::span<const uint8_t> data);
 
   uint8_t m_hi;
   uint8_t m_lo;
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp b/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
index 08caa2f..84df27c 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/DSCommPacketTest.cpp
@@ -1,36 +1,34 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "DSCommPacket.h"
 #include "gtest/gtest.h"
 
 class DSCommPacketTest : public ::testing::Test {
  public:
-  DSCommPacketTest() {}
+  DSCommPacketTest() = default;
 
   void SendJoysticks() { commPacket.SendJoysticks(); }
 
-  halsim::DSCommJoystickPacket& ReadJoystickTag(wpi::ArrayRef<uint8_t> data,
+  halsim::DSCommJoystickPacket& ReadJoystickTag(wpi::span<const uint8_t> data,
                                                 int index) {
     commPacket.ReadJoystickTag(data, index);
     return commPacket.m_joystick_packets[index];
   }
 
-  halsim::DSCommJoystickPacket& ReadDescriptorTag(wpi::ArrayRef<uint8_t> data) {
+  halsim::DSCommJoystickPacket& ReadDescriptorTag(
+      wpi::span<const uint8_t> data) {
     commPacket.ReadJoystickDescriptionTag(data);
     return commPacket.m_joystick_packets[data[3]];
   }
 
-  HAL_MatchInfo& ReadNewMatchInfoTag(wpi::ArrayRef<uint8_t> data) {
+  HAL_MatchInfo& ReadNewMatchInfoTag(wpi::span<const uint8_t> data) {
     commPacket.ReadNewMatchInfoTag(data);
     return commPacket.matchInfo;
   }
 
-  HAL_MatchInfo& ReadGameSpecificTag(wpi::ArrayRef<uint8_t> data) {
+  HAL_MatchInfo& ReadGameSpecificTag(wpi::span<const uint8_t> data) {
     commPacket.ReadGameSpecificMessageTag(data);
     return commPacket.matchInfo;
   }
@@ -70,7 +68,8 @@
     std::array<uint8_t, 12> _buttons{{0, 1, 0, 0, 1, 1, 0, 1, 0, 1, 0, 1}};
 
     std::array<uint8_t, 2> _button_bytes{{0, 0}};
-    for (int btn = 0; btn < 8; btn++) _button_bytes[1] |= _buttons[btn] << btn;
+    for (int btn = 0; btn < 8; btn++)
+      _button_bytes[1] |= _buttons[btn] << btn;
     for (int btn = 8; btn < 12; btn++)
       _button_bytes[0] |= _buttons[btn] << (btn - 8);
 
diff --git a/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
index c6b6c58..6aea19a 100644
--- a/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ds_socket/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <hal/HALBase.h>
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/build.gradle b/third_party/allwpilib/simulation/halsim_gazebo/build.gradle
index d476ef0..8d340d3 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/build.gradle
+++ b/third_party/allwpilib/simulation/halsim_gazebo/build.gradle
@@ -19,10 +19,10 @@
     gazebo_linker_args = "pkg-config --libs gazebo protobuf".execute().text.split()
 } catch(Exception ex) { }
 
-if (project.hasProperty("makeSim")) {
+if (project.hasProperty("forceGazebo")) {
     if (!gazebo_version?.trim()) {
         println "Gazebo development files are not available. (pkg-config --modversion gazebo failed)"
-        println "makeSim set. Forcing build - failure likely."
+        println "forceGazebo set. Forcing build - failure likely."
     }
 } else {
     ext.skip_frc_plugins = true
@@ -35,7 +35,6 @@
 if (!gz_msgs_project.hasProperty('skip_gz_msgs') && !project.hasProperty('skip_frc_plugins')) {
 
     apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
-
 }
 
 model {
@@ -48,6 +47,7 @@
             linker.args gazebo_linker_args
             cppCompiler.args gazebo_cppflags
             lib project: ":simulation:gz_msgs", library: "gz_msgs", linkage: "static"
+            lib project: ":wpiutil", library: "wpiutil", linkage: "static"
         }
     }
 }
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp
index ce20221..d9ce9a9 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboAnalogIn.cpp
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "GazeboAnalogIn.h"
 
-#include <string>
-
+#include <fmt/format.h>
 #include <hal/Power.h>
 #include <hal/Value.h>
 #include <hal/simulation/AnalogInData.h>
@@ -33,14 +29,14 @@
 void GazeboAnalogIn::Listen() {
   if (!m_sub)
     m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
-        "~/simulator/analog/" + std::to_string(m_index),
+        fmt::format("~/simulator/analog/{}", m_index),
         &GazeboAnalogIn::Callback, this);
 }
 
 void GazeboAnalogIn::Callback(const gazebo::msgs::ConstFloat64Ptr& msg) {
   /* This value is going to be divided by the 5V rail in the HAL, so
      we multiply by that value to make the change neutral */
-  int32_t status;
+  int32_t status = 0;
   HALSIM_SetAnalogInVoltage(m_index,
                             msg->data() * HAL_GetUserVoltage5V(&status));
 }
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp
index f95b4f5..85d4ea6 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboDIO.cpp
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "GazeboDIO.h"
 
-#include <string>
-
+#include <fmt/format.h>
 #include <hal/Value.h>
 #include <hal/simulation/DIOData.h>
 #include <hal/simulation/NotifyListener.h>
@@ -32,8 +28,7 @@
 void GazeboDIO::Listen() {
   if (!m_sub)
     m_sub = m_halsim->node.Subscribe<gazebo::msgs::Bool>(
-        "~/simulator/dio/" + std::to_string(m_index), &GazeboDIO::Callback,
-        this);
+        fmt::format("~/simulator/dio/{}", m_index), &GazeboDIO::Callback, this);
 }
 
 void GazeboDIO::Callback(const gazebo::msgs::ConstBoolPtr& msg) {
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp
index 03c2f79..519bf2b 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboEncoder.cpp
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "GazeboEncoder.h"
 
-#include <string>
-
+#include <fmt/format.h>
 #include <hal/Value.h>
 #include <hal/simulation/EncoderData.h>
 #include <hal/simulation/NotifyListener.h>
@@ -33,7 +29,8 @@
 static void encoder_reverse_callback(const char* name, void* param,
                                      const struct HAL_Value* value) {
   GazeboEncoder* encoder = static_cast<GazeboEncoder*>(param);
-  if (encoder->IsInitialized()) encoder->SetReverse(value->data.v_boolean);
+  if (encoder->IsInitialized())
+    encoder->SetReverse(value->data.v_boolean);
 }
 
 GazeboEncoder::GazeboEncoder(int index, HALSimGazebo* halsim) {
@@ -53,21 +50,21 @@
 void GazeboEncoder::Control(const char* command) {
   if (!m_pub) {
     m_pub = m_halsim->node.Advertise<gazebo::msgs::String>(
-        "~/simulator/encoder/dio/" +
-        std::to_string(HALSIM_GetEncoderDigitalChannelA(m_index)) + "/control");
+        fmt::format("~/simulator/encoder/dio/{}/control",
+                    HALSIM_GetEncoderDigitalChannelA(m_index)));
     m_pub->WaitForConnection(gazebo::common::Time(1, 0));
   }
   gazebo::msgs::String msg;
   msg.set_data(command);
-  if (m_pub) m_pub->Publish(msg);
+  if (m_pub)
+    m_pub->Publish(msg);
 }
 
 void GazeboEncoder::Listen() {
   if (!m_sub)
     m_sub = m_halsim->node.Subscribe<gazebo::msgs::Float64>(
-        "~/simulator/encoder/dio/" +
-            std::to_string(HALSIM_GetEncoderDigitalChannelA(m_index)) +
-            "/position",
+        fmt::format("~/simulator/encoder/dio/{}/position",
+                    HALSIM_GetEncoderDigitalChannelA(m_index)),
         &GazeboEncoder::Callback, this);
 }
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboNode.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboNode.cpp
index 13103b7..1c847ca 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboNode.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboNode.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "GazeboNode.h"
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp
index 1802f2e..2549061 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPCM.cpp
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "GazeboPCM.h"
 
-#include <string>
-
+#include <fmt/format.h>
 #include <hal/Value.h>
+#include <hal/simulation/CTREPCMData.h>
 #include <hal/simulation/NotifyListener.h>
-#include <hal/simulation/PCMData.h>
 
 #include "simulation/gz_msgs/msgs.h"
 
@@ -24,7 +20,8 @@
 static void output_callback(const char* name, void* param,
                             const struct HAL_Value* value) {
   GazeboPCM* pcm = static_cast<GazeboPCM*>(param);
-  if (pcm->IsInitialized()) pcm->Publish(value->data.v_boolean);
+  if (pcm->IsInitialized())
+    pcm->Publish(value->data.v_boolean);
 }
 
 GazeboPCM::GazeboPCM(int index, int channel, HALSimGazebo* halsim) {
@@ -32,24 +29,23 @@
   m_channel = channel;
   m_halsim = halsim;
   m_pub = NULL;
-  HALSIM_RegisterPCMSolenoidInitializedCallback(index, channel, init_callback,
-                                                this, true);
-  HALSIM_RegisterPCMSolenoidOutputCallback(index, channel, output_callback,
-                                           this, true);
+  HALSIM_RegisterCTREPCMInitializedCallback(index, init_callback, this, true);
+  HALSIM_RegisterCTREPCMSolenoidOutputCallback(index, channel, output_callback,
+                                               this, true);
 }
 
 void GazeboPCM::Publish(bool value) {
   if (!m_pub) {
     m_pub = m_halsim->node.Advertise<gazebo::msgs::Bool>(
-        "~/simulator/pneumatic/" + std::to_string(m_index + 1) + "/" +
-        std::to_string(m_channel));
+        fmt::format("~/simulator/pneumatic/{}/{}", m_index + 1, m_channel));
     m_pub->WaitForConnection(gazebo::common::Time(1, 0));
   }
   gazebo::msgs::Bool msg;
   msg.set_data(value);
-  if (m_pub) m_pub->Publish(msg);
+  if (m_pub)
+    m_pub->Publish(msg);
 }
 
 void GazeboPCM_SetPressureSwitch(int index, bool value) {
-  HALSIM_SetPCMPressureSwitch(index, value);
+  HALSIM_SetCTREPCMPressureSwitch(index, value);
 }
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp
index a297255..0780f07 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/GazeboPWM.cpp
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "GazeboPWM.h"
 
-#include <string>
-
+#include <fmt/format.h>
 #include <hal/Value.h>
 #include <hal/simulation/NotifyListener.h>
 #include <hal/simulation/PWMData.h>
@@ -24,7 +20,8 @@
 static void speed_callback(const char* name, void* param,
                            const struct HAL_Value* value) {
   GazeboPWM* pwm = static_cast<GazeboPWM*>(param);
-  if (pwm->IsInitialized()) pwm->Publish(value->data.v_double);
+  if (pwm->IsInitialized())
+    pwm->Publish(value->data.v_double);
 }
 
 GazeboPWM::GazeboPWM(int port, HALSimGazebo* halsim) {
@@ -37,10 +34,11 @@
 void GazeboPWM::Publish(double value) {
   if (!m_pub) {
     m_pub = m_halsim->node.Advertise<gazebo::msgs::Float64>(
-        "~/simulator/pwm/" + std::to_string(m_port));
+        fmt::format("~/simulator/pwm/{}", m_port));
     m_pub->WaitForConnection(gazebo::common::Time(1, 0));
   }
   gazebo::msgs::Float64 msg;
   msg.set_data(value);
-  if (m_pub) m_pub->Publish(msg);
+  if (m_pub)
+    m_pub->Publish(msg);
 }
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/main.cpp
index fa1c85a..a3d0576 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/cpp/main.cpp
@@ -1,12 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
-
+#include <fmt/core.h>
 #include <hal/Ports.h>
 
 #include "GazeboAnalogIn.h"
@@ -22,14 +18,14 @@
 
 extern "C" {
 int HALSIM_InitExtension(void) {
-  std::cout << "Gazebo Simulator Initializing." << std::endl;
+  fmt::print("Gazebo Simulator Initializing.\n");
 
   if (!halsim.node.Connect()) {
-    std::cerr << "Error: unable to connect to Gazebo.  Is it running?."
-              << std::endl;
+    fmt::print(stderr,
+               "Error: unable to connect to Gazebo.  Is it running?.\n");
     return -1;
   }
-  std::cout << "Gazebo Simulator Connected." << std::endl;
+  fmt::print("Gazebo Simulator Connected.\n");
 
   for (int i = 0; i < HALSimGazebo::kPWMCount; i++)
     halsim.pwms[i] = new GazeboPWM(i, &halsim);
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboAnalogIn.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboAnalogIn.h
index 9b1bbde..c54db0d 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboAnalogIn.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboAnalogIn.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboDIO.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboDIO.h
index e96650e..25ef0ad 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboDIO.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboDIO.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboEncoder.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboEncoder.h
index fafeb6a..b25a688 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboEncoder.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboEncoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboNode.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboNode.h
index cc68599..75acc55 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboNode.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboNode.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPCM.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPCM.h
index d74b20f..f964f3f 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPCM.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPCM.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPWM.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPWM.h
index 1321e8a..f4b131f 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPWM.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/GazeboPWM.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/HALSimGazebo.h b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/HALSimGazebo.h
index f7d8193..c1e454a 100644
--- a/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/HALSimGazebo.h
+++ b/third_party/allwpilib/simulation/halsim_gazebo/src/main/native/include/HALSimGazebo.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gui/CMakeLists.txt b/third_party/allwpilib/simulation/halsim_gui/CMakeLists.txt
index 424ff5b..949f9f1 100644
--- a/third_party/allwpilib/simulation/halsim_gui/CMakeLists.txt
+++ b/third_party/allwpilib/simulation/halsim_gui/CMakeLists.txt
@@ -5,12 +5,12 @@
 
 file(GLOB halsim_gui_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_gui MODULE ${halsim_gui_src})
+add_library(halsim_gui SHARED ${halsim_gui_src})
 wpilib_target_warnings(halsim_gui)
 set_target_properties(halsim_gui PROPERTIES DEBUG_POSTFIX "d")
 
 wpilib_link_macos_gui(halsim_gui)
-target_link_libraries(halsim_gui PUBLIC hal ntcore wpimath PRIVATE wpigui)
+target_link_libraries(halsim_gui PUBLIC hal wpimath PRIVATE libglassnt libglass)
 
 target_include_directories(halsim_gui PRIVATE src/main/native/include)
 
diff --git a/third_party/allwpilib/simulation/halsim_gui/build.gradle b/third_party/allwpilib/simulation/halsim_gui/build.gradle
index 0fd5c2c..7c26ac8 100644
--- a/third_party/allwpilib/simulation/halsim_gui/build.gradle
+++ b/third_party/allwpilib/simulation/halsim_gui/build.gradle
@@ -3,8 +3,6 @@
     description = "A plugin that creates a simulation gui"
 
     ext {
-        includeWpiutil = true
-        includeNtCore = true
         pluginName = 'halsim_gui'
     }
 
@@ -23,8 +21,12 @@
     model {
         binaries {
             all {
-                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':glass', library: 'glassnt', linkage: 'static'
+                lib project: ':glass', library: 'glass', linkage: 'static'
                 lib project: ':wpigui', library: 'wpigui', linkage: 'static'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 nativeUtils.useRequiredLibrary(it, 'imgui_static')
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
                     it.buildable = false
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/dev/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_gui/src/dev/native/cpp/main.cpp
index d23379d..a3e363e 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/dev/native/cpp/main.cpp
@@ -1,8 +1,5 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 int main() {}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp
deleted file mode 100644
index fa772ad..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.cpp
+++ /dev/null
@@ -1,75 +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 "AccelerometerGui.h"
-
-#include <cstdio>
-#include <memory>
-
-#include <hal/Value.h>
-#include <hal/simulation/AccelerometerData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "SimDeviceGui.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerX, "X Accel");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerY, "Y Accel");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerZ, "Z Accel");
-}  // namespace
-
-static std::unique_ptr<AccelerometerXSource> gAccelXSource;
-static std::unique_ptr<AccelerometerYSource> gAccelYSource;
-static std::unique_ptr<AccelerometerZSource> gAccelZSource;
-
-static void UpdateAccelSources() {
-  if (!HALSIM_GetAccelerometerActive(0)) return;
-  if (!gAccelXSource) gAccelXSource = std::make_unique<AccelerometerXSource>(0);
-  if (!gAccelYSource) gAccelYSource = std::make_unique<AccelerometerYSource>(0);
-  if (!gAccelZSource) gAccelZSource = std::make_unique<AccelerometerZSource>(0);
-}
-
-static void DisplayAccelerometers() {
-  if (!HALSIM_GetAccelerometerActive(0)) return;
-  if (SimDeviceGui::StartDevice("BuiltInAccel")) {
-    HAL_Value value;
-
-    // Range
-    value = HAL_MakeEnum(HALSIM_GetAccelerometerRange(0));
-    static const char* rangeOptions[] = {"2G", "4G", "8G"};
-    SimDeviceGui::DisplayValue("Range", true, &value, rangeOptions, 3);
-
-    // X Accel
-    value = HAL_MakeDouble(gAccelXSource->GetValue());
-    if (SimDeviceGui::DisplayValueSource("X Accel", false, &value,
-                                         gAccelXSource.get()))
-      HALSIM_SetAccelerometerX(0, value.data.v_double);
-
-    // Y Accel
-    value = HAL_MakeDouble(gAccelYSource->GetValue());
-    if (SimDeviceGui::DisplayValueSource("Y Accel", false, &value,
-                                         gAccelYSource.get()))
-      HALSIM_SetAccelerometerY(0, value.data.v_double);
-
-    // Z Accel
-    value = HAL_MakeDouble(gAccelZSource->GetValue());
-    if (SimDeviceGui::DisplayValueSource("Z Accel", false, &value,
-                                         gAccelZSource.get()))
-      HALSIM_SetAccelerometerZ(0, value.data.v_double);
-
-    SimDeviceGui::FinishDevice();
-  }
-}
-
-void AccelerometerGui::Initialize() {
-  HALSimGui::AddExecute(UpdateAccelSources);
-  SimDeviceGui::Add(DisplayAccelerometers);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.h
deleted file mode 100644
index e1fd81b..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class AccelerometerGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerSimGui.cpp
new file mode 100644
index 0000000..2267656
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerSimGui.cpp
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "AccelerometerSimGui.h"
+
+#include <glass/hardware/Accelerometer.h>
+#include <glass/other/DeviceTree.h>
+
+#include <memory>
+
+#include <hal/Value.h>
+#include <hal/simulation/AccelerometerData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+#include "SimDeviceGui.h"
+
+using namespace glass;
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerX, "X Accel");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerY, "Y Accel");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AccelerometerZ, "Z Accel");
+
+class AccelerometerSimModel : public glass::AccelerometerModel {
+ public:
+  explicit AccelerometerSimModel(int32_t index)
+      : m_index{index}, m_xData{m_index}, m_yData{m_index}, m_zData{m_index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetAccelerometerActive(m_index); }
+
+  glass::DataSource* GetXData() override { return &m_xData; }
+  glass::DataSource* GetYData() override { return &m_yData; }
+  glass::DataSource* GetZData() override { return &m_zData; }
+
+  int GetRange() override { return HALSIM_GetAccelerometerRange(m_index); }
+
+  void SetX(double val) override { HALSIM_SetAccelerometerX(m_index, val); }
+  void SetY(double val) override { HALSIM_SetAccelerometerY(m_index, val); }
+  void SetZ(double val) override { HALSIM_SetAccelerometerZ(m_index, val); }
+  void SetRange(int val) override {
+    HALSIM_SetAccelerometerRange(m_index,
+                                 static_cast<HAL_AccelerometerRange>(val));
+  }
+
+ private:
+  int32_t m_index;
+  AccelerometerXSource m_xData;
+  AccelerometerYSource m_yData;
+  AccelerometerZSource m_zData;
+};
+}  // namespace
+
+void AccelerometerSimGui::Initialize() {
+  SimDeviceGui::GetDeviceTree().Add(
+      std::make_unique<AccelerometerSimModel>(0), [](glass::Model* model) {
+        glass::DisplayAccelerometerDevice(
+            static_cast<AccelerometerSimModel*>(model));
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerSimGui.h
new file mode 100644
index 0000000..9028324
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AccelerometerSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class AccelerometerSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp
index 36b085c..23a5238 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.cpp
@@ -1,126 +1,113 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "AddressableLEDGui.h"
 
+#include <glass/hardware/LEDDisplay.h>
+
 #include <hal/Ports.h>
 #include <hal/simulation/AddressableLEDData.h>
 #include <imgui.h>
-#include <imgui_internal.h>
-#include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
 
-#include "ExtraGuiWidgets.h"
 #include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
 namespace {
-struct LEDDisplayInfo {
-  int numColumns = 10;
-  LEDConfig config;
+class AddressableLEDModel : public glass::LEDDisplayModel {
+ public:
+  explicit AddressableLEDModel(int32_t index) : m_index{index} {}
 
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out);
+  void Update() override {}
+  bool Exists() override {
+    return HALSIM_GetAddressableLEDInitialized(m_index);
+  }
+
+  bool IsRunning() override { return HALSIM_GetAddressableLEDRunning(m_index); }
+
+  wpi::span<const Data> GetData(wpi::SmallVectorImpl<Data>&) override {
+    size_t length = HALSIM_GetAddressableLEDData(m_index, m_data);
+    return {reinterpret_cast<Data*>(m_data), length};
+  }
+
+ private:
+  int32_t m_index;
+
+  HAL_AddressableLEDData m_data[HAL_kAddressableLEDMaxLength];
+};
+
+class AddressableLEDsModel : public glass::LEDDisplaysModel {
+ public:
+  AddressableLEDsModel() : m_models(HAL_GetNumAddressableLEDs()) {}
+
+  void Update() override;
+  bool Exists() override;
+
+  size_t GetNumLEDDisplays() override { return m_models.size(); }
+
+  void ForEachLEDDisplay(
+      wpi::function_ref<void(glass::LEDDisplayModel& model, int index)> func)
+      override;
+
+ private:
+  std::vector<std::unique_ptr<AddressableLEDModel>> m_models;
 };
 }  // namespace
 
-static IniSaver<LEDDisplayInfo> gDisplaySettings{"AddressableLED"};
-
-bool LEDDisplayInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name == "columns") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    numColumns = num;
-  } else if (name == "serpentine") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    config.serpentine = num != 0;
-  } else if (name == "order") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    config.order = static_cast<LEDConfig::Order>(num);
-  } else if (name == "start") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    config.start = static_cast<LEDConfig::Start>(num);
-  } else {
-    return false;
-  }
-  return true;
-}
-
-void LEDDisplayInfo::WriteIni(ImGuiTextBuffer* out) {
-  out->appendf("columns=%d\nserpentine=%d\norder=%d\nstart=%d\n", numColumns,
-               config.serpentine ? 1 : 0, static_cast<int>(config.order),
-               static_cast<int>(config.start));
-}
-
-static void DisplayAddressableLEDs() {
-  bool hasAny = false;
-  static const int numLED = HAL_GetNumAddressableLEDs();
-
-  for (int i = 0; i < numLED; ++i) {
-    if (!HALSIM_GetAddressableLEDInitialized(i)) continue;
-    hasAny = true;
-
-    if (numLED > 1) ImGui::Text("LEDs[%d]", i);
-
-    static HAL_AddressableLEDData data[HAL_kAddressableLEDMaxLength];
-    int length = HALSIM_GetAddressableLEDData(i, data);
-    bool running = HALSIM_GetAddressableLEDRunning(i);
-    auto& info = gDisplaySettings[i];
-
-    ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
-    ImGui::LabelText("Length", "%d", length);
-    ImGui::LabelText("Running", "%s", running ? "Yes" : "No");
-    ImGui::InputInt("Columns", &info.numColumns);
-    {
-      static const char* options[] = {"Row Major", "Column Major"};
-      int val = info.config.order;
-      if (ImGui::Combo("Order", &val, options, 2))
-        info.config.order = static_cast<LEDConfig::Order>(val);
-    }
-    {
-      static const char* options[] = {"Upper Left", "Lower Left", "Upper Right",
-                                      "Lower Right"};
-      int val = info.config.start;
-      if (ImGui::Combo("Start", &val, options, 4))
-        info.config.start = static_cast<LEDConfig::Start>(val);
-    }
-    ImGui::Checkbox("Serpentine", &info.config.serpentine);
-    if (info.numColumns < 1) info.numColumns = 1;
-    ImGui::PopItemWidth();
-
-    // show as LED indicators
-    static int values[HAL_kAddressableLEDMaxLength];
-    static ImU32 colors[HAL_kAddressableLEDMaxLength];
-
-    if (!running) {
-      colors[0] = IM_COL32(128, 128, 128, 255);
-      for (int j = 0; j < length; ++j) values[j] = -1;
-    } else {
-      for (int j = 0; j < length; ++j) {
-        values[j] = j + 1;
-        colors[j] = IM_COL32(data[j].r, data[j].g, data[j].b, 255);
+void AddressableLEDsModel::Update() {
+  for (int i = 0; i < static_cast<int>(m_models.size()); ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetAddressableLEDInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<AddressableLEDModel>(i);
       }
+      if (model) {
+        model->Update();
+      }
+    } else {
+      model.reset();
     }
-
-    DrawLEDs(values, length, info.numColumns, colors, 0, 0, info.config);
   }
-  if (!hasAny) ImGui::Text("No addressable LEDs");
+}
+
+bool AddressableLEDsModel::Exists() {
+  for (auto&& model : m_models) {
+    if (model && model->Exists()) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void AddressableLEDsModel::ForEachLEDDisplay(
+    wpi::function_ref<void(glass::LEDDisplayModel& model, int index)> func) {
+  for (int i = 0; i < static_cast<int>(m_models.size()); ++i) {
+    if (m_models[i]) {
+      func(*m_models[i], i);
+    }
+  }
+}
+
+static bool AddressableLEDsExists() {
+  static const int numLED = HAL_GetNumAddressableLEDs();
+  for (int i = 0; i < numLED; ++i) {
+    if (HALSIM_GetAddressableLEDInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
 }
 
 void AddressableLEDGui::Initialize() {
-  gDisplaySettings.Initialize();
-  HALSimGui::AddWindow("Addressable LEDs", DisplayAddressableLEDs,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetWindowVisibility("Addressable LEDs", HALSimGui::kHide);
-  HALSimGui::SetDefaultWindowPos("Addressable LEDs", 290, 100);
+  HALSimGui::halProvider.Register(
+      "Addressable LEDs", [] { return AddressableLEDsExists(); },
+      [] { return std::make_unique<AddressableLEDsModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(290, 100);
+        return glass::MakeFunctionView([=] {
+          glass::DisplayLEDDisplays(static_cast<AddressableLEDsModel*>(model));
+        });
+      });
 }
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h
index e376b9b..920b965 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AddressableLEDGui.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp
deleted file mode 100644
index 5df30e6..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.cpp
+++ /dev/null
@@ -1,80 +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 "AnalogGyroGui.h"
-
-#include <cstdio>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/Value.h>
-#include <hal/simulation/AnalogGyroData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "SimDeviceGui.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogGyroAngle, "AGyro Angle");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogGyroRate, "AGyro Rate");
-struct AnalogGyroSource {
-  explicit AnalogGyroSource(int32_t index) : angle{index}, rate{index} {}
-  AnalogGyroAngleSource angle;
-  AnalogGyroRateSource rate;
-};
-}  // namespace
-
-static std::vector<std::unique_ptr<AnalogGyroSource>> gAnalogGyroSources;
-
-static void UpdateAnalogGyroSources() {
-  for (int i = 0, iend = gAnalogGyroSources.size(); i < iend; ++i) {
-    auto& source = gAnalogGyroSources[i];
-    if (HALSIM_GetAnalogGyroInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<AnalogGyroSource>(i);
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayAnalogGyros() {
-  for (int i = 0, iend = gAnalogGyroSources.size(); i < iend; ++i) {
-    if (auto source = gAnalogGyroSources[i].get()) {
-      char name[32];
-      std::snprintf(name, sizeof(name), "AnalogGyro[%d]", i);
-      if (SimDeviceGui::StartDevice(name)) {
-        HAL_Value value;
-
-        // angle
-        value = HAL_MakeDouble(source->angle.GetValue());
-        if (SimDeviceGui::DisplayValueSource("Angle", false, &value,
-                                             &source->angle))
-          HALSIM_SetAnalogGyroAngle(i, value.data.v_double);
-
-        // rate
-        value = HAL_MakeDouble(source->rate.GetValue());
-        if (SimDeviceGui::DisplayValueSource("Rate", false, &value,
-                                             &source->rate))
-          HALSIM_SetAnalogGyroRate(i, value.data.v_double);
-
-        SimDeviceGui::FinishDevice();
-      }
-    }
-  }
-}
-
-void AnalogGyroGui::Initialize() {
-  gAnalogGyroSources.resize(HAL_GetNumAccumulators());
-  HALSimGui::AddExecute(UpdateAnalogGyroSources);
-  SimDeviceGui::Add(DisplayAnalogGyros);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.h
deleted file mode 100644
index e528279..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class AnalogGyroGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroSimGui.cpp
new file mode 100644
index 0000000..12149ec
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroSimGui.cpp
@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "AnalogGyroSimGui.h"
+
+#include <glass/hardware/AnalogGyro.h>
+#include <glass/other/DeviceTree.h>
+
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/Value.h>
+#include <hal/simulation/AnalogGyroData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+#include "SimDeviceGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogGyroAngle, "AGyro Angle");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogGyroRate, "AGyro Rate");
+
+class AnalogGyroSimModel : public glass::AnalogGyroModel {
+ public:
+  explicit AnalogGyroSimModel(int32_t index)
+      : m_index{index}, m_angle{index}, m_rate{index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetAnalogGyroInitialized(m_index); }
+
+  glass::DataSource* GetAngleData() override { return &m_angle; }
+  glass::DataSource* GetRateData() override { return &m_rate; }
+
+  void SetAngle(double val) override {
+    HALSIM_SetAnalogGyroAngle(m_index, val);
+  }
+  void SetRate(double val) override { HALSIM_SetAnalogGyroRate(m_index, val); }
+
+ private:
+  int32_t m_index;
+  AnalogGyroAngleSource m_angle;
+  AnalogGyroRateSource m_rate;
+};
+
+class AnalogGyrosSimModel : public glass::AnalogGyrosModel {
+ public:
+  AnalogGyrosSimModel() : m_models(HAL_GetNumAccumulators()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachAnalogGyro(
+      wpi::function_ref<void(glass::AnalogGyroModel& model, int index)> func)
+      override;
+
+ private:
+  // indexed by channel
+  std::vector<std::unique_ptr<AnalogGyroSimModel>> m_models;
+};
+}  // namespace
+
+void AnalogGyrosSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetAnalogGyroInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<AnalogGyroSimModel>(i);
+      }
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void AnalogGyrosSimModel::ForEachAnalogGyro(
+    wpi::function_ref<void(glass::AnalogGyroModel& model, int index)> func) {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+void AnalogGyroSimGui::Initialize() {
+  SimDeviceGui::GetDeviceTree().Add(
+      std::make_unique<AnalogGyrosSimModel>(), [](glass::Model* model) {
+        glass::DisplayAnalogGyrosDevice(
+            static_cast<AnalogGyrosSimModel*>(model));
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroSimGui.h
new file mode 100644
index 0000000..89f91ca
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogGyroSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class AnalogGyroSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
deleted file mode 100644
index 8ba1459..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.cpp
+++ /dev/null
@@ -1,102 +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 "AnalogInputGui.h"
-
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/AnalogGyroData.h>
-#include <hal/simulation/AnalogInData.h>
-#include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogInVoltage, "AIn");
-}  // namespace
-
-// indexed by channel
-static IniSaver<NameInfo> gAnalogInputs{"AnalogInput"};
-static std::vector<std::unique_ptr<AnalogInVoltageSource>> gAnalogInputSources;
-
-static void UpdateAnalogInputSources() {
-  for (int i = 0, iend = gAnalogInputSources.size(); i < iend; ++i) {
-    auto& source = gAnalogInputSources[i];
-    if (HALSIM_GetAnalogInInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<AnalogInVoltageSource>(i);
-        source->SetName(gAnalogInputs[i].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayAnalogInputs() {
-  ImGui::Text("(Use Ctrl+Click to edit value)");
-  bool hasInputs = false;
-  static const int numAccum = HAL_GetNumAccumulators();
-  bool first = true;
-  for (int i = 0, iend = gAnalogInputSources.size(); i < iend; ++i) {
-    if (auto source = gAnalogInputSources[i].get()) {
-      ImGui::PushID(i);
-      hasInputs = true;
-
-      if (!first) {
-        ImGui::Spacing();
-        ImGui::Spacing();
-      } else {
-        first = false;
-      }
-
-      auto& info = gAnalogInputs[i];
-      // build label
-      char label[128];
-      info.GetLabel(label, sizeof(label), "In", i);
-
-      if (i < numAccum && HALSIM_GetAnalogGyroInitialized(i)) {
-        ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-        ImGui::LabelText(label, "AnalogGyro[%d]", i);
-        ImGui::PopStyleColor();
-      } else if (auto simDevice = HALSIM_GetAnalogInSimDevice(i)) {
-        ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-        ImGui::LabelText(label, "%s", HALSIM_GetSimDeviceName(simDevice));
-        ImGui::PopStyleColor();
-      } else {
-        float val = source->GetValue();
-        if (source->SliderFloat(label, &val, 0.0, 5.0))
-          HALSIM_SetAnalogInVoltage(i, val);
-      }
-
-      // context menu to change name
-      if (info.PopupEditName(i)) {
-        source->SetName(info.GetName());
-      }
-      ImGui::PopID();
-    }
-  }
-  if (!hasInputs) ImGui::Text("No analog inputs");
-}
-
-void AnalogInputGui::Initialize() {
-  gAnalogInputs.Initialize();
-  gAnalogInputSources.resize(HAL_GetNumAnalogInputs());
-
-  HALSimGui::AddExecute(UpdateAnalogInputSources);
-  HALSimGui::AddWindow("Analog Inputs", DisplayAnalogInputs,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("Analog Inputs", 640, 20);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.h
deleted file mode 100644
index 74c3e63..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class AnalogInputGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputSimGui.cpp
new file mode 100644
index 0000000..5a3b2d8
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputSimGui.cpp
@@ -0,0 +1,122 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "AnalogInputSimGui.h"
+
+#include <glass/View.h>
+#include <glass/hardware/AnalogInput.h>
+
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/simulation/AnalogGyroData.h>
+#include <hal/simulation/AnalogInData.h>
+#include <hal/simulation/SimDeviceData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogInVoltage, "AIn");
+
+class AnalogInputSimModel : public glass::AnalogInputModel {
+ public:
+  explicit AnalogInputSimModel(int32_t index)
+      : m_index{index}, m_voltageData{m_index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetAnalogInInitialized(m_index); }
+
+  bool IsGyro() const override {
+    return m_index < HAL_GetNumAccumulators() &&
+           HALSIM_GetAnalogGyroInitialized(m_index);
+  }
+
+  const char* GetSimDevice() const override {
+    if (auto simDevice = HALSIM_GetAnalogInSimDevice(m_index)) {
+      return HALSIM_GetSimDeviceName(simDevice);
+    } else {
+      return nullptr;
+    }
+  }
+
+  glass::DataSource* GetVoltageData() override { return &m_voltageData; }
+
+  void SetVoltage(double val) override {
+    HALSIM_SetAnalogInVoltage(m_index, val);
+  }
+
+ private:
+  int32_t m_index;
+  AnalogInVoltageSource m_voltageData;
+};
+
+class AnalogInputsSimModel : public glass::AnalogInputsModel {
+ public:
+  AnalogInputsSimModel() : m_models(HAL_GetNumAnalogInputs()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachAnalogInput(
+      wpi::function_ref<void(glass::AnalogInputModel& model, int index)> func)
+      override;
+
+ private:
+  // indexed by channel
+  std::vector<std::unique_ptr<AnalogInputSimModel>> m_models;
+};
+}  // namespace
+
+void AnalogInputsSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetAnalogInInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<AnalogInputSimModel>(i);
+      }
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void AnalogInputsSimModel::ForEachAnalogInput(
+    wpi::function_ref<void(glass::AnalogInputModel& model, int index)> func) {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool AnalogInputsAnyInitialized() {
+  static const int32_t num = HAL_GetNumAnalogInputs();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetAnalogInInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void AnalogInputSimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "Analog Inputs", AnalogInputsAnyInitialized,
+      [] { return std::make_unique<AnalogInputsSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(640, 20);
+        return glass::MakeFunctionView([=] {
+          glass::DisplayAnalogInputs(static_cast<AnalogInputsSimModel*>(model));
+        });
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputSimGui.h
new file mode 100644
index 0000000..ca358f4
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogInputSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class AnalogInputSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp
deleted file mode 100644
index 3e7f4ea..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.cpp
+++ /dev/null
@@ -1,81 +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 "AnalogOutGui.h"
-
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/AnalogOutData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-#include "SimDeviceGui.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogOutVoltage, "AOut");
-}  // namespace
-
-static IniSaver<NameInfo> gAnalogOuts{"AnalogOut"};  // indexed by channel
-static std::vector<std::unique_ptr<AnalogOutVoltageSource>> gAnalogOutSources;
-
-static void UpdateAnalogOutSources() {
-  for (int i = 0, iend = gAnalogOutSources.size(); i < iend; ++i) {
-    auto& source = gAnalogOutSources[i];
-    if (HALSIM_GetAnalogOutInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<AnalogOutVoltageSource>(i);
-        source->SetName(gAnalogOuts[i].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayAnalogOutputs() {
-  int count = 0;
-  for (auto&& source : gAnalogOutSources) {
-    if (source) ++count;
-  }
-
-  if (count == 0) return;
-
-  if (SimDeviceGui::StartDevice("Analog Outputs")) {
-    for (int i = 0, iend = gAnalogOutSources.size(); i < iend; ++i) {
-      if (auto source = gAnalogOutSources[i].get()) {
-        ImGui::PushID(i);
-
-        auto& info = gAnalogOuts[i];
-        char label[128];
-        info.GetLabel(label, sizeof(label), "Out", i);
-        HAL_Value value = HAL_MakeDouble(source->GetValue());
-        SimDeviceGui::DisplayValueSource(label, true, &value, source);
-
-        if (info.PopupEditName(i)) {
-          if (source) source->SetName(info.GetName());
-        }
-        ImGui::PopID();
-      }
-    }
-
-    SimDeviceGui::FinishDevice();
-  }
-}
-
-void AnalogOutGui::Initialize() {
-  gAnalogOuts.Initialize();
-  gAnalogOutSources.resize(HAL_GetNumAnalogOutputs());
-  HALSimGui::AddExecute(UpdateAnalogOutSources);
-  SimDeviceGui::Add(DisplayAnalogOutputs);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.h
deleted file mode 100644
index dd01699..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class AnalogOutGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutputSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutputSimGui.cpp
new file mode 100644
index 0000000..8e02fb6
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutputSimGui.cpp
@@ -0,0 +1,93 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "AnalogOutputSimGui.h"
+
+#include <glass/hardware/AnalogOutput.h>
+#include <glass/other/DeviceTree.h>
+
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/simulation/AnalogOutData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+#include "SimDeviceGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(AnalogOutVoltage, "AOut");
+
+class AnalogOutputSimModel : public glass::AnalogOutputModel {
+ public:
+  explicit AnalogOutputSimModel(int32_t index)
+      : m_index{index}, m_voltageData{m_index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetAnalogOutInitialized(m_index); }
+
+  glass::DataSource* GetVoltageData() override { return &m_voltageData; }
+
+  void SetVoltage(double val) override {
+    HALSIM_SetAnalogOutVoltage(m_index, val);
+  }
+
+ private:
+  int32_t m_index;
+  AnalogOutVoltageSource m_voltageData;
+};
+
+class AnalogOutputsSimModel : public glass::AnalogOutputsModel {
+ public:
+  AnalogOutputsSimModel() : m_models(HAL_GetNumAnalogOutputs()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachAnalogOutput(
+      wpi::function_ref<void(glass::AnalogOutputModel& model, int index)> func)
+      override;
+
+ private:
+  // indexed by channel
+  std::vector<std::unique_ptr<AnalogOutputSimModel>> m_models;
+};
+}  // namespace
+
+void AnalogOutputsSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetAnalogOutInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<AnalogOutputSimModel>(i);
+      }
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void AnalogOutputsSimModel::ForEachAnalogOutput(
+    wpi::function_ref<void(glass::AnalogOutputModel& model, int index)> func) {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+void AnalogOutputSimGui::Initialize() {
+  SimDeviceGui::GetDeviceTree().Add(
+      std::make_unique<AnalogOutputsSimModel>(), [](glass::Model* model) {
+        glass::DisplayAnalogOutputsDevice(
+            static_cast<AnalogOutputsSimModel*>(model));
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutputSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutputSimGui.h
new file mode 100644
index 0000000..f7a816b
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/AnalogOutputSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class AnalogOutputSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp
deleted file mode 100644
index 43215fa..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/CompressorGui.cpp
+++ /dev/null
@@ -1,104 +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 "CompressorGui.h"
-
-#include <cstdio>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/Value.h>
-#include <hal/simulation/PCMData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "SimDeviceGui.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(PCMCompressorOn, "Compressor On");
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(PCMClosedLoopEnabled, "Closed Loop");
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(PCMPressureSwitch, "Pressure Switch");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PCMCompressorCurrent, "Comp Current");
-struct CompressorSource {
-  explicit CompressorSource(int32_t index)
-      : running{index}, enabled{index}, pressureSwitch{index}, current{index} {}
-  PCMCompressorOnSource running;
-  PCMClosedLoopEnabledSource enabled;
-  PCMPressureSwitchSource pressureSwitch;
-  PCMCompressorCurrentSource current;
-};
-}  // namespace
-
-static std::vector<std::unique_ptr<CompressorSource>> gCompressorSources;
-
-static void UpdateCompressorSources() {
-  for (int i = 0, iend = gCompressorSources.size(); i < iend; ++i) {
-    auto& source = gCompressorSources[i];
-    if (HALSIM_GetPCMCompressorInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<CompressorSource>(i);
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayCompressors() {
-  for (int i = 0, iend = gCompressorSources.size(); i < iend; ++i) {
-    if (auto source = gCompressorSources[i].get()) {
-      char name[32];
-      std::snprintf(name, sizeof(name), "Compressor[%d]", i);
-      if (SimDeviceGui::StartDevice(name)) {
-        HAL_Value value;
-
-        // enabled
-        if (HALSimGui::AreOutputsDisabled())
-          value = HAL_MakeBoolean(false);
-        else
-          value = HAL_MakeBoolean(source->running.GetValue());
-        if (SimDeviceGui::DisplayValueSource("Running", false, &value,
-                                             &source->running))
-          HALSIM_SetPCMCompressorOn(i, value.data.v_boolean);
-
-        // closed loop
-        value = HAL_MakeEnum(source->enabled.GetValue() ? 1 : 0);
-        static const char* enabledOptions[] = {"disabled", "enabled"};
-        if (SimDeviceGui::DisplayValueSource("Closed Loop", true, &value,
-                                             &source->enabled, enabledOptions,
-                                             2))
-          HALSIM_SetPCMClosedLoopEnabled(i, value.data.v_enum);
-
-        // pressure switch
-        value = HAL_MakeEnum(source->pressureSwitch.GetValue() ? 1 : 0);
-        static const char* switchOptions[] = {"full", "low"};
-        if (SimDeviceGui::DisplayValueSource("Pressure", false, &value,
-                                             &source->pressureSwitch,
-                                             switchOptions, 2))
-          HALSIM_SetPCMPressureSwitch(i, value.data.v_enum);
-
-        // compressor current
-        value = HAL_MakeDouble(source->current.GetValue());
-        if (SimDeviceGui::DisplayValueSource("Current (A)", false, &value,
-                                             &source->current))
-          HALSIM_SetPCMCompressorCurrent(i, value.data.v_double);
-
-        SimDeviceGui::FinishDevice();
-      }
-    }
-  }
-}
-
-void CompressorGui::Initialize() {
-  gCompressorSources.resize(HAL_GetNumPCMModules());
-  HALSimGui::AddExecute(UpdateCompressorSources);
-  SimDeviceGui::Add(DisplayCompressors);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/CompressorGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/CompressorGui.h
deleted file mode 100644
index f403ece..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/CompressorGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class CompressorGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
deleted file mode 100644
index 2f82ff2..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOGui.cpp
+++ /dev/null
@@ -1,214 +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 "DIOGui.h"
-
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/DIOData.h>
-#include <hal/simulation/DigitalPWMData.h>
-#include <hal/simulation/DutyCycleData.h>
-#include <hal/simulation/EncoderData.h>
-#include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(DIOValue, "DIO");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(DigitalPWMDutyCycle, "DPWM");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(DutyCycleOutput, "DutyCycle");
-}  // namespace
-
-static IniSaver<NameInfo> gDIO{"DIO"};
-static std::vector<std::unique_ptr<DIOValueSource>> gDIOSources;
-static std::vector<std::unique_ptr<DigitalPWMDutyCycleSource>> gDPWMSources;
-static std::vector<std::unique_ptr<DutyCycleOutputSource>> gDutyCycleSources;
-
-static void LabelSimDevice(const char* name, HAL_SimDeviceHandle simDevice) {
-  ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-  ImGui::LabelText(name, "%s", HALSIM_GetSimDeviceName(simDevice));
-  ImGui::PopStyleColor();
-}
-
-static void UpdateDIOSources() {
-  for (int i = 0, iend = gDIOSources.size(); i < iend; ++i) {
-    auto& source = gDIOSources[i];
-    if (HALSIM_GetDIOInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<DIOValueSource>(i);
-        source->SetName(gDIO[i].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void UpdateDPWMSources() {
-  const int numDIO = gDIOSources.size();
-  for (int i = 0, iend = gDPWMSources.size(); i < iend; ++i) {
-    auto& source = gDPWMSources[i];
-    if (HALSIM_GetDigitalPWMInitialized(i)) {
-      if (!source) {
-        int channel = HALSIM_GetDigitalPWMPin(i);
-        if (channel >= 0 && channel < numDIO) {
-          source = std::make_unique<DigitalPWMDutyCycleSource>(i, channel);
-          source->SetName(gDIO[channel].GetName());
-        }
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void UpdateDutyCycleSources() {
-  const int numDIO = gDIOSources.size();
-  for (int i = 0, iend = gDutyCycleSources.size(); i < iend; ++i) {
-    auto& source = gDutyCycleSources[i];
-    if (HALSIM_GetDutyCycleInitialized(i)) {
-      if (!source) {
-        int channel = HALSIM_GetDutyCycleDigitalChannel(i);
-        if (channel >= 0 && channel < numDIO) {
-          source = std::make_unique<DutyCycleOutputSource>(i, channel);
-          source->SetName(gDIO[channel].GetName());
-        }
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayDIO() {
-  bool hasAny = false;
-  const int numDIO = gDIOSources.size();
-  const int numPWM = gDPWMSources.size();
-  static const int numEncoder = HAL_GetNumEncoders();
-  const int numDutyCycle = gDutyCycleSources.size();
-  static auto pwmMap = std::make_unique<int[]>(numDIO);
-  static auto encoderMap = std::make_unique<int[]>(numDIO);
-  static auto dutyCycleMap = std::make_unique<int[]>(numDIO);
-
-  std::memset(pwmMap.get(), 0, numDIO * sizeof(pwmMap[0]));
-  std::memset(encoderMap.get(), 0, numDIO * sizeof(encoderMap[0]));
-  std::memset(dutyCycleMap.get(), 0, numDIO * sizeof(dutyCycleMap[0]));
-
-  for (int i = 0; i < numPWM; ++i) {
-    if (auto source = gDPWMSources[i].get()) {
-      int channel = source->GetChannel();
-      if (channel >= 0 && channel < numDIO) pwmMap[channel] = i + 1;
-    }
-  }
-
-  for (int i = 0; i < numEncoder; ++i) {
-    if (HALSIM_GetEncoderInitialized(i)) {
-      int channel;
-      channel = HALSIM_GetEncoderDigitalChannelA(i);
-      if (channel >= 0 && channel < numDIO) encoderMap[channel] = i + 1;
-      channel = HALSIM_GetEncoderDigitalChannelB(i);
-      if (channel >= 0 && channel < numDIO) encoderMap[channel] = i + 1;
-    }
-  }
-
-  for (int i = 0; i < numDutyCycle; ++i) {
-    if (auto source = gDutyCycleSources[i].get()) {
-      int channel = source->GetChannel();
-      if (channel >= 0 && channel < numDIO) dutyCycleMap[channel] = i + 1;
-    }
-  }
-
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
-  for (int i = 0; i < numDIO; ++i) {
-    if (auto dioSource = gDIOSources[i].get()) {
-      ImGui::PushID(i);
-      hasAny = true;
-      DigitalPWMDutyCycleSource* dpwmSource = nullptr;
-      DutyCycleOutputSource* dutyCycleSource = nullptr;
-      auto& info = gDIO[i];
-      char label[128];
-      if (pwmMap[i] > 0) {
-        dpwmSource = gDPWMSources[pwmMap[i] - 1].get();
-        info.GetLabel(label, sizeof(label), "PWM", i);
-        if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
-          LabelSimDevice(label, simDevice);
-        } else {
-          dpwmSource->LabelText(label, "%0.3f", dpwmSource->GetValue());
-        }
-      } else if (encoderMap[i] > 0) {
-        info.GetLabel(label, sizeof(label), " In", i);
-        if (auto simDevice = HALSIM_GetEncoderSimDevice(encoderMap[i] - 1)) {
-          LabelSimDevice(label, simDevice);
-        } else {
-          ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-          ImGui::LabelText(label, "Encoder[%d,%d]",
-                           HALSIM_GetEncoderDigitalChannelA(encoderMap[i] - 1),
-                           HALSIM_GetEncoderDigitalChannelB(encoderMap[i] - 1));
-          ImGui::PopStyleColor();
-        }
-      } else if (dutyCycleMap[i] > 0) {
-        dutyCycleSource = gDutyCycleSources[dutyCycleMap[i] - 1].get();
-        info.GetLabel(label, sizeof(label), "Dty", i);
-        if (auto simDevice =
-                HALSIM_GetDutyCycleSimDevice(dutyCycleMap[i] - 1)) {
-          LabelSimDevice(label, simDevice);
-        } else {
-          double val = dutyCycleSource->GetValue();
-          if (dutyCycleSource->InputDouble(label, &val))
-            HALSIM_SetDutyCycleOutput(dutyCycleMap[i] - 1, val);
-        }
-      } else if (!HALSIM_GetDIOIsInput(i)) {
-        info.GetLabel(label, sizeof(label), "Out", i);
-        if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
-          LabelSimDevice(label, simDevice);
-        } else {
-          dioSource->LabelText(
-              label, "%s", dioSource->GetValue() != 0 ? "1 (high)" : "0 (low)");
-        }
-      } else {
-        info.GetLabel(label, sizeof(label), " In", i);
-        if (auto simDevice = HALSIM_GetDIOSimDevice(i)) {
-          LabelSimDevice(label, simDevice);
-        } else {
-          static const char* options[] = {"0 (low)", "1 (high)"};
-          int val = dioSource->GetValue() != 0 ? 1 : 0;
-          if (dioSource->Combo(label, &val, options, 2))
-            HALSIM_SetDIOValue(i, val);
-        }
-      }
-      if (info.PopupEditName(i)) {
-        dioSource->SetName(info.GetName());
-        if (dpwmSource) dpwmSource->SetName(info.GetName());
-        if (dutyCycleSource) dutyCycleSource->SetName(info.GetName());
-      }
-      ImGui::PopID();
-    }
-  }
-  ImGui::PopItemWidth();
-  if (!hasAny) ImGui::Text("No Digital I/O");
-}
-
-void DIOGui::Initialize() {
-  gDIO.Initialize();
-  gDIOSources.resize(HAL_GetNumDigitalChannels());
-  gDPWMSources.resize(HAL_GetNumDigitalPWMOutputs());
-  gDutyCycleSources.resize(HAL_GetNumDutyCycles());
-
-  HALSimGui::AddExecute(UpdateDIOSources);
-  HALSimGui::AddExecute(UpdateDPWMSources);
-  HALSimGui::AddExecute(UpdateDutyCycleSources);
-  HALSimGui::AddWindow("DIO", DisplayDIO, ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("DIO", 470, 20);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOGui.h
deleted file mode 100644
index 70181e3..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class DIOGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOSimGui.cpp
new file mode 100644
index 0000000..22c06dd
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOSimGui.cpp
@@ -0,0 +1,245 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "DIOSimGui.h"
+
+#include <glass/hardware/DIO.h>
+#include <glass/hardware/Encoder.h>
+
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/simulation/DIOData.h>
+#include <hal/simulation/DigitalPWMData.h>
+#include <hal/simulation/DutyCycleData.h>
+#include <hal/simulation/EncoderData.h>
+#include <hal/simulation/SimDeviceData.h>
+
+#include "EncoderSimGui.h"
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(DIOValue, "DIO");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(DigitalPWMDutyCycle, "DPWM");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(DutyCycleOutput, "DutyCycle");
+
+class DPWMSimModel : public glass::DPWMModel {
+ public:
+  DPWMSimModel(int32_t index, int32_t dioChannel)
+      : m_dioChannel{dioChannel}, m_index{index}, m_valueData{index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetDigitalPWMInitialized(m_index); }
+
+  const char* GetSimDevice() const override {
+    if (auto simDevice = HALSIM_GetDIOSimDevice(m_dioChannel)) {
+      return HALSIM_GetSimDeviceName(simDevice);
+    } else {
+      return nullptr;
+    }
+  }
+
+  glass::DataSource* GetValueData() override { return &m_valueData; }
+
+  void SetValue(double val) override {
+    HALSIM_SetDigitalPWMDutyCycle(m_index, val);
+  }
+
+ private:
+  int32_t m_dioChannel;
+  int32_t m_index;
+  DigitalPWMDutyCycleSource m_valueData;
+};
+
+class DutyCycleSimModel : public glass::DutyCycleModel {
+ public:
+  explicit DutyCycleSimModel(int32_t index)
+      : m_index{index}, m_valueData{index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetDutyCycleInitialized(m_index); }
+
+  const char* GetSimDevice() const override {
+    if (auto simDevice = HALSIM_GetDutyCycleSimDevice(m_index)) {
+      return HALSIM_GetSimDeviceName(simDevice);
+    } else {
+      return nullptr;
+    }
+  }
+
+  glass::DataSource* GetValueData() override { return &m_valueData; }
+
+  void SetValue(double val) override {
+    HALSIM_SetDutyCycleOutput(m_index, val);
+  }
+
+ private:
+  int32_t m_index;
+  DutyCycleOutputSource m_valueData;
+};
+
+class DIOSimModel : public glass::DIOModel {
+ public:
+  explicit DIOSimModel(int32_t channel)
+      : m_channel{channel}, m_valueData{channel} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetDIOInitialized(m_channel); }
+
+  bool IsReadOnly() override { return !IsInput(); }
+
+  const char* GetName() const override { return ""; }
+
+  const char* GetSimDevice() const override {
+    if (auto simDevice = HALSIM_GetDIOSimDevice(m_channel)) {
+      return HALSIM_GetSimDeviceName(simDevice);
+    } else {
+      return nullptr;
+    }
+  }
+
+  DPWMSimModel* GetDPWM() override { return m_dpwmSource; }
+  DutyCycleSimModel* GetDutyCycle() override { return m_dutyCycleSource; }
+  glass::EncoderModel* GetEncoder() override { return m_encoderSource; }
+
+  void SetDPWM(DPWMSimModel* model) { m_dpwmSource = model; }
+  void SetDutyCycle(DutyCycleSimModel* model) { m_dutyCycleSource = model; }
+  void SetEncoder(glass::EncoderModel* model) { m_encoderSource = model; }
+
+  bool IsInput() const override { return HALSIM_GetDIOIsInput(m_channel); }
+
+  glass::DataSource* GetValueData() override { return &m_valueData; }
+
+  void SetValue(bool val) override { HALSIM_SetDIOValue(m_channel, val); }
+
+ private:
+  int32_t m_channel;
+  DIOValueSource m_valueData;
+  DPWMSimModel* m_dpwmSource = nullptr;
+  DutyCycleSimModel* m_dutyCycleSource = nullptr;
+  glass::EncoderModel* m_encoderSource = nullptr;
+};
+
+class DIOsSimModel : public glass::DIOsModel {
+ public:
+  DIOsSimModel()
+      : m_dioModels(HAL_GetNumDigitalChannels()),
+        m_dpwmModels(HAL_GetNumDigitalPWMOutputs()),
+        m_dutyCycleModels(HAL_GetNumDutyCycles()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachDIO(
+      wpi::function_ref<void(glass::DIOModel& model, int index)> func) override;
+
+ private:
+  // indexed by channel
+  std::vector<std::unique_ptr<DIOSimModel>> m_dioModels;
+  // indexed by index
+  std::vector<std::unique_ptr<DPWMSimModel>> m_dpwmModels;
+  std::vector<std::unique_ptr<DutyCycleSimModel>> m_dutyCycleModels;
+};
+}  // namespace
+
+void DIOsSimModel::Update() {
+  const int32_t numDIO = m_dioModels.size();
+  for (int i = 0; i < numDIO; ++i) {
+    auto& model = m_dioModels[i];
+    if (HALSIM_GetDIOInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<DIOSimModel>(i);
+      }
+      model->SetDPWM(nullptr);
+      model->SetDutyCycle(nullptr);
+      model->SetEncoder(nullptr);
+    } else {
+      model.reset();
+    }
+  }
+
+  const int32_t numPWM = m_dpwmModels.size();
+  for (int32_t i = 0; i < numPWM; ++i) {
+    auto& model = m_dpwmModels[i];
+    if (HALSIM_GetDigitalPWMInitialized(i)) {
+      if (!model) {
+        int channel = HALSIM_GetDigitalPWMPin(i);
+        if (channel >= 0 && channel < numDIO && m_dioModels[channel]) {
+          model = std::make_unique<DPWMSimModel>(i, channel);
+          m_dioModels[channel]->SetDPWM(model.get());
+        }
+      }
+    } else {
+      model.reset();
+    }
+  }
+
+  const int32_t numDutyCycle = m_dutyCycleModels.size();
+  for (int32_t i = 0; i < numDutyCycle; ++i) {
+    auto& model = m_dutyCycleModels[i];
+    if (HALSIM_GetDutyCycleInitialized(i)) {
+      if (!model) {
+        int channel = HALSIM_GetDutyCycleDigitalChannel(i);
+        if (channel >= 0 && channel < numDIO && m_dioModels[channel]) {
+          model = std::make_unique<DutyCycleSimModel>(i);
+          m_dioModels[channel]->SetDutyCycle(model.get());
+        }
+      }
+    } else {
+      model.reset();
+    }
+  }
+
+  EncoderSimGui::GetEncodersModel().ForEachEncoder([&](auto& encoder, int i) {
+    int channel = encoder.GetChannelA();
+    if (channel >= 0 && channel < numDIO && m_dioModels[channel]) {
+      m_dioModels[channel]->SetEncoder(&encoder);
+    }
+    channel = encoder.GetChannelB();
+    if (channel >= 0 && channel < numDIO && m_dioModels[channel]) {
+      m_dioModels[channel]->SetEncoder(&encoder);
+    }
+  });
+}
+
+void DIOsSimModel::ForEachDIO(
+    wpi::function_ref<void(glass::DIOModel& model, int index)> func) {
+  const int32_t numDIO = m_dioModels.size();
+  for (int32_t i = 0; i < numDIO; ++i) {
+    if (auto model = m_dioModels[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool DIOAnyInitialized() {
+  static const int32_t num = HAL_GetNumDigitalChannels();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetDIOInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void DIOSimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "DIO", DIOAnyInitialized, [] { return std::make_unique<DIOsSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(470, 20);
+        return glass::MakeFunctionView([=] {
+          glass::DisplayDIOs(static_cast<DIOsSimModel*>(model),
+                             HALSimGui::halProvider.AreOutputsEnabled());
+        });
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOSimGui.h
new file mode 100644
index 0000000..9e9e52b
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DIOSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class DIOSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
index 96b53cb..42ec8a3 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp
@@ -1,90 +1,196 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "DriverStationGui.h"
 
+#include <glass/other/FMS.h>
+#include <glass/support/ExtraGuiWidgets.h>
+#include <glass/support/IniSaverInfo.h>
+
+#include <algorithm>
 #include <atomic>
 #include <cstring>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include <GLFW/glfw3.h>
+#include <fmt/format.h>
+#include <hal/DriverStationTypes.h>
 #include <hal/simulation/DriverStationData.h>
 #include <hal/simulation/MockHooks.h>
 #include <imgui.h>
 #include <imgui_internal.h>
-#include <wpi/Format.h>
-#include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
+#include <wpigui.h>
 
-#include "ExtraGuiWidgets.h"
-#include "GuiDataSource.h"
+#include "HALDataSource.h"
 #include "HALSimGui.h"
-#include "IniSaverInfo.h"
 
 using namespace halsimgui;
 
 namespace {
 
-struct SystemJoystick {
-  bool present = false;
-  int axisCount = 0;
-  const float* axes = nullptr;
-  int buttonCount = 0;
-  const unsigned char* buttons = nullptr;
-  int hatCount = 0;
-  const unsigned char* hats = nullptr;
-  const char* name = nullptr;
-  bool isGamepad = false;
-  GLFWgamepadstate gamepadState;
-
-  bool anyButtonPressed = false;
-
-  void Update(int i);
-};
-
-struct RobotJoystick {
-  NameInfo name;
-  std::string guid;
-  const SystemJoystick* sys = nullptr;
-  bool useGamepad = false;
-
+struct HALJoystickData {
+  HALJoystickData() {
+    std::memset(&desc, 0, sizeof(desc));
+    desc.type = -1;
+    std::memset(&axes, 0, sizeof(axes));
+    std::memset(&buttons, 0, sizeof(buttons));
+    std::memset(&povs, 0, sizeof(povs));
+  }
   HAL_JoystickDescriptor desc;
   HAL_JoystickAxes axes;
   HAL_JoystickButtons buttons;
   HAL_JoystickPOVs povs;
+};
 
-  void Clear();
+class SystemJoystick {
+ public:
+  virtual ~SystemJoystick() = default;
+
+  bool IsPresent() const { return m_present; }
+  bool IsAnyButtonPressed() const { return m_anyButtonPressed; }
+  bool IsGamepad() const { return m_isGamepad; }
+
+  virtual void SettingsDisplay() {}
+  virtual void Update() = 0;
+  virtual const char* GetName() const = 0;
+  virtual void GetData(HALJoystickData* data, bool mapGamepad) const = 0;
+  virtual const char* GetGUID() const = 0;
+  virtual int GetIndex() const = 0;
+
+ protected:
+  bool m_present = false;
+  bool m_anyButtonPressed = false;
+  bool m_isGamepad = false;
+};
+
+class GlfwSystemJoystick : public SystemJoystick {
+ public:
+  explicit GlfwSystemJoystick(int i) : m_index{i} {}
+
+  void Update() override;
+  const char* GetName() const override { return m_name ? m_name : "(null)"; }
+  void GetData(HALJoystickData* data, bool mapGamepad) const override;
+  const char* GetGUID() const override { return glfwGetJoystickGUID(m_index); }
+  int GetIndex() const override { return m_index; }
+
+ private:
+  int m_index;
+  int m_axisCount = 0;
+  const float* m_axes = nullptr;
+  int m_buttonCount = 0;
+  const unsigned char* m_buttons = nullptr;
+  int m_hatCount = 0;
+  const unsigned char* m_hats = nullptr;
+  const char* m_name = nullptr;
+  GLFWgamepadstate m_gamepadState;
+};
+
+class KeyboardJoystick : public SystemJoystick {
+ public:
+  explicit KeyboardJoystick(int index);
+
+  void SettingsDisplay() override;
+  void Update() override;
+  const char* GetName() const override { return m_name; }
+  void GetData(HALJoystickData* data, bool mapGamepad) const override {
+    *data = m_data;
+  }
+  const char* GetGUID() const override { return m_guid; }
+  int GetIndex() const override { return m_index + GLFW_JOYSTICK_LAST + 1; }
+
+  void ClearKey(int key);
+  virtual const char* GetKeyName(int key) const = 0;
+
+  void ReadIni(std::string_view name, std::string_view value);
+  void WriteIni(ImGuiTextBuffer* out_buf) const;
+
+ protected:
+  void EditKey(const char* label, int* key);
+
+  int m_index;
+  char m_name[20];
+  char m_guid[20];
+
+  static int* s_keyEdit;
+
+  HALJoystickData m_data;
+
+  struct AxisConfig {
+    int incKey = -1;
+    int decKey = -1;
+    float keyRate = 0.05f;
+    float decayRate = 0.05f;
+    float maxAbsValue = 1.0f;
+  };
+  AxisConfig m_axisConfig[HAL_kMaxJoystickAxes];
+
+  static constexpr int kMaxButtonCount = 32;
+  int m_buttonKey[kMaxButtonCount];
+
+  struct PovConfig {
+    int key0 = -1;
+    int key45 = -1;
+    int key90 = -1;
+    int key135 = -1;
+    int key180 = -1;
+    int key225 = -1;
+    int key270 = -1;
+    int key315 = -1;
+  };
+
+  PovConfig m_povConfig[HAL_kMaxJoystickPOVs];
+};
+
+class GlfwKeyboardJoystick : public KeyboardJoystick {
+ public:
+  explicit GlfwKeyboardJoystick(int index, bool noDefaults = false);
+
+  const char* GetKeyName(int key) const override;
+};
+
+struct RobotJoystick {
+  glass::NameInfo name;
+  std::string guid;
+  const SystemJoystick* sys = nullptr;
+  bool useGamepad = false;
+
+  HALJoystickData data;
+
+  void Clear() { data = HALJoystickData{}; }
   void Update();
   void SetHAL(int i);
   void GetHAL(int i);
-  bool IsButtonPressed(int i) { return (buttons.buttons & (1u << i)) != 0; }
+  bool IsButtonPressed(int i) {
+    return (data.buttons.buttons & (1u << i)) != 0;
+  }
 };
 
-class JoystickSource {
+class JoystickModel {
  public:
-  explicit JoystickSource(int index);
-  ~JoystickSource() {
+  explicit JoystickModel(int index);
+  ~JoystickModel() {
     HALSIM_CancelDriverStationNewDataCallback(m_callback);
-    for (int i = 0; i < buttonCount; ++i) delete buttons[i];
+    for (int i = 0; i < buttonCount; ++i) {
+      delete buttons[i];
+    }
   }
-  JoystickSource(const JoystickSource&) = delete;
-  JoystickSource& operator=(const JoystickSource&) = delete;
+  JoystickModel(const JoystickModel&) = delete;
+  JoystickModel& operator=(const JoystickModel&) = delete;
 
   int axisCount;
   int buttonCount;
   int povCount;
-  std::unique_ptr<GuiDataSource> axes[HAL_kMaxJoystickAxes];
+  std::unique_ptr<glass::DataSource> axes[HAL_kMaxJoystickAxes];
   // use pointer instead of unique_ptr to allow it to be passed directly
   // to DrawLEDSources()
-  GuiDataSource* buttons[32];
-  std::unique_ptr<GuiDataSource> povs[HAL_kMaxJoystickPOVs];
+  glass::DataSource* buttons[32];
+  std::unique_ptr<glass::DataSource> povs[HAL_kMaxJoystickPOVs];
 
  private:
   static void CallbackFunc(const char*, void* param, const HAL_Value*);
@@ -92,103 +198,188 @@
   int m_index;
   int32_t m_callback;
 };
+
+class FMSSimModel : public glass::FMSModel {
+ public:
+  FMSSimModel();
+
+  glass::DataSource* GetFmsAttachedData() override { return &m_fmsAttached; }
+  glass::DataSource* GetDsAttachedData() override { return &m_dsAttached; }
+  glass::DataSource* GetAllianceStationIdData() override {
+    return &m_allianceStationId;
+  }
+  glass::DataSource* GetMatchTimeData() override { return &m_matchTime; }
+  glass::DataSource* GetEStopData() override { return &m_estop; }
+  glass::DataSource* GetEnabledData() override { return &m_enabled; }
+  glass::DataSource* GetTestData() override { return &m_test; }
+  glass::DataSource* GetAutonomousData() override { return &m_autonomous; }
+  std::string_view GetGameSpecificMessage(
+      wpi::SmallVectorImpl<char>& buf) override {
+    HAL_MatchInfo info;
+    HALSIM_GetMatchInfo(&info);
+    buf.clear();
+    buf.append(info.gameSpecificMessage,
+               info.gameSpecificMessage + info.gameSpecificMessageSize);
+    return std::string_view(buf.begin(), buf.size());
+  }
+
+  void SetFmsAttached(bool val) override {
+    HALSIM_SetDriverStationFmsAttached(val);
+  }
+  void SetDsAttached(bool val) override {
+    HALSIM_SetDriverStationDsAttached(val);
+  }
+  void SetAllianceStationId(int val) override {
+    HALSIM_SetDriverStationAllianceStationId(
+        static_cast<HAL_AllianceStationID>(val));
+  }
+  void SetMatchTime(double val) override {
+    HALSIM_SetDriverStationMatchTime(val);
+    int32_t status = 0;
+    m_startMatchTime = HAL_GetFPGATime(&status) * 1.0e-6 - val;
+  }
+  void SetEStop(bool val) override { HALSIM_SetDriverStationEStop(val); }
+  void SetEnabled(bool val) override { HALSIM_SetDriverStationEnabled(val); }
+  void SetTest(bool val) override { HALSIM_SetDriverStationTest(val); }
+  void SetAutonomous(bool val) override {
+    HALSIM_SetDriverStationAutonomous(val);
+  }
+  void SetGameSpecificMessage(const char* val) override {
+    HALSIM_SetGameSpecificMessage(val);
+  }
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  bool IsReadOnly() override;
+
+  bool m_matchTimeEnabled = true;
+
+ private:
+  glass::DataSource m_fmsAttached{"FMS:FMSAttached"};
+  glass::DataSource m_dsAttached{"FMS:DSAttached"};
+  glass::DataSource m_allianceStationId{"FMS:AllianceStationID"};
+  glass::DataSource m_matchTime{"FMS:MatchTime"};
+  glass::DataSource m_estop{"FMS:EStop"};
+  glass::DataSource m_enabled{"FMS:RobotEnabled"};
+  glass::DataSource m_test{"FMS:TestMode"};
+  glass::DataSource m_autonomous{"FMS:AutonomousMode"};
+  double m_startMatchTime = 0.0;
+  double m_prevTime = 0.0;
+};
+
 }  // namespace
 
 // system joysticks
-static SystemJoystick gSystemJoysticks[GLFW_JOYSTICK_LAST + 1];
-static int gNumSystemJoysticks = 0;
+static std::vector<std::unique_ptr<SystemJoystick>> gGlfwJoysticks;
+static int gNumGlfwJoysticks = 0;
+static std::vector<std::unique_ptr<GlfwKeyboardJoystick>> gKeyboardJoysticks;
 
 // robot joysticks
 static RobotJoystick gRobotJoysticks[HAL_kMaxJoysticks];
-static std::unique_ptr<JoystickSource> gJoystickSources[HAL_kMaxJoysticks];
+static std::unique_ptr<JoystickModel> gJoystickSources[HAL_kMaxJoysticks];
+
+// FMS
+static std::unique_ptr<FMSSimModel> gFMSModel;
+
+// Window management
+DSManager DriverStationGui::dsManager{"DSManager"};
 
 static bool gDisableDS = false;
+static bool gZeroDisconnectedJoysticks = true;
+static bool gUseEnableDisableHotkeys = false;
+static bool gUseEstopHotkey = false;
 static std::atomic<bool>* gDSSocketConnected = nullptr;
 
 static inline bool IsDSDisabled() {
   return gDisableDS || (gDSSocketConnected && *gDSSocketConnected);
 }
 
-JoystickSource::JoystickSource(int index) : m_index{index} {
+JoystickModel::JoystickModel(int index) : m_index{index} {
   HAL_JoystickAxes halAxes;
   HALSIM_GetJoystickAxes(index, &halAxes);
   axisCount = halAxes.count;
   for (int i = 0; i < axisCount; ++i) {
-    axes[i] = std::make_unique<GuiDataSource>("Joystick[" + wpi::Twine{index} +
-                                              "] Axis[" + wpi::Twine{i} +
-                                              wpi::Twine{']'});
+    axes[i] = std::make_unique<glass::DataSource>(
+        fmt::format("Joystick[{}] Axis[{}]", index, i));
   }
 
   HAL_JoystickButtons halButtons;
   HALSIM_GetJoystickButtons(index, &halButtons);
   buttonCount = halButtons.count;
   for (int i = 0; i < buttonCount; ++i) {
-    buttons[i] =
-        new GuiDataSource("Joystick[" + wpi::Twine{index} + "] Button[" +
-                          wpi::Twine{i + 1} + wpi::Twine{']'});
+    buttons[i] = new glass::DataSource(
+        fmt::format("Joystick[{}] Button[{}]", index, i + 1));
     buttons[i]->SetDigital(true);
   }
-  for (int i = buttonCount; i < 32; ++i) buttons[i] = nullptr;
+  for (int i = buttonCount; i < 32; ++i) {
+    buttons[i] = nullptr;
+  }
 
   HAL_JoystickPOVs halPOVs;
   HALSIM_GetJoystickPOVs(index, &halPOVs);
   povCount = halPOVs.count;
   for (int i = 0; i < povCount; ++i) {
-    povs[i] = std::make_unique<GuiDataSource>("Joystick[" + wpi::Twine{index} +
-                                              "] POV[" + wpi::Twine{i} +
-                                              wpi::Twine{']'});
+    povs[i] = std::make_unique<glass::DataSource>(
+        fmt::format("Joystick[{}] POV [{}]", index, i));
   }
 
   m_callback =
       HALSIM_RegisterDriverStationNewDataCallback(CallbackFunc, this, true);
 }
 
-void JoystickSource::CallbackFunc(const char*, void* param, const HAL_Value*) {
-  auto self = static_cast<JoystickSource*>(param);
+void JoystickModel::CallbackFunc(const char*, void* param, const HAL_Value*) {
+  auto self = static_cast<JoystickModel*>(param);
 
   HAL_JoystickAxes halAxes;
   HALSIM_GetJoystickAxes(self->m_index, &halAxes);
   for (int i = 0; i < halAxes.count; ++i) {
-    if (auto axis = self->axes[i].get()) axis->SetValue(halAxes.axes[i]);
+    if (auto axis = self->axes[i].get()) {
+      axis->SetValue(halAxes.axes[i]);
+    }
   }
 
   HAL_JoystickButtons halButtons;
   HALSIM_GetJoystickButtons(self->m_index, &halButtons);
   for (int i = 0; i < halButtons.count; ++i) {
-    if (auto button = self->buttons[i])
+    if (auto button = self->buttons[i]) {
       button->SetValue((halButtons.buttons & (1u << i)) != 0 ? 1 : 0);
+    }
   }
 
   HAL_JoystickPOVs halPOVs;
   HALSIM_GetJoystickPOVs(self->m_index, &halPOVs);
   for (int i = 0; i < halPOVs.count; ++i) {
-    if (auto pov = self->povs[i].get()) pov->SetValue(halPOVs.povs[i]);
+    if (auto pov = self->povs[i].get()) {
+      pov->SetValue(halPOVs.povs[i]);
+    }
   }
 }
 
 // read/write joystick mapping to ini file
 static void* JoystickReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                               const char* name) {
-  int num;
-  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
-  if (num < 0 || num >= HAL_kMaxJoysticks) return nullptr;
+  int num = wpi::parse_integer<int>(name, 10).value_or(-1);
+  if (num < 0 || num >= HAL_kMaxJoysticks) {
+    return nullptr;
+  }
   return &gRobotJoysticks[num];
 }
 
 static void JoystickReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             void* entry, const char* lineStr) {
+                             void* entry, const char* line) {
   RobotJoystick* joy = static_cast<RobotJoystick*>(entry);
   // format: guid=guid or useGamepad=0/1
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
+  auto [name, value] = wpi::split(line, '=');
+  name = wpi::trim(name);
+  value = wpi::trim(value);
   if (name == "guid") {
     joy->guid = value;
   } else if (name == "useGamepad") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    joy->useGamepad = num;
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      joy->useGamepad = num.value();
+    }
   } else {
     joy->name.ReadIni(name, value);
   }
@@ -198,14 +389,52 @@
                              ImGuiTextBuffer* out_buf) {
   for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
     auto& joy = gRobotJoysticks[i];
-    if (!joy.name.HasName() && !joy.sys) continue;
+    if (!joy.name.HasName() && !joy.sys) {
+      continue;
+    }
     out_buf->appendf("[Joystick][%d]\nuseGamepad=%d\n", i,
                      joy.useGamepad ? 1 : 0);
-    if (joy.name.HasName()) joy.name.WriteIni(out_buf);
-    if (joy.sys) {
-      const char* guid = glfwGetJoystickGUID(joy.sys - gSystemJoysticks);
-      if (guid) out_buf->appendf("guid=%s\n", guid);
+    if (joy.name.HasName()) {
+      joy.name.WriteIni(out_buf);
     }
+    if (joy.sys) {
+      const char* guid = joy.sys->GetGUID();
+      if (guid) {
+        out_buf->appendf("guid=%s\n", guid);
+      }
+    }
+    out_buf->append("\n");
+  }
+}
+
+// read/write keyboard joystick mapping to ini file
+static void* KeyboardJoystickReadOpen(ImGuiContext* ctx,
+                                      ImGuiSettingsHandler* handler,
+                                      const char* name) {
+  int num = wpi::parse_integer<int>(name, 10).value_or(-1);
+  if (num < 0 || num >= static_cast<int>(gKeyboardJoysticks.size())) {
+    return nullptr;
+  }
+  auto joy = gKeyboardJoysticks[num].get();
+  *joy = GlfwKeyboardJoystick(num, true);
+  return joy;
+}
+
+static void KeyboardJoystickReadLine(ImGuiContext* ctx,
+                                     ImGuiSettingsHandler* handler, void* entry,
+                                     const char* line) {
+  auto joy = static_cast<KeyboardJoystick*>(entry);
+  // format: guid=guid or useGamepad=0/1
+  auto [name, value] = wpi::split(line, '=');
+  joy->ReadIni(wpi::trim(name), wpi::trim(value));
+}
+
+static void KeyboardJoystickWriteAll(ImGuiContext* ctx,
+                                     ImGuiSettingsHandler* handler,
+                                     ImGuiTextBuffer* out_buf) {
+  for (unsigned int i = 0; i < gKeyboardJoysticks.size(); ++i) {
+    out_buf->appendf("[KeyboardJoystick][%u]\n", i);
+    gKeyboardJoysticks[i]->WriteIni(out_buf);
     out_buf->append("\n");
   }
 }
@@ -214,62 +443,83 @@
 static void* DriverStationReadOpen(ImGuiContext* ctx,
                                    ImGuiSettingsHandler* handler,
                                    const char* name) {
-  if (name == wpi::StringRef{"Main"}) return &gDisableDS;
+  if (name == std::string_view{"Main"}) {
+    return &gDisableDS;
+  }
   return nullptr;
 }
 
 static void DriverStationReadLine(ImGuiContext* ctx,
                                   ImGuiSettingsHandler* handler, void* entry,
-                                  const char* lineStr) {
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
+                                  const char* line) {
+  auto [name, value] = wpi::split(line, '=');
+  name = wpi::trim(name);
+  value = wpi::trim(value);
   if (name == "disable") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    gDisableDS = num;
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      gDisableDS = num.value();
+    }
+  } else if (name == "zeroDisconnectedJoysticks") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      gZeroDisconnectedJoysticks = num.value();
+    }
+  } else if (name == "enableDisableKeys") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      gUseEnableDisableHotkeys = num.value();
+    }
+  } else if (name == "estopKey") {
+    if (auto num = wpi::parse_integer<int>(value, 10)) {
+      gUseEstopHotkey = num.value();
+    }
   }
 }
 
 static void DriverStationWriteAll(ImGuiContext* ctx,
                                   ImGuiSettingsHandler* handler,
                                   ImGuiTextBuffer* out_buf) {
-  out_buf->appendf("[DriverStation][Main]\ndisable=%d\n\n", gDisableDS ? 1 : 0);
+  out_buf->appendf(
+      "[DriverStation][Main]\ndisable=%d\nzeroDisconnectedJoysticks=%d\n"
+      "enableDisableKeys=%d\nestopKey=%d\n\n",
+      gDisableDS ? 1 : 0, gZeroDisconnectedJoysticks ? 1 : 0,
+      gUseEnableDisableHotkeys ? 1 : 0, gUseEstopHotkey ? 1 : 0);
 }
 
-void SystemJoystick::Update(int i) {
-  bool wasPresent = present;
-  present = glfwJoystickPresent(i);
+void GlfwSystemJoystick::Update() {
+  bool wasPresent = m_present;
+  m_present = glfwJoystickPresent(m_index);
 
-  if (!present) return;
-  axes = glfwGetJoystickAxes(i, &axisCount);
-  buttons = glfwGetJoystickButtons(i, &buttonCount);
-  hats = glfwGetJoystickHats(i, &hatCount);
-  isGamepad = glfwGetGamepadState(i, &gamepadState);
+  if (!m_present) {
+    return;
+  }
+  m_axes = glfwGetJoystickAxes(m_index, &m_axisCount);
+  m_buttons = glfwGetJoystickButtons(m_index, &m_buttonCount);
+  m_hats = glfwGetJoystickHats(m_index, &m_hatCount);
+  m_isGamepad = glfwGetGamepadState(m_index, &m_gamepadState);
 
-  anyButtonPressed = false;
-  for (int j = 0; j < buttonCount; ++j) {
-    if (buttons[j]) {
-      anyButtonPressed = true;
+  m_anyButtonPressed = false;
+  for (int j = 0; j < m_buttonCount; ++j) {
+    if (m_buttons[j]) {
+      m_anyButtonPressed = true;
       break;
     }
   }
-  for (int j = 0; j < hatCount; ++j) {
-    if (hats[j] != GLFW_HAT_CENTERED) {
-      anyButtonPressed = true;
+  for (int j = 0; j < m_hatCount; ++j) {
+    if (m_hats[j] != GLFW_HAT_CENTERED) {
+      m_anyButtonPressed = true;
       break;
     }
   }
 
-  if (!present || wasPresent) return;
-  name = glfwGetJoystickName(i);
+  if (!m_present || wasPresent) {
+    return;
+  }
+  m_name = glfwGetJoystickName(m_index);
 
   // try to find matching GUID
-  if (const char* guid = glfwGetJoystickGUID(i)) {
+  if (const char* guid = glfwGetJoystickGUID(m_index)) {
     for (auto&& joy : gRobotJoysticks) {
       if (guid == joy.guid) {
-        joy.sys = &gSystemJoysticks[i];
+        joy.sys = this;
         joy.guid.clear();
         break;
       }
@@ -300,88 +550,582 @@
   }
 }
 
-void RobotJoystick::Clear() {
-  std::memset(&desc, 0, sizeof(desc));
-  desc.type = -1;
-  std::memset(&axes, 0, sizeof(axes));
-  std::memset(&buttons, 0, sizeof(buttons));
-  std::memset(&povs, 0, sizeof(povs));
-}
-
-void RobotJoystick::Update() {
-  Clear();
-
-  if (!sys || !sys->present) return;
+void GlfwSystemJoystick::GetData(HALJoystickData* data, bool mapGamepad) const {
+  if (!m_present) {
+    return;
+  }
 
   // use gamepad mappings if present and enabled
   const float* sysAxes;
   const unsigned char* sysButtons;
-  if (sys->isGamepad && useGamepad) {
-    sysAxes = sys->gamepadState.axes;
+  if (m_isGamepad && mapGamepad) {
+    sysAxes = m_gamepadState.axes;
     // don't remap on windows
 #ifdef _WIN32
-    sysButtons = sys->buttons;
+    sysButtons = m_buttons;
 #else
-    sysButtons = sys->gamepadState.buttons;
+    sysButtons = m_gamepadState.buttons;
 #endif
   } else {
-    sysAxes = sys->axes;
-    sysButtons = sys->buttons;
+    sysAxes = m_axes;
+    sysButtons = m_buttons;
   }
 
   // copy into HAL structures
-  desc.isXbox = sys->isGamepad ? 1 : 0;
-  desc.type = sys->isGamepad ? 21 : 20;
-  std::strncpy(desc.name, sys->name, 256);
-  desc.axisCount = (std::min)(sys->axisCount, HAL_kMaxJoystickAxes);
+  data->desc.isXbox = m_isGamepad ? 1 : 0;
+  data->desc.type = m_isGamepad ? 21 : 20;
+  std::strncpy(data->desc.name, m_name, sizeof(data->desc.name) - 1);
+  data->desc.name[sizeof(data->desc.name) - 1] = '\0';
+  data->desc.axisCount = (std::min)(m_axisCount, HAL_kMaxJoystickAxes);
   // desc.axisTypes ???
-  desc.buttonCount = (std::min)(sys->buttonCount, 32);
-  desc.povCount = (std::min)(sys->hatCount, HAL_kMaxJoystickPOVs);
+  data->desc.buttonCount = (std::min)(m_buttonCount, 32);
+  data->desc.povCount = (std::min)(m_hatCount, HAL_kMaxJoystickPOVs);
 
-  buttons.count = desc.buttonCount;
-  for (int j = 0; j < buttons.count; ++j)
-    buttons.buttons |= (sysButtons[j] ? 1u : 0u) << j;
+  data->buttons.count = data->desc.buttonCount;
+  for (int j = 0; j < data->buttons.count; ++j) {
+    data->buttons.buttons |= (sysButtons[j] ? 1u : 0u) << j;
+  }
 
-  axes.count = desc.axisCount;
-  if (sys->isGamepad && useGamepad) {
+  data->axes.count = data->desc.axisCount;
+  if (m_isGamepad && mapGamepad) {
     // the FRC DriverStation maps gamepad (XInput) trigger values to 0-1 range
     // on axis 2 and 3.
-    axes.axes[0] = sysAxes[0];
-    axes.axes[1] = sysAxes[1];
-    axes.axes[2] = 0.5 + sysAxes[4] / 2.0;
-    axes.axes[3] = 0.5 + sysAxes[5] / 2.0;
-    axes.axes[4] = sysAxes[2];
-    axes.axes[5] = sysAxes[3];
+    data->axes.axes[0] = sysAxes[0];
+    data->axes.axes[1] = sysAxes[1];
+    data->axes.axes[2] = 0.5 + sysAxes[4] / 2.0;
+    data->axes.axes[3] = 0.5 + sysAxes[5] / 2.0;
+    data->axes.axes[4] = sysAxes[2];
+    data->axes.axes[5] = sysAxes[3];
 
     // the start button for gamepads is not mapped on the FRC DriverStation
     // platforms, so remove it if present
-    if (buttons.count == 11) {
-      --desc.buttonCount;
-      --buttons.count;
-      buttons.buttons =
-          (buttons.buttons & 0xff) | ((buttons.buttons >> 1) & 0x300);
+    if (data->buttons.count == 11) {
+      --data->desc.buttonCount;
+      --data->buttons.count;
+      data->buttons.buttons = (data->buttons.buttons & 0xff) |
+                              ((data->buttons.buttons >> 1) & 0x300);
     }
   } else {
-    std::memcpy(axes.axes, sysAxes, axes.count * sizeof(&axes.axes[0]));
+    std::memcpy(data->axes.axes, sysAxes,
+                data->axes.count * sizeof(&data->axes.axes[0]));
   }
 
-  povs.count = desc.povCount;
-  for (int j = 0; j < povs.count; ++j) povs.povs[j] = HatToAngle(sys->hats[j]);
+  data->povs.count = data->desc.povCount;
+  for (int j = 0; j < data->povs.count; ++j) {
+    data->povs.povs[j] = HatToAngle(m_hats[j]);
+  }
+}
+
+KeyboardJoystick::KeyboardJoystick(int index) : m_index{index} {
+  std::snprintf(m_name, sizeof(m_name), "Keyboard %d", index);
+  std::snprintf(m_guid, sizeof(m_guid), "Keyboard%d", index);
+
+  // init axes
+  m_data.axes.count = 0;
+
+  // init buttons
+  m_data.buttons.count = 0;
+  for (int i = 0; i < kMaxButtonCount; ++i) {
+    m_buttonKey[i] = -1;
+  }
+
+  // init POVs
+  m_data.povs.count = 0;
+
+  // init desc structure
+  m_data.desc.isXbox = 0;
+  m_data.desc.type = 20;
+  std::strncpy(m_data.desc.name, m_name, 256);
+}
+
+int* KeyboardJoystick::s_keyEdit = nullptr;
+
+void KeyboardJoystick::EditKey(const char* label, int* key) {
+  ImGui::PushID(label);
+  ImGui::Text("%s", label);
+  ImGui::SameLine();
+  char editLabel[32];
+  std::snprintf(editLabel, sizeof(editLabel), "%s###edit",
+                s_keyEdit == key ? "(press key)" : GetKeyName(*key));
+  if (ImGui::SmallButton(editLabel)) {
+    s_keyEdit = key;
+  }
+  ImGui::SameLine();
+  if (ImGui::SmallButton("Clear")) {
+    *key = -1;
+  }
+  ImGui::PopID();
+}
+
+void KeyboardJoystick::SettingsDisplay() {
+  if (s_keyEdit) {
+    ImGuiIO& io = ImGui::GetIO();
+    for (int i = 0; i < IM_ARRAYSIZE(io.KeysDown); ++i) {
+      if (io.KeysDown[i]) {
+        // remove all other uses
+        for (auto&& joy : gKeyboardJoysticks) {
+          joy->ClearKey(i);
+        }
+        *s_keyEdit = i;
+        s_keyEdit = nullptr;
+        break;
+      }
+    }
+  }
+
+  char label[64];
+  ImGui::PushItemWidth(ImGui::GetFontSize() * 6);
+  // axes
+  if (ImGui::CollapsingHeader("Axes", ImGuiTreeNodeFlags_DefaultOpen)) {
+    ImGui::PushID("Axes");
+    int axisCount = m_data.axes.count;
+    if (ImGui::InputInt("Count", &axisCount)) {
+      if (axisCount < 0) {
+        axisCount = 0;
+      }
+      if (axisCount > HAL_kMaxJoystickAxes) {
+        axisCount = HAL_kMaxJoystickAxes;
+      }
+      m_data.axes.count = axisCount;
+    }
+    for (int i = 0; i < axisCount; ++i) {
+      std::snprintf(label, sizeof(label), "Axis %d", i);
+      if (ImGui::TreeNodeEx(label, ImGuiTreeNodeFlags_DefaultOpen)) {
+        EditKey("Increase", &m_axisConfig[i].incKey);
+        EditKey("Decrease", &m_axisConfig[i].decKey);
+        ImGui::InputFloat("Key Rate", &m_axisConfig[i].keyRate);
+        ImGui::InputFloat("Decay Rate", &m_axisConfig[i].decayRate);
+
+        float maxAbsValue = m_axisConfig[i].maxAbsValue;
+        if (ImGui::InputFloat("Max Absolute Value", &maxAbsValue)) {
+          m_axisConfig[i].maxAbsValue = std::clamp(maxAbsValue, -1.0f, 1.0f);
+        }
+
+        ImGui::TreePop();
+      }
+    }
+    ImGui::PopID();
+  }
+
+  // buttons
+  if (ImGui::CollapsingHeader("Buttons", ImGuiTreeNodeFlags_DefaultOpen)) {
+    ImGui::PushID("Buttons");
+    int buttonCount = m_data.buttons.count;
+    if (ImGui::InputInt("Count", &buttonCount)) {
+      if (buttonCount < 0) {
+        buttonCount = 0;
+      }
+      if (buttonCount > kMaxButtonCount) {
+        buttonCount = kMaxButtonCount;
+      }
+      m_data.buttons.count = buttonCount;
+    }
+    for (int i = 0; i < buttonCount; ++i) {
+      std::snprintf(label, sizeof(label), "Button %d", i + 1);
+      EditKey(label, &m_buttonKey[i]);
+    }
+    ImGui::PopID();
+  }
+
+  // povs
+  if (ImGui::CollapsingHeader("POVs", ImGuiTreeNodeFlags_DefaultOpen)) {
+    ImGui::PushID("POVs");
+    int povCount = m_data.povs.count;
+    if (ImGui::InputInt("Count", &povCount)) {
+      if (povCount < 0) {
+        povCount = 0;
+      }
+      if (povCount > HAL_kMaxJoystickPOVs) {
+        povCount = HAL_kMaxJoystickPOVs;
+      }
+      m_data.povs.count = povCount;
+    }
+    for (int i = 0; i < povCount; ++i) {
+      std::snprintf(label, sizeof(label), "POV %d", i);
+      if (ImGui::TreeNodeEx(label, ImGuiTreeNodeFlags_DefaultOpen)) {
+        EditKey("  0 deg", &m_povConfig[i].key0);
+        EditKey(" 45 deg", &m_povConfig[i].key45);
+        EditKey(" 90 deg", &m_povConfig[i].key90);
+        EditKey("135 deg", &m_povConfig[i].key135);
+        EditKey("180 deg", &m_povConfig[i].key180);
+        EditKey("225 deg", &m_povConfig[i].key225);
+        EditKey("270 deg", &m_povConfig[i].key270);
+        EditKey("315 deg", &m_povConfig[i].key315);
+        ImGui::TreePop();
+      }
+    }
+    ImGui::PopID();
+  }
+
+  ImGui::PopItemWidth();
+}
+
+static inline bool IsKeyDown(ImGuiIO& io, int key) {
+  return key >= 0 && key < IM_ARRAYSIZE(ImGuiIO::KeysDown) && io.KeysDown[key];
+}
+
+void KeyboardJoystick::Update() {
+  ImGuiIO& io = ImGui::GetIO();
+
+  if (m_data.axes.count > 0 || m_data.buttons.count > 0 ||
+      m_data.povs.count > 0) {
+    m_present = true;
+  }
+
+  // axes
+  for (int i = 0; i < m_data.axes.count; ++i) {
+    auto& config = m_axisConfig[i];
+    float& axisValue = m_data.axes.axes[i];
+    // increase/decrease while key held down (to saturation); decay back to 0
+    if (IsKeyDown(io, config.incKey)) {
+      axisValue += config.keyRate;
+      if (axisValue > config.maxAbsValue) {
+        axisValue = config.maxAbsValue;
+      }
+    } else if (axisValue > 0) {
+      if (axisValue < config.decayRate) {
+        axisValue = 0;
+      } else {
+        axisValue -= config.decayRate;
+      }
+    }
+
+    if (IsKeyDown(io, config.decKey)) {
+      axisValue -= config.keyRate;
+      if (axisValue < -config.maxAbsValue) {
+        axisValue = -config.maxAbsValue;
+      }
+    } else if (axisValue < 0) {
+      if (axisValue > -config.decayRate) {
+        axisValue = 0;
+      } else {
+        axisValue += config.decayRate;
+      }
+    }
+  }
+
+  // buttons
+  m_data.buttons.buttons = 0;
+  m_anyButtonPressed = false;
+  for (int i = 0; i < m_data.buttons.count; ++i) {
+    if (IsKeyDown(io, m_buttonKey[i])) {
+      m_data.buttons.buttons |= 1u << i;
+      m_anyButtonPressed = true;
+    }
+  }
+
+  // povs
+  for (int i = 0; i < m_data.povs.count; ++i) {
+    auto& config = m_povConfig[i];
+    auto& povValue = m_data.povs.povs[i];
+    povValue = -1;
+    if (IsKeyDown(io, config.key0)) {
+      povValue = 0;
+    } else if (IsKeyDown(io, config.key45)) {
+      povValue = 45;
+    } else if (IsKeyDown(io, config.key90)) {
+      povValue = 90;
+    } else if (IsKeyDown(io, config.key135)) {
+      povValue = 135;
+    } else if (IsKeyDown(io, config.key180)) {
+      povValue = 180;
+    } else if (IsKeyDown(io, config.key225)) {
+      povValue = 225;
+    } else if (IsKeyDown(io, config.key270)) {
+      povValue = 270;
+    } else if (IsKeyDown(io, config.key315)) {
+      povValue = 315;
+    }
+  }
+
+  // try to find matching GUID
+  for (auto&& joy : gRobotJoysticks) {
+    if (m_guid == joy.guid) {
+      joy.sys = this;
+      joy.guid.clear();
+      break;
+    }
+  }
+
+  // update desc
+  m_data.desc.axisCount = m_data.axes.count;
+  m_data.desc.buttonCount = m_data.buttons.count;
+  m_data.desc.povCount = m_data.povs.count;
+}
+
+void KeyboardJoystick::ClearKey(int key) {
+  for (auto&& config : m_axisConfig) {
+    if (config.incKey == key) {
+      config.incKey = -1;
+    }
+    if (config.decKey == key) {
+      config.decKey = -1;
+    }
+  }
+  for (auto&& buttonKey : m_buttonKey) {
+    if (buttonKey == key) {
+      buttonKey = -1;
+    }
+  }
+  for (auto&& config : m_povConfig) {
+    if (config.key0 == key) {
+      config.key0 = -1;
+    }
+    if (config.key45 == key) {
+      config.key45 = -1;
+    }
+    if (config.key90 == key) {
+      config.key90 = -1;
+    }
+    if (config.key135 == key) {
+      config.key135 = -1;
+    }
+    if (config.key180 == key) {
+      config.key180 = -1;
+    }
+    if (config.key225 == key) {
+      config.key225 = -1;
+    }
+    if (config.key270 == key) {
+      config.key270 = -1;
+    }
+    if (config.key315 == key) {
+      config.key315 = -1;
+    }
+  }
+}
+
+void KeyboardJoystick::ReadIni(std::string_view name, std::string_view value) {
+  if (wpi::starts_with(name, "axis")) {
+    name.remove_prefix(4);
+    if (name == "Count") {
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_data.axes.count = (std::min)(v.value(), HAL_kMaxJoystickAxes);
+      }
+      return;
+    }
+
+    auto index = wpi::consume_integer<unsigned int>(&name, 10).value_or(
+        HAL_kMaxJoystickAxes);
+    if (index >= HAL_kMaxJoystickAxes) {
+      return;
+    }
+    if (name == "incKey") {
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_axisConfig[index].incKey = v.value();
+      }
+    } else if (name == "decKey") {
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_axisConfig[index].decKey = v.value();
+      }
+    } else if (name == "keyRate") {
+      if (auto v = wpi::parse_float<float>(value)) {
+        m_axisConfig[index].keyRate = v.value();
+      }
+    } else if (name == "decayRate") {
+      if (auto v = wpi::parse_float<float>(value)) {
+        m_axisConfig[index].decayRate = v.value();
+      }
+    } else if (name == "maxAbsValue") {
+      if (auto v = wpi::parse_float<float>(value)) {
+        m_axisConfig[index].maxAbsValue = v.value();
+      }
+    }
+  } else if (wpi::starts_with(name, "button")) {
+    name.remove_prefix(6);
+    if (name == "Count") {
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_data.buttons.count = (std::min)(v.value(), kMaxButtonCount);
+      }
+      return;
+    }
+
+    auto index =
+        wpi::parse_integer<unsigned int>(name, 10).value_or(kMaxButtonCount);
+    if (index >= kMaxButtonCount) {
+      return;
+    }
+    int v = wpi::parse_integer<int>(value, 10).value_or(-1);
+    if (v < 0 || v >= IM_ARRAYSIZE(ImGuiIO::KeysDown)) {
+      return;
+    }
+    m_buttonKey[index] = v;
+  } else if (wpi::starts_with(name, "pov")) {
+    name.remove_prefix(3);
+    if (name == "Count") {
+      if (auto v = wpi::parse_integer<int>(value, 10)) {
+        m_data.povs.count = (std::min)(v.value(), HAL_kMaxJoystickPOVs);
+      }
+      return;
+    }
+
+    auto index = wpi::consume_integer<unsigned int>(&name, 10).value_or(
+        HAL_kMaxJoystickPOVs);
+    if (index >= HAL_kMaxJoystickPOVs) {
+      return;
+    }
+    int v = wpi::parse_integer<int>(value, 10).value_or(-1);
+    if (v < 0 || v >= IM_ARRAYSIZE(ImGuiIO::KeysDown)) {
+      return;
+    }
+    if (name == "key0") {
+      m_povConfig[index].key0 = v;
+    } else if (name == "key45") {
+      m_povConfig[index].key45 = v;
+    } else if (name == "key90") {
+      m_povConfig[index].key90 = v;
+    } else if (name == "key135") {
+      m_povConfig[index].key135 = v;
+    } else if (name == "key180") {
+      m_povConfig[index].key180 = v;
+    } else if (name == "key225") {
+      m_povConfig[index].key225 = v;
+    } else if (name == "key270") {
+      m_povConfig[index].key270 = v;
+    } else if (name == "key315") {
+      m_povConfig[index].key315 = v;
+    }
+  }
+}
+
+void KeyboardJoystick::WriteIni(ImGuiTextBuffer* out_buf) const {
+  out_buf->appendf("axisCount=%d\nbuttonCount=%d\npovCount=%d\n",
+                   m_data.axes.count, m_data.buttons.count, m_data.povs.count);
+  for (int i = 0; i < m_data.axes.count; ++i) {
+    auto& c = m_axisConfig[i];
+    out_buf->appendf(
+        "axis%dincKey=%d\naxis%ddecKey=%d\naxis%dkeyRate=%f\n"
+        "axis%ddecayRate=%f\naxis%dmaxAbsValue=%f\n",
+        i, c.incKey, i, c.decKey, i, c.keyRate, i, c.decayRate, i,
+        c.maxAbsValue);
+  }
+  for (int i = 0; i < m_data.buttons.count; ++i) {
+    out_buf->appendf("button%d=%d\n", i, m_buttonKey[i]);
+  }
+  for (int i = 0; i < m_data.povs.count; ++i) {
+    auto& c = m_povConfig[i];
+    out_buf->appendf(
+        "pov%dkey0=%d\npov%dkey45=%d\npov%dkey90=%d\npov%dkey135=%d\n"
+        "pov%dkey180=%d\npov%dkey225=%d\npov%dkey270=%d\npov%dkey315=%d\n",
+        i, c.key0, i, c.key45, i, c.key90, i, c.key135, i, c.key180, i,
+        c.key225, i, c.key270, i, c.key315);
+  }
+}
+
+GlfwKeyboardJoystick::GlfwKeyboardJoystick(int index, bool noDefaults)
+    : KeyboardJoystick{index} {
+  if (noDefaults) {
+    return;
+  }
+  // set up a default keyboard config for 0, 1, and 2
+  if (index == 0) {
+    m_data.axes.count = 3;
+    m_axisConfig[0].incKey = GLFW_KEY_D;
+    m_axisConfig[0].decKey = GLFW_KEY_A;
+    m_axisConfig[1].incKey = GLFW_KEY_S;
+    m_axisConfig[1].decKey = GLFW_KEY_W;
+    m_axisConfig[2].incKey = GLFW_KEY_R;
+    m_axisConfig[2].decKey = GLFW_KEY_E;
+    m_axisConfig[2].keyRate = 0.01f;
+    m_axisConfig[2].decayRate = 0;  // works like a throttle
+    m_data.buttons.count = 4;
+    m_buttonKey[0] = GLFW_KEY_Z;
+    m_buttonKey[1] = GLFW_KEY_X;
+    m_buttonKey[2] = GLFW_KEY_C;
+    m_buttonKey[3] = GLFW_KEY_V;
+    m_data.povs.count = 1;
+    m_povConfig[0].key0 = GLFW_KEY_KP_8;
+    m_povConfig[0].key45 = GLFW_KEY_KP_9;
+    m_povConfig[0].key90 = GLFW_KEY_KP_6;
+    m_povConfig[0].key135 = GLFW_KEY_KP_3;
+    m_povConfig[0].key180 = GLFW_KEY_KP_2;
+    m_povConfig[0].key225 = GLFW_KEY_KP_1;
+    m_povConfig[0].key270 = GLFW_KEY_KP_4;
+    m_povConfig[0].key315 = GLFW_KEY_KP_7;
+  } else if (index == 1) {
+    m_data.axes.count = 2;
+    m_axisConfig[0].incKey = GLFW_KEY_L;
+    m_axisConfig[0].decKey = GLFW_KEY_J;
+    m_axisConfig[1].incKey = GLFW_KEY_K;
+    m_axisConfig[1].decKey = GLFW_KEY_I;
+    m_data.buttons.count = 4;
+    m_buttonKey[0] = GLFW_KEY_M;
+    m_buttonKey[1] = GLFW_KEY_COMMA;
+    m_buttonKey[2] = GLFW_KEY_PERIOD;
+    m_buttonKey[3] = GLFW_KEY_SLASH;
+  } else if (index == 2) {
+    m_data.axes.count = 2;
+    m_axisConfig[0].incKey = GLFW_KEY_RIGHT;
+    m_axisConfig[0].decKey = GLFW_KEY_LEFT;
+    m_axisConfig[1].incKey = GLFW_KEY_DOWN;
+    m_axisConfig[1].decKey = GLFW_KEY_UP;
+    m_data.buttons.count = 6;
+    m_buttonKey[0] = GLFW_KEY_INSERT;
+    m_buttonKey[1] = GLFW_KEY_HOME;
+    m_buttonKey[2] = GLFW_KEY_PAGE_UP;
+    m_buttonKey[3] = GLFW_KEY_DELETE;
+    m_buttonKey[4] = GLFW_KEY_END;
+    m_buttonKey[5] = GLFW_KEY_PAGE_DOWN;
+  }
+}
+
+const char* GlfwKeyboardJoystick::GetKeyName(int key) const {
+  if (key < 0) {
+    return "(None)";
+  }
+  const char* name = glfwGetKeyName(key, 0);
+  if (name) {
+    return name;
+  }
+  // glfwGetKeyName sometimes doesn't have these keys
+  switch (key) {
+    case GLFW_KEY_RIGHT:
+      return "Right";
+    case GLFW_KEY_LEFT:
+      return "Left";
+    case GLFW_KEY_DOWN:
+      return "Down";
+    case GLFW_KEY_UP:
+      return "Up";
+    case GLFW_KEY_INSERT:
+      return "Insert";
+    case GLFW_KEY_HOME:
+      return "Home";
+    case GLFW_KEY_PAGE_UP:
+      return "PgUp";
+    case GLFW_KEY_DELETE:
+      return "Delete";
+    case GLFW_KEY_END:
+      return "End";
+    case GLFW_KEY_PAGE_DOWN:
+      return "PgDn";
+  }
+  return "(Unknown)";
+}
+
+void RobotJoystick::Update() {
+  Clear();
+  if (sys) {
+    sys->GetData(&data, useGamepad);
+  }
 }
 
 void RobotJoystick::SetHAL(int i) {
+  if (!gZeroDisconnectedJoysticks && (!sys || !sys->IsPresent())) {
+    return;
+  }
   // set at HAL level
-  HALSIM_SetJoystickDescriptor(i, &desc);
-  HALSIM_SetJoystickAxes(i, &axes);
-  HALSIM_SetJoystickButtons(i, &buttons);
-  HALSIM_SetJoystickPOVs(i, &povs);
+  HALSIM_SetJoystickDescriptor(i, &data.desc);
+  HALSIM_SetJoystickAxes(i, &data.axes);
+  HALSIM_SetJoystickButtons(i, &data.buttons);
+  HALSIM_SetJoystickPOVs(i, &data.povs);
 }
 
 void RobotJoystick::GetHAL(int i) {
-  HALSIM_GetJoystickDescriptor(i, &desc);
-  HALSIM_GetJoystickAxes(i, &axes);
-  HALSIM_GetJoystickButtons(i, &buttons);
-  HALSIM_GetJoystickPOVs(i, &povs);
+  HALSIM_GetJoystickDescriptor(i, &data.desc);
+  HALSIM_GetJoystickAxes(i, &data.axes);
+  HALSIM_GetJoystickButtons(i, &data.buttons);
+  HALSIM_GetJoystickPOVs(i, &data.povs);
 }
 
 static void DriverStationExecute() {
@@ -394,7 +1138,7 @@
       if (!source || source->axisCount != axisCount ||
           source->buttonCount != buttonCount || source->povCount != povCount) {
         source.reset();
-        source = std::make_unique<JoystickSource>(i);
+        source = std::make_unique<JoystickModel>(i);
       }
     } else {
       source.reset();
@@ -405,23 +1149,37 @@
 
   bool disableDS = IsDSDisabled();
   if (disableDS && !prevDisableDS) {
-    HALSimGui::SetWindowVisibility("System Joysticks", HALSimGui::kDisabled);
+    if (auto win = HALSimGui::manager.GetWindow("System Joysticks")) {
+      win->SetVisibility(glass::Window::kDisabled);
+    }
   } else if (!disableDS && prevDisableDS) {
-    HALSimGui::SetWindowVisibility("System Joysticks", HALSimGui::kShow);
+    if (auto win = HALSimGui::manager.GetWindow("System Joysticks")) {
+      win->SetVisibility(glass::Window::kShow);
+    }
   }
   prevDisableDS = disableDS;
-  if (disableDS) return;
+  if (disableDS) {
+    return;
+  }
 
   double curTime = glfwGetTime();
 
   // update system joysticks
+  gNumGlfwJoysticks = 0;
   for (int i = 0; i <= GLFW_JOYSTICK_LAST; ++i) {
-    gSystemJoysticks[i].Update(i);
-    if (gSystemJoysticks[i].present) gNumSystemJoysticks = i + 1;
+    gGlfwJoysticks[i]->Update();
+    if (gGlfwJoysticks[i]->IsPresent()) {
+      gNumGlfwJoysticks = i + 1;
+    }
+  }
+  for (auto&& joy : gKeyboardJoysticks) {
+    joy->Update();
   }
 
   // update robot joysticks
-  for (auto&& joy : gRobotJoysticks) joy.Update();
+  for (auto&& joy : gRobotJoysticks) {
+    joy.Update();
+  }
 
   bool isEnabled = HALSIM_GetDriverStationEnabled();
   bool isAuto = HALSIM_GetDriverStationAutonomous();
@@ -429,16 +1187,38 @@
 
   // Robot state
   {
+    // DS hotkeys
+    bool enableHotkey = false;
+    bool disableHotkey = false;
+    if (gUseEnableDisableHotkeys) {
+      ImGuiIO& io = ImGui::GetIO();
+      if (io.KeysDown[GLFW_KEY_ENTER] || io.KeysDown[GLFW_KEY_KP_ENTER]) {
+        disableHotkey = true;
+      } else if (io.KeysDown[GLFW_KEY_LEFT_BRACKET] &&
+                 io.KeysDown[GLFW_KEY_RIGHT_BRACKET] &&
+                 io.KeysDown[GLFW_KEY_BACKSLASH]) {
+        enableHotkey = true;
+      }
+    }
+    if (gUseEstopHotkey) {
+      ImGuiIO& io = ImGui::GetIO();
+      if (io.KeysDown[GLFW_KEY_SPACE]) {
+        HALSIM_SetDriverStationEnabled(false);
+      }
+    }
+
     ImGui::SetNextWindowPos(ImVec2{5, 20}, ImGuiCond_FirstUseEver);
     ImGui::Begin("Robot State", nullptr, ImGuiWindowFlags_AlwaysAutoResize);
-    if (ImGui::Selectable("Disabled", !isEnabled))
+    if (ImGui::Selectable("Disabled", !isEnabled) || disableHotkey) {
       HALSIM_SetDriverStationEnabled(false);
+    }
     if (ImGui::Selectable("Autonomous", isEnabled && isAuto && !isTest)) {
       HALSIM_SetDriverStationAutonomous(true);
       HALSIM_SetDriverStationTest(false);
       HALSIM_SetDriverStationEnabled(true);
     }
-    if (ImGui::Selectable("Teleoperated", isEnabled && !isAuto && !isTest)) {
+    if (ImGui::Selectable("Teleoperated", isEnabled && !isAuto && !isTest) ||
+        enableHotkey) {
       HALSIM_SetDriverStationAutonomous(false);
       HALSIM_SetDriverStationTest(false);
       HALSIM_SetDriverStationEnabled(true);
@@ -452,7 +1232,9 @@
   }
 
   // Update HAL
-  for (int i = 0; i < HAL_kMaxJoysticks; ++i) gRobotJoysticks[i].SetHAL(i);
+  for (int i = 0; i < HAL_kMaxJoysticks; ++i) {
+    gRobotJoysticks[i].SetHAL(i);
+  }
 
   // Send new data every 20 ms (may be slower depending on GUI refresh rate)
   static double lastNewDataTime = 0.0;
@@ -462,106 +1244,97 @@
   }
 }
 
-static void DisplayFMS() {
-  bool fmsAttached = HALSIM_GetDriverStationFmsAttached();
-  bool dsAttached = HALSIM_GetDriverStationDsAttached();
-  static const char* stations[] = {"Red 1",  "Red 2",  "Red 3",
-                                   "Blue 1", "Blue 2", "Blue 3"};
-  int allianceStationId = HALSIM_GetDriverStationAllianceStationId();
+FMSSimModel::FMSSimModel() {
+  m_fmsAttached.SetDigital(true);
+  m_dsAttached.SetDigital(true);
+  m_estop.SetDigital(true);
+  m_enabled.SetDigital(true);
+  m_test.SetDigital(true);
+  m_autonomous.SetDigital(true);
+}
+
+void FMSSimModel::Update() {
+  bool enabled = HALSIM_GetDriverStationEnabled();
+  m_fmsAttached.SetValue(HALSIM_GetDriverStationFmsAttached());
+  m_dsAttached.SetValue(HALSIM_GetDriverStationDsAttached());
+  m_allianceStationId.SetValue(HALSIM_GetDriverStationAllianceStationId());
+  m_estop.SetValue(HALSIM_GetDriverStationEStop());
+  m_enabled.SetValue(enabled);
+  m_test.SetValue(HALSIM_GetDriverStationTest());
+  m_autonomous.SetValue(HALSIM_GetDriverStationAutonomous());
+
   double matchTime = HALSIM_GetDriverStationMatchTime();
-  HAL_MatchInfo matchInfo;
-  HALSIM_GetMatchInfo(&matchInfo);
+  if (m_matchTimeEnabled && !IsDSDisabled()) {
+    int32_t status = 0;
+    double curTime = HAL_GetFPGATime(&status) * 1.0e-6;
+    if (m_startMatchTime == 0.0) {
+      m_startMatchTime = curTime;
+    }
+    if (enabled) {
+      matchTime = curTime - m_startMatchTime;
+      HALSIM_SetDriverStationMatchTime(matchTime);
+    } else {
+      if (m_prevTime == 0.0) {
+        m_prevTime = curTime;
+      }
+      m_startMatchTime += (curTime - m_prevTime);
+    }
+    m_prevTime = curTime;
+  } else {
+    m_startMatchTime = 0.0;
+    m_prevTime = 0.0;
+  }
+  m_matchTime.SetValue(matchTime);
+}
 
-  if (IsDSDisabled()) {
-    if (!HALSIM_GetDriverStationEnabled())
-      ImGui::Text("Robot State: Disabled");
-    else if (HALSIM_GetDriverStationTest())
-      ImGui::Text("Robot State: Test");
-    else if (HALSIM_GetDriverStationAutonomous())
-      ImGui::Text("Robot State: Autonomous");
-    else
-      ImGui::Text("Robot State: Teleoperated");
+bool FMSSimModel::IsReadOnly() {
+  return IsDSDisabled();
+}
 
-    ImGui::Text("FMS Attached: %s", fmsAttached ? "Yes" : "No");
-    ImGui::Text("DS Attached: %s", dsAttached ? "Yes" : "No");
-    ImGui::Text("Alliance Station: %s", stations[allianceStationId]);
-    ImGui::Text("Match Time: %.1f", matchTime);
-    ImGui::Text("Game Specific: %s", matchInfo.gameSpecificMessage);
-    return;
+static void DisplaySystemJoystick(SystemJoystick& joy, int i) {
+  char label[64];
+  std::snprintf(label, sizeof(label), "%d: %s", i, joy.GetName());
+
+  // highlight if any buttons pressed
+  bool anyButtonPressed = joy.IsAnyButtonPressed();
+  if (anyButtonPressed) {
+    ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 255, 0, 255));
+  }
+  ImGui::Selectable(label, false,
+                    joy.IsPresent() ? ImGuiSelectableFlags_None
+                                    : ImGuiSelectableFlags_Disabled);
+  if (anyButtonPressed) {
+    ImGui::PopStyleColor();
   }
 
-  double curTime = glfwGetTime();
-
-  // FMS Attached
-  if (ImGui::Checkbox("FMS Attached", &fmsAttached))
-    HALSIM_SetDriverStationFmsAttached(fmsAttached);
-
-  // DS Attached
-  if (ImGui::Checkbox("DS Attached", &dsAttached))
-    HALSIM_SetDriverStationDsAttached(dsAttached);
-
-  // Alliance Station
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-  if (ImGui::Combo("Alliance Station", &allianceStationId, stations, 6))
-    HALSIM_SetDriverStationAllianceStationId(
-        static_cast<HAL_AllianceStationID>(allianceStationId));
-
-  // Match Time
-  static bool matchTimeEnabled = true;
-  ImGui::Checkbox("Match Time Enabled", &matchTimeEnabled);
-
-  static double startMatchTime = 0.0;
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-  if (ImGui::InputDouble("Match Time", &matchTime, 0, 0, "%.1f",
-                         ImGuiInputTextFlags_EnterReturnsTrue)) {
-    HALSIM_SetDriverStationMatchTime(matchTime);
-    startMatchTime = curTime - matchTime;
-  } else if (!HALSIM_GetDriverStationEnabled() || HALSIM_IsTimingPaused()) {
-    startMatchTime = curTime - matchTime;
-  } else if (matchTimeEnabled) {
-    HALSIM_SetDriverStationMatchTime(curTime - startMatchTime);
-  }
-  ImGui::SameLine();
-  if (ImGui::Button("Reset")) {
-    HALSIM_SetDriverStationMatchTime(0.0);
-    startMatchTime = curTime;
-  }
-
-  // Game Specific Message
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-  if (ImGui::InputText("Game Specific",
-                       reinterpret_cast<char*>(matchInfo.gameSpecificMessage),
-                       sizeof(matchInfo.gameSpecificMessage),
-                       ImGuiInputTextFlags_EnterReturnsTrue)) {
-    matchInfo.gameSpecificMessageSize =
-        std::strlen(reinterpret_cast<char*>(matchInfo.gameSpecificMessage));
-    HALSIM_SetMatchInfo(&matchInfo);
+  // drag and drop sources are the low level joysticks
+  if (ImGui::BeginDragDropSource()) {
+    SystemJoystick* joyPtr = &joy;
+    ImGui::SetDragDropPayload("Joystick", &joyPtr, sizeof(joyPtr));  // NOLINT
+    ImGui::Text("%d: %s", i, joy.GetName());
+    ImGui::EndDragDropSource();
   }
 }
 
 static void DisplaySystemJoysticks() {
   ImGui::Text("(Drag and drop to Joysticks)");
-  int numShowJoysticks = gNumSystemJoysticks < 6 ? 6 : gNumSystemJoysticks;
+  int numShowJoysticks = gNumGlfwJoysticks < 6 ? 6 : gNumGlfwJoysticks;
   for (int i = 0; i < numShowJoysticks; ++i) {
-    auto& joy = gSystemJoysticks[i];
-    wpi::SmallString<128> label;
-    wpi::raw_svector_ostream os(label);
-    os << wpi::format("%d: %s", i, joy.name);
-
-    // highlight if any buttons pressed
-    if (joy.anyButtonPressed)
-      ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(255, 255, 0, 255));
-    ImGui::Selectable(label.c_str(), false,
-                      joy.present ? ImGuiSelectableFlags_None
-                                  : ImGuiSelectableFlags_Disabled);
-    if (joy.anyButtonPressed) ImGui::PopStyleColor();
-
-    // drag and drop sources are the low level joysticks
-    if (ImGui::BeginDragDropSource()) {
-      SystemJoystick* joyPtr = &joy;
-      ImGui::SetDragDropPayload("Joystick", &joyPtr, sizeof(joyPtr));
-      ImGui::Text("%d: %s", i, joy.name);
-      ImGui::EndDragDropSource();
+    DisplaySystemJoystick(*gGlfwJoysticks[i], i);
+  }
+  for (size_t i = 0; i < gKeyboardJoysticks.size(); ++i) {
+    auto joy = gKeyboardJoysticks[i].get();
+    DisplaySystemJoystick(*joy, i + GLFW_JOYSTICK_LAST + 1);
+    if (ImGui::BeginPopupContextItem()) {
+      char buf[64];
+      std::snprintf(buf, sizeof(buf), "%s Settings", joy->GetName());
+      if (ImGui::MenuItem(buf)) {
+        if (auto win = DriverStationGui::dsManager.GetWindow(buf)) {
+          win->SetVisible(true);
+        }
+        ImGui::CloseCurrentPopup();
+      }
+      ImGui::EndPopup();
     }
   }
 }
@@ -579,9 +1352,9 @@
     if (!disableDS && joy.sys) {
       ImGui::Selectable(label, false);
       if (ImGui::BeginDragDropSource()) {
-        ImGui::SetDragDropPayload("Joystick", &joy.sys, sizeof(joy.sys));
-        ImGui::Text("%d: %s", static_cast<int>(joy.sys - gSystemJoysticks),
-                    joy.sys->name);
+        ImGui::SetDragDropPayload("Joystick", &joy.sys,
+                                  sizeof(joy.sys));  // NOLINT
+        ImGui::Text("%d: %s", joy.sys->GetIndex(), joy.sys->GetName());
         ImGui::EndDragDropSource();
       }
     } else {
@@ -590,16 +1363,20 @@
     if (!disableDS && ImGui::BeginDragDropTarget()) {
       if (const ImGuiPayload* payload =
               ImGui::AcceptDragDropPayload("Joystick")) {
-        IM_ASSERT(payload->DataSize == sizeof(SystemJoystick*));
+        IM_ASSERT(payload->DataSize == sizeof(SystemJoystick*));  // NOLINT
         SystemJoystick* payload_sys =
             *static_cast<SystemJoystick* const*>(payload->Data);
         // clear it from the other joysticks
         for (auto&& joy2 : gRobotJoysticks) {
-          if (joy2.sys == payload_sys) joy2.sys = nullptr;
+          if (joy2.sys == payload_sys) {
+            joy2.sys = nullptr;
+          }
         }
         joy.sys = payload_sys;
         joy.guid.clear();
-        joy.useGamepad = false;
+        std::string_view name{payload_sys->GetName()};
+        joy.useGamepad =
+            wpi::starts_with(name, "Xbox") || wpi::contains(name, "pad");
       }
       ImGui::EndDragDropTarget();
     }
@@ -612,53 +1389,58 @@
     auto& joy = gRobotJoysticks[i];
     auto source = gJoystickSources[i].get();
 
-    if (disableDS) joy.GetHAL(i);
+    if (disableDS) {
+      joy.GetHAL(i);
+    }
 
-    if ((disableDS && joy.desc.type != 0) || (joy.sys && joy.sys->present)) {
+    if ((disableDS && joy.data.desc.type != 0) ||
+        (joy.sys && joy.sys->IsPresent())) {
       // update GUI display
       ImGui::PushID(i);
       if (disableDS) {
-        ImGui::Text("%s", joy.desc.name);
-        ImGui::Text("Gamepad: %s", joy.desc.isXbox ? "Yes" : "No");
+        ImGui::Text("%s", joy.data.desc.name);
+        ImGui::Text("Gamepad: %s", joy.data.desc.isXbox ? "Yes" : "No");
       } else {
-        ImGui::Text("%d: %s", static_cast<int>(joy.sys - gSystemJoysticks),
-                    joy.sys->name);
+        ImGui::Text("%d: %s", joy.sys->GetIndex(), joy.sys->GetName());
 
-        if (joy.sys->isGamepad) ImGui::Checkbox("Map gamepad", &joy.useGamepad);
+        if (joy.sys->IsGamepad()) {
+          ImGui::Checkbox("Map gamepad", &joy.useGamepad);
+        }
       }
 
-      for (int j = 0; j < joy.axes.count; ++j) {
+      for (int j = 0; j < joy.data.axes.count; ++j) {
         if (source && source->axes[j]) {
           char label[64];
           std::snprintf(label, sizeof(label), "Axis[%d]", j);
           ImGui::Selectable(label);
           source->axes[j]->EmitDrag();
           ImGui::SameLine();
-          ImGui::Text(": %.3f", joy.axes.axes[j]);
+          ImGui::Text(": %.3f", joy.data.axes.axes[j]);
         } else {
-          ImGui::Text("Axis[%d]: %.3f", j, joy.axes.axes[j]);
+          ImGui::Text("Axis[%d]: %.3f", j, joy.data.axes.axes[j]);
         }
       }
 
-      for (int j = 0; j < joy.povs.count; ++j) {
+      for (int j = 0; j < joy.data.povs.count; ++j) {
         if (source && source->povs[j]) {
           char label[64];
           std::snprintf(label, sizeof(label), "POVs[%d]", j);
           ImGui::Selectable(label);
           source->povs[j]->EmitDrag();
           ImGui::SameLine();
-          ImGui::Text(": %d", joy.povs.povs[j]);
+          ImGui::Text(": %d", joy.data.povs.povs[j]);
         } else {
-          ImGui::Text("POVs[%d]: %d", j, joy.povs.povs[j]);
+          ImGui::Text("POVs[%d]: %d", j, joy.data.povs.povs[j]);
         }
       }
 
       // show buttons as multiple lines of LED indicators, 8 per line
       static const ImU32 color = IM_COL32(255, 255, 102, 255);
       wpi::SmallVector<int, 64> buttons;
-      buttons.resize(joy.buttons.count);
-      for (int j = 0; j < joy.buttons.count; ++j)
+      buttons.resize(joy.data.buttons.count);
+      for (int j = 0; j < joy.data.buttons.count; ++j) {
         buttons[j] = joy.IsButtonPressed(j) ? 1 : -1;
+      }
       DrawLEDSources(buttons.data(), source ? source->buttons : nullptr,
                      buttons.size(), 8, &color);
       ImGui::PopID();
@@ -670,15 +1452,25 @@
   ImGui::Columns(1);
 }
 
-static void DriverStationOptionMenu() {
+void DSManager::DisplayMenu() {
   if (gDSSocketConnected && *gDSSocketConnected) {
     ImGui::MenuItem("Turn off DS (real DS connected)", nullptr, true, false);
   } else {
     ImGui::MenuItem("Turn off DS", nullptr, &gDisableDS);
+    ImGui::MenuItem("Zero disconnected joysticks", nullptr,
+                    &gZeroDisconnectedJoysticks);
+    ImGui::MenuItem("Enable on []\\ combo, Disable on Enter", nullptr,
+                    &gUseEnableDisableHotkeys);
+    ImGui::MenuItem("Disable on Spacebar", nullptr, &gUseEstopHotkey);
+  }
+  ImGui::Separator();
+
+  for (auto&& window : m_windows) {
+    window->DisplayMenuItem();
   }
 }
 
-void DriverStationGui::Initialize() {
+static void DriverStationInitialize() {
   // hook ini handler to save joystick settings
   ImGuiSettingsHandler iniHandler;
   iniHandler.TypeName = "Joystick";
@@ -688,6 +1480,14 @@
   iniHandler.WriteAllFn = JoystickWriteAll;
   ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
 
+  // hook ini handler to save keyboard settings
+  iniHandler.TypeName = "KeyboardJoystick";
+  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
+  iniHandler.ReadOpenFn = KeyboardJoystickReadOpen;
+  iniHandler.ReadLineFn = KeyboardJoystickReadLine;
+  iniHandler.WriteAllFn = KeyboardJoystickWriteAll;
+  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+
   // hook ini handler to save DS settings
   iniHandler.TypeName = "DriverStation";
   iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
@@ -695,18 +1495,58 @@
   iniHandler.ReadLineFn = DriverStationReadLine;
   iniHandler.WriteAllFn = DriverStationWriteAll;
   ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
+}
 
-  HALSimGui::AddExecute(DriverStationExecute);
-  HALSimGui::AddWindow("FMS", DisplayFMS, ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::AddWindow("System Joysticks", DisplaySystemJoysticks,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::AddWindow("Joysticks", DisplayJoysticks,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::AddOptionMenu(DriverStationOptionMenu);
+void DriverStationGui::GlobalInit() {
+  // set up system joysticks (both GLFW and keyboard)
+  for (int i = 0; i <= GLFW_JOYSTICK_LAST; ++i) {
+    gGlfwJoysticks.emplace_back(std::make_unique<GlfwSystemJoystick>(i));
+  }
+  for (int i = 0; i < 4; ++i) {
+    gKeyboardJoysticks.emplace_back(std::make_unique<GlfwKeyboardJoystick>(i));
+  }
 
-  HALSimGui::SetDefaultWindowPos("FMS", 5, 540);
-  HALSimGui::SetDefaultWindowPos("System Joysticks", 5, 385);
-  HALSimGui::SetDefaultWindowPos("Joysticks", 250, 465);
+  dsManager.GlobalInit();
+
+  wpi::gui::AddInit(DriverStationInitialize);
+
+  gFMSModel = std::make_unique<FMSSimModel>();
+
+  wpi::gui::AddEarlyExecute(DriverStationExecute);
+  wpi::gui::AddEarlyExecute([] { gFMSModel->Update(); });
+  if (auto win = dsManager.AddWindow("FMS", [] {
+        DisplayFMS(gFMSModel.get(), &gFMSModel->m_matchTimeEnabled);
+      })) {
+    win->DisableRenamePopup();
+    win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+    win->SetDefaultPos(5, 540);
+  }
+  if (auto win =
+          dsManager.AddWindow("System Joysticks", DisplaySystemJoysticks)) {
+    win->DisableRenamePopup();
+    win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+    win->SetDefaultPos(5, 350);
+  }
+  if (auto win = dsManager.AddWindow("Joysticks", DisplayJoysticks)) {
+    win->DisableRenamePopup();
+    win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+    win->SetDefaultPos(250, 465);
+  }
+  int i = 0;
+  for (auto&& joy : gKeyboardJoysticks) {
+    char label[64];
+    std::snprintf(label, sizeof(label), "%s Settings", joy->GetName());
+    if (auto win = dsManager.AddWindow(
+            label, [j = joy.get()] { j->SettingsDisplay(); })) {
+      win->SetVisible(false);
+      win->DisableRenamePopup();
+      win->SetDefaultPos(10 + 310 * i++, 50);
+      if (i > 3) {
+        i = 0;
+      }
+      win->SetDefaultSize(300, 560);
+    }
+  }
 }
 
 void DriverStationGui::SetDSSocketExtension(void* data) {
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h
index 5bf25b5..51f7554 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.h
@@ -1,18 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <glass/WindowManager.h>
+
+#include <string_view>
+
 namespace halsimgui {
 
+class DSManager : public glass::WindowManager {
+ public:
+  explicit DSManager(std::string_view iniName) : WindowManager{iniName} {}
+
+  void DisplayMenu() override;
+};
+
 class DriverStationGui {
  public:
-  static void Initialize();
+  static void GlobalInit();
   static void SetDSSocketExtension(void* data);
+
+  static DSManager dsManager;
 };
 
 }  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp
deleted file mode 100644
index 5b169f0..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderGui.cpp
+++ /dev/null
@@ -1,286 +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 "EncoderGui.h"
-
-#include <limits>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/EncoderData.h>
-#include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-
-struct EncoderInfo : public NameInfo, public OpenInfo {
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
-    if (NameInfo::ReadIni(name, value)) return true;
-    if (OpenInfo::ReadIni(name, value)) return true;
-    return false;
-  }
-  void WriteIni(ImGuiTextBuffer* out) {
-    NameInfo::WriteIni(out);
-    OpenInfo::WriteIni(out);
-  }
-};
-
-class EncoderSource {
- public:
-  EncoderSource(const wpi::Twine& id, int32_t index, int channelA, int channelB)
-      : distancePerPulse(id + " Dist/Count"),
-        count(id + " Count"),
-        period(id + " Period"),
-        direction(id + " Direction"),
-        distance(id + " Distance"),
-        rate(id + " Rate"),
-        m_index{index},
-        m_channelA{channelA},
-        m_channelB{channelB},
-        m_distancePerPulseCallback{
-            HALSIM_RegisterEncoderDistancePerPulseCallback(
-                index, DistancePerPulseCallbackFunc, this, true)},
-        m_countCallback{HALSIM_RegisterEncoderCountCallback(
-            index, CountCallbackFunc, this, true)},
-        m_periodCallback{HALSIM_RegisterEncoderPeriodCallback(
-            index, PeriodCallbackFunc, this, true)},
-        m_directionCallback{HALSIM_RegisterEncoderDirectionCallback(
-            index, DirectionCallbackFunc, this, true)} {
-    direction.SetDigital(true);
-  }
-
-  EncoderSource(int32_t index, int channelA, int channelB)
-      : EncoderSource("Encoder[" + wpi::Twine(channelA) + wpi::Twine(',') +
-                          wpi::Twine(channelB) + wpi::Twine(']'),
-                      index, channelA, channelB) {}
-
-  explicit EncoderSource(int32_t index)
-      : EncoderSource(index, HALSIM_GetEncoderDigitalChannelA(index),
-                      HALSIM_GetEncoderDigitalChannelB(index)) {}
-
-  ~EncoderSource() {
-    if (m_distancePerPulseCallback != 0)
-      HALSIM_CancelEncoderDistancePerPulseCallback(m_index,
-                                                   m_distancePerPulseCallback);
-    if (m_countCallback != 0)
-      HALSIM_CancelEncoderCountCallback(m_index, m_countCallback);
-    if (m_periodCallback != 0)
-      HALSIM_CancelEncoderCountCallback(m_index, m_periodCallback);
-    if (m_directionCallback != 0)
-      HALSIM_CancelEncoderCountCallback(m_index, m_directionCallback);
-  }
-
-  void SetName(const wpi::Twine& name) {
-    if (name.str().empty()) {
-      distancePerPulse.SetName("");
-      count.SetName("");
-      period.SetName("");
-      direction.SetName("");
-      distance.SetName("");
-      rate.SetName("");
-    } else {
-      distancePerPulse.SetName(name + " Distance/Count");
-      count.SetName(name + " Count");
-      period.SetName(name + " Period");
-      direction.SetName(name + " Direction");
-      distance.SetName(name + " Distance");
-      rate.SetName(name + " Rate");
-    }
-  }
-
-  int32_t GetIndex() const { return m_index; }
-  int GetChannelA() const { return m_channelA; }
-  int GetChannelB() const { return m_channelB; }
-
-  GuiDataSource distancePerPulse;
-  GuiDataSource count;
-  GuiDataSource period;
-  GuiDataSource direction;
-  GuiDataSource distance;
-  GuiDataSource rate;
-
- private:
-  static void DistancePerPulseCallbackFunc(const char*, void* param,
-                                           const HAL_Value* value) {
-    if (value->type == HAL_DOUBLE) {
-      auto self = static_cast<EncoderSource*>(param);
-      double distPerPulse = value->data.v_double;
-      self->distancePerPulse.SetValue(distPerPulse);
-      self->distance.SetValue(self->count.GetValue() * distPerPulse);
-      double period = self->period.GetValue();
-      if (period == 0)
-        self->rate.SetValue(std::numeric_limits<double>::infinity());
-      else if (period == std::numeric_limits<double>::infinity())
-        self->rate.SetValue(0);
-      else
-        self->rate.SetValue(static_cast<float>(distPerPulse / period));
-    }
-  }
-
-  static void CountCallbackFunc(const char*, void* param,
-                                const HAL_Value* value) {
-    if (value->type == HAL_INT) {
-      auto self = static_cast<EncoderSource*>(param);
-      double count = value->data.v_int;
-      self->count.SetValue(count);
-      self->distance.SetValue(count * self->distancePerPulse.GetValue());
-    }
-  }
-
-  static void PeriodCallbackFunc(const char*, void* param,
-                                 const HAL_Value* value) {
-    if (value->type == HAL_DOUBLE) {
-      auto self = static_cast<EncoderSource*>(param);
-      double period = value->data.v_double;
-      self->period.SetValue(period);
-      if (period == 0)
-        self->rate.SetValue(std::numeric_limits<double>::infinity());
-      else if (period == std::numeric_limits<double>::infinity())
-        self->rate.SetValue(0);
-      else
-        self->rate.SetValue(
-            static_cast<float>(self->distancePerPulse.GetValue() / period));
-    }
-  }
-
-  static void DirectionCallbackFunc(const char*, void* param,
-                                    const HAL_Value* value) {
-    if (value->type == HAL_BOOLEAN) {
-      static_cast<EncoderSource*>(param)->direction.SetValue(
-          value->data.v_boolean);
-    }
-  }
-
-  int32_t m_index;
-  int m_channelA;
-  int m_channelB;
-  int32_t m_distancePerPulseCallback;
-  int32_t m_countCallback;
-  int32_t m_periodCallback;
-  int32_t m_directionCallback;
-};
-
-}  // namespace
-
-static IniSaver<EncoderInfo> gEncoders{"Encoder"};  // indexed by channel A
-static std::vector<std::unique_ptr<EncoderSource>> gEncoderSources;
-
-static void UpdateEncoderSources() {
-  for (int i = 0, iend = gEncoderSources.size(); i < iend; ++i) {
-    auto& source = gEncoderSources[i];
-    if (HALSIM_GetEncoderInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<EncoderSource>(i);
-        source->SetName(gEncoders[source->GetChannelA()].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayEncoders() {
-  bool hasAny = false;
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
-  for (int i = 0, iend = gEncoderSources.size(); i < iend; ++i) {
-    if (auto source = gEncoderSources[i].get()) {
-      hasAny = true;
-      if (auto simDevice = HALSIM_GetEncoderSimDevice(i)) {
-        ImGui::PushStyleColor(ImGuiCol_Text, IM_COL32(96, 96, 96, 255));
-        ImGui::Text("%s", HALSIM_GetSimDeviceName(simDevice));
-        ImGui::PopStyleColor();
-      } else {
-        int chA = source->GetChannelA();
-        int chB = source->GetChannelB();
-
-        // build header name
-        auto& info = gEncoders[chA];
-        char name[128];
-        info.GetLabel(name, sizeof(name), "Encoder", chA, chB);
-
-        // header
-        bool open = ImGui::CollapsingHeader(
-            name, gEncoders[chA].IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
-        info.SetOpen(open);
-
-        // context menu to change name
-        if (info.PopupEditName(chA)) {
-          source->SetName(info.GetName());
-        }
-
-        if (open) {
-          ImGui::PushID(i);
-          // distance per pulse
-          double distancePerPulse = source->distancePerPulse.GetValue();
-          source->distancePerPulse.LabelText("Dist/Count", "%.6f",
-                                             distancePerPulse);
-
-          // count
-          int count = source->count.GetValue();
-          if (ImGui::InputInt("##input", &count))
-            HALSIM_SetEncoderCount(i, count);
-          ImGui::SameLine();
-          if (ImGui::Button("Reset")) HALSIM_SetEncoderCount(i, 0);
-          ImGui::SameLine();
-          ImGui::Selectable("Count");
-          source->count.EmitDrag();
-
-          // max period
-          double maxPeriod = HALSIM_GetEncoderMaxPeriod(i);
-          ImGui::LabelText("Max Period", "%.6f", maxPeriod);
-
-          // period
-          double period = source->period.GetValue();
-          if (source->period.InputDouble("Period", &period, 0, 0, "%.6g"))
-            HALSIM_SetEncoderPeriod(i, period);
-
-          // reverse direction
-          ImGui::LabelText(
-              "Reverse Direction", "%s",
-              HALSIM_GetEncoderReverseDirection(i) ? "true" : "false");
-
-          // direction
-          static const char* options[] = {"reverse", "forward"};
-          int direction = source->direction.GetValue() ? 1 : 0;
-          if (source->direction.Combo("Direction", &direction, options, 2))
-            HALSIM_SetEncoderDirection(i, direction);
-
-          // distance
-          double distance = source->distance.GetValue();
-          if (source->distance.InputDouble("Distance", &distance, 0, 0, "%.6g"))
-            HALSIM_SetEncoderDistance(i, distance);
-
-          // rate
-          double rate = source->rate.GetValue();
-          if (source->rate.InputDouble("Rate", &rate, 0, 0, "%.6g"))
-            HALSIM_SetEncoderRate(i, rate);
-
-          ImGui::PopID();
-        }
-      }
-    }
-  }
-  ImGui::PopItemWidth();
-  if (!hasAny) ImGui::Text("No encoders");
-}
-
-void EncoderGui::Initialize() {
-  gEncoders.Initialize();
-  gEncoderSources.resize(HAL_GetNumEncoders());
-  HALSimGui::AddExecute(UpdateEncoderSources);
-  HALSimGui::AddWindow("Encoders", DisplayEncoders,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("Encoders", 5, 250);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h
deleted file mode 100644
index b7c9dbf..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderGui.h
+++ /dev/null
@@ -1,17 +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
-
-namespace halsimgui {
-
-class EncoderGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderSimGui.cpp
new file mode 100644
index 0000000..56e8ea2
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderSimGui.cpp
@@ -0,0 +1,264 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "EncoderSimGui.h"
+
+#include <glass/hardware/Encoder.h>
+#include <stdint.h>
+
+#include <limits>
+#include <memory>
+#include <string_view>
+#include <vector>
+
+#include <fmt/format.h>
+#include <hal/Ports.h>
+#include <hal/simulation/EncoderData.h>
+#include <hal/simulation/SimDeviceData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+
+class EncoderSimModel : public glass::EncoderModel {
+ public:
+  EncoderSimModel(std::string_view id, int32_t index, int channelA,
+                  int channelB)
+      : m_distancePerPulse(fmt::format("{} Dist/Count", id)),
+        m_count(fmt::format("{} Count", id)),
+        m_period(fmt::format("{} Period", id)),
+        m_direction(fmt::format("{} Direction", id)),
+        m_distance(fmt::format("{} Distance", id)),
+        m_rate(fmt::format("{} Rate", id)),
+        m_index{index},
+        m_channelA{channelA},
+        m_channelB{channelB},
+        m_distancePerPulseCallback{
+            HALSIM_RegisterEncoderDistancePerPulseCallback(
+                index, DistancePerPulseCallbackFunc, this, true)},
+        m_countCallback{HALSIM_RegisterEncoderCountCallback(
+            index, CountCallbackFunc, this, true)},
+        m_periodCallback{HALSIM_RegisterEncoderPeriodCallback(
+            index, PeriodCallbackFunc, this, true)},
+        m_directionCallback{HALSIM_RegisterEncoderDirectionCallback(
+            index, DirectionCallbackFunc, this, true)} {
+    m_direction.SetDigital(true);
+  }
+
+  EncoderSimModel(int32_t index, int channelA, int channelB)
+      : EncoderSimModel(fmt::format("Encoder[{},{}]", channelA, channelB),
+                        index, channelA, channelB) {}
+
+  explicit EncoderSimModel(int32_t index)
+      : EncoderSimModel(index, HALSIM_GetEncoderDigitalChannelA(index),
+                        HALSIM_GetEncoderDigitalChannelB(index)) {}
+
+  ~EncoderSimModel() override {
+    if (m_distancePerPulseCallback != 0) {
+      HALSIM_CancelEncoderDistancePerPulseCallback(m_index,
+                                                   m_distancePerPulseCallback);
+    }
+    if (m_countCallback != 0) {
+      HALSIM_CancelEncoderCountCallback(m_index, m_countCallback);
+    }
+    if (m_periodCallback != 0) {
+      HALSIM_CancelEncoderCountCallback(m_index, m_periodCallback);
+    }
+    if (m_directionCallback != 0) {
+      HALSIM_CancelEncoderCountCallback(m_index, m_directionCallback);
+    }
+  }
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetEncoderInitialized(m_index); }
+
+  int32_t GetIndex() const { return m_index; }
+
+  const char* GetSimDevice() const override {
+    if (auto simDevice = HALSIM_GetEncoderSimDevice(m_index)) {
+      return HALSIM_GetSimDeviceName(simDevice);
+    } else {
+      return nullptr;
+    }
+  }
+
+  int GetChannelA() const override { return m_channelA; }
+  int GetChannelB() const override { return m_channelB; }
+
+  glass::DataSource* GetDistancePerPulseData() override {
+    return &m_distancePerPulse;
+  }
+  glass::DataSource* GetCountData() override { return &m_count; }
+  glass::DataSource* GetPeriodData() override { return &m_period; }
+  glass::DataSource* GetDirectionData() override { return &m_direction; }
+  glass::DataSource* GetDistanceData() override { return &m_distance; }
+  glass::DataSource* GetRateData() override { return &m_rate; }
+
+  double GetMaxPeriod() override { return HALSIM_GetEncoderMaxPeriod(m_index); }
+  bool GetReverseDirection() override {
+    return HALSIM_GetEncoderReverseDirection(m_index);
+  }
+
+  void SetDistancePerPulse(double val) override {
+    HALSIM_SetEncoderDistancePerPulse(m_index, val);
+  }
+  void SetCount(int val) override { HALSIM_SetEncoderCount(m_index, val); }
+  void SetPeriod(double val) override { HALSIM_SetEncoderPeriod(m_index, val); }
+  void SetDirection(bool val) override {
+    HALSIM_SetEncoderDirection(m_index, val);
+  }
+  void SetDistance(double val) override {
+    HALSIM_SetEncoderDistance(m_index, val);
+  }
+  void SetRate(double val) override { HALSIM_SetEncoderRate(m_index, val); }
+
+  void SetMaxPeriod(double val) override {
+    HALSIM_SetEncoderMaxPeriod(m_index, val);
+  }
+  void SetReverseDirection(bool val) override {
+    HALSIM_SetEncoderReverseDirection(m_index, val);
+  }
+
+ private:
+  static void DistancePerPulseCallbackFunc(const char*, void* param,
+                                           const HAL_Value* value) {
+    if (value->type == HAL_DOUBLE) {
+      auto self = static_cast<EncoderSimModel*>(param);
+      double distPerPulse = value->data.v_double;
+      self->m_distancePerPulse.SetValue(distPerPulse);
+      self->m_distance.SetValue(self->m_count.GetValue() * distPerPulse);
+      double period = self->m_period.GetValue();
+      if (period == 0) {
+        self->m_rate.SetValue(std::numeric_limits<double>::infinity());
+      } else if (period == std::numeric_limits<double>::infinity()) {
+        self->m_rate.SetValue(0);
+      } else {
+        self->m_rate.SetValue(static_cast<float>(distPerPulse / period));
+      }
+    }
+  }
+
+  static void CountCallbackFunc(const char*, void* param,
+                                const HAL_Value* value) {
+    if (value->type == HAL_INT) {
+      auto self = static_cast<EncoderSimModel*>(param);
+      double count = value->data.v_int;
+      self->m_count.SetValue(count);
+      self->m_distance.SetValue(count * self->m_distancePerPulse.GetValue());
+    }
+  }
+
+  static void PeriodCallbackFunc(const char*, void* param,
+                                 const HAL_Value* value) {
+    if (value->type == HAL_DOUBLE) {
+      auto self = static_cast<EncoderSimModel*>(param);
+      double period = value->data.v_double;
+      self->m_period.SetValue(period);
+      if (period == 0) {
+        self->m_rate.SetValue(std::numeric_limits<double>::infinity());
+      } else if (period == std::numeric_limits<double>::infinity()) {
+        self->m_rate.SetValue(0);
+      } else {
+        self->m_rate.SetValue(
+            static_cast<float>(self->m_distancePerPulse.GetValue() / period));
+      }
+    }
+  }
+
+  static void DirectionCallbackFunc(const char*, void* param,
+                                    const HAL_Value* value) {
+    if (value->type == HAL_BOOLEAN) {
+      static_cast<EncoderSimModel*>(param)->m_direction.SetValue(
+          value->data.v_boolean);
+    }
+  }
+
+  glass::DataSource m_distancePerPulse;
+  glass::DataSource m_count;
+  glass::DataSource m_period;
+  glass::DataSource m_direction;
+  glass::DataSource m_distance;
+  glass::DataSource m_rate;
+
+  int32_t m_index;
+  int m_channelA;
+  int m_channelB;
+  int32_t m_distancePerPulseCallback;
+  int32_t m_countCallback;
+  int32_t m_periodCallback;
+  int32_t m_directionCallback;
+};
+
+class EncodersSimModel : public glass::EncodersModel {
+ public:
+  EncodersSimModel() : m_models(HAL_GetNumEncoders()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachEncoder(
+      wpi::function_ref<void(glass::EncoderModel& model, int index)> func)
+      override;
+
+ private:
+  std::vector<std::unique_ptr<EncoderSimModel>> m_models;
+};
+}  // namespace
+
+void EncodersSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetEncoderInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<EncoderSimModel>(i);
+      }
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void EncodersSimModel::ForEachEncoder(
+    wpi::function_ref<void(glass::EncoderModel& model, int index)> func) {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool EncodersAnyInitialized() {
+  static const int32_t num = HAL_GetNumEncoders();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetEncoderInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void EncoderSimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "Encoders", EncodersAnyInitialized,
+      [] { return std::make_unique<EncodersSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(5, 250);
+        return glass::MakeFunctionView(
+            [=] { DisplayEncoders(static_cast<EncodersSimModel*>(model)); });
+      });
+}
+
+glass::EncodersModel& EncoderSimGui::GetEncodersModel() {
+  static auto model = HALSimGui::halProvider.GetModel("Encoders");
+  assert(model);
+  return *static_cast<glass::EncodersModel*>(model);
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderSimGui.h
new file mode 100644
index 0000000..f27d180
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/EncoderSimGui.h
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace glass {
+class EncodersModel;
+}  // namespace glass
+
+namespace halsimgui {
+
+class EncoderSimGui {
+ public:
+  static void Initialize();
+  static glass::EncodersModel& GetEncodersModel();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
deleted file mode 100644
index b5bf92f..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/ExtraGuiWidgets.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "ExtraGuiWidgets.h"
-
-#include "GuiDataSource.h"
-
-namespace halsimgui {
-
-void DrawLEDSources(const int* values, GuiDataSource** sources, int numValues,
-                    int cols, const ImU32* colors, float size, float spacing,
-                    const LEDConfig& config) {
-  if (numValues == 0 || cols < 1) return;
-  if (size == 0) size = ImGui::GetFontSize() / 2.0;
-  if (spacing == 0) spacing = ImGui::GetFontSize() / 3.0;
-
-  int rows = (numValues + cols - 1) / cols;
-  float inc = size + spacing;
-
-  ImDrawList* drawList = ImGui::GetWindowDrawList();
-  const ImVec2 p = ImGui::GetCursorScreenPos();
-
-  float sized2 = size / 2;
-  float ystart, yinc;
-  if (config.start & 1) {
-    // lower
-    ystart = p.y + sized2 + inc * (rows - 1);
-    yinc = -inc;
-  } else {
-    // upper
-    ystart = p.y + sized2;
-    yinc = inc;
-  }
-
-  float xstart, xinc;
-  if (config.start & 2) {
-    // right
-    xstart = p.x + sized2 + inc * (cols - 1);
-    xinc = -inc;
-  } else {
-    // left
-    xstart = p.x + sized2;
-    xinc = inc;
-  }
-
-  float x = xstart, y = ystart;
-  int rowcol = 1;  // row for row-major, column for column-major
-  for (int i = 0; i < numValues; ++i) {
-    if (config.order == LEDConfig::RowMajor) {
-      if (i >= (rowcol * cols)) {
-        ++rowcol;
-        if (config.serpentine) {
-          x -= xinc;
-          xinc = -xinc;
-        } else {
-          x = xstart;
-        }
-        y += yinc;
-      }
-    } else {
-      if (i >= (rowcol * rows)) {
-        ++rowcol;
-        if (config.serpentine) {
-          y -= yinc;
-          yinc = -yinc;
-        } else {
-          y = ystart;
-        }
-        x += xinc;
-      }
-    }
-    if (values[i] > 0)
-      drawList->AddRectFilled(ImVec2(x, y), ImVec2(x + size, y + size),
-                              colors[values[i] - 1]);
-    else if (values[i] < 0)
-      drawList->AddRect(ImVec2(x, y), ImVec2(x + size, y + size),
-                        colors[-values[i] - 1], 0.0f, 0, 1.0);
-    if (sources) {
-      ImGui::SetCursorScreenPos(ImVec2(x - sized2, y - sized2));
-      if (sources[i]) {
-        ImGui::PushID(i);
-        ImGui::Selectable("", false, 0, ImVec2(inc, inc));
-        sources[i]->EmitDrag();
-        ImGui::PopID();
-      } else {
-        ImGui::Dummy(ImVec2(inc, inc));
-      }
-    }
-    if (config.order == LEDConfig::RowMajor) {
-      x += xinc;
-    } else {
-      y += yinc;
-    }
-  }
-
-  if (!sources) ImGui::Dummy(ImVec2(inc * cols, inc * rows));
-}
-
-void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
-              float size, float spacing, const LEDConfig& config) {
-  DrawLEDSources(values, nullptr, numValues, cols, colors, size, spacing,
-                 config);
-}
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Field2D.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Field2D.cpp
deleted file mode 100644
index b1e4f0c..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Field2D.cpp
+++ /dev/null
@@ -1,652 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "Field2D.h"
-
-#include <cmath>
-
-#include <hal/SimDevice.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
-#include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
-#include <imgui_internal.h>
-#include <units/angle.h>
-#include <units/length.h>
-#include <wpi/Path.h>
-#include <wpi/SmallString.h>
-#include <wpi/json.h>
-#include <wpi/raw_istream.h>
-#include <wpi/raw_ostream.h>
-#include <wpigui.h>
-
-#include "HALSimGui.h"
-#include "SimDeviceGui.h"
-#include "portable-file-dialogs.h"
-
-using namespace halsimgui;
-
-namespace gui = wpi::gui;
-
-namespace {
-
-// Per-frame field data (not persistent)
-struct FieldFrameData {
-  // in window coordinates
-  ImVec2 imageMin;
-  ImVec2 imageMax;
-  ImVec2 min;
-  ImVec2 max;
-
-  float scale;  // scaling from field units to screen units
-};
-
-class FieldInfo {
- public:
-  static constexpr float kDefaultWidth = 15.98f;
-  static constexpr float kDefaultHeight = 8.21f;
-
-  std::unique_ptr<pfd::open_file> m_fileOpener;
-  float m_width = kDefaultWidth;
-  float m_height = kDefaultHeight;
-
-  void Reset();
-  void LoadImage();
-  void LoadJson(const wpi::Twine& jsonfile);
-  FieldFrameData GetFrameData() const;
-  void Draw(ImDrawList* drawList, const ImVec2& windowPos,
-            const FieldFrameData& frameData) const;
-
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out) const;
-
- private:
-  bool LoadImageImpl(const char* fn);
-
-  std::string m_filename;
-  gui::Texture m_texture;
-  int m_imageWidth = 0;
-  int m_imageHeight = 0;
-  int m_top = 0;
-  int m_left = 0;
-  int m_bottom = -1;
-  int m_right = -1;
-};
-
-// Per-frame robot data (not persistent)
-struct RobotFrameData {
-  // in window coordinates
-  ImVec2 center;
-  ImVec2 corners[4];
-  ImVec2 arrow[3];
-
-  // scaled width/2 and length/2, in screen units
-  float width2;
-  float length2;
-};
-
-class RobotInfo {
- public:
-  static constexpr float kDefaultWidth = 0.6858f;
-  static constexpr float kDefaultLength = 0.8204f;
-
-  std::unique_ptr<pfd::open_file> m_fileOpener;
-  float m_width = kDefaultWidth;
-  float m_length = kDefaultLength;
-
-  void Reset();
-  void LoadImage();
-  void UpdateFromSimDevice();
-  void SetPosition(double x, double y);
-  // set and get rotation in radians
-  void SetRotation(double rot);
-  double GetRotation() const {
-    return units::convert<units::degrees, units::radians>(m_rot);
-  }
-  RobotFrameData GetFrameData(const FieldFrameData& ffd) const;
-  void Draw(ImDrawList* drawList, const ImVec2& windowPos,
-            const RobotFrameData& frameData, int hit, float hitRadius) const;
-
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out) const;
-
- private:
-  bool LoadImageImpl(const char* fn);
-
-  std::string m_filename;
-  gui::Texture m_texture;
-
-  HAL_SimDeviceHandle m_devHandle = 0;
-  hal::SimDouble m_xHandle;
-  hal::SimDouble m_yHandle;
-  hal::SimDouble m_rotHandle;
-
-  double m_x = 0;
-  double m_y = 0;
-  double m_rot = 0;
-};
-
-}  // namespace
-
-static FieldInfo gField;
-static RobotInfo gRobot;
-static int gDragRobot = 0;
-static ImVec2 gDragInitialOffset;
-static double gDragInitialAngle;
-
-// read/write settings to ini file
-static void* Field2DReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                             const char* name) {
-  if (name == wpi::StringRef{"Field"}) return &gField;
-  if (name == wpi::StringRef{"Robot"}) return &gRobot;
-  return nullptr;
-}
-
-static void Field2DReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                            void* entry, const char* lineStr) {
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (entry == &gField)
-    gField.ReadIni(name, value);
-  else if (entry == &gRobot)
-    gRobot.ReadIni(name, value);
-}
-
-static void Field2DWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                            ImGuiTextBuffer* out_buf) {
-  gField.WriteIni(out_buf);
-  gRobot.WriteIni(out_buf);
-}
-
-void FieldInfo::Reset() {
-  m_texture = gui::Texture{};
-  m_filename.clear();
-  m_imageWidth = 0;
-  m_imageHeight = 0;
-  m_top = 0;
-  m_left = 0;
-  m_bottom = -1;
-  m_right = -1;
-}
-
-void FieldInfo::LoadImage() {
-  if (m_fileOpener && m_fileOpener->ready(0)) {
-    auto result = m_fileOpener->result();
-    if (!result.empty()) {
-      if (wpi::StringRef(result[0]).endswith(".json")) {
-        LoadJson(result[0]);
-      } else {
-        LoadImageImpl(result[0].c_str());
-        m_top = 0;
-        m_left = 0;
-        m_bottom = -1;
-        m_right = -1;
-      }
-    }
-    m_fileOpener.reset();
-  }
-  if (!m_texture && !m_filename.empty()) {
-    if (!LoadImageImpl(m_filename.c_str())) m_filename.clear();
-  }
-}
-
-void FieldInfo::LoadJson(const wpi::Twine& jsonfile) {
-  std::error_code ec;
-  wpi::raw_fd_istream f(jsonfile, ec);
-  if (ec) {
-    wpi::errs() << "GUI: could not open field JSON file\n";
-    return;
-  }
-
-  // parse file
-  wpi::json j;
-  try {
-    j = wpi::json::parse(f);
-  } catch (const wpi::json::parse_error& e) {
-    wpi::errs() << "GUI: JSON: could not parse: " << e.what() << '\n';
-  }
-
-  // top level must be an object
-  if (!j.is_object()) {
-    wpi::errs() << "GUI: JSON: does not contain a top object\n";
-    return;
-  }
-
-  // image filename
-  std::string image;
-  try {
-    image = j.at("field-image").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "GUI: JSON: could not read field-image: " << e.what()
-                << '\n';
-    return;
-  }
-
-  // corners
-  int top, left, bottom, right;
-  try {
-    top = j.at("field-corners").at("top-left").at(1).get<int>();
-    left = j.at("field-corners").at("top-left").at(0).get<int>();
-    bottom = j.at("field-corners").at("bottom-right").at(1).get<int>();
-    right = j.at("field-corners").at("bottom-right").at(0).get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "GUI: JSON: could not read field-corners: " << e.what()
-                << '\n';
-    return;
-  }
-
-  // size
-  float width;
-  float height;
-  try {
-    width = j.at("field-size").at(0).get<float>();
-    height = j.at("field-size").at(1).get<float>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "GUI: JSON: could not read field-size: " << e.what() << '\n';
-    return;
-  }
-
-  // units for size
-  std::string unit;
-  try {
-    unit = j.at("field-unit").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "GUI: JSON: could not read field-unit: " << e.what() << '\n';
-    return;
-  }
-
-  // convert size units to meters
-  if (unit == "foot" || unit == "feet") {
-    width = units::convert<units::feet, units::meters>(width);
-    height = units::convert<units::feet, units::meters>(height);
-  }
-
-  // the image filename is relative to the json file
-  wpi::SmallString<128> pathname;
-  jsonfile.toVector(pathname);
-  wpi::sys::path::remove_filename(pathname);
-  wpi::sys::path::append(pathname, image);
-
-  // load field image
-  if (!LoadImageImpl(pathname.c_str())) return;
-
-  // save to field info
-  m_filename = pathname.str();
-  m_top = top;
-  m_left = left;
-  m_bottom = bottom;
-  m_right = right;
-  m_width = width;
-  m_height = height;
-}
-
-bool FieldInfo::LoadImageImpl(const char* fn) {
-  wpi::outs() << "GUI: loading field image '" << fn << "'\n";
-  auto texture = gui::Texture::CreateFromFile(fn);
-  if (!texture) {
-    wpi::errs() << "GUI: could not read field image\n";
-    return false;
-  }
-  m_texture = std::move(texture);
-  m_imageWidth = m_texture.GetWidth();
-  m_imageHeight = m_texture.GetHeight();
-  m_filename = fn;
-  return true;
-}
-
-FieldFrameData FieldInfo::GetFrameData() const {
-  FieldFrameData ffd;
-
-  // get window content region
-  ffd.imageMin = ImGui::GetWindowContentRegionMin();
-  ffd.imageMax = ImGui::GetWindowContentRegionMax();
-
-  // fit the image into the window
-  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0)
-    gui::MaxFit(&ffd.imageMin, &ffd.imageMax, m_imageWidth, m_imageHeight);
-
-  ImVec2 min = ffd.imageMin;
-  ImVec2 max = ffd.imageMax;
-
-  // size down the box by the image corners (if any)
-  if (m_bottom > 0 && m_right > 0) {
-    min.x += m_left * (max.x - min.x) / m_imageWidth;
-    min.y += m_top * (max.y - min.y) / m_imageHeight;
-    max.x -= (m_imageWidth - m_right) * (max.x - min.x) / m_imageWidth;
-    max.y -= (m_imageHeight - m_bottom) * (max.y - min.y) / m_imageHeight;
-  }
-
-  // draw the field "active area" as a yellow boundary box
-  gui::MaxFit(&min, &max, m_width, m_height);
-
-  ffd.min = min;
-  ffd.max = max;
-  ffd.scale = (max.x - min.x) / m_width;
-  return ffd;
-}
-
-void FieldInfo::Draw(ImDrawList* drawList, const ImVec2& windowPos,
-                     const FieldFrameData& ffd) const {
-  if (m_texture && m_imageHeight != 0 && m_imageWidth != 0) {
-    drawList->AddImage(m_texture, windowPos + ffd.imageMin,
-                       windowPos + ffd.imageMax);
-  }
-
-  // draw the field "active area" as a yellow boundary box
-  drawList->AddRect(windowPos + ffd.min, windowPos + ffd.max,
-                    IM_COL32(255, 255, 0, 255));
-}
-
-bool FieldInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name == "image") {
-    m_filename = value;
-  } else if (name == "top") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_top = num;
-  } else if (name == "left") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_left = num;
-  } else if (name == "bottom") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_bottom = num;
-  } else if (name == "right") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_right = num;
-  } else if (name == "width") {
-    std::sscanf(value.data(), "%f", &m_width);
-  } else if (name == "height") {
-    std::sscanf(value.data(), "%f", &m_height);
-  } else {
-    return false;
-  }
-  return true;
-}
-
-void FieldInfo::WriteIni(ImGuiTextBuffer* out) const {
-  out->appendf(
-      "[Field2D][Field]\nimage=%s\ntop=%d\nleft=%d\nbottom=%d\nright=%d\nwidth="
-      "%f\nheight=%f\n\n",
-      m_filename.c_str(), m_top, m_left, m_bottom, m_right, m_width, m_height);
-}
-
-void RobotInfo::Reset() {
-  m_texture = gui::Texture{};
-  m_filename.clear();
-}
-
-void RobotInfo::LoadImage() {
-  if (m_fileOpener && m_fileOpener->ready(0)) {
-    auto result = m_fileOpener->result();
-    if (!result.empty()) LoadImageImpl(result[0].c_str());
-    m_fileOpener.reset();
-  }
-  if (!m_texture && !m_filename.empty()) {
-    if (!LoadImageImpl(m_filename.c_str())) m_filename.clear();
-  }
-}
-
-bool RobotInfo::LoadImageImpl(const char* fn) {
-  wpi::outs() << "GUI: loading robot image '" << fn << "'\n";
-  auto texture = gui::Texture::CreateFromFile(fn);
-  if (!texture) {
-    wpi::errs() << "GUI: could not read robot image\n";
-    return false;
-  }
-  m_texture = std::move(texture);
-  m_filename = fn;
-  return true;
-}
-
-void RobotInfo::UpdateFromSimDevice() {
-  if (m_devHandle == 0) m_devHandle = HALSIM_GetSimDeviceHandle("Field2D");
-  if (m_devHandle == 0) return;
-
-  if (!m_xHandle) m_xHandle = HALSIM_GetSimValueHandle(m_devHandle, "x");
-  if (m_xHandle) m_x = m_xHandle.Get();
-
-  if (!m_yHandle) m_yHandle = HALSIM_GetSimValueHandle(m_devHandle, "y");
-  if (m_yHandle) m_y = m_yHandle.Get();
-
-  if (!m_rotHandle) m_rotHandle = HALSIM_GetSimValueHandle(m_devHandle, "rot");
-  if (m_rotHandle) m_rot = m_rotHandle.Get();
-}
-
-void RobotInfo::SetPosition(double x, double y) {
-  m_x = x;
-  m_y = y;
-  if (m_xHandle) m_xHandle.Set(x);
-  if (m_yHandle) m_yHandle.Set(y);
-}
-
-void RobotInfo::SetRotation(double rot) {
-  double rotDegrees = units::convert<units::radians, units::degrees>(rot);
-  // force to -180 to +180 range
-  rotDegrees = rotDegrees + std::ceil((-rotDegrees - 180) / 360) * 360;
-  m_rot = rotDegrees;
-  if (m_rotHandle) m_rotHandle.Set(rotDegrees);
-}
-
-RobotFrameData RobotInfo::GetFrameData(const FieldFrameData& ffd) const {
-  RobotFrameData rfd;
-  float width2 = ffd.scale * m_width / 2;
-  float length2 = ffd.scale * m_length / 2;
-
-  // (0,0) origin is bottom left
-  ImVec2 center(ffd.min.x + ffd.scale * m_x, ffd.max.y - ffd.scale * m_y);
-
-  // build rotated points around center
-  double rot = GetRotation();
-  float cos_a = std::cos(-rot);
-  float sin_a = std::sin(-rot);
-
-  rfd.corners[0] = center + ImRotate(ImVec2(-length2, -width2), cos_a, sin_a);
-  rfd.corners[1] = center + ImRotate(ImVec2(length2, -width2), cos_a, sin_a);
-  rfd.corners[2] = center + ImRotate(ImVec2(length2, width2), cos_a, sin_a);
-  rfd.corners[3] = center + ImRotate(ImVec2(-length2, width2), cos_a, sin_a);
-  rfd.arrow[0] =
-      center + ImRotate(ImVec2(-length2 / 2, -width2 / 2), cos_a, sin_a);
-  rfd.arrow[1] = center + ImRotate(ImVec2(length2 / 2, 0), cos_a, sin_a);
-  rfd.arrow[2] =
-      center + ImRotate(ImVec2(-length2 / 2, width2 / 2), cos_a, sin_a);
-
-  rfd.center = center;
-  rfd.width2 = width2;
-  rfd.length2 = length2;
-  return rfd;
-}
-
-void RobotInfo::Draw(ImDrawList* drawList, const ImVec2& windowPos,
-                     const RobotFrameData& rfd, int hit,
-                     float hitRadius) const {
-  if (m_texture) {
-    drawList->AddImageQuad(
-        m_texture, windowPos + rfd.corners[0], windowPos + rfd.corners[1],
-        windowPos + rfd.corners[2], windowPos + rfd.corners[3]);
-  } else {
-    drawList->AddQuad(windowPos + rfd.corners[0], windowPos + rfd.corners[1],
-                      windowPos + rfd.corners[2], windowPos + rfd.corners[3],
-                      IM_COL32(255, 0, 0, 255), 4.0);
-    drawList->AddTriangle(windowPos + rfd.arrow[0], windowPos + rfd.arrow[1],
-                          windowPos + rfd.arrow[2], IM_COL32(0, 255, 0, 255),
-                          4.0);
-  }
-
-  if (hit > 0) {
-    if (hit == 1) {
-      drawList->AddCircle(windowPos + rfd.center, hitRadius,
-                          IM_COL32(0, 255, 0, 255));
-    } else {
-      drawList->AddCircle(windowPos + rfd.corners[hit - 2], hitRadius,
-                          IM_COL32(0, 255, 0, 255));
-    }
-  }
-}
-
-bool RobotInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name == "image") {
-    m_filename = value;
-  } else if (name == "width") {
-    std::sscanf(value.data(), "%f", &m_width);
-  } else if (name == "length") {
-    std::sscanf(value.data(), "%f", &m_length);
-  } else {
-    return false;
-  }
-  return true;
-}
-
-void RobotInfo::WriteIni(ImGuiTextBuffer* out) const {
-  out->appendf("[Field2D][Robot]\nimage=%s\nwidth=%f\nlength=%f\n\n",
-               m_filename.c_str(), m_width, m_length);
-}
-
-static void OptionMenuField2D() {
-  if (ImGui::BeginMenu("2D Field View")) {
-    if (ImGui::MenuItem("Choose field image...")) {
-      gField.m_fileOpener = std::make_unique<pfd::open_file>(
-          "Choose field image", "",
-          std::vector<std::string>{"Image File",
-                                   "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
-                                   "*.hdr *.pic *.ppm *.pgm",
-                                   "PathWeaver JSON File", "*.json"});
-    }
-    if (ImGui::MenuItem("Reset field image")) {
-      gField.Reset();
-    }
-    if (ImGui::MenuItem("Choose robot image...")) {
-      gRobot.m_fileOpener = std::make_unique<pfd::open_file>(
-          "Choose robot image", "",
-          std::vector<std::string>{"Image File",
-                                   "*.jpg *.jpeg *.png *.bmp *.psd *.tga *.gif "
-                                   "*.hdr *.pic *.ppm *.pgm"});
-    }
-    if (ImGui::MenuItem("Reset robot image")) {
-      gRobot.Reset();
-    }
-    ImGui::EndMenu();
-  }
-}
-
-static void DisplayField2DSettings() {
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
-  ImGui::InputFloat("Field Width", &gField.m_width);
-  ImGui::InputFloat("Field Height", &gField.m_height);
-  // ImGui::InputInt("Field Top", &gField.m_top);
-  // ImGui::InputInt("Field Left", &gField.m_left);
-  // ImGui::InputInt("Field Right", &gField.m_right);
-  // ImGui::InputInt("Field Bottom", &gField.m_bottom);
-  ImGui::InputFloat("Robot Width", &gRobot.m_width);
-  ImGui::InputFloat("Robot Length", &gRobot.m_length);
-  ImGui::PopItemWidth();
-}
-
-static void DisplayField2D() {
-  // load images
-  gField.LoadImage();
-  gRobot.LoadImage();
-
-  // get robot coordinates from SimDevice
-  gRobot.UpdateFromSimDevice();
-
-  FieldFrameData ffd = gField.GetFrameData();
-  RobotFrameData rfd = gRobot.GetFrameData(ffd);
-
-  ImVec2 windowPos = ImGui::GetWindowPos();
-
-  // for dragging to work, there needs to be a button (otherwise the window is
-  // dragged)
-  ImVec2 contentSize =
-      ImGui::GetWindowContentRegionMax() - ImGui::GetWindowContentRegionMin();
-  if (contentSize.x <= 0 || contentSize.y <= 0) return;
-  ImGui::InvisibleButton("field", contentSize);
-
-  // allow dragging the robot around
-  ImVec2 cursor = ImGui::GetIO().MousePos - windowPos;
-
-  int hit = 0;
-  float hitRadius = (std::min)(rfd.width2, rfd.length2) / 2;
-  // only allow initiation of dragging when invisible button is hovered; this
-  // prevents the window resize handles from simultaneously activating the drag
-  // functionality
-  if (ImGui::IsItemHovered()) {
-    float hitRadiusSquared = hitRadius * hitRadius;
-    // it's within the hit radius of the center?
-    if (gui::GetDistSquared(cursor, rfd.center) < hitRadiusSquared)
-      hit = 1;
-    else if (gui::GetDistSquared(cursor, rfd.corners[0]) < hitRadiusSquared)
-      hit = 2;
-    else if (gui::GetDistSquared(cursor, rfd.corners[1]) < hitRadiusSquared)
-      hit = 3;
-    else if (gui::GetDistSquared(cursor, rfd.corners[2]) < hitRadiusSquared)
-      hit = 4;
-    else if (gui::GetDistSquared(cursor, rfd.corners[3]) < hitRadiusSquared)
-      hit = 5;
-    if (hit > 0 && ImGui::IsMouseClicked(0)) {
-      if (hit == 1) {
-        gDragRobot = hit;
-        gDragInitialOffset = cursor - rfd.center;
-      } else {
-        gDragRobot = hit;
-        ImVec2 off = cursor - rfd.center;
-        gDragInitialAngle = std::atan2(off.y, off.x) + gRobot.GetRotation();
-      }
-    }
-  }
-
-  if (gDragRobot > 0 && ImGui::IsMouseDown(0)) {
-    if (gDragRobot == 1) {
-      ImVec2 newPos = cursor - gDragInitialOffset;
-      gRobot.SetPosition(
-          (std::clamp(newPos.x, ffd.min.x, ffd.max.x) - ffd.min.x) / ffd.scale,
-          (ffd.max.y - std::clamp(newPos.y, ffd.min.y, ffd.max.y)) / ffd.scale);
-      rfd = gRobot.GetFrameData(ffd);
-    } else {
-      ImVec2 off = cursor - rfd.center;
-      gRobot.SetRotation(gDragInitialAngle - std::atan2(off.y, off.x));
-    }
-    hit = gDragRobot;  // keep it highlighted
-  } else {
-    gDragRobot = 0;
-  }
-
-  // draw
-  auto drawList = ImGui::GetWindowDrawList();
-  gField.Draw(drawList, windowPos, ffd);
-  gRobot.Draw(drawList, windowPos, rfd, hit, hitRadius);
-}
-
-void Field2D::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Field2D";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = Field2DReadOpen;
-  iniHandler.ReadLineFn = Field2DReadLine;
-  iniHandler.WriteAllFn = Field2DWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
-  HALSimGui::AddOptionMenu(OptionMenuField2D);
-
-  HALSimGui::AddWindow("2D Field Settings", DisplayField2DSettings,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetWindowVisibility("2D Field Settings", HALSimGui::kHide);
-  HALSimGui::SetDefaultWindowPos("2D Field Settings", 200, 150);
-
-  HALSimGui::AddWindow("2D Field View", DisplayField2D);
-  HALSimGui::SetWindowVisibility("2D Field View", HALSimGui::kHide);
-  HALSimGui::SetDefaultWindowPos("2D Field View", 200, 200);
-  HALSimGui::SetDefaultWindowSize("2D Field View", 400, 200);
-  HALSimGui::SetWindowPadding("2D Field View", 0, 0);
-
-  // SimDeviceGui::Hide("Field2D");
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Field2D.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Field2D.h
deleted file mode 100644
index 52218f3..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Field2D.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-class Field2D {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/GuiDataSource.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/GuiDataSource.cpp
deleted file mode 100644
index df967c1..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/GuiDataSource.cpp
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "GuiDataSource.h"
-
-#include <wpi/StringMap.h>
-
-using namespace halsimgui;
-
-wpi::sig::Signal<const char*, GuiDataSource*> GuiDataSource::sourceCreated;
-
-static wpi::StringMap<GuiDataSource*> gSources;
-
-GuiDataSource::GuiDataSource(const wpi::Twine& id) : m_id{id.str()} {
-  gSources.try_emplace(m_id, this);
-  sourceCreated(m_id.c_str(), this);
-}
-
-GuiDataSource::GuiDataSource(const wpi::Twine& id, int index)
-    : GuiDataSource{id + wpi::Twine('[') + wpi::Twine(index) +
-                    wpi::Twine(']')} {}
-
-GuiDataSource::GuiDataSource(const wpi::Twine& id, int index, int index2)
-    : GuiDataSource{id + wpi::Twine('[') + wpi::Twine(index) + wpi::Twine(',') +
-                    wpi::Twine(index2) + wpi::Twine(']')} {}
-
-GuiDataSource::~GuiDataSource() {
-  auto it = gSources.find(m_id);
-  if (it == gSources.end()) return;
-  if (it->getValue() == this) gSources.erase(it);
-}
-
-void GuiDataSource::LabelText(const char* label, const char* fmt, ...) const {
-  va_list args;
-  va_start(args, fmt);
-  LabelTextV(label, fmt, args);
-  va_end(args);
-}
-
-// Add a label+text combo aligned to other label+value widgets
-void GuiDataSource::LabelTextV(const char* label, const char* fmt,
-                               va_list args) const {
-  ImGui::PushID(label);
-  ImGui::LabelTextV("##input", fmt, args);
-  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
-  ImGui::Selectable(label);
-  ImGui::PopID();
-  EmitDrag();
-}
-
-bool GuiDataSource::Combo(const char* label, int* current_item,
-                          const char* const items[], int items_count,
-                          int popup_max_height_in_items) const {
-  ImGui::PushID(label);
-  bool rv = ImGui::Combo("##input", current_item, items, items_count,
-                         popup_max_height_in_items);
-  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
-  ImGui::Selectable(label);
-  EmitDrag();
-  ImGui::PopID();
-  return rv;
-}
-
-bool GuiDataSource::SliderFloat(const char* label, float* v, float v_min,
-                                float v_max, const char* format,
-                                float power) const {
-  ImGui::PushID(label);
-  bool rv = ImGui::SliderFloat("##input", v, v_min, v_max, format, power);
-  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
-  ImGui::Selectable(label);
-  EmitDrag();
-  ImGui::PopID();
-  return rv;
-}
-
-bool GuiDataSource::InputDouble(const char* label, double* v, double step,
-                                double step_fast, const char* format,
-                                ImGuiInputTextFlags flags) const {
-  ImGui::PushID(label);
-  bool rv = ImGui::InputDouble("##input", v, step, step_fast, format, flags);
-  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
-  ImGui::Selectable(label);
-  EmitDrag();
-  ImGui::PopID();
-  return rv;
-}
-
-bool GuiDataSource::InputInt(const char* label, int* v, int step, int step_fast,
-                             ImGuiInputTextFlags flags) const {
-  ImGui::PushID(label);
-  bool rv = ImGui::InputInt("##input", v, step, step_fast, flags);
-  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
-  ImGui::Selectable(label);
-  EmitDrag();
-  ImGui::PopID();
-  return rv;
-}
-
-void GuiDataSource::EmitDrag(ImGuiDragDropFlags flags) const {
-  if (ImGui::BeginDragDropSource(flags)) {
-    auto self = this;
-    ImGui::SetDragDropPayload("DataSource", &self, sizeof(self));
-    ImGui::TextUnformatted(m_name.empty() ? m_id.c_str() : m_name.c_str());
-    ImGui::EndDragDropSource();
-  }
-}
-
-GuiDataSource* GuiDataSource::Find(wpi::StringRef id) {
-  auto it = gSources.find(id);
-  if (it == gSources.end()) return nullptr;
-  return it->getValue();
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALProvider.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALProvider.cpp
new file mode 100644
index 0000000..72b9c29
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALProvider.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "HALProvider.h"
+
+#include <glass/Model.h>
+
+#include <algorithm>
+#include <string>
+
+#include <hal/simulation/DriverStationData.h>
+
+using namespace halsimgui;
+
+static bool gDisableOutputsOnDSDisable = true;
+
+bool HALProvider::AreOutputsDisabled() {
+  return gDisableOutputsOnDSDisable && !HALSIM_GetDriverStationEnabled();
+}
+
+void HALProvider::DisplayMenu() {
+  ImGui::MenuItem("Disable outputs on DS disable", nullptr,
+                  &gDisableOutputsOnDSDisable, true);
+  ImGui::Separator();
+
+  for (auto&& viewEntry : m_viewEntries) {
+    bool visible = viewEntry->window && viewEntry->window->IsVisible();
+    bool wasVisible = visible;
+    bool exists = viewEntry->modelEntry->exists();
+    ImGui::MenuItem(viewEntry->name.c_str(), nullptr, &visible,
+                    visible || exists);
+    if (!wasVisible && visible) {
+      Show(viewEntry.get(), viewEntry->window);
+    } else if (wasVisible && !visible && viewEntry->window) {
+      viewEntry->window->SetVisible(false);
+    }
+  }
+}
+
+void HALProvider::Update() {
+  Provider::Update();
+
+  // check for visible windows that need displays (typically this is due to
+  // file loading)
+  for (auto&& window : m_windows) {
+    if (!window->IsVisible() || window->HasView()) {
+      continue;
+    }
+    auto id = window->GetId();
+    auto it = FindViewEntry(id);
+    if (it == m_viewEntries.end() || (*it)->name != id) {
+      continue;
+    }
+    Show(it->get(), window.get());
+  }
+}
+
+glass::Model* HALProvider::GetModel(std::string_view name) {
+  auto it = FindModelEntry(name);
+  if (it == m_modelEntries.end() || (*it)->name != name) {
+    return nullptr;
+  }
+  auto entry = it->get();
+
+  // get or create model
+  if (!entry->model) {
+    entry->model = entry->createModel();
+  }
+  return entry->model.get();
+}
+
+void HALProvider::Show(ViewEntry* entry, glass::Window* window) {
+  // if there's already a window, just show it
+  if (entry->window) {
+    entry->window->SetVisible(true);
+    return;
+  }
+
+  // get or create model
+  if (!entry->modelEntry->model) {
+    entry->modelEntry->model = entry->modelEntry->createModel();
+  }
+  if (!entry->modelEntry->model) {
+    return;
+  }
+
+  // the window might exist and we're just not associated to it yet
+  if (!window) {
+    window = GetOrAddWindow(entry->name, true);
+  }
+  if (!window) {
+    return;
+  }
+  entry->window = window;
+
+  // create view
+  auto view = entry->createView(window, entry->modelEntry->model.get());
+  if (!view) {
+    return;
+  }
+  window->SetView(std::move(view));
+
+  entry->window->SetVisible(true);
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
index 9803494..8d66ac7 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/HALSimGui.cpp
@@ -1,343 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimGui.h"
 
-#include <algorithm>
-
-#include <hal/simulation/DriverStationData.h>
 #include <imgui.h>
-#include <imgui_internal.h>
-#include <wpi/StringMap.h>
-#include <wpi/raw_ostream.h>
 #include <wpigui.h>
 
 using namespace halsimgui;
 
-namespace gui = wpi::gui;
+glass::MainMenuBar HALSimGui::mainMenu;
+glass::WindowManager HALSimGui::manager{"SimWindow"};
+HALProvider HALSimGui::halProvider{"HALProvider"};
+glass::NetworkTablesProvider HALSimGui::ntProvider{"NTProvider"};
 
-namespace {
-struct WindowInfo {
-  WindowInfo() = default;
-  explicit WindowInfo(const char* name_) : name{name_} {}
-  WindowInfo(const char* name_, std::function<void()> display_,
-             ImGuiWindowFlags flags_)
-      : name{name_}, display{std::move(display_)}, flags{flags_} {}
+void HALSimGui::GlobalInit() {
+  manager.GlobalInit();
+  halProvider.GlobalInit();
+  ntProvider.GlobalInit();
 
-  std::string name;
-  std::function<void()> display;
-  ImGuiWindowFlags flags = 0;
-  bool visible = true;
-  bool enabled = true;
-  ImGuiCond posCond = 0;
-  ImGuiCond sizeCond = 0;
-  ImVec2 pos;
-  ImVec2 size;
-  bool setPadding = false;
-  ImVec2 padding;
-};
-}  // namespace
+  wpi::gui::AddLateExecute([] { mainMenu.Display(); });
 
-static std::vector<WindowInfo> gWindows;
-static wpi::StringMap<int> gWindowMap;   // index into gWindows
-static std::vector<int> gSortedWindows;  // index into gWindows
-static std::vector<std::function<void()>> gOptionMenus;
-static std::vector<std::function<void()>> gMenus;
-static bool gDisableOutputsOnDSDisable = true;
-
-// read/write open state to ini file
-static void* SimWindowsReadOpen(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler,
-                                const char* name) {
-  if (wpi::StringRef{name} == "GLOBAL") return &gDisableOutputsOnDSDisable;
-
-  int index = gWindowMap.try_emplace(name, gWindows.size()).first->second;
-  if (index == static_cast<int>(gWindows.size())) {
-    gSortedWindows.push_back(index);
-    gWindows.emplace_back(name);
-    std::sort(gSortedWindows.begin(), gSortedWindows.end(),
-              [](int a, int b) { return gWindows[a].name < gWindows[b].name; });
-  }
-  return &gWindows[index];
+  glass::AddStandardNetworkTablesViews(ntProvider);
 }
-
-static void SimWindowsReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                               void* entry, const char* lineStr) {
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-
-  if (entry == &gDisableOutputsOnDSDisable) {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    if (name == "disableOutputsOnDS") {
-      gDisableOutputsOnDSDisable = num;
-    }
-    return;
-  }
-
-  auto element = static_cast<WindowInfo*>(entry);
-  if (name == "visible") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    element->visible = num;
-  } else if (name == "enabled") {
-    int num;
-    if (value.getAsInteger(10, num)) return;
-    element->enabled = num;
-  }
-}
-
-static void SimWindowsWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                               ImGuiTextBuffer* out_buf) {
-  out_buf->appendf("[SimWindow][GLOBAL]\ndisableOutputsOnDS=%d\n\n",
-                   gDisableOutputsOnDSDisable ? 1 : 0);
-  for (auto&& window : gWindows)
-    out_buf->appendf("[SimWindow][%s]\nvisible=%d\nenabled=%d\n\n",
-                     window.name.c_str(), window.visible ? 1 : 0,
-                     window.enabled ? 1 : 0);
-}
-
-void HALSimGui::Add(std::function<void()> initialize) {
-  gui::AddInit(std::move(initialize));
-}
-
-void HALSimGui::AddExecute(std::function<void()> execute) {
-  gui::AddEarlyExecute(std::move(execute));
-}
-
-void HALSimGui::AddWindow(const char* name, std::function<void()> display,
-                          int flags) {
-  if (display) {
-    int index = gWindowMap.try_emplace(name, gWindows.size()).first->second;
-    if (index < static_cast<int>(gWindows.size())) {
-      if (gWindows[index].display) {
-        wpi::errs() << "halsim_gui: ignoring duplicate window '" << name
-                    << "'\n";
-      } else {
-        gWindows[index].display = display;
-        gWindows[index].flags = flags;
-      }
-      return;
-    }
-    gSortedWindows.push_back(index);
-    gWindows.emplace_back(name, std::move(display),
-                          static_cast<ImGuiWindowFlags>(flags));
-    std::sort(gSortedWindows.begin(), gSortedWindows.end(),
-              [](int a, int b) { return gWindows[a].name < gWindows[b].name; });
-  }
-}
-
-void HALSimGui::AddMainMenu(std::function<void()> menu) {
-  if (menu) gMenus.emplace_back(std::move(menu));
-}
-
-void HALSimGui::AddOptionMenu(std::function<void()> menu) {
-  if (menu) gOptionMenus.emplace_back(std::move(menu));
-}
-
-void HALSimGui::SetWindowVisibility(const char* name,
-                                    WindowVisibility visibility) {
-  auto it = gWindowMap.find(name);
-  if (it == gWindowMap.end()) return;
-  auto& window = gWindows[it->second];
-  switch (visibility) {
-    case kHide:
-      window.visible = false;
-      window.enabled = true;
-      break;
-    case kShow:
-      window.visible = true;
-      window.enabled = true;
-      break;
-    case kDisabled:
-      window.enabled = false;
-      break;
-  }
-}
-
-void HALSimGui::SetDefaultWindowPos(const char* name, float x, float y) {
-  auto it = gWindowMap.find(name);
-  if (it == gWindowMap.end()) return;
-  auto& window = gWindows[it->second];
-  window.posCond = ImGuiCond_FirstUseEver;
-  window.pos = ImVec2{x, y};
-}
-
-void HALSimGui::SetDefaultWindowSize(const char* name, float width,
-                                     float height) {
-  auto it = gWindowMap.find(name);
-  if (it == gWindowMap.end()) return;
-  auto& window = gWindows[it->second];
-  window.sizeCond = ImGuiCond_FirstUseEver;
-  window.size = ImVec2{width, height};
-}
-
-void HALSimGui::SetWindowPadding(const char* name, float x, float y) {
-  auto it = gWindowMap.find(name);
-  if (it == gWindowMap.end()) return;
-  auto& window = gWindows[it->second];
-  window.setPadding = true;
-  window.padding = ImVec2{x, y};
-}
-
-bool HALSimGui::AreOutputsDisabled() {
-  return gDisableOutputsOnDSDisable && !HALSIM_GetDriverStationEnabled();
-}
-
-void HALSimGui::GlobalInit() { gui::CreateContext(); }
-
-bool HALSimGui::Initialize() {
-  gui::AddInit([] {
-    // Hook ini handler to save settings
-    ImGuiSettingsHandler iniHandler;
-    iniHandler.TypeName = "SimWindow";
-    iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-    iniHandler.ReadOpenFn = SimWindowsReadOpen;
-    iniHandler.ReadLineFn = SimWindowsReadLine;
-    iniHandler.WriteAllFn = SimWindowsWriteAll;
-    ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-  });
-
-  gui::AddWindowScaler([](float windowScale) {
-    // scale default window positions
-    for (auto&& window : gWindows) {
-      if ((window.posCond & ImGuiCond_FirstUseEver) != 0) {
-        window.pos.x *= windowScale;
-        window.pos.y *= windowScale;
-        window.size.x *= windowScale;
-        window.size.y *= windowScale;
-      }
-    }
-  });
-
-  gui::AddLateExecute([] {
-    {
-      ImGui::BeginMainMenuBar();
-
-      if (ImGui::BeginMenu("Options")) {
-        ImGui::MenuItem("Disable outputs on DS disable", nullptr,
-                        &gDisableOutputsOnDSDisable, true);
-        for (auto&& menu : gOptionMenus) {
-          if (menu) menu();
-        }
-        ImGui::EndMenu();
-      }
-
-      gui::EmitViewMenu();
-
-      if (ImGui::BeginMenu("Window")) {
-        for (auto&& windowIndex : gSortedWindows) {
-          auto& window = gWindows[windowIndex];
-          ImGui::MenuItem(window.name.c_str(), nullptr, &window.visible,
-                          window.enabled);
-        }
-        ImGui::EndMenu();
-      }
-
-      for (auto&& menu : gMenus) {
-        if (menu) menu();
-      }
-
-#if 0
-      char str[64];
-      std::snprintf(str, sizeof(str), "%.3f ms/frame (%.1f FPS)",
-                    1000.0f / ImGui::GetIO().Framerate,
-                    ImGui::GetIO().Framerate);
-      ImGui::SameLine(ImGui::GetWindowWidth() - ImGui::CalcTextSize(str).x -
-                      10);
-      ImGui::Text("%s", str);
-#endif
-      ImGui::EndMainMenuBar();
-    }
-
-    for (auto&& window : gWindows) {
-      if (window.display && window.visible && window.enabled) {
-        if (window.posCond != 0)
-          ImGui::SetNextWindowPos(window.pos, window.posCond);
-        if (window.sizeCond != 0)
-          ImGui::SetNextWindowSize(window.size, window.sizeCond);
-        if (window.setPadding)
-          ImGui::PushStyleVar(ImGuiStyleVar_WindowPadding, window.padding);
-        if (ImGui::Begin(window.name.c_str(), &window.visible, window.flags))
-          window.display();
-        ImGui::End();
-        if (window.setPadding) ImGui::PopStyleVar();
-      }
-    }
-  });
-
-  if (!gui::Initialize("", 1280, 720)) return false;
-
-  return true;
-}
-
-void HALSimGui::Main(void*) {
-  gui::Main();
-  gui::DestroyContext();
-}
-
-void HALSimGui::Exit(void*) { gui::Exit(); }
-
-extern "C" {
-
-void HALSIMGUI_Add(void* param, void (*initialize)(void*)) {
-  if (initialize) {
-    HALSimGui::Add([=] { initialize(param); });
-  }
-}
-
-void HALSIMGUI_AddExecute(void* param, void (*execute)(void*)) {
-  if (execute) {
-    HALSimGui::AddExecute([=] { execute(param); });
-  }
-}
-
-void HALSIMGUI_AddWindow(const char* name, void* param, void (*display)(void*),
-                         int32_t flags) {
-  if (display) {
-    HALSimGui::AddWindow(
-        name, [=] { display(param); }, flags);
-  }
-}
-
-void HALSIMGUI_AddMainMenu(void* param, void (*menu)(void*)) {
-  if (menu) {
-    HALSimGui::AddMainMenu([=] { menu(param); });
-  }
-}
-
-void HALSIMGUI_AddOptionMenu(void* param, void (*menu)(void*)) {
-  if (menu) {
-    HALSimGui::AddOptionMenu([=] { menu(param); });
-  }
-}
-
-void HALSIMGUI_SetWindowVisibility(const char* name, int32_t visibility) {
-  HALSimGui::SetWindowVisibility(
-      name, static_cast<HALSimGui::WindowVisibility>(visibility));
-}
-
-void HALSIMGUI_SetDefaultWindowPos(const char* name, float x, float y) {
-  HALSimGui::SetDefaultWindowPos(name, x, y);
-}
-
-void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
-                                    float height) {
-  HALSimGui::SetDefaultWindowSize(name, width, height);
-}
-
-void HALSIMGUI_SetWindowPadding(const char* name, float x, float y) {
-  HALSimGui::SetDefaultWindowSize(name, x, y);
-}
-
-int HALSIMGUI_AreOutputsDisabled(void) {
-  return HALSimGui::AreOutputsDisabled();
-}
-
-}  // extern "C"
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/IniSaverInfo.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/IniSaverInfo.cpp
deleted file mode 100644
index 6d01cd3..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/IniSaverInfo.cpp
+++ /dev/null
@@ -1,141 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "IniSaverInfo.h"
-
-#include <cstdio>
-#include <cstring>
-
-#include <imgui_internal.h>
-
-using namespace halsimgui;
-
-void NameInfo::GetName(char* buf, size_t size, const char* defaultName) const {
-  if (m_name[0] != '\0') {
-    std::snprintf(buf, size, "%s", m_name);
-  } else {
-    std::snprintf(buf, size, "%s", defaultName);
-  }
-}
-
-void NameInfo::GetName(char* buf, size_t size, const char* defaultName,
-                       int index) const {
-  if (m_name[0] != '\0') {
-    std::snprintf(buf, size, "%s [%d]", m_name, index);
-  } else {
-    std::snprintf(buf, size, "%s[%d]", defaultName, index);
-  }
-}
-
-void NameInfo::GetName(char* buf, size_t size, const char* defaultName,
-                       int index, int index2) const {
-  if (m_name[0] != '\0') {
-    std::snprintf(buf, size, "%s [%d,%d]", m_name, index, index2);
-  } else {
-    std::snprintf(buf, size, "%s[%d,%d]", defaultName, index, index2);
-  }
-}
-
-void NameInfo::GetLabel(char* buf, size_t size, const char* defaultName) const {
-  if (m_name[0] != '\0') {
-    std::snprintf(buf, size, "%s###Name%s", m_name, defaultName);
-  } else {
-    std::snprintf(buf, size, "%s###Name%s", defaultName, defaultName);
-  }
-}
-
-void NameInfo::GetLabel(char* buf, size_t size, const char* defaultName,
-                        int index) const {
-  if (m_name[0] != '\0') {
-    std::snprintf(buf, size, "%s [%d]###Name%d", m_name, index, index);
-  } else {
-    std::snprintf(buf, size, "%s[%d]###Name%d", defaultName, index, index);
-  }
-}
-
-void NameInfo::GetLabel(char* buf, size_t size, const char* defaultName,
-                        int index, int index2) const {
-  if (m_name[0] != '\0') {
-    std::snprintf(buf, size, "%s [%d,%d]###Name%d", m_name, index, index2,
-                  index);
-  } else {
-    std::snprintf(buf, size, "%s[%d,%d]###Name%d", defaultName, index, index2,
-                  index);
-  }
-}
-
-bool NameInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name != "name") return false;
-  size_t len = (std::min)(value.size(), sizeof(m_name) - 1);
-  std::memcpy(m_name, value.data(), len);
-  m_name[len] = '\0';
-  return true;
-}
-
-void NameInfo::WriteIni(ImGuiTextBuffer* out) {
-  out->appendf("name=%s\n", m_name);
-}
-
-void NameInfo::PushEditNameId(int index) {
-  char id[64];
-  std::snprintf(id, sizeof(id), "Name%d", index);
-  ImGui::PushID(id);
-}
-
-void NameInfo::PushEditNameId(const char* name) {
-  char id[128];
-  std::snprintf(id, sizeof(id), "Name%s", name);
-  ImGui::PushID(id);
-}
-
-bool NameInfo::PopupEditName(int index) {
-  bool rv = false;
-  char id[64];
-  std::snprintf(id, sizeof(id), "Name%d", index);
-  if (ImGui::BeginPopupContextItem(id)) {
-    ImGui::Text("Edit name:");
-    if (InputTextName("##edit", ImGuiInputTextFlags_EnterReturnsTrue)) {
-      ImGui::CloseCurrentPopup();
-      rv = true;
-    }
-    if (ImGui::Button("Close")) ImGui::CloseCurrentPopup();
-    ImGui::EndPopup();
-  }
-  return rv;
-}
-
-bool NameInfo::PopupEditName(const char* name) {
-  bool rv = false;
-  char id[128];
-  std::snprintf(id, sizeof(id), "Name%s", name);
-  if (ImGui::BeginPopupContextItem(id)) {
-    ImGui::Text("Edit name:");
-    if (InputTextName("##edit", ImGuiInputTextFlags_EnterReturnsTrue)) {
-      ImGui::CloseCurrentPopup();
-      rv = true;
-    }
-    if (ImGui::Button("Close")) ImGui::CloseCurrentPopup();
-    ImGui::EndPopup();
-  }
-  return rv;
-}
-
-bool NameInfo::InputTextName(const char* label_id, ImGuiInputTextFlags flags) {
-  return ImGui::InputText(label_id, m_name, sizeof(m_name), flags);
-}
-
-bool OpenInfo::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name != "open") return false;
-  int num;
-  if (value.getAsInteger(10, num)) return true;
-  m_open = num;
-  return true;
-}
-
-void OpenInfo::WriteIni(ImGuiTextBuffer* out) {
-  out->appendf("open=%d\n", m_open ? 1 : 0);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp
deleted file mode 100644
index 8203325..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.cpp
+++ /dev/null
@@ -1,322 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "Mechanism2D.h"
-
-#include <cmath>
-#include <string>
-
-#include <hal/SimDevice.h>
-#include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
-#include <imgui_internal.h>
-#include <wpi/json.h>
-#include <wpi/math>
-#include <wpi/raw_istream.h>
-#include <wpi/raw_ostream.h>
-
-#include "HALSimGui.h"
-#include "portable-file-dialogs.h"
-
-using namespace halsimgui;
-
-static HAL_SimDeviceHandle devHandle = 0;
-static wpi::StringMap<ImColor> colorLookUpTable;
-static std::unique_ptr<pfd::open_file> m_fileOpener;
-static std::string previousJsonLocation = "Not empty";
-namespace {
-struct BodyConfig {
-  std::string name;
-  std::string type = "line";
-  int length = 100;
-  std::string color = "green";
-  int angle = 0;
-  std::vector<BodyConfig> children;
-  int lineWidth = 1;
-};
-}  // namespace
-static std::vector<BodyConfig> bodyConfigVector;
-namespace {
-struct DrawLineStruct {
-  float xEnd;
-  float yEnd;
-  float angle;
-};
-}  // namespace
-static struct NamedColor {
-  const char* name;
-  ImColor value;
-} staticColors[] = {{"white", IM_COL32(255, 255, 255, 255)},
-                    {"silver", IM_COL32(192, 192, 192, 255)},
-                    {"gray", IM_COL32(128, 128, 128, 255)},
-                    {"black", IM_COL32(0, 0, 0, 255)},
-                    {"red", IM_COL32(255, 0, 0, 255)},
-                    {"maroon", IM_COL32(128, 0, 0, 255)},
-                    {"yellow", IM_COL32(255, 255, 0, 255)},
-                    {"olive", IM_COL32(128, 128, 0, 255)},
-                    {"lime", IM_COL32(0, 255, 0, 255)},
-                    {"green", IM_COL32(0, 128, 0, 255)},
-                    {"aqua", IM_COL32(0, 255, 255, 255)},
-                    {"teal", IM_COL32(0, 128, 128, 255)},
-                    {"blue", IM_COL32(0, 0, 255, 255)},
-                    {"navy", IM_COL32(0, 0, 128, 255)},
-                    {"fuchsia", IM_COL32(255, 0, 255, 255)},
-                    {"purple", IM_COL32(128, 0, 128, 255)}};
-
-static void buildColorTable() {
-  for (auto&& namedColor : staticColors) {
-    colorLookUpTable.try_emplace(namedColor.name, namedColor.value);
-  }
-}
-namespace {
-class Mechanism2DInfo {
- public:
-  std::string jsonLocation;
-};
-}  // namespace
-
-static Mechanism2DInfo mechanism2DInfo;
-
-bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (name == "jsonLocation") {
-    mechanism2DInfo.jsonLocation = value;
-  } else {
-    return false;
-  }
-  return true;
-}
-
-void WriteIni(ImGuiTextBuffer* out) {
-  out->appendf("[Mechanism2D][Mechanism2D]\njsonLocation=%s\n\n",
-               mechanism2DInfo.jsonLocation.c_str());
-}
-
-// read/write settings to ini file
-static void* Mechanism2DReadOpen(ImGuiContext* ctx,
-                                 ImGuiSettingsHandler* handler,
-                                 const char* name) {
-  if (name == wpi::StringRef{"Mechanism2D"}) return &mechanism2DInfo;
-  return nullptr;
-}
-
-static void Mechanism2DReadLine(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler, void* entry,
-                                const char* lineStr) {
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  if (entry == &mechanism2DInfo) ReadIni(name, value);
-}
-
-static void Mechanism2DWriteAll(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler,
-                                ImGuiTextBuffer* out_buf) {
-  WriteIni(out_buf);
-}
-
-static void GetJsonFileLocation() {
-  if (m_fileOpener && m_fileOpener->ready(0)) {
-    auto result = m_fileOpener->result();
-    if (!result.empty()) {
-      mechanism2DInfo.jsonLocation = result[0];
-    } else {
-      wpi::errs() << "Can not find json file!!!";
-    }
-  }
-}
-
-DrawLineStruct DrawLine(float startXLocation, float startYLocation, int length,
-                        float angle, ImDrawList* drawList, ImVec2 windowPos,
-                        ImColor color, const BodyConfig& bodyConfig,
-                        const std::string& previousPath) {
-  DrawLineStruct drawLineStruct;
-  drawLineStruct.angle = angle;
-  // Find the current path do the ligament
-  std::string currentPath = previousPath + bodyConfig.name;
-  // Find the angle in radians
-  double radAngle = (drawLineStruct.angle - 90) * wpi::math::pi / 180;
-  // Get the start X and Y location
-  drawLineStruct.xEnd = startXLocation + length * std::cos(radAngle);
-  drawLineStruct.yEnd = startYLocation + length * std::sin(radAngle);
-  // Add the line to the drawList
-  drawList->AddLine(
-      windowPos + ImVec2(startXLocation, startYLocation),
-      windowPos + ImVec2(drawLineStruct.xEnd, drawLineStruct.yEnd), color,
-      bodyConfig.lineWidth);
-  // Return the end X, Y, and angle
-  return drawLineStruct;
-}
-
-static void buildDrawList(float startXLocation, float startYLocation,
-                          ImDrawList* drawList, float previousAngle,
-                          const std::vector<BodyConfig>& subBodyConfigs,
-                          ImVec2 windowPos) {
-  for (BodyConfig const& bodyConfig : subBodyConfigs) {
-    hal::SimDouble angleHandle;
-    hal::SimDouble lengthHandle;
-    float angle = 0;
-    float length = 0;
-    // Get the smallest of width or height
-    double minSize;
-    // Find the min size of the window
-    minSize = ImGui::GetWindowHeight() > ImGui::GetWindowWidth()
-                  ? ImGui::GetWindowWidth()
-                  : ImGui::GetWindowHeight();
-    if (devHandle == 0) devHandle = HALSIM_GetSimDeviceHandle("Mechanism2D");
-    // Get the length
-    if (!lengthHandle)
-      lengthHandle = HALSIM_GetSimValueHandle(
-          devHandle, (bodyConfig.name + "/length").c_str());
-    if (lengthHandle) length = lengthHandle.Get();
-    if (length <= 0) {
-      length = bodyConfig.length;
-    }
-    // Get the angle
-    if (!angleHandle)
-      angleHandle = HALSIM_GetSimValueHandle(
-          devHandle, (bodyConfig.name + "/angle").c_str());
-    if (angleHandle) angle = angleHandle.Get();
-    // Calculate the next angle to go to
-    float angleToGoTo = angle + bodyConfig.angle + previousAngle;
-    // Draw the first line and get the ending coordinates
-
-    DrawLineStruct drawLine =
-        DrawLine(startXLocation, startYLocation, minSize / 100 * length,
-                 angleToGoTo, drawList, windowPos,
-                 colorLookUpTable[bodyConfig.color], bodyConfig, "");
-
-    // If the line has children then draw them with the stating points being the
-    // end of the parent
-    if (!bodyConfig.children.empty()) {
-      buildDrawList(drawLine.xEnd, drawLine.yEnd, drawList, drawLine.angle,
-                    bodyConfig.children, windowPos);
-    }
-  }
-}
-
-static BodyConfig readSubJson(const std::string& name, wpi::json const& body) {
-  BodyConfig c;
-  try {
-    c.name = name + "/" + body.at("name").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "could not read body name: " << e.what() << '\n';
-  }
-  try {
-    c.length = body.at("length").get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "length '" << c.name
-                << "': could not find length path: " << e.what() << '\n';
-  }
-  try {
-    c.color = body.at("color").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "color '" << c.name
-                << "': could not find color path: " << e.what() << '\n';
-  }
-  try {
-    c.angle = body.at("angle").get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "angle '" << c.name
-                << "': could not find angle path: " << e.what() << '\n';
-  }
-  try {
-    c.lineWidth = body.at("lineWidth").get<int>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "lineWidth '" << c.name
-                << "': could not find lineWidth path: " << e.what() << '\n';
-  }
-  try {
-    for (wpi::json const& child : body.at("children")) {
-      c.children.push_back(readSubJson(c.name, child));
-      wpi::outs() << "Reading Child with name " << c.name << '\n';
-    }
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "could not read body: " << e.what() << '\n';
-  }
-  return c;
-}
-
-static void readJson(std::string jFile) {
-  std::error_code ec;
-  std::string name;
-  wpi::raw_fd_istream is(jFile, ec);
-  if (ec) {
-    wpi::errs() << "could not open '" << jFile << "': " << ec.message() << '\n';
-  }
-  // parse file
-  wpi::json j;
-  try {
-    j = wpi::json::parse(is);
-  } catch (const wpi::json::parse_error& e) {
-    wpi::errs() << "byte " << e.byte << ": " << e.what() << '\n';
-  }
-  // top level must be an object
-  if (!j.is_object()) {
-    wpi::errs() << "must be JSON object\n";
-  }
-  try {
-    name = j.at("name").get<std::string>();
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "name '" << name
-                << "': could not find name path: " << e.what() << '\n';
-  }
-  try {
-    for (wpi::json const& body : j.at("body")) {
-      bodyConfigVector.push_back(readSubJson(name, body));
-    }
-  } catch (const wpi::json::exception& e) {
-    wpi::errs() << "could not read body: " << e.what() << '\n';
-  }
-}
-
-static void OptionMenuLocateJson() {
-  if (ImGui::BeginMenu("Mechanism2D")) {
-    if (ImGui::MenuItem("Load Json")) {
-      m_fileOpener = std::make_unique<pfd::open_file>(
-          "Choose Mechanism2D json", "", std::vector<std::string>{"*.json"});
-    }
-    ImGui::EndMenu();
-  }
-}
-
-static void DisplayAssembly2D() {
-  GetJsonFileLocation();
-  if (!mechanism2DInfo.jsonLocation.empty()) {
-    // Only read the json file if it changed
-    if (mechanism2DInfo.jsonLocation != previousJsonLocation) {
-      bodyConfigVector.clear();
-      readJson(mechanism2DInfo.jsonLocation);
-    }
-    previousJsonLocation = mechanism2DInfo.jsonLocation;
-    ImVec2 windowPos = ImGui::GetWindowPos();
-    ImDrawList* drawList = ImGui::GetWindowDrawList();
-    buildDrawList(ImGui::GetWindowWidth() / 2, ImGui::GetWindowHeight(),
-                  drawList, 0, bodyConfigVector, windowPos);
-  }
-}
-
-void Mechanism2D::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "Mechanism2D";
-  iniHandler.TypeHash = ImHashStr(iniHandler.TypeName);
-  iniHandler.ReadOpenFn = Mechanism2DReadOpen;
-  iniHandler.ReadLineFn = Mechanism2DReadLine;
-  iniHandler.WriteAllFn = Mechanism2DWriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
-  buildColorTable();
-  HALSimGui::AddWindow("Mechanism 2D", DisplayAssembly2D);
-  HALSimGui::SetWindowVisibility("Mechanism 2D", HALSimGui::kHide);
-  HALSimGui::AddOptionMenu(OptionMenuLocateJson);
-  HALSimGui::SetDefaultWindowPos("Mechanism 2D", 200, 200);
-  HALSimGui::SetDefaultWindowSize("Mechanism 2D", 600, 600);
-  HALSimGui::SetWindowPadding("Mechanism 2D", 0, 0);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h
deleted file mode 100644
index 2f291dd..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/Mechanism2D.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-class Mechanism2D {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.cpp
deleted file mode 100644
index 2d442b5..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.cpp
+++ /dev/null
@@ -1,409 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "NetworkTablesGui.h"
-
-#include <cstdio>
-#include <cstring>
-#include <initializer_list>
-#include <memory>
-#include <vector>
-
-#include <imgui.h>
-#include <networktables/NetworkTableInstance.h>
-#include <networktables/NetworkTableValue.h>
-#include <ntcore_cpp.h>
-#include <wpi/DenseMap.h>
-#include <wpi/Format.h>
-#include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-
-using namespace halsimgui;
-
-static NT_EntryListenerPoller gNetworkTablesPoller;
-static wpi::DenseMap<NT_Entry, std::unique_ptr<GuiDataSource>>
-    gNetworkTableSources;
-
-static void UpdateNetworkTableSources() {
-  bool timedOut = false;
-  for (auto&& event :
-       nt::PollEntryListener(gNetworkTablesPoller, 0, &timedOut)) {
-    if (!event.value->IsBoolean() && !event.value->IsDouble()) continue;
-    if (event.flags & NT_NOTIFY_NEW) {
-      auto& source = gNetworkTableSources[event.entry];
-      if (!source)
-        source =
-            std::make_unique<GuiDataSource>(wpi::Twine{"NT:"} + event.name);
-    }
-    if (event.flags & NT_NOTIFY_DELETE) {
-      if (auto& source = gNetworkTableSources[event.entry]) source.reset();
-    }
-    if (event.flags & (NT_NOTIFY_NEW | NT_NOTIFY_UPDATE)) {
-      if (auto& source = gNetworkTableSources[event.entry]) {
-        if (event.value->IsBoolean()) {
-          source->SetValue(event.value->GetBoolean() ? 1 : 0);
-          source->SetDigital(true);
-        } else if (event.value->IsDouble()) {
-          source->SetValue(event.value->GetDouble());
-          source->SetDigital(false);
-        }
-      }
-    }
-  }
-}
-
-static void BooleanArrayToString(wpi::SmallVectorImpl<char>& out,
-                                 wpi::ArrayRef<int> in) {
-  out.clear();
-  wpi::raw_svector_ostream os{out};
-  os << '[';
-  bool first = true;
-  for (auto v : in) {
-    if (!first) os << ',';
-    first = false;
-    if (v)
-      os << "true";
-    else
-      os << "false";
-  }
-  os << ']';
-}
-
-static std::shared_ptr<nt::Value> StringToBooleanArray(wpi::StringRef in) {
-  in = in.trim();
-  if (in.empty())
-    return nt::NetworkTableValue::MakeBooleanArray(
-        std::initializer_list<bool>{});
-  if (in.front() == '[') in = in.drop_front();
-  if (in.back() == ']') in = in.drop_back();
-  in = in.trim();
-
-  wpi::SmallVector<wpi::StringRef, 16> inSplit;
-  wpi::SmallVector<int, 16> out;
-
-  in.split(inSplit, ',', -1, false);
-  for (auto val : inSplit) {
-    val = val.trim();
-    if (val.equals_lower("true")) {
-      out.emplace_back(1);
-    } else if (val.equals_lower("false")) {
-      out.emplace_back(0);
-    } else {
-      wpi::errs() << "GUI: NetworkTables: Could not understand value '" << val
-                  << "'\n";
-      return nullptr;
-    }
-  }
-
-  return nt::NetworkTableValue::MakeBooleanArray(out);
-}
-
-static void DoubleArrayToString(wpi::SmallVectorImpl<char>& out,
-                                wpi::ArrayRef<double> in) {
-  out.clear();
-  wpi::raw_svector_ostream os{out};
-  os << '[';
-  bool first = true;
-  for (auto v : in) {
-    if (!first) os << ',';
-    first = false;
-    os << wpi::format("%.6f", v);
-  }
-  os << ']';
-}
-
-static std::shared_ptr<nt::Value> StringToDoubleArray(wpi::StringRef in) {
-  in = in.trim();
-  if (in.empty())
-    return nt::NetworkTableValue::MakeBooleanArray(
-        std::initializer_list<bool>{});
-  if (in.front() == '[') in = in.drop_front();
-  if (in.back() == ']') in = in.drop_back();
-  in = in.trim();
-
-  wpi::SmallVector<wpi::StringRef, 16> inSplit;
-  wpi::SmallVector<double, 16> out;
-
-  in.split(inSplit, ',', -1, false);
-  for (auto val : inSplit) {
-    val = val.trim();
-    wpi::SmallString<32> valStr = val;
-    double d;
-    if (std::sscanf(valStr.c_str(), "%lf", &d) == 1) {
-      out.emplace_back(d);
-    } else {
-      wpi::errs() << "GUI: NetworkTables: Could not understand value '" << val
-                  << "'\n";
-      return nullptr;
-    }
-  }
-
-  return nt::NetworkTableValue::MakeDoubleArray(out);
-}
-
-static void StringArrayToString(wpi::SmallVectorImpl<char>& out,
-                                wpi::ArrayRef<std::string> in) {
-  out.clear();
-  wpi::raw_svector_ostream os{out};
-  os << '[';
-  bool first = true;
-  for (auto&& v : in) {
-    if (!first) os << ',';
-    first = false;
-    os << '"';
-    os.write_escaped(v);
-    os << '"';
-  }
-  os << ']';
-}
-
-static int fromxdigit(char ch) {
-  if (ch >= 'a' && ch <= 'f')
-    return (ch - 'a' + 10);
-  else if (ch >= 'A' && ch <= 'F')
-    return (ch - 'A' + 10);
-  else
-    return ch - '0';
-}
-
-static wpi::StringRef UnescapeString(wpi::StringRef source,
-                                     wpi::SmallVectorImpl<char>& buf) {
-  assert(source.size() >= 2 && source.front() == '"' && source.back() == '"');
-  buf.clear();
-  buf.reserve(source.size() - 2);
-  for (auto s = source.begin() + 1, end = source.end() - 1; s != end; ++s) {
-    if (*s != '\\') {
-      buf.push_back(*s);
-      continue;
-    }
-    switch (*++s) {
-      case 't':
-        buf.push_back('\t');
-        break;
-      case 'n':
-        buf.push_back('\n');
-        break;
-      case 'x': {
-        if (!isxdigit(*(s + 1))) {
-          buf.push_back('x');  // treat it like a unknown escape
-          break;
-        }
-        int ch = fromxdigit(*++s);
-        if (std::isxdigit(*(s + 1))) {
-          ch <<= 4;
-          ch |= fromxdigit(*++s);
-        }
-        buf.push_back(static_cast<char>(ch));
-        break;
-      }
-      default:
-        buf.push_back(*s);
-        break;
-    }
-  }
-  return wpi::StringRef{buf.data(), buf.size()};
-}
-
-static std::shared_ptr<nt::Value> StringToStringArray(wpi::StringRef in) {
-  in = in.trim();
-  if (in.empty())
-    return nt::NetworkTableValue::MakeStringArray(
-        std::initializer_list<std::string>{});
-  if (in.front() == '[') in = in.drop_front();
-  if (in.back() == ']') in = in.drop_back();
-  in = in.trim();
-
-  wpi::SmallVector<wpi::StringRef, 16> inSplit;
-  std::vector<std::string> out;
-  wpi::SmallString<32> buf;
-
-  in.split(inSplit, ',', -1, false);
-  for (auto val : inSplit) {
-    val = val.trim();
-    if (val.empty()) continue;
-    if (val.front() != '"' || val.back() != '"') {
-      wpi::errs() << "GUI: NetworkTables: Could not understand value '" << val
-                  << "'\n";
-      return nullptr;
-    }
-    out.emplace_back(UnescapeString(val, buf));
-  }
-
-  return nt::NetworkTableValue::MakeStringArray(std::move(out));
-}
-
-static constexpr size_t kTextBufferSize = 4096;
-
-static char* GetTextBuffer(wpi::StringRef in) {
-  static char textBuffer[kTextBufferSize];
-  size_t len = (std::min)(in.size(), kTextBufferSize - 1);
-  std::memcpy(textBuffer, in.data(), len);
-  textBuffer[len] = '\0';
-  return textBuffer;
-}
-
-static void DisplayNetworkTables() {
-  static auto inst = nt::NetworkTableInstance::GetDefault();
-
-  if (ImGui::CollapsingHeader("Connections")) {
-    ImGui::Columns(4, "connections");
-    ImGui::Text("Id");
-    ImGui::NextColumn();
-    ImGui::Text("Address");
-    ImGui::NextColumn();
-    ImGui::Text("Updated");
-    ImGui::NextColumn();
-    ImGui::Text("Proto");
-    ImGui::NextColumn();
-    ImGui::Separator();
-    for (auto&& i : inst.GetConnections()) {
-      ImGui::Text("%s", i.remote_id.c_str());
-      ImGui::NextColumn();
-      ImGui::Text("%s", i.remote_ip.c_str());
-      ImGui::NextColumn();
-      ImGui::Text("%llu",
-                  static_cast<unsigned long long>(  // NOLINT(runtime/int)
-                      i.last_update));
-      ImGui::NextColumn();
-      ImGui::Text("%d.%d", i.protocol_version >> 8, i.protocol_version & 0xff);
-      ImGui::NextColumn();
-    }
-    ImGui::Columns();
-  }
-
-  if (ImGui::CollapsingHeader("Values", ImGuiTreeNodeFlags_DefaultOpen)) {
-    static bool first = true;
-    ImGui::Columns(4, "values");
-    if (first) ImGui::SetColumnWidth(-1, 0.5f * ImGui::GetWindowWidth());
-    ImGui::Text("Name");
-    ImGui::NextColumn();
-    ImGui::Text("Value");
-    ImGui::NextColumn();
-    if (first) ImGui::SetColumnWidth(-1, 12 * ImGui::GetFontSize());
-    ImGui::Text("Flags");
-    ImGui::NextColumn();
-    ImGui::Text("Changed");
-    ImGui::NextColumn();
-    ImGui::Separator();
-    first = false;
-
-    auto info = inst.GetEntryInfo("", 0);
-    std::sort(info.begin(), info.end(),
-              [](const auto& a, const auto& b) { return a.name < b.name; });
-
-    for (auto&& i : info) {
-      if (auto source = gNetworkTableSources[i.entry].get()) {
-        ImGui::Selectable(i.name.c_str());
-        source->EmitDrag();
-      } else {
-        ImGui::Text("%s", i.name.c_str());
-      }
-      ImGui::NextColumn();
-
-      if (auto val = nt::GetEntryValue(i.entry)) {
-        ImGui::PushID(i.name.c_str());
-        switch (val->type()) {
-          case NT_BOOLEAN: {
-            static const char* boolOptions[] = {"false", "true"};
-            int v = val->GetBoolean() ? 1 : 0;
-            if (ImGui::Combo("boolean", &v, boolOptions, 2))
-              nt::SetEntryValue(i.entry, nt::NetworkTableValue::MakeBoolean(v));
-            break;
-          }
-          case NT_DOUBLE: {
-            double v = val->GetDouble();
-            if (ImGui::InputDouble("double", &v, 0, 0, "%.6f",
-                                   ImGuiInputTextFlags_EnterReturnsTrue))
-              nt::SetEntryValue(i.entry, nt::NetworkTableValue::MakeDouble(v));
-            break;
-          }
-          case NT_STRING: {
-            char* v = GetTextBuffer(val->GetString());
-            if (ImGui::InputText("string", v, kTextBufferSize,
-                                 ImGuiInputTextFlags_EnterReturnsTrue))
-              nt::SetEntryValue(i.entry, nt::NetworkTableValue::MakeString(v));
-            break;
-          }
-          case NT_BOOLEAN_ARRAY: {
-            wpi::SmallString<64> buf;
-            BooleanArrayToString(buf, val->GetBooleanArray());
-            char* v = GetTextBuffer(buf);
-            if (ImGui::InputText("boolean[]", v, kTextBufferSize,
-                                 ImGuiInputTextFlags_EnterReturnsTrue)) {
-              if (auto outv = StringToBooleanArray(v))
-                nt::SetEntryValue(i.entry, std::move(outv));
-            }
-            break;
-          }
-          case NT_DOUBLE_ARRAY: {
-            wpi::SmallString<64> buf;
-            DoubleArrayToString(buf, val->GetDoubleArray());
-            char* v = GetTextBuffer(buf);
-            if (ImGui::InputText("double[]", v, kTextBufferSize,
-                                 ImGuiInputTextFlags_EnterReturnsTrue)) {
-              if (auto outv = StringToDoubleArray(v))
-                nt::SetEntryValue(i.entry, std::move(outv));
-            }
-            break;
-          }
-          case NT_STRING_ARRAY: {
-            wpi::SmallString<64> buf;
-            StringArrayToString(buf, val->GetStringArray());
-            char* v = GetTextBuffer(buf);
-            if (ImGui::InputText("string[]", v, kTextBufferSize,
-                                 ImGuiInputTextFlags_EnterReturnsTrue)) {
-              if (auto outv = StringToStringArray(v))
-                nt::SetEntryValue(i.entry, std::move(outv));
-            }
-            break;
-          }
-          case NT_RAW:
-            ImGui::LabelText("raw", "[...]");
-            break;
-          case NT_RPC:
-            ImGui::LabelText("rpc", "[...]");
-            break;
-          default:
-            ImGui::LabelText("other", "?");
-            break;
-        }
-        ImGui::PopID();
-      }
-      ImGui::NextColumn();
-
-      if ((i.flags & NT_PERSISTENT) != 0)
-        ImGui::Text("Persistent");
-      else if (i.flags != 0)
-        ImGui::Text("%02x", i.flags);
-      ImGui::NextColumn();
-
-      ImGui::Text("%llu",
-                  static_cast<unsigned long long>(  // NOLINT(runtime/int)
-                      i.last_change));
-      ImGui::NextColumn();
-      ImGui::Separator();
-    }
-    ImGui::Columns();
-  }
-}
-
-void NetworkTablesGui::Initialize() {
-  gNetworkTablesPoller =
-      nt::CreateEntryListenerPoller(nt::GetDefaultInstance());
-  nt::AddPolledEntryListener(gNetworkTablesPoller, "",
-                             NT_NOTIFY_LOCAL | NT_NOTIFY_NEW |
-                                 NT_NOTIFY_UPDATE | NT_NOTIFY_DELETE |
-                                 NT_NOTIFY_IMMEDIATE);
-  HALSimGui::AddExecute(UpdateNetworkTableSources);
-  HALSimGui::AddWindow("NetworkTables", DisplayNetworkTables);
-  HALSimGui::SetDefaultWindowPos("NetworkTables", 250, 277);
-  HALSimGui::SetDefaultWindowSize("NetworkTables", 750, 185);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.h
deleted file mode 100644
index f4e5d7b..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-class NetworkTablesGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesSimGui.cpp
new file mode 100644
index 0000000..c17a4f8
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesSimGui.cpp
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "NetworkTablesSimGui.h"
+
+#include <glass/networktables/NetworkTables.h>
+
+#include <wpigui.h>
+
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+static std::unique_ptr<glass::NetworkTablesModel> gNetworkTablesModel;
+static std::unique_ptr<glass::NetworkTablesView> gNetworkTablesView;
+static glass::Window* gNetworkTablesWindow;
+
+void NetworkTablesSimGui::Initialize() {
+  gNetworkTablesModel = std::make_unique<glass::NetworkTablesModel>();
+  gNetworkTablesView =
+      std::make_unique<glass::NetworkTablesView>(gNetworkTablesModel.get());
+  wpi::gui::AddEarlyExecute([] { gNetworkTablesModel->Update(); });
+  gNetworkTablesWindow = HALSimGui::ntProvider.AddWindow(
+      "NetworkTables", [] { gNetworkTablesView->Display(); });
+  if (gNetworkTablesWindow) {
+    gNetworkTablesWindow->SetDefaultPos(250, 277);
+    gNetworkTablesWindow->SetDefaultSize(750, 185);
+    gNetworkTablesWindow->DisableRenamePopup();
+  }
+}
+
+void NetworkTablesSimGui::DisplayMenu() {
+  if (gNetworkTablesWindow) {
+    gNetworkTablesWindow->DisplayMenuItem("NetworkTables View");
+  }
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesSimGui.h
new file mode 100644
index 0000000..d0d02b3
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/NetworkTablesSimGui.h
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class NetworkTablesSimGui {
+ public:
+  static void Initialize();
+  static void DisplayMenu();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp
new file mode 100644
index 0000000..bcc510a
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.cpp
@@ -0,0 +1,231 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "PCMSimGui.h"
+
+#include <glass/hardware/PCM.h>
+#include <glass/other/DeviceTree.h>
+
+#include <cstdio>
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/Value.h>
+#include <hal/simulation/CTREPCMData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+#include "SimDeviceGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(CTREPCMCompressorOn, "Compressor On");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(CTREPCMClosedLoopEnabled, "Closed Loop");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(CTREPCMPressureSwitch, "Pressure Switch");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(CTREPCMCompressorCurrent, "Comp Current");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED2(CTREPCMSolenoidOutput, "Solenoid");
+
+class CompressorSimModel : public glass::CompressorModel {
+ public:
+  explicit CompressorSimModel(int32_t index)
+      : m_index{index},
+        m_running{index},
+        m_enabled{index},
+        m_pressureSwitch{index},
+        m_current{index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetCTREPCMInitialized(m_index); }
+
+  glass::DataSource* GetRunningData() override { return &m_running; }
+  glass::DataSource* GetEnabledData() override { return &m_enabled; }
+  glass::DataSource* GetPressureSwitchData() override {
+    return &m_pressureSwitch;
+  }
+  glass::DataSource* GetCurrentData() override { return &m_current; }
+
+  void SetRunning(bool val) override {
+    HALSIM_SetCTREPCMCompressorOn(m_index, val);
+  }
+  void SetEnabled(bool val) override {
+    HALSIM_SetCTREPCMClosedLoopEnabled(m_index, val);
+  }
+  void SetPressureSwitch(bool val) override {
+    HALSIM_SetCTREPCMPressureSwitch(m_index, val);
+  }
+  void SetCurrent(double val) override {
+    HALSIM_SetCTREPCMCompressorCurrent(m_index, val);
+  }
+
+ private:
+  int32_t m_index;
+  CTREPCMCompressorOnSource m_running;
+  CTREPCMClosedLoopEnabledSource m_enabled;
+  CTREPCMPressureSwitchSource m_pressureSwitch;
+  CTREPCMCompressorCurrentSource m_current;
+};
+
+class SolenoidSimModel : public glass::SolenoidModel {
+ public:
+  SolenoidSimModel(int32_t index, int32_t channel)
+      : m_index{index}, m_channel{channel}, m_output{index, channel} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetCTREPCMInitialized(m_index); }
+
+  glass::DataSource* GetOutputData() override { return &m_output; }
+
+  void SetOutput(bool val) override {
+    HALSIM_SetCTREPCMSolenoidOutput(m_index, m_channel, val);
+  }
+
+ private:
+  int32_t m_index;
+  int32_t m_channel;
+  CTREPCMSolenoidOutputSource m_output;
+};
+
+class PCMSimModel : public glass::PCMModel {
+ public:
+  explicit PCMSimModel(int32_t index)
+      : m_index{index},
+        m_compressor{index},
+        m_solenoids(HAL_GetNumCTRESolenoidChannels()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  CompressorSimModel* GetCompressor() override { return &m_compressor; }
+
+  void ForEachSolenoid(
+      wpi::function_ref<void(glass::SolenoidModel& model, int index)> func)
+      override;
+
+  int GetNumSolenoids() const { return m_solenoidInitCount; }
+
+ private:
+  int32_t m_index;
+  CompressorSimModel m_compressor;
+  std::vector<std::unique_ptr<SolenoidSimModel>> m_solenoids;
+  int m_solenoidInitCount = 0;
+};
+
+class PCMsSimModel : public glass::PCMsModel {
+ public:
+  PCMsSimModel() : m_models(HAL_GetNumCTREPCMModules()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachPCM(
+      wpi::function_ref<void(glass::PCMModel& model, int index)> func) override;
+
+ private:
+  std::vector<std::unique_ptr<PCMSimModel>> m_models;
+};
+}  // namespace
+
+void PCMSimModel::Update() {
+  int32_t numChannels = m_solenoids.size();
+  m_solenoidInitCount = 0;
+  for (int32_t i = 0; i < numChannels; ++i) {
+    auto& model = m_solenoids[i];
+    if (HALSIM_GetCTREPCMInitialized(m_index)) {
+      if (!model) {
+        model = std::make_unique<SolenoidSimModel>(m_index, i);
+      }
+      ++m_solenoidInitCount;
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void PCMSimModel::ForEachSolenoid(
+    wpi::function_ref<void(glass::SolenoidModel& model, int index)> func) {
+  if (m_solenoidInitCount == 0) {
+    return;
+  }
+  int32_t numSolenoids = m_solenoids.size();
+  for (int32_t i = 0; i < numSolenoids; ++i) {
+    if (auto model = m_solenoids[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+void PCMsSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetCTREPCMInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<PCMSimModel>(i);
+      }
+      model->Update();
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void PCMsSimModel::ForEachPCM(
+    wpi::function_ref<void(glass::PCMModel& model, int index)> func) {
+  int32_t numCTREPCMs = m_models.size();
+  for (int32_t i = 0; i < numCTREPCMs; ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool PCMsAnyInitialized() {
+  static const int32_t num = HAL_GetNumCTREPCMModules();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetCTREPCMInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void PCMSimGui::Initialize() {
+  HALSimGui::halProvider.RegisterModel("CTREPCMs", PCMsAnyInitialized, [] {
+    return std::make_unique<PCMsSimModel>();
+  });
+  HALSimGui::halProvider.RegisterView(
+      "Solenoids", "CTREPCMs",
+      [](glass::Model* model) {
+        bool any = false;
+        static_cast<PCMsSimModel*>(model)->ForEachPCM(
+            [&](glass::PCMModel& CTREPCM, int) {
+              if (static_cast<PCMSimModel*>(&CTREPCM)->GetNumSolenoids() > 0) {
+                any = true;
+              }
+            });
+        return any;
+      },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(290, 20);
+        return glass::MakeFunctionView([=] {
+          glass::DisplayPCMsSolenoids(
+              static_cast<PCMsSimModel*>(model),
+              HALSimGui::halProvider.AreOutputsEnabled());
+        });
+      });
+
+  SimDeviceGui::GetDeviceTree().Add(
+      HALSimGui::halProvider.GetModel("CTREPCMs"), [](glass::Model* model) {
+        glass::DisplayCompressorsDevice(
+            static_cast<PCMsSimModel*>(model),
+            HALSimGui::halProvider.AreOutputsEnabled());
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.h
new file mode 100644
index 0000000..aef7717
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PCMSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class PCMSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
deleted file mode 100644
index 35ba492..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PDPGui.cpp
+++ /dev/null
@@ -1,141 +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 "PDPGui.h"
-
-#include <algorithm>
-#include <cstdio>
-#include <cstring>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/PDPData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PDPTemperature, "PDP Temp");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PDPVoltage, "PDP Voltage");
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED2(PDPCurrent, "PDP Current");
-struct PDPSource {
-  explicit PDPSource(int32_t index) : temp{index}, voltage{index} {
-    const int numChannels = HAL_GetNumPDPChannels();
-    currents.reserve(numChannels);
-    for (int i = 0; i < numChannels; ++i)
-      currents.emplace_back(std::make_unique<PDPCurrentSource>(index, i));
-  }
-  PDPTemperatureSource temp;
-  PDPVoltageSource voltage;
-  std::vector<std::unique_ptr<PDPCurrentSource>> currents;
-};
-}  // namespace
-
-static IniSaver<NameInfo> gChannels{"PDP"};
-static std::vector<std::unique_ptr<PDPSource>> gPDPSources;
-
-static void UpdatePDPSources() {
-  for (int i = 0, iend = gPDPSources.size(); i < iend; ++i) {
-    auto& source = gPDPSources[i];
-    if (HALSIM_GetPDPInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<PDPSource>(i);
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayPDP() {
-  bool hasAny = false;
-  for (int i = 0, iend = gPDPSources.size(); i < iend; ++i) {
-    if (auto source = gPDPSources[i].get()) {
-      hasAny = true;
-
-      char name[128];
-      std::snprintf(name, sizeof(name), "PDP[%d]", i);
-      if (ImGui::CollapsingHeader(name, ImGuiTreeNodeFlags_DefaultOpen)) {
-        ImGui::PushID(i);
-
-        // temperature
-        double temp = source->temp.GetValue();
-        ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-        if (source->temp.InputDouble("Temp", &temp, 0, 0, "%.3f"))
-          HALSIM_SetPDPTemperature(i, temp);
-
-        // voltage
-        double volts = source->voltage.GetValue();
-        ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-        if (source->voltage.InputDouble("Voltage", &volts, 0, 0, "%.3f"))
-          HALSIM_SetPDPVoltage(i, volts);
-
-        // channel currents; show as two columns laid out like PDP
-        const int numChannels = source->currents.size();
-        ImGui::Text("Channel Current (A)");
-        ImGui::Columns(2, "channels", false);
-        float maxWidth = ImGui::GetFontSize() * 13;
-        for (int left = 0, right = numChannels - 1; left < right;
-             ++left, --right) {
-          double val;
-
-          ImGui::PushID(left);
-          auto& leftInfo = gChannels[i * numChannels + left];
-          leftInfo.GetLabel(name, sizeof(name), "", left);
-          val = source->currents[left]->GetValue();
-          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-          if (source->currents[left]->InputDouble(name, &val, 0, 0, "%.3f"))
-            HALSIM_SetPDPCurrent(i, left, val);
-          float leftWidth = ImGui::GetItemRectSize().x;
-          if (leftInfo.PopupEditName(left)) {
-            source->currents[left]->SetName(leftInfo.GetName());
-          }
-          ImGui::PopID();
-          ImGui::NextColumn();
-
-          ImGui::PushID(right);
-          auto& rightInfo = gChannels[i * numChannels + right];
-          rightInfo.GetLabel(name, sizeof(name), "", right);
-          val = source->currents[right]->GetValue();
-          ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-          if (source->currents[right]->InputDouble(name, &val, 0, 0, "%.3f"))
-            HALSIM_SetPDPCurrent(i, right, val);
-          float rightWidth = ImGui::GetItemRectSize().x;
-          if (rightInfo.PopupEditName(right)) {
-            source->currents[right]->SetName(rightInfo.GetName());
-          }
-          ImGui::PopID();
-          ImGui::NextColumn();
-
-          float width =
-              (std::max)(leftWidth, rightWidth) * 2 + ImGui::GetFontSize() * 4;
-          if (width > maxWidth) maxWidth = width;
-        }
-        ImGui::Columns(1);
-        ImGui::Dummy(ImVec2(maxWidth, 0));
-        ImGui::PopID();
-      }
-    }
-  }
-  if (!hasAny) ImGui::Text("No PDPs");
-}
-
-void PDPGui::Initialize() {
-  gChannels.Initialize();
-  gPDPSources.resize(HAL_GetNumPDPModules());
-  HALSimGui::AddExecute(UpdatePDPSources);
-  HALSimGui::AddWindow("PDP", DisplayPDP);
-  // hide it by default
-  HALSimGui::SetWindowVisibility("PDP", HALSimGui::kHide);
-  HALSimGui::SetDefaultWindowPos("PDP", 245, 155);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PDPGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PDPGui.h
deleted file mode 100644
index aa53a3c..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PDPGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class PDPGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
deleted file mode 100644
index 59d7a93..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMGui.cpp
+++ /dev/null
@@ -1,104 +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 "PWMGui.h"
-
-#include <cstdio>
-#include <cstring>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/AddressableLEDData.h>
-#include <hal/simulation/PWMData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PWMSpeed, "PWM");
-}  // namespace
-
-static IniSaver<NameInfo> gPWM{"PWM"};
-static std::vector<std::unique_ptr<PWMSpeedSource>> gPWMSources;
-
-static void UpdatePWMSources() {
-  static const int numPWM = HAL_GetNumPWMChannels();
-  if (static_cast<size_t>(numPWM) != gPWMSources.size())
-    gPWMSources.resize(numPWM);
-
-  for (int i = 0; i < numPWM; ++i) {
-    auto& source = gPWMSources[i];
-    if (HALSIM_GetPWMInitialized(i)) {
-      if (!source) {
-        source = std::make_unique<PWMSpeedSource>(i);
-        source->SetName(gPWM[i].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayPWMs() {
-  bool hasOutputs = false;
-  static const int numPWM = HAL_GetNumPWMChannels();
-  static const int numLED = HAL_GetNumAddressableLEDs();
-  static auto ledMap = std::make_unique<int[]>(numPWM);
-
-  std::memset(ledMap.get(), 0, numPWM * sizeof(ledMap[0]));
-
-  for (int i = 0; i < numLED; ++i) {
-    if (HALSIM_GetAddressableLEDInitialized(i)) {
-      int channel = HALSIM_GetAddressableLEDOutputPort(i);
-      if (channel >= 0 && channel < numPWM) ledMap[channel] = i + 1;
-    }
-  }
-
-  bool first = true;
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
-  for (int i = 0; i < numPWM; ++i) {
-    if (auto source = gPWMSources[i].get()) {
-      ImGui::PushID(i);
-      hasOutputs = true;
-
-      if (!first)
-        ImGui::Separator();
-      else
-        first = false;
-
-      auto& info = gPWM[i];
-      char label[128];
-      info.GetLabel(label, sizeof(label), "PWM", i);
-      if (ledMap[i] > 0) {
-        ImGui::LabelText(label, "LED[%d]", ledMap[i] - 1);
-      } else {
-        float val = HALSimGui::AreOutputsDisabled() ? 0 : HALSIM_GetPWMSpeed(i);
-        source->LabelText(label, "%0.3f", val);
-      }
-      if (info.PopupEditName(i)) {
-        source->SetName(info.GetName());
-      }
-      ImGui::PopID();
-    }
-  }
-  ImGui::PopItemWidth();
-  if (!hasOutputs) ImGui::Text("No PWM outputs");
-}
-
-void PWMGui::Initialize() {
-  gPWM.Initialize();
-  HALSimGui::AddExecute(UpdatePWMSources);
-  HALSimGui::AddWindow("PWM Outputs", DisplayPWMs,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("PWM Outputs", 910, 20);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMGui.h
deleted file mode 100644
index 211eaba..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class PWMGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMSimGui.cpp
new file mode 100644
index 0000000..3cb5e1a
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMSimGui.cpp
@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "PWMSimGui.h"
+
+#include <glass/hardware/PWM.h>
+
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/simulation/AddressableLEDData.h>
+#include <hal/simulation/PWMData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PWMSpeed, "PWM");
+
+class PWMSimModel : public glass::PWMModel {
+ public:
+  explicit PWMSimModel(int32_t index) : m_index{index}, m_speed{m_index} {}
+
+  void Update() override {}
+
+  bool Exists() override { return HALSIM_GetPWMInitialized(m_index); }
+
+  void SetAddressableLED(int led) { m_led = led; }
+  int GetAddressableLED() const override { return m_led; }
+
+  glass::DataSource* GetSpeedData() override { return &m_speed; }
+
+  void SetSpeed(double val) override { HALSIM_SetPWMSpeed(m_index, val); }
+
+ private:
+  int32_t m_index;
+  int m_led = -1;
+  PWMSpeedSource m_speed;
+};
+
+class PWMsSimModel : public glass::PWMsModel {
+ public:
+  PWMsSimModel() : m_sources(HAL_GetNumPWMChannels()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachPWM(
+      wpi::function_ref<void(glass::PWMModel& model, int index)> func) override;
+
+ private:
+  // indexed by channel
+  std::vector<std::unique_ptr<PWMSimModel>> m_sources;
+};
+}  // namespace
+
+void PWMsSimModel::Update() {
+  const int32_t numPWM = m_sources.size();
+  for (int32_t i = 0; i < numPWM; ++i) {
+    auto& model = m_sources[i];
+    if (HALSIM_GetPWMInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<PWMSimModel>(i);
+      }
+      model->SetAddressableLED(-1);
+    } else {
+      model.reset();
+    }
+  }
+
+  static const int32_t numLED = HAL_GetNumAddressableLEDs();
+  for (int32_t i = 0; i < numLED; ++i) {
+    if (HALSIM_GetAddressableLEDInitialized(i)) {
+      int32_t channel = HALSIM_GetAddressableLEDOutputPort(i);
+      if (channel >= 0 && channel < numPWM && m_sources[channel]) {
+        m_sources[channel]->SetAddressableLED(i);
+      }
+    }
+  }
+}
+
+void PWMsSimModel::ForEachPWM(
+    wpi::function_ref<void(glass::PWMModel& model, int index)> func) {
+  const int32_t numPWM = m_sources.size();
+  for (int32_t i = 0; i < numPWM; ++i) {
+    if (auto model = m_sources[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool PWMsAnyInitialized() {
+  static const int32_t num = HAL_GetNumPWMChannels();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetPWMInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void PWMSimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "PWM Outputs", PWMsAnyInitialized,
+      [] { return std::make_unique<PWMsSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(910, 20);
+        return glass::MakeFunctionView([=] {
+          glass::DisplayPWMs(static_cast<PWMsSimModel*>(model),
+                             HALSimGui::halProvider.AreOutputsEnabled());
+        });
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMSimGui.h
new file mode 100644
index 0000000..d947643
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PWMSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class PWMSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PlotGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PlotGui.cpp
deleted file mode 100644
index ecaec63..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PlotGui.cpp
+++ /dev/null
@@ -1,894 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 "PlotGui.h"
-
-#include <algorithm>
-#include <atomic>
-#include <cstdio>
-#include <cstring>
-#include <memory>
-#include <vector>
-
-#define IMGUI_DEFINE_MATH_OPERATORS
-#include <hal/simulation/MockHooks.h>
-#include <imgui.h>
-#include <imgui_internal.h>
-#include <implot.h>
-#include <wpi/Signal.h>
-#include <wpi/SmallVector.h>
-#include <wpi/timestamp.h>
-#include <wpi/raw_ostream.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaverInfo.h"
-#include "IniSaverVector.h"
-
-using namespace halsimgui;
-
-namespace {
-struct PlotSeriesRef {
-  size_t plotIndex;
-  size_t seriesIndex;
-};
-
-class PlotSeries : public NameInfo, public OpenInfo {
- public:
-  explicit PlotSeries(wpi::StringRef id);
-  explicit PlotSeries(GuiDataSource* source, int yAxis = 0);
-
-  const std::string& GetId() const { return m_id; }
-
-  void CheckSource();
-
-  void SetSource(GuiDataSource* source);
-  GuiDataSource* GetSource() const { return m_source; }
-
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out);
-
-  bool EmitPlot(double now, size_t i, size_t plotIndex);
-  bool EmitSettings(size_t i, size_t plotIndex);
-  bool EmitSettingsDetail(size_t i);
-  void EmitDragDropPayload(size_t i, size_t plotIndex);
-
-  void GetLabel(char* buf, size_t size) const;
-
-  int GetYAxis() const { return m_yAxis; }
-  void SetYAxis(int yAxis) { m_yAxis = yAxis; }
-
- private:
-  bool IsDigital() const {
-    return m_digital == kDigital ||
-           (m_digital == kAuto && m_source && m_source->IsDigital());
-  }
-  void AppendValue(double value);
-
-  // source linkage
-  GuiDataSource* m_source = nullptr;
-  wpi::sig::ScopedConnection m_sourceCreatedConn;
-  wpi::sig::ScopedConnection m_newValueConn;
-  std::string m_id;
-
-  // user settings
-  int m_yAxis = 0;
-  ImVec4 m_color = IMPLOT_AUTO_COL;
-  int m_marker = 0;
-
-  enum Digital { kAuto, kDigital, kAnalog };
-  int m_digital = 0;
-  int m_digitalBitHeight = 8;
-  int m_digitalBitGap = 4;
-
-  // value storage
-  static constexpr int kMaxSize = 2000;
-  static constexpr double kTimeGap = 0.05;
-  std::atomic<int> m_size = 0;
-  std::atomic<int> m_offset = 0;
-  ImPlotPoint m_data[kMaxSize];
-};
-
-class Plot : public NameInfo, public OpenInfo {
- public:
-  Plot();
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out);
-
-  void GetLabel(char* buf, size_t size, int index) const;
-  void GetName(char* buf, size_t size, int index) const;
-
-  void DragDropTarget(size_t i, bool inPlot);
-  void EmitPlot(double now, size_t i);
-  void EmitSettings(size_t i);
-
-  std::vector<std::unique_ptr<PlotSeries>> m_series;
-
- private:
-  void EmitSettingsLimits(int axis);
-
-  bool m_visible = true;
-  unsigned int m_plotFlags = ImPlotFlags_Default;
-  bool m_lockPrevX = false;
-  bool m_paused = false;
-  float m_viewTime = 10;
-  int m_height = 300;
-  struct PlotRange {
-    double min = 0;
-    double max = 1;
-    bool lockMin = false;
-    bool lockMax = false;
-    bool apply = false;
-  };
-  PlotRange m_axisRange[3];
-  ImPlotRange m_xaxisRange;  // read from plot, used for lockPrevX
-};
-}  // namespace
-
-static IniSaverVector<Plot> gPlots{"Plot"};
-
-PlotSeries::PlotSeries(wpi::StringRef id) : m_id(id) {
-  if (GuiDataSource* source = GuiDataSource::Find(id)) {
-    SetSource(source);
-    return;
-  }
-  CheckSource();
-}
-
-PlotSeries::PlotSeries(GuiDataSource* source, int yAxis) : m_yAxis(yAxis) {
-  SetSource(source);
-}
-
-void PlotSeries::CheckSource() {
-  if (!m_newValueConn.connected() && !m_sourceCreatedConn.connected()) {
-    m_source = nullptr;
-    m_sourceCreatedConn = GuiDataSource::sourceCreated.connect_connection(
-        [this](const char* id, GuiDataSource* source) {
-          if (m_id == id) {
-            SetSource(source);
-            m_sourceCreatedConn.disconnect();
-          }
-        });
-  }
-}
-
-void PlotSeries::SetSource(GuiDataSource* source) {
-  m_source = source;
-  m_id = source->GetId();
-
-  // add initial value
-  m_data[m_size++] = ImPlotPoint{wpi::Now() * 1.0e-6, source->GetValue()};
-
-  m_newValueConn = source->valueChanged.connect_connection(
-      [this](double value) { AppendValue(value); });
-}
-
-void PlotSeries::AppendValue(double value) {
-  double time = wpi::Now() * 1.0e-6;
-  if (IsDigital()) {
-    if (m_size < kMaxSize) {
-      m_data[m_size] = ImPlotPoint{time, value};
-      ++m_size;
-    } else {
-      m_data[m_offset] = ImPlotPoint{time, value};
-      m_offset = (m_offset + 1) % kMaxSize;
-    }
-  } else {
-    // as an analog graph draws linear lines in between each value,
-    // insert duplicate value if "long" time between updates so it
-    // looks appropriately flat
-    if (m_size < kMaxSize) {
-      if (m_size > 0) {
-        if ((time - m_data[m_size - 1].x) > kTimeGap) {
-          m_data[m_size] = ImPlotPoint{time, m_data[m_size - 1].y};
-          ++m_size;
-        }
-      }
-      m_data[m_size] = ImPlotPoint{time, value};
-      ++m_size;
-    } else {
-      if (m_offset == 0) {
-        if ((time - m_data[kMaxSize - 1].x) > kTimeGap) {
-          m_data[m_offset] = ImPlotPoint{time, m_data[kMaxSize - 1].y};
-          ++m_offset;
-        }
-      } else {
-        if ((time - m_data[m_offset - 1].x) > kTimeGap) {
-          m_data[m_offset] = ImPlotPoint{time, m_data[m_offset - 1].y};
-          m_offset = (m_offset + 1) % kMaxSize;
-        }
-      }
-      m_data[m_offset] = ImPlotPoint{time, value};
-      m_offset = (m_offset + 1) % kMaxSize;
-    }
-  }
-}
-
-bool PlotSeries::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (NameInfo::ReadIni(name, value)) return true;
-  if (OpenInfo::ReadIni(name, value)) return true;
-  if (name == "yAxis") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_yAxis = num;
-    return true;
-  } else if (name == "color") {
-    unsigned int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_color = ImColor(num);
-    return true;
-  } else if (name == "marker") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_marker = num;
-    return true;
-  } else if (name == "digital") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_digital = num;
-    return true;
-  } else if (name == "digitalBitHeight") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_digitalBitHeight = num;
-    return true;
-  } else if (name == "digitalBitGap") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_digitalBitGap = num;
-    return true;
-  }
-  return false;
-}
-
-void PlotSeries::WriteIni(ImGuiTextBuffer* out) {
-  NameInfo::WriteIni(out);
-  OpenInfo::WriteIni(out);
-  out->appendf(
-      "yAxis=%d\ncolor=%u\nmarker=%d\ndigital=%d\n"
-      "digitalBitHeight=%d\ndigitalBitGap=%d\n",
-      m_yAxis, static_cast<ImU32>(ImColor(m_color)), m_marker, m_digital,
-      m_digitalBitHeight, m_digitalBitGap);
-}
-
-void PlotSeries::GetLabel(char* buf, size_t size) const {
-  const char* name = GetName();
-  if (name[0] == '\0' && m_newValueConn.connected()) name = m_source->GetName();
-  if (name[0] == '\0') name = m_id.c_str();
-  std::snprintf(buf, size, "%s###%s", name, m_id.c_str());
-}
-
-bool PlotSeries::EmitPlot(double now, size_t i, size_t plotIndex) {
-  CheckSource();
-
-  char label[128];
-  GetLabel(label, sizeof(label));
-
-  int size = m_size;
-  int offset = m_offset;
-
-  // need to have last value at current time, so need to create fake last value
-  // we handle the offset logic ourselves to avoid wrap issues with size + 1
-  struct GetterData {
-    double now;
-    ImPlotPoint* data;
-    int size;
-    int offset;
-  };
-  GetterData getterData = {now, m_data, size, offset};
-  auto getter = [](void* data, int idx) {
-    auto d = static_cast<GetterData*>(data);
-    if (idx == d->size)
-      return ImPlotPoint{
-          d->now, d->data[d->offset == 0 ? d->size - 1 : d->offset - 1].y};
-    if (d->offset + idx < d->size)
-      return d->data[d->offset + idx];
-    else
-      return d->data[d->offset + idx - d->size];
-  };
-
-  if (m_color.w == IMPLOT_AUTO_COL.w) m_color = ImPlot::GetColormapColor(i);
-  ImPlot::PushStyleColor(ImPlotCol_Line, m_color);
-  if (IsDigital()) {
-    ImPlot::PushStyleVar(ImPlotStyleVar_DigitalBitHeight, m_digitalBitHeight);
-    ImPlot::PushStyleVar(ImPlotStyleVar_DigitalBitGap, m_digitalBitGap);
-    ImPlot::PlotDigital(label, getter, &getterData, size + 1);
-    ImPlot::PopStyleVar();
-    ImPlot::PopStyleVar();
-  } else {
-    ImPlot::SetPlotYAxis(m_yAxis);
-    ImPlot::SetNextMarkerStyle(m_marker - 1);
-    ImPlot::PlotLine(label, getter, &getterData, size + 1);
-  }
-  ImPlot::PopStyleColor();
-
-  // DND source for PlotSeries
-  if (ImPlot::BeginLegendDragDropSource(label)) {
-    EmitDragDropPayload(i, plotIndex);
-    ImPlot::EndLegendDragDropSource();
-  }
-
-  // Plot-specific variant of IniSaverInfo::PopupEditName() that also
-  // allows editing of other settings
-  bool rv = false;
-  if (ImPlot::BeginLegendPopup(label)) {
-    if (ImGui::Button("Close")) ImGui::CloseCurrentPopup();
-    ImGui::Text("Edit name:");
-    if (InputTextName("##edit", ImGuiInputTextFlags_EnterReturnsTrue)) {
-      ImGui::CloseCurrentPopup();
-    }
-    rv = EmitSettingsDetail(i);
-    ImPlot::EndLegendPopup();
-  }
-
-  return rv;
-}
-
-void PlotSeries::EmitDragDropPayload(size_t i, size_t plotIndex) {
-  PlotSeriesRef ref = {plotIndex, i};
-  ImGui::SetDragDropPayload("PlotSeries", &ref, sizeof(ref));
-  const char* name = GetName();
-  if (name[0] == '\0' && m_newValueConn.connected()) name = m_source->GetName();
-  if (name[0] == '\0') name = m_id.c_str();
-  ImGui::TextUnformatted(name);
-}
-
-static void MovePlotSeries(size_t fromPlotIndex, size_t fromSeriesIndex,
-                           size_t toPlotIndex, size_t toSeriesIndex,
-                           int yAxis = -1) {
-  if (fromPlotIndex == toPlotIndex) {
-    // need to handle this specially as the index of the old location changes
-    if (fromSeriesIndex != toSeriesIndex) {
-      auto& plotSeries = gPlots[fromPlotIndex].m_series;
-      auto val = std::move(plotSeries[fromSeriesIndex]);
-      // only set Y-axis if actually set
-      if (yAxis != -1) val->SetYAxis(yAxis);
-      plotSeries.insert(plotSeries.begin() + toSeriesIndex, std::move(val));
-      plotSeries.erase(plotSeries.begin() + fromSeriesIndex +
-                       (fromSeriesIndex > toSeriesIndex ? 1 : 0));
-    }
-  } else {
-    auto& fromPlot = gPlots[fromPlotIndex];
-    auto& toPlot = gPlots[toPlotIndex];
-    // always set Y-axis if moving plots
-    fromPlot.m_series[fromSeriesIndex]->SetYAxis(yAxis == -1 ? 0 : yAxis);
-    toPlot.m_series.insert(toPlot.m_series.begin() + toSeriesIndex,
-                           std::move(fromPlot.m_series[fromSeriesIndex]));
-    fromPlot.m_series.erase(fromPlot.m_series.begin() + fromSeriesIndex);
-  }
-}
-
-bool PlotSeries::EmitSettings(size_t i, size_t plotIndex) {
-  char label[128];
-  GetLabel(label, sizeof(label));
-
-  bool open = ImGui::CollapsingHeader(
-      label, IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
-
-  // DND source for PlotSeries
-  if (ImGui::BeginDragDropSource()) {
-    EmitDragDropPayload(i, plotIndex);
-    ImGui::EndDragDropSource();
-  }
-
-  // If another PlotSeries is dropped, move it before this series
-  if (ImGui::BeginDragDropTarget()) {
-    if (const ImGuiPayload* payload =
-            ImGui::AcceptDragDropPayload("PlotSeries")) {
-      auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
-      MovePlotSeries(ref->plotIndex, ref->seriesIndex, plotIndex, i);
-    }
-  }
-
-  SetOpen(open);
-  PopupEditName(i);
-  if (!open) return false;
-
-  return EmitSettingsDetail(i);
-}
-
-bool PlotSeries::EmitSettingsDetail(size_t i) {
-  if (ImGui::Button("Delete")) {
-    return true;
-  }
-
-  // Line color
-  {
-    ImGui::ColorEdit3("Color", &m_color.x, ImGuiColorEditFlags_NoInputs);
-    ImGui::SameLine();
-    if (ImGui::Button("Default")) m_color = ImPlot::GetColormapColor(i);
-  }
-
-  // Digital
-  {
-    static const char* const options[] = {"Auto", "Digital", "Analog"};
-    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
-    ImGui::Combo("Digital", &m_digital, options,
-                 sizeof(options) / sizeof(options[0]));
-  }
-
-  if (IsDigital()) {
-    // Bit Height
-    {
-      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-      ImGui::InputInt("Bit Height", &m_digitalBitHeight);
-    }
-
-    // Bit Gap
-    {
-      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-      ImGui::InputInt("Bit Gap", &m_digitalBitGap);
-    }
-  } else {
-    // Y-axis
-    {
-      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 4);
-      static const char* const options[] = {"1", "2", "3"};
-      ImGui::Combo("Y-Axis", &m_yAxis, options, 3);
-    }
-
-    // Marker
-    {
-      static const char* const options[] = {
-          "None", "Circle", "Square", "Diamond", "Up",      "Down",
-          "Left", "Right",  "Cross",  "Plus",    "Asterisk"};
-      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 8);
-      ImGui::Combo("Marker", &m_marker, options,
-                   sizeof(options) / sizeof(options[0]));
-    }
-  }
-
-  return false;
-}
-
-Plot::Plot() {
-  for (int i = 0; i < 3; ++i) {
-    m_axisRange[i] = PlotRange{};
-  }
-}
-
-bool Plot::ReadIni(wpi::StringRef name, wpi::StringRef value) {
-  if (NameInfo::ReadIni(name, value)) return true;
-  if (OpenInfo::ReadIni(name, value)) return true;
-  if (name == "visible") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_visible = num != 0;
-    return true;
-  } else if (name == "lockPrevX") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_lockPrevX = num != 0;
-    return true;
-  } else if (name == "legend") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    if (num == 0)
-      m_plotFlags &= ~ImPlotFlags_Legend;
-    else
-      m_plotFlags |= ImPlotFlags_Legend;
-    return true;
-  } else if (name == "yaxis2") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    if (num == 0)
-      m_plotFlags &= ~ImPlotFlags_YAxis2;
-    else
-      m_plotFlags |= ImPlotFlags_YAxis2;
-    return true;
-  } else if (name == "yaxis3") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    if (num == 0)
-      m_plotFlags &= ~ImPlotFlags_YAxis3;
-    else
-      m_plotFlags |= ImPlotFlags_YAxis3;
-    return true;
-  } else if (name == "viewTime") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_viewTime = num / 1000.0;
-    return true;
-  } else if (name == "height") {
-    int num;
-    if (value.getAsInteger(10, num)) return true;
-    m_height = num;
-    return true;
-  } else if (name.startswith("y")) {
-    auto [yAxisStr, yName] = name.split('_');
-    int yAxis;
-    if (yAxisStr.substr(1).getAsInteger(10, yAxis)) return false;
-    if (yAxis < 0 || yAxis > 3) return false;
-    if (yName == "min") {
-      int num;
-      if (value.getAsInteger(10, num)) return true;
-      m_axisRange[yAxis].min = num / 1000.0;
-      return true;
-    } else if (yName == "max") {
-      int num;
-      if (value.getAsInteger(10, num)) return true;
-      m_axisRange[yAxis].max = num / 1000.0;
-      return true;
-    } else if (yName == "lockMin") {
-      int num;
-      if (value.getAsInteger(10, num)) return true;
-      m_axisRange[yAxis].lockMin = num != 0;
-      return true;
-    } else if (yName == "lockMax") {
-      int num;
-      if (value.getAsInteger(10, num)) return true;
-      m_axisRange[yAxis].lockMax = num != 0;
-      return true;
-    }
-  }
-  return false;
-}
-
-void Plot::WriteIni(ImGuiTextBuffer* out) {
-  NameInfo::WriteIni(out);
-  OpenInfo::WriteIni(out);
-  out->appendf(
-      "visible=%d\nlockPrevX=%d\nlegend=%d\nyaxis2=%d\nyaxis3=%d\n"
-      "viewTime=%d\nheight=%d\n",
-      m_visible ? 1 : 0, m_lockPrevX ? 1 : 0,
-      (m_plotFlags & ImPlotFlags_Legend) ? 1 : 0,
-      (m_plotFlags & ImPlotFlags_YAxis2) ? 1 : 0,
-      (m_plotFlags & ImPlotFlags_YAxis3) ? 1 : 0,
-      static_cast<int>(m_viewTime * 1000), m_height);
-  for (int i = 0; i < 3; ++i) {
-    out->appendf("y%d_min=%d\ny%d_max=%d\ny%d_lockMin=%d\ny%d_lockMax=%d\n", i,
-                 static_cast<int>(m_axisRange[i].min * 1000), i,
-                 static_cast<int>(m_axisRange[i].max * 1000), i,
-                 m_axisRange[i].lockMin ? 1 : 0, i,
-                 m_axisRange[i].lockMax ? 1 : 0);
-  }
-}
-
-void Plot::GetLabel(char* buf, size_t size, int index) const {
-  const char* name = NameInfo::GetName();
-  if (name[0] != '\0') {
-    std::snprintf(buf, size, "%s##Plot%d", name, index);
-  } else {
-    std::snprintf(buf, size, "Plot %d##Plot%d", index, index);
-  }
-}
-
-void Plot::GetName(char* buf, size_t size, int index) const {
-  const char* name = NameInfo::GetName();
-  if (name[0] != '\0') {
-    std::snprintf(buf, size, "%s", name);
-  } else {
-    std::snprintf(buf, size, "Plot %d", index);
-  }
-}
-
-void Plot::DragDropTarget(size_t i, bool inPlot) {
-  if (!ImGui::BeginDragDropTarget()) return;
-  // handle dragging onto a specific Y axis
-  int yAxis = -1;
-  if (inPlot) {
-    for (int y = 0; y < 3; ++y) {
-      if (ImPlot::IsPlotYAxisHovered(y)) {
-        yAxis = y;
-        break;
-      }
-    }
-  }
-  if (const ImGuiPayload* payload =
-          ImGui::AcceptDragDropPayload("DataSource")) {
-    auto source = *static_cast<GuiDataSource**>(payload->Data);
-    // don't add duplicates unless it's onto a different Y axis
-    auto it =
-        std::find_if(m_series.begin(), m_series.end(), [=](const auto& elem) {
-          return elem->GetId() == source->GetId() &&
-                 (yAxis == -1 || elem->GetYAxis() == yAxis);
-        });
-    if (it == m_series.end()) {
-      m_series.emplace_back(
-          std::make_unique<PlotSeries>(source, yAxis == -1 ? 0 : yAxis));
-    }
-  } else if (const ImGuiPayload* payload =
-                 ImGui::AcceptDragDropPayload("PlotSeries")) {
-    auto ref = static_cast<const PlotSeriesRef*>(payload->Data);
-    MovePlotSeries(ref->plotIndex, ref->seriesIndex, i, m_series.size(), yAxis);
-  } else if (const ImGuiPayload* payload =
-                 ImGui::AcceptDragDropPayload("Plot")) {
-    auto fromPlotIndex = *static_cast<const size_t*>(payload->Data);
-    if (i != fromPlotIndex) {
-      auto val = std::move(gPlots[fromPlotIndex]);
-      gPlots.insert(gPlots.begin() + i, std::move(val));
-      gPlots.erase(gPlots.begin() + fromPlotIndex +
-                   (fromPlotIndex > i ? 1 : 0));
-    }
-  }
-}
-
-void Plot::EmitPlot(double now, size_t i) {
-  if (!m_visible) return;
-
-  bool lockX = (i != 0 && m_lockPrevX);
-
-  if (!lockX && ImGui::Button(m_paused ? "Resume" : "Pause"))
-    m_paused = !m_paused;
-
-  char label[128];
-  GetLabel(label, sizeof(label), i);
-
-  if (lockX) {
-    ImPlot::SetNextPlotLimitsX(gPlots[i - 1].m_xaxisRange.Min,
-                               gPlots[i - 1].m_xaxisRange.Max,
-                               ImGuiCond_Always);
-  } else {
-    // also force-pause plots if overall timing is paused
-    ImPlot::SetNextPlotLimitsX(now - m_viewTime, now,
-                               (m_paused || HALSIM_IsTimingPaused())
-                                   ? ImGuiCond_Once
-                                   : ImGuiCond_Always);
-  }
-
-  ImPlotAxisFlags yFlags[3] = {ImPlotAxisFlags_Default,
-                               ImPlotAxisFlags_Auxiliary,
-                               ImPlotAxisFlags_Auxiliary};
-  for (int i = 0; i < 3; ++i) {
-    ImPlot::SetNextPlotLimitsY(
-        m_axisRange[i].min, m_axisRange[i].max,
-        m_axisRange[i].apply ? ImGuiCond_Always : ImGuiCond_Once, i);
-    m_axisRange[i].apply = false;
-    if (m_axisRange[i].lockMin) yFlags[i] |= ImPlotAxisFlags_LockMin;
-    if (m_axisRange[i].lockMax) yFlags[i] |= ImPlotAxisFlags_LockMax;
-  }
-
-  if (ImPlot::BeginPlot(label, nullptr, nullptr, ImVec2(-1, m_height),
-                        m_plotFlags, ImPlotAxisFlags_Default, yFlags[0],
-                        yFlags[1], yFlags[2])) {
-    for (size_t j = 0; j < m_series.size(); ++j) {
-      if (m_series[j]->EmitPlot(now, j, i)) {
-        m_series.erase(m_series.begin() + j);
-      }
-    }
-    DragDropTarget(i, true);
-    m_xaxisRange = ImPlot::GetPlotLimits().X;
-    ImPlot::EndPlot();
-  }
-}
-
-void Plot::EmitSettingsLimits(int axis) {
-  ImGui::Indent();
-  ImGui::PushID(axis);
-
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 3.5);
-  ImGui::InputDouble("Min", &m_axisRange[axis].min, 0, 0, "%.3f");
-  ImGui::SameLine();
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 3.5);
-  ImGui::InputDouble("Max", &m_axisRange[axis].max, 0, 0, "%.3f");
-  ImGui::SameLine();
-  if (ImGui::Button("Apply")) m_axisRange[axis].apply = true;
-
-  ImGui::TextUnformatted("Lock Axis");
-  ImGui::SameLine();
-  ImGui::Checkbox("Min##minlock", &m_axisRange[axis].lockMin);
-  ImGui::SameLine();
-  ImGui::Checkbox("Max##maxlock", &m_axisRange[axis].lockMax);
-
-  ImGui::PopID();
-  ImGui::Unindent();
-}
-
-// Delete button (X in circle), based on ImGui::CloseButton()
-static bool DeleteButton(ImGuiID id, const ImVec2& pos) {
-  ImGuiContext& g = *GImGui;
-  ImGuiWindow* window = g.CurrentWindow;
-
-  // We intentionally allow interaction when clipped so that a mechanical
-  // Alt,Right,Validate sequence close a window. (this isn't the regular
-  // behavior of buttons, but it doesn't affect the user much because navigation
-  // tends to keep items visible).
-  const ImRect bb(
-      pos, pos + ImVec2(g.FontSize, g.FontSize) + g.Style.FramePadding * 2.0f);
-  bool is_clipped = !ImGui::ItemAdd(bb, id);
-
-  bool hovered, held;
-  bool pressed = ImGui::ButtonBehavior(bb, id, &hovered, &held);
-  if (is_clipped) return pressed;
-
-  // Render
-  ImU32 col =
-      ImGui::GetColorU32(held ? ImGuiCol_ButtonActive : ImGuiCol_ButtonHovered);
-  ImVec2 center = bb.GetCenter();
-  if (hovered)
-    window->DrawList->AddCircleFilled(
-        center, ImMax(2.0f, g.FontSize * 0.5f + 1.0f), col, 12);
-
-  ImU32 cross_col = ImGui::GetColorU32(ImGuiCol_Text);
-  window->DrawList->AddCircle(center, ImMax(2.0f, g.FontSize * 0.5f + 1.0f),
-                              cross_col, 12);
-  float cross_extent = g.FontSize * 0.5f * 0.5f - 1.0f;
-  center -= ImVec2(0.5f, 0.5f);
-  window->DrawList->AddLine(center + ImVec2(+cross_extent, +cross_extent),
-                            center + ImVec2(-cross_extent, -cross_extent),
-                            cross_col, 1.0f);
-  window->DrawList->AddLine(center + ImVec2(+cross_extent, -cross_extent),
-                            center + ImVec2(-cross_extent, +cross_extent),
-                            cross_col, 1.0f);
-
-  return pressed;
-}
-
-void Plot::EmitSettings(size_t i) {
-  char label[128];
-  GetLabel(label, sizeof(label), i);
-
-  bool open = ImGui::CollapsingHeader(
-      label, ImGuiTreeNodeFlags_AllowItemOverlap |
-                 ImGuiTreeNodeFlags_ClipLabelForTrailingButton |
-                 (IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0));
-
-  {
-    // Create a small overlapping delete button
-    ImGuiWindow* window = ImGui::GetCurrentWindow();
-    ImGuiContext& g = *GImGui;
-    ImGuiItemHoveredDataBackup last_item_backup;
-    ImGuiID id = window->GetID(label);
-    float button_size = g.FontSize;
-    float button_x = ImMax(window->DC.LastItemRect.Min.x,
-                           window->DC.LastItemRect.Max.x -
-                               g.Style.FramePadding.x * 2.0f - button_size);
-    float button_y = window->DC.LastItemRect.Min.y;
-    if (DeleteButton(window->GetID(reinterpret_cast<void*>(
-                         static_cast<intptr_t>(id) + 1)),
-                     ImVec2(button_x, button_y))) {
-      gPlots.erase(gPlots.begin() + i);
-      return;
-    }
-    last_item_backup.Restore();
-  }
-
-  // DND source for Plot
-  if (ImGui::BeginDragDropSource()) {
-    ImGui::SetDragDropPayload("Plot", &i, sizeof(i));
-    char name[64];
-    GetName(name, sizeof(name), i);
-    ImGui::TextUnformatted(name);
-    ImGui::EndDragDropSource();
-  }
-  DragDropTarget(i, false);
-  SetOpen(open);
-  PopupEditName(i);
-  if (!open) return;
-  ImGui::PushID(i);
-#if 0
-  if (ImGui::Button("Move Up") && i > 0) {
-    std::swap(gPlots[i - 1], gPlots[i]);
-    ImGui::PopID();
-    return;
-  }
-  ImGui::SameLine();
-  if (ImGui::Button("Move Down") && i < (gPlots.size() - 1)) {
-    std::swap(gPlots[i], gPlots[i + 1]);
-    ImGui::PopID();
-    return;
-  }
-  ImGui::SameLine();
-  if (ImGui::Button("Delete")) {
-    gPlots.erase(gPlots.begin() + i);
-    ImGui::PopID();
-    return;
-  }
-#endif
-  ImGui::Checkbox("Visible", &m_visible);
-  ImGui::CheckboxFlags("Show Legend", &m_plotFlags, ImPlotFlags_Legend);
-  if (i != 0) ImGui::Checkbox("Lock X-axis to previous plot", &m_lockPrevX);
-  ImGui::TextUnformatted("Primary Y-Axis");
-  EmitSettingsLimits(0);
-  ImGui::CheckboxFlags("2nd Y-Axis", &m_plotFlags, ImPlotFlags_YAxis2);
-  if ((m_plotFlags & ImPlotFlags_YAxis2) != 0) EmitSettingsLimits(1);
-  ImGui::CheckboxFlags("3rd Y-Axis", &m_plotFlags, ImPlotFlags_YAxis3);
-  if ((m_plotFlags & ImPlotFlags_YAxis3) != 0) EmitSettingsLimits(2);
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
-  ImGui::InputFloat("View Time (s)", &m_viewTime, 0.1f, 1.0f, "%.1f");
-  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 6);
-  if (ImGui::InputInt("Height", &m_height, 10)) {
-    if (m_height < 0) m_height = 0;
-  }
-
-  ImGui::Indent();
-  for (size_t j = 0; j < m_series.size(); ++j) {
-    ImGui::PushID(j);
-    if (m_series[j]->EmitSettings(j, i)) {
-      m_series.erase(m_series.begin() + j);
-    }
-    ImGui::PopID();
-  }
-  ImGui::Unindent();
-
-  ImGui::PopID();
-}
-
-static void DisplayPlot() {
-  if (gPlots.empty()) {
-    ImGui::Text("No Plots");
-    return;
-  }
-  double now = wpi::Now() * 1.0e-6;
-  for (size_t i = 0; i < gPlots.size(); ++i) {
-    ImGui::PushID(i);
-    gPlots[i].EmitPlot(now, i);
-    ImGui::PopID();
-  }
-  ImGui::Text("(Right double click for more settings)");
-}
-
-static void DisplayPlotSettings() {
-  if (ImGui::Button("Add new plot")) {
-    gPlots.emplace_back();
-  }
-  for (size_t i = 0; i < gPlots.size(); ++i) {
-    gPlots[i].EmitSettings(i);
-  }
-}
-
-static void* PlotSeries_ReadOpen(ImGuiContext* ctx,
-                                 ImGuiSettingsHandler* handler,
-                                 const char* name) {
-  wpi::StringRef plotIndexStr, id;
-  std::tie(plotIndexStr, id) = wpi::StringRef{name}.split(',');
-  unsigned int plotIndex;
-  if (plotIndexStr.getAsInteger(10, plotIndex)) return nullptr;
-  if (plotIndex >= gPlots.size()) gPlots.resize(plotIndex + 1);
-  auto& plot = gPlots[plotIndex];
-  auto it = std::find_if(
-      plot.m_series.begin(), plot.m_series.end(),
-      [&](const auto& elem) { return elem && elem->GetId() == id; });
-  if (it != plot.m_series.end()) return it->get();
-  return plot.m_series.emplace_back(std::make_unique<PlotSeries>(id)).get();
-}
-
-static void PlotSeries_ReadLine(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler, void* entry,
-                                const char* lineStr) {
-  auto element = static_cast<PlotSeries*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  element->ReadIni(name, value);
-}
-
-static void PlotSeries_WriteAll(ImGuiContext* ctx,
-                                ImGuiSettingsHandler* handler,
-                                ImGuiTextBuffer* out_buf) {
-  for (size_t i = 0; i < gPlots.size(); ++i) {
-    for (const auto& series : gPlots[i].m_series) {
-      out_buf->appendf("[PlotSeries][%d,%s]\n", static_cast<int>(i),
-                       series->GetId().c_str());
-      series->WriteIni(out_buf);
-      out_buf->append("\n");
-    }
-  }
-}
-
-void PlotGui::Initialize() {
-  gPlots.Initialize();
-
-  // hook ini handler for PlotSeries to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = "PlotSeries";
-  iniHandler.TypeHash = ImHashStr("PlotSeries");
-  iniHandler.ReadOpenFn = PlotSeries_ReadOpen;
-  iniHandler.ReadLineFn = PlotSeries_ReadLine;
-  iniHandler.WriteAllFn = PlotSeries_WriteAll;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-
-  // HALSimGui::AddExecute([] { ImPlot::ShowDemoWindow(); });
-  HALSimGui::AddWindow("Plot", DisplayPlot);
-  HALSimGui::SetDefaultWindowPos("Plot", 600, 75);
-  HALSimGui::SetDefaultWindowSize("Plot", 300, 200);
-
-  HALSimGui::AddWindow("Plot Settings", DisplayPlotSettings);
-  HALSimGui::SetDefaultWindowPos("Plot Settings", 902, 75);
-  HALSimGui::SetDefaultWindowSize("Plot Settings", 120, 200);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PlotGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PlotGui.h
deleted file mode 100644
index 6bbd337..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PlotGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-class PlotGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp
new file mode 100644
index 0000000..c511345
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.cpp
@@ -0,0 +1,137 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "PowerDistributionSimGui.h"
+
+#include <glass/hardware/PowerDistribution.h>
+
+#include <cstdio>
+#include <cstring>
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/simulation/PowerDistributionData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PowerDistributionTemperature,
+                                    "Power Distribution Temp");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(PowerDistributionVoltage,
+                                    "Power Distribution Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE_INDEXED2(PowerDistributionCurrent,
+                                     "Power Distribution Current");
+
+class PowerDistributionSimModel : public glass::PowerDistributionModel {
+ public:
+  explicit PowerDistributionSimModel(int32_t index)
+      : m_index{index}, m_temp{index}, m_voltage{index} {
+    // TODO make this better
+    const int numChannels = HAL_GetNumREVPDHChannels();
+    m_currents.reserve(numChannels);
+    for (int i = 0; i < numChannels; ++i) {
+      m_currents.emplace_back(
+          std::make_unique<PowerDistributionCurrentSource>(index, i));
+    }
+  }
+
+  void Update() override {}
+
+  bool Exists() override {
+    return HALSIM_GetPowerDistributionInitialized(m_index);
+  }
+
+  int GetNumChannels() const override { return m_currents.size(); }
+
+  glass::DataSource* GetTemperatureData() override { return &m_temp; }
+  glass::DataSource* GetVoltageData() override { return &m_voltage; }
+  glass::DataSource* GetCurrentData(int channel) override {
+    return m_currents[channel].get();
+  }
+
+  void SetTemperature(double val) override {
+    HALSIM_SetPowerDistributionTemperature(m_index, val);
+  }
+  void SetVoltage(double val) override {
+    HALSIM_SetPowerDistributionVoltage(m_index, val);
+  }
+  void SetCurrent(int channel, double val) override {
+    HALSIM_SetPowerDistributionCurrent(m_index, channel, val);
+  }
+
+ private:
+  int32_t m_index;
+  PowerDistributionTemperatureSource m_temp;
+  PowerDistributionVoltageSource m_voltage;
+  std::vector<std::unique_ptr<PowerDistributionCurrentSource>> m_currents;
+};
+
+class PowerDistributionsSimModel : public glass::PowerDistributionsModel {
+ public:
+  PowerDistributionsSimModel() : m_models(HAL_GetNumREVPDHModules()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachPowerDistribution(
+      wpi::function_ref<void(glass::PowerDistributionModel& model, int index)>
+          func) override;
+
+ private:
+  std::vector<std::unique_ptr<PowerDistributionSimModel>> m_models;
+};
+}  // namespace
+
+void PowerDistributionsSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetPowerDistributionInitialized(i)) {
+      if (!model) {
+        model = std::make_unique<PowerDistributionSimModel>(i);
+      }
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void PowerDistributionsSimModel::ForEachPowerDistribution(
+    wpi::function_ref<void(glass::PowerDistributionModel& model, int index)>
+        func) {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool PowerDistributionsAnyInitialized() {
+  static const int32_t num = HAL_GetNumREVPDHModules();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetPowerDistributionInitialized(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void PowerDistributionSimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "PowerDistributions", PowerDistributionsAnyInitialized,
+      [] { return std::make_unique<PowerDistributionsSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetDefaultPos(245, 155);
+        return glass::MakeFunctionView([=] {
+          DisplayPowerDistributions(
+              static_cast<PowerDistributionsSimModel*>(model));
+        });
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.h
new file mode 100644
index 0000000..10a78cc
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/PowerDistributionSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class PowerDistributionSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp
deleted file mode 100644
index 9171678..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelayGui.cpp
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "RelayGui.h"
-
-#include <cstdio>
-#include <cstring>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/RelayData.h>
-#include <imgui.h>
-
-#include "ExtraGuiWidgets.h"
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(RelayForward, "RelayFwd");
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(RelayReverse, "RelayRev");
-}  // namespace
-
-static IniSaver<NameInfo> gRelays{"Relay"};
-static std::vector<std::unique_ptr<RelayForwardSource>> gRelayForwardSources;
-static std::vector<std::unique_ptr<RelayReverseSource>> gRelayReverseSources;
-
-static void UpdateRelaySources() {
-  for (int i = 0, iend = gRelayForwardSources.size(); i < iend; ++i) {
-    auto& source = gRelayForwardSources[i];
-    if (HALSIM_GetRelayInitializedForward(i)) {
-      if (!source) {
-        source = std::make_unique<RelayForwardSource>(i);
-        source->SetName(gRelays[i].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-  for (int i = 0, iend = gRelayReverseSources.size(); i < iend; ++i) {
-    auto& source = gRelayReverseSources[i];
-    if (HALSIM_GetRelayInitializedReverse(i)) {
-      if (!source) {
-        source = std::make_unique<RelayReverseSource>(i);
-        source->SetName(gRelays[i].GetName());
-      }
-    } else {
-      source.reset();
-    }
-  }
-}
-
-static void DisplayRelays() {
-  bool hasOutputs = false;
-  bool first = true;
-  for (int i = 0, iend = gRelayForwardSources.size(); i < iend; ++i) {
-    auto forwardSource = gRelayForwardSources[i].get();
-    auto reverseSource = gRelayReverseSources[i].get();
-
-    if (forwardSource || reverseSource) {
-      hasOutputs = true;
-
-      if (!first)
-        ImGui::Separator();
-      else
-        first = false;
-
-      bool forward = false;
-      bool reverse = false;
-      if (!HALSimGui::AreOutputsDisabled()) {
-        if (forwardSource) forward = forwardSource->GetValue();
-        if (reverseSource) reverse = reverseSource->GetValue();
-      }
-
-      auto& info = gRelays[i];
-      info.PushEditNameId(i);
-      if (info.HasName())
-        ImGui::Text("%s [%d]", info.GetName(), i);
-      else
-        ImGui::Text("Relay[%d]", i);
-      ImGui::PopID();
-      if (info.PopupEditName(i)) {
-        if (forwardSource) forwardSource->SetName(info.GetName());
-        if (reverseSource) reverseSource->SetName(info.GetName());
-      }
-      ImGui::SameLine();
-
-      // show forward and reverse as LED indicators
-      static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
-                                     IM_COL32(255, 0, 0, 255),
-                                     IM_COL32(128, 128, 128, 255)};
-      int values[2] = {reverseSource ? (reverse ? 2 : -2) : -3,
-                       forwardSource ? (forward ? 1 : -1) : -3};
-      GuiDataSource* sources[2] = {reverseSource, forwardSource};
-      ImGui::PushID(i);
-      DrawLEDSources(values, sources, 2, 2, colors);
-      ImGui::PopID();
-    }
-  }
-  if (!hasOutputs) ImGui::Text("No relays");
-}
-
-void RelayGui::Initialize() {
-  gRelays.Initialize();
-  int numRelays = HAL_GetNumRelayHeaders();
-  gRelayForwardSources.resize(numRelays);
-  gRelayReverseSources.resize(numRelays);
-  HALSimGui::AddExecute(UpdateRelaySources);
-  HALSimGui::AddWindow("Relays", DisplayRelays,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("Relays", 180, 20);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelayGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelayGui.h
deleted file mode 100644
index ccc2fb6..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelayGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class RelayGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelaySimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelaySimGui.cpp
new file mode 100644
index 0000000..3815617
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelaySimGui.cpp
@@ -0,0 +1,118 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "RelaySimGui.h"
+
+#include <glass/hardware/Relay.h>
+
+#include <memory>
+#include <vector>
+
+#include <hal/Ports.h>
+#include <hal/simulation/RelayData.h>
+#include <wpigui.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(RelayForward, "RelayFwd");
+HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(RelayReverse, "RelayRev");
+
+class RelaySimModel : public glass::RelayModel {
+ public:
+  explicit RelaySimModel(int32_t index)
+      : m_index{index}, m_forward{index}, m_reverse{index} {}
+
+  void Update() override {}
+
+  bool Exists() override {
+    return HALSIM_GetRelayInitializedForward(m_index) ||
+           HALSIM_GetRelayInitializedReverse(m_index);
+  }
+
+  glass::DataSource* GetForwardData() override {
+    return HALSIM_GetRelayInitializedForward(m_index) ? &m_forward : nullptr;
+  }
+  glass::DataSource* GetReverseData() override {
+    return HALSIM_GetRelayInitializedReverse(m_index) ? &m_reverse : nullptr;
+  }
+
+  void SetForward(bool val) override { HALSIM_SetRelayForward(m_index, val); }
+  void SetReverse(bool val) override { HALSIM_SetRelayReverse(m_index, val); }
+
+ private:
+  int32_t m_index;
+  RelayForwardSource m_forward;
+  RelayReverseSource m_reverse;
+};
+
+class RelaysSimModel : public glass::RelaysModel {
+ public:
+  RelaysSimModel() : m_models(HAL_GetNumRelayHeaders()) {}
+
+  void Update() override;
+
+  bool Exists() override { return true; }
+
+  void ForEachRelay(wpi::function_ref<void(glass::RelayModel& model, int index)>
+                        func) override;
+
+ private:
+  // indexed by channel
+  std::vector<std::unique_ptr<RelaySimModel>> m_models;
+};
+}  // namespace
+
+void RelaysSimModel::Update() {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    auto& model = m_models[i];
+    if (HALSIM_GetRelayInitializedForward(i) ||
+        HALSIM_GetRelayInitializedReverse(i)) {
+      if (!model) {
+        model = std::make_unique<RelaySimModel>(i);
+      }
+    } else {
+      model.reset();
+    }
+  }
+}
+
+void RelaysSimModel::ForEachRelay(
+    wpi::function_ref<void(glass::RelayModel& model, int index)> func) {
+  for (int32_t i = 0, iend = static_cast<int32_t>(m_models.size()); i < iend;
+       ++i) {
+    if (auto model = m_models[i].get()) {
+      func(*model, i);
+    }
+  }
+}
+
+static bool RelayAnyInitialized() {
+  static const int32_t num = HAL_GetNumRelayHeaders();
+  for (int32_t i = 0; i < num; ++i) {
+    if (HALSIM_GetRelayInitializedForward(i) ||
+        HALSIM_GetRelayInitializedReverse(i)) {
+      return true;
+    }
+  }
+  return false;
+}
+
+void RelaySimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "Relays", RelayAnyInitialized,
+      [] { return std::make_unique<RelaysSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(180, 20);
+        return glass::MakeFunctionView([=] {
+          glass::DisplayRelays(static_cast<RelaysSimModel*>(model),
+                               HALSimGui::halProvider.AreOutputsEnabled());
+        });
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelaySimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelaySimGui.h
new file mode 100644
index 0000000..cb27057
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RelaySimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class RelaySimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp
deleted file mode 100644
index b4f8909..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.cpp
+++ /dev/null
@@ -1,170 +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 "RoboRioGui.h"
-
-#include <memory>
-
-#include <hal/simulation/RoboRioData.h>
-#include <imgui.h>
-
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioVInVoltage, "Rio Input Voltage");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioVInCurrent, "Rio Input Current");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage6V, "Rio 6V Voltage");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent6V, "Rio 6V Current");
-HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive6V, "Rio 6V Active");
-HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults6V, "Rio 6V Faults");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage5V, "Rio 5V Voltage");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent5V, "Rio 5V Current");
-HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive5V, "Rio 5V Active");
-HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults5V, "Rio 5V Faults");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage3V3, "Rio 3.3V Voltage");
-HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent3V3, "Rio 3.3V Current");
-HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive3V3, "Rio 3.3V Active");
-HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults3V3, "Rio 3.3V Faults");
-struct RoboRioSource {
-  RoboRioVInVoltageSource vInVoltage;
-  RoboRioVInCurrentSource vInCurrent;
-  RoboRioUserVoltage6VSource userVoltage6V;
-  RoboRioUserCurrent6VSource userCurrent6V;
-  RoboRioUserActive6VSource userActive6V;
-  RoboRioUserFaults6VSource userFaults6V;
-  RoboRioUserVoltage5VSource userVoltage5V;
-  RoboRioUserCurrent5VSource userCurrent5V;
-  RoboRioUserActive5VSource userActive5V;
-  RoboRioUserFaults5VSource userFaults5V;
-  RoboRioUserVoltage3V3Source userVoltage3V3;
-  RoboRioUserCurrent3V3Source userCurrent3V3;
-  RoboRioUserActive3V3Source userActive3V3;
-  RoboRioUserFaults3V3Source userFaults3V3;
-};
-}  // namespace
-
-static std::unique_ptr<RoboRioSource> gRioSource;
-
-static void UpdateRoboRioSources() {
-  if (!gRioSource) gRioSource = std::make_unique<RoboRioSource>();
-}
-
-static void DisplayRoboRio() {
-  ImGui::Button("User Button");
-  HALSIM_SetRoboRioFPGAButton(ImGui::IsItemActive());
-
-  ImGui::PushItemWidth(ImGui::GetFontSize() * 8);
-
-  if (ImGui::CollapsingHeader("RoboRIO Input")) {
-    {
-      double val = gRioSource->vInVoltage.GetValue();
-      if (gRioSource->vInVoltage.InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioVInVoltage(val);
-    }
-
-    {
-      double val = gRioSource->vInCurrent.GetValue();
-      if (gRioSource->vInCurrent.InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioVInCurrent(val);
-    }
-  }
-
-  if (ImGui::CollapsingHeader("6V Rail")) {
-    {
-      double val = gRioSource->userVoltage6V.GetValue();
-      if (gRioSource->userVoltage6V.InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioUserVoltage6V(val);
-    }
-
-    {
-      double val = gRioSource->userCurrent6V.GetValue();
-      if (gRioSource->userCurrent6V.InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioUserCurrent6V(val);
-    }
-
-    {
-      static const char* options[] = {"inactive", "active"};
-      int val = gRioSource->userActive6V.GetValue() ? 1 : 0;
-      if (gRioSource->userActive6V.Combo("Active", &val, options, 2))
-        HALSIM_SetRoboRioUserActive6V(val);
-    }
-
-    {
-      int val = gRioSource->userFaults6V.GetValue();
-      if (gRioSource->userFaults6V.InputInt("Faults", &val))
-        HALSIM_SetRoboRioUserFaults6V(val);
-    }
-  }
-
-  if (ImGui::CollapsingHeader("5V Rail")) {
-    {
-      double val = gRioSource->userVoltage5V.GetValue();
-      if (gRioSource->userVoltage5V.InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioUserVoltage5V(val);
-    }
-
-    {
-      double val = gRioSource->userCurrent5V.GetValue();
-      if (gRioSource->userCurrent5V.InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioUserCurrent5V(val);
-    }
-
-    {
-      static const char* options[] = {"inactive", "active"};
-      int val = gRioSource->userActive5V.GetValue() ? 1 : 0;
-      if (gRioSource->userActive5V.Combo("Active", &val, options, 2))
-        HALSIM_SetRoboRioUserActive5V(val);
-    }
-
-    {
-      int val = gRioSource->userFaults5V.GetValue();
-      if (gRioSource->userFaults5V.InputInt("Faults", &val))
-        HALSIM_SetRoboRioUserFaults5V(val);
-    }
-  }
-
-  if (ImGui::CollapsingHeader("3.3V Rail")) {
-    {
-      double val = gRioSource->userVoltage3V3.GetValue();
-      if (gRioSource->userVoltage3V3.InputDouble("Voltage (V)", &val))
-        HALSIM_SetRoboRioUserVoltage3V3(val);
-    }
-
-    {
-      double val = gRioSource->userCurrent3V3.GetValue();
-      if (gRioSource->userCurrent3V3.InputDouble("Current (A)", &val))
-        HALSIM_SetRoboRioUserCurrent3V3(val);
-    }
-
-    {
-      static const char* options[] = {"inactive", "active"};
-      int val = HALSIM_GetRoboRioUserActive3V3() ? 1 : 0;
-      if (gRioSource->userActive3V3.Combo("Active", &val, options, 2))
-        HALSIM_SetRoboRioUserActive3V3(val);
-    }
-
-    {
-      int val = gRioSource->userFaults3V3.GetValue();
-      if (gRioSource->userFaults3V3.InputInt("Faults", &val))
-        HALSIM_SetRoboRioUserFaults3V3(val);
-    }
-  }
-
-  ImGui::PopItemWidth();
-}
-
-void RoboRioGui::Initialize() {
-  HALSimGui::AddExecute(UpdateRoboRioSources);
-  HALSimGui::AddWindow("RoboRIO", DisplayRoboRio,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  // hide it by default
-  HALSimGui::SetWindowVisibility("RoboRIO", HALSimGui::kHide);
-  HALSimGui::SetDefaultWindowPos("RoboRIO", 5, 125);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.h
deleted file mode 100644
index 603abf0..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class RoboRioGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.cpp
new file mode 100644
index 0000000..ba13a2a
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.cpp
@@ -0,0 +1,144 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "RoboRioSimGui.h"
+
+#include <glass/hardware/RoboRio.h>
+
+#include <memory>
+
+#include <hal/simulation/RoboRioData.h>
+
+#include "HALDataSource.h"
+#include "HALSimGui.h"
+
+using namespace halsimgui;
+
+namespace {
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioFPGAButton, "Rio User Button");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioVInVoltage, "Rio Input Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioVInCurrent, "Rio Input Current");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage6V, "Rio 6V Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent6V, "Rio 6V Current");
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive6V, "Rio 6V Active");
+HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults6V, "Rio 6V Faults");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage5V, "Rio 5V Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent5V, "Rio 5V Current");
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive5V, "Rio 5V Active");
+HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults5V, "Rio 5V Faults");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserVoltage3V3, "Rio 3.3V Voltage");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioUserCurrent3V3, "Rio 3.3V Current");
+HALSIMGUI_DATASOURCE_BOOLEAN(RoboRioUserActive3V3, "Rio 3.3V Active");
+HALSIMGUI_DATASOURCE_INT(RoboRioUserFaults3V3, "Rio 3.3V Faults");
+HALSIMGUI_DATASOURCE_DOUBLE(RoboRioBrownoutVoltage, "Rio Brownout Voltage");
+
+class RoboRioUser6VRailSimModel : public glass::RoboRioRailModel {
+ public:
+  void Update() override {}
+  bool Exists() override { return true; }
+  glass::DataSource* GetVoltageData() override { return &m_voltage; }
+  glass::DataSource* GetCurrentData() override { return &m_current; }
+  glass::DataSource* GetActiveData() override { return &m_active; }
+  glass::DataSource* GetFaultsData() override { return &m_faults; }
+
+  void SetVoltage(double val) override { HALSIM_SetRoboRioUserVoltage6V(val); }
+  void SetCurrent(double val) override { HALSIM_SetRoboRioUserCurrent6V(val); }
+  void SetActive(bool val) override { HALSIM_SetRoboRioUserActive6V(val); }
+  void SetFaults(int val) override { HALSIM_SetRoboRioUserFaults6V(val); }
+
+ private:
+  RoboRioUserVoltage6VSource m_voltage;
+  RoboRioUserCurrent6VSource m_current;
+  RoboRioUserActive6VSource m_active;
+  RoboRioUserFaults6VSource m_faults;
+};
+
+class RoboRioUser5VRailSimModel : public glass::RoboRioRailModel {
+ public:
+  void Update() override {}
+  bool Exists() override { return true; }
+  glass::DataSource* GetVoltageData() override { return &m_voltage; }
+  glass::DataSource* GetCurrentData() override { return &m_current; }
+  glass::DataSource* GetActiveData() override { return &m_active; }
+  glass::DataSource* GetFaultsData() override { return &m_faults; }
+
+  void SetVoltage(double val) override { HALSIM_SetRoboRioUserVoltage5V(val); }
+  void SetCurrent(double val) override { HALSIM_SetRoboRioUserCurrent5V(val); }
+  void SetActive(bool val) override { HALSIM_SetRoboRioUserActive5V(val); }
+  void SetFaults(int val) override { HALSIM_SetRoboRioUserFaults5V(val); }
+
+ private:
+  RoboRioUserVoltage5VSource m_voltage;
+  RoboRioUserCurrent5VSource m_current;
+  RoboRioUserActive5VSource m_active;
+  RoboRioUserFaults5VSource m_faults;
+};
+
+class RoboRioUser3V3RailSimModel : public glass::RoboRioRailModel {
+ public:
+  void Update() override {}
+  bool Exists() override { return true; }
+  glass::DataSource* GetVoltageData() override { return &m_voltage; }
+  glass::DataSource* GetCurrentData() override { return &m_current; }
+  glass::DataSource* GetActiveData() override { return &m_active; }
+  glass::DataSource* GetFaultsData() override { return &m_faults; }
+
+  void SetVoltage(double val) override { HALSIM_SetRoboRioUserVoltage3V3(val); }
+  void SetCurrent(double val) override { HALSIM_SetRoboRioUserCurrent3V3(val); }
+  void SetActive(bool val) override { HALSIM_SetRoboRioUserActive3V3(val); }
+  void SetFaults(int val) override { HALSIM_SetRoboRioUserFaults3V3(val); }
+
+ private:
+  RoboRioUserVoltage3V3Source m_voltage;
+  RoboRioUserCurrent3V3Source m_current;
+  RoboRioUserActive3V3Source m_active;
+  RoboRioUserFaults3V3Source m_faults;
+};
+
+class RoboRioSimModel : public glass::RoboRioModel {
+ public:
+  void Update() override {}
+
+  bool Exists() override { return true; }
+
+  glass::RoboRioRailModel* GetUser6VRail() override { return &m_user6VRail; }
+  glass::RoboRioRailModel* GetUser5VRail() override { return &m_user5VRail; }
+  glass::RoboRioRailModel* GetUser3V3Rail() override { return &m_user3V3Rail; }
+
+  glass::DataSource* GetUserButton() override { return &m_userButton; }
+  glass::DataSource* GetVInVoltageData() override { return &m_vInVoltage; }
+  glass::DataSource* GetVInCurrentData() override { return &m_vInCurrent; }
+  glass::DataSource* GetBrownoutVoltage() override {
+    return &m_brownoutVoltage;
+  }
+
+  void SetUserButton(bool val) override { HALSIM_SetRoboRioFPGAButton(val); }
+  void SetVInVoltage(double val) override { HALSIM_SetRoboRioVInVoltage(val); }
+  void SetVInCurrent(double val) override { HALSIM_SetRoboRioVInCurrent(val); }
+  void SetBrownoutVoltage(double val) override {
+    HALSIM_SetRoboRioBrownoutVoltage(val);
+  }
+
+ private:
+  RoboRioFPGAButtonSource m_userButton;
+  RoboRioVInVoltageSource m_vInVoltage;
+  RoboRioVInCurrentSource m_vInCurrent;
+  RoboRioUser6VRailSimModel m_user6VRail;
+  RoboRioUser5VRailSimModel m_user5VRail;
+  RoboRioUser3V3RailSimModel m_user3V3Rail;
+  RoboRioBrownoutVoltageSource m_brownoutVoltage;
+};
+}  // namespace
+
+void RoboRioSimGui::Initialize() {
+  HALSimGui::halProvider.Register(
+      "RoboRIO", [] { return true; },
+      [] { return std::make_unique<RoboRioSimModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(5, 125);
+        return glass::MakeFunctionView(
+            [=] { DisplayRoboRio(static_cast<RoboRioSimModel*>(model)); });
+      });
+}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.h
new file mode 100644
index 0000000..27999b5
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/RoboRioSimGui.h
@@ -0,0 +1,14 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace halsimgui {
+
+class RoboRioSimGui {
+ public:
+  static void Initialize();
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp
index 2802000..62e2896 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SimDeviceGui.cpp
@@ -1,59 +1,40 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SimDeviceGui.h"
 
+#include <glass/other/DeviceTree.h>
 #include <stdint.h>
 
-#include <functional>
-#include <memory>
-#include <vector>
-
+#include <fmt/format.h>
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
-#include <imgui.h>
 #include <wpi/DenseMap.h>
+#include <wpi/StringExtras.h>
 
-#include "GuiDataSource.h"
+#include "HALDataSource.h"
 #include "HALSimGui.h"
-#include "IniSaverInfo.h"
-#include "IniSaverString.h"
 
 using namespace halsimgui;
 
 namespace {
-
-struct ElementInfo : public NameInfo, public OpenInfo {
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value) {
-    if (NameInfo::ReadIni(name, value)) return true;
-    if (OpenInfo::ReadIni(name, value)) return true;
-    return false;
-  }
-  void WriteIni(ImGuiTextBuffer* out) {
-    NameInfo::WriteIni(out);
-    OpenInfo::WriteIni(out);
-  }
-  bool visible = true;  // not saved
-};
-
-class SimValueSource : public GuiDataSource {
+class SimValueSource : public glass::DataSource {
  public:
   explicit SimValueSource(HAL_SimValueHandle handle, const char* device,
                           const char* name)
-      : GuiDataSource(wpi::Twine{device} + wpi::Twine{'-'} + name),
+      : DataSource(fmt::format("{}-{}", device, name)),
         m_callback{HALSIM_RegisterSimValueChangedCallback(
             handle, this, CallbackFunc, true)} {}
-  ~SimValueSource() {
-    if (m_callback != 0) HALSIM_CancelSimValueChangedCallback(m_callback);
+  ~SimValueSource() override {
+    if (m_callback != 0) {
+      HALSIM_CancelSimValueChangedCallback(m_callback);
+    }
   }
 
  private:
   static void CallbackFunc(const char*, void* param, HAL_SimValueHandle,
-                           HAL_Bool, const HAL_Value* value) {
+                           int32_t, const HAL_Value* value) {
     auto source = static_cast<SimValueSource*>(param);
     if (value->type == HAL_BOOLEAN) {
       source->SetValue(value->data.v_boolean);
@@ -67,226 +48,144 @@
   int32_t m_callback;
 };
 
+class SimDevicesModel : public glass::Model {
+ public:
+  void Update() override;
+  bool Exists() override { return true; }
+
+  glass::DataSource* GetSource(HAL_SimValueHandle handle) {
+    return m_sources[handle].get();
+  }
+
+ private:
+  wpi::DenseMap<HAL_SimValueHandle, std::unique_ptr<SimValueSource>> m_sources;
+};
 }  // namespace
 
-static std::vector<std::function<void()>> gDeviceExecutors;
-static IniSaverString<ElementInfo> gElements{"Device"};
-static wpi::DenseMap<HAL_SimValueHandle, std::unique_ptr<SimValueSource>>
-    gSimValueSources;
+static SimDevicesModel* gSimDevicesModel;
+static bool gSimDevicesShowPrefix = false;
 
-static void UpdateSimValueSources() {
+void SimDevicesModel::Update() {
   HALSIM_EnumerateSimDevices(
-      "", nullptr, [](const char* name, void*, HAL_SimDeviceHandle handle) {
+      "", this, [](const char* name, void* self, HAL_SimDeviceHandle handle) {
+        struct Data {
+          SimDevicesModel* self;
+          const char* device;
+        } data = {static_cast<SimDevicesModel*>(self), name};
         HALSIM_EnumerateSimValues(
-            handle, const_cast<char*>(name),
-            [](const char* name, void* deviceV, HAL_SimValueHandle handle,
-               HAL_Bool readonly, const HAL_Value* value) {
-              auto device = static_cast<const char*>(deviceV);
-              auto& source = gSimValueSources[handle];
+            handle, &data,
+            [](const char* name, void* dataV, HAL_SimValueHandle handle,
+               int32_t direction, const HAL_Value* value) {
+              auto data = static_cast<Data*>(dataV);
+              auto& source = data->self->m_sources[handle];
               if (!source) {
-                source = std::make_unique<SimValueSource>(handle, device, name);
+                source = std::make_unique<SimValueSource>(handle, data->device,
+                                                          name);
               }
             });
       });
 }
 
-void SimDeviceGui::Hide(const char* name) { gElements[name].visible = false; }
+static void DisplaySimValue(const char* name, void* data,
+                            HAL_SimValueHandle handle, int32_t direction,
+                            const HAL_Value* value) {
+  auto model = static_cast<SimDevicesModel*>(data);
 
-void SimDeviceGui::Add(std::function<void()> execute) {
-  if (execute) gDeviceExecutors.emplace_back(std::move(execute));
-}
+  HAL_Value valueCopy = *value;
 
-bool SimDeviceGui::StartDevice(const char* label, ImGuiTreeNodeFlags flags) {
-  auto& element = gElements[label];
-  if (!element.visible) return false;
-
-  char name[128];
-  element.GetLabel(name, sizeof(name), label);
-
-  bool open = ImGui::CollapsingHeader(
-      name, flags | (element.IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0));
-  element.SetOpen(open);
-  element.PopupEditName(label);
-
-  if (open) ImGui::PushID(label);
-  return open;
-}
-
-void SimDeviceGui::FinishDevice() { ImGui::PopID(); }
-
-static bool DisplayValueImpl(const char* name, bool readonly, HAL_Value* value,
-                             const char** options, int32_t numOptions) {
-  // read-only
-  if (readonly) {
-    switch (value->type) {
-      case HAL_BOOLEAN:
-        ImGui::LabelText(name, "%s", value->data.v_boolean ? "true" : "false");
-        break;
-      case HAL_DOUBLE:
-        ImGui::LabelText(name, "%.6f", value->data.v_double);
-        break;
-      case HAL_ENUM: {
-        int current = value->data.v_enum;
-        if (current < 0 || current >= numOptions)
-          ImGui::LabelText(name, "%d (unknown)", current);
-        else
-          ImGui::LabelText(name, "%s", options[current]);
-        break;
-      }
-      case HAL_INT:
-        ImGui::LabelText(name, "%d", static_cast<int>(value->data.v_int));
-        break;
-      case HAL_LONG:
-        ImGui::LabelText(name, "%lld",
-                         static_cast<long long int>(  // NOLINT(runtime/int)
-                             value->data.v_long));
-        break;
-      default:
-        break;
-    }
-    return false;
-  }
-
-  // writable
   switch (value->type) {
     case HAL_BOOLEAN: {
-      static const char* boolOptions[] = {"false", "true"};
-      int val = value->data.v_boolean ? 1 : 0;
-      if (ImGui::Combo(name, &val, boolOptions, 2)) {
-        value->data.v_boolean = val;
-        return true;
+      bool v = value->data.v_boolean;
+      if (glass::DeviceBoolean(name, direction == HAL_SimValueOutput, &v,
+                               model->GetSource(handle))) {
+        valueCopy.data.v_boolean = v ? 1 : 0;
+        HAL_SetSimValue(handle, valueCopy);
       }
       break;
     }
-    case HAL_DOUBLE: {
-      if (ImGui::InputDouble(name, &value->data.v_double, 0, 0, "%.6f",
-                             ImGuiInputTextFlags_EnterReturnsTrue))
-        return true;
+    case HAL_DOUBLE:
+      if (glass::DeviceDouble(name, direction == HAL_SimValueOutput,
+                              &valueCopy.data.v_double,
+                              model->GetSource(handle))) {
+        HAL_SetSimValue(handle, valueCopy);
+      }
       break;
-    }
     case HAL_ENUM: {
-      int current = value->data.v_enum;
-      if (ImGui::Combo(name, &current, options, numOptions)) {
-        value->data.v_enum = current;
-        return true;
+      int32_t numOptions = 0;
+      const char** options = HALSIM_GetSimValueEnumOptions(handle, &numOptions);
+      if (glass::DeviceEnum(name, direction == HAL_SimValueOutput,
+                            &valueCopy.data.v_enum, options, numOptions,
+                            model->GetSource(handle))) {
+        HAL_SetSimValue(handle, valueCopy);
       }
       break;
     }
-    case HAL_INT: {
-      if (ImGui::InputScalar(name, ImGuiDataType_S32, &value->data.v_int,
-                             nullptr, nullptr, nullptr,
-                             ImGuiInputTextFlags_EnterReturnsTrue))
-        return true;
+    case HAL_INT:
+      if (glass::DeviceInt(name, direction == HAL_SimValueOutput,
+                           &valueCopy.data.v_int, model->GetSource(handle))) {
+        HAL_SetSimValue(handle, valueCopy);
+      }
       break;
-    }
-    case HAL_LONG: {
-      if (ImGui::InputScalar(name, ImGuiDataType_S64, &value->data.v_long,
-                             nullptr, nullptr, nullptr,
-                             ImGuiInputTextFlags_EnterReturnsTrue))
-        return true;
+    case HAL_LONG:
+      if (glass::DeviceLong(name, direction == HAL_SimValueOutput,
+                            &valueCopy.data.v_long, model->GetSource(handle))) {
+        HAL_SetSimValue(handle, valueCopy);
+      }
       break;
-    }
     default:
       break;
   }
-  return false;
 }
 
-static bool DisplayValueSourceImpl(const char* name, bool readonly,
-                                   HAL_Value* value,
-                                   const GuiDataSource* source,
-                                   const char** options, int32_t numOptions) {
-  if (!source)
-    return DisplayValueImpl(name, readonly, value, options, numOptions);
-  ImGui::PushID(name);
-  bool rv = DisplayValueImpl("", readonly, value, options, numOptions);
-  ImGui::SameLine(0, ImGui::GetStyle().ItemInnerSpacing.x);
-  ImGui::Selectable(name);
-  source->EmitDrag();
-  ImGui::PopID();
-  return rv;
-}
-
-bool SimDeviceGui::DisplayValue(const char* name, bool readonly,
-                                HAL_Value* value, const char** options,
-                                int32_t numOptions) {
-  return DisplayValueSource(name, readonly, value, nullptr, options,
-                            numOptions);
-}
-
-bool SimDeviceGui::DisplayValueSource(const char* name, bool readonly,
-                                      HAL_Value* value,
-                                      const GuiDataSource* source,
-                                      const char** options,
-                                      int32_t numOptions) {
-  ImGui::SetNextItemWidth(ImGui::GetWindowWidth() * 0.5f);
-  return DisplayValueSourceImpl(name, readonly, value, source, options,
-                                numOptions);
-}
-
-static void SimDeviceDisplayValue(const char* name, void*,
-                                  HAL_SimValueHandle handle, HAL_Bool readonly,
-                                  const HAL_Value* value) {
-  int32_t numOptions = 0;
-  const char** options = nullptr;
-
-  if (value->type == HAL_ENUM)
-    options = HALSIM_GetSimValueEnumOptions(handle, &numOptions);
-
-  HAL_Value valueCopy = *value;
-  if (DisplayValueSourceImpl(name, readonly, &valueCopy,
-                             gSimValueSources[handle].get(), options,
-                             numOptions))
-    HAL_SetSimValue(handle, valueCopy);
-}
-
-static void SimDeviceDisplayDevice(const char* name, void*,
-                                   HAL_SimDeviceHandle handle) {
-  auto it = gElements.find(name);
-  if (it != gElements.end() && !it->second.visible) return;
-
-  if (SimDeviceGui::StartDevice(name)) {
-    ImGui::PushItemWidth(ImGui::GetWindowWidth() * 0.5f);
-    HALSIM_EnumerateSimValues(handle, nullptr, SimDeviceDisplayValue);
-    ImGui::PopItemWidth();
-    SimDeviceGui::FinishDevice();
+static void DisplaySimDevice(const char* name, void* data,
+                             HAL_SimDeviceHandle handle) {
+  std::string_view id{name};
+  if (!gSimDevicesShowPrefix) {
+    // only show "Foo" portion of "Accel:Foo"
+    std::string_view type;
+    std::tie(type, id) = wpi::split(id, ':');
+    if (id.empty()) {
+      id = type;
+    }
   }
-}
-
-static void DisplayDeviceTree() {
-  for (auto&& execute : gDeviceExecutors) {
-    if (execute) execute();
+  if (glass::BeginDevice(id.data())) {
+    HALSIM_EnumerateSimValues(handle, data, DisplaySimValue);
+    glass::EndDevice();
   }
-  HALSIM_EnumerateSimDevices("", nullptr, SimDeviceDisplayDevice);
 }
 
 void SimDeviceGui::Initialize() {
-  gElements.Initialize();
-  HALSimGui::AddExecute(UpdateSimValueSources);
-  HALSimGui::AddWindow("Other Devices", DisplayDeviceTree);
-  HALSimGui::SetDefaultWindowPos("Other Devices", 1025, 20);
-  HALSimGui::SetDefaultWindowSize("Other Devices", 250, 695);
+  HALSimGui::halProvider.Register(
+      "Other Devices", [] { return true; },
+      [] { return std::make_unique<glass::DeviceTreeModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->SetDefaultPos(1025, 20);
+        win->SetDefaultSize(250, 695);
+        win->DisableRenamePopup();
+        return glass::MakeFunctionView([=] {
+          if (ImGui::BeginPopupContextItem()) {
+            ImGui::Checkbox("Show prefix", &gSimDevicesShowPrefix);
+            ImGui::EndPopup();
+          }
+          static_cast<glass::DeviceTreeModel*>(model)->Display();
+        });
+      });
+  HALSimGui::halProvider.ShowDefault("Other Devices");
+
+  auto model = std::make_unique<SimDevicesModel>();
+  gSimDevicesModel = model.get();
+  GetDeviceTree().Add(std::move(model), [](glass::Model* model) {
+    HALSIM_EnumerateSimDevices("", static_cast<SimDevicesModel*>(model),
+                               DisplaySimDevice);
+  });
 }
 
-extern "C" {
-
-void HALSIMGUI_DeviceTreeAdd(void* param, void (*execute)(void*)) {
-  if (execute) SimDeviceGui::Add([=] { execute(param); });
+glass::DataSource* SimDeviceGui::GetValueSource(HAL_SimValueHandle handle) {
+  return gSimDevicesModel->GetSource(handle);
 }
 
-void HALSIMGUI_DeviceTreeHide(const char* name) { SimDeviceGui::Hide(name); }
-
-HAL_Bool HALSIMGUI_DeviceTreeDisplayValue(const char* name, HAL_Bool readonly,
-                                          struct HAL_Value* value,
-                                          const char** options,
-                                          int32_t numOptions) {
-  return SimDeviceGui::DisplayValue(name, readonly, value, options, numOptions);
+glass::DeviceTreeModel& SimDeviceGui::GetDeviceTree() {
+  static auto model = HALSimGui::halProvider.GetModel("Other Devices");
+  assert(model);
+  return *static_cast<glass::DeviceTreeModel*>(model);
 }
-
-HAL_Bool HALSIMGUI_DeviceTreeStartDevice(const char* label, int32_t flags) {
-  return SimDeviceGui::StartDevice(label, flags);
-}
-
-void HALSIMGUI_DeviceTreeFinishDevice(void) { SimDeviceGui::FinishDevice(); }
-
-}  // extern "C"
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp
deleted file mode 100644
index b0f802e..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.cpp
+++ /dev/null
@@ -1,130 +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 "SolenoidGui.h"
-
-#include <cstdio>
-#include <cstring>
-#include <memory>
-#include <vector>
-
-#include <hal/Ports.h>
-#include <hal/simulation/PCMData.h>
-#include <imgui.h>
-#include <wpi/SmallVector.h>
-
-#include "ExtraGuiWidgets.h"
-#include "GuiDataSource.h"
-#include "HALSimGui.h"
-#include "IniSaver.h"
-#include "IniSaverInfo.h"
-
-using namespace halsimgui;
-
-namespace {
-HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED2(PCMSolenoidOutput, "Solenoid");
-struct PCMSource {
-  explicit PCMSource(int numChannels) : solenoids(numChannels) {}
-  std::vector<std::unique_ptr<PCMSolenoidOutputSource>> solenoids;
-  int initCount = 0;
-};
-}  // namespace
-
-static IniSaver<OpenInfo> gPCMs{"PCM"};
-static IniSaver<NameInfo> gSolenoids{"Solenoid"};
-static std::vector<PCMSource> gPCMSources;
-
-static void UpdateSolenoidSources() {
-  for (int i = 0, iend = gPCMSources.size(); i < iend; ++i) {
-    auto& pcmSource = gPCMSources[i];
-    int numChannels = pcmSource.solenoids.size();
-    pcmSource.initCount = 0;
-    for (int j = 0; j < numChannels; ++j) {
-      auto& source = pcmSource.solenoids[j];
-      if (HALSIM_GetPCMSolenoidInitialized(i, j)) {
-        if (!source) {
-          source = std::make_unique<PCMSolenoidOutputSource>(i, j);
-          source->SetName(gSolenoids[i * numChannels + j].GetName());
-        }
-        ++pcmSource.initCount;
-      } else {
-        source.reset();
-      }
-    }
-  }
-}
-
-static void DisplaySolenoids() {
-  bool hasOutputs = false;
-  for (int i = 0, iend = gPCMSources.size(); i < iend; ++i) {
-    auto& pcmSource = gPCMSources[i];
-    if (pcmSource.initCount == 0) continue;
-    hasOutputs = true;
-
-    int numChannels = pcmSource.solenoids.size();
-    wpi::SmallVector<int, 16> channels;
-    channels.resize(numChannels);
-    for (int j = 0; j < numChannels; ++j) {
-      if (pcmSource.solenoids[j]) {
-        channels[j] = (!HALSimGui::AreOutputsDisabled() &&
-                       pcmSource.solenoids[j]->GetValue())
-                          ? 1
-                          : -1;
-      } else {
-        channels[j] = -2;
-      }
-    }
-
-    char name[128];
-    std::snprintf(name, sizeof(name), "PCM[%d]", i);
-    auto& pcmInfo = gPCMs[i];
-    bool open = ImGui::CollapsingHeader(
-        name, pcmInfo.IsOpen() ? ImGuiTreeNodeFlags_DefaultOpen : 0);
-    pcmInfo.SetOpen(open);
-    ImGui::SetItemAllowOverlap();
-    ImGui::SameLine();
-
-    // show channels as LED indicators
-    static const ImU32 colors[] = {IM_COL32(255, 255, 102, 255),
-                                   IM_COL32(128, 128, 128, 255)};
-    DrawLEDs(channels.data(), channels.size(), channels.size(), colors);
-
-    if (open) {
-      ImGui::PushID(i);
-      ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
-      for (int j = 0; j < numChannels; ++j) {
-        if (!pcmSource.solenoids[j]) continue;
-        auto& info = gSolenoids[i * numChannels + j];
-        info.GetLabel(name, sizeof(name), "Solenoid", j);
-        ImGui::PushID(j);
-        pcmSource.solenoids[j]->LabelText(name, "%s",
-                                          channels[j] == 1 ? "On" : "Off");
-        if (info.PopupEditName(j)) {
-          pcmSource.solenoids[j]->SetName(info.GetName());
-        }
-        ImGui::PopID();
-      }
-      ImGui::PopItemWidth();
-      ImGui::PopID();
-    }
-  }
-  if (!hasOutputs) ImGui::Text("No solenoids");
-}
-
-void SolenoidGui::Initialize() {
-  gPCMs.Initialize();
-  gSolenoids.Initialize();
-  const int numModules = HAL_GetNumPCMModules();
-  const int numChannels = HAL_GetNumSolenoidChannels();
-  gPCMSources.reserve(numModules);
-  for (int i = 0; i < numModules; ++i) gPCMSources.emplace_back(numChannels);
-
-  HALSimGui::AddExecute(UpdateSolenoidSources);
-  HALSimGui::AddWindow("Solenoids", DisplaySolenoids,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("Solenoids", 290, 20);
-}
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.h
deleted file mode 100644
index 35905cf..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/SolenoidGui.h
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace halsimgui {
-
-class SolenoidGui {
- public:
-  static void Initialize();
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
index 18c76b0..1928063 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.cpp
@@ -1,12 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "TimingGui.h"
 
+#include <glass/Model.h>
+#include <glass/View.h>
+
 #include <cstdio>
 #include <cstring>
 #include <vector>
@@ -20,20 +20,33 @@
 
 using namespace halsimgui;
 
+namespace {
+class TimingModel : public glass::Model {
+ public:
+  void Update() override {}
+  bool Exists() override { return true; }
+};
+}  // namespace
+
 static void DisplayTiming() {
   int32_t status = 0;
   uint64_t curTime = HAL_GetFPGATime(&status);
 
-  if (ImGui::Button("Run")) HALSIM_ResumeTiming();
+  if (ImGui::Button("Run")) {
+    HALSIM_ResumeTiming();
+  }
   ImGui::SameLine();
-  if (ImGui::Button("Pause")) HALSIM_PauseTiming();
+  if (ImGui::Button("Pause")) {
+    HALSIM_PauseTiming();
+  }
   ImGui::SameLine();
   ImGui::PushButtonRepeat(true);
   if (ImGui::Button("Step")) {
     HALSIM_PauseTiming();
     uint64_t nextTimeout = HALSIM_GetNextNotifierTimeout();
-    if (nextTimeout != UINT64_MAX)
+    if (nextTimeout != UINT64_MAX) {
       HALSIM_StepTimingAsync(nextTimeout - curTime);
+    }
   }
   ImGui::PopButtonRepeat();
   ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
@@ -46,16 +59,26 @@
     notifiers.resize(num);
     HALSIM_GetNotifierInfo(notifiers.data(), notifiers.size());
   }
-  if (num > 0) ImGui::Separator();
+  if (num > 0) {
+    ImGui::Separator();
+  }
   ImGui::PushItemWidth(ImGui::GetFontSize() * 4);
-  for (int32_t i = 0; i < num; ++i)
+  for (int32_t i = 0; i < num; ++i) {
     ImGui::LabelText(notifiers[i].name, "%.3f",
                      notifiers[i].timeout / 1000000.0);
+  }
   ImGui::PopItemWidth();
 }
 
 void TimingGui::Initialize() {
-  HALSimGui::AddWindow("Timing", DisplayTiming,
-                       ImGuiWindowFlags_AlwaysAutoResize);
-  HALSimGui::SetDefaultWindowPos("Timing", 5, 150);
+  HALSimGui::halProvider.Register(
+      "Timing", [] { return true; },
+      [] { return std::make_unique<TimingModel>(); },
+      [](glass::Window* win, glass::Model* model) {
+        win->DisableRenamePopup();
+        win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize);
+        win->SetDefaultPos(5, 150);
+        return glass::MakeFunctionView(DisplayTiming);
+      });
+  HALSimGui::halProvider.ShowDefault("Timing");
 }
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.h
index 49f33cc..e87b791 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.h
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/TimingGui.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/main.cpp
index a904580..6df6f35 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/cpp/main.cpp
@@ -1,76 +1,121 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <glass/Context.h>
+#include <glass/other/Plot.h>
+
+#include <cstdio>
+#include <string_view>
 
 #include <hal/Extensions.h>
 #include <hal/Main.h>
-#include <wpi/StringRef.h>
-#include <wpi/raw_ostream.h>
+#include <imgui.h>
+#include <wpigui.h>
 
-#include "AccelerometerGui.h"
+#include "AccelerometerSimGui.h"
 #include "AddressableLEDGui.h"
-#include "AnalogGyroGui.h"
-#include "AnalogInputGui.h"
-#include "AnalogOutGui.h"
-#include "CompressorGui.h"
-#include "DIOGui.h"
+#include "AnalogGyroSimGui.h"
+#include "AnalogInputSimGui.h"
+#include "AnalogOutputSimGui.h"
+#include "DIOSimGui.h"
 #include "DriverStationGui.h"
-#include "EncoderGui.h"
-#include "Field2D.h"
+#include "EncoderSimGui.h"
 #include "HALSimGui.h"
-#include "Mechanism2D.h"
-#include "NetworkTablesGui.h"
-#include "PDPGui.h"
-#include "PWMGui.h"
-#include "PlotGui.h"
-#include "RelayGui.h"
-#include "RoboRioGui.h"
+#include "NetworkTablesSimGui.h"
+#include "PCMSimGui.h"
+#include "PWMSimGui.h"
+#include "PowerDistributionSimGui.h"
+#include "RelaySimGui.h"
+#include "RoboRioSimGui.h"
 #include "SimDeviceGui.h"
-#include "SolenoidGui.h"
 #include "TimingGui.h"
 
 using namespace halsimgui;
 
+namespace gui = wpi::gui;
+
+static glass::PlotProvider gPlotProvider{"Plot"};
+
 extern "C" {
 #if defined(WIN32) || defined(_WIN32)
 __declspec(dllexport)
 #endif
     int HALSIM_InitExtension(void) {
-  HALSimGui::GlobalInit();
-  HALSimGui::Add(AccelerometerGui::Initialize);
-  HALSimGui::Add(AddressableLEDGui::Initialize);
-  HALSimGui::Add(AnalogGyroGui::Initialize);
-  HALSimGui::Add(AnalogInputGui::Initialize);
-  HALSimGui::Add(AnalogOutGui::Initialize);
-  HALSimGui::Add(CompressorGui::Initialize);
-  HALSimGui::Add(DriverStationGui::Initialize);
-  HALSimGui::Add(DIOGui::Initialize);
-  HALSimGui::Add(EncoderGui::Initialize);
-  HALSimGui::Add(Field2D::Initialize);
-  HALSimGui::Add(Mechanism2D::Initialize);
-  HALSimGui::Add(NetworkTablesGui::Initialize);
-  HALSimGui::Add(PDPGui::Initialize);
-  HALSimGui::Add(PlotGui::Initialize);
-  HALSimGui::Add(PWMGui::Initialize);
-  HALSimGui::Add(RelayGui::Initialize);
-  HALSimGui::Add(RoboRioGui::Initialize);
-  HALSimGui::Add(SimDeviceGui::Initialize);
-  HALSimGui::Add(SolenoidGui::Initialize);
-  HALSimGui::Add(TimingGui::Initialize);
+  std::puts("Simulator GUI Initializing.");
 
-  wpi::outs() << "Simulator GUI Initializing.\n";
-  if (!HALSimGui::Initialize()) return 0;
+  gui::CreateContext();
+  glass::CreateContext();
+  HALSimGui::GlobalInit();
+  DriverStationGui::GlobalInit();
+  gPlotProvider.GlobalInit();
+
+  // These need to initialize first
+  gui::AddInit(EncoderSimGui::Initialize);
+  gui::AddInit(SimDeviceGui::Initialize);
+
+  gui::AddInit(AccelerometerSimGui::Initialize);
+  gui::AddInit(AddressableLEDGui::Initialize);
+  gui::AddInit(AnalogGyroSimGui::Initialize);
+  gui::AddInit(AnalogInputSimGui::Initialize);
+  gui::AddInit(AnalogOutputSimGui::Initialize);
+  gui::AddInit(DIOSimGui::Initialize);
+  gui::AddInit(NetworkTablesSimGui::Initialize);
+  gui::AddInit(PCMSimGui::Initialize);
+  gui::AddInit(PowerDistributionSimGui::Initialize);
+  gui::AddInit(PWMSimGui::Initialize);
+  gui::AddInit(RelaySimGui::Initialize);
+  gui::AddInit(RoboRioSimGui::Initialize);
+  gui::AddInit(TimingGui::Initialize);
+
+  HALSimGui::mainMenu.AddMainMenu([] {
+    if (ImGui::BeginMenu("Hardware")) {
+      HALSimGui::halProvider.DisplayMenu();
+      ImGui::EndMenu();
+    }
+    if (ImGui::BeginMenu("NetworkTables")) {
+      NetworkTablesSimGui::DisplayMenu();
+      ImGui::Separator();
+      HALSimGui::ntProvider.DisplayMenu();
+      ImGui::EndMenu();
+    }
+    if (ImGui::BeginMenu("DS")) {
+      DriverStationGui::dsManager.DisplayMenu();
+      ImGui::EndMenu();
+    }
+    if (ImGui::BeginMenu("Plot")) {
+      bool paused = gPlotProvider.IsPaused();
+      if (ImGui::MenuItem("Pause All Plots", nullptr, &paused)) {
+        gPlotProvider.SetPaused(paused);
+      }
+      ImGui::Separator();
+      gPlotProvider.DisplayMenu();
+      ImGui::EndMenu();
+    }
+    if (ImGui::BeginMenu("Window")) {
+      HALSimGui::manager.DisplayMenu();
+      ImGui::EndMenu();
+    }
+  });
+
+  if (!gui::Initialize("Robot Simulation", 1280, 720)) {
+    return 0;
+  }
   HAL_RegisterExtensionListener(
       nullptr, [](void*, const char* name, void* data) {
-        if (wpi::StringRef{name} == "ds_socket") {
+        if (std::string_view{name} == "ds_socket") {
           DriverStationGui::SetDSSocketExtension(data);
         }
       });
-  HAL_SetMain(nullptr, HALSimGui::Main, HALSimGui::Exit);
-  wpi::outs() << "Simulator GUI Initialized!\n";
+  HAL_SetMain(
+      nullptr,
+      [](void*) {
+        gui::Main();
+        glass::DestroyContext();
+        gui::DestroyContext();
+      },
+      [](void*) { gui::Exit(); });
+  std::puts("Simulator GUI Initialized!");
 
   return 0;
 }
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
deleted file mode 100644
index 91d7301..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/ExtraGuiWidgets.h
+++ /dev/null
@@ -1,89 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <imgui.h>
-
-namespace halsimgui {
-
-class GuiDataSource;
-
-/**
- * DrawLEDs() configuration for 2D arrays.
- */
-struct LEDConfig {
-  /**
-   * Whether the major order is serpentined (e.g. the first row goes left to
-   * right, the second row right to left, the third row left to right, and so
-   * on).
-   */
-  bool serpentine = false;
-
-  /**
-   * The input array order (row-major or column-major).
-   */
-  enum Order { RowMajor = 0, ColumnMajor } order = RowMajor;
-
-  /**
-   * The starting location of the array (0 location).
-   */
-  enum Start {
-    UpperLeft = 0,
-    LowerLeft,
-    UpperRight,
-    LowerRight
-  } start = UpperLeft;
-};
-
-/**
- * Draw a 2D array of LEDs.
- *
- * Values are indices into colors array.  Positive values are filled (lit),
- * negative values are unfilled (dark / border only).  The actual color index
- * is the absolute value of the value - 1.  0 values are not drawn at all
- * (an empty space is left).
- *
- * @param values values array
- * @param numValues size of values array
- * @param cols number of columns
- * @param colors colors array
- * @param size size of each LED (both horizontal and vertical);
- *             if 0, defaults to 1/2 of font size
- * @param spacing spacing between each LED (both horizontal and vertical);
- *                if 0, defaults to 1/3 of font size
- * @param config 2D array configuration
- */
-void DrawLEDs(const int* values, int numValues, int cols, const ImU32* colors,
-              float size = 0.0f, float spacing = 0.0f,
-              const LEDConfig& config = LEDConfig{});
-
-/**
- * Draw a 2D array of LEDs.
- *
- * Values are indices into colors array.  Positive values are filled (lit),
- * negative values are unfilled (dark / border only).  The actual color index
- * is the absolute value of the value - 1.  0 values are not drawn at all
- * (an empty space is left).
- *
- * @param values values array
- * @param sources sources array
- * @param numValues size of values and sources arrays
- * @param cols number of columns
- * @param colors colors array
- * @param size size of each LED (both horizontal and vertical);
- *             if 0, defaults to 1/2 of font size
- * @param spacing spacing between each LED (both horizontal and vertical);
- *                if 0, defaults to 1/3 of font size
- * @param config 2D array configuration
- */
-void DrawLEDSources(const int* values, GuiDataSource** sources, int numValues,
-                    int cols, const ImU32* colors, float size = 0.0f,
-                    float spacing = 0.0f,
-                    const LEDConfig& config = LEDConfig{});
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/GuiDataSource.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/GuiDataSource.h
deleted file mode 100644
index 117d6ea..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/GuiDataSource.h
+++ /dev/null
@@ -1,186 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <atomic>
-#include <string>
-
-#include <imgui.h>
-#include <wpi/Signal.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-#include <wpi/spinlock.h>
-
-namespace halsimgui {
-
-/**
- * A data source.
- */
-class GuiDataSource {
- public:
-  explicit GuiDataSource(const wpi::Twine& id);
-  GuiDataSource(const wpi::Twine& id, int index);
-  GuiDataSource(const wpi::Twine& id, int index, int index2);
-  ~GuiDataSource();
-
-  GuiDataSource(const GuiDataSource&) = delete;
-  GuiDataSource& operator=(const GuiDataSource&) = delete;
-
-  const char* GetId() const { return m_id.c_str(); }
-
-  void SetName(const wpi::Twine& name) { m_name = name.str(); }
-  const char* GetName() const { return m_name.c_str(); }
-
-  void SetDigital(bool digital) { m_digital = digital; }
-  bool IsDigital() const { return m_digital; }
-
-  void SetValue(double value) {
-    m_value = value;
-    valueChanged(value);
-  }
-  double GetValue() const { return m_value; }
-
-  // drag source helpers
-  void LabelText(const char* label, const char* fmt, ...) const;
-  void LabelTextV(const char* label, const char* fmt, va_list args) const;
-  bool Combo(const char* label, int* current_item, const char* const items[],
-             int items_count, int popup_max_height_in_items = -1) const;
-  bool SliderFloat(const char* label, float* v, float v_min, float v_max,
-                   const char* format = "%.3f", float power = 1.0f) const;
-  bool InputDouble(const char* label, double* v, double step = 0.0,
-                   double step_fast = 0.0, const char* format = "%.6f",
-                   ImGuiInputTextFlags flags = 0) const;
-  bool InputInt(const char* label, int* v, int step = 1, int step_fast = 100,
-                ImGuiInputTextFlags flags = 0) const;
-  void EmitDrag(ImGuiDragDropFlags flags = 0) const;
-
-  wpi::sig::SignalBase<wpi::spinlock, double> valueChanged;
-
-  static GuiDataSource* Find(wpi::StringRef id);
-
-  static wpi::sig::Signal<const char*, GuiDataSource*> sourceCreated;
-
- private:
-  std::string m_id;
-  std::string m_name;
-  bool m_digital = false;
-  std::atomic<double> m_value = 0;
-};
-
-}  // namespace halsimgui
-
-#define HALSIMGUI_DATASOURCE(cbname, id, TYPE, vtype)                         \
-  class cbname##Source : public ::halsimgui::GuiDataSource {                  \
-   public:                                                                    \
-    cbname##Source()                                                          \
-        : GuiDataSource(id),                                                  \
-          m_callback{                                                         \
-              HALSIM_Register##cbname##Callback(CallbackFunc, this, true)} {  \
-      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
-    }                                                                         \
-                                                                              \
-    ~cbname##Source() {                                                       \
-      if (m_callback != 0) HALSIM_Cancel##cbname##Callback(m_callback);       \
-    }                                                                         \
-                                                                              \
-   private:                                                                   \
-    static void CallbackFunc(const char*, void* param,                        \
-                             const HAL_Value* value) {                        \
-      if (value->type == HAL_##TYPE)                                          \
-        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
-    }                                                                         \
-                                                                              \
-    int32_t m_callback;                                                       \
-  }
-
-#define HALSIMGUI_DATASOURCE_BOOLEAN(cbname, id) \
-  HALSIMGUI_DATASOURCE(cbname, id, BOOLEAN, boolean)
-
-#define HALSIMGUI_DATASOURCE_DOUBLE(cbname, id) \
-  HALSIMGUI_DATASOURCE(cbname, id, DOUBLE, double)
-
-#define HALSIMGUI_DATASOURCE_INT(cbname, id) \
-  HALSIMGUI_DATASOURCE(cbname, id, INT, int)
-
-#define HALSIMGUI_DATASOURCE_INDEXED(cbname, id, TYPE, vtype)                 \
-  class cbname##Source : public ::halsimgui::GuiDataSource {                  \
-   public:                                                                    \
-    explicit cbname##Source(int32_t index, int channel = -1)                  \
-        : GuiDataSource(id, channel < 0 ? index : channel),                   \
-          m_index{index},                                                     \
-          m_channel{channel < 0 ? index : channel},                           \
-          m_callback{HALSIM_Register##cbname##Callback(index, CallbackFunc,   \
-                                                       this, true)} {         \
-      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
-    }                                                                         \
-                                                                              \
-    ~cbname##Source() {                                                       \
-      if (m_callback != 0)                                                    \
-        HALSIM_Cancel##cbname##Callback(m_index, m_callback);                 \
-    }                                                                         \
-                                                                              \
-    int32_t GetIndex() const { return m_index; }                              \
-                                                                              \
-    int GetChannel() const { return m_channel; }                              \
-                                                                              \
-   private:                                                                   \
-    static void CallbackFunc(const char*, void* param,                        \
-                             const HAL_Value* value) {                        \
-      if (value->type == HAL_##TYPE)                                          \
-        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
-    }                                                                         \
-                                                                              \
-    int32_t m_index;                                                          \
-    int m_channel;                                                            \
-    int32_t m_callback;                                                       \
-  }
-
-#define HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(cbname, id) \
-  HALSIMGUI_DATASOURCE_INDEXED(cbname, id, BOOLEAN, boolean)
-
-#define HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(cbname, id) \
-  HALSIMGUI_DATASOURCE_INDEXED(cbname, id, DOUBLE, double)
-
-#define HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, TYPE, vtype)                \
-  class cbname##Source : public ::halsimgui::GuiDataSource {                  \
-   public:                                                                    \
-    explicit cbname##Source(int32_t index, int32_t channel)                   \
-        : GuiDataSource(id, index, channel),                                  \
-          m_index{index},                                                     \
-          m_channel{channel},                                                 \
-          m_callback{HALSIM_Register##cbname##Callback(                       \
-              index, channel, CallbackFunc, this, true)} {                    \
-      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
-    }                                                                         \
-                                                                              \
-    ~cbname##Source() {                                                       \
-      if (m_callback != 0)                                                    \
-        HALSIM_Cancel##cbname##Callback(m_index, m_channel, m_callback);      \
-    }                                                                         \
-                                                                              \
-    int32_t GetIndex() const { return m_index; }                              \
-                                                                              \
-    int32_t GetChannel() const { return m_channel; }                          \
-                                                                              \
-   private:                                                                   \
-    static void CallbackFunc(const char*, void* param,                        \
-                             const HAL_Value* value) {                        \
-      if (value->type == HAL_##TYPE)                                          \
-        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
-    }                                                                         \
-                                                                              \
-    int32_t m_index;                                                          \
-    int32_t m_channel;                                                        \
-    int32_t m_callback;                                                       \
-  }
-
-#define HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED2(cbname, id) \
-  HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, BOOLEAN, boolean)
-
-#define HALSIMGUI_DATASOURCE_DOUBLE_INDEXED2(cbname, id) \
-  HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, DOUBLE, double)
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALDataSource.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALDataSource.h
new file mode 100644
index 0000000..c04f358
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALDataSource.h
@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <glass/DataSource.h>
+
+#define HALSIMGUI_DATASOURCE(cbname, id, TYPE, vtype)                         \
+  class cbname##Source : public ::glass::DataSource {                         \
+   public:                                                                    \
+    cbname##Source()                                                          \
+        : DataSource(id),                                                     \
+          m_callback{                                                         \
+              HALSIM_Register##cbname##Callback(CallbackFunc, this, true)} {  \
+      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
+    }                                                                         \
+                                                                              \
+    ~cbname##Source() {                                                       \
+      if (m_callback != 0)                                                    \
+        HALSIM_Cancel##cbname##Callback(m_callback);                          \
+    }                                                                         \
+                                                                              \
+   private:                                                                   \
+    static void CallbackFunc(const char*, void* param,                        \
+                             const HAL_Value* value) {                        \
+      if (value->type == HAL_##TYPE)                                          \
+        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
+    }                                                                         \
+                                                                              \
+    int32_t m_callback;                                                       \
+  }
+
+#define HALSIMGUI_DATASOURCE_BOOLEAN(cbname, id) \
+  HALSIMGUI_DATASOURCE(cbname, id, BOOLEAN, boolean)
+
+#define HALSIMGUI_DATASOURCE_DOUBLE(cbname, id) \
+  HALSIMGUI_DATASOURCE(cbname, id, DOUBLE, double)
+
+#define HALSIMGUI_DATASOURCE_INT(cbname, id) \
+  HALSIMGUI_DATASOURCE(cbname, id, INT, int)
+
+#define HALSIMGUI_DATASOURCE_INDEXED(cbname, id, TYPE, vtype)                 \
+  class cbname##Source : public ::glass::DataSource {                         \
+   public:                                                                    \
+    explicit cbname##Source(int32_t index, int channel = -1)                  \
+        : DataSource(id, channel < 0 ? index : channel),                      \
+          m_index{index},                                                     \
+          m_channel{channel < 0 ? index : channel},                           \
+          m_callback{HALSIM_Register##cbname##Callback(index, CallbackFunc,   \
+                                                       this, true)} {         \
+      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
+    }                                                                         \
+                                                                              \
+    ~cbname##Source() {                                                       \
+      if (m_callback != 0)                                                    \
+        HALSIM_Cancel##cbname##Callback(m_index, m_callback);                 \
+    }                                                                         \
+                                                                              \
+    int32_t GetIndex() const { return m_index; }                              \
+                                                                              \
+    int GetChannel() const { return m_channel; }                              \
+                                                                              \
+   private:                                                                   \
+    static void CallbackFunc(const char*, void* param,                        \
+                             const HAL_Value* value) {                        \
+      if (value->type == HAL_##TYPE)                                          \
+        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
+    }                                                                         \
+                                                                              \
+    int32_t m_index;                                                          \
+    int m_channel;                                                            \
+    int32_t m_callback;                                                       \
+  }
+
+#define HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED(cbname, id, BOOLEAN, boolean)
+
+#define HALSIMGUI_DATASOURCE_DOUBLE_INDEXED(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED(cbname, id, DOUBLE, double)
+
+#define HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, TYPE, vtype)                \
+  class cbname##Source : public ::glass::DataSource {                         \
+   public:                                                                    \
+    explicit cbname##Source(int32_t index, int32_t channel)                   \
+        : DataSource(id, index, channel),                                     \
+          m_index{index},                                                     \
+          m_channel{channel},                                                 \
+          m_callback{HALSIM_Register##cbname##Callback(                       \
+              index, channel, CallbackFunc, this, true)} {                    \
+      SetDigital(HAL_##TYPE == HAL_BOOLEAN);                                  \
+    }                                                                         \
+                                                                              \
+    ~cbname##Source() {                                                       \
+      if (m_callback != 0)                                                    \
+        HALSIM_Cancel##cbname##Callback(m_index, m_channel, m_callback);      \
+    }                                                                         \
+                                                                              \
+    int32_t GetIndex() const { return m_index; }                              \
+                                                                              \
+    int32_t GetChannel() const { return m_channel; }                          \
+                                                                              \
+   private:                                                                   \
+    static void CallbackFunc(const char*, void* param,                        \
+                             const HAL_Value* value) {                        \
+      if (value->type == HAL_##TYPE)                                          \
+        static_cast<cbname##Source*>(param)->SetValue(value->data.v_##vtype); \
+    }                                                                         \
+                                                                              \
+    int32_t m_index;                                                          \
+    int32_t m_channel;                                                        \
+    int32_t m_callback;                                                       \
+  }
+
+#define HALSIMGUI_DATASOURCE_BOOLEAN_INDEXED2(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, BOOLEAN, boolean)
+
+#define HALSIMGUI_DATASOURCE_DOUBLE_INDEXED2(cbname, id) \
+  HALSIMGUI_DATASOURCE_INDEXED2(cbname, id, DOUBLE, double)
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALProvider.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALProvider.h
new file mode 100644
index 0000000..e3098db
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALProvider.h
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <glass/Model.h>
+#include <glass/Provider.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+namespace halsimgui {
+
+class HALProvider : public glass::Provider<> {
+ public:
+  explicit HALProvider(std::string_view iniName) : Provider{iniName} {}
+
+  void DisplayMenu() override;
+
+  glass::Model* GetModel(std::string_view name);
+
+  /**
+   * Returns true if outputs are disabled.
+   *
+   * @return true if outputs are disabled, false otherwise.
+   */
+  static bool AreOutputsDisabled();
+
+  /**
+   * Returns true if outputs are enabled.
+   *
+   * @return true if outputs are enabled, false otherwise.
+   */
+  static bool AreOutputsEnabled() { return !AreOutputsDisabled(); }
+
+ private:
+  void Update() override;
+
+  void Show(ViewEntry* entry, glass::Window* window) override;
+};
+
+}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALSimGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALSimGui.h
index 4a4c912..33ff242 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALSimGui.h
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/HALSimGui.h
@@ -1,150 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#ifdef __cplusplus
-#include <functional>
-#endif
+#include <glass/MainMenuBar.h>
+#include <glass/WindowManager.h>
+#include <glass/networktables/NetworkTablesProvider.h>
 
-extern "C" {
-
-void HALSIMGUI_Add(void* param, void (*initialize)(void*));
-void HALSIMGUI_AddExecute(void* param, void (*execute)(void*));
-void HALSIMGUI_AddWindow(const char* name, void* param, void (*display)(void*),
-                         int32_t flags);
-void HALSIMGUI_AddMainMenu(void* param, void (*menu)(void*));
-void HALSIMGUI_AddOptionMenu(void* param, void (*menu)(void*));
-void HALSIMGUI_SetWindowVisibility(const char* name, int32_t visibility);
-void HALSIMGUI_SetDefaultWindowPos(const char* name, float x, float y);
-void HALSIMGUI_SetDefaultWindowSize(const char* name, float width,
-                                    float height);
-void HALSIMGUI_SetWindowPadding(const char* name, float x, float y);
-int HALSIMGUI_AreOutputsDisabled(void);
-
-}  // extern "C"
-
-#ifdef __cplusplus
+#include "HALProvider.h"
 
 namespace halsimgui {
 
 class HALSimGui {
  public:
   static void GlobalInit();
-  static bool Initialize();
-  static void Main(void*);
-  static void Exit(void*);
 
-  /**
-   * Adds feature to GUI.  The initialize function is called once, immediately
-   * after the GUI (both GLFW and Dear ImGui) are initialized.
-   *
-   * @param initialize initialization function
-   * @param execute frame execution function
-   */
-  static void Add(std::function<void()> initialize);
+  static glass::MainMenuBar mainMenu;
+  static glass::WindowManager manager;
 
-  /**
-   * Adds per-frame executor to GUI.  The passed function is called on each
-   * Dear ImGui frame prior to window and menu functions.
-   *
-   * @param execute frame execution function
-   */
-  static void AddExecute(std::function<void()> execute);
-
-  /**
-   * Adds window to GUI.  The display function is called from within a
-   * ImGui::Begin()/End() block.  While windows can be created within the
-   * execute function passed to AddExecute(), using this function ensures the
-   * windows are consistently integrated with the rest of the GUI.
-   *
-   * On each Dear ImGui frame, AddExecute() functions are always called prior
-   * to AddWindow display functions.  Note that windows may be shaded or
-   * completely hidden, in which case this function will not be called.
-   * It's important to perform any processing steps that must be performed
-   * every frame in the AddExecute() function.
-   *
-   * @param name name of the window (title bar)
-   * @param display window contents display function
-   * @param flags Dear ImGui window flags
-   */
-  static void AddWindow(const char* name, std::function<void()> display,
-                        int flags = 0);
-
-  /**
-   * Adds to GUI's main menu bar.  The menu function is called from within a
-   * ImGui::BeginMainMenuBar()/EndMainMenuBar() block.  Usually it's only
-   * appropriate to create a menu with ImGui::BeginMenu()/EndMenu() inside of
-   * this function.
-   *
-   * On each Dear ImGui frame, AddExecute() functions are always called prior
-   * to AddMainMenu menu functions.
-   *
-   * @param menu menu display function
-   */
-  static void AddMainMenu(std::function<void()> menu);
-
-  /**
-   * Adds to GUI's option menu.  The menu function is called from within a
-   * ImGui::BeginMenu()/EndMenu() block.  Usually it's only appropriate to
-   * create menu items inside of this function.
-   *
-   * On each Dear ImGui frame, AddExecute() functions are always called prior
-   * to AddMainMenu menu functions.
-   *
-   * @param menu menu display function
-   */
-  static void AddOptionMenu(std::function<void()> menu);
-
-  enum WindowVisibility { kHide = 0, kShow, kDisabled };
-
-  /**
-   * Sets visibility of window added with AddWindow().
-   *
-   * @param name window name (same as name passed to AddWindow())
-   * @param visibility 0=hide, 1=show, 2=disabled (force-hide)
-   */
-  static void SetWindowVisibility(const char* name,
-                                  WindowVisibility visibility);
-
-  /**
-   * Sets default position of window added with AddWindow().
-   *
-   * @param name window name (same as name passed to AddWindow())
-   * @param x x location of upper left corner
-   * @param y y location of upper left corner
-   */
-  static void SetDefaultWindowPos(const char* name, float x, float y);
-
-  /**
-   * Sets default size of window added with AddWindow().
-   *
-   * @param name window name (same as name passed to AddWindow())
-   * @param width width
-   * @param height height
-   */
-  static void SetDefaultWindowSize(const char* name, float width, float height);
-
-  /**
-   * Sets internal padding of window added with AddWindow().
-   * @param name window name (same as name passed to AddWindow())
-   * @param x horizontal padding
-   * @param y vertical padding
-   */
-  static void SetWindowPadding(const char* name, float x, float y);
-
-  /**
-   * Returns true if outputs are disabled.
-   *
-   * @return true if outputs are disabled, false otherwise.
-   */
-  static bool AreOutputsDisabled();
+  static HALProvider halProvider;
+  static glass::NetworkTablesProvider ntProvider;
 };
 
 }  // namespace halsimgui
-
-#endif  // __cplusplus
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaver.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaver.h
deleted file mode 100644
index a36fa8b..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaver.h
+++ /dev/null
@@ -1,47 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <imgui.h>
-#include <imgui_internal.h>
-#include <wpi/DenseMap.h>
-
-namespace halsimgui {
-
-template <typename Info>
-class IniSaver {
- public:
-  explicit IniSaver(const char* typeName) : m_typeName(typeName) {}
-  void Initialize();
-
-  // pass through useful functions to map
-  Info& operator[](int index) { return m_map[index]; }
-
-  auto begin() { return m_map.begin(); }
-  auto end() { return m_map.end(); }
-  auto find(int index) { return m_map.find(index); }
-
-  auto begin() const { return m_map.begin(); }
-  auto end() const { return m_map.end(); }
-  auto find(int index) const { return m_map.find(index); }
-
- private:
-  static void* ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                        const char* name);
-  static void ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                       void* entry, const char* lineStr);
-  static void WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                       ImGuiTextBuffer* out_buf);
-
-  const char* m_typeName;
-  wpi::DenseMap<int, Info> m_map;
-};
-
-}  // namespace halsimgui
-
-#include "IniSaver.inl"
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaver.inl b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaver.inl
deleted file mode 100644
index 007ad5a..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaver.inl
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-template <typename Info>
-void IniSaver<Info>::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = m_typeName;
-  iniHandler.TypeHash = ImHashStr(m_typeName);
-  iniHandler.ReadOpenFn = ReadOpen;
-  iniHandler.ReadLineFn = ReadLine;
-  iniHandler.WriteAllFn = WriteAll;
-  iniHandler.UserData = this;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-}
-
-template <typename Info>
-void* IniSaver<Info>::ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                               const char* name) {
-  auto self = static_cast<IniSaver*>(handler->UserData);
-  int num;
-  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
-  return &self->m_map[num];
-}
-
-template <typename Info>
-void IniSaver<Info>::ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                              void* entry, const char* lineStr) {
-  auto element = static_cast<Info*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  element->ReadIni(name, value);
-}
-
-template <typename Info>
-void IniSaver<Info>::WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                              ImGuiTextBuffer* out_buf) {
-  auto self = static_cast<IniSaver*>(handler->UserData);
-  for (auto&& it : self->m_map) {
-    out_buf->appendf("[%s][%d]\n", self->m_typeName, it.first);
-    it.second.WriteIni(out_buf);
-    out_buf->append("\n");
-  }
-}
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverInfo.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverInfo.h
deleted file mode 100644
index 25fbe58..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverInfo.h
+++ /dev/null
@@ -1,58 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <imgui.h>
-#include <wpi/StringRef.h>
-
-namespace halsimgui {
-
-class NameInfo {
- public:
-  NameInfo() { m_name[0] = '\0'; }
-
-  bool HasName() const { return m_name[0] != '\0'; }
-  const char* GetName() const { return m_name; }
-  void GetName(char* buf, size_t size, const char* defaultName) const;
-  void GetName(char* buf, size_t size, const char* defaultName,
-               int index) const;
-  void GetName(char* buf, size_t size, const char* defaultName, int index,
-               int index2) const;
-  void GetLabel(char* buf, size_t size, const char* defaultName) const;
-  void GetLabel(char* buf, size_t size, const char* defaultName,
-                int index) const;
-  void GetLabel(char* buf, size_t size, const char* defaultName, int index,
-                int index2) const;
-
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out);
-  void PushEditNameId(int index);
-  void PushEditNameId(const char* name);
-  bool PopupEditName(int index);
-  bool PopupEditName(const char* name);
-  bool InputTextName(const char* label_id, ImGuiInputTextFlags flags = 0);
-
- private:
-  char m_name[64];
-};
-
-class OpenInfo {
- public:
-  OpenInfo() = default;
-  explicit OpenInfo(bool open) : m_open(open) {}
-
-  bool IsOpen() const { return m_open; }
-  void SetOpen(bool open) { m_open = open; }
-  bool ReadIni(wpi::StringRef name, wpi::StringRef value);
-  void WriteIni(ImGuiTextBuffer* out);
-
- private:
-  bool m_open = false;
-};
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverString.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverString.h
deleted file mode 100644
index 206d695..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverString.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <imgui.h>
-#include <imgui_internal.h>
-#include <wpi/StringMap.h>
-#include <wpi/StringRef.h>
-
-namespace halsimgui {
-
-template <typename Info>
-class IniSaverString {
- public:
-  explicit IniSaverString(const char* typeName) : m_typeName(typeName) {}
-  void Initialize();
-
-  // pass through useful functions to map
-  Info& operator[](wpi::StringRef key) { return m_map[key]; }
-
-  auto begin() { return m_map.begin(); }
-  auto end() { return m_map.end(); }
-  auto find(wpi::StringRef key) { return m_map.find(key); }
-
-  auto begin() const { return m_map.begin(); }
-  auto end() const { return m_map.end(); }
-  auto find(wpi::StringRef key) const { return m_map.find(key); }
-
- private:
-  static void* ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                        const char* name);
-  static void ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                       void* entry, const char* lineStr);
-  static void WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                       ImGuiTextBuffer* out_buf);
-
-  const char* m_typeName;
-  wpi::StringMap<Info> m_map;
-};
-
-}  // namespace halsimgui
-
-#include "IniSaverString.inl"
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverString.inl b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverString.inl
deleted file mode 100644
index 5ac7dc2..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverString.inl
+++ /dev/null
@@ -1,57 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-template <typename Info>
-void IniSaverString<Info>::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = m_typeName;
-  iniHandler.TypeHash = ImHashStr(m_typeName);
-  iniHandler.ReadOpenFn = ReadOpen;
-  iniHandler.ReadLineFn = ReadLine;
-  iniHandler.WriteAllFn = WriteAll;
-  iniHandler.UserData = this;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-}
-
-template <typename Info>
-void* IniSaverString<Info>::ReadOpen(ImGuiContext* ctx,
-                                     ImGuiSettingsHandler* handler,
-                                     const char* name) {
-  auto self = static_cast<IniSaverString*>(handler->UserData);
-  return &self->m_map[name];
-}
-
-template <typename Info>
-void IniSaverString<Info>::ReadLine(ImGuiContext* ctx,
-                                    ImGuiSettingsHandler* handler, void* entry,
-                                    const char* lineStr) {
-  auto element = static_cast<Info*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  element->ReadIni(name, value);
-}
-
-template <typename Info>
-void IniSaverString<Info>::WriteAll(ImGuiContext* ctx,
-                                    ImGuiSettingsHandler* handler,
-                                    ImGuiTextBuffer* out_buf) {
-  auto self = static_cast<IniSaverString*>(handler->UserData);
-  for (auto&& it : self->m_map) {
-    out_buf->appendf("[%s][%s]\n", self->m_typeName, it.getKey().data());
-    it.second.WriteIni(out_buf);
-    out_buf->append("\n");
-  }
-}
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverVector.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverVector.h
deleted file mode 100644
index 0816933..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverVector.h
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <vector>
-
-#include <imgui.h>
-#include <imgui_internal.h>
-
-namespace halsimgui {
-
-template <typename Info>
-class IniSaverVector : public std::vector<Info> {
- public:
-  explicit IniSaverVector(const char* typeName) : m_typeName(typeName) {}
-  void Initialize();
-
- private:
-  static void* ReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                        const char* name);
-  static void ReadLine(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                       void* entry, const char* lineStr);
-  static void WriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
-                       ImGuiTextBuffer* out_buf);
-
-  const char* m_typeName;
-};
-
-}  // namespace halsimgui
-
-#include "IniSaverVector.inl"
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverVector.inl b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverVector.inl
deleted file mode 100644
index b2979bc..0000000
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/IniSaverVector.inl
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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
-
-namespace halsimgui {
-
-template <typename Info>
-void IniSaverVector<Info>::Initialize() {
-  // hook ini handler to save settings
-  ImGuiSettingsHandler iniHandler;
-  iniHandler.TypeName = m_typeName;
-  iniHandler.TypeHash = ImHashStr(m_typeName);
-  iniHandler.ReadOpenFn = ReadOpen;
-  iniHandler.ReadLineFn = ReadLine;
-  iniHandler.WriteAllFn = WriteAll;
-  iniHandler.UserData = this;
-  ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
-}
-
-template <typename Info>
-void* IniSaverVector<Info>::ReadOpen(ImGuiContext* ctx,
-                                     ImGuiSettingsHandler* handler,
-                                     const char* name) {
-  auto self = static_cast<IniSaverVector*>(handler->UserData);
-  unsigned int num;
-  if (wpi::StringRef{name}.getAsInteger(10, num)) return nullptr;
-  if (num >= self->size()) self->resize(num + 1);
-  return &(*self)[num];
-}
-
-template <typename Info>
-void IniSaverVector<Info>::ReadLine(ImGuiContext* ctx,
-                                    ImGuiSettingsHandler* handler, void* entry,
-                                    const char* lineStr) {
-  auto element = static_cast<Info*>(entry);
-  wpi::StringRef line{lineStr};
-  auto [name, value] = line.split('=');
-  name = name.trim();
-  value = value.trim();
-  element->ReadIni(name, value);
-}
-
-template <typename Info>
-void IniSaverVector<Info>::WriteAll(ImGuiContext* ctx,
-                                    ImGuiSettingsHandler* handler,
-                                    ImGuiTextBuffer* out_buf) {
-  auto self = static_cast<IniSaverVector*>(handler->UserData);
-  for (size_t i = 0; i < self->size(); ++i) {
-    out_buf->appendf("[%s][%d]\n", self->m_typeName, static_cast<int>(i));
-    (*self)[i].WriteIni(out_buf);
-    out_buf->append("\n");
-  }
-}
-
-}  // namespace halsimgui
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h
index 8c24cc9..84f432c 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h
+++ b/third_party/allwpilib/simulation/halsim_gui/src/main/native/include/SimDeviceGui.h
@@ -1,109 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <stdint.h>
+#include <hal/SimDevice.h>
 
-#include <hal/Types.h>
-#include <hal/Value.h>
-
-#ifdef __cplusplus
-#include <functional>
-
-#include <imgui.h>
-#endif
-
-extern "C" {
-
-void HALSIMGUI_DeviceTreeHide(const char* name);
-void HALSIMGUI_DeviceTreeAdd(void* param, void (*execute)(void*));
-HAL_Bool HALSIMGUI_DeviceTreeDisplayValue(const char* name, HAL_Bool readonly,
-                                          struct HAL_Value* value,
-                                          const char** options,
-                                          int32_t numOptions);
-HAL_Bool HALSIMGUI_DeviceTreeStartDevice(const char* label, int32_t flags);
-void HALSIMGUI_DeviceTreeFinishDevice(void);
-
-}  // extern "C"
-
-#ifdef __cplusplus
+namespace glass {
+class DataSource;
+class DeviceTreeModel;
+}  // namespace glass
 
 namespace halsimgui {
 
-class GuiDataSource;
-
 class SimDeviceGui {
  public:
   static void Initialize();
-
-  /**
-   * Hides device on tree.
-   *
-   * @param name device name
-   */
-  static void Hide(const char* name);
-
-  /**
-   * Adds device to tree.  The execute function is called from within the
-   * device tree window context on every frame, so it should implement an
-   * TreeNodeEx() block for each device to display.
-   *
-   * @param execute execute function
-   */
-  static void Add(std::function<void()> execute);
-
-  /**
-   * Displays device value formatted the same way as SimDevice device values.
-   *
-   * @param name value name
-   * @param readonly prevent value from being modified by the user
-   * @param value value contents (modified in place)
-   * @param options options array for enum values
-   * @param numOptions size of options array for enum values
-   * @return True if value was modified by the user
-   */
-  static bool DisplayValue(const char* name, bool readonly, HAL_Value* value,
-                           const char** options = nullptr,
-                           int32_t numOptions = 0);
-
-  /**
-   * Displays device value formatted the same way as SimDevice device values.
-   *
-   * @param name value name
-   * @param readonly prevent value from being modified by the user
-   * @param value value contents (modified in place)
-   * @param source data source (may be nullptr)
-   * @param options options array for enum values
-   * @param numOptions size of options array for enum values
-   * @return True if value was modified by the user
-   */
-  static bool DisplayValueSource(const char* name, bool readonly,
-                                 HAL_Value* value, const GuiDataSource* source,
-                                 const char** options = nullptr,
-                                 int32_t numOptions = 0);
-
-  /**
-   * Wraps ImGui::CollapsingHeader() to provide consistency and open
-   * persistence.  As with the ImGui function, returns true if the tree node
-   * is expanded.  If returns true, call StopDevice() to finish the block.
-   *
-   * @param label label
-   * @param flags ImGuiTreeNodeFlags flags
-   * @return True if expanded
-   */
-  static bool StartDevice(const char* label, ImGuiTreeNodeFlags flags = 0);
-
-  /**
-   * Finish a device block started with StartDevice().
-   */
-  static void FinishDevice();
+  static glass::DataSource* GetValueSource(HAL_SimValueHandle handle);
+  static glass::DeviceTreeModel& GetDeviceTree();
 };
 
 }  // namespace halsimgui
-
-#endif  // __cplusplus
diff --git a/third_party/allwpilib/simulation/halsim_gui/src/test/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_gui/src/test/native/cpp/main.cpp
index c6b6c58..6aea19a 100644
--- a/third_party/allwpilib/simulation/halsim_gui/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_gui/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <hal/HALBase.h>
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/CMakeLists.txt b/third_party/allwpilib/simulation/halsim_ws_client/CMakeLists.txt
index d869f28..5bc99db 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/CMakeLists.txt
+++ b/third_party/allwpilib/simulation/halsim_ws_client/CMakeLists.txt
@@ -4,7 +4,7 @@
 
 file(GLOB halsim_ws_client_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_ws_client MODULE ${halsim_ws_client_src})
+add_library(halsim_ws_client SHARED ${halsim_ws_client_src})
 wpilib_target_warnings(halsim_ws_client)
 set_target_properties(halsim_ws_client PROPERTIES DEBUG_POSTFIX "d")
 target_link_libraries(halsim_ws_client PUBLIC hal halsim_ws_core)
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/README.md b/third_party/allwpilib/simulation/halsim_ws_client/README.md
new file mode 100644
index 0000000..c5111be
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_client/README.md
@@ -0,0 +1,13 @@
+# HAL WebSockets Client
+
+This is an extension that provides a client version of a WebSockets API for transmitting robot hardware interface state over a network.  See the [Robot Hardware Interface WebSockets API specification](../halsim_ws_core/doc/hardware_ws_api.md) for more details on the protocol.
+
+## Configuration
+
+The WebSockets client has a number of configuration options available through environment variables.
+
+``HALSIMWS_HOST``: The host to connect to.  Defaults to localhost.
+
+``HALSIMWS_PORT``: The port number to connect to.  Defaults to 3300.
+
+``HALSIMWS_URI``: The URI path to connect to.  Defaults to ``"/wpilibws"``.
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/build.gradle b/third_party/allwpilib/simulation/halsim_ws_client/build.gradle
index c0137a4..7d32586 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/build.gradle
+++ b/third_party/allwpilib/simulation/halsim_ws_client/build.gradle
@@ -28,7 +28,6 @@
                 }
 
                 lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static"
-
             }
         }
     }
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp
index 1efcefc..cf32a3f 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/dev/native/cpp/main.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
 #include <thread>
 
+#include <fmt/core.h>
 #include <hal/DriverStation.h>
 #include <hal/HALBase.h>
 #include <hal/Main.h>
@@ -24,9 +21,9 @@
   while (cycleCount < 100) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     cycleCount++;
-    std::cout << "Count: " << cycleCount << std::endl;
+    fmt::print("Count: {}\n", cycleCount);
   }
 
-  std::cout << "DONE" << std::endl;
+  fmt::print("DONE\n");
   HAL_ExitMain();
 }
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp
index 0b40a61..9c23885 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWS.cpp
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimWS.h"
 
+#include <cstdio>
+
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/uv/util.h>
 
 #include "HALSimWSClientConnection.h"
@@ -25,7 +24,7 @@
       m_providers(providers),
       m_simDevicesProvider(simDevicesProvider) {
   m_loop.error.connect([](uv::Error err) {
-    wpi::errs() << "HALSim WS Client libuv Error: " << err.str() << "\n";
+    fmt::print(stderr, "HALSim WS Client libuv Error: {}\n", err.str());
   });
 
   m_tcp_client = uv::Tcp::Create(m_loop);
@@ -42,26 +41,26 @@
   }
 
   const char* host = std::getenv("HALSIMWS_HOST");
-  if (host != NULL) {
+  if (host != nullptr) {
     m_host = host;
   } else {
     m_host = "localhost";
   }
 
   const char* port = std::getenv("HALSIMWS_PORT");
-  if (port != NULL) {
+  if (port != nullptr) {
     try {
       m_port = std::stoi(port);
     } catch (const std::invalid_argument& err) {
-      wpi::errs() << "Error decoding HALSIMWS_PORT (" << err.what() << ")\n";
+      fmt::print(stderr, "Error decoding HALSIMWS_PORT ({})\n", err.what());
       return false;
     }
   } else {
-    m_port = 8080;
+    m_port = 3300;
   }
 
   const char* uri = std::getenv("HALSIMWS_URI");
-  if (uri != NULL) {
+  if (uri != nullptr) {
     m_uri = uri;
   } else {
     m_uri = "/wpilibws";
@@ -86,13 +85,12 @@
         m_connect_timer->Start(uv::Timer::Time(kTcpConnectAttemptTimeout));
       });
 
-  m_tcp_client->closed.connect(
-      []() { wpi::outs() << "TCP connection closed\n"; });
+  m_tcp_client->closed.connect([]() { std::puts("TCP connection closed"); });
 
   // Set up the connection timer
-  wpi::outs() << "HALSimWS Initialized\n";
-  wpi::outs() << "Will attempt to connect to ws://" << m_host << ":" << m_port
-              << m_uri << "\n";
+  std::puts("HALSimWS Initialized");
+  fmt::print("Will attempt to connect to ws://{}:{}{}\n", m_host, m_port,
+             m_uri);
 
   // Set up the timer to attempt connection
   m_connect_timer->timeout.connect([this] { AttemptConnect(); });
@@ -105,7 +103,7 @@
 void HALSimWS::AttemptConnect() {
   m_connect_attempts++;
 
-  wpi::outs() << "Connection Attempt " << m_connect_attempts << "\n";
+  fmt::print("Connection Attempt {}\n", m_connect_attempts);
 
   struct sockaddr_in dest;
   uv::NameToAddr(m_host, m_port, &dest);
@@ -170,6 +168,6 @@
       provider->OnNetValueChanged(msg.at("data"));
     }
   } catch (wpi::json::exception& e) {
-    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+    fmt::print(stderr, "Error with incoming message: {}\n", e.what());
   }
 }
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp
index f019e27..dfa54d2 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClient.cpp
@@ -1,22 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimWSClient.h"
 
 #include <WSProviderContainer.h>
+#include <WSProvider_AddressableLED.h>
 #include <WSProvider_Analog.h>
+#include <WSProvider_BuiltInAccelerometer.h>
 #include <WSProvider_DIO.h>
 #include <WSProvider_DriverStation.h>
 #include <WSProvider_Encoder.h>
 #include <WSProvider_Joystick.h>
+#include <WSProvider_PCM.h>
 #include <WSProvider_PWM.h>
 #include <WSProvider_Relay.h>
 #include <WSProvider_RoboRIO.h>
 #include <WSProvider_SimDevice.h>
+#include <WSProvider_Solenoid.h>
 #include <WSProvider_dPWM.h>
 #include <wpi/EventLoopRunner.h>
 
@@ -36,16 +37,20 @@
       providers.Add(key, provider);
     };
 
+    HALSimWSProviderAddressableLED::Initialize(registerFunc);
     HALSimWSProviderAnalogIn::Initialize(registerFunc);
     HALSimWSProviderAnalogOut::Initialize(registerFunc);
+    HALSimWSProviderBuiltInAccelerometer::Initialize(registerFunc);
     HALSimWSProviderDIO::Initialize(registerFunc);
     HALSimWSProviderDigitalPWM::Initialize(registerFunc);
     HALSimWSProviderDriverStation::Initialize(registerFunc);
     HALSimWSProviderEncoder::Initialize(registerFunc);
     HALSimWSProviderJoystick::Initialize(registerFunc);
+    HALSimWSProviderPCM::Initialize(registerFunc);
     HALSimWSProviderPWM::Initialize(registerFunc);
     HALSimWSProviderRelay::Initialize(registerFunc);
     HALSimWSProviderRoboRIO::Initialize(registerFunc);
+    HALSimWSProviderSolenoid::Initialize(registerFunc);
 
     simDevices.Initialize(loop);
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp
index 5cfec5c..22dadb1 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/HALSimWSClientConnection.cpp
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimWSClientConnection.h"
 
-#include <wpi/raw_ostream.h>
+#include <cstdio>
+
+#include <fmt/format.h>
 #include <wpi/raw_uv_ostream.h>
 
 #include "HALSimWS.h"
@@ -20,29 +19,29 @@
   // Get a shared pointer to ourselves
   auto self = this->shared_from_this();
 
-  auto ws =
-      wpi::WebSocket::CreateClient(*m_stream, m_client->GetTargetUri(),
-                                   wpi::Twine{m_client->GetTargetHost()} + ":" +
-                                       wpi::Twine{m_client->GetTargetPort()});
+  auto ws = wpi::WebSocket::CreateClient(
+      *m_stream, m_client->GetTargetUri(),
+      fmt::format("{}:{}", m_client->GetTargetHost(),
+                  m_client->GetTargetPort()));
 
   ws->SetData(self);
 
   m_websocket = ws.get();
 
   // Hook up events
-  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+  m_websocket->open.connect_extended([this](auto conn, auto) {
     conn.disconnect();
 
     if (!m_client->RegisterWebsocket(shared_from_this())) {
-      wpi::errs() << "Unable to register websocket\n";
+      std::fputs("Unable to register websocket\n", stderr);
       return;
     }
 
     m_ws_connected = true;
-    wpi::outs() << "HALSimWS: WebSocket Connected\n";
+    std::puts("HALSimWS: WebSocket Connected");
   });
 
-  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+  m_websocket->text.connect([this](auto msg, bool) {
     if (!m_ws_connected) {
       return;
     }
@@ -53,7 +52,7 @@
     } catch (const wpi::json::parse_error& e) {
       std::string err("JSON parse failed: ");
       err += e.what();
-      wpi::errs() << err << "\n";
+      fmt::print(stderr, "{}\n", err);
       m_websocket->Fail(1003, err);
       return;
     }
@@ -61,9 +60,9 @@
     m_client->OnNetValueChanged(j);
   });
 
-  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+  m_websocket->closed.connect([this](uint16_t, auto) {
     if (m_ws_connected) {
-      wpi::outs() << "HALSimWS: Websocket Disconnected\n";
+      std::puts("HALSimWS: Websocket Disconnected");
       m_ws_connected = false;
 
       m_client->CloseWebsocket(shared_from_this());
@@ -93,8 +92,8 @@
                                   }
 
                                   if (err) {
-                                    wpi::errs() << err.str() << "\n";
-                                    wpi::errs().flush();
+                                    fmt::print(stderr, "{}\n", err.str());
+                                    std::fflush(stderr);
                                   }
                                 });
   });
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/main.cpp
index 1112e61..8e2e2fa 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/cpp/main.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
+#include <cstdio>
 #include <memory>
 
 #include <hal/Extensions.h>
-#include <wpi/raw_ostream.h>
 
 #include "HALSimWSClient.h"
 
@@ -22,7 +19,7 @@
 #endif
 
     int HALSIM_InitExtension(void) {
-  wpi::outs() << "HALSim WS Client Extension Initializing\n";
+  std::puts("HALSim WS Client Extension Initializing");
 
   HAL_OnShutdown(nullptr, [](void*) { gClient.reset(); });
 
@@ -31,7 +28,7 @@
     return -1;
   }
 
-  wpi::outs() << "HALSim WS Client Extension Initialized\n";
+  std::puts("HALSim WS Client Extension Initialized");
   return 0;
 }
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h
index e126ac1..9563482 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWS.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -44,8 +41,8 @@
 
   void OnNetValueChanged(const wpi::json& msg);
 
-  wpi::StringRef GetTargetHost() const { return m_host; }
-  wpi::StringRef GetTargetUri() const { return m_uri; }
+  const std::string& GetTargetHost() const { return m_host; }
+  const std::string& GetTargetUri() const { return m_uri; }
   int GetTargetPort() const { return m_port; }
   wpi::uv::Loop& GetLoop() { return m_loop; }
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h
index 5812140..f048b40 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClient.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h
index 1bd23e9..dddd885 100644
--- a/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h
+++ b/third_party/allwpilib/simulation/halsim_ws_client/src/main/native/include/HALSimWSClientConnection.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/README.md b/third_party/allwpilib/simulation/halsim_ws_core/README.md
new file mode 100644
index 0000000..ae15e0e
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/README.md
@@ -0,0 +1,5 @@
+# HAL WebSockets Core
+
+This is the common WebSockets implementation shared by the [HAL WebSockets Client](../halsim_ws_client/) and [HAL WebSockets Server](../halsim_ws_server/) extensions.  It is not a standalone extension.
+
+These extensions provide a WebSockets API for transmitting robot hardware interface state over a network and implement the [Robot Hardware Interface WebSockets API specification](doc/hardware_ws_api.md).  See the specification for more details on the protocol.
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/build.gradle b/third_party/allwpilib/simulation/halsim_ws_core/build.gradle
index ccf02ce..8e1f7ab 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/build.gradle
+++ b/third_party/allwpilib/simulation/halsim_ws_core/build.gradle
@@ -23,6 +23,7 @@
     apply from: "${rootDir}/shared/googletest.gradle"
 
     apply from: "${rootDir}/shared/config.gradle"
+    apply from: "${rootDir}/shared/plugins/publish.gradle"
 
     model {
         components {
@@ -41,7 +42,6 @@
                 binaries.all {
                     project(':hal').addHalDependency(it, 'shared')
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-
                 }
                 appendDebugPathToBinaries(binaries)
             }
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/doc/hardware_ws_api.md b/third_party/allwpilib/simulation/halsim_ws_core/doc/hardware_ws_api.md
new file mode 100644
index 0000000..b2b40fc
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/doc/hardware_ws_api.md
@@ -0,0 +1,342 @@
+# Robot Hardware Interface WebSockets API Specification
+
+- [Summary](#summary)
+- [Motivation](#motivation)
+- [References](#references)
+- [Design](#design)
+  - [WebSockets Protocol Configuration](#websockets-protocol-configuration)
+  - [Text Data Frames](#text-data-frames)
+  - [Robot Program Behavior](#robot-program-behavior)
+  - [Hardware Behavior](#hardware-behavior)
+  - [Hardware Messages](#hardware-messages)
+  - [CAN Messages](#can-messages)
+  - [RoboRIO Messages](#roborio-messages)
+  - [Other Device Messages ("SimDevice")](#other-device-messages-simdevice)
+
+## Summary
+
+A WebSockets API for transmitting robot hardware interface state over a network.  In typical use, one end of the connection is a robot program running in a desktop environment, and the other end of the connection is either a simulation engine, physical robot, or simulation dashboard GUI.
+
+## Motivation
+
+Provide a standard interface for 3rd party software to easily interface with a robot program running on a desktop computer.  Currently this is possible in WPILib only by writing a custom C++ simulation plugin.  Providing a standard text-based network interface to the "hardware" interface layer of a robot program lowers the barrier to entry for this type of development and enables unique capabilities such as direct control of a simple physical robot over a wireless link.
+
+## References
+
+- [RFC 6455](https://tools.ietf.org/html/rfc6455) The WebSocket Protocol
+- [RFC 7159](https://tools.ietf.org/html/rfc7159) The JavaScript Object Notation (JSON) Data Interchange Format
+
+## Design
+
+The messages in the protocol are based around typical representations of electronic physical devices, rather than higher level abstractions.  As such, the messages e.g. represent an analog input as a voltage, rather than a “potentiometer” device.  However, the “SimDevice” message can be used to communicate a higher level of abstraction device such as a gyro.  What this means is that simulation engines are responsible for implementing the mapping from their system knowledge (e.g. a joint angle) into an electronic value (e.g. an analog voltage).  See the Trades section for more discussion.
+
+### WebSockets Protocol Configuration
+
+Binary WebSocket frames are not used.  Text WebSocket frames are JSON messages for human readability and ease of debugging.
+
+Both clients and servers shall support unsecure connections (``ws:``) and may support secure connections (``wss:``).  In a trusted network environment (e.g. a robot network), clients that support secure connections should fall back to an unsecure connection if a secure connection is not available.
+
+The resource name for the websockets connection is ``/wpilibws``.  Servers shall reject a second connection to the same resource location as a currently active connection, but may support multiple connections via additional resource names; the resource name is used to prevent duplicate client connections (such as when a web browser is used).  Servers may support multiplexed HTTP file serving on the same port.
+
+The unsecure standard server port number shall be 3300.
+
+### Text Data Frames
+
+Each WebSockets text data frame shall consist of a single JSON object (“message“).
+
+Each message shall be a JSON object with three keys: a ``"type"`` key and lowercase string value describing the type of message, a ``"device"`` key and string value identifying the device, and a ``"data"`` key containing the message data as a JSON object.
+
+The contents of the data object depends on the message type; see the sections for each message for details for the standard message types.  The contents of the data object shall be transmitted as deltas (e.g. the message should only contain the values actually being changed).  Clients and servers are free to ignore data values they don’t find useful, and/or transmit additional data values not specified here.  Clients and servers shall ignore data values they don’t understand.
+
+Data keys have a prefix of either ``"<"``, ``">"``, or ``"<>"``.  This indicates whether the data value is an output from robot code (``"<"``), an input to the robot code (``">"``), or both (``"<>"``).
+
+Clients and servers shall ignore JSON messages that:
+
+* are not objects
+* have no ``"type"`` key, ``"device"`` key, or ``"data"`` key
+* have a ``"type"`` or ``"device"`` value that is not a string
+* have a ``"data"`` value that is not an object
+* have a ``"type"`` value that the client or server does not recognize
+
+### Robot Program Behavior
+
+The robot program may operate as either a client or a server.  Generally, the robot program only pays attention to data values with ``">"`` or ``"<>"`` prefixes in received messages.
+
+Upon initial connection, the robot program shall send a message for every initialized device in the program with the current state of that device (both input and output values).  When a robot program removes a device, it shall send a message for that device with ``"<init"`` = false.
+
+For example, if a robot program has an analog input configured for port 1, it will send a message upon initial connection with type ``"AI"``, device ``"1"``, ``"<init"`` true, and ``">voltage"`` set to an indeterminate value.  The remote "hardware" would send messages with type ``"AI"``, device ``"1"``, and ``">voltage"`` set to the (simulated or real) voltage.  When the robot program reads the voltage, it will read the last received ``">voltage"`` value.
+
+The initial state includes joystick and driver station state.
+
+### “Hardware“ Behavior
+
+The “hardware“ (which might be a full-fledged 3D simulation engine, a physical robot, or an interactive GUI) is responsible for mapping the robot program’s inputs and outputs into the real (or virtual) world.  For example, a robot program’s Analog Input 1 might show up as simply ``Analog Input #1`` on a GUI, connect to analog input port #1 on a physical robot, or map to a virtual potentiometer in a 3D simulation engine.
+
+### Hardware Messages
+
+| Type value              | Description                | Device value              |
+| ----------------------- | -------------------------- | ------------------------- |
+| [``"Accel"``][]         | Accelerometer              | Arbitrary device name     |
+| [``"AI"``][]            | Analog input               | Port index, e.g. "1", "2" |
+| [``"DIO"``][]           | Digital input/output       | Port index, e.g. "1", "2" |
+| [``"dPWM"``][]          | Duty cycle output          | Arbitrary device number   |
+| [``"DriverStation"``][] | Driver station / FMS state | Blank                     |
+| [``"DutyCycle"``][]     | Duty cycle input           | Arbitrary device name     |
+| [``"Encoder"``][]       | Quadrature encoder         | Arbitrary device number   |
+| [``"Gyro"``][]          | Gyro                       | Arbitrary device name     |
+| [``"Joystick"``][]      | Joystick data              | Joystick number           |
+| [``"PWM"``][]           | PWM output                 | Port index, e.g. "1", "2" |
+| [``"Relay"``][]         | Relay output               | Port index, e.g. "1", "2" |
+| [``"Solenoid"``][]      | Solenoid output            | Module +Port index, e.g. "0,1", "2,5" |
+
+#### Accelerometer ("Accel")
+
+[``"Accel"``]:#accelerometer-accel
+
+A 3-axis accelerometer.
+
+C++/Java implementation note: these are created as either BuiltInAccelerometer or SimDevice nodes where the device name is prefixed by ``"Accel:"``, for example ``"Accel:ADXL362[1]"``.  The BuiltInAccelerometer uses a device name of ``"BuiltInAccel"``.
+
+| Data Key     | Type    | Description                                          |
+| ------------ | ------- | ---------------------------------------------------- |
+| ``"<init"``  | Boolean | If accelerometer is initialized in the robot program |
+| ``"<range"`` | Float   | Desired range in G’s                                 |
+| ``">x"``     | Float   | Acceleration in G’s                                  |
+| ``">y"``     | Float   | Acceleration in G’s                                  |
+| ``">z"``     | Float   | Acceleration in G’s                                  |
+
+#### Analog Input ("AI")
+
+[``"AI"``]:#analog-input-ai
+
+The basic analog input just reads a voltage.
+
+| Data Key       | Type    | Description                                         |
+| -------------- | ------- | --------------------------------------------------- |
+| ``"<init"``    | Boolean | If analog input is initialized in the robot program |
+| ``">voltage"`` | Float   | Input voltage, in volts                             |
+
+#### Digital Input/Output ("DIO")
+
+[``"DIO"``]:#digital-inputoutput-dio
+
+| Data Key      | Type    | Description                                |
+| ------------- | ------- | ------------------------------------------ |
+| ``"<init"``   | Boolean | If DIO is initialized in the robot program |
+| ``"<input"``  | Boolean | True if input, false if output             |
+| ``"<>value"`` | Boolean | Input or output state                      |
+
+#### Duty Cycle Output ("dPWM")
+
+[``"dPWM"``]:#duty-cycle-output-dpwm
+
+| Data Key          | Type    | Description                                   |
+| ----------------- | ------- | --------------------------------------------- |
+| ``"<init"``       | Boolean | If output is initialized in the robot program |
+| ``"<duty_cycle"`` | Float   | Duty cycle % (0.0 to 1.0)                     |
+| ``"<dio_pin"``    | Integer | DIO pin number                                |
+
+#### Driver Station ("DriverStation")
+
+[``"DriverStation"``]:#driver-station-driverstation
+
+| Data Key          | Type    | Description                                      |
+| ----------------- | ------- | ------------------------------------------------ |
+| ``">new_data"``   | Boolean | One shot.  If set to true in a message, notifies the robot program that new DS and Joystick data is available. |
+| ``">enabled"``    | Boolean | True to enable the robot program |
+| ``">autonomous"`` | Boolean | True for autonomous mode; false for teleoperated mode |
+| ``">test"``       | Boolean | True for test mode; false for other modes |
+| ``">estop"``      | Boolean | True to emergency stop (no motor outputs) |
+| ``">fms"``        | Boolean | True if the DS is connected to a Field Management System (FMS) |
+| ``">ds"``         | Boolean | True if a DS application is connected |
+| ``">station"``    | String  | Station color and number; supported values are ``"red1"``, ``"red2"``, ``"red3"``, ``"blue1"``, ``"blue2"``, ``"blue3"``. |
+| ``">match_time"`` | Float   | Match time countdown, in seconds, for each match period (e.g. for 15 second period, starts at 15 and counts down to 0).  If not in a match, -1. |
+| ``">game_data"``  | String  | Game-specific data; arbitrary string contents |
+
+#### Duty Cycle Input ("DutyCycle")
+
+[``"DutyCycle"``]:#duty-cycle-input-dutycycle
+
+Duty Cycle inputs are commonly used for absolute encoders.  The position is accumulated over multiple rotations.
+
+C++/Java implementation note: these are created as SimDevice nodes where the device name is prefixed by ``"DutyCycle:"``, for example ``"DutyCycle:DutyCycleEncoder[1]"``.
+
+| Data Key         | Type    | Description                      |
+| ---------------- | ------- | -------------------------------- |
+| ``">connected"`` | Boolean | True if the encoder is connected |
+| ``">position"``  | Float   | The position in rotations        |
+
+#### Quadrature Encoder ("Encoder")
+
+[``"Encoder"``]:#quadrature-encoder-encoder
+
+A relative encoder.  For absolute encoders, use ``"DutyCycle"``.
+
+| Data Key              | Type    | Description                                         |
+| --------------------- | ------- | --------------------------------------------------- |
+| ``"<init"``           | Boolean | If encoder is initialized in the robot program      |
+| ``"<channel_a"``      | Integer | Digital channel number for “A” phase                |
+| ``"<channel_b"``      | Integer | Digital channel number for “B” phase                |
+| ``"<samples_to_avg"`` | Integer | Number of samples to average for period measurement |
+| ``">count"``          | Integer | Accumulated count (pulses)                          |
+| ``">period"``         | Float   | Period between pulses in seconds                    |
+
+#### Gyro ("Gyro")
+
+[``"Gyro"``]:#gyro-gyro
+
+A single axis or 3-axis gyro.  Single axis gyros only use the X angle parameter.
+
+C++/Java implementation note: these are created as SimDevice nodes where the device name is prefixed by ``"Gyro:"``, for example ``"Gyro:ADXRS450[1]"``.
+
+| Data Key          | Type    | Description                                               |
+| ----------------- | ------- | --------------------------------------------------------- |
+| ``"<init"``       | Boolean | If gyro is initialized in the robot program               |
+| ``"<range"``      | Float   | Gyro range in degrees/second (optional)                   |
+| ``">connected"``  | Boolean | True if the gyro is connected                             |
+| ``">angle_x"``    | Float   | The gyro angle in degrees                                 |
+| ``">angle_y"``    | Float   | The gyro angle in degrees                                 |
+| ``">angle_z"``    | Float   | The gyro angle in degrees                                 |
+| ``">rate_x"``     | Float   | The current gyro angular rate of change in degrees/second |
+| ``">rate_y"``     | Float   | The current gyro angular rate of change in degrees/second |
+| ``">rate_z"``     | Float   | The current gyro angular rate of change in degrees/second |
+
+#### Joystick Data ("Joystick")
+
+[``"Joystick"``]:#joystick-data-joystick
+
+Joystick data is an input to the robot program and should be updated for each input joystick on a periodic basis.  To enable synchronous updates of joystick and driver station state, joystick data is not made visible to the robot program until a DriverStation message with ``">new_data"`` set to true is received.
+
+| Data Key            | Type             | Description |
+| ------------------- | ---------------- | --- |
+| ``">axes"``         | Array of float   | One array element per axis; value is -1 to 1 range |
+| ``">povs"``         | Array of integer | One array element per POV; value is angle in degrees of the POV (e.g. 0, 90, 315) if pressed, or -1 if the POV is not pressed |
+| ``">buttons"``      | Array of boolean | One array element per button; true if button is pressed, false if button is released |
+| ``"<rumble_left"``  | Float            | Left rumble, value is 0-1 range |
+| ``"<rumble_right"`` | Float            | Right rumble, value is 0-1 range |
+
+#### PWM Output ("PWM")
+
+[``"PWM"``]:#pwm-output-pwm
+
+PWMs may be used to control either speed controllers or servos.  Typically only one of either ``"<speed"`` (for a speed controller) or ``"<position"`` (for a servo) is used for a given PWM.
+
+| Data Key        | Type    | Description                                |
+| --------------- | ------- | ------------------------------------------ |
+| ``"<init"``     | Boolean | If PWM is initialized in the robot program |
+| ``"<speed"``    | Float   | Speed, -1.0 to 1.0 range                   |
+| ``"<position"`` | Float   | Servo position, 0.0 to 1.0 range           |
+
+#### Relay Output ("Relay")
+
+[``"Relay"``]:#relay-output-relay
+
+| Data Key        | Type    | Description                                                    |
+| --------------- | ------- | -------------------------------------------------------------- |
+| ``"<init_fwd"`` | Boolean | If relay forward direction is initialized in the robot program |
+| ``"<init_rev"`` | Boolean | If relay reverse direction is initialized in the robot program |
+| ``"<fwd"``      | Boolean | True if forward direction is enabled                           |
+| ``"<rev"``      | Boolean | True if reverse direction is enabled                           |
+
+#### Solenoid Output ("Solenoid")
+
+[``"Solenoid"``]:#solenoid-output-solenoid
+
+Solenoids are used to control pneumatic pistons
+
+| Data Key        | Type    | Description                                     |
+| --------------- | ------- | ----------------------------------------------- |
+| ``"<init"``     | Boolean | If Solenoid is initialized in the robot program |
+| ``"<output"``   | Boolean | The state of the solenoid                       |
+
+### CAN Messages
+
+CAN messages all use a device value of ``"DeviceType[Number]"``, where the DeviceType is the vendor-specific CAN device type (motor controller class) name and Number is the CAN device number (the user-visible number passed to the device constructor).
+
+Many of the CAN messages use the same data key/values as other standard messages.  They are separately namespaced to make it easier for implementations to separate them from main robot controller messages.
+
+C++/Java implementation note: these are created as SimDevice nodes where the device name is prefixed by the message name and ``":"``, for example ``"CANMotor:Controller[1]"``.
+
+#### CANMotor
+
+Only one of ``"supplyCurrent"`` or ``"motorCurrent"`` should be sent by the hardware; the other value should be set to zero.  If ``"busVoltage"`` is not simulated it should also be set to zero.
+
+| Data Key             | Type             | Description                                      |
+| -------------------- | ---------------- | ------------------------------------------------ |
+| ``"<percentOutput"`` | Integer or Float | Percent output (-1 to 1 range)                   |
+| ``">supplyCurrent"`` | Float            | The supply current in amps as simulated/measured |
+| ``">motorCurrent"``  | Float            | The motor current in amps as simulated/measured  |
+| ``">busVoltage"``    | Float            | The bus voltage as simulated/measured            |
+
+#### CANEncoder
+
+A relative encoder (typically quadrature).  For absolute encoders, use ``"CANDutyCycle"``.
+
+| Data Key        | Type  | Description                      |
+| --------------- | ----- | -------------------------------- |
+| ``">position"`` | Float | Relative position, in rotations  |
+| ``">velocity"`` | Float | Velocity in rotations per second |
+
+#### CANGyro
+
+Uses the same keys as [``"Gyro"``][].
+
+#### CANAccel
+
+Uses the same keys as [``"Accel"``][].
+
+#### CANAIn
+
+Uses the same keys as [``"AI"``][].
+
+The device value may have a suffix for multiple inputs on a single CAN device; ``"-"`` followed by the input name or number is recommended but not required.
+
+#### CANDIO
+
+Uses the same keys as [``"DIO"``][].  This is commonly used for limit switches.
+
+The device value may have a suffix for multiple inputs on a single CAN device; ``"-"`` followed by the input name or number is recommended but not required.
+
+#### CANDutyCycle
+
+Uses the same keys as [``"DutyCycle"``][].
+
+The device value may have a suffix for multiple inputs on a single CAN device; ``"-"`` followed by the input name or number is recommended but not required.
+
+### RoboRIO Messages
+
+These messages are specific to the RoboRIO and will not likely be found in other simulators or hardware devices except in a very limited capacity (e.g. Vin voltage).
+
+| Type value        | Description         | Device value |
+| ----------------- | ------------------- | ------------ |
+| [``"RoboRIO"``][] | RoboRIO information | Blank        |
+
+#### RoboRIO
+
+[``"RoboRIO"``]:#roborio
+
+The RoboRIO.
+
+| Data Key           | Type    | Description                                 |
+| ------------------ | ------- | ------------------------------------------- |
+| ``">fpga_button"`` | Boolean | FPGA button state                           |
+| ``">vin_voltage"`` | Float   | Vin rail voltage                            |
+| ``">vin_current"`` | Float   | Vin rail current                            |
+| ``">6v_voltage"``  | Float   | 6V rail voltage                             |
+| ``">6v_current"``  | Float   | 6V rail current                             |
+| ``">6v_active"``   | Boolean | True if 6V rail active, false if inactive   |
+| ``">6v_faults"``   | Integer | Number of faults on 6V rail                 |
+| ``">5v_voltage"``  | Float   | 5V rail voltage                             |
+| ``">5v_current"``  | Float   | 5V rail current                             |
+| ``">5v_active"``   | Boolean | True if 5V rail active, false if inactive   |
+| ``">5v_faults"``   | Integer | Number of faults on 5V rail                 |
+| ``">3v3_voltage"`` | Float   | 3.3V rail voltage                           |
+| ``">3v3_current"`` | Float   | 3.3V rail current                           |
+| ``">3v3_active"``  | Boolean | True if 3.3V rail active, false if inactive |
+| ``">3v3_faults"``  | Integer | Number of faults on 3.3V rail               |
+
+### Other Device Messages ("SimDevice")
+
+[``"SimDevice"``]:#other-device-messages-simdevice
+
+A device type of ``"SimDevice"`` may be used for extending the protocol for arbitrary complex devices.  The device value is an arbitrary string, generally ``"DeviceName[Port/Index]"``, and the data keys are arbitrary and device-dependent.
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/doc/wpilib-ws.yaml b/third_party/allwpilib/simulation/halsim_ws_core/doc/wpilib-ws.yaml
new file mode 100644
index 0000000..b4ddb04
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/doc/wpilib-ws.yaml
@@ -0,0 +1,516 @@
+asyncapi: 2.0.0
+info:
+  title: WPILib WebSocket Remote Endpoint API
+  version: "1.0.0"
+  description: |
+    API to route WPILib HAL calls over WebSockets.
+  license:
+    name: WPILib BSD
+
+channels:
+  wpilibws:
+    description: General channel for WPILib WebSocket messages
+    publish:
+      operationId: wpilibwsPublish
+      message:
+        $ref: "#/components/messages/wpilibwsMsg"
+    subscribe:
+      operationId: wpilibwsSubscribe
+      message:
+        $ref: "#/components/messages/wpilibwsMsg"
+
+components:
+  messages:
+    wpilibwsMsg:
+      title: WPILib WebSocket Message
+      summary: Message envelope. Note that the "data" field contains a diff of the current state of a particular device. E.g. If only the "value" changes for a DIO device, then only the "<>value" field will be sent.
+      contentType: application/json
+      examples:
+      - payload:
+          type: PWM
+          device: "1"
+          data:
+            "<speed": 0.5
+      - payload:
+          type: DIO
+          device: "3"
+          data:
+            "<init": true
+      payload:
+        type: object
+        oneOf:
+            - $ref: "#/components/schemas/accelData"
+            - $ref: "#/components/schemas/aiData"
+            - $ref: "#/components/schemas/dioData"
+            - $ref: "#/components/schemas/dpwmData"
+            - $ref: "#/components/schemas/driverstationData"
+            - $ref: "#/components/schemas/dutycycleData"
+            - $ref: "#/components/schemas/encoderData"
+            - $ref: "#/components/schemas/gyroData"
+            - $ref: "#/components/schemas/joystickData"
+            - $ref: "#/components/schemas/pwmData"
+            - $ref: "#/components/schemas/relayData"
+            - $ref: "#/components/schemas/solenoidData"
+            - $ref: "#/components/schemas/roborioData"
+
+  schemas:
+    accelData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: Accel
+        device:
+          type: string
+          description: Arbitrary device name
+        data:
+          type: object
+          description: "Accelerometer Data (type: Accelerometer, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If accelerometer is initialized in the robot program"
+            <range:
+              type: number
+              description: "Desired range in G’s"
+            ">x":
+              type: number
+              description: "Acceleration in G’s "
+            ">y":
+              type: number
+              description: "Acceleration in G’s "
+            ">z":
+              type: number
+              description: "Acceleration in G’s "
+
+    aiData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: AI
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Analog Input Data (type: AI, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If analog input is initialized in the robot program"
+            ">voltage":
+              type: number
+              description: "Input voltage, in volts"
+
+    dioData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: DIO
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Digital Input/Output Data (type: DIO, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If DIO is initialized in the robot program"
+            <input:
+              type: boolean
+              description: "True if input, false if output"
+            <>value:
+              type: boolean
+              description: "Input or output state"
+
+    dpwmData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: dPWM
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Duty Cycle Output Data (type: dPWM, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If output is initialized in the robot program"
+            <duty_cycle:
+              type: number
+              description: "Duty cycle %"
+              minimum: 0.0
+              maximum: 1.0
+            <dio_pin:
+              type: integer
+              description: "DIO pin number"
+
+    driverstationData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: DriverStation
+        device:
+          type: string
+          description: Should be left blank
+        data:
+          type: object
+          description: "Driver Station Data (type: DriverStation)"
+          properties:
+            ">new_data":
+              type: boolean
+              description: "One shot.  If set to true in a message, notifies the robot program that new DS and Joystick data is available."
+            ">enabled":
+              type: boolean
+              description: "True to enable the robot program"
+            ">autonomous":
+              type: boolean
+              description: "True for autonomous mode; false for teleoperated mode"
+            ">test":
+              type: boolean
+              description: "True for test mode; false for other modes"
+            ">estop":
+              type: boolean
+              description: "True to emergency stop (no motor outputs)"
+            ">fms":
+              type: boolean
+              description: "True if the DS is connected to a Field Management System (FMS)"
+            ">ds":
+              type: boolean
+              description: "True if a DS application is connected"
+            ">station":
+              type: string
+              description: "Station color and number; supported values are 'red1', 'red2', 'red3', 'blue1', 'blue2', 'blue3'."
+            ">match_time":
+              type: number
+              description: "Match time countdown, in seconds, for each match period (e.g. for 15 second period, starts at 15 and counts down to 0).  If not in a match, -1."
+            ">game_data":
+              type: string
+              description: "Game-specific data; arbitrary string contents"
+
+    dutycycleData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: DutyCycle
+        device:
+          type: string
+          description: Arbitrary device name
+        data:
+          type: object
+          description: "Duty Cycle Input Data (type: DutyCycle, device: channel number)"
+          properties:
+            ">connected":
+              type: boolean
+              description: "True if the encoder is connected"
+            ">position":
+              type: number
+              description: "The position in rotations"
+
+    encoderData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: Encoder
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Quadrature Encoder Data (type: Encoder, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If encoder is initialized in the robot program"
+            <channel_a:
+              type: integer
+              description: "Digital channel number for 'A' phase"
+            <channel_b:
+              type: integer
+              description: "Digital channel number for 'B' phase"
+            <samples_to_avg:
+              type: integer
+              description: "Number of samples to average for period measurement"
+            ">count":
+              type: integer
+              description: "Accumulated count (pulses)"
+            ">period":
+              type: number
+              description: "Period between pulses in seconds"
+
+    gyroData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: Gyro
+        device:
+          type: string
+          description: Arbitrary device name
+        data:
+          type: object
+          description: "Gyro Data (type: Gyro, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If gyro is initialized in the robot program"
+            <range:
+              type: number
+              description: "Gyro range in degrees/second (optional)"
+            ">connected":
+              type: boolean
+              description: "True if the gyro is connected"
+            ">angle_x":
+              type: number
+              description: "The gyro angle in degrees"
+            ">angle_y":
+              type: number
+              description: "The gyro angle in degrees"
+            ">angle_z":
+              type: number
+              description: "The gyro angle in degrees"
+            ">rate_x":
+              type: number
+              description: "The current gyro angular rate of change in degrees/second"
+            ">rate_y":
+              type: number
+              description: "The current gyro angular rate of change in degrees/second"
+            ">rate_z":
+              type: number
+              description: "The current gyro angular rate of change in degrees/second"
+
+    joystickData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: Joystick
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Joystick Data (type: Joystick, device: channel number)"
+          properties:
+            ">axes":
+              type: array
+              items:
+                type: number
+                description: "Value of an individual axis on this joystick"
+                minimum: -1.0
+                maximum: 1.0
+            ">povs":
+              type: array
+              description: "One array element per POV; value is a"
+              items:
+                type: integer
+                description: "State of all POV switches on this joystick; an angle in degrees of the POV (e.g. 0, 90, 315) if pressed, or -1 if the POV is not pressed"
+            ">buttons":
+              type: array
+              description: State of all buttons on this joystick
+              items:
+                type: boolean
+                description: Pressed state of an individual button
+            <rumble_left:
+              type: number
+              description: "Left rumble"
+              minimum: 0.0
+              maximum: 1.0
+            <rumble_right:
+              type: number
+              description: "Right rumble"
+              minimum: 0.0
+              maximum: 1.0
+
+    pwmData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: PWM
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "PWM Output Data (type: PWM, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If PWM is initialized in the robot program"
+            <speed:
+              type: number
+              description: "Speed"
+              minimum: -1.0
+              maximum: 1.0
+            <position:
+              type: number
+              description: "Servo position"
+              minimum: 0.0
+              maximum: 1.0
+
+    relayData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: Relay
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Relay Output Data (type: Relay, device: channel number)"
+          properties:
+            <init_fwd:
+              type: boolean
+              description: "If relay forward direction is initialized in the robot program"
+            <init_rev:
+              type: boolean
+              description: "If relay reverse direction is initialized in the robot program"
+            <fwd:
+              type: boolean
+              description: "True if forward direction is enabled"
+            <rev:
+              type: boolean
+              description: "True if reverse direction is enabled"
+
+    solenoidData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: Solenoid
+        device:
+          type: string
+          description: Device Identifier (usually channel)
+        data:
+          type: object
+          description: "Solenoid Data (type: Solenoid, device: channel number)"
+          properties:
+            <init:
+              type: boolean
+              description: "If Solenoid is initialized in the robot program"
+            <output:
+              type: boolean
+              description: "The state of the solenoid"
+
+    roborioData:
+      type: object
+      required:
+      - type
+      - device
+      properties:
+        type:
+          type: string
+          description: Device Type (e.g. DIO/AI/PWM/Encoder etc)
+          const: RoboRIO
+        device:
+          type: string
+          description: Should be left blank
+        data:
+          type: object
+          description: "RoboRIO Data (type: RoboRIO)"
+          properties:
+            ">fpga_button":
+              type: boolean
+              description: "FPGA button state"
+            ">vin_voltage":
+              type: number
+              description: "Vin rail voltage"
+            ">vin_current":
+              type: number
+              description: "Vin rail current"
+            ">6v_voltage":
+              type: number
+              description: "6V rail voltage"
+            ">6v_current":
+              type: number
+              description: "6V rail current"
+            ">6v_active":
+              type: boolean
+              description: "True if 6V rail active, false if inactive"
+            ">6v_faults":
+              type: integer
+              description: "Number of faults on 6V rail"
+            ">5v_voltage":
+              type: number
+              description: "5V rail voltage"
+            ">5v_current":
+              type: number
+              description: "5V rail current"
+            ">5v_active":
+              type: boolean
+              description: "True if 5V rail active, false if inactive"
+            ">5v_faults":
+              type: integer
+              description: "Number of faults on 5V rail"
+            ">3v3_voltage":
+              type: number
+              description: "3.3V rail voltage"
+            ">3v3_current":
+              type: number
+              description: "3.3V rail current"
+            ">3v3_active":
+              type: boolean
+              description: "True if 3.3V rail active, false if inactive"
+            ">3v3_faults":
+              type: integer
+              description: "Number of faults on 3.3V rail"
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp
index 39057a1..16d92fa 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSBaseProvider.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSBaseProvider.h"
 
+#include <utility>
+
 namespace wpilibws {
 
-HALSimWSBaseProvider::HALSimWSBaseProvider(const std::string& key,
-                                           const std::string& type)
+HALSimWSBaseProvider::HALSimWSBaseProvider(std::string_view key,
+                                           std::string_view type)
     : m_key(key), m_type(type) {}
 
 void HALSimWSBaseProvider::OnNetValueChanged(const wpi::json& json) {
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp
index bc78202..4d6c024 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSHalProviders.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSHalProviders.h"
 
+#include <fmt/format.h>
+
 namespace wpilibws {
 
 void HALSimWSHalProvider::OnNetworkConnected(
@@ -19,7 +18,9 @@
   RegisterCallbacks();
 }
 
-void HALSimWSHalProvider::OnNetworkDisconnected() { CancelCallbacks(); }
+void HALSimWSHalProvider::OnNetworkDisconnected() {
+  CancelCallbacks();
+}
 
 void HALSimWSHalProvider::ProcessHalCallback(const wpi::json& payload) {
   auto ws = m_ws.lock();
@@ -31,10 +32,10 @@
 }
 
 HALSimWSHalChanProvider::HALSimWSHalChanProvider(int32_t channel,
-                                                 const std::string& key,
-                                                 const std::string& type)
+                                                 std::string_view key,
+                                                 std::string_view type)
     : HALSimWSHalProvider(key, type), m_channel(channel) {
-  m_deviceId = std::to_string(channel);
+  m_deviceId = fmt::format("{}", channel);
 }
 
 }  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_AddressableLED.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_AddressableLED.cpp
new file mode 100644
index 0000000..4cac7f5
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_AddressableLED.cpp
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WSProvider_AddressableLED.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/AddressableLEDData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                          \
+  HALSIM_RegisterAddressableLED##halsim##Callback(                        \
+      m_channel,                                                          \
+      [](const char* name, void* param, const struct HAL_Value* value) {  \
+        static_cast<HALSimWSProviderAddressableLED*>(param)               \
+            ->ProcessHalCallback(                                         \
+                {{jsonid, static_cast<ctype>(value->data.v_##haltype)}}); \
+      },                                                                  \
+      this, true)
+namespace wpilibws {
+void HALSimWSProviderAddressableLED::Initialize(
+    WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderAddressableLED>(
+      "AddressableLED", HAL_GetNumAddressableLEDs(), webRegisterFunc);
+}
+
+HALSimWSProviderAddressableLED::~HALSimWSProviderAddressableLED() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderAddressableLED::RegisterCallbacks() {
+  m_initCbKey = REGISTER(Initialized, "<init", bool, boolean);
+  m_outputPortCbKey = REGISTER(OutputPort, "<output_port", int32_t, int);
+  m_lengthCbKey = REGISTER(Length, "<length", int32_t, int);
+  m_runningCbKey = REGISTER(Running, "<running", bool, boolean);
+
+  m_dataCbKey = HALSIM_RegisterAddressableLEDDataCallback(
+      0,
+      [](const char* name, void* param, const unsigned char* buffer,
+         unsigned int count) {
+        auto provider = static_cast<HALSimWSProviderAddressableLED*>(param);
+
+        size_t numLeds = count / sizeof(HAL_AddressableLEDData);
+        const HAL_AddressableLEDData* data =
+            reinterpret_cast<const HAL_AddressableLEDData*>(buffer);
+
+        std::vector<wpi::json> jsonData;
+
+        for (size_t i = 0; i < numLeds; ++i) {
+          jsonData.push_back(
+              {{"r", data[i].r}, {"g", data[i].g}, {"b", data[i].b}});
+        }
+
+        wpi::json payload;
+        payload[">data"] = jsonData;
+
+        provider->ProcessHalCallback(payload);
+      },
+      this);
+}
+
+void HALSimWSProviderAddressableLED::CancelCallbacks() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderAddressableLED::DoCancelCallbacks() {
+  HALSIM_CancelAddressableLEDInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelAddressableLEDOutputPortCallback(m_channel, m_outputPortCbKey);
+  HALSIM_CancelAddressableLEDLengthCallback(m_channel, m_lengthCbKey);
+  HALSIM_CancelAddressableLEDRunningCallback(m_channel, m_runningCbKey);
+  HALSIM_CancelAddressableLEDDataCallback(m_channel, m_dataCbKey);
+
+  m_initCbKey = 0;
+  m_outputPortCbKey = 0;
+  m_lengthCbKey = 0;
+  m_runningCbKey = 0;
+  m_dataCbKey = 0;
+}
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp
index 91a8fa9..be19a1d 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Analog.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_Analog.h"
 
@@ -45,7 +42,9 @@
                                             webRegisterFunc);
 }
 
-HALSimWSProviderAnalogIn::~HALSimWSProviderAnalogIn() { DoCancelCallbacks(); }
+HALSimWSProviderAnalogIn::~HALSimWSProviderAnalogIn() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderAnalogIn::RegisterCallbacks() {
   m_initCbKey = REGISTER_AIN(Initialized, "<init", bool, boolean);
@@ -66,7 +65,9 @@
       REGISTER_AIN_ACCUM(Deadband, "<accum_deadband", int32_t, int);
 }
 
-void HALSimWSProviderAnalogIn::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderAnalogIn::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderAnalogIn::DoCancelCallbacks() {
   // Cancel callbacks
@@ -112,7 +113,9 @@
                                              webRegisterFunc);
 }
 
-HALSimWSProviderAnalogOut::~HALSimWSProviderAnalogOut() { CancelCallbacks(); }
+HALSimWSProviderAnalogOut::~HALSimWSProviderAnalogOut() {
+  CancelCallbacks();
+}
 
 void HALSimWSProviderAnalogOut::RegisterCallbacks() {
   m_initCbKey = REGISTER_AOUT(Initialized, "<init", bool, boolean);
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_BuiltInAccelerometer.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_BuiltInAccelerometer.cpp
new file mode 100644
index 0000000..fefbcd5
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_BuiltInAccelerometer.cpp
@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WSProvider_BuiltInAccelerometer.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/AccelerometerData.h>
+
+#define REGISTER(halsim, jsonid, ctype, haltype)                          \
+  HALSIM_RegisterAccelerometer##halsim##Callback(                         \
+      0,                                                                  \
+      [](const char* name, void* param, const struct HAL_Value* value) {  \
+        static_cast<HALSimWSProviderBuiltInAccelerometer*>(param)         \
+            ->ProcessHalCallback(                                         \
+                {{jsonid, static_cast<ctype>(value->data.v_##haltype)}}); \
+      },                                                                  \
+      this, true)
+namespace wpilibws {
+HALSimWSProviderBuiltInAccelerometer::HALSimWSProviderBuiltInAccelerometer()
+    : HALSimWSHalProvider("Accel/BuiltInAccel", "Accel") {
+  m_deviceId = "BuiltInAccel";
+}
+
+void HALSimWSProviderBuiltInAccelerometer::Initialize(
+    WSRegisterFunc webRegisterFunc) {
+  webRegisterFunc("Accel/BuiltInAccel",
+                  std::make_unique<HALSimWSProviderBuiltInAccelerometer>());
+}
+
+HALSimWSProviderBuiltInAccelerometer::~HALSimWSProviderBuiltInAccelerometer() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderBuiltInAccelerometer::RegisterCallbacks() {
+  m_activeCbKey = REGISTER(Active, "<init", bool, boolean);
+  m_rangeCbKey = HALSIM_RegisterAccelerometerRangeCallback(
+      0,
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        double range;
+        switch (value->data.v_int) {
+          case 0:
+            range = 2;
+            break;
+          case 1:
+            range = 4;
+            break;
+          case 2:
+          default:
+            range = 8;
+            break;
+        }
+        static_cast<HALSimWSProviderBuiltInAccelerometer*>(param)
+            ->ProcessHalCallback({{"<range", range}});
+      },
+      this, true);
+  m_xCbKey = REGISTER(X, ">x", double, double);
+  m_yCbKey = REGISTER(Y, ">y", double, double);
+  m_zCbKey = REGISTER(Z, ">z", double, double);
+}
+
+void HALSimWSProviderBuiltInAccelerometer::CancelCallbacks() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderBuiltInAccelerometer::DoCancelCallbacks() {
+  HALSIM_CancelAccelerometerActiveCallback(0, m_activeCbKey);
+  HALSIM_CancelAccelerometerRangeCallback(0, m_rangeCbKey);
+  HALSIM_CancelAccelerometerXCallback(0, m_xCbKey);
+  HALSIM_CancelAccelerometerYCallback(0, m_yCbKey);
+  HALSIM_CancelAccelerometerZCallback(0, m_zCbKey);
+
+  m_activeCbKey = 0;
+  m_rangeCbKey = 0;
+  m_xCbKey = 0;
+  m_yCbKey = 0;
+  m_zCbKey = 0;
+}
+
+void HALSimWSProviderBuiltInAccelerometer::OnNetValueChanged(
+    const wpi::json& json) {
+  wpi::json::const_iterator it;
+  if ((it = json.find(">x")) != json.end()) {
+    HALSIM_SetAccelerometerX(0, it.value());
+  }
+  if ((it = json.find(">y")) != json.end()) {
+    HALSIM_SetAccelerometerY(0, it.value());
+  }
+  if ((it = json.find(">z")) != json.end()) {
+    HALSIM_SetAccelerometerZ(0, it.value());
+  }
+}
+
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp
index 88fb29f..405a2df 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DIO.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_DIO.h"
 
@@ -26,7 +23,9 @@
                                        webRegisterFunc);
 }
 
-HALSimWSProviderDIO::~HALSimWSProviderDIO() { DoCancelCallbacks(); }
+HALSimWSProviderDIO::~HALSimWSProviderDIO() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderDIO::RegisterCallbacks() {
   m_initCbKey = REGISTER(Initialized, "<init", bool, boolean);
@@ -35,7 +34,9 @@
   m_inputCbKey = REGISTER(IsInput, "<input", bool, boolean);
 }
 
-void HALSimWSProviderDIO::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderDIO::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderDIO::DoCancelCallbacks() {
   HALSIM_CancelDIOInitializedCallback(m_channel, m_initCbKey);
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp
index b3cec6d..6fbc3d7 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_DriverStation.cpp
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_DriverStation.h"
 
 #include <algorithm>
 #include <atomic>
+#include <string_view>
 
 #include <hal/DriverStation.h>
 #include <hal/Extensions.h>
@@ -35,7 +33,7 @@
     registered = true;
     HAL_RegisterExtensionListener(
         nullptr, [](void*, const char* name, void* data) {
-          if (wpi::StringRef{name} == "ds_socket") {
+          if (std::string_view{name} == "ds_socket") {
             gDSSocketConnected = static_cast<std::atomic<bool>*>(data);
           }
         });
@@ -92,10 +90,12 @@
       },
       this, true);
 
-  m_matchTimeCbKey = REGISTER(MatchTime, "<match_time", double, double);
+  m_matchTimeCbKey = REGISTER(MatchTime, ">match_time", double, double);
 }
 
-void HALSimWSProviderDriverStation::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderDriverStation::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderDriverStation::DoCancelCallbacks() {
   HALSIM_CancelDriverStationEnabledCallback(m_enabledCbKey);
@@ -121,7 +121,9 @@
 
 void HALSimWSProviderDriverStation::OnNetValueChanged(const wpi::json& json) {
   // ignore if DS connected
-  if (gDSSocketConnected && *gDSSocketConnected) return;
+  if (gDSSocketConnected && *gDSSocketConnected) {
+    return;
+  }
 
   wpi::json::const_iterator it;
   if ((it = json.find(">enabled")) != json.end()) {
@@ -160,6 +162,14 @@
     }
   }
 
+  if ((it = json.find(">match_time")) != json.end()) {
+    HALSIM_SetDriverStationMatchTime(it.value());
+  }
+  if ((it = json.find(">game_data")) != json.end()) {
+    HALSIM_SetGameSpecificMessage(
+        it.value().get_ref<const std::string&>().c_str());
+  }
+
   // Only notify usercode if we get the new data message
   if ((it = json.find(">new_data")) != json.end()) {
     HALSIM_NotifyDriverStationNewData();
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
index 58d47d0..73f05dc 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Encoder.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_Encoder.h"
 
@@ -26,7 +23,9 @@
                                            webRegisterFunc);
 }
 
-HALSimWSProviderEncoder::~HALSimWSProviderEncoder() { DoCancelCallbacks(); }
+HALSimWSProviderEncoder::~HALSimWSProviderEncoder() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderEncoder::RegisterCallbacks() {
   // Special case for initialization since we will need to send
@@ -49,15 +48,35 @@
         provider->ProcessHalCallback(payload);
       },
       this, true);
-  m_countCbKey = REGISTER(Count, ">count", int32_t, int);
+  m_countCbKey = HALSIM_RegisterEncoderCountCallback(
+      m_channel,
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        auto provider = static_cast<HALSimWSProviderEncoder*>(param);
+        provider->ProcessHalCallback(
+            {{">count", static_cast<int32_t>(value->data.v_int +
+                                             provider->m_countOffset)}});
+      },
+      this, true);
   m_periodCbKey = REGISTER(Period, ">period", double, double);
-  m_resetCbKey = REGISTER(Reset, "<reset", bool, boolean);
+  m_resetCbKey = HALSIM_RegisterEncoderResetCallback(
+      m_channel,
+      [](const char* name, void* param, const struct HAL_Value* value) {
+        auto provider = static_cast<HALSimWSProviderEncoder*>(param);
+        bool reset = static_cast<bool>(value->data.v_boolean);
+        if (reset) {
+          provider->m_countOffset +=
+              HALSIM_GetEncoderCount(provider->m_channel);
+        }
+      },
+      this, true);
   m_reverseDirectionCbKey =
       REGISTER(ReverseDirection, "<reverse_direction", bool, boolean);
   m_samplesCbKey = REGISTER(SamplesToAverage, "<samples_to_avg", int32_t, int);
 }
 
-void HALSimWSProviderEncoder::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderEncoder::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderEncoder::DoCancelCallbacks() {
   HALSIM_CancelEncoderInitializedCallback(m_channel, m_initCbKey);
@@ -79,7 +98,8 @@
 void HALSimWSProviderEncoder::OnNetValueChanged(const wpi::json& json) {
   wpi::json::const_iterator it;
   if ((it = json.find(">count")) != json.end()) {
-    HALSIM_SetEncoderCount(m_channel, it.value());
+    HALSIM_SetEncoderCount(m_channel,
+                           static_cast<int32_t>(it.value()) - m_countOffset);
   }
   if ((it = json.find(">period")) != json.end()) {
     HALSIM_SetEncoderPeriod(m_channel, it.value());
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp
index 6f3f27b..3c16026 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Joystick.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_Joystick.h"
 
@@ -21,7 +18,9 @@
                                             webregisterFunc);
 }
 
-HALSimWSProviderJoystick::~HALSimWSProviderJoystick() { DoCancelCallbacks(); }
+HALSimWSProviderJoystick::~HALSimWSProviderJoystick() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderJoystick::RegisterCallbacks() {
   m_dsNewDataCbKey = HALSIM_RegisterDriverStationNewDataCallback(
@@ -57,16 +56,28 @@
           buttonsValues.push_back(((buttons.buttons >> i) & 0x1) == 1);
         }
 
+        // Rumble data
+        int64_t outputs = 0;
+        int32_t leftRumble = 0;
+        int32_t rightRumble = 0;
+        HALSIM_GetJoystickOutputs(provider->GetChannel(), &outputs, &leftRumble,
+                                  &rightRumble);
+
         payload[">axes"] = axesValues;
         payload[">povs"] = povsValues;
         payload[">buttons"] = buttonsValues;
+        payload["<outputs"] = outputs;
+        payload["<rumble_left"] = leftRumble;
+        payload["<rumble_right"] = rightRumble;
 
         provider->ProcessHalCallback(payload);
       },
       this, true);
 }
 
-void HALSimWSProviderJoystick::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderJoystick::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderJoystick::DoCancelCallbacks() {
   HALSIM_CancelDriverStationNewDataCallback(m_dsNewDataCbKey);
@@ -76,13 +87,16 @@
 
 void HALSimWSProviderJoystick::OnNetValueChanged(const wpi::json& json) {
   // ignore if DS connected
-  if (gDSSocketConnected && *gDSSocketConnected) return;
+  if (gDSSocketConnected && *gDSSocketConnected) {
+    return;
+  }
 
   wpi::json::const_iterator it;
   if ((it = json.find(">axes")) != json.end()) {
     HAL_JoystickAxes axes{};
     axes.count =
-        std::min(it.value().size(), (wpi::json::size_type)HAL_kMaxJoystickAxes);
+        std::min(it.value().size(),
+                 static_cast<wpi::json::size_type>(HAL_kMaxJoystickAxes));
     for (int i = 0; i < axes.count; i++) {
       axes.axes[i] = it.value()[i];
     }
@@ -92,10 +106,11 @@
 
   if ((it = json.find(">buttons")) != json.end()) {
     HAL_JoystickButtons buttons{};
-    buttons.count = std::min(it.value().size(), (wpi::json::size_type)32);
+    buttons.count =
+        std::min(it.value().size(), static_cast<wpi::json::size_type>(32));
     for (int i = 0; i < buttons.count; i++) {
       if (it.value()[i]) {
-        buttons.buttons |= 1 << (i - 1);
+        buttons.buttons |= 1 << i;
       }
     }
 
@@ -105,7 +120,8 @@
   if ((it = json.find(">povs")) != json.end()) {
     HAL_JoystickPOVs povs{};
     povs.count =
-        std::min(it.value().size(), (wpi::json::size_type)HAL_kMaxJoystickPOVs);
+        std::min(it.value().size(),
+                 static_cast<wpi::json::size_type>(HAL_kMaxJoystickPOVs));
     for (int i = 0; i < povs.count; i++) {
       povs.povs[i] = it.value()[i];
     }
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PCM.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PCM.cpp
new file mode 100644
index 0000000..b7eab57
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PCM.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WSProvider_PCM.h"
+
+#include <hal/Ports.h>
+#include <hal/simulation/CTREPCMData.h>
+
+#define REGISTER_CTREPCM(halsim, jsonid, ctype, haltype)                 \
+  HALSIM_RegisterCTREPCM##halsim##Callback(                              \
+      m_channel,                                                         \
+      [](const char* name, void* param, const struct HAL_Value* value) { \
+        static_cast<HALSimWSProviderPCM*>(param)->ProcessHalCallback(    \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});    \
+      },                                                                 \
+      this, true)
+namespace wpilibws {
+void HALSimWSProviderPCM::Initialize(WSRegisterFunc webRegisterFunc) {
+  CreateProviders<HALSimWSProviderPCM>("CTREPCM", HAL_GetNumCTREPCMModules(),
+                                       webRegisterFunc);
+}
+
+HALSimWSProviderPCM::~HALSimWSProviderPCM() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderPCM::RegisterCallbacks() {
+  m_initCbKey = REGISTER_CTREPCM(Initialized, "<init", bool, boolean);
+  m_onCbKey = REGISTER_CTREPCM(CompressorOn, ">on", bool, boolean);
+  m_closedLoopCbKey =
+      REGISTER_CTREPCM(ClosedLoopEnabled, "<closed_loop", bool, boolean);
+  m_pressureSwitchCbKey =
+      REGISTER_CTREPCM(PressureSwitch, ">pressure_switch", bool, boolean);
+  m_currentCbKey =
+      REGISTER_CTREPCM(CompressorCurrent, ">current", double, double);
+}
+
+void HALSimWSProviderPCM::CancelCallbacks() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderPCM::DoCancelCallbacks() {
+  HALSIM_CancelCTREPCMInitializedCallback(m_channel, m_initCbKey);
+  HALSIM_CancelCTREPCMCompressorOnCallback(m_channel, m_onCbKey);
+  HALSIM_CancelCTREPCMClosedLoopEnabledCallback(m_channel, m_closedLoopCbKey);
+  HALSIM_CancelCTREPCMPressureSwitchCallback(m_channel, m_pressureSwitchCbKey);
+  HALSIM_CancelCTREPCMCompressorCurrentCallback(m_channel, m_currentCbKey);
+
+  m_initCbKey = 0;
+  m_onCbKey = 0;
+  m_closedLoopCbKey = 0;
+  m_pressureSwitchCbKey = 0;
+  m_currentCbKey = 0;
+}
+
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp
index cd0f4f4..5792ef5 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_PWM.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_PWM.h"
 
@@ -25,7 +22,9 @@
                                        webRegisterFunc);
 }
 
-HALSimWSProviderPWM::~HALSimWSProviderPWM() { DoCancelCallbacks(); }
+HALSimWSProviderPWM::~HALSimWSProviderPWM() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderPWM::RegisterCallbacks() {
   m_initCbKey = REGISTER(Initialized, "<init", bool, boolean);
@@ -36,7 +35,9 @@
   m_zeroLatchCbKey = REGISTER(ZeroLatch, "<zero_latch", bool, boolean);
 }
 
-void HALSimWSProviderPWM::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderPWM::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderPWM::DoCancelCallbacks() {
   HALSIM_CancelPWMInitializedCallback(m_channel, m_initCbKey);
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp
index e51658f..4bb7e55 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Relay.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_Relay.h"
 
@@ -25,7 +22,9 @@
                                          webRegisterFunc);
 }
 
-HALSimWSProviderRelay::~HALSimWSProviderRelay() { DoCancelCallbacks(); }
+HALSimWSProviderRelay::~HALSimWSProviderRelay() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderRelay::RegisterCallbacks() {
   m_initFwdCbKey = REGISTER(InitializedForward, "<init_fwd", bool, boolean);
@@ -34,7 +33,9 @@
   m_revCbKey = REGISTER(Reverse, "<rev", bool, boolean);
 }
 
-void HALSimWSProviderRelay::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderRelay::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderRelay::DoCancelCallbacks() {
   HALSIM_CancelRelayInitializedForwardCallback(m_channel, m_initFwdCbKey);
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp
index 42e2cc8..c2998cb 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_RoboRIO.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_RoboRIO.h"
 
@@ -24,7 +21,9 @@
   CreateSingleProvider<HALSimWSProviderRoboRIO>("RoboRIO", webRegisterFunc);
 }
 
-HALSimWSProviderRoboRIO::~HALSimWSProviderRoboRIO() { DoCancelCallbacks(); }
+HALSimWSProviderRoboRIO::~HALSimWSProviderRoboRIO() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderRoboRIO::RegisterCallbacks() {
   m_fpgaCbKey = REGISTER(FPGAButton, ">fpga_button", bool, boolean);
@@ -47,7 +46,9 @@
   m_3v3FaultsCbKey = REGISTER(UserFaults3V3, ">3v3_faults", int32_t, int);
 }
 
-void HALSimWSProviderRoboRIO::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderRoboRIO::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderRoboRIO::DoCancelCallbacks() {
   HALSIM_CancelRoboRioFPGAButtonCallback(m_fpgaCbKey);
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp
index ee8fded..93852b6 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_SimDevice.cpp
@@ -1,17 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_SimDevice.h"
 
+#include <algorithm>
+#include <cmath>
+
+#include <fmt/format.h>
 #include <hal/Ports.h>
+#include <wpi/StringExtras.h>
 
 namespace wpilibws {
 
-HALSimWSProviderSimDevice::~HALSimWSProviderSimDevice() { CancelCallbacks(); }
+HALSimWSProviderSimDevice::~HALSimWSProviderSimDevice() {
+  CancelCallbacks();
+}
 
 void HALSimWSProviderSimDevice::OnNetworkConnected(
     std::shared_ptr<HALSimBaseWebSocketConnection> ws) {
@@ -69,15 +73,38 @@
           break;
         case HAL_DOUBLE:
           value.data.v_double = it.value();
+          value.data.v_double -= vd->second->doubleOffset;
           break;
-        case HAL_ENUM:
+        case HAL_ENUM: {
+          if (it->is_string()) {
+            auto& options = vd->second->options;
+            auto& str = it.value().get_ref<const std::string&>();
+            auto optionIt =
+                std::find_if(options.begin(), options.end(),
+                             [&](const std::string& v) { return v == str; });
+            if (optionIt != options.end()) {
+              value.data.v_enum = optionIt - options.begin();
+            }
+          } else if (it->is_number()) {
+            auto& values = vd->second->optionValues;
+            double num = it.value();
+            auto valueIt = std::find_if(
+                values.begin(), values.end(),
+                [&](double v) { return std::fabs(v - num) < 1e-4; });
+            if (valueIt != values.end()) {
+              value.data.v_enum = valueIt - values.begin();
+            }
+          }
           value.data.v_enum = it.value();
           break;
+        }
         case HAL_INT:
           value.data.v_int = it.value();
+          value.data.v_int -= vd->second->intOffset;
           break;
         case HAL_LONG:
           value.data.v_long = it.value();
+          value.data.v_long -= vd->second->intOffset;
           break;
         default:
           break;
@@ -88,15 +115,51 @@
   }
 }
 
+void HALSimWSProviderSimDevice::OnValueCreatedStatic(
+    const char* name, void* param, HAL_SimValueHandle handle, int32_t direction,
+    const struct HAL_Value* value) {
+  (reinterpret_cast<HALSimWSProviderSimDevice*>(param))
+      ->OnValueCreated(name, handle, direction, value);
+}
+
 void HALSimWSProviderSimDevice::OnValueCreated(const char* name,
                                                HAL_SimValueHandle handle,
-                                               HAL_Bool readonly,
+                                               int32_t direction,
                                                const struct HAL_Value* value) {
-  wpi::Twine key = wpi::Twine(readonly ? "<" : "<>") + name;
+  const char* prefix = "";
+  if (name[0] != '<' && name[0] != '>') {
+    switch (direction) {
+      case HAL_SimValueInput:
+        prefix = ">";
+        break;
+      case HAL_SimValueOutput:
+        prefix = "<";
+        break;
+      case HAL_SimValueBidir:
+        prefix = "<>";
+        break;
+      default:
+        break;
+    }
+  }
+  std::string key = fmt::format("{}{}", prefix, name);
   auto data = std::make_unique<SimDeviceValueData>();
   data->device = this;
   data->handle = handle;
-  data->key = key.str();
+  data->key = key;
+  if (value->type == HAL_ENUM) {
+    int32_t numOptions = 0;
+
+    const char** options = HALSIM_GetSimValueEnumOptions(handle, &numOptions);
+    data->options.reserve(numOptions);
+    for (int32_t i = 0; i < numOptions; ++i) {
+      data->options.emplace_back(options[i]);
+    }
+
+    const double* values =
+        HALSIM_GetSimValueEnumDoubleValues(handle, &numOptions);
+    data->optionValues.assign(values, values + numOptions);
+  }
   data->valueType = value->type;
 
   auto param = data.get();
@@ -109,7 +172,14 @@
   int32_t cbKey = HALSIM_RegisterSimValueChangedCallback(
       handle, param, HALSimWSProviderSimDevice::OnValueChangedStatic, true);
 
-  m_simValueChangedCbKeys[key.str()] = cbKey;
+  m_simValueChangedCbKeys[key] = cbKey;
+}
+
+void HALSimWSProviderSimDevice::OnValueChangedStatic(
+    const char* name, void* param, HAL_SimValueHandle handle, int32_t direction,
+    const struct HAL_Value* value) {
+  auto valueData = (reinterpret_cast<SimDeviceValueData*>(param));
+  valueData->device->OnValueChanged(valueData, value);
 }
 
 void HALSimWSProviderSimDevice::OnValueChanged(SimDeviceValueData* valueData,
@@ -121,16 +191,25 @@
         ProcessHalCallback({{valueData->key, value->data.v_boolean}});
         break;
       case HAL_DOUBLE:
-        ProcessHalCallback({{valueData->key, value->data.v_double}});
+        ProcessHalCallback(
+            {{valueData->key, value->data.v_double + valueData->doubleOffset}});
         break;
-      case HAL_ENUM:
-        ProcessHalCallback({{valueData->key, value->data.v_enum}});
+      case HAL_ENUM: {
+        int v = value->data.v_enum;
+        if (v >= 0 && v < static_cast<int>(valueData->optionValues.size())) {
+          ProcessHalCallback({{valueData->key, valueData->optionValues[v]}});
+        } else if (v >= 0 && v < static_cast<int>(valueData->options.size())) {
+          ProcessHalCallback({{valueData->key, valueData->options[v]}});
+        }
         break;
+      }
       case HAL_INT:
-        ProcessHalCallback({{valueData->key, value->data.v_int}});
+        ProcessHalCallback(
+            {{valueData->key, value->data.v_int + valueData->intOffset}});
         break;
       case HAL_LONG:
-        ProcessHalCallback({{valueData->key, value->data.v_long}});
+        ProcessHalCallback(
+            {{valueData->key, value->data.v_long + valueData->intOffset}});
         break;
       default:
         break;
@@ -138,23 +217,61 @@
   }
 }
 
+void HALSimWSProviderSimDevice::OnValueResetStatic(
+    const char* name, void* param, HAL_SimValueHandle handle, int32_t direction,
+    const struct HAL_Value* value) {
+  auto valueData = (reinterpret_cast<SimDeviceValueData*>(param));
+  valueData->device->OnValueReset(valueData, value);
+}
+
+void HALSimWSProviderSimDevice::OnValueReset(SimDeviceValueData* valueData,
+                                             const struct HAL_Value* value) {
+  switch (value->type) {
+    case HAL_BOOLEAN:
+    case HAL_ENUM:
+      break;
+    case HAL_DOUBLE:
+      valueData->doubleOffset += value->data.v_double;
+      break;
+    case HAL_INT:
+      valueData->intOffset += value->data.v_int;
+      break;
+    case HAL_LONG:
+      valueData->intOffset += value->data.v_long;
+      break;
+    default:
+      break;
+  }
+}
+
 void HALSimWSProviderSimDevice::ProcessHalCallback(const wpi::json& payload) {
   auto ws = m_ws.lock();
   if (ws) {
     wpi::json netValue = {
-        {"type", "SimDevices"}, {"device", m_deviceId}, {"data", payload}};
+        {"type", m_type}, {"device", m_deviceId}, {"data", payload}};
     ws->OnSimValueChanged(netValue);
   }
 }
 
-HALSimWSProviderSimDevices::~HALSimWSProviderSimDevices() { CancelCallbacks(); }
+HALSimWSProviderSimDevices::~HALSimWSProviderSimDevices() {
+  CancelCallbacks();
+}
 
 void HALSimWSProviderSimDevices::DeviceCreatedCallback(
     const char* name, HAL_SimDeviceHandle handle) {
-  auto key = (wpi::Twine("SimDevices/") + name).str();
-  auto dev = std::make_shared<HALSimWSProviderSimDevice>(
-      handle, key, wpi::Twine(name).str());
-  m_providers.Add(key, dev);
+  // Map "Accel:Foo" -> type=Accel, device=Foo
+  auto [type, id] = wpi::split(name, ':');
+  std::shared_ptr<HALSimWSProviderSimDevice> dev;
+  if (id.empty()) {
+    auto key = fmt::format("SimDevice/{}", type);
+    dev = std::make_shared<HALSimWSProviderSimDevice>(handle, key, "SimDevice",
+                                                      type);
+    m_providers.Add(key, dev);
+  } else {
+    auto key = fmt::format("{}/{}", type, id);
+    dev = std::make_shared<HALSimWSProviderSimDevice>(handle, key, type, id);
+    m_providers.Add(key, dev);
+  }
 
   if (m_ws) {
     m_exec->Call([this, dev]() { dev->OnNetworkConnected(GetWSConnection()); });
@@ -170,7 +287,7 @@
   m_deviceCreatedCbKey = HALSIM_RegisterSimDeviceCreatedCallback(
       "", this, HALSimWSProviderSimDevices::DeviceCreatedCallbackStatic, 1);
   m_deviceFreedCbKey = HALSIM_RegisterSimDeviceFreedCallback(
-      "", this, HALSimWSProviderSimDevices::DeviceFreedCallbackStatic);
+      "", this, HALSimWSProviderSimDevices::DeviceFreedCallbackStatic, false);
 
   m_exec = UvExecFn::Create(loop, [](auto out, LoopFn func) {
     func();
@@ -191,6 +308,8 @@
   m_ws = hws;
 }
 
-void HALSimWSProviderSimDevices::OnNetworkDisconnected() { m_ws = nullptr; }
+void HALSimWSProviderSimDevices::OnNetworkDisconnected() {
+  m_ws = nullptr;
+}
 
 }  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp
new file mode 100644
index 0000000..a26a4e5
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_Solenoid.cpp
@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "WSProvider_Solenoid.h"
+
+#include <fmt/format.h>
+#include <hal/Ports.h>
+#include <hal/simulation/CTREPCMData.h>
+
+#define REGISTER_SOLENOID(halsim, jsonid, ctype, haltype)                  \
+  HALSIM_RegisterCTREPCMSolenoid##halsim##Callback(                        \
+      m_pcmIndex, m_solenoidIndex,                                         \
+      [](const char* name, void* param, const struct HAL_Value* value) {   \
+        static_cast<HALSimWSProviderSolenoid*>(param)->ProcessHalCallback( \
+            {{jsonid, static_cast<ctype>(value->data.v_##haltype)}});      \
+      },                                                                   \
+      this, true)
+
+namespace wpilibws {
+void HALSimWSProviderSolenoid::Initialize(WSRegisterFunc webRegisterFunc) {
+  for (int32_t CTREPCMIndex = 0; CTREPCMIndex < HAL_GetNumCTREPCMModules();
+       ++CTREPCMIndex) {
+    for (int32_t solenoidIndex = 0;
+         solenoidIndex < HAL_GetNumCTRESolenoidChannels(); ++solenoidIndex) {
+      auto key = fmt::format("Solenoid/{},{}", CTREPCMIndex, solenoidIndex);
+      auto ptr = std::make_unique<HALSimWSProviderSolenoid>(
+          CTREPCMIndex, solenoidIndex, key, "Solenoid");
+      webRegisterFunc(key, std::move(ptr));
+    }
+  }
+}
+
+HALSimWSProviderSolenoid::HALSimWSProviderSolenoid(int32_t CTREPCMChannel,
+                                                   int32_t solenoidChannel,
+                                                   const std::string& key,
+                                                   const std::string& type)
+    : HALSimWSHalProvider(key, type),
+      m_pcmIndex(CTREPCMChannel),
+      m_solenoidIndex(solenoidChannel) {
+  m_deviceId = fmt::format("{},{}", m_pcmIndex, solenoidChannel);
+}
+
+HALSimWSProviderSolenoid::~HALSimWSProviderSolenoid() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderSolenoid::RegisterCallbacks() {
+  // m_initCbKey = REGISTER_SOLENOID(Initialized, "<init", bool, boolean);
+  m_outputCbKey = REGISTER_SOLENOID(Output, "<output", bool, boolean);
+}
+
+void HALSimWSProviderSolenoid::CancelCallbacks() {
+  DoCancelCallbacks();
+}
+
+void HALSimWSProviderSolenoid::DoCancelCallbacks() {
+  // HALSIM_CancelCTREPCMSolenoidInitializedCallback(m_pcmIndex,
+  // m_solenoidIndex,
+  //                                                 m_initCbKey);
+  HALSIM_CancelCTREPCMSolenoidOutputCallback(m_pcmIndex, m_solenoidIndex,
+                                             m_outputCbKey);
+
+  m_initCbKey = 0;
+  m_outputCbKey = 0;
+}
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp
index ce4fd79..af8a8e4 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/cpp/WSProvider_dPWM.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WSProvider_dPWM.h"
 
@@ -36,7 +33,9 @@
   m_pinCbKey = REGISTER(Pin, "<dio_pin", int32_t, int);
 }
 
-void HALSimWSProviderDigitalPWM::CancelCallbacks() { DoCancelCallbacks(); }
+void HALSimWSProviderDigitalPWM::CancelCallbacks() {
+  DoCancelCallbacks();
+}
 
 void HALSimWSProviderDigitalPWM::DoCancelCallbacks() {
   HALSIM_CancelDigitalPWMInitializedCallback(m_channel, m_initCbKey);
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h
index 9f01100..bcf29c2 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/HALSimBaseWebSocketConnection.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h
index 46873b7..268c47a 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSBaseProvider.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <wpi/json.h>
 
@@ -19,9 +17,9 @@
 
 class HALSimWSBaseProvider {
  public:
-  explicit HALSimWSBaseProvider(const std::string& key,
-                                const std::string& type = "");
-  virtual ~HALSimWSBaseProvider() {}
+  explicit HALSimWSBaseProvider(std::string_view key,
+                                std::string_view type = "");
+  virtual ~HALSimWSBaseProvider() = default;
 
   HALSimWSBaseProvider(const HALSimWSBaseProvider&) = delete;
   HALSimWSBaseProvider& operator=(const HALSimWSBaseProvider&) = delete;
@@ -38,8 +36,8 @@
   // network -> sim
   virtual void OnNetValueChanged(const wpi::json& json);
 
-  const std::string GetDeviceType() { return m_type; }
-  const std::string GetDeviceId() { return m_deviceId; }
+  const std::string& GetDeviceType() { return m_type; }
+  const std::string& GetDeviceId() { return m_deviceId; }
 
  protected:
   // sim -> network
@@ -47,7 +45,7 @@
   std::string m_key;
 
   std::string m_type;
-  std::string m_deviceId = "";
+  std::string m_deviceId;
 };
 
 }  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h
index be32607..e1bc1f8 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <hal/simulation/NotifyListener.h>
 #include <wpi/json.h>
@@ -19,19 +17,20 @@
 
 namespace wpilibws {
 
-typedef void (*HALCbRegisterIndexedFunc)(int32_t index,
-                                         HAL_NotifyCallback callback,
+using HALCbRegisterIndexedFunc = void (*)(int32_t index,
+                                          HAL_NotifyCallback callback,
+                                          void* param, HAL_Bool initialNotify);
+using HALCbRegisterSingleFunc = void (*)(HAL_NotifyCallback callback,
                                          void* param, HAL_Bool initialNotify);
-typedef void (*HALCbRegisterSingleFunc)(HAL_NotifyCallback callback,
-                                        void* param, HAL_Bool initialNotify);
 
 // provider generates diffs based on values
 class HALSimWSHalProvider : public HALSimWSBaseProvider {
  public:
   using HALSimWSBaseProvider::HALSimWSBaseProvider;
 
-  void OnNetworkConnected(std::shared_ptr<HALSimBaseWebSocketConnection> ws);
-  void OnNetworkDisconnected();
+  void OnNetworkConnected(
+      std::shared_ptr<HALSimBaseWebSocketConnection> ws) override;
+  void OnNetworkDisconnected() override;
 
   void ProcessHalCallback(const wpi::json& payload);
 
@@ -43,8 +42,8 @@
 // provider generates per-channel diffs
 class HALSimWSHalChanProvider : public HALSimWSHalProvider {
  public:
-  explicit HALSimWSHalChanProvider(int32_t channel, const std::string& key,
-                                   const std::string& type);
+  explicit HALSimWSHalChanProvider(int32_t channel, std::string_view key,
+                                   std::string_view type);
 
   int32_t GetChannel() { return m_channel; }
 
@@ -53,16 +52,15 @@
 };
 
 using WSRegisterFunc = std::function<void(
-    const std::string&, std::shared_ptr<HALSimWSBaseProvider>)>;
+    std::string_view, std::shared_ptr<HALSimWSBaseProvider>)>;
 
 template <typename T>
-void CreateProviders(const std::string& prefix, int32_t numChannels,
+void CreateProviders(std::string_view prefix, int32_t numChannels,
                      WSRegisterFunc webRegisterFunc);
 
 template <typename T>
-void CreateSingleProvider(const std::string& key,
-                          WSRegisterFunc webRegisterFunc);
-
-#include "WSHalProviders.inl"
+void CreateSingleProvider(std::string_view key, WSRegisterFunc webRegisterFunc);
 
 }  // namespace wpilibws
+
+#include "WSHalProviders.inc"
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inc b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inc
new file mode 100644
index 0000000..c9ce9d1
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inc
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+#include <utility>
+
+#include <fmt/format.h>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+
+template <typename T>
+void CreateProviders(std::string_view prefix, int numChannels,
+                     WSRegisterFunc webRegisterFunc) {
+  for (int32_t i = 0; i < numChannels; i++) {
+    auto key = fmt::format("{}/{}", prefix, i);
+    auto ptr = std::make_unique<T>(i, key, prefix);
+    webRegisterFunc(key, std::move(ptr));
+  }
+}
+
+template <typename T>
+void CreateSingleProvider(std::string_view key,
+                          WSRegisterFunc webRegisterFunc) {
+  auto ptr = std::make_unique<T>(key, key);
+  webRegisterFunc(key, std::move(ptr));
+}
+
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl
deleted file mode 100644
index 6bcdcdd..0000000
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSHalProviders.inl
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-#include <utility>
-
-template <typename T>
-void CreateProviders(const std::string& prefix, int numChannels,
-                     WSRegisterFunc webRegisterFunc) {
-  for (int32_t i = 0; i < numChannels; i++) {
-    auto key = (prefix + "/" + wpi::Twine(i)).str();
-    auto ptr = std::make_unique<T>(i, key, prefix);
-    webRegisterFunc(key, std::move(ptr));
-  }
-}
-
-template <typename T>
-void CreateSingleProvider(const std::string& key,
-                          WSRegisterFunc webRegisterFunc) {
-  auto ptr = std::make_unique<T>(key, key);
-  webRegisterFunc(key, std::move(ptr));
-}
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h
index 7cd1240..7c8f861 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProviderContainer.h
@@ -1,15 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <shared_mutex>
+#include <string_view>
 
 #include <wpi/StringMap.h>
 
@@ -22,17 +21,18 @@
   using ProviderPtr = std::shared_ptr<HALSimWSBaseProvider>;
   using IterFn = std::function<void(ProviderPtr)>;
 
-  ProviderContainer() {}
+  ProviderContainer() = default;
 
   ProviderContainer(const ProviderContainer&) = delete;
   ProviderContainer& operator=(const ProviderContainer&) = delete;
 
-  void Add(wpi::StringRef key, std::shared_ptr<HALSimWSBaseProvider> provider) {
+  void Add(std::string_view key,
+           std::shared_ptr<HALSimWSBaseProvider> provider) {
     std::unique_lock lock(m_mutex);
     m_providers[key] = provider;
   }
 
-  void Delete(wpi::StringRef key) {
+  void Delete(std::string_view key) {
     std::unique_lock lock(m_mutex);
     m_providers.erase(key);
   }
@@ -44,7 +44,7 @@
     }
   }
 
-  ProviderPtr Get(wpi::StringRef key) {
+  ProviderPtr Get(std::string_view key) {
     std::shared_lock lock(m_mutex);
     auto fiter = m_providers.find(key);
     return fiter != m_providers.end() ? fiter->second : ProviderPtr();
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_AddressableLED.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_AddressableLED.h
new file mode 100644
index 0000000..3970774
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_AddressableLED.h
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+class HALSimWSProviderAddressableLED : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderAddressableLED() override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_outputPortCbKey = 0;
+  int32_t m_lengthCbKey = 0;
+  int32_t m_runningCbKey = 0;
+  int32_t m_dataCbKey = 0;
+};
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h
index 8ebc235..ec5771e 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Analog.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,13 +15,13 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderAnalogIn();
+  ~HALSimWSProviderAnalogIn() override;
 
   void OnNetValueChanged(const wpi::json& json) override;
 
  protected:
   void RegisterCallbacks() override;
-  void CancelCallbacks() override;
+  void CancelCallbacks() final;
   void DoCancelCallbacks();
 
  private:
@@ -44,11 +41,11 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderAnalogOut();
+  ~HALSimWSProviderAnalogOut() override;
 
  protected:
   void RegisterCallbacks() override;
-  void CancelCallbacks() override;
+  void CancelCallbacks() final;
   void DoCancelCallbacks();
 
  private:
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_BuiltInAccelerometer.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_BuiltInAccelerometer.h
new file mode 100644
index 0000000..62568e7
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_BuiltInAccelerometer.h
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+class HALSimWSProviderBuiltInAccelerometer : public HALSimWSHalProvider {
+ public:
+  HALSimWSProviderBuiltInAccelerometer();
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalProvider::HALSimWSHalProvider;
+  ~HALSimWSProviderBuiltInAccelerometer() override;
+
+  void OnNetValueChanged(const wpi::json& json) override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_activeCbKey = 0;
+  int32_t m_rangeCbKey = 0;
+  int32_t m_xCbKey = 0;
+  int32_t m_yCbKey = 0;
+  int32_t m_zCbKey = 0;
+};
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h
index 8d6da7e..5fc3296 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DIO.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderDIO();
+  ~HALSimWSProviderDIO() override;
 
   void OnNetValueChanged(const wpi::json& json) override;
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h
index c61419c..f164a70 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_DriverStation.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalProvider::HALSimWSHalProvider;
-  ~HALSimWSProviderDriverStation();
+  ~HALSimWSProviderDriverStation() override;
 
   void OnNetValueChanged(const wpi::json& json) override;
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
index 882e447..b7601b3 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Encoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderEncoder();
+  ~HALSimWSProviderEncoder() override;
 
   void OnNetValueChanged(const wpi::json& json) override;
 
@@ -34,6 +31,8 @@
   int32_t m_resetCbKey = 0;
   int32_t m_reverseDirectionCbKey = 0;
   int32_t m_samplesCbKey = 0;
+
+  int32_t m_countOffset = 0;
 };
 
 }  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h
index 4b06bee..edabad0 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Joystick.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderJoystick();
+  ~HALSimWSProviderJoystick() override;
 
   void OnNetValueChanged(const wpi::json& json) override;
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PCM.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PCM.h
new file mode 100644
index 0000000..d12d6df
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PCM.h
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+class HALSimWSProviderPCM : public HALSimWSHalChanProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
+  ~HALSimWSProviderPCM() override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_initCbKey = 0;
+  int32_t m_onCbKey = 0;
+  int32_t m_closedLoopCbKey = 0;
+  int32_t m_pressureSwitchCbKey = 0;
+  int32_t m_currentCbKey = 0;
+};
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h
index 19be17a..badb02d 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_PWM.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderPWM();
+  ~HALSimWSProviderPWM() override;
 
  protected:
   void RegisterCallbacks() override;
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h
index aa3f84f..d712865 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Relay.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderRelay();
+  ~HALSimWSProviderRelay() override;
 
  protected:
   void RegisterCallbacks() override;
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h
index 9ffc2ed..0b8e44e 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_RoboRIO.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalProvider::HALSimWSHalProvider;
-  ~HALSimWSProviderRoboRIO();
+  ~HALSimWSProviderRoboRIO() override;
 
   void OnNetValueChanged(const wpi::json& json) override;
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h
index 63e5d22..c4d4347 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_SimDevice.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
+#include <vector>
 
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
@@ -27,18 +26,22 @@
   HALSimWSProviderSimDevice* device;
   HAL_SimValueHandle handle;
   std::string key;
+  std::vector<std::string> options;
+  std::vector<double> optionValues;
   HAL_Type valueType;
+  double doubleOffset = 0;
+  int64_t intOffset = 0;
 };
 
 class HALSimWSProviderSimDevice : public HALSimWSBaseProvider {
  public:
-  HALSimWSProviderSimDevice(HAL_SimDeviceHandle handle, const std::string& key,
-                            const std::string& deviceId)
-      : HALSimWSBaseProvider(key, "SimDevices"), m_handle(handle) {
+  HALSimWSProviderSimDevice(HAL_SimDeviceHandle handle, std::string_view key,
+                            std::string_view type, std::string_view deviceId)
+      : HALSimWSBaseProvider(key, type), m_handle(handle) {
     m_deviceId = deviceId;
   }
 
-  ~HALSimWSProviderSimDevice();
+  ~HALSimWSProviderSimDevice() override;
 
   void OnNetworkConnected(
       std::shared_ptr<HALSimBaseWebSocketConnection> ws) override;
@@ -51,23 +54,23 @@
 
  private:
   static void OnValueCreatedStatic(const char* name, void* param,
-                                   HAL_SimValueHandle handle, HAL_Bool readonly,
-                                   const struct HAL_Value* value) {
-    (reinterpret_cast<HALSimWSProviderSimDevice*>(param))
-        ->OnValueCreated(name, handle, readonly, value);
-  }
+                                   HAL_SimValueHandle handle, int32_t direction,
+                                   const struct HAL_Value* value);
   void OnValueCreated(const char* name, HAL_SimValueHandle handle,
-                      HAL_Bool readonly, const struct HAL_Value* value);
+                      int32_t direction, const struct HAL_Value* value);
 
   static void OnValueChangedStatic(const char* name, void* param,
-                                   HAL_SimValueHandle handle, HAL_Bool readonly,
-                                   const struct HAL_Value* value) {
-    auto valueData = (reinterpret_cast<SimDeviceValueData*>(param));
-    valueData->device->OnValueChanged(valueData, value);
-  }
+                                   HAL_SimValueHandle handle, int32_t direction,
+                                   const struct HAL_Value* value);
   void OnValueChanged(SimDeviceValueData* valueData,
                       const struct HAL_Value* value);
 
+  static void OnValueResetStatic(const char* name, void* param,
+                                 HAL_SimValueHandle handle, int32_t direction,
+                                 const struct HAL_Value* value);
+  void OnValueReset(SimDeviceValueData* valueData,
+                    const struct HAL_Value* value);
+
   void CancelCallbacks();
 
   wpi::StringMap<std::unique_ptr<SimDeviceValueData>> m_valueHandles;
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Solenoid.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Solenoid.h
new file mode 100644
index 0000000..b6bc47a
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_Solenoid.h
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include "WSHalProviders.h"
+
+namespace wpilibws {
+class HALSimWSProviderSolenoid : public HALSimWSHalProvider {
+ public:
+  static void Initialize(WSRegisterFunc webRegisterFunc);
+
+  explicit HALSimWSProviderSolenoid(int32_t pcmChannel, int32_t solenoidChannel,
+                                    const std::string& key,
+                                    const std::string& type);
+  ~HALSimWSProviderSolenoid() override;
+
+ protected:
+  void RegisterCallbacks() override;
+  void CancelCallbacks() override;
+  void DoCancelCallbacks();
+
+ private:
+  int32_t m_pcmIndex = 0;
+  int32_t m_solenoidIndex = 0;
+
+  int32_t m_initCbKey = 0;
+  int32_t m_outputCbKey = 0;
+};
+}  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h
index 49b7000..0340a59 100644
--- a/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h
+++ b/third_party/allwpilib/simulation/halsim_ws_core/src/main/native/include/WSProvider_dPWM.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,7 +15,7 @@
   static void Initialize(WSRegisterFunc webRegisterFunc);
 
   using HALSimWSHalChanProvider::HALSimWSHalChanProvider;
-  ~HALSimWSProviderDigitalPWM();
+  ~HALSimWSProviderDigitalPWM() override;
 
  protected:
   void RegisterCallbacks() override;
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/CMakeLists.txt b/third_party/allwpilib/simulation/halsim_ws_server/CMakeLists.txt
index d1c7983..e5b55c8 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/CMakeLists.txt
+++ b/third_party/allwpilib/simulation/halsim_ws_server/CMakeLists.txt
@@ -4,7 +4,7 @@
 
 file(GLOB halsim_ws_server_src src/main/native/cpp/*.cpp)
 
-add_library(halsim_ws_server MODULE ${halsim_ws_server_src})
+add_library(halsim_ws_server SHARED ${halsim_ws_server_src})
 wpilib_target_warnings(halsim_ws_server)
 set_target_properties(halsim_ws_server PROPERTIES DEBUG_POSTFIX "d")
 target_link_libraries(halsim_ws_server PUBLIC hal halsim_ws_core)
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/README.md b/third_party/allwpilib/simulation/halsim_ws_server/README.md
new file mode 100644
index 0000000..c75414d
--- /dev/null
+++ b/third_party/allwpilib/simulation/halsim_ws_server/README.md
@@ -0,0 +1,15 @@
+# HAL WebSockets Server
+
+This is an extension that provides a server version of a WebSockets API for transmitting robot hardware interface state over a network.  See the [Robot Hardware Interface WebSockets API specification](../halsim_ws_core/doc/hardware_ws_api.md) for more details on the protocol.
+
+## Configuration
+
+The WebSockets server has a number of configuration options available through environment variables.
+
+``HALSIMWS_SYSROOT``: The local directory to serve non-websocket HTTP content from (e.g. HTML files) starting from `/`.  Defaults to the `sim` subdirectory of the current working directory.
+
+``HALSIMWS_USERROOT``: The local directory to serve non-websocket HTTP content from (e.g. HTML files) starting from `/user/`.  Defaults to the `sim/user` subdirectory of the current working directory.
+
+``HALSIMWS_PORT``: The port number to listen at.  Defaults to 3300.
+
+``HALSIMWS_URI``: The URI path to use for WebSockets connections.  Defaults to ``"/wpilibws"``.
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/build.gradle b/third_party/allwpilib/simulation/halsim_ws_server/build.gradle
index 6c6be48..16cb24e 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/build.gradle
+++ b/third_party/allwpilib/simulation/halsim_ws_server/build.gradle
@@ -59,11 +59,10 @@
             lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
             lib library: pluginName, linkage: 'shared'
             if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
             }
         }
     }
-
 }
 
 tasks.withType(RunTestExecutable) {
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp
index c98c5e4..d215df6 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/dev/native/cpp/main.cpp
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
+#include <cstdio>
 #include <iostream>
 #include <thread>
 
+#include <fmt/format.h>
 #include <hal/DriverStation.h>
 #include <hal/HALBase.h>
 #include <hal/Main.h>
-#include <wpi/Format.h>
-#include <wpi/raw_ostream.h>
 
 extern "C" int HALSIM_InitExtension(void);
 
@@ -28,9 +25,9 @@
   while (cycleCount < 1000) {
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
     cycleCount++;
-    std::cout << "Count: " << cycleCount << std::endl;
+    fmt::print("Count: {}\n", cycleCount);
   }
 
-  std::cout << "DONE" << std::endl;
+  std::puts("DONE");
   HAL_ExitMain();
 }
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp
index 73ebaca..3a2e3b2 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimHttpConnection.cpp
@@ -1,21 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimHttpConnection.h"
 
 #include <uv.h>
 
-#include <wpi/FileSystem.h>
+#include <string_view>
+
+#include <fmt/format.h>
 #include <wpi/MimeTypes.h>
-#include <wpi/Path.h>
 #include <wpi/SmallVector.h>
+#include <wpi/StringExtras.h>
 #include <wpi/UrlParser.h>
+#include <wpi/fs.h>
 #include <wpi/raw_istream.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/raw_uv_ostream.h>
 #include <wpi/uv/Request.h>
 
@@ -23,7 +22,7 @@
 
 using namespace wpilibws;
 
-bool HALSimHttpConnection::IsValidWsUpgrade(wpi::StringRef protocol) {
+bool HALSimHttpConnection::IsValidWsUpgrade(std::string_view protocol) {
   if (m_request.GetUrl() != m_server->GetServerUri()) {
     MySendError(404, "invalid websocket address");
     return false;
@@ -33,7 +32,7 @@
 }
 
 void HALSimHttpConnection::ProcessWsUpgrade() {
-  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+  m_websocket->open.connect_extended([this](auto conn, auto) {
     conn.disconnect();  // one-shot
 
     if (!m_server->RegisterWebsocket(shared_from_this())) {
@@ -44,11 +43,11 @@
 
     Log(200);
     m_isWsConnected = true;
-    wpi::errs() << "HALWebSim: websocket connected\n";
+    std::fputs("HALWebSim: websocket connected\n", stderr);
   });
 
   // parse incoming JSON, dispatch to parent
-  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+  m_websocket->text.connect([this](auto msg, bool) {
     if (!m_isWsConnected) {
       return;
     }
@@ -65,10 +64,10 @@
     m_server->OnNetValueChanged(j);
   });
 
-  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+  m_websocket->closed.connect([this](uint16_t, auto) {
     // unset the global, allow another websocket to connect
     if (m_isWsConnected) {
-      wpi::errs() << "HALWebSim: websocket disconnected\n";
+      std::fputs("HALWebSim: websocket disconnected\n", stderr);
       m_isWsConnected = false;
 
       m_server->CloseWebsocket(shared_from_this());
@@ -95,47 +94,36 @@
                                   }
 
                                   if (err) {
-                                    wpi::errs() << err.str() << "\n";
-                                    wpi::errs().flush();
+                                    fmt::print(stderr, "{}\n", err.str());
+                                    std::fflush(stderr);
                                   }
                                 });
   });
 }
 
-void HALSimHttpConnection::SendFileResponse(int code,
-                                            const wpi::Twine& codeText,
-                                            const wpi::Twine& contentType,
-                                            const wpi::Twine& filename,
-                                            const wpi::Twine& extraHeader) {
-  // open file
-  int infd;
-  if (wpi::sys::fs::openFileForRead(filename, infd)) {
-    MySendError(404, "error opening file");
-    return;
-  }
+void HALSimHttpConnection::SendFileResponse(int code, std::string_view codeText,
+                                            std::string_view contentType,
+                                            std::string_view filename,
+                                            std::string_view extraHeader) {
+  std::error_code ec;
 
-  // get status (to get file size)
-  wpi::sys::fs::file_status status;
-  if (wpi::sys::fs::status(infd, status)) {
+  // get file size
+  auto size = fs::file_size(filename, ec);
+  if (ec) {
     MySendError(404, "error getting file size");
-    wpi::sys::fs::file_t file = uv_get_osfhandle(infd);
-    wpi::sys::fs::closeFile(file);
     return;
   }
 
-  uv_os_fd_t outfd;
-  int err = uv_fileno(m_stream.GetRawHandle(), &outfd);
-  if (err < 0) {
-    m_stream.GetLoopRef().ReportError(err);
-    MySendError(404, "error getting fd");
-    wpi::sys::fs::file_t file = uv_get_osfhandle(infd);
-    wpi::sys::fs::closeFile(file);
+  // open file
+  wpi::raw_fd_istream is{filename, ec, true};
+  if (ec) {
+    MySendError(404, "error opening file");
     return;
   }
 
   wpi::SmallVector<uv::Buffer, 4> toSend;
   wpi::raw_uv_ostream os{toSend, 4096};
-  BuildHeader(os, code, codeText, contentType, status.getSize(), extraHeader);
+  BuildHeader(os, code, codeText, contentType, size, extraHeader);
   SendData(os.bufs(), false);
 
   Log(code);
@@ -144,11 +132,10 @@
   wpi::SmallVector<uv::Buffer, 4> bodyData;
   wpi::raw_uv_ostream bodyOs{bodyData, 4096};
 
-  wpi::raw_fd_istream is{infd, true};
   std::string fileBuf;
   size_t oldSize = 0;
 
-  while (fileBuf.size() < status.getSize()) {
+  while (fileBuf.size() < size) {
     oldSize = fileBuf.size();
     fileBuf.resize(oldSize + 1);
     is.read(&(*fileBuf.begin()) + oldSize, 1);
@@ -171,51 +158,47 @@
     return;
   }
 
-  wpi::StringRef path;
-  if (url.HasPath()) path = url.GetPath();
+  std::string_view path;
+  if (url.HasPath()) {
+    path = url.GetPath();
+  }
 
-  if (m_request.GetMethod() == wpi::HTTP_GET && path.startswith("/") &&
-      !path.contains("..")) {
+  if (m_request.GetMethod() == wpi::HTTP_GET && wpi::starts_with(path, '/') &&
+      !wpi::contains(path, "..") && !wpi::contains(path, "//")) {
     // convert to fs native representation
-    wpi::SmallVector<char, 32> nativePath;
-    wpi::sys::path::native(path, nativePath);
-
-    if (path.startswith("/user/")) {
-      std::string prefix = (wpi::sys::path::get_separator() + "user" +
-                            wpi::sys::path::get_separator())
-                               .str();
-      wpi::sys::path::replace_path_prefix(nativePath, prefix,
-                                          m_server->GetWebrootUser());
+    fs::path nativePath;
+    if (wpi::starts_with(path, "/user/")) {
+      nativePath =
+          fs::path{m_server->GetWebrootSys()} /
+          fs::path{wpi::drop_front(path, 6), fs::path::format::generic_format};
     } else {
-      wpi::sys::path::replace_path_prefix(nativePath,
-                                          wpi::sys::path::get_separator(),
-                                          m_server->GetWebrootSys());
+      nativePath =
+          fs::path{m_server->GetWebrootSys()} /
+          fs::path{wpi::drop_front(path, 1), fs::path::format::generic_format};
     }
 
-    if (wpi::sys::fs::is_directory(nativePath)) {
-      wpi::sys::path::append(nativePath, "index.html");
+    if (fs::is_directory(nativePath)) {
+      nativePath.append("index.html");
     }
 
-    if (!wpi::sys::fs::exists(nativePath) ||
-        wpi::sys::fs::is_directory(nativePath)) {
-      MySendError(404, "Resource '" + path + "' not found");
+    if (!fs::exists(nativePath) || fs::is_directory(nativePath)) {
+      MySendError(404, fmt::format("Resource '{}' not found", path));
     } else {
-      auto contentType = wpi::MimeTypeFromPath(wpi::Twine(nativePath).str());
-      SendFileResponse(200, "OK", contentType, nativePath);
+      auto contentType = wpi::MimeTypeFromPath(nativePath.string());
+      SendFileResponse(200, "OK", contentType, nativePath.string());
     }
   } else {
     MySendError(404, "Resource not found");
   }
 }
 
-void HALSimHttpConnection::MySendError(int code, const wpi::Twine& message) {
+void HALSimHttpConnection::MySendError(int code, std::string_view message) {
   Log(code);
   SendError(code, message);
 }
 
 void HALSimHttpConnection::Log(int code) {
   auto method = wpi::http_method_str(m_request.GetMethod());
-  wpi::errs() << method << " " << m_request.GetUrl() << " HTTP/"
-              << m_request.GetMajor() << "." << m_request.GetMinor() << " "
-              << code << "\n";
+  fmt::print(stderr, "{} {} HTTP/{}.{} {}\n", method, m_request.GetUrl(),
+             m_request.GetMajor(), m_request.GetMinor(), code);
 }
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp
index 2bc846b..9ecccb0 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWSServer.cpp
@@ -1,22 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimWSServer.h"
 
 #include <WSProviderContainer.h>
+#include <WSProvider_AddressableLED.h>
 #include <WSProvider_Analog.h>
+#include <WSProvider_BuiltInAccelerometer.h>
 #include <WSProvider_DIO.h>
 #include <WSProvider_DriverStation.h>
 #include <WSProvider_Encoder.h>
 #include <WSProvider_Joystick.h>
+#include <WSProvider_PCM.h>
 #include <WSProvider_PWM.h>
 #include <WSProvider_Relay.h>
 #include <WSProvider_RoboRIO.h>
 #include <WSProvider_SimDevice.h>
+#include <WSProvider_Solenoid.h>
 #include <WSProvider_dPWM.h>
 
 using namespace wpilibws;
@@ -35,16 +36,20 @@
       providers.Add(key, provider);
     };
 
+    HALSimWSProviderAddressableLED::Initialize(registerFunc);
     HALSimWSProviderAnalogIn::Initialize(registerFunc);
     HALSimWSProviderAnalogOut::Initialize(registerFunc);
+    HALSimWSProviderBuiltInAccelerometer::Initialize(registerFunc);
     HALSimWSProviderDIO::Initialize(registerFunc);
     HALSimWSProviderDigitalPWM::Initialize(registerFunc);
     HALSimWSProviderDriverStation::Initialize(registerFunc);
     HALSimWSProviderEncoder::Initialize(registerFunc);
     HALSimWSProviderJoystick::Initialize(registerFunc);
+    HALSimWSProviderPCM::Initialize(registerFunc);
     HALSimWSProviderPWM::Initialize(registerFunc);
     HALSimWSProviderRelay::Initialize(registerFunc);
     HALSimWSProviderRoboRIO::Initialize(registerFunc);
+    HALSimWSProviderSolenoid::Initialize(registerFunc);
 
     simDevices.Initialize(loop);
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp
index fe5393d..43e3b08 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/HALSimWeb.cpp
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "HALSimWeb.h"
 
-#include <wpi/FileSystem.h>
-#include <wpi/Path.h>
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
-#include <wpi/Twine.h>
 #include <wpi/UrlParser.h>
 #include <wpi/WebSocketServer.h>
+#include <wpi/fs.h>
 #include <wpi/raw_uv_ostream.h>
 #include <wpi/uv/Loop.h>
 #include <wpi/uv/Tcp.h>
@@ -29,7 +25,7 @@
       m_providers(providers),
       m_simDevicesProvider(simDevicesProvider) {
   m_loop.error.connect([](uv::Error err) {
-    wpi::errs() << "HALSim WS Server libuv ERROR: " << err.str() << '\n';
+    fmt::print(stderr, "HALSim WS Server libuv ERROR: {}\n", err.str());
   });
 
   m_server = uv::Tcp::Create(m_loop);
@@ -45,49 +41,40 @@
   }
 
   // determine where to get static content from
-  // wpi::SmallVector<char, 64> tmp;
-  wpi::SmallString<64> tmp;
-
+  fs::path path;
   const char* webroot_sys = std::getenv("HALSIMWS_SYSROOT");
-  if (webroot_sys != NULL) {
-    wpi::StringRef tstr(webroot_sys);
-    tmp.append(tstr);
+  if (webroot_sys != nullptr) {
+    path = webroot_sys;
   } else {
-    wpi::sys::fs::current_path(tmp);
-    wpi::sys::path::append(tmp, "sim");
+    path = fs::current_path() / "sim";
   }
-  wpi::sys::fs::make_absolute(tmp);
-  m_webroot_sys = wpi::Twine(tmp).str();
+  m_webroot_sys = fs::absolute(path).string();
 
-  tmp.clear();
   const char* webroot_user = std::getenv("HALSIMWS_USERROOT");
-  if (webroot_user != NULL) {
-    wpi::StringRef tstr(webroot_user);
-    tmp.append(tstr);
+  if (webroot_user != nullptr) {
+    path = webroot_sys;
   } else {
-    wpi::sys::fs::current_path(tmp);
-    wpi::sys::path::append(tmp, "sim", "user");
+    path = fs::current_path() / "sim" / "user";
   }
-  wpi::sys::fs::make_absolute(tmp);
-  m_webroot_user = wpi::Twine(tmp).str();
+  m_webroot_user = fs::absolute(path).string();
 
   const char* uri = std::getenv("HALSIMWS_URI");
-  if (uri != NULL) {
+  if (uri != nullptr) {
     m_uri = uri;
   } else {
     m_uri = "/wpilibws";
   }
 
   const char* port = std::getenv("HALSIMWS_PORT");
-  if (port != NULL) {
+  if (port != nullptr) {
     try {
       m_port = std::stoi(port);
     } catch (const std::invalid_argument& err) {
-      wpi::errs() << "Error decoding HALSIMWS_PORT (" << err.what() << ")\n";
+      fmt::print(stderr, "Error decoding HALSIMWS_PORT ({})\n", err.what());
       return false;
     }
   } else {
-    m_port = 8080;
+    m_port = 3300;
   }
 
   return true;
@@ -99,7 +86,9 @@
   // when we get a connection, accept it and start reading
   m_server->connection.connect([this, srv = m_server.get()] {
     auto tcp = srv->Accept();
-    if (!tcp) return;
+    if (!tcp) {
+      return;
+    }
 
     tcp->SetNoDelay(true);
 
@@ -109,8 +98,8 @@
 
   // start listening for incoming connections
   m_server->Listen();
-  wpi::outs() << "Listening at http://localhost:" << m_port << "\n";
-  wpi::outs() << "WebSocket URI: " << m_uri << "\n";
+  fmt::print("Listening at http://localhost:{}\n", m_port);
+  fmt::print("WebSocket URI: {}\n", m_uri);
 }
 
 bool HALSimWeb::RegisterWebsocket(
@@ -165,6 +154,6 @@
       provider->OnNetValueChanged(msg.at("data"));
     }
   } catch (wpi::json::exception& e) {
-    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+    fmt::print(stderr, "Error with incoming message: {}\n", e.what());
   }
 }
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/main.cpp
index 025a902..1ee0ae8 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/cpp/main.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
+#include <cstdio>
 #include <memory>
 
 #include <hal/Extensions.h>
-#include <wpi/raw_ostream.h>
 
 #include "HALSimWSServer.h"
 
@@ -22,7 +19,7 @@
 __declspec(dllexport)
 #endif
     int HALSIM_InitExtension(void) {
-  wpi::outs() << "Websocket WS Server Initializing.\n";
+  std::puts("Websocket WS Server Initializing.");
 
   HAL_OnShutdown(nullptr, [](void*) { gServer.reset(); });
 
@@ -31,7 +28,7 @@
     return -1;
   }
 
-  wpi::outs() << "Websocket WS Server Initialized!\n";
+  std::puts("Websocket WS Server Initialized!");
   return 0;
 }
 }  // extern "C"
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h
index 89e6c83..217e2ba 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimHttpConnection.h
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <cinttypes>
 #include <memory>
+#include <string_view>
 #include <utility>
 
 #include <HALSimBaseWebSocketConnection.h>
@@ -41,14 +39,13 @@
 
  protected:
   void ProcessRequest() override;
-  bool IsValidWsUpgrade(wpi::StringRef protocol) override;
+  bool IsValidWsUpgrade(std::string_view protocol) override;
   void ProcessWsUpgrade() override;
-  void SendFileResponse(int code, const wpi::Twine& codeText,
-                        const wpi::Twine& contentType,
-                        const wpi::Twine& filename,
-                        const wpi::Twine& extraHeader = wpi::Twine{});
+  void SendFileResponse(int code, std::string_view codeText,
+                        std::string_view contentType, std::string_view filename,
+                        std::string_view extraHeader = {});
 
-  void MySendError(int code, const wpi::Twine& message);
+  void MySendError(int code, std::string_view message);
   void Log(int code);
 
  private:
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h
index d75341b..484bfff 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWSServer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h
index f71cbdf..83a680e 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/main/native/include/HALSimWeb.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,7 +11,6 @@
 #include <WSBaseProvider.h>
 #include <WSProviderContainer.h>
 #include <WSProvider_SimDevice.h>
-#include <wpi/StringRef.h>
 #include <wpi/uv/Async.h>
 #include <wpi/uv/Loop.h>
 #include <wpi/uv/Tcp.h>
@@ -45,9 +41,9 @@
   // network -> sim
   void OnNetValueChanged(const wpi::json& msg);
 
-  wpi::StringRef GetWebrootSys() const { return m_webroot_sys; }
-  wpi::StringRef GetWebrootUser() const { return m_webroot_user; }
-  wpi::StringRef GetServerUri() const { return m_uri; }
+  const std::string& GetWebrootSys() const { return m_webroot_sys; }
+  const std::string& GetWebrootUser() const { return m_webroot_user; }
+  const std::string& GetServerUri() const { return m_uri; }
   int GetServerPort() const { return m_port; }
   wpi::uv::Loop& GetLoop() { return m_loop; }
 
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp
index 6cdac5a..f13a0c6 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/WebServerClientTest.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "WebServerClientTest.h"
 
-#include <sstream>
+#include <cstdio>
 
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/raw_uv_ostream.h>
 #include <wpi/uv/util.h>
 
@@ -23,14 +20,12 @@
 // Create Web Socket and specify event callbacks
 void WebServerClientTest::InitializeWebSocket(const std::string& host, int port,
                                               const std::string& uri) {
-  std::stringstream ss;
-  ss << host << ":" << port;
-  wpi::outs() << "Will attempt to connect to: " << ss.str() << uri << "\n";
-  m_websocket =
-      wpi::WebSocket::CreateClient(*m_tcp_client.get(), uri, ss.str());
+  fmt::print("Will attempt to connect to: {}:{}{}\n", host, port, uri);
+  m_websocket = wpi::WebSocket::CreateClient(*m_tcp_client.get(), uri,
+                                             fmt::format("{}:{}", host, port));
 
   // Hook up events
-  m_websocket->open.connect_extended([this](auto conn, wpi::StringRef) {
+  m_websocket->open.connect_extended([this](auto conn, auto) {
     conn.disconnect();
     m_buffers = std::make_unique<BufferPool>();
 
@@ -41,17 +36,17 @@
                                 });
 
     m_ws_connected = true;
-    wpi::errs() << "WebServerClientTest: WebSocket Connected\n";
+    std::fputs("WebServerClientTest: WebSocket Connected\n", stderr);
   });
 
-  m_websocket->text.connect([this](wpi::StringRef msg, bool) {
+  m_websocket->text.connect([this](auto msg, bool) {
     wpi::json j;
     try {
       j = wpi::json::parse(msg);
     } catch (const wpi::json::parse_error& e) {
       std::string err("JSON parse failed: ");
       err += e.what();
-      wpi::errs() << err << "\n";
+      fmt::print(stderr, "{}\n", err);
       m_websocket->Fail(1003, err);
       return;
     }
@@ -59,9 +54,9 @@
     m_json = j;
   });
 
-  m_websocket->closed.connect([this](uint16_t, wpi::StringRef) {
+  m_websocket->closed.connect([this](uint16_t, auto) {
     if (m_ws_connected) {
-      wpi::errs() << "WebServerClientTest: Websocket Disconnected\n";
+      std::fputs("WebServerClientTest: Websocket Disconnected\n", stderr);
       m_ws_connected = false;
     }
   });
@@ -70,11 +65,11 @@
 // Create tcp client, specify callbacks, and create timers for loop
 bool WebServerClientTest::Initialize() {
   m_loop.error.connect(
-      [](uv::Error err) { wpi::errs() << "uv Error: " << err.str() << "\n"; });
+      [](uv::Error err) { fmt::print(stderr, "uv Error: {}\n", err.str()); });
 
   m_tcp_client = uv::Tcp::Create(m_loop);
   if (!m_tcp_client) {
-    wpi::errs() << "ERROR: Could not create TCP Client\n";
+    std::fputs("ERROR: Could not create TCP Client\n", stderr);
     return false;
   }
 
@@ -93,7 +88,7 @@
       });
 
   m_tcp_client->closed.connect(
-      []() { wpi::errs() << "TCP connection closed\n"; });
+      []() { std::fputs("TCP connection closed\n", stderr); });
 
   // Set up the connection timer
   m_connect_timer = uv::Timer::Create(m_loop);
@@ -104,33 +99,32 @@
   m_connect_timer->timeout.connect([this] { AttemptConnect(); });
   m_connect_timer->Start(uv::Timer::Time(0));
 
-  wpi::outs() << "WebServerClientTest Initialized\n";
+  std::puts("WebServerClientTest Initialized");
 
   return true;
 }
 
 void WebServerClientTest::AttemptConnect() {
   m_connect_attempts++;
-  wpi::outs() << "Test Client Connection Attempt " << m_connect_attempts
-              << "\n";
+  fmt::print("Test Client Connection Attempt {}\n", m_connect_attempts);
 
   if (m_connect_attempts >= 5) {
-    wpi::errs() << "Test Client Timeout. Unable to connect\n";
+    std::fputs("Test Client Timeout. Unable to connect\n", stderr);
     m_loop.Stop();
     return;
   }
 
   struct sockaddr_in dest;
-  uv::NameToAddr("localhost", 8080, &dest);
+  uv::NameToAddr("localhost", 3300, &dest);
   m_tcp_client->Connect(dest, [this]() {
     m_tcp_connected = true;
-    InitializeWebSocket("localhost", 8080, "/wpilibws");
+    InitializeWebSocket("localhost", 3300, "/wpilibws");
   });
 }
 
 void WebServerClientTest::SendMessage(const wpi::json& msg) {
   if (msg.empty()) {
-    wpi::errs() << "Message to send is empty\n";
+    std::fputs("Message to send is empty\n", stderr);
     return;
   }
 
@@ -150,13 +144,15 @@
         m_buffers->Release(bufs);
       }
       if (err) {
-        wpi::errs() << err.str() << "\n";
-        wpi::errs().flush();
+        fmt::print(stderr, "{}\n", err.str());
+        std::fflush(stderr);
       }
     });
   });
 }
 
-const wpi::json& WebServerClientTest::GetLastMessage() { return m_json; }
+const wpi::json& WebServerClientTest::GetLastMessage() {
+  return m_json;
+}
 
 }  // namespace wpilibws
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/main.cpp b/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/main.cpp
index 4172f19..ef31ba1 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/cpp/main.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <thread>
 
+#include <fmt/format.h>
 #include <hal/DriverStation.h>
 #include <hal/HALBase.h>
 #include <hal/Main.h>
 #include <hal/simulation/DIOData.h>
-#include <wpi/raw_ostream.h>
 #include <wpi/uv/Loop.h>
 
 #include "HALSimWSServer.h"
@@ -44,7 +41,7 @@
   HALSimWSServer m_server;
 };
 
-TEST_F(WebServerIntegrationTest, DigitalOutput) {
+TEST_F(WebServerIntegrationTest, DISABLED_DigitalOutput) {
   // Create expected results
   const bool EXPECTED_VALUE = false;
   const int PIN = 0;
@@ -58,8 +55,8 @@
         return;
       }
       if (IsConnectedClientWS()) {
-        wpi::outs() << "***** Setting DIO value for pin " << PIN << " to "
-                    << (EXPECTED_VALUE ? "true" : "false") << "\n";
+        fmt::print("***** Setting DIO value for pin {} to {}\n", PIN,
+                   (EXPECTED_VALUE ? "true" : "false"));
         HALSIM_SetDIOValue(PIN, EXPECTED_VALUE);
         done = true;
       }
@@ -86,7 +83,7 @@
       test_value = it.value();
     }
   } catch (wpi::json::exception& e) {
-    wpi::errs() << "Error with incoming message: " << e.what() << "\n";
+    fmt::print(stderr, "Error with incoming message: {}\n", e.what());
   }
 
   // Compare results
@@ -95,7 +92,7 @@
   EXPECT_EQ(EXPECTED_VALUE, test_value);
 }
 
-TEST_F(WebServerIntegrationTest, DigitalInput) {
+TEST_F(WebServerIntegrationTest, DISABLED_DigitalInput) {
   // Create expected results
   const bool EXPECTED_VALUE = false;
   const int PIN = 0;
@@ -112,7 +109,7 @@
         wpi::json msg = {{"type", "DIO"},
                          {"device", std::to_string(PIN)},
                          {"data", {{"<>value", EXPECTED_VALUE}}}};
-        wpi::outs() << "***** Input JSON: " << msg.dump() << "\n";
+        fmt::print("***** Input JSON: {}\n", msg.dump());
         m_webserverClient->SendMessage(msg);
         done = true;
       }
@@ -147,7 +144,7 @@
             {"type", "DriverStation"},
             {"device", ""},
             {"data", {{">enabled", EXPECTED_VALUE}, {">new_data", true}}}};
-        wpi::outs() << "***** Input JSON: " << msg.dump() << "\n";
+        fmt::print("***** Input JSON: {}\n", msg.dump());
         m_webserverClient->SendMessage(msg);
         done = true;
       }
diff --git a/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h b/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h
index 909c4ba..08db565 100644
--- a/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h
+++ b/third_party/allwpilib/simulation/halsim_ws_server/src/test/native/include/WebServerClientTest.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/styleguide/checkstyle-suppressions.xml b/third_party/allwpilib/styleguide/checkstyle-suppressions.xml
new file mode 100644
index 0000000..54d4c2b
--- /dev/null
+++ b/third_party/allwpilib/styleguide/checkstyle-suppressions.xml
@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<!DOCTYPE
+suppressions PUBLIC "-//Puppy Crawl//DTD Suppressions 1.1//EN"
+"http://www.puppycrawl.com/dtds/suppressions_1_1.dtd">
+<suppressions>
+  <suppress files=".*test.*" checks="MissingJavadocMethod" />
+  <suppress files=".*wpilibjIntegrationTests.*"
+    checks="MissingJavadocMethod" />
+  <suppress files=".*JNI.*"
+    checks="(LineLength|EmptyLineSeparator|ParameterName|MissingJavadocMethod)" />
+</suppressions>
diff --git a/third_party/allwpilib/styleguide/checkstyle.xml b/third_party/allwpilib/styleguide/checkstyle.xml
index eeeaf08..3796fe3 100644
--- a/third_party/allwpilib/styleguide/checkstyle.xml
+++ b/third_party/allwpilib/styleguide/checkstyle.xml
@@ -2,98 +2,78 @@
 <!DOCTYPE
 module PUBLIC "-//Puppy Crawl//DTD Check Configuration 1.3//EN"
 "http://www.puppycrawl.com/dtds/configuration_1_3.dtd">
-<!--
-    Checkstyle configuration that checks the Google coding conventions from Google Java Style
-    that can be found at https://google.github.io/styleguide/javaguide.html.
-
-    Checkstyle is very configurable. Be sure to read the documentation at
-    http://checkstyle.sf.net (or in your downloaded distribution).
-
-    To completely disable a check, just comment it out or delete it from the file.
-
-    Authors: Max Vetrenko, Ruslan Diachenko, Roman Ivanov.
- -->
+<!-- Checkstyle configuration that checks the Google coding conventions from
+  Google Java Style that can be found at https://google.github.io/styleguide/javaguide.html.
+  Checkstyle is very configurable. Be sure to read the documentation at http://checkstyle.sf.net
+  (or in your downloaded distribution). To completely disable a check, just
+  comment it out or delete it from the file. Authors: Max Vetrenko, Ruslan
+  Diachenko, Roman Ivanov. -->
 <module name="Checker">
-  <property name="charset"
-            value="UTF-8" />
-  <property name="severity"
-            value="error" />
+  <property name="charset" value="UTF-8" />
+  <property name="severity" value="error" />
   <module name="SuppressionFilter">
     <property name="file"
-              value="${config_loc}/suppressions.xml" />
+      value="${config_loc}/checkstyle-suppressions.xml" />
   </module>
-  <property name="fileExtensions"
-            value="java, properties, xml" />
-  <!-- Checks for whitespace                               -->
+  <property name="fileExtensions" value="java, properties, xml" />
+  <!-- Checks for whitespace -->
   <!-- See http://checkstyle.sf.net/config_whitespace.html -->
   <module name="FileTabCharacter">
-    <property name="eachLine"
-              value="true" />
+    <property name="eachLine" value="true" />
   </module>
-  <module name="NewlineAtEndOfFile">
-    <property name="lineSeparator"
-              value="lf" />
+  <module name="LineLength">
+    <property name="fileExtensions" value="java" />
+    <property name="max" value="100" />
+    <property name="ignorePattern"
+      value="^package.*|^import.*|a href|href|http://|https://|ftp://" />
   </module>
+  <module name="NewlineAtEndOfFile" />
   <module name="SuppressWarningsFilter" />
   <module name="TreeWalker">
     <module name="SuppressionCommentFilter">
-        <property name="offCommentFormat" value="CHECKSTYLE.OFF\: ([\w\|]+)"/>
-        <property name="onCommentFormat" value="CHECKSTYLE.ON\: ([\w\|]+)"/>
-        <property name="checkFormat" value="$1"/>
+      <property name="offCommentFormat"
+        value="CHECKSTYLE.OFF\: ([\w\|]+)" />
+      <property name="onCommentFormat"
+        value="CHECKSTYLE.ON\: ([\w\|]+)" />
+      <property name="checkFormat" value="$1" />
     </module>
     <module name="SuppressWarningsHolder" />
     <module name="OuterTypeFilename" />
     <module name="IllegalTokenText">
-      <property name="tokens"
-                value="STRING_LITERAL, CHAR_LITERAL" />
+      <property name="tokens" value="STRING_LITERAL, CHAR_LITERAL" />
       <property name="format"
-                value="\\u00(08|09|0(a|A)|0(c|C)|0(d|D)|22|27|5(C|c))|\\(0(10|11|12|14|15|42|47)|134)" />
+        value="\\u00(08|09|0(a|A)|0(c|C)|0(d|D)|22|27|5(C|c))|\\(0(10|11|12|14|15|42|47)|134)" />
       <property name="message"
-                value="Avoid using corresponding octal or Unicode escape." />
+        value="Avoid using corresponding octal or Unicode escape." />
     </module>
     <module name="IllegalTokenText">
-      <property name="tokens"
-                value="NUM_INT,NUM_LONG"/>
-      <property name="format"
-                value="^0[^lx]"/>
-      <property name="ignoreCase"
-                value="true"/>
+      <property name="tokens" value="NUM_INT,NUM_LONG" />
+      <property name="format" value="^0[^lx]" />
+      <property name="ignoreCase" value="true" />
       <property name="message"
-                value="Forbid leading zeros in an integer literal, other than zero and a hex literal." />
+        value="Forbid leading zeros in an integer literal, other than zero and a hex literal." />
     </module>
     <module name="IllegalTokenText">
-      <property name="tokens"
-                value="NUM_DOUBLE,NUM_FLOAT"/>
-      <property name="format"
-                value="(^\.|\.$)"/>
-      <property name="ignoreCase"
-                value="true"/>
+      <property name="tokens" value="NUM_DOUBLE,NUM_FLOAT" />
+      <property name="format" value="(^\.|\.$)" />
+      <property name="ignoreCase" value="true" />
       <property name="message"
-                value="Must use leading and trailing zero in floating point numbers." />
+        value="Must use leading and trailing zero in floating point numbers." />
     </module>
     <module name="AvoidEscapedUnicodeCharacters">
       <property name="allowEscapesForControlCharacters"
-                value="true" />
-      <property name="allowByTailComment"
-                value="true" />
-      <property name="allowNonPrintableEscapes"
-                value="true" />
+        value="true" />
+      <property name="allowByTailComment" value="true" />
+      <property name="allowNonPrintableEscapes" value="true" />
     </module>
-    <module name="LineLength">
-      <property name="max"
-                value="100" />
-      <property name="ignorePattern"
-                value="^package.*|^import.*|a href|href|http://|https://|ftp://" />
-    </module>
-    <module name="ImportOrder">
-      <property name="option"
-                value="bottom"/>
-      <property name="groups"
-                value="/^java\./,javax,com,org,/^edu\./,*,/^edu\.wpi\./,/^edu\.wpi\.first\.wpilibj\.examples\./"/>
-      <property name="separated"
-                value="true"/>
-      <property name="sortStaticImportsAlphabetically"
-                value="true"/>
+    <module name="CustomImportOrder">
+      <property name="sortImportsInGroupAlphabetically"
+        value="true" />
+      <property name="separateLineBetweenGroups" value="true" />
+      <property name="customImportOrderRules"
+        value="STATIC###THIRD_PARTY_PACKAGE" />
+      <property name="tokens"
+        value="IMPORT, STATIC_IMPORT, PACKAGE_DEF" />
     </module>
     <module name="AvoidStarImport" />
     <module name="RedundantImport" />
@@ -101,33 +81,59 @@
     <module name="OneTopLevelClass" />
     <module name="NoLineWrap" />
     <module name="EmptyBlock">
-      <property name="option"
-                value="TEXT" />
+      <property name="option" value="TEXT" />
       <property name="tokens"
-                value="LITERAL_TRY, LITERAL_FINALLY, LITERAL_IF, LITERAL_ELSE, LITERAL_SWITCH" />
+        value="LITERAL_TRY, LITERAL_FINALLY, LITERAL_IF, LITERAL_ELSE, LITERAL_SWITCH" />
     </module>
     <module name="NeedBraces" />
-    <module name="LeftCurly" />
-    <module name="RightCurly" />
-    <module name="RightCurly">
-      <property name="option"
-                value="alone" />
+    <module name="LeftCurly">
       <property name="tokens"
-                value="CLASS_DEF, METHOD_DEF, CTOR_DEF, LITERAL_FOR, LITERAL_WHILE, LITERAL_DO, STATIC_INIT, INSTANCE_INIT" />
+        value="ANNOTATION_DEF, CLASS_DEF, CTOR_DEF, ENUM_CONSTANT_DEF, ENUM_DEF,
+                    INTERFACE_DEF, LAMBDA, LITERAL_CATCH, LITERAL_DEFAULT,
+                    LITERAL_DO, LITERAL_ELSE, LITERAL_FINALLY, LITERAL_FOR, LITERAL_IF,
+                    LITERAL_SWITCH, LITERAL_SYNCHRONIZED, LITERAL_TRY, LITERAL_WHILE, METHOD_DEF,
+                    OBJBLOCK, STATIC_INIT, RECORD_DEF, COMPACT_CTOR_DEF" />
+    </module>
+    <module name="RightCurly">
+      <property name="id" value="RightCurlySame" />
+      <property name="tokens"
+        value="LITERAL_TRY, LITERAL_CATCH, LITERAL_FINALLY, LITERAL_IF, LITERAL_ELSE,
+                    LITERAL_DO" />
+    </module>
+    <module name="RightCurly">
+      <property name="id" value="RightCurlyAlone" />
+      <property name="option" value="alone" />
+      <property name="tokens"
+        value="CLASS_DEF, METHOD_DEF, CTOR_DEF, LITERAL_FOR, LITERAL_WHILE, STATIC_INIT,
+                    INSTANCE_INIT, ANNOTATION_DEF, ENUM_DEF, INTERFACE_DEF, RECORD_DEF,
+                    COMPACT_CTOR_DEF" />
+    </module>
+    <module name="SuppressionXpathSingleFilter">
+      <!-- suppresion is required till https://github.com/checkstyle/checkstyle/issues/7541 -->
+      <property name="id" value="RightCurlyAlone" />
+      <property name="query"
+        value="//RCURLY[parent::SLIST[count(./*)=1]
+                                     or preceding-sibling::*[last()][self::LCURLY]]" />
     </module>
     <module name="WhitespaceAround">
-      <property name="allowEmptyConstructors"
-                value="true" />
-      <property name="allowEmptyMethods"
-                value="true" />
-      <property name="allowEmptyTypes"
-                value="true" />
-      <property name="allowEmptyLoops"
-                value="true" />
+      <property name="allowEmptyConstructors" value="true" />
+      <property name="allowEmptyLambdas" value="true" />
+      <property name="allowEmptyMethods" value="true" />
+      <property name="allowEmptyTypes" value="true" />
+      <property name="allowEmptyLoops" value="true" />
+      <property name="ignoreEnhancedForColon" value="false" />
+      <property name="tokens"
+        value="ASSIGN, BAND, BAND_ASSIGN, BOR, BOR_ASSIGN, BSR, BSR_ASSIGN, BXOR,
+                    BXOR_ASSIGN, COLON, DIV, DIV_ASSIGN, DO_WHILE, EQUAL, GE, GT, LAMBDA, LAND,
+                    LCURLY, LE, LITERAL_CATCH, LITERAL_DO, LITERAL_ELSE, LITERAL_FINALLY,
+                    LITERAL_FOR, LITERAL_IF, LITERAL_RETURN, LITERAL_SWITCH, LITERAL_SYNCHRONIZED,
+                    LITERAL_TRY, LITERAL_WHILE, LOR, LT, MINUS, MINUS_ASSIGN, MOD, MOD_ASSIGN,
+                    NOT_EQUAL, PLUS, PLUS_ASSIGN, QUESTION, RCURLY, SL, SLIST, SL_ASSIGN, SR,
+                    SR_ASSIGN, STAR, STAR_ASSIGN, LITERAL_ASSERT, TYPE_EXTENSION_AND" />
       <message key="ws.notFollowed"
-               value="WhitespaceAround: ''{0}'' is not followed by whitespace. Empty blocks may only be represented as '{}' when not part of a multi-block statement (4.1.3)" />
+        value="WhitespaceAround: ''{0}'' is not followed by whitespace. Empty blocks may only be represented as '{}' when not part of a multi-block statement (4.1.3)" />
       <message key="ws.notPreceded"
-               value="WhitespaceAround: ''{0}'' is not preceded with whitespace." />
+        value="WhitespaceAround: ''{0}'' is not preceded with whitespace." />
     </module>
     <module name="WhitespaceAfter" />
     <module name="OneStatementPerLine" />
@@ -143,177 +149,150 @@
     <module name="ModifierOrder" />
     <module name="RedundantModifier" />
     <module name="EmptyLineSeparator">
-      <property name="allowNoEmptyLineBetweenFields"
-                value="true" />
+      <property name="allowNoEmptyLineBetweenFields" value="true" />
     </module>
     <module name="SeparatorWrap">
-      <property name="tokens"
-                value="DOT" />
-      <property name="option"
-                value="nl" />
+      <property name="tokens" value="DOT" />
+      <property name="option" value="nl" />
     </module>
     <module name="SeparatorWrap">
-      <property name="tokens"
-                value="COMMA" />
-      <property name="option"
-                value="EOL" />
+      <property name="tokens" value="COMMA" />
+      <property name="option" value="EOL" />
     </module>
     <module name="PackageName">
-      <property name="format"
-                value="^[a-z]+(\.[a-z][a-z0-9]*)*$" />
+      <property name="format" value="^[a-z]+(\.[a-z][a-z0-9]*)*$" />
       <message key="name.invalidPattern"
-               value="Package name ''{0}'' must match pattern ''{1}''." />
+        value="Package name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="TypeName">
       <message key="name.invalidPattern"
-               value="Type name ''{0}'' must match pattern ''{1}''." />
+        value="Type name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="MemberName">
       <property name="format"
-                value="^m_[a-z][a-z0-9][a-zA-Z0-9]*$" />
+        value="^(m_[a-z]([a-zA-Z0-9]*)|value)$" />
       <message key="name.invalidPattern"
-               value="Member name ''{0}'' must match pattern ''{1}''." />
+        value="Member name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="ParameterName">
       <property name="format"
-                value="^[a-z][a-z0-9][a-zA-Z0-9]*$" />
+        value="^[a-z]([a-z0-9][a-zA-Z0-9]*)?$" />
       <message key="name.invalidPattern"
-               value="Parameter name ''{0}'' must match pattern ''{1}''." />
+        value="Parameter name ''{0}'' must match pattern ''{1}''." />
+    </module>
+    <module name="LambdaParameterName">
+      <property name="format"
+        value="^[a-z]([a-z0-9][a-zA-Z0-9]*)?$" />
+      <message key="name.invalidPattern"
+        value="Lambda parameter name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="CatchParameterName">
       <property name="format"
-                value="^[a-z][a-z0-9][a-zA-Z0-9]*$" />
+        value="^[a-z]([a-z0-9][a-zA-Z0-9]*)?$" />
       <message key="name.invalidPattern"
-               value="Catch parameter name ''{0}'' must match pattern ''{1}''." />
+        value="Catch parameter name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="LocalVariableName">
-      <property name="tokens"
-                value="VARIABLE_DEF" />
+      <property name="tokens" value="VARIABLE_DEF" />
       <property name="format"
-                value="^[a-z][a-z0-9][a-zA-Z0-9]*$" />
-      <property name="allowOneCharVarInForLoop"
-                value="true" />
+        value="^[a-z]([a-z0-9][a-zA-Z0-9]*)?$" />
+      <property name="allowOneCharVarInForLoop" value="true" />
       <message key="name.invalidPattern"
-               value="Local variable name ''{0}'' must match pattern ''{1}''." />
+        value="Local variable name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="ClassTypeParameterName">
       <property name="format"
-                value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*[T]$)" />
+        value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*[T]$)" />
       <message key="name.invalidPattern"
-               value="Class type name ''{0}'' must match pattern ''{1}''." />
+        value="Class type name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="MethodTypeParameterName">
       <property name="format"
-                value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*[T]$)" />
+        value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*[T]$)" />
       <message key="name.invalidPattern"
-               value="Method type name ''{0}'' must match pattern ''{1}''." />
+        value="Method type name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="InterfaceTypeParameterName">
       <property name="format"
-                value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*[T]$)" />
+        value="(^[A-Z][0-9]?)$|([A-Z][a-zA-Z0-9]*[T]$)" />
       <message key="name.invalidPattern"
-               value="Interface type name ''{0}'' must match pattern ''{1}''." />
+        value="Interface type name ''{0}'' must match pattern ''{1}''." />
     </module>
     <module name="NoFinalizer" />
     <module name="GenericWhitespace">
       <message key="ws.followed"
-               value="GenericWhitespace ''{0}'' is followed by whitespace." />
+        value="GenericWhitespace ''{0}'' is followed by whitespace." />
       <message key="ws.preceded"
-               value="GenericWhitespace ''{0}'' is preceded with whitespace." />
+        value="GenericWhitespace ''{0}'' is preceded with whitespace." />
       <message key="ws.illegalFollow"
-               value="GenericWhitespace ''{0}'' should followed by whitespace." />
+        value="GenericWhitespace ''{0}'' should followed by whitespace." />
       <message key="ws.notPreceded"
-               value="GenericWhitespace ''{0}'' is not preceded with whitespace." />
-    </module>
-    <module name="Indentation">
-      <property name="basicOffset"
-                value="2" />
-      <property name="braceAdjustment"
-                value="0" />
-      <property name="caseIndent"
-                value="2" />
-      <property name="throwsIndent"
-                value="4" />
-      <property name="lineWrappingIndentation"
-                value="4" />
-      <property name="arrayInitIndent"
-                value="2" />
+        value="GenericWhitespace ''{0}'' is not preceded with whitespace." />
     </module>
     <module name="AbbreviationAsWordInName">
-      <property name="ignoreFinal"
-                value="false" />
-      <property name="allowedAbbreviationLength"
-                value="3" />
+      <property name="ignoreFinal" value="false" />
+      <property name="allowedAbbreviationLength" value="4" />
     </module>
     <module name="OverloadMethodsDeclarationOrder" />
     <module name="VariableDeclarationUsageDistance" />
     <module name="MethodParamPad" />
     <module name="TypecastParenPad" />
     <module name="OperatorWrap">
-      <property name="option"
-                value="NL" />
+      <property name="option" value="NL" />
       <property name="tokens"
-                value="BAND, BOR, BSR, BXOR, DIV, EQUAL, GE, GT, LAND, LE, LITERAL_INSTANCEOF, LOR, LT, MINUS, MOD, NOT_EQUAL, PLUS, QUESTION, SL, SR, STAR " />
+        value="BAND, BOR, BSR, BXOR, DIV, EQUAL, GE, GT, LAND, LE, LITERAL_INSTANCEOF, LOR, LT, MINUS, MOD, NOT_EQUAL, PLUS, QUESTION, SL, SR, STAR " />
     </module>
     <module name="AnnotationLocation">
       <property name="tokens"
-                value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF" />
+        value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF" />
     </module>
     <module name="AnnotationLocation">
-      <property name="tokens"
-                value="VARIABLE_DEF" />
+      <property name="tokens" value="VARIABLE_DEF" />
       <property name="allowSamelineMultipleAnnotations"
-                value="true" />
+        value="true" />
     </module>
     <module name="MissingOverride" />
     <module name="NonEmptyAtclauseDescription" />
     <module name="JavadocTagContinuationIndentation" />
     <module name="SummaryJavadoc">
       <property name="forbiddenSummaryFragments"
-                value="^@return the *|^This method returns |^A [{]@code [a-zA-Z0-9]+[}]( is a )" />
+        value="^@return the *|^This method returns |^A [{]@code [a-zA-Z0-9]+[}]( is a )" />
     </module>
     <module name="JavadocParagraph" />
     <module name="AtclauseOrder">
       <property name="tagOrder"
-                value="@param, @return, @throws, @deprecated" />
+        value="@param, @return, @throws, @deprecated" />
       <property name="target"
-                value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF, VARIABLE_DEF" />
+        value="CLASS_DEF, INTERFACE_DEF, ENUM_DEF, METHOD_DEF, CTOR_DEF, VARIABLE_DEF" />
     </module>
     <module name="JavadocMethod">
-      <property name="scope"
-                value="public" />
-      <property name="allowMissingParamTags"
-                value="true" />
-      <property name="allowMissingThrowsTags"
-                value="true" />
-      <property name="allowMissingReturnTag"
-                value="true" />
-      <property name="minLineCount"
-                value="2" />
-      <property name="allowedAnnotations"
-                value="Override, Test, Before, After, BeforeClass, AfterClass, Parameters" />
-      <property name="allowUndeclaredRTE"
-                value="true" />
-      <property name="allowThrowsTagsForSubclasses"
-                value="true" />
-      <property name="suppressLoadErrors"
-                value="true" />
+      <property name="scope" value="public" />
+      <property name="allowMissingParamTags" value="true" />
+      <property name="allowMissingReturnTag" value="true" />
+      <property name="allowedAnnotations" value="Override, Test" />
+      <property name="tokens"
+        value="METHOD_DEF, CTOR_DEF, ANNOTATION_FIELD_DEF, COMPACT_CTOR_DEF" />
+    </module>
+    <module name="MissingJavadocMethod">
+      <property name="scope" value="public" />
+      <property name="minLineCount" value="2" />
+      <property name="allowedAnnotations" value="Override, Test" />
+      <property name="tokens"
+        value="METHOD_DEF, CTOR_DEF, ANNOTATION_FIELD_DEF,
+                                   COMPACT_CTOR_DEF" />
     </module>
     <module name="MethodName">
-      <property name="format"
-                value="^[a-z][a-z0-9][a-zA-Z0-9_]*$" />
+      <property name="format" value="^[a-z][a-z0-9][a-zA-Z0-9_]*$" />
       <message key="name.invalidPattern"
-               value="Method name ''{0}'' must match pattern ''{1}''." />
+        value="Method name ''{0}'' must match pattern ''{1}''." />
     </module>
-    <module name="SingleLineJavadoc">
-      <property name="ignoreInlineTags"
-                value="false" />
-    </module>
+    <!-- https://github.com/checkstyle/checkstyle/issues/4052 -->
+    <!-- <module name="SingleLineJavadoc"> <property name="ignoreInlineTags"
+      value="false" /> </module> -->
     <module name="EmptyCatchBlock">
-      <property name="exceptionVariableName"
-                value="expected" />
+      <property name="exceptionVariableName" value="expected" />
     </module>
     <module name="CommentsIndentation" />
-    <module name="PackageDeclaration"/>
+    <module name="PackageDeclaration" />
   </module>
 </module>
diff --git a/third_party/allwpilib/styleguide/pmd-ruleset.xml b/third_party/allwpilib/styleguide/pmd-ruleset.xml
index 57ff225..bdfb9a7 100644
--- a/third_party/allwpilib/styleguide/pmd-ruleset.xml
+++ b/third_party/allwpilib/styleguide/pmd-ruleset.xml
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <ruleset name="WPILibRuleset"
-         xmlns="http://pmd.sourceforge.net/ruleset/2.0.0"
-         xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
-         xsi:schemaLocation="http://pmd.sourceforge.net/ruleset/2.0.0 http://pmd.sourceforge.net/ruleset_2_0_0.xsd">
+  xmlns="http://pmd.sourceforge.net/ruleset/2.0.0"
+  xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
+  xsi:schemaLocation="http://pmd.sourceforge.net/ruleset/2.0.0 http://pmd.sourceforge.net/ruleset_2_0_0.xsd">
 
   <description>PMD Ruleset for WPILib</description>
 
@@ -14,11 +14,14 @@
     <exclude name="AccessorMethodGeneration" />
     <exclude name="AvoidPrintStackTrace" />
     <exclude name="AvoidReassigningParameters" />
+    <exclude name="AvoidUsingHardCodedIP" />
+    <exclude name="ConstantsInInterface" />
     <exclude name="JUnitAssertionsShouldIncludeMessage" />
     <exclude name="JUnitTestContainsTooManyAsserts" />
     <exclude name="JUnit4TestShouldUseAfterAnnotation" />
     <exclude name="JUnit4TestShouldUseBeforeAnnotation" />
     <exclude name="JUnit4TestShouldUseTestAnnotation" />
+    <exclude name="LooseCoupling" />
     <exclude name="ReplaceHashtableWithMap" />
     <exclude name="ReplaceVectorWithList" />
     <exclude name="SwitchStmtsShouldHaveDefault" />
@@ -28,17 +31,27 @@
   <rule ref="category/java/bestpractices.xml/UnusedPrivateMethod">
     <properties>
       <property name="violationSuppressRegex"
-                value=".*'.*Arguments\(\)'.*" />
+        value=".*'.*Arguments\(\)'.*" />
     </properties>
   </rule>
 
   <rule ref="category/java/design.xml">
+    <exclude name="AvoidThrowingRawExceptionTypes" />
+    <exclude name="CyclomaticComplexity" />
     <exclude name="DataClass" />
+    <exclude name="ExcessiveClassLength" />
+    <exclude name="ExcessiveImports" />
+    <exclude name="ExcessiveMethodLength" />
+    <exclude name="ExcessiveParameterList" />
+    <exclude name="ExcessivePublicCount" />
+    <exclude name="GodClass" />
     <exclude name="LawOfDemeter" />
     <exclude name="LoosePackageCoupling" />
+    <exclude name="NPathComplexity" />
     <exclude name="NcssConstructorCount" />
     <exclude name="NcssCount" />
     <exclude name="NcssMethodCount" />
+    <exclude name="TooManyFields" />
     <exclude name="TooManyMethods" />
   </rule>
 
@@ -65,21 +78,20 @@
 
   <rule ref="category/java/performance.xml">
     <exclude name="AvoidUsingShortType" />
+    <exclude name="AvoidInstantiatingObjectsInLoops" />
   </rule>
 
-  <rule name="UnnecessaryCastRule"
-        language="java"
-        message="Avoid unnecessary casts"
-        class="net.sourceforge.pmd.lang.java.rule.migrating.UnnecessaryCastRule"
-        externalInfoUrl="https://github.com/pmd/pmd/blob/master/pmd-java/src/main/java/net/sourceforge/pmd/lang/java/rule/migrating/UnnecessaryCastRule.java" />
+  <rule name="UnnecessaryCastRule" language="java"
+    message="Avoid unnecessary casts"
+    class="net.sourceforge.pmd.lang.java.rule.migrating.UnnecessaryCastRule"
+    externalInfoUrl="https://github.com/pmd/pmd/blob/master/pmd-java/src/main/java/net/sourceforge/pmd/lang/java/rule/migrating/UnnecessaryCastRule.java" />
 
   <!-- Custom Rules -->
   <rule name="UseRequireNonNull"
-        message="Use Objects.requireNonNull() instead of throwing a NullPointerException yourself."
-        language="java"
-        class="net.sourceforge.pmd.lang.rule.XPathRule">
+    message="Use Objects.requireNonNull() instead of throwing a NullPointerException yourself."
+    language="java" class="net.sourceforge.pmd.lang.rule.XPathRule">
     <description>Use Objects.requireNonNull() instead of throwing a
-    NullPointerException yourself.</description>
+      NullPointerException yourself.</description>
     <properties>
       <property name="xpath">
         <value>
diff --git a/third_party/allwpilib/styleguide/spotbugs-exclude.xml b/third_party/allwpilib/styleguide/spotbugs-exclude.xml
new file mode 100644
index 0000000..feec066
--- /dev/null
+++ b/third_party/allwpilib/styleguide/spotbugs-exclude.xml
@@ -0,0 +1,81 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<FindBugsFilter>
+  <Match>
+    <Bug pattern="DM_DEFAULT_ENCODING" />
+  </Match>
+  <Match>
+    <Bug pattern="DMI_HARDCODED_ABSOLUTE_FILENAME" />
+  </Match>
+  <Match>
+    <Bug pattern="DMI_RANDOM_USED_ONLY_ONCE" />
+  </Match>
+  <Match>
+    <Bug pattern="EI_EXPOSE_REP" />
+  </Match>
+  <Match>
+    <Bug pattern="EI_EXPOSE_REP2" />
+  </Match>
+  <Match>
+    <Bug pattern="MS_CANNOT_BE_FINAL" />
+    <Class name="edu.wpi.first.wpilibj.examples.pacgoat.Robot" />
+  </Match>
+  <Match>
+    <Bug pattern="MS_EXPOSE_REP" />
+  </Match>
+  <Match>
+    <Bug pattern="MS_MUTABLE_ARRAY" />
+  </Match>
+  <Match>
+    <Bug pattern="NM_CLASS_NAMING_CONVENTION" />
+    <Source name="FRCNetComm.java" />
+  </Match>
+  <Match>
+    <Bug pattern="NS_DANGEROUS_NON_SHORT_CIRCUIT" />
+    <Source name="ParallelCommandGroup.java" />
+  </Match>
+  <Match>
+    <Bug pattern="NS_DANGEROUS_NON_SHORT_CIRCUIT" />
+    <Source name="ParallelDeadlineGroup.java" />
+  </Match>
+  <Match>
+    <Bug pattern="NS_DANGEROUS_NON_SHORT_CIRCUIT" />
+    <Source name="ParallelRaceGroup.java" />
+  </Match>
+  <Match>
+    <Bug pattern="NS_DANGEROUS_NON_SHORT_CIRCUIT" />
+    <Source name="ProxyScheduleCommand.java" />
+  </Match>
+  <Match>
+    <Bug pattern="NS_DANGEROUS_NON_SHORT_CIRCUIT" />
+    <Source name="SequentialCommandGroup.java" />
+  </Match>
+  <Match>
+    <Bug pattern="RV_RETURN_VALUE_IGNORED_BAD_PRACTICE" />
+    <Source name="AntJunitLauncher.java" />
+  </Match>
+  <Match>
+    <Bug pattern="RV_RETURN_VALUE_IGNORED_BAD_PRACTICE" />
+    <Source name="CombinedRuntimeLoader.java" />
+  </Match>
+  <Match>
+    <Bug pattern="RV_RETURN_VALUE_IGNORED_BAD_PRACTICE" />
+    <Source name="RuntimeLoader.java" />
+  </Match>
+  <Match>
+    <Bug pattern="ST_WRITE_TO_STATIC_FROM_INSTANCE_METHOD" />
+  </Match>
+  <Match>
+    <Bug pattern="UC_USELESS_VOID_METHOD" />
+    <Class name="edu.wpi.first.wpilibj.templates.romitimed.Robot" />
+  </Match>
+  <Match>
+    <Bug pattern="UC_USELESS_VOID_METHOD" />
+    <Class name="edu.wpi.first.wpilibj.templates.timed.Robot" />
+  </Match>
+  <Match>
+    <Bug pattern="URF_UNREAD_PUBLIC_OR_PROTECTED_FIELD" />
+  </Match>
+  <Match>
+    <Bug pattern="VA_FORMAT_STRING_USES_NEWLINE" />
+  </Match>
+</FindBugsFilter>
diff --git a/third_party/allwpilib/styleguide/suppressions.xml b/third_party/allwpilib/styleguide/suppressions.xml
deleted file mode 100644
index 0c1d9f0..0000000
--- a/third_party/allwpilib/styleguide/suppressions.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-<?xml version="1.0"?>
-<!DOCTYPE
-suppressions PUBLIC "-//Puppy Crawl//DTD Suppressions 1.1//EN"
-"http://www.puppycrawl.com/dtds/suppressions_1_1.dtd">
-<suppressions>
-  <suppress files=".*sim.*"
-            checks="(LineLength|EmptyLineSeparator|ParameterName|ImportOrder|AbbreviationAsWordInName|JavadocMethod|NoFinalizer)" />
-  <suppress files=".*test.*"
-            checks="JavadocMethod" />
-  <suppress files=".*JNI.*"
-            checks="(LineLength|EmptyLineSeparator|ParameterName)" />
-</suppressions>
diff --git a/third_party/allwpilib/test-scripts/README.md b/third_party/allwpilib/test-scripts/README.md
index d09cb8e..e877a7d 100644
--- a/third_party/allwpilib/test-scripts/README.md
+++ b/third_party/allwpilib/test-scripts/README.md
@@ -9,7 +9,7 @@
 ## roboRIO Setup
 The roboRIO on the test bench must be updated every time NI releases a new image.
 
-1. [Use the roboRIO Imaging Tool to format the roboRIO with the lastest image.](https://frcdocs.wpi.edu/en/latest/docs/getting-started/getting-started-frc-control-system/imaging-your-roborio.html)
+1. [Use the roboRIO Imaging Tool to format the roboRIO with the latest image.](https://frcdocs.wpi.edu/en/stable/docs/zero-to-robot/step-3/imaging-your-roborio.html)
 2. Set a static ip on the roboRIO web dashboard to `10.1.90.2`
 2. Install Java on the roboRIO
     1. [Download the JRE from Maven.](https://frcmaven.wpi.edu/artifactory/list/release/edu/wpi/first/jdk/)
diff --git a/third_party/allwpilib/upstream_utils/drake-dllexport-dare.patch b/third_party/allwpilib/upstream_utils/drake-dllexport-dare.patch
new file mode 100644
index 0000000..9e5fb00
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/drake-dllexport-dare.patch
@@ -0,0 +1,28 @@
+diff --git b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+index cb0a4ee13..5d7a316f3 100644
+--- b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
++++ a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+@@ -4,6 +4,7 @@
+ #include <cstdlib>
+ 
+ #include <Eigen/Core>
++#include <wpi/SymbolExports.h>
+ 
+ namespace drake {
+ namespace math {
+@@ -20,6 +21,7 @@ namespace math {
+ /// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
+ /// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
+ ///
++WPILIB_DLLEXPORT
+ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+     const Eigen::Ref<const Eigen::MatrixXd>& A,
+     const Eigen::Ref<const Eigen::MatrixXd>& B,
+@@ -69,6 +71,7 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+ /// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
+ /// @throws std::runtime_error if R is not positive definite.
+ ///
++WPILIB_DLLEXPORT
+ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+     const Eigen::Ref<const Eigen::MatrixXd>& A,
+     const Eigen::Ref<const Eigen::MatrixXd>& B,
diff --git a/third_party/allwpilib/upstream_utils/drake-fix-doxygen.patch b/third_party/allwpilib/upstream_utils/drake-fix-doxygen.patch
new file mode 100644
index 0000000..8a48333
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/drake-fix-doxygen.patch
@@ -0,0 +1,130 @@
+diff --git a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+index 5d7a316f3..dc08be95e 100644
+--- a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
++++ b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+@@ -9,18 +9,19 @@
+ namespace drake {
+ namespace math {
+ 
+-/// Computes the unique stabilizing solution X to the discrete-time algebraic
+-/// Riccati equation:
+-///
+-/// AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+-///
+-/// @throws std::exception if Q is not positive semi-definite.
+-/// @throws std::exception if R is not positive definite.
+-///
+-/// Based on the Schur Vector approach outlined in this paper:
+-/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
+-/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
+-///
++/**
++Computes the unique stabilizing solution X to the discrete-time algebraic
++Riccati equation:
++
++AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
++
++@throws std::exception if Q is not positive semi-definite.
++@throws std::exception if R is not positive definite.
++
++Based on the Schur Vector approach outlined in this paper:
++"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
++by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
++*/
+ WPILIB_DLLEXPORT
+ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+     const Eigen::Ref<const Eigen::MatrixXd>& A,
+@@ -28,49 +29,50 @@ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+     const Eigen::Ref<const Eigen::MatrixXd>& Q,
+     const Eigen::Ref<const Eigen::MatrixXd>& R);
+ 
+-/// Computes the unique stabilizing solution X to the discrete-time algebraic
+-/// Riccati equation:
+-///
+-/// AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+-///
+-/// This is equivalent to solving the original DARE:
+-///
+-/// A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+-///
+-/// where A₂ and Q₂ are a change of variables:
+-///
+-/// A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+-///
+-/// This overload of the DARE is useful for finding the control law uₖ that
+-/// minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+-///
+-/// @verbatim
+-///     ∞ [xₖ]ᵀ[Q  N][xₖ]
+-/// J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+-///    k=0
+-/// @endverbatim
+-///
+-/// This is a more general form of the following. The linear-quadratic regulator
+-/// is the feedback control law uₖ that minimizes the following cost function
+-/// subject to xₖ₊₁ = Axₖ + Buₖ:
+-///
+-/// @verbatim
+-///     ∞
+-/// J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+-///    k=0
+-/// @endverbatim
+-///
+-/// This can be refactored as:
+-///
+-/// @verbatim
+-///     ∞ [xₖ]ᵀ[Q 0][xₖ]
+-/// J = Σ [uₖ] [0 R][uₖ] ΔT
+-///    k=0
+-/// @endverbatim
+-///
+-/// @throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
+-/// @throws std::runtime_error if R is not positive definite.
+-///
++/**
++Computes the unique stabilizing solution X to the discrete-time algebraic
++Riccati equation:
++
++AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
++
++This is equivalent to solving the original DARE:
++
++A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
++
++where A₂ and Q₂ are a change of variables:
++
++A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
++
++This overload of the DARE is useful for finding the control law uₖ that
++minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
++
++@verbatim
++    ∞ [xₖ]ᵀ[Q  N][xₖ]
++J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
++   k=0
++@endverbatim
++
++This is a more general form of the following. The linear-quadratic regulator
++is the feedback control law uₖ that minimizes the following cost function
++subject to xₖ₊₁ = Axₖ + Buₖ:
++
++@verbatim
++    ∞
++J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
++   k=0
++@endverbatim
++
++This can be refactored as:
++
++@verbatim
++    ∞ [xₖ]ᵀ[Q 0][xₖ]
++J = Σ [uₖ] [0 R][uₖ] ΔT
++   k=0
++@endverbatim
++
++@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
++@throws std::runtime_error if R is not positive definite.
++*/
+ WPILIB_DLLEXPORT
+ Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+     const Eigen::Ref<const Eigen::MatrixXd>& A,
diff --git a/third_party/allwpilib/upstream_utils/drake-replace-dense-with-core.patch b/third_party/allwpilib/upstream_utils/drake-replace-dense-with-core.patch
new file mode 100644
index 0000000..d225597
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/drake-replace-dense-with-core.patch
@@ -0,0 +1,64 @@
+diff --git b/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h a/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
+index 9af0c4525..b3f369ca0 100644
+--- b/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
++++ a/wpimath/src/main/native/include/drake/common/is_approx_equal_abstol.h
+@@ -2,7 +2,7 @@
+ 
+ #include <vector>
+ 
+-#include <Eigen/Dense>
++#include <Eigen/Core>
+ 
+ namespace drake {
+ 
+diff --git a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
+index 9585c4dae..49c2fefe7 100644
+--- a/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
++++ b/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
+@@ -1,5 +1,8 @@
+ #include "drake/math/discrete_algebraic_riccati_equation.h"
+ 
++#include <Eigen/Eigenvalues>
++#include <Eigen/QR>
++
+ #include "drake/common/drake_assert.h"
+ #include "drake/common/drake_throw.h"
+ #include "drake/common/is_approx_equal_abstol.h"
+diff --git b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+index b64bfe75e..fc5efb313 100644
+--- b/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
++++ a/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+@@ -3,7 +3,7 @@
+ #include <cmath>
+ #include <cstdlib>
+ 
+-#include <Eigen/Dense>
++#include <Eigen/Core>
+ 
+ namespace drake {
+ namespace math {
+ 
+diff --git b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
+index 74aa4b23d..2deb039a0 100644
+--- b/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
++++ a/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
+@@ -1,5 +1,6 @@
+ #include "drake/math/discrete_algebraic_riccati_equation.h"
+ 
++#include <Eigen/Eigenvalues>
+ #include <gtest/gtest.h>
+ 
+ #include "drake/common/test_utilities/eigen_matrix_compare.h"
+diff --git b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
+index e3bd85349..d6bcbb8ec 100644
+--- b/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
++++ a/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
+@@ -4,7 +4,7 @@
+ #include <cmath>
+ #include <limits>
+ 
+-#include <Eigen/Dense>
++#include <Eigen/Core>
+ #include <gtest/gtest.h>
+ 
+ // #include "drake/common/text_logging.h"
diff --git a/third_party/allwpilib/upstream_utils/eigen-maybe-uninitialized.patch b/third_party/allwpilib/upstream_utils/eigen-maybe-uninitialized.patch
new file mode 100644
index 0000000..27144de
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/eigen-maybe-uninitialized.patch
@@ -0,0 +1,15 @@
+diff --git a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
+index 74f74cc42..5fe86fa0d 100644
+--- a/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
++++ b/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
+@@ -61,6 +61,10 @@
+     // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
+     #pragma GCC diagnostic ignored "-Wattributes"
+   #endif
++  #if __GNUC__==11
++    // This warning is a false positive
++    #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
++  #endif
+ #endif
+ 
+ #if defined __NVCC__
diff --git a/third_party/allwpilib/upstream_utils/update_drake.py b/third_party/allwpilib/upstream_utils/update_drake.py
new file mode 100755
index 0000000..a6b6499
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/update_drake.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import os
+import shutil
+
+from upstream_utils import setup_upstream_repo, comment_out_invalid_includes, walk_cwd_and_copy_if, apply_patches
+
+
+def main():
+    root, repo = setup_upstream_repo("https://github.com/RobotLocomotion/drake",
+                                     "v0.35.0")
+    wpimath = os.path.join(root, "wpimath")
+
+    # Delete old install
+    for d in [
+            "src/main/native/cpp/drake", "src/main/native/include/drake",
+            "src/test/native/cpp/drake", "src/test/native/include/drake"
+    ]:
+        shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
+
+    # Copy drake source files into allwpilib
+    src_files = walk_cwd_and_copy_if(
+        lambda dp, f: f in
+        ["drake_assert_and_throw.cc", "discrete_algebraic_riccati_equation.cc"],
+        os.path.join(wpimath, "src/main/native/cpp/drake"))
+
+    # Copy drake header files into allwpilib
+    include_files = walk_cwd_and_copy_if(
+        lambda dp, f: f in [
+            "drake_assert.h", "drake_assertion_error.h",
+            "is_approx_equal_abstol.h", "never_destroyed.h", "drake_copyable.h",
+            "drake_throw.h", "discrete_algebraic_riccati_equation.h"
+        ], os.path.join(wpimath, "src/main/native/include/drake"))
+
+    # Copy drake test source files into allwpilib
+    os.chdir(os.path.join(repo, "math/test"))
+    test_src_files = walk_cwd_and_copy_if(
+        lambda dp, f: f == "discrete_algebraic_riccati_equation_test.cc",
+        os.path.join(wpimath, "src/test/native/cpp/drake"))
+    os.chdir(repo)
+
+    # Copy drake test header files into allwpilib
+    test_include_files = walk_cwd_and_copy_if(
+        lambda dp, f: f == "eigen_matrix_compare.h",
+        os.path.join(wpimath, "src/test/native/include/drake"))
+
+    for f in src_files:
+        comment_out_invalid_includes(
+            f, [os.path.join(wpimath, "src/main/native/include")])
+    for f in include_files:
+        comment_out_invalid_includes(
+            f, [os.path.join(wpimath, "src/main/native/include")])
+    for f in test_src_files:
+        comment_out_invalid_includes(f, [
+            os.path.join(wpimath, "src/main/native/include"),
+            os.path.join(wpimath, "src/test/native/include")
+        ])
+    for f in test_include_files:
+        comment_out_invalid_includes(f, [
+            os.path.join(wpimath, "src/main/native/include"),
+            os.path.join(wpimath, "src/test/native/include")
+        ])
+
+    apply_patches(root, [
+        "upstream_utils/drake-replace-dense-with-core.patch",
+        "upstream_utils/drake-dllexport-dare.patch",
+        "upstream_utils/drake-fix-doxygen.patch"
+    ])
+
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/allwpilib/upstream_utils/update_eigen.py b/third_party/allwpilib/upstream_utils/update_eigen.py
new file mode 100755
index 0000000..0df72a8
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/update_eigen.py
@@ -0,0 +1,121 @@
+#!/usr/bin/env python3
+
+import os
+import re
+import shutil
+
+from upstream_utils import setup_upstream_repo, comment_out_invalid_includes, walk_cwd_and_copy_if, apply_patches
+
+
+def eigen_inclusions(dp, f):
+    """Returns true if the given file in the "Eigen" include directory of the
+    Eigen git repo should be copied into allwpilib
+
+    Keyword arguments:
+    dp -- directory path
+    f -- filename
+    """
+    if not dp.startswith("./Eigen"):
+        return False
+
+    abspath = os.path.join(dp, f)
+
+    # Exclude NonMPL2.h since all non-MPL2 code will be excluded anyway
+    if f == "NonMPL2.h":
+        return False
+
+    # Exclude BLAS support
+    if f.endswith("_BLAS.h") or "blas" in f:
+        return False
+
+    # Exclude LAPACK support
+    if f.endswith("_LAPACKE.h") or "lapack" in f:
+        return False
+
+    # Exclude MKL support
+    if "MKL" in f:
+        return False
+
+    # Include architectures we care about
+    if "Core/arch/" in abspath:
+        return ("arch/AVX/" in abspath or "arch/Default" in abspath or
+                "arch/NEON" in abspath or "arch/SSE" in abspath)
+
+    # Include the following modules
+    modules = [
+        "Cholesky",
+        "Core",
+        "Eigenvalues",
+        "Householder",
+        "Jacobi",
+        "LU",
+        "QR",
+        "SVD",
+        "StlSupport",
+        "misc",
+        "plugins",
+    ]
+    modules_rgx = r"|".join("/" + m for m in modules)
+
+    # "Std" matches StdDeque, StdList, and StdVector headers
+    if re.search(modules_rgx, abspath) or "Std" in f:
+        return True
+    else:
+        # Exclude all other modules
+        return False
+
+
+def unsupported_inclusions(dp, f):
+    """Returns true if the given file in the "unsupported" include directory of
+    the Eigen git repo should be copied into allwpilib
+
+    Keyword arguments:
+    dp -- directory path
+    f -- filename
+    """
+    if not dp.startswith("./unsupported"):
+        return False
+
+    abspath = os.path.join(dp, f)
+
+    # Exclude build system and READMEs
+    if f == "CMakeLists.txt" or "README" in f:
+        return False
+
+    # Include the AutoDiff and MatrixFunctions modules
+    return "AutoDiff" in abspath or "MatrixFunctions" in abspath
+
+
+def main():
+    root, repo = setup_upstream_repo("https://gitlab.com/libeigen/eigen.git",
+                                     "3.4.0")
+    wpimath = os.path.join(root, "wpimath")
+
+    # Delete old install
+    for d in [
+            "src/main/native/eigeninclude/Eigen",
+            "src/main/native/eigeninclude/unsupported"
+    ]:
+        shutil.rmtree(os.path.join(wpimath, d), ignore_errors=True)
+
+    # Copy Eigen headers into allwpilib
+    eigen_files = walk_cwd_and_copy_if(
+        eigen_inclusions, os.path.join(wpimath, "src/main/native/eigeninclude"))
+
+    # Copy unsupported headers into allwpilib
+    unsupported_files = walk_cwd_and_copy_if(
+        unsupported_inclusions,
+        os.path.join(wpimath, "src/main/native/eigeninclude"))
+
+    for f in eigen_files:
+        comment_out_invalid_includes(
+            f, [os.path.join(wpimath, "src/main/native/eigeninclude")])
+    for f in unsupported_files:
+        comment_out_invalid_includes(
+            f, [os.path.join(wpimath, "src/main/native/eigeninclude")])
+
+    apply_patches(root, ["upstream_utils/eigen-maybe-uninitialized.patch"])
+
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/allwpilib/upstream_utils/update_fmt.py b/third_party/allwpilib/upstream_utils/update_fmt.py
new file mode 100755
index 0000000..faddbae
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/update_fmt.py
@@ -0,0 +1,36 @@
+#!/usr/bin/env python3
+
+import os
+import shutil
+
+from upstream_utils import setup_upstream_repo, comment_out_invalid_includes, walk_cwd_and_copy_if
+
+
+def main():
+    root, repo = setup_upstream_repo("https://github.com/fmtlib/fmt", "8.0.1")
+    wpiutil = os.path.join(root, "wpiutil")
+
+    # Delete old install
+    for d in ["src/main/native/fmtlib/src", "src/main/native/fmtlib/include"]:
+        shutil.rmtree(os.path.join(wpiutil, d), ignore_errors=True)
+
+    # Copy fmt source files into allwpilib
+    src_files = walk_cwd_and_copy_if(
+        lambda dp, f: dp.endswith("src") and f.endswith(".cc") and f !=
+        "fmt.cc", os.path.join(wpiutil, "src/main/native/fmtlib"))
+
+    # Copy fmt header files into allwpilib
+    include_files = walk_cwd_and_copy_if(
+        lambda dp, f: dp.endswith("include/fmt"),
+        os.path.join(wpiutil, "src/main/native/fmtlib"))
+
+    for f in src_files:
+        comment_out_invalid_includes(
+            f, [os.path.join(wpiutil, "src/main/native/fmtlib/include")])
+    for f in include_files:
+        comment_out_invalid_includes(
+            f, [os.path.join(wpiutil, "src/main/native/fmtlib/include")])
+
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/allwpilib/upstream_utils/upstream_utils.py b/third_party/allwpilib/upstream_utils/upstream_utils.py
new file mode 100644
index 0000000..c4ee1ae
--- /dev/null
+++ b/third_party/allwpilib/upstream_utils/upstream_utils.py
@@ -0,0 +1,201 @@
+import os
+import re
+import shutil
+import subprocess
+import tempfile
+
+
+def clone_repo(url, treeish):
+    """Clones a git repo at the given URL into a temp folder and checks out the
+    given tree-ish (either branch or tag).
+
+    The current working directory will be set to the repository folder.
+
+    Keyword argument:
+    url -- The URL of the git repo
+    treeish -- The tree-ish to check out (branch or tag)
+    """
+    os.chdir(tempfile.gettempdir())
+
+    repo = os.path.basename(url)
+    dest = os.path.join(os.getcwd(), repo).removesuffix(".git")
+
+    # Clone Git repository into current directory or update it
+    if not os.path.exists(dest):
+        subprocess.run(["git", "clone", url, dest])
+        os.chdir(dest)
+    else:
+        os.chdir(dest)
+        subprocess.run(["git", "fetch", "origin", treeish])
+
+    # Get list of heads
+    # Example output of "git ls-remote --heads":
+    #   From https://gitlab.com/libeigen/eigen.git
+    #   77c66e368c7e355f8be299659f57b0ffcaedb505  refs/heads/3.4
+    #   3e006bfd31e4389e8c5718c30409cddb65a73b04  refs/heads/master
+    ls_out = subprocess.check_output(["git", "ls-remote",
+                                      "--heads"]).decode().rstrip()
+    heads = [x.split()[1] for x in ls_out.split("\n")[1:]]
+
+    if f"refs/heads/{treeish}" in heads:
+        # Checking out the remote branch avoids needing to handle syncing a
+        # preexisting local one
+        subprocess.run(["git", "checkout", f"origin/{treeish}"])
+    else:
+        subprocess.run(["git", "checkout", treeish])
+
+
+def get_repo_root():
+    """Returns the Git repository root as an absolute path.
+
+    An empty string is returned if no repository root was found.
+    """
+    current_dir = os.path.abspath(os.getcwd())
+    while current_dir != os.path.dirname(current_dir):
+        if os.path.exists(current_dir + os.sep + ".git"):
+            return current_dir
+        current_dir = os.path.dirname(current_dir)
+    return ""
+
+
+def setup_upstream_repo(url, treeish):
+    """Clones the given upstream repository, then returns the root of the
+    destination Git repository as well as the cloned upstream Git repository.
+
+    The current working directory will be set to the cloned upstream repository
+    folder.
+
+    Keyword arguments:
+    url -- The URL of the git repo
+    treeish -- The tree-ish to check out (branch or tag)
+
+    Returns:
+    root -- root directory of destination Git repository
+    repo -- root directory of cloned upstream Git repository
+    """
+    root = get_repo_root()
+    clone_repo(url, treeish)
+    return root, os.getcwd()
+
+
+def walk_if(top, pred):
+    """Walks the current directory, then returns a list of files for which the
+    given predicate is true.
+
+    Keyword arguments:
+    top -- the top directory to walk
+    pred -- a function that takes a directory path and a filename, then returns
+            True if the file should be included in the output list
+    """
+    return [
+        os.path.join(dp, f)
+        for dp, dn, fn in os.walk(top)
+        for f in fn
+        if pred(dp, f)
+    ]
+
+
+def copy_to(files, root):
+    """Copies list of files to root by appending the relative paths of the files
+    to root.
+
+    The leading directories of root will be created if they don't yet exist.
+
+    Keyword arguments:
+    files -- list of files to copy
+    root -- destination
+
+    Returns:
+    The list of files in their destination.
+    """
+    if not os.path.exists(root):
+        os.makedirs(root)
+
+    dest_files = []
+    for f in files:
+        dest_file = os.path.join(root, f)
+
+        # Rename .cc file to .cpp
+        if dest_file.endswith(".cc"):
+            dest_file = os.path.splitext(dest_file)[0] + ".cpp"
+
+        # Make leading directory
+        dest_dir = os.path.dirname(dest_file)
+        if not os.path.exists(dest_dir):
+            os.makedirs(dest_dir)
+
+        shutil.copyfile(f, dest_file)
+        dest_files.append(dest_file)
+    return dest_files
+
+
+def walk_cwd_and_copy_if(pred, root):
+    """Walks the current directory, generates a list of files for which the
+    given predicate is true, then copies that list to root by appending the
+    relative paths of the files to root.
+
+    The leading directories of root will be created if they don't yet exist.
+
+    Keyword arguments:
+    pred -- a function that takes a directory path and a filename, then returns
+            True if the file should be included in the output list
+    root -- destination
+
+    Returns:
+    The list of files in their destination.
+    """
+    files = walk_if(".", pred)
+    files = copy_to(files, root)
+    return files
+
+
+def comment_out_invalid_includes(filename, include_roots):
+    """Comment out #include directives that include a nonexistent file
+
+    Keyword arguments:
+    filename -- file to search for includes
+    include_roots -- list of search paths for includes
+    """
+    # Read header
+    with open(filename) as f:
+        old_contents = f.read()
+
+    new_contents = ""
+    pos = 0
+    for match in re.finditer(r"#include \"([^\"]+)\"", old_contents):
+        include = match.group(1)
+
+        # Write contents from before this match
+        new_contents += old_contents[pos:match.span()[0]]
+
+        # Comment out #include if the file doesn't exist in current directory or
+        # include root
+        if not os.path.exists(os.path.join(
+                os.path.dirname(filename), include)) and not any(
+                    os.path.exists(os.path.join(include_root, include))
+                    for include_root in include_roots):
+            new_contents += "// "
+
+        new_contents += match.group()
+        pos = match.span()[1]
+
+    # Write rest of file if it wasn't all processed
+    if pos < len(old_contents):
+        new_contents += old_contents[pos:]
+
+    # Write modified file back out
+    if old_contents != new_contents:
+        with open(filename, "w") as f:
+            f.write(new_contents)
+
+
+def apply_patches(root, patches):
+    """Apply list of patches to the destination Git repository.
+
+    Keyword arguments:
+    root -- the root directory of the destination Git repository
+    patches -- list of patch files relative to the root
+    """
+    os.chdir(root)
+    for patch in patches:
+        subprocess.check_output(["git", "apply", patch])
diff --git a/third_party/allwpilib/wpigui/.styleguide b/third_party/allwpilib/wpigui/.styleguide
index 9a462a3..225b12a 100644
--- a/third_party/allwpilib/wpigui/.styleguide
+++ b/third_party/allwpilib/wpigui/.styleguide
@@ -27,4 +27,5 @@
   ^imgui
   ^implot
   ^stb
+  ^wpi/
 }
diff --git a/third_party/allwpilib/wpigui/CMakeLists.txt b/third_party/allwpilib/wpigui/CMakeLists.txt
index b0dbf43..ec46289 100644
--- a/third_party/allwpilib/wpigui/CMakeLists.txt
+++ b/third_party/allwpilib/wpigui/CMakeLists.txt
@@ -15,7 +15,7 @@
 set_property(TARGET wpigui PROPERTY FOLDER "libraries")
 
 wpilib_target_warnings(wpigui)
-target_link_libraries(wpigui PUBLIC imgui)
+target_link_libraries(wpigui PUBLIC imgui wpiutil)
 
 target_include_directories(wpigui PUBLIC
                             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
@@ -46,6 +46,6 @@
 #    set (wpigui_config_dir share/wpigui)
 #endif()
 
-#configure_file(wpigui-config.cmake.in ${CMAKE_BINARY_DIR}/wpigui-config.cmake )
-#install(FILES ${CMAKE_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir})
+#configure_file(wpigui-config.cmake.in ${WPILIB_BINARY_DIR}/wpigui-config.cmake )
+#install(FILES ${WPILIB_BINARY_DIR}/wpigui-config.cmake DESTINATION ${wpigui_config_dir})
 #install(EXPORT wpigui DESTINATION ${wpigui_config_dir})
diff --git a/third_party/allwpilib/wpigui/build.gradle b/third_party/allwpilib/wpigui/build.gradle
index 8151616..5b8c4b2 100644
--- a/third_party/allwpilib/wpigui/build.gradle
+++ b/third_party/allwpilib/wpigui/build.gradle
@@ -17,14 +17,32 @@
 
     nativeUtils.exportsConfigs {
         wpigui {
-            x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                                '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                                '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                                '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-            x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                                '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                                '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                                '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+            x86ExcludeSymbols = [
+                '_CT??_R0?AV_System_error',
+                '_CT??_R0?AVexception',
+                '_CT??_R0?AVfailure',
+                '_CT??_R0?AVruntime_error',
+                '_CT??_R0?AVsystem_error',
+                '_CTA5?AVfailure',
+                '_TI5?AVfailure',
+                '_CT??_R0?AVout_of_range',
+                '_CTA3?AVout_of_range',
+                '_TI3?AVout_of_range',
+                '_CT??_R0?AVbad_cast'
+            ]
+            x64ExcludeSymbols = [
+                '_CT??_R0?AV_System_error',
+                '_CT??_R0?AVexception',
+                '_CT??_R0?AVfailure',
+                '_CT??_R0?AVruntime_error',
+                '_CT??_R0?AVsystem_error',
+                '_CTA5?AVfailure',
+                '_TI5?AVfailure',
+                '_CT??_R0?AVout_of_range',
+                '_CTA3?AVout_of_range',
+                '_TI3?AVout_of_range',
+                '_CT??_R0?AVbad_cast'
+            ]
         }
     }
 
@@ -44,6 +62,7 @@
                 }
                 binaries.all {
                     nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name == nativeUtils.wpi.platforms.raspbian || it.targetPlatform.name == nativeUtils.wpi.platforms.aarch64bionic) {
                         it.buildable = false
                         return
@@ -101,6 +120,7 @@
                 binaries.all {
                     lib library: 'wpigui'
                     nativeUtils.useRequiredLibrary(it, 'imgui_static')
+                    lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                 }
             }
         }
diff --git a/third_party/allwpilib/wpigui/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpigui/src/dev/native/cpp/main.cpp
index abd58a1..b8464dd 100644
--- a/third_party/allwpilib/wpigui/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpigui/src/dev/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpigui.h"
 
diff --git a/third_party/allwpilib/wpigui/src/main/native/cpp/portable-file-dialogs.cpp b/third_party/allwpilib/wpigui/src/main/native/cpp/portable-file-dialogs.cpp
index 09f5d37..1330fa7 100644
--- a/third_party/allwpilib/wpigui/src/main/native/cpp/portable-file-dialogs.cpp
+++ b/third_party/allwpilib/wpigui/src/main/native/cpp/portable-file-dialogs.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 //
 //  Portable File Dialogs
diff --git a/third_party/allwpilib/wpigui/src/main/native/cpp/wpigui.cpp b/third_party/allwpilib/wpigui/src/main/native/cpp/wpigui.cpp
index 955b3f5..4b200b5 100644
--- a/third_party/allwpilib/wpigui/src/main/native/cpp/wpigui.cpp
+++ b/third_party/allwpilib/wpigui/src/main/native/cpp/wpigui.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpigui.h"
 
@@ -18,6 +15,7 @@
 #include <imgui_internal.h>
 #include <implot.h>
 #include <stb_image.h>
+#include <wpi/fs.h>
 
 #include "wpigui_internal.h"
 
@@ -56,7 +54,9 @@
 
 static void* IniReadOpen(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                          const char* name) {
-  if (std::strcmp(name, "GLOBAL") != 0) return nullptr;
+  if (std::strcmp(name, "GLOBAL") != 0) {
+    return nullptr;
+  }
   return static_cast<SavedSettings*>(gContext);
 }
 
@@ -64,7 +64,9 @@
                         void* entry, const char* lineStr) {
   auto impl = static_cast<SavedSettings*>(entry);
   const char* value = std::strchr(lineStr, '=');
-  if (!value) return;
+  if (!value) {
+    return;
+  }
   ++value;
   int num = std::atoi(value);
   if (std::strncmp(lineStr, "width=", 6) == 0) {
@@ -88,7 +90,9 @@
 
 static void IniWriteAll(ImGuiContext* ctx, ImGuiSettingsHandler* handler,
                         ImGuiTextBuffer* out_buf) {
-  if (!gContext) return;
+  if (!gContext) {
+    return;
+  }
   out_buf->appendf(
       "[MainWindow][GLOBAL]\nwidth=%d\nheight=%d\nmaximized=%d\n"
       "xpos=%d\nypos=%d\nuserScale=%d\nstyle=%d\n\n",
@@ -121,7 +125,9 @@
   glfwSetErrorCallback(ErrorCallback);
   glfwInitHint(GLFW_JOYSTICK_HAT_BUTTONS, GLFW_FALSE);
   PlatformGlfwInitHints();
-  if (!glfwInit()) return false;
+  if (!glfwInit()) {
+    return false;
+  }
 
   PlatformGlfwWindowHints();
 
@@ -140,8 +146,12 @@
   iniHandler.WriteAllFn = IniWriteAll;
   ImGui::GetCurrentContext()->SettingsHandlers.push_back(iniHandler);
 
+  io.IniFilename = gContext->iniPath.c_str();
+
   for (auto&& initialize : gContext->initializers) {
-    if (initialize) initialize();
+    if (initialize) {
+      initialize();
+    }
   }
 
   // Load INI file
@@ -171,31 +181,56 @@
       }
     }
   }
-  if (gContext->xPos != -1 && gContext->yPos != -1)
+  if (gContext->xPos != -1 && gContext->yPos != -1) {
     glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE);
+  }
 
   // Create window with graphics context
   gContext->window =
       glfwCreateWindow(gContext->width, gContext->height,
                        gContext->title.c_str(), nullptr, nullptr);
-  if (!gContext->window) return false;
+  if (!gContext->window) {
+    return false;
+  }
 
   if (!gContext->loadedWidthHeight) {
-    if (windowScale == 1.0)
+#ifndef __APPLE__
+    if (windowScale == 1.0) {
       glfwGetWindowContentScale(gContext->window, &windowScale, nullptr);
+    }
+#endif
     // force user scale if window scale is smaller
-    if (windowScale <= 0.5)
+    if (windowScale <= 0.5) {
       gContext->userScale = 0;
-    else if (windowScale <= 0.75)
+    } else if (windowScale <= 0.75) {
       gContext->userScale = 1;
+    }
     if (windowScale != 1.0) {
-      for (auto&& func : gContext->windowScalers) func(windowScale);
+      for (auto&& func : gContext->windowScalers) {
+        func(windowScale);
+      }
     }
   }
 
   // Update window settings
   if (gContext->xPos != -1 && gContext->yPos != -1) {
-    glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    // check to make sure the position isn't off-screen
+    bool found = false;
+    int monCount;
+    GLFWmonitor** monitors = glfwGetMonitors(&monCount);
+    for (int i = 0; i < monCount; ++i) {
+      int monXPos, monYPos, monWidth, monHeight;
+      glfwGetMonitorWorkarea(monitors[i], &monXPos, &monYPos, &monWidth,
+                             &monHeight);
+      if (gContext->xPos >= monXPos && gContext->xPos < (monXPos + monWidth) &&
+          gContext->yPos >= monYPos && gContext->yPos < (monYPos + monHeight)) {
+        found = true;
+        break;
+      }
+    }
+    if (found) {
+      glfwSetWindowPos(gContext->window, gContext->xPos, gContext->yPos);
+    }
     glfwShowWindow(gContext->window);
   }
 
@@ -206,6 +241,16 @@
   glfwSetWindowMaximizeCallback(gContext->window, WindowMaximizeCallback);
   glfwSetWindowPosCallback(gContext->window, WindowPosCallback);
 
+  // Set icons
+  if (!gContext->icons.empty()) {
+    glfwSetWindowIcon(gContext->window, gContext->icons.size(),
+                      gContext->icons.data());
+    for (auto&& icon : gContext->icons) {
+      stbi_image_free(icon.pixels);
+    }
+    gContext->icons.clear();
+  }
+
   // Setup Dear ImGui style
   SetStyle(static_cast<Style>(gContext->style));
 
@@ -225,7 +270,9 @@
     }
   }
 
-  if (!PlatformInitRenderer()) return false;
+  if (!PlatformInitRenderer()) {
+    return false;
+  }
 
   return true;
 }
@@ -244,6 +291,11 @@
   ImPlot::DestroyContext();
   ImGui::DestroyContext();
 
+  // Delete the save file if requested.
+  if (gContext->resetOnExit) {
+    fs::remove(fs::path{gContext->iniPath});
+  }
+
   glfwDestroyWindow(gContext->window);
   glfwTerminate();
 }
@@ -256,7 +308,9 @@
 
   // Scale based on OS window content scaling
   float windowScale = 1.0;
+#ifndef __APPLE__
   glfwGetWindowContentScale(gContext->window, &windowScale, nullptr);
+#endif
   // map to closest font size: 0 = 0.5x, 1 = 0.75x, 2 = 1.0x, 3 = 1.25x,
   // 4 = 1.5x, 5 = 1.75x, 6 = 2x
   gContext->fontScale = std::clamp(
@@ -266,12 +320,16 @@
 
   for (size_t i = 0; i < gContext->earlyExecutors.size(); ++i) {
     auto& execute = gContext->earlyExecutors[i];
-    if (execute) execute();
+    if (execute) {
+      execute();
+    }
   }
 
   for (size_t i = 0; i < gContext->lateExecutors.size(); ++i) {
     auto& execute = gContext->lateExecutors[i];
-    if (execute) execute();
+    if (execute) {
+      execute();
+    }
   }
 
   // Rendering
@@ -279,34 +337,59 @@
 }
 
 void gui::Exit() {
-  if (!gContext) return;
+  if (!gContext) {
+    return;
+  }
   gContext->exit = true;
 }
 
 void gui::AddInit(std::function<void()> initialize) {
-  if (initialize) gContext->initializers.emplace_back(std::move(initialize));
+  if (initialize) {
+    gContext->initializers.emplace_back(std::move(initialize));
+  }
 }
 
 void gui::AddWindowScaler(std::function<void(float scale)> windowScaler) {
-  if (windowScaler)
+  if (windowScaler) {
     gContext->windowScalers.emplace_back(std::move(windowScaler));
+  }
 }
 
 void gui::AddEarlyExecute(std::function<void()> execute) {
-  if (execute) gContext->earlyExecutors.emplace_back(std::move(execute));
+  if (execute) {
+    gContext->earlyExecutors.emplace_back(std::move(execute));
+  }
 }
 
 void gui::AddLateExecute(std::function<void()> execute) {
-  if (execute) gContext->lateExecutors.emplace_back(std::move(execute));
+  if (execute) {
+    gContext->lateExecutors.emplace_back(std::move(execute));
+  }
 }
 
-GLFWwindow* gui::GetSystemWindow() { return gContext->window; }
+GLFWwindow* gui::GetSystemWindow() {
+  return gContext->window;
+}
+
+bool gui::AddIcon(const unsigned char* data, int len) {
+  // Load from memory
+  GLFWimage image;
+  image.pixels =
+      stbi_load_from_memory(data, len, &image.width, &image.height, nullptr, 4);
+  if (!data) {
+    return false;
+  }
+  gContext->icons.emplace_back(std::move(image));
+  return true;
+}
 
 int gui::AddFont(
     const char* name,
     std::function<ImFont*(ImGuiIO& io, float size, const ImFontConfig* cfg)>
         makeFont) {
-  if (makeFont) gContext->makeFonts.emplace_back(name, std::move(makeFont));
+  if (makeFont) {
+    gContext->makeFonts.emplace_back(name, std::move(makeFont));
+  }
   return gContext->makeFonts.size() - 1;
 }
 
@@ -329,21 +412,49 @@
   }
 }
 
-void gui::SetClearColor(ImVec4 color) { gContext->clearColor = color; }
+void gui::SetClearColor(ImVec4 color) {
+  gContext->clearColor = color;
+}
+
+void gui::ConfigurePlatformSaveFile(const std::string& name) {
+  gContext->iniPath = name;
+#if defined(_MSC_VER)
+  const char* env = std::getenv("APPDATA");
+  if (env) {
+    gContext->iniPath = env + std::string("/" + name);
+  }
+#elif defined(__APPLE__)
+  const char* env = std::getenv("HOME");
+  if (env) {
+    gContext->iniPath = env + std::string("/Library/Preferences/" + name);
+  }
+#else
+  const char* xdg = std::getenv("XDG_CONFIG_HOME");
+  const char* env = std::getenv("HOME");
+  if (xdg) {
+    gContext->iniPath = xdg + std::string("/" + name);
+  } else if (env) {
+    gContext->iniPath = env + std::string("/.config/" + name);
+  }
+#endif
+}
 
 void gui::EmitViewMenu() {
   if (ImGui::BeginMenu("View")) {
     if (ImGui::BeginMenu("Style")) {
       bool selected;
       selected = gContext->style == kStyleClassic;
-      if (ImGui::MenuItem("Classic", nullptr, &selected, true))
+      if (ImGui::MenuItem("Classic", nullptr, &selected, true)) {
         SetStyle(kStyleClassic);
+      }
       selected = gContext->style == kStyleDark;
-      if (ImGui::MenuItem("Dark", nullptr, &selected, true))
+      if (ImGui::MenuItem("Dark", nullptr, &selected, true)) {
         SetStyle(kStyleDark);
+      }
       selected = gContext->style == kStyleLight;
-      if (ImGui::MenuItem("Light", nullptr, &selected, true))
+      if (ImGui::MenuItem("Light", nullptr, &selected, true)) {
         SetStyle(kStyleLight);
+      }
       ImGui::EndMenu();
     }
 
@@ -355,11 +466,14 @@
         bool enabled = (gContext->fontScale - gContext->userScale + i) >= 0 &&
                        (gContext->fontScale - gContext->userScale + i) <
                            Font::kScaledLevels;
-        if (ImGui::MenuItem(label, nullptr, &selected, enabled))
+        if (ImGui::MenuItem(label, nullptr, &selected, enabled)) {
           gContext->userScale = i;
+        }
       }
       ImGui::EndMenu();
     }
+
+    ImGui::MenuItem("Reset UI on Exit?", nullptr, &gContext->resetOnExit);
     ImGui::EndMenu();
   }
 }
@@ -371,12 +485,15 @@
   int height2 = 0;
   unsigned char* imgData =
       stbi_load_from_memory(data, len, &width2, &height2, nullptr, 4);
-  if (!data) return false;
+  if (!data) {
+    return false;
+  }
 
-  if (width2 == width && height2 == height)
+  if (width2 == width && height2 == height) {
     UpdateTexture(texture, kPixelRGBA, width2, height2, imgData);
-  else
+  } else {
     *texture = CreateTexture(kPixelRGBA, width2, height2, imgData);
+  }
 
   stbi_image_free(imgData);
 
@@ -389,11 +506,17 @@
   int width = 0;
   int height = 0;
   unsigned char* data = stbi_load(filename, &width, &height, nullptr, 4);
-  if (!data) return false;
+  if (!data) {
+    return false;
+  }
 
   *out_texture = CreateTexture(kPixelRGBA, width, height, data);
-  if (out_width) *out_width = width;
-  if (out_height) *out_height = height;
+  if (out_width) {
+    *out_width = width;
+  }
+  if (out_height) {
+    *out_height = height;
+  }
 
   stbi_image_free(data);
 
@@ -408,11 +531,17 @@
   int height = 0;
   unsigned char* imgData =
       stbi_load_from_memory(data, len, &width, &height, nullptr, 4);
-  if (!imgData) return false;
+  if (!imgData) {
+    return false;
+  }
 
   *out_texture = CreateTexture(kPixelRGBA, width, height, imgData);
-  if (out_width) *out_width = width;
-  if (out_height) *out_height = height;
+  if (out_width) {
+    *out_width = width;
+  }
+  if (out_height) {
+    *out_height = height;
+  }
 
   stbi_image_free(imgData);
 
@@ -422,7 +551,9 @@
 void gui::MaxFit(ImVec2* min, ImVec2* max, float width, float height) {
   float destWidth = max->x - min->x;
   float destHeight = max->y - min->y;
-  if (width == 0 || height == 0) return;
+  if (width == 0 || height == 0) {
+    return;
+  }
   if (destWidth * height > destHeight * width) {
     float outputWidth = width * destHeight / height;
     min->x += (destWidth - outputWidth) / 2;
diff --git a/third_party/allwpilib/wpigui/src/main/native/directx11/wpigui_directx11.cpp b/third_party/allwpilib/wpigui/src/main/native/directx11/wpigui_directx11.cpp
index 6ecf155..771a2ed 100644
--- a/third_party/allwpilib/wpigui/src/main/native/directx11/wpigui_directx11.cpp
+++ b/third_party/allwpilib/wpigui/src/main/native/directx11/wpigui_directx11.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <d3d11.h>
 
@@ -19,6 +16,8 @@
 #include "wpigui.h"
 #include "wpigui_internal.h"
 
+#pragma comment(lib, "d3d11.lib")
+
 using namespace wpi::gui;
 
 namespace {
@@ -102,7 +101,9 @@
 
 namespace wpi {
 
-void gui::PlatformCreateContext() { gPlatformContext = new PlatformContext; }
+void gui::PlatformCreateContext() {
+  gPlatformContext = new PlatformContext;
+}
 
 void gui::PlatformDestroyContext() {
   CleanupDeviceD3D();
@@ -174,7 +175,9 @@
 
 ImTextureID gui::CreateTexture(PixelFormat format, int width, int height,
                                const unsigned char* data) {
-  if (!gPlatformValid) return nullptr;
+  if (!gPlatformValid) {
+    return nullptr;
+  }
 
   // Create texture
   D3D11_TEXTURE2D_DESC desc;
@@ -213,7 +216,9 @@
 
 void gui::UpdateTexture(ImTextureID texture, PixelFormat, int width, int height,
                         const unsigned char* data) {
-  if (!texture) return;
+  if (!texture) {
+    return;
+  }
 
   D3D11_BOX box;
   box.front = 0;
@@ -235,8 +240,12 @@
 }
 
 void gui::DeleteTexture(ImTextureID texture) {
-  if (!gPlatformValid) return;
-  if (texture) static_cast<ID3D11ShaderResourceView*>(texture)->Release();
+  if (!gPlatformValid) {
+    return;
+  }
+  if (texture) {
+    static_cast<ID3D11ShaderResourceView*>(texture)->Release();
+  }
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpigui/src/main/native/include/wpigui.h b/third_party/allwpilib/wpigui/src/main/native/include/wpigui.h
index c1dbc15..7860f0d 100644
--- a/third_party/allwpilib/wpigui/src/main/native/include/wpigui.h
+++ b/third_party/allwpilib/wpigui/src/main/native/include/wpigui.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
+#include <string>
+#include <string_view>
 
 #include <imgui.h>
 
@@ -86,6 +85,21 @@
 GLFWwindow* GetSystemWindow();
 
 /**
+ * Adds an application icon.  Multiple icons (of different sizes) may be
+ * set.  This must be called prior to initialization to have an effect.
+ *
+ * @param data image data
+ * @param len image data length
+ * @return False if image data could not be read
+ */
+bool AddIcon(const unsigned char* data, int len);
+
+inline bool AddIcon(std::string_view data) {
+  return AddIcon(reinterpret_cast<const unsigned char*>(data.data()),
+                 data.size());
+}
+
+/**
  * Adds a font to the GUI.  The passed function is called during
  * initialization as many times as necessary to create a range of sizes.
  *
@@ -124,6 +138,15 @@
 void SetClearColor(ImVec4 color);
 
 /**
+ * Configures a save file (.ini) in a platform specific location. On Windows,
+ * the .ini is saved in %APPDATA%; on macOS the .ini is saved in
+ * ~/Library/Preferences; on Linux the .ini is stored in $XDG_CONFIG_HOME or
+ * ~/.config if the former is not defined. This must be called before
+ * gui::Initialize().
+ */
+void ConfigurePlatformSaveFile(const std::string& name);
+
+/**
  * Emits a View menu (e.g. for a main menu bar) that allows setting of
  * style and zoom.  Internally starts with ImGui::BeginMenu("View").
  */
@@ -238,14 +261,16 @@
         m_format{oth.m_format},
         m_width{oth.m_width},
         m_height{oth.m_height} {
-    oth.m_texture = 0;
+    oth.m_texture = 0;  // NOLINT
   }
 
   Texture& operator=(const Texture&) = delete;
   Texture& operator=(Texture&& oth) {
-    if (m_texture) DeleteTexture(m_texture);
+    if (m_texture) {
+      DeleteTexture(m_texture);
+    }
     m_texture = oth.m_texture;
-    oth.m_texture = 0;
+    oth.m_texture = 0;  // NOLINT
     m_format = oth.m_format;
     m_width = oth.m_width;
     m_height = oth.m_height;
@@ -253,7 +278,9 @@
   }
 
   ~Texture() {
-    if (m_texture) DeleteTexture(m_texture);
+    if (m_texture) {
+      DeleteTexture(m_texture);
+    }
   }
 
   /**
@@ -264,7 +291,7 @@
   /**
    * Implicit conversion to ImTextureID.
    */
-  operator ImTextureID() const { return m_texture; }
+  operator ImTextureID() const { return m_texture; }  // NOLINT
 
   /**
    * Gets the texture pixel format.
@@ -323,8 +350,9 @@
   static Texture CreateFromFile(const char* filename) {
     Texture texture;
     if (!CreateTextureFromFile(filename, &texture.m_texture, &texture.m_width,
-                               &texture.m_height))
+                               &texture.m_height)) {
       return {};
+    }
     return texture;
   }
 
@@ -339,8 +367,9 @@
   static Texture CreateFromImage(const unsigned char* data, int len) {
     Texture texture;
     if (!CreateTextureFromImage(data, len, &texture.m_texture, &texture.m_width,
-                                &texture.m_height))
+                                &texture.m_height)) {
       return {};
+    }
     return texture;
   }
 
diff --git a/third_party/allwpilib/wpigui/src/main/native/include/wpigui_internal.h b/third_party/allwpilib/wpigui/src/main/native/include/wpigui_internal.h
index 9a2c70f..13e504c 100644
--- a/third_party/allwpilib/wpigui/src/main/native/include/wpigui_internal.h
+++ b/third_party/allwpilib/wpigui/src/main/native/include/wpigui_internal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -56,6 +53,11 @@
 
   int fontScale = 2;  // updated by main loop
   std::vector<Font> fonts;
+
+  std::vector<GLFWimage> icons;
+
+  std::string iniPath = "imgui.ini";
+  bool resetOnExit = false;
 };
 
 extern Context* gContext;
diff --git a/third_party/allwpilib/wpigui/src/main/native/metal/wpigui_metal.mm b/third_party/allwpilib/wpigui/src/main/native/metal/wpigui_metal.mm
index cf46247..95c78c4 100644
--- a/third_party/allwpilib/wpigui/src/main/native/metal/wpigui_metal.mm
+++ b/third_party/allwpilib/wpigui/src/main/native/metal/wpigui_metal.mm
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #define GLFW_INCLUDE_NONE
 #define GLFW_EXPOSE_NATIVE_COCOA
diff --git a/third_party/allwpilib/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp b/third_party/allwpilib/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp
index bf14700..c85ad04 100644
--- a/third_party/allwpilib/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp
+++ b/third_party/allwpilib/wpigui/src/main/native/opengl3/wpigui_opengl3.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstdio>
 
@@ -114,7 +111,9 @@
 
 ImTextureID gui::CreateTexture(PixelFormat format, int width, int height,
                                const unsigned char* data) {
-  if (!gPlatformValid) return nullptr;
+  if (!gPlatformValid) {
+    return nullptr;
+  }
 
   // Create a OpenGL texture identifier
   GLuint texture;
@@ -136,16 +135,22 @@
 void gui::UpdateTexture(ImTextureID texture, PixelFormat format, int width,
                         int height, const unsigned char* data) {
   GLuint glTexture = static_cast<GLuint>(reinterpret_cast<uintptr_t>(texture));
-  if (glTexture == 0) return;
+  if (glTexture == 0) {
+    return;
+  }
   glBindTexture(GL_TEXTURE_2D, glTexture);
   glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, width, height, GLPixelFormat(format),
                   GL_UNSIGNED_BYTE, data);
 }
 
 void gui::DeleteTexture(ImTextureID texture) {
-  if (!gPlatformValid) return;
+  if (!gPlatformValid) {
+    return;
+  }
   GLuint glTexture = static_cast<GLuint>(reinterpret_cast<uintptr_t>(texture));
-  if (glTexture != 0) glDeleteTextures(1, &glTexture);
+  if (glTexture != 0) {
+    glDeleteTextures(1, &glTexture);
+  }
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpilibNewCommands/.styleguide b/third_party/allwpilib/wpilibNewCommands/.styleguide
index 11a1339..3f20ec8 100644
--- a/third_party/allwpilib/wpilibNewCommands/.styleguide
+++ b/third_party/allwpilib/wpilibNewCommands/.styleguide
@@ -15,6 +15,7 @@
 includeOtherLibs {
   ^cameraserver/
   ^cscore
+  ^fmt/
   ^frc/
   ^hal/
   ^imgui
diff --git a/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt b/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt
new file mode 100644
index 0000000..d44939f
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/CMakeLists.txt
@@ -0,0 +1,59 @@
+project(wpilibNewCommands)
+
+include(SubDirList)
+include(CompileWarnings)
+include(AddTest)
+
+if (WITH_JAVA)
+  find_package(Java REQUIRED)
+  include(UseJava)
+  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+  add_jar(wpilibNewCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibNewCommands)
+
+  get_property(WPILIBNEWCOMMANDS_JAR_FILE TARGET wpilibNewCommands_jar PROPERTY JAR_FILE)
+  install(FILES ${WPILIBNEWCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET wpilibNewCommands_jar PROPERTY FOLDER "java")
+
+  if (WITH_FLAT_INSTALL)
+      set (wpilibNewCommands_config_dir ${wpilib_dest})
+  else()
+      set (wpilibNewCommands_config_dir share/wpilibNewCommands)
+  endif()
+
+  install(FILES wpilibNewCommands-config.cmake DESTINATION ${wpilibNewCommands_config_dir})
+endif()
+
+file(GLOB_RECURSE wpilibNewCommands_native_src src/main/native/cpp/*.cpp)
+add_library(wpilibNewCommands ${wpilibNewCommands_native_src})
+set_target_properties(wpilibNewCommands PROPERTIES DEBUG_POSTFIX "d")
+set_property(TARGET wpilibNewCommands PROPERTY FOLDER "libraries")
+
+target_compile_features(wpilibNewCommands PUBLIC cxx_std_17)
+wpilib_target_warnings(wpilibNewCommands)
+target_link_libraries(wpilibNewCommands wpilibc)
+
+target_include_directories(wpilibNewCommands PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/wpilibNewCommands>)
+
+install(TARGETS wpilibNewCommands EXPORT wpilibNewCommands DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibNewCommands")
+
+if (MSVC OR FLAT_INSTALL_WPILIB)
+     set(wpilibNewCommands_config_dir ${wpilib_dest})
+ else()
+     set(wpilibNewCommands_config_dir share/wpilibNewCommands)
+ endif()
+
+ configure_file(wpilibNewCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake)
+ install(FILES ${WPILIB_BINARY_DIR}/wpilibNewCommands-config.cmake DESTINATION ${wpilibNewCommands_config_dir})
+ install(EXPORT wpilibNewCommands DESTINATION ${wpilibNewCommands_config_dir})
+
+ if (WITH_TESTS)
+     wpilib_add_test(wpilibNewCommands src/test/native/cpp)
+     target_include_directories(wpilibNewCommands_test PRIVATE src/test/native/include)
+     target_link_libraries(wpilibNewCommands_test wpilibNewCommands gmock_main)
+ endif()
diff --git a/third_party/allwpilib/wpilibNewCommands/build.gradle b/third_party/allwpilib/wpilibNewCommands/build.gradle
index 82affa0..ed81160 100644
--- a/third_party/allwpilib/wpilibNewCommands/build.gradle
+++ b/third_party/allwpilib/wpilibNewCommands/build.gradle
@@ -1,101 +1,117 @@
-ext {
-    nativeName = 'wpilibNewCommands'
-    devMain = 'edu.wpi.first.wpilibj.commands.DevMain'
-}
-
-evaluationDependsOn(':ntcore')
-evaluationDependsOn(':cscore')
-evaluationDependsOn(':hal')
-evaluationDependsOn(':wpimath')
-evaluationDependsOn(':wpilibc')
-evaluationDependsOn(':cameraserver')
-evaluationDependsOn(':wpilibj')
-
-apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
-
-dependencies {
-    implementation project(':wpiutil')
-    implementation project(':ntcore')
-    implementation project(':cscore')
-    implementation project(':hal')
-    implementation project(':wpimath')
-    implementation project(':wpilibj')
-    devImplementation project(':wpiutil')
-    devImplementation project(':ntcore')
-    devImplementation project(':cscore')
-    devImplementation project(':hal')
-    devImplementation project(':wpimath')
-    devImplementation project(':wpilibj')
-    testImplementation 'com.google.guava:guava:19.0'
-    testImplementation 'org.mockito:mockito-core:2.27.0'
-}
-
-nativeUtils.exportsConfigs {
-    wpilibNewCommands {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-    }
-}
-
-apply plugin: DisableBuildingGTest
-
-model {
-    components {}
-    binaries {
-        all {
-            if (!it.buildable || !(it instanceof NativeBinarySpec)) {
-                return
-            }
-            lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
-            lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
-            project(':hal').addHalDependency(it, 'shared')
-            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
-            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
-
-            if (it.component.name == "${nativeName}Dev") {
-              lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
-              project(':hal').addHalJniDependency(it)
-            }
-
-            if (it instanceof GoogleTestTestSuiteBinarySpec) {
-                nativeUtils.useRequiredLibrary(it, 'opencv_shared')
-                lib project: ':cscore', library: 'cscore', linkage: 'shared'
-            }
-            if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
-            }
-        }
-    }
-    tasks {
-        def c = $.components
-        def found = false
-        def systemArch = getCurrentArch()
-        c.each {
-            if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
-                it.binaries.each {
-                    if (!found) {
-                        def arch = it.targetPlatform.name
-                        if (arch == systemArch) {
-                            def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
-
-                            found = true
-                        }
-                    }
-                }
-            }
-        }
-    }
-}
-
-test {
-    testLogging {
-        outputs.upToDateWhen {false}
-        showStandardStreams = true
-    }
-}
+ext {
+    nativeName = 'wpilibNewCommands'
+    devMain = 'edu.wpi.first.wpilibj.commands.DevMain'
+}
+
+evaluationDependsOn(':ntcore')
+evaluationDependsOn(':cscore')
+evaluationDependsOn(':hal')
+evaluationDependsOn(':wpimath')
+evaluationDependsOn(':wpilibc')
+evaluationDependsOn(':cameraserver')
+evaluationDependsOn(':wpilibj')
+
+apply from: "${rootDir}/shared/javacpp/setupBuild.gradle"
+
+dependencies {
+    implementation project(':wpiutil')
+    implementation project(':ntcore')
+    implementation project(':cscore')
+    implementation project(':hal')
+    implementation project(':wpimath')
+    implementation project(':wpilibj')
+    devImplementation project(':wpiutil')
+    devImplementation project(':ntcore')
+    devImplementation project(':cscore')
+    devImplementation project(':hal')
+    devImplementation project(':wpimath')
+    devImplementation project(':wpilibj')
+    testImplementation 'com.google.guava:guava:19.0'
+    testImplementation 'org.mockito:mockito-core:2.27.0'
+}
+
+nativeUtils.exportsConfigs {
+    wpilibNewCommands {
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+    }
+}
+
+model {
+    components {}
+    binaries {
+        all {
+            if (!it.buildable || !(it instanceof NativeBinarySpec)) {
+                return
+            }
+            lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+            lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+            project(':hal').addHalDependency(it, 'shared')
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+
+            if (it.component.name == "${nativeName}Dev") {
+                lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+                project(':hal').addHalJniDependency(it)
+            }
+
+            if (it instanceof GoogleTestTestSuiteBinarySpec) {
+                nativeUtils.useRequiredLibrary(it, 'opencv_shared')
+                lib project: ':cscore', library: 'cscore', linkage: 'shared'
+            }
+            if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
+            }
+        }
+    }
+    tasks {
+        def c = $.components
+        def found = false
+        def systemArch = getCurrentArch()
+        c.each {
+            if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
+                it.binaries.each {
+                    if (!found) {
+                        def arch = it.targetPlatform.name
+                        if (arch == systemArch) {
+                            def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+                            found = true
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+test {
+    testLogging {
+        outputs.upToDateWhen {false}
+        showStandardStreams = true
+    }
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java b/third_party/allwpilib/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
index ca6ce63..484d735 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/dev/java/edu/wpi/first/wpilibj2/commands/DevMain.java
@@ -1,20 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.commands;
 
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.networktables.NetworkTablesJNI;
-import edu.wpi.first.wpiutil.RuntimeDetector;
+import edu.wpi.first.util.RuntimeDetector;
 
 public final class DevMain {
-  /**
-   * Main entry point.
-   */
+  /** Main entry point. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
     System.out.println(RuntimeDetector.getPlatformPath());
@@ -22,6 +17,5 @@
     System.out.println(HALUtil.getHALRuntimeType());
   }
 
-  private DevMain() {
-  }
+  private DevMain() {}
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpilibNewCommands/src/dev/native/cpp/main.cpp
index 5312a1d..a3e363e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/dev/native/cpp/main.cpp
@@ -1,8 +1,5 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 int main() {}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
index ba950c7..8f9901b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Command.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -11,42 +8,34 @@
 import java.util.function.BooleanSupplier;
 
 /**
- * A state machine representing a complete action to be performed by the robot.  Commands are
- * run by the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to
- * build complicated multi-step actions without the need to roll the state machine logic themselves.
+ * A state machine representing a complete action to be performed by the robot. Commands are run by
+ * the {@link CommandScheduler}, and can be composed into CommandGroups to allow users to build
+ * complicated multi-step actions without the need to roll the state machine logic themselves.
  *
  * <p>Commands are run synchronously from the main robot loop; no multithreading is used, unless
  * specified explicitly from the command implementation.
  */
 public interface Command {
+  /** The initial subroutine of a command. Called once when the command is initially scheduled. */
+  default void initialize() {}
+
+  /** The main body of a command. Called repeatedly while the command is scheduled. */
+  default void execute() {}
 
   /**
-   * The initial subroutine of a command.  Called once when the command is initially scheduled.
-   */
-  default void initialize() {
-  }
-
-  /**
-   * The main body of a command.  Called repeatedly while the command is scheduled.
-   */
-  default void execute() {
-  }
-
-  /**
-   * The action to take when the command ends.  Called when either the command finishes normally,
-   * or when it interrupted/canceled.
+   * The action to take when the command ends. Called when either the command finishes normally, or
+   * when it interrupted/canceled.
    *
-   * <p>Do not schedule commands here that share requirements with this command.
-   * Use {@link #andThen(Command...)} instead.
+   * <p>Do not schedule commands here that share requirements with this command. Use {@link
+   * #andThen(Command...)} instead.
    *
    * @param interrupted whether the command was interrupted/canceled
    */
-  default void end(boolean interrupted) {
-  }
+  default void end(boolean interrupted) {}
 
   /**
-   * Whether the command has finished.  Once a command finishes, the scheduler will call its
-   * end() method and un-schedule it.
+   * Whether the command has finished. Once a command finishes, the scheduler will call its end()
+   * method and un-schedule it.
    *
    * @return whether the command has finished.
    */
@@ -55,99 +44,114 @@
   }
 
   /**
-   * Specifies the set of subsystems used by this command.  Two commands cannot use the same
-   * subsystem at the same time.  If the command is scheduled as interruptible and another
-   * command is scheduled that shares a requirement, the command will be interrupted.  Else,
-   * the command will not be scheduled.  If no subsystems are required, return an empty set.
+   * Specifies the set of subsystems used by this command. Two commands cannot use the same
+   * subsystem at the same time. If the command is scheduled as interruptible and another command is
+   * scheduled that shares a requirement, the command will be interrupted. Else, the command will
+   * not be scheduled. If no subsystems are required, return an empty set.
    *
-   * <p>Note: it is recommended that user implementations contain the requirements as a field,
-   * and return that field here, rather than allocating a new set every time this is called.
+   * <p>Note: it is recommended that user implementations contain the requirements as a field, and
+   * return that field here, rather than allocating a new set every time this is called.
    *
    * @return the set of subsystems that are required
    */
   Set<Subsystem> getRequirements();
 
   /**
-   * Decorates this command with a timeout.  If the specified timeout is exceeded before the command
-   * finishes normally, the command will be interrupted and un-scheduled.  Note that the
-   * timeout only applies to the command returned by this method; the calling command is
-   * not itself changed.
+   * Decorates this command with a timeout. If the specified timeout is exceeded before the command
+   * finishes normally, the command will be interrupted and un-scheduled. Note that the timeout only
+   * applies to the command returned by this method; the calling command is not itself changed.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @param seconds the timeout duration
    * @return the command with the timeout added
    */
   default ParallelRaceGroup withTimeout(double seconds) {
-    return new ParallelRaceGroup(this, new WaitCommand(seconds));
+    return raceWith(new WaitCommand(seconds));
   }
 
   /**
-   * Decorates this command with an interrupt condition.  If the specified condition becomes true
-   * before the command finishes normally, the command will be interrupted and un-scheduled.
-   * Note that this only applies to the command returned by this method; the calling command
-   * is not itself changed.
+   * Decorates this command with an interrupt condition. If the specified condition becomes true
+   * before the command finishes normally, the command will be interrupted and un-scheduled. Note
+   * that this only applies to the command returned by this method; the calling command is not
+   * itself changed.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @param condition the interrupt condition
    * @return the command with the interrupt condition added
    */
   default ParallelRaceGroup withInterrupt(BooleanSupplier condition) {
-    return new ParallelRaceGroup(this, new WaitUntilCommand(condition));
+    return raceWith(new WaitUntilCommand(condition));
   }
 
   /**
    * Decorates this command with a runnable to run before this command starts.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
-   * @param toRun        the Runnable to run
+   * @param toRun the Runnable to run
    * @param requirements the required subsystems
    * @return the decorated command
    */
   default SequentialCommandGroup beforeStarting(Runnable toRun, Subsystem... requirements) {
-    return new SequentialCommandGroup(new InstantCommand(toRun, requirements), this);
+    return beforeStarting(new InstantCommand(toRun, requirements));
+  }
+
+  /**
+   * Decorates this command with another command to run before this command starts.
+   *
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
+   * cannot be used independently after being decorated, or be re-decorated with a different
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
+   *
+   * @param before the command to run before this one
+   * @return the decorated command
+   */
+  default SequentialCommandGroup beforeStarting(Command before) {
+    return new SequentialCommandGroup(before, this);
   }
 
   /**
    * Decorates this command with a runnable to run after the command finishes.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
-   * @param toRun        the Runnable to run
+   * @param toRun the Runnable to run
    * @param requirements the required subsystems
    * @return the decorated command
    */
   default SequentialCommandGroup andThen(Runnable toRun, Subsystem... requirements) {
-    return new SequentialCommandGroup(this, new InstantCommand(toRun, requirements));
+    return andThen(new InstantCommand(toRun, requirements));
   }
 
   /**
-   * Decorates this command with a set of commands to run after it in sequence.  Often more
+   * Decorates this command with a set of commands to run after it in sequence. Often more
    * convenient/less-verbose than constructing a new {@link SequentialCommandGroup} explicitly.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @param next the commands to run next
    * @return the decorated command
@@ -160,14 +164,14 @@
 
   /**
    * Decorates this command with a set of commands to run parallel to it, ending when the calling
-   * command ends and interrupting all the others.  Often more convenient/less-verbose than
+   * command ends and interrupting all the others. Often more convenient/less-verbose than
    * constructing a new {@link ParallelDeadlineGroup} explicitly.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @param parallel the commands to run in parallel
    * @return the decorated command
@@ -178,14 +182,14 @@
 
   /**
    * Decorates this command with a set of commands to run parallel to it, ending when the last
-   * command ends.  Often more convenient/less-verbose than constructing a new
-   * {@link ParallelCommandGroup} explicitly.
+   * command ends. Often more convenient/less-verbose than constructing a new {@link
+   * ParallelCommandGroup} explicitly.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @param parallel the commands to run in parallel
    * @return the decorated command
@@ -198,14 +202,14 @@
 
   /**
    * Decorates this command with a set of commands to run parallel to it, ending when the first
-   * command ends.  Often more convenient/less-verbose than constructing a new
-   * {@link ParallelRaceGroup} explicitly.
+   * command ends. Often more convenient/less-verbose than constructing a new {@link
+   * ParallelRaceGroup} explicitly.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @param parallel the commands to run in parallel
    * @return the decorated command
@@ -217,14 +221,14 @@
   }
 
   /**
-   * Decorates this command to run perpetually, ignoring its ordinary end conditions.  The decorated
+   * Decorates this command to run perpetually, ignoring its ordinary end conditions. The decorated
    * command can still be interrupted or canceled.
    *
-   * <p>Note: This decorator works by composing this command within a CommandGroup.  The command
+   * <p>Note: This decorator works by composing this command within a CommandGroup. The command
    * cannot be used independently after being decorated, or be re-decorated with a different
-   * decorator, unless it is manually cleared from the list of grouped commands with
-   * {@link CommandGroupBase#clearGroupedCommand(Command)}.  The decorated command can, however, be
-   * further decorated without issue.
+   * decorator, unless it is manually cleared from the list of grouped commands with {@link
+   * CommandGroupBase#clearGroupedCommand(Command)}. The decorated command can, however, be further
+   * decorated without issue.
    *
    * @return the decorated command
    */
@@ -233,9 +237,9 @@
   }
 
   /**
-   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}.
-   * This is useful for "forking off" from command groups when the user does not wish to extend
-   * the command's requirements to the entire command group.
+   * Decorates this command to run "by proxy" by wrapping it in a {@link ProxyScheduleCommand}. This
+   * is useful for "forking off" from command groups when the user does not wish to extend the
+   * command's requirements to the entire command group.
    *
    * @return the decorated command
    */
@@ -246,32 +250,29 @@
   /**
    * Schedules this command.
    *
-   * @param interruptible whether this command can be interrupted by another command that
-   *                      shares one of its requirements
+   * @param interruptible whether this command can be interrupted by another command that shares one
+   *     of its requirements
    */
   default void schedule(boolean interruptible) {
     CommandScheduler.getInstance().schedule(interruptible, this);
   }
 
-  /**
-   * Schedules this command, defaulting to interruptible.
-   */
+  /** Schedules this command, defaulting to interruptible. */
   default void schedule() {
     schedule(true);
   }
 
   /**
-   * Cancels this command.  Will call the command's interrupted() method.
-   * Commands will be canceled even if they are not marked as interruptible.
+   * Cancels this command. Will call the command's interrupted() method. Commands will be canceled
+   * even if they are not marked as interruptible.
    */
   default void cancel() {
     CommandScheduler.getInstance().cancel(this);
   }
 
   /**
-   * Whether or not the command is currently scheduled.  Note that this does not detect whether
-   * the command is being run by a CommandGroup, only whether it is directly being run by
-   * the scheduler.
+   * Whether or not the command is currently scheduled. Note that this does not detect whether the
+   * command is being run by a CommandGroup, only whether it is directly being run by the scheduler.
    *
    * @return Whether the command is scheduled.
    */
@@ -280,10 +281,7 @@
   }
 
   /**
-   * Whether the command requires a given subsystem.  Named "hasRequirement" rather than "requires"
-   * to avoid confusion with
-   * {@link edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
-   * - this may be able to be changed in a few years.
+   * Whether the command requires a given subsystem.
    *
    * @param requirement the subsystem to inquire about
    * @return whether the subsystem is required
@@ -293,8 +291,8 @@
   }
 
   /**
-   * Whether the given command should run when the robot is disabled.  Override to return true
-   * if the command should run when disabled.
+   * Whether the given command should run when the robot is disabled. Override to return true if the
+   * command should run when disabled.
    *
    * @return whether the command should run when the robot is disabled
    */
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
index 49180a8..bb1df60 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandBase.java
@@ -1,25 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.HashSet;
 import java.util.Set;
 
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * A {@link Sendable} base class for {@link Command}s.
- */
-@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+/** A {@link Sendable} base class for {@link Command}s. */
 public abstract class CommandBase implements Sendable, Command {
-
   protected Set<Subsystem> m_requirements = new HashSet<>();
 
   protected CommandBase() {
@@ -51,19 +43,17 @@
    *
    * @param name name
    */
-  @Override
   public void setName(String name) {
     SendableRegistry.setName(this, name);
   }
 
   /**
-  * Decorates this Command with a name.
-  * Is an inline function for #setName(String);
-  *
-  * @param name name
-  * @return the decorated Command
-  */
-  public Command withName(String name) {
+   * Decorates this Command with a name. Is an inline function for #setName(String);
+   *
+   * @param name name
+   * @return the decorated Command
+   */
+  public CommandBase withName(String name) {
     this.setName(name);
     return this;
   }
@@ -73,7 +63,6 @@
    *
    * @return Subsystem name
    */
-  @Override
   public String getSubsystem() {
     return SendableRegistry.getSubsystem(this);
   }
@@ -83,13 +72,12 @@
    *
    * @param subsystem subsystem name
    */
-  @Override
   public void setSubsystem(String subsystem) {
     SendableRegistry.setSubsystem(this, subsystem);
   }
 
   /**
-   * Initializes this sendable.  Useful for allowing implementations to easily extend SendableBase.
+   * Initializes this sendable. Useful for allowing implementations to easily extend SendableBase.
    *
    * @param builder the builder used to construct this sendable
    */
@@ -97,18 +85,21 @@
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Command");
     builder.addStringProperty(".name", this::getName, null);
-    builder.addBooleanProperty("running", this::isScheduled, value -> {
-      if (value) {
-        if (!isScheduled()) {
-          schedule();
-        }
-      } else {
-        if (isScheduled()) {
-          cancel();
-        }
-      }
-    });
-    builder.addBooleanProperty(".isParented",
-        () -> CommandGroupBase.getGroupedCommands().contains(this), null);
+    builder.addBooleanProperty(
+        "running",
+        this::isScheduled,
+        value -> {
+          if (value) {
+            if (!isScheduled()) {
+              schedule();
+            }
+          } else {
+            if (isScheduled()) {
+              cancel();
+            }
+          }
+        });
+    builder.addBooleanProperty(
+        ".isParented", () -> CommandGroupBase.getGroupedCommands().contains(this), null);
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
index 6ac0667..d3a35d4 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandGroupBase.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -13,11 +10,11 @@
 import java.util.WeakHashMap;
 
 /**
- * A base for CommandGroups.  Statically tracks commands that have been allocated to groups to
- * ensure those commands are not also used independently, which can result in inconsistent command
- * state and unpredictable execution.
+ * A base for CommandGroups. Statically tracks commands that have been allocated to groups to ensure
+ * those commands are not also used independently, which can result in inconsistent command state
+ * and unpredictable execution.
  */
-public abstract class CommandGroupBase extends CommandBase implements Command {
+public abstract class CommandGroupBase extends CommandBase {
   private static final Set<Command> m_groupedCommands =
       Collections.newSetFromMap(new WeakHashMap<>());
 
@@ -28,8 +25,8 @@
   /**
    * Clears the list of grouped commands, allowing all commands to be freely used again.
    *
-   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior.  Do not
-   * use this unless you fully understand what you are doing.
+   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use
+   * this unless you fully understand what you are doing.
    */
   public static void clearGroupedCommands() {
     m_groupedCommands.clear();
@@ -39,8 +36,8 @@
    * Removes a single command from the list of grouped commands, allowing it to be freely used
    * again.
    *
-   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior.  Do not
-   * use this unless you fully understand what you are doing.
+   * <p>WARNING: Using this haphazardly can result in unexpected/undesirable behavior. Do not use
+   * this unless you fully understand what you are doing.
    *
    * @param command the command to remove from the list of grouped commands
    */
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
index 60488d2..79be1bf 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandScheduler.java
@@ -1,12 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import edu.wpi.first.hal.FRCNetComm.tInstances;
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.RobotBase;
+import edu.wpi.first.wpilibj.RobotState;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Watchdog;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import java.util.ArrayList;
 import java.util.Collection;
 import java.util.Collections;
@@ -18,31 +27,15 @@
 import java.util.Set;
 import java.util.function.Consumer;
 
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.RobotBase;
-import edu.wpi.first.wpilibj.RobotState;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.Watchdog;
-import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
 /**
- * The scheduler responsible for running {@link Command}s.  A Command-based robot should call {@link
+ * The scheduler responsible for running {@link Command}s. A Command-based robot should call {@link
  * CommandScheduler#run()} on the singleton instance in its periodic block in order to run commands
- * synchronously from the main loop.  Subsystems should be registered with the scheduler using
- * {@link CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link
- * Subsystem#periodic()} methods to be called and for their default commands to be scheduled.
+ * synchronously from the main loop. Subsystems should be registered with the scheduler using {@link
+ * CommandScheduler#registerSubsystem(Subsystem...)} in order for their {@link Subsystem#periodic()}
+ * methods to be called and for their default commands to be scheduled.
  */
-@SuppressWarnings({"PMD.GodClass", "PMD.TooManyFields"})
-public final class CommandScheduler implements Sendable, AutoCloseable {
-  /**
-   * The Singleton Instance.
-   */
+public final class CommandScheduler implements NTSendable, AutoCloseable {
+  /** The Singleton Instance. */
   private static CommandScheduler instance;
 
   /**
@@ -57,24 +50,24 @@
     return instance;
   }
 
-  //A map from commands to their scheduling state.  Also used as a set of the currently-running
-  //commands.
+  // A map from commands to their scheduling state.  Also used as a set of the currently-running
+  // commands.
   private final Map<Command, CommandState> m_scheduledCommands = new LinkedHashMap<>();
 
-  //A map from required subsystems to their requiring commands.  Also used as a set of the
-  //currently-required subsystems.
+  // A map from required subsystems to their requiring commands.  Also used as a set of the
+  // currently-required subsystems.
   private final Map<Subsystem, Command> m_requirements = new LinkedHashMap<>();
 
-  //A map from subsystems registered with the scheduler to their default commands.  Also used
-  //as a list of currently-registered subsystems.
+  // A map from subsystems registered with the scheduler to their default commands.  Also used
+  // as a list of currently-registered subsystems.
   private final Map<Subsystem, Command> m_subsystems = new LinkedHashMap<>();
 
-  //The set of currently-registered buttons that will be polled every iteration.
+  // The set of currently-registered buttons that will be polled every iteration.
   private final Collection<Runnable> m_buttons = new LinkedHashSet<>();
 
   private boolean m_disabled;
 
-  //Lists of user-supplied actions to be executed on scheduling events for every command.
+  // Lists of user-supplied actions to be executed on scheduling events for every command.
   private final List<Consumer<Command>> m_initActions = new ArrayList<>();
   private final List<Consumer<Command>> m_executeActions = new ArrayList<>();
   private final List<Consumer<Command>> m_interruptActions = new ArrayList<>();
@@ -86,22 +79,24 @@
   private final Map<Command, Boolean> m_toSchedule = new LinkedHashMap<>();
   private final List<Command> m_toCancel = new ArrayList<>();
 
-  private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> { });
+  private final Watchdog m_watchdog = new Watchdog(TimedRobot.kDefaultPeriod, () -> {});
 
   CommandScheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand2_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
-    LiveWindow.setEnabledListener(() -> {
-      disable();
-      cancelAll();
-    });
-    LiveWindow.setDisabledListener(() -> {
-      enable();
-    });
+    LiveWindow.setEnabledListener(
+        () -> {
+          disable();
+          cancelAll();
+        });
+    LiveWindow.setDisabledListener(
+        () -> {
+          enable();
+        });
   }
 
   /**
-   * Changes the period of the loop overrun watchdog.  This should be be kept in sync with the
+   * Changes the period of the loop overrun watchdog. This should be be kept in sync with the
    * TimedRobot period.
    *
    * @param period Period in seconds.
@@ -126,9 +121,7 @@
     m_buttons.add(button);
   }
 
-  /**
-   * Removes all button bindings from the scheduler.
-   */
+  /** Removes all button bindings from the scheduler. */
   public void clearButtons() {
     m_buttons.clear();
   }
@@ -136,9 +129,9 @@
   /**
    * Initializes a given command, adds its requirements to the list, and performs the init actions.
    *
-   * @param command       The command to initialize
+   * @param command The command to initialize
    * @param interruptible Whether the command is interruptible
-   * @param requirements  The command requirements
+   * @param requirements The command requirements
    */
   private void initCommand(Command command, boolean interruptible, Set<Subsystem> requirements) {
     CommandState scheduledCommand = new CommandState(interruptible);
@@ -155,15 +148,14 @@
   }
 
   /**
-   * Schedules a command for execution.  Does nothing if the command is already scheduled. If a
+   * Schedules a command for execution. Does nothing if the command is already scheduled. If a
    * command's requirements are not available, it will only be started if all the commands currently
-   * using those requirements have been scheduled as interruptible.  If this is the case, they will
+   * using those requirements have been scheduled as interruptible. If this is the case, they will
    * be interrupted and the command will be scheduled.
    *
    * @param interruptible whether this command can be interrupted
-   * @param command       the command to schedule
+   * @param command the command to schedule
    */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
   private void schedule(boolean interruptible, Command command) {
     if (m_inRunLoop) {
       m_toSchedule.put(command, interruptible);
@@ -175,21 +167,22 @@
           "A command that is part of a command group cannot be independently scheduled");
     }
 
-    //Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
-    //run when disabled, or the command is already scheduled.
-    if (m_disabled || (RobotState.isDisabled() && !command.runsWhenDisabled())
+    // Do nothing if the scheduler is disabled, the robot is disabled and the command doesn't
+    // run when disabled, or the command is already scheduled.
+    if (m_disabled
+        || (RobotState.isDisabled() && !command.runsWhenDisabled())
         || m_scheduledCommands.containsKey(command)) {
       return;
     }
 
     Set<Subsystem> requirements = command.getRequirements();
 
-    //Schedule the command if the requirements are not currently in-use.
+    // Schedule the command if the requirements are not currently in-use.
     if (Collections.disjoint(m_requirements.keySet(), requirements)) {
       initCommand(command, interruptible, requirements);
     } else {
-      //Else check if the requirements that are in use have all have interruptible commands,
-      //and if so, interrupt those commands and schedule the new command.
+      // Else check if the requirements that are in use have all have interruptible commands,
+      // and if so, interrupt those commands and schedule the new command.
       for (Subsystem requirement : requirements) {
         if (m_requirements.containsKey(requirement)
             && !m_scheduledCommands.get(m_requirements.get(requirement)).isInterruptible()) {
@@ -206,13 +199,13 @@
   }
 
   /**
-   * Schedules multiple commands for execution.  Does nothing if the command is already scheduled.
-   * If a command's requirements are not available, it will only be started if all the commands
-   * currently using those requirements have been scheduled as interruptible.  If this is the case,
+   * Schedules multiple commands for execution. Does nothing if the command is already scheduled. If
+   * a command's requirements are not available, it will only be started if all the commands
+   * currently using those requirements have been scheduled as interruptible. If this is the case,
    * they will be interrupted and the command will be scheduled.
    *
    * @param interruptible whether the commands should be interruptible
-   * @param commands      the commands to schedule
+   * @param commands the commands to schedule
    */
   public void schedule(boolean interruptible, Command... commands) {
     for (Command command : commands) {
@@ -221,7 +214,7 @@
   }
 
   /**
-   * Schedules multiple commands for execution, with interruptible defaulted to true.  Does nothing
+   * Schedules multiple commands for execution, with interruptible defaulted to true. Does nothing
    * if the command is already scheduled.
    *
    * @param commands the commands to schedule
@@ -231,7 +224,7 @@
   }
 
   /**
-   * Runs a single iteration of the scheduler.  The execution occurs in the following order:
+   * Runs a single iteration of the scheduler. The execution occurs in the following order:
    *
    * <p>Subsystem periodic methods are called.
    *
@@ -244,14 +237,13 @@
    *
    * <p>Any subsystems not being used as requirements have their default methods started.
    */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
   public void run() {
     if (m_disabled) {
       return;
     }
     m_watchdog.reset();
 
-    //Run the periodic method of all registered subsystems.
+    // Run the periodic method of all registered subsystems.
     for (Subsystem subsystem : m_subsystems.keySet()) {
       subsystem.periodic();
       if (RobotBase.isSimulation()) {
@@ -260,16 +252,16 @@
       m_watchdog.addEpoch(subsystem.getClass().getSimpleName() + ".periodic()");
     }
 
-    //Poll buttons for new commands to add.
+    // Poll buttons for new commands to add.
     for (Runnable button : m_buttons) {
       button.run();
     }
     m_watchdog.addEpoch("buttons.run()");
 
     m_inRunLoop = true;
-    //Run scheduled commands, remove finished commands.
+    // Run scheduled commands, remove finished commands.
     for (Iterator<Command> iterator = m_scheduledCommands.keySet().iterator();
-         iterator.hasNext(); ) {
+        iterator.hasNext(); ) {
       Command command = iterator.next();
 
       if (!command.runsWhenDisabled() && RobotState.isDisabled()) {
@@ -301,7 +293,7 @@
     }
     m_inRunLoop = false;
 
-    //Schedule/cancel commands from queues populated during loop
+    // Schedule/cancel commands from queues populated during loop
     for (Map.Entry<Command, Boolean> commandInterruptible : m_toSchedule.entrySet()) {
       schedule(commandInterruptible.getValue(), commandInterruptible.getKey());
     }
@@ -313,7 +305,7 @@
     m_toSchedule.clear();
     m_toCancel.clear();
 
-    //Add default commands for un-required registered subsystems.
+    // Add default commands for un-required registered subsystems.
     for (Map.Entry<Subsystem, Command> subsystemCommand : m_subsystems.entrySet()) {
       if (!m_requirements.containsKey(subsystemCommand.getKey())
           && subsystemCommand.getValue() != null) {
@@ -329,10 +321,9 @@
   }
 
   /**
-   * Registers subsystems with the scheduler.  This must be called for the subsystem's periodic
-   * block to run when the scheduler is run, and for the subsystem's default command to be
-   * scheduled.  It is recommended to call this from the constructor of your subsystem
-   * implementations.
+   * Registers subsystems with the scheduler. This must be called for the subsystem's periodic block
+   * to run when the scheduler is run, and for the subsystem's default command to be scheduled. It
+   * is recommended to call this from the constructor of your subsystem implementations.
    *
    * @param subsystems the subsystem to register
    */
@@ -343,7 +334,7 @@
   }
 
   /**
-   * Un-registers subsystems with the scheduler.  The subsystem will no longer have its periodic
+   * Un-registers subsystems with the scheduler. The subsystem will no longer have its periodic
    * block called, and will not have its default command scheduled.
    *
    * @param subsystems the subsystem to un-register
@@ -353,13 +344,13 @@
   }
 
   /**
-   * Sets the default command for a subsystem.  Registers that subsystem if it is not already
-   * registered.  Default commands will run whenever there is no other command currently scheduled
-   * that requires the subsystem.  Default commands should be written to never end (i.e. their
-   * {@link Command#isFinished()} method should return false), as they would simply be re-scheduled
-   * if they do.  Default commands must also require their subsystem.
+   * Sets the default command for a subsystem. Registers that subsystem if it is not already
+   * registered. Default commands will run whenever there is no other command currently scheduled
+   * that requires the subsystem. Default commands should be written to never end (i.e. their {@link
+   * Command#isFinished()} method should return false), as they would simply be re-scheduled if they
+   * do. Default commands must also require their subsystem.
    *
-   * @param subsystem      the subsystem whose default command will be set
+   * @param subsystem the subsystem whose default command will be set
    * @param defaultCommand the default command to associate with the subsystem
    */
   public void setDefaultCommand(Subsystem subsystem, Command defaultCommand) {
@@ -375,7 +366,7 @@
   }
 
   /**
-   * Gets the default command associated with this subsystem.  Null if this subsystem has no default
+   * Gets the default command associated with this subsystem. Null if this subsystem has no default
    * command associated with it.
    *
    * @param subsystem the subsystem to inquire about
@@ -386,9 +377,9 @@
   }
 
   /**
-   * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method
-   * of the canceled command with {@code true},
-   * indicating they were canceled (as opposed to finishing normally).
+   * Cancels commands. The scheduler will only call {@link Command#end(boolean)} method of the
+   * canceled command with {@code true}, indicating they were canceled (as opposed to finishing
+   * normally).
    *
    * <p>Commands will be canceled even if they are not scheduled as interruptible.
    *
@@ -415,9 +406,7 @@
     }
   }
 
-  /**
-   * Cancels all commands that are currently scheduled.
-   */
+  /** Cancels all commands that are currently scheduled. */
   public void cancelAll() {
     for (Command command : m_scheduledCommands.keySet().toArray(new Command[0])) {
       cancel(command);
@@ -425,7 +414,7 @@
   }
 
   /**
-   * Returns the time since a given command was scheduled.  Note that this only works on commands
+   * Returns the time since a given command was scheduled. Note that this only works on commands
    * that are directly scheduled by the scheduler; it will not work on commands inside of
    * commandgroups, as the scheduler does not see them.
    *
@@ -442,9 +431,9 @@
   }
 
   /**
-   * Whether the given commands are running.  Note that this only works on commands that are
-   * directly scheduled by the scheduler; it will not work on commands inside of CommandGroups, as
-   * the scheduler does not see them.
+   * Whether the given commands are running. Note that this only works on commands that are directly
+   * scheduled by the scheduler; it will not work on commands inside of CommandGroups, as the
+   * scheduler does not see them.
    *
    * @param commands the command to query
    * @return whether the command is currently scheduled
@@ -454,26 +443,23 @@
   }
 
   /**
-   * Returns the command currently requiring a given subsystem.  Null if no command is currently
+   * Returns the command currently requiring a given subsystem. Null if no command is currently
    * requiring the subsystem
    *
    * @param subsystem the subsystem to be inquired about
-   * @return the command currently requiring the subsystem
+   * @return the command currently requiring the subsystem, or null if no command is currently
+   *     scheduled
    */
   public Command requiring(Subsystem subsystem) {
     return m_requirements.get(subsystem);
   }
 
-  /**
-   * Disables the command scheduler.
-   */
+  /** Disables the command scheduler. */
   public void disable() {
     m_disabled = true;
   }
 
-  /**
-   * Enables the command scheduler.
-   */
+  /** Enables the command scheduler. */
   public void enable() {
     m_disabled = false;
   }
@@ -515,39 +501,38 @@
   }
 
   @Override
-  public void initSendable(SendableBuilder builder) {
+  public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
     final NetworkTableEntry namesEntry = builder.getEntry("Names");
     final NetworkTableEntry idsEntry = builder.getEntry("Ids");
     final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
-    builder.setUpdateTable(() -> {
+    builder.setUpdateTable(
+        () -> {
+          if (namesEntry == null || idsEntry == null || cancelEntry == null) {
+            return;
+          }
 
-      if (namesEntry == null || idsEntry == null || cancelEntry == null) {
-        return;
-      }
+          Map<Double, Command> ids = new LinkedHashMap<>();
 
-      Map<Double, Command> ids = new LinkedHashMap<>();
+          for (Command command : m_scheduledCommands.keySet()) {
+            ids.put((double) command.hashCode(), command);
+          }
 
+          double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
+          if (toCancel.length > 0) {
+            for (double hash : toCancel) {
+              cancel(ids.get(hash));
+              ids.remove(hash);
+            }
+            cancelEntry.setDoubleArray(new double[0]);
+          }
 
-      for (Command command : m_scheduledCommands.keySet()) {
-        ids.put((double) command.hashCode(), command);
-      }
+          List<String> names = new ArrayList<>();
 
-      double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
-      if (toCancel.length > 0) {
-        for (double hash : toCancel) {
-          cancel(ids.get(hash));
-          ids.remove(hash);
-        }
-        cancelEntry.setDoubleArray(new double[0]);
-      }
+          ids.values().forEach(command -> names.add(command.getName()));
 
-      List<String> names = new ArrayList<>();
-
-      ids.values().forEach(command -> names.add(command.getName()));
-
-      namesEntry.setStringArray(names.toArray(new String[0]));
-      idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
-    });
+          namesEntry.setStringArray(names.toArray(new String[0]));
+          idsEntry.setNumberArray(ids.keySet().toArray(new Double[0]));
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
index 2e2dc7e..2d60982 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/CommandState.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
 import edu.wpi.first.wpilibj.Timer;
 
 /**
- * Class that holds scheduling state for a command.  Used internally by the
- * {@link CommandScheduler}.
+ * Class that holds scheduling state for a command. Used internally by the {@link CommandScheduler}.
  */
 class CommandState {
-  //The time since this command was initialized.
+  // The time since this command was initialized.
   private double m_startTime = -1;
 
-  //Whether or not it is interruptible.
+  // Whether or not it is interruptible.
   private final boolean m_interruptible;
 
   CommandState(boolean interruptible) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
index 8e3239c..74bf1ce 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ConditionalCommand.java
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.function.BooleanSupplier;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
 
+import java.util.function.BooleanSupplier;
+
 /**
  * Runs one of two commands, depending on the value of the given condition when this command is
- * initialized.  Does not actually schedule the selected command - rather, the command is run
- * through this command; this ensures that the command will behave as expected if used as part of a
- * CommandGroup.  Requires the requirements of both commands, again to ensure proper functioning
- * when used in a CommandGroup.  If this is undesired, consider using {@link ScheduleCommand}.
+ * initialized. Does not actually schedule the selected command - rather, the command is run through
+ * this command; this ensures that the command will behave as expected if used as part of a
+ * CommandGroup. Requires the requirements of both commands, again to ensure proper functioning when
+ * used in a CommandGroup. If this is undesired, consider using {@link ScheduleCommand}.
  *
  * <p>As this command contains multiple component commands within it, it is technically a command
  * group; the command instances that are passed to it cannot be added to any other groups, or
@@ -34,8 +31,8 @@
   /**
    * Creates a new ConditionalCommand.
    *
-   * @param onTrue    the command to run if the condition is true
-   * @param onFalse   the command to run if the condition is false
+   * @param onTrue the command to run if the condition is true
+   * @param onFalse the command to run if the condition is false
    * @param condition the condition to determine which command to run
    */
   public ConditionalCommand(Command onTrue, Command onFalse, BooleanSupplier condition) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
index b9dd6db..d9469c3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/FunctionalCommand.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 import java.util.function.BooleanSupplier;
 import java.util.function.Consumer;
 
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
  * A command that allows the user to pass in functions for each of the basic command methods through
- * the constructor.  Useful for inline definitions of complex commands - note, however, that if a
+ * the constructor. Useful for inline definitions of complex commands - note, however, that if a
  * command is beyond a certain complexity it is usually better practice to write a proper class for
  * it than to inline it.
  */
@@ -27,14 +24,18 @@
   /**
    * Creates a new FunctionalCommand.
    *
-   * @param onInit       the function to run on command initialization
-   * @param onExecute    the function to run on command execution
-   * @param onEnd        the function to run on command end
-   * @param isFinished   the function that determines whether the command has finished
+   * @param onInit the function to run on command initialization
+   * @param onExecute the function to run on command execution
+   * @param onEnd the function to run on command end
+   * @param isFinished the function that determines whether the command has finished
    * @param requirements the subsystems required by this command
    */
-  public FunctionalCommand(Runnable onInit, Runnable onExecute, Consumer<Boolean> onEnd,
-                           BooleanSupplier isFinished, Subsystem... requirements) {
+  public FunctionalCommand(
+      Runnable onInit,
+      Runnable onExecute,
+      Consumer<Boolean> onEnd,
+      BooleanSupplier isFinished,
+      Subsystem... requirements) {
     m_onInit = requireNonNullParam(onInit, "onInit", "FunctionalCommand");
     m_onExecute = requireNonNullParam(onExecute, "onExecute", "FunctionalCommand");
     m_onEnd = requireNonNullParam(onEnd, "onEnd", "FunctionalCommand");
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
index 8518890..10816df 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/InstantCommand.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 /**
- * A Command that runs instantly; it will initialize, execute once, and end on the same
- * iteration of the scheduler.  Users can either pass in a Runnable and a set of requirements,
- * or else subclass this command if desired.
+ * A Command that runs instantly; it will initialize, execute once, and end on the same iteration of
+ * the scheduler. Users can either pass in a Runnable and a set of requirements, or else subclass
+ * this command if desired.
  */
 public class InstantCommand extends CommandBase {
   private final Runnable m_toRun;
@@ -20,7 +17,7 @@
   /**
    * Creates a new InstantCommand that runs the given Runnable with the given requirements.
    *
-   * @param toRun        the Runnable to run
+   * @param toRun the Runnable to run
    * @param requirements the subsystems required by this command
    */
   public InstantCommand(Runnable toRun, Subsystem... requirements) {
@@ -30,12 +27,11 @@
   }
 
   /**
-   * Creates a new InstantCommand with a Runnable that does nothing.  Useful only as a no-arg
+   * Creates a new InstantCommand with a Runnable that does nothing. Useful only as a no-arg
    * constructor to call implicitly from subclass constructors.
    */
   public InstantCommand() {
-    m_toRun = () -> {
-    };
+    m_toRun = () -> {};
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
index 9105f7a..28c75a9 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommand.java
@@ -1,50 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
- * A command that uses two PID controllers ({@link PIDController}) and a
- * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
- * {@link Trajectory} with a mecanum drive.
+ * A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
+ * ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a mecanum drive.
  *
- * <p>The command handles trajectory-following,
- * Velocity PID calculations, and feedforwards internally. This
- * is intended to be a more-or-less "complete solution" that can be used by teams without a great
- * deal of controls expertise.
+ * <p>The command handles trajectory-following, Velocity PID calculations, and feedforwards
+ * internally. This is intended to be a more-or-less "complete solution" that can be used by teams
+ * without a great deal of controls expertise.
  *
- * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
- * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
- * the PID and feedforward functionality, returning only the raw wheel speeds from the PID
- * controllers.
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
+ * functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
+ * and feedforward functionality, returning only the raw wheel speeds from the PID controllers.
  *
- * <p>The robot angle controller does not follow the angle given by
- * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ * <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
+ * to the angle given in the final state of the trajectory.
  */
-
-@SuppressWarnings({"PMD.TooManyFields", "MemberName"})
+@SuppressWarnings("MemberName")
 public class MecanumControllerCommand extends CommandBase {
   private final Timer m_timer = new Timer();
   private final boolean m_usePID;
@@ -73,85 +65,79 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
    * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * @param trajectory                      The trajectory to follow.
-   * @param pose                            A function that supplies the robot pose - use one of
-   *                                        the odometry classes to provide this.
-   * @param feedforward                     The feedforward to use for the drivetrain.
-   * @param kinematics                      The kinematics for the robot drivetrain.
-   * @param xController                     The Trajectory Tracker PID controller
-   *                                        for the robot's x position.
-   * @param yController                     The Trajectory Tracker PID controller
-   *                                        for the robot's y position.
-   * @param thetaController                 The Trajectory Tracker PID controller
-   *                                        for angle for the robot.
-   * @param desiredRotation                 The angle that the robot should be facing. This
-   *                                        is sampled at each time step.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param feedforward The feedforward to use for the drivetrain.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
+   *     step.
    * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
-   * @param frontLeftController             The front left wheel velocity PID.
-   * @param rearLeftController              The rear left wheel velocity PID.
-   * @param frontRightController            The front right wheel velocity PID.
-   * @param rearRightController             The rear right wheel velocity PID.
-   * @param currentWheelSpeeds              A MecanumDriveWheelSpeeds object containing
-   *                                        the current wheel speeds.
-   * @param outputDriveVoltages             A MecanumDriveMotorVoltages object containing
-   *                                        the output motor voltages.
-   * @param requirements                    The subsystems to require.
+   * @param frontLeftController The front left wheel velocity PID.
+   * @param rearLeftController The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController The rear right wheel velocity PID.
+   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
+   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
+   *     voltages.
+   * @param requirements The subsystems to require.
    */
-
-  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
-  public MecanumControllerCommand(Trajectory trajectory,
-                                  Supplier<Pose2d> pose,
-                                  SimpleMotorFeedforward feedforward,
-                                  MecanumDriveKinematics kinematics,
-                                  PIDController xController,
-                                  PIDController yController,
-                                  ProfiledPIDController thetaController,
-                                  Supplier<Rotation2d> desiredRotation,
-                                  double maxWheelVelocityMetersPerSecond,
-                                  PIDController frontLeftController,
-                                  PIDController rearLeftController,
-                                  PIDController frontRightController,
-                                  PIDController rearRightController,
-                                  Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
-                                  Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
-                                  Subsystem... requirements) {
-    m_trajectory = requireNonNullParam(trajectory, "trajectory",
-        "MecanumControllerCommand");
+  @SuppressWarnings("ParameterName")
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SimpleMotorFeedforward feedforward,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      Supplier<Rotation2d> desiredRotation,
+      double maxWheelVelocityMetersPerSecond,
+      PIDController frontLeftController,
+      PIDController rearLeftController,
+      PIDController frontRightController,
+      PIDController rearRightController,
+      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
     m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
-    m_feedforward = requireNonNullParam(feedforward, "feedforward",
-        "MecanumControllerCommand");
-    m_kinematics = requireNonNullParam(kinematics, "kinematics",
-        "MecanumControllerCommand");
+    m_feedforward = requireNonNullParam(feedforward, "feedforward", "MecanumControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
 
-    m_controller = new HolonomicDriveController(
-        requireNonNullParam(xController,
-            "xController", "SwerveControllerCommand"),
-        requireNonNullParam(yController,
-            "xController", "SwerveControllerCommand"),
-        requireNonNullParam(thetaController,
-            "thetaController", "SwerveControllerCommand")
-    );
+    m_controller =
+        new HolonomicDriveController(
+            requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
+            requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
+            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
 
-    m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
-        "MecanumControllerCommand");
+    m_desiredRotation =
+        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
 
-    m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
-        "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+    m_maxWheelVelocityMetersPerSecond =
+        requireNonNullParam(
+            maxWheelVelocityMetersPerSecond,
+            "maxWheelVelocityMetersPerSecond",
+            "MecanumControllerCommand");
 
-    m_frontLeftController = requireNonNullParam(frontLeftController,
-        "frontLeftController", "MecanumControllerCommand");
-    m_rearLeftController = requireNonNullParam(rearLeftController,
-        "rearLeftController", "MecanumControllerCommand");
-    m_frontRightController = requireNonNullParam(frontRightController,
-        "frontRightController", "MecanumControllerCommand");
-    m_rearRightController = requireNonNullParam(rearRightController,
-        "rearRightController", "MecanumControllerCommand");
+    m_frontLeftController =
+        requireNonNullParam(frontLeftController, "frontLeftController", "MecanumControllerCommand");
+    m_rearLeftController =
+        requireNonNullParam(rearLeftController, "rearLeftController", "MecanumControllerCommand");
+    m_frontRightController =
+        requireNonNullParam(
+            frontRightController, "frontRightController", "MecanumControllerCommand");
+    m_rearRightController =
+        requireNonNullParam(rearRightController, "rearRightController", "MecanumControllerCommand");
 
-    m_currentWheelSpeeds = requireNonNullParam(currentWheelSpeeds,
-        "currentWheelSpeeds", "MecanumControllerCommand");
+    m_currentWheelSpeeds =
+        requireNonNullParam(currentWheelSpeeds, "currentWheelSpeeds", "MecanumControllerCommand");
 
-    m_outputDriveVoltages = requireNonNullParam(outputDriveVoltages,
-        "outputDriveVoltages", "MecanumControllerCommand");
+    m_outputDriveVoltages =
+        requireNonNullParam(outputDriveVoltages, "outputDriveVoltages", "MecanumControllerCommand");
 
     m_outputWheelSpeeds = null;
 
@@ -168,56 +154,63 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path
    * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The final rotation of the robot will be set to the rotation of
-   * the final pose in the trajectory. The robot will not follow the rotations
-   * from the poses at each timestep. If alternate rotation behavior is desired,
-   * the other constructor with a supplier for rotation should be used.
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
+   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
+   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
+   * should be used.
    *
-   * @param trajectory                      The trajectory to follow.
-   * @param pose                            A function that supplies the robot pose - use one of
-   *                                        the odometry classes to provide this.
-   * @param feedforward                     The feedforward to use for the drivetrain.
-   * @param kinematics                      The kinematics for the robot drivetrain.
-   * @param xController                     The Trajectory Tracker PID controller
-   *                                        for the robot's x position.
-   * @param yController                     The Trajectory Tracker PID controller
-   *                                        for the robot's y position.
-   * @param thetaController                 The Trajectory Tracker PID controller
-   *                                        for angle for the robot.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param feedforward The feedforward to use for the drivetrain.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
    * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
-   * @param frontLeftController             The front left wheel velocity PID.
-   * @param rearLeftController              The rear left wheel velocity PID.
-   * @param frontRightController            The front right wheel velocity PID.
-   * @param rearRightController             The rear right wheel velocity PID.
-   * @param currentWheelSpeeds              A MecanumDriveWheelSpeeds object containing
-   *                                        the current wheel speeds.
-   * @param outputDriveVoltages             A MecanumDriveMotorVoltages object containing
-   *                                        the output motor voltages.
-   * @param requirements                    The subsystems to require.
+   * @param frontLeftController The front left wheel velocity PID.
+   * @param rearLeftController The rear left wheel velocity PID.
+   * @param frontRightController The front right wheel velocity PID.
+   * @param rearRightController The rear right wheel velocity PID.
+   * @param currentWheelSpeeds A MecanumDriveWheelSpeeds object containing the current wheel speeds.
+   * @param outputDriveVoltages A MecanumDriveMotorVoltages object containing the output motor
+   *     voltages.
+   * @param requirements The subsystems to require.
    */
-
-  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
-  public MecanumControllerCommand(Trajectory trajectory,
-                                  Supplier<Pose2d> pose,
-                                  SimpleMotorFeedforward feedforward,
-                                  MecanumDriveKinematics kinematics,
-                                  PIDController xController,
-                                  PIDController yController,
-                                  ProfiledPIDController thetaController,
-                                  double maxWheelVelocityMetersPerSecond,
-                                  PIDController frontLeftController,
-                                  PIDController rearLeftController,
-                                  PIDController frontRightController,
-                                  PIDController rearRightController,
-                                  Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
-                                  Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
-                                  Subsystem... requirements) {
-
-    this(trajectory, pose, feedforward, kinematics, xController, yController, thetaController,
-        () -> trajectory.getStates().get(trajectory.getStates().size() - 1)
-            .poseMeters.getRotation(),
-        maxWheelVelocityMetersPerSecond, frontLeftController, rearLeftController,
-        frontRightController, rearRightController, currentWheelSpeeds, outputDriveVoltages,
+  @SuppressWarnings("ParameterName")
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SimpleMotorFeedforward feedforward,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      double maxWheelVelocityMetersPerSecond,
+      PIDController frontLeftController,
+      PIDController rearLeftController,
+      PIDController frontRightController,
+      PIDController rearRightController,
+      Supplier<MecanumDriveWheelSpeeds> currentWheelSpeeds,
+      Consumer<MecanumDriveMotorVoltages> outputDriveVoltages,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        feedforward,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        () ->
+            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
+        maxWheelVelocityMetersPerSecond,
+        frontLeftController,
+        rearLeftController,
+        frontRightController,
+        rearRightController,
+        currentWheelSpeeds,
+        outputDriveVoltages,
         requirements);
   }
 
@@ -228,56 +221,50 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
    * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
    *
-   * @param trajectory                      The trajectory to follow.
-   * @param pose                            A function that supplies the robot pose - use one of
-   *                                        the odometry classes to provide this.
-   * @param kinematics                      The kinematics for the robot drivetrain.
-   * @param xController                     The Trajectory Tracker PID controller
-   *                                        for the robot's x position.
-   * @param yController                     The Trajectory Tracker PID controller
-   *                                        for the robot's y position.
-   * @param thetaController                 The Trajectory Tracker PID controller
-   *                                        for angle for the robot.
-   * @param desiredRotation                 The angle that the robot should be facing. This
-   *                                        is sampled at each time step.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param desiredRotation The angle that the robot should be facing. This is sampled at each time
+   *     step.
    * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
-   * @param outputWheelSpeeds               A MecanumDriveWheelSpeeds object containing
-   *                                        the output wheel speeds.
-   * @param requirements                    The subsystems to require.
+   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
+   * @param requirements The subsystems to require.
    */
-
-  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
-  public MecanumControllerCommand(Trajectory trajectory,
-                                  Supplier<Pose2d> pose,
-                                  MecanumDriveKinematics kinematics,
-                                  PIDController xController,
-                                  PIDController yController,
-                                  ProfiledPIDController thetaController,
-                                  Supplier<Rotation2d> desiredRotation,
-                                  double maxWheelVelocityMetersPerSecond,
-                                  Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
-                                  Subsystem... requirements) {
-    m_trajectory = requireNonNullParam(trajectory, "trajectory",
-        "MecanumControllerCommand");
+  @SuppressWarnings("ParameterName")
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      Supplier<Rotation2d> desiredRotation,
+      double maxWheelVelocityMetersPerSecond,
+      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+      Subsystem... requirements) {
+    m_trajectory = requireNonNullParam(trajectory, "trajectory", "MecanumControllerCommand");
     m_pose = requireNonNullParam(pose, "pose", "MecanumControllerCommand");
     m_feedforward = new SimpleMotorFeedforward(0, 0, 0);
-    m_kinematics = requireNonNullParam(kinematics,
-        "kinematics", "MecanumControllerCommand");
+    m_kinematics = requireNonNullParam(kinematics, "kinematics", "MecanumControllerCommand");
 
-    m_controller = new HolonomicDriveController(
-        requireNonNullParam(xController,
-            "xController", "SwerveControllerCommand"),
-        requireNonNullParam(yController,
-            "xController", "SwerveControllerCommand"),
-        requireNonNullParam(thetaController,
-            "thetaController", "SwerveControllerCommand")
-    );
+    m_controller =
+        new HolonomicDriveController(
+            requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
+            requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
+            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
 
-    m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
-        "MecanumControllerCommand");
+    m_desiredRotation =
+        requireNonNullParam(desiredRotation, "desiredRotation", "MecanumControllerCommand");
 
-    m_maxWheelVelocityMetersPerSecond = requireNonNullParam(maxWheelVelocityMetersPerSecond,
-        "maxWheelVelocityMetersPerSecond", "MecanumControllerCommand");
+    m_maxWheelVelocityMetersPerSecond =
+        requireNonNullParam(
+            maxWheelVelocityMetersPerSecond,
+            "maxWheelVelocityMetersPerSecond",
+            "MecanumControllerCommand");
 
     m_frontLeftController = null;
     m_rearLeftController = null;
@@ -286,8 +273,8 @@
 
     m_currentWheelSpeeds = null;
 
-    m_outputWheelSpeeds = requireNonNullParam(outputWheelSpeeds,
-        "outputWheelSpeeds", "MecanumControllerCommand");
+    m_outputWheelSpeeds =
+        requireNonNullParam(outputWheelSpeeds, "outputWheelSpeeds", "MecanumControllerCommand");
 
     m_outputDriveVoltages = null;
 
@@ -303,54 +290,58 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path -
    * this is left to the user, since it is not appropriate for paths with non-stationary end-states.
    *
-   * <p>Note 2: The final rotation of the robot will be set to the rotation of
-   * the final pose in the trajectory. The robot will not follow the rotations
-   * from the poses at each timestep. If alternate rotation behavior is desired,
-   * the other constructor with a supplier for rotation should be used.
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
+   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
+   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
+   * should be used.
    *
-   * @param trajectory                      The trajectory to follow.
-   * @param pose                            A function that supplies the robot pose - use one of
-   *                                        the odometry classes to provide this.
-   * @param kinematics                      The kinematics for the robot drivetrain.
-   * @param xController                     The Trajectory Tracker PID controller
-   *                                        for the robot's x position.
-   * @param yController                     The Trajectory Tracker PID controller
-   *                                        for the robot's y position.
-   * @param thetaController                 The Trajectory Tracker PID controller
-   *                                        for angle for the robot.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
    * @param maxWheelVelocityMetersPerSecond The maximum velocity of a drivetrain wheel.
-   * @param outputWheelSpeeds               A MecanumDriveWheelSpeeds object containing
-   *                                        the output wheel speeds.
-   * @param requirements                    The subsystems to require.
+   * @param outputWheelSpeeds A MecanumDriveWheelSpeeds object containing the output wheel speeds.
+   * @param requirements The subsystems to require.
    */
-
-  @SuppressWarnings({"PMD.ExcessiveParameterList", "ParameterName"})
-  public MecanumControllerCommand(Trajectory trajectory,
-                                  Supplier<Pose2d> pose,
-                                  MecanumDriveKinematics kinematics,
-                                  PIDController xController,
-                                  PIDController yController,
-                                  ProfiledPIDController thetaController,
-                                  double maxWheelVelocityMetersPerSecond,
-                                  Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
-                                  Subsystem... requirements) {
-    this(trajectory, pose, kinematics, xController, yController, thetaController,
-        () -> trajectory.getStates().get(trajectory.getStates().size() - 1)
-            .poseMeters.getRotation(),
-        maxWheelVelocityMetersPerSecond, outputWheelSpeeds, requirements);
+  @SuppressWarnings("ParameterName")
+  public MecanumControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      MecanumDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      double maxWheelVelocityMetersPerSecond,
+      Consumer<MecanumDriveWheelSpeeds> outputWheelSpeeds,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        () ->
+            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
+        maxWheelVelocityMetersPerSecond,
+        outputWheelSpeeds,
+        requirements);
   }
 
   @Override
   public void initialize() {
     var initialState = m_trajectory.sample(0);
 
-    var initialXVelocity = initialState.velocityMetersPerSecond
-        * initialState.poseMeters.getRotation().getCos();
-    var initialYVelocity = initialState.velocityMetersPerSecond
-        * initialState.poseMeters.getRotation().getSin();
+    var initialXVelocity =
+        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getCos();
+    var initialYVelocity =
+        initialState.velocityMetersPerSecond * initialState.poseMeters.getRotation().getSin();
 
-    m_prevSpeeds = m_kinematics.toWheelSpeeds(
-        new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
+    m_prevSpeeds =
+        m_kinematics.toWheelSpeeds(new ChassisSpeeds(initialXVelocity, initialYVelocity, 0.0));
 
     m_timer.reset();
     m_timer.start();
@@ -364,8 +355,8 @@
 
     var desiredState = m_trajectory.sample(curTime);
 
-    var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
-        m_desiredRotation.get());
+    var targetChassisSpeeds =
+        m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
     var targetWheelSpeeds = m_kinematics.toWheelSpeeds(targetChassisSpeeds);
 
     targetWheelSpeeds.normalize(m_maxWheelVelocityMetersPerSecond);
@@ -381,46 +372,57 @@
     double rearRightOutput;
 
     if (m_usePID) {
-      final double frontLeftFeedforward = m_feedforward.calculate(frontLeftSpeedSetpoint,
-          (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
+      final double frontLeftFeedforward =
+          m_feedforward.calculate(
+              frontLeftSpeedSetpoint,
+              (frontLeftSpeedSetpoint - m_prevSpeeds.frontLeftMetersPerSecond) / dt);
 
-      final double rearLeftFeedforward = m_feedforward.calculate(rearLeftSpeedSetpoint,
-          (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
+      final double rearLeftFeedforward =
+          m_feedforward.calculate(
+              rearLeftSpeedSetpoint,
+              (rearLeftSpeedSetpoint - m_prevSpeeds.rearLeftMetersPerSecond) / dt);
 
-      final double frontRightFeedforward = m_feedforward.calculate(frontRightSpeedSetpoint,
-          (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
+      final double frontRightFeedforward =
+          m_feedforward.calculate(
+              frontRightSpeedSetpoint,
+              (frontRightSpeedSetpoint - m_prevSpeeds.frontRightMetersPerSecond) / dt);
 
-      final double rearRightFeedforward = m_feedforward.calculate(rearRightSpeedSetpoint,
-          (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
+      final double rearRightFeedforward =
+          m_feedforward.calculate(
+              rearRightSpeedSetpoint,
+              (rearRightSpeedSetpoint - m_prevSpeeds.rearRightMetersPerSecond) / dt);
 
-      frontLeftOutput = frontLeftFeedforward + m_frontLeftController.calculate(
-          m_currentWheelSpeeds.get().frontLeftMetersPerSecond,
-          frontLeftSpeedSetpoint);
+      frontLeftOutput =
+          frontLeftFeedforward
+              + m_frontLeftController.calculate(
+                  m_currentWheelSpeeds.get().frontLeftMetersPerSecond, frontLeftSpeedSetpoint);
 
-      rearLeftOutput = rearLeftFeedforward + m_rearLeftController.calculate(
-          m_currentWheelSpeeds.get().rearLeftMetersPerSecond,
-          rearLeftSpeedSetpoint);
+      rearLeftOutput =
+          rearLeftFeedforward
+              + m_rearLeftController.calculate(
+                  m_currentWheelSpeeds.get().rearLeftMetersPerSecond, rearLeftSpeedSetpoint);
 
-      frontRightOutput = frontRightFeedforward + m_frontRightController.calculate(
-          m_currentWheelSpeeds.get().frontRightMetersPerSecond,
-          frontRightSpeedSetpoint);
+      frontRightOutput =
+          frontRightFeedforward
+              + m_frontRightController.calculate(
+                  m_currentWheelSpeeds.get().frontRightMetersPerSecond, frontRightSpeedSetpoint);
 
-      rearRightOutput = rearRightFeedforward + m_rearRightController.calculate(
-          m_currentWheelSpeeds.get().rearRightMetersPerSecond,
-          rearRightSpeedSetpoint);
+      rearRightOutput =
+          rearRightFeedforward
+              + m_rearRightController.calculate(
+                  m_currentWheelSpeeds.get().rearRightMetersPerSecond, rearRightSpeedSetpoint);
 
-      m_outputDriveVoltages.accept(new MecanumDriveMotorVoltages(
-          frontLeftOutput,
-          frontRightOutput,
-          rearLeftOutput,
-          rearRightOutput));
+      m_outputDriveVoltages.accept(
+          new MecanumDriveMotorVoltages(
+              frontLeftOutput, frontRightOutput, rearLeftOutput, rearRightOutput));
 
     } else {
-      m_outputWheelSpeeds.accept(new MecanumDriveWheelSpeeds(
-          frontLeftSpeedSetpoint,
-          frontRightSpeedSetpoint,
-          rearLeftSpeedSetpoint,
-          rearRightSpeedSetpoint));
+      m_outputWheelSpeeds.accept(
+          new MecanumDriveWheelSpeeds(
+              frontLeftSpeedSetpoint,
+              frontRightSpeedSetpoint,
+              rearLeftSpeedSetpoint,
+              rearRightSpeedSetpoint));
     }
 
     m_prevTime = curTime;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
index e63fcc5..487aa12 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/NotifierCommand.java
@@ -1,24 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.function.BooleanSupplier;
-
 import edu.wpi.first.wpilibj.Notifier;
 
 /**
- * A command that starts a notifier to run the given runnable periodically in a separate thread.
- * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
- * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ * A command that starts a notifier to run the given runnable periodically in a separate thread. Has
+ * no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or {@link
+ * Command#withInterrupt(java.util.function.BooleanSupplier)} to give it one.
  *
  * <p>WARNING: Do not use this class unless you are confident in your ability to make the executed
- * code thread-safe.  If you do not know what "thread-safe" means, that is a good sign that
- * you should not use this class.
+ * code thread-safe. If you do not know what "thread-safe" means, that is a good sign that you
+ * should not use this class.
  */
 public class NotifierCommand extends CommandBase {
   protected final Notifier m_notifier;
@@ -27,8 +22,8 @@
   /**
    * Creates a new NotifierCommand.
    *
-   * @param toRun        the runnable for the notifier to run
-   * @param period       the period at which the notifier should run, in seconds
+   * @param toRun the runnable for the notifier to run
+   * @param period the period at which the notifier should run, in seconds
    * @param requirements the subsystems required by this command
    */
   public NotifierCommand(Runnable toRun, double period, Subsystem... requirements) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
index 5f2b093..dcf8dec 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDCommand.java
@@ -1,24 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.PIDController;
 import java.util.Set;
 import java.util.function.DoubleConsumer;
 import java.util.function.DoubleSupplier;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
- * A command that controls an output with a {@link PIDController}.  Runs forever by default - to add
- * exit conditions and/or other behavior, subclass this class.  The controller calculation and
- * output are performed synchronously in the command's execute() method.
+ * A command that controls an output with a {@link PIDController}. Runs forever by default - to add
+ * exit conditions and/or other behavior, subclass this class. The controller calculation and output
+ * are performed synchronously in the command's execute() method.
  */
 public class PIDCommand extends CommandBase {
   protected final PIDController m_controller;
@@ -29,15 +25,18 @@
   /**
    * Creates a new PIDCommand, which controls the given output with a PIDController.
    *
-   * @param controller        the controller that controls the output.
+   * @param controller the controller that controls the output.
    * @param measurementSource the measurement of the process variable
-   * @param setpointSource    the controller's setpoint
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
+   * @param setpointSource the controller's setpoint
+   * @param useOutput the controller's output
+   * @param requirements the subsystems required by this command
    */
-  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
-                    DoubleSupplier setpointSource, DoubleConsumer useOutput,
-                    Subsystem... requirements) {
+  public PIDCommand(
+      PIDController controller,
+      DoubleSupplier measurementSource,
+      DoubleSupplier setpointSource,
+      DoubleConsumer useOutput,
+      Subsystem... requirements) {
     requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
     requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
     requireNonNullParam(setpointSource, "setpointSource", "SynchronousPIDCommand");
@@ -53,15 +52,18 @@
   /**
    * Creates a new PIDCommand, which controls the given output with a PIDController.
    *
-   * @param controller        the controller that controls the output.
+   * @param controller the controller that controls the output.
    * @param measurementSource the measurement of the process variable
-   * @param setpoint          the controller's setpoint
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
+   * @param setpoint the controller's setpoint
+   * @param useOutput the controller's output
+   * @param requirements the subsystems required by this command
    */
-  public PIDCommand(PIDController controller, DoubleSupplier measurementSource,
-                    double setpoint, DoubleConsumer useOutput,
-                    Subsystem... requirements) {
+  public PIDCommand(
+      PIDController controller,
+      DoubleSupplier measurementSource,
+      double setpoint,
+      DoubleConsumer useOutput,
+      Subsystem... requirements) {
     this(controller, measurementSource, () -> setpoint, useOutput, requirements);
   }
 
@@ -72,8 +74,8 @@
 
   @Override
   public void execute() {
-    m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(),
-                                              m_setpoint.getAsDouble()));
+    m_useOutput.accept(
+        m_controller.calculate(m_measurement.getAsDouble(), m_setpoint.getAsDouble()));
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
index 4cc578d..ad9bb77 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PIDSubsystem.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.math.controller.PIDController;
+
 /**
- * A subsystem that uses a {@link PIDController} to control an output.  The controller is run
+ * A subsystem that uses a {@link PIDController} to control an output. The controller is run
  * synchronously from the subsystem's periodic() method.
  */
 public abstract class PIDSubsystem extends SubsystemBase {
@@ -34,7 +31,7 @@
   }
 
   /**
-   * Creates a new PIDSubsystem.  Initial setpoint is zero.
+   * Creates a new PIDSubsystem. Initial setpoint is zero.
    *
    * @param controller the PIDController to use
    */
@@ -86,17 +83,13 @@
    */
   protected abstract double getMeasurement();
 
-  /**
-   * Enables the PID control.  Resets the controller.
-   */
+  /** Enables the PID control. Resets the controller. */
   public void enable() {
     m_enabled = true;
     m_controller.reset();
   }
 
-  /**
-   * Disables the PID control.  Sets output to zero.
-   */
+  /** Disables the PID control. Sets output to zero. */
   public void disable() {
     m_enabled = false;
     useOutput(0, 0);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
index 38cc3c1..7195606 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -17,14 +14,14 @@
  * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
  */
 public class ParallelCommandGroup extends CommandGroupBase {
-  //maps commands in this group to whether they are still running
+  // maps commands in this group to whether they are still running
   private final Map<Command, Boolean> m_commands = new HashMap<>();
   private boolean m_runWhenDisabled = true;
 
   /**
-   * Creates a new ParallelCommandGroup.  The given commands will be executed simultaneously.
-   * The command group will finish when the last command finishes.  If the CommandGroup is
-   * interrupted, only the commands that are still running will be interrupted.
+   * Creates a new ParallelCommandGroup. The given commands will be executed simultaneously. The
+   * command group will finish when the last command finishes. If the CommandGroup is interrupted,
+   * only the commands that are still running will be interrupted.
    *
    * @param commands the commands to include in this group.
    */
@@ -45,8 +42,8 @@
 
     for (Command command : commands) {
       if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
-        throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
-            + "require the same subsystems");
+        throw new IllegalArgumentException(
+            "Multiple commands in a parallel group cannot" + "require the same subsystems");
       }
       m_commands.put(command, false);
       m_requirements.addAll(command.getRequirements());
@@ -89,11 +86,17 @@
 
   @Override
   public boolean isFinished() {
-    return !m_commands.values().contains(true);
+    return !m_commands.containsValue(true);
   }
 
   @Override
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public ParallelCommandGroup alongWith(Command... parallel) {
+    addCommands(parallel);
+    return this;
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
index 37be702..d444991 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -12,23 +9,23 @@
 import java.util.Map;
 
 /**
- * A CommandGroup that runs a set of commands in parallel, ending only when a specific command
- * (the "deadline") ends, interrupting all other commands that are still running at that point.
+ * A CommandGroup that runs a set of commands in parallel, ending only when a specific command (the
+ * "deadline") ends, interrupting all other commands that are still running at that point.
  *
  * <p>As a rule, CommandGroups require the union of the requirements of their component commands.
  */
 public class ParallelDeadlineGroup extends CommandGroupBase {
-  //maps commands in this group to whether they are still running
+  // maps commands in this group to whether they are still running
   private final Map<Command, Boolean> m_commands = new HashMap<>();
   private boolean m_runWhenDisabled = true;
   private boolean m_finished = true;
   private Command m_deadline;
 
   /**
-   * Creates a new ParallelDeadlineGroup.  The given commands (including the deadline) will be
-   * executed simultaneously.  The CommandGroup will finish when the deadline finishes,
-   * interrupting all other still-running commands.  If the CommandGroup is interrupted, only
-   * the commands still running will be interrupted.
+   * Creates a new ParallelDeadlineGroup. The given commands (including the deadline) will be
+   * executed simultaneously. The CommandGroup will finish when the deadline finishes, interrupting
+   * all other still-running commands. If the CommandGroup is interrupted, only the commands still
+   * running will be interrupted.
    *
    * @param deadline the command that determines when the group ends
    * @param commands the commands to be executed
@@ -42,8 +39,8 @@
   }
 
   /**
-   * Sets the deadline to the given command.  The deadline is added to the group if it is not
-   * already contained.
+   * Sets the deadline to the given command. The deadline is added to the group if it is not already
+   * contained.
    *
    * @param deadline the command that determines when the group ends
    */
@@ -67,8 +64,8 @@
 
     for (Command command : commands) {
       if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
-        throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
-            + "require the same subsystems");
+        throw new IllegalArgumentException(
+            "Multiple commands in a parallel group cannot" + "require the same subsystems");
       }
       m_commands.put(command, false);
       m_requirements.addAll(command.getRequirements());
@@ -120,4 +117,10 @@
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public ParallelDeadlineGroup deadlineWith(Command... parallel) {
+    addCommands(parallel);
+    return this;
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
index dcaf5f5..2d79885 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -23,8 +20,8 @@
   private boolean m_finished = true;
 
   /**
-   * Creates a new ParallelCommandRace.  The given commands will be executed simultaneously, and
-   * will "race to the finish" - the first command to finish ends the entire command, with all other
+   * Creates a new ParallelCommandRace. The given commands will be executed simultaneously, and will
+   * "race to the finish" - the first command to finish ends the entire command, with all other
    * commands being interrupted.
    *
    * @param commands the commands to include in this group.
@@ -46,8 +43,8 @@
 
     for (Command command : commands) {
       if (!Collections.disjoint(command.getRequirements(), m_requirements)) {
-        throw new IllegalArgumentException("Multiple commands in a parallel group cannot"
-            + " require the same subsystems");
+        throw new IllegalArgumentException(
+            "Multiple commands in a parallel group cannot" + " require the same subsystems");
       }
       m_commands.add(command);
       m_requirements.addAll(command.getRequirements());
@@ -89,4 +86,10 @@
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public ParallelRaceGroup raceWith(Command... parallel) {
+    addCommands(parallel);
+    return this;
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
index 6ebb376..af219dc 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PerpetualCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -11,7 +8,7 @@
 import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
 
 /**
- * A command that runs another command in perpetuity, ignoring that command's end conditions.  While
+ * A command that runs another command in perpetuity, ignoring that command's end conditions. While
  * this class does not extend {@link CommandGroupBase}, it is still considered a CommandGroup, as it
  * allows one to compose another command within it; the command instances that are passed to it
  * cannot be added to any other groups, or scheduled individually.
@@ -22,8 +19,8 @@
   protected final Command m_command;
 
   /**
-   * Creates a new PerpetualCommand.  Will run another command in perpetuity, ignoring that
-   * command's end conditions, unless this command itself is interrupted.
+   * Creates a new PerpetualCommand. Will run another command in perpetuity, ignoring that command's
+   * end conditions, unless this command itself is interrupted.
    *
    * @param command the command to run perpetually
    */
@@ -53,4 +50,9 @@
   public boolean runsWhenDisabled() {
     return m_command.runsWhenDisabled();
   }
+
+  @Override
+  public PerpetualCommand perpetually() {
+    return this;
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
index 4fb4126..33415d0 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/PrintCommand.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-/**
- * A command that prints a string when initialized.
- */
+/** A command that prints a string when initialized. */
 public class PrintCommand extends InstantCommand {
   /**
    * Creates a new a PrintCommand.
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
index 96e18a0..df56561 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDCommand.java
@@ -1,26 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.math.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
 import java.util.Set;
 import java.util.function.BiConsumer;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-
-import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
- * A command that controls an output with a {@link ProfiledPIDController}.  Runs forever by
- * default - to add
- * exit conditions and/or other behavior, subclass this class.  The controller calculation and
+ * A command that controls an output with a {@link ProfiledPIDController}. Runs forever by default -
+ * to add exit conditions and/or other behavior, subclass this class. The controller calculation and
  * output are performed synchronously in the command's execute() method.
  */
 public class ProfiledPIDCommand extends CommandBase {
@@ -30,18 +25,21 @@
   protected BiConsumer<Double, State> m_useOutput;
 
   /**
-   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
-   * Goal velocity is specified.
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+   * velocity is specified.
    *
-   * @param controller        the controller that controls the output.
+   * @param controller the controller that controls the output.
    * @param measurementSource the measurement of the process variable
-   * @param goalSource        the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
+   * @param goalSource the controller's goal
+   * @param useOutput the controller's output
+   * @param requirements the subsystems required by this command
    */
-  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
-                            Supplier<State> goalSource, BiConsumer<Double, State> useOutput,
-                            Subsystem... requirements) {
+  public ProfiledPIDCommand(
+      ProfiledPIDController controller,
+      DoubleSupplier measurementSource,
+      Supplier<State> goalSource,
+      BiConsumer<Double, State> useOutput,
+      Subsystem... requirements) {
     requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
     requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
     requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
@@ -55,18 +53,21 @@
   }
 
   /**
-   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController.
-   * Goal velocity is implicitly zero.
+   * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
+   * velocity is implicitly zero.
    *
-   * @param controller        the controller that controls the output.
+   * @param controller the controller that controls the output.
    * @param measurementSource the measurement of the process variable
-   * @param goalSource        the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
+   * @param goalSource the controller's goal
+   * @param useOutput the controller's output
+   * @param requirements the subsystems required by this command
    */
-  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
-                            DoubleSupplier goalSource, BiConsumer<Double, State> useOutput,
-                            Subsystem... requirements) {
+  public ProfiledPIDCommand(
+      ProfiledPIDController controller,
+      DoubleSupplier measurementSource,
+      DoubleSupplier goalSource,
+      BiConsumer<Double, State> useOutput,
+      Subsystem... requirements) {
     requireNonNullParam(controller, "controller", "SynchronousPIDCommand");
     requireNonNullParam(measurementSource, "measurementSource", "SynchronousPIDCommand");
     requireNonNullParam(goalSource, "goalSource", "SynchronousPIDCommand");
@@ -83,15 +84,18 @@
    * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
    * velocity is specified.
    *
-   * @param controller        the controller that controls the output.
+   * @param controller the controller that controls the output.
    * @param measurementSource the measurement of the process variable
-   * @param goal              the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
+   * @param goal the controller's goal
+   * @param useOutput the controller's output
+   * @param requirements the subsystems required by this command
    */
-  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
-                            State goal, BiConsumer<Double, State> useOutput,
-                            Subsystem... requirements) {
+  public ProfiledPIDCommand(
+      ProfiledPIDController controller,
+      DoubleSupplier measurementSource,
+      State goal,
+      BiConsumer<Double, State> useOutput,
+      Subsystem... requirements) {
     this(controller, measurementSource, () -> goal, useOutput, requirements);
   }
 
@@ -99,15 +103,18 @@
    * Creates a new PIDCommand, which controls the given output with a ProfiledPIDController. Goal
    * velocity is implicitly zero.
    *
-   * @param controller        the controller that controls the output.
+   * @param controller the controller that controls the output.
    * @param measurementSource the measurement of the process variable
-   * @param goal              the controller's goal
-   * @param useOutput         the controller's output
-   * @param requirements      the subsystems required by this command
+   * @param goal the controller's goal
+   * @param useOutput the controller's output
+   * @param requirements the subsystems required by this command
    */
-  public ProfiledPIDCommand(ProfiledPIDController controller, DoubleSupplier measurementSource,
-                            double goal, BiConsumer<Double, State> useOutput,
-                            Subsystem... requirements) {
+  public ProfiledPIDCommand(
+      ProfiledPIDController controller,
+      DoubleSupplier measurementSource,
+      double goal,
+      BiConsumer<Double, State> useOutput,
+      Subsystem... requirements) {
     this(controller, measurementSource, () -> goal, useOutput, requirements);
   }
 
@@ -118,8 +125,9 @@
 
   @Override
   public void execute() {
-    m_useOutput.accept(m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
-                       m_controller.getSetpoint());
+    m_useOutput.accept(
+        m_controller.calculate(m_measurement.getAsDouble(), m_goal.get()),
+        m_controller.getSetpoint());
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
index 2385ada..85ece1f 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProfiledPIDSubsystem.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
 /**
- * A subsystem that uses a {@link ProfiledPIDController} to control an output.  The controller is
- * run synchronously from the subsystem's periodic() method.
+ * A subsystem that uses a {@link ProfiledPIDController} to control an output. The controller is run
+ * synchronously from the subsystem's periodic() method.
  */
 public abstract class ProfiledPIDSubsystem extends SubsystemBase {
   protected final ProfiledPIDController m_controller;
@@ -29,14 +26,13 @@
    * @param controller the ProfiledPIDController to use
    * @param initialPosition the initial goal position of the controller
    */
-  public ProfiledPIDSubsystem(ProfiledPIDController controller,
-                              double initialPosition) {
+  public ProfiledPIDSubsystem(ProfiledPIDController controller, double initialPosition) {
     m_controller = requireNonNullParam(controller, "controller", "ProfiledPIDSubsystem");
     setGoal(initialPosition);
   }
 
   /**
-   * Creates a new ProfiledPIDSubsystem.  Initial goal position is zero.
+   * Creates a new ProfiledPIDSubsystem. Initial goal position is zero.
    *
    * @param controller the ProfiledPIDController to use
    */
@@ -65,7 +61,7 @@
   }
 
   /**
-   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
    *
    * @param goal The goal position for the subsystem's motion profile.
    */
@@ -76,7 +72,7 @@
   /**
    * Uses the output from the ProfiledPIDController.
    *
-   * @param output   the output of the ProfiledPIDController
+   * @param output the output of the ProfiledPIDController
    * @param setpoint the setpoint state of the ProfiledPIDController, for feedforward
    */
   protected abstract void useOutput(double output, State setpoint);
@@ -88,17 +84,13 @@
    */
   protected abstract double getMeasurement();
 
-  /**
-   * Enables the PID control.  Resets the controller.
-   */
+  /** Enables the PID control. Resets the controller. */
   public void enable() {
     m_enabled = true;
     m_controller.reset(getMeasurement());
   }
 
-  /**
-   * Disables the PID control.  Sets output to zero.
-   */
+  /** Disables the PID control. Sets output to zero. */
   public void disable() {
     m_enabled = false;
     useOutput(0, new State());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
index 27d67bc..ae403a4 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -11,7 +8,7 @@
 
 /**
  * Schedules the given commands when this command is initialized, and ends when all the commands are
- * no longer scheduled.  Useful for forking off from CommandGroups.  If this command is interrupted,
+ * no longer scheduled. Useful for forking off from CommandGroups. If this command is interrupted,
  * it will cancel all of the commands.
  */
 public class ProxyScheduleCommand extends CommandBase {
@@ -19,8 +16,8 @@
   private boolean m_finished;
 
   /**
-   * Creates a new ProxyScheduleCommand that schedules the given commands when initialized,
-   * and ends when they are all no longer scheduled.
+   * Creates a new ProxyScheduleCommand that schedules the given commands when initialized, and ends
+   * when they are all no longer scheduled.
    *
    * @param toSchedule the commands to schedule
    */
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
index c6a2b4c..8d8d4d3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RamseteCommand.java
@@ -1,41 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.Timer;
 import java.util.function.BiConsumer;
 import java.util.function.Supplier;
 
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
  * A command that uses a RAMSETE controller ({@link RamseteController}) to follow a trajectory
  * {@link Trajectory} with a differential drive.
  *
- * <p>The command handles trajectory-following, PID calculations, and feedforwards internally.  This
+ * <p>The command handles trajectory-following, PID calculations, and feedforwards internally. This
  * is intended to be a more-or-less "complete solution" that can be used by teams without a great
  * deal of controls expertise.
  *
- * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard
- * PID functionality of a "smart" motor controller) may use the secondary constructor that omits
- * the PID and feedforward functionality, returning only the raw wheel speeds from the RAMSETE
- * controller.
+ * <p>Advanced teams seeking more flexibility (for example, those who wish to use the onboard PID
+ * functionality of a "smart" motor controller) may use the secondary constructor that omits the PID
+ * and feedforward functionality, returning only the raw wheel speeds from the RAMSETE controller.
  */
-@SuppressWarnings("PMD.TooManyFields")
 public class RamseteCommand extends CommandBase {
   private final Timer m_timer = new Timer();
   private final boolean m_usePID;
@@ -52,39 +46,38 @@
   private double m_prevTime;
 
   /**
-   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
-   * PID control and feedforward are handled internally, and outputs are scaled -12 to 12
-   * representing units of volts.
+   * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory. PID
+   * control and feedforward are handled internally, and outputs are scaled -12 to 12 representing
+   * units of volts.
    *
    * <p>Note: The controller will *not* set the outputVolts to zero upon completion of the path -
-   * this
-   * is left to the user, since it is not appropriate for paths with nonstationary endstates.
+   * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * @param trajectory      The trajectory to follow.
-   * @param pose            A function that supplies the robot pose - use one of
-   *                        the odometry classes to provide this.
-   * @param controller      The RAMSETE controller used to follow the trajectory.
-   * @param feedforward     The feedforward to use for the drive.
-   * @param kinematics      The kinematics for the robot drivetrain.
-   * @param wheelSpeeds     A function that supplies the speeds of the left and
-   *                        right sides of the robot drive.
-   * @param leftController  The PIDController for the left side of the robot drive.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param controller The RAMSETE controller used to follow the trajectory.
+   * @param feedforward The feedforward to use for the drive.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param wheelSpeeds A function that supplies the speeds of the left and right sides of the robot
+   *     drive.
+   * @param leftController The PIDController for the left side of the robot drive.
    * @param rightController The PIDController for the right side of the robot drive.
-   * @param outputVolts     A function that consumes the computed left and right
-   *                        outputs (in volts) for the robot drive.
-   * @param requirements    The subsystems to require.
+   * @param outputVolts A function that consumes the computed left and right outputs (in volts) for
+   *     the robot drive.
+   * @param requirements The subsystems to require.
    */
-  @SuppressWarnings("PMD.ExcessiveParameterList")
-  public RamseteCommand(Trajectory trajectory,
-                        Supplier<Pose2d> pose,
-                        RamseteController controller,
-                        SimpleMotorFeedforward feedforward,
-                        DifferentialDriveKinematics kinematics,
-                        Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
-                        PIDController leftController,
-                        PIDController rightController,
-                        BiConsumer<Double, Double> outputVolts,
-                        Subsystem... requirements) {
+  public RamseteCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      RamseteController controller,
+      SimpleMotorFeedforward feedforward,
+      DifferentialDriveKinematics kinematics,
+      Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds,
+      PIDController leftController,
+      PIDController rightController,
+      BiConsumer<Double, Double> outputVolts,
+      Subsystem... requirements) {
     m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
     m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
     m_follower = requireNonNullParam(controller, "controller", "RamseteCommand");
@@ -102,24 +95,24 @@
 
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the provided trajectory.
-   * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds
-   * from the RAMSETE controller, and will need to be converted into a usable form by the user.
+   * Performs no PID control and calculates no feedforwards; outputs are the raw wheel speeds from
+   * the RAMSETE controller, and will need to be converted into a usable form by the user.
    *
-   * @param trajectory            The trajectory to follow.
-   * @param pose                  A function that supplies the robot pose - use one of
-   *                              the odometry classes to provide this.
-   * @param follower              The RAMSETE follower used to follow the trajectory.
-   * @param kinematics            The kinematics for the robot drivetrain.
-   * @param outputMetersPerSecond A function that consumes the computed left and right
-   *                              wheel speeds.
-   * @param requirements          The subsystems to require.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param follower The RAMSETE follower used to follow the trajectory.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param outputMetersPerSecond A function that consumes the computed left and right wheel speeds.
+   * @param requirements The subsystems to require.
    */
-  public RamseteCommand(Trajectory trajectory,
-                        Supplier<Pose2d> pose,
-                        RamseteController follower,
-                        DifferentialDriveKinematics kinematics,
-                        BiConsumer<Double, Double> outputMetersPerSecond,
-                        Subsystem... requirements) {
+  public RamseteCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      RamseteController follower,
+      DifferentialDriveKinematics kinematics,
+      BiConsumer<Double, Double> outputMetersPerSecond,
+      Subsystem... requirements) {
     m_trajectory = requireNonNullParam(trajectory, "trajectory", "RamseteCommand");
     m_pose = requireNonNullParam(pose, "pose", "RamseteCommand");
     m_follower = requireNonNullParam(follower, "follower", "RamseteCommand");
@@ -138,13 +131,14 @@
 
   @Override
   public void initialize() {
-    m_prevTime = 0;
+    m_prevTime = -1;
     var initialState = m_trajectory.sample(0);
-    m_prevSpeeds = m_kinematics.toWheelSpeeds(
-        new ChassisSpeeds(initialState.velocityMetersPerSecond,
-            0,
-            initialState.curvatureRadPerMeter
-                * initialState.velocityMetersPerSecond));
+    m_prevSpeeds =
+        m_kinematics.toWheelSpeeds(
+            new ChassisSpeeds(
+                initialState.velocityMetersPerSecond,
+                0,
+                initialState.curvatureRadPerMeter * initialState.velocityMetersPerSecond));
     m_timer.reset();
     m_timer.start();
     if (m_usePID) {
@@ -158,8 +152,15 @@
     double curTime = m_timer.get();
     double dt = curTime - m_prevTime;
 
-    var targetWheelSpeeds = m_kinematics.toWheelSpeeds(
-        m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
+    if (m_prevTime < 0) {
+      m_output.accept(0.0, 0.0);
+      m_prevTime = curTime;
+      return;
+    }
+
+    var targetWheelSpeeds =
+        m_kinematics.toWheelSpeeds(
+            m_follower.calculate(m_pose.get(), m_trajectory.sample(curTime)));
 
     var leftSpeedSetpoint = targetWheelSpeeds.leftMetersPerSecond;
     var rightSpeedSetpoint = targetWheelSpeeds.rightMetersPerSecond;
@@ -169,34 +170,38 @@
 
     if (m_usePID) {
       double leftFeedforward =
-          m_feedforward.calculate(leftSpeedSetpoint,
-              (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
+          m_feedforward.calculate(
+              leftSpeedSetpoint, (leftSpeedSetpoint - m_prevSpeeds.leftMetersPerSecond) / dt);
 
       double rightFeedforward =
-          m_feedforward.calculate(rightSpeedSetpoint,
-              (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
+          m_feedforward.calculate(
+              rightSpeedSetpoint, (rightSpeedSetpoint - m_prevSpeeds.rightMetersPerSecond) / dt);
 
-      leftOutput = leftFeedforward
-          + m_leftController.calculate(m_speeds.get().leftMetersPerSecond,
-          leftSpeedSetpoint);
+      leftOutput =
+          leftFeedforward
+              + m_leftController.calculate(m_speeds.get().leftMetersPerSecond, leftSpeedSetpoint);
 
-      rightOutput = rightFeedforward
-          + m_rightController.calculate(m_speeds.get().rightMetersPerSecond,
-          rightSpeedSetpoint);
+      rightOutput =
+          rightFeedforward
+              + m_rightController.calculate(
+                  m_speeds.get().rightMetersPerSecond, rightSpeedSetpoint);
     } else {
       leftOutput = leftSpeedSetpoint;
       rightOutput = rightSpeedSetpoint;
     }
 
     m_output.accept(leftOutput, rightOutput);
-
-    m_prevTime = curTime;
     m_prevSpeeds = targetWheelSpeeds;
+    m_prevTime = curTime;
   }
 
   @Override
   public void end(boolean interrupted) {
     m_timer.stop();
+
+    if (interrupted) {
+      m_output.accept(0.0, 0.0);
+    }
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
index 9099be0..e1a7296 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/RunCommand.java
@@ -1,30 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.function.BooleanSupplier;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import java.util.function.BooleanSupplier;
+
 /**
- * A command that runs a Runnable continuously.  Has no end condition as-is;
- * either subclass it or use {@link Command#withTimeout(double)} or
- * {@link Command#withInterrupt(BooleanSupplier)} to give it one.  If you only wish
- * to execute a Runnable once, use {@link InstantCommand}.
+ * A command that runs a Runnable continuously. Has no end condition as-is; either subclass it or
+ * use {@link Command#withTimeout(double)} or {@link Command#withInterrupt(BooleanSupplier)} to give
+ * it one. If you only wish to execute a Runnable once, use {@link InstantCommand}.
  */
 public class RunCommand extends CommandBase {
   protected final Runnable m_toRun;
 
   /**
-   * Creates a new RunCommand.  The Runnable will be run continuously until the command
-   * ends.  Does not run when disabled.
+   * Creates a new RunCommand. The Runnable will be run continuously until the command ends. Does
+   * not run when disabled.
    *
-   * @param toRun        the Runnable to run
+   * @param toRun the Runnable to run
    * @param requirements the subsystems to require
    */
   public RunCommand(Runnable toRun, Subsystem... requirements) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
index 700925b..75b5774 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/ScheduleCommand.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
 import java.util.Set;
 
 /**
- * Schedules the given commands when this command is initialized.  Useful for forking off from
- * CommandGroups.  Note that if run from a CommandGroup, the group will not know about the status
- * of the scheduled commands, and will treat this command as finishing instantly.
+ * Schedules the given commands when this command is initialized. Useful for forking off from
+ * CommandGroups. Note that if run from a CommandGroup, the group will not know about the status of
+ * the scheduled commands, and will treat this command as finishing instantly.
  */
 public class ScheduleCommand extends CommandBase {
   private final Set<Command> m_toSchedule;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
index 92fe07f..a5c8859 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SelectCommand.java
@@ -1,24 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Map;
-import java.util.function.Supplier;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 import static edu.wpi.first.wpilibj2.command.CommandGroupBase.requireUngrouped;
 
+import java.util.Map;
+import java.util.function.Supplier;
+
 /**
  * Runs one of a selection of commands, either using a selector and a key to command mapping, or a
- * supplier that returns the command directly at runtime.  Does not actually schedule the selected
+ * supplier that returns the command directly at runtime. Does not actually schedule the selected
  * command - rather, the command is run through this command; this ensures that the command will
- * behave as expected if used as part of a CommandGroup.  Requires the requirements of all included
- * commands, again to ensure proper functioning when used in a CommandGroup.  If this is undesired,
+ * behave as expected if used as part of a CommandGroup. Requires the requirements of all included
+ * commands, again to ensure proper functioning when used in a CommandGroup. If this is undesired,
  * consider using {@link ScheduleCommand}.
  *
  * <p>As this command contains multiple component commands within it, it is technically a command
@@ -42,7 +39,7 @@
   public SelectCommand(Map<Object, Command> commands, Supplier<Object> selector) {
     requireUngrouped(commands.values());
 
-    CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[]{}));
+    CommandGroupBase.registerGroupedCommands(commands.values().toArray(new Command[] {}));
 
     m_commands = requireNonNullParam(commands, "commands", "SelectCommand");
     m_selector = requireNonNullParam(selector, "selector", "SelectCommand");
@@ -69,8 +66,9 @@
   public void initialize() {
     if (m_selector != null) {
       if (!m_commands.keySet().contains(m_selector.get())) {
-        m_selectedCommand = new PrintCommand(
-            "SelectCommand selector value does not correspond to" + " any command!");
+        m_selectedCommand =
+            new PrintCommand(
+                "SelectCommand selector value does not correspond to" + " any command!");
         return;
       }
       m_selectedCommand = m_commands.get(m_selector.get());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
index 0d01f11..38dff8d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
@@ -21,8 +18,8 @@
   private boolean m_runWhenDisabled = true;
 
   /**
-   * Creates a new SequentialCommandGroup.  The given commands will be run sequentially, with
-   * the CommandGroup finishing when the last command finishes.
+   * Creates a new SequentialCommandGroup. The given commands will be run sequentially, with the
+   * CommandGroup finishing when the last command finishes.
    *
    * @param commands the commands to include in this group.
    */
@@ -77,9 +74,10 @@
 
   @Override
   public void end(boolean interrupted) {
-    if (interrupted && !m_commands.isEmpty() && m_currentCommandIndex > -1
-        && m_currentCommandIndex < m_commands.size()
-    ) {
+    if (interrupted
+        && !m_commands.isEmpty()
+        && m_currentCommandIndex > -1
+        && m_currentCommandIndex < m_commands.size()) {
       m_commands.get(m_currentCommandIndex).end(true);
     }
     m_currentCommandIndex = -1;
@@ -94,4 +92,28 @@
   public boolean runsWhenDisabled() {
     return m_runWhenDisabled;
   }
+
+  @Override
+  public SequentialCommandGroup beforeStarting(Command before) {
+    // store all the commands
+    var commands = new ArrayList<Command>();
+    commands.add(before);
+    commands.addAll(m_commands);
+
+    // reset current state
+    commands.forEach(CommandGroupBase::clearGroupedCommand);
+    m_commands.clear();
+    m_requirements.clear();
+    m_runWhenDisabled = true;
+
+    // add them back
+    addCommands(commands.toArray(Command[]::new));
+    return this;
+  }
+
+  @Override
+  public SequentialCommandGroup andThen(Command... next) {
+    addCommands(next);
+    return this;
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
index 9029135..05e46cb 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/StartEndCommand.java
@@ -1,32 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.function.BooleanSupplier;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 /**
  * A command that runs a given runnable when it is initialized, and another runnable when it ends.
- * Useful for running and then stopping a motor, or extending and then retracting a solenoid.
- * Has no end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or
- * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ * Useful for running and then stopping a motor, or extending and then retracting a solenoid. Has no
+ * end condition as-is; either subclass it or use {@link Command#withTimeout(double)} or {@link
+ * Command#withInterrupt(java.util.function.BooleanSupplier)} to give it one.
  */
 public class StartEndCommand extends CommandBase {
   protected final Runnable m_onInit;
   protected final Runnable m_onEnd;
 
   /**
-   * Creates a new StartEndCommand.  Will run the given runnables when the command starts and when
-   * it ends.
+   * Creates a new StartEndCommand. Will run the given runnables when the command starts and when it
+   * ends.
    *
-   * @param onInit       the Runnable to run on command init
-   * @param onEnd        the Runnable to run on command end
+   * @param onInit the Runnable to run on command init
+   * @param onEnd the Runnable to run on command end
    * @param requirements the subsystems required by this command
    */
   public StartEndCommand(Runnable onInit, Runnable onEnd, Subsystem... requirements) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
index e693d87..4e28b16 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/Subsystem.java
@@ -1,52 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
 /**
- * A robot subsystem.  Subsystems are the basic unit of robot organization in the Command-based
+ * A robot subsystem. Subsystems are the basic unit of robot organization in the Command-based
  * framework; they encapsulate low-level hardware objects (motor controllers, sensors, etc) and
- * provide methods through which they can be used by {@link Command}s.  Subsystems are used by the
+ * provide methods through which they can be used by {@link Command}s. Subsystems are used by the
  * {@link CommandScheduler}'s resource management system to ensure multiple robot actions are not
- * "fighting" over the same hardware; Commands that use a subsystem should include that subsystem
- * in their {@link Command#getRequirements()} method, and resources used within a subsystem should
+ * "fighting" over the same hardware; Commands that use a subsystem should include that subsystem in
+ * their {@link Command#getRequirements()} method, and resources used within a subsystem should
  * generally remain encapsulated and not be shared by other parts of the robot.
  *
- * <p>Subsystems must be registered with the scheduler with the
- * {@link CommandScheduler#registerSubsystem(Subsystem...)} method in order for the
- * {@link Subsystem#periodic()} method to be called.  It is recommended that this method be called
- * from the constructor of users' Subsystem implementations.  The {@link SubsystemBase}
- * class offers a simple base for user implementations that handles this.
+ * <p>Subsystems must be registered with the scheduler with the {@link
+ * CommandScheduler#registerSubsystem(Subsystem...)} method in order for the {@link
+ * Subsystem#periodic()} method to be called. It is recommended that this method be called from the
+ * constructor of users' Subsystem implementations. The {@link SubsystemBase} class offers a simple
+ * base for user implementations that handles this.
  */
 public interface Subsystem {
-
   /**
-   * This method is called periodically by the {@link CommandScheduler}.  Useful for updating
-   * subsystem-specific state that you don't want to offload to a {@link Command}.  Teams should
-   * try to be consistent within their own codebases about which responsibilities will be handled
-   * by Commands, and which will be handled here.
+   * This method is called periodically by the {@link CommandScheduler}. Useful for updating
+   * subsystem-specific state that you don't want to offload to a {@link Command}. Teams should try
+   * to be consistent within their own codebases about which responsibilities will be handled by
+   * Commands, and which will be handled here.
    */
-  default void periodic() {
-  }
+  default void periodic() {}
 
   /**
-   * This method is called periodically by the {@link CommandScheduler}.  Useful for updating
+   * This method is called periodically by the {@link CommandScheduler}. Useful for updating
    * subsystem-specific state that needs to be maintained for simulations, such as for updating
    * {@link edu.wpi.first.wpilibj.simulation} classes and setting simulated sensor readings.
    */
-  default void simulationPeriodic() {
-  }
+  default void simulationPeriodic() {}
 
   /**
-   * Sets the default {@link Command} of the subsystem.  The default command will be
-   * automatically scheduled when no other commands are scheduled that require the subsystem.
-   * Default commands should generally not end on their own, i.e. their {@link Command#isFinished()}
-   * method should always return false.  Will automatically register this subsystem with the
-   * {@link CommandScheduler}.
+   * Sets the default {@link Command} of the subsystem. The default command will be automatically
+   * scheduled when no other commands are scheduled that require the subsystem. Default commands
+   * should generally not end on their own, i.e. their {@link Command#isFinished()} method should
+   * always return false. Will automatically register this subsystem with the {@link
+   * CommandScheduler}.
    *
    * @param defaultCommand the default command to associate with this subsystem
    */
@@ -55,8 +49,8 @@
   }
 
   /**
-   * Gets the default command for this subsystem.  Returns null if no default command is
-   * currently associated with the subsystem.
+   * Gets the default command for this subsystem. Returns null if no default command is currently
+   * associated with the subsystem.
    *
    * @return the default command associated with this subsystem
    */
@@ -65,7 +59,7 @@
   }
 
   /**
-   * Returns the command currently running on this subsystem.  Returns null if no command is
+   * Returns the command currently running on this subsystem. Returns null if no command is
    * currently scheduled that requires this subsystem.
    *
    * @return the scheduled command currently requiring this subsystem
@@ -75,8 +69,8 @@
   }
 
   /**
-   * Registers this subsystem with the {@link CommandScheduler}, allowing its
-   * {@link Subsystem#periodic()} method to be called when the scheduler runs.
+   * Registers this subsystem with the {@link CommandScheduler}, allowing its {@link
+   * Subsystem#periodic()} method to be called when the scheduler runs.
    */
   default void register() {
     CommandScheduler.getInstance().registerSubsystem(this);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
index e22a5c4..c685803 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SubsystemBase.java
@@ -1,25 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * A base for subsystems that handles registration in the constructor, and provides a more intuitive
  * method for setting the default command.
  */
 public abstract class SubsystemBase implements Subsystem, Sendable {
-
-  /**
-   * Constructor.
-   */
+  /** Constructor. */
   public SubsystemBase() {
     String name = this.getClass().getSimpleName();
     name = name.substring(name.lastIndexOf('.') + 1);
@@ -32,7 +26,6 @@
    *
    * @return Name
    */
-  @Override
   public String getName() {
     return SendableRegistry.getName(this);
   }
@@ -42,7 +35,6 @@
    *
    * @param name name
    */
-  @Override
   public void setName(String name) {
     SendableRegistry.setName(this, name);
   }
@@ -52,7 +44,6 @@
    *
    * @return Subsystem name
    */
-  @Override
   public String getSubsystem() {
     return SendableRegistry.getSubsystem(this);
   }
@@ -62,14 +53,12 @@
    *
    * @param subsystem subsystem name
    */
-  @Override
   public void setSubsystem(String subsystem) {
     SendableRegistry.setSubsystem(this, subsystem);
   }
 
   /**
-   * Associates a {@link Sendable} with this Subsystem.
-   * Also update the child's name.
+   * Associates a {@link Sendable} with this Subsystem. Also update the child's name.
    *
    * @param name name to give child
    * @param child sendable
@@ -83,10 +72,14 @@
     builder.setSmartDashboardType("Subsystem");
 
     builder.addBooleanProperty(".hasDefault", () -> getDefaultCommand() != null, null);
-    builder.addStringProperty(".default",
-        () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none", null);
+    builder.addStringProperty(
+        ".default",
+        () -> getDefaultCommand() != null ? getDefaultCommand().getName() : "none",
+        null);
     builder.addBooleanProperty(".hasCommand", () -> getCurrentCommand() != null, null);
-    builder.addStringProperty(".command",
-        () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none", null);
+    builder.addStringProperty(
+        ".command",
+        () -> getCurrentCommand() != null ? getCurrentCommand().getName() : "none",
+        null);
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
index c0303b9..95b42ee 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommand.java
@@ -1,40 +1,34 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.HolonomicDriveController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.Timer;
 import java.util.function.Consumer;
 import java.util.function.Supplier;
 
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.controller.HolonomicDriveController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
- * A command that uses two PID controllers ({@link PIDController}) and a
- * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
- * {@link Trajectory} with a swerve drive.
+ * A command that uses two PID controllers ({@link PIDController}) and a ProfiledPIDController
+ * ({@link ProfiledPIDController}) to follow a trajectory {@link Trajectory} with a swerve drive.
  *
- * <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState})
- * in an array. The desired wheel and module rotation velocities should be taken
- * from those and used in velocity PIDs.
+ * <p>This command outputs the raw desired Swerve Module States ({@link SwerveModuleState}) in an
+ * array. The desired wheel and module rotation velocities should be taken from those and used in
+ * velocity PIDs.
  *
- * <p>The robot angle controller does not follow the angle given by
- * the trajectory but rather goes to the angle given in the final state of the trajectory.
+ * <p>The robot angle controller does not follow the angle given by the trajectory but rather goes
+ * to the angle given in the final state of the trajectory.
  */
-
 @SuppressWarnings("MemberName")
 public class SwerveControllerCommand extends CommandBase {
   private final Timer m_timer = new Timer();
@@ -53,50 +47,44 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
    * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * @param trajectory         The trajectory to follow.
-   * @param pose               A function that supplies the robot pose - use one of
-   *                           the odometry classes to provide this.
-   * @param kinematics         The kinematics for the robot drivetrain.
-   * @param xController        The Trajectory Tracker PID controller
-   *                           for the robot's x position.
-   * @param yController        The Trajectory Tracker PID controller
-   *                           for the robot's y position.
-   * @param thetaController    The Trajectory Tracker PID controller
-   *                           for angle for the robot.
-   * @param desiredRotation    The angle that the drivetrain should be facing. This
-   *                           is sampled at each time step.
-   * @param outputModuleStates The raw output module states from the
-   *                           position controllers.
-   * @param requirements       The subsystems to require.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param desiredRotation The angle that the drivetrain should be facing. This is sampled at each
+   *     time step.
+   * @param outputModuleStates The raw output module states from the position controllers.
+   * @param requirements The subsystems to require.
    */
   @SuppressWarnings("ParameterName")
-  public SwerveControllerCommand(Trajectory trajectory,
-                                 Supplier<Pose2d> pose,
-                                 SwerveDriveKinematics kinematics,
-                                 PIDController xController,
-                                 PIDController yController,
-                                 ProfiledPIDController thetaController,
-                                 Supplier<Rotation2d> desiredRotation,
-                                 Consumer<SwerveModuleState[]> outputModuleStates,
-                                 Subsystem... requirements) {
+  public SwerveControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SwerveDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      Supplier<Rotation2d> desiredRotation,
+      Consumer<SwerveModuleState[]> outputModuleStates,
+      Subsystem... requirements) {
     m_trajectory = requireNonNullParam(trajectory, "trajectory", "SwerveControllerCommand");
     m_pose = requireNonNullParam(pose, "pose", "SwerveControllerCommand");
     m_kinematics = requireNonNullParam(kinematics, "kinematics", "SwerveControllerCommand");
 
-    m_controller = new HolonomicDriveController(
-        requireNonNullParam(xController,
-            "xController", "SwerveControllerCommand"),
-        requireNonNullParam(yController,
-            "xController", "SwerveControllerCommand"),
-        requireNonNullParam(thetaController,
-            "thetaController", "SwerveControllerCommand")
-    );
+    m_controller =
+        new HolonomicDriveController(
+            requireNonNullParam(xController, "xController", "SwerveControllerCommand"),
+            requireNonNullParam(yController, "xController", "SwerveControllerCommand"),
+            requireNonNullParam(thetaController, "thetaController", "SwerveControllerCommand"));
 
-    m_outputModuleStates = requireNonNullParam(outputModuleStates,
-        "frontLeftOutput", "SwerveControllerCommand");
+    m_outputModuleStates =
+        requireNonNullParam(outputModuleStates, "frontLeftOutput", "SwerveControllerCommand");
 
-    m_desiredRotation = requireNonNullParam(desiredRotation, "desiredRotation",
-        "SwerveControllerCommand");
+    m_desiredRotation =
+        requireNonNullParam(desiredRotation, "desiredRotation", "SwerveControllerCommand");
 
     addRequirements(requirements);
   }
@@ -109,38 +97,42 @@
    * <p>Note: The controllers will *not* set the outputVolts to zero upon completion of the path-
    * this is left to the user, since it is not appropriate for paths with nonstationary endstates.
    *
-   * <p>Note 2: The final rotation of the robot will be set to the rotation of
-   * the final pose in the trajectory. The robot will not follow the rotations
-   * from the poses at each timestep. If alternate rotation behavior is desired,
-   * the other constructor with a supplier for rotation should be used.
+   * <p>Note 2: The final rotation of the robot will be set to the rotation of the final pose in the
+   * trajectory. The robot will not follow the rotations from the poses at each timestep. If
+   * alternate rotation behavior is desired, the other constructor with a supplier for rotation
+   * should be used.
    *
-   * @param trajectory         The trajectory to follow.
-   * @param pose               A function that supplies the robot pose - use one of
-   *                           the odometry classes to provide this.
-   * @param kinematics         The kinematics for the robot drivetrain.
-   * @param xController        The Trajectory Tracker PID controller
-   *                           for the robot's x position.
-   * @param yController        The Trajectory Tracker PID controller
-   *                           for the robot's y position.
-   * @param thetaController    The Trajectory Tracker PID controller
-   *                           for angle for the robot.
-   * @param outputModuleStates The raw output module states from the
-   *                           position controllers.
-   * @param requirements       The subsystems to require.
+   * @param trajectory The trajectory to follow.
+   * @param pose A function that supplies the robot pose - use one of the odometry classes to
+   *     provide this.
+   * @param kinematics The kinematics for the robot drivetrain.
+   * @param xController The Trajectory Tracker PID controller for the robot's x position.
+   * @param yController The Trajectory Tracker PID controller for the robot's y position.
+   * @param thetaController The Trajectory Tracker PID controller for angle for the robot.
+   * @param outputModuleStates The raw output module states from the position controllers.
+   * @param requirements The subsystems to require.
    */
   @SuppressWarnings("ParameterName")
-  public SwerveControllerCommand(Trajectory trajectory,
-                                 Supplier<Pose2d> pose,
-                                 SwerveDriveKinematics kinematics,
-                                 PIDController xController,
-                                 PIDController yController,
-                                 ProfiledPIDController thetaController,
-                                 Consumer<SwerveModuleState[]> outputModuleStates,
-                                 Subsystem... requirements) {
-    this(trajectory, pose, kinematics, xController, yController, thetaController,
-        () -> trajectory.getStates().get(trajectory.getStates().size() - 1)
-            .poseMeters.getRotation(),
-        outputModuleStates, requirements);
+  public SwerveControllerCommand(
+      Trajectory trajectory,
+      Supplier<Pose2d> pose,
+      SwerveDriveKinematics kinematics,
+      PIDController xController,
+      PIDController yController,
+      ProfiledPIDController thetaController,
+      Consumer<SwerveModuleState[]> outputModuleStates,
+      Subsystem... requirements) {
+    this(
+        trajectory,
+        pose,
+        kinematics,
+        xController,
+        yController,
+        thetaController,
+        () ->
+            trajectory.getStates().get(trajectory.getStates().size() - 1).poseMeters.getRotation(),
+        outputModuleStates,
+        requirements);
   }
 
   @Override
@@ -155,8 +147,8 @@
     double curTime = m_timer.get();
     var desiredState = m_trajectory.sample(curTime);
 
-    var targetChassisSpeeds = m_controller.calculate(m_pose.get(), desiredState,
-        m_desiredRotation.get());
+    var targetChassisSpeeds =
+        m_controller.calculate(m_pose.get(), desiredState, m_desiredRotation.get());
     var targetModuleStates = m_kinematics.toSwerveModuleStates(targetChassisSpeeds);
 
     m_outputModuleStates.accept(targetModuleStates);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
index 0e645c6..674328e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileCommand.java
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.function.Consumer;
-
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-import static edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
+import static edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Timer;
+import java.util.function.Consumer;
+
 /**
- * A command that runs a {@link TrapezoidProfile}.  Useful for smoothly controlling mechanism
- * motion.
+ * A command that runs a {@link TrapezoidProfile}. Useful for smoothly controlling mechanism motion.
  */
 public class TrapezoidProfileCommand extends CommandBase {
   private final TrapezoidProfile m_profile;
@@ -29,13 +24,12 @@
    * Creates a new TrapezoidProfileCommand that will execute the given {@link TrapezoidProfile}.
    * Output will be piped to the provided consumer function.
    *
-   * @param profile      The motion profile to execute.
-   * @param output       The consumer for the profile output.
+   * @param profile The motion profile to execute.
+   * @param output The consumer for the profile output.
    * @param requirements The subsystems required by this command.
    */
-  public TrapezoidProfileCommand(TrapezoidProfile profile,
-                                 Consumer<State> output,
-                                 Subsystem... requirements) {
+  public TrapezoidProfileCommand(
+      TrapezoidProfile profile, Consumer<State> output, Subsystem... requirements) {
     m_profile = requireNonNullParam(profile, "profile", "TrapezoidProfileCommand");
     m_output = requireNonNullParam(output, "output", "TrapezoidProfileCommand");
     addRequirements(requirements);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
index 05e6386..0a6c658 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/TrapezoidProfileSubsystem.java
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
 /**
- * A subsystem that generates and runs trapezoidal motion profiles automatically.  The user
- * specifies how to use the current state of the motion profile by overriding the `useState` method.
+ * A subsystem that generates and runs trapezoidal motion profiles automatically. The user specifies
+ * how to use the current state of the motion profile by overriding the `useState` method.
  */
 public abstract class TrapezoidProfileSubsystem extends SubsystemBase {
   private final double m_period;
@@ -27,14 +24,13 @@
   /**
    * Creates a new TrapezoidProfileSubsystem.
    *
-   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
-   * @param initialPosition The initial position of the controlled mechanism when the subsystem
-   *                        is constructed.
-   * @param period          The period of the main robot loop, in seconds.
+   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem is
+   *     constructed.
+   * @param period The period of the main robot loop, in seconds.
    */
-  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
-                                   double initialPosition,
-                                   double period) {
+  public TrapezoidProfileSubsystem(
+      TrapezoidProfile.Constraints constraints, double initialPosition, double period) {
     m_constraints = requireNonNullParam(constraints, "constraints", "TrapezoidProfileSubsystem");
     m_state = new TrapezoidProfile.State(initialPosition, 0);
     setGoal(initialPosition);
@@ -44,19 +40,19 @@
   /**
    * Creates a new TrapezoidProfileSubsystem.
    *
-   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
-   * @param initialPosition The initial position of the controlled mechanism when the subsystem
-   *                        is constructed.
+   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
+   * @param initialPosition The initial position of the controlled mechanism when the subsystem is
+   *     constructed.
    */
-  public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints,
-                                   double initialPosition) {
+  public TrapezoidProfileSubsystem(
+      TrapezoidProfile.Constraints constraints, double initialPosition) {
     this(constraints, initialPosition, 0.02);
   }
 
   /**
    * Creates a new TrapezoidProfileSubsystem.
    *
-   * @param constraints     The constraints (maximum velocity and acceleration) for the profiles.
+   * @param constraints The constraints (maximum velocity and acceleration) for the profiles.
    */
   public TrapezoidProfileSubsystem(TrapezoidProfile.Constraints constraints) {
     this(constraints, 0, 0.02);
@@ -81,7 +77,7 @@
   }
 
   /**
-   * Sets the goal state for the subsystem.  Goal velocity assumed to be zero.
+   * Sets the goal state for the subsystem. Goal velocity assumed to be zero.
    *
    * @param goal The goal position for the subsystem's motion profile.
    */
@@ -89,16 +85,12 @@
     setGoal(new TrapezoidProfile.State(goal, 0));
   }
 
-  /**
-   * Enable the TrapezoidProfileSubsystem's output.
-   */
+  /** Enable the TrapezoidProfileSubsystem's output. */
   public void enable() {
     m_enabled = true;
   }
 
-  /**
-   * Disable the TrapezoidProfileSubsystem's output.
-   */
+  /** Disable the TrapezoidProfileSubsystem's output. */
   public void disable() {
     m_enabled = false;
   }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
index c53fb42..b285669 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitCommand.java
@@ -1,25 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
 /**
- * A command that does nothing but takes a specified amount of time to finish.  Useful for
- * CommandGroups.  Can also be subclassed to make a command with an internal timer.
+ * A command that does nothing but takes a specified amount of time to finish. Useful for
+ * CommandGroups. Can also be subclassed to make a command with an internal timer.
  */
 public class WaitCommand extends CommandBase {
   protected Timer m_timer = new Timer();
   private final double m_duration;
 
   /**
-   * Creates a new WaitCommand.  This command will do nothing, and end after the specified duration.
+   * Creates a new WaitCommand. This command will do nothing, and end after the specified duration.
    *
    * @param seconds the time to wait, in seconds
    */
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
index 5c55fff..9228362 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/WaitUntilCommand.java
@@ -1,21 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-
-import java.util.function.BooleanSupplier;
-
-import edu.wpi.first.wpilibj.Timer;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.wpilibj.Timer;
+import java.util.function.BooleanSupplier;
+
 /**
- * A command that does nothing but ends after a specified match time or condition.  Useful for
+ * A command that does nothing but ends after a specified match time or condition. Useful for
  * CommandGroups.
  */
 public class WaitUntilCommand extends CommandBase {
@@ -33,9 +28,9 @@
   /**
    * Creates a new WaitUntilCommand that ends after a given match time.
    *
-   * <p>NOTE: The match timer used for this command is UNOFFICIAL.  Using this command does NOT
+   * <p>NOTE: The match timer used for this command is UNOFFICIAL. Using this command does NOT
    * guarantee that the time at which the action is performed will be judged to be legal by the
-   * referees.  When in doubt, add a safety factor or time the action manually.
+   * referees. When in doubt, add a safety factor or time the action manually.
    *
    * @param time the match time after which to end, in seconds
    */
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
index d672a38..0f8594d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Button.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import java.util.function.BooleanSupplier;
-
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Subsystem;
+import java.util.function.BooleanSupplier;
 
 /**
  * This class provides an easy way to link commands to OI inputs.
  *
- * <p>It is very easy to link a button to a command. For instance, you could link the trigger
- * button of a joystick to a "score" command.
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger button
+ * of a joystick to a "score" command.
  *
  * <p>This class represents a subclass of Trigger that is specifically aimed at buttons on an
  * operator interface as a common use case of the more generalized Trigger objects. This is a simple
@@ -27,8 +23,7 @@
    * Default constructor; creates a button that is never pressed (unless {@link Button#get()} is
    * overridden).
    */
-  public Button() {
-  }
+  public Button() {}
 
   /**
    * Creates a new button with the given condition determining whether it is pressed.
@@ -42,7 +37,7 @@
   /**
    * Starts the given command whenever the button is newly pressed.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this button, so calls can be chained
    */
@@ -66,7 +61,7 @@
   /**
    * Runs the given runnable whenever the button is newly pressed.
    *
-   * @param toRun        the runnable to run
+   * @param toRun the runnable to run
    * @param requirements the required subsystems
    * @return this button, so calls can be chained
    */
@@ -78,10 +73,10 @@
   /**
    * Constantly starts the given command while the button is held.
    *
-   * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
-   * be canceled when the button is released.
+   * <p>{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
+   * will be canceled when the button is released.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this button, so calls can be chained
    */
@@ -93,8 +88,8 @@
   /**
    * Constantly starts the given command while the button is held.
    *
-   * {@link Command#schedule(boolean)} will be called repeatedly while the button is held, and will
-   * be canceled when the button is released.  The command is set to be interruptible.
+   * <p>{@link Command#schedule(boolean)} will be called repeatedly while the button is held, and
+   * will be canceled when the button is released. The command is set to be interruptible.
    *
    * @param command the command to start
    * @return this button, so calls can be chained
@@ -107,7 +102,7 @@
   /**
    * Constantly runs the given runnable while the button is held.
    *
-   * @param toRun        the runnable to run
+   * @param toRun the runnable to run
    * @param requirements the required subsystems
    * @return this button, so calls can be chained
    */
@@ -120,7 +115,7 @@
    * Starts the given command when the button is first pressed, and cancels it when it is released,
    * but does not start it again if it ends or is otherwise interrupted.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this button, so calls can be chained
    */
@@ -131,7 +126,7 @@
 
   /**
    * Starts the given command when the button is first pressed, and cancels it when it is released,
-   * but does not start it again if it ends or is otherwise interrupted.  The command is set to be
+   * but does not start it again if it ends or is otherwise interrupted. The command is set to be
    * interruptible.
    *
    * @param command the command to start
@@ -142,11 +137,10 @@
     return this;
   }
 
-
   /**
    * Starts the command when the button is released.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this button, so calls can be chained
    */
@@ -156,7 +150,7 @@
   }
 
   /**
-   * Starts the command when the button is released.  The command is set to be interruptible.
+   * Starts the command when the button is released. The command is set to be interruptible.
    *
    * @param command the command to start
    * @return this button, so calls can be chained
@@ -169,7 +163,7 @@
   /**
    * Runs the given runnable when the button is released.
    *
-   * @param toRun        the runnable to run
+   * @param toRun the runnable to run
    * @param requirements the required subsystems
    * @return this button, so calls can be chained
    */
@@ -181,8 +175,9 @@
   /**
    * Toggles the command whenever the button is pressed (on then off then on).
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
+   * @return this button, so calls can be chained
    */
   public Button toggleWhenPressed(final Command command, boolean interruptible) {
     toggleWhenActive(command, interruptible);
@@ -190,8 +185,8 @@
   }
 
   /**
-   * Toggles the command whenever the button is pressed (on then off then on).  The command is set
-   * to be interruptible.
+   * Toggles the command whenever the button is pressed (on then off then on). The command is set to
+   * be interruptible.
    *
    * @param command the command to start
    * @return this button, so calls can be chained
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
index 3f74f48..e02f078 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/InternalButton.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
@@ -15,9 +12,7 @@
   private boolean m_pressed;
   private boolean m_inverted;
 
-  /**
-   * Creates an InternalButton that is not inverted.
-   */
+  /** Creates an InternalButton that is not inverted. */
   public InternalButton() {
     this(false);
   }
@@ -26,7 +21,7 @@
    * Creates an InternalButton which is inverted depending on the input.
    *
    * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
-   *                 when set to false.
+   *     when set to false.
    */
   public InternalButton(boolean inverted) {
     m_pressed = m_inverted = inverted;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
index 918bb6a..c42ab4f 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/JoystickButton.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import edu.wpi.first.wpilibj.GenericHID;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
-/**
- * A {@link Button} that gets its state from a {@link GenericHID}.
- */
+import edu.wpi.first.wpilibj.GenericHID;
+
+/** A {@link Button} that gets its state from a {@link GenericHID}. */
 public class JoystickButton extends Button {
   private final GenericHID m_joystick;
   private final int m_buttonNumber;
@@ -21,8 +16,7 @@
   /**
    * Creates a joystick button for triggering commands.
    *
-   * @param joystick     The GenericHID object that has the button (e.g. Joystick, KinectStick,
-   *                     etc)
+   * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, etc)
    * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
    */
   public JoystickButton(GenericHID joystick, int buttonNumber) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java
index 4cb1859..ec4f819 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/NetworkButton.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 
-/**
- * A {@link Button} that uses a {@link NetworkTable} boolean field.
- */
+/** A {@link Button} that uses a {@link NetworkTable} boolean field. */
 public class NetworkButton extends Button {
   private final NetworkTableEntry m_entry;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
index 823b756..bd0c585 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/POVButton.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import edu.wpi.first.wpilibj.GenericHID;
-
 import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
-/**
- * A {@link Button} that gets its state from a POV on a {@link GenericHID}.
- */
+import edu.wpi.first.wpilibj.GenericHID;
+
+/** A {@link Button} that gets its state from a POV on a {@link GenericHID}. */
 public class POVButton extends Button {
   private final GenericHID m_joystick;
   private final int m_angle;
@@ -35,8 +30,7 @@
   }
 
   /**
-   * Creates a POV button for triggering commands.
-   * By default, acts on POV 0
+   * Creates a POV button for triggering commands. By default, acts on POV 0
    *
    * @param joystick The GenericHID object that has the POV
    * @param angle The desired angle (e.g. 90, 270)
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
index e5e1763..029d2fd 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/java/edu/wpi/first/wpilibj2/command/button/Trigger.java
@@ -1,31 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import java.util.function.BooleanSupplier;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
+import edu.wpi.first.wpilibj.Debouncer;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.Subsystem;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import java.util.function.BooleanSupplier;
 
 /**
  * This class provides an easy way to link commands to inputs.
  *
- * <p>It is very easy to link a button to a command. For instance, you could link the trigger
- * button of a joystick to a "score" command.
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger button
+ * of a joystick to a "score" command.
  *
- * <p>It is encouraged that teams write a subclass of Trigger if they want to have something
- * unusual (for instance, if they want to react to the user holding a button while the robot is
- * reading a certain sensor input). For this, they only have to write the {@link Trigger#get()}
- * method to get the full functionality of the Trigger class.
+ * <p>It is encouraged that teams write a subclass of Trigger if they want to have something unusual
+ * (for instance, if they want to react to the user holding a button while the robot is reading a
+ * certain sensor input). For this, they only have to write the {@link Trigger#get()} method to get
+ * the full functionality of the Trigger class.
  */
 public class Trigger {
   private final BooleanSupplier m_isActive;
@@ -40,7 +37,7 @@
   }
 
   /**
-   * Creates a new trigger that is always inactive.  Useful only as a no-arg constructor for
+   * Creates a new trigger that is always inactive. Useful only as a no-arg constructor for
    * subclasses that will be overriding {@link Trigger#get()} anyway.
    */
   public Trigger() {
@@ -61,33 +58,35 @@
   /**
    * Starts the given command whenever the trigger just becomes active.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this trigger, so calls can be chained
    */
   public Trigger whenActive(final Command command, boolean interruptible) {
     requireNonNullParam(command, "command", "whenActive");
 
-    CommandScheduler.getInstance().addButton(new Runnable() {
-      private boolean m_pressedLast = get();
+    CommandScheduler.getInstance()
+        .addButton(
+            new Runnable() {
+              private boolean m_pressedLast = get();
 
-      @Override
-      public void run() {
-        boolean pressed = get();
+              @Override
+              public void run() {
+                boolean pressed = get();
 
-        if (!m_pressedLast && pressed) {
-          command.schedule(interruptible);
-        }
+                if (!m_pressedLast && pressed) {
+                  command.schedule(interruptible);
+                }
 
-        m_pressedLast = pressed;
-      }
-    });
+                m_pressedLast = pressed;
+              }
+            });
 
     return this;
   }
 
   /**
-   * Starts the given command whenever the trigger just becomes active.  The command is set to be
+   * Starts the given command whenever the trigger just becomes active. The command is set to be
    * interruptible.
    *
    * @param command the command to start
@@ -100,7 +99,7 @@
   /**
    * Runs the given runnable whenever the trigger just becomes active.
    *
-   * @param toRun        the runnable to run
+   * @param toRun the runnable to run
    * @param requirements the required subsystems
    * @return this trigger, so calls can be chained
    */
@@ -111,40 +110,42 @@
   /**
    * Constantly starts the given command while the button is held.
    *
-   * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
+   * <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
    * will be canceled when the trigger becomes inactive.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this trigger, so calls can be chained
    */
   public Trigger whileActiveContinuous(final Command command, boolean interruptible) {
     requireNonNullParam(command, "command", "whileActiveContinuous");
 
-    CommandScheduler.getInstance().addButton(new Runnable() {
-      private boolean m_pressedLast = get();
+    CommandScheduler.getInstance()
+        .addButton(
+            new Runnable() {
+              private boolean m_pressedLast = get();
 
-      @Override
-      public void run() {
-        boolean pressed = get();
+              @Override
+              public void run() {
+                boolean pressed = get();
 
-        if (pressed) {
-          command.schedule(interruptible);
-        } else if (m_pressedLast) {
-          command.cancel();
-        }
+                if (pressed) {
+                  command.schedule(interruptible);
+                } else if (m_pressedLast) {
+                  command.cancel();
+                }
 
-        m_pressedLast = pressed;
-      }
-    });
+                m_pressedLast = pressed;
+              }
+            });
     return this;
   }
 
   /**
    * Constantly starts the given command while the button is held.
    *
-   * {@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
-   * will be canceled when the trigger becomes inactive.  The command is set to be interruptible.
+   * <p>{@link Command#schedule(boolean)} will be called repeatedly while the trigger is active, and
+   * will be canceled when the trigger becomes inactive. The command is set to be interruptible.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -156,7 +157,7 @@
   /**
    * Constantly runs the given runnable while the button is held.
    *
-   * @param toRun        the runnable to run
+   * @param toRun the runnable to run
    * @param requirements the required subsystems
    * @return this trigger, so calls can be chained
    */
@@ -168,35 +169,37 @@
    * Starts the given command when the trigger initially becomes active, and ends it when it becomes
    * inactive, but does not re-start it in-between.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this trigger, so calls can be chained
    */
   public Trigger whileActiveOnce(final Command command, boolean interruptible) {
     requireNonNullParam(command, "command", "whileActiveOnce");
 
-    CommandScheduler.getInstance().addButton(new Runnable() {
-      private boolean m_pressedLast = get();
+    CommandScheduler.getInstance()
+        .addButton(
+            new Runnable() {
+              private boolean m_pressedLast = get();
 
-      @Override
-      public void run() {
-        boolean pressed = get();
+              @Override
+              public void run() {
+                boolean pressed = get();
 
-        if (!m_pressedLast && pressed) {
-          command.schedule(interruptible);
-        } else if (m_pressedLast && !pressed) {
-          command.cancel();
-        }
+                if (!m_pressedLast && pressed) {
+                  command.schedule(interruptible);
+                } else if (m_pressedLast && !pressed) {
+                  command.cancel();
+                }
 
-        m_pressedLast = pressed;
-      }
-    });
+                m_pressedLast = pressed;
+              }
+            });
     return this;
   }
 
   /**
    * Starts the given command when the trigger initially becomes active, and ends it when it becomes
-   * inactive, but does not re-start it in-between.  The command is set to be interruptible.
+   * inactive, but does not re-start it in-between. The command is set to be interruptible.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -208,32 +211,34 @@
   /**
    * Starts the command when the trigger becomes inactive.
    *
-   * @param command       the command to start
+   * @param command the command to start
    * @param interruptible whether the command is interruptible
    * @return this trigger, so calls can be chained
    */
   public Trigger whenInactive(final Command command, boolean interruptible) {
     requireNonNullParam(command, "command", "whenInactive");
 
-    CommandScheduler.getInstance().addButton(new Runnable() {
-      private boolean m_pressedLast = get();
+    CommandScheduler.getInstance()
+        .addButton(
+            new Runnable() {
+              private boolean m_pressedLast = get();
 
-      @Override
-      public void run() {
-        boolean pressed = get();
+              @Override
+              public void run() {
+                boolean pressed = get();
 
-        if (m_pressedLast && !pressed) {
-          command.schedule(interruptible);
-        }
+                if (m_pressedLast && !pressed) {
+                  command.schedule(interruptible);
+                }
 
-        m_pressedLast = pressed;
-      }
-    });
+                m_pressedLast = pressed;
+              }
+            });
     return this;
   }
 
   /**
-   * Starts the command when the trigger becomes inactive.  The command is set to be interruptible.
+   * Starts the command when the trigger becomes inactive. The command is set to be interruptible.
    *
    * @param command the command to start
    * @return this trigger, so calls can be chained
@@ -245,7 +250,7 @@
   /**
    * Runs the given runnable when the trigger becomes inactive.
    *
-   * @param toRun        the runnable to run
+   * @param toRun the runnable to run
    * @param requirements the required subsystems
    * @return this trigger, so calls can be chained
    */
@@ -256,36 +261,38 @@
   /**
    * Toggles a command when the trigger becomes active.
    *
-   * @param command       the command to toggle
+   * @param command the command to toggle
    * @param interruptible whether the command is interruptible
    * @return this trigger, so calls can be chained
    */
   public Trigger toggleWhenActive(final Command command, boolean interruptible) {
     requireNonNullParam(command, "command", "toggleWhenActive");
 
-    CommandScheduler.getInstance().addButton(new Runnable() {
-      private boolean m_pressedLast = get();
+    CommandScheduler.getInstance()
+        .addButton(
+            new Runnable() {
+              private boolean m_pressedLast = get();
 
-      @Override
-      public void run() {
-        boolean pressed = get();
+              @Override
+              public void run() {
+                boolean pressed = get();
 
-        if (!m_pressedLast && pressed) {
-          if (command.isScheduled()) {
-            command.cancel();
-          } else {
-            command.schedule(interruptible);
-          }
-        }
+                if (!m_pressedLast && pressed) {
+                  if (command.isScheduled()) {
+                    command.cancel();
+                  } else {
+                    command.schedule(interruptible);
+                  }
+                }
 
-        m_pressedLast = pressed;
-      }
-    });
+                m_pressedLast = pressed;
+              }
+            });
     return this;
   }
 
   /**
-   * Toggles a command when the trigger becomes active.  The command is set to be interruptible.
+   * Toggles a command when the trigger becomes active. The command is set to be interruptible.
    *
    * @param command the command to toggle
    * @return this trigger, so calls can be chained
@@ -303,20 +310,22 @@
   public Trigger cancelWhenActive(final Command command) {
     requireNonNullParam(command, "command", "cancelWhenActive");
 
-    CommandScheduler.getInstance().addButton(new Runnable() {
-      private boolean m_pressedLast = get();
+    CommandScheduler.getInstance()
+        .addButton(
+            new Runnable() {
+              private boolean m_pressedLast = get();
 
-      @Override
-      public void run() {
-        boolean pressed = get();
+              @Override
+              public void run() {
+                boolean pressed = get();
 
-        if (!m_pressedLast && pressed) {
-          command.cancel();
-        }
+                if (!m_pressedLast && pressed) {
+                  command.cancel();
+                }
 
-        m_pressedLast = pressed;
-      }
-    });
+                m_pressedLast = pressed;
+              }
+            });
     return this;
   }
 
@@ -351,4 +360,23 @@
   public Trigger negate() {
     return new Trigger(() -> !get());
   }
+
+  /**
+   * Creates a new debounced trigger from this trigger - it will become active when this trigger has
+   * been active for longer than the specified period.
+   *
+   * @param seconds the debounce period
+   * @return the debounced trigger
+   */
+  public Trigger debounce(double seconds) {
+    return new Trigger(
+        new BooleanSupplier() {
+          Debouncer m_debouncer = new Debouncer(seconds);
+
+          @Override
+          public boolean getAsBoolean() {
+            return m_debouncer.calculate(get());
+          }
+        });
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
index 248368d..b434c1b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/Command.h"
 
@@ -20,12 +17,11 @@
 
 using namespace frc2;
 
-Command::~Command() { CommandScheduler::GetInstance().Cancel(this); }
-
-Command::Command(const Command& rhs) : ErrorBase(rhs) {}
+Command::~Command() {
+  CommandScheduler::GetInstance().Cancel(this);
+}
 
 Command& Command::operator=(const Command& rhs) {
-  ErrorBase::operator=(rhs);
   m_isGrouped = false;
   return *this;
 }
@@ -52,12 +48,11 @@
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
   return std::move(*this).BeforeStarting(
-      std::move(toRun),
-      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+      std::move(toRun), {requirements.begin(), requirements.end()});
 }
 
 SequentialCommandGroup Command::BeforeStarting(
-    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(
       std::make_unique<InstantCommand>(std::move(toRun), requirements));
@@ -68,13 +63,12 @@
 SequentialCommandGroup Command::AndThen(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) && {
-  return std::move(*this).AndThen(
-      std::move(toRun),
-      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+  return std::move(*this).AndThen(std::move(toRun),
+                                  {requirements.begin(), requirements.end()});
 }
 
 SequentialCommandGroup Command::AndThen(
-    std::function<void()> toRun, wpi::ArrayRef<Subsystem*> requirements) && {
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
   std::vector<std::unique_ptr<Command>> temp;
   temp.emplace_back(std::move(*this).TransferOwnership());
   temp.emplace_back(
@@ -86,13 +80,17 @@
   return PerpetualCommand(std::move(*this).TransferOwnership());
 }
 
-ProxyScheduleCommand Command::AsProxy() { return ProxyScheduleCommand(this); }
+ProxyScheduleCommand Command::AsProxy() {
+  return ProxyScheduleCommand(this);
+}
 
 void Command::Schedule(bool interruptible) {
   CommandScheduler::GetInstance().Schedule(interruptible, this);
 }
 
-void Command::Cancel() { CommandScheduler::GetInstance().Cancel(this); }
+void Command::Cancel() {
+  CommandScheduler::GetInstance().Cancel(this);
+}
 
 bool Command::IsScheduled() const {
   return CommandScheduler::GetInstance().IsScheduled(this);
@@ -106,11 +104,17 @@
   return hasRequirement;
 }
 
-std::string Command::GetName() const { return GetTypeName(*this); }
+std::string Command::GetName() const {
+  return GetTypeName(*this);
+}
 
-bool Command::IsGrouped() const { return m_isGrouped; }
+bool Command::IsGrouped() const {
+  return m_isGrouped;
+}
 
-void Command::SetGrouped(bool grouped) { m_isGrouped = grouped; }
+void Command::SetGrouped(bool grouped) {
+  m_isGrouped = grouped;
+}
 
 namespace frc2 {
 bool RequirementsDisjoint(Command* first, Command* second) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
index 30c3af6..b6f76f3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandBase.cpp
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/CommandBase.h"
 
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 using namespace frc2;
 
 CommandBase::CommandBase() {
-  frc::SendableRegistry::GetInstance().Add(this, GetTypeName(*this));
+  wpi::SendableRegistry::Add(this, GetTypeName(*this));
 }
 
 void CommandBase::AddRequirements(
@@ -21,7 +18,7 @@
   m_requirements.insert(requirements.begin(), requirements.end());
 }
 
-void CommandBase::AddRequirements(wpi::ArrayRef<Subsystem*> requirements) {
+void CommandBase::AddRequirements(wpi::span<Subsystem* const> requirements) {
   m_requirements.insert(requirements.begin(), requirements.end());
 }
 
@@ -29,27 +26,31 @@
   m_requirements.insert(requirements.begin(), requirements.end());
 }
 
+void CommandBase::AddRequirements(Subsystem* requirement) {
+  m_requirements.insert(requirement);
+}
+
 wpi::SmallSet<Subsystem*, 4> CommandBase::GetRequirements() const {
   return m_requirements;
 }
 
-void CommandBase::SetName(const wpi::Twine& name) {
-  frc::SendableRegistry::GetInstance().SetName(this, name);
+void CommandBase::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
 }
 
 std::string CommandBase::GetName() const {
-  return frc::SendableRegistry::GetInstance().GetName(this);
+  return wpi::SendableRegistry::GetName(this);
 }
 
 std::string CommandBase::GetSubsystem() const {
-  return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+  return wpi::SendableRegistry::GetSubsystem(this);
 }
 
-void CommandBase::SetSubsystem(const wpi::Twine& subsystem) {
-  frc::SendableRegistry::GetInstance().SetSubsystem(this, subsystem);
+void CommandBase::SetSubsystem(std::string_view subsystem) {
+  wpi::SendableRegistry::SetSubsystem(this, subsystem);
 }
 
-void CommandBase::InitSendable(frc::SendableBuilder& builder) {
+void CommandBase::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Command");
   builder.AddStringProperty(
       ".name", [this] { return GetName(); }, nullptr);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
index 8869d4c..a3d275d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandGroupBase.cpp
@@ -1,50 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/CommandGroupBase.h"
 
-#include <frc/WPIErrors.h>
-
 using namespace frc2;
 
-bool CommandGroupBase::RequireUngrouped(Command& command) {
+bool CommandGroupBase::RequireUngrouped(const Command& command) {
   if (command.IsGrouped()) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
+    throw FRC_MakeError(
+        frc::err::CommandIllegalUse, "{}",
         "Commands cannot be added to more than one CommandGroup");
-    return false;
-  } else {
-    return true;
   }
+  return true;
+}
+
+bool CommandGroupBase::RequireUngrouped(const Command* command) {
+  return RequireUngrouped(*command);
 }
 
 bool CommandGroupBase::RequireUngrouped(
-    wpi::ArrayRef<std::unique_ptr<Command>> commands) {
+    wpi::span<const std::unique_ptr<Command>> commands) {
   bool allUngrouped = true;
   for (auto&& command : commands) {
     allUngrouped &= !command.get()->IsGrouped();
   }
   if (!allUngrouped) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
+    throw FRC_MakeError(
+        frc::err::CommandIllegalUse, "{}",
         "Commands cannot be added to more than one CommandGroup");
   }
   return allUngrouped;
 }
 
 bool CommandGroupBase::RequireUngrouped(
-    std::initializer_list<Command*> commands) {
+    std::initializer_list<const Command*> commands) {
   bool allUngrouped = true;
   for (auto&& command : commands) {
     allUngrouped &= !command->IsGrouped();
   }
   if (!allUngrouped) {
-    wpi_setGlobalWPIErrorWithContext(
-        CommandIllegalUse,
+    throw FRC_MakeError(
+        frc::err::CommandIllegalUse, "{}",
         "Commands cannot be added to more than one CommandGroup");
   }
   return allUngrouped;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
index a59c68b..f79fa59 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandScheduler.cpp
@@ -1,25 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/CommandScheduler.h"
 
+#include <cstdio>
+
 #include <frc/RobotBase.h>
 #include <frc/RobotState.h>
 #include <frc/TimedRobot.h>
-#include <frc/WPIErrors.h>
 #include <frc/livewindow/LiveWindow.h>
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
+#include <networktables/NTSendableBuilder.h>
 #include <networktables/NetworkTableEntry.h>
 #include <wpi/DenseMap.h>
 #include <wpi/SmallVector.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include "frc2/command/CommandGroupBase.h"
 #include "frc2/command/CommandState.h"
@@ -69,24 +66,24 @@
 
 CommandScheduler::CommandScheduler()
     : m_impl(new Impl), m_watchdog(frc::TimedRobot::kDefaultPeriod, [] {
-        wpi::outs() << "CommandScheduler loop time overrun.\n";
+        std::puts("CommandScheduler loop time overrun.");
       }) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand2_Scheduler);
-  frc::SendableRegistry::GetInstance().AddLW(this, "Scheduler");
-  auto scheduler = frc::LiveWindow::GetInstance();
-  scheduler->enabled = [this] {
+  wpi::SendableRegistry::AddLW(this, "Scheduler");
+  frc::LiveWindow::SetEnabledCallback([this] {
     this->Disable();
     this->CancelAll();
-  };
-  scheduler->disabled = [this] { this->Enable(); };
+  });
+  frc::LiveWindow::SetDisabledCallback([this] { this->Enable(); });
 }
 
 CommandScheduler::~CommandScheduler() {
-  frc::SendableRegistry::GetInstance().Remove(this);
-  auto scheduler = frc::LiveWindow::GetInstance();
-  scheduler->enabled = nullptr;
-  scheduler->disabled = nullptr;
+  wpi::SendableRegistry::Remove(this);
+  frc::LiveWindow::SetEnabledCallback(nullptr);
+  frc::LiveWindow::SetDisabledCallback(nullptr);
+
+  std::unique_ptr<Impl>().swap(m_impl);
 }
 
 CommandScheduler& CommandScheduler::GetInstance() {
@@ -102,7 +99,9 @@
   m_impl->buttons.emplace_back(std::move(button));
 }
 
-void CommandScheduler::ClearButtons() { m_impl->buttons.clear(); }
+void CommandScheduler::ClearButtons() {
+  m_impl->buttons.clear();
+}
 
 void CommandScheduler::Schedule(bool interruptible, Command* command) {
   if (m_impl->inRunLoop) {
@@ -111,9 +110,9 @@
   }
 
   if (command->IsGrouped()) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "A command that is part of a command group "
-                               "cannot be independently scheduled");
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "A command that is part of a command group "
+                        "cannot be independently scheduled");
     return;
   }
   if (m_impl->disabled ||
@@ -155,10 +154,12 @@
   }
 }
 
-void CommandScheduler::Schedule(Command* command) { Schedule(true, command); }
+void CommandScheduler::Schedule(Command* command) {
+  Schedule(true, command);
+}
 
 void CommandScheduler::Schedule(bool interruptible,
-                                wpi::ArrayRef<Command*> commands) {
+                                wpi::span<Command* const> commands) {
   for (auto command : commands) {
     Schedule(interruptible, command);
   }
@@ -171,7 +172,7 @@
   }
 }
 
-void CommandScheduler::Schedule(wpi::ArrayRef<Command*> commands) {
+void CommandScheduler::Schedule(wpi::span<Command* const> commands) {
   for (auto command : commands) {
     Schedule(true, command);
   }
@@ -281,7 +282,8 @@
   }
 }
 
-void CommandScheduler::RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems) {
+void CommandScheduler::RegisterSubsystem(
+    wpi::span<Subsystem* const> subsystems) {
   for (auto* subsystem : subsystems) {
     RegisterSubsystem(subsystem);
   }
@@ -295,7 +297,7 @@
 }
 
 void CommandScheduler::UnregisterSubsystem(
-    wpi::ArrayRef<Subsystem*> subsystems) {
+    wpi::span<Subsystem* const> subsystems) {
   for (auto* subsystem : subsystems) {
     UnregisterSubsystem(subsystem);
   }
@@ -311,13 +313,19 @@
 }
 
 void CommandScheduler::Cancel(Command* command) {
+  if (!m_impl) {
+    return;
+  }
+
   if (m_impl->inRunLoop) {
     m_impl->toCancel.emplace_back(command);
     return;
   }
 
   auto find = m_impl->scheduledCommands.find(command);
-  if (find == m_impl->scheduledCommands.end()) return;
+  if (find == m_impl->scheduledCommands.end()) {
+    return;
+  }
   command->End(true);
   for (auto&& action : m_impl->interruptActions) {
     action(*command);
@@ -331,7 +339,7 @@
   }
 }
 
-void CommandScheduler::Cancel(wpi::ArrayRef<Command*> commands) {
+void CommandScheduler::Cancel(wpi::span<Command* const> commands) {
   for (auto command : commands) {
     Cancel(command);
   }
@@ -351,16 +359,17 @@
   Cancel(commands);
 }
 
-double CommandScheduler::TimeSinceScheduled(const Command* command) const {
+units::second_t CommandScheduler::TimeSinceScheduled(
+    const Command* command) const {
   auto find = m_impl->scheduledCommands.find(command);
   if (find != m_impl->scheduledCommands.end()) {
     return find->second.TimeSinceInitialized();
   } else {
-    return -1;
+    return -1_s;
   }
 }
 bool CommandScheduler::IsScheduled(
-    wpi::ArrayRef<const Command*> commands) const {
+    wpi::span<const Command* const> commands) const {
   for (auto command : commands) {
     if (!IsScheduled(command)) {
       return false;
@@ -393,9 +402,13 @@
   }
 }
 
-void CommandScheduler::Disable() { m_impl->disabled = true; }
+void CommandScheduler::Disable() {
+  m_impl->disabled = true;
+}
 
-void CommandScheduler::Enable() { m_impl->disabled = false; }
+void CommandScheduler::Enable() {
+  m_impl->disabled = false;
+}
 
 void CommandScheduler::OnCommandInitialize(Action action) {
   m_impl->initActions.emplace_back(std::move(action));
@@ -413,7 +426,7 @@
   m_impl->finishActions.emplace_back(std::move(action));
 }
 
-void CommandScheduler::InitSendable(frc::SendableBuilder& builder) {
+void CommandScheduler::InitSendable(nt::NTSendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
   auto namesEntry = builder.GetEntry("Names");
   auto idsEntry = builder.GetEntry("Ids");
@@ -430,8 +443,7 @@
           m_impl->scheduledCommands.end()) {
         Cancel(command);
       }
-      nt::NetworkTableEntry(cancelEntry)
-          .SetDoubleArray(wpi::ArrayRef<double>{});
+      nt::NetworkTableEntry(cancelEntry).SetDoubleArray({});
     }
 
     wpi::SmallVector<std::string, 8> names;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
index b41fa70..f53a694 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/CommandState.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/CommandState.h"
 
@@ -19,7 +16,12 @@
 void CommandState::StartTiming() {
   m_startTime = frc::Timer::GetFPGATimestamp();
 }
-void CommandState::StartRunning() { m_startTime = -1; }
-double CommandState::TimeSinceInitialized() const {
-  return m_startTime != -1 ? frc::Timer::GetFPGATimestamp() - m_startTime : -1;
+
+void CommandState::StartRunning() {
+  m_startTime = -1_s;
+}
+
+units::second_t CommandState::TimeSinceInitialized() const {
+  return m_startTime != -1_s ? frc::Timer::GetFPGATimestamp() - m_startTime
+                             : -1_s;
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
index 2344513..30ef410 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ConditionalCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/ConditionalCommand.h"
 
@@ -39,7 +36,9 @@
   m_selectedCommand->Initialize();
 }
 
-void ConditionalCommand::Execute() { m_selectedCommand->Execute(); }
+void ConditionalCommand::Execute() {
+  m_selectedCommand->Execute();
+}
 
 void ConditionalCommand::End(bool interrupted) {
   m_selectedCommand->End(interrupted);
@@ -49,4 +48,6 @@
   return m_selectedCommand->IsFinished();
 }
 
-bool ConditionalCommand::RunsWhenDisabled() const { return m_runsWhenDisabled; }
+bool ConditionalCommand::RunsWhenDisabled() const {
+  return m_runsWhenDisabled;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
index ee28ba7..0c45099 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/FunctionalCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/FunctionalCommand.h"
 
@@ -24,7 +21,7 @@
                                      std::function<void()> onExecute,
                                      std::function<void(bool)> onEnd,
                                      std::function<bool()> isFinished,
-                                     wpi::ArrayRef<Subsystem*> requirements)
+                                     wpi::span<Subsystem* const> requirements)
     : m_onInit{std::move(onInit)},
       m_onExecute{std::move(onExecute)},
       m_onEnd{std::move(onEnd)},
@@ -32,10 +29,18 @@
   AddRequirements(requirements);
 }
 
-void FunctionalCommand::Initialize() { m_onInit(); }
+void FunctionalCommand::Initialize() {
+  m_onInit();
+}
 
-void FunctionalCommand::Execute() { m_onExecute(); }
+void FunctionalCommand::Execute() {
+  m_onExecute();
+}
 
-void FunctionalCommand::End(bool interrupted) { m_onEnd(interrupted); }
+void FunctionalCommand::End(bool interrupted) {
+  m_onEnd(interrupted);
+}
 
-bool FunctionalCommand::IsFinished() { return m_isFinished(); }
+bool FunctionalCommand::IsFinished() {
+  return m_isFinished();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
index 6f66c5c..1e9d173 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/InstantCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/InstantCommand.h"
 
@@ -16,13 +13,17 @@
 }
 
 InstantCommand::InstantCommand(std::function<void()> toRun,
-                               wpi::ArrayRef<Subsystem*> requirements)
+                               wpi::span<Subsystem* const> requirements)
     : m_toRun{std::move(toRun)} {
   AddRequirements(requirements);
 }
 
 InstantCommand::InstantCommand() : m_toRun{[] {}} {}
 
-void InstantCommand::Initialize() { m_toRun(); }
+void InstantCommand::Initialize() {
+  m_toRun();
+}
 
-bool InstantCommand::IsFinished() { return true; }
+bool InstantCommand::IsFinished() {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
index eb32853..c521f8b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/MecanumControllerCommand.h"
 
+#include <utility>
+
 using namespace frc2;
 using namespace units;
 
@@ -27,12 +26,12 @@
                        units::volt_t)>
         output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
-      m_desiredRotation(desiredRotation),
+      m_desiredRotation(std::move(desiredRotation)),
       m_maxWheelVelocity(maxWheelVelocity),
       m_frontLeftController(
           std::make_unique<frc2::PIDController>(frontLeftController)),
@@ -42,8 +41,8 @@
           std::make_unique<frc2::PIDController>(frontRightController)),
       m_rearRightController(
           std::make_unique<frc2::PIDController>(rearRightController)),
-      m_currentWheelSpeeds(currentWheelSpeeds),
-      m_outputVolts(output),
+      m_currentWheelSpeeds(std::move(currentWheelSpeeds)),
+      m_outputVolts(std::move(output)),
       m_usePID(true) {
   AddRequirements(requirements);
 }
@@ -64,8 +63,8 @@
                        units::volt_t)>
         output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
@@ -78,8 +77,8 @@
           std::make_unique<frc2::PIDController>(frontRightController)),
       m_rearRightController(
           std::make_unique<frc2::PIDController>(rearRightController)),
-      m_currentWheelSpeeds(currentWheelSpeeds),
-      m_outputVolts(output),
+      m_currentWheelSpeeds(std::move(currentWheelSpeeds)),
+      m_outputVolts(std::move(output)),
       m_usePID(true) {
   AddRequirements(requirements);
   m_desiredRotation = [&] {
@@ -103,13 +102,13 @@
     std::function<void(units::volt_t, units::volt_t, units::volt_t,
                        units::volt_t)>
         output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
-      m_desiredRotation(desiredRotation),
+      m_desiredRotation(std::move(desiredRotation)),
       m_maxWheelVelocity(maxWheelVelocity),
       m_frontLeftController(
           std::make_unique<frc2::PIDController>(frontLeftController)),
@@ -119,8 +118,8 @@
           std::make_unique<frc2::PIDController>(frontRightController)),
       m_rearRightController(
           std::make_unique<frc2::PIDController>(rearRightController)),
-      m_currentWheelSpeeds(currentWheelSpeeds),
-      m_outputVolts(output),
+      m_currentWheelSpeeds(std::move(currentWheelSpeeds)),
+      m_outputVolts(std::move(output)),
       m_usePID(true) {
   AddRequirements(requirements);
 }
@@ -140,9 +139,9 @@
     std::function<void(units::volt_t, units::volt_t, units::volt_t,
                        units::volt_t)>
         output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
@@ -155,8 +154,8 @@
           std::make_unique<frc2::PIDController>(frontRightController)),
       m_rearRightController(
           std::make_unique<frc2::PIDController>(rearRightController)),
-      m_currentWheelSpeeds(currentWheelSpeeds),
-      m_outputVolts(output),
+      m_currentWheelSpeeds(std::move(currentWheelSpeeds)),
+      m_outputVolts(std::move(output)),
       m_usePID(true) {
   AddRequirements(requirements);
   m_desiredRotation = [&] {
@@ -175,13 +174,13 @@
                        units::meters_per_second_t, units::meters_per_second_t)>
         output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
-      m_desiredRotation(desiredRotation),
+      m_desiredRotation(std::move(desiredRotation)),
       m_maxWheelVelocity(maxWheelVelocity),
-      m_outputVel(output),
+      m_outputVel(std::move(output)),
       m_usePID(false) {
   AddRequirements(requirements);
 }
@@ -196,12 +195,12 @@
                        units::meters_per_second_t, units::meters_per_second_t)>
         output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
       m_maxWheelVelocity(maxWheelVelocity),
-      m_outputVel(output),
+      m_outputVel(std::move(output)),
       m_usePID(false) {
   AddRequirements(requirements);
   m_desiredRotation = [&] {
@@ -219,14 +218,14 @@
     std::function<void(units::meters_per_second_t, units::meters_per_second_t,
                        units::meters_per_second_t, units::meters_per_second_t)>
         output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
-      m_desiredRotation(desiredRotation),
+      m_desiredRotation(std::move(desiredRotation)),
       m_maxWheelVelocity(maxWheelVelocity),
-      m_outputVel(output),
+      m_outputVel(std::move(output)),
       m_usePID(false) {
   AddRequirements(requirements);
 }
@@ -240,13 +239,13 @@
     std::function<void(units::meters_per_second_t, units::meters_per_second_t,
                        units::meters_per_second_t, units::meters_per_second_t)>
         output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
       m_maxWheelVelocity(maxWheelVelocity),
-      m_outputVel(output),
+      m_outputVel(std::move(output)),
       m_usePID(false) {
   AddRequirements(requirements);
   m_desiredRotation = [&] {
@@ -311,20 +310,20 @@
         (rearRightSpeedSetpoint - m_prevSpeeds.rearRight) / dt);
 
     auto frontLeftOutput = volt_t(m_frontLeftController->Calculate(
-                               m_currentWheelSpeeds().frontLeft.to<double>(),
-                               frontLeftSpeedSetpoint.to<double>())) +
+                               m_currentWheelSpeeds().frontLeft.value(),
+                               frontLeftSpeedSetpoint.value())) +
                            frontLeftFeedforward;
     auto rearLeftOutput = volt_t(m_rearLeftController->Calculate(
-                              m_currentWheelSpeeds().rearLeft.to<double>(),
-                              rearLeftSpeedSetpoint.to<double>())) +
+                              m_currentWheelSpeeds().rearLeft.value(),
+                              rearLeftSpeedSetpoint.value())) +
                           rearLeftFeedforward;
     auto frontRightOutput = volt_t(m_frontRightController->Calculate(
-                                m_currentWheelSpeeds().frontRight.to<double>(),
-                                frontRightSpeedSetpoint.to<double>())) +
+                                m_currentWheelSpeeds().frontRight.value(),
+                                frontRightSpeedSetpoint.value())) +
                             frontRightFeedforward;
     auto rearRightOutput = volt_t(m_rearRightController->Calculate(
-                               m_currentWheelSpeeds().rearRight.to<double>(),
-                               rearRightSpeedSetpoint.to<double>())) +
+                               m_currentWheelSpeeds().rearRight.value(),
+                               rearRightSpeedSetpoint.value())) +
                            rearRightFeedforward;
 
     m_outputVolts(frontLeftOutput, rearLeftOutput, frontRightOutput,
@@ -338,7 +337,9 @@
   }
 }
 
-void MecanumControllerCommand::End(bool interrupted) { m_timer.Stop(); }
+void MecanumControllerCommand::End(bool interrupted) {
+  m_timer.Stop();
+}
 
 bool MecanumControllerCommand::IsFinished() {
   return m_timer.HasElapsed(m_trajectory.TotalTime());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
index 1b13d75..37e5bc6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/NotifierCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/NotifierCommand.h"
 
@@ -18,7 +15,7 @@
 
 NotifierCommand::NotifierCommand(std::function<void()> toRun,
                                  units::second_t period,
-                                 wpi::ArrayRef<Subsystem*> requirements)
+                                 wpi::span<Subsystem* const> requirements)
     : m_toRun(toRun), m_notifier{std::move(toRun)}, m_period{period} {
   AddRequirements(requirements);
 }
@@ -35,6 +32,10 @@
       m_notifier(frc::Notifier(other.m_toRun)),
       m_period(other.m_period) {}
 
-void NotifierCommand::Initialize() { m_notifier.StartPeriodic(m_period); }
+void NotifierCommand::Initialize() {
+  m_notifier.StartPeriodic(m_period);
+}
 
-void NotifierCommand::End(bool interrupted) { m_notifier.Stop(); }
+void NotifierCommand::End(bool interrupted) {
+  m_notifier.Stop();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
index ea6c105..d11735a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDCommand.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/PIDCommand.h"
 
+#include <utility>
+
 using namespace frc2;
 
 PIDCommand::PIDCommand(PIDController controller,
@@ -14,7 +13,7 @@
                        std::function<double()> setpointSource,
                        std::function<void(double)> useOutput,
                        std::initializer_list<Subsystem*> requirements)
-    : m_controller{controller},
+    : m_controller{std::move(controller)},
       m_measurement{std::move(measurementSource)},
       m_setpoint{std::move(setpointSource)},
       m_useOutput{std::move(useOutput)} {
@@ -25,8 +24,8 @@
                        std::function<double()> measurementSource,
                        std::function<double()> setpointSource,
                        std::function<void(double)> useOutput,
-                       wpi::ArrayRef<Subsystem*> requirements)
-    : m_controller{controller},
+                       wpi::span<Subsystem* const> requirements)
+    : m_controller{std::move(controller)},
       m_measurement{std::move(measurementSource)},
       m_setpoint{std::move(setpointSource)},
       m_useOutput{std::move(useOutput)} {
@@ -44,17 +43,23 @@
 PIDCommand::PIDCommand(PIDController controller,
                        std::function<double()> measurementSource,
                        double setpoint, std::function<void(double)> useOutput,
-                       wpi::ArrayRef<Subsystem*> requirements)
+                       wpi::span<Subsystem* const> requirements)
     : PIDCommand(
           controller, measurementSource, [setpoint] { return setpoint; },
           useOutput, requirements) {}
 
-void PIDCommand::Initialize() { m_controller.Reset(); }
+void PIDCommand::Initialize() {
+  m_controller.Reset();
+}
 
 void PIDCommand::Execute() {
   m_useOutput(m_controller.Calculate(m_measurement(), m_setpoint()));
 }
 
-void PIDCommand::End(bool interrupted) { m_useOutput(0); }
+void PIDCommand::End(bool interrupted) {
+  m_useOutput(0);
+}
 
-PIDController& PIDCommand::GetController() { return m_controller; }
+PIDController& PIDCommand::GetController() {
+  return m_controller;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
index f7d988d..7c820ae 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PIDSubsystem.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/PIDSubsystem.h"
 
+#include <utility>
+
 using namespace frc2;
 
 PIDSubsystem::PIDSubsystem(PIDController controller, double initialPosition)
-    : m_controller{controller} {
+    : m_controller{std::move(controller)} {
   SetSetpoint(initialPosition);
   AddChild("PID Controller", &m_controller);
 }
@@ -21,9 +20,13 @@
   }
 }
 
-void PIDSubsystem::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
+void PIDSubsystem::SetSetpoint(double setpoint) {
+  m_setpoint = setpoint;
+}
 
-double PIDSubsystem::GetSetpoint() const { return m_setpoint; }
+double PIDSubsystem::GetSetpoint() const {
+  return m_setpoint;
+}
 
 void PIDSubsystem::Enable() {
   m_controller.Reset();
@@ -35,6 +38,10 @@
   m_enabled = false;
 }
 
-bool PIDSubsystem::IsEnabled() { return m_enabled; }
+bool PIDSubsystem::IsEnabled() {
+  return m_enabled;
+}
 
-PIDController& PIDSubsystem::GetController() { return m_controller; }
+PIDController& PIDSubsystem::GetController() {
+  return m_controller;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
index 557984e..a12d2b4 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelCommandGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/ParallelCommandGroup.h"
 
@@ -24,7 +21,9 @@
 
 void ParallelCommandGroup::Execute() {
   for (auto& commandRunning : m_commands) {
-    if (!commandRunning.second) continue;
+    if (!commandRunning.second) {
+      continue;
+    }
     commandRunning.first->Execute();
     if (commandRunning.first->IsFinished()) {
       commandRunning.first->End(false);
@@ -46,7 +45,9 @@
 
 bool ParallelCommandGroup::IsFinished() {
   for (auto& command : m_commands) {
-    if (command.second) return false;
+    if (command.second) {
+      return false;
+    }
   }
   return true;
 }
@@ -58,13 +59,15 @@
 void ParallelCommandGroup::AddCommands(
     std::vector<std::unique_ptr<Command>>&& commands) {
   for (auto&& command : commands) {
-    if (!RequireUngrouped(*command)) return;
+    if (!RequireUngrouped(*command)) {
+      return;
+    }
   }
 
   if (isRunning) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
   }
 
   for (auto&& command : commands) {
@@ -74,10 +77,9 @@
       m_runWhenDisabled &= command->RunsWhenDisabled();
       m_commands.emplace_back(std::move(command), false);
     } else {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Multiple commands in a parallel group cannot "
-                                 "require the same subsystems");
-      return;
+      throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                          "Multiple commands in a parallel group cannot "
+                          "require the same subsystems");
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
index 935cf8b..93dd2fd 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelDeadlineGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/ParallelDeadlineGroup.h"
 
@@ -26,7 +23,9 @@
 
 void ParallelDeadlineGroup::Execute() {
   for (auto& commandRunning : m_commands) {
-    if (!commandRunning.second) continue;
+    if (!commandRunning.second) {
+      continue;
+    }
     commandRunning.first->Execute();
     if (commandRunning.first->IsFinished()) {
       commandRunning.first->End(false);
@@ -46,7 +45,9 @@
   }
 }
 
-bool ParallelDeadlineGroup::IsFinished() { return m_finished; }
+bool ParallelDeadlineGroup::IsFinished() {
+  return m_finished;
+}
 
 bool ParallelDeadlineGroup::RunsWhenDisabled() const {
   return m_runWhenDisabled;
@@ -59,9 +60,9 @@
   }
 
   if (!m_finished) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
   }
 
   for (auto&& command : commands) {
@@ -71,10 +72,9 @@
       m_runWhenDisabled &= command->RunsWhenDisabled();
       m_commands.emplace_back(std::move(command), false);
     } else {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Multiple commands in a parallel group cannot "
-                                 "require the same subsystems");
-      return;
+      throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                          "Multiple commands in a parallel group cannot "
+                          "require the same subsystems");
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
index 5fc5e31..63c7cbc 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ParallelRaceGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/ParallelRaceGroup.h"
 
@@ -38,9 +35,13 @@
   isRunning = false;
 }
 
-bool ParallelRaceGroup::IsFinished() { return m_finished; }
+bool ParallelRaceGroup::IsFinished() {
+  return m_finished;
+}
 
-bool ParallelRaceGroup::RunsWhenDisabled() const { return m_runWhenDisabled; }
+bool ParallelRaceGroup::RunsWhenDisabled() const {
+  return m_runWhenDisabled;
+}
 
 void ParallelRaceGroup::AddCommands(
     std::vector<std::unique_ptr<Command>>&& commands) {
@@ -49,9 +50,9 @@
   }
 
   if (isRunning) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
   }
 
   for (auto&& command : commands) {
@@ -61,10 +62,9 @@
       m_runWhenDisabled &= command->RunsWhenDisabled();
       m_commands.emplace_back(std::move(command));
     } else {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Multiple commands in a parallel group cannot "
-                                 "require the same subsystems");
-      return;
+      throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                          "Multiple commands in a parallel group cannot "
+                          "require the same subsystems");
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
index f29850b..bd8bb93 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PerpetualCommand.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/PerpetualCommand.h"
 
 using namespace frc2;
 
 PerpetualCommand::PerpetualCommand(std::unique_ptr<Command>&& command) {
-  if (!CommandGroupBase::RequireUngrouped(command)) {
+  if (!CommandGroupBase::RequireUngrouped(*command)) {
     return;
   }
   m_command = std::move(command);
@@ -18,8 +15,18 @@
   AddRequirements(m_command->GetRequirements());
 }
 
-void PerpetualCommand::Initialize() { m_command->Initialize(); }
+void PerpetualCommand::Initialize() {
+  m_command->Initialize();
+}
 
-void PerpetualCommand::Execute() { m_command->Execute(); }
+void PerpetualCommand::Execute() {
+  m_command->Execute();
+}
 
-void PerpetualCommand::End(bool interrupted) { m_command->End(interrupted); }
+void PerpetualCommand::End(bool interrupted) {
+  m_command->End(interrupted);
+}
+
+PerpetualCommand PerpetualCommand::Perpetually() && {
+  return std::move(*this);
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
index e077760..8d7c855 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/PrintCommand.cpp
@@ -1,18 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/PrintCommand.h"
 
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 using namespace frc2;
 
-PrintCommand::PrintCommand(const wpi::Twine& message)
-    : CommandHelper{[str = message.str()] { wpi::outs() << str << "\n"; }, {}} {
-}
+PrintCommand::PrintCommand(std::string_view message)
+    : CommandHelper{[str = std::string(message)] { fmt::print("{}\n", str); },
+                    {}} {}
 
-bool PrintCommand::RunsWhenDisabled() const { return true; }
+bool PrintCommand::RunsWhenDisabled() const {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
index 6f96315..116ce3c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ProxyScheduleCommand.cpp
@@ -1,18 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/ProxyScheduleCommand.h"
 
 using namespace frc2;
 
-ProxyScheduleCommand::ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+ProxyScheduleCommand::ProxyScheduleCommand(
+    wpi::span<Command* const> toSchedule) {
   SetInsert(m_toSchedule, toSchedule);
 }
 
+ProxyScheduleCommand::ProxyScheduleCommand(Command* toSchedule) {
+  SetInsert(m_toSchedule, {&toSchedule, 1});
+}
+
 void ProxyScheduleCommand::Initialize() {
   for (auto* command : m_toSchedule) {
     command->Schedule();
@@ -34,4 +36,6 @@
   }
 }
 
-bool ProxyScheduleCommand::IsFinished() { return m_finished; }
+bool ProxyScheduleCommand::IsFinished() {
+  return m_finished;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
index e2c56ab..4674031 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/RamseteCommand.h"
 
+#include <utility>
+
 using namespace frc2;
 using namespace units;
 
@@ -19,15 +18,15 @@
     frc2::PIDController leftController, frc2::PIDController rightController,
     std::function<void(volt_t, volt_t)> output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_controller(controller),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
-      m_speeds(wheelSpeeds),
+      m_speeds(std::move(wheelSpeeds)),
       m_leftController(std::make_unique<frc2::PIDController>(leftController)),
       m_rightController(std::make_unique<frc2::PIDController>(rightController)),
-      m_outputVolts(output),
+      m_outputVolts(std::move(output)),
       m_usePID(true) {
   AddRequirements(requirements);
 }
@@ -40,16 +39,16 @@
     std::function<frc::DifferentialDriveWheelSpeeds()> wheelSpeeds,
     frc2::PIDController leftController, frc2::PIDController rightController,
     std::function<void(volt_t, volt_t)> output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_controller(controller),
       m_feedforward(feedforward),
       m_kinematics(kinematics),
-      m_speeds(wheelSpeeds),
+      m_speeds(std::move(wheelSpeeds)),
       m_leftController(std::make_unique<frc2::PIDController>(leftController)),
       m_rightController(std::make_unique<frc2::PIDController>(rightController)),
-      m_outputVolts(output),
+      m_outputVolts(std::move(output)),
       m_usePID(true) {
   AddRequirements(requirements);
 }
@@ -61,11 +60,11 @@
     std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
         output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_controller(controller),
       m_kinematics(kinematics),
-      m_outputVel(output),
+      m_outputVel(std::move(output)),
       m_usePID(false) {
   AddRequirements(requirements);
 }
@@ -76,18 +75,18 @@
     frc::DifferentialDriveKinematics kinematics,
     std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
         output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_controller(controller),
       m_kinematics(kinematics),
-      m_outputVel(output),
+      m_outputVel(std::move(output)),
       m_usePID(false) {
   AddRequirements(requirements);
 }
 
 void RamseteCommand::Initialize() {
-  m_prevTime = 0_s;
+  m_prevTime = -1_s;
   auto initialState = m_trajectory.Sample(0_s);
   m_prevSpeeds = m_kinematics.ToWheelSpeeds(
       frc::ChassisSpeeds{initialState.velocity, 0_mps,
@@ -104,6 +103,17 @@
   auto curTime = m_timer.Get();
   auto dt = curTime - m_prevTime;
 
+  if (m_prevTime < 0_s) {
+    if (m_usePID) {
+      m_outputVolts(0_V, 0_V);
+    } else {
+      m_outputVel(0_mps, 0_mps);
+    }
+
+    m_prevTime = curTime;
+    return;
+  }
+
   auto targetWheelSpeeds = m_kinematics.ToWheelSpeeds(
       m_controller.Calculate(m_pose(), m_trajectory.Sample(curTime)));
 
@@ -116,26 +126,35 @@
         targetWheelSpeeds.right,
         (targetWheelSpeeds.right - m_prevSpeeds.right) / dt);
 
-    auto leftOutput = volt_t(m_leftController->Calculate(
-                          m_speeds().left.to<double>(),
-                          targetWheelSpeeds.left.to<double>())) +
-                      leftFeedforward;
+    auto leftOutput =
+        volt_t(m_leftController->Calculate(m_speeds().left.value(),
+                                           targetWheelSpeeds.left.value())) +
+        leftFeedforward;
 
-    auto rightOutput = volt_t(m_rightController->Calculate(
-                           m_speeds().right.to<double>(),
-                           targetWheelSpeeds.right.to<double>())) +
-                       rightFeedforward;
+    auto rightOutput =
+        volt_t(m_rightController->Calculate(m_speeds().right.value(),
+                                            targetWheelSpeeds.right.value())) +
+        rightFeedforward;
 
     m_outputVolts(leftOutput, rightOutput);
   } else {
     m_outputVel(targetWheelSpeeds.left, targetWheelSpeeds.right);
   }
-
-  m_prevTime = curTime;
   m_prevSpeeds = targetWheelSpeeds;
+  m_prevTime = curTime;
 }
 
-void RamseteCommand::End(bool interrupted) { m_timer.Stop(); }
+void RamseteCommand::End(bool interrupted) {
+  m_timer.Stop();
+
+  if (interrupted) {
+    if (m_usePID) {
+      m_outputVolts(0_V, 0_V);
+    } else {
+      m_outputVel(0_mps, 0_mps);
+    }
+  }
+}
 
 bool RamseteCommand::IsFinished() {
   return m_timer.HasElapsed(m_trajectory.TotalTime());
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
index dff0ffe..c63e5d6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/RunCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/RunCommand.h"
 
@@ -16,9 +13,11 @@
 }
 
 RunCommand::RunCommand(std::function<void()> toRun,
-                       wpi::ArrayRef<Subsystem*> requirements)
+                       wpi::span<Subsystem* const> requirements)
     : m_toRun{std::move(toRun)} {
   AddRequirements(requirements);
 }
 
-void RunCommand::Execute() { m_toRun(); }
+void RunCommand::Execute() {
+  m_toRun();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
index ea1ea8d..ff50bc5 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/ScheduleCommand.cpp
@@ -1,24 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/ScheduleCommand.h"
 
 using namespace frc2;
 
-ScheduleCommand::ScheduleCommand(wpi::ArrayRef<Command*> toSchedule) {
+ScheduleCommand::ScheduleCommand(wpi::span<Command* const> toSchedule) {
   SetInsert(m_toSchedule, toSchedule);
 }
 
+ScheduleCommand::ScheduleCommand(Command* toSchedule) {
+  SetInsert(m_toSchedule, {&toSchedule, 1});
+}
+
 void ScheduleCommand::Initialize() {
   for (auto command : m_toSchedule) {
     command->Schedule();
   }
 }
 
-bool ScheduleCommand::IsFinished() { return true; }
+bool ScheduleCommand::IsFinished() {
+  return true;
+}
 
-bool ScheduleCommand::RunsWhenDisabled() const { return true; }
+bool ScheduleCommand::RunsWhenDisabled() const {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
index 1aa19e4..345a415 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/SequentialCommandGroup.h"
 
+#include "frc2/command/InstantCommand.h"
+
 using namespace frc2;
 
 SequentialCommandGroup::SequentialCommandGroup(
@@ -23,7 +22,9 @@
 }
 
 void SequentialCommandGroup::Execute() {
-  if (m_commands.empty()) return;
+  if (m_commands.empty()) {
+    return;
+  }
 
   auto& currentCommand = m_commands[m_currentCommandIndex];
 
@@ -61,9 +62,9 @@
   }
 
   if (m_currentCommandIndex != invalid_index) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Commands cannot be added to a CommandGroup "
-                               "while the group is running");
+    throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                        "Commands cannot be added to a CommandGroup "
+                        "while the group is running");
   }
 
   for (auto&& command : commands) {
@@ -73,3 +74,33 @@
     m_commands.emplace_back(std::move(command));
   }
 }
+
+SequentialCommandGroup SequentialCommandGroup::BeforeStarting(
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
+  // store all the commands
+  std::vector<std::unique_ptr<Command>> tmp;
+  tmp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  for (auto&& command : m_commands) {
+    command->SetGrouped(false);
+    tmp.emplace_back(std::move(command));
+  }
+
+  // reset current state
+  m_commands.clear();
+  m_requirements.clear();
+  m_runWhenDisabled = true;
+
+  // add the commands back
+  AddCommands(std::move(tmp));
+  return std::move(*this);
+}
+
+SequentialCommandGroup SequentialCommandGroup::AndThen(
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) && {
+  std::vector<std::unique_ptr<Command>> tmp;
+  tmp.emplace_back(
+      std::make_unique<InstantCommand>(std::move(toRun), requirements));
+  AddCommands(std::move(tmp));
+  return std::move(*this);
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
index ad54ae0..dc40200 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/StartEndCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/StartEndCommand.h"
 
@@ -18,7 +15,7 @@
 
 StartEndCommand::StartEndCommand(std::function<void()> onInit,
                                  std::function<void()> onEnd,
-                                 wpi::ArrayRef<Subsystem*> requirements)
+                                 wpi::span<Subsystem* const> requirements)
     : m_onInit{std::move(onInit)}, m_onEnd{std::move(onEnd)} {
   AddRequirements(requirements);
 }
@@ -29,6 +26,10 @@
   m_onEnd = other.m_onEnd;
 }
 
-void StartEndCommand::Initialize() { m_onInit(); }
+void StartEndCommand::Initialize() {
+  m_onInit();
+}
 
-void StartEndCommand::End(bool interrupted) { m_onEnd(); }
+void StartEndCommand::End(bool interrupted) {
+  m_onEnd();
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
index 010fcb1..51cbddf 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/Subsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/Subsystem.h"
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
index a28de20..8216a07 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/SubsystemBase.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/SubsystemBase.h"
 
-#include <frc/smartdashboard/SendableBuilder.h>
-#include <frc/smartdashboard/SendableRegistry.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include "frc2/command/Command.h"
 #include "frc2/command/CommandScheduler.h"
@@ -16,11 +13,11 @@
 using namespace frc2;
 
 SubsystemBase::SubsystemBase() {
-  frc::SendableRegistry::GetInstance().AddLW(this, GetTypeName(*this));
+  wpi::SendableRegistry::AddLW(this, GetTypeName(*this));
   CommandScheduler::GetInstance().RegisterSubsystem({this});
 }
 
-void SubsystemBase::InitSendable(frc::SendableBuilder& builder) {
+void SubsystemBase::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Subsystem");
   builder.AddBooleanProperty(
       ".hasDefault", [this] { return GetDefaultCommand() != nullptr; },
@@ -29,7 +26,9 @@
       ".default",
       [this]() -> std::string {
         auto command = GetDefaultCommand();
-        if (command == nullptr) return "none";
+        if (command == nullptr) {
+          return "none";
+        }
         return command->GetName();
       },
       nullptr);
@@ -40,29 +39,30 @@
       ".command",
       [this]() -> std::string {
         auto command = GetCurrentCommand();
-        if (command == nullptr) return "none";
+        if (command == nullptr) {
+          return "none";
+        }
         return command->GetName();
       },
       nullptr);
 }
 
 std::string SubsystemBase::GetName() const {
-  return frc::SendableRegistry::GetInstance().GetName(this);
+  return wpi::SendableRegistry::GetName(this);
 }
 
-void SubsystemBase::SetName(const wpi::Twine& name) {
-  frc::SendableRegistry::GetInstance().SetName(this, name);
+void SubsystemBase::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
 }
 
 std::string SubsystemBase::GetSubsystem() const {
-  return frc::SendableRegistry::GetInstance().GetSubsystem(this);
+  return wpi::SendableRegistry::GetSubsystem(this);
 }
 
-void SubsystemBase::SetSubsystem(const wpi::Twine& name) {
-  frc::SendableRegistry::GetInstance().SetSubsystem(this, name);
+void SubsystemBase::SetSubsystem(std::string_view name) {
+  wpi::SendableRegistry::SetSubsystem(this, name);
 }
 
-void SubsystemBase::AddChild(std::string name, frc::Sendable* child) {
-  auto& registry = frc::SendableRegistry::GetInstance();
-  registry.AddLW(child, GetSubsystem(), name);
+void SubsystemBase::AddChild(std::string name, wpi::Sendable* child) {
+  wpi::SendableRegistry::AddLW(child, GetSubsystem(), name);
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
index 5c49bee..da03540 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitCommand.cpp
@@ -1,17 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/WaitCommand.h"
 
+#include <fmt/format.h>
+#include <frc/fmt/Units.h>
+
 using namespace frc2;
 
 WaitCommand::WaitCommand(units::second_t duration) : m_duration{duration} {
-  auto durationStr = std::to_string(duration.to<double>());
-  SetName(wpi::Twine(GetName()) + ": " + wpi::Twine(durationStr) + " seconds");
+  SetName(fmt::format("{}: {}", GetName(), duration));
 }
 
 void WaitCommand::Initialize() {
@@ -19,8 +18,14 @@
   m_timer.Start();
 }
 
-void WaitCommand::End(bool interrupted) { m_timer.Stop(); }
+void WaitCommand::End(bool interrupted) {
+  m_timer.Stop();
+}
 
-bool WaitCommand::IsFinished() { return m_timer.HasElapsed(m_duration); }
+bool WaitCommand::IsFinished() {
+  return m_timer.HasElapsed(m_duration);
+}
 
-bool WaitCommand::RunsWhenDisabled() const { return true; }
+bool WaitCommand::RunsWhenDisabled() const {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
index 739a049..9ccd94e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/WaitUntilCommand.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/WaitUntilCommand.h"
 
-#include "frc2/Timer.h"
+#include <frc/Timer.h>
 
 using namespace frc2;
 
@@ -15,8 +12,12 @@
     : m_condition{std::move(condition)} {}
 
 WaitUntilCommand::WaitUntilCommand(units::second_t time)
-    : m_condition{[=] { return Timer::GetMatchTime() - time > 0_s; }} {}
+    : m_condition{[=] { return frc::Timer::GetMatchTime() - time > 0_s; }} {}
 
-bool WaitUntilCommand::IsFinished() { return m_condition(); }
+bool WaitUntilCommand::IsFinished() {
+  return m_condition();
+}
 
-bool WaitUntilCommand::RunsWhenDisabled() const { return true; }
+bool WaitUntilCommand::RunsWhenDisabled() const {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
index d8a7372..955830a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Button.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/button/Button.h"
 
@@ -23,7 +20,7 @@
 }
 
 Button Button::WhenPressed(std::function<void()> toRun,
-                           wpi::ArrayRef<Subsystem*> requirements) {
+                           wpi::span<Subsystem* const> requirements) {
   WhenActive(std::move(toRun), requirements);
   return *this;
 }
@@ -40,7 +37,7 @@
 }
 
 Button Button::WhileHeld(std::function<void()> toRun,
-                         wpi::ArrayRef<Subsystem*> requirements) {
+                         wpi::span<Subsystem* const> requirements) {
   WhileActiveContinous(std::move(toRun), requirements);
   return *this;
 }
@@ -62,7 +59,7 @@
 }
 
 Button Button::WhenReleased(std::function<void()> toRun,
-                            wpi::ArrayRef<Subsystem*> requirements) {
+                            wpi::span<Subsystem* const> requirements) {
   WhenInactive(std::move(toRun), requirements);
   return *this;
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp
index 01214e0..3886c62 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/NetworkButton.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/button/NetworkButton.h"
 
@@ -15,9 +12,9 @@
       }) {}
 
 NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
-                             const wpi::Twine& field)
+                             std::string_view field)
     : NetworkButton(table->GetEntry(field)) {}
 
-NetworkButton::NetworkButton(const wpi::Twine& table, const wpi::Twine& field)
+NetworkButton::NetworkButton(std::string_view table, std::string_view field)
     : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(table),
                     field) {}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
index 875b54e..0801549 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/cpp/frc2/command/button/Trigger.cpp
@@ -1,17 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc2/command/button/Trigger.h"
 
+#include <frc/Debouncer.h>
+
 #include "frc2/command/InstantCommand.h"
 
 using namespace frc2;
 
-Trigger::Trigger(const Trigger& other) : m_isActive(other.m_isActive) {}
+Trigger::Trigger(const Trigger& other) = default;
 
 Trigger Trigger::WhenActive(Command* command, bool interruptible) {
   CommandScheduler::GetInstance().AddButton(
@@ -30,12 +29,12 @@
 
 Trigger Trigger::WhenActive(std::function<void()> toRun,
                             std::initializer_list<Subsystem*> requirements) {
-  return WhenActive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
-                                                        requirements.end()));
+  return WhenActive(std::move(toRun),
+                    {requirements.begin(), requirements.end()});
 }
 
 Trigger Trigger::WhenActive(std::function<void()> toRun,
-                            wpi::ArrayRef<Subsystem*> requirements) {
+                            wpi::span<Subsystem* const> requirements) {
   return WhenActive(InstantCommand(std::move(toRun), requirements));
 }
 
@@ -58,13 +57,12 @@
 Trigger Trigger::WhileActiveContinous(
     std::function<void()> toRun,
     std::initializer_list<Subsystem*> requirements) {
-  return WhileActiveContinous(
-      std::move(toRun),
-      wpi::makeArrayRef(requirements.begin(), requirements.end()));
+  return WhileActiveContinous(std::move(toRun),
+                              {requirements.begin(), requirements.end()});
 }
 
-Trigger Trigger::WhileActiveContinous(std::function<void()> toRun,
-                                      wpi::ArrayRef<Subsystem*> requirements) {
+Trigger Trigger::WhileActiveContinous(
+    std::function<void()> toRun, wpi::span<Subsystem* const> requirements) {
   return WhileActiveContinous(InstantCommand(std::move(toRun), requirements));
 }
 
@@ -100,12 +98,12 @@
 
 Trigger Trigger::WhenInactive(std::function<void()> toRun,
                               std::initializer_list<Subsystem*> requirements) {
-  return WhenInactive(std::move(toRun), wpi::makeArrayRef(requirements.begin(),
-                                                          requirements.end()));
+  return WhenInactive(std::move(toRun),
+                      {requirements.begin(), requirements.end()});
 }
 
 Trigger Trigger::WhenInactive(std::function<void()> toRun,
-                              wpi::ArrayRef<Subsystem*> requirements) {
+                              wpi::span<Subsystem* const> requirements) {
   return WhenInactive(InstantCommand(std::move(toRun), requirements));
 }
 
@@ -140,3 +138,9 @@
       });
   return *this;
 }
+
+Trigger Trigger::Debounce(units::second_t debounceTime) {
+  return Trigger([debouncer = frc::Debouncer(debounceTime), *this]() mutable {
+    return debouncer.Calculate(m_isActive());
+  });
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
index 060832c..66f4f7e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Command.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,11 +9,10 @@
 #include <memory>
 #include <string>
 
-#include <frc/ErrorBase.h>
 #include <units/time.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/Demangle.h>
 #include <wpi/SmallSet.h>
+#include <wpi/span.h>
 
 #include "frc2/command/Subsystem.h"
 
@@ -49,13 +45,13 @@
  * @see CommandScheduler
  * @see CommandHelper
  */
-class Command : public frc::ErrorBase {
+class Command {
  public:
   Command() = default;
   virtual ~Command();
 
-  Command(const Command&);
-  Command& operator=(const Command&);
+  Command(const Command&) = default;
+  Command& operator=(const Command& rhs);
   Command(Command&&) = default;
   Command& operator=(Command&&) = default;
 
@@ -111,7 +107,7 @@
    * @param duration the timeout duration
    * @return the command with the timeout added
    */
-  ParallelRaceGroup WithTimeout(units::second_t duration) &&;
+  virtual ParallelRaceGroup WithTimeout(units::second_t duration) &&;
 
   /**
    * Decorates this command with an interrupt condition.  If the specified
@@ -122,7 +118,7 @@
    * @param condition the interrupt condition
    * @return the command with the interrupt condition added
    */
-  ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
+  virtual ParallelRaceGroup WithInterrupt(std::function<bool()> condition) &&;
 
   /**
    * Decorates this command with a runnable to run before this command starts.
@@ -131,7 +127,7 @@
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup BeforeStarting(
+  virtual SequentialCommandGroup BeforeStarting(
       std::function<void()> toRun,
       std::initializer_list<Subsystem*> requirements) &&;
 
@@ -142,9 +138,9 @@
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup BeforeStarting(
+  virtual SequentialCommandGroup BeforeStarting(
       std::function<void()> toRun,
-      wpi::ArrayRef<Subsystem*> requirements = {}) &&;
+      wpi::span<Subsystem* const> requirements = {}) &&;
 
   /**
    * Decorates this command with a runnable to run after the command finishes.
@@ -153,7 +149,7 @@
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup AndThen(
+  virtual SequentialCommandGroup AndThen(
       std::function<void()> toRun,
       std::initializer_list<Subsystem*> requirements) &&;
 
@@ -164,9 +160,9 @@
    * @param requirements the required subsystems
    * @return the decorated command
    */
-  SequentialCommandGroup AndThen(
+  virtual SequentialCommandGroup AndThen(
       std::function<void()> toRun,
-      wpi::ArrayRef<Subsystem*> requirements = {}) &&;
+      wpi::span<Subsystem* const> requirements = {}) &&;
 
   /**
    * Decorates this command to run perpetually, ignoring its ordinary end
@@ -174,17 +170,17 @@
    *
    * @return the decorated command
    */
-  PerpetualCommand Perpetually() &&;
+  virtual PerpetualCommand Perpetually() &&;
 
   /**
-   * Decorates this command to run "by proxy" by wrapping it in a {@link
-   * ProxyScheduleCommand}. This is useful for "forking off" from command groups
+   * Decorates this command to run "by proxy" by wrapping it in a
+   * ProxyScheduleCommand. This is useful for "forking off" from command groups
    * when the user does not wish to extend the command's requirements to the
    * entire command group.
    *
    * @return the decorated command
    */
-  ProxyScheduleCommand AsProxy();
+  virtual ProxyScheduleCommand AsProxy();
 
   /**
    * Schedules this command.
@@ -215,11 +211,9 @@
   bool IsScheduled() const;
 
   /**
-   * Whether the command requires a given subsystem.  Named "hasRequirement"
-   * rather than "requires" to avoid confusion with
-   * {@link
-   * edu.wpi.first.wpilibj.command.Command#requires(edu.wpi.first.wpilibj.command.Subsystem)}
-   *  - this may be able to be changed in a few years.
+   * Whether the command requires a given subsystem.  Named "HasRequirement"
+   * rather than "requires" to avoid confusion with Command::Requires(Subsystem)
+   * -- this may be able to be changed in a few years.
    *
    * @param requirement the subsystem to inquire about
    * @return whether the subsystem is required
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
index 481a51a..9a498bc 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandBase.h
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <initializer_list>
 #include <string>
+#include <string_view>
 
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallSet.h>
-#include <wpi/Twine.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+#include <wpi/span.h>
 
 #include "frc2/command/Command.h"
 
@@ -23,25 +20,42 @@
  * A Sendable base class for Commands.
  */
 class CommandBase : public Command,
-                    public frc::Sendable,
-                    public frc::SendableHelper<CommandBase> {
+                    public wpi::Sendable,
+                    public wpi::SendableHelper<CommandBase> {
  public:
   /**
-   * Adds the specified requirements to the command.
+   * Adds the specified Subsystem requirements to the command.
    *
-   * @param requirements the requirements to add
+   * @param requirements the Subsystem requirements to add
    */
   void AddRequirements(std::initializer_list<Subsystem*> requirements);
 
   /**
-   * Adds the specified requirements to the command.
+   * Adds the specified Subsystem requirements to the command.
    *
-   * @param requirements the requirements to add
+   * @param requirements the Subsystem requirements to add
    */
-  void AddRequirements(wpi::ArrayRef<Subsystem*> requirements);
+  void AddRequirements(wpi::span<Subsystem* const> requirements);
 
+  /**
+   * Adds the specified Subsystem requirements to the command.
+   *
+   * @param requirements the Subsystem requirements to add
+   */
   void AddRequirements(wpi::SmallSet<Subsystem*, 4> requirements);
 
+  /**
+   * Adds the specified Subsystem requirement to the command.
+   *
+   * @param requirement the Subsystem requirement to add
+   */
+  void AddRequirements(Subsystem* requirement);
+
+  /**
+   * Gets the Subsystem requirements of the command.
+   *
+   * @return the Command's Subsystem requirements
+   */
   wpi::SmallSet<Subsystem*, 4> GetRequirements() const override;
 
   /**
@@ -49,7 +63,7 @@
    *
    * @param name name
    */
-  void SetName(const wpi::Twine& name);
+  void SetName(std::string_view name);
 
   /**
    * Gets the name of this Command.
@@ -70,9 +84,9 @@
    *
    * @param subsystem subsystem name
    */
-  void SetSubsystem(const wpi::Twine& subsystem);
+  void SetSubsystem(std::string_view subsystem);
 
-  void InitSendable(frc::SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  protected:
   CommandBase();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
index 8b04b4c..6a84ee0 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandGroupBase.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <memory>
 #include <vector>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 
@@ -28,10 +25,19 @@
    * Requires that the specified command not have been already allocated to a
    * CommandGroup. Reports an error if the command is already grouped.
    *
-   * @param commands The command to check
+   * @param command The command to check
    * @return True if all the command is ungrouped.
    */
-  static bool RequireUngrouped(Command& command);
+  static bool RequireUngrouped(const Command& command);
+
+  /**
+   * Requires that the specified command not have been already allocated to a
+   * CommandGroup. Reports an error if the command is already grouped.
+   *
+   * @param command The command to check
+   * @return True if all the command is ungrouped.
+   */
+  static bool RequireUngrouped(const Command* command);
 
   /**
    * Requires that the specified commands not have been already allocated to a
@@ -40,7 +46,8 @@
    * @param commands The commands to check
    * @return True if all the commands are ungrouped.
    */
-  static bool RequireUngrouped(wpi::ArrayRef<std::unique_ptr<Command>>);
+  static bool RequireUngrouped(
+      wpi::span<const std::unique_ptr<Command>> commands);
 
   /**
    * Requires that the specified commands not have been already allocated to a
@@ -49,7 +56,7 @@
    * @param commands The commands to check
    * @return True if all the commands are ungrouped.
    */
-  static bool RequireUngrouped(std::initializer_list<Command*>);
+  static bool RequireUngrouped(std::initializer_list<const Command*> commands);
 
   /**
    * Adds the given commands to the command group.
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
index f78a848..5709f0c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandHelper.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
index 4711695..8976e85 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,14 +8,13 @@
 #include <memory>
 #include <utility>
 
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
+#include <frc/Errors.h>
 #include <frc/Watchdog.h>
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
+#include <networktables/NTSendable.h>
 #include <units/time.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/FunctionExtras.h>
+#include <wpi/sendable/SendableHelper.h>
+#include <wpi/span.h>
 
 namespace frc2 {
 class Command;
@@ -31,9 +27,8 @@
  * with the scheduler using RegisterSubsystem() in order for their Periodic()
  * methods to be called and for their default commands to be scheduled.
  */
-class CommandScheduler final : public frc::Sendable,
-                               public frc::ErrorBase,
-                               public frc::SendableHelper<CommandScheduler> {
+class CommandScheduler final : public nt::NTSendable,
+                               public wpi::SendableHelper<CommandScheduler> {
  public:
   /**
    * Returns the Scheduler instance.
@@ -42,7 +37,7 @@
    */
   static CommandScheduler& GetInstance();
 
-  ~CommandScheduler();
+  ~CommandScheduler() override;
   CommandScheduler(const CommandScheduler&) = delete;
   CommandScheduler& operator=(const CommandScheduler&) = delete;
 
@@ -97,7 +92,7 @@
    * @param interruptible whether the commands should be interruptible
    * @param commands      the commands to schedule
    */
-  void Schedule(bool interruptible, wpi::ArrayRef<Command*> commands);
+  void Schedule(bool interruptible, wpi::span<Command* const> commands);
 
   /**
    * Schedules multiple commands for execution.  Does nothing if the command is
@@ -117,7 +112,7 @@
    *
    * @param commands the commands to schedule
    */
-  void Schedule(wpi::ArrayRef<Command*> commands);
+  void Schedule(wpi::span<Command* const> commands);
 
   /**
    * Schedules multiple commands for execution, with interruptible defaulted to
@@ -165,10 +160,10 @@
   void UnregisterSubsystem(Subsystem* subsystem);
 
   void RegisterSubsystem(std::initializer_list<Subsystem*> subsystems);
-  void RegisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
+  void RegisterSubsystem(wpi::span<Subsystem* const> subsystems);
 
   void UnregisterSubsystem(std::initializer_list<Subsystem*> subsystems);
-  void UnregisterSubsystem(wpi::ArrayRef<Subsystem*> subsystems);
+  void UnregisterSubsystem(wpi::span<Subsystem* const> subsystems);
 
   /**
    * Sets the default command for a subsystem.  Registers that subsystem if it
@@ -185,14 +180,12 @@
                          Command, std::remove_reference_t<T>>>>
   void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
     if (!defaultCommand.HasRequirement(subsystem)) {
-      wpi_setWPIErrorWithContext(
-          CommandIllegalUse, "Default commands must require their subsystem!");
-      return;
+      throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                          "Default commands must require their subsystem!");
     }
     if (defaultCommand.IsFinished()) {
-      wpi_setWPIErrorWithContext(CommandIllegalUse,
-                                 "Default commands should not end!");
-      return;
+      throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
+                          "Default commands should not end!");
     }
     SetDefaultCommandImpl(subsystem,
                           std::make_unique<std::remove_reference_t<T>>(
@@ -216,7 +209,7 @@
    * <p>Commands will be canceled even if they are not scheduled as
    * interruptible.
    *
-   * @param commands the commands to cancel
+   * @param command the command to cancel
    */
   void Cancel(Command* command);
 
@@ -230,7 +223,7 @@
    *
    * @param commands the commands to cancel
    */
-  void Cancel(wpi::ArrayRef<Command*> commands);
+  void Cancel(wpi::span<Command* const> commands);
 
   /**
    * Cancels commands. The scheduler will only call Command::End()
@@ -256,9 +249,9 @@
    * them.
    *
    * @param command the command to query
-   * @return the time since the command was scheduled, in seconds
+   * @return the time since the command was scheduled
    */
-  double TimeSinceScheduled(const Command* command) const;
+  units::second_t TimeSinceScheduled(const Command* command) const;
 
   /**
    * Whether the given commands are running.  Note that this only works on
@@ -268,7 +261,7 @@
    * @param commands the command to query
    * @return whether the command is currently scheduled
    */
-  bool IsScheduled(wpi::ArrayRef<const Command*> commands) const;
+  bool IsScheduled(wpi::span<const Command* const> commands) const;
 
   /**
    * Whether the given commands are running.  Note that this only works on
@@ -285,7 +278,7 @@
    * that are directly scheduled by the scheduler; it will not work on commands
    * inside of CommandGroups, as the scheduler does not see them.
    *
-   * @param commands the command to query
+   * @param command the command to query
    * @return whether the command is currently scheduled
    */
   bool IsScheduled(const Command* command) const;
@@ -339,7 +332,7 @@
    */
   void OnCommandFinish(Action action);
 
-  void InitSendable(frc::SendableBuilder& builder) override;
+  void InitSendable(nt::NTSendableBuilder& builder) override;
 
  private:
   // Constructor; private as this is a singleton
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
index 41fc5d0..4c731b3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/CommandState.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <units/time.h>
+
 namespace frc2 {
 /**
  * Class that holds scheduling state for a command.  Used internally by the
@@ -21,10 +20,10 @@
   bool IsInterruptible() const { return m_interruptible; }
 
   // The time since this command was initialized.
-  double TimeSinceInitialized() const;
+  units::second_t TimeSinceInitialized() const;
 
  private:
-  double m_startTime = -1;
+  units::second_t m_startTime = -1_s;
   bool m_interruptible;
 
   void StartTiming();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
index af9f113..2c37200 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ConditionalCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
index eb88b5e..31b2ada 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/FunctionalCommand.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <initializer_list>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -55,7 +52,7 @@
                     std::function<void()> onExecute,
                     std::function<void(bool)> onEnd,
                     std::function<bool()> isFinished,
-                    wpi::ArrayRef<Subsystem*> requirements = {});
+                    wpi::span<Subsystem* const> requirements = {});
 
   FunctionalCommand(FunctionalCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
index 3f29b17..3aba587 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/InstantCommand.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <initializer_list>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -40,8 +37,8 @@
    * @param toRun        the Runnable to run
    * @param requirements the subsystems required by this command
    */
-  InstantCommand(std::function<void()> toRun,
-                 wpi::ArrayRef<Subsystem*> requirements = {});
+  explicit InstantCommand(std::function<void()> toRun,
+                          wpi::span<Subsystem* const> requirements = {});
 
   InstantCommand(InstantCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
index 87b16ee..29f0fd4 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/MecanumControllerCommand.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 #include <functional>
 #include <initializer_list>
 #include <memory>
 
+#include <frc/Timer.h>
 #include <frc/controller/HolonomicDriveController.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
@@ -23,19 +21,18 @@
 #include <units/length.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "CommandBase.h"
 #include "CommandHelper.h"
-#include "frc2/Timer.h"
 
 #pragma once
 
 namespace frc2 {
 /**
- * A command that uses two PID controllers ({@link PIDController}) and a
- * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
- * {@link Trajectory} with a mecanum drive.
+ * A command that uses two PID controllers (PIDController) and a profiled PID
+ * controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a
+ * mecanum drive.
  *
  * <p>The command handles trajectory-following,
  * Velocity PID calculations, and feedforwards internally. This
@@ -207,7 +204,7 @@
       std::function<void(units::volt_t, units::volt_t, units::volt_t,
                          units::volt_t)>
           output,
-      wpi::ArrayRef<Subsystem*> requirements = {});
+      wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow
@@ -260,7 +257,7 @@
       std::function<void(units::volt_t, units::volt_t, units::volt_t,
                          units::volt_t)>
           output,
-      wpi::ArrayRef<Subsystem*> requirements = {});
+      wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow
@@ -376,7 +373,7 @@
                          units::meters_per_second_t,
                          units::meters_per_second_t)>
           output,
-      wpi::ArrayRef<Subsystem*> requirements = {});
+      wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Constructs a new MecanumControllerCommand that when executed will follow
@@ -416,7 +413,7 @@
                          units::meters_per_second_t,
                          units::meters_per_second_t)>
           output,
-      wpi::ArrayRef<Subsystem*> requirements = {});
+      wpi::span<Subsystem* const> requirements = {});
 
   void Initialize() override;
 
@@ -447,7 +444,7 @@
       m_outputVolts;
 
   bool m_usePID;
-  frc2::Timer m_timer;
+  frc::Timer m_timer;
   frc::MecanumDriveWheelSpeeds m_prevSpeeds;
   units::second_t m_prevTime;
 };
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
index 1c6d776..ec68fe7 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/NotifierCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,7 +9,7 @@
 
 #include <frc/Notifier.h>
 #include <units/time.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -20,9 +17,9 @@
 namespace frc2 {
 /**
  * A command that starts a notifier to run the given runnable periodically in a
- * separate thread. Has no end condition as-is; either subclass it or use {@link
- * Command#withTimeout(double)} or
- * {@link Command#withInterrupt(BooleanSupplier)} to give it one.
+ * separate thread. Has no end condition as-is; either subclass it or use
+ * Command::WithTimeout(double) or Command::WithInterrupt(BooleanSupplier) to
+ * give it one.
  *
  * <p>WARNING: Do not use this class unless you are confident in your ability to
  * make the executed code thread-safe.  If you do not know what "thread-safe"
@@ -48,7 +45,7 @@
    * @param requirements the subsystems required by this command
    */
   NotifierCommand(std::function<void()> toRun, units::second_t period,
-                  wpi::ArrayRef<Subsystem*> requirements = {});
+                  wpi::span<Subsystem* const> requirements = {});
 
   NotifierCommand(NotifierCommand&& other);
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
index 8653313..31baf9e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,6 +8,7 @@
 #include <initializer_list>
 
 #include <frc/controller/PIDController.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -56,7 +54,7 @@
              std::function<double()> measurementSource,
              std::function<double()> setpointSource,
              std::function<void(double)> useOutput,
-             wpi::ArrayRef<Subsystem*> requirements = {});
+             wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Creates a new PIDCommand, which controls the given output with a
@@ -86,7 +84,7 @@
   PIDCommand(PIDController controller,
              std::function<double()> measurementSource, double setpoint,
              std::function<void(double)> useOutput,
-             wpi::ArrayRef<Subsystem*> requirements = {});
+             wpi::span<Subsystem* const> requirements = {});
 
   PIDCommand(PIDCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
index 608fe56..c8a53db 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PIDSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
index 5b0f0da..342265a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelCommandGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -86,7 +83,7 @@
   bool RunsWhenDisabled() const override;
 
  private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
 
   std::vector<std::pair<std::unique_ptr<Command>, bool>> m_commands;
   bool m_runWhenDisabled{true};
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
index 4debe75..af78c3b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelDeadlineGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -94,7 +91,7 @@
   bool RunsWhenDisabled() const override;
 
  private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
 
   void SetDeadline(std::unique_ptr<Command>&& deadline);
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
index 167dee4..3ba3907 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ParallelRaceGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -74,7 +71,7 @@
   bool RunsWhenDisabled() const override;
 
  private:
-  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) override;
+  void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
 
   std::vector<std::unique_ptr<Command>> m_commands;
   bool m_runWhenDisabled{true};
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
index 4a93704..a288ae2 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PerpetualCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -68,6 +65,8 @@
 
   void End(bool interrupted) override;
 
+  PerpetualCommand Perpetually() && override;
+
  private:
   std::unique_ptr<Command> m_command;
 };
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
index 7b706a0..536e77f 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/PrintCommand.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc2/command/CommandHelper.h"
 #include "frc2/command/InstantCommand.h"
@@ -23,7 +20,7 @@
    *
    * @param message the message to print
    */
-  explicit PrintCommand(const wpi::Twine& message);
+  explicit PrintCommand(std::string_view message);
 
   PrintCommand(PrintCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
index 5bc4978..b02c81d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -13,7 +10,7 @@
 
 #include <frc/controller/ProfiledPIDController.h>
 #include <units/time.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -73,7 +70,7 @@
                      std::function<Distance_t()> measurementSource,
                      std::function<State()> goalSource,
                      std::function<void(double, State)> useOutput,
-                     wpi::ArrayRef<Subsystem*> requirements = {})
+                     wpi::span<Subsystem* const> requirements = {})
       : m_controller{controller},
         m_measurement{std::move(measurementSource)},
         m_goal{std::move(goalSource)},
@@ -117,7 +114,7 @@
                      std::function<Distance_t()> measurementSource,
                      std::function<Distance_t()> goalSource,
                      std::function<void(double, State)> useOutput,
-                     wpi::ArrayRef<Subsystem*> requirements = {})
+                     wpi::span<Subsystem* const> requirements = {})
       : ProfiledPIDCommand(
             controller, measurementSource,
             [&goalSource]() {
@@ -156,7 +153,7 @@
   ProfiledPIDCommand(frc::ProfiledPIDController<Distance> controller,
                      std::function<Distance_t()> measurementSource, State goal,
                      std::function<void(double, State)> useOutput,
-                     wpi::ArrayRef<Subsystem*> requirements = {})
+                     wpi::span<Subsystem* const> requirements = {})
       : ProfiledPIDCommand(
             controller, measurementSource, [goal] { return goal; }, useOutput,
             requirements) {}
@@ -194,7 +191,7 @@
                      std::function<Distance_t()> measurementSource,
                      Distance_t goal,
                      std::function<void(double, State)> useOutput,
-                     wpi::ArrayRef<Subsystem*> requirements = {})
+                     wpi::span<Subsystem* const> requirements = {})
       : ProfiledPIDCommand(
             controller, measurementSource, [goal] { return goal; }, useOutput,
             requirements) {}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
index 2bbe149..c8639b6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProfiledPIDSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
index ef5204f..ad603f9 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ProxyScheduleCommand.h
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -30,7 +27,9 @@
    *
    * @param toSchedule the commands to schedule
    */
-  explicit ProxyScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+  explicit ProxyScheduleCommand(wpi::span<Command* const> toSchedule);
+
+  explicit ProxyScheduleCommand(Command* toSchedule);
 
   ProxyScheduleCommand(ProxyScheduleCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
index 21d5cfd..5f84997 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RamseteCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,6 +8,7 @@
 #include <initializer_list>
 #include <memory>
 
+#include <frc/Timer.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
@@ -19,9 +17,8 @@
 #include <frc/trajectory/Trajectory.h>
 #include <units/length.h>
 #include <units/voltage.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
-#include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -117,7 +114,7 @@
                  frc2::PIDController leftController,
                  frc2::PIDController rightController,
                  std::function<void(units::volt_t, units::volt_t)> output,
-                 wpi::ArrayRef<Subsystem*> requirements = {});
+                 wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Constructs a new RamseteCommand that, when executed, will follow the
@@ -165,7 +162,7 @@
                  std::function<void(units::meters_per_second_t,
                                     units::meters_per_second_t)>
                      output,
-                 wpi::ArrayRef<Subsystem*> requirements = {});
+                 wpi::span<Subsystem* const> requirements = {});
 
   void Initialize() override;
 
@@ -188,7 +185,7 @@
   std::function<void(units::meters_per_second_t, units::meters_per_second_t)>
       m_outputVel;
 
-  Timer m_timer;
+  frc::Timer m_timer;
   units::second_t m_prevTime;
   frc::DifferentialDriveWheelSpeeds m_prevSpeeds;
   bool m_usePID;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
index 2036230..510840e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/RunCommand.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <initializer_list>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -41,14 +38,14 @@
    * @param toRun        the Runnable to run
    * @param requirements the subsystems to require
    */
-  RunCommand(std::function<void()> toRun,
-             wpi::ArrayRef<Subsystem*> requirements = {});
+  explicit RunCommand(std::function<void()> toRun,
+                      wpi::span<Subsystem* const> requirements = {});
 
   RunCommand(RunCommand&& other) = default;
 
   RunCommand(const RunCommand& other) = default;
 
-  void Execute();
+  void Execute() override;
 
  protected:
   std::function<void()> m_toRun;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
index c2824ca..ca476d0 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/ScheduleCommand.h
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -29,7 +26,9 @@
    *
    * @param toSchedule the commands to schedule
    */
-  explicit ScheduleCommand(wpi::ArrayRef<Command*> toSchedule);
+  explicit ScheduleCommand(wpi::span<Command* const> toSchedule);
+
+  explicit ScheduleCommand(Command* toSchedule);
 
   ScheduleCommand(ScheduleCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
index e1074f4..b4e7f89 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SelectCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -51,8 +48,8 @@
   template <class... Types,
             typename = std::enable_if_t<std::conjunction_v<
                 std::is_base_of<Command, std::remove_reference_t<Types>>...>>>
-  SelectCommand(std::function<Key()> selector,
-                std::pair<Key, Types>... commands)
+  explicit SelectCommand(std::function<Key()> selector,
+                         std::pair<Key, Types>... commands)
       : m_selector{std::move(selector)} {
     std::vector<std::pair<Key, std::unique_ptr<Command>>> foo;
 
@@ -62,7 +59,7 @@
      ...);
 
     for (auto&& command : foo) {
-      if (!CommandGroupBase::RequireUngrouped(command.second)) {
+      if (!CommandGroupBase::RequireUngrouped(*command.second)) {
         return;
       }
     }
@@ -79,7 +76,7 @@
       std::vector<std::pair<Key, std::unique_ptr<Command>>>&& commands)
       : m_selector{std::move(selector)} {
     for (auto&& command : commands) {
-      if (!CommandGroupBase::RequireUngrouped(command.second)) {
+      if (!CommandGroupBase::RequireUngrouped(*command.second)) {
         return;
       }
     }
@@ -102,7 +99,8 @@
    *
    * @param toRun a supplier providing the command to run
    */
-  explicit SelectCommand(std::function<Command*()> toRun) : m_toRun{toRun} {}
+  explicit SelectCommand(std::function<Command*()> toRun)
+      : m_toRun{std::move(toRun)} {}
 
   SelectCommand(SelectCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
index c49356e..d80c07b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SequentialCommandGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,9 +15,7 @@
 #include <utility>
 #include <vector>
 
-#include <frc/ErrorBase.h>
-#include <frc/WPIErrors.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandGroupBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -91,6 +86,16 @@
 
   bool RunsWhenDisabled() const override;
 
+  SequentialCommandGroup BeforeStarting(
+      std::function<void()> toRun,
+      wpi::span<Subsystem* const> requirements = {}) &&
+      override;
+
+  SequentialCommandGroup AndThen(
+      std::function<void()> toRun,
+      wpi::span<Subsystem* const> requirements = {}) &&
+      override;
+
  private:
   void AddCommands(std::vector<std::unique_ptr<Command>>&& commands) final;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
index d21f572..d40fbec 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SetUtilities.h
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
+#include <wpi/span.h>
 
 namespace frc2 {
 template <typename T>
-void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::ArrayRef<T*> toAdd) {
+void SetInsert(wpi::SmallVectorImpl<T*>& vector, wpi::span<T* const> toAdd) {
   for (auto addCommand : toAdd) {
     bool exists = false;
     for (auto existingCommand : vector) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
index 9b1252e..d364696 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/StartEndCommand.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <initializer_list>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
@@ -45,7 +42,7 @@
    * @param requirements the subsystems required by this command
    */
   StartEndCommand(std::function<void()> onInit, std::function<void()> onEnd,
-                  wpi::ArrayRef<Subsystem*> requirements = {});
+                  wpi::span<Subsystem* const> requirements = {});
 
   StartEndCommand(StartEndCommand&& other) = default;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
index 3f8a278..4744463 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/Subsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
index d75ad0d..748271a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SubsystemBase.h
@@ -1,16 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <string>
+#include <string_view>
 
-#include <frc/smartdashboard/Sendable.h>
-#include <frc/smartdashboard/SendableHelper.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc2/command/Subsystem.h"
 
@@ -20,10 +18,10 @@
  * provides a more intuitive method for setting the default command.
  */
 class SubsystemBase : public Subsystem,
-                      public frc::Sendable,
-                      public frc::SendableHelper<SubsystemBase> {
+                      public wpi::Sendable,
+                      public wpi::SendableHelper<SubsystemBase> {
  public:
-  void InitSendable(frc::SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
   /**
    * Gets the name of this Subsystem.
@@ -37,7 +35,7 @@
    *
    * @param name name
    */
-  void SetName(const wpi::Twine& name);
+  void SetName(std::string_view name);
 
   /**
    * Gets the subsystem name of this Subsystem.
@@ -49,9 +47,9 @@
   /**
    * Sets the subsystem name of this Subsystem.
    *
-   * @param subsystem subsystem name
+   * @param name subsystem name
    */
-  void SetSubsystem(const wpi::Twine& name);
+  void SetSubsystem(std::string_view name);
 
   /**
    * Associate a Sendable with this Subsystem.
@@ -60,7 +58,7 @@
    * @param name name to give child
    * @param child sendable
    */
-  void AddChild(std::string name, frc::Sendable* child);
+  void AddChild(std::string name, wpi::Sendable* child);
 
  protected:
   SubsystemBase();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
index 6036ce8..9620fef 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 #include <functional>
 #include <initializer_list>
 #include <memory>
 
+#include <frc/Timer.h>
 #include <frc/controller/HolonomicDriveController.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
@@ -21,20 +19,19 @@
 #include <units/length.h>
 #include <units/time.h>
 #include <units/voltage.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "CommandBase.h"
 #include "CommandHelper.h"
-#include "frc2/Timer.h"
 
 #pragma once
 
 namespace frc2 {
 
 /**
- * A command that uses two PID controllers ({@link PIDController}) and a
- * ProfiledPIDController ({@link ProfiledPIDController}) to follow a trajectory
- * {@link Trajectory} with a swerve drive.
+ * A command that uses two PID controllers (PIDController) and a profiled PID
+ * controller (ProfiledPIDController) to follow a trajectory (Trajectory) with a
+ * swerve drive.
  *
  * <p>The command handles trajectory-following, Velocity PID calculations, and
  * feedforwards internally. This is intended to be a more-or-less "complete
@@ -170,7 +167,7 @@
       std::function<frc::Rotation2d()> desiredRotation,
       std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
           output,
-      wpi::ArrayRef<Subsystem*> requirements = {});
+      wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Constructs a new SwerveControllerCommand that when executed will follow the
@@ -208,7 +205,7 @@
       frc::ProfiledPIDController<units::radians> thetaController,
       std::function<void(std::array<frc::SwerveModuleState, NumModules>)>
           output,
-      wpi::ArrayRef<Subsystem*> requirements = {});
+      wpi::span<Subsystem* const> requirements = {});
 
   void Initialize() override;
 
@@ -228,7 +225,7 @@
 
   std::function<frc::Rotation2d()> m_desiredRotation;
 
-  frc2::Timer m_timer;
+  frc::Timer m_timer;
   units::second_t m_prevTime;
   frc::Rotation2d m_finalRotation;
 };
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
index 616244e..1f65b78 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/SwerveControllerCommand.inc
@@ -1,13 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <utility>
+
+#include "frc2/command/SwerveControllerCommand.h"
 
 namespace frc2 {
 
@@ -20,11 +20,11 @@
     std::function<frc::Rotation2d()> desiredRotation,
     std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
-      m_desiredRotation(desiredRotation),
+      m_desiredRotation(std::move(desiredRotation)),
       m_outputStates(output) {
   this->AddRequirements(requirements);
 }
@@ -37,8 +37,8 @@
     frc::ProfiledPIDController<units::radians> thetaController,
     std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
     std::initializer_list<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
       m_outputStates(output) {
@@ -56,12 +56,12 @@
     frc::ProfiledPIDController<units::radians> thetaController,
     std::function<frc::Rotation2d()> desiredRotation,
     std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
-      m_desiredRotation(desiredRotation),
+      m_desiredRotation(std::move(desiredRotation)),
       m_outputStates(output) {
   this->AddRequirements(requirements);
 }
@@ -73,9 +73,9 @@
     frc2::PIDController xController, frc2::PIDController yController,
     frc::ProfiledPIDController<units::radians> thetaController,
     std::function<void(std::array<frc::SwerveModuleState, NumModules>)> output,
-    wpi::ArrayRef<Subsystem*> requirements)
-    : m_trajectory(trajectory),
-      m_pose(pose),
+    wpi::span<Subsystem* const> requirements)
+    : m_trajectory(std::move(trajectory)),
+      m_pose(std::move(pose)),
       m_kinematics(kinematics),
       m_controller(xController, yController, thetaController),
       m_outputStates(output) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
index 70bf7f9..1d8d2c5 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileCommand.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <initializer_list>
 
+#include <frc/Timer.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
-#include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -38,8 +35,9 @@
    * Creates a new TrapezoidProfileCommand that will execute the given
    * TrapezoidalProfile. Output will be piped to the provided consumer function.
    *
-   * @param profile The motion profile to execute.
-   * @param output  The consumer for the profile output.
+   * @param profile      The motion profile to execute.
+   * @param output       The consumer for the profile output.
+   * @param requirements The list of requirements.
    */
   TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                           std::function<void(State)> output,
@@ -52,12 +50,13 @@
    * Creates a new TrapezoidProfileCommand that will execute the given
    * TrapezoidalProfile. Output will be piped to the provided consumer function.
    *
-   * @param profile The motion profile to execute.
-   * @param output  The consumer for the profile output.
+   * @param profile      The motion profile to execute.
+   * @param output       The consumer for the profile output.
+   * @param requirements The list of requirements.
    */
   TrapezoidProfileCommand(frc::TrapezoidProfile<Distance> profile,
                           std::function<void(State)> output,
-                          wpi::ArrayRef<Subsystem*> requirements = {})
+                          wpi::span<Subsystem* const> requirements = {})
       : m_profile(profile), m_output(output) {
     this->AddRequirements(requirements);
   }
@@ -79,7 +78,7 @@
   frc::TrapezoidProfile<Distance> m_profile;
   std::function<void(State)> m_output;
 
-  Timer m_timer;
+  frc::Timer m_timer;
 };
 
 }  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
index 08336f1..fbebaf3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/TrapezoidProfileSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
index 568db65..82249c3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitCommand.h
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <frc/Timer.h>
 #include <units/time.h>
 
-#include "frc2/Timer.h"
 #include "frc2/command/CommandBase.h"
 #include "frc2/command/CommandHelper.h"
 
@@ -42,7 +39,7 @@
   bool RunsWhenDisabled() const override;
 
  protected:
-  Timer m_timer;
+  frc::Timer m_timer;
 
  private:
   units::second_t m_duration;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
index 741d3eb..ca74218 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/WaitUntilCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
index 5e01edf..bab7a0f 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Button.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <initializer_list>
 #include <utility>
 
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
 #include "Trigger.h"
 
@@ -28,7 +25,7 @@
   /**
    * Create a new button that is pressed when the given condition is true.
    *
-   * @param isActive Whether the button is pressed.
+   * @param isPressed Whether the button is pressed.
    */
   explicit Button(std::function<bool()> isPressed);
 
@@ -82,7 +79,7 @@
    * @param requirements the required subsystems.
    */
   Button WhenPressed(std::function<void()> toRun,
-                     wpi::ArrayRef<Subsystem*> requirements = {});
+                     wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Binds a command to be started repeatedly while the button is pressed, and
@@ -128,7 +125,7 @@
    * @param requirements the required subsystems.
    */
   Button WhileHeld(std::function<void()> toRun,
-                   wpi::ArrayRef<Subsystem*> requirements = {});
+                   wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Binds a command to be started when the button is pressed, and canceled
@@ -202,7 +199,7 @@
    * @param requirements the required subsystems.
    */
   Button WhenReleased(std::function<void()> toRun,
-                      wpi::ArrayRef<Subsystem*> requirements = {});
+                      wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Binds a command to start when the button is pressed, and be canceled when
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
index 421562e..4cd4689 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/JoystickButton.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 #include <frc/GenericHID.h>
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h
index 9c09dd1..191e55d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/NetworkButton.h
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
-#include <wpi/Twine.h>
 
 #include "Button.h"
 
 namespace frc2 {
 /**
- * A {@link Button} that uses a {@link NetworkTable} boolean field.
+ * A Button that uses a NetworkTable boolean field.
  */
 class NetworkButton : public Button {
  public:
@@ -35,7 +32,7 @@
    * @param field The field that is the value.
    */
   NetworkButton(std::shared_ptr<nt::NetworkTable> table,
-                const wpi::Twine& field);
+                std::string_view field);
 
   /**
    * Creates a NetworkButton that commands can be bound to.
@@ -43,6 +40,6 @@
    * @param table The table where the networktable value is located.
    * @param field The field that is the value.
    */
-  NetworkButton(const wpi::Twine& table, const wpi::Twine& field);
+  NetworkButton(std::string_view table, std::string_view field);
 };
 }  // namespace frc2
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
index e4f8f0e..cd8b62c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/POVButton.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 #include <frc/GenericHID.h>
diff --git a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
index ecfc4d1..3213d4b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/main/native/include/frc2/command/button/Trigger.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,7 +9,8 @@
 #include <memory>
 #include <utility>
 
-#include <wpi/ArrayRef.h>
+#include <units/time.h>
+#include <wpi/span.h>
 
 #include "frc2/command/Command.h"
 #include "frc2/command/CommandScheduler.h"
@@ -92,7 +90,7 @@
    * Binds a runnable to execute when the trigger becomes active.
    *
    * @param toRun the runnable to execute.
-   * @paaram requirements the required subsystems.
+   * @param requirements the required subsystems.
    */
   Trigger WhenActive(std::function<void()> toRun,
                      std::initializer_list<Subsystem*> requirements);
@@ -101,10 +99,10 @@
    * Binds a runnable to execute when the trigger becomes active.
    *
    * @param toRun the runnable to execute.
-   * @paaram requirements the required subsystems.
+   * @param requirements the required subsystems.
    */
   Trigger WhenActive(std::function<void()> toRun,
-                     wpi::ArrayRef<Subsystem*> requirements = {});
+                     wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Binds a command to be started repeatedly while the trigger is active, and
@@ -164,7 +162,7 @@
    * @param requirements the required subsystems.
    */
   Trigger WhileActiveContinous(std::function<void()> toRun,
-                               wpi::ArrayRef<Subsystem*> requirements = {});
+                               wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Binds a command to be started when the trigger becomes active, and
@@ -264,7 +262,7 @@
    * @param requirements the required subsystems.
    */
   Trigger WhenInactive(std::function<void()> toRun,
-                       wpi::ArrayRef<Subsystem*> requirements = {});
+                       wpi::span<Subsystem* const> requirements = {});
 
   /**
    * Binds a command to start when the trigger becomes active, and be canceled
@@ -348,6 +346,15 @@
     return Trigger([*this] { return !m_isActive(); });
   }
 
+  /**
+   * Creates a new debounced trigger from this trigger - it will become active
+   * when this trigger has been active for longer than the specified period.
+   *
+   * @param debounceTime the debounce period
+   * @return the debounced trigger
+   */
+  Trigger Debounce(units::second_t debounceTime);
+
  private:
   std::function<bool()> m_isActive;
 };
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
index 0547b66..b0ffc88 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/MockHardwareExtension.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import org.junit.jupiter.api.extension.BeforeAllCallback;
 import org.junit.jupiter.api.extension.ExtensionContext;
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-
 public final class MockHardwareExtension implements BeforeAllCallback {
   private static ExtensionContext getRoot(ExtensionContext context) {
     return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
@@ -21,10 +17,15 @@
 
   @Override
   public void beforeAll(ExtensionContext context) {
-    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initialized", key -> {
-      initializeHardware();
-      return true;
-    }, Boolean.class);
+    getRoot(context)
+        .getStore(Namespace.GLOBAL)
+        .getOrComputeIfAbsent(
+            "HAL Initialized",
+            key -> {
+              initializeHardware();
+              return true;
+            },
+            Boolean.class);
   }
 
   private void initializeHardware() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
index d661598..b2f482c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandDecoratorTest.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.parallel.ResourceLock;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.simulation.SimHooks;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
 
 class CommandDecoratorTest extends CommandTestBase {
   @Test
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
index 82e78c5..8da92c7 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandGroupErrorTest.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 
+import org.junit.jupiter.api.Test;
+
 class CommandGroupErrorTest extends CommandTestBase {
   @Test
   void commandInMultipleGroupsTest() {
-
     MockCommandHolder command1Holder = new MockCommandHolder(true);
     Command command1 = command1Holder.getMock();
     MockCommandHolder command2Holder = new MockCommandHolder(true);
     Command command2 = command2Holder.getMock();
 
-    @SuppressWarnings("PMD.UnusedLocalVariable")
-    Command group = new ParallelCommandGroup(command1, command2);
-    assertThrows(IllegalArgumentException.class,
-        () -> new ParallelCommandGroup(command1, command2));
+    new ParallelCommandGroup(command1, command2);
+    assertThrows(
+        IllegalArgumentException.class, () -> new ParallelCommandGroup(command1, command2));
   }
 
   @Test
@@ -35,11 +30,9 @@
       MockCommandHolder command2Holder = new MockCommandHolder(true);
       Command command2 = command2Holder.getMock();
 
-      @SuppressWarnings("PMD.UnusedLocalVariable")
-      Command group = new ParallelCommandGroup(command1, command2);
+      new ParallelCommandGroup(command1, command2);
 
-      assertThrows(IllegalArgumentException.class,
-          () -> scheduler.schedule(command1));
+      assertThrows(IllegalArgumentException.class, () -> scheduler.schedule(command1));
     }
   }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
index d19331b..b5058c7 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandRequirementsTest.java
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class CommandRequirementsTest extends CommandTestBase {
   @Test
   void requirementInterruptTest() {
@@ -66,13 +63,12 @@
       Subsystem system = new TestSubsystem();
 
       Command missingRequirement = new WaitUntilCommand(() -> false);
-      Command ends = new InstantCommand(() -> {
-      }, system);
+      Command ends = new InstantCommand(() -> {}, system);
 
-      assertThrows(IllegalArgumentException.class,
+      assertThrows(
+          IllegalArgumentException.class,
           () -> scheduler.setDefaultCommand(system, missingRequirement));
-      assertThrows(IllegalArgumentException.class,
-          () -> scheduler.setDefaultCommand(system, ends));
+      assertThrows(IllegalArgumentException.class, () -> scheduler.setDefaultCommand(system, ends));
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
index 74d2d5d..a30a6ca 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandScheduleTest.java
@@ -1,14 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -16,6 +11,8 @@
 import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class CommandScheduleTest extends CommandTestBase {
   @Test
   void instantScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
index 6500f69..88190ed 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/CommandTestBase.java
@@ -1,27 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Set;
-
-import org.junit.jupiter.api.BeforeEach;
-
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-
 import static org.mockito.Mockito.mock;
 import static org.mockito.Mockito.when;
 
-/**
- * Basic setup for all {@link Command tests}."
- */
-@SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import java.util.Set;
+import org.junit.jupiter.api.BeforeEach;
+
+/** Basic setup for all {@link Command tests}." */
 public abstract class CommandTestBase {
+  protected CommandTestBase() {}
+
   @BeforeEach
   void commandSetup() {
     CommandScheduler.getInstance().cancelAll();
@@ -37,8 +31,8 @@
 
     DriverStationSim.setEnabled(enabled);
     DriverStationSim.notifyNewData();
-    DriverStation.getInstance().isNewControlData();
-    while (DriverStation.getInstance().isEnabled() != enabled) {
+    DriverStation.isNewControlData();
+    while (DriverStation.isEnabled() != enabled) {
       try {
         Thread.sleep(1);
       } catch (InterruptedException exception) {
@@ -47,10 +41,9 @@
     }
   }
 
-  public class TestSubsystem extends SubsystemBase {
-  }
+  public static class TestSubsystem extends SubsystemBase {}
 
-  public class MockCommandHolder {
+  public static class MockCommandHolder {
     private final Command m_mockCommand = mock(Command.class);
 
     public MockCommandHolder(boolean runWhenDisabled, Subsystem... requirements) {
@@ -66,10 +59,9 @@
     public void setFinished(boolean finished) {
       when(m_mockCommand.isFinished()).thenReturn(finished);
     }
-
   }
 
-  public class Counter {
+  public static class Counter {
     public int m_counter;
 
     public void increment() {
@@ -77,7 +69,7 @@
     }
   }
 
-  public class ConditionHolder {
+  public static class ConditionHolder {
     private boolean m_condition;
 
     public void setCondition(boolean condition) {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
index 939fa01..ccf4205 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ConditionalCommandTest.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.mockito.Mockito.never;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class ConditionalCommandTest extends CommandTestBase {
   @Test
   void conditionalCommandTest() {
@@ -55,8 +52,7 @@
           new ConditionalCommand(command1, command2, () -> true);
 
       scheduler.schedule(conditionalCommand);
-      scheduler.schedule(new InstantCommand(() -> {
-      }, system3));
+      scheduler.schedule(new InstantCommand(() -> {}, system3));
 
       assertFalse(scheduler.isScheduled(conditionalCommand));
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
index dccf0ff..dc910fc 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/DefaultCommandTest.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class DefaultCommandTest extends CommandTestBase {
   @Test
   void defaultCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
index 484cdf9..e6a073f 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/FunctionalCommandTest.java
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.junit.jupiter.api.Test;
+
 class FunctionalCommandTest extends CommandTestBase {
   @Test
   void functionalCommandScheduleTest() {
@@ -22,8 +19,11 @@
       ConditionHolder cond4 = new ConditionHolder();
 
       FunctionalCommand command =
-          new FunctionalCommand(() -> cond1.setCondition(true), () -> cond2.setCondition(true),
-              interrupted -> cond3.setCondition(true), cond4::getCondition);
+          new FunctionalCommand(
+              () -> cond1.setCondition(true),
+              () -> cond2.setCondition(true),
+              interrupted -> cond3.setCondition(true),
+              cond4::getCondition);
 
       scheduler.schedule(command);
       scheduler.run();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
index e2b9dbe..88c55a6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/InstantCommandTest.java
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.junit.jupiter.api.Test;
+
 class InstantCommandTest extends CommandTestBase {
   @Test
   void instantCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
index 9d9ebfd..ac158c2 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/MecanumControllerCommandTest.java
@@ -1,37 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.ArrayList;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.ArrayList;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 class MecanumControllerCommandTest {
   @BeforeEach
   void setupAll() {
@@ -52,8 +47,8 @@
   private double m_frontRightSpeed;
   private double m_rearRightSpeed;
 
-  private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
-      new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+  private final ProfiledPIDController m_rotController =
+      new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
 
   private static final double kxTolerance = 1 / 12.0;
   private static final double kyTolerance = 1 / 12.0;
@@ -62,14 +57,16 @@
   private static final double kWheelBase = 0.5;
   private static final double kTrackWidth = 0.5;
 
-  private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
-      new Translation2d(kWheelBase / 2, kTrackWidth / 2),
-      new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
-      new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
-      new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(
+          new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+          new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+          new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+          new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
-  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
-      new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+  private final MecanumDriveOdometry m_odometry =
+      new MecanumDriveOdometry(
+          m_kinematics, new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
 
   public void setWheelSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
     this.m_frontLeftSpeed = wheelSpeeds.frontLeftMetersPerSecond;
@@ -79,8 +76,8 @@
   }
 
   public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
-    return new MecanumDriveWheelSpeeds(m_frontLeftSpeed,
-      m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
+    return new MecanumDriveWheelSpeeds(
+        m_frontLeftSpeed, m_frontRightSpeed, m_rearLeftSpeed, m_rearRightSpeed);
   }
 
   public Pose2d getRobotPose() {
@@ -90,7 +87,6 @@
 
   @Test
   @ResourceLock("timing")
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
     final var subsystem = new Subsystem() {};
 
@@ -102,15 +98,17 @@
 
     final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
 
-    final var command = new MecanumControllerCommand(trajectory,
-        this::getRobotPose,
-        m_kinematics,
-        new PIDController(0.6, 0, 0),
-        new PIDController(0.6, 0, 0),
-        m_rotController,
-        8.8,
-        this::setWheelSpeeds,
-        subsystem);
+    final var command =
+        new MecanumControllerCommand(
+            trajectory,
+            this::getRobotPose,
+            m_kinematics,
+            new PIDController(0.6, 0, 0),
+            new PIDController(0.6, 0, 0),
+            m_rotController,
+            8.8,
+            this::setWheelSpeeds,
+            subsystem);
 
     m_timer.reset();
     m_timer.start();
@@ -125,12 +123,12 @@
     command.end(true);
 
     assertAll(
-        () -> assertEquals(endState.poseMeters.getX(),
-          getRobotPose().getX(), kxTolerance),
-        () -> assertEquals(endState.poseMeters.getY(),
-          getRobotPose().getY(), kyTolerance),
-        () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
-          getRobotPose().getRotation().getRadians(), kAngularTolerance)
-    );
+        () -> assertEquals(endState.poseMeters.getX(), getRobotPose().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getY(), getRobotPose().getY(), kyTolerance),
+        () ->
+            assertEquals(
+                endState.poseMeters.getRotation().getRadians(),
+                getRobotPose().getRotation().getRadians(),
+                kAngularTolerance));
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
index 2448821..283a3de 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/NotifierCommandTest.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 class NotifierCommandTest extends CommandTestBase {
   @BeforeEach
   void setup() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
index a32b994..afc32ae 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelCommandGroupTest.java
@@ -1,14 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
@@ -17,6 +12,8 @@
 import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class ParallelCommandGroupTest extends CommandTestBase {
   @Test
   void parallelGroupScheduleTest() {
@@ -126,7 +123,7 @@
     MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
     Command command2 = command2Holder.getMock();
 
-    assertThrows(IllegalArgumentException.class,
-        () -> new ParallelCommandGroup(command1, command2));
+    assertThrows(
+        IllegalArgumentException.class, () -> new ParallelCommandGroup(command1, command2));
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
index bc10a0e..1fe3866 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelDeadlineGroupTest.java
@@ -1,14 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -16,6 +11,8 @@
 import static org.mockito.Mockito.verify;
 import static org.mockito.internal.verification.VerificationModeFactory.times;
 
+import org.junit.jupiter.api.Test;
+
 class ParallelDeadlineGroupTest extends CommandTestBase {
   @Test
   void parallelDeadlineScheduleTest() {
@@ -86,7 +83,6 @@
     }
   }
 
-
   @Test
   void parallelDeadlineRequirementTest() {
     Subsystem system1 = new TestSubsystem();
@@ -123,7 +119,7 @@
     MockCommandHolder command2Holder = new MockCommandHolder(true, system2, system3);
     Command command2 = command2Holder.getMock();
 
-    assertThrows(IllegalArgumentException.class,
-        () -> new ParallelDeadlineGroup(command1, command2));
+    assertThrows(
+        IllegalArgumentException.class, () -> new ParallelDeadlineGroup(command1, command2));
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
index 360882d..3201a52 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ParallelRaceGroupTest.java
@@ -1,14 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertNotNull;
@@ -19,6 +14,8 @@
 import static org.mockito.Mockito.times;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class ParallelRaceGroupTest extends CommandTestBase {
   @Test
   void parallelRaceScheduleTest() {
@@ -92,7 +89,6 @@
     }
   }
 
-
   @Test
   void parallelRaceRequirementTest() {
     Subsystem system1 = new TestSubsystem();
@@ -205,5 +201,4 @@
       assertFalse(scheduler.isScheduled(group));
     }
   }
-
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
index 58195f3..f9e93a3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PerpetualCommandTest.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.junit.jupiter.api.Test;
+
 class PerpetualCommandTest extends CommandTestBase {
   @Test
   void perpetualCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
index 1895fc0..6abd94b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/PrintCommandTest.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.io.ByteArrayOutputStream;
-import java.io.PrintStream;
-
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 
+import java.io.ByteArrayOutputStream;
+import java.io.PrintStream;
+import org.junit.jupiter.api.Test;
+
 class PrintCommandTest extends CommandTestBase {
   @Test
   void printCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
index 5364b95..3c34688 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ProxyScheduleCommandTest.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class ProxyScheduleCommandTest extends CommandTestBase {
   @Test
   void proxyScheduleCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
index 3b607bf..b807c71 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RobotDisabledCommandTest.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Map;
-
-import org.junit.jupiter.api.Test;
-
 import static edu.wpi.first.wpilibj2.command.CommandGroupBase.parallel;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import java.util.Map;
+import org.junit.jupiter.api.Test;
+
 class RobotDisabledCommandTest extends CommandTestBase {
   @Test
   void robotDisabledCommandCancelTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
index 07c18dd..92e2d75 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/RunCommandTest.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import org.junit.jupiter.api.Test;
+
 class RunCommandTest extends CommandTestBase {
   @Test
   void runCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
index c8d5127..c9ae4b2 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/ScheduleCommandTest.java
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class ScheduleCommandTest extends CommandTestBase {
   @Test
   void scheduleCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
index c148816..558ef57 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SchedulerTest.java
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import org.junit.jupiter.api.Test;
+
 class SchedulerTest extends CommandTestBase {
   @Test
   void schedulerLambdaTestNoInterrupt() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
index 53f6a98..de40ef8 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SelectCommandTest.java
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.Map;
-
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.mockito.Mockito.never;
 import static org.mockito.Mockito.verify;
 
+import java.util.Map;
+import org.junit.jupiter.api.Test;
+
 class SelectCommandTest extends CommandTestBase {
   @Test
   void selectCommandTest() {
@@ -29,10 +25,11 @@
       Command command3 = command3Holder.getMock();
 
       SelectCommand selectCommand =
-          new SelectCommand(Map.ofEntries(
-              Map.entry("one", command1),
-              Map.entry("two", command2),
-              Map.entry("three", command3)),
+          new SelectCommand(
+              Map.ofEntries(
+                  Map.entry("one", command1),
+                  Map.entry("two", command2),
+                  Map.entry("three", command3)),
               () -> "one");
 
       scheduler.schedule(selectCommand);
@@ -64,17 +61,17 @@
       Command command3 = command3Holder.getMock();
 
       SelectCommand selectCommand =
-          new SelectCommand(Map.ofEntries(
-              Map.entry("one", command1),
-              Map.entry("two", command2),
-              Map.entry("three", command3)),
+          new SelectCommand(
+              Map.ofEntries(
+                  Map.entry("one", command1),
+                  Map.entry("two", command2),
+                  Map.entry("three", command3)),
               () -> "four");
 
       assertDoesNotThrow(() -> scheduler.schedule(selectCommand));
     }
   }
 
-
   @Test
   void selectCommandRequirementTest() {
     Subsystem system1 = new TestSubsystem();
@@ -90,13 +87,16 @@
       MockCommandHolder command3Holder = new MockCommandHolder(true, system3, system4);
       Command command3 = command3Holder.getMock();
 
-      SelectCommand selectCommand = new SelectCommand(
-          Map.ofEntries(Map.entry("one", command1), Map.entry("two", command2),
-              Map.entry("three", command3)), () -> "one");
+      SelectCommand selectCommand =
+          new SelectCommand(
+              Map.ofEntries(
+                  Map.entry("one", command1),
+                  Map.entry("two", command2),
+                  Map.entry("three", command3)),
+              () -> "one");
 
       scheduler.schedule(selectCommand);
-      scheduler.schedule(new InstantCommand(() -> {
-      }, system3));
+      scheduler.schedule(new InstantCommand(() -> {}, system3));
 
       assertFalse(scheduler.isScheduled(selectCommand));
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
index a47f865..811034c 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SequentialCommandGroupTest.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.Mockito.never;
 import static org.mockito.Mockito.verify;
 
+import org.junit.jupiter.api.Test;
+
 class SequentialCommandGroupTest extends CommandTestBase {
   @Test
   void sequentialGroupScheduleTest() {
@@ -100,7 +97,6 @@
     }
   }
 
-
   @Test
   void sequentialGroupRequirementTest() {
     Subsystem system1 = new TestSubsystem();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
index 90ad41a..60c5c44 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/StartEndCommandTest.java
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.junit.jupiter.api.Test;
+
 class StartEndCommandTest extends CommandTestBase {
   @Test
   void startEndCommandScheduleTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
index 2febfbf..0cf6d67 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/SwerveControllerCommandTest.java
@@ -1,37 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import java.util.ArrayList;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.ArrayList;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 class SwerveControllerCommandTest {
   @BeforeEach
   void setup() {
@@ -47,14 +42,16 @@
   private final Timer m_timer = new Timer();
   private Rotation2d m_angle = new Rotation2d(0);
 
-  private SwerveModuleState[] m_moduleStates = new SwerveModuleState[]{
-    new SwerveModuleState(0, new Rotation2d(0)),
-    new SwerveModuleState(0, new Rotation2d(0)),
-    new SwerveModuleState(0, new Rotation2d(0)),
-    new SwerveModuleState(0, new Rotation2d(0))};
+  private SwerveModuleState[] m_moduleStates =
+      new SwerveModuleState[] {
+        new SwerveModuleState(0, new Rotation2d(0)),
+        new SwerveModuleState(0, new Rotation2d(0)),
+        new SwerveModuleState(0, new Rotation2d(0)),
+        new SwerveModuleState(0, new Rotation2d(0))
+      };
 
-  private final ProfiledPIDController m_rotController = new ProfiledPIDController(1, 0, 0,
-      new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
+  private final ProfiledPIDController m_rotController =
+      new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(3 * Math.PI, Math.PI));
 
   private static final double kxTolerance = 1 / 12.0;
   private static final double kyTolerance = 1 / 12.0;
@@ -63,14 +60,15 @@
   private static final double kWheelBase = 0.5;
   private static final double kTrackWidth = 0.5;
 
-  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
-      new Translation2d(kWheelBase / 2, kTrackWidth / 2),
-      new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
-      new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
-      new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(
+          new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+          new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+          new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+          new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
-      new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
+  private final SwerveDriveOdometry m_odometry =
+      new SwerveDriveOdometry(m_kinematics, new Rotation2d(0), new Pose2d(0, 0, new Rotation2d(0)));
 
   @SuppressWarnings("PMD.ArrayIsStoredDirectly")
   public void setModuleStates(SwerveModuleState[] moduleStates) {
@@ -84,7 +82,6 @@
 
   @Test
   @ResourceLock("timing")
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
   void testReachesReference() {
     final var subsystem = new Subsystem() {};
 
@@ -96,14 +93,16 @@
 
     final var endState = trajectory.sample(trajectory.getTotalTimeSeconds());
 
-    final var command = new SwerveControllerCommand(trajectory,
-        this::getRobotPose,
-        m_kinematics,
-        new PIDController(0.6, 0, 0),
-        new PIDController(0.6, 0, 0),
-        m_rotController,
-        this::setModuleStates,
-        subsystem);
+    final var command =
+        new SwerveControllerCommand(
+            trajectory,
+            this::getRobotPose,
+            m_kinematics,
+            new PIDController(0.6, 0, 0),
+            new PIDController(0.6, 0, 0),
+            m_rotController,
+            this::setModuleStates,
+            subsystem);
 
     m_timer.reset();
     m_timer.start();
@@ -118,12 +117,12 @@
     command.end(true);
 
     assertAll(
-        () -> assertEquals(endState.poseMeters.getX(),
-          getRobotPose().getX(), kxTolerance),
-        () -> assertEquals(endState.poseMeters.getY(),
-          getRobotPose().getY(), kyTolerance),
-        () -> assertEquals(endState.poseMeters.getRotation().getRadians(),
-          getRobotPose().getRotation().getRadians(), kAngularTolerance)
-    );
+        () -> assertEquals(endState.poseMeters.getX(), getRobotPose().getX(), kxTolerance),
+        () -> assertEquals(endState.poseMeters.getY(), getRobotPose().getY(), kyTolerance),
+        () ->
+            assertEquals(
+                endState.poseMeters.getRotation().getRadians(),
+                getRobotPose().getRotation().getRadians(),
+                kAngularTolerance));
   }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
index f7f9296..7cf1a28 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitCommandTest.java
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.parallel.ResourceLock;
-
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.mockito.ArgumentMatchers.anyDouble;
+import static org.mockito.ArgumentMatchers.notNull;
 import static org.mockito.Mockito.never;
 import static org.mockito.Mockito.verify;
 import static org.mockito.Mockito.when;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
 class WaitCommandTest extends CommandTestBase {
   @BeforeEach
   void setup() {
@@ -62,6 +59,7 @@
       MockCommandHolder command1Holder = new MockCommandHolder(true);
       Command command1 = command1Holder.getMock();
       when(command1.withTimeout(anyDouble())).thenCallRealMethod();
+      when(command1.raceWith(notNull())).thenCallRealMethod();
 
       Command timeout = command1.withTimeout(2);
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
index c38feee..85539dc 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/WaitUntilCommandTest.java
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.junit.jupiter.api.Test;
+
 class WaitUntilCommandTest extends CommandTestBase {
   @Test
   void waitUntilTest() {
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
index 44083d0..1a4f05d 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/ButtonTest.java
@@ -1,18 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.CommandTestBase;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
@@ -21,6 +12,11 @@
 import static org.mockito.Mockito.verify;
 import static org.mockito.Mockito.when;
 
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj2.command.CommandTestBase;
+import org.junit.jupiter.api.Test;
 
 class ButtonTest extends CommandTestBase {
   @Test
@@ -140,7 +136,6 @@
 
   @Test
   void runnableBindingTest() {
-
     InternalButton buttonWhenPressed = new InternalButton();
     InternalButton buttonWhileHeld = new InternalButton();
     InternalButton buttonWhenReleased = new InternalButton();
@@ -178,4 +173,26 @@
     assertFalse(button1.negate().get());
     assertTrue(button1.and(button2.negate()).get());
   }
+
+  @Test
+  void debounceTest() {
+    CommandScheduler scheduler = CommandScheduler.getInstance();
+    MockCommandHolder commandHolder = new MockCommandHolder(true);
+    Command command = commandHolder.getMock();
+
+    InternalButton button = new InternalButton();
+    Trigger debounced = button.debounce(0.1);
+
+    debounced.whenActive(command);
+
+    button.setPressed(true);
+    scheduler.run();
+    verify(command, never()).schedule(true);
+
+    SimHooks.stepTiming(0.3);
+
+    button.setPressed(true);
+    scheduler.run();
+    verify(command).schedule(true);
+  }
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
index 26a4a1c..9502977 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/java/edu/wpi/first/wpilibj2/command/button/NetworkButtonTest.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj2.command.button;
 
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
+import static org.mockito.Mockito.never;
+import static org.mockito.Mockito.verify;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.CommandTestBase;
-
-import static org.mockito.Mockito.never;
-import static org.mockito.Mockito.verify;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
 
 class NetworkButtonTest extends CommandTestBase {
   @BeforeEach
@@ -35,9 +31,7 @@
     var scheduler = CommandScheduler.getInstance();
     var commandHolder = new MockCommandHolder(true);
     var command = commandHolder.getMock();
-    var entry = NetworkTableInstance.getDefault()
-        .getTable("TestTable")
-        .getEntry("Test");
+    var entry = NetworkTableInstance.getDefault().getTable("TestTable").getEntry("Test");
 
     var button = new NetworkButton("TestTable", "Test");
     entry.setBoolean(false);
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
deleted file mode 100644
index d504ff7..0000000
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ButtonTest.cpp
+++ /dev/null
@@ -1,195 +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 "CommandTestBase.h"
-#include "frc2/command/CommandScheduler.h"
-#include "frc2/command/RunCommand.h"
-#include "frc2/command/WaitUntilCommand.h"
-#include "frc2/command/button/Trigger.h"
-#include "gtest/gtest.h"
-
-using namespace frc2;
-class ButtonTest : public CommandTestBase {};
-
-TEST_F(ButtonTest, WhenPressedTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-
-  WaitUntilCommand command([&finished] { return finished; });
-
-  Trigger([&pressed] { return pressed; }).WhenActive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, WhenReleasedTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = true;
-  Trigger([&pressed] { return pressed; }).WhenInactive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, WhileHeldTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  finished = false;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, WhenHeldTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  finished = true;
-  scheduler.Run();
-  finished = false;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
-  pressed = true;
-  scheduler.Run();
-  pressed = false;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, ToggleWhenPressedTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  pressed = false;
-  Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  pressed = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, AndTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed1 = false;
-  bool pressed2 = false;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  (Trigger([&pressed1] { return pressed1; }) && Trigger([&pressed2] {
-     return pressed2;
-   })).WhenActive(&command);
-  pressed1 = true;
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed2 = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, OrTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed1 = false;
-  bool pressed2 = false;
-  WaitUntilCommand command1([&finished] { return finished; });
-  WaitUntilCommand command2([&finished] { return finished; });
-
-  (Trigger([&pressed1] { return pressed1; }) || Trigger([&pressed2] {
-     return pressed2;
-   })).WhenActive(&command1);
-  pressed1 = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command1));
-
-  pressed1 = false;
-
-  (Trigger([&pressed1] { return pressed1; }) || Trigger([&pressed2] {
-     return pressed2;
-   })).WhenActive(&command2);
-  pressed2 = true;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command2));
-}
-
-TEST_F(ButtonTest, NegateTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  bool finished = false;
-  bool pressed = true;
-  WaitUntilCommand command([&finished] { return finished; });
-
-  (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
-  scheduler.Run();
-  EXPECT_FALSE(scheduler.IsScheduled(&command));
-  pressed = false;
-  scheduler.Run();
-  EXPECT_TRUE(scheduler.IsScheduled(&command));
-}
-
-TEST_F(ButtonTest, RValueButtonTest) {
-  auto& scheduler = CommandScheduler::GetInstance();
-  int counter = 0;
-  bool pressed = false;
-
-  RunCommand command([&counter] { counter++; }, {});
-
-  Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
-  scheduler.Run();
-  EXPECT_EQ(counter, 0);
-  pressed = true;
-  scheduler.Run();
-  EXPECT_EQ(counter, 1);
-}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
index 67eb428..5ae17cc 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandDecoratorTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/simulation/SimHooks.h>
 
@@ -17,7 +14,7 @@
 using namespace frc2;
 class CommandDecoratorTest : public CommandTestBase {};
 
-TEST_F(CommandDecoratorTest, WithTimeoutTest) {
+TEST_F(CommandDecoratorTest, WithTimeout) {
   CommandScheduler scheduler = GetScheduler();
 
   frc::sim::PauseTiming();
@@ -37,7 +34,7 @@
   frc::sim::ResumeTiming();
 }
 
-TEST_F(CommandDecoratorTest, WithInterruptTest) {
+TEST_F(CommandDecoratorTest, WithInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -56,7 +53,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
-TEST_F(CommandDecoratorTest, BeforeStartingTest) {
+TEST_F(CommandDecoratorTest, BeforeStarting) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -74,7 +71,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
-TEST_F(CommandDecoratorTest, AndThenTest) {
+TEST_F(CommandDecoratorTest, AndThen) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -93,7 +90,7 @@
   EXPECT_TRUE(finished);
 }
 
-TEST_F(CommandDecoratorTest, PerpetuallyTest) {
+TEST_F(CommandDecoratorTest, Perpetually) {
   CommandScheduler scheduler = GetScheduler();
 
   auto command = InstantCommand([] {}, {}).Perpetually();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
index 2600430..c812670 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandRequirementsTest.cpp
@@ -1,9 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/Errors.h>
 
 #include "CommandTestBase.h"
 #include "frc2/command/CommandScheduler.h"
@@ -18,7 +17,7 @@
 using namespace frc2;
 class CommandRequirementsTest : public CommandTestBase {};
 
-TEST_F(CommandRequirementsTest, RequirementInterruptTest) {
+TEST_F(CommandRequirementsTest, RequirementInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement;
@@ -45,7 +44,7 @@
   scheduler.Run();
 }
 
-TEST_F(CommandRequirementsTest, RequirementUninterruptibleTest) {
+TEST_F(CommandRequirementsTest, RequirementUninterruptible) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement;
@@ -72,13 +71,11 @@
   scheduler.Run();
 }
 
-TEST_F(CommandRequirementsTest, DefaultCommandRequirementErrorTest) {
+TEST_F(CommandRequirementsTest, DefaultCommandRequirementError) {
   TestSubsystem requirement1;
-  ErrorConfirmer confirmer("require");
 
   MockCommand command1;
 
-  requirement1.SetDefaultCommand(std::move(command1));
-
-  EXPECT_TRUE(requirement1.GetDefaultCommand() == NULL);
+  ASSERT_THROW(requirement1.SetDefaultCommand(std::move(command1)),
+               frc::RuntimeError);
 }
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
index 6572a98..cb16b48 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandScheduleTest.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 
 using namespace frc2;
 class CommandScheduleTest : public CommandTestBase {};
 
-TEST_F(CommandScheduleTest, InstantScheduleTest) {
+TEST_F(CommandScheduleTest, InstantSchedule) {
   CommandScheduler scheduler = GetScheduler();
   MockCommand command;
 
@@ -25,7 +22,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
-TEST_F(CommandScheduleTest, SingleIterationScheduleTest) {
+TEST_F(CommandScheduleTest, SingleIterationSchedule) {
   CommandScheduler scheduler = GetScheduler();
   MockCommand command;
 
@@ -41,7 +38,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
-TEST_F(CommandScheduleTest, MultiScheduleTest) {
+TEST_F(CommandScheduleTest, MultiSchedule) {
   CommandScheduler scheduler = GetScheduler();
   MockCommand command1;
   MockCommand command2;
@@ -78,7 +75,7 @@
   EXPECT_FALSE(scheduler.IsScheduled({&command1, &command2, &command3}));
 }
 
-TEST_F(CommandScheduleTest, SchedulerCancelTest) {
+TEST_F(CommandScheduleTest, SchedulerCancel) {
   CommandScheduler scheduler = GetScheduler();
   MockCommand command;
 
@@ -95,7 +92,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
-TEST_F(CommandScheduleTest, NotScheduledCancelTest) {
+TEST_F(CommandScheduleTest, NotScheduledCancel) {
   CommandScheduler scheduler = GetScheduler();
   MockCommand command;
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
index e5136ba..7ebbb70 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 
@@ -16,9 +13,13 @@
   scheduler.ClearButtons();
 }
 
-CommandScheduler CommandTestBase::GetScheduler() { return CommandScheduler(); }
+CommandScheduler CommandTestBase::GetScheduler() {
+  return CommandScheduler();
+}
 
-void CommandTestBase::SetUp() { frc::sim::DriverStationSim::SetEnabled(true); }
+void CommandTestBase::SetUp() {
+  frc::sim::DriverStationSim::SetEnabled(true);
+}
 
 void CommandTestBase::TearDown() {
   CommandScheduler::GetInstance().ClearButtons();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
index c6400a8..a3890f6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/CommandTestBase.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,7 +9,6 @@
 
 #include <frc/simulation/DriverStationSim.h>
 
-#include "ErrorConfirmer.h"
 #include "frc2/command/CommandGroupBase.h"
 #include "frc2/command/CommandScheduler.h"
 #include "frc2/command/SetUtilities.h"
@@ -68,20 +64,20 @@
           .WillRepeatedly(::testing::Return(m_requirements));
     }
 
-    MockCommand(const MockCommand& other) : Command{} {}
+    MockCommand(const MockCommand& other) : Command{other} {}
 
     void SetFinished(bool finished) {
       EXPECT_CALL(*this, IsFinished())
           .WillRepeatedly(::testing::Return(finished));
     }
 
-    ~MockCommand() {
+    ~MockCommand() {  // NOLINT
       auto& scheduler = CommandScheduler::GetInstance();
       scheduler.Cancel(this);
     }
 
    protected:
-    std::unique_ptr<Command> TransferOwnership() && {
+    std::unique_ptr<Command> TransferOwnership() && {  // NOLINT
       return std::make_unique<MockCommand>(std::move(*this));
     }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
index 927721a..27b89e5 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ConditionalCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/ConditionalCommand.h"
@@ -13,7 +10,7 @@
 using namespace frc2;
 class ConditionalCommandTest : public CommandTestBase {};
 
-TEST_F(ConditionalCommandTest, ConditionalCommandScheduleTest) {
+TEST_F(ConditionalCommandTest, ConditionalCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
@@ -34,7 +31,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&conditional));
 }
 
-TEST_F(ConditionalCommandTest, ConditionalCommandRequirementTest) {
+TEST_F(ConditionalCommandTest, ConditionalCommandRequirement) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement1;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
index b97cbb6..a9e3fc6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/DefaultCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/RunCommand.h"
@@ -11,7 +8,7 @@
 using namespace frc2;
 class DefaultCommandTest : public CommandTestBase {};
 
-TEST_F(DefaultCommandTest, DefaultCommandScheduleTest) {
+TEST_F(DefaultCommandTest, DefaultCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem subsystem;
@@ -25,7 +22,7 @@
   EXPECT_TRUE(scheduler.IsScheduled(handle));
 }
 
-TEST_F(DefaultCommandTest, DefaultCommandInterruptResumeTest) {
+TEST_F(DefaultCommandTest, DefaultCommandInterruptResume) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem subsystem;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
deleted file mode 100644
index 2c13f5d..0000000
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.cpp
+++ /dev/null
@@ -1,22 +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 "ErrorConfirmer.h"
-
-#include <regex>
-
-ErrorConfirmer* ErrorConfirmer::instance;
-
-int32_t ErrorConfirmer::HandleError(HAL_Bool isError, int32_t errorCode,
-                                    HAL_Bool isLVCode, const char* details,
-                                    const char* location, const char* callStack,
-                                    HAL_Bool printMsg) {
-  if (std::regex_search(details, std::regex(instance->m_msg))) {
-    instance->ConfirmError();
-  }
-  return 1;
-}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
deleted file mode 100644
index 2df5e26..0000000
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ErrorConfirmer.h
+++ /dev/null
@@ -1,39 +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 <hal/simulation/MockHooks.h>
-
-#include "gmock/gmock.h"
-
-class ErrorConfirmer {
- public:
-  explicit ErrorConfirmer(const char* msg) : m_msg(msg) {
-    if (instance != nullptr) return;
-    HALSIM_SetSendError(HandleError);
-    EXPECT_CALL(*this, ConfirmError());
-    instance = this;
-  }
-
-  ~ErrorConfirmer() {
-    HALSIM_SetSendError(nullptr);
-    instance = nullptr;
-  }
-
-  MOCK_METHOD0(ConfirmError, void());
-
-  const char* m_msg;
-
-  static int32_t HandleError(HAL_Bool isError, int32_t errorCode,
-                             HAL_Bool isLVCode, const char* details,
-                             const char* location, const char* callStack,
-                             HAL_Bool printMsg);
-
- private:
-  static ErrorConfirmer* instance;
-};
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
index 140d4fb..0f0ebda 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/FunctionalCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/FunctionalCommand.h"
@@ -11,7 +8,7 @@
 using namespace frc2;
 class FunctionalCommandTest : public CommandTestBase {};
 
-TEST_F(FunctionalCommandTest, FunctionalCommandScheduleTest) {
+TEST_F(FunctionalCommandTest, FunctionalCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   int counter = 0;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
index e91f677..90eab85 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/InstantCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -11,7 +8,7 @@
 using namespace frc2;
 class InstantCommandTest : public CommandTestBase {};
 
-TEST_F(InstantCommandTest, InstantCommandScheduleTest) {
+TEST_F(InstantCommandTest, InstantCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   int counter = 0;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
index 07bc07a..537d892 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/MecanumControllerCommandTest.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc2/Timer.h>
 #include <frc2/command/MecanumControllerCommand.h>
 #include <frc2/command/Subsystem.h>
 
+#include <frc/Timer.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
@@ -17,7 +14,7 @@
 #include <frc/kinematics/MecanumDriveOdometry.h>
 #include <frc/simulation/SimHooks.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "gtest/gtest.h"
 
@@ -30,7 +27,7 @@
                            units::inverse<units::squared<units::second>>>;
 
  protected:
-  frc2::Timer m_timer;
+  frc::Timer m_timer;
   frc::Rotation2d m_angle{0_rad};
 
   units::meters_per_second_t m_frontLeftSpeed = 0.0_mps;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
index b4df5a7..00c9b89 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/NotifierCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/simulation/SimHooks.h>
 
@@ -15,7 +12,7 @@
 
 class NotifierCommandTest : public CommandTestBase {};
 
-TEST_F(NotifierCommandTest, NotifierCommandScheduleTest) {
+TEST_F(NotifierCommandTest, NotifierCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   frc::sim::PauseTiming();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp
index 2a1646d..5f6e733 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/POVButtonTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
 #include <frc/simulation/JoystickSim.h>
@@ -18,7 +15,7 @@
 using namespace frc2;
 class POVButtonTest : public CommandTestBase {};
 
-TEST_F(POVButtonTest, SetPOVTest) {
+TEST_F(POVButtonTest, SetPOV) {
   frc::sim::JoystickSim joysim(1);
   joysim.SetPOV(0);
   joysim.NotifyNewData();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
index 55c7b98..d6c0295 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelCommandGroupTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -13,7 +10,7 @@
 using namespace frc2;
 class ParallelCommandGroupTest : public CommandTestBase {};
 
-TEST_F(ParallelCommandGroupTest, ParallelGroupScheduleTest) {
+TEST_F(ParallelCommandGroupTest, ParallelGroupSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -43,7 +40,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelCommandGroupTest, ParallelGroupInterruptTest) {
+TEST_F(ParallelCommandGroupTest, ParallelGroupInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -74,7 +71,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancelTest) {
+TEST_F(ParallelCommandGroupTest, ParallelGroupNotScheduledCancel) {
   CommandScheduler scheduler = GetScheduler();
 
   ParallelCommandGroup group((InstantCommand(), InstantCommand()));
@@ -82,7 +79,7 @@
   EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
 }
 
-TEST_F(ParallelCommandGroupTest, ParallelGroupCopyTest) {
+TEST_F(ParallelCommandGroupTest, ParallelGroupCopy) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -98,7 +95,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelCommandGroupTest, ParallelGroupRequirementTest) {
+TEST_F(ParallelCommandGroupTest, ParallelGroupRequirement) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement1;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
index 6e3246f..67d8720 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelDeadlineGroupTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -13,7 +10,7 @@
 using namespace frc2;
 class ParallelDeadlineGroupTest : public CommandTestBase {};
 
-TEST_F(ParallelDeadlineGroupTest, DeadlineGroupScheduleTest) {
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -51,7 +48,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterruptTest) {
+TEST_F(ParallelDeadlineGroupTest, SequentialGroupInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem subsystem;
@@ -90,7 +87,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancelTest) {
+TEST_F(ParallelDeadlineGroupTest, DeadlineGroupNotScheduledCancel) {
   CommandScheduler scheduler = GetScheduler();
 
   ParallelDeadlineGroup group{InstantCommand(), InstantCommand()};
@@ -98,7 +95,7 @@
   EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
 }
 
-TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopyTest) {
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineCopy) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -114,7 +111,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirementTest) {
+TEST_F(ParallelDeadlineGroupTest, ParallelDeadlineRequirement) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement1;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
index 072bcb0..72f4274 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ParallelRaceGroupTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -14,7 +11,7 @@
 using namespace frc2;
 class ParallelRaceGroupTest : public CommandTestBase {};
 
-TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTest) {
+TEST_F(ParallelRaceGroupTest, ParallelRaceSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -50,7 +47,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelRaceGroupTest, ParallelRaceInterruptTest) {
+TEST_F(ParallelRaceGroupTest, ParallelRaceInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -86,7 +83,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancelTest) {
+TEST_F(ParallelRaceGroupTest, ParallelRaceNotScheduledCancel) {
   CommandScheduler scheduler = GetScheduler();
 
   ParallelRaceGroup group{InstantCommand(), InstantCommand()};
@@ -94,7 +91,7 @@
   EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
 }
 
-TEST_F(ParallelRaceGroupTest, ParallelRaceCopyTest) {
+TEST_F(ParallelRaceGroupTest, ParallelRaceCopy) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -110,7 +107,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelRaceGroupTest, RaceGroupRequirementTest) {
+TEST_F(ParallelRaceGroupTest, RaceGroupRequirement) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement1;
@@ -131,7 +128,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnceTest) {
+TEST_F(ParallelRaceGroupTest, ParallelRaceOnlyCallsEndOnce) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished1 = false;
@@ -155,7 +152,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group2));
 }
 
-TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwiceTest) {
+TEST_F(ParallelRaceGroupTest, ParallelRaceScheduleTwice) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
index b3ef861..6254b85 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PerpetualCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -12,7 +9,7 @@
 using namespace frc2;
 class PerpetualCommandTest : public CommandTestBase {};
 
-TEST_F(PerpetualCommandTest, PerpetualCommandScheduleTest) {
+TEST_F(PerpetualCommandTest, PerpetualCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   bool check = false;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
index b940566..02575a3 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/PrintCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <regex>
 
@@ -13,7 +10,7 @@
 using namespace frc2;
 class PrintCommandTest : public CommandTestBase {};
 
-TEST_F(PrintCommandTest, PrintCommandScheduleTest) {
+TEST_F(PrintCommandTest, PrintCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   PrintCommand command("Test!");
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
index 09a569f..def7be9 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ProxyScheduleCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <regex>
 
@@ -15,7 +12,7 @@
 using namespace frc2;
 class ProxyScheduleCommandTest : public CommandTestBase {};
 
-TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandScheduleTest) {
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandSchedule) {
   CommandScheduler& scheduler = CommandScheduler::GetInstance();
 
   bool scheduled = false;
@@ -30,7 +27,7 @@
   EXPECT_TRUE(scheduled);
 }
 
-TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEndTest) {
+TEST_F(ProxyScheduleCommandTest, ProxyScheduleCommandEnd) {
   CommandScheduler& scheduler = CommandScheduler::GetInstance();
 
   bool finished = false;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
index bac40d5..1595a1e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RobotDisabledCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/ConditionalCommand.h"
@@ -16,7 +13,7 @@
 using namespace frc2;
 class RobotDisabledCommandTest : public CommandTestBase {};
 
-TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancelTest) {
+TEST_F(RobotDisabledCommandTest, RobotDisabledCommandCancel) {
   CommandScheduler scheduler = GetScheduler();
 
   MockCommand command({}, false, false);
@@ -37,7 +34,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&command));
 }
 
-TEST_F(RobotDisabledCommandTest, RunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, RunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   MockCommand command1;
@@ -55,7 +52,7 @@
   EXPECT_TRUE(scheduler.IsScheduled(&command2));
 }
 
-TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, SequentialGroupRunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   SequentialCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
@@ -71,7 +68,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
 }
 
-TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, ParallelGroupRunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   ParallelCommandGroup runWhenDisabled{MockCommand(), MockCommand()};
@@ -87,7 +84,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
 }
 
-TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, ParallelRaceRunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   ParallelRaceGroup runWhenDisabled{MockCommand(), MockCommand()};
@@ -103,7 +100,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
 }
 
-TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, ParallelDeadlineRunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   ParallelDeadlineGroup runWhenDisabled{MockCommand(), MockCommand()};
@@ -119,7 +116,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
 }
 
-TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, ConditionalCommandRunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   ConditionalCommand runWhenDisabled{MockCommand(), MockCommand(),
@@ -136,7 +133,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&dontRunWhenDisabled));
 }
 
-TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabledTest) {
+TEST_F(RobotDisabledCommandTest, SelectCommandRunWhenDisabled) {
   CommandScheduler scheduler = GetScheduler();
 
   SelectCommand<int> runWhenDisabled{[] { return 1; },
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
index 07eecb3..11d4ada 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/RunCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/RunCommand.h"
@@ -11,7 +8,7 @@
 using namespace frc2;
 class RunCommandTest : public CommandTestBase {};
 
-TEST_F(RunCommandTest, RunCommandScheduleTest) {
+TEST_F(RunCommandTest, RunCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   int counter = 0;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
index 29e8dc5..4003b4a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/ScheduleCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <regex>
 
@@ -15,7 +12,7 @@
 using namespace frc2;
 class ScheduleCommandTest : public CommandTestBase {};
 
-TEST_F(ScheduleCommandTest, ScheduleCommandScheduleTest) {
+TEST_F(ScheduleCommandTest, ScheduleCommandSchedule) {
   CommandScheduler& scheduler = CommandScheduler::GetInstance();
 
   bool scheduled = false;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
index 2582ad9..c0f1e5a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SchedulerTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -29,7 +26,7 @@
   EXPECT_EQ(counter, 3);
 }
 
-TEST_F(SchedulerTest, SchedulerLambdaInterruptTest) {
+TEST_F(SchedulerTest, SchedulerLambdaInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   RunCommand command([] {}, {});
@@ -45,7 +42,7 @@
   EXPECT_EQ(counter, 1);
 }
 
-TEST_F(SchedulerTest, UnregisterSubsystemTest) {
+TEST_F(SchedulerTest, UnregisterSubsystem) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem system;
@@ -55,7 +52,7 @@
   EXPECT_NO_FATAL_FAILURE(scheduler.UnregisterSubsystem(&system));
 }
 
-TEST_F(SchedulerTest, SchedulerCancelAllTest) {
+TEST_F(SchedulerTest, SchedulerCancelAll) {
   CommandScheduler scheduler = GetScheduler();
 
   RunCommand command([] {}, {});
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
index 6be14ef..409e2a6 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SelectCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/ConditionalCommand.h"
@@ -13,7 +10,7 @@
 using namespace frc2;
 class SelectCommandTest : public CommandTestBase {};
 
-TEST_F(SelectCommandTest, SelectCommandTest) {
+TEST_F(SelectCommandTest, SelectCommand) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> mock = std::make_unique<MockCommand>();
@@ -39,7 +36,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&select));
 }
 
-TEST_F(SelectCommandTest, SelectCommandRequirementTest) {
+TEST_F(SelectCommandTest, SelectCommandRequirement) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement1;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
index 3a6f8d8..2d88c0a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SequentialCommandGroupTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/InstantCommand.h"
@@ -13,7 +10,7 @@
 using namespace frc2;
 class SequentialCommandGroupTest : public CommandTestBase {};
 
-TEST_F(SequentialCommandGroupTest, SequentialGroupScheduleTest) {
+TEST_F(SequentialCommandGroupTest, SequentialGroupSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -52,7 +49,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(SequentialCommandGroupTest, SequentialGroupInterruptTest) {
+TEST_F(SequentialCommandGroupTest, SequentialGroupInterrupt) {
   CommandScheduler scheduler = GetScheduler();
 
   std::unique_ptr<MockCommand> command1Holder = std::make_unique<MockCommand>();
@@ -91,7 +88,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancelTest) {
+TEST_F(SequentialCommandGroupTest, SequentialGroupNotScheduledCancel) {
   CommandScheduler scheduler = GetScheduler();
 
   SequentialCommandGroup group{InstantCommand(), InstantCommand()};
@@ -99,7 +96,7 @@
   EXPECT_NO_FATAL_FAILURE(scheduler.Cancel(&group));
 }
 
-TEST_F(SequentialCommandGroupTest, SequentialGroupCopyTest) {
+TEST_F(SequentialCommandGroupTest, SequentialGroupCopy) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
@@ -115,7 +112,7 @@
   EXPECT_FALSE(scheduler.IsScheduled(&group));
 }
 
-TEST_F(SequentialCommandGroupTest, SequentialGroupRequirementTest) {
+TEST_F(SequentialCommandGroupTest, SequentialGroupRequirement) {
   CommandScheduler scheduler = GetScheduler();
 
   TestSubsystem requirement1;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
index 3f25792..53a2df2 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/StartEndCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/StartEndCommand.h"
@@ -11,7 +8,7 @@
 using namespace frc2;
 class StartEndCommandTest : public CommandTestBase {};
 
-TEST_F(StartEndCommandTest, StartEndCommandScheduleTest) {
+TEST_F(StartEndCommandTest, StartEndCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   int counter = 0;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
index b2c47d0..93117b8 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/SwerveControllerCommandTest.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc2/Timer.h>
 #include <frc2/command/Subsystem.h>
 #include <frc2/command/SwerveControllerCommand.h>
 
+#include <frc/Timer.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
@@ -18,7 +15,7 @@
 #include <frc/kinematics/SwerveModuleState.h>
 #include <frc/simulation/SimHooks.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "gtest/gtest.h"
 
@@ -31,10 +28,10 @@
                            units::inverse<units::squared<units::second>>>;
 
  protected:
-  frc2::Timer m_timer;
+  frc::Timer m_timer;
   frc::Rotation2d m_angle{0_rad};
 
-  std::array<frc::SwerveModuleState, 4> m_moduleStates{
+  wpi::array<frc::SwerveModuleState, 4> m_moduleStates{
       frc::SwerveModuleState{}, frc::SwerveModuleState{},
       frc::SwerveModuleState{}, frc::SwerveModuleState{}};
 
@@ -63,7 +60,7 @@
 
   void TearDown() override { frc::sim::ResumeTiming(); }
 
-  std::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
+  wpi::array<frc::SwerveModuleState, 4> getCurrentWheelSpeeds() {
     return m_moduleStates;
   }
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
index 000a9bb..30eea6b 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/simulation/SimHooks.h>
 
@@ -14,7 +11,7 @@
 using namespace frc2;
 class WaitCommandTest : public CommandTestBase {};
 
-TEST_F(WaitCommandTest, WaitCommandScheduleTest) {
+TEST_F(WaitCommandTest, WaitCommandSchedule) {
   frc::sim::PauseTiming();
 
   CommandScheduler scheduler = GetScheduler();
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
index e9728db..01acc39 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/WaitUntilCommandTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "CommandTestBase.h"
 #include "frc2/command/WaitUntilCommand.h"
@@ -11,7 +8,7 @@
 using namespace frc2;
 class WaitUntilCommandTest : public CommandTestBase {};
 
-TEST_F(WaitUntilCommandTest, WaitUntilCommandScheduleTest) {
+TEST_F(WaitUntilCommandTest, WaitUntilCommandSchedule) {
   CommandScheduler scheduler = GetScheduler();
 
   bool finished = false;
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/ButtonTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/ButtonTest.cpp
new file mode 100644
index 0000000..db4276d
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/ButtonTest.cpp
@@ -0,0 +1,210 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/simulation/SimHooks.h>
+
+#include "../CommandTestBase.h"
+#include "frc2/command/CommandScheduler.h"
+#include "frc2/command/RunCommand.h"
+#include "frc2/command/WaitUntilCommand.h"
+#include "frc2/command/button/Trigger.h"
+#include "gtest/gtest.h"
+
+using namespace frc2;
+class ButtonTest : public CommandTestBase {};
+
+TEST_F(ButtonTest, WhenPressed) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+
+  WaitUntilCommand command([&finished] { return finished; });
+
+  Trigger([&pressed] { return pressed; }).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenReleased) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = true;
+  Trigger([&pressed] { return pressed; }).WhenInactive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhileHeld) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveContinous(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, WhenHeld) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  finished = true;
+  scheduler.Run();
+  finished = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).WhileActiveOnce(&command);
+  pressed = true;
+  scheduler.Run();
+  pressed = false;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, ToggleWhenPressed) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  pressed = false;
+  Trigger([&pressed] { return pressed; }).ToggleWhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, And) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) && Trigger([&pressed2] {
+     return pressed2;
+   })).WhenActive(&command);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, Or) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed1 = false;
+  bool pressed2 = false;
+  WaitUntilCommand command1([&finished] { return finished; });
+  WaitUntilCommand command2([&finished] { return finished; });
+
+  (Trigger([&pressed1] { return pressed1; }) || Trigger([&pressed2] {
+     return pressed2;
+   })).WhenActive(&command1);
+  pressed1 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command1));
+
+  pressed1 = false;
+
+  (Trigger([&pressed1] { return pressed1; }) || Trigger([&pressed2] {
+     return pressed2;
+   })).WhenActive(&command2);
+  pressed2 = true;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command2));
+}
+
+TEST_F(ButtonTest, Negate) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool finished = false;
+  bool pressed = true;
+  WaitUntilCommand command([&finished] { return finished; });
+
+  (!Trigger([&pressed] { return pressed; })).WhenActive(&command);
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+  pressed = false;
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
+
+TEST_F(ButtonTest, RValueButton) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  int counter = 0;
+  bool pressed = false;
+
+  RunCommand command([&counter] { counter++; }, {});
+
+  Trigger([&pressed] { return pressed; }).WhenActive(std::move(command));
+  scheduler.Run();
+  EXPECT_EQ(counter, 0);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_EQ(counter, 1);
+}
+
+TEST_F(ButtonTest, Debounce) {
+  auto& scheduler = CommandScheduler::GetInstance();
+  bool pressed = false;
+  RunCommand command([] {});
+
+  Trigger([&pressed] { return pressed; }).Debounce(100_ms).WhenActive(&command);
+  pressed = true;
+  scheduler.Run();
+  EXPECT_FALSE(scheduler.IsScheduled(&command));
+
+  frc::sim::StepTiming(300_ms);
+
+  scheduler.Run();
+  EXPECT_TRUE(scheduler.IsScheduled(&command));
+}
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp
index 18fef35..7de1084 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/button/NetworkButtonTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <networktables/NetworkTableInstance.h>
 
@@ -25,7 +22,7 @@
   }
 };
 
-TEST_F(NetworkButtonTest, SetNetworkButtonTest) {
+TEST_F(NetworkButtonTest, SetNetworkButton) {
   auto& scheduler = CommandScheduler::GetInstance();
   auto entry = nt::NetworkTableInstance::GetDefault()
                    .GetTable("TestTable")
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
index 89d55b6..05adf8e 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/frc2/command/make_vector.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/main.cpp b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/main.cpp
index c6b6c58..6aea19a 100644
--- a/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibNewCommands/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <hal/HALBase.h>
 
diff --git a/third_party/allwpilib/wpilibNewCommands/wpilibNewCommands-config.cmake.in b/third_party/allwpilib/wpilibNewCommands/wpilibNewCommands-config.cmake.in
new file mode 100644
index 0000000..75aa6ad
--- /dev/null
+++ b/third_party/allwpilib/wpilibNewCommands/wpilibNewCommands-config.cmake.in
@@ -0,0 +1,11 @@
+include(CMakeFindDependencyMacro)
+ @FILENAME_DEP_REPLACE@
+ @WPIUTIL_DEP_REPLACE@
+ @NTCORE_DEP_REPLACE@
+ @CSCORE_DEP_REPLACE@
+ @CAMERASERVER_DEP_REPLACE@
+ @HAL_DEP_REPLACE@
+ @WPILIBC_DEP_REPLACE@
+ @WPIMATH_DEP_REPLACE@
+
+ include(${SELF_DIR}/wpilibNewCommands.cmake)
diff --git a/third_party/allwpilib/wpilibOldCommands/CMakeLists.txt b/third_party/allwpilib/wpilibOldCommands/CMakeLists.txt
new file mode 100644
index 0000000..a9b7044
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/CMakeLists.txt
@@ -0,0 +1,59 @@
+project(wpilibOldCommands)
+
+include(SubDirList)
+include(CompileWarnings)
+include(AddTest)
+
+if (WITH_JAVA)
+  find_package(Java REQUIRED)
+  include(UseJava)
+  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
+
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
+  add_jar(wpilibOldCommands_jar ${JAVA_SOURCES} INCLUDE_JARS hal_jar ntcore_jar cscore_jar cameraserver_jar wpimath_jar wpiutil_jar wpilibj_jar OUTPUT_NAME wpilibOldCommands)
+
+  get_property(WPIlIBOLDCOMMANDS_JAR_FILE TARGET wpilibOldCommands_jar PROPERTY JAR_FILE)
+  install(FILES ${WPIlIBOLDCOMMANDS_JAR_FILE} DESTINATION "${java_lib_dest}")
+
+  set_property(TARGET wpilibOldCommands_jar PROPERTY FOLDER "java")
+
+  if (WITH_FLAT_INSTALL)
+      set (wpilibOldCommands_config_dir ${wpilib_dest})
+  else()
+      set (wpilibOldCommands_config_dir share/wpilibOldCommands)
+  endif()
+
+  install(FILES wpilibOldCommands-config.cmake DESTINATION ${wpilibOldCommands_config_dir})
+endif()
+
+file(GLOB_RECURSE wpilibOldCommands_native_src src/main/native/cpp/*.cpp)
+add_library(wpilibOldCommands ${wpilibOldCommands_native_src})
+set_target_properties(wpilibOldCommands PROPERTIES DEBUG_POSTFIX "d")
+set_property(TARGET wpilibOldCommands PROPERTY FOLDER "libraries")
+
+target_compile_features(wpilibOldCommands PUBLIC cxx_std_17)
+wpilib_target_warnings(wpilibOldCommands)
+target_link_libraries(wpilibOldCommands wpilibc)
+
+target_include_directories(wpilibOldCommands PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
+                            $<INSTALL_INTERFACE:${include_dest}/wpilibOldCommands>)
+
+install(TARGETS wpilibOldCommands EXPORT wpilibOldCommands DESTINATION "${main_lib_dest}")
+install(DIRECTORY src/main/native/include/ DESTINATION "${include_dest}/wpilibOldCommands")
+
+if (MSVC OR FLAT_INSTALL_WPILIB)
+     set(wpilibOldCommands_config_dir ${wpilib_dest})
+ else()
+     set(wpilibOldCommands_config_dir share/wpilibOldCommands)
+ endif()
+
+ configure_file(wpilibOldCommands-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibOldCommands-config.cmake)
+ install(FILES ${WPILIB_BINARY_DIR}/wpilibOldCommands-config.cmake DESTINATION ${wpilibOldCommands_config_dir})
+ install(EXPORT wpilibOldCommands DESTINATION ${wpilibOldCommands_config_dir})
+
+ if (WITH_TESTS)
+     wpilib_add_test(wpilibOldCommands src/test/native/cpp)
+     target_include_directories(wpilibOldCommands_test PRIVATE src/test/native/include)
+     target_link_libraries(wpilibOldCommands_test wpilibOldCommands gmock_main)
+ endif()
diff --git a/third_party/allwpilib/wpilibOldCommands/build.gradle b/third_party/allwpilib/wpilibOldCommands/build.gradle
index c8a4cb9..39870ee 100644
--- a/third_party/allwpilib/wpilibOldCommands/build.gradle
+++ b/third_party/allwpilib/wpilibOldCommands/build.gradle
@@ -30,19 +30,35 @@
 
 nativeUtils.exportsConfigs {
     wpilibOldCommands {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
 }
 
-apply plugin: DisableBuildingGTest
-
 model {
     components {}
     binaries {
@@ -57,8 +73,8 @@
             lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
 
             if (it.component.name == "${nativeName}Dev") {
-              lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
-              project(':hal').addHalJniDependency(it)
+                lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+                project(':hal').addHalJniDependency(it)
             }
 
             if (it instanceof GoogleTestTestSuiteBinarySpec) {
@@ -66,7 +82,7 @@
                 lib project: ':cscore', library: 'cscore', linkage: 'shared'
             }
             if ((it instanceof NativeExecutableBinarySpec || it instanceof GoogleTestTestSuiteBinarySpec) && it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
             }
         }
     }
@@ -97,3 +113,7 @@
         showStandardStreams = true
     }
 }
+
+tasks.withType(JavaCompile) {
+    options.compilerArgs += "-Xlint:-removal"
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java b/third_party/allwpilib/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
index 5a5f6a9..719fdcc 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/dev/java/edu/wpi/first/wpilibj/commands/DevMain.java
@@ -1,20 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands;
 
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.networktables.NetworkTablesJNI;
-import edu.wpi.first.wpiutil.RuntimeDetector;
+import edu.wpi.first.util.RuntimeDetector;
 
 public final class DevMain {
-  /**
-   * Main entry point.
-   */
+  /** Main entry point. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
     System.out.println(RuntimeDetector.getPlatformPath());
@@ -22,6 +17,5 @@
     System.out.println(HALUtil.getHALRuntimeType());
   }
 
-  private DevMain() {
-  }
+  private DevMain() {}
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpilibOldCommands/src/dev/native/cpp/main.cpp
index 5312a1d..a3e363e 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/dev/native/cpp/main.cpp
@@ -1,8 +1,5 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 int main() {}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDBase.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
new file mode 100644
index 0000000..505801a
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
@@ -0,0 +1,815 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.BoundaryException;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.concurrent.locks.ReentrantLock;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
+ * calculations, as well as writing the given PIDOutput.
+ *
+ * <p>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.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
+  public static final double kDefaultPeriod = 0.05;
+  private static int instances;
+
+  // Factor for "proportional" control
+  @SuppressWarnings("MemberName")
+  private double m_P;
+
+  // Factor for "integral" control
+  @SuppressWarnings("MemberName")
+  private double m_I;
+
+  // Factor for "derivative" control
+  @SuppressWarnings("MemberName")
+  private double m_D;
+
+  // Factor for "feed forward" control
+  @SuppressWarnings("MemberName")
+  private double m_F;
+
+  // |maximum output|
+  private double m_maximumOutput = 1.0;
+
+  // |minimum output|
+  private double m_minimumOutput = -1.0;
+
+  // Maximum input - limit setpoint to this
+  private double m_maximumInput;
+
+  // Minimum input - limit setpoint to this
+  private double m_minimumInput;
+
+  // Input range - difference between maximum and minimum
+  private double m_inputRange;
+
+  // Do the endpoints wrap around? (e.g., absolute encoder)
+  private boolean m_continuous;
+
+  // Is the PID controller enabled
+  protected boolean m_enabled;
+
+  // The prior error (used to compute velocity)
+  private double m_prevError;
+
+  // The sum of the errors for use in the integral calc
+  private double m_totalError;
+
+  // The tolerance object used to check if on target
+  private Tolerance m_tolerance;
+
+  private double m_setpoint;
+  private double m_prevSetpoint;
+
+  private double m_result;
+
+  private LinearFilter m_filter;
+
+  protected ReentrantLock m_thisMutex = new ReentrantLock();
+
+  // Ensures when disable() is called, pidWrite() won't run if calculate()
+  // is already running at that time.
+  protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
+
+  protected PIDSource m_pidInput;
+  protected PIDOutput m_pidOutput;
+  protected Timer m_setpointTimer;
+
+  /**
+   * Tolerance is the type of tolerance used to specify if the PID controller is on target.
+   *
+   * <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
+   * specify types of tolerance specifications to use.
+   */
+  public interface Tolerance {
+    boolean onTarget();
+  }
+
+  /** Used internally for when Tolerance hasn't been set. */
+  public static class NullTolerance implements Tolerance {
+    @Override
+    public boolean onTarget() {
+      throw new IllegalStateException("No tolerance value set when calling onTarget().");
+    }
+  }
+
+  public class PercentageTolerance implements Tolerance {
+    private final double m_percentage;
+
+    PercentageTolerance(double value) {
+      m_percentage = value;
+    }
+
+    @Override
+    public boolean onTarget() {
+      return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
+    }
+  }
+
+  public class AbsoluteTolerance implements Tolerance {
+    private final double m_value;
+
+    AbsoluteTolerance(double value) {
+      m_value = value;
+    }
+
+    @Override
+    public boolean onTarget() {
+      return Math.abs(getError()) < m_value;
+    }
+  }
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D, and F.
+   *
+   * @param Kp the proportional coefficient
+   * @param Ki the integral coefficient
+   * @param Kd the derivative coefficient
+   * @param Kf the feed forward term
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output percentage
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
+    requireNonNullParam(source, "PIDSource", "PIDBase");
+    requireNonNullParam(output, "output", "PIDBase");
+
+    m_setpointTimer = new Timer();
+    m_setpointTimer.start();
+
+    m_P = Kp;
+    m_I = Ki;
+    m_D = Kd;
+    m_F = Kf;
+
+    m_pidInput = source;
+    m_filter = LinearFilter.movingAverage(1);
+
+    m_pidOutput = output;
+
+    instances++;
+    HAL.report(tResourceType.kResourceType_PIDController, instances);
+    m_tolerance = new NullTolerance();
+    SendableRegistry.add(this, "PIDController", instances);
+  }
+
+  /**
+   * Allocate a PID object with the given constants for P, I, and D.
+   *
+   * @param Kp the proportional coefficient
+   * @param Ki the integral coefficient
+   * @param Kd the derivative coefficient
+   * @param source the PIDSource object that is used to get values
+   * @param output the PIDOutput object that is set to the output percentage
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+    this(Kp, Ki, Kd, 0.0, source, output);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Read the input, calculate the output accordingly, and write to the output. This should only be
+   * called by the PIDTask and is created during initialization.
+   */
+  @SuppressWarnings("LocalVariableName")
+  protected void calculate() {
+    if (m_pidInput == null || m_pidOutput == null) {
+      return;
+    }
+
+    boolean enabled;
+
+    m_thisMutex.lock();
+    try {
+      enabled = m_enabled;
+    } finally {
+      m_thisMutex.unlock();
+    }
+
+    if (enabled) {
+      double input;
+
+      // Storage for function inputs
+      PIDSourceType pidSourceType;
+      double P;
+      double I;
+      double D;
+      double feedForward = calculateFeedForward();
+      double minimumOutput;
+      double maximumOutput;
+
+      // Storage for function input-outputs
+      double prevError;
+      double error;
+      double totalError;
+
+      m_thisMutex.lock();
+      try {
+        input = m_filter.calculate(m_pidInput.pidGet());
+
+        pidSourceType = m_pidInput.getPIDSourceType();
+        P = m_P;
+        I = m_I;
+        D = m_D;
+        minimumOutput = m_minimumOutput;
+        maximumOutput = m_maximumOutput;
+
+        prevError = m_prevError;
+        error = getContinuousError(m_setpoint - input);
+        totalError = m_totalError;
+      } finally {
+        m_thisMutex.unlock();
+      }
+
+      // Storage for function outputs
+      double result;
+
+      if (pidSourceType.equals(PIDSourceType.kRate)) {
+        if (P != 0) {
+          totalError = clamp(totalError + error, minimumOutput / P, maximumOutput / P);
+        }
+
+        result = P * totalError + D * error + feedForward;
+      } else {
+        if (I != 0) {
+          totalError = clamp(totalError + error, minimumOutput / I, maximumOutput / I);
+        }
+
+        result = P * error + I * totalError + D * (error - prevError) + feedForward;
+      }
+
+      result = clamp(result, minimumOutput, maximumOutput);
+
+      // Ensures m_enabled check and pidWrite() call occur atomically
+      m_pidWriteMutex.lock();
+      m_thisMutex.lock();
+      try {
+        if (m_enabled) {
+          // Don't block other PIDController operations on pidWrite()
+          m_thisMutex.unlock();
+
+          m_pidOutput.pidWrite(result);
+        }
+      } finally {
+        if (!m_enabled) {
+          m_thisMutex.unlock();
+        }
+        m_pidWriteMutex.unlock();
+      }
+
+      m_thisMutex.lock();
+      try {
+        m_prevError = error;
+        m_totalError = totalError;
+        m_result = result;
+      } finally {
+        m_thisMutex.unlock();
+      }
+    }
+  }
+
+  /**
+   * Calculate the feed forward term.
+   *
+   * <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
+   * feed forward calculation is desired, the user can override this function and provide his or her
+   * own. This function does no synchronization because the PIDController class only calls it in
+   * synchronized code, so be careful if calling it oneself.
+   *
+   * <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
+   * setpoint for the output. If a position PID controller is being used, the F term should be set
+   * to 1 over the maximum speed for the output measured in setpoint units per this controller's
+   * update period (see the default period in this class's constructor).
+   *
+   * @return The feedforward value.
+   */
+  protected double calculateFeedForward() {
+    if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
+      return m_F * getSetpoint();
+    } else {
+      double temp = m_F * getDeltaSetpoint();
+      m_prevSetpoint = m_setpoint;
+      m_setpointTimer.reset();
+      return temp;
+    }
+  }
+
+  /**
+   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
+   * coefficients.
+   *
+   * @param p Proportional coefficient
+   * @param i Integral coefficient
+   * @param d Differential coefficient
+   */
+  @Override
+  @SuppressWarnings("ParameterName")
+  public void setPID(double p, double i, double d) {
+    m_thisMutex.lock();
+    try {
+      m_P = p;
+      m_I = i;
+      m_D = d;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
+   * coefficients.
+   *
+   * @param p Proportional coefficient
+   * @param i Integral coefficient
+   * @param d Differential coefficient
+   * @param f Feed forward coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setPID(double p, double i, double d, double f) {
+    m_thisMutex.lock();
+    try {
+      m_P = p;
+      m_I = i;
+      m_D = d;
+      m_F = f;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the Proportional coefficient of the PID controller gain.
+   *
+   * @param p Proportional coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setP(double p) {
+    m_thisMutex.lock();
+    try {
+      m_P = p;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the Integral coefficient of the PID controller gain.
+   *
+   * @param i Integral coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setI(double i) {
+    m_thisMutex.lock();
+    try {
+      m_I = i;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the Differential coefficient of the PID controller gain.
+   *
+   * @param d differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setD(double d) {
+    m_thisMutex.lock();
+    try {
+      m_D = d;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the Feed forward coefficient of the PID controller gain.
+   *
+   * @param f feed forward coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setF(double f) {
+    m_thisMutex.lock();
+    try {
+      m_F = f;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  @Override
+  public double getP() {
+    m_thisMutex.lock();
+    try {
+      return m_P;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  @Override
+  public double getI() {
+    m_thisMutex.lock();
+    try {
+      return m_I;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  @Override
+  public double getD() {
+    m_thisMutex.lock();
+    try {
+      return m_D;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Get the Feed forward coefficient.
+   *
+   * @return feed forward coefficient
+   */
+  public double getF() {
+    m_thisMutex.lock();
+    try {
+      return m_F;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Return the current PID result This is always centered on zero and constrained the the max and
+   * min outs.
+   *
+   * @return the latest calculated output
+   */
+  public double get() {
+    m_thisMutex.lock();
+    try {
+      return m_result;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the PID controller to consider the input to be continuous, Rather then using the max and
+   * min input range as constraints, it considers them to be the same point and automatically
+   * calculates the shortest route to the setpoint.
+   *
+   * @param continuous Set to true turns on continuous, false turns off continuous
+   */
+  public void setContinuous(boolean continuous) {
+    if (continuous && m_inputRange <= 0) {
+      throw new IllegalStateException("No input range set when calling setContinuous().");
+    }
+    m_thisMutex.lock();
+    try {
+      m_continuous = continuous;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the PID controller to consider the input to be continuous, Rather then using the max and
+   * min input range as constraints, it considers them to be the same point and automatically
+   * calculates the shortest route to the setpoint.
+   */
+  public void setContinuous() {
+    setContinuous(true);
+  }
+
+  /**
+   * Sets the maximum and minimum values expected from the input and setpoint.
+   *
+   * @param minimumInput the minimum value expected from the input
+   * @param maximumInput the maximum value expected from the input
+   */
+  public void setInputRange(double minimumInput, double maximumInput) {
+    m_thisMutex.lock();
+    try {
+      if (minimumInput > maximumInput) {
+        throw new BoundaryException("Lower bound is greater than upper bound");
+      }
+      m_minimumInput = minimumInput;
+      m_maximumInput = maximumInput;
+      m_inputRange = maximumInput - minimumInput;
+    } finally {
+      m_thisMutex.unlock();
+    }
+
+    setSetpoint(m_setpoint);
+  }
+
+  /**
+   * Sets the minimum and maximum values to write.
+   *
+   * @param minimumOutput the minimum percentage to write to the output
+   * @param maximumOutput the maximum percentage to write to the output
+   */
+  public void setOutputRange(double minimumOutput, double maximumOutput) {
+    m_thisMutex.lock();
+    try {
+      if (minimumOutput > maximumOutput) {
+        throw new BoundaryException("Lower bound is greater than upper bound");
+      }
+      m_minimumOutput = minimumOutput;
+      m_maximumOutput = maximumOutput;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the setpoint for the PIDController.
+   *
+   * @param setpoint the desired setpoint
+   */
+  @Override
+  public void setSetpoint(double setpoint) {
+    m_thisMutex.lock();
+    try {
+      if (m_maximumInput > m_minimumInput) {
+        if (setpoint > m_maximumInput) {
+          m_setpoint = m_maximumInput;
+        } else if (setpoint < m_minimumInput) {
+          m_setpoint = m_minimumInput;
+        } else {
+          m_setpoint = setpoint;
+        }
+      } else {
+        m_setpoint = setpoint;
+      }
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return the current setpoint
+   */
+  @Override
+  public double getSetpoint() {
+    m_thisMutex.lock();
+    try {
+      return m_setpoint;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the change in setpoint over time of the PIDController.
+   *
+   * @return the change in setpoint over time
+   */
+  public double getDeltaSetpoint() {
+    m_thisMutex.lock();
+    try {
+      return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the current difference of the input from the setpoint.
+   *
+   * @return the current error
+   */
+  @Override
+  public double getError() {
+    m_thisMutex.lock();
+    try {
+      return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Returns the current difference of the error over the past few iterations. You can specify the
+   * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
+   * used for the onTarget() function.
+   *
+   * @deprecated Use getError(), which is now already filtered.
+   * @return the current average of the error
+   */
+  @Deprecated
+  public double getAvgError() {
+    m_thisMutex.lock();
+    try {
+      return getError();
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Sets what type of input the PID controller will use.
+   *
+   * @param pidSource the type of input
+   */
+  public void setPIDSourceType(PIDSourceType pidSource) {
+    m_pidInput.setPIDSourceType(pidSource);
+  }
+
+  /**
+   * Returns the type of input the PID controller is using.
+   *
+   * @return the PID controller input type
+   */
+  public PIDSourceType getPIDSourceType() {
+    return m_pidInput.getPIDSourceType();
+  }
+
+  /**
+   * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
+   * the range or as an absolute value. The Tolerance object encapsulates those options in an
+   * object. Use it by creating the type of tolerance that you want to use: setTolerance(new
+   * PIDController.AbsoluteTolerance(0.1))
+   *
+   * @deprecated Use setPercentTolerance() instead.
+   * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
+   *     AbsoluteTolerance
+   */
+  @Deprecated
+  public void setTolerance(Tolerance tolerance) {
+    m_tolerance = tolerance;
+  }
+
+  /**
+   * Set the absolute error which is considered tolerable for use with OnTarget.
+   *
+   * @param absvalue absolute error which is tolerable in the units of the input object
+   */
+  public void setAbsoluteTolerance(double absvalue) {
+    m_thisMutex.lock();
+    try {
+      m_tolerance = new AbsoluteTolerance(absvalue);
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
+   * 15 percent)
+   *
+   * @param percentage percent error which is tolerable
+   */
+  public void setPercentTolerance(double percentage) {
+    m_thisMutex.lock();
+    try {
+      m_tolerance = new PercentageTolerance(percentage);
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the number of previous error samples to average for tolerancing. When determining whether a
+   * mechanism is on target, the user may want to use a rolling average of previous measurements
+   * instead of a precise position or velocity. This is useful for noisy sensors which return a few
+   * erroneous measurements when the mechanism is on target. However, the mechanism will not
+   * register as on target for at least the specified bufLength cycles.
+   *
+   * @deprecated Use a LinearFilter as the input.
+   * @param bufLength Number of previous cycles to average.
+   */
+  @Deprecated
+  public void setToleranceBuffer(int bufLength) {
+    m_thisMutex.lock();
+    try {
+      m_filter = LinearFilter.movingAverage(bufLength);
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Return true if the error is within the percentage of the total input range, determined by
+   * setTolerance. This assumes that the maximum and minimum input were set using setInput.
+   *
+   * @return true if the error is less than the tolerance
+   */
+  public boolean onTarget() {
+    m_thisMutex.lock();
+    try {
+      return m_tolerance.onTarget();
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /** Reset the previous error, the integral term, and disable the controller. */
+  @Override
+  public void reset() {
+    m_thisMutex.lock();
+    try {
+      m_prevError = 0;
+      m_totalError = 0;
+      m_result = 0;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /**
+   * Passes the output directly to setSetpoint().
+   *
+   * <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
+   * In that case, the output of the parent controller becomes the input (i.e., the reference) of
+   * the child.
+   *
+   * <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
+   */
+  @Override
+  public void pidWrite(double output) {
+    setSetpoint(output);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PIDController");
+    builder.setSafeState(this::reset);
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("f", this::getF, this::setF);
+    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+  }
+
+  /**
+   * Wraps error around for continuous inputs. The original error is returned if continuous mode is
+   * disabled. This is an unsynchronized function.
+   *
+   * @param error The current error of the PID controller.
+   * @return Error for continuous inputs.
+   */
+  protected double getContinuousError(double error) {
+    if (m_continuous && m_inputRange > 0) {
+      error %= m_inputRange;
+      if (Math.abs(error) > m_inputRange / 2) {
+        if (error > 0) {
+          return error - m_inputRange;
+        } else {
+          return error + m_inputRange;
+        }
+      }
+    }
+
+    return error;
+  }
+
+  private static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDController.java
new file mode 100644
index 0000000..ea7e98d
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDController.java
@@ -0,0 +1,180 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
+ * calculations, as well as writing the given PIDOutput.
+ *
+ * <p>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 {@link edu.wpi.first.math.controller.PIDController} instead.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public class PIDController extends PIDBase implements Controller {
+  Notifier m_controlLoop = new Notifier(this::calculate);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D, and F.
+   *
+   * @param Kp the proportional coefficient
+   * @param Ki the integral coefficient
+   * @param Kd the derivative coefficient
+   * @param Kf the feed forward term
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output percentage
+   * @param period the loop time for doing calculations in seconds. This particularly affects
+   *     calculations of the integral and differential terms. The default is 0.05 (50ms).
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(
+      double Kp,
+      double Ki,
+      double Kd,
+      double Kf,
+      PIDSource source,
+      PIDOutput output,
+      double period) {
+    super(Kp, Ki, Kd, Kf, source, output);
+    m_controlLoop.startPeriodic(period);
+  }
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D and period.
+   *
+   * @param Kp the proportional coefficient
+   * @param Ki the integral coefficient
+   * @param Kd the derivative coefficient
+   * @param source the PIDSource object that is used to get values
+   * @param output the PIDOutput object that is set to the output percentage
+   * @param period the loop time for doing calculations in seconds. This particularly affects
+   *     calculations of the integral and differential terms. The default is 0.05 (50ms).
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(
+      double Kp, double Ki, double Kd, PIDSource source, PIDOutput output, double period) {
+    this(Kp, Ki, Kd, 0.0, source, output, period);
+  }
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+   *
+   * @param Kp the proportional coefficient
+   * @param Ki the integral coefficient
+   * @param Kd the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output percentage
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
+    this(Kp, Ki, Kd, source, output, kDefaultPeriod);
+  }
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
+   *
+   * @param Kp the proportional coefficient
+   * @param Ki the integral coefficient
+   * @param Kd the derivative coefficient
+   * @param Kf the feed forward term
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output percentage
+   */
+  @SuppressWarnings("ParameterName")
+  public PIDController(
+      double Kp, double Ki, double Kd, double Kf, PIDSource source, PIDOutput output) {
+    this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
+  }
+
+  @Override
+  public void close() {
+    m_controlLoop.close();
+    m_thisMutex.lock();
+    try {
+      m_pidOutput = null;
+      m_pidInput = null;
+      m_controlLoop = null;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /** Begin running the PIDController. */
+  @Override
+  public void enable() {
+    m_thisMutex.lock();
+    try {
+      m_enabled = true;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /** Stop running the PIDController, this sets the output to zero before stopping. */
+  @Override
+  public void disable() {
+    // Ensures m_enabled check and pidWrite() call occur atomically
+    m_pidWriteMutex.lock();
+    try {
+      m_thisMutex.lock();
+      try {
+        m_enabled = false;
+      } finally {
+        m_thisMutex.unlock();
+      }
+
+      m_pidOutput.pidWrite(0);
+    } finally {
+      m_pidWriteMutex.unlock();
+    }
+  }
+
+  /**
+   * Set the enabled state of the PIDController.
+   *
+   * @param enable True to enable the PIDController.
+   */
+  public void setEnabled(boolean enable) {
+    if (enable) {
+      enable();
+    } else {
+      disable();
+    }
+  }
+
+  /**
+   * Return true if PIDController is enabled.
+   *
+   * @return True if PIDController is enabled.
+   */
+  public boolean isEnabled() {
+    m_thisMutex.lock();
+    try {
+      return m_enabled;
+    } finally {
+      m_thisMutex.unlock();
+    }
+  }
+
+  /** Reset the previous error, the integral term, and disable the controller. */
+  @Override
+  public void reset() {
+    disable();
+
+    super.reset();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    super.initSendable(builder);
+    builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
+  }
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
new file mode 100644
index 0000000..0e8b2f1
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+@Deprecated(since = "2020", forRemoval = true)
+@SuppressWarnings("SummaryJavadoc")
+public interface PIDInterface {
+  void setPID(double p, double i, double d);
+
+  double getP();
+
+  double getI();
+
+  double getD();
+
+  void setSetpoint(double setpoint);
+
+  double getSetpoint();
+
+  double getError();
+
+  void reset();
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
new file mode 100644
index 0000000..52f5a99
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows PIDController to write it's results to its output.
+ *
+ * @deprecated Use DoubleConsumer and new PIDController class.
+ */
+@FunctionalInterface
+@Deprecated(since = "2020", forRemoval = true)
+public interface PIDOutput {
+  /**
+   * Set the output to the value calculated by PIDController.
+   *
+   * @param output the value calculated by PIDController
+   */
+  void pidWrite(double output);
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
new file mode 100644
index 0000000..c50a988
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * This interface allows for PIDController to automatically read from this object.
+ *
+ * @deprecated Use DoubleSupplier and new PIDController class.
+ */
+@Deprecated(since = "2020", forRemoval = true)
+public interface PIDSource {
+  /**
+   * Set which parameter of the device you are using as a process control variable.
+   *
+   * @param pidSource An enum to select the parameter.
+   */
+  void setPIDSourceType(PIDSourceType pidSource);
+
+  /**
+   * Get which parameter of the device you are using as a process control variable.
+   *
+   * @return the currently selected PID source parameter
+   */
+  PIDSourceType getPIDSourceType();
+
+  /**
+   * Get the result to use in PIDController.
+   *
+   * @return the result to use in PIDController
+   */
+  double pidGet();
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
new file mode 100644
index 0000000..4018ba6
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/** A description for the type of output value to provide to a PIDController. */
+@Deprecated(since = "2020", forRemoval = true)
+public enum PIDSourceType {
+  kDisplacement,
+  kRate
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
index dcd4401..f674be3 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Button.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.buttons;
 
@@ -32,7 +29,7 @@
   /**
    * Constantly starts the given command while the button is held.
    *
-   * {@link Command#start()} will be called repeatedly while the button is held, and will be
+   * <p>{@link Command#start()} will be called repeatedly while the button is held, and will be
    * canceled when the button is released.
    *
    * @param command the command to start
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
index c2a6eb1..c77e9aa 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/InternalButton.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.buttons;
 
@@ -15,9 +12,7 @@
   private boolean m_pressed;
   private boolean m_inverted;
 
-  /**
-   * Creates an InternalButton that is not inverted.
-   */
+  /** Creates an InternalButton that is not inverted. */
   public InternalButton() {
     this(false);
   }
@@ -26,7 +21,7 @@
    * Creates an InternalButton which is inverted depending on the input.
    *
    * @param inverted if false, then this button is pressed when set to true, otherwise it is pressed
-   *                 when set to false.
+   *     when set to false.
    */
   public InternalButton(boolean inverted) {
     m_pressed = m_inverted = inverted;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
index 8d2b20e..f7f09e7 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/JoystickButton.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.buttons;
 
 import edu.wpi.first.wpilibj.GenericHID;
 
-/**
- * A {@link Button} that gets its state from a {@link GenericHID}.
- */
+/** A {@link Button} that gets its state from a {@link GenericHID}. */
 public class JoystickButton extends Button {
   private final GenericHID m_joystick;
   private final int m_buttonNumber;
@@ -19,8 +14,7 @@
   /**
    * Create a joystick button for triggering commands.
    *
-   * @param joystick     The GenericHID object that has the button (e.g. Joystick, KinectStick,
-   *                     etc)
+   * @param joystick The GenericHID object that has the button (e.g. Joystick, KinectStick, etc)
    * @param buttonNumber The button number (see {@link GenericHID#getRawButton(int) }
    */
   public JoystickButton(GenericHID joystick, int buttonNumber) {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
index 3176977..1351d16 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/NetworkButton.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.buttons;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 
-/**
- * A {@link Button} that uses a {@link NetworkTable} boolean field.
- */
+/** A {@link Button} that uses a {@link NetworkTable} boolean field. */
 public class NetworkButton extends Button {
   private final NetworkTableEntry m_entry;
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
index 39c891d..73db913 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/POVButton.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.buttons;
 
 import edu.wpi.first.wpilibj.GenericHID;
 
-/**
- * A {@link Button} that gets its state from a POV on a {@link GenericHID}.
- */
+/** A {@link Button} that gets its state from a POV on a {@link GenericHID}. */
 public class POVButton extends Button {
   private final GenericHID m_joystick;
   private final int m_angle;
@@ -31,8 +26,7 @@
   }
 
   /**
-   * Creates a POV button for triggering commands.
-   * By default, acts on POV 0
+   * Creates a POV button for triggering commands. By default, acts on POV 0
    *
    * @param joystick The GenericHID object that has the POV
    * @param angle The desired angle (e.g. 90, 270)
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
index f051776..22db6f6 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/buttons/Trigger.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.buttons;
 
-import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.command.Command;
 import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 
 /**
  * This class provides an easy way to link commands to inputs.
  *
- * <p>It is very easy to link a button to a command. For instance, you could link the trigger
- * button of a joystick to a "score" command.
+ * <p>It is very easy to link a button to a command. For instance, you could link the trigger button
+ * of a joystick to a "score" command.
  *
  * <p>It is encouraged that teams write a subclass of Trigger if they want to have something unusual
  * (for instance, if they want to react to the user holding a button while the robot is reading a
@@ -40,7 +37,6 @@
    *
    * @return whether get() return true or the internal table for SmartDashboard use is pressed.
    */
-  @SuppressWarnings("PMD.UselessParentheses")
   private boolean grab() {
     return get() || m_sendablePressed;
   }
@@ -70,7 +66,7 @@
   /**
    * Constantly starts the given command while the button is held.
    *
-   * {@link Command#start()} will be called repeatedly while the trigger is active, and will be
+   * <p>{@link Command#start()} will be called repeatedly while the trigger is active, and will be
    * canceled when the trigger becomes inactive.
    *
    * @param command the command to start
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
index 6233a43..234fed4 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Command.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
-import java.util.Enumeration;
-
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.RobotState;
-import edu.wpi.first.wpilibj.Sendable;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import java.util.Enumeration;
 
 /**
  * The Command class is at the very core of the entire command framework. Every command can be
@@ -22,16 +18,16 @@
  * execute()} until the {@link Command#isFinished() isFinished()} returns true. Once it does, {@link
  * Command#end() end()} will be called.
  *
- * <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called,
- * then the command will be stopped and {@link Command#interrupted() interrupted()} will be called.
+ * <p>However, if at any point while it is running {@link Command#cancel() cancel()} is called, then
+ * the command will be stopped and {@link Command#interrupted() interrupted()} will be called.
  *
  * <p>If a command uses a {@link Subsystem}, then it should specify that it does so by calling the
  * {@link Command#requires(Subsystem) requires(...)} method in its constructor. Note that a Command
  * may have multiple requirements, and {@link Command#requires(Subsystem) requires(...)} should be
  * called for each one.
  *
- * <p>If a command is running and a new command with shared requirements is started, then one of
- * two things will happen. If the active command is interruptible, then {@link Command#cancel()
+ * <p>If a command is running and a new command with shared requirements is started, then one of two
+ * things will happen. If the active command is interruptible, then {@link Command#cancel()
  * cancel()} will be called and the command will be removed to make way for the new one. If the
  * active command is not interruptible, the other one will not even be started, and the active one
  * will continue functioning.
@@ -41,64 +37,40 @@
  * @see IllegalUseOfCommandException
  */
 public abstract class Command implements Sendable, AutoCloseable {
-  /**
-   * The time since this command was initialized.
-   */
+  /** The time since this command was initialized. */
   private double m_startTime = -1;
 
-  /**
-   * The time (in seconds) before this command "times out" (or -1 if no timeout).
-   */
+  /** The time (in seconds) before this command "times out" (or -1 if no timeout). */
   private double m_timeout = -1;
 
-  /**
-   * Whether or not this command has been initialized.
-   */
+  /** Whether or not this command has been initialized. */
   private boolean m_initialized;
 
-  /**
-   * The required subsystems.
-   */
+  /** The required subsystems. */
   private final Set m_requirements = new Set();
 
-  /**
-   * Whether or not it is running.
-   */
+  /** Whether or not it is running. */
   private boolean m_running;
 
-  /**
-   * Whether or not it is interruptible.
-   */
+  /** Whether or not it is interruptible. */
   private boolean m_interruptible = true;
 
-  /**
-   * Whether or not it has been canceled.
-   */
+  /** Whether or not it has been canceled. */
   private boolean m_canceled;
 
-  /**
-   * Whether or not it has been locked.
-   */
+  /** Whether or not it has been locked. */
   private boolean m_locked;
 
-  /**
-   * Whether this command should run when the robot is disabled.
-   */
+  /** Whether this command should run when the robot is disabled. */
   private boolean m_runWhenDisabled;
 
-  /**
-   * Whether or not this command has completed running.
-   */
+  /** Whether or not this command has completed running. */
   private boolean m_completed;
 
-  /**
-   * The {@link CommandGroup} this is in.
-   */
+  /** The {@link CommandGroup} this is in. */
   private CommandGroup m_parent;
 
-  /**
-   * Creates a new command. The name of this command will be set to its class name.
-   */
+  /** Creates a new command. The name of this command will be set to its class name. */
   public Command() {
     String name = getClass().getName();
     SendableRegistry.add(this, name.substring(name.lastIndexOf('.') + 1));
@@ -149,7 +121,7 @@
   /**
    * Creates a new command with the given name.
    *
-   * @param name      the name for this command
+   * @param name the name for this command
    * @param subsystem the subsystem that this command requires
    * @throws IllegalArgumentException if name is null
    */
@@ -162,7 +134,7 @@
    * Creates a new command with the given timeout and a default name. The default name is the name
    * of the class.
    *
-   * @param timeout   the time (in seconds) before this command "times out"
+   * @param timeout the time (in seconds) before this command "times out"
    * @param subsystem the subsystem that this command requires
    * @throws IllegalArgumentException if given a negative timeout
    * @see Command#isTimedOut() isTimedOut()
@@ -175,7 +147,7 @@
   /**
    * Creates a new command with the given name and timeout.
    *
-   * @param name    the name of the command
+   * @param name the name of the command
    * @param timeout the time (in seconds) before this command "times out"
    * @throws IllegalArgumentException if given a negative timeout or name was null.
    * @see Command#isTimedOut() isTimedOut()
@@ -191,8 +163,8 @@
   /**
    * Creates a new command with the given name and timeout.
    *
-   * @param name      the name of the command
-   * @param timeout   the time (in seconds) before this command "times out"
+   * @param name the name of the command
+   * @param timeout the time (in seconds) before this command "times out"
    * @param subsystem the subsystem that this command requires
    * @throws IllegalArgumentException if given a negative timeout
    * @throws IllegalArgumentException if given a negative timeout or name was null.
@@ -239,9 +211,9 @@
    * <p>Note that the recommended way to call this method is in the constructor.
    *
    * @param subsystem the {@link Subsystem} required
-   * @throws IllegalArgumentException     if subsystem is null
+   * @throws IllegalArgumentException if subsystem is null
    * @throws IllegalUseOfCommandException if this command has started before or if it has been given
-   *                                      to a {@link CommandGroup}
+   *     to a {@link CommandGroup}
    * @see Subsystem
    */
   protected synchronized void requires(Subsystem subsystem) {
@@ -296,30 +268,20 @@
     return !isFinished();
   }
 
-  /**
-   * The initialize method is called the first time this Command is run after being started.
-   */
+  /** The initialize method is called the first time this Command is run after being started. */
   protected void initialize() {}
 
-  /**
-   * A shadow method called before {@link Command#initialize() initialize()}.
-   */
+  /** A shadow method called before {@link Command#initialize() initialize()}. */
   @SuppressWarnings("MethodName")
-  void _initialize() {
-  }
+  void _initialize() {}
 
-  /**
-   * The execute method is called repeatedly until this Command either finishes or is canceled.
-   */
+  /** The execute method is called repeatedly until this Command either finishes or is canceled. */
   @SuppressWarnings("MethodName")
   protected void execute() {}
 
-  /**
-   * A shadow method called before {@link Command#execute() execute()}.
-   */
+  /** A shadow method called before {@link Command#execute() execute()}. */
   @SuppressWarnings("MethodName")
-  void _execute() {
-  }
+  void _execute() {}
 
   /**
    * Returns whether this command is finished. If it is, then the command will be removed and {@link
@@ -329,9 +291,8 @@
    * method for time-sensitive commands.
    *
    * <p>Returning false will result in the command never ending automatically. It may still be
-   * canceled manually or interrupted by another command. Returning true will result in the
-   * command executing once and finishing immediately. We recommend using {@link InstantCommand}
-   * for this.
+   * canceled manually or interrupted by another command. Returning true will result in the command
+   * executing once and finishing immediately. We recommend using {@link InstantCommand} for this.
    *
    * @return whether this command is finished.
    * @see Command#isTimedOut() isTimedOut()
@@ -344,12 +305,9 @@
    */
   protected void end() {}
 
-  /**
-   * A shadow method called after {@link Command#end() end()}.
-   */
+  /** A shadow method called after {@link Command#end() end()}. */
   @SuppressWarnings("MethodName")
-  void _end() {
-  }
+  void _end() {}
 
   /**
    * Called when the command ends because somebody called {@link Command#cancel() cancel()} or
@@ -365,9 +323,7 @@
     end();
   }
 
-  /**
-   * A shadow method called after {@link Command#interrupted() interrupted()}.
-   */
+  /** A shadow method called after {@link Command#interrupted() interrupted()}. */
   @SuppressWarnings("MethodName")
   void _interrupted() {}
 
@@ -395,15 +351,13 @@
    * Subsystems}) of this command.
    *
    * @return the requirements (as an {@link Enumeration Enumeration} of {@link Subsystem
-   * Subsystems}) of this command
+   *     Subsystems}) of this command
    */
-  synchronized Enumeration getRequirements() {
+  synchronized Enumeration<?> getRequirements() {
     return m_requirements.getElements();
   }
 
-  /**
-   * Prevents further changes from being made.
-   */
+  /** Prevents further changes from being made. */
   synchronized void lockChanges() {
     m_locked = true;
   }
@@ -415,8 +369,8 @@
    */
   synchronized void validate(String message) {
     if (m_locked) {
-      throw new IllegalUseOfCommandException(message
-          + " after being started or being added to a command group");
+      throw new IllegalUseOfCommandException(
+          message + " after being started or being added to a command group");
     }
   }
 
@@ -445,18 +399,18 @@
   }
 
   /**
-   * Clears list of subsystem requirements. This is only used by
-   * {@link ConditionalCommand} so canceling the chosen command works properly
-   * in {@link CommandGroup}.
+   * Clears list of subsystem requirements. This is only used by {@link ConditionalCommand} so
+   * canceling the chosen command works properly in {@link CommandGroup}.
    */
   protected void clearRequirements() {
     m_requirements.clear();
   }
 
   /**
-   * Starts up the command. Gets the command ready to start. <p> Note that the command will
-   * eventually start, however it will not necessarily do so immediately, and may in fact be
-   * canceled before initialize is even called. </p>
+   * Starts up the command. Gets the command ready to start.
+   *
+   * <p>Note that the command will eventually start, however it will not necessarily do so
+   * immediately, and may in fact be canceled before initialize is even called.
    *
    * @throws IllegalUseOfCommandException if the command is a part of a CommandGroup
    */
@@ -495,18 +449,21 @@
   }
 
   /**
-   * This will cancel the current command. <p> This will cancel the current command eventually. It
-   * can be called multiple times. And it can be called when the command is not running. If the
-   * command is running though, then the command will be marked as canceled and eventually removed.
-   * </p> <p> A command can not be canceled if it is a part of a command group, you must cancel the
-   * command group instead. </p>
+   * This will cancel the current command.
+   *
+   * <p>This will cancel the current command eventually. It can be called multiple times. And it can
+   * be called when the command is not running. If the command is running though, then the command
+   * will be marked as canceled and eventually removed.
+   *
+   * <p>A command can not be canceled if it is a part of a command group, you must cancel the
+   * command group instead.
    *
    * @throws IllegalUseOfCommandException if this command is a part of a command group
    */
   public synchronized void cancel() {
     if (m_parent != null) {
-      throw new IllegalUseOfCommandException("Can not manually cancel a command in a command "
-          + "group");
+      throw new IllegalUseOfCommandException(
+          "Can not manually cancel a command in a command " + "group");
     }
     _cancel();
   }
@@ -604,7 +561,6 @@
    *
    * @return Name
    */
-  @Override
   public String getName() {
     return SendableRegistry.getName(this);
   }
@@ -614,7 +570,6 @@
    *
    * @param name name
    */
-  @Override
   public void setName(String name) {
     SendableRegistry.setName(this, name);
   }
@@ -624,7 +579,6 @@
    *
    * @return Subsystem name
    */
-  @Override
   public String getSubsystem() {
     return SendableRegistry.getSubsystem(this);
   }
@@ -634,7 +588,6 @@
    *
    * @param subsystem subsystem name
    */
-  @Override
   public void setSubsystem(String subsystem) {
     SendableRegistry.setSubsystem(this, subsystem);
   }
@@ -653,17 +606,20 @@
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Command");
     builder.addStringProperty(".name", this::getName, null);
-    builder.addBooleanProperty("running", this::isRunning, value -> {
-      if (value) {
-        if (!isRunning()) {
-          start();
-        }
-      } else {
-        if (isRunning()) {
-          cancel();
-        }
-      }
-    });
+    builder.addBooleanProperty(
+        "running",
+        this::isRunning,
+        value -> {
+          if (value) {
+            if (!isRunning()) {
+              start();
+            }
+          } else {
+            if (isRunning()) {
+              cancel();
+            }
+          }
+        });
     builder.addBooleanProperty(".isParented", this::isParented, null);
   }
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
index ef4f353..18e0ca6 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/CommandGroup.java
@@ -1,61 +1,51 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
+import static java.util.Objects.requireNonNull;
+
 import java.util.Enumeration;
 import java.util.Vector;
 
-import static java.util.Objects.requireNonNull;
-
 /**
  * A {@link CommandGroup} is a list of commands which are executed in sequence.
  *
- * <p> Commands in a {@link CommandGroup} are added using the {@link
+ * <p>Commands in a {@link CommandGroup} are added using the {@link
  * CommandGroup#addSequential(Command) addSequential(...)} method and are called sequentially.
  * {@link CommandGroup CommandGroups} are themselves {@link Command commands} and can be given to
- * other {@link CommandGroup CommandGroups}. </p>
+ * other {@link CommandGroup CommandGroups}.
  *
- * <p> {@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command
+ * <p>{@link CommandGroup CommandGroups} will carry all of the requirements of their {@link Command
  * subcommands}. Additional requirements can be specified by calling {@link
- * CommandGroup#requires(Subsystem) requires(...)} normally in the constructor. </P>
+ * CommandGroup#requires(Subsystem) requires(...)} normally in the constructor.
  *
- * <p> CommandGroups can also execute commands in parallel, simply by adding them using {@link
- * CommandGroup#addParallel(Command) addParallel(...)}. </p>
+ * <p>CommandGroups can also execute commands in parallel, simply by adding them using {@link
+ * CommandGroup#addParallel(Command) addParallel(...)}.
  *
  * @see Command
  * @see Subsystem
  * @see IllegalUseOfCommandException
  */
 public class CommandGroup extends Command {
-  /**
-   * The commands in this group (stored in entries).
-   */
-  @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+  /** The commands in this group (stored in entries). */
+  @SuppressWarnings("PMD.UseArrayListInsteadOfVector")
   private final Vector<Entry> m_commands = new Vector<>();
   /*
    * Intentionally package private
    */
-  /**
-   * The active children in this group (stored in entries).
-   */
-  @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+  /** The active children in this group (stored in entries). */
+  @SuppressWarnings("PMD.UseArrayListInsteadOfVector")
   final Vector<Entry> m_children = new Vector<>();
-  /**
-   * The current command, -1 signifies that none have been run.
-   */
+  /** The current command, -1 signifies that none have been run. */
   private int m_currentCommandIndex = -1;
 
   /**
    * Creates a new {@link CommandGroup CommandGroup}. The name of this command will be set to its
    * class name.
    */
-  public CommandGroup() {
-  }
+  public CommandGroup() {}
 
   /**
    * Creates a new {@link CommandGroup CommandGroup} with the given name.
@@ -71,16 +61,16 @@
    * Adds a new {@link Command Command} to the group. The {@link Command Command} will be started
    * after all the previously added {@link Command Commands}.
    *
-   * <p> Note that any requirements the given {@link Command Command} has will be added to the
-   * group. For this reason, a {@link Command Command's} requirements can not be changed after being
-   * added to a group. </p>
+   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
+   * For this reason, a {@link Command Command's} requirements can not be changed after being added
+   * to a group.
    *
-   * <p> It is recommended that this method be called in the constructor. </p>
+   * <p>It is recommended that this method be called in the constructor.
    *
    * @param command The {@link Command Command} to be added
    * @throws IllegalUseOfCommandException if the group has been started before or been given to
-   *                                      another group
-   * @throws IllegalArgumentException     if command is null
+   *     another group
+   * @throws IllegalArgumentException if command is null
    */
   public final synchronized void addSequential(Command command) {
     validate("Can not add new command to command group");
@@ -91,7 +81,7 @@
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -100,22 +90,22 @@
    * Adds a new {@link Command Command} to the group with a given timeout. The {@link Command
    * Command} will be started after all the previously added commands.
    *
-   * <p> Once the {@link Command Command} is started, it will be run until it finishes or the time
+   * <p>Once the {@link Command Command} is started, it will be run until it finishes or the time
    * expires, whichever is sooner. Note that the given {@link Command Command} will have no
-   * knowledge that it is on a timer. </p>
+   * knowledge that it is on a timer.
    *
-   * <p> Note that any requirements the given {@link Command Command} has will be added to the
-   * group. For this reason, a {@link Command Command's} requirements can not be changed after being
-   * added to a group. </p>
+   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
+   * For this reason, a {@link Command Command's} requirements can not be changed after being added
+   * to a group.
    *
-   * <p> It is recommended that this method be called in the constructor. </p>
+   * <p>It is recommended that this method be called in the constructor.
    *
    * @param command The {@link Command Command} to be added
    * @param timeout The timeout (in seconds)
    * @throws IllegalUseOfCommandException if the group has been started before or been given to
-   *                                      another group or if the {@link Command Command} has been
-   *                                      started before or been given to another group
-   * @throws IllegalArgumentException     if command is null or timeout is negative
+   *     another group or if the {@link Command Command} has been started before or been given to
+   *     another group
+   * @throws IllegalArgumentException if command is null or timeout is negative
    */
   public final synchronized void addSequential(Command command, double timeout) {
     validate("Can not add new command to command group");
@@ -129,7 +119,7 @@
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.IN_SEQUENCE, timeout));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -138,22 +128,22 @@
    * Adds a new child {@link Command} to the group. The {@link Command} will be started after all
    * the previously added {@link Command Commands}.
    *
-   * <p> Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
+   * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
    * same time as the subsequent {@link Command Commands}. The child will run until either it
    * finishes, a new child with conflicting requirements is started, or the main sequence runs a
    * {@link Command} with conflicting requirements. In the latter two cases, the child will be
-   * canceled even if it says it can't be interrupted. </p>
+   * canceled even if it says it can't be interrupted.
    *
-   * <p> Note that any requirements the given {@link Command Command} has will be added to the
-   * group. For this reason, a {@link Command Command's} requirements can not be changed after being
-   * added to a group. </p>
+   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
+   * For this reason, a {@link Command Command's} requirements can not be changed after being added
+   * to a group.
    *
-   * <p> It is recommended that this method be called in the constructor. </p>
+   * <p>It is recommended that this method be called in the constructor.
    *
    * @param command The command to be added
    * @throws IllegalUseOfCommandException if the group has been started before or been given to
-   *                                      another command group
-   * @throws IllegalArgumentException     if command is null
+   *     another command group
+   * @throws IllegalArgumentException if command is null
    */
   public final synchronized void addParallel(Command command) {
     requireNonNull(command, "Provided command was null");
@@ -162,7 +152,7 @@
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -171,27 +161,27 @@
    * Adds a new child {@link Command} to the group with the given timeout. The {@link Command} will
    * be started after all the previously added {@link Command Commands}.
    *
-   * <p> Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
+   * <p>Once the {@link Command Command} is started, it will run until it finishes, is interrupted,
    * or the time expires, whichever is sooner. Note that the given {@link Command Command} will have
-   * no knowledge that it is on a timer. </p>
+   * no knowledge that it is on a timer.
    *
-   * <p> Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
+   * <p>Instead of waiting for the child to finish, a {@link CommandGroup} will have it run at the
    * same time as the subsequent {@link Command Commands}. The child will run until either it
    * finishes, the timeout expires, a new child with conflicting requirements is started, or the
    * main sequence runs a {@link Command} with conflicting requirements. In the latter two cases,
-   * the child will be canceled even if it says it can't be interrupted. </p>
+   * the child will be canceled even if it says it can't be interrupted.
    *
-   * <p> Note that any requirements the given {@link Command Command} has will be added to the
-   * group. For this reason, a {@link Command Command's} requirements can not be changed after being
-   * added to a group. </p>
+   * <p>Note that any requirements the given {@link Command Command} has will be added to the group.
+   * For this reason, a {@link Command Command's} requirements can not be changed after being added
+   * to a group.
    *
-   * <p> It is recommended that this method be called in the constructor. </p>
+   * <p>It is recommended that this method be called in the constructor.
    *
    * @param command The command to be added
    * @param timeout The timeout (in seconds)
    * @throws IllegalUseOfCommandException if the group has been started before or been given to
-   *                                      another command group
-   * @throws IllegalArgumentException     if command is null
+   *     another command group
+   * @throws IllegalArgumentException if command is null
    */
   public final synchronized void addParallel(Command command, double timeout) {
     requireNonNull(command, "Provided command was null");
@@ -203,7 +193,7 @@
     command.setParent(this);
 
     m_commands.addElement(new Entry(command, Entry.BRANCH_CHILD, timeout));
-    for (Enumeration e = command.getRequirements(); e.hasMoreElements(); ) {
+    for (Enumeration<?> e = command.getRequirements(); e.hasMoreElements(); ) {
       requires((Subsystem) e.nextElement());
     }
   }
@@ -215,7 +205,7 @@
   }
 
   @Override
-  @SuppressWarnings({"MethodName", "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+  @SuppressWarnings("MethodName")
   void _execute() {
     Entry entry = null;
     Command cmd = null;
@@ -293,7 +283,7 @@
       cmd.removed();
     }
 
-    Enumeration children = m_children.elements();
+    Enumeration<?> children = m_children.elements();
     while (children.hasMoreElements()) {
       Command cmd = ((Entry) children.nextElement()).m_command;
       cmd._cancel();
@@ -312,8 +302,8 @@
    * Returns true if all the {@link Command Commands} in this group have been started and have
    * finished.
    *
-   * <p> Teams may override this method, although they should probably reference super.isFinished()
-   * if they do. </p>
+   * <p>Teams may override this method, although they should probably reference super.isFinished()
+   * if they do.
    *
    * @return whether this {@link CommandGroup} is finished
    */
@@ -324,27 +314,23 @@
 
   // Can be overwritten by teams
   @Override
-  protected void initialize() {
-  }
+  protected void initialize() {}
 
   // Can be overwritten by teams
   @Override
-  protected void execute() {
-  }
+  protected void execute() {}
 
   // Can be overwritten by teams
   @Override
-  protected void end() {
-  }
+  protected void end() {}
 
   // Can be overwritten by teams
   @Override
-  protected void interrupted() {
-  }
+  protected void interrupted() {}
 
   /**
    * Returns whether or not this group is interruptible. A command group will be uninterruptible if
-   * {@link CommandGroup#setInterruptible(boolean) setInterruptable(false)} was called or if it is
+   * {@link CommandGroup#setInterruptible(boolean) setInterruptible(false)} was called or if it is
    * currently running an uninterruptible command or child.
    *
    * @return whether or not this {@link CommandGroup} is interruptible.
@@ -375,7 +361,7 @@
     for (int i = 0; i < m_children.size(); i++) {
       Command child = m_children.elementAt(i).m_command;
 
-      Enumeration requirements = command.getRequirements();
+      Enumeration<?> requirements = command.getRequirements();
 
       while (requirements.hasMoreElements()) {
         Object requirement = requirements.nextElement();
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
index 01d316e..80c2903 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/ConditionalCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -12,55 +9,41 @@
 /**
  * A {@link ConditionalCommand} is a {@link Command} that starts one of two commands.
  *
- * <p>
- * A {@link ConditionalCommand} uses m_condition to determine whether it should run m_onTrue or
+ * <p>A {@link ConditionalCommand} uses m_condition to determine whether it should run m_onTrue or
  * m_onFalse.
- * </p>
  *
- * <p>
- * A {@link ConditionalCommand} adds the proper {@link Command} to the {@link Scheduler} during
+ * <p>A {@link ConditionalCommand} adds the proper {@link Command} to the {@link Scheduler} during
  * {@link ConditionalCommand#initialize()} and then {@link ConditionalCommand#isFinished()} will
  * return true once that {@link Command} has finished executing.
- * </p>
  *
- * <p>
- * If no {@link Command} is specified for m_onFalse, the occurrence of that condition will be a
+ * <p>If no {@link Command} is specified for m_onFalse, the occurrence of that condition will be a
  * no-op.
- * </p>
  *
- * <p>
- * A ConditionalCommand will require the superset of subsystems of the onTrue
- * and onFalse commands.
- * </p>
+ * <p>A ConditionalCommand will require the superset of subsystems of the onTrue and onFalse
+ * commands.
  *
  * @see Command
  * @see Scheduler
  */
 public abstract class ConditionalCommand extends Command {
-  /**
-   * The Command to execute if {@link ConditionalCommand#condition()} returns true.
-   */
+  /** The Command to execute if {@link ConditionalCommand#condition()} returns true. */
   private Command m_onTrue;
 
-  /**
-   * The Command to execute if {@link ConditionalCommand#condition()} returns false.
-   */
+  /** The Command to execute if {@link ConditionalCommand#condition()} returns false. */
   private Command m_onFalse;
 
-  /**
-   * Stores command chosen by condition.
-   */
+  /** Stores command chosen by condition. */
   private Command m_chosenCommand;
 
   private void requireAll() {
     if (m_onTrue != null) {
-      for (Enumeration e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
+      for (Enumeration<?> e = m_onTrue.getRequirements(); e.hasMoreElements(); ) {
         requires((Subsystem) e.nextElement());
       }
     }
 
     if (m_onFalse != null) {
-      for (Enumeration e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
+      for (Enumeration<?> e = m_onFalse.getRequirements(); e.hasMoreElements(); ) {
         requires((Subsystem) e.nextElement());
       }
     }
@@ -128,9 +111,7 @@
    */
   protected abstract boolean condition();
 
-  /**
-   * Calls {@link ConditionalCommand#condition()} and runs the proper command.
-   */
+  /** Calls {@link ConditionalCommand#condition()} and runs the proper command. */
   @Override
   protected void _initialize() {
     if (condition()) {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
index 4ed7459..0579fc7 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/IllegalUseOfCommandException.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -11,20 +8,16 @@
  * This exception will be thrown if a command is used illegally. There are several ways for this to
  * happen.
  *
- * <p> Basically, a command becomes "locked" after it is first started or added to a command group.
- * </p>
+ * <p>Basically, a command becomes "locked" after it is first started or added to a command group.
  *
- * <p> This exception should be thrown if (after a command has been locked) its requirements change,
+ * <p>This exception should be thrown if (after a command has been locked) its requirements change,
  * it is put into multiple command groups, it is started from outside its command group, or it adds
- * a new child. </p>
+ * a new child.
  */
 @SuppressWarnings("serial")
 public class IllegalUseOfCommandException extends RuntimeException {
-  /**
-   * Instantiates an {@link IllegalUseOfCommandException}.
-   */
-  public IllegalUseOfCommandException() {
-  }
+  /** Instantiates an {@link IllegalUseOfCommandException}. */
+  public IllegalUseOfCommandException() {}
 
   /**
    * Instantiates an {@link IllegalUseOfCommandException} with the given message.
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
index f255e48..d40f626 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/InstantCommand.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 /**
  * This command will execute once, then finish immediately afterward.
  *
- * <p>Subclassing {@link InstantCommand} is shorthand for returning true from
- * {@link Command isFinished}.
+ * <p>Subclassing {@link InstantCommand} is shorthand for returning true from {@link Command
+ * isFinished}.
  */
 public class InstantCommand extends Command {
   private Runnable m_func;
 
-  public InstantCommand() {
-  }
+  public InstantCommand() {}
 
   /**
    * Creates a new {@link InstantCommand InstantCommand} with the given name.
@@ -40,7 +36,7 @@
   /**
    * Creates a new {@link InstantCommand InstantCommand} with the given name and requirement.
    *
-   * @param name      the name for this command
+   * @param name the name for this command
    * @param subsystem the subsystem this command requires
    */
   public InstantCommand(String name, Subsystem subsystem) {
@@ -71,7 +67,7 @@
    * Creates a new {@link InstantCommand InstantCommand}.
    *
    * @param requirement the subsystem this command requires
-   * @param func        the function to run when {@link Command#initialize() initialize()} is run
+   * @param func the function to run when {@link Command#initialize() initialize()} is run
    */
   public InstantCommand(Subsystem requirement, Runnable func) {
     super(requirement);
@@ -81,9 +77,9 @@
   /**
    * Creates a new {@link InstantCommand InstantCommand}.
    *
-   * @param name        the name for this command
+   * @param name the name for this command
    * @param requirement the subsystem this command requires
-   * @param func        the function to run when {@link Command#initialize() initialize()} is run
+   * @param func the function to run when {@link Command#initialize() initialize()} is run
    */
   public InstantCommand(String name, Subsystem requirement, Runnable func) {
     super(name, requirement);
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
index 78da002..365667a 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/LinkedListElement.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
-/**
- * An element that is in a LinkedList.
- */
+/** An element that is in a LinkedList. */
 class LinkedListElement {
   private LinkedListElement m_next;
   private LinkedListElement m_previous;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
index 8b60254..0334316 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDCommand.java
@@ -1,62 +1,52 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
+import edu.wpi.first.util.sendable.SendableBuilder;
 import edu.wpi.first.wpilibj.PIDController;
 import edu.wpi.first.wpilibj.PIDOutput;
 import edu.wpi.first.wpilibj.PIDSource;
 import edu.wpi.first.wpilibj.PIDSourceType;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
 
 /**
  * This class defines a {@link Command} which interacts heavily with a PID loop.
  *
- * <p> It provides some convenience methods to run an internal {@link PIDController} . It will also
+ * <p>It provides some convenience methods to run an internal {@link PIDController} . It will also
  * start and stop said {@link PIDController} when the {@link PIDCommand} is first initialized and
- * ended/interrupted. </p>
+ * ended/interrupted.
  */
 public abstract class PIDCommand extends Command {
-  /**
-   * The internal {@link PIDController}.
-   */
+  /** The internal {@link PIDController}. */
   private final PIDController m_controller;
-  /**
-   * An output which calls {@link PIDCommand#usePIDOutput(double)}.
-   */
+  /** An output which calls {@link PIDCommand#usePIDOutput(double)}. */
   private final PIDOutput m_output = this::usePIDOutput;
-  /**
-   * A source which calls {@link PIDCommand#returnPIDInput()}.
-   */
-  private final PIDSource m_source = new PIDSource() {
-    @Override
-    public void setPIDSourceType(PIDSourceType pidSource) {
-    }
+  /** A source which calls {@link PIDCommand#returnPIDInput()}. */
+  private final PIDSource m_source =
+      new PIDSource() {
+        @Override
+        public void setPIDSourceType(PIDSourceType pidSource) {}
 
-    @Override
-    public PIDSourceType getPIDSourceType() {
-      return PIDSourceType.kDisplacement;
-    }
+        @Override
+        public PIDSourceType getPIDSourceType() {
+          return PIDSourceType.kDisplacement;
+        }
 
-    @Override
-    public double pidGet() {
-      return returnPIDInput();
-    }
-  };
+        @Override
+        public double pidGet() {
+          return returnPIDInput();
+        }
+      };
 
   /**
    * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
    *
    * @param name the name of the command
-   * @param p    the proportional value
-   * @param i    the integral value
-   * @param d    the derivative value
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(String name, double p, double i, double d) {
     super(name);
     m_controller = new PIDController(p, i, d, m_source, m_output);
@@ -66,13 +56,12 @@
    * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
    * the time between PID loop calculations to be equal to the given period.
    *
-   * @param name   the name
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
+   * @param name the name
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    * @param period the time (in seconds) between calculations
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(String name, double p, double i, double d, double period) {
     super(name);
     m_controller = new PIDController(p, i, d, m_source, m_output, period);
@@ -86,7 +75,6 @@
    * @param i the integral value
    * @param d the derivative value
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(double p, double i, double d) {
     m_controller = new PIDController(p, i, d, m_source, m_output);
   }
@@ -96,12 +84,11 @@
    * class name as its name. It will also space the time between PID loop calculations to be equal
    * to the given period.
    *
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    * @param period the time (in seconds) between calculations
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(double p, double i, double d, double period) {
     m_controller = new PIDController(p, i, d, m_source, m_output, period);
   }
@@ -109,13 +96,12 @@
   /**
    * Instantiates a {@link PIDCommand} that will use the given p, i and d values.
    *
-   * @param name      the name of the command
-   * @param p         the proportional value
-   * @param i         the integral value
-   * @param d         the derivative value
+   * @param name the name of the command
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    * @param subsystem the subsystem that this command requires
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(String name, double p, double i, double d, Subsystem subsystem) {
     super(name, subsystem);
     m_controller = new PIDController(p, i, d, m_source, m_output);
@@ -125,16 +111,14 @@
    * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will also space
    * the time between PID loop calculations to be equal to the given period.
    *
-   * @param name      the name
-   * @param p         the proportional value
-   * @param i         the integral value
-   * @param d         the derivative value
-   * @param period    the time (in seconds) between calculations
+   * @param name the name
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   * @param period the time (in seconds) between calculations
    * @param subsystem the subsystem that this command requires
    */
-  @SuppressWarnings("ParameterName")
-  public PIDCommand(String name, double p, double i, double d, double period,
-                    Subsystem subsystem) {
+  public PIDCommand(String name, double p, double i, double d, double period, Subsystem subsystem) {
     super(name, subsystem);
     m_controller = new PIDController(p, i, d, m_source, m_output, period);
   }
@@ -143,12 +127,11 @@
    * Instantiates a {@link PIDCommand} that will use the given p, i and d values. It will use the
    * class name as its name.
    *
-   * @param p         the proportional value
-   * @param i         the integral value
-   * @param d         the derivative value
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    * @param subsystem the subsystem that this command requires
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(double p, double i, double d, Subsystem subsystem) {
     super(subsystem);
     m_controller = new PIDController(p, i, d, m_source, m_output);
@@ -159,13 +142,12 @@
    * class name as its name. It will also space the time between PID loop calculations to be equal
    * to the given period.
    *
-   * @param p         the proportional value
-   * @param i         the integral value
-   * @param d         the derivative value
-   * @param period    the time (in seconds) between calculations
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   * @param period the time (in seconds) between calculations
    * @param subsystem the subsystem that this command requires
    */
-  @SuppressWarnings("ParameterName")
   public PIDCommand(double p, double i, double d, double period, Subsystem subsystem) {
     super(subsystem);
     m_controller = new PIDController(p, i, d, m_source, m_output, period);
@@ -264,8 +246,8 @@
 
   /**
    * Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
-   * This method is a good time to set motor values, maybe something along the lines of
-   * <code>driveline.tankDrive(output, -output)</code>
+   * This method is a good time to set motor values, maybe something along the lines of <code>
+   * driveline.tankDrive(output, -output)</code>
    *
    * <p>All subclasses of {@link PIDCommand} must override this method.
    *
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
index 7bb6331..bf70f91 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PIDSubsystem.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -17,48 +14,40 @@
  * {@link PIDController} almost constantly (for instance, an elevator which attempts to stay at a
  * constant height).
  *
- * <p>It provides some convenience methods to run an internal {@link PIDController} . It also
- * allows access to the internal {@link PIDController} in order to give total control to the
- * programmer.
+ * <p>It provides some convenience methods to run an internal {@link PIDController} . It also allows
+ * access to the internal {@link PIDController} in order to give total control to the programmer.
  */
 public abstract class PIDSubsystem extends Subsystem {
-  /**
-   * The internal {@link PIDController}.
-   */
+  /** The internal {@link PIDController}. */
   private final PIDController m_controller;
-  /**
-   * An output which calls {@link PIDCommand#usePIDOutput(double)}.
-   */
+  /** An output which calls {@link PIDCommand#usePIDOutput(double)}. */
   private final PIDOutput m_output = this::usePIDOutput;
 
-  /**
-   * A source which calls {@link PIDCommand#returnPIDInput()}.
-   */
-  private final PIDSource m_source = new PIDSource() {
-    @Override
-    public void setPIDSourceType(PIDSourceType pidSource) {
-    }
+  /** A source which calls {@link PIDCommand#returnPIDInput()}. */
+  private final PIDSource m_source =
+      new PIDSource() {
+        @Override
+        public void setPIDSourceType(PIDSourceType pidSource) {}
 
-    @Override
-    public PIDSourceType getPIDSourceType() {
-      return PIDSourceType.kDisplacement;
-    }
+        @Override
+        public PIDSourceType getPIDSourceType() {
+          return PIDSourceType.kDisplacement;
+        }
 
-    @Override
-    public double pidGet() {
-      return returnPIDInput();
-    }
-  };
+        @Override
+        public double pidGet() {
+          return returnPIDInput();
+        }
+      };
 
   /**
    * Instantiates a {@link PIDSubsystem} that will use the given p, i and d values.
    *
    * @param name the name
-   * @param p    the proportional value
-   * @param i    the integral value
-   * @param d    the derivative value
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    */
-  @SuppressWarnings("ParameterName")
   public PIDSubsystem(String name, double p, double i, double d) {
     super(name);
     m_controller = new PIDController(p, i, d, m_source, m_output);
@@ -69,12 +58,11 @@
    * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values.
    *
    * @param name the name
-   * @param p    the proportional value
-   * @param i    the integral value
-   * @param d    the derivative value
-   * @param f    the feed forward value
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   * @param f the feed forward value
    */
-  @SuppressWarnings("ParameterName")
   public PIDSubsystem(String name, double p, double i, double d, double f) {
     super(name);
     m_controller = new PIDController(p, i, d, f, m_source, m_output);
@@ -85,14 +73,13 @@
    * Instantiates a {@link PIDSubsystem} that will use the given p, i, d, and f values. It will also
    * space the time between PID loop calculations to be equal to the given period.
    *
-   * @param name   the name
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
-   * @param f      the feed forward value
+   * @param name the name
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   * @param f the feed forward value
    * @param period the time (in seconds) between calculations
    */
-  @SuppressWarnings("ParameterName")
   public PIDSubsystem(String name, double p, double i, double d, double f, double period) {
     super(name);
     m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
@@ -107,7 +94,6 @@
    * @param i the integral value
    * @param d the derivative value
    */
-  @SuppressWarnings("ParameterName")
   public PIDSubsystem(double p, double i, double d) {
     m_controller = new PIDController(p, i, d, m_source, m_output);
     addChild("PIDController", m_controller);
@@ -118,13 +104,12 @@
    * the class name as its name. It will also space the time between PID loop calculations to be
    * equal to the given period.
    *
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
-   * @param f      the feed forward coefficient
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
+   * @param f the feed forward coefficient
    * @param period the time (in seconds) between calculations
    */
-  @SuppressWarnings("ParameterName")
   public PIDSubsystem(double p, double i, double d, double f, double period) {
     m_controller = new PIDController(p, i, d, f, m_source, m_output, period);
     addChild("PIDController", m_controller);
@@ -135,12 +120,11 @@
    * class name as its name. It will also space the time between PID loop calculations to be equal
    * to the given period.
    *
-   * @param p      the proportional value
-   * @param i      the integral value
-   * @param d      the derivative value
+   * @param p the proportional value
+   * @param i the integral value
+   * @param d the derivative value
    * @param period the time (in seconds) between calculations
    */
-  @SuppressWarnings("ParameterName")
   public PIDSubsystem(double p, double i, double d, double period) {
     m_controller = new PIDController(p, i, d, m_source, m_output, period);
     addChild("PIDController", m_controller);
@@ -156,7 +140,6 @@
     return m_controller;
   }
 
-
   /**
    * Adds the given value to the setpoint. If {@link PIDSubsystem#setInputRange(double, double)
    * setInputRange(...)} was used, then the bounds will still be honored by this method.
@@ -222,7 +205,6 @@
    *
    * @param t the absolute tolerance
    */
-  @SuppressWarnings("ParameterName")
   public void setAbsoluteTolerance(double t) {
     m_controller.setAbsoluteTolerance(t);
   }
@@ -233,7 +215,6 @@
    *
    * @param p the percent tolerance
    */
-  @SuppressWarnings("ParameterName")
   public void setPercentTolerance(double p) {
     m_controller.setPercentTolerance(p);
   }
@@ -251,8 +232,8 @@
   /**
    * Returns the input for the pid loop.
    *
-   * <p>It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then
-   * it should return the angle of the gyro.
+   * <p>It returns the input for the pid loop, so if this Subsystem was based off of a gyro, then it
+   * should return the angle of the gyro.
    *
    * <p>All subclasses of {@link PIDSubsystem} must override this method.
    *
@@ -262,8 +243,8 @@
 
   /**
    * Uses the value that the pid loop calculated. The calculated value is the "output" parameter.
-   * This method is a good time to set motor values, maybe something along the lines of
-   * <code>driveline.tankDrive(output, -output)</code>.
+   * This method is a good time to set motor values, maybe something along the lines of <code>
+   * driveline.tankDrive(output, -output)</code>.
    *
    * <p>All subclasses of {@link PIDSubsystem} must override this method.
    *
@@ -271,16 +252,12 @@
    */
   protected abstract void usePIDOutput(double output);
 
-  /**
-   * Enables the internal {@link PIDController}.
-   */
+  /** Enables the internal {@link PIDController}. */
   public void enable() {
     m_controller.enable();
   }
 
-  /**
-   * Disables the internal {@link PIDController}.
-   */
+  /** Disables the internal {@link PIDController}. */
   public void disable() {
     m_controller.disable();
   }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
index b8d7fed..5013b1b 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/PrintCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -13,9 +10,7 @@
  * it reaches a certain point.
  */
 public class PrintCommand extends InstantCommand {
-  /**
-   * The message to print out.
-   */
+  /** The message to print out. */
   private final String m_message;
 
   /**
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
index c48f604..ca1ed33 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Scheduler.java
@@ -1,42 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
-import java.util.Enumeration;
-import java.util.Hashtable;
-import java.util.Vector;
-
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
 import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import java.util.Enumeration;
+import java.util.Hashtable;
+import java.util.Map;
+import java.util.Vector;
 
 /**
  * The {@link Scheduler} is a singleton which holds the top-level running commands. It is in charge
  * of both calling the command's {@link Command#run() run()} method and to make sure that there are
  * no two commands with conflicting requirements running.
  *
- * <p> It is fine if teams wish to take control of the {@link Scheduler} themselves, all that needs
+ * <p>It is fine if teams wish to take control of the {@link Scheduler} themselves, all that needs
  * to be done is to call {@link Scheduler#getInstance() Scheduler.getInstance()}.{@link
  * Scheduler#run() run()} often to have {@link Command Commands} function correctly. However, this
- * is already done for you if you use the CommandBased Robot template. </p>
+ * is already done for you if you use the CommandBased Robot template.
  *
  * @see Command
  */
-public final class Scheduler implements Sendable, AutoCloseable {
-  /**
-   * The Singleton Instance.
-   */
+public final class Scheduler implements NTSendable, AutoCloseable {
+  /** The Singleton Instance. */
   private static Scheduler instance;
 
   /**
@@ -51,57 +46,42 @@
     return instance;
   }
 
-  /**
-   * A hashtable of active {@link Command Commands} to their {@link LinkedListElement}.
-   */
-  @SuppressWarnings("PMD.LooseCoupling")
-  private final Hashtable<Command, LinkedListElement> m_commandTable = new Hashtable<>();
-  /**
-   * The {@link Set} of all {@link Subsystem Subsystems}.
-   */
+  /** A hashtable of active {@link Command Commands} to their {@link LinkedListElement}. */
+  private final Map<Command, LinkedListElement> m_commandTable = new Hashtable<>();
+  /** The {@link Set} of all {@link Subsystem Subsystems}. */
   private final Set m_subsystems = new Set();
-  /**
-   * The first {@link Command} in the list.
-   */
+  /** The first {@link Command} in the list. */
   private LinkedListElement m_firstCommand;
-  /**
-   * The last {@link Command} in the list.
-   */
+  /** The last {@link Command} in the list. */
   private LinkedListElement m_lastCommand;
-  /**
-   * Whether or not we are currently adding a command.
-   */
+  /** Whether or not we are currently adding a command. */
   private boolean m_adding;
-  /**
-   * Whether or not we are currently disabled.
-   */
+  /** Whether or not we are currently disabled. */
   private boolean m_disabled;
-  /**
-   * A list of all {@link Command Commands} which need to be added.
-   */
-  @SuppressWarnings({"PMD.LooseCoupling", "PMD.UseArrayListInsteadOfVector"})
+  /** A list of all {@link Command Commands} which need to be added. */
+  @SuppressWarnings("PMD.UseArrayListInsteadOfVector")
   private final Vector<Command> m_additions = new Vector<>();
   /**
    * A list of all {@link edu.wpi.first.wpilibj.buttons.Trigger.ButtonScheduler Buttons}. It is
    * created lazily.
    */
-  @SuppressWarnings("PMD.LooseCoupling")
   private Vector<ButtonScheduler> m_buttons;
+
   private boolean m_runningCommandsChanged;
 
-  /**
-   * Instantiates a {@link Scheduler}.
-   */
+  /** Instantiates a {@link Scheduler}. */
   private Scheduler() {
     HAL.report(tResourceType.kResourceType_Command, tInstances.kCommand_Scheduler);
     SendableRegistry.addLW(this, "Scheduler");
-    LiveWindow.setEnabledListener(() -> {
-      disable();
-      removeAll();
-    });
-    LiveWindow.setDisabledListener(() -> {
-      enable();
-    });
+    LiveWindow.setEnabledListener(
+        () -> {
+          disable();
+          removeAll();
+        });
+    LiveWindow.setDisabledListener(
+        () -> {
+          enable();
+        });
   }
 
   @Override
@@ -116,8 +96,8 @@
    * but will instead wait for the proper time in the {@link Scheduler#run()} loop before doing so.
    * The command returns immediately and does nothing if given null.
    *
-   * <p> Adding a {@link Command} to the {@link Scheduler} involves the {@link Scheduler} removing
-   * any {@link Command} which has shared requirements. </p>
+   * <p>Adding a {@link Command} to the {@link Scheduler} involves the {@link Scheduler} removing
+   * any {@link Command} which has shared requirements.
    *
    * @param command the command to add
    */
@@ -144,11 +124,11 @@
   /**
    * Adds a command immediately to the {@link Scheduler}. This should only be called in the {@link
    * Scheduler#run()} loop. Any command with conflicting requirements will be removed, unless it is
-   * uninterruptable. Giving <code>null</code> does nothing.
+   * uninterruptible. Giving <code>null</code> does nothing.
    *
    * @param command the {@link Command} to add
    */
-  @SuppressWarnings({"MethodName", "PMD.CyclomaticComplexity"})
+  @SuppressWarnings("MethodName")
   private void _add(Command command) {
     if (command == null) {
       return;
@@ -163,7 +143,7 @@
     // Only add if not already in
     if (!m_commandTable.containsKey(command)) {
       // Check that the requirements can be had
-      Enumeration requirements = command.getRequirements();
+      Enumeration<?> requirements = command.getRequirements();
       while (requirements.hasMoreElements()) {
         Subsystem lock = (Subsystem) requirements.nextElement();
         if (lock.getCurrentCommand() != null && !lock.getCurrentCommand().isInterruptible()) {
@@ -205,10 +185,14 @@
    * Runs a single iteration of the loop. This method should be called often in order to have a
    * functioning {@link Command} system. The loop has five stages:
    *
-   * <ol> <li>Poll the Buttons</li> <li>Execute/Remove the Commands</li> <li>Send values to
-   * SmartDashboard</li> <li>Add Commands</li> <li>Add Defaults</li> </ol>
+   * <ol>
+   *   <li>Poll the Buttons
+   *   <li>Execute/Remove the Commands
+   *   <li>Send values to SmartDashboard
+   *   <li>Add Commands
+   *   <li>Add Defaults
+   * </ol>
    */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
   public void run() {
     m_runningCommandsChanged = false;
 
@@ -224,7 +208,7 @@
     }
 
     // Call every subsystem's periodic method
-    Enumeration subsystems = m_subsystems.getElements();
+    Enumeration<?> subsystems = m_subsystems.getElements();
     while (subsystems.hasMoreElements()) {
       ((Subsystem) subsystems.nextElement()).periodic();
     }
@@ -247,7 +231,7 @@
     m_additions.removeAllElements();
 
     // Add in the defaults
-    Enumeration locks = m_subsystems.getElements();
+    Enumeration<?> locks = m_subsystems.getElements();
     while (locks.hasMoreElements()) {
       Subsystem lock = (Subsystem) locks.nextElement();
       if (lock.getCurrentCommand() == null) {
@@ -290,7 +274,7 @@
     }
     element.remove();
 
-    Enumeration requirements = command.getRequirements();
+    Enumeration<?> requirements = command.getRequirements();
     while (requirements.hasMoreElements()) {
       ((Subsystem) requirements.nextElement()).setCurrentCommand(null);
     }
@@ -298,9 +282,7 @@
     command.removed();
   }
 
-  /**
-   * Removes all commands.
-   */
+  /** Removes all commands. */
   public void removeAll() {
     // TODO: Confirm that this works with "uninteruptible" commands
     while (m_firstCommand != null) {
@@ -308,59 +290,56 @@
     }
   }
 
-  /**
-   * Disable the command scheduler.
-   */
+  /** Disable the command scheduler. */
   public void disable() {
     m_disabled = true;
   }
 
-  /**
-   * Enable the command scheduler.
-   */
+  /** Enable the command scheduler. */
   public void enable() {
     m_disabled = false;
   }
 
   @Override
-  public void initSendable(SendableBuilder builder) {
+  public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("Scheduler");
     final NetworkTableEntry namesEntry = builder.getEntry("Names");
     final NetworkTableEntry idsEntry = builder.getEntry("Ids");
     final NetworkTableEntry cancelEntry = builder.getEntry("Cancel");
-    builder.setUpdateTable(() -> {
-      if (namesEntry != null && idsEntry != null && cancelEntry != null) {
-        // Get the commands to cancel
-        double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
-        if (toCancel.length > 0) {
-          for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
-            for (double d : toCancel) {
-              if (e.getData().hashCode() == d) {
-                e.getData().cancel();
+    builder.setUpdateTable(
+        () -> {
+          if (namesEntry != null && idsEntry != null && cancelEntry != null) {
+            // Get the commands to cancel
+            double[] toCancel = cancelEntry.getDoubleArray(new double[0]);
+            if (toCancel.length > 0) {
+              for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
+                for (double d : toCancel) {
+                  if (e.getData().hashCode() == d) {
+                    e.getData().cancel();
+                  }
+                }
               }
+              cancelEntry.setDoubleArray(new double[0]);
+            }
+
+            if (m_runningCommandsChanged) {
+              // Set the the running commands
+              int number = 0;
+              for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
+                number++;
+              }
+              String[] commands = new String[number];
+              double[] ids = new double[number];
+              number = 0;
+              for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
+                commands[number] = e.getData().getName();
+                ids[number] = e.getData().hashCode();
+                number++;
+              }
+              namesEntry.setStringArray(commands);
+              idsEntry.setDoubleArray(ids);
             }
           }
-          cancelEntry.setDoubleArray(new double[0]);
-        }
-
-        if (m_runningCommandsChanged) {
-          // Set the the running commands
-          int number = 0;
-          for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
-            number++;
-          }
-          String[] commands = new String[number];
-          double[] ids = new double[number];
-          number = 0;
-          for (LinkedListElement e = m_firstCommand; e != null; e = e.getNext()) {
-            commands[number] = e.getData().getName();
-            ids[number] = e.getData().hashCode();
-            number++;
-          }
-          namesEntry.setStringArray(commands);
-          idsEntry.setDoubleArray(ids);
-        }
-      }
-    });
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
index 6aac6d7..d47f16f 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Set.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -11,14 +8,11 @@
 import java.util.Vector;
 
 @SuppressWarnings("all")
-/**
- * A set.
- */
+/** A set. */
 class Set {
   private Vector m_set = new Vector();
 
-  public Set() {
-  }
+  public Set() {}
 
   public void add(Object o) {
     if (m_set.contains(o)) {
@@ -35,7 +29,7 @@
   }
 
   public void clear() {
-      m_set.clear();
+    m_set.clear();
   }
 
   public boolean contains(Object o) {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
index bd8c658..56b4150 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/StartCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -12,9 +9,7 @@
  * when it is initialized and will finish immediately.
  */
 public class StartCommand extends InstantCommand {
-  /**
-   * The command to fork.
-   */
+  /** The command to fork. */
   private final Command m_commandToFork;
 
   /**
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
index ac1107b..2ba01af 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/Subsystem.java
@@ -1,46 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.Collections;
 
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
 /**
  * This class defines a major component of the robot.
  *
- * <p> A good example of a subsystem is the driveline, or a claw if the robot has one. </p>
+ * <p>A good example of a subsystem is the driveline, or a claw if the robot has one.
  *
- * <p> All motors should be a part of a subsystem. For instance, all the wheel motors should be a
- * part of some kind of "Driveline" subsystem. </p>
+ * <p>All motors should be a part of a subsystem. For instance, all the wheel motors should be a
+ * part of some kind of "Driveline" subsystem.
  *
- * <p> Subsystems are used within the command system as requirements for {@link Command}. Only one
+ * <p>Subsystems are used within the command system as requirements for {@link Command}. Only one
  * command which requires a subsystem can run at a time. Also, subsystems can have default commands
- * which are started if there is no command running which requires this subsystem. </p>
+ * which are started if there is no command running which requires this subsystem.
  *
  * @see Command
  */
 public abstract class Subsystem implements Sendable, AutoCloseable {
-  /**
-   * Whether or not getDefaultCommand() was called.
-   */
+  /** Whether or not getDefaultCommand() was called. */
   private boolean m_initializedDefaultCommand;
-  /**
-   * The current command.
-   */
+  /** The current command. */
   private Command m_currentCommand;
+
   private boolean m_currentCommandChanged;
 
-  /**
-   * The default command.
-   */
+  /** The default command. */
   private Command m_defaultCommand;
 
   /**
@@ -53,9 +44,7 @@
     Scheduler.getInstance().registerSubsystem(this);
   }
 
-  /**
-   * Creates a subsystem. This will set the name to the name of the class.
-   */
+  /** Creates a subsystem. This will set the name to the name of the class. */
   public Subsystem() {
     String name = getClass().getName();
     name = name.substring(name.lastIndexOf('.') + 1);
@@ -76,9 +65,7 @@
    */
   protected abstract void initDefaultCommand();
 
-  /**
-   * When the run method of the scheduler is called this method will be called.
-   */
+  /** When the run method of the scheduler is called this method will be called. */
   public void periodic() {
     // Override me!
   }
@@ -87,8 +74,8 @@
    * Sets the default command. If this is not called or is called with null, then there will be no
    * default command for the subsystem.
    *
-   * <p> <b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
-   * singleton. </p>
+   * <p><b>WARNING:</b> This should <b>NOT</b> be called in a constructor if the subsystem is a
+   * singleton.
    *
    * @param command the default command (or null if there should be none)
    * @throws IllegalUseOfCommandException if the command does not require the subsystem
@@ -176,8 +163,7 @@
   }
 
   /**
-   * Associate a {@link Sendable} with this Subsystem.
-   * Also update the child's name.
+   * Associate a {@link Sendable} with this Subsystem. Also update the child's name.
    *
    * @param name name to give child
    * @param child sendable
@@ -201,7 +187,6 @@
    *
    * @return Name
    */
-  @Override
   public String getName() {
     return SendableRegistry.getName(this);
   }
@@ -211,7 +196,6 @@
    *
    * @param name name
    */
-  @Override
   public void setName(String name) {
     SendableRegistry.setName(this, name);
   }
@@ -221,7 +205,6 @@
    *
    * @return Subsystem name
    */
-  @Override
   public String getSubsystem() {
     return SendableRegistry.getSubsystem(this);
   }
@@ -231,7 +214,6 @@
    *
    * @param subsystem subsystem name
    */
-  @Override
   public void setSubsystem(String subsystem) {
     SendableRegistry.setSubsystem(this, subsystem);
   }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
index 386bb0e..36e651b 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/TimedCommand.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 /**
- * A {@link TimedCommand} will wait for a timeout before finishing.
- * {@link TimedCommand} is used to execute a command for a given amount of time.
+ * A {@link TimedCommand} will wait for a timeout before finishing. {@link TimedCommand} is used to
+ * execute a command for a given amount of time.
  */
 public class TimedCommand extends Command {
   /**
    * Instantiates a TimedCommand with the given name and timeout.
    *
-   * @param name    the name of the command
+   * @param name the name of the command
    * @param timeout the time the command takes to run (seconds)
    */
   public TimedCommand(String name, double timeout) {
@@ -34,8 +31,8 @@
   /**
    * Instantiates a TimedCommand with the given name and timeout.
    *
-   * @param name      the name of the command
-   * @param timeout   the time the command takes to run (seconds)
+   * @param name the name of the command
+   * @param timeout the time the command takes to run (seconds)
    * @param subsystem the subsystem that this command requires
    */
   public TimedCommand(String name, double timeout, Subsystem subsystem) {
@@ -45,16 +42,14 @@
   /**
    * Instantiates a TimedCommand with the given timeout.
    *
-   * @param timeout   the time the command takes to run (seconds)
+   * @param timeout the time the command takes to run (seconds)
    * @param subsystem the subsystem that this command requires
    */
   public TimedCommand(double timeout, Subsystem subsystem) {
     super(timeout, subsystem);
   }
 
-  /**
-  * Ends command when timed out.
-  */
+  /** Ends command when timed out. */
   @Override
   protected boolean isFinished() {
     return isTimedOut();
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
index 0e1762a..a541944 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -26,7 +23,7 @@
   /**
    * Instantiates a {@link WaitCommand} with the given timeout.
    *
-   * @param name    the name of the command
+   * @param name the name of the command
    * @param timeout the time the command takes to run (seconds)
    */
   public WaitCommand(String name, double timeout) {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
index 118da6a..23e21d3 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitForChildren.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -12,8 +9,8 @@
  * If it is not a part of a {@link CommandGroup}, then it will finish immediately. If it is itself
  * an active child, then the {@link CommandGroup} will never end.
  *
- * <p>This class is useful for the situation where you want to allow anything running in parallel
- * to finish, before continuing in the main {@link CommandGroup} sequence.
+ * <p>This class is useful for the situation where you want to allow anything running in parallel to
+ * finish, before continuing in the main {@link CommandGroup} sequence.
  */
 public class WaitForChildren extends Command {
   @Override
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
index dadcab5..b7226bd 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/java/edu/wpi/first/wpilibj/command/WaitUntilCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -21,9 +18,7 @@
     m_time = time;
   }
 
-  /**
-   * Check if we've reached the actual finish time.
-   */
+  /** Check if we've reached the actual finish time. */
   @Override
   public boolean isFinished() {
     return Timer.getMatchTime() >= m_time;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp
new file mode 100644
index 0000000..825d4eb
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDBase.cpp
@@ -0,0 +1,358 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PIDBase.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <hal/FRCUsageReporting.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/PIDOutput.h"
+
+using namespace frc;
+
+template <class T>
+constexpr const T& clamp(const T& value, const T& low, const T& high) {
+  return std::max(low, std::min(value, high));
+}
+
+PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source,
+                 PIDOutput& output)
+    : PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
+
+PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
+                 PIDOutput& output) {
+  m_P = Kp;
+  m_I = Ki;
+  m_D = Kd;
+  m_F = Kf;
+
+  m_pidInput = &source;
+  m_filter = LinearFilter<double>::MovingAverage(1);
+
+  m_pidOutput = &output;
+
+  m_setpointTimer.Start();
+
+  static int instances = 0;
+  instances++;
+  HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
+  wpi::SendableRegistry::Add(this, "PIDController", instances);
+}
+
+double PIDBase::Get() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_result;
+}
+
+void PIDBase::SetContinuous(bool continuous) {
+  std::scoped_lock lock(m_thisMutex);
+  m_continuous = continuous;
+}
+
+void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
+  {
+    std::scoped_lock lock(m_thisMutex);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+    m_inputRange = maximumInput - minimumInput;
+  }
+
+  SetSetpoint(m_setpoint);
+}
+
+void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
+  std::scoped_lock lock(m_thisMutex);
+  m_minimumOutput = minimumOutput;
+  m_maximumOutput = maximumOutput;
+}
+
+void PIDBase::SetPID(double p, double i, double d) {
+  {
+    std::scoped_lock lock(m_thisMutex);
+    m_P = p;
+    m_I = i;
+    m_D = d;
+  }
+}
+
+void PIDBase::SetPID(double p, double i, double d, double f) {
+  std::scoped_lock lock(m_thisMutex);
+  m_P = p;
+  m_I = i;
+  m_D = d;
+  m_F = f;
+}
+
+void PIDBase::SetP(double p) {
+  std::scoped_lock lock(m_thisMutex);
+  m_P = p;
+}
+
+void PIDBase::SetI(double i) {
+  std::scoped_lock lock(m_thisMutex);
+  m_I = i;
+}
+
+void PIDBase::SetD(double d) {
+  std::scoped_lock lock(m_thisMutex);
+  m_D = d;
+}
+
+void PIDBase::SetF(double f) {
+  std::scoped_lock lock(m_thisMutex);
+  m_F = f;
+}
+
+double PIDBase::GetP() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_P;
+}
+
+double PIDBase::GetI() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_I;
+}
+
+double PIDBase::GetD() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_D;
+}
+
+double PIDBase::GetF() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_F;
+}
+
+void PIDBase::SetSetpoint(double setpoint) {
+  {
+    std::scoped_lock lock(m_thisMutex);
+
+    if (m_maximumInput > m_minimumInput) {
+      if (setpoint > m_maximumInput) {
+        m_setpoint = m_maximumInput;
+      } else if (setpoint < m_minimumInput) {
+        m_setpoint = m_minimumInput;
+      } else {
+        m_setpoint = setpoint;
+      }
+    } else {
+      m_setpoint = setpoint;
+    }
+  }
+}
+
+double PIDBase::GetSetpoint() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_setpoint;
+}
+
+double PIDBase::GetDeltaSetpoint() const {
+  std::scoped_lock lock(m_thisMutex);
+  return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get().value();
+}
+
+double PIDBase::GetError() const {
+  double setpoint = GetSetpoint();
+  {
+    std::scoped_lock lock(m_thisMutex);
+    return GetContinuousError(setpoint - m_pidInput->PIDGet());
+  }
+}
+
+double PIDBase::GetAvgError() const {
+  return GetError();
+}
+
+void PIDBase::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidInput->SetPIDSourceType(pidSource);
+}
+
+PIDSourceType PIDBase::GetPIDSourceType() const {
+  return m_pidInput->GetPIDSourceType();
+}
+
+void PIDBase::SetTolerance(double percent) {
+  std::scoped_lock lock(m_thisMutex);
+  m_toleranceType = kPercentTolerance;
+  m_tolerance = percent;
+}
+
+void PIDBase::SetAbsoluteTolerance(double absTolerance) {
+  std::scoped_lock lock(m_thisMutex);
+  m_toleranceType = kAbsoluteTolerance;
+  m_tolerance = absTolerance;
+}
+
+void PIDBase::SetPercentTolerance(double percent) {
+  std::scoped_lock lock(m_thisMutex);
+  m_toleranceType = kPercentTolerance;
+  m_tolerance = percent;
+}
+
+void PIDBase::SetToleranceBuffer(int bufLength) {
+  std::scoped_lock lock(m_thisMutex);
+  m_filter = LinearFilter<double>::MovingAverage(bufLength);
+}
+
+bool PIDBase::OnTarget() const {
+  double error = GetError();
+
+  std::scoped_lock lock(m_thisMutex);
+  switch (m_toleranceType) {
+    case kPercentTolerance:
+      return std::fabs(error) < m_tolerance / 100 * m_inputRange;
+      break;
+    case kAbsoluteTolerance:
+      return std::fabs(error) < m_tolerance;
+      break;
+    case kNoTolerance:
+      // TODO: this case needs an error
+      return false;
+  }
+  return false;
+}
+
+void PIDBase::Reset() {
+  std::scoped_lock lock(m_thisMutex);
+  m_prevError = 0;
+  m_totalError = 0;
+  m_result = 0;
+}
+
+void PIDBase::PIDWrite(double output) {
+  SetSetpoint(output);
+}
+
+void PIDBase::InitSendable(wpi::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); });
+}
+
+void PIDBase::Calculate() {
+  if (m_pidInput == nullptr || m_pidOutput == nullptr) {
+    return;
+  }
+
+  bool enabled;
+  {
+    std::scoped_lock lock(m_thisMutex);
+    enabled = m_enabled;
+  }
+
+  if (enabled) {
+    double input;
+
+    // Storage for function inputs
+    PIDSourceType pidSourceType;
+    double P;
+    double I;
+    double D;
+    double feedForward = CalculateFeedForward();
+    double minimumOutput;
+    double maximumOutput;
+
+    // Storage for function input-outputs
+    double prevError;
+    double error;
+    double totalError;
+
+    {
+      std::scoped_lock lock(m_thisMutex);
+
+      input = m_filter.Calculate(m_pidInput->PIDGet());
+
+      pidSourceType = m_pidInput->GetPIDSourceType();
+      P = m_P;
+      I = m_I;
+      D = m_D;
+      minimumOutput = m_minimumOutput;
+      maximumOutput = m_maximumOutput;
+
+      prevError = m_prevError;
+      error = GetContinuousError(m_setpoint - input);
+      totalError = m_totalError;
+    }
+
+    // Storage for function outputs
+    double result;
+
+    if (pidSourceType == PIDSourceType::kRate) {
+      if (P != 0) {
+        totalError =
+            clamp(totalError + error, minimumOutput / P, maximumOutput / P);
+      }
+
+      result = D * error + P * totalError + feedForward;
+    } else {
+      if (I != 0) {
+        totalError =
+            clamp(totalError + error, minimumOutput / I, maximumOutput / I);
+      }
+
+      result =
+          P * error + I * totalError + D * (error - prevError) + feedForward;
+    }
+
+    result = clamp(result, minimumOutput, maximumOutput);
+
+    {
+      // Ensures m_enabled check and PIDWrite() call occur atomically
+      std::scoped_lock pidWriteLock(m_pidWriteMutex);
+      std::unique_lock mainLock(m_thisMutex);
+      if (m_enabled) {
+        // Don't block other PIDBase operations on PIDWrite()
+        mainLock.unlock();
+
+        m_pidOutput->PIDWrite(result);
+      }
+    }
+
+    std::scoped_lock lock(m_thisMutex);
+    m_prevError = m_error;
+    m_error = error;
+    m_totalError = totalError;
+    m_result = result;
+  }
+}
+
+double PIDBase::CalculateFeedForward() {
+  if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
+    return m_F * GetSetpoint();
+  } else {
+    double temp = m_F * GetDeltaSetpoint();
+    m_prevSetpoint = m_setpoint;
+    m_setpointTimer.Reset();
+    return temp;
+  }
+}
+
+double PIDBase::GetContinuousError(double error) const {
+  if (m_continuous && m_inputRange != 0) {
+    error = std::fmod(error, m_inputRange);
+    if (std::fabs(error) > m_inputRange / 2) {
+      if (error > 0) {
+        return error - m_inputRange;
+      } else {
+        return error + m_inputRange;
+      }
+    }
+  }
+
+  return error;
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDController.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDController.cpp
new file mode 100644
index 0000000..ee8382c
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDController.cpp
@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PIDController.h"
+
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/Notifier.h"
+#include "frc/PIDOutput.h"
+
+using namespace frc;
+
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
+                             PIDOutput* output, double period)
+    : PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+                             PIDSource* source, PIDOutput* output,
+                             double period)
+    : PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source,
+                             PIDOutput& output, double period)
+    : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
+
+PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
+                             PIDSource& source, PIDOutput& output,
+                             double period)
+    : PIDBase(Kp, Ki, Kd, Kf, source, output) {
+  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
+  m_controlLoop->StartPeriodic(units::second_t(period));
+}
+
+PIDController::~PIDController() {
+  // Forcefully stopping the notifier so the callback can successfully run.
+  m_controlLoop->Stop();
+}
+
+void PIDController::Enable() {
+  {
+    std::scoped_lock lock(m_thisMutex);
+    m_enabled = true;
+  }
+}
+
+void PIDController::Disable() {
+  {
+    // Ensures m_enabled modification and PIDWrite() call occur atomically
+    std::scoped_lock pidWriteLock(m_pidWriteMutex);
+    {
+      std::scoped_lock mainLock(m_thisMutex);
+      m_enabled = false;
+    }
+
+    m_pidOutput->PIDWrite(0);
+  }
+}
+
+void PIDController::SetEnabled(bool enable) {
+  if (enable) {
+    Enable();
+  } else {
+    Disable();
+  }
+}
+
+bool PIDController::IsEnabled() const {
+  std::scoped_lock lock(m_thisMutex);
+  return m_enabled;
+}
+
+void PIDController::Reset() {
+  Disable();
+
+  PIDBase::Reset();
+}
+
+void PIDController::InitSendable(wpi::SendableBuilder& builder) {
+  PIDBase::InitSendable(builder);
+  builder.AddBooleanProperty(
+      "enabled", [=] { return IsEnabled(); },
+      [=](bool value) { SetEnabled(value); });
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDSource.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDSource.cpp
new file mode 100644
index 0000000..3a9e3ea
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/PIDSource.cpp
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PIDSource.h"
+
+using namespace frc;
+
+void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
+  m_pidSource = pidSource;
+}
+
+PIDSourceType PIDSource::GetPIDSourceType() const {
+  return m_pidSource;
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
index 57c86bd..ef2c03d 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Button.cpp
@@ -1,20 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/Button.h"
 
 using namespace frc;
 
-void Button::WhenPressed(Command* command) { WhenActive(command); }
+void Button::WhenPressed(Command* command) {
+  WhenActive(command);
+}
 
-void Button::WhileHeld(Command* command) { WhileActive(command); }
+void Button::WhileHeld(Command* command) {
+  WhileActive(command);
+}
 
-void Button::WhenReleased(Command* command) { WhenInactive(command); }
+void Button::WhenReleased(Command* command) {
+  WhenInactive(command);
+}
 
-void Button::CancelWhenPressed(Command* command) { CancelWhenActive(command); }
+void Button::CancelWhenPressed(Command* command) {
+  CancelWhenActive(command);
+}
 
-void Button::ToggleWhenPressed(Command* command) { ToggleWhenActive(command); }
+void Button::ToggleWhenPressed(Command* command) {
+  ToggleWhenActive(command);
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
index f79c487..b210c8d 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ButtonScheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/ButtonScheduler.h"
 
@@ -14,4 +11,6 @@
 ButtonScheduler::ButtonScheduler(bool last, Trigger* button, Command* orders)
     : m_pressedLast(last), m_button(button), m_command(orders) {}
 
-void ButtonScheduler::Start() { Scheduler::GetInstance()->AddButton(this); }
+void ButtonScheduler::Start() {
+  Scheduler::GetInstance()->AddButton(this);
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
index b0f4433..faff376 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/CancelButtonScheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/CancelButtonScheduler.h"
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
index 9abf8bc..8fb1064 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/HeldButtonScheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/HeldButtonScheduler.h"
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
index 6ff7971..5adf83c 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/InternalButton.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/InternalButton.h"
 
@@ -12,8 +9,14 @@
 InternalButton::InternalButton(bool inverted)
     : m_pressed(inverted), m_inverted(inverted) {}
 
-void InternalButton::SetInverted(bool inverted) { m_inverted = inverted; }
+void InternalButton::SetInverted(bool inverted) {
+  m_inverted = inverted;
+}
 
-void InternalButton::SetPressed(bool pressed) { m_pressed = pressed; }
+void InternalButton::SetPressed(bool pressed) {
+  m_pressed = pressed;
+}
 
-bool InternalButton::Get() { return m_pressed ^ m_inverted; }
+bool InternalButton::Get() {
+  return m_pressed ^ m_inverted;
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
index c1d5a29..459c076 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/JoystickButton.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/JoystickButton.h"
 
@@ -12,4 +9,6 @@
 JoystickButton::JoystickButton(GenericHID* joystick, int buttonNumber)
     : m_joystick(joystick), m_buttonNumber(buttonNumber) {}
 
-bool JoystickButton::Get() { return m_joystick->GetRawButton(m_buttonNumber); }
+bool JoystickButton::Get() {
+  return m_joystick->GetRawButton(m_buttonNumber);
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
index 8bca356..f5694a1 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/NetworkButton.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/NetworkButton.h"
 
@@ -12,13 +9,12 @@
 
 using namespace frc;
 
-NetworkButton::NetworkButton(const wpi::Twine& tableName,
-                             const wpi::Twine& field)
+NetworkButton::NetworkButton(std::string_view tableName, std::string_view field)
     : NetworkButton(nt::NetworkTableInstance::GetDefault().GetTable(tableName),
                     field) {}
 
 NetworkButton::NetworkButton(std::shared_ptr<nt::NetworkTable> table,
-                             const wpi::Twine& field)
+                             std::string_view field)
     : m_entry(table->GetEntry(field)) {}
 
 bool NetworkButton::Get() {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
index 73e847a..67b8cae 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/POVButton.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/POVButton.h"
 
@@ -12,4 +9,6 @@
 POVButton::POVButton(GenericHID& joystick, int angle, int povNumber)
     : m_joystick(&joystick), m_angle(angle), m_povNumber(povNumber) {}
 
-bool POVButton::Get() { return m_joystick->GetPOV(m_povNumber) == m_angle; }
+bool POVButton::Get() {
+  return m_joystick->GetPOV(m_povNumber) == m_angle;
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
index 4a470f5..73705bc 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/PressedButtonScheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/PressedButtonScheduler.h"
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
index 671c13e..647169b 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ReleasedButtonScheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/ReleasedButtonScheduler.h"
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
index cefccb5..d17b2e8 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/ToggleButtonScheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/buttons/ToggleButtonScheduler.h"
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
index 0cce9d0..9040df6 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/buttons/Trigger.cpp
@@ -1,9 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/sendable/SendableBuilder.h>
 
 #include "frc/buttons/Button.h"
 #include "frc/buttons/CancelButtonScheduler.h"
@@ -11,7 +10,6 @@
 #include "frc/buttons/PressedButtonScheduler.h"
 #include "frc/buttons/ReleasedButtonScheduler.h"
 #include "frc/buttons/ToggleButtonScheduler.h"
-#include "frc/smartdashboard/SendableBuilder.h"
 
 using namespace frc;
 
@@ -36,7 +34,9 @@
   return *this;
 }
 
-bool Trigger::Grab() { return Get() || m_sendablePressed; }
+bool Trigger::Grab() {
+  return Get() || m_sendablePressed;
+}
 
 void Trigger::WhenActive(Command* command) {
   auto pbs = new PressedButtonScheduler(Grab(), this, command);
@@ -63,10 +63,10 @@
   tbs->Start();
 }
 
-void Trigger::InitSendable(SendableBuilder& builder) {
+void Trigger::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Button");
-  builder.SetSafeState([=]() { m_sendablePressed = false; });
+  builder.SetSafeState([=] { m_sendablePressed = false; });
   builder.AddBooleanProperty(
-      "pressed", [=]() { return Grab(); },
+      "pressed", [=] { return Grab(); },
       [=](bool value) { m_sendablePressed = value; });
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
index 8c2abcf..778f956 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Command.cpp
@@ -1,100 +1,109 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/Command.h"
 
 #include <typeinfo>
 
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
 #include "frc/RobotState.h"
 #include "frc/Timer.h"
-#include "frc/WPIErrors.h"
 #include "frc/commands/CommandGroup.h"
 #include "frc/commands/Scheduler.h"
 #include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 int Command::m_commandCounter = 0;
 
-Command::Command() : Command("", -1.0) {}
+Command::Command() : Command("", -1_s) {}
 
-Command::Command(const wpi::Twine& name) : Command(name, -1.0) {}
+Command::Command(std::string_view name) : Command(name, -1_s) {}
 
-Command::Command(double timeout) : Command("", timeout) {}
+Command::Command(units::second_t timeout) : Command("", timeout) {}
 
-Command::Command(Subsystem& subsystem) : Command("", -1.0) {
+Command::Command(Subsystem& subsystem) : Command("", -1_s) {
   Requires(&subsystem);
 }
 
-Command::Command(const wpi::Twine& name, double timeout) {
+Command::Command(std::string_view name, units::second_t timeout) {
   // We use -1.0 to indicate no timeout.
-  if (timeout < 0.0 && timeout != -1.0)
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
+  if (timeout < 0_s && timeout != -1_s) {
+    throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
+                        timeout.value());
+  }
 
   m_timeout = timeout;
 
   // If name contains an empty string
-  if (name.isTriviallyEmpty() ||
-      (name.isSingleStringRef() && name.getSingleStringRef().empty())) {
-    SendableRegistry::GetInstance().Add(
-        this, "Command_" + wpi::Twine(typeid(*this).name()));
+  if (name.empty()) {
+    wpi::SendableRegistry::Add(this,
+                               fmt::format("Command_{}", typeid(*this).name()));
   } else {
-    SendableRegistry::GetInstance().Add(this, name);
+    wpi::SendableRegistry::Add(this, name);
   }
 }
 
-Command::Command(const wpi::Twine& name, Subsystem& subsystem)
-    : Command(name, -1.0) {
+Command::Command(std::string_view name, Subsystem& subsystem)
+    : Command(name, -1_s) {
   Requires(&subsystem);
 }
 
-Command::Command(double timeout, Subsystem& subsystem) : Command("", timeout) {
+Command::Command(units::second_t timeout, Subsystem& subsystem)
+    : Command("", timeout) {
   Requires(&subsystem);
 }
 
-Command::Command(const wpi::Twine& name, double timeout, Subsystem& subsystem)
+Command::Command(std::string_view name, units::second_t timeout,
+                 Subsystem& subsystem)
     : Command(name, timeout) {
   Requires(&subsystem);
 }
 
-double Command::TimeSinceInitialized() const {
-  if (m_startTime < 0.0)
-    return 0.0;
-  else
+units::second_t Command::TimeSinceInitialized() const {
+  if (m_startTime < 0_s) {
+    return 0_s;
+  } else {
     return Timer::GetFPGATimestamp() - m_startTime;
+  }
 }
 
 void Command::Requires(Subsystem* subsystem) {
-  if (!AssertUnlocked("Can not add new requirement to command")) return;
+  if (!AssertUnlocked("Can not add new requirement to command")) {
+    return;
+  }
 
-  if (subsystem != nullptr)
+  if (subsystem != nullptr) {
     m_requirements.insert(subsystem);
-  else
-    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
+  } else {
+    throw FRC_MakeError(err::NullParameter, "{}", "subsystem");
+  }
 }
 
 void Command::Start() {
   LockChanges();
-  if (m_parent != nullptr)
-    wpi_setWPIErrorWithContext(
-        CommandIllegalUse,
+  if (m_parent != nullptr) {
+    throw FRC_MakeError(
+        err::CommandIllegalUse, "{}",
         "Can not start a command that is part of a command group");
+  }
 
   m_completed = false;
   Scheduler::GetInstance()->AddCommand(this);
 }
 
 bool Command::Run() {
-  if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled())
+  if (!m_runWhenDisabled && m_parent == nullptr && RobotState::IsDisabled()) {
     Cancel();
+  }
 
-  if (IsCanceled()) return false;
+  if (IsCanceled()) {
+    return false;
+  }
 
   if (!m_initialized) {
     m_initialized = true;
@@ -108,23 +117,34 @@
 }
 
 void Command::Cancel() {
-  if (m_parent != nullptr)
-    wpi_setWPIErrorWithContext(
-        CommandIllegalUse,
+  if (m_parent != nullptr) {
+    throw FRC_MakeError(
+        err::CommandIllegalUse, "{}",
         "Can not cancel a command that is part of a command group");
+  }
 
   _Cancel();
 }
 
-bool Command::IsRunning() const { return m_running; }
+bool Command::IsRunning() const {
+  return m_running;
+}
 
-bool Command::IsInitialized() const { return m_initialized; }
+bool Command::IsInitialized() const {
+  return m_initialized;
+}
 
-bool Command::IsCompleted() const { return m_completed; }
+bool Command::IsCompleted() const {
+  return m_completed;
+}
 
-bool Command::IsCanceled() const { return m_canceled; }
+bool Command::IsCanceled() const {
+  return m_canceled;
+}
 
-bool Command::IsInterruptible() const { return m_interruptible; }
+bool Command::IsInterruptible() const {
+  return m_interruptible;
+}
 
 void Command::SetInterruptible(bool interruptible) {
   m_interruptible = interruptible;
@@ -138,51 +158,64 @@
   return m_requirements;
 }
 
-CommandGroup* Command::GetGroup() const { return m_parent; }
+CommandGroup* Command::GetGroup() const {
+  return m_parent;
+}
 
-void Command::SetRunWhenDisabled(bool run) { m_runWhenDisabled = run; }
+void Command::SetRunWhenDisabled(bool run) {
+  m_runWhenDisabled = run;
+}
 
-bool Command::WillRunWhenDisabled() const { return m_runWhenDisabled; }
+bool Command::WillRunWhenDisabled() const {
+  return m_runWhenDisabled;
+}
 
-int Command::GetID() const { return m_commandID; }
+int Command::GetID() const {
+  return m_commandID;
+}
 
-void Command::SetTimeout(double timeout) {
-  if (timeout < 0.0)
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-  else
+void Command::SetTimeout(units::second_t timeout) {
+  if (timeout < 0_s) {
+    throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
+                        timeout.value());
+  } else {
     m_timeout = timeout;
+  }
 }
 
 bool Command::IsTimedOut() const {
-  return m_timeout != -1 && TimeSinceInitialized() >= m_timeout;
+  return m_timeout != -1_s && TimeSinceInitialized() >= m_timeout;
 }
 
-bool Command::AssertUnlocked(const std::string& message) {
+bool Command::AssertUnlocked(std::string_view message) {
   if (m_locked) {
-    std::string buf =
-        message + " after being started or being added to a command group";
-    wpi_setWPIErrorWithContext(CommandIllegalUse, buf);
-    return false;
+    throw FRC_MakeError(
+        err::CommandIllegalUse,
+        "{} after being started or being added to a command group", message);
   }
   return true;
 }
 
 void Command::SetParent(CommandGroup* parent) {
   if (parent == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "parent");
+    throw FRC_MakeError(err::NullParameter, "{}", "parent");
   } else if (m_parent != nullptr) {
-    wpi_setWPIErrorWithContext(CommandIllegalUse,
-                               "Can not give command to a command group after "
-                               "already being put in a command group");
+    throw FRC_MakeError(err::CommandIllegalUse, "{}",
+                        "Can not give command to a command group after "
+                        "already being put in a command group");
   } else {
     LockChanges();
     m_parent = parent;
   }
 }
 
-bool Command::IsParented() const { return m_parent != nullptr; }
+bool Command::IsParented() const {
+  return m_parent != nullptr;
+}
 
-void Command::ClearRequirements() { m_requirements.clear(); }
+void Command::ClearRequirements() {
+  m_requirements.clear();
+}
 
 void Command::Initialize() {}
 
@@ -190,21 +223,33 @@
 
 void Command::End() {}
 
-void Command::Interrupted() { End(); }
+void Command::Interrupted() {
+  End();
+}
 
-void Command::_Initialize() { m_completed = false; }
+void Command::_Initialize() {
+  m_completed = false;
+}
 
-void Command::_Interrupted() { m_completed = true; }
+void Command::_Interrupted() {
+  m_completed = true;
+}
 
 void Command::_Execute() {}
 
-void Command::_End() { m_completed = true; }
-
-void Command::_Cancel() {
-  if (IsRunning()) m_canceled = true;
+void Command::_End() {
+  m_completed = true;
 }
 
-void Command::LockChanges() { m_locked = true; }
+void Command::_Cancel() {
+  if (IsRunning()) {
+    m_canceled = true;
+  }
+}
+
+void Command::LockChanges() {
+  m_locked = true;
+}
 
 void Command::Removed() {
   if (m_initialized) {
@@ -224,42 +269,47 @@
 
 void Command::StartRunning() {
   m_running = true;
-  m_startTime = -1;
+  m_startTime = -1_s;
   m_completed = false;
 }
 
-void Command::StartTiming() { m_startTime = Timer::GetFPGATimestamp(); }
-
-std::string Command::GetName() const {
-  return SendableRegistry::GetInstance().GetName(this);
+void Command::StartTiming() {
+  m_startTime = Timer::GetFPGATimestamp();
 }
 
-void Command::SetName(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetName(this, name);
+std::string Command::GetName() const {
+  return wpi::SendableRegistry::GetName(this);
+}
+
+void Command::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
 }
 
 std::string Command::GetSubsystem() const {
-  return SendableRegistry::GetInstance().GetSubsystem(this);
+  return wpi::SendableRegistry::GetSubsystem(this);
 }
 
-void Command::SetSubsystem(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetSubsystem(this, name);
+void Command::SetSubsystem(std::string_view name) {
+  wpi::SendableRegistry::SetSubsystem(this, name);
 }
 
-void Command::InitSendable(SendableBuilder& builder) {
+void Command::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Command");
   builder.AddStringProperty(
-      ".name", [=]() { return SendableRegistry::GetInstance().GetName(this); },
-      nullptr);
+      ".name", [=] { return wpi::SendableRegistry::GetName(this); }, nullptr);
   builder.AddBooleanProperty(
-      "running", [=]() { return IsRunning(); },
+      "running", [=] { return IsRunning(); },
       [=](bool value) {
         if (value) {
-          if (!IsRunning()) Start();
+          if (!IsRunning()) {
+            Start();
+          }
         } else {
-          if (IsRunning()) Cancel();
+          if (IsRunning()) {
+            Cancel();
+          }
         }
       });
   builder.AddBooleanProperty(
-      ".isParented", [=]() { return IsParented(); }, nullptr);
+      ".isParented", [=] { return IsParented(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
index 31354e3..977795d 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroup.cpp
@@ -1,24 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/CommandGroup.h"
 
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
-CommandGroup::CommandGroup(const wpi::Twine& name) : Command(name) {}
+CommandGroup::CommandGroup(std::string_view name) : Command(name) {}
 
 void CommandGroup::AddSequential(Command* command) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
+  if (!command) {
+    throw FRC_MakeError(err::NullParameter, "{}", "command");
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) {
     return;
   }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
 
   m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence);
 
@@ -26,18 +24,21 @@
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) {
+    Requires(requirement);
+  }
 }
 
-void CommandGroup::AddSequential(Command* command, double timeout) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
+void CommandGroup::AddSequential(Command* command, units::second_t timeout) {
+  if (!command) {
+    throw FRC_MakeError(err::NullParameter, "{}", "command");
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) {
     return;
   }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
-  if (timeout < 0.0) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-    return;
+  if (timeout < 0_s) {
+    throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
+                        timeout.value());
   }
 
   m_commands.emplace_back(command, CommandGroupEntry::kSequence_InSequence,
@@ -47,15 +48,19 @@
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) {
+    Requires(requirement);
+  }
 }
 
 void CommandGroup::AddParallel(Command* command) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
+  if (!command) {
+    throw FRC_MakeError(err::NullParameter, "{}", "command");
     return;
   }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
+  if (!AssertUnlocked("Cannot add new command to command group")) {
+    return;
+  }
 
   m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild);
 
@@ -63,18 +68,21 @@
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) {
+    Requires(requirement);
+  }
 }
 
-void CommandGroup::AddParallel(Command* command, double timeout) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
+void CommandGroup::AddParallel(Command* command, units::second_t timeout) {
+  if (!command) {
+    throw FRC_MakeError(err::NullParameter, "{}", "command");
+  }
+  if (!AssertUnlocked("Cannot add new command to command group")) {
     return;
   }
-  if (!AssertUnlocked("Cannot add new command to command group")) return;
-  if (timeout < 0.0) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "timeout < 0.0");
-    return;
+  if (timeout < 0_s) {
+    throw FRC_MakeError(err::ParameterOutOfRange, "timeout {} < 0 s",
+                        timeout.value());
   }
 
   m_commands.emplace_back(command, CommandGroupEntry::kSequence_BranchChild,
@@ -84,26 +92,36 @@
 
   // Iterate through command->GetRequirements() and call Requires() on each
   // required subsystem
-  for (auto&& requirement : command->GetRequirements()) Requires(requirement);
+  for (auto&& requirement : command->GetRequirements()) {
+    Requires(requirement);
+  }
 }
 
 bool CommandGroup::IsInterruptible() const {
-  if (!Command::IsInterruptible()) return false;
+  if (!Command::IsInterruptible()) {
+    return false;
+  }
 
   if (m_currentCommandIndex != -1 &&
       static_cast<size_t>(m_currentCommandIndex) < m_commands.size()) {
     Command* cmd = m_commands[m_currentCommandIndex].m_command;
-    if (!cmd->IsInterruptible()) return false;
+    if (!cmd->IsInterruptible()) {
+      return false;
+    }
   }
 
   for (const auto& child : m_children) {
-    if (!child->m_command->IsInterruptible()) return false;
+    if (!child->m_command->IsInterruptible()) {
+      return false;
+    }
   }
 
   return true;
 }
 
-int CommandGroup::GetSize() const { return m_children.size(); }
+int CommandGroup::GetSize() const {
+  return m_children.size();
+}
 
 void CommandGroup::Initialize() {}
 
@@ -118,7 +136,9 @@
 
 void CommandGroup::Interrupted() {}
 
-void CommandGroup::_Initialize() { m_currentCommandIndex = -1; }
+void CommandGroup::_Initialize() {
+  m_currentCommandIndex = -1;
+}
 
 void CommandGroup::_Execute() {
   CommandGroupEntry* entry;
@@ -135,7 +155,9 @@
     // If a command is prepared to run
     if (cmd != nullptr) {
       // If command timed out, cancel it so it's removed from the Scheduler
-      if (entry->IsTimedOut()) cmd->_Cancel();
+      if (entry->IsTimedOut()) {
+        cmd->_Cancel();
+      }
 
       // If command finished or was canceled, remove it from Scheduler
       if (cmd->Run()) {
@@ -222,7 +244,9 @@
   m_children.clear();
 }
 
-void CommandGroup::_Interrupted() { _End(); }
+void CommandGroup::_Interrupted() {
+  _End();
+}
 
 void CommandGroup::CancelConflicts(Command* command) {
   for (auto childIter = m_children.begin(); childIter != m_children.end();) {
@@ -238,6 +262,8 @@
         break;
       }
     }
-    if (!erased) childIter++;
+    if (!erased) {
+      childIter++;
+    }
   }
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
index 3441743..1e6f8c7 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/CommandGroupEntry.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/CommandGroupEntry.h"
 
@@ -12,12 +9,16 @@
 using namespace frc;
 
 CommandGroupEntry::CommandGroupEntry(Command* command, Sequence state,
-                                     double timeout)
+                                     units::second_t timeout)
     : m_timeout(timeout), m_command(command), m_state(state) {}
 
 bool CommandGroupEntry::IsTimedOut() const {
-  if (m_timeout < 0.0) return false;
-  double time = m_command->TimeSinceInitialized();
-  if (time == 0.0) return false;
+  if (m_timeout < 0_s) {
+    return false;
+  }
+  auto time = m_command->TimeSinceInitialized();
+  if (time == 0_s) {
+    return false;
+  }
   return time >= m_timeout;
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
index 3d89d92..e2867b5 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/ConditionalCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/ConditionalCommand.h"
 
@@ -13,12 +10,14 @@
 
 static void RequireAll(Command& command, Command* onTrue, Command* onFalse) {
   if (onTrue != nullptr) {
-    for (auto requirement : onTrue->GetRequirements())
+    for (auto requirement : onTrue->GetRequirements()) {
       command.Requires(requirement);
+    }
   }
   if (onFalse != nullptr) {
-    for (auto requirement : onFalse->GetRequirements())
+    for (auto requirement : onFalse->GetRequirements()) {
       command.Requires(requirement);
+    }
   }
 }
 
@@ -29,7 +28,7 @@
   RequireAll(*this, onTrue, onFalse);
 }
 
-ConditionalCommand::ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+ConditionalCommand::ConditionalCommand(std::string_view name, Command* onTrue,
                                        Command* onFalse)
     : Command(name) {
   m_onTrue = onTrue;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
index 445c5e0..d8f3309 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/InstantCommand.cpp
@@ -1,35 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/InstantCommand.h"
 
+#include <utility>
+
 using namespace frc;
 
-InstantCommand::InstantCommand(const wpi::Twine& name) : Command(name) {}
+InstantCommand::InstantCommand(std::string_view name) : Command(name) {}
 
 InstantCommand::InstantCommand(Subsystem& subsystem) : Command(subsystem) {}
 
-InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem)
+InstantCommand::InstantCommand(std::string_view name, Subsystem& subsystem)
     : Command(name, subsystem) {}
 
-InstantCommand::InstantCommand(std::function<void()> func) : m_func(func) {}
+InstantCommand::InstantCommand(std::function<void()> func)
+    : m_func(std::move(func)) {}
 
 InstantCommand::InstantCommand(Subsystem& subsystem, std::function<void()> func)
     : InstantCommand(subsystem) {
   m_func = func;
 }
 
-InstantCommand::InstantCommand(const wpi::Twine& name,
+InstantCommand::InstantCommand(std::string_view name,
                                std::function<void()> func)
     : InstantCommand(name) {
   m_func = func;
 }
 
-InstantCommand::InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+InstantCommand::InstantCommand(std::string_view name, Subsystem& subsystem,
                                std::function<void()> func)
     : InstantCommand(name, subsystem) {
   m_func = func;
@@ -42,4 +42,6 @@
   }
 }
 
-bool InstantCommand::IsFinished() { return true; }
+bool InstantCommand::IsFinished() {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
index 90a05f4..e71556a 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDCommand.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/PIDCommand.h"
 
-#include "frc/smartdashboard/SendableBuilder.h"
+#include <wpi/sendable/SendableBuilder.h>
 
 using namespace frc;
 
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
                        double f, double period)
     : Command(name) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
@@ -22,12 +19,12 @@
       std::make_shared<PIDController>(p, i, d, f, this, this, period);
 }
 
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d)
+PIDCommand::PIDCommand(std::string_view name, double p, double i, double d)
     : Command(name) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this);
 }
 
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
                        double period)
     : Command(name) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
@@ -41,7 +38,7 @@
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
 }
 
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
                        double f, double period, Subsystem& subsystem)
     : Command(name, subsystem) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
@@ -54,13 +51,13 @@
       std::make_shared<PIDController>(p, i, d, f, this, this, period);
 }
 
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
                        Subsystem& subsystem)
     : Command(name, subsystem) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this);
 }
 
-PIDCommand::PIDCommand(const wpi::Twine& name, double p, double i, double d,
+PIDCommand::PIDCommand(std::string_view name, double p, double i, double d,
                        double period, Subsystem& subsystem)
     : Command(name, subsystem) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
@@ -75,19 +72,29 @@
   m_controller = std::make_shared<PIDController>(p, i, d, this, this, period);
 }
 
-void PIDCommand::_Initialize() { m_controller->Enable(); }
+void PIDCommand::_Initialize() {
+  m_controller->Enable();
+}
 
-void PIDCommand::_End() { m_controller->Disable(); }
+void PIDCommand::_End() {
+  m_controller->Disable();
+}
 
-void PIDCommand::_Interrupted() { _End(); }
+void PIDCommand::_Interrupted() {
+  _End();
+}
 
 void PIDCommand::SetSetpointRelative(double deltaSetpoint) {
   SetSetpoint(GetSetpoint() + deltaSetpoint);
 }
 
-void PIDCommand::PIDWrite(double output) { UsePIDOutput(output); }
+void PIDCommand::PIDWrite(double output) {
+  UsePIDOutput(output);
+}
 
-double PIDCommand::PIDGet() { return ReturnPIDInput(); }
+double PIDCommand::PIDGet() {
+  return ReturnPIDInput();
+}
 
 std::shared_ptr<PIDController> PIDCommand::GetPIDController() const {
   return m_controller;
@@ -97,11 +104,15 @@
   m_controller->SetSetpoint(setpoint);
 }
 
-double PIDCommand::GetSetpoint() const { return m_controller->GetSetpoint(); }
+double PIDCommand::GetSetpoint() const {
+  return m_controller->GetSetpoint();
+}
 
-double PIDCommand::GetPosition() { return ReturnPIDInput(); }
+double PIDCommand::GetPosition() {
+  return ReturnPIDInput();
+}
 
-void PIDCommand::InitSendable(SendableBuilder& builder) {
+void PIDCommand::InitSendable(wpi::SendableBuilder& builder) {
   m_controller->InitSendable(builder);
   Command::InitSendable(builder);
   builder.SetSmartDashboardType("PIDCommand");
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
index 4d35943..f76288b 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PIDSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/PIDSubsystem.h"
 
@@ -11,20 +8,20 @@
 
 using namespace frc;
 
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d)
+PIDSubsystem::PIDSubsystem(std::string_view name, double p, double i, double d)
     : Subsystem(name) {
   m_controller = std::make_shared<PIDController>(p, i, d, this, this);
   AddChild("PIDController", m_controller);
 }
 
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+PIDSubsystem::PIDSubsystem(std::string_view name, double p, double i, double d,
                            double f)
     : Subsystem(name) {
   m_controller = std::make_shared<PIDController>(p, i, d, f, this, this);
   AddChild("PIDController", m_controller);
 }
 
-PIDSubsystem::PIDSubsystem(const wpi::Twine& name, double p, double i, double d,
+PIDSubsystem::PIDSubsystem(std::string_view name, double p, double i, double d,
                            double f, double period)
     : Subsystem(name) {
   m_controller =
@@ -52,13 +49,21 @@
   AddChild("PIDController", m_controller);
 }
 
-void PIDSubsystem::Enable() { m_controller->Enable(); }
+void PIDSubsystem::Enable() {
+  m_controller->Enable();
+}
 
-void PIDSubsystem::Disable() { m_controller->Disable(); }
+void PIDSubsystem::Disable() {
+  m_controller->Disable();
+}
 
-void PIDSubsystem::PIDWrite(double output) { UsePIDOutput(output); }
+void PIDSubsystem::PIDWrite(double output) {
+  UsePIDOutput(output);
+}
 
-double PIDSubsystem::PIDGet() { return ReturnPIDInput(); }
+double PIDSubsystem::PIDGet() {
+  return ReturnPIDInput();
+}
 
 void PIDSubsystem::SetSetpoint(double setpoint) {
   m_controller->SetSetpoint(setpoint);
@@ -76,11 +81,17 @@
   m_controller->SetOutputRange(minimumOutput, maximumOutput);
 }
 
-double PIDSubsystem::GetSetpoint() { return m_controller->GetSetpoint(); }
+double PIDSubsystem::GetSetpoint() {
+  return m_controller->GetSetpoint();
+}
 
-double PIDSubsystem::GetPosition() { return ReturnPIDInput(); }
+double PIDSubsystem::GetPosition() {
+  return ReturnPIDInput();
+}
 
-double PIDSubsystem::GetRate() { return ReturnPIDInput(); }
+double PIDSubsystem::GetRate() {
+  return ReturnPIDInput();
+}
 
 void PIDSubsystem::SetAbsoluteTolerance(double absValue) {
   m_controller->SetAbsoluteTolerance(absValue);
@@ -90,7 +101,9 @@
   m_controller->SetPercentTolerance(percent);
 }
 
-bool PIDSubsystem::OnTarget() const { return m_controller->OnTarget(); }
+bool PIDSubsystem::OnTarget() const {
+  return m_controller->OnTarget();
+}
 
 std::shared_ptr<PIDController> PIDSubsystem::GetPIDController() {
   return m_controller;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
index 888e8a1..ca8f49a 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/PrintCommand.cpp
@@ -1,19 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/PrintCommand.h"
 
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 using namespace frc;
 
-PrintCommand::PrintCommand(const wpi::Twine& message)
-    : InstantCommand("Print \"" + message + wpi::Twine('"')) {
-  m_message = message.str();
+PrintCommand::PrintCommand(std::string_view message)
+    : InstantCommand(fmt::format("Print \"{}\"", message)) {
+  m_message = message;
 }
 
-void PrintCommand::Initialize() { wpi::outs() << m_message << '\n'; }
+void PrintCommand::Initialize() {
+  fmt::print("{}\n", m_message);
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
index 47212e5..701162c 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Scheduler.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/Scheduler.h"
 
@@ -13,16 +10,16 @@
 #include <vector>
 
 #include <hal/FRCUsageReporting.h>
+#include <networktables/NTSendableBuilder.h>
 #include <networktables/NetworkTableEntry.h>
 #include <wpi/mutex.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 #include "frc/buttons/ButtonScheduler.h"
 #include "frc/commands/Command.h"
 #include "frc/commands/Subsystem.h"
 #include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
@@ -55,8 +52,9 @@
 void Scheduler::AddCommand(Command* command) {
   std::scoped_lock lock(m_impl->additionsMutex);
   if (std::find(m_impl->additions.begin(), m_impl->additions.end(), command) !=
-      m_impl->additions.end())
+      m_impl->additions.end()) {
     return;
+  }
   m_impl->additions.push_back(command);
 }
 
@@ -66,9 +64,8 @@
 }
 
 void Scheduler::RegisterSubsystem(Subsystem* subsystem) {
-  if (subsystem == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "subsystem");
-    return;
+  if (!subsystem) {
+    throw FRC_MakeError(err::NullParameter, "{}", "subsystem");
   }
   m_impl->subsystems.insert(subsystem);
 }
@@ -76,7 +73,9 @@
 void Scheduler::Run() {
   // Get button input (going backwards preserves button priority)
   {
-    if (!m_impl->enabled) return;
+    if (!m_impl->enabled) {
+      return;
+    }
 
     std::scoped_lock lock(m_impl->buttonsMutex);
     for (auto& button : m_impl->buttons) {
@@ -109,8 +108,8 @@
     for (auto& addition : m_impl->additions) {
       // Check to make sure no adding during adding
       if (m_impl->adding) {
-        wpi_setWPIErrorWithContext(IncompatibleState,
-                                   "Can not start command from cancel method");
+        FRC_ReportError(warn::IncompatibleState, "{}",
+                        "Can not start command from cancel method");
       } else {
         m_impl->ProcessCommandAddition(addition);
       }
@@ -122,8 +121,8 @@
   for (auto& subsystem : m_impl->subsystems) {
     if (subsystem->GetCurrentCommand() == nullptr) {
       if (m_impl->adding) {
-        wpi_setWPIErrorWithContext(IncompatibleState,
-                                   "Can not start command from cancel method");
+        FRC_ReportError(warn::IncompatibleState, "{}",
+                        "Can not start command from cancel method");
       } else {
         m_impl->ProcessCommandAddition(subsystem->GetDefaultCommand());
       }
@@ -133,9 +132,8 @@
 }
 
 void Scheduler::Remove(Command* command) {
-  if (command == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "command");
-    return;
+  if (!command) {
+    throw FRC_MakeError(err::NullParameter, "{}", "command");
   }
 
   m_impl->Remove(command);
@@ -155,18 +153,22 @@
   m_impl->commands.clear();
 }
 
-void Scheduler::SetEnabled(bool enabled) { m_impl->enabled = enabled; }
+void Scheduler::SetEnabled(bool enabled) {
+  m_impl->enabled = enabled;
+}
 
-void Scheduler::InitSendable(SendableBuilder& builder) {
+void Scheduler::InitSendable(nt::NTSendableBuilder& builder) {
   builder.SetSmartDashboardType("Scheduler");
   auto namesEntry = builder.GetEntry("Names");
   auto idsEntry = builder.GetEntry("Ids");
   auto cancelEntry = builder.GetEntry("Cancel");
-  builder.SetUpdateTable([=]() {
+  builder.SetUpdateTable([=] {
     // Get the list of possible commands to cancel
     auto new_toCancel = cancelEntry.GetValue();
-    wpi::ArrayRef<double> toCancel;
-    if (new_toCancel) toCancel = new_toCancel->GetDoubleArray();
+    wpi::span<const double> toCancel;
+    if (new_toCancel) {
+      toCancel = new_toCancel->GetDoubleArray();
+    }
 
     // Cancel commands whose cancel buttons were pressed on the SmartDashboard
     if (!toCancel.empty()) {
@@ -184,9 +186,9 @@
     if (m_impl->runningCommandsChanged) {
       m_impl->commandsBuf.resize(0);
       m_impl->idsBuf.resize(0);
-      auto& registry = SendableRegistry::GetInstance();
       for (const auto& command : m_impl->commands) {
-        m_impl->commandsBuf.emplace_back(registry.GetName(command));
+        m_impl->commandsBuf.emplace_back(
+            wpi::SendableRegistry::GetName(command));
         m_impl->idsBuf.emplace_back(command->GetID());
       }
       nt::NetworkTableEntry(namesEntry).SetStringArray(m_impl->commandsBuf);
@@ -198,24 +200,24 @@
 Scheduler::Scheduler() : m_impl(new Impl) {
   HAL_Report(HALUsageReporting::kResourceType_Command,
              HALUsageReporting::kCommand_Scheduler);
-  SendableRegistry::GetInstance().AddLW(this, "Scheduler");
-  auto scheduler = frc::LiveWindow::GetInstance();
-  scheduler->enabled = [this] {
+  wpi::SendableRegistry::AddLW(this, "Scheduler");
+  frc::LiveWindow::SetEnabledCallback([this] {
     this->SetEnabled(false);
     this->RemoveAll();
-  };
-  scheduler->disabled = [this] { this->SetEnabled(true); };
+  });
+  frc::LiveWindow::SetDisabledCallback([this] { this->SetEnabled(true); });
 }
 
 Scheduler::~Scheduler() {
-  SendableRegistry::GetInstance().Remove(this);
-  auto scheduler = frc::LiveWindow::GetInstance();
-  scheduler->enabled = nullptr;
-  scheduler->disabled = nullptr;
+  wpi::SendableRegistry::Remove(this);
+  frc::LiveWindow::SetEnabledCallback(nullptr);
+  frc::LiveWindow::SetDisabledCallback(nullptr);
 }
 
 void Scheduler::Impl::Remove(Command* command) {
-  if (!commands.erase(command)) return;
+  if (!commands.erase(command)) {
+    return;
+  }
 
   for (auto&& requirement : command->GetRequirements()) {
     requirement->SetCurrentCommand(nullptr);
@@ -225,7 +227,9 @@
 }
 
 void Scheduler::Impl::ProcessCommandAddition(Command* command) {
-  if (command == nullptr) return;
+  if (command == nullptr) {
+    return;
+  }
 
   // Only add if not already in
   auto found = commands.find(command);
@@ -234,8 +238,9 @@
     const auto& requirements = command->GetRequirements();
     for (const auto requirement : requirements) {
       if (requirement->GetCurrentCommand() != nullptr &&
-          !requirement->GetCurrentCommand()->IsInterruptible())
+          !requirement->GetCurrentCommand()->IsInterruptible()) {
         return;
+      }
     }
 
     // Give it the requirements
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
index 97ddfb9..05dc079 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/StartCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/StartCommand.h"
 
@@ -14,4 +11,6 @@
   m_commandToFork = commandToStart;
 }
 
-void StartCommand::Initialize() { m_commandToFork->Start(); }
+void StartCommand::Initialize() {
+  m_commandToFork->Start();
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
index 85e800e..824872f 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/Subsystem.cpp
@@ -1,23 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/Subsystem.h"
 
-#include "frc/WPIErrors.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
 #include "frc/commands/Command.h"
 #include "frc/commands/Scheduler.h"
 #include "frc/livewindow/LiveWindow.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
-Subsystem::Subsystem(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().AddLW(this, name, name);
+Subsystem::Subsystem(std::string_view name) {
+  wpi::SendableRegistry::AddLW(this, name, name);
   Scheduler::GetInstance()->RegisterSubsystem(this);
 }
 
@@ -27,9 +25,8 @@
   } else {
     const auto& reqs = command->GetRequirements();
     if (std::find(reqs.begin(), reqs.end(), this) == reqs.end()) {
-      wpi_setWPIErrorWithContext(
-          CommandIllegalUse, "A default command must require the subsystem");
-      return;
+      throw FRC_MakeError(err::CommandIllegalUse, "{}",
+                          "A default command must require the subsystem");
     }
 
     m_defaultCommand = command;
@@ -44,12 +41,12 @@
   return m_defaultCommand;
 }
 
-wpi::StringRef Subsystem::GetDefaultCommandName() {
+std::string Subsystem::GetDefaultCommandName() {
   Command* defaultCommand = GetDefaultCommand();
   if (defaultCommand) {
-    return SendableRegistry::GetInstance().GetName(defaultCommand);
+    return wpi::SendableRegistry::GetName(defaultCommand);
   } else {
-    return wpi::StringRef();
+    return {};
   }
 }
 
@@ -58,14 +55,16 @@
   m_currentCommandChanged = true;
 }
 
-Command* Subsystem::GetCurrentCommand() const { return m_currentCommand; }
+Command* Subsystem::GetCurrentCommand() const {
+  return m_currentCommand;
+}
 
-wpi::StringRef Subsystem::GetCurrentCommandName() const {
+std::string Subsystem::GetCurrentCommandName() const {
   Command* currentCommand = GetCurrentCommand();
   if (currentCommand) {
-    return SendableRegistry::GetInstance().GetName(currentCommand);
+    return wpi::SendableRegistry::GetName(currentCommand);
   } else {
-    return wpi::StringRef();
+    return {};
   }
 }
 
@@ -74,59 +73,65 @@
 void Subsystem::InitDefaultCommand() {}
 
 std::string Subsystem::GetName() const {
-  return SendableRegistry::GetInstance().GetName(this);
+  return wpi::SendableRegistry::GetName(this);
 }
 
-void Subsystem::SetName(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetName(this, name);
+void Subsystem::SetName(std::string_view name) {
+  wpi::SendableRegistry::SetName(this, name);
 }
 
 std::string Subsystem::GetSubsystem() const {
-  return SendableRegistry::GetInstance().GetSubsystem(this);
+  return wpi::SendableRegistry::GetSubsystem(this);
 }
 
-void Subsystem::SetSubsystem(const wpi::Twine& name) {
-  SendableRegistry::GetInstance().SetSubsystem(this, name);
+void Subsystem::SetSubsystem(std::string_view name) {
+  wpi::SendableRegistry::SetSubsystem(this, name);
 }
 
-void Subsystem::AddChild(const wpi::Twine& name,
+void Subsystem::AddChild(std::string_view name,
                          std::shared_ptr<Sendable> child) {
   AddChild(name, *child);
 }
 
-void Subsystem::AddChild(const wpi::Twine& name, Sendable* child) {
+void Subsystem::AddChild(std::string_view name, wpi::Sendable* child) {
   AddChild(name, *child);
 }
 
-void Subsystem::AddChild(const wpi::Twine& name, Sendable& child) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddLW(&child, registry.GetSubsystem(this), name);
+void Subsystem::AddChild(std::string_view name, wpi::Sendable& child) {
+  wpi::SendableRegistry::AddLW(&child,
+                               wpi::SendableRegistry::GetSubsystem(this), name);
 }
 
-void Subsystem::AddChild(std::shared_ptr<Sendable> child) { AddChild(*child); }
+void Subsystem::AddChild(std::shared_ptr<wpi::Sendable> child) {
+  AddChild(*child);
+}
 
-void Subsystem::AddChild(Sendable* child) { AddChild(*child); }
+void Subsystem::AddChild(wpi::Sendable* child) {
+  AddChild(*child);
+}
 
-void Subsystem::AddChild(Sendable& child) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.SetSubsystem(&child, registry.GetSubsystem(this));
-  registry.EnableLiveWindow(&child);
+void Subsystem::AddChild(wpi::Sendable& child) {
+  wpi::SendableRegistry::SetSubsystem(
+      &child, wpi::SendableRegistry::GetSubsystem(this));
+  wpi::SendableRegistry::EnableLiveWindow(&child);
 }
 
 void Subsystem::ConfirmCommand() {
-  if (m_currentCommandChanged) m_currentCommandChanged = false;
+  if (m_currentCommandChanged) {
+    m_currentCommandChanged = false;
+  }
 }
 
-void Subsystem::InitSendable(SendableBuilder& builder) {
+void Subsystem::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Subsystem");
 
   builder.AddBooleanProperty(
-      ".hasDefault", [=]() { return m_defaultCommand != nullptr; }, nullptr);
+      ".hasDefault", [=] { return m_defaultCommand != nullptr; }, nullptr);
   builder.AddStringProperty(
-      ".default", [=]() { return GetDefaultCommandName(); }, nullptr);
+      ".default", [=] { return GetDefaultCommandName(); }, nullptr);
 
   builder.AddBooleanProperty(
-      ".hasCommand", [=]() { return m_currentCommand != nullptr; }, nullptr);
+      ".hasCommand", [=] { return m_currentCommand != nullptr; }, nullptr);
   builder.AddStringProperty(
-      ".command", [=]() { return GetCurrentCommandName(); }, nullptr);
+      ".command", [=] { return GetCurrentCommandName(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
index f5113b2..5c2ccd4 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/TimedCommand.cpp
@@ -1,24 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/TimedCommand.h"
 
 using namespace frc;
 
-TimedCommand::TimedCommand(const wpi::Twine& name, double timeout)
+TimedCommand::TimedCommand(std::string_view name, units::second_t timeout)
     : Command(name, timeout) {}
 
-TimedCommand::TimedCommand(double timeout) : Command(timeout) {}
+TimedCommand::TimedCommand(units::second_t timeout) : Command(timeout) {}
 
-TimedCommand::TimedCommand(const wpi::Twine& name, double timeout,
+TimedCommand::TimedCommand(std::string_view name, units::second_t timeout,
                            Subsystem& subsystem)
     : Command(name, timeout, subsystem) {}
 
-TimedCommand::TimedCommand(double timeout, Subsystem& subsystem)
+TimedCommand::TimedCommand(units::second_t timeout, Subsystem& subsystem)
     : Command(timeout, subsystem) {}
 
-bool TimedCommand::IsFinished() { return IsTimedOut(); }
+bool TimedCommand::IsFinished() {
+  return IsTimedOut();
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
index d326193..b4d44e0 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitCommand.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/WaitCommand.h"
 
+#include <fmt/format.h>
+
 using namespace frc;
 
-WaitCommand::WaitCommand(double timeout)
-    : TimedCommand("Wait(" + std::to_string(timeout) + ")", timeout) {}
+WaitCommand::WaitCommand(units::second_t timeout)
+    : TimedCommand(fmt::format("Wait({})", timeout.value()), timeout) {}
 
-WaitCommand::WaitCommand(const wpi::Twine& name, double timeout)
+WaitCommand::WaitCommand(std::string_view name, units::second_t timeout)
     : TimedCommand(name, timeout) {}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
index 374c11c..e02a630 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitForChildren.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/WaitForChildren.h"
 
@@ -11,10 +8,10 @@
 
 using namespace frc;
 
-WaitForChildren::WaitForChildren(double timeout)
+WaitForChildren::WaitForChildren(units::second_t timeout)
     : Command("WaitForChildren", timeout) {}
 
-WaitForChildren::WaitForChildren(const wpi::Twine& name, double timeout)
+WaitForChildren::WaitForChildren(std::string_view name, units::second_t timeout)
     : Command(name, timeout) {}
 
 bool WaitForChildren::IsFinished() {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
index 07ff00b..bb127b1 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/cpp/commands/WaitUntilCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/commands/WaitUntilCommand.h"
 
@@ -11,14 +8,16 @@
 
 using namespace frc;
 
-WaitUntilCommand::WaitUntilCommand(double time)
+WaitUntilCommand::WaitUntilCommand(units::second_t time)
     : Command("WaitUntilCommand", time) {
   m_time = time;
 }
 
-WaitUntilCommand::WaitUntilCommand(const wpi::Twine& name, double time)
+WaitUntilCommand::WaitUntilCommand(std::string_view name, units::second_t time)
     : Command(name, time) {
   m_time = time;
 }
 
-bool WaitUntilCommand::IsFinished() { return Timer::GetMatchTime() >= m_time; }
+bool WaitUntilCommand::IsFinished() {
+  return Timer::GetMatchTime() >= m_time;
+}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDBase.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDBase.h
new file mode 100644
index 0000000..e834006
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDBase.h
@@ -0,0 +1,405 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/deprecated.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/PIDInterface.h"
+#include "frc/PIDOutput.h"
+#include "frc/PIDSource.h"
+#include "frc/Timer.h"
+#include "frc/filter/LinearFilter.h"
+
+namespace frc {
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * Creates a separate thread which reads the given PIDSource and takes care of
+ * the integral calculations, as well as writing the given PIDOutput.
+ *
+ * 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,
+                public wpi::Sendable,
+                public wpi::SendableHelper<PIDBase> {
+ public:
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param p      the proportional coefficient
+   * @param i      the integral coefficient
+   * @param d      the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   */
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
+  PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param p      the proportional coefficient
+   * @param i      the integral coefficient
+   * @param d      the derivative coefficient
+   * @param f      the feedforward coefficient
+   * @param source The PIDSource object that is used to get values
+   * @param output The PIDOutput object that is set to the output value
+   */
+  WPI_DEPRECATED("All APIs which use this have been deprecated.")
+  PIDBase(double p, double i, double d, double f, PIDSource& source,
+          PIDOutput& output);
+
+  ~PIDBase() override = default;
+
+  /**
+   * Return the current PID result.
+   *
+   * This is always centered on zero and constrained the the max and min outs.
+   *
+   * @return the latest calculated output
+   */
+  virtual double Get() const;
+
+  /**
+   * Set the PID controller to consider the input to be continuous,
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param continuous true turns on continuous, false turns off continuous
+   */
+  virtual void SetContinuous(bool continuous = true);
+
+  /**
+   * Sets the maximum and minimum values expected from the input.
+   *
+   * @param minimumInput the minimum value expected from the input
+   * @param maximumInput the maximum value expected from the output
+   */
+  virtual void SetInputRange(double minimumInput, double maximumInput);
+
+  /**
+   * Sets the minimum and maximum values to write.
+   *
+   * @param minimumOutput the minimum value to write to the output
+   * @param maximumOutput the maximum value to write to the output
+   */
+  virtual void SetOutputRange(double minimumOutput, double maximumOutput);
+
+  /**
+   * Set the PID Controller gain parameters.
+   *
+   * Set the proportional, integral, and differential coefficients.
+   *
+   * @param p Proportional coefficient
+   * @param i Integral coefficient
+   * @param d Differential coefficient
+   */
+  void SetPID(double p, double i, double d) override;
+
+  /**
+   * Set the PID Controller gain parameters.
+   *
+   * Set the proportional, integral, and differential coefficients.
+   *
+   * @param p Proportional coefficient
+   * @param i Integral coefficient
+   * @param d Differential coefficient
+   * @param f Feed forward coefficient
+   */
+  virtual void SetPID(double p, double i, double d, double f);
+
+  /**
+   * Set the Proportional coefficient of the PID controller gain.
+   *
+   * @param p proportional coefficient
+   */
+  void SetP(double p);
+
+  /**
+   * Set the Integral coefficient of the PID controller gain.
+   *
+   * @param i integral coefficient
+   */
+  void SetI(double i);
+
+  /**
+   * Set the Differential coefficient of the PID controller gain.
+   *
+   * @param d differential coefficient
+   */
+  void SetD(double d);
+
+  /**
+   * Get the Feed forward coefficient of the PID controller gain.
+   *
+   * @param f Feed forward coefficient
+   */
+  void SetF(double f);
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const override;
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const override;
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const override;
+
+  /**
+   * Get the Feed forward coefficient.
+   *
+   * @return Feed forward coefficient
+   */
+  virtual double GetF() const;
+
+  /**
+   * Set the setpoint for the PIDBase.
+   *
+   * @param setpoint the desired setpoint
+   */
+  void SetSetpoint(double setpoint) override;
+
+  /**
+   * Returns the current setpoint of the PIDBase.
+   *
+   * @return the current setpoint
+   */
+  double GetSetpoint() const override;
+
+  /**
+   * Returns the change in setpoint over time of the PIDBase.
+   *
+   * @return the change in setpoint over time
+   */
+  double GetDeltaSetpoint() const;
+
+  /**
+   * Returns the current difference of the input from the setpoint.
+   *
+   * @return the current error
+   */
+  virtual double GetError() const;
+
+  /**
+   * Returns the current average of the error over the past few iterations.
+   *
+   * You can specify the number of iterations to average with
+   * SetToleranceBuffer() (defaults to 1). This is the same value that is used
+   * for OnTarget().
+   *
+   * @return the average error
+   */
+  WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
+  virtual double GetAvgError() const;
+
+  /**
+   * Sets what type of input the PID controller will use.
+   */
+  virtual void SetPIDSourceType(PIDSourceType pidSource);
+
+  /**
+   * Returns the type of input the PID controller is using.
+   *
+   * @return the PID controller input type
+   */
+  virtual PIDSourceType GetPIDSourceType() const;
+
+  /**
+   * Set the percentage error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param percent error which is tolerable
+   */
+  WPI_DEPRECATED("Use SetPercentTolerance() instead.")
+  virtual void SetTolerance(double percent);
+
+  /**
+   * Set the absolute error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param absTolerance error which is tolerable
+   */
+  virtual void SetAbsoluteTolerance(double absTolerance);
+
+  /**
+   * Set the percentage error which is considered tolerable for use with
+   * OnTarget.
+   *
+   * @param percent error which is tolerable
+   */
+  virtual void SetPercentTolerance(double percent);
+
+  /**
+   * Set the number of previous error samples to average for tolerancing. When
+   * determining whether a mechanism is on target, the user may want to use a
+   * rolling average of previous measurements instead of a precise position or
+   * velocity. This is useful for noisy sensors which return a few erroneous
+   * measurements when the mechanism is on target. However, the mechanism will
+   * not register as on target for at least the specified bufLength cycles.
+   *
+   * @param bufLength Number of previous cycles to average. Defaults to 1.
+   */
+  WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
+  virtual void SetToleranceBuffer(int bufLength = 1);
+
+  /**
+   * Return true if the error is within the percentage of the total input range,
+   * determined by SetTolerance. This asssumes that the maximum and minimum
+   * input were set using SetInput.
+   *
+   * Currently this just reports on target as the actual value passes through
+   * the setpoint. Ideally it should be based on being within the tolerance for
+   * some period of time.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  virtual bool OnTarget() const;
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset() override;
+
+  /**
+   * Passes the output directly to SetSetpoint().
+   *
+   * PIDControllers can be nested by passing a PIDController as another
+   * PIDController's output. In that case, the output of the parent controller
+   * becomes the input (i.e., the reference) of the child.
+   *
+   * It is the caller's responsibility to put the data into a valid form for
+   * SetSetpoint().
+   */
+  void PIDWrite(double output) override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ protected:
+  // Is the pid controller enabled
+  bool m_enabled = false;
+
+  mutable wpi::mutex m_thisMutex;
+
+  // Ensures when Disable() is called, PIDWrite() won't run if Calculate()
+  // is already running at that time.
+  mutable wpi::mutex m_pidWriteMutex;
+
+  PIDSource* m_pidInput;
+  PIDOutput* m_pidOutput;
+  Timer m_setpointTimer;
+
+  /**
+   * Read the input, calculate the output accordingly, and write to the output.
+   * This should only be called by the Notifier.
+   */
+  virtual void Calculate();
+
+  /**
+   * Calculate the feed forward term.
+   *
+   * Both of the provided feed forward calculations are velocity feed forwards.
+   * If a different feed forward calculation is desired, the user can override
+   * this function and provide his or her own. This function does no
+   * synchronization because the PIDBase class only calls it in synchronized
+   * code, so be careful if calling it oneself.
+   *
+   * If a velocity PID controller is being used, the F term should be set to 1
+   * over the maximum setpoint for the output. If a position PID controller is
+   * being used, the F term should be set to 1 over the maximum speed for the
+   * output measured in setpoint units per this controller's update period (see
+   * the default period in this class's constructor).
+   */
+  virtual double CalculateFeedForward();
+
+  /**
+   * Wraps error around for continuous inputs. The original error is returned if
+   * continuous mode is disabled. This is an unsynchronized function.
+   *
+   * @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_P;
+
+  // Factor for "integral" control
+  double m_I;
+
+  // Factor for "derivative" control
+  double m_D;
+
+  // Factor for "feed forward" control
+  double m_F;
+
+  // |maximum output|
+  double m_maximumOutput = 1.0;
+
+  // |minimum output|
+  double m_minimumOutput = -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;
+
+  // The prior error (used to compute velocity)
+  double m_prevError = 0;
+
+  // The sum of the errors for use in the integral calc
+  double m_totalError = 0;
+
+  enum {
+    kAbsoluteTolerance,
+    kPercentTolerance,
+    kNoTolerance
+  } m_toleranceType = kNoTolerance;
+
+  // The percetage or absolute error that is considered on target.
+  double m_tolerance = 0.05;
+
+  double m_setpoint = 0;
+  double m_prevSetpoint = 0;
+  double m_error = 0;
+  double m_result = 0;
+
+  LinearFilter<double> m_filter{{}, {}};
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDController.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDController.h
new file mode 100644
index 0000000..763752d
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDController.h
@@ -0,0 +1,137 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <wpi/deprecated.h>
+#include <wpi/mutex.h>
+
+#include "frc/Controller.h"
+#include "frc/Notifier.h"
+#include "frc/PIDBase.h"
+#include "frc/PIDSource.h"
+#include "frc/Timer.h"
+
+namespace frc {
+
+class PIDOutput;
+
+/**
+ * Class implements a PID Control Loop.
+ *
+ * Creates a separate thread which reads the given PIDSource and takes care of
+ * the integral calculations, as well as writing the given PIDOutput.
+ *
+ * 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:
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param p      the proportional coefficient
+   * @param i      the integral coefficient
+   * @param d      the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @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
+   *               differential terms. The default is 0.05 (50ms).
+   */
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
+  PIDController(double p, double i, double d, PIDSource* source,
+                PIDOutput* output, double period = 0.05);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param p      the proportional coefficient
+   * @param i      the integral coefficient
+   * @param d      the derivative coefficient
+   * @param f      the feedforward coefficient
+   * @param source The PIDSource object that is used to get values
+   * @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
+   *               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,
+                PIDOutput* output, double period = 0.05);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param p      the proportional coefficient
+   * @param i      the integral coefficient
+   * @param d      the derivative coefficient
+   * @param source The PIDSource object that is used to get values
+   * @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
+   *               differential terms. The default is 0.05 (50ms).
+   */
+  WPI_DEPRECATED("Use frc2::PIDController class instead.")
+  PIDController(double p, double i, double d, PIDSource& source,
+                PIDOutput& output, double period = 0.05);
+
+  /**
+   * Allocate a PID object with the given constants for P, I, D.
+   *
+   * @param p      the proportional coefficient
+   * @param i      the integral coefficient
+   * @param d      the derivative coefficient
+   * @param f      the feedforward coefficient
+   * @param source The PIDSource object that is used to get values
+   * @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
+   *               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,
+                PIDOutput& output, double period = 0.05);
+
+  ~PIDController() override;
+
+  /**
+   * Begin running the PIDController.
+   */
+  void Enable() override;
+
+  /**
+   * Stop running the PIDController, this sets the output to zero before
+   * stopping.
+   */
+  void Disable() override;
+
+  /**
+   * Set the enabled state of the PIDController.
+   */
+  void SetEnabled(bool enable);
+
+  /**
+   * Return true if PIDController is enabled.
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset() override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  std::unique_ptr<Notifier> m_controlLoop;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDInterface.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDInterface.h
new file mode 100644
index 0000000..20d26ef
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDInterface.h
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/deprecated.h>
+
+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.")
+  PIDInterface() = default;
+  PIDInterface(PIDInterface&&) = default;
+  PIDInterface& operator=(PIDInterface&&) = default;
+
+  virtual void SetPID(double p, double i, double d) = 0;
+  virtual double GetP() const = 0;
+  virtual double GetI() const = 0;
+  virtual double GetD() const = 0;
+
+  virtual void SetSetpoint(double setpoint) = 0;
+  virtual double GetSetpoint() const = 0;
+
+  virtual void Reset() = 0;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDOutput.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDOutput.h
new file mode 100644
index 0000000..98c4af4
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDOutput.h
@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+
+/**
+ * PIDOutput interface is a generic output for the PID class.
+ *
+ * PWMs use this class. Users implement this interface to allow for a
+ * PIDController to read directly from the inputs.
+ */
+class PIDOutput {
+ public:
+  virtual void PIDWrite(double output) = 0;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDSource.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDSource.h
new file mode 100644
index 0000000..3f6ca23
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/PIDSource.h
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+
+enum class PIDSourceType { kDisplacement, kRate };
+
+/**
+ * PIDSource interface is a generic sensor source for the PID class.
+ *
+ * All sensors that can be used with the PID class will implement the PIDSource
+ * that returns a standard value that will be used in the PID code.
+ */
+class PIDSource {
+ public:
+  virtual ~PIDSource() = default;
+
+  /**
+   * Set which parameter you are using as a process control variable.
+   *
+   * @param pidSource An enum to select the parameter.
+   */
+  virtual void SetPIDSourceType(PIDSourceType pidSource);
+
+  virtual PIDSourceType GetPIDSourceType() const;
+
+  virtual double PIDGet() = 0;
+
+ protected:
+  PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
index c0830bf..89e2b41 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Button.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
index 960fbc8..0c3ad20 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ButtonScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
index 9131fc0..c3938eb 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/CancelButtonScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,12 +14,12 @@
 class CancelButtonScheduler : public ButtonScheduler {
  public:
   CancelButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~CancelButtonScheduler() = default;
+  ~CancelButtonScheduler() override = default;
 
   CancelButtonScheduler(CancelButtonScheduler&&) = default;
   CancelButtonScheduler& operator=(CancelButtonScheduler&&) = default;
 
-  virtual void Execute();
+  void Execute() override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
index 2b51ce6..721688f 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/HeldButtonScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,12 +14,12 @@
 class HeldButtonScheduler : public ButtonScheduler {
  public:
   HeldButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~HeldButtonScheduler() = default;
+  ~HeldButtonScheduler() override = default;
 
   HeldButtonScheduler(HeldButtonScheduler&&) = default;
   HeldButtonScheduler& operator=(HeldButtonScheduler&&) = default;
 
-  virtual void Execute();
+  void Execute() override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
index ed4dc56..e9c1fb1 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/InternalButton.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,7 +12,7 @@
  public:
   InternalButton() = default;
   explicit InternalButton(bool inverted);
-  virtual ~InternalButton() = default;
+  ~InternalButton() override = default;
 
   InternalButton(InternalButton&&) = default;
   InternalButton& operator=(InternalButton&&) = default;
@@ -23,7 +20,7 @@
   void SetInverted(bool inverted);
   void SetPressed(bool pressed);
 
-  virtual bool Get();
+  bool Get() override;
 
  private:
   bool m_pressed = false;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
index e0e2d4c..24900b4 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/JoystickButton.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,12 +12,12 @@
 class JoystickButton : public Button {
  public:
   JoystickButton(GenericHID* joystick, int buttonNumber);
-  virtual ~JoystickButton() = default;
+  ~JoystickButton() override = default;
 
   JoystickButton(JoystickButton&&) = default;
   JoystickButton& operator=(JoystickButton&&) = default;
 
-  virtual bool Get();
+  bool Get() override;
 
  private:
   GenericHID* m_joystick;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
index f16821b..3c468df 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/NetworkButton.h
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
-#include <wpi/Twine.h>
 
 #include "frc/buttons/Button.h"
 
@@ -19,15 +16,15 @@
 
 class NetworkButton : public Button {
  public:
-  NetworkButton(const wpi::Twine& tableName, const wpi::Twine& field);
+  NetworkButton(std::string_view tableName, std::string_view field);
   NetworkButton(std::shared_ptr<nt::NetworkTable> table,
-                const wpi::Twine& field);
-  virtual ~NetworkButton() = default;
+                std::string_view field);
+  ~NetworkButton() override = default;
 
   NetworkButton(NetworkButton&&) = default;
   NetworkButton& operator=(NetworkButton&&) = default;
 
-  virtual bool Get();
+  bool Get() override;
 
  private:
   nt::NetworkTableEntry m_entry;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
index bd73984..87e1789 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/POVButton.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -21,7 +18,7 @@
    * @param povNumber The POV number (@see GenericHID#GetPOV)
    */
   POVButton(GenericHID& joystick, int angle, int povNumber = 0);
-  virtual ~POVButton() = default;
+  ~POVButton() override = default;
 
   POVButton(POVButton&&) = default;
   POVButton& operator=(POVButton&&) = default;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
index 29c6a95..378d2c5 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/PressedButtonScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,12 +14,12 @@
 class PressedButtonScheduler : public ButtonScheduler {
  public:
   PressedButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~PressedButtonScheduler() = default;
+  ~PressedButtonScheduler() override = default;
 
   PressedButtonScheduler(PressedButtonScheduler&&) = default;
   PressedButtonScheduler& operator=(PressedButtonScheduler&&) = default;
 
-  virtual void Execute();
+  void Execute() override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
index a567a7b..6ed53ee 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ReleasedButtonScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,12 +14,12 @@
 class ReleasedButtonScheduler : public ButtonScheduler {
  public:
   ReleasedButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~ReleasedButtonScheduler() = default;
+  ~ReleasedButtonScheduler() override = default;
 
   ReleasedButtonScheduler(ReleasedButtonScheduler&&) = default;
   ReleasedButtonScheduler& operator=(ReleasedButtonScheduler&&) = default;
 
-  virtual void Execute();
+  void Execute() override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
index 406d131..8df27a9 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/ToggleButtonScheduler.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,12 +14,12 @@
 class ToggleButtonScheduler : public ButtonScheduler {
  public:
   ToggleButtonScheduler(bool last, Trigger* button, Command* orders);
-  virtual ~ToggleButtonScheduler() = default;
+  ~ToggleButtonScheduler() override = default;
 
   ToggleButtonScheduler(ToggleButtonScheduler&&) = default;
   ToggleButtonScheduler& operator=(ToggleButtonScheduler&&) = default;
 
-  virtual void Execute();
+  void Execute() override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
index 56700e9..966a29f 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/buttons/Trigger.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <atomic>
 
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
@@ -26,10 +23,10 @@
  * It is encouraged that teams write a subclass of Trigger if they want to have
  * something unusual (for instance, if they want to react to the user holding
  * a button while the robot is reading a certain sensor input). For this, they
- * only have to write the {@link Trigger#Get()} method to get the full
- * functionality of the Trigger class.
+ * only have to write the Trigger::Get() method to get the full functionality of
+ * the Trigger class.
  */
-class Trigger : public Sendable, public SendableHelper<Trigger> {
+class Trigger : public wpi::Sendable, public wpi::SendableHelper<Trigger> {
  public:
   Trigger() = default;
   ~Trigger() override = default;
@@ -47,7 +44,7 @@
   void CancelWhenActive(Command* command);
   void ToggleWhenActive(Command* command);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   std::atomic_bool m_sendablePressed{false};
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Command.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
index cc6b795..aa20134 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Command.h
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 
+#include <units/time.h>
 #include <wpi/SmallPtrSet.h>
-#include <wpi/Twine.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/ErrorBase.h"
 #include "frc/commands/Subsystem.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
@@ -46,9 +43,7 @@
  * @see CommandGroup
  * @see Subsystem
  */
-class Command : public ErrorBase,
-                public Sendable,
-                public SendableHelper<Command> {
+class Command : public wpi::Sendable, public wpi::SendableHelper<Command> {
   friend class CommandGroup;
   friend class Scheduler;
 
@@ -65,15 +60,15 @@
    *
    * @param name the name for this command
    */
-  explicit Command(const wpi::Twine& name);
+  explicit Command(std::string_view name);
 
   /**
    * Creates a new command with the given timeout and a default name.
    *
-   * @param timeout the time (in seconds) before this command "times out"
+   * @param timeout the time before this command "times out"
    * @see IsTimedOut()
    */
-  explicit Command(double timeout);
+  explicit Command(units::second_t timeout);
 
   /**
    * Creates a new command with the given timeout and a default name.
@@ -86,10 +81,10 @@
    * Creates a new command with the given name and timeout.
    *
    * @param name    the name of the command
-   * @param timeout the time (in seconds) before this command "times out"
+   * @param timeout the time before this command "times out"
    * @see IsTimedOut()
    */
-  Command(const wpi::Twine& name, double timeout);
+  Command(std::string_view name, units::second_t timeout);
 
   /**
    * Creates a new command with the given name and timeout.
@@ -97,24 +92,24 @@
    * @param name      the name of the command
    * @param subsystem the subsystem that the command requires
    */
-  Command(const wpi::Twine& name, Subsystem& subsystem);
+  Command(std::string_view name, Subsystem& subsystem);
 
   /**
    * Creates a new command with the given name and timeout.
    *
-   * @param timeout   the time (in seconds) before this command "times out"
+   * @param timeout   the time before this command "times out"
    * @param subsystem the subsystem that the command requires @see IsTimedOut()
    */
-  Command(double timeout, Subsystem& subsystem);
+  Command(units::second_t timeout, Subsystem& subsystem);
 
   /**
    * Creates a new command with the given name and timeout.
    *
    * @param name      the name of the command
-   * @param timeout   the time (in seconds) before this command "times out"
+   * @param timeout   the time before this command "times out"
    * @param subsystem the subsystem that the command requires @see IsTimedOut()
    */
-  Command(const wpi::Twine& name, double timeout, Subsystem& subsystem);
+  Command(std::string_view name, units::second_t timeout, Subsystem& subsystem);
 
   ~Command() override = default;
 
@@ -122,13 +117,13 @@
   Command& operator=(Command&&) = default;
 
   /**
-   * Returns the time since this command was initialized (in seconds).
+   * Returns the time since this command was initialized.
    *
    * This function will work even if there is no specified timeout.
    *
-   * @return the time since this command was initialized (in seconds).
+   * @return the time since this command was initialized.
    */
-  double TimeSinceInitialized() const;
+  units::second_t TimeSinceInitialized() const;
 
   /**
    * This method specifies that the given Subsystem is used by this command.
@@ -140,7 +135,7 @@
    * @param subsystem The Subsystem required
    * @see Subsystem
    */
-  void Requires(Subsystem* s);
+  void Requires(Subsystem* subsystem);
 
   /**
    * Starts up the command. Gets the command ready to start.
@@ -219,7 +214,7 @@
   /**
    * Checks if the command requires the given Subsystem.
    *
-   * @param system the system
+   * @param subsystem the subsystem
    * @return whether or not the subsystem is required (false if given nullptr)
    */
   bool DoesRequire(Subsystem* subsystem) const;
@@ -277,10 +272,10 @@
   /**
    * Sets the timeout of this command.
    *
-   * @param timeout the timeout (in seconds)
+   * @param timeout the timeout
    * @see IsTimedOut()
    */
-  void SetTimeout(double timeout);
+  void SetTimeout(units::second_t timeout);
 
   /**
    * Returns whether or not the TimeSinceInitialized() method returns a number
@@ -299,7 +294,7 @@
    *                message)
    * @return True if assert passed, false if assert failed.
    */
-  bool AssertUnlocked(const std::string& message);
+  bool AssertUnlocked(std::string_view message);
 
   /**
    * Sets the parent of this command. No actual change is made to the group.
@@ -401,7 +396,7 @@
    *
    * @param name name
    */
-  void SetName(const wpi::Twine& name);
+  void SetName(std::string_view name);
 
   /**
    * Gets the subsystem name of this Command.
@@ -415,7 +410,7 @@
    *
    * @param subsystem subsystem name
    */
-  void SetSubsystem(const wpi::Twine& subsystem);
+  void SetSubsystem(std::string_view subsystem);
 
  private:
   /**
@@ -451,10 +446,10 @@
   void StartTiming();
 
   // The time since this command was initialized
-  double m_startTime = -1;
+  units::second_t m_startTime = -1_s;
 
   // The time (in seconds) before this command "times out" (-1 if no timeout)
-  double m_timeout;
+  units::second_t m_timeout;
 
   // Whether or not this command has been initialized
   bool m_initialized = false;
@@ -487,7 +482,7 @@
   static int m_commandCounter;
 
  public:
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
index 0275cb5..016f700 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroup.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <string_view>
 #include <vector>
 
-#include <wpi/Twine.h>
+#include <units/time.h>
 
 #include "frc/commands/Command.h"
 #include "frc/commands/CommandGroupEntry.h"
@@ -42,9 +40,9 @@
    *
    * @param name The name for this command group
    */
-  explicit CommandGroup(const wpi::Twine& name);
+  explicit CommandGroup(std::string_view name);
 
-  virtual ~CommandGroup() = default;
+  ~CommandGroup() override = default;
 
   CommandGroup(CommandGroup&&) = default;
   CommandGroup& operator=(CommandGroup&&) = default;
@@ -78,9 +76,9 @@
    * It is recommended that this method be called in the constructor.
    *
    * @param command The Command to be added
-   * @param timeout The timeout (in seconds)
+   * @param timeout The timeout
    */
-  void AddSequential(Command* command, double timeout);
+  void AddSequential(Command* command, units::second_t timeout);
 
   /**
    * Adds a new child Command to the group. The Command will be started after
@@ -125,9 +123,9 @@
    * It is recommended that this method be called in the constructor.
    *
    * @param command The command to be added
-   * @param timeout The timeout (in seconds)
+   * @param timeout The timeout
    */
-  void AddParallel(Command* command, double timeout);
+  void AddParallel(Command* command, units::second_t timeout);
 
   bool IsInterruptible() const;
 
@@ -137,32 +135,32 @@
   /**
    * Can be overridden by teams.
    */
-  virtual void Initialize();
+  void Initialize() override;
 
   /**
    * Can be overridden by teams.
    */
-  virtual void Execute();
+  void Execute() override;
 
   /**
    * Can be overridden by teams.
    */
-  virtual bool IsFinished();
+  bool IsFinished() override;
 
   /**
    * Can be overridden by teams.
    */
-  virtual void End();
+  void End() override;
 
   /**
    * Can be overridden by teams.
    */
-  virtual void Interrupted();
+  void Interrupted() override;
 
-  virtual void _Initialize();
-  virtual void _Execute();
-  virtual void _End();
-  virtual void _Interrupted();
+  void _Initialize() override;
+  void _Execute() override;
+  void _End() override;
+  void _Interrupted() override;
 
  private:
   void CancelConflicts(Command* command);
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
index d1d2264..b6162f3 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/CommandGroupEntry.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <units/time.h>
+
 namespace frc {
 
 class Command;
@@ -20,14 +19,15 @@
   };
 
   CommandGroupEntry() = default;
-  CommandGroupEntry(Command* command, Sequence state, double timeout = -1.0);
+  CommandGroupEntry(Command* command, Sequence state,
+                    units::second_t timeout = -1_s);
 
   CommandGroupEntry(CommandGroupEntry&&) = default;
   CommandGroupEntry& operator=(CommandGroupEntry&&) = default;
 
   bool IsTimedOut() const;
 
-  double m_timeout = -1.0;
+  units::second_t m_timeout = -1_s;
   Command* m_command = nullptr;
   Sequence m_state = kSequence_InSequence;
 };
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
index f5cd738..19d6d75 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/ConditionalCommand.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/commands/Command.h"
 
@@ -49,10 +46,10 @@
    * @param onTrue  The Command to execute if Condition() returns true
    * @param onFalse The Command to execute if Condition() returns false
    */
-  ConditionalCommand(const wpi::Twine& name, Command* onTrue,
+  ConditionalCommand(std::string_view name, Command* onTrue,
                      Command* onFalse = nullptr);
 
-  virtual ~ConditionalCommand() = default;
+  ~ConditionalCommand() override = default;
 
   ConditionalCommand(ConditionalCommand&&) = default;
   ConditionalCommand& operator=(ConditionalCommand&&) = default;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
index 3663b7f..d16b34e 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/InstantCommand.h
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
-
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/commands/Command.h"
 #include "frc/commands/Subsystem.h"
@@ -28,7 +24,7 @@
    *
    * @param name The name for this command
    */
-  explicit InstantCommand(const wpi::Twine& name);
+  explicit InstantCommand(std::string_view name);
 
   /**
    * Creates a new InstantCommand with the given requirement.
@@ -43,7 +39,7 @@
    * @param name      The name for this command
    * @param subsystem The subsystem that the command requires
    */
-  InstantCommand(const wpi::Twine& name, Subsystem& subsystem);
+  InstantCommand(std::string_view name, Subsystem& subsystem);
 
   /**
    * Create a command that calls the given function when run.
@@ -66,7 +62,7 @@
    * @param name   The name of the command.
    * @param func   The function to run when Initialize() is run.
    */
-  InstantCommand(const wpi::Twine& name, std::function<void()> func);
+  InstantCommand(std::string_view name, std::function<void()> func);
 
   /**
    * Create a command that calls the given function when run.
@@ -75,11 +71,11 @@
    * @param subsystem The subsystems that this command runs on.
    * @param func      The function to run when Initialize() is run.
    */
-  InstantCommand(const wpi::Twine& name, Subsystem& subsystem,
+  InstantCommand(std::string_view name, Subsystem& subsystem,
                  std::function<void()> func);
 
   InstantCommand() = default;
-  virtual ~InstantCommand() = default;
+  ~InstantCommand() override = default;
 
   InstantCommand(InstantCommand&&) = default;
   InstantCommand& operator=(InstantCommand&&) = default;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
index d00ac9b..7eb85ec 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDCommand.h
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
-
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/PIDController.h"
 #include "frc/PIDOutput.h"
@@ -20,25 +16,25 @@
 
 class PIDCommand : public Command, public PIDOutput, public PIDSource {
  public:
-  PIDCommand(const wpi::Twine& name, double p, double i, double d);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+  PIDCommand(std::string_view name, double p, double i, double d);
+  PIDCommand(std::string_view name, double p, double i, double d,
              double period);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+  PIDCommand(std::string_view name, double p, double i, double d, double f,
              double period);
   PIDCommand(double p, double i, double d);
   PIDCommand(double p, double i, double d, double period);
   PIDCommand(double p, double i, double d, double f, double period);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d,
+  PIDCommand(std::string_view name, double p, double i, double d,
              Subsystem& subsystem);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d,
-             double period, Subsystem& subsystem);
-  PIDCommand(const wpi::Twine& name, double p, double i, double d, double f,
+  PIDCommand(std::string_view name, double p, double i, double d, double period,
+             Subsystem& subsystem);
+  PIDCommand(std::string_view name, double p, double i, double d, double f,
              double period, Subsystem& subsystem);
   PIDCommand(double p, double i, double d, Subsystem& subsystem);
   PIDCommand(double p, double i, double d, double period, Subsystem& subsystem);
   PIDCommand(double p, double i, double d, double f, double period,
              Subsystem& subsystem);
-  virtual ~PIDCommand() = default;
+  ~PIDCommand() override = default;
 
   PIDCommand(PIDCommand&&) = default;
   PIDCommand& operator=(PIDCommand&&) = default;
@@ -68,7 +64,7 @@
   std::shared_ptr<PIDController> m_controller;
 
  public:
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
index fd9b487..6339c26 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PIDSubsystem.h
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
-
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/PIDController.h"
 #include "frc/PIDOutput.h"
@@ -37,7 +33,7 @@
    * @param i    the integral value
    * @param d    the derivative value
    */
-  PIDSubsystem(const wpi::Twine& name, double p, double i, double d);
+  PIDSubsystem(std::string_view name, double p, double i, double d);
 
   /**
    * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
@@ -48,7 +44,7 @@
    * @param d    the derivative value
    * @param f    the feedforward value
    */
-  PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f);
+  PIDSubsystem(std::string_view name, double p, double i, double d, double f);
 
   /**
    * Instantiates a PIDSubsystem that will use the given P, I, D, and F values.
@@ -63,7 +59,7 @@
    * @param f      the feedfoward value
    * @param period the time (in seconds) between calculations
    */
-  PIDSubsystem(const wpi::Twine& name, double p, double i, double d, double f,
+  PIDSubsystem(std::string_view name, double p, double i, double d, double f,
                double period);
 
   /**
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
index 5e987d3..01d0d4b 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/PrintCommand.h
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <string>
-
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/commands/InstantCommand.h"
 
@@ -17,14 +13,14 @@
 
 class PrintCommand : public InstantCommand {
  public:
-  explicit PrintCommand(const wpi::Twine& message);
-  virtual ~PrintCommand() = default;
+  explicit PrintCommand(std::string_view message);
+  ~PrintCommand() override = default;
 
   PrintCommand(PrintCommand&&) = default;
   PrintCommand& operator=(PrintCommand&&) = default;
 
  protected:
-  virtual void Initialize();
+  void Initialize() override;
 
  private:
   std::string m_message;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
index d15ea39..ad82f63 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Scheduler.h
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <networktables/NTSendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
@@ -19,9 +15,7 @@
 class Command;
 class Subsystem;
 
-class Scheduler : public ErrorBase,
-                  public Sendable,
-                  public SendableHelper<Scheduler> {
+class Scheduler : public nt::NTSendable, public wpi::SendableHelper<Scheduler> {
  public:
   /**
    * Returns the Scheduler, creating it if one does not exist.
@@ -48,7 +42,7 @@
    *
    * All Subsystems should call this.
    *
-   * @param system the system
+   * @param subsystem the system
    */
   void RegisterSubsystem(Subsystem* subsystem);
 
@@ -84,7 +78,7 @@
 
   void SetEnabled(bool enabled);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(nt::NTSendableBuilder& builder) override;
 
  private:
   Scheduler();
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
index 5062f92..a58c058 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/StartCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,13 +11,13 @@
 class StartCommand : public InstantCommand {
  public:
   explicit StartCommand(Command* commandToStart);
-  virtual ~StartCommand() = default;
+  ~StartCommand() override = default;
 
   StartCommand(StartCommand&&) = default;
   StartCommand& operator=(StartCommand&&) = default;
 
  protected:
-  virtual void Initialize();
+  void Initialize() override;
 
  private:
   Command* m_commandToFork;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
index 2637dd0..8c18d5c 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/Subsystem.h
@@ -1,29 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
 class Command;
 
-class Subsystem : public ErrorBase,
-                  public Sendable,
-                  public SendableHelper<Subsystem> {
+class Subsystem : public wpi::Sendable, public wpi::SendableHelper<Subsystem> {
   friend class Scheduler;
 
  public:
@@ -32,7 +24,7 @@
    *
    * @param name the name of the subsystem
    */
-  explicit Subsystem(const wpi::Twine& name);
+  explicit Subsystem(std::string_view name);
 
   Subsystem(Subsystem&&) = default;
   Subsystem& operator=(Subsystem&&) = default;
@@ -60,7 +52,7 @@
    *
    * @return the default command name
    */
-  wpi::StringRef GetDefaultCommandName();
+  std::string GetDefaultCommandName();
 
   /**
    * Sets the current command.
@@ -81,7 +73,7 @@
    *
    * @return the current command name
    */
-  wpi::StringRef GetCurrentCommandName() const;
+  std::string GetCurrentCommandName() const;
 
   /**
    * When the run method of the scheduler is called this method will be called.
@@ -112,7 +104,7 @@
    *
    * @param name name
    */
-  void SetName(const wpi::Twine& name);
+  void SetName(std::string_view name);
 
   /**
    * Gets the subsystem name of this Subsystem.
@@ -126,7 +118,7 @@
    *
    * @param subsystem subsystem name
    */
-  void SetSubsystem(const wpi::Twine& subsystem);
+  void SetSubsystem(std::string_view subsystem);
 
   /**
    * Associate a Sendable with this Subsystem.
@@ -135,7 +127,7 @@
    * @param name name to give child
    * @param child sendable
    */
-  void AddChild(const wpi::Twine& name, std::shared_ptr<Sendable> child);
+  void AddChild(std::string_view name, std::shared_ptr<wpi::Sendable> child);
 
   /**
    * Associate a Sendable with this Subsystem.
@@ -144,7 +136,7 @@
    * @param name name to give child
    * @param child sendable
    */
-  void AddChild(const wpi::Twine& name, Sendable* child);
+  void AddChild(std::string_view name, wpi::Sendable* child);
 
   /**
    * Associate a Sendable with this Subsystem.
@@ -153,28 +145,28 @@
    * @param name name to give child
    * @param child sendable
    */
-  void AddChild(const wpi::Twine& name, Sendable& child);
+  void AddChild(std::string_view name, wpi::Sendable& child);
 
   /**
-   * Associate a {@link Sendable} with this Subsystem.
+   * Associate a Sendable with this Subsystem.
    *
    * @param child sendable
    */
-  void AddChild(std::shared_ptr<Sendable> child);
+  void AddChild(std::shared_ptr<wpi::Sendable> child);
 
   /**
-   * Associate a {@link Sendable} with this Subsystem.
+   * Associate a Sendable with this Subsystem.
    *
    * @param child sendable
    */
-  void AddChild(Sendable* child);
+  void AddChild(wpi::Sendable* child);
 
   /**
-   * Associate a {@link Sendable} with this Subsystem.
+   * Associate a Sendable with this Subsystem.
    *
    * @param child sendable
    */
-  void AddChild(Sendable& child);
+  void AddChild(wpi::Sendable& child);
 
  private:
   /**
@@ -193,7 +185,7 @@
   bool m_initializedDefaultCommand = false;
 
  public:
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
index 16f3348..17eb4f4 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/TimedCommand.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
+
+#include <units/time.h>
 
 #include "frc/commands/Command.h"
 
@@ -24,35 +23,36 @@
    * Creates a new TimedCommand with the given name and timeout.
    *
    * @param name    the name of the command
-   * @param timeout the time (in seconds) before this command "times out"
+   * @param timeout the time before this command "times out"
    */
-  TimedCommand(const wpi::Twine& name, double timeout);
+  TimedCommand(std::string_view name, units::second_t timeout);
 
   /**
    * Creates a new WaitCommand with the given timeout.
    *
-   * @param timeout the time (in seconds) before this command "times out"
+   * @param timeout the time before this command "times out"
    */
-  explicit TimedCommand(double timeout);
+  explicit TimedCommand(units::second_t timeout);
 
   /**
    * Creates a new TimedCommand with the given name and timeout.
    *
    * @param name      the name of the command
-   * @param timeout   the time (in seconds) before this command "times out"
+   * @param timeout   the time before this command "times out"
    * @param subsystem the subsystem that the command requires
    */
-  TimedCommand(const wpi::Twine& name, double timeout, Subsystem& subsystem);
+  TimedCommand(std::string_view name, units::second_t timeout,
+               Subsystem& subsystem);
 
   /**
    * Creates a new WaitCommand with the given timeout.
    *
-   * @param timeout   the time (in seconds) before this command "times out"
+   * @param timeout   the time before this command "times out"
    * @param subsystem the subsystem that the command requires
    */
-  TimedCommand(double timeout, Subsystem& subsystem);
+  TimedCommand(units::second_t timeout, Subsystem& subsystem);
 
-  virtual ~TimedCommand() = default;
+  ~TimedCommand() override = default;
 
   TimedCommand(TimedCommand&&) = default;
   TimedCommand& operator=(TimedCommand&&) = default;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
index 794b0f7..3902fb6 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitCommand.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
+
+#include <units/time.h>
 
 #include "frc/commands/TimedCommand.h"
 
@@ -18,19 +17,19 @@
   /**
    * Creates a new WaitCommand with the given name and timeout.
    *
-   * @param name    the name of the command
-   * @param timeout the time (in seconds) before this command "times out"
+   * @param timeout the time before this command "times out"
    */
-  explicit WaitCommand(double timeout);
+  explicit WaitCommand(units::second_t timeout);
 
   /**
    * Creates a new WaitCommand with the given timeout.
    *
-   * @param timeout the time (in seconds) before this command "times out"
+   * @param name    the name of the command
+   * @param timeout the time before this command "times out"
    */
-  WaitCommand(const wpi::Twine& name, double timeout);
+  WaitCommand(std::string_view name, units::second_t timeout);
 
-  virtual ~WaitCommand() = default;
+  ~WaitCommand() override = default;
 
   WaitCommand(WaitCommand&&) = default;
   WaitCommand& operator=(WaitCommand&&) = default;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
index cd1f85a..b92d78d 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitForChildren.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
+
+#include <units/time.h>
 
 #include "frc/commands/Command.h"
 
@@ -15,15 +14,15 @@
 
 class WaitForChildren : public Command {
  public:
-  explicit WaitForChildren(double timeout);
-  WaitForChildren(const wpi::Twine& name, double timeout);
-  virtual ~WaitForChildren() = default;
+  explicit WaitForChildren(units::second_t timeout);
+  WaitForChildren(std::string_view name, units::second_t timeout);
+  ~WaitForChildren() override = default;
 
   WaitForChildren(WaitForChildren&&) = default;
   WaitForChildren& operator=(WaitForChildren&&) = default;
 
  protected:
-  virtual bool IsFinished();
+  bool IsFinished() override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
index b2f1ffe..715e7bb 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
+++ b/third_party/allwpilib/wpilibOldCommands/src/main/native/include/frc/commands/WaitUntilCommand.h
@@ -1,13 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
+
+#include <units/time.h>
 
 #include "frc/commands/Command.h"
 
@@ -23,11 +22,11 @@
    *
    * @see CommandGroup
    */
-  explicit WaitUntilCommand(double time);
+  explicit WaitUntilCommand(units::second_t time);
 
-  WaitUntilCommand(const wpi::Twine& name, double time);
+  WaitUntilCommand(std::string_view name, units::second_t time);
 
-  virtual ~WaitUntilCommand() = default;
+  ~WaitUntilCommand() override = default;
 
   WaitUntilCommand(WaitUntilCommand&&) = default;
   WaitUntilCommand& operator=(WaitUntilCommand&&) = default;
@@ -36,10 +35,10 @@
   /**
    * Check if we've reached the actual finish time.
    */
-  virtual bool IsFinished();
+  bool IsFinished() override;
 
  private:
-  double m_time;
+  units::second_t m_time;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
index fe7429b..e70f182 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import org.junit.jupiter.api.extension.BeforeAllCallback;
 import org.junit.jupiter.api.extension.ExtensionContext;
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-
 public final class MockHardwareExtension implements BeforeAllCallback {
   private static ExtensionContext getRoot(ExtensionContext context) {
     return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
@@ -21,20 +17,22 @@
 
   @Override
   public void beforeAll(ExtensionContext context) {
-    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initialized", key -> {
-      initializeHardware();
-      return true;
-    }, Boolean.class);
+    getRoot(context)
+        .getStore(Namespace.GLOBAL)
+        .getOrComputeIfAbsent(
+            "HAL Initialized",
+            key -> {
+              initializeHardware();
+              return true;
+            },
+            Boolean.class);
   }
 
   private void initializeHardware() {
     HAL.initialize(500, 0);
-    DriverStationSim dsSim = new DriverStationSim();
-    dsSim.setDsAttached(true);
-    dsSim.setAutonomous(false);
-    dsSim.setEnabled(true);
-    dsSim.setTest(true);
-
-
+    DriverStationSim.setDsAttached(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setTest(true);
   }
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
index 409a74f..d305595 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/AbstractCommandTest.java
@@ -1,21 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
-import org.junit.jupiter.api.BeforeEach;
-
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
-/**
- * The basic test for all {@link Command} tests.
- */
+import org.junit.jupiter.api.BeforeEach;
+
+/** The basic test for all {@link Command} tests. */
 public abstract class AbstractCommandTest {
   @BeforeEach
   void commandSetup() {
@@ -23,7 +18,7 @@
     Scheduler.getInstance().enable();
   }
 
-  public class ASubsystem extends Subsystem {
+  public static class ASubsystem extends Subsystem {
     Command m_command;
 
     @Override
@@ -38,16 +33,14 @@
     }
   }
 
-
-  protected void assertCommandState(MockCommand command, int initialize, int execute,
-                                    int isFinished, int end, int interrupted) {
+  protected void assertCommandState(
+      MockCommand command, int initialize, int execute, int isFinished, int end, int interrupted) {
     assertAll(
         () -> assertEquals(initialize, command.getInitializeCount()),
         () -> assertEquals(execute, command.getExecuteCount()),
         () -> assertEquals(isFinished, command.getIsFinishedCount()),
         () -> assertEquals(end, command.getEndCount()),
-        () -> assertEquals(interrupted, command.getInterruptedCount())
-    );
+        () -> assertEquals(interrupted, command.getInterruptedCount()));
   }
 
   protected void sleep(int time) {
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
index 690eade..0569268 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ButtonTest.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
+import edu.wpi.first.wpilibj.buttons.InternalButton;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 
-import edu.wpi.first.wpilibj.buttons.InternalButton;
-
-
 /**
  * Test that covers the {@link edu.wpi.first.wpilibj.buttons.Button} with the {@link Command}
  * library.
@@ -27,9 +22,7 @@
     m_button2 = new InternalButton();
   }
 
-  /**
-   * Simple Button Test.
-   */
+  /** Simple Button Test. */
   @Test
   void buttonTest() {
     final MockCommand command1 = new MockCommand();
@@ -112,5 +105,4 @@
     assertCommandState(command3, 1, 4, 4, 0, 1);
     assertCommandState(command4, 1, 3, 3, 0, 1);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
index 108c49b..04a920c 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandParallelGroupTest.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 import org.junit.jupiter.api.Test;
 
-/**
- * Ported from the old CrioTest Classes.
- */
+/** Ported from the old CrioTest Classes. */
 class CommandParallelGroupTest extends AbstractCommandTest {
-  /**
-   * Simple Parallel Command Group With 2 commands one command terminates first.
-   */
+  /** Simple Parallel Command Group With 2 commands one command terminates first. */
   @Test
   void parallelCommandGroupWithTwoCommandsTest() {
     final MockCommand command1 = new MockCommand();
@@ -52,5 +45,4 @@
       assertCommandState(command2, 1, 5, 5, 1, 0);
     }
   }
-
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
index 5711032..5592045 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandScheduleTest.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 import org.junit.jupiter.api.Test;
 
-/**
- * Ported from the old CrioTest Classes.
- */
+/** Ported from the old CrioTest Classes. */
 class CommandScheduleTest extends AbstractCommandTest {
   /**
    * Simple scheduling of a command and making sure the command is run and successfully terminates.
@@ -35,9 +30,7 @@
     assertCommandState(command, 1, 3, 3, 1, 0);
   }
 
-  /**
-   * Simple scheduling of a command and making sure the command is run and cancels correctly.
-   */
+  /** Simple scheduling of a command and making sure the command is run and cancels correctly. */
   @Test
   void runAndCancelTest() {
     final MockCommand command = new MockCommand();
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
index cc2b76c..3a299ef 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSequentialGroupTest.java
@@ -1,19 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 import java.util.logging.Logger;
-
 import org.junit.jupiter.api.Test;
 
-/**
- * Ported from the old CrioTest Classes.
- */
+/** Ported from the old CrioTest Classes. */
 class CommandSequentialGroupTest extends AbstractCommandTest {
   private static final Logger logger = Logger.getLogger(CommandSequentialGroupTest.class.getName());
 
@@ -39,7 +33,6 @@
       commandGroup.addSequential(command2, 2.0);
       commandGroup.addSequential(command3);
 
-
       assertCommandState(command1, 0, 0, 0, 0, 0);
       assertCommandState(command2, 0, 0, 0, 0, 0);
       assertCommandState(command3, 0, 0, 0, 0, 0);
@@ -90,5 +83,4 @@
       assertCommandState(command3, 1, 3, 3, 1, 0);
     }
   }
-
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
index 91ec0de..85f58ea 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandSupersedeTest.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 import org.junit.jupiter.api.Test;
 
-/**
- * Ported from the old CrioTest Classes.
- */
+/** Ported from the old CrioTest Classes. */
 class CommandSupersedeTest extends AbstractCommandTest {
-  /**
-   * Testing one command superseding another because of dependencies.
-   */
+  /** Testing one command superseding another because of dependencies. */
   @Test
   void oneCommandSupersedingAnotherBecauseOfDependenciesTest() {
     final ASubsystem subsystem = new ASubsystem();
@@ -67,11 +60,12 @@
   void commandFailingSupersedingBecauseFirstCanNotBeInterruptedTest() {
     final ASubsystem subsystem = new ASubsystem();
 
-    final MockCommand command1 = new MockCommand(subsystem) {
-      {
-        setInterruptible(false);
-      }
-    };
+    final MockCommand command1 =
+        new MockCommand(subsystem) {
+          {
+            setInterruptible(false);
+          }
+        };
 
     final MockCommand command2 = new MockCommand(subsystem);
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
index 229742c..069e099 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/CommandTimeoutTest.java
@@ -1,32 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 import org.junit.jupiter.api.Test;
 
-/**
- * Test a {@link Command} that times out.
- */
+/** Test a {@link Command} that times out. */
 class CommandTimeoutTest extends AbstractCommandTest {
-  /**
-   * Command 2 second Timeout Test.
-   */
+  /** Command 2 second Timeout Test. */
   @Test
   void twoSecondTimeoutTest() {
     final ASubsystem subsystem = new ASubsystem();
 
-
-    final MockCommand command = new MockCommand(subsystem, 2) {
-      @Override
-      public boolean isFinished() {
-        return super.isFinished() || isTimedOut();
-      }
-    };
+    final MockCommand command =
+        new MockCommand(subsystem, 2) {
+          @Override
+          public boolean isFinished() {
+            return super.isFinished() || isTimedOut();
+          }
+        };
 
     command.start();
     assertCommandState(command, 0, 0, 0, 0, 0);
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
index 76ec10b..97d3377 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/ConditionalCommandTest.java
@@ -1,26 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertSame;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
 class ConditionalCommandTest extends AbstractCommandTest {
   MockConditionalCommand m_command;
   MockConditionalCommand m_commandNull;
   MockCommand m_onTrue;
   MockCommand m_onFalse;
   MockSubsystem m_subsys;
-  Boolean m_condition;
 
   @BeforeEach
   void initCommands() {
@@ -31,9 +27,13 @@
     m_commandNull = new MockConditionalCommand(m_onTrue, null);
   }
 
-  protected void assertConditionalCommandState(MockConditionalCommand command, int initialize,
-                                               int execute, int isFinished, int end,
-                                               int interrupted) {
+  protected void assertConditionalCommandState(
+      MockConditionalCommand command,
+      int initialize,
+      int execute,
+      int isFinished,
+      int end,
+      int interrupted) {
     assertEquals(initialize, command.getInitializeCount());
     assertEquals(execute, command.getExecuteCount());
     assertEquals(isFinished, command.getIsFinishedCount());
@@ -49,11 +49,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -87,11 +87,11 @@
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onFalse
+    Scheduler.getInstance().run(); // init command and select m_onFalse
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onFalse
+    Scheduler.getInstance().run(); // init m_onFalse
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -125,11 +125,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -164,11 +164,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -199,11 +199,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -234,11 +234,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -267,11 +267,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -303,11 +303,11 @@
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onTrue
+    Scheduler.getInstance().run(); // init command and select m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onTrue
+    Scheduler.getInstance().run(); // init m_onTrue
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertCommandState(m_onFalse, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_command, 1, 1, 1, 0, 0);
@@ -332,10 +332,10 @@
     Scheduler.getInstance().add(m_commandNull);
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init command and select m_onFalse
+    Scheduler.getInstance().run(); // init command and select m_onFalse
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_commandNull, 0, 0, 0, 0, 0);
-    Scheduler.getInstance().run();  // init m_onFalse
+    Scheduler.getInstance().run(); // init m_onFalse
     assertCommandState(m_onTrue, 0, 0, 0, 0, 0);
     assertConditionalCommandState(m_commandNull, 1, 1, 1, 1, 0);
     Scheduler.getInstance().run();
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
index 3ec2148..698c0e6 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/DefaultCommandTest.java
@@ -1,26 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
 import org.junit.jupiter.api.Test;
 
-/**
- * Tests the {@link Command} library.
- */
+/** Tests the {@link Command} library. */
 public class DefaultCommandTest extends AbstractCommandTest {
-  /**
-   * Testing of default commands where the interrupting command ends itself.
-   */
+  /** Testing of default commands where the interrupting command ends itself. */
   @Test
   void defaultCommandWhereTheInteruptingCommandEndsItselfTest() {
     final ASubsystem subsystem = new ASubsystem();
 
-
     final MockCommand defaultCommand = new MockCommand(subsystem);
 
     final MockCommand anotherCommand = new MockCommand(subsystem);
@@ -61,10 +53,7 @@
     assertCommandState(anotherCommand, 1, 3, 3, 1, 0);
   }
 
-
-  /**
-   * Testing of default commands where the interrupting command is canceled.
-   */
+  /** Testing of default commands where the interrupting command is canceled. */
   @Test
   void defaultCommandsInterruptingCommandCanceledTest() {
     final ASubsystem subsystem = new ASubsystem();
@@ -107,5 +96,4 @@
     assertCommandState(defaultCommand, 2, 5, 5, 0, 1);
     assertCommandState(anotherCommand, 1, 2, 2, 0, 1);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
index 82fe3e2..5d1715b 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -59,31 +56,22 @@
     ++m_interruptedCount;
   }
 
-
-  /**
-   * How many times the initialize method has been called.
-   */
+  /** How many times the initialize method has been called. */
   public int getInitializeCount() {
     return m_initializeCount;
   }
 
-  /**
-   * If the initialize method has been called at least once.
-   */
+  /** If the initialize method has been called at least once. */
   public boolean hasInitialized() {
     return getInitializeCount() > 0;
   }
 
-  /**
-   * How many time the execute method has been called.
-   */
+  /** How many time the execute method has been called. */
   public int getExecuteCount() {
     return m_executeCount;
   }
 
-  /**
-   * How many times the isFinished method has been called.
-   */
+  /** How many times the isFinished method has been called. */
   public int getIsFinishedCount() {
     return m_isFinishedCount;
   }
@@ -106,37 +94,27 @@
     m_hasFinished = hasFinished;
   }
 
-  /**
-   * How many times the end method has been called.
-   */
+  /** How many times the end method has been called. */
   public int getEndCount() {
     return m_endCount;
   }
 
-  /**
-   * If the end method has been called at least once.
-   */
+  /** If the end method has been called at least once. */
   public boolean hasEnd() {
     return getEndCount() > 0;
   }
 
-  /**
-   * How many times the interrupted method has been called.
-   */
+  /** How many times the interrupted method has been called. */
   public int getInterruptedCount() {
     return m_interruptedCount;
   }
 
-  /**
-   * If the interrupted method has been called at least once.
-   */
+  /** If the interrupted method has been called at least once. */
   public boolean hasInterrupted() {
     return getInterruptedCount() > 0;
   }
 
-  /**
-   * Reset internal counters.
-   */
+  /** Reset internal counters. */
   public void resetCounters() {
     m_initializeCount = 0;
     m_executeCount = 0;
@@ -145,5 +123,4 @@
     m_endCount = 0;
     m_interruptedCount = 0;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
index 688afa3..8ac2aee 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockConditionalCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
@@ -54,66 +51,47 @@
     ++m_interruptedCount;
   }
 
-
-  /**
-   * How many times the initialize method has been called.
-   */
+  /** How many times the initialize method has been called. */
   public int getInitializeCount() {
     return m_initializeCount;
   }
 
-  /**
-   * If the initialize method has been called at least once.
-   */
+  /** If the initialize method has been called at least once. */
   public boolean hasInitialized() {
     return getInitializeCount() > 0;
   }
 
-  /**
-   * How many time the execute method has been called.
-   */
+  /** How many time the execute method has been called. */
   public int getExecuteCount() {
     return m_executeCount;
   }
 
-  /**
-   * How many times the isFinished method has been called.
-   */
+  /** How many times the isFinished method has been called. */
   public int getIsFinishedCount() {
     return m_isFinishedCount;
   }
 
-  /**
-   * How many times the end method has been called.
-   */
+  /** How many times the end method has been called. */
   public int getEndCount() {
     return m_endCount;
   }
 
-  /**
-   * If the end method has been called at least once.
-   */
+  /** If the end method has been called at least once. */
   public boolean hasEnd() {
     return getEndCount() > 0;
   }
 
-  /**
-   * How many times the interrupted method has been called.
-   */
+  /** How many times the interrupted method has been called. */
   public int getInterruptedCount() {
     return m_interruptedCount;
   }
 
-  /**
-   * If the interrupted method has been called at least once.
-   */
+  /** If the interrupted method has been called at least once. */
   public boolean hasInterrupted() {
     return getInterruptedCount() > 0;
   }
 
-  /**
-   * Reset internal counters.
-   */
+  /** Reset internal counters. */
   public void resetCounters() {
     m_condition = false;
     m_initializeCount = 0;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
index 604df89..e2b1bf7 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/command/MockSubsystem.java
@@ -1,15 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.command;
 
-/**
- * A class to simulate a simple subsystem.
- */
+/** A class to simulate a simple subsystem. */
 public class MockSubsystem extends Subsystem {
   @Override
   protected void initDefaultCommand() {}
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
index 8e304eb..6236e62 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTabTest.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import java.util.HashMap;
-import java.util.Map;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.command.InstantCommand;
-
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertArrayEquals;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.wpilibj.command.InstantCommand;
+import java.util.HashMap;
+import java.util.Map;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
 public class ShuffleboardTabTest {
   private NetworkTableInstance m_ntInstance;
   private ShuffleboardTab m_tab;
@@ -66,7 +61,6 @@
         () -> assertEquals(1.0, entry.getValue().getDouble()));
   }
 
-
   @Test
   void testAddBoolean() {
     NetworkTableEntry entry = m_tab.add("Bool", false).getEntry();
@@ -87,47 +81,51 @@
   void testAddNamedSendableWithProperties() {
     Sendable sendable = new InstantCommand("Command");
     String widgetType = "Command Widget";
-    m_tab.add(sendable)
-       .withWidget(widgetType)
-       .withProperties(mapOf("foo", 1234, "bar", "baz"));
+    m_tab.add(sendable).withWidget(widgetType).withProperties(mapOf("foo", 1234, "bar", "baz"));
 
     m_instance.update();
     String meta = "/Shuffleboard/.metadata/Tab/Command";
 
     assertAll(
-        () -> assertEquals(1234,
-                           m_ntInstance.getEntry(meta + "/Properties/foo").getDouble(-1),
-                           "Property 'foo' not set correctly"),
-        () -> assertEquals("baz",
-                           m_ntInstance.getEntry(meta + "/Properties/bar").getString(null),
-                           "Property 'bar' not set correctly"),
-        () -> assertEquals(widgetType,
-                           m_ntInstance.getEntry(meta + "/PreferredComponent").getString(null),
-                           "Preferred component not set correctly"));
+        () ->
+            assertEquals(
+                1234,
+                m_ntInstance.getEntry(meta + "/Properties/foo").getDouble(-1),
+                "Property 'foo' not set correctly"),
+        () ->
+            assertEquals(
+                "baz",
+                m_ntInstance.getEntry(meta + "/Properties/bar").getString(null),
+                "Property 'bar' not set correctly"),
+        () ->
+            assertEquals(
+                widgetType,
+                m_ntInstance.getEntry(meta + "/PreferredComponent").getString(null),
+                "Preferred component not set correctly"));
   }
 
   @Test
   void testAddNumberArray() {
-    NetworkTableEntry entry = m_tab.add("DoubleArray", new double[]{1, 2, 3}).getEntry();
+    NetworkTableEntry entry = m_tab.add("DoubleArray", new double[] {1, 2, 3}).getEntry();
     assertAll(
         () -> assertEquals("/Shuffleboard/Tab/DoubleArray", entry.getName()),
-        () -> assertArrayEquals(new double[]{1, 2, 3}, entry.getValue().getDoubleArray()));
+        () -> assertArrayEquals(new double[] {1, 2, 3}, entry.getValue().getDoubleArray()));
   }
 
   @Test
   void testAddBooleanArray() {
-    NetworkTableEntry entry = m_tab.add("BoolArray", new boolean[]{true, false}).getEntry();
+    NetworkTableEntry entry = m_tab.add("BoolArray", new boolean[] {true, false}).getEntry();
     assertAll(
         () -> assertEquals("/Shuffleboard/Tab/BoolArray", entry.getName()),
-        () -> assertArrayEquals(new boolean[]{true, false}, entry.getValue().getBooleanArray()));
+        () -> assertArrayEquals(new boolean[] {true, false}, entry.getValue().getBooleanArray()));
   }
 
   @Test
   void testAddStringArray() {
-    NetworkTableEntry entry = m_tab.add("StringArray", new String[]{"foo", "bar"}).getEntry();
+    NetworkTableEntry entry = m_tab.add("StringArray", new String[] {"foo", "bar"}).getEntry();
     assertAll(
         () -> assertEquals("/Shuffleboard/Tab/StringArray", entry.getName()),
-        () -> assertArrayEquals(new String[]{"foo", "bar"}, entry.getValue().getStringArray()));
+        () -> assertArrayEquals(new String[] {"foo", "bar"}, entry.getValue().getStringArray()));
   }
 
   @Test
@@ -136,10 +134,8 @@
     assertThrows(IllegalArgumentException.class, () -> m_tab.add("foo", "baz"));
   }
 
-  /**
-   * Stub for Java 9 {@code Map.of()}.
-   */
-  @SuppressWarnings({"unchecked", "PMD"})
+  /** Stub for Java 9 {@code Map.of()}. */
+  @SuppressWarnings("unchecked")
   private static <K, V> Map<K, V> mapOf(Object... entries) {
     Map<K, V> map = new HashMap<>();
     for (int i = 0; i < entries.length; i += 2) {
@@ -147,5 +143,4 @@
     }
     return map;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/main.cpp b/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/main.cpp
index c6b6c58..6aea19a 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <hal/HALBase.h>
 
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp b/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
index e0e3db0..4cbad01 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <array>
 #include <memory>
@@ -15,7 +12,6 @@
 #include "frc/commands/InstantCommand.h"
 #include "frc/shuffleboard/ShuffleboardInstance.h"
 #include "frc/shuffleboard/ShuffleboardTab.h"
-#include "frc/smartdashboard/Sendable.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
diff --git a/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp b/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
index 0b06d7f..9460330 100644
--- a/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
+++ b/third_party/allwpilib/wpilibOldCommands/src/test/native/cpp/shuffleboard/ShuffleboardWidgetTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <array>
 #include <memory>
@@ -17,7 +14,6 @@
 #include "frc/shuffleboard/ShuffleboardInstance.h"
 #include "frc/shuffleboard/ShuffleboardTab.h"
 #include "frc/shuffleboard/ShuffleboardWidget.h"
-#include "frc/smartdashboard/Sendable.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
diff --git a/third_party/allwpilib/wpilibOldCommands/wpilibOldCommands-config.cmake.in b/third_party/allwpilib/wpilibOldCommands/wpilibOldCommands-config.cmake.in
new file mode 100644
index 0000000..81dde79
--- /dev/null
+++ b/third_party/allwpilib/wpilibOldCommands/wpilibOldCommands-config.cmake.in
@@ -0,0 +1,11 @@
+include(CMakeFindDependencyMacro)
+@FILENAME_DEP_REPLACE@
+@WPIUTIL_DEP_REPLACE@
+@NTCORE_DEP_REPLACE@
+@CSCORE_DEP_REPLACE@
+@CAMERASERVER_DEP_REPLACE@
+@HAL_DEP_REPLACE@
+@WPILIBC_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
+
+include(${SELF_DIR}/wpilibOldCommands.cmake)
diff --git a/third_party/allwpilib/wpilibc/CMakeLists.txt b/third_party/allwpilib/wpilibc/CMakeLists.txt
index 6435040..4c8dfde 100644
--- a/third_party/allwpilib/wpilibc/CMakeLists.txt
+++ b/third_party/allwpilib/wpilibc/CMakeLists.txt
@@ -3,8 +3,6 @@
 include(CompileWarnings)
 include(AddTest)
 
-find_package( OpenCV REQUIRED )
-
 configure_file(src/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
 
 file(GLOB_RECURSE
@@ -17,7 +15,18 @@
                 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/include>
                             $<INSTALL_INTERFACE:${include_dest}/wpilibc>)
 wpilib_target_warnings(wpilibc)
-target_link_libraries(wpilibc PUBLIC cameraserver hal ntcore cscore wpimath wpiutil ${OpenCV_LIBS})
+
+if (WITH_CSCORE)
+    find_package( OpenCV )
+    target_link_libraries(wpilibc PUBLIC cameraserver cscore ${OpenCV_LIBS})
+else()
+    target_compile_definitions(wpilibc PRIVATE DYNAMIC_CAMERA_SERVER)
+    # Add just the camera server include directory
+    target_include_directories(wpilibc PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../cameraserver/src/main/native/include)
+endif()
+
+target_link_libraries(wpilibc PUBLIC hal ntcore wpimath wpiutil)
+
 
 set_property(TARGET wpilibc PROPERTY FOLDER "libraries")
 
@@ -30,8 +39,8 @@
     set (wpilibc_config_dir share/wpilibc)
 endif()
 
-configure_file(wpilibc-config.cmake.in ${CMAKE_BINARY_DIR}/wpilibc-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/wpilibc-config.cmake DESTINATION ${wpilibc_config_dir})
+configure_file(wpilibc-config.cmake.in ${WPILIB_BINARY_DIR}/wpilibc-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/wpilibc-config.cmake DESTINATION ${wpilibc_config_dir})
 install(EXPORT wpilibc DESTINATION ${wpilibc_config_dir})
 
 if (WITH_TESTS)
@@ -43,4 +52,9 @@
     else()
         target_compile_options(wpilibc_test PRIVATE /WX-)
     endif()
+    if (NOT WITH_CSCORE)
+        target_compile_definitions(wpilibc_test PRIVATE DYNAMIC_CAMERA_SERVER)
+        # Add just the camera server include directory
+        target_include_directories(wpilibc_test PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../cameraserver/src/main/native/include)
+    endif()
 endif()
diff --git a/third_party/allwpilib/wpilibc/build.gradle b/third_party/allwpilib/wpilibc/build.gradle
index ec85691..c9c2670 100644
--- a/third_party/allwpilib/wpilibc/build.gradle
+++ b/third_party/allwpilib/wpilibc/build.gradle
@@ -60,18 +60,34 @@
 
 apply from: "${rootDir}/shared/googletest.gradle"
 
-apply plugin: DisableBuildingGTest
-
 nativeUtils.exportsConfigs {
     wpilibc {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
 }
 
@@ -81,7 +97,10 @@
             sources {
                 cpp {
                     source {
-                        srcDirs = ['src/main/native/cpp', "$buildDir/generated/cpp"]
+                        srcDirs = [
+                            'src/main/native/cpp',
+                            "$buildDir/generated/cpp"
+                        ]
                         include '**/*.cpp'
                     }
                     exportedHeaders {
@@ -158,7 +177,7 @@
                 lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                 nativeUtils.useRequiredLibrary(it, 'opencv_shared')
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                 }
             }
         }
@@ -209,7 +228,7 @@
             nativeUtils.useRequiredLibrary(it, 'opencv_shared')
             lib library: nativeName, linkage: 'shared'
             if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
             }
         }
     }
@@ -224,7 +243,7 @@
                             def arch = it.targetPlatform.architecture.name
                             if (arch == 'x86-64' || arch == 'x86') {
                                 dependsOn it.tasks.install
-
+                                commandLine it.tasks.install.runScriptFile.get().asFile.toString()
                                 found = true
                             }
                         }
diff --git a/third_party/allwpilib/wpilibc/publish.gradle b/third_party/allwpilib/wpilibc/publish.gradle
index 025c09d..d49f157 100644
--- a/third_party/allwpilib/wpilibc/publish.gradle
+++ b/third_party/allwpilib/wpilibc/publish.gradle
@@ -34,8 +34,14 @@
         into '/'
     }
 
-    from('src/main/native/include') {
-        into '/'
+    ext.includeDirs = [
+        project.file('src/main/native/include')
+    ]
+
+    ext.includeDirs.each {
+        from(it) {
+            into '/'
+        }
     }
 }
 
diff --git a/third_party/allwpilib/wpilibc/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpilibc/src/dev/native/cpp/main.cpp
index f1370bb..28fac9d 100644
--- a/third_party/allwpilib/wpilibc/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibc/src/dev/native/cpp/main.cpp
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
-
+#include <fmt/core.h>
 #include <hal/HALBase.h>
 
 #include "WPILibVersion.h"
 
 int main() {
-  std::cout << "Hello World" << std::endl;
-  std::cout << HAL_GetRuntimeType() << std::endl;
-  std::cout << GetWPILibVersion() << std::endl;
-  return 0;
+  fmt::print("Hello World\n");
+  fmt::print("{}\n", HAL_GetRuntimeType());
+  fmt::print("{}\n", GetWPILibVersion());
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
index aff0802..484e690 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/ADXL345_I2C.h"
 
 #include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 using namespace frc;
 
 ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
     : m_i2c(port, deviceAddress),
-      m_simDevice("ADXL345_I2C", port, deviceAddress) {
+      m_simDevice("Accel:ADXL345_I2C", port, deviceAddress) {
   if (m_simDevice) {
-    m_simRange =
-        m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
-    m_simX = m_simDevice.CreateDouble("X 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_simRange = m_simDevice.CreateEnumDouble("range", hal::SimDevice::kOutput,
+                                              {"2G", "4G", "8G", "16G"},
+                                              {2.0, 4.0, 8.0, 16.0}, 0);
+    m_simX = m_simDevice.CreateDouble("x", hal::SimDevice::kInput, 0.0);
+    m_simY = m_simDevice.CreateDouble("y", hal::SimDevice::kInput, 0.0);
+    m_simZ = m_simDevice.CreateDouble("z", hal::SimDevice::kInput, 0.0);
   }
   // Turn on the measurements
   m_i2c.Write(kPowerCtlRegister, kPowerCtl_Measure);
@@ -32,7 +29,15 @@
   HAL_Report(HALUsageReporting::kResourceType_ADXL345,
              HALUsageReporting::kADXL345_I2C, 0);
 
-  SendableRegistry::GetInstance().AddLW(this, "ADXL345_I2C", port);
+  wpi::SendableRegistry::AddLW(this, "ADXL345_I2C", port);
+}
+
+I2C::Port ADXL345_I2C::GetI2CPort() const {
+  return m_i2c.GetPort();
+}
+
+int ADXL345_I2C::GetI2CDeviceAddress() const {
+  return m_i2c.GetDeviceAddress();
 }
 
 void ADXL345_I2C::SetRange(Range range) {
@@ -40,16 +45,28 @@
               kDataFormat_FullRes | static_cast<uint8_t>(range));
 }
 
-double ADXL345_I2C::GetX() { return GetAcceleration(kAxis_X); }
+double ADXL345_I2C::GetX() {
+  return GetAcceleration(kAxis_X);
+}
 
-double ADXL345_I2C::GetY() { return GetAcceleration(kAxis_Y); }
+double ADXL345_I2C::GetY() {
+  return GetAcceleration(kAxis_Y);
+}
 
-double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
+double ADXL345_I2C::GetZ() {
+  return GetAcceleration(kAxis_Z);
+}
 
 double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
-  if (axis == kAxis_X && m_simX) return m_simX.Get();
-  if (axis == kAxis_Y && m_simY) return m_simY.Get();
-  if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+  if (axis == kAxis_X && m_simX) {
+    return m_simX.Get();
+  }
+  if (axis == kAxis_Y && m_simY) {
+    return m_simY.Get();
+  }
+  if (axis == kAxis_Z && m_simZ) {
+    return m_simZ.Get();
+  }
   int16_t rawAccel = 0;
   m_i2c.Read(kDataRegister + static_cast<int>(axis), sizeof(rawAccel),
              reinterpret_cast<uint8_t*>(&rawAccel));
@@ -74,12 +91,12 @@
   return data;
 }
 
-void ADXL345_I2C::InitSendable(SendableBuilder& builder) {
+void ADXL345_I2C::InitSendable(nt::NTSendableBuilder& builder) {
   builder.SetSmartDashboardType("3AxisAccelerometer");
   auto x = builder.GetEntry("X").GetHandle();
   auto y = builder.GetEntry("Y").GetHandle();
   auto z = builder.GetEntry("Z").GetHandle();
-  builder.SetUpdateTable([=]() {
+  builder.SetUpdateTable([=] {
     auto data = GetAccelerations();
     nt::NetworkTableEntry(x).SetDouble(data.XAxis);
     nt::NetworkTableEntry(y).SetDouble(data.YAxis);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
index 329b148..9a95bcc 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/ADXL345_SPI.h"
 
 #include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 using namespace frc;
 
 ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
-    : m_spi(port), m_simDevice("ADXL345_SPI", port) {
+    : m_spi(port), m_simDevice("Accel:ADXL345_SPI", port) {
   if (m_simDevice) {
-    m_simRange =
-        m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
-    m_simX = m_simDevice.CreateDouble("X 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_simRange = m_simDevice.CreateEnumDouble("range", hal::SimDevice::kOutput,
+                                              {"2G", "4G", "8G", "16G"},
+                                              {2.0, 4.0, 8.0, 16.0}, 0);
+    m_simX = m_simDevice.CreateDouble("x", hal::SimDevice::kInput, 0.0);
+    m_simY = m_simDevice.CreateDouble("y", hal::SimDevice::kInput, 0.0);
+    m_simZ = m_simDevice.CreateDouble("z", hal::SimDevice::kInput, 0.0);
   }
   m_spi.SetClockRate(500000);
   m_spi.SetMSBFirst();
@@ -40,7 +37,11 @@
   HAL_Report(HALUsageReporting::kResourceType_ADXL345,
              HALUsageReporting::kADXL345_SPI);
 
-  SendableRegistry::GetInstance().AddLW(this, "ADXL345_SPI", port);
+  wpi::SendableRegistry::AddLW(this, "ADXL345_SPI", port);
+}
+
+SPI::Port ADXL345_SPI::GetSpiPort() const {
+  return m_spi.GetPort();
 }
 
 void ADXL345_SPI::SetRange(Range range) {
@@ -51,19 +52,33 @@
   commands[1] = kDataFormat_FullRes | static_cast<uint8_t>(range & 0x03);
   m_spi.Transaction(commands, commands, 2);
 
-  if (m_simRange) m_simRange.Set(range);
+  if (m_simRange) {
+    m_simRange.Set(range);
+  }
 }
 
-double ADXL345_SPI::GetX() { return GetAcceleration(kAxis_X); }
+double ADXL345_SPI::GetX() {
+  return GetAcceleration(kAxis_X);
+}
 
-double ADXL345_SPI::GetY() { return GetAcceleration(kAxis_Y); }
+double ADXL345_SPI::GetY() {
+  return GetAcceleration(kAxis_Y);
+}
 
-double ADXL345_SPI::GetZ() { return GetAcceleration(kAxis_Z); }
+double ADXL345_SPI::GetZ() {
+  return GetAcceleration(kAxis_Z);
+}
 
 double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
-  if (axis == kAxis_X && m_simX) return m_simX.Get();
-  if (axis == kAxis_Y && m_simY) return m_simY.Get();
-  if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+  if (axis == kAxis_X && m_simX) {
+    return m_simX.Get();
+  }
+  if (axis == kAxis_Y && m_simY) {
+    return m_simY.Get();
+  }
+  if (axis == kAxis_Z && m_simZ) {
+    return m_simZ.Get();
+  }
   uint8_t buffer[3];
   uint8_t command[3] = {0, 0, 0};
   command[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister) +
@@ -103,12 +118,12 @@
   return data;
 }
 
-void ADXL345_SPI::InitSendable(SendableBuilder& builder) {
+void ADXL345_SPI::InitSendable(nt::NTSendableBuilder& builder) {
   builder.SetSmartDashboardType("3AxisAccelerometer");
   auto x = builder.GetEntry("X").GetHandle();
   auto y = builder.GetEntry("Y").GetHandle();
   auto z = builder.GetEntry("Z").GetHandle();
-  builder.SetUpdateTable([=]() {
+  builder.SetUpdateTable([=] {
     auto data = GetAccelerations();
     nt::NetworkTableEntry(x).SetDouble(data.XAxis);
     nt::NetworkTableEntry(y).SetDouble(data.YAxis);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL362.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL362.cpp
index aff572f..deda965 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL362.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXL362.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/ADXL362.h"
 
 #include <hal/FRCUsageReporting.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/DriverStation.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -35,13 +32,14 @@
 ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {}
 
 ADXL362::ADXL362(SPI::Port port, Range range)
-    : m_spi(port), m_simDevice("ADXL362", port) {
+    : m_spi(port), m_simDevice("Accel:ADXL362", port) {
   if (m_simDevice) {
-    m_simRange =
-        m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
-    m_simX = m_simDevice.CreateDouble("X 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_simRange = m_simDevice.CreateEnumDouble("range", hal::SimDevice::kOutput,
+                                              {"2G", "4G", "8G", "16G"},
+                                              {2.0, 4.0, 8.0, 16.0}, 0);
+    m_simX = m_simDevice.CreateDouble("x", hal::SimDevice::kInput, 0.0);
+    m_simY = m_simDevice.CreateDouble("y", hal::SimDevice::kInput, 0.0);
+    m_simZ = m_simDevice.CreateDouble("z", hal::SimDevice::kInput, 0.0);
   }
 
   m_spi.SetClockRate(3000000);
@@ -58,7 +56,7 @@
     commands[2] = 0;
     m_spi.Transaction(commands, commands, 3);
     if (commands[2] != 0xF2) {
-      DriverStation::ReportError("could not find ADXL362");
+      FRC_ReportError(err::Error, "{}", "could not find ADXL362");
       m_gsPerLSB = 0.0;
       return;
     }
@@ -74,11 +72,17 @@
 
   HAL_Report(HALUsageReporting::kResourceType_ADXL362, port + 1);
 
-  SendableRegistry::GetInstance().AddLW(this, "ADXL362", port);
+  wpi::SendableRegistry::AddLW(this, "ADXL362", port);
+}
+
+SPI::Port ADXL362::GetSpiPort() const {
+  return m_spi.GetPort();
 }
 
 void ADXL362::SetRange(Range range) {
-  if (m_gsPerLSB == 0.0) return;
+  if (m_gsPerLSB == 0.0) {
+    return;
+  }
 
   uint8_t commands[3];
 
@@ -102,21 +106,37 @@
       kFilterCtl_ODR_100Hz | static_cast<uint8_t>((range & 0x03) << 6);
   m_spi.Write(commands, 3);
 
-  if (m_simRange) m_simRange.Set(range);
+  if (m_simRange) {
+    m_simRange.Set(range);
+  }
 }
 
-double ADXL362::GetX() { return GetAcceleration(kAxis_X); }
+double ADXL362::GetX() {
+  return GetAcceleration(kAxis_X);
+}
 
-double ADXL362::GetY() { return GetAcceleration(kAxis_Y); }
+double ADXL362::GetY() {
+  return GetAcceleration(kAxis_Y);
+}
 
-double ADXL362::GetZ() { return GetAcceleration(kAxis_Z); }
+double ADXL362::GetZ() {
+  return GetAcceleration(kAxis_Z);
+}
 
 double ADXL362::GetAcceleration(ADXL362::Axes axis) {
-  if (m_gsPerLSB == 0.0) return 0.0;
+  if (m_gsPerLSB == 0.0) {
+    return 0.0;
+  }
 
-  if (axis == kAxis_X && m_simX) return m_simX.Get();
-  if (axis == kAxis_Y && m_simY) return m_simY.Get();
-  if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
+  if (axis == kAxis_X && m_simX) {
+    return m_simX.Get();
+  }
+  if (axis == kAxis_Y && m_simY) {
+    return m_simY.Get();
+  }
+  if (axis == kAxis_Z && m_simZ) {
+    return m_simZ.Get();
+  }
 
   uint8_t buffer[4];
   uint8_t command[4] = {0, 0, 0, 0};
@@ -162,12 +182,12 @@
   return data;
 }
 
-void ADXL362::InitSendable(SendableBuilder& builder) {
+void ADXL362::InitSendable(nt::NTSendableBuilder& builder) {
   builder.SetSmartDashboardType("3AxisAccelerometer");
   auto x = builder.GetEntry("X").GetHandle();
   auto y = builder.GetEntry("Y").GetHandle();
   auto z = builder.GetEntry("Z").GetHandle();
-  builder.SetUpdateTable([=]() {
+  builder.SetUpdateTable([=] {
     auto data = GetAccelerations();
     nt::NetworkTableEntry(x).SetDouble(data.XAxis);
     nt::NetworkTableEntry(y).SetDouble(data.YAxis);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
index 8ea5b41..5a2f625 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
@@ -1,22 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/ADXRS450_Gyro.h"
 
 #include <hal/FRCUsageReporting.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/DriverStation.h"
+#include "frc/Errors.h"
 #include "frc/Timer.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
-static constexpr auto kSamplePeriod = 0.0005_s;
-static constexpr double kCalibrationSampleTime = 5.0;
+static constexpr auto kSamplePeriod = 0.5_ms;
+static constexpr auto kCalibrationSampleTime = 5_s;
 static constexpr double kDegreePerSecondPerLSB = 0.0125;
 
 // static constexpr int kRateRegister = 0x00;
@@ -32,10 +30,11 @@
 ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
 
 ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
-    : m_spi(port), m_port(port), m_simDevice("ADXRS450_Gyro", port) {
+    : m_spi(port), m_port(port), m_simDevice("Gyro:ADXRS450", port) {
   if (m_simDevice) {
-    m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
-    m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
+    m_simAngle =
+        m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0);
+    m_simRate = m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0);
   }
 
   m_spi.SetClockRate(3000000);
@@ -47,7 +46,7 @@
   if (!m_simDevice) {
     // Validate the part ID
     if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
-      DriverStation::ReportError("could not find ADXRS450 gyro");
+      FRC_ReportError(err::Error, "{}", "could not find ADXRS450 gyro");
       return;
     }
 
@@ -59,7 +58,7 @@
 
   HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1);
 
-  SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
+  wpi::SendableRegistry::AddLW(this, "ADXRS450_Gyro", port);
 }
 
 static bool CalcParity(int v) {
@@ -81,7 +80,9 @@
 
 uint16_t ADXRS450_Gyro::ReadRegister(int reg) {
   int cmd = 0x80000000 | static_cast<int>(reg) << 17;
-  if (!CalcParity(cmd)) cmd |= 1u;
+  if (!CalcParity(cmd)) {
+    cmd |= 1u;
+  }
 
   // big endian
   uint8_t buf[4] = {static_cast<uint8_t>((cmd >> 24) & 0xff),
@@ -91,29 +92,36 @@
 
   m_spi.Write(buf, 4);
   m_spi.Read(false, buf, 4);
-  if ((buf[0] & 0xe0) == 0) return 0;  // error, return 0
+  if ((buf[0] & 0xe0) == 0) {
+    return 0;  // error, return 0
+  }
   return static_cast<uint16_t>((BytesToIntBE(buf) >> 5) & 0xffff);
 }
 
 double ADXRS450_Gyro::GetAngle() const {
-  if (m_simAngle) return m_simAngle.Get();
+  if (m_simAngle) {
+    return m_simAngle.Get();
+  }
   return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
 }
 
 double ADXRS450_Gyro::GetRate() const {
-  if (m_simRate) return m_simRate.Get();
+  if (m_simRate) {
+    return m_simRate.Get();
+  }
   return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
          kDegreePerSecondPerLSB;
 }
 
 void ADXRS450_Gyro::Reset() {
-  if (m_simAngle) m_simAngle.Set(0.0);
-  if (m_simRate) m_simRate.Set(0.0);
+  if (m_simAngle) {
+    m_simAngle.Reset();
+  }
   m_spi.ResetAccumulator();
 }
 
 void ADXRS450_Gyro::Calibrate() {
-  Wait(0.1);
+  Wait(100_ms);
 
   m_spi.SetAccumulatorIntegratedCenter(0);
   m_spi.ResetAccumulator();
@@ -124,4 +132,12 @@
   m_spi.ResetAccumulator();
 }
 
-int ADXRS450_Gyro::GetPort() const { return m_port; }
+int ADXRS450_Gyro::GetPort() const {
+  return m_port;
+}
+
+void ADXRS450_Gyro::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Gyro");
+  builder.AddDoubleProperty(
+      "Value", [=] { return GetAngle(); }, nullptr);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AddressableLED.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AddressableLED.cpp
index 81adc7b..759c386 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AddressableLED.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AddressableLED.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AddressableLED.h"
 
@@ -12,22 +9,25 @@
 #include <hal/HALBase.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
+#include <wpi/StackTrace.h>
 
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
-AddressableLED::AddressableLED(int port) {
+AddressableLED::AddressableLED(int port) : m_port{port} {
   int32_t status = 0;
 
-  m_pwmHandle = HAL_InitializePWMPort(HAL_GetPort(port), &status);
-  wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), port);
+  auto stack = wpi::GetStackTrace(1);
+  m_pwmHandle =
+      HAL_InitializePWMPort(HAL_GetPort(port), stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Port {}", port);
   if (m_pwmHandle == HAL_kInvalidHandle) {
     return;
   }
 
   m_handle = HAL_InitializeAddressableLED(m_pwmHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", port);
   if (m_handle == HAL_kInvalidHandle) {
     HAL_FreePWMPort(m_pwmHandle, &status);
   }
@@ -39,29 +39,30 @@
   HAL_FreeAddressableLED(m_handle);
   int32_t status = 0;
   HAL_FreePWMPort(m_pwmHandle, &status);
+  FRC_ReportError(status, "Port {}", m_port);
 }
 
 void AddressableLED::SetLength(int length) {
   int32_t status = 0;
   HAL_SetAddressableLEDLength(m_handle, length, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {} length {}", m_port, length);
 }
 
 static_assert(sizeof(AddressableLED::LEDData) == sizeof(HAL_AddressableLEDData),
               "LED Structs MUST be the same size");
 
-void AddressableLED::SetData(wpi::ArrayRef<LEDData> ledData) {
+void AddressableLED::SetData(wpi::span<const LEDData> ledData) {
   int32_t status = 0;
   HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
                               &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void AddressableLED::SetData(std::initializer_list<LEDData> ledData) {
   int32_t status = 0;
   HAL_WriteAddressableLEDData(m_handle, ledData.begin(), ledData.size(),
                               &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void AddressableLED::SetBitTiming(units::nanosecond_t lowTime0,
@@ -72,25 +73,25 @@
   HAL_SetAddressableLEDBitTiming(
       m_handle, lowTime0.to<int32_t>(), highTime0.to<int32_t>(),
       lowTime1.to<int32_t>(), highTime1.to<int32_t>(), &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void AddressableLED::SetSyncTime(units::microsecond_t syncTime) {
   int32_t status = 0;
   HAL_SetAddressableLEDSyncTime(m_handle, syncTime.to<int32_t>(), &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void AddressableLED::Start() {
   int32_t status = 0;
   HAL_StartAddressableLEDOutput(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void AddressableLED::Stop() {
   int32_t status = 0;
   HAL_StopAddressableLEDOutput(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void AddressableLED::LEDData::SetHSV(int h, int s, int v) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
index 09bc2f5..0b39e64 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogAccelerometer.cpp
@@ -1,42 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogAccelerometer.h"
 
 #include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/Base.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
 AnalogAccelerometer::AnalogAccelerometer(int channel)
     : AnalogAccelerometer(std::make_shared<AnalogInput>(channel)) {
-  SendableRegistry::GetInstance().AddChild(this, m_analogInput.get());
+  wpi::SendableRegistry::AddChild(this, m_analogInput.get());
 }
 
 AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
-    : m_analogInput(channel, NullDeleter<AnalogInput>()) {
-  if (channel == nullptr) {
-    wpi_setWPIError(NullParameter);
-  } else {
-    InitAccelerometer();
+    : m_analogInput(channel, wpi::NullDeleter<AnalogInput>()) {
+  if (!channel) {
+    throw FRC_MakeError(err::NullParameter, "{}", "channel");
   }
+  InitAccelerometer();
 }
 
 AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
     : m_analogInput(channel) {
-  if (channel == nullptr) {
-    wpi_setWPIError(NullParameter);
-  } else {
-    InitAccelerometer();
+  if (!channel) {
+    throw FRC_MakeError(err::NullParameter, "{}", "channel");
   }
+  InitAccelerometer();
 }
 
 double AnalogAccelerometer::GetAcceleration() const {
@@ -47,20 +42,20 @@
   m_voltsPerG = sensitivity;
 }
 
-void AnalogAccelerometer::SetZero(double zero) { m_zeroGVoltage = zero; }
+void AnalogAccelerometer::SetZero(double zero) {
+  m_zeroGVoltage = zero;
+}
 
-double AnalogAccelerometer::PIDGet() { return GetAcceleration(); }
-
-void AnalogAccelerometer::InitSendable(SendableBuilder& builder) {
+void AnalogAccelerometer::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Accelerometer");
   builder.AddDoubleProperty(
-      "Value", [=]() { return GetAcceleration(); }, nullptr);
+      "Value", [=] { return GetAcceleration(); }, nullptr);
 }
 
 void AnalogAccelerometer::InitAccelerometer() {
   HAL_Report(HALUsageReporting::kResourceType_Accelerometer,
              m_analogInput->GetChannel() + 1);
 
-  SendableRegistry::GetInstance().AddLW(this, "Accelerometer",
-                                        m_analogInput->GetChannel());
+  wpi::SendableRegistry::AddLW(this, "Accelerometer",
+                               m_analogInput->GetChannel());
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogEncoder.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
index c1f4e8d..5f26e87 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogEncoder.cpp
@@ -1,29 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogEncoder.h"
 
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
 #include "frc/AnalogInput.h"
-#include "frc/Base.h"
 #include "frc/Counter.h"
-#include "frc/DriverStation.h"
-#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
+AnalogEncoder::AnalogEncoder(int channel)
+    : AnalogEncoder(std::make_shared<AnalogInput>(channel)) {}
+
 AnalogEncoder::AnalogEncoder(AnalogInput& analogInput)
-    : m_analogInput{&analogInput, NullDeleter<AnalogInput>{}},
+    : m_analogInput{&analogInput, wpi::NullDeleter<AnalogInput>{}},
       m_analogTrigger{m_analogInput.get()},
       m_counter{} {
   Init();
 }
 
 AnalogEncoder::AnalogEncoder(AnalogInput* analogInput)
-    : m_analogInput{analogInput, NullDeleter<AnalogInput>{}},
+    : m_analogInput{analogInput, wpi::NullDeleter<AnalogInput>{}},
       m_analogTrigger{m_analogInput.get()},
       m_counter{} {
   Init();
@@ -49,12 +50,14 @@
   m_counter.SetDownSource(
       m_analogTrigger.CreateOutput(AnalogTriggerType::kFallingPulse));
 
-  SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
-                                        m_analogInput->GetChannel());
+  wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
+                               m_analogInput->GetChannel());
 }
 
 units::turn_t AnalogEncoder::Get() const {
-  if (m_simPosition) return units::turn_t{m_simPosition.Get()};
+  if (m_simPosition) {
+    return units::turn_t{m_simPosition.Get()};
+  }
 
   // As the values are not atomic, keep trying until we get 2 reads of the same
   // value If we don't within 10 attempts, error
@@ -70,13 +73,16 @@
     }
   }
 
-  frc::DriverStation::GetInstance().ReportWarning(
+  FRC_ReportError(
+      warn::Warning, "{}",
       "Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
       "value");
   return m_lastPosition;
 }
 
-double AnalogEncoder::GetPositionOffset() const { return m_positionOffset; }
+double AnalogEncoder::GetPositionOffset() const {
+  return m_positionOffset;
+}
 
 void AnalogEncoder::SetDistancePerRotation(double distancePerRotation) {
   m_distancePerRotation = distancePerRotation;
@@ -87,7 +93,7 @@
 }
 
 double AnalogEncoder::GetDistance() const {
-  return Get().to<double>() * GetDistancePerRotation();
+  return Get().value() * GetDistancePerRotation();
 }
 
 void AnalogEncoder::Reset() {
@@ -95,9 +101,11 @@
   m_positionOffset = m_analogInput->GetVoltage();
 }
 
-int AnalogEncoder::GetChannel() const { return m_analogInput->GetChannel(); }
+int AnalogEncoder::GetChannel() const {
+  return m_analogInput->GetChannel();
+}
 
-void AnalogEncoder::InitSendable(SendableBuilder& builder) {
+void AnalogEncoder::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("AbsoluteEncoder");
   builder.AddDoubleProperty(
       "Distance", [this] { return this->GetDistance(); }, nullptr);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogGyro.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogGyro.cpp
index 2ec4439..106293b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogGyro.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogGyro.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogGyro.h"
 
@@ -13,89 +10,83 @@
 #include <hal/AnalogGyro.h>
 #include <hal/Errors.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/AnalogInput.h"
-#include "frc/Base.h"
+#include "frc/Errors.h"
 #include "frc/Timer.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 AnalogGyro::AnalogGyro(int channel)
     : AnalogGyro(std::make_shared<AnalogInput>(channel)) {
-  SendableRegistry::GetInstance().AddChild(this, m_analog.get());
+  wpi::SendableRegistry::AddChild(this, m_analog.get());
 }
 
 AnalogGyro::AnalogGyro(AnalogInput* channel)
-    : AnalogGyro(
-          std::shared_ptr<AnalogInput>(channel, NullDeleter<AnalogInput>())) {}
+    : AnalogGyro(std::shared_ptr<AnalogInput>(
+          channel, wpi::NullDeleter<AnalogInput>())) {}
 
 AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
     : m_analog(channel) {
-  if (channel == nullptr) {
-    wpi_setWPIError(NullParameter);
-  } else {
-    InitGyro();
-    Calibrate();
+  if (!channel) {
+    throw FRC_MakeError(err::NullParameter, "{}", "channel");
   }
+  InitGyro();
+  Calibrate();
 }
 
 AnalogGyro::AnalogGyro(int channel, int center, double offset)
     : AnalogGyro(std::make_shared<AnalogInput>(channel), center, offset) {
-  SendableRegistry::GetInstance().AddChild(this, m_analog.get());
+  wpi::SendableRegistry::AddChild(this, m_analog.get());
 }
 
 AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, int center,
                        double offset)
     : m_analog(channel) {
-  if (channel == nullptr) {
-    wpi_setWPIError(NullParameter);
-  } else {
-    InitGyro();
-    int32_t status = 0;
-    HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
-                                offset, center, &status);
-    if (status != 0) {
-      wpi_setHALError(status);
-      m_gyroHandle = HAL_kInvalidHandle;
-      return;
-    }
-    Reset();
+  if (!channel) {
+    throw FRC_MakeError(err::NullParameter, "{}", "channel");
   }
+  InitGyro();
+  int32_t status = 0;
+  HAL_SetAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
+                              offset, center, &status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
+  Reset();
 }
 
-AnalogGyro::~AnalogGyro() { HAL_FreeAnalogGyro(m_gyroHandle); }
+AnalogGyro::~AnalogGyro() {
+  HAL_FreeAnalogGyro(m_gyroHandle);
+}
 
 double AnalogGyro::GetAngle() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetAnalogGyroAngle(m_gyroHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
   return value;
 }
 
 double AnalogGyro::GetRate() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetAnalogGyroRate(m_gyroHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
   return value;
 }
 
 int AnalogGyro::GetCenter() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetAnalogGyroCenter(m_gyroHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
   return value;
 }
 
 double AnalogGyro::GetOffset() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetAnalogGyroOffset(m_gyroHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
   return value;
 }
 
@@ -103,65 +94,51 @@
   int32_t status = 0;
   HAL_SetAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
                                            voltsPerDegreePerSecond, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
 }
 
 void AnalogGyro::SetDeadband(double volts) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogGyroDeadband(m_gyroHandle, volts, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
 }
 
 void AnalogGyro::Reset() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetAnalogGyro(m_gyroHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
 }
 
 void AnalogGyro::InitGyro() {
-  if (StatusIsFatal()) return;
   if (m_gyroHandle == HAL_kInvalidHandle) {
     int32_t status = 0;
-    m_gyroHandle = HAL_InitializeAnalogGyro(m_analog->m_port, &status);
-    if (status == PARAMETER_OUT_OF_RANGE) {
-      wpi_setWPIErrorWithContext(ParameterOutOfRange,
-                                 " channel (must be accumulator channel)");
-      m_analog = nullptr;
-      m_gyroHandle = HAL_kInvalidHandle;
-      return;
-    }
-    if (status != 0) {
-      wpi_setHALError(status);
-      m_analog = nullptr;
-      m_gyroHandle = HAL_kInvalidHandle;
-      return;
-    }
+    std::string stackTrace = wpi::GetStackTrace(1);
+    m_gyroHandle =
+        HAL_InitializeAnalogGyro(m_analog->m_port, stackTrace.c_str(), &status);
+    FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
   }
 
   int32_t status = 0;
   HAL_SetupAnalogGyro(m_gyroHandle, &status);
-  if (status != 0) {
-    wpi_setHALError(status);
-    m_analog = nullptr;
-    m_gyroHandle = HAL_kInvalidHandle;
-    return;
-  }
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
 
   HAL_Report(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel() + 1);
 
-  SendableRegistry::GetInstance().AddLW(this, "AnalogGyro",
-                                        m_analog->GetChannel());
+  wpi::SendableRegistry::AddLW(this, "AnalogGyro", m_analog->GetChannel());
 }
 
 void AnalogGyro::Calibrate() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_CalibrateAnalogGyro(m_gyroHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_analog->GetChannel());
 }
 
 std::shared_ptr<AnalogInput> AnalogGyro::GetAnalogInput() const {
   return m_analog;
 }
+
+void AnalogGyro::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Gyro");
+  builder.AddDoubleProperty(
+      "Value", [=] { return GetAngle(); }, nullptr);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogInput.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogInput.cpp
index 11c30ee..dfa5764 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogInput.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogInput.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogInput.h"
 
@@ -12,222 +9,193 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Ports.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
 #include "frc/Timer.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 AnalogInput::AnalogInput(int channel) {
   if (!SensorUtil::CheckAnalogInputChannel(channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "Analog Input " + wpi::Twine(channel));
-    return;
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
   }
 
   m_channel = channel;
 
   HAL_PortHandle port = HAL_GetPort(channel);
   int32_t status = 0;
-  m_port = HAL_InitializeAnalogInputPort(port, &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogInputs(), channel);
-    m_channel = std::numeric_limits<int>::max();
-    m_port = HAL_kInvalidHandle;
-    return;
-  }
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_port = HAL_InitializeAnalogInputPort(port, stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
 
   HAL_Report(HALUsageReporting::kResourceType_AnalogChannel, channel + 1);
 
-  SendableRegistry::GetInstance().AddLW(this, "AnalogInput", channel);
+  wpi::SendableRegistry::AddLW(this, "AnalogInput", channel);
 }
 
-AnalogInput::~AnalogInput() { HAL_FreeAnalogInputPort(m_port); }
+AnalogInput::~AnalogInput() {
+  HAL_FreeAnalogInputPort(m_port);
+}
 
 int AnalogInput::GetValue() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetAnalogValue(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return value;
 }
 
 int AnalogInput::GetAverageValue() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetAnalogAverageValue(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return value;
 }
 
 double AnalogInput::GetVoltage() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double voltage = HAL_GetAnalogVoltage(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return voltage;
 }
 
 double AnalogInput::GetAverageVoltage() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double voltage = HAL_GetAnalogAverageVoltage(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return voltage;
 }
 
 int AnalogInput::GetChannel() const {
-  if (StatusIsFatal()) return 0;
   return m_channel;
 }
 
 void AnalogInput::SetAverageBits(int bits) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogAverageBits(m_port, bits, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 int AnalogInput::GetAverageBits() const {
   int32_t status = 0;
   int averageBits = HAL_GetAnalogAverageBits(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return averageBits;
 }
 
 void AnalogInput::SetOversampleBits(int bits) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogOversampleBits(m_port, bits, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 int AnalogInput::GetOversampleBits() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int oversampleBits = HAL_GetAnalogOversampleBits(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return oversampleBits;
 }
 
 int AnalogInput::GetLSBWeight() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int lsbWeight = HAL_GetAnalogLSBWeight(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return lsbWeight;
 }
 
 int AnalogInput::GetOffset() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int offset = HAL_GetAnalogOffset(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return offset;
 }
 
 bool AnalogInput::IsAccumulatorChannel() const {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool isAccum = HAL_IsAccumulatorChannel(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return isAccum;
 }
 
 void AnalogInput::InitAccumulator() {
-  if (StatusIsFatal()) return;
   m_accumulatorOffset = 0;
   int32_t status = 0;
   HAL_InitAccumulator(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void AnalogInput::SetAccumulatorInitialValue(int64_t initialValue) {
-  if (StatusIsFatal()) return;
   m_accumulatorOffset = initialValue;
 }
 
 void AnalogInput::ResetAccumulator() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetAccumulator(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 
-  if (!StatusIsFatal()) {
-    // Wait until the next sample, so the next call to GetAccumulator*()
-    // won't have old values.
-    const double sampleTime = 1.0 / GetSampleRate();
-    const double overSamples = 1 << GetOversampleBits();
-    const double averageSamples = 1 << GetAverageBits();
-    Wait(sampleTime * overSamples * averageSamples);
-  }
+  // Wait until the next sample, so the next call to GetAccumulator*()
+  // won't have old values.
+  const double sampleTime = 1.0 / GetSampleRate();
+  const double overSamples = 1 << GetOversampleBits();
+  const double averageSamples = 1 << GetAverageBits();
+  Wait(units::second_t{sampleTime * overSamples * averageSamples});
 }
 
 void AnalogInput::SetAccumulatorCenter(int center) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAccumulatorCenter(m_port, center, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void AnalogInput::SetAccumulatorDeadband(int deadband) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAccumulatorDeadband(m_port, deadband, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 int64_t AnalogInput::GetAccumulatorValue() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int64_t value = HAL_GetAccumulatorValue(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return value + m_accumulatorOffset;
 }
 
 int64_t AnalogInput::GetAccumulatorCount() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int64_t count = HAL_GetAccumulatorCount(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return count;
 }
 
 void AnalogInput::GetAccumulatorOutput(int64_t& value, int64_t& count) const {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_GetAccumulatorOutput(m_port, &value, &count, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   value += m_accumulatorOffset;
 }
 
 void AnalogInput::SetSampleRate(double samplesPerSecond) {
   int32_t status = 0;
   HAL_SetAnalogSampleRate(samplesPerSecond, &status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetSampleRate");
 }
 
 double AnalogInput::GetSampleRate() {
   int32_t status = 0;
   double sampleRate = HAL_GetAnalogSampleRate(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetSampleRate");
   return sampleRate;
 }
 
-double AnalogInput::PIDGet() {
-  if (StatusIsFatal()) return 0.0;
-  return GetAverageVoltage();
-}
-
 void AnalogInput::SetSimDevice(HAL_SimDeviceHandle device) {
   HAL_SetAnalogInputSimDevice(m_port, device);
 }
 
-void AnalogInput::InitSendable(SendableBuilder& builder) {
+void AnalogInput::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Input");
   builder.AddDoubleProperty(
-      "Value", [=]() { return GetAverageVoltage(); }, nullptr);
+      "Value", [=] { return GetAverageVoltage(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogOutput.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogOutput.cpp
index bd8126a..a391271 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogOutput.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogOutput.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogOutput.h"
 
@@ -14,62 +11,56 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Ports.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 AnalogOutput::AnalogOutput(int channel) {
   if (!SensorUtil::CheckAnalogOutputChannel(channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "analog output " + wpi::Twine(channel));
-    m_channel = std::numeric_limits<int>::max();
-    m_port = HAL_kInvalidHandle;
-    return;
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
   }
 
   m_channel = channel;
 
   HAL_PortHandle port = HAL_GetPort(m_channel);
   int32_t status = 0;
-  m_port = HAL_InitializeAnalogOutputPort(port, &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumAnalogOutputs(), channel);
-    m_channel = std::numeric_limits<int>::max();
-    m_port = HAL_kInvalidHandle;
-    return;
-  }
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_port = HAL_InitializeAnalogOutputPort(port, stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
 
   HAL_Report(HALUsageReporting::kResourceType_AnalogOutput, m_channel + 1);
-  SendableRegistry::GetInstance().AddLW(this, "AnalogOutput", m_channel);
+  wpi::SendableRegistry::AddLW(this, "AnalogOutput", m_channel);
 }
 
-AnalogOutput::~AnalogOutput() { HAL_FreeAnalogOutputPort(m_port); }
+AnalogOutput::~AnalogOutput() {
+  HAL_FreeAnalogOutputPort(m_port);
+}
 
 void AnalogOutput::SetVoltage(double voltage) {
   int32_t status = 0;
   HAL_SetAnalogOutput(m_port, voltage, &status);
-
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 double AnalogOutput::GetVoltage() const {
   int32_t status = 0;
   double voltage = HAL_GetAnalogOutput(m_port, &status);
-
-  wpi_setHALError(status);
-
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return voltage;
 }
 
-int AnalogOutput::GetChannel() const { return m_channel; }
+int AnalogOutput::GetChannel() const {
+  return m_channel;
+}
 
-void AnalogOutput::InitSendable(SendableBuilder& builder) {
+void AnalogOutput::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Output");
   builder.AddDoubleProperty(
-      "Value", [=]() { return GetVoltage(); },
+      "Value", [=] { return GetVoltage(); },
       [=](double value) { SetVoltage(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
index c7aedd6..ba94613 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogPotentiometer.cpp
@@ -1,16 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogPotentiometer.h"
 
-#include "frc/Base.h"
+#include <utility>
+
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
 #include "frc/RobotController.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
@@ -18,20 +18,22 @@
                                          double offset)
     : AnalogPotentiometer(std::make_shared<AnalogInput>(channel), fullRange,
                           offset) {
-  SendableRegistry::GetInstance().AddChild(this, m_analog_input.get());
+  wpi::SendableRegistry::AddChild(this, m_analog_input.get());
 }
 
 AnalogPotentiometer::AnalogPotentiometer(AnalogInput* input, double fullRange,
                                          double offset)
     : AnalogPotentiometer(
-          std::shared_ptr<AnalogInput>(input, NullDeleter<AnalogInput>()),
+          std::shared_ptr<AnalogInput>(input, wpi::NullDeleter<AnalogInput>()),
           fullRange, offset) {}
 
 AnalogPotentiometer::AnalogPotentiometer(std::shared_ptr<AnalogInput> input,
                                          double fullRange, double offset)
-    : m_analog_input(input), m_fullRange(fullRange), m_offset(offset) {
-  SendableRegistry::GetInstance().AddLW(this, "AnalogPotentiometer",
-                                        m_analog_input->GetChannel());
+    : m_analog_input(std::move(input)),
+      m_fullRange(fullRange),
+      m_offset(offset) {
+  wpi::SendableRegistry::AddLW(this, "AnalogPotentiometer",
+                               m_analog_input->GetChannel());
 }
 
 double AnalogPotentiometer::Get() const {
@@ -41,10 +43,8 @@
          m_offset;
 }
 
-double AnalogPotentiometer::PIDGet() { return Get(); }
-
-void AnalogPotentiometer::InitSendable(SendableBuilder& builder) {
+void AnalogPotentiometer::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Analog Input");
   builder.AddDoubleProperty(
-      "Value", [=]() { return Get(); }, nullptr);
+      "Value", [=] { return Get(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTrigger.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
index 45821ba..ea33c73 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTrigger.cpp
@@ -1,156 +1,129 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogTrigger.h"
 
 #include <utility>
 
+#include <hal/AnalogTrigger.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/AnalogInput.h"
-#include "frc/Base.h"
 #include "frc/DutyCycle.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
 AnalogTrigger::AnalogTrigger(int channel)
     : AnalogTrigger(new AnalogInput(channel)) {
   m_ownsAnalog = true;
-  SendableRegistry::GetInstance().AddChild(this, m_analogInput);
+  wpi::SendableRegistry::AddChild(this, m_analogInput);
 }
 
 AnalogTrigger::AnalogTrigger(AnalogInput* input) {
   m_analogInput = input;
   int32_t status = 0;
   m_trigger = HAL_InitializeAnalogTrigger(input->m_port, &status);
-  if (status != 0) {
-    wpi_setHALError(status);
-    m_trigger = HAL_kInvalidHandle;
-    return;
-  }
+  FRC_CheckErrorStatus(status, "Channel {}", input->GetChannel());
   int index = GetIndex();
 
   HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
-  SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
+  wpi::SendableRegistry::AddLW(this, "AnalogTrigger", index);
 }
 
 AnalogTrigger::AnalogTrigger(DutyCycle* input) {
   m_dutyCycle = input;
   int32_t status = 0;
   m_trigger = HAL_InitializeAnalogTriggerDutyCycle(input->m_handle, &status);
-  if (status != 0) {
-    wpi_setHALError(status);
-    m_trigger = HAL_kInvalidHandle;
-    return;
-  }
+  FRC_CheckErrorStatus(status, "Channel {}", m_dutyCycle->GetSourceChannel());
   int index = GetIndex();
 
   HAL_Report(HALUsageReporting::kResourceType_AnalogTrigger, index + 1);
-  SendableRegistry::GetInstance().AddLW(this, "AnalogTrigger", index);
+  wpi::SendableRegistry::AddLW(this, "AnalogTrigger", index);
 }
 
 AnalogTrigger::~AnalogTrigger() {
   int32_t status = 0;
   HAL_CleanAnalogTrigger(m_trigger, &status);
+  FRC_ReportError(status, "Channel {}", GetSourceChannel());
 
   if (m_ownsAnalog) {
     delete m_analogInput;
   }
 }
 
-AnalogTrigger::AnalogTrigger(AnalogTrigger&& rhs)
-    : ErrorBase(std::move(rhs)),
-      SendableHelper(std::move(rhs)),
-      m_trigger(std::move(rhs.m_trigger)) {
-  std::swap(m_analogInput, rhs.m_analogInput);
-  std::swap(m_dutyCycle, rhs.m_dutyCycle);
-  std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
-}
-
-AnalogTrigger& AnalogTrigger::operator=(AnalogTrigger&& rhs) {
-  ErrorBase::operator=(std::move(rhs));
-  SendableHelper::operator=(std::move(rhs));
-
-  m_trigger = std::move(rhs.m_trigger);
-  std::swap(m_analogInput, rhs.m_analogInput);
-  std::swap(m_dutyCycle, rhs.m_dutyCycle);
-  std::swap(m_ownsAnalog, rhs.m_ownsAnalog);
-
-  return *this;
-}
-
 void AnalogTrigger::SetLimitsVoltage(double lower, double upper) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerLimitsVoltage(m_trigger, lower, upper, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
 }
 
 void AnalogTrigger::SetLimitsDutyCycle(double lower, double upper) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerLimitsDutyCycle(m_trigger, lower, upper, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
 }
 
 void AnalogTrigger::SetLimitsRaw(int lower, int upper) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerLimitsRaw(m_trigger, lower, upper, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
 }
 
 void AnalogTrigger::SetAveraged(bool useAveragedValue) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerAveraged(m_trigger, useAveragedValue, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
 }
 
 void AnalogTrigger::SetFiltered(bool useFilteredValue) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetAnalogTriggerFiltered(m_trigger, useFilteredValue, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
 }
 
 int AnalogTrigger::GetIndex() const {
-  if (StatusIsFatal()) return -1;
   int32_t status = 0;
   auto ret = HAL_GetAnalogTriggerFPGAIndex(m_trigger, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return ret;
 }
 
 bool AnalogTrigger::GetInWindow() {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool result = HAL_GetAnalogTriggerInWindow(m_trigger, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return result;
 }
 
 bool AnalogTrigger::GetTriggerState() {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool result = HAL_GetAnalogTriggerTriggerState(m_trigger, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return result;
 }
 
 std::shared_ptr<AnalogTriggerOutput> AnalogTrigger::CreateOutput(
     AnalogTriggerType type) const {
-  if (StatusIsFatal()) return nullptr;
   return std::shared_ptr<AnalogTriggerOutput>(
-      new AnalogTriggerOutput(*this, type), NullDeleter<AnalogTriggerOutput>());
+      new AnalogTriggerOutput(*this, type));
 }
 
-void AnalogTrigger::InitSendable(SendableBuilder& builder) {
-  if (m_ownsAnalog) m_analogInput->InitSendable(builder);
+void AnalogTrigger::InitSendable(wpi::SendableBuilder& builder) {
+  if (m_ownsAnalog) {
+    m_analogInput->InitSendable(builder);
+  }
+}
+
+int AnalogTrigger::GetSourceChannel() const {
+  if (m_analogInput) {
+    return m_analogInput->GetChannel();
+  } else if (m_dutyCycle) {
+    return m_dutyCycle->GetSourceChannel();
+  } else {
+    return -1;
+  }
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
index 8fba479..93d1969 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AnalogTriggerOutput.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogTriggerOutput.h"
 
+#include <hal/AnalogTrigger.h>
 #include <hal/FRCUsageReporting.h>
 
 #include "frc/AnalogTrigger.h"
-#include "frc/WPIErrors.h"
+#include "frc/AnalogTriggerType.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -19,7 +18,7 @@
   bool result = HAL_GetAnalogTriggerOutput(
       m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
       &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Get");
   return result;
 }
 
@@ -31,11 +30,15 @@
   return m_outputType;
 }
 
-bool AnalogTriggerOutput::IsAnalogTrigger() const { return true; }
+bool AnalogTriggerOutput::IsAnalogTrigger() const {
+  return true;
+}
 
-int AnalogTriggerOutput::GetChannel() const { return m_trigger->GetIndex(); }
+int AnalogTriggerOutput::GetChannel() const {
+  return m_trigger->GetIndex();
+}
 
-void AnalogTriggerOutput::InitSendable(SendableBuilder&) {}
+void AnalogTriggerOutput::InitSendable(wpi::SendableBuilder&) {}
 
 AnalogTriggerOutput::AnalogTriggerOutput(const AnalogTrigger& trigger,
                                          AnalogTriggerType outputType)
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/AsynchronousInterrupt.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/AsynchronousInterrupt.cpp
new file mode 100644
index 0000000..d45d5e4
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/AsynchronousInterrupt.cpp
@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/AsynchronousInterrupt.h"
+
+#include <frc/DigitalSource.h>
+
+using namespace frc;
+
+AsynchronousInterrupt::AsynchronousInterrupt(
+    DigitalSource& source, std::function<void(bool, bool)> callback)
+    : m_interrupt{source}, m_callback{std::move(callback)} {}
+AsynchronousInterrupt::AsynchronousInterrupt(
+    DigitalSource* source, std::function<void(bool, bool)> callback)
+    : m_interrupt{source}, m_callback{std::move(callback)} {}
+AsynchronousInterrupt::AsynchronousInterrupt(
+    std::shared_ptr<DigitalSource> source,
+    std::function<void(bool, bool)> callback)
+    : m_interrupt{source}, m_callback{std::move(callback)} {}
+
+AsynchronousInterrupt::~AsynchronousInterrupt() {
+  Disable();
+}
+
+void AsynchronousInterrupt::ThreadMain() {
+  while (m_keepRunning) {
+    auto result = m_interrupt.WaitForInterrupt(10_s, false);
+    if (!m_keepRunning) {
+      break;
+    }
+    if (result == SynchronousInterrupt::WaitResult::kTimeout) {
+      continue;
+    }
+    m_callback((result & SynchronousInterrupt::WaitResult::kRisingEdge) != 0,
+               (result & SynchronousInterrupt::WaitResult::kFallingEdge) != 0);
+  }
+}
+
+void AsynchronousInterrupt::Enable() {
+  if (m_keepRunning) {
+    return;
+  }
+
+  m_keepRunning = true;
+  m_thread = std::thread([this] { this->ThreadMain(); });
+}
+
+void AsynchronousInterrupt::Disable() {
+  m_keepRunning = false;
+  m_interrupt.WakeupWaitingInterrupt();
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
+}
+
+void AsynchronousInterrupt::SetInterruptEdges(bool risingEdge,
+                                              bool fallingEdge) {
+  m_interrupt.SetInterruptEdges(risingEdge, fallingEdge);
+}
+
+units::second_t AsynchronousInterrupt::GetRisingTimestamp() {
+  return m_interrupt.GetRisingTimestamp();
+}
+units::second_t AsynchronousInterrupt::GetFallingTimestamp() {
+  return m_interrupt.GetFallingTimestamp();
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
index efc0fe6..1b8da85 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/BuiltInAccelerometer.cpp
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/BuiltInAccelerometer.h"
 
 #include <hal/Accelerometer.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -21,32 +18,38 @@
 
   HAL_Report(HALUsageReporting::kResourceType_Accelerometer, 0, 0,
              "Built-in accelerometer");
-  SendableRegistry::GetInstance().AddLW(this, "BuiltInAccel");
+  wpi::SendableRegistry::AddLW(this, "BuiltInAccel");
 }
 
 void BuiltInAccelerometer::SetRange(Range range) {
   if (range == kRange_16G) {
-    wpi_setWPIErrorWithContext(
-        ParameterOutOfRange, "16G range not supported (use k2G, k4G, or k8G)");
+    throw FRC_MakeError(err::ParameterOutOfRange, "{}",
+                        "16G range not supported (use k2G, k4G, or k8G)");
   }
 
   HAL_SetAccelerometerActive(false);
-  HAL_SetAccelerometerRange((HAL_AccelerometerRange)range);
+  HAL_SetAccelerometerRange(static_cast<HAL_AccelerometerRange>(range));
   HAL_SetAccelerometerActive(true);
 }
 
-double BuiltInAccelerometer::GetX() { return HAL_GetAccelerometerX(); }
+double BuiltInAccelerometer::GetX() {
+  return HAL_GetAccelerometerX();
+}
 
-double BuiltInAccelerometer::GetY() { return HAL_GetAccelerometerY(); }
+double BuiltInAccelerometer::GetY() {
+  return HAL_GetAccelerometerY();
+}
 
-double BuiltInAccelerometer::GetZ() { return HAL_GetAccelerometerZ(); }
+double BuiltInAccelerometer::GetZ() {
+  return HAL_GetAccelerometerZ();
+}
 
-void BuiltInAccelerometer::InitSendable(SendableBuilder& builder) {
+void BuiltInAccelerometer::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("3AxisAccelerometer");
   builder.AddDoubleProperty(
-      "X", [=]() { return GetX(); }, nullptr);
+      "X", [=] { return GetX(); }, nullptr);
   builder.AddDoubleProperty(
-      "Y", [=]() { return GetY(); }, nullptr);
+      "Y", [=] { return GetY(); }, nullptr);
   builder.AddDoubleProperty(
-      "Z", [=]() { return GetZ(); }, nullptr);
+      "Z", [=] { return GetZ(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/CAN.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/CAN.cpp
index e56e435..e26ca37 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/CAN.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/CAN.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/CAN.h"
 
@@ -14,17 +11,15 @@
 #include <hal/Errors.h>
 #include <hal/FRCUsageReporting.h>
 
+#include "frc/Errors.h"
+
 using namespace frc;
 
 CAN::CAN(int deviceId) {
   int32_t status = 0;
   m_handle =
       HAL_InitializeCAN(kTeamManufacturer, deviceId, kTeamDeviceType, &status);
-  if (status != 0) {
-    wpi_setHALError(status);
-    m_handle = HAL_kInvalidHandle;
-    return;
-  }
+  FRC_CheckErrorStatus(status, "device id {}", deviceId);
 
   HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
 }
@@ -34,17 +29,13 @@
   m_handle = HAL_InitializeCAN(
       static_cast<HAL_CANManufacturer>(deviceManufacturer), deviceId,
       static_cast<HAL_CANDeviceType>(deviceType), &status);
-  if (status != 0) {
-    wpi_setHALError(status);
-    m_handle = HAL_kInvalidHandle;
-    return;
-  }
+  FRC_CheckErrorStatus(status, "device id {} mfg {} type {}", deviceId,
+                       deviceManufacturer, deviceType);
 
   HAL_Report(HALUsageReporting::kResourceType_CAN, deviceId + 1);
 }
 
 CAN::~CAN() {
-  if (StatusIsFatal()) return;
   if (m_handle != HAL_kInvalidHandle) {
     HAL_CleanCAN(m_handle);
     m_handle = HAL_kInvalidHandle;
@@ -54,20 +45,20 @@
 void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
   int32_t status = 0;
   HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "WritePacket");
 }
 
 void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId,
                                int repeatMs) {
   int32_t status = 0;
   HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "WritePacketRepeating");
 }
 
 void CAN::WriteRTRFrame(int length, int apiId) {
   int32_t status = 0;
   HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "WriteRTRFrame");
 }
 
 int CAN::WritePacketNoError(const uint8_t* data, int length, int apiId) {
@@ -92,7 +83,7 @@
 void CAN::StopPacketRepeating(int apiId) {
   int32_t status = 0;
   HAL_StopCANPacketRepeating(m_handle, apiId, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "StopPacketRepeating");
 }
 
 bool CAN::ReadPacketNew(int apiId, CANData* data) {
@@ -103,7 +94,7 @@
     return false;
   }
   if (status != 0) {
-    wpi_setHALError(status);
+    FRC_CheckErrorStatus(status, "{}", "ReadPacketNew");
     return false;
   } else {
     return true;
@@ -118,7 +109,7 @@
     return false;
   }
   if (status != 0) {
-    wpi_setHALError(status);
+    FRC_CheckErrorStatus(status, "{}", "ReadPacketLatest");
     return false;
   } else {
     return true;
@@ -134,7 +125,7 @@
     return false;
   }
   if (status != 0) {
-    wpi_setHALError(status);
+    FRC_CheckErrorStatus(status, "{}", "ReadPacketTimeout");
     return false;
   } else {
     return true;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Compressor.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Compressor.cpp
index e40758f..0302acb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -1,221 +1,72 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Compressor.h"
 
-#include <hal/Compressor.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/Ports.h>
-#include <hal/Solenoid.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
-Compressor::Compressor(int pcmID) : m_module(pcmID) {
-  int32_t status = 0;
-  m_compressorHandle = HAL_InitializeCompressor(m_module, &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumPCMModules(), pcmID);
-    return;
+Compressor::Compressor(int module, PneumaticsModuleType moduleType)
+    : m_module{PneumaticsBase::GetForType(module, moduleType)} {
+  if (!m_module->ReserveCompressor()) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", module);
   }
+
   SetClosedLoopControl(true);
 
-  HAL_Report(HALUsageReporting::kResourceType_Compressor, pcmID + 1);
-  SendableRegistry::GetInstance().AddLW(this, "Compressor", pcmID);
+  HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1);
+  wpi::SendableRegistry::AddLW(this, "Compressor", module);
+}
+
+Compressor::Compressor(PneumaticsModuleType moduleType)
+    : Compressor{PneumaticsBase::GetDefaultForType(moduleType), moduleType} {}
+
+Compressor::~Compressor() {
+  m_module->UnreserveCompressor();
 }
 
 void Compressor::Start() {
-  if (StatusIsFatal()) return;
   SetClosedLoopControl(true);
 }
 
 void Compressor::Stop() {
-  if (StatusIsFatal()) return;
   SetClosedLoopControl(false);
 }
 
 bool Compressor::Enabled() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressor(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
+  return m_module->GetCompressor();
 }
 
 bool Compressor::GetPressureSwitchValue() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorPressureSwitch(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
+  return m_module->GetPressureSwitch();
 }
 
 double Compressor::GetCompressorCurrent() const {
-  if (StatusIsFatal()) return 0;
-  int32_t status = 0;
-  double value;
-
-  value = HAL_GetCompressorCurrent(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
+  return m_module->GetCompressorCurrent();
 }
 
 void Compressor::SetClosedLoopControl(bool on) {
-  if (StatusIsFatal()) return;
-  int32_t status = 0;
-
-  HAL_SetCompressorClosedLoopControl(m_compressorHandle, on, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
+  m_module->SetClosedLoopControl(on);
 }
 
 bool Compressor::GetClosedLoopControl() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorClosedLoopControl(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
+  return m_module->GetClosedLoopControl();
 }
 
-bool Compressor::GetCompressorCurrentTooHighFault() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorCurrentTooHighFault(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
-}
-
-bool Compressor::GetCompressorCurrentTooHighStickyFault() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value =
-      HAL_GetCompressorCurrentTooHighStickyFault(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
-}
-
-bool Compressor::GetCompressorShortedStickyFault() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorShortedStickyFault(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
-}
-
-bool Compressor::GetCompressorShortedFault() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorShortedFault(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
-}
-
-bool Compressor::GetCompressorNotConnectedStickyFault() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorNotConnectedStickyFault(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
-}
-
-bool Compressor::GetCompressorNotConnectedFault() const {
-  if (StatusIsFatal()) return false;
-  int32_t status = 0;
-  bool value;
-
-  value = HAL_GetCompressorNotConnectedFault(m_compressorHandle, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-
-  return value;
-}
-
-void Compressor::ClearAllPCMStickyFaults() {
-  if (StatusIsFatal()) return;
-  int32_t status = 0;
-
-  HAL_ClearAllPCMStickyFaults(m_module, &status);
-
-  if (status) {
-    wpi_setWPIError(Timeout);
-  }
-}
-
-int Compressor::GetModule() const { return m_module; }
-
-void Compressor::InitSendable(SendableBuilder& builder) {
+void Compressor::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Compressor");
   builder.AddBooleanProperty(
-      "Enabled", [=]() { return Enabled(); },
-      [=](bool value) {
-        if (value)
-          Start();
-        else
-          Stop();
-      });
+      "Closed Loop Control", [=]() { return GetClosedLoopControl(); },
+      [=](bool value) { SetClosedLoopControl(value); });
+  builder.AddBooleanProperty(
+      "Enabled", [=] { return Enabled(); }, nullptr);
   builder.AddBooleanProperty(
       "Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Counter.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Counter.cpp
index ca74ab8..a1ad9e7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Counter.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Counter.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Counter.h"
 
@@ -11,25 +8,26 @@
 
 #include <hal/Counter.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/AnalogTrigger.h"
-#include "frc/Base.h"
 #include "frc/DigitalInput.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
 Counter::Counter(Mode mode) {
   int32_t status = 0;
-  m_counter = HAL_InitializeCounter((HAL_Counter_Mode)mode, &m_index, &status);
-  wpi_setHALError(status);
+  m_counter = HAL_InitializeCounter(static_cast<HAL_Counter_Mode>(mode),
+                                    &m_index, &status);
+  FRC_CheckErrorStatus(status, "{}", "InitializeCounter");
 
-  SetMaxPeriod(0.5);
+  SetMaxPeriod(0.5_s);
 
   HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1, mode + 1);
-  SendableRegistry::GetInstance().AddLW(this, "Counter", m_index);
+  wpi::SendableRegistry::AddLW(this, "Counter", m_index);
 }
 
 Counter::Counter(int channel) : Counter(kTwoPulse) {
@@ -56,9 +54,9 @@
                  DigitalSource* downSource, bool inverted)
     : Counter(encodingType,
               std::shared_ptr<DigitalSource>(upSource,
-                                             NullDeleter<DigitalSource>()),
+                                             wpi::NullDeleter<DigitalSource>()),
               std::shared_ptr<DigitalSource>(downSource,
-                                             NullDeleter<DigitalSource>()),
+                                             wpi::NullDeleter<DigitalSource>()),
               inverted) {}
 
 Counter::Counter(EncodingType encodingType,
@@ -66,10 +64,8 @@
                  std::shared_ptr<DigitalSource> downSource, bool inverted)
     : Counter(kExternalDirection) {
   if (encodingType != k1X && encodingType != k2X) {
-    wpi_setWPIErrorWithContext(
-        ParameterOutOfRange,
-        "Counter only supports 1X and 2X quadrature decoding.");
-    return;
+    throw FRC_MakeError(err::ParameterOutOfRange, "{}",
+                        "Counter only supports 1X and 2X quadrature decoding");
   }
   SetUpSource(upSource);
   SetDownSource(downSource);
@@ -83,256 +79,235 @@
     HAL_SetCounterAverageSize(m_counter, 2, &status);
   }
 
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Counter constructor");
   SetDownSourceEdge(inverted, true);
 }
 
 Counter::~Counter() {
-  SetUpdateWhenEmpty(true);
+  try {
+    SetUpdateWhenEmpty(true);
+  } catch (const RuntimeError& e) {
+    e.Report();
+  }
 
   int32_t status = 0;
   HAL_FreeCounter(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_ReportError(status, "{}", "Counter destructor");
 }
 
 void Counter::SetUpSource(int channel) {
-  if (StatusIsFatal()) return;
   SetUpSource(std::make_shared<DigitalInput>(channel));
-  SendableRegistry::GetInstance().AddChild(this, m_upSource.get());
+  wpi::SendableRegistry::AddChild(this, m_upSource.get());
 }
 
 void Counter::SetUpSource(AnalogTrigger* analogTrigger,
                           AnalogTriggerType triggerType) {
   SetUpSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
-                                             NullDeleter<AnalogTrigger>()),
+                                             wpi::NullDeleter<AnalogTrigger>()),
               triggerType);
 }
 
 void Counter::SetUpSource(std::shared_ptr<AnalogTrigger> analogTrigger,
                           AnalogTriggerType triggerType) {
-  if (StatusIsFatal()) return;
   SetUpSource(analogTrigger->CreateOutput(triggerType));
 }
 
 void Counter::SetUpSource(DigitalSource* source) {
-  SetUpSource(
-      std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+  SetUpSource(std::shared_ptr<DigitalSource>(
+      source, wpi::NullDeleter<DigitalSource>()));
 }
 
 void Counter::SetUpSource(std::shared_ptr<DigitalSource> source) {
-  if (StatusIsFatal()) return;
   m_upSource = source;
-  if (m_upSource->StatusIsFatal()) {
-    CloneError(*m_upSource);
-  } else {
-    int32_t status = 0;
-    HAL_SetCounterUpSource(
-        m_counter, source->GetPortHandleForRouting(),
-        (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
-        &status);
-    wpi_setHALError(status);
-  }
+  int32_t status = 0;
+  HAL_SetCounterUpSource(m_counter, source->GetPortHandleForRouting(),
+                         static_cast<HAL_AnalogTriggerType>(
+                             source->GetAnalogTriggerTypeForRouting()),
+                         &status);
+  FRC_CheckErrorStatus(status, "{}", "SetUpSource");
 }
 
 void Counter::SetUpSource(DigitalSource& source) {
-  SetUpSource(
-      std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+  SetUpSource(std::shared_ptr<DigitalSource>(
+      &source, wpi::NullDeleter<DigitalSource>()));
 }
 
 void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) {
-  if (StatusIsFatal()) return;
   if (m_upSource == nullptr) {
-    wpi_setWPIErrorWithContext(
-        NullParameter,
+    throw FRC_MakeError(
+        err::NullParameter, "{}",
         "Must set non-nullptr UpSource before setting UpSourceEdge");
   }
   int32_t status = 0;
   HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetUpSourceEdge");
 }
 
 void Counter::ClearUpSource() {
-  if (StatusIsFatal()) return;
   m_upSource.reset();
   int32_t status = 0;
   HAL_ClearCounterUpSource(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "ClearUpSource");
 }
 
 void Counter::SetDownSource(int channel) {
-  if (StatusIsFatal()) return;
   SetDownSource(std::make_shared<DigitalInput>(channel));
-  SendableRegistry::GetInstance().AddChild(this, m_downSource.get());
+  wpi::SendableRegistry::AddChild(this, m_downSource.get());
 }
 
 void Counter::SetDownSource(AnalogTrigger* analogTrigger,
                             AnalogTriggerType triggerType) {
-  SetDownSource(std::shared_ptr<AnalogTrigger>(analogTrigger,
-                                               NullDeleter<AnalogTrigger>()),
+  SetDownSource(std::shared_ptr<AnalogTrigger>(
+                    analogTrigger, wpi::NullDeleter<AnalogTrigger>()),
                 triggerType);
 }
 
 void Counter::SetDownSource(std::shared_ptr<AnalogTrigger> analogTrigger,
                             AnalogTriggerType triggerType) {
-  if (StatusIsFatal()) return;
   SetDownSource(analogTrigger->CreateOutput(triggerType));
 }
 
 void Counter::SetDownSource(DigitalSource* source) {
-  SetDownSource(
-      std::shared_ptr<DigitalSource>(source, NullDeleter<DigitalSource>()));
+  SetDownSource(std::shared_ptr<DigitalSource>(
+      source, wpi::NullDeleter<DigitalSource>()));
 }
 
 void Counter::SetDownSource(DigitalSource& source) {
-  SetDownSource(
-      std::shared_ptr<DigitalSource>(&source, NullDeleter<DigitalSource>()));
+  SetDownSource(std::shared_ptr<DigitalSource>(
+      &source, wpi::NullDeleter<DigitalSource>()));
 }
 
 void Counter::SetDownSource(std::shared_ptr<DigitalSource> source) {
-  if (StatusIsFatal()) return;
   m_downSource = source;
-  if (m_downSource->StatusIsFatal()) {
-    CloneError(*m_downSource);
-  } else {
-    int32_t status = 0;
-    HAL_SetCounterDownSource(
-        m_counter, source->GetPortHandleForRouting(),
-        (HAL_AnalogTriggerType)source->GetAnalogTriggerTypeForRouting(),
-        &status);
-    wpi_setHALError(status);
-  }
+  int32_t status = 0;
+  HAL_SetCounterDownSource(m_counter, source->GetPortHandleForRouting(),
+                           static_cast<HAL_AnalogTriggerType>(
+                               source->GetAnalogTriggerTypeForRouting()),
+                           &status);
+  FRC_CheckErrorStatus(status, "{}", "SetDownSource");
 }
 
 void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) {
-  if (StatusIsFatal()) return;
   if (m_downSource == nullptr) {
-    wpi_setWPIErrorWithContext(
-        NullParameter,
+    throw FRC_MakeError(
+        err::NullParameter, "{}",
         "Must set non-nullptr DownSource before setting DownSourceEdge");
   }
   int32_t status = 0;
   HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetDownSourceEdge");
 }
 
 void Counter::ClearDownSource() {
-  if (StatusIsFatal()) return;
   m_downSource.reset();
   int32_t status = 0;
   HAL_ClearCounterDownSource(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "ClearDownSource");
 }
 
 void Counter::SetUpDownCounterMode() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterUpDownMode(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetUpDownCounterMode");
 }
 
 void Counter::SetExternalDirectionMode() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterExternalDirectionMode(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetExternalDirectionMode");
 }
 
 void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterSemiPeriodMode(m_counter, highSemiPeriod, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSemiPeriodMode to {}",
+                       highSemiPeriod ? "true" : "false");
 }
 
 void Counter::SetPulseLengthMode(double threshold) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetPulseLengthMode");
 }
 
 void Counter::SetReverseDirection(bool reverseDirection) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterReverseDirection(m_counter, reverseDirection, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetReverseDirection to {}",
+                       reverseDirection ? "true" : "false");
 }
 
 void Counter::SetSamplesToAverage(int samplesToAverage) {
   if (samplesToAverage < 1 || samplesToAverage > 127) {
-    wpi_setWPIErrorWithContext(
-        ParameterOutOfRange,
-        "Average counter values must be between 1 and 127");
+    throw FRC_MakeError(
+        err::ParameterOutOfRange,
+        "Average counter values must be between 1 and 127, {} out of range",
+        samplesToAverage);
   }
   int32_t status = 0;
   HAL_SetCounterSamplesToAverage(m_counter, samplesToAverage, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSamplesToAverage to {}", samplesToAverage);
 }
 
 int Counter::GetSamplesToAverage() const {
   int32_t status = 0;
   int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetSamplesToAverage");
   return samples;
 }
 
-int Counter::GetFPGAIndex() const { return m_index; }
+int Counter::GetFPGAIndex() const {
+  return m_index;
+}
 
 int Counter::Get() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetCounter(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Get");
   return value;
 }
 
 void Counter::Reset() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetCounter(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Reset");
 }
 
-double Counter::GetPeriod() const {
-  if (StatusIsFatal()) return 0.0;
+units::second_t Counter::GetPeriod() const {
   int32_t status = 0;
   double value = HAL_GetCounterPeriod(m_counter, &status);
-  wpi_setHALError(status);
-  return value;
+  FRC_CheckErrorStatus(status, "{}", "GetPeriod");
+  return units::second_t{value};
 }
 
-void Counter::SetMaxPeriod(double maxPeriod) {
-  if (StatusIsFatal()) return;
+void Counter::SetMaxPeriod(units::second_t maxPeriod) {
   int32_t status = 0;
-  HAL_SetCounterMaxPeriod(m_counter, maxPeriod, &status);
-  wpi_setHALError(status);
+  HAL_SetCounterMaxPeriod(m_counter, maxPeriod.value(), &status);
+  FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod");
 }
 
 void Counter::SetUpdateWhenEmpty(bool enabled) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetUpdateWhenEmpty");
 }
 
 bool Counter::GetStopped() const {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetCounterStopped(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetStopped");
   return value;
 }
 
 bool Counter::GetDirection() const {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetCounterDirection(m_counter, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetDirection");
   return value;
 }
 
-void Counter::InitSendable(SendableBuilder& builder) {
+void Counter::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Counter");
   builder.AddDoubleProperty(
-      "Value", [=]() { return Get(); }, nullptr);
+      "Value", [=] { return Get(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp
index 1861555..80c0adb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DMA.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DMA.h"
 
@@ -12,104 +9,154 @@
 #include <frc/DigitalSource.h>
 #include <frc/DutyCycle.h>
 #include <frc/Encoder.h>
+#include <frc/PWM.h>
+#include <frc/motorcontrol/PWMMotorController.h>
 
 #include <hal/DMA.h>
 #include <hal/HALBase.h>
 
+#include "frc/AnalogInput.h"
+#include "frc/Counter.h"
+#include "frc/DigitalSource.h"
+#include "frc/DutyCycle.h"
+#include "frc/Encoder.h"
+#include "frc/Errors.h"
+
 using namespace frc;
 
 DMA::DMA() {
   int32_t status = 0;
   dmaHandle = HAL_InitializeDMA(&status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "InitializeDMA");
 }
 
-DMA::~DMA() { HAL_FreeDMA(dmaHandle); }
+DMA::~DMA() {
+  HAL_FreeDMA(dmaHandle);
+}
 
 void DMA::SetPause(bool pause) {
   int32_t status = 0;
   HAL_SetDMAPause(dmaHandle, pause, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "SetPause");
 }
 
-void DMA::SetRate(int cycles) {
+void DMA::SetTimedTrigger(units::second_t seconds) {
   int32_t status = 0;
-  HAL_SetDMARate(dmaHandle, cycles, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  HAL_SetDMATimedTrigger(dmaHandle, seconds.value(), &status);
+  FRC_CheckErrorStatus(status, "{}", "SetTimedTrigger");
+}
+
+void DMA::SetTimedTriggerCycles(int cycles) {
+  int32_t status = 0;
+  HAL_SetDMATimedTriggerCycles(dmaHandle, cycles, &status);
+  FRC_CheckErrorStatus(status, "{}", "SetTimedTriggerCycles");
 }
 
 void DMA::AddEncoder(const Encoder* encoder) {
   int32_t status = 0;
   HAL_AddDMAEncoder(dmaHandle, encoder->m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddEncoder");
 }
 
 void DMA::AddEncoderPeriod(const Encoder* encoder) {
   int32_t status = 0;
   HAL_AddDMAEncoderPeriod(dmaHandle, encoder->m_encoder, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddEncoderPeriod");
 }
 
 void DMA::AddCounter(const Counter* counter) {
   int32_t status = 0;
   HAL_AddDMACounter(dmaHandle, counter->m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddCounter");
 }
 
 void DMA::AddCounterPeriod(const Counter* counter) {
   int32_t status = 0;
   HAL_AddDMACounterPeriod(dmaHandle, counter->m_counter, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddCounterPeriod");
 }
 
 void DMA::AddDigitalSource(const DigitalSource* digitalSource) {
   int32_t status = 0;
   HAL_AddDMADigitalSource(dmaHandle, digitalSource->GetPortHandleForRouting(),
                           &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddDigitalSource");
 }
 
 void DMA::AddDutyCycle(const DutyCycle* dutyCycle) {
   int32_t status = 0;
   HAL_AddDMADutyCycle(dmaHandle, dutyCycle->m_handle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddDutyCycle");
 }
 
 void DMA::AddAnalogInput(const AnalogInput* analogInput) {
   int32_t status = 0;
   HAL_AddDMAAnalogInput(dmaHandle, analogInput->m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddAnalogInput");
 }
 
 void DMA::AddAveragedAnalogInput(const AnalogInput* analogInput) {
   int32_t status = 0;
   HAL_AddDMAAveragedAnalogInput(dmaHandle, analogInput->m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddAveragedAnalogInput");
 }
 
 void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) {
   int32_t status = 0;
   HAL_AddDMAAnalogAccumulator(dmaHandle, analogInput->m_port, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "AddAnalogAccumulator");
 }
 
-void DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
+int DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
   int32_t status = 0;
-  HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(),
-                            static_cast<HAL_AnalogTriggerType>(
-                                source->GetAnalogTriggerTypeForRouting()),
-                            rising, falling, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  int32_t idx =
+      HAL_SetDMAExternalTrigger(dmaHandle, source->GetPortHandleForRouting(),
+                                static_cast<HAL_AnalogTriggerType>(
+                                    source->GetAnalogTriggerTypeForRouting()),
+                                rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", "SetExternalTrigger");
+  return idx;
 }
 
-void DMA::StartDMA(int queueDepth) {
+int DMA::SetPwmEdgeTrigger(PWMMotorController* source, bool rising,
+                           bool falling) {
+  int32_t status = 0;
+  int32_t idx = HAL_SetDMAExternalTrigger(
+      dmaHandle, source->GetPwm()->m_handle,
+      HAL_AnalogTriggerType::HAL_Trigger_kInWindow, rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", "SetPWmEdgeTrigger");
+  return idx;
+}
+
+int DMA::SetPwmEdgeTrigger(PWM* source, bool rising, bool falling) {
+  int32_t status = 0;
+  int32_t idx = HAL_SetDMAExternalTrigger(
+      dmaHandle, source->m_handle, HAL_AnalogTriggerType::HAL_Trigger_kInWindow,
+      rising, falling, &status);
+  FRC_CheckErrorStatus(status, "{}", "SetPWmEdgeTrigger");
+  return idx;
+}
+
+void DMA::ClearSensors() {
+  int32_t status = 0;
+  HAL_ClearDMASensors(dmaHandle, &status);
+  FRC_CheckErrorStatus(status, "{}", "ClearSensors");
+}
+
+void DMA::ClearExternalTriggers() {
+  int32_t status = 0;
+  HAL_ClearDMAExternalTriggers(dmaHandle, &status);
+  FRC_CheckErrorStatus(status, "{}", "ClearExternalTriggers");
+}
+
+void DMA::Start(int queueDepth) {
   int32_t status = 0;
   HAL_StartDMA(dmaHandle, queueDepth, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "Start");
 }
 
-void DMA::StopDMA() {
+void DMA::Stop() {
   int32_t status = 0;
   HAL_StopDMA(dmaHandle, &status);
-  wpi_setErrorWithContext(status, HAL_GetErrorMessage(status));
+  FRC_CheckErrorStatus(status, "{}", "Stop");
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DMC60.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DMC60.cpp
deleted file mode 100644
index e7d2c65..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DMC60.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/DMC60.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-DMC60::DMC60(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "DMC60", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DSControlWord.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DSControlWord.cpp
new file mode 100644
index 0000000..15d209a
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DSControlWord.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/DSControlWord.h"
+
+#include <hal/DriverStation.h>
+
+using namespace frc;
+
+DSControlWord::DSControlWord() {
+  HAL_GetControlWord(&m_controlWord);
+}
+
+bool DSControlWord::IsEnabled() const {
+  return m_controlWord.enabled && m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsDisabled() const {
+  return !(m_controlWord.enabled && m_controlWord.dsAttached);
+}
+
+bool DSControlWord::IsEStopped() const {
+  return m_controlWord.eStop;
+}
+
+bool DSControlWord::IsAutonomous() const {
+  return m_controlWord.autonomous;
+}
+
+bool DSControlWord::IsAutonomousEnabled() const {
+  return m_controlWord.autonomous && m_controlWord.enabled &&
+         m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsTeleop() const {
+  return !(m_controlWord.autonomous || m_controlWord.test);
+}
+
+bool DSControlWord::IsTeleopEnabled() const {
+  return !m_controlWord.autonomous && !m_controlWord.test &&
+         m_controlWord.enabled && m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsTest() const {
+  return m_controlWord.test;
+}
+
+bool DSControlWord::IsDSAttached() const {
+  return m_controlWord.dsAttached;
+}
+
+bool DSControlWord::IsFMSAttached() const {
+  return m_controlWord.fmsAttached;
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Debouncer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Debouncer.cpp
new file mode 100644
index 0000000..eb402cd
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Debouncer.cpp
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Debouncer.h"
+
+using namespace frc;
+
+Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
+    : m_debounceTime(debounceTime), m_debounceType(type) {
+  switch (type) {
+    case DebounceType::kBoth:  // fall-through
+    case DebounceType::kRising:
+      m_baseline = false;
+      break;
+    case DebounceType::kFalling:
+      m_baseline = true;
+      break;
+  }
+  m_timer.Start();
+}
+
+bool Debouncer::Calculate(bool input) {
+  if (input == m_baseline) {
+    m_timer.Reset();
+  }
+
+  if (m_timer.HasElapsed(m_debounceTime)) {
+    if (m_debounceType == DebounceType::kBoth) {
+      m_baseline = input;
+      m_timer.Reset();
+    }
+    return input;
+  } else {
+    return m_baseline;
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
index 3cec3f1..3294a99 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalGlitchFilter.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DigitalGlitchFilter.h"
 
@@ -14,13 +11,12 @@
 #include <hal/Constants.h>
 #include <hal/DIO.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 #include "frc/Counter.h"
 #include "frc/Encoder.h"
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
@@ -32,15 +28,14 @@
   std::scoped_lock lock(m_mutex);
   auto index =
       std::find(m_filterAllocated.begin(), m_filterAllocated.end(), false);
-  wpi_assert(index != m_filterAllocated.end());
+  FRC_Assert(index != m_filterAllocated.end());
 
   m_channelIndex = std::distance(m_filterAllocated.begin(), index);
   *index = true;
 
   HAL_Report(HALUsageReporting::kResourceType_DigitalGlitchFilter,
              m_channelIndex + 1);
-  SendableRegistry::GetInstance().AddLW(this, "DigitalGlitchFilter",
-                                        m_channelIndex);
+  wpi::SendableRegistry::AddLW(this, "DigitalGlitchFilter", m_channelIndex);
 }
 
 DigitalGlitchFilter::~DigitalGlitchFilter() {
@@ -50,20 +45,6 @@
   }
 }
 
-DigitalGlitchFilter::DigitalGlitchFilter(DigitalGlitchFilter&& rhs)
-    : ErrorBase(std::move(rhs)), SendableHelper(std::move(rhs)) {
-  std::swap(m_channelIndex, rhs.m_channelIndex);
-}
-
-DigitalGlitchFilter& DigitalGlitchFilter::operator=(DigitalGlitchFilter&& rhs) {
-  ErrorBase::operator=(std::move(rhs));
-  SendableHelper::operator=(std::move(rhs));
-
-  std::swap(m_channelIndex, rhs.m_channelIndex);
-
-  return *this;
-}
-
 void DigitalGlitchFilter::Add(DigitalSource* input) {
   DoAdd(input, m_channelIndex + 1);
 }
@@ -74,60 +55,50 @@
   if (input) {
     // We don't support GlitchFilters on AnalogTriggers.
     if (input->IsAnalogTrigger()) {
-      wpi_setErrorWithContext(
-          -1, "Analog Triggers not supported for DigitalGlitchFilters");
-      return;
+      throw FRC_MakeError(
+          -1, "{}", "Analog Triggers not supported for DigitalGlitchFilters");
     }
     int32_t status = 0;
     HAL_SetFilterSelect(input->GetPortHandleForRouting(), requestedIndex,
                         &status);
-    wpi_setHALError(status);
+    FRC_CheckErrorStatus(status, "requested index {}", requestedIndex);
 
     // Validate that we set it correctly.
     int actualIndex =
         HAL_GetFilterSelect(input->GetPortHandleForRouting(), &status);
-    wpi_assertEqual(actualIndex, requestedIndex);
+    FRC_CheckErrorStatus(status, "requested index {}", requestedIndex);
+    FRC_Assert(actualIndex == requestedIndex);
   }
 }
 
 void DigitalGlitchFilter::Add(Encoder* input) {
   Add(input->m_aSource.get());
-  if (StatusIsFatal()) {
-    return;
-  }
   Add(input->m_bSource.get());
 }
 
 void DigitalGlitchFilter::Add(Counter* input) {
   Add(input->m_upSource.get());
-  if (StatusIsFatal()) {
-    return;
-  }
   Add(input->m_downSource.get());
 }
 
-void DigitalGlitchFilter::Remove(DigitalSource* input) { DoAdd(input, 0); }
+void DigitalGlitchFilter::Remove(DigitalSource* input) {
+  DoAdd(input, 0);
+}
 
 void DigitalGlitchFilter::Remove(Encoder* input) {
   Remove(input->m_aSource.get());
-  if (StatusIsFatal()) {
-    return;
-  }
   Remove(input->m_bSource.get());
 }
 
 void DigitalGlitchFilter::Remove(Counter* input) {
   Remove(input->m_upSource.get());
-  if (StatusIsFatal()) {
-    return;
-  }
   Remove(input->m_downSource.get());
 }
 
 void DigitalGlitchFilter::SetPeriodCycles(int fpgaCycles) {
   int32_t status = 0;
   HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channelIndex);
 }
 
 void DigitalGlitchFilter::SetPeriodNanoSeconds(uint64_t nanoseconds) {
@@ -135,27 +106,22 @@
   int fpgaCycles =
       nanoseconds * HAL_GetSystemClockTicksPerMicrosecond() / 4 / 1000;
   HAL_SetFilterPeriod(m_channelIndex, fpgaCycles, &status);
-
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channelIndex);
 }
 
 int DigitalGlitchFilter::GetPeriodCycles() {
   int32_t status = 0;
   int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
-
-  wpi_setHALError(status);
-
+  FRC_CheckErrorStatus(status, "Channel {}", m_channelIndex);
   return fpgaCycles;
 }
 
 uint64_t DigitalGlitchFilter::GetPeriodNanoSeconds() {
   int32_t status = 0;
   int fpgaCycles = HAL_GetFilterPeriod(m_channelIndex, &status);
-
-  wpi_setHALError(status);
-
+  FRC_CheckErrorStatus(status, "Channel {}", m_channelIndex);
   return static_cast<uint64_t>(fpgaCycles) * 1000L /
          static_cast<uint64_t>(HAL_GetSystemClockTicksPerMicrosecond() / 4);
 }
 
-void DigitalGlitchFilter::InitSendable(SendableBuilder&) {}
+void DigitalGlitchFilter::InitSendable(wpi::SendableBuilder&) {}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalInput.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalInput.cpp
index d82f570..7bce921 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -1,77 +1,74 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DigitalInput.h"
 
+#include <iostream>
 #include <limits>
 
 #include <hal/DIO.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Ports.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 DigitalInput::DigitalInput(int channel) {
   if (!SensorUtil::CheckDigitalChannel(channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "Digital Channel " + wpi::Twine(channel));
-    m_channel = std::numeric_limits<int>::max();
-    return;
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
   }
   m_channel = channel;
 
   int32_t status = 0;
-  m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true, &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
-    m_handle = HAL_kInvalidHandle;
-    m_channel = std::numeric_limits<int>::max();
-    return;
-  }
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), true,
+                                   stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
 
   HAL_Report(HALUsageReporting::kResourceType_DigitalInput, channel + 1);
-  SendableRegistry::GetInstance().AddLW(this, "DigitalInput", channel);
+  wpi::SendableRegistry::AddLW(this, "DigitalInput", channel);
 }
 
 DigitalInput::~DigitalInput() {
-  if (StatusIsFatal()) return;
   HAL_FreeDIOPort(m_handle);
 }
 
 bool DigitalInput::Get() const {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetDIO(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return value;
 }
 
-HAL_Handle DigitalInput::GetPortHandleForRouting() const { return m_handle; }
-
-AnalogTriggerType DigitalInput::GetAnalogTriggerTypeForRouting() const {
-  return (AnalogTriggerType)0;
+HAL_Handle DigitalInput::GetPortHandleForRouting() const {
+  return m_handle;
 }
 
-bool DigitalInput::IsAnalogTrigger() const { return false; }
+AnalogTriggerType DigitalInput::GetAnalogTriggerTypeForRouting() const {
+  return static_cast<AnalogTriggerType>(0);
+}
+
+bool DigitalInput::IsAnalogTrigger() const {
+  return false;
+}
 
 void DigitalInput::SetSimDevice(HAL_SimDeviceHandle device) {
   HAL_SetDIOSimDevice(m_handle, device);
 }
 
-int DigitalInput::GetChannel() const { return m_channel; }
+int DigitalInput::GetChannel() const {
+  return m_channel;
+}
 
-void DigitalInput::InitSendable(SendableBuilder& builder) {
+void DigitalInput::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Digital Input");
   builder.AddBooleanProperty(
-      "Value", [=]() { return Get(); }, nullptr);
+      "Value", [=] { return Get(); }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalOutput.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalOutput.cpp
index 0bdc57c..5810e30 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalOutput.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DigitalOutput.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DigitalOutput.h"
 
@@ -13,146 +10,138 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Ports.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 DigitalOutput::DigitalOutput(int channel) {
   m_pwmGenerator = HAL_kInvalidHandle;
   if (!SensorUtil::CheckDigitalChannel(channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "Digital Channel " + wpi::Twine(channel));
-    m_channel = std::numeric_limits<int>::max();
-    return;
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
   }
   m_channel = channel;
 
   int32_t status = 0;
-  m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false, &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumDigitalChannels(), channel);
-    m_channel = std::numeric_limits<int>::max();
-    m_handle = HAL_kInvalidHandle;
-    return;
-  }
+  std::string stackTrace = wpi::GetStackTrace(1);
+  m_handle = HAL_InitializeDIOPort(HAL_GetPort(channel), false,
+                                   stackTrace.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
 
   HAL_Report(HALUsageReporting::kResourceType_DigitalOutput, channel + 1);
-  SendableRegistry::GetInstance().AddLW(this, "DigitalOutput", channel);
+  wpi::SendableRegistry::AddLW(this, "DigitalOutput", channel);
 }
 
 DigitalOutput::~DigitalOutput() {
-  if (StatusIsFatal()) return;
   // Disable the PWM in case it was running.
-  DisablePWM();
+  try {
+    DisablePWM();
+  } catch (const RuntimeError& e) {
+    e.Report();
+  }
 
   HAL_FreeDIOPort(m_handle);
 }
 
 void DigitalOutput::Set(bool value) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
   HAL_SetDIO(m_handle, value, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 bool DigitalOutput::Get() const {
-  if (StatusIsFatal()) return false;
-
   int32_t status = 0;
   bool val = HAL_GetDIO(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return val;
 }
 
-HAL_Handle DigitalOutput::GetPortHandleForRouting() const { return m_handle; }
-
-AnalogTriggerType DigitalOutput::GetAnalogTriggerTypeForRouting() const {
-  return (AnalogTriggerType)0;
+HAL_Handle DigitalOutput::GetPortHandleForRouting() const {
+  return m_handle;
 }
 
-bool DigitalOutput::IsAnalogTrigger() const { return false; }
+AnalogTriggerType DigitalOutput::GetAnalogTriggerTypeForRouting() const {
+  return static_cast<AnalogTriggerType>(0);
+}
 
-int DigitalOutput::GetChannel() const { return m_channel; }
+bool DigitalOutput::IsAnalogTrigger() const {
+  return false;
+}
+
+int DigitalOutput::GetChannel() const {
+  return m_channel;
+}
 
 void DigitalOutput::Pulse(double length) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
   HAL_Pulse(m_handle, length, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 bool DigitalOutput::IsPulsing() const {
-  if (StatusIsFatal()) return false;
-
   int32_t status = 0;
   bool value = HAL_IsPulsing(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return value;
 }
 
 void DigitalOutput::SetPWMRate(double rate) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
   HAL_SetDigitalPWMRate(rate, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void DigitalOutput::EnablePWM(double initialDutyCycle) {
-  if (m_pwmGenerator != HAL_kInvalidHandle) return;
+  if (m_pwmGenerator != HAL_kInvalidHandle) {
+    return;
+  }
 
   int32_t status = 0;
 
-  if (StatusIsFatal()) return;
   m_pwmGenerator = HAL_AllocateDigitalPWM(&status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 
-  if (StatusIsFatal()) return;
   HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, initialDutyCycle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 
-  if (StatusIsFatal()) return;
   HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, m_channel, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void DigitalOutput::DisablePWM() {
-  if (StatusIsFatal()) return;
-  if (m_pwmGenerator == HAL_kInvalidHandle) return;
+  if (m_pwmGenerator == HAL_kInvalidHandle) {
+    return;
+  }
 
   int32_t status = 0;
 
   // Disable the output by routing to a dead bit.
   HAL_SetDigitalPWMOutputChannel(m_pwmGenerator, SensorUtil::kDigitalChannels,
                                  &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 
   HAL_FreeDigitalPWM(m_pwmGenerator, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 
   m_pwmGenerator = HAL_kInvalidHandle;
 }
 
 void DigitalOutput::UpdateDutyCycle(double dutyCycle) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
   HAL_SetDigitalPWMDutyCycle(m_pwmGenerator, dutyCycle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void DigitalOutput::SetSimDevice(HAL_SimDeviceHandle device) {
   HAL_SetDIOSimDevice(m_handle, device);
 }
 
-void DigitalOutput::InitSendable(SendableBuilder& builder) {
+void DigitalOutput::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Digital Output");
   builder.AddBooleanProperty(
-      "Value", [=]() { return Get(); }, [=](bool value) { Set(value); });
+      "Value", [=] { return Get(); }, [=](bool value) { Set(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
index 67eb0b7..212673a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DoubleSolenoid.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DoubleSolenoid.h"
 
@@ -12,128 +9,92 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Ports.h>
-#include <hal/Solenoid.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
-DoubleSolenoid::DoubleSolenoid(int forwardChannel, int reverseChannel)
-    : DoubleSolenoid(SensorUtil::GetDefaultSolenoidModule(), forwardChannel,
-                     reverseChannel) {}
-
-DoubleSolenoid::DoubleSolenoid(int moduleNumber, int forwardChannel,
-                               int reverseChannel)
-    : SolenoidBase(moduleNumber),
-      m_forwardChannel(forwardChannel),
-      m_reverseChannel(reverseChannel) {
-  if (!SensorUtil::CheckSolenoidModule(m_moduleNumber)) {
-    wpi_setWPIErrorWithContext(ModuleIndexOutOfRange,
-                               "Solenoid Module " + wpi::Twine(m_moduleNumber));
-    return;
+DoubleSolenoid::DoubleSolenoid(int module, PneumaticsModuleType moduleType,
+                               int forwardChannel, int reverseChannel)
+    : m_module{PneumaticsBase::GetForType(module, moduleType)},
+      m_forwardChannel{forwardChannel},
+      m_reverseChannel{reverseChannel} {
+  if (!m_module->CheckSolenoidChannel(m_forwardChannel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}",
+                        m_forwardChannel);
   }
-  if (!SensorUtil::CheckSolenoidChannel(m_forwardChannel)) {
-    wpi_setWPIErrorWithContext(
-        ChannelIndexOutOfRange,
-        "Solenoid Channel " + wpi::Twine(m_forwardChannel));
-    return;
-  }
-  if (!SensorUtil::CheckSolenoidChannel(m_reverseChannel)) {
-    wpi_setWPIErrorWithContext(
-        ChannelIndexOutOfRange,
-        "Solenoid Channel " + wpi::Twine(m_reverseChannel));
-    return;
-  }
-  int32_t status = 0;
-  m_forwardHandle = HAL_InitializeSolenoidPort(
-      HAL_GetPortWithModule(moduleNumber, m_forwardChannel), &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
-                             forwardChannel);
-    m_forwardHandle = HAL_kInvalidHandle;
-    m_reverseHandle = HAL_kInvalidHandle;
-    return;
+  if (!m_module->CheckSolenoidChannel(m_reverseChannel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}",
+                        m_reverseChannel);
   }
 
-  m_reverseHandle = HAL_InitializeSolenoidPort(
-      HAL_GetPortWithModule(moduleNumber, m_reverseChannel), &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(),
-                             reverseChannel);
-    // free forward solenoid
-    HAL_FreeSolenoidPort(m_forwardHandle);
-    m_forwardHandle = HAL_kInvalidHandle;
-    m_reverseHandle = HAL_kInvalidHandle;
-    return;
-  }
+  m_forwardMask = 1 << forwardChannel;
+  m_reverseMask = 1 << reverseChannel;
+  m_mask = m_forwardMask | m_reverseMask;
 
-  m_forwardMask = 1 << m_forwardChannel;
-  m_reverseMask = 1 << m_reverseChannel;
+  int allocMask = m_module->CheckAndReserveSolenoids(m_mask);
+  if (allocMask != 0) {
+    if (allocMask == m_mask) {
+      throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channels {} and {}",
+                          m_forwardChannel, m_reverseChannel);
+    } else if (allocMask == m_forwardMask) {
+      throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}",
+                          m_forwardChannel);
+    } else {
+      throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}",
+                          m_reverseChannel);
+    }
+  }
 
   HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_forwardChannel + 1,
-             m_moduleNumber + 1);
+             m_module->GetModuleNumber() + 1);
   HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_reverseChannel + 1,
-             m_moduleNumber + 1);
+             m_module->GetModuleNumber() + 1);
 
-  SendableRegistry::GetInstance().AddLW(this, "DoubleSolenoid", m_moduleNumber,
-                                        m_forwardChannel);
+  wpi::SendableRegistry::AddLW(this, "DoubleSolenoid",
+                               m_module->GetModuleNumber(), m_forwardChannel);
 }
 
+DoubleSolenoid::DoubleSolenoid(PneumaticsModuleType moduleType,
+                               int forwardChannel, int reverseChannel)
+    : DoubleSolenoid{PneumaticsBase::GetDefaultForType(moduleType), moduleType,
+                     forwardChannel, reverseChannel} {}
+
 DoubleSolenoid::~DoubleSolenoid() {
-  HAL_FreeSolenoidPort(m_forwardHandle);
-  HAL_FreeSolenoidPort(m_reverseHandle);
+  m_module->UnreserveSolenoids(m_mask);
 }
 
 void DoubleSolenoid::Set(Value value) {
-  if (StatusIsFatal()) return;
-
-  bool forward = false;
-  bool reverse = false;
+  int setValue = 0;
 
   switch (value) {
     case kOff:
-      forward = false;
-      reverse = false;
+      setValue = 0;
       break;
     case kForward:
-      forward = true;
-      reverse = false;
+      setValue = m_forwardMask;
       break;
     case kReverse:
-      forward = false;
-      reverse = true;
+      setValue = m_reverseMask;
       break;
   }
 
-  int fstatus = 0;
-  HAL_SetSolenoid(m_forwardHandle, forward, &fstatus);
-  int rstatus = 0;
-  HAL_SetSolenoid(m_reverseHandle, reverse, &rstatus);
-
-  wpi_setHALError(fstatus);
-  wpi_setHALError(rstatus);
+  m_module->SetSolenoids(m_mask, setValue);
 }
 
 DoubleSolenoid::Value DoubleSolenoid::Get() const {
-  if (StatusIsFatal()) return kOff;
+  auto values = m_module->GetSolenoids();
 
-  int fstatus = 0;
-  int rstatus = 0;
-  bool valueForward = HAL_GetSolenoid(m_forwardHandle, &fstatus);
-  bool valueReverse = HAL_GetSolenoid(m_reverseHandle, &rstatus);
-
-  wpi_setHALError(fstatus);
-  wpi_setHALError(rstatus);
-
-  if (valueForward) {
-    return kForward;
-  } else if (valueReverse) {
-    return kReverse;
+  if ((values & m_forwardMask) != 0) {
+    return Value::kForward;
+  } else if ((values & m_reverseMask) != 0) {
+    return Value::kReverse;
   } else {
-    return kOff;
+    return Value::kOff;
   }
 }
 
@@ -147,23 +108,29 @@
   }
 }
 
-bool DoubleSolenoid::IsFwdSolenoidBlackListed() const {
-  int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
-  return (blackList & m_forwardMask) != 0;
+int DoubleSolenoid::GetFwdChannel() const {
+  return m_forwardChannel;
 }
 
-bool DoubleSolenoid::IsRevSolenoidBlackListed() const {
-  int blackList = GetPCMSolenoidBlackList(m_moduleNumber);
-  return (blackList & m_reverseMask) != 0;
+int DoubleSolenoid::GetRevChannel() const {
+  return m_reverseChannel;
 }
 
-void DoubleSolenoid::InitSendable(SendableBuilder& builder) {
+bool DoubleSolenoid::IsFwdSolenoidDisabled() const {
+  return (m_module->GetSolenoidDisabledList() & m_forwardMask) != 0;
+}
+
+bool DoubleSolenoid::IsRevSolenoidDisabled() const {
+  return (m_module->GetSolenoidDisabledList() & m_reverseMask) != 0;
+}
+
+void DoubleSolenoid::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Double Solenoid");
   builder.SetActuator(true);
-  builder.SetSafeState([=]() { Set(kOff); });
+  builder.SetSafeState([=] { Set(kOff); });
   builder.AddSmallStringProperty(
       "Value",
-      [=](wpi::SmallVectorImpl<char>& buf) -> wpi::StringRef {
+      [=](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
         switch (Get()) {
           case kForward:
             return "Forward";
@@ -173,12 +140,13 @@
             return "Off";
         }
       },
-      [=](wpi::StringRef value) {
+      [=](std::string_view value) {
         Value lvalue = kOff;
-        if (value == "Forward")
+        if (value == "Forward") {
           lvalue = kForward;
-        else if (value == "Reverse")
+        } else if (value == "Reverse") {
           lvalue = kReverse;
+        }
         Set(lvalue);
       });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DriverStation.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DriverStation.cpp
index dbb99f9..37fc65f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DriverStation.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DriverStation.cpp
@@ -1,69 +1,163 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DriverStation.h"
 
-#include <chrono>
+#include <stdint.h>
 
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <string>
+#include <string_view>
+#include <thread>
+#include <type_traits>
+
+#include <fmt/format.h>
 #include <hal/DriverStation.h>
+#include <hal/DriverStationTypes.h>
 #include <hal/HALBase.h>
 #include <hal/Power.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableInstance.h>
-#include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
 
+#include "frc/Errors.h"
 #include "frc/MotorSafety.h"
 #include "frc/Timer.h"
-#include "frc/WPIErrors.h"
-
-namespace frc {
-
-class MatchDataSender {
- public:
-  std::shared_ptr<nt::NetworkTable> table;
-  nt::NetworkTableEntry typeMetadata;
-  nt::NetworkTableEntry gameSpecificMessage;
-  nt::NetworkTableEntry eventName;
-  nt::NetworkTableEntry matchNumber;
-  nt::NetworkTableEntry replayNumber;
-  nt::NetworkTableEntry matchType;
-  nt::NetworkTableEntry alliance;
-  nt::NetworkTableEntry station;
-  nt::NetworkTableEntry controlWord;
-
-  MatchDataSender() {
-    table = nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
-    typeMetadata = table->GetEntry(".type");
-    typeMetadata.ForceSetString("FMSInfo");
-    gameSpecificMessage = table->GetEntry("GameSpecificMessage");
-    gameSpecificMessage.ForceSetString("");
-    eventName = table->GetEntry("EventName");
-    eventName.ForceSetString("");
-    matchNumber = table->GetEntry("MatchNumber");
-    matchNumber.ForceSetDouble(0);
-    replayNumber = table->GetEntry("ReplayNumber");
-    replayNumber.ForceSetDouble(0);
-    matchType = table->GetEntry("MatchType");
-    matchType.ForceSetDouble(0);
-    alliance = table->GetEntry("IsRedAlliance");
-    alliance.ForceSetBoolean(true);
-    station = table->GetEntry("StationNumber");
-    station.ForceSetDouble(1);
-    controlWord = table->GetEntry("FMSControlData");
-    controlWord.ForceSetDouble(0);
-  }
-};
-}  // namespace frc
 
 using namespace frc;
 
-static constexpr double kJoystickUnpluggedMessageInterval = 1.0;
+namespace {
+// A simple class which caches the previous value written to an NT entry
+// Used to prevent redundant, repeated writes of the same value
+template <class T>
+class MatchDataSenderEntry {
+ public:
+  MatchDataSenderEntry(const std::shared_ptr<nt::NetworkTable>& table,
+                       std::string_view key, const T& initialVal) {
+    static_assert(std::is_same_v<T, bool> || std::is_same_v<T, double> ||
+                      std::is_same_v<T, std::string>,
+                  "Invalid type for MatchDataSenderEntry - must be "
+                  "to bool, double or std::string");
+
+    ntEntry = table->GetEntry(key);
+    if constexpr (std::is_same_v<T, bool>) {
+      ntEntry.ForceSetBoolean(initialVal);
+    } else if constexpr (std::is_same_v<T, double>) {
+      ntEntry.ForceSetDouble(initialVal);
+    } else if constexpr (std::is_same_v<T, std::string>) {
+      ntEntry.ForceSetString(initialVal);
+    }
+    prevVal = initialVal;
+  }
+
+  void Set(const T& val) {
+    if (val != prevVal) {
+      SetValue(val);
+      prevVal = val;
+    }
+  }
+
+ private:
+  nt::NetworkTableEntry ntEntry;
+  T prevVal;
+
+  void SetValue(bool val) { ntEntry.SetBoolean(val); }
+  void SetValue(double val) { ntEntry.SetDouble(val); }
+  void SetValue(std::string_view val) { ntEntry.SetString(val); }
+};
+
+struct MatchDataSender {
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("FMSInfo");
+  MatchDataSenderEntry<std::string> typeMetaData{table, ".type", "FMSInfo"};
+  MatchDataSenderEntry<std::string> gameSpecificMessage{
+      table, "GameSpecificMessage", ""};
+  MatchDataSenderEntry<std::string> eventName{table, "EventName", ""};
+  MatchDataSenderEntry<double> matchNumber{table, "MatchNumber", 0.0};
+  MatchDataSenderEntry<double> replayNumber{table, "ReplayNumber", 0.0};
+  MatchDataSenderEntry<double> matchType{table, "MatchType", 0.0};
+  MatchDataSenderEntry<bool> alliance{table, "IsRedAlliance", true};
+  MatchDataSenderEntry<double> station{table, "StationNumber", 1.0};
+  MatchDataSenderEntry<double> controlWord{table, "FMSControlData", 0.0};
+};
+
+struct Instance {
+  Instance();
+  ~Instance();
+
+  MatchDataSender matchDataSender;
+
+  // Joystick button rising/falling edge flags
+  wpi::mutex buttonEdgeMutex;
+  std::array<HAL_JoystickButtons, DriverStation::kJoystickPorts>
+      previousButtonStates;
+  std::array<uint32_t, DriverStation::kJoystickPorts> joystickButtonsPressed;
+  std::array<uint32_t, DriverStation::kJoystickPorts> joystickButtonsReleased;
+
+  // Internal Driver Station thread
+  std::thread dsThread;
+  std::atomic<bool> isRunning{false};
+
+  mutable wpi::mutex waitForDataMutex;
+  wpi::condition_variable waitForDataCond;
+  int waitForDataCounter = 0;
+
+  bool silenceJoystickWarning = false;
+
+  // Robot state status variables
+  bool userInDisabled = false;
+  bool userInAutonomous = false;
+  bool userInTeleop = false;
+  bool userInTest = false;
+
+  units::second_t nextMessageTime = 0_s;
+};
+}  // namespace
+
+static constexpr auto kJoystickUnpluggedMessageInterval = 1_s;
+
+static Instance& GetInstance() {
+  static Instance instance;
+  return instance;
+}
+
+static void Run();
+static void SendMatchData();
+
+/**
+ * Reports errors related to unplugged joysticks.
+ *
+ * Throttles the errors so that they don't overwhelm the DS.
+ */
+static void ReportJoystickUnpluggedErrorV(fmt::string_view format,
+                                          fmt::format_args args);
+
+template <typename S, typename... Args>
+static inline void ReportJoystickUnpluggedError(const S& format,
+                                                Args&&... args) {
+  ReportJoystickUnpluggedErrorV(
+      format, fmt::make_args_checked<Args...>(format, args...));
+}
+
+/**
+ * Reports errors related to unplugged joysticks.
+ *
+ * Throttles the errors so that they don't overwhelm the DS.
+ */
+static void ReportJoystickUnpluggedWarningV(fmt::string_view format,
+                                            fmt::format_args args);
+
+template <typename S, typename... Args>
+static inline void ReportJoystickUnpluggedWarning(const S& format,
+                                                  Args&&... args) {
+  ReportJoystickUnpluggedWarningV(
+      format, fmt::make_args_checked<Args...>(format, args...));
+}
 
 static int& GetDSLastCount() {
   // There is a rollover error condition here. At Packet# = n * (uintmax), this
@@ -74,51 +168,42 @@
   return lastCount;
 }
 
-DriverStation::~DriverStation() {
-  m_isRunning = false;
+Instance::Instance() {
+  HAL_Initialize(500, 0);
+
+  // All joysticks should default to having zero axes, povs and buttons, so
+  // uninitialized memory doesn't get sent to motor controllers.
+  for (unsigned int i = 0; i < DriverStation::kJoystickPorts; i++) {
+    joystickButtonsPressed[i] = 0;
+    joystickButtonsReleased[i] = 0;
+    previousButtonStates[i].count = 0;
+    previousButtonStates[i].buttons = 0;
+  }
+
+  dsThread = std::thread(&Run);
+}
+
+Instance::~Instance() {
+  isRunning = false;
   // Trigger a DS mutex release in case there is no driver station running.
   HAL_ReleaseDSMutex();
-  m_dsThread.join();
+  dsThread.join();
 }
 
 DriverStation& DriverStation::GetInstance() {
+  ::GetInstance();
   static DriverStation instance;
   return instance;
 }
 
-void DriverStation::ReportError(const wpi::Twine& error) {
-  wpi::SmallString<128> temp;
-  HAL_SendError(1, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
-                1);
-}
-
-void DriverStation::ReportWarning(const wpi::Twine& error) {
-  wpi::SmallString<128> temp;
-  HAL_SendError(0, 1, 0, error.toNullTerminatedStringRef(temp).data(), "", "",
-                1);
-}
-
-void DriverStation::ReportError(bool isError, int32_t code,
-                                const wpi::Twine& error,
-                                const wpi::Twine& location,
-                                const wpi::Twine& stack) {
-  wpi::SmallString<128> errorTemp;
-  wpi::SmallString<128> locationTemp;
-  wpi::SmallString<128> stackTemp;
-  HAL_SendError(isError, code, 0,
-                error.toNullTerminatedStringRef(errorTemp).data(),
-                location.toNullTerminatedStringRef(locationTemp).data(),
-                stack.toNullTerminatedStringRef(stackTemp).data(), 1);
-}
-
 bool DriverStation::GetStickButton(int stick, int button) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return false;
   }
   if (button <= 0) {
     ReportJoystickUnpluggedError(
-        "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+        "Joystick Button {} index out of range; indexes begin at 1", button);
     return false;
   }
 
@@ -127,7 +212,9 @@
 
   if (button > buttons.count) {
     ReportJoystickUnpluggedWarning(
-        "Joystick Button missing, check if all controllers are plugged in");
+        "Joystick Button {} missing (max {}), check if all controllers are "
+        "plugged in",
+        button, buttons.count);
     return false;
   }
 
@@ -136,12 +223,12 @@
 
 bool DriverStation::GetStickButtonPressed(int stick, int button) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return false;
   }
   if (button <= 0) {
     ReportJoystickUnpluggedError(
-        "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+        "Joystick Button {} index out of range; indexes begin at 1", button);
     return false;
   }
 
@@ -150,27 +237,29 @@
 
   if (button > buttons.count) {
     ReportJoystickUnpluggedWarning(
-        "Joystick Button missing, check if all controllers are plugged in");
+        "Joystick Button {} missing (max {}), check if all controllers are "
+        "plugged in",
+        button, buttons.count);
     return false;
   }
-  std::unique_lock lock(m_buttonEdgeMutex);
+  auto& inst = ::GetInstance();
+  std::unique_lock lock(inst.buttonEdgeMutex);
   // If button was pressed, clear flag and return true
-  if (m_joystickButtonsPressed[stick] & 1 << (button - 1)) {
-    m_joystickButtonsPressed[stick] &= ~(1 << (button - 1));
+  if (inst.joystickButtonsPressed[stick] & 1 << (button - 1)) {
+    inst.joystickButtonsPressed[stick] &= ~(1 << (button - 1));
     return true;
-  } else {
-    return false;
   }
+  return false;
 }
 
 bool DriverStation::GetStickButtonReleased(int stick, int button) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return false;
   }
   if (button <= 0) {
     ReportJoystickUnpluggedError(
-        "ERROR: Button indexes begin at 1 in WPILib for C++ and Java");
+        "Joystick Button {} index out of range; indexes begin at 1", button);
     return false;
   }
 
@@ -179,26 +268,28 @@
 
   if (button > buttons.count) {
     ReportJoystickUnpluggedWarning(
-        "Joystick Button missing, check if all controllers are plugged in");
+        "Joystick Button {} missing (max {}), check if all controllers are "
+        "plugged in",
+        button, buttons.count);
     return false;
   }
-  std::unique_lock lock(m_buttonEdgeMutex);
+  auto& inst = ::GetInstance();
+  std::unique_lock lock(inst.buttonEdgeMutex);
   // If button was released, clear flag and return true
-  if (m_joystickButtonsReleased[stick] & 1 << (button - 1)) {
-    m_joystickButtonsReleased[stick] &= ~(1 << (button - 1));
+  if (inst.joystickButtonsReleased[stick] & 1 << (button - 1)) {
+    inst.joystickButtonsReleased[stick] &= ~(1 << (button - 1));
     return true;
-  } else {
-    return false;
   }
+  return false;
 }
 
 double DriverStation::GetStickAxis(int stick, int axis) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return 0.0;
   }
   if (axis < 0 || axis >= HAL_kMaxJoystickAxes) {
-    wpi_setWPIError(BadJoystickAxis);
+    FRC_ReportError(warn::BadJoystickAxis, "axis {} out of range", axis);
     return 0.0;
   }
 
@@ -207,7 +298,9 @@
 
   if (axis >= axes.count) {
     ReportJoystickUnpluggedWarning(
-        "Joystick Axis missing, check if all controllers are plugged in");
+        "Joystick Axis {} missing (max {}), check if all controllers are "
+        "plugged in",
+        axis, axes.count);
     return 0.0;
   }
 
@@ -216,11 +309,11 @@
 
 int DriverStation::GetStickPOV(int stick, int pov) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return -1;
   }
   if (pov < 0 || pov >= HAL_kMaxJoystickPOVs) {
-    wpi_setWPIError(BadJoystickAxis);
+    FRC_ReportError(warn::BadJoystickAxis, "POV {} out of range", pov);
     return -1;
   }
 
@@ -229,16 +322,18 @@
 
   if (pov >= povs.count) {
     ReportJoystickUnpluggedWarning(
-        "Joystick POV missing, check if all controllers are plugged in");
+        "Joystick POV {} missing (max {}), check if all controllers are "
+        "plugged in",
+        pov, povs.count);
     return -1;
   }
 
   return povs.povs[pov];
 }
 
-int DriverStation::GetStickButtons(int stick) const {
+int DriverStation::GetStickButtons(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return 0;
   }
 
@@ -248,9 +343,9 @@
   return buttons.buttons;
 }
 
-int DriverStation::GetStickAxisCount(int stick) const {
+int DriverStation::GetStickAxisCount(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return 0;
   }
 
@@ -260,9 +355,9 @@
   return axes.count;
 }
 
-int DriverStation::GetStickPOVCount(int stick) const {
+int DriverStation::GetStickPOVCount(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return 0;
   }
 
@@ -272,9 +367,9 @@
   return povs.count;
 }
 
-int DriverStation::GetStickButtonCount(int stick) const {
+int DriverStation::GetStickButtonCount(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return 0;
   }
 
@@ -284,9 +379,9 @@
   return buttons.count;
 }
 
-bool DriverStation::GetJoystickIsXbox(int stick) const {
+bool DriverStation::GetJoystickIsXbox(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return false;
   }
 
@@ -296,9 +391,9 @@
   return static_cast<bool>(descriptor.isXbox);
 }
 
-int DriverStation::GetJoystickType(int stick) const {
+int DriverStation::GetJoystickType(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return -1;
   }
 
@@ -308,9 +403,9 @@
   return static_cast<int>(descriptor.type);
 }
 
-std::string DriverStation::GetJoystickName(int stick) const {
+std::string DriverStation::GetJoystickName(int stick) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
   }
 
   HAL_JoystickDescriptor descriptor;
@@ -319,9 +414,9 @@
   return descriptor.name;
 }
 
-int DriverStation::GetJoystickAxisType(int stick, int axis) const {
+int DriverStation::GetJoystickAxisType(int stick, int axis) {
   if (stick < 0 || stick >= kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+    FRC_ReportError(warn::BadJoystickIndex, "stick {} out of range", stick);
     return -1;
   }
 
@@ -331,112 +426,123 @@
   return static_cast<bool>(descriptor.axisTypes);
 }
 
-bool DriverStation::IsJoystickConnected(int stick) const {
+bool DriverStation::IsJoystickConnected(int stick) {
   return GetStickAxisCount(stick) > 0 || GetStickButtonCount(stick) > 0 ||
          GetStickPOVCount(stick) > 0;
 }
 
-bool DriverStation::IsEnabled() const {
+bool DriverStation::IsEnabled() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.enabled && controlWord.dsAttached;
 }
 
-bool DriverStation::IsDisabled() const {
+bool DriverStation::IsDisabled() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return !(controlWord.enabled && controlWord.dsAttached);
 }
 
-bool DriverStation::IsEStopped() const {
+bool DriverStation::IsEStopped() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.eStop;
 }
 
-bool DriverStation::IsAutonomous() const {
+bool DriverStation::IsAutonomous() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.autonomous;
 }
 
-bool DriverStation::IsAutonomousEnabled() const {
+bool DriverStation::IsAutonomousEnabled() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.autonomous && controlWord.enabled;
 }
 
-bool DriverStation::IsOperatorControl() const {
+bool DriverStation::IsOperatorControl() {
+  return IsTeleop();
+}
+
+bool DriverStation::IsTeleop() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return !(controlWord.autonomous || controlWord.test);
 }
 
-bool DriverStation::IsOperatorControlEnabled() const {
+bool DriverStation::IsOperatorControlEnabled() {
+  return IsTeleopEnabled();
+}
+
+bool DriverStation::IsTeleopEnabled() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return !controlWord.autonomous && !controlWord.test && controlWord.enabled;
 }
 
-bool DriverStation::IsTest() const {
+bool DriverStation::IsTest() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.test;
 }
 
-bool DriverStation::IsDSAttached() const {
+bool DriverStation::IsDSAttached() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.dsAttached;
 }
 
-bool DriverStation::IsNewControlData() const {
-  std::unique_lock lock(m_waitForDataMutex);
+bool DriverStation::IsNewControlData() {
+  auto& inst = ::GetInstance();
+  std::unique_lock lock(inst.waitForDataMutex);
   int& lastCount = GetDSLastCount();
-  int currentCount = m_waitForDataCounter;
-  if (lastCount == currentCount) return false;
+  int currentCount = inst.waitForDataCounter;
+  if (lastCount == currentCount) {
+    return false;
+  }
   lastCount = currentCount;
   return true;
 }
 
-bool DriverStation::IsFMSAttached() const {
+bool DriverStation::IsFMSAttached() {
   HAL_ControlWord controlWord;
   HAL_GetControlWord(&controlWord);
   return controlWord.fmsAttached;
 }
 
-std::string DriverStation::GetGameSpecificMessage() const {
+std::string DriverStation::GetGameSpecificMessage() {
   HAL_MatchInfo info;
   HAL_GetMatchInfo(&info);
   return std::string(reinterpret_cast<char*>(info.gameSpecificMessage),
                      info.gameSpecificMessageSize);
 }
 
-std::string DriverStation::GetEventName() const {
+std::string DriverStation::GetEventName() {
   HAL_MatchInfo info;
   HAL_GetMatchInfo(&info);
   return info.eventName;
 }
 
-DriverStation::MatchType DriverStation::GetMatchType() const {
+DriverStation::MatchType DriverStation::GetMatchType() {
   HAL_MatchInfo info;
   HAL_GetMatchInfo(&info);
   return static_cast<DriverStation::MatchType>(info.matchType);
 }
 
-int DriverStation::GetMatchNumber() const {
+int DriverStation::GetMatchNumber() {
   HAL_MatchInfo info;
   HAL_GetMatchInfo(&info);
   return info.matchNumber;
 }
 
-int DriverStation::GetReplayNumber() const {
+int DriverStation::GetReplayNumber() {
   HAL_MatchInfo info;
   HAL_GetMatchInfo(&info);
   return info.replayNumber;
 }
 
-DriverStation::Alliance DriverStation::GetAlliance() const {
+DriverStation::Alliance DriverStation::GetAlliance() {
   int32_t status = 0;
   auto allianceStationID = HAL_GetAllianceStation(&status);
   switch (allianceStationID) {
@@ -453,7 +559,7 @@
   }
 }
 
-int DriverStation::GetLocation() const {
+int DriverStation::GetLocation() {
   int32_t status = 0;
   auto allianceStationID = HAL_GetAllianceStation(&status);
   switch (allianceStationID) {
@@ -471,143 +577,171 @@
   }
 }
 
-void DriverStation::WaitForData() { WaitForData(0); }
+void DriverStation::WaitForData() {
+  WaitForData(0_s);
+}
 
-bool DriverStation::WaitForData(double timeout) {
-  auto timeoutTime =
-      std::chrono::steady_clock::now() + std::chrono::duration<double>(timeout);
+bool DriverStation::WaitForData(units::second_t timeout) {
+  auto& inst = ::GetInstance();
+  auto timeoutTime = std::chrono::steady_clock::now() +
+                     std::chrono::steady_clock::duration{timeout};
 
-  std::unique_lock lock(m_waitForDataMutex);
+  std::unique_lock lock(inst.waitForDataMutex);
   int& lastCount = GetDSLastCount();
-  int currentCount = m_waitForDataCounter;
+  int currentCount = inst.waitForDataCounter;
   if (lastCount != currentCount) {
     lastCount = currentCount;
     return true;
   }
-  while (m_waitForDataCounter == currentCount) {
-    if (timeout > 0) {
-      auto timedOut = m_waitForDataCond.wait_until(lock, timeoutTime);
+  while (inst.waitForDataCounter == currentCount) {
+    if (timeout > 0_s) {
+      auto timedOut = inst.waitForDataCond.wait_until(lock, timeoutTime);
       if (timedOut == std::cv_status::timeout) {
         return false;
       }
     } else {
-      m_waitForDataCond.wait(lock);
+      inst.waitForDataCond.wait(lock);
     }
   }
-  lastCount = m_waitForDataCounter;
+  lastCount = inst.waitForDataCounter;
   return true;
 }
 
-double DriverStation::GetMatchTime() const {
-  int32_t status;
+double DriverStation::GetMatchTime() {
+  int32_t status = 0;
   return HAL_GetMatchTime(&status);
 }
 
-double DriverStation::GetBatteryVoltage() const {
+double DriverStation::GetBatteryVoltage() {
   int32_t status = 0;
   double voltage = HAL_GetVinVoltage(&status);
-  wpi_setErrorWithContext(status, "getVinVoltage");
+  FRC_CheckErrorStatus(status, "{}", "getVinVoltage");
 
   return voltage;
 }
 
-void DriverStation::WakeupWaitForData() {
-  std::scoped_lock waitLock(m_waitForDataMutex);
-  // Nofify all threads
-  m_waitForDataCounter++;
-  m_waitForDataCond.notify_all();
+void DriverStation::InDisabled(bool entering) {
+  ::GetInstance().userInDisabled = entering;
 }
 
-void DriverStation::GetData() {
+void DriverStation::InAutonomous(bool entering) {
+  ::GetInstance().userInAutonomous = entering;
+}
+
+void DriverStation::InOperatorControl(bool entering) {
+  InTeleop(entering);
+}
+
+void DriverStation::InTeleop(bool entering) {
+  ::GetInstance().userInTeleop = entering;
+}
+
+void DriverStation::InTest(bool entering) {
+  ::GetInstance().userInTest = entering;
+}
+
+void DriverStation::WakeupWaitForData() {
+  auto& inst = ::GetInstance();
+  std::scoped_lock waitLock(inst.waitForDataMutex);
+  // Nofify all threads
+  inst.waitForDataCounter++;
+  inst.waitForDataCond.notify_all();
+}
+
+/**
+ * Copy data from the DS task for the user.
+ *
+ * If no new data exists, it will just be returned, otherwise
+ * the data will be copied from the DS polling loop.
+ */
+void GetData() {
+  auto& inst = ::GetInstance();
   {
     // Compute the pressed and released buttons
     HAL_JoystickButtons currentButtons;
-    std::unique_lock lock(m_buttonEdgeMutex);
+    std::unique_lock lock(inst.buttonEdgeMutex);
 
-    for (int32_t i = 0; i < kJoystickPorts; i++) {
+    for (int32_t i = 0; i < DriverStation::kJoystickPorts; i++) {
       HAL_GetJoystickButtons(i, &currentButtons);
 
       // If buttons weren't pressed and are now, set flags in m_buttonsPressed
-      m_joystickButtonsPressed[i] |=
-          ~m_previousButtonStates[i].buttons & currentButtons.buttons;
+      inst.joystickButtonsPressed[i] |=
+          ~inst.previousButtonStates[i].buttons & currentButtons.buttons;
 
       // If buttons were pressed and aren't now, set flags in m_buttonsReleased
-      m_joystickButtonsReleased[i] |=
-          m_previousButtonStates[i].buttons & ~currentButtons.buttons;
+      inst.joystickButtonsReleased[i] |=
+          inst.previousButtonStates[i].buttons & ~currentButtons.buttons;
 
-      m_previousButtonStates[i] = currentButtons;
+      inst.previousButtonStates[i] = currentButtons;
     }
   }
 
-  WakeupWaitForData();
+  DriverStation::WakeupWaitForData();
   SendMatchData();
 }
 
 void DriverStation::SilenceJoystickConnectionWarning(bool silence) {
-  m_silenceJoystickWarning = silence;
+  ::GetInstance().silenceJoystickWarning = silence;
 }
 
-bool DriverStation::IsJoystickConnectionWarningSilenced() const {
-  return !IsFMSAttached() && m_silenceJoystickWarning;
+bool DriverStation::IsJoystickConnectionWarningSilenced() {
+  return !IsFMSAttached() && ::GetInstance().silenceJoystickWarning;
 }
 
-DriverStation::DriverStation() {
-  HAL_Initialize(500, 0);
-  m_waitForDataCounter = 0;
-
-  m_matchDataSender = std::make_unique<MatchDataSender>();
-
-  // All joysticks should default to having zero axes, povs and buttons, so
-  // uninitialized memory doesn't get sent to speed controllers.
-  for (unsigned int i = 0; i < kJoystickPorts; i++) {
-    m_joystickButtonsPressed[i] = 0;
-    m_joystickButtonsReleased[i] = 0;
-    m_previousButtonStates[i].count = 0;
-    m_previousButtonStates[i].buttons = 0;
-  }
-
-  m_dsThread = std::thread(&DriverStation::Run, this);
-}
-
-void DriverStation::ReportJoystickUnpluggedError(const wpi::Twine& message) {
-  double currentTime = Timer::GetFPGATimestamp();
-  if (currentTime > m_nextMessageTime) {
-    ReportError(message);
-    m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+void ReportJoystickUnpluggedErrorV(fmt::string_view format,
+                                   fmt::format_args args) {
+  auto& inst = GetInstance();
+  auto currentTime = Timer::GetFPGATimestamp();
+  if (currentTime > inst.nextMessageTime) {
+    ReportErrorV(err::Error, "", 0, "", format, args);
+    inst.nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
   }
 }
 
-void DriverStation::ReportJoystickUnpluggedWarning(const wpi::Twine& message) {
-  if (IsFMSAttached() || !m_silenceJoystickWarning) {
-    double currentTime = Timer::GetFPGATimestamp();
-    if (currentTime > m_nextMessageTime) {
-      ReportWarning(message);
-      m_nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
+void ReportJoystickUnpluggedWarningV(fmt::string_view format,
+                                     fmt::format_args args) {
+  auto& inst = GetInstance();
+  if (DriverStation::IsFMSAttached() || !inst.silenceJoystickWarning) {
+    auto currentTime = Timer::GetFPGATimestamp();
+    if (currentTime > inst.nextMessageTime) {
+      ReportErrorV(warn::Warning, "", 0, "", format, args);
+      inst.nextMessageTime = currentTime + kJoystickUnpluggedMessageInterval;
     }
   }
 }
 
-void DriverStation::Run() {
-  m_isRunning = true;
+void Run() {
+  auto& inst = GetInstance();
+  inst.isRunning = true;
   int safetyCounter = 0;
-  while (m_isRunning) {
+  while (inst.isRunning) {
     HAL_WaitForDSData();
     GetData();
 
-    if (IsDisabled()) safetyCounter = 0;
+    if (DriverStation::IsDisabled()) {
+      safetyCounter = 0;
+    }
 
     if (++safetyCounter >= 4) {
       MotorSafety::CheckMotors();
       safetyCounter = 0;
     }
-    if (m_userInDisabled) HAL_ObserveUserProgramDisabled();
-    if (m_userInAutonomous) HAL_ObserveUserProgramAutonomous();
-    if (m_userInTeleop) HAL_ObserveUserProgramTeleop();
-    if (m_userInTest) HAL_ObserveUserProgramTest();
+    if (inst.userInDisabled) {
+      HAL_ObserveUserProgramDisabled();
+    }
+    if (inst.userInAutonomous) {
+      HAL_ObserveUserProgramAutonomous();
+    }
+    if (inst.userInTeleop) {
+      HAL_ObserveUserProgramTeleop();
+    }
+    if (inst.userInTest) {
+      HAL_ObserveUserProgramTest();
+    }
   }
 }
 
-void DriverStation::SendMatchData() {
+void SendMatchData() {
   int32_t status = 0;
   HAL_AllianceStationID alliance = HAL_GetAllianceStation(&status);
   bool isRedAlliance = false;
@@ -642,20 +776,20 @@
   HAL_MatchInfo tmpDataStore;
   HAL_GetMatchInfo(&tmpDataStore);
 
-  m_matchDataSender->alliance.SetBoolean(isRedAlliance);
-  m_matchDataSender->station.SetDouble(stationNumber);
-  m_matchDataSender->eventName.SetString(tmpDataStore.eventName);
-  m_matchDataSender->gameSpecificMessage.SetString(
+  auto& inst = GetInstance();
+  inst.matchDataSender.alliance.Set(isRedAlliance);
+  inst.matchDataSender.station.Set(stationNumber);
+  inst.matchDataSender.eventName.Set(tmpDataStore.eventName);
+  inst.matchDataSender.gameSpecificMessage.Set(
       std::string(reinterpret_cast<char*>(tmpDataStore.gameSpecificMessage),
                   tmpDataStore.gameSpecificMessageSize));
-  m_matchDataSender->matchNumber.SetDouble(tmpDataStore.matchNumber);
-  m_matchDataSender->replayNumber.SetDouble(tmpDataStore.replayNumber);
-  m_matchDataSender->matchType.SetDouble(
-      static_cast<int>(tmpDataStore.matchType));
+  inst.matchDataSender.matchNumber.Set(tmpDataStore.matchNumber);
+  inst.matchDataSender.replayNumber.Set(tmpDataStore.replayNumber);
+  inst.matchDataSender.matchType.Set(static_cast<int>(tmpDataStore.matchType));
 
   HAL_ControlWord ctlWord;
   HAL_GetControlWord(&ctlWord);
   int32_t wordInt = 0;
   std::memcpy(&wordInt, &ctlWord, sizeof(wordInt));
-  m_matchDataSender->controlWord.SetDouble(wordInt);
+  inst.matchDataSender.controlWord.Set(wordInt);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycle.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycle.cpp
index 7e4e98d..a8375e0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycle.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycle.cpp
@@ -1,46 +1,43 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DutyCycle.h"
 
 #include <hal/DutyCycle.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
 
-#include "frc/Base.h"
 #include "frc/DigitalSource.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
 DutyCycle::DutyCycle(DigitalSource* source)
-    : m_source{source, NullDeleter<DigitalSource>()} {
-  if (m_source == nullptr) {
-    wpi_setWPIError(NullParameter);
-  } else {
-    InitDutyCycle();
+    : m_source{source, wpi::NullDeleter<DigitalSource>()} {
+  if (!m_source) {
+    throw FRC_MakeError(err::NullParameter, "{}", "source");
   }
+  InitDutyCycle();
 }
 
 DutyCycle::DutyCycle(DigitalSource& source)
-    : m_source{&source, NullDeleter<DigitalSource>()} {
+    : m_source{&source, wpi::NullDeleter<DigitalSource>()} {
   InitDutyCycle();
 }
 
 DutyCycle::DutyCycle(std::shared_ptr<DigitalSource> source)
     : m_source{std::move(source)} {
-  if (m_source == nullptr) {
-    wpi_setWPIError(NullParameter);
-  } else {
-    InitDutyCycle();
+  if (!m_source) {
+    throw FRC_MakeError(err::NullParameter, "{}", "source");
   }
+  InitDutyCycle();
 }
 
-DutyCycle::~DutyCycle() { HAL_FreeDutyCycle(m_handle); }
+DutyCycle::~DutyCycle() {
+  HAL_FreeDutyCycle(m_handle);
+}
 
 void DutyCycle::InitDutyCycle() {
   int32_t status = 0;
@@ -49,50 +46,52 @@
                               static_cast<HAL_AnalogTriggerType>(
                                   m_source->GetAnalogTriggerTypeForRouting()),
                               &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   int index = GetFPGAIndex();
   HAL_Report(HALUsageReporting::kResourceType_DutyCycle, index + 1);
-  SendableRegistry::GetInstance().AddLW(this, "Duty Cycle", index);
+  wpi::SendableRegistry::AddLW(this, "Duty Cycle", index);
 }
 
 int DutyCycle::GetFPGAIndex() const {
   int32_t status = 0;
   auto retVal = HAL_GetDutyCycleFPGAIndex(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return retVal;
 }
 
 int DutyCycle::GetFrequency() const {
   int32_t status = 0;
   auto retVal = HAL_GetDutyCycleFrequency(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return retVal;
 }
 
 double DutyCycle::GetOutput() const {
   int32_t status = 0;
   auto retVal = HAL_GetDutyCycleOutput(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return retVal;
 }
 
 unsigned int DutyCycle::GetOutputRaw() const {
   int32_t status = 0;
   auto retVal = HAL_GetDutyCycleOutputRaw(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return retVal;
 }
 
 unsigned int DutyCycle::GetOutputScaleFactor() const {
   int32_t status = 0;
   auto retVal = HAL_GetDutyCycleOutputScaleFactor(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", GetSourceChannel());
   return retVal;
 }
 
-int DutyCycle::GetSourceChannel() const { return m_source->GetChannel(); }
+int DutyCycle::GetSourceChannel() const {
+  return m_source->GetChannel();
+}
 
-void DutyCycle::InitSendable(SendableBuilder& builder) {
+void DutyCycle::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Duty Cycle");
   builder.AddDoubleProperty(
       "Frequency", [this] { return this->GetFrequency(); }, nullptr);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index 4fc5457..8e994e9 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -1,19 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DutyCycleEncoder.h"
 
-#include "frc/Base.h"
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalSource.h"
-#include "frc/DriverStation.h"
 #include "frc/DutyCycle.h"
-#include "frc/smartdashboard/SendableBuilder.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -24,12 +22,12 @@
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle& dutyCycle)
-    : m_dutyCycle{&dutyCycle, NullDeleter<DutyCycle>{}} {
+    : m_dutyCycle{&dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
   Init();
 }
 
 DutyCycleEncoder::DutyCycleEncoder(DutyCycle* dutyCycle)
-    : m_dutyCycle{dutyCycle, NullDeleter<DutyCycle>{}} {
+    : m_dutyCycle{dutyCycle, wpi::NullDeleter<DutyCycle>{}} {
   Init();
 }
 
@@ -54,13 +52,16 @@
 }
 
 void DutyCycleEncoder::Init() {
-  m_simDevice = hal::SimDevice{"DutyCycleEncoder", m_dutyCycle->GetFPGAIndex()};
+  m_simDevice = hal::SimDevice{"DutyCycle:DutyCycleEncoder",
+                               m_dutyCycle->GetSourceChannel()};
 
   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);
+    m_simPosition =
+        m_simDevice.CreateDouble("position", hal::SimDevice::kInput, 0.0);
+    m_simDistancePerRotation = m_simDevice.CreateDouble(
+        "distance_per_rot", hal::SimDevice::kOutput, 1.0);
+    m_simIsConnected =
+        m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
   } else {
     m_analogTrigger = std::make_unique<AnalogTrigger>(m_dutyCycle.get());
     m_analogTrigger->SetLimitsDutyCycle(0.25, 0.75);
@@ -71,12 +72,14 @@
         m_analogTrigger->CreateOutput(AnalogTriggerType::kFallingPulse));
   }
 
-  SendableRegistry::GetInstance().AddLW(this, "DutyCycle Encoder",
-                                        m_dutyCycle->GetSourceChannel());
+  wpi::SendableRegistry::AddLW(this, "DutyCycle Encoder",
+                               m_dutyCycle->GetSourceChannel());
 }
 
 units::turn_t DutyCycleEncoder::Get() const {
-  if (m_simPosition) return units::turn_t{m_simPosition.Get()};
+  if (m_simPosition) {
+    return units::turn_t{m_simPosition.Get()};
+  }
 
   // As the values are not atomic, keep trying until we get 2 reads of the same
   // value If we don't within 10 attempts, error
@@ -92,7 +95,8 @@
     }
   }
 
-  frc::DriverStation::GetInstance().ReportWarning(
+  FRC_ReportError(
+      warn::Warning, "{}",
       "Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning "
       "last value");
   return m_lastPosition;
@@ -108,7 +112,7 @@
 }
 
 double DutyCycleEncoder::GetDistance() const {
-  return Get().to<double>() * GetDistancePerRotation();
+  return Get().value() * GetDistancePerRotation();
 }
 
 int DutyCycleEncoder::GetFrequency() const {
@@ -116,12 +120,16 @@
 }
 
 void DutyCycleEncoder::Reset() {
-  if (m_counter) m_counter->Reset();
+  if (m_counter) {
+    m_counter->Reset();
+  }
   m_positionOffset = m_dutyCycle->GetOutput();
 }
 
 bool DutyCycleEncoder::IsConnected() const {
-  if (m_simIsConnected) return m_simIsConnected.Get();
+  if (m_simIsConnected) {
+    return m_simIsConnected.Get();
+  }
   return GetFrequency() > m_frequencyThreshold;
 }
 
@@ -140,7 +148,7 @@
   return m_dutyCycle->GetSourceChannel();
 }
 
-void DutyCycleEncoder::InitSendable(SendableBuilder& builder) {
+void DutyCycleEncoder::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("AbsoluteEncoder");
   builder.AddDoubleProperty(
       "Distance", [this] { return this->GetDistance(); }, nullptr);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Encoder.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Encoder.cpp
index 87bb45f..bef6d76 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Encoder.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Encoder.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Encoder.h"
 
@@ -11,12 +8,12 @@
 
 #include <hal/Encoder.h>
 #include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/Base.h"
 #include "frc/DigitalInput.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -25,196 +22,177 @@
   m_aSource = std::make_shared<DigitalInput>(aChannel);
   m_bSource = std::make_shared<DigitalInput>(bChannel);
   InitEncoder(reverseDirection, encodingType);
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, m_aSource.get());
-  registry.AddChild(this, m_bSource.get());
+  wpi::SendableRegistry::AddChild(this, m_aSource.get());
+  wpi::SendableRegistry::AddChild(this, m_bSource.get());
 }
 
 Encoder::Encoder(DigitalSource* aSource, DigitalSource* bSource,
                  bool reverseDirection, EncodingType encodingType)
-    : m_aSource(aSource, NullDeleter<DigitalSource>()),
-      m_bSource(bSource, NullDeleter<DigitalSource>()) {
-  if (m_aSource == nullptr || m_bSource == nullptr)
-    wpi_setWPIError(NullParameter);
-  else
-    InitEncoder(reverseDirection, encodingType);
+    : m_aSource(aSource, wpi::NullDeleter<DigitalSource>()),
+      m_bSource(bSource, wpi::NullDeleter<DigitalSource>()) {
+  if (!m_aSource) {
+    throw FRC_MakeError(err::NullParameter, "{}", "aSource");
+  }
+  if (!m_bSource) {
+    throw FRC_MakeError(err::NullParameter, "{}", "bSource");
+  }
+  InitEncoder(reverseDirection, encodingType);
 }
 
 Encoder::Encoder(DigitalSource& aSource, DigitalSource& bSource,
                  bool reverseDirection, EncodingType encodingType)
-    : m_aSource(&aSource, NullDeleter<DigitalSource>()),
-      m_bSource(&bSource, NullDeleter<DigitalSource>()) {
+    : m_aSource(&aSource, wpi::NullDeleter<DigitalSource>()),
+      m_bSource(&bSource, wpi::NullDeleter<DigitalSource>()) {
   InitEncoder(reverseDirection, encodingType);
 }
 
 Encoder::Encoder(std::shared_ptr<DigitalSource> aSource,
                  std::shared_ptr<DigitalSource> bSource, bool reverseDirection,
                  EncodingType encodingType)
-    : m_aSource(aSource), m_bSource(bSource) {
-  if (m_aSource == nullptr || m_bSource == nullptr)
-    wpi_setWPIError(NullParameter);
-  else
-    InitEncoder(reverseDirection, encodingType);
+    : m_aSource(std::move(aSource)), m_bSource(std::move(bSource)) {
+  if (!m_aSource) {
+    throw FRC_MakeError(err::NullParameter, "{}", "aSource");
+  }
+  if (!m_bSource) {
+    throw FRC_MakeError(err::NullParameter, "{}", "bSource");
+  }
+  InitEncoder(reverseDirection, encodingType);
 }
 
 Encoder::~Encoder() {
   int32_t status = 0;
   HAL_FreeEncoder(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_ReportError(status, "{}", "FreeEncoder");
 }
 
 int Encoder::Get() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetEncoder(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Get");
   return value;
 }
 
 void Encoder::Reset() {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_ResetEncoder(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Reset");
 }
 
-double Encoder::GetPeriod() const {
-  if (StatusIsFatal()) return 0.0;
+units::second_t Encoder::GetPeriod() const {
   int32_t status = 0;
   double value = HAL_GetEncoderPeriod(m_encoder, &status);
-  wpi_setHALError(status);
-  return value;
+  FRC_CheckErrorStatus(status, "{}", "GetPeriod");
+  return units::second_t{value};
 }
 
-void Encoder::SetMaxPeriod(double maxPeriod) {
-  if (StatusIsFatal()) return;
+void Encoder::SetMaxPeriod(units::second_t maxPeriod) {
   int32_t status = 0;
-  HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod, &status);
-  wpi_setHALError(status);
+  HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod.value(), &status);
+  FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod");
 }
 
 bool Encoder::GetStopped() const {
-  if (StatusIsFatal()) return true;
   int32_t status = 0;
   bool value = HAL_GetEncoderStopped(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetStopped");
   return value;
 }
 
 bool Encoder::GetDirection() const {
-  if (StatusIsFatal()) return false;
   int32_t status = 0;
   bool value = HAL_GetEncoderDirection(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetDirection");
   return value;
 }
 
 int Encoder::GetRaw() const {
-  if (StatusIsFatal()) return 0;
   int32_t status = 0;
   int value = HAL_GetEncoderRaw(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetRaw");
   return value;
 }
 
 int Encoder::GetEncodingScale() const {
   int32_t status = 0;
   int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetEncodingScale");
   return val;
 }
 
 double Encoder::GetDistance() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetEncoderDistance(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetDistance");
   return value;
 }
 
 double Encoder::GetRate() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double value = HAL_GetEncoderRate(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetRate");
   return value;
 }
 
 void Encoder::SetMinRate(double minRate) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderMinRate(m_encoder, minRate, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetMinRate");
 }
 
 void Encoder::SetDistancePerPulse(double distancePerPulse) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetDistancePerPulse");
 }
 
 double Encoder::GetDistancePerPulse() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetDistancePerPulse");
   return distancePerPulse;
 }
 
 void Encoder::SetReverseDirection(bool reverseDirection) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetReverseDirection");
 }
 
 void Encoder::SetSamplesToAverage(int samplesToAverage) {
   if (samplesToAverage < 1 || samplesToAverage > 127) {
-    wpi_setWPIErrorWithContext(
-        ParameterOutOfRange,
-        "Average counter values must be between 1 and 127");
-    return;
+    throw FRC_MakeError(
+        err::ParameterOutOfRange,
+        "Average counter values must be between 1 and 127, got {}",
+        samplesToAverage);
   }
   int32_t status = 0;
   HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetSamplesToAverage");
 }
 
 int Encoder::GetSamplesToAverage() const {
   int32_t status = 0;
   int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetSamplesToAverage");
   return result;
 }
 
-double Encoder::PIDGet() {
-  if (StatusIsFatal()) return 0.0;
-  switch (GetPIDSourceType()) {
-    case PIDSourceType::kDisplacement:
-      return GetDistance();
-    case PIDSourceType::kRate:
-      return GetRate();
-    default:
-      return 0.0;
-  }
-}
-
 void Encoder::SetIndexSource(int channel, Encoder::IndexingType type) {
   // Force digital input if just given an index
   m_indexSource = std::make_shared<DigitalInput>(channel);
-  SendableRegistry::GetInstance().AddChild(this, m_indexSource.get());
+  wpi::SendableRegistry::AddChild(this, m_indexSource.get());
   SetIndexSource(*m_indexSource.get(), type);
 }
 
 void Encoder::SetIndexSource(const DigitalSource& source,
                              Encoder::IndexingType type) {
   int32_t status = 0;
-  HAL_SetEncoderIndexSource(
-      m_encoder, source.GetPortHandleForRouting(),
-      (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(),
-      (HAL_EncoderIndexingType)type, &status);
-  wpi_setHALError(status);
+  HAL_SetEncoderIndexSource(m_encoder, source.GetPortHandleForRouting(),
+                            static_cast<HAL_AnalogTriggerType>(
+                                source.GetAnalogTriggerTypeForRouting()),
+                            static_cast<HAL_EncoderIndexingType>(type),
+                            &status);
+  FRC_CheckErrorStatus(status, "{}", "SetIndexSource");
 }
 
 void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
@@ -224,47 +202,49 @@
 int Encoder::GetFPGAIndex() const {
   int32_t status = 0;
   int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFPGAIndex");
   return val;
 }
 
-void Encoder::InitSendable(SendableBuilder& builder) {
+void Encoder::InitSendable(wpi::SendableBuilder& builder) {
   int32_t status = 0;
   HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
-  wpi_setHALError(status);
-  if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X)
+  FRC_CheckErrorStatus(status, "{}", "GetEncodingType");
+  if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X) {
     builder.SetSmartDashboardType("Quadrature Encoder");
-  else
+  } else {
     builder.SetSmartDashboardType("Encoder");
+  }
 
   builder.AddDoubleProperty(
-      "Speed", [=]() { return GetRate(); }, nullptr);
+      "Speed", [=] { return GetRate(); }, nullptr);
   builder.AddDoubleProperty(
-      "Distance", [=]() { return GetDistance(); }, nullptr);
+      "Distance", [=] { return GetDistance(); }, nullptr);
   builder.AddDoubleProperty(
-      "Distance per Tick", [=]() { return GetDistancePerPulse(); }, nullptr);
+      "Distance per Tick", [=] { return GetDistancePerPulse(); }, nullptr);
 }
 
 void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
   int32_t status = 0;
   m_encoder = HAL_InitializeEncoder(
       m_aSource->GetPortHandleForRouting(),
-      (HAL_AnalogTriggerType)m_aSource->GetAnalogTriggerTypeForRouting(),
+      static_cast<HAL_AnalogTriggerType>(
+          m_aSource->GetAnalogTriggerTypeForRouting()),
       m_bSource->GetPortHandleForRouting(),
-      (HAL_AnalogTriggerType)m_bSource->GetAnalogTriggerTypeForRouting(),
-      reverseDirection, (HAL_EncoderEncodingType)encodingType, &status);
-  wpi_setHALError(status);
+      static_cast<HAL_AnalogTriggerType>(
+          m_bSource->GetAnalogTriggerTypeForRouting()),
+      reverseDirection, static_cast<HAL_EncoderEncodingType>(encodingType),
+      &status);
+  FRC_CheckErrorStatus(status, "{}", "InitEncoder");
 
   HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
              encodingType);
-  SendableRegistry::GetInstance().AddLW(this, "Encoder",
-                                        m_aSource->GetChannel());
+  wpi::SendableRegistry::AddLW(this, "Encoder", m_aSource->GetChannel());
 }
 
 double Encoder::DecodingScaleFactor() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "DecodingScaleFactor");
   return val;
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Error.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Error.cpp
deleted file mode 100644
index 5e072c9..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Error.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Error.h"
-
-#include <wpi/Path.h>
-#include <wpi/StackTrace.h>
-
-#include "frc/Base.h"
-#include "frc/DriverStation.h"
-#include "frc/Timer.h"
-
-using namespace frc;
-
-Error::Error(Code code, const wpi::Twine& contextMessage,
-             wpi::StringRef filename, wpi::StringRef function, int lineNumber,
-             const ErrorBase* originatingObject) {
-  Set(code, contextMessage, filename, function, lineNumber, originatingObject);
-}
-
-bool Error::operator<(const Error& rhs) const {
-  if (m_code < rhs.m_code) {
-    return true;
-  } else if (m_message < rhs.m_message) {
-    return true;
-  } else if (m_filename < rhs.m_filename) {
-    return true;
-  } else if (m_function < rhs.m_function) {
-    return true;
-  } else if (m_lineNumber < rhs.m_lineNumber) {
-    return true;
-  } else if (m_originatingObject < rhs.m_originatingObject) {
-    return true;
-  } else if (m_timestamp < rhs.m_timestamp) {
-    return true;
-  } else {
-    return false;
-  }
-}
-
-Error::Code Error::GetCode() const { return m_code; }
-
-std::string Error::GetMessage() const { return m_message; }
-
-std::string Error::GetFilename() const { return m_filename; }
-
-std::string Error::GetFunction() const { return m_function; }
-
-int Error::GetLineNumber() const { return m_lineNumber; }
-
-const ErrorBase* Error::GetOriginatingObject() const {
-  return m_originatingObject;
-}
-
-double Error::GetTimestamp() const { return m_timestamp; }
-
-void Error::Set(Code code, const wpi::Twine& contextMessage,
-                wpi::StringRef filename, wpi::StringRef function,
-                int lineNumber, const ErrorBase* originatingObject) {
-  bool report = true;
-
-  if (code == m_code && GetTime() - m_timestamp < 1) {
-    report = false;
-  }
-
-  m_code = code;
-  m_message = contextMessage.str();
-  m_filename = filename;
-  m_function = function;
-  m_lineNumber = lineNumber;
-  m_originatingObject = originatingObject;
-
-  if (report) {
-    m_timestamp = GetTime();
-    Report();
-  }
-}
-
-void Error::Report() {
-  DriverStation::ReportError(
-      true, m_code, m_message,
-      m_function + wpi::Twine(" [") + wpi::sys::path::filename(m_filename) +
-          wpi::Twine(':') + wpi::Twine(m_lineNumber) + wpi::Twine(']'),
-      wpi::GetStackTrace(4));
-}
-
-void Error::Clear() {
-  m_code = 0;
-  m_message = "";
-  m_filename = "";
-  m_function = "";
-  m_lineNumber = 0;
-  m_originatingObject = nullptr;
-  m_timestamp = 0.0;
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ErrorBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ErrorBase.cpp
deleted file mode 100644
index 8c7c5a2..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ErrorBase.cpp
+++ /dev/null
@@ -1,187 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/ErrorBase.h"
-
-#include <cerrno>
-#include <cstring>
-#include <set>
-#include <utility>
-
-#include <hal/HALBase.h>
-#include <wpi/Format.h>
-#include <wpi/SmallString.h>
-#include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
-
-#include "frc/Base.h"
-
-using namespace frc;
-
-namespace {
-struct GlobalErrors {
-  wpi::mutex mutex;
-  std::set<Error> errors;
-  const Error* lastError{nullptr};
-
-  static GlobalErrors& GetInstance();
-  static void Insert(const Error& error);
-  static void Insert(Error&& error);
-};
-}  // namespace
-
-GlobalErrors& GlobalErrors::GetInstance() {
-  static GlobalErrors inst;
-  return inst;
-}
-
-void GlobalErrors::Insert(const Error& error) {
-  GlobalErrors& inst = GetInstance();
-  std::scoped_lock lock(inst.mutex);
-  inst.lastError = &(*inst.errors.insert(error).first);
-}
-
-void GlobalErrors::Insert(Error&& error) {
-  GlobalErrors& inst = GetInstance();
-  std::scoped_lock lock(inst.mutex);
-  inst.lastError = &(*inst.errors.insert(std::move(error)).first);
-}
-
-ErrorBase::ErrorBase() { HAL_Initialize(500, 0); }
-
-Error& ErrorBase::GetError() { return m_error; }
-
-const Error& ErrorBase::GetError() const { return m_error; }
-
-void ErrorBase::ClearError() const { m_error.Clear(); }
-
-void ErrorBase::SetErrnoError(const wpi::Twine& contextMessage,
-                              wpi::StringRef filename, wpi::StringRef function,
-                              int lineNumber) const {
-  wpi::SmallString<128> buf;
-  wpi::raw_svector_ostream err(buf);
-  int errNo = errno;
-  if (errNo == 0) {
-    err << "OK: ";
-  } else {
-    err << std::strerror(errNo) << " (" << wpi::format_hex(errNo, 10, true)
-        << "): ";
-  }
-
-  // Set the current error information for this object.
-  m_error.Set(-1, err.str() + contextMessage, filename, function, lineNumber,
-              this);
-
-  // Update the global error if there is not one already set.
-  GlobalErrors::Insert(m_error);
-}
-
-void ErrorBase::SetImaqError(int success, const wpi::Twine& contextMessage,
-                             wpi::StringRef filename, wpi::StringRef function,
-                             int lineNumber) const {
-  // If there was an error
-  if (success <= 0) {
-    // Set the current error information for this object.
-    m_error.Set(success, wpi::Twine(success) + ": " + contextMessage, filename,
-                function, lineNumber, this);
-
-    // Update the global error if there is not one already set.
-    GlobalErrors::Insert(m_error);
-  }
-}
-
-void ErrorBase::SetError(Error::Code code, const wpi::Twine& contextMessage,
-                         wpi::StringRef filename, wpi::StringRef function,
-                         int lineNumber) const {
-  //  If there was an error
-  if (code != 0) {
-    //  Set the current error information for this object.
-    m_error.Set(code, contextMessage, filename, function, lineNumber, this);
-
-    // Update the global error if there is not one already set.
-    GlobalErrors::Insert(m_error);
-  }
-}
-
-void ErrorBase::SetErrorRange(Error::Code code, int32_t minRange,
-                              int32_t maxRange, int32_t requestedValue,
-                              const wpi::Twine& contextMessage,
-                              wpi::StringRef filename, wpi::StringRef function,
-                              int lineNumber) const {
-  //  If there was an error
-  if (code != 0) {
-    //  Set the current error information for this object.
-    m_error.Set(code,
-                contextMessage + ", Minimum Value: " + wpi::Twine(minRange) +
-                    ", MaximumValue: " + wpi::Twine(maxRange) +
-                    ", Requested Value: " + wpi::Twine(requestedValue),
-                filename, function, lineNumber, this);
-
-    // Update the global error if there is not one already set.
-    GlobalErrors::Insert(m_error);
-  }
-}
-
-void ErrorBase::SetWPIError(const wpi::Twine& errorMessage, Error::Code code,
-                            const wpi::Twine& contextMessage,
-                            wpi::StringRef filename, wpi::StringRef function,
-                            int lineNumber) const {
-  //  Set the current error information for this object.
-  m_error.Set(code, errorMessage + ": " + contextMessage, filename, function,
-              lineNumber, this);
-
-  // Update the global error if there is not one already set.
-  GlobalErrors::Insert(m_error);
-}
-
-void ErrorBase::CloneError(const ErrorBase& rhs) const {
-  m_error = rhs.GetError();
-}
-
-bool ErrorBase::StatusIsFatal() const { return m_error.GetCode() < 0; }
-
-void ErrorBase::SetGlobalError(Error::Code code,
-                               const wpi::Twine& contextMessage,
-                               wpi::StringRef filename, wpi::StringRef function,
-                               int lineNumber) {
-  // If there was an error
-  if (code != 0) {
-    // Set the current error information for this object.
-    GlobalErrors::Insert(
-        Error(code, contextMessage, filename, function, lineNumber, nullptr));
-  }
-}
-
-void ErrorBase::SetGlobalWPIError(const wpi::Twine& errorMessage,
-                                  const wpi::Twine& contextMessage,
-                                  wpi::StringRef filename,
-                                  wpi::StringRef function, int lineNumber) {
-  GlobalErrors::Insert(Error(-1, errorMessage + ": " + contextMessage, filename,
-                             function, lineNumber, nullptr));
-}
-
-Error ErrorBase::GetGlobalError() {
-  auto& inst = GlobalErrors::GetInstance();
-  std::scoped_lock mutex(inst.mutex);
-  if (!inst.lastError) return Error{};
-  return *inst.lastError;
-}
-
-std::vector<Error> ErrorBase::GetGlobalErrors() {
-  auto& inst = GlobalErrors::GetInstance();
-  std::scoped_lock mutex(inst.mutex);
-  std::vector<Error> rv;
-  for (auto&& error : inst.errors) rv.push_back(error);
-  return rv;
-}
-
-void ErrorBase::ClearGlobalErrors() {
-  auto& inst = GlobalErrors::GetInstance();
-  std::scoped_lock mutex(inst.mutex);
-  inst.errors.clear();
-  inst.lastError = nullptr;
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Errors.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Errors.cpp
new file mode 100644
index 0000000..0ff4dc1
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Errors.cpp
@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Errors.h"
+
+#include <exception>
+
+#include <hal/DriverStation.h>
+#include <hal/HALBase.h>
+#include <wpi/StackTrace.h>
+#include <wpi/fs.h>
+
+using namespace frc;
+
+RuntimeError::RuntimeError(int32_t code, std::string&& loc, std::string&& stack,
+                           std::string&& message)
+    : runtime_error{std::move(message)}, m_data{std::make_shared<Data>()} {
+  m_data->code = code;
+  m_data->loc = std::move(loc);
+  m_data->stack = stack;
+}
+
+RuntimeError::RuntimeError(int32_t code, const char* fileName, int lineNumber,
+                           const char* funcName, std::string&& stack,
+                           std::string&& message)
+    : RuntimeError{
+          code,
+          fmt::format("{} [{}:{}]", funcName,
+                      fs::path{fileName}.filename().string(), lineNumber),
+          std::move(stack), std::move(message)} {}
+
+void RuntimeError::Report() const {
+  HAL_SendError(m_data->code < 0, m_data->code, 0, what(), m_data->loc.c_str(),
+                m_data->stack.c_str(), 1);
+}
+
+const char* frc::GetErrorMessage(int32_t* code) {
+  switch (*code) {
+#define S(label, offset, message) \
+  case err::label:                \
+    return message;
+#include "frc/WPIErrors.mac"
+#undef S
+#define S(label, offset, message) \
+  case warn::label:               \
+    return message;
+#include "frc/WPIWarnings.mac"
+#undef S
+    default:
+      return HAL_GetLastError(code);
+  }
+}
+
+void frc::ReportErrorV(int32_t status, const char* fileName, int lineNumber,
+                       const char* funcName, fmt::string_view format,
+                       fmt::format_args args) {
+  if (status == 0) {
+    return;
+  }
+  fmt::memory_buffer out;
+  fmt::format_to(fmt::appender{out}, "{}: ", GetErrorMessage(&status));
+  fmt::vformat_to(fmt::appender{out}, format, args);
+  out.push_back('\0');
+  HAL_SendError(status < 0, status, 0, out.data(), funcName,
+                wpi::GetStackTrace(2).c_str(), 1);
+}
+
+RuntimeError frc::MakeErrorV(int32_t status, const char* fileName,
+                             int lineNumber, const char* funcName,
+                             fmt::string_view format, fmt::format_args args) {
+  fmt::memory_buffer out;
+  fmt::format_to(fmt::appender{out}, "{}: ", GetErrorMessage(&status));
+  fmt::vformat_to(fmt::appender{out}, format, args);
+  return RuntimeError{status,
+                      fileName,
+                      lineNumber,
+                      funcName,
+                      wpi::GetStackTrace(2),
+                      fmt::to_string(out)};
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Filesystem.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Filesystem.cpp
index 7d2ea1d..d497779 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Filesystem.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Filesystem.cpp
@@ -1,37 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Filesystem.h"
 
-#include <wpi/FileSystem.h>
-#include <wpi/Path.h>
+#include <wpi/fs.h>
 
 #include "frc/RobotBase.h"
 
-void frc::filesystem::GetLaunchDirectory(wpi::SmallVectorImpl<char>& result) {
-  wpi::sys::fs::current_path(result);
+std::string frc::filesystem::GetLaunchDirectory() {
+  return fs::current_path().string();
 }
 
-void frc::filesystem::GetOperatingDirectory(
-    wpi::SmallVectorImpl<char>& result) {
+std::string frc::filesystem::GetOperatingDirectory() {
   if constexpr (RobotBase::IsReal()) {
-    wpi::sys::path::native("/home/lvuser", result);
+    return "/home/lvuser";
   } else {
-    frc::filesystem::GetLaunchDirectory(result);
+    return frc::filesystem::GetLaunchDirectory();
   }
 }
 
-void frc::filesystem::GetDeployDirectory(wpi::SmallVectorImpl<char>& result) {
-  frc::filesystem::GetOperatingDirectory(result);
+std::string frc::filesystem::GetDeployDirectory() {
   if constexpr (RobotBase::IsReal()) {
-    wpi::sys::path::append(result, "deploy");
+    return "/home/lvuser/deploy";
   } else {
-    wpi::sys::path::append(result, "src");
-    wpi::sys::path::append(result, "main");
-    wpi::sys::path::append(result, "deploy");
+    return (fs::current_path() / "src" / "main" / "deploy").string();
   }
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/GearTooth.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/GearTooth.cpp
deleted file mode 100644
index 5fb5021..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/GearTooth.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/GearTooth.h"
-
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-constexpr double GearTooth::kGearToothThreshold;
-
-GearTooth::GearTooth(int channel, bool directionSensitive) : Counter(channel) {
-  EnableDirectionSensing(directionSensitive);
-  SendableRegistry::GetInstance().SetName(this, "GearTooth", channel);
-}
-
-GearTooth::GearTooth(DigitalSource* source, bool directionSensitive)
-    : Counter(source) {
-  EnableDirectionSensing(directionSensitive);
-  SendableRegistry::GetInstance().SetName(this, "GearTooth",
-                                          source->GetChannel());
-}
-
-GearTooth::GearTooth(std::shared_ptr<DigitalSource> source,
-                     bool directionSensitive)
-    : Counter(source) {
-  EnableDirectionSensing(directionSensitive);
-  SendableRegistry::GetInstance().SetName(this, "GearTooth",
-                                          source->GetChannel());
-}
-
-void GearTooth::EnableDirectionSensing(bool directionSensitive) {
-  if (directionSensitive) {
-    SetPulseLengthMode(kGearToothThreshold);
-  }
-}
-
-void GearTooth::InitSendable(SendableBuilder& builder) {
-  Counter::InitSendable(builder);
-  builder.SetSmartDashboardType("Gear Tooth");
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/GenericHID.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/GenericHID.cpp
index 4a264c8..6c186eb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/GenericHID.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/GenericHID.cpp
@@ -1,69 +1,74 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/GenericHID.h"
 
 #include <hal/DriverStation.h>
 
 #include "frc/DriverStation.h"
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
-GenericHID::GenericHID(int port) : m_ds(&DriverStation::GetInstance()) {
-  if (port >= DriverStation::kJoystickPorts) {
-    wpi_setWPIError(BadJoystickIndex);
+GenericHID::GenericHID(int port) {
+  if (port < 0 || port >= DriverStation::kJoystickPorts) {
+    throw FRC_MakeError(warn::BadJoystickIndex, "port {} out of range", port);
   }
   m_port = port;
 }
 
 bool GenericHID::GetRawButton(int button) const {
-  return m_ds->GetStickButton(m_port, button);
+  return DriverStation::GetStickButton(m_port, button);
 }
 
 bool GenericHID::GetRawButtonPressed(int button) {
-  return m_ds->GetStickButtonPressed(m_port, button);
+  return DriverStation::GetStickButtonPressed(m_port, button);
 }
 
 bool GenericHID::GetRawButtonReleased(int button) {
-  return m_ds->GetStickButtonReleased(m_port, button);
+  return DriverStation::GetStickButtonReleased(m_port, button);
 }
 
 double GenericHID::GetRawAxis(int axis) const {
-  return m_ds->GetStickAxis(m_port, axis);
+  return DriverStation::GetStickAxis(m_port, axis);
 }
 
-int GenericHID::GetPOV(int pov) const { return m_ds->GetStickPOV(m_port, pov); }
+int GenericHID::GetPOV(int pov) const {
+  return DriverStation::GetStickPOV(m_port, pov);
+}
 
-int GenericHID::GetAxisCount() const { return m_ds->GetStickAxisCount(m_port); }
+int GenericHID::GetAxisCount() const {
+  return DriverStation::GetStickAxisCount(m_port);
+}
 
-int GenericHID::GetPOVCount() const { return m_ds->GetStickPOVCount(m_port); }
+int GenericHID::GetPOVCount() const {
+  return DriverStation::GetStickPOVCount(m_port);
+}
 
 int GenericHID::GetButtonCount() const {
-  return m_ds->GetStickButtonCount(m_port);
+  return DriverStation::GetStickButtonCount(m_port);
 }
 
 bool GenericHID::IsConnected() const {
-  return m_ds->IsJoystickConnected(m_port);
+  return DriverStation::IsJoystickConnected(m_port);
 }
 
 GenericHID::HIDType GenericHID::GetType() const {
-  return static_cast<HIDType>(m_ds->GetJoystickType(m_port));
+  return static_cast<HIDType>(DriverStation::GetJoystickType(m_port));
 }
 
 std::string GenericHID::GetName() const {
-  return m_ds->GetJoystickName(m_port);
+  return DriverStation::GetJoystickName(m_port);
 }
 
 int GenericHID::GetAxisType(int axis) const {
-  return m_ds->GetJoystickAxisType(m_port, axis);
+  return DriverStation::GetJoystickAxisType(m_port, axis);
 }
 
-int GenericHID::GetPort() const { return m_port; }
+int GenericHID::GetPort() const {
+  return m_port;
+}
 
 void GenericHID::SetOutput(int outputNumber, bool value) {
   m_outputs =
@@ -78,10 +83,11 @@
 }
 
 void GenericHID::SetRumble(RumbleType type, double value) {
-  if (value < 0)
+  if (value < 0) {
     value = 0;
-  else if (value > 1)
+  } else if (value > 1) {
     value = 1;
+  }
   if (type == kLeftRumble) {
     m_leftRumble = value * 65535;
   } else {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/GyroBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/GyroBase.cpp
deleted file mode 100644
index ba9b2f0..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/GyroBase.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/GyroBase.h"
-
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-double GyroBase::PIDGet() {
-  switch (GetPIDSourceType()) {
-    case PIDSourceType::kRate:
-      return GetRate();
-    case PIDSourceType::kDisplacement:
-      return GetAngle();
-    default:
-      return 0;
-  }
-}
-
-void GyroBase::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Gyro");
-  builder.AddDoubleProperty(
-      "Value", [=]() { return GetAngle(); }, nullptr);
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp
index 21d92f2..adf1c54 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/I2C.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/I2C.h"
 
@@ -12,7 +9,7 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/I2C.h>
 
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -20,23 +17,34 @@
     : m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
   int32_t status = 0;
   HAL_InitializeI2C(m_port, &status);
-  // wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", port);
 
   HAL_Report(HALUsageReporting::kResourceType_I2C, deviceAddress);
 }
 
-I2C::~I2C() { HAL_CloseI2C(m_port); }
+I2C::~I2C() {
+  HAL_CloseI2C(m_port);
+}
+
+I2C::Port I2C::GetPort() const {
+  return static_cast<Port>(static_cast<int>(m_port));
+}
+
+int I2C::GetDeviceAddress() const {
+  return m_deviceAddress;
+}
 
 bool I2C::Transaction(uint8_t* dataToSend, int sendSize, uint8_t* dataReceived,
                       int receiveSize) {
   int32_t status = 0;
   status = HAL_TransactionI2C(m_port, m_deviceAddress, dataToSend, sendSize,
                               dataReceived, receiveSize);
-  // wpi_setHALError(status);
   return status < 0;
 }
 
-bool I2C::AddressOnly() { return Transaction(nullptr, 0, nullptr, 0); }
+bool I2C::AddressOnly() {
+  return Transaction(nullptr, 0, nullptr, 0);
+}
 
 bool I2C::Write(int registerAddress, uint8_t data) {
   uint8_t buffer[2];
@@ -55,12 +63,10 @@
 
 bool I2C::Read(int registerAddress, int count, uint8_t* buffer) {
   if (count < 1) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
-    return true;
+    throw FRC_MakeError(err::ParameterOutOfRange, "count {}", count);
   }
-  if (buffer == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "buffer");
-    return true;
+  if (!buffer) {
+    throw FRC_MakeError(err::NullParameter, "{}", "buffer");
   }
   uint8_t regAddr = registerAddress;
   return Transaction(&regAddr, sizeof(regAddr), buffer, count);
@@ -68,12 +74,10 @@
 
 bool I2C::ReadOnly(int count, uint8_t* buffer) {
   if (count < 1) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange, "count");
-    return true;
+    throw FRC_MakeError(err::ParameterOutOfRange, "count {}", count);
   }
-  if (buffer == nullptr) {
-    wpi_setWPIErrorWithContext(NullParameter, "buffer");
-    return true;
+  if (!buffer) {
+    throw FRC_MakeError(err::NullParameter, "{}", "buffer");
   }
   return HAL_ReadI2C(m_port, m_deviceAddress, buffer, count) < 0;
 }
@@ -86,10 +90,14 @@
        i += 4, curRegisterAddress += 4) {
     int toRead = count - i < 4 ? count - i : 4;
     // Read the chunk of data.  Return false if the sensor does not respond.
-    if (Read(curRegisterAddress, toRead, deviceData)) return false;
+    if (Read(curRegisterAddress, toRead, deviceData)) {
+      return false;
+    }
 
     for (int j = 0; j < toRead; j++) {
-      if (deviceData[j] != expected[i + j]) return false;
+      if (deviceData[j] != expected[i + j]) {
+        return false;
+      }
     }
   }
   return true;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
deleted file mode 100644
index 744e349..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/InterruptableSensorBase.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/InterruptableSensorBase.h"
-
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
-
-using namespace frc;
-
-InterruptableSensorBase::~InterruptableSensorBase() {
-  if (m_interrupt == HAL_kInvalidHandle) return;
-  int32_t status = 0;
-  HAL_CleanInterrupts(m_interrupt, &status);
-  // Ignore status, as an invalid handle just needs to be ignored.
-}
-
-void InterruptableSensorBase::RequestInterrupts(
-    HAL_InterruptHandlerFunction handler, void* param) {
-  if (StatusIsFatal()) return;
-
-  wpi_assert(m_interrupt == HAL_kInvalidHandle);
-  AllocateInterrupts(false);
-  if (StatusIsFatal()) return;  // if allocate failed, out of interrupts
-
-  int32_t status = 0;
-  HAL_RequestInterrupts(
-      m_interrupt, GetPortHandleForRouting(),
-      static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
-      &status);
-  SetUpSourceEdge(true, false);
-  HAL_AttachInterruptHandler(m_interrupt, handler, param, &status);
-  wpi_setHALError(status);
-}
-
-void InterruptableSensorBase::RequestInterrupts(InterruptEventHandler handler) {
-  if (StatusIsFatal()) return;
-
-  wpi_assert(m_interrupt == HAL_kInvalidHandle);
-  AllocateInterrupts(false);
-  if (StatusIsFatal()) return;  // if allocate failed, out of interrupts
-
-  m_interruptHandler =
-      std::make_unique<InterruptEventHandler>(std::move(handler));
-
-  int32_t status = 0;
-  HAL_RequestInterrupts(
-      m_interrupt, GetPortHandleForRouting(),
-      static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
-      &status);
-  SetUpSourceEdge(true, false);
-  HAL_AttachInterruptHandler(
-      m_interrupt,
-      [](uint32_t mask, void* param) {
-        auto self = reinterpret_cast<InterruptEventHandler*>(param);
-        // Rising edge result is the interrupt bit set in the byte 0xFF
-        // Falling edge result is the interrupt bit set in the byte 0xFF00
-        // Set any bit set to be true for that edge, and AND the 2 results
-        // together to match the existing enum for all interrupts
-        int32_t rising = (mask & 0xFF) ? 0x1 : 0x0;
-        int32_t falling = ((mask & 0xFF00) ? 0x0100 : 0x0);
-        WaitResult res = static_cast<WaitResult>(falling | rising);
-        (*self)(res);
-      },
-      m_interruptHandler.get(), &status);
-  wpi_setHALError(status);
-}
-
-void InterruptableSensorBase::RequestInterrupts() {
-  if (StatusIsFatal()) return;
-
-  wpi_assert(m_interrupt == HAL_kInvalidHandle);
-  AllocateInterrupts(true);
-  if (StatusIsFatal()) return;  // if allocate failed, out of interrupts
-
-  int32_t status = 0;
-  HAL_RequestInterrupts(
-      m_interrupt, GetPortHandleForRouting(),
-      static_cast<HAL_AnalogTriggerType>(GetAnalogTriggerTypeForRouting()),
-      &status);
-  wpi_setHALError(status);
-  SetUpSourceEdge(true, false);
-}
-
-void InterruptableSensorBase::CancelInterrupts() {
-  if (StatusIsFatal()) return;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  HAL_CleanInterrupts(m_interrupt, &status);
-  // Ignore status, as an invalid handle just needs to be ignored.
-  m_interrupt = HAL_kInvalidHandle;
-  m_interruptHandler = nullptr;
-}
-
-InterruptableSensorBase::WaitResult InterruptableSensorBase::WaitForInterrupt(
-    double timeout, bool ignorePrevious) {
-  if (StatusIsFatal()) return InterruptableSensorBase::kTimeout;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  int result;
-
-  result = HAL_WaitForInterrupt(m_interrupt, timeout, ignorePrevious, &status);
-  wpi_setHALError(status);
-
-  // Rising edge result is the interrupt bit set in the byte 0xFF
-  // Falling edge result is the interrupt bit set in the byte 0xFF00
-  // Set any bit set to be true for that edge, and AND the 2 results
-  // together to match the existing enum for all interrupts
-  int32_t rising = (result & 0xFF) ? 0x1 : 0x0;
-  int32_t falling = ((result & 0xFF00) ? 0x0100 : 0x0);
-  return static_cast<WaitResult>(falling | rising);
-}
-
-void InterruptableSensorBase::EnableInterrupts() {
-  if (StatusIsFatal()) return;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  HAL_EnableInterrupts(m_interrupt, &status);
-  wpi_setHALError(status);
-}
-
-void InterruptableSensorBase::DisableInterrupts() {
-  if (StatusIsFatal()) return;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  HAL_DisableInterrupts(m_interrupt, &status);
-  wpi_setHALError(status);
-}
-
-double InterruptableSensorBase::ReadRisingTimestamp() {
-  if (StatusIsFatal()) return 0.0;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  int64_t timestamp = HAL_ReadInterruptRisingTimestamp(m_interrupt, &status);
-  wpi_setHALError(status);
-  return timestamp * 1e-6;
-}
-
-double InterruptableSensorBase::ReadFallingTimestamp() {
-  if (StatusIsFatal()) return 0.0;
-  wpi_assert(m_interrupt != HAL_kInvalidHandle);
-  int32_t status = 0;
-  int64_t timestamp = HAL_ReadInterruptFallingTimestamp(m_interrupt, &status);
-  wpi_setHALError(status);
-  return timestamp * 1e-6;
-}
-
-void InterruptableSensorBase::SetUpSourceEdge(bool risingEdge,
-                                              bool fallingEdge) {
-  if (StatusIsFatal()) return;
-  if (m_interrupt == HAL_kInvalidHandle) {
-    wpi_setWPIErrorWithContext(
-        NullParameter,
-        "You must call RequestInterrupts before SetUpSourceEdge");
-    return;
-  }
-  if (m_interrupt != HAL_kInvalidHandle) {
-    int32_t status = 0;
-    HAL_SetInterruptUpSourceEdge(m_interrupt, risingEdge, fallingEdge, &status);
-    wpi_setHALError(status);
-  }
-}
-
-void InterruptableSensorBase::AllocateInterrupts(bool watcher) {
-  wpi_assert(m_interrupt == HAL_kInvalidHandle);
-  // Expects the calling leaf class to allocate an interrupt index.
-  int32_t status = 0;
-  m_interrupt = HAL_InitializeInterrupts(watcher, &status);
-  wpi_setHALError(status);
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobot.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobot.cpp
deleted file mode 100644
index 72fb79b..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobot.cpp
+++ /dev/null
@@ -1,47 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/IterativeRobot.h"
-
-#include <hal/DriverStation.h>
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/DriverStation.h"
-
-using namespace frc;
-
-static constexpr auto kPacketPeriod = 0.02_s;
-
-IterativeRobot::IterativeRobot() : IterativeRobotBase(kPacketPeriod) {
-  HAL_Report(HALUsageReporting::kResourceType_Framework,
-             HALUsageReporting::kFramework_Iterative);
-}
-
-void IterativeRobot::StartCompetition() {
-  RobotInit();
-
-  if constexpr (IsSimulation()) {
-    SimulationInit();
-  }
-
-  // Tell the DS that the robot is ready to be enabled
-  HAL_ObserveUserProgramStarting();
-
-  // Loop forever, calling the appropriate mode-dependent function
-  while (true) {
-    // Wait for driver station data so the loop doesn't hog the CPU
-    DriverStation::GetInstance().WaitForData();
-    if (m_exit) break;
-
-    LoopFunc();
-  }
-}
-
-void IterativeRobot::EndCompetition() {
-  m_exit = true;
-  DriverStation::GetInstance().WakeupWaitForData();
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
index 7b29130..47eb299 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/IterativeRobotBase.cpp
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/IterativeRobotBase.h"
 
+#include <fmt/format.h>
 #include <hal/DriverStation.h>
-#include <wpi/Format.h>
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
+#include <networktables/NetworkTableInstance.h>
 
-#include "frc/DriverStation.h"
+#include "frc/DSControlWord.h"
+#include "frc/Errors.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/shuffleboard/Shuffleboard.h"
 #include "frc/smartdashboard/SmartDashboard.h"
@@ -26,34 +23,22 @@
     : m_period(period),
       m_watchdog(period, [this] { PrintLoopOverrunMessage(); }) {}
 
-void IterativeRobotBase::RobotInit() {
-  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
+void IterativeRobotBase::RobotInit() {}
 
-void IterativeRobotBase::SimulationInit() {
-  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
+void IterativeRobotBase::SimulationInit() {}
 
-void IterativeRobotBase::DisabledInit() {
-  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
+void IterativeRobotBase::DisabledInit() {}
 
-void IterativeRobotBase::AutonomousInit() {
-  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
+void IterativeRobotBase::AutonomousInit() {}
 
-void IterativeRobotBase::TeleopInit() {
-  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
+void IterativeRobotBase::TeleopInit() {}
 
-void IterativeRobotBase::TestInit() {
-  wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
-}
+void IterativeRobotBase::TestInit() {}
 
 void IterativeRobotBase::RobotPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
-    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    fmt::print("Default {}() method... Override me!\n", __FUNCTION__);
     firstRun = false;
   }
 }
@@ -61,7 +46,7 @@
 void IterativeRobotBase::SimulationPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
-    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    fmt::print("Default {}() method... Override me!\n", __FUNCTION__);
     firstRun = false;
   }
 }
@@ -69,7 +54,7 @@
 void IterativeRobotBase::DisabledPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
-    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    fmt::print("Default {}() method... Override me!\n", __FUNCTION__);
     firstRun = false;
   }
 }
@@ -77,7 +62,7 @@
 void IterativeRobotBase::AutonomousPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
-    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    fmt::print("Default {}() method... Override me!\n", __FUNCTION__);
     firstRun = false;
   }
 }
@@ -85,7 +70,7 @@
 void IterativeRobotBase::TeleopPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
-    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    fmt::print("Default {}() method... Override me!\n", __FUNCTION__);
     firstRun = false;
   }
 }
@@ -93,68 +78,92 @@
 void IterativeRobotBase::TestPeriodic() {
   static bool firstRun = true;
   if (firstRun) {
-    wpi::outs() << "Default " << __FUNCTION__ << "() method... Override me!\n";
+    fmt::print("Default {}() method... Override me!\n", __FUNCTION__);
     firstRun = false;
   }
 }
 
+void IterativeRobotBase::DisabledExit() {}
+
+void IterativeRobotBase::AutonomousExit() {}
+
+void IterativeRobotBase::TeleopExit() {}
+
+void IterativeRobotBase::TestExit() {}
+
+void IterativeRobotBase::SetNetworkTablesFlushEnabled(bool enabled) {
+  m_ntFlushEnabled = enabled;
+}
+
+units::second_t IterativeRobotBase::GetPeriod() const {
+  return m_period;
+}
+
 void IterativeRobotBase::LoopFunc() {
   m_watchdog.Reset();
 
-  // Call the appropriate function depending upon the current robot mode
-  if (IsDisabled()) {
-    // Call DisabledInit() if we are now just entering disabled mode from
-    // either a different mode or from power-on.
-    if (m_lastMode != Mode::kDisabled) {
-      LiveWindow::GetInstance()->SetEnabled(false);
+  // Get current mode
+  DSControlWord word;
+  Mode mode = Mode::kNone;
+  if (word.IsDisabled()) {
+    mode = Mode::kDisabled;
+  } else if (word.IsAutonomous()) {
+    mode = Mode::kAutonomous;
+  } else if (word.IsTeleop()) {
+    mode = Mode::kTeleop;
+  } else if (word.IsTest()) {
+    mode = Mode::kTest;
+  }
+
+  // If mode changed, call mode exit and entry functions
+  if (m_lastMode != mode) {
+    // Call last mode's exit function
+    if (m_lastMode == Mode::kDisabled) {
+      DisabledExit();
+    } else if (m_lastMode == Mode::kAutonomous) {
+      AutonomousExit();
+    } else if (m_lastMode == Mode::kTeleop) {
+      TeleopExit();
+    } else if (m_lastMode == Mode::kTest) {
+      LiveWindow::SetEnabled(false);
       Shuffleboard::DisableActuatorWidgets();
+      TestExit();
+    }
+
+    // Call current mode's entry function
+    if (mode == Mode::kDisabled) {
       DisabledInit();
       m_watchdog.AddEpoch("DisabledInit()");
-      m_lastMode = Mode::kDisabled;
-    }
-
-    HAL_ObserveUserProgramDisabled();
-    DisabledPeriodic();
-    m_watchdog.AddEpoch("DisabledPeriodic()");
-  } else if (IsAutonomous()) {
-    // Call AutonomousInit() if we are now just entering autonomous mode from
-    // either a different mode or from power-on.
-    if (m_lastMode != Mode::kAutonomous) {
-      LiveWindow::GetInstance()->SetEnabled(false);
-      Shuffleboard::DisableActuatorWidgets();
+    } else if (mode == Mode::kAutonomous) {
       AutonomousInit();
       m_watchdog.AddEpoch("AutonomousInit()");
-      m_lastMode = Mode::kAutonomous;
-    }
-
-    HAL_ObserveUserProgramAutonomous();
-    AutonomousPeriodic();
-    m_watchdog.AddEpoch("AutonomousPeriodic()");
-  } else if (IsOperatorControl()) {
-    // Call TeleopInit() if we are now just entering teleop mode from
-    // either a different mode or from power-on.
-    if (m_lastMode != Mode::kTeleop) {
-      LiveWindow::GetInstance()->SetEnabled(false);
-      Shuffleboard::DisableActuatorWidgets();
+    } else if (mode == Mode::kTeleop) {
       TeleopInit();
       m_watchdog.AddEpoch("TeleopInit()");
-      m_lastMode = Mode::kTeleop;
-    }
-
-    HAL_ObserveUserProgramTeleop();
-    TeleopPeriodic();
-    m_watchdog.AddEpoch("TeleopPeriodic()");
-  } else {
-    // Call TestInit() if we are now just entering test mode from
-    // either a different mode or from power-on.
-    if (m_lastMode != Mode::kTest) {
-      LiveWindow::GetInstance()->SetEnabled(true);
+    } else if (mode == Mode::kTest) {
+      LiveWindow::SetEnabled(true);
       Shuffleboard::EnableActuatorWidgets();
       TestInit();
       m_watchdog.AddEpoch("TestInit()");
-      m_lastMode = Mode::kTest;
     }
 
+    m_lastMode = mode;
+  }
+
+  // Call the appropriate function depending upon the current robot mode
+  if (mode == Mode::kDisabled) {
+    HAL_ObserveUserProgramDisabled();
+    DisabledPeriodic();
+    m_watchdog.AddEpoch("DisabledPeriodic()");
+  } else if (mode == Mode::kAutonomous) {
+    HAL_ObserveUserProgramAutonomous();
+    AutonomousPeriodic();
+    m_watchdog.AddEpoch("AutonomousPeriodic()");
+  } else if (mode == Mode::kTeleop) {
+    HAL_ObserveUserProgramTeleop();
+    TeleopPeriodic();
+    m_watchdog.AddEpoch("TeleopPeriodic()");
+  } else if (mode == Mode::kTest) {
     HAL_ObserveUserProgramTest();
     TestPeriodic();
     m_watchdog.AddEpoch("TestPeriodic()");
@@ -165,18 +174,25 @@
 
   SmartDashboard::UpdateValues();
   m_watchdog.AddEpoch("SmartDashboard::UpdateValues()");
-  LiveWindow::GetInstance()->UpdateValues();
+  LiveWindow::UpdateValues();
   m_watchdog.AddEpoch("LiveWindow::UpdateValues()");
   Shuffleboard::Update();
   m_watchdog.AddEpoch("Shuffleboard::Update()");
 
   if constexpr (IsSimulation()) {
+    HAL_SimPeriodicBefore();
     SimulationPeriodic();
+    HAL_SimPeriodicAfter();
     m_watchdog.AddEpoch("SimulationPeriodic()");
   }
 
   m_watchdog.Disable();
 
+  // Flush NetworkTables
+  if (m_ntFlushEnabled) {
+    nt::NetworkTableInstance::GetDefault().Flush();
+  }
+
   // Warn on loop time overruns
   if (m_watchdog.IsExpired()) {
     m_watchdog.PrintEpochs();
@@ -184,11 +200,5 @@
 }
 
 void IterativeRobotBase::PrintLoopOverrunMessage() {
-  wpi::SmallString<128> str;
-  wpi::raw_svector_ostream buf(str);
-
-  buf << "Loop time of " << wpi::format("%.6f", m_period.to<double>())
-      << "s overrun\n";
-
-  DriverStation::ReportWarning(str);
+  FRC_ReportError(err::Error, "Loop time of {:.6f}s overrun", m_period.value());
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Jaguar.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Jaguar.cpp
deleted file mode 100644
index c87e4d1..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Jaguar.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Jaguar.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-Jaguar::Jaguar(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.31, 1.55, 1.507, 1.454, 0.697);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "Jaguar", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Joystick.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Joystick.cpp
index c2cdaea..48f0c77 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Joystick.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Joystick.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Joystick.h"
 
 #include <cmath>
 
 #include <hal/FRCUsageReporting.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 using namespace frc;
 
@@ -24,45 +21,69 @@
   HAL_Report(HALUsageReporting::kResourceType_Joystick, port + 1);
 }
 
-void Joystick::SetXChannel(int channel) { m_axes[Axis::kX] = channel; }
+void Joystick::SetXChannel(int channel) {
+  m_axes[Axis::kX] = channel;
+}
 
-void Joystick::SetYChannel(int channel) { m_axes[Axis::kY] = channel; }
+void Joystick::SetYChannel(int channel) {
+  m_axes[Axis::kY] = channel;
+}
 
-void Joystick::SetZChannel(int channel) { m_axes[Axis::kZ] = channel; }
+void Joystick::SetZChannel(int channel) {
+  m_axes[Axis::kZ] = channel;
+}
 
-void Joystick::SetTwistChannel(int channel) { m_axes[Axis::kTwist] = channel; }
+void Joystick::SetTwistChannel(int channel) {
+  m_axes[Axis::kTwist] = channel;
+}
 
 void Joystick::SetThrottleChannel(int channel) {
   m_axes[Axis::kThrottle] = channel;
 }
 
-int Joystick::GetXChannel() const { return m_axes[Axis::kX]; }
+int Joystick::GetXChannel() const {
+  return m_axes[Axis::kX];
+}
 
-int Joystick::GetYChannel() const { return m_axes[Axis::kY]; }
+int Joystick::GetYChannel() const {
+  return m_axes[Axis::kY];
+}
 
-int Joystick::GetZChannel() const { return m_axes[Axis::kZ]; }
+int Joystick::GetZChannel() const {
+  return m_axes[Axis::kZ];
+}
 
-int Joystick::GetTwistChannel() const { return m_axes[Axis::kTwist]; }
+int Joystick::GetTwistChannel() const {
+  return m_axes[Axis::kTwist];
+}
 
-int Joystick::GetThrottleChannel() const { return m_axes[Axis::kThrottle]; }
+int Joystick::GetThrottleChannel() const {
+  return m_axes[Axis::kThrottle];
+}
 
-double Joystick::GetX(JoystickHand hand) const {
+double Joystick::GetX() const {
   return GetRawAxis(m_axes[Axis::kX]);
 }
 
-double Joystick::GetY(JoystickHand hand) const {
+double Joystick::GetY() const {
   return GetRawAxis(m_axes[Axis::kY]);
 }
 
-double Joystick::GetZ() const { return GetRawAxis(m_axes[Axis::kZ]); }
+double Joystick::GetZ() const {
+  return GetRawAxis(m_axes[Axis::kZ]);
+}
 
-double Joystick::GetTwist() const { return GetRawAxis(m_axes[Axis::kTwist]); }
+double Joystick::GetTwist() const {
+  return GetRawAxis(m_axes[Axis::kTwist]);
+}
 
 double Joystick::GetThrottle() const {
   return GetRawAxis(m_axes[Axis::kThrottle]);
 }
 
-bool Joystick::GetTrigger() const { return GetRawButton(Button::kTrigger); }
+bool Joystick::GetTrigger() const {
+  return GetRawButton(Button::kTrigger);
+}
 
 bool Joystick::GetTriggerPressed() {
   return GetRawButtonPressed(Button::kTrigger);
@@ -72,11 +93,17 @@
   return GetRawButtonReleased(Button::kTrigger);
 }
 
-bool Joystick::GetTop() const { return GetRawButton(Button::kTop); }
+bool Joystick::GetTop() const {
+  return GetRawButton(Button::kTop);
+}
 
-bool Joystick::GetTopPressed() { return GetRawButtonPressed(Button::kTop); }
+bool Joystick::GetTopPressed() {
+  return GetRawButtonPressed(Button::kTop);
+}
 
-bool Joystick::GetTopReleased() { return GetRawButtonReleased(Button::kTop); }
+bool Joystick::GetTopReleased() {
+  return GetRawButtonReleased(Button::kTop);
+}
 
 double Joystick::GetMagnitude() const {
   return std::sqrt(std::pow(GetX(), 2) + std::pow(GetY(), 2));
@@ -87,5 +114,5 @@
 }
 
 double Joystick::GetDirectionDegrees() const {
-  return (180 / wpi::math::pi) * GetDirectionRadians();
+  return (180 / wpi::numbers::pi) * GetDirectionRadians();
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/MotorSafety.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/MotorSafety.cpp
index 1806e86..9cb28cd 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/MotorSafety.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/MotorSafety.h"
 
@@ -11,11 +8,9 @@
 #include <utility>
 
 #include <wpi/SmallPtrSet.h>
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
 
 #include "frc/DriverStation.h"
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -33,16 +28,13 @@
 }
 
 MotorSafety::MotorSafety(MotorSafety&& rhs)
-    : ErrorBase(std::move(rhs)),
-      m_expiration(std::move(rhs.m_expiration)),
+    : m_expiration(std::move(rhs.m_expiration)),
       m_enabled(std::move(rhs.m_enabled)),
       m_stopTime(std::move(rhs.m_stopTime)) {}
 
 MotorSafety& MotorSafety::operator=(MotorSafety&& rhs) {
   std::scoped_lock lock(m_thisMutex, rhs.m_thisMutex);
 
-  ErrorBase::operator=(std::move(rhs));
-
   m_expiration = std::move(rhs.m_expiration);
   m_enabled = std::move(rhs.m_enabled);
   m_stopTime = std::move(rhs.m_stopTime);
@@ -55,12 +47,12 @@
   m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
 }
 
-void MotorSafety::SetExpiration(double expirationTime) {
+void MotorSafety::SetExpiration(units::second_t expirationTime) {
   std::scoped_lock lock(m_thisMutex);
   m_expiration = expirationTime;
 }
 
-double MotorSafety::GetExpiration() const {
+units::second_t MotorSafety::GetExpiration() const {
   std::scoped_lock lock(m_thisMutex);
   return m_expiration;
 }
@@ -82,7 +74,7 @@
 
 void MotorSafety::Check() {
   bool enabled;
-  double stopTime;
+  units::second_t stopTime;
 
   {
     std::scoped_lock lock(m_thisMutex);
@@ -90,17 +82,13 @@
     stopTime = m_stopTime;
   }
 
-  DriverStation& ds = DriverStation::GetInstance();
-  if (!enabled || ds.IsDisabled() || ds.IsTest()) {
+  if (!enabled || DriverStation::IsDisabled() || DriverStation::IsTest()) {
     return;
   }
 
   if (stopTime < Timer::GetFPGATimestamp()) {
-    wpi::SmallString<128> buf;
-    wpi::raw_svector_ostream desc(buf);
-    GetDescription(desc);
-    desc << "... Output not updated often enough.";
-    wpi_setWPIErrorWithContext(Timeout, desc.str());
+    FRC_ReportError(err::Timeout, "{}... Output not updated often enough",
+                    GetDescription());
     StopMotor();
   }
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/NidecBrushless.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/NidecBrushless.cpp
deleted file mode 100644
index 82a278e..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/NidecBrushless.cpp
+++ /dev/null
@@ -1,75 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/NidecBrushless.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
-    : m_dio(dioChannel), m_pwm(pwmChannel) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, &m_dio);
-  registry.AddChild(this, &m_pwm);
-  SetExpiration(0.0);
-  SetSafetyEnabled(false);
-
-  // the dio controls the output (in PWM mode)
-  m_dio.SetPWMRate(15625);
-  m_dio.EnablePWM(0.5);
-
-  HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
-  registry.AddLW(this, "Nidec Brushless", pwmChannel);
-}
-
-void NidecBrushless::Set(double speed) {
-  if (!m_disabled) {
-    m_speed = speed;
-    m_dio.UpdateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
-    m_pwm.SetRaw(0xffff);
-  }
-  Feed();
-}
-
-double NidecBrushless::Get() const { return m_speed; }
-
-void NidecBrushless::SetInverted(bool isInverted) { m_isInverted = isInverted; }
-
-bool NidecBrushless::GetInverted() const { return m_isInverted; }
-
-void NidecBrushless::Disable() {
-  m_disabled = true;
-  m_dio.UpdateDutyCycle(0.5);
-  m_pwm.SetDisabled();
-}
-
-void NidecBrushless::Enable() { m_disabled = false; }
-
-void NidecBrushless::PIDWrite(double output) { Set(output); }
-
-void NidecBrushless::StopMotor() {
-  m_dio.UpdateDutyCycle(0.5);
-  m_pwm.SetDisabled();
-}
-
-void NidecBrushless::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "Nidec " << GetChannel();
-}
-
-int NidecBrushless::GetChannel() const { return m_pwm.GetChannel(); }
-
-void NidecBrushless::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Nidec Brushless");
-  builder.SetActuator(true);
-  builder.SetSafeState([=]() { StopMotor(); });
-  builder.AddDoubleProperty(
-      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Notifier.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Notifier.cpp
index 4280ce4..441f750 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Notifier.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Notifier.cpp
@@ -1,40 +1,41 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Notifier.h"
 
 #include <utility>
 
+#include <fmt/format.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/Notifier.h>
 #include <hal/Threads.h>
-#include <wpi/SmallString.h>
 
+#include "frc/Errors.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
 
 using namespace frc;
 
 Notifier::Notifier(std::function<void()> handler) {
-  if (handler == nullptr)
-    wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+  if (!handler) {
+    throw FRC_MakeError(err::NullParameter, "{}", "handler");
+  }
   m_handler = handler;
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
 
   m_thread = std::thread([=] {
     for (;;) {
       int32_t status = 0;
       HAL_NotifierHandle notifier = m_notifier.load();
-      if (notifier == 0) break;
+      if (notifier == 0) {
+        break;
+      }
       uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
-      if (curTime == 0 || status != 0) break;
+      if (curTime == 0 || status != 0) {
+        break;
+      }
 
       std::function<void()> handler;
       {
@@ -50,27 +51,34 @@
       }
 
       // call callback
-      if (handler) handler();
+      if (handler) {
+        handler();
+      }
     }
   });
 }
 
 Notifier::Notifier(int priority, std::function<void()> handler) {
-  if (handler == nullptr)
-    wpi_setWPIErrorWithContext(NullParameter, "handler must not be nullptr");
+  if (!handler) {
+    throw FRC_MakeError(err::NullParameter, "{}", "handler");
+  }
   m_handler = handler;
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
 
   m_thread = std::thread([=] {
     int32_t status = 0;
     HAL_SetCurrentThreadPriority(true, priority, &status);
     for (;;) {
       HAL_NotifierHandle notifier = m_notifier.load();
-      if (notifier == 0) break;
+      if (notifier == 0) {
+        break;
+      }
       uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
-      if (curTime == 0 || status != 0) break;
+      if (curTime == 0 || status != 0) {
+        break;
+      }
 
       std::function<void()> handler;
       {
@@ -86,7 +94,9 @@
       }
 
       // call callback
-      if (handler) handler();
+      if (handler) {
+        handler();
+      }
     }
   });
 }
@@ -96,17 +106,18 @@
   // atomically set handle to 0, then clean
   HAL_NotifierHandle handle = m_notifier.exchange(0);
   HAL_StopNotifier(handle, &status);
-  wpi_setHALError(status);
+  FRC_ReportError(status, "{}", "StopNotifier");
 
   // Join the thread to ensure the handler has exited.
-  if (m_thread.joinable()) m_thread.join();
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
 
   HAL_CleanNotifier(handle, &status);
 }
 
 Notifier::Notifier(Notifier&& rhs)
-    : ErrorBase(std::move(rhs)),
-      m_thread(std::move(rhs.m_thread)),
+    : m_thread(std::move(rhs.m_thread)),
       m_notifier(rhs.m_notifier.load()),
       m_handler(std::move(rhs.m_handler)),
       m_expirationTime(std::move(rhs.m_expirationTime)),
@@ -116,8 +127,6 @@
 }
 
 Notifier& Notifier::operator=(Notifier&& rhs) {
-  ErrorBase::operator=(std::move(rhs));
-
   m_thread = std::move(rhs.m_thread);
   m_notifier = rhs.m_notifier.load();
   rhs.m_notifier = HAL_kInvalidHandle;
@@ -129,11 +138,12 @@
   return *this;
 }
 
-void Notifier::SetName(const wpi::Twine& name) {
-  wpi::SmallString<64> nameBuf;
+void Notifier::SetName(std::string_view name) {
+  fmt::memory_buffer buf;
+  fmt::format_to(fmt::appender{buf}, "{}", name);
+  buf.push_back('\0');  // null terminate
   int32_t status = 0;
-  HAL_SetNotifierName(m_notifier,
-                      name.toNullTerminatedStringRef(nameBuf).data(), &status);
+  HAL_SetNotifierName(m_notifier, buf.data(), &status);
 }
 
 void Notifier::SetHandler(std::function<void()> handler) {
@@ -141,26 +151,18 @@
   m_handler = handler;
 }
 
-void Notifier::StartSingle(double delay) {
-  StartSingle(units::second_t(delay));
-}
-
 void Notifier::StartSingle(units::second_t delay) {
   std::scoped_lock lock(m_processMutex);
   m_periodic = false;
-  m_period = delay.to<double>();
+  m_period = delay;
   m_expirationTime = Timer::GetFPGATimestamp() + m_period;
   UpdateAlarm();
 }
 
-void Notifier::StartPeriodic(double period) {
-  StartPeriodic(units::second_t(period));
-}
-
 void Notifier::StartPeriodic(units::second_t period) {
   std::scoped_lock lock(m_processMutex);
   m_periodic = true;
-  m_period = period.to<double>();
+  m_period = period;
   m_expirationTime = Timer::GetFPGATimestamp() + m_period;
   UpdateAlarm();
 }
@@ -170,18 +172,25 @@
   m_periodic = false;
   int32_t status = 0;
   HAL_CancelNotifierAlarm(m_notifier, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "CancelNotifierAlarm");
 }
 
 void Notifier::UpdateAlarm(uint64_t triggerTime) {
   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 (notifier == 0) {
+    return;
+  }
   HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "UpdateNotifierAlarm");
 }
 
 void Notifier::UpdateAlarm() {
   UpdateAlarm(static_cast<uint64_t>(m_expirationTime * 1e6));
 }
+
+bool Notifier::SetHALThreadPriority(bool realTime, int32_t priority) {
+  int32_t status = 0;
+  return HAL_SetNotifierThreadPriority(realTime, priority, &status);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDBase.cpp
deleted file mode 100644
index 5802d98..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDBase.cpp
+++ /dev/null
@@ -1,354 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PIDBase.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/PIDOutput.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-template <class T>
-constexpr const T& clamp(const T& value, const T& low, const T& high) {
-  return std::max(low, std::min(value, high));
-}
-
-PIDBase::PIDBase(double Kp, double Ki, double Kd, PIDSource& source,
-                 PIDOutput& output)
-    : PIDBase(Kp, Ki, Kd, 0.0, source, output) {}
-
-PIDBase::PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource& source,
-                 PIDOutput& output) {
-  m_P = Kp;
-  m_I = Ki;
-  m_D = Kd;
-  m_F = Kf;
-
-  m_pidInput = &source;
-  m_filter = LinearFilter<double>::MovingAverage(1);
-
-  m_pidOutput = &output;
-
-  m_setpointTimer.Start();
-
-  static int instances = 0;
-  instances++;
-  HAL_Report(HALUsageReporting::kResourceType_PIDController, instances);
-  SendableRegistry::GetInstance().Add(this, "PIDController", instances);
-}
-
-double PIDBase::Get() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_result;
-}
-
-void PIDBase::SetContinuous(bool continuous) {
-  std::scoped_lock lock(m_thisMutex);
-  m_continuous = continuous;
-}
-
-void PIDBase::SetInputRange(double minimumInput, double maximumInput) {
-  {
-    std::scoped_lock lock(m_thisMutex);
-    m_minimumInput = minimumInput;
-    m_maximumInput = maximumInput;
-    m_inputRange = maximumInput - minimumInput;
-  }
-
-  SetSetpoint(m_setpoint);
-}
-
-void PIDBase::SetOutputRange(double minimumOutput, double maximumOutput) {
-  std::scoped_lock lock(m_thisMutex);
-  m_minimumOutput = minimumOutput;
-  m_maximumOutput = maximumOutput;
-}
-
-void PIDBase::SetPID(double p, double i, double d) {
-  {
-    std::scoped_lock lock(m_thisMutex);
-    m_P = p;
-    m_I = i;
-    m_D = d;
-  }
-}
-
-void PIDBase::SetPID(double p, double i, double d, double f) {
-  std::scoped_lock lock(m_thisMutex);
-  m_P = p;
-  m_I = i;
-  m_D = d;
-  m_F = f;
-}
-
-void PIDBase::SetP(double p) {
-  std::scoped_lock lock(m_thisMutex);
-  m_P = p;
-}
-
-void PIDBase::SetI(double i) {
-  std::scoped_lock lock(m_thisMutex);
-  m_I = i;
-}
-
-void PIDBase::SetD(double d) {
-  std::scoped_lock lock(m_thisMutex);
-  m_D = d;
-}
-
-void PIDBase::SetF(double f) {
-  std::scoped_lock lock(m_thisMutex);
-  m_F = f;
-}
-
-double PIDBase::GetP() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_P;
-}
-
-double PIDBase::GetI() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_I;
-}
-
-double PIDBase::GetD() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_D;
-}
-
-double PIDBase::GetF() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_F;
-}
-
-void PIDBase::SetSetpoint(double setpoint) {
-  {
-    std::scoped_lock lock(m_thisMutex);
-
-    if (m_maximumInput > m_minimumInput) {
-      if (setpoint > m_maximumInput)
-        m_setpoint = m_maximumInput;
-      else if (setpoint < m_minimumInput)
-        m_setpoint = m_minimumInput;
-      else
-        m_setpoint = setpoint;
-    } else {
-      m_setpoint = setpoint;
-    }
-  }
-}
-
-double PIDBase::GetSetpoint() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_setpoint;
-}
-
-double PIDBase::GetDeltaSetpoint() const {
-  std::scoped_lock lock(m_thisMutex);
-  return (m_setpoint - m_prevSetpoint) / m_setpointTimer.Get();
-}
-
-double PIDBase::GetError() const {
-  double setpoint = GetSetpoint();
-  {
-    std::scoped_lock lock(m_thisMutex);
-    return GetContinuousError(setpoint - m_pidInput->PIDGet());
-  }
-}
-
-double PIDBase::GetAvgError() const { return GetError(); }
-
-void PIDBase::SetPIDSourceType(PIDSourceType pidSource) {
-  m_pidInput->SetPIDSourceType(pidSource);
-}
-
-PIDSourceType PIDBase::GetPIDSourceType() const {
-  return m_pidInput->GetPIDSourceType();
-}
-
-void PIDBase::SetTolerance(double percent) {
-  std::scoped_lock lock(m_thisMutex);
-  m_toleranceType = kPercentTolerance;
-  m_tolerance = percent;
-}
-
-void PIDBase::SetAbsoluteTolerance(double absTolerance) {
-  std::scoped_lock lock(m_thisMutex);
-  m_toleranceType = kAbsoluteTolerance;
-  m_tolerance = absTolerance;
-}
-
-void PIDBase::SetPercentTolerance(double percent) {
-  std::scoped_lock lock(m_thisMutex);
-  m_toleranceType = kPercentTolerance;
-  m_tolerance = percent;
-}
-
-void PIDBase::SetToleranceBuffer(int bufLength) {
-  std::scoped_lock lock(m_thisMutex);
-  m_filter = LinearFilter<double>::MovingAverage(bufLength);
-}
-
-bool PIDBase::OnTarget() const {
-  double error = GetError();
-
-  std::scoped_lock lock(m_thisMutex);
-  switch (m_toleranceType) {
-    case kPercentTolerance:
-      return std::fabs(error) < m_tolerance / 100 * m_inputRange;
-      break;
-    case kAbsoluteTolerance:
-      return std::fabs(error) < m_tolerance;
-      break;
-    case kNoTolerance:
-      // TODO: this case needs an error
-      return false;
-  }
-  return false;
-}
-
-void PIDBase::Reset() {
-  std::scoped_lock lock(m_thisMutex);
-  m_prevError = 0;
-  m_totalError = 0;
-  m_result = 0;
-}
-
-void PIDBase::PIDWrite(double output) { SetSetpoint(output); }
-
-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); });
-}
-
-void PIDBase::Calculate() {
-  if (m_pidInput == nullptr || m_pidOutput == nullptr) return;
-
-  bool enabled;
-  {
-    std::scoped_lock lock(m_thisMutex);
-    enabled = m_enabled;
-  }
-
-  if (enabled) {
-    double input;
-
-    // Storage for function inputs
-    PIDSourceType pidSourceType;
-    double P;
-    double I;
-    double D;
-    double feedForward = CalculateFeedForward();
-    double minimumOutput;
-    double maximumOutput;
-
-    // Storage for function input-outputs
-    double prevError;
-    double error;
-    double totalError;
-
-    {
-      std::scoped_lock lock(m_thisMutex);
-
-      input = m_filter.Calculate(m_pidInput->PIDGet());
-
-      pidSourceType = m_pidInput->GetPIDSourceType();
-      P = m_P;
-      I = m_I;
-      D = m_D;
-      minimumOutput = m_minimumOutput;
-      maximumOutput = m_maximumOutput;
-
-      prevError = m_prevError;
-      error = GetContinuousError(m_setpoint - input);
-      totalError = m_totalError;
-    }
-
-    // Storage for function outputs
-    double result;
-
-    if (pidSourceType == PIDSourceType::kRate) {
-      if (P != 0) {
-        totalError =
-            clamp(totalError + error, minimumOutput / P, maximumOutput / P);
-      }
-
-      result = D * error + P * totalError + feedForward;
-    } else {
-      if (I != 0) {
-        totalError =
-            clamp(totalError + error, minimumOutput / I, maximumOutput / I);
-      }
-
-      result =
-          P * error + I * totalError + D * (error - prevError) + feedForward;
-    }
-
-    result = clamp(result, minimumOutput, maximumOutput);
-
-    {
-      // Ensures m_enabled check and PIDWrite() call occur atomically
-      std::scoped_lock pidWriteLock(m_pidWriteMutex);
-      std::unique_lock mainLock(m_thisMutex);
-      if (m_enabled) {
-        // Don't block other PIDBase operations on PIDWrite()
-        mainLock.unlock();
-
-        m_pidOutput->PIDWrite(result);
-      }
-    }
-
-    std::scoped_lock lock(m_thisMutex);
-    m_prevError = m_error;
-    m_error = error;
-    m_totalError = totalError;
-    m_result = result;
-  }
-}
-
-double PIDBase::CalculateFeedForward() {
-  if (m_pidInput->GetPIDSourceType() == PIDSourceType::kRate) {
-    return m_F * GetSetpoint();
-  } else {
-    double temp = m_F * GetDeltaSetpoint();
-    m_prevSetpoint = m_setpoint;
-    m_setpointTimer.Reset();
-    return temp;
-  }
-}
-
-double PIDBase::GetContinuousError(double error) const {
-  if (m_continuous && m_inputRange != 0) {
-    error = std::fmod(error, m_inputRange);
-    if (std::fabs(error) > m_inputRange / 2) {
-      if (error > 0) {
-        return error - m_inputRange;
-      } else {
-        return error + m_inputRange;
-      }
-    }
-  }
-
-  return error;
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDController.cpp
deleted file mode 100644
index a90a645..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDController.cpp
+++ /dev/null
@@ -1,86 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PIDController.h"
-
-#include "frc/Notifier.h"
-#include "frc/PIDOutput.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource* source,
-                             PIDOutput* output, double period)
-    : PIDController(Kp, Ki, Kd, 0.0, *source, *output, period) {}
-
-PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
-                             PIDSource* source, PIDOutput* output,
-                             double period)
-    : PIDController(Kp, Ki, Kd, Kf, *source, *output, period) {}
-
-PIDController::PIDController(double Kp, double Ki, double Kd, PIDSource& source,
-                             PIDOutput& output, double period)
-    : PIDController(Kp, Ki, Kd, 0.0, source, output, period) {}
-
-PIDController::PIDController(double Kp, double Ki, double Kd, double Kf,
-                             PIDSource& source, PIDOutput& output,
-                             double period)
-    : PIDBase(Kp, Ki, Kd, Kf, source, output) {
-  m_controlLoop = std::make_unique<Notifier>(&PIDController::Calculate, this);
-  m_controlLoop->StartPeriodic(units::second_t(period));
-}
-
-PIDController::~PIDController() {
-  // Forcefully stopping the notifier so the callback can successfully run.
-  m_controlLoop->Stop();
-}
-
-void PIDController::Enable() {
-  {
-    std::scoped_lock lock(m_thisMutex);
-    m_enabled = true;
-  }
-}
-
-void PIDController::Disable() {
-  {
-    // Ensures m_enabled modification and PIDWrite() call occur atomically
-    std::scoped_lock pidWriteLock(m_pidWriteMutex);
-    {
-      std::scoped_lock mainLock(m_thisMutex);
-      m_enabled = false;
-    }
-
-    m_pidOutput->PIDWrite(0);
-  }
-}
-
-void PIDController::SetEnabled(bool enable) {
-  if (enable) {
-    Enable();
-  } else {
-    Disable();
-  }
-}
-
-bool PIDController::IsEnabled() const {
-  std::scoped_lock lock(m_thisMutex);
-  return m_enabled;
-}
-
-void PIDController::Reset() {
-  Disable();
-
-  PIDBase::Reset();
-}
-
-void PIDController::InitSendable(SendableBuilder& builder) {
-  PIDBase::InitSendable(builder);
-  builder.AddBooleanProperty(
-      "enabled", [=]() { return IsEnabled(); },
-      [=](bool value) { SetEnabled(value); });
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDSource.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDSource.cpp
deleted file mode 100644
index 28291dd..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PIDSource.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PIDSource.h"
-
-using namespace frc;
-
-void PIDSource::SetPIDSourceType(PIDSourceType pidSource) {
-  m_pidSource = pidSource;
-}
-
-PIDSourceType PIDSource::GetPIDSourceType() const { return m_pidSource; }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp
new file mode 100644
index 0000000..91fd304
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PS4Controller.cpp
@@ -0,0 +1,205 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PS4Controller.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PS4Controller::PS4Controller(int port) : GenericHID(port) {
+  HAL_Report(HALUsageReporting::kResourceType_PS4Controller, port + 1);
+}
+
+double PS4Controller::GetLeftX() const {
+  return GetRawAxis(Axis::kLeftX);
+}
+
+double PS4Controller::GetRightX() const {
+  return GetRawAxis(Axis::kRightX);
+}
+
+double PS4Controller::GetLeftY() const {
+  return GetRawAxis(Axis::kLeftY);
+}
+
+double PS4Controller::GetRightY() const {
+  return GetRawAxis(Axis::kRightY);
+}
+
+double PS4Controller::GetL2Axis() const {
+  return GetRawAxis(Axis::kL2);
+}
+
+double PS4Controller::GetR2Axis() const {
+  return GetRawAxis(Axis::kR2);
+}
+
+bool PS4Controller::GetSquareButton() const {
+  return GetRawButton(Button::kSquare);
+}
+
+bool PS4Controller::GetSquareButtonPressed() {
+  return GetRawButtonPressed(Button::kSquare);
+}
+
+bool PS4Controller::GetSquareButtonReleased() {
+  return GetRawButtonReleased(Button::kSquare);
+}
+
+bool PS4Controller::GetCrossButton() const {
+  return GetRawButton(Button::kCross);
+}
+
+bool PS4Controller::GetCrossButtonPressed() {
+  return GetRawButtonPressed(Button::kCross);
+}
+
+bool PS4Controller::GetCrossButtonReleased() {
+  return GetRawButtonReleased(Button::kCross);
+}
+
+bool PS4Controller::GetCircleButton() const {
+  return GetRawButton(Button::kCircle);
+}
+
+bool PS4Controller::GetCircleButtonPressed() {
+  return GetRawButtonPressed(Button::kCircle);
+}
+
+bool PS4Controller::GetCircleButtonReleased() {
+  return GetRawButtonReleased(Button::kCircle);
+}
+
+bool PS4Controller::GetTriangleButton() const {
+  return GetRawButton(Button::kTriangle);
+}
+
+bool PS4Controller::GetTriangleButtonPressed() {
+  return GetRawButtonPressed(Button::kTriangle);
+}
+
+bool PS4Controller::GetTriangleButtonReleased() {
+  return GetRawButtonReleased(Button::kTriangle);
+}
+
+bool PS4Controller::GetL1Button() const {
+  return GetRawButton(Button::kL1);
+}
+
+bool PS4Controller::GetL1ButtonPressed() {
+  return GetRawButtonPressed(Button::kL1);
+}
+
+bool PS4Controller::GetL1ButtonReleased() {
+  return GetRawButtonReleased(Button::kL1);
+}
+
+bool PS4Controller::GetR1Button() const {
+  return GetRawButton(Button::kR1);
+}
+
+bool PS4Controller::GetR1ButtonPressed() {
+  return GetRawButtonPressed(Button::kR1);
+}
+
+bool PS4Controller::GetR1ButtonReleased() {
+  return GetRawButtonReleased(Button::kR1);
+}
+
+bool PS4Controller::GetL2Button() const {
+  return GetRawButton(Button::kL2);
+}
+
+bool PS4Controller::GetL2ButtonPressed() {
+  return GetRawButtonPressed(Button::kL2);
+}
+
+bool PS4Controller::GetL2ButtonReleased() {
+  return GetRawButtonReleased(Button::kL2);
+}
+
+bool PS4Controller::GetR2Button() const {
+  return GetRawButton(Button::kR2);
+}
+
+bool PS4Controller::GetR2ButtonPressed() {
+  return GetRawButtonPressed(Button::kR2);
+}
+
+bool PS4Controller::GetR2ButtonReleased() {
+  return GetRawButtonReleased(Button::kR2);
+}
+
+bool PS4Controller::GetShareButton() const {
+  return GetRawButton(Button::kShare);
+}
+
+bool PS4Controller::GetShareButtonPressed() {
+  return GetRawButtonPressed(Button::kShare);
+}
+
+bool PS4Controller::GetShareButtonReleased() {
+  return GetRawButtonReleased(Button::kShare);
+}
+
+bool PS4Controller::GetOptionsButton() const {
+  return GetRawButton(Button::kOptions);
+}
+
+bool PS4Controller::GetOptionsButtonPressed() {
+  return GetRawButtonPressed(Button::kOptions);
+}
+
+bool PS4Controller::GetOptionsButtonReleased() {
+  return GetRawButtonReleased(Button::kOptions);
+}
+
+bool PS4Controller::GetL3Button() const {
+  return GetRawButton(Button::kL3);
+}
+
+bool PS4Controller::GetL3ButtonPressed() {
+  return GetRawButtonPressed(Button::kL3);
+}
+
+bool PS4Controller::GetL3ButtonReleased() {
+  return GetRawButtonReleased(Button::kL3);
+}
+
+bool PS4Controller::GetR3Button() const {
+  return GetRawButton(Button::kR3);
+}
+
+bool PS4Controller::GetR3ButtonPressed() {
+  return GetRawButtonPressed(Button::kR3);
+}
+
+bool PS4Controller::GetR3ButtonReleased() {
+  return GetRawButtonReleased(Button::kR3);
+}
+
+bool PS4Controller::GetPSButton() const {
+  return GetRawButton(Button::kPS);
+}
+
+bool PS4Controller::GetPSButtonPressed() {
+  return GetRawButtonPressed(Button::kPS);
+}
+
+bool PS4Controller::GetPSButtonReleased() {
+  return GetRawButtonReleased(Button::kPS);
+}
+
+bool PS4Controller::GetTouchpad() const {
+  return GetRawButton(Button::kTouchpad);
+}
+
+bool PS4Controller::GetTouchpadPressed() {
+  return GetRawButtonPressed(Button::kTouchpad);
+}
+
+bool PS4Controller::GetTouchpadReleased() {
+  return GetRawButtonReleased(Button::kTouchpad);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWM.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWM.cpp
index c2cd2e2..393accc 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWM.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWM.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/PWM.h"
 
@@ -13,123 +10,97 @@
 #include <hal/HALBase.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
-PWM::PWM(int channel) {
+PWM::PWM(int channel, bool registerSendable) {
   if (!SensorUtil::CheckPWMChannel(channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "PWM Channel " + wpi::Twine(channel));
-    return;
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", channel);
   }
 
+  auto stack = wpi::GetStackTrace(1);
   int32_t status = 0;
-  m_handle = HAL_InitializePWMPort(HAL_GetPort(channel), &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumPWMChannels(), channel);
-    m_channel = std::numeric_limits<int>::max();
-    m_handle = HAL_kInvalidHandle;
-    return;
-  }
+  m_handle =
+      HAL_InitializePWMPort(HAL_GetPort(channel), stack.c_str(), &status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
 
   m_channel = channel;
 
   HAL_SetPWMDisabled(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
   status = 0;
   HAL_SetPWMEliminateDeadband(m_handle, false, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", channel);
 
   HAL_Report(HALUsageReporting::kResourceType_PWM, channel + 1);
-  SendableRegistry::GetInstance().AddLW(this, "PWM", channel);
-
-  SetSafetyEnabled(false);
+  if (registerSendable) {
+    wpi::SendableRegistry::AddLW(this, "PWM", channel);
+  }
 }
 
 PWM::~PWM() {
   int32_t status = 0;
 
   HAL_SetPWMDisabled(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_ReportError(status, "Channel {}", m_channel);
 
   HAL_FreePWMPort(m_handle, &status);
-  wpi_setHALError(status);
-}
-
-void PWM::StopMotor() { SetDisabled(); }
-
-void PWM::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "PWM " << GetChannel();
+  FRC_ReportError(status, "Channel {}", m_channel);
 }
 
 void PWM::SetRaw(uint16_t value) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
   HAL_SetPWMRaw(m_handle, value, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 uint16_t PWM::GetRaw() const {
-  if (StatusIsFatal()) return 0;
-
   int32_t status = 0;
   uint16_t value = HAL_GetPWMRaw(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 
   return value;
 }
 
 void PWM::SetPosition(double pos) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMPosition(m_handle, pos, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 double PWM::GetPosition() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double position = HAL_GetPWMPosition(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return position;
 }
 
 void PWM::SetSpeed(double speed) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMSpeed(m_handle, speed, &status);
-  wpi_setHALError(status);
-
-  Feed();
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 double PWM::GetSpeed() const {
-  if (StatusIsFatal()) return 0.0;
   int32_t status = 0;
   double speed = HAL_GetPWMSpeed(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   return speed;
 }
 
 void PWM::SetDisabled() {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
-
   HAL_SetPWMDisabled(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void PWM::SetPeriodMultiplier(PeriodMultiplier mult) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
 
   switch (mult) {
@@ -145,44 +116,39 @@
       HAL_SetPWMPeriodScale(m_handle, 0, &status);  // Don't squelch any outputs
       break;
     default:
-      wpi_assert(false);
+      throw FRC_MakeError(err::InvalidParameter, "PeriodMultiplier value {}",
+                          mult);
   }
 
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void PWM::SetZeroLatch() {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
-
   HAL_LatchPWMZero(m_handle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void PWM::EnableDeadbandElimination(bool eliminateDeadband) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMEliminateDeadband(m_handle, eliminateDeadband, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void PWM::SetBounds(double max, double deadbandMax, double center,
                     double deadbandMin, double min) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min,
                    &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void PWM::SetRawBounds(int max, int deadbandMax, int center, int deadbandMin,
                        int min) {
-  if (StatusIsFatal()) return;
   int32_t status = 0;
   HAL_SetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
                       &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 void PWM::GetRawBounds(int* max, int* deadbandMax, int* center,
@@ -190,16 +156,17 @@
   int32_t status = 0;
   HAL_GetPWMConfigRaw(m_handle, max, deadbandMax, center, deadbandMin, min,
                       &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
-int PWM::GetChannel() const { return m_channel; }
+int PWM::GetChannel() const {
+  return m_channel;
+}
 
-void PWM::InitSendable(SendableBuilder& builder) {
+void PWM::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("PWM");
   builder.SetActuator(true);
-  builder.SetSafeState([=]() { SetDisabled(); });
+  builder.SetSafeState([=] { SetDisabled(); });
   builder.AddDoubleProperty(
-      "Value", [=]() { return GetRaw(); },
-      [=](double value) { SetRaw(value); });
+      "Value", [=] { return GetRaw(); }, [=](double value) { SetRaw(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMSparkMax.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
deleted file mode 100644
index c5375f4..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMSparkMax.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PWMSparkMax.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-PWMSparkMax::PWMSparkMax(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "PWMSparkMax", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMSpeedController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
deleted file mode 100644
index ec0be7b..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMSpeedController.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PWMSpeedController.h"
-
-#include "frc/smartdashboard/SendableBuilder.h"
-
-using namespace frc;
-
-void PWMSpeedController::Set(double speed) {
-  SetSpeed(m_isInverted ? -speed : speed);
-}
-
-double PWMSpeedController::Get() const { return GetSpeed(); }
-
-void PWMSpeedController::SetInverted(bool isInverted) {
-  m_isInverted = isInverted;
-}
-
-bool PWMSpeedController::GetInverted() const { return m_isInverted; }
-
-void PWMSpeedController::Disable() { SetDisabled(); }
-
-void PWMSpeedController::StopMotor() { PWM::StopMotor(); }
-
-void PWMSpeedController::PIDWrite(double output) { Set(output); }
-
-PWMSpeedController::PWMSpeedController(int channel) : PWM(channel) {}
-
-void PWMSpeedController::InitSendable(SendableBuilder& builder) {
-  builder.SetSmartDashboardType("Speed Controller");
-  builder.SetActuator(true);
-  builder.SetSafeState([=]() { SetDisabled(); });
-  builder.AddDoubleProperty(
-      "Value", [=]() { return GetSpeed(); },
-      [=](double value) { SetSpeed(value); });
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMTalonFX.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
deleted file mode 100644
index 09b7163..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMTalonFX.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PWMTalonFX.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-PWMTalonFX::PWMTalonFX(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_TalonFX, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "PWMTalonFX", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
deleted file mode 100644
index 1242be9..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMTalonSRX.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PWMTalonSRX.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-PWMTalonSRX::PWMTalonSRX(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "PWMTalonSRX", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMVenom.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMVenom.cpp
deleted file mode 100644
index 9fa14b7..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMVenom.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PWMVenom.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-PWMVenom::PWMVenom(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_FusionVenom, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "PWMVenom", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
deleted file mode 100644
index 0d966ae..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PWMVictorSPX.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PWMVictorSPX.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-PWMVictorSPX::PWMVictorSPX(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "PWMVictorSPX", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp
new file mode 100644
index 0000000..8217e86
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticHub.cpp
@@ -0,0 +1,209 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PneumaticHub.h"
+
+#include <hal/REVPH.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/StackTrace.h>
+
+#include "frc/Compressor.h"
+#include "frc/DoubleSolenoid.h"
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+#include "frc/Solenoid.h"
+
+using namespace frc;
+
+wpi::mutex PneumaticHub::m_handleLock;
+std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>>
+    PneumaticHub::m_handleMap = nullptr;
+
+// Always called under lock, so we can avoid the double lock from the magic
+// static
+std::weak_ptr<PneumaticHub::DataStore>& PneumaticHub::GetDataStore(int module) {
+  if (!m_handleMap) {
+    m_handleMap = std::make_unique<
+        wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>>();
+  }
+  return (*m_handleMap)[module];
+}
+
+class PneumaticHub::DataStore {
+ public:
+  explicit DataStore(int module, const char* stackTrace) {
+    int32_t status = 0;
+    HAL_REVPHHandle handle = HAL_InitializeREVPH(module, stackTrace, &status);
+    FRC_CheckErrorStatus(status, "Module {}", module);
+    m_moduleObject = PneumaticHub{handle, module};
+    m_moduleObject.m_dataStore =
+        std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+  }
+
+  ~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); }
+
+  DataStore(DataStore&&) = delete;
+  DataStore& operator=(DataStore&&) = delete;
+
+ private:
+  friend class PneumaticHub;
+  uint32_t m_reservedMask{0};
+  bool m_compressorReserved{false};
+  wpi::mutex m_reservedLock;
+  PneumaticHub m_moduleObject{HAL_kInvalidHandle, 0};
+};
+
+PneumaticHub::PneumaticHub()
+    : PneumaticHub{SensorUtil::GetDefaultREVPHModule()} {}
+
+PneumaticHub::PneumaticHub(int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(module);
+  m_dataStore = res.lock();
+  if (!m_dataStore) {
+    m_dataStore = std::make_shared<DataStore>(module, stackTrace.c_str());
+    res = m_dataStore;
+  }
+  m_handle = m_dataStore->m_moduleObject.m_handle;
+  m_module = module;
+}
+
+PneumaticHub::PneumaticHub(HAL_REVPHHandle handle, int module)
+    : m_handle{handle}, m_module{module} {}
+
+bool PneumaticHub::GetCompressor() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressor(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticHub::SetClosedLoopControl(bool enabled) {
+  int32_t status = 0;
+  HAL_SetREVPHClosedLoopControl(m_handle, enabled, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+bool PneumaticHub::GetClosedLoopControl() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHClosedLoopControl(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+bool PneumaticHub::GetPressureSwitch() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHPressureSwitch(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+double PneumaticHub::GetCompressorCurrent() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticHub::SetSolenoids(int mask, int values) {
+  int32_t status = 0;
+  HAL_SetREVPHSolenoids(m_handle, mask, values, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+int PneumaticHub::GetSolenoids() const {
+  int32_t status = 0;
+  auto result = HAL_GetREVPHSolenoids(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+int PneumaticHub::GetModuleNumber() const {
+  return m_module;
+}
+
+int PneumaticHub::GetSolenoidDisabledList() const {
+  return 0;
+  // TODO Fix me
+  // int32_t status = 0;
+  // auto result = HAL_GetREVPHSolenoidDisabledList(m_handle, &status);
+  // FRC_CheckErrorStatus(status, "Module {}", m_module);
+  // return result;
+}
+
+void PneumaticHub::FireOneShot(int index) {
+  // TODO Fix me
+  // int32_t status = 0;
+  // HAL_FireREVPHOneShot(m_handle, index, &status);
+  // FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+void PneumaticHub::SetOneShotDuration(int index, units::second_t duration) {
+  // TODO Fix me
+  // int32_t status = 0;
+  // units::millisecond_t millis = duration;
+  // HAL_SetREVPHOneShotDuration(m_handle, index, millis.to<int32_t>(),
+  // &status); FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+bool PneumaticHub::CheckSolenoidChannel(int channel) const {
+  return HAL_CheckREVPHSolenoidChannel(channel);
+}
+
+int PneumaticHub::CheckAndReserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  uint32_t uMask = static_cast<uint32_t>(mask);
+  if ((m_dataStore->m_reservedMask & uMask) != 0) {
+    return m_dataStore->m_reservedMask & uMask;
+  }
+  m_dataStore->m_reservedMask |= uMask;
+  return 0;
+}
+
+void PneumaticHub::UnreserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_reservedMask &= ~(static_cast<uint32_t>(mask));
+}
+
+bool PneumaticHub::ReserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  if (m_dataStore->m_compressorReserved) {
+    return false;
+  }
+  m_dataStore->m_compressorReserved = true;
+  return true;
+}
+
+void PneumaticHub::UnreserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_compressorReserved = false;
+}
+
+Solenoid PneumaticHub::MakeSolenoid(int channel) {
+  return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
+}
+
+DoubleSolenoid PneumaticHub::MakeDoubleSolenoid(int forwardChannel,
+                                                int reverseChannel) {
+  return DoubleSolenoid{m_module, PneumaticsModuleType::REVPH, forwardChannel,
+                        reverseChannel};
+}
+
+Compressor PneumaticHub::MakeCompressor() {
+  return Compressor{m_module, PneumaticsModuleType::REVPH};
+}
+
+std::shared_ptr<PneumaticsBase> PneumaticHub::GetForModule(int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(module);
+  std::shared_ptr<DataStore> dataStore = res.lock();
+  if (!dataStore) {
+    dataStore = std::make_shared<DataStore>(module, stackTrace.c_str());
+    res = dataStore;
+  }
+
+  return std::shared_ptr<PneumaticsBase>{dataStore, &dataStore->m_moduleObject};
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticsBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
new file mode 100644
index 0000000..9d96651
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PneumaticsBase.h"
+
+#include "frc/Errors.h"
+#include "frc/PneumaticHub.h"
+#include "frc/PneumaticsControlModule.h"
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+
+std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
+    int module, PneumaticsModuleType moduleType) {
+  if (moduleType == PneumaticsModuleType::CTREPCM) {
+    return PneumaticsControlModule::GetForModule(module);
+  } else if (moduleType == PneumaticsModuleType::REVPH) {
+    return PneumaticHub::GetForModule(module);
+  }
+  throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+}
+
+int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) {
+  if (moduleType == PneumaticsModuleType::CTREPCM) {
+    return SensorUtil::GetDefaultCTREPCMModule();
+  } else if (moduleType == PneumaticsModuleType::REVPH) {
+    return SensorUtil::GetDefaultREVPHModule();
+  }
+  throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
new file mode 100644
index 0000000..32de5ca
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
@@ -0,0 +1,269 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PneumaticsControlModule.h"
+
+#include <hal/CTREPCM.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/StackTrace.h>
+
+#include "frc/Compressor.h"
+#include "frc/DoubleSolenoid.h"
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+#include "frc/Solenoid.h"
+
+using namespace frc;
+
+wpi::mutex PneumaticsControlModule::m_handleLock;
+std::unique_ptr<
+    wpi::DenseMap<int, std::weak_ptr<PneumaticsControlModule::DataStore>>>
+    PneumaticsControlModule::m_handleMap = nullptr;
+
+// Always called under lock, so we can avoid the double lock from the magic
+// static
+std::weak_ptr<PneumaticsControlModule::DataStore>&
+PneumaticsControlModule::GetDataStore(int module) {
+  if (!m_handleMap) {
+    m_handleMap = std::make_unique<wpi::DenseMap<
+        int, std::weak_ptr<PneumaticsControlModule::DataStore>>>();
+  }
+  return (*m_handleMap)[module];
+}
+
+class PneumaticsControlModule::DataStore {
+ public:
+  explicit DataStore(int module, const char* stackTrace) {
+    int32_t status = 0;
+    HAL_CTREPCMHandle handle =
+        HAL_InitializeCTREPCM(module, stackTrace, &status);
+    FRC_CheckErrorStatus(status, "Module {}", module);
+    m_moduleObject = PneumaticsControlModule{handle, module};
+    m_moduleObject.m_dataStore =
+        std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+  }
+
+  ~DataStore() noexcept { HAL_FreeCTREPCM(m_moduleObject.m_handle); }
+
+  DataStore(DataStore&&) = delete;
+  DataStore& operator=(DataStore&&) = delete;
+
+ private:
+  friend class PneumaticsControlModule;
+  uint32_t m_reservedMask{0};
+  bool m_compressorReserved{false};
+  wpi::mutex m_reservedLock;
+  PneumaticsControlModule m_moduleObject{HAL_kInvalidHandle, 0};
+};
+
+PneumaticsControlModule::PneumaticsControlModule()
+    : PneumaticsControlModule{SensorUtil::GetDefaultCTREPCMModule()} {}
+
+PneumaticsControlModule::PneumaticsControlModule(int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(module);
+  m_dataStore = res.lock();
+  if (!m_dataStore) {
+    m_dataStore = std::make_shared<DataStore>(module, stackTrace.c_str());
+    res = m_dataStore;
+  }
+  m_handle = m_dataStore->m_moduleObject.m_handle;
+  m_module = module;
+}
+
+PneumaticsControlModule::PneumaticsControlModule(HAL_CTREPCMHandle handle,
+                                                 int module)
+    : m_handle{handle}, m_module{module} {}
+
+bool PneumaticsControlModule::GetCompressor() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressor(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticsControlModule::SetClosedLoopControl(bool enabled) {
+  int32_t status = 0;
+  HAL_SetCTREPCMClosedLoopControl(m_handle, enabled, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+bool PneumaticsControlModule::GetClosedLoopControl() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+bool PneumaticsControlModule::GetPressureSwitch() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+double PneumaticsControlModule::GetCompressorCurrent() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() const {
+  int32_t status = 0;
+  auto result =
+      HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorShortedFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorShortedFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorShortedStickyFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorShortedStickyFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorNotConnectedFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMCompressorNotConnectedFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() const {
+  int32_t status = 0;
+  auto result =
+      HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+bool PneumaticsControlModule::GetSolenoidVoltageFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidVoltageFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticsControlModule::ClearAllStickyFaults() {
+  int32_t status = 0;
+  HAL_ClearAllCTREPCMStickyFaults(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::SetSolenoids(int mask, int values) {
+  int32_t status = 0;
+  HAL_SetCTREPCMSolenoids(m_handle, mask, values, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+int PneumaticsControlModule::GetSolenoids() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoids(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+int PneumaticsControlModule::GetModuleNumber() const {
+  return m_module;
+}
+
+int PneumaticsControlModule::GetSolenoidDisabledList() const {
+  int32_t status = 0;
+  auto result = HAL_GetCTREPCMSolenoidDisabledList(m_handle, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+  return result;
+}
+
+void PneumaticsControlModule::FireOneShot(int index) {
+  int32_t status = 0;
+  HAL_FireCTREPCMOneShot(m_handle, index, &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::SetOneShotDuration(int index,
+                                                 units::second_t duration) {
+  int32_t status = 0;
+  units::millisecond_t millis = duration;
+  HAL_SetCTREPCMOneShotDuration(m_handle, index, millis.to<int32_t>(), &status);
+  FRC_CheckErrorStatus(status, "Module {}", m_module);
+}
+
+bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const {
+  return HAL_CheckCTREPCMSolenoidChannel(channel);
+}
+
+int PneumaticsControlModule::CheckAndReserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  uint32_t uMask = static_cast<uint32_t>(mask);
+  if ((m_dataStore->m_reservedMask & uMask) != 0) {
+    return m_dataStore->m_reservedMask & uMask;
+  }
+  m_dataStore->m_reservedMask |= uMask;
+  return 0;
+}
+
+void PneumaticsControlModule::UnreserveSolenoids(int mask) {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_reservedMask &= ~(static_cast<uint32_t>(mask));
+}
+
+bool PneumaticsControlModule::ReserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  if (m_dataStore->m_compressorReserved) {
+    return false;
+  }
+  m_dataStore->m_compressorReserved = true;
+  return true;
+}
+
+void PneumaticsControlModule::UnreserveCompressor() {
+  std::scoped_lock lock{m_dataStore->m_reservedLock};
+  m_dataStore->m_compressorReserved = false;
+}
+
+Solenoid PneumaticsControlModule::MakeSolenoid(int channel) {
+  return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel};
+}
+
+DoubleSolenoid PneumaticsControlModule::MakeDoubleSolenoid(int forwardChannel,
+                                                           int reverseChannel) {
+  return DoubleSolenoid{m_module, PneumaticsModuleType::CTREPCM, forwardChannel,
+                        reverseChannel};
+}
+
+Compressor PneumaticsControlModule::MakeCompressor() {
+  return Compressor{m_module, PneumaticsModuleType::CTREPCM};
+}
+
+std::shared_ptr<PneumaticsBase> PneumaticsControlModule::GetForModule(
+    int module) {
+  std::string stackTrace = wpi::GetStackTrace(1);
+  std::scoped_lock lock(m_handleLock);
+  auto& res = GetDataStore(module);
+  std::shared_ptr<DataStore> dataStore = res.lock();
+  if (!dataStore) {
+    dataStore = std::make_shared<DataStore>(module, stackTrace.c_str());
+    res = dataStore;
+  }
+
+  return std::shared_ptr<PneumaticsBase>{dataStore, &dataStore->m_moduleObject};
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp
new file mode 100644
index 0000000..4c2acfc
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistribution.cpp
@@ -0,0 +1,146 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PowerDistribution.h"
+
+#include <fmt/format.h>
+#include <hal/FRCUsageReporting.h>
+#include <hal/Ports.h>
+#include <hal/PowerDistribution.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+#include "frc/SensorUtil.h"
+
+static_assert(static_cast<HAL_PowerDistributionType>(
+                  frc::PowerDistribution::ModuleType::kAutomatic) ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic);
+static_assert(static_cast<HAL_PowerDistributionType>(
+                  frc::PowerDistribution::ModuleType::kCTRE) ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE);
+static_assert(static_cast<HAL_PowerDistributionType>(
+                  frc::PowerDistribution::ModuleType::kRev) ==
+              HAL_PowerDistributionType::HAL_PowerDistributionType_kRev);
+static_assert(frc::PowerDistribution::kDefaultModule ==
+              HAL_DEFAULT_POWER_DISTRIBUTION_MODULE);
+
+using namespace frc;
+
+PowerDistribution::PowerDistribution()
+    : PowerDistribution(-1, ModuleType::kAutomatic) {}
+
+PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
+  auto stack = wpi::GetStackTrace(1);
+
+  int32_t status = 0;
+  m_handle = HAL_InitializePowerDistribution(
+      module, static_cast<HAL_PowerDistributionType>(moduleType), stack.c_str(),
+      &status);
+  FRC_CheckErrorStatus(status, "Module {}", module);
+  m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status);
+  FRC_ReportError(status, "Module {}", module);
+
+  HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1);
+  wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
+}
+
+PowerDistribution::~PowerDistribution() {
+  if (m_handle != HAL_kInvalidHandle) {
+    HAL_CleanPowerDistribution(m_handle);
+    m_handle = HAL_kInvalidHandle;
+  }
+}
+
+double PowerDistribution::GetVoltage() const {
+  int32_t status = 0;
+  double voltage = HAL_GetPowerDistributionVoltage(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return voltage;
+}
+
+double PowerDistribution::GetTemperature() const {
+  int32_t status = 0;
+  double temperature = HAL_GetPowerDistributionTemperature(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return temperature;
+}
+
+double PowerDistribution::GetCurrent(int channel) const {
+  int32_t status = 0;
+  double current =
+      HAL_GetPowerDistributionChannelCurrent(m_handle, channel, &status);
+  FRC_ReportError(status, "Module {} Channel {}", m_module, channel);
+
+  return current;
+}
+
+double PowerDistribution::GetTotalCurrent() const {
+  int32_t status = 0;
+  double current = HAL_GetPowerDistributionTotalCurrent(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return current;
+}
+
+double PowerDistribution::GetTotalPower() const {
+  int32_t status = 0;
+  double power = HAL_GetPowerDistributionTotalPower(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return power;
+}
+
+double PowerDistribution::GetTotalEnergy() const {
+  int32_t status = 0;
+  double energy = HAL_GetPowerDistributionTotalEnergy(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return energy;
+}
+
+void PowerDistribution::ResetTotalEnergy() {
+  int32_t status = 0;
+  HAL_ResetPowerDistributionTotalEnergy(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PowerDistribution::ClearStickyFaults() {
+  int32_t status = 0;
+  HAL_ClearPowerDistributionStickyFaults(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+int PowerDistribution::GetModule() const {
+  return m_module;
+}
+
+bool PowerDistribution::GetSwitchableChannel() const {
+  int32_t status = 0;
+  bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  return state;
+}
+
+void PowerDistribution::SetSwitchableChannel(bool enabled) {
+  int32_t status = 0;
+  HAL_SetPowerDistributionSwitchableChannel(m_handle, enabled, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("PowerDistribution");
+  int32_t status = 0;
+  int numChannels = HAL_GetPowerDistributionNumChannels(m_handle, &status);
+  FRC_ReportError(status, "Module {}", m_module);
+  for (int i = 0; i < numChannels; ++i) {
+    builder.AddDoubleProperty(
+        fmt::format("Chan{}", i), [=] { return GetCurrent(i); }, nullptr);
+  }
+  builder.AddDoubleProperty(
+      "Voltage", [=] { return GetVoltage(); }, nullptr);
+  builder.AddDoubleProperty(
+      "TotalCurrent", [=] { return GetTotalCurrent(); }, nullptr);
+  builder.AddBooleanProperty(
+      "SwitchableChannel", [=] { return GetSwitchableChannel(); },
+      [=](bool value) { SetSwitchableChannel(value); });
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
deleted file mode 100644
index 4070633..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/PowerDistributionPanel.cpp
+++ /dev/null
@@ -1,151 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/PowerDistributionPanel.h"
-
-#include <hal/FRCUsageReporting.h>
-#include <hal/PDP.h>
-#include <hal/Ports.h>
-#include <wpi/SmallString.h>
-#include <wpi/raw_ostream.h>
-
-#include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-PowerDistributionPanel::PowerDistributionPanel() : PowerDistributionPanel(0) {}
-
-/**
- * Initialize the PDP.
- */
-PowerDistributionPanel::PowerDistributionPanel(int module) : m_module(module) {
-  int32_t status = 0;
-  m_handle = HAL_InitializePDP(module, &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumPDPModules(), module);
-    return;
-  }
-
-  HAL_Report(HALUsageReporting::kResourceType_PDP, module + 1);
-  SendableRegistry::GetInstance().AddLW(this, "PowerDistributionPanel", module);
-}
-
-double PowerDistributionPanel::GetVoltage() const {
-  int32_t status = 0;
-
-  double voltage = HAL_GetPDPVoltage(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-
-  return voltage;
-}
-
-double PowerDistributionPanel::GetTemperature() const {
-  int32_t status = 0;
-
-  double temperature = HAL_GetPDPTemperature(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-
-  return temperature;
-}
-
-double PowerDistributionPanel::GetCurrent(int channel) const {
-  int32_t status = 0;
-
-  if (!SensorUtil::CheckPDPChannel(channel)) {
-    wpi::SmallString<32> str;
-    wpi::raw_svector_ostream buf(str);
-    buf << "PDP Channel " << channel;
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, buf.str());
-  }
-
-  double current = HAL_GetPDPChannelCurrent(m_handle, channel, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-
-  return current;
-}
-
-double PowerDistributionPanel::GetTotalCurrent() const {
-  int32_t status = 0;
-
-  double current = HAL_GetPDPTotalCurrent(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-
-  return current;
-}
-
-double PowerDistributionPanel::GetTotalPower() const {
-  int32_t status = 0;
-
-  double power = HAL_GetPDPTotalPower(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-
-  return power;
-}
-
-double PowerDistributionPanel::GetTotalEnergy() const {
-  int32_t status = 0;
-
-  double energy = HAL_GetPDPTotalEnergy(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-
-  return energy;
-}
-
-void PowerDistributionPanel::ResetTotalEnergy() {
-  int32_t status = 0;
-
-  HAL_ResetPDPTotalEnergy(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-}
-
-void PowerDistributionPanel::ClearStickyFaults() {
-  int32_t status = 0;
-
-  HAL_ClearPDPStickyFaults(m_handle, &status);
-
-  if (status) {
-    wpi_setWPIErrorWithContext(Timeout, "");
-  }
-}
-
-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(
-      "Voltage", [=]() { return GetVoltage(); }, nullptr);
-  builder.AddDoubleProperty(
-      "TotalCurrent", [=]() { return GetTotalCurrent(); }, nullptr);
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Preferences.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Preferences.cpp
index 8c1eb87..0c82f54 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -1,128 +1,168 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Preferences.h"
 
 #include <algorithm>
 
 #include <hal/FRCUsageReporting.h>
+#include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
-#include <wpi/StringRef.h>
-
-#include "frc/WPIErrors.h"
 
 using namespace frc;
 
 // The Preferences table name
-static wpi::StringRef kTableName{"Preferences"};
+static constexpr std::string_view kTableName{"Preferences"};
+
+namespace {
+struct Instance {
+  Instance();
+
+  std::shared_ptr<nt::NetworkTable> table{
+      nt::NetworkTableInstance::GetDefault().GetTable(kTableName)};
+  NT_EntryListener listener;
+};
+}  // namespace
+
+static Instance& GetInstance() {
+  static Instance instance;
+  return instance;
+}
 
 Preferences* Preferences::GetInstance() {
+  ::GetInstance();
   static Preferences instance;
   return &instance;
 }
 
-std::vector<std::string> Preferences::GetKeys() { return m_table->GetKeys(); }
-
-std::string Preferences::GetString(wpi::StringRef key,
-                                   wpi::StringRef defaultValue) {
-  return m_table->GetString(key, defaultValue);
+std::vector<std::string> Preferences::GetKeys() {
+  return ::GetInstance().table->GetKeys();
 }
 
-int Preferences::GetInt(wpi::StringRef key, int defaultValue) {
-  return static_cast<int>(m_table->GetNumber(key, defaultValue));
+std::string Preferences::GetString(std::string_view key,
+                                   std::string_view defaultValue) {
+  return ::GetInstance().table->GetString(key, defaultValue);
 }
 
-double Preferences::GetDouble(wpi::StringRef key, double defaultValue) {
-  return m_table->GetNumber(key, defaultValue);
+int Preferences::GetInt(std::string_view key, int defaultValue) {
+  return static_cast<int>(::GetInstance().table->GetNumber(key, defaultValue));
 }
 
-float Preferences::GetFloat(wpi::StringRef key, float defaultValue) {
-  return m_table->GetNumber(key, defaultValue);
+double Preferences::GetDouble(std::string_view key, double defaultValue) {
+  return ::GetInstance().table->GetNumber(key, defaultValue);
 }
 
-bool Preferences::GetBoolean(wpi::StringRef key, bool defaultValue) {
-  return m_table->GetBoolean(key, defaultValue);
+float Preferences::GetFloat(std::string_view key, float defaultValue) {
+  return ::GetInstance().table->GetNumber(key, defaultValue);
 }
 
-int64_t Preferences::GetLong(wpi::StringRef key, int64_t defaultValue) {
-  return static_cast<int64_t>(m_table->GetNumber(key, defaultValue));
+bool Preferences::GetBoolean(std::string_view key, bool defaultValue) {
+  return ::GetInstance().table->GetBoolean(key, defaultValue);
 }
 
-void Preferences::PutString(wpi::StringRef key, wpi::StringRef value) {
-  auto entry = m_table->GetEntry(key);
+int64_t Preferences::GetLong(std::string_view key, int64_t defaultValue) {
+  return static_cast<int64_t>(
+      ::GetInstance().table->GetNumber(key, defaultValue));
+}
+
+void Preferences::SetString(std::string_view key, std::string_view value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetString(value);
   entry.SetPersistent();
 }
 
-void Preferences::InitString(wpi::StringRef key, wpi::StringRef value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::PutString(std::string_view key, std::string_view value) {
+  SetString(key, value);
+}
+
+void Preferences::InitString(std::string_view key, std::string_view value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultString(value);
 }
 
-void Preferences::PutInt(wpi::StringRef key, int value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::SetInt(std::string_view key, int value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
-void Preferences::InitInt(wpi::StringRef key, int value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::PutInt(std::string_view key, int value) {
+  SetInt(key, value);
+}
+
+void Preferences::InitInt(std::string_view key, int value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
 }
 
-void Preferences::PutDouble(wpi::StringRef key, double value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::SetDouble(std::string_view key, double value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
-void Preferences::InitDouble(wpi::StringRef key, double value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::PutDouble(std::string_view key, double value) {
+  SetDouble(key, value);
+}
+
+void Preferences::InitDouble(std::string_view key, double value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
 }
 
-void Preferences::PutFloat(wpi::StringRef key, float value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::SetFloat(std::string_view key, float value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
-void Preferences::InitFloat(wpi::StringRef key, float value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::PutFloat(std::string_view key, float value) {
+  SetFloat(key, value);
+}
+
+void Preferences::InitFloat(std::string_view key, float value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
 }
 
-void Preferences::PutBoolean(wpi::StringRef key, bool value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::SetBoolean(std::string_view key, bool value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetBoolean(value);
   entry.SetPersistent();
 }
 
-void Preferences::InitBoolean(wpi::StringRef key, bool value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::PutBoolean(std::string_view key, bool value) {
+  SetBoolean(key, value);
+}
+
+void Preferences::InitBoolean(std::string_view key, bool value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultBoolean(value);
 }
 
-void Preferences::PutLong(wpi::StringRef key, int64_t value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::SetLong(std::string_view key, int64_t value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDouble(value);
   entry.SetPersistent();
 }
 
-void Preferences::InitLong(wpi::StringRef key, int64_t value) {
-  auto entry = m_table->GetEntry(key);
+void Preferences::PutLong(std::string_view key, int64_t value) {
+  SetLong(key, value);
+}
+
+void Preferences::InitLong(std::string_view key, int64_t value) {
+  auto entry = ::GetInstance().table->GetEntry(key);
   entry.SetDefaultDouble(value);
 }
 
-bool Preferences::ContainsKey(wpi::StringRef key) {
-  return m_table->ContainsKey(key);
+bool Preferences::ContainsKey(std::string_view key) {
+  return ::GetInstance().table->ContainsKey(key);
 }
 
-void Preferences::Remove(wpi::StringRef key) { m_table->Delete(key); }
+void Preferences::Remove(std::string_view key) {
+  ::GetInstance().table->Delete(key);
+}
 
 void Preferences::RemoveAll() {
   for (auto preference : GetKeys()) {
@@ -132,11 +172,10 @@
   }
 }
 
-Preferences::Preferences()
-    : m_table(nt::NetworkTableInstance::GetDefault().GetTable(kTableName)) {
-  m_table->GetEntry(".type").SetString("RobotPreferences");
-  m_listener = m_table->AddEntryListener(
-      [=](nt::NetworkTable* table, wpi::StringRef name,
+Instance::Instance() {
+  table->GetEntry(".type").SetString("RobotPreferences");
+  listener = table->AddEntryListener(
+      [=](nt::NetworkTable* table, std::string_view name,
           nt::NetworkTableEntry entry, std::shared_ptr<nt::Value> value,
           int flags) { entry.SetPersistent(); },
       NT_NOTIFY_NEW | NT_NOTIFY_IMMEDIATE);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Relay.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Relay.cpp
index cb95223..9bec566 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Relay.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Relay.cpp
@@ -1,32 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Relay.h"
 
 #include <utility>
 
+#include <fmt/format.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <hal/Ports.h>
 #include <hal/Relay.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/StackTrace.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
 Relay::Relay(int channel, Relay::Direction direction)
     : m_channel(channel), m_direction(direction) {
   if (!SensorUtil::CheckRelayChannel(m_channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "Relay Channel " + wpi::Twine(m_channel));
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", m_channel);
     return;
   }
 
@@ -34,49 +31,32 @@
 
   if (m_direction == kBothDirections || m_direction == kForwardOnly) {
     int32_t status = 0;
-    m_forwardHandle = HAL_InitializeRelayPort(portHandle, true, &status);
-    if (status != 0) {
-      wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
-      m_forwardHandle = HAL_kInvalidHandle;
-      m_reverseHandle = HAL_kInvalidHandle;
-      return;
-    }
+    std::string stackTrace = wpi::GetStackTrace(1);
+    m_forwardHandle =
+        HAL_InitializeRelayPort(portHandle, true, stackTrace.c_str(), &status);
+    FRC_CheckErrorStatus(status, "Channel {}", m_channel);
     HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel + 1);
   }
   if (m_direction == kBothDirections || m_direction == kReverseOnly) {
     int32_t status = 0;
-    m_reverseHandle = HAL_InitializeRelayPort(portHandle, false, &status);
-    if (status != 0) {
-      wpi_setHALErrorWithRange(status, 0, HAL_GetNumRelayChannels(), channel);
-      m_forwardHandle = HAL_kInvalidHandle;
-      m_reverseHandle = HAL_kInvalidHandle;
-      return;
-    }
-
+    std::string stackTrace = wpi::GetStackTrace(1);
+    m_reverseHandle =
+        HAL_InitializeRelayPort(portHandle, false, stackTrace.c_str(), &status);
+    FRC_CheckErrorStatus(status, "Channel {}", m_channel);
     HAL_Report(HALUsageReporting::kResourceType_Relay, m_channel + 128);
   }
 
   int32_t status = 0;
   if (m_forwardHandle != HAL_kInvalidHandle) {
     HAL_SetRelay(m_forwardHandle, false, &status);
-    if (status != 0) {
-      wpi_setHALError(status);
-      m_forwardHandle = HAL_kInvalidHandle;
-      m_reverseHandle = HAL_kInvalidHandle;
-      return;
-    }
+    FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   }
   if (m_reverseHandle != HAL_kInvalidHandle) {
     HAL_SetRelay(m_reverseHandle, false, &status);
-    if (status != 0) {
-      wpi_setHALError(status);
-      m_forwardHandle = HAL_kInvalidHandle;
-      m_reverseHandle = HAL_kInvalidHandle;
-      return;
-    }
+    FRC_CheckErrorStatus(status, "Channel {}", m_channel);
   }
 
-  SendableRegistry::GetInstance().AddLW(this, "Relay", m_channel);
+  wpi::SendableRegistry::AddLW(this, "Relay", m_channel);
 }
 
 Relay::~Relay() {
@@ -84,13 +64,15 @@
   HAL_SetRelay(m_forwardHandle, false, &status);
   HAL_SetRelay(m_reverseHandle, false, &status);
   // ignore errors, as we want to make sure a free happens.
-  if (m_forwardHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_forwardHandle);
-  if (m_reverseHandle != HAL_kInvalidHandle) HAL_FreeRelayPort(m_reverseHandle);
+  if (m_forwardHandle != HAL_kInvalidHandle) {
+    HAL_FreeRelayPort(m_forwardHandle);
+  }
+  if (m_reverseHandle != HAL_kInvalidHandle) {
+    HAL_FreeRelayPort(m_reverseHandle);
+  }
 }
 
 void Relay::Set(Relay::Value value) {
-  if (StatusIsFatal()) return;
-
   int32_t status = 0;
 
   switch (value) {
@@ -112,7 +94,8 @@
       break;
     case kForward:
       if (m_direction == kReverseOnly) {
-        wpi_setWPIError(IncompatibleMode);
+        FRC_ReportError(err::IncompatibleMode, "channel {} setting {}",
+                        m_channel, "forward");
         break;
       }
       if (m_direction == kBothDirections || m_direction == kForwardOnly) {
@@ -124,7 +107,8 @@
       break;
     case kReverse:
       if (m_direction == kForwardOnly) {
-        wpi_setWPIError(IncompatibleMode);
+        FRC_ReportError(err::IncompatibleMode, "channel {} setting {}",
+                        m_channel, "reverse");
         break;
       }
       if (m_direction == kBothDirections) {
@@ -136,58 +120,65 @@
       break;
   }
 
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
 }
 
 Relay::Value Relay::Get() const {
-  int32_t status;
+  Relay::Value value = kOff;
+  int32_t status = 0;
 
   if (m_direction == kForwardOnly) {
     if (HAL_GetRelay(m_forwardHandle, &status)) {
-      return kOn;
+      value = kOn;
     } else {
-      return kOff;
+      value = kOff;
     }
   } else if (m_direction == kReverseOnly) {
     if (HAL_GetRelay(m_reverseHandle, &status)) {
-      return kOn;
+      value = kOn;
     } else {
-      return kOff;
+      value = kOff;
     }
   } else {
     if (HAL_GetRelay(m_forwardHandle, &status)) {
       if (HAL_GetRelay(m_reverseHandle, &status)) {
-        return kOn;
+        value = kOn;
       } else {
-        return kForward;
+        value = kForward;
       }
     } else {
       if (HAL_GetRelay(m_reverseHandle, &status)) {
-        return kReverse;
+        value = kReverse;
       } else {
-        return kOff;
+        value = kOff;
       }
     }
   }
 
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Channel {}", m_channel);
+
+  return value;
 }
 
-int Relay::GetChannel() const { return m_channel; }
-
-void Relay::StopMotor() { Set(kOff); }
-
-void Relay::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "Relay " << GetChannel();
+int Relay::GetChannel() const {
+  return m_channel;
 }
 
-void Relay::InitSendable(SendableBuilder& builder) {
+void Relay::StopMotor() {
+  Set(kOff);
+}
+
+std::string Relay::GetDescription() const {
+  return fmt::format("Relay {}", GetChannel());
+}
+
+void Relay::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Relay");
   builder.SetActuator(true);
-  builder.SetSafeState([=]() { Set(kOff); });
+  builder.SetSafeState([=] { Set(kOff); });
   builder.AddSmallStringProperty(
       "Value",
-      [=](wpi::SmallVectorImpl<char>& buf) -> wpi::StringRef {
+      [=](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
         switch (Get()) {
           case kOn:
             return "On";
@@ -199,14 +190,15 @@
             return "Off";
         }
       },
-      [=](wpi::StringRef value) {
-        if (value == "Off")
+      [=](std::string_view value) {
+        if (value == "Off") {
           Set(kOff);
-        else if (value == "Forward")
+        } else if (value == "Forward") {
           Set(kForward);
-        else if (value == "Reverse")
+        } else if (value == "Reverse") {
           Set(kReverse);
-        else if (value == "On")
+        } else if (value == "On") {
           Set(kOn);
+        }
       });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Resource.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Resource.cpp
index d546461..1f0e348 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Resource.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Resource.cpp
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Resource.h"
 
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
@@ -34,19 +30,16 @@
       return i;
     }
   }
-  wpi_setWPIErrorWithContext(NoAvailableResources, resourceDesc);
-  return std::numeric_limits<uint32_t>::max();
+  throw FRC_MakeError(err::NoAvailableResources, "{}", resourceDesc);
 }
 
 uint32_t Resource::Allocate(uint32_t index, const std::string& resourceDesc) {
   std::scoped_lock lock(m_allocateMutex);
   if (index >= m_isAllocated.size()) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange, resourceDesc);
-    return std::numeric_limits<uint32_t>::max();
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "{}", resourceDesc);
   }
   if (m_isAllocated[index]) {
-    wpi_setWPIErrorWithContext(ResourceAlreadyAllocated, resourceDesc);
-    return std::numeric_limits<uint32_t>::max();
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", resourceDesc);
   }
   m_isAllocated[index] = true;
   return index;
@@ -54,14 +47,14 @@
 
 void Resource::Free(uint32_t index) {
   std::unique_lock lock(m_allocateMutex);
-  if (index == std::numeric_limits<uint32_t>::max()) return;
-  if (index >= m_isAllocated.size()) {
-    wpi_setWPIError(NotAllocated);
+  if (index == std::numeric_limits<uint32_t>::max()) {
     return;
   }
+  if (index >= m_isAllocated.size()) {
+    throw FRC_MakeError(err::NotAllocated, "index {}", index);
+  }
   if (!m_isAllocated[index]) {
-    wpi_setWPIError(NotAllocated);
-    return;
+    throw FRC_MakeError(err::NotAllocated, "index {}", index);
   }
   m_isAllocated[index] = false;
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp
index b33479f..8bc2d6b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotController.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/RobotController.h"
 
@@ -11,152 +8,170 @@
 #include <hal/HALBase.h>
 #include <hal/Power.h>
 
-#include "frc/ErrorBase.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
 int RobotController::GetFPGAVersion() {
   int32_t status = 0;
   int version = HAL_GetFPGAVersion(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFPGAVersion");
   return version;
 }
 
 int64_t RobotController::GetFPGARevision() {
   int32_t status = 0;
   int64_t revision = HAL_GetFPGARevision(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFPGARevision");
   return revision;
 }
 
 uint64_t RobotController::GetFPGATime() {
   int32_t status = 0;
   uint64_t time = HAL_GetFPGATime(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFPGATime");
   return time;
 }
 
 bool RobotController::GetUserButton() {
   int32_t status = 0;
-
   bool value = HAL_GetFPGAButton(&status);
-  wpi_setGlobalError(status);
-
+  FRC_CheckErrorStatus(status, "{}", "GetUserButton");
   return value;
 }
 
+units::volt_t RobotController::GetBatteryVoltage() {
+  int32_t status = 0;
+  double retVal = HAL_GetVinVoltage(&status);
+  FRC_CheckErrorStatus(status, "{}", "GetBatteryVoltage");
+  return units::volt_t{retVal};
+}
+
 bool RobotController::IsSysActive() {
   int32_t status = 0;
   bool retVal = HAL_GetSystemActive(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "IsSysActive");
   return retVal;
 }
 
 bool RobotController::IsBrownedOut() {
   int32_t status = 0;
   bool retVal = HAL_GetBrownedOut(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "IsBrownedOut");
   return retVal;
 }
 
 double RobotController::GetInputVoltage() {
   int32_t status = 0;
   double retVal = HAL_GetVinVoltage(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetInputVoltage");
   return retVal;
 }
 
 double RobotController::GetInputCurrent() {
   int32_t status = 0;
   double retVal = HAL_GetVinCurrent(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetInputCurrent");
   return retVal;
 }
 
 double RobotController::GetVoltage3V3() {
   int32_t status = 0;
   double retVal = HAL_GetUserVoltage3V3(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetVoltage3V3");
   return retVal;
 }
 
 double RobotController::GetCurrent3V3() {
   int32_t status = 0;
   double retVal = HAL_GetUserCurrent3V3(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetCurrent3V3");
   return retVal;
 }
 
 bool RobotController::GetEnabled3V3() {
   int32_t status = 0;
   bool retVal = HAL_GetUserActive3V3(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetEnabled3V3");
   return retVal;
 }
 
 int RobotController::GetFaultCount3V3() {
   int32_t status = 0;
   int retVal = HAL_GetUserCurrentFaults3V3(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFaultCount3V3");
   return retVal;
 }
 
 double RobotController::GetVoltage5V() {
   int32_t status = 0;
   double retVal = HAL_GetUserVoltage5V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetVoltage5V");
   return retVal;
 }
 
 double RobotController::GetCurrent5V() {
   int32_t status = 0;
   double retVal = HAL_GetUserCurrent5V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetCurrent5V");
   return retVal;
 }
 
 bool RobotController::GetEnabled5V() {
   int32_t status = 0;
   bool retVal = HAL_GetUserActive5V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetEnabled5V");
   return retVal;
 }
 
 int RobotController::GetFaultCount5V() {
   int32_t status = 0;
   int retVal = HAL_GetUserCurrentFaults5V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFaultCount5V");
   return retVal;
 }
 
 double RobotController::GetVoltage6V() {
   int32_t status = 0;
   double retVal = HAL_GetUserVoltage6V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetVoltage6V");
   return retVal;
 }
 
 double RobotController::GetCurrent6V() {
   int32_t status = 0;
   double retVal = HAL_GetUserCurrent6V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetCurrent6V");
   return retVal;
 }
 
 bool RobotController::GetEnabled6V() {
   int32_t status = 0;
   bool retVal = HAL_GetUserActive6V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetEnabled6V");
   return retVal;
 }
 
 int RobotController::GetFaultCount6V() {
   int32_t status = 0;
   int retVal = HAL_GetUserCurrentFaults6V(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetFaultCount6V");
   return retVal;
 }
 
+units::volt_t RobotController::GetBrownoutVoltage() {
+  int32_t status = 0;
+  double retVal = HAL_GetBrownoutVoltage(&status);
+  FRC_CheckErrorStatus(status, "{}", "GetBrownoutVoltage");
+  return units::volt_t(retVal);
+}
+
+void RobotController::SetBrownoutVoltage(units::volt_t brownoutVoltage) {
+  int32_t status = 0;
+  HAL_SetBrownoutVoltage(brownoutVoltage.value(), &status);
+  FRC_CheckErrorStatus(status, "{}", "SetBrownoutVoltage");
+}
+
 CANStatus RobotController::GetCANStatus() {
   int32_t status = 0;
   float percentBusUtilization = 0;
@@ -166,10 +181,7 @@
   uint32_t transmitErrorCount = 0;
   HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
                        &receiveErrorCount, &transmitErrorCount, &status);
-  if (status != 0) {
-    wpi_setGlobalHALError(status);
-    return {};
-  }
+  FRC_CheckErrorStatus(status, "{}", "GetCANStatus");
   return {percentBusUtilization, static_cast<int>(busOffCount),
           static_cast<int>(txFullCount), static_cast<int>(receiveErrorCount),
           static_cast<int>(transmitErrorCount)};
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotDrive.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotDrive.cpp
deleted file mode 100644
index 9eafe0c..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotDrive.cpp
+++ /dev/null
@@ -1,427 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/RobotDrive.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/GenericHID.h"
-#include "frc/Joystick.h"
-#include "frc/Talon.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
-
-using namespace frc;
-
-static std::shared_ptr<SpeedController> make_shared_nodelete(
-    SpeedController* ptr) {
-  return std::shared_ptr<SpeedController>(ptr, NullDeleter<SpeedController>());
-}
-
-RobotDrive::RobotDrive(int leftMotorChannel, int rightMotorChannel) {
-  InitRobotDrive();
-  m_rearLeftMotor = std::make_shared<Talon>(leftMotorChannel);
-  m_rearRightMotor = std::make_shared<Talon>(rightMotorChannel);
-  SetLeftRightMotorOutputs(0.0, 0.0);
-}
-
-RobotDrive::RobotDrive(int frontLeftMotor, int rearLeftMotor,
-                       int frontRightMotor, int rearRightMotor) {
-  InitRobotDrive();
-  m_rearLeftMotor = std::make_shared<Talon>(rearLeftMotor);
-  m_rearRightMotor = std::make_shared<Talon>(rearRightMotor);
-  m_frontLeftMotor = std::make_shared<Talon>(frontLeftMotor);
-  m_frontRightMotor = std::make_shared<Talon>(frontRightMotor);
-  SetLeftRightMotorOutputs(0.0, 0.0);
-}
-
-RobotDrive::RobotDrive(SpeedController* leftMotor,
-                       SpeedController* rightMotor) {
-  InitRobotDrive();
-  if (leftMotor == nullptr || rightMotor == nullptr) {
-    wpi_setWPIError(NullParameter);
-    m_rearLeftMotor = m_rearRightMotor = nullptr;
-    return;
-  }
-  m_rearLeftMotor = make_shared_nodelete(leftMotor);
-  m_rearRightMotor = make_shared_nodelete(rightMotor);
-}
-
-RobotDrive::RobotDrive(SpeedController& leftMotor,
-                       SpeedController& rightMotor) {
-  InitRobotDrive();
-  m_rearLeftMotor = make_shared_nodelete(&leftMotor);
-  m_rearRightMotor = make_shared_nodelete(&rightMotor);
-}
-
-RobotDrive::RobotDrive(std::shared_ptr<SpeedController> leftMotor,
-                       std::shared_ptr<SpeedController> rightMotor) {
-  InitRobotDrive();
-  if (leftMotor == nullptr || rightMotor == nullptr) {
-    wpi_setWPIError(NullParameter);
-    m_rearLeftMotor = m_rearRightMotor = nullptr;
-    return;
-  }
-  m_rearLeftMotor = leftMotor;
-  m_rearRightMotor = rightMotor;
-}
-
-RobotDrive::RobotDrive(SpeedController* frontLeftMotor,
-                       SpeedController* rearLeftMotor,
-                       SpeedController* frontRightMotor,
-                       SpeedController* rearRightMotor) {
-  InitRobotDrive();
-  if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
-      frontRightMotor == nullptr || rearRightMotor == nullptr) {
-    wpi_setWPIError(NullParameter);
-    return;
-  }
-  m_frontLeftMotor = make_shared_nodelete(frontLeftMotor);
-  m_rearLeftMotor = make_shared_nodelete(rearLeftMotor);
-  m_frontRightMotor = make_shared_nodelete(frontRightMotor);
-  m_rearRightMotor = make_shared_nodelete(rearRightMotor);
-}
-
-RobotDrive::RobotDrive(SpeedController& frontLeftMotor,
-                       SpeedController& rearLeftMotor,
-                       SpeedController& frontRightMotor,
-                       SpeedController& rearRightMotor) {
-  InitRobotDrive();
-  m_frontLeftMotor = make_shared_nodelete(&frontLeftMotor);
-  m_rearLeftMotor = make_shared_nodelete(&rearLeftMotor);
-  m_frontRightMotor = make_shared_nodelete(&frontRightMotor);
-  m_rearRightMotor = make_shared_nodelete(&rearRightMotor);
-}
-
-RobotDrive::RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
-                       std::shared_ptr<SpeedController> rearLeftMotor,
-                       std::shared_ptr<SpeedController> frontRightMotor,
-                       std::shared_ptr<SpeedController> rearRightMotor) {
-  InitRobotDrive();
-  if (frontLeftMotor == nullptr || rearLeftMotor == nullptr ||
-      frontRightMotor == nullptr || rearRightMotor == nullptr) {
-    wpi_setWPIError(NullParameter);
-    return;
-  }
-  m_frontLeftMotor = frontLeftMotor;
-  m_rearLeftMotor = rearLeftMotor;
-  m_frontRightMotor = frontRightMotor;
-  m_rearRightMotor = rearRightMotor;
-}
-
-void RobotDrive::Drive(double outputMagnitude, double curve) {
-  double leftOutput, rightOutput;
-  static bool reported = false;
-  if (!reported) {
-    HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
-               HALUsageReporting::kRobotDrive_ArcadeRatioCurve, GetNumMotors());
-    reported = true;
-  }
-
-  if (curve < 0) {
-    double value = std::log(-curve);
-    double ratio = (value - m_sensitivity) / (value + m_sensitivity);
-    if (ratio == 0) ratio = 0.0000000001;
-    leftOutput = outputMagnitude / ratio;
-    rightOutput = outputMagnitude;
-  } else if (curve > 0) {
-    double value = std::log(curve);
-    double ratio = (value - m_sensitivity) / (value + m_sensitivity);
-    if (ratio == 0) ratio = 0.0000000001;
-    leftOutput = outputMagnitude;
-    rightOutput = outputMagnitude / ratio;
-  } else {
-    leftOutput = outputMagnitude;
-    rightOutput = outputMagnitude;
-  }
-  SetLeftRightMotorOutputs(leftOutput, rightOutput);
-}
-
-void RobotDrive::TankDrive(GenericHID* leftStick, GenericHID* rightStick,
-                           bool squaredInputs) {
-  if (leftStick == nullptr || rightStick == nullptr) {
-    wpi_setWPIError(NullParameter);
-    return;
-  }
-  TankDrive(leftStick->GetY(), rightStick->GetY(), squaredInputs);
-}
-
-void RobotDrive::TankDrive(GenericHID& leftStick, GenericHID& rightStick,
-                           bool squaredInputs) {
-  TankDrive(leftStick.GetY(), rightStick.GetY(), squaredInputs);
-}
-
-void RobotDrive::TankDrive(GenericHID* leftStick, int leftAxis,
-                           GenericHID* rightStick, int rightAxis,
-                           bool squaredInputs) {
-  if (leftStick == nullptr || rightStick == nullptr) {
-    wpi_setWPIError(NullParameter);
-    return;
-  }
-  TankDrive(leftStick->GetRawAxis(leftAxis), rightStick->GetRawAxis(rightAxis),
-            squaredInputs);
-}
-
-void RobotDrive::TankDrive(GenericHID& leftStick, int leftAxis,
-                           GenericHID& rightStick, int rightAxis,
-                           bool squaredInputs) {
-  TankDrive(leftStick.GetRawAxis(leftAxis), rightStick.GetRawAxis(rightAxis),
-            squaredInputs);
-}
-
-void RobotDrive::TankDrive(double leftValue, double rightValue,
-                           bool squaredInputs) {
-  static bool reported = false;
-  if (!reported) {
-    HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
-               HALUsageReporting::kRobotDrive_Tank, GetNumMotors());
-    reported = true;
-  }
-
-  leftValue = Limit(leftValue);
-  rightValue = Limit(rightValue);
-
-  // square the inputs (while preserving the sign) to increase fine control
-  // while permitting full power
-  if (squaredInputs) {
-    leftValue = std::copysign(leftValue * leftValue, leftValue);
-    rightValue = std::copysign(rightValue * rightValue, rightValue);
-  }
-
-  SetLeftRightMotorOutputs(leftValue, rightValue);
-}
-
-void RobotDrive::ArcadeDrive(GenericHID* stick, bool squaredInputs) {
-  // simply call the full-featured ArcadeDrive with the appropriate values
-  ArcadeDrive(stick->GetY(), stick->GetX(), squaredInputs);
-}
-
-void RobotDrive::ArcadeDrive(GenericHID& stick, bool squaredInputs) {
-  // simply call the full-featured ArcadeDrive with the appropriate values
-  ArcadeDrive(stick.GetY(), stick.GetX(), squaredInputs);
-}
-
-void RobotDrive::ArcadeDrive(GenericHID* moveStick, int moveAxis,
-                             GenericHID* rotateStick, int rotateAxis,
-                             bool squaredInputs) {
-  double moveValue = moveStick->GetRawAxis(moveAxis);
-  double rotateValue = rotateStick->GetRawAxis(rotateAxis);
-
-  ArcadeDrive(moveValue, rotateValue, squaredInputs);
-}
-
-void RobotDrive::ArcadeDrive(GenericHID& moveStick, int moveAxis,
-                             GenericHID& rotateStick, int rotateAxis,
-                             bool squaredInputs) {
-  double moveValue = moveStick.GetRawAxis(moveAxis);
-  double rotateValue = rotateStick.GetRawAxis(rotateAxis);
-
-  ArcadeDrive(moveValue, rotateValue, squaredInputs);
-}
-
-void RobotDrive::ArcadeDrive(double moveValue, double rotateValue,
-                             bool squaredInputs) {
-  static bool reported = false;
-  if (!reported) {
-    HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
-               HALUsageReporting::kRobotDrive_ArcadeStandard, GetNumMotors());
-    reported = true;
-  }
-
-  // local variables to hold the computed PWM values for the motors
-  double leftMotorOutput;
-  double rightMotorOutput;
-
-  moveValue = Limit(moveValue);
-  rotateValue = Limit(rotateValue);
-
-  // square the inputs (while preserving the sign) to increase fine control
-  // while permitting full power
-  if (squaredInputs) {
-    moveValue = std::copysign(moveValue * moveValue, moveValue);
-    rotateValue = std::copysign(rotateValue * rotateValue, rotateValue);
-  }
-
-  if (moveValue > 0.0) {
-    if (rotateValue > 0.0) {
-      leftMotorOutput = moveValue - rotateValue;
-      rightMotorOutput = std::max(moveValue, rotateValue);
-    } else {
-      leftMotorOutput = std::max(moveValue, -rotateValue);
-      rightMotorOutput = moveValue + rotateValue;
-    }
-  } else {
-    if (rotateValue > 0.0) {
-      leftMotorOutput = -std::max(-moveValue, rotateValue);
-      rightMotorOutput = moveValue + rotateValue;
-    } else {
-      leftMotorOutput = moveValue - rotateValue;
-      rightMotorOutput = -std::max(-moveValue, -rotateValue);
-    }
-  }
-  SetLeftRightMotorOutputs(leftMotorOutput, rightMotorOutput);
-}
-
-void RobotDrive::MecanumDrive_Cartesian(double x, double y, double rotation,
-                                        double gyroAngle) {
-  static bool reported = false;
-  if (!reported) {
-    HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
-               HALUsageReporting::kRobotDrive_MecanumCartesian, GetNumMotors());
-    reported = true;
-  }
-
-  double xIn = x;
-  double yIn = y;
-  // Negate y for the joystick.
-  yIn = -yIn;
-  // Compensate for gyro angle.
-  RotateVector(xIn, yIn, gyroAngle);
-
-  double wheelSpeeds[kMaxNumberOfMotors];
-  wheelSpeeds[kFrontLeftMotor] = xIn + yIn + rotation;
-  wheelSpeeds[kFrontRightMotor] = -xIn + yIn - rotation;
-  wheelSpeeds[kRearLeftMotor] = -xIn + yIn + rotation;
-  wheelSpeeds[kRearRightMotor] = xIn + yIn - rotation;
-
-  Normalize(wheelSpeeds);
-
-  m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
-  m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
-  m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
-  m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
-
-  Feed();
-}
-
-void RobotDrive::MecanumDrive_Polar(double magnitude, double direction,
-                                    double rotation) {
-  static bool reported = false;
-  if (!reported) {
-    HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
-               HALUsageReporting::kRobotDrive_MecanumPolar, GetNumMotors());
-    reported = true;
-  }
-
-  // Normalized for full power along the Cartesian axes.
-  magnitude = Limit(magnitude) * std::sqrt(2.0);
-  // The rollers are at 45 degree angles.
-  double dirInRad = (direction + 45.0) * 3.14159 / 180.0;
-  double cosD = std::cos(dirInRad);
-  double sinD = std::sin(dirInRad);
-
-  double wheelSpeeds[kMaxNumberOfMotors];
-  wheelSpeeds[kFrontLeftMotor] = sinD * magnitude + rotation;
-  wheelSpeeds[kFrontRightMotor] = cosD * magnitude - rotation;
-  wheelSpeeds[kRearLeftMotor] = cosD * magnitude + rotation;
-  wheelSpeeds[kRearRightMotor] = sinD * magnitude - rotation;
-
-  Normalize(wheelSpeeds);
-
-  m_frontLeftMotor->Set(wheelSpeeds[kFrontLeftMotor] * m_maxOutput);
-  m_frontRightMotor->Set(wheelSpeeds[kFrontRightMotor] * m_maxOutput);
-  m_rearLeftMotor->Set(wheelSpeeds[kRearLeftMotor] * m_maxOutput);
-  m_rearRightMotor->Set(wheelSpeeds[kRearRightMotor] * m_maxOutput);
-
-  Feed();
-}
-
-void RobotDrive::HolonomicDrive(double magnitude, double direction,
-                                double rotation) {
-  MecanumDrive_Polar(magnitude, direction, rotation);
-}
-
-void RobotDrive::SetLeftRightMotorOutputs(double leftOutput,
-                                          double rightOutput) {
-  wpi_assert(m_rearLeftMotor != nullptr && m_rearRightMotor != nullptr);
-
-  if (m_frontLeftMotor != nullptr)
-    m_frontLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
-  m_rearLeftMotor->Set(Limit(leftOutput) * m_maxOutput);
-
-  if (m_frontRightMotor != nullptr)
-    m_frontRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
-  m_rearRightMotor->Set(-Limit(rightOutput) * m_maxOutput);
-
-  Feed();
-}
-
-void RobotDrive::SetInvertedMotor(MotorType motor, bool isInverted) {
-  if (motor < 0 || motor > 3) {
-    wpi_setWPIError(InvalidMotorIndex);
-    return;
-  }
-  switch (motor) {
-    case kFrontLeftMotor:
-      m_frontLeftMotor->SetInverted(isInverted);
-      break;
-    case kFrontRightMotor:
-      m_frontRightMotor->SetInverted(isInverted);
-      break;
-    case kRearLeftMotor:
-      m_rearLeftMotor->SetInverted(isInverted);
-      break;
-    case kRearRightMotor:
-      m_rearRightMotor->SetInverted(isInverted);
-      break;
-  }
-}
-
-void RobotDrive::SetSensitivity(double sensitivity) {
-  m_sensitivity = sensitivity;
-}
-
-void RobotDrive::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
-
-void RobotDrive::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "RobotDrive";
-}
-
-void RobotDrive::StopMotor() {
-  if (m_frontLeftMotor != nullptr) m_frontLeftMotor->StopMotor();
-  if (m_frontRightMotor != nullptr) m_frontRightMotor->StopMotor();
-  if (m_rearLeftMotor != nullptr) m_rearLeftMotor->StopMotor();
-  if (m_rearRightMotor != nullptr) m_rearRightMotor->StopMotor();
-  Feed();
-}
-
-void RobotDrive::InitRobotDrive() { SetSafetyEnabled(true); }
-
-double RobotDrive::Limit(double number) {
-  if (number > 1.0) {
-    return 1.0;
-  }
-  if (number < -1.0) {
-    return -1.0;
-  }
-  return number;
-}
-
-void RobotDrive::Normalize(double* wheelSpeeds) {
-  double maxMagnitude = std::fabs(wheelSpeeds[0]);
-  for (int i = 1; i < kMaxNumberOfMotors; i++) {
-    double temp = std::fabs(wheelSpeeds[i]);
-    if (maxMagnitude < temp) maxMagnitude = temp;
-  }
-  if (maxMagnitude > 1.0) {
-    for (int i = 0; i < kMaxNumberOfMotors; i++) {
-      wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
-    }
-  }
-}
-
-void RobotDrive::RotateVector(double& x, double& y, double angle) {
-  double cosA = std::cos(angle * (3.14159 / 180.0));
-  double sinA = std::sin(angle * (3.14159 / 180.0));
-  double xOut = x * cosA - y * sinA;
-  double yOut = x * sinA + y * cosA;
-  x = xOut;
-  y = yOut;
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotState.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotState.cpp
index 530cee3..651a644 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotState.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/RobotState.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/RobotState.h"
 
@@ -12,23 +9,29 @@
 using namespace frc;
 
 bool RobotState::IsDisabled() {
-  return DriverStation::GetInstance().IsDisabled();
+  return DriverStation::IsDisabled();
 }
 
 bool RobotState::IsEnabled() {
-  return DriverStation::GetInstance().IsEnabled();
+  return DriverStation::IsEnabled();
 }
 
 bool RobotState::IsEStopped() {
-  return DriverStation::GetInstance().IsEStopped();
+  return DriverStation::IsEStopped();
 }
 
 bool RobotState::IsOperatorControl() {
-  return DriverStation::GetInstance().IsOperatorControl();
+  return IsTeleop();
+}
+
+bool RobotState::IsTeleop() {
+  return DriverStation::IsTeleop();
 }
 
 bool RobotState::IsAutonomous() {
-  return DriverStation::GetInstance().IsAutonomous();
+  return DriverStation::IsAutonomous();
 }
 
-bool RobotState::IsTest() { return DriverStation::GetInstance().IsTest(); }
+bool RobotState::IsTest() {
+  return DriverStation::IsTest();
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SD540.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SD540.cpp
deleted file mode 100644
index 9611f66..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SD540.cpp
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/SD540.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-SD540::SD540(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.05, 1.55, 1.50, 1.44, 0.94);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540,
-             GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "SD540", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SPI.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SPI.cpp
index d51fa3b..bdcee45 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SPI.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/SPI.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/SPI.h"
 
 #include <cstring>
+#include <memory>
 #include <utility>
 
 #include <hal/FRCUsageReporting.h>
@@ -16,8 +14,8 @@
 #include <wpi/mutex.h>
 
 #include "frc/DigitalSource.h"
+#include "frc/Errors.h"
 #include "frc/Notifier.h"
-#include "frc/WPIErrors.h"
 
 using namespace frc;
 
@@ -27,7 +25,7 @@
  public:
   Accumulator(HAL_SPIPort port, int xferSize, int validMask, int validValue,
               int dataShift, int dataSize, bool isSigned, bool bigEndian)
-      : m_notifier([=]() {
+      : m_notifier([=] {
           std::scoped_lock lock(m_mutex);
           Update();
         }),
@@ -79,7 +77,7 @@
     // get amount of data available
     int32_t numToRead =
         HAL_ReadSPIAutoReceivedData(m_port, m_buf, 0, 0, &status);
-    if (status != 0) return;  // error reading
+    FRC_CheckErrorStatus(status, "Port {}", m_port);
 
     // only get whole responses; +1 is for timestamp
     numToRead -= numToRead % m_xferSize;
@@ -87,11 +85,13 @@
       numToRead = m_xferSize * kAccumulateDepth;
       done = false;
     }
-    if (numToRead == 0) return;  // no samples
+    if (numToRead == 0) {
+      return;  // no samples
+    }
 
     // read buffered data
     HAL_ReadSPIAutoReceivedData(m_port, m_buf, numToRead, 0, &status);
-    if (status != 0) return;  // error reading
+    FRC_CheckErrorStatus(status, "Port {}", m_port);
 
     // loop over all responses
     for (int32_t off = 0; off < numToRead; off += m_xferSize) {
@@ -118,7 +118,9 @@
         int32_t data = static_cast<int32_t>(resp >> m_dataShift);
         data &= m_dataMax - 1;
         // 2s complement conversion if signed MSB is set
-        if (m_isSigned && (data & m_dataMsbMask) != 0) data -= m_dataMax;
+        if (m_isSigned && (data & m_dataMsbMask) != 0) {
+          data -= m_dataMax;
+        }
         // center offset
         int32_t dataNoCenter = data;
         data -= m_center;
@@ -127,18 +129,19 @@
           m_value += data;
           if (m_count != 0) {
             // timestamps use the 1us FPGA clock; also handle rollover
-            if (timestamp >= m_lastTimestamp)
+            if (timestamp >= m_lastTimestamp) {
               m_integratedValue +=
                   dataNoCenter *
                       static_cast<int32_t>(timestamp - m_lastTimestamp) * 1e-6 -
                   m_integratedCenter;
-            else
+            } else {
               m_integratedValue +=
                   dataNoCenter *
                       static_cast<int32_t>((1ULL << 32) - m_lastTimestamp +
                                            timestamp) *
                       1e-6 -
                   m_integratedCenter;
+            }
           }
         }
         ++m_count;
@@ -155,15 +158,23 @@
 SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
   int32_t status = 0;
   HAL_InitializeSPI(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 
   HAL_Report(HALUsageReporting::kResourceType_SPI,
              static_cast<uint8_t>(port) + 1);
 }
 
-SPI::~SPI() { HAL_CloseSPI(m_port); }
+SPI::~SPI() {
+  HAL_CloseSPI(m_port);
+}
 
-void SPI::SetClockRate(int hz) { HAL_SetSPISpeed(m_port, hz); }
+SPI::Port SPI::GetPort() const {
+  return static_cast<Port>(static_cast<int>(m_port));
+}
+
+void SPI::SetClockRate(int hz) {
+  HAL_SetSPISpeed(m_port, hz);
+}
 
 void SPI::SetMSBFirst() {
   m_msbFirst = true;
@@ -208,13 +219,13 @@
 void SPI::SetChipSelectActiveHigh() {
   int32_t status = 0;
   HAL_SetSPIChipSelectActiveHigh(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::SetChipSelectActiveLow() {
   int32_t status = 0;
   HAL_SetSPIChipSelectActiveLow(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 int SPI::Write(uint8_t* data, int size) {
@@ -244,26 +255,27 @@
 void SPI::InitAuto(int bufferSize) {
   int32_t status = 0;
   HAL_InitSPIAuto(m_port, bufferSize, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::FreeAuto() {
   int32_t status = 0;
   HAL_FreeSPIAuto(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
-void SPI::SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize) {
+void SPI::SetAutoTransmitData(wpi::span<const uint8_t> dataToSend,
+                              int zeroSize) {
   int32_t status = 0;
   HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
                              zeroSize, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::StartAutoRate(units::second_t period) {
   int32_t status = 0;
-  HAL_StartSPIAutoRate(m_port, period.to<double>(), &status);
-  wpi_setHALError(status);
+  HAL_StartSPIAutoRate(m_port, period.value(), &status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::StartAutoRate(double period) {
@@ -272,42 +284,38 @@
 
 void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
   int32_t status = 0;
-  HAL_StartSPIAutoTrigger(
-      m_port, source.GetPortHandleForRouting(),
-      (HAL_AnalogTriggerType)source.GetAnalogTriggerTypeForRouting(), rising,
-      falling, &status);
-  wpi_setHALError(status);
+  HAL_StartSPIAutoTrigger(m_port, source.GetPortHandleForRouting(),
+                          static_cast<HAL_AnalogTriggerType>(
+                              source.GetAnalogTriggerTypeForRouting()),
+                          rising, falling, &status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::StopAuto() {
   int32_t status = 0;
   HAL_StopSPIAuto(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::ForceAutoRead() {
   int32_t status = 0;
   HAL_ForceSPIAutoRead(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
                               units::second_t timeout) {
   int32_t status = 0;
   int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
-                                            timeout.to<double>(), &status);
-  wpi_setHALError(status);
+                                            timeout.value(), &status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
   return val;
 }
 
-int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout) {
-  return ReadAutoReceivedData(buffer, numToRead, units::second_t(timeout));
-}
-
 int SPI::GetAutoDroppedCount() {
   int32_t status = 0;
   int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
   return val;
 }
 
@@ -316,7 +324,7 @@
   int32_t status = 0;
   HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
                             &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "Port {}", m_port);
 }
 
 void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
@@ -341,8 +349,9 @@
   SetAutoTransmitData(cmdBytes, xferSize - 4);
   StartAutoRate(period);
 
-  m_accum.reset(new Accumulator(m_port, xferSize, validMask, validValue,
-                                dataShift, dataSize, isSigned, bigEndian));
+  m_accum =
+      std::make_unique<Accumulator>(m_port, xferSize, validMask, validValue,
+                                    dataShift, dataSize, isSigned, bigEndian);
   m_accum->m_notifier.StartPeriodic(period * kAccumulateDepth / 2);
 }
 
@@ -359,7 +368,9 @@
 }
 
 void SPI::ResetAccumulator() {
-  if (!m_accum) return;
+  if (!m_accum) {
+    return;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->m_value = 0;
   m_accum->m_count = 0;
@@ -369,43 +380,57 @@
 }
 
 void SPI::SetAccumulatorCenter(int center) {
-  if (!m_accum) return;
+  if (!m_accum) {
+    return;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->m_center = center;
 }
 
 void SPI::SetAccumulatorDeadband(int deadband) {
-  if (!m_accum) return;
+  if (!m_accum) {
+    return;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->m_deadband = deadband;
 }
 
 int SPI::GetAccumulatorLastValue() const {
-  if (!m_accum) return 0;
+  if (!m_accum) {
+    return 0;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->Update();
   return m_accum->m_lastValue;
 }
 
 int64_t SPI::GetAccumulatorValue() const {
-  if (!m_accum) return 0;
+  if (!m_accum) {
+    return 0;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->Update();
   return m_accum->m_value;
 }
 
 int64_t SPI::GetAccumulatorCount() const {
-  if (!m_accum) return 0;
+  if (!m_accum) {
+    return 0;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->Update();
   return m_accum->m_count;
 }
 
 double SPI::GetAccumulatorAverage() const {
-  if (!m_accum) return 0;
+  if (!m_accum) {
+    return 0;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->Update();
-  if (m_accum->m_count == 0) return 0.0;
+  if (m_accum->m_count == 0) {
+    return 0.0;
+  }
   return static_cast<double>(m_accum->m_value) / m_accum->m_count;
 }
 
@@ -422,23 +447,31 @@
 }
 
 void SPI::SetAccumulatorIntegratedCenter(double center) {
-  if (!m_accum) return;
+  if (!m_accum) {
+    return;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->m_integratedCenter = center;
 }
 
 double SPI::GetAccumulatorIntegratedValue() const {
-  if (!m_accum) return 0;
+  if (!m_accum) {
+    return 0;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->Update();
   return m_accum->m_integratedValue;
 }
 
 double SPI::GetAccumulatorIntegratedAverage() const {
-  if (!m_accum) return 0;
+  if (!m_accum) {
+    return 0;
+  }
   std::scoped_lock lock(m_accum->m_mutex);
   m_accum->Update();
-  if (m_accum->m_count <= 1) return 0.0;
+  if (m_accum->m_count <= 1) {
+    return 0.0;
+  }
   // count-1 due to not integrating the first value received
   return m_accum->m_integratedValue / (m_accum->m_count - 1);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/ScopedTracer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/ScopedTracer.cpp
index 2024a65..3c8fc80 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/ScopedTracer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/ScopedTracer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/ScopedTracer.h"
 
@@ -11,8 +8,8 @@
 
 using namespace frc;
 
-ScopedTracer::ScopedTracer(wpi::Twine name, wpi::raw_ostream& os)
-    : m_name(name.str()), m_os(os) {
+ScopedTracer::ScopedTracer(std::string_view name, wpi::raw_ostream& os)
+    : m_name(name), m_os(os) {
   m_tracer.ResetTimer();
 }
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SensorUtil.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SensorUtil.cpp
index f86c72c..55aacce 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SensorUtil.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/SensorUtil.cpp
@@ -1,36 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/SensorUtil.h"
 
 #include <hal/AnalogInput.h>
 #include <hal/AnalogOutput.h>
 #include <hal/DIO.h>
-#include <hal/PDP.h>
 #include <hal/PWM.h>
 #include <hal/Ports.h>
 #include <hal/Relay.h>
-#include <hal/Solenoid.h>
 
 using namespace frc;
 
 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();
 const int SensorUtil::kRelayChannels = HAL_GetNumRelayHeaders();
-const int SensorUtil::kPDPChannels = HAL_GetNumPDPChannels();
 
-int SensorUtil::GetDefaultSolenoidModule() { return 0; }
+int SensorUtil::GetDefaultCTREPCMModule() {
+  return 0;
+}
 
-bool SensorUtil::CheckSolenoidModule(int moduleNumber) {
-  return HAL_CheckSolenoidModule(moduleNumber);
+int SensorUtil::GetDefaultREVPHModule() {
+  return 1;
 }
 
 bool SensorUtil::CheckDigitalChannel(int channel) {
@@ -52,15 +46,3 @@
 bool SensorUtil::CheckAnalogOutputChannel(int channel) {
   return HAL_CheckAnalogOutputChannel(channel);
 }
-
-bool SensorUtil::CheckSolenoidChannel(int channel) {
-  return HAL_CheckSolenoidChannel(channel);
-}
-
-bool SensorUtil::CheckPDPChannel(int channel) {
-  return HAL_CheckPDPChannel(channel);
-}
-
-bool SensorUtil::CheckPDPModule(int module) {
-  return HAL_CheckPDPModule(module);
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SerialPort.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SerialPort.cpp
index e092fc2..fb984f1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SerialPort.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/SerialPort.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/SerialPort.h"
 
@@ -12,6 +9,8 @@
 #include <hal/FRCUsageReporting.h>
 #include <hal/SerialPort.h>
 
+#include "frc/Errors.h"
+
 using namespace frc;
 
 SerialPort::SerialPort(int baudRate, Port port, int dataBits,
@@ -21,20 +20,18 @@
 
   m_portHandle =
       HAL_InitializeSerialPort(static_cast<HAL_SerialPort>(port), &status);
-  wpi_setHALError(status);
-  // Don't continue if initialization failed
-  if (status < 0) return;
+  FRC_CheckErrorStatus(status, "Port {}", port);
   HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialBaudRate {}", baudRate);
   HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialDataBits {}", dataBits);
   HAL_SetSerialParity(m_portHandle, parity, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialParity {}", parity);
   HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialStopBits {}", stopBits);
 
   // Set the default timeout to 5 seconds.
-  SetTimeout(5.0);
+  SetTimeout(5_s);
 
   // Don't wait until the buffer is full to transmit.
   SetWriteBufferMode(kFlushOnAccess);
@@ -45,30 +42,26 @@
              static_cast<uint8_t>(port) + 1);
 }
 
-SerialPort::SerialPort(int baudRate, const wpi::Twine& portName, Port port,
+SerialPort::SerialPort(int baudRate, std::string_view portName, Port port,
                        int dataBits, SerialPort::Parity parity,
                        SerialPort::StopBits stopBits) {
   int32_t status = 0;
 
-  wpi::SmallVector<char, 64> buf;
-  const char* portNameC = portName.toNullTerminatedStringRef(buf).data();
-
-  m_portHandle = HAL_InitializeSerialPortDirect(
-      static_cast<HAL_SerialPort>(port), portNameC, &status);
-  wpi_setHALError(status);
-  // Don't continue if initialization failed
-  if (status < 0) return;
+  m_portHandle =
+      HAL_InitializeSerialPortDirect(static_cast<HAL_SerialPort>(port),
+                                     std::string(portName).c_str(), &status);
+  FRC_CheckErrorStatus(status, "Port {}", port);
   HAL_SetSerialBaudRate(m_portHandle, baudRate, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialBaudRate {}", baudRate);
   HAL_SetSerialDataBits(m_portHandle, dataBits, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialDataBits {}", dataBits);
   HAL_SetSerialParity(m_portHandle, parity, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialParity {}", parity);
   HAL_SetSerialStopBits(m_portHandle, stopBits, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetSerialStopBits {}", stopBits);
 
   // Set the default timeout to 5 seconds.
-  SetTimeout(5.0);
+  SetTimeout(5_s);
 
   // Don't wait until the buffer is full to transmit.
   SetWriteBufferMode(kFlushOnAccess);
@@ -82,85 +75,85 @@
 SerialPort::~SerialPort() {
   int32_t status = 0;
   HAL_CloseSerial(m_portHandle, &status);
-  wpi_setHALError(status);
+  FRC_ReportError(status, "{}", "CloseSerial");
 }
 
 void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
   int32_t status = 0;
   HAL_SetSerialFlowControl(m_portHandle, flowControl, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetFlowControl {}", flowControl);
 }
 
 void SerialPort::EnableTermination(char terminator) {
   int32_t status = 0;
   HAL_EnableSerialTermination(m_portHandle, terminator, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "EnableTermination {}", terminator);
 }
 
 void SerialPort::DisableTermination() {
   int32_t status = 0;
   HAL_DisableSerialTermination(m_portHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "DisableTermination");
 }
 
 int SerialPort::GetBytesReceived() {
   int32_t status = 0;
   int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetBytesReceived");
   return retVal;
 }
 
 int SerialPort::Read(char* buffer, int count) {
   int32_t status = 0;
   int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Read");
   return retVal;
 }
 
 int SerialPort::Write(const char* buffer, int count) {
-  return Write(wpi::StringRef(buffer, static_cast<size_t>(count)));
+  return Write(std::string_view(buffer, static_cast<size_t>(count)));
 }
 
-int SerialPort::Write(wpi::StringRef buffer) {
+int SerialPort::Write(std::string_view buffer) {
   int32_t status = 0;
   int retVal =
       HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Write");
   return retVal;
 }
 
-void SerialPort::SetTimeout(double timeout) {
+void SerialPort::SetTimeout(units::second_t timeout) {
   int32_t status = 0;
-  HAL_SetSerialTimeout(m_portHandle, timeout, &status);
-  wpi_setHALError(status);
+  HAL_SetSerialTimeout(m_portHandle, timeout.value(), &status);
+  FRC_CheckErrorStatus(status, "{}", "SetTimeout");
 }
 
 void SerialPort::SetReadBufferSize(int size) {
   int32_t status = 0;
   HAL_SetSerialReadBufferSize(m_portHandle, size, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetReadBufferSize {}", size);
 }
 
 void SerialPort::SetWriteBufferSize(int size) {
   int32_t status = 0;
   HAL_SetSerialWriteBufferSize(m_portHandle, size, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetWriteBufferSize {}", size);
 }
 
 void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
   int32_t status = 0;
   HAL_SetSerialWriteMode(m_portHandle, mode, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "SetWriteBufferMode {}", mode);
 }
 
 void SerialPort::Flush() {
   int32_t status = 0;
   HAL_FlushSerial(m_portHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Flush");
 }
 
 void SerialPort::Reset() {
   int32_t status = 0;
   HAL_ClearSerial(m_portHandle, &status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "Reset");
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Servo.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Servo.cpp
index 5edcebc..c410bff 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Servo.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Servo.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Servo.h"
 
 #include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 using namespace frc;
 
@@ -28,14 +24,20 @@
   SetPeriodMultiplier(kPeriodMultiplier_4X);
 
   HAL_Report(HALUsageReporting::kResourceType_Servo, channel + 1);
-  SendableRegistry::GetInstance().SetName(this, "Servo", channel);
+  wpi::SendableRegistry::SetName(this, "Servo", channel);
 }
 
-void Servo::Set(double value) { SetPosition(value); }
+void Servo::Set(double value) {
+  SetPosition(value);
+}
 
-void Servo::SetOffline() { SetRaw(0); }
+void Servo::SetOffline() {
+  SetRaw(0);
+}
 
-double Servo::Get() const { return GetPosition(); }
+double Servo::Get() const {
+  return GetPosition();
+}
 
 void Servo::SetAngle(double degrees) {
   if (degrees < kMinServoAngle) {
@@ -51,14 +53,18 @@
   return GetPosition() * GetServoAngleRange() + kMinServoAngle;
 }
 
-double Servo::GetMaxAngle() const { return kMaxServoAngle; }
+double Servo::GetMaxAngle() const {
+  return kMaxServoAngle;
+}
 
-double Servo::GetMinAngle() const { return kMinServoAngle; }
+double Servo::GetMinAngle() const {
+  return kMinServoAngle;
+}
 
-void Servo::InitSendable(SendableBuilder& builder) {
+void Servo::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Servo");
   builder.AddDoubleProperty(
-      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
+      "Value", [=] { return Get(); }, [=](double value) { Set(value); });
 }
 
 double Servo::GetServoAngleRange() const {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Solenoid.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Solenoid.cpp
index b5abf20..49000c2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Solenoid.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Solenoid.cpp
@@ -1,103 +1,81 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Solenoid.h"
 
 #include <utility>
 
 #include <hal/FRCUsageReporting.h>
-#include <hal/HALBase.h>
-#include <hal/Ports.h>
-#include <hal/Solenoid.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/SensorUtil.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
-Solenoid::Solenoid(int channel)
-    : Solenoid(SensorUtil::GetDefaultSolenoidModule(), channel) {}
-
-Solenoid::Solenoid(int moduleNumber, int channel)
-    : SolenoidBase(moduleNumber), m_channel(channel) {
-  if (!SensorUtil::CheckSolenoidModule(m_moduleNumber)) {
-    wpi_setWPIErrorWithContext(ModuleIndexOutOfRange,
-                               "Solenoid Module " + wpi::Twine(m_moduleNumber));
-    return;
+Solenoid::Solenoid(int module, PneumaticsModuleType moduleType, int channel)
+    : m_module{PneumaticsBase::GetForType(module, moduleType)},
+      m_channel{channel} {
+  if (!m_module->CheckSolenoidChannel(m_channel)) {
+    throw FRC_MakeError(err::ChannelIndexOutOfRange, "Channel {}", m_channel);
   }
-  if (!SensorUtil::CheckSolenoidChannel(m_channel)) {
-    wpi_setWPIErrorWithContext(ChannelIndexOutOfRange,
-                               "Solenoid Channel " + wpi::Twine(m_channel));
-    return;
-  }
+  m_mask = 1 << channel;
 
-  int32_t status = 0;
-  m_solenoidHandle = HAL_InitializeSolenoidPort(
-      HAL_GetPortWithModule(moduleNumber, channel), &status);
-  if (status != 0) {
-    wpi_setHALErrorWithRange(status, 0, HAL_GetNumSolenoidChannels(), channel);
-    m_solenoidHandle = HAL_kInvalidHandle;
-    return;
+  if (m_module->CheckAndReserveSolenoids(m_mask) != 0) {
+    throw FRC_MakeError(err::ResourceAlreadyAllocated, "Channel {}", m_channel);
   }
 
   HAL_Report(HALUsageReporting::kResourceType_Solenoid, m_channel + 1,
-             m_moduleNumber + 1);
-  SendableRegistry::GetInstance().AddLW(this, "Solenoid", m_moduleNumber,
-                                        m_channel);
+             m_module->GetModuleNumber() + 1);
+  wpi::SendableRegistry::AddLW(this, "Solenoid", m_module->GetModuleNumber(),
+                               m_channel);
 }
 
-Solenoid::~Solenoid() { HAL_FreeSolenoidPort(m_solenoidHandle); }
+Solenoid::Solenoid(PneumaticsModuleType moduleType, int channel)
+    : Solenoid{PneumaticsBase::GetDefaultForType(moduleType), moduleType,
+               channel} {}
+
+Solenoid::~Solenoid() {
+  m_module->UnreserveSolenoids(m_mask);
+}
 
 void Solenoid::Set(bool on) {
-  if (StatusIsFatal()) return;
-
-  int32_t status = 0;
-  HAL_SetSolenoid(m_solenoidHandle, on, &status);
-  wpi_setHALError(status);
+  int value = on ? (0xFFFF & m_mask) : 0;
+  m_module->SetSolenoids(m_mask, value);
 }
 
 bool Solenoid::Get() const {
-  if (StatusIsFatal()) return false;
-
-  int32_t status = 0;
-  bool value = HAL_GetSolenoid(m_solenoidHandle, &status);
-  wpi_setHALError(status);
-
-  return value;
+  int currentAll = m_module->GetSolenoids();
+  return (currentAll & m_mask) != 0;
 }
 
-void Solenoid::Toggle() { Set(!Get()); }
-
-bool Solenoid::IsBlackListed() const {
-  int value = GetPCMSolenoidBlackList(m_moduleNumber) & (1 << m_channel);
-  return (value != 0);
+void Solenoid::Toggle() {
+  Set(!Get());
 }
 
-void Solenoid::SetPulseDuration(double durationSeconds) {
-  int32_t durationMS = durationSeconds * 1000;
-  if (StatusIsFatal()) return;
-  int32_t status = 0;
-  HAL_SetOneShotDuration(m_solenoidHandle, durationMS, &status);
-  wpi_setHALError(status);
+int Solenoid::GetChannel() const {
+  return m_channel;
+}
+
+bool Solenoid::IsDisabled() const {
+  return (m_module->GetSolenoidDisabledList() & m_mask) != 0;
+}
+
+void Solenoid::SetPulseDuration(units::second_t duration) {
+  m_module->SetOneShotDuration(m_channel, duration);
 }
 
 void Solenoid::StartPulse() {
-  if (StatusIsFatal()) return;
-  int32_t status = 0;
-  HAL_FireOneShot(m_solenoidHandle, &status);
-  wpi_setHALError(status);
+  m_module->FireOneShot(m_channel);
 }
 
-void Solenoid::InitSendable(SendableBuilder& builder) {
+void Solenoid::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Solenoid");
   builder.SetActuator(true);
-  builder.SetSafeState([=]() { Set(false); });
+  builder.SetSafeState([=] { Set(false); });
   builder.AddBooleanProperty(
-      "Value", [=]() { return Get(); }, [=](bool value) { Set(value); });
+      "Value", [=] { return Get(); }, [=](bool value) { Set(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SolenoidBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SolenoidBase.cpp
deleted file mode 100644
index f5f8aad..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SolenoidBase.cpp
+++ /dev/null
@@ -1,63 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/SolenoidBase.h"
-
-#include <hal/FRCUsageReporting.h>
-#include <hal/Solenoid.h>
-
-using namespace frc;
-
-int SolenoidBase::GetAll(int module) {
-  int value = 0;
-  int32_t status = 0;
-  value = HAL_GetAllSolenoids(module, &status);
-  wpi_setGlobalHALError(status);
-  return value;
-}
-
-int SolenoidBase::GetAll() const {
-  return SolenoidBase::GetAll(m_moduleNumber);
-}
-
-int SolenoidBase::GetPCMSolenoidBlackList(int module) {
-  int32_t status = 0;
-  return HAL_GetPCMSolenoidBlackList(module, &status);
-}
-
-int SolenoidBase::GetPCMSolenoidBlackList() const {
-  return SolenoidBase::GetPCMSolenoidBlackList(m_moduleNumber);
-}
-
-bool SolenoidBase::GetPCMSolenoidVoltageStickyFault(int module) {
-  int32_t status = 0;
-  return HAL_GetPCMSolenoidVoltageStickyFault(module, &status);
-}
-
-bool SolenoidBase::GetPCMSolenoidVoltageStickyFault() const {
-  return SolenoidBase::GetPCMSolenoidVoltageStickyFault(m_moduleNumber);
-}
-
-bool SolenoidBase::GetPCMSolenoidVoltageFault(int module) {
-  int32_t status = 0;
-  return HAL_GetPCMSolenoidVoltageFault(module, &status);
-}
-
-bool SolenoidBase::GetPCMSolenoidVoltageFault() const {
-  return SolenoidBase::GetPCMSolenoidVoltageFault(m_moduleNumber);
-}
-
-void SolenoidBase::ClearAllPCMStickyFaults(int module) {
-  int32_t status = 0;
-  return HAL_ClearAllPCMStickyFaults(module, &status);
-}
-
-void SolenoidBase::ClearAllPCMStickyFaults() {
-  SolenoidBase::ClearAllPCMStickyFaults(m_moduleNumber);
-}
-
-SolenoidBase::SolenoidBase(int moduleNumber) : m_moduleNumber(moduleNumber) {}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Spark.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Spark.cpp
deleted file mode 100644
index 3717df4..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Spark.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Spark.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-Spark::Spark(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "Spark", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedController.cpp
index cb3cc5b..e0b0cb2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedController.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/SpeedController.h"
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
index 8fea1b2..c3704df 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/SpeedControllerGroup.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/SpeedControllerGroup.h"
 
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 using namespace frc;
 
@@ -22,11 +19,12 @@
 }
 
 void SpeedControllerGroup::Initialize() {
-  for (auto& speedController : m_speedControllers)
-    SendableRegistry::GetInstance().AddChild(this, &speedController.get());
+  for (auto& speedController : m_speedControllers) {
+    wpi::SendableRegistry::AddChild(this, &speedController.get());
+  }
   static int instances = 0;
   ++instances;
-  SendableRegistry::GetInstance().Add(this, "SpeedControllerGroup", instances);
+  wpi::SendableRegistry::Add(this, "SpeedControllerGroup", instances);
 }
 
 void SpeedControllerGroup::Set(double speed) {
@@ -46,7 +44,9 @@
   m_isInverted = isInverted;
 }
 
-bool SpeedControllerGroup::GetInverted() const { return m_isInverted; }
+bool SpeedControllerGroup::GetInverted() const {
+  return m_isInverted;
+}
 
 void SpeedControllerGroup::Disable() {
   for (auto speedController : m_speedControllers) {
@@ -60,12 +60,10 @@
   }
 }
 
-void SpeedControllerGroup::PIDWrite(double output) { Set(output); }
-
-void SpeedControllerGroup::InitSendable(SendableBuilder& builder) {
+void SpeedControllerGroup::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Speed Controller");
   builder.SetActuator(true);
-  builder.SetSafeState([=]() { StopMotor(); });
+  builder.SetSafeState([=] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Value", [=]() { return Get(); }, [=](double value) { Set(value); });
+      "Value", [=] { return Get(); }, [=](double value) { Set(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp
new file mode 100644
index 0000000..49acd84
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/SynchronousInterrupt.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/SynchronousInterrupt.h"
+
+#include <type_traits>
+
+#include <hal/Interrupts.h>
+#include <wpi/NullDeleter.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+SynchronousInterrupt::SynchronousInterrupt(DigitalSource& source)
+    : m_source{&source, wpi::NullDeleter<DigitalSource>()} {
+  InitSynchronousInterrupt();
+}
+SynchronousInterrupt::SynchronousInterrupt(DigitalSource* source)
+    : m_source{source, wpi::NullDeleter<DigitalSource>()} {
+  if (m_source == nullptr) {
+    FRC_CheckErrorStatus(frc::err::NullParameter, "{}", "Source is null");
+  } else {
+    InitSynchronousInterrupt();
+  }
+}
+SynchronousInterrupt::SynchronousInterrupt(
+    std::shared_ptr<DigitalSource> source)
+    : m_source{std::move(source)} {
+  if (m_source == nullptr) {
+    FRC_CheckErrorStatus(frc::err::NullParameter, "{}", "Source is null");
+  } else {
+    InitSynchronousInterrupt();
+  }
+}
+
+void SynchronousInterrupt::InitSynchronousInterrupt() {
+  int32_t status = 0;
+  m_handle = HAL_InitializeInterrupts(&status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt failed to initialize");
+  HAL_RequestInterrupts(m_handle, m_source->GetPortHandleForRouting(),
+                        static_cast<HAL_AnalogTriggerType>(
+                            m_source->GetAnalogTriggerTypeForRouting()),
+                        &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt request failed");
+  HAL_SetInterruptUpSourceEdge(m_handle, true, false, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt setting up source edge failed");
+}
+
+SynchronousInterrupt::~SynchronousInterrupt() {
+  HAL_CleanInterrupts(m_handle);
+}
+
+inline SynchronousInterrupt::WaitResult operator|(
+    SynchronousInterrupt::WaitResult lhs,
+    SynchronousInterrupt::WaitResult rhs) {
+  using T = std::underlying_type_t<SynchronousInterrupt::WaitResult>;
+  return static_cast<SynchronousInterrupt::WaitResult>(static_cast<T>(lhs) |
+                                                       static_cast<T>(rhs));
+}
+
+SynchronousInterrupt::WaitResult SynchronousInterrupt::WaitForInterrupt(
+    units::second_t timeout, bool ignorePrevious) {
+  int32_t status = 0;
+  auto result =
+      HAL_WaitForInterrupt(m_handle, timeout.value(), ignorePrevious, &status);
+
+  auto rising =
+      ((result & 0xFF) != 0) ? WaitResult::kRisingEdge : WaitResult::kTimeout;
+  auto falling = ((result & 0xFF00) != 0) ? WaitResult::kFallingEdge
+                                          : WaitResult::kTimeout;
+
+  return rising | falling;
+}
+
+void SynchronousInterrupt::SetInterruptEdges(bool risingEdge,
+                                             bool fallingEdge) {
+  int32_t status = 0;
+  HAL_SetInterruptUpSourceEdge(m_handle, risingEdge, fallingEdge, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt setting edges failed");
+}
+
+void SynchronousInterrupt::WakeupWaitingInterrupt() {
+  int32_t status = 0;
+  HAL_ReleaseWaitingInterrupt(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt wakeup failed");
+}
+
+units::second_t SynchronousInterrupt::GetRisingTimestamp() {
+  int32_t status = 0;
+  auto ts = HAL_ReadInterruptRisingTimestamp(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt rising timestamp failed");
+  units::microsecond_t ms{static_cast<double>(ts)};
+  return ms;
+}
+
+units::second_t SynchronousInterrupt::GetFallingTimestamp() {
+  int32_t status = 0;
+  auto ts = HAL_ReadInterruptFallingTimestamp(m_handle, &status);
+  FRC_CheckErrorStatus(status, "{}", "Interrupt falling timestamp failed");
+  units::microsecond_t ms{static_cast<double>(ts)};
+  return ms;
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Talon.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Talon.cpp
deleted file mode 100644
index 4807d71..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Talon.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Talon.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-Talon::Talon(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.037, 1.539, 1.513, 1.487, 0.989);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "Talon", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Threads.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Threads.cpp
index 7b1e647..2b9c151 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Threads.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Threads.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Threads.h"
 
 #include <hal/FRCUsageReporting.h>
 #include <hal/Threads.h>
 
-#include "frc/ErrorBase.h"
+#include "frc/Errors.h"
 
 namespace frc {
 
@@ -19,7 +16,7 @@
   HAL_Bool rt = false;
   auto native = thread.native_handle();
   auto ret = HAL_GetThreadPriority(&native, &rt, &status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetThreadPriority");
   *isRealTime = rt;
   return ret;
 }
@@ -28,7 +25,7 @@
   int32_t status = 0;
   HAL_Bool rt = false;
   auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "GetCurrentThreadPriority");
   *isRealTime = rt;
   return ret;
 }
@@ -37,14 +34,14 @@
   int32_t status = 0;
   auto native = thread.native_handle();
   auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetThreadPriority");
   return ret;
 }
 
 bool SetCurrentThreadPriority(bool realTime, int priority) {
   int32_t status = 0;
   auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "SetCurrentThreadPriority");
   return ret;
 }
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/TimedRobot.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/TimedRobot.cpp
index 74e658a..93e6698 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/TimedRobot.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/TimedRobot.cpp
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/TimedRobot.h"
 
 #include <stdint.h>
 
+#include <cstdio>
 #include <utility>
 
 #include <hal/DriverStation.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/Notifier.h>
 
+#include "frc/Errors.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
 
 using namespace frc;
 
@@ -29,6 +26,7 @@
   }
 
   // Tell the DS that the robot is ready to be enabled
+  std::puts("\n********** Robot program startup complete **********");
   HAL_ObserveUserProgramStarting();
 
   // Loop forever, calling the appropriate mode-dependent function
@@ -42,10 +40,12 @@
     HAL_UpdateNotifierAlarm(
         m_notifier, static_cast<uint64_t>(callback.expirationTime * 1e6),
         &status);
-    wpi_setHALError(status);
+    FRC_CheckErrorStatus(status, "{}", "UpdateNotifierAlarm");
 
     uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
-    if (curTime == 0 || status != 0) break;
+    if (curTime == 0 || status != 0) {
+      break;
+    }
 
     callback.func();
 
@@ -70,19 +70,15 @@
   HAL_StopNotifier(m_notifier, &status);
 }
 
-units::second_t TimedRobot::GetPeriod() const {
-  return units::second_t(m_period);
-}
-
 TimedRobot::TimedRobot(double period) : TimedRobot(units::second_t(period)) {}
 
 TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
-  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_startTime = Timer::GetFPGATimestamp();
   AddPeriodic([=] { LoopFunc(); }, period);
 
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
-  wpi_setHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
   HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
 
   HAL_Report(HALUsageReporting::kResourceType_Framework,
@@ -93,7 +89,7 @@
   int32_t status = 0;
 
   HAL_StopNotifier(m_notifier, &status);
-  wpi_setHALError(status);
+  FRC_ReportError(status, "{}", "StopNotifier");
 
   HAL_CleanNotifier(m_notifier, &status);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Timer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Timer.cpp
index b428e03..bbd2262 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Timer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Timer.cpp
@@ -1,42 +1,90 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Timer.h"
 
-#include <units/time.h>
+#include <chrono>
+#include <thread>
+
+#include "frc/DriverStation.h"
+#include "frc/RobotController.h"
 
 namespace frc {
 
-void Wait(double seconds) { frc2::Wait(units::second_t(seconds)); }
+void Wait(units::second_t seconds) {
+  std::this_thread::sleep_for(std::chrono::duration<double>(seconds.value()));
+}
 
-double GetTime() { return frc2::GetTime().to<double>(); }
+units::second_t GetTime() {
+  using std::chrono::duration;
+  using std::chrono::duration_cast;
+  using std::chrono::system_clock;
+
+  return units::second_t(
+      duration_cast<duration<double>>(system_clock::now().time_since_epoch())
+          .count());
+}
 
 }  // namespace frc
 
 using namespace frc;
 
-Timer::Timer() { Reset(); }
-
-double Timer::Get() const { return m_timer.Get().to<double>(); }
-
-void Timer::Reset() { m_timer.Reset(); }
-
-void Timer::Start() { m_timer.Start(); }
-
-void Timer::Stop() { m_timer.Stop(); }
-
-bool Timer::HasPeriodPassed(double period) {
-  return m_timer.HasPeriodPassed(units::second_t(period));
+Timer::Timer() {
+  Reset();
 }
 
-double Timer::GetFPGATimestamp() {
-  return frc2::Timer::GetFPGATimestamp().to<double>();
+units::second_t Timer::Get() const {
+  if (m_running) {
+    return (GetFPGATimestamp() - m_startTime) + m_accumulatedTime;
+  } else {
+    return m_accumulatedTime;
+  }
 }
 
-double Timer::GetMatchTime() {
-  return frc2::Timer::GetMatchTime().to<double>();
+void Timer::Reset() {
+  m_accumulatedTime = 0_s;
+  m_startTime = GetFPGATimestamp();
+}
+
+void Timer::Start() {
+  if (!m_running) {
+    m_startTime = GetFPGATimestamp();
+    m_running = true;
+  }
+}
+
+void Timer::Stop() {
+  if (m_running) {
+    m_accumulatedTime = Get();
+    m_running = false;
+  }
+}
+
+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) {
+    // 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;
+  }
+}
+
+units::second_t Timer::GetFPGATimestamp() {
+  // FPGA returns the timestamp in microseconds
+  return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
+}
+
+units::second_t Timer::GetMatchTime() {
+  return units::second_t(frc::DriverStation::GetMatchTime());
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/TimesliceRobot.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/TimesliceRobot.cpp
new file mode 100644
index 0000000..d212c10
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/TimesliceRobot.cpp
@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/TimesliceRobot.h"
+
+#include "frc/Errors.h"
+#include "frc/fmt/Units.h"
+
+using namespace frc;
+
+TimesliceRobot::TimesliceRobot(units::second_t robotPeriodicAllocation,
+                               units::second_t controllerPeriod)
+    : m_nextOffset{robotPeriodicAllocation},
+      m_controllerPeriod{controllerPeriod} {}
+
+void TimesliceRobot::Schedule(std::function<void()> func,
+                              units::second_t allocation) {
+  if (m_nextOffset + allocation > m_controllerPeriod) {
+    throw FRC_MakeError(err::Error,
+                        "Function scheduled at offset {} with allocation {} "
+                        "exceeded controller period of {}\n",
+                        m_nextOffset, allocation, m_controllerPeriod);
+  }
+
+  AddPeriodic(func, m_controllerPeriod, m_nextOffset);
+  m_nextOffset += allocation;
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Tracer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Tracer.cpp
index af5177a..1375411 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Tracer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Tracer.cpp
@@ -1,30 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Tracer.h"
 
-#include <wpi/Format.h>
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
 #include <wpi/raw_ostream.h>
 
-#include "frc/DriverStation.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 
-Tracer::Tracer() { ResetTimer(); }
+Tracer::Tracer() {
+  ResetTimer();
+}
 
-void Tracer::ResetTimer() { m_startTime = hal::fpga_clock::now(); }
+void Tracer::ResetTimer() {
+  m_startTime = hal::fpga_clock::now();
+}
 
 void Tracer::ClearEpochs() {
   ResetTimer();
   m_epochs.clear();
 }
 
-void Tracer::AddEpoch(wpi::StringRef epochName) {
+void Tracer::AddEpoch(std::string_view epochName) {
   auto currentTime = hal::fpga_clock::now();
   m_epochs[epochName] = currentTime - m_startTime;
   m_startTime = currentTime;
@@ -34,7 +35,9 @@
   wpi::SmallString<128> buf;
   wpi::raw_svector_ostream os(buf);
   PrintEpochs(os);
-  if (!buf.empty()) DriverStation::ReportWarning(buf);
+  if (!buf.empty()) {
+    FRC_ReportError(warn::Warning, "{}", buf.c_str());
+  }
 }
 
 void Tracer::PrintEpochs(wpi::raw_ostream& os) {
@@ -45,11 +48,9 @@
   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";
+      os << fmt::format(
+          "\t{}: {:.6f}s\n", epoch.getKey(),
+          duration_cast<microseconds>(epoch.getValue()).count() / 1.0e6);
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Ultrasonic.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Ultrasonic.cpp
index dbd0d13..02035dd 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Ultrasonic.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Ultrasonic.cpp
@@ -1,23 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Ultrasonic.h"
 
-#include <hal/FRCUsageReporting.h>
+#include <utility>
 
-#include "frc/Base.h"
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
 #include "frc/Counter.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalOutput.h"
+#include "frc/Errors.h"
 #include "frc/Timer.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
@@ -27,47 +25,40 @@
 std::vector<Ultrasonic*> Ultrasonic::m_sensors;
 std::thread Ultrasonic::m_thread;
 
-Ultrasonic::Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units)
+Ultrasonic::Ultrasonic(int pingChannel, int echoChannel)
     : m_pingChannel(std::make_shared<DigitalOutput>(pingChannel)),
       m_echoChannel(std::make_shared<DigitalInput>(echoChannel)),
       m_counter(m_echoChannel) {
-  m_units = units;
   Initialize();
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, m_pingChannel.get());
-  registry.AddChild(this, m_echoChannel.get());
+  wpi::SendableRegistry::AddChild(this, m_pingChannel.get());
+  wpi::SendableRegistry::AddChild(this, m_echoChannel.get());
 }
 
-Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
-                       DistanceUnit units)
-    : m_pingChannel(pingChannel, NullDeleter<DigitalOutput>()),
-      m_echoChannel(echoChannel, NullDeleter<DigitalInput>()),
+Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel)
+    : m_pingChannel(pingChannel, wpi::NullDeleter<DigitalOutput>()),
+      m_echoChannel(echoChannel, wpi::NullDeleter<DigitalInput>()),
       m_counter(m_echoChannel) {
-  if (pingChannel == nullptr || echoChannel == nullptr) {
-    wpi_setWPIError(NullParameter);
-    m_units = units;
-    return;
+  if (!pingChannel) {
+    throw FRC_MakeError(err::NullParameter, "{}", "pingChannel");
   }
-  m_units = units;
+  if (!echoChannel) {
+    throw FRC_MakeError(err::NullParameter, "{}", "echoChannel");
+  }
   Initialize();
 }
 
-Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel,
-                       DistanceUnit units)
-    : m_pingChannel(&pingChannel, NullDeleter<DigitalOutput>()),
-      m_echoChannel(&echoChannel, NullDeleter<DigitalInput>()),
+Ultrasonic::Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel)
+    : m_pingChannel(&pingChannel, wpi::NullDeleter<DigitalOutput>()),
+      m_echoChannel(&echoChannel, wpi::NullDeleter<DigitalInput>()),
       m_counter(m_echoChannel) {
-  m_units = units;
   Initialize();
 }
 
 Ultrasonic::Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
-                       std::shared_ptr<DigitalInput> echoChannel,
-                       DistanceUnit units)
-    : m_pingChannel(pingChannel),
-      m_echoChannel(echoChannel),
+                       std::shared_ptr<DigitalInput> echoChannel)
+    : m_pingChannel(std::move(pingChannel)),
+      m_echoChannel(std::move(echoChannel)),
       m_counter(m_echoChannel) {
-  m_units = units;
   Initialize();
 }
 
@@ -89,8 +80,15 @@
   }
 }
 
+int Ultrasonic::GetEchoChannel() const {
+  return m_echoChannel->GetChannel();
+}
+
 void Ultrasonic::Ping() {
-  wpi_assert(!m_automaticEnabled);
+  if (m_automaticEnabled) {
+    throw FRC_MakeError(err::IncompatibleMode, "{}",
+                        "cannot call Ping() in automatic mode");
+  }
 
   // Reset the counter to zero (invalid data now)
   m_counter.Reset();
@@ -100,12 +98,16 @@
 }
 
 bool Ultrasonic::IsRangeValid() const {
-  if (m_simRangeValid) return m_simRangeValid.Get();
+  if (m_simRangeValid) {
+    return m_simRangeValid.Get();
+  }
   return m_counter.Get() > 1;
 }
 
 void Ultrasonic::SetAutomaticMode(bool enabling) {
-  if (enabling == m_automaticEnabled) return;  // ignore the case of no change
+  if (enabling == m_automaticEnabled) {
+    return;  // ignore the case of no change
+  }
 
   m_automaticEnabled = enabling;
 
@@ -138,48 +140,29 @@
   }
 }
 
-double Ultrasonic::GetRangeInches() const {
+units::meter_t Ultrasonic::GetRange() const {
   if (IsRangeValid()) {
-    if (m_simRange) return m_simRange.Get();
-    return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
+    if (m_simRange) {
+      return units::meter_t{m_simRange.Get()};
+    }
+    return m_counter.GetPeriod() * kSpeedOfSound / 2.0;
   } else {
-    return 0;
+    return 0_m;
   }
 }
 
-double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
-
-bool Ultrasonic::IsEnabled() const { return m_enabled; }
-
-void Ultrasonic::SetEnabled(bool enable) { m_enabled = enable; }
-
-void Ultrasonic::SetDistanceUnits(DistanceUnit units) { m_units = units; }
-
-Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits() const {
-  return m_units;
+bool Ultrasonic::IsEnabled() const {
+  return m_enabled;
 }
 
-double Ultrasonic::PIDGet() {
-  switch (m_units) {
-    case Ultrasonic::kInches:
-      return GetRangeInches();
-    case Ultrasonic::kMilliMeters:
-      return GetRangeMM();
-    default:
-      return 0.0;
-  }
+void Ultrasonic::SetEnabled(bool enable) {
+  m_enabled = enable;
 }
 
-void Ultrasonic::SetPIDSourceType(PIDSourceType pidSource) {
-  if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
-    m_pidSource = pidSource;
-  }
-}
-
-void Ultrasonic::InitSendable(SendableBuilder& builder) {
+void Ultrasonic::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("Ultrasonic");
   builder.AddDoubleProperty(
-      "Value", [=]() { return GetRangeInches(); }, nullptr);
+      "Value", [=] { return units::inch_t{GetRange()}.value(); }, nullptr);
 }
 
 void Ultrasonic::Initialize() {
@@ -196,7 +179,7 @@
   // Link this instance on the list
   m_sensors.emplace_back(this);
 
-  m_counter.SetMaxPeriod(1.0);
+  m_counter.SetMaxPeriod(1_s);
   m_counter.SetSemiPeriodMode(true);
   m_counter.Reset();
   m_enabled = true;  // Make it available for round robin scheduling
@@ -205,20 +188,21 @@
   static int instances = 0;
   instances++;
   HAL_Report(HALUsageReporting::kResourceType_Ultrasonic, instances);
-  SendableRegistry::GetInstance().AddLW(this, "Ultrasonic",
-                                        m_echoChannel->GetChannel());
+  wpi::SendableRegistry::AddLW(this, "Ultrasonic", m_echoChannel->GetChannel());
 }
 
 void Ultrasonic::UltrasonicChecker() {
   while (m_automaticEnabled) {
     for (auto& sensor : m_sensors) {
-      if (!m_automaticEnabled) break;
+      if (!m_automaticEnabled) {
+        break;
+      }
 
       if (sensor->IsEnabled()) {
         sensor->m_pingChannel->Pulse(kPingTime);  // do the ping
       }
 
-      Wait(0.1);  // wait for ping to return
+      Wait(100_ms);  // wait for ping to return
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Utility.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Utility.cpp
deleted file mode 100644
index 4b69cc8..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Utility.cpp
+++ /dev/null
@@ -1,112 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Utility.h"
-
-#ifndef _WIN32
-#include <cxxabi.h>
-#include <execinfo.h>
-#endif
-
-#include <frc/Base.h>
-#include <hal/DriverStation.h>
-#include <wpi/Path.h>
-#include <wpi/SmallString.h>
-#include <wpi/StackTrace.h>
-#include <wpi/raw_ostream.h>
-
-using namespace frc;
-
-bool wpi_assert_impl(bool conditionValue, const wpi::Twine& conditionText,
-                     const wpi::Twine& message, wpi::StringRef fileName,
-                     int lineNumber, wpi::StringRef funcName) {
-  if (!conditionValue) {
-    wpi::SmallString<128> locBuf;
-    wpi::raw_svector_ostream locStream(locBuf);
-    locStream << funcName << " [" << wpi::sys::path::filename(fileName) << ":"
-              << lineNumber << "]";
-
-    wpi::SmallString<128> errorBuf;
-    wpi::raw_svector_ostream errorStream(errorBuf);
-
-    errorStream << "Assertion \"" << conditionText << "\" ";
-
-    if (message.isTriviallyEmpty() ||
-        (message.isSingleStringRef() && message.getSingleStringRef().empty())) {
-      errorStream << "failed.\n";
-    } else {
-      errorStream << "failed: " << message << "\n";
-    }
-
-    std::string stack = wpi::GetStackTrace(2);
-
-    // Print the error and send it to the DriverStation
-    HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), stack.c_str(), 1);
-  }
-
-  return conditionValue;
-}
-
-/**
- * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl.
- *
- * This should not be called directly; it should only be used by
- * wpi_assertEqual_impl and wpi_assertNotEqual_impl.
- */
-void wpi_assertEqual_common_impl(const wpi::Twine& valueA,
-                                 const wpi::Twine& valueB,
-                                 const wpi::Twine& equalityType,
-                                 const wpi::Twine& message,
-                                 wpi::StringRef fileName, int lineNumber,
-                                 wpi::StringRef funcName) {
-  wpi::SmallString<128> locBuf;
-  wpi::raw_svector_ostream locStream(locBuf);
-  locStream << funcName << " [" << wpi::sys::path::filename(fileName) << ":"
-            << lineNumber << "]";
-
-  wpi::SmallString<128> errorBuf;
-  wpi::raw_svector_ostream errorStream(errorBuf);
-
-  errorStream << "Assertion \"" << valueA << " " << equalityType << " "
-              << valueB << "\" ";
-
-  if (message.isTriviallyEmpty() ||
-      (message.isSingleStringRef() && message.getSingleStringRef().empty())) {
-    errorStream << "failed.\n";
-  } else {
-    errorStream << "failed: " << message << "\n";
-  }
-
-  std::string trace = wpi::GetStackTrace(3);
-
-  // Print the error and send it to the DriverStation
-  HAL_SendError(1, 1, 0, errorBuf.c_str(), locBuf.c_str(), trace.c_str(), 1);
-}
-
-bool wpi_assertEqual_impl(int valueA, int valueB,
-                          const wpi::Twine& valueAString,
-                          const wpi::Twine& valueBString,
-                          const wpi::Twine& message, wpi::StringRef fileName,
-                          int lineNumber, wpi::StringRef funcName) {
-  if (!(valueA == valueB)) {
-    wpi_assertEqual_common_impl(valueAString, valueBString, "==", message,
-                                fileName, lineNumber, funcName);
-  }
-  return valueA == valueB;
-}
-
-bool wpi_assertNotEqual_impl(int valueA, int valueB,
-                             const wpi::Twine& valueAString,
-                             const wpi::Twine& valueBString,
-                             const wpi::Twine& message, wpi::StringRef fileName,
-                             int lineNumber, wpi::StringRef funcName) {
-  if (!(valueA != valueB)) {
-    wpi_assertEqual_common_impl(valueAString, valueBString, "!=", message,
-                                fileName, lineNumber, funcName);
-  }
-  return valueA != valueB;
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Victor.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Victor.cpp
deleted file mode 100644
index bce1913..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Victor.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/Victor.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-Victor::Victor(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
-  SetPeriodMultiplier(kPeriodMultiplier_2X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "Victor", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/VictorSP.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/VictorSP.cpp
deleted file mode 100644
index 221777d..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/VictorSP.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/VictorSP.h"
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-VictorSP::VictorSP(int channel) : PWMSpeedController(channel) {
-  SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-  SetPeriodMultiplier(kPeriodMultiplier_1X);
-  SetSpeed(0.0);
-  SetZeroLatch();
-
-  HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel() + 1);
-  SendableRegistry::GetInstance().SetName(this, "VictorSP", GetChannel());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/Watchdog.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/Watchdog.cpp
index 81c7cc8..854f9e9 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/Watchdog.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/Watchdog.cpp
@@ -1,22 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Watchdog.h"
 
 #include <atomic>
+#include <thread>
+#include <utility>
 
+#include <fmt/format.h>
 #include <hal/Notifier.h>
-#include <wpi/Format.h>
-#include <wpi/SmallString.h>
+#include <wpi/mutex.h>
 #include <wpi/priority_queue.h>
-#include <wpi/raw_ostream.h>
 
-#include "frc/DriverStation.h"
-#include "frc2/Timer.h"
+#include "frc/Errors.h"
+#include "frc/Timer.h"
 
 using namespace frc;
 
@@ -49,7 +47,7 @@
 Watchdog::Impl::Impl() {
   int32_t status = 0;
   m_notifier = HAL_InitializeNotifier(&status);
-  wpi_setGlobalHALError(status);
+  FRC_CheckErrorStatus(status, "{}", "starting watchdog notifier");
   HAL_SetNotifierName(m_notifier, "Watchdog", &status);
 
   m_thread = std::thread([=] { Main(); });
@@ -60,10 +58,12 @@
   // atomically set handle to 0, then clean
   HAL_NotifierHandle handle = m_notifier.exchange(0);
   HAL_StopNotifier(handle, &status);
-  wpi_setGlobalHALError(status);
+  FRC_ReportError(status, "{}", "stopping watchdog notifier");
 
   // Join the thread to ensure the handler has exited.
-  if (m_thread.joinable()) m_thread.join();
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
 
   HAL_CleanNotifier(handle, &status);
 }
@@ -72,29 +72,38 @@
   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())
+  if (notifier == 0) {
+    return;
+  }
+  if (m_watchdogs.empty()) {
     HAL_CancelNotifierAlarm(notifier, &status);
-  else
+  } else {
     HAL_UpdateNotifierAlarm(
         notifier,
-        static_cast<uint64_t>(m_watchdogs.top()->m_expirationTime.to<double>() *
+        static_cast<uint64_t>(m_watchdogs.top()->m_expirationTime.value() *
                               1e6),
         &status);
-  wpi_setGlobalHALError(status);
+  }
+  FRC_CheckErrorStatus(status, "{}", "updating watchdog notifier alarm");
 }
 
 void Watchdog::Impl::Main() {
   for (;;) {
     int32_t status = 0;
     HAL_NotifierHandle notifier = m_notifier.load();
-    if (notifier == 0) break;
+    if (notifier == 0) {
+      break;
+    }
     uint64_t curTime = HAL_WaitForNotifierAlarm(notifier, &status);
-    if (curTime == 0 || status != 0) break;
+    if (curTime == 0 || status != 0) {
+      break;
+    }
 
     std::unique_lock lock(m_mutex);
 
-    if (m_watchdogs.empty()) continue;
+    if (m_watchdogs.empty()) {
+      continue;
+    }
 
     // If the condition variable timed out, that means a Watchdog timeout
     // has occurred, so call its timeout function.
@@ -104,11 +113,8 @@
     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());
+        FRC_ReportError(warn::Warning, "Watchdog not fed within {:.6f}s",
+                        watchdog->m_timeout.value());
       }
     }
 
@@ -125,15 +131,20 @@
   }
 }
 
-Watchdog::Watchdog(double timeout, std::function<void()> callback)
-    : Watchdog(units::second_t{timeout}, callback) {}
-
 Watchdog::Watchdog(units::second_t timeout, std::function<void()> callback)
-    : m_timeout(timeout), m_callback(callback), m_impl(GetImpl()) {}
+    : m_timeout(timeout), m_callback(std::move(callback)), m_impl(GetImpl()) {}
 
-Watchdog::~Watchdog() { Disable(); }
+Watchdog::~Watchdog() {
+  try {
+    Disable();
+  } catch (const RuntimeError& e) {
+    e.Report();
+  }
+}
 
-Watchdog::Watchdog(Watchdog&& rhs) { *this = std::move(rhs); }
+Watchdog::Watchdog(Watchdog&& rhs) {
+  *this = std::move(rhs);
+}
 
 Watchdog& Watchdog::operator=(Watchdog&& rhs) {
   m_impl = rhs.m_impl;
@@ -153,16 +164,12 @@
   return *this;
 }
 
-double Watchdog::GetTime() const {
-  return (frc2::Timer::GetFPGATimestamp() - m_startTime).to<double>();
-}
-
-void Watchdog::SetTimeout(double timeout) {
-  SetTimeout(units::second_t{timeout});
+units::second_t Watchdog::GetTime() const {
+  return Timer::GetFPGATimestamp() - m_startTime;
 }
 
 void Watchdog::SetTimeout(units::second_t timeout) {
-  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_startTime = Timer::GetFPGATimestamp();
   m_tracer.ClearEpochs();
 
   std::scoped_lock lock(m_impl->m_mutex);
@@ -175,9 +182,9 @@
   m_impl->UpdateAlarm();
 }
 
-double Watchdog::GetTimeout() const {
+units::second_t Watchdog::GetTimeout() const {
   std::scoped_lock lock(m_impl->m_mutex);
-  return m_timeout.to<double>();
+  return m_timeout;
 }
 
 bool Watchdog::IsExpired() const {
@@ -185,16 +192,20 @@
   return m_isExpired;
 }
 
-void Watchdog::AddEpoch(wpi::StringRef epochName) {
+void Watchdog::AddEpoch(std::string_view epochName) {
   m_tracer.AddEpoch(epochName);
 }
 
-void Watchdog::PrintEpochs() { m_tracer.PrintEpochs(); }
+void Watchdog::PrintEpochs() {
+  m_tracer.PrintEpochs();
+}
 
-void Watchdog::Reset() { Enable(); }
+void Watchdog::Reset() {
+  Enable();
+}
 
 void Watchdog::Enable() {
-  m_startTime = frc2::Timer::GetFPGATimestamp();
+  m_startTime = Timer::GetFPGATimestamp();
   m_tracer.ClearEpochs();
 
   std::scoped_lock lock(m_impl->m_mutex);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/XboxController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/XboxController.cpp
index de77624..a08225b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/XboxController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/XboxController.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/XboxController.h"
 
@@ -15,146 +12,146 @@
   HAL_Report(HALUsageReporting::kResourceType_XboxController, port + 1);
 }
 
-double XboxController::GetX(JoystickHand hand) const {
-  if (hand == kLeftHand) {
-    return GetRawAxis(static_cast<int>(Axis::kLeftX));
-  } else {
-    return GetRawAxis(static_cast<int>(Axis::kRightX));
-  }
+double XboxController::GetLeftX() const {
+  return GetRawAxis(Axis::kLeftX);
 }
 
-double XboxController::GetY(JoystickHand hand) const {
-  if (hand == kLeftHand) {
-    return GetRawAxis(static_cast<int>(Axis::kLeftY));
-  } else {
-    return GetRawAxis(static_cast<int>(Axis::kRightY));
-  }
+double XboxController::GetRightX() const {
+  return GetRawAxis(Axis::kRightX);
 }
 
-double XboxController::GetTriggerAxis(JoystickHand hand) const {
-  if (hand == kLeftHand) {
-    return GetRawAxis(static_cast<int>(Axis::kLeftTrigger));
-  } else {
-    return GetRawAxis(static_cast<int>(Axis::kRightTrigger));
-  }
+double XboxController::GetLeftY() const {
+  return GetRawAxis(Axis::kLeftY);
 }
 
-bool XboxController::GetBumper(JoystickHand hand) const {
-  if (hand == kLeftHand) {
-    return GetRawButton(static_cast<int>(Button::kBumperLeft));
-  } else {
-    return GetRawButton(static_cast<int>(Button::kBumperRight));
-  }
+double XboxController::GetRightY() const {
+  return GetRawAxis(Axis::kRightY);
 }
 
-bool XboxController::GetBumperPressed(JoystickHand hand) {
-  if (hand == kLeftHand) {
-    return GetRawButtonPressed(static_cast<int>(Button::kBumperLeft));
-  } else {
-    return GetRawButtonPressed(static_cast<int>(Button::kBumperRight));
-  }
+double XboxController::GetLeftTriggerAxis() const {
+  return GetRawAxis(Axis::kLeftTrigger);
 }
 
-bool XboxController::GetBumperReleased(JoystickHand hand) {
-  if (hand == kLeftHand) {
-    return GetRawButtonReleased(static_cast<int>(Button::kBumperLeft));
-  } else {
-    return GetRawButtonReleased(static_cast<int>(Button::kBumperRight));
-  }
+double XboxController::GetRightTriggerAxis() const {
+  return GetRawAxis(Axis::kRightTrigger);
 }
 
-bool XboxController::GetStickButton(JoystickHand hand) const {
-  if (hand == kLeftHand) {
-    return GetRawButton(static_cast<int>(Button::kStickLeft));
-  } else {
-    return GetRawButton(static_cast<int>(Button::kStickRight));
-  }
+bool XboxController::GetLeftBumper() const {
+  return GetRawButton(Button::kLeftBumper);
 }
 
-bool XboxController::GetStickButtonPressed(JoystickHand hand) {
-  if (hand == kLeftHand) {
-    return GetRawButtonPressed(static_cast<int>(Button::kStickLeft));
-  } else {
-    return GetRawButtonPressed(static_cast<int>(Button::kStickRight));
-  }
+bool XboxController::GetRightBumper() const {
+  return GetRawButton(Button::kRightBumper);
 }
 
-bool XboxController::GetStickButtonReleased(JoystickHand hand) {
-  if (hand == kLeftHand) {
-    return GetRawButtonReleased(static_cast<int>(Button::kStickLeft));
-  } else {
-    return GetRawButtonReleased(static_cast<int>(Button::kStickRight));
-  }
+bool XboxController::GetLeftBumperPressed() {
+  return GetRawButtonPressed(Button::kLeftBumper);
+}
+
+bool XboxController::GetRightBumperPressed() {
+  return GetRawButtonPressed(Button::kRightBumper);
+}
+
+bool XboxController::GetLeftBumperReleased() {
+  return GetRawButtonReleased(Button::kLeftBumper);
+}
+
+bool XboxController::GetRightBumperReleased() {
+  return GetRawButtonReleased(Button::kRightBumper);
+}
+
+bool XboxController::GetLeftStickButton() const {
+  return GetRawButton(Button::kLeftStick);
+}
+
+bool XboxController::GetRightStickButton() const {
+  return GetRawButton(Button::kRightStick);
+}
+
+bool XboxController::GetLeftStickButtonPressed() {
+  return GetRawButtonPressed(Button::kLeftStick);
+}
+
+bool XboxController::GetRightStickButtonPressed() {
+  return GetRawButtonPressed(Button::kRightStick);
+}
+
+bool XboxController::GetLeftStickButtonReleased() {
+  return GetRawButtonReleased(Button::kLeftStick);
+}
+
+bool XboxController::GetRightStickButtonReleased() {
+  return GetRawButtonReleased(Button::kRightStick);
 }
 
 bool XboxController::GetAButton() const {
-  return GetRawButton(static_cast<int>(Button::kA));
+  return GetRawButton(Button::kA);
 }
 
 bool XboxController::GetAButtonPressed() {
-  return GetRawButtonPressed(static_cast<int>(Button::kA));
+  return GetRawButtonPressed(Button::kA);
 }
 
 bool XboxController::GetAButtonReleased() {
-  return GetRawButtonReleased(static_cast<int>(Button::kA));
+  return GetRawButtonReleased(Button::kA);
 }
 
 bool XboxController::GetBButton() const {
-  return GetRawButton(static_cast<int>(Button::kB));
+  return GetRawButton(Button::kB);
 }
 
 bool XboxController::GetBButtonPressed() {
-  return GetRawButtonPressed(static_cast<int>(Button::kB));
+  return GetRawButtonPressed(Button::kB);
 }
 
 bool XboxController::GetBButtonReleased() {
-  return GetRawButtonReleased(static_cast<int>(Button::kB));
+  return GetRawButtonReleased(Button::kB);
 }
 
 bool XboxController::GetXButton() const {
-  return GetRawButton(static_cast<int>(Button::kX));
+  return GetRawButton(Button::kX);
 }
 
 bool XboxController::GetXButtonPressed() {
-  return GetRawButtonPressed(static_cast<int>(Button::kX));
+  return GetRawButtonPressed(Button::kX);
 }
 
 bool XboxController::GetXButtonReleased() {
-  return GetRawButtonReleased(static_cast<int>(Button::kX));
+  return GetRawButtonReleased(Button::kX);
 }
 
 bool XboxController::GetYButton() const {
-  return GetRawButton(static_cast<int>(Button::kY));
+  return GetRawButton(Button::kY);
 }
 
 bool XboxController::GetYButtonPressed() {
-  return GetRawButtonPressed(static_cast<int>(Button::kY));
+  return GetRawButtonPressed(Button::kY);
 }
 
 bool XboxController::GetYButtonReleased() {
-  return GetRawButtonReleased(static_cast<int>(Button::kY));
+  return GetRawButtonReleased(Button::kY);
 }
 
 bool XboxController::GetBackButton() const {
-  return GetRawButton(static_cast<int>(Button::kBack));
+  return GetRawButton(Button::kBack);
 }
 
 bool XboxController::GetBackButtonPressed() {
-  return GetRawButtonPressed(static_cast<int>(Button::kBack));
+  return GetRawButtonPressed(Button::kBack);
 }
 
 bool XboxController::GetBackButtonReleased() {
-  return GetRawButtonReleased(static_cast<int>(Button::kBack));
+  return GetRawButtonReleased(Button::kBack);
 }
 
 bool XboxController::GetStartButton() const {
-  return GetRawButton(static_cast<int>(Button::kStart));
+  return GetRawButton(Button::kStart);
 }
 
 bool XboxController::GetStartButtonPressed() {
-  return GetRawButtonPressed(static_cast<int>(Button::kStart));
+  return GetRawButtonPressed(Button::kStart);
 }
 
 bool XboxController::GetStartButtonReleased() {
-  return GetRawButtonReleased(static_cast<int>(Button::kStart));
+  return GetRawButtonReleased(Button::kStart);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
deleted file mode 100644
index 7482e22..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/HolonomicDriveController.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/PIDController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/PIDController.cpp
deleted file mode 100644
index 7b9b623..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/PIDController.cpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/PIDController.h"
-
-#include <algorithm>
-#include <cmath>
-
-#include <hal/FRCUsageReporting.h>
-
-#include "frc/controller/ControllerUtil.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc2;
-
-PIDController::PIDController(double Kp, double Ki, double Kd,
-                             units::second_t period)
-    : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
-  static int instances = 0;
-  instances++;
-  HAL_Report(HALUsageReporting::kResourceType_PIDController2, instances);
-  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; }
-
-void PIDController::SetD(double Kd) { m_Kd = Kd; }
-
-double PIDController::GetP() const { return m_Kp; }
-
-double PIDController::GetI() const { return m_Ki; }
-
-double PIDController::GetD() const { return m_Kd; }
-
-units::second_t PIDController::GetPeriod() const {
-  return units::second_t(m_period);
-}
-
-void PIDController::SetSetpoint(double setpoint) { m_setpoint = setpoint; }
-
-double PIDController::GetSetpoint() const { return m_setpoint; }
-
-bool PIDController::AtSetpoint() const {
-  return std::abs(m_positionError) < m_positionTolerance &&
-         std::abs(m_velocityError) < m_velocityTolerance;
-}
-
-void PIDController::EnableContinuousInput(double minimumInput,
-                                          double maximumInput) {
-  m_continuous = true;
-  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;
-  m_maximumIntegral = maximumIntegral;
-}
-
-void PIDController::SetTolerance(double positionTolerance,
-                                 double velocityTolerance) {
-  m_positionTolerance = positionTolerance;
-  m_velocityTolerance = velocityTolerance;
-}
-
-double PIDController::GetPositionError() const { return m_positionError; }
-
-double PIDController::GetVelocityError() const { return m_velocityError; }
-
-double PIDController::Calculate(double measurement) {
-  m_prevError = m_positionError;
-
-  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) {
-    m_totalError =
-        std::clamp(m_totalError + m_positionError * m_period.to<double>(),
-                   m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
-  }
-
-  return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
-}
-
-double PIDController::Calculate(double measurement, double setpoint) {
-  // Set setpoint to provided value
-  SetSetpoint(setpoint);
-  return Calculate(measurement);
-}
-
-void PIDController::Reset() {
-  m_prevError = 0;
-  m_totalError = 0;
-}
-
-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); });
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
deleted file mode 100644
index a0fffc6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/ProfiledPIDController.cpp
+++ /dev/null
@@ -1,16 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/ProfiledPIDController.h"
-
-#include <hal/FRCUsageReporting.h>
-
-void frc::detail::ReportProfiledPIDController() {
-  static int instances = 0;
-  ++instances;
-  HAL_Report(HALUsageReporting::kResourceType_ProfiledPIDController, instances);
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/RamseteController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
deleted file mode 100644
index aad6937..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/controller/RamseteController.cpp
+++ /dev/null
@@ -1,78 +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/controller/RamseteController.h"
-
-#include <cmath>
-
-#include <units/math.h>
-
-using namespace frc;
-
-/**
- * Returns sin(x) / x.
- *
- * @param x Value of which to take sinc(x).
- */
-static double Sinc(double x) {
-  if (std::abs(x) < 1e-9) {
-    return 1.0 - 1.0 / 6.0 * x * x;
-  } else {
-    return std::sin(x) / x;
-  }
-}
-
-RamseteController::RamseteController(double b, double zeta)
-    : m_b{b}, m_zeta{zeta} {}
-
-bool RamseteController::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 RamseteController::SetTolerance(const Pose2d& poseTolerance) {
-  m_poseTolerance = poseTolerance;
-}
-
-ChassisSpeeds RamseteController::Calculate(
-    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.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>();
-
-  double k =
-      2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
-
-  units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
-  units::radians_per_second_t omega{omegaRef + k * eTheta +
-                                    m_b * vRef * Sinc(eTheta) * eY};
-  return ChassisSpeeds{v, 0_mps, omega};
-}
-
-ChassisSpeeds RamseteController::Calculate(
-    const Pose2d& currentPose, const Trajectory::State& desiredState) {
-  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
-                   desiredState.velocity * desiredState.curvature);
-}
-
-void RamseteController::SetEnabled(bool enabled) { m_enabled = enabled; }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index ce9aa05..b54b607 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/drive/DifferentialDrive.h"
 
@@ -11,22 +8,30 @@
 #include <cmath>
 
 #include <hal/FRCUsageReporting.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/MathUtil.h"
 #include "frc/SpeedController.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
+#if defined(_MSC_VER)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 DifferentialDrive::DifferentialDrive(SpeedController& leftMotor,
                                      SpeedController& rightMotor)
     : m_leftMotor(&leftMotor), m_rightMotor(&rightMotor) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, m_leftMotor);
-  registry.AddChild(this, m_rightMotor);
+  wpi::SendableRegistry::AddChild(this, m_leftMotor);
+  wpi::SendableRegistry::AddChild(this, m_rightMotor);
   static int instances = 0;
   ++instances;
-  registry.AddLW(this, "DifferentialDrive", instances);
+  wpi::SendableRegistry::AddLW(this, "DifferentialDrive", instances);
 }
 
 void DifferentialDrive::ArcadeDrive(double xSpeed, double zRotation,
@@ -38,54 +43,19 @@
     reported = true;
   }
 
-  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
   xSpeed = ApplyDeadband(xSpeed, m_deadband);
-
-  zRotation = std::clamp(zRotation, -1.0, 1.0);
   zRotation = ApplyDeadband(zRotation, m_deadband);
 
-  // Square the inputs (while preserving the sign) to increase fine control
-  // while permitting full power.
-  if (squareInputs) {
-    xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
-    zRotation = std::copysign(zRotation * zRotation, zRotation);
-  }
+  auto [left, right] = ArcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-  double leftMotorOutput;
-  double rightMotorOutput;
-
-  double maxInput =
-      std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
-
-  if (xSpeed >= 0.0) {
-    // First quadrant, else second quadrant
-    if (zRotation >= 0.0) {
-      leftMotorOutput = maxInput;
-      rightMotorOutput = xSpeed - zRotation;
-    } else {
-      leftMotorOutput = xSpeed + zRotation;
-      rightMotorOutput = maxInput;
-    }
-  } else {
-    // Third quadrant, else fourth quadrant
-    if (zRotation >= 0.0) {
-      leftMotorOutput = xSpeed + zRotation;
-      rightMotorOutput = maxInput;
-    } else {
-      leftMotorOutput = maxInput;
-      rightMotorOutput = xSpeed - zRotation;
-    }
-  }
-
-  m_leftMotor->Set(std::clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
-  double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
-  m_rightMotor->Set(std::clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+  m_leftMotor->Set(left);
+  m_rightMotor->Set(right);
 
   Feed();
 }
 
 void DifferentialDrive::CurvatureDrive(double xSpeed, double zRotation,
-                                       bool isQuickTurn) {
+                                       bool allowTurnInPlace) {
   static bool reported = false;
   if (!reported) {
     HAL_Report(HALUsageReporting::kResourceType_RobotDrive,
@@ -93,67 +63,13 @@
     reported = true;
   }
 
-  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
   xSpeed = ApplyDeadband(xSpeed, m_deadband);
-
-  zRotation = std::clamp(zRotation, -1.0, 1.0);
   zRotation = ApplyDeadband(zRotation, m_deadband);
 
-  double angularPower;
-  bool overPower;
+  auto [left, right] = CurvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-  if (isQuickTurn) {
-    if (std::abs(xSpeed) < m_quickStopThreshold) {
-      m_quickStopAccumulator =
-          (1 - m_quickStopAlpha) * m_quickStopAccumulator +
-          m_quickStopAlpha * std::clamp(zRotation, -1.0, 1.0) * 2;
-    }
-    overPower = true;
-    angularPower = zRotation;
-  } else {
-    overPower = false;
-    angularPower = std::abs(xSpeed) * zRotation - m_quickStopAccumulator;
-
-    if (m_quickStopAccumulator > 1) {
-      m_quickStopAccumulator -= 1;
-    } else if (m_quickStopAccumulator < -1) {
-      m_quickStopAccumulator += 1;
-    } else {
-      m_quickStopAccumulator = 0.0;
-    }
-  }
-
-  double leftMotorOutput = xSpeed + angularPower;
-  double rightMotorOutput = xSpeed - angularPower;
-
-  // If rotation is overpowered, reduce both outputs to within acceptable range
-  if (overPower) {
-    if (leftMotorOutput > 1.0) {
-      rightMotorOutput -= leftMotorOutput - 1.0;
-      leftMotorOutput = 1.0;
-    } else if (rightMotorOutput > 1.0) {
-      leftMotorOutput -= rightMotorOutput - 1.0;
-      rightMotorOutput = 1.0;
-    } else if (leftMotorOutput < -1.0) {
-      rightMotorOutput -= leftMotorOutput + 1.0;
-      leftMotorOutput = -1.0;
-    } else if (rightMotorOutput < -1.0) {
-      leftMotorOutput -= rightMotorOutput + 1.0;
-      rightMotorOutput = -1.0;
-    }
-  }
-
-  // Normalize the wheel speeds
-  double maxMagnitude =
-      std::max(std::abs(leftMotorOutput), std::abs(rightMotorOutput));
-  if (maxMagnitude > 1.0) {
-    leftMotorOutput /= maxMagnitude;
-    rightMotorOutput /= maxMagnitude;
-  }
-
-  m_leftMotor->Set(leftMotorOutput * m_maxOutput);
-  m_rightMotor->Set(rightMotorOutput * m_maxOutput *
-                    m_rightSideInvertMultiplier);
+  m_leftMotor->Set(left * m_maxOutput);
+  m_rightMotor->Set(right * m_maxOutput);
 
   Feed();
 }
@@ -167,12 +83,96 @@
     reported = true;
   }
 
-  leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
   leftSpeed = ApplyDeadband(leftSpeed, m_deadband);
-
-  rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
   rightSpeed = ApplyDeadband(rightSpeed, m_deadband);
 
+  auto [left, right] = TankDriveIK(leftSpeed, rightSpeed, squareInputs);
+
+  m_leftMotor->Set(left * m_maxOutput);
+  m_rightMotor->Set(right * m_maxOutput);
+
+  Feed();
+}
+
+DifferentialDrive::WheelSpeeds DifferentialDrive::ArcadeDriveIK(
+    double xSpeed, double zRotation, bool squareInputs) {
+  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+  zRotation = std::clamp(zRotation, -1.0, 1.0);
+
+  // Square the inputs (while preserving the sign) to increase fine control
+  // while permitting full power.
+  if (squareInputs) {
+    xSpeed = std::copysign(xSpeed * xSpeed, xSpeed);
+    zRotation = std::copysign(zRotation * zRotation, zRotation);
+  }
+
+  double leftSpeed;
+  double rightSpeed;
+
+  double maxInput =
+      std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
+
+  if (xSpeed >= 0.0) {
+    // First quadrant, else second quadrant
+    if (zRotation >= 0.0) {
+      leftSpeed = maxInput;
+      rightSpeed = xSpeed - zRotation;
+    } else {
+      leftSpeed = xSpeed + zRotation;
+      rightSpeed = maxInput;
+    }
+  } else {
+    // Third quadrant, else fourth quadrant
+    if (zRotation >= 0.0) {
+      leftSpeed = xSpeed + zRotation;
+      rightSpeed = maxInput;
+    } else {
+      leftSpeed = maxInput;
+      rightSpeed = xSpeed - zRotation;
+    }
+  }
+
+  // Normalize the wheel speeds
+  double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
+  if (maxMagnitude > 1.0) {
+    leftSpeed /= maxMagnitude;
+    rightSpeed /= maxMagnitude;
+  }
+
+  return {leftSpeed, rightSpeed};
+}
+
+DifferentialDrive::WheelSpeeds DifferentialDrive::CurvatureDriveIK(
+    double xSpeed, double zRotation, bool allowTurnInPlace) {
+  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+  zRotation = std::clamp(zRotation, -1.0, 1.0);
+
+  double leftSpeed = 0.0;
+  double rightSpeed = 0.0;
+
+  if (allowTurnInPlace) {
+    leftSpeed = xSpeed + zRotation;
+    rightSpeed = xSpeed - zRotation;
+  } else {
+    leftSpeed = xSpeed + std::abs(xSpeed) * zRotation;
+    rightSpeed = xSpeed - std::abs(xSpeed) * zRotation;
+  }
+
+  // Normalize wheel speeds
+  double maxMagnitude = std::max(std::abs(leftSpeed), std::abs(rightSpeed));
+  if (maxMagnitude > 1.0) {
+    leftSpeed /= maxMagnitude;
+    rightSpeed /= maxMagnitude;
+  }
+
+  return {leftSpeed, rightSpeed};
+}
+
+DifferentialDrive::WheelSpeeds DifferentialDrive::TankDriveIK(
+    double leftSpeed, double rightSpeed, bool squareInputs) {
+  leftSpeed = std::clamp(leftSpeed, -1.0, 1.0);
+  rightSpeed = std::clamp(rightSpeed, -1.0, 1.0);
+
   // Square the inputs (while preserving the sign) to increase fine control
   // while permitting full power.
   if (squareInputs) {
@@ -180,26 +180,7 @@
     rightSpeed = std::copysign(rightSpeed * rightSpeed, rightSpeed);
   }
 
-  m_leftMotor->Set(leftSpeed * m_maxOutput);
-  m_rightMotor->Set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
-
-  Feed();
-}
-
-void DifferentialDrive::SetQuickStopThreshold(double threshold) {
-  m_quickStopThreshold = threshold;
-}
-
-void DifferentialDrive::SetQuickStopAlpha(double alpha) {
-  m_quickStopAlpha = alpha;
-}
-
-bool DifferentialDrive::IsRightSideInverted() const {
-  return m_rightSideInvertMultiplier == -1.0;
-}
-
-void DifferentialDrive::SetRightSideInverted(bool rightSideInverted) {
-  m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+  return {leftSpeed, rightSpeed};
 }
 
 void DifferentialDrive::StopMotor() {
@@ -208,21 +189,18 @@
   Feed();
 }
 
-void DifferentialDrive::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "DifferentialDrive";
+std::string DifferentialDrive::GetDescription() const {
+  return "DifferentialDrive";
 }
 
-void DifferentialDrive::InitSendable(SendableBuilder& builder) {
+void DifferentialDrive::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("DifferentialDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Left Motor Speed", [=]() { return m_leftMotor->Get(); },
+      "Left Motor Speed", [=] { return m_leftMotor->Get(); },
       [=](double value) { m_leftMotor->Set(value); });
   builder.AddDoubleProperty(
-      "Right Motor Speed",
-      [=]() { return m_rightMotor->Get() * m_rightSideInvertMultiplier; },
-      [=](double value) {
-        m_rightMotor->Set(value * m_rightSideInvertMultiplier);
-      });
+      "Right Motor Speed", [=] { return m_rightMotor->Get(); },
+      [=](double value) { m_rightMotor->Set(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index 983ce6a..c847678 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/drive/KilloughDrive.h"
 
@@ -11,14 +8,23 @@
 #include <cmath>
 
 #include <hal/FRCUsageReporting.h>
-#include <wpi/math>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/MathUtil.h"
 #include "frc/SpeedController.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
+#if defined(_MSC_VER)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 KilloughDrive::KilloughDrive(SpeedController& leftMotor,
                              SpeedController& rightMotor,
                              SpeedController& backMotor)
@@ -32,19 +38,18 @@
     : m_leftMotor(&leftMotor),
       m_rightMotor(&rightMotor),
       m_backMotor(&backMotor) {
-  m_leftVec = {std::cos(leftMotorAngle * (wpi::math::pi / 180.0)),
-               std::sin(leftMotorAngle * (wpi::math::pi / 180.0))};
-  m_rightVec = {std::cos(rightMotorAngle * (wpi::math::pi / 180.0)),
-                std::sin(rightMotorAngle * (wpi::math::pi / 180.0))};
-  m_backVec = {std::cos(backMotorAngle * (wpi::math::pi / 180.0)),
-               std::sin(backMotorAngle * (wpi::math::pi / 180.0))};
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, m_leftMotor);
-  registry.AddChild(this, m_rightMotor);
-  registry.AddChild(this, m_backMotor);
+  m_leftVec = {std::cos(leftMotorAngle * (wpi::numbers::pi / 180.0)),
+               std::sin(leftMotorAngle * (wpi::numbers::pi / 180.0))};
+  m_rightVec = {std::cos(rightMotorAngle * (wpi::numbers::pi / 180.0)),
+                std::sin(rightMotorAngle * (wpi::numbers::pi / 180.0))};
+  m_backVec = {std::cos(backMotorAngle * (wpi::numbers::pi / 180.0)),
+               std::sin(backMotorAngle * (wpi::numbers::pi / 180.0))};
+  wpi::SendableRegistry::AddChild(this, m_leftMotor);
+  wpi::SendableRegistry::AddChild(this, m_rightMotor);
+  wpi::SendableRegistry::AddChild(this, m_backMotor);
   static int instances = 0;
   ++instances;
-  registry.AddLW(this, "KilloughDrive", instances);
+  wpi::SendableRegistry::AddLW(this, "KilloughDrive", instances);
 }
 
 void KilloughDrive::DriveCartesian(double ySpeed, double xSpeed,
@@ -55,26 +60,15 @@
     reported = true;
   }
 
-  ySpeed = std::clamp(ySpeed, -1.0, 1.0);
   ySpeed = ApplyDeadband(ySpeed, m_deadband);
-
-  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
   xSpeed = ApplyDeadband(xSpeed, m_deadband);
 
-  // Compensate for gyro angle.
-  Vector2d input{ySpeed, xSpeed};
-  input.Rotate(-gyroAngle);
+  auto [left, right, back] =
+      DriveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
 
-  double wheelSpeeds[3];
-  wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
-  wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
-  wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
-
-  Normalize(wheelSpeeds);
-
-  m_leftMotor->Set(wheelSpeeds[kLeft] * m_maxOutput);
-  m_rightMotor->Set(wheelSpeeds[kRight] * m_maxOutput);
-  m_backMotor->Set(wheelSpeeds[kBack] * m_maxOutput);
+  m_leftMotor->Set(left * m_maxOutput);
+  m_rightMotor->Set(right * m_maxOutput);
+  m_backMotor->Set(back * m_maxOutput);
 
   Feed();
 }
@@ -87,11 +81,32 @@
     reported = true;
   }
 
-  DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
-                 magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
+  DriveCartesian(magnitude * std::sin(angle * (wpi::numbers::pi / 180.0)),
+                 magnitude * std::cos(angle * (wpi::numbers::pi / 180.0)),
                  zRotation, 0.0);
 }
 
+KilloughDrive::WheelSpeeds KilloughDrive::DriveCartesianIK(double ySpeed,
+                                                           double xSpeed,
+                                                           double zRotation,
+                                                           double gyroAngle) {
+  ySpeed = std::clamp(ySpeed, -1.0, 1.0);
+  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+
+  // Compensate for gyro angle.
+  Vector2d input{ySpeed, xSpeed};
+  input.Rotate(-gyroAngle);
+
+  double wheelSpeeds[3];
+  wheelSpeeds[kLeft] = input.ScalarProject(m_leftVec) + zRotation;
+  wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
+  wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
+
+  Normalize(wheelSpeeds);
+
+  return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]};
+}
+
 void KilloughDrive::StopMotor() {
   m_leftMotor->StopMotor();
   m_rightMotor->StopMotor();
@@ -99,21 +114,21 @@
   Feed();
 }
 
-void KilloughDrive::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "KilloughDrive";
+std::string KilloughDrive::GetDescription() const {
+  return "KilloughDrive";
 }
 
-void KilloughDrive::InitSendable(SendableBuilder& builder) {
+void KilloughDrive::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("KilloughDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Left Motor Speed", [=]() { return m_leftMotor->Get(); },
+      "Left Motor Speed", [=] { return m_leftMotor->Get(); },
       [=](double value) { m_leftMotor->Set(value); });
   builder.AddDoubleProperty(
-      "Right Motor Speed", [=]() { return m_rightMotor->Get(); },
+      "Right Motor Speed", [=] { return m_rightMotor->Get(); },
       [=](double value) { m_rightMotor->Set(value); });
   builder.AddDoubleProperty(
-      "Back Motor Speed", [=]() { return m_backMotor->Get(); },
+      "Back Motor Speed", [=] { return m_backMotor->Get(); },
       [=](double value) { m_backMotor->Set(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index cb11d8a..b74c611 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/drive/MecanumDrive.h"
 
@@ -11,15 +8,24 @@
 #include <cmath>
 
 #include <hal/FRCUsageReporting.h>
-#include <wpi/math>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/MathUtil.h"
 #include "frc/SpeedController.h"
 #include "frc/drive/Vector2d.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
+#if defined(_MSC_VER)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 MecanumDrive::MecanumDrive(SpeedController& frontLeftMotor,
                            SpeedController& rearLeftMotor,
                            SpeedController& frontRightMotor,
@@ -28,14 +34,13 @@
       m_rearLeftMotor(&rearLeftMotor),
       m_frontRightMotor(&frontRightMotor),
       m_rearRightMotor(&rearRightMotor) {
-  auto& registry = SendableRegistry::GetInstance();
-  registry.AddChild(this, m_frontLeftMotor);
-  registry.AddChild(this, m_rearLeftMotor);
-  registry.AddChild(this, m_frontRightMotor);
-  registry.AddChild(this, m_rearRightMotor);
+  wpi::SendableRegistry::AddChild(this, m_frontLeftMotor);
+  wpi::SendableRegistry::AddChild(this, m_rearLeftMotor);
+  wpi::SendableRegistry::AddChild(this, m_frontRightMotor);
+  wpi::SendableRegistry::AddChild(this, m_rearRightMotor);
   static int instances = 0;
   ++instances;
-  registry.AddLW(this, "MecanumDrive", instances);
+  wpi::SendableRegistry::AddLW(this, "MecanumDrive", instances);
 }
 
 void MecanumDrive::DriveCartesian(double ySpeed, double xSpeed,
@@ -46,30 +51,16 @@
     reported = true;
   }
 
-  ySpeed = std::clamp(ySpeed, -1.0, 1.0);
   ySpeed = ApplyDeadband(ySpeed, m_deadband);
-
-  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
   xSpeed = ApplyDeadband(xSpeed, m_deadband);
 
-  // Compensate for gyro angle.
-  Vector2d input{ySpeed, xSpeed};
-  input.Rotate(-gyroAngle);
+  auto [frontLeft, frontRight, rearLeft, rearRight] =
+      DriveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
 
-  double wheelSpeeds[4];
-  wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
-  wheelSpeeds[kFrontRight] = -input.x + input.y - zRotation;
-  wheelSpeeds[kRearLeft] = -input.x + input.y + zRotation;
-  wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
-
-  Normalize(wheelSpeeds);
-
-  m_frontLeftMotor->Set(wheelSpeeds[kFrontLeft] * m_maxOutput);
-  m_frontRightMotor->Set(wheelSpeeds[kFrontRight] * m_maxOutput *
-                         m_rightSideInvertMultiplier);
-  m_rearLeftMotor->Set(wheelSpeeds[kRearLeft] * m_maxOutput);
-  m_rearRightMotor->Set(wheelSpeeds[kRearRight] * m_maxOutput *
-                        m_rightSideInvertMultiplier);
+  m_frontLeftMotor->Set(frontLeft * m_maxOutput);
+  m_frontRightMotor->Set(frontRight * m_maxOutput);
+  m_rearLeftMotor->Set(rearLeft * m_maxOutput);
+  m_rearRightMotor->Set(rearRight * m_maxOutput);
 
   Feed();
 }
@@ -82,19 +73,11 @@
     reported = true;
   }
 
-  DriveCartesian(magnitude * std::sin(angle * (wpi::math::pi / 180.0)),
-                 magnitude * std::cos(angle * (wpi::math::pi / 180.0)),
+  DriveCartesian(magnitude * std::cos(angle * (wpi::numbers::pi / 180.0)),
+                 magnitude * std::sin(angle * (wpi::numbers::pi / 180.0)),
                  zRotation, 0.0);
 }
 
-bool MecanumDrive::IsRightSideInverted() const {
-  return m_rightSideInvertMultiplier == -1.0;
-}
-
-void MecanumDrive::SetRightSideInverted(bool rightSideInverted) {
-  m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
-}
-
 void MecanumDrive::StopMotor() {
   m_frontLeftMotor->StopMotor();
   m_frontRightMotor->StopMotor();
@@ -103,30 +86,47 @@
   Feed();
 }
 
-void MecanumDrive::GetDescription(wpi::raw_ostream& desc) const {
-  desc << "MecanumDrive";
+MecanumDrive::WheelSpeeds MecanumDrive::DriveCartesianIK(double ySpeed,
+                                                         double xSpeed,
+                                                         double zRotation,
+                                                         double gyroAngle) {
+  ySpeed = std::clamp(ySpeed, -1.0, 1.0);
+  xSpeed = std::clamp(xSpeed, -1.0, 1.0);
+
+  // Compensate for gyro angle.
+  Vector2d input{ySpeed, xSpeed};
+  input.Rotate(-gyroAngle);
+
+  double wheelSpeeds[4];
+  wheelSpeeds[kFrontLeft] = input.x + input.y + zRotation;
+  wheelSpeeds[kFrontRight] = input.x - input.y - zRotation;
+  wheelSpeeds[kRearLeft] = input.x - input.y + zRotation;
+  wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
+
+  Normalize(wheelSpeeds);
+
+  return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
+          wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};
 }
 
-void MecanumDrive::InitSendable(SendableBuilder& builder) {
+std::string MecanumDrive::GetDescription() const {
+  return "MecanumDrive";
+}
+
+void MecanumDrive::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetSmartDashboardType("MecanumDrive");
   builder.SetActuator(true);
   builder.SetSafeState([=] { StopMotor(); });
   builder.AddDoubleProperty(
-      "Front Left Motor Speed", [=]() { return m_frontLeftMotor->Get(); },
+      "Front Left Motor Speed", [=] { return m_frontLeftMotor->Get(); },
       [=](double value) { m_frontLeftMotor->Set(value); });
   builder.AddDoubleProperty(
-      "Front Right Motor Speed",
-      [=]() { return m_frontRightMotor->Get() * m_rightSideInvertMultiplier; },
-      [=](double value) {
-        m_frontRightMotor->Set(value * m_rightSideInvertMultiplier);
-      });
+      "Front Right Motor Speed", [=] { return m_frontRightMotor->Get(); },
+      [=](double value) { m_frontRightMotor->Set(value); });
   builder.AddDoubleProperty(
-      "Rear Left Motor Speed", [=]() { return m_rearLeftMotor->Get(); },
+      "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; },
-      [=](double value) {
-        m_rearRightMotor->Set(value * m_rightSideInvertMultiplier);
-      });
+      "Rear Right Motor Speed", [=] { return m_rearRightMotor->Get(); },
+      [=](double value) { m_rearRightMotor->Set(value); });
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index fbce5a1..2159958 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/drive/RobotDriveBase.h"
 
@@ -13,32 +10,32 @@
 
 #include <hal/FRCUsageReporting.h>
 
-#include "frc/Base.h"
-#include "frc/SpeedController.h"
+#include "frc/MathUtil.h"
+#include "frc/motorcontrol/MotorController.h"
 
 using namespace frc;
 
-RobotDriveBase::RobotDriveBase() { SetSafetyEnabled(true); }
-
-void RobotDriveBase::SetDeadband(double deadband) { m_deadband = deadband; }
-
-void RobotDriveBase::SetMaxOutput(double maxOutput) { m_maxOutput = maxOutput; }
-
-void RobotDriveBase::FeedWatchdog() { Feed(); }
-
-double RobotDriveBase::ApplyDeadband(double value, double deadband) {
-  if (std::abs(value) > deadband) {
-    if (value > 0.0) {
-      return (value - deadband) / (1.0 - deadband);
-    } else {
-      return (value + deadband) / (1.0 - deadband);
-    }
-  } else {
-    return 0.0;
-  }
+RobotDriveBase::RobotDriveBase() {
+  SetSafetyEnabled(true);
 }
 
-void RobotDriveBase::Normalize(wpi::MutableArrayRef<double> wheelSpeeds) {
+void RobotDriveBase::SetDeadband(double deadband) {
+  m_deadband = deadband;
+}
+
+void RobotDriveBase::SetMaxOutput(double maxOutput) {
+  m_maxOutput = maxOutput;
+}
+
+void RobotDriveBase::FeedWatchdog() {
+  Feed();
+}
+
+double RobotDriveBase::ApplyDeadband(double value, double deadband) {
+  return frc::ApplyDeadband(value, deadband);
+}
+
+void RobotDriveBase::Normalize(wpi::span<double> wheelSpeeds) {
   double maxMagnitude = std::abs(wheelSpeeds[0]);
   for (size_t i = 1; i < wheelSpeeds.size(); i++) {
     double temp = std::abs(wheelSpeeds[i]);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/Vector2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
index a6e68a6..a342dc2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/drive/Vector2d.cpp
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/drive/Vector2d.h"
 
 #include <cmath>
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 using namespace frc;
 
@@ -19,8 +16,8 @@
 }
 
 void Vector2d::Rotate(double angle) {
-  double cosA = std::cos(angle * (wpi::math::pi / 180.0));
-  double sinA = std::sin(angle * (wpi::math::pi / 180.0));
+  double cosA = std::cos(angle * (wpi::numbers::pi / 180.0));
+  double sinA = std::sin(angle * (wpi::numbers::pi / 180.0));
   double out[2];
   out[0] = x * cosA - y * sinA;
   out[1] = x * sinA + y * cosA;
@@ -32,7 +29,9 @@
   return x * vec.x + y * vec.y;
 }
 
-double Vector2d::Magnitude() const { return std::sqrt(x * x + y * y); }
+double Vector2d::Magnitude() const {
+  return std::sqrt(x * x + y * y);
+}
 
 double Vector2d::ScalarProject(const Vector2d& vec) const {
   return Dot(vec) / vec.Magnitude();
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/filters/Filter.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/filters/Filter.cpp
deleted file mode 100644
index c25d7af..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/filters/Filter.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/filters/Filter.h"
-
-#include "frc/Base.h"
-
-using namespace frc;
-
-Filter::Filter(PIDSource& source)
-    : m_source(std::shared_ptr<PIDSource>(&source, NullDeleter<PIDSource>())) {}
-
-Filter::Filter(std::shared_ptr<PIDSource> source)
-    : m_source(std::move(source)) {}
-
-void Filter::SetPIDSourceType(PIDSourceType pidSource) {
-  m_source->SetPIDSourceType(pidSource);
-}
-
-PIDSourceType Filter::GetPIDSourceType() const {
-  return m_source->GetPIDSourceType();
-}
-
-double Filter::PIDGetSource() { return m_source->PIDGet(); }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.cpp
deleted file mode 100644
index 434cc27..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/filters/LinearDigitalFilter.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/filters/LinearDigitalFilter.h"
-
-#include <cassert>
-#include <cmath>
-
-#include <hal/FRCUsageReporting.h>
-
-using namespace frc;
-
-LinearDigitalFilter::LinearDigitalFilter(PIDSource& source,
-                                         wpi::ArrayRef<double> ffGains,
-                                         wpi::ArrayRef<double> fbGains)
-    : Filter(source),
-      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);
-}
-
-LinearDigitalFilter::LinearDigitalFilter(PIDSource& source,
-                                         std::initializer_list<double> ffGains,
-                                         std::initializer_list<double> fbGains)
-    : LinearDigitalFilter(source,
-                          wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
-                          wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
-
-LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
-                                         wpi::ArrayRef<double> ffGains,
-                                         wpi::ArrayRef<double> fbGains)
-    : Filter(source),
-      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);
-}
-
-LinearDigitalFilter::LinearDigitalFilter(std::shared_ptr<PIDSource> source,
-                                         std::initializer_list<double> ffGains,
-                                         std::initializer_list<double> fbGains)
-    : LinearDigitalFilter(source,
-                          wpi::makeArrayRef(ffGains.begin(), ffGains.end()),
-                          wpi::makeArrayRef(fbGains.begin(), fbGains.end())) {}
-
-LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(PIDSource& source,
-                                                       double timeConstant,
-                                                       double period) {
-  double gain = std::exp(-period / timeConstant);
-  return LinearDigitalFilter(source, {1.0 - gain}, {-gain});
-}
-
-LinearDigitalFilter LinearDigitalFilter::HighPass(PIDSource& source,
-                                                  double timeConstant,
-                                                  double period) {
-  double gain = std::exp(-period / timeConstant);
-  return LinearDigitalFilter(source, {gain, -gain}, {-gain});
-}
-
-LinearDigitalFilter LinearDigitalFilter::MovingAverage(PIDSource& source,
-                                                       int taps) {
-  assert(taps > 0);
-
-  std::vector<double> gains(taps, 1.0 / taps);
-  return LinearDigitalFilter(source, gains, {});
-}
-
-LinearDigitalFilter LinearDigitalFilter::SinglePoleIIR(
-    std::shared_ptr<PIDSource> source, double timeConstant, double period) {
-  double gain = std::exp(-period / timeConstant);
-  return LinearDigitalFilter(std::move(source), {1.0 - gain}, {-gain});
-}
-
-LinearDigitalFilter LinearDigitalFilter::HighPass(
-    std::shared_ptr<PIDSource> source, double timeConstant, double period) {
-  double gain = std::exp(-period / timeConstant);
-  return LinearDigitalFilter(std::move(source), {gain, -gain}, {-gain});
-}
-
-LinearDigitalFilter LinearDigitalFilter::MovingAverage(
-    std::shared_ptr<PIDSource> source, int taps) {
-  assert(taps > 0);
-
-  std::vector<double> gains(taps, 1.0 / taps);
-  return LinearDigitalFilter(std::move(source), gains, {});
-}
-
-double LinearDigitalFilter::Get() const {
-  double retVal = 0.0;
-
-  // 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];
-  }
-
-  return retVal;
-}
-
-void LinearDigitalFilter::Reset() {
-  m_inputs.reset();
-  m_outputs.reset();
-}
-
-double LinearDigitalFilter::PIDGet() {
-  double retVal = 0.0;
-
-  // Rotate the inputs
-  m_inputs.push_front(PIDGetSource());
-
-  // Calculate the new value
-  for (size_t i = 0; i < m_inputGains.size(); i++) {
-    retVal += m_inputs[i] * m_inputGains[i];
-  }
-  for (size_t i = 0; i < m_outputGains.size(); i++) {
-    retVal -= m_outputs[i] * m_outputGains[i];
-  }
-
-  // Rotate the outputs
-  m_outputs.push_front(retVal);
-
-  return retVal;
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/frc2/Timer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/frc2/Timer.cpp
deleted file mode 100644
index 36da4c6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/frc2/Timer.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc2/Timer.h"
-
-#include <chrono>
-#include <thread>
-
-#include "frc/DriverStation.h"
-#include "frc/RobotController.h"
-
-namespace frc2 {
-
-void Wait(units::second_t seconds) {
-  std::this_thread::sleep_for(
-      std::chrono::duration<double>(seconds.to<double>()));
-}
-
-units::second_t GetTime() {
-  using std::chrono::duration;
-  using std::chrono::duration_cast;
-  using std::chrono::system_clock;
-
-  return units::second_t(
-      duration_cast<duration<double>>(system_clock::now().time_since_epoch())
-          .count());
-}
-
-}  // namespace frc2
-
-using namespace frc2;
-
-Timer::Timer() { Reset(); }
-
-Timer::Timer(const Timer& rhs)
-    : m_startTime(rhs.m_startTime),
-      m_accumulatedTime(rhs.m_accumulatedTime),
-      m_running(rhs.m_running) {}
-
-Timer& Timer::operator=(const Timer& rhs) {
-  std::scoped_lock lock(m_mutex, rhs.m_mutex);
-
-  m_startTime = rhs.m_startTime;
-  m_accumulatedTime = rhs.m_accumulatedTime;
-  m_running = rhs.m_running;
-
-  return *this;
-}
-
-Timer::Timer(Timer&& rhs)
-    : m_startTime(std::move(rhs.m_startTime)),
-      m_accumulatedTime(std::move(rhs.m_accumulatedTime)),
-      m_running(std::move(rhs.m_running)) {}
-
-Timer& Timer::operator=(Timer&& rhs) {
-  std::scoped_lock lock(m_mutex, rhs.m_mutex);
-
-  m_startTime = std::move(rhs.m_startTime);
-  m_accumulatedTime = std::move(rhs.m_accumulatedTime);
-  m_running = std::move(rhs.m_running);
-
-  return *this;
-}
-
-units::second_t Timer::Get() const {
-  units::second_t result;
-  units::second_t currentTime = GetFPGATimestamp();
-
-  std::scoped_lock lock(m_mutex);
-  if (m_running) {
-    result = (currentTime - m_startTime) + m_accumulatedTime;
-  } else {
-    result = m_accumulatedTime;
-  }
-
-  return result;
-}
-
-void Timer::Reset() {
-  std::scoped_lock lock(m_mutex);
-  m_accumulatedTime = 0_s;
-  m_startTime = GetFPGATimestamp();
-}
-
-void Timer::Start() {
-  std::scoped_lock lock(m_mutex);
-  if (!m_running) {
-    m_startTime = GetFPGATimestamp();
-    m_running = true;
-  }
-}
-
-void Timer::Stop() {
-  units::second_t temp = Get();
-
-  std::scoped_lock lock(m_mutex);
-  if (m_running) {
-    m_accumulatedTime = temp;
-    m_running = false;
-  }
-}
-
-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;
-  }
-}
-
-units::second_t Timer::GetFPGATimestamp() {
-  // FPGA returns the timestamp in microseconds
-  return units::second_t(frc::RobotController::GetFPGATime() * 1.0e-6);
-}
-
-units::second_t Timer::GetMatchTime() {
-  return units::second_t(frc::DriverStation::GetInstance().GetMatchTime());
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/interfaces/Potentiometer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/interfaces/Potentiometer.cpp
deleted file mode 100644
index 44f4aab..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/interfaces/Potentiometer.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/interfaces/Potentiometer.h"
-
-#include "frc/Utility.h"
-
-using namespace frc;
-
-void Potentiometer::SetPIDSourceType(PIDSourceType pidSource) {
-  if (wpi_assert(pidSource == PIDSourceType::kDisplacement)) {
-    m_pidSource = pidSource;
-  }
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index 517ecf7..f8be7de 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/livewindow/LiveWindow.h"
 
@@ -11,153 +8,191 @@
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/mutex.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/smartdashboard/Sendable.h"
 #include "frc/smartdashboard/SendableBuilderImpl.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
-using wpi::Twine;
+namespace {
+struct Component {
+  bool firstTime = true;
+  bool telemetryEnabled = true;
+};
 
-struct LiveWindow::Impl {
-  Impl();
-
-  struct Component {
-    bool firstTime = true;
-    bool telemetryEnabled = true;
-  };
+struct Instance {
+  Instance() {
+    wpi::SendableRegistry::SetLiveWindowBuilderFactory(
+        [] { return std::make_unique<SendableBuilderImpl>(); });
+  }
 
   wpi::mutex mutex;
 
-  SendableRegistry& registry;
-  int dataHandle;
+  int dataHandle = wpi::SendableRegistry::GetDataHandle();
 
-  std::shared_ptr<nt::NetworkTable> liveWindowTable;
-  std::shared_ptr<nt::NetworkTable> statusTable;
-  nt::NetworkTableEntry enabledEntry;
+  std::shared_ptr<nt::NetworkTable> liveWindowTable =
+      nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow");
+  std::shared_ptr<nt::NetworkTable> statusTable =
+      liveWindowTable->GetSubTable(".status");
+  nt::NetworkTableEntry enabledEntry = statusTable->GetEntry("LW Enabled");
 
   bool startLiveWindow = false;
   bool liveWindowEnabled = false;
   bool telemetryEnabled = true;
 
-  std::shared_ptr<Component> GetOrAdd(Sendable* sendable);
-};
+  std::function<void()> enabled;
+  std::function<void()> disabled;
 
-LiveWindow::Impl::Impl()
-    : registry(SendableRegistry::GetInstance()),
-      dataHandle(registry.GetDataHandle()),
-      liveWindowTable(
-          nt::NetworkTableInstance::GetDefault().GetTable("LiveWindow")) {
-  statusTable = liveWindowTable->GetSubTable(".status");
-  enabledEntry = statusTable->GetEntry("LW Enabled");
+  std::shared_ptr<Component> GetOrAdd(wpi::Sendable* sendable);
+};
+}  // namespace
+
+static Instance& GetInstance() {
+  static Instance instance;
+  return instance;
 }
 
-std::shared_ptr<LiveWindow::Impl::Component> LiveWindow::Impl::GetOrAdd(
-    Sendable* sendable) {
+std::shared_ptr<Component> Instance::GetOrAdd(wpi::Sendable* sendable) {
   auto data = std::static_pointer_cast<Component>(
-      registry.GetData(sendable, dataHandle));
+      wpi::SendableRegistry::GetData(sendable, dataHandle));
   if (!data) {
     data = std::make_shared<Component>();
-    registry.SetData(sendable, dataHandle, data);
+    wpi::SendableRegistry::SetData(sendable, dataHandle, data);
   }
   return data;
 }
 
 LiveWindow* LiveWindow::GetInstance() {
+  ::GetInstance();
   static LiveWindow instance;
   return &instance;
 }
 
-void LiveWindow::EnableTelemetry(Sendable* sendable) {
-  std::scoped_lock lock(m_impl->mutex);
-  // Re-enable global setting in case DisableAllTelemetry() was called.
-  m_impl->telemetryEnabled = true;
-  m_impl->GetOrAdd(sendable)->telemetryEnabled = true;
+void LiveWindow::SetEnabledCallback(std::function<void()> func) {
+  ::GetInstance().enabled = func;
 }
 
-void LiveWindow::DisableTelemetry(Sendable* sendable) {
-  std::scoped_lock lock(m_impl->mutex);
-  m_impl->GetOrAdd(sendable)->telemetryEnabled = false;
+void LiveWindow::SetDisabledCallback(std::function<void()> func) {
+  ::GetInstance().disabled = func;
+}
+
+void LiveWindow::EnableTelemetry(wpi::Sendable* sendable) {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  // Re-enable global setting in case DisableAllTelemetry() was called.
+  inst.telemetryEnabled = true;
+  inst.GetOrAdd(sendable)->telemetryEnabled = true;
+}
+
+void LiveWindow::DisableTelemetry(wpi::Sendable* sendable) {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  inst.GetOrAdd(sendable)->telemetryEnabled = false;
 }
 
 void LiveWindow::DisableAllTelemetry() {
-  std::scoped_lock lock(m_impl->mutex);
-  m_impl->telemetryEnabled = false;
-  m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
-    if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
-    std::static_pointer_cast<Impl::Component>(cbdata.data)->telemetryEnabled =
-        false;
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  inst.telemetryEnabled = false;
+  wpi::SendableRegistry::ForeachLiveWindow(inst.dataHandle, [&](auto& cbdata) {
+    if (!cbdata.data) {
+      cbdata.data = std::make_shared<Component>();
+    }
+    std::static_pointer_cast<Component>(cbdata.data)->telemetryEnabled = false;
   });
 }
 
-bool LiveWindow::IsEnabled() const {
-  std::scoped_lock lock(m_impl->mutex);
-  return m_impl->liveWindowEnabled;
+bool LiveWindow::IsEnabled() {
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  return inst.liveWindowEnabled;
 }
 
 void LiveWindow::SetEnabled(bool enabled) {
-  std::scoped_lock lock(m_impl->mutex);
-  if (m_impl->liveWindowEnabled == enabled) return;
-  m_impl->startLiveWindow = enabled;
-  m_impl->liveWindowEnabled = enabled;
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  if (inst.liveWindowEnabled == enabled) {
+    return;
+  }
+  inst.startLiveWindow = enabled;
+  inst.liveWindowEnabled = enabled;
   // Force table generation now to make sure everything is defined
   UpdateValuesUnsafe();
   if (enabled) {
-    if (this->enabled) this->enabled();
+    if (inst.enabled) {
+      inst.enabled();
+    }
   } else {
-    m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
-      cbdata.builder.StopLiveWindowMode();
-    });
-    if (this->disabled) this->disabled();
+    wpi::SendableRegistry::ForeachLiveWindow(
+        inst.dataHandle, [&](auto& cbdata) {
+          static_cast<SendableBuilderImpl&>(cbdata.builder)
+              .StopLiveWindowMode();
+        });
+    if (inst.disabled) {
+      inst.disabled();
+    }
   }
-  m_impl->enabledEntry.SetBoolean(enabled);
+  inst.enabledEntry.SetBoolean(enabled);
 }
 
 void LiveWindow::UpdateValues() {
-  std::scoped_lock lock(m_impl->mutex);
+  auto& inst = ::GetInstance();
+  std::scoped_lock lock(inst.mutex);
   UpdateValuesUnsafe();
 }
 
 void LiveWindow::UpdateValuesUnsafe() {
+  auto& inst = ::GetInstance();
   // Only do this if either LiveWindow mode or telemetry is enabled.
-  if (!m_impl->liveWindowEnabled && !m_impl->telemetryEnabled) return;
+  if (!inst.liveWindowEnabled && !inst.telemetryEnabled) {
+    return;
+  }
 
-  m_impl->registry.ForeachLiveWindow(m_impl->dataHandle, [&](auto& cbdata) {
-    if (!cbdata.sendable || cbdata.parent) return;
+  wpi::SendableRegistry::ForeachLiveWindow(inst.dataHandle, [&](auto& cbdata) {
+    if (!cbdata.sendable || cbdata.parent) {
+      return;
+    }
 
-    if (!cbdata.data) cbdata.data = std::make_shared<Impl::Component>();
+    if (!cbdata.data) {
+      cbdata.data = std::make_shared<Component>();
+    }
 
-    auto& comp = *std::static_pointer_cast<Impl::Component>(cbdata.data);
+    auto& comp = *std::static_pointer_cast<Component>(cbdata.data);
 
-    if (!m_impl->liveWindowEnabled && !comp.telemetryEnabled) return;
+    if (!inst.liveWindowEnabled && !comp.telemetryEnabled) {
+      return;
+    }
 
     if (comp.firstTime) {
       // By holding off creating the NetworkTable entries, it allows the
       // components to be redefined. This allows default sensor and actuator
       // values to be created that are replaced with the custom names from
       // users calling setName.
-      if (cbdata.name.empty()) return;
-      auto ssTable = m_impl->liveWindowTable->GetSubTable(cbdata.subsystem);
-      std::shared_ptr<NetworkTable> table;
+      if (cbdata.name.empty()) {
+        return;
+      }
+      auto ssTable = inst.liveWindowTable->GetSubTable(cbdata.subsystem);
+      std::shared_ptr<nt::NetworkTable> table;
       // Treat name==subsystem as top level of subsystem
-      if (cbdata.name == cbdata.subsystem)
+      if (cbdata.name == cbdata.subsystem) {
         table = ssTable;
-      else
+      } else {
         table = ssTable->GetSubTable(cbdata.name);
+      }
       table->GetEntry(".name").SetString(cbdata.name);
-      cbdata.builder.SetTable(table);
+      static_cast<SendableBuilderImpl&>(cbdata.builder).SetTable(table);
       cbdata.sendable->InitSendable(cbdata.builder);
       ssTable->GetEntry(".type").SetString("LW Subsystem");
 
       comp.firstTime = false;
     }
 
-    if (m_impl->startLiveWindow) cbdata.builder.StartLiveWindowMode();
-    cbdata.builder.UpdateTable();
+    if (inst.startLiveWindow) {
+      static_cast<SendableBuilderImpl&>(cbdata.builder).StartLiveWindowMode();
+    }
+    cbdata.builder.Update();
   });
 
-  m_impl->startLiveWindow = false;
+  inst.startLiveWindow = false;
 }
-
-LiveWindow::LiveWindow() : m_impl(new Impl) {}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/DMC60.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/DMC60.cpp
new file mode 100644
index 0000000..81fa868
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/DMC60.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/DMC60.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+DMC60::DMC60(int channel) : PWMMotorController("DMC60", channel) {
+  m_pwm.SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_DigilentDMC60, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Jaguar.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Jaguar.cpp
new file mode 100644
index 0000000..c68ae0c
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Jaguar.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/Jaguar.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+Jaguar::Jaguar(int channel) : PWMMotorController("Jaguar", channel) {
+  m_pwm.SetBounds(2.31, 1.55, 1.507, 1.454, 0.697);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_Jaguar, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
new file mode 100644
index 0000000..b7f26d1
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/MotorControllerGroup.cpp
@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/MotorControllerGroup.h"
+
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/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
+
+MotorControllerGroup::MotorControllerGroup(
+    std::vector<std::reference_wrapper<MotorController>>&& motorControllers)
+    : m_motorControllers(std::move(motorControllers)) {
+  Initialize();
+}
+
+void MotorControllerGroup::Initialize() {
+  for (auto& motorController : m_motorControllers) {
+    wpi::SendableRegistry::AddChild(this, &motorController.get());
+  }
+  static int instances = 0;
+  ++instances;
+  wpi::SendableRegistry::Add(this, "MotorControllerGroup", instances);
+}
+
+void MotorControllerGroup::Set(double speed) {
+  for (auto motorController : m_motorControllers) {
+    motorController.get().Set(m_isInverted ? -speed : speed);
+  }
+}
+
+double MotorControllerGroup::Get() const {
+  if (!m_motorControllers.empty()) {
+    return m_motorControllers.front().get().Get() * (m_isInverted ? -1 : 1);
+  }
+  return 0.0;
+}
+
+void MotorControllerGroup::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool MotorControllerGroup::GetInverted() const {
+  return m_isInverted;
+}
+
+void MotorControllerGroup::Disable() {
+  for (auto motorController : m_motorControllers) {
+    motorController.get().Disable();
+  }
+}
+
+void MotorControllerGroup::StopMotor() {
+  for (auto motorController : m_motorControllers) {
+    motorController.get().StopMotor();
+  }
+}
+
+void MotorControllerGroup::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Motor Controller");
+  builder.SetActuator(true);
+  builder.SetSafeState([=] { StopMotor(); });
+  builder.AddDoubleProperty(
+      "Value", [=] { return Get(); }, [=](double value) { Set(value); });
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
new file mode 100644
index 0000000..21164eb
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/NidecBrushless.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/NidecBrushless.h"
+
+#include <fmt/format.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+using namespace frc;
+
+NidecBrushless::NidecBrushless(int pwmChannel, int dioChannel)
+    : m_dio(dioChannel), m_pwm(pwmChannel) {
+  wpi::SendableRegistry::AddChild(this, &m_dio);
+  wpi::SendableRegistry::AddChild(this, &m_pwm);
+  SetExpiration(0_s);
+  SetSafetyEnabled(false);
+
+  // the dio controls the output (in PWM mode)
+  m_dio.SetPWMRate(15625);
+  m_dio.EnablePWM(0.5);
+
+  HAL_Report(HALUsageReporting::kResourceType_NidecBrushless, pwmChannel + 1);
+  wpi::SendableRegistry::AddLW(this, "Nidec Brushless", pwmChannel);
+}
+
+void NidecBrushless::Set(double speed) {
+  if (!m_disabled) {
+    m_speed = speed;
+    m_dio.UpdateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
+    m_pwm.SetRaw(0xffff);
+  }
+  Feed();
+}
+
+double NidecBrushless::Get() const {
+  return m_speed;
+}
+
+void NidecBrushless::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool NidecBrushless::GetInverted() const {
+  return m_isInverted;
+}
+
+void NidecBrushless::Disable() {
+  m_disabled = true;
+  m_dio.UpdateDutyCycle(0.5);
+  m_pwm.SetDisabled();
+}
+
+void NidecBrushless::Enable() {
+  m_disabled = false;
+}
+
+void NidecBrushless::StopMotor() {
+  m_dio.UpdateDutyCycle(0.5);
+  m_pwm.SetDisabled();
+}
+
+std::string NidecBrushless::GetDescription() const {
+  return fmt::format("Nidec {}", GetChannel());
+}
+
+int NidecBrushless::GetChannel() const {
+  return m_pwm.GetChannel();
+}
+
+void NidecBrushless::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Nidec Brushless");
+  builder.SetActuator(true);
+  builder.SetSafeState([=] { StopMotor(); });
+  builder.AddDoubleProperty(
+      "Value", [=] { return Get(); }, [=](double value) { Set(value); });
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
new file mode 100644
index 0000000..9829a75
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+#include <fmt/format.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+using namespace frc;
+
+void PWMMotorController::Set(double speed) {
+  m_pwm.SetSpeed(m_isInverted ? -speed : speed);
+}
+
+double PWMMotorController::Get() const {
+  return m_pwm.GetSpeed() * (m_isInverted ? -1.0 : 1.0);
+}
+
+void PWMMotorController::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool PWMMotorController::GetInverted() const {
+  return m_isInverted;
+}
+
+void PWMMotorController::Disable() {
+  m_pwm.SetDisabled();
+}
+
+void PWMMotorController::StopMotor() {
+  Disable();
+}
+
+std::string PWMMotorController::GetDescription() const {
+  return fmt::format("PWM {}", GetChannel());
+}
+
+int PWMMotorController::GetChannel() const {
+  return m_pwm.GetChannel();
+}
+
+PWMMotorController::PWMMotorController(std::string_view name, int channel)
+    : m_pwm(channel, false) {
+  wpi::SendableRegistry::AddLW(this, name, channel);
+}
+
+void PWMMotorController::InitSendable(wpi::SendableBuilder& builder) {
+  builder.SetSmartDashboardType("Motor Controller");
+  builder.SetActuator(true);
+  builder.SetSafeState([=] { Disable(); });
+  builder.AddDoubleProperty(
+      "Value", [=] { return Get(); }, [=](double value) { Set(value); });
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkMax.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkMax.cpp
new file mode 100644
index 0000000..608d452
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMSparkMax.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/PWMSparkMax.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMSparkMax::PWMSparkMax(int channel)
+    : PWMMotorController("PWMSparkMax", channel) {
+  m_pwm.SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_RevSparkMaxPWM, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMTalonFX.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMTalonFX.cpp
new file mode 100644
index 0000000..2c6982b
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMTalonFX.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/PWMTalonFX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMTalonFX::PWMTalonFX(int channel)
+    : PWMMotorController("PWMTalonFX", channel) {
+  m_pwm.SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_TalonFX, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMTalonSRX.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMTalonSRX.cpp
new file mode 100644
index 0000000..b253412
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMTalonSRX.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/PWMTalonSRX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMTalonSRX::PWMTalonSRX(int channel)
+    : PWMMotorController("PWMTalonSRX", channel) {
+  m_pwm.SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_PWMTalonSRX, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMVenom.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMVenom.cpp
new file mode 100644
index 0000000..e558028
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMVenom.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/PWMVenom.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMVenom::PWMVenom(int channel) : PWMMotorController("PWMVenom", channel) {
+  m_pwm.SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_FusionVenom, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMVictorSPX.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMVictorSPX.cpp
new file mode 100644
index 0000000..10ce992
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/PWMVictorSPX.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/PWMVictorSPX.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+PWMVictorSPX::PWMVictorSPX(int channel)
+    : PWMMotorController("PWMVictorSPX", channel) {
+  m_pwm.SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_PWMVictorSPX, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/SD540.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/SD540.cpp
new file mode 100644
index 0000000..3d5738f
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/SD540.cpp
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/SD540.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+SD540::SD540(int channel) : PWMMotorController("SD540", channel) {
+  m_pwm.SetBounds(2.05, 1.55, 1.50, 1.44, 0.94);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_MindsensorsSD540,
+             GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Spark.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Spark.cpp
new file mode 100644
index 0000000..45394df
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Spark.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/Spark.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+Spark::Spark(int channel) : PWMMotorController("Spark", channel) {
+  m_pwm.SetBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_RevSPARK, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Talon.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Talon.cpp
new file mode 100644
index 0000000..f4b3b69
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Talon.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/Talon.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+Talon::Talon(int channel) : PWMMotorController("Talon", channel) {
+  m_pwm.SetBounds(2.037, 1.539, 1.513, 1.487, 0.989);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_Talon, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Victor.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Victor.cpp
new file mode 100644
index 0000000..3ad29f7
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/Victor.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/Victor.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+Victor::Victor(int channel) : PWMMotorController("Victor", channel) {
+  m_pwm.SetBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_2X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_Victor, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/VictorSP.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/VictorSP.cpp
new file mode 100644
index 0000000..6dc888e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/motorcontrol/VictorSP.cpp
@@ -0,0 +1,18 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/motorcontrol/VictorSP.h"
+
+#include <hal/FRCUsageReporting.h>
+
+using namespace frc;
+
+VictorSP::VictorSP(int channel) : PWMMotorController("VictorSP", channel) {
+  m_pwm.SetBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+  m_pwm.SetPeriodMultiplier(PWM::kPeriodMultiplier_1X);
+  m_pwm.SetSpeed(0.0);
+  m_pwm.SetZeroLatch();
+
+  HAL_Report(HALUsageReporting::kResourceType_VictorSP, GetChannel() + 1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp
index 19e17bb..eaac173 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ComplexWidget.cpp
@@ -1,42 +1,44 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ComplexWidget.h"
 
-#include "frc/smartdashboard/Sendable.h"
+#include <wpi/sendable/Sendable.h>
+
+#include "frc/smartdashboard/SendableBuilderImpl.h"
 
 using namespace frc;
 
 ComplexWidget::ComplexWidget(ShuffleboardContainer& parent,
-                             const wpi::Twine& title, Sendable& sendable)
+                             std::string_view title, wpi::Sendable& sendable)
     : ShuffleboardValue(title),
       ShuffleboardWidget(parent, title),
       m_sendable(sendable) {}
 
+ComplexWidget::~ComplexWidget() = default;
+
 void ComplexWidget::EnableIfActuator() {
-  if (m_builder.IsActuator()) {
-    m_builder.StartLiveWindowMode();
+  if (m_builder && static_cast<SendableBuilderImpl&>(*m_builder).IsActuator()) {
+    static_cast<SendableBuilderImpl&>(*m_builder).StartLiveWindowMode();
   }
 }
 
 void ComplexWidget::DisableIfActuator() {
-  if (m_builder.IsActuator()) {
-    m_builder.StopLiveWindowMode();
+  if (m_builder && static_cast<SendableBuilderImpl&>(*m_builder).IsActuator()) {
+    static_cast<SendableBuilderImpl&>(*m_builder).StopLiveWindowMode();
   }
 }
 
 void ComplexWidget::BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
                               std::shared_ptr<nt::NetworkTable> metaTable) {
   BuildMetadata(metaTable);
-  if (!m_builderInit) {
-    m_builder.SetTable(parentTable->GetSubTable(GetTitle()));
-    m_sendable.InitSendable(m_builder);
-    m_builder.StartListeners();
-    m_builderInit = true;
+  if (!m_builder) {
+    m_builder = std::make_unique<SendableBuilderImpl>();
+    static_cast<SendableBuilderImpl&>(*m_builder)
+        .SetTable(parentTable->GetSubTable(GetTitle()));
+    m_sendable.InitSendable(static_cast<SendableBuilderImpl&>(*m_builder));
+    static_cast<SendableBuilderImpl&>(*m_builder).StartListeners();
   }
-  m_builder.UpdateTable();
+  m_builder->Update();
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp
index a21cad9..9c47efb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/LayoutType.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/LayoutType.h"
 
 using namespace frc;
 
-wpi::StringRef LayoutType::GetLayoutName() const { return m_layoutName; }
+std::string_view LayoutType::GetLayoutName() const {
+  return m_layoutName;
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
index d80001e..834c4be 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/RecordingController.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/RecordingController.h"
 
-#include "frc/DriverStation.h"
+#include "frc/Errors.h"
 
 using namespace frc;
 using namespace frc::detail;
@@ -29,7 +26,7 @@
   m_recordingControlEntry.SetBoolean(false);
 }
 
-void RecordingController::SetRecordingFileNameFormat(wpi::StringRef format) {
+void RecordingController::SetRecordingFileNameFormat(std::string_view format) {
   m_recordingFileNameFormatEntry.SetString(format);
 }
 
@@ -38,12 +35,14 @@
 }
 
 void RecordingController::AddEventMarker(
-    wpi::StringRef name, wpi::StringRef description,
+    std::string_view name, std::string_view description,
     ShuffleboardEventImportance importance) {
   if (name.empty()) {
-    DriverStation::ReportError("Shuffleboard event name was not specified");
+    FRC_ReportError(err::Error, "{}",
+                    "Shuffleboard event name was not specified");
     return;
   }
   m_eventsTable->GetSubTable(name)->GetEntry("Info").SetStringArray(
-      {description, ShuffleboardEventImportanceName(importance)});
+      {std::string{description},
+       std::string{ShuffleboardEventImportanceName(importance)}});
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index 663cc19..1675c64 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/SendableCameraWrapper.h"
 
@@ -12,9 +9,8 @@
 #include <string>
 
 #include <wpi/DenseMap.h>
-
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
 namespace frc {
 namespace detail {
@@ -24,12 +20,12 @@
   return wrappers[static_cast<int>(source)];
 }
 
-void AddToSendableRegistry(frc::Sendable* sendable, std::string name) {
-  SendableRegistry::GetInstance().Add(sendable, name);
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string name) {
+  wpi::SendableRegistry::Add(sendable, name);
 }
 }  // namespace detail
 
-void SendableCameraWrapper::InitSendable(SendableBuilder& builder) {
+void SendableCameraWrapper::InitSendable(wpi::SendableBuilder& builder) {
   builder.AddStringProperty(
       ".ShuffleboardURI", [this] { return m_uri; }, nullptr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp
index 2d69847..16b404c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/Shuffleboard.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/Shuffleboard.h"
 
@@ -13,15 +10,19 @@
 
 using namespace frc;
 
-void Shuffleboard::Update() { GetInstance().Update(); }
+void Shuffleboard::Update() {
+  GetInstance().Update();
+}
 
-ShuffleboardTab& Shuffleboard::GetTab(wpi::StringRef title) {
+ShuffleboardTab& Shuffleboard::GetTab(std::string_view title) {
   return GetInstance().GetTab(title);
 }
 
-void Shuffleboard::SelectTab(int index) { GetInstance().SelectTab(index); }
+void Shuffleboard::SelectTab(int index) {
+  GetInstance().SelectTab(index);
+}
 
-void Shuffleboard::SelectTab(wpi::StringRef title) {
+void Shuffleboard::SelectTab(std::string_view title) {
   GetInstance().SelectTab(title);
 }
 
@@ -39,9 +40,11 @@
   GetRecordingController().StartRecording();
 }
 
-void Shuffleboard::StopRecording() { GetRecordingController().StopRecording(); }
+void Shuffleboard::StopRecording() {
+  GetRecordingController().StopRecording();
+}
 
-void Shuffleboard::SetRecordingFileNameFormat(wpi::StringRef format) {
+void Shuffleboard::SetRecordingFileNameFormat(std::string_view format) {
   GetRecordingController().SetRecordingFileNameFormat(format);
 }
 
@@ -49,13 +52,13 @@
   GetRecordingController().ClearRecordingFileNameFormat();
 }
 
-void Shuffleboard::AddEventMarker(wpi::StringRef name,
-                                  wpi::StringRef description,
+void Shuffleboard::AddEventMarker(std::string_view name,
+                                  std::string_view description,
                                   ShuffleboardEventImportance importance) {
   GetRecordingController().AddEventMarker(name, description, importance);
 }
 
-void Shuffleboard::AddEventMarker(wpi::StringRef name,
+void Shuffleboard::AddEventMarker(std::string_view name,
                                   ShuffleboardEventImportance importance) {
   AddEventMarker(name, "", importance);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp
index 7617333..940f6ab 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardComponentBase.cpp
@@ -1,27 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardComponentBase.h"
 
-#include <wpi/SmallVector.h>
-
 using namespace frc;
 
 ShuffleboardComponentBase::ShuffleboardComponentBase(
-    ShuffleboardContainer& parent, const wpi::Twine& title,
-    const wpi::Twine& type)
-    : ShuffleboardValue(title), m_parent(parent) {
-  wpi::SmallVector<char, 16> storage;
-  m_type = type.toStringRef(storage);
-}
+    ShuffleboardContainer& parent, std::string_view title,
+    std::string_view type)
+    : ShuffleboardValue(title), m_parent(parent), m_type(type) {}
 
-void ShuffleboardComponentBase::SetType(const wpi::Twine& type) {
-  wpi::SmallVector<char, 16> storage;
-  m_type = type.toStringRef(storage);
+void ShuffleboardComponentBase::SetType(std::string_view type) {
+  m_type = type;
   m_metadataDirty = true;
 }
 
@@ -68,7 +59,9 @@
   return m_parent;
 }
 
-const std::string& ShuffleboardComponentBase::GetType() const { return m_type; }
+const std::string& ShuffleboardComponentBase::GetType() const {
+  return m_type;
+}
 
 const wpi::StringMap<std::shared_ptr<nt::Value>>&
 ShuffleboardComponentBase::GetProperties() const {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
index d73f635..004f7c5 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardContainer.cpp
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardContainer.h"
 
-#include <wpi/SmallVector.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/sendable/SendableRegistry.h>
 
+#include "frc/Errors.h"
 #include "frc/shuffleboard/ComplexWidget.h"
 #include "frc/shuffleboard/ShuffleboardComponent.h"
 #include "frc/shuffleboard/ShuffleboardLayout.h"
 #include "frc/shuffleboard/SimpleWidget.h"
-#include "frc/smartdashboard/SendableRegistry.h"
 
 using namespace frc;
 
@@ -24,7 +20,7 @@
   return layoutStrings[static_cast<int>(layout)];
 }
 
-ShuffleboardContainer::ShuffleboardContainer(const wpi::Twine& title)
+ShuffleboardContainer::ShuffleboardContainer(std::string_view title)
     : ShuffleboardValue(title) {}
 
 const std::vector<std::unique_ptr<ShuffleboardComponentBase>>&
@@ -32,41 +28,37 @@
   return m_components;
 }
 
-ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title,
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(std::string_view title,
                                                      BuiltInLayouts type) {
   return GetLayout(title, GetStringFromBuiltInLayout(type));
 }
 
-ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title,
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(std::string_view title,
                                                      const LayoutType& type) {
   return GetLayout(title, type.GetLayoutName());
 }
 
-ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title,
-                                                     const wpi::Twine& type) {
-  wpi::SmallVector<char, 16> storage;
-  auto titleRef = title.toStringRef(storage);
-  if (m_layouts.count(titleRef) == 0) {
-    auto layout = std::make_unique<ShuffleboardLayout>(*this, titleRef, type);
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(std::string_view title,
+                                                     std::string_view type) {
+  if (m_layouts.count(title) == 0) {
+    auto layout = std::make_unique<ShuffleboardLayout>(*this, title, type);
     auto ptr = layout.get();
     m_components.emplace_back(std::move(layout));
-    m_layouts.insert(std::make_pair(titleRef, ptr));
+    m_layouts.insert(std::make_pair(title, ptr));
   }
-  return *m_layouts[titleRef];
+  return *m_layouts[title];
 }
 
-ShuffleboardLayout& ShuffleboardContainer::GetLayout(const wpi::Twine& title) {
-  wpi::SmallVector<char, 16> storage;
-  auto titleRef = title.toStringRef(storage);
-  if (m_layouts.count(titleRef) == 0) {
-    wpi_setWPIErrorWithContext(
-        InvalidParameter, "No layout with the given title has been defined");
+ShuffleboardLayout& ShuffleboardContainer::GetLayout(std::string_view title) {
+  if (m_layouts.count(title) == 0) {
+    throw FRC_MakeError(err::InvalidParameter,
+                        "No layout with title {} has been defined", title);
   }
-  return *m_layouts[titleRef];
+  return *m_layouts[title];
 }
 
-ComplexWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
-                                          Sendable& sendable) {
+ComplexWidget& ShuffleboardContainer::Add(std::string_view title,
+                                          wpi::Sendable& sendable) {
   CheckTitle(title);
   auto widget = std::make_unique<ComplexWidget>(*this, title, sendable);
   auto ptr = widget.get();
@@ -74,16 +66,16 @@
   return *ptr;
 }
 
-ComplexWidget& ShuffleboardContainer::Add(Sendable& sendable) {
-  auto name = SendableRegistry::GetInstance().GetName(&sendable);
+ComplexWidget& ShuffleboardContainer::Add(wpi::Sendable& sendable) {
+  auto name = wpi::SendableRegistry::GetName(&sendable);
   if (name.empty()) {
-    wpi::outs() << "Sendable must have a name\n";
+    FRC_ReportError(err::Error, "{}", "Sendable must have a name");
   }
   return Add(name, sendable);
 }
 
 SimpleWidget& ShuffleboardContainer::Add(
-    const wpi::Twine& title, std::shared_ptr<nt::Value> defaultValue) {
+    std::string_view title, std::shared_ptr<nt::Value> defaultValue) {
   CheckTitle(title);
 
   auto widget = std::make_unique<SimpleWidget>(*this, title);
@@ -93,48 +85,48 @@
   return *ptr;
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
                                          bool defaultValue) {
   return Add(title, nt::Value::MakeBoolean(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
                                          double defaultValue) {
   return Add(title, nt::Value::MakeDouble(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
                                          int defaultValue) {
   return Add(title, nt::Value::MakeDouble(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
-                                         const wpi::Twine& defaultValue) {
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
+                                         std::string_view defaultValue) {
   return Add(title, nt::Value::MakeString(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
                                          const char* defaultValue) {
   return Add(title, nt::Value::MakeString(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
-                                         wpi::ArrayRef<bool> defaultValue) {
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
+                                         wpi::span<const bool> defaultValue) {
   return Add(title, nt::Value::MakeBooleanArray(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::Add(const wpi::Twine& title,
-                                         wpi::ArrayRef<double> defaultValue) {
+SimpleWidget& ShuffleboardContainer::Add(std::string_view title,
+                                         wpi::span<const double> defaultValue) {
   return Add(title, nt::Value::MakeDoubleArray(defaultValue));
 }
 
 SimpleWidget& ShuffleboardContainer::Add(
-    const wpi::Twine& title, wpi::ArrayRef<std::string> defaultValue) {
+    std::string_view title, wpi::span<const std::string> defaultValue) {
   return Add(title, nt::Value::MakeStringArray(defaultValue));
 }
 
 SuppliedValueWidget<std::string>& ShuffleboardContainer::AddString(
-    const wpi::Twine& title, std::function<std::string()> supplier) {
+    std::string_view title, std::function<std::string()> supplier) {
   static auto setter = [](nt::NetworkTableEntry entry, std::string value) {
     entry.SetString(value);
   };
@@ -148,7 +140,7 @@
 }
 
 SuppliedValueWidget<double>& ShuffleboardContainer::AddNumber(
-    const wpi::Twine& title, std::function<double()> supplier) {
+    std::string_view title, std::function<double()> supplier) {
   static auto setter = [](nt::NetworkTableEntry entry, double value) {
     entry.SetDouble(value);
   };
@@ -162,7 +154,7 @@
 }
 
 SuppliedValueWidget<bool>& ShuffleboardContainer::AddBoolean(
-    const wpi::Twine& title, std::function<bool()> supplier) {
+    std::string_view title, std::function<bool()> supplier) {
   static auto setter = [](nt::NetworkTableEntry entry, bool value) {
     entry.SetBoolean(value);
   };
@@ -177,7 +169,7 @@
 
 SuppliedValueWidget<std::vector<std::string>>&
 ShuffleboardContainer::AddStringArray(
-    const wpi::Twine& title,
+    std::string_view title,
     std::function<std::vector<std::string>()> supplier) {
   static auto setter = [](nt::NetworkTableEntry entry,
                           std::vector<std::string> value) {
@@ -193,7 +185,7 @@
 }
 
 SuppliedValueWidget<std::vector<double>>& ShuffleboardContainer::AddNumberArray(
-    const wpi::Twine& title, std::function<std::vector<double>()> supplier) {
+    std::string_view title, std::function<std::vector<double>()> supplier) {
   static auto setter = [](nt::NetworkTableEntry entry,
                           std::vector<double> value) {
     entry.SetDoubleArray(value);
@@ -208,7 +200,7 @@
 }
 
 SuppliedValueWidget<std::vector<int>>& ShuffleboardContainer::AddBooleanArray(
-    const wpi::Twine& title, std::function<std::vector<int>()> supplier) {
+    std::string_view title, std::function<std::vector<int>()> supplier) {
   static auto setter = [](nt::NetworkTableEntry entry, std::vector<int> value) {
     entry.SetBooleanArray(value);
   };
@@ -221,14 +213,14 @@
   return *ptr;
 }
 
-SuppliedValueWidget<wpi::StringRef>& ShuffleboardContainer::AddRaw(
-    const wpi::Twine& title, std::function<wpi::StringRef()> supplier) {
-  static auto setter = [](nt::NetworkTableEntry entry, wpi::StringRef value) {
+SuppliedValueWidget<std::string_view>& ShuffleboardContainer::AddRaw(
+    std::string_view title, std::function<std::string_view()> supplier) {
+  static auto setter = [](nt::NetworkTableEntry entry, std::string_view value) {
     entry.SetRaw(value);
   };
 
   CheckTitle(title);
-  auto widget = std::make_unique<SuppliedValueWidget<wpi::StringRef>>(
+  auto widget = std::make_unique<SuppliedValueWidget<std::string_view>>(
       *this, title, supplier, setter);
   auto ptr = widget.get();
   m_components.emplace_back(std::move(widget));
@@ -236,44 +228,44 @@
 }
 
 SimpleWidget& ShuffleboardContainer::AddPersistent(
-    const wpi::Twine& title, std::shared_ptr<nt::Value> defaultValue) {
+    std::string_view title, std::shared_ptr<nt::Value> defaultValue) {
   auto& widget = Add(title, defaultValue);
   widget.GetEntry().SetPersistent();
   return widget;
 }
 
-SimpleWidget& ShuffleboardContainer::AddPersistent(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::AddPersistent(std::string_view title,
                                                    bool defaultValue) {
   return AddPersistent(title, nt::Value::MakeBoolean(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::AddPersistent(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::AddPersistent(std::string_view title,
                                                    double defaultValue) {
   return AddPersistent(title, nt::Value::MakeDouble(defaultValue));
 }
 
-SimpleWidget& ShuffleboardContainer::AddPersistent(const wpi::Twine& title,
+SimpleWidget& ShuffleboardContainer::AddPersistent(std::string_view title,
                                                    int defaultValue) {
   return AddPersistent(title, nt::Value::MakeDouble(defaultValue));
 }
 
 SimpleWidget& ShuffleboardContainer::AddPersistent(
-    const wpi::Twine& title, const wpi::Twine& defaultValue) {
+    std::string_view title, std::string_view defaultValue) {
   return AddPersistent(title, nt::Value::MakeString(defaultValue));
 }
 
 SimpleWidget& ShuffleboardContainer::AddPersistent(
-    const wpi::Twine& title, wpi::ArrayRef<bool> defaultValue) {
+    std::string_view title, wpi::span<const bool> defaultValue) {
   return AddPersistent(title, nt::Value::MakeBooleanArray(defaultValue));
 }
 
 SimpleWidget& ShuffleboardContainer::AddPersistent(
-    const wpi::Twine& title, wpi::ArrayRef<double> defaultValue) {
+    std::string_view title, wpi::span<const double> defaultValue) {
   return AddPersistent(title, nt::Value::MakeDoubleArray(defaultValue));
 }
 
 SimpleWidget& ShuffleboardContainer::AddPersistent(
-    const wpi::Twine& title, wpi::ArrayRef<std::string> defaultValue) {
+    std::string_view title, wpi::span<const std::string> defaultValue) {
   return AddPersistent(title, nt::Value::MakeStringArray(defaultValue));
 }
 
@@ -289,12 +281,11 @@
   }
 }
 
-void ShuffleboardContainer::CheckTitle(const wpi::Twine& title) {
-  wpi::SmallVector<char, 16> storage;
-  auto titleRef = title.toStringRef(storage);
-  if (m_usedTitles.count(titleRef) > 0) {
-    wpi::errs() << "Title is already in use: " << title << "\n";
+void ShuffleboardContainer::CheckTitle(std::string_view title) {
+  std::string titleStr{title};
+  if (m_usedTitles.count(titleStr) > 0) {
+    FRC_ReportError(err::Error, "Title is already in use: {}", title);
     return;
   }
-  m_usedTitles.insert(titleRef);
+  m_usedTitles.insert(titleStr);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
index 3316b3e..083b4a2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardInstance.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardInstance.h"
 
@@ -18,7 +15,7 @@
 using namespace frc::detail;
 
 struct ShuffleboardInstance::Impl {
-  wpi::StringMap<ShuffleboardTab> tabs;
+  wpi::StringMap<std::unique_ptr<ShuffleboardTab>> tabs;
 
   bool tabsChanged = false;
   std::shared_ptr<nt::NetworkTable> rootTable;
@@ -32,27 +29,28 @@
   HAL_Report(HALUsageReporting::kResourceType_Shuffleboard, 0);
 }
 
-ShuffleboardInstance::~ShuffleboardInstance() {}
+ShuffleboardInstance::~ShuffleboardInstance() = default;
 
-frc::ShuffleboardTab& ShuffleboardInstance::GetTab(wpi::StringRef title) {
+frc::ShuffleboardTab& ShuffleboardInstance::GetTab(std::string_view title) {
   if (m_impl->tabs.find(title) == m_impl->tabs.end()) {
-    m_impl->tabs.try_emplace(title, ShuffleboardTab(*this, title));
+    m_impl->tabs.try_emplace(title,
+                             std::make_unique<ShuffleboardTab>(*this, title));
     m_impl->tabsChanged = true;
   }
-  return m_impl->tabs.find(title)->second;
+  return *m_impl->tabs.find(title)->second;
 }
 
 void ShuffleboardInstance::Update() {
   if (m_impl->tabsChanged) {
     wpi::SmallVector<std::string, 16> tabTitles;
     for (auto& entry : m_impl->tabs) {
-      tabTitles.emplace_back(entry.second.GetTitle());
+      tabTitles.emplace_back(entry.second->GetTitle());
     }
     m_impl->rootMetaTable->GetEntry("Tabs").ForceSetStringArray(tabTitles);
     m_impl->tabsChanged = false;
   }
   for (auto& entry : m_impl->tabs) {
-    auto& tab = entry.second;
+    auto& tab = *entry.second;
     tab.BuildInto(m_impl->rootTable,
                   m_impl->rootMetaTable->GetSubTable(tab.GetTitle()));
   }
@@ -60,7 +58,7 @@
 
 void ShuffleboardInstance::EnableActuatorWidgets() {
   for (auto& entry : m_impl->tabs) {
-    auto& tab = entry.second;
+    auto& tab = *entry.second;
     for (auto& component : tab.GetComponents()) {
       component->EnableIfActuator();
     }
@@ -69,7 +67,7 @@
 
 void ShuffleboardInstance::DisableActuatorWidgets() {
   for (auto& entry : m_impl->tabs) {
-    auto& tab = entry.second;
+    auto& tab = *entry.second;
     for (auto& component : tab.GetComponents()) {
       component->DisableIfActuator();
     }
@@ -80,6 +78,6 @@
   m_impl->rootMetaTable->GetEntry("Selected").ForceSetDouble(index);
 }
 
-void ShuffleboardInstance::SelectTab(wpi::StringRef title) {
+void ShuffleboardInstance::SelectTab(std::string_view title) {
   m_impl->rootMetaTable->GetEntry("Selected").ForceSetString(title);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
index 1cbfb80..c4c933e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardLayout.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardLayout.h"
 
 using namespace frc;
 
 ShuffleboardLayout::ShuffleboardLayout(ShuffleboardContainer& parent,
-                                       const wpi::Twine& title,
-                                       const wpi::Twine& type)
+                                       std::string_view title,
+                                       std::string_view type)
     : ShuffleboardValue(title),
       ShuffleboardComponent(parent, title, type),
       ShuffleboardContainer(title) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp
index 7a8338e..61e0e45 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardTab.cpp
@@ -1,18 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardTab.h"
 
 using namespace frc;
 
-ShuffleboardTab::ShuffleboardTab(ShuffleboardRoot& root, wpi::StringRef title)
+ShuffleboardTab::ShuffleboardTab(ShuffleboardRoot& root, std::string_view title)
     : ShuffleboardValue(title), ShuffleboardContainer(title), m_root(root) {}
 
-ShuffleboardRoot& ShuffleboardTab::GetRoot() { return m_root; }
+ShuffleboardRoot& ShuffleboardTab::GetRoot() {
+  return m_root;
+}
 
 void ShuffleboardTab::BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
                                 std::shared_ptr<nt::NetworkTable> metaTable) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
index 3b79a9c..81f6671 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardWidget.h"
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp
index 89af25d..390c9c4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/SimpleWidget.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/SimpleWidget.h"
 
@@ -14,7 +11,7 @@
 using namespace frc;
 
 SimpleWidget::SimpleWidget(ShuffleboardContainer& parent,
-                           const wpi::Twine& title)
+                           std::string_view title)
     : ShuffleboardValue(title), ShuffleboardWidget(parent, title), m_entry() {}
 
 nt::NetworkTableEntry SimpleWidget::GetEntry() {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp
index cb73d30..3062cba 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/shuffleboard/WidgetType.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/WidgetType.h"
 
 using namespace frc;
 
-wpi::StringRef WidgetType::GetWidgetName() const { return m_widgetName; }
+std::string_view WidgetType::GetWidgetName() const {
+  return m_widgetName;
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp
new file mode 100644
index 0000000..27f9c1f
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADXL345Sim.h"
+
+#include "frc/ADXL345_I2C.h"
+#include "frc/ADXL345_SPI.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXL345Sim::ADXL345Sim(const frc::ADXL345_I2C& accel) {
+  frc::sim::SimDeviceSim deviceSim{"Accel:ADXL345_I2C", accel.GetI2CPort(),
+                                   accel.GetI2CDeviceAddress()};
+  m_simX = deviceSim.GetDouble("x");
+  m_simY = deviceSim.GetDouble("y");
+  m_simZ = deviceSim.GetDouble("z");
+}
+
+ADXL345Sim::ADXL345Sim(const frc::ADXL345_SPI& accel) {
+  frc::sim::SimDeviceSim deviceSim{"Accel:ADXL345_SPI", accel.GetSpiPort()};
+  m_simX = deviceSim.GetDouble("x");
+  m_simY = deviceSim.GetDouble("y");
+  m_simZ = deviceSim.GetDouble("z");
+}
+
+void ADXL345Sim::SetX(double accel) {
+  m_simX.Set(accel);
+}
+
+void ADXL345Sim::SetY(double accel) {
+  m_simY.Set(accel);
+}
+
+void ADXL345Sim::SetZ(double accel) {
+  m_simZ.Set(accel);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp
new file mode 100644
index 0000000..ac5bb5a
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADXL362Sim.h"
+
+#include "frc/ADXL362.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+ADXL362Sim::ADXL362Sim(const frc::ADXL362& accel) {
+  frc::sim::SimDeviceSim deviceSim{"Accel:ADXL362", accel.GetSpiPort()};
+  m_simX = deviceSim.GetDouble("x");
+  m_simY = deviceSim.GetDouble("y");
+  m_simZ = deviceSim.GetDouble("z");
+}
+
+void ADXL362Sim::SetX(double accel) {
+  m_simX.Set(accel);
+}
+
+void ADXL362Sim::SetY(double accel) {
+  m_simY.Set(accel);
+}
+
+void ADXL362Sim::SetZ(double accel) {
+  m_simZ.Set(accel);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
index 74a5180..a21d1b7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
@@ -1,33 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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");
+  frc::sim::SimDeviceSim deviceSim{"Gyro:ADXRS450", gyro.GetPort()};
+  m_simAngle = deviceSim.GetDouble("angle_x");
+  m_simRate = deviceSim.GetDouble("rate_x");
 }
 
 void ADXRS450_GyroSim::SetAngle(units::degree_t angle) {
-  m_simAngle.Set(angle.to<double>());
+  m_simAngle.Set(angle.value());
 }
 
 void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) {
-  m_simRate.Set(rate.to<double>());
+  m_simRate.Set(rate.value());
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
index 743c51d..8114bb3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AddressableLEDSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/AddressableLEDSim.h"
 
@@ -23,8 +20,9 @@
 
 AddressableLEDSim AddressableLEDSim::CreateForChannel(int pwmChannel) {
   int index = HALSIM_FindAddressableLEDForChannel(pwmChannel);
-  if (index < 0)
+  if (index < 0) {
     throw std::out_of_range("no addressable LED found for PWM channel");
+  }
   return AddressableLEDSim{index};
 }
 
@@ -101,7 +99,7 @@
 }
 
 std::unique_ptr<CallbackStore> AddressableLEDSim::RegisterDataCallback(
-    NotifyCallback callback, bool initialNotify) {
+    ConstBufferCallback callback, bool initialNotify) {
   auto store = std::make_unique<CallbackStore>(
       m_index, -1, callback, &HALSIM_CancelAddressableLEDDataCallback);
   store->SetUid(HALSIM_RegisterAddressableLEDDataCallback(
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
index 5fee737..049241e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogEncoderSim.cpp
@@ -1,25 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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()};
+  frc::sim::SimDeviceSim deviceSim{"AnalogEncoder", encoder.GetChannel()};
   m_positionSim = deviceSim.GetDouble("Position");
 }
 
@@ -28,7 +19,7 @@
 }
 
 void AnalogEncoderSim::SetTurns(units::turn_t turns) {
-  m_positionSim.Set(turns.to<double>());
+  m_positionSim.Set(turns.value());
 }
 
 units::turn_t AnalogEncoderSim::GetTurns() {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
index b7cab6b..625a3c0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogGyroSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/AnalogGyroSim.h"
 
@@ -74,4 +71,6 @@
   HALSIM_SetAnalogGyroInitialized(m_index, initialized);
 }
 
-void AnalogGyroSim::ResetData() { HALSIM_ResetAnalogGyroData(m_index); }
+void AnalogGyroSim::ResetData() {
+  HALSIM_ResetAnalogGyroData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
index 057a188..14a6596 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogInputSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/AnalogInputSim.h"
 
@@ -179,4 +176,6 @@
   HALSIM_SetAnalogInAccumulatorDeadband(m_index, accumulatorDeadband);
 }
 
-void AnalogInputSim::ResetData() { HALSIM_ResetAnalogInData(m_index); }
+void AnalogInputSim::ResetData() {
+  HALSIM_ResetAnalogInData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
index 4de9082..c2b01cf 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogOutputSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/AnalogOutputSim.h"
 
@@ -56,4 +53,6 @@
   HALSIM_SetAnalogOutInitialized(m_index, initialized);
 }
 
-void AnalogOutputSim::ResetData() { HALSIM_ResetAnalogOutData(m_index); }
+void AnalogOutputSim::ResetData() {
+  HALSIM_ResetAnalogOutData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
index 3325827..09e0a8d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/AnalogTriggerSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/AnalogTriggerSim.h"
 
@@ -23,7 +20,9 @@
 
 AnalogTriggerSim AnalogTriggerSim::CreateForChannel(int channel) {
   int index = HALSIM_FindAnalogTriggerForChannel(channel);
-  if (index < 0) throw std::out_of_range("no analog trigger found for channel");
+  if (index < 0) {
+    throw std::out_of_range("no analog trigger found for channel");
+  }
   return AnalogTriggerSim{index};
 }
 
@@ -86,4 +85,6 @@
   HALSIM_SetAnalogTriggerTriggerUpperBound(m_index, triggerUpperBound);
 }
 
-void AnalogTriggerSim::ResetData() { HALSIM_ResetAnalogTriggerData(m_index); }
+void AnalogTriggerSim::ResetData() {
+  HALSIM_ResetAnalogTriggerData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
index 601ee6e..0f6aa88 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/BuiltInAccelerometerSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/BuiltInAccelerometerSim.h"
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp
new file mode 100644
index 0000000..9c7450b
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CTREPCMSim.cpp
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/CTREPCMSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/CTREPCMData.h>
+
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+CTREPCMSim::CTREPCMSim() : m_index{SensorUtil::GetDefaultCTREPCMModule()} {}
+
+CTREPCMSim::CTREPCMSim(int module) : m_index{module} {}
+
+CTREPCMSim::CTREPCMSim(const PneumaticsBase& pneumatics)
+    : m_index{pneumatics.GetModuleNumber()} {}
+
+std::unique_ptr<CallbackStore> CTREPCMSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelCTREPCMInitializedCallback);
+  store->SetUid(HALSIM_RegisterCTREPCMInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool CTREPCMSim::GetInitialized() const {
+  return HALSIM_GetCTREPCMInitialized(m_index);
+}
+
+void CTREPCMSim::SetInitialized(bool solenoidInitialized) {
+  HALSIM_SetCTREPCMInitialized(m_index, solenoidInitialized);
+}
+
+std::unique_ptr<CallbackStore> CTREPCMSim::RegisterSolenoidOutputCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback,
+      &HALSIM_CancelCTREPCMSolenoidOutputCallback);
+  store->SetUid(HALSIM_RegisterCTREPCMSolenoidOutputCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool CTREPCMSim::GetSolenoidOutput(int channel) const {
+  return HALSIM_GetCTREPCMSolenoidOutput(m_index, channel);
+}
+
+void CTREPCMSim::SetSolenoidOutput(int channel, bool solenoidOutput) {
+  HALSIM_SetCTREPCMSolenoidOutput(m_index, channel, solenoidOutput);
+}
+
+std::unique_ptr<CallbackStore> CTREPCMSim::RegisterCompressorOnCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelCTREPCMCompressorOnCallback);
+  store->SetUid(HALSIM_RegisterCTREPCMCompressorOnCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool CTREPCMSim::GetCompressorOn() const {
+  return HALSIM_GetCTREPCMCompressorOn(m_index);
+}
+
+void CTREPCMSim::SetCompressorOn(bool compressorOn) {
+  HALSIM_SetCTREPCMCompressorOn(m_index, compressorOn);
+}
+
+std::unique_ptr<CallbackStore> CTREPCMSim::RegisterClosedLoopEnabledCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelCTREPCMClosedLoopEnabledCallback);
+  store->SetUid(HALSIM_RegisterCTREPCMClosedLoopEnabledCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool CTREPCMSim::GetClosedLoopEnabled() const {
+  return HALSIM_GetCTREPCMClosedLoopEnabled(m_index);
+}
+
+void CTREPCMSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
+  HALSIM_SetCTREPCMClosedLoopEnabled(m_index, closedLoopEnabled);
+}
+
+std::unique_ptr<CallbackStore> CTREPCMSim::RegisterPressureSwitchCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelCTREPCMPressureSwitchCallback);
+  store->SetUid(HALSIM_RegisterCTREPCMPressureSwitchCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool CTREPCMSim::GetPressureSwitch() const {
+  return HALSIM_GetCTREPCMPressureSwitch(m_index);
+}
+
+void CTREPCMSim::SetPressureSwitch(bool pressureSwitch) {
+  HALSIM_SetCTREPCMPressureSwitch(m_index, pressureSwitch);
+}
+
+std::unique_ptr<CallbackStore> CTREPCMSim::RegisterCompressorCurrentCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelCTREPCMCompressorCurrentCallback);
+  store->SetUid(HALSIM_RegisterCTREPCMCompressorCurrentCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double CTREPCMSim::GetCompressorCurrent() const {
+  return HALSIM_GetCTREPCMCompressorCurrent(m_index);
+}
+
+void CTREPCMSim::SetCompressorCurrent(double compressorCurrent) {
+  HALSIM_SetCTREPCMCompressorCurrent(m_index, compressorCurrent);
+}
+
+uint8_t CTREPCMSim::GetAllSolenoidOutputs() const {
+  uint8_t ret = 0;
+  HALSIM_GetCTREPCMAllSolenoids(m_index, &ret);
+  return ret;
+}
+
+void CTREPCMSim::SetAllSolenoidOutputs(uint8_t outputs) {
+  HALSIM_SetCTREPCMAllSolenoids(m_index, outputs);
+}
+
+void CTREPCMSim::ResetData() {
+  HALSIM_ResetCTREPCMData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
index 5c38372..be071e9 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/CallbackStore.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/CallbackStore.h"
 
+#include <utility>
+
 using namespace frc;
 using namespace frc::sim;
 
@@ -24,31 +23,35 @@
 
 CallbackStore::CallbackStore(int32_t i, NotifyCallback cb,
                              CancelCallbackNoIndexFunc ccf)
-    : index(i), callback(cb), cancelType(NoIndex) {
+    : index(i), callback(std::move(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) {
+    : index(i), uid(u), callback(std::move(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) {
+    : index(i),
+      channel(c),
+      uid(u),
+      callback(std::move(cb)),
+      cancelType(Channel) {
   this->cccf = ccf;
 }
 
 CallbackStore::CallbackStore(int32_t i, ConstBufferCallback cb,
                              CancelCallbackNoIndexFunc ccf)
-    : index(i), constBufferCallback(cb), cancelType(NoIndex) {
+    : index(i), constBufferCallback(std::move(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) {
+    : index(i), uid(u), constBufferCallback(std::move(cb)), cancelType(Normal) {
   this->ccf = ccf;
 }
 
@@ -58,7 +61,7 @@
     : index(i),
       channel(c),
       uid(u),
-      constBufferCallback(cb),
+      constBufferCallback(std::move(cb)),
       cancelType(Channel) {
   this->cccf = ccf;
 }
@@ -77,4 +80,6 @@
   }
 }
 
-void CallbackStore::SetUid(int32_t uid) { this->uid = uid; }
+void CallbackStore::SetUid(int32_t uid) {
+  this->uid = uid;
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
index 9330735..8b73b7c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DIOSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/DIOSim.h"
 
@@ -50,9 +47,13 @@
   return store;
 }
 
-bool DIOSim::GetValue() const { return HALSIM_GetDIOValue(m_index); }
+bool DIOSim::GetValue() const {
+  return HALSIM_GetDIOValue(m_index);
+}
 
-void DIOSim::SetValue(bool value) { HALSIM_SetDIOValue(m_index, value); }
+void DIOSim::SetValue(bool value) {
+  HALSIM_SetDIOValue(m_index, value);
+}
 
 std::unique_ptr<CallbackStore> DIOSim::RegisterPulseLengthCallback(
     NotifyCallback callback, bool initialNotify) {
@@ -80,7 +81,9 @@
   return store;
 }
 
-bool DIOSim::GetIsInput() const { return HALSIM_GetDIOIsInput(m_index); }
+bool DIOSim::GetIsInput() const {
+  return HALSIM_GetDIOIsInput(m_index);
+}
 
 void DIOSim::SetIsInput(bool isInput) {
   HALSIM_SetDIOIsInput(m_index, isInput);
@@ -95,10 +98,14 @@
   return store;
 }
 
-int DIOSim::GetFilterIndex() const { return HALSIM_GetDIOFilterIndex(m_index); }
+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); }
+void DIOSim::ResetData() {
+  HALSIM_ResetDIOData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 51ed94a..0bbf9c3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -1,44 +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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/DifferentialDrivetrainSim.h"
 
 #include <frc/system/plant/LinearSystemId.h>
 
-#include "frc/system/RungeKutta.h"
+#include <utility>
+
+#include <wpi/MathExtras.h>
+
+#include "frc/RobotController.h"
+#include "frc/system/NumericalIntegration.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),
+    LinearSystem<2, 2, 2> plant, units::meter_t trackWidth, DCMotor driveMotor,
+    double gearRatio, units::meter_t wheelRadius,
+    const std::array<double, 7>& measurementStdDevs)
+    : m_plant(std::move(plant)),
       m_rb(trackWidth / 2.0),
       m_wheelRadius(wheelRadius),
       m_motor(driveMotor),
       m_originalGearing(gearRatio),
-      m_currentGearing(gearRatio) {
+      m_currentGearing(gearRatio),
+      m_measurementStdDevs(measurementStdDevs) {
   m_x.setZero();
   m_u.setZero();
+  m_y.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)
+    units::meter_t trackWidth, const std::array<double, 7>& measurementStdDevs)
     : DifferentialDrivetrainSim(
           frc::LinearSystemId::DrivetrainVelocitySystem(
               driveMotor, mass, wheelRadius, trackWidth / 2.0, J, gearing),
-          trackWidth, driveMotor, gearing, wheelRadius) {}
+          trackWidth, driveMotor, gearing, wheelRadius, measurementStdDevs) {}
+
+Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
+    const Eigen::Vector<double, 2>& u) {
+  return frc::NormalizeInputVector<2>(u,
+                                      frc::RobotController::GetInputVoltage());
+}
 
 void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
                                           units::volt_t rightVoltage) {
-  m_u << leftVoltage.to<double>(), rightVoltage.to<double>();
+  m_u << leftVoltage.value(), rightVoltage.value();
+  m_u = ClampInput(m_u);
 }
 
 void DifferentialDrivetrainSim::SetGearing(double newGearing) {
@@ -46,65 +58,79 @@
 }
 
 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);
+  m_x = RK4([this](auto& x, auto& u) { return Dynamics(x, u); }, m_x, m_u, dt);
+  m_y = m_x + frc::MakeWhiteNoiseVector<7>(m_measurementStdDevs);
 }
 
 double DifferentialDrivetrainSim::GetGearing() const {
   return m_currentGearing;
 }
 
-Eigen::Matrix<double, 7, 1> DifferentialDrivetrainSim::GetState() const {
+Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetOutput() const {
+  return m_y;
+}
+
+Eigen::Vector<double, 7> DifferentialDrivetrainSim::GetState() const {
   return m_x;
 }
 
+double DifferentialDrivetrainSim::GetOutput(int output) const {
+  return m_y(output);
+}
+
+double DifferentialDrivetrainSim::GetState(int state) const {
+  return m_x(state);
+}
+
 Rotation2d DifferentialDrivetrainSim::GetHeading() const {
-  return Rotation2d(units::radian_t(m_x(State::kHeading)));
+  return Rotation2d(units::radian_t(GetOutput(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))));
+  return Pose2d(units::meter_t(GetOutput(State::kX)),
+                units::meter_t(GetOutput(State::kY)), GetHeading());
 }
 
-units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
+units::ampere_t DifferentialDrivetrainSim::GetLeftCurrentDraw() 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))) *
+      m_motor.Current(
+          units::radians_per_second_t(m_x(State::kLeftVelocity) *
+                                      m_currentGearing / m_wheelRadius.value()),
+          units::volt_t(m_u(0))) *
       wpi::sgn(m_u(0));
 
+  return loadIleft;
+}
+
+units::ampere_t DifferentialDrivetrainSim::GetRightCurrentDraw() const {
   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))) *
+      m_motor.Current(
+          units::radians_per_second_t(m_x(State::kRightVelocity) *
+                                      m_currentGearing / m_wheelRadius.value()),
+          units::volt_t(m_u(1))) *
       wpi::sgn(m_u(1));
 
-  return loadIleft + loadIRight;
+  return loadIRight;
+}
+units::ampere_t DifferentialDrivetrainSim::GetCurrentDraw() const {
+  return GetLeftCurrentDraw() + GetRightCurrentDraw();
 }
 
 void DifferentialDrivetrainSim::SetState(
-    const Eigen::Matrix<double, 7, 1>& state) {
+    const Eigen::Vector<double, 7>& 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::kX) = pose.X().value();
+  m_x(State::kY) = pose.Y().value();
+  m_x(State::kHeading) = pose.Rotation().Radians().value();
   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) {
+Eigen::Vector<double, 7> DifferentialDrivetrainSim::Dynamics(
+    const Eigen::Vector<double, 7>& x, const Eigen::Vector<double, 2>& 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.
@@ -123,12 +149,12 @@
 
   double v = (x(State::kLeftVelocity) + x(State::kRightVelocity)) / 2.0;
 
-  Eigen::Matrix<double, 7, 1> xdot;
+  Eigen::Vector<double, 7> 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>();
+          .value();
   xdot.block<4, 1>(3, 0) = A * x.block<4, 1>(3, 0) + B * u;
   return xdot;
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
index 2ee55ce..eb1e02e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DigitalPWMSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/DigitalPWMSim.h"
 
@@ -23,7 +20,9 @@
 
 DigitalPWMSim DigitalPWMSim::CreateForChannel(int channel) {
   int index = HALSIM_FindDigitalPWMForChannel(channel);
-  if (index < 0) throw std::out_of_range("no digital PWM found for channel");
+  if (index < 0) {
+    throw std::out_of_range("no digital PWM found for channel");
+  }
   return DigitalPWMSim{index};
 }
 
@@ -74,8 +73,14 @@
   return store;
 }
 
-int DigitalPWMSim::GetPin() const { return HALSIM_GetDigitalPWMPin(m_index); }
+int DigitalPWMSim::GetPin() const {
+  return HALSIM_GetDigitalPWMPin(m_index);
+}
 
-void DigitalPWMSim::SetPin(int pin) { HALSIM_SetDigitalPWMPin(m_index, pin); }
+void DigitalPWMSim::SetPin(int pin) {
+  HALSIM_SetDigitalPWMPin(m_index, pin);
+}
 
-void DigitalPWMSim::ResetData() { HALSIM_ResetDigitalPWMData(m_index); }
+void DigitalPWMSim::ResetData() {
+  HALSIM_ResetDigitalPWMData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
index 8c0bf70..5f2c645 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DriverStationSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/DriverStationSim.h"
 
@@ -27,7 +24,9 @@
   return store;
 }
 
-bool DriverStationSim::GetEnabled() { return HALSIM_GetDriverStationEnabled(); }
+bool DriverStationSim::GetEnabled() {
+  return HALSIM_GetDriverStationEnabled();
+}
 
 void DriverStationSim::SetEnabled(bool enabled) {
   HALSIM_SetDriverStationEnabled(enabled);
@@ -59,9 +58,13 @@
   return store;
 }
 
-bool DriverStationSim::GetTest() { return HALSIM_GetDriverStationTest(); }
+bool DriverStationSim::GetTest() {
+  return HALSIM_GetDriverStationTest();
+}
 
-void DriverStationSim::SetTest(bool test) { HALSIM_SetDriverStationTest(test); }
+void DriverStationSim::SetTest(bool test) {
+  HALSIM_SetDriverStationTest(test);
+}
 
 std::unique_ptr<CallbackStore> DriverStationSim::RegisterEStopCallback(
     NotifyCallback callback, bool initialNotify) {
@@ -72,7 +75,9 @@
   return store;
 }
 
-bool DriverStationSim::GetEStop() { return HALSIM_GetDriverStationEStop(); }
+bool DriverStationSim::GetEStop() {
+  return HALSIM_GetDriverStationEStop();
+}
 
 void DriverStationSim::SetEStop(bool eStop) {
   HALSIM_SetDriverStationEStop(eStop);
@@ -150,7 +155,7 @@
 
 void DriverStationSim::NotifyNewData() {
   HALSIM_NotifyDriverStationNewData();
-  DriverStation::GetInstance().WaitForData();
+  DriverStation::WaitForData();
 }
 
 void DriverStationSim::SetSendError(bool shouldSend) {
@@ -252,4 +257,6 @@
   HALSIM_SetReplayNumber(replayNumber);
 }
 
-void DriverStationSim::ResetData() { HALSIM_ResetDriverStationData(); }
+void DriverStationSim::ResetData() {
+  HALSIM_ResetDriverStationData();
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
index b12f00a..cb83ccb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleEncoderSim.cpp
@@ -1,31 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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");
+  frc::sim::SimDeviceSim deviceSim{"DutyCycle:DutyCycleEncoder",
+                                   encoder.GetSourceChannel()};
+  m_simPosition = deviceSim.GetDouble("position");
+  m_simDistancePerRotation = deviceSim.GetDouble("distance_per_rot");
 }
 
 void DutyCycleEncoderSim::Set(units::turn_t turns) {
-  m_simPosition.Set(turns.to<double>());
+  m_simPosition.Set(turns.value());
 }
 
 void DutyCycleEncoderSim::SetDistance(double distance) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
index 7ec19fd..29f2df4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/DutyCycleSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/DutyCycleSim.h"
 
@@ -23,7 +20,9 @@
 
 DutyCycleSim DutyCycleSim::CreateForChannel(int channel) {
   int index = HALSIM_FindDutyCycleForChannel(channel);
-  if (index < 0) throw std::out_of_range("no duty cycle found for channel");
+  if (index < 0) {
+    throw std::out_of_range("no duty cycle found for channel");
+  }
   return DutyCycleSim{index};
 }
 
@@ -61,8 +60,8 @@
   return HALSIM_GetDutyCycleFrequency(m_index);
 }
 
-void DutyCycleSim::SetFrequency(int count) {
-  HALSIM_SetDutyCycleFrequency(m_index, count);
+void DutyCycleSim::SetFrequency(int frequency) {
+  HALSIM_SetDutyCycleFrequency(m_index, frequency);
 }
 
 std::unique_ptr<CallbackStore> DutyCycleSim::RegisterOutputCallback(
@@ -78,8 +77,10 @@
   return HALSIM_GetDutyCycleOutput(m_index);
 }
 
-void DutyCycleSim::SetOutput(double period) {
-  HALSIM_SetDutyCycleOutput(m_index, period);
+void DutyCycleSim::SetOutput(double output) {
+  HALSIM_SetDutyCycleOutput(m_index, output);
 }
 
-void DutyCycleSim::ResetData() { HALSIM_ResetDutyCycleData(m_index); }
+void DutyCycleSim::ResetData() {
+  HALSIM_ResetDutyCycleData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
index 89c09cb..a2d5c66 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/ElevatorSim.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/ElevatorSim.h"
 
 #include <wpi/MathExtras.h>
 
-#include "frc/StateSpaceUtil.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 
 using namespace frc;
@@ -42,16 +38,24 @@
       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::WouldHitLowerLimit(units::meter_t elevatorHeight) const {
+  return elevatorHeight < m_minHeight;
 }
 
-bool ElevatorSim::HasHitUpperLimit(const Eigen::Matrix<double, 2, 1>& x) const {
-  return x(0) > m_maxHeight.to<double>();
+bool ElevatorSim::WouldHitUpperLimit(units::meter_t elevatorHeight) const {
+  return elevatorHeight > m_maxHeight;
+}
+
+bool ElevatorSim::HasHitLowerLimit() const {
+  return WouldHitLowerLimit(units::meter_t(m_y(0)));
+}
+
+bool ElevatorSim::HasHitUpperLimit() const {
+  return WouldHitUpperLimit(units::meter_t(m_y(0)));
 }
 
 units::meter_t ElevatorSim::GetPosition() const {
-  return units::meter_t{m_x(0)};
+  return units::meter_t{m_y(0)};
 }
 
 units::meters_per_second_t ElevatorSim::GetVelocity() const {
@@ -74,25 +78,25 @@
 }
 
 void ElevatorSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+  SetInput(Eigen::Vector<double, 1>{voltage.value()});
 }
 
-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);
+Eigen::Vector<double, 2> ElevatorSim::UpdateX(
+    const Eigen::Vector<double, 2>& currentXhat,
+    const Eigen::Vector<double, 1>& u, units::second_t dt) {
+  auto updatedXhat = RKDP(
+      [&](const Eigen::Vector<double, 2>& x,
+          const Eigen::Vector<double, 1>& u_) -> Eigen::Vector<double, 2> {
+        return m_plant.A() * x + m_plant.B() * u_ +
+               Eigen::Vector<double, 2>{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 (WouldHitLowerLimit(units::meter_t(updatedXhat(0)))) {
+    return Eigen::Vector<double, 2>{m_minHeight.value(), 0.0};
   }
-  if (HasHitUpperLimit(updatedXhat)) {
-    return MakeMatrix<2, 1>(m_maxHeight.to<double>(), 0.0);
+  if (WouldHitUpperLimit(units::meter_t(updatedXhat(0)))) {
+    return Eigen::Vector<double, 2>{m_maxHeight.value(), 0.0};
   }
   return updatedXhat;
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
index 5bc5d52..146328d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/EncoderSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/EncoderSim.h"
 
@@ -23,11 +20,15 @@
 
 EncoderSim EncoderSim::CreateForChannel(int channel) {
   int index = HALSIM_FindEncoderForChannel(channel);
-  if (index < 0) throw std::out_of_range("no encoder found for 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}; }
+EncoderSim EncoderSim::CreateForIndex(int index) {
+  return EncoderSim{index};
+}
 
 std::unique_ptr<CallbackStore> EncoderSim::RegisterInitializedCallback(
     NotifyCallback callback, bool initialNotify) {
@@ -55,9 +56,13 @@
   return store;
 }
 
-int EncoderSim::GetCount() const { return HALSIM_GetEncoderCount(m_index); }
+int EncoderSim::GetCount() const {
+  return HALSIM_GetEncoderCount(m_index);
+}
 
-void EncoderSim::SetCount(int count) { HALSIM_SetEncoderCount(m_index, count); }
+void EncoderSim::SetCount(int count) {
+  HALSIM_SetEncoderCount(m_index, count);
+}
 
 std::unique_ptr<CallbackStore> EncoderSim::RegisterPeriodCallback(
     NotifyCallback callback, bool initialNotify) {
@@ -85,7 +90,9 @@
   return store;
 }
 
-bool EncoderSim::GetReset() const { return HALSIM_GetEncoderReset(m_index); }
+bool EncoderSim::GetReset() const {
+  return HALSIM_GetEncoderReset(m_index);
+}
 
 void EncoderSim::SetReset(bool reset) {
   HALSIM_SetEncoderReset(m_index, reset);
@@ -176,14 +183,22 @@
   HALSIM_SetEncoderDistancePerPulse(m_index, distancePerPulse);
 }
 
-void EncoderSim::ResetData() { HALSIM_ResetEncoderData(m_index); }
+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); }
+double EncoderSim::GetDistance() {
+  return HALSIM_GetEncoderDistance(m_index);
+}
 
-void EncoderSim::SetRate(double rate) { HALSIM_SetEncoderRate(m_index, rate); }
+void EncoderSim::SetRate(double rate) {
+  HALSIM_SetEncoderRate(m_index, rate);
+}
 
-double EncoderSim::GetRate() { return HALSIM_GetEncoderRate(m_index); }
+double EncoderSim::GetRate() {
+  return HALSIM_GetEncoderRate(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/Field2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/Field2d.cpp
deleted file mode 100644
index a6098b6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/Field2d.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
index 65fc3c8..f4371b1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/FlywheelSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/FlywheelSim.h"
 
@@ -41,5 +38,5 @@
 }
 
 void FlywheelSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+  SetInput(Eigen::Vector<double, 1>{voltage.value()});
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
index 8df265a..f4981e9 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/GenericHIDSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/GenericHIDSim.h"
 
@@ -18,7 +15,9 @@
 
 GenericHIDSim::GenericHIDSim(int port) : m_port{port} {}
 
-void GenericHIDSim::NotifyNewData() { DriverStationSim::NotifyNewData(); }
+void GenericHIDSim::NotifyNewData() {
+  DriverStationSim::NotifyNewData();
+}
 
 void GenericHIDSim::SetRawButton(int button, bool value) {
   DriverStationSim::SetJoystickButton(m_port, button, value);
@@ -32,7 +31,9 @@
   DriverStationSim::SetJoystickPOV(m_port, pov, value);
 }
 
-void GenericHIDSim::SetPOV(int value) { SetPOV(0, value); }
+void GenericHIDSim::SetPOV(int value) {
+  SetPOV(0, value);
+}
 
 void GenericHIDSim::SetAxisCount(int count) {
   DriverStationSim::SetJoystickAxisCount(m_port, count);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
index 55f6089..d103822 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/JoystickSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/JoystickSim.h"
 
@@ -58,6 +55,10 @@
              value);
 }
 
-void JoystickSim::SetTrigger(bool state) { SetRawButton(1, state); }
+void JoystickSim::SetTrigger(bool state) {
+  SetRawButton(1, state);
+}
 
-void JoystickSim::SetTop(bool state) { SetRawButton(2, state); }
+void JoystickSim::SetTop(bool state) {
+  SetRawButton(2, state);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp
deleted file mode 100644
index 43178b1..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/Mechanism2D.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp
deleted file mode 100644
index 5d0566a..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PCMSim.cpp
+++ /dev/null
@@ -1,158 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp
deleted file mode 100644
index 26d6a45..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PDPSim.cpp
+++ /dev/null
@@ -1,98 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PS4ControllerSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PS4ControllerSim.cpp
new file mode 100644
index 0000000..3403755
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PS4ControllerSim.cpp
@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/PS4ControllerSim.h"
+
+#include "frc/PS4Controller.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PS4ControllerSim::PS4ControllerSim(const PS4Controller& joystick)
+    : GenericHIDSim{joystick} {
+  SetAxisCount(6);
+  SetButtonCount(14);
+}
+
+PS4ControllerSim::PS4ControllerSim(int port) : GenericHIDSim{port} {
+  SetAxisCount(6);
+  SetButtonCount(14);
+}
+
+void PS4ControllerSim::SetLeftX(double value) {
+  SetRawAxis(PS4Controller::Axis::kLeftX, value);
+}
+
+void PS4ControllerSim::SetRightX(double value) {
+  SetRawAxis(PS4Controller::Axis::kRightX, value);
+}
+
+void PS4ControllerSim::SetLeftY(double value) {
+  SetRawAxis(PS4Controller::Axis::kLeftY, value);
+}
+
+void PS4ControllerSim::SetRightY(double value) {
+  SetRawAxis(PS4Controller::Axis::kRightY, value);
+}
+
+void PS4ControllerSim::SetL2Axis(double value) {
+  SetRawAxis(PS4Controller::Axis::kL2, value);
+}
+
+void PS4ControllerSim::SetR2Axis(double value) {
+  SetRawAxis(PS4Controller::Axis::kR2, value);
+}
+
+void PS4ControllerSim::SetSquareButton(bool value) {
+  SetRawButton(PS4Controller::Button::kSquare, value);
+}
+
+void PS4ControllerSim::SetCrossButton(bool value) {
+  SetRawButton(PS4Controller::Button::kCross, value);
+}
+
+void PS4ControllerSim::SetCircleButton(bool value) {
+  SetRawButton(PS4Controller::Button::kCircle, value);
+}
+
+void PS4ControllerSim::SetTriangleButton(bool value) {
+  SetRawButton(PS4Controller::Button::kTriangle, value);
+}
+
+void PS4ControllerSim::SetL1Button(bool value) {
+  SetRawButton(PS4Controller::Button::kL1, value);
+}
+
+void PS4ControllerSim::SetR1Button(bool value) {
+  SetRawButton(PS4Controller::Button::kR1, value);
+}
+
+void PS4ControllerSim::SetL2Button(bool value) {
+  SetRawButton(PS4Controller::Button::kL2, value);
+}
+
+void PS4ControllerSim::SetR2Button(bool value) {
+  SetRawButton(PS4Controller::Button::kR2, value);
+}
+
+void PS4ControllerSim::SetShareButton(bool value) {
+  SetRawButton(PS4Controller::Button::kShare, value);
+}
+
+void PS4ControllerSim::SetOptionsButton(bool value) {
+  SetRawButton(PS4Controller::Button::kOptions, value);
+}
+
+void PS4ControllerSim::SetL3Button(bool value) {
+  SetRawButton(PS4Controller::Button::kL3, value);
+}
+
+void PS4ControllerSim::SetR3Button(bool value) {
+  SetRawButton(PS4Controller::Button::kR3, value);
+}
+
+void PS4ControllerSim::SetPSButton(bool value) {
+  SetRawButton(PS4Controller::Button::kPS, value);
+}
+
+void PS4ControllerSim::SetTouchpad(bool value) {
+  SetRawButton(PS4Controller::Button::kTouchpad, value);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
index 2fae8fb..f5d69db 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PWMSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/PWMSim.h"
 
@@ -47,7 +44,9 @@
   return store;
 }
 
-int PWMSim::GetRawValue() const { return HALSIM_GetPWMRawValue(m_index); }
+int PWMSim::GetRawValue() const {
+  return HALSIM_GetPWMRawValue(m_index);
+}
 
 void PWMSim::SetRawValue(int rawValue) {
   HALSIM_SetPWMRawValue(m_index, rawValue);
@@ -62,9 +61,13 @@
   return store;
 }
 
-double PWMSim::GetSpeed() const { return HALSIM_GetPWMSpeed(m_index); }
+double PWMSim::GetSpeed() const {
+  return HALSIM_GetPWMSpeed(m_index);
+}
 
-void PWMSim::SetSpeed(double speed) { HALSIM_SetPWMSpeed(m_index, speed); }
+void PWMSim::SetSpeed(double speed) {
+  HALSIM_SetPWMSpeed(m_index, speed);
+}
 
 std::unique_ptr<CallbackStore> PWMSim::RegisterPositionCallback(
     NotifyCallback callback, bool initialNotify) {
@@ -75,7 +78,9 @@
   return store;
 }
 
-double PWMSim::GetPosition() const { return HALSIM_GetPWMPosition(m_index); }
+double PWMSim::GetPosition() const {
+  return HALSIM_GetPWMPosition(m_index);
+}
 
 void PWMSim::SetPosition(double position) {
   HALSIM_SetPWMPosition(m_index, position);
@@ -90,7 +95,9 @@
   return store;
 }
 
-int PWMSim::GetPeriodScale() const { return HALSIM_GetPWMPeriodScale(m_index); }
+int PWMSim::GetPeriodScale() const {
+  return HALSIM_GetPWMPeriodScale(m_index);
+}
 
 void PWMSim::SetPeriodScale(int periodScale) {
   HALSIM_SetPWMPeriodScale(m_index, periodScale);
@@ -105,10 +112,14 @@
   return store;
 }
 
-bool PWMSim::GetZeroLatch() const { return HALSIM_GetPWMZeroLatch(m_index); }
+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); }
+void PWMSim::ResetData() {
+  HALSIM_ResetPWMData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp
new file mode 100644
index 0000000..844a1b6
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/PowerDistributionSim.cpp
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/PowerDistributionSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/PowerDistributionData.h>
+
+#include "frc/PowerDistribution.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+PowerDistributionSim::PowerDistributionSim(int module) : m_index{module} {}
+
+PowerDistributionSim::PowerDistributionSim(const PowerDistribution& pdp)
+    : m_index{pdp.GetModule()} {}
+
+std::unique_ptr<CallbackStore>
+PowerDistributionSim::RegisterInitializedCallback(NotifyCallback callback,
+                                                  bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelPowerDistributionInitializedCallback);
+  store->SetUid(HALSIM_RegisterPowerDistributionInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool PowerDistributionSim::GetInitialized() const {
+  return HALSIM_GetPowerDistributionInitialized(m_index);
+}
+
+void PowerDistributionSim::SetInitialized(bool initialized) {
+  HALSIM_SetPowerDistributionInitialized(m_index, initialized);
+}
+
+std::unique_ptr<CallbackStore>
+PowerDistributionSim::RegisterTemperatureCallback(NotifyCallback callback,
+                                                  bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback,
+      &HALSIM_CancelPowerDistributionTemperatureCallback);
+  store->SetUid(HALSIM_RegisterPowerDistributionTemperatureCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PowerDistributionSim::GetTemperature() const {
+  return HALSIM_GetPowerDistributionTemperature(m_index);
+}
+
+void PowerDistributionSim::SetTemperature(double temperature) {
+  HALSIM_SetPowerDistributionTemperature(m_index, temperature);
+}
+
+std::unique_ptr<CallbackStore> PowerDistributionSim::RegisterVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelPowerDistributionVoltageCallback);
+  store->SetUid(HALSIM_RegisterPowerDistributionVoltageCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PowerDistributionSim::GetVoltage() const {
+  return HALSIM_GetPowerDistributionVoltage(m_index);
+}
+
+void PowerDistributionSim::SetVoltage(double voltage) {
+  HALSIM_SetPowerDistributionVoltage(m_index, voltage);
+}
+
+std::unique_ptr<CallbackStore> PowerDistributionSim::RegisterCurrentCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback,
+      &HALSIM_CancelPowerDistributionCurrentCallback);
+  store->SetUid(HALSIM_RegisterPowerDistributionCurrentCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double PowerDistributionSim::GetCurrent(int channel) const {
+  return HALSIM_GetPowerDistributionCurrent(m_index, channel);
+}
+
+void PowerDistributionSim::SetCurrent(int channel, double current) {
+  HALSIM_SetPowerDistributionCurrent(m_index, channel, current);
+}
+
+void PowerDistributionSim::GetAllCurrents(double* currents, int length) const {
+  HALSIM_GetPowerDistributionAllCurrents(m_index, currents, length);
+}
+
+void PowerDistributionSim::SetAllCurrents(const double* currents, int length) {
+  HALSIM_SetPowerDistributionAllCurrents(m_index, currents, length);
+}
+
+void PowerDistributionSim::ResetData() {
+  HALSIM_ResetPowerDistributionData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
new file mode 100644
index 0000000..dd6dba8
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/REVPHSim.h"
+
+#include <memory>
+#include <utility>
+
+#include <hal/simulation/REVPHData.h>
+
+#include "frc/SensorUtil.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+REVPHSim::REVPHSim() : m_index{SensorUtil::GetDefaultREVPHModule()} {}
+
+REVPHSim::REVPHSim(int module) : m_index{module} {}
+
+REVPHSim::REVPHSim(const PneumaticsBase& pneumatics)
+    : m_index{pneumatics.GetModuleNumber()} {}
+
+std::unique_ptr<CallbackStore> REVPHSim::RegisterInitializedCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelREVPHInitializedCallback);
+  store->SetUid(HALSIM_RegisterREVPHInitializedCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool REVPHSim::GetInitialized() const {
+  return HALSIM_GetREVPHInitialized(m_index);
+}
+
+void REVPHSim::SetInitialized(bool solenoidInitialized) {
+  HALSIM_SetREVPHInitialized(m_index, solenoidInitialized);
+}
+
+std::unique_ptr<CallbackStore> REVPHSim::RegisterSolenoidOutputCallback(
+    int channel, NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, channel, -1, callback,
+      &HALSIM_CancelREVPHSolenoidOutputCallback);
+  store->SetUid(HALSIM_RegisterREVPHSolenoidOutputCallback(
+      m_index, channel, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool REVPHSim::GetSolenoidOutput(int channel) const {
+  return HALSIM_GetREVPHSolenoidOutput(m_index, channel);
+}
+
+void REVPHSim::SetSolenoidOutput(int channel, bool solenoidOutput) {
+  HALSIM_SetREVPHSolenoidOutput(m_index, channel, solenoidOutput);
+}
+
+std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorOnCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelREVPHCompressorOnCallback);
+  store->SetUid(HALSIM_RegisterREVPHCompressorOnCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool REVPHSim::GetCompressorOn() const {
+  return HALSIM_GetREVPHCompressorOn(m_index);
+}
+
+void REVPHSim::SetCompressorOn(bool compressorOn) {
+  HALSIM_SetREVPHCompressorOn(m_index, compressorOn);
+}
+
+std::unique_ptr<CallbackStore> REVPHSim::RegisterClosedLoopEnabledCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelREVPHClosedLoopEnabledCallback);
+  store->SetUid(HALSIM_RegisterREVPHClosedLoopEnabledCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool REVPHSim::GetClosedLoopEnabled() const {
+  return HALSIM_GetREVPHClosedLoopEnabled(m_index);
+}
+
+void REVPHSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
+  HALSIM_SetREVPHClosedLoopEnabled(m_index, closedLoopEnabled);
+}
+
+std::unique_ptr<CallbackStore> REVPHSim::RegisterPressureSwitchCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelREVPHPressureSwitchCallback);
+  store->SetUid(HALSIM_RegisterREVPHPressureSwitchCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+bool REVPHSim::GetPressureSwitch() const {
+  return HALSIM_GetREVPHPressureSwitch(m_index);
+}
+
+void REVPHSim::SetPressureSwitch(bool pressureSwitch) {
+  HALSIM_SetREVPHPressureSwitch(m_index, pressureSwitch);
+}
+
+std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorCurrentCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      m_index, -1, callback, &HALSIM_CancelREVPHCompressorCurrentCallback);
+  store->SetUid(HALSIM_RegisterREVPHCompressorCurrentCallback(
+      m_index, &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+double REVPHSim::GetCompressorCurrent() const {
+  return HALSIM_GetREVPHCompressorCurrent(m_index);
+}
+
+void REVPHSim::SetCompressorCurrent(double compressorCurrent) {
+  HALSIM_SetREVPHCompressorCurrent(m_index, compressorCurrent);
+}
+
+uint8_t REVPHSim::GetAllSolenoidOutputs() const {
+  uint8_t ret = 0;
+  HALSIM_GetREVPHAllSolenoids(m_index, &ret);
+  return ret;
+}
+
+void REVPHSim::SetAllSolenoidOutputs(uint8_t outputs) {
+  HALSIM_SetREVPHAllSolenoids(m_index, outputs);
+}
+
+void REVPHSim::ResetData() {
+  HALSIM_ResetREVPHData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
index 7f7aa65..84ce120 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RelaySim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/RelaySim.h"
 
@@ -64,7 +61,9 @@
   return store;
 }
 
-bool RelaySim::GetForward() const { return HALSIM_GetRelayForward(m_index); }
+bool RelaySim::GetForward() const {
+  return HALSIM_GetRelayForward(m_index);
+}
 
 void RelaySim::SetForward(bool forward) {
   HALSIM_SetRelayForward(m_index, forward);
@@ -79,10 +78,14 @@
   return store;
 }
 
-bool RelaySim::GetReverse() const { return HALSIM_GetRelayReverse(m_index); }
+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); }
+void RelaySim::ResetData() {
+  HALSIM_ResetRelayData(m_index);
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
index 0d1d104..dc2b557 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/RoboRioSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/RoboRioSim.h"
 
@@ -24,7 +21,9 @@
   return store;
 }
 
-bool RoboRioSim::GetFPGAButton() { return HALSIM_GetRoboRioFPGAButton(); }
+bool RoboRioSim::GetFPGAButton() {
+  return HALSIM_GetRoboRioFPGAButton();
+}
 
 void RoboRioSim::SetFPGAButton(bool fPGAButton) {
   HALSIM_SetRoboRioFPGAButton(fPGAButton);
@@ -44,7 +43,7 @@
 }
 
 void RoboRioSim::SetVInVoltage(units::volt_t vInVoltage) {
-  HALSIM_SetRoboRioVInVoltage(vInVoltage.to<double>());
+  HALSIM_SetRoboRioVInVoltage(vInVoltage.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterVInCurrentCallback(
@@ -61,7 +60,7 @@
 }
 
 void RoboRioSim::SetVInCurrent(units::ampere_t vInCurrent) {
-  HALSIM_SetRoboRioVInCurrent(vInCurrent.to<double>());
+  HALSIM_SetRoboRioVInCurrent(vInCurrent.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserVoltage6VCallback(
@@ -78,7 +77,7 @@
 }
 
 void RoboRioSim::SetUserVoltage6V(units::volt_t userVoltage6V) {
-  HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.to<double>());
+  HALSIM_SetRoboRioUserVoltage6V(userVoltage6V.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent6VCallback(
@@ -95,7 +94,7 @@
 }
 
 void RoboRioSim::SetUserCurrent6V(units::ampere_t userCurrent6V) {
-  HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.to<double>());
+  HALSIM_SetRoboRioUserCurrent6V(userCurrent6V.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive6VCallback(
@@ -107,7 +106,9 @@
   return store;
 }
 
-bool RoboRioSim::GetUserActive6V() { return HALSIM_GetRoboRioUserActive6V(); }
+bool RoboRioSim::GetUserActive6V() {
+  return HALSIM_GetRoboRioUserActive6V();
+}
 
 void RoboRioSim::SetUserActive6V(bool userActive6V) {
   HALSIM_SetRoboRioUserActive6V(userActive6V);
@@ -127,7 +128,7 @@
 }
 
 void RoboRioSim::SetUserVoltage5V(units::volt_t userVoltage5V) {
-  HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.to<double>());
+  HALSIM_SetRoboRioUserVoltage5V(userVoltage5V.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent5VCallback(
@@ -144,7 +145,7 @@
 }
 
 void RoboRioSim::SetUserCurrent5V(units::ampere_t userCurrent5V) {
-  HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.to<double>());
+  HALSIM_SetRoboRioUserCurrent5V(userCurrent5V.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive5VCallback(
@@ -156,7 +157,9 @@
   return store;
 }
 
-bool RoboRioSim::GetUserActive5V() { return HALSIM_GetRoboRioUserActive5V(); }
+bool RoboRioSim::GetUserActive5V() {
+  return HALSIM_GetRoboRioUserActive5V();
+}
 
 void RoboRioSim::SetUserActive5V(bool userActive5V) {
   HALSIM_SetRoboRioUserActive5V(userActive5V);
@@ -176,7 +179,7 @@
 }
 
 void RoboRioSim::SetUserVoltage3V3(units::volt_t userVoltage3V3) {
-  HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.to<double>());
+  HALSIM_SetRoboRioUserVoltage3V3(userVoltage3V3.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserCurrent3V3Callback(
@@ -193,7 +196,7 @@
 }
 
 void RoboRioSim::SetUserCurrent3V3(units::ampere_t userCurrent3V3) {
-  HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.to<double>());
+  HALSIM_SetRoboRioUserCurrent3V3(userCurrent3V3.value());
 }
 
 std::unique_ptr<CallbackStore> RoboRioSim::RegisterUserActive3V3Callback(
@@ -205,7 +208,9 @@
   return store;
 }
 
-bool RoboRioSim::GetUserActive3V3() { return HALSIM_GetRoboRioUserActive3V3(); }
+bool RoboRioSim::GetUserActive3V3() {
+  return HALSIM_GetRoboRioUserActive3V3();
+}
 
 void RoboRioSim::SetUserActive3V3(bool userActive3V3) {
   HALSIM_SetRoboRioUserActive3V3(userActive3V3);
@@ -220,7 +225,9 @@
   return store;
 }
 
-int RoboRioSim::GetUserFaults6V() { return HALSIM_GetRoboRioUserFaults6V(); }
+int RoboRioSim::GetUserFaults6V() {
+  return HALSIM_GetRoboRioUserFaults6V();
+}
 
 void RoboRioSim::SetUserFaults6V(int userFaults6V) {
   HALSIM_SetRoboRioUserFaults6V(userFaults6V);
@@ -235,7 +242,9 @@
   return store;
 }
 
-int RoboRioSim::GetUserFaults5V() { return HALSIM_GetRoboRioUserFaults5V(); }
+int RoboRioSim::GetUserFaults5V() {
+  return HALSIM_GetRoboRioUserFaults5V();
+}
 
 void RoboRioSim::SetUserFaults5V(int userFaults5V) {
   HALSIM_SetRoboRioUserFaults5V(userFaults5V);
@@ -250,10 +259,31 @@
   return store;
 }
 
-int RoboRioSim::GetUserFaults3V3() { return HALSIM_GetRoboRioUserFaults3V3(); }
+int RoboRioSim::GetUserFaults3V3() {
+  return HALSIM_GetRoboRioUserFaults3V3();
+}
 
 void RoboRioSim::SetUserFaults3V3(int userFaults3V3) {
   HALSIM_SetRoboRioUserFaults3V3(userFaults3V3);
 }
 
-void ResetData() { HALSIM_ResetRoboRioData(); }
+std::unique_ptr<CallbackStore> RoboRioSim::RegisterBrownoutVoltageCallback(
+    NotifyCallback callback, bool initialNotify) {
+  auto store = std::make_unique<CallbackStore>(
+      -1, callback, &HALSIM_CancelRoboRioBrownoutVoltageCallback);
+  store->SetUid(HALSIM_RegisterRoboRioBrownoutVoltageCallback(
+      &CallbackStoreThunk, store.get(), initialNotify));
+  return store;
+}
+
+units::volt_t RoboRioSim::GetBrownoutVoltage() {
+  return units::volt_t(HALSIM_GetRoboRioBrownoutVoltage());
+}
+
+void RoboRioSim::SetBrownoutVoltage(units::volt_t vInVoltage) {
+  HALSIM_SetRoboRioBrownoutVoltage(vInVoltage.value());
+}
+
+void RoboRioSim::ResetData() {
+  HALSIM_ResetRoboRioData();
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
index b71f99d..9483af3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/SPIAccelerometerSim.h"
 
@@ -15,7 +12,9 @@
 using namespace frc;
 using namespace frc::sim;
 
-SPIAccelerometerSim::SPIAccelerometerSim(int index) { m_index = index; }
+SPIAccelerometerSim::SPIAccelerometerSim(int index) {
+  m_index = index;
+}
 
 std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterActiveCallback(
     NotifyCallback callback, bool initialNotify) {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
index 3d52cdf..34fd1e3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimDeviceSim.cpp
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/SimDeviceSim.h"
 
 #include <string>
 #include <vector>
 
+#include <fmt/format.h>
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
 
@@ -19,10 +17,28 @@
 SimDeviceSim::SimDeviceSim(const char* name)
     : m_handle{HALSIM_GetSimDeviceHandle(name)} {}
 
+SimDeviceSim::SimDeviceSim(const char* name, int index) {
+  m_handle =
+      HALSIM_GetSimDeviceHandle(fmt::format("{}[{}]", name, index).c_str());
+}
+
+SimDeviceSim::SimDeviceSim(const char* name, int index, int channel) {
+  m_handle = HALSIM_GetSimDeviceHandle(
+      fmt::format("{}[{},{}]", name, index, channel).c_str());
+}
+
 hal::SimValue SimDeviceSim::GetValue(const char* name) const {
   return HALSIM_GetSimValueHandle(m_handle, name);
 }
 
+hal::SimInt SimDeviceSim::GetInt(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
+hal::SimLong SimDeviceSim::GetLong(const char* name) const {
+  return HALSIM_GetSimValueHandle(m_handle, name);
+}
+
 hal::SimDouble SimDeviceSim::GetDouble(const char* name) const {
   return HALSIM_GetSimValueHandle(m_handle, name);
 }
@@ -40,8 +56,12 @@
   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]);
+  for (int32_t i = 0; i < numOptions; ++i) {
+    rv.emplace_back(options[i]);
+  }
   return rv;
 }
 
-void SimDeviceSim::ResetData() { HALSIM_ResetSimDeviceData(); }
+void SimDeviceSim::ResetData() {
+  HALSIM_ResetSimDeviceData();
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
index 28c434c..fe1cc65 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SimHooks.cpp
@@ -1,40 +1,51 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/SimHooks.h"
 
 #include <hal/simulation/MockHooks.h>
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 
-void SetRuntimeType(HAL_RuntimeType type) { HALSIM_SetRuntimeType(type); }
+void SetRuntimeType(HAL_RuntimeType type) {
+  HALSIM_SetRuntimeType(type);
+}
 
-void WaitForProgramStart() { HALSIM_WaitForProgramStart(); }
+void WaitForProgramStart() {
+  HALSIM_WaitForProgramStart();
+}
 
-void SetProgramStarted() { HALSIM_SetProgramStarted(); }
+void SetProgramStarted() {
+  HALSIM_SetProgramStarted();
+}
 
-bool GetProgramStarted() { return HALSIM_GetProgramStarted(); }
+bool GetProgramStarted() {
+  return HALSIM_GetProgramStarted();
+}
 
-void RestartTiming() { HALSIM_RestartTiming(); }
+void RestartTiming() {
+  HALSIM_RestartTiming();
+}
 
-void PauseTiming() { HALSIM_PauseTiming(); }
+void PauseTiming() {
+  HALSIM_PauseTiming();
+}
 
-void ResumeTiming() { HALSIM_ResumeTiming(); }
+void ResumeTiming() {
+  HALSIM_ResumeTiming();
+}
 
-bool IsTimingPaused() { return HALSIM_IsTimingPaused(); }
+bool IsTimingPaused() {
+  return HALSIM_IsTimingPaused();
+}
 
 void StepTiming(units::second_t delta) {
-  HALSIM_StepTiming(static_cast<uint64_t>(delta.to<double>() * 1e6));
+  HALSIM_StepTiming(static_cast<uint64_t>(delta.value() * 1e6));
 }
 
 void StepTimingAsync(units::second_t delta) {
-  HALSIM_StepTimingAsync(static_cast<uint64_t>(delta.to<double>() * 1e6));
+  HALSIM_StepTimingAsync(static_cast<uint64_t>(delta.value() * 1e6));
 }
 
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
index b33a19e..db56434 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/SingleJointedArmSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/SingleJointedArmSim.h"
 
@@ -12,7 +9,7 @@
 #include <units/voltage.h>
 #include <wpi/MathExtras.h>
 
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/LinearSystemId.h"
 
 using namespace frc;
@@ -42,18 +39,24 @@
           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::WouldHitLowerLimit(units::radian_t armAngle) const {
+  return armAngle < m_minAngle;
 }
 
-bool SingleJointedArmSim::HasHitUpperLimit(
-    const Eigen::Matrix<double, 2, 1>& x) const {
-  return x(0) > m_maxAngle.to<double>();
+bool SingleJointedArmSim::WouldHitUpperLimit(units::radian_t armAngle) const {
+  return armAngle > m_maxAngle;
+}
+
+bool SingleJointedArmSim::HasHitLowerLimit() const {
+  return WouldHitLowerLimit(units::radian_t(m_y(0)));
+}
+
+bool SingleJointedArmSim::HasHitUpperLimit() const {
+  return WouldHitUpperLimit(units::radian_t(m_y(0)));
 }
 
 units::radian_t SingleJointedArmSim::GetAngle() const {
-  return units::radian_t{m_x(0)};
+  return units::radian_t{m_y(0)};
 }
 
 units::radians_per_second_t SingleJointedArmSim::GetVelocity() const {
@@ -69,12 +72,12 @@
 }
 
 void SingleJointedArmSim::SetInputVoltage(units::volt_t voltage) {
-  SetInput(frc::MakeMatrix<1, 1>(voltage.to<double>()));
+  SetInput(Eigen::Vector<double, 1>{voltage.value()});
 }
 
-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) {
+Eigen::Vector<double, 2> SingleJointedArmSim::UpdateX(
+    const Eigen::Vector<double, 2>& currentXhat,
+    const Eigen::Vector<double, 1>& u, units::second_t dt) {
   // Horizontal case:
   // Torque = F * r = I * alpha
   // alpha = F * r / I
@@ -85,24 +88,25 @@
   // 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;
+  Eigen::Vector<double, 2> updatedXhat = RKDP(
+      [&](const auto& x, const auto& u) -> Eigen::Vector<double, 2> {
+        Eigen::Vector<double, 2> 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>());
+          xdot += Eigen::Vector<double, 2>{
+              0.0, (m_mass * m_r * -9.8 * 3.0 / (m_mass * m_r * m_r) *
+                    std::cos(x(0)))
+                       .value()};
         }
         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);
+  if (WouldHitLowerLimit(units::radian_t(updatedXhat(0)))) {
+    return Eigen::Vector<double, 2>{m_minAngle.value(), 0.0};
+  } else if (WouldHitUpperLimit(units::radian_t(updatedXhat(0)))) {
+    return Eigen::Vector<double, 2>{m_maxAngle.value(), 0.0};
   }
   return updatedXhat;
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
new file mode 100644
index 0000000..d2d1687
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/UltrasonicSim.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/UltrasonicSim.h"
+
+#include "frc/Ultrasonic.h"
+#include "frc/simulation/SimDeviceSim.h"
+
+using namespace frc::sim;
+
+UltrasonicSim::UltrasonicSim(const frc::Ultrasonic& ultrasonic) {
+  frc::sim::SimDeviceSim deviceSim{"Ultrasonic", ultrasonic.GetEchoChannel()};
+  m_simRangeValid = deviceSim.GetBoolean("Range Valid");
+  m_simRange = deviceSim.GetDouble("Range (in)");
+}
+
+void UltrasonicSim::SetRangeValid(bool isValid) {
+  m_simRangeValid.Set(isValid);
+}
+
+void UltrasonicSim::SetRange(units::meter_t range) {
+  m_simRange.Set(range.value());
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
index e3bbdce..393c41a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/simulation/XboxControllerSim.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/XboxControllerSim.h"
 
@@ -23,68 +20,66 @@
   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::SetLeftX(double value) {
+  SetRawAxis(XboxController::Axis::kLeftX, 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::SetRightX(double value) {
+  SetRawAxis(XboxController::Axis::kRightX, 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::SetLeftY(double value) {
+  SetRawAxis(XboxController::Axis::kLeftY, 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::SetRightY(double value) {
+  SetRawAxis(XboxController::Axis::kRightY, value);
 }
 
-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::SetLeftTriggerAxis(double value) {
+  SetRawAxis(XboxController::Axis::kLeftTrigger, value);
+}
+
+void XboxControllerSim::SetRightTriggerAxis(double value) {
+  SetRawAxis(XboxController::Axis::kRightTrigger, value);
+}
+
+void XboxControllerSim::SetLeftBumper(bool state) {
+  SetRawButton(XboxController::Button::kLeftBumper, state);
+}
+
+void XboxControllerSim::SetRightBumper(bool state) {
+  SetRawButton(XboxController::Button::kRightBumper, state);
+}
+
+void XboxControllerSim::SetLeftStickButton(bool state) {
+  SetRawButton(XboxController::Button::kLeftStick, state);
+}
+
+void XboxControllerSim::SetRightStickButton(bool state) {
+  SetRawButton(XboxController::Button::kRightStick, state);
 }
 
 void XboxControllerSim::SetAButton(bool state) {
-  SetRawButton(static_cast<int>(XboxController::Button::kA), state);
+  SetRawButton(XboxController::Button::kA, state);
 }
 
 void XboxControllerSim::SetBButton(bool state) {
-  SetRawButton(static_cast<int>(XboxController::Button::kB), state);
+  SetRawButton(XboxController::Button::kB, state);
 }
 
 void XboxControllerSim::SetXButton(bool state) {
-  SetRawButton(static_cast<int>(XboxController::Button::kX), state);
+  SetRawButton(XboxController::Button::kX, state);
 }
 
 void XboxControllerSim::SetYButton(bool state) {
-  SetRawButton(static_cast<int>(XboxController::Button::kY), state);
+  SetRawButton(XboxController::Button::kY, state);
 }
 
 void XboxControllerSim::SetBackButton(bool state) {
-  SetRawButton(static_cast<int>(XboxController::Button::kBack), state);
+  SetRawButton(XboxController::Button::kBack, state);
 }
 
 void XboxControllerSim::SetStartButton(bool state) {
-  SetRawButton(static_cast<int>(XboxController::Button::kStart), state);
+  SetRawButton(XboxController::Button::kStart, state);
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp
new file mode 100644
index 0000000..2915a12
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/Field2d.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/smartdashboard/Field2d.h"
+
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+using namespace frc;
+
+Field2d::Field2d() {
+  m_objects.emplace_back(
+      std::make_unique<FieldObject2d>("Robot", FieldObject2d::private_init{}));
+  m_objects[0]->SetPose(Pose2d{});
+  wpi::SendableRegistry::Add(this, "Field");
+}
+
+Field2d::Field2d(Field2d&& rhs) : SendableHelper(std::move(rhs)) {
+  std::swap(m_table, rhs.m_table);
+  std::swap(m_objects, rhs.m_objects);
+}
+
+Field2d& Field2d::operator=(Field2d&& rhs) {
+  SendableHelper::operator=(std::move(rhs));
+
+  std::swap(m_table, rhs.m_table);
+  std::swap(m_objects, rhs.m_objects);
+
+  return *this;
+}
+
+void Field2d::SetRobotPose(const Pose2d& pose) {
+  std::scoped_lock lock(m_mutex);
+  m_objects[0]->SetPose(pose);
+}
+
+void Field2d::SetRobotPose(units::meter_t x, units::meter_t y,
+                           Rotation2d rotation) {
+  std::scoped_lock lock(m_mutex);
+  m_objects[0]->SetPose(x, y, rotation);
+}
+
+Pose2d Field2d::GetRobotPose() const {
+  std::scoped_lock lock(m_mutex);
+  return m_objects[0]->GetPose();
+}
+
+FieldObject2d* Field2d::GetObject(std::string_view name) {
+  std::scoped_lock lock(m_mutex);
+  for (auto&& obj : m_objects) {
+    if (obj->m_name == name) {
+      return obj.get();
+    }
+  }
+  m_objects.emplace_back(
+      std::make_unique<FieldObject2d>(name, FieldObject2d::private_init{}));
+  auto obj = m_objects.back().get();
+  if (m_table) {
+    obj->m_entry = m_table->GetEntry(obj->m_name);
+  }
+  return obj;
+}
+
+FieldObject2d* Field2d::GetRobotObject() {
+  std::scoped_lock lock(m_mutex);
+  return m_objects[0].get();
+}
+
+void Field2d::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("Field2d");
+
+  std::scoped_lock lock(m_mutex);
+  m_table = builder.GetTable();
+  for (auto&& obj : m_objects) {
+    std::scoped_lock lock2(obj->m_mutex);
+    obj->m_entry = m_table->GetEntry(obj->m_name);
+    obj->UpdateEntry(true);
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
new file mode 100644
index 0000000..93d6d7e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/FieldObject2d.cpp
@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/smartdashboard/FieldObject2d.h"
+
+#include <vector>
+
+#include <wpi/Endian.h>
+#include <wpi/MathExtras.h>
+
+#include "frc/trajectory/Trajectory.h"
+
+using namespace frc;
+
+FieldObject2d::FieldObject2d(FieldObject2d&& rhs) {
+  std::swap(m_name, rhs.m_name);
+  std::swap(m_entry, rhs.m_entry);
+  std::swap(m_poses, rhs.m_poses);
+}
+
+FieldObject2d& FieldObject2d::operator=(FieldObject2d&& rhs) {
+  std::swap(m_name, rhs.m_name);
+  std::swap(m_entry, rhs.m_entry);
+  std::swap(m_poses, rhs.m_poses);
+
+  return *this;
+}
+
+void FieldObject2d::SetPose(const Pose2d& pose) {
+  SetPoses({pose});
+}
+
+void FieldObject2d::SetPose(units::meter_t x, units::meter_t y,
+                            Rotation2d rotation) {
+  SetPoses({{x, y, rotation}});
+}
+
+Pose2d FieldObject2d::GetPose() const {
+  std::scoped_lock lock(m_mutex);
+  UpdateFromEntry();
+  if (m_poses.empty()) {
+    return {};
+  }
+  return m_poses[0];
+}
+
+void FieldObject2d::SetPoses(wpi::span<const Pose2d> poses) {
+  std::scoped_lock lock(m_mutex);
+  m_poses.assign(poses.begin(), poses.end());
+  UpdateEntry();
+}
+
+void FieldObject2d::SetPoses(std::initializer_list<Pose2d> poses) {
+  SetPoses({poses.begin(), poses.end()});
+}
+
+void FieldObject2d::SetTrajectory(const Trajectory& trajectory) {
+  std::scoped_lock lock(m_mutex);
+  m_poses.clear();
+  m_poses.reserve(trajectory.States().size());
+  for (auto&& state : trajectory.States()) {
+    m_poses.push_back(state.pose);
+  }
+  UpdateEntry();
+}
+
+std::vector<Pose2d> FieldObject2d::GetPoses() const {
+  std::scoped_lock lock(m_mutex);
+  UpdateFromEntry();
+  return std::vector<Pose2d>(m_poses.begin(), m_poses.end());
+}
+
+wpi::span<const Pose2d> FieldObject2d::GetPoses(
+    wpi::SmallVectorImpl<Pose2d>& out) const {
+  std::scoped_lock lock(m_mutex);
+  UpdateFromEntry();
+  out.assign(m_poses.begin(), m_poses.end());
+  return out;
+}
+
+void FieldObject2d::UpdateEntry(bool setDefault) {
+  if (!m_entry) {
+    return;
+  }
+  if (m_poses.size() < (255 / 3)) {
+    wpi::SmallVector<double, 9> arr;
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      arr.push_back(translation.X().value());
+      arr.push_back(translation.Y().value());
+      arr.push_back(pose.Rotation().Degrees().value());
+    }
+    if (setDefault) {
+      m_entry.SetDefaultDoubleArray(arr);
+    } else {
+      m_entry.ForceSetDoubleArray(arr);
+    }
+  } else {
+    // send as raw array of doubles if too big for NT array
+    std::vector<char> arr;
+    arr.resize(m_poses.size() * 3 * 8);
+    char* p = arr.data();
+    for (auto&& pose : m_poses) {
+      auto& translation = pose.Translation();
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.X().value()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(translation.Y().value()));
+      p += 8;
+      wpi::support::endian::write64be(
+          p, wpi::DoubleToBits(pose.Rotation().Degrees().value()));
+      p += 8;
+    }
+    if (setDefault) {
+      m_entry.SetDefaultRaw({arr.data(), arr.size()});
+    } else {
+      m_entry.ForceSetRaw({arr.data(), arr.size()});
+    }
+  }
+}
+
+void FieldObject2d::UpdateFromEntry() const {
+  if (!m_entry) {
+    return;
+  }
+  auto val = m_entry.GetValue();
+  if (!val) {
+    return;
+  }
+
+  if (val->IsDoubleArray()) {
+    auto arr = val->GetDoubleArray();
+    auto size = arr.size();
+    if ((size % 3) != 0) {
+      return;
+    }
+    m_poses.resize(size / 3);
+    for (size_t i = 0; i < size / 3; ++i) {
+      m_poses[i] =
+          Pose2d{units::meter_t{arr[i * 3 + 0]}, units::meter_t{arr[i * 3 + 1]},
+                 Rotation2d{units::degree_t{arr[i * 3 + 2]}}};
+    }
+  } else if (val->IsRaw()) {
+    // treat it simply as an array of doubles
+    std::string_view data = val->GetRaw();
+
+    // must be triples of doubles
+    auto size = data.size();
+    if ((size % (3 * 8)) != 0) {
+      return;
+    }
+    m_poses.resize(size / (3 * 8));
+    const char* p = data.data();
+    for (size_t i = 0; i < size / (3 * 8); ++i) {
+      double x = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double y = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      double rot = wpi::BitsToDouble(
+          wpi::support::endian::readNext<uint64_t, wpi::support::big,
+                                         wpi::support::unaligned>(p));
+      m_poses[i] = Pose2d{units::meter_t{x}, units::meter_t{y},
+                          Rotation2d{units::degree_t{rot}}};
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
index 75b373a..2a53196 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/ListenerExecutor.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/smartdashboard/ListenerExecutor.h"
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.cpp
new file mode 100644
index 0000000..bfa827a
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/Mechanism2d.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/smartdashboard/Mechanism2d.h"
+
+#include <cstdio>
+
+#include <networktables/NTSendableBuilder.h>
+
+using namespace frc;
+
+static constexpr char kBackgroundColor[] = "backgroundColor";
+static constexpr char kDims[] = "dims";
+
+Mechanism2d::Mechanism2d(double width, double height,
+                         const Color8Bit& backgroundColor)
+    : m_width{width}, m_height{height} {
+  SetBackgroundColor(backgroundColor);
+}
+
+MechanismRoot2d* Mechanism2d::GetRoot(std::string_view name, double x,
+                                      double y) {
+  auto& obj = m_roots[name];
+  if (obj) {
+    return obj.get();
+  }
+  obj = std::make_unique<MechanismRoot2d>(name, x, y,
+                                          MechanismRoot2d::private_init{});
+  if (m_table) {
+    obj->Update(m_table->GetSubTable(name));
+  }
+  return obj.get();
+}
+
+void Mechanism2d::SetBackgroundColor(const Color8Bit& color) {
+  std::snprintf(m_color, sizeof(m_color), "#%02X%02X%02X", color.red,
+                color.green, color.blue);
+  if (m_table) {
+    m_table->GetEntry(kBackgroundColor).SetString(m_color);
+  }
+}
+
+void Mechanism2d::InitSendable(nt::NTSendableBuilder& builder) {
+  builder.SetSmartDashboardType("Mechanism2d");
+
+  std::scoped_lock lock(m_mutex);
+  m_table = builder.GetTable();
+  m_table->GetEntry(kDims).SetDoubleArray({m_width, m_height});
+  m_table->GetEntry(kBackgroundColor).SetString(m_color);
+  for (const auto& entry : m_roots) {
+    const auto& root = entry.getValue().get();
+    root->Update(m_table->GetSubTable(entry.getKey()));
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
new file mode 100644
index 0000000..175db0d
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/smartdashboard/MechanismLigament2d.h"
+
+#include <cstdio>
+
+using namespace frc;
+
+MechanismLigament2d::MechanismLigament2d(std::string_view name, double length,
+                                         units::degree_t angle,
+                                         double lineWeight,
+                                         const frc::Color8Bit& color)
+    : MechanismObject2d(name),
+      m_length{length},
+      m_angle{angle.value()},
+      m_weight{lineWeight} {
+  SetColor(color);
+}
+
+void MechanismLigament2d::UpdateEntries(
+    std::shared_ptr<nt::NetworkTable> table) {
+  std::scoped_lock lock(m_mutex);
+  table->GetEntry(".type").SetString("line");
+
+  m_colorEntry = table->GetEntry("color");
+  m_angleEntry = table->GetEntry("angle");
+  m_weightEntry = table->GetEntry("weight");
+  m_lengthEntry = table->GetEntry("length");
+  Flush();
+}
+
+void MechanismLigament2d::SetColor(const Color8Bit& color) {
+  std::scoped_lock lock(m_mutex);
+  std::snprintf(m_color, sizeof(m_color), "#%02X%02X%02X", color.red,
+                color.green, color.blue);
+  Flush();
+}
+
+void MechanismLigament2d::SetAngle(units::degree_t angle) {
+  std::scoped_lock lock(m_mutex);
+  m_angle = angle.value();
+  Flush();
+}
+
+void MechanismLigament2d::SetLineWeight(double lineWidth) {
+  std::scoped_lock lock(m_mutex);
+  m_weight = lineWidth;
+  Flush();
+}
+
+double MechanismLigament2d::GetAngle() {
+  if (m_angleEntry) {
+    m_angle = m_angleEntry.GetDouble(0.0);
+  }
+  return m_angle;
+}
+
+double MechanismLigament2d::GetLength() {
+  if (m_lengthEntry) {
+    m_length = m_lengthEntry.GetDouble(0.0);
+  }
+  return m_length;
+}
+
+void MechanismLigament2d::SetLength(double length) {
+  std::scoped_lock lock(m_mutex);
+  m_length = length;
+  Flush();
+}
+
+#define SAFE_WRITE(data, Type)           \
+  if (m_##data##Entry) {                 \
+    m_##data##Entry.Set##Type(m_##data); \
+  }
+void MechanismLigament2d::Flush() {
+  SAFE_WRITE(color, String)
+  SAFE_WRITE(angle, Double)
+  SAFE_WRITE(length, Double)
+  SAFE_WRITE(weight, Double)
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp
new file mode 100644
index 0000000..15d24a3
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismObject2d.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/smartdashboard/MechanismObject2d.h"
+
+using namespace frc;
+
+MechanismObject2d::MechanismObject2d(std::string_view name) : m_name{name} {}
+
+const std::string& MechanismObject2d::GetName() const {
+  return m_name;
+}
+
+void MechanismObject2d::Update(std::shared_ptr<nt::NetworkTable> table) {
+  std::scoped_lock lock(m_mutex);
+  m_table = table;
+  UpdateEntries(m_table);
+  for (const wpi::StringMapEntry<std::unique_ptr<MechanismObject2d>>& entry :
+       m_objects) {
+    entry.getValue()->Update(m_table->GetSubTable(entry.getKey()));
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
new file mode 100644
index 0000000..07d15f0
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/smartdashboard/MechanismRoot2d.h"
+
+#include "frc/util/Color8Bit.h"
+
+using namespace frc;
+
+MechanismRoot2d::MechanismRoot2d(std::string_view name, double x, double y,
+                                 const private_init&)
+    : MechanismObject2d(name), m_x{x}, m_y{y} {}
+
+void MechanismRoot2d::SetPosition(double x, double y) {
+  std::scoped_lock lock(m_mutex);
+  m_x = x;
+  m_y = y;
+  Flush();
+}
+
+void MechanismRoot2d::UpdateEntries(std::shared_ptr<nt::NetworkTable> table) {
+  std::scoped_lock lock(m_mutex);
+  m_xEntry = table->GetEntry("x");
+  m_yEntry = table->GetEntry("y");
+  Flush();
+}
+
+inline void MechanismRoot2d::Flush() {
+  std::scoped_lock lock(m_mutex);
+  if (m_xEntry) {
+    m_xEntry.SetDouble(m_x);
+  }
+  if (m_yEntry) {
+    m_yEntry.SetDouble(m_y);
+  }
+}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
deleted file mode 100644
index 7fc8635..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBase.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/smartdashboard/SendableBase.h"
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-SendableBase::SendableBase(bool addLiveWindow) {
-  if (addLiveWindow)
-    SendableRegistry::GetInstance().AddLW(this, "");
-  else
-    SendableRegistry::GetInstance().Add(this, "");
-}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
index 135f4be..48af198 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableBuilderImpl.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/smartdashboard/SendableBuilderImpl.h"
 
@@ -23,14 +20,20 @@
   return m_table;
 }
 
-bool SendableBuilderImpl::HasTable() const { return m_table != nullptr; }
+bool SendableBuilderImpl::IsPublished() const {
+  return m_table != nullptr;
+}
 
-bool SendableBuilderImpl::IsActuator() const { return m_actuator; }
+bool SendableBuilderImpl::IsActuator() const {
+  return m_actuator;
+}
 
-void SendableBuilderImpl::UpdateTable() {
+void SendableBuilderImpl::Update() {
   uint64_t time = nt::Now();
   for (auto& property : m_properties) {
-    if (property.update) property.update(property.entry, time);
+    if (property.update) {
+      property.update(property.entry, time);
+    }
   }
   for (auto& updateTable : m_updateTables) {
     updateTable();
@@ -38,28 +41,42 @@
 }
 
 void SendableBuilderImpl::StartListeners() {
-  for (auto& property : m_properties) property.StartListener();
-  if (m_controllableEntry) m_controllableEntry.SetBoolean(true);
+  for (auto& property : m_properties) {
+    property.StartListener();
+  }
+  if (m_controllableEntry) {
+    m_controllableEntry.SetBoolean(true);
+  }
 }
 
 void SendableBuilderImpl::StopListeners() {
-  for (auto& property : m_properties) property.StopListener();
-  if (m_controllableEntry) m_controllableEntry.SetBoolean(false);
+  for (auto& property : m_properties) {
+    property.StopListener();
+  }
+  if (m_controllableEntry) {
+    m_controllableEntry.SetBoolean(false);
+  }
 }
 
 void SendableBuilderImpl::StartLiveWindowMode() {
-  if (m_safeState) m_safeState();
+  if (m_safeState) {
+    m_safeState();
+  }
   StartListeners();
 }
 
 void SendableBuilderImpl::StopLiveWindowMode() {
   StopListeners();
-  if (m_safeState) m_safeState();
+  if (m_safeState) {
+    m_safeState();
+  }
 }
 
-void SendableBuilderImpl::ClearProperties() { m_properties.clear(); }
+void SendableBuilderImpl::ClearProperties() {
+  m_properties.clear();
+}
 
-void SendableBuilderImpl::SetSmartDashboardType(const wpi::Twine& type) {
+void SendableBuilderImpl::SetSmartDashboardType(std::string_view type) {
   m_table->GetEntry(".type").SetString(type);
 }
 
@@ -76,11 +93,11 @@
   m_updateTables.emplace_back(std::move(func));
 }
 
-nt::NetworkTableEntry SendableBuilderImpl::GetEntry(const wpi::Twine& key) {
+nt::NetworkTableEntry SendableBuilderImpl::GetEntry(std::string_view key) {
   return m_table->GetEntry(key);
 }
 
-void SendableBuilderImpl::AddBooleanProperty(const wpi::Twine& key,
+void SendableBuilderImpl::AddBooleanProperty(std::string_view key,
                                              std::function<bool()> getter,
                                              std::function<void(bool)> setter) {
   m_properties.emplace_back(*m_table, key);
@@ -95,7 +112,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsBoolean()) return;
+            if (!event.value->IsBoolean()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetBoolean()); });
           },
@@ -105,7 +124,7 @@
 }
 
 void SendableBuilderImpl::AddDoubleProperty(
-    const wpi::Twine& key, std::function<double()> getter,
+    std::string_view key, std::function<double()> getter,
     std::function<void(double)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
@@ -119,7 +138,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsDouble()) return;
+            if (!event.value->IsDouble()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetDouble()); });
           },
@@ -129,8 +150,8 @@
 }
 
 void SendableBuilderImpl::AddStringProperty(
-    const wpi::Twine& key, std::function<std::string()> getter,
-    std::function<void(wpi::StringRef)> setter) {
+    std::string_view key, std::function<std::string()> getter,
+    std::function<void(std::string_view)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -143,7 +164,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsString()) return;
+            if (!event.value->IsString()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetString()); });
           },
@@ -153,8 +176,8 @@
 }
 
 void SendableBuilderImpl::AddBooleanArrayProperty(
-    const wpi::Twine& key, std::function<std::vector<int>()> getter,
-    std::function<void(wpi::ArrayRef<int>)> setter) {
+    std::string_view key, std::function<std::vector<int>()> getter,
+    std::function<void(wpi::span<const int>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -167,7 +190,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsBooleanArray()) return;
+            if (!event.value->IsBooleanArray()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetBooleanArray()); });
           },
@@ -177,8 +202,8 @@
 }
 
 void SendableBuilderImpl::AddDoubleArrayProperty(
-    const wpi::Twine& key, std::function<std::vector<double>()> getter,
-    std::function<void(wpi::ArrayRef<double>)> setter) {
+    std::string_view key, std::function<std::vector<double>()> getter,
+    std::function<void(wpi::span<const double>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -191,7 +216,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsDoubleArray()) return;
+            if (!event.value->IsDoubleArray()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetDoubleArray()); });
           },
@@ -201,8 +228,8 @@
 }
 
 void SendableBuilderImpl::AddStringArrayProperty(
-    const wpi::Twine& key, std::function<std::vector<std::string>()> getter,
-    std::function<void(wpi::ArrayRef<std::string>)> setter) {
+    std::string_view key, std::function<std::vector<std::string>()> getter,
+    std::function<void(wpi::span<const std::string>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -215,7 +242,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsStringArray()) return;
+            if (!event.value->IsStringArray()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetStringArray()); });
           },
@@ -225,8 +254,8 @@
 }
 
 void SendableBuilderImpl::AddRawProperty(
-    const wpi::Twine& key, std::function<std::string()> getter,
-    std::function<void(wpi::StringRef)> setter) {
+    std::string_view key, std::function<std::string()> getter,
+    std::function<void(std::string_view)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -239,7 +268,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsRaw()) return;
+            if (!event.value->IsRaw()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetRaw()); });
           },
@@ -249,7 +280,7 @@
 }
 
 void SendableBuilderImpl::AddValueProperty(
-    const wpi::Twine& key, std::function<std::shared_ptr<nt::Value>()> getter,
+    std::string_view key, std::function<std::shared_ptr<nt::Value>()> getter,
     std::function<void(std::shared_ptr<nt::Value>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
@@ -271,9 +302,9 @@
 }
 
 void SendableBuilderImpl::AddSmallStringProperty(
-    const wpi::Twine& key,
-    std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
-    std::function<void(wpi::StringRef)> setter) {
+    std::string_view key,
+    std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+    std::function<void(std::string_view)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -287,7 +318,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsString()) return;
+            if (!event.value->IsString()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetString()); });
           },
@@ -297,9 +330,9 @@
 }
 
 void SendableBuilderImpl::AddSmallBooleanArrayProperty(
-    const wpi::Twine& key,
-    std::function<wpi::ArrayRef<int>(wpi::SmallVectorImpl<int>& buf)> getter,
-    std::function<void(wpi::ArrayRef<int>)> setter) {
+    std::string_view key,
+    std::function<wpi::span<const int>(wpi::SmallVectorImpl<int>& buf)> getter,
+    std::function<void(wpi::span<const int>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -313,7 +346,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsBooleanArray()) return;
+            if (!event.value->IsBooleanArray()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetBooleanArray()); });
           },
@@ -323,10 +358,10 @@
 }
 
 void SendableBuilderImpl::AddSmallDoubleArrayProperty(
-    const wpi::Twine& key,
-    std::function<wpi::ArrayRef<double>(wpi::SmallVectorImpl<double>& buf)>
+    std::string_view key,
+    std::function<wpi::span<const double>(wpi::SmallVectorImpl<double>& buf)>
         getter,
-    std::function<void(wpi::ArrayRef<double>)> setter) {
+    std::function<void(wpi::span<const double>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -340,7 +375,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsDoubleArray()) return;
+            if (!event.value->IsDoubleArray()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetDoubleArray()); });
           },
@@ -350,11 +387,11 @@
 }
 
 void SendableBuilderImpl::AddSmallStringArrayProperty(
-    const wpi::Twine& key,
+    std::string_view key,
     std::function<
-        wpi::ArrayRef<std::string>(wpi::SmallVectorImpl<std::string>& buf)>
+        wpi::span<const std::string>(wpi::SmallVectorImpl<std::string>& buf)>
         getter,
-    std::function<void(wpi::ArrayRef<std::string>)> setter) {
+    std::function<void(wpi::span<const std::string>)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -368,7 +405,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsStringArray()) return;
+            if (!event.value->IsStringArray()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetStringArray()); });
           },
@@ -378,9 +417,9 @@
 }
 
 void SendableBuilderImpl::AddSmallRawProperty(
-    const wpi::Twine& key,
-    std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
-    std::function<void(wpi::StringRef)> setter) {
+    std::string_view key,
+    std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+    std::function<void(std::string_view)> setter) {
   m_properties.emplace_back(*m_table, key);
   if (getter) {
     m_properties.back().update = [=](nt::NetworkTableEntry entry,
@@ -394,7 +433,9 @@
         [=](nt::NetworkTableEntry entry) -> NT_EntryListener {
       return entry.AddListener(
           [=](const nt::EntryNotification& event) {
-            if (!event.value->IsRaw()) return;
+            if (!event.value->IsRaw()) {
+              return;
+            }
             SmartDashboard::PostListenerTask(
                 [=] { setter(event.value->GetRaw()); });
           },
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
index 2f7af9c..550d40c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableChooserBase.cpp
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/smartdashboard/SendableChooserBase.h"
 
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <wpi/sendable/SendableRegistry.h>
 
 using namespace frc;
 
 std::atomic_int SendableChooserBase::s_instances{0};
 
 SendableChooserBase::SendableChooserBase() : m_instance{s_instances++} {
-  SendableRegistry::GetInstance().Add(this, "SendableChooser", m_instance);
+  wpi::SendableRegistry::Add(this, "SendableChooser", m_instance);
 }
 
 SendableChooserBase::SendableChooserBase(SendableChooserBase&& oth)
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
deleted file mode 100644
index 4480313..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SendableRegistry.cpp
+++ /dev/null
@@ -1,372 +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/smartdashboard/SendableRegistry.h"
-
-#include <memory>
-
-#include <wpi/DenseMap.h>
-#include <wpi/SmallVector.h>
-#include <wpi/UidVector.h>
-#include <wpi/mutex.h>
-
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableBuilderImpl.h"
-
-using namespace frc;
-
-struct SendableRegistry::Impl {
-  struct Component {
-    Sendable* sendable = nullptr;
-    SendableBuilderImpl builder;
-    std::string name;
-    std::string subsystem = "Ungrouped";
-    Sendable* parent = nullptr;
-    bool liveWindow = false;
-    wpi::SmallVector<std::shared_ptr<void>, 2> data;
-
-    void SetName(const wpi::Twine& moduleType, int channel) {
-      name =
-          (moduleType + wpi::Twine('[') + wpi::Twine(channel) + wpi::Twine(']'))
-              .str();
-    }
-
-    void SetName(const wpi::Twine& moduleType, int moduleNumber, int channel) {
-      name = (moduleType + wpi::Twine('[') + wpi::Twine(moduleNumber) +
-              wpi::Twine(',') + wpi::Twine(channel) + wpi::Twine(']'))
-                 .str();
-    }
-  };
-
-  wpi::recursive_mutex mutex;
-
-  wpi::UidVector<std::unique_ptr<Component>, 32> components;
-  wpi::DenseMap<void*, UID> componentMap;
-  int nextDataHandle = 0;
-
-  Component& GetOrAdd(void* sendable, UID* uid = nullptr);
-};
-
-SendableRegistry::Impl::Component& SendableRegistry::Impl::GetOrAdd(
-    void* sendable, UID* uid) {
-  UID& compUid = componentMap[sendable];
-  if (compUid == 0)
-    compUid = components.emplace_back(std::make_unique<Component>()) + 1;
-  if (uid) *uid = compUid;
-
-  return *components[compUid - 1];
-}
-
-SendableRegistry& SendableRegistry::GetInstance() {
-  static SendableRegistry instance;
-  return instance;
-}
-
-void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.name = name.str();
-}
-
-void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
-                           int channel) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.SetName(moduleType, channel);
-}
-
-void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& moduleType,
-                           int moduleNumber, int channel) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.SetName(moduleType, moduleNumber, channel);
-}
-
-void SendableRegistry::Add(Sendable* sendable, const wpi::Twine& subsystem,
-                           const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.name = name.str();
-  comp.subsystem = subsystem.str();
-}
-
-void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.liveWindow = true;
-  comp.name = name.str();
-}
-
-void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
-                             int channel) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.liveWindow = true;
-  comp.SetName(moduleType, channel);
-}
-
-void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& moduleType,
-                             int moduleNumber, int channel) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.liveWindow = true;
-  comp.SetName(moduleType, moduleNumber, channel);
-}
-
-void SendableRegistry::AddLW(Sendable* sendable, const wpi::Twine& subsystem,
-                             const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(sendable);
-  comp.sendable = sendable;
-  comp.liveWindow = true;
-  comp.name = name.str();
-  comp.subsystem = subsystem.str();
-}
-
-void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(child);
-  comp.parent = parent;
-}
-
-void SendableRegistry::AddChild(Sendable* parent, void* child) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto& comp = m_impl->GetOrAdd(child);
-  comp.parent = parent;
-}
-
-bool SendableRegistry::Remove(Sendable* sendable) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end()) return false;
-  UID compUid = it->getSecond();
-  m_impl->components.erase(compUid - 1);
-  m_impl->componentMap.erase(it);
-  // update any parent pointers
-  for (auto&& comp : m_impl->components) {
-    if (comp->parent == sendable) comp->parent = nullptr;
-  }
-  return true;
-}
-
-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() ||
-      !m_impl->components[it->getSecond() - 1])
-    return;
-  UID compUid = it->getSecond();
-  m_impl->componentMap.erase(it);
-  m_impl->componentMap[to] = compUid;
-  auto& comp = *m_impl->components[compUid - 1];
-  comp.sendable = to;
-  if (comp.builder.HasTable()) {
-    // rebuild builder, as lambda captures can point to "from"
-    comp.builder.ClearProperties();
-    to->InitSendable(comp.builder);
-  }
-  // update any parent pointers
-  for (auto&& comp : m_impl->components) {
-    if (comp->parent == from) comp->parent = to;
-  }
-}
-
-bool SendableRegistry::Contains(const Sendable* sendable) const {
-  std::scoped_lock lock(m_impl->mutex);
-  return m_impl->componentMap.count(sendable) != 0;
-}
-
-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() ||
-      !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() ||
-      !m_impl->components[it->getSecond() - 1])
-    return;
-  m_impl->components[it->getSecond() - 1]->name = name.str();
-}
-
-void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
-                               int channel) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end() ||
-      !m_impl->components[it->getSecond() - 1])
-    return;
-  m_impl->components[it->getSecond() - 1]->SetName(moduleType, channel);
-}
-
-void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& moduleType,
-                               int moduleNumber, int channel) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end() ||
-      !m_impl->components[it->getSecond() - 1])
-    return;
-  m_impl->components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
-                                                   channel);
-}
-
-void SendableRegistry::SetName(Sendable* sendable, const wpi::Twine& subsystem,
-                               const wpi::Twine& name) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  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();
-}
-
-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() ||
-      !m_impl->components[it->getSecond() - 1])
-    return std::string{};
-  return m_impl->components[it->getSecond() - 1]->subsystem;
-}
-
-void SendableRegistry::SetSubsystem(Sendable* sendable,
-                                    const wpi::Twine& subsystem) {
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  if (it == m_impl->componentMap.end() ||
-      !m_impl->components[it->getSecond() - 1])
-    return;
-  m_impl->components[it->getSecond() - 1]->subsystem = subsystem.str();
-}
-
-int SendableRegistry::GetDataHandle() {
-  std::scoped_lock lock(m_impl->mutex);
-  return m_impl->nextDataHandle++;
-}
-
-std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
-                                                std::shared_ptr<void> data) {
-  assert(handle >= 0);
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  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())
-    rv = std::move(comp.data[handle]);
-  else
-    comp.data.resize(handle + 1);
-  comp.data[handle] = std::move(data);
-  return rv;
-}
-
-std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
-                                                int handle) {
-  assert(handle >= 0);
-  std::scoped_lock lock(m_impl->mutex);
-  auto it = m_impl->componentMap.find(sendable);
-  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];
-}
-
-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() ||
-      !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() ||
-      !m_impl->components[it->getSecond() - 1])
-    return;
-  m_impl->components[it->getSecond() - 1]->liveWindow = false;
-}
-
-SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
-  std::scoped_lock lock(m_impl->mutex);
-  UID uid;
-  auto& comp = m_impl->GetOrAdd(sendable, &uid);
-  comp.sendable = sendable;
-  return uid;
-}
-
-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 || (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);
-  comp.sendable->InitSendable(comp.builder);
-  comp.builder.UpdateTable();
-  comp.builder.StartListeners();
-}
-
-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();
-}
-
-void SendableRegistry::ForeachLiveWindow(
-    int dataHandle,
-    wpi::function_ref<void(CallbackData& data)> callback) const {
-  assert(dataHandle >= 0);
-  std::scoped_lock lock(m_impl->mutex);
-  wpi::SmallVector<Impl::Component*, 128> components;
-  for (auto&& comp : m_impl->components) components.emplace_back(comp.get());
-  for (auto comp : components) {
-    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,
-                          comp->subsystem,        comp->parent,
-                          comp->data[dataHandle], comp->builder};
-      callback(cbdata);
-    }
-  }
-}
-
-SendableRegistry::SendableRegistry() : m_impl(new Impl) {}
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index 5e70a4c..25bb61f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/smartdashboard/SmartDashboard.h"
 
@@ -12,252 +9,239 @@
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/StringMap.h>
 #include <wpi/mutex.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-#include "frc/WPIErrors.h"
-#include "frc/smartdashboard/SendableRegistry.h"
+#include "frc/Errors.h"
+#include "frc/smartdashboard/ListenerExecutor.h"
+#include "frc/smartdashboard/SendableBuilderImpl.h"
 
 using namespace frc;
 
 namespace {
-class Singleton {
- public:
-  static Singleton& GetInstance();
-
-  std::shared_ptr<nt::NetworkTable> table;
-  wpi::StringMap<SendableRegistry::UID> tablesToData;
+struct Instance {
+  detail::ListenerExecutor listenerExecutor;
+  std::shared_ptr<nt::NetworkTable> table =
+      nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
+  wpi::StringMap<wpi::SendableRegistry::UID> tablesToData;
   wpi::mutex tablesToDataMutex;
-
- private:
-  Singleton() {
-    table = nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
-    HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0);
-  }
-  Singleton(const Singleton&) = delete;
-  Singleton& operator=(const Singleton&) = delete;
 };
-
 }  // namespace
 
-Singleton& Singleton::GetInstance() {
-  static Singleton instance;
+static Instance& GetInstance() {
+  HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0);
+  static Instance instance;
   return instance;
 }
 
-void SmartDashboard::init() { Singleton::GetInstance(); }
+void SmartDashboard::init() {
+  GetInstance();
+}
 
-bool SmartDashboard::ContainsKey(wpi::StringRef key) {
-  return Singleton::GetInstance().table->ContainsKey(key);
+bool SmartDashboard::ContainsKey(std::string_view key) {
+  return GetInstance().table->ContainsKey(key);
 }
 
 std::vector<std::string> SmartDashboard::GetKeys(int types) {
-  return Singleton::GetInstance().table->GetKeys(types);
+  return GetInstance().table->GetKeys(types);
 }
 
-void SmartDashboard::SetPersistent(wpi::StringRef key) {
-  Singleton::GetInstance().table->GetEntry(key).SetPersistent();
+void SmartDashboard::SetPersistent(std::string_view key) {
+  GetInstance().table->GetEntry(key).SetPersistent();
 }
 
-void SmartDashboard::ClearPersistent(wpi::StringRef key) {
-  Singleton::GetInstance().table->GetEntry(key).ClearPersistent();
+void SmartDashboard::ClearPersistent(std::string_view key) {
+  GetInstance().table->GetEntry(key).ClearPersistent();
 }
 
-bool SmartDashboard::IsPersistent(wpi::StringRef key) {
-  return Singleton::GetInstance().table->GetEntry(key).IsPersistent();
+bool SmartDashboard::IsPersistent(std::string_view key) {
+  return GetInstance().table->GetEntry(key).IsPersistent();
 }
 
-void SmartDashboard::SetFlags(wpi::StringRef key, unsigned int flags) {
-  Singleton::GetInstance().table->GetEntry(key).SetFlags(flags);
+void SmartDashboard::SetFlags(std::string_view key, unsigned int flags) {
+  GetInstance().table->GetEntry(key).SetFlags(flags);
 }
 
-void SmartDashboard::ClearFlags(wpi::StringRef key, unsigned int flags) {
-  Singleton::GetInstance().table->GetEntry(key).ClearFlags(flags);
+void SmartDashboard::ClearFlags(std::string_view key, unsigned int flags) {
+  GetInstance().table->GetEntry(key).ClearFlags(flags);
 }
 
-unsigned int SmartDashboard::GetFlags(wpi::StringRef key) {
-  return Singleton::GetInstance().table->GetEntry(key).GetFlags();
+unsigned int SmartDashboard::GetFlags(std::string_view key) {
+  return GetInstance().table->GetEntry(key).GetFlags();
 }
 
-void SmartDashboard::Delete(wpi::StringRef key) {
-  Singleton::GetInstance().table->Delete(key);
+void SmartDashboard::Delete(std::string_view key) {
+  GetInstance().table->Delete(key);
 }
 
-nt::NetworkTableEntry SmartDashboard::GetEntry(wpi::StringRef key) {
-  return Singleton::GetInstance().table->GetEntry(key);
+nt::NetworkTableEntry SmartDashboard::GetEntry(std::string_view key) {
+  return GetInstance().table->GetEntry(key);
 }
 
-void SmartDashboard::PutData(wpi::StringRef key, Sendable* data) {
-  if (data == nullptr) {
-    wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
-    return;
+void SmartDashboard::PutData(std::string_view key, wpi::Sendable* data) {
+  if (!data) {
+    throw FRC_MakeError(err::NullParameter, "{}", "value");
   }
-  auto& inst = Singleton::GetInstance();
+  auto& inst = GetInstance();
   std::scoped_lock lock(inst.tablesToDataMutex);
   auto& uid = inst.tablesToData[key];
-  auto& registry = SendableRegistry::GetInstance();
-  Sendable* sddata = registry.GetSendable(uid);
+  wpi::Sendable* sddata = wpi::SendableRegistry::GetSendable(uid);
   if (sddata != data) {
-    uid = registry.GetUniqueId(data);
+    uid = wpi::SendableRegistry::GetUniqueId(data);
     auto dataTable = inst.table->GetSubTable(key);
-    registry.Publish(uid, dataTable);
+    auto builder = std::make_unique<SendableBuilderImpl>();
+    auto builderPtr = builder.get();
+    builderPtr->SetTable(dataTable);
+    wpi::SendableRegistry::Publish(uid, std::move(builder));
+    builderPtr->StartListeners();
     dataTable->GetEntry(".name").SetString(key);
   }
 }
 
-void SmartDashboard::PutData(Sendable* value) {
-  if (value == nullptr) {
-    wpi_setGlobalWPIErrorWithContext(NullParameter, "value");
-    return;
+void SmartDashboard::PutData(wpi::Sendable* value) {
+  if (!value) {
+    throw FRC_MakeError(err::NullParameter, "{}", "value");
   }
-  auto name = SendableRegistry::GetInstance().GetName(value);
-  if (!name.empty()) PutData(name, value);
+  auto name = wpi::SendableRegistry::GetName(value);
+  if (!name.empty()) {
+    PutData(name, value);
+  }
 }
 
-Sendable* SmartDashboard::GetData(wpi::StringRef key) {
-  auto& inst = Singleton::GetInstance();
+wpi::Sendable* SmartDashboard::GetData(std::string_view key) {
+  auto& inst = GetInstance();
   std::scoped_lock lock(inst.tablesToDataMutex);
   auto it = inst.tablesToData.find(key);
   if (it == inst.tablesToData.end()) {
-    wpi_setGlobalWPIErrorWithContext(SmartDashboardMissingKey, key);
-    return nullptr;
+    throw FRC_MakeError(err::SmartDashboardMissingKey, "{}", key);
   }
-  return SendableRegistry::GetInstance().GetSendable(it->getValue());
+  return wpi::SendableRegistry::GetSendable(it->getValue());
 }
 
-bool SmartDashboard::PutBoolean(wpi::StringRef keyName, bool value) {
-  return Singleton::GetInstance().table->GetEntry(keyName).SetBoolean(value);
+bool SmartDashboard::PutBoolean(std::string_view keyName, bool value) {
+  return GetInstance().table->GetEntry(keyName).SetBoolean(value);
 }
 
-bool SmartDashboard::SetDefaultBoolean(wpi::StringRef key, bool defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultBoolean(
-      defaultValue);
+bool SmartDashboard::SetDefaultBoolean(std::string_view key,
+                                       bool defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultBoolean(defaultValue);
 }
 
-bool SmartDashboard::GetBoolean(wpi::StringRef keyName, bool defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(keyName).GetBoolean(
-      defaultValue);
+bool SmartDashboard::GetBoolean(std::string_view keyName, bool defaultValue) {
+  return GetInstance().table->GetEntry(keyName).GetBoolean(defaultValue);
 }
 
-bool SmartDashboard::PutNumber(wpi::StringRef keyName, double value) {
-  return Singleton::GetInstance().table->GetEntry(keyName).SetDouble(value);
+bool SmartDashboard::PutNumber(std::string_view keyName, double value) {
+  return GetInstance().table->GetEntry(keyName).SetDouble(value);
 }
 
-bool SmartDashboard::SetDefaultNumber(wpi::StringRef key, double defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultDouble(
-      defaultValue);
+bool SmartDashboard::SetDefaultNumber(std::string_view key,
+                                      double defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultDouble(defaultValue);
 }
 
-double SmartDashboard::GetNumber(wpi::StringRef keyName, double defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(keyName).GetDouble(
-      defaultValue);
+double SmartDashboard::GetNumber(std::string_view keyName,
+                                 double defaultValue) {
+  return GetInstance().table->GetEntry(keyName).GetDouble(defaultValue);
 }
 
-bool SmartDashboard::PutString(wpi::StringRef keyName, wpi::StringRef value) {
-  return Singleton::GetInstance().table->GetEntry(keyName).SetString(value);
+bool SmartDashboard::PutString(std::string_view keyName,
+                               std::string_view value) {
+  return GetInstance().table->GetEntry(keyName).SetString(value);
 }
 
-bool SmartDashboard::SetDefaultString(wpi::StringRef key,
-                                      wpi::StringRef defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultString(
-      defaultValue);
+bool SmartDashboard::SetDefaultString(std::string_view key,
+                                      std::string_view defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultString(defaultValue);
 }
 
-std::string SmartDashboard::GetString(wpi::StringRef keyName,
-                                      wpi::StringRef defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(keyName).GetString(
-      defaultValue);
+std::string SmartDashboard::GetString(std::string_view keyName,
+                                      std::string_view defaultValue) {
+  return GetInstance().table->GetEntry(keyName).GetString(defaultValue);
 }
 
-bool SmartDashboard::PutBooleanArray(wpi::StringRef key,
-                                     wpi::ArrayRef<int> value) {
-  return Singleton::GetInstance().table->GetEntry(key).SetBooleanArray(value);
+bool SmartDashboard::PutBooleanArray(std::string_view key,
+                                     wpi::span<const int> value) {
+  return GetInstance().table->GetEntry(key).SetBooleanArray(value);
 }
 
-bool SmartDashboard::SetDefaultBooleanArray(wpi::StringRef key,
-                                            wpi::ArrayRef<int> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultBooleanArray(
+bool SmartDashboard::SetDefaultBooleanArray(std::string_view key,
+                                            wpi::span<const int> defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultBooleanArray(
       defaultValue);
 }
 
 std::vector<int> SmartDashboard::GetBooleanArray(
-    wpi::StringRef key, wpi::ArrayRef<int> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).GetBooleanArray(
-      defaultValue);
+    std::string_view key, wpi::span<const int> defaultValue) {
+  return GetInstance().table->GetEntry(key).GetBooleanArray(defaultValue);
 }
 
-bool SmartDashboard::PutNumberArray(wpi::StringRef key,
-                                    wpi::ArrayRef<double> value) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDoubleArray(value);
+bool SmartDashboard::PutNumberArray(std::string_view key,
+                                    wpi::span<const double> value) {
+  return GetInstance().table->GetEntry(key).SetDoubleArray(value);
 }
 
-bool SmartDashboard::SetDefaultNumberArray(wpi::StringRef key,
-                                           wpi::ArrayRef<double> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultDoubleArray(
-      defaultValue);
+bool SmartDashboard::SetDefaultNumberArray(
+    std::string_view key, wpi::span<const double> defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultDoubleArray(defaultValue);
 }
 
 std::vector<double> SmartDashboard::GetNumberArray(
-    wpi::StringRef key, wpi::ArrayRef<double> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).GetDoubleArray(
-      defaultValue);
+    std::string_view key, wpi::span<const double> defaultValue) {
+  return GetInstance().table->GetEntry(key).GetDoubleArray(defaultValue);
 }
 
-bool SmartDashboard::PutStringArray(wpi::StringRef key,
-                                    wpi::ArrayRef<std::string> value) {
-  return Singleton::GetInstance().table->GetEntry(key).SetStringArray(value);
+bool SmartDashboard::PutStringArray(std::string_view key,
+                                    wpi::span<const std::string> value) {
+  return GetInstance().table->GetEntry(key).SetStringArray(value);
 }
 
 bool SmartDashboard::SetDefaultStringArray(
-    wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultStringArray(
-      defaultValue);
+    std::string_view key, wpi::span<const std::string> defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultStringArray(defaultValue);
 }
 
 std::vector<std::string> SmartDashboard::GetStringArray(
-    wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).GetStringArray(
-      defaultValue);
+    std::string_view key, wpi::span<const std::string> defaultValue) {
+  return GetInstance().table->GetEntry(key).GetStringArray(defaultValue);
 }
 
-bool SmartDashboard::PutRaw(wpi::StringRef key, wpi::StringRef value) {
-  return Singleton::GetInstance().table->GetEntry(key).SetRaw(value);
+bool SmartDashboard::PutRaw(std::string_view key, std::string_view value) {
+  return GetInstance().table->GetEntry(key).SetRaw(value);
 }
 
-bool SmartDashboard::SetDefaultRaw(wpi::StringRef key,
-                                   wpi::StringRef defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultRaw(
-      defaultValue);
+bool SmartDashboard::SetDefaultRaw(std::string_view key,
+                                   std::string_view defaultValue) {
+  return GetInstance().table->GetEntry(key).SetDefaultRaw(defaultValue);
 }
 
-std::string SmartDashboard::GetRaw(wpi::StringRef key,
-                                   wpi::StringRef defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).GetRaw(defaultValue);
+std::string SmartDashboard::GetRaw(std::string_view key,
+                                   std::string_view defaultValue) {
+  return GetInstance().table->GetEntry(key).GetRaw(defaultValue);
 }
 
-bool SmartDashboard::PutValue(wpi::StringRef keyName,
+bool SmartDashboard::PutValue(std::string_view keyName,
                               std::shared_ptr<nt::Value> value) {
-  return Singleton::GetInstance().table->GetEntry(keyName).SetValue(value);
+  return GetInstance().table->GetEntry(keyName).SetValue(value);
 }
 
-bool SmartDashboard::SetDefaultValue(wpi::StringRef key,
+bool SmartDashboard::SetDefaultValue(std::string_view key,
                                      std::shared_ptr<nt::Value> defaultValue) {
-  return Singleton::GetInstance().table->GetEntry(key).SetDefaultValue(
-      defaultValue);
+  return GetInstance().table->GetEntry(key).SetDefaultValue(defaultValue);
 }
 
-std::shared_ptr<nt::Value> SmartDashboard::GetValue(wpi::StringRef keyName) {
-  return Singleton::GetInstance().table->GetEntry(keyName).GetValue();
+std::shared_ptr<nt::Value> SmartDashboard::GetValue(std::string_view keyName) {
+  return GetInstance().table->GetEntry(keyName).GetValue();
 }
 
-detail::ListenerExecutor SmartDashboard::listenerExecutor;
-
 void SmartDashboard::PostListenerTask(std::function<void()> task) {
-  listenerExecutor.Execute(task);
+  GetInstance().listenerExecutor.Execute(task);
 }
 
 void SmartDashboard::UpdateValues() {
-  auto& registry = SendableRegistry::GetInstance();
-  auto& inst = Singleton::GetInstance();
+  auto& inst = GetInstance();
+  inst.listenerExecutor.RunListenerTasks();
   std::scoped_lock lock(inst.tablesToDataMutex);
-  for (auto& i : inst.tablesToData) registry.Update(i.getValue());
-  listenerExecutor.RunListenerTasks();
+  for (auto& i : inst.tablesToData) {
+    wpi::SendableRegistry::Update(i.getValue());
+  }
 }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/cppcs/RobotBase.cpp b/third_party/allwpilib/wpilibc/src/main/native/cppcs/RobotBase.cpp
index 5f34e77..78c286c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/cppcs/RobotBase.cpp
+++ b/third_party/allwpilib/wpilibc/src/main/native/cppcs/RobotBase.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/RobotBase.h"
 
@@ -14,6 +11,7 @@
 #include <cstdio>
 
 #include <cameraserver/CameraServerShared.h>
+#include <fmt/format.h>
 #include <hal/FRCUsageReporting.h>
 #include <hal/HALBase.h>
 #include <networktables/NetworkTableInstance.h>
@@ -21,24 +19,37 @@
 
 #include "WPILibVersion.h"
 #include "frc/DriverStation.h"
+#include "frc/Errors.h"
+#include "frc/Notifier.h"
 #include "frc/RobotState.h"
-#include "frc/Utility.h"
-#include "frc/WPIErrors.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "frc/smartdashboard/SmartDashboard.h"
 
-typedef void (*SetCameraServerSharedFP)(frc::CameraServerShared* shared);
+static_assert(frc::RuntimeType::kRoboRIO ==
+              static_cast<frc::RuntimeType>(HAL_Runtime_RoboRIO));
+static_assert(frc::RuntimeType::kRoboRIO2 ==
+              static_cast<frc::RuntimeType>(HAL_Runtime_RoboRIO2));
+static_assert(frc::RuntimeType::kSimulation ==
+              static_cast<frc::RuntimeType>(HAL_Runtime_Simulation));
+
+using SetCameraServerSharedFP = void (*)(frc::CameraServerShared*);
 
 using namespace frc;
 
 int frc::RunHALInitialization() {
   if (!HAL_Initialize(500, 0)) {
-    wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
+    std::puts("FATAL ERROR: HAL could not be initialized");
     return -1;
   }
   HAL_Report(HALUsageReporting::kResourceType_Language,
              HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
-  wpi::outs() << "\n********** Robot program starting **********\n";
+
+  if (!frc::Notifier::SetHALThreadPriority(true, 40)) {
+    FRC_ReportError(warn::Warning, "{}",
+                    "Setting HAL Notifier RT priority to 40 failed\n");
+  }
+
+  std::puts("\n********** Robot program starting **********");
   return 0;
 }
 
@@ -56,14 +67,18 @@
   void ReportVideoServer(int id) override {
     HAL_Report(HALUsageReporting::kResourceType_PCVideoServer, id);
   }
-  void SetCameraServerError(const wpi::Twine& error) override {
-    wpi_setGlobalWPIErrorWithContext(CameraServerError, error);
+  void SetCameraServerErrorV(fmt::string_view format,
+                             fmt::format_args args) override {
+    ReportErrorV(err::CameraServerError, __FILE__, __LINE__, __FUNCTION__,
+                 format, args);
   }
-  void SetVisionRunnerError(const wpi::Twine& error) override {
-    wpi_setGlobalErrorWithContext(-1, error);
+  void SetVisionRunnerErrorV(fmt::string_view format,
+                             fmt::format_args args) override {
+    ReportErrorV(err::Error, __FILE__, __LINE__, __FUNCTION__, format, args);
   }
-  void ReportDriverStationError(const wpi::Twine& error) override {
-    DriverStation::ReportError(error);
+  void ReportDriverStationErrorV(fmt::string_view format,
+                                 fmt::format_args args) override {
+    ReportErrorV(err::Error, __FILE__, __LINE__, __FUNCTION__, format, args);
   }
   std::pair<std::thread::id, bool> GetRobotMainThreadId() const override {
     return std::make_pair(RobotBase::GetThreadId(), true);
@@ -71,8 +86,14 @@
 };
 class WPILibMathShared : public wpi::math::MathShared {
  public:
-  void ReportError(const wpi::Twine& error) override {
-    DriverStation::ReportError(error);
+  void ReportErrorV(fmt::string_view format, fmt::format_args args) override {
+    frc::ReportErrorV(err::Error, __FILE__, __LINE__, __FUNCTION__, format,
+                      args);
+  }
+
+  void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
+    frc::ReportErrorV(warn::Warning, __FILE__, __LINE__, __FUNCTION__, format,
+                      args);
   }
 
   void ReportUsage(wpi::math::MathUsageId id, int count) override {
@@ -107,6 +128,13 @@
         HAL_Report(HALUsageReporting::kResourceType_Odometry,
                    HALUsageReporting::kOdometry_MecanumDrive);
         break;
+      case wpi::math::MathUsageId::kController_PIDController2:
+        HAL_Report(HALUsageReporting::kResourceType_PIDController2, count);
+        break;
+      case wpi::math::MathUsageId::kController_ProfiledPIDController:
+        HAL_Report(HALUsageReporting::kResourceType_ProfiledPIDController,
+                   count);
+        break;
     }
   }
 };
@@ -122,8 +150,8 @@
 #endif
 
   if (!cameraServerLib) {
-    wpi::outs() << "Camera Server Library Not Found\n";
-    wpi::outs().flush();
+    std::puts("Camera Server Library Not Found");
+    std::fflush(stdout);
     return;
   }
   auto symbol = dlsym(cameraServerLib, "CameraServer_SetCameraServerShared");
@@ -131,15 +159,15 @@
     auto setCameraServerShared = (SetCameraServerSharedFP)symbol;
     setCameraServerShared(new WPILibCameraServerShared{});
   } else {
-    wpi::outs() << "Camera Server Shared Symbol Missing\n";
-    wpi::outs().flush();
+    std::puts("Camera Server Shared Symbol Missing");
+    std::fflush(stdout);
   }
 #else
   CameraServer_SetCameraServerShared(new WPILibCameraServerShared{});
 #endif
 #else
-  wpi::outs() << "Not loading CameraServerShared\n";
-  wpi::outs().flush();
+  std::puts("Not loading CameraServerShared");
+  std::fflush(stdout);
 #endif
 }
 
@@ -148,29 +176,55 @@
       std::make_unique<WPILibMathShared>());
 }
 
-bool RobotBase::IsEnabled() const { return m_ds.IsEnabled(); }
+bool RobotBase::IsEnabled() const {
+  return DriverStation::IsEnabled();
+}
 
-bool RobotBase::IsDisabled() const { return m_ds.IsDisabled(); }
+bool RobotBase::IsDisabled() const {
+  return DriverStation::IsDisabled();
+}
 
-bool RobotBase::IsAutonomous() const { return m_ds.IsAutonomous(); }
+bool RobotBase::IsAutonomous() const {
+  return DriverStation::IsAutonomous();
+}
 
 bool RobotBase::IsAutonomousEnabled() const {
-  return m_ds.IsAutonomousEnabled();
+  return DriverStation::IsAutonomousEnabled();
 }
 
-bool RobotBase::IsOperatorControl() const { return m_ds.IsOperatorControl(); }
+bool RobotBase::IsOperatorControl() const {
+  return DriverStation::IsTeleop();
+}
+
+bool RobotBase::IsTeleop() const {
+  return DriverStation::IsTeleop();
+}
 
 bool RobotBase::IsOperatorControlEnabled() const {
-  return m_ds.IsOperatorControlEnabled();
+  return DriverStation::IsTeleopEnabled();
 }
 
-bool RobotBase::IsTest() const { return m_ds.IsTest(); }
+bool RobotBase::IsTeleopEnabled() const {
+  return DriverStation::IsTeleopEnabled();
+}
 
-bool RobotBase::IsNewDataAvailable() const { return m_ds.IsNewControlData(); }
+bool RobotBase::IsTest() const {
+  return DriverStation::IsTest();
+}
 
-std::thread::id RobotBase::GetThreadId() { return m_threadId; }
+bool RobotBase::IsNewDataAvailable() const {
+  return DriverStation::IsNewControlData();
+}
 
-RobotBase::RobotBase() : m_ds(DriverStation::GetInstance()) {
+std::thread::id RobotBase::GetThreadId() {
+  return m_threadId;
+}
+
+RuntimeType RobotBase::GetRuntimeType() {
+  return static_cast<RuntimeType>(HAL_GetRuntimeType());
+}
+
+RobotBase::RobotBase() {
   m_threadId = std::this_thread::get_id();
 
   SetupCameraServerShared();
@@ -203,12 +257,5 @@
       ->GetEntry("LW Enabled")
       .SetBoolean(false);
 
-  LiveWindow::GetInstance()->SetEnabled(false);
+  LiveWindow::SetEnabled(false);
 }
-
-RobotBase::RobotBase(RobotBase&&) noexcept
-    : m_ds(DriverStation::GetInstance()) {}
-
-RobotBase::~RobotBase() {}
-
-RobotBase& RobotBase::operator=(RobotBase&&) noexcept { return *this; }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/WPILibVersion.h b/third_party/allwpilib/wpilibc/src/main/native/include/WPILibVersion.h
index 8aab880..c11bef9 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/WPILibVersion.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/WPILibVersion.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
index 202acbb..c997327 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_I2C.h
@@ -1,24 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/ErrorBase.h"
 #include "frc/I2C.h"
 #include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * ADXL345 Accelerometer on I2C.
  *
@@ -26,10 +20,9 @@
  * an I2C bus. This class assumes the default (not alternate) sensor address of
  * 0x1D (7-bit address).
  */
-class ADXL345_I2C : public ErrorBase,
-                    public Accelerometer,
-                    public Sendable,
-                    public SendableHelper<ADXL345_I2C> {
+class ADXL345_I2C : public Accelerometer,
+                    public nt::NTSendable,
+                    public wpi::SendableHelper<ADXL345_I2C> {
  public:
   enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
 
@@ -53,8 +46,11 @@
   ADXL345_I2C(ADXL345_I2C&&) = default;
   ADXL345_I2C& operator=(ADXL345_I2C&&) = default;
 
+  I2C::Port GetI2CPort() const;
+  int GetI2CDeviceAddress() const;
+
   // Accelerometer interface
-  void SetRange(Range range) override;
+  void SetRange(Range range) final;
   double GetX() override;
   double GetY() override;
   double GetZ() override;
@@ -75,7 +71,7 @@
    */
   virtual AllAxes GetAccelerations();
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(nt::NTSendableBuilder& builder) override;
 
  protected:
   I2C m_i2c;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
index 90454c0..18d48a3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL345_SPI.h
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/ErrorBase.h"
 #include "frc/SPI.h"
 #include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
@@ -23,10 +19,9 @@
  * This class allows access to an Analog Devices ADXL345 3-axis accelerometer
  * via SPI. This class assumes the sensor is wired in 4-wire SPI mode.
  */
-class ADXL345_SPI : public ErrorBase,
-                    public Accelerometer,
-                    public Sendable,
-                    public SendableHelper<ADXL345_SPI> {
+class ADXL345_SPI : public Accelerometer,
+                    public nt::NTSendable,
+                    public wpi::SendableHelper<ADXL345_SPI> {
  public:
   enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
 
@@ -49,8 +44,10 @@
   ADXL345_SPI(ADXL345_SPI&&) = default;
   ADXL345_SPI& operator=(ADXL345_SPI&&) = default;
 
+  SPI::Port GetSpiPort() const;
+
   // Accelerometer interface
-  void SetRange(Range range) override;
+  void SetRange(Range range) final;
   double GetX() override;
   double GetY() override;
   double GetZ() override;
@@ -71,7 +68,7 @@
    */
   virtual AllAxes GetAccelerations();
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(nt::NTSendableBuilder& builder) override;
 
  protected:
   SPI m_spi;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h
index e1d659b..451b5fb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXL362.h
@@ -1,33 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/SimDevice.h>
+#include <networktables/NTSendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/ErrorBase.h"
 #include "frc/SPI.h"
 #include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * ADXL362 SPI Accelerometer.
  *
  * This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
  */
-class ADXL362 : public ErrorBase,
-                public Accelerometer,
-                public Sendable,
-                public SendableHelper<ADXL362> {
+class ADXL362 : public Accelerometer,
+                public nt::NTSendable,
+                public wpi::SendableHelper<ADXL362> {
  public:
   enum Axes { kAxis_X = 0x00, kAxis_Y = 0x02, kAxis_Z = 0x04 };
   struct AllAxes {
@@ -57,8 +50,10 @@
   ADXL362(ADXL362&&) = default;
   ADXL362& operator=(ADXL362&&) = default;
 
+  SPI::Port GetSpiPort() const;
+
   // Accelerometer interface
-  void SetRange(Range range) override;
+  void SetRange(Range range) final;
   double GetX() override;
   double GetY() override;
   double GetZ() override;
@@ -79,7 +74,7 @@
    */
   virtual AllAxes GetAccelerations();
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(nt::NTSendableBuilder& builder) override;
 
  private:
   SPI m_spi;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
index 309fab3..cb0dff8 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
@@ -1,25 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
 #include <hal/SimDevice.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/GyroBase.h"
 #include "frc/SPI.h"
+#include "frc/interfaces/Gyro.h"
 
 namespace frc {
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position.
  *
- * The Gyro class tracks the robots heading based on the starting position. As
+ * The %Gyro class tracks the robots heading based on the starting position. As
  * the robot rotates the new heading is computed by integrating the rate of
  * rotation returned by the sensor. When the class is instantiated, it does a
  * short calibration routine where it samples the gyro while at rest to
@@ -27,17 +26,19 @@
  * 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.
+ * Only one instance of an ADXRS %Gyro is supported.
  */
-class ADXRS450_Gyro : public GyroBase {
+class ADXRS450_Gyro : public Gyro,
+                      public wpi::Sendable,
+                      public wpi::SendableHelper<ADXRS450_Gyro> {
  public:
   /**
-   * Gyro constructor on onboard CS0.
+   * %Gyro constructor on onboard CS0.
    */
   ADXRS450_Gyro();
 
   /**
-   * Gyro constructor on the specified SPI port.
+   * %Gyro constructor on the specified SPI port.
    *
    * @param port The SPI port the gyro is attached to.
    */
@@ -90,7 +91,7 @@
    * robot is first turned on while it's sitting at rest before the competition
    * starts.
    */
-  void Calibrate() override;
+  void Calibrate() final;
 
   /**
    * Get the SPI port number.
@@ -99,6 +100,8 @@
    */
   int GetPort() const;
 
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
  private:
   SPI m_spi;
   SPI::Port m_port;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h
index 3bcbab5..561eb37 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AddressableLED.h
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <memory>
+#include <initializer_list>
 
 #include <hal/AddressableLEDTypes.h>
 #include <hal/Types.h>
 #include <units/time.h>
-#include <wpi/ArrayRef.h>
+#include <wpi/span.h>
 
-#include "frc/ErrorBase.h"
 #include "util/Color.h"
 #include "util/Color8Bit.h"
 
@@ -25,7 +21,7 @@
  *
  * <p>Only 1 LED driver is currently supported by the roboRIO.
  */
-class AddressableLED : public ErrorBase {
+class AddressableLED {
  public:
   class LEDData : public HAL_AddressableLEDData {
    public:
@@ -89,7 +85,7 @@
    */
   explicit AddressableLED(int port);
 
-  ~AddressableLED() override;
+  ~AddressableLED();
 
   /**
    * Sets the length of the LED strip.
@@ -111,7 +107,7 @@
    *
    * @param ledData the buffer to write
    */
-  void SetData(wpi::ArrayRef<LEDData> ledData);
+  void SetData(wpi::span<const LEDData> ledData);
 
   /**
    * Sets the led output data.
@@ -144,7 +140,7 @@
    * <p>The sync time is the time to hold output so LEDs enable. Default set for
    * WS2812.
    *
-   * @param syncTimeMicroSeconds the sync time
+   * @param syncTime the sync time
    */
   void SetSyncTime(units::microsecond_t syncTime);
 
@@ -163,5 +159,6 @@
  private:
   hal::Handle<HAL_DigitalHandle> m_pwmHandle;
   hal::Handle<HAL_AddressableLEDHandle> m_handle;
+  int m_port;
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h
index 27015e4..b14ff7c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogAccelerometer.h
@@ -1,24 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
 #include "frc/AnalogInput.h"
-#include "frc/ErrorBase.h"
-#include "frc/PIDSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * Handle operation of an analog accelerometer.
  *
@@ -26,10 +20,8 @@
  * sensors have multiple axis and can be treated as multiple devices. Each is
  * calibrated by finding the center value over a period of time.
  */
-class AnalogAccelerometer : public ErrorBase,
-                            public PIDSource,
-                            public Sendable,
-                            public SendableHelper<AnalogAccelerometer> {
+class AnalogAccelerometer : public wpi::Sendable,
+                            public wpi::SendableHelper<AnalogAccelerometer> {
  public:
   /**
    * Create a new instance of an accelerometer.
@@ -100,14 +92,7 @@
    */
   void SetZero(double zero);
 
-  /**
-   * Get the Acceleration for the PID Source parent.
-   *
-   * @return The current acceleration in Gs.
-   */
-  double PIDGet() override;
-
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogEncoder.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogEncoder.h
index 3dc22ea..6f12cb8 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogEncoder.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogEncoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,12 +9,11 @@
 #include <hal/SimDevice.h>
 #include <hal/Types.h>
 #include <units/angle.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/Counter.h"
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 class AnalogInput;
@@ -25,11 +21,17 @@
 /**
  * Class for supporting continuous analog encoders, such as the US Digital MA3.
  */
-class AnalogEncoder : public ErrorBase,
-                      public Sendable,
-                      public SendableHelper<AnalogEncoder> {
+class AnalogEncoder : public wpi::Sendable,
+                      public wpi::SendableHelper<AnalogEncoder> {
  public:
   /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  explicit AnalogEncoder(int channel);
+
+  /**
    * Construct a new AnalogEncoder attached to a specific AnalogInput.
    *
    * @param analogInput the analog input to attach to
@@ -115,7 +117,7 @@
    */
   int GetChannel() const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   void Init();
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h
index b59df5b..6565c93 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogGyro.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
 #include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/GyroBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include "frc/interfaces/Gyro.h"
 
 namespace frc {
 
@@ -32,7 +29,9 @@
  *
  * This class is for gyro sensors that connect to an analog input.
  */
-class AnalogGyro : public GyroBase {
+class AnalogGyro : public Gyro,
+                   public wpi::Sendable,
+                   public wpi::SendableHelper<AnalogGyro> {
  public:
   static constexpr int kOversampleBits = 10;
   static constexpr int kAverageBits = 0;
@@ -41,7 +40,7 @@
   static constexpr double kDefaultVoltsPerDegreePerSecond = 0.007;
 
   /**
-   * Gyro constructor using the Analog Input channel number.
+   * %Gyro constructor using the Analog Input channel number.
    *
    * @param channel The analog channel the gyro is connected to. Gyros can only
    *                be used on on-board Analog Inputs 0-1.
@@ -63,7 +62,7 @@
   explicit AnalogGyro(AnalogInput* channel);
 
   /**
-   * Gyro constructor with a precreated AnalogInput object.
+   * %Gyro constructor with a precreated AnalogInput object.
    *
    * Use this constructor when the analog channel needs to be shared.
    * This object will not clean up the AnalogInput object when using this
@@ -75,7 +74,7 @@
   explicit AnalogGyro(std::shared_ptr<AnalogInput> channel);
 
   /**
-   * Gyro constructor using the Analog Input channel number with parameters for
+   * %Gyro constructor using the Analog Input channel number with parameters for
    * presetting the center and offset values. Bypasses calibration.
    *
    * @param channel The analog channel the gyro is connected to. Gyros can only
@@ -87,7 +86,7 @@
   AnalogGyro(int channel, int center, double offset);
 
   /**
-   * Gyro constructor with a precreated AnalogInput object and calibrated
+   * %Gyro constructor with a precreated AnalogInput object and calibrated
    * parameters.
    *
    * Use this constructor when the analog channel needs to be shared.
@@ -104,8 +103,8 @@
 
   ~AnalogGyro() override;
 
-  AnalogGyro(AnalogGyro&& rhs);
-  AnalogGyro& operator=(AnalogGyro&& rhs);
+  AnalogGyro(AnalogGyro&& rhs) = default;
+  AnalogGyro& operator=(AnalogGyro&& rhs) = default;
 
   /**
    * Return the actual angle in degrees that the robot is currently facing.
@@ -175,16 +174,16 @@
    * significant drift in the gyro and it needs to be recalibrated after it has
    * been running.
    */
-  void Reset() override;
+  void Reset() final;
 
   /**
    * Initialize the gyro.
    *
    * Calibration is handled by Calibrate().
    */
-  virtual void InitGyro();
+  void InitGyro();
 
-  void Calibrate() override;
+  void Calibrate() final;
 
   /**
    * Gets the analog input for the gyro.
@@ -193,6 +192,8 @@
    */
   std::shared_ptr<AnalogInput> GetAnalogInput() const;
 
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
  protected:
   std::shared_ptr<AnalogInput> m_analog;
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogInput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogInput.h
index 3d14c4c..ebc4e70 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogInput.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogInput.h
@@ -1,24 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
 #include <hal/Types.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/PIDSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
-class SendableBuilder;
 class DMA;
 class DMASample;
 
@@ -34,10 +27,8 @@
  * are divided by the number of samples to retain the resolution, but get more
  * stable values.
  */
-class AnalogInput : public ErrorBase,
-                    public PIDSource,
-                    public Sendable,
-                    public SendableHelper<AnalogInput> {
+class AnalogInput : public wpi::Sendable,
+                    public wpi::SendableHelper<AnalogInput> {
   friend class AnalogTrigger;
   friend class AnalogGyro;
   friend class DMA;
@@ -207,8 +198,7 @@
    *
    * This will be added to all values returned to the user.
    *
-   * @param initialValue The value that the accumulator should start from when
-   *                     reset.
+   * @param value The value that the accumulator should start from when reset.
    */
   void SetAccumulatorInitialValue(int64_t value);
 
@@ -284,20 +274,13 @@
   static double GetSampleRate();
 
   /**
-   * Get the Average value for the PID Source base object.
-   *
-   * @return The average voltage.
-   */
-  double PIDGet() override;
-
-  /**
    * Indicates this input is used by a simulated device.
    *
    * @param device simulated device handle
    */
   void SetSimDevice(HAL_SimDeviceHandle device);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   int m_channel;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h
index 026986b..ccb3c9c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogOutput.h
@@ -1,28 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/Types.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * MXP analog output class.
  */
-class AnalogOutput : public ErrorBase,
-                     public Sendable,
-                     public SendableHelper<AnalogOutput> {
+class AnalogOutput : public wpi::Sendable,
+                     public wpi::SendableHelper<AnalogOutput> {
  public:
   /**
    * Construct an analog output on the given channel.
@@ -57,7 +49,7 @@
    */
   int GetChannel() const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  protected:
   int m_channel;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
index ed943f7..3d0588d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogPotentiometer.h
@@ -1,34 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
 #include "frc/AnalogInput.h"
-#include "frc/ErrorBase.h"
-#include "frc/interfaces/Potentiometer.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * Class for reading analog potentiometers. Analog potentiometers read in an
  * analog voltage that corresponds to a position. The position is in whichever
  * units you choose, by way of the scaling and offset constants passed to the
  * constructor.
  */
-class AnalogPotentiometer : public ErrorBase,
-                            public Potentiometer,
-                            public Sendable,
-                            public SendableHelper<AnalogPotentiometer> {
+class AnalogPotentiometer : public wpi::Sendable,
+                            public wpi::SendableHelper<AnalogPotentiometer> {
  public:
   /**
    * Construct an Analog Potentiometer object from a channel number.
@@ -66,7 +58,7 @@
    * This will calculate the result from the fullRange times the fraction of the
    * supply voltage, plus the offset.
    *
-   * @param channel   The existing Analog Input pointer
+   * @param input     The existing Analog Input pointer
    * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
    * @param offset    The value (in desired units) representing the
@@ -88,7 +80,7 @@
    * This will calculate the result from the fullRange times the fraction of the
    * supply voltage, plus the offset.
    *
-   * @param channel   The existing Analog Input pointer
+   * @param input     The existing Analog Input pointer
    * @param fullRange The value (in desired units) representing the full
    *                  0-5V range of the input.
    * @param offset    The value (in desired units) representing the
@@ -108,16 +100,9 @@
    * @return The current position of the potentiometer (in the units used for
    *         fullRange and offset).
    */
-  double Get() const override;
+  double Get() const;
 
-  /**
-   * Implement the PIDSource interface.
-   *
-   * @return The current reading.
-   */
-  double PIDGet() override;
-
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   std::shared_ptr<AnalogInput> m_analog_input;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTrigger.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTrigger.h
index d465652..f9c5602 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTrigger.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTrigger.h
@@ -1,30 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
 #include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/AnalogTriggerOutput.h"
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
 class AnalogInput;
 class DutyCycle;
-class SendableBuilder;
 
-class AnalogTrigger : public ErrorBase,
-                      public Sendable,
-                      public SendableHelper<AnalogTrigger> {
+class AnalogTrigger : public wpi::Sendable,
+                      public wpi::SendableHelper<AnalogTrigger> {
   friend class AnalogTriggerOutput;
 
  public:
@@ -42,21 +36,21 @@
    * This should be used in the case of sharing an analog channel between the
    * trigger and an analog input object.
    *
-   * @param channel The pointer to the existing AnalogInput object
+   * @param input The pointer to the existing AnalogInput object
    */
-  explicit AnalogTrigger(AnalogInput* channel);
+  explicit AnalogTrigger(AnalogInput* input);
 
   /**
    * Construct an analog trigger given a duty cycle input.
    *
-   * @param channel The pointer to the existing DutyCycle object
+   * @param dutyCycle The pointer to the existing DutyCycle object
    */
   explicit AnalogTrigger(DutyCycle* dutyCycle);
 
   ~AnalogTrigger() override;
 
-  AnalogTrigger(AnalogTrigger&& rhs);
-  AnalogTrigger& operator=(AnalogTrigger&& rhs);
+  AnalogTrigger(AnalogTrigger&&) = default;
+  AnalogTrigger& operator=(AnalogTrigger&&) = default;
 
   /**
    * Set the upper and lower limits of the analog trigger.
@@ -154,9 +148,11 @@
   std::shared_ptr<AnalogTriggerOutput> CreateOutput(
       AnalogTriggerType type) const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
+  int GetSourceChannel() const;
+
   hal::Handle<HAL_AnalogTriggerHandle> m_trigger;
   AnalogInput* m_analogInput = nullptr;
   DutyCycle* m_dutyCycle = nullptr;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h
index 989a93f..6f52cab 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerOutput.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
 #include "frc/DigitalSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
@@ -47,8 +45,8 @@
  * may help with this, but rotational speeds of the sensor will then be limited.
  */
 class AnalogTriggerOutput : public DigitalSource,
-                            public Sendable,
-                            public SendableHelper<AnalogTriggerOutput> {
+                            public wpi::Sendable,
+                            public wpi::SendableHelper<AnalogTriggerOutput> {
   friend class AnalogTrigger;
 
  public:
@@ -80,7 +78,7 @@
    */
   int GetChannel() const override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  protected:
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h
index f15fb03..c706c34 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AnalogTriggerType.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/AsynchronousInterrupt.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AsynchronousInterrupt.h
new file mode 100644
index 0000000..4531dfd
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/AsynchronousInterrupt.h
@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/SynchronousInterrupt.h>
+
+#include <atomic>
+#include <functional>
+#include <memory>
+#include <thread>
+#include <utility>
+
+#include <units/time.h>
+
+namespace frc {
+/**
+ * Class for handling asynchronous interrupts using a callback thread.
+ *
+ * <p> By default, interrupts will occur on rising edge. Callbacks are disabled
+ * by default, and Enable() must be called before they will occur.
+ *
+ * <p> Both rising and falling edges can be indicated in one callback if both a
+ * rising and falling edge occurred since the previous callback.
+ *
+ * <p>Synchronous (blocking) interrupts are handled by the SynchronousInterrupt
+ * class.
+ */
+class AsynchronousInterrupt {
+ public:
+  /**
+   * Construct an Asynchronous Interrupt from a Digital Source.
+   *
+   * <p> At construction, the interrupt will trigger on the rising edge.
+   *
+   * <p> The first bool in the callback indicates the rising edge triggered the
+   * interrupt, the second bool is falling edge.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   * @param callback the callback function to call when the interrupt is
+   * triggered
+   */
+  AsynchronousInterrupt(DigitalSource& source,
+                        std::function<void(bool, bool)> callback);
+
+  /**
+   * Construct an Asynchronous Interrupt from a Digital Source.
+   *
+   * <p> At construction, the interrupt will trigger on the rising edge.
+   *
+   * <p> The first bool in the callback indicates the rising edge triggered the
+   * interrupt, the second bool is falling edge.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   * @param callback the callback function to call when the interrupt is
+   * triggered
+   */
+  AsynchronousInterrupt(DigitalSource* source,
+                        std::function<void(bool, bool)> callback);
+
+  /**
+   * Construct an Asynchronous Interrupt from a Digital Source.
+   *
+   * <p> At construction, the interrupt will trigger on the rising edge.
+   *
+   * <p> The first bool in the callback indicates the rising edge triggered the
+   * interrupt, the second bool is falling edge.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   * @param callback the callback function to call when the interrupt is
+   * triggered
+   */
+  AsynchronousInterrupt(std::shared_ptr<DigitalSource> source,
+                        std::function<void(bool, bool)> callback);
+
+  /**
+   * Construct an Asynchronous Interrupt from a Digital Source.
+   *
+   * <p> At construction, the interrupt will trigger on the rising edge.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   * @param f the callback function to call when the interrupt is triggered
+   * @param arg the first argument, interrupt was triggered on rising edge
+   * @param args the remaing arguments, interrupt was triggered on falling edge
+   */
+  template <typename Callable, typename Arg, typename... Args>
+  AsynchronousInterrupt(DigitalSource& source, Callable&& f, Arg&& arg,
+                        Args&&... args)
+      : AsynchronousInterrupt(
+            source, std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                              std::forward<Args>(args)...)) {}
+
+  /**
+   * Construct an Asynchronous Interrupt from a Digital Source.
+   *
+   * <p> At construction, the interrupt will trigger on the rising edge.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   * @param f the callback function to call when the interrupt is triggered
+   * @param arg the first argument, interrupt was triggered on rising edge
+   * @param args the remaing arguments, interrupt was triggered on falling edge
+   */
+  template <typename Callable, typename Arg, typename... Args>
+  AsynchronousInterrupt(DigitalSource* source, Callable&& f, Arg&& arg,
+                        Args&&... args)
+      : AsynchronousInterrupt(
+            source, std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                              std::forward<Args>(args)...)) {}
+
+  /**
+   * Construct an Asynchronous Interrupt from a Digital Source.
+   *
+   * <p> At construction, the interrupt will trigger on the rising edge.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   * @param f the callback function to call when the interrupt is triggered
+   * @param arg the first argument, interrupt was triggered on rising edge
+   * @param args the remaing arguments, interrupt was triggered on falling edge
+   */
+  template <typename Callable, typename Arg, typename... Args>
+  AsynchronousInterrupt(std::shared_ptr<DigitalSource> source, Callable&& f,
+                        Arg&& arg, Args&&... args)
+      : AsynchronousInterrupt(
+            source, std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
+                              std::forward<Args>(args)...)) {}
+
+  ~AsynchronousInterrupt();
+
+  /**
+   * Enables interrupt callbacks. Before this, callbacks will not occur. Does
+   * nothing if already enabled.
+   */
+  void Enable();
+
+  /**
+   * Disables interrupt callbacks. Does nothing if already disabled.
+   */
+  void Disable();
+
+  /**
+   * Set which edges to trigger the interrupt on.
+   *
+   * @param risingEdge %Trigger on rising edge
+   * @param fallingEdge %Trigger on falling edge
+   */
+  void SetInterruptEdges(bool risingEdge, bool fallingEdge);
+
+  /**
+   * Get the timestamp of the last rising edge.
+   *
+   * <p>This function does not require the interrupt to be enabled to work.
+   *
+   * <p>This only works if rising edge was configured using SetInterruptEdges.
+   * @return the timestamp in seconds relative to GetFPGATime
+   */
+  units::second_t GetRisingTimestamp();
+
+  /**
+   * Get the timestamp of the last falling edge.
+   *
+   * <p>This function does not require the interrupt to be enabled to work.
+   *
+   * <p>This only works if falling edge was configured using SetInterruptEdges.
+   * @return the timestamp in seconds relative to GetFPGATime
+   */
+  units::second_t GetFallingTimestamp();
+
+ private:
+  void ThreadMain();
+
+  std::atomic_bool m_keepRunning{false};
+  std::thread m_thread;
+  SynchronousInterrupt m_interrupt;
+  std::function<void(bool, bool)> m_callback;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Base.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Base.h
deleted file mode 100644
index 1fdbe5d..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Base.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#if !defined(__clang__) && defined(__GNUC__) && __GNUC__ < 5
-static_assert(0,
-              "GCC must be 5 or greater. If building for the roboRIO, please "
-              "update to the 2018 toolchains.");
-#endif
-
-#if defined(_MSC_VER) && _MSC_VER < 1900
-static_assert(0, "Visual Studio 2015 or greater required.");
-#endif
-
-/** WPILib FRC namespace */
-namespace frc {
-
-// A struct to use as a deleter when a std::shared_ptr must wrap a raw pointer
-// that is being deleted by someone else.
-template <class T>
-struct NullDeleter {
-  void operator()(T*) const noexcept {};
-};
-
-}  // namespace frc
-
-// For backwards compatibility
-#ifdef NO_NAMESPACED_WPILIB
-using namespace frc;  // NOLINT
-#endif
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
index 8148e72..0e5bee6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/BuiltInAccelerometer.h
@@ -1,30 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include "frc/ErrorBase.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
 #include "frc/interfaces/Accelerometer.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * Built-in accelerometer.
  *
  * This class allows access to the roboRIO's internal accelerometer.
  */
-class BuiltInAccelerometer : public ErrorBase,
-                             public Accelerometer,
-                             public Sendable,
-                             public SendableHelper<BuiltInAccelerometer> {
+class BuiltInAccelerometer : public Accelerometer,
+                             public wpi::Sendable,
+                             public wpi::SendableHelper<BuiltInAccelerometer> {
  public:
   /**
    * Constructor.
@@ -44,7 +38,7 @@
    *              accelerometer will measure. Not all accelerometers support all
    *              ranges.
    */
-  void SetRange(Range range) override;
+  void SetRange(Range range) final;
 
   /**
    * @return The acceleration of the roboRIO along the X axis in g-forces
@@ -61,7 +55,7 @@
    */
   double GetZ() override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h
index 612d533..4c9b9bd 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CAN.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,8 +8,6 @@
 
 #include <hal/CANAPITypes.h>
 
-#include "frc/ErrorBase.h"
-
 namespace frc {
 struct CANData {
   uint8_t data[8];
@@ -30,7 +25,7 @@
  * All methods are thread save, however the buffer objects passed in
  * by the user need to not be modified for the duration of their calls.
  */
-class CAN : public ErrorBase {
+class CAN {
  public:
   /**
    * Create a new CAN communication interface with the specific device ID.
@@ -55,7 +50,7 @@
   /**
    * Closes the CAN communication.
    */
-  ~CAN() override;
+  ~CAN();
 
   CAN(CAN&&) = default;
   CAN& operator=(CAN&&) = default;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Compressor.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Compressor.h
index d1a4dca..cc065ff 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Compressor.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Compressor.h
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <hal/Types.h>
+#include <memory>
 
-#include "frc/ErrorBase.h"
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
 #include "frc/SensorUtil.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
- * Class for operating a compressor connected to a %PCM (Pneumatic Control
- * Module).
+ * Class for operating a compressor connected to a pneumatics module.
  *
- * The PCM will automatically run in closed loop mode by default whenever a
+ * The module will automatically run in closed loop mode by default whenever a
  * Solenoid object is created. For most cases, a Compressor object does not need
  * to be instantiated or used in a robot program. This class is only required in
  * cases where the robot program needs a more detailed status of the compressor
@@ -33,18 +30,28 @@
  * loop control. You can only turn off closed loop control, thereby stopping
  * the compressor from operating.
  */
-class Compressor : public ErrorBase,
-                   public Sendable,
-                   public SendableHelper<Compressor> {
+class Compressor : public wpi::Sendable,
+                   public wpi::SendableHelper<Compressor> {
  public:
   /**
-   * Constructor. The default PCM ID is 0.
+   * Constructs a compressor for a specified module and type.
    *
-   * @param module The PCM ID to use (0-62)
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
    */
-  explicit Compressor(int pcmID = SensorUtil::GetDefaultSolenoidModule());
+  Compressor(int module, PneumaticsModuleType moduleType);
 
-  ~Compressor() override = default;
+  /**
+   * Constructs a compressor for a default module and specified type.
+   *
+   * @param moduleType The module type to use.
+   */
+  explicit Compressor(PneumaticsModuleType moduleType);
+
+  ~Compressor() override;
+
+  Compressor(const Compressor&) = delete;
+  Compressor& operator=(const Compressor&) = delete;
 
   Compressor(Compressor&&) = default;
   Compressor& operator=(Compressor&&) = default;
@@ -100,92 +107,10 @@
    */
   bool GetClosedLoopControl() const;
 
-  /**
-   * Query if the compressor output has been disabled due to high current draw.
-   *
-   * @return true if PCM is in fault state : Compressor Drive is
-   *         disabled due to compressor current being too high.
-   */
-  bool GetCompressorCurrentTooHighFault() const;
-
-  /**
-   * Query if the compressor output has been disabled due to high current draw
-   * (sticky).
-   *
-   * A sticky fault will not clear on device reboot, it must be cleared through
-   * code or the webdash.
-   *
-   * @return true if PCM sticky fault is set : Compressor Drive is
-   *         disabled due to compressor current being too high.
-   */
-  bool GetCompressorCurrentTooHighStickyFault() const;
-
-  /**
-   * Query if the compressor output has been disabled due to a short circuit
-   * (sticky).
-   *
-   * A sticky fault will not clear on device reboot, it must be cleared through
-   * code or the webdash.
-   *
-   * @return true if PCM sticky fault is set : Compressor output
-   *         appears to be shorted.
-   */
-  bool GetCompressorShortedStickyFault() const;
-
-  /**
-   * Query if the compressor output has been disabled due to a short circuit.
-   *
-   * @return true if PCM is in fault state : Compressor output
-   *         appears to be shorted.
-   */
-  bool GetCompressorShortedFault() const;
-
-  /**
-   * Query if the compressor output does not appear to be wired (sticky).
-   *
-   * A sticky fault will not clear on device reboot, it must be cleared through
-   * code or the webdash.
-   *
-   * @return true if PCM sticky fault is set : Compressor does not
-   *         appear to be wired, i.e. compressor is not drawing enough current.
-   */
-  bool GetCompressorNotConnectedStickyFault() const;
-
-  /**
-   * Query if the compressor output does not appear to be wired.
-   *
-   * @return true if PCM is in fault state : Compressor does not
-   *         appear to be wired, i.e. compressor is not drawing enough current.
-   */
-  bool GetCompressorNotConnectedFault() const;
-
-  /**
-   * Clear ALL sticky faults inside PCM that Compressor is wired to.
-   *
-   * If a sticky fault is set, then it will be persistently cleared.  Compressor
-   * drive maybe momentarily disable while flags are being cleared. Care should
-   * be taken to not call this too frequently, otherwise normal compressor
-   * functionality may be prevented.
-   *
-   * If no sticky faults are set then this call will have no effect.
-   */
-  void ClearAllPCMStickyFaults();
-
-  /**
-   * Gets module number (CAN ID).
-   *
-   * @return Module number
-   */
-  int GetModule() const;
-
-  void InitSendable(SendableBuilder& builder) override;
-
- protected:
-  hal::Handle<HAL_CompressorHandle> m_compressorHandle;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  void SetCompressor(bool on);
-  int m_module;
+  std::shared_ptr<PneumaticsBase> m_module;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Controller.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Controller.h
index c6b25ab..d750a0e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Controller.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Controller.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h
index 2770a02..d501b7e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Counter.h
@@ -1,26 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
 #include <hal/Types.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/CounterBase.h"
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
 class DigitalGlitchFilter;
-class SendableBuilder;
 class DMA;
 class DMASample;
 
@@ -34,10 +30,9 @@
  * All counters will immediately start counting - Reset() them if you need them
  * to be zeroed before use.
  */
-class Counter : public ErrorBase,
-                public CounterBase,
-                public Sendable,
-                public SendableHelper<Counter> {
+class Counter : public CounterBase,
+                public wpi::Sendable,
+                public wpi::SendableHelper<Counter> {
   friend class DMA;
   friend class DMASample;
 
@@ -82,7 +77,7 @@
    *
    * This is used if an existing digital input is to be shared by multiple other
    * objects such as encoders or if the Digital Source is not a Digital Input
-   * channel (such as an Analog Trigger).
+   * channel (such as an Analog %Trigger).
    *
    * The counter will start counting immediately.
    * @param source A pointer to the existing DigitalSource object. It will be
@@ -96,7 +91,7 @@
    *
    * This is used if an existing digital input is to be shared by multiple other
    * objects such as encoders or if the Digital Source is not a Digital Input
-   * channel (such as an Analog Trigger).
+   * channel (such as an Analog %Trigger).
    *
    * The counter will start counting immediately.
    *
@@ -373,7 +368,7 @@
    *
    * @returns The period between the last two pulses in units of seconds.
    */
-  double GetPeriod() const override;
+  units::second_t GetPeriod() const override;
 
   /**
    * Set the maximum period where the device is still considered "moving".
@@ -385,7 +380,7 @@
    * @param maxPeriod The maximum period where the counted device is considered
    *                  moving in seconds.
    */
-  void SetMaxPeriod(double maxPeriod) override;
+  void SetMaxPeriod(units::second_t maxPeriod) final;
 
   /**
    * Select whether you want to continue updating the event timer output when
@@ -425,7 +420,7 @@
    */
   bool GetDirection() const override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  protected:
   // Makes the counter count up.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CounterBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CounterBase.h
index 7fde3ac..33febbf 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/CounterBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/CounterBase.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <units/time.h>
+
 namespace frc {
 
 /**
@@ -30,8 +29,8 @@
 
   virtual int Get() const = 0;
   virtual void Reset() = 0;
-  virtual double GetPeriod() const = 0;
-  virtual void SetMaxPeriod(double maxPeriod) = 0;
+  virtual units::second_t GetPeriod() const = 0;
+  virtual void SetMaxPeriod(units::second_t maxPeriod) = 0;
   virtual bool GetStopped() const = 0;
   virtual bool GetDirection() const = 0;
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h
index 57cdd27..1bbf268 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMA.h
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/Types.h>
-
-#include "frc/ErrorBase.h"
+#include <units/time.h>
 
 namespace frc {
 class Encoder;
@@ -18,19 +14,22 @@
 class DutyCycle;
 class AnalogInput;
 class DMASample;
+class PWM;
+class PWMMotorController;
 
-class DMA : public ErrorBase {
+class DMA {
   friend class DMASample;
 
  public:
   DMA();
-  ~DMA() override;
+  ~DMA();
 
   DMA& operator=(DMA&& other) = default;
   DMA(DMA&& other) = default;
 
   void SetPause(bool pause);
-  void SetRate(int cycles);
+  void SetTimedTrigger(units::second_t seconds);
+  void SetTimedTriggerCycles(int cycles);
 
   void AddEncoder(const Encoder* encoder);
   void AddEncoderPeriod(const Encoder* encoder);
@@ -46,10 +45,15 @@
   void AddAveragedAnalogInput(const AnalogInput* analogInput);
   void AddAnalogAccumulator(const AnalogInput* analogInput);
 
-  void SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
+  int SetExternalTrigger(DigitalSource* source, bool rising, bool falling);
+  int SetPwmEdgeTrigger(PWM* pwm, bool rising, bool falling);
+  int SetPwmEdgeTrigger(PWMMotorController* pwm, bool rising, bool falling);
 
-  void StartDMA(int queueDepth);
-  void StopDMA();
+  void ClearSensors();
+  void ClearExternalTriggers();
+
+  void Start(int queueDepth);
+  void Stop();
 
  private:
   hal::Handle<HAL_DMAHandle> dmaHandle;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h
index c286c8d..48a0a9e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMASample.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -22,11 +19,16 @@
 namespace frc {
 class DMASample : public HAL_DMASample {
  public:
-  HAL_DMAReadStatus Update(const DMA* dma, units::second_t timeout,
-                           int32_t* remaining, int32_t* status) {
-    units::millisecond_t ms = timeout;
-    auto timeoutMs = ms.to<int32_t>();
-    return HAL_ReadDMA(dma->dmaHandle, this, timeoutMs, remaining, status);
+  enum class DMAReadStatus {
+    kOk = HAL_DMA_OK,
+    kTimeout = HAL_DMA_TIMEOUT,
+    kError = HAL_DMA_ERROR
+  };
+
+  DMAReadStatus Update(const DMA* dma, units::second_t timeout,
+                       int32_t* remaining, int32_t* status) {
+    return static_cast<DMAReadStatus>(
+        HAL_ReadDMA(dma->dmaHandle, this, timeout.value(), remaining, status));
   }
 
   uint64_t GetTime() const { return timeStamp; }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMC60.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMC60.h
deleted file mode 100644
index ecf01e1..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DMC60.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Digilent DMC 60 Speed Controller.
- *
- * Note that the DMC 60 uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the DMC 60 User
- * Manual available from Digilent.
- *
- * \li 2.004ms = full "forward"
- * \li 1.520ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.480ms = the "low end" of the deadband range
- * \li 0.997ms = full "reverse"
- */
-class DMC60 : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a Digilent DMC 60.
-   *
-   * @param channel The PWM channel that the DMC 60 is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit DMC60(int channel);
-
-  DMC60(DMC60&&) = default;
-  DMC60& operator=(DMC60&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DSControlWord.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DSControlWord.h
new file mode 100644
index 0000000..fb0709f
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DSControlWord.h
@@ -0,0 +1,102 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/DriverStationTypes.h>
+
+namespace frc {
+
+/**
+ * A wrapper around Driver Station control word.
+ */
+class DSControlWord {
+ public:
+  /**
+   * DSControlWord constructor.
+   *
+   * Upon construction, the current Driver Station control word is read and
+   * stored internally.
+   */
+  DSControlWord();
+
+  /**
+   * Check if the DS has enabled the robot.
+   *
+   * @return True if the robot is enabled and the DS is connected
+   */
+  bool IsEnabled() const;
+
+  /**
+   * Check if the robot is disabled.
+   *
+   * @return True if the robot is explicitly disabled or the DS is not connected
+   */
+  bool IsDisabled() const;
+
+  /**
+   * Check if the robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped
+   */
+  bool IsEStopped() const;
+
+  /**
+   * Check if the DS is commanding autonomous mode.
+   *
+   * @return True if the robot is being commanded to be in autonomous mode
+   */
+  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
+   */
+  bool IsTeleop() 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 IsTeleopEnabled() const;
+
+  /**
+   * Check if the DS is commanding test mode.
+   *
+   * @return True if the robot is being commanded to be in test mode
+   */
+  bool IsTest() const;
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return True if the DS is connected to the robot
+   */
+  bool IsDSAttached() const;
+
+  /**
+   * Is the driver station attached to a Field Management System?
+   *
+   * @return True if the robot is competing on a field being controlled by a
+   *         Field Management System
+   */
+  bool IsFMSAttached() const;
+
+ private:
+  HAL_ControlWord m_controlWord;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Debouncer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Debouncer.h
new file mode 100644
index 0000000..8f583f0
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Debouncer.h
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/time.h>
+
+#include "frc/Timer.h"
+
+namespace frc {
+/**
+ * A simple debounce filter for boolean streams.  Requires that the boolean
+ * change value from baseline for a specified period of time before the filtered
+ * value changes.
+ */
+class Debouncer {
+ public:
+  enum DebounceType { kRising, kFalling, kBoth };
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounceTime The number of seconds the value must change from
+   *                     baseline for the filtered value to change.
+   * @param type         Which type of state change the debouncing will be
+   *                     performed on.
+   */
+  explicit Debouncer(units::second_t debounceTime,
+                     DebounceType type = DebounceType::kRising);
+
+  /**
+   * Applies the debouncer to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return The debounced value of the input stream.
+   */
+  bool Calculate(bool input);
+
+ private:
+  frc::Timer m_timer;
+  units::second_t m_debounceTime;
+  bool m_baseline;
+  DebounceType m_debounceType;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h
index 0690e53..7172f21 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalGlitchFilter.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,11 +9,10 @@
 #include <array>
 
 #include <wpi/mutex.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/DigitalSource.h"
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
@@ -30,15 +26,14 @@
  * filter. The filter lets the user configure the time that an input must remain
  * high or low before it is classified as high or low.
  */
-class DigitalGlitchFilter : public ErrorBase,
-                            public Sendable,
-                            public SendableHelper<DigitalGlitchFilter> {
+class DigitalGlitchFilter : public wpi::Sendable,
+                            public wpi::SendableHelper<DigitalGlitchFilter> {
  public:
   DigitalGlitchFilter();
   ~DigitalGlitchFilter() override;
 
-  DigitalGlitchFilter(DigitalGlitchFilter&& rhs);
-  DigitalGlitchFilter& operator=(DigitalGlitchFilter&& rhs);
+  DigitalGlitchFilter(DigitalGlitchFilter&&) = default;
+  DigitalGlitchFilter& operator=(DigitalGlitchFilter&&) = default;
 
   /**
    * Assigns the DigitalSource to this glitch filter.
@@ -119,7 +114,7 @@
    */
   uint64_t GetPeriodNanoSeconds();
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   // Sets the filter for the input to be the requested index. A value of 0
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalInput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalInput.h
index 33aa716..cee31fd 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalInput.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalInput.h
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
 #include "frc/DigitalSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
 class DigitalGlitchFilter;
-class SendableBuilder;
 
 /**
  * Class to read a digital input.
@@ -26,8 +23,8 @@
  * implemented anywhere else.
  */
 class DigitalInput : public DigitalSource,
-                     public Sendable,
-                     public SendableHelper<DigitalInput> {
+                     public wpi::Sendable,
+                     public wpi::SendableHelper<DigitalInput> {
  public:
   /**
    * Create an instance of a Digital Input class.
@@ -78,7 +75,7 @@
    */
   void SetSimDevice(HAL_SimDeviceHandle device);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   int m_channel;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalOutput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalOutput.h
index 1d1d152..0e124f1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalOutput.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalOutput.h
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/DigitalSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * Class to write to digital outputs.
  *
@@ -25,8 +20,8 @@
  * shouldn't be done here.
  */
 class DigitalOutput : public DigitalSource,
-                      public Sendable,
-                      public SendableHelper<DigitalOutput> {
+                      public wpi::Sendable,
+                      public wpi::SendableHelper<DigitalOutput> {
  public:
   /**
    * Create an instance of a digital output.
@@ -148,7 +143,7 @@
    */
   void SetSimDevice(HAL_SimDeviceHandle device);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   int m_channel;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalSource.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalSource.h
index 5d77daa..0dd35d6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalSource.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DigitalSource.h
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <hal/Types.h>
 
-#include "frc/InterruptableSensorBase.h"
+#include "frc/AnalogTriggerType.h"
 
 namespace frc {
 
@@ -22,7 +19,7 @@
  * constructed and freed when finished for the source. The source can either be
  * a digital input or analog trigger but not both.
  */
-class DigitalSource : public InterruptableSensorBase {
+class DigitalSource {
  public:
   DigitalSource() = default;
   DigitalSource(DigitalSource&&) = default;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
index 302c52c..381e3a1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DoubleSolenoid.h
@@ -1,53 +1,54 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <hal/Types.h>
+#include <memory>
 
-#include "frc/SolenoidBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * DoubleSolenoid class for running 2 channels of high voltage Digital Output
- * (PCM).
+ * on a pneumatics module.
  *
  * The DoubleSolenoid class is typically used for pneumatics solenoids that
  * have two positions controlled by two separate channels.
  */
-class DoubleSolenoid : public SolenoidBase,
-                       public Sendable,
-                       public SendableHelper<DoubleSolenoid> {
+class DoubleSolenoid : public wpi::Sendable,
+                       public wpi::SendableHelper<DoubleSolenoid> {
  public:
   enum Value { kOff, kForward, kReverse };
 
   /**
-   * Constructor.
+   * Constructs a double solenoid for a specified module of a specific module
+   * type.
    *
-   * Uses the default PCM ID of 0.
-   *
-   * @param forwardChannel The forward channel number on the PCM (0..7).
-   * @param reverseChannel The reverse channel number on the PCM (0..7).
+   * @param module The module of the solenoid module to use.
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
    */
-  explicit DoubleSolenoid(int forwardChannel, int reverseChannel);
+  DoubleSolenoid(int module, PneumaticsModuleType moduleType,
+                 int forwardChannel, int reverseChannel);
 
   /**
-   * Constructor.
+   * Constructs a double solenoid for a default module of a specific module
+   * type.
    *
-   * @param moduleNumber   The CAN ID of the PCM.
-   * @param forwardChannel The forward channel on the PCM to control (0..7).
-   * @param reverseChannel The reverse channel on the PCM to control (0..7).
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
    */
-  DoubleSolenoid(int moduleNumber, int forwardChannel, int reverseChannel);
+  DoubleSolenoid(PneumaticsModuleType moduleType, int forwardChannel,
+                 int reverseChannel);
 
   ~DoubleSolenoid() override;
 
@@ -78,36 +79,50 @@
   void Toggle();
 
   /**
-   * Check if the forward solenoid is blacklisted.
+   * Get the forward channel.
    *
-   * If a solenoid is shorted, it is added to the blacklist and disabled until
-   * power cycle, or until faults are cleared.
-   *
-   * @see ClearAllPCMStickyFaults()
-   * @return If solenoid is disabled due to short.
+   * @return the forward channel.
    */
-  bool IsFwdSolenoidBlackListed() const;
+  int GetFwdChannel() const;
 
   /**
-   * Check if the reverse solenoid is blacklisted.
+   * Get the reverse channel.
    *
-   * If a solenoid is shorted, it is added to the blacklist and disabled until
-   * power cycle, or until faults are cleared.
+   * @return the reverse channel.
+   */
+  int GetRevChannel() const;
+
+  /**
+   * Check if the forward solenoid is Disabled.
+   *
+   * If a solenoid is shorted, it is added to the DisabledList and disabled
+   * until power cycle, or until faults are cleared.
    *
    * @see ClearAllPCMStickyFaults()
    * @return If solenoid is disabled due to short.
    */
-  bool IsRevSolenoidBlackListed() const;
+  bool IsFwdSolenoidDisabled() const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  /**
+   * Check if the reverse solenoid is Disabled.
+   *
+   * If a solenoid is shorted, it is added to the DisabledList and disabled
+   * until power cycle, or until faults are cleared.
+   *
+   * @see ClearAllPCMStickyFaults()
+   * @return If solenoid is disabled due to short.
+   */
+  bool IsRevSolenoidDisabled() const;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
+  std::shared_ptr<PneumaticsBase> m_module;
   int m_forwardChannel;  // The forward channel on the module to control.
   int m_reverseChannel;  // The reverse channel on the module to control.
   int m_forwardMask;     // The mask for the forward channel.
   int m_reverseMask;     // The mask for the reverse channel.
-  hal::Handle<HAL_SolenoidHandle> m_forwardHandle;
-  hal::Handle<HAL_SolenoidHandle> m_reverseHandle;
+  int m_mask;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h
index a6c40d6..30bf4fc 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DriverStation.h
@@ -1,102 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
-#include <atomic>
-#include <memory>
 #include <string>
-#include <thread>
 
-#include <hal/DriverStationTypes.h>
-#include <wpi/Twine.h>
-#include <wpi/condition_variable.h>
-#include <wpi/mutex.h>
-
-#include "frc/ErrorBase.h"
+#include <units/time.h>
+#include <wpi/deprecated.h>
 
 namespace frc {
 
-class MatchDataSender;
-
 /**
  * Provide access to the network communication data to / from the Driver
  * Station.
  */
-class DriverStation : public ErrorBase {
+class DriverStation {
  public:
   enum Alliance { kRed, kBlue, kInvalid };
   enum MatchType { kNone, kPractice, kQualification, kElimination };
 
-  ~DriverStation() override;
-
-  DriverStation(const DriverStation&) = delete;
-  DriverStation& operator=(const DriverStation&) = delete;
-
   /**
    * Return a reference to the singleton DriverStation.
    *
    * @return Reference to the DS instance
+   * @deprecated Use the static methods
    */
+  WPI_DEPRECATED("Use static methods")
   static DriverStation& GetInstance();
 
-  /**
-   * Report an error to the DriverStation messages window.
-   *
-   * The error is also printed to the program console.
-   */
-  static void ReportError(const wpi::Twine& error);
-
-  /**
-   * Report a warning to the DriverStation messages window.
-   *
-   * The warning is also printed to the program console.
-   */
-  static void ReportWarning(const wpi::Twine& error);
-
-  /**
-   * Report an error to the DriverStation messages window.
-   *
-   * The error is also printed to the program console.
-   */
-  static void ReportError(bool isError, int code, const wpi::Twine& error,
-                          const wpi::Twine& location, const wpi::Twine& stack);
-
   static constexpr int kJoystickPorts = 6;
 
   /**
-   * The state of one joystick button. Button indexes begin at 1.
+   * The state of one joystick button. %Button indexes begin at 1.
    *
    * @param stick  The joystick to read.
    * @param button The button index, beginning at 1.
    * @return The state of the joystick button.
    */
-  bool GetStickButton(int stick, int button);
+  static bool GetStickButton(int stick, int button);
 
   /**
-   * Whether one joystick button was pressed since the last check. Button
+   * Whether one joystick button was pressed since the last check. %Button
    * indexes begin at 1.
    *
    * @param stick  The joystick to read.
    * @param button The button index, beginning at 1.
    * @return Whether the joystick button was pressed since the last check.
    */
-  bool GetStickButtonPressed(int stick, int button);
+  static bool GetStickButtonPressed(int stick, int button);
 
   /**
-   * Whether one joystick button was released since the last check. Button
+   * Whether one joystick button was released since the last check. %Button
    * indexes begin at 1.
    *
    * @param stick  The joystick to read.
    * @param button The button index, beginning at 1.
    * @return Whether the joystick button was released since the last check.
    */
-  bool GetStickButtonReleased(int stick, int button);
+  static bool GetStickButtonReleased(int stick, int button);
 
   /**
    * Get the value of the axis on a joystick.
@@ -108,14 +70,14 @@
    * @param axis  The analog axis value to read from the joystick.
    * @return The value of the axis on the joystick.
    */
-  double GetStickAxis(int stick, int axis);
+  static double GetStickAxis(int stick, int axis);
 
   /**
    * Get the state of a POV on the joystick.
    *
    * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
    */
-  int GetStickPOV(int stick, int pov);
+  static int GetStickPOV(int stick, int pov);
 
   /**
    * The state of the buttons on the joystick.
@@ -123,7 +85,7 @@
    * @param stick The joystick to read.
    * @return The state of the buttons on the joystick.
    */
-  int GetStickButtons(int stick) const;
+  static int GetStickButtons(int stick);
 
   /**
    * Returns the number of axes on a given joystick port.
@@ -131,7 +93,7 @@
    * @param stick The joystick port number
    * @return The number of axes on the indicated joystick
    */
-  int GetStickAxisCount(int stick) const;
+  static int GetStickAxisCount(int stick);
 
   /**
    * Returns the number of POVs on a given joystick port.
@@ -139,7 +101,7 @@
    * @param stick The joystick port number
    * @return The number of POVs on the indicated joystick
    */
-  int GetStickPOVCount(int stick) const;
+  static int GetStickPOVCount(int stick);
 
   /**
    * Returns the number of buttons on a given joystick port.
@@ -147,7 +109,7 @@
    * @param stick The joystick port number
    * @return The number of buttons on the indicated joystick
    */
-  int GetStickButtonCount(int stick) const;
+  static int GetStickButtonCount(int stick);
 
   /**
    * Returns a boolean indicating if the controller is an xbox controller.
@@ -155,7 +117,7 @@
    * @param stick The joystick port number
    * @return A boolean that is true if the controller is an xbox controller.
    */
-  bool GetJoystickIsXbox(int stick) const;
+  static bool GetJoystickIsXbox(int stick);
 
   /**
    * Returns the type of joystick at a given port.
@@ -163,7 +125,7 @@
    * @param stick The joystick port number
    * @return The HID type of joystick at the given port
    */
-  int GetJoystickType(int stick) const;
+  static int GetJoystickType(int stick);
 
   /**
    * Returns the name of the joystick at the given port.
@@ -171,15 +133,16 @@
    * @param stick The joystick port number
    * @return The name of the joystick at the given port
    */
-  std::string GetJoystickName(int stick) const;
+  static std::string GetJoystickName(int stick);
 
   /**
    * Returns the types of Axes on a given joystick port.
    *
    * @param stick The joystick port number and the target axis
+   * @param axis  The analog axis value to read from the joystick.
    * @return What type of axis the axis is reporting to be
    */
-  int GetJoystickAxisType(int stick, int axis) const;
+  static int GetJoystickAxisType(int stick, int axis);
 
   /**
    * Returns if a joystick is connected to the Driver Station.
@@ -190,35 +153,35 @@
    * @param stick The joystick port number
    * @return true if a joystick is connected
    */
-  bool IsJoystickConnected(int stick) const;
+  static bool IsJoystickConnected(int stick);
 
   /**
    * Check if the DS has enabled the robot.
    *
    * @return True if the robot is enabled and the DS is connected
    */
-  bool IsEnabled() const;
+  static bool IsEnabled();
 
   /**
    * Check if the robot is disabled.
    *
    * @return True if the robot is explicitly disabled or the DS is not connected
    */
-  bool IsDisabled() const;
+  static bool IsDisabled();
 
   /**
    * Check if the robot is e-stopped.
    *
    * @return True if the robot is e-stopped
    */
-  bool IsEStopped() const;
+  static bool IsEStopped();
 
   /**
    * Check if the DS is commanding autonomous mode.
    *
    * @return True if the robot is being commanded to be in autonomous mode
    */
-  bool IsAutonomous() const;
+  static bool IsAutonomous();
 
   /**
    * Check if the DS is commanding autonomous mode and if it has enabled the
@@ -227,14 +190,33 @@
    * @return True if the robot is being commanded to be in autonomous mode and
    * enabled.
    */
-  bool IsAutonomousEnabled() const;
+  static bool IsAutonomousEnabled();
+
+  /**
+   * Check if the DS is commanding teleop mode.
+   *
+   * @return True if the robot is being commanded to be in teleop mode
+   * @deprecated Use IsTeleop() instead.
+   */
+  WPI_DEPRECATED("Use IsTeleop() instead")
+  static bool IsOperatorControl();
 
   /**
    * Check if the DS is commanding teleop mode.
    *
    * @return True if the robot is being commanded to be in teleop mode
    */
-  bool IsOperatorControl() const;
+  static bool IsTeleop();
+
+  /**
+   * 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.
+   * @deprecated Use IsTeleopEnabled() instead.
+   */
+  WPI_DEPRECATED("Use IsTeleopEnabled() instead")
+  static bool IsOperatorControlEnabled();
 
   /**
    * Check if the DS is commanding teleop mode and if it has enabled the robot.
@@ -242,21 +224,21 @@
    * @return True if the robot is being commanded to be in teleop mode and
    * enabled.
    */
-  bool IsOperatorControlEnabled() const;
+  static bool IsTeleopEnabled();
 
   /**
    * Check if the DS is commanding test mode.
    *
    * @return True if the robot is being commanded to be in test mode
    */
-  bool IsTest() const;
+  static bool IsTest();
 
   /**
    * Check if the DS is attached.
    *
    * @return True if the DS is connected to the robot
    */
-  bool IsDSAttached() const;
+  static bool IsDSAttached();
 
   /**
    * Has a new control packet from the driver station arrived since the last
@@ -267,7 +249,7 @@
    *
    * @return True if the control data has been updated since the last call.
    */
-  bool IsNewControlData() const;
+  static bool IsNewControlData();
 
   /**
    * Is the driver station attached to a Field Management System?
@@ -275,21 +257,21 @@
    * @return True if the robot is competing on a field being controlled by a
    *         Field Management System
    */
-  bool IsFMSAttached() const;
+  static bool IsFMSAttached();
 
   /**
    * Returns the game specific message provided by the FMS.
    *
    * @return A string containing the game specific message.
    */
-  std::string GetGameSpecificMessage() const;
+  static std::string GetGameSpecificMessage();
 
   /**
    * Returns the name of the competition event provided by the FMS.
    *
    * @return A string containing the event name
    */
-  std::string GetEventName() const;
+  static std::string GetEventName();
 
   /**
    * Returns the type of match being played provided by the FMS.
@@ -297,14 +279,14 @@
    * @return The match type enum (kNone, kPractice, kQualification,
    *         kElimination)
    */
-  MatchType GetMatchType() const;
+  static MatchType GetMatchType();
 
   /**
    * Returns the match number provided by the FMS.
    *
    * @return The number of the match
    */
-  int GetMatchNumber() const;
+  static int GetMatchNumber();
 
   /**
    * Returns the number of times the current match has been replayed from the
@@ -312,7 +294,7 @@
    *
    * @return The number of replays
    */
-  int GetReplayNumber() const;
+  static int GetReplayNumber();
 
   /**
    * Return the alliance that the driver station says it is on.
@@ -321,7 +303,7 @@
    *
    * @return The Alliance enum (kRed, kBlue or kInvalid)
    */
-  Alliance GetAlliance() const;
+  static Alliance GetAlliance();
 
   /**
    * Return the driver station location on the field.
@@ -330,7 +312,7 @@
    *
    * @return The location of the driver station (1-3, 0 for invalid)
    */
-  int GetLocation() const;
+  static int GetLocation();
 
   /**
    * Wait until a new packet comes from the driver station.
@@ -343,7 +325,7 @@
    * 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();
+  static void WaitForData();
 
   /**
    * Wait until a new packet comes from the driver station, or wait for a
@@ -361,11 +343,11 @@
    * This is a good way to delay processing until there is new driver station
    * data to act on.
    *
-   * @param timeout Timeout time in seconds
+   * @param timeout Timeout
    *
    * @return true if new data, otherwise false
    */
-  bool WaitForData(double timeout);
+  static bool WaitForData(units::second_t timeout);
 
   /**
    * Return the approximate match time.
@@ -382,14 +364,14 @@
    *
    * @return Time remaining in current match period (auto or teleop)
    */
-  double GetMatchTime() const;
+  static double GetMatchTime();
 
   /**
    * Read the battery voltage.
    *
    * @return The battery voltage in Volts.
    */
-  double GetBatteryVoltage() const;
+  static double GetBatteryVoltage();
 
   /**
    * Only to be used to tell the Driver Station what code you claim to be
@@ -398,7 +380,7 @@
    * @param entering If true, starting disabled code; if false, leaving disabled
    *                 code.
    */
-  void InDisabled(bool entering) { m_userInDisabled = entering; }
+  static void InDisabled(bool entering);
 
   /**
    * Only to be used to tell the Driver Station what code you claim to be
@@ -407,7 +389,18 @@
    * @param entering If true, starting autonomous code; if false, leaving
    *                 autonomous code.
    */
-  void InAutonomous(bool entering) { m_userInAutonomous = entering; }
+  static void InAutonomous(bool entering);
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be
+   * executing for diagnostic purposes only.
+   *
+   * @param entering If true, starting teleop code; if false, leaving teleop
+   *                 code.
+   * @deprecated Use InTeleop() instead.
+   */
+  WPI_DEPRECATED("Use InTeleop() instead")
+  static void InOperatorControl(bool entering);
 
   /**
    * Only to be used to tell the Driver Station what code you claim to be
@@ -416,7 +409,7 @@
    * @param entering If true, starting teleop code; if false, leaving teleop
    *                 code.
    */
-  void InOperatorControl(bool entering) { m_userInTeleop = entering; }
+  static void InTeleop(bool entering);
 
   /**
    * Only to be used to tell the Driver Station what code you claim to be
@@ -424,12 +417,12 @@
    *
    * @param entering If true, starting test code; if false, leaving test code.
    */
-  void InTest(bool entering) { m_userInTest = entering; }
+  static void InTest(bool entering);
 
   /**
    * Forces WaitForData() to return immediately.
    */
-  void WakeupWaitForData();
+  static void WakeupWaitForData();
 
   /**
    * Allows the user to specify whether they want joystick connection warnings
@@ -438,7 +431,7 @@
    *
    * @param silence Whether warning messages should be silenced.
    */
-  void SilenceJoystickConnectionWarning(bool silence);
+  static void SilenceJoystickConnectionWarning(bool silence);
 
   /**
    * Returns whether joystick connection warnings are silenced. This will
@@ -446,68 +439,10 @@
    *
    * @return Whether joystick connection warnings are silenced.
    */
-  bool IsJoystickConnectionWarningSilenced() const;
-
- protected:
-  /**
-   * Copy data from the DS task for the user.
-   *
-   * If no new data exists, it will just be returned, otherwise
-   * the data will be copied from the DS polling loop.
-   */
-  void GetData();
+  static bool IsJoystickConnectionWarningSilenced();
 
  private:
-  /**
-   * DriverStation constructor.
-   *
-   * This is only called once the first time GetInstance() is called
-   */
-  DriverStation();
-
-  /**
-   * Reports errors related to unplugged joysticks.
-   *
-   * Throttles the errors so that they don't overwhelm the DS.
-   */
-  void ReportJoystickUnpluggedError(const wpi::Twine& message);
-
-  /**
-   * Reports errors related to unplugged joysticks.
-   *
-   * Throttles the errors so that they don't overwhelm the DS.
-   */
-  void ReportJoystickUnpluggedWarning(const wpi::Twine& message);
-
-  void Run();
-
-  void SendMatchData();
-
-  std::unique_ptr<MatchDataSender> m_matchDataSender;
-
-  // Joystick button rising/falling edge flags
-  wpi::mutex m_buttonEdgeMutex;
-  std::array<HAL_JoystickButtons, kJoystickPorts> m_previousButtonStates;
-  std::array<uint32_t, kJoystickPorts> m_joystickButtonsPressed;
-  std::array<uint32_t, kJoystickPorts> m_joystickButtonsReleased;
-
-  // Internal Driver Station thread
-  std::thread m_dsThread;
-  std::atomic<bool> m_isRunning{false};
-
-  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;
-  bool m_userInTeleop = false;
-  bool m_userInTest = false;
-
-  double m_nextMessageTime = 0;
+  DriverStation() = default;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycle.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycle.h
index f0fc2d8..7f45d06 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycle.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycle.h
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
 #include <hal/Types.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 class DigitalSource;
@@ -32,9 +27,7 @@
  * order to implement rollover checking.
  *
  */
-class DutyCycle : public ErrorBase,
-                  public Sendable,
-                  public SendableHelper<DutyCycle> {
+class DutyCycle : public wpi::Sendable, public wpi::SendableHelper<DutyCycle> {
   friend class AnalogTrigger;
   friend class DMA;
   friend class DMASample;
@@ -125,7 +118,7 @@
   int GetSourceChannel() const;
 
  protected:
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   void InitDutyCycle();
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
index 17b038f..ccf56ad 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/DutyCycleEncoder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,12 +9,11 @@
 #include <hal/SimDevice.h>
 #include <hal/Types.h>
 #include <units/angle.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/AnalogTrigger.h"
 #include "frc/Counter.h"
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 class DutyCycle;
@@ -28,9 +24,8 @@
  * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
  * Encoder.
  */
-class DutyCycleEncoder : public ErrorBase,
-                         public Sendable,
-                         public SendableHelper<DutyCycleEncoder> {
+class DutyCycleEncoder : public wpi::Sendable,
+                         public wpi::SendableHelper<DutyCycleEncoder> {
  public:
   /**
    * Construct a new DutyCycleEncoder on a specific channel.
@@ -63,21 +58,21 @@
   /**
    * Construct a new DutyCycleEncoder attached to a DigitalSource object.
    *
-   * @param source the digital source to attach to
+   * @param digitalSource the digital source to attach to
    */
   explicit DutyCycleEncoder(DigitalSource& digitalSource);
 
   /**
    * Construct a new DutyCycleEncoder attached to a DigitalSource object.
    *
-   * @param source the digital source to attach to
+   * @param digitalSource the digital source to attach to
    */
   explicit DutyCycleEncoder(DigitalSource* digitalSource);
 
   /**
    * Construct a new DutyCycleEncoder attached to a DigitalSource object.
    *
-   * @param source the digital source to attach to
+   * @param digitalSource the digital source to attach to
    */
   explicit DutyCycleEncoder(std::shared_ptr<DigitalSource> digitalSource);
 
@@ -168,7 +163,7 @@
    */
   int GetSourceChannel() const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   void Init();
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h
index 76f9d1c..f6753c2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Encoder.h
@@ -1,28 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
 #include <hal/Types.h>
+#include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/Counter.h"
 #include "frc/CounterBase.h"
-#include "frc/ErrorBase.h"
-#include "frc/PIDSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
 class DigitalSource;
 class DigitalGlitchFilter;
-class SendableBuilder;
 class DMA;
 class DMASample;
 
@@ -41,11 +36,9 @@
  * All encoders will immediately start counting - Reset() them if you need them
  * to be zeroed before use.
  */
-class Encoder : public ErrorBase,
-                public CounterBase,
-                public PIDSource,
-                public Sendable,
-                public SendableHelper<Encoder> {
+class Encoder : public CounterBase,
+                public wpi::Sendable,
+                public wpi::SendableHelper<Encoder> {
   friend class DMA;
   friend class DMASample;
 
@@ -174,7 +167,7 @@
    *
    * @return Period in seconds of the most recent pulse.
    */
-  double GetPeriod() const override;
+  units::second_t GetPeriod() const override;
 
   /**
    * Sets the maximum period for stopped detection.
@@ -195,7 +188,7 @@
   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;
+  void SetMaxPeriod(units::second_t maxPeriod) override;
 
   /**
    * Determine if the encoder is stopped.
@@ -317,8 +310,6 @@
    */
   int GetSamplesToAverage() const;
 
-  double PIDGet() override;
-
   /**
    * Set the index source for the encoder.
    *
@@ -334,8 +325,8 @@
    *
    * When this source is activated, the encoder count automatically resets.
    *
-   * @param channel A digital source to set as the encoder index
-   * @param type    The state that will cause the encoder to reset
+   * @param source A digital source to set as the encoder index
+   * @param type   The state that will cause the encoder to reset
    */
   void SetIndexSource(const DigitalSource& source,
                       IndexingType type = kResetOnRisingEdge);
@@ -349,7 +340,7 @@
 
   int GetFPGAIndex() const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   /**
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Error.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Error.h
deleted file mode 100644
index d63fb62..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Error.h
+++ /dev/null
@@ -1,66 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <string>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#ifdef _WIN32
-#pragma push_macro("GetMessage")
-#undef GetMessage
-#endif
-
-namespace frc {
-
-class ErrorBase;
-
-/**
- * Error object represents a library error.
- */
-class Error {
- public:
-  using Code = int;
-
-  Error() = default;
-  Error(Code code, const wpi::Twine& contextMessage, wpi::StringRef filename,
-        wpi::StringRef function, int lineNumber,
-        const ErrorBase* originatingObject);
-
-  bool operator<(const Error& rhs) const;
-
-  Code GetCode() const;
-  std::string GetMessage() const;
-  std::string GetFilename() const;
-  std::string GetFunction() const;
-  int GetLineNumber() const;
-  const ErrorBase* GetOriginatingObject() const;
-  double GetTimestamp() const;
-  void Clear();
-  void Set(Code code, const wpi::Twine& contextMessage, wpi::StringRef filename,
-           wpi::StringRef function, int lineNumber,
-           const ErrorBase* originatingObject);
-
- private:
-  void Report();
-
-  Code m_code = 0;
-  std::string m_message;
-  std::string m_filename;
-  std::string m_function;
-  int m_lineNumber = 0;
-  const ErrorBase* m_originatingObject = nullptr;
-  double m_timestamp = 0.0;
-};
-
-}  // namespace frc
-
-#ifdef _WIN32
-#pragma pop_macro("GetMessage")
-#endif
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ErrorBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ErrorBase.h
deleted file mode 100644
index 0ced9a2..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ErrorBase.h
+++ /dev/null
@@ -1,242 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <vector>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#include "frc/Error.h"
-
-// Forward declared manually to avoid needing to pull in entire HAL header.
-extern "C" const char* HAL_GetErrorMessage(int32_t code);
-
-#define wpi_setErrnoErrorWithContext(context) \
-  this->SetErrnoError((context), __FILE__, __FUNCTION__, __LINE__)
-#define wpi_setErrnoError() wpi_setErrnoErrorWithContext("")
-#define wpi_setImaqErrorWithContext(code, context)                             \
-  do {                                                                         \
-    if ((code) != 0)                                                           \
-      this->SetImaqError((code), (context), __FILE__, __FUNCTION__, __LINE__); \
-  } while (0)
-#define wpi_setErrorWithContext(code, context)                             \
-  do {                                                                     \
-    if ((code) != 0)                                                       \
-      this->SetError((code), (context), __FILE__, __FUNCTION__, __LINE__); \
-  } while (0)
-#define wpi_setErrorWithContextRange(code, min, max, req, context)          \
-  do {                                                                      \
-    if ((code) != 0)                                                        \
-      this->SetErrorRange((code), (min), (max), (req), (context), __FILE__, \
-                          __FUNCTION__, __LINE__);                          \
-  } while (0)
-
-#define wpi_setHALError(code)                                     \
-  do {                                                            \
-    if ((code) != 0)                                              \
-      this->SetError((code), HAL_GetErrorMessage(code), __FILE__, \
-                     __FUNCTION__, __LINE__);                     \
-  } while (0)
-
-#define wpi_setHALErrorWithRange(code, min, max, req)                        \
-  do {                                                                       \
-    if ((code) != 0)                                                         \
-      this->SetErrorRange((code), (min), (max), (req),                       \
-                          HAL_GetErrorMessage(code), __FILE__, __FUNCTION__, \
-                          __LINE__);                                         \
-  } while (0)
-
-#define wpi_setError(code) wpi_setErrorWithContext(code, "")
-#define wpi_setStaticErrorWithContext(object, code, context)                 \
-  do {                                                                       \
-    if ((code) != 0)                                                         \
-      object->SetError((code), (context), __FILE__, __FUNCTION__, __LINE__); \
-  } while (0)
-#define wpi_setStaticError(object, code) \
-  wpi_setStaticErrorWithContext(object, code, "")
-
-#define wpi_setGlobalErrorWithContext(code, context)                \
-  do {                                                              \
-    if ((code) != 0)                                                \
-      ::frc::ErrorBase::SetGlobalError((code), (context), __FILE__, \
-                                       __FUNCTION__, __LINE__);     \
-  } while (0)
-
-#define wpi_setGlobalHALError(code)                                       \
-  do {                                                                    \
-    if ((code) != 0)                                                      \
-      ::frc::ErrorBase::SetGlobalError((code), HAL_GetErrorMessage(code), \
-                                       __FILE__, __FUNCTION__, __LINE__); \
-  } while (0)
-
-#define wpi_setGlobalError(code) wpi_setGlobalErrorWithContext(code, "")
-#define wpi_setWPIErrorWithContext(error, context)                    \
-  this->SetWPIError(wpi_error_s_##error(), wpi_error_value_##error(), \
-                    (context), __FILE__, __FUNCTION__, __LINE__)
-#define wpi_setWPIError(error) (wpi_setWPIErrorWithContext(error, ""))
-#define wpi_setStaticWPIErrorWithContext(object, error, context)  \
-  object->SetWPIError(wpi_error_s_##error(), (context), __FILE__, \
-                      __FUNCTION__, __LINE__)
-#define wpi_setStaticWPIError(object, error) \
-  wpi_setStaticWPIErrorWithContext(object, error, "")
-#define wpi_setGlobalWPIErrorWithContext(error, context)                \
-  ::frc::ErrorBase::SetGlobalWPIError(wpi_error_s_##error(), (context), \
-                                      __FILE__, __FUNCTION__, __LINE__)
-#define wpi_setGlobalWPIError(error) wpi_setGlobalWPIErrorWithContext(error, "")
-
-namespace frc {
-
-/**
- * Base class for most objects.
- *
- * ErrorBase is the base class for most objects since it holds the generated
- * error for that object. In addition, there is a single instance of a global
- * error object.
- */
-class ErrorBase {
-  // TODO: Consider initializing instance variables and cleanup in destructor
- public:
-  ErrorBase();
-  virtual ~ErrorBase() = default;
-
-  ErrorBase(const ErrorBase&) = default;
-  ErrorBase& operator=(const ErrorBase&) = default;
-  ErrorBase(ErrorBase&&) = default;
-  ErrorBase& operator=(ErrorBase&&) = default;
-
-  /**
-   * @brief Retrieve the current error.
-   *
-   * Get the current error information associated with this sensor.
-   */
-  virtual Error& GetError();
-
-  /**
-   * @brief Retrieve the current error.
-   *
-   * Get the current error information associated with this sensor.
-   */
-  virtual const Error& GetError() const;
-
-  /**
-   * @brief Clear the current error information associated with this sensor.
-   */
-  virtual void ClearError() const;
-
-  /**
-   * @brief Set error information associated with a C library call that set an
-   *        error to the "errno" global variable.
-   *
-   * @param contextMessage A custom message from the code that set the error.
-   * @param filename       Filename of the error source
-   * @param function       Function of the error source
-   * @param lineNumber     Line number of the error source
-   */
-  virtual void SetErrnoError(const wpi::Twine& contextMessage,
-                             wpi::StringRef filename, wpi::StringRef function,
-                             int lineNumber) const;
-
-  /**
-   * @brief Set the current error information associated from the nivision Imaq
-   *        API.
-   *
-   * @param success        The return from the function
-   * @param contextMessage A custom message from the code that set the error.
-   * @param filename       Filename of the error source
-   * @param function       Function of the error source
-   * @param lineNumber     Line number of the error source
-   */
-  virtual void SetImaqError(int success, const wpi::Twine& contextMessage,
-                            wpi::StringRef filename, wpi::StringRef function,
-                            int lineNumber) const;
-
-  /**
-   * @brief Set the current error information associated with this sensor.
-   *
-   * @param code           The error code
-   * @param contextMessage A custom message from the code that set the error.
-   * @param filename       Filename of the error source
-   * @param function       Function of the error source
-   * @param lineNumber     Line number of the error source
-   */
-  virtual void SetError(Error::Code code, const wpi::Twine& contextMessage,
-                        wpi::StringRef filename, wpi::StringRef function,
-                        int lineNumber) const;
-
-  /**
-   * @brief Set the current error information associated with this sensor.
-   * Range versions use for initialization code.
-   *
-   * @param code           The error code
-   * @param minRange       The minimum allowed allocation range
-   * @param maxRange       The maximum allowed allocation range
-   * @param requestedValue The requested value to allocate
-   * @param contextMessage A custom message from the code that set the error.
-   * @param filename       Filename of the error source
-   * @param function       Function of the error source
-   * @param lineNumber     Line number of the error source
-   */
-  virtual void SetErrorRange(Error::Code code, int32_t minRange,
-                             int32_t maxRange, int32_t requestedValue,
-                             const wpi::Twine& contextMessage,
-                             wpi::StringRef filename, wpi::StringRef function,
-                             int lineNumber) const;
-
-  /**
-   * @brief Set the current error information associated with this sensor.
-   *
-   * @param errorMessage   The error message from WPIErrors.h
-   * @param contextMessage A custom message from the code that set the error.
-   * @param filename       Filename of the error source
-   * @param function       Function of the error source
-   * @param lineNumber     Line number of the error source
-   */
-  virtual void SetWPIError(const wpi::Twine& errorMessage, Error::Code code,
-                           const wpi::Twine& contextMessage,
-                           wpi::StringRef filename, wpi::StringRef function,
-                           int lineNumber) const;
-
-  virtual void CloneError(const ErrorBase& rhs) const;
-
-  /**
-   * @brief Check if the current error code represents a fatal error.
-   *
-   * @return true if the current error is fatal.
-   */
-  virtual bool StatusIsFatal() const;
-
-  static void SetGlobalError(Error::Code code, const wpi::Twine& contextMessage,
-                             wpi::StringRef filename, wpi::StringRef function,
-                             int lineNumber);
-
-  static void SetGlobalWPIError(const wpi::Twine& errorMessage,
-                                const wpi::Twine& contextMessage,
-                                wpi::StringRef filename,
-                                wpi::StringRef function, int lineNumber);
-
-  /**
-   * Retrieve the last global error.
-   */
-  static Error GetGlobalError();
-
-  /**
-   * Retrieve all global errors.
-   */
-  static std::vector<Error> GetGlobalErrors();
-
-  /**
-   * Clear global errors.
-   */
-  void ClearGlobalErrors();
-
- protected:
-  mutable Error m_error;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Errors.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Errors.h
new file mode 100644
index 0000000..84edc18
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Errors.h
@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+
+#include <fmt/format.h>
+
+namespace frc {
+
+/**
+ * Runtime error exception.
+ */
+class RuntimeError : public std::runtime_error {
+ public:
+  RuntimeError(int32_t code, std::string&& loc, std::string&& stack,
+               std::string&& message);
+  RuntimeError(int32_t code, const char* fileName, int lineNumber,
+               const char* funcName, std::string&& stack,
+               std::string&& message);
+
+  int32_t code() const noexcept { return m_data->code; }
+  const char* loc() const noexcept { return m_data->loc.c_str(); }
+  const char* stack() const noexcept { return m_data->stack.c_str(); }
+
+  /**
+   * Reports error to Driver Station (using HAL_SendError).
+   */
+  void Report() const;
+
+ private:
+  struct Data {
+    int32_t code;
+    std::string loc;
+    std::string stack;
+  };
+  std::shared_ptr<Data> m_data;
+};
+
+/**
+ * Gets error message string for an error code.
+ */
+const char* GetErrorMessage(int32_t* code);
+
+/**
+ * Reports an error to the driver station (using HAL_SendError).
+ * Generally the FRC_ReportError wrapper macro should be used instead.
+ *
+ * @param[out] status error code
+ * @param[in]  fileName source file name
+ * @param[in]  lineNumber source line number
+ * @param[in]  funcName source function name
+ * @param[in]  format error message format
+ * @param[in]  args error message format args
+ */
+void ReportErrorV(int32_t status, const char* fileName, int lineNumber,
+                  const char* funcName, fmt::string_view format,
+                  fmt::format_args args);
+
+/**
+ * Reports an error to the driver station (using HAL_SendError).
+ * Generally the FRC_ReportError wrapper macro should be used instead.
+ *
+ * @param[out] status error code
+ * @param[in]  fileName source file name
+ * @param[in]  lineNumber source line number
+ * @param[in]  funcName source function name
+ * @param[in]  format error message format
+ * @param[in]  args error message format args
+ */
+template <typename S, typename... Args>
+inline void ReportError(int32_t status, const char* fileName, int lineNumber,
+                        const char* funcName, const S& format, Args&&... args) {
+  ReportErrorV(status, fileName, lineNumber, funcName, format,
+               fmt::make_args_checked<Args...>(format, args...));
+}
+
+/**
+ * Makes a runtime error exception object. This object should be thrown
+ * by the caller. Generally the FRC_MakeError wrapper macro should be used
+ * instead.
+ *
+ * @param[out] status error code
+ * @param[in]  fileName source file name
+ * @param[in]  lineNumber source line number
+ * @param[in]  funcName source function name
+ * @param[in]  format error message format
+ * @param[in]  args error message format args
+ * @return runtime error object
+ */
+[[nodiscard]] RuntimeError MakeErrorV(int32_t status, const char* fileName,
+                                      int lineNumber, const char* funcName,
+                                      fmt::string_view format,
+                                      fmt::format_args args);
+
+template <typename S, typename... Args>
+[[nodiscard]] inline RuntimeError MakeError(int32_t status,
+                                            const char* fileName,
+                                            int lineNumber,
+                                            const char* funcName,
+                                            const S& format, Args&&... args) {
+  return MakeErrorV(status, fileName, lineNumber, funcName, format,
+                    fmt::make_args_checked<Args...>(format, args...));
+}
+
+namespace err {
+#define S(label, offset, message) inline constexpr int label = offset;
+#include "frc/WPIErrors.mac"
+#undef S
+}  // namespace err
+
+namespace warn {
+#define S(label, offset, message) inline constexpr int label = offset;
+#include "frc/WPIWarnings.mac"
+#undef S
+}  // namespace warn
+}  // namespace frc
+
+/**
+ * Reports an error to the driver station (using HAL_SendError).
+ *
+ * @param[out] status error code
+ * @param[in]  format error message format
+ */
+#define FRC_ReportError(status, format, ...)                       \
+  do {                                                             \
+    if ((status) != 0) {                                           \
+      ::frc::ReportError(status, __FILE__, __LINE__, __FUNCTION__, \
+                         FMT_STRING(format), __VA_ARGS__);         \
+    }                                                              \
+  } while (0)
+
+/**
+ * Makes a runtime error exception object. This object should be thrown
+ * by the caller.
+ *
+ * @param[out] status error code
+ * @param[in]  format error message format
+ * @return runtime error object
+ */
+#define FRC_MakeError(status, format, ...)                   \
+  ::frc::MakeError(status, __FILE__, __LINE__, __FUNCTION__, \
+                   FMT_STRING(format), __VA_ARGS__)
+
+/**
+ * Checks a status code and depending on its value, either throws a
+ * RuntimeError exception, calls ReportError, or does nothing (if no error).
+ *
+ * @param[out] status error code
+ * @param[in]  format error message format
+ */
+#define FRC_CheckErrorStatus(status, format, ...)                      \
+  do {                                                                 \
+    if ((status) < 0) {                                                \
+      throw ::frc::MakeError(status, __FILE__, __LINE__, __FUNCTION__, \
+                             FMT_STRING(format), __VA_ARGS__);         \
+    } else if ((status) > 0) {                                         \
+      ::frc::ReportError(status, __FILE__, __LINE__, __FUNCTION__,     \
+                         FMT_STRING(format), __VA_ARGS__);             \
+    }                                                                  \
+  } while (0)
+
+#define FRC_AssertMessage(condition, format, ...)                            \
+  do {                                                                       \
+    if (!(condition)) {                                                      \
+      throw ::frc::MakeError(err::AssertionFailure, __FILE__, __LINE__,      \
+                             __FUNCTION__, FMT_STRING(format), __VA_ARGS__); \
+    }                                                                        \
+  } while (0)
+
+#define FRC_Assert(condition) FRC_AssertMessage(condition, "{}", #condition)
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Filesystem.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Filesystem.h
index f196f7a..fbfc681 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Filesystem.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Filesystem.h
@@ -1,34 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/SmallVector.h>
+#include <string>
 
-namespace frc {
 /** WPILib FileSystem namespace */
-namespace filesystem {
+namespace frc::filesystem {
 
 /**
  * Obtains the current working path that the program was launched with.
  * This is analogous to the `pwd` command on unix.
  *
- * @param result The result of the current working path lookup.
+ * @return The result of the current working path lookup.
  */
-void GetLaunchDirectory(wpi::SmallVectorImpl<char>& result);
+std::string GetLaunchDirectory();
 
 /**
  * Obtains the operating directory of the program. On the roboRIO, this
  * is /home/lvuser. In simulation, it is where the simulation was launched
  * from (`pwd`).
  *
- * @param result The result of the operating directory lookup.
+ * @return The result of the operating directory lookup.
  */
-void GetOperatingDirectory(wpi::SmallVectorImpl<char>& result);
+std::string GetOperatingDirectory();
 
 /**
  * Obtains the deploy directory of the program, which is the remote location
@@ -36,9 +32,8 @@
  * /home/lvuser/deploy. In simulation, it is where the simulation was launched
  * from, in the subdirectory "src/main/deploy" (`pwd`/src/main/deploy).
  *
- * @param result The result of the operating directory lookup
+ * @return The result of the operating directory lookup
  */
-void GetDeployDirectory(wpi::SmallVectorImpl<char>& result);
+std::string GetDeployDirectory();
 
-}  // namespace filesystem
-}  // namespace frc
+}  // namespace frc::filesystem
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GearTooth.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/GearTooth.h
deleted file mode 100644
index 2d1f792..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GearTooth.h
+++ /dev/null
@@ -1,88 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/deprecated.h>
-
-#include "frc/Counter.h"
-
-namespace frc {
-
-/**
- * Alias for counter class.
- *
- * 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:
-  // 55 uSec for threshold
-  static constexpr double kGearToothThreshold = 55e-6;
-
-  /**
-   * Construct a GearTooth sensor given a channel.
-   *
-   * @param channel            The DIO channel that the sensor is connected to.
-   *                           0-9 are on-board, 10-25 are on the MXP.
-   * @param directionSensitive True to enable the pulse length decoding in
-   *                           hardware to specify count direction.
-   */
-  WPI_DEPRECATED(
-      "The only sensor this works with is no longer available and no teams use "
-      "it according to FMS usage reporting.")
-  explicit GearTooth(int channel, bool directionSensitive = false);
-
-  /**
-   * Construct a GearTooth sensor given a digital input.
-   *
-   * This should be used when sharing digital inputs.
-   *
-   * @param source             A pointer to the existing DigitalSource object
-   *                           (such as a DigitalInput)
-   * @param directionSensitive True to enable the pulse length decoding in
-   *                           hardware to specify count direction.
-   */
-  WPI_DEPRECATED(
-      "The only sensor this works with is no longer available and no teams use "
-      "it according to FMS usage reporting.")
-  explicit GearTooth(DigitalSource* source, bool directionSensitive = false);
-
-  /**
-   * Construct a GearTooth sensor given a digital input.
-   *
-   * This should be used when sharing digital inputs.
-   *
-   * @param source             A reference to the existing DigitalSource object
-   *                           (such as a DigitalInput)
-   * @param directionSensitive True to enable the pulse length decoding in
-   *                           hardware to specify count direction.
-   */
-  WPI_DEPRECATED(
-      "The only sensor this works with is no longer available and no teams use "
-      "it according to FMS usage reporting.")
-  explicit GearTooth(std::shared_ptr<DigitalSource> source,
-                     bool directionSensitive = false);
-
-  GearTooth(GearTooth&&) = default;
-  GearTooth& operator=(GearTooth&&) = default;
-
-  /**
-   * Common code called by the constructors.
-   */
-  void EnableDirectionSensing(bool directionSensitive);
-
-  void InitSendable(SendableBuilder& builder) override;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h
index 001b984..b6c18d3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/GenericHID.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,16 +8,19 @@
 
 #include <string>
 
-#include "frc/ErrorBase.h"
-
 namespace frc {
 
 class DriverStation;
 
 /**
- * GenericHID Interface.
+ * Handle input from standard HID devices connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each
+ * time a value is requested the most recent value is returned. There is a
+ * single class instance for each device and the mapping of ports to hardware
+ * buttons depends on the code in the Driver Station.
  */
-class GenericHID : public ErrorBase {
+class GenericHID {
  public:
   enum RumbleType { kLeftRumble, kRightRumble };
 
@@ -44,17 +44,12 @@
     kHID1stPerson = 24
   };
 
-  enum JoystickHand { kLeftHand = 0, kRightHand = 1 };
-
   explicit GenericHID(int port);
   virtual ~GenericHID() = default;
 
   GenericHID(GenericHID&&) = default;
   GenericHID& operator=(GenericHID&&) = default;
 
-  virtual double GetX(JoystickHand hand = kRightHand) const = 0;
-  virtual double GetY(JoystickHand hand = kRightHand) const = 0;
-
   /**
    * Get the button value (starting at button 1).
    *
@@ -71,7 +66,7 @@
   bool GetRawButton(int button) const;
 
   /**
-   * Whether the button was pressed since the last check. Button indexes begin
+   * 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
@@ -84,7 +79,7 @@
   bool GetRawButtonPressed(int button);
 
   /**
-   * Whether the button was released since the last check. Button indexes begin
+   * 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
@@ -197,7 +192,6 @@
   void SetRumble(RumbleType type, double value);
 
  private:
-  DriverStation* m_ds;
   int m_port;
   int m_outputs = 0;
   uint16_t m_leftRumble = 0;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GyroBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/GyroBase.h
deleted file mode 100644
index 037686f..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/GyroBase.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/ErrorBase.h"
-#include "frc/PIDSource.h"
-#include "frc/interfaces/Gyro.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-/**
- * GyroBase is the common base class for Gyro implementations such as
- * AnalogGyro.
- */
-class GyroBase : public Gyro,
-                 public ErrorBase,
-                 public PIDSource,
-                 public Sendable,
-                 public SendableHelper<GyroBase> {
- public:
-  GyroBase() = default;
-  GyroBase(GyroBase&&) = default;
-  GyroBase& operator=(GyroBase&&) = default;
-
-  // PIDSource interface
-  /**
-   * Get the PIDOutput for the PIDSource base object. Can be set to return
-   * angle or rate using SetPIDSourceType(). Defaults to angle.
-   *
-   * @return The PIDOutput (angle or rate, defaults to angle)
-   */
-  double PIDGet() override;
-
-  void InitSendable(SendableBuilder& builder) override;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h
index 2f12615..d874a46 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/I2C.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,8 +8,6 @@
 
 #include <hal/I2CTypes.h>
 
-#include "frc/ErrorBase.h"
-
 namespace frc {
 
 /**
@@ -21,7 +16,7 @@
  * This class is intended to be used by sensor (and other I2C device) drivers.
  * It probably should not be used directly.
  */
-class I2C : public ErrorBase {
+class I2C {
  public:
   enum Port { kOnboard = 0, kMXP };
 
@@ -33,11 +28,14 @@
    */
   I2C(Port port, int deviceAddress);
 
-  ~I2C() override;
+  ~I2C();
 
   I2C(I2C&&) = default;
   I2C& operator=(I2C&&) = default;
 
+  Port GetPort() const;
+  int GetDeviceAddress() const;
+
   /**
    * Generic transaction.
    *
@@ -100,7 +98,7 @@
    *
    * @param registerAddress The register to read first in the transaction.
    * @param count           The number of bytes to read in the transaction.
-   * @param buffer          A pointer to the array of bytes to store the data
+   * @param data            A pointer to the array of bytes to store the data
    *                        read from the device.
    * @return Transfer Aborted... false for success, true for aborted.
    */
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
deleted file mode 100644
index 42b7434..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/InterruptableSensorBase.h
+++ /dev/null
@@ -1,151 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-#include <memory>
-
-#include <hal/Interrupts.h>
-
-#include "frc/AnalogTriggerType.h"
-#include "frc/ErrorBase.h"
-
-namespace frc {
-
-class InterruptableSensorBase : public ErrorBase {
- public:
-  enum WaitResult {
-    kTimeout = 0x0,
-    kRisingEdge = 0x1,
-    kFallingEdge = 0x100,
-    kBoth = 0x101,
-  };
-
-  /**
-   * Handler for interrupts.
-   *
-   * First parameter is if rising, 2nd is if falling.
-   */
-  using InterruptEventHandler = std::function<void(WaitResult)>;
-
-  InterruptableSensorBase() = default;
-
-  /**
-   * Free the resources for an interrupt event.
-   */
-  virtual ~InterruptableSensorBase();
-
-  InterruptableSensorBase(InterruptableSensorBase&&) = default;
-  InterruptableSensorBase& operator=(InterruptableSensorBase&&) = default;
-
-  virtual HAL_Handle GetPortHandleForRouting() const = 0;
-  virtual AnalogTriggerType GetAnalogTriggerTypeForRouting() const = 0;
-
-  /**
-   * Request one of the 8 interrupts asynchronously on this digital input.
-   *
-   * Request interrupts in asynchronous mode where the user's interrupt handler
-   * will be called when the interrupt fires. Users that want control over the
-   * thread priority should use the synchronous method with their own spawned
-   * thread. The default is interrupt on rising edges only.
-   */
-  virtual void RequestInterrupts(HAL_InterruptHandlerFunction handler,
-                                 void* param);
-
-  /**
-   * Request one of the 8 interrupts asynchronously on this digital input.
-   *
-   * Request interrupts in asynchronous mode where the user's interrupt handler
-   * will be called when the interrupt fires. Users that want control over the
-   * thread priority should use the synchronous method with their own spawned
-   * thread. The default is interrupt on rising edges only.
-   */
-  virtual void RequestInterrupts(InterruptEventHandler handler);
-
-  /**
-   * Request one of the 8 interrupts synchronously on this digital input.
-   *
-   * Request interrupts in synchronous mode where the user program will have to
-   * explicitly wait for the interrupt to occur using WaitForInterrupt.
-   * The default is interrupt on rising edges only.
-   */
-  virtual void RequestInterrupts();
-
-  /**
-   * Cancel interrupts on this device.
-   *
-   * This deallocates all the chipobject structures and disables any interrupts.
-   */
-  virtual void CancelInterrupts();
-
-  /**
-   * In synchronous mode, wait for the defined interrupt to occur.
-   *
-   * You should <b>NOT</b> attempt to read the sensor from another thread while
-   * waiting for an interrupt. This is not threadsafe, and can cause memory
-   * corruption
-   *
-   * @param timeout        Timeout in seconds
-   * @param ignorePrevious If true, ignore interrupts that happened before
-   *                       WaitForInterrupt was called.
-   * @return What interrupts fired
-   */
-  virtual WaitResult WaitForInterrupt(double timeout,
-                                      bool ignorePrevious = true);
-
-  /**
-   * Enable interrupts to occur on this input.
-   *
-   * Interrupts are disabled when the RequestInterrupt call is made. This gives
-   * time to do the setup of the other options before starting to field
-   * interrupts.
-   */
-  virtual void EnableInterrupts();
-
-  /**
-   * Disable Interrupts without without deallocating structures.
-   */
-  virtual void DisableInterrupts();
-
-  /**
-   * 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 SetUpSourceEdge().
-   *
-   * @return Timestamp in seconds since boot.
-   */
-  virtual double ReadRisingTimestamp();
-
-  /**
-   * Return the timestamp for the falling interrupt that occurred most recently.
-   *
-   * This is in the same time domain as GetClock().
-   * The falling-edge interrupt should be enabled with
-   * {@link #DigitalInput.SetUpSourceEdge}
-   *
-   * @return Timestamp in seconds since boot.
-   */
-  virtual double ReadFallingTimestamp();
-
-  /**
-   * Set which edge to trigger interrupts on
-   *
-   * @param risingEdge  true to interrupt on rising edge
-   * @param fallingEdge true to interrupt on falling edge
-   */
-  virtual void SetUpSourceEdge(bool risingEdge, bool fallingEdge);
-
- protected:
-  hal::Handle<HAL_InterruptHandle> m_interrupt;
-  std::unique_ptr<InterruptEventHandler> m_interruptHandler{nullptr};
-
-  void AllocateInterrupts(bool watcher);
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobot.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobot.h
deleted file mode 100644
index 24fdba3..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobot.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <atomic>
-
-#include "frc/IterativeRobotBase.h"
-
-namespace frc {
-
-/**
- * IterativeRobot implements the IterativeRobotBase robot program framework.
- *
- * The IterativeRobot class is intended to be subclassed by a user creating a
- * robot program.
- *
- * 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:
-  WPI_DEPRECATED(
-      "Use TimedRobot instead. It's a drop-in replacement that provides more "
-      "regular execution periods.")
-  IterativeRobot();
-  virtual ~IterativeRobot() = default;
-
-  /**
-   * Provide an alternate "main loop" via StartCompetition().
-   *
-   * This specific StartCompetition() implements "main loop" behavior synced
-   * with the DS packets.
-   */
-  void StartCompetition() override;
-
-  /**
-   * Ends the main loop in StartCompetition().
-   */
-  void EndCompetition() override;
-
- private:
-  std::atomic<bool> m_exit{false};
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
index 66897aa..c4253ef 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/IterativeRobotBase.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -28,22 +25,32 @@
  * RobotInit() -- provide for initialization at robot power-on
  *
  * Init() functions -- each of the following functions is called once when the
- *                     appropriate mode is entered:
- *   - DisabledInit()   -- called each and every time disabled is entered from
- *                         another mode
- *   - AutonomousInit() -- called each and every time autonomous is entered from
- *                         another mode
- *   - TeleopInit()     -- called each and every time teleop is entered from
- *                         another mode
- *   - TestInit()       -- called each and every time test is entered from
- *                         another mode
+ * appropriate mode is entered:
+ *
+ * \li DisabledInit() -- called each and every time disabled is entered from
+ *   another mode
+ * \li AutonomousInit() -- called each and every time autonomous is entered from
+ *   another mode
+ * \li TeleopInit() -- called each and every time teleop is entered from another
+ *   mode
+ * \li TestInit() -- called each and every time test is entered from another
+ *   mode
  *
  * Periodic() functions -- each of these functions is called on an interval:
- *   - RobotPeriodic()
- *   - DisabledPeriodic()
- *   - AutonomousPeriodic()
- *   - TeleopPeriodic()
- *   - TestPeriodic()
+ *
+ * \li RobotPeriodic()
+ * \li DisabledPeriodic()
+ * \li AutonomousPeriodic()
+ * \li TeleopPeriodic()
+ * \li TestPeriodic()
+ *
+ * Exit() functions -- each of the following functions is called once when the
+ * appropriate mode is exited:
+ *
+ * \li DisabledExit() -- called each and every time disabled is exited
+ * \li AutonomousExit() -- called each and every time autonomous is exited
+ * \li TeleopExit() -- called each and every time teleop is exited
+ * \li TestExit() -- called each and every time test is exited
  */
 class IterativeRobotBase : public RobotBase {
  public:
@@ -156,6 +163,51 @@
   virtual void TestPeriodic();
 
   /**
+   * Exit code for disabled mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits disabled mode.
+   */
+  virtual void DisabledExit();
+
+  /**
+   * Exit code for autonomous mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits autonomous mode.
+   */
+  virtual void AutonomousExit();
+
+  /**
+   * Exit code for teleop mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits teleop mode.
+   */
+  virtual void TeleopExit();
+
+  /**
+   * Exit code for test mode should go here.
+   *
+   * Users should override this method for code which will be called each time
+   * the robot exits test mode.
+   */
+  virtual void TestExit();
+
+  /**
+   * Enables or disables flushing NetworkTables every loop iteration.
+   * By default, this is disabled.
+   *
+   * @param enabled True to enable, false to disable
+   */
+  void SetNetworkTablesFlushEnabled(bool enabled);
+
+  /**
+   * Gets time period between calls to Periodic() functions.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
    * Constructor for IterativeRobotBase.
    *
    * @param period Period in seconds.
@@ -173,7 +225,7 @@
    */
   explicit IterativeRobotBase(units::second_t period);
 
-  virtual ~IterativeRobotBase() = default;
+  ~IterativeRobotBase() override = default;
 
  protected:
   IterativeRobotBase(IterativeRobotBase&&) = default;
@@ -181,13 +233,13 @@
 
   void LoopFunc();
 
-  units::second_t m_period;
-
  private:
   enum class Mode { kNone, kDisabled, kAutonomous, kTeleop, kTest };
 
   Mode m_lastMode = Mode::kNone;
+  units::second_t m_period;
   Watchdog m_watchdog;
+  bool m_ntFlushEnabled = false;
 
   void PrintLoopOverrunMessage();
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Jaguar.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Jaguar.h
deleted file mode 100644
index 7a8503e..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Jaguar.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Luminary Micro / Vex Robotics Jaguar Speed Controller with PWM control.
- *
- * Note that the Jaguar uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended. The
- * calibration procedure can be found in the Jaguar User Manual available from
- * Vex.
- *
- * \li 2.310ms = full "forward"
- * \li 1.550ms = the "high end" of the deadband range
- * \li 1.507ms = center of the deadband range (off)
- * \li 1.454ms = the "low end" of the deadband range
- * \li 0.697ms = full "reverse"
- */
-class Jaguar : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a Jaguar connected via PWM.
-   *
-   * @param channel The PWM channel that the Jaguar is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit Jaguar(int channel);
-
-  Jaguar(Jaguar&&) = default;
-  Jaguar& operator=(Jaguar&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h
index 0975e6d..0d63e4e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Joystick.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -42,7 +39,7 @@
    */
   explicit Joystick(int port);
 
-  virtual ~Joystick() = default;
+  ~Joystick() override = default;
 
   Joystick(Joystick&&) = default;
   Joystick& operator=(Joystick&&) = default;
@@ -57,7 +54,6 @@
   /**
    * Set the channel associated with the Y axis.
    *
-   * @param axis    The axis to set the channel for.
    * @param channel The channel to set the axis to.
    */
   void SetYChannel(int channel);
@@ -65,7 +61,6 @@
   /**
    * Set the channel associated with the Z axis.
    *
-   * @param axis    The axis to set the channel for.
    * @param channel The channel to set the axis to.
    */
   void SetZChannel(int channel);
@@ -73,7 +68,6 @@
   /**
    * Set the channel associated with the twist axis.
    *
-   * @param axis    The axis to set the channel for.
    * @param channel The channel to set the axis to.
    */
   void SetTwistChannel(int channel);
@@ -81,7 +75,6 @@
   /**
    * Set the channel associated with the throttle axis.
    *
-   * @param axis    The axis to set the channel for.
    * @param channel The channel to set the axis to.
    */
   void SetThrottleChannel(int channel);
@@ -122,24 +115,18 @@
   int GetThrottleChannel() const;
 
   /**
-   * Get the X value of the joystick.
+   * Get the X value of the current joystick.
    *
    * This depends on the mapping of the joystick connected to the current port.
-   *
-   * @param hand This parameter is ignored for the Joystick class and is only
-   *             here to complete the GenericHID interface.
    */
-  double GetX(JoystickHand hand = kRightHand) const override;
+  double GetX() const;
 
   /**
-   * Get the Y value of the joystick.
+   * Get the Y value of the current joystick.
    *
    * This depends on the mapping of the joystick connected to the current port.
-   *
-   * @param hand This parameter is ignored for the Joystick class and is only
-   *             here to complete the GenericHID interface.
    */
-  double GetY(JoystickHand hand = kRightHand) const override;
+  double GetY() const;
 
   /**
    * Get the Z value of the current joystick.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h
index afd0853..f84af17 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/MotorSafety.h
@@ -1,16 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
+#include <string>
 
-#include "frc/ErrorBase.h"
+#include <units/time.h>
+#include <wpi/mutex.h>
+
 #include "frc/Timer.h"
 
 namespace frc {
@@ -21,7 +19,7 @@
  *
  * The subclass should call Feed() whenever the motor value is updated.
  */
-class MotorSafety : public ErrorBase {
+class MotorSafety {
  public:
   MotorSafety();
   virtual ~MotorSafety();
@@ -39,16 +37,16 @@
   /**
    * Set the expiration time for the corresponding motor safety object.
    *
-   * @param expirationTime The timeout value in seconds.
+   * @param expirationTime The timeout value.
    */
-  void SetExpiration(double expirationTime);
+  void SetExpiration(units::second_t expirationTime);
 
   /**
    * Retrieve the timeout value for the corresponding motor safety object.
    *
-   * @return the timeout value in seconds.
+   * @return the timeout value.
    */
-  double GetExpiration() const;
+  units::second_t GetExpiration() const;
 
   /**
    * Determine if the motor is still operating or has timed out.
@@ -93,19 +91,19 @@
   static void CheckMotors();
 
   virtual void StopMotor() = 0;
-  virtual void GetDescription(wpi::raw_ostream& desc) const = 0;
+  virtual std::string GetDescription() const = 0;
 
  private:
-  static constexpr double kDefaultSafetyExpiration = 0.1;
+  static constexpr auto kDefaultSafetyExpiration = 100_ms;
 
   // The expiration time for this object
-  double m_expiration = kDefaultSafetyExpiration;
+  units::second_t m_expiration = kDefaultSafetyExpiration;
 
   // True if motor safety is enabled for this motor
   bool m_enabled = false;
 
   // The FPGA clock value when the motor has expired
-  double m_stopTime = Timer::GetFPGATimestamp();
+  units::second_t m_stopTime = Timer::GetFPGATimestamp();
 
   mutable wpi::mutex m_thisMutex;
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/NidecBrushless.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/NidecBrushless.h
deleted file mode 100644
index fa77e28..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/NidecBrushless.h
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/DigitalOutput.h"
-#include "frc/ErrorBase.h"
-#include "frc/MotorSafety.h"
-#include "frc/PWM.h"
-#include "frc/SpeedController.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class SendableBuilder;
-
-/**
- * Nidec Brushless Motor.
- */
-class NidecBrushless : public SpeedController,
-                       public MotorSafety,
-                       public Sendable,
-                       public SendableHelper<NidecBrushless> {
- public:
-  /**
-   * Constructor.
-   *
-   * @param pwmChannel The PWM channel that the Nidec Brushless controller is
-   *                   attached to. 0-9 are on-board, 10-19 are on the MXP port.
-   * @param dioChannel The DIO channel that the Nidec Brushless controller is
-   *                   attached to. 0-9 are on-board, 10-25 are on the MXP port.
-   */
-  NidecBrushless(int pwmChannel, int dioChannel);
-
-  ~NidecBrushless() override = default;
-
-  NidecBrushless(NidecBrushless&&) = default;
-  NidecBrushless& operator=(NidecBrushless&&) = default;
-
-  // SpeedController interface
-  /**
-   * Set the PWM value.
-   *
-   * The PWM value is set using a range of -1.0 to 1.0, appropriately scaling
-   * the value for the FPGA.
-   *
-   * @param speed The speed value between -1.0 and 1.0 to set.
-   */
-  void Set(double speed) override;
-
-  /**
-   * Get the recently set value of the PWM.
-   *
-   * @return The most recently set value for the PWM between -1.0 and 1.0.
-   */
-  double Get() const override;
-
-  void SetInverted(bool isInverted) override;
-
-  bool GetInverted() const override;
-
-  /**
-   * Disable the motor. The Enable() function must be called to re-enable the
-   * motor.
-   */
-  void Disable() override;
-
-  /**
-   * Re-enable the motor after Disable() has been called. The Set() function
-   * must be called to set a new motor speed.
-   */
-  void Enable();
-
-  // PIDOutput interface
-  /**
-   * Write out the PID value as seen in the PIDOutput base object.
-   *
-   * @param output Write out the PWM value as was found in the PIDController
-   */
-  void PIDWrite(double output) override;
-
-  // MotorSafety interface
-  void StopMotor() override;
-  void GetDescription(wpi::raw_ostream& desc) const override;
-
-  /**
-   * Gets the channel number associated with the object.
-   *
-   * @return The channel number.
-   */
-  int GetChannel() const;
-
-  // Sendable interface
-  void InitSendable(SendableBuilder& builder) override;
-
- private:
-  bool m_isInverted = false;
-  bool m_disabled = false;
-  DigitalOutput m_dio;
-  PWM m_pwm;
-  double m_speed = 0.0;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Notifier.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Notifier.h
index c9348a6..61b7fbb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Notifier.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Notifier.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,21 +8,18 @@
 
 #include <atomic>
 #include <functional>
+#include <string_view>
 #include <thread>
 #include <type_traits>
 #include <utility>
 
 #include <hal/Types.h>
 #include <units/time.h>
-#include <wpi/Twine.h>
-#include <wpi/deprecated.h>
 #include <wpi/mutex.h>
 
-#include "frc/ErrorBase.h"
-
 namespace frc {
 
-class Notifier : public ErrorBase {
+class Notifier {
  public:
   /**
    * Create a Notifier for timer event notification.
@@ -49,8 +43,9 @@
    * 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 priority The FIFO real-time scheduler priority ([1..99] where a
+   *                 higher number represents higher priority). See "man 7
+   *                 sched" for more details.
    * @param handler  The handler is called at the notification time which is set
    *                 using StartSingle or StartPeriodic.
    */
@@ -65,7 +60,7 @@
   /**
    * Free the resources for a timer event.
    */
-  virtual ~Notifier();
+  ~Notifier();
 
   Notifier(Notifier&& rhs);
   Notifier& operator=(Notifier&& rhs);
@@ -75,7 +70,7 @@
    *
    * @param name Name
    */
-  void SetName(const wpi::Twine& name);
+  void SetName(std::string_view name);
 
   /**
    * Change the handler function.
@@ -89,19 +84,6 @@
    *
    * 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.")
-  void StartSingle(double delay);
-
-  /**
-   * Register for single event notification.
-   *
-   * A timer event is queued for a single event after the specified delay.
-   *
    * @param delay Amount of time to wait before the handler is called.
    */
   void StartSingle(units::second_t delay);
@@ -113,22 +95,6 @@
    * 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.
-   */
-  WPI_DEPRECATED("Use unit-safe StartPeriodic method instead.")
-  void StartPeriodic(double period);
-
-  /**
-   * Register for periodic event notification.
-   *
-   * A timer event is queued for periodic event notification. Each time the
-   * interrupt occurs, the event will be immediately requeued for the same time
-   * interval.
-   *
    * @param period Period to call the handler starting one period
    *               after the call to this method.
    */
@@ -145,6 +111,23 @@
    */
   void Stop();
 
+  /**
+   * Sets the HAL notifier thread priority.
+   *
+   * The HAL notifier thread is responsible for managing the FPGA's notifier
+   * interrupt and waking up user's Notifiers when it's their time to run.
+   * Giving the HAL notifier thread real-time priority helps ensure the user's
+   * real-time Notifiers, if any, are notified to run in a timely manner.
+   *
+   * @param realTime Set to true to set a real-time priority, false for standard
+   *                 priority.
+   * @param priority Priority to set the thread to. For real-time, this is 1-99
+   *                 with 99 being highest. For non-real-time, this is forced to
+   *                 0. See "man 7 sched" for more details.
+   * @return         True on success.
+   */
+  static bool SetHALThreadPriority(bool realTime, int32_t priority);
+
  private:
   /**
    * Update the HAL alarm time.
@@ -171,10 +154,10 @@
   std::function<void()> m_handler;
 
   // The absolute expiration time
-  double m_expirationTime = 0;
+  units::second_t m_expirationTime = 0_s;
 
   // The relative time (either periodic or single)
-  double m_period = 0;
+  units::second_t m_period = 0_s;
 
   // True if this is a periodic event
   bool m_periodic = false;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDBase.h
deleted file mode 100644
index 79d8eba..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDBase.h
+++ /dev/null
@@ -1,410 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/deprecated.h>
-#include <wpi/mutex.h>
-
-#include "frc/Base.h"
-#include "frc/LinearFilter.h"
-#include "frc/PIDInterface.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
-#include "frc/Timer.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class SendableBuilder;
-
-/**
- * Class implements a PID Control Loop.
- *
- * Creates a separate thread which reads the given PIDSource and takes care of
- * the integral calculations, as well as writing the given PIDOutput.
- *
- * 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,
-                public Sendable,
-                public SendableHelper<PIDBase> {
- public:
-  /**
-   * Allocate a PID object with the given constants for P, I, D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @param output The PIDOutput object that is set to the output value
-   */
-  WPI_DEPRECATED("All APIs which use this have been deprecated.")
-  PIDBase(double p, double i, double d, PIDSource& source, PIDOutput& output);
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @param output The PIDOutput object that is set to the output value
-   */
-  WPI_DEPRECATED("All APIs which use this have been deprecated.")
-  PIDBase(double p, double i, double d, double f, PIDSource& source,
-          PIDOutput& output);
-
-  virtual ~PIDBase() = default;
-
-  /**
-   * Return the current PID result.
-   *
-   * This is always centered on zero and constrained the the max and min outs.
-   *
-   * @return the latest calculated output
-   */
-  virtual double Get() const;
-
-  /**
-   * Set the PID controller to consider the input to be continuous,
-   *
-   * Rather then using the max and min input range as constraints, it considers
-   * them to be the same point and automatically calculates the shortest route
-   * to the setpoint.
-   *
-   * @param continuous true turns on continuous, false turns off continuous
-   */
-  virtual void SetContinuous(bool continuous = true);
-
-  /**
-   * Sets the maximum and minimum values expected from the input.
-   *
-   * @param minimumInput the minimum value expected from the input
-   * @param maximumInput the maximum value expected from the output
-   */
-  virtual void SetInputRange(double minimumInput, double maximumInput);
-
-  /**
-   * Sets the minimum and maximum values to write.
-   *
-   * @param minimumOutput the minimum value to write to the output
-   * @param maximumOutput the maximum value to write to the output
-   */
-  virtual void SetOutputRange(double minimumOutput, double maximumOutput);
-
-  /**
-   * Set the PID Controller gain parameters.
-   *
-   * Set the proportional, integral, and differential coefficients.
-   *
-   * @param p Proportional coefficient
-   * @param i Integral coefficient
-   * @param d Differential coefficient
-   */
-  void SetPID(double p, double i, double d) override;
-
-  /**
-   * Set the PID Controller gain parameters.
-   *
-   * Set the proportional, integral, and differential coefficients.
-   *
-   * @param p Proportional coefficient
-   * @param i Integral coefficient
-   * @param d Differential coefficient
-   * @param f Feed forward coefficient
-   */
-  virtual void SetPID(double p, double i, double d, double f);
-
-  /**
-   * Set the Proportional coefficient of the PID controller gain.
-   *
-   * @param p proportional coefficient
-   */
-  void SetP(double p);
-
-  /**
-   * Set the Integral coefficient of the PID controller gain.
-   *
-   * @param i integral coefficient
-   */
-  void SetI(double i);
-
-  /**
-   * Set the Differential coefficient of the PID controller gain.
-   *
-   * @param d differential coefficient
-   */
-  void SetD(double d);
-
-  /**
-   * Get the Feed forward coefficient of the PID controller gain.
-   *
-   * @param f Feed forward coefficient
-   */
-  void SetF(double f);
-
-  /**
-   * Get the Proportional coefficient.
-   *
-   * @return proportional coefficient
-   */
-  double GetP() const override;
-
-  /**
-   * Get the Integral coefficient.
-   *
-   * @return integral coefficient
-   */
-  double GetI() const override;
-
-  /**
-   * Get the Differential coefficient.
-   *
-   * @return differential coefficient
-   */
-  double GetD() const override;
-
-  /**
-   * Get the Feed forward coefficient.
-   *
-   * @return Feed forward coefficient
-   */
-  virtual double GetF() const;
-
-  /**
-   * Set the setpoint for the PIDBase.
-   *
-   * @param setpoint the desired setpoint
-   */
-  void SetSetpoint(double setpoint) override;
-
-  /**
-   * Returns the current setpoint of the PIDBase.
-   *
-   * @return the current setpoint
-   */
-  double GetSetpoint() const override;
-
-  /**
-   * Returns the change in setpoint over time of the PIDBase.
-   *
-   * @return the change in setpoint over time
-   */
-  double GetDeltaSetpoint() const;
-
-  /**
-   * Returns the current difference of the input from the setpoint.
-   *
-   * @return the current error
-   */
-  virtual double GetError() const;
-
-  /**
-   * Returns the current average of the error over the past few iterations.
-   *
-   * You can specify the number of iterations to average with
-   * SetToleranceBuffer() (defaults to 1). This is the same value that is used
-   * for OnTarget().
-   *
-   * @return the average error
-   */
-  WPI_DEPRECATED("Use a LinearFilter as the input and GetError().")
-  virtual double GetAvgError() const;
-
-  /**
-   * Sets what type of input the PID controller will use.
-   */
-  virtual void SetPIDSourceType(PIDSourceType pidSource);
-
-  /**
-   * Returns the type of input the PID controller is using.
-   *
-   * @return the PID controller input type
-   */
-  virtual PIDSourceType GetPIDSourceType() const;
-
-  /**
-   * Set the percentage error which is considered tolerable for use with
-   * OnTarget.
-   *
-   * @param percentage error which is tolerable
-   */
-  WPI_DEPRECATED("Use SetPercentTolerance() instead.")
-  virtual void SetTolerance(double percent);
-
-  /**
-   * Set the absolute error which is considered tolerable for use with
-   * OnTarget.
-   *
-   * @param percentage error which is tolerable
-   */
-  virtual void SetAbsoluteTolerance(double absValue);
-
-  /**
-   * Set the percentage error which is considered tolerable for use with
-   * OnTarget.
-   *
-   * @param percentage error which is tolerable
-   */
-  virtual void SetPercentTolerance(double percentValue);
-
-  /**
-   * Set the number of previous error samples to average for tolerancing. When
-   * determining whether a mechanism is on target, the user may want to use a
-   * rolling average of previous measurements instead of a precise position or
-   * velocity. This is useful for noisy sensors which return a few erroneous
-   * measurements when the mechanism is on target. However, the mechanism will
-   * not register as on target for at least the specified bufLength cycles.
-   *
-   * @param bufLength Number of previous cycles to average. Defaults to 1.
-   */
-  WPI_DEPRECATED("Use a LinearDigitalFilter as the input.")
-  virtual void SetToleranceBuffer(int buf = 1);
-
-  /**
-   * Return true if the error is within the percentage of the total input range,
-   * determined by SetTolerance. This asssumes that the maximum and minimum
-   * input were set using SetInput.
-   *
-   * Currently this just reports on target as the actual value passes through
-   * the setpoint. Ideally it should be based on being within the tolerance for
-   * some period of time.
-   *
-   * This will return false until at least one input value has been computed.
-   */
-  virtual bool OnTarget() const;
-
-  /**
-   * Reset the previous error, the integral term, and disable the controller.
-   */
-  void Reset() override;
-
-  /**
-   * Passes the output directly to SetSetpoint().
-   *
-   * PIDControllers can be nested by passing a PIDController as another
-   * PIDController's output. In that case, the output of the parent controller
-   * becomes the input (i.e., the reference) of the child.
-   *
-   * It is the caller's responsibility to put the data into a valid form for
-   * SetSetpoint().
-   */
-  void PIDWrite(double output) override;
-
-  void InitSendable(SendableBuilder& builder) override;
-
- protected:
-  // Is the pid controller enabled
-  bool m_enabled = false;
-
-  mutable wpi::mutex m_thisMutex;
-
-  // Ensures when Disable() is called, PIDWrite() won't run if Calculate()
-  // is already running at that time.
-  mutable wpi::mutex m_pidWriteMutex;
-
-  PIDSource* m_pidInput;
-  PIDOutput* m_pidOutput;
-  Timer m_setpointTimer;
-
-  /**
-   * Read the input, calculate the output accordingly, and write to the output.
-   * This should only be called by the Notifier.
-   */
-  virtual void Calculate();
-
-  /**
-   * Calculate the feed forward term.
-   *
-   * Both of the provided feed forward calculations are velocity feed forwards.
-   * If a different feed forward calculation is desired, the user can override
-   * this function and provide his or her own. This function does no
-   * synchronization because the PIDBase class only calls it in synchronized
-   * code, so be careful if calling it oneself.
-   *
-   * If a velocity PID controller is being used, the F term should be set to 1
-   * over the maximum setpoint for the output. If a position PID controller is
-   * being used, the F term should be set to 1 over the maximum speed for the
-   * output measured in setpoint units per this controller's update period (see
-   * the default period in this class's constructor).
-   */
-  virtual double CalculateFeedForward();
-
-  /**
-   * Wraps error around for continuous inputs. The original error is returned if
-   * continuous mode is disabled. This is an unsynchronized function.
-   *
-   * @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_P;
-
-  // Factor for "integral" control
-  double m_I;
-
-  // Factor for "derivative" control
-  double m_D;
-
-  // Factor for "feed forward" control
-  double m_F;
-
-  // |maximum output|
-  double m_maximumOutput = 1.0;
-
-  // |minimum output|
-  double m_minimumOutput = -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;
-
-  // The prior error (used to compute velocity)
-  double m_prevError = 0;
-
-  // The sum of the errors for use in the integral calc
-  double m_totalError = 0;
-
-  enum {
-    kAbsoluteTolerance,
-    kPercentTolerance,
-    kNoTolerance
-  } m_toleranceType = kNoTolerance;
-
-  // The percetage or absolute error that is considered on target.
-  double m_tolerance = 0.05;
-
-  double m_setpoint = 0;
-  double m_prevSetpoint = 0;
-  double m_error = 0;
-  double m_result = 0;
-
-  LinearFilter<double> m_filter{{}, {}};
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDController.h
deleted file mode 100644
index 39e4deb..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDController.h
+++ /dev/null
@@ -1,139 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-#include <string>
-
-#include <wpi/deprecated.h>
-#include <wpi/mutex.h>
-
-#include "frc/Base.h"
-#include "frc/Controller.h"
-#include "frc/Notifier.h"
-#include "frc/PIDBase.h"
-#include "frc/PIDSource.h"
-#include "frc/Timer.h"
-
-namespace frc {
-
-class PIDOutput;
-
-/**
- * Class implements a PID Control Loop.
- *
- * Creates a separate thread which reads the given PIDSource and takes care of
- * the integral calculations, as well as writing the given PIDOutput.
- *
- * 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:
-  /**
-   * Allocate a PID object with the given constants for P, I, D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @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
-   *               differential terms. The default is 0.05 (50ms).
-   */
-  WPI_DEPRECATED("Use frc2::PIDController class instead.")
-  PIDController(double p, double i, double d, PIDSource* source,
-                PIDOutput* output, double period = 0.05);
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @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
-   *               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,
-                PIDOutput* output, double period = 0.05);
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @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
-   *               differential terms. The default is 0.05 (50ms).
-   */
-  WPI_DEPRECATED("Use frc2::PIDController class instead.")
-  PIDController(double p, double i, double d, PIDSource& source,
-                PIDOutput& output, double period = 0.05);
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @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
-   *               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,
-                PIDOutput& output, double period = 0.05);
-
-  ~PIDController() override;
-
-  /**
-   * Begin running the PIDController.
-   */
-  void Enable() override;
-
-  /**
-   * Stop running the PIDController, this sets the output to zero before
-   * stopping.
-   */
-  void Disable() override;
-
-  /**
-   * Set the enabled state of the PIDController.
-   */
-  void SetEnabled(bool enable);
-
-  /**
-   * Return true if PIDController is enabled.
-   */
-  bool IsEnabled() const;
-
-  /**
-   * Reset the previous error, the integral term, and disable the controller.
-   */
-  void Reset() override;
-
-  void InitSendable(SendableBuilder& builder) override;
-
- private:
-  std::unique_ptr<Notifier> m_controlLoop;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDInterface.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDInterface.h
deleted file mode 100644
index 8d847a6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDInterface.h
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/deprecated.h>
-
-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.")
-  PIDInterface() = default;
-  PIDInterface(PIDInterface&&) = default;
-  PIDInterface& operator=(PIDInterface&&) = default;
-
-  virtual void SetPID(double p, double i, double d) = 0;
-  virtual double GetP() const = 0;
-  virtual double GetI() const = 0;
-  virtual double GetD() const = 0;
-
-  virtual void SetSetpoint(double setpoint) = 0;
-  virtual double GetSetpoint() const = 0;
-
-  virtual void Reset() = 0;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDOutput.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDOutput.h
deleted file mode 100644
index 37fb2a1..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDOutput.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/Base.h"
-
-namespace frc {
-
-/**
- * PIDOutput interface is a generic output for the PID class.
- *
- * PWMs use this class. Users implement this interface to allow for a
- * PIDController to read directly from the inputs.
- */
-class PIDOutput {
- public:
-  virtual void PIDWrite(double output) = 0;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDSource.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDSource.h
deleted file mode 100644
index 1a807b1..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PIDSource.h
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-
-enum class PIDSourceType { kDisplacement, kRate };
-
-/**
- * PIDSource interface is a generic sensor source for the PID class.
- *
- * All sensors that can be used with the PID class will implement the PIDSource
- * that returns a standard value that will be used in the PID code.
- */
-class PIDSource {
- public:
-  virtual ~PIDSource() = default;
-
-  /**
-   * Set which parameter you are using as a process control variable.
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  virtual void SetPIDSourceType(PIDSourceType pidSource);
-
-  virtual PIDSourceType GetPIDSourceType() const;
-
-  virtual double PIDGet() = 0;
-
- protected:
-  PIDSourceType m_pidSource = PIDSourceType::kDisplacement;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h
new file mode 100644
index 0000000..ee87501
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PS4Controller.h
@@ -0,0 +1,403 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/GenericHID.h"
+
+namespace frc {
+
+/**
+ * Handle input from PS4 controllers connected to the Driver Station.
+ *
+ * <p>This class handles PS4 input that comes from the Driver Station. Each time
+ * a value is requested the most recent value is returned. There is a single
+ * class instance for each controller and the mapping of ports to hardware
+ * buttons depends on the code in the Driver Station.
+ */
+class PS4Controller : public GenericHID {
+ public:
+  /**
+   * Construct an instance of an PS4 controller.
+   *
+   * The controller index is the USB port on the Driver Station.
+   *
+   * @param port The port on the Driver Station that the controller is plugged
+   *             into (0-5).
+   */
+  explicit PS4Controller(int port);
+
+  ~PS4Controller() override = default;
+
+  PS4Controller(PS4Controller&&) = default;
+  PS4Controller& operator=(PS4Controller&&) = default;
+
+  /**
+   * Get the X axis value of left side of the controller.
+   *
+   * @return the axis value.
+   */
+  double GetLeftX() const;
+
+  /**
+   * Get the X axis value of right side of the controller.
+   *
+   * @return the axis value.
+   */
+  double GetRightX() const;
+
+  /**
+   * Get the Y axis value of left side of the controller.
+   *
+   * @return the axis value.
+   */
+  double GetLeftY() const;
+
+  /**
+   * Get the Y axis value of right side of the controller.
+   *
+   * @return the axis value.
+   */
+  double GetRightY() const;
+
+  /**
+   * Get the L2 axis value of the controller. Note that this axis is bound to
+   * the range of [0, 1] as opposed to the usual [-1, 1].
+   *
+   * @return the axis value.
+   */
+  double GetL2Axis() const;
+
+  /**
+   * Get the R2 axis value of the controller. Note that this axis is bound to
+   * the range of [0, 1] as opposed to the usual [-1, 1].
+   *
+   * @return the axis value.
+   */
+  double GetR2Axis() const;
+
+  /**
+   * Read the value of the Square button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetSquareButton() const;
+
+  /**
+   * Whether the Square button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetSquareButtonPressed();
+
+  /**
+   * Whether the Square button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetSquareButtonReleased();
+
+  /**
+   * Read the value of the Cross button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetCrossButton() const;
+
+  /**
+   * Whether the Cross button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetCrossButtonPressed();
+
+  /**
+   * Whether the Cross button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetCrossButtonReleased();
+
+  /**
+   * Read the value of the Circle button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetCircleButton() const;
+
+  /**
+   * Whether the Circle button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetCircleButtonPressed();
+
+  /**
+   * Whether the Circle button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetCircleButtonReleased();
+
+  /**
+   * Read the value of the Triangle button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetTriangleButton() const;
+
+  /**
+   * Whether the Triangle button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetTriangleButtonPressed();
+
+  /**
+   * Whether the Triangle button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetTriangleButtonReleased();
+
+  /**
+   * Read the value of the L1 button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetL1Button() const;
+
+  /**
+   * Whether the L1 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetL1ButtonPressed();
+
+  /**
+   * Whether the L1 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetL1ButtonReleased();
+
+  /**
+   * Read the value of the R1 button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetR1Button() const;
+
+  /**
+   * Whether the R1 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetR1ButtonPressed();
+
+  /**
+   * Whether the R1 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetR1ButtonReleased();
+
+  /**
+   * Read the value of the L2 button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetL2Button() const;
+
+  /**
+   * Whether the L2 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetL2ButtonPressed();
+
+  /**
+   * Whether the L2 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetL2ButtonReleased();
+
+  /**
+   * Read the value of the R2 button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetR2Button() const;
+
+  /**
+   * Whether the R2 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetR2ButtonPressed();
+
+  /**
+   * Whether the R2 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetR2ButtonReleased();
+
+  /**
+   * Read the value of the Share button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetShareButton() const;
+
+  /**
+   * Whether the Share button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetShareButtonPressed();
+
+  /**
+   * Whether the Share button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetShareButtonReleased();
+
+  /**
+   * Read the value of the Options button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetOptionsButton() const;
+
+  /**
+   * Whether the Options button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetOptionsButtonPressed();
+
+  /**
+   * Whether the Options button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetOptionsButtonReleased();
+
+  /**
+   * Read the value of the L3 button (pressing the left analog stick) on the
+   * controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetL3Button() const;
+
+  /**
+   * Whether the L3 (left stick) button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetL3ButtonPressed();
+
+  /**
+   * Whether the L3 (left stick) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetL3ButtonReleased();
+
+  /**
+   * Read the value of the R3 button (pressing the right analog stick) on the
+   * controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetR3Button() const;
+
+  /**
+   * Whether the R3 (right stick) button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetR3ButtonPressed();
+
+  /**
+   * Whether the R3 (right stick) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetR3ButtonReleased();
+
+  /**
+   * Read the value of the PS button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetPSButton() const;
+
+  /**
+   * Whether the PS button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  bool GetPSButtonPressed();
+
+  /**
+   * Whether the PS button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  bool GetPSButtonReleased();
+
+  /**
+   * Read the value of the touchpad button on the controller.
+   *
+   * @return The state of the button.
+   */
+  bool GetTouchpad() const;
+
+  /**
+   * Whether the touchpad was pressed since the last check.
+   *
+   * @return Whether the touchpad was pressed since the last check.
+   */
+  bool GetTouchpadPressed();
+
+  /**
+   * Whether the touchpad was released since the last check.
+   *
+   * @return Whether the touchpad was released since the last check.
+   */
+  bool GetTouchpadReleased();
+
+  struct Button {
+    static constexpr int kSquare = 1;
+    static constexpr int kCross = 2;
+    static constexpr int kCircle = 3;
+    static constexpr int kTriangle = 4;
+    static constexpr int kL1 = 5;
+    static constexpr int kR1 = 6;
+    static constexpr int kL2 = 7;
+    static constexpr int kR2 = 8;
+    static constexpr int kShare = 9;
+    static constexpr int kOptions = 10;
+    static constexpr int kL3 = 11;
+    static constexpr int kR3 = 12;
+    static constexpr int kPS = 13;
+    static constexpr int kTouchpad = 14;
+  };
+
+  struct Axis {
+    static constexpr int kLeftX = 0;
+    static constexpr int kLeftY = 1;
+    static constexpr int kRightX = 2;
+    static constexpr int kRightY = 5;
+    static constexpr int kL2 = 3;
+    static constexpr int kR2 = 4;
+  };
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h
index 406a93e..efff540 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWM.h
@@ -1,24 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
 #include <hal/Types.h>
-#include <wpi/raw_ostream.h>
-
-#include "frc/MotorSafety.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 class AddressableLED;
-class SendableBuilder;
+class DMA;
 
 /**
  * Class implements the PWM generation in the FPGA.
@@ -37,9 +31,10 @@
  *   - 1 = minimum pulse width (currently 0.5ms)
  *   - 0 = disabled (i.e. PWM output is held low)
  */
-class PWM : public MotorSafety, public Sendable, public SendableHelper<PWM> {
+class PWM : public wpi::Sendable, public wpi::SendableHelper<PWM> {
  public:
   friend class AddressableLED;
+  friend class DMA;
   /**
    * Represents the amount to multiply the minimum servo-pulse pwm period by.
    */
@@ -67,8 +62,10 @@
    *
    * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the
    *                MXP port
+   * @param registerSendable If true, adds this instance to SendableRegistry
+   *                         and LiveWindow
    */
-  explicit PWM(int channel);
+  explicit PWM(int channel, bool registerSendable = true);
 
   /**
    * Free the PWM channel.
@@ -80,10 +77,6 @@
   PWM(PWM&&) = default;
   PWM& operator=(PWM&&) = default;
 
-  // MotorSafety interface
-  void StopMotor() override;
-  void GetDescription(wpi::raw_ostream& desc) const override;
-
   /**
    * Set the PWM value directly to the hardware.
    *
@@ -129,7 +122,7 @@
   /**
    * Set the PWM value based on a speed.
    *
-   * This is intended to be used by speed controllers.
+   * This is intended to be used by motor controllers.
    *
    * @pre SetMaxPositivePwm() called.
    * @pre SetMinPositivePwm() called.
@@ -137,14 +130,14 @@
    * @pre SetMaxNegativePwm() called.
    * @pre SetMinNegativePwm() called.
    *
-   * @param speed The speed to set the speed controller between -1.0 and 1.0.
+   * @param speed The speed to set the motor controller between -1.0 and 1.0.
    */
   virtual void SetSpeed(double speed);
 
   /**
    * Get the PWM value in terms of speed.
    *
-   * This is intended to be used by speed controllers.
+   * This is intended to be used by motor controllers.
    *
    * @pre SetMaxPositivePwm() called.
    * @pre SetMinPositivePwm() called.
@@ -171,12 +164,12 @@
   void SetZeroLatch();
 
   /**
-   * Optionally eliminate the deadband from a speed controller.
+   * Optionally eliminate the deadband from a motor controller.
    *
-   * @param eliminateDeadband If true, set the motor curve on the Jaguar to
-   *                          eliminate the deadband in the middle of the range.
-   *                          Otherwise, keep the full range without modifying
-   *                          any values.
+   * @param eliminateDeadband If true, set the motor curve on the speed
+   *                          controller to eliminate the deadband in the middle
+   *                          of the range. Otherwise, keep the full range
+   *                          without modifying any values.
    */
   void EnableDeadbandElimination(bool eliminateDeadband);
 
@@ -231,7 +224,7 @@
   int GetChannel() const;
 
  protected:
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   int m_channel;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMSparkMax.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMSparkMax.h
deleted file mode 100644
index 3ce6466..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMSparkMax.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * REV Robotics SPARK MAX Speed Controller.
- *
- * Note that the SPARK MAX uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the SPARK MAX User
- * Manual available from REV Robotics.
- *
- * \li 2.003ms = full "forward"
- * \li 1.550ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.460ms = the "low end" of the deadband range
- * \li 0.999ms = full "reverse"
- */
-class PWMSparkMax : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a SPARK MAX.
-   *
-   * @param channel The PWM channel that the SPARK MAX is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit PWMSparkMax(int channel);
-
-  PWMSparkMax(PWMSparkMax&&) = default;
-  PWMSparkMax& operator=(PWMSparkMax&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMSpeedController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMSpeedController.h
deleted file mode 100644
index b827d30..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMSpeedController.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWM.h"
-#include "frc/SpeedController.h"
-
-namespace frc {
-
-/**
- * Common base class for all PWM Speed Controllers.
- */
-class PWMSpeedController : public PWM, public SpeedController {
- public:
-  PWMSpeedController(PWMSpeedController&&) = default;
-  PWMSpeedController& operator=(PWMSpeedController&&) = default;
-
-  /**
-   * Set the PWM value.
-   *
-   * The PWM value is set using a range of -1.0 to 1.0, appropriately scaling
-   * the value for the FPGA.
-   *
-   * @param speed The speed value between -1.0 and 1.0 to set.
-   */
-  void Set(double value) override;
-
-  /**
-   * Get the recently set value of the PWM.
-   *
-   * @return The most recently set value for the PWM between -1.0 and 1.0.
-   */
-  double Get() const override;
-
-  void SetInverted(bool isInverted) override;
-
-  bool GetInverted() const override;
-
-  void Disable() override;
-
-  void StopMotor() override;
-
-  /**
-   * Write out the PID value as seen in the PIDOutput base object.
-   *
-   * @param output Write out the PWM value as was found in the PIDController
-   */
-  void PIDWrite(double output) override;
-
- protected:
-  /**
-   * Constructor for a PWM Speed Controller connected via PWM.
-   *
-   * @param channel The PWM channel that the controller is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit PWMSpeedController(int channel);
-
-  void InitSendable(SendableBuilder& builder) override;
-
- private:
-  bool m_isInverted = false;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMTalonFX.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMTalonFX.h
deleted file mode 100644
index d85c7ca..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMTalonFX.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 "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM
- * control.
- *
- * Note that the Talon FX uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Talon FX User
- * Manual available from Cross The Road Electronics.
- *
- * \li 2.004ms = full "forward"
- * \li 1.520ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.480ms = the "low end" of the deadband range
- * \li 0.997ms = full "reverse"
- */
-class PWMTalonFX : public PWMSpeedController {
- public:
-  /**
-   * Construct a Talon FX connected via PWM.
-   *
-   * @param channel The PWM channel that the Talon FX is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit PWMTalonFX(int channel);
-
-  PWMTalonFX(PWMTalonFX&&) = default;
-  PWMTalonFX& operator=(PWMTalonFX&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMTalonSRX.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
deleted file mode 100644
index b9c8369..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMTalonSRX.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM
- * control.
- *
- * Note that the Talon SRX uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Talon SRX User
- * Manual available from Cross The Road Electronics.
- *
- * \li 2.004ms = full "forward"
- * \li 1.520ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.480ms = the "low end" of the deadband range
- * \li 0.997ms = full "reverse"
- */
-class PWMTalonSRX : public PWMSpeedController {
- public:
-  /**
-   * Construct a Talon SRX connected via PWM.
-   *
-   * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit PWMTalonSRX(int channel);
-
-  PWMTalonSRX(PWMTalonSRX&&) = default;
-  PWMTalonSRX& operator=(PWMTalonSRX&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMVenom.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMVenom.h
deleted file mode 100644
index 189db43..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMVenom.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Playing with Fusion Venom Smart Motor with PWM control.
- *
- * Note that the Venom uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended.
- *
- * \li 2.004ms = full "forward"
- * \li 1.520ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.480ms = the "low end" of the deadband range
- * \li 0.997ms = full "reverse"
- */
-class PWMVenom : public PWMSpeedController {
- public:
-  /**
-   * Construct a Venom connected via PWM.
-   *
-   * @param channel The PWM channel that the Venom is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit PWMVenom(int channel);
-
-  PWMVenom(PWMVenom&&) = default;
-  PWMVenom& operator=(PWMVenom&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMVictorSPX.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
deleted file mode 100644
index a19e704..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PWMVictorSPX.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM
- * control.
- *
- * Note that the Victor SPX uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Victor SPX User
- * Manual available from Cross The Road Electronics.
- *
- * \li 2.004ms = full "forward"
- * \li 1.520ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.480ms = the "low end" of the deadband range
- * \li 0.997ms = full "reverse"
- */
-class PWMVictorSPX : public PWMSpeedController {
- public:
-  /**
-   * Construct a Victor SPX connected via PWM.
-   *
-   * @param channel The PWM channel that the Victor SPX is attached to. 0-9
-   *                are on-board, 10-19 are on the MXP port
-   */
-  explicit PWMVictorSPX(int channel);
-
-  PWMVictorSPX(PWMVictorSPX&&) = default;
-  PWMVictorSPX& operator=(PWMVictorSPX&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h
new file mode 100644
index 0000000..857ed51
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticHub.h
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/DenseMap.h>
+#include <wpi/mutex.h>
+
+#include "PneumaticsBase.h"
+
+namespace frc {
+class PneumaticHub : public PneumaticsBase {
+ public:
+  PneumaticHub();
+  explicit PneumaticHub(int module);
+
+  ~PneumaticHub() override = default;
+
+  bool GetCompressor() const override;
+
+  void SetClosedLoopControl(bool enabled) override;
+
+  bool GetClosedLoopControl() const override;
+
+  bool GetPressureSwitch() const override;
+
+  double GetCompressorCurrent() const override;
+
+  void SetSolenoids(int mask, int values) override;
+
+  int GetSolenoids() const override;
+
+  int GetModuleNumber() const override;
+
+  int GetSolenoidDisabledList() const override;
+
+  void FireOneShot(int index) override;
+
+  void SetOneShotDuration(int index, units::second_t duration) override;
+
+  bool CheckSolenoidChannel(int channel) const override;
+
+  int CheckAndReserveSolenoids(int mask) override;
+
+  void UnreserveSolenoids(int mask) override;
+
+  bool ReserveCompressor() override;
+
+  void UnreserveCompressor() override;
+
+  Solenoid MakeSolenoid(int channel) override;
+  DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
+                                    int reverseChannel) override;
+  Compressor MakeCompressor() override;
+
+ private:
+  class DataStore;
+  friend class DataStore;
+  friend class PneumaticsBase;
+  PneumaticHub(HAL_REVPHHandle handle, int module);
+
+  static std::shared_ptr<PneumaticsBase> GetForModule(int module);
+
+  std::shared_ptr<DataStore> m_dataStore;
+  HAL_REVPHHandle m_handle;
+  int m_module;
+
+  static wpi::mutex m_handleLock;
+  static std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<DataStore>>>
+      m_handleMap;
+  static std::weak_ptr<DataStore>& GetDataStore(int module);
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h
new file mode 100644
index 0000000..06cad5d
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsBase.h
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <units/time.h>
+
+#include "frc/PneumaticsModuleType.h"
+
+namespace frc {
+class Solenoid;
+class DoubleSolenoid;
+class Compressor;
+class PneumaticsBase {
+ public:
+  virtual ~PneumaticsBase() = default;
+
+  virtual bool GetCompressor() const = 0;
+
+  virtual bool GetPressureSwitch() const = 0;
+
+  virtual double GetCompressorCurrent() const = 0;
+
+  virtual void SetClosedLoopControl(bool on) = 0;
+
+  virtual bool GetClosedLoopControl() const = 0;
+
+  virtual void SetSolenoids(int mask, int values) = 0;
+
+  virtual int GetSolenoids() const = 0;
+
+  virtual int GetModuleNumber() const = 0;
+
+  virtual int GetSolenoidDisabledList() const = 0;
+
+  virtual void FireOneShot(int index) = 0;
+
+  virtual void SetOneShotDuration(int index, units::second_t duration) = 0;
+
+  virtual bool CheckSolenoidChannel(int channel) const = 0;
+
+  virtual int CheckAndReserveSolenoids(int mask) = 0;
+
+  virtual void UnreserveSolenoids(int mask) = 0;
+
+  virtual bool ReserveCompressor() = 0;
+
+  virtual void UnreserveCompressor() = 0;
+
+  virtual Solenoid MakeSolenoid(int channel) = 0;
+  virtual DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
+                                            int reverseChannel) = 0;
+  virtual Compressor MakeCompressor() = 0;
+
+  static std::shared_ptr<PneumaticsBase> GetForType(
+      int module, PneumaticsModuleType moduleType);
+  static int GetDefaultForType(PneumaticsModuleType moduleType);
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
new file mode 100644
index 0000000..64686d6
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsControlModule.h
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <wpi/DenseMap.h>
+#include <wpi/mutex.h>
+
+#include "PneumaticsBase.h"
+
+namespace frc {
+class PneumaticsControlModule : public PneumaticsBase {
+ public:
+  PneumaticsControlModule();
+  explicit PneumaticsControlModule(int module);
+
+  ~PneumaticsControlModule() override = default;
+
+  bool GetCompressor() const override;
+
+  void SetClosedLoopControl(bool enabled) override;
+
+  bool GetClosedLoopControl() const override;
+
+  bool GetPressureSwitch() const override;
+
+  double GetCompressorCurrent() const override;
+
+  bool GetCompressorCurrentTooHighFault() const;
+  bool GetCompressorCurrentTooHighStickyFault() const;
+  bool GetCompressorShortedFault() const;
+  bool GetCompressorShortedStickyFault() const;
+  bool GetCompressorNotConnectedFault() const;
+  bool GetCompressorNotConnectedStickyFault() const;
+
+  bool GetSolenoidVoltageFault() const;
+  bool GetSolenoidVoltageStickyFault() const;
+
+  void ClearAllStickyFaults();
+
+  void SetSolenoids(int mask, int values) override;
+
+  int GetSolenoids() const override;
+
+  int GetModuleNumber() const override;
+
+  int GetSolenoidDisabledList() const override;
+
+  void FireOneShot(int index) override;
+
+  void SetOneShotDuration(int index, units::second_t duration) override;
+
+  bool CheckSolenoidChannel(int channel) const override;
+
+  int CheckAndReserveSolenoids(int mask) override;
+
+  void UnreserveSolenoids(int mask) override;
+
+  bool ReserveCompressor() override;
+
+  void UnreserveCompressor() override;
+
+  Solenoid MakeSolenoid(int channel) override;
+  DoubleSolenoid MakeDoubleSolenoid(int forwardChannel,
+                                    int reverseChannel) override;
+  Compressor MakeCompressor() override;
+
+ private:
+  class DataStore;
+  friend class DataStore;
+  friend class PneumaticsBase;
+  PneumaticsControlModule(HAL_CTREPCMHandle handle, int module);
+
+  static std::shared_ptr<PneumaticsBase> GetForModule(int module);
+
+  std::shared_ptr<DataStore> m_dataStore;
+  HAL_CTREPCMHandle m_handle;
+  int m_module;
+
+  static wpi::mutex m_handleLock;
+  static std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<DataStore>>>
+      m_handleMap;
+  static std::weak_ptr<DataStore>& GetDataStore(int module);
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h
new file mode 100644
index 0000000..7f70662
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PneumaticsModuleType.h
@@ -0,0 +1,9 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+enum class PneumaticsModuleType { CTREPCM, REVPH };
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h
new file mode 100644
index 0000000..2863daf
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistribution.h
@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/Types.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+namespace frc {
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the
+ * CAN PDP.
+ */
+class PowerDistribution : public wpi::Sendable,
+                          public wpi::SendableHelper<PowerDistribution> {
+ public:
+  static constexpr int kDefaultModule = -1;
+  enum class ModuleType { kAutomatic = 0, kCTRE = 1, kRev = 2 };
+
+  /**
+   * Constructs a PowerDistribution.
+   *
+   * Uses the default CAN ID.
+   */
+  PowerDistribution();
+
+  /**
+   * Constructs a PowerDistribution.
+   *
+   * @param module The CAN ID of the PDP
+   * @param moduleType The type of module
+   */
+  PowerDistribution(int module, ModuleType moduleType);
+
+  ~PowerDistribution() override;
+  PowerDistribution(PowerDistribution&&) = default;
+  PowerDistribution& operator=(PowerDistribution&&) = default;
+
+  /**
+   * Query the input voltage of the PDP.
+   *
+   * @return The voltage of the PDP in volts
+   */
+  double GetVoltage() const;
+
+  /**
+   * Query the temperature of the PDP.
+   *
+   * @return The temperature of the PDP in degrees Celsius
+   */
+  double GetTemperature() const;
+
+  /**
+   * Query the current of a single channel of the PDP.
+   *
+   * @return The current of one of the PDP channels (channels 0-15) in Amperes
+   */
+  double GetCurrent(int channel) const;
+
+  /**
+   * Query the total current of all monitored PDP channels (0-15).
+   *
+   * @return The the total current drawn from the PDP channels in Amperes
+   */
+  double GetTotalCurrent() const;
+
+  /**
+   * Query the total power drawn from the monitored PDP channels.
+   *
+   * @return The the total power drawn from the PDP channels in Watts
+   */
+  double GetTotalPower() const;
+
+  /**
+   * Query the total energy drawn from the monitored PDP channels.
+   *
+   * @return The the total energy drawn from the PDP channels in Joules
+   */
+  double GetTotalEnergy() const;
+
+  /**
+   * Reset the total energy drawn from the PDP.
+   *
+   * @see PowerDistribution#GetTotalEnergy
+   */
+  void ResetTotalEnergy();
+
+  /**
+   * Remove all of the fault flags on the PDP.
+   */
+  void ClearStickyFaults();
+
+  /**
+   * Gets module number (CAN ID).
+   */
+  int GetModule() const;
+
+  bool GetSwitchableChannel() const;
+
+  void SetSwitchableChannel(bool enabled);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  hal::Handle<HAL_PowerDistributionHandle> m_handle;
+  int m_module;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
deleted file mode 100644
index 9ec00a3..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/PowerDistributionPanel.h
+++ /dev/null
@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <hal/Types.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class SendableBuilder;
-
-/**
- * Class for getting voltage, current, temperature, power and energy from the
- * CAN PDP.
- */
-class PowerDistributionPanel : public ErrorBase,
-                               public Sendable,
-                               public SendableHelper<PowerDistributionPanel> {
- public:
-  PowerDistributionPanel();
-  explicit PowerDistributionPanel(int module);
-
-  PowerDistributionPanel(PowerDistributionPanel&&) = default;
-  PowerDistributionPanel& operator=(PowerDistributionPanel&&) = default;
-
-  /**
-   * Query the input voltage of the PDP.
-   *
-   * @return The voltage of the PDP in volts
-   */
-  double GetVoltage() const;
-
-  /**
-   * Query the temperature of the PDP.
-   *
-   * @return The temperature of the PDP in degrees Celsius
-   */
-  double GetTemperature() const;
-
-  /**
-   * Query the current of a single channel of the PDP.
-   *
-   * @return The current of one of the PDP channels (channels 0-15) in Amperes
-   */
-  double GetCurrent(int channel) const;
-
-  /**
-   * Query the total current of all monitored PDP channels (0-15).
-   *
-   * @return The the total current drawn from the PDP channels in Amperes
-   */
-  double GetTotalCurrent() const;
-
-  /**
-   * Query the total power drawn from the monitored PDP channels.
-   *
-   * @return The the total power drawn from the PDP channels in Watts
-   */
-  double GetTotalPower() const;
-
-  /**
-   * Query the total energy drawn from the monitored PDP channels.
-   *
-   * @return The the total energy drawn from the PDP channels in Joules
-   */
-  double GetTotalEnergy() const;
-
-  /**
-   * Reset the total energy drawn from the PDP.
-   *
-   * @see PowerDistributionPanel#GetTotalEnergy
-   */
-  void ResetTotalEnergy();
-
-  /**
-   * Remove all of the fault flags on the PDP.
-   */
-  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/third_party/allwpilib/wpilibc/src/main/native/include/frc/Preferences.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Preferences.h
index f04b012..b939d9e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Preferences.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Preferences.h
@@ -1,21 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
-#include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
-#include <networktables/NetworkTable.h>
-
-#include "frc/ErrorBase.h"
+#include <wpi/deprecated.h>
 
 namespace frc {
 
@@ -30,16 +25,18 @@
  *
  * This class is thread safe.
  *
- * This will also interact with {@link NetworkTable} by creating a table called
+ * This will also interact with NetworkTable by creating a table called
  * "Preferences" with all the key-value pairs.
  */
-class Preferences : public ErrorBase {
+class Preferences {
  public:
   /**
    * Get the one and only {@link Preferences} object.
    *
    * @return pointer to the {@link Preferences}
+   * @deprecated Use the static methods
    */
+  WPI_DEPRECATED("Use static methods")
   static Preferences* GetInstance();
 
   /**
@@ -47,7 +44,7 @@
    *
    * @return a vector of the keys
    */
-  std::vector<std::string> GetKeys();
+  static std::vector<std::string> GetKeys();
 
   /**
    * Returns the string at the given key.  If this table does not have a value
@@ -57,7 +54,8 @@
    * @param defaultValue the value to return if none exists in the table
    * @return either the value in the table, or the defaultValue
    */
-  std::string GetString(wpi::StringRef key, wpi::StringRef defaultValue = "");
+  static std::string GetString(std::string_view key,
+                               std::string_view defaultValue = "");
 
   /**
    * Returns the int at the given key.  If this table does not have a value for
@@ -67,7 +65,7 @@
    * @param defaultValue the value to return if none exists in the table
    * @return either the value in the table, or the defaultValue
    */
-  int GetInt(wpi::StringRef key, int defaultValue = 0);
+  static int GetInt(std::string_view key, int defaultValue = 0);
 
   /**
    * Returns the double at the given key.  If this table does not have a value
@@ -77,7 +75,7 @@
    * @param defaultValue the value to return if none exists in the table
    * @return either the value in the table, or the defaultValue
    */
-  double GetDouble(wpi::StringRef key, double defaultValue = 0.0);
+  static double GetDouble(std::string_view key, double defaultValue = 0.0);
 
   /**
    * Returns the float at the given key.  If this table does not have a value
@@ -87,7 +85,7 @@
    * @param defaultValue the value to return if none exists in the table
    * @return either the value in the table, or the defaultValue
    */
-  float GetFloat(wpi::StringRef key, float defaultValue = 0.0);
+  static float GetFloat(std::string_view key, float defaultValue = 0.0);
 
   /**
    * Returns the boolean at the given key.  If this table does not have a value
@@ -97,7 +95,7 @@
    * @param defaultValue the value to return if none exists in the table
    * @return either the value in the table, or the defaultValue
    */
-  bool GetBoolean(wpi::StringRef key, bool defaultValue = false);
+  static bool GetBoolean(std::string_view key, bool defaultValue = false);
 
   /**
    * Returns the long (int64_t) at the given key.  If this table does not have a
@@ -108,7 +106,7 @@
    * @param defaultValue the value to return if none exists in the table
    * @return either the value in the table, or the defaultValue
    */
-  int64_t GetLong(wpi::StringRef key, int64_t defaultValue = 0);
+  static int64_t GetLong(std::string_view key, int64_t defaultValue = 0);
 
   /**
    * Puts the given string into the preferences table.
@@ -119,13 +117,25 @@
    * @param key   the key
    * @param value the value
    */
-  void PutString(wpi::StringRef key, wpi::StringRef value);
+  static void SetString(std::string_view key, std::string_view value);
+
+  /**
+   * Puts the given string into the preferences table.
+   *
+   * The value may not have quotation marks, nor may the key have any whitespace
+   * nor an equals sign.
+   *
+   * @param key   the key
+   * @param value the value
+   */
+  WPI_DEPRECATED("Use SetString instead.")
+  static void PutString(std::string_view key, std::string_view value);
 
   /**
    * Puts the given string into the preferences table if it doesn't
    * already exist.
    */
-  void InitString(wpi::StringRef key, wpi::StringRef value);
+  static void InitString(std::string_view key, std::string_view value);
 
   /**
    * Puts the given int into the preferences table.
@@ -135,13 +145,24 @@
    * @param key   the key
    * @param value the value
    */
-  void PutInt(wpi::StringRef key, int value);
+  static void SetInt(std::string_view key, int value);
+
+  /**
+   * Puts the given int into the preferences table.
+   *
+   * The key may not have any whitespace nor an equals sign.
+   *
+   * @param key   the key
+   * @param value the value
+   */
+  WPI_DEPRECATED("Use SetInt instead.")
+  static void PutInt(std::string_view key, int value);
 
   /**
    * Puts the given int into the preferences table if it doesn't
    * already exist.
    */
-  void InitInt(wpi::StringRef key, int value);
+  static void InitInt(std::string_view key, int value);
 
   /**
    * Puts the given double into the preferences table.
@@ -151,13 +172,24 @@
    * @param key   the key
    * @param value the value
    */
-  void PutDouble(wpi::StringRef key, double value);
+  static void SetDouble(std::string_view key, double value);
+
+  /**
+   * Puts the given double into the preferences table.
+   *
+   * The key may not have any whitespace nor an equals sign.
+   *
+   * @param key   the key
+   * @param value the value
+   */
+  WPI_DEPRECATED("Use SetDouble instead.")
+  static void PutDouble(std::string_view key, double value);
 
   /**
    * Puts the given double into the preferences table if it doesn't
    * already exist.
    */
-  void InitDouble(wpi::StringRef key, double value);
+  static void InitDouble(std::string_view key, double value);
 
   /**
    * Puts the given float into the preferences table.
@@ -167,13 +199,24 @@
    * @param key   the key
    * @param value the value
    */
-  void PutFloat(wpi::StringRef key, float value);
+  static void SetFloat(std::string_view key, float value);
+
+  /**
+   * Puts the given float into the preferences table.
+   *
+   * The key may not have any whitespace nor an equals sign.
+   *
+   * @param key   the key
+   * @param value the value
+   */
+  WPI_DEPRECATED("Use SetFloat instead.")
+  static void PutFloat(std::string_view key, float value);
 
   /**
    * Puts the given float into the preferences table if it doesn't
    * already exist.
    */
-  void InitFloat(wpi::StringRef key, float value);
+  static void InitFloat(std::string_view key, float value);
 
   /**
    * Puts the given boolean into the preferences table.
@@ -183,13 +226,24 @@
    * @param key   the key
    * @param value the value
    */
-  void PutBoolean(wpi::StringRef key, bool value);
+  static void SetBoolean(std::string_view key, bool value);
+
+  /**
+   * Puts the given boolean into the preferences table.
+   *
+   * The key may not have any whitespace nor an equals sign.
+   *
+   * @param key   the key
+   * @param value the value
+   */
+  WPI_DEPRECATED("Use SetBoolean instead.")
+  static void PutBoolean(std::string_view key, bool value);
 
   /**
    * Puts the given boolean into the preferences table if it doesn't
    * already exist.
    */
-  void InitBoolean(wpi::StringRef key, bool value);
+  static void InitBoolean(std::string_view key, bool value);
 
   /**
    * Puts the given long (int64_t) into the preferences table.
@@ -199,13 +253,24 @@
    * @param key   the key
    * @param value the value
    */
-  void PutLong(wpi::StringRef key, int64_t value);
+  static void SetLong(std::string_view key, int64_t value);
+
+  /**
+   * Puts the given long (int64_t) into the preferences table.
+   *
+   * The key may not have any whitespace nor an equals sign.
+   *
+   * @param key   the key
+   * @param value the value
+   */
+  WPI_DEPRECATED("Use SetLong instead.")
+  static void PutLong(std::string_view 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);
+  static void InitLong(std::string_view key, int64_t value);
 
   /**
    * Returns whether or not there is a key with the given name.
@@ -213,30 +278,22 @@
    * @param key the key
    * @return if there is a value at the given key
    */
-  bool ContainsKey(wpi::StringRef key);
+  static bool ContainsKey(std::string_view key);
 
   /**
    * Remove a preference.
    *
    * @param key the key
    */
-  void Remove(wpi::StringRef key);
+  static void Remove(std::string_view key);
 
   /**
    * Remove all preferences.
    */
-  void RemoveAll();
-
- protected:
-  Preferences();
-  virtual ~Preferences() = default;
-
-  Preferences(Preferences&&) = default;
-  Preferences& operator=(Preferences&&) = default;
+  static void RemoveAll();
 
  private:
-  std::shared_ptr<nt::NetworkTable> m_table;
-  NT_EntryListener m_listener;
+  Preferences() = default;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h
index c903fc0..4765c64 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Relay.h
@@ -1,26 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string>
 
 #include <hal/Types.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
-#include "frc/ErrorBase.h"
 #include "frc/MotorSafety.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
  * Class for Spike style relay outputs.
  *
@@ -34,8 +28,8 @@
  * a solenoid).
  */
 class Relay : public MotorSafety,
-              public Sendable,
-              public SendableHelper<Relay> {
+              public wpi::Sendable,
+              public wpi::SendableHelper<Relay> {
  public:
   enum Value { kOff, kOn, kForward, kReverse };
   enum Direction { kBothDirections, kForwardOnly, kReverseOnly };
@@ -95,9 +89,9 @@
   // MotorSafety interface
   void StopMotor() override;
 
-  void GetDescription(wpi::raw_ostream& desc) const override;
+  std::string GetDescription() const override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   int m_channel;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Resource.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Resource.h
index e9759d5..4109cc4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Resource.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Resource.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,8 +12,6 @@
 
 #include <wpi/mutex.h>
 
-#include "frc/ErrorBase.h"
-
 namespace frc {
 
 /**
@@ -29,7 +24,7 @@
  * resources; it just tracks which indices were marked in use by Allocate and
  * not yet freed by Free.
  */
-class Resource : public ErrorBase {
+class Resource {
  public:
   virtual ~Resource() = default;
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h
index 66d5637..70c6093 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotBase.h
@@ -1,39 +1,50 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <chrono>
 #include <thread>
 
+#include <hal/DriverStation.h>
 #include <hal/HALBase.h>
 #include <hal/Main.h>
 #include <wpi/condition_variable.h>
+#include <wpi/deprecated.h>
 #include <wpi/mutex.h>
-#include <wpi/raw_ostream.h>
 
-#include "frc/Base.h"
+#include "frc/Errors.h"
+#include "frc/RuntimeType.h"
 
 namespace frc {
 
-class DriverStation;
-
 int RunHALInitialization();
 
 namespace impl {
 
 template <class Robot>
 void RunRobot(wpi::mutex& m, Robot** robot) {
-  static Robot theRobot;
-  {
-    std::scoped_lock lock{m};
-    *robot = &theRobot;
+  try {
+    static Robot theRobot;
+    {
+      std::scoped_lock lock{m};
+      *robot = &theRobot;
+    }
+    theRobot.StartCompetition();
+  } catch (const frc::RuntimeError& e) {
+    e.Report();
+    FRC_ReportError(
+        err::Error, "{}",
+        "The robot program quit unexpectedly."
+        " This is usually due to a code error.\n"
+        "  The above stacktrace can help determine where the error occurred.\n"
+        "  See https://wpilib.org/stacktrace for more information.\n");
+    throw;
+  } catch (const std::exception& e) {
+    HAL_SendError(1, err::Error, 0, e.what(), "", "", 1);
+    throw;
   }
-  theRobot.StartCompetition();
 }
 
 }  // namespace impl
@@ -77,15 +88,18 @@
     HAL_RunMain();
 
     // signal loop to exit
-    if (robot) robot->EndCompetition();
+    if (robot) {
+      robot->EndCompetition();
+    }
 
     // prefer to join, but detach to exit if it doesn't exit in a timely manner
     using namespace std::chrono_literals;
     std::unique_lock lock{m};
-    if (cv.wait_for(lock, 1s, [] { return exited; }))
+    if (cv.wait_for(lock, 1s, [] { return exited; })) {
       thr.join();
-    else
+    } else {
       thr.detach();
+    }
   } else {
     impl::RunRobot<Robot>(m, &robot);
   }
@@ -95,13 +109,6 @@
   return 0;
 }
 
-#define START_ROBOT_CLASS(_ClassName_)                                 \
-  WPI_DEPRECATED("Call frc::StartRobot<" #_ClassName_                  \
-                 ">() in your own main() instead of using the "        \
-                 "START_ROBOT_CLASS(" #_ClassName_ ") macro.")         \
-  int StartRobotClassImpl() { return frc::StartRobot<_ClassName_>(); } \
-  int main() { return StartRobotClassImpl(); }
-
 /**
  * Implement a Robot Program framework.
  *
@@ -149,16 +156,36 @@
    *
    * @return True if the robot is currently operating in Tele-Op mode as
    *         determined by the field controls.
+   * @deprecated Use IsTeleop() instead.
    */
+  WPI_DEPRECATED("Use IsTeleop() instead")
   bool IsOperatorControl() const;
 
   /**
+   * Determine if the robot is currently in Operator Control mode.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode as
+   *         determined by the field controls.
+   */
+  bool IsTeleop() 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
+   *         enabled as determined by the field-controls.
+   * @deprecated Use IsTeleopEnabled() instead.
+   */
+  WPI_DEPRECATED("Use IsTeleopEnabled() instead")
+  bool IsOperatorControlEnabled() 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;
+  bool IsTeleopEnabled() const;
 
   /**
    * Determine if the robot is currently in Test mode.
@@ -186,6 +213,13 @@
   virtual void EndCompetition() = 0;
 
   /**
+   * Get the current runtime type.
+   *
+   * @return Current runtime type.
+   */
+  static RuntimeType GetRuntimeType();
+
+  /**
    * Get if the robot is real.
    *
    * @return If the robot is running in the real world.
@@ -218,15 +252,11 @@
    */
   RobotBase();
 
-  virtual ~RobotBase();
+  virtual ~RobotBase() = default;
 
  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;
-  RobotBase& operator=(RobotBase&&) noexcept;
-
-  DriverStation& m_ds;
+  RobotBase(RobotBase&&) = default;
+  RobotBase& operator=(RobotBase&&) = default;
 
   static std::thread::id m_threadId;
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h
index fae136b..e9750f0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotController.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <stdint.h>
 
+#include <units/voltage.h>
+
 namespace frc {
 
 struct CANStatus {
@@ -59,6 +58,13 @@
   static bool GetUserButton();
 
   /**
+   * Read the battery voltage.
+   *
+   * @return The battery voltage in Volts.
+   */
+  static units::volt_t GetBatteryVoltage();
+
+  /**
    * Check if the FPGA outputs are enabled.
    *
    * The outputs may be disabled if the robot is disabled or e-stopped, the
@@ -182,6 +188,28 @@
    */
   static int GetFaultCount6V();
 
+  /**
+   * Get the current brownout voltage setting.
+   *
+   * @return The brownout voltage
+   */
+  static units::volt_t GetBrownoutVoltage();
+
+  /**
+   * Set the voltage the roboRIO will brownout and disable all outputs.
+   *
+   * Note that this only does anything on the roboRIO 2.
+   * On the roboRIO it is a no-op.
+   *
+   * @param brownoutVoltage The brownout voltage
+   */
+  static void SetBrownoutVoltage(units::volt_t brownoutVoltage);
+
+  /**
+   * Get the current status of the CAN bus.
+   *
+   * @return The status of the CAN bus
+   */
   static CANStatus GetCANStatus();
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotDrive.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotDrive.h
deleted file mode 100644
index a8b63e6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotDrive.h
+++ /dev/null
@@ -1,456 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/deprecated.h>
-#include <wpi/raw_ostream.h>
-
-#include "frc/ErrorBase.h"
-#include "frc/MotorSafety.h"
-
-namespace frc {
-
-class SpeedController;
-class GenericHID;
-
-/**
- * Utility class for handling Robot drive based on a definition of the motor
- * configuration.
- *
- * The robot drive class handles basic driving for a robot. Currently, 2 and 4
- * motor tank and mecanum drive trains are supported. In the future other drive
- * types like swerve might be implemented. Motor channel numbers are passed
- * 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:
-  enum MotorType {
-    kFrontLeftMotor = 0,
-    kFrontRightMotor = 1,
-    kRearLeftMotor = 2,
-    kRearRightMotor = 3
-  };
-
-  /**
-   * Constructor for RobotDrive with 2 motors specified with channel numbers.
-   *
-   * Set up parameters for a two wheel drive system where the
-   * left and right motor pwm channels are specified in the call.
-   * This call assumes Talons for controlling the motors.
-   *
-   * @param leftMotorChannel  The PWM channel number that drives the left motor.
-   *                          0-9 are on-board, 10-19 are on the MXP port
-   * @param rightMotorChannel The PWM channel number that drives the right
-   *                          motor. 0-9 are on-board, 10-19 are on the MXP port
-   */
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(int leftMotorChannel, int rightMotorChannel);
-
-  /**
-   * Constructor for RobotDrive with 4 motors specified with channel numbers.
-   *
-   * Set up parameters for a four wheel drive system where all four motor
-   * pwm channels are specified in the call.
-   * This call assumes Talons for controlling the motors.
-   *
-   * @param frontLeftMotor  Front left motor channel number. 0-9 are on-board,
-   *                        10-19 are on the MXP port
-   * @param rearLeftMotor   Rear Left motor channel number. 0-9 are on-board,
-   *                        10-19 are on the MXP port
-   * @param frontRightMotor Front right motor channel number. 0-9 are on-board,
-   *                        10-19 are on the MXP port
-   * @param rearRightMotor  Rear Right motor channel number. 0-9 are on-board,
-   *                        10-19 are on the MXP port
-   */
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(int frontLeftMotorChannel, int rearLeftMotorChannel,
-             int frontRightMotorChannel, int rearRightMotorChannel);
-
-  /**
-   * Constructor for RobotDrive with 2 motors specified as SpeedController
-   * objects.
-   *
-   * The SpeedController version of the constructor enables programs to use the
-   * RobotDrive classes with subclasses of the SpeedController objects, for
-   * example, versions with ramping or reshaping of the curve to suit motor bias
-   * or deadband elimination.
-   *
-   * @param leftMotor  The left SpeedController object used to drive the robot.
-   * @param rightMotor The right SpeedController object used to drive the robot.
-   */
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(SpeedController* leftMotor, SpeedController* rightMotor);
-
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(SpeedController& leftMotor, SpeedController& rightMotor);
-
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(std::shared_ptr<SpeedController> leftMotor,
-             std::shared_ptr<SpeedController> rightMotor);
-
-  /**
-   * Constructor for RobotDrive with 4 motors specified as SpeedController
-   * objects.
-   *
-   * Speed controller input version of RobotDrive (see previous comments).
-   *
-   * @param frontLeftMotor  The front left SpeedController object used to drive
-   *                        the robot.
-   * @param rearLeftMotor   The back left SpeedController object used to drive
-   *                        the robot.
-   * @param frontRightMotor The front right SpeedController object used to drive
-   *                        the robot.
-   * @param rearRightMotor  The back right SpeedController object used to drive
-   *                        the robot.
-   */
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(SpeedController* frontLeftMotor, SpeedController* rearLeftMotor,
-             SpeedController* frontRightMotor, SpeedController* rearRightMotor);
-
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(SpeedController& frontLeftMotor, SpeedController& rearLeftMotor,
-             SpeedController& frontRightMotor, SpeedController& rearRightMotor);
-
-  WPI_DEPRECATED("Use DifferentialDrive or MecanumDrive classes instead.")
-  RobotDrive(std::shared_ptr<SpeedController> frontLeftMotor,
-             std::shared_ptr<SpeedController> rearLeftMotor,
-             std::shared_ptr<SpeedController> frontRightMotor,
-             std::shared_ptr<SpeedController> rearRightMotor);
-
-  virtual ~RobotDrive() = default;
-
-  RobotDrive(RobotDrive&&) = default;
-  RobotDrive& operator=(RobotDrive&&) = default;
-
-  /**
-   * Drive the motors at "outputMagnitude" and "curve".
-   *
-   * Both outputMagnitude and curve are -1.0 to +1.0 values, where 0.0
-   * represents stopped and not turning. curve < 0 will turn left and curve > 0
-   * will turn right.
-   *
-   * The algorithm for steering provides a constant turn radius for any normal
-   * speed range, both forward and backward. Increasing m_sensitivity causes
-   * sharper turns for fixed values of curve.
-   *
-   * This function will most likely be used in an autonomous routine.
-   *
-   * @param outputMagnitude The speed setting for the outside wheel in a turn,
-   *                        forward or backwards, +1 to -1.
-   * @param curve           The rate of turn, constant for different forward
-   *                        speeds. Set curve < 0 for left turn or curve > 0 for
-   *                        right turn.
-   *
-   * Set curve = e^(-r/w) to get a turn radius r for wheelbase w of your robot.
-   * Conversely, turn radius r = -ln(curve)*w for a given value of curve and
-   * wheelbase w.
-   */
-  void Drive(double outputMagnitude, double curve);
-
-  /**
-   * Provide tank steering using the stored robot configuration.
-   *
-   * Drive the robot using two joystick inputs. The Y-axis will be selected from
-   * each Joystick object.
-   *
-   * @param leftStick  The joystick to control the left side of the robot.
-   * @param rightStick The joystick to control the right side of the robot.
-   * @param squaredInputs If true, the sensitivity will be decreased for small
-   *                      values
-   */
-  void TankDrive(GenericHID* leftStick, GenericHID* rightStick,
-                 bool squaredInputs = true);
-
-  /**
-   * Provide tank steering using the stored robot configuration.
-   *
-   * Drive the robot using two joystick inputs. The Y-axis will be selected from
-   * each Joystick object.
-   *
-   * @param leftStick  The joystick to control the left side of the robot.
-   * @param rightStick The joystick to control the right side of the robot.
-   * @param squaredInputs If true, the sensitivity will be decreased for small
-   *                      values
-   */
-  void TankDrive(GenericHID& leftStick, GenericHID& rightStick,
-                 bool squaredInputs = true);
-
-  /**
-   * Provide tank steering using the stored robot configuration.
-   *
-   * This function lets you pick the axis to be used on each Joystick object for
-   * the left and right sides of the robot.
-   *
-   * @param leftStick  The Joystick object to use for the left side of the
-   *                   robot.
-   * @param leftAxis   The axis to select on the left side Joystick object.
-   * @param rightStick The Joystick object to use for the right side of the
-   *                   robot.
-   * @param rightAxis  The axis to select on the right side Joystick object.
-   * @param squaredInputs If true, the sensitivity will be decreased for small
-   *                      values
-   */
-  void TankDrive(GenericHID* leftStick, int leftAxis, GenericHID* rightStick,
-                 int rightAxis, bool squaredInputs = true);
-
-  void TankDrive(GenericHID& leftStick, int leftAxis, GenericHID& rightStick,
-                 int rightAxis, bool squaredInputs = true);
-
-  /**
-   * Provide tank steering using the stored robot configuration.
-   *
-   * This function lets you directly provide joystick values from any source.
-   *
-   * @param leftValue  The value of the left stick.
-   * @param rightValue The value of the right stick.
-   * @param squaredInputs If true, the sensitivity will be decreased for small
-   *                      values
-   */
-  void TankDrive(double leftValue, double rightValue,
-                 bool squaredInputs = true);
-
-  /**
-   * Arcade drive implements single stick driving.
-   *
-   * Given a single Joystick, the class assumes the Y axis for the move value
-   * and the X axis for the rotate value. (Should add more information here
-   * regarding the way that arcade drive works.)
-   *
-   * @param stick         The joystick to use for Arcade single-stick driving.
-   *                      The Y-axis will be selected for forwards/backwards and
-   *                      the X-axis will be selected for rotation rate.
-   * @param squaredInputs If true, the sensitivity will be decreased for small
-   *                      values
-   */
-  void ArcadeDrive(GenericHID* stick, bool squaredInputs = true);
-
-  /**
-   * Arcade drive implements single stick driving.
-   *
-   * Given a single Joystick, the class assumes the Y axis for the move value
-   * and the X axis for the rotate value. (Should add more information here
-   * regarding the way that arcade drive works.)
-   *
-   * @param stick         The joystick to use for Arcade single-stick driving.
-   *                      The Y-axis will be selected for forwards/backwards and
-   *                      the X-axis will be selected for rotation rate.
-   * @param squaredInputs If true, the sensitivity will be decreased for small
-   *                      values
-   */
-  void ArcadeDrive(GenericHID& stick, bool squaredInputs = true);
-
-  /**
-   * Arcade drive implements single stick driving.
-   *
-   * Given two joystick instances and two axis, compute the values to send to
-   * either two or four motors.
-   *
-   * @param moveStick     The Joystick object that represents the
-   *                      forward/backward direction
-   * @param moveAxis      The axis on the moveStick object to use for
-   *                      forwards/backwards (typically Y_AXIS)
-   * @param rotateStick   The Joystick object that represents the rotation value
-   * @param rotateAxis    The axis on the rotation object to use for the rotate
-   *                      right/left (typically X_AXIS)
-   * @param squaredInputs Setting this parameter to true increases the
-   *                      sensitivity at lower speeds
-   */
-  void ArcadeDrive(GenericHID* moveStick, int moveChannel,
-                   GenericHID* rotateStick, int rotateChannel,
-                   bool squaredInputs = true);
-
-  /**
-   * Arcade drive implements single stick driving.
-   *
-   * Given two joystick instances and two axis, compute the values to send to
-   * either two or four motors.
-   *
-   * @param moveStick     The Joystick object that represents the
-   *                      forward/backward direction
-   * @param moveAxis      The axis on the moveStick object to use for
-   *                      forwards/backwards (typically Y_AXIS)
-   * @param rotateStick   The Joystick object that represents the rotation value
-   * @param rotateAxis    The axis on the rotation object to use for the rotate
-   *                      right/left (typically X_AXIS)
-   * @param squaredInputs Setting this parameter to true increases the
-   *                      sensitivity at lower speeds
-   */
-  void ArcadeDrive(GenericHID& moveStick, int moveChannel,
-                   GenericHID& rotateStick, int rotateChannel,
-                   bool squaredInputs = true);
-
-  /**
-   * Arcade drive implements single stick driving.
-   *
-   * This function lets you directly provide joystick values from any source.
-   *
-   * @param moveValue     The value to use for fowards/backwards
-   * @param rotateValue   The value to use for the rotate right/left
-   * @param squaredInputs If set, increases the sensitivity at low speeds
-   */
-  void ArcadeDrive(double moveValue, double rotateValue,
-                   bool squaredInputs = true);
-
-  /**
-   * Drive method for Mecanum wheeled robots.
-   *
-   * A method for driving with Mecanum wheeled robots. There are 4 wheels
-   * on the robot, arranged so that the front and back wheels are toed in 45
-   * degrees.
-   * When looking at the wheels from the top, the roller axles should form an X
-   * across the robot.
-   *
-   * This is designed to be directly driven by joystick axes.
-   *
-   * @param x         The speed that the robot should drive in the X direction.
-   *                  [-1.0..1.0]
-   * @param y         The speed that the robot should drive in the Y direction.
-   *                  This input is inverted to match the forward == -1.0 that
-   *                  joysticks produce. [-1.0..1.0]
-   * @param rotation  The rate of rotation for the robot that is completely
-   *                  independent of the translation. [-1.0..1.0]
-   * @param gyroAngle The current angle reading from the gyro.  Use this to
-   *                  implement field-oriented controls.
-   */
-  void MecanumDrive_Cartesian(double x, double y, double rotation,
-                              double gyroAngle = 0.0);
-
-  /**
-   * Drive method for Mecanum wheeled robots.
-   *
-   * A method for driving with Mecanum wheeled robots. There are 4 wheels
-   * on the robot, arranged so that the front and back wheels are toed in 45
-   * degrees.
-   * When looking at the wheels from the top, the roller axles should form an X
-   * across the robot.
-   *
-   * @param magnitude The speed that the robot should drive in a given
-   *                  direction. [-1.0..1.0]
-   * @param direction The direction the robot should drive in degrees. The
-   *                  direction and maginitute are independent of the rotation
-   *                  rate.
-   * @param rotation  The rate of rotation for the robot that is completely
-   *                  independent of the magnitute or direction. [-1.0..1.0]
-   */
-  void MecanumDrive_Polar(double magnitude, double direction, double rotation);
-
-  /**
-   * Holonomic Drive method for Mecanum wheeled robots.
-   *
-   * 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]
-   * @param direction The direction the robot should drive. The direction and
-   *                  magnitude are independent of the rotation rate.
-   * @param rotation  The rate of rotation for the robot that is completely
-   *                  independent of the magnitude or direction.  [-1.0..1.0]
-   */
-  void HolonomicDrive(double magnitude, double direction, double rotation);
-
-  /**
-   * Set the speed of the right and left motors.
-   *
-   * This is used once an appropriate drive setup function is called such as
-   * TwoWheelDrive(). The motors are set to "leftOutput" and "rightOutput"
-   * and includes flipping the direction of one side for opposing motors.
-   *
-   * @param leftOutput  The speed to send to the left side of the robot.
-   * @param rightOutput The speed to send to the right side of the robot.
-   */
-  virtual void SetLeftRightMotorOutputs(double leftOutput, double rightOutput);
-
-  /*
-   * Invert a motor direction.
-   *
-   * This is used when a motor should run in the opposite direction as the drive
-   * code would normally run it. Motors that are direct drive would be inverted,
-   * the Drive code assumes that the motors are geared with one reversal.
-   *
-   * @param motor      The motor index to invert.
-   * @param isInverted True if the motor should be inverted when operated.
-   */
-  void SetInvertedMotor(MotorType motor, bool isInverted);
-
-  /**
-   * Set the turning sensitivity.
-   *
-   * This only impacts the Drive() entry-point.
-   *
-   * @param sensitivity Effectively sets the turning sensitivity (or turn radius
-   *                    for a given value)
-   */
-  void SetSensitivity(double sensitivity);
-
-  /**
-   * Configure the scaling factor for using RobotDrive with motor controllers in
-   * a mode other than PercentVbus.
-   *
-   * @param maxOutput Multiplied with the output percentage computed by the
-   *                  drive functions.
-   */
-  void SetMaxOutput(double maxOutput);
-
-  void StopMotor() override;
-  void GetDescription(wpi::raw_ostream& desc) const override;
-
- protected:
-  /**
-   * Common function to initialize all the robot drive constructors.
-   *
-   * Create a motor safety object (the real reason for the common code) and
-   * initialize all the motor assignments. The default timeout is set for the
-   * robot drive.
-   */
-  void InitRobotDrive();
-
-  /**
-   * Limit motor values to the -1.0 to +1.0 range.
-   */
-  double Limit(double number);
-
-  /**
-   * Normalize all wheel speeds if the magnitude of any wheel is greater than
-   * 1.0.
-   */
-  void Normalize(double* wheelSpeeds);
-
-  /**
-   * Rotate a vector in Cartesian space.
-   */
-  void RotateVector(double& x, double& y, double angle);
-
-  static constexpr int kMaxNumberOfMotors = 4;
-
-  double m_sensitivity = 0.5;
-  double m_maxOutput = 1.0;
-
-  std::shared_ptr<SpeedController> m_frontLeftMotor;
-  std::shared_ptr<SpeedController> m_frontRightMotor;
-  std::shared_ptr<SpeedController> m_rearLeftMotor;
-  std::shared_ptr<SpeedController> m_rearRightMotor;
-
- private:
-  int GetNumMotors() {
-    int motors = 0;
-    if (m_frontLeftMotor) motors++;
-    if (m_frontRightMotor) motors++;
-    if (m_rearLeftMotor) motors++;
-    if (m_rearRightMotor) motors++;
-    return motors;
-  }
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h
index 5f1c136..cb97b13 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RobotState.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/deprecated.h>
+
 namespace frc {
 
 class RobotState {
@@ -16,7 +15,9 @@
   static bool IsDisabled();
   static bool IsEnabled();
   static bool IsEStopped();
+  WPI_DEPRECATED("Use IsTeleop() instead")
   static bool IsOperatorControl();
+  static bool IsTeleop();
   static bool IsAutonomous();
   static bool IsTest();
 };
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h
new file mode 100644
index 0000000..c3a8a0b
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/RuntimeType.h
@@ -0,0 +1,9 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace frc {
+enum RuntimeType { kRoboRIO, kRoboRIO2, kSimulation };
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SD540.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SD540.h
deleted file mode 100644
index 07f7f18..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SD540.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Mindsensors SD540 Speed Controller.
- *
- * Note that the SD540 uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended.
- * The calibration procedure can be found in the SD540 User Manual available
- * from Mindsensors.
- *
- * \li 2.05ms = full "forward"
- * \li 1.55ms = the "high end" of the deadband range
- * \li 1.50ms = center of the deadband range (off)
- * \li 1.44ms = the "low end" of the deadband range
- * \li 0.94ms = full "reverse"
- */
-class SD540 : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a SD540.
-   *
-   * @param channel The PWM channel that the SD540 is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit SD540(int channel);
-
-  SD540(SD540&&) = default;
-  SD540& operator=(SD540&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h
index 70f934c..0ec5ce6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SPI.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -13,10 +10,8 @@
 
 #include <hal/SPITypes.h>
 #include <units/time.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/deprecated.h>
-
-#include "frc/ErrorBase.h"
+#include <wpi/span.h>
 
 namespace frc {
 
@@ -29,7 +24,7 @@
  * It probably should not be used directly.
  *
  */
-class SPI : public ErrorBase {
+class SPI {
  public:
   enum Port { kOnboardCS0 = 0, kOnboardCS1, kOnboardCS2, kOnboardCS3, kMXP };
 
@@ -40,11 +35,13 @@
    */
   explicit SPI(Port port);
 
-  ~SPI() override;
+  ~SPI();
 
   SPI(SPI&&) = default;
   SPI& operator=(SPI&&) = default;
 
+  Port GetPort() const;
+
   /**
    * Configure the rate of the generated clock signal.
    *
@@ -138,10 +135,12 @@
    * If the receive FIFO is empty, there is no active transfer, and initiate
    * is false, errors.
    *
-   * @param initiate If true, this function pushes "0" into the transmit buffer
-   *                 and initiates a transfer. If false, this function assumes
-   *                 that data is already in the receive FIFO from a previous
-   *                 write.
+   * @param initiate     If true, this function pushes "0" into the transmit
+   *                     buffer and initiates a transfer. If false, this
+   *                     function assumes that data is already in the receive
+   *                     FIFO from a previous write.
+   * @param dataReceived Buffer to receive data from the device
+   * @param size         The length of the transaction, in bytes
    */
   virtual int Read(bool initiate, uint8_t* dataReceived, int size);
 
@@ -178,7 +177,7 @@
    * @param dataToSend data to send (maximum 23 bytes)
    * @param zeroSize number of zeros to send after the data
    */
-  void SetAutoTransmitData(wpi::ArrayRef<uint8_t> dataToSend, int zeroSize);
+  void SetAutoTransmitData(wpi::span<const uint8_t> dataToSend, int zeroSize);
 
   /**
    * Start running the automatic SPI transfer engine at a periodic rate.
@@ -248,32 +247,6 @@
                            units::second_t timeout);
 
   /**
-   * Read data that has been transferred by the automatic SPI transfer engine.
-   *
-   * Transfers may be made a byte at a time, so it's necessary for the caller
-   * to handle cases where an entire transfer has not been completed.
-   *
-   * Each received data sequence consists of a timestamp followed by the
-   * received data bytes, one byte per word (in the least significant byte).
-   * The length of each received data sequence is the same as the combined
-   * size of the data and zeroSize set in SetAutoTransmitData().
-   *
-   * 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)
-   * @return Number of words remaining to be read
-   */
-  WPI_DEPRECATED("Use ReadAutoReceivedData with unit-safety instead")
-  int ReadAutoReceivedData(uint32_t* buffer, int numToRead, double timeout);
-
-  /**
    * Get the number of bytes dropped by the automatic SPI transfer engine due
    * to the receive buffer being full.
    *
@@ -297,17 +270,17 @@
   /**
    * Initialize the accumulator.
    *
-   * @param period    Time between reads
-   * @param cmd       SPI command to send to request data
-   * @param xferSize  SPI transfer size, in bytes
-   * @param validMask Mask to apply to received data for validity checking
-   * @param validData After valid_mask is applied, required matching value for
-   *                  validity checking
-   * @param dataShift Bit shift to apply to received data to get actual data
-   *                  value
-   * @param dataSize  Size (in bits) of data field
-   * @param isSigned  Is data field signed?
-   * @param bigEndian Is device big endian?
+   * @param period     Time between reads
+   * @param cmd        SPI command to send to request data
+   * @param xferSize   SPI transfer size, in bytes
+   * @param validMask  Mask to apply to received data for validity checking
+   * @param validValue After valid_mask is applied, required matching value for
+   *                   validity checking
+   * @param dataShift  Bit shift to apply to received data to get actual data
+   *                   value
+   * @param dataSize   Size (in bits) of data field
+   * @param isSigned   Is data field signed?
+   * @param bigEndian  Is device big endian?
    */
   void InitAccumulator(units::second_t period, int cmd, int xferSize,
                        int validMask, int validValue, int dataShift,
@@ -321,17 +294,17 @@
    * -->         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
-   * @param validMask Mask to apply to received data for validity checking
-   * @param validData After valid_mask is applied, required matching value for
-   *                  validity checking
-   * @param dataShift Bit shift to apply to received data to get actual data
-   *                  value
-   * @param dataSize  Size (in bits) of data field
-   * @param isSigned  Is data field signed?
-   * @param bigEndian Is device big endian?
+   * @param period     Time between reads
+   * @param cmd        SPI command to send to request data
+   * @param xferSize   SPI transfer size, in bytes
+   * @param validMask  Mask to apply to received data for validity checking
+   * @param validValue After valid_mask is applied, required matching value for
+   *                   validity checking
+   * @param dataShift  Bit shift to apply to received data to get actual data
+   *                   value
+   * @param dataSize   Size (in bits) of data field
+   * @param isSigned   Is data field signed?
+   * @param bigEndian  Is device big endian?
    */
   WPI_DEPRECATED("Use InitAccumulator with unit-safety instead")
   void InitAccumulator(double period, int cmd, int xferSize, int validMask,
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ScopedTracer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ScopedTracer.h
index 8634c1e..e700616 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/ScopedTracer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/ScopedTracer.h
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <string>
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/Tracer.h"
 
@@ -34,7 +29,7 @@
    * @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(std::string_view name, wpi::raw_ostream& os);
   ~ScopedTracer();
 
   ScopedTracer(const ScopedTracer&) = delete;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SensorUtil.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SensorUtil.h
index ab471b1..60536b1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SensorUtil.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SensorUtil.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -22,14 +19,14 @@
    *
    * @return The number of the default solenoid module.
    */
-  static int GetDefaultSolenoidModule();
+  static int GetDefaultCTREPCMModule();
 
   /**
-   * Check that the solenoid module number is valid. module numbers are 0-based
+   * Get the number of the default solenoid module.
    *
-   * @return Solenoid module is valid and present
+   * @return The number of the default solenoid module.
    */
-  static bool CheckSolenoidModule(int moduleNumber);
+  static int GetDefaultREVPHModule();
 
   /**
    * Check that the digital channel number is valid.
@@ -81,36 +78,11 @@
    */
   static bool CheckAnalogOutputChannel(int channel);
 
-  /**
-   * Verify that the solenoid channel number is within limits.
-   *
-   * @return Solenoid channel is valid
-   */
-  static bool CheckSolenoidChannel(int channel);
-
-  /**
-   * Verify that the power distribution channel number is within limits.
-   *
-   * @return PDP channel is valid
-   */
-  static bool CheckPDPChannel(int channel);
-
-  /**
-   * Verify that the PDP module number is within limits. module numbers are
-   * 0-based
-   *
-   * @return PDP module is valid
-   */
-  static bool CheckPDPModule(int module);
-
   static const int kDigitalChannels;
   static const int kAnalogInputs;
   static const int kAnalogOutputs;
-  static const int kSolenoidChannels;
-  static const int kSolenoidModules;
   static const int kPwmChannels;
   static const int kRelayChannels;
-  static const int kPDPChannels;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h
index 7c44c75..7fff14a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SerialPort.h
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <string>
+#include <string_view>
 
 #include <hal/Types.h>
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-#include <wpi/deprecated.h>
-
-#include "frc/ErrorBase.h"
+#include <units/time.h>
 
 namespace frc {
 
@@ -30,7 +24,7 @@
  * and the NI-VISA Programmer's Reference Manual here:
  *   http://www.ni.com/pdf/manuals/370132c.pdf
  */
-class SerialPort : public ErrorBase {
+class SerialPort {
  public:
   enum Parity {
     kParity_None = 0,
@@ -68,8 +62,9 @@
    * @param stopBits The number of stop bits to use as defined by the enum
    *                 StopBits.
    */
-  SerialPort(int baudRate, Port port = kOnboard, int dataBits = 8,
-             Parity parity = kParity_None, StopBits stopBits = kStopBits_One);
+  explicit SerialPort(int baudRate, Port port = kOnboard, int dataBits = 8,
+                      Parity parity = kParity_None,
+                      StopBits stopBits = kStopBits_One);
 
   /**
    * Create an instance of a Serial Port class.
@@ -86,7 +81,7 @@
    * @param stopBits The number of stop bits to use as defined by the enum
    *                 StopBits.
    */
-  SerialPort(int baudRate, const wpi::Twine& portName, Port port = kOnboard,
+  SerialPort(int baudRate, std::string_view portName, Port port = kOnboard,
              int dataBits = 8, Parity parity = kParity_None,
              StopBits stopBits = kStopBits_One);
 
@@ -149,10 +144,10 @@
    * Use Write({data, len}) to get a buffer that is shorter than the length of
    * the string.
    *
-   * @param buffer StringRef to the buffer to read the bytes from.
+   * @param buffer the buffer to read the bytes from.
    * @return The number of bytes actually written into the port.
    */
-  int Write(wpi::StringRef buffer);
+  int Write(std::string_view buffer);
 
   /**
    * Configure the timeout of the serial port.
@@ -160,9 +155,9 @@
    * This defines the timeout for transactions with the hardware.
    * It will affect reads and very large writes.
    *
-   * @param timeout The number of seconds to to wait for I/O.
+   * @param timeout The time to wait for I/O.
    */
-  void SetTimeout(double timeout);
+  void SetTimeout(units::second_t timeout);
 
   /**
    * Specify the size of the input buffer.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h
index 16b4281..b1df655 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Servo.h
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include "frc/PWM.h"
-#include "frc/SpeedController.h"
 
 namespace frc {
 
@@ -68,7 +64,7 @@
    * X being set and angles of more than Y degrees result in an angle of Y being
    * set.
    *
-   * @param degrees The angle in degrees to set the servo.
+   * @param angle The angle in degrees to set the servo.
    */
   void SetAngle(double angle);
 
@@ -96,7 +92,7 @@
    */
   double GetMinAngle() const;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   double GetServoAngleRange() const;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SlewRateLimiter.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
deleted file mode 100644
index d18fb09..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SlewRateLimiter.h
+++ /dev/null
@@ -1,76 +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 <frc2/Timer.h>
-
-#include <algorithm>
-
-#include <units/time.h>
-
-namespace frc {
-/**
- * A class that limits the rate of change of an input value.  Useful for
- * implementing voltage, setpoint, and/or output ramps.  A slew-rate limit
- * is most appropriate when the quantity being controlled is a velocity or
- * a voltage; when controlling a position, consider using a TrapezoidProfile
- * instead.
- *
- * @see TrapezoidProfile
- */
-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>;
-
-  /**
-   * Creates a new SlewRateLimiter with the given rate limit and initial value.
-   *
-   * @param rateLimit The rate-of-change limit.
-   * @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_prevTime{frc2::Timer::GetFPGATimestamp()} {}
-
-  /**
-   * Filters the input to limit its slew rate.
-   *
-   * @param input The input value whose slew rate is to be limited.
-   * @return The filtered value, which will not change faster than the slew
-   * rate.
-   */
-  Unit_t Calculate(Unit_t input) {
-    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;
-  }
-
-  /**
-   * Resets the slew rate limiter to the specified value; ignores the rate limit
-   * when doing so.
-   *
-   * @param value The value to reset to.
-   */
-  void Reset(Unit_t value) {
-    m_prevVal = value;
-    m_prevTime = frc2::Timer::GetFPGATimestamp();
-  }
-
- private:
-  Rate_t m_rateLimit;
-  Unit_t m_prevVal;
-  units::second_t m_prevTime;
-};
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Solenoid.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Solenoid.h
index 9939b41..a09ffc0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Solenoid.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Solenoid.h
@@ -1,46 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <hal/Types.h>
+#include <memory>
 
-#include "frc/SolenoidBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <hal/Types.h>
+#include <units/time.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/PneumaticsModuleType.h"
 
 namespace frc {
 
-class SendableBuilder;
-
 /**
- * Solenoid class for running high voltage Digital Output (PCM).
+ * Solenoid class for running high voltage Digital Output on a pneumatics
+ * module.
  *
  * The Solenoid class is typically used for pneumatics solenoids, but could be
- * used for any device within the current spec of the PCM.
+ * used for any device within the current spec of the module.
  */
-class Solenoid : public SolenoidBase,
-                 public Sendable,
-                 public SendableHelper<Solenoid> {
+class Solenoid : public wpi::Sendable, public wpi::SendableHelper<Solenoid> {
  public:
   /**
-   * Constructor using the default PCM ID (0).
+   * Constructs a solenoid for a specified module and type.
    *
-   * @param channel The channel on the PCM to control (0..7).
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
    */
-  explicit Solenoid(int channel);
+  Solenoid(int module, PneumaticsModuleType moduleType, int channel);
 
   /**
-   * Constructor.
+   * Constructs a solenoid for a default module and specified type.
    *
-   * @param moduleNumber The CAN ID of the PCM the solenoid is attached to
-   * @param channel      The channel on the PCM to control (0..7).
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
    */
-  Solenoid(int moduleNumber, int channel);
+  Solenoid(PneumaticsModuleType moduleType, int channel);
 
   ~Solenoid() override;
 
@@ -70,42 +70,47 @@
   void Toggle();
 
   /**
-   * Check if solenoid is blacklisted.
+   * Get the channel this solenoid is connected to.
+   */
+  int GetChannel() const;
+
+  /**
+   * Check if solenoid is Disabled.
    *
-   * If a solenoid is shorted, it is added to the blacklist and
+   * If a solenoid is shorted, it is added to the DisabledList and
    * disabled until power cycle, or until faults are cleared.
    *
    * @see ClearAllPCMStickyFaults()
    *
    * @return If solenoid is disabled due to short.
    */
-  bool IsBlackListed() const;
+  bool IsDisabled() const;
 
   /**
    * Set the pulse duration in the PCM. This is used in conjunction with
    * the startPulse method to allow the PCM to control the timing of a pulse.
    * The timing can be controlled in 0.01 second increments.
    *
-   * @param durationSeconds The duration of the pulse, from 0.01 to 2.55
-   *                        seconds.
+   * @param duration The duration of the pulse, from 0.01 to 2.55 seconds.
    *
    * @see startPulse()
    */
-  void SetPulseDuration(double durationSeconds);
+  void SetPulseDuration(units::second_t duration);
 
   /**
-   * Trigger the PCM to generate a pulse of the duration set in
+   * %Trigger the PCM to generate a pulse of the duration set in
    * setPulseDuration.
    *
    * @see setPulseDuration()
    */
   void StartPulse();
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
-  hal::Handle<HAL_SolenoidHandle> m_solenoidHandle;
-  int m_channel;  // The channel on the module to control
+  std::shared_ptr<PneumaticsBase> m_module;
+  int m_mask;
+  int m_channel;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SolenoidBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SolenoidBase.h
deleted file mode 100644
index 314df5c..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SolenoidBase.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/ErrorBase.h"
-
-namespace frc {
-
-/**
- * SolenoidBase class is the common base class for the Solenoid and
- * DoubleSolenoid classes.
- */
-class SolenoidBase : public ErrorBase {
- public:
-  /**
-   * Read all 8 solenoids as a single byte
-   *
-   * @param module the module to read from
-   * @return The current value of all 8 solenoids on the module.
-   */
-  static int GetAll(int module);
-
-  /**
-   * Read all 8 solenoids as a single byte
-   *
-   * @return The current value of all 8 solenoids on the module.
-   */
-  int GetAll() const;
-
-  /**
-   * Reads complete solenoid blacklist for all 8 solenoids as a single byte.
-   *
-   * If a solenoid is shorted, it is added to the blacklist and
-   * disabled until power cycle, or until faults are cleared.
-   * @see ClearAllPCMStickyFaults()
-   *
-   * @param module the module to read from
-   * @return The solenoid blacklist of all 8 solenoids on the module.
-   */
-  static int GetPCMSolenoidBlackList(int module);
-
-  /**
-   * Reads complete solenoid blacklist for all 8 solenoids as a single byte.
-   *
-   * If a solenoid is shorted, it is added to the blacklist and
-   * disabled until power cycle, or until faults are cleared.
-   * @see ClearAllPCMStickyFaults()
-   *
-   * @return The solenoid blacklist of all 8 solenoids on the module.
-   */
-  int GetPCMSolenoidBlackList() const;
-
-  /**
-   * @param module the module to read from
-   * @return true if PCM sticky fault is set : The common highside solenoid
-   *         voltage rail is too low, most likely a solenoid channel is shorted.
-   */
-  static bool GetPCMSolenoidVoltageStickyFault(int module);
-
-  /**
-   * @return true if PCM sticky fault is set : The common highside solenoid
-   *         voltage rail is too low, most likely a solenoid channel is shorted.
-   */
-  bool GetPCMSolenoidVoltageStickyFault() const;
-
-  /**
-   * @param module the module to read from
-   * @return true if PCM is in fault state : The common highside solenoid
-   *         voltage rail is too low, most likely a solenoid channel is shorted.
-   */
-  static bool GetPCMSolenoidVoltageFault(int module);
-
-  /**
-   * @return true if PCM is in fault state : The common highside solenoid
-   *         voltage rail is too low, most likely a solenoid channel is shorted.
-   */
-  bool GetPCMSolenoidVoltageFault() const;
-
-  /**
-   * Clear ALL sticky faults inside PCM that Compressor is wired to.
-   *
-   * If a sticky fault is set, then it will be persistently cleared.  Compressor
-   * drive maybe momentarily disable while flags are being cleared. Care should
-   * be taken to not call this too frequently, otherwise normal compressor
-   * functionality may be prevented.
-   *
-   * If no sticky faults are set then this call will have no effect.
-   *
-   * @param module the module to read from
-   */
-  static void ClearAllPCMStickyFaults(int module);
-
-  /**
-   * Clear ALL sticky faults inside PCM that Compressor is wired to.
-   *
-   * If a sticky fault is set, then it will be persistently cleared.  Compressor
-   * drive maybe momentarily disable while flags are being cleared. Care should
-   * be taken to not call this too frequently, otherwise normal compressor
-   * functionality may be prevented.
-   *
-   * If no sticky faults are set then this call will have no effect.
-   */
-  void ClearAllPCMStickyFaults();
-
- protected:
-  /**
-   * Constructor.
-   *
-   * @param moduleNumber The CAN PCM ID.
-   */
-  explicit SolenoidBase(int pcmID);
-
-  SolenoidBase(SolenoidBase&&) = default;
-  SolenoidBase& operator=(SolenoidBase&&) = default;
-
-  static constexpr int m_maxModules = 63;
-  static constexpr int m_maxPorts = 8;
-
-  int m_moduleNumber;  // PCM module number
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Spark.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Spark.h
deleted file mode 100644
index 24ed8f5..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Spark.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * REV Robotics SPARK Speed Controller.
- *
- * Note that the SPARK uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended.
- * The calibration procedure can be found in the SPARK User Manual available
- * from REV Robotics.
- *
- * \li 2.003ms = full "forward"
- * \li 1.550ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.460ms = the "low end" of the deadband range
- * \li 0.999ms = full "reverse"
- */
-class Spark : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a SPARK.
-   *
-   * @param channel The PWM channel that the SPARK is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit Spark(int channel);
-
-  Spark(Spark&&) = default;
-  Spark& operator=(Spark&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedController.h
index 3053c3b..8473015 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedController.h
@@ -1,22 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <units/voltage.h>
-
-#include "frc/PIDOutput.h"
+#include <wpi/deprecated.h>
 
 namespace frc {
 
 /**
  * Interface for speed controlling devices.
+ *
+ * @deprecated Use MotorController.
  */
-class SpeedController : public PIDOutput {
+class WPI_DEPRECATED("use MotorController") SpeedController {
  public:
   virtual ~SpeedController() = default;
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
index e62563c..5a097b5 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.h
@@ -1,24 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <vector>
 
-#include "frc/SpeedController.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/deprecated.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/motorcontrol/MotorController.h"
 
 namespace frc {
 
-class SpeedControllerGroup : public Sendable,
-                             public SpeedController,
-                             public SendableHelper<SpeedControllerGroup> {
+/**
+ * Allows multiple SpeedController objects to be linked together.
+ *
+ * @deprecated Use MotorControllerGroup.
+ */
+class WPI_DEPRECATED("use MotorControllerGroup") SpeedControllerGroup
+    : public wpi::Sendable,
+      public MotorController,
+      public wpi::SendableHelper<SpeedControllerGroup> {
  public:
   template <class... SpeedControllers>
   explicit SpeedControllerGroup(SpeedController& speedController,
@@ -35,9 +40,8 @@
   bool GetInverted() const override;
   void Disable() override;
   void StopMotor() override;
-  void PIDWrite(double output) override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   bool m_isInverted = false;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
index d9e773d..d5f17b4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SpeedControllerGroup.inc
@@ -1,15 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <vector>
 
+#include "frc/SpeedControllerGroup.h"
+
 namespace frc {
 
 template <class... SpeedControllers>
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h
new file mode 100644
index 0000000..03cc4f8
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/SynchronousInterrupt.h
@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <hal/Types.h>
+#include <units/time.h>
+
+namespace frc {
+class DigitalSource;
+
+/**
+ * Class for handling synchronous (blocking) interrupts.
+ *
+ * <p> By default, interrupts will occur on rising edge.
+ *
+ * <p> Asynchronous interrupts are handled by the AsynchronousInterrupt class.
+ */
+class SynchronousInterrupt {
+ public:
+  enum WaitResult {
+    kTimeout = 0x0,
+    kRisingEdge = 0x1,
+    kFallingEdge = 0x100,
+    kBoth = 0x101,
+  };
+
+  /**
+   * Construct a Synchronous Interrupt from a Digital Source.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   */
+  explicit SynchronousInterrupt(DigitalSource& source);
+
+  /**
+   * Construct a Synchronous Interrupt from a Digital Source.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   */
+  explicit SynchronousInterrupt(DigitalSource* source);
+
+  /**
+   * Construct a Synchronous Interrupt from a Digital Source.
+   *
+   * @param source the DigitalSource the interrupts are triggered from
+   */
+  explicit SynchronousInterrupt(std::shared_ptr<DigitalSource> source);
+
+  ~SynchronousInterrupt();
+
+  SynchronousInterrupt(SynchronousInterrupt&&) = default;
+  SynchronousInterrupt& operator=(SynchronousInterrupt&&) = default;
+
+  /**
+   * Wait for an interrupt to occur.
+   *
+   * <p> Both rising and falling edge can be returned if both a rising and
+   * falling happened between calls, and ignorePrevious is false.
+   *
+   * @param timeout The timeout to wait for. 0s or less will return immediately.
+   * @param ignorePrevious True to ignore any previous interrupts, false to
+   * return interrupt value if an interrupt has occured since last call.
+   * @return The edge(s) that were triggered, or timeout.
+   */
+  WaitResult WaitForInterrupt(units::second_t timeout,
+                              bool ignorePrevious = true);
+
+  /**
+   * Set which edges cause an interrupt to occur.
+   *
+   * @param risingEdge true to trigger on rising edge, false otherwise.
+   * @param fallingEdge true to trigger on falling edge, false otherwise
+   */
+  void SetInterruptEdges(bool risingEdge, bool fallingEdge);
+
+  /**
+   * Get the timestamp (relative to FPGA Time) of the last rising edge.
+   *
+   * @return the timestamp in seconds relative to getFPGATime
+   */
+  units::second_t GetRisingTimestamp();
+
+  /**
+   * Get the timestamp of the last falling edge.
+   *
+   * <p>This function does not require the interrupt to be enabled to work.
+   *
+   * <p>This only works if falling edge was configured using setInterruptEdges.
+   * @return the timestamp in seconds relative to getFPGATime
+   */
+  units::second_t GetFallingTimestamp();
+
+  /**
+   * Wake up an existing wait call. Can be called from any thread.
+   */
+  void WakeupWaitingInterrupt();
+
+ private:
+  void InitSynchronousInterrupt();
+  std::shared_ptr<DigitalSource> m_source;
+  hal::Handle<HAL_InterruptHandle> m_handle;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Talon.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Talon.h
deleted file mode 100644
index 6e92dfc..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Talon.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
- *
- * Note that the Talon uses the following bounds for PWM values. These values
- * should work reasonably well for most controllers, but if users experience
- * issues such as asymmetric behavior around the deadband or inability to
- * saturate the controller in either direction, calibration is recommended.
- * The calibration procedure can be found in the Talon User Manual available
- * from CTRE.
- *
- * \li 2.037ms = full "forward"
- * \li 1.539ms = the "high end" of the deadband range
- * \li 1.513ms = center of the deadband range (off)
- * \li 1.487ms = the "low end" of the deadband range
- * \li 0.989ms = full "reverse"
- */
-class Talon : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a Talon (original or Talon SR).
-   *
-   * @param channel The PWM channel number that the Talon is attached to. 0-9
-   *                are on-board, 10-19 are on the MXP port
-   */
-  explicit Talon(int channel);
-
-  Talon(Talon&&) = default;
-  Talon& operator=(Talon&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Threads.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Threads.h
index 3c44961..5822670 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Threads.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Threads.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,43 +12,45 @@
  * Get the thread priority for the specified thread.
  *
  * @param thread     Reference to the thread to get the priority for.
- * @param isRealTime Set to true if thread is realtime, otherwise false.
- * @return The current thread priority. Scaled 1-99, with 1 being highest.
+ * @param isRealTime Set to true if thread is real-time, otherwise false.
+ * @return           The current thread priority. For real-time, this is 1-99
+ *                   with 99 being highest. For non-real-time, this is 0. See
+ *                   "man 7 sched" for details.
  */
 int GetThreadPriority(std::thread& thread, bool* isRealTime);
 
 /**
- * Get the thread priority for the current thread
+ * Get the thread priority for the current thread.
  *
- * @param isRealTime Set to true if thread is realtime, otherwise false.
- * @return The current thread priority. Scaled 1-99.
+ * @param isRealTime Set to true if thread is real-time, otherwise false.
+ * @return           The current thread priority. For real-time, this is 1-99
+ *                   with 99 being highest. For non-real-time, this is 0. See
+ *                   "man 7 sched" for details.
  */
 int GetCurrentThreadPriority(bool* isRealTime);
 
 /**
- * Sets the thread priority for the specified thread
+ * Sets the thread priority for the specified thread.
  *
  * @param thread   Reference to the thread to set the priority of.
- * @param realTime Set to true to set a realtime priority, false for standard
+ * @param realTime Set to true to set a real-time priority, false for standard
  *                 priority.
- * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
- *                 highest. On RoboRIO, priority is ignored for non realtime
- *                 setting.
- *
- * @return The success state of setting the priority
+ * @param priority Priority to set the thread to. For real-time, this is 1-99
+ *                 with 99 being highest. For non-real-time, this is forced to
+ *                 0. See "man 7 sched" for more details.
+ * @return         True on success.
  */
 bool SetThreadPriority(std::thread& thread, bool realTime, int priority);
 
 /**
- * Sets the thread priority for the current thread
+ * Sets the thread priority for the current thread.
  *
- * @param realTime Set to true to set a realtime priority, false for standard
+ * @param realTime Set to true to set a real-time priority, false for standard
  *                 priority.
- * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
- *                 highest. On RoboRIO, priority is ignored for non realtime
- *                 setting.
- *
- * @return The success state of setting the priority
+ * @param priority Priority to set the thread to. For real-time, this is 1-99
+ *                 with 99 being highest. For non-real-time, this is forced to
+ *                 0. See "man 7 sched" for more details.
+ * @return         True on success.
  */
 bool SetCurrentThreadPriority(bool realTime, int priority);
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h
index 9dbc3f2..cc64c03 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimedRobot.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
+#include <utility>
 #include <vector>
 
 #include <hal/Types.h>
@@ -16,9 +14,8 @@
 #include <wpi/deprecated.h>
 #include <wpi/priority_queue.h>
 
-#include "frc/ErrorBase.h"
 #include "frc/IterativeRobotBase.h"
-#include "frc2/Timer.h"
+#include "frc/Timer.h"
 
 namespace frc {
 
@@ -31,9 +28,9 @@
  * Periodic() functions from the base class are called on an interval by a
  * Notifier instance.
  */
-class TimedRobot : public IterativeRobotBase, public ErrorBase {
+class TimedRobot : public IterativeRobotBase {
  public:
-  static constexpr units::second_t kDefaultPeriod = 20_ms;
+  static constexpr auto kDefaultPeriod = 20_ms;
 
   /**
    * Provide an alternate "main loop" via StartCompetition().
@@ -46,11 +43,6 @@
   void EndCompetition() override;
 
   /**
-   * Get the time period between calls to Periodic() functions.
-   */
-  units::second_t GetPeriod() const;
-
-  /**
    * Constructor for TimedRobot.
    *
    * @deprecated use unit safe constructor instead.
@@ -105,14 +97,13 @@
      */
     Callback(std::function<void()> func, units::second_t startTime,
              units::second_t period, units::second_t offset)
-        : func{func},
+        : func{std::move(func)},
           period{period},
-          expirationTime{
-              startTime + offset +
-              units::math::floor((frc2::Timer::GetFPGATimestamp() - startTime) /
-                                 period) *
-                  period +
-              period} {}
+          expirationTime{startTime + offset +
+                         units::math::floor(
+                             (Timer::GetFPGATimestamp() - startTime) / period) *
+                             period +
+                         period} {}
 
     bool operator>(const Callback& rhs) const {
       return expirationTime > rhs.expirationTime;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Timer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Timer.h
index ac166c5..0aed658 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Timer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Timer.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include "frc2/Timer.h"
+#include <units/time.h>
+#include <wpi/deprecated.h>
 
 namespace frc {
 
@@ -21,23 +19,20 @@
  *
  * @param seconds Length of time to pause, in seconds.
  */
-void Wait(double seconds);
+void Wait(units::second_t seconds);
 
 /**
  * @brief  Gives real-time clock system time with nanosecond resolution
  * @return The time, just in case you want the robot to start autonomous at 8pm
  *         on Saturday.
  */
-double GetTime();
+units::second_t GetTime();
 
 /**
- * Timer objects measure accumulated time in seconds.
+ * A timer class.
  *
- * The timer object functions like a stopwatch. It can be started, stopped, and
- * cleared. When the timer is running its value counts up in seconds. When
- * stopped, the timer holds the current value. The implementation simply records
- * the time when started and subtracts the current time whenever the value is
- * requested.
+ * Note that if the user calls frc::sim::RestartTiming(), they should also reset
+ * the timer so Get() won't return a negative duration.
  */
 class Timer {
  public:
@@ -51,10 +46,10 @@
 
   virtual ~Timer() = default;
 
-  Timer(const Timer& rhs) = default;
-  Timer& operator=(const Timer& rhs) = default;
-  Timer(Timer&& rhs) = default;
-  Timer& operator=(Timer&& rhs) = default;
+  Timer(const Timer&) = default;
+  Timer& operator=(const Timer&) = default;
+  Timer(Timer&&) = default;
+  Timer& operator=(Timer&&) = default;
 
   /**
    * Get the current time from the timer. If the clock is running it is derived
@@ -63,7 +58,7 @@
    *
    * @return Current time value for this timer in seconds
    */
-  double Get() const;
+  units::second_t Get() const;
 
   /**
    * Reset the timer by setting the time to 0.
@@ -77,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();
 
@@ -91,14 +87,34 @@
   void Stop();
 
   /**
+   * Check if the period specified has passed.
+   *
+   * @param period 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.
    *
-   * @param period The period to check for (in seconds).
+   * @param period The period to check for.
+   * @return       True if the period has passed.
+   * @deprecated Use AdvanceIfElapsed() instead.
+   */
+  WPI_DEPRECATED("Use AdvanceIfElapsed() instead.")
+  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 HasPeriodPassed(double period);
+  bool AdvanceIfElapsed(units::second_t period);
 
   /**
    * Return the FPGA system clock time in seconds.
@@ -108,7 +124,7 @@
    *
    * @returns Robot running time in seconds.
    */
-  static double GetFPGATimestamp();
+  static units::second_t GetFPGATimestamp();
 
   /**
    * Return the approximate match time.
@@ -125,10 +141,12 @@
    *
    * @return Time remaining in current match period (auto or teleop)
    */
-  static double GetMatchTime();
+  static units::second_t GetMatchTime();
 
  private:
-  frc2::Timer m_timer;
+  units::second_t m_startTime = 0_s;
+  units::second_t m_accumulatedTime = 0_s;
+  bool m_running = false;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimesliceRobot.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimesliceRobot.h
new file mode 100644
index 0000000..bc045d5
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/TimesliceRobot.h
@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include <units/time.h>
+
+#include "frc/TimedRobot.h"
+
+namespace frc {
+
+/**
+ * TimesliceRobot extends the TimedRobot robot program framework to provide
+ * timeslice scheduling of periodic functions.
+ *
+ * The TimesliceRobot class is intended to be subclassed by a user creating a
+ * robot program.
+ *
+ * This class schedules robot operations serially in a timeslice format.
+ * TimedRobot's periodic functions are the first in the timeslice table with 0
+ * ms offset and 20 ms period. You can schedule additional controller periodic
+ * functions at a shorter period (5 ms by default). You give each one a
+ * timeslice duration, then they're run sequentially. The main benefit of this
+ * approach is consistent starting times for each controller periodic, which can
+ * make odometry and estimators more accurate and controller outputs change more
+ * consistently.
+ *
+ * Here's an example of measured subsystem durations and their timeslice
+ * allocations:
+ *
+ * <table>
+ *   <tr>
+ *     <td><b>Subsystem</b></td>
+ *     <td><b>Duration (ms)</b></td>
+ *     <td><b>Allocation (ms)</b></td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Total</b></td>
+ *     <td>5.0</td>
+ *     <td>5.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>TimedRobot</td>
+ *     <td>?</td>
+ *     <td>2.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Drivetrain</td>
+ *     <td>1.32</td>
+ *     <td>1.5</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Flywheel</td>
+ *     <td>0.6</td>
+ *     <td>0.7</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Turret</td>
+ *     <td>0.6</td>
+ *     <td>0.8</td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Free</b></td>
+ *     <td>0.0</td>
+ *     <td>N/A</td>
+ *   </tr>
+ * </table>
+ *
+ * Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms
+ * empty spot in the allocation table for three of the four 5 ms cycles
+ * comprising 20 ms. That's OK because the OS needs time to do other things.
+ *
+ * If the robot periodic functions and the controller periodic functions have a
+ * lot of scheduling jitter that cause them to occasionally overlap with later
+ * timeslices, consider giving the main robot thread a real-time priority using
+ * frc::SetCurrentThreadPriority(). An RT priority of 15 is a reasonable choice.
+ *
+ * If you do enable RT though, <i>make sure your periodic functions do not
+ * block</i>. If they do, the operating system will lock up, and you'll have to
+ * boot the roboRIO into safe mode and delete the robot program to recover.
+ */
+class TimesliceRobot : public TimedRobot {
+ public:
+  /**
+   * Constructor for TimesliceRobot.
+   *
+   * @param robotPeriodicAllocation The allocation to give the TimesliceRobot
+   *                                periodic functions.
+   * @param controllerPeriod The controller period. The sum of all scheduler
+   *                         allocations should be less than or equal to this
+   *                         value.
+   */
+  explicit TimesliceRobot(units::second_t robotPeriodicAllocation,
+                          units::second_t controllerPeriod);
+
+  /**
+   * Schedule a periodic function with the constructor's controller period and
+   * the given allocation. The function's runtime allocation will be placed
+   * after the end of the previous one's.
+   *
+   * If a call to this function makes the allocations exceed the controller
+   * period, an exception will be thrown since that means the TimesliceRobot
+   * periodic functions and the given function will have conflicting
+   * timeslices.
+   *
+   * @param func       Function to schedule.
+   * @param allocation The function's runtime allocation out of the controller
+   *                   period.
+   */
+  void Schedule(std::function<void()> func, units::second_t allocation);
+
+ private:
+  units::second_t m_nextOffset;
+  units::second_t m_controllerPeriod;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Tracer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Tracer.h
index 627b07b..2acb017 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Tracer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Tracer.h
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <chrono>
+#include <string_view>
 
 #include <hal/cpp/fpga_clock.h>
 #include <wpi/StringMap.h>
-#include <wpi/StringRef.h>
 
 namespace wpi {
 class raw_ostream;
@@ -51,7 +48,7 @@
    *
    * @param epochName The name to associate with the epoch.
    */
-  void AddEpoch(wpi::StringRef epochName);
+  void AddEpoch(std::string_view epochName);
 
   /**
    * Prints list of epochs added so far and their times to the DriverStation.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h
index f6d301b..253192f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Ultrasonic.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -13,12 +10,13 @@
 #include <vector>
 
 #include <hal/SimDevice.h>
+#include <units/length.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/Counter.h"
-#include "frc/ErrorBase.h"
-#include "frc/PIDSource.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
@@ -37,13 +35,9 @@
  * received. The time that the line is high determines the round trip distance
  * (time of flight).
  */
-class Ultrasonic : public ErrorBase,
-                   public Sendable,
-                   public PIDSource,
-                   public SendableHelper<Ultrasonic> {
+class Ultrasonic : public wpi::Sendable,
+                   public wpi::SendableHelper<Ultrasonic> {
  public:
-  enum DistanceUnit { kInches = 0, kMilliMeters = 1 };
-
   /**
    * Create an instance of the Ultrasonic Sensor.
    *
@@ -54,9 +48,8 @@
    * @param echoChannel The digital input channel that receives the echo. The
    *                    length of time that the echo is high represents the
    *                    round trip time of the ping, and the distance.
-   * @param units       The units returned in either kInches or kMilliMeters
    */
-  Ultrasonic(int pingChannel, int echoChannel, DistanceUnit units = kInches);
+  Ultrasonic(int pingChannel, int echoChannel);
 
   /**
    * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
@@ -66,10 +59,8 @@
    *                    ping. Requires a 10uS pulse to start.
    * @param echoChannel The digital input object that times the return pulse to
    *                    determine the range.
-   * @param units       The units returned in either kInches or kMilliMeters
    */
-  Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel,
-             DistanceUnit units = kInches);
+  Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel);
 
   /**
    * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
@@ -79,10 +70,8 @@
    *                    ping. Requires a 10uS pulse to start.
    * @param echoChannel The digital input object that times the return pulse to
    *                    determine the range.
-   * @param units       The units returned in either kInches or kMilliMeters
    */
-  Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel,
-             DistanceUnit units = kInches);
+  Ultrasonic(DigitalOutput& pingChannel, DigitalInput& echoChannel);
 
   /**
    * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo
@@ -92,17 +81,17 @@
    *                    ping. Requires a 10uS pulse to start.
    * @param echoChannel The digital input object that times the return pulse to
    *                    determine the range.
-   * @param units       The units returned in either kInches or kMilliMeters
    */
   Ultrasonic(std::shared_ptr<DigitalOutput> pingChannel,
-             std::shared_ptr<DigitalInput> echoChannel,
-             DistanceUnit units = kInches);
+             std::shared_ptr<DigitalInput> echoChannel);
 
   ~Ultrasonic() override;
 
   Ultrasonic(Ultrasonic&&) = default;
   Ultrasonic& operator=(Ultrasonic&&) = default;
 
+  int GetEchoChannel() const;
+
   /**
    * Single ping to ultrasonic sensor.
    *
@@ -138,52 +127,19 @@
   static void SetAutomaticMode(bool enabling);
 
   /**
-   * Get the range in inches from the ultrasonic sensor.
+   * Get the range from the ultrasonic sensor.
    *
-   * @return Range in inches of the target returned from the ultrasonic sensor.
-   *         If there is no valid value yet, i.e. at least one measurement
-   *         hasn't completed, then return 0.
+   * @return Range of the target returned from the ultrasonic sensor. If there
+   *         is no valid value yet, i.e. at least one measurement hasn't
+   *         completed, then return 0.
    */
-  double GetRangeInches() const;
-
-  /**
-   * Get the range in millimeters from the ultrasonic sensor.
-   *
-   * @return Range in millimeters of the target returned by the ultrasonic
-   *         sensor. If there is no valid value yet, i.e. at least one
-   *         measurement hasn't completed, then return 0.
-   */
-  double GetRangeMM() const;
+  units::meter_t GetRange() const;
 
   bool IsEnabled() const;
 
   void SetEnabled(bool enable);
 
-  /**
-   * Set the current DistanceUnit that should be used for the PIDSource base
-   * object.
-   *
-   * @param units The DistanceUnit that should be used.
-   */
-  void SetDistanceUnits(DistanceUnit units);
-
-  /**
-   * Get the current DistanceUnit that is used for the PIDSource base object.
-   *
-   * @return The type of DistanceUnit that is being used.
-   */
-  DistanceUnit GetDistanceUnits() const;
-
-  /**
-   * Get the range in the current DistanceUnit for the PIDSource base object.
-   *
-   * @return The range in DistanceUnit
-   */
-  double PIDGet() override;
-
-  void SetPIDSourceType(PIDSourceType pidSource) override;
-
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   /**
@@ -215,8 +171,8 @@
   static constexpr int kPriority = 64;
 
   // Max time (ms) between readings.
-  static constexpr double kMaxUltrasonicTime = 0.1;
-  static constexpr double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
+  static constexpr auto kMaxUltrasonicTime = 0.1_s;
+  static constexpr auto kSpeedOfSound = 1130_fps;
 
   // Thread doing the round-robin automatic sensing
   static std::thread m_thread;
@@ -231,7 +187,6 @@
   std::shared_ptr<DigitalInput> m_echoChannel;
   bool m_enabled = false;
   Counter m_counter;
-  DistanceUnit m_units;
 
   hal::SimDevice m_simDevice;
   hal::SimBoolean m_simRangeValid;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Utility.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Utility.h
deleted file mode 100644
index a02e583..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Utility.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-/**
- * @file Contains global utility functions
- */
-
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
-
-#define wpi_assert(condition) \
-  wpi_assert_impl(condition, #condition, "", __FILE__, __LINE__, __FUNCTION__)
-#define wpi_assertWithMessage(condition, message)                     \
-  wpi_assert_impl(condition, #condition, message, __FILE__, __LINE__, \
-                  __FUNCTION__)
-
-#define wpi_assertEqual(a, b) \
-  wpi_assertEqual_impl(a, b, #a, #b, "", __FILE__, __LINE__, __FUNCTION__)
-#define wpi_assertEqualWithMessage(a, b, message) \
-  wpi_assertEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, __FUNCTION__)
-
-#define wpi_assertNotEqual(a, b) \
-  wpi_assertNotEqual_impl(a, b, #a, #b, "", __FILE__, __LINE__, __FUNCTION__)
-#define wpi_assertNotEqualWithMessage(a, b, message)                 \
-  wpi_assertNotEqual_impl(a, b, #a, #b, message, __FILE__, __LINE__, \
-                          __FUNCTION__)
-
-/**
- * Assert implementation.
- *
- * This allows breakpoints to be set on an assert. The users don't call this,
- * but instead use the wpi_assert macros in Utility.h.
- */
-bool wpi_assert_impl(bool conditionValue, const wpi::Twine& conditionText,
-                     const wpi::Twine& message, wpi::StringRef fileName,
-                     int lineNumber, wpi::StringRef funcName);
-
-/**
- * Assert equal implementation.
- *
- * This determines whether the two given integers are equal. If not, the value
- * of each is printed along with an optional message string. The users don't
- * call this, but instead use the wpi_assertEqual macros in Utility.h.
- */
-bool wpi_assertEqual_impl(int valueA, int valueB,
-                          const wpi::Twine& valueAString,
-                          const wpi::Twine& valueBString,
-                          const wpi::Twine& message, wpi::StringRef fileName,
-                          int lineNumber, wpi::StringRef funcName);
-
-/**
- * Assert not equal implementation.
- *
- * This determines whether the two given integers are equal. If so, the value of
- * each is printed along with an optional message string. The users don't call
- * this, but instead use the wpi_assertNotEqual macros in Utility.h.
- */
-bool wpi_assertNotEqual_impl(int valueA, int valueB,
-                             const wpi::Twine& valueAString,
-                             const wpi::Twine& valueBString,
-                             const wpi::Twine& message, wpi::StringRef fileName,
-                             int lineNumber, wpi::StringRef funcName);
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Victor.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Victor.h
deleted file mode 100644
index 89c4a89..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Victor.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Vex Robotics Victor 888 Speed Controller.
- *
- * The Vex Robotics Victor 884 Speed Controller can also be used with this
- * class but may need to be calibrated per the Victor 884 user manual.
- *
- * Note that the Victor uses the following bounds for PWM values.  These
- * values were determined empirically and optimized for the Victor 888. These
- * values should work reasonably well for Victor 884 controllers as well but
- * if users experience issues such as asymmetric behavior around the deadband
- * or inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Victor 884 User
- * Manual available from Vex.
- *
- * \li 2.027ms = full "forward"
- * \li 1.525ms = the "high end" of the deadband range
- * \li 1.507ms = center of the deadband range (off)
- * \li 1.490ms = the "low end" of the deadband range
- * \li 1.026ms = full "reverse"
- */
-class Victor : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a Victor.
-   *
-   * @param channel The PWM channel number that the Victor is attached to. 0-9
-   *                are on-board, 10-19 are on the MXP port
-   */
-  explicit Victor(int channel);
-
-  Victor(Victor&&) = default;
-  Victor& operator=(Victor&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/VictorSP.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/VictorSP.h
deleted file mode 100644
index d8aa8e8..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/VictorSP.h
+++ /dev/null
@@ -1,44 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PWMSpeedController.h"
-
-namespace frc {
-
-/**
- * Vex Robotics Victor SP Speed Controller.
- *
- * Note that the Victor SP uses the following bounds for PWM values. These
- * values should work reasonably well for most controllers, but if users
- * experience issues such as asymmetric behavior around the deadband or
- * inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Victor SP User
- * Manual available from Vex.
- *
- * \li 2.004ms = full "forward"
- * \li 1.520ms = the "high end" of the deadband range
- * \li 1.500ms = center of the deadband range (off)
- * \li 1.480ms = the "low end" of the deadband range
- * \li 0.997ms = full "reverse"
- */
-class VictorSP : public PWMSpeedController {
- public:
-  /**
-   * Constructor for a Victor SP.
-   *
-   * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
-   *                on-board, 10-19 are on the MXP port
-   */
-  explicit VictorSP(int channel);
-
-  VictorSP(VictorSP&&) = default;
-  VictorSP& operator=(VictorSP&&) = default;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIErrors.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIErrors.h
deleted file mode 100644
index 8325c92..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIErrors.h
+++ /dev/null
@@ -1,95 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <stdint.h>
-
-#define S(label, offset, message)                                        \
-  constexpr inline const char* wpi_error_s_##label() { return message; } \
-  constexpr inline int wpi_error_value_##label() { return offset; }
-
-// Fatal errors
-S(ModuleIndexOutOfRange, -1,
-  "Allocating module that is out of range or not found")
-S(ChannelIndexOutOfRange, -1, "Allocating channel that is out of range")
-S(NotAllocated, -2, "Attempting to free unallocated resource")
-S(ResourceAlreadyAllocated, -3, "Attempted to reuse an allocated resource")
-S(NoAvailableResources, -4, "No available resources to allocate")
-S(NullParameter, -5, "A pointer parameter to a method is nullptr")
-S(Timeout, -6, "A timeout has been exceeded")
-S(CompassManufacturerError, -7, "Compass manufacturer doesn't match HiTechnic")
-S(CompassTypeError, -8,
-  "Compass type doesn't match expected type for HiTechnic compass")
-S(IncompatibleMode, -9, "The object is in an incompatible mode")
-S(AnalogTriggerLimitOrderError, -10,
-  "AnalogTrigger limits error.  Lower limit > Upper Limit")
-S(AnalogTriggerPulseOutputError, -11,
-  "Attempted to read AnalogTrigger pulse output.")
-S(TaskError, -12, "Task can't be started")
-S(TaskIDError, -13, "Task error: Invalid ID.")
-S(TaskDeletedError, -14, "Task error: Task already deleted.")
-S(TaskOptionsError, -15, "Task error: Invalid options.")
-S(TaskMemoryError, -16, "Task can't be started due to insufficient memory.")
-S(TaskPriorityError, -17, "Task error: Invalid priority [1-255].")
-S(DriveUninitialized, -18, "RobotDrive not initialized for the C interface")
-S(CompressorNonMatching, -19,
-  "Compressor slot/channel doesn't match previous instance")
-S(CompressorAlreadyDefined, -20, "Creating a second compressor instance")
-S(CompressorUndefined, -21,
-  "Using compressor functions without defining compressor")
-S(InconsistentArrayValueAdded, -22,
-  "When packing data into an array to the dashboard, not all values added were "
-  "of the same type.")
-S(MismatchedComplexTypeClose, -23,
-  "When packing data to the dashboard, a Close for a complex type was called "
-  "without a matching Open.")
-S(DashboardDataOverflow, -24,
-  "When packing data to the dashboard, too much data was packed and the buffer "
-  "overflowed.")
-S(DashboardDataCollision, -25,
-  "The same buffer was used for packing data and for printing.")
-S(EnhancedIOMissing, -26, "IO is not attached or Enhanced IO is not enabled.")
-S(LineNotOutput, -27,
-  "Cannot SetDigitalOutput for a line not configured for output.")
-S(ParameterOutOfRange, -28, "A parameter is out of range.")
-S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported")
-S(JaguarVersionError, -30, "Jaguar firmware version error")
-S(JaguarMessageNotFound, -31, "Jaguar message not found")
-S(NetworkTablesReadError, -40, "Error reading NetworkTables socket")
-S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket")
-S(NetworkTablesWrongType, -42,
-  "The wrong type was read from the NetworkTables entry")
-S(NetworkTablesCorrupt, -43, "NetworkTables data stream is corrupt")
-S(SmartDashboardMissingKey, -43, "SmartDashboard data does not exist")
-S(CommandIllegalUse, -50, "Illegal use of Command")
-S(UnsupportedInSimulation, -80, "Unsupported in simulation")
-S(CameraServerError, -90, "CameraServer error")
-S(InvalidParameter, -100, "Invalid parameter value")
-
-// Warnings
-S(SampleRateTooHigh, 1, "Analog module sample rate is too high")
-S(VoltageOutOfRange, 2,
-  "Voltage to convert to raw value is out of range [-10; 10]")
-S(CompressorTaskError, 3, "Compressor task won't start")
-S(LoopTimingError, 4, "Digital module loop timing is not the expected value")
-S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1")
-S(IncorrectBatteryChannel, 6,
-  "Battery measurement channel is not correct value")
-S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3")
-S(BadJoystickAxis, 8, "Joystick axis or POV is out of range")
-S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3")
-S(DriverStationTaskError, 10, "Driver Station task won't start")
-S(EnhancedIOPWMPeriodOutOfRange, 11,
-  "Driver Station Enhanced IO PWM Output period out of range.")
-S(SPIWriteNoMOSI, 12, "Cannot write to SPI port with no MOSI output")
-S(SPIReadNoMISO, 13, "Cannot read from SPI port with no MISO input")
-S(SPIReadNoData, 14, "No data available to read from SPI")
-S(IncompatibleState, 15,
-  "Incompatible State: The operation cannot be completed")
-
-#undef S
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIErrors.mac b/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIErrors.mac
new file mode 100644
index 0000000..c6c96e9
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIErrors.mac
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+S(ModuleIndexOutOfRange, -1,
+  "Allocating module that is out of range or not found")
+S(ChannelIndexOutOfRange, -45, "Allocating channel that is out of range")
+S(NotAllocated, -2, "Attempting to free unallocated resource")
+S(ResourceAlreadyAllocated, -3, "Attempted to reuse an allocated resource")
+S(NoAvailableResources, -4, "No available resources to allocate")
+S(NullParameter, -5, "A pointer parameter to a method is nullptr")
+S(Timeout, -6, "A timeout has been exceeded")
+S(CompassManufacturerError, -7, "Compass manufacturer doesn't match HiTechnic")
+S(CompassTypeError, -8,
+  "Compass type doesn't match expected type for HiTechnic compass")
+S(IncompatibleMode, -9, "The object is in an incompatible mode")
+S(AnalogTriggerLimitOrderError, -10,
+  "AnalogTrigger limits error.  Lower limit > Upper Limit")
+S(AnalogTriggerPulseOutputError, -11,
+  "Attempted to read AnalogTrigger pulse output")
+S(TaskError, -12, "Task can't be started")
+S(TaskIDError, -13, "Task error: Invalid ID")
+S(TaskDeletedError, -14, "Task error: Task already deleted")
+S(TaskOptionsError, -15, "Task error: Invalid options")
+S(TaskMemoryError, -16, "Task can't be started due to insufficient memory")
+S(TaskPriorityError, -17, "Task error: Invalid priority [1-255]")
+S(DriveUninitialized, -18, "RobotDrive not initialized for the C interface")
+S(CompressorNonMatching, -19,
+  "Compressor slot/channel doesn't match previous instance")
+S(CompressorAlreadyDefined, -20, "Creating a second compressor instance")
+S(CompressorUndefined, -21,
+  "Using compressor functions without defining compressor")
+S(InconsistentArrayValueAdded, -22,
+  "When packing data into an array to the dashboard, not all values added were "
+  "of the same type")
+S(MismatchedComplexTypeClose, -23,
+  "When packing data to the dashboard, a Close for a complex type was called "
+  "without a matching Open")
+S(DashboardDataOverflow, -24,
+  "When packing data to the dashboard, too much data was packed and the buffer "
+  "overflowed")
+S(DashboardDataCollision, -25,
+  "The same buffer was used for packing data and for printing")
+S(EnhancedIOMissing, -26, "IO is not attached or Enhanced IO is not enabled")
+S(LineNotOutput, -27,
+  "Cannot SetDigitalOutput for a line not configured for output")
+S(ParameterOutOfRange, -28, "A parameter is out of range")
+S(SPIClockRateTooLow, -29, "SPI clock rate was below the minimum supported")
+S(JaguarVersionError, -30, "Jaguar firmware version error")
+S(JaguarMessageNotFound, -31, "Jaguar message not found")
+S(NetworkTablesReadError, -40, "Error reading NetworkTables socket")
+S(NetworkTablesBufferFull, -41, "Buffer full writing to NetworkTables socket")
+S(NetworkTablesWrongType, -42,
+  "The wrong type was read from the NetworkTables entry")
+S(NetworkTablesCorrupt, -43, "NetworkTables data stream is corrupt")
+S(SmartDashboardMissingKey, -44, "SmartDashboard data does not exist")
+S(CommandIllegalUse, -50, "Illegal use of Command")
+S(UnsupportedInSimulation, -80, "Unsupported in simulation")
+S(CameraServerError, -90, "CameraServer error")
+S(InvalidParameter, -100, "Invalid parameter value")
+S(AssertionFailure, -110, "Assertion failed")
+S(Error, -111, "Error")
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPILib.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPILib.h
deleted file mode 100644
index 634d0f5..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPILib.h
+++ /dev/null
@@ -1,110 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-// 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.")
-#else
-#warning "Including this header drastically increases compilation times and is bad style. Include only what you use instead."
-#endif
-
-// clang-format on
-
-#if __has_include(<cameraserver/CameraServer.h>)
-#include <cameraserver/CameraServer.h>
-#include <vision/VisionRunner.h>
-#endif
-
-#include "frc/ADXL345_I2C.h"
-#include "frc/ADXL345_SPI.h"
-#include "frc/ADXL362.h"
-#include "frc/ADXRS450_Gyro.h"
-#include "frc/AnalogAccelerometer.h"
-#include "frc/AnalogGyro.h"
-#include "frc/AnalogInput.h"
-#include "frc/AnalogOutput.h"
-#include "frc/AnalogPotentiometer.h"
-#include "frc/AnalogTrigger.h"
-#include "frc/AnalogTriggerOutput.h"
-#include "frc/BuiltInAccelerometer.h"
-#include "frc/Compressor.h"
-#include "frc/Counter.h"
-#include "frc/DMC60.h"
-#include "frc/DigitalInput.h"
-#include "frc/DigitalOutput.h"
-#include "frc/DigitalSource.h"
-#include "frc/DoubleSolenoid.h"
-#include "frc/DriverStation.h"
-#include "frc/Encoder.h"
-#include "frc/ErrorBase.h"
-#include "frc/GearTooth.h"
-#include "frc/GenericHID.h"
-#include "frc/I2C.h"
-#include "frc/InterruptableSensorBase.h"
-#include "frc/IterativeRobot.h"
-#include "frc/Jaguar.h"
-#include "frc/Joystick.h"
-#include "frc/NidecBrushless.h"
-#include "frc/Notifier.h"
-#include "frc/PIDController.h"
-#include "frc/PIDOutput.h"
-#include "frc/PIDSource.h"
-#include "frc/PWM.h"
-#include "frc/PWMSpeedController.h"
-#include "frc/PWMTalonSRX.h"
-#include "frc/PWMVictorSPX.h"
-#include "frc/PowerDistributionPanel.h"
-#include "frc/Preferences.h"
-#include "frc/Relay.h"
-#include "frc/RobotBase.h"
-#include "frc/RobotController.h"
-#include "frc/RobotDrive.h"
-#include "frc/SD540.h"
-#include "frc/SPI.h"
-#include "frc/SensorUtil.h"
-#include "frc/SerialPort.h"
-#include "frc/Servo.h"
-#include "frc/Solenoid.h"
-#include "frc/Spark.h"
-#include "frc/SpeedController.h"
-#include "frc/SpeedControllerGroup.h"
-#include "frc/Talon.h"
-#include "frc/Threads.h"
-#include "frc/TimedRobot.h"
-#include "frc/Timer.h"
-#include "frc/Ultrasonic.h"
-#include "frc/Utility.h"
-#include "frc/Victor.h"
-#include "frc/VictorSP.h"
-#include "frc/WPIErrors.h"
-#include "frc/XboxController.h"
-#if __has_include("frc/buttons/InternalButton.h")
-#include "frc/buttons/InternalButton.h"
-#include "frc/buttons/JoystickButton.h"
-#include "frc/buttons/NetworkButton.h"
-#include "frc/commands/Command.h"
-#include "frc/commands/CommandGroup.h"
-#include "frc/commands/PIDCommand.h"
-#include "frc/commands/PIDSubsystem.h"
-#include "frc/commands/PrintCommand.h"
-#include "frc/commands/StartCommand.h"
-#include "frc/commands/Subsystem.h"
-#include "frc/commands/WaitCommand.h"
-#include "frc/commands/WaitForChildren.h"
-#include "frc/commands/WaitUntilCommand.h"
-#endif
-#include "frc/drive/DifferentialDrive.h"
-#include "frc/drive/KilloughDrive.h"
-#include "frc/drive/MecanumDrive.h"
-#include "frc/filters/LinearDigitalFilter.h"
-#include "frc/interfaces/Accelerometer.h"
-#include "frc/interfaces/Gyro.h"
-#include "frc/interfaces/Potentiometer.h"
-#include "frc/smartdashboard/SendableChooser.h"
-#include "frc/smartdashboard/SmartDashboard.h"
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIWarnings.mac b/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIWarnings.mac
new file mode 100644
index 0000000..fa63494
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/WPIWarnings.mac
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+S(SampleRateTooHigh, 1, "Analog module sample rate is too high")
+S(VoltageOutOfRange, 2,
+  "Voltage to convert to raw value is out of range [-10; 10]")
+S(CompressorTaskError, 3, "Compressor task won't start")
+S(LoopTimingError, 4, "Digital module loop timing is not the expected value")
+S(NonBinaryDigitalValue, 5, "Digital output value is not 0 or 1")
+S(IncorrectBatteryChannel, 6,
+  "Battery measurement channel is not correct value")
+S(BadJoystickIndex, 7, "Joystick index is out of range, should be 0-3")
+S(BadJoystickAxis, 8, "Joystick axis or POV is out of range")
+S(InvalidMotorIndex, 9, "Motor index is out of range, should be 0-3")
+S(DriverStationTaskError, 10, "Driver Station task won't start")
+S(EnhancedIOPWMPeriodOutOfRange, 11,
+  "Driver Station Enhanced IO PWM Output period out of range")
+S(SPIWriteNoMOSI, 12, "Cannot write to SPI port with no MOSI output")
+S(SPIReadNoMISO, 13, "Cannot read from SPI port with no MISO input")
+S(SPIReadNoData, 14, "No data available to read from SPI")
+S(IncompatibleState, 15,
+  "Incompatible State: The operation cannot be completed")
+S(Warning, 16, "Warning")
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Watchdog.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Watchdog.h
index 6fc3793..d85674d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/Watchdog.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/Watchdog.h
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
+#include <string_view>
 #include <utility>
 
 #include <units/time.h>
-#include <wpi/StringRef.h>
-#include <wpi/deprecated.h>
 
 #include "frc/Tracer.h"
 
@@ -32,19 +28,6 @@
   /**
    * 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.
-   */
-  WPI_DEPRECATED("Use unit-safe version instead")
-  Watchdog(double timeout, std::function<void()> callback);
-
-  /**
-   * Watchdog constructor.
-   *
    * @param timeout  The watchdog's timeout in seconds with microsecond
    *                 resolution.
    * @param callback This function is called when the timeout expires.
@@ -52,11 +35,6 @@
   Watchdog(units::second_t timeout, std::function<void()> callback);
 
   template <typename Callable, typename Arg, typename... Args>
-  WPI_DEPRECATED("Use unit-safe version instead")
-  Watchdog(double timeout, Callable&& f, Arg&& arg, Args&&... args)
-      : Watchdog(units::second_t{timeout}, arg, args...) {}
-
-  template <typename Callable, typename Arg, typename... Args>
   Watchdog(units::second_t timeout, Callable&& f, Arg&& arg, Args&&... args)
       : Watchdog(timeout,
                  std::bind(std::forward<Callable>(f), std::forward<Arg>(arg),
@@ -68,21 +46,9 @@
   Watchdog& operator=(Watchdog&& rhs);
 
   /**
-   * Returns the time in seconds since the watchdog was last fed.
+   * Returns the time since the watchdog was last fed.
    */
-  double GetTime() const;
-
-  /**
-   * 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.
-   */
-  WPI_DEPRECATED("Use unit-safe version instead")
-  void SetTimeout(double timeout);
+  units::second_t GetTime() const;
 
   /**
    * Sets the watchdog's timeout.
@@ -93,9 +59,9 @@
   void SetTimeout(units::second_t timeout);
 
   /**
-   * Returns the watchdog's timeout in seconds.
+   * Returns the watchdog's timeout.
    */
-  double GetTimeout() const;
+  units::second_t GetTimeout() const;
 
   /**
    * Returns true if the watchdog timer has expired.
@@ -110,7 +76,7 @@
    *
    * @param epochName The name to associate with the epoch.
    */
-  void AddEpoch(wpi::StringRef epochName);
+  void AddEpoch(std::string_view epochName);
 
   /**
    * Prints list of epochs added so far and their times.
@@ -146,7 +112,7 @@
 
  private:
   // Used for timeout print rate-limiting
-  static constexpr units::second_t kMinPrintPeriod = 1_s;
+  static constexpr auto kMinPrintPeriod = 1_s;
 
   units::second_t m_startTime = 0_s;
   units::second_t m_timeout;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h
index ddf4dc2..cbdc7d7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/XboxController.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -32,78 +29,102 @@
    */
   explicit XboxController(int port);
 
-  virtual ~XboxController() = default;
+  ~XboxController() override = default;
 
   XboxController(XboxController&&) = default;
   XboxController& operator=(XboxController&&) = default;
 
   /**
-   * Get the X axis value of the controller.
-   *
-   * @param hand Side of controller whose value should be returned.
+   * Get the X axis value of left side of the controller.
    */
-  double GetX(JoystickHand hand) const override;
+  double GetLeftX() const;
 
   /**
-   * Get the Y axis value of the controller.
-   *
-   * @param hand Side of controller whose value should be returned.
+   * Get the X axis value of right side of the controller.
    */
-  double GetY(JoystickHand hand) const override;
+  double GetRightX() const;
 
   /**
-   * Get the trigger axis value of the controller.
-   *
-   * @param hand Side of controller whose value should be returned.
+   * Get the Y axis value of left side of the controller.
    */
-  double GetTriggerAxis(JoystickHand hand) const;
+  double GetLeftY() const;
 
   /**
-   * Read the value of the bumper button on the controller.
-   *
-   * @param hand Side of controller whose value should be returned.
+   * Get the Y axis value of right side of the controller.
    */
-  bool GetBumper(JoystickHand hand) const;
+  double GetRightY() const;
 
   /**
-   * Whether the bumper was pressed since the last check.
-   *
-   * @param hand Side of controller whose value should be returned.
-   * @return Whether the button was pressed since the last check.
+   * Get the left trigger (LT) axis value of the controller. Note that this axis
+   * is bound to the range of [0, 1] as opposed to the usual [-1, 1].
    */
-  bool GetBumperPressed(JoystickHand hand);
+  double GetLeftTriggerAxis() const;
 
   /**
-   * Whether the bumper was released since the last check.
-   *
-   * @param hand Side of controller whose value should be returned.
-   * @return Whether the button was released since the last check.
+   * Get the right trigger (RT) axis value of the controller. Note that this
+   * axis is bound to the range of [0, 1] as opposed to the usual [-1, 1].
    */
-  bool GetBumperReleased(JoystickHand hand);
+  double GetRightTriggerAxis() const;
 
   /**
-   * Read the value of the stick button on the controller.
-   *
-   * @param hand Side of controller whose value should be returned.
-   * @return The state of the button.
+   * Read the value of the left bumper (LB) button on the controller.
    */
-  bool GetStickButton(JoystickHand hand) const;
+  bool GetLeftBumper() const;
 
   /**
-   * Whether the stick button was pressed since the last check.
-   *
-   * @param hand Side of controller whose value should be returned.
-   * @return Whether the button was pressed since the last check.
+   * Read the value of the right bumper (RB) button on the controller.
    */
-  bool GetStickButtonPressed(JoystickHand hand);
+  bool GetRightBumper() const;
 
   /**
-   * Whether the stick button was released since the last check.
-   *
-   * @param hand Side of controller whose value should be returned.
-   * @return Whether the button was released since the last check.
+   * Whether the left bumper (LB) was pressed since the last check.
    */
-  bool GetStickButtonReleased(JoystickHand hand);
+  bool GetLeftBumperPressed();
+
+  /**
+   * Whether the right bumper (RB) was pressed since the last check.
+   */
+  bool GetRightBumperPressed();
+
+  /**
+   * Whether the left bumper (LB) was released since the last check.
+   */
+  bool GetLeftBumperReleased();
+
+  /**
+   * Whether the right bumper (RB) was released since the last check.
+   */
+  bool GetRightBumperReleased();
+
+  /**
+   * Read the value of the left stick button (LSB) on the controller.
+   */
+  bool GetLeftStickButton() const;
+
+  /**
+   * Read the value of the right stick button (RSB) on the controller.
+   */
+  bool GetRightStickButton() const;
+
+  /**
+   * Whether the left stick button (LSB) was pressed since the last check.
+   */
+  bool GetLeftStickButtonPressed();
+
+  /**
+   * Whether the right stick button (RSB) was pressed since the last check.
+   */
+  bool GetRightStickButtonPressed();
+
+  /**
+   * Whether the left stick button (LSB) was released since the last check.
+   */
+  bool GetLeftStickButtonReleased();
+
+  /**
+   * Whether the right stick button (RSB) was released since the last check.
+   */
+  bool GetRightStickButtonReleased();
 
   /**
    * Read the value of the A button on the controller.
@@ -213,7 +234,6 @@
   /**
    * Read the value of the start button on the controller.
    *
-   * @param hand Side of controller whose value should be returned.
    * @return The state of the button.
    */
   bool GetStartButton() const;
@@ -232,26 +252,26 @@
    */
   bool GetStartButtonReleased();
 
-  enum class Button {
-    kBumperLeft = 5,
-    kBumperRight = 6,
-    kStickLeft = 9,
-    kStickRight = 10,
-    kA = 1,
-    kB = 2,
-    kX = 3,
-    kY = 4,
-    kBack = 7,
-    kStart = 8
+  struct Button {
+    static constexpr int kLeftBumper = 5;
+    static constexpr int kRightBumper = 6;
+    static constexpr int kLeftStick = 9;
+    static constexpr int kRightStick = 10;
+    static constexpr int kA = 1;
+    static constexpr int kB = 2;
+    static constexpr int kX = 3;
+    static constexpr int kY = 4;
+    static constexpr int kBack = 7;
+    static constexpr int kStart = 8;
   };
 
-  enum class Axis {
-    kLeftX = 0,
-    kRightX = 4,
-    kLeftY = 1,
-    kRightY = 5,
-    kLeftTrigger = 2,
-    kRightTrigger = 3
+  struct Axis {
+    static constexpr int kLeftX = 0;
+    static constexpr int kRightX = 4;
+    static constexpr int kLeftY = 1;
+    static constexpr int kRightY = 5;
+    static constexpr int kLeftTrigger = 2;
+    static constexpr int kRightTrigger = 3;
   };
 };
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
deleted file mode 100644
index 7603fde..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/ControllerUtil.h
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
deleted file mode 100644
index 916e1a9..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/HolonomicDriveController.h
+++ /dev/null
@@ -1,112 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/PIDController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/PIDController.h
deleted file mode 100644
index 5f97c1e..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/PIDController.h
+++ /dev/null
@@ -1,243 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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 <limits>
-
-#include <units/time.h>
-
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc2 {
-
-/**
- * Implements a PID control loop.
- */
-class PIDController : public frc::Sendable,
-                      public frc::SendableHelper<PIDController> {
- public:
-  /**
-   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
-   *
-   * @param Kp     The proportional coefficient.
-   * @param Ki     The integral coefficient.
-   * @param Kd     The derivative coefficient.
-   * @param period The period between controller updates in seconds. The
-   *               default is 20 milliseconds.
-   */
-  PIDController(double Kp, double Ki, double Kd,
-                units::second_t period = 20_ms);
-
-  ~PIDController() override = default;
-
-  PIDController(const PIDController&) = default;
-  PIDController& operator=(const PIDController&) = default;
-  PIDController(PIDController&&) = default;
-  PIDController& operator=(PIDController&&) = default;
-
-  /**
-   * Sets the PID Controller gain parameters.
-   *
-   * Sets the proportional, integral, and differential coefficients.
-   *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
-   */
-  void SetPID(double Kp, double Ki, double Kd);
-
-  /**
-   * Sets the proportional coefficient of the PID controller gain.
-   *
-   * @param Kp proportional coefficient
-   */
-  void SetP(double Kp);
-
-  /**
-   * Sets the integral coefficient of the PID controller gain.
-   *
-   * @param Ki integral coefficient
-   */
-  void SetI(double Ki);
-
-  /**
-   * Sets the differential coefficient of the PID controller gain.
-   *
-   * @param Kd differential coefficient
-   */
-  void SetD(double Kd);
-
-  /**
-   * Gets the proportional coefficient.
-   *
-   * @return proportional coefficient
-   */
-  double GetP() const;
-
-  /**
-   * Gets the integral coefficient.
-   *
-   * @return integral coefficient
-   */
-  double GetI() const;
-
-  /**
-   * Gets the differential coefficient.
-   *
-   * @return differential coefficient
-   */
-  double GetD() const;
-
-  /**
-   * Gets the period of this controller.
-   *
-   * @return The period of the controller.
-   */
-  units::second_t GetPeriod() const;
-
-  /**
-   * Sets the setpoint for the PIDController.
-   *
-   * @param setpoint The desired setpoint.
-   */
-  void SetSetpoint(double setpoint);
-
-  /**
-   * Returns the current setpoint of the PIDController.
-   *
-   * @return The current setpoint.
-   */
-  double GetSetpoint() const;
-
-  /**
-   * 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.
-   */
-  bool AtSetpoint() const;
-
-  /**
-   * Enables continuous input.
-   *
-   * Rather then using the max and min input range as constraints, it considers
-   * them to be the same point and automatically calculates the shortest route
-   * to the setpoint.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  void EnableContinuousInput(double minimumInput, double maximumInput);
-
-  /**
-   * Disables continuous input.
-   */
-  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
-   * output rather than the integrator value times the integral gain.
-   *
-   * @param minimumIntegral The minimum value of the integrator.
-   * @param maximumIntegral The maximum value of the integrator.
-   */
-  void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
-
-  /**
-   * Sets the error which is considered tolerable for use with AtSetpoint().
-   *
-   * @param positionTolerance Position error which is tolerable.
-   * @param velociytTolerance Velocity error which is tolerable.
-   */
-  void SetTolerance(
-      double positionTolerance,
-      double velocityTolerance = std::numeric_limits<double>::infinity());
-
-  /**
-   * Returns the difference between the setpoint and the measurement.
-   */
-  double GetPositionError() const;
-
-  /**
-   * Returns the velocity error.
-   */
-  double GetVelocityError() const;
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   */
-  double Calculate(double measurement);
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param setpoint The new setpoint of the controller.
-   */
-  double Calculate(double measurement, double setpoint);
-
-  /**
-   * Reset the previous error, the integral term, and disable the controller.
-   */
-  void Reset();
-
-  void InitSendable(frc::SendableBuilder& builder) override;
-
- private:
-  // Factor for "proportional" control
-  double m_Kp;
-
-  // Factor for "integral" control
-  double m_Ki;
-
-  // Factor for "derivative" control
-  double m_Kd;
-
-  // The period (in seconds) of the control loop running this controller
-  units::second_t m_period;
-
-  double m_maximumIntegral = 1.0;
-
-  double m_minimumIntegral = -1.0;
-
-  double m_maximumInput = 0;
-
-  double m_minimumInput = 0;
-
-  // Do the endpoints wrap around? eg. Absolute encoder
-  bool m_continuous = false;
-
-  // The error at the time of the most recent call to Calculate()
-  double m_positionError = 0;
-  double m_velocityError = 0;
-
-  // The error at the time of the second-most-recent call to Calculate() (used
-  // to compute velocity)
-  double m_prevError = 0;
-
-  // The sum of the errors for use in the integral calc
-  double m_totalError = 0;
-
-  // The error that is considered at setpoint.
-  double m_positionTolerance = 0.05;
-  double m_velocityTolerance = std::numeric_limits<double>::infinity();
-
-  double m_setpoint = 0;
-};
-
-}  // namespace frc2
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
deleted file mode 100644
index fd246cd..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/ProfiledPIDController.h
+++ /dev/null
@@ -1,367 +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 <algorithm>
-#include <cmath>
-#include <functional>
-#include <limits>
-
-#include <units/time.h>
-
-#include "frc/controller/ControllerUtil.h"
-#include "frc/controller/PIDController.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableHelper.h"
-#include "frc/trajectory/TrapezoidProfile.h"
-
-namespace frc {
-namespace detail {
-void ReportProfiledPIDController();
-}  // namespace detail
-
-/**
- * Implements a PID control loop whose setpoint is constrained by a trapezoid
- * profile.
- */
-template <class Distance>
-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>>;
-  using Velocity_t = units::unit_t<Velocity>;
-  using Acceleration =
-      units::compound_unit<Velocity, units::inverse<units::seconds>>;
-  using Acceleration_t = units::unit_t<Acceleration>;
-  using State = typename TrapezoidProfile<Distance>::State;
-  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
-
-  /**
-   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
-   * Kd. Users should call reset() when they first start running the controller
-   * to avoid unwanted behavior.
-   *
-   * @param Kp          The proportional coefficient.
-   * @param Ki          The integral coefficient.
-   * @param Kd          The derivative coefficient.
-   * @param constraints Velocity and acceleration constraints for goal.
-   * @param period      The period between controller updates in seconds. The
-   *                    default is 20 milliseconds.
-   */
-  ProfiledPIDController(double Kp, double Ki, double Kd,
-                        Constraints constraints, units::second_t period = 20_ms)
-      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
-    detail::ReportProfiledPIDController();
-  }
-
-  ~ProfiledPIDController() override = default;
-
-  ProfiledPIDController(const ProfiledPIDController&) = default;
-  ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
-  ProfiledPIDController(ProfiledPIDController&&) = default;
-  ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
-
-  /**
-   * Sets the PID Controller gain parameters.
-   *
-   * Sets the proportional, integral, and differential coefficients.
-   *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
-   */
-  void SetPID(double Kp, double Ki, double Kd) {
-    m_controller.SetPID(Kp, Ki, Kd);
-  }
-
-  /**
-   * Sets the proportional coefficient of the PID controller gain.
-   *
-   * @param Kp proportional coefficient
-   */
-  void SetP(double Kp) { m_controller.SetP(Kp); }
-
-  /**
-   * Sets the integral coefficient of the PID controller gain.
-   *
-   * @param Ki integral coefficient
-   */
-  void SetI(double Ki) { m_controller.SetI(Ki); }
-
-  /**
-   * Sets the differential coefficient of the PID controller gain.
-   *
-   * @param Kd differential coefficient
-   */
-  void SetD(double Kd) { m_controller.SetD(Kd); }
-
-  /**
-   * Gets the proportional coefficient.
-   *
-   * @return proportional coefficient
-   */
-  double GetP() const { return m_controller.GetP(); }
-
-  /**
-   * Gets the integral coefficient.
-   *
-   * @return integral coefficient
-   */
-  double GetI() const { return m_controller.GetI(); }
-
-  /**
-   * Gets the differential coefficient.
-   *
-   * @return differential coefficient
-   */
-  double GetD() const { return m_controller.GetD(); }
-
-  /**
-   * Gets the period of this controller.
-   *
-   * @return The period of the controller.
-   */
-  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
-
-  /**
-   * Sets the goal for the ProfiledPIDController.
-   *
-   * @param goal The desired unprofiled setpoint.
-   */
-  void SetGoal(State goal) { m_goal = goal; }
-
-  /**
-   * Sets the goal for the ProfiledPIDController.
-   *
-   * @param goal The desired unprofiled setpoint.
-   */
-  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
-
-  /**
-   * Gets the goal for the ProfiledPIDController.
-   */
-  State GetGoal() const { return m_goal; }
-
-  /**
-   * Returns true if the error is within the tolerance of the error.
-   *
-   * This will return false until at least one input value has been computed.
-   */
-  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
-
-  /**
-   * Set velocity and acceleration constraints for goal.
-   *
-   * @param constraints Velocity and acceleration constraints for goal.
-   */
-  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
-
-  /**
-   * Returns the current setpoint of the ProfiledPIDController.
-   *
-   * @return The current setpoint.
-   */
-  State GetSetpoint() const { return m_setpoint; }
-
-  /**
-   * Returns true if the error is within the tolerance of the error.
-   *
-   * Currently this just reports on target as the actual value passes through
-   * the setpoint. Ideally it should be based on being within the tolerance for
-   * some period of time.
-   *
-   * This will return false until at least one input value has been computed.
-   */
-  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
-
-  /**
-   * Enables continuous input.
-   *
-   * Rather then using the max and min input range as constraints, it considers
-   * them to be the same point and automatically calculates the shortest route
-   * to the setpoint.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  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;
-  }
-
-  /**
-   * Disables continuous input.
-   */
-  void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
-
-  /**
-   * Sets the minimum and maximum values for the integrator.
-   *
-   * When the cap is reached, the integrator value is added to the controller
-   * output rather than the integrator value times the integral gain.
-   *
-   * @param minimumIntegral The minimum value of the integrator.
-   * @param maximumIntegral The maximum value of the integrator.
-   */
-  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
-    m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
-  }
-
-  /**
-   * Sets the error which is considered tolerable for use with
-   * AtSetpoint().
-   *
-   * @param positionTolerance Position error which is tolerable.
-   * @param velocityTolerance Velocity error which is tolerable.
-   */
-  void SetTolerance(
-      Distance_t positionTolerance,
-      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
-    m_controller.SetTolerance(positionTolerance.template to<double>(),
-                              velocityTolerance.template to<double>());
-  }
-
-  /**
-   * Returns the difference between the setpoint and the measurement.
-   *
-   * @return The error.
-   */
-  Distance_t GetPositionError() const {
-    return Distance_t(m_controller.GetPositionError());
-  }
-
-  /**
-   * Returns the change in error per second.
-   */
-  Velocity_t GetVelocityError() const {
-    return Velocity_t(m_controller.GetVelocityError());
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   */
-  double Calculate(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>(),
-                                  m_setpoint.position.template to<double>());
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param goal The new goal of the controller.
-   */
-  double Calculate(Distance_t measurement, State goal) {
-    SetGoal(goal);
-    return Calculate(measurement);
-  }
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param goal The new goal of the controller.
-   */
-  double Calculate(Distance_t measurement, Distance_t goal) {
-    SetGoal(goal);
-    return Calculate(measurement);
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param goal        The new goal of the controller.
-   * @param constraints Velocity and acceleration constraints for goal.
-   */
-  double Calculate(
-      Distance_t measurement, Distance_t goal,
-      typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
-    SetConstraints(constraints);
-    return Calculate(measurement, goal);
-  }
-
-  /**
-   * Reset the previous error and the integral term.
-   *
-   * @param measurement The current measured State of the system.
-   */
-  void Reset(const State& measurement) {
-    m_controller.Reset();
-    m_setpoint = measurement;
-  }
-
-  /**
-   * Reset the previous error and the integral term.
-   *
-   * @param measuredPosition The current measured position of the system.
-   * @param measuredVelocity The current measured velocity of the system.
-   */
-  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
-    Reset(State{measuredPosition, measuredVelocity});
-  }
-
-  /**
-   * Reset the previous error and the integral term.
-   *
-   * @param measuredPosition The current measured position of the system. The
-   * velocity is assumed to be zero.
-   */
-  void Reset(Distance_t measuredPosition) {
-    Reset(measuredPosition, Velocity_t(0));
-  }
-
-  void InitSendable(frc::SendableBuilder& builder) override {
-    builder.SetSmartDashboardType("ProfiledPIDController");
-    builder.AddDoubleProperty(
-        "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
-    builder.AddDoubleProperty(
-        "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
-    builder.AddDoubleProperty(
-        "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
-    builder.AddDoubleProperty(
-        "goal", [this] { return GetGoal().position.template to<double>(); },
-        [this](double value) { SetGoal(Distance_t{value}); });
-  }
-
- private:
-  frc2::PIDController m_controller;
-  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;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/RamseteController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/RamseteController.h
deleted file mode 100644
index e746bb3..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/controller/RamseteController.h
+++ /dev/null
@@ -1,122 +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 <units/angular_velocity.h>
-#include <units/velocity.h>
-
-#include "frc/geometry/Pose2d.h"
-#include "frc/kinematics/ChassisSpeeds.h"
-#include "frc/trajectory/Trajectory.h"
-
-namespace frc {
-
-/**
- * Ramsete is a nonlinear time-varying feedback controller for unicycle models
- * that drives the model to a desired pose along a two-dimensional trajectory.
- * Why would we need a nonlinear control law in addition to the linear ones we
- * have used so far like PID? If we use the original approach with PID
- * controllers for left and right position and velocity states, the controllers
- * only deal with the local pose. If the robot deviates from the path, there is
- * no way for the controllers to correct and the robot may not reach the desired
- * global pose. This is due to multiple endpoints existing for the robot which
- * have the same encoder path arc lengths.
- *
- * Instead of using wheel path arc lengths (which are in the robot's local
- * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
- * global pose. The controller uses this extra information to guide a linear
- * reference tracker like the PID controllers back in by adjusting the
- * references of the PID controllers.
- *
- * The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
- * describes a nonlinear controller for a wheeled vehicle with unicycle-like
- * kinematics; a global pose consisting of x, y, and theta; and a desired pose
- * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
- * acronym for the title of the book it came from in Italian ("Robotica
- * Articolata e Mobile per i SErvizi e le TEcnologie").
- *
- * See <https://file.tavsys.net/control/controls-engineering-in-frc.pdf> section
- * on Ramsete unicycle controller for a derivation and analysis.
- */
-class RamseteController {
- public:
-  /**
-   * Construct a Ramsete unicycle controller.
-   *
-   * @param b    Tuning parameter (b > 0) for which larger values make
-   *             convergence more aggressive like a proportional term.
-   * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide
-   *             more damping in response.
-   */
-  RamseteController(double b, double zeta);
-
-  /**
-   * Construct a Ramsete unicycle controller. The default arguments for
-   * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
-   * results.
-   */
-  RamseteController() : RamseteController(2.0, 0.7) {}
-
-  /**
-   * 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& poseTolerance);
-
-  /**
-   * Returns the next output of the Ramsete 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 angularVelocityRef The desired angular velocity.
-   */
-  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
-                          units::meters_per_second_t linearVelocityRef,
-                          units::radians_per_second_t angularVelocityRef);
-
-  /**
-   * Returns the next output of the Ramsete 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.
-   */
-  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/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
index 86103de..50a7a3f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/DifferentialDrive.h
@@ -1,20 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/raw_ostream.h>
+#include <string>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/drive/RobotDriveBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 class SpeedController;
 
 /**
@@ -22,9 +31,9 @@
  * the Kit of Parts drive base, "tank drive", or West Coast Drive.
  *
  * These drive bases typically have drop-center / skid-steer with two or more
- * wheels per side (e.g., 6WD or 8WD). This class takes a SpeedController per
+ * wheels per side (e.g., 6WD or 8WD). This class takes a MotorController per
  * side. For four and six motor drivetrains, construct and pass in
- * SpeedControllerGroup instances as follows.
+ * MotorControllerGroup instances as follows.
  *
  * Four motor drivetrain:
  * @code{.cpp}
@@ -32,11 +41,11 @@
  *  public:
  *   frc::PWMVictorSPX m_frontLeft{1};
  *   frc::PWMVictorSPX m_rearLeft{2};
- *   frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
+ *   frc::MotorControllerGroup m_left{m_frontLeft, m_rearLeft};
  *
  *   frc::PWMVictorSPX m_frontRight{3};
  *   frc::PWMVictorSPX m_rearRight{4};
- *   frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
+ *   frc::MotorControllerGroup m_right{m_frontRight, m_rearRight};
  *
  *   frc::DifferentialDrive m_drive{m_left, m_right};
  * };
@@ -49,12 +58,12 @@
  *   frc::PWMVictorSPX m_frontLeft{1};
  *   frc::PWMVictorSPX m_midLeft{2};
  *   frc::PWMVictorSPX m_rearLeft{3};
- *   frc::SpeedControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
+ *   frc::MotorControllerGroup m_left{m_frontLeft, m_midLeft, m_rearLeft};
  *
  *   frc::PWMVictorSPX m_frontRight{4};
  *   frc::PWMVictorSPX m_midRight{5};
  *   frc::PWMVictorSPX m_rearRight{6};
- *   frc::SpeedControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
+ *   frc::MotorControllerGroup m_right{m_frontRight, m_midRight, m_rearRight};
  *
  *   frc::DifferentialDrive m_drive{m_left, m_right};
  * };
@@ -87,28 +96,20 @@
  * Inputs smaller then 0.02 will be set to 0, and larger values will be scaled
  * so that the full range is still used. This deadband value can be changed
  * with SetDeadband().
- *
- * <p>RobotDrive porting guide:
- * <br>TankDrive(double, double, bool) is equivalent to
- * RobotDrive#TankDrive(double, double, bool) if a deadband of 0 is used.
- * <br>ArcadeDrive(double, double, bool) is equivalent to
- * RobotDrive#ArcadeDrive(double, double, bool) if a deadband of 0 is used
- * and the the rotation input is inverted eg ArcadeDrive(y, -rotation, false)
- * <br>CurvatureDrive(double, double, bool) is similar in concept to
- * RobotDrive#Drive(double, double) with the addition of a quick turn
- * mode. However, it is not designed to give exactly the same response.
  */
 class DifferentialDrive : public RobotDriveBase,
-                          public Sendable,
-                          public SendableHelper<DifferentialDrive> {
+                          public wpi::Sendable,
+                          public wpi::SendableHelper<DifferentialDrive> {
  public:
-  static constexpr double kDefaultQuickStopThreshold = 0.2;
-  static constexpr double kDefaultQuickStopAlpha = 0.1;
+  struct WheelSpeeds {
+    double left = 0.0;
+    double right = 0.0;
+  };
 
   /**
    * Construct a DifferentialDrive.
    *
-   * To pass multiple motors per side, use a SpeedControllerGroup. If a motor
+   * To pass multiple motors per side, use a MotorControllerGroup. If a motor
    * needs to be inverted, do so before passing it in.
    */
   DifferentialDrive(SpeedController& leftMotor, SpeedController& rightMotor);
@@ -137,17 +138,16 @@
    *
    * The rotation argument controls the curvature of the robot's path rather
    * than its rate of heading change. This makes the robot more controllable at
-   * high speeds. Also handles the robot's quick turn functionality - "quick
-   * turn" overrides constant-curvature turning for turn-in-place maneuvers.
+   * high speeds.
    *
-   * @param xSpeed      The robot's speed along the X axis [-1.0..1.0]. Forward
-   *                    is positive.
-   * @param zRotation   The robot's rotation rate around the Z axis [-1.0..1.0].
-   *                    Clockwise is positive.
-   * @param isQuickTurn If set, overrides constant-curvature turning for
-   *                    turn-in-place maneuvers.
+   * @param xSpeed           The robot's speed along the X axis [-1.0..1.0].
+   *                         Forward is positive.
+   * @param zRotation        The robot's rotation rate around the Z axis
+   *                         [-1.0..1.0]. Clockwise is positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for
+   *                         turn-in-place maneuvers.
    */
-  void CurvatureDrive(double xSpeed, double zRotation, bool isQuickTurn);
+  void CurvatureDrive(double xSpeed, double zRotation, bool allowTurnInPlace);
 
   /**
    * Tank drive method for differential drive platform.
@@ -161,65 +161,65 @@
   void TankDrive(double leftSpeed, double rightSpeed, bool squareInputs = true);
 
   /**
-   * Sets the QuickStop speed threshold in curvature drive.
+   * Arcade drive inverse kinematics for differential drive platform.
    *
-   * QuickStop compensates for the robot's moment of inertia when stopping after
-   * a QuickTurn.
+   * Note: Some drivers may prefer inverted rotation controls. This can be done
+   * by negating the value passed for rotation.
    *
-   * While QuickTurn is enabled, the QuickStop accumulator takes on the rotation
-   * rate value outputted by the low-pass filter when the robot's speed along
-   * the X axis is below the threshold. When QuickTurn is disabled, the
-   * accumulator's value is applied against the computed angular power request
-   * to slow the robot's rotation.
-   *
-   * @param threshold X speed below which quick stop accumulator will receive
-   *                  rotation rate values [0..1.0].
+   * @param xSpeed       The speed at which the robot should drive along the X
+   *                     axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation    The rotation rate of the robot around the Z axis
+   *                     [-1.0..1.0]. Clockwise is positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
    */
-  void SetQuickStopThreshold(double threshold);
+  static WheelSpeeds ArcadeDriveIK(double xSpeed, double zRotation,
+                                   bool squareInputs = true);
 
   /**
-   * Sets the low-pass filter gain for QuickStop in curvature drive.
+   * Curvature drive inverse kinematics for differential drive platform.
    *
-   * The low-pass filter filters incoming rotation rate commands to smooth out
-   * high frequency changes.
+   * The rotation argument controls the curvature of the robot's path rather
+   * than its rate of heading change. This makes the robot more controllable at
+   * high speeds.
    *
-   * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in
-   *              slower output changes. Values between 1.0 and 2.0 result in
-   *              output oscillation. Values below 0.0 and above 2.0 are
-   *              unstable.
+   * @param xSpeed           The robot's speed along the X axis [-1.0..1.0].
+   *                         Forward is positive.
+   * @param zRotation        The robot's rotation rate around the Z axis
+   *                         [-1.0..1.0]. Clockwise is positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for
+   *                         turn-in-place maneuvers.
    */
-  void SetQuickStopAlpha(double alpha);
+  static WheelSpeeds CurvatureDriveIK(double xSpeed, double zRotation,
+                                      bool allowTurnInPlace);
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multiplied by
-   * -1.
+   * Tank drive inverse kinematics for differential drive platform.
    *
-   * @return true if the right side is inverted
+   * @param leftSpeed    The robot left side's speed along the X axis
+   *                     [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed   The robot right side's speed along the X axis
+   *                     [-1.0..1.0]. Forward is positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
    */
-  bool IsRightSideInverted() const;
-
-  /**
-   * Sets if the power sent to the right side of the drivetrain should be
-   * multiplied by -1.
-   *
-   * @param rightSideInverted true if right side power should be multiplied by
-   * -1
-   */
-  void SetRightSideInverted(bool rightSideInverted);
+  static WheelSpeeds TankDriveIK(double leftSpeed, double rightSpeed,
+                                 bool squareInputs = true);
 
   void StopMotor() override;
-  void GetDescription(wpi::raw_ostream& desc) const override;
+  std::string GetDescription() const override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   SpeedController* m_leftMotor;
   SpeedController* m_rightMotor;
-
-  double m_quickStopThreshold = kDefaultQuickStopThreshold;
-  double m_quickStopAlpha = kDefaultQuickStopAlpha;
-  double m_quickStopAccumulator = 0.0;
-  double m_rightSideInvertMultiplier = -1.0;
 };
 
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#elif defined(__clang__)
+#pragma clang diagnostic pop
+#elif defined(__GNUC__)
+#pragma GCC diagnostic pop
+#endif
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
index 2cc1c91..1acc69f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/KilloughDrive.h
@@ -1,23 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string>
 
-#include <wpi/raw_ostream.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/drive/RobotDriveBase.h"
 #include "frc/drive/Vector2d.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 class SpeedController;
 
 /**
@@ -47,13 +55,19 @@
  * clockwise rotation around the Z axis is positive.
  */
 class KilloughDrive : public RobotDriveBase,
-                      public Sendable,
-                      public SendableHelper<KilloughDrive> {
+                      public wpi::Sendable,
+                      public wpi::SendableHelper<KilloughDrive> {
  public:
   static constexpr double kDefaultLeftMotorAngle = 60.0;
   static constexpr double kDefaultRightMotorAngle = 120.0;
   static constexpr double kDefaultBackMotorAngle = 270.0;
 
+  struct WheelSpeeds {
+    double left = 0.0;
+    double right = 0.0;
+    double back = 0.0;
+  };
+
   /**
    * Construct a Killough drive with the given motors and default motor angles.
    *
@@ -126,10 +140,28 @@
    */
   void DrivePolar(double magnitude, double angle, double zRotation);
 
-  void StopMotor() override;
-  void GetDescription(wpi::raw_ostream& desc) const override;
+  /**
+   * Cartesian inverse kinematics for Killough platform.
+   *
+   * Angles are measured clockwise from the positive X axis. The robot's speed
+   * is independent from its angle or rotation rate.
+   *
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is
+   *                  positive.
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   *                  positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
+   *                  Clockwise is positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around
+   *                  the Z axis. Use this to implement field-oriented controls.
+   */
+  WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed, double zRotation,
+                               double gyroAngle = 0.0);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   SpeedController* m_leftMotor;
@@ -143,4 +175,12 @@
   bool reported = false;
 };
 
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#elif defined(__clang__)
+#pragma clang diagnostic pop
+#elif defined(__GNUC__)
+#pragma GCC diagnostic pop
+#endif
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
index 8435b4d..809e7ea 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/MecanumDrive.h
@@ -1,22 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string>
 
-#include <wpi/raw_ostream.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 #include "frc/drive/RobotDriveBase.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
 
 namespace frc {
 
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 class SpeedController;
 
 /**
@@ -52,22 +60,29 @@
  * with SetDeadband().
  *
  * RobotDrive porting guide:
- * <br>In MecanumDrive, the right side speed controllers are automatically
- * inverted, while in RobotDrive, no speed controllers are automatically
+ * <br>In MecanumDrive, the right side motor controllers are automatically
+ * inverted, while in RobotDrive, no motor controllers are automatically
  * inverted.
  * <br>DriveCartesian(double, double, double, double) is equivalent to
- * RobotDrive#MecanumDrive_Cartesian(double, double, double, double)
+ * RobotDrive's MecanumDrive_Cartesian(double, double, double, double)
  * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted
  * compared to RobotDrive (eg DriveCartesian(xSpeed, -ySpeed, zRotation,
  * -gyroAngle).
  * <br>DrivePolar(double, double, double) is equivalent to
- * RobotDrive#MecanumDrive_Polar(double, double, double) if a
+ * RobotDrive's MecanumDrive_Polar(double, double, double) if a
  * deadband of 0 is used.
  */
 class MecanumDrive : public RobotDriveBase,
-                     public Sendable,
-                     public SendableHelper<MecanumDrive> {
+                     public wpi::Sendable,
+                     public wpi::SendableHelper<MecanumDrive> {
  public:
+  struct WheelSpeeds {
+    double frontLeft = 0.0;
+    double frontRight = 0.0;
+    double rearLeft = 0.0;
+    double rearRight = 0.0;
+  };
+
   /**
    * Construct a MecanumDrive.
    *
@@ -116,26 +131,27 @@
   void DrivePolar(double magnitude, double angle, double zRotation);
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multiplied by
-   * -1.
+   * Cartesian inverse kinematics for Mecanum platform.
    *
-   * @return true if the right side is inverted
-   */
-  bool IsRightSideInverted() const;
-
-  /**
-   * Sets if the power sent to the right side of the drivetrain should be
-   * multiplied by -1.
+   * Angles are measured clockwise from the positive X axis. The robot's speed
+   * is independent from its angle or rotation rate.
    *
-   * @param rightSideInverted true if right side power should be multiplied by
-   * -1
+   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is
+   *                  positive.
+   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is
+   *                  positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0].
+   *                  Clockwise is positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around
+   *                  the Z axis. Use this to implement field-oriented controls.
    */
-  void SetRightSideInverted(bool rightSideInverted);
+  static WheelSpeeds DriveCartesianIK(double ySpeed, double xSpeed,
+                                      double zRotation, double gyroAngle = 0.0);
 
   void StopMotor() override;
-  void GetDescription(wpi::raw_ostream& desc) const override;
+  std::string GetDescription() const override;
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   SpeedController* m_frontLeftMotor;
@@ -143,9 +159,15 @@
   SpeedController* m_frontRightMotor;
   SpeedController* m_rearRightMotor;
 
-  double m_rightSideInvertMultiplier = -1.0;
-
   bool reported = false;
 };
 
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#elif defined(__clang__)
+#pragma clang diagnostic pop
+#elif defined(__GNUC__)
+#pragma GCC diagnostic pop
+#endif
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
index ce9cbc5..389d67b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/RobotDriveBase.h
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string>
 
-#include <wpi/ArrayRef.h>
-#include <wpi/raw_ostream.h>
+#include <wpi/deprecated.h>
+#include <wpi/span.h>
 
 #include "frc/MotorSafety.h"
 
 namespace frc {
 
-class SpeedController;
-
 /**
  * Common base class for drive platforms.
  */
@@ -47,7 +43,7 @@
    *
    * The default value is 0.02. Inputs smaller than the deadband are set to 0.0
    * while inputs larger than the deadband are scaled from 0.0 to 1.0. See
-   * ApplyDeadband().
+   * frc::ApplyDeadband().
    *
    * @param deadband The deadband to set.
    */
@@ -71,7 +67,7 @@
   void FeedWatchdog();
 
   void StopMotor() override = 0;
-  void GetDescription(wpi::raw_ostream& desc) const override = 0;
+  std::string GetDescription() const override = 0;
 
  protected:
   /**
@@ -80,14 +76,16 @@
    *
    * @param value    value to clip
    * @param deadband range around zero
+   * @deprecated Use ApplyDeadband() in frc/MathUtil.h.
    */
-  double ApplyDeadband(double number, double deadband);
+  WPI_DEPRECATED("Use ApplyDeadband() in frc/MathUtil.h")
+  static double ApplyDeadband(double value, double deadband);
 
   /**
    * Normalize all wheel speeds if the magnitude of any wheel is greater than
    * 1.0.
    */
-  void Normalize(wpi::MutableArrayRef<double> wheelSpeeds);
+  static void Normalize(wpi::span<double> wheelSpeeds);
 
   double m_deadband = 0.02;
   double m_maxOutput = 1.0;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/Vector2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/Vector2d.h
index c9bd08a..92a3de6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/Vector2d.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/drive/Vector2d.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/filters/Filter.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/filters/Filter.h
deleted file mode 100644
index fb14d28..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/filters/Filter.h
+++ /dev/null
@@ -1,64 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <memory>
-
-#include <wpi/deprecated.h>
-
-#include "frc/PIDSource.h"
-
-namespace frc {
-
-/**
- * Interface for filters
- *
- * @deprecated only used by the deprecated LinearDigitalFilter
- */
-class Filter : public PIDSource {
- public:
-  WPI_DEPRECATED("This class is no longer used.")
-  explicit Filter(PIDSource& source);
-  WPI_DEPRECATED("This class is no longer used.")
-  explicit Filter(std::shared_ptr<PIDSource> source);
-  virtual ~Filter() = default;
-
-  Filter(Filter&&) = default;
-  Filter& operator=(Filter&&) = default;
-
-  // PIDSource interface
-  void SetPIDSourceType(PIDSourceType pidSource) override;
-  PIDSourceType GetPIDSourceType() const override;
-  double PIDGet() override = 0;
-
-  /**
-   * Returns the current filter estimate without also inserting new data as
-   * PIDGet() would do.
-   *
-   * @return The current filter estimate
-   */
-  virtual double Get() const = 0;
-
-  /**
-   * Reset the filter state
-   */
-  virtual void Reset() = 0;
-
- protected:
-  /**
-   * Calls PIDGet() of source
-   *
-   * @return Current value of source
-   */
-  double PIDGetSource();
-
- private:
-  std::shared_ptr<PIDSource> m_source;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
deleted file mode 100644
index c4fc3ef..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/filters/LinearDigitalFilter.h
+++ /dev/null
@@ -1,226 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <initializer_list>
-#include <memory>
-#include <vector>
-
-#include <wpi/ArrayRef.h>
-#include <wpi/circular_buffer.h>
-#include <wpi/deprecated.h>
-
-#include "frc/filters/Filter.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, I highly recommend the following articles:<br>
- *  http://en.wikipedia.org/wiki/Linear_filter<br>
- *  http://en.wikipedia.org/wiki/Iir_filter<br>
- *  http://en.wikipedia.org/wiki/Fir_filter<br>
- *
- * Note 1: PIDGet() should be called by the user on a known, regular period.
- * You can set up a Notifier to do this (look at the WPILib PIDController
- * class), 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 PIDGet() gets called at the desired, constant frequency!
- *
- * @deprecated Use LinearFilter class instead
- */
-class LinearDigitalFilter : public Filter {
- public:
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param source  The PIDSource object that is used to get values
-   * @param ffGains The "feed forward" or FIR gains
-   * @param fbGains The "feed back" or IIR gains
-   */
-  WPI_DEPRECATED("Use LinearFilter class instead.")
-  LinearDigitalFilter(PIDSource& source, wpi::ArrayRef<double> ffGains,
-                      wpi::ArrayRef<double> fbGains);
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param source  The PIDSource object that is used to get values
-   * @param ffGains The "feed forward" or FIR gains
-   * @param fbGains The "feed back" or IIR gains
-   */
-  WPI_DEPRECATED("Use LinearFilter class instead.")
-  LinearDigitalFilter(PIDSource& source, std::initializer_list<double> ffGains,
-                      std::initializer_list<double> fbGains);
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param source  The PIDSource object that is used to get values
-   * @param ffGains The "feed forward" or FIR gains
-   * @param fbGains The "feed back" or IIR gains
-   */
-  WPI_DEPRECATED("Use LinearFilter class instead.")
-  LinearDigitalFilter(std::shared_ptr<PIDSource> source,
-                      wpi::ArrayRef<double> ffGains,
-                      wpi::ArrayRef<double> fbGains);
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param source  The PIDSource object that is used to get values
-   * @param ffGains The "feed forward" or FIR gains
-   * @param fbGains The "feed back" or IIR gains
-   */
-  WPI_DEPRECATED("Use LinearFilter class instead.")
-  LinearDigitalFilter(std::shared_ptr<PIDSource> source,
-                      std::initializer_list<double> ffGains,
-                      std::initializer_list<double> fbGains);
-
-  LinearDigitalFilter(LinearDigitalFilter&&) = default;
-  LinearDigitalFilter& operator=(LinearDigitalFilter&&) = default;
-
-  // 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 source       The PIDSource object that is used to get values
-   * @param timeConstant The discrete-time time constant in seconds
-   * @param period       The period in seconds between samples taken by the user
-   */
-  static LinearDigitalFilter SinglePoleIIR(PIDSource& source,
-                                           double timeConstant, double period);
-
-  /**
-   * 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 source       The PIDSource object that is used to get values
-   * @param timeConstant The discrete-time time constant in seconds
-   * @param period       The period in seconds between samples taken by the user
-   */
-  static LinearDigitalFilter HighPass(PIDSource& source, double timeConstant,
-                                      double period);
-
-  /**
-   * 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 source The PIDSource object that is used to get values
-   * @param taps   The number of samples to average over. Higher = smoother but
-   *               slower
-   */
-  static LinearDigitalFilter MovingAverage(PIDSource& source, int taps);
-
-  /**
-   * 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 source       The PIDSource object that is used to get values
-   * @param timeConstant The discrete-time time constant in seconds
-   * @param period       The period in seconds between samples taken by the user
-   */
-  static LinearDigitalFilter SinglePoleIIR(std::shared_ptr<PIDSource> source,
-                                           double timeConstant, double period);
-
-  /**
-   * 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 source       The PIDSource object that is used to get values
-   * @param timeConstant The discrete-time time constant in seconds
-   * @param period       The period in seconds between samples taken by the user
-   */
-  static LinearDigitalFilter HighPass(std::shared_ptr<PIDSource> source,
-                                      double timeConstant, double period);
-
-  /**
-   * 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 source The PIDSource object that is used to get values
-   * @param taps   The number of samples to average over. Higher = smoother but
-   *               slower
-   */
-  static LinearDigitalFilter MovingAverage(std::shared_ptr<PIDSource> source,
-                                           int taps);
-
-  // Filter interface
-  double Get() const override;
-  void Reset() override;
-
-  // PIDSource interface
-  /**
-   * Calculates the next value of the filter
-   *
-   * @return The filtered value at this step
-   */
-  double PIDGet() override;
-
- private:
-  wpi::circular_buffer<double> m_inputs;
-  wpi::circular_buffer<double> m_outputs;
-  std::vector<double> m_inputGains;
-  std::vector<double> m_outputGains;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h
index 499ba5b..c95466a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Accelerometer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Gyro.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
index 8c06993..50417ea 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Gyro.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Potentiometer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Potentiometer.h
deleted file mode 100644
index 219e6ba..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/interfaces/Potentiometer.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/PIDSource.h"
-
-namespace frc {
-
-/**
- * Interface for potentiometers.
- */
-class Potentiometer : public PIDSource {
- public:
-  Potentiometer() = default;
-  virtual ~Potentiometer() = default;
-
-  Potentiometer(Potentiometer&&) = default;
-  Potentiometer& operator=(Potentiometer&&) = default;
-
-  /**
-   * Common interface for getting the current value of a potentiometer.
-   *
-   * @return The current set speed. Value is between -1.0 and 1.0.
-   */
-  virtual double Get() const = 0;
-
-  void SetPIDSourceType(PIDSourceType pidSource) override;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
index b755f46..6851cde 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/livewindow/LiveWindow.h
@@ -1,66 +1,77 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
-#include <memory>
+
+#include <wpi/deprecated.h>
+
+namespace wpi {
+class Sendable;
+}  // namespace wpi
 
 namespace frc {
 
-class Sendable;
-
 /**
  * The LiveWindow class is the public interface for putting sensors and
  * actuators on the LiveWindow.
  */
 class LiveWindow {
  public:
-  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.
    *
    * This is a singleton to guarantee that there is only a single instance
    * regardless of how many times GetInstance is called.
+   * @deprecated Use the static methods unless guaranteeing LiveWindow is
+   * instantiated
    */
+  WPI_DEPRECATED("Use static methods")
   static LiveWindow* GetInstance();
 
   /**
+   * Set function to be called when LiveWindow is enabled.
+   *
+   * @param func function (or nullptr for none)
+   */
+  static void SetEnabledCallback(std::function<void()> func);
+
+  /**
+   * Set function to be called when LiveWindow is disabled.
+   *
+   * @param func function (or nullptr for none)
+   */
+  static void SetDisabledCallback(std::function<void()> func);
+
+  /**
    * Enable telemetry for a single component.
    *
-   * @param sendable component
+   * @param component sendable
    */
-  void EnableTelemetry(Sendable* component);
+  static void EnableTelemetry(wpi::Sendable* component);
 
   /**
    * Disable telemetry for a single component.
    *
-   * @param sendable component
+   * @param component sendable
    */
-  void DisableTelemetry(Sendable* component);
+  static void DisableTelemetry(wpi::Sendable* component);
 
   /**
    * Disable ALL telemetry.
    */
-  void DisableAllTelemetry();
+  static void DisableAllTelemetry();
 
-  bool IsEnabled() const;
+  static bool IsEnabled();
 
   /**
    * Change the enabled status of LiveWindow.
    *
    * If it changes to enabled, start livewindow running otherwise stop it
    */
-  void SetEnabled(bool enabled);
+  static void SetEnabled(bool enabled);
 
   /**
    * Tell all the sensors to update (send) their values.
@@ -68,18 +79,15 @@
    * Actuators are handled through callbacks on their value changing from the
    * SmartDashboard widgets.
    */
-  void UpdateValues();
+  static void UpdateValues();
 
  private:
-  LiveWindow();
-
-  struct Impl;
-  std::unique_ptr<Impl> m_impl;
+  LiveWindow() = default;
 
   /**
    * Updates the entries, without using a mutex or lock.
    */
-  void UpdateValuesUnsafe();
+  static void UpdateValuesUnsafe();
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/DMC60.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/DMC60.h
new file mode 100644
index 0000000..f47826a
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/DMC60.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Digilent DMC 60 Motor %Controller.
+ *
+ * Note that the DMC 60 uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the DMC 60 User
+ * Manual available from Digilent.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class DMC60 : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a Digilent DMC 60.
+   *
+   * @param channel The PWM channel that the DMC 60 is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit DMC60(int channel);
+
+  DMC60(DMC60&&) = default;
+  DMC60& operator=(DMC60&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Jaguar.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Jaguar.h
new file mode 100644
index 0000000..d25a457
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Jaguar.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Luminary Micro / Vex Robotics Jaguar Motor %Controller with PWM control.
+ *
+ * Note that the Jaguar uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended. The
+ * calibration procedure can be found in the Jaguar User Manual available from
+ * Vex.
+ *
+ * \li 2.310ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.507ms = center of the deadband range (off)
+ * \li 1.454ms = the "low end" of the deadband range
+ * \li 0.697ms = full "reverse"
+ */
+class Jaguar : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a Jaguar connected via PWM.
+   *
+   * @param channel The PWM channel that the Jaguar is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit Jaguar(int channel);
+
+  Jaguar(Jaguar&&) = default;
+  Jaguar& operator=(Jaguar&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorController.h
new file mode 100644
index 0000000..8ed19bc
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorController.h
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <units/voltage.h>
+
+#include "frc/SpeedController.h"
+
+namespace frc {
+
+#if defined(_MSC_VER)
+#pragma warning(push)
+#pragma warning(disable : 4996)  // was declared deprecated
+#elif defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+#elif defined(__GNUC__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+/**
+ * Interface for motor controlling devices.
+ */
+class MotorController : public SpeedController {};
+
+#if defined(_MSC_VER)
+#pragma warning(pop)
+#elif defined(__clang__)
+#pragma clang diagnostic pop
+#elif defined(__GNUC__)
+#pragma GCC diagnostic pop
+#endif
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
new file mode 100644
index 0000000..6f47bf9
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.h
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/motorcontrol/MotorController.h"
+
+namespace frc {
+
+/**
+ * Allows multiple MotorController objects to be linked together.
+ */
+class MotorControllerGroup : public wpi::Sendable,
+                             public MotorController,
+                             public wpi::SendableHelper<MotorControllerGroup> {
+ public:
+  template <class... MotorControllers>
+  explicit MotorControllerGroup(MotorController& motorController,
+                                MotorControllers&... motorControllers);
+  explicit MotorControllerGroup(
+      std::vector<std::reference_wrapper<MotorController>>&& motorControllers);
+
+  MotorControllerGroup(MotorControllerGroup&&) = default;
+  MotorControllerGroup& operator=(MotorControllerGroup&&) = default;
+
+  void Set(double speed) override;
+  double Get() const override;
+  void SetInverted(bool isInverted) override;
+  bool GetInverted() const override;
+  void Disable() override;
+  void StopMotor() override;
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  bool m_isInverted = false;
+  std::vector<std::reference_wrapper<MotorController>> m_motorControllers;
+
+  void Initialize();
+};
+
+}  // namespace frc
+
+#include "frc/motorcontrol/MotorControllerGroup.inc"
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.inc b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.inc
new file mode 100644
index 0000000..8f87635
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/MotorControllerGroup.inc
@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include "frc/motorcontrol/MotorControllerGroup.h"
+
+namespace frc {
+
+template <class... MotorControllers>
+MotorControllerGroup::MotorControllerGroup(
+    MotorController& motorController, MotorControllers&... motorControllers)
+    : m_motorControllers(std::vector<std::reference_wrapper<MotorController>>{
+          motorController, motorControllers...}) {
+  Initialize();
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h
new file mode 100644
index 0000000..cc95d71
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/NidecBrushless.h
@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/DigitalOutput.h"
+#include "frc/MotorSafety.h"
+#include "frc/PWM.h"
+#include "frc/motorcontrol/MotorController.h"
+
+namespace frc {
+
+/**
+ * Nidec Brushless Motor.
+ */
+class NidecBrushless : public MotorController,
+                       public MotorSafety,
+                       public wpi::Sendable,
+                       public wpi::SendableHelper<NidecBrushless> {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param pwmChannel The PWM channel that the Nidec Brushless controller is
+   *                   attached to. 0-9 are on-board, 10-19 are on the MXP port.
+   * @param dioChannel The DIO channel that the Nidec Brushless controller is
+   *                   attached to. 0-9 are on-board, 10-25 are on the MXP port.
+   */
+  NidecBrushless(int pwmChannel, int dioChannel);
+
+  ~NidecBrushless() override = default;
+
+  NidecBrushless(NidecBrushless&&) = default;
+  NidecBrushless& operator=(NidecBrushless&&) = default;
+
+  // MotorController interface
+  /**
+   * Set the PWM value.
+   *
+   * The PWM value is set using a range of -1.0 to 1.0, appropriately scaling
+   * the value for the FPGA.
+   *
+   * @param speed The speed value between -1.0 and 1.0 to set.
+   */
+  void Set(double speed) override;
+
+  /**
+   * Get the recently set value of the PWM.
+   *
+   * @return The most recently set value for the PWM between -1.0 and 1.0.
+   */
+  double Get() const override;
+
+  void SetInverted(bool isInverted) override;
+
+  bool GetInverted() const override;
+
+  /**
+   * Disable the motor. The Enable() function must be called to re-enable the
+   * motor.
+   */
+  void Disable() override;
+
+  /**
+   * Re-enable the motor after Disable() has been called. The Set() function
+   * must be called to set a new motor speed.
+   */
+  void Enable();
+
+  // MotorSafety interface
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+  /**
+   * Gets the channel number associated with the object.
+   *
+   * @return The channel number.
+   */
+  int GetChannel() const;
+
+  // Sendable interface
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  bool m_isInverted = false;
+  bool m_disabled = false;
+  DigitalOutput m_dio;
+  PWM m_pwm;
+  double m_speed = 0.0;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
new file mode 100644
index 0000000..ac6ba4b
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMMotorController.h
@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+#include <string_view>
+
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/MotorSafety.h"
+#include "frc/PWM.h"
+#include "frc/motorcontrol/MotorController.h"
+
+namespace frc {
+class DMA;
+
+/**
+ * Common base class for all PWM Motor Controllers.
+ */
+class PWMMotorController : public MotorController,
+                           public MotorSafety,
+                           public wpi::Sendable,
+                           public wpi::SendableHelper<PWMMotorController> {
+ public:
+  friend class DMA;
+
+  PWMMotorController(PWMMotorController&&) = default;
+  PWMMotorController& operator=(PWMMotorController&&) = default;
+
+  /**
+   * Set the PWM value.
+   *
+   * The PWM value is set using a range of -1.0 to 1.0, appropriately scaling
+   * the value for the FPGA.
+   *
+   * @param value The speed value between -1.0 and 1.0 to set.
+   */
+  void Set(double value) override;
+
+  /**
+   * Get the recently set value of the PWM. This value is affected by the
+   * inversion property. If you want the value that is sent directly to the
+   * MotorController, use PWM::GetSpeed() instead.
+   *
+   * @return The most recently set value for the PWM between -1.0 and 1.0.
+   */
+  double Get() const override;
+
+  void SetInverted(bool isInverted) override;
+
+  bool GetInverted() const override;
+
+  void Disable() override;
+
+  // MotorSafety interface
+  void StopMotor() override;
+  std::string GetDescription() const override;
+
+  int GetChannel() const;
+
+ protected:
+  /**
+   * Constructor for a PWM Motor %Controller connected via PWM.
+   *
+   * @param name Name to use for SendableRegistry
+   * @param channel The PWM channel that the controller is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  PWMMotorController(std::string_view name, int channel);
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+  PWM m_pwm;
+
+ private:
+  bool m_isInverted = false;
+
+  PWM* GetPwm() { return &m_pwm; }
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMSparkMax.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMSparkMax.h
new file mode 100644
index 0000000..45c7bae
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMSparkMax.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * REV Robotics SPARK MAX Motor %Controller.
+ *
+ * Note that the SPARK MAX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SPARK MAX User
+ * Manual available from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
+ */
+class PWMSparkMax : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a SPARK MAX.
+   *
+   * @param channel The PWM channel that the SPARK MAX is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMSparkMax(int channel);
+
+  PWMSparkMax(PWMSparkMax&&) = default;
+  PWMSparkMax& operator=(PWMSparkMax&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMTalonFX.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMTalonFX.h
new file mode 100644
index 0000000..29aada6
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMTalonFX.h
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) %Talon FX Motor %Controller with PWM
+ * control.
+ *
+ * Note that the %Talon FX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the %Talon FX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMTalonFX : public PWMMotorController {
+ public:
+  /**
+   * Construct a %Talon FX connected via PWM.
+   *
+   * @param channel The PWM channel that the %Talon FX is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMTalonFX(int channel);
+
+  PWMTalonFX(PWMTalonFX&&) = default;
+  PWMTalonFX& operator=(PWMTalonFX&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMTalonSRX.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMTalonSRX.h
new file mode 100644
index 0000000..9f9e384
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMTalonSRX.h
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) %Talon SRX Motor %Controller with PWM
+ * control.
+ *
+ * Note that the %Talon SRX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the %Talon SRX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMTalonSRX : public PWMMotorController {
+ public:
+  /**
+   * Construct a %Talon SRX connected via PWM.
+   *
+   * @param channel The PWM channel that the %Talon SRX is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMTalonSRX(int channel);
+
+  PWMTalonSRX(PWMTalonSRX&&) = default;
+  PWMTalonSRX& operator=(PWMTalonSRX&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMVenom.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMVenom.h
new file mode 100644
index 0000000..1047e8e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMVenom.h
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * Note that the Venom uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMVenom : public PWMMotorController {
+ public:
+  /**
+   * Construct a Venom connected via PWM.
+   *
+   * @param channel The PWM channel that the Venom is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit PWMVenom(int channel);
+
+  PWMVenom(PWMVenom&&) = default;
+  PWMVenom& operator=(PWMVenom&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMVictorSPX.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMVictorSPX.h
new file mode 100644
index 0000000..b6bb324
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/PWMVictorSPX.h
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) %Victor SPX Motor %Controller with PWM
+ * control.
+ *
+ * Note that the %Victor SPX uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the %Victor SPX User
+ * Manual available from Cross The Road Electronics.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class PWMVictorSPX : public PWMMotorController {
+ public:
+  /**
+   * Construct a %Victor SPX connected via PWM.
+   *
+   * @param channel The PWM channel that the %Victor SPX is attached to. 0-9
+   *                are on-board, 10-19 are on the MXP port
+   */
+  explicit PWMVictorSPX(int channel);
+
+  PWMVictorSPX(PWMVictorSPX&&) = default;
+  PWMVictorSPX& operator=(PWMVictorSPX&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/SD540.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/SD540.h
new file mode 100644
index 0000000..44a0c8c
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/SD540.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Mindsensors SD540 Motor %Controller.
+ *
+ * Note that the SD540 uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SD540 User Manual available
+ * from Mindsensors.
+ *
+ * \li 2.05ms = full "forward"
+ * \li 1.55ms = the "high end" of the deadband range
+ * \li 1.50ms = center of the deadband range (off)
+ * \li 1.44ms = the "low end" of the deadband range
+ * \li 0.94ms = full "reverse"
+ */
+class SD540 : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a SD540.
+   *
+   * @param channel The PWM channel that the SD540 is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit SD540(int channel);
+
+  SD540(SD540&&) = default;
+  SD540& operator=(SD540&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Spark.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Spark.h
new file mode 100644
index 0000000..6163234
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Spark.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * REV Robotics SPARK Motor %Controller.
+ *
+ * Note that the SPARK uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the SPARK User Manual available
+ * from REV Robotics.
+ *
+ * \li 2.003ms = full "forward"
+ * \li 1.550ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.460ms = the "low end" of the deadband range
+ * \li 0.999ms = full "reverse"
+ */
+class Spark : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a SPARK.
+   *
+   * @param channel The PWM channel that the SPARK is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit Spark(int channel);
+
+  Spark(Spark&&) = default;
+  Spark& operator=(Spark&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Talon.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Talon.h
new file mode 100644
index 0000000..acf5c55
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Talon.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Cross the Road Electronics (CTRE) %Talon and %Talon SR Motor %Controller.
+ *
+ * Note that the %Talon uses the following bounds for PWM values. These values
+ * should work reasonably well for most controllers, but if users experience
+ * issues such as asymmetric behavior around the deadband or inability to
+ * saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the %Talon User Manual available
+ * from CTRE.
+ *
+ * \li 2.037ms = full "forward"
+ * \li 1.539ms = the "high end" of the deadband range
+ * \li 1.513ms = center of the deadband range (off)
+ * \li 1.487ms = the "low end" of the deadband range
+ * \li 0.989ms = full "reverse"
+ */
+class Talon : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a %Talon (original or %Talon SR).
+   *
+   * @param channel The PWM channel number that the %Talon is attached to. 0-9
+   *                are on-board, 10-19 are on the MXP port
+   */
+  explicit Talon(int channel);
+
+  Talon(Talon&&) = default;
+  Talon& operator=(Talon&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Victor.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Victor.h
new file mode 100644
index 0000000..9627925
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/Victor.h
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Vex Robotics %Victor 888 Motor %Controller.
+ *
+ * The Vex Robotics %Victor 884 Motor %Controller can also be used with this
+ * class but may need to be calibrated per the Victor 884 user manual.
+ *
+ * Note that the %Victor uses the following bounds for PWM values.  These
+ * values were determined empirically and optimized for the %Victor 888. These
+ * values should work reasonably well for %Victor 884 controllers as well but
+ * if users experience issues such as asymmetric behavior around the deadband
+ * or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the %Victor 884 User
+ * Manual available from Vex.
+ *
+ * \li 2.027ms = full "forward"
+ * \li 1.525ms = the "high end" of the deadband range
+ * \li 1.507ms = center of the deadband range (off)
+ * \li 1.490ms = the "low end" of the deadband range
+ * \li 1.026ms = full "reverse"
+ */
+class Victor : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a %Victor.
+   *
+   * @param channel The PWM channel number that the %Victor is attached to. 0-9
+   *                are on-board, 10-19 are on the MXP port
+   */
+  explicit Victor(int channel);
+
+  Victor(Victor&&) = default;
+  Victor& operator=(Victor&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/VictorSP.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/VictorSP.h
new file mode 100644
index 0000000..f599ff4
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/motorcontrol/VictorSP.h
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/PWMMotorController.h"
+
+namespace frc {
+
+/**
+ * Vex Robotics %Victor SP Motor %Controller.
+ *
+ * Note that the %Victor SP uses the following bounds for PWM values. These
+ * values should work reasonably well for most controllers, but if users
+ * experience issues such as asymmetric behavior around the deadband or
+ * inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the %Victor SP User
+ * Manual available from Vex.
+ *
+ * \li 2.004ms = full "forward"
+ * \li 1.520ms = the "high end" of the deadband range
+ * \li 1.500ms = center of the deadband range (off)
+ * \li 1.480ms = the "low end" of the deadband range
+ * \li 0.997ms = full "reverse"
+ */
+class VictorSP : public PWMMotorController {
+ public:
+  /**
+   * Constructor for a %Victor SP.
+   *
+   * @param channel The PWM channel that the Victor SP is attached to. 0-9 are
+   *                on-board, 10-19 are on the MXP port
+   */
+  explicit VictorSP(int channel);
+
+  VictorSP(VictorSP&&) = default;
+  VictorSP& operator=(VictorSP&&) = default;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h
index 1d1ea5f..ca5e29f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInLayouts.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
index df943b2..9ac315f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/BuiltInWidgets.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -163,8 +160,8 @@
    */
   kVoltageView,
   /**
-   * Displays a PowerDistributionPanel. <br>Supported types: <ul> <li>
-   * PowerDistributionPanel</li>
+   * Displays a PowerDistribution. <br>Supported types: <ul> <li>
+   * PowerDistribution</li>
    * </ul>
    * <br>Custom properties:
    * <table>
@@ -173,7 +170,7 @@
    * <td>Whether or not to display the voltage and current draw</td></tr>
    * </table>
    */
-  kPowerDistributionPanel,
+  kPowerDistribution,
   /**
    * Displays a SendableChooser with a dropdown combo box with a list of
    * options.
@@ -203,10 +200,10 @@
    */
   kEncoder,
   /**
-   * Displays a SpeedController.
-   * The speed controller will be controllable from the dashboard when test mode
+   * Displays a MotorController.
+   * The motor controller will be controllable from the dashboard when test mode
    * is enabled, but will otherwise be view-only. <br>Supported types: <ul>
-   * <li>PWMSpeedController</li>
+   * <li>PWMMotorController</li>
    * <li>DMC60</li>
    * <li>Jaguar</li>
    * <li>PWMTalonSRX</li>
@@ -216,7 +213,7 @@
    * <li>Talon</li>
    * <li>Victor</li>
    * <li>VictorSP</li>
-   * <li>SpeedControllerGroup</li>
+   * <li>MotorControllerGroup</li>
    * <li>Any custom subclass of {@code SpeedContorller}</li>
    * </ul>
    * <br>Custom properties:
@@ -226,7 +223,7 @@
    * <td>One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
    * </table>
    */
-  kSpeedController,
+  kMotorController,
   /**
    * Displays a command with a toggle button. Pressing the button will start the
    * command, and the button will automatically release when the command
@@ -283,7 +280,7 @@
    * <br>Custom properties:
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
-   * <tr><td>Range</td><td>{@link Range}</td><td>k16G</td><td>The accelerometer
+   * <tr><td>Range</td><td>Range</td><td>k16G</td><td>The accelerometer
    * range</td></tr> <tr><td>Show value</td><td>Boolean</td><td>true</td>
    * <td>Show or hide the acceleration values</td></tr>
    * <tr><td>Precision</td><td>Number</td><td>2</td>
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ComplexWidget.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ComplexWidget.h
index 218ed2f..dc49d64 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ComplexWidget.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ComplexWidget.h
@@ -1,34 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
-#include <wpi/Twine.h>
 
 #include "frc/shuffleboard/ShuffleboardWidget.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableBuilderImpl.h"
+
+namespace wpi {
+class Sendable;
+class SendableBuilder;
+}  // namespace wpi
 
 namespace frc {
 
-class Sendable;
 class ShuffleboardContainer;
 
 /**
- * A Shuffleboard widget that handles a {@link Sendable} object such as a speed
+ * A Shuffleboard widget that handles a Sendable object such as a speed
  * controller or sensor.
  */
 class ComplexWidget final : public ShuffleboardWidget<ComplexWidget> {
  public:
-  ComplexWidget(ShuffleboardContainer& parent, const wpi::Twine& title,
-                Sendable& sendable);
+  ComplexWidget(ShuffleboardContainer& parent, std::string_view title,
+                wpi::Sendable& sendable);
+
+  ~ComplexWidget() override;
 
   void EnableIfActuator() override;
 
@@ -38,9 +39,8 @@
                  std::shared_ptr<nt::NetworkTable> metaTable) override;
 
  private:
-  Sendable& m_sendable;
-  SendableBuilderImpl m_builder;
-  bool m_builderInit = false;
+  wpi::Sendable& m_sendable;
+  std::unique_ptr<wpi::SendableBuilder> m_builder;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h
index 00a5e36..5fa5ca4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/LayoutType.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace frc {
 
@@ -28,7 +25,7 @@
    * Gets the string type of the layout as defined by that layout in
    * Shuffleboard.
    */
-  wpi::StringRef GetLayoutName() const;
+  std::string_view GetLayoutName() const;
 
  private:
   const char* m_layoutName;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/RecordingController.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/RecordingController.h
index dacbdb4..83b23a4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/RecordingController.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/RecordingController.h
@@ -1,24 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableInstance.h>
 #include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
 
 #include "frc/shuffleboard/ShuffleboardEventImportance.h"
 
-namespace frc {
-namespace detail {
+namespace frc::detail {
 
 class RecordingController final {
  public:
@@ -27,10 +23,10 @@
 
   void StartRecording();
   void StopRecording();
-  void SetRecordingFileNameFormat(wpi::StringRef format);
+  void SetRecordingFileNameFormat(std::string_view format);
   void ClearRecordingFileNameFormat();
 
-  void AddEventMarker(wpi::StringRef name, wpi::StringRef description,
+  void AddEventMarker(std::string_view name, std::string_view description,
                       ShuffleboardEventImportance importance);
 
  private:
@@ -39,5 +35,4 @@
   std::shared_ptr<nt::NetworkTable> m_eventsTable;
 };
 
-}  // namespace detail
-}  // namespace frc
+}  // namespace frc::detail
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
index 5610cf8..e7c9a81 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SendableCameraWrapper.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -17,12 +14,12 @@
 namespace cs {
 class VideoSource;
 }  // namespace cs
-typedef int CS_Handle;
-typedef CS_Handle CS_Source;
+using CS_Handle = int;        // NOLINT
+using CS_Source = CS_Handle;  // NOLINT
 #endif
 
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
@@ -32,14 +29,15 @@
 constexpr const char* kProtocol = "camera_server://";
 std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
     CS_Source source);
-void AddToSendableRegistry(Sendable* sendable, std::string name);
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string name);
 }  // namespace detail
 
 /**
  * A wrapper to make video sources sendable and usable from Shuffleboard.
  */
-class SendableCameraWrapper : public Sendable,
-                              public SendableHelper<SendableCameraWrapper> {
+class SendableCameraWrapper
+    : public wpi::Sendable,
+      public wpi::SendableHelper<SendableCameraWrapper> {
  private:
   struct private_init {};
 
@@ -63,7 +61,7 @@
   static SendableCameraWrapper& Wrap(const cs::VideoSource& source);
   static SendableCameraWrapper& Wrap(CS_Source source);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 
  private:
   std::string m_uri;
@@ -86,8 +84,9 @@
 
 inline SendableCameraWrapper& SendableCameraWrapper::Wrap(CS_Source source) {
   auto& wrapper = detail::GetSendableCameraWrapper(source);
-  if (!wrapper)
+  if (!wrapper) {
     wrapper = std::make_shared<SendableCameraWrapper>(source, private_init{});
+  }
   return *wrapper;
 }
 #endif
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/Shuffleboard.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/Shuffleboard.h
index e49d1c3..252d74a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/Shuffleboard.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/Shuffleboard.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
 #include "frc/shuffleboard/RecordingController.h"
 #include "frc/shuffleboard/ShuffleboardEventImportance.h"
@@ -80,7 +77,7 @@
    * @param title the title of the tab
    * @return the tab with the given title
    */
-  static ShuffleboardTab& GetTab(wpi::StringRef title);
+  static ShuffleboardTab& GetTab(std::string_view title);
 
   /**
    * Selects the tab in the dashboard with the given index in the range
@@ -96,10 +93,10 @@
    *
    * @param title the title of the tab to select
    */
-  static void SelectTab(wpi::StringRef title);
+  static void SelectTab(std::string_view title);
 
   /**
-   * Enables user control of widgets containing actuators: speed controllers,
+   * Enables user control of widgets containing actuators: motor controllers,
    * relays, etc. This should only be used when the robot is in test mode.
    * IterativeRobotBase and SampleRobot are both configured to call this method
    * when entering test mode; most users should not need to use this method
@@ -144,13 +141,13 @@
    *
    * @param format the format for the
    */
-  static void SetRecordingFileNameFormat(wpi::StringRef format);
+  static void SetRecordingFileNameFormat(std::string_view format);
 
   /**
    * Clears the custom name format for recording files. New recordings will use
    * the default format.
    *
-   * @see #setRecordingFileNameFormat(String)
+   * @see SetRecordingFileNameFormat(std::string_view)
    */
   static void ClearRecordingFileNameFormat();
 
@@ -166,7 +163,8 @@
    * @param description a description of the event
    * @param importance  the importance of the event
    */
-  static void AddEventMarker(wpi::StringRef name, wpi::StringRef description,
+  static void AddEventMarker(std::string_view name,
+                             std::string_view description,
                              ShuffleboardEventImportance importance);
 
   /**
@@ -180,7 +178,7 @@
    * @param name        the name of the event
    * @param importance  the importance of the event
    */
-  static void AddEventMarker(wpi::StringRef name,
+  static void AddEventMarker(std::string_view name,
                              ShuffleboardEventImportance importance);
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h
index d6b4bc4..24f65a3 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableValue.h>
 #include <wpi/StringMap.h>
-#include <wpi/Twine.h>
 
 #include "frc/shuffleboard/ShuffleboardComponentBase.h"
 
@@ -29,10 +26,8 @@
 template <typename Derived>
 class ShuffleboardComponent : public ShuffleboardComponentBase {
  public:
-  ShuffleboardComponent(ShuffleboardContainer& parent, const wpi::Twine& title,
-                        const wpi::Twine& type = "");
-
-  virtual ~ShuffleboardComponent() = default;
+  ShuffleboardComponent(ShuffleboardContainer& parent, std::string_view title,
+                        std::string_view type = "");
 
   /**
    * Sets custom properties for this component. Property names are
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.inc b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.inc
index f75fb0d..9750d4a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.inc
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponent.inc
@@ -1,21 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
+
+#include "frc/shuffleboard/ShuffleboardComponent.h"
 
 namespace frc {
 
 template <typename Derived>
 ShuffleboardComponent<Derived>::ShuffleboardComponent(
-    ShuffleboardContainer& parent, const wpi::Twine& title,
-    const wpi::Twine& type)
+    ShuffleboardContainer& parent, std::string_view title,
+    std::string_view type)
     : ShuffleboardValue(title),
       ShuffleboardComponentBase(parent, title, type) {}
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponentBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponentBase.h
index c63e517..d33a234 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponentBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardComponentBase.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableValue.h>
 #include <wpi/StringMap.h>
-#include <wpi/Twine.h>
 
 #include "frc/shuffleboard/ShuffleboardValue.h"
 
@@ -27,12 +24,9 @@
 class ShuffleboardComponentBase : public virtual ShuffleboardValue {
  public:
   ShuffleboardComponentBase(ShuffleboardContainer& parent,
-                            const wpi::Twine& title,
-                            const wpi::Twine& type = "");
+                            std::string_view title, std::string_view type = "");
 
-  virtual ~ShuffleboardComponentBase() = default;
-
-  void SetType(const wpi::Twine& type);
+  void SetType(std::string_view type);
 
   void BuildMetadata(std::shared_ptr<nt::NetworkTable> metaTable);
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
index 7b8b49d..db58a21 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardContainer.h
@@ -1,26 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableValue.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallSet.h>
 #include <wpi/StringMap.h>
-#include <wpi/Twine.h>
+#include <wpi/span.h>
 
-#include "frc/ErrorBase.h"
-#include "frc/WPIErrors.h"
 #include "frc/shuffleboard/BuiltInLayouts.h"
 #include "frc/shuffleboard/LayoutType.h"
 #include "frc/shuffleboard/ShuffleboardComponentBase.h"
@@ -31,24 +26,26 @@
 class VideoSource;
 }  // namespace cs
 
+namespace wpi {
+class Sendable;
+}  // namespace wpi
+
 namespace frc {
 
 class ComplexWidget;
-class Sendable;
 class ShuffleboardLayout;
 class SimpleWidget;
 
 /**
  * Common interface for objects that can contain shuffleboard components.
  */
-class ShuffleboardContainer : public virtual ShuffleboardValue,
-                              public ErrorBase {
+class ShuffleboardContainer : public virtual ShuffleboardValue {
  public:
-  explicit ShuffleboardContainer(const wpi::Twine& title);
+  explicit ShuffleboardContainer(std::string_view title);
 
   ShuffleboardContainer(ShuffleboardContainer&& rhs) = default;
 
-  virtual ~ShuffleboardContainer() = default;
+  ~ShuffleboardContainer() override = default;
 
   /**
    * Gets the components that are direct children of this container.
@@ -60,38 +57,36 @@
    * Gets the layout with the given type and title, creating it if it does not
    * already exist at the time this method is called.
    *
-   * @param title      the title of the layout
-   * @param layoutType the type of the layout, eg "List" or "Grid"
+   * @param title the title of the layout
+   * @param type  the type of the layout, eg "List" or "Grid"
    * @return the layout
    */
-  ShuffleboardLayout& GetLayout(const wpi::Twine& title, BuiltInLayouts type);
+  ShuffleboardLayout& GetLayout(std::string_view title, BuiltInLayouts type);
 
   /**
    * Gets the layout with the given type and title, creating it if it does not
    * already exist at the time this method is called.
    *
-   * @param title      the title of the layout
-   * @param layoutType the type of the layout, eg "List" or "Grid"
+   * @param title the title of the layout
+   * @param type  the type of the layout, eg "List" or "Grid"
    * @return the layout
    */
-  ShuffleboardLayout& GetLayout(const wpi::Twine& title,
-                                const LayoutType& type);
+  ShuffleboardLayout& GetLayout(std::string_view title, const LayoutType& type);
 
   /**
    * Gets the layout with the given type and title, creating it if it does not
    * already exist at the time this method is called. Note: this method should
    * only be used to use a layout type that is not already built into
    * Shuffleboard. To use a layout built into Shuffleboard, use
-   * {@link #GetLayout(String, LayoutType)} and the layouts in {@link
-   * BuiltInLayouts}.
+   * GetLayout(std::string_view, const LayoutType&) and the layouts in
+   * BuiltInLayouts.
    *
    * @param title the title of the layout
    * @param type  the type of the layout, eg "List Layout" or "Grid Layout"
    * @return the layout
-   * @see #GetLayout(String, LayoutType)
+   * @see GetLayout(std::string_view, const LayoutType&)
    */
-  ShuffleboardLayout& GetLayout(const wpi::Twine& title,
-                                const wpi::Twine& type);
+  ShuffleboardLayout& GetLayout(std::string_view title, std::string_view type);
 
   /**
    * Gets the already-defined layout in this container with the given title.
@@ -108,7 +103,7 @@
    * @return the layout with the given title
    * @throws if no layout has yet been defined with the given title
    */
-  ShuffleboardLayout& GetLayout(const wpi::Twine& title);
+  ShuffleboardLayout& GetLayout(std::string_view title);
 
   /**
    * Adds a widget to this container to display the given sendable.
@@ -119,7 +114,7 @@
    * @throws IllegalArgumentException if a widget already exists in this
    * container with the given title
    */
-  ComplexWidget& Add(const wpi::Twine& title, Sendable& sendable);
+  ComplexWidget& Add(std::string_view title, wpi::Sendable& sendable);
 
   /**
    * Adds a widget to this container to display the given video stream.
@@ -130,7 +125,7 @@
    * @throws IllegalArgumentException if a widget already exists in this
    * container with the given title
    */
-  ComplexWidget& Add(const wpi::Twine& title, const cs::VideoSource& video);
+  ComplexWidget& Add(std::string_view title, const cs::VideoSource& video);
 
   /**
    * Adds a widget to this container to display the given sendable.
@@ -141,7 +136,7 @@
    * container with the given title, or if the sendable's name has not been
    * specified
    */
-  ComplexWidget& Add(Sendable& sendable);
+  ComplexWidget& Add(wpi::Sendable& sendable);
 
   /**
    * Adds a widget to this container to display the given video stream.
@@ -161,9 +156,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, std::shared_ptr<nt::Value>)
+   *      Add(std::string_view title, std::shared_ptr<nt::Value> defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title,
+  SimpleWidget& Add(std::string_view title,
                     std::shared_ptr<nt::Value> defaultValue);
 
   /**
@@ -174,9 +170,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, bool)
+   *      Add(std::string_view title, bool defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title, bool defaultValue);
+  SimpleWidget& Add(std::string_view title, bool defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -186,9 +183,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, double)
+   *      Add(std::string_view title, double defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title, double defaultValue);
+  SimpleWidget& Add(std::string_view title, double defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -198,9 +196,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, int)
+   *      Add(std::string_view title, int defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title, int defaultValue);
+  SimpleWidget& Add(std::string_view title, int defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -210,9 +209,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, std::string_view)
+   *      Add(std::string_view title, std::string_view defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title, const wpi::Twine& defaultValue);
+  SimpleWidget& Add(std::string_view title, std::string_view defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -222,9 +222,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, const char*)
+   *      Add(std::string_view title, const char* defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title, const char* defaultValue);
+  SimpleWidget& Add(std::string_view title, const char* defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -234,9 +235,10 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, wpi::span<const bool>)
+   *      Add(std::string_view title, wpi::span<const bool> defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title, wpi::ArrayRef<bool> defaultValue);
+  SimpleWidget& Add(std::string_view title, wpi::span<const bool> defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -246,10 +248,11 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, wpi::span<const double>)
+   *      Add(std::string_view title, wpi::span<const double> defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title,
-                    wpi::ArrayRef<double> defaultValue);
+  SimpleWidget& Add(std::string_view title,
+                    wpi::span<const double> defaultValue);
 
   /**
    * Adds a widget to this container to display the given data.
@@ -259,10 +262,11 @@
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this
    *         container with the given title
-   * @see #addPersistent(String, Object) add(String title, Object defaultValue)
+   * @see AddPersistent(std::string_view, wpi::span<const std::string>)
+   *      Add(std::string_view title, wpi::span<const std::string> defaultValue)
    */
-  SimpleWidget& Add(const wpi::Twine& title,
-                    wpi::ArrayRef<std::string> defaultValue);
+  SimpleWidget& Add(std::string_view title,
+                    wpi::span<const std::string> defaultValue);
 
   /**
    * Adds a widget to this container. The widget will display the data provided
@@ -271,11 +275,11 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
   SuppliedValueWidget<std::string>& AddString(
-      const wpi::Twine& title, std::function<std::string()> supplier);
+      std::string_view title, std::function<std::string()> supplier);
 
   /**
    * Adds a widget to this container. The widget will display the data provided
@@ -284,10 +288,10 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
-  SuppliedValueWidget<double>& AddNumber(const wpi::Twine& title,
+  SuppliedValueWidget<double>& AddNumber(std::string_view title,
                                          std::function<double()> supplier);
 
   /**
@@ -297,10 +301,10 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
-  SuppliedValueWidget<bool>& AddBoolean(const wpi::Twine& title,
+  SuppliedValueWidget<bool>& AddBoolean(std::string_view title,
                                         std::function<bool()> supplier);
 
   /**
@@ -310,11 +314,11 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
   SuppliedValueWidget<std::vector<std::string>>& AddStringArray(
-      const wpi::Twine& title,
+      std::string_view title,
       std::function<std::vector<std::string>()> supplier);
 
   /**
@@ -324,11 +328,11 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
   SuppliedValueWidget<std::vector<double>>& AddNumberArray(
-      const wpi::Twine& title, std::function<std::vector<double>()> supplier);
+      std::string_view title, std::function<std::vector<double>()> supplier);
 
   /**
    * Adds a widget to this container. The widget will display the data provided
@@ -337,11 +341,11 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
   SuppliedValueWidget<std::vector<int>>& AddBooleanArray(
-      const wpi::Twine& title, std::function<std::vector<int>()> supplier);
+      std::string_view title, std::function<std::vector<int>()> supplier);
 
   /**
    * Adds a widget to this container. The widget will display the data provided
@@ -350,128 +354,136 @@
    * supplier.
    *
    * @param title the title of the widget
-   * @param valueSupplier the supplier for values
+   * @param supplier the supplier for values
    * @return a widget to display data
    */
-  SuppliedValueWidget<wpi::StringRef>& AddRaw(
-      const wpi::Twine& title, std::function<wpi::StringRef()> supplier);
+  SuppliedValueWidget<std::string_view>& AddRaw(
+      std::string_view title, std::function<std::string_view()> supplier);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
-   * on the robot and will be used when the robot program next starts rather
-   * than {@code defaultValue}.
+   * Unlike Add(std::string_view, std::shared_ptr<nt::Value>), the value in the
+   * widget will be saved on the robot and will be used when the robot program
+   * next starts rather than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(stdd::string_view, std::shared_ptr<nt::Value>)
+   *      Add(std::string_view title, std::shared_ptr<nt::Value> defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title,
+  SimpleWidget& AddPersistent(std::string_view title,
                               std::shared_ptr<nt::Value> defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
+   * Unlike Add(std::string_view, bool), the value in the widget will be saved
    * on the robot and will be used when the robot program next starts rather
    * than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std::string_view, bool)
+   *      Add(std::string_view title, bool defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title, bool defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title, bool defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
+   * Unlike Add(std::string_view, double), the value in the widget will be saved
    * on the robot and will be used when the robot program next starts rather
    * than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std::string_view, double)
+   *      Add(std::string_view title, double defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title, double defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title, double defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
-   * on the robot and will be used when the robot program next starts rather
-   * than {@code defaultValue}.
+   * Unlike Add(std::string_view, int), the value in the widget will be saved on
+   * the robot and will be used when the robot program next starts rather than
+   * {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std:string_view, int)
+   *      Add(std::string_view title, int defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title, int defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title, int defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
-   * on the robot and will be used when the robot program next starts rather
-   * than {@code defaultValue}.
+   * Unlike Add(std::string_view, std::string_view), the value in the widget
+   * will be saved on the robot and will be used when the robot program next
+   * starts rather than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std::string_view, std::string_view)
+   *      Add(std::string_view title, std::string_view defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title,
-                              const wpi::Twine& defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title,
+                              std::string_view defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
-   * on the robot and will be used when the robot program next starts rather
-   * than {@code defaultValue}.
+   * Unlike Add(std::string_view, wpi::span<const bool>), the value in the
+   * widget will be saved on the robot and will be used when the robot program
+   * next starts rather than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std::string_view, wpi::span<const bool>)
+   *      Add(std::string_view title, wpi::span<const bool> defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title,
-                              wpi::ArrayRef<bool> defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title,
+                              wpi::span<const bool> defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
-   * on the robot and will be used when the robot program next starts rather
-   * than {@code defaultValue}.
+   * Unlike Add(std::string_view, wpi::span<const double>), the value in the
+   * widget will be saved on the robot and will be used when the robot program
+   * next starts rather than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std::string_view, wpi::span<const double>)
+   *      Add(std::string_view title, wpi::span<const double> defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title,
-                              wpi::ArrayRef<double> defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title,
+                              wpi::span<const double> defaultValue);
 
   /**
    * Adds a widget to this container to display a simple piece of data.
    *
-   * Unlike {@link #add(String, Object)}, the value in the widget will be saved
-   * on the robot and will be used when the robot program next starts rather
-   * than {@code defaultValue}.
+   * Unlike Add(std::string_view, wpi::span<const std::string>), the value in
+   * the widget will be saved on the robot and will be used when the robot
+   * program next starts rather than {@code defaultValue}.
    *
    * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
-   * @see #add(String, Object) add(String title, Object defaultValue)
+   * @see Add(std::string_view, wpi::span<const std::string>)
+   *      Add(std::string_view title, wpi::span<const std::string> defaultValue)
    */
-  SimpleWidget& AddPersistent(const wpi::Twine& title,
-                              wpi::ArrayRef<std::string> defaultValue);
+  SimpleWidget& AddPersistent(std::string_view title,
+                              wpi::span<const std::string> defaultValue);
 
   void EnableIfActuator() override;
 
@@ -490,7 +502,7 @@
    *
    * @return True if title isn't in use; false otherwise.
    */
-  void CheckTitle(const wpi::Twine& title);
+  void CheckTitle(std::string_view title);
 
   friend class SimpleWidget;
 };
@@ -511,7 +523,7 @@
 }
 
 inline frc::ComplexWidget& frc::ShuffleboardContainer::Add(
-    const wpi::Twine& title, const cs::VideoSource& video) {
+    std::string_view title, const cs::VideoSource& video) {
   return Add(title, frc::SendableCameraWrapper::Wrap(video));
 }
 #endif
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
index 57e4743..1fa9c91 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardEventImportance.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace frc {
 
@@ -17,7 +14,7 @@
 
 enum ShuffleboardEventImportance { kTrivial, kLow, kNormal, kHigh, kCritical };
 
-inline wpi::StringRef ShuffleboardEventImportanceName(
+inline std::string_view ShuffleboardEventImportanceName(
     ShuffleboardEventImportance importance) {
   switch (importance) {
     case kTrivial:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
index 2be9859..2885af7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardInstance.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include "frc/shuffleboard/ShuffleboardRoot.h"
 #include "frc/shuffleboard/ShuffleboardTab.h"
 
-namespace frc {
-namespace detail {
+namespace frc::detail {
 
 class ShuffleboardInstance final : public ShuffleboardRoot {
  public:
@@ -23,7 +20,7 @@
   ShuffleboardInstance(ShuffleboardInstance&&) = default;
   ShuffleboardInstance& operator=(ShuffleboardInstance&&) = default;
 
-  frc::ShuffleboardTab& GetTab(wpi::StringRef title) override;
+  frc::ShuffleboardTab& GetTab(std::string_view title) override;
 
   void Update() override;
 
@@ -33,12 +30,11 @@
 
   void SelectTab(int index) override;
 
-  void SelectTab(wpi::StringRef) override;
+  void SelectTab(std::string_view) override;
 
  private:
   struct Impl;
   std::unique_ptr<Impl> m_impl;
 };
 
-}  // namespace detail
-}  // namespace frc
+}  // namespace frc::detail
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h
index effa3da..fe9e102 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardLayout.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
-#include <wpi/Twine.h>
 
 #include "frc/shuffleboard/ShuffleboardComponent.h"
 #include "frc/shuffleboard/ShuffleboardContainer.h"
@@ -29,8 +26,8 @@
 class ShuffleboardLayout : public ShuffleboardComponent<ShuffleboardLayout>,
                            public ShuffleboardContainer {
  public:
-  ShuffleboardLayout(ShuffleboardContainer& parent, const wpi::Twine& name,
-                     const wpi::Twine& type);
+  ShuffleboardLayout(ShuffleboardContainer& parent, std::string_view name,
+                     std::string_view type);
 
   void BuildInto(std::shared_ptr<nt::NetworkTable> parentTable,
                  std::shared_ptr<nt::NetworkTable> metaTable) override;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardRoot.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardRoot.h
index 0c50545..1e766a8 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardRoot.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardRoot.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace frc {
 
@@ -28,7 +25,7 @@
    * @param title the title of the tab
    * @return the tab with the given title
    */
-  virtual ShuffleboardTab& GetTab(wpi::StringRef title) = 0;
+  virtual ShuffleboardTab& GetTab(std::string_view title) = 0;
 
   /**
    * Updates all tabs.
@@ -60,7 +57,7 @@
    *
    * @param title the title of the tab to select
    */
-  virtual void SelectTab(wpi::StringRef title) = 0;
+  virtual void SelectTab(std::string_view title) = 0;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h
index 75896ae..8864cdb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardTab.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
-#include <wpi/StringRef.h>
 
 #include "frc/shuffleboard/ShuffleboardContainer.h"
 
@@ -20,14 +17,15 @@
 
 /**
  * Represents a tab in the Shuffleboard dashboard. Widgets can be added to the
- * tab with {@link #add(Sendable)}, {@link #add(String, Object)}, and
- * {@link #add(String, Sendable)}. Widgets can also be added to layouts with
- * {@link #getLayout(String, String)}; layouts can be nested arbitrarily deep
- * (note that too many levels may make deeper components unusable).
+ * tab with Add(Sendable), Add(std::string_view, Object), and
+ * Add(String, Sendable). Widgets can also be added to layouts with
+ * GetLayout(std::string_view, std::string_view); layouts can be nested
+ * arbitrarily deep (note that too many levels may make deeper components
+ * unusable).
  */
 class ShuffleboardTab final : public ShuffleboardContainer {
  public:
-  ShuffleboardTab(ShuffleboardRoot& root, wpi::StringRef title);
+  ShuffleboardTab(ShuffleboardRoot& root, std::string_view title);
 
   ShuffleboardRoot& GetRoot();
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardValue.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardValue.h
index 4a88cc1..4b7ccd4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardValue.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardValue.h
@@ -1,34 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
-#include <wpi/SmallVector.h>
-#include <wpi/StringRef.h>
 
 namespace frc {
 
 class ShuffleboardValue {
  public:
-  explicit ShuffleboardValue(const wpi::Twine& title) {
-    wpi::SmallVector<char, 16> storage;
-    m_title = title.toStringRef(storage);
-  }
+  explicit ShuffleboardValue(std::string_view title) : m_title(title) {}
 
   virtual ~ShuffleboardValue() = default;
 
+  ShuffleboardValue(const ShuffleboardValue&) = delete;
+  ShuffleboardValue& operator=(const ShuffleboardValue&) = delete;
+
   /**
    * Gets the title of this Shuffleboard value.
    */
-  wpi::StringRef GetTitle() const { return m_title; }
+  const std::string& GetTitle() const { return m_title; }
 
   /**
    * Builds the entries for this value.
@@ -51,7 +47,7 @@
    *
    * This method is package-private to prevent users from enabling control
    * themselves. Has no effect if the sendable is not marked as an actuator with
-   * {@link SendableBuilder#setActuator}.
+   * SendableBuilder::SetActuator().
    */
   virtual void EnableIfActuator() {}
 
@@ -60,7 +56,7 @@
    *
    * This method is package-private to prevent users from enabling control
    * themselves. Has no effect if the sendable is not marked as an actuator with
-   * {@link SendableBuilder#setActuator}.
+   * SendableBuilder::SetActuator().
    */
   virtual void DisableIfActuator() {}
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
index 729da40..ce01367 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/ShuffleboardWidget.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/Twine.h>
+#include <string_view>
 
 #include "frc/shuffleboard/BuiltInWidgets.h"
 #include "frc/shuffleboard/ShuffleboardComponent.h"
@@ -31,7 +28,7 @@
 template <typename Derived>
 class ShuffleboardWidget : public ShuffleboardComponent<Derived> {
  public:
-  ShuffleboardWidget(ShuffleboardContainer& parent, const wpi::Twine& title)
+  ShuffleboardWidget(ShuffleboardContainer& parent, std::string_view title)
       : ShuffleboardValue(title),
         ShuffleboardComponent<Derived>(parent, title) {}
 
@@ -63,13 +60,12 @@
    * 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 third-party plugin). To use a widget that is built into
-   * Shuffleboard, use {@link #withWidget(WidgetType)} and {@link
-   * BuiltInWidgets}.
+   * Shuffleboard, use WithWidget(WidgetType) and BuiltInWidgets.
    *
    * @param widgetType the type of the widget used to display the data
    * @return this widget object
    */
-  Derived& WithWidget(const wpi::Twine& widgetType) {
+  Derived& WithWidget(std::string_view widgetType) {
     this->SetType(widgetType);
     return *static_cast<Derived*>(this);
   }
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SimpleWidget.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SimpleWidget.h
index 973f8cb..746b7d6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SimpleWidget.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SimpleWidget.h
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
-#include <wpi/Twine.h>
 
 #include "frc/shuffleboard/ShuffleboardWidget.h"
 
@@ -25,7 +22,7 @@
  */
 class SimpleWidget final : public ShuffleboardWidget<SimpleWidget> {
  public:
-  SimpleWidget(ShuffleboardContainer& parent, const wpi::Twine& title);
+  SimpleWidget(ShuffleboardContainer& parent, std::string_view title);
 
   /**
    * Gets the NetworkTable entry that contains the data for this widget.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h
index c806db4..a2b042e 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/SuppliedValueWidget.h
@@ -1,34 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <memory>
+#include <string_view>
 
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
-#include <wpi/Twine.h>
 
 #include "frc/shuffleboard/ShuffleboardComponent.h"
 #include "frc/shuffleboard/ShuffleboardComponent.inc"
 #include "frc/shuffleboard/ShuffleboardComponentBase.h"
-#include "frc/shuffleboard/ShuffleboardContainer.h"
 #include "frc/shuffleboard/ShuffleboardWidget.h"
 
 namespace frc {
+class ShuffleboardContainer;
+
 template <typename T>
-class SuppliedValueWidget : public ShuffleboardWidget<SuppliedValueWidget<T> > {
+class SuppliedValueWidget : public ShuffleboardWidget<SuppliedValueWidget<T>> {
  public:
-  SuppliedValueWidget(ShuffleboardContainer& parent, const wpi::Twine& title,
+  SuppliedValueWidget(ShuffleboardContainer& parent, std::string_view title,
                       std::function<T()> supplier,
                       std::function<void(nt::NetworkTableEntry, T)> setter)
       : ShuffleboardValue(title),
-        ShuffleboardWidget<SuppliedValueWidget<T> >(parent, title),
+        ShuffleboardWidget<SuppliedValueWidget<T>>(parent, title),
         m_supplier(supplier),
         m_setter(setter) {}
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h
index ff92fe7..13805bb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/shuffleboard/WidgetType.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
 namespace frc {
 
@@ -28,7 +25,7 @@
    * Gets the string type of the widget as defined by that widget in
    * Shuffleboard.
    */
-  wpi::StringRef GetWidgetName() const;
+  std::string_view GetWidgetName() const;
 
  private:
   const char* m_widgetName;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h
new file mode 100644
index 0000000..af96b39
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+namespace frc {
+
+class ADXL345_SPI;
+class ADXL345_I2C;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXL345Sim {
+ public:
+  /**
+   * Constructs from a ADXL345_I2C object.
+   *
+   * @param accel ADXL345 accel to simulate
+   */
+  explicit ADXL345Sim(const ADXL345_I2C& accel);
+
+  /**
+   * Constructs from a ADXL345_SPI object.
+   *
+   * @param accel ADXL345 accel to simulate
+   */
+  explicit ADXL345Sim(const ADXL345_SPI& accel);
+
+  /**
+   * Sets the X acceleration.
+   *
+   * @param accel The X acceleration.
+   */
+  void SetX(double accel);
+
+  /**
+   * Sets the Y acceleration.
+   *
+   * @param accel The Y acceleration.
+   */
+  void SetY(double accel);
+
+  /**
+   * Sets the Z acceleration.
+   *
+   * @param accel The Z acceleration.
+   */
+  void SetZ(double accel);
+
+ private:
+  hal::SimDouble m_simX;
+  hal::SimDouble m_simY;
+  hal::SimDouble m_simZ;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h
new file mode 100644
index 0000000..269732b
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+namespace frc {
+
+class ADXL362;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class ADXL362Sim {
+ public:
+  /**
+   * Constructs from a ADXL362 object.
+   *
+   * @param accel ADXL362 accel to simulate
+   */
+  explicit ADXL362Sim(const ADXL362& accel);
+
+  /**
+   * Sets the X acceleration.
+   *
+   * @param accel The X acceleration.
+   */
+  void SetX(double accel);
+
+  /**
+   * Sets the Y acceleration.
+   *
+   * @param accel The Y acceleration.
+   */
+  void SetY(double accel);
+
+  /**
+   * Sets the Z acceleration.
+   *
+   * @param accel The Z acceleration.
+   */
+  void SetZ(double accel);
+
+ private:
+  hal::SimDouble m_simX;
+  hal::SimDouble m_simY;
+  hal::SimDouble m_simZ;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
index 1d2d2c3..4cb4737 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
index 0c578f4..4533086 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AddressableLEDSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -55,39 +52,131 @@
    */
   static AddressableLEDSim CreateForIndex(int index);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback on the Initialized property.
+   *
+   * @param callback the callback that will be called whenever the Initialized
+   *                 property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object storing this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Change the Initialized value of the LED strip.
+   *
+   * @param initialized the new value
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterOutputPortCallback(
+  /**
+   * Register a callback on the output port.
+   *
+   * @param callback the callback that will be called whenever the output port
+   *                 is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterOutputPortCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the output port.
+   *
+   * @return the output port
+   */
   int GetOutputPort() const;
 
+  /**
+   * Change the output port.
+   *
+   * @param outputPort the new output port
+   */
   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(
+  /**
+   * Register a callback on the length.
+   *
+   * @param callback the callback that will be called whenever the length is
+   *                 changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterLengthCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the length of the LED strip.
+   *
+   * @return the length
+   */
+  int GetLength() const;
+
+  /**
+   * Change the length of the LED strip.
+   *
+   * @param length the new value
+   */
+  void SetLength(int length);
+
+  /**
+   * Register a callback on whether the LEDs are running.
+   *
+   * @param callback the callback that will be called whenever the LED state is
+   *                 changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterRunningCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the LEDs are running.
+   *
+   * @return true if they are
+   */
   int GetRunning() const;
 
+  /**
+   * Change whether the LEDs are active.
+   *
+   * @param running the new value
+   */
   void SetRunning(bool running);
 
-  std::unique_ptr<CallbackStore> RegisterDataCallback(NotifyCallback callback,
-                                                      bool initialNotify);
+  /**
+   * Register a callback on the LED data.
+   *
+   * @param callback the callback that will be called whenever the LED data is
+   *                 changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterDataCallback(
+      ConstBufferCallback callback, bool initialNotify);
 
+  /**
+   * Get the LED data.
+   *
+   * @param data output parameter to fill with LED data
+   * @return the length of the LED data
+   */
   int GetData(struct HAL_AddressableLEDData* data) const;
 
+  /**
+   * Change the LED data.
+   *
+   * @param data the new data
+   * @param length the length of the LED data
+   */
   void SetData(struct HAL_AddressableLEDData* data, int length);
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
index aef979a..6d76326 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogEncoderSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -31,7 +28,7 @@
   explicit AnalogEncoderSim(const AnalogEncoder& encoder);
 
   /**
-   * Set the position using an {@link Rotation2d}.
+   * Set the position using an Rotation2d.
    *
    * @param angle The angle.
    */
@@ -50,7 +47,7 @@
   units::turn_t GetTurns();
 
   /**
-   * Get the position as a {@link Rotation2d}.
+   * Get the position as a Rotation2d.
    */
   Rotation2d GetPosition();
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
index 685e3ed..faa11d7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogGyroSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,27 +33,82 @@
    */
   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(
+  /**
+   * Register a callback on the angle.
+   *
+   * @param callback the callback that will be called whenever the angle changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterAngleCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the current angle of the gyro.
+   *
+   * @return the angle measured by the gyro
+   */
+  double GetAngle() const;
+
+  /**
+   * Change the angle measured by the gyro.
+   *
+   * @param angle the new value
+   */
+  void SetAngle(double angle);
+
+  /**
+   * Register a callback on the rate.
+   *
+   * @param callback the callback that will be called whenever the rate changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterRateCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Get the rate of angle change on this gyro.
+   *
+   * @return the rate
+   */
+  double GetRate() const;
+
+  /**
+   * Change the rate of the gyro.
+   *
+   * @param rate the new rate
+   */
+  void SetRate(double rate);
+
+  /**
+   * Register a callback on whether the gyro is initialized.
+   *
+   * @param callback the callback that will be called whenever the gyro is
+   *                 initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the gyro is initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Set whether this gyro is initialized.
+   *
+   * @param initialized the new value
+   */
   void SetInitialized(bool initialized);
 
+  /**
+   * Reset all simulation data for this object.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
index a52cec4..03d7548 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogInputSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,69 +33,237 @@
    */
   explicit AnalogInputSim(int channel);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback on whether the analog input is initialized.
+   *
+   * @param callback the callback that will be called whenever the analog input
+   *                 is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if this analog input has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Change whether this analog input has been initialized.
+   *
+   * @param initialized the new value
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
+  /**
+   * Register a callback on the number of average bits.
+   *
+   * @param callback the callback that will be called whenever the number of
+   *                 average bits is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterAverageBitsCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the number of average bits.
+   *
+   * @return the number of average bits
+   */
   int GetAverageBits() const;
 
+  /**
+   * Change the number of average bits.
+   *
+   * @param averageBits the new value
+   */
   void SetAverageBits(int averageBits);
 
-  std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
+  /**
+   * Register a callback on the amount of oversampling bits.
+   *
+   * @param callback the callback that will be called whenever the oversampling
+   *                 bits are changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterOversampleBitsCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the amount of oversampling bits.
+   *
+   * @return the amount of oversampling bits
+   */
   int GetOversampleBits() const;
 
+  /**
+   * Change the amount of oversampling bits.
+   *
+   * @param oversampleBits the new value
+   */
   void SetOversampleBits(int oversampleBits);
 
-  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+  /**
+   * Register a callback on the voltage.
+   *
+   * @param callback the callback that will be called whenever the voltage is
+   *                 changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterVoltageCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the voltage.
+   *
+   * @return the voltage
+   */
   double GetVoltage() const;
 
+  /**
+   * Change the voltage.
+   *
+   * @param voltage the new value
+   */
   void SetVoltage(double voltage);
 
-  std::unique_ptr<CallbackStore> RegisterAccumulatorInitializedCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on whether the accumulator is initialized.
+   *
+   * @param callback the callback that will be called whenever the accumulator
+   *                 is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterAccumulatorInitializedCallback(NotifyCallback callback,
+                                         bool initialNotify);
 
+  /**
+   * Check if the accumulator has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetAccumulatorInitialized() const;
 
+  /**
+   * Change whether the accumulator has been initialized.
+   *
+   * @param accumulatorInitialized the new value
+   */
   void SetAccumulatorInitialized(bool accumulatorInitialized);
 
-  std::unique_ptr<CallbackStore> RegisterAccumulatorValueCallback(
+  /**
+   * Register a callback on the accumulator value.
+   *
+   * @param callback the callback that will be called whenever the accumulator
+   *                 value is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterAccumulatorValueCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the accumulator value.
+   *
+   * @return the accumulator value
+   */
   int64_t GetAccumulatorValue() const;
 
+  /**
+   * Change the accumulator value.
+   *
+   * @param accumulatorValue the new value
+   */
   void SetAccumulatorValue(int64_t accumulatorValue);
 
-  std::unique_ptr<CallbackStore> RegisterAccumulatorCountCallback(
+  /**
+   * Register a callback on the accumulator count.
+   *
+   * @param callback the callback that will be called whenever the accumulator
+   *                 count is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterAccumulatorCountCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the accumulator count.
+   *
+   * @return the accumulator count.
+   */
   int64_t GetAccumulatorCount() const;
 
+  /**
+   * Change the accumulator count.
+   *
+   * @param accumulatorCount the new count.
+   */
   void SetAccumulatorCount(int64_t accumulatorCount);
 
-  std::unique_ptr<CallbackStore> RegisterAccumulatorCenterCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on the accumulator center.
+   *
+   * @param callback the callback that will be called whenever the accumulator
+   *                 center is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterAccumulatorCenterCallback(NotifyCallback callback,
+                                    bool initialNotify);
 
+  /**
+   * Get the accumulator center.
+   *
+   * @return the accumulator center
+   */
   int GetAccumulatorCenter() const;
 
+  /**
+   * Change the accumulator center.
+   *
+   * @param accumulatorCenter the new center
+   */
   void SetAccumulatorCenter(int accumulatorCenter);
 
-  std::unique_ptr<CallbackStore> RegisterAccumulatorDeadbandCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on the accumulator deadband.
+   *
+   * @param callback the callback that will be called whenever the accumulator
+   *                 deadband is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterAccumulatorDeadbandCallback(NotifyCallback callback,
+                                      bool initialNotify);
 
+  /**
+   * Get the accumulator deadband.
+   *
+   * @return the accumulator deadband
+   */
   int GetAccumulatorDeadband() const;
 
+  /**
+   * Change the accumulator deadband.
+   *
+   * @param accumulatorDeadband the new deadband
+   */
   void SetAccumulatorDeadband(int accumulatorDeadband);
 
+  /**
+   * Reset all simulation data for this object.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
index e468fa5..ffec03a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogOutputSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,20 +33,57 @@
    */
   explicit AnalogOutputSim(int channel);
 
-  std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+  /**
+   * Register a callback to be run whenever the voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterVoltageCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the analog output voltage.
+   *
+   * @return the voltage on this analog output
+   */
   double GetVoltage() const;
 
+  /**
+   * Set the analog output voltage.
+   *
+   * @param voltage the new voltage on this analog output
+   */
   void SetVoltage(double voltage);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback to be run when this analog output is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether this analog output has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Define whether this analog output has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   void SetInitialized(bool initialized);
 
+  /**
+   * Reset all simulation data on this object.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
index 60d62e1..04e9e9b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/AnalogTriggerSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -48,27 +45,86 @@
    */
   static AnalogTriggerSim CreateForIndex(int index);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback on whether the analog trigger is initialized.
+   *
+   * @param callback the callback that will be called whenever the analog
+   *                 trigger is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if this analog trigger has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Change whether this analog trigger has been initialized.
+   *
+   * @param initialized the new value
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterTriggerLowerBoundCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on the lower bound.
+   *
+   * @param callback the callback that will be called whenever the lower bound
+   *                 is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterTriggerLowerBoundCallback(NotifyCallback callback,
+                                    bool initialNotify);
 
+  /**
+   * Get the lower bound.
+   *
+   * @return the lower bound
+   */
   double GetTriggerLowerBound() const;
 
+  /**
+   * Change the lower bound.
+   *
+   * @param triggerLowerBound the new lower bound
+   */
   void SetTriggerLowerBound(double triggerLowerBound);
 
-  std::unique_ptr<CallbackStore> RegisterTriggerUpperBoundCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on the upper bound.
+   *
+   * @param callback the callback that will be called whenever the upper bound
+   *                 is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterTriggerUpperBoundCallback(NotifyCallback callback,
+                                    bool initialNotify);
 
+  /**
+   * Get the upper bound.
+   *
+   * @return the upper bound
+   */
   double GetTriggerUpperBound() const;
 
+  /**
+   * Change the upper bound.
+   *
+   * @param triggerUpperBound the new upper bound
+   */
   void SetTriggerUpperBound(double triggerUpperBound);
 
+  /**
+   * Reset all simulation data for this object.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BatterySim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
index ef8fb5d..4a350cf 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BatterySim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,13 +12,16 @@
 
 namespace frc::sim {
 
+/**
+ * A utility class to simulate the robot battery.
+ */
 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.
+   * RoboRioSim::SetVInVoltage(double) to set the simulated battery voltage,
+   * which can then be retrieved with the RobotController::GetBatteryVoltage()
+   * method.
    *
    * @param nominalVoltage The nominal battery voltage. Usually 12v.
    * @param resistance     The forward resistance of the battery. Most batteries
@@ -38,10 +38,10 @@
 
   /**
    * 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)
+   * RoboRioSimSetVInVoltage(double) to set the simulated battery voltage, which
+   * can then be retrieved with the 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.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
index 49a581c..9ffcf5b 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/BuiltInAccelerometerSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,41 +33,129 @@
    */
   explicit BuiltInAccelerometerSim(const BuiltInAccelerometer& accel);
 
-  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
-                                                        bool initialNotify);
+  /**
+   * Register a callback to be run when this accelerometer activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterActiveCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether the accelerometer is active.
+   *
+   * @return true if active
+   */
   bool GetActive() const;
 
+  /**
+   * Define whether this accelerometer is active.
+   *
+   * @param active the new state
+   */
   void SetActive(bool active);
 
-  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
-                                                       bool initialNotify);
+  /**
+   * Register a callback to be run whenever the range changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterRangeCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check the range of this accelerometer.
+   *
+   * @return the accelerometer range
+   */
   HAL_AccelerometerRange GetRange() const;
 
+  /**
+   * Change the range of this accelerometer.
+   *
+   * @param range the new accelerometer range
+   */
   void SetRange(HAL_AccelerometerRange range);
 
-  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
-                                                   bool initialNotify);
+  /**
+   * Register a callback to be run whenever the X axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterXCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the X axis value.
+   *
+   * @return the X axis measurement
+   */
   double GetX() const;
 
+  /**
+   * Change the X axis value of the accelerometer.
+   *
+   * @param x the new reading of the X axis
+   */
   void SetX(double x);
 
-  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
-                                                   bool initialNotify);
+  /**
+   * Register a callback to be run whenever the Y axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterYCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the Y axis value.
+   *
+   * @return the Y axis measurement
+   */
   double GetY() const;
 
+  /**
+   * Change the Y axis value of the accelerometer.
+   *
+   * @param y the new reading of the Y axis
+   */
   void SetY(double y);
 
-  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
-                                                   bool initialNotify);
+  /**
+   * Register a callback to be run whenever the Z axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterZCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the Z axis value.
+   *
+   * @return the Z axis measurement
+   */
   double GetZ() const;
 
+  /**
+   * Change the Z axis value of the accelerometer.
+   *
+   * @param z the new reading of the Z axis
+   */
   void SetZ(double z);
 
+  /**
+   * Reset all simulation data of this object.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CTREPCMSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CTREPCMSim.h
new file mode 100644
index 0000000..0929308
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CTREPCMSim.h
@@ -0,0 +1,212 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Compressor;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class CTREPCMSim {
+ public:
+  /**
+   * Constructs with the default PCM module number (CAN ID).
+   */
+  CTREPCMSim();
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit CTREPCMSim(int module);
+
+  explicit CTREPCMSim(const PneumaticsBase& pneumatics);
+
+  /**
+   * Register a callback to be run when a solenoid is initialized on a channel.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if a solenoid has been initialized on a specific channel.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether a solenoid has been initialized on a specific channel.
+   *
+   * @param solenoidInitialized is there a solenoid initialized on that channel
+   */
+  void SetInitialized(bool solenoidInitialized);
+
+  /**
+   * Register a callback to be run when the solenoid output on a channel
+   * changes.
+   *
+   * @param channel the channel to monitor
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @return the solenoid output
+   */
+  bool GetSolenoidOutput(int channel) const;
+
+  /**
+   * Change the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @param solenoidOutput the new solenoid output
+   */
+  void SetSolenoidOutput(int channel, bool solenoidOutput);
+
+  /**
+   * Register a callback to be run when the compressor activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  bool GetCompressorOn() const;
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  void SetCompressorOn(bool compressorOn);
+
+  /**
+   * Register a callback to be run whenever the closed loop state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterClosedLoopEnabledCallback(NotifyCallback callback,
+                                    bool initialNotify);
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return true if active
+   */
+  bool GetClosedLoopEnabled() const;
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param closedLoopEnabled whether the control loop is active
+   */
+  void SetClosedLoopEnabled(bool closedLoopEnabled);
+
+  /**
+   * Register a callback to be run whenever the pressure switch value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  bool GetPressureSwitch() const;
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  void SetPressureSwitch(bool pressureSwitch);
+
+  /**
+   * Register a callback to be run whenever the compressor current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterCompressorCurrentCallback(NotifyCallback callback,
+                                    bool initialNotify);
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  double GetCompressorCurrent() const;
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  void SetCompressorCurrent(double compressorCurrent);
+
+  /**
+   * Get the current value of all solenoid outputs.
+   *
+   * @return the solenoid outputs (1 bit per output)
+   */
+  uint8_t GetAllSolenoidOutputs() const;
+
+  /**
+   * Change all of the solenoid outputs.
+   *
+   * @param outputs the new solenoid outputs (1 bit per output)
+   */
+  void SetAllSolenoidOutputs(uint8_t outputs);
+
+  /**
+   * Reset all simulation data for this object.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
index 379296a..61cc6b2 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/CallbackStore.h
@@ -1,33 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
+#include <string_view>
 
 #include <hal/Value.h>
-#include <wpi/StringRef.h>
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 
-using NotifyCallback = std::function<void(wpi::StringRef, const HAL_Value*)>;
+using NotifyCallback = std::function<void(std::string_view, 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);
+    std::string_view, const unsigned char* buffer, unsigned int count)>;
+using CancelCallbackFunc = void (*)(int32_t index, int32_t uid);
+using CancelCallbackNoIndexFunc = void (*)(int32_t uid);
+using CancelCallbackChannelFunc = void (*)(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);
 
+/**
+ * Manages simulation callbacks; each object is associated with a callback.
+ */
 class CallbackStore {
  public:
   CallbackStore(int32_t i, NotifyCallback cb, CancelCallbackNoIndexFunc ccf);
@@ -76,5 +75,4 @@
   enum CancelType { Normal, Channel, NoIndex };
   CancelType cancelType;
 };
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DIOSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
index f6edb39..9bbd3fb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DIOSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -44,41 +41,132 @@
    */
   explicit DIOSim(int channel);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback to be run when this DIO is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether this DIO has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Define whether this DIO has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterValueCallback(NotifyCallback callback,
-                                                       bool initialNotify);
+  /**
+   * Register a callback to be run whenever the DIO value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterValueCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the value of the DIO port.
+   *
+   * @return the DIO value
+   */
   bool GetValue() const;
 
+  /**
+   * Change the DIO value.
+   *
+   * @param value the new value
+   */
   void SetValue(bool value);
 
-  std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
+  /**
+   * Register a callback to be run whenever the pulse length changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPulseLengthCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the pulse length.
+   *
+   * @return the pulse length of this DIO port
+   */
   double GetPulseLength() const;
 
+  /**
+   * Change the pulse length of this DIO port.
+   *
+   * @param pulseLength the new pulse length
+   */
   void SetPulseLength(double pulseLength);
 
-  std::unique_ptr<CallbackStore> RegisterIsInputCallback(
+  /**
+   * Register a callback to be run whenever this DIO changes to be an input.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterIsInputCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether this DIO port is currently an Input.
+   *
+   * @return true if Input
+   */
   bool GetIsInput() const;
 
+  /**
+   * Define whether this DIO port is an Input.
+   *
+   * @param isInput whether this DIO should be an Input
+   */
   void SetIsInput(bool isInput);
 
-  std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
+  /**
+   * Register a callback to be run whenever the filter index changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterFilterIndexCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the filter index.
+   *
+   * @return the filter index of this DIO port
+   */
   int GetFilterIndex() const;
 
+  /**
+   * Change the filter index of this DIO port.
+   *
+   * @param filterIndex the new filter index
+   */
   void SetFilterIndex(int filterIndex);
 
+  /**
+   * Reset all simulation data of this object.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
index a75edb9..e64a652 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DifferentialDrivetrainSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,42 +21,66 @@
   /**
    * 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.
+   * @param plant The LinearSystem representing the robot's drivetrain. This
+   *              system can be created with
+   *              LinearSystemId::DrivetrainVelocitySystem() or
+   *              LinearSystemId::IdentifyDrivetrainSystem().
+   * @param trackWidth   The robot's track width.
+   * @param driveMotor   A 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 plant.
+   * @param wheelRadius  The radius of the wheels on the drivetrain, in meters.
+   * @param measurementStdDevs Standard deviations for measurements, in the form
+   *                           [x, y, heading, left velocity, right velocity,
+   *                           left distance, right distance]ᵀ. Can be omitted
+   *                           if no noise is desired. Gyro standard deviations
+   *                           of 0.0001 radians, velocity standard deviations
+   *                           of 0.05 m/s, and position measurement standard
+   *                           deviations of 0.005 meters are a reasonable
+   *                           starting point.
    */
-  DifferentialDrivetrainSim(const LinearSystem<2, 2, 2>& plant,
-                            units::meter_t trackWidth, DCMotor driveMotor,
-                            double gearingRatio, units::meter_t wheelRadius);
+  DifferentialDrivetrainSim(
+      LinearSystem<2, 2, 2> plant, units::meter_t trackWidth,
+      DCMotor driveMotor, double gearingRatio, units::meter_t wheelRadius,
+      const std::array<double, 7>& measurementStdDevs = {});
 
   /**
    * Create a SimDrivetrain.
    *
-   * @param driveMotor  A {@link DCMotor} representing the left side of the
-   * drivetrain.
+   * @param driveMotor  A 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.
+   *                    output over input. This must be the same ratio as the
+   *                    ratio used to identify or create the plant.
    * @param J           The moment of inertia of the drivetrain about its
-   * center.
+   *                    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.
+   *                    right wheels.
+   * @param measurementStdDevs Standard deviations for measurements, in the form
+   *                           [x, y, heading, left velocity, right velocity,
+   *                           left distance, right distance]ᵀ. Can be omitted
+   *                           if no noise is desired. Gyro standard deviations
+   *                           of 0.0001 radians, velocity standard deviations
+   *                           of 0.05 m/s, and position measurement standard
+   *                           deviations of 0.005 meters are a reasonable
+   *                           starting point.
    */
-  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::DCMotor driveMotor, double gearing, units::kilogram_square_meter_t J,
+      units::kilogram_t mass, units::meter_t wheelRadius,
+      units::meter_t trackWidth,
+      const std::array<double, 7>& measurementStdDevs = {});
+
+  /**
+   * Clamp the input vector such that no element exceeds the battery voltage.
+   * If any does, the relative magnitudes of the input will be maintained.
+   *
+   * @param u The input vector.
+   * @return The normalized input.
+   */
+  Eigen::Vector<double, 2> ClampInput(const Eigen::Vector<double, 2>& u);
 
   /**
    * Sets the applied voltage to the drivetrain. Note that positive voltage must
@@ -81,30 +102,18 @@
   /**
    * Updates the simulation.
    *
-   * @param dt The time that's passed since the last {@link #update(double)}
-   * call.
+   * @param dt The time that's passed since the last Update(units::second_t)
+   *           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
@@ -118,6 +127,48 @@
   Pose2d GetPose() const;
 
   /**
+   * Get the right encoder position in meters.
+   * @return The encoder position.
+   */
+  units::meter_t GetRightPosition() const {
+    return units::meter_t{GetOutput(State::kRightPosition)};
+  }
+
+  /**
+   * Get the right encoder velocity in meters per second.
+   * @return The encoder velocity.
+   */
+  units::meters_per_second_t GetRightVelocity() const {
+    return units::meters_per_second_t{GetOutput(State::kRightVelocity)};
+  }
+
+  /**
+   * Get the left encoder position in meters.
+   * @return The encoder position.
+   */
+  units::meter_t GetLeftPosition() const {
+    return units::meter_t{GetOutput(State::kLeftPosition)};
+  }
+
+  /**
+   * Get the left encoder velocity in meters per second.
+   * @return The encoder velocity.
+   */
+  units::meters_per_second_t GetLeftVelocity() const {
+    return units::meters_per_second_t{GetOutput(State::kLeftVelocity)};
+  }
+
+  /**
+   * Returns the currently drawn current for the right side.
+   */
+  units::ampere_t GetRightCurrentDraw() const;
+
+  /**
+   * Returns the currently drawn current for the left side.
+   */
+  units::ampere_t GetLeftCurrentDraw() const;
+
+  /**
    * Returns the currently drawn current.
    */
   units::ampere_t GetCurrentDraw() const;
@@ -127,7 +178,7 @@
    *
    * @param state The state.
    */
-  void SetState(const Eigen::Matrix<double, 7, 1>& state);
+  void SetState(const Eigen::Vector<double, 7>& state);
 
   /**
    * Sets the system pose.
@@ -136,8 +187,8 @@
    */
   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);
+  Eigen::Vector<double, 7> Dynamics(const Eigen::Vector<double, 7>& x,
+                                    const Eigen::Vector<double, 2>& u);
 
   class State {
    public:
@@ -189,10 +240,16 @@
    * @param motor     The motors installed in the bot.
    * @param gearing   The gearing reduction used.
    * @param wheelSize The wheel size.
+   * @param measurementStdDevs Standard deviations for measurements, in the form
+   * [x, y, heading, left velocity, right velocity, left distance, right
+   * distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
+   * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
+   * position measurement standard deviations of 0.005 meters are a reasonable
+   * starting point.
    */
-  static DifferentialDrivetrainSim CreateKitbotSim(frc::DCMotor motor,
-                                                   double gearing,
-                                                   units::meter_t wheelSize) {
+  static DifferentialDrivetrainSim CreateKitbotSim(
+      frc::DCMotor motor, double gearing, units::meter_t wheelSize,
+      const std::array<double, 7>& measurementStdDevs = {}) {
     // 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) *
@@ -200,7 +257,8 @@
                                                 * (26_in / 2) * (26_in / 2);
 
     return DifferentialDrivetrainSim{
-        motor, gearing, batteryMoi + gearboxMoi, 25_kg, wheelSize / 2.0, 26_in};
+        motor,           gearing, batteryMoi + gearboxMoi, 60_lb,
+        wheelSize / 2.0, 26_in,   measurementStdDevs};
   }
 
   /**
@@ -210,16 +268,48 @@
    * @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.
+   * calculated using SysId.
+   * @param measurementStdDevs Standard deviations for measurements, in the form
+   * [x, y, heading, left velocity, right velocity, left distance, right
+   * distance]ᵀ. Can be omitted if no noise is desired. Gyro standard
+   * deviations of 0.0001 radians, velocity standard deviations of 0.05 m/s, and
+   * position measurement standard deviations of 0.005 meters are a reasonable
+   * starting point.
    */
   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};
+      units::kilogram_square_meter_t J,
+      const std::array<double, 7>& measurementStdDevs = {}) {
+    return DifferentialDrivetrainSim{
+        motor, gearing, J, 60_lb, wheelSize / 2.0, 26_in, measurementStdDevs};
   }
 
  private:
+  /**
+   * Returns an element of the state vector.
+   *
+   * @param output The row of the output vector.
+   */
+  double GetOutput(int output) const;
+
+  /**
+   * Returns the current output vector y.
+   */
+  Eigen::Vector<double, 7> GetOutput() const;
+
+  /**
+   * Returns an element of the state vector. Note that this will not include
+   * noise!
+   *
+   * @param output The row of the output vector.
+   */
+  double GetState(int state) const;
+
+  /**
+   * Returns the current state vector x. Note that this will not include noise!
+   */
+  Eigen::Vector<double, 7> GetState() const;
+
   LinearSystem<2, 2, 2> m_plant;
   units::meter_t m_rb;
   units::meter_t m_wheelRadius;
@@ -229,7 +319,9 @@
   double m_originalGearing;
   double m_currentGearing;
 
-  Eigen::Matrix<double, 7, 1> m_x;
-  Eigen::Matrix<double, 2, 1> m_u;
+  Eigen::Vector<double, 7> m_x;
+  Eigen::Vector<double, 2> m_u;
+  Eigen::Vector<double, 7> m_y;
+  std::array<double, 7> m_measurementStdDevs;
 };
 }  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
index 2ff8a12..53caee7 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DigitalPWMSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -50,27 +47,81 @@
    */
   static DigitalPWMSim CreateForIndex(int index);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback to be run when this PWM output is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether this PWM output has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Define whether this PWM output has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
+  /**
+   * Register a callback to be run whenever the duty cycle value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterDutyCycleCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the duty cycle value.
+   *
+   * @return the duty cycle value of this PWM output
+   */
   double GetDutyCycle() const;
 
+  /**
+   * Set the duty cycle value of this PWM output.
+   *
+   * @param dutyCycle the new value
+   */
   void SetDutyCycle(double dutyCycle);
 
-  std::unique_ptr<CallbackStore> RegisterPinCallback(NotifyCallback callback,
-                                                     bool initialNotify);
+  /**
+   * Register a callback to be run whenever the pin changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPinCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check the pin number.
+   *
+   * @return the pin number
+   */
   int GetPin() const;
 
+  /**
+   * Change the pin number.
+   *
+   * @param pin the new pin number
+   */
   void SetPin(int pin);
 
+  /**
+   * Reset all simulation data.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
index 80cf942..412a921 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DriverStationSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,68 +11,212 @@
 #include "frc/DriverStation.h"
 #include "frc/simulation/CallbackStore.h"
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 
 /**
  * Class to control a simulated driver station.
  */
 class DriverStationSim {
  public:
-  static std::unique_ptr<CallbackStore> RegisterEnabledCallback(
+  /**
+   * Register a callback on whether the DS is enabled.
+   *
+   * @param callback the callback that will be called whenever the enabled
+   *                 state is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore> RegisterEnabledCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if the DS is enabled.
+   *
+   * @return true if enabled
+   */
   static bool GetEnabled();
 
+  /**
+   * Change whether the DS is enabled.
+   *
+   * @param enabled the new value
+   */
   static void SetEnabled(bool enabled);
 
-  static std::unique_ptr<CallbackStore> RegisterAutonomousCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on whether the DS is in autonomous mode.
+   *
+   * @param callback the callback that will be called on autonomous mode
+   *                 entrance/exit
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterAutonomousCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if the DS is in autonomous.
+   *
+   * @return true if autonomous
+   */
   static bool GetAutonomous();
 
+  /**
+   * Change whether the DS is in autonomous.
+   *
+   * @param autonomous the new value
+   */
   static void SetAutonomous(bool autonomous);
 
-  static std::unique_ptr<CallbackStore> RegisterTestCallback(
+  /**
+   * Register a callback on whether the DS is in test mode.
+   *
+   * @param callback the callback that will be called whenever the test mode
+   *                 is entered or left
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore> RegisterTestCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if the DS is in test.
+   *
+   * @return true if test
+   */
   static bool GetTest();
 
+  /**
+   * Change whether the DS is in test.
+   *
+   * @param test the new value
+   */
   static void SetTest(bool test);
 
-  static std::unique_ptr<CallbackStore> RegisterEStopCallback(
+  /**
+   * Register a callback on the eStop state.
+   *
+   * @param callback the callback that will be called whenever the eStop state
+   *                 changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore> RegisterEStopCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if eStop has been activated.
+   *
+   * @return true if eStopped
+   */
   static bool GetEStop();
 
+  /**
+   * Set whether eStop is active.
+   *
+   * @param eStop true to activate
+   */
   static void SetEStop(bool eStop);
 
-  static std::unique_ptr<CallbackStore> RegisterFmsAttachedCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on whether the FMS is connected.
+   *
+   * @param callback the callback that will be called whenever the FMS
+   *                 connection changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterFmsAttachedCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if the FMS is connected.
+   *
+   * @return true if FMS is connected
+   */
   static bool GetFmsAttached();
 
+  /**
+   * Change whether the FMS is connected.
+   *
+   * @param fmsAttached the new value
+   */
   static void SetFmsAttached(bool fmsAttached);
 
-  static std::unique_ptr<CallbackStore> RegisterDsAttachedCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on whether the DS is connected.
+   *
+   * @param callback the callback that will be called whenever the DS
+   *                 connection changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterDsAttachedCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if the DS is attached.
+   *
+   * @return true if attached
+   */
   static bool GetDsAttached();
 
+  /**
+   * Change whether the DS is attached.
+   *
+   * @param dsAttached the new value
+   */
   static void SetDsAttached(bool dsAttached);
 
-  static std::unique_ptr<CallbackStore> RegisterAllianceStationIdCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback on the alliance station ID.
+   *
+   * @param callback the callback that will be called whenever the alliance
+   *                 station changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterAllianceStationIdCallback(NotifyCallback callback,
+                                    bool initialNotify);
 
+  /**
+   * Get the alliance station ID (color + number).
+   *
+   * @return the alliance station color and number
+   */
   static HAL_AllianceStationID GetAllianceStationId();
 
+  /**
+   * Change the alliance station.
+   *
+   * @param allianceStationId the new alliance station
+   */
   static void SetAllianceStationId(HAL_AllianceStationID allianceStationId);
 
-  static std::unique_ptr<CallbackStore> RegisterMatchTimeCallback(
+  /**
+   * Register a callback on match time.
+   *
+   * @param callback the callback that will be called whenever match time
+   *                 changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore> RegisterMatchTimeCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the current value of the match timer.
+   *
+   * @return the current match time
+   */
   static double GetMatchTime();
 
+  /**
+   * Sets the match timer.
+   *
+   * @param matchTime the new match time
+   */
   static void SetMatchTime(double matchTime);
 
   /**
@@ -116,7 +257,7 @@
   static int GetJoystickRumble(int stick, int rumbleNum);
 
   /**
-   * Sets the state of one joystick button. Button indexes begin at 1.
+   * 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
@@ -242,7 +383,9 @@
    */
   static void SetReplayNumber(int replayNumber);
 
+  /**
+   * Reset all simulation data for the Driver Station.
+   */
   static void ResetData();
 };
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
index 5d9a039..19577bb 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleEncoderSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -24,7 +21,7 @@
   /**
    * Constructs from a DutyCycleEncoder object.
    *
-   * @param dutyCycleEncoder DutyCycleEncoder to simulate
+   * @param encoder DutyCycleEncoder to simulate
    */
   explicit DutyCycleEncoderSim(const DutyCycleEncoder& encoder);
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
index 40dad13..ec7b48c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/DutyCycleSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -47,27 +44,81 @@
    */
   static DutyCycleSim CreateForIndex(int index);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback to be run when this duty cycle input is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether this duty cycle input has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Define whether this duty cycle input has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
+  /**
+   * Register a callback to be run whenever the frequency changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterFrequencyCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the frequency.
+   *
+   * @return the duty cycle frequency
+   */
   int GetFrequency() const;
 
-  void SetFrequency(int count);
+  /**
+   * Change the duty cycle frequency.
+   *
+   * @param frequency the new frequency
+   */
+  void SetFrequency(int frequency);
 
-  std::unique_ptr<CallbackStore> RegisterOutputCallback(NotifyCallback callback,
-                                                        bool initialNotify);
+  /**
+   * Register a callback to be run whenever the output changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterOutputCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the output from this duty cycle port.
+   *
+   * @return the output value
+   */
   double GetOutput() const;
 
-  void SetOutput(double period);
+  /**
+   * Change the duty cycle output.
+   *
+   * @param output the new output value
+   */
+  void SetOutput(double output);
 
+  /**
+   * Reset all simulation data for the duty cycle output.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
index 9b7069f..14b6d2d 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/ElevatorSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -61,20 +58,34 @@
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**
+   * Returns whether the elevator would hit the lower limit.
+   *
+   * @param elevatorHeight The elevator height.
+   * @return Whether the elevator would hit the lower limit.
+   */
+  bool WouldHitLowerLimit(units::meter_t elevatorHeight) const;
+
+  /**
+   * Returns whether the elevator would hit the upper limit.
+   *
+   * @param elevatorHeight The elevator height.
+   * @return Whether the elevator would hit the upper limit.
+   */
+  bool WouldHitUpperLimit(units::meter_t elevatorHeight) const;
+
+  /**
    * 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;
+  bool HasHitLowerLimit() 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;
+  bool HasHitUpperLimit() const;
 
   /**
    * Returns the position of the elevator.
@@ -112,9 +123,9 @@
    * @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;
+  Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
+                                   const Eigen::Vector<double, 1>& u,
+                                   units::second_t dt) override;
 
  private:
   DCMotor m_gearbox;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
index 4049ab8..0d578b4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/EncoderSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -48,77 +45,261 @@
    */
   static EncoderSim CreateForIndex(int index);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback on the Initialized property of the encoder.
+   *
+   * @param callback the callback that will be called whenever the Initialized
+   *                 property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the Initialized value of the encoder.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Change the Initialized value of the encoder.
+   *
+   * @param initialized the new value
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterCountCallback(NotifyCallback callback,
-                                                       bool initialNotify);
+  /**
+   * Register a callback on the count property of the encoder.
+   *
+   * @param callback the callback that will be called whenever the count
+   *                 property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterCountCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the count of the encoder.
+   *
+   * @return the count
+   */
   int GetCount() const;
 
+  /**
+   * Change the count of the encoder.
+   *
+   * @param count the new count
+   */
   void SetCount(int count);
 
-  std::unique_ptr<CallbackStore> RegisterPeriodCallback(NotifyCallback callback,
-                                                        bool initialNotify);
+  /**
+   * Register a callback on the period of the encoder.
+   *
+   * @param callback the callback that will be called whenever the period is
+   *                 changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPeriodCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the period of the encoder.
+   *
+   * @return the encoder period
+   */
   double GetPeriod() const;
 
+  /**
+   * Change the encoder period.
+   *
+   * @param period the new period
+   */
   void SetPeriod(double period);
 
-  std::unique_ptr<CallbackStore> RegisterResetCallback(NotifyCallback callback,
-                                                       bool initialNotify);
+  /**
+   * Register a callback to be called whenever the encoder is reset.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterResetCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check if the encoder has been reset.
+   *
+   * @return true if reset
+   */
   bool GetReset() const;
 
+  /**
+   * Change the reset property of the encoder.
+   *
+   * @param reset the new value
+   */
   void SetReset(bool reset);
 
-  std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
+  /**
+   * Register a callback to be run whenever the max period of the encoder is
+   * changed.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterMaxPeriodCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the max period of the encoder.
+   *
+   * @return the max period of the encoder
+   */
   double GetMaxPeriod() const;
 
+  /**
+   * Change the max period of the encoder.
+   *
+   * @param maxPeriod the new value
+   */
   void SetMaxPeriod(double maxPeriod);
 
-  std::unique_ptr<CallbackStore> RegisterDirectionCallback(
+  /**
+   * Register a callback on the direction of the encoder.
+   *
+   * @param callback the callback that will be called whenever the direction
+   *                 is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterDirectionCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the direction of the encoder.
+   *
+   * @return the direction of the encoder
+   */
   bool GetDirection() const;
 
+  /**
+   * Set the direction of the encoder.
+   *
+   * @param direction the new direction
+   */
   void SetDirection(bool direction);
 
-  std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
+  /**
+   * Register a callback on the reverse direction.
+   *
+   * @param callback the callback that will be called whenever the reverse
+   *                 direction is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterReverseDirectionCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the reverse direction of the encoder.
+   *
+   * @return the reverse direction of the encoder
+   */
   bool GetReverseDirection() const;
 
+  /**
+   * Set the reverse direction.
+   *
+   * @param reverseDirection the new value
+   */
   void SetReverseDirection(bool reverseDirection);
 
-  std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
+  /**
+   * Register a callback on the samples-to-average value of this encoder.
+   *
+   * @param callback the callback that will be called whenever the
+   *                 samples-to-average is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterSamplesToAverageCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the samples-to-average value.
+   *
+   * @return the samples-to-average value
+   */
   int GetSamplesToAverage() const;
 
+  /**
+   * Set the samples-to-average value.
+   *
+   * @param samplesToAverage the new value
+   */
   void SetSamplesToAverage(int samplesToAverage);
 
-  std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
+  /**
+   * Register a callback on the distance per pulse value of this encoder.
+   *
+   * @param callback the callback that will be called whenever the
+   *                 distance per pulse is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterDistancePerPulseCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Read the distance per pulse of the encoder.
+   *
+   * @return the encoder distance per pulse
+   */
   double GetDistancePerPulse() const;
 
+  /**
+   * Change the encoder distance per pulse.
+   *
+   * @param distancePerPulse the new distance per pulse
+   */
   void SetDistancePerPulse(double distancePerPulse);
 
+  /**
+   * Resets all simulation data for this encoder.
+   */
   void ResetData();
 
+  /**
+   * Change the encoder distance.
+   *
+   * @param distance the new distance
+   */
   void SetDistance(double distance);
 
+  /**
+   * Read the distance of the encoder.
+   *
+   * @return the encoder distance
+   */
   double GetDistance();
 
+  /**
+   * Change the rate of the encoder.
+   *
+   * @param rate the new rate
+   */
   void SetRate(double rate);
 
+  /**
+   * Get the rate of the encoder.
+   *
+   * @return the rate of change
+   */
   double GetRate();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/Field2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/Field2d.h
deleted file mode 100644
index de30847..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/Field2d.h
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
index 1c47c45..9f2272c 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/FlywheelSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -21,7 +18,7 @@
 class FlywheelSim : public LinearSystemSim<1, 1, 1> {
  public:
   /**
-   * Creates a simulated flywhel mechanism.
+   * Creates a simulated flywheel mechanism.
    *
    * @param plant              The linear system representing the flywheel.
    * @param gearbox            The type of and number of motors in the flywheel
@@ -35,7 +32,7 @@
               const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**
-   * Creates a simulated flywhel mechanism.
+   * Creates a simulated flywheel mechanism.
    *
    * @param gearbox            The type of and number of motors in the flywheel
    *                           gearbox.
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
index 1705200..399e0c1 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/GenericHIDSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -41,30 +38,101 @@
    */
   void NotifyNewData();
 
+  /**
+   * Set the value of a given button.
+   *
+   * @param button the button to set
+   * @param value the new value
+   */
   void SetRawButton(int button, bool value);
 
+  /**
+   * Set the value of a given axis.
+   *
+   * @param axis the axis to set
+   * @param value the new value
+   */
   void SetRawAxis(int axis, double value);
 
+  /**
+   * Set the value of a given POV.
+   *
+   * @param pov the POV to set
+   * @param value the new value
+   */
   void SetPOV(int pov, int value);
 
+  /**
+   * Set the value of the default POV (port 0).
+   *
+   * @param value the new value
+   */
   void SetPOV(int value);
 
+  /**
+   * Set the axis count of this device.
+   *
+   * @param count the new axis count
+   */
   void SetAxisCount(int count);
 
+  /**
+   * Set the POV count of this device.
+   *
+   * @param count the new POV count
+   */
   void SetPOVCount(int count);
 
+  /**
+   * Set the button count of this device.
+   *
+   * @param count the new button count
+   */
   void SetButtonCount(int count);
 
+  /**
+   * Set the type of this device.
+   *
+   * @param type the new device type
+   */
   void SetType(GenericHID::HIDType type);
 
+  /**
+   * Set the name of this device.
+   *
+   * @param name the new device name
+   */
   void SetName(const char* name);
 
+  /**
+   * Set the type of an axis.
+   *
+   * @param axis the axis
+   * @param type the type
+   */
   void SetAxisType(int axis, int type);
 
+  /**
+   * Read the output of a button.
+   *
+   * @param outputNumber the button number
+   * @return the value of the button (true = pressed)
+   */
   bool GetOutput(int outputNumber);
 
+  /**
+   * Get the encoded 16-bit integer that passes button values.
+   *
+   * @return the button values
+   */
   int64_t GetOutputs();
 
+  /**
+   * Get the joystick rumble.
+   *
+   * @param type the rumble to read
+   * @return the rumble value
+   */
   double GetRumble(GenericHID::RumbleType type);
 
  protected:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
index 5735728..db573ad 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/JoystickSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -34,18 +31,53 @@
    */
   explicit JoystickSim(int port);
 
+  /**
+   * Set the X value of the joystick.
+   *
+   * @param value the new X value
+   */
   void SetX(double value);
 
+  /**
+   * Set the Y value of the joystick.
+   *
+   * @param value the new Y value
+   */
   void SetY(double value);
 
+  /**
+   * Set the Z value of the joystick.
+   *
+   * @param value the new Z value
+   */
   void SetZ(double value);
 
+  /**
+   * Set the twist value of the joystick.
+   *
+   * @param value the new twist value
+   */
   void SetTwist(double value);
 
+  /**
+   * Set the throttle value of the joystick.
+   *
+   * @param value the new throttle value
+   */
   void SetThrottle(double value);
 
+  /**
+   * Set the trigger value of the joystick.
+   *
+   * @param state the new value
+   */
   void SetTrigger(bool state);
 
+  /**
+   * Set the top state of the joystick.
+   *
+   * @param state the new state
+   */
   void SetTop(bool state);
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
index 4f3c658..61cbd46 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/LinearSystemSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -13,6 +10,7 @@
 #include <units/current.h>
 #include <units/time.h>
 
+#include "frc/RobotController.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/LinearSystem.h"
 
@@ -38,13 +36,13 @@
    * @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>{})
+  explicit LinearSystemSim(
+      const LinearSystem<States, Inputs, Outputs>& system,
+      const std::array<double, Outputs>& measurementStdDevs = {})
       : 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();
+    m_x = Eigen::Vector<double, States>::Zero();
+    m_y = Eigen::Vector<double, Outputs>::Zero();
+    m_u = Eigen::Vector<double, Inputs>::Zero();
   }
 
   /**
@@ -71,7 +69,7 @@
    *
    * @return The current output of the plant.
    */
-  const Eigen::Matrix<double, Outputs, 1>& GetOutput() const { return m_y; }
+  const Eigen::Vector<double, Outputs>& GetOutput() const { return m_y; }
 
   /**
    * Returns an element of the current output of the plant.
@@ -86,7 +84,7 @@
    *
    * @param u The system inputs.
    */
-  void SetInput(const Eigen::Matrix<double, Inputs, 1>& u) { m_u = u; }
+  void SetInput(const Eigen::Vector<double, Inputs>& u) { m_u = ClampInput(u); }
 
   /*
    * Sets the system inputs.
@@ -94,14 +92,17 @@
    * @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; }
+  void SetInput(int row, double value) {
+    m_u(row, 0) = value;
+    ClampInput(m_u);
+  }
 
   /**
    * Sets the system state.
    *
    * @param state The new state.
    */
-  void SetState(const Eigen::Matrix<double, States, 1>& state) { m_x = state; }
+  void SetState(const Eigen::Vector<double, States>& state) { m_x = state; }
 
   /**
    * Returns the current drawn by this simulated system. Override this method to
@@ -121,17 +122,29 @@
    * @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) {
+  virtual Eigen::Vector<double, States> UpdateX(
+      const Eigen::Vector<double, States>& currentXhat,
+      const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     return m_plant.CalculateX(currentXhat, u, dt);
   }
 
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If
+   * any does, the relative magnitudes of the input will be maintained.
+   *
+   * @param u          The input vector.
+   * @return The normalized input.
+   */
+  Eigen::Vector<double, Inputs> ClampInput(Eigen::Vector<double, Inputs> u) {
+    return frc::NormalizeInputVector<Inputs>(
+        u, frc::RobotController::GetInputVoltage());
+  }
+
   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;
+  Eigen::Vector<double, States> m_x;
+  Eigen::Vector<double, Outputs> m_y;
+  Eigen::Vector<double, Inputs> m_u;
   std::array<double, Outputs> m_measurementStdDevs;
 };
 }  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h
deleted file mode 100644
index 3d61cf5..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/Mechanism2D.h
+++ /dev/null
@@ -1,45 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PCMSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PCMSim.h
deleted file mode 100644
index 2e3ed5e..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PCMSim.h
+++ /dev/null
@@ -1,103 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PDPSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PDPSim.h
deleted file mode 100644
index c67815f..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PDPSim.h
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PS4ControllerSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PS4ControllerSim.h
new file mode 100644
index 0000000..a599ad9
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PS4ControllerSim.h
@@ -0,0 +1,176 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/simulation/GenericHIDSim.h"
+
+namespace frc {
+
+class PS4Controller;
+
+namespace sim {
+
+/**
+ * Class to control a simulated PS4 controller.
+ */
+class PS4ControllerSim : public GenericHIDSim {
+ public:
+  /**
+   * Constructs from a PS4Controller object.
+   *
+   * @param joystick controller to simulate
+   */
+  explicit PS4ControllerSim(const PS4Controller& joystick);
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  explicit PS4ControllerSim(int port);
+
+  /**
+   * Change the X axis value of the controller's left stick.
+   *
+   * @param value the new value
+   */
+  void SetLeftX(double value);
+
+  /**
+   * Change the X axis value of the controller's right stick.
+   *
+   * @param value the new value
+   */
+  void SetRightX(double value);
+
+  /**
+   * Change the Y axis value of the controller's left stick.
+   *
+   * @param value the new value
+   */
+  void SetLeftY(double value);
+
+  /**
+   * Change the Y axis value of the controller's right stick.
+   *
+   * @param value the new value
+   */
+  void SetRightY(double value);
+
+  /**
+   * Change the L2 axis axis value of the controller.
+   *
+   * @param value the new value
+   */
+  void SetL2Axis(double value);
+
+  /**
+   * Change the R2 axis value of the controller.
+   *
+   * @param value the new value
+   */
+  void SetR2Axis(double value);
+
+  /**
+   * Change the value of the Square button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetSquareButton(bool value);
+
+  /**
+   * Change the value of the Cross button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetCrossButton(bool value);
+
+  /**
+   * Change the value of the Circle button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetCircleButton(bool value);
+
+  /**
+   * Change the value of the Triangle button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetTriangleButton(bool value);
+
+  /**
+   * Change the value of the L1 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetL1Button(bool value);
+
+  /**
+   * Change the value of the R1 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetR1Button(bool value);
+
+  /**
+   * Change the value of the L2 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetL2Button(bool value);
+
+  /**
+   * Change the value of the R2 button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetR2Button(bool value);
+
+  /**
+   * Change the value of the Share button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetShareButton(bool value);
+
+  /**
+   * Change the value of the Options button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetOptionsButton(bool value);
+
+  /**
+   * Change the value of the L3 (left stick) button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetL3Button(bool value);
+
+  /**
+   * Change the value of the R3 (right stick) button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetR3Button(bool value);
+
+  /**
+   * Change the value of the PS button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetPSButton(bool value);
+
+  /**
+   * Change the value of the touchpad button on the controller.
+   *
+   * @param value the new value
+   */
+  void SetTouchpad(bool value);
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PWMSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
index ca2dfca..97e9b1f 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PWMSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,48 +33,153 @@
    */
   explicit PWMSim(int channel);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+  /**
+   * Register a callback to be run when the PWM is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether the PWM has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitialized() const;
 
+  /**
+   * Define whether the PWM has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   void SetInitialized(bool initialized);
 
-  std::unique_ptr<CallbackStore> RegisterRawValueCallback(
+  /**
+   * Register a callback to be run when the PWM raw value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterRawValueCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the PWM raw value.
+   *
+   * @return the PWM raw value
+   */
   int GetRawValue() const;
 
+  /**
+   * Set the PWM raw value.
+   *
+   * @param rawValue the PWM raw value
+   */
   void SetRawValue(int rawValue);
 
-  std::unique_ptr<CallbackStore> RegisterSpeedCallback(NotifyCallback callback,
-                                                       bool initialNotify);
+  /**
+   * Register a callback to be run when the PWM speed changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterSpeedCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the PWM speed.
+   *
+   * @return the PWM speed (-1.0 to 1.0)
+   */
   double GetSpeed() const;
 
+  /**
+   * Set the PWM speed.
+   *
+   * @param speed the PWM speed (-1.0 to 1.0)
+   */
   void SetSpeed(double speed);
 
-  std::unique_ptr<CallbackStore> RegisterPositionCallback(
+  /**
+   * Register a callback to be run when the PWM position changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPositionCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the PWM position.
+   *
+   * @return the PWM position (0.0 to 1.0)
+   */
   double GetPosition() const;
 
+  /**
+   * Set the PWM position.
+   *
+   * @param position the PWM position (0.0 to 1.0)
+   */
   void SetPosition(double position);
 
-  std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
+  /**
+   * Register a callback to be run when the PWM period scale changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPeriodScaleCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the PWM period scale.
+   *
+   * @return the PWM period scale
+   */
   int GetPeriodScale() const;
 
+  /**
+   * Set the PWM period scale.
+   *
+   * @param periodScale the PWM period scale
+   */
   void SetPeriodScale(int periodScale);
 
-  std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
+  /**
+   * Register a callback to be run when the PWM zero latch state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterZeroLatchCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether the PWM is zero latched.
+   *
+   * @return true if zero latched
+   */
   bool GetZeroLatch() const;
 
+  /**
+   * Define whether the PWM has been zero latched.
+   *
+   * @param zeroLatch true to indicate zero latched
+   */
   void SetZeroLatch(bool zeroLatch);
 
+  /**
+   * Reset all simulation data.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h
new file mode 100644
index 0000000..ac54da2
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/PowerDistributionSim.h
@@ -0,0 +1,167 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class PowerDistribution;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Power Distribution Panel (PowerDistribution).
+ */
+class PowerDistributionSim {
+ public:
+  /**
+   * Constructs from a PowerDistribution module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit PowerDistributionSim(int module = 0);
+
+  /**
+   * Constructs from a PowerDistribution object.
+   *
+   * @param pdp PowerDistribution to simulate
+   */
+  explicit PowerDistributionSim(const PowerDistribution& pdp);
+
+  /**
+   * Register a callback to be run when the PowerDistribution is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check whether the PowerDistribution has been initialized.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether the PowerDistribution has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
+  void SetInitialized(bool initialized);
+
+  /**
+   * Register a callback to be run whenever the PowerDistribution temperature
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterTemperatureCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the temperature of the PowerDistribution.
+   *
+   * @return the PowerDistribution temperature
+   */
+  double GetTemperature() const;
+
+  /**
+   * Define the PowerDistribution temperature.
+   *
+   * @param temperature the new PowerDistribution temperature
+   */
+  void SetTemperature(double temperature);
+
+  /**
+   * Register a callback to be run whenever the PowerDistribution voltage
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterVoltageCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the PowerDistribution voltage.
+   *
+   * @return the PowerDistribution voltage.
+   */
+  double GetVoltage() const;
+
+  /**
+   * Set the PowerDistribution voltage.
+   *
+   * @param voltage the new PowerDistribution voltage
+   */
+  void SetVoltage(double voltage);
+
+  /**
+   * Register a callback to be run whenever the current of a specific channel
+   * changes.
+   *
+   * @param channel the channel
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterCurrentCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Read the current in one of the PowerDistribution channels.
+   *
+   * @param channel the channel to check
+   * @return the current in the given channel
+   */
+  double GetCurrent(int channel) const;
+
+  /**
+   * Change the current in the given channel.
+   *
+   * @param channel the channel to edit
+   * @param current the new current for the channel
+   */
+  void SetCurrent(int channel, double current);
+
+  /**
+   * Read the current of all of the PowerDistribution channels.
+   *
+   * @param currents output array; set to the current in each channel. The
+   *                 array must be big enough to hold all the PowerDistribution
+   *                 channels
+   * @param length   length of output array
+   */
+  void GetAllCurrents(double* currents, int length) const;
+
+  /**
+   * Change the current in all of the PowerDistribution channels.
+   *
+   * @param currents array containing the current values for each channel. The
+   *                 array must be big enough to hold all the PowerDistribution
+   *                 channels
+   * @param length   length of array
+   */
+  void SetAllCurrents(const double* currents, int length);
+
+  /**
+   * Reset all PowerDistribution simulation data.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
new file mode 100644
index 0000000..97e31bf
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/REVPHSim.h
@@ -0,0 +1,212 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include "frc/PneumaticsBase.h"
+#include "frc/simulation/CallbackStore.h"
+
+namespace frc {
+
+class Compressor;
+
+namespace sim {
+
+/**
+ * Class to control a simulated Pneumatic Control Module (PCM).
+ */
+class REVPHSim {
+ public:
+  /**
+   * Constructs with the default PCM module number (CAN ID).
+   */
+  REVPHSim();
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  explicit REVPHSim(int module);
+
+  explicit REVPHSim(const PneumaticsBase& pneumatics);
+
+  /**
+   * Register a callback to be run when a solenoid is initialized on a channel.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterInitializedCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if a solenoid has been initialized on a specific channel.
+   *
+   * @return true if initialized
+   */
+  bool GetInitialized() const;
+
+  /**
+   * Define whether a solenoid has been initialized on a specific channel.
+   *
+   * @param solenoidInitialized is there a solenoid initialized on that channel
+   */
+  void SetInitialized(bool solenoidInitialized);
+
+  /**
+   * Register a callback to be run when the solenoid output on a channel
+   * changes.
+   *
+   * @param channel the channel to monitor
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterSolenoidOutputCallback(
+      int channel, NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @return the solenoid output
+   */
+  bool GetSolenoidOutput(int channel) const;
+
+  /**
+   * Change the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @param solenoidOutput the new solenoid output
+   */
+  void SetSolenoidOutput(int channel, bool solenoidOutput);
+
+  /**
+   * Register a callback to be run when the compressor activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterCompressorOnCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  bool GetCompressorOn() const;
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  void SetCompressorOn(bool compressorOn);
+
+  /**
+   * Register a callback to be run whenever the closed loop state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterClosedLoopEnabledCallback(NotifyCallback callback,
+                                    bool initialNotify);
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return true if active
+   */
+  bool GetClosedLoopEnabled() const;
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param closedLoopEnabled whether the control loop is active
+   */
+  void SetClosedLoopEnabled(bool closedLoopEnabled);
+
+  /**
+   * Register a callback to be run whenever the pressure switch value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterPressureSwitchCallback(
+      NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  bool GetPressureSwitch() const;
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  void SetPressureSwitch(bool pressureSwitch);
+
+  /**
+   * Register a callback to be run whenever the compressor current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterCompressorCurrentCallback(NotifyCallback callback,
+                                    bool initialNotify);
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  double GetCompressorCurrent() const;
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  void SetCompressorCurrent(double compressorCurrent);
+
+  /**
+   * Get the current value of all solenoid outputs.
+   *
+   * @return the solenoid outputs (1 bit per output)
+   */
+  uint8_t GetAllSolenoidOutputs() const;
+
+  /**
+   * Change all of the solenoid outputs.
+   *
+   * @param outputs the new solenoid outputs (1 bit per output)
+   */
+  void SetAllSolenoidOutputs(uint8_t outputs);
+
+  /**
+   * Reset all simulation data for this object.
+   */
+  void ResetData();
+
+ private:
+  int m_index;
+};
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RelaySim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
index 99a8eef..5034885 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RelaySim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -36,34 +33,107 @@
    */
   explicit RelaySim(int channel);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedForwardCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run when the forward direction is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterInitializedForwardCallback(NotifyCallback callback,
+                                     bool initialNotify);
 
+  /**
+   * Check whether the forward direction has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitializedForward() const;
 
+  /**
+   * Define whether the forward direction has been initialized.
+   *
+   * @param initializedForward whether this object is initialized
+   */
   void SetInitializedForward(bool initializedForward);
 
-  std::unique_ptr<CallbackStore> RegisterInitializedReverseCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run when the reverse direction is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore>
+  RegisterInitializedReverseCallback(NotifyCallback callback,
+                                     bool initialNotify);
 
+  /**
+   * Check whether the reverse direction has been initialized.
+   *
+   * @return true if initialized
+   */
   bool GetInitializedReverse() const;
 
+  /**
+   * Define whether the reverse direction has been initialized.
+   *
+   * @param initializedReverse whether this object is initialized
+   */
   void SetInitializedReverse(bool initializedReverse);
 
-  std::unique_ptr<CallbackStore> RegisterForwardCallback(
+  /**
+   * Register a callback to be run when the forward direction changes state.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterForwardCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether the forward direction is active.
+   *
+   * @return true if active
+   */
   bool GetForward() const;
 
+  /**
+   * Set whether the forward direction is active.
+   *
+   * @param forward true to make active
+   */
   void SetForward(bool forward);
 
-  std::unique_ptr<CallbackStore> RegisterReverseCallback(
+  /**
+   * Register a callback to be run when the reverse direction changes state.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterReverseCallback(
       NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether the reverse direction is active.
+   *
+   * @return true if active
+   */
   bool GetReverse() const;
 
+  /**
+   * Set whether the reverse direction is active.
+   *
+   * @param reverse true to make active
+   */
   void SetReverse(bool reverse);
 
+  /**
+   * Reset all simulation data.
+   */
   void ResetData();
 
  private:
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
index 32eb462..1095106 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/RoboRioSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,120 +11,416 @@
 
 #include "frc/simulation/CallbackStore.h"
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 
 /**
- * Class to control a simulated RoboRIO.
+ * A utility class to control a simulated RoboRIO.
  */
 class RoboRioSim {
  public:
-  static std::unique_ptr<CallbackStore> RegisterFPGAButtonCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run when the FPGA button state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterFPGAButtonCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Query the state of the FPGA button.
+   *
+   * @return the FPGA button state
+   */
   static bool GetFPGAButton();
 
+  /**
+   * Define the state of the FPGA button.
+   *
+   * @param fPGAButton the new state
+   */
   static void SetFPGAButton(bool fPGAButton);
 
-  static std::unique_ptr<CallbackStore> RegisterVInVoltageCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the Vin voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterVInVoltageCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the Vin voltage.
+   *
+   * @return the Vin voltage
+   */
   static units::volt_t GetVInVoltage();
 
+  /**
+   * Define the Vin voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
   static void SetVInVoltage(units::volt_t vInVoltage);
 
-  static std::unique_ptr<CallbackStore> RegisterVInCurrentCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the Vin current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterVInCurrentCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the Vin current.
+   *
+   * @return the Vin current
+   */
   static units::ampere_t GetVInCurrent();
 
+  /**
+   * Define the Vin current.
+   *
+   * @param vInCurrent the new current
+   */
   static void SetVInCurrent(units::ampere_t vInCurrent);
 
-  static std::unique_ptr<CallbackStore> RegisterUserVoltage6VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 6V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserVoltage6VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the 6V rail voltage.
+   *
+   * @return the 6V rail voltage
+   */
   static units::volt_t GetUserVoltage6V();
 
+  /**
+   * Define the 6V rail voltage.
+   *
+   * @param userVoltage6V the new voltage
+   */
   static void SetUserVoltage6V(units::volt_t userVoltage6V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserCurrent6VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 6V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserCurrent6VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the 6V rail current.
+   *
+   * @return the 6V rail current
+   */
   static units::ampere_t GetUserCurrent6V();
 
+  /**
+   * Define the 6V rail current.
+   *
+   * @param userCurrent6V the new current
+   */
   static void SetUserCurrent6V(units::ampere_t userCurrent6V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserActive6VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 6V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserActive6VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the 6V rail active state.
+   *
+   * @return true if the 6V rail is active
+   */
   static bool GetUserActive6V();
 
+  /**
+   * Set the 6V rail active state.
+   *
+   * @param userActive6V true to make rail active
+   */
   static void SetUserActive6V(bool userActive6V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserVoltage5VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 5V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserVoltage5VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the 5V rail voltage.
+   *
+   * @return the 5V rail voltage
+   */
   static units::volt_t GetUserVoltage5V();
 
+  /**
+   * Define the 5V rail voltage.
+   *
+   * @param userVoltage5V the new voltage
+   */
   static void SetUserVoltage5V(units::volt_t userVoltage5V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserCurrent5VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 5V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserCurrent5VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the 5V rail current.
+   *
+   * @return the 5V rail current
+   */
   static units::ampere_t GetUserCurrent5V();
 
+  /**
+   * Define the 5V rail current.
+   *
+   * @param userCurrent5V the new current
+   */
   static void SetUserCurrent5V(units::ampere_t userCurrent5V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserActive5VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 5V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserActive5VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the 5V rail active state.
+   *
+   * @return true if the 5V rail is active
+   */
   static bool GetUserActive5V();
 
+  /**
+   * Set the 5V rail active state.
+   *
+   * @param userActive5V true to make rail active
+   */
   static void SetUserActive5V(bool userActive5V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserVoltage3V3Callback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 3.3V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserVoltage3V3Callback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the 3.3V rail voltage.
+   *
+   * @return the 3.3V rail voltage
+   */
   static units::volt_t GetUserVoltage3V3();
 
+  /**
+   * Define the 3.3V rail voltage.
+   *
+   * @param userVoltage3V3 the new voltage
+   */
   static void SetUserVoltage3V3(units::volt_t userVoltage3V3);
 
-  static std::unique_ptr<CallbackStore> RegisterUserCurrent3V3Callback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 3.3V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserCurrent3V3Callback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the 3.3V rail current.
+   *
+   * @return the 3.3V rail current
+   */
   static units::ampere_t GetUserCurrent3V3();
 
+  /**
+   * Define the 3.3V rail current.
+   *
+   * @param userCurrent3V3 the new current
+   */
   static void SetUserCurrent3V3(units::ampere_t userCurrent3V3);
 
-  static std::unique_ptr<CallbackStore> RegisterUserActive3V3Callback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 3.3V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserActive3V3Callback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the 3.3V rail active state.
+   *
+   * @return true if the 3.3V rail is active
+   */
   static bool GetUserActive3V3();
 
+  /**
+   * Set the 3.3V rail active state.
+   *
+   * @param userActive3V3 true to make rail active
+   */
   static void SetUserActive3V3(bool userActive3V3);
 
-  static std::unique_ptr<CallbackStore> RegisterUserFaults6VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 6V rail number of faults
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserFaults6VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the 6V rail number of faults.
+   *
+   * @return number of faults
+   */
   static int GetUserFaults6V();
 
+  /**
+   * Set the 6V rail number of faults.
+   *
+   * @param userFaults6V number of faults
+   */
   static void SetUserFaults6V(int userFaults6V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserFaults5VCallback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 5V rail number of faults
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserFaults5VCallback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the 5V rail number of faults.
+   *
+   * @return number of faults
+   */
   static int GetUserFaults5V();
 
+  /**
+   * Set the 5V rail number of faults.
+   *
+   * @param userFaults5V number of faults
+   */
   static void SetUserFaults5V(int userFaults5V);
 
-  static std::unique_ptr<CallbackStore> RegisterUserFaults3V3Callback(
-      NotifyCallback callback, bool initialNotify);
+  /**
+   * Register a callback to be run whenever the 3.3V rail number of faults
+   * changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the
+   *                      initial value
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterUserFaults3V3Callback(NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Get the 3.3V rail number of faults.
+   *
+   * @return number of faults
+   */
   static int GetUserFaults3V3();
 
+  /**
+   * Set the 3.3V rail number of faults.
+   *
+   * @param userFaults3V3 number of faults
+   */
   static void SetUserFaults3V3(int userFaults3V3);
 
+  /**
+   * Register a callback to be run whenever the brownout voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] static std::unique_ptr<CallbackStore>
+  RegisterBrownoutVoltageCallback(NotifyCallback callback, bool initialNotify);
+
+  /**
+   * Measure the brownout voltage.
+   *
+   * @return the brownout voltage
+   */
+  static units::volt_t GetBrownoutVoltage();
+
+  /**
+   * Define the brownout voltage.
+   *
+   * @param brownoutVoltage the new voltage
+   */
+  static void SetBrownoutVoltage(units::volt_t brownoutVoltage);
+
+  /**
+   * Reset all simulation data.
+   */
   static void ResetData();
 };
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
index f2e3249..9d31bd0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,51 +8,142 @@
 
 #include "frc/simulation/CallbackStore.h"
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 class SPIAccelerometerSim {
  public:
+  /**
+   * Construct a new simulation object.
+   *
+   * @param index the HAL index of the accelerometer
+   */
   explicit SPIAccelerometerSim(int index);
 
-  std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
-                                                        bool initialNotify);
+  /**
+   * Register a callback to be run when this accelerometer activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterActiveCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check whether the accelerometer is active.
+   *
+   * @return true if active
+   */
   bool GetActive() const;
 
+  /**
+   * Define whether this accelerometer is active.
+   *
+   * @param active the new state
+   */
   void SetActive(bool active);
 
-  std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
-                                                       bool initialNotify);
+  /**
+   * Register a callback to be run whenever the range changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterRangeCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Check the range of this accelerometer.
+   *
+   * @return the accelerometer range
+   */
   int GetRange() const;
 
+  /**
+   * Change the range of this accelerometer.
+   *
+   * @param range the new accelerometer range
+   */
   void SetRange(int range);
 
-  std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
-                                                   bool initialNotify);
+  /**
+   * Register a callback to be run whenever the X axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterXCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the X axis value.
+   *
+   * @return the X axis measurement
+   */
   double GetX() const;
 
+  /**
+   * Change the X axis value of the accelerometer.
+   *
+   * @param x the new reading of the X axis
+   */
   void SetX(double x);
 
-  std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
-                                                   bool initialNotify);
+  /**
+   * Register a callback to be run whenever the Y axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterYCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the Y axis value.
+   *
+   * @return the Y axis measurement
+   */
   double GetY() const;
 
+  /**
+   * Change the Y axis value of the accelerometer.
+   *
+   * @param y the new reading of the Y axis
+   */
   void SetY(double y);
 
-  std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
-                                                   bool initialNotify);
+  /**
+   * Register a callback to be run whenever the Z axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the CallbackStore object associated with this callback
+   */
+  [[nodiscard]] std::unique_ptr<CallbackStore> RegisterZCallback(
+      NotifyCallback callback, bool initialNotify);
 
+  /**
+   * Measure the Z axis value.
+   *
+   * @return the Z axis measurement
+   */
   double GetZ() const;
 
+  /**
+   * Change the Z axis value of the accelerometer.
+   *
+   * @param z the new reading of the Z axis
+   */
   void SetZ(double z);
 
+  /**
+   * Reset all simulation data of this object.
+   */
   void ResetData();
 
  private:
   int m_index;
 };
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
index 640cc0e..0a4e4d8 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimDeviceSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,8 +11,7 @@
 #include <hal/SimDevice.h>
 #include <hal/simulation/SimDeviceData.h>
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 
 /**
  * Class to control the simulation side of a SimDevice.
@@ -29,28 +25,109 @@
    */
   explicit SimDeviceSim(const char* name);
 
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   */
+  SimDeviceSim(const char* name, int index);
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   * @param channel device channel number to append to name
+   */
+  SimDeviceSim(const char* name, int index, int channel);
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   hal::SimValue GetValue(const char* name) const;
 
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimInt GetInt(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  hal::SimLong GetLong(const char* name) const;
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   hal::SimDouble GetDouble(const char* name) const;
 
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   hal::SimEnum GetEnum(const char* name) const;
 
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   hal::SimBoolean GetBoolean(const char* name) const;
 
+  /**
+   * Get all options for the given enum.
+   *
+   * @param val the enum
+   * @return names of the different values for that enum
+   */
   static std::vector<std::string> GetEnumOptions(hal::SimEnum val);
 
+  /**
+   * Get all properties.
+   *
+   * @param callback callback called for each property (SimValue).  Signature
+   *                 of the callback must be const char*, HAL_SimValueHandle,
+   *                 int, const HAL_Value*
+   */
   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);
+           int direction, const struct HAL_Value* value) {
+          std::invoke(*static_cast<F*>(param), name, handle, direction, value);
         });
   }
 
-  operator HAL_SimDeviceHandle() const { return m_handle; }
+  /**
+   * Get the raw handle of this object.
+   *
+   * @return the handle used to refer to this object
+   */
+  operator HAL_SimDeviceHandle() const { return m_handle; }  // NOLINT
 
+  /**
+   * Get all sim devices with the given prefix.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @param callback callback function to call for each sim device
+   */
   template <typename F>
   static void EnumerateDevices(const char* prefix, F callback) {
     return HALSIM_EnumerateSimDevices(
@@ -60,10 +137,12 @@
         });
   }
 
+  /**
+   * Reset all SimDevice data.
+   */
   static void ResetData();
 
  private:
   HAL_SimDeviceHandle m_handle;
 };
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimHooks.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
index 7690a4f..5d8a6b4 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SimHooks.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,9 +9,13 @@
 #include <hal/HALBase.h>
 #include <units/time.h>
 
-namespace frc {
-namespace sim {
+namespace frc::sim {
 
+/**
+ * Override the HAL runtime type (simulated/real).
+ *
+ * @param type runtime type
+ */
 void SetRuntimeType(HAL_RuntimeType type);
 
 void WaitForProgramStart();
@@ -23,17 +24,40 @@
 
 bool GetProgramStarted();
 
+/**
+ * Restart the simulator time.
+ */
 void RestartTiming();
 
+/**
+ * Pause the simulator time.
+ */
 void PauseTiming();
 
+/**
+ * Resume the simulator time.
+ */
 void ResumeTiming();
 
+/**
+ * Check if the simulator time is paused.
+ *
+ * @return true if paused
+ */
 bool IsTimingPaused();
 
+/**
+ * Advance the simulator time and wait for all notifiers to run.
+ *
+ * @param delta the amount to advance (in seconds)
+ */
 void StepTiming(units::second_t delta);
 
+/**
+ * Advance the simulator time and return immediately.
+ *
+ * @param delta the amount to advance (in seconds)
+ */
 void StepTimingAsync(units::second_t delta);
 
-}  // namespace sim
-}  // namespace frc
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
index f8de822..4ff29cc 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/SingleJointedArmSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -66,20 +63,34 @@
                       const std::array<double, 1>& measurementStdDevs = {0.0});
 
   /**
+   * Returns whether the arm would hit the lower limit.
+   *
+   * @param armAngle The arm height.
+   * @return Whether the arm would hit the lower limit.
+   */
+  bool WouldHitLowerLimit(units::radian_t armAngle) const;
+
+  /**
+   * Returns whether the arm would hit the upper limit.
+   *
+   * @param armAngle The arm height.
+   * @return Whether the arm would hit the upper limit.
+   */
+  bool WouldHitUpperLimit(units::radian_t armAngle) const;
+
+  /**
    * 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;
+  bool HasHitLowerLimit() 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;
+  bool HasHitUpperLimit() const;
 
   /**
    * Returns the current arm angle.
@@ -103,7 +114,7 @@
   units::ampere_t GetCurrentDraw() const override;
 
   /**
-   * Sets the input voltage for the elevator.
+   * Sets the input voltage for the arm.
    *
    * @param voltage The input voltage.
    */
@@ -131,9 +142,9 @@
    * @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;
+  Eigen::Vector<double, 2> UpdateX(const Eigen::Vector<double, 2>& currentXhat,
+                                   const Eigen::Vector<double, 1>& u,
+                                   units::second_t dt) override;
 
  private:
   units::meter_t m_r;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h
new file mode 100644
index 0000000..efa9b37
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/UltrasonicSim.h
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+#include <units/length.h>
+
+namespace frc {
+
+class Ultrasonic;
+
+namespace sim {
+
+/**
+ * Class to control a simulated ADXRS450 gyroscope.
+ */
+class UltrasonicSim {
+ public:
+  /**
+   * Constructs from a ADXRS450_Gyro object.
+   *
+   * @param gyro ADXRS450_Gyro to simulate
+   */
+  explicit UltrasonicSim(const Ultrasonic& gyro);
+
+  /**
+   * Sets if the range measurement is valid.
+   *
+   * @param isValid True if valid
+   */
+  void SetRangeValid(bool isValid);
+
+  /**
+   * Sets the range measurement
+   *
+   * @param range The range
+   */
+  void SetRange(units::meter_t range);
+
+ private:
+  hal::SimBoolean m_simRangeValid;
+  hal::SimDouble m_simRange;
+};
+
+}  // namespace sim
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
index d981111..e609ff0 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/simulation/XboxControllerSim.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -34,27 +31,117 @@
    */
   explicit XboxControllerSim(int port);
 
-  void SetX(GenericHID::JoystickHand hand, double value);
+  /**
+   * Change the X axis value of the controller's left stick.
+   *
+   * @param value the new value
+   */
+  void SetLeftX(double value);
 
-  void SetY(GenericHID::JoystickHand hand, double value);
+  /**
+   * Change the X axis value of the controller's right stick.
+   *
+   * @param value the new value
+   */
+  void SetRightX(double value);
 
-  void SetTriggerAxis(GenericHID::JoystickHand hand, double value);
+  /**
+   * Change the Y axis value of the controller's left stick.
+   *
+   * @param value the new value
+   */
+  void SetLeftY(double value);
 
-  void SetBumper(GenericHID::JoystickHand hand, bool state);
+  /**
+   * Change the Y axis value of the controller's right stick.
+   *
+   * @param value the new value
+   */
+  void SetRightY(double value);
 
-  void SetStickButton(GenericHID::JoystickHand hand, bool state);
+  /**
+   * Change the left trigger axis value of the joystick.
+   *
+   * @param value the new value
+   */
+  void SetLeftTriggerAxis(double value);
 
-  void SetAButton(bool state);
+  /**
+   * Change the right trigger axis value of the joystick.
+   *
+   * @param value the new value
+   */
+  void SetRightTriggerAxis(double value);
 
-  void SetBButton(bool state);
+  /**
+   * Change the left bumper value of the joystick.
+   *
+   * @param value the new value
+   */
+  void SetLeftBumper(bool value);
 
-  void SetXButton(bool state);
+  /**
+   * Change the right bumper value of the joystick.
+   *
+   * @param value the new value
+   */
+  void SetRightBumper(bool value);
 
-  void SetYButton(bool state);
+  /**
+   * Change the left button value of the joystick.
+   *
+   * @param value the new value
+   */
+  void SetLeftStickButton(bool value);
 
-  void SetBackButton(bool state);
+  /**
+   * Change the right button value of the joystick.
+   *
+   * @param value the new value
+   */
+  void SetRightStickButton(bool value);
 
-  void SetStartButton(bool state);
+  /**
+   * Change the value of the A button.
+   *
+   * @param value the new value
+   */
+  void SetAButton(bool value);
+
+  /**
+   * Change the value of the B button.
+   *
+   * @param value the new value
+   */
+  void SetBButton(bool value);
+
+  /**
+   * Change the value of the X button.
+   *
+   * @param value the new value
+   */
+  void SetXButton(bool value);
+
+  /**
+   * Change the value of the Y button.
+   *
+   * @param value the new value
+   */
+  void SetYButton(bool value);
+
+  /**
+   * Change the value of the Back button.
+   *
+   * @param value the new value
+   */
+  void SetBackButton(bool value);
+
+  /**
+   * Change the value of the Start button.
+   *
+   * @param value the new value
+   */
+  void SetStartButton(bool value);
 };
 
 }  // namespace sim
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Field2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Field2d.h
new file mode 100644
index 0000000..61d7a72
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Field2d.h
@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+#include <vector>
+
+#include <networktables/NTSendable.h>
+#include <networktables/NetworkTable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <units/length.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/smartdashboard/FieldObject2d.h"
+
+namespace frc {
+
+/**
+ * 2D representation of game field for dashboards.
+ *
+ * An object's pose is the location shown on the dashboard view.  Note that
+ * for the robot, this may or may not match the 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 wheel odometry),
+ * and set the new pose.
+ *
+ * This class provides methods to set the robot pose, but other objects can
+ * also be shown by using the GetObject() function.  Other objects can
+ * also have multiple poses (which will show the object at multiple locations).
+ */
+class Field2d : public nt::NTSendable, public wpi::SendableHelper<Field2d> {
+ public:
+  using Entry = size_t;
+
+  Field2d();
+
+  Field2d(Field2d&& rhs);
+  Field2d& operator=(Field2d&& rhs);
+
+  /**
+   * 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() const;
+
+  /**
+   * Get or create a field object.
+   *
+   * @return Field object
+   */
+  FieldObject2d* GetObject(std::string_view name);
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  FieldObject2d* GetRobotObject();
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  std::shared_ptr<nt::NetworkTable> m_table;
+
+  mutable wpi::mutex m_mutex;
+  std::vector<std::unique_ptr<FieldObject2d>> m_objects;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h
new file mode 100644
index 0000000..2e41d84
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/FieldObject2d.h
@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <initializer_list>
+#include <string>
+#include <string_view>
+#include <utility>
+#include <vector>
+
+#include <networktables/NetworkTableEntry.h>
+#include <units/length.h>
+#include <wpi/SmallVector.h>
+#include <wpi/mutex.h>
+#include <wpi/span.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+
+namespace frc {
+
+class Field2d;
+class Trajectory;
+
+/**
+ * Game field object on a Field2d.
+ */
+class FieldObject2d {
+  friend class Field2d;
+  struct private_init {};
+
+ public:
+  FieldObject2d(std::string_view name, const private_init&) : m_name{name} {}
+
+  FieldObject2d(FieldObject2d&& rhs);
+  FieldObject2d& operator=(FieldObject2d&& rhs);
+
+  /**
+   * Set the pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  void SetPose(const Pose2d& pose);
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param x X location
+   * @param y Y location
+   * @param rotation rotation
+   */
+  void SetPose(units::meter_t x, units::meter_t y, Rotation2d rotation);
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose, or 0,0,0 if unknown / does not exist
+   */
+  Pose2d GetPose() const;
+
+  /**
+   * Set multiple poses from an array of Pose objects.
+   * The total number of poses is limited to 85.
+   *
+   * @param poses array of 2D poses
+   */
+  void SetPoses(wpi::span<const Pose2d> poses);
+
+  /**
+   * Set multiple poses from an array of Pose objects.
+   * The total number of poses is limited to 85.
+   *
+   * @param poses array of 2D poses
+   */
+  void SetPoses(std::initializer_list<Pose2d> poses);
+
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which poses should be added.
+   */
+  void SetTrajectory(const Trajectory& trajectory);
+
+  /**
+   * Get multiple poses.
+   *
+   * @param obj Object entry
+   * @return vector of 2D poses
+   */
+  std::vector<Pose2d> GetPoses() const;
+
+  /**
+   * Get multiple poses.
+   *
+   * @param out output SmallVector to hold 2D poses
+   * @return span referring to output SmallVector
+   */
+  wpi::span<const Pose2d> GetPoses(wpi::SmallVectorImpl<Pose2d>& out) const;
+
+ private:
+  void UpdateEntry(bool setDefault = false);
+  void UpdateFromEntry() const;
+
+  mutable wpi::mutex m_mutex;
+  std::string m_name;
+  nt::NetworkTableEntry m_entry;
+  mutable wpi::SmallVector<Pose2d, 1> m_poses;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h
index 9500278..cdfdafd 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/ListenerExecutor.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.h
new file mode 100644
index 0000000..cbe25be
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Mechanism2d.h
@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+
+#include <networktables/NTSendable.h>
+#include <networktables/NetworkTableEntry.h>
+#include <wpi/StringMap.h>
+#include <wpi/mutex.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/smartdashboard/MechanismRoot2d.h"
+#include "frc/util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * Visual 2D representation of arms, elevators, and general mechanisms through
+ * a node-based API.
+ *
+ * A Mechanism2d object is published and contains at least one root node. A root
+ * is the anchor point of other nodes (such as ligaments). Other nodes are
+ * recursively appended based on other nodes.
+ *
+ * Except for the Mechanism2d container object, none of the objects should be
+ * passed or interacted with by value! Obtain pointers from factory methods such
+ * as Mechanism2d.GetRoot() and MechanismObject2d.Append<>(). The Mechanism2d
+ * container object owns the root nodes, and each node internally owns the nodes
+ * based on it. Beware not to let the Mechanism2d object out of scope - all
+ * nodes will be recursively destructed!
+ *
+ * @see MechanismObject2d
+ * @see MechanismLigament2d
+ * @see MechanismRoot2d
+ */
+class Mechanism2d : public nt::NTSendable,
+                    public wpi::SendableHelper<Mechanism2d> {
+ public:
+  /**
+   * Create a new Mechanism2d with the given dimensions and background color.
+   *
+   * The dimensions represent the canvas that all the nodes are drawn on. The
+   * default color is dark blue.
+   *
+   * @param width the width
+   * @param height the height
+   * @param backgroundColor the background color
+   */
+  Mechanism2d(double width, double height,
+              const Color8Bit& backgroundColor = {0, 0, 32});
+
+  /**
+   * Get or create a root in this Mechanism2d with the given name and
+   * position.
+   *
+   * <p>If a root with the given name already exists, the given x and y
+   * coordinates are not used.
+   *
+   * @param name the root name
+   * @param x the root x coordinate
+   * @param y the root y coordinate
+   * @return a new root object, or the existing one with the given name.
+   */
+  MechanismRoot2d* GetRoot(std::string_view name, double x, double y);
+
+  /**
+   * Set the Mechanism2d background color.
+   *
+   * @param color the new background color
+   */
+  void SetBackgroundColor(const Color8Bit& color);
+
+  void InitSendable(nt::NTSendableBuilder& builder) override;
+
+ private:
+  double m_width;
+  double m_height;
+  char m_color[10];
+  mutable wpi::mutex m_mutex;
+  std::shared_ptr<nt::NetworkTable> m_table;
+  wpi::StringMap<std::unique_ptr<MechanismRoot2d>> m_roots;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
new file mode 100644
index 0000000..70abaa6
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismLigament2d.h
@@ -0,0 +1,88 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <networktables/NetworkTableEntry.h>
+#include <units/angle.h>
+
+#include "frc/smartdashboard/MechanismObject2d.h"
+#include "frc/util/Color8Bit.h"
+
+namespace frc {
+
+/**
+ * Ligament node on a Mechanism2d.
+ *
+ * A ligament can have its length changed (like an elevator) or angle changed,
+ * like an arm.
+ *
+ * @see Mechanism2d
+ */
+class MechanismLigament2d : public MechanismObject2d {
+ public:
+  MechanismLigament2d(std::string_view name, double length,
+                      units::degree_t angle, double lineWidth = 6,
+                      const frc::Color8Bit& color = {235, 137, 52});
+
+  /**
+   * Set the ligament color.
+   *
+   * @param color the color of the line
+   */
+  void SetColor(const frc::Color8Bit& color);
+
+  /**
+   * Set the ligament's length.
+   *
+   * @param length the line length
+   */
+  void SetLength(double length);
+
+  /**
+   * Get the ligament length.
+   *
+   * @return the line length
+   */
+  double GetLength();
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param angle the angle
+   */
+  void SetAngle(units::degree_t angle);
+
+  /**
+   * Get the ligament's angle relative to its parent.
+   *
+   * @return the angle
+   */
+  double GetAngle();
+
+  /**
+   * Set the line thickness.
+   *
+   * @param lineWidth the line thickness
+   */
+  void SetLineWeight(double lineWidth);
+
+ protected:
+  void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
+
+ private:
+  void Flush();
+  double m_length;
+  nt::NetworkTableEntry m_lengthEntry;
+  double m_angle;
+  nt::NetworkTableEntry m_angleEntry;
+  double m_weight;
+  nt::NetworkTableEntry m_weightEntry;
+  char m_color[10];
+  nt::NetworkTableEntry m_colorEntry;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.h
new file mode 100644
index 0000000..c4185e7
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismObject2d.h
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <stdexcept>
+#include <string>
+#include <string_view>
+#include <type_traits>
+#include <utility>
+
+#include <networktables/NetworkTable.h>
+#include <wpi/StringMap.h>
+
+#include "frc/Errors.h"
+
+namespace frc {
+
+/**
+ * Common base class for all Mechanism2d node types.
+ *
+ * To append another node, call Append with the type of node and its
+ * construction parameters. None of the node types are designed to be
+ * constructed directly, and are owned by their parent node/container - obtain
+ * pointers from the Append function or similar factory methods.
+ *
+ * @see Mechanism2d.
+ */
+class MechanismObject2d {
+  friend class Mechanism2d;
+
+ protected:
+  explicit MechanismObject2d(std::string_view name);
+
+  /**
+   * Update all entries with new ones from a new table.
+   *
+   * @param table the new table.
+   */
+  virtual void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) = 0;
+
+  mutable wpi::mutex m_mutex;
+
+ public:
+  virtual ~MechanismObject2d() = default;
+
+  /**
+   * Retrieve the object's name.
+   *
+   * @return the object's name relative to its parent.
+   */
+  const std::string& GetName() const;
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param name the name of the new object.
+   * @param args constructor arguments of the object type.
+   * @return the constructed and appended object, useful for variable
+   * assignments and call chaining.
+   * @throw if an object with the given name already exists.
+   */
+  template <typename T, typename... Args,
+            typename =
+                std::enable_if_t<std::is_convertible_v<T*, MechanismObject2d*>>>
+  T* Append(std::string_view name, Args&&... args) {
+    std::scoped_lock lock(m_mutex);
+    auto& obj = m_objects[name];
+    if (obj) {
+      throw FRC_MakeError(
+          err::Error,
+          "MechanismObject names must be unique! `{}` was inserted twice!",
+          name);
+    }
+    obj = std::make_unique<T>(name, std::forward<Args>(args)...);
+    T* ex = static_cast<T*>(obj.get());
+    if (m_table) {
+      ex->Update(m_table->GetSubTable(name));
+    }
+    return ex;
+  }
+
+ private:
+  std::string m_name;
+  wpi::StringMap<std::unique_ptr<MechanismObject2d>> m_objects;
+  std::shared_ptr<nt::NetworkTable> m_table;
+  void Update(std::shared_ptr<nt::NetworkTable> table);
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h
new file mode 100644
index 0000000..5072547
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/MechanismRoot2d.h
@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string_view>
+
+#include <networktables/NetworkTableEntry.h>
+
+#include "MechanismObject2d.h"
+
+namespace frc {
+
+/**
+ * Root Mechanism2d node.
+ *
+ * A root is the anchor point of other nodes (such as ligaments).
+ *
+ * Do not create objects of this class directly! Obtain pointers from the
+ * Mechanism2d.GetRoot() factory method.
+ *
+ * <p>Append other nodes by using Append().
+ */
+class MechanismRoot2d : private MechanismObject2d {
+  friend class Mechanism2d;
+  struct private_init {};
+
+ public:
+  MechanismRoot2d(std::string_view name, double x, double y,
+                  const private_init&);
+
+  /**
+   * Set the root's position.
+   *
+   * @param x new x coordinate
+   * @param y new y coordinate
+   */
+  void SetPosition(double x, double y);
+
+  using MechanismObject2d::GetName;
+
+  using MechanismObject2d::Append;
+
+ private:
+  void UpdateEntries(std::shared_ptr<nt::NetworkTable> table) override;
+  inline void Flush();
+  double m_x;
+  double m_y;
+  nt::NetworkTableEntry m_xEntry;
+  nt::NetworkTableEntry m_yEntry;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h
deleted file mode 100644
index 5000855..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/Sendable.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-namespace frc {
-
-class SendableBuilder;
-
-/**
- * Interface for Sendable objects.
- */
-class Sendable {
- public:
-  virtual ~Sendable() = default;
-
-  /**
-   * Initializes this Sendable object.
-   *
-   * @param builder sendable builder
-   */
-  virtual void InitSendable(SendableBuilder& builder) = 0;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
deleted file mode 100644
index 98419b9..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBase.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/deprecated.h>
-
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
-
-class SendableBase : public Sendable, public SendableHelper<SendableBase> {
- public:
-  /**
-   * Creates an instance of the sensor base
-   *
-   * @deprecated use Sendable and SendableHelper
-   *
-   * @param addLiveWindow if true, add this Sendable to LiveWindow
-   */
-  WPI_DEPRECATED("use Sendable and SendableHelper")
-  explicit SendableBase(bool addLiveWindow = true);
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilder.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilder.h
deleted file mode 100644
index 3f5f892..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilder.h
+++ /dev/null
@@ -1,222 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <functional>
-#include <memory>
-#include <string>
-#include <vector>
-
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableValue.h>
-#include <wpi/ArrayRef.h>
-#include <wpi/SmallVector.h>
-#include <wpi/Twine.h>
-
-namespace frc {
-
-class SendableBuilder {
- public:
-  virtual ~SendableBuilder() = default;
-
-  /**
-   * Set the string representation of the named data type that will be used
-   * by the smart dashboard for this sendable.
-   *
-   * @param type    data type
-   */
-  virtual void SetSmartDashboardType(const wpi::Twine& type) = 0;
-
-  /**
-   * Set a flag indicating if this sendable should be treated as an actuator.
-   * By default this flag is false.
-   *
-   * @param value   true if actuator, false if not
-   */
-  virtual void SetActuator(bool value) = 0;
-
-  /**
-   * Set the function that should be called to set the Sendable into a safe
-   * state.  This is called when entering and exiting Live Window mode.
-   *
-   * @param func    function
-   */
-  virtual void SetSafeState(std::function<void()> func) = 0;
-
-  /**
-   * Set the function that should be called to update the network table
-   * for things other than properties.  Note this function is not passed
-   * the network table object; instead it should use the entry handles
-   * returned by GetEntry().
-   *
-   * @param func    function
-   */
-  virtual void SetUpdateTable(std::function<void()> func) = 0;
-
-  /**
-   * Add a property without getters or setters.  This can be used to get
-   * entry handles for the function called by SetUpdateTable().
-   *
-   * @param key   property name
-   * @return Network table entry
-   */
-  virtual nt::NetworkTableEntry GetEntry(const wpi::Twine& key) = 0;
-
-  /**
-   * Add a boolean property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddBooleanProperty(const wpi::Twine& key,
-                                  std::function<bool()> getter,
-                                  std::function<void(bool)> setter) = 0;
-
-  /**
-   * Add a double property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddDoubleProperty(const wpi::Twine& key,
-                                 std::function<double()> getter,
-                                 std::function<void(double)> setter) = 0;
-
-  /**
-   * Add a string property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddStringProperty(
-      const wpi::Twine& key, std::function<std::string()> getter,
-      std::function<void(wpi::StringRef)> setter) = 0;
-
-  /**
-   * Add a boolean array property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddBooleanArrayProperty(
-      const wpi::Twine& key, std::function<std::vector<int>()> getter,
-      std::function<void(wpi::ArrayRef<int>)> setter) = 0;
-
-  /**
-   * Add a double array property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddDoubleArrayProperty(
-      const wpi::Twine& key, std::function<std::vector<double>()> getter,
-      std::function<void(wpi::ArrayRef<double>)> setter) = 0;
-
-  /**
-   * Add a string array property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddStringArrayProperty(
-      const wpi::Twine& key, std::function<std::vector<std::string>()> getter,
-      std::function<void(wpi::ArrayRef<std::string>)> setter) = 0;
-
-  /**
-   * Add a raw property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddRawProperty(const wpi::Twine& key,
-                              std::function<std::string()> getter,
-                              std::function<void(wpi::StringRef)> setter) = 0;
-
-  /**
-   * Add a NetworkTableValue property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddValueProperty(
-      const wpi::Twine& key, std::function<std::shared_ptr<nt::Value>()> getter,
-      std::function<void(std::shared_ptr<nt::Value>)> setter) = 0;
-
-  /**
-   * Add a string property (SmallString form).
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddSmallStringProperty(
-      const wpi::Twine& key,
-      std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
-      std::function<void(wpi::StringRef)> setter) = 0;
-
-  /**
-   * Add a boolean array property (SmallVector form).
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddSmallBooleanArrayProperty(
-      const wpi::Twine& key,
-      std::function<wpi::ArrayRef<int>(wpi::SmallVectorImpl<int>& buf)> getter,
-      std::function<void(wpi::ArrayRef<int>)> setter) = 0;
-
-  /**
-   * Add a double array property (SmallVector form).
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddSmallDoubleArrayProperty(
-      const wpi::Twine& key,
-      std::function<wpi::ArrayRef<double>(wpi::SmallVectorImpl<double>& buf)>
-          getter,
-      std::function<void(wpi::ArrayRef<double>)> setter) = 0;
-
-  /**
-   * Add a string array property (SmallVector form).
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddSmallStringArrayProperty(
-      const wpi::Twine& key,
-      std::function<
-          wpi::ArrayRef<std::string>(wpi::SmallVectorImpl<std::string>& buf)>
-          getter,
-      std::function<void(wpi::ArrayRef<std::string>)> setter) = 0;
-
-  /**
-   * Add a raw property (SmallVector form).
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  virtual void AddSmallRawProperty(
-      const wpi::Twine& key,
-      std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
-      std::function<void(wpi::StringRef)> setter) = 0;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
index e5dea44..36830f6 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableBuilderImpl.h
@@ -1,30 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
+#include <networktables/NTSendableBuilder.h>
 #include <networktables/NetworkTable.h>
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableValue.h>
-#include <wpi/ArrayRef.h>
 #include <wpi/SmallVector.h>
-#include <wpi/Twine.h>
-
-#include "frc/smartdashboard/SendableBuilder.h"
+#include <wpi/span.h>
 
 namespace frc {
 
-class SendableBuilderImpl : public SendableBuilder {
+class SendableBuilderImpl : public nt::NTSendableBuilder {
  public:
   SendableBuilderImpl() = default;
   ~SendableBuilderImpl() override = default;
@@ -43,13 +39,13 @@
    * Get the network table.
    * @return The network table
    */
-  std::shared_ptr<nt::NetworkTable> GetTable();
+  std::shared_ptr<nt::NetworkTable> GetTable() override;
 
   /**
    * Return whether this sendable has an associated table.
    * @return True if it has a table, false if not.
    */
-  bool HasTable() const;
+  bool IsPublished() const override;
 
   /**
    * Return whether this sendable should be treated as an actuator.
@@ -60,7 +56,7 @@
   /**
    * Update the network table values by calling the getters for all properties.
    */
-  void UpdateTable();
+  void Update() override;
 
   /**
    * Hook setters for all properties.
@@ -87,75 +83,75 @@
   /**
    * Clear properties.
    */
-  void ClearProperties();
+  void ClearProperties() override;
 
-  void SetSmartDashboardType(const wpi::Twine& type) override;
+  void SetSmartDashboardType(std::string_view type) override;
   void SetActuator(bool value) override;
   void SetSafeState(std::function<void()> func) override;
   void SetUpdateTable(std::function<void()> func) override;
-  nt::NetworkTableEntry GetEntry(const wpi::Twine& key) override;
+  nt::NetworkTableEntry GetEntry(std::string_view key) override;
 
-  void AddBooleanProperty(const wpi::Twine& key, std::function<bool()> getter,
+  void AddBooleanProperty(std::string_view key, std::function<bool()> getter,
                           std::function<void(bool)> setter) override;
 
-  void AddDoubleProperty(const wpi::Twine& key, std::function<double()> getter,
+  void AddDoubleProperty(std::string_view key, std::function<double()> getter,
                          std::function<void(double)> setter) override;
 
-  void AddStringProperty(const wpi::Twine& key,
+  void AddStringProperty(std::string_view key,
                          std::function<std::string()> getter,
-                         std::function<void(wpi::StringRef)> setter) override;
+                         std::function<void(std::string_view)> setter) override;
 
   void AddBooleanArrayProperty(
-      const wpi::Twine& key, std::function<std::vector<int>()> getter,
-      std::function<void(wpi::ArrayRef<int>)> setter) override;
+      std::string_view key, std::function<std::vector<int>()> getter,
+      std::function<void(wpi::span<const int>)> setter) override;
 
   void AddDoubleArrayProperty(
-      const wpi::Twine& key, std::function<std::vector<double>()> getter,
-      std::function<void(wpi::ArrayRef<double>)> setter) override;
+      std::string_view key, std::function<std::vector<double>()> getter,
+      std::function<void(wpi::span<const double>)> setter) override;
 
   void AddStringArrayProperty(
-      const wpi::Twine& key, std::function<std::vector<std::string>()> getter,
-      std::function<void(wpi::ArrayRef<std::string>)> setter) override;
+      std::string_view key, std::function<std::vector<std::string>()> getter,
+      std::function<void(wpi::span<const std::string>)> setter) override;
 
-  void AddRawProperty(const wpi::Twine& key,
-                      std::function<std::string()> getter,
-                      std::function<void(wpi::StringRef)> setter) override;
+  void AddRawProperty(std::string_view key, std::function<std::string()> getter,
+                      std::function<void(std::string_view)> setter) override;
 
   void AddValueProperty(
-      const wpi::Twine& key, std::function<std::shared_ptr<nt::Value>()> getter,
+      std::string_view key, std::function<std::shared_ptr<nt::Value>()> getter,
       std::function<void(std::shared_ptr<nt::Value>)> setter) override;
 
   void AddSmallStringProperty(
-      const wpi::Twine& key,
-      std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
-      std::function<void(wpi::StringRef)> setter) override;
+      std::string_view key,
+      std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+      std::function<void(std::string_view)> setter) override;
 
   void AddSmallBooleanArrayProperty(
-      const wpi::Twine& key,
-      std::function<wpi::ArrayRef<int>(wpi::SmallVectorImpl<int>& buf)> getter,
-      std::function<void(wpi::ArrayRef<int>)> setter) override;
+      std::string_view key,
+      std::function<wpi::span<const int>(wpi::SmallVectorImpl<int>& buf)>
+          getter,
+      std::function<void(wpi::span<const int>)> setter) override;
 
   void AddSmallDoubleArrayProperty(
-      const wpi::Twine& key,
-      std::function<wpi::ArrayRef<double>(wpi::SmallVectorImpl<double>& buf)>
+      std::string_view key,
+      std::function<wpi::span<const double>(wpi::SmallVectorImpl<double>& buf)>
           getter,
-      std::function<void(wpi::ArrayRef<double>)> setter) override;
+      std::function<void(wpi::span<const double>)> setter) override;
 
   void AddSmallStringArrayProperty(
-      const wpi::Twine& key,
+      std::string_view key,
       std::function<
-          wpi::ArrayRef<std::string>(wpi::SmallVectorImpl<std::string>& buf)>
+          wpi::span<const std::string>(wpi::SmallVectorImpl<std::string>& buf)>
           getter,
-      std::function<void(wpi::ArrayRef<std::string>)> setter) override;
+      std::function<void(wpi::span<const std::string>)> setter) override;
 
   void AddSmallRawProperty(
-      const wpi::Twine& key,
-      std::function<wpi::StringRef(wpi::SmallVectorImpl<char>& buf)> getter,
-      std::function<void(wpi::StringRef)> setter) override;
+      std::string_view key,
+      std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+      std::function<void(std::string_view)> setter) override;
 
  private:
   struct Property {
-    Property(nt::NetworkTable& table, const wpi::Twine& key)
+    Property(nt::NetworkTable& table, std::string_view key)
         : entry(table.GetEntry(key)) {}
 
     Property(const Property&) = delete;
@@ -183,8 +179,9 @@
     ~Property() { StopListener(); }
 
     void StartListener() {
-      if (entry && listener == 0 && createListener)
+      if (entry && listener == 0 && createListener) {
         listener = createListener(entry);
+      }
     }
 
     void StopListener() {
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
index b370fc0..b693762 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.h
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
+#include <string_view>
 
 #include <wpi/StringMap.h>
-#include <wpi/StringRef.h>
 #include <wpi/deprecated.h>
 
-#include "frc/smartdashboard/SendableBuilder.h"
 #include "frc/smartdashboard/SendableChooserBase.h"
 
 namespace frc {
@@ -45,27 +41,21 @@
   static std::weak_ptr<U> _unwrap_smart_ptr(const std::shared_ptr<U>& value);
 
  public:
-  ~SendableChooser() override = default;
   SendableChooser() = default;
+  ~SendableChooser() override = 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); }
+  void AddOption(std::string_view name, T object);
 
   /**
    * Add the given object to the list of options and marks it as the default.
@@ -73,19 +63,55 @@
    * 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
+   */
+  void SetDefaultOption(std::string_view 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(std::string_view name, T object) instead.
+   *
+   * @param name   the name of the option
+   * @param object the option
+   */
+  WPI_DEPRECATED("use AddOption() instead")
+  void AddObject(std::string_view 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(std::string_view 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) {
+  void AddDefault(std::string_view name, T object) {
     SetDefaultOption(name, object);
   }
 
+  /**
+   * Returns a copy of the selected option (a raw pointer U* if T =
+   * std::unique_ptr<U> or a std::weak_ptr<U> if T = std::shared_ptr<U>).
+   *
+   * If there is none selected, it will return the default. If there is none
+   * selected and no default, then it will return a value-initialized instance.
+   * For integer types, this is 0. For container types like std::string, this is
+   * an empty string.
+   *
+   * @return The option selected
+   */
   auto GetSelected() -> decltype(_unwrap_smart_ptr(m_choices[""]));
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(nt::NTSendableBuilder& builder) override;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
index 57e2828..25e2551 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooser.inc
@@ -1,69 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <algorithm>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
-#include <wpi/StringRef.h>
+#include <networktables/NTSendableBuilder.h>
+
+#include "frc/smartdashboard/SendableChooser.h"
 
 namespace frc {
 
-/**
- * Adds the given object to the list of options.
- *
- * On the SmartDashboard on the desktop, the object will appear as the given
- * name.
- *
- * @param name   the name of the option
- * @param object the option
- */
 template <class T>
-void SendableChooser<T>::AddOption(wpi::StringRef name, T object) {
+void SendableChooser<T>::AddOption(std::string_view name, T object) {
   m_choices[name] = std::move(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.
- *
- * @param name   the name of the option
- * @param object the option
- */
 template <class T>
-void SendableChooser<T>::SetDefaultOption(wpi::StringRef name, T object) {
+void SendableChooser<T>::SetDefaultOption(std::string_view name, T object) {
   m_defaultChoice = name;
   AddOption(name, std::move(object));
 }
 
-/**
- * Returns a copy of the selected option (a raw pointer U* if T =
- * std::unique_ptr<U> or a std::weak_ptr<U> if T = std::shared_ptr<U>).
- *
- * If there is none selected, it will return the default. If there is none
- * selected and no default, then it will return a value-initialized instance.
- * For integer types, this is 0. For container types like std::string, this is
- * an empty string.
- *
- * @return The option selected
- */
 template <class T>
 auto SendableChooser<T>::GetSelected()
     -> decltype(_unwrap_smart_ptr(m_choices[""])) {
   std::string selected = m_defaultChoice;
   {
     std::scoped_lock lock(m_mutex);
-    if (m_haveSelected) selected = m_selected;
+    if (m_haveSelected) {
+      selected = m_selected;
+    }
   }
   if (selected.empty()) {
     return decltype(_unwrap_smart_ptr(m_choices[""])){};
@@ -73,15 +46,15 @@
 }
 
 template <class T>
-void SendableChooser<T>::InitSendable(SendableBuilder& builder) {
+void SendableChooser<T>::InitSendable(nt::NTSendableBuilder& 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());
+          keys.emplace_back(choice.first());
         }
 
         // Unlike std::map, wpi::StringMap elements
@@ -93,17 +66,17 @@
       nullptr);
   builder.AddSmallStringProperty(
       kDefault,
-      [=](wpi::SmallVectorImpl<char>&) -> wpi::StringRef {
+      [=](wpi::SmallVectorImpl<char>&) -> std::string_view {
         return m_defaultChoice;
       },
       nullptr);
   builder.AddSmallStringProperty(
       kActive,
-      [=](wpi::SmallVectorImpl<char>& buf) -> wpi::StringRef {
+      [=](wpi::SmallVectorImpl<char>& buf) -> std::string_view {
         std::scoped_lock lock(m_mutex);
         if (m_haveSelected) {
           buf.assign(m_selected.begin(), m_selected.end());
-          return wpi::StringRef(buf.data(), buf.size());
+          return {buf.data(), buf.size()};
         } else {
           return m_defaultChoice;
         }
@@ -113,11 +86,13 @@
     std::scoped_lock lock(m_mutex);
     m_activeEntries.emplace_back(builder.GetEntry(kActive));
   }
-  builder.AddStringProperty(kSelected, nullptr, [=](wpi::StringRef val) {
+  builder.AddStringProperty(kSelected, nullptr, [=](std::string_view val) {
     std::scoped_lock lock(m_mutex);
     m_haveSelected = true;
     m_selected = val;
-    for (auto& entry : m_activeEntries) entry.SetString(val);
+    for (auto& entry : m_activeEntries) {
+      entry.SetString(val);
+    }
   });
 }
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h
index 2a5f5ab..78f891a 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableChooserBase.h
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <atomic>
 #include <string>
 
+#include <networktables/NTSendable.h>
 #include <networktables/NetworkTableEntry.h>
 #include <wpi/SmallVector.h>
 #include <wpi/mutex.h>
-
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+#include <wpi/sendable/SendableHelper.h>
 
 namespace frc {
 
@@ -25,8 +21,8 @@
  * It contains static, non-templated variables to avoid their duplication in the
  * template class.
  */
-class SendableChooserBase : public Sendable,
-                            public SendableHelper<SendableChooserBase> {
+class SendableChooserBase : public nt::NTSendable,
+                            public wpi::SendableHelper<SendableChooserBase> {
  public:
   SendableChooserBase();
   ~SendableChooserBase() override = default;
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
deleted file mode 100644
index 1f39216..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableHelper.h
+++ /dev/null
@@ -1,179 +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 <string>
-
-#include <wpi/Twine.h>
-#include <wpi/deprecated.h>
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-namespace frc {
-
-/**
- * A helper class for use with objects that add themselves to SendableRegistry.
- * It takes care of properly calling Move() and Remove() on move and
- * destruction.  No action is taken if the object is copied.
- * Use public inheritance with CRTP when using this class.
- * @tparam CRTP derived class
- */
-template <typename Derived>
-class SendableHelper {
- public:
-  SendableHelper(const SendableHelper& rhs) = default;
-  SendableHelper& operator=(const SendableHelper& rhs) = default;
-
-  SendableHelper(SendableHelper&& rhs) {
-    // it is safe to call Move() multiple times with the same rhs
-    SendableRegistry::GetInstance().Move(static_cast<Derived*>(this),
-                                         static_cast<Derived*>(&rhs));
-  }
-
-  SendableHelper& operator=(SendableHelper&& rhs) {
-    // it is safe to call Move() multiple times with the same rhs
-    SendableRegistry::GetInstance().Move(static_cast<Derived*>(this),
-                                         static_cast<Derived*>(&rhs));
-    return *this;
-  }
-
-  /**
-   * Gets the name of this Sendable object.
-   *
-   * @deprecated use SendableRegistry::GetName()
-   *
-   * @return Name
-   */
-  WPI_DEPRECATED("use SendableRegistry::GetName()")
-  std::string GetName() const {
-    return SendableRegistry::GetInstance().GetName(
-        static_cast<const Derived*>(this));
-  }
-
-  /**
-   * Sets the name of this Sendable object.
-   *
-   * @deprecated use SendableRegistry::SetName()
-   *
-   * @param name name
-   */
-  WPI_DEPRECATED("use SendableRegistry::SetName()")
-  void SetName(const wpi::Twine& name) {
-    SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this), name);
-  }
-
-  /**
-   * Sets both the subsystem name and device name of this Sendable object.
-   *
-   * @deprecated use SendableRegistry::SetName()
-   *
-   * @param subsystem subsystem name
-   * @param name device name
-   */
-  WPI_DEPRECATED("use SendableRegistry::SetName()")
-  void SetName(const wpi::Twine& subsystem, const wpi::Twine& name) {
-    SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this),
-                                            subsystem, name);
-  }
-
-  /**
-   * Gets the subsystem name of this Sendable object.
-   *
-   * @deprecated use SendableRegistry::GetSubsystem().
-   *
-   * @return Subsystem name
-   */
-  WPI_DEPRECATED("use SendableRegistry::GetSubsystem()")
-  std::string GetSubsystem() const {
-    return SendableRegistry::GetInstance().GetSubsystem(
-        static_cast<const Derived*>(this));
-  }
-
-  /**
-   * Sets the subsystem name of this Sendable object.
-   *
-   * @deprecated use SendableRegistry::SetSubsystem()
-   *
-   * @param subsystem subsystem name
-   */
-  WPI_DEPRECATED("use SendableRegistry::SetSubsystem()")
-  void SetSubsystem(const wpi::Twine& subsystem) {
-    SendableRegistry::GetInstance().SetSubsystem(static_cast<Derived*>(this),
-                                                 subsystem);
-  }
-
- protected:
-  /**
-   * Add a child component.
-   *
-   * @deprecated use SendableRegistry::AddChild()
-   *
-   * @param child child component
-   */
-  WPI_DEPRECATED("use SendableRegistry::AddChild()")
-  void AddChild(std::shared_ptr<Sendable> child) {
-    SendableRegistry::GetInstance().AddChild(static_cast<Derived*>(this),
-                                             child.get());
-  }
-
-  /**
-   * Add a child component.
-   *
-   * @deprecated use SendableRegistry::AddChild()
-   *
-   * @param child child component
-   */
-  WPI_DEPRECATED("use SendableRegistry::AddChild()")
-  void AddChild(void* child) {
-    SendableRegistry::GetInstance().AddChild(static_cast<Derived*>(this),
-                                             child);
-  }
-
-  /**
-   * 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
-   */
-  WPI_DEPRECATED("use SendableRegistry::SetName()")
-  void SetName(const wpi::Twine& moduleType, int channel) {
-    SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this),
-                                            moduleType, channel);
-  }
-
-  /**
-   * 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
-   * @param channel      The channel number the device is plugged into (usually
-   * PWM)
-   */
-  WPI_DEPRECATED("use SendableRegistry::SetName()")
-  void SetName(const wpi::Twine& moduleType, int moduleNumber, int channel) {
-    SendableRegistry::GetInstance().SetName(static_cast<Derived*>(this),
-                                            moduleType, moduleNumber, channel);
-  }
-
- protected:
-  SendableHelper() = default;
-
-  ~SendableHelper() {
-    // it is safe to call Remove() multiple times with the same object
-    SendableRegistry::GetInstance().Remove(static_cast<Derived*>(this));
-  }
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
deleted file mode 100644
index 78dabc6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SendableRegistry.h
+++ /dev/null
@@ -1,341 +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 <string>
-
-#include <networktables/NetworkTable.h>
-#include <wpi/STLExtras.h>
-#include <wpi/Twine.h>
-
-namespace frc {
-
-class Sendable;
-class SendableBuilderImpl;
-
-/**
- * The SendableRegistry class is the public interface for registering sensors
- * and actuators for use on dashboards and LiveWindow.
- */
-class SendableRegistry {
- public:
-  SendableRegistry(const SendableRegistry&) = delete;
-  SendableRegistry& operator=(const SendableRegistry&) = delete;
-
-  using UID = size_t;
-
-  /**
-   * Gets an instance of the SendableRegistry class.
-   *
-   * This is a singleton to guarantee that there is only a single instance
-   * regardless of how many times GetInstance is called.
-   */
-  static SendableRegistry& GetInstance();
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable object to add
-   * @param name component name
-   */
-  void Add(Sendable* sendable, const wpi::Twine& name);
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable     object to add
-   * @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
-   */
-  void Add(Sendable* sendable, const wpi::Twine& moduleType, int channel);
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable     object to add
-   * @param moduleType   A string that defines the module name in the label for
-   *                     the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into
-   */
-  void Add(Sendable* sendable, const wpi::Twine& moduleType, int moduleNumber,
-           int channel);
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable object to add
-   * @param subsystem subsystem name
-   * @param name component name
-   */
-  void Add(Sendable* sendable, const wpi::Twine& subsystem,
-           const wpi::Twine& name);
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable object to add
-   * @param name component name
-   */
-  void AddLW(Sendable* sendable, const wpi::Twine& name);
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable     object to add
-   * @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
-   */
-  void AddLW(Sendable* sendable, const wpi::Twine& moduleType, int channel);
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable     object to add
-   * @param moduleType   A string that defines the module name in the label for
-   *                     the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into
-   */
-  void AddLW(Sendable* sendable, const wpi::Twine& moduleType, int moduleNumber,
-             int channel);
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable object to add
-   * @param subsystem subsystem name
-   * @param name component name
-   */
-  void AddLW(Sendable* sendable, const wpi::Twine& subsystem,
-             const wpi::Twine& name);
-
-  /**
-   * Adds a child object to an object.  Adds the child object to the registry
-   * if it's not already present.
-   *
-   * @param parent parent object
-   * @param child child object
-   */
-  void AddChild(Sendable* parent, Sendable* child);
-
-  /**
-   * Adds a child object to an object.  Adds the child object to the registry
-   * if it's not already present.
-   *
-   * @param parent parent object
-   * @param child child object
-   */
-  void AddChild(Sendable* parent, void* child);
-
-  /**
-   * Removes an object from the registry.
-   *
-   * @param sendable object to remove
-   * @return true if the object was removed; false if it was not present
-   */
-  bool Remove(Sendable* sendable);
-
-  /**
-   * Moves an object in the registry (for use in move constructors/assignments).
-   *
-   * @param to new object
-   * @param from old object
-   */
-  void Move(Sendable* to, Sendable* from);
-
-  /**
-   * Determines if an object is in the registry.
-   *
-   * @param sendable object to check
-   * @return True if in registry, false if not.
-   */
-  bool Contains(const Sendable* sendable) const;
-
-  /**
-   * Gets the name of an object.
-   *
-   * @param sendable object
-   * @return Name (empty if object is not in registry)
-   */
-  std::string GetName(const Sendable* sendable) const;
-
-  /**
-   * Sets the name of an object.
-   *
-   * @param sendable object
-   * @param name name
-   */
-  void SetName(Sendable* sendable, const wpi::Twine& name);
-
-  /**
-   * Sets the name of an object with a channel number.
-   *
-   * @param sendable   object
-   * @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
-   */
-  void SetName(Sendable* sendable, const wpi::Twine& moduleType, int channel);
-
-  /**
-   * Sets the name of an object with a module and channel number.
-   *
-   * @param sendable     object
-   * @param moduleType   A string that defines the module name in the label for
-   *                     the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into
-   */
-  void SetName(Sendable* sendable, const wpi::Twine& moduleType,
-               int moduleNumber, int channel);
-
-  /**
-   * Sets both the subsystem name and device name of an object.
-   *
-   * @param sendable object
-   * @param subsystem subsystem name
-   * @param name device name
-   */
-  void SetName(Sendable* sendable, const wpi::Twine& subsystem,
-               const wpi::Twine& name);
-
-  /**
-   * Gets the subsystem name of an object.
-   *
-   * @param sendable object
-   * @return Subsystem name (empty if object is not in registry)
-   */
-  std::string GetSubsystem(const Sendable* sendable) const;
-
-  /**
-   * Sets the subsystem name of an object.
-   *
-   * @param sendable object
-   * @param subsystem subsystem name
-   */
-  void SetSubsystem(Sendable* sendable, const wpi::Twine& subsystem);
-
-  /**
-   * Gets a unique handle for setting/getting data with SetData() and GetData().
-   *
-   * @return Handle
-   */
-  int GetDataHandle();
-
-  /**
-   * Associates arbitrary data with an object in the registry.
-   *
-   * @param sendable object
-   * @param handle data handle returned by GetDataHandle()
-   * @param data data to set
-   * @return Previous data (may be null)
-   */
-  std::shared_ptr<void> SetData(Sendable* sendable, int handle,
-                                std::shared_ptr<void> data);
-
-  /**
-   * Gets arbitrary data associated with an object in the registry.
-   *
-   * @param sendable object
-   * @param handle data handle returned by GetDataHandle()
-   * @return data (may be null if none associated)
-   */
-  std::shared_ptr<void> GetData(Sendable* sendable, int handle);
-
-  /**
-   * Enables LiveWindow for an object.
-   *
-   * @param sendable object
-   */
-  void EnableLiveWindow(Sendable* sendable);
-
-  /**
-   * Disables LiveWindow for an object.
-   *
-   * @param sendable object
-   */
-  void DisableLiveWindow(Sendable* sendable);
-
-  /**
-   * Get unique id for an object.  Since objects can move, use this instead
-   * of storing Sendable* directly if ownership is in question.
-   *
-   * @param sendable object
-   * @return unique id
-   */
-  UID GetUniqueId(Sendable* sendable);
-
-  /**
-   * Get sendable object for a given unique id.
-   *
-   * @param uid unique id
-   * @return sendable object (may be null)
-   */
-  Sendable* GetSendable(UID uid);
-
-  /**
-   * Publishes an object in the registry to a network table.
-   *
-   * @param sendableUid sendable unique id
-   * @param table network table
-   */
-  void Publish(UID sendableUid, std::shared_ptr<NetworkTable> table);
-
-  /**
-   * Updates network table information from an object.
-   *
-   * @param sendableUid sendable unique id
-   */
-  void Update(UID sendableUid);
-
-  /**
-   * Data passed to ForeachLiveWindow() callback function
-   */
-  struct CallbackData {
-    CallbackData(Sendable* sendable_, wpi::StringRef name_,
-                 wpi::StringRef subsystem_, Sendable* parent_,
-                 std::shared_ptr<void>& data_, SendableBuilderImpl& builder_)
-        : sendable(sendable_),
-          name(name_),
-          subsystem(subsystem_),
-          parent(parent_),
-          data(data_),
-          builder(builder_) {}
-
-    Sendable* sendable;
-    wpi::StringRef name;
-    wpi::StringRef subsystem;
-    Sendable* parent;
-    std::shared_ptr<void>& data;
-    SendableBuilderImpl& builder;
-  };
-
-  /**
-   * Iterates over LiveWindow-enabled objects in the registry.
-   * It is *not* safe to call other SendableRegistry functions from the
-   * callback (this will likely deadlock).
-   *
-   * @param dataHandle data handle to get data pointer passed to callback
-   * @param callback function to call for each object
-   */
-  void ForeachLiveWindow(
-      int dataHandle,
-      wpi::function_ref<void(CallbackData& cbdata)> callback) const;
-
- private:
-  SendableRegistry();
-
-  struct Impl;
-  std::unique_ptr<Impl> m_impl;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
index 9fdc049..47c4a28 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/smartdashboard/SmartDashboard.h
@@ -1,30 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2011-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 #include <string>
+#include <string_view>
 #include <vector>
 
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableValue.h>
+#include <wpi/span.h>
 
-#include "frc/ErrorBase.h"
-#include "frc/smartdashboard/ListenerExecutor.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableHelper.h"
+namespace wpi {
+class Sendable;
+}  // namespace wpi
 
 namespace frc {
 
-class SmartDashboard : public ErrorBase,
-                       public Sendable,
-                       public SendableHelper<SmartDashboard> {
+class SmartDashboard {
  public:
+  SmartDashboard() = delete;
+
   static void init();
 
   /**
@@ -33,7 +31,7 @@
    * @param key the key to search for
    * @return true if the table as a value assigned to the given key
    */
-  static bool ContainsKey(wpi::StringRef key);
+  static bool ContainsKey(std::string_view key);
 
   /**
    * @param types bitmask of types; 0 is treated as a "don't care".
@@ -46,7 +44,7 @@
    *
    * @param key the key to make persistent
    */
-  static void SetPersistent(wpi::StringRef key);
+  static void SetPersistent(std::string_view key);
 
   /**
    * Stop making a key's value persistent through program restarts.
@@ -54,7 +52,7 @@
    *
    * @param key the key name
    */
-  static void ClearPersistent(wpi::StringRef key);
+  static void ClearPersistent(std::string_view key);
 
   /**
    * Returns whether the value is persistent through program restarts.
@@ -62,7 +60,7 @@
    *
    * @param key the key name
    */
-  static bool IsPersistent(wpi::StringRef key);
+  static bool IsPersistent(std::string_view key);
 
   /**
    * Sets flags on the specified key in this table. The key can
@@ -71,7 +69,7 @@
    * @param key the key name
    * @param flags the flags to set (bitmask)
    */
-  static void SetFlags(wpi::StringRef key, unsigned int flags);
+  static void SetFlags(std::string_view key, unsigned int flags);
 
   /**
    * Clears flags on the specified key in this table. The key can
@@ -80,7 +78,7 @@
    * @param key the key name
    * @param flags the flags to clear (bitmask)
    */
-  static void ClearFlags(wpi::StringRef key, unsigned int flags);
+  static void ClearFlags(std::string_view key, unsigned int flags);
 
   /**
    * Returns the flags for the specified key.
@@ -88,14 +86,14 @@
    * @param key the key name
    * @return the flags, or 0 if the key is not defined
    */
-  static unsigned int GetFlags(wpi::StringRef key);
+  static unsigned int GetFlags(std::string_view key);
 
   /**
    * Deletes the specified key in this table.
    *
    * @param key the key name
    */
-  static void Delete(wpi::StringRef key);
+  static void Delete(std::string_view key);
 
   /**
    * Returns an NT Entry mapping to the specified key
@@ -105,7 +103,7 @@
    * @param key the key
    * @return    the entry for the key
    */
-  static nt::NetworkTableEntry GetEntry(wpi::StringRef key);
+  static nt::NetworkTableEntry GetEntry(std::string_view key);
 
   /**
    * Maps the specified key to the specified value in this table.
@@ -116,10 +114,10 @@
    * In order for the value to appear in the dashboard, it must be registered
    * with SendableRegistry.  WPILib components do this automatically.
    *
-   * @param keyName the key
-   * @param value   the value
+   * @param key  the key
+   * @param data the value
    */
-  static void PutData(wpi::StringRef key, Sendable* data);
+  static void PutData(std::string_view key, wpi::Sendable* data);
 
   /**
    * Maps the specified key (where the key is the name of the Sendable)
@@ -133,7 +131,7 @@
    *
    * @param value the value
    */
-  static void PutData(Sendable* value);
+  static void PutData(wpi::Sendable* value);
 
   /**
    * Returns the value at the specified key.
@@ -141,7 +139,7 @@
    * @param keyName the key
    * @return the value
    */
-  static Sendable* GetData(wpi::StringRef keyName);
+  static wpi::Sendable* GetData(std::string_view keyName);
 
   /**
    * Maps the specified key to the specified value in this table.
@@ -153,7 +151,7 @@
    * @param value   the value
    * @return        False if the table key already exists with a different type
    */
-  static bool PutBoolean(wpi::StringRef keyName, bool value);
+  static bool PutBoolean(std::string_view keyName, bool value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -161,7 +159,7 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultBoolean(wpi::StringRef key, bool defaultValue);
+  static bool SetDefaultBoolean(std::string_view key, bool defaultValue);
 
   /**
    * Returns the value at the specified key.
@@ -169,9 +167,10 @@
    * If the key is not found, returns the default value.
    *
    * @param keyName the key
+   * @param defaultValue the default value to set if key doesn't exist
    * @return the value
    */
-  static bool GetBoolean(wpi::StringRef keyName, bool defaultValue);
+  static bool GetBoolean(std::string_view keyName, bool defaultValue);
 
   /**
    * Maps the specified key to the specified value in this table.
@@ -183,7 +182,7 @@
    * @param value   the value
    * @return        False if the table key already exists with a different type
    */
-  static bool PutNumber(wpi::StringRef keyName, double value);
+  static bool PutNumber(std::string_view keyName, double value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -192,7 +191,7 @@
    * @param defaultValue The default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultNumber(wpi::StringRef key, double defaultValue);
+  static bool SetDefaultNumber(std::string_view key, double defaultValue);
 
   /**
    * Returns the value at the specified key.
@@ -200,9 +199,10 @@
    * If the key is not found, returns the default value.
    *
    * @param keyName the key
+   * @param defaultValue the default value to set if the key doesn't exist
    * @return the value
    */
-  static double GetNumber(wpi::StringRef keyName, double defaultValue);
+  static double GetNumber(std::string_view keyName, double defaultValue);
 
   /**
    * Maps the specified key to the specified value in this table.
@@ -214,7 +214,7 @@
    * @param value   the value
    * @return        False if the table key already exists with a different type
    */
-  static bool PutString(wpi::StringRef keyName, wpi::StringRef value);
+  static bool PutString(std::string_view keyName, std::string_view value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -223,7 +223,8 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultString(wpi::StringRef key, wpi::StringRef defaultValue);
+  static bool SetDefaultString(std::string_view key,
+                               std::string_view defaultValue);
 
   /**
    * Returns the value at the specified key.
@@ -231,10 +232,11 @@
    * If the key is not found, returns the default value.
    *
    * @param keyName the key
+   * @param defaultValue the default value to set if the key doesn't exist
    * @return the value
    */
-  static std::string GetString(wpi::StringRef keyName,
-                               wpi::StringRef defaultValue);
+  static std::string GetString(std::string_view keyName,
+                               std::string_view defaultValue);
 
   /**
    * Put a boolean array in the table.
@@ -247,7 +249,7 @@
    *       std::vector<bool> is special-cased in C++. 0 is false, any
    *       non-zero value is true.
    */
-  static bool PutBooleanArray(wpi::StringRef key, wpi::ArrayRef<int> value);
+  static bool PutBooleanArray(std::string_view key, wpi::span<const int> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -256,8 +258,8 @@
    * @param defaultValue the default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultBooleanArray(wpi::StringRef key,
-                                     wpi::ArrayRef<int> defaultValue);
+  static bool SetDefaultBooleanArray(std::string_view key,
+                                     wpi::span<const int> defaultValue);
 
   /**
    * Returns the boolean array the key maps to.
@@ -277,8 +279,8 @@
    *       because std::vector<bool> is special-cased in C++. 0 is false, any
    *       non-zero value is true.
    */
-  static std::vector<int> GetBooleanArray(wpi::StringRef key,
-                                          wpi::ArrayRef<int> defaultValue);
+  static std::vector<int> GetBooleanArray(std::string_view key,
+                                          wpi::span<const int> defaultValue);
 
   /**
    * Put a number array in the table.
@@ -287,7 +289,8 @@
    * @param value The value that will be assigned.
    * @return False if the table key already exists with a different type
    */
-  static bool PutNumberArray(wpi::StringRef key, wpi::ArrayRef<double> value);
+  static bool PutNumberArray(std::string_view key,
+                             wpi::span<const double> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -296,8 +299,8 @@
    * @param defaultValue The default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultNumberArray(wpi::StringRef key,
-                                    wpi::ArrayRef<double> defaultValue);
+  static bool SetDefaultNumberArray(std::string_view key,
+                                    wpi::span<const double> defaultValue);
 
   /**
    * Returns the number array the key maps to.
@@ -313,8 +316,8 @@
    * @note This makes a copy of the array. If the overhead of this is a concern,
    *       use GetValue() instead.
    */
-  static std::vector<double> GetNumberArray(wpi::StringRef key,
-                                            wpi::ArrayRef<double> defaultValue);
+  static std::vector<double> GetNumberArray(
+      std::string_view key, wpi::span<const double> defaultValue);
 
   /**
    * Put a string array in the table.
@@ -323,8 +326,8 @@
    * @param value The value that will be assigned.
    * @return False if the table key already exists with a different type
    */
-  static bool PutStringArray(wpi::StringRef key,
-                             wpi::ArrayRef<std::string> value);
+  static bool PutStringArray(std::string_view key,
+                             wpi::span<const std::string> value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -333,8 +336,8 @@
    * @param defaultValue The default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultStringArray(wpi::StringRef key,
-                                    wpi::ArrayRef<std::string> defaultValue);
+  static bool SetDefaultStringArray(std::string_view key,
+                                    wpi::span<const std::string> defaultValue);
 
   /**
    * Returns the string array the key maps to.
@@ -351,7 +354,7 @@
    *       use GetValue() instead.
    */
   static std::vector<std::string> GetStringArray(
-      wpi::StringRef key, wpi::ArrayRef<std::string> defaultValue);
+      std::string_view key, wpi::span<const std::string> defaultValue);
 
   /**
    * Put a raw value (byte array) in the table.
@@ -360,7 +363,7 @@
    * @param value The value that will be assigned.
    * @return False if the table key already exists with a different type
    */
-  static bool PutRaw(wpi::StringRef key, wpi::StringRef value);
+  static bool PutRaw(std::string_view key, std::string_view value);
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
@@ -369,7 +372,8 @@
    * @param defaultValue The default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultRaw(wpi::StringRef key, wpi::StringRef defaultValue);
+  static bool SetDefaultRaw(std::string_view key,
+                            std::string_view defaultValue);
 
   /**
    * Returns the raw value (byte array) the key maps to.
@@ -385,7 +389,8 @@
    * @note This makes a copy of the raw contents. If the overhead of this is a
    *       concern, use GetValue() instead.
    */
-  static std::string GetRaw(wpi::StringRef key, wpi::StringRef defaultValue);
+  static std::string GetRaw(std::string_view key,
+                            std::string_view defaultValue);
 
   /**
    * Maps the specified key to the specified complex value (such as an array) in
@@ -398,7 +403,7 @@
    * @param value   the value
    * @return        False if the table key already exists with a different type
    */
-  static bool PutValue(wpi::StringRef keyName,
+  static bool PutValue(std::string_view keyName,
                        std::shared_ptr<nt::Value> value);
 
   /**
@@ -408,7 +413,7 @@
    * @param defaultValue The default value to set if key doesn't exist.
    * @returns False if the table key exists with a different type
    */
-  static bool SetDefaultValue(wpi::StringRef key,
+  static bool SetDefaultValue(std::string_view key,
                               std::shared_ptr<nt::Value> defaultValue);
 
   /**
@@ -416,14 +421,12 @@
    * complex data object.
    *
    * @param keyName the key
-   * @param value   the object to retrieve the value into
    */
-  static std::shared_ptr<nt::Value> GetValue(wpi::StringRef keyName);
+  static std::shared_ptr<nt::Value> GetValue(std::string_view keyName);
 
   /**
    * Posts a task from a listener to the ListenerExecutor, so that it can be run
-   * synchronously from the main loop on the next call to {@link
-   * SmartDashboard#updateValues()}.
+   * synchronously from the main loop on the next call to updateValues().
    *
    * @param task The task to run synchronously from the main thread.
    */
@@ -433,11 +436,6 @@
    * Puts all sendable data to the dashboard.
    */
   static void UpdateValues();
-
- private:
-  virtual ~SmartDashboard() = default;
-
-  static detail::ListenerExecutor listenerExecutor;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h
index 1321380..00bc4af 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -23,17 +20,17 @@
    */
 
   /**
-   * #1560BD.
+   * 0x1560BD.
    */
   static const Color kDenim;
 
   /**
-   * #0066B3.
+   * 0x0066B3.
    */
   static const Color kFirstBlue;
 
   /**
-   * #ED1C24.
+   * 0xED1C24.
    */
   static const Color kFirstRed;
 
@@ -42,702 +39,702 @@
    */
 
   /**
-   * #F0F8FF.
+   * 0xF0F8FF.
    */
   static const Color kAliceBlue;
 
   /**
-   * #FAEBD7.
+   * 0xFAEBD7.
    */
   static const Color kAntiqueWhite;
 
   /**
-   * #00FFFF.
+   * 0x00FFFF.
    */
   static const Color kAqua;
 
   /**
-   * #7FFFD4.
+   * 0x7FFFD4.
    */
   static const Color kAquamarine;
 
   /**
-   * #F0FFFF.
+   * 0xF0FFFF.
    */
   static const Color kAzure;
 
   /**
-   * #F5F5DC.
+   * 0xF5F5DC.
    */
   static const Color kBeige;
 
   /**
-   * #FFE4C4.
+   * 0xFFE4C4.
    */
   static const Color kBisque;
 
   /**
-   * #000000.
+   * 0x000000.
    */
   static const Color kBlack;
 
   /**
-   * #FFEBCD.
+   * 0xFFEBCD.
    */
   static const Color kBlanchedAlmond;
 
   /**
-   * #0000FF.
+   * 0x0000FF.
    */
   static const Color kBlue;
 
   /**
-   * #8A2BE2.
+   * 0x8A2BE2.
    */
   static const Color kBlueViolet;
 
   /**
-   * #A52A2A.
+   * 0xA52A2A.
    */
   static const Color kBrown;
 
   /**
-   * #DEB887.
+   * 0xDEB887.
    */
   static const Color kBurlywood;
 
   /**
-   * #5F9EA0.
+   * 0x5F9EA0.
    */
   static const Color kCadetBlue;
 
   /**
-   * #7FFF00.
+   * 0x7FFF00.
    */
   static const Color kChartreuse;
 
   /**
-   * #D2691E.
+   * 0xD2691E.
    */
   static const Color kChocolate;
 
   /**
-   * #FF7F50.
+   * 0xFF7F50.
    */
   static const Color kCoral;
 
   /**
-   * #6495ED.
+   * 0x6495ED.
    */
   static const Color kCornflowerBlue;
 
   /**
-   * #FFF8DC.
+   * 0xFFF8DC.
    */
   static const Color kCornsilk;
 
   /**
-   * #DC143C.
+   * 0xDC143C.
    */
   static const Color kCrimson;
 
   /**
-   * #00FFFF.
+   * 0x00FFFF.
    */
   static const Color kCyan;
 
   /**
-   * #00008B.
+   * 0x00008B.
    */
   static const Color kDarkBlue;
 
   /**
-   * #008B8B.
+   * 0x008B8B.
    */
   static const Color kDarkCyan;
 
   /**
-   * #B8860B.
+   * 0xB8860B.
    */
   static const Color kDarkGoldenrod;
 
   /**
-   * #A9A9A9.
+   * 0xA9A9A9.
    */
   static const Color kDarkGray;
 
   /**
-   * #006400.
+   * 0x006400.
    */
   static const Color kDarkGreen;
 
   /**
-   * #BDB76B.
+   * 0xBDB76B.
    */
   static const Color kDarkKhaki;
 
   /**
-   * #8B008B.
+   * 0x8B008B.
    */
   static const Color kDarkMagenta;
 
   /**
-   * #556B2F.
+   * 0x556B2F.
    */
   static const Color kDarkOliveGreen;
 
   /**
-   * #FF8C00.
+   * 0xFF8C00.
    */
   static const Color kDarkOrange;
 
   /**
-   * #9932CC.
+   * 0x9932CC.
    */
   static const Color kDarkOrchid;
 
   /**
-   * #8B0000.
+   * 0x8B0000.
    */
   static const Color kDarkRed;
 
   /**
-   * #E9967A.
+   * 0xE9967A.
    */
   static const Color kDarkSalmon;
 
   /**
-   * #8FBC8F.
+   * 0x8FBC8F.
    */
   static const Color kDarkSeaGreen;
 
   /**
-   * #483D8B.
+   * 0x483D8B.
    */
   static const Color kDarkSlateBlue;
 
   /**
-   * #2F4F4F.
+   * 0x2F4F4F.
    */
   static const Color kDarkSlateGray;
 
   /**
-   * #00CED1.
+   * 0x00CED1.
    */
   static const Color kDarkTurquoise;
 
   /**
-   * #9400D3.
+   * 0x9400D3.
    */
   static const Color kDarkViolet;
 
   /**
-   * #FF1493.
+   * 0xFF1493.
    */
   static const Color kDeepPink;
 
   /**
-   * #00BFFF.
+   * 0x00BFFF.
    */
   static const Color kDeepSkyBlue;
 
   /**
-   * #696969.
+   * 0x696969.
    */
   static const Color kDimGray;
 
   /**
-   * #1E90FF.
+   * 0x1E90FF.
    */
   static const Color kDodgerBlue;
 
   /**
-   * #B22222.
+   * 0xB22222.
    */
   static const Color kFirebrick;
 
   /**
-   * #FFFAF0.
+   * 0xFFFAF0.
    */
   static const Color kFloralWhite;
 
   /**
-   * #228B22.
+   * 0x228B22.
    */
   static const Color kForestGreen;
 
   /**
-   * #FF00FF.
+   * 0xFF00FF.
    */
   static const Color kFuchsia;
 
   /**
-   * #DCDCDC.
+   * 0xDCDCDC.
    */
   static const Color kGainsboro;
 
   /**
-   * #F8F8FF.
+   * 0xF8F8FF.
    */
   static const Color kGhostWhite;
 
   /**
-   * #FFD700.
+   * 0xFFD700.
    */
   static const Color kGold;
 
   /**
-   * #DAA520.
+   * 0xDAA520.
    */
   static const Color kGoldenrod;
 
   /**
-   * #808080.
+   * 0x808080.
    */
   static const Color kGray;
 
   /**
-   * #008000.
+   * 0x008000.
    */
   static const Color kGreen;
 
   /**
-   * #ADFF2F.
+   * 0xADFF2F.
    */
   static const Color kGreenYellow;
 
   /**
-   * #F0FFF0.
+   * 0xF0FFF0.
    */
   static const Color kHoneydew;
 
   /**
-   * #FF69B4.
+   * 0xFF69B4.
    */
   static const Color kHotPink;
 
   /**
-   * #CD5C5C.
+   * 0xCD5C5C.
    */
   static const Color kIndianRed;
 
   /**
-   * #4B0082.
+   * 0x4B0082.
    */
   static const Color kIndigo;
 
   /**
-   * #FFFFF0.
+   * 0xFFFFF0.
    */
   static const Color kIvory;
 
   /**
-   * #F0E68C.
+   * 0xF0E68C.
    */
   static const Color kKhaki;
 
   /**
-   * #E6E6FA.
+   * 0xE6E6FA.
    */
   static const Color kLavender;
 
   /**
-   * #FFF0F5.
+   * 0xFFF0F5.
    */
   static const Color kLavenderBlush;
 
   /**
-   * #7CFC00.
+   * 0x7CFC00.
    */
   static const Color kLawnGreen;
 
   /**
-   * #FFFACD.
+   * 0xFFFACD.
    */
   static const Color kLemonChiffon;
 
   /**
-   * #ADD8E6.
+   * 0xADD8E6.
    */
   static const Color kLightBlue;
 
   /**
-   * #F08080.
+   * 0xF08080.
    */
   static const Color kLightCoral;
 
   /**
-   * #E0FFFF.
+   * 0xE0FFFF.
    */
   static const Color kLightCyan;
 
   /**
-   * #FAFAD2.
+   * 0xFAFAD2.
    */
   static const Color kLightGoldenrodYellow;
 
   /**
-   * #D3D3D3.
+   * 0xD3D3D3.
    */
   static const Color kLightGray;
 
   /**
-   * #90EE90.
+   * 0x90EE90.
    */
   static const Color kLightGreen;
 
   /**
-   * #FFB6C1.
+   * 0xFFB6C1.
    */
   static const Color kLightPink;
 
   /**
-   * #FFA07A.
+   * 0xFFA07A.
    */
   static const Color kLightSalmon;
 
   /**
-   * #20B2AA.
+   * 0x20B2AA.
    */
   static const Color kLightSeaGreen;
 
   /**
-   * #87CEFA.
+   * 0x87CEFA.
    */
   static const Color kLightSkyBlue;
 
   /**
-   * #778899.
+   * 0x778899.
    */
   static const Color kLightSlateGray;
 
   /**
-   * #B0C4DE.
+   * 0xB0C4DE.
    */
   static const Color kLightSteelBlue;
 
   /**
-   * #FFFFE0.
+   * 0xFFFFE0.
    */
   static const Color kLightYellow;
 
   /**
-   * #00FF00.
+   * 0x00FF00.
    */
   static const Color kLime;
 
   /**
-   * #32CD32.
+   * 0x32CD32.
    */
   static const Color kLimeGreen;
 
   /**
-   * #FAF0E6.
+   * 0xFAF0E6.
    */
   static const Color kLinen;
 
   /**
-   * #FF00FF.
+   * 0xFF00FF.
    */
   static const Color kMagenta;
 
   /**
-   * #800000.
+   * 0x800000.
    */
   static const Color kMaroon;
 
   /**
-   * #66CDAA.
+   * 0x66CDAA.
    */
   static const Color kMediumAquamarine;
 
   /**
-   * #0000CD.
+   * 0x0000CD.
    */
   static const Color kMediumBlue;
 
   /**
-   * #BA55D3.
+   * 0xBA55D3.
    */
   static const Color kMediumOrchid;
 
   /**
-   * #9370DB.
+   * 0x9370DB.
    */
   static const Color kMediumPurple;
 
   /**
-   * #3CB371.
+   * 0x3CB371.
    */
   static const Color kMediumSeaGreen;
 
   /**
-   * #7B68EE.
+   * 0x7B68EE.
    */
   static const Color kMediumSlateBlue;
 
   /**
-   * #00FA9A.
+   * 0x00FA9A.
    */
   static const Color kMediumSpringGreen;
 
   /**
-   * #48D1CC.
+   * 0x48D1CC.
    */
   static const Color kMediumTurquoise;
 
   /**
-   * #C71585.
+   * 0xC71585.
    */
   static const Color kMediumVioletRed;
 
   /**
-   * #191970.
+   * 0x191970.
    */
   static const Color kMidnightBlue;
 
   /**
-   * #F5FFFA.
+   * 0xF5FFFA.
    */
   static const Color kMintcream;
 
   /**
-   * #FFE4E1.
+   * 0xFFE4E1.
    */
   static const Color kMistyRose;
 
   /**
-   * #FFE4B5.
+   * 0xFFE4B5.
    */
   static const Color kMoccasin;
 
   /**
-   * #FFDEAD.
+   * 0xFFDEAD.
    */
   static const Color kNavajoWhite;
 
   /**
-   * #000080.
+   * 0x000080.
    */
   static const Color kNavy;
 
   /**
-   * #FDF5E6.
+   * 0xFDF5E6.
    */
   static const Color kOldLace;
 
   /**
-   * #808000.
+   * 0x808000.
    */
   static const Color kOlive;
 
   /**
-   * #6B8E23.
+   * 0x6B8E23.
    */
   static const Color kOliveDrab;
 
   /**
-   * #FFA500.
+   * 0xFFA500.
    */
   static const Color kOrange;
 
   /**
-   * #FF4500.
+   * 0xFF4500.
    */
   static const Color kOrangeRed;
 
   /**
-   * #DA70D6.
+   * 0xDA70D6.
    */
   static const Color kOrchid;
 
   /**
-   * #EEE8AA.
+   * 0xEEE8AA.
    */
   static const Color kPaleGoldenrod;
 
   /**
-   * #98FB98.
+   * 0x98FB98.
    */
   static const Color kPaleGreen;
 
   /**
-   * #AFEEEE.
+   * 0xAFEEEE.
    */
   static const Color kPaleTurquoise;
 
   /**
-   * #DB7093.
+   * 0xDB7093.
    */
   static const Color kPaleVioletRed;
 
   /**
-   * #FFEFD5.
+   * 0xFFEFD5.
    */
   static const Color kPapayaWhip;
 
   /**
-   * #FFDAB9.
+   * 0xFFDAB9.
    */
   static const Color kPeachPuff;
 
   /**
-   * #CD853F.
+   * 0xCD853F.
    */
   static const Color kPeru;
 
   /**
-   * #FFC0CB.
+   * 0xFFC0CB.
    */
   static const Color kPink;
 
   /**
-   * #DDA0DD.
+   * 0xDDA0DD.
    */
   static const Color kPlum;
 
   /**
-   * #B0E0E6.
+   * 0xB0E0E6.
    */
   static const Color kPowderBlue;
 
   /**
-   * #800080.
+   * 0x800080.
    */
   static const Color kPurple;
 
   /**
-   * #FF0000.
+   * 0xFF0000.
    */
   static const Color kRed;
 
   /**
-   * #BC8F8F.
+   * 0xBC8F8F.
    */
   static const Color kRosyBrown;
 
   /**
-   * #4169E1.
+   * 0x4169E1.
    */
   static const Color kRoyalBlue;
 
   /**
-   * #8B4513.
+   * 0x8B4513.
    */
   static const Color kSaddleBrown;
 
   /**
-   * #FA8072.
+   * 0xFA8072.
    */
   static const Color kSalmon;
 
   /**
-   * #F4A460.
+   * 0xF4A460.
    */
   static const Color kSandyBrown;
 
   /**
-   * #2E8B57.
+   * 0x2E8B57.
    */
   static const Color kSeaGreen;
 
   /**
-   * #FFF5EE.
+   * 0xFFF5EE.
    */
   static const Color kSeashell;
 
   /**
-   * #A0522D.
+   * 0xA0522D.
    */
   static const Color kSienna;
 
   /**
-   * #C0C0C0.
+   * 0xC0C0C0.
    */
   static const Color kSilver;
 
   /**
-   * #87CEEB.
+   * 0x87CEEB.
    */
   static const Color kSkyBlue;
 
   /**
-   * #6A5ACD.
+   * 0x6A5ACD.
    */
   static const Color kSlateBlue;
 
   /**
-   * #708090.
+   * 0x708090.
    */
   static const Color kSlateGray;
 
   /**
-   * #FFFAFA.
+   * 0xFFFAFA.
    */
   static const Color kSnow;
 
   /**
-   * #00FF7F.
+   * 0x00FF7F.
    */
   static const Color kSpringGreen;
 
   /**
-   * #4682B4.
+   * 0x4682B4.
    */
   static const Color kSteelBlue;
 
   /**
-   * #D2B48C.
+   * 0xD2B48C.
    */
   static const Color kTan;
 
   /**
-   * #008080.
+   * 0x008080.
    */
   static const Color kTeal;
 
   /**
-   * #D8BFD8.
+   * 0xD8BFD8.
    */
   static const Color kThistle;
 
   /**
-   * #FF6347.
+   * 0xFF6347.
    */
   static const Color kTomato;
 
   /**
-   * #40E0D0.
+   * 0x40E0D0.
    */
   static const Color kTurquoise;
 
   /**
-   * #EE82EE.
+   * 0xEE82EE.
    */
   static const Color kViolet;
 
   /**
-   * #F5DEB3.
+   * 0xF5DEB3.
    */
   static const Color kWheat;
 
   /**
-   * #FFFFFF.
+   * 0xFFFFFF.
    */
   static const Color kWhite;
 
   /**
-   * #F5F5F5.
+   * 0xF5F5F5.
    */
   static const Color kWhiteSmoke;
 
   /**
-   * #FFFF00.
+   * 0xFFFF00.
    */
   static const Color kYellow;
 
   /**
-   * #9ACD32.
+   * 0x9ACD32.
    */
   static const Color kYellowGreen;
 
@@ -746,9 +743,9 @@
   /**
    * Constructs a Color.
    *
-   * @param red Red value (0-1)
-   * @param green Green value (0-1)
-   * @param blue Blue value (0-1)
+   * @param r Red value (0-1)
+   * @param g Green value (0-1)
+   * @param b Blue value (0-1)
    */
   constexpr Color(double r, double g, double b)
       : red(roundAndClamp(r)),
@@ -809,6 +806,10 @@
   return c1.red == c2.red && c1.green == c2.green && c1.blue == c2.blue;
 }
 
+inline bool operator!=(const Color& c1, const Color& c2) {
+  return !(c1 == c2);
+}
+
 /*
  * FIRST Colors
  */
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h
index d5dba07..62cbde9 100644
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h
+++ b/third_party/allwpilib/wpilibc/src/main/native/include/frc/util/Color8Bit.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -23,9 +20,9 @@
   /**
    * Constructs a Color8Bit.
    *
-   * @param red Red value (0-255)
-   * @param green Green value (0-255)
-   * @param blue Blue value (0-255)
+   * @param r Red value (0-255)
+   * @param g Green value (0-255)
+   * @param b Blue value (0-255)
    */
   constexpr Color8Bit(int r, int g, int b)
       : red(std::clamp(r, 0, 255)),
@@ -37,12 +34,12 @@
    *
    * @param color The color
    */
-  constexpr Color8Bit(const Color& color)
+  constexpr Color8Bit(const Color& color)  // NOLINT
       : red(color.red * 255),
         green(color.green * 255),
         blue(color.blue * 255) {}
 
-  constexpr operator Color() const {
+  constexpr operator Color() const {  // NOLINT
     return Color(red / 255.0, green / 255.0, blue / 255.0);
   }
 
diff --git a/third_party/allwpilib/wpilibc/src/main/native/include/frc2/Timer.h b/third_party/allwpilib/wpilibc/src/main/native/include/frc2/Timer.h
deleted file mode 100644
index 8f943a6..0000000
--- a/third_party/allwpilib/wpilibc/src/main/native/include/frc2/Timer.h
+++ /dev/null
@@ -1,151 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <units/time.h>
-#include <wpi/mutex.h>
-
-namespace frc2 {
-
-/**
- * Pause the task for a specified time.
- *
- * Pause the execution of the program for a specified period of time given in
- * seconds. Motors will continue to run at their last assigned values, and
- * sensors will continue to update. Only the task containing the wait will pause
- * until the wait time is expired.
- *
- * @param seconds Length of time to pause, in seconds.
- */
-void Wait(units::second_t seconds);
-
-/**
- * @brief  Gives real-time clock system time with nanosecond resolution
- * @return The time, just in case you want the robot to start autonomous at 8pm
- *         on Saturday.
- */
-units::second_t GetTime();
-
-/**
- * A wrapper for the frc::Timer class that returns unit-typed values.
- */
-class Timer {
- public:
-  /**
-   * Create a new timer object.
-   *
-   * Create a new timer object and reset the time to zero. The timer is
-   * initially not running and must be started.
-   */
-  Timer();
-
-  virtual ~Timer() = default;
-
-  Timer(const Timer& rhs);
-  Timer& operator=(const Timer& rhs);
-  Timer(Timer&& rhs);
-  Timer& operator=(Timer&& rhs);
-
-  /**
-   * Get the current time from the timer. If the clock is running it is derived
-   * from the current system clock the start time stored in the timer class. If
-   * the clock is not running, then return the time when it was last stopped.
-   *
-   * @return Current time value for this timer in seconds
-   */
-  units::second_t Get() const;
-
-  /**
-   * Reset the timer by setting the time to 0.
-   *
-   * Make the timer startTime the current time so new requests will be relative
-   * to now.
-   */
-  void Reset();
-
-  /**
-   * Start the timer running.
-   *
-   * Just set the running flag to true indicating that all time requests should
-   * be relative to the system clock. Note that this method is a no-op if the
-   * timer is already running.
-   */
-  void Start();
-
-  /**
-   * Stop the timer.
-   *
-   * This computes the time as of now and clears the running flag, causing all
-   * subsequent time requests to be read from the accumulated time rather than
-   * looking at the system clock.
-   */
-  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.
-   *
-   * @param period The period to check for.
-   * @return       True if the period has passed.
-   */
-  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
-   * started. Rolls over after 71 minutes.
-   *
-   * @returns Robot running time in seconds.
-   */
-  static units::second_t GetFPGATimestamp();
-
-  /**
-   * Return the approximate match time.
-   *
-   * The FMS does not send an official match time to the robots, but does send
-   * an approximate match time. The value will count down the time remaining in
-   * the current period (auto or teleop).
-   *
-   * 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 behavior seen on the
-   * field.
-   *
-   * @return Time remaining in current match period (auto or teleop)
-   */
-  static units::second_t GetMatchTime();
-
- private:
-  units::second_t m_startTime = 0_s;
-  units::second_t m_accumulatedTime = 0_s;
-  bool m_running = false;
-  mutable wpi::mutex m_mutex;
-};
-
-}  // namespace frc2
diff --git a/third_party/allwpilib/wpilibc/src/test/native/c/test.c b/third_party/allwpilib/wpilibc/src/test/native/c/test.c
index 105e289..36d4f56 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/c/test.c
+++ b/third_party/allwpilib/wpilibc/src/test/native/c/test.c
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cscore.h>
 #include <hal/HAL.h>
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/AnalogPotentiometerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/AnalogPotentiometerTest.cpp
new file mode 100644
index 0000000..de69894
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/AnalogPotentiometerTest.cpp
@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/HAL.h>
+
+#include "frc/AnalogPotentiometer.h"
+#include "frc/simulation/AnalogInputSim.h"
+#include "frc/simulation/RoboRioSim.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+using namespace frc::sim;
+TEST(AnalogPotentiometerTest, InitializeWithAnalogInput) {
+  HAL_Initialize(500, 0);
+
+  AnalogInput ai{0};
+  AnalogPotentiometer pot{&ai};
+  AnalogInputSim sim{ai};
+
+  RoboRioSim::ResetData();
+  sim.SetVoltage(4.0);
+  EXPECT_EQ(0.8, pot.Get());
+}
+
+TEST(AnalogPotentiometerTest, InitializeWithAnalogInputAndScale) {
+  HAL_Initialize(500, 0);
+
+  AnalogInput ai{0};
+  AnalogPotentiometer pot{&ai, 270.0};
+  RoboRioSim::ResetData();
+  AnalogInputSim sim{ai};
+
+  sim.SetVoltage(5.0);
+  EXPECT_EQ(270.0, pot.Get());
+
+  sim.SetVoltage(2.5);
+  EXPECT_EQ(135, pot.Get());
+
+  sim.SetVoltage(0.0);
+  EXPECT_EQ(0.0, pot.Get());
+}
+
+TEST(AnalogPotentiometerTest, InitializeWithChannel) {
+  HAL_Initialize(500, 0);
+
+  AnalogPotentiometer pot{1};
+  AnalogInputSim sim{1};
+
+  sim.SetVoltage(5.0);
+  EXPECT_EQ(1.0, pot.Get());
+}
+
+TEST(AnalogPotentiometerTest, InitializeWithChannelAndScale) {
+  HAL_Initialize(500, 0);
+
+  AnalogPotentiometer pot{1, 180.0};
+  RoboRioSim::ResetData();
+  AnalogInputSim sim{1};
+
+  sim.SetVoltage(5.0);
+  EXPECT_EQ(180.0, pot.Get());
+
+  sim.SetVoltage(0.0);
+  EXPECT_EQ(0.0, pot.Get());
+}
+
+TEST(AnalogPotentiometerTest, WithModifiedBatteryVoltage) {
+  AnalogPotentiometer pot{1, 180.0, 90.0};
+  RoboRioSim::ResetData();
+  AnalogInputSim sim{1};
+
+  // Test at 5v
+  sim.SetVoltage(5.0);
+  EXPECT_EQ(270, pot.Get());
+
+  sim.SetVoltage(0.0);
+  EXPECT_EQ(90, pot.Get());
+
+  // Simulate a lower battery voltage
+  RoboRioSim::SetUserVoltage5V(units::volt_t{2.5});
+
+  sim.SetVoltage(2.5);
+  EXPECT_EQ(270, pot.Get());
+
+  sim.SetVoltage(2.0);
+  EXPECT_EQ(234, pot.Get());
+
+  sim.SetVoltage(0.0);
+  EXPECT_EQ(90, pot.Get());
+}
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/DebouncerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/DebouncerTest.cpp
new file mode 100644
index 0000000..379a6d5
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/DebouncerTest.cpp
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Debouncer.h"  // NOLINT(build/include_order)
+
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(DebouncerTest, DebounceRising) {
+  Debouncer debouncer{20_ms};
+
+  debouncer.Calculate(false);
+  EXPECT_FALSE(debouncer.Calculate(true));
+
+  frc::sim::StepTiming(100_ms);
+
+  EXPECT_TRUE(debouncer.Calculate(true));
+}
+
+TEST(DebouncerTest, DebounceFalling) {
+  Debouncer debouncer{20_ms, Debouncer::DebounceType::kFalling};
+
+  debouncer.Calculate(true);
+  EXPECT_TRUE(debouncer.Calculate(false));
+
+  frc::sim::StepTiming(100_ms);
+
+  EXPECT_FALSE(debouncer.Calculate(false));
+}
+
+TEST(DebouncerTest, DebounceBoth) {
+  Debouncer debouncer{20_ms, Debouncer::DebounceType::kBoth};
+
+  debouncer.Calculate(false);
+  EXPECT_FALSE(debouncer.Calculate(true));
+
+  frc::sim::StepTiming(100_ms);
+
+  EXPECT_TRUE(debouncer.Calculate(true));
+  EXPECT_TRUE(debouncer.Calculate(false));
+
+  frc::sim::StepTiming(100_ms);
+
+  EXPECT_FALSE(debouncer.Calculate(false));
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/DoubleSolenoidTestCTRE.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/DoubleSolenoidTestCTRE.cpp
new file mode 100644
index 0000000..2678395
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/DoubleSolenoidTestCTRE.cpp
@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/HAL.h>
+
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsControlModule.h"
+#include "frc/Solenoid.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+
+TEST(DoubleSolenoidCTRETest, ValidInitialization) {
+  DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3};
+  solenoid.Set(DoubleSolenoid::kReverse);
+  EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get());
+
+  solenoid.Set(DoubleSolenoid::kForward);
+  EXPECT_EQ(DoubleSolenoid::kForward, solenoid.Get());
+
+  solenoid.Set(DoubleSolenoid::kOff);
+  EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get());
+}
+
+TEST(DoubleSolenoidCTRETest, ThrowForwardPortAlreadyInitialized) {
+  // Single solenoid that is reused for forward port
+  Solenoid solenoid{5, frc::PneumaticsModuleType::CTREPCM, 2};
+  EXPECT_THROW(DoubleSolenoid(5, frc::PneumaticsModuleType::CTREPCM, 2, 3),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidCTRETest, ThrowReversePortAlreadyInitialized) {
+  // Single solenoid that is reused for forward port
+  Solenoid solenoid{6, frc::PneumaticsModuleType::CTREPCM, 3};
+  EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidCTRETest, ThrowBothPortsAlreadyInitialized) {
+  PneumaticsControlModule pcm{6};
+  // Single solenoid that is reused for forward port
+  Solenoid solenoid0(6, frc::PneumaticsModuleType::CTREPCM, 2);
+  Solenoid solenoid1(6, frc::PneumaticsModuleType::CTREPCM, 3);
+  EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidCTRETest, Toggle) {
+  DoubleSolenoid solenoid{4, frc::PneumaticsModuleType::CTREPCM, 2, 3};
+  // Bootstrap it into reverse
+  solenoid.Set(DoubleSolenoid::kReverse);
+
+  solenoid.Toggle();
+  EXPECT_EQ(DoubleSolenoid::kForward, solenoid.Get());
+
+  solenoid.Toggle();
+  EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get());
+
+  // Of shouldn't do anything on toggle
+  solenoid.Set(DoubleSolenoid::kOff);
+  solenoid.Toggle();
+  EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get());
+}
+
+TEST(DoubleSolenoidCTRETest, InvalidForwardPort) {
+  EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 100, 1),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidCTRETest, InvalidReversePort) {
+  EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 0, 100),
+               std::runtime_error);
+}
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/DoubleSolenoidTestREV.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/DoubleSolenoidTestREV.cpp
new file mode 100644
index 0000000..23893c4
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/DoubleSolenoidTestREV.cpp
@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/HAL.h>
+
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsControlModule.h"
+#include "frc/Solenoid.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+
+TEST(DoubleSolenoidREVTest, ValidInitialization) {
+  DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3};
+  solenoid.Set(DoubleSolenoid::kReverse);
+  EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get());
+
+  solenoid.Set(DoubleSolenoid::kForward);
+  EXPECT_EQ(DoubleSolenoid::kForward, solenoid.Get());
+
+  solenoid.Set(DoubleSolenoid::kOff);
+  EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get());
+}
+
+TEST(DoubleSolenoidREVTest, ThrowForwardPortAlreadyInitialized) {
+  // Single solenoid that is reused for forward port
+  Solenoid solenoid{5, frc::PneumaticsModuleType::CTREPCM, 2};
+  EXPECT_THROW(DoubleSolenoid(5, frc::PneumaticsModuleType::CTREPCM, 2, 3),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidREVTest, ThrowReversePortAlreadyInitialized) {
+  // Single solenoid that is reused for forward port
+  Solenoid solenoid{6, frc::PneumaticsModuleType::CTREPCM, 3};
+  EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidREVTest, ThrowBothPortsAlreadyInitialized) {
+  PneumaticsControlModule pcm{6};
+  // Single solenoid that is reused for forward port
+  Solenoid solenoid0(6, frc::PneumaticsModuleType::CTREPCM, 2);
+  Solenoid solenoid1(6, frc::PneumaticsModuleType::CTREPCM, 3);
+  EXPECT_THROW(DoubleSolenoid(6, frc::PneumaticsModuleType::CTREPCM, 2, 3),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidREVTest, Toggle) {
+  DoubleSolenoid solenoid{4, frc::PneumaticsModuleType::CTREPCM, 2, 3};
+  // Bootstrap it into reverse
+  solenoid.Set(DoubleSolenoid::kReverse);
+
+  solenoid.Toggle();
+  EXPECT_EQ(DoubleSolenoid::kForward, solenoid.Get());
+
+  solenoid.Toggle();
+  EXPECT_EQ(DoubleSolenoid::kReverse, solenoid.Get());
+
+  // Of shouldn't do anything on toggle
+  solenoid.Set(DoubleSolenoid::kOff);
+  solenoid.Toggle();
+  EXPECT_EQ(DoubleSolenoid::kOff, solenoid.Get());
+}
+
+TEST(DoubleSolenoidREVTest, InvalidForwardPort) {
+  EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 100, 1),
+               std::runtime_error);
+}
+
+TEST(DoubleSolenoidREVTest, InvalidReversePort) {
+  EXPECT_THROW(DoubleSolenoid(0, frc::PneumaticsModuleType::CTREPCM, 0, 100),
+               std::runtime_error);
+}
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/DriverStationTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/DriverStationTest.cpp
index 3b8272f..c191e2b 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/DriverStationTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/DriverStationTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <string>
 #include <tuple>
@@ -14,10 +11,10 @@
 #include "frc/simulation/SimHooks.h"
 #include "gtest/gtest.h"
 
-class IsJoystickConnectedParametersTests
+class IsJoystickConnectedParametersTest
     : public ::testing::TestWithParam<std::tuple<int, int, int, bool>> {};
 
-TEST_P(IsJoystickConnectedParametersTests, IsJoystickConnected) {
+TEST_P(IsJoystickConnectedParametersTest, IsJoystickConnected) {
   frc::sim::DriverStationSim::SetJoystickAxisCount(1, std::get<0>(GetParam()));
   frc::sim::DriverStationSim::SetJoystickButtonCount(1,
                                                      std::get<1>(GetParam()));
@@ -25,50 +22,54 @@
   frc::sim::DriverStationSim::NotifyNewData();
 
   ASSERT_EQ(std::get<3>(GetParam()),
-            frc::DriverStation::GetInstance().IsJoystickConnected(1));
+            frc::DriverStation::IsJoystickConnected(1));
 }
 
-INSTANTIATE_TEST_SUITE_P(IsConnectedTests, IsJoystickConnectedParametersTests,
+INSTANTIATE_TEST_SUITE_P(IsConnectedTests, IsJoystickConnectedParametersTest,
                          ::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
+class JoystickConnectionWarningTest
     : public ::testing::TestWithParam<
           std::tuple<bool, bool, bool, std::string>> {};
 
-TEST_P(JoystickConnectionWarningTests, JoystickConnectionWarnings) {
+TEST_P(JoystickConnectionWarningTest, 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()));
+  frc::DriverStation::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()));
+  EXPECT_EQ(frc::DriverStation::IsJoystickConnectionWarningSilenced(),
+            std::get<2>(GetParam()));
+  EXPECT_EQ(::testing::internal::GetCapturedStderr().substr(
+                0, std::get<3>(GetParam()).size()),
+            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")));
+    DriverStationTests, JoystickConnectionWarningTest,
+    ::testing::Values(
+        std::make_tuple(false, true, true, ""),
+        std::make_tuple(
+            false, false, false,
+            "Warning: Joystick Button 1 missing (max 0), check if all "
+            "controllers are plugged in\n"),
+        std::make_tuple(
+            true, true, false,
+            "Warning: Joystick Button 1 missing (max 0), check if all "
+            "controllers are plugged in\n"),
+        std::make_tuple(
+            true, false, false,
+            "Warning: Joystick Button 1 missing (max 0), check if all "
+            "controllers are plugged in\n")));
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/InterruptTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/InterruptTest.cpp
new file mode 100644
index 0000000..1bf0aa2
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/InterruptTest.cpp
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <atomic>
+
+#include <hal/HAL.h>
+
+#include "frc/AsynchronousInterrupt.h"
+#include "frc/DigitalInput.h"
+#include "frc/Timer.h"
+#include "frc/simulation/DIOSim.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+using namespace frc::sim;
+TEST(InterruptTest, AsynchronousInterrupt) {
+  HAL_Initialize(500, 0);
+
+  std::atomic_int counter{0};
+  std::atomic_bool hasFired{false};
+
+  DigitalInput di{0};
+  AsynchronousInterrupt interrupt{di, [&](bool rising, bool falling) {
+                                    counter++;
+                                    hasFired = true;
+                                  }};
+
+  interrupt.Enable();
+  frc::Wait(0.5_s);
+  DIOSim digitalSim{di};
+  digitalSim.SetValue(false);
+  frc::Wait(20_ms);
+  digitalSim.SetValue(true);
+  frc::Wait(10_ms);
+
+  int count = 0;
+  while (!hasFired) {
+    frc::Wait(5_ms);
+    count++;
+    ASSERT_TRUE(count < 1000);
+  }
+  ASSERT_EQ(1, counter.load());
+}
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTest.cpp
index 0480b76..5697879 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTest.cpp
@@ -1,39 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Joystick.h"  // NOLINT(build/include_order)
 
+#include "JoystickTestMacros.h"
 #include "frc/simulation/JoystickSim.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-TEST(JoystickTests, GetX) {
-  Joystick joy{1};
-  sim::JoystickSim joysim{joy};
+AXIS_TEST(Joystick, X)
+AXIS_TEST(Joystick, Y)
+AXIS_TEST(Joystick, Z)
+AXIS_TEST(Joystick, Throttle)
+AXIS_TEST(Joystick, Twist)
 
-  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);
-}
+BUTTON_TEST(Joystick, Trigger)
+BUTTON_TEST(Joystick, Top)
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTestMacros.h b/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTestMacros.h
new file mode 100644
index 0000000..81ccedc
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/JoystickTestMacros.h
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#define AXIS_TEST(JoystickType, AxisName)          \
+  TEST(JoystickType##Test, Get##AxisName) {        \
+    JoystickType joy{2};                           \
+    sim::JoystickType##Sim joysim{joy};            \
+    joysim.Set##AxisName(0.35);                    \
+    joysim.NotifyNewData();                        \
+    ASSERT_NEAR(joy.Get##AxisName(), 0.35, 0.001); \
+  }
+
+#define BUTTON_TEST(JoystickType, ButtonName)              \
+  TEST(JoystickType##Test, Get##ButtonName) {              \
+    JoystickType joy{1};                                   \
+    sim::JoystickType##Sim joysim{joy};                    \
+                                                           \
+    joysim.Set##ButtonName(false);                         \
+    joysim.NotifyNewData();                                \
+    ASSERT_FALSE(joy.Get##ButtonName());                   \
+    /* need to call pressed and released to clear flags */ \
+    joy.Get##ButtonName##Pressed();                        \
+    joy.Get##ButtonName##Released();                       \
+                                                           \
+    joysim.Set##ButtonName(true);                          \
+    joysim.NotifyNewData();                                \
+    ASSERT_TRUE(joy.Get##ButtonName());                    \
+    ASSERT_TRUE(joy.Get##ButtonName##Pressed());           \
+    ASSERT_FALSE(joy.Get##ButtonName##Released());         \
+                                                           \
+    joysim.Set##ButtonName(false);                         \
+    joysim.NotifyNewData();                                \
+    ASSERT_FALSE(joy.Get##ButtonName());                   \
+    ASSERT_FALSE(joy.Get##ButtonName##Pressed());          \
+    ASSERT_TRUE(joy.Get##ButtonName##Released());          \
+  }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/MockMotorController.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/MockMotorController.cpp
new file mode 100644
index 0000000..62fd54a
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/MockMotorController.cpp
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "MockMotorController.h"
+
+using namespace frc;
+
+void MockMotorController::Set(double speed) {
+  m_speed = m_isInverted ? -speed : speed;
+}
+
+double MockMotorController::Get() const {
+  return m_speed;
+}
+
+void MockMotorController::SetInverted(bool isInverted) {
+  m_isInverted = isInverted;
+}
+
+bool MockMotorController::GetInverted() const {
+  return m_isInverted;
+}
+
+void MockMotorController::Disable() {
+  m_speed = 0;
+}
+
+void MockMotorController::StopMotor() {
+  Disable();
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/MockSpeedController.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/MockSpeedController.cpp
deleted file mode 100644
index 875fec1..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/MockSpeedController.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "MockSpeedController.h"
-
-using namespace frc;
-
-void MockSpeedController::Set(double speed) {
-  m_speed = m_isInverted ? -speed : speed;
-}
-
-double MockSpeedController::Get() const { return m_speed; }
-
-void MockSpeedController::SetInverted(bool isInverted) {
-  m_isInverted = isInverted;
-}
-
-bool MockSpeedController::GetInverted() const { return m_isInverted; }
-
-void MockSpeedController::Disable() { m_speed = 0; }
-
-void MockSpeedController::StopMotor() { Disable(); }
-
-void MockSpeedController::PIDWrite(double output) { Set(output); }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/PS4ControllerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/PS4ControllerTest.cpp
new file mode 100644
index 0000000..329a165
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/PS4ControllerTest.cpp
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/PS4Controller.h"  // NOLINT(build/include_order)
+
+#include "JoystickTestMacros.h"
+#include "frc/simulation/PS4ControllerSim.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+BUTTON_TEST(PS4Controller, SquareButton)
+BUTTON_TEST(PS4Controller, CrossButton)
+BUTTON_TEST(PS4Controller, CircleButton)
+BUTTON_TEST(PS4Controller, TriangleButton)
+
+BUTTON_TEST(PS4Controller, L1Button)
+BUTTON_TEST(PS4Controller, R1Button)
+BUTTON_TEST(PS4Controller, L2Button)
+BUTTON_TEST(PS4Controller, R2Button)
+
+BUTTON_TEST(PS4Controller, ShareButton)
+BUTTON_TEST(PS4Controller, OptionsButton)
+
+BUTTON_TEST(PS4Controller, L3Button)
+BUTTON_TEST(PS4Controller, R3Button)
+
+BUTTON_TEST(PS4Controller, PSButton)
+BUTTON_TEST(PS4Controller, Touchpad)
+
+AXIS_TEST(PS4Controller, LeftX)
+AXIS_TEST(PS4Controller, RightX)
+AXIS_TEST(PS4Controller, LeftY)
+AXIS_TEST(PS4Controller, RightY)
+AXIS_TEST(PS4Controller, L2Axis)
+AXIS_TEST(PS4Controller, R2Axis)
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
index da222de..c4ef317 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/ScopedTracerTest.cpp
@@ -1,33 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <string>
-#include <thread>
+#include <string_view>
 
 #include <wpi/SmallString.h>
-#include <wpi/StringRef.h>
+#include <wpi/StringExtras.h>
 #include <wpi/raw_ostream.h>
 
 #include "frc/ScopedTracer.h"
+#include "frc/simulation/SimHooks.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
+  wpi::SmallString<128> buf;
+  wpi::raw_svector_ostream os(buf);
+
+  frc::sim::PauseTiming();
   {
     frc::ScopedTracer tracer("timing_test", os);
-    std::this_thread::sleep_for(std::chrono::milliseconds(1500));
+    frc::sim::StepTiming(1.5_s);
   }
+  frc::sim::ResumeTiming();
 
-  wpi::StringRef out = os.str();
-  EXPECT_TRUE(out.startswith("	timing_test: 1.5"));
+  std::string_view out = os.str();
+  EXPECT_TRUE(wpi::starts_with(out, "	timing_test: 1.5"));
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
deleted file mode 100644
index dea56bb..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/SlewRateLimiterTest.cpp
+++ /dev/null
@@ -1,32 +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 <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);
-
-  frc::sim::StepTiming(1.0_s);
-
-  EXPECT_TRUE(limiter.Calculate(2_m) < 2_m);
-}
-
-TEST(SlewRateLimiterTest, SlewRateNoLimitTest) {
-  frc::SlewRateLimiter<units::meters> limiter(1_mps);
-
-  frc::sim::StepTiming(1.0_s);
-
-  EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
-}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/SolenoidTestCTRE.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/SolenoidTestCTRE.cpp
new file mode 100644
index 0000000..61e3fb6
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/SolenoidTestCTRE.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/HAL.h>
+
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsControlModule.h"
+#include "frc/Solenoid.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+TEST(SolenoidCTRETest, ValidInitialization) {
+  Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2};
+  EXPECT_EQ(2, solenoid.GetChannel());
+
+  solenoid.Set(true);
+  EXPECT_TRUE(solenoid.Get());
+
+  solenoid.Set(false);
+  EXPECT_FALSE(solenoid.Get());
+}
+
+TEST(SolenoidCTRETest, DoubleInitialization) {
+  Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2};
+  EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 2),
+               std::runtime_error);
+}
+
+TEST(SolenoidCTRETest, DoubleInitializationFromDoubleSolenoid) {
+  DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2, 3};
+  EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 2),
+               std::runtime_error);
+}
+
+TEST(SolenoidCTRETest, InvalidChannel) {
+  EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::CTREPCM, 100),
+               std::runtime_error);
+}
+
+TEST(SolenoidCTRETest, Toggle) {
+  Solenoid solenoid{3, frc::PneumaticsModuleType::CTREPCM, 2};
+  solenoid.Set(true);
+  EXPECT_TRUE(solenoid.Get());
+
+  solenoid.Toggle();
+  EXPECT_FALSE(solenoid.Get());
+
+  solenoid.Toggle();
+  EXPECT_TRUE(solenoid.Get());
+}
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/SolenoidTestREV.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/SolenoidTestREV.cpp
new file mode 100644
index 0000000..75ea261
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/SolenoidTestREV.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <hal/HAL.h>
+
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsControlModule.h"
+#include "frc/Solenoid.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+TEST(SolenoidREVTest, ValidInitialization) {
+  Solenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2};
+  EXPECT_EQ(2, solenoid.GetChannel());
+
+  solenoid.Set(true);
+  EXPECT_TRUE(solenoid.Get());
+
+  solenoid.Set(false);
+  EXPECT_FALSE(solenoid.Get());
+}
+
+TEST(SolenoidREVTest, DoubleInitialization) {
+  Solenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2};
+  EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::REVPH, 2),
+               std::runtime_error);
+}
+
+TEST(SolenoidREVTest, DoubleInitializationFromDoubleSolenoid) {
+  DoubleSolenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2, 3};
+  EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::REVPH, 2),
+               std::runtime_error);
+}
+
+TEST(SolenoidREVTest, InvalidChannel) {
+  EXPECT_THROW(Solenoid(3, frc::PneumaticsModuleType::REVPH, 100),
+               std::runtime_error);
+}
+
+TEST(SolenoidREVTest, Toggle) {
+  Solenoid solenoid{3, frc::PneumaticsModuleType::REVPH, 2};
+  solenoid.Set(true);
+  EXPECT_TRUE(solenoid.Get());
+
+  solenoid.Toggle();
+  EXPECT_FALSE(solenoid.Get());
+
+  solenoid.Toggle();
+  EXPECT_TRUE(solenoid.Get());
+}
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
index cb81fc9..38e1592 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/SpeedControllerGroupTest.cpp
@@ -1,33 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include "frc/SpeedControllerGroup.h"  // NOLINT(build/include_order)
+#include "frc/motorcontrol/MotorControllerGroup.h"  // NOLINT(build/include_order)
 
 #include <memory>
 #include <vector>
 
-#include "MockSpeedController.h"
+#include "MockMotorController.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-enum SpeedControllerGroupTestType { TEST_ONE, TEST_TWO, TEST_THREE };
+enum MotorControllerGroupTestType { TEST_ONE, TEST_TWO, TEST_THREE };
 
 std::ostream& operator<<(std::ostream& os,
-                         const SpeedControllerGroupTestType& type) {
+                         const MotorControllerGroupTestType& type) {
   switch (type) {
     case TEST_ONE:
-      os << "SpeedControllerGroup with one speed controller";
+      os << "MotorControllerGroup with one speed controller";
       break;
     case TEST_TWO:
-      os << "SpeedControllerGroup with two speed controllers";
+      os << "MotorControllerGroup with two speed controllers";
       break;
     case TEST_THREE:
-      os << "SpeedControllerGroup with three speed controllers";
+      os << "MotorControllerGroup with three speed controllers";
       break;
   }
 
@@ -35,26 +32,26 @@
 }
 
 /**
- * A fixture used for SpeedControllerGroup testing.
+ * A fixture used for MotorControllerGroup testing.
  */
-class SpeedControllerGroupTest
-    : public testing::TestWithParam<SpeedControllerGroupTestType> {
+class MotorControllerGroupTest
+    : public testing::TestWithParam<MotorControllerGroupTestType> {
  protected:
-  std::vector<MockSpeedController> m_speedControllers;
-  std::unique_ptr<SpeedControllerGroup> m_group;
+  std::vector<MockMotorController> m_speedControllers;
+  std::unique_ptr<MotorControllerGroup> m_group;
 
   void SetUp() override {
     switch (GetParam()) {
       case TEST_ONE: {
         m_speedControllers.emplace_back();
-        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0]);
+        m_group = std::make_unique<MotorControllerGroup>(m_speedControllers[0]);
         break;
       }
 
       case TEST_TWO: {
         m_speedControllers.emplace_back();
         m_speedControllers.emplace_back();
-        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+        m_group = std::make_unique<MotorControllerGroup>(m_speedControllers[0],
                                                          m_speedControllers[1]);
         break;
       }
@@ -63,7 +60,7 @@
         m_speedControllers.emplace_back();
         m_speedControllers.emplace_back();
         m_speedControllers.emplace_back();
-        m_group = std::make_unique<SpeedControllerGroup>(m_speedControllers[0],
+        m_group = std::make_unique<MotorControllerGroup>(m_speedControllers[0],
                                                          m_speedControllers[1],
                                                          m_speedControllers[2]);
         break;
@@ -72,7 +69,7 @@
   }
 };
 
-TEST_P(SpeedControllerGroupTest, Set) {
+TEST_P(MotorControllerGroupTest, Set) {
   m_group->Set(1.0);
 
   for (auto& speedController : m_speedControllers) {
@@ -80,13 +77,13 @@
   }
 }
 
-TEST_P(SpeedControllerGroupTest, GetInverted) {
+TEST_P(MotorControllerGroupTest, GetInverted) {
   m_group->SetInverted(true);
 
   EXPECT_TRUE(m_group->GetInverted());
 }
 
-TEST_P(SpeedControllerGroupTest, SetInvertedDoesNotModifySpeedControllers) {
+TEST_P(MotorControllerGroupTest, SetInvertedDoesNotModifyMotorControllers) {
   for (auto& speedController : m_speedControllers) {
     speedController.SetInverted(false);
   }
@@ -97,7 +94,7 @@
   }
 }
 
-TEST_P(SpeedControllerGroupTest, SetInvertedDoesInvert) {
+TEST_P(MotorControllerGroupTest, SetInvertedDoesInvert) {
   m_group->SetInverted(true);
   m_group->Set(1.0);
 
@@ -106,7 +103,7 @@
   }
 }
 
-TEST_P(SpeedControllerGroupTest, Disable) {
+TEST_P(MotorControllerGroupTest, Disable) {
   m_group->Set(1.0);
   m_group->Disable();
 
@@ -115,7 +112,7 @@
   }
 }
 
-TEST_P(SpeedControllerGroupTest, StopMotor) {
+TEST_P(MotorControllerGroupTest, StopMotor) {
   m_group->Set(1.0);
   m_group->StopMotor();
 
@@ -124,13 +121,5 @@
   }
 }
 
-TEST_P(SpeedControllerGroupTest, PIDWrite) {
-  m_group->PIDWrite(1.0);
-
-  for (auto& speedController : m_speedControllers) {
-    EXPECT_FLOAT_EQ(speedController.Get(), 1.0);
-  }
-}
-
-INSTANTIATE_TEST_SUITE_P(Test, SpeedControllerGroupTest,
+INSTANTIATE_TEST_SUITE_P(Tests, MotorControllerGroupTest,
                          testing::Values(TEST_ONE, TEST_TWO, TEST_THREE));
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
new file mode 100644
index 0000000..d98f539
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/TestCallbackHelpers.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "callback_helpers/TestCallbackHelpers.h"
+
+#include <iostream>
+
+#include <fmt/format.h>
+
+namespace frc::sim {
+
+void BooleanCallback::HandleCallback(std::string_view name,
+                                     const HAL_Value* value) {
+  if (!value) {
+    throw std::invalid_argument("Null value");
+  }
+  if (value->type != HAL_BOOLEAN) {
+    throw std::invalid_argument(
+        fmt::format("Wrong type '{}' for boolean", value->type).c_str());
+  }
+  m_wasTriggered = true;
+  m_lastValue = value->data.v_boolean;
+}
+
+void EnumCallback::HandleCallback(std::string_view name,
+                                  const HAL_Value* value) {
+  if (!value) {
+    throw std::invalid_argument("Null value");
+  }
+  if (value->type != HAL_ENUM) {
+    throw std::invalid_argument(
+        fmt::format("Wrong type '{}' for enum", value->type).c_str());
+  }
+
+  m_wasTriggered = true;
+  m_lastValue = value->data.v_enum;
+}
+
+void IntCallback::HandleCallback(std::string_view name,
+                                 const HAL_Value* value) {
+  if (!value) {
+    throw std::invalid_argument("Null value");
+  }
+  if (value->type != HAL_INT) {
+    throw std::invalid_argument(
+        fmt::format("Wrong type '{}' for integer", value->type).c_str());
+  }
+
+  m_wasTriggered = true;
+  m_lastValue = value->data.v_int;
+}
+
+void LongCallback::HandleCallback(std::string_view name,
+                                  const HAL_Value* value) {
+  if (!value) {
+    throw std::invalid_argument("Null value");
+  }
+  if (value->type != HAL_LONG) {
+    throw std::invalid_argument(
+        fmt::format("Wrong type '{}' for long", value->type).c_str());
+  }
+
+  m_wasTriggered = true;
+  m_lastValue = value->data.v_long;
+}
+
+void DoubleCallback::HandleCallback(std::string_view name,
+                                    const HAL_Value* value) {
+  if (!value) {
+    throw std::invalid_argument("Null value");
+  }
+  if (value->type != HAL_DOUBLE) {
+    throw std::invalid_argument(
+        fmt::format("Wrong type '{}' for double", value->type).c_str());
+  }
+
+  m_wasTriggered = true;
+  m_lastValue = value->data.v_double;
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/TimedRobotTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
index f91efb8..ea9656b 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/TimedRobotTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/TimedRobot.h"  // NOLINT(build/include_order)
 
@@ -35,6 +32,11 @@
   std::atomic<uint32_t> m_teleopInitCount{0};
   std::atomic<uint32_t> m_testInitCount{0};
 
+  std::atomic<uint32_t> m_disabledExitCount{0};
+  std::atomic<uint32_t> m_autonomousExitCount{0};
+  std::atomic<uint32_t> m_teleopExitCount{0};
+  std::atomic<uint32_t> m_testExitCount{0};
+
   std::atomic<uint32_t> m_robotPeriodicCount{0};
   std::atomic<uint32_t> m_simulationPeriodicCount{0};
   std::atomic<uint32_t> m_disabledPeriodicCount{0};
@@ -65,10 +67,18 @@
   void TeleopPeriodic() override { m_teleopPeriodicCount++; }
 
   void TestPeriodic() override { m_testPeriodicCount++; }
+
+  void DisabledExit() override { m_disabledExitCount++; }
+
+  void AutonomousExit() override { m_autonomousExitCount++; }
+
+  void TeleopExit() override { m_teleopExitCount++; }
+
+  void TestExit() override { m_testExitCount++; }
 };
 }  // namespace
 
-TEST_F(TimedRobotTest, Disabled) {
+TEST_F(TimedRobotTest, DisabledMode) {
   MockRobot robot;
 
   std::thread robotThread{[&] { robot.StartCompetition(); }};
@@ -91,6 +101,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -107,6 +122,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -123,11 +143,16 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   robot.EndCompetition();
   robotThread.join();
 }
 
-TEST_F(TimedRobotTest, Autonomous) {
+TEST_F(TimedRobotTest, AutonomousMode) {
   MockRobot robot;
 
   std::thread robotThread{[&] { robot.StartCompetition(); }};
@@ -152,6 +177,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -168,6 +198,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -184,11 +219,16 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   robot.EndCompetition();
   robotThread.join();
 }
 
-TEST_F(TimedRobotTest, Teleop) {
+TEST_F(TimedRobotTest, TeleopMode) {
   MockRobot robot;
 
   std::thread robotThread{[&] { robot.StartCompetition(); }};
@@ -213,6 +253,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -229,6 +274,11 @@
   EXPECT_EQ(1u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -245,11 +295,16 @@
   EXPECT_EQ(2u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   robot.EndCompetition();
   robotThread.join();
 }
 
-TEST_F(TimedRobotTest, Test) {
+TEST_F(TimedRobotTest, TestMode) {
   MockRobot robot;
 
   std::thread robotThread{[&] { robot.StartCompetition(); }};
@@ -274,6 +329,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(0u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -290,6 +350,11 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(1u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
   frc::sim::StepTiming(20_ms);
 
   EXPECT_EQ(1u, robot.m_robotInitCount);
@@ -306,6 +371,117 @@
   EXPECT_EQ(0u, robot.m_teleopPeriodicCount);
   EXPECT_EQ(2u, robot.m_testPeriodicCount);
 
+  EXPECT_EQ(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimedRobotTest, ModeChange) {
+  MockRobot robot;
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  // Start in disabled
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  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_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
+  frc::sim::StepTiming(20_ms);
+
+  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(0u, robot.m_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
+  // Transition to autonomous
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(true);
+  frc::sim::DriverStationSim::SetTest(false);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, 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_disabledExitCount);
+  EXPECT_EQ(0u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
+  // Transition to teleop
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(false);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(0u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_disabledExitCount);
+  EXPECT_EQ(1u, robot.m_autonomousExitCount);
+  EXPECT_EQ(0u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
+  // Transition to test
+  frc::sim::DriverStationSim::SetEnabled(true);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(true);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(1u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_disabledExitCount);
+  EXPECT_EQ(1u, robot.m_autonomousExitCount);
+  EXPECT_EQ(1u, robot.m_teleopExitCount);
+  EXPECT_EQ(0u, robot.m_testExitCount);
+
+  // Transition to disabled
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::SetAutonomous(false);
+  frc::sim::DriverStationSim::SetTest(false);
+
+  frc::sim::StepTiming(20_ms);
+
+  EXPECT_EQ(2u, robot.m_disabledInitCount);
+  EXPECT_EQ(1u, robot.m_autonomousInitCount);
+  EXPECT_EQ(1u, robot.m_teleopInitCount);
+  EXPECT_EQ(1u, robot.m_testInitCount);
+
+  EXPECT_EQ(1u, robot.m_disabledExitCount);
+  EXPECT_EQ(1u, robot.m_autonomousExitCount);
+  EXPECT_EQ(1u, robot.m_teleopExitCount);
+  EXPECT_EQ(1u, robot.m_testExitCount);
+
   robot.EndCompetition();
   robotThread.join();
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/TimerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/TimerTest.cpp
new file mode 100644
index 0000000..a7adc45
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/TimerTest.cpp
@@ -0,0 +1,115 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Timer.h"  // NOLINT(build/include_order)
+
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+namespace {
+class TimerTest : public ::testing::Test {
+ protected:
+  void SetUp() override {
+    frc::sim::PauseTiming();
+    frc::sim::RestartTiming();
+  }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+}  // namespace
+
+TEST_F(TimerTest, StartStop) {
+  Timer timer;
+
+  // Verify timer is initialized as stopped
+  EXPECT_EQ(timer.Get(), 0_s);
+  frc::sim::StepTiming(500_ms);
+  EXPECT_EQ(timer.Get(), 0_s);
+
+  // Verify timer increments after it's started
+  timer.Start();
+  frc::sim::StepTiming(500_ms);
+  EXPECT_EQ(timer.Get(), 500_ms);
+
+  // Verify timer stops incrementing after it's stopped
+  timer.Stop();
+  frc::sim::StepTiming(500_ms);
+  EXPECT_EQ(timer.Get(), 500_ms);
+}
+
+TEST_F(TimerTest, Reset) {
+  Timer timer;
+  timer.Start();
+
+  // Advance timer to 500 ms
+  EXPECT_EQ(timer.Get(), 0_s);
+  frc::sim::StepTiming(500_ms);
+  EXPECT_EQ(timer.Get(), 500_ms);
+
+  // Verify timer reports 0 ms after reset
+  timer.Reset();
+  EXPECT_EQ(timer.Get(), 0_s);
+
+  // Verify timer continues incrementing
+  frc::sim::StepTiming(500_ms);
+  EXPECT_EQ(timer.Get(), 500_ms);
+
+  // Verify timer doesn't start incrementing after reset if it was stopped
+  timer.Stop();
+  timer.Reset();
+  frc::sim::StepTiming(500_ms);
+  EXPECT_EQ(timer.Get(), 0_ms);
+}
+
+TEST_F(TimerTest, HasElapsed) {
+  Timer timer;
+
+  // Verify 0 ms has elapsed since timer hasn't started
+  EXPECT_TRUE(timer.HasElapsed(0_s));
+
+  // Verify timer doesn't report elapsed time when stopped
+  frc::sim::StepTiming(500_ms);
+  EXPECT_FALSE(timer.HasElapsed(400_ms));
+
+  timer.Start();
+
+  // Verify timer reports >= 400 ms has elapsed after multiple calls
+  frc::sim::StepTiming(500_ms);
+  EXPECT_TRUE(timer.HasElapsed(400_ms));
+  EXPECT_TRUE(timer.HasElapsed(400_ms));
+}
+
+TEST_F(TimerTest, AdvanceIfElapsed) {
+  Timer timer;
+
+  // Verify 0 ms has elapsed since timer hasn't started
+  EXPECT_TRUE(timer.AdvanceIfElapsed(0_s));
+
+  // Verify timer doesn't report elapsed time when stopped
+  frc::sim::StepTiming(500_ms);
+  EXPECT_FALSE(timer.AdvanceIfElapsed(400_ms));
+
+  timer.Start();
+
+  // Verify timer reports >= 400 ms has elapsed for only first call
+  frc::sim::StepTiming(500_ms);
+  EXPECT_TRUE(timer.AdvanceIfElapsed(400_ms));
+  EXPECT_FALSE(timer.AdvanceIfElapsed(400_ms));
+
+  // Verify timer reports >= 400 ms has elapsed for two calls
+  frc::sim::StepTiming(1_s);
+  EXPECT_TRUE(timer.AdvanceIfElapsed(400_ms));
+  EXPECT_TRUE(timer.AdvanceIfElapsed(400_ms));
+  EXPECT_FALSE(timer.AdvanceIfElapsed(400_ms));
+}
+
+TEST_F(TimerTest, GetFPGATimestamp) {
+  auto start = frc::Timer::GetFPGATimestamp();
+  frc::sim::StepTiming(500_ms);
+  auto end = frc::Timer::GetFPGATimestamp();
+  EXPECT_EQ(start + 500_ms, end);
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp
new file mode 100644
index 0000000..a2e4e13
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/TimesliceRobotTest.cpp
@@ -0,0 +1,100 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/TimesliceRobot.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 TimesliceRobotTest : public ::testing::Test {
+ protected:
+  void SetUp() override { frc::sim::PauseTiming(); }
+
+  void TearDown() override { frc::sim::ResumeTiming(); }
+};
+
+class MockRobot : public TimesliceRobot {
+ public:
+  std::atomic<uint32_t> m_robotPeriodicCount{0};
+
+  MockRobot() : TimesliceRobot{2_ms, 5_ms} {}
+
+  void RobotPeriodic() override { m_robotPeriodicCount++; }
+};
+}  // namespace
+
+TEST_F(TimesliceRobotTest, Schedule) {
+  MockRobot robot;
+
+  std::atomic<uint32_t> callbackCount1{0};
+  std::atomic<uint32_t> callbackCount2{0};
+
+  // Timeslice allocation table
+  //
+  // |       Name      | Offset (ms) | Allocation (ms)|
+  // |-----------------|-------------|----------------|
+  // | RobotPeriodic() |           0 |              2 |
+  // | Callback 1      |           2 |            0.5 |
+  // | Callback 2      |         2.5 |              1 |
+  robot.Schedule([&] { callbackCount1++; }, 0.5_ms);
+  robot.Schedule([&] { callbackCount2++; }, 1_ms);
+
+  std::thread robotThread{[&] { robot.StartCompetition(); }};
+
+  frc::sim::DriverStationSim::SetEnabled(false);
+  frc::sim::DriverStationSim::NotifyNewData();
+  frc::sim::StepTiming(0_ms);  // Wait for Notifiers
+
+  // Functions scheduled with addPeriodic() are delayed by one period before
+  // their first run (5 ms for this test's callbacks here and 20 ms for
+  // robotPeriodic()).
+  frc::sim::StepTiming(5_ms);
+
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, callbackCount1);
+  EXPECT_EQ(0u, callbackCount2);
+
+  // Step to 1.5 ms
+  frc::sim::StepTiming(1.5_ms);
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(0u, callbackCount1);
+  EXPECT_EQ(0u, callbackCount2);
+
+  // Step to 2.25 ms
+  frc::sim::StepTiming(0.75_ms);
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, callbackCount1);
+  EXPECT_EQ(0u, callbackCount2);
+
+  // Step to 2.75 ms
+  frc::sim::StepTiming(0.5_ms);
+  EXPECT_EQ(0u, robot.m_robotPeriodicCount);
+  EXPECT_EQ(1u, callbackCount1);
+  EXPECT_EQ(1u, callbackCount2);
+
+  robot.EndCompetition();
+  robotThread.join();
+}
+
+TEST_F(TimesliceRobotTest, ScheduleOverrun) {
+  MockRobot robot;
+
+  robot.Schedule([] {}, 0.5_ms);
+  robot.Schedule([] {}, 1_ms);
+
+  // offset = 2 ms + 0.5 ms + 1 ms = 3.5 ms
+  // 3.5 ms + 3 ms allocation = 6.5 ms > max of 5 ms
+  EXPECT_THROW(robot.Schedule([] {}, 3_ms), std::runtime_error);
+
+  robot.EndCompetition();
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/UltrasonicTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/UltrasonicTest.cpp
new file mode 100644
index 0000000..72d72d2
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/UltrasonicTest.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/Ultrasonic.h"
+#include "frc/simulation/UltrasonicSim.h"
+#include "gtest/gtest.h"
+
+namespace frc {
+
+TEST(UltrasonicTest, SimDevices) {
+  Ultrasonic ultrasonic{0, 1};
+  sim::UltrasonicSim sim{ultrasonic};
+
+  EXPECT_EQ(0, ultrasonic.GetRange().value());
+  EXPECT_TRUE(ultrasonic.IsRangeValid());
+
+  sim.SetRange(units::meter_t{35.04});
+  EXPECT_EQ(35.04, ultrasonic.GetRange().value());
+
+  sim.SetRangeValid(false);
+  EXPECT_FALSE(ultrasonic.IsRangeValid());
+  EXPECT_EQ(0, ultrasonic.GetRange().value());
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/WatchdogTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/WatchdogTest.cpp
index 0e33511..5f55c2f 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/WatchdogTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/WatchdogTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Watchdog.h"  // NOLINT(build/include_order)
 
@@ -78,7 +75,7 @@
   frc::sim::StepTiming(0.2_s);
   watchdog.SetTimeout(0.2_s);
 
-  EXPECT_EQ(0.2, watchdog.GetTimeout());
+  EXPECT_EQ(0.2_s, watchdog.GetTimeout());
   EXPECT_EQ(0u, watchdogCounter) << "Watchdog triggered early";
 
   frc::sim::StepTiming(0.3_s);
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/XboxControllerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
index 66ccfcd..0a8316b 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/XboxControllerTest.cpp
@@ -1,74 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/XboxController.h"  // NOLINT(build/include_order)
 
+#include "JoystickTestMacros.h"
 #include "frc/simulation/XboxControllerSim.h"
 #include "gtest/gtest.h"
 
 using namespace frc;
 
-TEST(XboxControllerTests, GetX) {
-  XboxController joy{2};
-  sim::XboxControllerSim joysim{joy};
+BUTTON_TEST(XboxController, LeftBumper)
+BUTTON_TEST(XboxController, RightBumper)
 
-  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);
-}
+BUTTON_TEST(XboxController, LeftStickButton)
+BUTTON_TEST(XboxController, RightStickButton)
 
-TEST(XboxControllerTests, GetBumper) {
-  XboxController joy{1};
-  sim::XboxControllerSim joysim{joy};
+BUTTON_TEST(XboxController, AButton)
+BUTTON_TEST(XboxController, BButton)
+BUTTON_TEST(XboxController, XButton)
+BUTTON_TEST(XboxController, YButton)
+BUTTON_TEST(XboxController, BackButton)
+BUTTON_TEST(XboxController, StartButton)
 
-  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);
+AXIS_TEST(XboxController, LeftX)
+AXIS_TEST(XboxController, RightX)
+AXIS_TEST(XboxController, LeftY)
+AXIS_TEST(XboxController, RightY)
 
-  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());
-}
+AXIS_TEST(XboxController, LeftTriggerAxis)
+AXIS_TEST(XboxController, RightTriggerAxis)
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
deleted file mode 100644
index 00259d1..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/ControllerUtilTest.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
deleted file mode 100644
index fb72a4a..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
deleted file mode 100644
index 3580308..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/PIDInputOutputTest.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
-
-class PIDInputOutputTest : public testing::Test {
- protected:
-  frc2::PIDController* controller;
-
-  void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
-
-  void TearDown() override { delete controller; }
-};
-
-TEST_F(PIDInputOutputTest, ContinuousInputTest) {
-  controller->SetP(1);
-  controller->EnableContinuousInput(-180, 180);
-
-  EXPECT_LT(controller->Calculate(-179, 179), 0);
-}
-
-TEST_F(PIDInputOutputTest, ProportionalGainOutputTest) {
-  controller->SetP(4);
-
-  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
-}
-
-TEST_F(PIDInputOutputTest, IntegralGainOutputTest) {
-  controller->SetI(4);
-
-  double out = 0;
-
-  for (int i = 0; i < 5; i++) {
-    out = controller->Calculate(0.025, 0);
-  }
-
-  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().to<double>(), out);
-}
-
-TEST_F(PIDInputOutputTest, DerivativeGainOutputTest) {
-  controller->SetD(4);
-
-  controller->Calculate(0, 0);
-
-  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
-                   controller->Calculate(0.0025, 0));
-}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
deleted file mode 100644
index 3251098..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/PIDToleranceTest.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/controller/PIDController.h"
-#include "gtest/gtest.h"
-
-static constexpr double kSetpoint = 50.0;
-static constexpr double kRange = 200;
-static constexpr double kTolerance = 10.0;
-
-TEST(PIDToleranceTest, InitialTolerance) {
-  frc2::PIDController controller{0.5, 0.0, 0.0};
-  controller.EnableContinuousInput(-kRange / 2, kRange / 2);
-
-  EXPECT_TRUE(controller.AtSetpoint());
-}
-
-TEST(PIDToleranceTest, AbsoluteTolerance) {
-  frc2::PIDController controller{0.5, 0.0, 0.0};
-  controller.EnableContinuousInput(-kRange / 2, kRange / 2);
-
-  controller.SetTolerance(kTolerance);
-  controller.SetSetpoint(kSetpoint);
-
-  controller.Calculate(0.0);
-
-  EXPECT_FALSE(controller.AtSetpoint())
-      << "Error was in tolerance when it should not have been. Error was "
-      << controller.GetPositionError();
-
-  controller.Calculate(kSetpoint + kTolerance / 2);
-
-  EXPECT_TRUE(controller.AtSetpoint())
-      << "Error was not in tolerance when it should have been. Error was "
-      << controller.GetPositionError();
-
-  controller.Calculate(kSetpoint + 10 * kTolerance);
-
-  EXPECT_FALSE(controller.AtSetpoint())
-      << "Error was in tolerance when it should not have been. Error was "
-      << controller.GetPositionError();
-}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
deleted file mode 100644
index 6f9c36d..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
+++ /dev/null
@@ -1,111 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.cpp
deleted file mode 100644
index fb34385..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/controller/RamseteControllerTest.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 <units/math.h>
-
-#include "frc/controller/RamseteController.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(RamseteControllerTest, ReachesReference) {
-  frc::RamseteController controller{2.0, 0.7};
-  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.8_mps, 0.1_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);
-    static_cast<void>(vy);
-
-    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, 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(endPose.Rotation().Radians() -
-                                                robotPose.Rotation().Radians()),
-                    0_rad, kAngularTolerance);
-}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
new file mode 100644
index 0000000..1d13a77
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DifferentialDriveTest.cpp
@@ -0,0 +1,229 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "MockMotorController.h"
+#include "frc/drive/DifferentialDrive.h"
+#include "gtest/gtest.h"
+
+TEST(DifferentialDriveTest, ArcadeDrive) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::DifferentialDrive drive{left, right};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.ArcadeDrive(1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward left turn
+  drive.ArcadeDrive(0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(0.0, left.Get());
+  EXPECT_DOUBLE_EQ(0.5, right.Get());
+
+  // Forward right turn
+  drive.ArcadeDrive(0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(0.5, left.Get());
+  EXPECT_DOUBLE_EQ(0.0, right.Get());
+
+  // Backward
+  drive.ArcadeDrive(-1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward left turn
+  drive.ArcadeDrive(-0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(-0.5, left.Get());
+  EXPECT_DOUBLE_EQ(0.0, right.Get());
+
+  // Backward right turn
+  drive.ArcadeDrive(-0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(0.0, left.Get());
+  EXPECT_DOUBLE_EQ(-0.5, right.Get());
+}
+
+TEST(DifferentialDriveTest, ArcadeDriveSquared) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::DifferentialDrive drive{left, right};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.ArcadeDrive(1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward left turn
+  drive.ArcadeDrive(0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, left.Get());
+  EXPECT_DOUBLE_EQ(0.25, right.Get());
+
+  // Forward right turn
+  drive.ArcadeDrive(0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(0.25, left.Get());
+  EXPECT_DOUBLE_EQ(0.0, right.Get());
+
+  // Backward
+  drive.ArcadeDrive(-1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward left turn
+  drive.ArcadeDrive(-0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(-0.25, left.Get());
+  EXPECT_DOUBLE_EQ(0.0, right.Get());
+
+  // Backward right turn
+  drive.ArcadeDrive(-0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, left.Get());
+  EXPECT_DOUBLE_EQ(-0.25, right.Get());
+}
+
+TEST(DifferentialDriveTest, CurvatureDrive) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::DifferentialDrive drive{left, right};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.CurvatureDrive(1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward left turn
+  drive.CurvatureDrive(0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(0.25, left.Get());
+  EXPECT_DOUBLE_EQ(0.75, right.Get());
+
+  // Forward right turn
+  drive.CurvatureDrive(0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(0.75, left.Get());
+  EXPECT_DOUBLE_EQ(0.25, right.Get());
+
+  // Backward
+  drive.CurvatureDrive(-1.0, 0.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward left turn
+  drive.CurvatureDrive(-0.5, -0.5, false);
+  EXPECT_DOUBLE_EQ(-0.75, left.Get());
+  EXPECT_DOUBLE_EQ(-0.25, right.Get());
+
+  // Backward right turn
+  drive.CurvatureDrive(-0.5, 0.5, false);
+  EXPECT_DOUBLE_EQ(-0.25, left.Get());
+  EXPECT_DOUBLE_EQ(-0.75, right.Get());
+}
+
+TEST(DifferentialDriveTest, CurvatureDriveTurnInPlace) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::DifferentialDrive drive{left, right};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.CurvatureDrive(1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward left turn
+  drive.CurvatureDrive(0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward right turn
+  drive.CurvatureDrive(0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(0.0, right.Get());
+
+  // Backward
+  drive.CurvatureDrive(-1.0, 0.0, true);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward left turn
+  drive.CurvatureDrive(-0.5, -0.5, true);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(0.0, right.Get());
+
+  // Backward right turn
+  drive.CurvatureDrive(-0.5, 0.5, true);
+  EXPECT_DOUBLE_EQ(0.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+}
+
+TEST(DifferentialDriveTest, TankDrive) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::DifferentialDrive drive{left, right};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.TankDrive(1.0, 1.0, false);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward left turn
+  drive.TankDrive(0.5, 1.0, false);
+  EXPECT_DOUBLE_EQ(0.5, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward right turn
+  drive.TankDrive(1.0, 0.5, false);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(0.5, right.Get());
+
+  // Backward
+  drive.TankDrive(-1.0, -1.0, false);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward left turn
+  drive.TankDrive(-0.5, -1.0, false);
+  EXPECT_DOUBLE_EQ(-0.5, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward right turn
+  drive.TankDrive(-0.5, 1.0, false);
+  EXPECT_DOUBLE_EQ(-0.5, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+}
+
+TEST(DifferentialDriveTest, TankDriveSquared) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::DifferentialDrive drive{left, right};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.TankDrive(1.0, 1.0, true);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward left turn
+  drive.TankDrive(0.5, 1.0, true);
+  EXPECT_DOUBLE_EQ(0.25, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+
+  // Forward right turn
+  drive.TankDrive(1.0, 0.5, true);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(0.25, right.Get());
+
+  // Backward
+  drive.TankDrive(-1.0, -1.0, true);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward left turn
+  drive.TankDrive(-0.5, -1.0, true);
+  EXPECT_DOUBLE_EQ(-0.25, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+
+  // Backward right turn
+  drive.TankDrive(-1.0, -0.5, true);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-0.25, right.Get());
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DriveTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DriveTest.cpp
deleted file mode 100644
index 1ebb6b3..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/DriveTest.cpp
+++ /dev/null
@@ -1,190 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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 "MockSpeedController.h"
-#include "frc/RobotDrive.h"
-#include "frc/drive/DifferentialDrive.h"
-#include "frc/drive/MecanumDrive.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class DriveTest : public testing::Test {
- protected:
-  MockSpeedController m_rdFrontLeft;
-  MockSpeedController m_rdRearLeft;
-  MockSpeedController m_rdFrontRight;
-  MockSpeedController m_rdRearRight;
-  MockSpeedController m_frontLeft;
-  MockSpeedController m_rearLeft;
-  MockSpeedController m_frontRight;
-  MockSpeedController m_rearRight;
-  frc::RobotDrive m_robotDrive{m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight,
-                               m_rdRearRight};
-  frc::DifferentialDrive m_differentialDrive{m_frontLeft, m_frontRight};
-  frc::MecanumDrive m_mecanumDrive{m_frontLeft, m_rearLeft, m_frontRight,
-                                   m_rearRight};
-
-  double m_testJoystickValues[9] = {-1.0, -0.9, -0.5, -0.01, 0.0,
-                                    0.01, 0.5,  0.9,  1.0};
-  double m_testGyroValues[19] = {0,    45,   90,   135,  180, 225,  270,
-                                 305,  360,  540,  -45,  -90, -135, -180,
-                                 -225, -270, -305, -360, -540};
-};
-
-TEST_F(DriveTest, TankDrive) {
-  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
-  double leftJoystick, rightJoystick;
-  m_differentialDrive.SetDeadband(0.0);
-  m_differentialDrive.SetSafetyEnabled(false);
-  m_mecanumDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetSafetyEnabled(false);
-  for (int i = 0; i < joystickSize; i++) {
-    for (int j = 0; j < joystickSize; j++) {
-      leftJoystick = m_testJoystickValues[i];
-      rightJoystick = m_testJoystickValues[j];
-      m_robotDrive.TankDrive(leftJoystick, rightJoystick, false);
-      m_differentialDrive.TankDrive(leftJoystick, rightJoystick, false);
-      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
-      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
-    }
-  }
-}
-
-TEST_F(DriveTest, TankDriveSquared) {
-  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
-  double leftJoystick, rightJoystick;
-  m_differentialDrive.SetDeadband(0.0);
-  m_differentialDrive.SetSafetyEnabled(false);
-  m_mecanumDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetSafetyEnabled(false);
-  for (int i = 0; i < joystickSize; i++) {
-    for (int j = 0; j < joystickSize; j++) {
-      leftJoystick = m_testJoystickValues[i];
-      rightJoystick = m_testJoystickValues[j];
-      m_robotDrive.TankDrive(leftJoystick, rightJoystick, true);
-      m_differentialDrive.TankDrive(leftJoystick, rightJoystick, true);
-      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
-      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
-    }
-  }
-}
-
-TEST_F(DriveTest, ArcadeDriveSquared) {
-  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
-  double moveJoystick, rotateJoystick;
-  m_differentialDrive.SetDeadband(0.0);
-  m_differentialDrive.SetSafetyEnabled(false);
-  m_mecanumDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetSafetyEnabled(false);
-  for (int i = 0; i < joystickSize; i++) {
-    for (int j = 0; j < joystickSize; j++) {
-      moveJoystick = m_testJoystickValues[i];
-      rotateJoystick = m_testJoystickValues[j];
-      m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, true);
-      m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, true);
-      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
-      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
-    }
-  }
-}
-
-TEST_F(DriveTest, ArcadeDrive) {
-  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
-  double moveJoystick, rotateJoystick;
-  m_differentialDrive.SetDeadband(0.0);
-  m_differentialDrive.SetSafetyEnabled(false);
-  m_mecanumDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetSafetyEnabled(false);
-  for (int i = 0; i < joystickSize; i++) {
-    for (int j = 0; j < joystickSize; j++) {
-      moveJoystick = m_testJoystickValues[i];
-      rotateJoystick = m_testJoystickValues[j];
-      m_robotDrive.ArcadeDrive(moveJoystick, rotateJoystick, false);
-      m_differentialDrive.ArcadeDrive(moveJoystick, -rotateJoystick, false);
-      ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01);
-      ASSERT_NEAR(m_rdFrontRight.Get(), m_frontRight.Get(), 0.01);
-    }
-  }
-}
-
-TEST_F(DriveTest, MecanumCartesian) {
-  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
-  int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
-  double xJoystick, yJoystick, rotateJoystick, gyroValue;
-  m_mecanumDrive.SetDeadband(0.0);
-  m_mecanumDrive.SetSafetyEnabled(false);
-  m_differentialDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetSafetyEnabled(false);
-  for (int i = 0; i < joystickSize; i++) {
-    for (int j = 0; j < joystickSize; j++) {
-      for (int k = 0; k < joystickSize; k++) {
-        for (int l = 0; l < gyroSize; l++) {
-          xJoystick = m_testJoystickValues[i];
-          yJoystick = m_testJoystickValues[j];
-          rotateJoystick = m_testJoystickValues[k];
-          gyroValue = m_testGyroValues[l];
-          m_robotDrive.MecanumDrive_Cartesian(xJoystick, yJoystick,
-                                              rotateJoystick, gyroValue);
-          m_mecanumDrive.DriveCartesian(xJoystick, -yJoystick, rotateJoystick,
-                                        -gyroValue);
-          ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
-              << "X: " << xJoystick << " Y: " << yJoystick
-              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
-          ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
-              << "X: " << xJoystick << " Y: " << yJoystick
-              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
-          ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
-              << "X: " << xJoystick << " Y: " << yJoystick
-              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
-          ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
-              << "X: " << xJoystick << " Y: " << yJoystick
-              << " Rotate: " << rotateJoystick << " Gyro: " << gyroValue;
-        }
-      }
-    }
-  }
-}
-
-TEST_F(DriveTest, MecanumPolar) {
-  int joystickSize = sizeof(m_testJoystickValues) / sizeof(double);
-  int gyroSize = sizeof(m_testGyroValues) / sizeof(double);
-  double magnitudeJoystick, directionJoystick, rotateJoystick;
-  m_mecanumDrive.SetDeadband(0.0);
-  m_mecanumDrive.SetSafetyEnabled(false);
-  m_differentialDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetSafetyEnabled(false);
-  for (int i = 0; i < joystickSize; i++) {
-    for (int j = 0; j < gyroSize; j++) {
-      for (int k = 0; k < joystickSize; k++) {
-        magnitudeJoystick = m_testJoystickValues[i];
-        directionJoystick = m_testGyroValues[j];
-        rotateJoystick = m_testJoystickValues[k];
-        m_robotDrive.MecanumDrive_Polar(magnitudeJoystick, directionJoystick,
-                                        rotateJoystick);
-        m_mecanumDrive.DrivePolar(magnitudeJoystick, directionJoystick,
-                                  rotateJoystick);
-        ASSERT_NEAR(m_rdFrontLeft.Get(), m_frontLeft.Get(), 0.01)
-            << "Magnitude: " << magnitudeJoystick
-            << " Direction: " << directionJoystick
-            << " Rotate: " << rotateJoystick;
-        ASSERT_NEAR(m_rdFrontRight.Get(), -m_frontRight.Get(), 0.01)
-            << "Magnitude: " << magnitudeJoystick
-            << " Direction: " << directionJoystick
-            << " Rotate: " << rotateJoystick;
-        ASSERT_NEAR(m_rdRearLeft.Get(), m_rearLeft.Get(), 0.01)
-            << "Magnitude: " << magnitudeJoystick
-            << " Direction: " << directionJoystick
-            << " Rotate: " << rotateJoystick;
-        ASSERT_NEAR(m_rdRearRight.Get(), -m_rearRight.Get(), 0.01)
-            << "Magnitude: " << magnitudeJoystick
-            << " Direction: " << directionJoystick
-            << " Rotate: " << rotateJoystick;
-      }
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
new file mode 100644
index 0000000..510b377
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/KilloughDriveTest.cpp
@@ -0,0 +1,123 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cmath>
+
+#include "MockMotorController.h"
+#include "frc/drive/KilloughDrive.h"
+#include "gtest/gtest.h"
+
+TEST(KilloughDriveTest, Cartesian) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::MockMotorController back;
+  frc::KilloughDrive drive{left, right, back};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.DriveCartesian(1.0, 0.0, 0.0);
+  EXPECT_DOUBLE_EQ(0.5, left.Get());
+  EXPECT_DOUBLE_EQ(-0.5, right.Get());
+  EXPECT_NEAR(0.0, back.Get(), 1e-9);
+
+  // Left
+  drive.DriveCartesian(0.0, -1.0, 0.0);
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, left.Get());
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, right.Get());
+  EXPECT_DOUBLE_EQ(1.0, back.Get());
+
+  // Right
+  drive.DriveCartesian(0.0, 1.0, 0.0);
+  EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, left.Get());
+  EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, right.Get());
+  EXPECT_DOUBLE_EQ(-1.0, back.Get());
+
+  // Rotate CCW
+  drive.DriveCartesian(0.0, 0.0, -1.0);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+  EXPECT_DOUBLE_EQ(-1.0, back.Get());
+
+  // Rotate CW
+  drive.DriveCartesian(0.0, 0.0, 1.0);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+  EXPECT_DOUBLE_EQ(1.0, back.Get());
+}
+
+TEST(KilloughDriveTest, CartesianGyro90CW) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::MockMotorController back;
+  frc::KilloughDrive drive{left, right, back};
+  drive.SetDeadband(0.0);
+
+  // Forward in global frame; left in robot frame
+  drive.DriveCartesian(1.0, 0.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, left.Get());
+  EXPECT_DOUBLE_EQ(-std::sqrt(3) / 2, right.Get());
+  EXPECT_DOUBLE_EQ(1.0, back.Get());
+
+  // Left in global frame; backward in robot frame
+  drive.DriveCartesian(0.0, -1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-0.5, left.Get());
+  EXPECT_NEAR(0.5, right.Get(), 1e-9);
+  EXPECT_NEAR(0.0, back.Get(), 1e-9);
+
+  // Right in global frame; forward in robot frame
+  drive.DriveCartesian(0.0, 1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(0.5, left.Get());
+  EXPECT_NEAR(-0.5, right.Get(), 1e-9);
+  EXPECT_NEAR(0.0, back.Get(), 1e-9);
+
+  // Rotate CCW
+  drive.DriveCartesian(0.0, 0.0, -1.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+  EXPECT_DOUBLE_EQ(-1.0, back.Get());
+
+  // Rotate CW
+  drive.DriveCartesian(0.0, 0.0, 1.0, 90.0);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+  EXPECT_DOUBLE_EQ(1.0, back.Get());
+}
+
+TEST(KilloughDriveTest, Polar) {
+  frc::MockMotorController left;
+  frc::MockMotorController right;
+  frc::MockMotorController back;
+  frc::KilloughDrive drive{left, right, back};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.DrivePolar(1.0, 0.0, 0.0);
+  EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, left.Get());
+  EXPECT_DOUBLE_EQ(std::sqrt(3) / 2, right.Get());
+  EXPECT_DOUBLE_EQ(-1.0, back.Get());
+
+  // Left
+  drive.DrivePolar(1.0, -90.0, 0.0);
+  EXPECT_DOUBLE_EQ(-0.5, left.Get());
+  EXPECT_DOUBLE_EQ(0.5, right.Get());
+  EXPECT_NEAR(0.0, back.Get(), 1e-9);
+
+  // Right
+  drive.DrivePolar(1.0, 90.0, 0.0);
+  EXPECT_DOUBLE_EQ(0.5, left.Get());
+  EXPECT_NEAR(-0.5, right.Get(), 1e-9);
+  EXPECT_NEAR(0.0, back.Get(), 1e-9);
+
+  // Rotate CCW
+  drive.DrivePolar(0.0, 0.0, -1.0);
+  EXPECT_DOUBLE_EQ(-1.0, left.Get());
+  EXPECT_DOUBLE_EQ(-1.0, right.Get());
+  EXPECT_DOUBLE_EQ(-1.0, back.Get());
+
+  // Rotate CW
+  drive.DrivePolar(0.0, 0.0, 1.0);
+  EXPECT_DOUBLE_EQ(1.0, left.Get());
+  EXPECT_DOUBLE_EQ(1.0, right.Get());
+  EXPECT_DOUBLE_EQ(1.0, back.Get());
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
new file mode 100644
index 0000000..fe0a73b
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/drive/MecanumDriveTest.cpp
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "MockMotorController.h"
+#include "frc/drive/MecanumDrive.h"
+#include "gtest/gtest.h"
+
+TEST(MecanumDriveTest, Cartesian) {
+  frc::MockMotorController fl;
+  frc::MockMotorController fr;
+  frc::MockMotorController rl;
+  frc::MockMotorController rr;
+  frc::MecanumDrive drive{fl, fr, rl, rr};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.DriveCartesian(1.0, 0.0, 0.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Left
+  drive.DriveCartesian(0.0, -1.0, 0.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+
+  // Right
+  drive.DriveCartesian(0.0, 1.0, 0.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Rotate CCW
+  drive.DriveCartesian(0.0, 0.0, -1.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Rotate CW
+  drive.DriveCartesian(0.0, 0.0, 1.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+}
+
+TEST(MecanumDriveTest, CartesianGyro90CW) {
+  frc::MockMotorController fl;
+  frc::MockMotorController fr;
+  frc::MockMotorController rl;
+  frc::MockMotorController rr;
+  frc::MecanumDrive drive{fl, fr, rl, rr};
+  drive.SetDeadband(0.0);
+
+  // Forward in global frame; left in robot frame
+  drive.DriveCartesian(1.0, 0.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+
+  // Left in global frame; backward in robot frame
+  drive.DriveCartesian(0.0, -1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+
+  // Right in global frame; forward in robot frame
+  drive.DriveCartesian(0.0, 1.0, 0.0, 90.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Rotate CCW
+  drive.DriveCartesian(0.0, 0.0, -1.0, 90.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Rotate CW
+  drive.DriveCartesian(0.0, 0.0, 1.0, 90.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+}
+
+TEST(MecanumDriveTest, Polar) {
+  frc::MockMotorController fl;
+  frc::MockMotorController fr;
+  frc::MockMotorController rl;
+  frc::MockMotorController rr;
+  frc::MecanumDrive drive{fl, fr, rl, rr};
+  drive.SetDeadband(0.0);
+
+  // Forward
+  drive.DrivePolar(1.0, 0.0, 0.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Left
+  drive.DrivePolar(1.0, -90.0, 0.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+
+  // Right
+  drive.DrivePolar(1.0, 90.0, 0.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Rotate CCW
+  drive.DrivePolar(0.0, 0.0, -1.0);
+  EXPECT_DOUBLE_EQ(-1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(1.0, rr.Get());
+
+  // Rotate CW
+  drive.DrivePolar(0.0, 0.0, 1.0);
+  EXPECT_DOUBLE_EQ(1.0, fl.Get());
+  EXPECT_DOUBLE_EQ(1.0, fr.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rl.Get());
+  EXPECT_DOUBLE_EQ(-1.0, rr.Get());
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/main.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/main.cpp
index c6b6c58..6aea19a 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <hal/HALBase.h>
 
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
index 172d7c6..5a9f993 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/MockActuatorSendable.cpp
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "shuffleboard/MockActuatorSendable.h"
 
-#include "frc/smartdashboard/SendableRegistry.h"
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
 
-using namespace frc;
-
-MockActuatorSendable::MockActuatorSendable(wpi::StringRef name) {
-  SendableRegistry::GetInstance().Add(this, name);
+MockActuatorSendable::MockActuatorSendable(std::string_view name) {
+  wpi::SendableRegistry::Add(this, name);
 }
 
-void MockActuatorSendable::InitSendable(SendableBuilder& builder) {
+void MockActuatorSendable::InitSendable(wpi::SendableBuilder& builder) {
   builder.SetActuator(true);
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
index ae21526..d370a2d 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
@@ -1,50 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/ShuffleboardInstance.h"  // NOLINT(build/include_order)
 
-#include <memory>
-#include <string>
+#include <string_view>
 
-#include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableInstance.h>
 
 #include "frc/shuffleboard/ShuffleboardInstance.h"
 #include "gtest/gtest.h"
 #include "shuffleboard/MockActuatorSendable.h"
 
-using namespace frc;
+class NTWrapper {
+ public:
+  NTWrapper() { inst = nt::NetworkTableInstance::Create(); }
 
-class ShuffleboardInstanceTest : public testing::Test {
-  void SetUp() override {
-    m_ntInstance = nt::NetworkTableInstance::Create();
-    m_shuffleboardInstance =
-        std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
-  }
+  ~NTWrapper() { nt::NetworkTableInstance::Destroy(inst); }
 
- protected:
-  nt::NetworkTableInstance m_ntInstance;
-  std::unique_ptr<detail::ShuffleboardInstance> m_shuffleboardInstance;
+  nt::NetworkTableInstance inst;
 };
 
-TEST_F(ShuffleboardInstanceTest, PathFluent) {
-  auto entry = m_shuffleboardInstance->GetTab("Tab Title")
-                   .GetLayout("List Layout", "List")
+TEST(ShuffleboardInstanceTest, PathFluent) {
+  NTWrapper ntInst;
+  frc::detail::ShuffleboardInstance shuffleboardInst{ntInst.inst};
+
+  auto entry = shuffleboardInst.GetTab("Tab Title")
+                   .GetLayout("List", "List Layout")
                    .Add("Data", "string")
                    .WithWidget("Text View")
                    .GetEntry();
 
   EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
-  EXPECT_EQ("/Shuffleboard/Tab Title/List Layout/Data", entry.GetName())
+  EXPECT_EQ("/Shuffleboard/Tab Title/List/Data", entry.GetName())
       << "Entry path generated incorrectly";
 }
 
-TEST_F(ShuffleboardInstanceTest, NestedLayoutsFluent) {
-  auto entry = m_shuffleboardInstance->GetTab("Tab")
+TEST(ShuffleboardInstanceTest, NestedLayoutsFluent) {
+  NTWrapper ntInst;
+  frc::detail::ShuffleboardInstance shuffleboardInst{ntInst.inst};
+
+  auto entry = shuffleboardInst.GetTab("Tab")
                    .GetLayout("First", "List")
                    .GetLayout("Second", "List")
                    .GetLayout("Third", "List")
@@ -58,13 +54,16 @@
       << "Entry path generated incorrectly";
 }
 
-TEST_F(ShuffleboardInstanceTest, NestedLayoutsOop) {
-  ShuffleboardTab& tab = m_shuffleboardInstance->GetTab("Tab");
-  ShuffleboardLayout& first = tab.GetLayout("First", "List");
-  ShuffleboardLayout& second = first.GetLayout("Second", "List");
-  ShuffleboardLayout& third = second.GetLayout("Third", "List");
-  ShuffleboardLayout& fourth = third.GetLayout("Fourth", "List");
-  SimpleWidget& widget = fourth.Add("Value", "string");
+TEST(ShuffleboardInstanceTest, NestedLayoutsOop) {
+  NTWrapper ntInst;
+  frc::detail::ShuffleboardInstance shuffleboardInst{ntInst.inst};
+
+  frc::ShuffleboardTab& tab = shuffleboardInst.GetTab("Tab");
+  frc::ShuffleboardLayout& first = tab.GetLayout("First", "List");
+  frc::ShuffleboardLayout& second = first.GetLayout("Second", "List");
+  frc::ShuffleboardLayout& third = second.GetLayout("Third", "List");
+  frc::ShuffleboardLayout& fourth = third.GetLayout("Fourth", "List");
+  frc::SimpleWidget& widget = fourth.Add("Value", "string");
   auto entry = widget.GetEntry();
 
   EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
@@ -73,23 +72,27 @@
       << "Entry path generated incorrectly";
 }
 
-TEST_F(ShuffleboardInstanceTest, LayoutTypeIsSet) {
-  std::string layoutType = "Type";
-  m_shuffleboardInstance->GetTab("Tab").GetLayout("Title", layoutType);
-  m_shuffleboardInstance->Update();
-  nt::NetworkTableEntry entry = m_ntInstance.GetEntry(
+TEST(ShuffleboardInstanceTest, LayoutTypeIsSet) {
+  NTWrapper ntInst;
+  frc::detail::ShuffleboardInstance shuffleboardInst{ntInst.inst};
+
+  std::string_view layoutType = "Type";
+  shuffleboardInst.GetTab("Tab").GetLayout("Title", layoutType);
+  shuffleboardInst.Update();
+  auto entry = ntInst.inst.GetEntry(
       "/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
   EXPECT_EQ(layoutType, entry.GetString("Not Set")) << "Layout type not set";
 }
 
-TEST_F(ShuffleboardInstanceTest, NestedActuatorWidgetsAreDisabled) {
+TEST(ShuffleboardInstanceTest, NestedActuatorWidgetsAreDisabled) {
+  NTWrapper ntInst;
+  frc::detail::ShuffleboardInstance shuffleboardInst{ntInst.inst};
+
   MockActuatorSendable sendable("Actuator");
-  m_shuffleboardInstance->GetTab("Tab")
-      .GetLayout("Title", "Type")
-      .Add(sendable);
+  shuffleboardInst.GetTab("Tab").GetLayout("Title", "Layout").Add(sendable);
   auto controllableEntry =
-      m_ntInstance.GetEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
-  m_shuffleboardInstance->Update();
+      ntInst.inst.GetEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
+  shuffleboardInst.Update();
 
   // Note: we use the unsafe `GetBoolean()` method because if the value is NOT
   // a boolean, or if it is not present, then something has clearly gone very,
@@ -98,7 +101,7 @@
   // Sanity check
   EXPECT_TRUE(controllable)
       << "The nested actuator widget should be enabled by default";
-  m_shuffleboardInstance->DisableActuatorWidgets();
+  shuffleboardInst.DisableActuatorWidgets();
   controllable = controllableEntry.GetValue()->GetBoolean();
   EXPECT_FALSE(controllable)
       << "The nested actuator widget should have been disabled";
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp
index d39d59d..693aac4 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/ShuffleboardTest.cpp
@@ -1,20 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/shuffleboard/Shuffleboard.h"
-#include "frc/shuffleboard/ShuffleboardTab.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
-class ShuffleboardTest : public testing::Test {};
-
-TEST_F(ShuffleboardTest, TabObjectsCached) {
-  ShuffleboardTab& tab1 = Shuffleboard::GetTab("testTabObjectsCached");
-  ShuffleboardTab& tab2 = Shuffleboard::GetTab("testTabObjectsCached");
+TEST(ShuffleboardTest, TabObjectsCached) {
+  auto& tab1 = frc::Shuffleboard::GetTab("testTabObjectsCached");
+  auto& tab2 = frc::Shuffleboard::GetTab("testTabObjectsCached");
   EXPECT_EQ(&tab1, &tab2) << "Tab objects were not cached";
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
index 8e39915..c8449e5 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/shuffleboard/SuppliedValueWidgetTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <networktables/NetworkTableEntry.h>
 #include <networktables/NetworkTableInstance.h>
@@ -14,52 +11,55 @@
 
 using namespace frc;
 
-class SuppliedValueWidgetTest : public testing::Test {
-  void SetUp() override {
-    m_ntInstance = nt::NetworkTableInstance::Create();
-    m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
-    m_tab = &(m_instance->GetTab("Tab"));
-  }
+class NTWrapper {
+ public:
+  NTWrapper() { inst = nt::NetworkTableInstance::Create(); }
 
+  ~NTWrapper() { nt::NetworkTableInstance::Destroy(inst); }
+
+  nt::NetworkTableInstance inst;
+};
+
+class SuppliedValueWidgetTest : public testing::Test {
  protected:
-  nt::NetworkTableInstance m_ntInstance;
-  ShuffleboardTab* m_tab;
-  std::unique_ptr<detail::ShuffleboardInstance> m_instance;
+  NTWrapper m_ntInst;
+  frc::detail::ShuffleboardInstance m_shuffleboardInst{m_ntInst.inst};
+  frc::ShuffleboardTab* m_tab = &(m_shuffleboardInst.GetTab("Tab"));
 };
 
 TEST_F(SuppliedValueWidgetTest, AddString) {
   std::string str = "foo";
   m_tab->AddString("String", [&str]() { return str; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/String");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/String");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   EXPECT_EQ("foo", entry.GetValue()->GetString());
 }
 
 TEST_F(SuppliedValueWidgetTest, AddNumber) {
   int num = 0;
   m_tab->AddNumber("Num", [&num]() { return ++num; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Num");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/Num");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
 }
 
 TEST_F(SuppliedValueWidgetTest, AddBoolean) {
   bool value = true;
   m_tab->AddBoolean("Bool", [&value]() { return value; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Bool");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/Bool");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   EXPECT_EQ(true, entry.GetValue()->GetBoolean());
 }
 
 TEST_F(SuppliedValueWidgetTest, AddStringArray) {
   std::vector<std::string> strings = {"foo", "bar"};
   m_tab->AddStringArray("Strings", [&strings]() { return strings; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Strings");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/Strings");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   auto actual = entry.GetValue()->GetStringArray();
 
   EXPECT_EQ(strings.size(), actual.size());
@@ -71,9 +71,9 @@
 TEST_F(SuppliedValueWidgetTest, AddNumberArray) {
   std::vector<double> nums = {0, 1, 2, 3};
   m_tab->AddNumberArray("Numbers", [&nums]() { return nums; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Numbers");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/Numbers");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   auto actual = entry.GetValue()->GetDoubleArray();
 
   EXPECT_EQ(nums.size(), actual.size());
@@ -85,9 +85,9 @@
 TEST_F(SuppliedValueWidgetTest, AddBooleanArray) {
   std::vector<int> bools = {true, false};
   m_tab->AddBooleanArray("Booleans", [&bools]() { return bools; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Booleans");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/Booleans");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   auto actual = entry.GetValue()->GetBooleanArray();
 
   EXPECT_EQ(bools.size(), actual.size());
@@ -97,11 +97,11 @@
 }
 
 TEST_F(SuppliedValueWidgetTest, AddRaw) {
-  wpi::StringRef bytes = "\1\2\3";
+  std::string_view bytes = "\1\2\3";
   m_tab->AddRaw("Raw", [&bytes]() { return bytes; });
-  auto entry = m_ntInstance.GetEntry("/Shuffleboard/Tab/Raw");
+  auto entry = m_ntInst.inst.GetEntry("/Shuffleboard/Tab/Raw");
 
-  m_instance->Update();
+  m_shuffleboardInst.Update();
   auto actual = entry.GetValue()->GetRaw();
   EXPECT_EQ(bytes, actual);
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp
new file mode 100644
index 0000000..2bec0ed
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADXL345Sim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "frc/ADXL345_I2C.h"
+#include "frc/ADXL345_SPI.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(ADXL345SimTest, SetSpiAttributes) {
+  HAL_Initialize(500, 0);
+
+  ADXL345_SPI accel(SPI::kMXP, Accelerometer::kRange_2G);
+  ADXL345Sim sim(accel);
+
+  EXPECT_EQ(0, accel.GetX());
+  EXPECT_EQ(0, accel.GetY());
+  EXPECT_EQ(0, accel.GetZ());
+
+  sim.SetX(1.91);
+  sim.SetY(-3.405);
+  sim.SetZ(2.29);
+
+  EXPECT_EQ(1.91, accel.GetX());
+  EXPECT_EQ(-3.405, accel.GetY());
+  EXPECT_EQ(2.29, accel.GetZ());
+
+  ADXL345_SPI::AllAxes allAccel = accel.GetAccelerations();
+  EXPECT_EQ(1.91, allAccel.XAxis);
+  EXPECT_EQ(-3.405, allAccel.YAxis);
+  EXPECT_EQ(2.29, allAccel.ZAxis);
+}
+
+TEST(ADXL345SimTest, SetI2CAttribute) {
+  HAL_Initialize(500, 0);
+
+  ADXL345_I2C accel(I2C::kMXP);
+  ADXL345Sim sim(accel);
+
+  EXPECT_EQ(0, accel.GetX());
+  EXPECT_EQ(0, accel.GetY());
+  EXPECT_EQ(0, accel.GetZ());
+
+  sim.SetX(1.91);
+  sim.SetY(-3.405);
+  sim.SetZ(2.29);
+
+  EXPECT_EQ(1.91, accel.GetX());
+  EXPECT_EQ(-3.405, accel.GetY());
+  EXPECT_EQ(2.29, accel.GetZ());
+
+  ADXL345_I2C::AllAxes allAccel = accel.GetAccelerations();
+  EXPECT_EQ(1.91, allAccel.XAxis);
+  EXPECT_EQ(-3.405, allAccel.YAxis);
+  EXPECT_EQ(2.29, allAccel.ZAxis);
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp
new file mode 100644
index 0000000..13f6c00
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADXL362Sim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "frc/ADXL362.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(ADXL362SimTest, SetAttributes) {
+  HAL_Initialize(500, 0);
+
+  ADXL362 accel(SPI::kMXP, Accelerometer::kRange_2G);
+  ADXL362Sim sim(accel);
+
+  EXPECT_EQ(0, accel.GetX());
+  EXPECT_EQ(0, accel.GetY());
+  EXPECT_EQ(0, accel.GetZ());
+
+  sim.SetX(1.91);
+  sim.SetY(-3.405);
+  sim.SetZ(2.29);
+
+  EXPECT_EQ(1.91, accel.GetX());
+  EXPECT_EQ(-3.405, accel.GetY());
+  EXPECT_EQ(2.29, accel.GetZ());
+
+  ADXL362::AllAxes allAccel = accel.GetAccelerations();
+  EXPECT_EQ(1.91, allAccel.XAxis);
+  EXPECT_EQ(-3.405, allAccel.YAxis);
+  EXPECT_EQ(2.29, allAccel.ZAxis);
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp
new file mode 100644
index 0000000..57c4fbe
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/ADXRS450_GyroSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "frc/ADXRS450_Gyro.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(ADXRS450GyroSimTest, SetAttributes) {
+  HAL_Initialize(500, 0);
+
+  ADXRS450_Gyro gyro;
+  ADXRS450_GyroSim sim{gyro};
+
+  EXPECT_EQ(0, gyro.GetAngle());
+  EXPECT_EQ(0, gyro.GetRate());
+
+  constexpr units::degree_t TEST_ANGLE{123.456};
+  constexpr units::degrees_per_second_t TEST_RATE{229.3504};
+  sim.SetAngle(TEST_ANGLE);
+  sim.SetRate(TEST_RATE);
+
+  EXPECT_EQ(TEST_ANGLE.value(), gyro.GetAngle());
+  EXPECT_EQ(TEST_RATE.value(), gyro.GetRate());
+
+  gyro.Reset();
+  EXPECT_EQ(0, gyro.GetAngle());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
index d2c7ea8..3dc8582 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AccelerometerSimTest.cpp
@@ -1,20 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/simulation/BuiltInAccelerometerSim.h"  // NOLINT(build/include_order)
 
 #include <hal/Accelerometer.h>
 #include <hal/HAL.h>
 
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/BuiltInAccelerometer.h"
 #include "gtest/gtest.h"
 
-using namespace frc::sim;
+namespace frc::sim {
 
-TEST(AcclerometerSimTests, TestActiveCallback) {
+TEST(AcclerometerSimTest, ActiveCallback) {
   HAL_Initialize(500, 0);
 
   BuiltInAccelerometerSim sim;
@@ -25,7 +24,7 @@
   bool lastValue = false;
 
   auto cb = sim.RegisterActiveCallback(
-      [&](wpi::StringRef name, const HAL_Value* value) {
+      [&](std::string_view name, const HAL_Value* value) {
         wasTriggered = true;
         lastValue = value->data.v_boolean;
       },
@@ -37,4 +36,103 @@
 
   EXPECT_TRUE(wasTriggered);
   EXPECT_TRUE(lastValue);
+  EXPECT_TRUE(sim.GetActive());
 }
+
+TEST(AcclerometerSimTest, SetX) {
+  HAL_Initialize(500, 0);
+  BuiltInAccelerometerSim sim;
+  sim.ResetData();
+
+  DoubleCallback callback;
+  constexpr double kTestValue = 1.91;
+
+  BuiltInAccelerometer accel;
+  auto cb = sim.RegisterXCallback(callback.GetCallback(), false);
+  sim.SetX(kTestValue);
+  EXPECT_EQ(kTestValue, accel.GetX());
+  EXPECT_EQ(kTestValue, sim.GetX());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestValue, callback.GetLastValue());
+}
+
+TEST(AcclerometerSimTest, SetY) {
+  HAL_Initialize(500, 0);
+  BuiltInAccelerometerSim sim;
+  sim.ResetData();
+
+  DoubleCallback callback;
+  constexpr double kTestValue = 2.29;
+
+  BuiltInAccelerometer accel;
+  auto cb = sim.RegisterYCallback(callback.GetCallback(), false);
+  sim.SetY(kTestValue);
+  EXPECT_EQ(kTestValue, accel.GetY());
+  EXPECT_EQ(kTestValue, sim.GetY());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestValue, callback.GetLastValue());
+}
+
+TEST(AcclerometerSimTest, SetZ) {
+  HAL_Initialize(500, 0);
+
+  BuiltInAccelerometer accel;
+  BuiltInAccelerometerSim sim(accel);
+  sim.ResetData();
+
+  DoubleCallback callback;
+  constexpr double kTestValue = 3.405;
+
+  auto cb = sim.RegisterZCallback(callback.GetCallback(), false);
+  sim.SetZ(kTestValue);
+  EXPECT_EQ(kTestValue, accel.GetZ());
+  EXPECT_EQ(kTestValue, sim.GetZ());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestValue, callback.GetLastValue());
+}
+
+TEST(AcclerometerSimTest, SetRange) {
+  HAL_Initialize(500, 0);
+
+  BuiltInAccelerometerSim sim;
+  sim.ResetData();
+
+  EnumCallback callback;
+
+  Accelerometer::Range range = Accelerometer::kRange_4G;
+  auto cb = sim.RegisterRangeCallback(callback.GetCallback(), false);
+  BuiltInAccelerometer accel(range);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(static_cast<int>(range), sim.GetRange());
+  EXPECT_EQ(static_cast<int>(range), callback.GetLastValue());
+
+  // 2G
+  callback.Reset();
+  range = Accelerometer::kRange_2G;
+  accel.SetRange(range);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(static_cast<int>(range), sim.GetRange());
+  EXPECT_EQ(static_cast<int>(range), callback.GetLastValue());
+
+  // 4G
+  callback.Reset();
+  range = Accelerometer::kRange_4G;
+  accel.SetRange(range);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(static_cast<int>(range), sim.GetRange());
+  EXPECT_EQ(static_cast<int>(range), callback.GetLastValue());
+
+  // 8G
+  callback.Reset();
+  range = Accelerometer::kRange_8G;
+  accel.SetRange(range);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(static_cast<int>(range), sim.GetRange());
+  EXPECT_EQ(static_cast<int>(range), callback.GetLastValue());
+
+  // 16G - Not supported
+  callback.Reset();
+  EXPECT_THROW(accel.SetRange(Accelerometer::kRange_16G), std::runtime_error);
+  EXPECT_FALSE(callback.WasTriggered());
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AddressableLEDSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AddressableLEDSimTest.cpp
new file mode 100644
index 0000000..ad4452e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AddressableLEDSimTest.cpp
@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/AddressableLEDSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/AddressableLED.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(AddressableLEDSimTest, InitializationCallback) {
+  HAL_Initialize(500, 0);
+
+  BooleanCallback callback;
+  AddressableLEDSim sim;
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+
+  EXPECT_FALSE(callback.WasTriggered());
+  AddressableLED led{0};
+
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(AddressableLEDSimTest, SetLength) {
+  HAL_Initialize(500, 0);
+
+  AddressableLED led{0};
+  AddressableLEDSim sim{led};
+  IntCallback callback;
+
+  auto cb = sim.RegisterLengthCallback(callback.GetCallback(), false);
+
+  EXPECT_EQ(1, sim.GetLength());  // Defaults to 1 led
+
+  std::array<AddressableLED::LEDData, 50> ledData;
+  led.SetLength(ledData.max_size());
+  led.SetData(ledData);
+
+  EXPECT_EQ(50, sim.GetLength());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(50, callback.GetLastValue());
+}
+
+TEST(AddressableLEDSimTest, SetRunning) {
+  HAL_Initialize(500, 0);
+
+  AddressableLEDSim sim = AddressableLEDSim::CreateForIndex(0);
+  BooleanCallback callback;
+  auto cb = sim.RegisterRunningCallback(callback.GetCallback(), false);
+  AddressableLED led{0};
+
+  EXPECT_FALSE(sim.GetRunning());
+
+  led.Start();
+  EXPECT_TRUE(sim.GetRunning());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  callback.Reset();
+  led.Stop();
+  EXPECT_FALSE(sim.GetRunning());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(AddressableLEDSimTest, SetData) {
+  AddressableLED led{0};
+  AddressableLEDSim sim = AddressableLEDSim::CreateForChannel(0);
+
+  bool callbackHit = false;
+  std::array<AddressableLED::LEDData, 3> setData;
+  auto cb = sim.RegisterDataCallback(
+      [&](std::string_view, const unsigned char* buffer, unsigned int count) {
+        ASSERT_EQ(count, 12u);
+        EXPECT_EQ(0, buffer[0]);
+        EXPECT_EQ(0, buffer[1]);
+        EXPECT_EQ(255u, buffer[2]);
+        EXPECT_EQ(0, buffer[3]);
+
+        EXPECT_EQ(0, buffer[4]);
+        EXPECT_EQ(255u, buffer[5]);
+        EXPECT_EQ(0, buffer[6]);
+        EXPECT_EQ(0, buffer[7]);
+
+        EXPECT_EQ(255u, buffer[8]);
+        EXPECT_EQ(0, buffer[9]);
+        EXPECT_EQ(0, buffer[10]);
+        EXPECT_EQ(0, buffer[11]);
+
+        callbackHit = true;
+      },
+      false);
+
+  std::array<AddressableLED::LEDData, 3> ledData;
+  led.SetLength(ledData.max_size());
+
+  ledData[0].SetRGB(255, 0, 0);
+  ledData[1].SetRGB(0, 255, 0);
+  ledData[2].SetRGB(0, 0, 255);
+  led.SetData(ledData);
+
+  EXPECT_TRUE(callbackHit);
+
+  std::array<HAL_AddressableLEDData, 3> simData;
+  sim.GetData(simData.data());
+
+  EXPECT_EQ(0xFF, simData[0].r);
+  EXPECT_EQ(0x00, simData[0].g);
+  EXPECT_EQ(0x00, simData[0].b);
+  EXPECT_EQ(0x00, simData[0].padding);
+
+  EXPECT_EQ(0x00, simData[1].r);
+  EXPECT_EQ(0xFF, simData[1].g);
+  EXPECT_EQ(0x00, simData[1].b);
+  EXPECT_EQ(0x00, simData[1].padding);
+
+  EXPECT_EQ(0x00, simData[2].r);
+  EXPECT_EQ(0x00, simData[2].g);
+  EXPECT_EQ(0xFF, simData[2].b);
+  EXPECT_EQ(0x00, simData[2].padding);
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
index f32be7e..375e21a 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogEncoderSimTest.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <hal/HAL.h>
 #include <units/math.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/AnalogEncoder.h"
 #include "frc/AnalogInput.h"
@@ -17,14 +14,14 @@
 #define EXPECT_NEAR_UNITS(val1, val2, eps) \
   EXPECT_LE(units::math::abs(val1 - val2), eps)
 
-TEST(AnalogEncoderSimTest, TestBasic) {
+TEST(AnalogEncoderSimTest, Basic) {
   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,
+  EXPECT_NEAR(encoder.Get().value(), 0.5, 1E-8);
+  EXPECT_NEAR(encoderSim.GetTurns().value(), 0.5, 1E-8);
+  EXPECT_NEAR(encoderSim.GetPosition().Radians().value(), wpi::numbers::pi,
               1E-8);
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp
new file mode 100644
index 0000000..f1baaca
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogGyroSimTest.cpp
@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/AnalogGyroSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/AnalogGyro.h"
+#include "frc/AnalogInput.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(AnalogGyroSimTest, InitializeGyro) {
+  HAL_Initialize(500, 0);
+  AnalogGyroSim sim{0};
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback initializedCallback;
+
+  auto cb =
+      sim.RegisterInitializedCallback(initializedCallback.GetCallback(), false);
+  AnalogGyro gyro(0);
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(initializedCallback.WasTriggered());
+  EXPECT_TRUE(initializedCallback.GetLastValue());
+}
+
+TEST(AnalogGyroSimTest, SetAngle) {
+  HAL_Initialize(500, 0);
+
+  AnalogGyroSim sim{0};
+  DoubleCallback callback;
+
+  AnalogInput ai(0);
+  AnalogGyro gyro(&ai);
+  auto cb = sim.RegisterAngleCallback(callback.GetCallback(), false);
+  EXPECT_EQ(0, gyro.GetAngle());
+
+  constexpr double kTestAngle = 35.04;
+  sim.SetAngle(kTestAngle);
+  EXPECT_EQ(kTestAngle, sim.GetAngle());
+  EXPECT_EQ(kTestAngle, gyro.GetAngle());
+  EXPECT_EQ(-kTestAngle, gyro.GetRotation2d().Degrees().value());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestAngle, callback.GetLastValue());
+}
+
+TEST(AnalogGyroSimTest, SetRate) {
+  HAL_Initialize(500, 0);
+
+  AnalogGyroSim sim{0};
+  DoubleCallback callback;
+
+  std::shared_ptr<AnalogInput> ai(new AnalogInput(0));
+  AnalogGyro gyro(ai);
+  auto cb = sim.RegisterRateCallback(callback.GetCallback(), false);
+  EXPECT_EQ(0, gyro.GetRate());
+
+  constexpr double kTestRate = -19.1;
+  sim.SetRate(kTestRate);
+  EXPECT_EQ(kTestRate, sim.GetRate());
+  EXPECT_EQ(kTestRate, gyro.GetRate());
+
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestRate, callback.GetLastValue());
+}
+
+TEST(AnalogGyroSimTest, Reset) {
+  HAL_Initialize(500, 0);
+
+  AnalogInput ai{0};
+  AnalogGyro gyro(&ai);
+  AnalogGyroSim sim(gyro);
+  sim.SetAngle(12.34);
+  sim.SetRate(43.21);
+
+  sim.ResetData();
+  EXPECT_EQ(0, sim.GetAngle());
+  EXPECT_EQ(0, sim.GetRate());
+  EXPECT_EQ(0, gyro.GetAngle());
+  EXPECT_EQ(0, gyro.GetRate());
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogInputSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogInputSimTest.cpp
new file mode 100644
index 0000000..8b4569e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogInputSimTest.cpp
@@ -0,0 +1,188 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/AnalogInputSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/AnalogInput.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(AnalogInputSimTest, SetInitialized) {
+  HAL_Initialize(500, 0);
+
+  AnalogInputSim sim{5};
+  BooleanCallback callback;
+
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+  AnalogInput input{5};
+
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, SetVoltage) {
+  HAL_Initialize(500, 0);
+
+  AnalogInputSim sim{5};
+  DoubleCallback callback;
+
+  auto cb = sim.RegisterVoltageCallback(callback.GetCallback(), false);
+  AnalogInput input{5};
+
+  for (int i = 0; i < 50; ++i) {
+    double voltage = i * .1;
+
+    callback.Reset();
+
+    sim.SetVoltage(0);
+    EXPECT_NEAR(sim.GetVoltage(), 0, 0.001) << " on " << i;
+    EXPECT_NEAR(input.GetVoltage(), 0, 0.001) << " on " << i;
+    // 0 -> 0 isn't a change, so callback not called
+    if (i > 2) {
+      EXPECT_TRUE(callback.WasTriggered()) << " on " << i;
+      EXPECT_EQ(0, callback.GetLastValue()) << " on " << i;
+    }
+
+    callback.Reset();
+    sim.SetVoltage(voltage);
+    EXPECT_NEAR(sim.GetVoltage(), voltage, 0.001) << " on " << i;
+    EXPECT_NEAR(input.GetVoltage(), voltage, 0.001) << " on " << i;
+
+    // 0 -> 0 isn't a change, so callback not called
+    if (i != 0) {
+      EXPECT_TRUE(callback.WasTriggered()) << " on " << i;
+      EXPECT_EQ(voltage, callback.GetLastValue()) << " on " << i;
+    }
+  }
+}
+
+TEST(AnalogInputSimTest, SetOverSampleBits) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{5};
+  AnalogInputSim sim(input);
+
+  IntCallback callback;
+  auto cb = sim.RegisterOversampleBitsCallback(callback.GetCallback(), false);
+
+  input.SetOversampleBits(3504);
+  EXPECT_EQ(3504, sim.GetOversampleBits());
+  EXPECT_EQ(3504, input.GetOversampleBits());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504, callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, SetAverageBits) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{5};
+  AnalogInputSim sim(input);
+
+  IntCallback callback;
+  auto cb = sim.RegisterAverageBitsCallback(callback.GetCallback(), false);
+
+  input.SetAverageBits(3504);
+  EXPECT_EQ(3504, sim.GetAverageBits());
+  EXPECT_EQ(3504, input.GetAverageBits());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504, callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, InitAccumulator) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{0};
+  AnalogInputSim sim(input);
+
+  BooleanCallback callback;
+  auto cb =
+      sim.RegisterAccumulatorInitializedCallback(callback.GetCallback(), false);
+
+  input.InitAccumulator();
+  input.ResetAccumulator();
+  EXPECT_TRUE(sim.GetAccumulatorInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, InitAccumulatorOnInvalidPort) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{5};
+  AnalogInputSim sim(input);
+
+  BooleanCallback callback;
+  auto cb =
+      sim.RegisterAccumulatorInitializedCallback(callback.GetCallback(), false);
+
+  EXPECT_THROW(input.InitAccumulator(), std::runtime_error);
+  EXPECT_FALSE(callback.WasTriggered());
+}
+
+TEST(AnalogInputSimTest, SetAccumulatorValue) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{0};
+  AnalogInputSim sim(input);
+
+  LongCallback callback;
+  auto cb = sim.RegisterAccumulatorValueCallback(callback.GetCallback(), false);
+
+  input.InitAccumulator();
+  sim.SetAccumulatorValue(3504191229);
+  EXPECT_EQ(3504191229, sim.GetAccumulatorValue());
+  EXPECT_EQ(3504191229, input.GetAccumulatorValue());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504191229, callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, SetAccumulatorCount) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{0};
+  AnalogInputSim sim(input);
+
+  LongCallback callback;
+  auto cb = sim.RegisterAccumulatorCountCallback(callback.GetCallback(), false);
+
+  input.InitAccumulator();
+  sim.SetAccumulatorCount(3504191229);
+  EXPECT_EQ(3504191229, sim.GetAccumulatorCount());
+  EXPECT_EQ(3504191229, input.GetAccumulatorCount());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504191229, callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, SetAccumulatorDeadband) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{0};
+  AnalogInputSim sim(input);
+
+  IntCallback callback;
+  auto cb =
+      sim.RegisterAccumulatorDeadbandCallback(callback.GetCallback(), false);
+
+  input.InitAccumulator();
+  input.SetAccumulatorDeadband(3504);
+  EXPECT_EQ(3504, sim.GetAccumulatorDeadband());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504, callback.GetLastValue());
+}
+
+TEST(AnalogInputSimTest, SetAccumulatorCenter) {
+  HAL_Initialize(500, 0);
+  AnalogInput input{0};
+  AnalogInputSim sim(input);
+
+  IntCallback callback;
+  auto cb =
+      sim.RegisterAccumulatorCenterCallback(callback.GetCallback(), false);
+
+  input.InitAccumulator();
+  input.SetAccumulatorCenter(3504);
+  EXPECT_EQ(3504, sim.GetAccumulatorCenter());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504, callback.GetLastValue());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogOutputSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogOutputSimTest.cpp
new file mode 100644
index 0000000..630f2c9
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogOutputSimTest.cpp
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/AnalogOutputSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/AnalogOutput.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(AnalogOutputSimTest, Initialize) {
+  HAL_Initialize(500, 0);
+
+  AnalogOutputSim outputSim{0};
+  EXPECT_FALSE(outputSim.GetInitialized());
+
+  BooleanCallback callback;
+
+  auto cb =
+      outputSim.RegisterInitializedCallback(callback.GetCallback(), false);
+  AnalogOutput output(0);
+  EXPECT_TRUE(outputSim.GetInitialized());
+
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(AnalogOutputSimTest, SetCallback) {
+  HAL_Initialize(500, 0);
+
+  AnalogOutput output{0};
+  output.SetVoltage(0.5);
+
+  AnalogOutputSim outputSim(output);
+
+  DoubleCallback voltageCallback;
+
+  auto cb =
+      outputSim.RegisterVoltageCallback(voltageCallback.GetCallback(), false);
+
+  EXPECT_FALSE(voltageCallback.WasTriggered());
+
+  for (int i = 0; i < 50; ++i) {
+    double voltage = i * .1;
+    voltageCallback.Reset();
+
+    output.SetVoltage(0);
+
+    EXPECT_EQ(0, output.GetVoltage());
+    EXPECT_EQ(0, outputSim.GetVoltage());
+
+    // 0 -> 0 isn't a change, so callback not called
+    if (i > 2) {
+      EXPECT_TRUE(voltageCallback.WasTriggered()) << " on " << i;
+      EXPECT_NEAR(voltageCallback.GetLastValue(), 0, 0.001) << " on " << i;
+    }
+
+    voltageCallback.Reset();
+
+    output.SetVoltage(voltage);
+
+    EXPECT_EQ(voltage, output.GetVoltage());
+    EXPECT_EQ(voltage, outputSim.GetVoltage());
+
+    // 0 -> 0 isn't a change, so callback not called
+    if (i != 0) {
+      EXPECT_TRUE(voltageCallback.WasTriggered());
+      EXPECT_NEAR(voltageCallback.GetLastValue(), voltage, 0.001);
+    }
+  }
+}
+
+TEST(AnalogOutputSimTest, Reset) {
+  HAL_Initialize(500, 0);
+
+  AnalogOutputSim outputSim{0};
+
+  AnalogOutput output{0};
+  output.SetVoltage(1.2);
+
+  outputSim.ResetData();
+  EXPECT_EQ(0, output.GetVoltage());
+  EXPECT_EQ(0, outputSim.GetVoltage());
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogTriggerSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogTriggerSimTest.cpp
new file mode 100644
index 0000000..5e9e28e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/AnalogTriggerSimTest.cpp
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/AnalogTriggerSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/AnalogTrigger.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(AnalogTriggerSimTest, Initialization) {
+  HAL_Initialize(500, 0);
+
+  AnalogTriggerSim sim = AnalogTriggerSim::CreateForIndex(0);
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+
+  AnalogTrigger trigger{0};
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(AnalogTriggerSimTest, TriggerLowerBound) {
+  HAL_Initialize(500, 0);
+
+  AnalogTrigger trigger{0};
+  AnalogTriggerSim sim(trigger);
+
+  DoubleCallback lowerCallback;
+  DoubleCallback upperCallback;
+  auto lowerCb =
+      sim.RegisterTriggerLowerBoundCallback(lowerCallback.GetCallback(), false);
+  auto upperCb =
+      sim.RegisterTriggerUpperBoundCallback(upperCallback.GetCallback(), false);
+
+  trigger.SetLimitsVoltage(0.299, 1.91);
+
+  EXPECT_EQ(0.299, sim.GetTriggerLowerBound());
+  EXPECT_EQ(1.91, sim.GetTriggerUpperBound());
+
+  EXPECT_TRUE(lowerCallback.WasTriggered());
+  EXPECT_EQ(0.299, lowerCallback.GetLastValue());
+
+  EXPECT_TRUE(upperCallback.WasTriggered());
+  EXPECT_EQ(1.91, upperCallback.GetLastValue());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
new file mode 100644
index 0000000..a8f95da
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/CTREPCMSimTest.cpp
@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/CTREPCMSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsControlModule.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(CTREPCMSimTest, InitializedCallback) {
+  CTREPCMSim sim;
+
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+
+  PneumaticsControlModule pcm;
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(CTREPCMSimTest, SolenoidOutput) {
+  PneumaticsControlModule pcm;
+  CTREPCMSim sim(pcm);
+  sim.ResetData();
+
+  DoubleSolenoid doubleSolenoid{0, frc::PneumaticsModuleType::CTREPCM, 3, 4};
+
+  BooleanCallback callback3;
+  BooleanCallback callback4;
+  auto cb3 =
+      sim.RegisterSolenoidOutputCallback(3, callback3.GetCallback(), false);
+  auto cb4 =
+      sim.RegisterSolenoidOutputCallback(4, callback4.GetCallback(), false);
+
+  callback3.Reset();
+  callback4.Reset();
+  doubleSolenoid.Set(DoubleSolenoid::kReverse);
+  EXPECT_FALSE(callback3.WasTriggered());
+  EXPECT_FALSE(callback3.GetLastValue());
+  EXPECT_TRUE(callback4.WasTriggered());
+  EXPECT_TRUE(callback4.GetLastValue());
+  EXPECT_FALSE(sim.GetSolenoidOutput(3));
+  EXPECT_TRUE(sim.GetSolenoidOutput(4));
+  EXPECT_EQ(0b00010000, pcm.GetSolenoids());
+  EXPECT_EQ(0b00010000, sim.GetAllSolenoidOutputs());
+
+  callback3.Reset();
+  callback4.Reset();
+  doubleSolenoid.Set(DoubleSolenoid::kForward);
+  EXPECT_TRUE(callback3.WasTriggered());
+  EXPECT_TRUE(callback3.GetLastValue());
+  EXPECT_TRUE(callback4.WasTriggered());
+  EXPECT_FALSE(callback4.GetLastValue());
+  EXPECT_TRUE(sim.GetSolenoidOutput(3));
+  EXPECT_FALSE(sim.GetSolenoidOutput(4));
+  EXPECT_EQ(0b00001000, pcm.GetSolenoids());
+  EXPECT_EQ(0b00001000, sim.GetAllSolenoidOutputs());
+
+  callback3.Reset();
+  callback4.Reset();
+  doubleSolenoid.Set(DoubleSolenoid::kOff);
+  EXPECT_TRUE(callback3.WasTriggered());
+  EXPECT_FALSE(callback3.GetLastValue());
+  EXPECT_FALSE(callback4.WasTriggered());
+  EXPECT_FALSE(callback4.GetLastValue());
+  EXPECT_FALSE(sim.GetSolenoidOutput(3));
+  EXPECT_FALSE(sim.GetSolenoidOutput(4));
+  EXPECT_EQ(0b00000000, pcm.GetSolenoids());
+  EXPECT_EQ(0b00000000, sim.GetAllSolenoidOutputs());
+}
+
+TEST(CTREPCMSimTest, SetCompressorOn) {
+  PneumaticsControlModule pcm;
+  CTREPCMSim sim(pcm);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterCompressorOnCallback(callback.GetCallback(), false);
+
+  EXPECT_FALSE(pcm.GetCompressor());
+  EXPECT_FALSE(pcm.GetCompressor());
+  sim.SetCompressorOn(true);
+  EXPECT_TRUE(sim.GetCompressorOn());
+  EXPECT_TRUE(pcm.GetCompressor());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(CTREPCMSimTest, SetClosedLoopEnabled) {
+  PneumaticsControlModule pcm;
+  CTREPCMSim sim(pcm);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb =
+      sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false);
+
+  pcm.SetClosedLoopControl(false);
+  EXPECT_FALSE(pcm.GetClosedLoopControl());
+
+  pcm.SetClosedLoopControl(true);
+  EXPECT_TRUE(sim.GetClosedLoopEnabled());
+  EXPECT_TRUE(pcm.GetClosedLoopControl());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(CTREPCMSimTest, SetPressureSwitchEnabled) {
+  PneumaticsControlModule pcm;
+  CTREPCMSim sim(pcm);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterPressureSwitchCallback(callback.GetCallback(), false);
+
+  EXPECT_FALSE(pcm.GetPressureSwitch());
+
+  sim.SetPressureSwitch(true);
+  EXPECT_TRUE(sim.GetPressureSwitch());
+  EXPECT_TRUE(pcm.GetPressureSwitch());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(CTREPCMSimTest, SetCompressorCurrent) {
+  PneumaticsControlModule pcm;
+  CTREPCMSim sim(pcm);
+  sim.ResetData();
+
+  DoubleCallback callback;
+  auto cb =
+      sim.RegisterCompressorCurrentCallback(callback.GetCallback(), false);
+
+  sim.SetCompressorCurrent(35.04);
+  EXPECT_EQ(35.04, sim.GetCompressorCurrent());
+  EXPECT_EQ(35.04, pcm.GetCompressorCurrent());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(35.04, callback.GetLastValue());
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DIOSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DIOSimTest.cpp
new file mode 100644
index 0000000..338d24e
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DIOSimTest.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/DIOSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DigitalInput.h"
+#include "frc/DigitalOutput.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(DIOSimTest, Initialization) {
+  HAL_Initialize(500, 0);
+  DIOSim sim{2};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback initializeCallback;
+  BooleanCallback isInputCallback;
+
+  auto initializeCb =
+      sim.RegisterInitializedCallback(initializeCallback.GetCallback(), false);
+  auto inputCb =
+      sim.RegisterIsInputCallback(isInputCallback.GetCallback(), false);
+
+  DigitalOutput output(2);
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(initializeCallback.WasTriggered());
+  EXPECT_TRUE(initializeCallback.GetLastValue());
+  EXPECT_FALSE(sim.GetIsInput());
+  EXPECT_TRUE(isInputCallback.WasTriggered());
+  EXPECT_FALSE(isInputCallback.GetLastValue());
+
+  initializeCallback.Reset();
+  sim.SetInitialized(false);
+  EXPECT_TRUE(initializeCallback.WasTriggered());
+  EXPECT_FALSE(initializeCallback.GetLastValue());
+}
+
+TEST(DIOSimTest, Input) {
+  HAL_Initialize(500, 0);
+
+  DigitalInput input{0};
+  DIOSim sim(input);
+  EXPECT_TRUE(sim.GetIsInput());
+
+  BooleanCallback valueCallback;
+
+  auto cb = sim.RegisterValueCallback(valueCallback.GetCallback(), false);
+  EXPECT_TRUE(input.Get());
+  EXPECT_TRUE(sim.GetValue());
+
+  EXPECT_FALSE(valueCallback.WasTriggered());
+  sim.SetValue(false);
+  EXPECT_TRUE(valueCallback.WasTriggered());
+  EXPECT_FALSE(valueCallback.GetLastValue());
+}
+
+TEST(DIOSimTest, Output) {
+  HAL_Initialize(500, 0);
+  DigitalOutput output{0};
+  DIOSim sim(output);
+  EXPECT_FALSE(sim.GetIsInput());
+
+  BooleanCallback valueCallback;
+
+  auto cb = sim.RegisterValueCallback(valueCallback.GetCallback(), false);
+  EXPECT_FALSE(output.Get());
+  EXPECT_FALSE(sim.GetValue());
+
+  EXPECT_FALSE(valueCallback.WasTriggered());
+  output.Set(true);
+  EXPECT_TRUE(valueCallback.WasTriggered());
+  EXPECT_TRUE(valueCallback.GetLastValue());
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
index f09344e..eabbecb 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DifferentialDriveSimTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <units/current.h>
 #include <units/math.h>
@@ -13,29 +10,30 @@
 #include "frc/controller/RamseteController.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/simulation/DifferentialDrivetrainSim.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.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) {
+TEST(DifferentialDriveSimTest, 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::sim::DifferentialDrivetrainSim sim{
+      plant, 24_in, motor,
+      1.0,   2_in,  {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
 
   frc::LinearPlantInversionFeedforward feedforward{plant, 20_ms};
   frc::RamseteController ramsete;
 
-  feedforward.Reset(frc::MakeMatrix<2, 1>(0.0, 0.0));
+  feedforward.Reset(Eigen::Vector<double, 2>{0.0, 0.0});
 
   // Ground truth.
-  Eigen::Matrix<double, 7, 1> groundTruthX =
-      Eigen::Matrix<double, 7, 1>::Zero();
+  Eigen::Vector<double, 7> groundTruthX = Eigen::Vector<double, 7>::Zero();
 
   frc::TrajectoryConfig config{1_mps, 1_mps_sq};
   config.AddConstraint(
@@ -44,32 +42,35 @@
   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) {
+  // NOLINTNEXTLINE
+  for (double t = 0; t < trajectory.TotalTime().value(); 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>()));
+    auto voltages =
+        feedforward.Calculate(Eigen::Vector<double, 2>{l.value(), r.value()});
 
     // 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> {
+    groundTruthX = frc::RK4(
+        [&sim](const auto& x, const auto& u) -> Eigen::Vector<double, 7> {
           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);
+  // 2 inch tolerance is OK since our ground truth is an approximation of the
+  // ODE solution using RK4 anyway
+  EXPECT_NEAR(groundTruthX(0, 0), sim.GetPose().X().value(), 0.05);
+  EXPECT_NEAR(groundTruthX(1, 0), sim.GetPose().Y().value(), 0.05);
+  EXPECT_NEAR(groundTruthX(2, 0), sim.GetHeading().Radians().value(), 0.01);
 }
 
-TEST(DifferentialDriveSim, Current) {
+TEST(DifferentialDriveSimTest, 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);
@@ -96,7 +97,7 @@
   EXPECT_TRUE(sim.GetCurrentDraw() > 0_A);
 }
 
-TEST(DifferentialDriveSim, ModelStability) {
+TEST(DifferentialDriveSimTest, 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);
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DigitalPWMSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DigitalPWMSimTest.cpp
new file mode 100644
index 0000000..fd62edc
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DigitalPWMSimTest.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/DigitalPWMSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DigitalOutput.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(DigitalPWMSimTest, Initialize) {
+  HAL_Initialize(500, 0);
+
+  DigitalOutput output{0};
+  DigitalPWMSim sim(output);
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback initializeCallback;
+  auto initCb =
+      sim.RegisterInitializedCallback(initializeCallback.GetCallback(), false);
+
+  DoubleCallback dutyCycleCallback;
+  auto dutyCycleCB =
+      sim.RegisterDutyCycleCallback(dutyCycleCallback.GetCallback(), false);
+
+  constexpr double kTestDutyCycle = 0.191;
+  output.EnablePWM(kTestDutyCycle);
+
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(initializeCallback.WasTriggered());
+  EXPECT_TRUE(initializeCallback.GetLastValue());
+
+  EXPECT_EQ(kTestDutyCycle, sim.GetDutyCycle());
+  EXPECT_TRUE(dutyCycleCallback.WasTriggered());
+  EXPECT_EQ(kTestDutyCycle, dutyCycleCallback.GetLastValue());
+}
+
+TEST(DigitalPWMSimTest, SetPin) {
+  HAL_Initialize(500, 0);
+
+  DigitalOutput output{2};
+  DigitalPWMSim sim(output);
+
+  IntCallback callback;
+  auto cb = sim.RegisterPinCallback(callback.GetCallback(), false);
+
+  sim.SetPin(191);
+  EXPECT_EQ(191, sim.GetPin());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(191, callback.GetLastValue());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp
new file mode 100644
index 0000000..d01ca27
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp
@@ -0,0 +1,238 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <string>
+#include <tuple>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DriverStation.h"
+#include "frc/Joystick.h"
+#include "frc/RobotState.h"
+#include "frc/simulation/DriverStationSim.h"
+#include "frc/simulation/SimHooks.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+TEST(DriverStationTest, Enabled) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  EXPECT_FALSE(DriverStation::IsEnabled());
+  BooleanCallback callback;
+  auto cb =
+      DriverStationSim::RegisterEnabledCallback(callback.GetCallback(), false);
+  DriverStationSim::SetEnabled(true);
+  DriverStationSim::NotifyNewData();
+  EXPECT_TRUE(DriverStationSim::GetEnabled());
+  EXPECT_TRUE(DriverStation::IsEnabled());
+  EXPECT_TRUE(RobotState::IsEnabled());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(DriverStationTest, AutonomousMode) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  EXPECT_FALSE(DriverStation::IsAutonomous());
+  BooleanCallback callback;
+  auto cb = DriverStationSim::RegisterAutonomousCallback(callback.GetCallback(),
+                                                         false);
+  DriverStationSim::SetAutonomous(true);
+  DriverStationSim::NotifyNewData();
+  EXPECT_TRUE(DriverStationSim::GetAutonomous());
+  EXPECT_TRUE(DriverStation::IsAutonomous());
+  EXPECT_TRUE(RobotState::IsAutonomous());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(DriverStationTest, Mode) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  EXPECT_FALSE(DriverStation::IsTest());
+  BooleanCallback callback;
+  auto cb =
+      DriverStationSim::RegisterTestCallback(callback.GetCallback(), false);
+  DriverStationSim::SetTest(true);
+  DriverStationSim::NotifyNewData();
+  EXPECT_TRUE(DriverStationSim::GetTest());
+  EXPECT_TRUE(DriverStation::IsTest());
+  EXPECT_TRUE(RobotState::IsTest());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(DriverStationTest, Estop) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  EXPECT_FALSE(DriverStation::IsEStopped());
+  BooleanCallback callback;
+  auto cb =
+      DriverStationSim::RegisterEStopCallback(callback.GetCallback(), false);
+  DriverStationSim::SetEStop(true);
+  DriverStationSim::NotifyNewData();
+  EXPECT_TRUE(DriverStationSim::GetEStop());
+  EXPECT_TRUE(DriverStation::IsEStopped());
+  EXPECT_TRUE(RobotState::IsEStopped());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(DriverStationTest, FmsAttached) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  EXPECT_FALSE(DriverStation::IsFMSAttached());
+  BooleanCallback callback;
+  auto cb = DriverStationSim::RegisterFmsAttachedCallback(
+      callback.GetCallback(), false);
+  DriverStationSim::SetFmsAttached(true);
+  DriverStationSim::NotifyNewData();
+  EXPECT_TRUE(DriverStationSim::GetFmsAttached());
+  EXPECT_TRUE(DriverStation::IsFMSAttached());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(DriverStationTest, DsAttached) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  DriverStationSim::NotifyNewData();
+  EXPECT_TRUE(DriverStation::IsDSAttached());
+
+  BooleanCallback callback;
+  auto cb = DriverStationSim::RegisterDsAttachedCallback(callback.GetCallback(),
+                                                         false);
+  DriverStationSim::SetDsAttached(false);
+  DriverStationSim::NotifyNewData();
+  EXPECT_FALSE(DriverStationSim::GetDsAttached());
+  EXPECT_FALSE(DriverStation::IsDSAttached());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(DriverStationTest, AllianceStationId) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  EnumCallback callback;
+
+  HAL_AllianceStationID allianceStation = HAL_AllianceStationID_kBlue2;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+
+  auto cb = DriverStationSim::RegisterAllianceStationIdCallback(
+      callback.GetCallback(), false);
+  // B1
+  allianceStation = HAL_AllianceStationID_kBlue1;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+  EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
+  EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance());
+  EXPECT_EQ(1, DriverStation::GetLocation());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(allianceStation, callback.GetLastValue());
+
+  // B2
+  allianceStation = HAL_AllianceStationID_kBlue2;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+  EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
+  EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance());
+  EXPECT_EQ(2, DriverStation::GetLocation());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(allianceStation, callback.GetLastValue());
+
+  // B3
+  allianceStation = HAL_AllianceStationID_kBlue3;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+  EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
+  EXPECT_EQ(DriverStation::kBlue, DriverStation::GetAlliance());
+  EXPECT_EQ(3, DriverStation::GetLocation());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(allianceStation, callback.GetLastValue());
+
+  // R1
+  allianceStation = HAL_AllianceStationID_kRed1;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+  EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
+  EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance());
+  EXPECT_EQ(1, DriverStation::GetLocation());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(allianceStation, callback.GetLastValue());
+
+  // R2
+  allianceStation = HAL_AllianceStationID_kRed2;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+  EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
+  EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance());
+  EXPECT_EQ(2, DriverStation::GetLocation());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(allianceStation, callback.GetLastValue());
+
+  // R3
+  allianceStation = HAL_AllianceStationID_kRed3;
+  DriverStationSim::SetAllianceStationId(allianceStation);
+  EXPECT_EQ(allianceStation, DriverStationSim::GetAllianceStationId());
+  EXPECT_EQ(DriverStation::kRed, DriverStation::GetAlliance());
+  EXPECT_EQ(3, DriverStation::GetLocation());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(allianceStation, callback.GetLastValue());
+}
+
+TEST(DriverStationTest, ReplayNumber) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  DriverStationSim::SetReplayNumber(4);
+  DriverStationSim::NotifyNewData();
+  EXPECT_EQ(4, DriverStation::GetReplayNumber());
+}
+
+TEST(DriverStationTest, MatchNumber) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  DriverStationSim::SetMatchNumber(3);
+  DriverStationSim::NotifyNewData();
+  EXPECT_EQ(3, DriverStation::GetMatchNumber());
+}
+
+TEST(DriverStationTest, MatchTime) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  DoubleCallback callback;
+  auto cb = DriverStationSim::RegisterMatchTimeCallback(callback.GetCallback(),
+                                                        false);
+  constexpr double kTestTime = 19.174;
+  DriverStationSim::SetMatchTime(kTestTime);
+  EXPECT_EQ(kTestTime, DriverStationSim::GetMatchTime());
+  EXPECT_EQ(kTestTime, DriverStation::GetMatchTime());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestTime, callback.GetLastValue());
+}
+
+TEST(DriverStationTest, SetGameSpecificMessage) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  constexpr auto message = "Hello World!";
+  DriverStationSim::SetGameSpecificMessage(message);
+  DriverStationSim::NotifyNewData();
+  EXPECT_EQ(message, DriverStation::GetGameSpecificMessage());
+}
+
+TEST(DriverStationTest, SetEventName) {
+  HAL_Initialize(500, 0);
+  DriverStationSim::ResetData();
+
+  constexpr auto message = "The Best Event";
+  DriverStationSim::SetEventName(message);
+  DriverStationSim::NotifyNewData();
+  EXPECT_EQ(message, DriverStation::GetEventName());
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp
new file mode 100644
index 0000000..8249499
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DutyCycleEncoderSimTest.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/DutyCycleEncoderSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DutyCycleEncoder.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(DutyCycleEncoderSimTest, Set) {
+  HAL_Initialize(500, 0);
+
+  DutyCycleEncoder enc{0};
+  DutyCycleEncoderSim sim(enc);
+
+  constexpr units::turn_t kTestValue{5.67};
+  sim.Set(kTestValue);
+  EXPECT_EQ(kTestValue, enc.Get());
+}
+
+TEST(DutyCycleEncoderSimTest, SetDistance) {
+  HAL_Initialize(500, 0);
+
+  DutyCycleEncoder enc{0};
+  DutyCycleEncoderSim sim(enc);
+  sim.SetDistance(19.1);
+  EXPECT_EQ(19.1, enc.GetDistance());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DutyCycleSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DutyCycleSimTest.cpp
new file mode 100644
index 0000000..56e0592
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/DutyCycleSimTest.cpp
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/DutyCycleSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DigitalInput.h"
+#include "frc/DutyCycle.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(DutyCycleSimTest, Initialization) {
+  HAL_Initialize(500, 0);
+  DutyCycleSim sim = DutyCycleSim::CreateForIndex(0);
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+
+  DigitalInput di{2};
+  DutyCycle dc{&di};
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  callback.Reset();
+  sim.SetInitialized(false);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(DutyCycleSimTest, SetFrequency) {
+  HAL_Initialize(500, 0);
+
+  DigitalInput di{2};
+  DutyCycle dc{di};
+  DutyCycleSim sim(dc);
+
+  IntCallback callback;
+  auto cb = sim.RegisterFrequencyCallback(callback.GetCallback(), false);
+
+  sim.SetFrequency(191);
+  EXPECT_EQ(191, sim.GetFrequency());
+  EXPECT_EQ(191, dc.GetFrequency());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(191, callback.GetLastValue());
+}
+
+TEST(DutyCycleSimTest, SetOutput) {
+  HAL_Initialize(500, 0);
+
+  DigitalInput di{2};
+  DutyCycle dc{di};
+  DutyCycleSim sim(dc);
+
+  DoubleCallback callback;
+  auto cb = sim.RegisterOutputCallback(callback.GetCallback(), false);
+
+  sim.SetOutput(229.174);
+  EXPECT_EQ(229.174, sim.GetOutput());
+  EXPECT_EQ(229.174, dc.GetOutput());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(229.174, callback.GetLastValue());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
index 05b0dc9..bfc50e1 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/ElevatorSimTest.cpp
@@ -1,26 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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/motorcontrol/PWMVictorSPX.h"
 #include "frc/simulation/ElevatorSim.h"
 #include "frc/simulation/EncoderSim.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "gtest/gtest.h"
 
-TEST(ElevatorSim, StateSpaceSim) {
+TEST(ElevatorSimTest, 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});
@@ -35,8 +32,8 @@
     auto nextVoltage = controller.Calculate(encoderSim.GetDistance());
     motor.Set(nextVoltage / frc::RobotController::GetInputVoltage());
 
-    auto u = frc::MakeMatrix<1, 1>(motor.Get() *
-                                   frc::RobotController::GetInputVoltage());
+    Eigen::Vector<double, 1> u{motor.Get() *
+                               frc::RobotController::GetInputVoltage()};
     sim.SetInput(u);
     sim.Update(20_ms);
 
@@ -44,15 +41,15 @@
     encoderSim.SetDistance(y(0));
   }
 
-  EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().to<double>(), 0.2);
+  EXPECT_NEAR(controller.GetSetpoint(), sim.GetPosition().value(), 0.2);
 }
 
-TEST(ElevatorSim, MinMax) {
+TEST(ElevatorSimTest, 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.SetInput(Eigen::Vector<double, 1>{0.0});
     sim.Update(20_ms);
 
     auto height = sim.GetPosition();
@@ -60,10 +57,35 @@
   }
 
   for (size_t i = 0; i < 100; ++i) {
-    sim.SetInput(frc::MakeMatrix<1, 1>(12.0));
+    sim.SetInput(Eigen::Vector<double, 1>{12.0});
     sim.Update(20_ms);
 
     auto height = sim.GetPosition();
     EXPECT_TRUE(height < 1.05_m);
   }
 }
+
+TEST(ElevatorSimTest, Stability) {
+  static constexpr double kElevatorGearing = 100.0;
+  static constexpr units::meter_t kElevatorDrumRadius = 0.5_in;
+  static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
+  frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
+
+  frc::LinearSystem<2, 1, 1> system = frc::LinearSystemId::ElevatorSystem(
+      m_elevatorGearbox, kCarriageMass, kElevatorDrumRadius, kElevatorGearing);
+
+  Eigen::Vector<double, 2> x0{0.0, 0.0};
+  Eigen::Vector<double, 1> u0{12.0};
+
+  Eigen::Vector<double, 2> x1{0.0, 0.0};
+  for (size_t i = 0; i < 50; i++) {
+    x1 = frc::RKDP(
+        [&](const Eigen::Vector<double, 2>& x,
+            const Eigen::Vector<double, 1>& u) -> Eigen::Vector<double, 2> {
+          return system.A() * x + system.B() * u;
+        },
+        x1, u0, 0.020_s);
+  }
+
+  EXPECT_NEAR(x1(0), system.CalculateX(x0, u0, 1_s)(0), 0.1);
+}
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp
new file mode 100644
index 0000000..be13ca5
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/EncoderSimTest.cpp
@@ -0,0 +1,230 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/EncoderSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/Encoder.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+namespace {
+constexpr double kDefaultDistancePerPulse = .0005;
+}  // namespace
+
+TEST(EncoderSimTest, Initialize) {
+  HAL_Initialize(500, 0);
+
+  EncoderSim sim = EncoderSim::CreateForIndex(0);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+
+  Encoder encoder(0, 1);
+
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, Rate) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  sim.SetRate(1.91);
+  EXPECT_EQ(1.91, sim.GetRate());
+}
+
+TEST(EncoderSimTest, Count) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  IntCallback callback;
+  auto cb = sim.RegisterCountCallback(callback.GetCallback(), false);
+  sim.SetCount(3504);
+  EXPECT_EQ(3504, sim.GetCount());
+
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504, encoder.Get());
+  EXPECT_EQ(3504, callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, Distance) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  sim.SetDistance(229.174);
+  EXPECT_EQ(229.174, sim.GetDistance());
+  EXPECT_EQ(229.174, encoder.GetDistance());
+}
+
+TEST(EncoderSimTest, Period) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  DoubleCallback callback;
+  auto cb = sim.RegisterPeriodCallback(callback.GetCallback(), false);
+  sim.SetPeriod(123.456);
+  EXPECT_EQ(123.456, sim.GetPeriod());
+  EXPECT_EQ(123.456, encoder.GetPeriod().value());
+  EXPECT_EQ(kDefaultDistancePerPulse / 123.456, encoder.GetRate());
+
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(123.456, callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, SetMaxPeriod) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  DoubleCallback callback;
+  auto cb = sim.RegisterMaxPeriodCallback(callback.GetCallback(), false);
+
+  encoder.SetMaxPeriod(units::second_t{123.456});
+  EXPECT_EQ(123.456, sim.GetMaxPeriod());
+
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(123.456, callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, SetDirection) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterDirectionCallback(callback.GetCallback(), false);
+
+  sim.SetDirection(true);
+  EXPECT_TRUE(sim.GetDirection());
+  EXPECT_TRUE(encoder.GetDirection());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  sim.SetDirection(false);
+  EXPECT_FALSE(sim.GetDirection());
+  EXPECT_FALSE(encoder.GetDirection());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, SetReverseDirection) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterReverseDirectionCallback(callback.GetCallback(), false);
+
+  encoder.SetReverseDirection(true);
+  EXPECT_TRUE(sim.GetReverseDirection());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  encoder.SetReverseDirection(false);
+  EXPECT_FALSE(sim.GetReverseDirection());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, SetSamplesToAverage) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  IntCallback callback;
+  auto cb = sim.RegisterSamplesToAverageCallback(callback.GetCallback(), false);
+
+  encoder.SetSamplesToAverage(57);
+  EXPECT_EQ(57, sim.GetSamplesToAverage());
+  EXPECT_EQ(57, encoder.GetSamplesToAverage());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(57, callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, SetDistancePerPulse) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  DoubleCallback callback;
+  auto cb = sim.RegisterDistancePerPulseCallback(callback.GetCallback(), false);
+
+  encoder.SetDistancePerPulse(.03405);
+  EXPECT_EQ(.03405, sim.GetDistancePerPulse());
+  EXPECT_EQ(.03405, encoder.GetDistancePerPulse());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(.03405, callback.GetLastValue());
+}
+
+TEST(EncoderSimTest, Reset) {
+  HAL_Initialize(500, 0);
+
+  Encoder encoder(0, 1);
+  EncoderSim sim(encoder);
+  sim.ResetData();
+
+  encoder.SetDistancePerPulse(kDefaultDistancePerPulse);
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterResetCallback(callback.GetCallback(), false);
+
+  sim.SetCount(3504);
+  sim.SetDistance(229.191);
+
+  encoder.Reset();
+  EXPECT_TRUE(sim.GetReset());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  EXPECT_EQ(0, sim.GetCount());
+  EXPECT_EQ(0, encoder.Get());
+  EXPECT_EQ(0, sim.GetDistance());
+  EXPECT_EQ(0, encoder.GetDistance());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp
new file mode 100644
index 0000000..78711b1
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/PDPSimTest.cpp
@@ -0,0 +1,82 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/PowerDistributionSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/PowerDistribution.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(PowerDistributionSimTest, Initialize) {
+  HAL_Initialize(500, 0);
+  PowerDistributionSim sim{2};
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+  PowerDistribution pdp(2, frc::PowerDistribution::ModuleType::kCTRE);
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  callback.Reset();
+  sim.SetInitialized(false);
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(PowerDistributionSimTest, SetTemperature) {
+  HAL_Initialize(500, 0);
+  PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kCTRE};
+  PowerDistributionSim sim(pdp);
+
+  DoubleCallback callback;
+  auto cb = sim.RegisterTemperatureCallback(callback.GetCallback(), false);
+
+  sim.SetTemperature(35.04);
+  EXPECT_EQ(35.04, sim.GetTemperature());
+  EXPECT_EQ(35.04, pdp.GetTemperature());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(PowerDistributionSimTest, SetVoltage) {
+  HAL_Initialize(500, 0);
+  PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kCTRE};
+  PowerDistributionSim sim(pdp);
+
+  DoubleCallback callback;
+  auto cb = sim.RegisterVoltageCallback(callback.GetCallback(), false);
+
+  sim.SetVoltage(35.04);
+  EXPECT_EQ(35.04, sim.GetVoltage());
+  EXPECT_EQ(35.04, pdp.GetVoltage());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(PowerDistributionSimTest, SetCurrent) {
+  HAL_Initialize(500, 0);
+  PowerDistribution pdp{2, frc::PowerDistribution::ModuleType::kCTRE};
+  PowerDistributionSim sim(pdp);
+
+  for (int channel = 0; channel < HAL_GetNumCTREPDPChannels(); ++channel) {
+    DoubleCallback callback;
+    auto cb =
+        sim.RegisterCurrentCallback(channel, callback.GetCallback(), false);
+
+    const double kTestCurrent = 35.04 + channel;
+    sim.SetCurrent(channel, kTestCurrent);
+    EXPECT_EQ(kTestCurrent, sim.GetCurrent(channel));
+    EXPECT_EQ(kTestCurrent, pdp.GetCurrent(channel));
+    EXPECT_TRUE(callback.WasTriggered());
+    EXPECT_TRUE(callback.GetLastValue());
+  }
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/PWMSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/PWMSimTest.cpp
new file mode 100644
index 0000000..0df3590
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/PWMSimTest.cpp
@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/PWMSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/PWM.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(PWMSimTest, Initialize) {
+  HAL_Initialize(500, 0);
+
+  PWMSim sim{0};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+  PWM pwm{0};
+  EXPECT_TRUE(sim.GetInitialized());
+}
+
+TEST(PWMSimTest, SetRawValue) {
+  HAL_Initialize(500, 0);
+
+  PWMSim sim{0};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  IntCallback callback;
+
+  auto cb = sim.RegisterRawValueCallback(callback.GetCallback(), false);
+  PWM pwm{0};
+  sim.SetRawValue(229);
+  EXPECT_EQ(229, sim.GetRawValue());
+  EXPECT_EQ(229, pwm.GetRaw());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(229, callback.GetLastValue());
+}
+
+TEST(PWMSimTest, SetSpeed) {
+  HAL_Initialize(500, 0);
+
+  PWMSim sim{0};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  DoubleCallback callback;
+
+  auto cb = sim.RegisterSpeedCallback(callback.GetCallback(), false);
+  PWM pwm{0};
+  constexpr double kTestValue = 0.3504;
+  pwm.SetSpeed(kTestValue);
+
+  EXPECT_EQ(kTestValue, sim.GetSpeed());
+  EXPECT_EQ(kTestValue, pwm.GetSpeed());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestValue, callback.GetLastValue());
+}
+
+TEST(PWMSimTest, SetPosition) {
+  HAL_Initialize(500, 0);
+
+  PWMSim sim{0};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  DoubleCallback callback;
+
+  auto cb = sim.RegisterPositionCallback(callback.GetCallback(), false);
+  PWM pwm{0};
+  constexpr double kTestValue = 0.3504;
+  pwm.SetPosition(kTestValue);
+
+  EXPECT_EQ(kTestValue, sim.GetPosition());
+  EXPECT_EQ(kTestValue, pwm.GetPosition());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(kTestValue, callback.GetLastValue());
+}
+
+TEST(PWMSimTest, SetPeriodScale) {
+  HAL_Initialize(500, 0);
+
+  PWMSim sim{0};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  IntCallback callback;
+
+  auto cb = sim.RegisterPeriodScaleCallback(callback.GetCallback(), false);
+  PWM pwm{0};
+  sim.SetPeriodScale(3504);
+  EXPECT_EQ(3504, sim.GetPeriodScale());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(3504, callback.GetLastValue());
+}
+
+TEST(PWMSimTest, SetZeroLatch) {
+  HAL_Initialize(500, 0);
+
+  PWMSim sim{0};
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+
+  auto cb = sim.RegisterZeroLatchCallback(callback.GetCallback(), false);
+  PWM pwm{0};
+  pwm.SetZeroLatch();
+
+  EXPECT_TRUE(callback.WasTriggered());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
new file mode 100644
index 0000000..7786b29
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/REVPHSimTest.cpp
@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/REVPHSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticHub.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(REVPHSimTest, InitializedCallback) {
+  REVPHSim sim;
+
+  sim.ResetData();
+  EXPECT_FALSE(sim.GetInitialized());
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterInitializedCallback(callback.GetCallback(), false);
+
+  PneumaticHub ph;
+  EXPECT_TRUE(sim.GetInitialized());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(REVPHSimTest, SolenoidOutput) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  DoubleSolenoid doubleSolenoid{1, frc::PneumaticsModuleType::REVPH, 3, 4};
+
+  BooleanCallback callback3;
+  BooleanCallback callback4;
+  auto cb3 =
+      sim.RegisterSolenoidOutputCallback(3, callback3.GetCallback(), false);
+  auto cb4 =
+      sim.RegisterSolenoidOutputCallback(4, callback4.GetCallback(), false);
+
+  callback3.Reset();
+  callback4.Reset();
+  doubleSolenoid.Set(DoubleSolenoid::kReverse);
+  EXPECT_FALSE(callback3.WasTriggered());
+  EXPECT_FALSE(callback3.GetLastValue());
+  EXPECT_TRUE(callback4.WasTriggered());
+  EXPECT_TRUE(callback4.GetLastValue());
+  EXPECT_FALSE(sim.GetSolenoidOutput(3));
+  EXPECT_TRUE(sim.GetSolenoidOutput(4));
+  EXPECT_EQ(0b00010000, ph.GetSolenoids());
+  EXPECT_EQ(0b00010000, sim.GetAllSolenoidOutputs());
+
+  callback3.Reset();
+  callback4.Reset();
+  doubleSolenoid.Set(DoubleSolenoid::kForward);
+  EXPECT_TRUE(callback3.WasTriggered());
+  EXPECT_TRUE(callback3.GetLastValue());
+  EXPECT_TRUE(callback4.WasTriggered());
+  EXPECT_FALSE(callback4.GetLastValue());
+  EXPECT_TRUE(sim.GetSolenoidOutput(3));
+  EXPECT_FALSE(sim.GetSolenoidOutput(4));
+  EXPECT_EQ(0b00001000, ph.GetSolenoids());
+  EXPECT_EQ(0b00001000, sim.GetAllSolenoidOutputs());
+
+  callback3.Reset();
+  callback4.Reset();
+  doubleSolenoid.Set(DoubleSolenoid::kOff);
+  EXPECT_TRUE(callback3.WasTriggered());
+  EXPECT_FALSE(callback3.GetLastValue());
+  EXPECT_FALSE(callback4.WasTriggered());
+  EXPECT_FALSE(callback4.GetLastValue());
+  EXPECT_FALSE(sim.GetSolenoidOutput(3));
+  EXPECT_FALSE(sim.GetSolenoidOutput(4));
+  EXPECT_EQ(0b00000000, ph.GetSolenoids());
+  EXPECT_EQ(0b00000000, sim.GetAllSolenoidOutputs());
+}
+
+TEST(REVPHSimTest, SetCompressorOn) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterCompressorOnCallback(callback.GetCallback(), false);
+
+  EXPECT_FALSE(ph.GetCompressor());
+  EXPECT_FALSE(ph.GetCompressor());
+  sim.SetCompressorOn(true);
+  EXPECT_TRUE(sim.GetCompressorOn());
+  EXPECT_TRUE(ph.GetCompressor());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(REVPHSimTest, SetClosedLoopEnabled) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb =
+      sim.RegisterClosedLoopEnabledCallback(callback.GetCallback(), false);
+
+  ph.SetClosedLoopControl(false);
+  EXPECT_FALSE(ph.GetClosedLoopControl());
+
+  ph.SetClosedLoopControl(true);
+  EXPECT_TRUE(sim.GetClosedLoopEnabled());
+  EXPECT_TRUE(ph.GetClosedLoopControl());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(REVPHSimTest, SetPressureSwitchEnabled) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  BooleanCallback callback;
+  auto cb = sim.RegisterPressureSwitchCallback(callback.GetCallback(), false);
+
+  EXPECT_FALSE(ph.GetPressureSwitch());
+
+  sim.SetPressureSwitch(true);
+  EXPECT_TRUE(sim.GetPressureSwitch());
+  EXPECT_TRUE(ph.GetPressureSwitch());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+}
+
+TEST(REVPHSimTest, SetCompressorCurrent) {
+  PneumaticHub ph;
+  REVPHSim sim(ph);
+  sim.ResetData();
+
+  DoubleCallback callback;
+  auto cb =
+      sim.RegisterCompressorCurrentCallback(callback.GetCallback(), false);
+
+  sim.SetCompressorCurrent(35.04);
+  EXPECT_EQ(35.04, sim.GetCompressorCurrent());
+  EXPECT_EQ(35.04, ph.GetCompressorCurrent());
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_EQ(35.04, callback.GetLastValue());
+}
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RelaySimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RelaySimTest.cpp
new file mode 100644
index 0000000..292d629
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RelaySimTest.cpp
@@ -0,0 +1,204 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/RelaySim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/Relay.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(RelaySimTest, InitializationBidrectional) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim(0);
+  sim.ResetData();
+
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  EXPECT_FALSE(sim.GetInitializedForward());
+  EXPECT_FALSE(sim.GetInitializedReverse());
+
+  auto fwdCb = sim.RegisterInitializedForwardCallback(
+      forwardCallback.GetCallback(), false);
+  auto revCb = sim.RegisterInitializedReverseCallback(
+      reverseCallback.GetCallback(), false);
+  Relay relay{0};
+
+  EXPECT_TRUE(sim.GetInitializedForward());
+  EXPECT_TRUE(sim.GetInitializedReverse());
+
+  EXPECT_TRUE(forwardCallback.WasTriggered());
+  EXPECT_TRUE(forwardCallback.GetLastValue());
+  EXPECT_TRUE(reverseCallback.WasTriggered());
+  EXPECT_TRUE(reverseCallback.GetLastValue());
+}
+
+TEST(RelaySimTest, InitializationForwardOnly) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim{0};
+  sim.ResetData();
+
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  EXPECT_FALSE(sim.GetInitializedForward());
+  EXPECT_FALSE(sim.GetInitializedReverse());
+
+  auto fwdCb = sim.RegisterInitializedForwardCallback(
+      forwardCallback.GetCallback(), false);
+  auto revCb = sim.RegisterInitializedReverseCallback(
+      reverseCallback.GetCallback(), false);
+  Relay relay(0, Relay::kForwardOnly);
+
+  EXPECT_TRUE(sim.GetInitializedForward());
+  EXPECT_FALSE(sim.GetInitializedReverse());
+
+  EXPECT_TRUE(forwardCallback.WasTriggered());
+  EXPECT_TRUE(forwardCallback.GetLastValue());
+  EXPECT_FALSE(reverseCallback.WasTriggered());
+}
+
+TEST(RelaySimTest, InitializationReverseOnly) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim{0};
+  sim.ResetData();
+
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  EXPECT_FALSE(sim.GetInitializedForward());
+  EXPECT_FALSE(sim.GetInitializedReverse());
+
+  auto fwdCb = sim.RegisterInitializedForwardCallback(
+      forwardCallback.GetCallback(), false);
+  auto revCb = sim.RegisterInitializedReverseCallback(
+      reverseCallback.GetCallback(), false);
+  Relay relay(0, Relay::kReverseOnly);
+
+  EXPECT_FALSE(sim.GetInitializedForward());
+  EXPECT_TRUE(sim.GetInitializedReverse());
+
+  EXPECT_FALSE(forwardCallback.WasTriggered());
+  EXPECT_TRUE(reverseCallback.WasTriggered());
+  EXPECT_TRUE(reverseCallback.GetLastValue());
+}
+
+TEST(RelaySimTest, BidirectionalSetForward) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim{0};
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  Relay relay{0};
+  auto fwdCb =
+      sim.RegisterForwardCallback(forwardCallback.GetCallback(), false);
+  auto revCb =
+      sim.RegisterReverseCallback(reverseCallback.GetCallback(), false);
+
+  relay.Set(Relay::kForward);
+  EXPECT_EQ(Relay::kForward, relay.Get());
+  EXPECT_TRUE(sim.GetForward());
+  EXPECT_FALSE(sim.GetReverse());
+
+  EXPECT_TRUE(forwardCallback.WasTriggered());
+  EXPECT_TRUE(forwardCallback.GetLastValue());
+  EXPECT_FALSE(reverseCallback.WasTriggered());
+}
+
+TEST(RelaySimTest, BidirectionalSetReverse) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim{0};
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  Relay relay{0};
+  auto fwdCb =
+      sim.RegisterForwardCallback(forwardCallback.GetCallback(), false);
+  auto revCb =
+      sim.RegisterReverseCallback(reverseCallback.GetCallback(), false);
+
+  relay.Set(Relay::kReverse);
+  EXPECT_EQ(Relay::kReverse, relay.Get());
+  EXPECT_FALSE(sim.GetForward());
+  EXPECT_TRUE(sim.GetReverse());
+
+  EXPECT_FALSE(forwardCallback.WasTriggered());
+  EXPECT_TRUE(reverseCallback.WasTriggered());
+  EXPECT_TRUE(reverseCallback.GetLastValue());
+}
+
+TEST(RelaySimTest, BidirectionalSetOn) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim{0};
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  Relay relay{0};
+  auto fwdCb =
+      sim.RegisterForwardCallback(forwardCallback.GetCallback(), false);
+  auto revCb =
+      sim.RegisterReverseCallback(reverseCallback.GetCallback(), false);
+
+  relay.Set(Relay::kOn);
+  EXPECT_EQ(Relay::kOn, relay.Get());
+  EXPECT_TRUE(sim.GetForward());
+  EXPECT_TRUE(sim.GetReverse());
+
+  EXPECT_TRUE(forwardCallback.WasTriggered());
+  EXPECT_TRUE(forwardCallback.GetLastValue());
+  EXPECT_TRUE(reverseCallback.WasTriggered());
+  EXPECT_TRUE(reverseCallback.GetLastValue());
+}
+
+TEST(RelaySimTest, BidirectionalSetOff) {
+  HAL_Initialize(500, 0);
+
+  RelaySim sim{0};
+  BooleanCallback forwardCallback;
+  BooleanCallback reverseCallback;
+
+  Relay relay{0};
+  auto fwdCb =
+      sim.RegisterForwardCallback(forwardCallback.GetCallback(), false);
+  auto revCb =
+      sim.RegisterReverseCallback(reverseCallback.GetCallback(), false);
+
+  // Bootstrap into a non-off state to verify the callbacks
+  relay.Set(Relay::kOn);
+  forwardCallback.Reset();
+  reverseCallback.Reset();
+
+  relay.Set(Relay::kOff);
+  EXPECT_EQ(Relay::kOff, relay.Get());
+  EXPECT_FALSE(sim.GetForward());
+  EXPECT_FALSE(sim.GetReverse());
+
+  EXPECT_TRUE(forwardCallback.WasTriggered());
+  EXPECT_FALSE(forwardCallback.GetLastValue());
+  EXPECT_TRUE(reverseCallback.WasTriggered());
+  EXPECT_FALSE(reverseCallback.GetLastValue());
+}
+
+TEST(RelaySimTest, StopMotor) {
+  Relay relay{0};
+  RelaySim sim(relay);
+
+  // Bootstrap into non-off state
+  relay.Set(Relay::kOn);
+
+  relay.StopMotor();
+  EXPECT_EQ(Relay::kOff, relay.Get());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp
new file mode 100644
index 0000000..c6e93d4
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/RoboRioSimTest.cpp
@@ -0,0 +1,210 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/simulation/RoboRioSim.h"  // NOLINT(build/include_order)
+
+#include <hal/HAL.h>
+#include <hal/HALBase.h>
+
+#include "callback_helpers/TestCallbackHelpers.h"
+#include "frc/RobotController.h"
+#include "gtest/gtest.h"
+
+namespace frc::sim {
+
+TEST(RoboRioSimTest, FPGAButton) {
+  RoboRioSim::ResetData();
+
+  int dummyStatus = 0;
+
+  BooleanCallback callback;
+  auto cb =
+      RoboRioSim::RegisterFPGAButtonCallback(callback.GetCallback(), false);
+  RoboRioSim::SetFPGAButton(true);
+  EXPECT_TRUE(RoboRioSim::GetFPGAButton());
+  EXPECT_TRUE(HAL_GetFPGAButton(&dummyStatus));
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_TRUE(callback.GetLastValue());
+
+  callback.Reset();
+  RoboRioSim::SetFPGAButton(false);
+  EXPECT_FALSE(RoboRioSim::GetFPGAButton());
+  EXPECT_FALSE(HAL_GetFPGAButton(&dummyStatus));
+  EXPECT_TRUE(callback.WasTriggered());
+  EXPECT_FALSE(callback.GetLastValue());
+}
+
+TEST(RoboRioSimTest, SetVin) {
+  RoboRioSim::ResetData();
+
+  DoubleCallback voltageCallback;
+  DoubleCallback currentCallback;
+  auto voltageCb = RoboRioSim::RegisterVInVoltageCallback(
+      voltageCallback.GetCallback(), false);
+  auto currentCb = RoboRioSim::RegisterVInCurrentCallback(
+      currentCallback.GetCallback(), false);
+  constexpr double kTestVoltage = 1.91;
+  constexpr double kTestCurrent = 35.04;
+
+  RoboRioSim::SetVInVoltage(units::volt_t{kTestVoltage});
+  EXPECT_TRUE(voltageCallback.WasTriggered());
+  EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue());
+  EXPECT_EQ(kTestVoltage, RoboRioSim::GetVInVoltage().value());
+  EXPECT_EQ(kTestVoltage, RobotController::GetInputVoltage());
+
+  RoboRioSim::SetVInCurrent(units::ampere_t{kTestCurrent});
+  EXPECT_TRUE(currentCallback.WasTriggered());
+  EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue());
+  EXPECT_EQ(kTestCurrent, RoboRioSim::GetVInCurrent().value());
+  EXPECT_EQ(kTestCurrent, RobotController::GetInputCurrent());
+}
+
+TEST(RoboRioSimTest, SetBrownout) {
+  RoboRioSim::ResetData();
+
+  DoubleCallback voltageCallback;
+  auto voltageCb = RoboRioSim::RegisterBrownoutVoltageCallback(
+      voltageCallback.GetCallback(), false);
+  constexpr double kTestVoltage = 1.91;
+
+  RoboRioSim::SetBrownoutVoltage(units::volt_t{kTestVoltage});
+  EXPECT_TRUE(voltageCallback.WasTriggered());
+  EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue());
+  EXPECT_EQ(kTestVoltage, RoboRioSim::GetBrownoutVoltage().value());
+  EXPECT_EQ(kTestVoltage, RobotController::GetBrownoutVoltage().value());
+}
+
+TEST(RoboRioSimTest, Set6V) {
+  RoboRioSim::ResetData();
+
+  DoubleCallback voltageCallback;
+  DoubleCallback currentCallback;
+  BooleanCallback activeCallback;
+  IntCallback faultCallback;
+  auto voltageCb = RoboRioSim::RegisterUserVoltage6VCallback(
+      voltageCallback.GetCallback(), false);
+  auto currentCb = RoboRioSim::RegisterUserCurrent6VCallback(
+      currentCallback.GetCallback(), false);
+  auto activeCb = RoboRioSim::RegisterUserActive6VCallback(
+      activeCallback.GetCallback(), false);
+  auto faultsCb = RoboRioSim::RegisterUserFaults6VCallback(
+      faultCallback.GetCallback(), false);
+  constexpr double kTestVoltage = 22.9;
+  constexpr double kTestCurrent = 174;
+  constexpr int kTestFaults = 229;
+
+  RoboRioSim::SetUserVoltage6V(units::volt_t{kTestVoltage});
+  EXPECT_TRUE(voltageCallback.WasTriggered());
+  EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue());
+  EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage6V().value());
+  EXPECT_EQ(kTestVoltage, RobotController::GetVoltage6V());
+
+  RoboRioSim::SetUserCurrent6V(units::ampere_t{kTestCurrent});
+  EXPECT_TRUE(currentCallback.WasTriggered());
+  EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue());
+  EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent6V().value());
+  EXPECT_EQ(kTestCurrent, RobotController::GetCurrent6V());
+
+  RoboRioSim::SetUserActive6V(false);
+  EXPECT_TRUE(activeCallback.WasTriggered());
+  EXPECT_FALSE(activeCallback.GetLastValue());
+  EXPECT_FALSE(RoboRioSim::GetUserActive6V());
+  EXPECT_FALSE(RobotController::GetEnabled6V());
+
+  RoboRioSim::SetUserFaults6V(kTestFaults);
+  EXPECT_TRUE(faultCallback.WasTriggered());
+  EXPECT_EQ(kTestFaults, faultCallback.GetLastValue());
+  EXPECT_EQ(kTestFaults, RoboRioSim::GetUserFaults6V());
+  EXPECT_EQ(kTestFaults, RobotController::GetFaultCount6V());
+}
+
+TEST(RoboRioSimTest, Set5V) {
+  RoboRioSim::ResetData();
+
+  DoubleCallback voltageCallback;
+  DoubleCallback currentCallback;
+  BooleanCallback activeCallback;
+  IntCallback faultCallback;
+  auto voltageCb = RoboRioSim::RegisterUserVoltage5VCallback(
+      voltageCallback.GetCallback(), false);
+  auto currentCb = RoboRioSim::RegisterUserCurrent5VCallback(
+      currentCallback.GetCallback(), false);
+  auto activeCb = RoboRioSim::RegisterUserActive5VCallback(
+      activeCallback.GetCallback(), false);
+  auto faultsCb = RoboRioSim::RegisterUserFaults5VCallback(
+      faultCallback.GetCallback(), false);
+  constexpr double kTestVoltage = 22.9;
+  constexpr double kTestCurrent = 174;
+  constexpr int kTestFaults = 229;
+
+  RoboRioSim::SetUserVoltage5V(units::volt_t{kTestVoltage});
+  EXPECT_TRUE(voltageCallback.WasTriggered());
+  EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue());
+  EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage5V().value());
+  EXPECT_EQ(kTestVoltage, RobotController::GetVoltage5V());
+
+  RoboRioSim::SetUserCurrent5V(units::ampere_t{kTestCurrent});
+  EXPECT_TRUE(currentCallback.WasTriggered());
+  EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue());
+  EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent5V().value());
+  EXPECT_EQ(kTestCurrent, RobotController::GetCurrent5V());
+
+  RoboRioSim::SetUserActive5V(false);
+  EXPECT_TRUE(activeCallback.WasTriggered());
+  EXPECT_FALSE(activeCallback.GetLastValue());
+  EXPECT_FALSE(RoboRioSim::GetUserActive5V());
+  EXPECT_FALSE(RobotController::GetEnabled5V());
+
+  RoboRioSim::SetUserFaults5V(kTestFaults);
+  EXPECT_TRUE(faultCallback.WasTriggered());
+  EXPECT_EQ(kTestFaults, faultCallback.GetLastValue());
+  EXPECT_EQ(kTestFaults, RoboRioSim::GetUserFaults5V());
+  EXPECT_EQ(kTestFaults, RobotController::GetFaultCount5V());
+}
+
+TEST(RoboRioSimTest, Set3V3) {
+  RoboRioSim::ResetData();
+
+  DoubleCallback voltageCallback;
+  DoubleCallback currentCallback;
+  BooleanCallback activeCallback;
+  IntCallback faultCallback;
+  auto voltageCb = RoboRioSim::RegisterUserVoltage3V3Callback(
+      voltageCallback.GetCallback(), false);
+  auto currentCb = RoboRioSim::RegisterUserCurrent3V3Callback(
+      currentCallback.GetCallback(), false);
+  auto activeCb = RoboRioSim::RegisterUserActive3V3Callback(
+      activeCallback.GetCallback(), false);
+  auto faultsCb = RoboRioSim::RegisterUserFaults3V3Callback(
+      faultCallback.GetCallback(), false);
+  constexpr double kTestVoltage = 22.9;
+  constexpr double kTestCurrent = 174;
+  constexpr int kTestFaults = 229;
+
+  RoboRioSim::SetUserVoltage3V3(units::volt_t{kTestVoltage});
+  EXPECT_TRUE(voltageCallback.WasTriggered());
+  EXPECT_EQ(kTestVoltage, voltageCallback.GetLastValue());
+  EXPECT_EQ(kTestVoltage, RoboRioSim::GetUserVoltage3V3().value());
+  EXPECT_EQ(kTestVoltage, RobotController::GetVoltage3V3());
+
+  RoboRioSim::SetUserCurrent3V3(units::ampere_t{kTestCurrent});
+  EXPECT_TRUE(currentCallback.WasTriggered());
+  EXPECT_EQ(kTestCurrent, currentCallback.GetLastValue());
+  EXPECT_EQ(kTestCurrent, RoboRioSim::GetUserCurrent3V3().value());
+  EXPECT_EQ(kTestCurrent, RobotController::GetCurrent3V3());
+
+  RoboRioSim::SetUserActive3V3(false);
+  EXPECT_TRUE(activeCallback.WasTriggered());
+  EXPECT_FALSE(activeCallback.GetLastValue());
+  EXPECT_FALSE(RoboRioSim::GetUserActive3V3());
+  EXPECT_FALSE(RobotController::GetEnabled3V3());
+
+  RoboRioSim::SetUserFaults3V3(kTestFaults);
+  EXPECT_TRUE(faultCallback.WasTriggered());
+  EXPECT_EQ(kTestFaults, faultCallback.GetLastValue());
+  EXPECT_EQ(kTestFaults, RoboRioSim::GetUserFaults3V3());
+  EXPECT_EQ(kTestFaults, RobotController::GetFaultCount3V3());
+}
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
index ea7b809..5ef2675 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimDeviceSimTest.cpp
@@ -1,19 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <string_view>
 
 #include <hal/SimDevice.h>
-#include <wpi/StringRef.h>
 
 #include "frc/simulation/SimDeviceSim.h"
 #include "gtest/gtest.h"
 
 using namespace frc::sim;
 
-TEST(SimDeviceSimTests, TestBasic) {
+TEST(SimDeviceSimTest, Basic) {
   hal::SimDevice dev{"test"};
   hal::SimBoolean devBool = dev.CreateBoolean("bool", false, false);
 
@@ -24,13 +22,15 @@
   EXPECT_TRUE(devBool.Get());
 }
 
-TEST(SimDeviceSimTests, TestEnumerateDevices) {
+TEST(SimDeviceSimTest, EnumerateDevices) {
   hal::SimDevice dev{"test"};
 
   bool foundit = false;
   SimDeviceSim::EnumerateDevices(
       "te", [&](const char* name, HAL_SimDeviceHandle handle) {
-        if (wpi::StringRef(name) == "test") foundit = true;
+        if (std::string_view(name) == "test") {
+          foundit = true;
+        }
       });
   EXPECT_TRUE(foundit);
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
index 1b86e05..e703912 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <exception>
 
@@ -15,14 +12,14 @@
 #include "frc/simulation/AnalogOutputSim.h"
 #include "frc/simulation/AnalogTriggerSim.h"
 #include "frc/simulation/BuiltInAccelerometerSim.h"
+#include "frc/simulation/CTREPCMSim.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/PowerDistributionSim.h"
 #include "frc/simulation/RelaySim.h"
 #include "frc/simulation/RoboRioSim.h"
 #include "frc/simulation/SPIAccelerometerSim.h"
@@ -30,7 +27,7 @@
 
 using namespace frc::sim;
 
-TEST(SimInitializationTests, TestAllInitialize) {
+TEST(SimInitializationTest, AllInitialize) {
   HAL_Initialize(500, 0);
   BuiltInAccelerometerSim biacsim;
   AnalogGyroSim agsim{0};
@@ -43,8 +40,8 @@
   (void)dssim;
   EncoderSim esim = EncoderSim::CreateForIndex(0);
   (void)esim;
-  PCMSim pcmsim{0};
-  PDPSim pdpsim{0};
+  CTREPCMSim pcmsim{0};
+  PowerDistributionSim pdpsim{0};
   PWMSim pwmsim{0};
   RelaySim rsim{0};
   RoboRioSim rrsim;
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
index a553985..03df12b 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/SingleJointedArmSimTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/simulation/SingleJointedArmSim.h"
 #include "gtest/gtest.h"
@@ -13,13 +10,13 @@
 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));
+  sim.SetState(Eigen::Vector<double, 2>{0.0, 0.0});
 
   for (size_t i = 0; i < 12 / 0.02; ++i) {
-    sim.SetInput(frc::MakeMatrix<1, 1>(0.0));
+    sim.SetInput(Eigen::Vector<double, 1>{0.0});
     sim.Update(20_ms);
   }
 
   // The arm should swing down.
-  EXPECT_NEAR(sim.GetAngle().to<double>(), -wpi::math::pi / 2, 0.01);
+  EXPECT_NEAR(sim.GetAngle().value(), -wpi::numbers::pi / 2, 0.01);
 }
diff --git a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
index 5766751..e7e0405 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
+++ b/third_party/allwpilib/wpilibc/src/test/native/cpp/simulation/StateSpaceSimTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <iostream>
 
@@ -11,10 +8,10 @@
 #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/motorcontrol/PWMVictorSPX.h"
 #include "frc/simulation/BatterySim.h"
 #include "frc/simulation/DifferentialDrivetrainSim.h"
 #include "frc/simulation/ElevatorSim.h"
@@ -27,7 +24,7 @@
 #include "frc/system/plant/LinearSystemId.h"
 #include "gtest/gtest.h"
 
-TEST(StateSpaceSimTest, TestFlywheelSim) {
+TEST(StateSpaceSimTest, FlywheelSim) {
   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);
@@ -39,6 +36,9 @@
   frc::sim::EncoderSim encoderSim{encoder};
   frc::PWMVictorSPX motor{0};
 
+  frc::sim::RoboRioSim::ResetData();
+  encoderSim.ResetData();
+
   for (int i = 0; i < 100; i++) {
     // RobotPeriodic runs first
     auto voltageOut = controller.Calculate(encoder.GetRate(), 200.0);
@@ -48,10 +48,10 @@
     // 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.SetInput(Eigen::Vector<double, 1>{
+        motor.Get() * frc::RobotController::GetInputVoltage()});
     sim.Update(20_ms);
-    encoderSim.SetRate(sim.GetAngularVelocity().to<double>());
+    encoderSim.SetRate(sim.GetAngularVelocity().value());
   }
 
   ASSERT_TRUE(std::abs(200 - encoder.GetRate()) < 0.1);
diff --git a/third_party/allwpilib/wpilibc/src/test/native/include/MockMotorController.h b/third_party/allwpilib/wpilibc/src/test/native/include/MockMotorController.h
new file mode 100644
index 0000000..e17931f
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/include/MockMotorController.h
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "frc/motorcontrol/MotorController.h"
+
+namespace frc {
+
+class MockMotorController : public MotorController {
+ public:
+  void Set(double speed) override;
+  double Get() const override;
+  void SetInverted(bool isInverted) override;
+  bool GetInverted() const override;
+  void Disable() override;
+  void StopMotor() override;
+
+ private:
+  double m_speed = 0.0;
+  bool m_isInverted = false;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/include/MockSpeedController.h b/third_party/allwpilib/wpilibc/src/test/native/include/MockSpeedController.h
deleted file mode 100644
index e0c788f..0000000
--- a/third_party/allwpilib/wpilibc/src/test/native/include/MockSpeedController.h
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/SpeedController.h"
-
-namespace frc {
-
-class MockSpeedController : public SpeedController {
- public:
-  void Set(double speed) override;
-  double Get() const override;
-  void SetInverted(bool isInverted) override;
-  bool GetInverted() const override;
-  void Disable() override;
-  void StopMotor() override;
-
-  void PIDWrite(double output) override;
-
- private:
-  double m_speed = 0.0;
-  bool m_isInverted = false;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/src/test/native/include/callback_helpers/TestCallbackHelpers.h b/third_party/allwpilib/wpilibc/src/test/native/include/callback_helpers/TestCallbackHelpers.h
new file mode 100644
index 0000000..646bcdd
--- /dev/null
+++ b/third_party/allwpilib/wpilibc/src/test/native/include/callback_helpers/TestCallbackHelpers.h
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <string_view>
+
+#include <hal/Value.h>
+
+namespace frc::sim {
+
+template <typename ValueType>
+class BaseCallbackHelper {
+ public:
+  bool WasTriggered() const { return m_wasTriggered; }
+  ValueType GetLastValue() const { return m_lastValue; }
+
+  void Reset() {
+    m_wasTriggered = false;
+    m_lastValue = 0;
+  }
+
+  virtual void HandleCallback(std::string_view name,
+                              const HAL_Value* value) = 0;
+
+  std::function<void(std::string_view, const HAL_Value*)> GetCallback() {
+    return [&](std::string_view name, const HAL_Value* value) {
+      HandleCallback(name, value);
+    };
+  }
+
+ protected:
+  bool m_wasTriggered{false};
+  ValueType m_lastValue{0};
+};
+
+class BooleanCallback : public BaseCallbackHelper<bool> {
+ public:
+  void HandleCallback(std::string_view name, const HAL_Value* value) override;
+};
+
+class EnumCallback : public BaseCallbackHelper<int32_t> {
+ public:
+  void HandleCallback(std::string_view name, const HAL_Value* value) override;
+};
+
+class IntCallback : public BaseCallbackHelper<int32_t> {
+ public:
+  void HandleCallback(std::string_view name, const HAL_Value* value) override;
+};
+
+class LongCallback : public BaseCallbackHelper<int64_t> {
+ public:
+  void HandleCallback(std::string_view name, const HAL_Value* value) override;
+};
+
+class DoubleCallback : public BaseCallbackHelper<double> {
+ public:
+  void HandleCallback(std::string_view name, const HAL_Value* value) override;
+};
+
+}  // namespace frc::sim
diff --git a/third_party/allwpilib/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h b/third_party/allwpilib/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
index 1cba8e3..8baf2a2 100644
--- a/third_party/allwpilib/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
+++ b/third_party/allwpilib/wpilibc/src/test/native/include/shuffleboard/MockActuatorSendable.h
@@ -1,29 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/StringRef.h>
+#include <string_view>
 
-#include "frc/smartdashboard/Sendable.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-#include "frc/smartdashboard/SendableHelper.h"
-
-namespace frc {
+#include <wpi/sendable/Sendable.h>
 
 /**
  * A mock sendable that marks itself as an actuator.
  */
-class MockActuatorSendable : public Sendable,
-                             public SendableHelper<MockActuatorSendable> {
+class MockActuatorSendable : public wpi::Sendable {
  public:
-  explicit MockActuatorSendable(wpi::StringRef name);
+  explicit MockActuatorSendable(std::string_view name);
 
-  void InitSendable(SendableBuilder& builder) override;
+  void InitSendable(wpi::SendableBuilder& builder) override;
 };
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibc/wpilibc-config.cmake.in b/third_party/allwpilib/wpilibc/wpilibc-config.cmake.in
index 4332c55..3518a7f 100644
--- a/third_party/allwpilib/wpilibc/wpilibc-config.cmake.in
+++ b/third_party/allwpilib/wpilibc/wpilibc-config.cmake.in
@@ -1,9 +1,11 @@
 include(CMakeFindDependencyMacro)
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
+@WPIMATH_DEP_REPLACE@
 @NTCORE_DEP_REPLACE@
 @CSCORE_DEP_REPLACE@
 @CAMERASERVER_DEP_REPLACE@
 @HAL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpilibc.cmake)
diff --git a/third_party/allwpilib/wpilibcExamples/CMakeLists.txt b/third_party/allwpilib/wpilibcExamples/CMakeLists.txt
new file mode 100644
index 0000000..27cff6b
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/CMakeLists.txt
@@ -0,0 +1,31 @@
+project(wpilibcExamples)
+
+include(AddTest)
+include(SubDirList)
+
+SUBDIR_LIST(TEMPLATES ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/templates)
+SUBDIR_LIST(EXAMPLES  ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/main/cpp/examples)
+
+foreach(example ${EXAMPLES})
+  file(GLOB_RECURSE sources src/main/cpp/examples/${example}/cpp/*.cpp
+                            src/main/cpp/examples/${example}/c/*.c)
+  add_executable(${example} ${sources})
+  target_include_directories(${example} PUBLIC src/main/cpp/examples/${example}/include)
+  target_link_libraries(${example} wpilibc wpilibNewCommands wpilibOldCommands)
+
+  if (WITH_TESTS AND EXISTS ${CMAKE_SOURCE_DIR}/wpilibcExamples/src/test/cpp/examples/${example})
+    wpilib_add_test(${example} src/test/cpp/examples/${example}/cpp)
+    target_sources(${example}_test PRIVATE ${sources})
+    target_include_directories(${example}_test PRIVATE src/test/cpp/examples/${example}/include)
+    target_compile_definitions(${example}_test PUBLIC RUNNING_FRC_TESTS)
+    target_link_libraries(${example}_test wpilibc wpilibNewCommands gmock_main)
+  endif()
+endforeach()
+
+foreach(template ${TEMPLATES})
+  file(GLOB_RECURSE sources src/main/cpp/templates/${template}/cpp/*.cpp
+                            src/main/cpp/templates/${template}/c/*.c)
+  add_executable(${template} ${sources})
+  target_include_directories(${template} PUBLIC src/main/cpp/templates/${template}/include)
+  target_link_libraries(${template} wpilibc wpilibNewCommands)
+endforeach()
diff --git a/third_party/allwpilib/wpilibcExamples/build.gradle b/third_party/allwpilib/wpilibcExamples/build.gradle
index 35f20d4..975e90a 100644
--- a/third_party/allwpilib/wpilibcExamples/build.gradle
+++ b/third_party/allwpilib/wpilibcExamples/build.gradle
@@ -2,6 +2,7 @@
 
 apply plugin: 'cpp'
 apply plugin: 'c'
+apply plugin: 'google-test-test-suite'
 apply plugin: 'visual-studio'
 apply plugin: 'edu.wpi.first.NativeUtils'
 apply plugin: ExtraTasks
@@ -9,28 +10,29 @@
 evaluationDependsOn(':hal')
 
 apply from: '../shared/config.gradle'
+apply from: "${rootDir}/shared/googletest.gradle"
 
 ext.examplesMap = [:]
 ext.templatesMap = [:]
 
 File examplesTree = file("$projectDir/src/main/cpp/examples")
 examplesTree.list(new FilenameFilter() {
-    @Override
-    public boolean accept(File current, String name) {
-        return new File(current, name).isDirectory();
-    }
-}).each {
-    examplesMap.put(it, [])
-}
+            @Override
+            public boolean accept(File current, String name) {
+                return new File(current, name).isDirectory();
+            }
+        }).each {
+            examplesMap.put(it, [])
+        }
 File templatesTree = file("$projectDir/src/main/cpp/templates")
 templatesTree.list(new FilenameFilter() {
-    @Override
-    public boolean accept(File current, String name) {
-        return new File(current, name).isDirectory();
-    }
-}).each {
-    templatesMap.put(it, [])
-}
+            @Override
+            public boolean accept(File current, String name) {
+                return new File(current, name).isDirectory();
+            }
+        }).each {
+            templatesMap.put(it, [])
+        }
 
 nativeUtils.platformConfigs.named(nativeUtils.wpi.platforms.roborio).configure {
     cppCompiler.args.remove('-Wno-error=deprecated-declarations')
@@ -44,6 +46,8 @@
     useCpp = true
 }
 
+def simModules = ["halsim_gui"]
+
 apply from: "${rootDir}/shared/opencv.gradle"
 
 model {
@@ -82,7 +86,6 @@
             "${key}"(NativeExecutableSpec) {
                 targetBuildTypes 'debug'
                 binaries.all { binary ->
-                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                     lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                     lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
                     lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
@@ -92,7 +95,12 @@
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                        nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
+                    }
+                    if (binary.targetPlatform.name == getCurrentArch()) {
+                        simModules.each {
+                            lib project: ":simulation:$it", library: it, linkage: 'shared'
+                        }
                     }
                 }
                 sources {
@@ -125,7 +133,6 @@
             "${key}"(NativeExecutableSpec) {
                 targetBuildTypes 'debug'
                 binaries.all { binary ->
-                    lib project: ':wpilibOldCommands', library: 'wpilibOldCommands', linkage: 'shared'
                     lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
                     lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
                     lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
@@ -142,7 +149,7 @@
                         }
                     }
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                        nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                     }
                 }
                 sources {
@@ -160,39 +167,62 @@
             }
         }
     }
-    tasks {
-        def b = $.binaries
-        b.each { binary->
-            if (binary in NativeExecutableBinarySpec) {
-                def installDir = binary.tasks.install.installDirectory.get().toString() + File.separatorChar
-                def runFile = binary.tasks.install.runScriptFile.get().asFile.toString()
-
-                binary.tasks.install.doLast {
-                    if (binary.targetPlatform.operatingSystem.isWindows()) {
-                        // Windows batch scripts
-                        fileName = binary.component.name + 'RealDS.bat'
-                        file = new File(installDir + fileName)
-                        file.withWriter { out ->
-                            out.println '@ECHO OFF'
-                            out.print 'SET HALSIM_EXTENSIONS='
-                            out.println '"' + new File(installDir + 'lib\\halsim_ds_socket.dll').toString() + '"'
-                            out.println runFile + ' %*'
+    testSuites {
+        examplesMap.each { key, value ->
+            def testFolder = new File("${rootDir}/wpilibcExamples/src/test/cpp/examples/${key}")
+            if (testFolder.exists()) {
+                "${key}Test"(GoogleTestTestSuiteSpec) {
+                    for (NativeComponentSpec c : $.components) {
+                        if (c.name == key) {
+                            testing c
+                            break
                         }
-                    } else {
-                        fileName = binary.component.name + 'RealDS.sh'
-                        file = new File(installDir + fileName)
-                        file.withWriter { out ->
-                            out.print 'export HALSIM_EXTENSIONS='
-                            out.println '"' + new File(installDir + '/lib/libhalsim_ds_socket.so').toString() + '"'
-                            out.println runFile + ' "$@"'
+                    }
+                    sources {
+                        cpp {
+                            source {
+                                srcDirs "src/test/cpp/examples/${key}/cpp"
+                                include '**/*.cpp'
+                            }
+                            exportedHeaders {
+                                srcDirs "src/test/cpp/examples/${key}/include"
+                            }
+                        }
+                        c {
+                            source {
+                                srcDirs "src/test/cpp/examples/${key}/c"
+                                include '**/*.c'
+                            }
+                            exportedHeaders {
+                                srcDirs "src/test/cpp/examples/${key}/include"
+                            }
                         }
                     }
                 }
-
             }
         }
     }
+    binaries {
+        withType(GoogleTestTestSuiteBinarySpec) {
+            lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
+            lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+            lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+            lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+            lib project: ':cscore', library: 'cscore', linkage: 'shared'
+            project(':hal').addHalDependency(it, 'shared')
+            lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+            lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+            if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
+            }
+            nativeUtils.useRequiredLibrary(it, 'opencv_shared')
+
+            it.cppCompiler.define('RUNNING_FRC_TESTS')
+            it.cCompiler.define('RUNNING_FRC_TESTS')
+        }
+    }
 }
+
 apply from: 'publish.gradle'
 
 model {
@@ -223,6 +253,54 @@
     commandFile = new File("$projectDir/src/main/cpp/commands/commands.json")
 }
 
+model {
+    // Create run tasks for all examples.
+    tasks {
+        // Iterate through the components and check if it is an example.
+        $.components.each { component ->
+            if (examplesMap.containsKey(component.name)) {
+                // Get the appropriate binary and create the run task.
+                component.binaries.each { binary ->
+                    if (binary.targetPlatform.name == getCurrentArch() && binary.buildType.name == "debug") {
+                        project.tasks.create("run${component.name}", Exec) {
+                            // Add simulation modules to HALSIM_EXTENSIONS environment variable.
+                            def setupEnv = {
+                                String extensions = ""
+                                binary.tasks.install.installDirectory.get().getAsFile().eachFileRecurse {
+                                    def name = it.name
+
+                                    // If we don't have a shared library, skip.
+                                    if (!(name.endsWith('.dll') || name.endsWith('.so') || name.endsWith('.dylib')))
+                                        return
+
+                                    def file = it
+                                    simModules.each {
+                                        if (name.startsWith(it) || name.startsWith("lib$it".toString())) {
+                                            extensions += file.absolutePath + File.pathSeparator
+                                        }
+                                    }
+                                }
+
+                                if (extensions != '') {
+                                    environment 'HALSIM_EXTENSIONS', extensions
+                                }
+                            }
+
+                            // Create the task dependency and run the executable.
+                            doFirst { setupEnv() }
+                            dependsOn binary.tasks.install
+                            commandLine binary.tasks.install.runScriptFile.get().asFile.toString()
+
+                            group = "application"
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
+
 ext {
     isCppCommands = true
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp
index 07c233e..24726b0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeCommand.h"
 
@@ -19,7 +16,9 @@
 void ReplaceMeCommand::Execute() {}
 
 // Make this return true when this Command no longer needs to run execute()
-bool ReplaceMeCommand::IsFinished() { return false; }
+bool ReplaceMeCommand::IsFinished() {
+  return false;
+}
 
 // Called once after isFinished returns true
 void ReplaceMeCommand::End() {}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h
index 49a3139..64fcd26 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command/ReplaceMeCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp
index e8085a1..c17d267 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeCommand2.h"
 
@@ -21,4 +18,6 @@
 void ReplaceMeCommand2::End(bool interrupted) {}
 
 // Returns true when the command should end.
-bool ReplaceMeCommand2::IsFinished() { return false; }
+bool ReplaceMeCommand2::IsFinished() {
+  return false;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h
index fd041fa..8a1e8e0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/command2/ReplaceMeCommand2.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp
index 1a431f4..ced938d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeCommandGroup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h
index a58f586..c51a964 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commandgroup/ReplaceMeCommandGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commands.json b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commands.json
index d039a73..6b5bf1e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commands.json
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/commands.json
@@ -322,7 +322,7 @@
   },
   {
     "name": "TrapezoidProfileSubsystem (New)",
-    "description": "A command that executes a trapezoidal motion profile.",
+    "description": "A subsystem that executes a trapezoidal motion profile.",
     "tags": [
       "trapezoidprofilesubsystem"
     ],
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp
index 4ff8ad8..19de34f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.cpp
@@ -1,10 +1,7 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeEmptyClass.h"
 
-ReplaceMeEmptyClass::ReplaceMeEmptyClass() {}
+ReplaceMeEmptyClass::ReplaceMeEmptyClass() = default;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h
index 131edab..e6a733c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/emptyclass/ReplaceMeEmptyClass.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp
index c130e09..1a80288 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeInstantCommand.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h
index b50b617..c73cbcf 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instant/ReplaceMeInstantCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
index 5e21e56..4de2be6 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeInstantCommand2.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.h
index dec11c2..cd8de5f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/instantcommand/ReplaceMeInstantCommand2.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
index 3361a2c..e814a09 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeParallelCommandGroup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h
index a4f9519..1cbabdd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
index 5c3466c..67c12a3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeParallelDeadlineGroup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h
index 7ecebb4..91be75f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
index 6d977f9..9cc3784 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeParallelRaceGroup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h
index 0b13b18..a3f4227 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/parallelracegroup/ReplaceMeParallelRaceGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
index b39be7a..1bcc830 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMePIDCommand.h"
 
@@ -23,4 +20,6 @@
           }) {}
 
 // Returns true when the command should end.
-bool ReplaceMePIDCommand::IsFinished() { return false; }
+bool ReplaceMePIDCommand::IsFinished() {
+  return false;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h
index 03c874c..a78d216 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidcommand/ReplaceMePIDCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp
index f446f8e..7b44893 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMePIDSubsystem.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h
index 2ab3d92..f9aabc9 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem/ReplaceMePIDSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp
index 0fa81a2..2cdce1b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMePIDSubsystem2.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.h
index 6c8e344..0b450f4 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/pidsubsystem2/ReplaceMePIDSubsystem2.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
index eada0ac..880a222 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeProfiledPIDCommand.h"
 
@@ -34,4 +31,6 @@
           }) {}
 
 // Returns true when the command should end.
-bool ReplaceMeProfiledPIDCommand::IsFinished() { return false; }
+bool ReplaceMeProfiledPIDCommand::IsFinished() {
+  return false;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
index dc2c50e..0bd45ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
index 4ba8baf..d65a867 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeProfiledPIDSubsystem.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
index 6969c29..8e86f3f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
index c7d0702..c83a99d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeSequentialCommandGroup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h
index 5e629b8..f9fe09e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp
index 8ca89d3..ca589f1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeSubsystem.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h
index de84a48..3de6031 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem/ReplaceMeSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp
index 5d6805a..8d0ede0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeSubsystem2.h"
 
-ReplaceMeSubsystem2::ReplaceMeSubsystem2() {}
+ReplaceMeSubsystem2::ReplaceMeSubsystem2() = default;
 
 // This method will be called once per scheduler run
 void ReplaceMeSubsystem2::Periodic() {}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.h
index 6f15f46..ca11456 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/subsystem2/ReplaceMeSubsystem2.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -16,7 +13,7 @@
   /**
    * Will be called periodically whenever the CommandScheduler runs.
    */
-  void Periodic();
+  void Periodic() override;
 
  private:
   // Components (e.g. motor controllers and sensors) should generally be
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp
index 2076e5b..c6793e0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeTimedCommand.h"
 
-ReplaceMeTimedCommand::ReplaceMeTimedCommand(double timeout)
+ReplaceMeTimedCommand::ReplaceMeTimedCommand(units::second_t timeout)
     : TimedCommand(timeout) {
   // Use Requires() here to declare subsystem dependencies
   // eg. Requires(Robot::chassis.get());
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h
index bf55e45..48cfa2d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/timed/ReplaceMeTimedCommand.h
@@ -1,17 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/commands/TimedCommand.h>
+#include <units/time.h>
 
 class ReplaceMeTimedCommand : public frc::TimedCommand {
  public:
-  explicit ReplaceMeTimedCommand(double timeout);
+  explicit ReplaceMeTimedCommand(units::second_t timeout);
   void Initialize() override;
   void Execute() override;
   void End() override;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
index a337013..3928d3b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeTrapezoidProfileCommand.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
index 5cf7a20..a3cd8c8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
index 8bc1bee..a3f458c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeTrapezoidProfileSubsystem.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
index 42a8cb5..99b9410 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp
index 4759e0e..3c563ff 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "ReplaceMeTrigger.h"
 
-ReplaceMeTrigger::ReplaceMeTrigger() {}
+ReplaceMeTrigger::ReplaceMeTrigger() = default;
 
-bool ReplaceMeTrigger::Get() { return false; }
+bool ReplaceMeTrigger::Get() {
+  return false;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h
index 9da7ef8..a76dcd9 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/commands/trigger/ReplaceMeTrigger.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
index 3658824..d59b370 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AddressableLED/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <array>
 
@@ -55,5 +52,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
index 3361754..abe79f4 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDrive/cpp/Robot.cpp
@@ -1,32 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a demo program showing the use of the DifferentialDrive class.
  * Runs the motors with arcade steering.
  */
 class Robot : public frc::TimedRobot {
-  frc::PWMVictorSPX m_leftMotor{0};
-  frc::PWMVictorSPX m_rightMotor{1};
+  frc::PWMSparkMax m_leftMotor{0};
+  frc::PWMSparkMax m_rightMotor{1};
   frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
   frc::Joystick m_stick{0};
 
  public:
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     // Drive with arcade style
     m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
index 67f1e12..644e35f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArcadeDriveXboxController/cpp/Robot.cpp
@@ -1,37 +1,34 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a demo program showing the use of the DifferentialDrive class.
  * Runs the motors with split arcade steering and an Xbox controller.
  */
 class Robot : public frc::TimedRobot {
-  frc::PWMVictorSPX m_leftMotor{0};
-  frc::PWMVictorSPX m_rightMotor{1};
+  frc::PWMSparkMax m_leftMotor{0};
+  frc::PWMSparkMax m_rightMotor{1};
   frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
   frc::XboxController m_driverController{0};
 
  public:
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     // Drive with split arcade style
     // That means that the Y axis of the left stick moves forward
     // and backward, and the X of the right stick turns left and right.
-    m_robotDrive.ArcadeDrive(
-        m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
-        m_driverController.GetX(frc::GenericHID::JoystickHand::kRightHand));
+    m_robotDrive.ArcadeDrive(m_driverController.GetLeftY(),
+                             m_driverController.GetRightX());
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
index be32fbb..2803c5c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,14 +17,18 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
  * can use it to reset any subsystem information you want to clear when the
  * robot is disabled.
  */
-void Robot::DisabledInit() { m_container.DisablePIDSubsystems(); }
+void Robot::DisabledInit() {
+  m_container.DisablePIDSubsystems();
+}
 
 void Robot::DisabledPeriodic() {}
 
@@ -67,5 +68,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
index d3a0133..48310da 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -20,9 +17,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
@@ -31,7 +27,7 @@
   // Configure your button bindings here
 
   // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
-  frc2::JoystickButton(&m_driverController, 1)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
       .WhenPressed(
           [this] {
             m_arm.SetGoal(2_rad);
@@ -40,7 +36,7 @@
           {&m_arm});
 
   // Move the arm to neutral position when the 'B' button is pressed.
-  frc2::JoystickButton(&m_driverController, 2)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
       .WhenPressed(
           [this] {
             m_arm.SetGoal(ArmConstants::kArmOffset);
@@ -49,16 +45,19 @@
           {&m_arm});
 
   // Disable the arm controller when Y is pressed.
-  frc2::JoystickButton(&m_driverController, 4)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kY)
       .WhenPressed([this] { m_arm.Disable(); }, {&m_arm});
 
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
-      .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
+      .WhenPressed([this] { m_drive.SetMaxOutput(0.5); })
       .WhenReleased([this] { m_drive.SetMaxOutput(1); });
 }
 
-void RobotContainer::DisablePIDSubsystems() { m_arm.Disable(); }
+void RobotContainer::DisablePIDSubsystems() {
+  m_arm.Disable();
+}
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
   // Runs the chosen command in autonomous
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
index 5531c5b..8618aaa 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/ArmSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/ArmSubsystem.h"
 
@@ -19,7 +16,7 @@
       m_motor(kMotorPort),
       m_encoder(kEncoderPorts[0], kEncoderPorts[1]),
       m_feedforward(kS, kCos, kV, kA) {
-  m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.to<double>());
+  m_encoder.SetDistancePerPulse(kEncoderDistancePerPulse.value());
   // Start arm in neutral position
   SetGoal(State{kArmOffset, 0_rad_per_s});
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
index 64be1b8..5ba38b8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -38,9 +35,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
index ac5ffe0..16a4e00 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <units/angular_velocity.h>
 #include <units/time.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -37,7 +34,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ArmConstants {
@@ -57,7 +55,8 @@
 
 constexpr int kEncoderPorts[]{4, 5};
 constexpr int kEncoderPPR = 256;
-constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+constexpr auto kEncoderDistancePerPulse =
+    2.0_rad * wpi::numbers::pi / kEncoderPPR;
 
 // The offset of the arm from the horizontal in its neutral position,
 // measured from the horizontal
@@ -70,5 +69,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
index c238cb6..782207b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
index 1733efe..45cd582 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/ArmSubsystem.h
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/controller/ArmFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/ProfiledPIDSubsystem.h>
 #include <units/angle.h>
 
@@ -27,7 +24,7 @@
   units::radian_t GetMeasurement() override;
 
  private:
-  frc::PWMVictorSPX m_motor;
+  frc::PWMSparkMax m_motor;
   frc::Encoder m_encoder;
   frc::ArmFeedforward m_feedforward;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
index 3ed1357..47bf28e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBot/include/subsystems/DriveSubsystem.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -73,16 +70,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
index ba7bbf5..b5a4420 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -20,9 +17,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
@@ -31,17 +27,18 @@
   // Configure your button bindings here
 
   // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
-  frc2::JoystickButton(&m_driverController, 1)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
       .WhenPressed([this] { m_arm.SetGoal(2_rad); }, {&m_arm});
 
   // Move the arm to neutral position when the 'B' button is pressed.
-  frc2::JoystickButton(&m_driverController, 1)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
       .WhenPressed([this] { m_arm.SetGoal(ArmConstants::kArmOffset); },
                    {&m_arm});
 
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
-      .WhenPressed([this] { m_drive.SetMaxOutput(.5); })
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
+      .WhenPressed([this] { m_drive.SetMaxOutput(0.5); })
       .WhenReleased([this] { m_drive.SetMaxOutput(1); });
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
index 54a59ed..f80970a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/ArmSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/ArmSubsystem.h"
 
@@ -26,5 +23,5 @@
       m_feedforward.Calculate(setpoint.position, setpoint.velocity);
   // Add the feedforward to the PID output to get the motor output
   m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
-                      setpoint.position.to<double>(), feedforward / 12_V);
+                      setpoint.position.value(), feedforward / 12_V);
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
index 47c898e..03d3415 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -38,9 +35,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
index ac5ffe0..16a4e00 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <units/angular_velocity.h>
 #include <units/time.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -37,7 +34,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ArmConstants {
@@ -57,7 +55,8 @@
 
 constexpr int kEncoderPorts[]{4, 5};
 constexpr int kEncoderPPR = 256;
-constexpr auto kEncoderDistancePerPulse = 2.0_rad * wpi::math::pi / kEncoderPPR;
+constexpr auto kEncoderDistancePerPulse =
+    2.0_rad * wpi::numbers::pi / kEncoderPPR;
 
 // The offset of the arm from the horizontal in its neutral position,
 // measured from the horizontal
@@ -70,5 +69,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
index f0e888d..5d55839 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/ExampleSmartMotorController.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/SpeedController.h>
+#include <frc/motorcontrol/MotorController.h>
 
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
@@ -15,7 +12,7 @@
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::SpeedController {
+class ExampleSmartMotorController : public frc::MotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -82,6 +79,4 @@
   void Disable() override {}
 
   void StopMotor() override {}
-
-  void PIDWrite(double output) override {}
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
index fa07359..5aba470 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
index 8030cb4..1fb5b87 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/ArmSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
index 3ed1357..47bf28e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmBotOffboard/include/subsystems/DriveSubsystem.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -73,16 +70,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
index 18df257..b8b8368 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ArmSimulation/cpp/Robot.cpp
@@ -1,26 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
-#include <frc/GenericHID.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/RobotController.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/simulation/BatterySim.h>
 #include <frc/simulation/EncoderSim.h>
 #include <frc/simulation/RoboRioSim.h>
 #include <frc/simulation/SingleJointedArmSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/system/plant/LinearSystemId.h>
+#include <frc/util/Color.h>
+#include <frc/util/Color8Bit.h>
 #include <units/angle.h>
 #include <units/moment_of_inertia.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This is a sample program to demonstrate how to use a state-space controller
@@ -33,63 +34,81 @@
   static constexpr int kJoystickPort = 0;
 
   // The P gain for the PID controller that drives this arm.
-  static constexpr double kArmKp = 5.0;
+  static constexpr double kArmKp = 50.0;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
   static constexpr double kArmEncoderDistPerPulse =
-      2.0 * wpi::math::pi / 4096.0;
+      2.0 * wpi::numbers::pi / 4096.0;
 
-  // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
   frc::DCMotor m_armGearbox = frc::DCMotor::Vex775Pro(2);
 
   // Standard classes for controlling our arm
   frc2::PIDController m_controller{kArmKp, 0, 0};
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
-  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::PWMSparkMax m_motor{kMotorPort};
   frc::Joystick m_joystick{kJoystickPort};
 
   // Simulation classes help us simulate what's going on, including gravity.
-  // This sim represents an arm with 2 775s, a 100:1 reduction, a mass of 5kg,
-  // 30in overall arm length, range of motion nin [-180, 0] degrees, and noise
-  // with a standard deviation of 0.5 degrees.
+  // This sim represents an arm with 2 775s, a 600:1 reduction, a mass of 5kg,
+  // 30in overall arm length, range of motion in [-75, 255] degrees, and noise
+  // with a standard deviation of 1 encoder tick.
   frc::sim::SingleJointedArmSim m_armSim{
       m_armGearbox,
-      100.0,
+      600.0,
       frc::sim::SingleJointedArmSim::EstimateMOI(30_in, 5_kg),
       30_in,
-      -180_deg,
-      0_deg,
+      -75_deg,
+      255_deg,
       5_kg,
       true,
-      {(0.5_deg).to<double>()}};
+      {kArmEncoderDistPerPulse}};
   frc::sim::EncoderSim m_encoderSim{m_encoder};
 
- public:
-  void RobotInit() { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); }
+  // Create a Mechanism2d display of an Arm
+  frc::Mechanism2d m_mech2d{60, 60};
+  frc::MechanismRoot2d* m_armBase = m_mech2d.GetRoot("ArmBase", 30, 30);
+  frc::MechanismLigament2d* m_armTower =
+      m_armBase->Append<frc::MechanismLigament2d>(
+          "Arm Tower", 30, -90_deg, 6, frc::Color8Bit{frc::Color::kBlue});
+  frc::MechanismLigament2d* m_arm = m_armBase->Append<frc::MechanismLigament2d>(
+      "Arm", 30, m_armSim.GetAngle(), 6, frc::Color8Bit{frc::Color::kYellow});
 
-  void SimulationPeriodic() {
+ public:
+  void RobotInit() override {
+    m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    frc::SmartDashboard::PutData("Arm Sim", &m_mech2d);
+  }
+
+  void SimulationPeriodic() override {
     // In this method, we update our simulation of what our arm is doing
     // First, we set our "inputs" (voltages)
-    m_armSim.SetInput(frc::MakeMatrix<1, 1>(
-        m_motor.Get() * frc::RobotController::GetInputVoltage()));
+    m_armSim.SetInput(Eigen::Vector<double, 1>{
+        m_motor.Get() * frc::RobotController::GetInputVoltage()});
 
     // Next, we update it. The standard loop time is 20ms.
     m_armSim.Update(20_ms);
 
     // Finally, we set our simulated encoder's readings and simulated battery
     // voltage
-    m_encoderSim.SetDistance(m_armSim.GetAngle().to<double>());
+    m_encoderSim.SetDistance(m_armSim.GetAngle().value());
     // SimBattery estimates loaded battery voltages
     frc::sim::RoboRioSim::SetVInVoltage(
         frc::sim::BatterySim::Calculate({m_armSim.GetCurrentDraw()}));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm->SetAngle(m_armSim.GetAngle());
   }
 
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     if (m_joystick.GetTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 30in.
-      double pidOutput =
-          m_controller.Calculate(m_encoder.GetDistance(), (30_in).to<double>());
+      // Here, we run PID control like normal, with a constant setpoint of 75
+      // degrees.
+      double pidOutput = m_controller.Calculate(
+          m_encoder.GetDistance(), (units::radian_t(75_deg)).value());
       m_motor.SetVoltage(units::volt_t(pidOutput));
     } else {
       // Otherwise, we disable the motor.
@@ -97,12 +116,14 @@
     }
   }
 
-  void DisabledInit() {
+  void DisabledInit() override {
     // This just makes sure that our simulation code knows that the motor's off.
     m_motor.Set(0.0);
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp
index 2811cf1..7f4f761 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/AxisCameraSample/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cameraserver/CameraServer.h>
 #include <frc/TimedRobot.h>
@@ -22,15 +19,15 @@
   static void VisionThread() {
     // Get the Axis camera from CameraServer
     cs::AxisCamera camera =
-        frc::CameraServer::GetInstance()->AddAxisCamera("axis-camera.local");
+        frc::CameraServer::AddAxisCamera("axis-camera.local");
     // Set the resolution
     camera.SetResolution(640, 480);
 
     // Get a CvSink. This will capture Mats from the Camera
-    cs::CvSink cvSink = frc::CameraServer::GetInstance()->GetVideo();
+    cs::CvSink cvSink = frc::CameraServer::GetVideo();
     // Setup a CvSource. This will send images back to the Dashboard
     cs::CvSource outputStream =
-        frc::CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
+        frc::CameraServer::PutVideo("Rectangle", 640, 480);
 
     // Mats are very memory expensive. Lets reuse this Mat.
     cv::Mat mat;
@@ -61,5 +58,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp
index 76493b5..aed898f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/CANPDP/cpp/Robot.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/PowerDistributionPanel.h>
+#include <frc/PowerDistribution.h>
 #include <frc/TimedRobot.h>
 #include <frc/smartdashboard/SmartDashboard.h>
 
@@ -34,9 +31,11 @@
 
  private:
   // Object for dealing with the Power Distribution Panel (PDP).
-  frc::PowerDistributionPanel m_pdp;
+  frc::PowerDistribution m_pdp;
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
index fb3999b..9097d94 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DMA/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/AnalogInput.h>
 #include <frc/DMA.h>
@@ -42,7 +39,7 @@
 
     // Start DMA. No triggers or inputs can be added after this call
     // unless DMA is stopped.
-    m_dma.StartDMA(1024);
+    m_dma.Start(1024);
   }
 
   void RobotPeriodic() override {
@@ -56,10 +53,13 @@
     // Update our sample. remaining is the number of samples remaining in the
     // buffer status is more specific error messages if readStatus is not OK.
     // Wait 1ms if buffer is empty
-    HAL_DMAReadStatus readStatus =
+    frc::DMASample::DMAReadStatus readStatus =
         sample.Update(&m_dma, 1_ms, &remaining, &status);
 
-    if (readStatus == HAL_DMA_OK) {
+    // Unset trigger
+    m_dmaTrigger.Set(true);
+
+    if (readStatus == frc::DMASample::DMAReadStatus::kOk) {
       // Status value in all these reads should be checked, a non 0 value means
       // value could not be read
 
@@ -78,5 +78,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
index c639539..da638d5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Drivetrain.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Drivetrain.h"
 
@@ -11,9 +8,9 @@
   const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
   const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
   const double leftOutput = m_leftPIDController.Calculate(
-      m_leftEncoder.GetRate(), speeds.left.to<double>());
+      m_leftEncoder.GetRate(), speeds.left.value());
   const double rightOutput = m_rightPIDController.Calculate(
-      m_rightEncoder.GetRate(), speeds.right.to<double>());
+      m_rightEncoder.GetRate(), speeds.right.value());
 
   m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
   m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
index 22d1cf6..e9119c8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/cpp/Robot.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
 
 #include "Drivetrain.h"
 
@@ -21,16 +18,14 @@
   void TeleopPeriodic() override {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed = -m_speedLimiter.Calculate(
-                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+    const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
                         Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_rotLimiter.Calculate(
-                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+    const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_drive.Drive(xSpeed, rot);
@@ -48,5 +43,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
index d8f3521..81d0a71 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDriveBot/include/Drivetrain.h
@@ -1,25 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/AnalogGyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <units/angle.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
 #include <units/velocity.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * Represents a differential drive style drivetrain.
@@ -27,13 +24,18 @@
 class Drivetrain {
  public:
   Drivetrain() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
     m_gyro.Reset();
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
     // resolution.
-    m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+    m_leftEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
                                       kEncoderResolution);
-    m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+    m_rightEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
                                        kEncoderResolution);
 
     m_leftEncoder.Reset();
@@ -43,7 +45,7 @@
   static constexpr units::meters_per_second_t kMaxSpeed =
       3.0_mps;  // 3 meters per second
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
-      wpi::math::pi};  // 1/2 rotation per second
+      wpi::numbers::pi};  // 1/2 rotation per second
 
   void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
   void Drive(units::meters_per_second_t xSpeed,
@@ -55,13 +57,13 @@
   static constexpr double kWheelRadius = 0.0508;  // meters
   static constexpr int kEncoderResolution = 4096;
 
-  frc::PWMVictorSPX m_leftLeader{1};
-  frc::PWMVictorSPX m_leftFollower{2};
-  frc::PWMVictorSPX m_rightLeader{3};
-  frc::PWMVictorSPX m_rightFollower{4};
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
 
-  frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
 
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..1ed0e46
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Drivetrain.h"
+
+#include <frc/Timer.h>
+
+#include "ExampleGlobalMeasurementSensor.h"
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  const double leftOutput = m_leftPIDController.Calculate(
+      m_leftEncoder.GetRate(), speeds.left.value());
+  const double rightOutput = m_rightPIDController.Calculate(
+      m_rightEncoder.GetRate(), speeds.right.value());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(),
+                         {units::meters_per_second_t(m_leftEncoder.GetRate()),
+                          units::meters_per_second_t(m_rightEncoder.GetRate())},
+                         units::meter_t(m_leftEncoder.GetDistance()),
+                         units::meter_t(m_rightEncoder.GetDistance()));
+
+  // Also apply vision measurements. We use 0.3 seconds in the past as an
+  // example -- on a real robot, this must be calculated based either on latency
+  // or timestamps.
+  m_poseEstimator.AddVisionMeasurement(
+      ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
+          m_poseEstimator.GetEstimatedPosition()),
+      frc::Timer::GetFPGATimestamp() - 0.3_s);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp
new file mode 100644
index 0000000..e9119c8
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/cpp/Robot.cpp
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    TeleopPeriodic();
+    m_drive.UpdateOdometry();
+  }
+
+  void TeleopPeriodic() override {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_drive.Drive(xSpeed, rot);
+  }
+
+ private:
+  frc::XboxController m_controller{0};
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  Drivetrain m_drive;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
new file mode 100644
index 0000000..dfb57a6
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/Drivetrain.h
@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/DifferentialDrivePoseEstimator.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <wpi/numbers>
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
+    m_gyro.Reset();
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(
+        2 * wpi::numbers::pi * kWheelRadius.value() / kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(
+        2 * wpi::numbers::pi * kWheelRadius.value() / kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::numbers::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr units::meter_t kWheelRadius = 0.0508_m;
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
+
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{1.0, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::DifferentialDrivePoseEstimator m_poseEstimator{
+      frc::Rotation2d(),
+      frc::Pose2d(),
+      {0.01, 0.01, 0.01, 0.01, 0.01},
+      {0.1, 0.1, 0.1},
+      {0.1, 0.1, 0.1}};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h
new file mode 100644
index 0000000..a4caff4
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DifferentialDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/geometry/Pose2d.h>
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer
+ * vision solution.
+ */
+class ExampleGlobalMeasurementSensor {
+ public:
+  static frc::Pose2d GetEstimatedGlobalPose(
+      const frc::Pose2d& estimatedRobotPose) {
+    auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)),
+                       estimatedRobotPose.Y() + units::meter_t(randVec(1)),
+                       estimatedRobotPose.Rotation() +
+                           frc::Rotation2d(units::radian_t(randVec(3))));
+  }
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
index 78bb48a..fb879fe 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -21,9 +18,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
@@ -32,18 +28,19 @@
   // Configure your button bindings here
 
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
       .WhenPressed(&m_driveHalfSpeed)
       .WhenReleased(&m_driveFullSpeed);
 
   // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of
   // 10 seconds
-  frc2::JoystickButton(&m_driverController, 1)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
       .WhenPressed(DriveDistanceProfiled(3_m, &m_drive).WithTimeout(10_s));
 
   // Do the same thing as above when the 'B' button is pressed, but defined
   // inline
-  frc2::JoystickButton(&m_driverController, 2)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
       .WhenPressed(
           frc2::TrapezoidProfileCommand<units::meters>(
               frc::TrapezoidProfile<units::meters>(
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
index 7de607c..cecb9d6 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/commands/DriveDistanceProfiled.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/DriveDistanceProfiled.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
index d0bdef6..270124b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -15,6 +12,16 @@
       m_rightLeader{kRightMotor1Port},
       m_rightFollower{kRightMotor2Port},
       m_feedforward{ks, kv, ka} {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightLeader.SetInverted(true);
+
+  // You might need to not do this depending on the specific motor controller
+  // that you are using -- contact the respective vendor's documentation for
+  // more details.
+  m_rightFollower.SetInverted(true);
+
   m_leftFollower.Follow(m_leftLeader);
   m_rightFollower.Follow(m_rightLeader);
 
@@ -30,10 +37,10 @@
     frc::TrapezoidProfile<units::meters>::State left,
     frc::TrapezoidProfile<units::meters>::State right) {
   m_leftLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
-                           left.position.to<double>(),
+                           left.position.value(),
                            m_feedforward.Calculate(left.velocity) / 12_V);
   m_rightLeader.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
-                            right.position.to<double>(),
+                            right.position.value(),
                             m_feedforward.Calculate(right.velocity) / 12_V);
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
index 8ff887e..92f82ba 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,7 +9,7 @@
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -46,5 +43,5 @@
 }  // namespace DriveConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
index f0e888d..71dc4d4 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/ExampleSmartMotorController.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/SpeedController.h>
+#include <frc/motorcontrol/MotorController.h>
 
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
@@ -15,7 +12,7 @@
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::SpeedController {
+class ExampleSmartMotorController : public frc::MotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -71,9 +68,9 @@
    */
   void ResetEncoder() {}
 
-  void Set(double speed) override {}
+  void Set(double speed) override { m_value = speed; }
 
-  double Get() const override { return 0; }
+  double Get() const override { return m_value; }
 
   void SetInverted(bool isInverted) override {}
 
@@ -83,5 +80,6 @@
 
   void StopMotor() override {}
 
-  void PIDWrite(double output) override {}
+ private:
+  double m_value = 0.0;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
index af35be4..aced9e0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
index 2199590..34cf8f3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/commands/DriveDistanceProfiled.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
index 45fa982..9086353 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DriveDistanceOffboard/include/subsystems/DriveSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
index b75d99c..a9343d8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleEncoder/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/DutyCycleEncoder.h>
 #include <frc/TimedRobot.h>
@@ -34,11 +31,13 @@
 
     frc::SmartDashboard::PutBoolean("Connected", connected);
     frc::SmartDashboard::PutNumber("Frequency", frequency);
-    frc::SmartDashboard::PutNumber("Output", output.to<double>());
+    frc::SmartDashboard::PutNumber("Output", output.value());
     frc::SmartDashboard::PutNumber("Distance", distance);
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
index 1a113ae..69d41bc 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/DutyCycleInput/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/DigitalInput.h>
 #include <frc/DutyCycle.h>
@@ -31,5 +28,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
index b1bb1ec..ebafa75 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorProfiledPID/cpp/Robot.cpp
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/ProfiledPIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/trajectory/TrapezoidProfile.h>
 #include <units/acceleration.h>
 #include <units/length.h>
 #include <units/time.h>
 #include <units/velocity.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 class Robot : public frc::TimedRobot {
  public:
   static constexpr units::second_t kDt = 20_ms;
 
   Robot() {
-    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
+    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::numbers::pi * 1.5);
   }
 
   void TeleopPeriodic() override {
@@ -40,7 +37,7 @@
  private:
   frc::Joystick m_joystick{1};
   frc::Encoder m_encoder{1, 2};
-  frc::PWMVictorSPX m_motor{1};
+  frc::PWMSparkMax m_motor{1};
 
   // Create a PID controller whose setpoint's change is subject to maximum
   // velocity and acceleration constraints.
@@ -51,5 +48,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
index 1f67449..4b2891d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorSimulation/cpp/Robot.cpp
@@ -1,26 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
-#include <frc/GenericHID.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/RobotController.h>
 #include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/simulation/BatterySim.h>
 #include <frc/simulation/ElevatorSim.h>
 #include <frc/simulation/EncoderSim.h>
 #include <frc/simulation/RoboRioSim.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/MechanismRoot2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/system/plant/LinearSystemId.h>
+#include <frc/util/Color.h>
+#include <frc/util/Color8Bit.h>
 #include <units/angle.h>
+#include <units/length.h>
 #include <units/moment_of_inertia.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This is a sample program to demonstrate how to use a state-space controller
@@ -37,13 +40,13 @@
   static constexpr units::meter_t kElevatorDrumRadius = 2_in;
   static constexpr units::kilogram_t kCarriageMass = 4.0_kg;
 
-  static constexpr units::meter_t kMinElevatorHeight = 0_in;
+  static constexpr units::meter_t kMinElevatorHeight = 2_in;
   static constexpr units::meter_t kMaxElevatorHeight = 50_in;
 
   // distance per pulse = (distance per revolution) / (pulses per revolution)
   //  = (Pi * D) / ppr
   static constexpr double kArmEncoderDistPerPulse =
-      2.0 * wpi::math::pi * kElevatorDrumRadius.to<double>() / 4096.0;
+      2.0 * wpi::numbers::pi * kElevatorDrumRadius.value() / 4096.0;
 
   // This gearbox represents a gearbox containing 4 Vex 775pro motors.
   frc::DCMotor m_elevatorGearbox = frc::DCMotor::Vex775Pro(4);
@@ -51,7 +54,7 @@
   // Standard classes for controlling our elevator
   frc2::PIDController m_controller{kElevatorKp, 0, 0};
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
-  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::PWMSparkMax m_motor{kMotorPort};
   frc::Joystick m_joystick{kJoystickPort};
 
   // Simulation classes help us simulate what's going on, including gravity.
@@ -64,31 +67,49 @@
                                       {0.01}};
   frc::sim::EncoderSim m_encoderSim{m_encoder};
 
- public:
-  void RobotInit() { m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse); }
+  // Create a Mechanism2d display of an elevator
+  frc::Mechanism2d m_mech2d{20, 50};
+  frc::MechanismRoot2d* m_elevatorRoot =
+      m_mech2d.GetRoot("Elevator Root", 10, 0);
+  frc::MechanismLigament2d* m_elevatorMech2d =
+      m_elevatorRoot->Append<frc::MechanismLigament2d>(
+          "Elevator", units::inch_t(m_elevatorSim.GetPosition()).value(),
+          90_deg);
 
-  void SimulationPeriodic() {
+ public:
+  void RobotInit() override {
+    m_encoder.SetDistancePerPulse(kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    frc::SmartDashboard::PutData("Elevator Sim", &m_mech2d);
+  }
+
+  void SimulationPeriodic() override {
     // In this method, we update our simulation of what our elevator is doing
     // First, we set our "inputs" (voltages)
-    m_elevatorSim.SetInput(frc::MakeMatrix<1, 1>(
-        m_motor.Get() * frc::RobotController::GetInputVoltage()));
+    m_elevatorSim.SetInput(Eigen::Vector<double, 1>{
+        m_motor.Get() * frc::RobotController::GetInputVoltage()});
 
     // Next, we update it. The standard loop time is 20ms.
     m_elevatorSim.Update(20_ms);
 
     // Finally, we set our simulated encoder's readings and simulated battery
     // voltage
-    m_encoderSim.SetDistance(m_elevatorSim.GetPosition().to<double>());
+    m_encoderSim.SetDistance(m_elevatorSim.GetPosition().value());
     // SimBattery estimates loaded battery voltages
     frc::sim::RoboRioSim::SetVInVoltage(
         frc::sim::BatterySim::Calculate({m_elevatorSim.GetCurrentDraw()}));
+
+    // Update the Elevator length based on the simulated elevator height
+    m_elevatorMech2d->SetLength(
+        units::inch_t(m_elevatorSim.GetPosition()).value());
   }
 
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     if (m_joystick.GetTrigger()) {
       // Here, we run PID control like normal, with a constant setpoint of 30in.
-      double pidOutput =
-          m_controller.Calculate(m_encoder.GetDistance(), (30_in).to<double>());
+      double pidOutput = m_controller.Calculate(m_encoder.GetDistance(),
+                                                units::meter_t(30_in).value());
       m_motor.SetVoltage(units::volt_t(pidOutput));
     } else {
       // Otherwise, we disable the motor.
@@ -96,12 +117,14 @@
     }
   }
 
-  void DisabledInit() {
+  void DisabledInit() override {
     // This just makes sure that our simulation code knows that the motor's off.
     m_motor.Set(0.0);
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
index 5addac4..d629185 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
 #include <frc/TimedRobot.h>
@@ -14,7 +11,7 @@
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "ExampleSmartMotorController.h"
 
@@ -46,7 +43,7 @@
 
     // Send setpoint to offboard controller PID
     m_motor.SetSetpoint(ExampleSmartMotorController::PIDMode::kPosition,
-                        m_setpoint.position.to<double>(),
+                        m_setpoint.position.value(),
                         m_feedforward.Calculate(m_setpoint.velocity) / 12_V);
   }
 
@@ -64,5 +61,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
index f0e888d..5d55839 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ElevatorTrapezoidProfile/include/ExampleSmartMotorController.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/SpeedController.h>
+#include <frc/motorcontrol/MotorController.h>
 
 /**
  * A simplified stub class that simulates the API of a common "smart" motor
@@ -15,7 +12,7 @@
  *
  * <p>Has no actual functionality.
  */
-class ExampleSmartMotorController : public frc::SpeedController {
+class ExampleSmartMotorController : public frc::MotorController {
  public:
   enum PIDMode { kPosition, kVelocity, kMovementWitchcraft };
 
@@ -82,6 +79,4 @@
   void Disable() override {}
 
   void StopMotor() override {}
-
-  void PIDWrite(double output) override {}
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
index 3cfffd7..7db2c83 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Encoder/cpp/Robot.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
 #include <frc/TimedRobot.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * Sample program displaying the value of a quadrature encoder on the
@@ -43,7 +40,7 @@
      * inch diameter (1.5inch radius) wheel, and that we want to measure
      * distance in inches.
      */
-    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::math::pi * 1.5);
+    m_encoder.SetDistancePerPulse(1.0 / 360.0 * 2.0 * wpi::numbers::pi * 1.5);
 
     /* Defines the lowest rate at which the encoder will not be considered
      * stopped, for the purposes of the GetStopped() method. Units are in
@@ -82,5 +79,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp
index 729a6c6..b781422 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -19,9 +16,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
@@ -30,18 +26,21 @@
   // Configure your button bindings here
 
   // Spin up the shooter when the 'A' button is pressed
-  frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_spinUpShooter);
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
+      .WhenPressed(&m_spinUpShooter);
 
   // Turn off the shooter when the 'B' button is pressed
-  frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_stopShooter);
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
+      .WhenPressed(&m_stopShooter);
 
   // Shoot when the 'X' button is held
-  frc2::JoystickButton(&m_driverController, 3)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kX)
       .WhenPressed(&m_shoot)
       .WhenReleased(&m_stopFeeder);
 
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
       .WhenPressed(&m_driveHalfSpeed)
       .WhenReleased(&m_driveFullSpeed);
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
index 47c898e..f40a649 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -16,6 +13,11 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_leftMotors.SetInverted(true);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -38,9 +40,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
index 85be752..c386ee7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/cpp/subsystems/ShooterSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/ShooterSubsystem.h"
 
@@ -19,9 +16,9 @@
       m_feederMotor(kFeederMotorPort),
       m_shooterEncoder(kEncoderPorts[0], kEncoderPorts[1]),
       m_shooterFeedforward(kS, kV) {
-  m_controller.SetTolerance(kShooterToleranceRPS.to<double>());
+  m_controller.SetTolerance(kShooterToleranceRPS.value());
   m_shooterEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
-  SetSetpoint(kShooterTargetRPS.to<double>());
+  SetSetpoint(kShooterTargetRPS.value());
 }
 
 void ShooterSubsystem::UseOutput(double output, double setpoint) {
@@ -29,10 +26,18 @@
                             m_shooterFeedforward.Calculate(kShooterTargetRPS));
 }
 
-bool ShooterSubsystem::AtSetpoint() { return m_controller.AtSetpoint(); }
+bool ShooterSubsystem::AtSetpoint() {
+  return m_controller.AtSetpoint();
+}
 
-double ShooterSubsystem::GetMeasurement() { return m_shooterEncoder.GetRate(); }
+double ShooterSubsystem::GetMeasurement() {
+  return m_shooterEncoder.GetRate();
+}
 
-void ShooterSubsystem::RunFeeder() { m_feederMotor.Set(kFeederSpeed); }
+void ShooterSubsystem::RunFeeder() {
+  m_feederMotor.Set(kFeederSpeed);
+}
 
-void ShooterSubsystem::StopFeeder() { m_feederMotor.Set(0); }
+void ShooterSubsystem::StopFeeder() {
+  m_feederMotor.Set(0);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
index 0e2d0e7..8ac7b0e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Constants.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <units/angle.h>
 #include <units/time.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -36,7 +33,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace ShooterConstants {
@@ -74,5 +72,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
index 76dff49..26714fe 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -59,8 +56,7 @@
           // Shoot for the specified time
           frc2::WaitCommand(ac::kAutoShootTimeSeconds)}
           // Add a timeout (will end the command if, for instance, the shooter
-          // never gets up to
-          // speed)
+          // never gets up to speed)
           .WithTimeout(ac::kAutoTimeoutSeconds)
           // When the command ends, turn off the shooter and the feeder
           .AndThen([this] {
@@ -76,15 +72,14 @@
   frc2::InstantCommand m_stopShooter{[this] { m_shooter.Disable(); },
                                      {&m_shooter}};
 
-  // Shoots if the shooter wheen has reached the target speed
+  // Shoots if the shooter wheel has reached the target speed
   frc2::ConditionalCommand m_shoot{
       // Run the feeder
       frc2::InstantCommand{[this] { m_shooter.RunFeeder(); }, {&m_shooter}},
       // Do nothing
       frc2::InstantCommand(),
       // Determine which of the above to do based on whether the shooter has
-      // reached the
-      // desired speed
+      // reached the desired speed
       [this] { return m_shooter.AtSetpoint(); }};
 
   frc2::InstantCommand m_stopFeeder{[this] { m_shooter.StopFeeder(); },
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
index 3ed1357..47bf28e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/DriveSubsystem.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -73,16 +70,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
index 7553544..e53b32d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Frisbeebot/include/subsystems/ShooterSubsystem.h
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/PIDSubsystem.h>
 #include <units/angle.h>
 
@@ -28,8 +25,8 @@
   void StopFeeder();
 
  private:
-  frc::PWMVictorSPX m_shooterMotor;
-  frc::PWMVictorSPX m_feederMotor;
+  frc::PWMSparkMax m_shooterMotor;
+  frc::PWMSparkMax m_feederMotor;
   frc::Encoder m_shooterEncoder;
   frc::SimpleMotorFeedforward<units::turns> m_shooterFeedforward;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp
index 11a1d75..3ac0f75 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -25,10 +22,9 @@
   frc::SmartDashboard::PutData(&m_wrist);
   frc::SmartDashboard::PutData(&m_claw);
 
-  m_drivetrain.SetDefaultCommand(TankDrive(
-      [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kLeftHand); },
-      [this] { return m_joy.GetY(frc::GenericHID::JoystickHand::kRightHand); },
-      &m_drivetrain));
+  m_drivetrain.SetDefaultCommand(TankDrive([this] { return m_joy.GetLeftY(); },
+                                           [this] { return m_joy.GetRightY(); },
+                                           &m_drivetrain));
 
   // Configure the button bindings
   ConfigureButtonBindings();
@@ -36,24 +32,20 @@
 
 void RobotContainer::ConfigureButtonBindings() {
   // Configure your button bindings here
-  frc2::JoystickButton m_dUp{&m_joy, 5};
-  frc2::JoystickButton m_dRight{&m_joy, 6};
-  frc2::JoystickButton m_dDown{&m_joy, 7};
-  frc2::JoystickButton m_dLeft{&m_joy, 8};
-  frc2::JoystickButton m_l2{&m_joy, 9};
-  frc2::JoystickButton m_r2{&m_joy, 10};
-  frc2::JoystickButton m_l1{&m_joy, 11};
-  frc2::JoystickButton m_r1{&m_joy, 12};
-
-  m_dUp.WhenPressed(SetElevatorSetpoint(0.2, &m_elevator));
-  m_dDown.WhenPressed(SetElevatorSetpoint(-0.2, &m_elevator));
-  m_dRight.WhenPressed(CloseClaw(&m_claw));
-  m_dLeft.WhenPressed(OpenClaw(&m_claw));
-
-  m_r1.WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator));
-  m_r2.WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator));
-  m_l1.WhenPressed(Place(&m_claw, &m_wrist, &m_elevator));
-  m_l2.WhenPressed(Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain));
+  frc2::JoystickButton(&m_joy, 5).WhenPressed(
+      SetElevatorSetpoint(0.25, &m_elevator));
+  frc2::JoystickButton(&m_joy, 6).WhenPressed(CloseClaw(&m_claw));
+  frc2::JoystickButton(&m_joy, 7).WhenPressed(
+      SetElevatorSetpoint(0.0, &m_elevator));
+  frc2::JoystickButton(&m_joy, 8).WhenPressed(OpenClaw(&m_claw));
+  frc2::JoystickButton(&m_joy, 9).WhenPressed(
+      Autonomous(&m_claw, &m_wrist, &m_elevator, &m_drivetrain));
+  frc2::JoystickButton(&m_joy, 10)
+      .WhenPressed(Pickup(&m_claw, &m_wrist, &m_elevator));
+  frc2::JoystickButton(&m_joy, 11)
+      .WhenPressed(Place(&m_claw, &m_wrist, &m_elevator));
+  frc2::JoystickButton(&m_joy, 12)
+      .WhenPressed(PrepareToPickup(&m_claw, &m_wrist, &m_elevator));
 }
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
index 5db0cdb..23ccfff 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Autonomous.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/Autonomous.h"
 
@@ -18,7 +15,7 @@
 #include "commands/SetWristSetpoint.h"
 
 Autonomous::Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
-                       DriveTrain* drivetrain) {
+                       Drivetrain* drivetrain) {
   SetName("Autonomous");
   AddCommands(
       // clang-format off
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
index 97a9ccf..d10c310 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/CloseClaw.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/CloseClaw.h"
 
@@ -15,10 +12,14 @@
 }
 
 // Called just before this Command runs the first time
-void CloseClaw::Initialize() { m_claw->Close(); }
+void CloseClaw::Initialize() {
+  m_claw->Close();
+}
 
 // Make this return true when this Command no longer needs to run execute()
-bool CloseClaw::IsFinished() { return m_claw->IsGripping(); }
+bool CloseClaw::IsFinished() {
+  return m_claw->IsGripping();
+}
 
 // Called once after isFinished returns true
 void CloseClaw::End(bool) {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
index d84951c..60408db 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/DriveStraight.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/DriveStraight.h"
 
@@ -11,11 +8,11 @@
 
 #include "Robot.h"
 
-DriveStraight::DriveStraight(double distance, DriveTrain* drivetrain)
+DriveStraight::DriveStraight(double distance, Drivetrain* drivetrain)
     : frc2::CommandHelper<frc2::PIDCommand, DriveStraight>(
           frc2::PIDController(4, 0, 0),
-          [this]() { return m_drivetrain->GetDistance(); }, distance,
-          [this](double output) { m_drivetrain->Drive(output, output); },
+          [drivetrain] { return drivetrain->GetDistance(); }, distance,
+          [drivetrain](double output) { drivetrain->Drive(output, output); },
           {drivetrain}),
       m_drivetrain(drivetrain) {
   m_controller.SetTolerance(0.01);
@@ -28,4 +25,6 @@
   frc2::PIDCommand::Initialize();
 }
 
-bool DriveStraight::IsFinished() { return m_controller.AtSetpoint(); }
+bool DriveStraight::IsFinished() {
+  return m_controller.AtSetpoint();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
index bd1414a..e05b7bd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/OpenClaw.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/OpenClaw.h"
 
@@ -22,4 +19,6 @@
 }
 
 // Called once after isFinished returns true
-void OpenClaw::End(bool) { m_claw->Stop(); }
+void OpenClaw::End(bool) {
+  m_claw->Stop();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
index 996414f..7d32ef4 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Pickup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/Pickup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
index 764ab0e..ff52a8f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/Place.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/Place.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
index f1399d0..2a05e25 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/PrepareToPickup.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/PrepareToPickup.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
index 5da3045..d594797 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetDistanceToBox.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/SetDistanceToBox.h"
 
@@ -11,11 +8,12 @@
 
 #include "Robot.h"
 
-SetDistanceToBox::SetDistanceToBox(double distance, DriveTrain* drivetrain)
+SetDistanceToBox::SetDistanceToBox(double distance, Drivetrain* drivetrain)
     : frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox>(
           frc2::PIDController(-2, 0, 0),
-          [this]() { return m_drivetrain->GetDistanceToObstacle(); }, distance,
-          [this](double output) { m_drivetrain->Drive(output, output); },
+          [drivetrain] { return drivetrain->GetDistanceToObstacle(); },
+          distance,
+          [drivetrain](double output) { drivetrain->Drive(output, output); },
           {drivetrain}),
       m_drivetrain(drivetrain) {
   m_controller.SetTolerance(0.01);
@@ -28,4 +26,6 @@
   frc2::PIDCommand::Initialize();
 }
 
-bool SetDistanceToBox::IsFinished() { return m_controller.AtSetpoint(); }
+bool SetDistanceToBox::IsFinished() {
+  return m_controller.AtSetpoint();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
index 161c1ee..e539903 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetElevatorSetpoint.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/SetElevatorSetpoint.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
index 50c9d0a..5063a99 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/SetWristSetpoint.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/SetWristSetpoint.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp
index 58b1cfe..a68edab 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/commands/TankDrive.cpp
@@ -1,26 +1,33 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/TankDrive.h"
 
+#include <utility>
+
 #include "Robot.h"
 
 TankDrive::TankDrive(std::function<double()> left,
-                     std::function<double()> right, DriveTrain* drivetrain)
-    : m_left(left), m_right(right), m_drivetrain(drivetrain) {
+                     std::function<double()> right, Drivetrain* drivetrain)
+    : m_left(std::move(left)),
+      m_right(std::move(right)),
+      m_drivetrain(drivetrain) {
   SetName("TankDrive");
   AddRequirements({m_drivetrain});
 }
 
 // Called repeatedly when this Command is scheduled to run
-void TankDrive::Execute() { m_drivetrain->Drive(m_left(), m_right()); }
+void TankDrive::Execute() {
+  m_drivetrain->Drive(m_left(), m_right());
+}
 
 // Make this return true when this Command no longer needs to run execute()
-bool TankDrive::IsFinished() { return false; }
+bool TankDrive::IsFinished() {
+  return false;
+}
 
 // Called once after isFinished returns true
-void TankDrive::End(bool) { m_drivetrain->Drive(0, 0); }
+void TankDrive::End(bool) {
+  m_drivetrain->Drive(0, 0);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
index e4ebc14..b1560da 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Claw.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/Claw.h"
 
@@ -15,16 +12,26 @@
   AddChild("Motor", &m_motor);
 }
 
-void Claw::Open() { m_motor.Set(-1); }
+void Claw::Open() {
+  m_motor.Set(-1);
+}
 
-void Claw::Close() { m_motor.Set(1); }
+void Claw::Close() {
+  m_motor.Set(1);
+}
 
-void Claw::Stop() { m_motor.Set(0); }
+void Claw::Stop() {
+  m_motor.Set(0);
+}
 
-bool Claw::IsGripping() { return m_contact.Get(); }
+bool Claw::IsGripping() {
+  return m_contact.Get();
+}
 
 void Claw::Log() {
   frc::SmartDashboard::PutBoolean("Claw switch", IsGripping());
 }
 
-void Claw::Periodic() { Log(); }
+void Claw::Periodic() {
+  Log();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
deleted file mode 100644
index 6bc946c..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/DriveTrain.cpp
+++ /dev/null
@@ -1,71 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "subsystems/DriveTrain.h"
-
-#include <frc/Joystick.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-
-DriveTrain::DriveTrain() {
-// Encoders may measure differently in the real world and in
-// simulation. In this example the robot moves 0.042 barleycorns
-// per tick in the real world, but the simulated encoders
-// simulate 360 tick encoders. This if statement allows for the
-// real robot to handle this difference in devices.
-#ifndef SIMULATION
-  m_leftEncoder.SetDistancePerPulse(0.042);
-  m_rightEncoder.SetDistancePerPulse(0.042);
-#else
-  // Circumference in ft = 4in/12(in/ft)*PI
-  m_leftEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
-                                    360.0);
-  m_rightEncoder.SetDistancePerPulse(static_cast<double>(4.0 / 12.0 * M_PI) /
-                                     360.0);
-#endif
-  SetName("DriveTrain");
-  // Let's show everything on the LiveWindow
-  AddChild("Front_Left Motor", &m_frontLeft);
-  AddChild("Rear Left Motor", &m_rearLeft);
-  AddChild("Front Right Motor", &m_frontRight);
-  AddChild("Rear Right Motor", &m_rearRight);
-  AddChild("Left Encoder", &m_leftEncoder);
-  AddChild("Right Encoder", &m_rightEncoder);
-  AddChild("Rangefinder", &m_rangefinder);
-  AddChild("Gyro", &m_gyro);
-}
-
-void DriveTrain::Log() {
-  frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance());
-  frc::SmartDashboard::PutNumber("Right Distance",
-                                 m_rightEncoder.GetDistance());
-  frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
-  frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
-  frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
-}
-
-void DriveTrain::Drive(double left, double right) {
-  m_robotDrive.TankDrive(left, right);
-}
-
-double DriveTrain::GetHeading() { return m_gyro.GetAngle(); }
-
-void DriveTrain::Reset() {
-  m_gyro.Reset();
-  m_leftEncoder.Reset();
-  m_rightEncoder.Reset();
-}
-
-double DriveTrain::GetDistance() {
-  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
-}
-
-double DriveTrain::GetDistanceToObstacle() {
-  // Really meters in simulation since it's a rangefinder...
-  return m_rangefinder.GetAverageVoltage();
-}
-
-void DriveTrain::Periodic() { Log(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp
new file mode 100644
index 0000000..f3d41a2
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Drivetrain.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/Drivetrain.h"
+
+#include <frc/Joystick.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <units/length.h>
+#include <wpi/numbers>
+
+Drivetrain::Drivetrain() {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_right.SetInverted(true);
+
+// Encoders may measure differently in the real world and in
+// simulation. In this example the robot moves 0.042 barleycorns
+// per tick in the real world, but the simulated encoders
+// simulate 360 tick encoders. This if statement allows for the
+// real robot to handle this difference in devices.
+#ifndef SIMULATION
+  m_leftEncoder.SetDistancePerPulse(0.042);
+  m_rightEncoder.SetDistancePerPulse(0.042);
+#else
+  // Circumference = diameter * pi. 360 tick simulated encoders.
+  m_leftEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() *
+                                    wpi::numbers::pi / 360.0);
+  m_rightEncoder.SetDistancePerPulse(units::foot_t{4_in}.value() *
+                                     wpi::numbers::pi / 360.0);
+#endif
+  SetName("Drivetrain");
+  // Let's show everything on the LiveWindow
+  AddChild("Front_Left Motor", &m_frontLeft);
+  AddChild("Rear Left Motor", &m_rearLeft);
+  AddChild("Front Right Motor", &m_frontRight);
+  AddChild("Rear Right Motor", &m_rearRight);
+  AddChild("Left Encoder", &m_leftEncoder);
+  AddChild("Right Encoder", &m_rightEncoder);
+  AddChild("Rangefinder", &m_rangefinder);
+  AddChild("Gyro", &m_gyro);
+}
+
+void Drivetrain::Log() {
+  frc::SmartDashboard::PutNumber("Left Distance", m_leftEncoder.GetDistance());
+  frc::SmartDashboard::PutNumber("Right Distance",
+                                 m_rightEncoder.GetDistance());
+  frc::SmartDashboard::PutNumber("Left Speed", m_leftEncoder.GetRate());
+  frc::SmartDashboard::PutNumber("Right Speed", m_rightEncoder.GetRate());
+  frc::SmartDashboard::PutNumber("Gyro", m_gyro.GetAngle());
+}
+
+void Drivetrain::Drive(double left, double right) {
+  m_robotDrive.TankDrive(left, right);
+}
+
+double Drivetrain::GetHeading() {
+  return m_gyro.GetAngle();
+}
+
+void Drivetrain::Reset() {
+  m_gyro.Reset();
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+double Drivetrain::GetDistance() {
+  return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
+}
+
+double Drivetrain::GetDistanceToObstacle() {
+  // Really meters in simulation since it's a rangefinder...
+  return m_rangefinder.GetAverageVoltage();
+}
+
+void Drivetrain::Periodic() {
+  Log();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
index d5e4ddd..0c195e3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Elevator.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/Elevator.h"
 
@@ -24,12 +21,18 @@
   AddChild("Pot", &m_pot);
 }
 
-void Elevator::Log() { frc::SmartDashboard::PutData("Wrist Pot", &m_pot); }
+void Elevator::Log() {
+  frc::SmartDashboard::PutData("Wrist Pot", &m_pot);
+}
 
-double Elevator::GetMeasurement() { return m_pot.Get(); }
+double Elevator::GetMeasurement() {
+  return m_pot.Get();
+}
 
 void Elevator::UseOutput(double output, double setpoint) {
   m_motor.Set(output);
 }
 
-void Elevator::Periodic() { Log(); }
+void Elevator::Periodic() {
+  Log();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
index a335a63..fb3ab35 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/cpp/subsystems/Wrist.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/Wrist.h"
 
@@ -26,8 +23,14 @@
   frc::SmartDashboard::PutNumber("Wrist Angle", GetMeasurement());
 }
 
-double Wrist::GetMeasurement() { return m_pot.Get(); }
+double Wrist::GetMeasurement() {
+  return m_pot.Get();
+}
 
-void Wrist::UseOutput(double output, double setpoint) { m_motor.Set(output); }
+void Wrist::UseOutput(double output, double setpoint) {
+  m_motor.Set(output);
+}
 
-void Wrist::Periodic() { Log(); }
+void Wrist::Periodic() {
+  Log();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h
index 50fc7d6..ae1ec9d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/RobotContainer.h
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/Joystick.h>
+#include <frc/XboxController.h>
 #include <frc2/command/Command.h>
 
 #include "commands/Autonomous.h"
 #include "subsystems/Claw.h"
-#include "subsystems/DriveTrain.h"
+#include "subsystems/Drivetrain.h"
 #include "subsystems/Elevator.h"
 #include "subsystems/Wrist.h"
 
@@ -31,12 +28,12 @@
 
  private:
   // The robot's subsystems and commands are defined here...
-  frc::Joystick m_joy{0};
+  frc::XboxController m_joy{0};
 
   Claw m_claw;
   Wrist m_wrist;
   Elevator m_elevator;
-  DriveTrain m_drivetrain;
+  Drivetrain m_drivetrain;
 
   Autonomous m_autonomousCommand;
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
index c0d429c..5dae1c1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Autonomous.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <frc2/command/SequentialCommandGroup.h>
 
 #include "subsystems/Claw.h"
-#include "subsystems/DriveTrain.h"
+#include "subsystems/Drivetrain.h"
 #include "subsystems/Elevator.h"
 #include "subsystems/Wrist.h"
 
@@ -22,5 +19,5 @@
     : public frc2::CommandHelper<frc2::SequentialCommandGroup, Autonomous> {
  public:
   Autonomous(Claw* claw, Wrist* wrist, Elevator* elevator,
-             DriveTrain* drivetrain);
+             Drivetrain* drivetrain);
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
index a5e6c0b..fe8133b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/CloseClaw.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -13,8 +10,7 @@
 #include "subsystems/Claw.h"
 
 /**
- * Opens the claw for one second. Real robots should use sensors, stalling
- * motors is BAD!
+ * Closes the claw until the limit switch is tripped.
  */
 class CloseClaw : public frc2::CommandHelper<frc2::CommandBase, CloseClaw> {
  public:
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
index 1955169..abc598c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/DriveStraight.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc2/command/CommandHelper.h>
 #include <frc2/command/PIDCommand.h>
 
-#include "subsystems/DriveTrain.h"
+#include "subsystems/Drivetrain.h"
 
 /**
  * Drive the given distance straight (negative values go backwards).
@@ -21,10 +18,10 @@
 class DriveStraight
     : public frc2::CommandHelper<frc2::PIDCommand, DriveStraight> {
  public:
-  explicit DriveStraight(double distance, DriveTrain* drivetrain);
+  explicit DriveStraight(double distance, Drivetrain* drivetrain);
   void Initialize() override;
   bool IsFinished() override;
 
  private:
-  DriveTrain* m_drivetrain;
+  Drivetrain* m_drivetrain;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
index 486f86b..f7b3d0f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/OpenClaw.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
index 4d74588..33327eb 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Pickup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
index 85945c3..353ef1c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/Place.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
index b2aa4d1..0289d4f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/PrepareToPickup.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
index b5b7b13..2104a5c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetDistanceToBox.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc2/command/CommandHelper.h>
 #include <frc2/command/PIDCommand.h>
 
-#include "subsystems/DriveTrain.h"
+#include "subsystems/Drivetrain.h"
 
 /**
  * Drive until the robot is the given distance away from the box. Uses a local
@@ -21,10 +18,10 @@
 class SetDistanceToBox
     : public frc2::CommandHelper<frc2::PIDCommand, SetDistanceToBox> {
  public:
-  explicit SetDistanceToBox(double distance, DriveTrain* drivetrain);
+  explicit SetDistanceToBox(double distance, Drivetrain* drivetrain);
   void Initialize() override;
   bool IsFinished() override;
 
  private:
-  DriveTrain* m_drivetrain;
+  Drivetrain* m_drivetrain;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
index c3bcf6f..8df402d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetElevatorSetpoint.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
index 96c01fa..e362b30 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/SetWristSetpoint.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h
index bdfd592..c49fdf3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/commands/TankDrive.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc2/command/CommandBase.h>
 #include <frc2/command/CommandHelper.h>
 
-#include "subsystems/DriveTrain.h"
+#include "subsystems/Drivetrain.h"
 
 /**
  * Have the robot drive tank style using the PS3 Joystick until interrupted.
@@ -18,7 +15,7 @@
 class TankDrive : public frc2::CommandHelper<frc2::CommandBase, TankDrive> {
  public:
   TankDrive(std::function<double()> left, std::function<double()> right,
-            DriveTrain* drivetrain);
+            Drivetrain* drivetrain);
   void Execute() override;
   bool IsFinished() override;
   void End(bool interrupted) override;
@@ -26,5 +23,5 @@
  private:
   std::function<double()> m_left;
   std::function<double()> m_right;
-  DriveTrain* m_drivetrain;
+  Drivetrain* m_drivetrain;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
index a660c1c..3b65382 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Claw.h
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/DigitalInput.h>
-#include <frc/PWMVictorSPX.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 /**
@@ -53,6 +50,6 @@
   void Periodic() override;
 
  private:
-  frc::PWMVictorSPX m_motor{7};
+  frc::PWMSparkMax m_motor{7};
   frc::DigitalInput m_contact{5};
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
deleted file mode 100644
index 0fed552..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/DriveTrain.h
+++ /dev/null
@@ -1,84 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/AnalogGyro.h>
-#include <frc/AnalogInput.h>
-#include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
-#include <frc/drive/DifferentialDrive.h>
-#include <frc2/command/SubsystemBase.h>
-
-namespace frc {
-class Joystick;
-}  // namespace frc
-
-/**
- * The DriveTrain subsystem incorporates the sensors and actuators attached to
- * the robots chassis. These include four drive motors, a left and right encoder
- * and a gyro.
- */
-class DriveTrain : public frc2::SubsystemBase {
- public:
-  DriveTrain();
-
-  /**
-   * The log method puts interesting information to the SmartDashboard.
-   */
-  void Log();
-
-  /**
-   * Tank style driving for the DriveTrain.
-   * @param left Speed in range [-1,1]
-   * @param right Speed in range [-1,1]
-   */
-  void Drive(double left, double right);
-
-  /**
-   * @return The robots heading in degrees.
-   */
-  double GetHeading();
-
-  /**
-   * Reset the robots sensors to the zero states.
-   */
-  void Reset();
-
-  /**
-   * @return The distance driven (average of left and right encoders).
-   */
-  double GetDistance();
-
-  /**
-   * @return The distance to the obstacle detected by the rangefinder.
-   */
-  double GetDistanceToObstacle();
-
-  /**
-   * Log the data periodically. This method is automatically called
-   * by the subsystem.
-   */
-  void Periodic() override;
-
- private:
-  frc::PWMVictorSPX m_frontLeft{1};
-  frc::PWMVictorSPX m_rearLeft{2};
-  frc::SpeedControllerGroup m_left{m_frontLeft, m_rearLeft};
-
-  frc::PWMVictorSPX m_frontRight{3};
-  frc::PWMVictorSPX m_rearRight{4};
-  frc::SpeedControllerGroup m_right{m_frontRight, m_rearRight};
-
-  frc::DifferentialDrive m_robotDrive{m_left, m_right};
-
-  frc::Encoder m_leftEncoder{1, 2};
-  frc::Encoder m_rightEncoder{3, 4};
-  frc::AnalogInput m_rangefinder{6};
-  frc::AnalogGyro m_gyro{1};
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h
new file mode 100644
index 0000000..108a9c0
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Drivetrain.h
@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/AnalogInput.h>
+#include <frc/Encoder.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc2/command/SubsystemBase.h>
+
+namespace frc {
+class Joystick;
+}  // namespace frc
+
+/**
+ * The Drivetrain subsystem incorporates the sensors and actuators attached to
+ * the robots chassis. These include four drive motors, a left and right encoder
+ * and a gyro.
+ */
+class Drivetrain : public frc2::SubsystemBase {
+ public:
+  Drivetrain();
+
+  /**
+   * The log method puts interesting information to the SmartDashboard.
+   */
+  void Log();
+
+  /**
+   * Tank style driving for the Drivetrain.
+   * @param left Speed in range [-1,1]
+   * @param right Speed in range [-1,1]
+   */
+  void Drive(double left, double right);
+
+  /**
+   * @return The robots heading in degrees.
+   */
+  double GetHeading();
+
+  /**
+   * Reset the robots sensors to the zero states.
+   */
+  void Reset();
+
+  /**
+   * @return The distance driven (average of left and right encoders).
+   */
+  double GetDistance();
+
+  /**
+   * @return The distance to the obstacle detected by the rangefinder.
+   */
+  double GetDistanceToObstacle();
+
+  /**
+   * Log the data periodically. This method is automatically called
+   * by the subsystem.
+   */
+  void Periodic() override;
+
+ private:
+  frc::PWMSparkMax m_frontLeft{1};
+  frc::PWMSparkMax m_rearLeft{2};
+  frc::MotorControllerGroup m_left{m_frontLeft, m_rearLeft};
+
+  frc::PWMSparkMax m_frontRight{3};
+  frc::PWMSparkMax m_rearRight{4};
+  frc::MotorControllerGroup m_right{m_frontRight, m_rearRight};
+
+  frc::DifferentialDrive m_robotDrive{m_left, m_right};
+
+  frc::Encoder m_leftEncoder{1, 2};
+  frc::Encoder m_rightEncoder{3, 4};
+  frc::AnalogInput m_rangefinder{6};
+  frc::AnalogGyro m_gyro{1};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
index 5d2cbd1..80008c5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Elevator.h
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/AnalogPotentiometer.h>
-#include <frc/PWMVictorSPX.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/PIDSubsystem.h>
 
 /**
@@ -46,7 +43,7 @@
   void Periodic() override;
 
  private:
-  frc::PWMVictorSPX m_motor{5};
+  frc::PWMSparkMax m_motor{5};
   double m_setpoint = 0;
 
 // Conversion value of potentiometer varies between the real world and
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
index 795d2a4..e873c2c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GearsBot/include/subsystems/Wrist.h
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/AnalogPotentiometer.h>
-#include <frc/PWMVictorSPX.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/PIDSubsystem.h>
 
 /**
@@ -43,7 +40,7 @@
   void Periodic() override;
 
  private:
-  frc::PWMVictorSPX m_motor{6};
+  frc::PWMSparkMax m_motor{6};
   double m_setpoint = 0;
 
 // Conversion value of potentiometer varies between the real world and
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
index f43cb57..6b7d2b6 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GettingStarted/cpp/Robot.cpp
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/Timer.h>
 #include <frc/drive/DifferentialDrive.h>
-#include <frc/livewindow/LiveWindow.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 class Robot : public frc::TimedRobot {
  public:
   Robot() {
-    m_robotDrive.SetExpiration(0.1);
+    m_robotDrive.SetExpiration(100_ms);
     m_timer.Start();
   }
 
@@ -26,9 +22,9 @@
 
   void AutonomousPeriodic() override {
     // Drive for 2 seconds
-    if (m_timer.Get() < 2.0) {
+    if (m_timer.Get() < 2_s) {
       // Drive forwards half speed
-      m_robotDrive.ArcadeDrive(-0.5, 0.0);
+      m_robotDrive.ArcadeDrive(0.5, 0.0);
     } else {
       // Stop robot
       m_robotDrive.ArcadeDrive(0.0, 0.0);
@@ -42,19 +38,22 @@
     m_robotDrive.ArcadeDrive(m_stick.GetY(), m_stick.GetX());
   }
 
+  void TestInit() override {}
+
   void TestPeriodic() override {}
 
  private:
   // Robot drive system
-  frc::PWMVictorSPX m_left{0};
-  frc::PWMVictorSPX m_right{1};
+  frc::PWMSparkMax m_left{0};
+  frc::PWMSparkMax m_right{1};
   frc::DifferentialDrive m_robotDrive{m_left, m_right};
 
   frc::Joystick m_stick{0};
-  frc::LiveWindow& m_lw = *frc::LiveWindow::GetInstance();
   frc::Timer m_timer;
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
index b0c70d2..1dda356 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Gyro/cpp/Robot.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
 #include <frc/AnalogGyro.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a sample program to demonstrate how to use a gyro sensor to make a
@@ -47,8 +44,8 @@
   static constexpr int kGyroPort = 0;
   static constexpr int kJoystickPort = 0;
 
-  frc::PWMVictorSPX m_left{kLeftMotorPort};
-  frc::PWMVictorSPX m_right{kRightMotorPort};
+  frc::PWMSparkMax m_left{kLeftMotorPort};
+  frc::PWMSparkMax m_right{kRightMotorPort};
   frc::DifferentialDrive m_robotDrive{m_left, m_right};
 
   frc::AnalogGyro m_gyro{kGyroPort};
@@ -56,5 +53,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
index 6213333..eb95395 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -22,9 +19,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
@@ -34,8 +30,8 @@
 
   // Assorted commands to be bound to buttons
 
-  // Stabilize robot to drive straight with gyro when left bumper is held
-  frc2::JoystickButton(&m_driverController, 5)
+  // Stabilize robot to drive straight with gyro when L1 is held
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kL1)
       .WhenHeld(frc2::PIDCommand{
           frc2::PIDController{dc::kStabilizationP, dc::kStabilizationI,
                               dc::kStabilizationD},
@@ -45,24 +41,22 @@
           0,
           // Pipe the output to the turning controls
           [this](double output) {
-            m_drive.ArcadeDrive(m_driverController.GetY(
-                                    frc::GenericHID::JoystickHand::kLeftHand),
-                                output);
+            m_drive.ArcadeDrive(m_driverController.GetLeftY(), output);
           },
           // Require the robot drive
           {&m_drive}});
 
-  // Turn to 90 degrees when the 'X' button is pressed
-  frc2::JoystickButton(&m_driverController, 3)
+  // Turn to 90 degrees when the 'Cross' button is pressed
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCross)
       .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
 
-  // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5
-  // second timeout
-  frc2::JoystickButton(&m_driverController, 1)
+  // Turn to -90 degrees with a profile when the 'Square' button is pressed,
+  // with a 5 second timeout
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare)
       .WhenPressed(TurnToAngle{90_deg, &m_drive}.WithTimeout(5_s));
 
-  // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  // While holding R1, drive at half speed
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1)
       .WhenPressed(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(0.5); }})
       .WhenReleased(frc2::InstantCommand{[this] { m_drive.SetMaxOutput(1); }});
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
index 0be1f73..681bb39 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngle.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/TurnToAngle.h"
 
@@ -15,9 +12,9 @@
     : CommandHelper(
           frc2::PIDController(kTurnP, kTurnI, kTurnD),
           // Close loop on heading
-          [drive] { return drive->GetHeading().to<double>(); },
+          [drive] { return drive->GetHeading().value(); },
           // Set reference to target
-          target.to<double>(),
+          target.value(),
           // Pipe output to turn robot
           [drive](double output) { drive->ArcadeDrive(0, output); },
           // Require the drive
@@ -27,10 +24,11 @@
   // Set the controller tolerance - the delta tolerance ensures the robot is
   // stationary at the setpoint before it is considered as having reached the
   // reference
-  m_controller.SetTolerance(kTurnTolerance.to<double>(),
-                            kTurnRateTolerance.to<double>());
+  m_controller.SetTolerance(kTurnTolerance.value(), kTurnRateTolerance.value());
 
   AddRequirements({drive});
 }
 
-bool TurnToAngle::IsFinished() { return GetController().AtSetpoint(); }
+bool TurnToAngle::IsFinished() {
+  return GetController().AtSetpoint();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
index c2baa90..a359625 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/commands/TurnToAngleProfiled.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/TurnToAngleProfiled.h"
 
@@ -36,4 +33,6 @@
   AddRequirements({drive});
 }
 
-bool TurnToAngleProfiled::IsFinished() { return GetController().AtGoal(); }
+bool TurnToAngleProfiled::IsFinished() {
+  return GetController().AtGoal();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
index c6d8d8c..baa78de 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -16,6 +13,11 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -38,9 +40,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
index cc8b841..210f82d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Constants.h
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <units/angle.h>
 #include <units/angular_velocity.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -35,7 +32,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 
 constexpr bool kGyroReversed = true;
 
@@ -61,5 +59,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
index 37c7e4f..041812e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/RobotContainer.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/XboxController.h>
+#include <frc/PS4Controller.h>
 #include <frc/controller/PIDController.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
@@ -35,7 +32,7 @@
 
  private:
   // The driver's controller
-  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+  frc::PS4Controller m_driverController{OIConstants::kDriverControllerPort};
 
   // The robot's subsystems and commands are defined here...
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
index 4afa20e..6df79bf 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngle.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
index 0b52044..f3136aa 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/commands/TurnToAngleProfiled.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
index cd3d667..96174dd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroDriveCommands/include/subsystems/DriveSubsystem.h
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/angle.h>
 
@@ -89,16 +86,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
index 86b4f24..5885873 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/GyroMecanum/cpp/Robot.cpp
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/AnalogGyro.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/MecanumDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a sample program that uses mecanum drive with a gyro sensor to
@@ -47,10 +44,10 @@
   static constexpr int kGyroPort = 0;
   static constexpr int kJoystickPort = 0;
 
-  frc::PWMVictorSPX m_frontLeft{kFrontLeftMotorPort};
-  frc::PWMVictorSPX m_rearLeft{kRearLeftMotorPort};
-  frc::PWMVictorSPX m_frontRight{kFrontRightMotorPort};
-  frc::PWMVictorSPX m_rearRight{kRearRightMotorPort};
+  frc::PWMSparkMax m_frontLeft{kFrontLeftMotorPort};
+  frc::PWMSparkMax m_rearLeft{kRearLeftMotorPort};
+  frc::PWMSparkMax m_frontRight{kFrontRightMotorPort};
+  frc::PWMSparkMax m_rearRight{kRearRightMotorPort};
   frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
                                  m_rearRight};
 
@@ -59,5 +56,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
index ae5833e..0db402b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HAL/c/Robot.c
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 This example shows how to use the HAL directly, and what is needed to run a
@@ -65,10 +62,11 @@
 
   // Create a Motor Controller
   status = 0;
-  HAL_DigitalHandle pwmPort = HAL_InitializePWMPort(HAL_GetPort(2), &status);
+  HAL_DigitalHandle pwmPort =
+      HAL_InitializePWMPort(HAL_GetPort(2), NULL, &status);
 
   if (status != 0) {
-    const char* message = HAL_GetErrorMessage(status);
+    const char* message = HAL_GetLastError(&status);
     printf("%s\n", message);
     return 1;
   }
@@ -78,10 +76,11 @@
 
   // Create an Input
   status = 0;
-  HAL_DigitalHandle dio = HAL_InitializeDIOPort(HAL_GetPort(2), 1, &status);
+  HAL_DigitalHandle dio =
+      HAL_InitializeDIOPort(HAL_GetPort(2), 1, NULL, &status);
 
   if (status != 0) {
-    const char* message = HAL_GetErrorMessage(status);
+    const char* message = HAL_GetLastError(&status);
     printf("%s\n", message);
     status = 0;
     HAL_FreePWMPort(pwmPort, &status);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
index 90865ac..31a78b5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -26,9 +23,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
@@ -36,12 +32,14 @@
 void RobotContainer::ConfigureButtonBindings() {
   // Configure your button bindings here
 
-  // Grab the hatch when the 'A' button is pressed.
-  frc2::JoystickButton(&m_driverController, 1).WhenPressed(&m_grabHatch);
-  // Release the hatch when the 'B' button is pressed.
-  frc2::JoystickButton(&m_driverController, 2).WhenPressed(&m_releaseHatch);
-  // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  // Grab the hatch when the 'Circle' button is pressed.
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kCircle)
+      .WhenPressed(&m_grabHatch);
+  // Release the hatch when the 'Square' button is pressed.
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kSquare)
+      .WhenPressed(&m_releaseHatch);
+  // While holding R1, drive at half speed
+  frc2::JoystickButton(&m_driverController, frc::PS4Controller::Button::kR1)
       .WhenPressed(&m_driveHalfSpeed)
       .WhenReleased(&m_driveFullSpeed);
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp
index c5f928b..f88850e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/commands/ComplexAuto.cpp
@@ -1,37 +1,50 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/ComplexAuto.h"
 
+#include <frc2/command/FunctionalCommand.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
-#include <frc2/command/StartEndCommand.h>
 
 using namespace AutoConstants;
 
 ComplexAuto::ComplexAuto(DriveSubsystem* drive, HatchSubsystem* hatch) {
   AddCommands(
       // Drive forward the specified distance
-      frc2::StartEndCommand([drive] { drive->ArcadeDrive(kAutoDriveSpeed, 0); },
-                            [drive] { drive->ArcadeDrive(0, 0); }, {drive})
-          .BeforeStarting([drive] { drive->ResetEncoders(); })
-          .WithInterrupt([drive] {
+      frc2::FunctionalCommand(
+          // Reset encoders on command start
+          [&] { drive->ResetEncoders(); },
+          // Drive forward while the command is executing
+          [&] { drive->ArcadeDrive(kAutoDriveSpeed, 0); },
+          // Stop driving at the end of the command
+          [&](bool interrupted) { drive->ArcadeDrive(0, 0); },
+          // End the command when the robot's driven distance exceeds the
+          // desired value
+          [&] {
             return drive->GetAverageEncoderDistance() >=
                    kAutoDriveDistanceInches;
-          }),
+          },
+          // Requires the drive subsystem
+          {drive}),
       // Release the hatch
       frc2::InstantCommand([hatch] { hatch->ReleaseHatch(); }, {hatch}),
       // Drive backward the specified distance
-      frc2::StartEndCommand(
-          [drive] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); },
-          [drive] { drive->ArcadeDrive(0, 0); }, {drive})
-          .BeforeStarting([drive] { drive->ResetEncoders(); })
-          .WithInterrupt([drive] {
+      // Drive forward the specified distance
+      frc2::FunctionalCommand(
+          // Reset encoders on command start
+          [&] { drive->ResetEncoders(); },
+          // Drive backward while the command is executing
+          [&] { drive->ArcadeDrive(-kAutoDriveSpeed, 0); },
+          // Stop driving at the end of the command
+          [&](bool interrupted) { drive->ArcadeDrive(0, 0); },
+          // End the command when the robot's driven distance exceeds the
+          // desired value
+          [&] {
             return drive->GetAverageEncoderDistance() <=
                    kAutoBackupDistanceInches;
-          }));
+          },
+          // Requires the drive subsystem
+          {drive}));
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
index 47c898e..bd94b68 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -16,6 +13,11 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -38,9 +40,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp
index ea7b796..ba1c0dd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/cpp/subsystems/HatchSubsystem.cpp
@@ -1,16 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/HatchSubsystem.h"
 
 using namespace HatchConstants;
 
 HatchSubsystem::HatchSubsystem()
-    : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
+    : m_hatchSolenoid{frc::PneumaticsModuleType::CTREPCM,
+                      kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
 
 void HatchSubsystem::GrabHatch() {
   m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
index d465f6c..e9fbfdc 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Constants.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -33,7 +30,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace HatchConstants {
@@ -48,5 +46,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
index 106812b..b17c55d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/RobotContainer.h
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/XboxController.h>
+#include <frc/PS4Controller.h>
 #include <frc/smartdashboard/SendableChooser.h>
 #include <frc2/command/Command.h>
+#include <frc2/command/FunctionalCommand.h>
 #include <frc2/command/InstantCommand.h>
 #include <frc2/command/ParallelRaceGroup.h>
 #include <frc2/command/RunCommand.h>
 #include <frc2/command/SequentialCommandGroup.h>
-#include <frc2/command/StartEndCommand.h>
 
 #include "Constants.h"
 #include "commands/ComplexAuto.h"
@@ -38,7 +35,7 @@
 
  private:
   // The driver's controller
-  frc::XboxController m_driverController{OIConstants::kDriverControllerPort};
+  frc::PS4Controller m_driverController{OIConstants::kDriverControllerPort};
 
   // The robot's subsystems and commands are defined here...
 
@@ -47,15 +44,21 @@
   HatchSubsystem m_hatch;
 
   // The autonomous routines
-  frc2::ParallelRaceGroup m_simpleAuto =
-      frc2::StartEndCommand(
-          [this] { m_drive.ArcadeDrive(ac::kAutoDriveSpeed, 0); },
-          [this] { m_drive.ArcadeDrive(0, 0); }, {&m_drive})
-          .BeforeStarting([this] { m_drive.ResetEncoders(); })
-          .WithInterrupt([this] {
-            return m_drive.GetAverageEncoderDistance() >=
-                   ac::kAutoDriveDistanceInches;
-          });
+  frc2::FunctionalCommand m_simpleAuto = frc2::FunctionalCommand(
+      // Reset encoders on command start
+      [this] { m_drive.ResetEncoders(); },
+      // Drive forward while the command is executing
+      [this] { m_drive.ArcadeDrive(AutoConstants::kAutoDriveSpeed, 0); },
+      // Stop driving at the end of the command
+      [this](bool interrupted) { m_drive.ArcadeDrive(0, 0); },
+      // End the command when the robot's driven distance exceeds the desired
+      // value
+      [this] {
+        return m_drive.GetAverageEncoderDistance() >=
+               AutoConstants::kAutoDriveDistanceInches;
+      },
+      // Requires the drive subsystem
+      {&m_drive});
   ComplexAuto m_complexAuto{&m_drive, &m_hatch};
 
   // Assorted commands to be bound to buttons
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h
index b767f3b..e746d8a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/commands/ComplexAuto.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
index 3ed1357..47bf28e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/DriveSubsystem.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -73,16 +70,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h
index 681aea8..bb06100 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotInlined/include/subsystems/HatchSubsystem.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/DoubleSolenoid.h>
+#include <frc/PneumaticsControlModule.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
index d4199ef..b46efd4 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -30,9 +27,8 @@
 
   // Set up default drive command
   m_drive.SetDefaultCommand(DefaultDrive(
-      &m_drive,
-      [this] { return m_driverController.GetY(frc::GenericHID::kLeftHand); },
-      [this] { return m_driverController.GetX(frc::GenericHID::kRightHand); }));
+      &m_drive, [this] { return m_driverController.GetLeftY(); },
+      [this] { return m_driverController.GetRightX(); }));
 }
 
 void RobotContainer::ConfigureButtonBindings() {
@@ -44,13 +40,14 @@
   // stack-allocated and declared as members of RobotContainer.
 
   // Grab the hatch when the 'A' button is pressed.
-  frc2::JoystickButton(&m_driverController, 1)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
       .WhenPressed(new GrabHatch(&m_hatch));
   // Release the hatch when the 'B' button is pressed.
-  frc2::JoystickButton(&m_driverController, 2)
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kB)
       .WhenPressed(new ReleaseHatch(&m_hatch));
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
       .WhenHeld(new HalveDriveSpeed(&m_drive));
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp
index cb41de6..cfc5153 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ComplexAuto.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/ComplexAuto.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp
index 3bdee6e..7ef404e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DefaultDrive.cpp
@@ -1,16 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/DefaultDrive.h"
 
+#include <utility>
+
 DefaultDrive::DefaultDrive(DriveSubsystem* subsystem,
                            std::function<double()> forward,
                            std::function<double()> rotation)
-    : m_drive{subsystem}, m_forward{forward}, m_rotation{rotation} {
+    : m_drive{subsystem},
+      m_forward{std::move(forward)},
+      m_rotation{std::move(rotation)} {
   AddRequirements({subsystem});
 }
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp
index 6c7ef40..7ee1b2c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/DriveDistance.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/DriveDistance.h"
 
@@ -20,7 +17,13 @@
   m_drive->ArcadeDrive(m_speed, 0);
 }
 
-void DriveDistance::End(bool interrupted) { m_drive->ArcadeDrive(0, 0); }
+void DriveDistance::Execute() {
+  m_drive->ArcadeDrive(m_speed, 0);
+}
+
+void DriveDistance::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+}
 
 bool DriveDistance::IsFinished() {
   return std::abs(m_drive->GetAverageEncoderDistance()) >= m_distance;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp
index f665761..e769cbd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/GrabHatch.cpp
@@ -1,16 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/GrabHatch.h"
 
 GrabHatch::GrabHatch(HatchSubsystem* subsystem) : m_hatch(subsystem) {
-  AddRequirements({subsystem});
+  AddRequirements(subsystem);
 }
 
-void GrabHatch::Initialize() { m_hatch->GrabHatch(); }
+void GrabHatch::Initialize() {
+  m_hatch->GrabHatch();
+}
 
-bool GrabHatch::IsFinished() { return true; }
+bool GrabHatch::IsFinished() {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
index b35b2c6..0f561b3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/HalveDriveSpeed.cpp
@@ -1,15 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/HalveDriveSpeed.h"
 
 HalveDriveSpeed::HalveDriveSpeed(DriveSubsystem* subsystem)
     : m_drive(subsystem) {}
 
-void HalveDriveSpeed::Initialize() { m_drive->SetMaxOutput(0.5); }
+void HalveDriveSpeed::Initialize() {
+  m_drive->SetMaxOutput(0.5);
+}
 
-void HalveDriveSpeed::End(bool interrupted) { m_drive->SetMaxOutput(1); }
+void HalveDriveSpeed::End(bool interrupted) {
+  m_drive->SetMaxOutput(1);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp
index e8fbd61..7e6c9e8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/commands/ReleaseHatch.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/ReleaseHatch.h"
 
@@ -11,6 +8,10 @@
   AddRequirements({subsystem});
 }
 
-void ReleaseHatch::Initialize() { m_hatch->ReleaseHatch(); }
+void ReleaseHatch::Initialize() {
+  m_hatch->ReleaseHatch();
+}
 
-bool ReleaseHatch::IsFinished() { return true; }
+bool ReleaseHatch::IsFinished() {
+  return true;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
index 47c898e..bd94b68 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -16,6 +13,11 @@
       m_right2{kRightMotor2Port},
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]} {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -38,9 +40,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp
index ea7b796..ba1c0dd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/cpp/subsystems/HatchSubsystem.cpp
@@ -1,16 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/HatchSubsystem.h"
 
 using namespace HatchConstants;
 
 HatchSubsystem::HatchSubsystem()
-    : m_hatchSolenoid{kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
+    : m_hatchSolenoid{frc::PneumaticsModuleType::CTREPCM,
+                      kHatchSolenoidPorts[0], kHatchSolenoidPorts[1]} {}
 
 void HatchSubsystem::GrabHatch() {
   m_hatchSolenoid.Set(frc::DoubleSolenoid::kForward);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
index d465f6c..e9fbfdc 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Constants.h
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * The Constants header provides a convenient place for teams to hold robot-wide
@@ -33,7 +30,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 }  // namespace DriveConstants
 
 namespace HatchConstants {
@@ -48,5 +46,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h
index 881d2f5..980e6e5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h
index 88a2460..d66394c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ComplexAuto.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h
index d42d133..2e7cac8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DefaultDrive.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h
index 6f350a9..34d2577 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/DriveDistance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -26,6 +23,8 @@
 
   void Initialize() override;
 
+  void Execute() override;
+
   void End(bool interrupted) override;
 
   bool IsFinished() override;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h
index 0ab0c13..0cf5c5a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/GrabHatch.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h
index 0b5d7c7..efc572d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/HalveDriveSpeed.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h
index b98866f..e3628cf 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/commands/ReleaseHatch.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
index 3ed1357..47bf28e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/DriveSubsystem.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -73,16 +70,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h
index 681aea8..bb06100 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HatchbotTraditional/include/subsystems/HatchSubsystem.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/DoubleSolenoid.h>
+#include <frc/PneumaticsControlModule.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp
index 3deafa5..d0fdd6d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/HidRumble/cpp/Robot.cpp
@@ -1,11 +1,7 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/GenericHID.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 
@@ -27,9 +23,11 @@
   }
 
  private:
-  frc::XboxController m_hid{0};
+  frc::GenericHID m_hid{0};
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
index 01a9d04..b61f86d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/IntermediateVision/cpp/Robot.cpp
@@ -1,10 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
+#include <cstdio>
 #include <thread>
 
 #include <cameraserver/CameraServer.h>
@@ -12,7 +10,6 @@
 #include <opencv2/core/core.hpp>
 #include <opencv2/core/types.hpp>
 #include <opencv2/imgproc/imgproc.hpp>
-#include <wpi/raw_ostream.h>
 
 /**
  * This is a demo program showing the use of OpenCV to do vision processing. The
@@ -21,21 +18,20 @@
  * processing.
  */
 class Robot : public frc::TimedRobot {
-#if defined(__linux__)
+#if defined(__linux__) || defined(_WIN32)
 
  private:
   static void VisionThread() {
     // Get the USB camera from CameraServer
-    cs::UsbCamera camera =
-        frc::CameraServer::GetInstance()->StartAutomaticCapture();
+    cs::UsbCamera camera = frc::CameraServer::StartAutomaticCapture();
     // Set the resolution
     camera.SetResolution(640, 480);
 
     // Get a CvSink. This will capture Mats from the Camera
-    cs::CvSink cvSink = frc::CameraServer::GetInstance()->GetVideo();
+    cs::CvSink cvSink = frc::CameraServer::GetVideo();
     // Setup a CvSource. This will send images back to the Dashboard
     cs::CvSource outputStream =
-        frc::CameraServer::GetInstance()->PutVideo("Rectangle", 640, 480);
+        frc::CameraServer::PutVideo("Rectangle", 640, 480);
 
     // Mats are very memory expensive. Lets reuse this Mat.
     cv::Mat mat;
@@ -63,16 +59,18 @@
   void RobotInit() override {
     // We need to run our vision program in a separate thread. If not, our robot
     // program will not run.
-#if defined(__linux__)
+#if defined(__linux__) || defined(_WIN32)
     std::thread visionThread(VisionThread);
     visionThread.detach();
 #else
-    wpi::errs() << "Vision only available on Linux.\n";
-    wpi::errs().flush();
+    std::fputs("Vision only available on Linux or Windows.\n", stderr);
+    std::fflush(stderr);
 #endif
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
index b65b1d7..c33450d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Drivetrain.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Drivetrain.h"
 
@@ -25,13 +22,13 @@
       m_feedforward.Calculate(wheelSpeeds.rearRight);
 
   const double frontLeftOutput = m_frontLeftPIDController.Calculate(
-      m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.to<double>());
+      m_frontLeftEncoder.GetRate(), wheelSpeeds.frontLeft.value());
   const double frontRightOutput = m_frontRightPIDController.Calculate(
-      m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.to<double>());
+      m_frontRightEncoder.GetRate(), wheelSpeeds.frontRight.value());
   const double backLeftOutput = m_backLeftPIDController.Calculate(
-      m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.to<double>());
+      m_backLeftEncoder.GetRate(), wheelSpeeds.rearLeft.value());
   const double backRightOutput = m_backRightPIDController.Calculate(
-      m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.to<double>());
+      m_backRightEncoder.GetRate(), wheelSpeeds.rearRight.value());
 
   m_frontLeftMotor.SetVoltage(units::volt_t{frontLeftOutput} +
                               frontLeftFeedforward);
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
index a9695c4..4943184 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/cpp/Robot.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
 
 #include "Drivetrain.h"
 
@@ -33,23 +30,20 @@
   void DriveWithJoystick(bool fieldRelative) {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed = -m_xspeedLimiter.Calculate(
-                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+    const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
                         Drivetrain::kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    const auto ySpeed = -m_yspeedLimiter.Calculate(
-                            m_controller.GetX(frc::GenericHID::kLeftHand)) *
+    const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
                         Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_rotLimiter.Calculate(
-                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+    const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
@@ -57,5 +51,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
index 9c5d9e4..0aa3086 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumBot/include/Drivetrain.h
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/AnalogGyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
 #include <frc/kinematics/MecanumDriveOdometry.h>
 #include <frc/kinematics/MecanumDriveWheelSpeeds.h>
-#include <wpi/math>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <wpi/numbers>
 
 /**
  * Represents a mecanum drive style drivetrain.
@@ -35,13 +32,13 @@
   static constexpr units::meters_per_second_t kMaxSpeed =
       3.0_mps;  // 3 meters per second
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
-      wpi::math::pi};  // 1/2 rotation per second
+      wpi::numbers::pi};  // 1/2 rotation per second
 
  private:
-  frc::PWMVictorSPX m_frontLeftMotor{1};
-  frc::PWMVictorSPX m_frontRightMotor{2};
-  frc::PWMVictorSPX m_backLeftMotor{3};
-  frc::PWMVictorSPX m_backRightMotor{4};
+  frc::PWMSparkMax m_frontLeftMotor{1};
+  frc::PWMSparkMax m_frontRightMotor{2};
+  frc::PWMSparkMax m_backLeftMotor{3};
+  frc::PWMSparkMax m_backRightMotor{4};
 
   frc::Encoder m_frontLeftEncoder{0, 1};
   frc::Encoder m_frontRightEncoder{2, 3};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp
index 0cf9b96..36912b5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Constants.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Constants.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
index 7746099..3e2c6bf 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/RobotContainer.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
+#include <utility>
+
 #include <frc/controller/PIDController.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/shuffleboard/Shuffleboard.h>
@@ -31,10 +30,9 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.Drive(m_driverController.GetY(frc::GenericHID::kLeftHand),
-                      m_driverController.GetX(frc::GenericHID::kRightHand),
-                      m_driverController.GetX(frc::GenericHID::kLeftHand),
-                      false);
+        m_drive.Drive(m_driverController.GetLeftY(),
+                      m_driverController.GetRightX(),
+                      m_driverController.GetLeftX(), false);
       },
       {&m_drive}));
 }
@@ -43,7 +41,8 @@
   // Configure your button bindings here
 
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
       .WhenPressed(&m_driveHalfSpeed)
       .WhenReleased(&m_driveFullSpeed);
 }
@@ -97,7 +96,7 @@
 
       [this](units::volt_t frontLeft, units::volt_t rearLeft,
              units::volt_t frontRight, units::volt_t rearRight) {
-        m_drive.SetSpeedControllersVolts(frontLeft, rearLeft, frontRight,
+        m_drive.SetMotorControllersVolts(frontLeft, rearLeft, frontRight,
                                          rearRight);
       },
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index ef6564f..4cc4e47 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -51,15 +48,15 @@
 }
 
 void DriveSubsystem::Drive(double xSpeed, double ySpeed, double rot,
-                           bool feildRelative) {
-  if (feildRelative) {
+                           bool fieldRelative) {
+  if (fieldRelative) {
     m_drive.DriveCartesian(ySpeed, xSpeed, rot, -m_gyro.GetAngle());
   } else {
     m_drive.DriveCartesian(ySpeed, xSpeed, rot);
   }
 }
 
-void DriveSubsystem::SetSpeedControllersVolts(units::volt_t frontLeftPower,
+void DriveSubsystem::SetMotorControllersVolts(units::volt_t frontLeftPower,
                                               units::volt_t rearLeftPower,
                                               units::volt_t frontRightPower,
                                               units::volt_t rearRightPower) {
@@ -80,7 +77,9 @@
   return m_frontLeftEncoder;
 }
 
-frc::Encoder& DriveSubsystem::GetRearLeftEncoder() { return m_rearLeftEncoder; }
+frc::Encoder& DriveSubsystem::GetRearLeftEncoder() {
+  return m_rearLeftEncoder;
+}
 
 frc::Encoder& DriveSubsystem::GetFrontRightEncoder() {
   return m_frontRightEncoder;
@@ -106,11 +105,17 @@
   return m_gyro.GetRotation2d().Degrees();
 }
 
-void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+void DriveSubsystem::ZeroHeading() {
+  m_gyro.Reset();
+}
 
-double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
+double DriveSubsystem::GetTurnRate() {
+  return -m_gyro.GetRate();
+}
 
-frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::Pose2d DriveSubsystem::GetPose() {
+  return m_odometry.GetPose();
+}
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
   m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
index c37cb8a..592e948 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/MecanumDriveKinematics.h>
@@ -15,7 +12,7 @@
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #pragma once
 
@@ -37,7 +34,7 @@
 constexpr int kFrontLeftEncoderPorts[]{0, 1};
 constexpr int kRearLeftEncoderPorts[]{2, 3};
 constexpr int kFrontRightEncoderPorts[]{4, 5};
-constexpr int kRearRightEncoderPorts[]{5, 6};
+constexpr int kRearRightEncoderPorts[]{6, 7};
 
 constexpr bool kFrontLeftEncoderReversed = false;
 constexpr bool kRearLeftEncoderReversed = true;
@@ -51,10 +48,11 @@
 extern const frc::MecanumDriveKinematics kDriveKinematics;
 
 constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterMeters = .15;
+constexpr double kWheelDiameterMeters = 0.15;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterMeters * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
@@ -93,5 +91,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
index 3b4f6cc..93485e3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -42,7 +39,7 @@
   // The robot's subsystems
   DriveSubsystem m_drive;
 
-  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(.5); },
+  frc2::InstantCommand m_driveHalfSpeed{[this] { m_drive.SetMaxOutput(0.5); },
                                         {}};
   frc2::InstantCommand m_driveFullSpeed{[this] { m_drive.SetMaxOutput(1); },
                                         {}};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
index 1cd72d2..95b08b6 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/drive/MecanumDrive.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/interfaces/Gyro.h>
 #include <frc/kinematics/MecanumDriveOdometry.h>
 #include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -42,7 +39,7 @@
    * @param fieldRelative Whether the provided x and y speeds are relative to
    *                      the field.
    */
-  void Drive(double xSpeed, double ySpeed, double rot, bool feildRelative);
+  void Drive(double xSpeed, double ySpeed, double rot, bool fieldRelative);
 
   /**
    * Resets the drive encoders to currently read a position of 0.
@@ -85,9 +82,9 @@
   frc::MecanumDriveWheelSpeeds getCurrentWheelSpeeds();
 
   /**
-   * Sets the drive SpeedControllers to a desired voltage.
+   * Sets the drive MotorControllers to a desired voltage.
    */
-  void SetSpeedControllersVolts(units::volt_t frontLeftPower,
+  void SetMotorControllersVolts(units::volt_t frontLeftPower,
                                 units::volt_t rearLeftPower,
                                 units::volt_t frontRightPower,
                                 units::volt_t rearRightPower);
@@ -138,10 +135,10 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_frontLeft;
-  frc::PWMVictorSPX m_rearLeft;
-  frc::PWMVictorSPX m_frontRight;
-  frc::PWMVictorSPX m_rearRight;
+  frc::PWMSparkMax m_frontLeft;
+  frc::PWMSparkMax m_rearLeft;
+  frc::PWMSparkMax m_frontRight;
+  frc::PWMSparkMax m_rearRight;
 
   // The robot's drive
   frc::MecanumDrive m_drive{m_frontLeft, m_rearLeft, m_frontRight, m_rearRight};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
index 8b4c98c..822e9ee 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrive/cpp/Robot.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/MecanumDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a demo program showing how to use Mecanum control with the
@@ -38,10 +35,10 @@
 
   static constexpr int kJoystickChannel = 0;
 
-  frc::PWMVictorSPX m_frontLeft{kFrontLeftChannel};
-  frc::PWMVictorSPX m_rearLeft{kRearLeftChannel};
-  frc::PWMVictorSPX m_frontRight{kFrontRightChannel};
-  frc::PWMVictorSPX m_rearRight{kRearRightChannel};
+  frc::PWMSparkMax m_frontLeft{kFrontLeftChannel};
+  frc::PWMSparkMax m_rearLeft{kRearLeftChannel};
+  frc::PWMSparkMax m_frontRight{kFrontRightChannel};
+  frc::PWMSparkMax m_rearRight{kRearRightChannel};
   frc::MecanumDrive m_robotDrive{m_frontLeft, m_rearLeft, m_frontRight,
                                  m_rearRight};
 
@@ -49,5 +46,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..149f12e
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Drivetrain.h"
+
+#include <frc/Timer.h>
+
+#include "ExampleGlobalMeasurementSensor.h"
+
+frc::MecanumDriveWheelSpeeds Drivetrain::GetCurrentState() const {
+  return {units::meters_per_second_t(m_frontLeftEncoder.GetRate()),
+          units::meters_per_second_t(m_frontRightEncoder.GetRate()),
+          units::meters_per_second_t(m_backLeftEncoder.GetRate()),
+          units::meters_per_second_t(m_backRightEncoder.GetRate())};
+}
+
+void Drivetrain::SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds) {
+  std::function<void(units::meters_per_second_t, const frc::Encoder&,
+                     frc2::PIDController&, frc::PWMSparkMax&)>
+      calcAndSetSpeeds = [&m_feedforward = m_feedforward](
+                             units::meters_per_second_t speed,
+                             const auto& encoder, auto& controller,
+                             auto& motor) {
+        auto feedforward = m_feedforward.Calculate(speed);
+        double output = controller.Calculate(encoder.GetRate(), speed.value());
+        motor.SetVoltage(units::volt_t{output} + feedforward);
+      };
+
+  calcAndSetSpeeds(wheelSpeeds.frontLeft, m_frontLeftEncoder,
+                   m_frontLeftPIDController, m_frontLeftMotor);
+  calcAndSetSpeeds(wheelSpeeds.frontRight, m_frontRightEncoder,
+                   m_frontRightPIDController, m_frontRightMotor);
+  calcAndSetSpeeds(wheelSpeeds.rearLeft, m_backLeftEncoder,
+                   m_backLeftPIDController, m_backLeftMotor);
+  calcAndSetSpeeds(wheelSpeeds.rearRight, m_backRightEncoder,
+                   m_backRightPIDController, m_backRightMotor);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::meters_per_second_t ySpeed,
+                       units::radians_per_second_t rot, bool fieldRelative) {
+  auto wheelSpeeds = m_kinematics.ToWheelSpeeds(
+      fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
+                    : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+  wheelSpeeds.Normalize(kMaxSpeed);
+  SetSpeeds(wheelSpeeds);
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(), GetCurrentState());
+
+  // Also apply vision measurements. We use 0.3 seconds in the past as an
+  // example -- on a real robot, this must be calculated based either on latency
+  // or timestamps.
+  m_poseEstimator.AddVisionMeasurement(
+      ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
+          m_poseEstimator.GetEstimatedPosition()),
+      frc::Timer::GetFPGATimestamp() - 0.3_s);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp
new file mode 100644
index 0000000..4943184
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/cpp/Robot.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    DriveWithJoystick(false);
+    m_mecanum.UpdateOdometry();
+  }
+
+  void TeleopPeriodic() override { DriveWithJoystick(true); }
+
+ private:
+  frc::XboxController m_controller{0};
+  Drivetrain m_mecanum;
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  void DriveWithJoystick(bool fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_mecanum.Drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
new file mode 100644
index 0000000..d084fa5
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/Drivetrain.h
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/estimator/MecanumDrivePoseEstimator.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/MecanumDriveKinematics.h>
+#include <frc/kinematics/MecanumDriveOdometry.h>
+#include <frc/kinematics/MecanumDriveWheelSpeeds.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <wpi/numbers>
+
+/**
+ * Represents a mecanum drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() { m_gyro.Reset(); }
+
+  frc::MecanumDriveWheelSpeeds GetCurrentState() const;
+  void SetSpeeds(const frc::MecanumDriveWheelSpeeds& wheelSpeeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+             bool fieldRelative);
+  void UpdateOdometry();
+
+  static constexpr auto kMaxSpeed = 3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::numbers::pi};  // 1/2 rotation per second
+
+ private:
+  frc::PWMSparkMax m_frontLeftMotor{1};
+  frc::PWMSparkMax m_frontRightMotor{2};
+  frc::PWMSparkMax m_backLeftMotor{3};
+  frc::PWMSparkMax m_backRightMotor{4};
+
+  frc::Encoder m_frontLeftEncoder{0, 1};
+  frc::Encoder m_frontRightEncoder{2, 3};
+  frc::Encoder m_backLeftEncoder{4, 5};
+  frc::Encoder m_backRightEncoder{6, 7};
+
+  frc::Translation2d m_frontLeftLocation{0.381_m, 0.381_m};
+  frc::Translation2d m_frontRightLocation{0.381_m, -0.381_m};
+  frc::Translation2d m_backLeftLocation{-0.381_m, 0.381_m};
+  frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
+
+  frc2::PIDController m_frontLeftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_frontRightPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_backLeftPIDController{1.0, 0.0, 0.0};
+  frc2::PIDController m_backRightPIDController{1.0, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::MecanumDriveKinematics m_kinematics{
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
+      m_backRightLocation};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::MecanumDrivePoseEstimator m_poseEstimator{
+      frc::Rotation2d(), frc::Pose2d(), m_kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h
new file mode 100644
index 0000000..a4caff4
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MecanumDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/geometry/Pose2d.h>
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer
+ * vision solution.
+ */
+class ExampleGlobalMeasurementSensor {
+ public:
+  static frc::Pose2d GetEstimatedGlobalPose(
+      const frc::Pose2d& estimatedRobotPose) {
+    auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)),
+                       estimatedRobotPose.Y() + units::meter_t(randVec(1)),
+                       estimatedRobotPose.Rotation() +
+                           frc::Rotation2d(units::radian_t(randVec(3))));
+  }
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp
new file mode 100644
index 0000000..551dc78
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Mechanism2d/cpp/Robot.cpp
@@ -0,0 +1,76 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/AnalogPotentiometer.h>
+#include <frc/Encoder.h>
+#include <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/SimHooks.h>
+#include <frc/smartdashboard/Mechanism2d.h>
+#include <frc/smartdashboard/MechanismLigament2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/util/Color.h>
+#include <frc/util/Color8Bit.h>
+#include <units/angle.h>
+
+/**
+ * This sample program shows how to use Mechanism2d - a visual representation of
+ * arms, elevators, and other mechanisms on dashboards; driven by a node-based
+ * API.
+ *
+ * <p>Ligaments are based on other ligaments or roots, and roots are contained
+ * in the base Mechanism2d object. Use pointers for nodes, and beware not to let
+ * the container out of scope - the appended nodes will be recursively
+ * destructed!
+ */
+class Robot : public frc::TimedRobot {
+  static constexpr double kMetersPerPulse = 0.01;
+  static constexpr double kElevatorMinimumLength = 0.5;
+
+ public:
+  void RobotInit() override {
+    m_elevatorEncoder.SetDistancePerPulse(kMetersPerPulse);
+
+    // publish to dashboard
+    frc::SmartDashboard::PutData("Mech2d", &m_mech);
+  }
+
+  void RobotPeriodic() override {
+    // update the dashboard mechanism's state
+    m_elevator->SetLength(kElevatorMinimumLength +
+                          m_elevatorEncoder.GetDistance());
+    m_wrist->SetAngle(units::degree_t(m_wristPotentiometer.Get()));
+  }
+
+  void TeleopPeriodic() override {
+    m_elevatorMotor.Set(m_joystick.GetRawAxis(0));
+    m_wristMotor.Set(m_joystick.GetRawAxis(1));
+  }
+
+ private:
+  frc::PWMSparkMax m_elevatorMotor{0};
+  frc::PWMSparkMax m_wristMotor{1};
+  frc::Encoder m_elevatorEncoder{0, 1};
+  frc::AnalogPotentiometer m_wristPotentiometer{1, 90};
+  frc::Joystick m_joystick{0};
+
+  // the main mechanism object
+  frc::Mechanism2d m_mech{3, 3};
+  // the mechanism root node
+  frc::MechanismRoot2d* m_root = m_mech.GetRoot("climber", 2, 0);
+  // MechanismLigament2d objects represent each "section"/"stage" of the
+  // mechanism, and are based off the root node or another ligament object
+  frc::MechanismLigament2d* m_elevator =
+      m_root->Append<frc::MechanismLigament2d>("elevator", 1, 90_deg);
+  frc::MechanismLigament2d* m_wrist =
+      m_elevator->Append<frc::MechanismLigament2d>(
+          "wrist", 0.5, 90_deg, 6, frc::Color8Bit{frc::Color::kPurple});
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp
index 2217ec1..0092ec3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControl/cpp/Robot.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This sample program shows how to control a motor using a joystick. In the
@@ -23,9 +20,11 @@
 
  private:
   frc::Joystick m_stick{0};
-  frc::PWMVictorSPX m_motor{0};
+  frc::PWMSparkMax m_motor{0};
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
index 8e0a14a..7d0e9ba 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/MotorControlEncoder/cpp/Robot.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/smartdashboard/SmartDashboard.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This sample program shows how to control a motor using a joystick. In the
@@ -38,15 +35,17 @@
   void RobotInit() override {
     // Use SetDistancePerPulse to set the multiplier for GetDistance
     // This is set up assuming a 6 inch wheel with a 360 CPR encoder.
-    m_encoder.SetDistancePerPulse((wpi::math::pi * 6) / 360.0);
+    m_encoder.SetDistancePerPulse((wpi::numbers::pi * 6) / 360.0);
   }
 
  private:
   frc::Joystick m_stick{0};
-  frc::PWMVictorSPX m_motor{0};
+  frc::PWMSparkMax m_motor{0};
   frc::Encoder m_encoder{0, 1};
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/OI.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/OI.cpp
deleted file mode 100644
index c5fcb15..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/OI.cpp
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "OI.h"
-
-#include <frc/smartdashboard/SmartDashboard.h>
-
-#include "commands/Collect.h"
-#include "commands/DriveForward.h"
-#include "commands/LowGoal.h"
-#include "commands/SetCollectionSpeed.h"
-#include "commands/SetPivotSetpoint.h"
-#include "commands/Shoot.h"
-#include "subsystems/Collector.h"
-#include "subsystems/Pivot.h"
-
-OI::OI() {
-  m_r1.WhenPressed(new LowGoal());
-  m_r2.WhenPressed(new Collect());
-
-  m_l1.WhenPressed(new SetPivotSetpoint(Pivot::kShoot));
-  m_l2.WhenPressed(new SetPivotSetpoint(Pivot::kShootNear));
-
-  m_sticks.WhenActive(new Shoot());
-
-  // SmartDashboard Buttons
-  frc::SmartDashboard::PutData("Drive Forward", new DriveForward(2.25));
-  frc::SmartDashboard::PutData("Drive Backward", new DriveForward(-2.25));
-  frc::SmartDashboard::PutData("Start Rollers",
-                               new SetCollectionSpeed(Collector::kForward));
-  frc::SmartDashboard::PutData("Stop Rollers",
-                               new SetCollectionSpeed(Collector::kStop));
-  frc::SmartDashboard::PutData("Reverse Rollers",
-                               new SetCollectionSpeed(Collector::kReverse));
-}
-
-frc::Joystick& OI::GetJoystick() { return m_joystick; }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp
deleted file mode 100644
index 9da27f3..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/Robot.cpp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "Robot.h"
-
-#include <iostream>
-
-#include <frc/commands/Scheduler.h>
-#include <frc/livewindow/LiveWindow.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-
-DriveTrain Robot::drivetrain;
-Pivot Robot::pivot;
-Collector Robot::collector;
-Shooter Robot::shooter;
-Pneumatics Robot::pneumatics;
-OI Robot::oi;
-
-void Robot::RobotInit() {
-  // Show what command your subsystem is running on the SmartDashboard
-  frc::SmartDashboard::PutData(&drivetrain);
-  frc::SmartDashboard::PutData(&pivot);
-  frc::SmartDashboard::PutData(&collector);
-  frc::SmartDashboard::PutData(&shooter);
-  frc::SmartDashboard::PutData(&pneumatics);
-
-  // instantiate the command used for the autonomous period
-  m_autoChooser.SetDefaultOption("Drive and Shoot", &m_driveAndShootAuto);
-  m_autoChooser.AddOption("Drive Forward", &m_driveForwardAuto);
-  frc::SmartDashboard::PutData("Auto Mode", &m_autoChooser);
-
-  pneumatics.Start();  // Pressurize the pneumatics.
-}
-
-void Robot::AutonomousInit() {
-  m_autonomousCommand = m_autoChooser.GetSelected();
-  m_autonomousCommand->Start();
-}
-
-void Robot::AutonomousPeriodic() {
-  frc::Scheduler::GetInstance()->Run();
-  Log();
-}
-
-void Robot::TeleopInit() {
-  // This makes sure that the autonomous stops running when
-  // teleop starts running. If you want the autonomous to
-  // continue until interrupted by another command, remove
-  // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
-    m_autonomousCommand->Cancel();
-  }
-  std::cout << "Starting Teleop" << std::endl;
-}
-
-void Robot::TeleopPeriodic() {
-  frc::Scheduler::GetInstance()->Run();
-  Log();
-}
-
-void Robot::TestPeriodic() {}
-
-void Robot::DisabledInit() { shooter.Unlatch(); }
-
-void Robot::DisabledPeriodic() { Log(); }
-
-/**
- * Log interesting values to the SmartDashboard.
- */
-void Robot::Log() {
-  Robot::pneumatics.WritePressure();
-  frc::SmartDashboard::PutNumber("Pivot Pot Value", pivot.GetAngle());
-  frc::SmartDashboard::PutNumber("Left Distance",
-                                 drivetrain.GetLeftEncoder().GetDistance());
-  frc::SmartDashboard::PutNumber("Right Distance",
-                                 drivetrain.GetRightEncoder().GetDistance());
-}
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CheckForHotGoal.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CheckForHotGoal.cpp
deleted file mode 100644
index c5d80c0..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CheckForHotGoal.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/CheckForHotGoal.h"
-
-#include "Robot.h"
-
-CheckForHotGoal::CheckForHotGoal(double time) { SetTimeout(time); }
-
-// Make this return true when this Command no longer needs to run execute()
-bool CheckForHotGoal::IsFinished() {
-  return IsTimedOut() || Robot::shooter.GoalIsHot();
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CloseClaw.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CloseClaw.cpp
deleted file mode 100644
index 8db41f3..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/CloseClaw.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/CloseClaw.h"
-
-#include "Robot.h"
-
-CloseClaw::CloseClaw() { Requires(&Robot::collector); }
-
-// Called just before this Command runs the first time
-void CloseClaw::Initialize() { Robot::collector.Close(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Collect.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Collect.cpp
deleted file mode 100644
index b51c2df..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Collect.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/Collect.h"
-
-#include "Robot.h"
-#include "commands/CloseClaw.h"
-#include "commands/SetCollectionSpeed.h"
-#include "commands/SetPivotSetpoint.h"
-#include "commands/WaitForBall.h"
-
-Collect::Collect() {
-  AddSequential(new SetCollectionSpeed(Collector::kForward));
-  AddParallel(new CloseClaw());
-  AddSequential(new SetPivotSetpoint(Pivot::kCollect));
-  AddSequential(new WaitForBall());
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveAndShootAutonomous.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveAndShootAutonomous.cpp
deleted file mode 100644
index 3486292..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveAndShootAutonomous.cpp
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/DriveAndShootAutonomous.h"
-
-#include "Robot.h"
-#include "commands/CheckForHotGoal.h"
-#include "commands/CloseClaw.h"
-#include "commands/DriveForward.h"
-#include "commands/SetPivotSetpoint.h"
-#include "commands/Shoot.h"
-#include "commands/WaitForPressure.h"
-
-DriveAndShootAutonomous::DriveAndShootAutonomous() {
-  AddSequential(new CloseClaw());
-  AddSequential(new WaitForPressure(), 2);
-#ifndef SIMULATION
-  // NOTE: Simulation doesn't currently have the concept of hot.
-  AddSequential(new CheckForHotGoal(2));
-#endif
-  AddSequential(new SetPivotSetpoint(45));
-  AddSequential(new DriveForward(8, 0.3));
-  AddSequential(new Shoot());
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveForward.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveForward.cpp
deleted file mode 100644
index 1de5846..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveForward.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/DriveForward.h"
-
-#include <cmath>
-
-#include "Robot.h"
-
-void DriveForward::init(double dist, double maxSpeed) {
-  Requires(&Robot::drivetrain);
-  m_distance = dist;
-  m_driveForwardSpeed = maxSpeed;
-}
-
-DriveForward::DriveForward() { init(10, 0.5); }
-
-DriveForward::DriveForward(double dist) { init(dist, 0.5); }
-
-DriveForward::DriveForward(double dist, double maxSpeed) {
-  init(dist, maxSpeed);
-}
-
-// Called just before this Command runs the first time
-void DriveForward::Initialize() {
-  Robot::drivetrain.GetRightEncoder().Reset();
-  SetTimeout(2);
-}
-
-// Called repeatedly when this Command is scheduled to run
-void DriveForward::Execute() {
-  m_error = (m_distance - Robot::drivetrain.GetRightEncoder().GetDistance());
-  if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
-    Robot::drivetrain.TankDrive(m_driveForwardSpeed, m_driveForwardSpeed);
-  } else {
-    Robot::drivetrain.TankDrive(m_driveForwardSpeed * kP * m_error,
-                                m_driveForwardSpeed * kP * m_error);
-  }
-}
-
-// Make this return true when this Command no longer needs to run execute()
-bool DriveForward::IsFinished() {
-  return (std::fabs(m_error) <= kTolerance) || IsTimedOut();
-}
-
-// Called once after isFinished returns true
-void DriveForward::End() { Robot::drivetrain.Stop(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveWithJoystick.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveWithJoystick.cpp
deleted file mode 100644
index 7704d70..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/DriveWithJoystick.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/DriveWithJoystick.h"
-
-#include "Robot.h"
-
-DriveWithJoystick::DriveWithJoystick() { Requires(&Robot::drivetrain); }
-
-// Called repeatedly when this Command is scheduled to run
-void DriveWithJoystick::Execute() {
-  auto& joystick = Robot::oi.GetJoystick();
-  Robot::drivetrain.TankDrive(joystick.GetY(), joystick.GetRawAxis(4));
-}
-
-// Make this return true when this Command no longer needs to run execute()
-bool DriveWithJoystick::IsFinished() { return false; }
-
-// Called once after isFinished returns true
-void DriveWithJoystick::End() { Robot::drivetrain.Stop(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/ExtendShooter.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/ExtendShooter.cpp
deleted file mode 100644
index 53494c9..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/ExtendShooter.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/ExtendShooter.h"
-
-#include "Robot.h"
-
-ExtendShooter::ExtendShooter() : frc::TimedCommand(1.0) {
-  Requires(&Robot::shooter);
-}
-
-// Called just before this Command runs the first time
-void ExtendShooter::Initialize() { Robot::shooter.ExtendBoth(); }
-
-// Called once after isFinished returns true
-void ExtendShooter::End() { Robot::shooter.RetractBoth(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/LowGoal.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/LowGoal.cpp
deleted file mode 100644
index f4ed788..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/LowGoal.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/LowGoal.h"
-
-#include "Robot.h"
-#include "commands/ExtendShooter.h"
-#include "commands/SetCollectionSpeed.h"
-#include "commands/SetPivotSetpoint.h"
-
-LowGoal::LowGoal() {
-  AddSequential(new SetPivotSetpoint(Pivot::kLowGoal));
-  AddSequential(new SetCollectionSpeed(Collector::kReverse));
-  AddSequential(new ExtendShooter());
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/OpenClaw.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/OpenClaw.cpp
deleted file mode 100644
index 4e4a9c8..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/OpenClaw.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/OpenClaw.h"
-
-#include "Robot.h"
-
-OpenClaw::OpenClaw() { Requires(&Robot::collector); }
-
-// Called just before this Command runs the first time
-void OpenClaw::Initialize() { Robot::collector.Open(); }
-
-// Make this return true when this Command no longer needs to run execute()
-bool OpenClaw::IsFinished() { return Robot::collector.IsOpen(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetCollectionSpeed.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetCollectionSpeed.cpp
deleted file mode 100644
index 2011a02..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetCollectionSpeed.cpp
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/SetCollectionSpeed.h"
-
-#include "Robot.h"
-
-SetCollectionSpeed::SetCollectionSpeed(double speed) {
-  Requires(&Robot::collector);
-  m_speed = speed;
-}
-
-// Called just before this Command runs the first time
-void SetCollectionSpeed::Initialize() { Robot::collector.SetSpeed(m_speed); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetPivotSetpoint.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetPivotSetpoint.cpp
deleted file mode 100644
index f9929de..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/SetPivotSetpoint.cpp
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/SetPivotSetpoint.h"
-
-#include "Robot.h"
-
-SetPivotSetpoint::SetPivotSetpoint(double setpoint) {
-  m_setpoint = setpoint;
-  Requires(&Robot::pivot);
-}
-
-// Called just before this Command runs the first time
-void SetPivotSetpoint::Initialize() {
-  Robot::pivot.Enable();
-  Robot::pivot.SetSetpoint(m_setpoint);
-}
-
-// Make this return true when this Command no longer needs to run execute()
-bool SetPivotSetpoint::IsFinished() { return Robot::pivot.OnTarget(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Shoot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Shoot.cpp
deleted file mode 100644
index 836ccd2..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/Shoot.cpp
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/Shoot.h"
-
-#include "Robot.h"
-#include "commands/ExtendShooter.h"
-#include "commands/OpenClaw.h"
-#include "commands/SetCollectionSpeed.h"
-#include "commands/WaitForPressure.h"
-
-Shoot::Shoot() {
-  AddSequential(new WaitForPressure());
-  AddSequential(new SetCollectionSpeed(Collector::kStop));
-  AddSequential(new OpenClaw());
-  AddSequential(new ExtendShooter());
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForBall.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForBall.cpp
deleted file mode 100644
index daf9665..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForBall.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/WaitForBall.h"
-
-#include "Robot.h"
-
-WaitForBall::WaitForBall() { Requires(&Robot::collector); }
-
-// Make this return true when this Command no longer needs to run execute()
-bool WaitForBall::IsFinished() { return Robot::collector.HasBall(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForPressure.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForPressure.cpp
deleted file mode 100644
index 00cc454..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/commands/WaitForPressure.cpp
+++ /dev/null
@@ -1,15 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "commands/WaitForPressure.h"
-
-#include "Robot.h"
-
-WaitForPressure::WaitForPressure() { Requires(&Robot::pneumatics); }
-
-// Make this return true when this Command no longer needs to run execute()
-bool WaitForPressure::IsFinished() { return Robot::pneumatics.IsPressurized(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Collector.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Collector.cpp
deleted file mode 100644
index 8f345ce..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Collector.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "subsystems/Collector.h"
-
-#include <frc/livewindow/LiveWindow.h>
-
-Collector::Collector() : frc::Subsystem("Collector") {
-  // Put everything to the LiveWindow for testing.
-  AddChild("Roller Motor", m_rollerMotor);
-  AddChild("Ball Detector", m_ballDetector);
-  AddChild("Claw Open Detector", m_openDetector);
-  AddChild("Piston", m_piston);
-}
-
-bool Collector::HasBall() {
-  return m_ballDetector.Get();  // TODO: prepend ! to reflect real robot
-}
-
-void Collector::SetSpeed(double speed) { m_rollerMotor.Set(-speed); }
-
-void Collector::Stop() { m_rollerMotor.Set(0); }
-
-bool Collector::IsOpen() {
-  return m_openDetector.Get();  // TODO: prepend ! to reflect real robot
-}
-
-void Collector::Open() { m_piston.Set(true); }
-
-void Collector::Close() { m_piston.Set(false); }
-
-void Collector::InitDefaultCommand() {}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp
deleted file mode 100644
index 53cb625..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/DriveTrain.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "subsystems/DriveTrain.h"
-
-#include <cmath>
-
-#include <frc/Joystick.h>
-
-#include "commands/DriveWithJoystick.h"
-
-DriveTrain::DriveTrain() : frc::Subsystem("DriveTrain") {
-  // AddChild("Front Left CIM", m_frontLeftCIM);
-  // AddChild("Front Right CIM", m_frontRightCIM);
-  // AddChild("Back Left CIM", m_backLeftCIM);
-  // AddChild("Back Right CIM", m_backRightCIM);
-
-  // Configure the DifferentialDrive to reflect the fact that all our
-  // motors are wired backwards and our drivers sensitivity preferences.
-  m_robotDrive.SetSafetyEnabled(false);
-  m_robotDrive.SetExpiration(0.1);
-  m_robotDrive.SetMaxOutput(1.0);
-  m_leftCIMs.SetInverted(true);
-  m_rightCIMs.SetInverted(true);
-
-  // Configure encoders
-  m_rightEncoder.SetPIDSourceType(frc::PIDSourceType::kDisplacement);
-  m_leftEncoder.SetPIDSourceType(frc::PIDSourceType::kDisplacement);
-
-#ifndef SIMULATION
-  // Converts to feet
-  m_rightEncoder.SetDistancePerPulse(0.0785398);
-  m_leftEncoder.SetDistancePerPulse(0.0785398);
-#else
-  // Convert to feet 4in diameter wheels with 360 tick simulated encoders
-  m_rightEncoder.SetDistancePerPulse((4.0 /*in*/ * M_PI) /
-                                     (360.0 * 12.0 /*in/ft*/));
-  m_leftEncoder.SetDistancePerPulse((4.0 /*in*/ * M_PI) /
-                                    (360.0 * 12.0 /*in/ft*/));
-#endif
-
-  AddChild("Right Encoder", m_rightEncoder);
-  AddChild("Left Encoder", m_leftEncoder);
-
-// Configure gyro
-#ifndef SIMULATION
-  m_gyro.SetSensitivity(0.007);  // TODO: Handle more gracefully?
-#endif
-  AddChild("Gyro", m_gyro);
-}
-
-void DriveTrain::InitDefaultCommand() {
-  SetDefaultCommand(new DriveWithJoystick());
-}
-
-void DriveTrain::TankDrive(double leftAxis, double rightAxis) {
-  m_robotDrive.TankDrive(leftAxis, rightAxis);
-}
-
-void DriveTrain::Stop() { m_robotDrive.TankDrive(0.0, 0.0); }
-
-frc::Encoder& DriveTrain::GetLeftEncoder() { return m_leftEncoder; }
-
-frc::Encoder& DriveTrain::GetRightEncoder() { return m_rightEncoder; }
-
-double DriveTrain::GetAngle() { return m_gyro.GetAngle(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pivot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pivot.cpp
deleted file mode 100644
index 861d583..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pivot.cpp
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "subsystems/Pivot.h"
-
-Pivot::Pivot() : frc::PIDSubsystem("Pivot", 7.0, 0.0, 8.0) {
-  SetAbsoluteTolerance(0.005);
-  GetPIDController()->SetContinuous(false);
-#ifdef SIMULATION
-  // PID is different in simulation.
-  GetPIDController()->SetPID(0.5, 0.001, 2);
-  SetAbsoluteTolerance(5);
-#endif
-
-  // Put everything to the LiveWindow for testing.
-  AddChild("Upper Limit Switch", m_upperLimitSwitch);
-  AddChild("Lower Limit Switch", m_lowerLimitSwitch);
-  AddChild("Pot", m_pot);
-  AddChild("Motor", m_motor);
-}
-
-void InitDefaultCommand() {}
-
-double Pivot::ReturnPIDInput() { return m_pot.Get(); }
-
-void Pivot::UsePIDOutput(double output) { m_motor.PIDWrite(output); }
-
-bool Pivot::IsAtUpperLimit() {
-  return m_upperLimitSwitch.Get();  // TODO: inverted from real robot
-                                    // (prefix with !)
-}
-
-bool Pivot::IsAtLowerLimit() {
-  return m_lowerLimitSwitch.Get();  // TODO: inverted from real robot
-                                    // (prefix with !)
-}
-
-double Pivot::GetAngle() { return m_pot.Get(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp
deleted file mode 100644
index dd1a824..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Pneumatics.cpp
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "subsystems/Pneumatics.h"
-
-#include <frc/smartdashboard/SmartDashboard.h>
-
-Pneumatics::Pneumatics() : frc::Subsystem("Pneumatics") {
-  AddChild("Pressure Sensor", m_pressureSensor);
-}
-
-void Pneumatics::InitDefaultCommand() {
-  // No default command
-}
-
-void Pneumatics::Start() {
-#ifndef SIMULATION
-  m_compressor.Start();
-#endif
-}
-
-bool Pneumatics::IsPressurized() {
-#ifndef SIMULATION
-  return kMaxPressure <= m_pressureSensor.GetVoltage();
-#else
-  return true;  // NOTE: Simulation always has full pressure
-#endif
-}
-
-void Pneumatics::WritePressure() {
-  frc::SmartDashboard::PutNumber("Pressure", m_pressureSensor.GetVoltage());
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Shooter.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Shooter.cpp
deleted file mode 100644
index 80d7ac3..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/subsystems/Shooter.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "subsystems/Shooter.h"
-
-Shooter::Shooter() : Subsystem("Shooter") {
-  // Put everything to the LiveWindow for testing.
-  AddChild("Hot Goal Sensor", m_hotGoalSensor);
-  AddChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront);
-  AddChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack);
-  AddChild("Latch Piston", m_latchPiston);
-}
-
-void Shooter::InitDefaultCommand() {
-  // Set the default command for a subsystem here.
-  // SetDefaultCommand(new MySpecialCommand());
-}
-
-void Shooter::ExtendBoth() {
-  m_piston1.Set(frc::DoubleSolenoid::kForward);
-  m_piston2.Set(frc::DoubleSolenoid::kForward);
-}
-
-void Shooter::RetractBoth() {
-  m_piston1.Set(frc::DoubleSolenoid::kReverse);
-  m_piston2.Set(frc::DoubleSolenoid::kReverse);
-}
-
-void Shooter::Extend1() { m_piston1.Set(frc::DoubleSolenoid::kForward); }
-
-void Shooter::Retract1() { m_piston1.Set(frc::DoubleSolenoid::kReverse); }
-
-void Shooter::Extend2() { m_piston2.Set(frc::DoubleSolenoid::kReverse); }
-
-void Shooter::Retract2() { m_piston2.Set(frc::DoubleSolenoid::kForward); }
-
-void Shooter::Off1() { m_piston1.Set(frc::DoubleSolenoid::kOff); }
-
-void Shooter::Off2() { m_piston2.Set(frc::DoubleSolenoid::kOff); }
-
-void Shooter::Unlatch() { m_latchPiston.Set(true); }
-
-void Shooter::Latch() { m_latchPiston.Set(false); }
-
-void Shooter::ToggleLatchPosition() { m_latchPiston.Set(!m_latchPiston.Get()); }
-
-bool Shooter::Piston1IsExtended() { return !m_piston1ReedSwitchFront.Get(); }
-
-bool Shooter::Piston1IsRetracted() { return !m_piston1ReedSwitchBack.Get(); }
-
-void Shooter::OffBoth() {
-  m_piston1.Set(frc::DoubleSolenoid::kOff);
-  m_piston2.Set(frc::DoubleSolenoid::kOff);
-}
-
-bool Shooter::GoalIsHot() { return m_hotGoalSensor.Get(); }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/triggers/DoubleButton.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/triggers/DoubleButton.cpp
deleted file mode 100644
index f2f7f64..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/cpp/triggers/DoubleButton.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "triggers/DoubleButton.h"
-
-#include <frc/Joystick.h>
-
-DoubleButton::DoubleButton(frc::Joystick* joy, int button1, int button2)
-    : m_joy(*joy) {
-  m_button1 = button1;
-  m_button2 = button2;
-}
-
-bool DoubleButton::Get() {
-  return m_joy.GetRawButton(m_button1) && m_joy.GetRawButton(m_button2);
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/OI.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/OI.h
deleted file mode 100644
index 2df4395..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/OI.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/Joystick.h>
-#include <frc/buttons/JoystickButton.h>
-
-#include "triggers/DoubleButton.h"
-
-class OI {
- public:
-  OI();
-  frc::Joystick& GetJoystick();
-
- private:
-  frc::Joystick m_joystick{0};
-
-  frc::JoystickButton m_l1{&m_joystick, 11};
-  frc::JoystickButton m_l2{&m_joystick, 9};
-  frc::JoystickButton m_r1{&m_joystick, 12};
-  frc::JoystickButton m_r2{&m_joystick, 10};
-
-  DoubleButton m_sticks{&m_joystick, 2, 3};
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h
deleted file mode 100644
index b24085d..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/Robot.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/TimedRobot.h>
-#include <frc/commands/Command.h>
-#include <frc/smartdashboard/SendableChooser.h>
-
-#include "OI.h"
-#include "commands/DriveAndShootAutonomous.h"
-#include "commands/DriveForward.h"
-#include "subsystems/Collector.h"
-#include "subsystems/DriveTrain.h"
-#include "subsystems/Pivot.h"
-#include "subsystems/Pneumatics.h"
-#include "subsystems/Shooter.h"
-
-class Robot : public frc::TimedRobot {
- public:
-  static DriveTrain drivetrain;
-  static Pivot pivot;
-  static Collector collector;
-  static Shooter shooter;
-  static Pneumatics pneumatics;
-  static OI oi;
-
- private:
-  frc::Command* m_autonomousCommand = nullptr;
-  DriveAndShootAutonomous m_driveAndShootAuto;
-  DriveForward m_driveForwardAuto;
-  frc::SendableChooser<frc::Command*> m_autoChooser;
-
-  void RobotInit() override;
-  void AutonomousInit() override;
-  void AutonomousPeriodic() override;
-  void TeleopInit() override;
-  void TeleopPeriodic() override;
-  void TestPeriodic() override;
-  void DisabledInit() override;
-  void DisabledPeriodic() override;
-
-  void Log();
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CheckForHotGoal.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CheckForHotGoal.h
deleted file mode 100644
index 9f42461..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CheckForHotGoal.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * This command looks for the hot goal and waits until it's detected or timed
- * out. The timeout is because it's better to shoot and get some autonomous
- * points than get none. When called sequentially, this command will block until
- * the hot goal is detected or until it is timed out.
- */
-class CheckForHotGoal : public frc::Command {
- public:
-  explicit CheckForHotGoal(double time);
-  bool IsFinished() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CloseClaw.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CloseClaw.h
deleted file mode 100644
index 60f12a9..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/CloseClaw.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/InstantCommand.h>
-
-/**
- * Close the claw.
- *
- * NOTE: It doesn't wait for the claw to close since there is no sensor to
- * detect that.
- */
-class CloseClaw : public frc::InstantCommand {
- public:
-  CloseClaw();
-  void Initialize() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Collect.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Collect.h
deleted file mode 100644
index c2965c2..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Collect.h
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/CommandGroup.h>
-
-/**
- * Get the robot set to collect balls.
- */
-class Collect : public frc::CommandGroup {
- public:
-  Collect();
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveAndShootAutonomous.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveAndShootAutonomous.h
deleted file mode 100644
index e143738..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveAndShootAutonomous.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/CommandGroup.h>
-
-/**
- * Drive over the line and then shoot the ball. If the hot goal is not detected,
- * it will wait briefly.
- */
-class DriveAndShootAutonomous : public frc::CommandGroup {
- public:
-  DriveAndShootAutonomous();
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveForward.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveForward.h
deleted file mode 100644
index a89134e..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveForward.h
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * This command drives the robot over a given distance with simple proportional
- * control This command will drive a given distance limiting to a maximum speed.
- */
-class DriveForward : public frc::Command {
- public:
-  DriveForward();
-  explicit DriveForward(double dist);
-  DriveForward(double dist, double maxSpeed);
-  void Initialize() override;
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-
- private:
-  double m_driveForwardSpeed;
-  double m_distance;
-  double m_error = 0;
-  static constexpr double kTolerance = 0.1;
-  static constexpr double kP = -1.0 / 5.0;
-
-  void init(double dist, double maxSpeed);
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveWithJoystick.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveWithJoystick.h
deleted file mode 100644
index e6d9b0e..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/DriveWithJoystick.h
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * This command allows PS3 joystick to drive the robot. It is always running
- * except when interrupted by another command.
- */
-class DriveWithJoystick : public frc::Command {
- public:
-  DriveWithJoystick();
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/ExtendShooter.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/ExtendShooter.h
deleted file mode 100644
index 634860e..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/ExtendShooter.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/TimedCommand.h>
-
-/**
- * Extend the shooter and then retract it after a second.
- */
-class ExtendShooter : public frc::TimedCommand {
- public:
-  ExtendShooter();
-  void Initialize() override;
-  void End() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/LowGoal.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/LowGoal.h
deleted file mode 100644
index 62237fe..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/LowGoal.h
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/CommandGroup.h>
-
-/**
- * Spit the ball out into the low goal assuming that the robot is in front of
- * it.
- */
-class LowGoal : public frc::CommandGroup {
- public:
-  LowGoal();
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/OpenClaw.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/OpenClaw.h
deleted file mode 100644
index 0d78877..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/OpenClaw.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * Opens the claw
- */
-class OpenClaw : public frc::Command {
- public:
-  OpenClaw();
-  void Initialize() override;
-  bool IsFinished() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetCollectionSpeed.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetCollectionSpeed.h
deleted file mode 100644
index 24bb4e3..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetCollectionSpeed.h
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/InstantCommand.h>
-
-/**
- * This command sets the collector rollers spinning at the given speed. Since
- * there is no sensor for detecting speed, it finishes immediately. As a result,
- * the spinners may still be adjusting their speed.
- */
-class SetCollectionSpeed : public frc::InstantCommand {
- public:
-  explicit SetCollectionSpeed(double speed);
-  void Initialize() override;
-
- private:
-  double m_speed;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetPivotSetpoint.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetPivotSetpoint.h
deleted file mode 100644
index 8f8f615..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/SetPivotSetpoint.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * Moves the  pivot to a given angle. This command finishes when it is within
- * the tolerance, but leaves the PID loop running to maintain the position.
- * Other commands using the pivot should make sure they disable PID!
- */
-class SetPivotSetpoint : public frc::Command {
- public:
-  explicit SetPivotSetpoint(double setpoint);
-  void Initialize() override;
-  bool IsFinished() override;
-
- private:
-  double m_setpoint;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Shoot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Shoot.h
deleted file mode 100644
index 242221c..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/Shoot.h
+++ /dev/null
@@ -1,18 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/CommandGroup.h>
-
-/**
- * Shoot the ball at the current angle.
- */
-class Shoot : public frc::CommandGroup {
- public:
-  Shoot();
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForBall.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForBall.h
deleted file mode 100644
index b6d8d99..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForBall.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * Wait until the collector senses that it has the ball. This command does
- * nothing and is intended to be used in command groups to wait for this
- * condition.
- */
-class WaitForBall : public frc::Command {
- public:
-  WaitForBall();
-  bool IsFinished() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForPressure.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForPressure.h
deleted file mode 100644
index 49e1f17..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/commands/WaitForPressure.h
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-/**
- * Wait until the pneumatics are fully pressurized. This command does nothing
- * and is intended to be used in command groups to wait for this condition.
- */
-class WaitForPressure : public frc::Command {
- public:
-  WaitForPressure();
-  bool IsFinished() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h
deleted file mode 100644
index 3e237fa..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Collector.h
+++ /dev/null
@@ -1,76 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/DigitalInput.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/Solenoid.h>
-#include <frc/commands/Subsystem.h>
-
-/**
- * The Collector subsystem has one motor for the rollers, a limit switch for
- * ball
- * detection, a piston for opening and closing the claw, and a reed switch to
- * check if the piston is open.
- */
-class Collector : public frc::Subsystem {
- public:
-  // Constants for some useful speeds
-  static constexpr double kForward = 1;
-  static constexpr double kStop = 0;
-  static constexpr double kReverse = -1;
-
-  Collector();
-
-  /**
-   * NOTE: The current simulation model uses the the lower part of the
-   * claw
-   * since the limit switch wasn't exported. At some point, this will be
-   * updated.
-   *
-   * @return Whether or not the robot has the ball.
-   */
-  bool HasBall();
-
-  /**
-   * @param speed The speed to spin the rollers.
-   */
-  void SetSpeed(double speed);
-
-  /**
-   * Stop the rollers from spinning
-   */
-  void Stop();
-
-  /**
-   * @return Whether or not the claw is open.
-   */
-  bool IsOpen();
-
-  /**
-   * Open the claw up. (For shooting)
-   */
-  void Open();
-
-  /**
-   * Close the claw. (For collecting and driving)
-   */
-  void Close();
-
-  /**
-   * No default command.
-   */
-  void InitDefaultCommand() override;
-
- private:
-  // Subsystem devices
-  frc::PWMVictorSPX m_rollerMotor{6};
-  frc::DigitalInput m_ballDetector{10};
-  frc::Solenoid m_piston{1};
-  frc::DigitalInput m_openDetector{6};
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/DriveTrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/DriveTrain.h
deleted file mode 100644
index 0afa3eb..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/DriveTrain.h
+++ /dev/null
@@ -1,79 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/AnalogGyro.h>
-#include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
-#include <frc/commands/Subsystem.h>
-#include <frc/drive/DifferentialDrive.h>
-
-namespace frc {
-class Joystick;
-}  // namespace frc
-
-/**
- * The DriveTrain subsystem controls the robot's chassis and reads in
- * information about it's speed and position.
- */
-class DriveTrain : public frc::Subsystem {
- public:
-  DriveTrain();
-
-  /**
-   * When other commands aren't using the drivetrain, allow tank drive
-   * with
-   * the joystick.
-   */
-  void InitDefaultCommand();
-
-  /**
-   * @param leftAxis Left sides value
-   * @param rightAxis Right sides value
-   */
-  void TankDrive(double leftAxis, double rightAxis);
-
-  /**
-   * Stop the drivetrain from moving.
-   */
-  void Stop();
-
-  /**
-   * @return The encoder getting the distance and speed of left side of
-   * the drivetrain.
-   */
-  frc::Encoder& GetLeftEncoder();
-
-  /**
-   * @return The encoder getting the distance and speed of right side of
-   * the drivetrain.
-   */
-  frc::Encoder& GetRightEncoder();
-
-  /**
-   * @return The current angle of the drivetrain.
-   */
-  double GetAngle();
-
- private:
-  // Subsystem devices
-  frc::PWMVictorSPX m_frontLeftCIM{1};
-  frc::PWMVictorSPX m_rearLeftCIM{2};
-  frc::SpeedControllerGroup m_leftCIMs{m_frontLeftCIM, m_rearLeftCIM};
-
-  frc::PWMVictorSPX m_frontRightCIM{3};
-  frc::PWMVictorSPX m_rearRightCIM{4};
-  frc::SpeedControllerGroup m_rightCIMs{m_frontRightCIM, m_rearRightCIM};
-
-  frc::DifferentialDrive m_robotDrive{m_leftCIMs, m_rightCIMs};
-
-  frc::Encoder m_rightEncoder{1, 2, true, frc::Encoder::k4X};
-  frc::Encoder m_leftEncoder{3, 4, false, frc::Encoder::k4X};
-  frc::AnalogGyro m_gyro{0};
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pivot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pivot.h
deleted file mode 100644
index f9a3c25..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pivot.h
+++ /dev/null
@@ -1,74 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/AnalogPotentiometer.h>
-#include <frc/DigitalInput.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/commands/PIDSubsystem.h>
-
-/**
- * The Pivot subsystem contains the Van-door motor and the pot for PID control
- * of angle of the pivot and claw.
- */
-class Pivot : public frc::PIDSubsystem {
- public:
-  // Constants for some useful angles
-  static constexpr double kCollect = 105;
-  static constexpr double kLowGoal = 90;
-  static constexpr double kShoot = 45;
-  static constexpr double kShootNear = 30;
-
-  Pivot();
-
-  /**
-   *  No default command, if PID is enabled, the current setpoint will be
-   * maintained.
-   */
-  void InitDefaultCommand() override {}
-
-  /**
-   * @return The angle read in by the potentiometer
-   */
-  double ReturnPIDInput() override;
-
-  /**
-   * Set the motor speed based off of the PID output
-   */
-  void UsePIDOutput(double output) override;
-
-  /**
-   * @return If the pivot is at its upper limit.
-   */
-  bool IsAtUpperLimit();
-
-  /**
-   * @return If the pivot is at its lower limit.
-   */
-  bool IsAtLowerLimit();
-
-  /**
-   * @return The current angle of the pivot.
-   */
-  double GetAngle();
-
- private:
-  // Subsystem devices
-
-  // Sensors for measuring the position of the pivot
-  frc::DigitalInput m_upperLimitSwitch{13};
-  frc::DigitalInput m_lowerLimitSwitch{12};
-
-  /* 0 degrees is vertical facing up.
-   * Angle increases the more forward the pivot goes.
-   */
-  frc::AnalogPotentiometer m_pot{1};
-
-  // Motor to move the pivot
-  frc::PWMVictorSPX m_motor{5};
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h
deleted file mode 100644
index d321895..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Pneumatics.h
+++ /dev/null
@@ -1,53 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/AnalogInput.h>
-#include <frc/Compressor.h>
-#include <frc/commands/Subsystem.h>
-
-/**
- * The Pneumatics subsystem contains the compressor and a pressure sensor.
- *
- * NOTE: The simulator currently doesn't support the compressor or pressure
- * sensors.
- */
-class Pneumatics : public frc::Subsystem {
- public:
-  Pneumatics();
-
-  /**
-   * No default command
-   */
-  void InitDefaultCommand() override;
-
-  /**
-   * Start the compressor going. The compressor automatically starts and
-   * stops as it goes above and below maximum pressure.
-   */
-  void Start();
-
-  /**
-   * @return Whether or not the system is fully pressurized.
-   */
-  bool IsPressurized();
-
-  /**
-   * Puts the pressure on the SmartDashboard.
-   */
-  void WritePressure();
-
- private:
-  frc::AnalogInput m_pressureSensor{3};
-
-#ifndef SIMULATION
-  frc::Compressor m_compressor{1};  // TODO: (1, 14, 1, 8);
-#endif
-
-  static constexpr double kMaxPressure = 2.55;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h
deleted file mode 100644
index 43f111a..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/subsystems/Shooter.h
+++ /dev/null
@@ -1,127 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/DigitalInput.h>
-#include <frc/DoubleSolenoid.h>
-#include <frc/Solenoid.h>
-#include <frc/commands/Subsystem.h>
-
-/**
- * The Shooter subsystem handles shooting. The mechanism for shooting is
- * slightly complicated because it has to pneumatic cylinders for shooting, and
- * a third latch to allow the pressure to partially build up and reduce the
- * effect of the airflow. For shorter shots, when full power isn't needed, only
- * one cylinder fires.
- *
- * NOTE: Simulation currently approximates this as as single pneumatic cylinder
- * and ignores the latch.
- */
-class Shooter : public frc::Subsystem {
- public:
-  Shooter();
-  void InitDefaultCommand() override;
-
-  /**
-   * Extend both solenoids to shoot.
-   */
-  void ExtendBoth();
-
-  /**
-   * Retract both solenoids to prepare to shoot.
-   */
-  void RetractBoth();
-
-  /**
-   * Extend solenoid 1 to shoot.
-   */
-  void Extend1();
-
-  /**
-   * Retract solenoid 1 to prepare to shoot.
-   */
-  void Retract1();
-
-  /**
-   * Extend solenoid 2 to shoot.
-   */
-  void Extend2();
-
-  /**
-   * Retract solenoid 2 to prepare to shoot.
-   */
-  void Retract2();
-
-  /**
-   * Turns off the piston1 double solenoid. This won't actuate anything
-   * because double solenoids preserve their state when turned off. This
-   * should be called in order to reduce the amount of time that the coils
-   * are
-   * powered.
-   */
-  void Off1();
-
-  /**
-   * Turns off the piston1 double solenoid. This won't actuate anything
-   * because double solenoids preserve their state when turned off. This
-   * should be called in order to reduce the amount of time that the coils
-   * are
-   * powered.
-   */
-  void Off2();
-
-  /**
-   * Release the latch so that we can shoot
-   */
-  void Unlatch();
-
-  /**
-   * Latch so that pressure can build up and we aren't limited by air
-   * flow.
-   */
-  void Latch();
-
-  /**
-   * Toggles the latch postions
-   */
-  void ToggleLatchPosition();
-
-  /**
-   * @return Whether or not piston 1 is fully extended.
-   */
-  bool Piston1IsExtended();
-
-  /**
-   * @return Whether or not piston 1 is fully retracted.
-   */
-  bool Piston1IsRetracted();
-
-  /**
-   * Turns off all double solenoids. Double solenoids hold their position
-   * when
-   * they are turned off. We should turn them off whenever possible to
-   * extend
-   * the life of the coils
-   */
-  void OffBoth();
-
-  /**
-   * @return Whether or not the goal is hot as read by the banner sensor
-   */
-  bool GoalIsHot();
-
- private:
-  // Devices
-  frc::DoubleSolenoid m_piston1{3, 4};
-  frc::DoubleSolenoid m_piston2{5, 6};
-  frc::Solenoid m_latchPiston{1, 2};
-  frc::DigitalInput m_piston1ReedSwitchFront{9};
-  frc::DigitalInput m_piston1ReedSwitchBack{11};
-  frc::DigitalInput m_hotGoalSensor{
-      7};  // NOTE: Currently ignored in simulation
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/triggers/DoubleButton.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/triggers/DoubleButton.h
deleted file mode 100644
index 562154b..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PacGoat/include/triggers/DoubleButton.h
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/buttons/Trigger.h>
-
-namespace frc {
-class Joystick;
-}  // namespace frc
-
-class DoubleButton : public frc::Trigger {
- public:
-  DoubleButton(frc::Joystick* joy, int button1, int button2);
-
-  bool Get();
-
- private:
-  frc::Joystick& m_joy;
-  int m_button1;
-  int m_button2;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
index afc10a3..5b6b024 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/PotentiometerPID/cpp/Robot.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <array>
 
 #include <frc/AnalogInput.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a sample program to demonstrate how to use a soft potentiometer and a
@@ -20,6 +17,10 @@
  */
 class Robot : public frc::TimedRobot {
  public:
+  void RobotInit() override {
+    m_pidController.SetSetpoint(kSetPoints[m_index]);
+  }
+
   void TeleopPeriodic() override {
     // When the button is pressed once, the selected elevator setpoint is
     // incremented.
@@ -27,10 +28,10 @@
     if (currentButtonValue && !m_previousButtonValue) {
       // Index of the elevator setpoint wraps around
       m_index = (m_index + 1) % (sizeof(kSetPoints) / 8);
+      m_pidController.SetSetpoint(kSetPoints[m_index]);
     }
     m_previousButtonValue = currentButtonValue;
 
-    m_pidController.SetSetpoint(kSetPoints[m_index]);
     double output =
         m_pidController.Calculate(m_potentiometer.GetAverageVoltage());
     m_elevatorMotor.Set(output);
@@ -61,7 +62,7 @@
 
   frc::AnalogInput m_potentiometer{kPotChannel};
   frc::Joystick m_joystick{kJoystickChannel};
-  frc::PWMVictorSPX m_elevatorMotor{kMotorChannel};
+  frc::PWMSparkMax m_elevatorMotor{kMotorChannel};
 
   frc2::PIDController m_pidController{kP, kI, kD};
 };
@@ -69,5 +70,7 @@
 constexpr std::array<double, 3> Robot::kSetPoints;
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp
index 1ad76de..aae646f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/QuickVision/cpp/Robot.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdio>
 
 #include <cameraserver/CameraServer.h>
 #include <frc/TimedRobot.h>
-#include <wpi/raw_ostream.h>
 
 /**
  * Uses the CameraServer class to automatically capture video from a USB webcam
@@ -18,15 +16,17 @@
 class Robot : public frc::TimedRobot {
  public:
   void RobotInit() override {
-#if defined(__linux__)
-    frc::CameraServer::GetInstance()->StartAutomaticCapture();
+#if defined(__linux__) || defined(_WIN32)
+    frc::CameraServer::StartAutomaticCapture();
 #else
-    wpi::errs() << "Vision only available on Linux.\n";
-    wpi::errs().flush();
+    std::fputs("Vision only available on Linux or Windows.\n", stderr);
+    std::fflush(stderr);
 #endif
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp
index 46ffde3..d462c24 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Constants.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Constants.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
index 2f39216..49ee31b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/RobotContainer.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
+#include <utility>
+
 #include <frc/controller/PIDController.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/shuffleboard/Shuffleboard.h>
@@ -29,9 +28,8 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
index a82d22d..78355a2 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -20,6 +17,11 @@
       m_leftEncoder{kLeftEncoderPorts[0], kLeftEncoderPorts[1]},
       m_rightEncoder{kRightEncoderPorts[0], kRightEncoderPorts[1]},
       m_odometry{m_gyro.GetRotation2d()} {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
@@ -53,9 +55,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
@@ -65,9 +71,13 @@
   return m_gyro.GetRotation2d().Degrees();
 }
 
-double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
+double DriveSubsystem::GetTurnRate() {
+  return -m_gyro.GetRate();
+}
 
-frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::Pose2d DriveSubsystem::GetPose() {
+  return m_odometry.GetPose();
+}
 
 frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
   return {units::meters_per_second_t(m_leftEncoder.GetRate()),
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
index 7d3ea8f..cdd3a43 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h>
@@ -12,7 +9,7 @@
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #pragma once
 
@@ -43,7 +40,8 @@
 constexpr double kWheelDiameterInches = 6;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterInches * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterInches * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
 // These characterization values MUST be determined either experimentally or
@@ -69,5 +67,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
index d6e4288..3d6a36f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
index 73c6c21..8ea14da 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteCommand/include/subsystems/DriveSubsystem.h
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/voltage.h>
 
@@ -120,16 +117,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1;
-  frc::PWMVictorSPX m_left2;
-  frc::PWMVictorSPX m_right1;
-  frc::PWMVictorSPX m_right2;
+  frc::PWMSparkMax m_left1;
+  frc::PWMSparkMax m_left2;
+  frc::PWMSparkMax m_right1;
+  frc::PWMSparkMax m_right2;
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
index f52ddec..1871688 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Drivetrain.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Drivetrain.h"
 
@@ -11,9 +8,9 @@
   const auto leftFeedforward = m_feedforward.Calculate(speeds.left);
   const auto rightFeedforward = m_feedforward.Calculate(speeds.right);
   const double leftOutput = m_leftPIDController.Calculate(
-      m_leftEncoder.GetRate(), speeds.left.to<double>());
+      m_leftEncoder.GetRate(), speeds.left.value());
   const double rightOutput = m_rightPIDController.Calculate(
-      m_rightEncoder.GetRate(), speeds.right.to<double>());
+      m_rightEncoder.GetRate(), speeds.right.value());
 
   m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
   m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
@@ -34,4 +31,6 @@
   m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
 }
 
-frc::Pose2d Drivetrain::GetPose() const { return m_odometry.GetPose(); }
+frc::Pose2d Drivetrain::GetPose() const {
+  return m_odometry.GetPose();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
index 61e5d85..f47da51 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/cpp/Robot.cpp
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/SlewRateLimiter.h>
 #include <frc/TimedRobot.h>
+#include <frc/Timer.h>
 #include <frc/XboxController.h>
 #include <frc/controller/RamseteController.h>
+#include <frc/filter/SlewRateLimiter.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 #include <frc/trajectory/TrajectoryGenerator.h>
-#include <frc2/Timer.h>
 
 #include "Drivetrain.h"
 
@@ -20,14 +19,23 @@
     // Start the timer.
     m_timer.Start();
 
+    // Send Field2d to SmartDashboard.
+    frc::SmartDashboard::PutData(&m_field);
+
     // Reset the drivetrain's odometry to the starting pose of the trajectory.
     m_drive.ResetOdometry(m_trajectory.InitialPose());
+
+    // Send our generated trajectory to Field2d.
+    m_field.GetObject("traj")->SetTrajectory(m_trajectory);
   }
 
   void AutonomousPeriodic() override {
     // Update odometry.
     m_drive.UpdateOdometry();
 
+    // Update robot position on Field2d.
+    m_field.SetRobotPose(m_drive.GetPose());
+
     if (m_timer.Get() < m_trajectory.TotalTime()) {
       // Get the desired pose from the trajectory.
       auto desiredPose = m_trajectory.Sample(m_timer.Get());
@@ -46,16 +54,14 @@
   void TeleopPeriodic() override {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    const auto xSpeed = -m_speedLimiter.Calculate(
-                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+    const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
                         Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    const auto rot = -m_rotLimiter.Calculate(
-                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+    const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_drive.Drive(xSpeed, rot);
@@ -81,9 +87,14 @@
   frc::RamseteController m_ramseteController;
 
   // The timer to use during the autonomous period.
-  frc2::Timer m_timer;
+  frc::Timer m_timer;
+
+  // Create Field2d for robot and trajectory visualizations.
+  frc::Field2d m_field;
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
index a707014..11fa523 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RamseteController/include/Drivetrain.h
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/AnalogGyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <units/angular_velocity.h>
 #include <units/length.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * Represents a differential drive style drivetrain.
@@ -26,12 +23,18 @@
  public:
   Drivetrain() {
     m_gyro.Reset();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
     // resolution.
-    m_leftEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+    m_leftEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
                                       kEncoderResolution);
-    m_rightEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+    m_rightEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
                                        kEncoderResolution);
 
     m_leftEncoder.Reset();
@@ -41,7 +44,7 @@
   static constexpr units::meters_per_second_t kMaxSpeed =
       3.0_mps;  // 3 meters per second
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
-      wpi::math::pi};  // 1/2 rotation per second
+      wpi::numbers::pi};  // 1/2 rotation per second
 
   void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
   void Drive(units::meters_per_second_t xSpeed,
@@ -55,13 +58,13 @@
   static constexpr double kWheelRadius = 0.0508;  // meters
   static constexpr int kEncoderResolution = 4096;
 
-  frc::PWMVictorSPX m_leftLeader{1};
-  frc::PWMVictorSPX m_leftFollower{2};
-  frc::PWMVictorSPX m_rightLeader{3};
-  frc::PWMVictorSPX m_rightFollower{4};
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
 
-  frc::SpeedControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
-  frc::SpeedControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
 
   frc::Encoder m_leftEncoder{0, 1};
   frc::Encoder m_rightEncoder{2, 3};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp
index 8225b1c..b4db999 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Relay/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Joystick.h>
 #include <frc/Relay.h>
@@ -56,5 +53,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/Robot.cpp
new file mode 100644
index 0000000..fa9438d
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/Robot.cpp
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Robot.h"
+
+#include <frc2/command/CommandScheduler.h>
+
+void Robot::RobotInit() {}
+
+/**
+ * This function is called every robot packet, no matter the mode. Use
+ * this for items like diagnostics that you want to run during disabled,
+ * autonomous, teleoperated and test.
+ *
+ * <p> This runs after the mode specific periodic functions, but before
+ * LiveWindow and SmartDashboard integrated updating.
+ */
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
+
+/**
+ * This function is called once each time the robot enters Disabled mode. You
+ * can use it to reset any subsystem information you want to clear when the
+ * robot is disabled.
+ */
+void Robot::DisabledInit() {}
+
+void Robot::DisabledPeriodic() {}
+
+/**
+ * This autonomous runs the autonomous command selected by your {@link
+ * RobotContainer} class.
+ */
+void Robot::AutonomousInit() {
+  m_autonomousCommand = m_container.GetAutonomousCommand();
+
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Schedule();
+  }
+}
+
+void Robot::AutonomousPeriodic() {}
+
+void Robot::TeleopInit() {
+  // This makes sure that the autonomous stops running when
+  // teleop starts running. If you want the autonomous to
+  // continue until interrupted by another command, remove
+  // this line or comment it out.
+  if (m_autonomousCommand != nullptr) {
+    m_autonomousCommand->Cancel();
+    m_autonomousCommand = nullptr;
+  }
+}
+
+/**
+ * This function is called periodically during operator control.
+ */
+void Robot::TeleopPeriodic() {}
+
+/**
+ * This function is called periodically during test mode.
+ */
+void Robot::TestPeriodic() {}
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/RobotContainer.cpp
new file mode 100644
index 0000000..3cbad01
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/RobotContainer.cpp
@@ -0,0 +1,36 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "RobotContainer.h"
+
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc2/command/PrintCommand.h>
+#include <frc2/command/button/Button.h>
+
+#include "commands/TeleopArcadeDrive.h"
+
+RobotContainer::RobotContainer() {
+  // Configure the button bindings
+  ConfigureButtonBindings();
+}
+
+void RobotContainer::ConfigureButtonBindings() {
+  // Also set default commands here
+  m_drive.SetDefaultCommand(TeleopArcadeDrive(
+      &m_drive, [this] { return -m_controller.GetRawAxis(1); },
+      [this] { return m_controller.GetRawAxis(2); }));
+
+  // Example of how to use the onboard IO
+  m_onboardButtonA.WhenPressed(frc2::PrintCommand("Button A Pressed"))
+      .WhenReleased(frc2::PrintCommand("Button A Released"));
+
+  // Setup SmartDashboard options.
+  m_chooser.SetDefaultOption("Auto Routine Distance", &m_autoDistance);
+  m_chooser.AddOption("Auto Routine Time", &m_autoTime);
+  frc::SmartDashboard::PutData("Auto Selector", &m_chooser);
+}
+
+frc2::Command* RobotContainer::GetAutonomousCommand() {
+  return m_chooser.GetSelected();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveDistance.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveDistance.cpp
new file mode 100644
index 0000000..dcb59e3
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveDistance.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/DriveDistance.h"
+
+#include <units/math.h>
+
+void DriveDistance::Initialize() {
+  m_drive->ArcadeDrive(0, 0);
+  m_drive->ResetEncoders();
+}
+
+void DriveDistance::Execute() {
+  m_drive->ArcadeDrive(m_speed, 0);
+}
+
+void DriveDistance::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+}
+
+bool DriveDistance::IsFinished() {
+  return units::math::abs(m_drive->GetAverageDistance()) >= m_distance;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveTime.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveTime.cpp
new file mode 100644
index 0000000..52670a0
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/DriveTime.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/DriveTime.h"
+
+void DriveTime::Initialize() {
+  m_timer.Start();
+  m_drive->ArcadeDrive(0, 0);
+}
+
+void DriveTime::Execute() {
+  m_drive->ArcadeDrive(m_speed, 0);
+}
+
+void DriveTime::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+  m_timer.Stop();
+  m_timer.Reset();
+}
+
+bool DriveTime::IsFinished() {
+  return m_timer.HasElapsed(m_duration);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp
new file mode 100644
index 0000000..0f58527
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TeleopArcadeDrive.cpp
@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/TeleopArcadeDrive.h"
+
+#include "subsystems/Drivetrain.h"
+
+TeleopArcadeDrive::TeleopArcadeDrive(
+    Drivetrain* subsystem, std::function<double()> xaxisSpeedSupplier,
+    std::function<double()> zaxisRotateSuppplier)
+    : m_drive{subsystem},
+      m_xaxisSpeedSupplier{xaxisSpeedSupplier},
+      m_zaxisRotateSupplier{zaxisRotateSuppplier} {
+  AddRequirements({subsystem});
+}
+
+void TeleopArcadeDrive::Execute() {
+  m_drive->ArcadeDrive(m_xaxisSpeedSupplier(), m_zaxisRotateSupplier());
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp
new file mode 100644
index 0000000..2d6c148
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnDegrees.cpp
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/TurnDegrees.h"
+
+#include <units/math.h>
+#include <wpi/numbers>
+
+void TurnDegrees::Initialize() {
+  // Set motors to stop, read encoder values for starting point
+  m_drive->ArcadeDrive(0, 0);
+  m_drive->ResetEncoders();
+}
+
+void TurnDegrees::Execute() {
+  m_drive->ArcadeDrive(0, m_speed);
+}
+
+void TurnDegrees::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+}
+
+bool TurnDegrees::IsFinished() {
+  // Need to convert distance travelled to degrees. The Standard Romi Chassis
+  // found here https://www.pololu.com/category/203/romi-chassis-kits, has a
+  // wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm
+  // or 5.551 inches. We then take into consideration the width of the tires.
+  static auto inchPerDegree = (5.551_in * wpi::numbers::pi) / 360_deg;
+
+  // Compare distance traveled from start to distance based on degree turn.
+  return GetAverageTurningDistance() >= inchPerDegree * m_angle;
+}
+
+units::meter_t TurnDegrees::GetAverageTurningDistance() {
+  auto l = units::math::abs(m_drive->GetLeftDistance());
+  auto r = units::math::abs(m_drive->GetRightDistance());
+  return (l + r) / 2;
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnTime.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnTime.cpp
new file mode 100644
index 0000000..80c4bd2
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/commands/TurnTime.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "commands/TurnTime.h"
+
+void TurnTime::Initialize() {
+  m_timer.Start();
+  m_drive->ArcadeDrive(0, 0);
+}
+
+void TurnTime::Execute() {
+  m_drive->ArcadeDrive(0, m_speed);
+}
+
+void TurnTime::End(bool interrupted) {
+  m_drive->ArcadeDrive(0, 0);
+  m_timer.Stop();
+  m_timer.Reset();
+}
+
+bool TurnTime::IsFinished() {
+  return m_timer.HasElapsed(m_duration);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp
new file mode 100644
index 0000000..08a537b
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/sensors/RomiGyro.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "sensors/RomiGyro.h"
+
+RomiGyro::RomiGyro() : m_simDevice("Gyro:RomiGyro") {
+  if (m_simDevice) {
+    m_simDevice.CreateBoolean("init", hal::SimDevice::kOutput, true);
+    m_simRateX =
+        m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0);
+    m_simRateY =
+        m_simDevice.CreateDouble("rate_y", hal::SimDevice::kInput, 0.0);
+    m_simRateZ =
+        m_simDevice.CreateDouble("rate_z", hal::SimDevice::kInput, 0.0);
+    m_simAngleX =
+        m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0);
+    m_simAngleY =
+        m_simDevice.CreateDouble("angle_y", hal::SimDevice::kInput, 0.0);
+    m_simAngleZ =
+        m_simDevice.CreateDouble("angle_z", hal::SimDevice::kInput, 0.0);
+  }
+}
+
+double RomiGyro::GetRateX() {
+  if (m_simRateX) {
+    return m_simRateX.Get();
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetRateY() {
+  if (m_simRateY) {
+    return m_simRateY.Get();
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetRateZ() {
+  if (m_simRateZ) {
+    return m_simRateZ.Get();
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetAngleX() {
+  if (m_simAngleX) {
+    return m_simAngleX.Get() - m_angleXOffset;
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetAngleY() {
+  if (m_simAngleY) {
+    return m_simAngleY.Get() - m_angleYOffset;
+  }
+
+  return 0.0;
+}
+
+double RomiGyro::GetAngleZ() {
+  if (m_simAngleZ) {
+    return m_simAngleZ.Get() - m_angleZOffset;
+  }
+
+  return 0.0;
+}
+
+void RomiGyro::Reset() {
+  if (m_simAngleX) {
+    m_angleXOffset = m_simAngleX.Get();
+    m_angleYOffset = m_simAngleY.Get();
+    m_angleZOffset = m_simAngleZ.Get();
+  }
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp
new file mode 100644
index 0000000..13ec2d8
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/Drivetrain.cpp
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/Drivetrain.h"
+
+#include <wpi/numbers>
+
+#include "Constants.h"
+
+using namespace DriveConstants;
+
+// The Romi has the left and right motors set to
+// PWM channels 0 and 1 respectively
+// The Romi has onboard encoders that are hardcoded
+// to use DIO pins 4/5 and 6/7 for the left and right
+Drivetrain::Drivetrain() {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotor.SetInverted(true);
+
+  m_leftEncoder.SetDistancePerPulse(wpi::numbers::pi * kWheelDiameter.value() /
+                                    kCountsPerRevolution);
+  m_rightEncoder.SetDistancePerPulse(wpi::numbers::pi * kWheelDiameter.value() /
+                                     kCountsPerRevolution);
+  ResetEncoders();
+}
+
+void Drivetrain::Periodic() {
+  // This method will be called once per scheduler run.
+}
+
+void Drivetrain::ArcadeDrive(double xaxisSpeed, double zaxisRotate) {
+  m_drive.ArcadeDrive(xaxisSpeed, zaxisRotate);
+}
+
+void Drivetrain::ResetEncoders() {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+}
+
+int Drivetrain::GetLeftEncoderCount() {
+  return m_leftEncoder.Get();
+}
+
+int Drivetrain::GetRightEncoderCount() {
+  return m_rightEncoder.Get();
+}
+
+units::meter_t Drivetrain::GetLeftDistance() {
+  return units::meter_t(m_leftEncoder.GetDistance());
+}
+
+units::meter_t Drivetrain::GetRightDistance() {
+  return units::meter_t(m_rightEncoder.GetDistance());
+}
+
+units::meter_t Drivetrain::GetAverageDistance() {
+  return (GetLeftDistance() + GetRightDistance()) / 2.0;
+}
+
+double Drivetrain::GetAccelX() {
+  return m_accelerometer.GetX();
+}
+
+double Drivetrain::GetAccelY() {
+  return m_accelerometer.GetY();
+}
+
+double Drivetrain::GetAccelZ() {
+  return m_accelerometer.GetZ();
+}
+
+double Drivetrain::GetGyroAngleX() {
+  return m_gyro.GetAngleX();
+}
+
+double Drivetrain::GetGyroAngleY() {
+  return m_gyro.GetAngleY();
+}
+
+double Drivetrain::GetGyroAngleZ() {
+  return m_gyro.GetAngleZ();
+}
+
+void Drivetrain::ResetGyro() {
+  m_gyro.Reset();
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/OnBoardIO.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/OnBoardIO.cpp
new file mode 100644
index 0000000..b178ec0
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/cpp/subsystems/OnBoardIO.cpp
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "subsystems/OnBoardIO.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc/Errors.h>
+#include <frc/Timer.h>
+
+OnBoardIO::OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2) {
+  if (dio1 == ChannelMode::INPUT) {
+    m_buttonB = std::make_unique<frc::DigitalInput>(1);
+  } else {
+    m_greenLed = std::make_unique<frc::DigitalOutput>(1);
+  }
+  if (dio2 == ChannelMode::INPUT) {
+    m_buttonC = std::make_unique<frc::DigitalInput>(2);
+    m_redLed = std::make_unique<frc::DigitalOutput>(2);
+  }
+}
+
+bool OnBoardIO::GetButtonAPressed() {
+  return m_buttonA.Get();
+}
+
+bool OnBoardIO::GetButtonBPressed() {
+  if (m_buttonB) {
+    return m_buttonB->Get();
+  }
+
+  auto currentTime = frc::Timer::GetFPGATimestamp();
+  if (currentTime > m_nextMessageTime) {
+    FRC_ReportError(frc::err::Error, "Button {} was not configured", "B");
+    m_nextMessageTime = currentTime + kMessageInterval;
+  }
+  return false;
+}
+
+bool OnBoardIO::GetButtonCPressed() {
+  if (m_buttonC) {
+    return m_buttonC->Get();
+  }
+
+  auto currentTime = frc::Timer::GetFPGATimestamp();
+  if (currentTime > m_nextMessageTime) {
+    FRC_ReportError(frc::err::Error, "Button {} was not configured", "C");
+    m_nextMessageTime = currentTime + kMessageInterval;
+  }
+  return false;
+}
+
+void OnBoardIO::SetGreenLed(bool value) {
+  if (m_greenLed) {
+    m_greenLed->Set(value);
+  } else {
+    auto currentTime = frc::Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      FRC_ReportError(frc::err::Error, "{} LED was not configured", "Green");
+      m_nextMessageTime = currentTime + kMessageInterval;
+    }
+  }
+}
+
+void OnBoardIO::SetRedLed(bool value) {
+  if (m_redLed) {
+    m_redLed->Set(value);
+  } else {
+    auto currentTime = frc::Timer::GetFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      FRC_ReportError(frc::err::Error, "{} LED was not configured", "Red");
+      m_nextMessageTime = currentTime + kMessageInterval;
+    }
+  }
+}
+
+void OnBoardIO::SetYellowLed(bool value) {
+  m_yellowLed.Set(value);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h
new file mode 100644
index 0000000..e5cee33
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Constants.h
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+/**
+ * The Constants header provides a convenient place for teams to hold robot-wide
+ * numerical or bool constants.  This should not be used for any other purpose.
+ *
+ * It is generally a good idea to place constants into subsystem- or
+ * command-specific namespaces within this header, which can then be used where
+ * they are needed.
+ */
+
+namespace DriveConstants {
+constexpr double kCountsPerRevolution = 1440.0;
+constexpr double kWheelDiameterInch = 2.75;
+}  // namespace DriveConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Robot.h
new file mode 100644
index 0000000..eb35fab
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/Robot.h
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/TimedRobot.h>
+#include <frc2/command/Command.h>
+
+#include "RobotContainer.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override;
+  void RobotPeriodic() override;
+  void DisabledInit() override;
+  void DisabledPeriodic() override;
+  void AutonomousInit() override;
+  void AutonomousPeriodic() override;
+  void TeleopInit() override;
+  void TeleopPeriodic() override;
+  void TestPeriodic() override;
+
+ private:
+  // Have it null by default so that if testing teleop it
+  // doesn't have undefined behavior and potentially crash.
+  frc2::Command* m_autonomousCommand = nullptr;
+  RobotContainer m_container;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/RobotContainer.h
new file mode 100644
index 0000000..821542c
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/RobotContainer.h
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Joystick.h>
+#include <frc/smartdashboard/SendableChooser.h>
+#include <frc2/command/Command.h>
+#include <frc2/command/button/Button.h>
+
+#include "Constants.h"
+#include "commands/AutonomousDistance.h"
+#include "commands/AutonomousTime.h"
+#include "subsystems/Drivetrain.h"
+#include "subsystems/OnBoardIO.h"
+
+/**
+ * This class is where the bulk of the robot should be declared.  Since
+ * Command-based is a "declarative" paradigm, very little robot logic should
+ * actually be handled in the {@link Robot} periodic methods (other than the
+ * scheduler calls).  Instead, the structure of the robot (including subsystems,
+ * commands, and button mappings) should be declared here.
+ */
+class RobotContainer {
+  // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the
+  // hardware "overlay"
+  // that is specified when launching the wpilib-ws server on the Romi raspberry
+  // pi. By default, the following are available (listed in order from inside of
+  // the board to outside):
+  // - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
+  // - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
+  // - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
+  // - PWM 2 (mapped to Arduino Pin 21)
+  // - PWM 3 (mapped to Arduino Pin 22)
+  //
+  // Your subsystem configuration should take the overlays into account
+ public:
+  RobotContainer();
+  frc2::Command* GetAutonomousCommand();
+
+ private:
+  // Assumes a gamepad plugged into channnel 0
+  frc::Joystick m_controller{0};
+  frc::SendableChooser<frc2::Command*> m_chooser;
+
+  // The robot's subsystems
+  Drivetrain m_drive;
+  OnBoardIO m_onboardIO{OnBoardIO::ChannelMode::INPUT,
+                        OnBoardIO::ChannelMode::INPUT};
+
+  // Example button
+  frc2::Button m_onboardButtonA{
+      [this] { return m_onboardIO.GetButtonAPressed(); }};
+
+  // Autonomous commands.
+  AutonomousDistance m_autoDistance{&m_drive};
+  AutonomousTime m_autoTime{&m_drive};
+
+  void ConfigureButtonBindings();
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousDistance.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousDistance.h
new file mode 100644
index 0000000..8991ce6
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousDistance.h
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "commands/DriveDistance.h"
+#include "commands/TurnDegrees.h"
+#include "subsystems/Drivetrain.h"
+
+class AutonomousDistance
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup,
+                                 AutonomousDistance> {
+ public:
+  explicit AutonomousDistance(Drivetrain* drive) {
+    AddCommands(
+        DriveDistance(-0.5, 10_in, drive), TurnDegrees(-0.5, 180_deg, drive),
+        DriveDistance(-0.5, 10_in, drive), TurnDegrees(0.5, 180_deg, drive));
+  }
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousTime.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousTime.h
new file mode 100644
index 0000000..e25e5ea
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/AutonomousTime.h
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandHelper.h>
+#include <frc2/command/SequentialCommandGroup.h>
+
+#include "commands/DriveTime.h"
+#include "commands/TurnTime.h"
+#include "subsystems/Drivetrain.h"
+
+class AutonomousTime
+    : public frc2::CommandHelper<frc2::SequentialCommandGroup, AutonomousTime> {
+ public:
+  explicit AutonomousTime(Drivetrain* drive) {
+    AddCommands(DriveTime(-0.6, 2_s, drive), TurnTime(-0.5, 1.3_s, drive),
+                DriveTime(-0.6, 2_s, drive), TurnTime(0.5, 1.3_s, drive));
+  }
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h
new file mode 100644
index 0000000..34a1e93
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveDistance.h
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/length.h>
+
+#include "subsystems/Drivetrain.h"
+
+class DriveDistance
+    : public frc2::CommandHelper<frc2::CommandBase, DriveDistance> {
+ public:
+  DriveDistance(double speed, units::meter_t distance, Drivetrain* drive)
+      : m_speed(speed), m_distance(distance), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::meter_t m_distance;
+  Drivetrain* m_drive;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h
new file mode 100644
index 0000000..a0135e5
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/DriveTime.h
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Timer.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Drivetrain.h"
+
+class DriveTime : public frc2::CommandHelper<frc2::CommandBase, DriveTime> {
+ public:
+  DriveTime(double speed, units::second_t time, Drivetrain* drive)
+      : m_speed(speed), m_duration(time), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::second_t m_duration;
+  Drivetrain* m_drive;
+  frc::Timer m_timer;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TeleopArcadeDrive.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TeleopArcadeDrive.h
new file mode 100644
index 0000000..859b6b7
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TeleopArcadeDrive.h
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TeleopArcadeDrive
+    : public frc2::CommandHelper<frc2::CommandBase, TeleopArcadeDrive> {
+ public:
+  TeleopArcadeDrive(Drivetrain* subsystem,
+                    std::function<double()> xaxisSpeedSupplier,
+                    std::function<double()> zaxisRotateSupplier);
+  void Execute() override;
+
+ private:
+  Drivetrain* m_drive;
+  std::function<double()> m_xaxisSpeedSupplier;
+  std::function<double()> m_zaxisRotateSupplier;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h
new file mode 100644
index 0000000..7ce65df
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnDegrees.h
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/angle.h>
+#include <units/length.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TurnDegrees : public frc2::CommandHelper<frc2::CommandBase, TurnDegrees> {
+ public:
+  TurnDegrees(double speed, units::degree_t angle, Drivetrain* drive)
+      : m_speed(speed), m_angle(angle), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::degree_t m_angle;
+  Drivetrain* m_drive;
+
+  units::meter_t GetAverageTurningDistance();
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h
new file mode 100644
index 0000000..395825d
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/commands/TurnTime.h
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Timer.h>
+#include <frc2/command/CommandBase.h>
+#include <frc2/command/CommandHelper.h>
+#include <units/time.h>
+
+#include "subsystems/Drivetrain.h"
+
+class TurnTime : public frc2::CommandHelper<frc2::CommandBase, TurnTime> {
+ public:
+  TurnTime(double speed, units::second_t time, Drivetrain* drive)
+      : m_speed(speed), m_duration(time), m_drive(drive) {
+    AddRequirements({m_drive});
+  }
+
+  void Initialize() override;
+  void Execute() override;
+  void End(bool interrupted) override;
+  bool IsFinished() override;
+
+ private:
+  double m_speed;
+  units::second_t m_duration;
+  Drivetrain* m_drive;
+  frc::Timer m_timer;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h
new file mode 100644
index 0000000..0e93d48
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/sensors/RomiGyro.h
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <hal/SimDevice.h>
+
+class RomiGyro {
+ public:
+  RomiGyro();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the X-axis
+   */
+  double GetRateX();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Y-axis
+   */
+  double GetRateY();
+
+  /**
+   * Gets the rate of turn in degrees-per-second around the Z-axis
+   */
+  double GetRateZ();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleX();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleY();
+
+  /**
+   * Gets the currently reported angle around the X-axis
+   */
+  double GetAngleZ();
+
+  /**
+   * Resets the gyro
+   */
+  void Reset();
+
+ private:
+  hal::SimDevice m_simDevice;
+  hal::SimDouble m_simRateX;
+  hal::SimDouble m_simRateY;
+  hal::SimDouble m_simRateZ;
+  hal::SimDouble m_simAngleX;
+  hal::SimDouble m_simAngleY;
+  hal::SimDouble m_simAngleZ;
+
+  double m_angleXOffset = 0;
+  double m_angleYOffset = 0;
+  double m_angleZOffset = 0;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
new file mode 100644
index 0000000..98ae957
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/Drivetrain.h
@@ -0,0 +1,122 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/BuiltInAccelerometer.h>
+#include <frc/Encoder.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/Spark.h>
+#include <frc2/command/SubsystemBase.h>
+#include <units/length.h>
+
+#include "sensors/RomiGyro.h"
+
+class Drivetrain : public frc2::SubsystemBase {
+ public:
+  static constexpr double kCountsPerRevolution = 1440.0;
+  static constexpr units::meter_t kWheelDiameter = 70_mm;
+
+  Drivetrain();
+
+  /**
+   * Will be called periodically whenever the CommandScheduler runs.
+   */
+  void Periodic() override;
+
+  /**
+   * Drives the robot using arcade controls.
+   *
+   * @param xaxisSpeed the commanded forward movement
+   * @param zaxisRotate the commanded rotation
+   */
+  void ArcadeDrive(double xaxisSpeed, double zaxisRotate);
+
+  /**
+   * Resets the drive encoders to currently read a position of 0.
+   */
+  void ResetEncoders();
+
+  /**
+   * Gets the left drive encoder count.
+   *
+   * @return the left drive encoder count
+   */
+  int GetLeftEncoderCount();
+
+  /**
+   * Gets the right drive encoder count.
+   *
+   * @return the right drive encoder count
+   */
+  int GetRightEncoderCount();
+
+  /**
+   * Gets the left distance driven.
+   *
+   * @return the left-side distance driven
+   */
+  units::meter_t GetLeftDistance();
+
+  /**
+   * Gets the right distance driven.
+   *
+   * @return the right-side distance driven
+   */
+  units::meter_t GetRightDistance();
+
+  /**
+   * Returns the average distance traveled by the left and right encoders.
+   *
+   * @return The average distance traveled by the left and right encoders.
+   */
+  units::meter_t GetAverageDistance();
+
+  /**
+   * Returns the acceleration along the X-axis, in Gs.
+   */
+  double GetAccelX();
+
+  /**
+   * Returns the acceleration along the Y-axis, in Gs.
+   */
+  double GetAccelY();
+
+  /**
+   * Returns the acceleration along the Z-axis, in Gs.
+   */
+  double GetAccelZ();
+
+  /**
+   * Returns the current angle of the Romi around the X-axis, in degrees.
+   */
+  double GetGyroAngleX();
+
+  /**
+   * Returns the current angle of the Romi around the Y-axis, in degrees.
+   */
+  double GetGyroAngleY();
+
+  /**
+   * Returns the current angle of the Romi around the Z-axis, in degrees.
+   */
+  double GetGyroAngleZ();
+
+  /**
+   * Reset the gyro.
+   */
+  void ResetGyro();
+
+ private:
+  frc::Spark m_leftMotor{0};
+  frc::Spark m_rightMotor{1};
+
+  frc::Encoder m_leftEncoder{4, 5};
+  frc::Encoder m_rightEncoder{6, 7};
+
+  frc::DifferentialDrive m_drive{m_leftMotor, m_rightMotor};
+
+  RomiGyro m_gyro;
+  frc::BuiltInAccelerometer m_accelerometer;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/OnBoardIO.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/OnBoardIO.h
new file mode 100644
index 0000000..0bb5225
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/RomiReference/include/subsystems/OnBoardIO.h
@@ -0,0 +1,72 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc2/command/SubsystemBase.h>
+
+/**
+ * This class represents the onboard IO of the Romi
+ * reference robot. This includes the pushbuttons and
+ * LEDs.
+ *
+ * <p>DIO 0 - Button A (input only)
+ * DIO 1 - Button B (input) or Green LED (output)
+ * DIO 2 - Button C (input) or Red LED (output)
+ * DIO 3 - Yellow LED (output only)
+ */
+class OnBoardIO : public frc2::SubsystemBase {
+ public:
+  enum ChannelMode { INPUT, OUTPUT };
+  OnBoardIO(OnBoardIO::ChannelMode dio1, OnBoardIO::ChannelMode dio2);
+
+  static constexpr auto kMessageInterval = 1_s;
+  units::second_t m_nextMessageTime = 0_s;
+
+  /**
+   * Gets if the A button is pressed.
+   */
+  bool GetButtonAPressed();
+
+  /**
+   * Gets if the B button is pressed.
+   */
+  bool GetButtonBPressed();
+
+  /**
+   * Gets if the C button is pressed.
+   */
+  bool GetButtonCPressed();
+
+  /**
+   * Sets the green LED.
+   */
+  void SetGreenLed(bool value);
+
+  /**
+   * Sets the red LED.
+   */
+  void SetRedLed(bool value);
+
+  /**
+   * Sets the yellow LED.
+   */
+  void SetYellowLed(bool value);
+
+ private:
+  frc::DigitalInput m_buttonA{0};
+  frc::DigitalOutput m_yellowLed{3};
+
+  // DIO 1
+  std::unique_ptr<frc::DigitalInput> m_buttonB;
+  std::unique_ptr<frc::DigitalOutput> m_greenLed;
+
+  // DIO 2
+  std::unique_ptr<frc::DigitalInput> m_buttonC;
+  std::unique_ptr<frc::DigitalOutput> m_redLed;
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp
index dbc9d2a..0c2e79c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
@@ -48,12 +45,15 @@
   // Configure your button bindings here
 
   // Run instant command 1 when the 'A' button is pressed
-  frc2::JoystickButton(&m_driverController, 0).WhenPressed(&m_instantCommand1);
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kA)
+      .WhenPressed(&m_instantCommand1);
   // Run instant command 2 when the 'X' button is pressed
-  frc2::JoystickButton(&m_driverController, 3).WhenPressed(&m_instantCommand2);
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kX)
+      .WhenPressed(&m_instantCommand2);
   // Run instant command 3 when the 'Y' button is held; release early to
   // interrupt
-  frc2::JoystickButton(&m_driverController, 4).WhenHeld(&m_waitCommand);
+  frc2::JoystickButton(&m_driverController, frc::XboxController::Button::kY)
+      .WhenHeld(&m_waitCommand);
 }
 
 frc2::Command* RobotContainer::GetAutonomousCommand() {
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
index 9fb2281..ae5d02d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,5 +15,5 @@
  */
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h
index 6e3f321..715b9c5 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SchedulerEventLogging/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
index b06845e..573fcac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
index 9fb2281..ae5d02d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,5 +15,5 @@
  */
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
index c93e320..40eea33 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SelectCommand/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
index 3447504..a114b92 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/ShuffleBoard/cpp/Robot.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/AnalogPotentiometer.h>
 #include <frc/Encoder.h>
 #include <frc/Joystick.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/shuffleboard/Shuffleboard.h>
 #include <frc/shuffleboard/ShuffleboardLayout.h>
 #include <frc/shuffleboard/ShuffleboardTab.h>
@@ -64,9 +61,9 @@
   }
 
  private:
-  frc::PWMVictorSPX m_left{0};
-  frc::PWMVictorSPX m_right{1};
-  frc::PWMVictorSPX m_elevatorMotor{2};
+  frc::PWMSparkMax m_left{0};
+  frc::PWMSparkMax m_right{1};
+  frc::PWMSparkMax m_elevatorMotor{2};
 
   frc::DifferentialDrive m_robotDrive{m_left, m_right};
 
@@ -80,5 +77,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..05d99f3
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Drivetrain.cpp
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Drivetrain.h"
+
+#include <frc/RobotController.h>
+
+void Drivetrain::SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds) {
+  auto leftFeedforward = m_feedforward.Calculate(speeds.left);
+  auto rightFeedforward = m_feedforward.Calculate(speeds.right);
+  double leftOutput = m_leftPIDController.Calculate(m_leftEncoder.GetRate(),
+                                                    speeds.left.value());
+  double rightOutput = m_rightPIDController.Calculate(m_rightEncoder.GetRate(),
+                                                      speeds.right.value());
+
+  m_leftGroup.SetVoltage(units::volt_t{leftOutput} + leftFeedforward);
+  m_rightGroup.SetVoltage(units::volt_t{rightOutput} + rightFeedforward);
+}
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::radians_per_second_t rot) {
+  SetSpeeds(m_kinematics.ToWheelSpeeds({xSpeed, 0_mps, rot}));
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_odometry.Update(m_gyro.GetRotation2d(),
+                    units::meter_t(m_leftEncoder.GetDistance()),
+                    units::meter_t(m_rightEncoder.GetDistance()));
+}
+
+void Drivetrain::ResetOdometry(const frc::Pose2d& pose) {
+  m_leftEncoder.Reset();
+  m_rightEncoder.Reset();
+  m_drivetrainSimulator.SetPose(pose);
+  m_odometry.ResetPosition(pose, m_gyro.GetRotation2d());
+}
+
+void Drivetrain::SimulationPeriodic() {
+  // To update our simulation, we set motor voltage inputs, update the
+  // simulation, and write the simulated positions and velocities to our
+  // simulated encoder and gyro. We negate the right side so that positive
+  // voltages make the right side move forward.
+  m_drivetrainSimulator.SetInputs(units::volt_t{m_leftLeader.Get()} *
+                                      frc::RobotController::GetInputVoltage(),
+                                  units::volt_t{-m_rightLeader.Get()} *
+                                      frc::RobotController::GetInputVoltage());
+  m_drivetrainSimulator.Update(20_ms);
+
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
+  m_rightEncoderSim.SetDistance(
+      m_drivetrainSimulator.GetRightPosition().value());
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
+  m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees().value());
+}
+
+void Drivetrain::Periodic() {
+  UpdateOdometry();
+  m_fieldSim.SetRobotPose(m_odometry.GetPose());
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp
new file mode 100644
index 0000000..a6eeef4
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/cpp/Robot.cpp
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/TimedRobot.h>
+#include <frc/Timer.h>
+#include <frc/XboxController.h>
+#include <frc/controller/RamseteController.h>
+#include <frc/filter/SlewRateLimiter.h>
+#include <frc/trajectory/TrajectoryGenerator.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void RobotInit() override {
+    // Flush NetworkTables every loop. This ensures that robot pose and other
+    // values are sent during every iteration.
+    SetNetworkTablesFlushEnabled(true);
+
+    m_trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+        frc::Pose2d(2_m, 2_m, 0_rad), {}, frc::Pose2d(6_m, 4_m, 0_rad),
+        frc::TrajectoryConfig(2_mps, 2_mps_sq));
+  }
+
+  void RobotPeriodic() override { m_drive.Periodic(); }
+
+  void AutonomousInit() override {
+    m_timer.Reset();
+    m_timer.Start();
+    m_drive.ResetOdometry(m_trajectory.InitialPose());
+  }
+
+  void AutonomousPeriodic() override {
+    auto elapsed = m_timer.Get();
+    auto reference = m_trajectory.Sample(elapsed);
+    auto speeds = m_ramsete.Calculate(m_drive.GetPose(), reference);
+    m_drive.Drive(speeds.vx, speeds.omega);
+  }
+
+  void TeleopPeriodic() override {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_speedLimiter.Calculate(m_controller.GetLeftY()) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
+               Drivetrain::kMaxAngularSpeed;
+
+    m_drive.Drive(xSpeed, rot);
+  }
+
+  void SimulationPeriodic() override { m_drive.SimulationPeriodic(); }
+
+ private:
+  frc::XboxController m_controller{0};
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_speedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  Drivetrain m_drive;
+  frc::Trajectory m_trajectory;
+  frc::RamseteController m_ramsete;
+  frc::Timer m_timer;
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
new file mode 100644
index 0000000..d8c58c2
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SimpleDifferentialDriveSimulation/include/Drivetrain.h
@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/DifferentialDriveKinematics.h>
+#include <frc/kinematics/DifferentialDriveOdometry.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/simulation/DifferentialDrivetrainSim.h>
+#include <frc/simulation/EncoderSim.h>
+#include <frc/smartdashboard/Field2d.h>
+#include <frc/smartdashboard/SmartDashboard.h>
+#include <frc/system/plant/LinearSystemId.h>
+#include <units/angle.h>
+#include <units/angular_velocity.h>
+#include <units/length.h>
+#include <units/velocity.h>
+#include <wpi/numbers>
+
+/**
+ * Represents a differential drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() {
+    m_gyro.Reset();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.SetInverted(true);
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
+                                      kEncoderResolution);
+    m_rightEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
+                                       kEncoderResolution);
+
+    m_leftEncoder.Reset();
+    m_rightEncoder.Reset();
+
+    m_rightGroup.SetInverted(true);
+
+    frc::SmartDashboard::PutData("Field", &m_fieldSim);
+  }
+
+  static constexpr units::meters_per_second_t kMaxSpeed =
+      3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::numbers::pi};  // 1/2 rotation per second
+
+  void SetSpeeds(const frc::DifferentialDriveWheelSpeeds& speeds);
+  void Drive(units::meters_per_second_t xSpeed,
+             units::radians_per_second_t rot);
+  void UpdateOdometry();
+  void ResetOdometry(const frc::Pose2d& pose);
+
+  frc::Pose2d GetPose() const { return m_odometry.GetPose(); }
+
+  void SimulationPeriodic();
+  void Periodic();
+
+ private:
+  static constexpr units::meter_t kTrackWidth = 0.381_m * 2;
+  static constexpr double kWheelRadius = 0.0508;  // meters
+  static constexpr int kEncoderResolution = 4096;
+
+  frc::PWMSparkMax m_leftLeader{1};
+  frc::PWMSparkMax m_leftFollower{2};
+  frc::PWMSparkMax m_rightLeader{3};
+  frc::PWMSparkMax m_rightFollower{4};
+
+  frc::MotorControllerGroup m_leftGroup{m_leftLeader, m_leftFollower};
+  frc::MotorControllerGroup m_rightGroup{m_rightLeader, m_rightFollower};
+
+  frc::Encoder m_leftEncoder{0, 1};
+  frc::Encoder m_rightEncoder{2, 3};
+
+  frc2::PIDController m_leftPIDController{8.5, 0.0, 0.0};
+  frc2::PIDController m_rightPIDController{8.5, 0.0, 0.0};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::DifferentialDriveKinematics m_kinematics{kTrackWidth};
+  frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SimpleMotorFeedforward<units::meters> m_feedforward{1_V, 3_V / 1_mps};
+
+  // Simulation classes help us simulate our robot
+  frc::sim::AnalogGyroSim m_gyroSim{m_gyro};
+  frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
+  frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
+  frc::Field2d m_fieldSim;
+  frc::LinearSystem<2, 2, 2> m_drivetrainSystem =
+      frc::LinearSystemId::IdentifyDrivetrainSystem(
+          1.98_V / 1_mps, 0.2_V / 1_mps_sq, 1.5_V / 1_mps, 0.3_V / 1_mps_sq);
+  frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
+      m_drivetrainSystem, kTrackWidth, frc::DCMotor::CIM(2), 8, 2_in};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp
index 523a227..82e481b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Solenoid/cpp/Robot.cpp
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/DoubleSolenoid.h>
 #include <frc/Joystick.h>
+#include <frc/PneumaticsControlModule.h>
 #include <frc/Solenoid.h>
 #include <frc/TimedRobot.h>
 
@@ -57,10 +55,11 @@
   frc::Joystick m_stick{0};
 
   // Solenoid corresponds to a single solenoid.
-  frc::Solenoid m_solenoid{0};
+  frc::Solenoid m_solenoid{frc::PneumaticsModuleType::CTREPCM, 0};
 
   // DoubleSolenoid corresponds to a double solenoid.
-  frc::DoubleSolenoid m_doubleSolenoid{1, 2};
+  frc::DoubleSolenoid m_doubleSolenoid{frc::PneumaticsModuleType::CTREPCM, 1,
+                                       2};
 
   static constexpr int kSolenoidButton = 1;
   static constexpr int kDoubleSolenoidForward = 2;
@@ -68,5 +67,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
index 5efe04c..0f03d37 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceArm/cpp/Robot.cpp
@@ -1,27 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
-#include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearPlantInversionFeedforward.h>
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/estimator/KalmanFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/system/LinearSystemLoop.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
 #include <frc/trajectory/TrapezoidProfile.h>
 #include <units/angle.h>
 #include <units/moment_of_inertia.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This is a sample program to demonstrate how to use a state-space controller
@@ -69,7 +64,8 @@
       // qelms. Velocity error tolerance, in radians and radians per second.
       // Decrease this to more heavily penalize state excursion, or make the
       // controller behave more aggressively.
-      {1.0 * 2.0 * wpi::math::pi / 360.0, 10.0 * 2.0 * wpi::math::pi / 360.0},
+      {1.0 * 2.0 * wpi::numbers::pi / 360.0,
+       10.0 * 2.0 * wpi::numbers::pi / 360.0},
       // relms. Control effort (voltage) tolerance. Decrease this to more
       // heavily penalize control effort, or make the controller less
       // aggressive. 12 is a good starting point because that is the
@@ -87,7 +83,7 @@
   // An encoder set up to measure arm position in radians per second.
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
 
-  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::PWMSparkMax m_motor{kMotorPort};
   frc::XboxController m_joystick{kJoystickPort};
 
   frc::TrapezoidProfile<units::radians>::Constraints m_constraints{
@@ -96,25 +92,25 @@
   frc::TrapezoidProfile<units::radians>::State m_lastProfiledReference;
 
  public:
-  void RobotInit() {
+  void RobotInit() override {
     // We go 2 pi radians per 4096 clicks.
-    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0);
+    m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi / 4096.0);
   }
 
-  void TeleopInit() {
+  void TeleopInit() override {
     m_loop.Reset(
-        frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate()));
+        Eigen::Vector<double, 2>{m_encoder.GetDistance(), m_encoder.GetRate()});
 
     m_lastProfiledReference = {
         units::radian_t(m_encoder.GetDistance()),
         units::radians_per_second_t(m_encoder.GetRate())};
   }
 
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     // Sets the target position of our arm. This is similar to setting the
     // setpoint of a PID controller.
     frc::TrapezoidProfile<units::radians>::State goal;
-    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+    if (m_joystick.GetRightBumper()) {
       // We pressed the bumper, so let's set our next reference
       goal = {kRaisedPosition, 0_rad_per_s};
     } else {
@@ -127,11 +123,11 @@
             .Calculate(20_ms);
 
     m_loop.SetNextR(
-        frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to<double>(),
-                              m_lastProfiledReference.velocity.to<double>()));
+        Eigen::Vector<double, 2>{m_lastProfiledReference.position.value(),
+                                 m_lastProfiledReference.velocity.value()});
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
@@ -145,5 +141,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp
index 75849f1..be6bfe0 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Constants.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Constants.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
index 26d7499..0920db7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -12,7 +9,11 @@
 #include <frc/smartdashboard/SmartDashboard.h>
 #include <frc2/command/CommandScheduler.h>
 
-void Robot::RobotInit() {}
+void Robot::RobotInit() {
+  // Flush NetworkTables every loop. This ensures that robot pose and other
+  // values are sent during every iteration.
+  SetNetworkTablesFlushEnabled(true);
+}
 
 /**
  * This function is called every robot packet, no matter the mode. Use
@@ -22,7 +23,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -31,6 +34,7 @@
  */
 void Robot::DisabledInit() {
   frc2::CommandScheduler::GetInstance().CancelAll();
+  m_container.ZeroAllOutputs();
 }
 
 void Robot::DisabledPeriodic() {}
@@ -81,5 +85,7 @@
 }
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
index 7608122..554de50 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/RobotContainer.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
+#include <utility>
+
 #include <frc/controller/PIDController.h>
 #include <frc/controller/RamseteController.h>
 #include <frc/shuffleboard/Shuffleboard.h>
@@ -29,20 +28,26 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.ArcadeDrive(
-            m_driverController.GetY(frc::GenericHID::kLeftHand),
-            m_driverController.GetX(frc::GenericHID::kRightHand));
+        m_drive.ArcadeDrive(m_driverController.GetLeftY(),
+                            m_driverController.GetRightX());
       },
       {&m_drive}));
 }
 
-const DriveSubsystem& RobotContainer::GetRobotDrive() const { return m_drive; }
+void RobotContainer::ZeroAllOutputs() {
+  m_drive.TankDriveVolts(0_V, 0_V);
+}
+
+const DriveSubsystem& RobotContainer::GetRobotDrive() const {
+  return m_drive;
+}
 
 void RobotContainer::ConfigureButtonBindings() {
   // Configure your button bindings here
 
   // While holding the shoulder button, drive at half speed
-  frc2::JoystickButton(&m_driverController, 6)
+  frc2::JoystickButton(&m_driverController,
+                       frc::XboxController::Button::kRightBumper)
       .WhenPressed(&m_driveHalfSpeed)
       .WhenReleased(&m_driveFullSpeed);
 }
@@ -64,12 +69,12 @@
 
   // An example trajectory to follow.  All units in meters.
   auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
-      // Start at the origin facing the +X direction
-      frc::Pose2d(),
+      // Start at (1, 2) facing the +X direction
+      frc::Pose2d(1_m, 2_m, 0_deg),
       // Pass through these two interior waypoints, making an 's' curve path
-      {frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
+      {frc::Translation2d(2_m, 3_m), frc::Translation2d(3_m, 1_m)},
       // End 3 meters straight ahead of where we started, facing forward
-      frc::Pose2d(3_m, 0_m, 0_deg),
+      frc::Pose2d(4_m, 2_m, 0_deg),
       // Pass the config
       config);
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
index d8cc462..a94b860 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -12,15 +9,22 @@
 #include <frc/geometry/Rotation2d.h>
 #include <frc/kinematics/DifferentialDriveWheelSpeeds.h>
 #include <frc/simulation/SimDeviceSim.h>
+#include <frc/smartdashboard/SmartDashboard.h>
 
 using namespace DriveConstants;
 
 DriveSubsystem::DriveSubsystem() {
+  // We need to invert one side of the drivetrain so that positive voltages
+  // result in both sides moving forward. Depending on how your robot's
+  // gearbox is constructed, you might have to invert the left side instead.
+  m_rightMotors.SetInverted(true);
+
   // Set the distance per pulse for the encoders
   m_leftEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
   m_rightEncoder.SetDistancePerPulse(kEncoderDistancePerPulse);
 
   ResetEncoders();
+  frc::SmartDashboard::PutData("Field", &m_fieldSim);
 }
 
 void DriveSubsystem::Periodic() {
@@ -28,6 +32,7 @@
   m_odometry.Update(m_gyro.GetRotation2d(),
                     units::meter_t(m_leftEncoder.GetDistance()),
                     units::meter_t(m_rightEncoder.GetDistance()));
+  m_fieldSim.SetRobotPose(m_odometry.GetPose());
 }
 
 void DriveSubsystem::SimulationPeriodic() {
@@ -41,18 +46,12 @@
                                       frc::RobotController::GetInputVoltage());
   m_drivetrainSimulator.Update(20_ms);
 
-  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kLeftPosition));
-  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kLeftVelocity));
-  m_rightEncoderSim.SetDistance(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kRightPosition));
-  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetState(
-      frc::sim::DifferentialDrivetrainSim::State::kRightVelocity));
-  m_gyroAngleSim.SetAngle(
-      -m_drivetrainSimulator.GetHeading().Degrees().to<double>());
-
-  m_fieldSim.SetRobotPose(m_odometry.GetPose());
+  m_leftEncoderSim.SetDistance(m_drivetrainSimulator.GetLeftPosition().value());
+  m_leftEncoderSim.SetRate(m_drivetrainSimulator.GetLeftVelocity().value());
+  m_rightEncoderSim.SetDistance(
+      m_drivetrainSimulator.GetRightPosition().value());
+  m_rightEncoderSim.SetRate(m_drivetrainSimulator.GetRightVelocity().value());
+  m_gyroSim.SetAngle(-m_drivetrainSimulator.GetHeading().Degrees());
 }
 
 units::ampere_t DriveSubsystem::GetCurrentDraw() const {
@@ -78,9 +77,13 @@
   return (m_leftEncoder.GetDistance() + m_rightEncoder.GetDistance()) / 2.0;
 }
 
-frc::Encoder& DriveSubsystem::GetLeftEncoder() { return m_leftEncoder; }
+frc::Encoder& DriveSubsystem::GetLeftEncoder() {
+  return m_leftEncoder;
+}
 
-frc::Encoder& DriveSubsystem::GetRightEncoder() { return m_rightEncoder; }
+frc::Encoder& DriveSubsystem::GetRightEncoder() {
+  return m_rightEncoder;
+}
 
 void DriveSubsystem::SetMaxOutput(double maxOutput) {
   m_drive.SetMaxOutput(maxOutput);
@@ -90,9 +93,13 @@
   return m_gyro.GetRotation2d().Degrees();
 }
 
-double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
+double DriveSubsystem::GetTurnRate() {
+  return -m_gyro.GetRate();
+}
 
-frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::Pose2d DriveSubsystem::GetPose() {
+  return m_odometry.GetPose();
+}
 
 frc::DifferentialDriveWheelSpeeds DriveSubsystem::GetWheelSpeeds() {
   return {units::meters_per_second_t(m_leftEncoder.GetRate()),
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
index 77e1184..307d408 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/kinematics/DifferentialDriveKinematics.h>
 #include <frc/system/plant/DCMotor.h>
@@ -15,7 +12,7 @@
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #pragma once
 
@@ -46,7 +43,7 @@
 constexpr auto kWheelDiameter = 6_in;
 constexpr double kEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameter.to<double>() * wpi::math::pi) /
+    (kWheelDiameter.value() * wpi::numbers::pi) /
     static_cast<double>(kEncoderCPR);
 
 // These are example values only - DO NOT USE THESE FOR YOUR OWN ROBOT!
@@ -55,11 +52,11 @@
 // Toolsuite provides a convenient tool for obtaining these values for your
 // robot.
 constexpr auto ks = 0.22_V;
-constexpr auto kv = 1.98 * 1_V * 1_s / 1_m;
-constexpr auto ka = 0.2 * 1_V * 1_s * 1_s / 1_m;
+constexpr auto kv = 1.98 * 1_V / 1_mps;
+constexpr auto ka = 0.2 * 1_V / 1_mps_sq;
 
-constexpr auto kvAngular = 1.5 * 1_V * 1_s / 1_rad;
-constexpr auto kaAngular = 0.3 * 1_V * 1_s * 1_s / 1_rad;
+constexpr auto kvAngular = 1.5 * 1_V / 1_mps;
+constexpr auto kaAngular = 0.3 * 1_V / 1_mps_sq;
 
 extern const frc::LinearSystem<2, 2, 2> kDrivetrainPlant;
 
@@ -82,5 +79,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h
index 5050999..00271e6 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
index 06cbf7c..012e3c6 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -30,6 +27,7 @@
  public:
   RobotContainer();
 
+  void ZeroAllOutputs();
   frc2::Command* GetAutonomousCommand();
   const DriveSubsystem& GetRobotDrive() const;
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
index 2cbc990..395133c 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceDifferentialDriveSimulation/include/subsystems/DriveSubsystem.h
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <frc/AnalogGyro.h>
+#include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/SpeedControllerGroup.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/kinematics/DifferentialDriveOdometry.h>
-#include <frc/simulation/AnalogGyroSim.h>
+#include <frc/motorcontrol/MotorControllerGroup.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <frc/simulation/ADXRS450_GyroSim.h>
 #include <frc/simulation/DifferentialDrivetrainSim.h>
 #include <frc/simulation/EncoderSim.h>
-#include <frc/simulation/Field2d.h>
+#include <frc/smartdashboard/Field2d.h>
 #include <frc2/command/SubsystemBase.h>
 #include <units/voltage.h>
 
@@ -131,16 +128,16 @@
   // declared private and exposed only through public methods.
 
   // The motor controllers
-  frc::PWMVictorSPX m_left1{DriveConstants::kLeftMotor1Port};
-  frc::PWMVictorSPX m_left2{DriveConstants::kLeftMotor2Port};
-  frc::PWMVictorSPX m_right1{DriveConstants::kRightMotor1Port};
-  frc::PWMVictorSPX m_right2{DriveConstants::kRightMotor2Port};
+  frc::PWMSparkMax m_left1{DriveConstants::kLeftMotor1Port};
+  frc::PWMSparkMax m_left2{DriveConstants::kLeftMotor2Port};
+  frc::PWMSparkMax m_right1{DriveConstants::kRightMotor1Port};
+  frc::PWMSparkMax m_right2{DriveConstants::kRightMotor2Port};
 
   // The motors on the left side of the drive
-  frc::SpeedControllerGroup m_leftMotors{m_left1, m_left2};
+  frc::MotorControllerGroup m_leftMotors{m_left1, m_left2};
 
   // The motors on the right side of the drive
-  frc::SpeedControllerGroup m_rightMotors{m_right1, m_right2};
+  frc::MotorControllerGroup m_rightMotors{m_right1, m_right2};
 
   // The robot's drive
   frc::DifferentialDrive m_drive{m_leftMotors, m_rightMotors};
@@ -154,22 +151,24 @@
                               DriveConstants::kRightEncoderPorts[1]};
 
   // The gyro sensor
-  frc::AnalogGyro m_gyro{0};
+  frc::ADXRS450_Gyro m_gyro;
 
   // Odometry class for tracking robot pose
   frc::DifferentialDriveOdometry m_odometry{m_gyro.GetRotation2d()};
 
   // These classes help simulate our drivetrain.
   frc::sim::DifferentialDrivetrainSim m_drivetrainSimulator{
-      DriveConstants::kDrivetrainPlant, DriveConstants::kTrackwidth,
-      DriveConstants::kDrivetrainGearbox, DriveConstants::kDrivetrainGearing,
-      DriveConstants::kWheelDiameter / 2};
+      DriveConstants::kDrivetrainPlant,
+      DriveConstants::kTrackwidth,
+      DriveConstants::kDrivetrainGearbox,
+      DriveConstants::kDrivetrainGearing,
+      DriveConstants::kWheelDiameter / 2,
+      {0.001, 0.001, 0.0001, 0.1, 0.1, 0.005, 0.005}};
 
   frc::sim::EncoderSim m_leftEncoderSim{m_leftEncoder};
   frc::sim::EncoderSim m_rightEncoderSim{m_rightEncoder};
-  frc::sim::AnalogGyroSim m_gyroAngleSim{m_gyro};
+  frc::sim::ADXRS450_GyroSim m_gyroSim{m_gyro};
 
-  // The Field2d class simulates the field in the sim GUI. Note that we can have
-  // only one instance!
+  // The Field2d class shows the field in the sim GUI.
   frc::Field2d m_fieldSim;
 };
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
index e0eda4c..4c9d65e 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceElevator/cpp/Robot.cpp
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/Encoder.h>
-#include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/estimator/KalmanFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/system/LinearSystemLoop.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
@@ -22,7 +17,7 @@
 #include <units/length.h>
 #include <units/mass.h>
 #include <units/velocity.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This is a sample program to demonstrate how to use a state-space controller
@@ -84,7 +79,7 @@
   // An encoder set up to measure elevator height in meters.
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
 
-  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::PWMSparkMax m_motor{kMotorPort};
   frc::XboxController m_joystick{kJoystickPort};
 
   frc::TrapezoidProfile<units::meters>::Constraints m_constraints{3_fps,
@@ -93,26 +88,26 @@
   frc::TrapezoidProfile<units::meters>::State m_lastProfiledReference;
 
  public:
-  void RobotInit() {
+  void RobotInit() override {
     // Circumference = pi * d, so distance per click = pi * d / counts
-    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi *
-                                  kDrumRadius.to<double>() / 4096.0);
+    m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi * kDrumRadius.value() /
+                                  4096.0);
   }
 
-  void TeleopInit() {
+  void TeleopInit() override {
     // Reset our loop to make sure it's in a known state.
     m_loop.Reset(
-        frc::MakeMatrix<2, 1>(m_encoder.GetDistance(), m_encoder.GetRate()));
+        Eigen::Vector<double, 2>{m_encoder.GetDistance(), m_encoder.GetRate()});
 
     m_lastProfiledReference = {units::meter_t(m_encoder.GetDistance()),
                                units::meters_per_second_t(m_encoder.GetRate())};
   }
 
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     // Sets the target height of our elevator. This is similar to setting the
     // setpoint of a PID controller.
     frc::TrapezoidProfile<units::meters>::State goal;
-    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+    if (m_joystick.GetRightBumper()) {
       // We pressed the bumper, so let's set our next reference
       goal = {kRaisedPosition, 0_fps};
     } else {
@@ -125,11 +120,11 @@
             .Calculate(20_ms);
 
     m_loop.SetNextR(
-        frc::MakeMatrix<2, 1>(m_lastProfiledReference.position.to<double>(),
-                              m_lastProfiledReference.velocity.to<double>()));
+        Eigen::Vector<double, 2>{m_lastProfiledReference.position.value(),
+                                 m_lastProfiledReference.velocity.value()});
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetDistance()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetDistance()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
@@ -143,5 +138,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
index ca3d909..636916f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheel/cpp/Robot.cpp
@@ -1,25 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/DriverStation.h>
 #include <frc/Encoder.h>
-#include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/estimator/KalmanFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/system/LinearSystemLoop.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
 #include <units/angular_velocity.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This is a sample program to demonstrate how to use a state-space controller
@@ -80,32 +75,32 @@
   // An encoder set up to measure flywheel velocity in radians per second.
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
 
-  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::PWMSparkMax m_motor{kMotorPort};
   frc::XboxController m_joystick{kJoystickPort};
 
  public:
-  void RobotInit() {
+  void RobotInit() override {
     // We go 2 pi radians per 4096 clicks.
-    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0);
+    m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi / 4096.0);
   }
 
-  void TeleopInit() {
-    m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+  void TeleopInit() override {
+    m_loop.Reset(Eigen::Vector<double, 1>{m_encoder.GetRate()});
   }
 
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     // Sets the target speed of our flywheel. This is similar to setting the
     // setpoint of a PID controller.
-    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+    if (m_joystick.GetRightBumper()) {
       // We pressed the bumper, so let's set our next reference
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.value()});
     } else {
       // We released the bumper, so let's spin down
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
     }
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetRate()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
@@ -119,5 +114,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
index 02883fb..69d1953 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/StateSpaceFlywheelSysId/cpp/Robot.cpp
@@ -1,25 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/DriverStation.h>
 #include <frc/Encoder.h>
-#include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
-#include <frc/StateSpaceUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/controller/LinearPlantInversionFeedforward.h>
 #include <frc/controller/LinearQuadraticRegulator.h>
 #include <frc/drive/DifferentialDrive.h>
 #include <frc/estimator/KalmanFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc/system/LinearSystemLoop.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 /**
  * This is a sample program to demonstrate how to use a state-space controller
@@ -81,32 +76,32 @@
   // An encoder set up to measure flywheel velocity in radians per second.
   frc::Encoder m_encoder{kEncoderAChannel, kEncoderBChannel};
 
-  frc::PWMVictorSPX m_motor{kMotorPort};
+  frc::PWMSparkMax m_motor{kMotorPort};
   frc::XboxController m_joystick{kJoystickPort};
 
  public:
-  void RobotInit() {
+  void RobotInit() override {
     // We go 2 pi radians per 4096 clicks.
-    m_encoder.SetDistancePerPulse(2.0 * wpi::math::pi / 4096.0);
+    m_encoder.SetDistancePerPulse(2.0 * wpi::numbers::pi / 4096.0);
   }
 
-  void TeleopInit() {
-    m_loop.Reset(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+  void TeleopInit() override {
+    m_loop.Reset(Eigen::Vector<double, 1>{m_encoder.GetRate()});
   }
 
-  void TeleopPeriodic() {
+  void TeleopPeriodic() override {
     // Sets the target speed of our flywheel. This is similar to setting the
     // setpoint of a PID controller.
-    if (m_joystick.GetBumper(frc::GenericHID::kRightHand)) {
+    if (m_joystick.GetRightBumper()) {
       // We pressed the bumper, so let's set our next reference
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(kSpinup.to<double>()));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{kSpinup.value()});
     } else {
       // We released the bumper, so let's spin down
-      m_loop.SetNextR(frc::MakeMatrix<1, 1>(0.0));
+      m_loop.SetNextR(Eigen::Vector<double, 1>{0.0});
     }
 
     // Correct our Kalman filter's state vector estimate with encoder data.
-    m_loop.Correct(frc::MakeMatrix<1, 1>(m_encoder.GetRate()));
+    m_loop.Correct(Eigen::Vector<double, 1>{m_encoder.GetRate()});
 
     // Update our LQR to generate new voltage commands and use the voltages to
     // predict the next state with out Kalman filter.
@@ -120,5 +115,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
index fcc0bcd..7044fd7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Drivetrain.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Drivetrain.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
index 10e550b..39cfa14 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/Robot.cpp
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/SlewRateLimiter.h>
+#include <frc/MathUtil.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
 
 #include "Drivetrain.h"
 
@@ -34,14 +32,14 @@
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
     const auto xSpeed = -m_xspeedLimiter.Calculate(
-                            m_controller.GetY(frc::GenericHID::kLeftHand)) *
+                            frc::ApplyDeadband(m_controller.GetLeftY(), 0.02)) *
                         Drivetrain::kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
     const auto ySpeed = -m_yspeedLimiter.Calculate(
-                            m_controller.GetX(frc::GenericHID::kLeftHand)) *
+                            frc::ApplyDeadband(m_controller.GetLeftX(), 0.02)) *
                         Drivetrain::kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
@@ -49,7 +47,7 @@
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
     const auto rot = -m_rotLimiter.Calculate(
-                         m_controller.GetX(frc::GenericHID::kRightHand)) *
+                         frc::ApplyDeadband(m_controller.GetRightX(), 0.02)) *
                      Drivetrain::kMaxAngularSpeed;
 
     m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
@@ -57,5 +55,7 @@
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
index 720d554..24a6527 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/cpp/SwerveModule.cpp
@@ -1,33 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "SwerveModule.h"
 
 #include <frc/geometry/Rotation2d.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 SwerveModule::SwerveModule(const int driveMotorChannel,
-                           const int turningMotorChannel)
-    : m_driveMotor(driveMotorChannel), m_turningMotor(turningMotorChannel) {
+                           const int turningMotorChannel,
+                           const int driveEncoderChannelA,
+                           const int driveEncoderChannelB,
+                           const int turningEncoderChannelA,
+                           const int turningEncoderChannelB)
+    : m_driveMotor(driveMotorChannel),
+      m_turningMotor(turningMotorChannel),
+      m_driveEncoder(driveEncoderChannelA, driveEncoderChannelB),
+      m_turningEncoder(turningEncoderChannelA, turningEncoderChannelB) {
   // Set the distance per pulse for the drive encoder. We can simply use the
   // distance traveled for one rotation of the wheel divided by the encoder
   // resolution.
-  m_driveEncoder.SetDistancePerPulse(2 * wpi::math::pi * kWheelRadius /
+  m_driveEncoder.SetDistancePerPulse(2 * wpi::numbers::pi * kWheelRadius /
                                      kEncoderResolution);
 
   // Set the distance (in this case, angle) per pulse for the turning encoder.
-  // This is the the angle through an entire rotation (2 * wpi::math::pi)
+  // This is the the angle through an entire rotation (2 * wpi::numbers::pi)
   // divided by the encoder resolution.
-  m_turningEncoder.SetDistancePerPulse(2 * wpi::math::pi / kEncoderResolution);
+  m_turningEncoder.SetDistancePerPulse(2 * wpi::numbers::pi /
+                                       kEncoderResolution);
 
   // Limit the PID Controller's input range between -pi and pi and set the input
   // to be continuous.
-  m_turningPIDController.EnableContinuousInput(-units::radian_t(wpi::math::pi),
-                                               units::radian_t(wpi::math::pi));
+  m_turningPIDController.EnableContinuousInput(
+      -units::radian_t(wpi::numbers::pi), units::radian_t(wpi::numbers::pi));
 }
 
 frc::SwerveModuleState SwerveModule::GetState() const {
@@ -35,10 +40,15 @@
           frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
 }
 
-void SwerveModule::SetDesiredState(const frc::SwerveModuleState& state) {
+void SwerveModule::SetDesiredState(
+    const frc::SwerveModuleState& referenceState) {
+  // Optimize the reference state to avoid spinning further than 90 degrees
+  const auto state = frc::SwerveModuleState::Optimize(
+      referenceState, units::radian_t(m_turningEncoder.Get()));
+
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), state.speed.to<double>());
+      m_driveEncoder.GetRate(), state.speed.value());
 
   const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
index fed7ada..6ff5b48 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/Drivetrain.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,7 +8,7 @@
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/kinematics/SwerveDriveOdometry.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "SwerveModule.h"
 
@@ -30,7 +27,7 @@
   static constexpr units::meters_per_second_t kMaxSpeed =
       3.0_mps;  // 3 meters per second
   static constexpr units::radians_per_second_t kMaxAngularSpeed{
-      wpi::math::pi};  // 1/2 rotation per second
+      wpi::numbers::pi};  // 1/2 rotation per second
 
  private:
   frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m};
@@ -38,10 +35,10 @@
   frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m};
   frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
 
-  SwerveModule m_frontLeft{1, 2};
-  SwerveModule m_frontRight{2, 3};
-  SwerveModule m_backLeft{5, 6};
-  SwerveModule m_backRight{7, 8};
+  SwerveModule m_frontLeft{1, 2, 0, 1, 2, 3};
+  SwerveModule m_frontRight{2, 3, 4, 5, 6, 7};
+  SwerveModule m_backLeft{5, 6, 8, 9, 10, 11};
+  SwerveModule m_backRight{7, 8, 12, 13, 14, 15};
 
   frc::AnalogGyro m_gyro{0};
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
index bf82df4..00a938b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveBot/include/SwerveModule.h
@@ -1,27 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/controller/SimpleMotorFeedforward.h>
 #include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <units/angular_velocity.h>
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 class SwerveModule {
  public:
-  SwerveModule(int driveMotorChannel, int turningMotorChannel);
+  SwerveModule(int driveMotorChannel, int turningMotorChannel,
+               int driveEncoderChannelA, int driveEncoderChannelB,
+               int turningEncoderChannelA, int turningEncoderChannelB);
   frc::SwerveModuleState GetState() const;
   void SetDesiredState(const frc::SwerveModuleState& state);
 
@@ -30,15 +29,15 @@
   static constexpr int kEncoderResolution = 4096;
 
   static constexpr auto kModuleMaxAngularVelocity =
-      wpi::math::pi * 1_rad_per_s;  // radians per second
+      wpi::numbers::pi * 1_rad_per_s;  // radians per second
   static constexpr auto kModuleMaxAngularAcceleration =
-      wpi::math::pi * 2_rad_per_s / 1_s;  // radians per second^2
+      wpi::numbers::pi * 2_rad_per_s / 1_s;  // radians per second^2
 
-  frc::PWMVictorSPX m_driveMotor;
-  frc::PWMVictorSPX m_turningMotor;
+  frc::PWMSparkMax m_driveMotor;
+  frc::PWMSparkMax m_turningMotor;
 
-  frc::Encoder m_driveEncoder{0, 1};
-  frc::Encoder m_turningEncoder{2, 3};
+  frc::Encoder m_driveEncoder;
+  frc::Encoder m_turningEncoder;
 
   frc2::PIDController m_drivePIDController{1.0, 0, 0};
   frc::ProfiledPIDController<units::radians> m_turningPIDController{
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp
index a60d803..80fb825 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Constants.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Constants.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
index 4bb69bb..4ee98ae 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/RobotContainer.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
+#include <utility>
+
 #include <frc/controller/PIDController.h>
 #include <frc/geometry/Translation2d.h>
 #include <frc/shuffleboard/Shuffleboard.h>
@@ -33,13 +32,10 @@
   // Set up default drive command
   m_drive.SetDefaultCommand(frc2::RunCommand(
       [this] {
-        m_drive.Drive(units::meters_per_second_t(
-                          m_driverController.GetY(frc::GenericHID::kLeftHand)),
-                      units::meters_per_second_t(
-                          m_driverController.GetY(frc::GenericHID::kRightHand)),
-                      units::radians_per_second_t(
-                          m_driverController.GetX(frc::GenericHID::kLeftHand)),
-                      false);
+        m_drive.Drive(
+            units::meters_per_second_t(m_driverController.GetLeftY()),
+            units::meters_per_second_t(m_driverController.GetRightY()),
+            units::radians_per_second_t(m_driverController.GetLeftX()), false);
       },
       {&m_drive}));
 }
@@ -68,8 +64,8 @@
       AutoConstants::kPThetaController, 0, 0,
       AutoConstants::kThetaControllerConstraints};
 
-  thetaController.EnableContinuousInput(units::radian_t(-wpi::math::pi),
-                                        units::radian_t(wpi::math::pi));
+  thetaController.EnableContinuousInput(units::radian_t(-wpi::numbers::pi),
+                                        units::radian_t(wpi::numbers::pi));
 
   frc2::SwerveControllerCommand<4> swerveControllerCommand(
       exampleTrajectory, [this]() { return m_drive.GetPose(); },
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
index 37bb83d..7ef4af3 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/DriveSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/DriveSubsystem.h"
 
@@ -68,7 +65,7 @@
 }
 
 void DriveSubsystem::SetModuleStates(
-    std::array<frc::SwerveModuleState, 4> desiredStates) {
+    wpi::array<frc::SwerveModuleState, 4> desiredStates) {
   kDriveKinematics.NormalizeWheelSpeeds(&desiredStates,
                                         AutoConstants::kMaxSpeed);
   m_frontLeft.SetDesiredState(desiredStates[0]);
@@ -88,11 +85,17 @@
   return m_gyro.GetRotation2d().Degrees();
 }
 
-void DriveSubsystem::ZeroHeading() { m_gyro.Reset(); }
+void DriveSubsystem::ZeroHeading() {
+  m_gyro.Reset();
+}
 
-double DriveSubsystem::GetTurnRate() { return -m_gyro.GetRate(); }
+double DriveSubsystem::GetTurnRate() {
+  return -m_gyro.GetRate();
+}
 
-frc::Pose2d DriveSubsystem::GetPose() { return m_odometry.GetPose(); }
+frc::Pose2d DriveSubsystem::GetPose() {
+  return m_odometry.GetPose();
+}
 
 void DriveSubsystem::ResetOdometry(frc::Pose2d pose) {
   m_odometry.ResetPosition(pose,
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
index 22b3f0d..4d20ec8 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/cpp/subsystems/SwerveModule.cpp
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/SwerveModule.h"
 
 #include <frc/geometry/Rotation2d.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "Constants.h"
 
@@ -30,15 +27,15 @@
       ModuleConstants::kDriveEncoderDistancePerPulse);
 
   // Set the distance (in this case, angle) per pulse for the turning encoder.
-  // This is the the angle through an entire rotation (2 * wpi::math::pi)
+  // This is the the angle through an entire rotation (2 * wpi::numbers::pi)
   // divided by the encoder resolution.
   m_turningEncoder.SetDistancePerPulse(
       ModuleConstants::kTurningEncoderDistancePerPulse);
 
   // Limit the PID Controller's input range between -pi and pi and set the input
   // to be continuous.
-  m_turningPIDController.EnableContinuousInput(units::radian_t(-wpi::math::pi),
-                                               units::radian_t(wpi::math::pi));
+  m_turningPIDController.EnableContinuousInput(
+      units::radian_t(-wpi::numbers::pi), units::radian_t(wpi::numbers::pi));
 }
 
 frc::SwerveModuleState SwerveModule::GetState() {
@@ -46,10 +43,15 @@
           frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
 }
 
-void SwerveModule::SetDesiredState(frc::SwerveModuleState& state) {
+void SwerveModule::SetDesiredState(
+    const frc::SwerveModuleState& referenceState) {
+  // Optimize the reference state to avoid spinning further than 90 degrees
+  const auto state = frc::SwerveModuleState::Optimize(
+      referenceState, units::radian_t(m_turningEncoder.Get()));
+
   // Calculate the drive output from the drive PID controller.
   const auto driveOutput = m_drivePIDController.Calculate(
-      m_driveEncoder.GetRate(), state.speed.to<double>());
+      m_driveEncoder.GetRate(), state.speed.value());
 
   // Calculate the turning motor output from the turning PID controller.
   auto turnOutput = m_turningPIDController.Calculate(
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
index 8911b5f..3a3b75a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/geometry/Translation2d.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
@@ -15,7 +12,7 @@
 #include <units/time.h>
 #include <units/velocity.h>
 #include <units/voltage.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #pragma once
 
@@ -42,17 +39,17 @@
 constexpr int kFrontLeftTurningEncoderPorts[2]{0, 1};
 constexpr int kRearLeftTurningEncoderPorts[2]{2, 3};
 constexpr int kFrontRightTurningEncoderPorts[2]{4, 5};
-constexpr int kRearRightTurningEncoderPorts[2]{5, 6};
+constexpr int kRearRightTurningEncoderPorts[2]{6, 7};
 
 constexpr bool kFrontLeftTurningEncoderReversed = false;
 constexpr bool kRearLeftTurningEncoderReversed = true;
 constexpr bool kFrontRightTurningEncoderReversed = false;
 constexpr bool kRearRightTurningEncoderReversed = true;
 
-constexpr int kFrontLeftDriveEncoderPorts[2]{0, 1};
-constexpr int kRearLeftDriveEncoderPorts[2]{2, 3};
-constexpr int kFrontRightDriveEncoderPorts[2]{4, 5};
-constexpr int kRearRightDriveEncoderPorts[2]{5, 6};
+constexpr int kFrontLeftDriveEncoderPorts[2]{8, 9};
+constexpr int kRearLeftDriveEncoderPorts[2]{10, 11};
+constexpr int kFrontRightDriveEncoderPorts[2]{12, 13};
+constexpr int kRearRightDriveEncoderPorts[2]{14, 15};
 
 constexpr bool kFrontLeftDriveEncoderReversed = false;
 constexpr bool kRearLeftDriveEncoderReversed = true;
@@ -77,14 +74,15 @@
 
 namespace ModuleConstants {
 constexpr int kEncoderCPR = 1024;
-constexpr double kWheelDiameterMeters = .15;
+constexpr double kWheelDiameterMeters = 0.15;
 constexpr double kDriveEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (kWheelDiameterMeters * wpi::math::pi) / static_cast<double>(kEncoderCPR);
+    (kWheelDiameterMeters * wpi::numbers::pi) /
+    static_cast<double>(kEncoderCPR);
 
 constexpr double kTurningEncoderDistancePerPulse =
     // Assumes the encoders are directly mounted on the wheel shafts
-    (wpi::math::pi * 2) / static_cast<double>(kEncoderCPR);
+    (wpi::numbers::pi * 2) / static_cast<double>(kEncoderCPR);
 
 constexpr double kPModuleTurningController = 1;
 constexpr double kPModuleDriveController = 1;
@@ -113,5 +111,5 @@
 }  // namespace AutoConstants
 
 namespace OIConstants {
-constexpr int kDriverControllerPort = 1;
+constexpr int kDriverControllerPort = 0;
 }  // namespace OIConstants
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
index 8b36617..3466121 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
index 562af50..5233f3f 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/ADXRS450_Gyro.h>
 #include <frc/Encoder.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/drive/MecanumDrive.h>
 #include <frc/geometry/Pose2d.h>
 #include <frc/geometry/Rotation2d.h>
@@ -17,6 +13,7 @@
 #include <frc/kinematics/ChassisSpeeds.h>
 #include <frc/kinematics/SwerveDriveKinematics.h>
 #include <frc/kinematics/SwerveDriveOdometry.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 #include <frc2/command/SubsystemBase.h>
 
 #include "Constants.h"
@@ -46,7 +43,7 @@
    */
   void Drive(units::meters_per_second_t xSpeed,
              units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
-             bool feildRelative);
+             bool fieldRelative);
 
   /**
    * Resets the drive encoders to currently read a position of 0.
@@ -54,9 +51,9 @@
   void ResetEncoders();
 
   /**
-   * Sets the drive SpeedControllers to a power from -1 to 1.
+   * Sets the drive MotorControllers to a power from -1 to 1.
    */
-  void SetModuleStates(std::array<frc::SwerveModuleState, 4> desiredStates);
+  void SetModuleStates(wpi::array<frc::SwerveModuleState, 4> desiredStates);
 
   /**
    * Returns the heading of the robot.
@@ -92,9 +89,9 @@
   void ResetOdometry(frc::Pose2d pose);
 
   units::meter_t kTrackWidth =
-      .5_m;  // Distance between centers of right and left wheels on robot
+      0.5_m;  // Distance between centers of right and left wheels on robot
   units::meter_t kWheelBase =
-      .7_m;  // Distance between centers of front and back wheels on robot
+      0.7_m;  // Distance between centers of front and back wheels on robot
 
   frc::SwerveDriveKinematics<4> kDriveKinematics{
       frc::Translation2d(kWheelBase / 2, kTrackWidth / 2),
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
index f8d816a..4208b35 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/SwerveModule.h
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <frc/Encoder.h>
-#include <frc/Spark.h>
 #include <frc/controller/PIDController.h>
 #include <frc/controller/ProfiledPIDController.h>
 #include <frc/geometry/Rotation2d.h>
 #include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/Spark.h>
 #include <frc/trajectory/TrapezoidProfile.h>
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "Constants.h"
 
@@ -30,7 +27,7 @@
 
   frc::SwerveModuleState GetState();
 
-  void SetDesiredState(frc::SwerveModuleState& state);
+  void SetDesiredState(const frc::SwerveModuleState& state);
 
   void ResetEncoders();
 
@@ -40,11 +37,11 @@
   // meters per second squared.
 
   static constexpr units::radians_per_second_t kModuleMaxAngularVelocity =
-      units::radians_per_second_t(wpi::math::pi);  // radians per second
+      units::radians_per_second_t(wpi::numbers::pi);  // radians per second
   static constexpr units::unit_t<radians_per_second_squared_t>
       kModuleMaxAngularAcceleration =
           units::unit_t<radians_per_second_squared_t>(
-              wpi::math::pi * 2.0);  // radians per second squared
+              wpi::numbers::pi * 2.0);  // radians per second squared
 
   frc::Spark m_driveMotor;
   frc::Spark m_turningMotor;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
new file mode 100644
index 0000000..f132f76
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Drivetrain.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Drivetrain.h"
+
+#include <frc/Timer.h>
+
+#include "ExampleGlobalMeasurementSensor.h"
+
+void Drivetrain::Drive(units::meters_per_second_t xSpeed,
+                       units::meters_per_second_t ySpeed,
+                       units::radians_per_second_t rot, bool fieldRelative) {
+  auto states = m_kinematics.ToSwerveModuleStates(
+      fieldRelative ? frc::ChassisSpeeds::FromFieldRelativeSpeeds(
+                          xSpeed, ySpeed, rot, m_gyro.GetRotation2d())
+                    : frc::ChassisSpeeds{xSpeed, ySpeed, rot});
+
+  m_kinematics.NormalizeWheelSpeeds(&states, kMaxSpeed);
+
+  auto [fl, fr, bl, br] = states;
+
+  m_frontLeft.SetDesiredState(fl);
+  m_frontRight.SetDesiredState(fr);
+  m_backLeft.SetDesiredState(bl);
+  m_backRight.SetDesiredState(br);
+}
+
+void Drivetrain::UpdateOdometry() {
+  m_poseEstimator.Update(m_gyro.GetRotation2d(), m_frontLeft.GetState(),
+                         m_frontRight.GetState(), m_backLeft.GetState(),
+                         m_backRight.GetState());
+
+  // Also apply vision measurements. We use 0.3 seconds in the past as an
+  // example -- on a real robot, this must be calculated based either on latency
+  // or timestamps.
+  m_poseEstimator.AddVisionMeasurement(
+      ExampleGlobalMeasurementSensor::GetEstimatedGlobalPose(
+          m_poseEstimator.GetEstimatedPosition()),
+      frc::Timer::GetFPGATimestamp() - 0.3_s);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp
new file mode 100644
index 0000000..3f5f675
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/Robot.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/TimedRobot.h>
+#include <frc/XboxController.h>
+#include <frc/filter/SlewRateLimiter.h>
+
+#include "Drivetrain.h"
+
+class Robot : public frc::TimedRobot {
+ public:
+  void AutonomousPeriodic() override {
+    DriveWithJoystick(false);
+    m_swerve.UpdateOdometry();
+  }
+
+  void TeleopPeriodic() override { DriveWithJoystick(true); }
+
+ private:
+  frc::XboxController m_controller{0};
+  Drivetrain m_swerve;
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  frc::SlewRateLimiter<units::scalar> m_xspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_yspeedLimiter{3 / 1_s};
+  frc::SlewRateLimiter<units::scalar> m_rotLimiter{3 / 1_s};
+
+  void DriveWithJoystick(bool fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    const auto xSpeed = -m_xspeedLimiter.Calculate(m_controller.GetLeftY()) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    const auto ySpeed = -m_yspeedLimiter.Calculate(m_controller.GetLeftX()) *
+                        Drivetrain::kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    const auto rot = -m_rotLimiter.Calculate(m_controller.GetRightX()) *
+                     Drivetrain::kMaxAngularSpeed;
+
+    m_swerve.Drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
new file mode 100644
index 0000000..968ccad
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/cpp/SwerveModule.cpp
@@ -0,0 +1,65 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "SwerveModule.h"
+
+#include <frc/geometry/Rotation2d.h>
+#include <wpi/numbers>
+
+SwerveModule::SwerveModule(const int driveMotorChannel,
+                           const int turningMotorChannel,
+                           const int driveEncoderChannelA,
+                           const int driveEncoderChannelB,
+                           const int turningEncoderChannelA,
+                           const int turningEncoderChannelB)
+    : m_driveMotor(driveMotorChannel),
+      m_turningMotor(turningMotorChannel),
+      m_driveEncoder(driveEncoderChannelA, driveEncoderChannelB),
+      m_turningEncoder(turningEncoderChannelA, turningEncoderChannelB) {
+  // Set the distance per pulse for the drive encoder. We can simply use the
+  // distance traveled for one rotation of the wheel divided by the encoder
+  // resolution.
+  m_driveEncoder.SetDistancePerPulse(2 * wpi::numbers::pi *
+                                     kWheelRadius.value() / kEncoderResolution);
+
+  // Set the distance (in this case, angle) per pulse for the turning encoder.
+  // This is the the angle through an entire rotation (2 * wpi::numbers::pi)
+  // divided by the encoder resolution.
+  m_turningEncoder.SetDistancePerPulse(2 * wpi::numbers::pi /
+                                       kEncoderResolution);
+
+  // Limit the PID Controller's input range between -pi and pi and set the input
+  // to be continuous.
+  m_turningPIDController.EnableContinuousInput(
+      -units::radian_t(wpi::numbers::pi), units::radian_t(wpi::numbers::pi));
+}
+
+frc::SwerveModuleState SwerveModule::GetState() const {
+  return {units::meters_per_second_t{m_driveEncoder.GetRate()},
+          frc::Rotation2d(units::radian_t(m_turningEncoder.Get()))};
+}
+
+void SwerveModule::SetDesiredState(
+    const frc::SwerveModuleState& referenceState) {
+  // Optimize the reference state to avoid spinning further than 90 degrees
+  const auto state = frc::SwerveModuleState::Optimize(
+      referenceState, units::radian_t(m_turningEncoder.Get()));
+
+  // Calculate the drive output from the drive PID controller.
+  const auto driveOutput = m_drivePIDController.Calculate(
+      m_driveEncoder.GetRate(), state.speed.value());
+
+  const auto driveFeedforward = m_driveFeedforward.Calculate(state.speed);
+
+  // Calculate the turning motor output from the turning PID controller.
+  const auto turnOutput = m_turningPIDController.Calculate(
+      units::radian_t(m_turningEncoder.Get()), state.angle.Radians());
+
+  const auto turnFeedforward = m_turnFeedforward.Calculate(
+      m_turningPIDController.GetSetpoint().velocity);
+
+  // Set the motor outputs.
+  m_driveMotor.SetVoltage(units::volt_t{driveOutput} + driveFeedforward);
+  m_turningMotor.SetVoltage(units::volt_t{turnOutput} + turnFeedforward);
+}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
new file mode 100644
index 0000000..81f8bb7
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/Drivetrain.h
@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/AnalogGyro.h>
+#include <frc/estimator/SwerveDrivePoseEstimator.h>
+#include <frc/geometry/Translation2d.h>
+#include <frc/kinematics/SwerveDriveKinematics.h>
+#include <frc/kinematics/SwerveDriveOdometry.h>
+#include <wpi/numbers>
+
+#include "SwerveModule.h"
+
+/**
+ * Represents a swerve drive style drivetrain.
+ */
+class Drivetrain {
+ public:
+  Drivetrain() { m_gyro.Reset(); }
+
+  void Drive(units::meters_per_second_t xSpeed,
+             units::meters_per_second_t ySpeed, units::radians_per_second_t rot,
+             bool fieldRelative);
+  void UpdateOdometry();
+
+  static constexpr auto kMaxSpeed = 3.0_mps;  // 3 meters per second
+  static constexpr units::radians_per_second_t kMaxAngularSpeed{
+      wpi::numbers::pi};  // 1/2 rotation per second
+
+ private:
+  frc::Translation2d m_frontLeftLocation{+0.381_m, +0.381_m};
+  frc::Translation2d m_frontRightLocation{+0.381_m, -0.381_m};
+  frc::Translation2d m_backLeftLocation{-0.381_m, +0.381_m};
+  frc::Translation2d m_backRightLocation{-0.381_m, -0.381_m};
+
+  SwerveModule m_frontLeft{1, 2, 0, 1, 2, 3};
+  SwerveModule m_frontRight{2, 3, 4, 5, 6, 7};
+  SwerveModule m_backLeft{5, 6, 8, 9, 10, 11};
+  SwerveModule m_backRight{7, 8, 12, 13, 14, 15};
+
+  frc::AnalogGyro m_gyro{0};
+
+  frc::SwerveDriveKinematics<4> m_kinematics{
+      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation,
+      m_backRightLocation};
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  frc::SwerveDrivePoseEstimator<4> m_poseEstimator{
+      frc::Rotation2d(), frc::Pose2d(), m_kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h
new file mode 100644
index 0000000..a4caff4
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/ExampleGlobalMeasurementSensor.h
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+#include <frc/geometry/Pose2d.h>
+
+/**
+ * This dummy class represents a global measurement sensor, such as a computer
+ * vision solution.
+ */
+class ExampleGlobalMeasurementSensor {
+ public:
+  static frc::Pose2d GetEstimatedGlobalPose(
+      const frc::Pose2d& estimatedRobotPose) {
+    auto randVec = frc::MakeWhiteNoiseVector(0.1, 0.1, 0.1);
+    return frc::Pose2d(estimatedRobotPose.X() + units::meter_t(randVec(0)),
+                       estimatedRobotPose.Y() + units::meter_t(randVec(1)),
+                       estimatedRobotPose.Rotation() +
+                           frc::Rotation2d(units::radian_t(randVec(3))));
+  }
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h
new file mode 100644
index 0000000..7456584
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/SwerveDrivePoseEstimator/include/SwerveModule.h
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/Encoder.h>
+#include <frc/controller/PIDController.h>
+#include <frc/controller/ProfiledPIDController.h>
+#include <frc/controller/SimpleMotorFeedforward.h>
+#include <frc/kinematics/SwerveModuleState.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+#include <units/angular_velocity.h>
+#include <units/time.h>
+#include <units/velocity.h>
+#include <units/voltage.h>
+#include <wpi/numbers>
+
+class SwerveModule {
+ public:
+  SwerveModule(int driveMotorChannel, int turningMotorChannel,
+               int driveEncoderChannelA, int driveEncoderChannelB,
+               int turningEncoderChannelA, int turningEncoderChannelB);
+  frc::SwerveModuleState GetState() const;
+  void SetDesiredState(const frc::SwerveModuleState& state);
+
+ private:
+  static constexpr auto kWheelRadius = 0.0508_m;
+  static constexpr int kEncoderResolution = 4096;
+
+  static constexpr auto kModuleMaxAngularVelocity =
+      wpi::numbers::pi * 1_rad_per_s;  // radians per second
+  static constexpr auto kModuleMaxAngularAcceleration =
+      wpi::numbers::pi * 2_rad_per_s / 1_s;  // radians per second^2
+
+  frc::PWMSparkMax m_driveMotor;
+  frc::PWMSparkMax m_turningMotor;
+
+  frc::Encoder m_driveEncoder;
+  frc::Encoder m_turningEncoder;
+
+  frc2::PIDController m_drivePIDController{1.0, 0, 0};
+  frc::ProfiledPIDController<units::radians> m_turningPIDController{
+      1.0,
+      0.0,
+      0.0,
+      {kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration}};
+
+  frc::SimpleMotorFeedforward<units::meters> m_driveFeedforward{1_V,
+                                                                3_V / 1_mps};
+  frc::SimpleMotorFeedforward<units::radians> m_turnFeedforward{
+      1_V, 0.5_V / 1_rad_per_s};
+};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp
new file mode 100644
index 0000000..c003762
--- /dev/null
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDrive/cpp/Robot.cpp
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/Joystick.h>
+#include <frc/TimedRobot.h>
+#include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
+
+/**
+ * This is a demo program showing the use of the DifferentialDrive class.
+ * Runs the motors with tank steering.
+ */
+class Robot : public frc::TimedRobot {
+  frc::PWMSparkMax m_leftMotor{0};
+  frc::PWMSparkMax m_rightMotor{1};
+  frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
+  frc::Joystick m_leftStick{0};
+  frc::Joystick m_rightStick{1};
+
+ public:
+  void RobotInit() override {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.SetInverted(true);
+  }
+
+  void TeleopPeriodic() override {
+    // Drive with tank style
+    m_robotDrive.TankDrive(m_leftStick.GetY(), m_rightStick.GetY());
+  }
+};
+
+#ifndef RUNNING_FRC_TESTS
+int main() {
+  return frc::StartRobot<Robot>();
+}
+#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
index 3ca31ed..466fdcd 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/TankDriveXboxController/cpp/Robot.cpp
@@ -1,35 +1,39 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <frc/GenericHID.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/XboxController.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a demo program showing the use of the DifferentialDrive class.
  * Runs the motors with tank steering and an Xbox controller.
  */
 class Robot : public frc::TimedRobot {
-  frc::PWMVictorSPX m_leftMotor{0};
-  frc::PWMVictorSPX m_rightMotor{1};
+  frc::PWMSparkMax m_leftMotor{0};
+  frc::PWMSparkMax m_rightMotor{1};
   frc::DifferentialDrive m_robotDrive{m_leftMotor, m_rightMotor};
   frc::XboxController m_driverController{0};
 
  public:
-  void TeleopPeriodic() {
+  void RobotInit() override {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.SetInverted(true);
+  }
+
+  void TeleopPeriodic() override {
     // Drive with tank style
-    m_robotDrive.TankDrive(
-        m_driverController.GetY(frc::GenericHID::JoystickHand::kLeftHand),
-        m_driverController.GetY(frc::GenericHID::JoystickHand::kRightHand));
+    m_robotDrive.TankDrive(m_driverController.GetLeftY(),
+                           m_driverController.GetRightY());
   }
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
index 9f9c01d..e8f805d 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/Ultrasonic/cpp/Robot.cpp
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/AnalogInput.h>
-#include <frc/MedianFilter.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/filter/MedianFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a sample program demonstrating how to use an ultrasonic sensor and
@@ -52,11 +49,13 @@
 
   frc::AnalogInput m_ultrasonic{kUltrasonicPort};
 
-  frc::PWMVictorSPX m_left{kLeftMotorPort};
-  frc::PWMVictorSPX m_right{kRightMotorPort};
+  frc::PWMSparkMax m_left{kLeftMotorPort};
+  frc::PWMSparkMax m_right{kRightMotorPort};
   frc::DifferentialDrive m_robotDrive{m_left, m_right};
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
index 47bd62d..62d4106 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/UltrasonicPID/cpp/Robot.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/AnalogInput.h>
-#include <frc/MedianFilter.h>
-#include <frc/PWMVictorSPX.h>
 #include <frc/TimedRobot.h>
 #include <frc/controller/PIDController.h>
 #include <frc/drive/DifferentialDrive.h>
+#include <frc/filter/MedianFilter.h>
+#include <frc/motorcontrol/PWMSparkMax.h>
 
 /**
  * This is a sample program demonstrating how to use an ultrasonic sensor and
@@ -58,13 +55,15 @@
 
   frc::AnalogInput m_ultrasonic{kUltrasonicPort};
 
-  frc::PWMVictorSPX m_left{kLeftMotorPort};
-  frc::PWMVictorSPX m_right{kRightMotorPort};
+  frc::PWMSparkMax m_left{kLeftMotorPort};
+  frc::PWMSparkMax m_right{kRightMotorPort};
   frc::DifferentialDrive m_robotDrive{m_left, m_right};
 
   frc2::PIDController m_pidController{kP, kI, kD};
 };
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json
index 93bfad4..b639188 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/examples/examples.json
@@ -52,6 +52,14 @@
     "commandversion": 2
   },
   {
+    "name":"Mechanism2d",
+    "foldername":"Mechanism2d",
+    "gradlebase":"cpp",
+    "description":"An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "tags":["Mechanism2d"],
+    "commandversion": 2
+  },
+  {
     "name": "Solenoids",
     "description": "Demonstrate controlling a single and double solenoid from Joystick buttons.",
     "tags": [
@@ -90,6 +98,19 @@
     "commandversion": 2
   },
   {
+    "name": "Tank Drive",
+    "description": "An example program which demonstrates the use of Tank Drive with the DifferentialDrive class",
+    "tags": [
+      "Getting Started with C++",
+      "Robot and Motor",
+      "Joystick",
+      "Complete List"
+    ],
+    "foldername": "TankDrive",
+    "gradlebase": "cpp",
+    "commandversion": 2
+  },
+  {
     "name": "Mecanum Drive",
     "description": "An example program which demonstrates the use of Mecanum Drive with the MecanumDrive class",
     "tags": [
@@ -262,17 +283,6 @@
     "commandversion": 2
   },
   {
-    "name": "PacGoat",
-    "description": "A fully functional example CommandBased program for FRC Team 190&#39;s 2014 robot. This code can run on your computer if it supports simulation.",
-    "tags": [
-      "CommandBased Robot",
-      "Complete List"
-    ],
-    "foldername": "PacGoat",
-    "gradlebase": "cpp",
-    "commandversion": 1
-  },
-  {
     "name": "HAL",
     "description": "A program created using the HAL exclusively. This example is for advanced users",
     "tags": [
@@ -544,6 +554,17 @@
     "commandversion": 2
   },
   {
+    "name": "RomiReference",
+    "description": "An example command-based robot program that can be used with the Romi reference robot design.",
+    "tags": [
+      "Drivetrain",
+      "Romi"
+    ],
+    "foldername": "RomiReference",
+    "gradlebase": "cppromi",
+    "commandversion": 2
+  },
+  {
     "name": "StateSpaceFlywheel",
     "description": "An example state-space controller for a flywheel.",
     "tags": [
@@ -621,7 +642,8 @@
       "Digital",
       "Sensors",
       "Simulation",
-      "Physics"
+      "Physics",
+      "Mechanism2d"
     ],
     "foldername": "ElevatorSimulation",
     "gradlebase": "cpp",
@@ -629,6 +651,38 @@
     "commandversion": 2
   },
   {
+    "name": "DifferentialDrivePoseEstimator",
+    "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose"
+    ],
+    "foldername": "DifferentialDrivePoseEstimator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumDrivePoseEstimator",
+    "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose"
+    ],
+    "foldername": "MecanumDrivePoseEstimator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
     "name": "ArmSimulation",
     "description": "Demonstrates the use of physics simulation with a simple single-jointed arm.",
     "tags": [
@@ -637,7 +691,8 @@
       "Digital",
       "Sensors",
       "Simulation",
-      "Physics"
+      "Physics",
+      "Mechanism2d"
     ],
     "foldername": "ArmSimulation",
     "gradlebase": "cpp",
@@ -645,6 +700,24 @@
     "commandversion": 2
   },
   {
+    "name": "SimpleDifferentialDriveSimulation",
+    "description": "An example of a minimal drivetrain simulation project without the command-based library.",
+    "tags": [
+      "Differential Drive",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Simulation",
+      "Physics",
+      "Drivetrain",
+      "Field2d"
+    ],
+    "foldername": "SimpleDifferentialDriveSimulation",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
     "name": "StateSpaceDriveSimulation",
     "description": "Demonstrates the use of physics simulation with a differential drivetrain and the Field2d class.",
     "tags": [
@@ -661,5 +734,22 @@
     "gradlebase": "cpp",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "SwerveDrivePoseEstimator",
+    "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for mecanum drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "State",
+      "Swerve"
+    ],
+    "foldername": "SwerveDrivePoseEstimator",
+    "gradlebase": "cpp",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
index cd19aeb..8cd7b02 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -20,7 +17,9 @@
  * <p> This runs after the mode specific periodic functions, but before
  * LiveWindow and SmartDashboard integrated updating.
  */
-void Robot::RobotPeriodic() { frc2::CommandScheduler::GetInstance().Run(); }
+void Robot::RobotPeriodic() {
+  frc2::CommandScheduler::GetInstance().Run();
+}
 
 /**
  * This function is called once each time the robot enters Disabled mode. You
@@ -67,5 +66,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp
index 8210645..8ba2094 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/RobotContainer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "RobotContainer.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
index 0e709aa..e551aa1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/commands/ExampleCommand.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "commands/ExampleCommand.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
index 0cfda95..4a31aee 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/cpp/subsystems/ExampleSubsystem.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "subsystems/ExampleSubsystem.h"
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
index 5ac2de0..20cc88a 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
index fa173d3..a82f2ac 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h
index 46609ac..2988968 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/RobotContainer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
index c36477f..8ecd864 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/commands/ExampleCommand.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
index b64c26f..a805151 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/commandbased/include/subsystems/ExampleSubsystem.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.cpp
deleted file mode 100644
index 2a9ef50..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/OI.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 "OI.h"
-
-OI::OI() {
-  // Process operator interface input here.
-}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.cpp
deleted file mode 100644
index b2a8884..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/Robot.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 "Robot.h"
-
-#include <frc/commands/Scheduler.h>
-#include <frc/smartdashboard/SmartDashboard.h>
-
-ExampleSubsystem Robot::m_subsystem;
-OI Robot::m_oi;
-
-void Robot::RobotInit() {
-  m_chooser.SetDefaultOption("Default Auto", &m_defaultAuto);
-  m_chooser.AddOption("My Auto", &m_myAuto);
-  frc::SmartDashboard::PutData("Auto Modes", &m_chooser);
-}
-
-/**
- * This function is called every robot packet, no matter the mode. Use
- * this for items like diagnostics that you want ran during disabled,
- * autonomous, teleoperated and test.
- *
- * <p> This runs after the mode specific periodic functions, but before
- * LiveWindow and SmartDashboard integrated updating.
- */
-void Robot::RobotPeriodic() {}
-
-/**
- * This function is called once each time the robot enters Disabled mode. You
- * can use it to reset any subsystem information you want to clear when the
- * robot is disabled.
- */
-void Robot::DisabledInit() {}
-
-void Robot::DisabledPeriodic() { frc::Scheduler::GetInstance()->Run(); }
-
-/**
- * This autonomous (along with the chooser code above) shows how to select
- * between different autonomous modes using the dashboard. The sendable chooser
- * code works with the Java SmartDashboard. If you prefer the LabVIEW Dashboard,
- * remove all of the chooser code and uncomment the GetString code to get the
- * auto name from the text box below the Gyro.
- *
- * You can add additional auto modes by adding additional commands to the
- * chooser code above (like the commented example) or additional comparisons to
- * the if-else structure below with additional strings & commands.
- */
-void Robot::AutonomousInit() {
-  // std::string autoSelected = frc::SmartDashboard::GetString(
-  //     "Auto Selector", "Default");
-  // if (autoSelected == "My Auto") {
-  //   m_autonomousCommand = &m_myAuto;
-  // } else {
-  //   m_autonomousCommand = &m_defaultAuto;
-  // }
-
-  m_autonomousCommand = m_chooser.GetSelected();
-
-  if (m_autonomousCommand != nullptr) {
-    m_autonomousCommand->Start();
-  }
-}
-
-void Robot::AutonomousPeriodic() { frc::Scheduler::GetInstance()->Run(); }
-
-void Robot::TeleopInit() {
-  // This makes sure that the autonomous stops running when
-  // teleop starts running. If you want the autonomous to
-  // continue until interrupted by another command, remove
-  // this line or comment it out.
-  if (m_autonomousCommand != nullptr) {
-    m_autonomousCommand->Cancel();
-    m_autonomousCommand = nullptr;
-  }
-}
-
-void Robot::TeleopPeriodic() { frc::Scheduler::GetInstance()->Run(); }
-
-void Robot::TestPeriodic() {}
-
-#ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
-#endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.cpp
deleted file mode 100644
index a5596ef..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/ExampleCommand.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 "commands/ExampleCommand.h"
-
-#include "Robot.h"
-
-ExampleCommand::ExampleCommand() {
-  // Use Requires() here to declare subsystem dependencies
-  Requires(&Robot::m_subsystem);
-}
-
-// Called just before this Command runs the first time
-void ExampleCommand::Initialize() {}
-
-// Called repeatedly when this Command is scheduled to run
-void ExampleCommand::Execute() {}
-
-// Make this return true when this Command no longer needs to run execute()
-bool ExampleCommand::IsFinished() { return false; }
-
-// Called once after isFinished returns true
-void ExampleCommand::End() {}
-
-// Called when another command which requires one or more of the same
-// subsystems is scheduled to run
-void ExampleCommand::Interrupted() {}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.cpp
deleted file mode 100644
index 1389447..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/commands/MyAutoCommand.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 "commands/MyAutoCommand.h"
-
-#include "Robot.h"
-
-MyAutoCommand::MyAutoCommand() {
-  // Use Requires() here to declare subsystem dependencies
-  Requires(&Robot::m_subsystem);
-}
-
-// Called just before this Command runs the first time
-void MyAutoCommand::Initialize() {}
-
-// Called repeatedly when this Command is scheduled to run
-void MyAutoCommand::Execute() {}
-
-// Make this return true when this Command no longer needs to run execute()
-bool MyAutoCommand::IsFinished() { return false; }
-
-// Called once after isFinished returns true
-void MyAutoCommand::End() {}
-
-// Called when another command which requires one or more of the same
-// subsystems is scheduled to run
-void MyAutoCommand::Interrupted() {}
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.cpp
deleted file mode 100644
index ed60550..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/cpp/subsystems/ExampleSubsystem.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 "subsystems/ExampleSubsystem.h"
-
-#include "RobotMap.h"
-
-ExampleSubsystem::ExampleSubsystem() : frc::Subsystem("ExampleSubsystem") {}
-
-void ExampleSubsystem::InitDefaultCommand() {
-  // Set the default command for a subsystem here.
-  // SetDefaultCommand(new MySpecialCommand());
-}
-
-// Put methods for controlling this subsystem
-// here. Call these from Commands.
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
deleted file mode 100644
index 77007bb..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/OI.h
+++ /dev/null
@@ -1,13 +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
-
-class OI {
- public:
-  OI();
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
deleted file mode 100644
index 94c2314..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/Robot.h
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/TimedRobot.h>
-#include <frc/commands/Command.h>
-#include <frc/smartdashboard/SendableChooser.h>
-
-#include "OI.h"
-#include "commands/ExampleCommand.h"
-#include "commands/MyAutoCommand.h"
-#include "subsystems/ExampleSubsystem.h"
-
-class Robot : public frc::TimedRobot {
- public:
-  static ExampleSubsystem m_subsystem;
-  static OI m_oi;
-
-  void RobotInit() override;
-  void RobotPeriodic() override;
-  void DisabledInit() override;
-  void DisabledPeriodic() override;
-  void AutonomousInit() override;
-  void AutonomousPeriodic() override;
-  void TeleopInit() override;
-  void TeleopPeriodic() override;
-  void TestPeriodic() override;
-
- private:
-  // Have it null by default so that if testing teleop it
-  // doesn't have undefined behavior and potentially crash.
-  frc::Command* m_autonomousCommand = nullptr;
-  ExampleCommand m_defaultAuto;
-  MyAutoCommand m_myAuto;
-  frc::SendableChooser<frc::Command*> m_chooser;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
deleted file mode 100644
index 875d3aa..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/RobotMap.h
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-/**
- * The RobotMap is a mapping from the ports sensors and actuators are wired into
- * to a variable name. This provides flexibility changing wiring, makes checking
- * the wiring easier and significantly reduces the number of magic numbers
- * floating around.
- */
-
-// For example to map the left and right motors, you could define the
-// following variables to use with your drivetrain subsystem.
-// constexpr int kLeftMotor = 1;
-// constexpr int kRightMotor = 2;
-
-// If you are using multiple modules, make sure to define both the port
-// number and the module. For example you with a rangefinder:
-// constexpr int kRangeFinderPort = 1;
-// constexpr int kRangeFinderModule = 1;
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
deleted file mode 100644
index 558fced..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/ExampleCommand.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-class ExampleCommand : public frc::Command {
- public:
-  ExampleCommand();
-  void Initialize() override;
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-  void Interrupted() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
deleted file mode 100644
index f1892a7..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/commands/MyAutoCommand.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Command.h>
-
-class MyAutoCommand : public frc::Command {
- public:
-  MyAutoCommand();
-  void Initialize() override;
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-  void Interrupted() override;
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
deleted file mode 100644
index 0f7d470..0000000
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/oldcommandbased/include/subsystems/ExampleSubsystem.h
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <frc/commands/Subsystem.h>
-
-class ExampleSubsystem : public frc::Subsystem {
- public:
-  ExampleSubsystem();
-  void InitDefaultCommand() override;
-
- private:
-  // It's desirable that everything possible under private except
-  // for methods that implement subsystem capabilities
-};
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
index 4bfae1f..29bae0b 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -24,8 +21,6 @@
 void Robot::Test() {}
 
 void Robot::StartCompetition() {
-  auto& lw = *frc::LiveWindow::GetInstance();
-
   RobotInit();
 
   // Tell the DS that the robot is ready to be enabled
@@ -33,35 +28,47 @@
 
   while (!m_exit) {
     if (IsDisabled()) {
-      m_ds.InDisabled(true);
+      frc::DriverStation::InDisabled(true);
       Disabled();
-      m_ds.InDisabled(false);
-      while (IsDisabled()) m_ds.WaitForData();
+      frc::DriverStation::InDisabled(false);
+      while (IsDisabled()) {
+        frc::DriverStation::WaitForData();
+      }
     } else if (IsAutonomous()) {
-      m_ds.InAutonomous(true);
+      frc::DriverStation::InAutonomous(true);
       Autonomous();
-      m_ds.InAutonomous(false);
-      while (IsAutonomousEnabled()) m_ds.WaitForData();
+      frc::DriverStation::InAutonomous(false);
+      while (IsAutonomousEnabled()) {
+        frc::DriverStation::WaitForData();
+      }
     } else if (IsTest()) {
-      lw.SetEnabled(true);
+      frc::LiveWindow::SetEnabled(true);
       frc::Shuffleboard::EnableActuatorWidgets();
-      m_ds.InTest(true);
+      frc::DriverStation::InTest(true);
       Test();
-      m_ds.InTest(false);
-      while (IsTest() && IsEnabled()) m_ds.WaitForData();
-      lw.SetEnabled(false);
+      frc::DriverStation::InTest(false);
+      while (IsTest() && IsEnabled()) {
+        frc::DriverStation::WaitForData();
+      }
+      frc::LiveWindow::SetEnabled(false);
       frc::Shuffleboard::DisableActuatorWidgets();
     } else {
-      m_ds.InOperatorControl(true);
+      frc::DriverStation::InTeleop(true);
       Teleop();
-      m_ds.InOperatorControl(false);
-      while (IsOperatorControlEnabled()) m_ds.WaitForData();
+      frc::DriverStation::InTeleop(false);
+      while (IsTeleopEnabled()) {
+        frc::DriverStation::WaitForData();
+      }
     }
   }
 }
 
-void Robot::EndCompetition() { m_exit = true; }
+void Robot::EndCompetition() {
+  m_exit = true;
+}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
index 0057778..9b63fb2 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/robotbaseskeleton/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/templates.json b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/templates.json
index c31d41c..59d4084 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/templates.json
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/templates.json
@@ -7,7 +7,7 @@
     ],
     "foldername": "timed",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Timed Skeleton (Advanced)",
@@ -17,7 +17,7 @@
     ],
     "foldername": "timedskeleton",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "RobotBase Skeleton (Advanced)",
@@ -27,7 +27,7 @@
     ],
     "foldername": "robotbaseskeleton",
     "gradlebase": "cpp",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Command Robot",
@@ -40,13 +40,23 @@
     "commandversion": 2
   },
   {
-    "name": "Old Command Robot",
-    "description": "Old-Command style (deprecated)",
+    "name": "Romi - Timed Robot",
+    "description": "Romi - Timed style",
     "tags": [
-      "Command"
+      "Timed", "Romi"
     ],
-    "foldername": "oldcommandbased",
-    "gradlebase": "cpp",
-    "commandversion": 1
+    "foldername": "timed",
+    "gradlebase": "cppromi",
+    "commandversion": 2
+  },
+  {
+    "name": "Romi - Command Robot",
+    "description": "Romi - Command style",
+    "tags": [
+      "Command", "Romi"
+    ],
+    "foldername": "commandbased",
+    "gradlebase": "cppromi",
+    "commandversion": 2
   }
 ]
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
index dfb6993..577de89 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/cpp/Robot.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
-#include <iostream>
+#include <fmt/core.h>
 
 #include <frc/smartdashboard/SmartDashboard.h>
 
@@ -42,7 +39,7 @@
   m_autoSelected = m_chooser.GetSelected();
   // m_autoSelected = SmartDashboard::GetString("Auto Selector",
   //     kAutoNameDefault);
-  std::cout << "Auto selected: " << m_autoSelected << std::endl;
+  fmt::print("Auto selected: {}\n", m_autoSelected);
 
   if (m_autoSelected == kAutoNameCustom) {
     // Custom Auto goes here
@@ -72,5 +69,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
index fe4ea1f..b887605 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timed/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
index 425fa84..92f5df1 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/cpp/Robot.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Robot.h"
 
@@ -23,5 +20,7 @@
 void Robot::TestPeriodic() {}
 
 #ifndef RUNNING_FRC_TESTS
-int main() { return frc::StartRobot<Robot>(); }
+int main() {
+  return frc::StartRobot<Robot>();
+}
 #endif
diff --git a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
index d4bcbb6..99f84f7 100644
--- a/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
+++ b/third_party/allwpilib/wpilibcExamples/src/main/cpp/templates/timedskeleton/include/Robot.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/build.gradle b/third_party/allwpilib/wpilibcIntegrationTests/build.gradle
index 81f827b..4d66667 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/build.gradle
+++ b/third_party/allwpilib/wpilibcIntegrationTests/build.gradle
@@ -56,7 +56,7 @@
                     lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
                     lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
                     if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                        nativeUtils.useRequiredLibrary(binary, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                        nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
                     }
                 } else {
                     binary.sources {
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogLoopTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogLoopTest.cpp
index 40fd87c..3540095 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogLoopTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogLoopTest.cpp
@@ -1,53 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <units/time.h>
 
 #include "TestBench.h"
 #include "frc/AnalogInput.h"
 #include "frc/AnalogOutput.h"
 #include "frc/AnalogTrigger.h"
+#include "frc/AsynchronousInterrupt.h"
 #include "frc/Counter.h"
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
-static const double kDelayTime = 0.01;
-
-/**
- * A fixture with an analog input and an analog output wired together
- */
-class AnalogLoopTest : public testing::Test {
- protected:
-  AnalogInput* m_input;
-  AnalogOutput* m_output;
-
-  void SetUp() override {
-    m_input = new AnalogInput(TestBench::kFakeAnalogOutputChannel);
-    m_output = new AnalogOutput(TestBench::kAnalogOutputChannel);
-  }
-
-  void TearDown() override {
-    delete m_input;
-    delete m_output;
-  }
-};
+static constexpr auto kDelayTime = 10_ms;
 
 /**
  * Test analog inputs and outputs by setting one and making sure the other
  * matches.
  */
-TEST_F(AnalogLoopTest, AnalogInputWorks) {
+TEST(AnalogLoopTest, AnalogInputWorks) {
+  frc::AnalogInput input{TestBench::kFakeAnalogOutputChannel};
+  frc::AnalogOutput output{TestBench::kAnalogOutputChannel};
+
   // Set the output voltage and check if the input measures the same voltage
   for (int32_t i = 0; i < 50; i++) {
-    m_output->SetVoltage(i / 10.0);
+    output.SetVoltage(i / 10.0);
 
-    Wait(kDelayTime);
+    frc::Wait(kDelayTime);
 
-    EXPECT_NEAR(m_output->GetVoltage(), m_input->GetVoltage(), 0.01);
+    EXPECT_NEAR(output.GetVoltage(), input.GetVoltage(), 0.01);
   }
 }
 
@@ -55,26 +37,29 @@
  * Test if we can use an analog trigger to  check if the output is within a
  * range correctly.
  */
-TEST_F(AnalogLoopTest, AnalogTriggerWorks) {
-  AnalogTrigger trigger(m_input);
+TEST(AnalogLoopTest, AnalogTriggerWorks) {
+  frc::AnalogInput input{TestBench::kFakeAnalogOutputChannel};
+  frc::AnalogOutput output{TestBench::kAnalogOutputChannel};
+
+  frc::AnalogTrigger trigger{&input};
   trigger.SetLimitsVoltage(2.0, 3.0);
 
-  m_output->SetVoltage(1.0);
-  Wait(kDelayTime);
+  output.SetVoltage(1.0);
+  frc::Wait(kDelayTime);
 
   EXPECT_FALSE(trigger.GetInWindow())
       << "Analog trigger is in the window (2V, 3V)";
   EXPECT_FALSE(trigger.GetTriggerState()) << "Analog trigger is on";
 
-  m_output->SetVoltage(2.5);
-  Wait(kDelayTime);
+  output.SetVoltage(2.5);
+  frc::Wait(kDelayTime);
 
   EXPECT_TRUE(trigger.GetInWindow())
       << "Analog trigger is not in the window (2V, 3V)";
   EXPECT_FALSE(trigger.GetTriggerState()) << "Analog trigger is on";
 
-  m_output->SetVoltage(4.0);
-  Wait(kDelayTime);
+  output.SetVoltage(4.0);
+  frc::Wait(kDelayTime);
 
   EXPECT_FALSE(trigger.GetInWindow())
       << "Analog trigger is in the window (2V, 3V)";
@@ -85,18 +70,21 @@
  * Test if we can count the right number of ticks from an analog trigger with
  * a counter.
  */
-TEST_F(AnalogLoopTest, AnalogTriggerCounterWorks) {
-  AnalogTrigger trigger(m_input);
+TEST(AnalogLoopTest, AnalogTriggerCounterWorks) {
+  frc::AnalogInput input{TestBench::kFakeAnalogOutputChannel};
+  frc::AnalogOutput output{TestBench::kAnalogOutputChannel};
+
+  frc::AnalogTrigger trigger{&input};
   trigger.SetLimitsVoltage(2.0, 3.0);
 
-  Counter counter(trigger);
+  frc::Counter counter{trigger};
 
   // Turn the analog output low and high 50 times
   for (int32_t i = 0; i < 50; i++) {
-    m_output->SetVoltage(1.0);
-    Wait(kDelayTime);
-    m_output->SetVoltage(4.0);
-    Wait(kDelayTime);
+    output.SetVoltage(1.0);
+    frc::Wait(kDelayTime);
+    output.SetVoltage(4.0);
+    frc::Wait(kDelayTime);
   }
 
   // The counter should be 50
@@ -104,28 +92,30 @@
       << "Analog trigger counter did not count 50 ticks";
 }
 
-static void InterruptHandler(uint32_t interruptAssertedMask, void* param) {
-  *reinterpret_cast<int32_t*>(param) = 12345;
-}
+TEST(AnalogLoopTest, AsynchronusInterruptWorks) {
+  frc::AnalogInput input{TestBench::kFakeAnalogOutputChannel};
+  frc::AnalogOutput output{TestBench::kAnalogOutputChannel};
 
-TEST_F(AnalogLoopTest, AsynchronusInterruptWorks) {
   int32_t param = 0;
-  AnalogTrigger trigger(m_input);
+  frc::AnalogTrigger trigger{&input};
   trigger.SetLimitsVoltage(2.0, 3.0);
 
   // Given an interrupt handler that sets an int32_t to 12345
-  std::shared_ptr<AnalogTriggerOutput> triggerOutput =
-      trigger.CreateOutput(AnalogTriggerType::kState);
-  triggerOutput->RequestInterrupts(InterruptHandler, &param);
-  triggerOutput->EnableInterrupts();
+  std::shared_ptr<frc::AnalogTriggerOutput> triggerOutput =
+      trigger.CreateOutput(frc::AnalogTriggerType::kState);
+
+  frc::AsynchronousInterrupt interrupt{triggerOutput,
+                                       [&](auto a, auto b) { param = 12345; }};
+
+  interrupt.Enable();
 
   // If the analog output moves from below to above the window
-  m_output->SetVoltage(0.0);
-  Wait(kDelayTime);
-  m_output->SetVoltage(5.0);
-  triggerOutput->CancelInterrupts();
-
+  output.SetVoltage(0.0);
+  frc::Wait(kDelayTime);
+  output.SetVoltage(5.0);
   // Then the int32_t should be 12345
-  Wait(kDelayTime);
+  frc::Wait(kDelayTime);
+  interrupt.Disable();
+
   EXPECT_EQ(12345, param) << "The interrupt did not run.";
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogPotentiometerTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogPotentiometerTest.cpp
index 4497051..107310f 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogPotentiometerTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/AnalogPotentiometerTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/AnalogPotentiometer.h"  // NOLINT(build/include_order)
 
@@ -13,38 +10,25 @@
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
+static constexpr double kScale = 270.0;
+static constexpr double kAngle = 180.0;
 
-static const double kScale = 270.0;
-static const double kAngle = 180.0;
+TEST(AnalogPotentiometerTest, InitialSettings) {
+  frc::AnalogOutput m_fakePot{TestBench::kAnalogOutputChannel};
+  frc::AnalogPotentiometer m_pot{TestBench::kFakeAnalogOutputChannel, kScale};
 
-class AnalogPotentiometerTest : public testing::Test {
- protected:
-  AnalogOutput* m_fakePot;
-  AnalogPotentiometer* m_pot;
-
-  void SetUp() override {
-    m_fakePot = new AnalogOutput(TestBench::kAnalogOutputChannel);
-    m_pot =
-        new AnalogPotentiometer(TestBench::kFakeAnalogOutputChannel, kScale);
-  }
-
-  void TearDown() override {
-    delete m_fakePot;
-    delete m_pot;
-  }
-};
-
-TEST_F(AnalogPotentiometerTest, TestInitialSettings) {
-  m_fakePot->SetVoltage(0.0);
-  Wait(0.1);
-  EXPECT_NEAR(0.0, m_pot->Get(), 5.0)
+  m_fakePot.SetVoltage(0.0);
+  frc::Wait(100_ms);
+  EXPECT_NEAR(0.0, m_pot.Get(), 5.0)
       << "The potentiometer did not initialize to 0.";
 }
 
-TEST_F(AnalogPotentiometerTest, TestRangeValues) {
-  m_fakePot->SetVoltage(kAngle / kScale * RobotController::GetVoltage5V());
-  Wait(0.1);
-  EXPECT_NEAR(kAngle, m_pot->Get(), 2.0)
+TEST(AnalogPotentiometerTest, RangeValues) {
+  frc::AnalogOutput m_fakePot{TestBench::kAnalogOutputChannel};
+  frc::AnalogPotentiometer m_pot{TestBench::kFakeAnalogOutputChannel, kScale};
+
+  m_fakePot.SetVoltage(kAngle / kScale * frc::RobotController::GetVoltage5V());
+  frc::Wait(100_ms);
+  EXPECT_NEAR(kAngle, m_pot.Get(), 2.0)
       << "The potentiometer did not measure the correct angle.";
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/BuiltInAccelerometerTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/BuiltInAccelerometerTest.cpp
index bfb5f21..00f5f00 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/BuiltInAccelerometerTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/BuiltInAccelerometerTest.cpp
@@ -1,28 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/BuiltInAccelerometer.h"  // NOLINT(build/include_order)
 
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 static constexpr double kAccelerationTolerance = 0.1;
+
 /**
  * There's not much we can automatically test with the on-board accelerometer,
  * but checking for gravity is probably good enough to tell that it's working.
  */
 TEST(BuiltInAccelerometerTest, Accelerometer) {
-  BuiltInAccelerometer accelerometer;
+  frc::BuiltInAccelerometer accelerometer;
 
-  /* The testbench sometimes shakes a little from a previous test.  Give it
-          some time. */
-  Wait(1.0);
+  // The testbench sometimes shakes a little from a previous test. Give it some
+  // time.
+  frc::Wait(1_s);
 
   ASSERT_NEAR(0.0, accelerometer.GetX(), kAccelerationTolerance);
   ASSERT_NEAR(1.0, accelerometer.GetY(), kAccelerationTolerance);
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/CounterTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/CounterTest.cpp
index a3ddbd2..553fc96 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/CounterTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/CounterTest.cpp
@@ -1,59 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Counter.h"  // NOLINT(build/include_order)
 
+#include <units/time.h>
+
 #include "TestBench.h"
-#include "frc/Jaguar.h"
-#include "frc/Talon.h"
 #include "frc/Timer.h"
-#include "frc/Victor.h"
+#include "frc/motorcontrol/Jaguar.h"
+#include "frc/motorcontrol/Talon.h"
+#include "frc/motorcontrol/Victor.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
+static constexpr auto kMotorDelay = 2.5_s;
 
-static const double kMotorDelay = 2.5;
-
-static const double kMaxPeriod = 2.0;
+static constexpr auto kMaxPeriod = 2_s;
 
 class CounterTest : public testing::Test {
  protected:
-  Counter* m_talonCounter;
-  Counter* m_victorCounter;
-  Counter* m_jaguarCounter;
-  Talon* m_talon;
-  Victor* m_victor;
-  Jaguar* m_jaguar;
-
-  void SetUp() override {
-    m_talonCounter = new Counter(TestBench::kTalonEncoderChannelA);
-    m_victorCounter = new Counter(TestBench::kVictorEncoderChannelA);
-    m_jaguarCounter = new Counter(TestBench::kJaguarEncoderChannelA);
-    m_victor = new Victor(TestBench::kVictorChannel);
-    m_talon = new Talon(TestBench::kTalonChannel);
-    m_jaguar = new Jaguar(TestBench::kJaguarChannel);
-  }
-
-  void TearDown() override {
-    delete m_talonCounter;
-    delete m_victorCounter;
-    delete m_jaguarCounter;
-    delete m_victor;
-    delete m_talon;
-    delete m_jaguar;
-  }
+  frc::Counter m_talonCounter{TestBench::kTalonEncoderChannelA};
+  frc::Counter m_victorCounter{TestBench::kVictorEncoderChannelA};
+  frc::Counter m_jaguarCounter{TestBench::kJaguarEncoderChannelA};
+  frc::Talon m_talon{TestBench::kVictorChannel};
+  frc::Victor m_victor{TestBench::kTalonChannel};
+  frc::Jaguar m_jaguar{TestBench::kJaguarChannel};
 
   void Reset() {
-    m_talonCounter->Reset();
-    m_victorCounter->Reset();
-    m_jaguarCounter->Reset();
-    m_talon->Set(0.0);
-    m_victor->Set(0.0);
-    m_jaguar->Set(0.0);
+    m_talonCounter.Reset();
+    m_victorCounter.Reset();
+    m_jaguarCounter.Reset();
+    m_talon.Set(0.0);
+    m_victor.Set(0.0);
+    m_jaguar.Set(0.0);
   }
 };
 
@@ -65,17 +44,17 @@
   Reset();
 
   /* Run the motor forward and determine if the counter is counting. */
-  m_talon->Set(1.0);
-  Wait(0.5);
+  m_talon.Set(1.0);
+  frc::Wait(0.5_s);
 
-  EXPECT_NE(0.0, m_talonCounter->Get()) << "The counter did not count (talon)";
+  EXPECT_NE(0.0, m_talonCounter.Get()) << "The counter did not count (talon)";
 
   /* Set the motor to 0 and determine if the counter resets to 0. */
-  m_talon->Set(0.0);
-  Wait(0.5);
-  m_talonCounter->Reset();
+  m_talon.Set(0.0);
+  frc::Wait(0.5_s);
+  m_talonCounter.Reset();
 
-  EXPECT_FLOAT_EQ(0.0, m_talonCounter->Get())
+  EXPECT_FLOAT_EQ(0.0, m_talonCounter.Get())
       << "The counter did not restart to 0 (talon)";
 }
 
@@ -83,18 +62,17 @@
   Reset();
 
   /* Run the motor forward and determine if the counter is counting. */
-  m_victor->Set(1.0);
-  Wait(0.5);
+  m_victor.Set(1.0);
+  frc::Wait(0.5_s);
 
-  EXPECT_NE(0.0, m_victorCounter->Get())
-      << "The counter did not count (victor)";
+  EXPECT_NE(0.0, m_victorCounter.Get()) << "The counter did not count (victor)";
 
   /* Set the motor to 0 and determine if the counter resets to 0. */
-  m_victor->Set(0.0);
-  Wait(0.5);
-  m_victorCounter->Reset();
+  m_victor.Set(0.0);
+  frc::Wait(0.5_s);
+  m_victorCounter.Reset();
 
-  EXPECT_FLOAT_EQ(0.0, m_victorCounter->Get())
+  EXPECT_FLOAT_EQ(0.0, m_victorCounter.Get())
       << "The counter did not restart to 0 (jaguar)";
 }
 
@@ -102,18 +80,17 @@
   Reset();
 
   /* Run the motor forward and determine if the counter is counting. */
-  m_jaguar->Set(1.0);
-  Wait(0.5);
+  m_jaguar.Set(1.0);
+  frc::Wait(0.5_s);
 
-  EXPECT_NE(0.0, m_jaguarCounter->Get())
-      << "The counter did not count (jaguar)";
+  EXPECT_NE(0.0, m_jaguarCounter.Get()) << "The counter did not count (jaguar)";
 
   /* Set the motor to 0 and determine if the counter resets to 0. */
-  m_jaguar->Set(0.0);
-  Wait(0.5);
-  m_jaguarCounter->Reset();
+  m_jaguar.Set(0.0);
+  frc::Wait(0.5_s);
+  m_jaguarCounter.Reset();
 
-  EXPECT_FLOAT_EQ(0.0, m_jaguarCounter->Get())
+  EXPECT_FLOAT_EQ(0.0, m_jaguarCounter.Get())
       << "The counter did not restart to 0 (jaguar)";
 }
 
@@ -125,17 +102,17 @@
   Reset();
 
   /* Set the Max Period of the counter and run the motor */
-  m_talonCounter->SetMaxPeriod(kMaxPeriod);
-  m_talon->Set(1.0);
-  Wait(0.5);
+  m_talonCounter.SetMaxPeriod(kMaxPeriod);
+  m_talon.Set(1.0);
+  frc::Wait(0.5_s);
 
-  EXPECT_FALSE(m_talonCounter->GetStopped()) << "The counter did not count.";
+  EXPECT_FALSE(m_talonCounter.GetStopped()) << "The counter did not count.";
 
   /* Stop the motor and wait until the Max Period is exceeded */
-  m_talon->Set(0.0);
-  Wait(kMotorDelay);
+  m_talon.Set(0.0);
+  frc::Wait(kMotorDelay);
 
-  EXPECT_TRUE(m_talonCounter->GetStopped())
+  EXPECT_TRUE(m_talonCounter.GetStopped())
       << "The counter did not stop counting.";
 }
 
@@ -143,17 +120,17 @@
   Reset();
 
   /* Set the Max Period of the counter and run the motor */
-  m_victorCounter->SetMaxPeriod(kMaxPeriod);
-  m_victor->Set(1.0);
-  Wait(0.5);
+  m_victorCounter.SetMaxPeriod(kMaxPeriod);
+  m_victor.Set(1.0);
+  frc::Wait(0.5_s);
 
-  EXPECT_FALSE(m_victorCounter->GetStopped()) << "The counter did not count.";
+  EXPECT_FALSE(m_victorCounter.GetStopped()) << "The counter did not count.";
 
   /* Stop the motor and wait until the Max Period is exceeded */
-  m_victor->Set(0.0);
-  Wait(kMotorDelay);
+  m_victor.Set(0.0);
+  frc::Wait(kMotorDelay);
 
-  EXPECT_TRUE(m_victorCounter->GetStopped())
+  EXPECT_TRUE(m_victorCounter.GetStopped())
       << "The counter did not stop counting.";
 }
 
@@ -161,16 +138,16 @@
   Reset();
 
   /* Set the Max Period of the counter and run the motor */
-  m_jaguarCounter->SetMaxPeriod(kMaxPeriod);
-  m_jaguar->Set(1.0);
-  Wait(0.5);
+  m_jaguarCounter.SetMaxPeriod(kMaxPeriod);
+  m_jaguar.Set(1.0);
+  frc::Wait(0.5_s);
 
-  EXPECT_FALSE(m_jaguarCounter->GetStopped()) << "The counter did not count.";
+  EXPECT_FALSE(m_jaguarCounter.GetStopped()) << "The counter did not count.";
 
   /* Stop the motor and wait until the Max Period is exceeded */
-  m_jaguar->Set(0.0);
-  Wait(kMotorDelay);
+  m_jaguar.Set(0.0);
+  frc::Wait(kMotorDelay);
 
-  EXPECT_TRUE(m_jaguarCounter->GetStopped())
+  EXPECT_TRUE(m_jaguarCounter.GetStopped())
       << "The counter did not stop counting.";
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp
index 10b64d5..e230310 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DIOLoopTest.cpp
@@ -1,28 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DigitalInput.h"  // NOLINT(build/include_order)
 
 #include "frc/DigitalOutput.h"  // NOLINT(build/include_order)
 
+#include <units/math.h>
+#include <units/time.h>
+
 #include "TestBench.h"
+#include "frc/AsynchronousInterrupt.h"
 #include "frc/Counter.h"
-#include "frc/InterruptableSensorBase.h"
+#include "frc/SynchronousInterrupt.h"
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
 
-static const double kCounterTime = 0.001;
+static constexpr auto kCounterTime = 1_ms;
 
-static const double kDelayTime = 0.1;
+static constexpr auto kDelayTime = 100_ms;
 
-static const double kSynchronousInterruptTime = 2.0;
-static const double kSynchronousInterruptTimeTolerance = 0.01;
+static constexpr auto kSynchronousInterruptTime = 2_s;
+static constexpr auto kSynchronousInterruptTimeTolerance = 10_ms;
 
 /**
  * A fixture with a digital input and a digital output physically wired
@@ -30,20 +32,10 @@
  */
 class DIOLoopTest : public testing::Test {
  protected:
-  DigitalInput* m_input;
-  DigitalOutput* m_output;
+  frc::DigitalInput m_input{TestBench::kLoop1InputChannel};
+  frc::DigitalOutput m_output{TestBench::kLoop1OutputChannel};
 
-  void SetUp() override {
-    m_input = new DigitalInput(TestBench::kLoop1InputChannel);
-    m_output = new DigitalOutput(TestBench::kLoop1OutputChannel);
-  }
-
-  void TearDown() override {
-    delete m_input;
-    delete m_output;
-  }
-
-  void Reset() { m_output->Set(false); }
+  void Reset() { m_output.Set(false); }
 };
 
 /**
@@ -53,15 +45,15 @@
 TEST_F(DIOLoopTest, Loop) {
   Reset();
 
-  m_output->Set(false);
-  Wait(kDelayTime);
-  EXPECT_FALSE(m_input->Get()) << "The digital output was turned off, but "
-                               << "the digital input is on.";
+  m_output.Set(false);
+  frc::Wait(kDelayTime);
+  EXPECT_FALSE(m_input.Get()) << "The digital output was turned off, but "
+                              << "the digital input is on.";
 
-  m_output->Set(true);
-  Wait(kDelayTime);
-  EXPECT_TRUE(m_input->Get()) << "The digital output was turned on, but "
-                              << "the digital input is off.";
+  m_output.Set(true);
+  frc::Wait(kDelayTime);
+  EXPECT_TRUE(m_input.Get()) << "The digital output was turned on, but "
+                             << "the digital input is off.";
 }
 /**
  * Tests to see if the DIO PWM functionality works.
@@ -69,43 +61,43 @@
 TEST_F(DIOLoopTest, DIOPWM) {
   Reset();
 
-  m_output->Set(false);
-  Wait(kDelayTime);
-  EXPECT_FALSE(m_input->Get()) << "The digital output was turned off, but "
-                               << "the digital input is on.";
+  m_output.Set(false);
+  frc::Wait(kDelayTime);
+  EXPECT_FALSE(m_input.Get()) << "The digital output was turned off, but "
+                              << "the digital input is on.";
 
   // Set frequency to 2.0 Hz
-  m_output->SetPWMRate(2.0);
+  m_output.SetPWMRate(2.0);
   // Enable PWM, but leave it off
-  m_output->EnablePWM(0.0);
-  Wait(0.5);
-  m_output->UpdateDutyCycle(0.5);
-  m_input->RequestInterrupts();
-  m_input->SetUpSourceEdge(false, true);
-  InterruptableSensorBase::WaitResult result =
-      m_input->WaitForInterrupt(3.0, true);
+  m_output.EnablePWM(0.0);
+  frc::Wait(0.5_s);
+  m_output.UpdateDutyCycle(0.5);
+  frc::SynchronousInterrupt interrupt{m_output};
+  interrupt.SetInterruptEdges(false, true);
+  frc::SynchronousInterrupt::WaitResult result =
+      interrupt.WaitForInterrupt(3_s, true);
 
-  Wait(0.5);
-  bool firstCycle = m_input->Get();
-  Wait(0.5);
-  bool secondCycle = m_input->Get();
-  Wait(0.5);
-  bool thirdCycle = m_input->Get();
-  Wait(0.5);
-  bool forthCycle = m_input->Get();
-  Wait(0.5);
-  bool fifthCycle = m_input->Get();
-  Wait(0.5);
-  bool sixthCycle = m_input->Get();
-  Wait(0.5);
-  bool seventhCycle = m_input->Get();
-  m_output->DisablePWM();
-  Wait(0.5);
-  bool firstAfterStop = m_input->Get();
-  Wait(0.5);
-  bool secondAfterStop = m_input->Get();
+  frc::Wait(0.5_s);
+  bool firstCycle = m_input.Get();
+  frc::Wait(0.5_s);
+  bool secondCycle = m_input.Get();
+  frc::Wait(0.5_s);
+  bool thirdCycle = m_input.Get();
+  frc::Wait(0.5_s);
+  bool forthCycle = m_input.Get();
+  frc::Wait(0.5_s);
+  bool fifthCycle = m_input.Get();
+  frc::Wait(0.5_s);
+  bool sixthCycle = m_input.Get();
+  frc::Wait(0.5_s);
+  bool seventhCycle = m_input.Get();
+  m_output.DisablePWM();
+  frc::Wait(0.5_s);
+  bool firstAfterStop = m_input.Get();
+  frc::Wait(0.5_s);
+  bool secondAfterStop = m_input.Get();
 
-  EXPECT_EQ(InterruptableSensorBase::WaitResult::kFallingEdge, result)
+  EXPECT_EQ(frc::SynchronousInterrupt::WaitResult::kFallingEdge, result)
       << "WaitForInterrupt was not falling.";
 
   EXPECT_FALSE(firstCycle) << "Input not low after first delay";
@@ -126,63 +118,61 @@
 TEST_F(DIOLoopTest, FakeCounter) {
   Reset();
 
-  Counter counter(m_input);
+  frc::Counter counter{&m_input};
 
   EXPECT_EQ(0, counter.Get()) << "Counter did not initialize to 0.";
 
   /* Count 100 ticks.  The counter value should be 100 after this loop. */
-  for (int32_t i = 0; i < 100; i++) {
-    m_output->Set(true);
-    Wait(kCounterTime);
-    m_output->Set(false);
-    Wait(kCounterTime);
+  for (int32_t i = 0; i < 100; ++i) {
+    m_output.Set(true);
+    frc::Wait(kCounterTime);
+    m_output.Set(false);
+    frc::Wait(kCounterTime);
   }
 
   EXPECT_EQ(100, counter.Get()) << "Counter did not count up to 100.";
 }
 
-static void InterruptHandler(uint32_t interruptAssertedMask, void* param) {
-  *reinterpret_cast<int32_t*>(param) = 12345;
-}
-
 TEST_F(DIOLoopTest, AsynchronousInterruptWorks) {
+  Reset();
+
   int32_t param = 0;
 
-  // Given an interrupt handler that sets an int32_t to 12345
-  m_input->RequestInterrupts(InterruptHandler, &param);
-  m_input->EnableInterrupts();
+  frc::AsynchronousInterrupt interrupt(m_input,
+                                       [&](auto a, auto b) { param = 12345; });
 
+  interrupt.Enable();
   // If the voltage rises
-  m_output->Set(false);
-  m_output->Set(true);
-  m_input->CancelInterrupts();
+  m_output.Set(false);
+  m_output.Set(true);
 
   // Then the int32_t should be 12345
-  Wait(kDelayTime);
+  frc::Wait(kDelayTime);
+
+  interrupt.Disable();
+
   EXPECT_EQ(12345, param) << "The interrupt did not run.";
 }
 
-static void* InterruptTriggerer(void* data) {
-  DigitalOutput* output = static_cast<DigitalOutput*>(data);
-  output->Set(false);
-  Wait(kSynchronousInterruptTime);
-  output->Set(true);
-  return nullptr;
-}
-
 TEST_F(DIOLoopTest, SynchronousInterruptWorks) {
-  // Given a synchronous interrupt
-  m_input->RequestInterrupts();
+  Reset();
 
-  // If we have another thread trigger the interrupt in a few seconds
-  pthread_t interruptTriggererLoop;
-  pthread_create(&interruptTriggererLoop, nullptr, InterruptTriggerer,
-                 m_output);
+  // Given a synchronous interrupt
+  frc::SynchronousInterrupt interrupt(m_input);
+
+  std::thread thr([this]() {
+    m_output.Set(false);
+    frc::Wait(kSynchronousInterruptTime);
+    m_output.Set(true);
+  });
 
   // Then this thread should pause and resume after that number of seconds
-  Timer timer;
+  frc::Timer timer;
   timer.Start();
-  m_input->WaitForInterrupt(kSynchronousInterruptTime + 1.0);
-  EXPECT_NEAR(kSynchronousInterruptTime, timer.Get(),
-              kSynchronousInterruptTimeTolerance);
+  interrupt.WaitForInterrupt(kSynchronousInterruptTime + 1_s);
+  auto time = timer.Get().value();
+  if (thr.joinable())
+    thr.join();
+  EXPECT_NEAR(kSynchronousInterruptTime.value(), time,
+              kSynchronousInterruptTimeTolerance.value());
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DMATest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DMATest.cpp
new file mode 100644
index 0000000..39116db
--- /dev/null
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DMATest.cpp
@@ -0,0 +1,155 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <frc/AnalogInput.h>
+#include <frc/AnalogOutput.h>
+#include <frc/DigitalOutput.h>
+#include <frc/Timer.h>
+#include <frc/motorcontrol/Jaguar.h>
+
+#include "TestBench.h"
+#include "frc/DMA.h"
+#include "frc/DMASample.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr auto kDelayTime = 100_ms;
+
+class DMATest : public testing::Test {
+ protected:
+  AnalogInput m_analogInput{TestBench::kAnalogOutputChannel};
+  AnalogOutput m_analogOutput{TestBench::kFakeAnalogOutputChannel};
+  DigitalOutput m_manualTrigger{TestBench::kLoop1InputChannel};
+  Jaguar m_pwm{TestBench::kFakePwmOutput};
+  DMA m_dma;
+
+  void SetUp() override {
+    m_dma.AddAnalogInput(&m_analogInput);
+    m_dma.SetExternalTrigger(&m_manualTrigger, false, true);
+    m_manualTrigger.Set(true);
+  }
+};
+
+TEST_F(DMATest, DISABLED_PausingWorks) {
+  m_dma.Start(1024);
+  m_dma.SetPause(true);
+  m_manualTrigger.Set(false);
+
+  frc::DMASample sample;
+  int32_t remaining = 0;
+  int32_t status = 0;
+
+  auto timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status);
+
+  ASSERT_EQ(DMASample::DMAReadStatus::kTimeout, timedOut);
+}
+
+TEST_F(DMATest, DISABLED_RemovingTriggersWorks) {
+  m_dma.ClearExternalTriggers();
+  m_dma.Start(1024);
+  m_manualTrigger.Set(false);
+
+  frc::DMASample sample;
+  int32_t remaining = 0;
+  int32_t status = 0;
+
+  auto timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status);
+
+  ASSERT_EQ(DMASample::DMAReadStatus::kTimeout, timedOut);
+}
+
+TEST_F(DMATest, DISABLED_ManualTriggerOnlyHappensOnce) {
+  m_dma.Start(1024);
+  m_manualTrigger.Set(false);
+
+  frc::DMASample sample;
+  int32_t remaining = 0;
+  int32_t status = 0;
+
+  auto timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status);
+
+  ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut);
+  ASSERT_EQ(0, remaining);
+  timedOut = sample.Update(&m_dma, 5_ms, &remaining, &status);
+  ASSERT_EQ(DMASample::DMAReadStatus::kTimeout, timedOut);
+}
+
+TEST_F(DMATest, DISABLED_AnalogIndividualTriggers) {
+  m_dma.Start(1024);
+  for (double i = 0; i < 5; i += 0.5) {
+    frc::DMASample sample;
+    int32_t remaining = 0;
+    int32_t status = 0;
+
+    m_analogOutput.SetVoltage(i);
+    frc::Wait(kDelayTime);
+    m_manualTrigger.Set(false);
+    auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status);
+    m_manualTrigger.Set(true);
+    ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut);
+    ASSERT_EQ(0, status);
+    ASSERT_EQ(0, remaining);
+    ASSERT_DOUBLE_EQ(m_analogInput.GetVoltage(),
+                     sample.GetAnalogInputVoltage(&m_analogInput, &status));
+  }
+}
+
+TEST_F(DMATest, DISABLED_AnalogMultipleTriggers) {
+  m_dma.Start(1024);
+  std::vector<double> values;
+  for (double i = 0; i < 5; i += 0.5) {
+    values.push_back(i);
+    m_analogOutput.SetVoltage(i);
+    frc::Wait(kDelayTime);
+    m_manualTrigger.Set(false);
+    frc::Wait(kDelayTime);
+    m_manualTrigger.Set(true);
+  }
+
+  for (size_t i = 0; i < values.size(); i++) {
+    frc::DMASample sample;
+    int32_t remaining = 0;
+    int32_t status = 0;
+    auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status);
+    ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut);
+    ASSERT_EQ(0, status);
+    ASSERT_EQ(values.size() - i - 1, (uint32_t)remaining);
+    ASSERT_DOUBLE_EQ(values[i],
+                     sample.GetAnalogInputVoltage(&m_analogInput, &status));
+  }
+}
+
+TEST_F(DMATest, DISABLED_TimedTriggers) {
+  m_dma.SetTimedTrigger(10_ms);
+  m_dma.Start(1024);
+  frc::Wait(kDelayTime);
+  m_dma.SetPause(true);
+
+  frc::DMASample sample;
+  int32_t remaining = 0;
+  int32_t status = 0;
+
+  auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status);
+  ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut);
+  ASSERT_EQ(0, status);
+  ASSERT_GT(remaining, 5);
+}
+
+TEST_F(DMATest, DISABLED_PWMTimedTriggers) {
+  m_dma.ClearExternalTriggers();
+  m_dma.SetPwmEdgeTrigger(&m_pwm, true, false);
+  m_dma.Start(1024);
+  frc::Wait(kDelayTime);
+  m_dma.SetPause(true);
+
+  frc::DMASample sample;
+  int32_t remaining = 0;
+  int32_t status = 0;
+
+  auto timedOut = sample.Update(&m_dma, 1_ms, &remaining, &status);
+  ASSERT_EQ(DMASample::DMAReadStatus::kOk, timedOut);
+  ASSERT_EQ(0, status);
+  ASSERT_GT(remaining, 5);
+}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DigitalGlitchFilterTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DigitalGlitchFilterTest.cpp
index fa39e79..d7c1a23 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DigitalGlitchFilterTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DigitalGlitchFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DigitalGlitchFilter.h"  // NOLINT(build/include_order)
 
@@ -12,8 +9,6 @@
 #include "frc/Encoder.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 /**
  * Tests that configuring inputs to be filtered succeeds.
  *
@@ -21,28 +16,28 @@
  * implementation works as intended.  We configure the FPGA and then query it to
  * make sure that the acutal configuration matches.
  */
-TEST(DigitalGlitchFilterTest, BasicTest) {
-  DigitalInput input1(1);
-  DigitalInput input2(2);
-  DigitalInput input3(3);
-  DigitalInput input4(4);
-  Encoder encoder5(5, 6);
-  Counter counter7(7);
+TEST(DigitalGlitchFilterTest, Basic) {
+  frc::DigitalInput input1{1};
+  frc::DigitalInput input2{2};
+  frc::DigitalInput input3{3};
+  frc::DigitalInput input4{4};
+  frc::Encoder encoder5{5, 6};
+  frc::Counter counter7{7};
 
   // Check that we can make a single filter and set the period.
-  DigitalGlitchFilter filter1;
+  frc::DigitalGlitchFilter filter1;
   filter1.Add(&input1);
   filter1.SetPeriodNanoSeconds(4200);
 
   // Check that we can make a second filter with 2 inputs.
-  DigitalGlitchFilter filter2;
+  frc::DigitalGlitchFilter filter2;
   filter2.Add(&input2);
   filter2.Add(&input3);
   filter2.SetPeriodNanoSeconds(97100);
 
   // Check that we can make a third filter with an input, an encoder, and a
   // counter.
-  DigitalGlitchFilter filter3;
+  frc::DigitalGlitchFilter filter3;
   filter3.Add(&input4);
   filter3.Add(&encoder5);
   filter3.Add(&counter7);
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp
index df8eef3..e4a2721 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/DriverStationTest.cpp
@@ -1,35 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/DriverStation.h"  // NOLINT(build/include_order)
 
+#include <units/math.h>
+#include <units/time.h>
+
 #include "TestBench.h"
 #include "frc/RobotController.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
-constexpr double TIMER_TOLERANCE = 0.2;
-constexpr int64_t TIMER_RUNTIME = 1000000;  // 1 second
-
-class DriverStationTest : public testing::Test {};
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
 
 /**
  * Test if the WaitForData function works
  */
-TEST_F(DriverStationTest, WaitForData) {
-  uint64_t initialTime = RobotController::GetFPGATime();
+TEST(DriverStationTest, WaitForData) {
+  units::microsecond_t initialTime(frc::RobotController::GetFPGATime());
 
+  // 20ms waiting intervals * 50 = 1s
   for (int i = 0; i < 50; i++) {
-    DriverStation::GetInstance().WaitForData();
+    frc::DriverStation::WaitForData();
   }
 
-  uint64_t finalTime = RobotController::GetFPGATime();
+  units::microsecond_t finalTime(frc::RobotController::GetFPGATime());
 
-  EXPECT_NEAR(TIMER_RUNTIME, finalTime - initialTime,
-              TIMER_TOLERANCE * TIMER_RUNTIME);
+  EXPECT_NEAR_UNITS(1_s, finalTime - initialTime, 200_ms);
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/FakeEncoderTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/FakeEncoderTest.cpp
index bf5a8ee..3442b48 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/FakeEncoderTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/FakeEncoderTest.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Encoder.h"  // NOLINT(build/include_order)
 
+#include <units/time.h>
+
 #include "TestBench.h"
 #include "frc/AnalogOutput.h"
 #include "frc/AnalogTrigger.h"
@@ -14,41 +13,24 @@
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
-static const double kDelayTime = 0.001;
+static constexpr auto kDelayTime = 1_ms;
 
 class FakeEncoderTest : public testing::Test {
  protected:
-  DigitalOutput* m_outputA;
-  DigitalOutput* m_outputB;
-  AnalogOutput* m_indexOutput;
+  frc::DigitalOutput m_outputA{TestBench::kLoop2OutputChannel};
+  frc::DigitalOutput m_outputB{TestBench::kLoop1OutputChannel};
+  frc::AnalogOutput m_indexOutput{TestBench::kAnalogOutputChannel};
 
-  Encoder* m_encoder;
-  AnalogTrigger* m_indexAnalogTrigger;
-  std::shared_ptr<AnalogTriggerOutput> m_indexAnalogTriggerOutput;
+  frc::Encoder m_encoder{TestBench::kLoop1InputChannel,
+                         TestBench::kLoop2InputChannel};
+  frc::AnalogTrigger m_indexAnalogTrigger{TestBench::kFakeAnalogOutputChannel};
+  std::shared_ptr<frc::AnalogTriggerOutput> m_indexAnalogTriggerOutput =
+      m_indexAnalogTrigger.CreateOutput(frc::AnalogTriggerType::kState);
 
-  void SetUp() override {
-    m_outputA = new DigitalOutput(TestBench::kLoop2OutputChannel);
-    m_outputB = new DigitalOutput(TestBench::kLoop1OutputChannel);
-    m_indexOutput = new AnalogOutput(TestBench::kAnalogOutputChannel);
-    m_outputA->Set(false);
-    m_outputB->Set(false);
-    m_encoder = new Encoder(TestBench::kLoop1InputChannel,
-                            TestBench::kLoop2InputChannel);
-    m_indexAnalogTrigger =
-        new AnalogTrigger(TestBench::kFakeAnalogOutputChannel);
-    m_indexAnalogTrigger->SetLimitsVoltage(2.0, 3.0);
-    m_indexAnalogTriggerOutput =
-        m_indexAnalogTrigger->CreateOutput(AnalogTriggerType::kState);
-  }
-
-  void TearDown() override {
-    delete m_outputA;
-    delete m_outputB;
-    delete m_indexOutput;
-    delete m_encoder;
-    delete m_indexAnalogTrigger;
+  FakeEncoderTest() {
+    m_outputA.Set(false);
+    m_outputB.Set(false);
+    m_indexAnalogTrigger.SetLimitsVoltage(2.0, 3.0);
   }
 
   /**
@@ -57,105 +39,105 @@
    */
   void Simulate100QuadratureTicks() {
     for (int32_t i = 0; i < 100; i++) {
-      m_outputA->Set(true);
-      Wait(kDelayTime);
-      m_outputB->Set(true);
-      Wait(kDelayTime);
-      m_outputA->Set(false);
-      Wait(kDelayTime);
-      m_outputB->Set(false);
-      Wait(kDelayTime);
+      m_outputA.Set(true);
+      frc::Wait(kDelayTime);
+      m_outputB.Set(true);
+      frc::Wait(kDelayTime);
+      m_outputA.Set(false);
+      frc::Wait(kDelayTime);
+      m_outputB.Set(false);
+      frc::Wait(kDelayTime);
     }
   }
 
   void SetIndexHigh() {
-    m_indexOutput->SetVoltage(5.0);
-    Wait(kDelayTime);
+    m_indexOutput.SetVoltage(5.0);
+    frc::Wait(kDelayTime);
   }
 
   void SetIndexLow() {
-    m_indexOutput->SetVoltage(0.0);
-    Wait(kDelayTime);
+    m_indexOutput.SetVoltage(0.0);
+    frc::Wait(kDelayTime);
   }
 };
 
 /**
  * Test the encoder by reseting it to 0 and reading the value.
  */
-TEST_F(FakeEncoderTest, TestDefaultState) {
-  EXPECT_FLOAT_EQ(0.0, m_encoder->Get()) << "The encoder did not start at 0.";
+TEST_F(FakeEncoderTest, DefaultState) {
+  EXPECT_DOUBLE_EQ(0.0, m_encoder.Get()) << "The encoder did not start at 0.";
 }
 
 /**
  * Test the encoder by setting the digital outputs and reading the value.
  */
-TEST_F(FakeEncoderTest, TestCountUp) {
-  m_encoder->Reset();
+TEST_F(FakeEncoderTest, CountUp) {
+  m_encoder.Reset();
   Simulate100QuadratureTicks();
 
-  EXPECT_FLOAT_EQ(100.0, m_encoder->Get()) << "Encoder did not count to 100.";
+  EXPECT_DOUBLE_EQ(100.0, m_encoder.Get()) << "Encoder did not count to 100.";
 }
 
 /**
  * Test that the encoder can stay reset while the index source is high
  */
-TEST_F(FakeEncoderTest, TestResetWhileHigh) {
-  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
-                            Encoder::IndexingType::kResetWhileHigh);
+TEST_F(FakeEncoderTest, ResetWhileHigh) {
+  m_encoder.SetIndexSource(*m_indexAnalogTriggerOutput,
+                           frc::Encoder::IndexingType::kResetWhileHigh);
 
   SetIndexLow();
   Simulate100QuadratureTicks();
   SetIndexHigh();
-  EXPECT_EQ(0, m_encoder->Get());
+  EXPECT_EQ(0, m_encoder.Get());
 
   Simulate100QuadratureTicks();
-  EXPECT_EQ(0, m_encoder->Get());
+  EXPECT_EQ(0, m_encoder.Get());
 }
 
 /**
  * Test that the encoder can reset when the index source goes from low to high
  */
-TEST_F(FakeEncoderTest, TestResetOnRisingEdge) {
-  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
-                            Encoder::IndexingType::kResetOnRisingEdge);
+TEST_F(FakeEncoderTest, ResetOnRisingEdge) {
+  m_encoder.SetIndexSource(*m_indexAnalogTriggerOutput,
+                           frc::Encoder::IndexingType::kResetOnRisingEdge);
 
   SetIndexLow();
   Simulate100QuadratureTicks();
   SetIndexHigh();
-  EXPECT_EQ(0, m_encoder->Get());
+  EXPECT_EQ(0, m_encoder.Get());
 
   Simulate100QuadratureTicks();
-  EXPECT_EQ(100, m_encoder->Get());
+  EXPECT_EQ(100, m_encoder.Get());
 }
 
 /**
  * Test that the encoder can stay reset while the index source is low
  */
-TEST_F(FakeEncoderTest, TestResetWhileLow) {
-  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
-                            Encoder::IndexingType::kResetWhileLow);
+TEST_F(FakeEncoderTest, ResetWhileLow) {
+  m_encoder.SetIndexSource(*m_indexAnalogTriggerOutput,
+                           frc::Encoder::IndexingType::kResetWhileLow);
 
   SetIndexHigh();
   Simulate100QuadratureTicks();
   SetIndexLow();
-  EXPECT_EQ(0, m_encoder->Get());
+  EXPECT_EQ(0, m_encoder.Get());
 
   Simulate100QuadratureTicks();
-  EXPECT_EQ(0, m_encoder->Get());
+  EXPECT_EQ(0, m_encoder.Get());
 }
 
 /**
  * Test that the encoder can reset when the index source goes from high to low
  */
-TEST_F(FakeEncoderTest, TestResetOnFallingEdge) {
-  m_encoder->SetIndexSource(*m_indexAnalogTriggerOutput,
-                            Encoder::IndexingType::kResetOnFallingEdge);
+TEST_F(FakeEncoderTest, ResetOnFallingEdge) {
+  m_encoder.SetIndexSource(*m_indexAnalogTriggerOutput,
+                           frc::Encoder::IndexingType::kResetOnFallingEdge);
 
   SetIndexHigh();
   Simulate100QuadratureTicks();
   SetIndexLow();
-  EXPECT_EQ(0, m_encoder->Get());
+  EXPECT_EQ(0, m_encoder.Get());
 
   Simulate100QuadratureTicks();
-  EXPECT_EQ(100, m_encoder->Get());
+  EXPECT_EQ(100, m_encoder.Get());
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/Main.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/Main.cpp
new file mode 100644
index 0000000..95bc5b5
--- /dev/null
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/Main.cpp
@@ -0,0 +1,10 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "gtest/gtest.h"
+
+int main(int argc, char** argv) {
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
index e368fc2..46e2120 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorEncoderTest.cpp
@@ -1,25 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <algorithm>
 
+#include <units/time.h>
+
 #include "TestBench.h"
 #include "frc/Encoder.h"
-#include "frc/Jaguar.h"
-#include "frc/LinearFilter.h"
 #include "frc/Notifier.h"
-#include "frc/Talon.h"
 #include "frc/Timer.h"
-#include "frc/Victor.h"
 #include "frc/controller/PIDController.h"
+#include "frc/filter/LinearFilter.h"
+#include "frc/motorcontrol/Jaguar.h"
+#include "frc/motorcontrol/Talon.h"
+#include "frc/motorcontrol/Victor.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 enum MotorEncoderTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
 
 std::ostream& operator<<(std::ostream& os, MotorEncoderTestType const& type) {
@@ -38,50 +35,50 @@
   return os;
 }
 
-static constexpr double kMotorTime = 0.5;
+static constexpr auto kMotorTime = 0.5_s;
 
 /**
- * A fixture that includes a PWM speed controller and an encoder connected to
+ * A fixture that includes a PWM motor controller and an encoder connected to
  * the same motor.
  */
 class MotorEncoderTest : public testing::TestWithParam<MotorEncoderTestType> {
  protected:
-  SpeedController* m_speedController;
-  Encoder* m_encoder;
-  LinearFilter<double>* m_filter;
+  frc::MotorController* m_motorController;
+  frc::Encoder* m_encoder;
+  frc::LinearFilter<double>* m_filter;
 
-  void SetUp() override {
+  MotorEncoderTest() {
     switch (GetParam()) {
       case TEST_VICTOR:
-        m_speedController = new Victor(TestBench::kVictorChannel);
-        m_encoder = new Encoder(TestBench::kVictorEncoderChannelA,
-                                TestBench::kVictorEncoderChannelB);
+        m_motorController = new frc::Victor(TestBench::kVictorChannel);
+        m_encoder = new frc::Encoder(TestBench::kVictorEncoderChannelA,
+                                     TestBench::kVictorEncoderChannelB);
         break;
 
       case TEST_JAGUAR:
-        m_speedController = new Jaguar(TestBench::kJaguarChannel);
-        m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA,
-                                TestBench::kJaguarEncoderChannelB);
+        m_motorController = new frc::Jaguar(TestBench::kJaguarChannel);
+        m_encoder = new frc::Encoder(TestBench::kJaguarEncoderChannelA,
+                                     TestBench::kJaguarEncoderChannelB);
         break;
 
       case TEST_TALON:
-        m_speedController = new Talon(TestBench::kTalonChannel);
-        m_encoder = new Encoder(TestBench::kTalonEncoderChannelA,
-                                TestBench::kTalonEncoderChannelB);
+        m_motorController = new frc::Talon(TestBench::kTalonChannel);
+        m_encoder = new frc::Encoder(TestBench::kTalonEncoderChannelA,
+                                     TestBench::kTalonEncoderChannelB);
         break;
     }
-    m_filter =
-        new LinearFilter<double>(LinearFilter<double>::MovingAverage(50));
+    m_filter = new frc::LinearFilter<double>(
+        frc::LinearFilter<double>::MovingAverage(50));
   }
 
-  void TearDown() override {
-    delete m_speedController;
-    delete m_encoder;
+  ~MotorEncoderTest() {
     delete m_filter;
+    delete m_encoder;
+    delete m_motorController;
   }
 
   void Reset() {
-    m_speedController->Set(0.0);
+    m_motorController->Set(0.0);
     m_encoder->Reset();
     m_filter->Reset();
   }
@@ -93,10 +90,10 @@
 TEST_P(MotorEncoderTest, Increment) {
   Reset();
 
-  /* Drive the speed controller briefly to move the encoder */
-  m_speedController->Set(0.2f);
-  Wait(kMotorTime);
-  m_speedController->Set(0.0);
+  /* Drive the motor controller briefly to move the encoder */
+  m_motorController->Set(0.2f);
+  frc::Wait(kMotorTime);
+  m_motorController->Set(0.0);
 
   /* The encoder should be positive now */
   EXPECT_GT(m_encoder->Get(), 0)
@@ -109,10 +106,10 @@
 TEST_P(MotorEncoderTest, Decrement) {
   Reset();
 
-  /* Drive the speed controller briefly to move the encoder */
-  m_speedController->Set(-0.2);
-  Wait(kMotorTime);
-  m_speedController->Set(0.0);
+  /* Drive the motor controller briefly to move the encoder */
+  m_motorController->Set(-0.2);
+  frc::Wait(kMotorTime);
+  m_motorController->Set(0.0);
 
   /* The encoder should be positive now */
   EXPECT_LT(m_encoder->Get(), 0.0)
@@ -125,15 +122,15 @@
 TEST_P(MotorEncoderTest, ClampSpeed) {
   Reset();
 
-  m_speedController->Set(2.0);
-  Wait(kMotorTime);
+  m_motorController->Set(2.0);
+  frc::Wait(kMotorTime);
 
-  EXPECT_FLOAT_EQ(1.0, m_speedController->Get());
+  EXPECT_FLOAT_EQ(1.0, m_motorController->Get());
 
-  m_speedController->Set(-2.0);
-  Wait(kMotorTime);
+  m_motorController->Set(-2.0);
+  frc::Wait(kMotorTime);
 
-  EXPECT_FLOAT_EQ(-1.0, m_speedController->Get());
+  EXPECT_FLOAT_EQ(-1.0, m_motorController->Get());
 }
 
 /**
@@ -150,10 +147,10 @@
   /* 10 seconds should be plenty time to get to the reference */
   frc::Notifier pidRunner{[this, &pidController] {
     auto speed = pidController.Calculate(m_encoder->GetDistance());
-    m_speedController->Set(std::clamp(speed, -0.2, 0.2));
+    m_motorController->Set(std::clamp(speed, -0.2, 0.2));
   }};
   pidRunner.StartPeriodic(pidController.GetPeriod());
-  Wait(10.0);
+  frc::Wait(10_s);
   pidRunner.Stop();
 
   RecordProperty("PIDError", pidController.GetPositionError());
@@ -177,10 +174,10 @@
   frc::Notifier pidRunner{[this, &pidController] {
     auto speed =
         pidController.Calculate(m_filter->Calculate(m_encoder->GetRate()));
-    m_speedController->Set(std::clamp(speed, -0.3, 0.3));
+    m_motorController->Set(std::clamp(speed, -0.3, 0.3));
   }};
   pidRunner.StartPeriodic(pidController.GetPeriod());
-  Wait(10.0);
+  frc::Wait(10_s);
   pidRunner.Stop();
   RecordProperty("PIDError", pidController.GetPositionError());
 
@@ -198,5 +195,5 @@
   EXPECT_EQ(0, m_encoder->Get()) << "Encoder did not reset to 0";
 }
 
-INSTANTIATE_TEST_SUITE_P(Test, MotorEncoderTest,
+INSTANTIATE_TEST_SUITE_P(Tests, MotorEncoderTest,
                          testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON));
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp
index ed1821b..10ccd34 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/MotorInvertingTest.cpp
@@ -1,23 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <units/time.h>
 
 #include "TestBench.h"
 #include "frc/Encoder.h"
-#include "frc/Jaguar.h"
-#include "frc/Talon.h"
 #include "frc/Timer.h"
-#include "frc/Victor.h"
+#include "frc/motorcontrol/Jaguar.h"
+#include "frc/motorcontrol/Talon.h"
+#include "frc/motorcontrol/Victor.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 enum MotorInvertingTestType { TEST_VICTOR, TEST_JAGUAR, TEST_TALON };
-static const double motorSpeed = 0.15;
-static const double delayTime = 0.5;
+
+static constexpr double kMotorSpeed = 0.15;
+static constexpr auto kDelayTime = 0.5_s;
+
 std::ostream& operator<<(std::ostream& os, MotorInvertingTestType const& type) {
   switch (type) {
     case TEST_VICTOR:
@@ -36,39 +35,39 @@
 class MotorInvertingTest
     : public testing::TestWithParam<MotorInvertingTestType> {
  protected:
-  SpeedController* m_speedController;
-  Encoder* m_encoder;
+  frc::MotorController* m_motorController;
+  frc::Encoder* m_encoder;
 
-  void SetUp() override {
+  MotorInvertingTest() {
     switch (GetParam()) {
       case TEST_VICTOR:
-        m_speedController = new Victor(TestBench::kVictorChannel);
-        m_encoder = new Encoder(TestBench::kVictorEncoderChannelA,
-                                TestBench::kVictorEncoderChannelB);
+        m_motorController = new frc::Victor(TestBench::kVictorChannel);
+        m_encoder = new frc::Encoder(TestBench::kVictorEncoderChannelA,
+                                     TestBench::kVictorEncoderChannelB);
         break;
 
       case TEST_JAGUAR:
-        m_speedController = new Jaguar(TestBench::kJaguarChannel);
-        m_encoder = new Encoder(TestBench::kJaguarEncoderChannelA,
-                                TestBench::kJaguarEncoderChannelB);
+        m_motorController = new frc::Jaguar(TestBench::kJaguarChannel);
+        m_encoder = new frc::Encoder(TestBench::kJaguarEncoderChannelA,
+                                     TestBench::kJaguarEncoderChannelB);
         break;
 
       case TEST_TALON:
-        m_speedController = new Talon(TestBench::kTalonChannel);
-        m_encoder = new Encoder(TestBench::kTalonEncoderChannelA,
-                                TestBench::kTalonEncoderChannelB);
+        m_motorController = new frc::Talon(TestBench::kTalonChannel);
+        m_encoder = new frc::Encoder(TestBench::kTalonEncoderChannelA,
+                                     TestBench::kTalonEncoderChannelB);
         break;
     }
   }
 
-  void TearDown() override {
-    delete m_speedController;
+  ~MotorInvertingTest() {
     delete m_encoder;
+    delete m_motorController;
   }
 
   void Reset() {
-    m_speedController->SetInverted(false);
-    m_speedController->Set(0.0);
+    m_motorController->SetInverted(false);
+    m_motorController->Set(0.0);
     m_encoder->Reset();
   }
 };
@@ -76,15 +75,15 @@
 TEST_P(MotorInvertingTest, InvertingPositive) {
   Reset();
 
-  m_speedController->Set(motorSpeed);
+  m_motorController->Set(kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   bool initDirection = m_encoder->GetDirection();
-  m_speedController->SetInverted(true);
-  m_speedController->Set(motorSpeed);
+  m_motorController->SetInverted(true);
+  m_motorController->Set(kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   EXPECT_TRUE(m_encoder->GetDirection() != initDirection)
       << "Inverting with Positive value does not change direction";
@@ -95,16 +94,16 @@
 TEST_P(MotorInvertingTest, InvertingNegative) {
   Reset();
 
-  m_speedController->SetInverted(false);
-  m_speedController->Set(-motorSpeed);
+  m_motorController->SetInverted(false);
+  m_motorController->Set(-kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   bool initDirection = m_encoder->GetDirection();
-  m_speedController->SetInverted(true);
-  m_speedController->Set(-motorSpeed);
+  m_motorController->SetInverted(true);
+  m_motorController->Set(-kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   EXPECT_TRUE(m_encoder->GetDirection() != initDirection)
       << "Inverting with Negative value does not change direction";
@@ -115,16 +114,16 @@
 TEST_P(MotorInvertingTest, InvertingSwitchingPosToNeg) {
   Reset();
 
-  m_speedController->SetInverted(false);
-  m_speedController->Set(motorSpeed);
+  m_motorController->SetInverted(false);
+  m_motorController->Set(kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   bool initDirection = m_encoder->GetDirection();
-  m_speedController->SetInverted(true);
-  m_speedController->Set(-motorSpeed);
+  m_motorController->SetInverted(true);
+  m_motorController->Set(-kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   EXPECT_TRUE(m_encoder->GetDirection() == initDirection)
       << "Inverting with Switching value does change direction";
@@ -135,16 +134,16 @@
 TEST_P(MotorInvertingTest, InvertingSwitchingNegToPos) {
   Reset();
 
-  m_speedController->SetInverted(false);
-  m_speedController->Set(-motorSpeed);
+  m_motorController->SetInverted(false);
+  m_motorController->Set(-kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   bool initDirection = m_encoder->GetDirection();
-  m_speedController->SetInverted(true);
-  m_speedController->Set(motorSpeed);
+  m_motorController->SetInverted(true);
+  m_motorController->Set(kMotorSpeed);
 
-  Wait(delayTime);
+  frc::Wait(kDelayTime);
 
   EXPECT_TRUE(m_encoder->GetDirection() == initDirection)
       << "Inverting with Switching value does change direction";
@@ -152,5 +151,5 @@
   Reset();
 }
 
-INSTANTIATE_TEST_SUITE_P(Test, MotorInvertingTest,
+INSTANTIATE_TEST_SUITE_P(Tests, MotorInvertingTest,
                          testing::Values(TEST_VICTOR, TEST_JAGUAR, TEST_TALON));
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/NotifierTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/NotifierTest.cpp
index d7f0d6e..eaaaf7e 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/NotifierTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/NotifierTest.cpp
@@ -1,43 +1,43 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Notifier.h"  // NOLINT(build/include_order)
 
-#include <wpi/raw_ostream.h>
+#include <fmt/core.h>
 
-#include "TestBench.h"
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
+TEST(NotifierTest, StartPeriodicAndStop) {
+  uint32_t counter = 0;
 
-unsigned notifierCounter;
+  frc::Notifier notifier{[&] { ++counter; }};
+  notifier.StartPeriodic(1_s);
 
-void notifierHandler(void*) { notifierCounter++; }
+  frc::Wait(10.5_s);
 
-/**
- * Test if the Wait function works
- */
-TEST(NotifierTest, DISABLED_TestTimerNotifications) {
-  wpi::outs() << "NotifierTest...\n";
-  notifierCounter = 0;
-  wpi::outs() << "notifier(notifierHandler, nullptr)...\n";
-  Notifier notifier(notifierHandler, nullptr);
-  wpi::outs() << "Start Periodic...\n";
-  notifier.StartPeriodic(1.0);
+  notifier.Stop();
+  EXPECT_EQ(10u, counter) << "Received " << counter
+                          << " notifications in 10.5 seconds\n";
+  fmt::print("Received {} notifications in 10.5 seconds\n", counter);
 
-  wpi::outs() << "Wait...\n";
-  Wait(10.5);
-  wpi::outs() << "...Wait\n";
+  frc::Wait(3_s);
 
-  EXPECT_EQ(10u, notifierCounter)
-      << "Received " << notifierCounter << " notifications in 10.5 seconds";
-  wpi::outs() << "Received " << notifierCounter
-              << " notifications in 10.5 seconds";
+  EXPECT_EQ(10u, counter) << "Received " << counter - 10
+                          << " notifications in 3 seconds\n";
+  fmt::print("Received {} notifications in 3 seconds\n", counter - 10);
+}
 
-  wpi::outs() << "...NotifierTest\n";
+TEST(NotifierTest, StartSingle) {
+  uint32_t counter = 0;
+
+  frc::Notifier notifier{[&] { ++counter; }};
+  notifier.StartSingle(1_s);
+
+  frc::Wait(10.5_s);
+
+  EXPECT_EQ(1u, counter) << "Received " << counter
+                         << " notifications in 10.5 seconds\n";
+  fmt::print("Received {} notifications in 10.5 seconds\n", counter);
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp
index 5be4d3f..1cc70b7 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PCMTest.cpp
@@ -1,28 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "TestBench.h"
 #include "frc/AnalogInput.h"
-#include "frc/Compressor.h"
 #include "frc/DigitalInput.h"
 #include "frc/DigitalOutput.h"
 #include "frc/DoubleSolenoid.h"
+#include "frc/PneumaticsControlModule.h"
 #include "frc/Solenoid.h"
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 /* The PCM switches the compressor up to a couple seconds after the pressure
         switch changes. */
-static const double kCompressorDelayTime = 3.0;
+static constexpr auto kCompressorDelayTime = 3_s;
 
 /* Solenoids should change much more quickly */
-static const double kSolenoidDelayTime = 0.5;
+static constexpr auto kSolenoidDelayTime = 0.5_s;
 
 /* The voltage divider on the test bench should bring the compressor output
         to around these values. */
@@ -31,34 +26,17 @@
 
 class PCMTest : public testing::Test {
  protected:
-  Compressor* m_compressor;
+  frc::PneumaticsControlModule m_pneumaticsModule;
 
-  DigitalOutput* m_fakePressureSwitch;
-  AnalogInput* m_fakeCompressor;
-  DoubleSolenoid* m_doubleSolenoid;
-  DigitalInput *m_fakeSolenoid1, *m_fakeSolenoid2;
-
-  void SetUp() override {
-    m_compressor = new Compressor();
-
-    m_fakePressureSwitch =
-        new DigitalOutput(TestBench::kFakePressureSwitchChannel);
-    m_fakeCompressor = new AnalogInput(TestBench::kFakeCompressorChannel);
-    m_fakeSolenoid1 = new DigitalInput(TestBench::kFakeSolenoid1Channel);
-    m_fakeSolenoid2 = new DigitalInput(TestBench::kFakeSolenoid2Channel);
-  }
-
-  void TearDown() override {
-    delete m_compressor;
-    delete m_fakePressureSwitch;
-    delete m_fakeCompressor;
-    delete m_fakeSolenoid1;
-    delete m_fakeSolenoid2;
-  }
+  frc::DigitalOutput m_fakePressureSwitch{
+      TestBench::kFakePressureSwitchChannel};
+  frc::AnalogInput m_fakeCompressor{TestBench::kFakeCompressorChannel};
+  frc::DigitalInput m_fakeSolenoid1{TestBench::kFakeSolenoid1Channel};
+  frc::DigitalInput m_fakeSolenoid2{TestBench::kFakeSolenoid2Channel};
 
   void Reset() {
-    m_compressor->Stop();
-    m_fakePressureSwitch->Set(false);
+    m_pneumaticsModule.SetClosedLoopControl(false);
+    m_fakePressureSwitch.Set(false);
   }
 };
 
@@ -68,18 +46,18 @@
 TEST_F(PCMTest, PressureSwitch) {
   Reset();
 
-  m_compressor->SetClosedLoopControl(true);
+  m_pneumaticsModule.SetClosedLoopControl(true);
 
   // Turn on the compressor
-  m_fakePressureSwitch->Set(true);
-  Wait(kCompressorDelayTime);
-  EXPECT_NEAR(kCompressorOnVoltage, m_fakeCompressor->GetVoltage(), 0.5)
+  m_fakePressureSwitch.Set(true);
+  frc::Wait(kCompressorDelayTime);
+  EXPECT_NEAR(kCompressorOnVoltage, m_fakeCompressor.GetVoltage(), 0.5)
       << "Compressor did not turn on when the pressure switch turned on.";
 
   // Turn off the compressor
-  m_fakePressureSwitch->Set(false);
-  Wait(kCompressorDelayTime);
-  EXPECT_NEAR(kCompressorOffVoltage, m_fakeCompressor->GetVoltage(), 0.5)
+  m_fakePressureSwitch.Set(false);
+  frc::Wait(kCompressorDelayTime);
+  EXPECT_NEAR(kCompressorOffVoltage, m_fakeCompressor.GetVoltage(), 0.5)
       << "Compressor did not turn off when the pressure switch turned off.";
 }
 
@@ -88,42 +66,44 @@
  */
 TEST_F(PCMTest, Solenoid) {
   Reset();
-  Solenoid solenoid1(TestBench::kSolenoidChannel1);
-  Solenoid solenoid2(TestBench::kSolenoidChannel2);
+  frc::Solenoid solenoid1{frc::PneumaticsModuleType::CTREPCM,
+                          TestBench::kSolenoidChannel1};
+  frc::Solenoid solenoid2{frc::PneumaticsModuleType::CTREPCM,
+                          TestBench::kSolenoidChannel2};
 
   // Turn both solenoids off
   solenoid1.Set(false);
   solenoid2.Set(false);
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 
   // Turn one solenoid on and one off
   solenoid1.Set(true);
   solenoid2.Set(false);
-  Wait(kSolenoidDelayTime);
-  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn on";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 
   // Turn one solenoid on and one off
   solenoid1.Set(false);
   solenoid2.Set(true);
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
 
   // Turn both on
   solenoid1.Set(true);
   solenoid2.Set(true);
-  Wait(kSolenoidDelayTime);
-  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn on";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
   EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
   EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
 }
@@ -133,42 +113,45 @@
  * with the DoubleSolenoid class.
  */
 TEST_F(PCMTest, DoubleSolenoid) {
-  DoubleSolenoid solenoid(TestBench::kSolenoidChannel1,
-                          TestBench::kSolenoidChannel2);
+  frc::DoubleSolenoid solenoid{frc::PneumaticsModuleType::CTREPCM,
+                               TestBench::kSolenoidChannel1,
+                               TestBench::kSolenoidChannel2};
 
-  solenoid.Set(DoubleSolenoid::kOff);
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
-  EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kOff)
+  solenoid.Set(frc::DoubleSolenoid::kOff);
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
+  EXPECT_TRUE(solenoid.Get() == frc::DoubleSolenoid::kOff)
       << "Solenoid does not read off";
 
-  solenoid.Set(DoubleSolenoid::kForward);
-  Wait(kSolenoidDelayTime);
-  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
-  EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kForward)
+  solenoid.Set(frc::DoubleSolenoid::kForward);
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn on";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
+  EXPECT_TRUE(solenoid.Get() == frc::DoubleSolenoid::kForward)
       << "Solenoid does not read forward";
 
-  solenoid.Set(DoubleSolenoid::kReverse);
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
-  EXPECT_TRUE(solenoid.Get() == DoubleSolenoid::kReverse)
+  solenoid.Set(frc::DoubleSolenoid::kReverse);
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
+  EXPECT_TRUE(solenoid.Get() == frc::DoubleSolenoid::kReverse)
       << "Solenoid does not read reverse";
 }
 
 TEST_F(PCMTest, OneShot) {
   Reset();
-  Solenoid solenoid1(TestBench::kSolenoidChannel1);
-  Solenoid solenoid2(TestBench::kSolenoidChannel2);
+  frc::Solenoid solenoid1{frc::PneumaticsModuleType::CTREPCM,
+                          TestBench::kSolenoidChannel1};
+  frc::Solenoid solenoid2{frc::PneumaticsModuleType::CTREPCM,
+                          TestBench::kSolenoidChannel2};
 
   // Turn both solenoids off
   solenoid1.Set(false);
   solenoid2.Set(false);
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 
@@ -176,14 +159,14 @@
   solenoid1.SetPulseDuration(2 * kSolenoidDelayTime);
   solenoid2.Set(false);
   solenoid1.StartPulse();
-  Wait(kSolenoidDelayTime);
-  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn on";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
-  Wait(2 * kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 
@@ -191,14 +174,14 @@
   solenoid1.Set(false);
   solenoid2.SetPulseDuration(2 * kSolenoidDelayTime);
   solenoid2.StartPulse();
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
-  Wait(2 * kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 
@@ -207,14 +190,14 @@
   solenoid2.SetPulseDuration(2 * kSolenoidDelayTime);
   solenoid1.StartPulse();
   solenoid2.StartPulse();
-  Wait(kSolenoidDelayTime);
-  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn on";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
   EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
   EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
-  Wait(2 * kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 
@@ -223,19 +206,19 @@
   solenoid2.SetPulseDuration(2.5 * kSolenoidDelayTime);
   solenoid1.StartPulse();
   solenoid2.StartPulse();
-  Wait(kSolenoidDelayTime);
-  EXPECT_FALSE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn on";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_FALSE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn on";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
   EXPECT_TRUE(solenoid1.Get()) << "Solenoid #1 did not read on";
   EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
-  Wait(kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_FALSE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn on";
+  frc::Wait(kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_FALSE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn on";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_TRUE(solenoid2.Get()) << "Solenoid #2 did not read on";
-  Wait(2 * kSolenoidDelayTime);
-  EXPECT_TRUE(m_fakeSolenoid1->Get()) << "Solenoid #1 did not turn off";
-  EXPECT_TRUE(m_fakeSolenoid2->Get()) << "Solenoid #2 did not turn off";
+  frc::Wait(2 * kSolenoidDelayTime);
+  EXPECT_TRUE(m_fakeSolenoid1.Get()) << "Solenoid #1 did not turn off";
+  EXPECT_TRUE(m_fakeSolenoid2.Get()) << "Solenoid #2 did not turn off";
   EXPECT_FALSE(solenoid1.Get()) << "Solenoid #1 did not read off";
   EXPECT_FALSE(solenoid2.Get()) << "Solenoid #2 did not read off";
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp
index 5917361..4697553 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PowerDistributionPanelTest.cpp
@@ -1,78 +1,52 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include "frc/PowerDistributionPanel.h"  // NOLINT(build/include_order)
-
-#include <thread>
+#include "frc/PowerDistribution.h"  // NOLINT(build/include_order)
 
 #include <hal/Ports.h>
+#include <units/time.h>
 
 #include "TestBench.h"
-#include "frc/Jaguar.h"
-#include "frc/Talon.h"
 #include "frc/Timer.h"
-#include "frc/Victor.h"
+#include "frc/motorcontrol/Talon.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
+static constexpr auto kMotorTime = 0.25_s;
 
-static const double kMotorTime = 0.25;
-
-class PowerDistributionPanelTest : public testing::Test {
+class PowerDistributionTest : public testing::Test {
  protected:
-  PowerDistributionPanel* m_pdp;
-  Talon* m_talon;
-  Victor* m_victor;
-  Jaguar* m_jaguar;
-
-  void SetUp() override {
-    m_pdp = new PowerDistributionPanel();
-    m_talon = new Talon(TestBench::kTalonChannel);
-    m_victor = new Victor(TestBench::kVictorChannel);
-    m_jaguar = new Jaguar(TestBench::kJaguarChannel);
-  }
-
-  void TearDown() override {
-    delete m_pdp;
-    delete m_talon;
-    delete m_victor;
-    delete m_jaguar;
-  }
+  frc::PowerDistribution m_pdp;
+  frc::Talon m_talon{TestBench::kTalonChannel};
 };
 
-TEST_F(PowerDistributionPanelTest, CheckRepeatedCalls) {
-  auto numChannels = HAL_GetNumPDPChannels();
+TEST_F(PowerDistributionTest, CheckRepeatedCalls) {
+  auto numChannels = HAL_GetNumCTREPDPChannels();
   // 1 second
   for (int i = 0; i < 50; i++) {
     for (int j = 0; j < numChannels; j++) {
-      m_pdp->GetCurrent(j);
-      ASSERT_TRUE(m_pdp->GetError().GetCode() == 0);
+      m_pdp.GetCurrent(j);
     }
-    m_pdp->GetVoltage();
-    ASSERT_TRUE(m_pdp->GetError().GetCode() == 0);
+    m_pdp.GetVoltage();
   }
-  std::this_thread::sleep_for(std::chrono::milliseconds(20));
+  frc::Wait(20_ms);
 }
 
 /**
  * Test if the current changes when the motor is driven using a talon
  */
-TEST_F(PowerDistributionPanelTest, CheckCurrentTalon) {
-  Wait(kMotorTime);
+TEST_F(PowerDistributionTest, CheckCurrentTalon) {
+  frc::Wait(kMotorTime);
 
   /* The Current should be 0 */
-  EXPECT_FLOAT_EQ(0, m_pdp->GetCurrent(TestBench::kTalonPDPChannel))
+  EXPECT_FLOAT_EQ(0, m_pdp.GetCurrent(TestBench::kTalonPDPChannel))
       << "The Talon current was non-zero";
 
   /* Set the motor to full forward */
-  m_talon->Set(1.0);
-  Wait(kMotorTime);
+  m_talon.Set(1.0);
+  frc::Wait(kMotorTime);
 
   /* The current should now be positive */
-  ASSERT_GT(m_pdp->GetCurrent(TestBench::kTalonPDPChannel), 0)
+  ASSERT_GT(m_pdp.GetCurrent(TestBench::kTalonPDPChannel), 0)
       << "The Talon current was not positive";
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PreferencesTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PreferencesTest.cpp
index ffb4d9f..5e79f27 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PreferencesTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PreferencesTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Preferences.h"  // NOLINT(build/include_order)
 
@@ -12,14 +9,13 @@
 
 #include <networktables/NetworkTableInstance.h>
 #include <ntcore.h>
+#include <units/time.h>
 
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
 static const char* kFileName = "networktables.ini";
-static const double kSaveTime = 1.2;
+static constexpr auto kSaveTime = 1.2_s;
 
 /**
  * If we write a new networktables.ini with some sample values, test that
@@ -28,6 +24,7 @@
 TEST(PreferencesTest, ReadPreferencesFromFile) {
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.StopServer();
+
   std::remove(kFileName);
   std::ofstream preferencesFile(kFileName);
   preferencesFile << "[NetworkTables Storage 3.0]" << std::endl;
@@ -45,16 +42,17 @@
       << "double \"/Preferences/testFileGetLong\"=1000000000000000000"
       << std::endl;
   preferencesFile.close();
+
   inst.StartServer();
 
-  Preferences* preferences = Preferences::GetInstance();
   EXPECT_EQ("Hello, preferences file",
-            preferences->GetString("testFileGetString"));
-  EXPECT_EQ(1, preferences->GetInt("testFileGetInt"));
-  EXPECT_FLOAT_EQ(0.5, preferences->GetDouble("testFileGetDouble"));
-  EXPECT_FLOAT_EQ(0.25f, preferences->GetFloat("testFileGetFloat"));
-  EXPECT_TRUE(preferences->GetBoolean("testFileGetBoolean"));
-  EXPECT_EQ(1000000000000000000ll, preferences->GetLong("testFileGetLong"));
+            frc::Preferences::GetString("testFileGetString"));
+  EXPECT_EQ(1, frc::Preferences::GetInt("testFileGetInt"));
+  EXPECT_FLOAT_EQ(0.5, frc::Preferences::GetDouble("testFileGetDouble"));
+  EXPECT_FLOAT_EQ(0.25f, frc::Preferences::GetFloat("testFileGetFloat"));
+  EXPECT_TRUE(frc::Preferences::GetBoolean("testFileGetBoolean"));
+  EXPECT_EQ(1000000000000000000ll,
+            frc::Preferences::GetLong("testFileGetLong"));
 }
 
 /**
@@ -64,34 +62,33 @@
 TEST(PreferencesTest, WritePreferencesToFile) {
   auto inst = nt::NetworkTableInstance::GetDefault();
   inst.StartServer();
-  Preferences* preferences = Preferences::GetInstance();
-  preferences->Remove("testFileGetString");
-  preferences->Remove("testFileGetInt");
-  preferences->Remove("testFileGetDouble");
-  preferences->Remove("testFileGetFloat");
-  preferences->Remove("testFileGetBoolean");
-  preferences->Remove("testFileGetLong");
+  frc::Preferences::Remove("testFileGetString");
+  frc::Preferences::Remove("testFileGetInt");
+  frc::Preferences::Remove("testFileGetDouble");
+  frc::Preferences::Remove("testFileGetFloat");
+  frc::Preferences::Remove("testFileGetBoolean");
+  frc::Preferences::Remove("testFileGetLong");
 
-  Wait(kSaveTime);
+  frc::Wait(kSaveTime);
 
-  preferences->PutString("testFilePutString", "Hello, preferences file");
-  preferences->PutInt("testFilePutInt", 1);
-  preferences->PutDouble("testFilePutDouble", 0.5);
-  preferences->PutFloat("testFilePutFloat", 0.25f);
-  preferences->PutBoolean("testFilePutBoolean", true);
-  preferences->PutLong("testFilePutLong", 1000000000000000000ll);
+  frc::Preferences::SetString("testFileSetString", "Hello, preferences file");
+  frc::Preferences::SetInt("testFileSetInt", 1);
+  frc::Preferences::SetDouble("testFileSetDouble", 0.5);
+  frc::Preferences::SetFloat("testFileSetFloat", 0.25f);
+  frc::Preferences::SetBoolean("testFileSetBoolean", true);
+  frc::Preferences::SetLong("testFileSetLong", 1000000000000000000ll);
 
-  Wait(kSaveTime);
+  frc::Wait(kSaveTime);
 
   static char const* kExpectedFileContents[] = {
       "[NetworkTables Storage 3.0]",
       "string \"/Preferences/.type\"=\"RobotPreferences\"",
-      "boolean \"/Preferences/testFilePutBoolean\"=true",
-      "double \"/Preferences/testFilePutDouble\"=0.5",
-      "double \"/Preferences/testFilePutFloat\"=0.25",
-      "double \"/Preferences/testFilePutInt\"=1",
-      "double \"/Preferences/testFilePutLong\"=1e+18",
-      "string \"/Preferences/testFilePutString\"=\"Hello, preferences file\""};
+      "boolean \"/Preferences/testFileSetBoolean\"=true",
+      "double \"/Preferences/testFileSetDouble\"=0.5",
+      "double \"/Preferences/testFileSetFloat\"=0.25",
+      "double \"/Preferences/testFileSetInt\"=1",
+      "double \"/Preferences/testFileSetLong\"=1e+18",
+      "string \"/Preferences/testFileSetString\"=\"Hello, preferences file\""};
 
   std::ifstream preferencesFile(kFileName);
   for (auto& kExpectedFileContent : kExpectedFileContents) {
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp
new file mode 100644
index 0000000..3085bed
--- /dev/null
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/PriorityTest.cpp
@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <thread>
+
+#include <wpi/condition_variable.h>
+#include <wpi/mutex.h>
+
+#include "frc/Threads.h"
+#include "gtest/gtest.h"
+
+TEST(PriorityTest, SetThreadPriority) {
+  bool exited = false;
+  wpi::condition_variable cond;
+  wpi::mutex mutex;
+  std::thread thr{[&] {
+    std::unique_lock lock{mutex};
+    cond.wait(lock, [&] { return exited; });
+  }};
+
+  // Non-RT thread has priority of zero
+  bool isRealTime = false;
+  EXPECT_EQ(0, frc::GetThreadPriority(thr, &isRealTime));
+  EXPECT_FALSE(isRealTime);
+
+  // Set thread priority to 15
+  EXPECT_TRUE(frc::SetThreadPriority(thr, true, 15));
+
+  // Verify thread was given priority 15
+  EXPECT_EQ(15, frc::GetThreadPriority(thr, &isRealTime));
+  EXPECT_TRUE(isRealTime);
+
+  // Set thread back to non-RT
+  EXPECT_TRUE(frc::SetThreadPriority(thr, false, 0));
+
+  // Verify thread is now non-RT
+  EXPECT_EQ(0, frc::GetThreadPriority(thr, &isRealTime));
+  EXPECT_FALSE(isRealTime);
+
+  {
+    std::scoped_lock lock{mutex};
+    exited = true;
+  }
+  cond.notify_one();
+  thr.join();
+}
+
+TEST(PriorityTest, SetCurrentThreadPriority) {
+  // Non-RT thread has priority of zero
+  bool isRealTime = true;
+  EXPECT_EQ(0, frc::GetCurrentThreadPriority(&isRealTime));
+  EXPECT_FALSE(isRealTime);
+
+  // Set thread priority to 15
+  EXPECT_TRUE(frc::SetCurrentThreadPriority(true, 15));
+
+  // Verify thread was given priority 15
+  EXPECT_EQ(15, frc::GetCurrentThreadPriority(&isRealTime));
+  EXPECT_TRUE(isRealTime);
+
+  // Set thread back to non-RT
+  EXPECT_TRUE(frc::SetCurrentThreadPriority(false, 0));
+
+  // Verify thread is now non-RT
+  EXPECT_EQ(0, frc::GetCurrentThreadPriority(&isRealTime));
+  EXPECT_FALSE(isRealTime);
+}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/RelayTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/RelayTest.cpp
index eb45eeb..cf3cfdf 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/RelayTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/RelayTest.cpp
@@ -1,93 +1,72 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Relay.h"  // NOLINT(build/include_order)
 
+#include <units/time.h>
+
 #include "TestBench.h"
 #include "frc/DigitalInput.h"
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
+static constexpr auto kDelayTime = 10_ms;
 
-static const double kDelayTime = 0.01;
+TEST(RelayTest, BothDirections) {
+  frc::Relay relay{TestBench::kRelayChannel};
+  frc::DigitalInput forward{TestBench::kFakeRelayForward};
+  frc::DigitalInput reverse{TestBench::kFakeRelayReverse};
 
-class RelayTest : public testing::Test {
- protected:
-  Relay* m_relay;
-  DigitalInput* m_forward;
-  DigitalInput* m_reverse;
+  // Set the relay to forward
+  relay.Set(frc::Relay::kForward);
+  frc::Wait(kDelayTime);
+  EXPECT_TRUE(forward.Get()) << "Relay did not set forward";
+  EXPECT_FALSE(reverse.Get()) << "Relay did not set forward";
+  EXPECT_EQ(relay.Get(), frc::Relay::kForward);
 
-  void SetUp() override {
-    m_relay = new Relay(TestBench::kRelayChannel);
-    m_forward = new DigitalInput(TestBench::kFakeRelayForward);
-    m_reverse = new DigitalInput(TestBench::kFakeRelayReverse);
-  }
+  // Set the relay to reverse
+  relay.Set(frc::Relay::kReverse);
+  frc::Wait(kDelayTime);
+  EXPECT_TRUE(reverse.Get()) << "Relay did not set reverse";
+  EXPECT_FALSE(forward.Get()) << "Relay did not set reverse";
+  EXPECT_EQ(relay.Get(), frc::Relay::kReverse);
 
-  void TearDown() override {
-    delete m_relay;
-    delete m_forward;
-    delete m_reverse;
-  }
+  // Set the relay to off
+  relay.Set(frc::Relay::kOff);
+  frc::Wait(kDelayTime);
+  EXPECT_FALSE(forward.Get()) << "Relay did not set off";
+  EXPECT_FALSE(reverse.Get()) << "Relay did not set off";
+  EXPECT_EQ(relay.Get(), frc::Relay::kOff);
 
-  void Reset() { m_relay->Set(Relay::kOff); }
-};
+  // Set the relay to on
+  relay.Set(frc::Relay::kOn);
+  frc::Wait(kDelayTime);
+  EXPECT_TRUE(forward.Get()) << "Relay did not set on";
+  EXPECT_TRUE(reverse.Get()) << "Relay did not set on";
+  EXPECT_EQ(relay.Get(), frc::Relay::kOn);
+}
 
-/**
- * Test the relay by setting it forward, reverse, off, and on.
- */
-TEST_F(RelayTest, Relay) {
-  Reset();
+TEST(RelayTest, ForwardOnly) {
+  frc::Relay relay{TestBench::kRelayChannel, frc::Relay::kForwardOnly};
+  frc::DigitalInput forward{TestBench::kFakeRelayForward};
+  frc::DigitalInput reverse{TestBench::kFakeRelayReverse};
 
-  // set the relay to forward
-  m_relay->Set(Relay::kForward);
-  Wait(kDelayTime);
-  EXPECT_TRUE(m_forward->Get()) << "Relay did not set forward";
-  EXPECT_FALSE(m_reverse->Get()) << "Relay did not set forward";
-  EXPECT_EQ(m_relay->Get(), Relay::kForward);
+  relay.Set(frc::Relay::kOn);
+  frc::Wait(kDelayTime);
+  EXPECT_TRUE(forward.Get()) << "Relay did not set forward";
+  EXPECT_FALSE(reverse.Get()) << "Relay did not set forward";
+  EXPECT_EQ(relay.Get(), frc::Relay::kOn);
+}
 
-  // set the relay to reverse
-  m_relay->Set(Relay::kReverse);
-  Wait(kDelayTime);
-  EXPECT_TRUE(m_reverse->Get()) << "Relay did not set reverse";
-  EXPECT_FALSE(m_forward->Get()) << "Relay did not set reverse";
-  EXPECT_EQ(m_relay->Get(), Relay::kReverse);
+TEST(RelayTest, ReverseOnly) {
+  frc::Relay relay{TestBench::kRelayChannel, frc::Relay::kReverseOnly};
+  frc::DigitalInput forward{TestBench::kFakeRelayForward};
+  frc::DigitalInput reverse{TestBench::kFakeRelayReverse};
 
-  // set the relay to off
-  m_relay->Set(Relay::kOff);
-  Wait(kDelayTime);
-  EXPECT_FALSE(m_forward->Get()) << "Relay did not set off";
-  EXPECT_FALSE(m_reverse->Get()) << "Relay did not set off";
-  EXPECT_EQ(m_relay->Get(), Relay::kOff);
-
-  // set the relay to on
-  m_relay->Set(Relay::kOn);
-  Wait(kDelayTime);
-  EXPECT_TRUE(m_forward->Get()) << "Relay did not set on";
-  EXPECT_TRUE(m_reverse->Get()) << "Relay did not set on";
-  EXPECT_EQ(m_relay->Get(), Relay::kOn);
-
-  // test forward direction
-  delete m_relay;
-  m_relay = new Relay(TestBench::kRelayChannel, Relay::kForwardOnly);
-
-  m_relay->Set(Relay::kOn);
-  Wait(kDelayTime);
-  EXPECT_TRUE(m_forward->Get()) << "Relay did not set forward";
-  EXPECT_FALSE(m_reverse->Get()) << "Relay did not set forward";
-  EXPECT_EQ(m_relay->Get(), Relay::kOn);
-
-  // test reverse direction
-  delete m_relay;
-  m_relay = new Relay(TestBench::kRelayChannel, Relay::kReverseOnly);
-
-  m_relay->Set(Relay::kOn);
-  Wait(kDelayTime);
-  EXPECT_FALSE(m_forward->Get()) << "Relay did not set reverse";
-  EXPECT_TRUE(m_reverse->Get()) << "Relay did not set reverse";
-  EXPECT_EQ(m_relay->Get(), Relay::kOn);
+  relay.Set(frc::Relay::kOn);
+  frc::Wait(kDelayTime);
+  EXPECT_FALSE(forward.Get()) << "Relay did not set reverse";
+  EXPECT_TRUE(reverse.Get()) << "Relay did not set reverse";
+  EXPECT_EQ(relay.Get(), frc::Relay::kOn);
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
index 2bfe1b3..1f5e360 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TestEnvironment.cpp
@@ -1,66 +1,65 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstdlib>
+#include <thread>
 
+#include <fmt/core.h>
 #include <hal/HAL.h>
-#include <wpi/raw_ostream.h>
 
 #include "frc/DriverStation.h"
-#include "frc/Timer.h"
 #include "frc/livewindow/LiveWindow.h"
 #include "gtest/gtest.h"
 #include "mockds/MockDS.h"
 
-using namespace frc;
+using namespace std::chrono_literals;
 
 class TestEnvironment : public testing::Environment {
   bool m_alreadySetUp = false;
   MockDS m_mockDS;
 
  public:
-  void SetUp() override {
-    /* Only set up once.  This allows gtest_repeat to be used to
-            automatically repeat tests. */
-    if (m_alreadySetUp) return;
+  TestEnvironment() {
+    // Only set up once. This allows gtest_repeat to be used to automatically
+    // repeat tests.
+    if (m_alreadySetUp) {
+      return;
+    }
     m_alreadySetUp = true;
 
     if (!HAL_Initialize(500, 0)) {
-      wpi::errs() << "FATAL ERROR: HAL could not be initialized\n";
+      fmt::print(stderr, "FATAL ERROR: HAL could not be initialized\n");
       std::exit(-1);
     }
 
-    m_mockDS.start();
+    m_mockDS.Start();
 
-    /* This sets up the network communications library to enable the driver
-            station. After starting network coms, it will loop until the driver
-            station returns that the robot is enabled, to ensure that tests
-            will be able to run on the hardware. */
+    // This sets up the network communications library to enable the driver
+    // station. After starting network coms, it will loop until the driver
+    // station returns that the robot is enabled, to ensure that tests will be
+    // able to run on the hardware.
     HAL_ObserveUserProgramStarting();
-    LiveWindow::GetInstance()->SetEnabled(false);
+    frc::LiveWindow::SetEnabled(false);
 
-    wpi::outs() << "Started coms\n";
+    fmt::print("Started coms\n");
 
     int enableCounter = 0;
-    while (!DriverStation::GetInstance().IsEnabled()) {
+    while (!frc::DriverStation::IsEnabled()) {
       if (enableCounter > 50) {
         // Robot did not enable properly after 5 seconds.
         // Force exit
-        wpi::errs() << " Failed to enable. Aborting\n";
+        fmt::print(stderr, " Failed to enable. Aborting\n");
         std::terminate();
       }
 
-      Wait(0.1);
+      std::this_thread::sleep_for(100ms);
 
-      wpi::outs() << "Waiting for enable: " << enableCounter++ << "\n";
+      fmt::print("Waiting for enable: {}\n", enableCounter++);
     }
   }
 
-  void TearDown() override { m_mockDS.stop(); }
+  ~TestEnvironment() override { m_mockDS.Stop(); }
 };
 
 testing::Environment* const environment =
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TiltPanCameraTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TiltPanCameraTest.cpp
index 852429b..42e52fc 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TiltPanCameraTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TiltPanCameraTest.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
+#include <units/time.h>
+
 #include "TestBench.h"
 #include "frc/ADXL345_SPI.h"
 #include "frc/AnalogGyro.h"
@@ -14,137 +13,79 @@
 #include "frc/Timer.h"
 #include "gtest/gtest.h"
 
-using namespace frc;
-
-static constexpr double kServoResetTime = 2.0;
-
-static constexpr double kTestAngle = 90.0;
-
 static constexpr double kTiltSetpoint0 = 0.22;
 static constexpr double kTiltSetpoint45 = 0.45;
 static constexpr double kTiltSetpoint90 = 0.68;
-static constexpr double kTiltTime = 1.0;
-static constexpr double kAccelerometerTolerance = 0.2;
-static constexpr double kSensitivity = 0.013;
 
 /**
  * A fixture for the camera with two servos and a gyro
  */
 class TiltPanCameraTest : public testing::Test {
  protected:
-  static AnalogGyro* m_gyro;
-  Servo *m_tilt, *m_pan;
-  Accelerometer* m_spiAccel;
+  frc::Servo m_tilt{TestBench::kCameraTiltChannel};
+  frc::Servo m_pan{TestBench::kCameraPanChannel};
+  frc::ADXL345_SPI m_spiAccel{frc::SPI::kOnboardCS1};
 
-  static void SetUpTestCase() {
-    // The gyro object blocks for 5 seconds in the constructor, so only
-    // construct it once for the whole test case
-    m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel);
-    m_gyro->SetSensitivity(kSensitivity);
+  TiltPanCameraTest() {
+    m_tilt.Set(kTiltSetpoint45);
+    m_pan.SetAngle(0.0);
+
+    // Wait for servos to reset
+    frc::Wait(2_s);
   }
 
-  static void TearDownTestCase() { delete m_gyro; }
+  /**
+   * Test if the servo turns 90 degrees and the gyroscope measures this angle
+   * Note servo on TestBench is not the same type of servo that servo class
+   * was designed for so setAngle is significantly off. This has been calibrated
+   * for the servo on the rig.
+   */
+  void GyroTests(frc::AnalogGyro& gyro) {
+    gyro.SetSensitivity(0.013);
 
-  void SetUp() override {
-    m_tilt = new Servo(TestBench::kCameraTiltChannel);
-    m_pan = new Servo(TestBench::kCameraPanChannel);
-    m_spiAccel = new ADXL345_SPI(SPI::kOnboardCS1);
+    gyro.Reset();
 
-    m_tilt->Set(kTiltSetpoint45);
-    m_pan->SetAngle(0.0);
+    // Accumulator needs a small amount of time to reset before being tested
+    frc::Wait(1_ms);
 
-    Wait(kServoResetTime);
+    // Verify  the gyro angle defaults to 0 immediately after being reset
+    EXPECT_NEAR(0.0, gyro.GetAngle(), 1.0);
 
-    m_gyro->Reset();
-  }
+    // Make sure that the gyro doesn't get jerked when the servo goes to zero.
+    m_pan.SetAngle(0.0);
+    frc::Wait(0.5_s);
+    gyro.Reset();
 
-  void DefaultGyroAngle();
-  void GyroAngle();
-  void GyroCalibratedParameters();
+    for (int32_t i = 0; i < 600; i++) {
+      m_pan.Set(i / 1000.0);
+      frc::Wait(1_ms);
+    }
 
-  void TearDown() override {
-    delete m_tilt;
-    delete m_pan;
-    delete m_spiAccel;
+    double gyroAngle = gyro.GetAngle();
+
+    EXPECT_NEAR(gyroAngle, 90.0, 10.0)
+        << "Gyro measured " << gyroAngle
+        << " degrees, servo should have turned 90 degrees";
   }
 };
 
-AnalogGyro* TiltPanCameraTest::m_gyro = nullptr;
+TEST_F(TiltPanCameraTest, Gyro) {
+  int cCenter;
+  double cOffset;
 
-/**
- * Test if the gyro angle defaults to 0 immediately after being reset.
- */
-void TiltPanCameraTest::DefaultGyroAngle() {
-  EXPECT_NEAR(0.0, m_gyro->GetAngle(), 1.0);
-}
+  {
+    frc::AnalogGyro gyro{TestBench::kCameraGyroChannel};
+    GyroTests(gyro);
 
-/**
- * Test if the servo turns 90 degrees and the gyroscope measures this angle
- * Note servo on TestBench is not the same type of servo that servo class
- * was designed for so setAngle is significantly off. This has been calibrated
- * for the servo on the rig.
- */
-void TiltPanCameraTest::GyroAngle() {
-  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
-  m_pan->SetAngle(0.0);
-  Wait(0.5);
-  m_gyro->Reset();
-
-  for (int32_t i = 0; i < 600; i++) {
-    m_pan->Set(i / 1000.0);
-    Wait(0.001);
+    // Gets calibrated parameters from previously calibrated gyro, allocates a
+    // new gyro with the given parameters for center and offset, and re-runs
+    // tests on the new gyro.
+    cCenter = gyro.GetCenter();
+    cOffset = gyro.GetOffset();
   }
 
-  double gyroAngle = m_gyro->GetAngle();
-
-  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
-      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
-      << kTestAngle << " degrees";
-}
-
-/**
- * Gets calibrated parameters from previously calibrated gyro, allocates a new
- * gyro with the given parameters for center and offset, and re-runs tests on
- * the new gyro.
- */
-void TiltPanCameraTest::GyroCalibratedParameters() {
-  uint32_t cCenter = m_gyro->GetCenter();
-  double cOffset = m_gyro->GetOffset();
-  delete m_gyro;
-  m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset);
-  m_gyro->SetSensitivity(kSensitivity);
-
-  // Default gyro angle test
-  // Accumulator needs a small amount of time to reset before being tested
-  m_gyro->Reset();
-  Wait(0.001);
-  EXPECT_NEAR(0.0, m_gyro->GetAngle(), 1.0);
-
-  // Gyro angle test
-  // Make sure that the gyro doesn't get jerked when the servo goes to zero.
-  m_pan->SetAngle(0.0);
-  Wait(0.5);
-  m_gyro->Reset();
-
-  for (int32_t i = 0; i < 600; i++) {
-    m_pan->Set(i / 1000.0);
-    Wait(0.001);
-  }
-
-  double gyroAngle = m_gyro->GetAngle();
-
-  EXPECT_NEAR(gyroAngle, kTestAngle, 10.0)
-      << "Gyro measured " << gyroAngle << " degrees, servo should have turned "
-      << kTestAngle << " degrees";
-}
-
-/**
- * Run all gyro tests in one function to make sure they are run in order.
- */
-TEST_F(TiltPanCameraTest, TestAllGyroTests) {
-  DefaultGyroAngle();
-  GyroAngle();
-  GyroCalibratedParameters();
+  frc::AnalogGyro gyro{TestBench::kCameraGyroChannel, cCenter, cOffset};
+  GyroTests(gyro);
 }
 
 /**
@@ -152,21 +93,24 @@
  * camera rotates
  */
 TEST_F(TiltPanCameraTest, SPIAccelerometer) {
-  m_tilt->Set(kTiltSetpoint0);
-  Wait(kTiltTime);
-  EXPECT_NEAR(-1.0, m_spiAccel->GetX(), kAccelerometerTolerance);
-  EXPECT_NEAR(0.0, m_spiAccel->GetY(), kAccelerometerTolerance);
-  EXPECT_NEAR(0.0, m_spiAccel->GetZ(), kAccelerometerTolerance);
+  static constexpr auto kTiltTime = 1_s;
+  static constexpr double kAccelerometerTolerance = 0.2;
 
-  m_tilt->Set(kTiltSetpoint45);
-  Wait(kTiltTime);
-  EXPECT_NEAR(-std::sqrt(0.5), m_spiAccel->GetX(), kAccelerometerTolerance);
-  EXPECT_NEAR(0.0, m_spiAccel->GetY(), kAccelerometerTolerance);
-  EXPECT_NEAR(std::sqrt(0.5), m_spiAccel->GetZ(), kAccelerometerTolerance);
+  m_tilt.Set(kTiltSetpoint0);
+  frc::Wait(kTiltTime);
+  EXPECT_NEAR(-1.0, m_spiAccel.GetX(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel.GetY(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel.GetZ(), kAccelerometerTolerance);
 
-  m_tilt->Set(kTiltSetpoint90);
-  Wait(kTiltTime);
-  EXPECT_NEAR(0.0, m_spiAccel->GetX(), kAccelerometerTolerance);
-  EXPECT_NEAR(0.0, m_spiAccel->GetY(), kAccelerometerTolerance);
-  EXPECT_NEAR(1.0, m_spiAccel->GetZ(), kAccelerometerTolerance);
+  m_tilt.Set(kTiltSetpoint45);
+  frc::Wait(kTiltTime);
+  EXPECT_NEAR(-std::sqrt(0.5), m_spiAccel.GetX(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel.GetY(), kAccelerometerTolerance);
+  EXPECT_NEAR(std::sqrt(0.5), m_spiAccel.GetZ(), kAccelerometerTolerance);
+
+  m_tilt.Set(kTiltSetpoint90);
+  frc::Wait(kTiltTime);
+  EXPECT_NEAR(0.0, m_spiAccel.GetX(), kAccelerometerTolerance);
+  EXPECT_NEAR(0.0, m_spiAccel.GetY(), kAccelerometerTolerance);
+  EXPECT_NEAR(1.0, m_spiAccel.GetZ(), kAccelerometerTolerance);
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TimerTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TimerTest.cpp
index b8f859c..99ec9f5 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TimerTest.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/TimerTest.cpp
@@ -1,41 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/Timer.h"  // NOLINT(build/include_order)
 
-#include "TestBench.h"
+#include <units/math.h>
+
 #include "gtest/gtest.h"
 
-using namespace frc;
+#define EXPECT_NEAR_UNITS(val1, val2, eps) \
+  EXPECT_LE(units::math::abs(val1 - val2), eps)
 
-static const double kWaitTime = 0.5;
+TEST(TimerTest, Wait) {
+  auto initialTime = frc::Timer::GetFPGATimestamp();
 
-class TimerTest : public testing::Test {
- protected:
-  Timer* m_timer;
+  frc::Wait(500_ms);
 
-  void SetUp() override { m_timer = new Timer; }
+  auto finalTime = frc::Timer::GetFPGATimestamp();
 
-  void TearDown() override { delete m_timer; }
-
-  void Reset() { m_timer->Reset(); }
-};
-
-/**
- * Test if the Wait function works
- */
-TEST_F(TimerTest, Wait) {
-  Reset();
-
-  double initialTime = m_timer->GetFPGATimestamp();
-
-  Wait(kWaitTime);
-
-  double finalTime = m_timer->GetFPGATimestamp();
-
-  EXPECT_NEAR(kWaitTime, finalTime - initialTime, 0.001);
+  EXPECT_NEAR_UNITS(500_ms, finalTime - initialTime, 1_ms);
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp
deleted file mode 100644
index fec442a..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/CommandTest.cpp
+++ /dev/null
@@ -1,440 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "command/MockCommand.h"
-#include "frc/DriverStation.h"
-#include "frc/RobotState.h"
-#include "frc/Timer.h"
-#include "frc/commands/CommandGroup.h"
-#include "frc/commands/Scheduler.h"
-#include "frc/commands/Subsystem.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class CommandTest : public testing::Test {
- protected:
-  void SetUp() override { Scheduler::GetInstance()->SetEnabled(true); }
-
-  /**
-   * Tears Down the Scheduler at the end of each test.
-   * Must be called at the end of each test inside each test in order to prevent
-   * them being deallocated
-   * when they leave the scope of the test (causing a segfault).
-   * This can not be done within the virtual void Teardown() method because it
-   * is called outside of the
-   * scope of the test.
-   */
-  void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); }
-
-  void AssertCommandState(MockCommand* command, int32_t initialize,
-                          int32_t execute, int32_t isFinished, int32_t end,
-                          int32_t interrupted) {
-    EXPECT_EQ(initialize, command->GetInitializeCount());
-    EXPECT_EQ(execute, command->GetExecuteCount());
-    EXPECT_EQ(isFinished, command->GetIsFinishedCount());
-    EXPECT_EQ(end, command->GetEndCount());
-    EXPECT_EQ(interrupted, command->GetInterruptedCount());
-  }
-};
-
-class ASubsystem : public Subsystem {
- private:
-  Command* m_command = nullptr;
-
- public:
-  explicit ASubsystem(const std::string& name) : Subsystem(name) {}
-
-  void InitDefaultCommand() override {
-    if (m_command != nullptr) {
-      SetDefaultCommand(m_command);
-    }
-  }
-
-  void Init(Command* command) { m_command = command; }
-};
-
-// CommandParallelGroupTest ported from CommandParallelGroupTest.java
-TEST_F(CommandTest, ParallelCommands) {
-  auto command1 = new MockCommand;
-  auto command2 = new MockCommand;
-  CommandGroup commandGroup;
-
-  commandGroup.AddParallel(command1);
-  commandGroup.AddParallel(command2);
-
-  AssertCommandState(command1, 0, 0, 0, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  commandGroup.Start();
-  AssertCommandState(command1, 0, 0, 0, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 0, 0, 0, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 0);
-  AssertCommandState(command2, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 2, 2, 0, 0);
-  AssertCommandState(command2, 1, 2, 2, 0, 0);
-  command1->SetHasFinished(true);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 3, 3, 1, 0);
-  AssertCommandState(command2, 1, 3, 3, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 3, 3, 1, 0);
-  AssertCommandState(command2, 1, 4, 4, 0, 0);
-  command2->SetHasFinished(true);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 3, 3, 1, 0);
-  AssertCommandState(command2, 1, 5, 5, 1, 0);
-
-  TeardownScheduler();
-}
-// END CommandParallelGroupTest
-
-// CommandScheduleTest ported from CommandScheduleTest.java
-TEST_F(CommandTest, RunAndTerminate) {
-  MockCommand command;
-  command.Start();
-  AssertCommandState(&command, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 2, 2, 0, 0);
-  command.SetHasFinished(true);
-  AssertCommandState(&command, 1, 2, 2, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 3, 3, 1, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 3, 3, 1, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(CommandTest, RunAndCancel) {
-  MockCommand command;
-  command.Start();
-  AssertCommandState(&command, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 2, 2, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 3, 3, 0, 0);
-  command.Cancel();
-  AssertCommandState(&command, 1, 3, 3, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 3, 3, 0, 1);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 3, 3, 0, 1);
-
-  TeardownScheduler();
-}
-// END CommandScheduleTest
-
-// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
-TEST_F(CommandTest, ThreeCommandOnSubSystem) {
-  ASubsystem subsystem("Three Command Test Subsystem");
-  auto command1 = new MockCommand;
-  command1->Requires(&subsystem);
-  auto command2 = new MockCommand;
-  command2->Requires(&subsystem);
-  auto command3 = new MockCommand;
-  command3->Requires(&subsystem);
-
-  CommandGroup commandGroup;
-  commandGroup.AddSequential(command1, 1.0);
-  commandGroup.AddSequential(command2, 2.0);
-  commandGroup.AddSequential(command3);
-
-  AssertCommandState(command1, 0, 0, 0, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  AssertCommandState(command3, 0, 0, 0, 0, 0);
-
-  commandGroup.Start();
-  AssertCommandState(command1, 0, 0, 0, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  AssertCommandState(command3, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 0, 0, 0, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  AssertCommandState(command3, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 0);
-  AssertCommandState(command2, 0, 0, 0, 0, 0);
-  AssertCommandState(command3, 0, 0, 0, 0, 0);
-  Wait(1);  // command 1 timeout
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 1, 1, 0, 0);
-  AssertCommandState(command3, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 2, 2, 0, 0);
-  AssertCommandState(command3, 0, 0, 0, 0, 0);
-  Wait(2);  // command 2 timeout
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 2, 2, 0, 1);
-  AssertCommandState(command3, 1, 1, 1, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 2, 2, 0, 1);
-  AssertCommandState(command3, 1, 2, 2, 0, 0);
-  command3->SetHasFinished(true);
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 2, 2, 0, 1);
-  AssertCommandState(command3, 1, 2, 2, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 2, 2, 0, 1);
-  AssertCommandState(command3, 1, 3, 3, 1, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(command1, 1, 1, 1, 0, 1);
-  AssertCommandState(command2, 1, 2, 2, 0, 1);
-  AssertCommandState(command3, 1, 3, 3, 1, 0);
-
-  TeardownScheduler();
-}
-// END CommandSequentialGroupTest
-
-// CommandSequentialGroupTest ported from CommandSequentialGroupTest.java
-TEST_F(CommandTest, OneCommandSupersedingAnotherBecauseOfDependencies) {
-  ASubsystem subsystem("Command Superseding Test Subsystem");
-  MockCommand command1;
-  command1.Requires(&subsystem);
-  MockCommand command2;
-  command2.Requires(&subsystem);
-
-  AssertCommandState(&command1, 0, 0, 0, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  command1.Start();
-  AssertCommandState(&command1, 0, 0, 0, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 0, 0, 0, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 1, 1, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 2, 2, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 3, 3, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  command2.Start();
-  AssertCommandState(&command1, 1, 3, 3, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 4, 4, 0, 1);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 4, 4, 0, 1);
-  AssertCommandState(&command2, 1, 1, 1, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 4, 4, 0, 1);
-  AssertCommandState(&command2, 1, 2, 2, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 4, 4, 0, 1);
-  AssertCommandState(&command2, 1, 3, 3, 0, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(CommandTest,
-       OneCommandFailingSupersedingBecauseFirstCanNotBeInterrupted) {
-  ASubsystem subsystem("Command Superseding Test Subsystem");
-  MockCommand command1;
-
-  command1.Requires(&subsystem);
-
-  command1.SetInterruptible(false);
-  MockCommand command2;
-  command2.Requires(&subsystem);
-
-  AssertCommandState(&command1, 0, 0, 0, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  command1.Start();
-  AssertCommandState(&command1, 0, 0, 0, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 0, 0, 0, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 1, 1, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 2, 2, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 3, 3, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  command2.Start();
-  AssertCommandState(&command1, 1, 3, 3, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command1, 1, 4, 4, 0, 0);
-  AssertCommandState(&command2, 0, 0, 0, 0, 0);
-
-  TeardownScheduler();
-}
-
-// END CommandSequentialGroupTest
-
-class ModifiedMockCommand : public MockCommand {
- public:
-  ModifiedMockCommand() : MockCommand() { SetTimeout(2.0); }
-  bool IsFinished() override {
-    return MockCommand::IsFinished() || IsTimedOut();
-  }
-};
-
-TEST_F(CommandTest, TwoSecondTimeout) {
-  ASubsystem subsystem("Two Second Timeout Test Subsystem");
-  ModifiedMockCommand command;
-  command.Requires(&subsystem);
-
-  command.Start();
-  AssertCommandState(&command, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 2, 2, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 3, 3, 0, 0);
-  Wait(2);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 4, 4, 1, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(&command, 1, 4, 4, 1, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(CommandTest, DefaultCommandWhereTheInteruptingCommandEndsItself) {
-  ASubsystem subsystem("Default Command Test Subsystem");
-  auto defaultCommand = new MockCommand;
-  defaultCommand->Requires(&subsystem);
-  MockCommand anotherCommand;
-  anotherCommand.Requires(&subsystem);
-
-  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
-  subsystem.Init(defaultCommand);
-
-  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
-
-  anotherCommand.Start();
-  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
-  AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
-  anotherCommand.SetHasFinished(true);
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 3, 3, 1, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 3, 3, 1, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 3, 3, 1, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(CommandTest, DefaultCommandsInterruptingCommandCanceled) {
-  ASubsystem subsystem("Default Command Test Subsystem");
-  auto defaultCommand = new MockCommand;
-  defaultCommand->Requires(&subsystem);
-  MockCommand anotherCommand;
-  anotherCommand.Requires(&subsystem);
-
-  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
-  subsystem.Init(defaultCommand);
-  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
-
-  anotherCommand.Start();
-  AssertCommandState(defaultCommand, 1, 2, 2, 0, 0);
-  AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 0, 0, 0, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 1, 1, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
-  anotherCommand.Cancel();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 0);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 1, 3, 3, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 1);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 2, 4, 4, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 1);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(defaultCommand, 2, 5, 5, 0, 1);
-  AssertCommandState(&anotherCommand, 1, 2, 2, 0, 1);
-
-  TeardownScheduler();
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/ConditionalCommandTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/ConditionalCommandTest.cpp
deleted file mode 100644
index ca14e55..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/ConditionalCommandTest.cpp
+++ /dev/null
@@ -1,435 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "command/MockCommand.h"
-#include "command/MockConditionalCommand.h"
-#include "frc/DriverStation.h"
-#include "frc/RobotState.h"
-#include "frc/commands/ConditionalCommand.h"
-#include "frc/commands/Scheduler.h"
-#include "frc/commands/Subsystem.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class ConditionalCommandTest : public testing::Test {
- public:
-  MockConditionalCommand* m_command;
-  MockCommand* m_onTrue;
-  MockCommand* m_onFalse;
-  MockConditionalCommand* m_commandNull;
-  Subsystem* m_subsystem;
-
- protected:
-  void SetUp() override {
-    Scheduler::GetInstance()->SetEnabled(true);
-
-    m_subsystem = new Subsystem("MockSubsystem");
-    m_onTrue = new MockCommand(m_subsystem);
-    m_onFalse = new MockCommand(m_subsystem);
-    m_command = new MockConditionalCommand(m_onTrue, m_onFalse);
-    m_commandNull = new MockConditionalCommand(m_onTrue, nullptr);
-  }
-
-  void TearDown() override { delete m_command; }
-
-  /**
-   * Tears Down the Scheduler at the end of each test.
-   *
-   * Must be called at the end of each test inside each test in order to prevent
-   * them being deallocated when they leave the scope of the test (causing a
-   * segfault). This cannot be done within the virtual void Teardown() method
-   * because it is called outside of the scope of the test.
-   */
-  void TeardownScheduler() { Scheduler::GetInstance()->ResetAll(); }
-
-  void AssertCommandState(MockCommand& command, int32_t initialize,
-                          int32_t execute, int32_t isFinished, int32_t end,
-                          int32_t interrupted) {
-    EXPECT_EQ(initialize, command.GetInitializeCount());
-    EXPECT_EQ(execute, command.GetExecuteCount());
-    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
-    EXPECT_EQ(end, command.GetEndCount());
-    EXPECT_EQ(interrupted, command.GetInterruptedCount());
-  }
-
-  void AssertConditionalCommandState(MockConditionalCommand& command,
-                                     int32_t initialize, int32_t execute,
-                                     int32_t isFinished, int32_t end,
-                                     int32_t interrupted) {
-    EXPECT_EQ(initialize, command.GetInitializeCount());
-    EXPECT_EQ(execute, command.GetExecuteCount());
-    EXPECT_EQ(isFinished, command.GetIsFinishedCount());
-    EXPECT_EQ(end, command.GetEndCount());
-    EXPECT_EQ(interrupted, command.GetInterruptedCount());
-  }
-};
-
-TEST_F(ConditionalCommandTest, OnTrueTest) {
-  m_command->SetCondition(true);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("6");
-  m_onTrue->SetHasFinished(true);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-  SCOPED_TRACE("7");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-
-  EXPECT_TRUE(m_onTrue->GetInitializeCount() > 0)
-      << "Did not initialize the true command\n";
-  EXPECT_TRUE(m_onFalse->GetInitializeCount() == 0)
-      << "Initialized the false command\n";
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, OnFalseTest) {
-  m_command->SetCondition(false);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("6");
-  m_onFalse->SetHasFinished(true);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-  SCOPED_TRACE("7");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onFalse, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-
-  EXPECT_TRUE(m_onFalse->GetInitializeCount() > 0)
-      << "Did not initialize the false command";
-  EXPECT_TRUE(m_onTrue->GetInitializeCount() == 0)
-      << "Initialized the true command";
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, CancelSubCommandTest) {
-  m_command->SetCondition(true);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("6");
-  m_onTrue->Cancel();
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-  SCOPED_TRACE("7");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, CancelCondCommandTest) {
-  m_command->SetCondition(true);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("6");
-  m_command->Cancel();
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
-  SCOPED_TRACE("7");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 1);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 1);
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, OnTrueTwiceTest) {
-  m_command->SetCondition(true);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("6");
-  m_onTrue->SetHasFinished(true);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-  SCOPED_TRACE("7");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-
-  m_onTrue->ResetCounters();
-  m_command->ResetCounters();
-  Scheduler::GetInstance()->AddCommand(m_command);
-
-  SCOPED_TRACE("11");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("12");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("13");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("14");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("15");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("16");
-  m_onTrue->SetHasFinished(true);
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-  SCOPED_TRACE("17");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 1, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, OnTrueInstantTest) {
-  m_command->SetCondition(true);
-  m_onTrue->SetHasFinished(true);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 1, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 1, 0);
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, CancelRequiresTest) {
-  m_command->SetCondition(true);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_command);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 1, 1, 0, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 1, 1, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 2, 2, 0, 0);
-  SCOPED_TRACE("5");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 2, 2, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 3, 3, 0, 0);
-  SCOPED_TRACE("6");
-  m_onFalse->Start();
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 0);
-  AssertCommandState(*m_onFalse, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
-  SCOPED_TRACE("7");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 1, 3, 3, 0, 1);
-  AssertCommandState(*m_onFalse, 1, 1, 1, 0, 0);
-  AssertConditionalCommandState(*m_command, 1, 4, 4, 0, 1);
-
-  TeardownScheduler();
-}
-
-TEST_F(ConditionalCommandTest, OnFalseNullTest) {
-  m_command->SetCondition(false);
-
-  SCOPED_TRACE("1");
-  Scheduler::GetInstance()->AddCommand(m_commandNull);
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("2");
-  Scheduler::GetInstance()->Run();  // init command and select m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_commandNull, 0, 0, 0, 0, 0);
-  SCOPED_TRACE("3");
-  Scheduler::GetInstance()->Run();  // init m_onTrue
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
-  SCOPED_TRACE("4");
-  Scheduler::GetInstance()->Run();
-  AssertCommandState(*m_onTrue, 0, 0, 0, 0, 0);
-  AssertConditionalCommandState(*m_commandNull, 1, 1, 1, 1, 0);
-
-  TeardownScheduler();
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/MockCommand.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/MockCommand.cpp
deleted file mode 100644
index bcaed0b..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/MockCommand.cpp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "command/MockCommand.h"
-
-using namespace frc;
-
-MockCommand::MockCommand(Subsystem* subsys) : MockCommand() {
-  Requires(subsys);
-}
-
-MockCommand::MockCommand() {
-  m_initializeCount = 0;
-  m_executeCount = 0;
-  m_isFinishedCount = 0;
-  m_hasFinished = false;
-  m_endCount = 0;
-  m_interruptedCount = 0;
-}
-
-bool MockCommand::HasInitialized() { return GetInitializeCount() > 0; }
-
-bool MockCommand::HasEnd() { return GetEndCount() > 0; }
-
-bool MockCommand::HasInterrupted() { return GetInterruptedCount() > 0; }
-
-void MockCommand::Initialize() { ++m_initializeCount; }
-
-void MockCommand::Execute() { ++m_executeCount; }
-
-bool MockCommand::IsFinished() {
-  ++m_isFinishedCount;
-  return IsHasFinished();
-}
-
-void MockCommand::End() { ++m_endCount; }
-
-void MockCommand::Interrupted() { ++m_interruptedCount; }
-
-void MockCommand::ResetCounters() {
-  m_initializeCount = 0;
-  m_executeCount = 0;
-  m_isFinishedCount = 0;
-  m_hasFinished = false;
-  m_endCount = 0;
-  m_interruptedCount = 0;
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/MockConditionalCommand.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/MockConditionalCommand.cpp
deleted file mode 100644
index 9434131..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/command/MockConditionalCommand.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "command/MockConditionalCommand.h"
-
-using namespace frc;
-
-MockConditionalCommand::MockConditionalCommand(MockCommand* onTrue,
-                                               MockCommand* onFalse)
-    : ConditionalCommand(onTrue, onFalse) {
-  m_initializeCount = 0;
-  m_executeCount = 0;
-  m_isFinishedCount = 0;
-  m_endCount = 0;
-  m_interruptedCount = 0;
-}
-
-void MockConditionalCommand::SetCondition(bool condition) {
-  m_condition = condition;
-}
-
-bool MockConditionalCommand::Condition() { return m_condition; }
-
-bool MockConditionalCommand::HasInitialized() {
-  return GetInitializeCount() > 0;
-}
-
-bool MockConditionalCommand::HasEnd() { return GetEndCount() > 0; }
-
-bool MockConditionalCommand::HasInterrupted() {
-  return GetInterruptedCount() > 0;
-}
-
-void MockConditionalCommand::Initialize() { ++m_initializeCount; }
-
-void MockConditionalCommand::Execute() { ++m_executeCount; }
-
-bool MockConditionalCommand::IsFinished() {
-  ++m_isFinishedCount;
-  return ConditionalCommand::IsFinished();
-}
-
-void MockConditionalCommand::End() { ++m_endCount; }
-
-void MockConditionalCommand::Interrupted() { ++m_interruptedCount; }
-
-void MockConditionalCommand::ResetCounters() {
-  m_initializeCount = 0;
-  m_executeCount = 0;
-  m_isFinishedCount = 0;
-  m_endCount = 0;
-  m_interruptedCount = 0;
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/main.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/main.cpp
deleted file mode 100644
index 1e5ecf0..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/main.cpp
+++ /dev/null
@@ -1,14 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "gtest/gtest.h"
-
-int main(int argc, char** argv) {
-  ::testing::InitGoogleTest(&argc, argv);
-  int ret = RUN_ALL_TESTS();
-  return ret;
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp
index 173a23c..b2e5c2d 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.cpp
@@ -1,42 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "MockDS.h"
 
 #include <stdint.h>
 
+#include <string_view>
+
+#include <fmt/core.h>
 #include <hal/cpp/fpga_clock.h>
 #include <wpi/Logger.h>
-#include <wpi/SmallString.h>
 #include <wpi/SmallVector.h>
 #include <wpi/UDPClient.h>
-#include <wpi/raw_ostream.h>
 
 static void LoggerFunc(unsigned int level, const char* file, unsigned int line,
                        const char* msg) {
-  wpi::SmallString<128> buf;
-  wpi::raw_svector_ostream oss(buf);
   if (level == 20) {
-    oss << "DS: " << msg << '\n';
-    wpi::errs() << oss.str();
+    fmt::print(stderr, "DS: {}\n", msg);
     return;
   }
 
-  wpi::StringRef levelmsg;
-  if (level >= 50)
-    levelmsg = "CRITICAL: ";
-  else if (level >= 40)
-    levelmsg = "ERROR: ";
-  else if (level >= 30)
-    levelmsg = "WARNING: ";
-  else
+  std::string_view levelmsg;
+  if (level >= 50) {
+    levelmsg = "CRITICAL";
+  } else if (level >= 40) {
+    levelmsg = "ERROR";
+  } else if (level >= 30) {
+    levelmsg = "WARNING";
+  } else {
     return;
-  oss << "DS: " << levelmsg << msg << " (" << file << ':' << line << ")\n";
-  wpi::errs() << oss.str();
+  }
+  fmt::print(stderr, "DS: {}: {} ({}:{})\n", levelmsg, msg, file, line);
 }
 
 static void generateEnabledDsPacket(wpi::SmallVectorImpl<uint8_t>& data,
@@ -50,10 +45,10 @@
   data.push_back(0x00);  // red 1 station
 }
 
-using namespace frc;
-
-void MockDS::start() {
-  if (m_active) return;
+void MockDS::Start() {
+  if (m_active) {
+    return;
+  }
   m_active = true;
   m_thread = std::thread([&]() {
     wpi::Logger logger(LoggerFunc);
@@ -83,7 +78,9 @@
   });
 }
 
-void MockDS::stop() {
+void MockDS::Stop() {
   m_active = false;
-  if (m_thread.joinable()) m_thread.join();
+  if (m_thread.joinable()) {
+    m_thread.join();
+  }
 }
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.h b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.h
index 99b17b1..da5fcd9 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.h
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/mockds/MockDS.h
@@ -1,28 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <atomic>
 #include <thread>
 
-namespace frc {
 class MockDS {
  public:
   MockDS() = default;
-  ~MockDS() { stop(); }
+  ~MockDS() { Stop(); }
   MockDS(const MockDS& other) = delete;
   MockDS& operator=(const MockDS& other) = delete;
 
-  void start();
-  void stop();
+  void Start();
+  void Stop();
 
  private:
   std::thread m_thread;
   std::atomic_bool m_active{false};
 };
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/MockActuatorSendable.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/MockActuatorSendable.cpp
deleted file mode 100644
index 172d7c6..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/MockActuatorSendable.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "shuffleboard/MockActuatorSendable.h"
-
-#include "frc/smartdashboard/SendableRegistry.h"
-
-using namespace frc;
-
-MockActuatorSendable::MockActuatorSendable(wpi::StringRef name) {
-  SendableRegistry::GetInstance().Add(this, name);
-}
-
-void MockActuatorSendable::InitSendable(SendableBuilder& builder) {
-  builder.SetActuator(true);
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
deleted file mode 100644
index d06f510..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardInstanceTest.cpp
+++ /dev/null
@@ -1,105 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/shuffleboard/ShuffleboardInstance.h"  // NOLINT(build/include_order)
-
-#include <memory>
-#include <string>
-
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-
-#include "frc/shuffleboard/ShuffleboardInstance.h"
-#include "gtest/gtest.h"
-#include "shuffleboard/MockActuatorSendable.h"
-
-using namespace frc;
-
-class ShuffleboardInstanceTest : public testing::Test {
-  void SetUp() override {
-    m_ntInstance = nt::NetworkTableInstance::Create();
-    m_shuffleboardInstance =
-        std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
-  }
-
- protected:
-  nt::NetworkTableInstance m_ntInstance;
-  std::unique_ptr<detail::ShuffleboardInstance> m_shuffleboardInstance;
-};
-
-TEST_F(ShuffleboardInstanceTest, PathFluent) {
-  auto entry = m_shuffleboardInstance->GetTab("Tab Title")
-                   .GetLayout("List", "List Layout")
-                   .Add("Data", "string")
-                   .WithWidget("Text View")
-                   .GetEntry();
-
-  EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
-  EXPECT_EQ("/Shuffleboard/Tab Title/List/Data", entry.GetName())
-      << "Entry path generated incorrectly";
-}
-
-TEST_F(ShuffleboardInstanceTest, NestedLayoutsFluent) {
-  auto entry = m_shuffleboardInstance->GetTab("Tab")
-                   .GetLayout("First", "List")
-                   .GetLayout("Second", "List")
-                   .GetLayout("Third", "List")
-                   .GetLayout("Fourth", "List")
-                   .Add("Value", "string")
-                   .GetEntry();
-
-  EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
-  EXPECT_EQ("/Shuffleboard/Tab/First/Second/Third/Fourth/Value",
-            entry.GetName())
-      << "Entry path generated incorrectly";
-}
-
-TEST_F(ShuffleboardInstanceTest, NestedLayoutsOop) {
-  ShuffleboardTab& tab = m_shuffleboardInstance->GetTab("Tab");
-  ShuffleboardLayout& first = tab.GetLayout("First", "List");
-  ShuffleboardLayout& second = first.GetLayout("Second", "List");
-  ShuffleboardLayout& third = second.GetLayout("Third", "List");
-  ShuffleboardLayout& fourth = third.GetLayout("Fourth", "List");
-  SimpleWidget& widget = fourth.Add("Value", "string");
-  auto entry = widget.GetEntry();
-
-  EXPECT_EQ("string", entry.GetString("")) << "Wrong entry value";
-  EXPECT_EQ("/Shuffleboard/Tab/First/Second/Third/Fourth/Value",
-            entry.GetName())
-      << "Entry path generated incorrectly";
-}
-
-TEST_F(ShuffleboardInstanceTest, LayoutTypeIsSet) {
-  std::string layoutType = "Type";
-  m_shuffleboardInstance->GetTab("Tab").GetLayout("Title", layoutType);
-  m_shuffleboardInstance->Update();
-  nt::NetworkTableEntry entry = m_ntInstance.GetEntry(
-      "/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
-  EXPECT_EQ(layoutType, entry.GetString("Not Set")) << "Layout type not set";
-}
-
-TEST_F(ShuffleboardInstanceTest, NestedActuatorWidgetsAreDisabled) {
-  MockActuatorSendable sendable("Actuator");
-  m_shuffleboardInstance->GetTab("Tab")
-      .GetLayout("Title", "Layout")
-      .Add(sendable);
-  auto controllableEntry =
-      m_ntInstance.GetEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
-  m_shuffleboardInstance->Update();
-
-  // Note: we use the unsafe `GetBoolean()` method because if the value is NOT
-  // a boolean, or if it is not present, then something has clearly gone very,
-  // very wrong
-  bool controllable = controllableEntry.GetValue()->GetBoolean();
-  // Sanity check
-  EXPECT_TRUE(controllable)
-      << "The nested actuator widget should be enabled by default";
-  m_shuffleboardInstance->DisableActuatorWidgets();
-  controllable = controllableEntry.GetValue()->GetBoolean();
-  EXPECT_FALSE(controllable)
-      << "The nested actuator widget should have been disabled";
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardTabTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
deleted file mode 100644
index 23f3e3a..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardTabTest.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include <array>
-#include <memory>
-#include <string>
-
-#include <networktables/NetworkTableEntry.h>
-#include <networktables/NetworkTableInstance.h>
-
-#include "frc/commands/InstantCommand.h"
-#include "frc/shuffleboard/ShuffleboardInstance.h"
-#include "frc/shuffleboard/ShuffleboardTab.h"
-#include "frc/smartdashboard/Sendable.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class ShuffleboardTabTest : public testing::Test {
-  void SetUp() override {
-    m_ntInstance = nt::NetworkTableInstance::Create();
-    m_instance = std::make_unique<detail::ShuffleboardInstance>(m_ntInstance);
-    m_tab = &(m_instance->GetTab("Tab"));
-  }
-
- protected:
-  nt::NetworkTableInstance m_ntInstance;
-  ShuffleboardTab* m_tab;
-  std::unique_ptr<detail::ShuffleboardInstance> m_instance;
-};
-
-TEST_F(ShuffleboardTabTest, AddDouble) {
-  auto entry = m_tab->Add("Double", 1.0).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Double", entry.GetName());
-  EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
-}
-
-TEST_F(ShuffleboardTabTest, AddInteger) {
-  auto entry = m_tab->Add("Int", 1).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Int", entry.GetName());
-  EXPECT_FLOAT_EQ(1.0, entry.GetValue()->GetDouble());
-}
-
-TEST_F(ShuffleboardTabTest, AddBoolean) {
-  auto entry = m_tab->Add("Bool", false).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/Bool", entry.GetName());
-  EXPECT_FALSE(entry.GetValue()->GetBoolean());
-}
-
-TEST_F(ShuffleboardTabTest, AddString) {
-  auto entry = m_tab->Add("String", "foobar").GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/String", entry.GetName());
-  EXPECT_EQ("foobar", entry.GetValue()->GetString());
-}
-
-TEST_F(ShuffleboardTabTest, AddNamedSendableWithProperties) {
-  InstantCommand sendable("Command");
-  std::string widgetType = "Command Widget";
-  wpi::StringMap<std::shared_ptr<nt::Value>> map;
-  map.try_emplace("foo", nt::Value::MakeDouble(1234));
-  map.try_emplace("bar", nt::Value::MakeString("baz"));
-  m_tab->Add(sendable).WithWidget(widgetType).WithProperties(map);
-
-  m_instance->Update();
-  std::string meta = "/Shuffleboard/.metadata/Tab/Command";
-
-  EXPECT_EQ(1234, m_ntInstance.GetEntry(meta + "/Properties/foo").GetDouble(-1))
-      << "Property 'foo' not set correctly";
-  EXPECT_EQ("baz",
-            m_ntInstance.GetEntry(meta + "/Properties/bar").GetString(""))
-      << "Property 'bar' not set correctly";
-  EXPECT_EQ(widgetType,
-            m_ntInstance.GetEntry(meta + "/PreferredComponent").GetString(""))
-      << "Preferred component not set correctly";
-}
-
-TEST_F(ShuffleboardTabTest, AddNumberArray) {
-  std::array<double, 3> expect = {{1.0, 2.0, 3.0}};
-  auto entry = m_tab->Add("DoubleArray", expect).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/DoubleArray", entry.GetName());
-
-  auto actual = entry.GetValue()->GetDoubleArray();
-  EXPECT_EQ(expect.size(), actual.size());
-  for (size_t i = 0; i < expect.size(); i++) {
-    EXPECT_FLOAT_EQ(expect[i], actual[i]);
-  }
-}
-
-TEST_F(ShuffleboardTabTest, AddBooleanArray) {
-  std::array<bool, 2> expect = {{true, false}};
-  auto entry = m_tab->Add("BoolArray", expect).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/BoolArray", entry.GetName());
-
-  auto actual = entry.GetValue()->GetBooleanArray();
-  EXPECT_EQ(expect.size(), actual.size());
-  for (size_t i = 0; i < expect.size(); i++) {
-    EXPECT_EQ(expect[i], actual[i]);
-  }
-}
-
-TEST_F(ShuffleboardTabTest, AddStringArray) {
-  std::array<std::string, 2> expect = {{"foo", "bar"}};
-  auto entry = m_tab->Add("StringArray", expect).GetEntry();
-  EXPECT_EQ("/Shuffleboard/Tab/StringArray", entry.GetName());
-
-  auto actual = entry.GetValue()->GetStringArray();
-  EXPECT_EQ(expect.size(), actual.size());
-  for (size_t i = 0; i < expect.size(); i++) {
-    EXPECT_EQ(expect[i], actual[i]);
-  }
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardTest.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardTest.cpp
deleted file mode 100644
index d39d59d..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/cpp/shuffleboard/ShuffleboardTest.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/shuffleboard/Shuffleboard.h"
-#include "frc/shuffleboard/ShuffleboardTab.h"
-#include "gtest/gtest.h"
-
-using namespace frc;
-
-class ShuffleboardTest : public testing::Test {};
-
-TEST_F(ShuffleboardTest, TabObjectsCached) {
-  ShuffleboardTab& tab1 = Shuffleboard::GetTab("testTabObjectsCached");
-  ShuffleboardTab& tab2 = Shuffleboard::GetTab("testTabObjectsCached");
-  EXPECT_EQ(&tab1, &tab2) << "Tab objects were not cached";
-}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/dt/Main.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/dt/Main.cpp
new file mode 100644
index 0000000..a3e363e
--- /dev/null
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/dt/Main.cpp
@@ -0,0 +1,5 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+int main() {}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/dt/main.cpp b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/dt/main.cpp
deleted file mode 100644
index e324b44..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/dt/main.cpp
+++ /dev/null
@@ -1,8 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-int main() {}
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/TestBench.h b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/TestBench.h
index f9b5f30..0b6cf33 100644
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/TestBench.h
+++ b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/TestBench.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,48 +9,49 @@
 class TestBench {
  public:
   /* Analog input channels */
-  static const uint32_t kCameraGyroChannel = 0;
-  static const uint32_t kFakeCompressorChannel = 1;
-  static const uint32_t kFakeAnalogOutputChannel = 2;
+  static constexpr uint32_t kCameraGyroChannel = 0;
+  static constexpr uint32_t kFakeCompressorChannel = 1;
+  static constexpr uint32_t kFakeAnalogOutputChannel = 2;
 
   /* Analog output channels */
-  static const uint32_t kAnalogOutputChannel = 0;
+  static constexpr uint32_t kAnalogOutputChannel = 0;
 
   /* DIO channels */
-  static const uint32_t kTalonEncoderChannelA = 0;
-  static const uint32_t kTalonEncoderChannelB = 1;
-  static const uint32_t kVictorEncoderChannelA = 2;
-  static const uint32_t kVictorEncoderChannelB = 3;
-  static const uint32_t kJaguarEncoderChannelA = 4;
-  static const uint32_t kJaguarEncoderChannelB = 5;
-  static const uint32_t kLoop1OutputChannel = 6;
-  static const uint32_t kLoop1InputChannel = 7;
-  static const uint32_t kLoop2OutputChannel = 8;
-  static const uint32_t kLoop2InputChannel = 9;
+  static constexpr uint32_t kTalonEncoderChannelA = 0;
+  static constexpr uint32_t kTalonEncoderChannelB = 1;
+  static constexpr uint32_t kVictorEncoderChannelA = 2;
+  static constexpr uint32_t kVictorEncoderChannelB = 3;
+  static constexpr uint32_t kJaguarEncoderChannelA = 4;
+  static constexpr uint32_t kJaguarEncoderChannelB = 5;
+  static constexpr uint32_t kLoop1OutputChannel = 6;
+  static constexpr uint32_t kLoop1InputChannel = 7;
+  static constexpr uint32_t kLoop2OutputChannel = 8;
+  static constexpr uint32_t kLoop2InputChannel = 9;
 
   /* PWM channels */
-  static const uint32_t kVictorChannel = 1;
-  static const uint32_t kJaguarChannel = 2;
-  static const uint32_t kCameraPanChannel = 8;
-  static const uint32_t kCameraTiltChannel = 9;
+  static constexpr uint32_t kVictorChannel = 1;
+  static constexpr uint32_t kJaguarChannel = 2;
+  static constexpr uint32_t kCameraPanChannel = 8;
+  static constexpr uint32_t kCameraTiltChannel = 9;
 
   /* MXP digital channels */
-  static const uint32_t kTalonChannel = 10;
-  static const uint32_t kFakePressureSwitchChannel = 11;
-  static const uint32_t kFakeSolenoid1Channel = 12;
-  static const uint32_t kFakeSolenoid2Channel = 13;
-  static const uint32_t kFakeRelayForward = 18;
-  static const uint32_t kFakeRelayReverse = 19;
+  static constexpr uint32_t kTalonChannel = 10;
+  static constexpr uint32_t kFakePressureSwitchChannel = 11;
+  static constexpr uint32_t kFakeSolenoid1Channel = 12;
+  static constexpr uint32_t kFakeSolenoid2Channel = 13;
+  static constexpr uint32_t kFakeRelayForward = 18;
+  static constexpr uint32_t kFakeRelayReverse = 19;
+  static constexpr uint32_t kFakePwmOutput = 14;
 
   /* Relay channels */
-  static const uint32_t kRelayChannel = 0;
+  static constexpr uint32_t kRelayChannel = 0;
 
   /* PDP channels */
-  static const uint32_t kJaguarPDPChannel = 6;
-  static const uint32_t kVictorPDPChannel = 8;
-  static const uint32_t kTalonPDPChannel = 10;
+  static constexpr uint32_t kJaguarPDPChannel = 6;
+  static constexpr uint32_t kVictorPDPChannel = 8;
+  static constexpr uint32_t kTalonPDPChannel = 10;
 
   /* PCM channels */
-  static const int32_t kSolenoidChannel1 = 0;
-  static const int32_t kSolenoidChannel2 = 1;
+  static constexpr int32_t kSolenoidChannel1 = 0;
+  static constexpr int32_t kSolenoidChannel2 = 1;
 };
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/command/MockCommand.h b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/command/MockCommand.h
deleted file mode 100644
index bbcc419..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/command/MockCommand.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "frc/commands/Command.h"
-
-namespace frc {
-
-class MockCommand : public Command {
- public:
-  explicit MockCommand(Subsystem*);
-  MockCommand();
-  int32_t GetInitializeCount() { return m_initializeCount; }
-  bool HasInitialized();
-
-  int32_t GetExecuteCount() { return m_executeCount; }
-  int32_t GetIsFinishedCount() { return m_isFinishedCount; }
-  bool IsHasFinished() { return m_hasFinished; }
-  void SetHasFinished(bool hasFinished) { m_hasFinished = hasFinished; }
-  int32_t GetEndCount() { return m_endCount; }
-  bool HasEnd();
-
-  int32_t GetInterruptedCount() { return m_interruptedCount; }
-  bool HasInterrupted();
-  void ResetCounters();
-
- protected:
-  void Initialize() override;
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-  void Interrupted() override;
-
- private:
-  int32_t m_initializeCount;
-  int32_t m_executeCount;
-  int32_t m_isFinishedCount;
-  bool m_hasFinished;
-  int32_t m_endCount;
-  int32_t m_interruptedCount;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/command/MockConditionalCommand.h b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/command/MockConditionalCommand.h
deleted file mode 100644
index fc9d4ec..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/command/MockConditionalCommand.h
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include "command/MockCommand.h"
-#include "frc/commands/ConditionalCommand.h"
-
-namespace frc {
-
-class MockConditionalCommand : public ConditionalCommand {
- public:
-  MockConditionalCommand(MockCommand* onTrue, MockCommand* onFalse);
-  void SetCondition(bool condition);
-  int32_t GetInitializeCount() { return m_initializeCount; }
-  bool HasInitialized();
-
-  int32_t GetExecuteCount() { return m_executeCount; }
-  int32_t GetIsFinishedCount() { return m_isFinishedCount; }
-  int32_t GetEndCount() { return m_endCount; }
-  bool HasEnd();
-
-  int32_t GetInterruptedCount() { return m_interruptedCount; }
-  bool HasInterrupted();
-  void ResetCounters();
-
- protected:
-  bool Condition() override;
-  void Initialize() override;
-  void Execute() override;
-  bool IsFinished() override;
-  void End() override;
-  void Interrupted() override;
-
- private:
-  bool m_condition = false;
-  int32_t m_initializeCount;
-  int32_t m_executeCount;
-  int32_t m_isFinishedCount;
-  int32_t m_endCount;
-  int32_t m_interruptedCount;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/shuffleboard/MockActuatorSendable.h b/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/shuffleboard/MockActuatorSendable.h
deleted file mode 100644
index f56215c..0000000
--- a/third_party/allwpilib/wpilibcIntegrationTests/src/main/native/include/shuffleboard/MockActuatorSendable.h
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <wpi/StringRef.h>
-
-#include "frc/smartdashboard/SendableBase.h"
-#include "frc/smartdashboard/SendableBuilder.h"
-
-namespace frc {
-
-/**
- * A mock sendable that marks itself as an actuator.
- */
-class MockActuatorSendable : public SendableBase {
- public:
-  explicit MockActuatorSendable(wpi::StringRef name);
-
-  void InitSendable(SendableBuilder& builder) override;
-};
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpilibj/CMakeLists.txt b/third_party/allwpilib/wpilibj/CMakeLists.txt
index a07a563..cff7f22 100644
--- a/third_party/allwpilib/wpilibj/CMakeLists.txt
+++ b/third_party/allwpilib/wpilibj/CMakeLists.txt
@@ -1,12 +1,11 @@
 project (wpilibj)
 
-find_package( OpenCV REQUIRED )
-
 # Java bindings
 if (WITH_JAVA)
+    find_package( OpenCV REQUIRED )
     find_package(Java REQUIRED)
     include(UseJava)
-    set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+    set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
     set(OPENCV_JAVA_INSTALL_DIR ${OpenCV_INSTALL_PATH}/share/OpenCV/java/)
 
@@ -15,8 +14,8 @@
     configure_file(src/generate/WPILibVersion.java.in WPILibVersion.java)
 
     file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java)
-    file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
-    file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+    file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+    file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
 
     add_jar(wpilibj_jar ${JAVA_SOURCES} ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.java INCLUDE_JARS hal_jar ntcore_jar ${EJML_JARS} ${JACKSON_JARS} ${OPENCV_JAR_FILE} cscore_jar cameraserver_jar wpimath_jar wpiutil_jar OUTPUT_NAME wpilibj)
 
diff --git a/third_party/allwpilib/wpilibj/build.gradle b/third_party/allwpilib/wpilibj/build.gradle
index df16022..1004c20 100644
--- a/third_party/allwpilib/wpilibj/build.gradle
+++ b/third_party/allwpilib/wpilibj/build.gradle
@@ -56,7 +56,7 @@
 }
 
 repositories {
-    jcenter()
+    mavenCentral()
 }
 
 dependencies {
@@ -121,7 +121,7 @@
                 project(':hal').addHalDependency(it, 'shared')
                 project(':hal').addHalJniDependency(it)
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
-                    nativeUtils.useRequiredLibrary(it, 'netcomm_shared', 'chipobject_shared', 'visa_shared', 'ni_runtime_shared')
+                    nativeUtils.useRequiredLibrary(it, 'ni_link_libraries', 'ni_runtime_libraries')
                 }
             }
         }
diff --git a/third_party/allwpilib/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java b/third_party/allwpilib/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
index 2c08b17..e0db62e 100644
--- a/third_party/allwpilib/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
+++ b/third_party/allwpilib/wpilibj/src/dev/java/edu/wpi/first/wpilibj/DevMain.java
@@ -1,20 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.networktables.NetworkTablesJNI;
-import edu.wpi.first.wpiutil.RuntimeDetector;
+import edu.wpi.first.util.RuntimeDetector;
 
 public final class DevMain {
-  /**
-   * Main entry point.
-   */
+  /** Main entry point. */
   public static void main(String[] args) {
     System.out.println("Hello World!");
     System.out.println(RuntimeDetector.getPlatformPath());
@@ -22,6 +17,5 @@
     System.out.println(HALUtil.getHALRuntimeType());
   }
 
-  private DevMain() {
-  }
+  private DevMain() {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpilibj/src/dev/native/cpp/main.cpp
index e324b44..a3e363e 100644
--- a/third_party/allwpilib/wpilibj/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpilibj/src/dev/native/cpp/main.cpp
@@ -1,8 +1,5 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 int main() {}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
index c31782c..29d3930 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
@@ -1,31 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 
-/**
- * ADXL345 I2C Accelerometer.
- */
+/** ADXL345 I2C Accelerometer. */
 @SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
-public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
+public class ADXL345_I2C implements Accelerometer, NTSendable, AutoCloseable {
   private static final byte kAddress = 0x1D;
   private static final byte kPowerCtlRegister = 0x2D;
   private static final byte kDataFormatRegister = 0x31;
@@ -47,9 +42,7 @@
     kY((byte) 0x02),
     kZ((byte) 0x04);
 
-    /**
-     * The integer value representing this enumeration.
-     */
+    /** The integer value representing this enumeration. */
     @SuppressWarnings("MemberName")
     public final byte value;
 
@@ -76,7 +69,7 @@
   /**
    * Constructs the ADXL345 Accelerometer with I2C address 0x1D.
    *
-   * @param port  The I2C port the accelerometer is attached to
+   * @param port The I2C port the accelerometer is attached to
    * @param range The range (+ or -) that the accelerometer will measure.
    */
   public ADXL345_I2C(I2C.Port port, Range range) {
@@ -86,20 +79,26 @@
   /**
    * Constructs the ADXL345 Accelerometer over I2C.
    *
-   * @param port          The I2C port the accelerometer is attached to
-   * @param range         The range (+ or -) that the accelerometer will measure.
+   * @param port The I2C port the accelerometer is attached to
+   * @param range The range (+ or -) that the accelerometer will measure.
    * @param deviceAddress I2C address of the accelerometer (0x1D or 0x53)
    */
   public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
     m_i2c = new I2C(port, deviceAddress);
 
     // simulation
-    m_simDevice = SimDevice.create("ADXL345_I2C", port.value, deviceAddress);
+    m_simDevice = SimDevice.create("Accel:ADXL345_I2C", port.value, deviceAddress);
     if (m_simDevice != null) {
-      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
-      m_simX = m_simDevice.createDouble("X 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_simRange =
+          m_simDevice.createEnumDouble(
+              "range",
+              SimDevice.Direction.kOutput,
+              new String[] {"2G", "4G", "8G", "16G"},
+              new double[] {2.0, 4.0, 8.0, 16.0},
+              0);
+      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
+      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
+      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
     }
 
     // Turn on the measurements
@@ -111,6 +110,14 @@
     SendableRegistry.addLW(this, "ADXL345_I2C", port.value);
   }
 
+  public int getPort() {
+    return m_i2c.getPort();
+  }
+
+  public int getDeviceAddress() {
+    return m_i2c.getDeviceAddress();
+  }
+
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -217,16 +224,17 @@
   }
 
   @Override
-  public void initSendable(SendableBuilder builder) {
+  public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("3AxisAccelerometer");
     NetworkTableEntry entryX = builder.getEntry("X");
     NetworkTableEntry entryY = builder.getEntry("Y");
     NetworkTableEntry entryZ = builder.getEntry("Z");
-    builder.setUpdateTable(() -> {
-      AllAxes data = getAccelerations();
-      entryX.setDouble(data.XAxis);
-      entryY.setDouble(data.YAxis);
-      entryZ.setDouble(data.ZAxis);
-    });
+    builder.setUpdateTable(
+        () -> {
+          AllAxes data = getAccelerations();
+          entryX.setDouble(data.XAxis);
+          entryY.setDouble(data.YAxis);
+          entryZ.setDouble(data.ZAxis);
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
index 842ffd6..fa8b7d8 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
@@ -1,31 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 
-/**
- * ADXL345 SPI Accelerometer.
- */
+/** ADXL345 SPI Accelerometer. */
 @SuppressWarnings({"TypeName", "PMD.UnusedPrivateField"})
-public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
+public class ADXL345_SPI implements Accelerometer, NTSendable, AutoCloseable {
   private static final int kPowerCtlRegister = 0x2D;
   private static final int kDataFormatRegister = 0x31;
   private static final int kDataRegister = 0x32;
@@ -50,10 +45,7 @@
     kY((byte) 0x02),
     kZ((byte) 0x04);
 
-    /**
-     * The integer value representing this enumeration.
-     */
-    @SuppressWarnings("MemberName")
+    /** The integer value representing this enumeration. */
     public final byte value;
 
     Axes(byte value) {
@@ -79,23 +71,33 @@
   /**
    * Constructor.
    *
-   * @param port  The SPI port that the accelerometer is connected to
+   * @param port The SPI port that the accelerometer is connected to
    * @param range The range (+ or -) that the accelerometer will measure.
    */
   public ADXL345_SPI(SPI.Port port, Range range) {
     m_spi = new SPI(port);
     // simulation
-    m_simDevice = SimDevice.create("ADXL345_SPI", port.value);
+    m_simDevice = SimDevice.create("Accel:ADXL345_SPI", port.value);
     if (m_simDevice != null) {
-      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
-      m_simX = m_simDevice.createDouble("X 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_simRange =
+          m_simDevice.createEnumDouble(
+              "range",
+              SimDevice.Direction.kOutput,
+              new String[] {"2G", "4G", "8G", "16G"},
+              new double[] {2.0, 4.0, 8.0, 16.0},
+              0);
+      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
+      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
+      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
     }
     init(range);
     SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
   }
 
+  public int getPort() {
+    return m_spi.getPort();
+  }
+
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -154,7 +156,7 @@
     }
 
     // Specify the data format to read
-    byte[] commands = new byte[]{kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
+    byte[] commands = new byte[] {kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
     m_spi.write(commands, commands.length);
 
     if (m_simRange != null) {
@@ -194,8 +196,8 @@
       return m_simZ.get();
     }
     ByteBuffer transferBuffer = ByteBuffer.allocate(3);
-    transferBuffer.put(0,
-        (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
+    transferBuffer.put(
+        0, (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
     m_spi.transaction(transferBuffer, transferBuffer, 3);
     // Sensor is little endian
     transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
@@ -232,16 +234,17 @@
   }
 
   @Override
-  public void initSendable(SendableBuilder builder) {
+  public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("3AxisAccelerometer");
     NetworkTableEntry entryX = builder.getEntry("X");
     NetworkTableEntry entryY = builder.getEntry("Y");
     NetworkTableEntry entryZ = builder.getEntry("Z");
-    builder.setUpdateTable(() -> {
-      AllAxes data = getAccelerations();
-      entryX.setDouble(data.XAxis);
-      entryY.setDouble(data.YAxis);
-      entryZ.setDouble(data.ZAxis);
-    });
+    builder.setUpdateTable(
+        () -> {
+          AllAxes data = getAccelerations();
+          entryX.setDouble(data.XAxis);
+          entryY.setDouble(data.YAxis);
+          entryZ.setDouble(data.ZAxis);
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
index 340a776..2a9c2fe 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
@@ -1,32 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 
 /**
  * ADXL362 SPI Accelerometer.
  *
  * <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
  */
-@SuppressWarnings("PMD.UnusedPrivateField")
-public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
+public class ADXL362 implements Accelerometer, NTSendable, AutoCloseable {
   private static final byte kRegWrite = 0x0A;
   private static final byte kRegRead = 0x0B;
 
@@ -41,7 +37,10 @@
   private static final byte kFilterCtl_ODR_100Hz = 0x03;
 
   private static final byte kPowerCtl_UltraLowNoise = 0x20;
+
+  @SuppressWarnings("PMD.UnusedPrivateField")
   private static final byte kPowerCtl_AutoSleep = 0x04;
+
   private static final byte kPowerCtl_Measure = 0x02;
 
   public enum Axes {
@@ -49,7 +48,6 @@
     kY((byte) 0x02),
     kZ((byte) 0x04);
 
-    @SuppressWarnings("MemberName")
     public final byte value;
 
     Axes(byte value) {
@@ -75,7 +73,7 @@
   private double m_gsPerLSB;
 
   /**
-   * Constructor.  Uses the onboard CS1.
+   * Constructor. Uses the onboard CS1.
    *
    * @param range The range (+ or -) that the accelerometer will measure.
    */
@@ -86,19 +84,25 @@
   /**
    * Constructor.
    *
-   * @param port  The SPI port that the accelerometer is connected to
+   * @param port The SPI port that the accelerometer is connected to
    * @param range The range (+ or -) that the accelerometer will measure.
    */
   public ADXL362(SPI.Port port, Range range) {
     m_spi = new SPI(port);
 
     // simulation
-    m_simDevice = SimDevice.create("ADXL362", port.value);
+    m_simDevice = SimDevice.create("Accel:ADXL362", port.value);
     if (m_simDevice != null) {
-      m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
-      m_simX = m_simDevice.createDouble("X 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_simRange =
+          m_simDevice.createEnumDouble(
+              "range",
+              SimDevice.Direction.kOutput,
+              new String[] {"2G", "4G", "8G", "16G"},
+              new double[] {2.0, 4.0, 8.0, 16.0},
+              0);
+      m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
+      m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
+      m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
     }
 
     m_spi.setClockRate(3000000);
@@ -133,6 +137,10 @@
     SendableRegistry.addLW(this, "ADXL362", port.value);
   }
 
+  public int getPort() {
+    return m_spi.getPort();
+  }
+
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -163,18 +171,17 @@
         m_gsPerLSB = 0.002;
         break;
       case k8G:
-      case k16G:  // 16G not supported; treat as 8G
+      case k16G: // 16G not supported; treat as 8G
         value = kFilterCtl_Range8G;
         m_gsPerLSB = 0.004;
         break;
       default:
         throw new IllegalArgumentException(range + " unsupported");
-
     }
 
     // Specify the data format to read
-    byte[] commands = new byte[]{kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz
-        | value)};
+    byte[] commands =
+        new byte[] {kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz | value)};
     m_spi.write(commands, commands.length);
 
     if (m_simRange != null) {
@@ -182,7 +189,6 @@
     }
   }
 
-
   @Override
   public double getX() {
     return getAcceleration(Axes.kX);
@@ -257,16 +263,17 @@
   }
 
   @Override
-  public void initSendable(SendableBuilder builder) {
+  public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("3AxisAccelerometer");
     NetworkTableEntry entryX = builder.getEntry("X");
     NetworkTableEntry entryY = builder.getEntry("Y");
     NetworkTableEntry entryZ = builder.getEntry("Z");
-    builder.setUpdateTable(() -> {
-      AllAxes data = getAccelerations();
-      entryX.setDouble(data.XAxis);
-      entryY.setDouble(data.YAxis);
-      entryZ.setDouble(data.ZAxis);
-    });
+    builder.setUpdateTable(
+        () -> {
+          AllAxes data = getAccelerations();
+          entryX.setDouble(data.XAxis);
+          entryY.setDouble(data.YAxis);
+          entryZ.setDouble(data.ZAxis);
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
index 78ab8c4..5a826de 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
@@ -1,22 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
@@ -29,7 +27,7 @@
  * an ADXRS Gyro is supported.
  */
 @SuppressWarnings({"TypeName", "AbbreviationAsWordInName", "PMD.UnusedPrivateField"})
-public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable, AutoCloseable {
+public class ADXRS450_Gyro implements Gyro, Sendable {
   private static final double kSamplePeriod = 0.0005;
   private static final double kCalibrationSampleTime = 5.0;
   private static final double kDegreePerSecondPerLSB = 0.0125;
@@ -45,16 +43,13 @@
   private static final int kSNLowRegister = 0x10;
 
   private SPI m_spi;
-  private SPI.Port m_port;
 
   private SimDevice m_simDevice;
   private SimBoolean m_simConnected;
   private SimDouble m_simAngle;
   private SimDouble m_simRate;
 
-  /**
-   * Constructor.  Uses the onboard CS0.
-   */
+  /** Constructor. Uses the onboard CS0. */
   public ADXRS450_Gyro() {
     this(SPI.Port.kOnboardCS0);
   }
@@ -66,14 +61,13 @@
    */
   public ADXRS450_Gyro(SPI.Port port) {
     m_spi = new SPI(port);
-    m_port = port;
 
     // simulation
-    m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
+    m_simDevice = SimDevice.create("Gyro:ADXRS450", port.value);
     if (m_simDevice != null) {
-      m_simConnected = m_simDevice.createBoolean("Connected", false, true);
-      m_simAngle = m_simDevice.createDouble("Angle", false, 0.0);
-      m_simRate = m_simDevice.createDouble("Rate", false, 0.0);
+      m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
+      m_simAngle = m_simDevice.createDouble("angle_x", SimDevice.Direction.kInput, 0.0);
+      m_simRate = m_simDevice.createDouble("rate_x", SimDevice.Direction.kInput, 0.0);
     }
 
     m_spi.setClockRate(3000000);
@@ -87,13 +81,12 @@
       if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
         m_spi.close();
         m_spi = null;
-        DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
-            false);
+        DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value, false);
         return;
       }
 
-      m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
-          true, true);
+      m_spi.initAccumulator(
+          kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, true, true);
 
       calibrate();
     }
@@ -137,7 +130,7 @@
    * @return The SPI port number.
    */
   public int getPort() {
-    return m_port.value;
+    return m_spi.getPort();
   }
 
   private boolean calcParity(int value) {
@@ -164,7 +157,7 @@
     m_spi.read(false, buf, 4);
 
     if ((buf.get(0) & 0xe0) == 0) {
-      return 0;  // error, return 0
+      return 0; // error, return 0
     }
     return (buf.getInt(0) >> 5) & 0xffff;
   }
@@ -172,19 +165,14 @@
   @Override
   public void reset() {
     if (m_simAngle != null) {
-      m_simAngle.set(0.0);
-    }
-    if (m_simRate != null) {
-      m_simRate.set(0.0);
+      m_simAngle.reset();
     }
     if (m_spi != null) {
       m_spi.resetAccumulator();
     }
   }
 
-  /**
-   * Delete (free) the spi port used for the gyro and stop accumulating.
-   */
+  /** Delete (free) the spi port used for the gyro and stop accumulating. */
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -219,4 +207,10 @@
     }
     return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
   }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Gyro");
+    builder.addDoubleProperty("Value", this::getAngle, null);
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
index 5a333db..f4b367a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLED.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -58,8 +55,8 @@
   /**
    * Sets the led output data.
    *
-   * <p>If the output is enabled, this will start writing the next data cycle.
-   * It is safe to call, even while output is enabled.
+   * <p>If the output is enabled, this will start writing the next data cycle. It is safe to call,
+   * even while output is enabled.
    *
    * @param buffer the buffer to write
    */
@@ -77,10 +74,16 @@
    * @param lowTime1NanoSeconds low time for 1 bit
    * @param highTime1NanoSeconds high time for 1 bit
    */
-  public void setBitTiming(int lowTime0NanoSeconds, int highTime0NanoSeconds,
-      int lowTime1NanoSeconds, int highTime1NanoSeconds) {
-    AddressableLEDJNI.setBitTiming(m_handle, lowTime0NanoSeconds,
-        highTime0NanoSeconds, lowTime1NanoSeconds,
+  public void setBitTiming(
+      int lowTime0NanoSeconds,
+      int highTime0NanoSeconds,
+      int lowTime1NanoSeconds,
+      int highTime1NanoSeconds) {
+    AddressableLEDJNI.setBitTiming(
+        m_handle,
+        lowTime0NanoSeconds,
+        highTime0NanoSeconds,
+        lowTime1NanoSeconds,
         highTime1NanoSeconds);
   }
 
@@ -104,9 +107,7 @@
     AddressableLEDJNI.start(m_handle);
   }
 
-  /**
-   * Stops the output.
-   */
+  /** Stops the output. */
   public void stop() {
     AddressableLEDJNI.stop(m_handle);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
index 8b76429..c9659a1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AddressableLEDBuffer.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj.util.Color8Bit;
 
-/**
- * Buffer storage for Addressable LEDs.
- */
+/** Buffer storage for Addressable LEDs. */
 public class AddressableLEDBuffer {
   byte[] m_buffer;
 
@@ -29,9 +24,9 @@
    * Sets a specific led in the buffer.
    *
    * @param index the index to write
-   * @param r     the r value [0-255]
-   * @param g     the g value [0-255]
-   * @param b     the b value [0-255]
+   * @param r the r value [0-255]
+   * @param g the g value [0-255]
+   * @param b the b value [0-255]
    */
   @SuppressWarnings("ParameterName")
   public void setRGB(int index, int r, int g, int b) {
@@ -45,9 +40,9 @@
    * Sets a specific led in the buffer.
    *
    * @param index the index to write
-   * @param h     the h value [0-180]
-   * @param s     the s value [0-255]
-   * @param v     the v value [0-255]
+   * @param h the h value [0-180]
+   * @param s the s value [0-255]
+   * @param v the v value [0-255]
    */
   @SuppressWarnings("ParameterName")
   public void setHSV(final int index, final int h, final int s, final int v) {
@@ -92,10 +87,7 @@
    * @param color The color of the LED
    */
   public void setLED(int index, Color color) {
-    setRGB(index,
-        (int) (color.red * 255),
-        (int) (color.green * 255),
-        (int) (color.blue * 255));
+    setRGB(index, (int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255));
   }
 
   /**
@@ -124,8 +116,8 @@
    * @return the LED color at the specified index
    */
   public Color8Bit getLED8Bit(int index) {
-    return new Color8Bit(m_buffer[index * 4 + 2] & 0xFF, m_buffer[index * 4 + 1] & 0xFF,
-                         m_buffer[index * 4] & 0xFF);
+    return new Color8Bit(
+        m_buffer[index * 4 + 2] & 0xFF, m_buffer[index * 4 + 1] & 0xFF, m_buffer[index * 4] & 0xFF);
   }
 
   /**
@@ -135,9 +127,9 @@
    * @return the LED color at the specified index
    */
   public Color getLED(int index) {
-    return new Color((m_buffer[index * 4 + 2] & 0xFF) / 255.0,
-                     (m_buffer[index * 4 + 1] & 0xFF) / 255.0,
-                     (m_buffer[index * 4] & 0xFF) / 255.0);
+    return new Color(
+        (m_buffer[index * 4 + 2] & 0xFF) / 255.0,
+        (m_buffer[index * 4 + 1] & 0xFF) / 255.0,
+        (m_buffer[index * 4] & 0xFF) / 255.0);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
index 41c5b38..a2405b8 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogAccelerometer.java
@@ -1,37 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Handle operation of an analog accelerometer. The accelerometer reads acceleration directly
  * through the sensor. Many sensors have multiple axis and can be treated as multiple devices. Each
  * is calibrated by finding the center value over a period of time.
  */
-public class AnalogAccelerometer implements PIDSource, Sendable, AutoCloseable {
+public class AnalogAccelerometer implements Sendable, AutoCloseable {
   private AnalogInput m_analogChannel;
   private double m_voltsPerG = 1.0;
   private double m_zeroGVoltage = 2.5;
   private final boolean m_allocatedChannel;
-  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
-  /**
-   * Common initialization.
-   */
+  /** Common initialization. */
   private void initAccelerometer() {
-    HAL.report(tResourceType.kResourceType_Accelerometer,
-                                   m_analogChannel.getChannel() + 1);
+    HAL.report(tResourceType.kResourceType_Accelerometer, m_analogChannel.getChannel() + 1);
     SendableRegistry.addLW(this, "Accelerometer", m_analogChannel.getChannel());
   }
 
@@ -53,7 +47,7 @@
    * read as an analog channel as well as through the Accelerometer class.
    *
    * @param channel The existing AnalogInput object for the analog input the accelerometer is
-   *                connected to
+   *     connected to
    */
   public AnalogAccelerometer(final AnalogInput channel) {
     this(channel, false);
@@ -66,9 +60,7 @@
     initAccelerometer();
   }
 
-  /**
-   * Delete the analog components used for the accelerometer.
-   */
+  /** Delete the analog components used for the accelerometer. */
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -117,26 +109,6 @@
   }
 
   @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Get the Acceleration for the PID Source parent.
-   *
-   * @return The current acceleration in Gs.
-   */
-  @Override
-  public double pidGet() {
-    return getAcceleration();
-  }
-
-  @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Accelerometer");
     builder.addDoubleProperty("Value", this::getAcceleration, null);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
index bea4e6a..24989dd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogEncoder.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
-/**
- * Class for supporting continuous analog encoders, such as the US Digital MA3.
- */
+/** Class for supporting continuous analog encoders, such as the US Digital MA3. */
 public class AnalogEncoder implements Sendable, AutoCloseable {
   private final AnalogInput m_analogInput;
   private AnalogTrigger m_analogTrigger;
@@ -28,6 +25,15 @@
   protected SimDouble m_simPosition;
 
   /**
+   * Construct a new AnalogEncoder attached to a specific AnalogIn channel.
+   *
+   * @param channel the analog input channel to attach to
+   */
+  public AnalogEncoder(int channel) {
+    this(new AnalogInput(channel));
+  }
+
+  /**
    * Construct a new AnalogEncoder attached to a specific AnalogInput.
    *
    * @param analogInput the analog input to attach to
@@ -44,7 +50,7 @@
     m_simDevice = SimDevice.create("AnalogEncoder", m_analogInput.getChannel());
 
     if (m_simDevice != null) {
-      m_simPosition = m_simDevice.createDouble("Position", false, 0.0);
+      m_simPosition = m_simDevice.createDouble("Position", Direction.kInput, 0.0);
     }
 
     // Limits need to be 25% from each end
@@ -89,9 +95,9 @@
   /**
    * Get the offset of position relative to the last reset.
    *
-   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute
-   * position relative to the last reset. This could potentially be negative,
-   * which needs to be accounted for.
+   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute position
+   * relative to the last reset. This could potentially be negative, which needs to be accounted
+   * for.
    *
    * @return the position offset
    */
@@ -100,11 +106,10 @@
   }
 
   /**
-   * Set the distance per rotation of the encoder. This sets the multiplier used
-   * to determine the distance driven based on the rotation value from the
-   * encoder. Set this value based on the how far the mechanism travels in 1
-   * rotation of the encoder, and factor in gearing reductions following the
-   * encoder shaft. This distance can be in any units you like, linear or angular.
+   * Set the distance per rotation of the encoder. This sets the multiplier used to determine the
+   * distance driven based on the rotation value from the encoder. Set this value based on the how
+   * far the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions
+   * following the encoder shaft. This distance can be in any units you like, linear or angular.
    *
    * @param distancePerRotation the distance per rotation of the encoder
    */
@@ -115,16 +120,15 @@
   /**
    * Get the distance per rotation for this encoder.
    *
-   * @return The scale factor that will be used to convert rotation to useful
-   *         units.
+   * @return The scale factor that will be used to convert rotation to useful units.
    */
   public double getDistancePerRotation() {
     return m_distancePerRotation;
   }
 
   /**
-   * Get the distance the sensor has driven since the last reset as scaled by the
-   * value from {@link #setDistancePerRotation(double)}.
+   * Get the distance the sensor has driven since the last reset as scaled by the value from {@link
+   * #setDistancePerRotation(double)}.
    *
    * @return The distance driven since the last reset
    */
@@ -141,9 +145,7 @@
     return m_analogInput.getChannel();
   }
 
-  /**
-   * Reset the Encoder distance to zero.
-   */
+  /** Reset the Encoder distance to zero. */
   public void reset() {
     m_counter.reset();
     m_positionOffset = m_analogInput.getVoltage();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
index 0ed22d9..c9b9126 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogGyro.java
@@ -1,19 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 import edu.wpi.first.hal.AnalogGyroJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 /**
  * Use a rate gyro to return the robots heading relative to a starting position. The Gyro class
@@ -24,16 +23,14 @@
  *
  * <p>This class is for gyro sensors that connect to an analog input.
  */
-public class AnalogGyro extends GyroBase implements Gyro, PIDSource, Sendable, AutoCloseable {
+public class AnalogGyro implements Gyro, Sendable {
   private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
   protected AnalogInput m_analog;
   private boolean m_channelAllocated;
 
   private int m_gyroHandle;
 
-  /**
-   * Initialize the gyro. Calibration is handled by calibrate().
-   */
+  /** Initialize the gyro. Calibration is handled by calibrate(). */
   public void initGyro() {
     if (m_gyroHandle == 0) {
       m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
@@ -54,7 +51,7 @@
    * Gyro constructor using the channel number.
    *
    * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
-   *                channels 0-1.
+   *     channels 0-1.
    */
   public AnalogGyro(int channel) {
     this(new AnalogInput(channel));
@@ -67,7 +64,7 @@
    * channel needs to be shared.
    *
    * @param channel The AnalogInput object that the gyro is connected to. Gyros can only be used on
-   *                on-board channels 0-1.
+   *     on-board channels 0-1.
    */
   public AnalogGyro(AnalogInput channel) {
     requireNonNullParam(channel, "channel", "AnalogGyro");
@@ -82,9 +79,9 @@
    * offset values. Bypasses calibration.
    *
    * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
-   *                channels 0-1.
-   * @param center  Preset uncalibrated value to use as the accumulator center value.
-   * @param offset  Preset uncalibrated value to use as the gyro offset.
+   *     channels 0-1.
+   * @param center Preset uncalibrated value to use as the accumulator center value.
+   * @param offset Preset uncalibrated value to use as the gyro offset.
    */
   public AnalogGyro(int channel, int center, double offset) {
     this(new AnalogInput(channel), center, offset);
@@ -97,17 +94,18 @@
    * the center and offset values. Bypasses calibration.
    *
    * @param channel The analog channel the gyro is connected to. Gyros can only be used on on-board
-   *                channels 0-1.
-   * @param center  Preset uncalibrated value to use as the accumulator center value.
-   * @param offset  Preset uncalibrated value to use as the gyro offset.
+   *     channels 0-1.
+   * @param center Preset uncalibrated value to use as the accumulator center value.
+   * @param offset Preset uncalibrated value to use as the gyro offset.
    */
   public AnalogGyro(AnalogInput channel, int center, double offset) {
     requireNonNullParam(channel, "channel", "AnalogGyro");
 
     m_analog = channel;
     initGyro();
-    AnalogGyroJNI.setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
-                                          offset, center);
+    AnalogGyroJNI.setAnalogGyroParameters(
+        m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
+        offset, center);
     reset();
   }
 
@@ -116,9 +114,7 @@
     AnalogGyroJNI.resetAnalogGyro(m_gyroHandle);
   }
 
-  /**
-   * Delete (free) the accumulator and the analog components used for the gyro.
-   */
+  /** Delete (free) the accumulator and the analog components used for the gyro. */
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -173,8 +169,7 @@
    * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
    */
   public void setSensitivity(double voltsPerDegreePerSecond) {
-    AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle,
-                                                       voltsPerDegreePerSecond);
+    AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, voltsPerDegreePerSecond);
   }
 
   /**
@@ -196,4 +191,10 @@
   public AnalogInput getAnalogInput() {
     return m_analog;
   }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Gyro");
+    builder.addDoubleProperty("Value", this::getAngle, null);
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
index 3298058..e3bedcb 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogInput.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -13,8 +10,9 @@
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.util.AllocationException;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Analog channel class.
@@ -28,13 +26,12 @@
  * accumulated effectively increasing the resolution, while the averaged samples are divided by the
  * number of samples to retain the resolution, but get more stable values.
  */
-public class AnalogInput implements PIDSource, Sendable, AutoCloseable {
+public class AnalogInput implements Sendable, AutoCloseable {
   private static final int kAccumulatorSlot = 1;
   int m_port; // explicit no modifier, private and package accessible.
   private int m_channel;
   private static final int[] kAccumulatorChannels = {0, 1};
   private long m_accumulatorOffset;
-  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   /**
    * Construct an analog channel.
@@ -183,13 +180,16 @@
     return AnalogJNI.getAnalogOversampleBits(m_port);
   }
 
-  /**
-   * Initialize the accumulator.
-   */
+  /** Initialize the accumulator. */
   public void initAccumulator() {
     if (!isAccumulatorChannel()) {
-      throw new AllocationException("Accumulators are only available on slot " + kAccumulatorSlot
-          + " on channels " + kAccumulatorChannels[0] + ", " + kAccumulatorChannels[1]);
+      throw new AllocationException(
+          "Accumulators are only available on slot "
+              + kAccumulatorSlot
+              + " on channels "
+              + kAccumulatorChannels[0]
+              + ", "
+              + kAccumulatorChannels[1]);
     }
     m_accumulatorOffset = 0;
     AnalogJNI.initAccumulator(m_port);
@@ -206,9 +206,7 @@
     m_accumulatorOffset = initialValue;
   }
 
-  /**
-   * Resets the accumulator to the initial value.
-   */
+  /** Resets the accumulator to the initial value. */
   public void resetAccumulator() {
     AnalogJNI.resetAccumulator(m_port);
 
@@ -218,7 +216,6 @@
     final double overSamples = 1 << getOversampleBits();
     final double averageSamples = 1 << getAverageBits();
     Timer.delay(sampleTime * overSamples * averageSamples);
-
   }
 
   /**
@@ -231,6 +228,8 @@
    * <p>This center value is based on the output of the oversampled and averaged source the
    * accumulator channel. Because of this, any non-zero oversample bits will affect the size of the
    * value for this field.
+   *
+   * @param center The accumulator's center value.
    */
   public void setAccumulatorCenter(int center) {
     AnalogJNI.setAccumulatorCenter(m_port, center);
@@ -325,26 +324,6 @@
     return AnalogJNI.getAnalogSampleRate();
   }
 
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Get the average voltage for use with PIDController.
-   *
-   * @return the average voltage
-   */
-  @Override
-  public double pidGet() {
-    return getAverageVoltage();
-  }
-
   /**
    * Indicates this input is used by a simulated device.
    *
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
index feb6e1e..299d89c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogOutput.java
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.AnalogJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
-/**
- * Analog output class.
- */
+/** Analog output class. */
 public class AnalogOutput implements Sendable, AutoCloseable {
   private int m_port;
   private int m_channel;
@@ -46,6 +42,8 @@
 
   /**
    * Get the channel of this AnalogOutput.
+   *
+   * @return The channel of this AnalogOutput.
    */
   public int getChannel() {
     return m_channel;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
index 9d5d49d..4a5bd75 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometer.java
@@ -1,27 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import edu.wpi.first.wpilibj.interfaces.Potentiometer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Class for reading analog potentiometers. Analog potentiometers read in an analog voltage that
  * corresponds to a position. The position is in whichever units you choose, by way of the scaling
  * and offset constants passed to the constructor.
  */
-public class AnalogPotentiometer implements Potentiometer, Sendable, AutoCloseable {
+public class AnalogPotentiometer implements Sendable, AutoCloseable {
   private AnalogInput m_analogInput;
   private boolean m_initAnalogInput;
   private double m_fullRange;
   private double m_offset;
-  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   /**
    * AnalogPotentiometer constructor.
@@ -29,13 +25,13 @@
    * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
    * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
    * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
-   * point after scaling is 135 degrees. This will calculate the result from the fullRange times
-   * the fraction of the supply voltage, plus the offset.
+   * point after scaling is 135 degrees. This will calculate the result from the fullRange times the
+   * fraction of the supply voltage, plus the offset.
    *
-   * @param channel   The analog input channel this potentiometer is plugged into. 0-3 are
-   *                  on-board and 4-7 are on the MXP port.
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board
+   *     and 4-7 are on the MXP port.
    * @param fullRange The scaling to multiply the fraction by to get a meaningful unit.
-   * @param offset    The offset to add to the scaled value for controlling the zero value
+   * @param offset The offset to add to the scaled value for controlling the zero value
    */
   public AnalogPotentiometer(final int channel, double fullRange, double offset) {
     this(new AnalogInput(channel), fullRange, offset);
@@ -47,16 +43,15 @@
    * AnalogPotentiometer constructor.
    *
    * <p>Use the fullRange and offset values so that the output produces meaningful values. I.E: you
-   * have a 270 degree potentiometer and you want the output to be degrees with the halfway point
-   * as 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
-   * point after scaling is 135 degrees. This will calculate the result from the fullRange times
-   * the fraction of the supply voltage, plus the offset.
+   * have a 270 degree potentiometer and you want the output to be degrees with the halfway point as
+   * 0 degrees. The fullRange value is 270.0(degrees) and the offset is -135.0 since the halfway
+   * point after scaling is 135 degrees. This will calculate the result from the fullRange times the
+   * fraction of the supply voltage, plus the offset.
    *
-   * @param input     The {@link AnalogInput} this potentiometer is plugged into.
-   * @param fullRange The angular value (in desired units) representing the full
-   *                  0-5V range of the input.
-   * @param offset    The angular value (in desired units) representing the
-   *                  angular output at 0V.
+   * @param input The {@link AnalogInput} this potentiometer is plugged into.
+   * @param fullRange The angular value (in desired units) representing the full 0-5V range of the
+   *     input.
+   * @param offset The angular value (in desired units) representing the angular output at 0V.
    */
   public AnalogPotentiometer(final AnalogInput input, double fullRange, double offset) {
     SendableRegistry.addLW(this, "AnalogPotentiometer", input.getChannel());
@@ -70,13 +65,13 @@
   /**
    * AnalogPotentiometer constructor.
    *
-   * <p>Use the scale value so that the output produces meaningful values. I.E: you
-   * have a 270 degree potentiometer and you want the output to be degrees with the starting point
-   * as 0 degrees. The scale value is 270.0(degrees).
+   * <p>Use the scale value so that the output produces meaningful values. I.E: you have a 270
+   * degree potentiometer and you want the output to be degrees with the starting point as 0
+   * degrees. The scale value is 270.0(degrees).
    *
-   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are
-   *                on-board and 4-7 are on the MXP port.
-   * @param scale   The scaling to multiply the voltage by to get a meaningful unit.
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board
+   *     and 4-7 are on the MXP port.
+   * @param scale The scaling to multiply the voltage by to get a meaningful unit.
    */
   public AnalogPotentiometer(final int channel, double scale) {
     this(channel, scale, 0);
@@ -101,8 +96,8 @@
    *
    * <p>The potentiometer will return a value between 0 and 1.0.
    *
-   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are
-   *                  on-board and 4-7 are on the MXP port.
+   * @param channel The analog input channel this potentiometer is plugged into. 0-3 are on-board
+   *     and 4-7 are on the MXP port.
    */
   public AnalogPotentiometer(final int channel) {
     this(channel, 1, 0);
@@ -124,36 +119,12 @@
    *
    * @return The current position of the potentiometer.
    */
-  @Override
   public double get() {
     if (m_analogInput == null) {
       return m_offset;
     }
-    return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V())
-        * m_fullRange + m_offset;
-  }
-
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    if (!pidSource.equals(PIDSourceType.kDisplacement)) {
-      throw new IllegalArgumentException("Only displacement PID is allowed for potentiometers.");
-    }
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Implement the PIDSource interface.
-   *
-   * @return The current reading.
-   */
-  @Override
-  public double pidGet() {
-    return get();
+    return (m_analogInput.getAverageVoltage() / RobotController.getVoltage5V()) * m_fullRange
+        + m_offset;
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
index 282a346..d5825c1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTrigger.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,18 +8,14 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.util.BoundaryException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
-
-/**
- * Class for creating and configuring Analog Triggers.
- */
+/** Class for creating and configuring Analog Triggers. */
 public class AnalogTrigger implements Sendable, AutoCloseable {
-  /**
-   * Exceptions dealing with improper operation of the Analog trigger.
-   */
+  /** Exceptions dealing with improper operation of the Analog trigger. */
   @SuppressWarnings("serial")
   public static class AnalogTriggerException extends RuntimeException {
     /**
@@ -33,13 +26,11 @@
     public AnalogTriggerException(String message) {
       super(message);
     }
-
   }
 
-  /**
-   * Where the analog trigger is attached.
-   */
+  /** Where the analog trigger is attached. */
   protected int m_port;
+
   protected AnalogInput m_analogInput;
   protected DutyCycle m_dutyCycle;
   protected boolean m_ownsAnalog;
@@ -162,8 +153,7 @@
   }
 
   /**
-   * Return the index of the analog trigger. This is the FPGA index of this analog trigger
-   * instance.
+   * Return the index of the analog trigger. This is the FPGA index of this analog trigger instance.
    *
    * @return The index of the analog trigger.
    */
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
index f9fe950..0f513b4 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AnalogTriggerOutput.java
@@ -1,18 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 import edu.wpi.first.hal.AnalogJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
 
 /**
  * Class to represent a specific output from an analog trigger. This class is used to get the
@@ -41,9 +39,7 @@
  * limited.
  */
 public class AnalogTriggerOutput extends DigitalSource implements Sendable {
-  /**
-   * Exceptions dealing with improper operation of the Analog trigger output.
-   */
+  /** Exceptions dealing with improper operation of the Analog trigger output. */
   @SuppressWarnings("serial")
   public static class AnalogTriggerOutputException extends RuntimeException {
     /**
@@ -65,7 +61,7 @@
    * <p>Because this class derives from DigitalSource, it can be passed into routing functions for
    * Counter, Encoder, etc.
    *
-   * @param trigger    The trigger for which this is an output.
+   * @param trigger The trigger for which this is an output.
    * @param outputType An enum that specifies the output on the trigger to represent.
    */
   public AnalogTriggerOutput(AnalogTrigger trigger, final AnalogTriggerType outputType) {
@@ -74,8 +70,10 @@
 
     m_trigger = trigger;
     m_outputType = outputType;
-    HAL.report(tResourceType.kResourceType_AnalogTriggerOutput,
-        trigger.getIndex() + 1, outputType.value + 1);
+    HAL.report(
+        tResourceType.kResourceType_AnalogTriggerOutput,
+        trigger.getIndex() + 1,
+        outputType.value + 1);
   }
 
   /**
@@ -107,15 +105,13 @@
     return true;
   }
 
-  /**
-   * Defines the state in which the AnalogTrigger triggers.
-   */
+  /** Defines the state in which the AnalogTrigger triggers. */
   public enum AnalogTriggerType {
-    kInWindow(AnalogJNI.AnalogTriggerType.kInWindow), kState(AnalogJNI.AnalogTriggerType.kState),
+    kInWindow(AnalogJNI.AnalogTriggerType.kInWindow),
+    kState(AnalogJNI.AnalogTriggerType.kState),
     kRisingPulse(AnalogJNI.AnalogTriggerType.kRisingPulse),
     kFallingPulse(AnalogJNI.AnalogTriggerType.kFallingPulse);
 
-    @SuppressWarnings("MemberName")
     private final int value;
 
     AnalogTriggerType(int value) {
@@ -124,6 +120,5 @@
   }
 
   @Override
-  public void initSendable(SendableBuilder builder) {
-  }
+  public void initSendable(SendableBuilder builder) {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AsynchronousInterrupt.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AsynchronousInterrupt.java
new file mode 100644
index 0000000..1981671
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/AsynchronousInterrupt.java
@@ -0,0 +1,140 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.function.BiConsumer;
+
+/**
+ * Class for handling asynchronous interrupts using a callback thread.
+ *
+ * <p>By default, interrupts will occur on rising edge. Callbacks are disabled by default, and
+ * enable() must be called before they will occur.
+ *
+ * <p>Both rising and falling edges can be indicated in one callback if both a rising and falling
+ * edge occurred since the previous callback.
+ *
+ * <p>Synchronous (blocking) interrupts are handled by the SynchronousInterrupt class.
+ */
+public class AsynchronousInterrupt implements AutoCloseable {
+  private final BiConsumer<Boolean, Boolean> m_callback;
+  private final SynchronousInterrupt m_interrupt;
+
+  private final AtomicBoolean m_keepRunning = new AtomicBoolean(false);
+  private Thread m_thread;
+
+  /**
+   * Construct a new asynchronous interrupt using a Digital Source.
+   *
+   * <p>At construction, the interrupt will trigger on the rising edge.
+   *
+   * <p>Callbacks will not be triggered until enable() is called.
+   *
+   * <p>The first bool in the callback indicates the rising edge triggered the interrupt, the second
+   * bool is falling edge.
+   *
+   * @param source The digital source to use.
+   * @param callback The callback to call on an interrupt
+   */
+  public AsynchronousInterrupt(DigitalSource source, BiConsumer<Boolean, Boolean> callback) {
+    m_callback = requireNonNullParam(callback, "callback", "AsynchronousInterrupt");
+    m_interrupt = new SynchronousInterrupt(source);
+  }
+
+  /**
+   * Closes the interrupt.
+   *
+   * <p>This does not close the associated digital source.
+   *
+   * <p>This will disable the interrupt if it is enabled.
+   */
+  @Override
+  public void close() {
+    disable();
+    m_interrupt.close();
+  }
+
+  /**
+   * Enables interrupt callbacks. Before this, callbacks will not occur. Does nothing if already
+   * enabled.
+   */
+  public void enable() {
+    if (m_keepRunning.get()) {
+      return;
+    }
+
+    m_keepRunning.set(true);
+    m_thread = new Thread(this::threadMain);
+    m_thread.start();
+  }
+
+  /** Disables interrupt callbacks. Does nothing if already disabled. */
+  public void disable() {
+    m_keepRunning.set(false);
+    m_interrupt.wakeupWaitingInterrupt();
+    if (m_thread != null) {
+      if (m_thread.isAlive()) {
+        try {
+          m_thread.interrupt();
+          m_thread.join();
+        } catch (InterruptedException ex) {
+          Thread.currentThread().interrupt();
+        }
+      }
+      m_thread = null;
+    }
+  }
+
+  /**
+   * Set which edges to trigger the interrupt on.
+   *
+   * @param risingEdge Trigger on rising edge
+   * @param fallingEdge Trigger on falling edge
+   */
+  public void setInterruptEdges(boolean risingEdge, boolean fallingEdge) {
+    m_interrupt.setInterruptEdges(risingEdge, fallingEdge);
+  }
+
+  /**
+   * Get the timestamp of the last rising edge.
+   *
+   * <p>This function does not require the interrupt to be enabled to work.
+   *
+   * <p>This only works if rising edge was configured using setInterruptEdges.
+   *
+   * @return the timestamp in seconds relative to getFPGATime
+   */
+  public double getRisingTimestamp() {
+    return m_interrupt.getRisingTimestamp();
+  }
+
+  /**
+   * Get the timestamp of the last falling edge.
+   *
+   * <p>This function does not require the interrupt to be enabled to work.
+   *
+   * <p>This only works if falling edge was configured using setInterruptEdges.
+   *
+   * @return the timestamp in seconds relative to getFPGATime
+   */
+  public double getFallingTimestamp() {
+    return m_interrupt.getFallingTimestamp();
+  }
+
+  private void threadMain() {
+    while (m_keepRunning.get()) {
+      var result = m_interrupt.waitForInterruptRaw(10, false);
+      if (!m_keepRunning.get()) {
+        break;
+      }
+      if (result == 0) {
+        continue;
+      }
+      m_callback.accept((result & 0x1) != 0, (result & 0x100) != 0);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
index e367756..76c282a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometer.java
@@ -1,18 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.AccelerometerJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.interfaces.Accelerometer;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
 /**
  * Built-in accelerometer.
@@ -31,9 +29,7 @@
     SendableRegistry.addLW(this, "BuiltInAccel", 0);
   }
 
-  /**
-   * Constructor. The accelerometer will measure +/-8 g-forces
-   */
+  /** Constructor. The accelerometer will measure +/-8 g-forces */
   public BuiltInAccelerometer() {
     this(Range.k8G);
   }
@@ -60,7 +56,6 @@
       case k16G:
       default:
         throw new IllegalArgumentException(range + " range not supported (use k2G, k4G, or k8G)");
-
     }
 
     AccelerometerJNI.setAccelerometerActive(true);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
index 2dde3ab..3dc6c1d 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CAN.java
@@ -1,29 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.io.Closeable;
-
 import edu.wpi.first.hal.CANAPIJNI;
 import edu.wpi.first.hal.CANData;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import java.io.Closeable;
 
 /**
- * High level class for interfacing with CAN devices conforming to
- * the standard CAN spec.
+ * High level class for interfacing with CAN devices conforming to the standard CAN spec.
  *
- * <p>No packets that can be sent gets blocked by the RoboRIO, so all methods
- * work identically in all robot modes.
+ * <p>No packets that can be sent gets blocked by the RoboRIO, so all methods work identically in
+ * all robot modes.
  *
- * <p>All methods are thread safe, however the CANData object passed into the
- * read methods and the byte[] passed into the write methods need to not
- * be modified for the duration of their respective calls.
+ * <p>All methods are thread safe, however the CANData object passed into the read methods and the
+ * byte[] passed into the write methods need to not be modified for the duration of their respective
+ * calls.
  */
 public class CAN implements Closeable {
   public static final int kTeamManufacturer = 8;
@@ -32,9 +27,8 @@
   private final int m_handle;
 
   /**
-   * Create a new CAN communication interface with the specific device ID.
-   * This uses the team manufacturer and device types.
-   * The device ID is 6 bits (0-63).
+   * Create a new CAN communication interface with the specific device ID. This uses the team
+   * manufacturer and device types. The device ID is 6 bits (0-63).
    *
    * @param deviceId The device id
    */
@@ -44,22 +38,19 @@
   }
 
   /**
-   * Create a new CAN communication interface with a specific device ID,
-   * manufacturer and device type. The device ID is 6 bits, the
-   * manufacturer is 8 bits, and the device type is 5 bits.
+   * Create a new CAN communication interface with a specific device ID, manufacturer and device
+   * type. The device ID is 6 bits, the manufacturer is 8 bits, and the device type is 5 bits.
    *
-   * @param deviceId           The device ID
+   * @param deviceId The device ID
    * @param deviceManufacturer The device manufacturer
-   * @param deviceType         The device type
+   * @param deviceType The device type
    */
   public CAN(int deviceId, int deviceManufacturer, int deviceType) {
     m_handle = CANAPIJNI.initializeCAN(deviceManufacturer, deviceId, deviceType);
     HAL.report(tResourceType.kResourceType_CAN, deviceId + 1);
   }
 
-  /**
-   * Closes the CAN communication.
-   */
+  /** Closes the CAN communication. */
   @Override
   public void close() {
     if (m_handle != 0) {
@@ -78,8 +69,8 @@
   }
 
   /**
-   * 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
+   * 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 apiId The API ID to write.
@@ -90,8 +81,8 @@
   }
 
   /**
-   * 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
+   * 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.
@@ -105,29 +96,32 @@
    *
    * @param data The data to write (8 bytes max)
    * @param apiId The API ID to write.
+   * @return TODO
    */
   public int writePacketNoThrow(byte[] data, int apiId) {
     return CANAPIJNI.writeCANPacketNoThrow(m_handle, data, 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
+   * 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 apiId The API ID to write.
    * @param repeatMs The period to repeat the packet at.
+   * @return TODO
    */
   public int writePacketRepeatingNoThrow(byte[] data, int apiId, int repeatMs) {
     return CANAPIJNI.writeCANPacketRepeatingNoThrow(m_handle, data, apiId, 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
+   * 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.
+   * @return TODO
    */
   public int writeRTRFrameNoThrow(int length, int apiId) {
     return CANAPIJNI.writeCANRTRFrameNoThrow(m_handle, length, apiId);
@@ -143,8 +137,8 @@
   }
 
   /**
-   * Read a new CAN packet. This will only return properly once per packet
-   * received. Multiple calls without receiving another packet will return false.
+   * Read a new CAN packet. This will only return properly once per packet received. Multiple calls
+   * without receiving another packet will return false.
    *
    * @param apiId The API ID to read.
    * @param data Storage for the received data.
@@ -155,8 +149,8 @@
   }
 
   /**
-   * Read a CAN packet. The will continuously return the last packet received,
-   * without accounting for packet age.
+   * Read a CAN packet. The will continuously return the last packet received, without accounting
+   * for packet age.
    *
    * @param apiId The API ID to read.
    * @param data Storage for the received data.
@@ -167,8 +161,8 @@
   }
 
   /**
-   * Read a CAN packet. The will return the last packet received until the
-   * packet is older then the requested timeout. Then it will return false.
+   * Read a CAN packet. The will return the last packet received until the packet is older then the
+   * requested timeout. Then it will return false.
    *
    * @param apiId The API ID to read.
    * @param timeoutMs The timeout time for the packet
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
deleted file mode 100644
index c8948b9..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CameraServer.java
+++ /dev/null
@@ -1,783 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Hashtable;
-import java.util.Map;
-import java.util.concurrent.atomic.AtomicInteger;
-
-import edu.wpi.cscore.AxisCamera;
-import edu.wpi.cscore.CameraServerJNI;
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.CvSource;
-import edu.wpi.cscore.MjpegServer;
-import edu.wpi.cscore.UsbCamera;
-import edu.wpi.cscore.VideoEvent;
-import edu.wpi.cscore.VideoException;
-import edu.wpi.cscore.VideoListener;
-import edu.wpi.cscore.VideoMode;
-import edu.wpi.cscore.VideoMode.PixelFormat;
-import edu.wpi.cscore.VideoProperty;
-import edu.wpi.cscore.VideoSink;
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.cameraserver.CameraServerSharedStore;
-import edu.wpi.first.networktables.EntryListenerFlags;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-/**
- * Singleton class for creating and keeping camera servers.
- * Also publishes camera information to NetworkTables.
- *
- * @deprecated Replaced with edu.wpi.first.cameraserver.CameraServer
- */
-@Deprecated
-public final class CameraServer {
-  public static final int kBasePort = 1181;
-
-  @Deprecated
-  public static final int kSize640x480 = 0;
-  @Deprecated
-  public static final int kSize320x240 = 1;
-  @Deprecated
-  public static final int kSize160x120 = 2;
-
-  private static final String kPublishName = "/CameraPublisher";
-  private static CameraServer server;
-
-  /**
-   * Get the CameraServer instance.
-   */
-  public static synchronized CameraServer getInstance() {
-    if (server == null) {
-      server = new CameraServer();
-    }
-    return server;
-  }
-
-  private final AtomicInteger m_defaultUsbDevice;
-  private String m_primarySourceName;
-  private final Map<String, VideoSource> m_sources;
-  private final Map<String, VideoSink> m_sinks;
-  private final Map<Integer, NetworkTable> m_tables;  // indexed by source handle
-  private final NetworkTable m_publishTable;
-  private final VideoListener m_videoListener; //NOPMD
-  private final int m_tableListener; //NOPMD
-  private int m_nextPort;
-  private String[] m_addresses;
-
-  @SuppressWarnings("JavadocMethod")
-  private static String makeSourceValue(int source) {
-    switch (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))) {
-      case kUsb:
-        return "usb:" + CameraServerJNI.getUsbCameraPath(source);
-      case kHttp: {
-        String[] urls = CameraServerJNI.getHttpCameraUrls(source);
-        if (urls.length > 0) {
-          return "ip:" + urls[0];
-        } else {
-          return "ip:";
-        }
-      }
-      case kCv:
-        // FIXME: Should be "cv:", but LabVIEW dashboard requires "usb:".
-        // https://github.com/wpilibsuite/allwpilib/issues/407
-        return "usb:";
-      default:
-        return "unknown:";
-    }
-  }
-
-  @SuppressWarnings("JavadocMethod")
-  private static String makeStreamValue(String address, int port) {
-    return "mjpg:http://" + address + ":" + port + "/?action=stream";
-  }
-
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
-  private synchronized String[] getSinkStreamValues(int sink) {
-    // Ignore all but MjpegServer
-    if (VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink)) != VideoSink.Kind.kMjpeg) {
-      return new String[0];
-    }
-
-    // Get port
-    int port = CameraServerJNI.getMjpegServerPort(sink);
-
-    // Generate values
-    ArrayList<String> values = new ArrayList<>(m_addresses.length + 1);
-    String listenAddress = CameraServerJNI.getMjpegServerListenAddress(sink);
-    if (!listenAddress.isEmpty()) {
-      // If a listen address is specified, only use that
-      values.add(makeStreamValue(listenAddress, port));
-    } else {
-      // Otherwise generate for hostname and all interface addresses
-      values.add(makeStreamValue(CameraServerJNI.getHostname() + ".local", port));
-      for (String addr : m_addresses) {
-        if ("127.0.0.1".equals(addr)) {
-          continue;  // ignore localhost
-        }
-        values.add(makeStreamValue(addr, port));
-      }
-    }
-
-    return values.toArray(new String[0]);
-  }
-
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
-  private synchronized String[] getSourceStreamValues(int source) {
-    // Ignore all but HttpCamera
-    if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
-            != VideoSource.Kind.kHttp) {
-      return new String[0];
-    }
-
-    // Generate values
-    String[] values = CameraServerJNI.getHttpCameraUrls(source);
-    for (int j = 0; j < values.length; j++) {
-      values[j] = "mjpg:" + values[j];
-    }
-
-    if (CameraServerSharedStore.getCameraServerShared().isRoboRIO()) {
-      // Look to see if we have a passthrough server for this source
-      // Only do this on the roboRIO
-      for (VideoSink i : m_sinks.values()) {
-        int sink = i.getHandle();
-        int sinkSource = CameraServerJNI.getSinkSource(sink);
-        if (source == sinkSource
-            && VideoSink.getKindFromInt(CameraServerJNI.getSinkKind(sink))
-            == VideoSink.Kind.kMjpeg) {
-          // Add USB-only passthrough
-          String[] finalValues = Arrays.copyOf(values, values.length + 1);
-          int port = CameraServerJNI.getMjpegServerPort(sink);
-          finalValues[values.length] = makeStreamValue("172.22.11.2", port);
-          return finalValues;
-        }
-      }
-    }
-
-    return values;
-  }
-
-  @SuppressWarnings({"JavadocMethod", "PMD.AvoidUsingHardCodedIP"})
-  private synchronized void updateStreamValues() {
-    // Over all the sinks...
-    for (VideoSink i : m_sinks.values()) {
-      int sink = i.getHandle();
-
-      // Get the source's subtable (if none exists, we're done)
-      int source = CameraServerJNI.getSinkSource(sink);
-      if (source == 0) {
-        continue;
-      }
-      NetworkTable table = m_tables.get(source);
-      if (table != null) {
-        // Don't set stream values if this is a HttpCamera passthrough
-        if (VideoSource.getKindFromInt(CameraServerJNI.getSourceKind(source))
-            == VideoSource.Kind.kHttp) {
-          continue;
-        }
-
-        // Set table value
-        String[] values = getSinkStreamValues(sink);
-        if (values.length > 0) {
-          table.getEntry("streams").setStringArray(values);
-        }
-      }
-    }
-
-    // Over all the sources...
-    for (VideoSource i : m_sources.values()) {
-      int source = i.getHandle();
-
-      // Get the source's subtable (if none exists, we're done)
-      NetworkTable table = m_tables.get(source);
-      if (table != null) {
-        // Set table value
-        String[] values = getSourceStreamValues(source);
-        if (values.length > 0) {
-          table.getEntry("streams").setStringArray(values);
-        }
-      }
-    }
-  }
-
-  @SuppressWarnings("JavadocMethod")
-  private static String pixelFormatToString(PixelFormat pixelFormat) {
-    switch (pixelFormat) {
-      case kMJPEG:
-        return "MJPEG";
-      case kYUYV:
-        return "YUYV";
-      case kRGB565:
-        return "RGB565";
-      case kBGR:
-        return "BGR";
-      case kGray:
-        return "Gray";
-      default:
-        return "Unknown";
-    }
-  }
-
-  /// Provide string description of video mode.
-  /// The returned string is "{width}x{height} {format} {fps} fps".
-  @SuppressWarnings("JavadocMethod")
-  private static String videoModeToString(VideoMode mode) {
-    return mode.width + "x" + mode.height + " " + pixelFormatToString(mode.pixelFormat)
-        + " " + mode.fps + " fps";
-  }
-
-  @SuppressWarnings("JavadocMethod")
-  private static String[] getSourceModeValues(int sourceHandle) {
-    VideoMode[] modes = CameraServerJNI.enumerateSourceVideoModes(sourceHandle);
-    String[] modeStrings = new String[modes.length];
-    for (int i = 0; i < modes.length; i++) {
-      modeStrings[i] = videoModeToString(modes[i]);
-    }
-    return modeStrings;
-  }
-
-  @SuppressWarnings({"JavadocMethod", "PMD.CyclomaticComplexity"})
-  private static void putSourcePropertyValue(NetworkTable table, VideoEvent event, boolean isNew) {
-    String name;
-    String infoName;
-    if (event.name.startsWith("raw_")) {
-      name = "RawProperty/" + event.name;
-      infoName = "RawPropertyInfo/" + event.name;
-    } else {
-      name = "Property/" + event.name;
-      infoName = "PropertyInfo/" + event.name;
-    }
-
-    NetworkTableEntry entry = table.getEntry(name);
-    try {
-      switch (event.propertyKind) {
-        case kBoolean:
-          if (isNew) {
-            entry.setDefaultBoolean(event.value != 0);
-          } else {
-            entry.setBoolean(event.value != 0);
-          }
-          break;
-        case kInteger:
-        case kEnum:
-          if (isNew) {
-            entry.setDefaultDouble(event.value);
-            table.getEntry(infoName + "/min").setDouble(
-                CameraServerJNI.getPropertyMin(event.propertyHandle));
-            table.getEntry(infoName + "/max").setDouble(
-                CameraServerJNI.getPropertyMax(event.propertyHandle));
-            table.getEntry(infoName + "/step").setDouble(
-                CameraServerJNI.getPropertyStep(event.propertyHandle));
-            table.getEntry(infoName + "/default").setDouble(
-                CameraServerJNI.getPropertyDefault(event.propertyHandle));
-          } else {
-            entry.setDouble(event.value);
-          }
-          break;
-        case kString:
-          if (isNew) {
-            entry.setDefaultString(event.valueStr);
-          } else {
-            entry.setString(event.valueStr);
-          }
-          break;
-        default:
-          break;
-      }
-    } catch (VideoException ignored) {
-      // ignore
-    }
-  }
-
-  @SuppressWarnings({"JavadocMethod", "PMD.UnusedLocalVariable", "PMD.ExcessiveMethodLength",
-      "PMD.NPathComplexity"})
-  private CameraServer() {
-    m_defaultUsbDevice = new AtomicInteger();
-    m_sources = new Hashtable<>();
-    m_sinks = new Hashtable<>();
-    m_tables = new Hashtable<>();
-    m_publishTable = NetworkTableInstance.getDefault().getTable(kPublishName);
-    m_nextPort = kBasePort;
-    m_addresses = new String[0];
-
-    // We publish sources to NetworkTables using the following structure:
-    // "/CameraPublisher/{Source.Name}/" - root
-    // - "source" (string): Descriptive, prefixed with type (e.g. "usb:0")
-    // - "streams" (string array): URLs that can be used to stream data
-    // - "description" (string): Description of the source
-    // - "connected" (boolean): Whether source is connected
-    // - "mode" (string): Current video mode
-    // - "modes" (string array): Available video modes
-    // - "Property/{Property}" - Property values
-    // - "PropertyInfo/{Property}" - Property supporting information
-
-    // Listener for video events
-    m_videoListener = new VideoListener(event -> {
-      switch (event.kind) {
-        case kSourceCreated: {
-          // Create subtable for the camera
-          NetworkTable table = m_publishTable.getSubTable(event.name);
-          m_tables.put(event.sourceHandle, table);
-          table.getEntry("source").setString(makeSourceValue(event.sourceHandle));
-          table.getEntry("description").setString(
-              CameraServerJNI.getSourceDescription(event.sourceHandle));
-          table.getEntry("connected").setBoolean(
-              CameraServerJNI.isSourceConnected(event.sourceHandle));
-          table.getEntry("streams").setStringArray(getSourceStreamValues(event.sourceHandle));
-          try {
-            VideoMode mode = CameraServerJNI.getSourceVideoMode(event.sourceHandle);
-            table.getEntry("mode").setDefaultString(videoModeToString(mode));
-            table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
-          } catch (VideoException ignored) {
-            // Do nothing. Let the other event handlers update this if there is an error.
-          }
-          break;
-        }
-        case kSourceDestroyed: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("source").setString("");
-            table.getEntry("streams").setStringArray(new String[0]);
-            table.getEntry("modes").setStringArray(new String[0]);
-          }
-          break;
-        }
-        case kSourceConnected: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            // update the description too (as it may have changed)
-            table.getEntry("description").setString(
-                CameraServerJNI.getSourceDescription(event.sourceHandle));
-            table.getEntry("connected").setBoolean(true);
-          }
-          break;
-        }
-        case kSourceDisconnected: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("connected").setBoolean(false);
-          }
-          break;
-        }
-        case kSourceVideoModesUpdated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("modes").setStringArray(getSourceModeValues(event.sourceHandle));
-          }
-          break;
-        }
-        case kSourceVideoModeChanged: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            table.getEntry("mode").setString(videoModeToString(event.mode));
-          }
-          break;
-        }
-        case kSourcePropertyCreated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            putSourcePropertyValue(table, event, true);
-          }
-          break;
-        }
-        case kSourcePropertyValueUpdated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            putSourcePropertyValue(table, event, false);
-          }
-          break;
-        }
-        case kSourcePropertyChoicesUpdated: {
-          NetworkTable table = m_tables.get(event.sourceHandle);
-          if (table != null) {
-            try {
-              String[] choices = CameraServerJNI.getEnumPropertyChoices(event.propertyHandle);
-              table.getEntry("PropertyInfo/" + event.name + "/choices").setStringArray(choices);
-            } catch (VideoException ignored) {
-              // ignore
-            }
-          }
-          break;
-        }
-        case kSinkSourceChanged:
-        case kSinkCreated:
-        case kSinkDestroyed:
-        case kNetworkInterfacesChanged: {
-          m_addresses = CameraServerJNI.getNetworkInterfaces();
-          updateStreamValues();
-          break;
-        }
-        default:
-          break;
-      }
-    }, 0x4fff, true);
-
-    // Listener for NetworkTable events
-    // We don't currently support changing settings via NT due to
-    // synchronization issues, so just update to current setting if someone
-    // else tries to change it.
-    m_tableListener = NetworkTableInstance.getDefault().addEntryListener(kPublishName + "/",
-      event -> {
-        String relativeKey = event.name.substring(kPublishName.length() + 1);
-
-        // get source (sourceName/...)
-        int subKeyIndex = relativeKey.indexOf('/');
-        if (subKeyIndex == -1) {
-          return;
-        }
-        String sourceName = relativeKey.substring(0, subKeyIndex);
-        VideoSource source = m_sources.get(sourceName);
-        if (source == null) {
-          return;
-        }
-
-        // get subkey
-        relativeKey = relativeKey.substring(subKeyIndex + 1);
-
-        // handle standard names
-        String propName;
-        if ("mode".equals(relativeKey)) {
-          // reset to current mode
-          event.getEntry().setString(videoModeToString(source.getVideoMode()));
-          return;
-        } else if (relativeKey.startsWith("Property/")) {
-          propName = relativeKey.substring(9);
-        } else if (relativeKey.startsWith("RawProperty/")) {
-          propName = relativeKey.substring(12);
-        } else {
-          return;  // ignore
-        }
-
-        // everything else is a property
-        VideoProperty property = source.getProperty(propName);
-        switch (property.getKind()) {
-          case kNone:
-            return;
-          case kBoolean:
-            // reset to current setting
-            event.getEntry().setBoolean(property.get() != 0);
-            return;
-          case kInteger:
-          case kEnum:
-            // reset to current setting
-            event.getEntry().setDouble(property.get());
-            return;
-          case kString:
-            // reset to current setting
-            event.getEntry().setString(property.getString());
-            return;
-          default:
-            return;
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kUpdate);
-  }
-
-  /**
-   * Start automatically capturing images to send to the dashboard.
-   *
-   * <p>You should call this method to see a camera feed on the dashboard.
-   * If you also want to perform vision processing on the roboRIO, use
-   * getVideo() to get access to the camera images.
-   *
-   * <p>The first time this overload is called, it calls
-   * {@link #startAutomaticCapture(int)} with device 0, creating a camera
-   * named "USB Camera 0".  Subsequent calls increment the device number
-   * (e.g. 1, 2, etc).
-   */
-  public UsbCamera startAutomaticCapture() {
-    UsbCamera camera = startAutomaticCapture(m_defaultUsbDevice.getAndIncrement());
-    CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
-    return camera;
-  }
-
-  /**
-   * Start automatically capturing images to send to the dashboard.
-   *
-   * <p>This overload calls {@link #startAutomaticCapture(String, int)} with
-   * a name of "USB Camera {dev}".
-   *
-   * @param dev The device number of the camera interface
-   */
-  public UsbCamera startAutomaticCapture(int dev) {
-    UsbCamera camera = new UsbCamera("USB Camera " + dev, dev);
-    startAutomaticCapture(camera);
-    CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
-    return camera;
-  }
-
-  /**
-   * Start automatically capturing images to send to the dashboard.
-   *
-   * @param name The name to give the camera
-   * @param dev The device number of the camera interface
-   */
-  public UsbCamera startAutomaticCapture(String name, int dev) {
-    UsbCamera camera = new UsbCamera(name, dev);
-    startAutomaticCapture(camera);
-    CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
-    return camera;
-  }
-
-  /**
-   * Start automatically capturing images to send to the dashboard.
-   *
-   * @param name The name to give the camera
-   * @param path The device path (e.g. "/dev/video0") of the camera
-   */
-  public UsbCamera startAutomaticCapture(String name, String path) {
-    UsbCamera camera = new UsbCamera(name, path);
-    startAutomaticCapture(camera);
-    CameraServerSharedStore.getCameraServerShared().reportUsbCamera(camera.getHandle());
-    return camera;
-  }
-
-  /**
-   * Start automatically capturing images to send to the dashboard from
-   * an existing camera.
-   *
-   * @param camera Camera
-   */
-  public void startAutomaticCapture(VideoSource camera) {
-    addCamera(camera);
-    VideoSink server = addServer("serve_" + camera.getName());
-    server.setSource(camera);
-  }
-
-  /**
-   * Adds an Axis IP camera.
-   *
-   * <p>This overload calls {@link #addAxisCamera(String, String)} with
-   * name "Axis Camera".
-   *
-   * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   */
-  public AxisCamera addAxisCamera(String host) {
-    return addAxisCamera("Axis Camera", host);
-  }
-
-  /**
-   * Adds an Axis IP camera.
-   *
-   * <p>This overload calls {@link #addAxisCamera(String, String[])} with
-   * name "Axis Camera".
-   *
-   * @param hosts Array of Camera host IPs/DNS names
-   */
-  public AxisCamera addAxisCamera(String[] hosts) {
-    return addAxisCamera("Axis Camera", hosts);
-  }
-
-  /**
-   * Adds an Axis IP camera.
-   *
-   * @param name The name to give the camera
-   * @param host Camera host IP or DNS name (e.g. "10.x.y.11")
-   */
-  public AxisCamera addAxisCamera(String name, String host) {
-    AxisCamera camera = new AxisCamera(name, host);
-    // Create a passthrough MJPEG server for USB access
-    startAutomaticCapture(camera);
-    CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
-    return camera;
-  }
-
-  /**
-   * Adds an Axis IP camera.
-   *
-   * @param name The name to give the camera
-   * @param hosts Array of Camera host IPs/DNS names
-   */
-  public AxisCamera addAxisCamera(String name, String[] hosts) {
-    AxisCamera camera = new AxisCamera(name, hosts);
-    // Create a passthrough MJPEG server for USB access
-    startAutomaticCapture(camera);
-    CameraServerSharedStore.getCameraServerShared().reportAxisCamera(camera.getHandle());
-    return camera;
-  }
-
-  /**
-   * Get OpenCV access to the primary camera feed.  This allows you to
-   * get images from the camera for image processing on the roboRIO.
-   *
-   * <p>This is only valid to call after a camera feed has been added
-   * with startAutomaticCapture() or addServer().
-   */
-  public CvSink getVideo() {
-    VideoSource source;
-    synchronized (this) {
-      if (m_primarySourceName == null) {
-        throw new VideoException("no camera available");
-      }
-      source = m_sources.get(m_primarySourceName);
-    }
-    if (source == null) {
-      throw new VideoException("no camera available");
-    }
-    return getVideo(source);
-  }
-
-  /**
-   * Get OpenCV access to the specified camera.  This allows you to get
-   * images from the camera for image processing on the roboRIO.
-   *
-   * @param camera Camera (e.g. as returned by startAutomaticCapture).
-   */
-  public CvSink getVideo(VideoSource camera) {
-    String name = "opencv_" + camera.getName();
-
-    synchronized (this) {
-      VideoSink sink = m_sinks.get(name);
-      if (sink != null) {
-        VideoSink.Kind kind = sink.getKind();
-        if (kind != VideoSink.Kind.kCv) {
-          throw new VideoException("expected OpenCV sink, but got " + kind);
-        }
-        return (CvSink) sink;
-      }
-    }
-
-    CvSink newsink = new CvSink(name);
-    newsink.setSource(camera);
-    addServer(newsink);
-    return newsink;
-  }
-
-  /**
-   * Get OpenCV access to the specified camera.  This allows you to get
-   * images from the camera for image processing on the roboRIO.
-   *
-   * @param name Camera name
-   */
-  public CvSink getVideo(String name) {
-    VideoSource source;
-    synchronized (this) {
-      source = m_sources.get(name);
-      if (source == null) {
-        throw new VideoException("could not find camera " + name);
-      }
-    }
-    return getVideo(source);
-  }
-
-  /**
-   * Create a MJPEG stream with OpenCV input. This can be called to pass custom
-   * annotated images to the dashboard.
-   *
-   * @param name Name to give the stream
-   * @param width Width of the image being sent
-   * @param height Height of the image being sent
-   */
-  public CvSource putVideo(String name, int width, int height) {
-    CvSource source = new CvSource(name, VideoMode.PixelFormat.kMJPEG, width, height, 30);
-    startAutomaticCapture(source);
-    return source;
-  }
-
-  /**
-   * Adds a MJPEG server at the next available port.
-   *
-   * @param name Server name
-   */
-  public MjpegServer addServer(String name) {
-    int port;
-    synchronized (this) {
-      port = m_nextPort;
-      m_nextPort++;
-    }
-    return addServer(name, port);
-  }
-
-  /**
-   * Adds a MJPEG server.
-   *
-   * @param name Server name
-   */
-  public MjpegServer addServer(String name, int port) {
-    MjpegServer server = new MjpegServer(name, port);
-    addServer(server);
-    return server;
-  }
-
-  /**
-   * Adds an already created server.
-   *
-   * @param server Server
-   */
-  public void addServer(VideoSink server) {
-    synchronized (this) {
-      m_sinks.put(server.getName(), server);
-    }
-  }
-
-  /**
-   * Removes a server by name.
-   *
-   * @param name Server name
-   */
-  public void removeServer(String name) {
-    synchronized (this) {
-      m_sinks.remove(name);
-    }
-  }
-
-  /**
-   * Get server for the primary camera feed.
-   *
-   * <p>This is only valid to call after a camera feed has been added
-   * with startAutomaticCapture() or addServer().
-   */
-  public VideoSink getServer() {
-    synchronized (this) {
-      if (m_primarySourceName == null) {
-        throw new VideoException("no camera available");
-      }
-      return getServer("serve_" + m_primarySourceName);
-    }
-  }
-
-  /**
-   * Gets a server by name.
-   *
-   * @param name Server name
-   */
-  public VideoSink getServer(String name) {
-    synchronized (this) {
-      return m_sinks.get(name);
-    }
-  }
-
-  /**
-   * Adds an already created camera.
-   *
-   * @param camera Camera
-   */
-  public void addCamera(VideoSource camera) {
-    String name = camera.getName();
-    synchronized (this) {
-      if (m_primarySourceName == null) {
-        m_primarySourceName = name;
-      }
-      m_sources.put(name, camera);
-    }
-  }
-
-  /**
-   * Removes a camera by name.
-   *
-   * @param name Camera name
-   */
-  public void removeCamera(String name) {
-    synchronized (this) {
-      m_sources.remove(name);
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
index 8fbefd8..f48fb63 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Compressor.java
@@ -1,61 +1,79 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import edu.wpi.first.hal.CompressorJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
- * Class for operating a compressor connected to a PCM (Pneumatic Control Module). The PCM will
- * automatically run in closed loop mode by default whenever a {@link Solenoid} object is created.
- * For most cases, a Compressor object does not need to be instantiated or used in a robot program.
- * This class is only required in cases where the robot program needs a more detailed status of the
- * compressor or to enable/disable closed loop control.
+ * Class for operating a compressor connected to a pneumatics module. The module will automatically
+ * run in closed loop mode by default whenever a {@link Solenoid} object is created. For most cases,
+ * a Compressor object does not need to be instantiated or used in a robot program. This class is
+ * only required in cases where the robot program needs a more detailed status of the compressor or
+ * to enable/disable closed loop control.
  *
  * <p>Note: you cannot operate the compressor directly from this class as doing so would circumvent
  * the safety provided by using the pressure switch and closed loop control. You can only turn off
  * closed loop control, thereby stopping the compressor from operating.
  */
 public class Compressor implements Sendable, AutoCloseable {
-  private int m_compressorHandle;
-  private byte m_module;
+  private PneumaticsBase m_module;
 
   /**
-   * Makes a new instance of the compressor using the provided CAN device ID.  Use this constructor
-   * when you have more than one PCM.
+   * Constructs a compressor for a specified module and type.
    *
-   * @param module The PCM CAN device ID (0 - 62 inclusive)
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
    */
-  public Compressor(int module) {
-    m_module = (byte) module;
+  public Compressor(int module, PneumaticsModuleType moduleType) {
+    m_module = PneumaticsBase.getForType(module, moduleType);
+    boolean allocatedCompressor = false;
+    boolean successfulCompletion = false;
 
-    m_compressorHandle = CompressorJNI.initializeCompressor((byte) module);
+    try {
+      if (!m_module.reserveCompressor()) {
+        throw new AllocationException("Compressor already allocated");
+      }
 
-    HAL.report(tResourceType.kResourceType_Compressor, module + 1);
-    SendableRegistry.addLW(this, "Compressor", module);
+      allocatedCompressor = true;
+
+      m_module.setClosedLoopControl(true);
+
+      HAL.report(tResourceType.kResourceType_Compressor, module + 1);
+      SendableRegistry.addLW(this, "Compressor", module);
+      successfulCompletion = true;
+
+    } finally {
+      if (!successfulCompletion) {
+        if (allocatedCompressor) {
+          m_module.unreserveCompressor();
+        }
+        m_module.close();
+      }
+    }
   }
 
   /**
-   * Makes a new instance of the compressor using the default PCM ID of 0.
+   * Constructs a compressor for a default module and specified type.
    *
-   * <p>Additional modules can be supported by making a new instance and {@link #Compressor(int)
-   * specifying the CAN ID.}
+   * @param moduleType The module type to use.
    */
-  public Compressor() {
-    this(SensorUtil.getDefaultSolenoidModule());
+  public Compressor(PneumaticsModuleType moduleType) {
+    this(PneumaticsBase.getDefaultForType(moduleType), moduleType);
   }
 
   @Override
   public void close() {
     SendableRegistry.remove(this);
+    m_module.unreserveCompressor();
+    m_module.close();
+    m_module = null;
   }
 
   /**
@@ -86,7 +104,7 @@
    * @return true if the compressor is on
    */
   public boolean enabled() {
-    return CompressorJNI.getCompressor(m_compressorHandle);
+    return m_module.getCompressor();
   }
 
   /**
@@ -95,7 +113,7 @@
    * @return true if the pressure is low
    */
   public boolean getPressureSwitchValue() {
-    return CompressorJNI.getCompressorPressureSwitch(m_compressorHandle);
+    return m_module.getPressureSwitch();
   }
 
   /**
@@ -104,7 +122,7 @@
    * @return current consumed by the compressor in amps
    */
   public double getCompressorCurrent() {
-    return CompressorJNI.getCompressorCurrent(m_compressorHandle);
+    return m_module.getCompressorCurrent();
   }
 
   /**
@@ -113,7 +131,7 @@
    * @param on if true sets the compressor to be in closed loop control mode (default)
    */
   public void setClosedLoopControl(boolean on) {
-    CompressorJNI.setCompressorClosedLoopControl(m_compressorHandle, on);
+    m_module.setClosedLoopControl(on);
   }
 
   /**
@@ -122,99 +140,15 @@
    * @return true if compressor is operating on closed-loop mode
    */
   public boolean getClosedLoopControl() {
-    return CompressorJNI.getCompressorClosedLoopControl(m_compressorHandle);
-  }
-
-  /**
-   * If PCM is in fault state : Compressor Drive is disabled due to compressor current being too
-   * high.
-   *
-   * @return true if PCM is in fault state.
-   */
-  public boolean getCompressorCurrentTooHighFault() {
-    return CompressorJNI.getCompressorCurrentTooHighFault(m_compressorHandle);
-  }
-
-  /**
-   * If PCM sticky fault is set : Compressor is disabled due to compressor current being too
-   * high.
-   *
-   * @return true if PCM sticky fault is set.
-   */
-  public boolean getCompressorCurrentTooHighStickyFault() {
-    return CompressorJNI.getCompressorCurrentTooHighStickyFault(m_compressorHandle);
-  }
-
-  /**
-   * If PCM sticky fault is set : Compressor output appears to be shorted.
-   *
-   * @return true if PCM sticky fault is set.
-   */
-  public boolean getCompressorShortedStickyFault() {
-    return CompressorJNI.getCompressorShortedStickyFault(m_compressorHandle);
-  }
-
-  /**
-   * If PCM is in fault state : Compressor output appears to be shorted.
-   *
-   * @return true if PCM is in fault state.
-   */
-  public boolean getCompressorShortedFault() {
-    return CompressorJNI.getCompressorShortedFault(m_compressorHandle);
-  }
-
-  /**
-   * If PCM sticky fault is set : Compressor does not appear to be wired, i.e. compressor is not
-   * drawing enough current.
-   *
-   * @return true if PCM sticky fault is set.
-   */
-  public boolean getCompressorNotConnectedStickyFault() {
-    return CompressorJNI.getCompressorNotConnectedStickyFault(m_compressorHandle);
-  }
-
-  /**
-   * If PCM is in fault state : Compressor does not appear to be wired, i.e. compressor is not
-   * drawing enough current.
-   *
-   * @return true if PCM is in fault state.
-   */
-  public boolean getCompressorNotConnectedFault() {
-    return CompressorJNI.getCompressorNotConnectedFault(m_compressorHandle);
-  }
-
-  /**
-   * Clear ALL sticky faults inside PCM that Compressor is wired to.
-   *
-   * <p>If a sticky fault is set, then it will be persistently cleared. The compressor might
-   * momentarily disable while the flags are being cleared. Doo not call this method too
-   * frequently, otherwise normal compressor functionality may be prevented.
-   *
-   * <p>If no sticky faults are set then this call will have no effect.
-   */
-  public void clearAllPCMStickyFaults() {
-    CompressorJNI.clearAllPCMStickyFaults(m_module);
-  }
-
-  /**
-   * Gets the module number (CAN ID).
-   *
-   * @return Module number
-   */
-  public int getModule() {
-    return m_module;
+    return m_module.getClosedLoopControl();
   }
 
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Compressor");
-    builder.addBooleanProperty("Enabled", this::enabled, value -> {
-      if (value) {
-        start();
-      } else {
-        stop();
-      }
-    });
+    builder.addBooleanProperty(
+        "Closed Loop Control", this::getClosedLoopControl, this::setClosedLoopControl);
+    builder.addBooleanProperty("Enabled", this::enabled, null);
     builder.addBooleanProperty("Pressure switch", this::getPressureSwitchValue, null);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
index 31d50d8..886050b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Controller.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -16,9 +13,7 @@
  */
 @Deprecated(since = "2020", forRemoval = true)
 public interface Controller {
-  /**
-   * Allows the control loop to run.
-   */
+  /** Allows the control loop to run. */
   void enable();
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
index 0a26810..d445e9f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Counter.java
@@ -1,58 +1,44 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static java.util.Objects.requireNonNull;
 
 import edu.wpi.first.hal.CounterJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-import static java.util.Objects.requireNonNull;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
 
 /**
  * Class for counting the number of ticks on a digital input channel.
  *
  * <p>This is a general purpose class for counting repetitive events. It can return the number of
- * counts, the period of the most recent cycle, and detect when the signal being counted has
- * stopped by supplying a maximum cycle time.
+ * counts, the period of the most recent cycle, and detect when the signal being counted has stopped
+ * by supplying a maximum cycle time.
  *
  * <p>All counters will immediately start counting - reset() them if you need them to be zeroed
  * before use.
  */
-public class Counter implements CounterBase, PIDSource, Sendable, AutoCloseable {
-  /**
-   * Mode determines how and what the counter counts.
-   */
+public class Counter implements CounterBase, Sendable, AutoCloseable {
+  /** Mode determines how and what the counter counts. */
   public enum Mode {
-    /**
-     * mode: two pulse.
-     */
+    /** mode: two pulse. */
     kTwoPulse(0),
-    /**
-     * mode: semi period.
-     */
+    /** mode: semi period. */
     kSemiperiod(1),
-    /**
-     * mode: pulse length.
-     */
+    /** mode: pulse length. */
     kPulseLength(2),
-    /**
-     * mode: external direction.
-     */
+    /** mode: external direction. */
     kExternalDirection(3);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     Mode(int value) {
@@ -64,13 +50,14 @@
   protected DigitalSource m_downSource; // /< What makes the counter count down.
   private boolean m_allocatedUpSource;
   private boolean m_allocatedDownSource;
-  private int m_counter; // /< The FPGA counter object.
+  int m_counter; // /< The FPGA counter object.
   private int m_index; // /< The index of this counter.
-  private PIDSourceType m_pidSource;
   private double m_distancePerPulse; // distance of travel for each tick
 
   /**
    * Create an instance of a counter with the given mode.
+   *
+   * @param mode The counter mode.
    */
   public Counter(final Mode mode) {
     ByteBuffer index = ByteBuffer.allocateDirect(4);
@@ -135,12 +122,15 @@
    * <p>The counter will start counting immediately.
    *
    * @param encodingType which edges to count
-   * @param upSource     first source to count
-   * @param downSource   second source for direction
-   * @param inverted     true to invert the count
+   * @param upSource first source to count
+   * @param downSource second source for direction
+   * @param inverted true to invert the count
    */
-  public Counter(EncodingType encodingType, DigitalSource upSource, DigitalSource downSource,
-                 boolean inverted) {
+  public Counter(
+      EncodingType encodingType,
+      DigitalSource upSource,
+      DigitalSource downSource,
+      boolean inverted) {
     this(Mode.kExternalDirection);
 
     requireNonNullParam(encodingType, "encodingType", "Counter");
@@ -229,15 +219,15 @@
       m_allocatedUpSource = false;
     }
     m_upSource = source;
-    CounterJNI.setCounterUpSource(m_counter, source.getPortHandleForRouting(),
-        source.getAnalogTriggerTypeForRouting());
+    CounterJNI.setCounterUpSource(
+        m_counter, source.getPortHandleForRouting(), source.getAnalogTriggerTypeForRouting());
   }
 
   /**
    * Set the up counting source to be an analog trigger.
    *
    * @param analogTrigger The analog trigger object that is used for the Up Source
-   * @param triggerType   The analog trigger output that will trigger the counter.
+   * @param triggerType The analog trigger output that will trigger the counter.
    */
   public void setUpSource(AnalogTrigger analogTrigger, AnalogTriggerType triggerType) {
     requireNonNullParam(analogTrigger, "analogTrigger", "setUpSource");
@@ -251,7 +241,7 @@
    * Set the edge sensitivity on an up counting source. Set the up source to either detect rising
    * edges or falling edges.
    *
-   * @param risingEdge  true to count rising edge
+   * @param risingEdge true to count rising edge
    * @param fallingEdge true to count falling edge
    */
   public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
@@ -261,9 +251,7 @@
     CounterJNI.setCounterUpSourceEdge(m_counter, risingEdge, fallingEdge);
   }
 
-  /**
-   * Disable the up counting source to the counter.
-   */
+  /** Disable the up counting source to the counter. */
   public void clearUpSource() {
     if (m_upSource != null && m_allocatedUpSource) {
       m_upSource.close();
@@ -298,8 +286,8 @@
       m_downSource.close();
       m_allocatedDownSource = false;
     }
-    CounterJNI.setCounterDownSource(m_counter, source.getPortHandleForRouting(),
-        source.getAnalogTriggerTypeForRouting());
+    CounterJNI.setCounterDownSource(
+        m_counter, source.getPortHandleForRouting(), source.getAnalogTriggerTypeForRouting());
     m_downSource = source;
   }
 
@@ -307,7 +295,7 @@
    * Set the down counting source to be an analog trigger.
    *
    * @param analogTrigger The analog trigger object that is used for the Down Source
-   * @param triggerType   The analog trigger output that will trigger the counter.
+   * @param triggerType The analog trigger output that will trigger the counter.
    */
   public void setDownSource(AnalogTrigger analogTrigger, AnalogTriggerType triggerType) {
     requireNonNullParam(analogTrigger, "analogTrigger", "setDownSource");
@@ -321,7 +309,7 @@
    * Set the edge sensitivity on a down counting source. Set the down source to either detect rising
    * edges or falling edges.
    *
-   * @param risingEdge  true to count the rising edge
+   * @param risingEdge true to count the rising edge
    * @param fallingEdge true to count the falling edge
    */
   public void setDownSourceEdge(boolean risingEdge, boolean fallingEdge) {
@@ -330,9 +318,7 @@
     CounterJNI.setCounterDownSourceEdge(m_counter, risingEdge, fallingEdge);
   }
 
-  /**
-   * Disable the down counting source to the counter.
-   */
+  /** Disable the down counting source to the counter. */
   public void clearDownSource() {
     if (m_downSource != null && m_allocatedDownSource) {
       m_downSource.close();
@@ -373,7 +359,7 @@
    * is most useful for direction sensitive gear tooth sensors.
    *
    * @param threshold The pulse length beyond which the counter counts the opposite direction. Units
-   *                  are seconds.
+   *     are seconds.
    */
   public void setPulseLengthMode(double threshold) {
     CounterJNI.setCounterPulseLengthMode(m_counter, threshold);
@@ -525,39 +511,6 @@
     m_distancePerPulse = distancePerPulse;
   }
 
-  /**
-   * Set which parameter of the encoder you are using as a process control variable. The counter
-   * class supports the rate and distance parameters.
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    requireNonNullParam(pidSource, "pidSource", "setPIDSourceType");
-    if (pidSource != PIDSourceType.kDisplacement && pidSource != PIDSourceType.kRate) {
-      throw new IllegalArgumentException("PID Source parameter was not valid type: " + pidSource);
-    }
-
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  @Override
-  public double pidGet() {
-    switch (m_pidSource) {
-      case kDisplacement:
-        return getDistance();
-      case kRate:
-        return getRate();
-      default:
-        return 0.0;
-    }
-  }
-
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("Counter");
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
index 9d1d6b7..735b893 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/CounterBase.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -16,24 +13,15 @@
  * before use.
  */
 public interface CounterBase {
-  /**
-   * The number of edges for the counterbase to increment or decrement on.
-   */
+  /** The number of edges for the counterbase to increment or decrement on. */
   enum EncodingType {
-    /**
-     * Count only the rising edge.
-     */
+    /** Count only the rising edge. */
     k1X(0),
-    /**
-     * Count both the rising and falling edge.
-     */
+    /** Count both the rising and falling edge. */
     k2X(1),
-    /**
-     * Count rising and falling on both channels.
-     */
+    /** Count rising and falling on both channels. */
     k4X(2);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     EncodingType(int value) {
@@ -48,9 +36,7 @@
    */
   int get();
 
-  /**
-   * Reset the count to zero.
-   */
+  /** Reset the count to zero. */
   void reset();
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java
new file mode 100644
index 0000000..a919da5
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMA.java
@@ -0,0 +1,110 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.DMAJNI;
+import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
+
+public class DMA implements AutoCloseable {
+  final int m_dmaHandle;
+
+  public DMA() {
+    m_dmaHandle = DMAJNI.initialize();
+  }
+
+  @Override
+  public void close() {
+    DMAJNI.free(m_dmaHandle);
+  }
+
+  public void setPause(boolean pause) {
+    DMAJNI.setPause(m_dmaHandle, pause);
+  }
+
+  public void setTimedTrigger(double periodSeconds) {
+    DMAJNI.setTimedTrigger(m_dmaHandle, periodSeconds);
+  }
+
+  public void setTimedTriggerCycles(int cycles) {
+    DMAJNI.setTimedTriggerCycles(m_dmaHandle, cycles);
+  }
+
+  public void addEncoder(Encoder encoder) {
+    DMAJNI.addEncoder(m_dmaHandle, encoder.m_encoder);
+  }
+
+  public void addEncoderPeriod(Encoder encoder) {
+    DMAJNI.addEncoderPeriod(m_dmaHandle, encoder.m_encoder);
+  }
+
+  public void addCounter(Counter counter) {
+    DMAJNI.addCounter(m_dmaHandle, counter.m_counter);
+  }
+
+  public void addCounterPeriod(Counter counter) {
+    DMAJNI.addCounterPeriod(m_dmaHandle, counter.m_counter);
+  }
+
+  public void addDigitalSource(DigitalSource digitalSource) {
+    DMAJNI.addDigitalSource(m_dmaHandle, digitalSource.getPortHandleForRouting());
+  }
+
+  public void addDutyCycle(DutyCycle dutyCycle) {
+    DMAJNI.addDutyCycle(m_dmaHandle, dutyCycle.m_handle);
+  }
+
+  public void addAnalogInput(AnalogInput analogInput) {
+    DMAJNI.addAnalogInput(m_dmaHandle, analogInput.m_port);
+  }
+
+  public void addAveragedAnalogInput(AnalogInput analogInput) {
+    DMAJNI.addAveragedAnalogInput(m_dmaHandle, analogInput.m_port);
+  }
+
+  public void addAnalogAccumulator(AnalogInput analogInput) {
+    DMAJNI.addAnalogAccumulator(m_dmaHandle, analogInput.m_port);
+  }
+
+  /**
+   * Sets an external DMA trigger.
+   *
+   * @param source the source to trigger from.
+   * @param rising trigger on rising edge.
+   * @param falling trigger on falling edge.
+   * @return the index of the trigger
+   */
+  public int setExternalTrigger(DigitalSource source, boolean rising, boolean falling) {
+    return DMAJNI.setExternalTrigger(
+        m_dmaHandle,
+        source.getPortHandleForRouting(),
+        source.getAnalogTriggerTypeForRouting(),
+        rising,
+        falling);
+  }
+
+  public int setPwmEdgeTrigger(PWMMotorController pwm, boolean rising, boolean falling) {
+    return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getPwmHandle(), 0, rising, falling);
+  }
+
+  public int setPwmEdgeTrigger(PWM pwm, boolean rising, boolean falling) {
+    return DMAJNI.setExternalTrigger(m_dmaHandle, pwm.getHandle(), 0, rising, falling);
+  }
+
+  public void clearSensors() {
+    DMAJNI.clearSensors(m_dmaHandle);
+  }
+
+  public void clearExternalTriggers() {
+    DMAJNI.clearExternalTriggers(m_dmaHandle);
+  }
+
+  public void start(int queueDepth) {
+    DMAJNI.startDMA(m_dmaHandle, queueDepth);
+  }
+
+  public void stop() {
+    DMAJNI.stopDMA(m_dmaHandle);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java
new file mode 100644
index 0000000..1055dbc
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMASample.java
@@ -0,0 +1,126 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.AnalogJNI;
+import edu.wpi.first.hal.DMAJNISample;
+
+public class DMASample {
+  public enum DMAReadStatus {
+    kOk(1),
+    kTimeout(2),
+    kError(3);
+
+    private final int value;
+
+    DMAReadStatus(int value) {
+      this.value = value;
+    }
+
+    public int getValue() {
+      return value;
+    }
+
+    /**
+     * Constructs a DMAReadStatus from a raw value.
+     *
+     * @param value raw value
+     * @return enum value
+     */
+    public static DMAReadStatus getValue(int value) {
+      if (value == 1) {
+        return kOk;
+      } else if (value == 2) {
+        return kTimeout;
+      }
+      return kError;
+    }
+  }
+
+  private final DMAJNISample m_dmaSample = new DMAJNISample();
+
+  public DMAReadStatus update(DMA dma, double timeoutSeconds) {
+    return DMAReadStatus.getValue(m_dmaSample.update(dma.m_dmaHandle, timeoutSeconds));
+  }
+
+  public int getCaptureSize() {
+    return m_dmaSample.getCaptureSize();
+  }
+
+  public int getTriggerChannels() {
+    return m_dmaSample.getTriggerChannels();
+  }
+
+  public int getRemaining() {
+    return m_dmaSample.getRemaining();
+  }
+
+  public long getTime() {
+    return m_dmaSample.getTime();
+  }
+
+  public double getTimeStamp() {
+    return getTime() * 1.0e-6;
+  }
+
+  public int getEncoderRaw(Encoder encoder) {
+    return m_dmaSample.getEncoder(encoder.m_encoder);
+  }
+
+  /**
+   * Gets the scaled encoder distance for this sample.
+   *
+   * @param encoder the encoder to use to read
+   * @return the distance
+   */
+  public double getEncoderDistance(Encoder encoder) {
+    double val = getEncoderRaw(encoder);
+    val *= encoder.getDecodingScaleFactor();
+    val *= encoder.getDistancePerPulse();
+    return val;
+  }
+
+  public int getEncoderPeriodRaw(Encoder encoder) {
+    return m_dmaSample.getEncoderPeriod(encoder.m_encoder);
+  }
+
+  public int getCounter(Counter counter) {
+    return m_dmaSample.getCounter(counter.m_counter);
+  }
+
+  public int getCounterPeriod(Counter counter) {
+    return m_dmaSample.getCounterPeriod(counter.m_counter);
+  }
+
+  public boolean getDigitalSource(DigitalSource digitalSource) {
+    return m_dmaSample.getDigitalSource(digitalSource.getPortHandleForRouting());
+  }
+
+  public int getAnalogInputRaw(AnalogInput analogInput) {
+    return m_dmaSample.getAnalogInput(analogInput.m_port);
+  }
+
+  public double getAnalogInputVoltage(AnalogInput analogInput) {
+    return AnalogJNI.getAnalogValueToVolts(analogInput.m_port, getAnalogInputRaw(analogInput));
+  }
+
+  public int getAveragedAnalogInputRaw(AnalogInput analogInput) {
+    return m_dmaSample.getAnalogInputAveraged(analogInput.m_port);
+  }
+
+  public double getAveragedAnalogInputVoltage(AnalogInput analogInput) {
+    return AnalogJNI.getAnalogValueToVolts(
+        analogInput.m_port, getAveragedAnalogInputRaw(analogInput));
+  }
+
+  public int getDutyCycleOutputRaw(DutyCycle dutyCycle) {
+    return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle);
+  }
+
+  public double getDutyCycleOutput(DutyCycle dutyCycle) {
+    return m_dmaSample.getDutyCycleOutput(dutyCycle.m_handle)
+        / (double) dutyCycle.getOutputScaleFactor();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
deleted file mode 100644
index 0feeebb..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DMC60.java
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Digilent DMC 60 Speed Controller.
- *
- * <p>Note that the DMC uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the DMC 60 User Manual
- * available from Digilent.
- *
- * <p><ul>
- * <li>2.004ms = full "forward"
- * <li>1.520ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.480ms = the "low end" of the deadband range
- * <li>0.997ms = full "reverse"
- * </ul>
- */
-public class DMC60 extends PWMSpeedController {
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the DMC60 is attached to. 0-9 are
-   *        on-board, 10-19 are on the MXP port
-   */
-  public DMC60(final int channel) {
-    super(channel);
-
-    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel() + 1);
-    SendableRegistry.setName(this, "DMC60", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
new file mode 100644
index 0000000..016abc7
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DSControlWord.java
@@ -0,0 +1,127 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.ControlWord;
+import edu.wpi.first.hal.HAL;
+
+/** A wrapper around Driver Station control word. */
+public class DSControlWord {
+  private final ControlWord m_controlWord = new ControlWord();
+
+  /**
+   * DSControlWord constructor.
+   *
+   * <p>Upon construction, the current Driver Station control word is read and stored internally.
+   */
+  public DSControlWord() {
+    update();
+  }
+
+  /** Update internal Driver Station control word. */
+  public void update() {
+    HAL.getControlWord(m_controlWord);
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be enabled.
+   *
+   * @return True if the robot is enabled, false otherwise.
+   */
+  public boolean isEnabled() {
+    return m_controlWord.getEnabled() && m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be disabled.
+   *
+   * @return True if the robot should be disabled, false otherwise.
+   */
+  public boolean isDisabled() {
+    return !isEnabled();
+  }
+
+  /**
+   * Gets a value indicating whether the Robot is e-stopped.
+   *
+   * @return True if the robot is e-stopped, false otherwise.
+   */
+  public boolean isEStopped() {
+    return m_controlWord.getEStop();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * autonomous mode.
+   *
+   * @return True if autonomous mode should be enabled, false otherwise.
+   */
+  public boolean isAutonomous() {
+    return m_controlWord.getAutonomous();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * autonomous mode and enabled.
+   *
+   * @return True if autonomous should be set and the robot should be enabled.
+   */
+  public boolean isAutonomousEnabled() {
+    return m_controlWord.getAutonomous()
+        && m_controlWord.getEnabled()
+        && m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controlled mode.
+   *
+   * @return True if operator-controlled mode should be enabled, false otherwise.
+   */
+  public boolean isTeleop() {
+    return !(isAutonomous() || isTest());
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controller mode and enabled.
+   *
+   * @return True if operator-controlled mode should be set and the robot should be enabled.
+   */
+  public boolean isTeleopEnabled() {
+    return !m_controlWord.getAutonomous()
+        && !m_controlWord.getTest()
+        && m_controlWord.getEnabled()
+        && m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in test
+   * mode.
+   *
+   * @return True if test mode should be enabled, false otherwise.
+   */
+  public boolean isTest() {
+    return m_controlWord.getTest();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station is attached.
+   *
+   * @return True if Driver Station is attached, false otherwise.
+   */
+  public boolean isDSAttached() {
+    return m_controlWord.getDSAttached();
+  }
+
+  /**
+   * Gets if the driver station attached to a Field Management System.
+   *
+   * @return true if the robot is competing on a field being controlled by a Field Management System
+   */
+  public boolean isFMSAttached() {
+    return m_controlWord.getFMSAttached();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java
new file mode 100644
index 0000000..cef85aa
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Debouncer.java
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * A simple debounce filter for boolean streams. Requires that the boolean change value from
+ * baseline for a specified period of time before the filtered value changes.
+ */
+public class Debouncer {
+  public enum DebounceType {
+    kRising,
+    kFalling,
+    kBoth
+  }
+
+  private final Timer m_timer = new Timer();
+  private final double m_debounceTime;
+  private final DebounceType m_debounceType;
+  private boolean m_baseline;
+
+  /**
+   * Creates a new Debouncer.
+   *
+   * @param debounceTime The number of seconds the value must change from baseline for the filtered
+   *     value to change.
+   * @param type Which type of state change the debouncing will be performed on.
+   */
+  public Debouncer(double debounceTime, DebounceType type) {
+    m_debounceTime = debounceTime;
+    m_debounceType = type;
+    m_timer.start();
+
+    switch (m_debounceType) {
+      case kBoth: // fall-through
+      case kRising:
+        m_baseline = false;
+        break;
+      case kFalling:
+        m_baseline = true;
+        break;
+      default:
+        throw new IllegalArgumentException("Invalid debounce type!");
+    }
+  }
+
+  /**
+   * Creates a new Debouncer. Baseline value defaulted to "false."
+   *
+   * @param debounceTime The number of seconds the value must change from baseline for the filtered
+   *     value to change.
+   */
+  public Debouncer(double debounceTime) {
+    this(debounceTime, DebounceType.kRising);
+  }
+
+  /**
+   * Applies the debouncer to the input stream.
+   *
+   * @param input The current value of the input stream.
+   * @return The debounced value of the input stream.
+   */
+  public boolean calculate(boolean input) {
+    if (input == m_baseline) {
+      m_timer.reset();
+    }
+
+    if (m_timer.hasElapsed(m_debounceTime)) {
+      if (m_debounceType == DebounceType.kBoth) {
+        m_baseline = input;
+        m_timer.reset();
+      }
+      return input;
+    } else {
+      return m_baseline;
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
index 10155cc..abcc67b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilter.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.concurrent.locks.Lock;
-import java.util.concurrent.locks.ReentrantLock;
-
 import edu.wpi.first.hal.DigitalGlitchFilterJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
 
 /**
  * Class to enable glitch filtering on a set of digital inputs. This class will manage adding and
@@ -22,11 +19,10 @@
  * that an input must remain high or low before it is classified as high or low.
  */
 public class DigitalGlitchFilter implements Sendable, AutoCloseable {
-  /**
-   * Configures the Digital Glitch Filter to its default settings.
-   */
+  /** Configures the Digital Glitch Filter to its default settings. */
   public DigitalGlitchFilter() {
-    synchronized (m_mutex) {
+    m_mutex.lock();
+    try {
       int index = 0;
       while (m_filterAllocated[index] && index < m_filterAllocated.length) {
         index++;
@@ -34,10 +30,11 @@
       if (index != m_filterAllocated.length) {
         m_channelIndex = index;
         m_filterAllocated[index] = true;
-        HAL.report(tResourceType.kResourceType_DigitalGlitchFilter,
-            m_channelIndex + 1, 0);
+        HAL.report(tResourceType.kResourceType_DigitalGlitchFilter, m_channelIndex + 1, 0);
         SendableRegistry.addLW(this, "DigitalGlitchFilter", index);
       }
+    } finally {
+      m_mutex.unlock();
     }
   }
 
@@ -45,9 +42,13 @@
   public void close() {
     SendableRegistry.remove(this);
     if (m_channelIndex >= 0) {
-      synchronized (m_mutex) {
+      m_mutex.lock();
+      try {
         m_filterAllocated[m_channelIndex] = false;
+      } finally {
+        m_mutex.unlock();
       }
+
       m_channelIndex = -1;
     }
   }
@@ -62,8 +63,8 @@
 
       int selected = DigitalGlitchFilterJNI.getFilterSelect(input.getPortHandleForRouting());
       if (selected != channelIndex) {
-        throw new IllegalStateException("DigitalGlitchFilterJNI.setFilterSelect("
-            + channelIndex + ") failed -> " + selected);
+        throw new IllegalStateException(
+            "DigitalGlitchFilterJNI.setFilterSelect(" + channelIndex + ") failed -> " + selected);
       }
     }
   }
@@ -143,8 +144,7 @@
    * @param nanoseconds The number of nanoseconds.
    */
   public void setPeriodNanoSeconds(long nanoseconds) {
-    int fpgaCycles = (int) (nanoseconds * SensorUtil.kSystemClockTicksPerMicrosecond / 4
-        / 1000);
+    int fpgaCycles = (int) (nanoseconds * SensorUtil.kSystemClockTicksPerMicrosecond / 4 / 1000);
     setPeriodCycles(fpgaCycles);
   }
 
@@ -167,14 +167,11 @@
   public long getPeriodNanoSeconds() {
     int fpgaCycles = getPeriodCycles();
 
-    return (long) fpgaCycles * 1000L
-        / (long) (SensorUtil.kSystemClockTicksPerMicrosecond / 4);
+    return (long) fpgaCycles * 1000L / (long) (SensorUtil.kSystemClockTicksPerMicrosecond / 4);
   }
 
   @Override
-  @SuppressWarnings("PMD.UnusedFormalParameter")
-  public void initSendable(SendableBuilder builder) {
-  }
+  public void initSendable(SendableBuilder builder) {}
 
   private int m_channelIndex = -1;
   private static final Lock m_mutex = new ReentrantLock(true);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
index 7cbd1ed..5b4cf5e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalInput.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,8 +8,9 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Class to read a digital input. This class will read digital inputs and return the current value
@@ -20,7 +18,7 @@
  * elsewhere will automatically allocate digital inputs and outputs as required. This class is only
  * for devices like switches etc. that aren't implemented anywhere else.
  */
-public class DigitalInput extends DigitalSource implements Sendable, AutoCloseable {
+public class DigitalInput extends DigitalSource implements Sendable {
   private final int m_channel;
   private int m_handle;
 
@@ -43,9 +41,6 @@
   public void close() {
     super.close();
     SendableRegistry.remove(this);
-    if (m_interrupt != 0) {
-      cancelInterrupts();
-    }
     DIOJNI.freeDIOPort(m_handle);
     m_handle = 0;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
index ec1a44c..2c87df5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalOutput.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,14 +8,15 @@
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Class to write digital outputs. This class will write digital outputs. Other devices that are
  * implemented elsewhere will automatically allocate digital inputs and outputs as required.
  */
-public class DigitalOutput extends DigitalSource implements Sendable, AutoCloseable {
+public class DigitalOutput extends DigitalSource implements Sendable {
   private static final int invalidPwmGenerator = 0;
   private int m_pwmGenerator = invalidPwmGenerator;
 
@@ -26,11 +24,10 @@
   private int m_handle;
 
   /**
-   * Create an instance of a digital output. Create an instance of a digital output given a
-   * channel.
+   * Create an instance of a digital output. Create an instance of a digital output given a channel.
    *
    * @param channel the DIO channel to use for the digital output. 0-9 are on-board, 10-25 are on
-   *                the MXP
+   *     the MXP
    */
   public DigitalOutput(int channel) {
     SensorUtil.checkDigitalChannel(channel);
@@ -60,7 +57,7 @@
    * @param value true is on, off is false
    */
   public void set(boolean value) {
-    DIOJNI.setDIO(m_handle, (short) (value ? 1 : 0));
+    DIOJNI.setDIO(m_handle, value);
   }
 
   /**
@@ -153,8 +150,7 @@
    * Change the duty-cycle that is being generated on the line.
    *
    * <p>The resolution of the duty cycle is 8-bit for low frequencies (1kHz or less) but is reduced
-   * the
-   * higher the frequency of the PWM signal is.
+   * the higher the frequency of the PWM signal is.
    *
    * @param dutyCycle The duty-cycle to change to. [0..1]
    */
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
index d31e191..cb8b024 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DigitalSource.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -13,8 +10,25 @@
  * just provides a channel, then a digital input will be constructed and freed when finished for the
  * source. The source can either be a digital input or analog trigger but not both.
  */
-public abstract class DigitalSource extends InterruptableSensorBase {
+public abstract class DigitalSource implements AutoCloseable {
   public abstract boolean isAnalogTrigger();
 
   public abstract int getChannel();
+
+  /**
+   * If this is an analog trigger.
+   *
+   * @return true if this is an analog trigger.
+   */
+  public abstract int getAnalogTriggerTypeForRouting();
+
+  /**
+   * The channel routing number.
+   *
+   * @return channel routing number
+   */
+  public abstract int getPortHandleForRouting();
+
+  @Override
+  public void close() {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
index d6d4908..68a1bca 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DoubleSolenoid.java
@@ -1,94 +1,118 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.SolenoidJNI;
-import edu.wpi.first.hal.util.UncleanStatusException;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
- * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the PCM.
+ * DoubleSolenoid class for running 2 channels of high voltage Digital Output on the pneumatics
+ * module.
  *
  * <p>The DoubleSolenoid class is typically used for pneumatics solenoids that have two positions
  * controlled by two separate channels.
  */
-public class DoubleSolenoid extends SolenoidBase implements Sendable, AutoCloseable {
-  /**
-   * Possible values for a DoubleSolenoid.
-   */
+public class DoubleSolenoid implements Sendable, AutoCloseable {
+  /** Possible values for a DoubleSolenoid. */
   public enum Value {
     kOff,
     kForward,
     kReverse
   }
 
-  private byte m_forwardMask; // The mask for the forward channel.
-  private byte m_reverseMask; // The mask for the reverse channel.
-  private int m_forwardHandle;
-  private int m_reverseHandle;
+  private final int m_forwardMask; // The mask for the forward channel.
+  private final int m_reverseMask; // The mask for the reverse channel.
+  private final int m_mask; // The channel mask
+  private PneumaticsBase m_module;
+  private final int m_forwardChannel;
+  private final int m_reverseChannel;
 
   /**
-   * Constructor. Uses the default PCM ID (defaults to 0).
+   * Constructs a double solenoid for a default module of a specific module type.
    *
-   * @param forwardChannel The forward channel number on the PCM (0..7).
-   * @param reverseChannel The reverse channel number on the PCM (0..7).
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
    */
-  public DoubleSolenoid(final int forwardChannel, final int reverseChannel) {
-    this(SensorUtil.getDefaultSolenoidModule(), forwardChannel, reverseChannel);
+  public DoubleSolenoid(
+      final PneumaticsModuleType moduleType, final int forwardChannel, final int reverseChannel) {
+    this(PneumaticsBase.getDefaultForType(moduleType), moduleType, forwardChannel, reverseChannel);
   }
 
   /**
-   * Constructor.
+   * Constructs a double solenoid for a specified module of a specific module type.
    *
-   * @param moduleNumber   The module number of the solenoid module to use.
-   * @param forwardChannel The forward channel on the module to control (0..7).
-   * @param reverseChannel The reverse channel on the module to control (0..7).
+   * @param module The module of the solenoid module to use.
+   * @param moduleType The module type to use.
+   * @param forwardChannel The forward channel on the module to control.
+   * @param reverseChannel The reverse channel on the module to control.
    */
-  public DoubleSolenoid(final int moduleNumber, final int forwardChannel,
-                        final int reverseChannel) {
-    super(moduleNumber);
+  public DoubleSolenoid(
+      final int module,
+      final PneumaticsModuleType moduleType,
+      final int forwardChannel,
+      final int reverseChannel) {
+    m_module = PneumaticsBase.getForType(module, moduleType);
+    boolean allocatedSolenoids = false;
+    boolean successfulCompletion = false;
 
-    SensorUtil.checkSolenoidModule(m_moduleNumber);
-    SensorUtil.checkSolenoidChannel(forwardChannel);
-    SensorUtil.checkSolenoidChannel(reverseChannel);
+    m_forwardChannel = forwardChannel;
+    m_reverseChannel = reverseChannel;
 
-    int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) forwardChannel);
-    m_forwardHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+    m_forwardMask = 1 << forwardChannel;
+    m_reverseMask = 1 << reverseChannel;
+    m_mask = m_forwardMask | m_reverseMask;
 
     try {
-      portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) reverseChannel);
-      m_reverseHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
-    } catch (UncleanStatusException ex) {
-      // free the forward handle on exception, then rethrow
-      SolenoidJNI.freeSolenoidPort(m_forwardHandle);
-      m_forwardHandle = 0;
-      m_reverseHandle = 0;
-      throw ex;
+      if (!m_module.checkSolenoidChannel(forwardChannel)) {
+        throw new IllegalArgumentException("Channel " + forwardChannel + " out of range");
+      }
+
+      if (!m_module.checkSolenoidChannel(reverseChannel)) {
+        throw new IllegalArgumentException("Channel " + reverseChannel + " out of range");
+      }
+
+      int allocMask = m_module.checkAndReserveSolenoids(m_mask);
+      if (allocMask != 0) {
+        if (allocMask == m_mask) {
+          throw new AllocationException(
+              "Channels " + forwardChannel + " and " + reverseChannel + " already allocated");
+        } else if (allocMask == m_forwardMask) {
+          throw new AllocationException("Channel " + forwardChannel + " already allocated");
+        } else {
+          throw new AllocationException("Channel " + reverseChannel + " already allocated");
+        }
+      }
+      allocatedSolenoids = true;
+
+      HAL.report(
+          tResourceType.kResourceType_Solenoid, forwardChannel + 1, m_module.getModuleNumber() + 1);
+      HAL.report(
+          tResourceType.kResourceType_Solenoid, reverseChannel + 1, m_module.getModuleNumber() + 1);
+      SendableRegistry.addLW(this, "DoubleSolenoid", m_module.getModuleNumber(), forwardChannel);
+      successfulCompletion = true;
+    } finally {
+      if (!successfulCompletion) {
+        if (allocatedSolenoids) {
+          m_module.unreserveSolenoids(m_mask);
+        }
+        m_module.close();
+      }
     }
-
-    m_forwardMask = (byte) (1 << forwardChannel);
-    m_reverseMask = (byte) (1 << reverseChannel);
-
-    HAL.report(tResourceType.kResourceType_Solenoid, forwardChannel + 1,
-                                   m_moduleNumber + 1);
-    HAL.report(tResourceType.kResourceType_Solenoid, reverseChannel + 1,
-                                   m_moduleNumber + 1);
-    SendableRegistry.addLW(this, "DoubleSolenoid", m_moduleNumber, forwardChannel);
   }
 
   @Override
   public synchronized void close() {
     SendableRegistry.remove(this);
-    SolenoidJNI.freeSolenoidPort(m_forwardHandle);
-    SolenoidJNI.freeSolenoidPort(m_reverseHandle);
+    m_module.unreserveSolenoids(m_mask);
+    m_module.close();
+    m_module = null;
   }
 
   /**
@@ -97,29 +121,23 @@
    * @param value The value to set (Off, Forward, Reverse)
    */
   public void set(final Value value) {
-    boolean forward = false;
-    boolean reverse = false;
+    int setValue;
 
     switch (value) {
       case kOff:
-        forward = false;
-        reverse = false;
+        setValue = 0;
         break;
       case kForward:
-        forward = true;
-        reverse = false;
+        setValue = m_forwardMask;
         break;
       case kReverse:
-        forward = false;
-        reverse = true;
+        setValue = m_reverseMask;
         break;
       default:
         throw new AssertionError("Illegal value: " + value);
-
     }
 
-    SolenoidJNI.setSolenoid(m_forwardHandle, forward);
-    SolenoidJNI.setSolenoid(m_reverseHandle, reverse);
+    m_module.setSolenoids(m_mask, setValue);
   }
 
   /**
@@ -128,12 +146,11 @@
    * @return The current value of the solenoid.
    */
   public Value get() {
-    boolean valueForward = SolenoidJNI.getSolenoid(m_forwardHandle);
-    boolean valueReverse = SolenoidJNI.getSolenoid(m_reverseHandle);
+    int values = m_module.getSolenoids();
 
-    if (valueForward) {
+    if ((values & m_forwardMask) != 0) {
       return Value.kForward;
-    } else if (valueReverse) {
+    } else if ((values & m_reverseMask) != 0) {
       return Value.kReverse;
     } else {
       return Value.kOff;
@@ -157,27 +174,41 @@
   }
 
   /**
-   * Check if the forward solenoid is blacklisted. If a solenoid is shorted, it is added to the
-   * blacklist and disabled until power cycle, or until faults are cleared.
+   * Get the forward channel.
    *
-   * @return If solenoid is disabled due to short.
-   * @see #clearAllPCMStickyFaults()
+   * @return the forward channel.
    */
-  public boolean isFwdSolenoidBlackListed() {
-    int blackList = getPCMSolenoidBlackList();
-    return (blackList & m_forwardMask) != 0;
+  public int getFwdChannel() {
+    return m_forwardChannel;
   }
 
   /**
-   * Check if the reverse solenoid is blacklisted. If a solenoid is shorted, it is added to the
-   * blacklist and disabled until power cycle, or until faults are cleared.
+   * Get the reverse channel.
+   *
+   * @return the reverse channel.
+   */
+  public int getRevChannel() {
+    return m_reverseChannel;
+  }
+
+  /**
+   * Check if the forward solenoid is Disabled. If a solenoid is shorted, it is added to the
+   * DisabledList and disabled until power cycle, or until faults are cleared.
    *
    * @return If solenoid is disabled due to short.
-   * @see #clearAllPCMStickyFaults()
    */
-  public boolean isRevSolenoidBlackListed() {
-    int blackList = getPCMSolenoidBlackList();
-    return (blackList & m_reverseMask) != 0;
+  public boolean isFwdSolenoidDisabled() {
+    return (m_module.getSolenoidDisabledList() & m_forwardMask) != 0;
+  }
+
+  /**
+   * Check if the reverse solenoid is Disabled. If a solenoid is shorted, it is added to the
+   * DisabledList and disabled until power cycle, or until faults are cleared.
+   *
+   * @return If solenoid is disabled due to short.
+   */
+  public boolean isRevSolenoidDisabled() {
+    return (m_module.getSolenoidDisabledList() & m_reverseMask) != 0;
   }
 
   @Override
@@ -185,14 +216,17 @@
     builder.setSmartDashboardType("Double Solenoid");
     builder.setActuator(true);
     builder.setSafeState(() -> set(Value.kOff));
-    builder.addStringProperty("Value", () -> get().name().substring(1), value -> {
-      if ("Forward".equals(value)) {
-        set(Value.kForward);
-      } else if ("Reverse".equals(value)) {
-        set(Value.kReverse);
-      } else {
-        set(Value.kOff);
-      }
-    });
+    builder.addStringProperty(
+        "Value",
+        () -> get().name().substring(1),
+        value -> {
+          if ("Forward".equals(value)) {
+            set(Value.kForward);
+          } else if ("Reverse".equals(value)) {
+            set(Value.kReverse);
+          } else {
+            set(Value.kOff);
+          }
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
index fe3ca50..c450c5e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DriverStation.java
@@ -1,18 +1,9 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.util.concurrent.TimeUnit;
-import java.util.concurrent.locks.Condition;
-import java.util.concurrent.locks.Lock;
-import java.util.concurrent.locks.ReentrantLock;
-
 import edu.wpi.first.hal.AllianceStationID;
 import edu.wpi.first.hal.ControlWord;
 import edu.wpi.first.hal.HAL;
@@ -20,16 +11,15 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
+import java.nio.ByteBuffer;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.locks.Condition;
+import java.util.concurrent.locks.Lock;
+import java.util.concurrent.locks.ReentrantLock;
 
-/**
- * Provide access to the network communication data to / from the Driver Station.
- */
-@SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.ExcessiveClassLength",
-                   "PMD.ExcessivePublicCount", "PMD.GodClass", "PMD.TooManyFields"})
+/** Provide access to the network communication data to / from the Driver Station. */
 public class DriverStation {
-  /**
-   * Number of Joystick Ports.
-   */
+  /** Number of Joystick Ports. */
   public static final int kJoystickPorts = 6;
 
   private static class HALJoystickButtons {
@@ -58,55 +48,87 @@
     }
   }
 
-  /**
-   * The robot alliance that the robot is a part of.
-   */
+  /** The robot alliance that the robot is a part of. */
   public enum Alliance {
-    Red, Blue, Invalid
+    Red,
+    Blue,
+    Invalid
   }
 
   public enum MatchType {
-    None, Practice, Qualification, Elimination
+    None,
+    Practice,
+    Qualification,
+    Elimination
   }
 
   private static final double JOYSTICK_UNPLUGGED_MESSAGE_INTERVAL = 1.0;
-  private double m_nextMessageTime;
+  private static double m_nextMessageTime;
 
   private static class DriverStationTask implements Runnable {
-    private final DriverStation m_ds;
-
-    DriverStationTask(DriverStation ds) {
-      m_ds = ds;
-    }
+    DriverStationTask() {}
 
     @Override
     public void run() {
-      m_ds.run();
+      DriverStation.run();
     }
   } /* DriverStationTask */
 
   private static class MatchDataSender {
     @SuppressWarnings("MemberName")
     NetworkTable table;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry typeMetadata;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry gameSpecificMessage;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry eventName;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry matchNumber;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry replayNumber;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry matchType;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry alliance;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry station;
+
     @SuppressWarnings("MemberName")
     NetworkTableEntry controlWord;
 
+    @SuppressWarnings("MemberName")
+    boolean oldIsRedAlliance = true;
+
+    @SuppressWarnings("MemberName")
+    int oldStationNumber = 1;
+
+    @SuppressWarnings("MemberName")
+    String oldEventName = "";
+
+    @SuppressWarnings("MemberName")
+    String oldGameSpecificMessage = "";
+
+    @SuppressWarnings("MemberName")
+    int oldMatchNumber;
+
+    @SuppressWarnings("MemberName")
+    int oldReplayNumber;
+
+    @SuppressWarnings("MemberName")
+    int oldMatchType;
+
+    @SuppressWarnings("MemberName")
+    int oldControlWord;
+
     MatchDataSender() {
       table = NetworkTableInstance.getDefault().getTable("FMSInfo");
       typeMetadata = table.getEntry(".type");
@@ -128,61 +150,147 @@
       controlWord = table.getEntry("FMSControlData");
       controlWord.forceSetDouble(0);
     }
+
+    private void sendMatchData() {
+      AllianceStationID allianceID = HAL.getAllianceStation();
+      boolean isRedAlliance = false;
+      int stationNumber = 1;
+      switch (allianceID) {
+        case Blue1:
+          isRedAlliance = false;
+          stationNumber = 1;
+          break;
+        case Blue2:
+          isRedAlliance = false;
+          stationNumber = 2;
+          break;
+        case Blue3:
+          isRedAlliance = false;
+          stationNumber = 3;
+          break;
+        case Red1:
+          isRedAlliance = true;
+          stationNumber = 1;
+          break;
+        case Red2:
+          isRedAlliance = true;
+          stationNumber = 2;
+          break;
+        default:
+          isRedAlliance = true;
+          stationNumber = 3;
+          break;
+      }
+
+      String currentEventName;
+      String currentGameSpecificMessage;
+      int currentMatchNumber;
+      int currentReplayNumber;
+      int currentMatchType;
+      int currentControlWord;
+      m_cacheDataMutex.lock();
+      try {
+        currentEventName = DriverStation.m_matchInfo.eventName;
+        currentGameSpecificMessage = DriverStation.m_matchInfo.gameSpecificMessage;
+        currentMatchNumber = DriverStation.m_matchInfo.matchNumber;
+        currentReplayNumber = DriverStation.m_matchInfo.replayNumber;
+        currentMatchType = DriverStation.m_matchInfo.matchType;
+      } finally {
+        m_cacheDataMutex.unlock();
+      }
+      currentControlWord = HAL.nativeGetControlWord();
+
+      if (oldIsRedAlliance != isRedAlliance) {
+        alliance.setBoolean(isRedAlliance);
+        oldIsRedAlliance = isRedAlliance;
+      }
+      if (oldStationNumber != stationNumber) {
+        station.setDouble(stationNumber);
+        oldStationNumber = stationNumber;
+      }
+      if (!oldEventName.equals(currentEventName)) {
+        eventName.setString(currentEventName);
+        oldEventName = currentEventName;
+      }
+      if (!oldGameSpecificMessage.equals(currentGameSpecificMessage)) {
+        gameSpecificMessage.setString(currentGameSpecificMessage);
+        oldGameSpecificMessage = currentGameSpecificMessage;
+      }
+      if (currentMatchNumber != oldMatchNumber) {
+        matchNumber.setDouble(currentMatchNumber);
+        oldMatchNumber = currentMatchNumber;
+      }
+      if (currentReplayNumber != oldReplayNumber) {
+        replayNumber.setDouble(currentReplayNumber);
+        oldReplayNumber = currentReplayNumber;
+      }
+      if (currentMatchType != oldMatchType) {
+        matchType.setDouble(currentMatchType);
+        oldMatchType = currentMatchType;
+      }
+      if (currentControlWord != oldControlWord) {
+        controlWord.setDouble(currentControlWord);
+        oldControlWord = currentControlWord;
+      }
+    }
   }
 
   private static DriverStation instance = new DriverStation();
 
   // Joystick User Data
-  private HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
-  private HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
-  private HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
-  private MatchInfoData m_matchInfo = new MatchInfoData();
+  private static HALJoystickAxes[] m_joystickAxes = new HALJoystickAxes[kJoystickPorts];
+  private static HALJoystickPOVs[] m_joystickPOVs = new HALJoystickPOVs[kJoystickPorts];
+  private static HALJoystickButtons[] m_joystickButtons = new HALJoystickButtons[kJoystickPorts];
+  private static MatchInfoData m_matchInfo = new MatchInfoData();
 
   // Joystick Cached Data
-  private HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
-  private HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
-  private HALJoystickButtons[] m_joystickButtonsCache = new HALJoystickButtons[kJoystickPorts];
-  private MatchInfoData m_matchInfoCache = new MatchInfoData();
+  private static HALJoystickAxes[] m_joystickAxesCache = new HALJoystickAxes[kJoystickPorts];
+  private static HALJoystickPOVs[] m_joystickPOVsCache = new HALJoystickPOVs[kJoystickPorts];
+  private static HALJoystickButtons[] m_joystickButtonsCache =
+      new HALJoystickButtons[kJoystickPorts];
+  private static MatchInfoData m_matchInfoCache = new MatchInfoData();
 
   // Joystick button rising/falling edge flags
-  private int[] m_joystickButtonsPressed = new int[kJoystickPorts];
-  private int[] m_joystickButtonsReleased = new int[kJoystickPorts];
+  private static int[] m_joystickButtonsPressed = new int[kJoystickPorts];
+  private static int[] m_joystickButtonsReleased = new int[kJoystickPorts];
 
   // preallocated byte buffer for button count
-  private final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
+  private static final ByteBuffer m_buttonCountBuffer = ByteBuffer.allocateDirect(1);
 
-  private final MatchDataSender m_matchDataSender;
+  private static final MatchDataSender m_matchDataSender;
 
   // Internal Driver Station thread
-  @SuppressWarnings("PMD.SingularField")
-  private final Thread m_thread;
-  private volatile boolean m_threadKeepAlive = true;
+  private static Thread m_thread;
 
-  private final ReentrantLock m_cacheDataMutex = new ReentrantLock();
+  private static volatile boolean m_threadKeepAlive = true;
 
-  private final Lock m_waitForDataMutex;
-  private final Condition m_waitForDataCond;
-  private int m_waitForDataCount;
-  private final ThreadLocal<Integer> m_lastCount = ThreadLocal.withInitial(() -> 0);
+  private static final ReentrantLock m_cacheDataMutex = new ReentrantLock();
 
-  private boolean m_silenceJoystickWarning;
+  private static final Lock m_waitForDataMutex;
+  private static final Condition m_waitForDataCond;
+  private static int m_waitForDataCount;
+  private static final ThreadLocal<Integer> m_lastCount = ThreadLocal.withInitial(() -> 0);
+
+  private static boolean m_silenceJoystickWarning;
 
   // Robot state status variables
-  private boolean m_userInDisabled;
-  private boolean m_userInAutonomous;
-  private boolean m_userInTeleop;
-  private boolean m_userInTest;
+  private static boolean m_userInDisabled;
+  private static boolean m_userInAutonomous;
+  private static boolean m_userInTeleop;
+  private static boolean m_userInTest;
 
   // Control word variables
-  private final Object m_controlWordMutex;
-  private final ControlWord m_controlWordCache;
-  private long m_lastControlWordUpdate;
+  private static final Object m_controlWordMutex;
+  private static final ControlWord m_controlWordCache;
+  private static long m_lastControlWordUpdate;
 
   /**
    * Gets an instance of the DriverStation.
    *
    * @return The DriverStation.
+   * @deprecated Use the static methods
    */
+  @Deprecated
   public static DriverStation getInstance() {
     return DriverStation.instance;
   }
@@ -193,8 +301,9 @@
    * <p>The single DriverStation instance is created statically with the instance static member
    * variable.
    */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  private DriverStation() {
+  private DriverStation() {}
+
+  static {
     HAL.initialize(500, 0);
     m_waitForDataCount = 0;
     m_waitForDataMutex = new ReentrantLock();
@@ -216,23 +325,29 @@
 
     m_matchDataSender = new MatchDataSender();
 
-    m_thread = new Thread(new DriverStationTask(this), "FRCDriverStation");
+    m_thread = new Thread(new DriverStationTask(), "FRCDriverStation");
     m_thread.setPriority((Thread.NORM_PRIORITY + Thread.MAX_PRIORITY) / 2);
 
     m_thread.start();
   }
 
-  /**
-   * Kill the thread.
-   */
-  public void release() {
+  /** Kill the thread. */
+  public static synchronized void release() {
     m_threadKeepAlive = false;
+    if (m_thread != null) {
+      try {
+        m_thread.join();
+      } catch (InterruptedException ex) {
+        Thread.currentThread().interrupt();
+      }
+      m_thread = null;
+    }
   }
 
   /**
-   * Report error to Driver Station. Optionally appends Stack trace
-   * to error message.
+   * Report error to Driver Station. Optionally appends Stack trace to error message.
    *
+   * @param error The error to report.
    * @param printTrace If true, append stack trace to error string
    */
   public static void reportError(String error, boolean printTrace) {
@@ -240,9 +355,9 @@
   }
 
   /**
-   * Report error to Driver Station. Appends provided stack trace
-   * to error message.
+   * Report error to Driver Station. Appends provided stack trace to error message.
    *
+   * @param error The error to report.
    * @param stackTrace The stack trace to append
    */
   public static void reportError(String error, StackTraceElement[] stackTrace) {
@@ -250,37 +365,41 @@
   }
 
   /**
-   * Report warning to Driver Station. Optionally appends Stack
-   * trace to warning message.
+   * Report warning to Driver Station. Optionally appends Stack trace to warning message.
    *
+   * @param warning The warning to report.
    * @param printTrace If true, append stack trace to warning string
    */
-  public static void reportWarning(String error, boolean printTrace) {
-    reportErrorImpl(false, 1, error, printTrace);
+  public static void reportWarning(String warning, boolean printTrace) {
+    reportErrorImpl(false, 1, warning, printTrace);
   }
 
   /**
-   * Report warning to Driver Station. Appends provided stack
-   * trace to warning message.
+   * Report warning to Driver Station. Appends provided stack trace to warning message.
    *
+   * @param warning The warning to report.
    * @param stackTrace The stack trace to append
    */
-  public static void reportWarning(String error, StackTraceElement[] stackTrace) {
-    reportErrorImpl(false, 1, error, stackTrace);
+  public static void reportWarning(String warning, StackTraceElement[] stackTrace) {
+    reportErrorImpl(false, 1, warning, stackTrace);
   }
 
-  private static void reportErrorImpl(boolean isError, int code, String error, boolean
-      printTrace) {
+  private static void reportErrorImpl(boolean isError, int code, String error, boolean printTrace) {
     reportErrorImpl(isError, code, error, printTrace, Thread.currentThread().getStackTrace(), 3);
   }
 
-  private static void reportErrorImpl(boolean isError, int code, String error,
-      StackTraceElement[] stackTrace) {
+  private static void reportErrorImpl(
+      boolean isError, int code, String error, StackTraceElement[] stackTrace) {
     reportErrorImpl(isError, code, error, true, stackTrace, 0);
   }
 
-  private static void reportErrorImpl(boolean isError, int code, String error,
-      boolean printTrace, StackTraceElement[] stackTrace, int stackTraceFirst) {
+  private static void reportErrorImpl(
+      boolean isError,
+      int code,
+      String error,
+      boolean printTrace,
+      StackTraceElement[] stackTrace,
+      int stackTraceFirst) {
     String locString;
     if (stackTrace.length >= stackTraceFirst + 1) {
       locString = stackTrace[stackTraceFirst].toString();
@@ -306,11 +425,11 @@
   /**
    * The state of one joystick button. Button indexes begin at 1.
    *
-   * @param stick  The joystick to read.
+   * @param stick The joystick to read.
    * @param button The button index, beginning at 1.
    * @return The state of the joystick button.
    */
-  public boolean getStickButton(final int stick, final int button) {
+  public static boolean getStickButton(final int stick, final int button) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
     }
@@ -328,19 +447,23 @@
       m_cacheDataMutex.unlock();
     }
 
-    reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
-        + " not available, check if controller is plugged in");
+    reportJoystickUnpluggedWarning(
+        "Joystick Button "
+            + button
+            + " on port "
+            + stick
+            + " not available, check if controller is plugged in");
     return false;
   }
 
   /**
    * Whether one joystick button was pressed since the last check. Button indexes begin at 1.
    *
-   * @param stick  The joystick to read.
+   * @param stick The joystick to read.
    * @param button The button index, beginning at 1.
    * @return Whether the joystick button was pressed since the last check.
    */
-  public boolean getStickButtonPressed(final int stick, final int button) {
+  public static boolean getStickButtonPressed(final int stick, final int button) {
     if (button <= 0) {
       reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
       return false;
@@ -364,20 +487,23 @@
       m_cacheDataMutex.unlock();
     }
 
-    reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
-        + " not available, check if controller is plugged in");
+    reportJoystickUnpluggedWarning(
+        "Joystick Button "
+            + button
+            + " on port "
+            + stick
+            + " not available, check if controller is plugged in");
     return false;
   }
 
   /**
-   * Whether one joystick button was released since the last check. Button indexes
-   * begin at 1.
+   * Whether one joystick button was released since the last check. Button indexes begin at 1.
    *
-   * @param stick  The joystick to read.
+   * @param stick The joystick to read.
    * @param button The button index, beginning at 1.
    * @return Whether the joystick button was released since the last check.
    */
-  public boolean getStickButtonReleased(final int stick, final int button) {
+  public static boolean getStickButtonReleased(final int stick, final int button) {
     if (button <= 0) {
       reportJoystickUnpluggedError("Button indexes begin at 1 in WPILib for C++ and Java\n");
       return false;
@@ -401,8 +527,12 @@
       m_cacheDataMutex.unlock();
     }
 
-    reportJoystickUnpluggedWarning("Joystick Button " + button + " on port " + stick
-        + " not available, check if controller is plugged in");
+    reportJoystickUnpluggedWarning(
+        "Joystick Button "
+            + button
+            + " on port "
+            + stick
+            + " not available, check if controller is plugged in");
     return false;
   }
 
@@ -411,10 +541,10 @@
    * to the specified port.
    *
    * @param stick The joystick to read.
-   * @param axis  The analog axis value to read from the joystick.
+   * @param axis The analog axis value to read from the joystick.
    * @return The value of the axis on the joystick.
    */
-  public double getStickAxis(int stick, int axis) {
+  public static double getStickAxis(int stick, int axis) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -431,17 +561,23 @@
       m_cacheDataMutex.unlock();
     }
 
-    reportJoystickUnpluggedWarning("Joystick axis " + axis + " on port " + stick
-        + " not available, check if controller is plugged in");
+    reportJoystickUnpluggedWarning(
+        "Joystick axis "
+            + axis
+            + " on port "
+            + stick
+            + " not available, check if controller is plugged in");
     return 0.0;
   }
 
   /**
    * Get the state of a POV on the joystick.
    *
+   * @param stick The joystick to read.
+   * @param pov The POV to read.
    * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
    */
-  public int getStickPOV(int stick, int pov) {
+  public static int getStickPOV(int stick, int pov) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -458,8 +594,12 @@
       m_cacheDataMutex.unlock();
     }
 
-    reportJoystickUnpluggedWarning("Joystick POV " + pov + " on port " + stick
-        + " not available, check if controller is plugged in");
+    reportJoystickUnpluggedWarning(
+        "Joystick POV "
+            + pov
+            + " on port "
+            + stick
+            + " not available, check if controller is plugged in");
     return -1;
   }
 
@@ -469,7 +609,7 @@
    * @param stick The joystick to read.
    * @return The state of the buttons on the joystick.
    */
-  public int getStickButtons(final int stick) {
+  public static int getStickButtons(final int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-3");
     }
@@ -488,7 +628,7 @@
    * @param stick The joystick port number
    * @return The number of axes on the indicated joystick
    */
-  public int getStickAxisCount(int stick) {
+  public static int getStickAxisCount(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -507,7 +647,7 @@
    * @param stick The joystick port number
    * @return The number of POVs on the indicated joystick
    */
-  public int getStickPOVCount(int stick) {
+  public static int getStickPOVCount(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -526,7 +666,7 @@
    * @param stick The joystick port number
    * @return The number of buttons on the indicated joystick
    */
-  public int getStickButtonCount(int stick) {
+  public static int getStickButtonCount(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -545,7 +685,7 @@
    * @param stick The joystick port number
    * @return A boolean that returns the value of isXbox
    */
-  public boolean getJoystickIsXbox(int stick) {
+  public static boolean getJoystickIsXbox(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -559,7 +699,7 @@
    * @param stick The joystick port number
    * @return The value of type
    */
-  public int getJoystickType(int stick) {
+  public static int getJoystickType(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -573,7 +713,7 @@
    * @param stick The joystick port number
    * @return The value of name
    */
-  public String getJoystickName(int stick) {
+  public static String getJoystickName(int stick) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -588,7 +728,7 @@
    * @param axis The target axis
    * @return What type of axis the axis is reporting to be
    */
-  public int getJoystickAxisType(int stick, int axis) {
+  public static int getJoystickAxisType(int stick, int axis) {
     if (stick < 0 || stick >= kJoystickPorts) {
       throw new IllegalArgumentException("Joystick index is out of range, should be 0-5");
     }
@@ -599,16 +739,16 @@
   /**
    * Returns if a joystick is connected to the Driver Station.
    *
-   * <p>This makes a best effort guess by looking at the reported number of axis,
-   * buttons, and POVs attached.
+   * <p>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
    */
-  public boolean isJoystickConnected(int stick) {
+  public static boolean isJoystickConnected(int stick) {
     return getStickAxisCount(stick) > 0
-      || getStickButtonCount(stick) > 0
-      || getStickPOVCount(stick) > 0;
+        || getStickButtonCount(stick) > 0
+        || getStickPOVCount(stick) > 0;
   }
 
   /**
@@ -616,7 +756,7 @@
    *
    * @return True if the robot is enabled, false otherwise.
    */
-  public boolean isEnabled() {
+  public static boolean isEnabled() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getEnabled() && m_controlWordCache.getDSAttached();
@@ -628,7 +768,7 @@
    *
    * @return True if the robot should be disabled, false otherwise.
    */
-  public boolean isDisabled() {
+  public static boolean isDisabled() {
     return !isEnabled();
   }
 
@@ -637,7 +777,7 @@
    *
    * @return True if the robot is e-stopped, false otherwise.
    */
-  public boolean isEStopped() {
+  public static boolean isEStopped() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getEStop();
@@ -650,7 +790,7 @@
    *
    * @return True if autonomous mode should be enabled, false otherwise.
    */
-  public boolean isAutonomous() {
+  public static boolean isAutonomous() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getAutonomous();
@@ -663,7 +803,7 @@
    *
    * @return True if autonomous should be set and the robot should be enabled.
    */
-  public boolean isAutonomousEnabled() {
+  public static boolean isAutonomousEnabled() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getAutonomous() && m_controlWordCache.getEnabled();
@@ -675,8 +815,20 @@
    * operator-controlled mode.
    *
    * @return True if operator-controlled mode should be enabled, false otherwise.
+   * @deprecated Use isTeleop() instead.
    */
-  public boolean isOperatorControl() {
+  @Deprecated(since = "2022", forRemoval = true)
+  public static boolean isOperatorControl() {
+    return isTeleop();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controlled mode.
+   *
+   * @return True if operator-controlled mode should be enabled, false otherwise.
+   */
+  public static boolean isTeleop() {
     return !(isAutonomous() || isTest());
   }
 
@@ -685,11 +837,24 @@
    * operator-controller mode and enabled.
    *
    * @return True if operator-controlled mode should be set and the robot should be enabled.
+   * @deprecated Use isTeleopEnabled() instead.
    */
-  public boolean isOperatorControlEnabled() {
+  @Deprecated(since = "2022", forRemoval = true)
+  public static boolean isOperatorControlEnabled() {
+    return isTeleopEnabled();
+  }
+
+  /**
+   * Gets a value indicating whether the Driver Station requires the robot to be running in
+   * operator-controller mode and enabled.
+   *
+   * @return True if operator-controlled mode should be set and the robot should be enabled.
+   */
+  public static boolean isTeleopEnabled() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
-      return !m_controlWordCache.getAutonomous() && !m_controlWordCache.getTest()
+      return !m_controlWordCache.getAutonomous()
+          && !m_controlWordCache.getTest()
           && m_controlWordCache.getEnabled();
     }
   }
@@ -700,7 +865,7 @@
    *
    * @return True if test mode should be enabled, false otherwise.
    */
-  public boolean isTest() {
+  public static boolean isTest() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getTest();
@@ -712,7 +877,7 @@
    *
    * @return True if Driver Station is attached, false otherwise.
    */
-  public boolean isDSAttached() {
+  public static boolean isDSAttached() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getDSAttached();
@@ -725,7 +890,7 @@
    *
    * @return True if the control data has been updated since the last call.
    */
-  public boolean isNewControlData() {
+  public static boolean isNewControlData() {
     m_waitForDataMutex.lock();
     try {
       int currentCount = m_waitForDataCount;
@@ -744,7 +909,7 @@
    *
    * @return true if the robot is competing on a field being controlled by a Field Management System
    */
-  public boolean isFMSAttached() {
+  public static boolean isFMSAttached() {
     synchronized (m_controlWordMutex) {
       updateControlWord(false);
       return m_controlWordCache.getFMSAttached();
@@ -756,7 +921,7 @@
    *
    * @return the game specific message
    */
-  public String getGameSpecificMessage() {
+  public static String getGameSpecificMessage() {
     m_cacheDataMutex.lock();
     try {
       return m_matchInfo.gameSpecificMessage;
@@ -770,7 +935,7 @@
    *
    * @return the event name
    */
-  public String getEventName() {
+  public static String getEventName() {
     m_cacheDataMutex.lock();
     try {
       return m_matchInfo.eventName;
@@ -784,7 +949,7 @@
    *
    * @return the match type
    */
-  public MatchType getMatchType() {
+  public static MatchType getMatchType() {
     int matchType;
     m_cacheDataMutex.lock();
     try {
@@ -809,7 +974,7 @@
    *
    * @return the match number
    */
-  public int getMatchNumber() {
+  public static int getMatchNumber() {
     m_cacheDataMutex.lock();
     try {
       return m_matchInfo.matchNumber;
@@ -823,7 +988,7 @@
    *
    * @return the replay number
    */
-  public int getReplayNumber() {
+  public static int getReplayNumber() {
     m_cacheDataMutex.lock();
     try {
       return m_matchInfo.replayNumber;
@@ -837,7 +1002,7 @@
    *
    * @return the current alliance
    */
-  public Alliance getAlliance() {
+  public static Alliance getAlliance() {
     AllianceStationID allianceStationID = HAL.getAllianceStation();
     if (allianceStationID == null) {
       return Alliance.Invalid;
@@ -864,7 +1029,7 @@
    *
    * @return the location of the team's driver station controls: 1, 2, or 3
    */
-  public int getLocation() {
+  public static int getLocation() {
     AllianceStationID allianceStationID = HAL.getAllianceStation();
     if (allianceStationID == null) {
       return 0;
@@ -890,10 +1055,10 @@
   /**
    * Wait for new data from the driver station.
    *
-   * <p>Checks if new control data has arrived since the last waitForData call
-   * on the current thread. If new data has not arrived, returns immediately.
+   * <p>Checks if new control data has arrived since the last waitForData call on the current
+   * thread. If new data has not arrived, returns immediately.
    */
-  public void waitForData() {
+  public static void waitForData() {
     waitForData(0);
   }
 
@@ -901,15 +1066,15 @@
    * Wait for new data or for timeout, which ever comes first. If timeout is 0, wait for new data
    * only.
    *
-   * <p>Checks if new control data has arrived since the last waitForData call
-   * on the current thread. If new data has not arrived, returns immediately.
+   * <p>Checks if new control data has arrived since the last waitForData call on the current
+   * thread. If new data has not arrived, returns immediately.
    *
-   * @param timeout The maximum time in seconds to wait.
+   * @param timeoutSeconds The maximum time in seconds to wait.
    * @return true if there is new data, otherwise false
    */
-  public boolean waitForData(double timeout) {
-    long startTime = RobotController.getFPGATime();
-    long timeoutMicros = (long) (timeout * 1000000);
+  public static boolean waitForData(double timeoutSeconds) {
+    long startTimeMicroS = RobotController.getFPGATime();
+    long timeoutMicroS = (long) (timeoutSeconds * 1e6);
     m_waitForDataMutex.lock();
     try {
       int currentCount = m_waitForDataCount;
@@ -918,12 +1083,13 @@
         return true;
       }
       while (m_waitForDataCount == currentCount) {
-        if (timeout > 0) {
-          long now = RobotController.getFPGATime();
-          if (now < startTime + timeoutMicros) {
+        if (timeoutMicroS > 0) {
+          long nowMicroS = RobotController.getFPGATime();
+          if (nowMicroS < startTimeMicroS + timeoutMicroS) {
             // We still have time to wait
-            boolean signaled = m_waitForDataCond.await(startTime + timeoutMicros - now,
-                                                TimeUnit.MICROSECONDS);
+            boolean signaled =
+                m_waitForDataCond.await(
+                    startTimeMicroS + timeoutMicroS - nowMicroS, TimeUnit.MICROSECONDS);
             if (!signaled) {
               // Return false if a timeout happened
               return false;
@@ -951,12 +1117,12 @@
    * Return the approximate match time. The FMS does not send an official match time to the robots,
    * but does send an approximate match time. The value will count down the time remaining in the
    * current period (auto or teleop). 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 behavior seen on the field.
+   * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice
+   * Match function of the DS approximates the behavior seen on the field.
    *
    * @return Time remaining in current match period (auto or teleop) in seconds
    */
-  public double getMatchTime() {
+  public static double getMatchTime() {
     return HAL.getMatchTime();
   }
 
@@ -966,8 +1132,7 @@
    *
    * @param entering If true, starting disabled code; if false, leaving disabled code
    */
-  @SuppressWarnings("MethodName")
-  public void InDisabled(boolean entering) {
+  public static void inDisabled(boolean entering) {
     m_userInDisabled = entering;
   }
 
@@ -977,8 +1142,7 @@
    *
    * @param entering If true, starting autonomous code; if false, leaving autonomous code
    */
-  @SuppressWarnings("MethodName")
-  public void InAutonomous(boolean entering) {
+  public static void inAutonomous(boolean entering) {
     m_userInAutonomous = entering;
   }
 
@@ -987,9 +1151,20 @@
    * purposes only.
    *
    * @param entering If true, starting teleop code; if false, leaving teleop code
+   * @deprecated Use {@link #inTeleop(boolean)} instead.
    */
-  @SuppressWarnings("MethodName")
-  public void InOperatorControl(boolean entering) {
+  @Deprecated(since = "2022", forRemoval = true)
+  public static void inOperatorControl(boolean entering) {
+    m_userInTeleop = entering;
+  }
+
+  /**
+   * Only to be used to tell the Driver Station what code you claim to be executing for diagnostic
+   * purposes only.
+   *
+   * @param entering If true, starting teleop code; if false, leaving teleop code
+   */
+  public static void inTeleop(boolean entering) {
     m_userInTeleop = entering;
   }
 
@@ -999,94 +1174,39 @@
    *
    * @param entering If true, starting test code; if false, leaving test code
    */
-  @SuppressWarnings("MethodName")
-  public void InTest(boolean entering) {
+  public static void inTest(boolean entering) {
     m_userInTest = entering;
   }
 
-  private void sendMatchData() {
-    AllianceStationID alliance = HAL.getAllianceStation();
-    boolean isRedAlliance = false;
-    int stationNumber = 1;
-    switch (alliance) {
-      case Blue1:
-        isRedAlliance = false;
-        stationNumber = 1;
-        break;
-      case Blue2:
-        isRedAlliance = false;
-        stationNumber = 2;
-        break;
-      case Blue3:
-        isRedAlliance = false;
-        stationNumber = 3;
-        break;
-      case Red1:
-        isRedAlliance = true;
-        stationNumber = 1;
-        break;
-      case Red2:
-        isRedAlliance = true;
-        stationNumber = 2;
-        break;
-      default:
-        isRedAlliance = true;
-        stationNumber = 3;
-        break;
-    }
-
-
-    String eventName;
-    String gameSpecificMessage;
-    int matchNumber;
-    int replayNumber;
-    int matchType;
-    synchronized (m_cacheDataMutex) {
-      eventName = m_matchInfo.eventName;
-      gameSpecificMessage = m_matchInfo.gameSpecificMessage;
-      matchNumber = m_matchInfo.matchNumber;
-      replayNumber = m_matchInfo.replayNumber;
-      matchType = m_matchInfo.matchType;
-    }
-
-    m_matchDataSender.alliance.setBoolean(isRedAlliance);
-    m_matchDataSender.station.setDouble(stationNumber);
-    m_matchDataSender.eventName.setString(eventName);
-    m_matchDataSender.gameSpecificMessage.setString(gameSpecificMessage);
-    m_matchDataSender.matchNumber.setDouble(matchNumber);
-    m_matchDataSender.replayNumber.setDouble(replayNumber);
-    m_matchDataSender.matchType.setDouble(matchType);
-    m_matchDataSender.controlWord.setDouble(HAL.nativeGetControlWord());
-  }
-
-  /**
-   * Forces waitForData() to return immediately.
-   */
-  public void wakeupWaitForData() {
+  /** Forces waitForData() to return immediately. */
+  public static void wakeupWaitForData() {
     m_waitForDataMutex.lock();
-    m_waitForDataCount++;
-    m_waitForDataCond.signalAll();
-    m_waitForDataMutex.unlock();
+    try {
+      m_waitForDataCount++;
+      m_waitForDataCond.signalAll();
+    } finally {
+      m_waitForDataMutex.unlock();
+    }
   }
 
   /**
-   * 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.
+   * 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.
    */
-  public void silenceJoystickConnectionWarning(boolean silence) {
+  public static void silenceJoystickConnectionWarning(boolean silence) {
     m_silenceJoystickWarning = silence;
   }
 
   /**
-   * Returns whether joystick connection warnings are silenced. This will
-   * always return false when connected to the FMS.
+   * Returns whether joystick connection warnings are silenced. This will always return false when
+   * connected to the FMS.
    *
    * @return Whether joystick connection warnings are silenced.
    */
-  public boolean isJoystickConnectionWarningSilenced() {
+  public static boolean isJoystickConnectionWarningSilenced() {
     return !isFMSAttached() && m_silenceJoystickWarning;
   }
 
@@ -1094,7 +1214,7 @@
    * Copy data from the DS task for the user. If no new data exists, it will just be returned,
    * otherwise the data will be copied from the DS polling loop.
    */
-  protected void getData() {
+  protected static void getData() {
     // Get the status of all of the joysticks
     for (byte stick = 0; stick < kJoystickPorts; stick++) {
       m_joystickAxesCache[stick].m_count =
@@ -1144,14 +1264,14 @@
     }
 
     wakeupWaitForData();
-    sendMatchData();
+    m_matchDataSender.sendMatchData();
   }
 
   /**
    * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
    * the DS.
    */
-  private void reportJoystickUnpluggedError(String message) {
+  private static void reportJoystickUnpluggedError(String message) {
     double currentTime = Timer.getFPGATimestamp();
     if (currentTime > m_nextMessageTime) {
       reportError(message, false);
@@ -1163,7 +1283,7 @@
    * Reports errors related to unplugged joysticks Throttles the errors so that they don't overwhelm
    * the DS.
    */
-  private void reportJoystickUnpluggedWarning(String message) {
+  private static void reportJoystickUnpluggedWarning(String message) {
     if (isFMSAttached() || !m_silenceJoystickWarning) {
       double currentTime = Timer.getFPGATimestamp();
       if (currentTime > m_nextMessageTime) {
@@ -1173,10 +1293,8 @@
     }
   }
 
-  /**
-   * Provides the service routine for the DS polling m_thread.
-   */
-  private void run() {
+  /** Provides the service routine for the DS polling m_thread. */
+  private static void run() {
     int safetyCounter = 0;
     while (m_threadKeepAlive) {
       HAL.waitForDSData();
@@ -1207,12 +1325,12 @@
   }
 
   /**
-   * Updates the data in the control word cache. Updates if the force parameter is set, or if
-   * 50ms have passed since the last update.
+   * Updates the data in the control word cache. Updates if the force parameter is set, or if 50ms
+   * have passed since the last update.
    *
    * @param force True to force an update to the cache, otherwise update if 50ms have passed.
    */
-  private void updateControlWord(boolean force) {
+  private static void updateControlWord(boolean force) {
     long now = System.currentTimeMillis();
     synchronized (m_controlWordMutex) {
       if (now - m_lastControlWordUpdate > 50 || force) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
index 0834fad..3332f49 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycle.java
@@ -1,28 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.DutyCycleJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Class to read a duty cycle PWM input.
  *
- * <p>PWM input signals are specified with a frequency and a ratio of high to low
- * in that frequency. There are 8 of these in the roboRIO, and they can be
- * attached to any {@link DigitalSource}.
+ * <p>PWM input signals are specified with a frequency and a ratio of high to low in that frequency.
+ * There are 8 of these in the roboRIO, and they can be attached to any {@link DigitalSource}.
  *
- * <p>These can be combined as the input of an AnalogTrigger to a Counter in order
- * to implement rollover checking.
- *
+ * <p>These can be combined as the input of an AnalogTrigger to a Counter in order to implement
+ * rollover checking.
  */
 public class DutyCycle implements Sendable, AutoCloseable {
   // Explicitly package private
@@ -38,8 +34,10 @@
    * @param digitalSource The DigitalSource to use.
    */
   public DutyCycle(DigitalSource digitalSource) {
-    m_handle = DutyCycleJNI.initialize(digitalSource.getPortHandleForRouting(),
-        digitalSource.getAnalogTriggerTypeForRouting());
+    m_handle =
+        DutyCycleJNI.initialize(
+            digitalSource.getPortHandleForRouting(),
+            digitalSource.getAnalogTriggerTypeForRouting());
 
     m_source = digitalSource;
     int index = getFPGAIndex();
@@ -47,9 +45,7 @@
     SendableRegistry.addLW(this, "Duty Cycle", index);
   }
 
-  /**
-   * Close the DutyCycle and free all resources.
-   */
+  /** Close the DutyCycle and free all resources. */
   @Override
   public void close() {
     DutyCycleJNI.free(m_handle);
@@ -78,8 +74,7 @@
   /**
    * Get the raw output ratio of the duty cycle signal.
    *
-   * <p>0 means always low, an output equal to getOutputScaleFactor() means always
-   * high.
+   * <p>0 means always low, an output equal to getOutputScaleFactor() means always high.
    *
    * @return output ratio in raw units
    */
@@ -90,9 +85,8 @@
   /**
    * Get the scale factor of the output.
    *
-   * <p>An output equal to this value is always high, and then linearly scales down
-   * to 0. Divide the result of getOutputRaw by this in order to get the
-   * percentage between 0 and 1.
+   * <p>An output equal to this value is always high, and then linearly scales down to 0. Divide the
+   * result of getOutputRaw by this in order to get the percentage between 0 and 1.
    *
    * @return the output scale factor
    */
@@ -119,6 +113,5 @@
     builder.setSmartDashboardType("Duty Cycle");
     builder.addDoubleProperty("Frequency", this::getFrequency, null);
     builder.addDoubleProperty("Output", this::getOutput, null);
-
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
index f5bba5e..ce5b684 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/DutyCycleEncoder.java
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
 
 /**
- * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with
- * PWM Output, the CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag
- * Encoder.
+ * Class for supporting duty cycle/PWM encoders, such as the US Digital MA3 with PWM Output, the
+ * CTRE Mag Encoder, the Rev Hex Encoder, and the AM Mag Encoder.
  */
 public class DutyCycleEncoder implements Sendable, AutoCloseable {
   private final DutyCycle m_dutyCycle;
@@ -69,12 +66,13 @@
   }
 
   private void init() {
-    m_simDevice = SimDevice.create("DutyCycleEncoder", m_dutyCycle.getFPGAIndex());
+    m_simDevice = SimDevice.create("DutyCycle:DutyCycleEncoder", m_dutyCycle.getSourceChannel());
 
     if (m_simDevice != null) {
-      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);
+      m_simPosition = m_simDevice.createDouble("position", SimDevice.Direction.kInput, 0.0);
+      m_simDistancePerRotation =
+          m_simDevice.createDouble("distance_per_rot", SimDevice.Direction.kOutput, 1.0);
+      m_simIsConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
     } else {
       m_counter = new Counter();
       m_analogTrigger = new AnalogTrigger(m_dutyCycle);
@@ -121,9 +119,9 @@
   /**
    * Get the offset of position relative to the last reset.
    *
-   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute
-   * position relative to the last reset. This could potentially be negative,
-   * which needs to be accounted for.
+   * <p>getPositionInRotation() - getPositionOffset() will give an encoder absolute position
+   * relative to the last reset. This could potentially be negative, which needs to be accounted
+   * for.
    *
    * @return the position offset
    */
@@ -132,35 +130,29 @@
   }
 
   /**
-   * Set the distance per rotation of the encoder. This sets the multiplier used
-   * to determine the distance driven based on the rotation value from the
-   * encoder. Set this value based on the how far the mechanism travels in 1
-   * rotation of the encoder, and factor in gearing reductions following the
-   * encoder shaft. This distance can be in any units you like, linear or angular.
+   * Set the distance per rotation of the encoder. This sets the multiplier used to determine the
+   * distance driven based on the rotation value from the encoder. Set this value based on the how
+   * far the mechanism travels in 1 rotation of the encoder, and factor in gearing reductions
+   * following the encoder shaft. This distance can be in any units you like, linear or angular.
    *
    * @param distancePerRotation the distance per rotation of the encoder
    */
   public void setDistancePerRotation(double distancePerRotation) {
     m_distancePerRotation = distancePerRotation;
-
-    if (m_simDistancePerRotation != null) {
-      m_simDistancePerRotation.set(distancePerRotation);
-    }
   }
 
   /**
    * Get the distance per rotation for this encoder.
    *
-   * @return The scale factor that will be used to convert rotation to useful
-   *         units.
+   * @return The scale factor that will be used to convert rotation to useful units.
    */
   public double getDistancePerRotation() {
     return m_distancePerRotation;
   }
 
   /**
-   * Get the distance the sensor has driven since the last reset as scaled by the
-   * value from {@link #setDistancePerRotation(double)}.
+   * Get the distance the sensor has driven since the last reset as scaled by the value from {@link
+   * #setDistancePerRotation(double)}.
    *
    * @return The distance driven since the last reset
    */
@@ -177,9 +169,7 @@
     return m_dutyCycle.getFrequency();
   }
 
-  /**
-   * Reset the Encoder distance to zero.
-   */
+  /** Reset the Encoder distance to zero. */
   public void reset() {
     if (m_counter != null) {
       m_counter.reset();
@@ -190,9 +180,9 @@
   /**
    * Get if the sensor is connected
    *
-   * <p>This uses the duty cycle frequency to determine if the sensor is connected.
-   * By default, a value of 100 Hz is used as the threshold, and this value can be
-   * changed with {@link #setConnectedFrequencyThreshold(int)}.
+   * <p>This uses the duty cycle frequency to determine if the sensor is connected. By default, a
+   * value of 100 Hz is used as the threshold, and this value can be changed with {@link
+   * #setConnectedFrequencyThreshold(int)}.
    *
    * @return true if the sensor is connected
    */
@@ -204,8 +194,7 @@
   }
 
   /**
-   * Change the frequency threshold for detecting connection used by
-   * {@link #isConnected()}.
+   * Change the frequency threshold for detecting connection used by {@link #isConnected()}.
    *
    * @param frequency the minimum frequency in Hz.
    */
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
index 125f769..200d713 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Encoder.java
@@ -1,21 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
 import edu.wpi.first.hal.EncoderJNI;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimDevice;
 import edu.wpi.first.hal.util.AllocationException;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Class to read quadrature encoders.
@@ -30,11 +28,13 @@
  * <p>All encoders will immediately start counting - reset() them if you need them to be zeroed
  * before use.
  */
-public class Encoder implements CounterBase, PIDSource, Sendable, AutoCloseable {
+public class Encoder implements CounterBase, Sendable, AutoCloseable {
   public enum IndexingType {
-    kResetWhileHigh(0), kResetWhileLow(1), kResetOnFallingEdge(2), kResetOnRisingEdge(3);
+    kResetWhileHigh(0),
+    kResetWhileLow(1),
+    kResetOnFallingEdge(2),
+    kResetOnRisingEdge(3);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     IndexingType(int value) {
@@ -42,27 +42,19 @@
     }
   }
 
-  /**
-   * The a source.
-   */
-  @SuppressWarnings("MemberName")
+  /** The a source. */
   protected DigitalSource m_aSource; // the A phase of the quad encoder
-  /**
-   * The b source.
-   */
-  @SuppressWarnings("MemberName")
+  /** The b source. */
   protected DigitalSource m_bSource; // the B phase of the quad encoder
-  /**
-   * The index source.
-   */
+  /** The index source. */
   protected DigitalSource m_indexSource; // Index on some encoders
+
   private boolean m_allocatedA;
   private boolean m_allocatedB;
   private boolean m_allocatedI;
-  private PIDSourceType m_pidSource;
+  private final EncodingType m_encodingType;
 
-  private int m_encoder; // the HAL encoder object
-
+  int m_encoder; // the HAL encoder object
 
   /**
    * Common initialization code for Encoders. This code allocates resources for Encoders and is
@@ -73,11 +65,14 @@
    * @param reverseDirection If true, counts down instead of up (this is all relative)
    */
   private void initEncoder(boolean reverseDirection, final EncodingType type) {
-    m_encoder = EncoderJNI.initializeEncoder(m_aSource.getPortHandleForRouting(),
-        m_aSource.getAnalogTriggerTypeForRouting(), m_bSource.getPortHandleForRouting(),
-        m_bSource.getAnalogTriggerTypeForRouting(), reverseDirection, type.value);
-
-    m_pidSource = PIDSourceType.kDisplacement;
+    m_encoder =
+        EncoderJNI.initializeEncoder(
+            m_aSource.getPortHandleForRouting(),
+            m_aSource.getAnalogTriggerTypeForRouting(),
+            m_bSource.getPortHandleForRouting(),
+            m_bSource.getAnalogTriggerTypeForRouting(),
+            reverseDirection,
+            type.value);
 
     int fpgaIndex = getFPGAIndex();
     HAL.report(tResourceType.kResourceType_Encoder, fpgaIndex + 1, type.value + 1);
@@ -89,10 +84,10 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param channelA         The a channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
-   * @param channelB         The b channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+   * @param channelA The a channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
+   * @param channelB The b channel DIO channel. 0-9 are on-board, 10-25 are on the MXP port
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
-   *                         if necessary so forward represents positive values.
+   *     if necessary so forward represents positive values.
    */
   public Encoder(final int channelA, final int channelB, boolean reverseDirection) {
     this(channelA, channelB, reverseDirection, EncodingType.k4X);
@@ -115,19 +110,21 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param channelA         The a channel digital input channel.
-   * @param channelB         The b channel digital input channel.
+   * @param channelA The a channel digital input channel.
+   * @param channelB The b channel digital input channel.
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
-   *                         if necessary so forward represents positive values.
-   * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
-   *                         selected, then an encoder FPGA object is used and the returned counts
-   *                         will be 4x the encoder spec'd value since all rising and falling edges
-   *                         are counted. If 1X or 2X are selected then a m_counter object will be
-   *                         used and the returned value will either exactly match the spec'd count
-   *                         or be double (2x) the spec'd count.
+   *     if necessary so forward represents positive values.
+   * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+   *     selected, then an encoder FPGA object is used and the returned counts will be 4x the
+   *     encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are
+   *     selected then a m_counter object will be used and the returned value will either exactly
+   *     match the spec'd count or be double (2x) the spec'd count.
    */
-  public Encoder(final int channelA, final int channelB, boolean reverseDirection,
-                 final EncodingType encodingType) {
+  public Encoder(
+      final int channelA,
+      final int channelB,
+      boolean reverseDirection,
+      final EncodingType encodingType) {
     requireNonNullParam(encodingType, "encodingType", "Encoder");
 
     m_allocatedA = true;
@@ -135,6 +132,7 @@
     m_allocatedI = false;
     m_aSource = new DigitalInput(channelA);
     m_bSource = new DigitalInput(channelB);
+    m_encodingType = encodingType;
     SendableRegistry.addChild(this, m_aSource);
     SendableRegistry.addChild(this, m_bSource);
     initEncoder(reverseDirection, encodingType);
@@ -146,14 +144,14 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param channelA         The a channel digital input channel.
-   * @param channelB         The b channel digital input channel.
-   * @param indexChannel     The index channel digital input channel.
+   * @param channelA The a channel digital input channel.
+   * @param channelB The b channel digital input channel.
+   * @param indexChannel The index channel digital input channel.
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
-   *                         if necessary so forward represents positive values.
+   *     if necessary so forward represents positive values.
    */
-  public Encoder(final int channelA, final int channelB, final int indexChannel,
-                 boolean reverseDirection) {
+  public Encoder(
+      final int channelA, final int channelB, final int indexChannel, boolean reverseDirection) {
     this(channelA, channelB, reverseDirection);
     m_allocatedI = true;
     m_indexSource = new DigitalInput(indexChannel);
@@ -167,8 +165,8 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param channelA     The a channel digital input channel.
-   * @param channelB     The b channel digital input channel.
+   * @param channelA The a channel digital input channel.
+   * @param channelB The b channel digital input channel.
    * @param indexChannel The index channel digital input channel.
    */
   public Encoder(final int channelA, final int channelB, final int indexChannel) {
@@ -182,10 +180,10 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param sourceA          The source that should be used for the a channel.
-   * @param sourceB          the source that should be used for the b channel.
+   * @param sourceA The source that should be used for the a channel.
+   * @param sourceB the source that should be used for the b channel.
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
-   *                         if necessary so forward represents positive values.
+   *     if necessary so forward represents positive values.
    */
   public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection) {
     this(sourceA, sourceB, reverseDirection, EncodingType.k4X);
@@ -212,19 +210,21 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param sourceA          The source that should be used for the a channel.
-   * @param sourceB          the source that should be used for the b channel.
+   * @param sourceA The source that should be used for the a channel.
+   * @param sourceB the source that should be used for the b channel.
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
-   *                         if necessary so forward represents positive values.
-   * @param encodingType     either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
-   *                         selected, then an encoder FPGA object is used and the returned counts
-   *                         will be 4x the encoder spec'd value since all rising and falling edges
-   *                         are counted. If 1X or 2X are selected then a m_counter object will be
-   *                         used and the returned value will either exactly match the spec'd count
-   *                         or be double (2x) the spec'd count.
+   *     if necessary so forward represents positive values.
+   * @param encodingType either k1X, k2X, or k4X to indicate 1X, 2X or 4X decoding. If 4X is
+   *     selected, then an encoder FPGA object is used and the returned counts will be 4x the
+   *     encoder spec'd value since all rising and falling edges are counted. If 1X or 2X are
+   *     selected then a m_counter object will be used and the returned value will either exactly
+   *     match the spec'd count or be double (2x) the spec'd count.
    */
-  public Encoder(DigitalSource sourceA, DigitalSource sourceB, boolean reverseDirection,
-                 final EncodingType encodingType) {
+  public Encoder(
+      DigitalSource sourceA,
+      DigitalSource sourceB,
+      boolean reverseDirection,
+      final EncodingType encodingType) {
     requireNonNullParam(sourceA, "sourceA", "Encoder");
     requireNonNullParam(sourceB, "sourceB", "Encoder");
     requireNonNullParam(encodingType, "encodingType", "Encoder");
@@ -232,6 +232,7 @@
     m_allocatedA = false;
     m_allocatedB = false;
     m_allocatedI = false;
+    m_encodingType = encodingType;
     m_aSource = sourceA;
     m_bSource = sourceB;
     initEncoder(reverseDirection, encodingType);
@@ -244,14 +245,17 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param sourceA          The source that should be used for the a channel.
-   * @param sourceB          the source that should be used for the b channel.
-   * @param indexSource      the source that should be used for the index channel.
+   * @param sourceA The source that should be used for the a channel.
+   * @param sourceB the source that should be used for the b channel.
+   * @param indexSource the source that should be used for the index channel.
    * @param reverseDirection represents the orientation of the encoder and inverts the output values
-   *                         if necessary so forward represents positive values.
+   *     if necessary so forward represents positive values.
    */
-  public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource,
-                 boolean reverseDirection) {
+  public Encoder(
+      DigitalSource sourceA,
+      DigitalSource sourceB,
+      DigitalSource indexSource,
+      boolean reverseDirection) {
     this(sourceA, sourceB, reverseDirection);
     m_allocatedI = false;
     m_indexSource = indexSource;
@@ -265,8 +269,8 @@
    *
    * <p>The encoder will start counting immediately.
    *
-   * @param sourceA     The source that should be used for the a channel.
-   * @param sourceB     the source that should be used for the b channel.
+   * @param sourceA The source that should be used for the a channel.
+   * @param sourceB the source that should be used for the b channel.
    * @param indexSource the source that should be used for the index channel.
    */
   public Encoder(DigitalSource sourceA, DigitalSource sourceB, DigitalSource indexSource) {
@@ -336,9 +340,7 @@
     return EncoderJNI.getEncoder(m_encoder);
   }
 
-  /**
-   * Reset the Encoder distance to zero. Resets the current count to zero on the encoder.
-   */
+  /** Reset the Encoder distance to zero. Resets the current count to zero on the encoder. */
   @Override
   public void reset() {
     EncoderJNI.resetEncoder(m_encoder);
@@ -367,7 +369,7 @@
    * compensates for the decoding type.
    *
    * @param maxPeriod The maximum time between rising and falling edges before the FPGA will report
-   *                  the device stopped. This is expressed in seconds.
+   *     the device stopped. This is expressed in seconds.
    */
   @Override
   public void setMaxPeriod(double maxPeriod) {
@@ -420,7 +422,7 @@
    * Set the minimum rate of the device before the hardware reports it stopped.
    *
    * @param minRate The minimum rate. The units are in distance per second as scaled by the value
-   *                from setDistancePerPulse().
+   *     from setDistancePerPulse().
    */
   public void setMinRate(double minRate) {
     EncoderJNI.setEncoderMinRate(m_encoder, minRate);
@@ -481,39 +483,6 @@
   }
 
   /**
-   * Set which parameter of the encoder you are using as a process control variable. The encoder
-   * class supports the rate and distance parameters.
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Implement the PIDSource interface.
-   *
-   * @return The current value of the selected source parameter.
-   */
-  @Override
-  public double pidGet() {
-    switch (m_pidSource) {
-      case kDisplacement:
-        return getDistance();
-      case kRate:
-        return getRate();
-      default:
-        return 0.0;
-    }
-  }
-
-  /**
    * Set the index source for the encoder. When this source is activated, the encoder count
    * automatically resets.
    *
@@ -538,7 +507,7 @@
    * resets.
    *
    * @param channel A DIO channel to set as the encoder index
-   * @param type    The state that will cause the encoder to reset
+   * @param type The state that will cause the encoder to reset
    */
   public void setIndexSource(int channel, IndexingType type) {
     if (m_allocatedI) {
@@ -555,11 +524,14 @@
    * resets.
    *
    * @param source A digital source to set as the encoder index
-   * @param type   The state that will cause the encoder to reset
+   * @param type The state that will cause the encoder to reset
    */
   public void setIndexSource(DigitalSource source, IndexingType type) {
-    EncoderJNI.setEncoderIndexSource(m_encoder, source.getPortHandleForRouting(),
-        source.getAnalogTriggerTypeForRouting(), type.value);
+    EncoderJNI.setEncoderIndexSource(
+        m_encoder,
+        source.getPortHandleForRouting(),
+        source.getAnalogTriggerTypeForRouting(),
+        type.value);
   }
 
   /**
@@ -571,6 +543,24 @@
     EncoderJNI.setEncoderSimDevice(m_encoder, device.getNativeHandle());
   }
 
+  /**
+   * Gets the decoding scale factor for scaling raw values to full counts.
+   *
+   * @return decoding scale factor
+   */
+  public double getDecodingScaleFactor() {
+    switch (m_encodingType) {
+      case k1X:
+        return 1.0;
+      case k2X:
+        return 0.5;
+      case k4X:
+        return 0.25;
+      default:
+        return 0.0;
+    }
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     if (EncoderJNI.getEncoderEncodingType(m_encoder) == EncodingType.k4X.value) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
index c1327aa..98230db 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Filesystem.java
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import java.io.File;
 
 /**
- * Class for interacting with the Filesystem, particularly, interacting with
- * FRC-related paths on the system, such as the launch and deploy directories.
+ * Class for interacting with the Filesystem, particularly, interacting with FRC-related paths on
+ * the system, such as the launch and deploy directories.
  *
- * <p>This class is primarily used for obtaining resources in src/main/deploy, and
- * the RoboRIO path /home/lvuser in a simulation-compatible way.</p>
+ * <p>This class is primarily used for obtaining resources in src/main/deploy, and the RoboRIO path
+ * /home/lvuser in a simulation-compatible way.
  */
 public final class Filesystem {
-  private Filesystem() { }
+  private Filesystem() {}
 
   /**
-   * Obtains the current working path that the program was launched with.
-   * This is analogous to the `pwd` command on unix.
+   * Obtains the current working path that the program was launched with. This is analogous to the
+   * `pwd` command on unix.
    *
    * @return The current working directory (launch directory)
    */
@@ -30,9 +27,8 @@
   }
 
   /**
-   * Obtains the operating directory of the program. On the roboRIO, this is
-   * /home/lvuser. In simulation, it is where the simulation was launched from
-   * (`pwd`).
+   * Obtains the operating directory of the program. On the roboRIO, this is /home/lvuser. In
+   * simulation, it is where the simulation was launched from (`pwd`).
    *
    * @return The operating directory
    */
@@ -45,10 +41,10 @@
   }
 
   /**
-   * 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 "src/main/deploy" (`pwd`/src/main/deploy).
+   * 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 "src/main/deploy"
+   * (`pwd`/src/main/deploy).
    *
    * @return The deploy directory
    */
@@ -56,8 +52,8 @@
     if (RobotBase.isReal()) {
       return new File(getOperatingDirectory(), "deploy");
     } else {
-      return new File(getOperatingDirectory(), "src" + File.separator + "main"
-          + File.separator + "deploy");
+      return new File(
+          getOperatingDirectory(), "src" + File.separator + "main" + File.separator + "deploy");
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
deleted file mode 100644
index 41f9d09..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GearTooth.java
+++ /dev/null
@@ -1,102 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Alias for counter class. Implement 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 The only sensor this works with is no longer available and no teams use it according
- *             to FMS usage reporting.
- */
-@Deprecated(since = "2020", forRemoval = true)
-public class GearTooth extends Counter {
-  private static final double kGearToothThreshold = 55e-6;
-
-  /**
-   * Common code called by the constructors.
-   */
-  public void enableDirectionSensing(boolean directionSensitive) {
-    if (directionSensitive) {
-      setPulseLengthMode(kGearToothThreshold);
-    }
-  }
-
-  /**
-   * Construct a GearTooth sensor given a channel.
-   *
-   * <p>No direction sensing is assumed.
-   *
-   * @param channel The GPIO channel that the sensor is connected to.
-   */
-  public GearTooth(final int channel) {
-    this(channel, false);
-  }
-
-  /**
-   * Construct a GearTooth sensor given a channel.
-   *
-   * @param channel            The DIO channel that the sensor is connected to. 0-9 are on-board,
-   *                           10-25 are on the MXP port
-   * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
-   *                           direction.
-   */
-  public GearTooth(final int channel, boolean directionSensitive) {
-    super(channel);
-    enableDirectionSensing(directionSensitive);
-    if (directionSensitive) {
-      HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0, "D");
-    } else {
-      HAL.report(tResourceType.kResourceType_GearTooth, channel + 1, 0);
-    }
-    SendableRegistry.setName(this, "GearTooth", channel);
-  }
-
-  /**
-   * Construct a GearTooth sensor given a digital input. This should be used when sharing digital
-   * inputs.
-   *
-   * @param source             An existing DigitalSource object (such as a DigitalInput)
-   * @param directionSensitive True to enable the pulse length decoding in hardware to specify count
-   *                           direction.
-   */
-  public GearTooth(DigitalSource source, boolean directionSensitive) {
-    super(source);
-    enableDirectionSensing(directionSensitive);
-    if (directionSensitive) {
-      HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0, "D");
-    } else {
-      HAL.report(tResourceType.kResourceType_GearTooth, source.getChannel() + 1, 0);
-    }
-    SendableRegistry.setName(this, "GearTooth", source.getChannel());
-  }
-
-  /**
-   * Construct a GearTooth sensor given a digital input. This should be used when sharing digital
-   * inputs.
-   *
-   * <p>No direction sensing is assumed.
-   *
-   * @param source An object that fully describes the input that the sensor is connected to.
-   */
-  public GearTooth(DigitalSource source) {
-    this(source, false);
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    super.initSendable(builder);
-    builder.setSmartDashboardType("Gear Tooth");
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
index f573cb2..4903e00 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GenericHID.java
@@ -1,26 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.HAL;
 import java.util.HashMap;
 import java.util.Map;
 
-import edu.wpi.first.hal.HAL;
-
 /**
- * GenericHID Interface.
+ * Handle input from standard HID devices connected to the Driver Station.
+ *
+ * <p>This class handles standard input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each device and
+ * the mapping of ports to hardware buttons depends on the code in the Driver Station.
  */
-public abstract class GenericHID {
-  /**
-   * Represents a rumble output on the JoyStick.
-   */
+public class GenericHID {
+  /** Represents a rumble output on the JoyStick. */
   public enum RumbleType {
-    kLeftRumble, kRightRumble
+    kLeftRumble,
+    kRightRumble
   }
 
   public enum HIDType {
@@ -42,8 +41,8 @@
     kHIDFlight(23),
     kHID1stPerson(24);
 
-    @SuppressWarnings("MemberName")
     public final int value;
+
     @SuppressWarnings("PMD.UseConcurrentHashMap")
     private static final Map<Integer, HIDType> map = new HashMap<>();
 
@@ -62,109 +61,62 @@
     }
   }
 
-  /**
-   * Which hand the Human Interface Device is associated with.
-   */
-  public enum Hand {
-    kLeft(0), kRight(1);
-
-    @SuppressWarnings("MemberName")
-    public final int value;
-
-    Hand(int value) {
-      this.value = value;
-    }
-  }
-
-  private DriverStation m_ds;
   private final int m_port;
   private int m_outputs;
   private short m_leftRumble;
   private short m_rightRumble;
 
+  /**
+   * Construct an instance of a device.
+   *
+   * @param port The port index on the Driver Station that the device is plugged into.
+   */
   public GenericHID(int port) {
-    m_ds = DriverStation.getInstance();
     m_port = port;
   }
 
   /**
-   * Get the x position of the HID.
-   *
-   * @return the x position of the HID
-   */
-  public final double getX() {
-    return getX(Hand.kRight);
-  }
-
-  /**
-   * Get the x position of HID.
-   *
-   * @param hand which hand, left or right
-   * @return the x position
-   */
-  public abstract double getX(Hand hand);
-
-  /**
-   * Get the y position of the HID.
-   *
-   * @return the y position
-   */
-  public final double getY() {
-    return getY(Hand.kRight);
-  }
-
-  /**
-   * Get the y position of the HID.
-   *
-   * @param hand which hand, left or right
-   * @return the y position
-   */
-  public abstract double getY(Hand hand);
-
-  /**
    * Get the button value (starting at button 1).
    *
    * <p>The buttons are returned in a single 16 bit value with one bit representing the state of
    * each button. The appropriate button is returned as a boolean value.
    *
-   * <p>This method returns true if the button is being held down at the time
-   * that this method is being called.
+   * <p>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.
    */
   public boolean getRawButton(int button) {
-    return m_ds.getStickButton(m_port, (byte) button);
+    return DriverStation.getStickButton(m_port, (byte) button);
   }
 
   /**
-   * Whether the button was pressed since the last check. Button indexes begin at
-   * 1.
+   * Whether the button was pressed since the last check. Button indexes begin at 1.
    *
-   * <p>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.
+   * <p>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.
    */
   public boolean getRawButtonPressed(int button) {
-    return m_ds.getStickButtonPressed(m_port, (byte) button);
+    return DriverStation.getStickButtonPressed(m_port, (byte) button);
   }
 
   /**
-   * Whether the button was released since the last check. Button indexes begin at
-   * 1.
+   * Whether the button was released since the last check. Button indexes begin at 1.
    *
-   * <p>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.
+   * <p>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.
    */
   public boolean getRawButtonReleased(int button) {
-    return m_ds.getStickButtonReleased(m_port, button);
+    return DriverStation.getStickButtonReleased(m_port, button);
   }
 
   /**
@@ -174,7 +126,7 @@
    * @return The value of the axis.
    */
   public double getRawAxis(int axis) {
-    return m_ds.getStickAxis(m_port, axis);
+    return DriverStation.getStickAxis(m_port, axis);
   }
 
   /**
@@ -183,13 +135,21 @@
    * <p>The POV angles start at 0 in the up direction, and increase clockwise (eg right is 90,
    * upper-left is 315).
    *
-   * @param pov The index of the POV to read (starting at 0)
+   * @param pov The index of the POV to read (starting at 0). Defaults to 0.
    * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
    */
   public int getPOV(int pov) {
-    return m_ds.getStickPOV(m_port, pov);
+    return DriverStation.getStickPOV(m_port, pov);
   }
 
+  /**
+   * Get the angle in degrees of the default POV (index 0) on the HID.
+   *
+   * <p>The POV angles start at 0 in the up direction, and increase clockwise (eg right is 90,
+   * upper-left is 315).
+   *
+   * @return the angle of the POV in degrees, or -1 if the POV is not pressed.
+   */
   public int getPOV() {
     return getPOV(0);
   }
@@ -200,21 +160,25 @@
    * @return the number of axis for the current HID
    */
   public int getAxisCount() {
-    return m_ds.getStickAxisCount(m_port);
+    return DriverStation.getStickAxisCount(m_port);
   }
 
   /**
    * For the current HID, return the number of POVs.
+   *
+   * @return the number of POVs for the current HID
    */
   public int getPOVCount() {
-    return m_ds.getStickPOVCount(m_port);
+    return DriverStation.getStickPOVCount(m_port);
   }
 
   /**
    * For the current HID, return the number of buttons.
+   *
+   * @return the number of buttons for the current HID
    */
   public int getButtonCount() {
-    return m_ds.getStickButtonCount(m_port);
+    return DriverStation.getStickButtonCount(m_port);
   }
 
   /**
@@ -223,7 +187,7 @@
    * @return true if the HID is connected
    */
   public boolean isConnected() {
-    return m_ds.isJoystickConnected(m_port);
+    return DriverStation.isJoystickConnected(m_port);
   }
 
   /**
@@ -232,7 +196,7 @@
    * @return the type of the HID.
    */
   public HIDType getType() {
-    return HIDType.of(m_ds.getJoystickType(m_port));
+    return HIDType.of(DriverStation.getJoystickType(m_port));
   }
 
   /**
@@ -241,16 +205,17 @@
    * @return the name of the HID.
    */
   public String getName() {
-    return m_ds.getJoystickName(m_port);
+    return DriverStation.getJoystickName(m_port);
   }
 
   /**
    * Get the axis type of a joystick axis.
    *
+   * @param axis The axis to read, starting at 0.
    * @return the axis type of a joystick axis.
    */
   public int getAxisType(int axis) {
-    return m_ds.getJoystickAxisType(m_port, axis);
+    return DriverStation.getJoystickAxisType(m_port, axis);
   }
 
   /**
@@ -266,7 +231,7 @@
    * Set a single HID output value for the HID.
    *
    * @param outputNumber The index of the output to set (1-32)
-   * @param value        The value to set the output to
+   * @param value The value to set the output to
    */
   public void setOutput(int outputNumber, boolean value) {
     m_outputs = (m_outputs & ~(1 << (outputNumber - 1))) | ((value ? 1 : 0) << (outputNumber - 1));
@@ -287,7 +252,7 @@
    * Set the rumble output for the HID. The DS currently supports 2 rumble values, left rumble and
    * right rumble.
    *
-   * @param type  Which rumble value to set
+   * @param type Which rumble value to set
    * @param value The normalized value (0 to 1) to set the rumble to
    */
   public void setRumble(RumbleType type, double value) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
deleted file mode 100644
index 21330ce..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/GyroBase.java
+++ /dev/null
@@ -1,58 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-
-/**
- * GyroBase is the common base class for Gyro implementations such as AnalogGyro.
- */
-public abstract class GyroBase implements Gyro, PIDSource, Sendable {
-  private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
-
-  /**
-   * Set which parameter of the gyro you are using as a process control variable. The Gyro class
-   * supports the rate and displacement parameters
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Get the output of the gyro for use with PIDControllers. May be the angle or rate depending on
-   * the set PIDSourceType
-   *
-   * @return the output according to the gyro
-   */
-  @Override
-  public double pidGet() {
-    switch (m_pidSource) {
-      case kRate:
-        return getRate();
-      case kDisplacement:
-        return getAngle();
-      default:
-        return 0.0;
-    }
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Gyro");
-    builder.addDoubleProperty("Value", this::getAngle, null);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
index 58b96aa..1d148a7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/I2C.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.I2CJNI;
 import edu.wpi.first.hal.util.BoundaryException;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import java.nio.ByteBuffer;
 
 /**
  * I2C bus interface class.
@@ -22,12 +18,11 @@
  * <p>This class is intended to be used by sensor (and other I2C device) drivers. It probably should
  * not be used directly.
  */
-@SuppressWarnings("PMD.GodClass")
 public class I2C implements AutoCloseable {
   public enum Port {
-    kOnboard(0), kMXP(1);
+    kOnboard(0),
+    kMXP(1);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     Port(int value) {
@@ -41,7 +36,7 @@
   /**
    * Constructor.
    *
-   * @param port          The I2C port the device is connected to.
+   * @param port The I2C port the device is connected to.
    * @param deviceAddress The address of the device on the I2C bus.
    */
   public I2C(Port port, int deviceAddress) {
@@ -53,6 +48,14 @@
     HAL.report(tResourceType.kResourceType_I2C, deviceAddress);
   }
 
+  public int getPort() {
+    return m_port;
+  }
+
+  public int getDeviceAddress() {
+    return m_deviceAddress;
+  }
+
   @Override
   public void close() {
     I2CJNI.i2CClose(m_port);
@@ -62,18 +65,18 @@
    * Generic transaction.
    *
    * <p>This is a lower-level interface to the I2C hardware giving you more control over each
-   * transaction. If you intend to write multiple bytes in the same transaction and do not
-   * plan to receive anything back, use writeBulk() instead. Calling this with a receiveSize
-   * of 0 will result in an error.
+   * transaction. If you intend to write multiple bytes in the same transaction and do not plan to
+   * receive anything back, use writeBulk() instead. Calling this with a receiveSize of 0 will
+   * result in an error.
    *
-   * @param dataToSend   Buffer of data to send as part of the transaction.
-   * @param sendSize     Number of bytes to send as part of the transaction.
+   * @param dataToSend Buffer of data to send as part of the transaction.
+   * @param sendSize Number of bytes to send as part of the transaction.
    * @param dataReceived Buffer to read data into.
-   * @param receiveSize  Number of bytes to read from the device.
+   * @param receiveSize Number of bytes to read from the device.
    * @return Transfer Aborted... false for success, true for aborted.
    */
-  public synchronized boolean transaction(byte[] dataToSend, int sendSize,
-                                          byte[] dataReceived, int receiveSize) {
+  public synchronized boolean transaction(
+      byte[] dataToSend, int sendSize, byte[] dataReceived, int receiveSize) {
     if (dataToSend.length < sendSize) {
       throw new IllegalArgumentException("dataToSend is too small, must be at least " + sendSize);
     }
@@ -81,8 +84,14 @@
       throw new IllegalArgumentException(
           "dataReceived is too small, must be at least " + receiveSize);
     }
-    return I2CJNI.i2CTransactionB(m_port, (byte) m_deviceAddress, dataToSend,
-                                  (byte) sendSize, dataReceived, (byte) receiveSize) < 0;
+    return I2CJNI.i2CTransactionB(
+            m_port,
+            (byte) m_deviceAddress,
+            dataToSend,
+            (byte) sendSize,
+            dataReceived,
+            (byte) receiveSize)
+        < 0;
   }
 
   /**
@@ -91,15 +100,15 @@
    * <p>This is a lower-level interface to the I2C hardware giving you more control over each
    * transaction.
    *
-   * @param dataToSend   Buffer of data to send as part of the transaction.
-   * @param sendSize     Number of bytes to send as part of the transaction.
+   * @param dataToSend Buffer of data to send as part of the transaction.
+   * @param sendSize Number of bytes to send as part of the transaction.
    * @param dataReceived Buffer to read data into.
-   * @param receiveSize  Number of bytes to read from the device.
+   * @param receiveSize Number of bytes to read from the device.
    * @return Transfer Aborted... false for success, true for aborted.
    */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "ByteBufferBackingArray"})
-  public synchronized boolean transaction(ByteBuffer dataToSend, int sendSize,
-                                          ByteBuffer dataReceived, int receiveSize) {
+  @SuppressWarnings("ByteBufferBackingArray")
+  public synchronized boolean transaction(
+      ByteBuffer dataToSend, int sendSize, ByteBuffer dataReceived, int receiveSize) {
     if (dataToSend.hasArray() && dataReceived.hasArray()) {
       return transaction(dataToSend.array(), sendSize, dataReceived.array(), receiveSize);
     }
@@ -117,8 +126,14 @@
           "dataReceived is too small, must be at least " + receiveSize);
     }
 
-    return I2CJNI.i2CTransaction(m_port, (byte) m_deviceAddress, dataToSend,
-                                 (byte) sendSize, dataReceived, (byte) receiveSize) < 0;
+    return I2CJNI.i2CTransaction(
+            m_port,
+            (byte) m_deviceAddress,
+            dataToSend,
+            (byte) sendSize,
+            dataReceived,
+            (byte) receiveSize)
+        < 0;
   }
 
   /**
@@ -139,15 +154,14 @@
    * <p>Write a single byte to a register on a device and wait until the transaction is complete.
    *
    * @param registerAddress The address of the register on the device to be written.
-   * @param data            The byte to write to the register on the device.
+   * @param data The byte to write to the register on the device.
    * @return Transfer Aborted... false for success, true for aborted.
    */
   public synchronized boolean write(int registerAddress, int data) {
     byte[] buffer = new byte[2];
     buffer[0] = (byte) registerAddress;
     buffer[1] = (byte) data;
-    return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, buffer,
-                            (byte) buffer.length) < 0;
+    return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, buffer, (byte) buffer.length) < 0;
   }
 
   /**
@@ -173,8 +187,7 @@
    */
   public synchronized boolean writeBulk(byte[] data, int size) {
     if (data.length < size) {
-      throw new IllegalArgumentException(
-          "buffer is too small, must be at least " + size);
+      throw new IllegalArgumentException("buffer is too small, must be at least " + size);
     }
     return I2CJNI.i2CWriteB(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
   }
@@ -197,8 +210,7 @@
       throw new IllegalArgumentException("must be a direct buffer");
     }
     if (data.capacity() < size) {
-      throw new IllegalArgumentException(
-          "buffer is too small, must be at least " + size);
+      throw new IllegalArgumentException("buffer is too small, must be at least " + size);
     }
 
     return I2CJNI.i2CWrite(m_port, (byte) m_deviceAddress, data, (byte) size) < 0;
@@ -211,8 +223,8 @@
    * internally allowing you to read consecutive registers on a device in a single transaction.
    *
    * @param registerAddress The register to read first in the transaction.
-   * @param count           The number of bytes to read in the transaction.
-   * @param buffer          A pointer to the array of bytes to store the data read from the device.
+   * @param count The number of bytes to read in the transaction.
+   * @param buffer A pointer to the array of bytes to store the data read from the device.
    * @return Transfer Aborted... false for success, true for aborted.
    */
   public boolean read(int registerAddress, int count, byte[] buffer) {
@@ -240,8 +252,8 @@
    * internally allowing you to read consecutive registers on a device in a single transaction.
    *
    * @param registerAddress The register to read first in the transaction.
-   * @param count           The number of bytes to read in the transaction.
-   * @param buffer          A buffer to store the data read from the device.
+   * @param count The number of bytes to read in the transaction.
+   * @param buffer A buffer to store the data read from the device.
    * @return Transfer Aborted... false for success, true for aborted.
    */
   @SuppressWarnings("ByteBufferBackingArray")
@@ -277,7 +289,7 @@
    * <p>Read bytes from a device. This method does not write any data to prompt the device.
    *
    * @param buffer A pointer to the array of bytes to store the data read from the device.
-   * @param count  The number of bytes to read in the transaction.
+   * @param count The number of bytes to read in the transaction.
    * @return Transfer Aborted... false for success, true for aborted.
    */
   public boolean readOnly(byte[] buffer, int count) {
@@ -289,8 +301,7 @@
       throw new IllegalArgumentException("buffer is too small, must be at least " + count);
     }
 
-    return I2CJNI.i2CReadB(m_port, (byte) m_deviceAddress, buffer,
-                           (byte) count) < 0;
+    return I2CJNI.i2CReadB(m_port, (byte) m_deviceAddress, buffer, (byte) count) < 0;
   }
 
   /**
@@ -299,14 +310,13 @@
    * <p>Read bytes from a device. This method does not write any data to prompt the device.
    *
    * @param buffer A pointer to the array of bytes to store the data read from the device.
-   * @param count  The number of bytes to read in the transaction.
+   * @param count The number of bytes to read in the transaction.
    * @return Transfer Aborted... false for success, true for aborted.
    */
   @SuppressWarnings("ByteBufferBackingArray")
   public boolean readOnly(ByteBuffer buffer, int count) {
     if (count < 1) {
-      throw new BoundaryException("Value must be at least 1, " + count
-          + " given");
+      throw new BoundaryException("Value must be at least 1, " + count + " given");
     }
 
     if (buffer.hasArray()) {
@@ -320,8 +330,7 @@
       throw new IllegalArgumentException("buffer is too small, must be at least " + count);
     }
 
-    return I2CJNI.i2CRead(m_port, (byte) m_deviceAddress, buffer, (byte) count)
-        < 0;
+    return I2CJNI.i2CRead(m_port, (byte) m_deviceAddress, buffer, (byte) count) < 0;
   }
 
   /*
@@ -343,13 +352,12 @@
    * expected value.
    *
    * @param registerAddress The base register to start reading from the device.
-   * @param count           The size of the field to be verified.
-   * @param expected        A buffer containing the values expected from the device.
+   * @param count The size of the field to be verified.
+   * @param expected A buffer containing the values expected from the device.
    * @return true if the sensor was verified to be connected
    * @pre The device must support and be configured to use register auto-increment.
    */
-  public boolean verifySensor(int registerAddress, int count,
-                              byte[] expected) {
+  public boolean verifySensor(int registerAddress, int count, byte[] expected) {
     // TODO: Make use of all 7 read bytes
     byte[] dataToSend = new byte[1];
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java
deleted file mode 100644
index 6fd807b..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptHandlerFunction.java
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.InterruptJNI.InterruptJNIHandlerFunction;
-
-
-/**
- * It is recommended that you use this class in conjunction with classes from {@link
- * java.util.concurrent.atomic} as these objects are all thread safe.
- *
- * @param <T> The type of the parameter that should be returned to the the method {@link
- *            #interruptFired(int, Object)}
- */
-public abstract class InterruptHandlerFunction<T> {
-  /**
-   * The entry point for the interrupt. When the interrupt fires the {@link #apply(int, Object)}
-   * method is called. The outer class is provided as an interface to allow the implementer to pass
-   * a generic object to the interrupt fired method.
-   */
-  private class Function implements InterruptJNIHandlerFunction {
-    @SuppressWarnings("unchecked")
-    @Override
-    public void apply(int interruptAssertedMask, Object param) {
-      interruptFired(interruptAssertedMask, (T) param);
-    }
-  }
-
-  final Function m_function = new Function();
-
-  /**
-   * This method is run every time an interrupt is fired.
-   *
-   * @param interruptAssertedMask Interrupt Mask
-   * @param param                 The parameter provided by overriding the {@link
-   *                              #overridableParameter()} method.
-   */
-  public abstract void interruptFired(int interruptAssertedMask, T param);
-
-
-  /**
-   * Override this method if you would like to pass a specific parameter to the {@link
-   * #interruptFired(int, Object)} when it is fired by the interrupt. This method is called once
-   * when {@link InterruptableSensorBase#requestInterrupts(InterruptHandlerFunction)} is run.
-   *
-   * @return The object that should be passed to the interrupt when it runs
-   */
-  public T overridableParameter() {
-    return null;
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
deleted file mode 100644
index 8fb734b..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/InterruptableSensorBase.java
+++ /dev/null
@@ -1,283 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.function.Consumer;
-
-import edu.wpi.first.hal.InterruptJNI;
-import edu.wpi.first.hal.util.AllocationException;
-
-
-/**
- * Base for sensors to be used with interrupts.
- */
-public abstract class InterruptableSensorBase implements AutoCloseable {
-  @SuppressWarnings("JavadocMethod")
-  public enum WaitResult {
-    kTimeout(0x0), kRisingEdge(0x1), kFallingEdge(0x100), kBoth(0x101);
-
-    @SuppressWarnings("MemberName")
-    public final int value;
-
-    WaitResult(int value) {
-      this.value = value;
-    }
-
-    public static WaitResult getValue(boolean rising, boolean falling) {
-      if (rising && falling) {
-        return kBoth;
-      } else if (rising) {
-        return kRisingEdge;
-      } else if (falling) {
-        return kFallingEdge;
-      } else {
-        return kTimeout;
-      }
-    }
-  }
-
-  /**
-   * The interrupt resource.
-   */
-  protected int m_interrupt = InterruptJNI.HalInvalidHandle;
-
-  /**
-   * Flags if the interrupt being allocated is synchronous.
-   */
-  protected boolean m_isSynchronousInterrupt;
-
-  /**
-   * Create a new InterrupatableSensorBase.
-   */
-  public InterruptableSensorBase() {
-    m_interrupt = 0;
-  }
-
-  @Override
-  public void close() {
-    if (m_interrupt != 0) {
-      cancelInterrupts();
-    }
-  }
-
-  /**
-   * If this is an analog trigger.
-   *
-   * @return true if this is an analog trigger.
-   */
-  public abstract int getAnalogTriggerTypeForRouting();
-
-  /**
-   * The channel routing number.
-   *
-   * @return channel routing number
-   */
-  public abstract int getPortHandleForRouting();
-
-  /**
-   * Request one of the 8 interrupts asynchronously on this digital input.
-   *
-   * @param handler The {@link Consumer} that will be called whenever there is an interrupt on this
-   *                device. Request interrupts in synchronous mode where the user program interrupt
-   *                handler will be called when an interrupt occurs. The default is interrupt on
-   *                rising edges only.
-   */
-  public void requestInterrupts(Consumer<WaitResult> handler) {
-    if (m_interrupt != 0) {
-      throw new AllocationException("The interrupt has already been allocated");
-    }
-
-    allocateInterrupts(false);
-
-    assert m_interrupt != 0;
-
-    InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
-        getAnalogTriggerTypeForRouting());
-    setUpSourceEdge(true, false);
-    InterruptJNI.attachInterruptHandler(m_interrupt, (mask, obj) -> {
-      // Rising edge result is the interrupt bit set in the byte 0xFF
-      // Falling edge result is the interrupt bit set in the byte 0xFF00
-      boolean rising = (mask & 0xFF) != 0;
-      boolean falling = (mask & 0xFF00) != 0;
-      handler.accept(WaitResult.getValue(rising, falling));
-    }, null);
-  }
-
-  /**
-   * Request one of the 8 interrupts asynchronously on this digital input.
-   *
-   * @param handler The {@link InterruptHandlerFunction} that contains the method {@link
-   *                InterruptHandlerFunction#interruptFired(int, Object)} that will be called
-   *                whenever there is an interrupt on this device. Request interrupts in synchronous
-   *                mode where the user program interrupt handler will be called when an interrupt
-   *                occurs. The default is interrupt on rising edges only.
-   */
-  public void requestInterrupts(InterruptHandlerFunction<?> handler) {
-    if (m_interrupt != 0) {
-      throw new AllocationException("The interrupt has already been allocated");
-    }
-
-    allocateInterrupts(false);
-
-    assert m_interrupt != 0;
-
-    InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
-        getAnalogTriggerTypeForRouting());
-    setUpSourceEdge(true, false);
-    InterruptJNI.attachInterruptHandler(m_interrupt, handler.m_function,
-        handler.overridableParameter());
-  }
-
-  /**
-   * Request one of the 8 interrupts synchronously on this digital input. Request interrupts in
-   * synchronous mode where the user program will have to explicitly wait for the interrupt to occur
-   * using {@link #waitForInterrupt}. The default is interrupt on rising edges only.
-   */
-  public void requestInterrupts() {
-    if (m_interrupt != 0) {
-      throw new AllocationException("The interrupt has already been allocated");
-    }
-
-    allocateInterrupts(true);
-
-    assert m_interrupt != 0;
-
-    InterruptJNI.requestInterrupts(m_interrupt, getPortHandleForRouting(),
-        getAnalogTriggerTypeForRouting());
-    setUpSourceEdge(true, false);
-
-  }
-
-  /**
-   * Allocate the interrupt.
-   *
-   * @param watcher true if the interrupt should be in synchronous mode where the user program will
-   *                have to explicitly wait for the interrupt to occur.
-   */
-  protected void allocateInterrupts(boolean watcher) {
-    m_isSynchronousInterrupt = watcher;
-
-    m_interrupt = InterruptJNI.initializeInterrupts(watcher);
-  }
-
-  /**
-   * Cancel interrupts on this device. This deallocates all the chipobject structures and disables
-   * any interrupts.
-   */
-  public void cancelInterrupts() {
-    if (m_interrupt == 0) {
-      throw new IllegalStateException("The interrupt is not allocated.");
-    }
-    InterruptJNI.cleanInterrupts(m_interrupt);
-    m_interrupt = 0;
-  }
-
-  /**
-   * In synchronous mode, wait for the defined interrupt to occur.
-   *
-   * @param timeout        Timeout in seconds
-   * @param ignorePrevious If true, ignore interrupts that happened before waitForInterrupt was
-   *                       called.
-   * @return Result of the wait.
-   */
-  public WaitResult waitForInterrupt(double timeout, boolean ignorePrevious) {
-    if (m_interrupt == 0) {
-      throw new IllegalStateException("The interrupt is not allocated.");
-    }
-    int result = InterruptJNI.waitForInterrupt(m_interrupt, timeout, ignorePrevious);
-
-    // Rising edge result is the interrupt bit set in the byte 0xFF
-    // Falling edge result is the interrupt bit set in the byte 0xFF00
-    // Set any bit set to be true for that edge, and AND the 2 results
-    // together to match the existing enum for all interrupts
-    boolean rising = (result & 0xFF) != 0;
-    boolean falling = (result & 0xFF00) != 0;
-    return WaitResult.getValue(rising, falling);
-  }
-
-  /**
-   * In synchronous mode, wait for the defined interrupt to occur.
-   *
-   * @param timeout Timeout in seconds
-   * @return Result of the wait.
-   */
-  public WaitResult waitForInterrupt(double timeout) {
-    return waitForInterrupt(timeout, true);
-  }
-
-  /**
-   * Enable interrupts to occur on this input. Interrupts are disabled when the RequestInterrupt
-   * call is made. This gives time to do the setup of the other options before starting to field
-   * interrupts.
-   */
-  public void enableInterrupts() {
-    if (m_interrupt == 0) {
-      throw new IllegalStateException("The interrupt is not allocated.");
-    }
-    if (m_isSynchronousInterrupt) {
-      throw new IllegalStateException("You do not need to enable synchronous interrupts");
-    }
-    InterruptJNI.enableInterrupts(m_interrupt);
-  }
-
-  /**
-   * Disable Interrupts without without deallocating structures.
-   */
-  public void disableInterrupts() {
-    if (m_interrupt == 0) {
-      throw new IllegalStateException("The interrupt is not allocated.");
-    }
-    if (m_isSynchronousInterrupt) {
-      throw new IllegalStateException("You can not disable synchronous interrupts");
-    }
-    InterruptJNI.disableInterrupts(m_interrupt);
-  }
-
-  /**
-   * 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
-   * #setUpSourceEdge}.
-   *
-   * @return Timestamp in seconds since boot.
-   */
-  public double readRisingTimestamp() {
-    if (m_interrupt == 0) {
-      throw new IllegalStateException("The interrupt is not allocated.");
-    }
-    return InterruptJNI.readInterruptRisingTimestamp(m_interrupt) * 1e-6;
-  }
-
-  /**
-   * Return the timestamp for the falling interrupt that occurred most recently. This is in the same
-   * time domain as getClock(). The falling-edge interrupt should be enabled with {@link
-   * #setUpSourceEdge}.
-   *
-   * @return Timestamp in seconds since boot.
-   */
-  public double readFallingTimestamp() {
-    if (m_interrupt == 0) {
-      throw new IllegalStateException("The interrupt is not allocated.");
-    }
-    return InterruptJNI.readInterruptFallingTimestamp(m_interrupt) * 1e-6;
-  }
-
-  /**
-   * Set which edge to trigger interrupts on.
-   *
-   * @param risingEdge  true to interrupt on rising edge
-   * @param fallingEdge true to interrupt on falling edge
-   */
-  public void setUpSourceEdge(boolean risingEdge, boolean fallingEdge) {
-    if (m_interrupt != 0) {
-      InterruptJNI.setInterruptUpSourceEdge(m_interrupt, risingEdge,
-          fallingEdge);
-    } else {
-      throw new IllegalArgumentException("You must call RequestInterrupts before setUpSourceEdge");
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
deleted file mode 100644
index 66079a8..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobot.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-
-/**
- * IterativeRobot implements the IterativeRobotBase robot program framework.
- *
- * <p>The IterativeRobot class is intended to be subclassed by a user creating a robot program.
- *
- * <p>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.
- */
-@Deprecated
-public class IterativeRobot extends IterativeRobotBase {
-  private static final double kPacketPeriod = 0.02;
-  private volatile boolean m_exit;
-
-  /**
-   * Create a new IterativeRobot.
-   */
-  public IterativeRobot() {
-    super(kPacketPeriod);
-
-    HAL.report(tResourceType.kResourceType_Framework, tInstances.kFramework_Iterative);
-  }
-
-  /**
-   * Provide an alternate "main loop" via startCompetition().
-   */
-  @Override
-  public void startCompetition() {
-    robotInit();
-
-    if (isSimulation()) {
-      simulationInit();
-    }
-
-    // Tell the DS that the robot is ready to be enabled
-    HAL.observeUserProgramStarting();
-
-    // Loop forever, calling the appropriate mode-dependent function
-    while (!Thread.currentThread().isInterrupted()) {
-      // Wait for new data to arrive
-      m_ds.waitForData();
-      if (m_exit) {
-        break;
-      }
-
-      loopFunc();
-    }
-  }
-
-  /**
-   * Ends the main loop in startCompetition().
-   */
-  @Override
-  public void endCompetition() {
-    m_exit = true;
-    m_ds.wakeupWaitForData();
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
index daa5bdb..a8c92a6 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/IterativeRobotBase.java
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -24,27 +22,37 @@
  *
  * <p>robotInit() -- provide for initialization at robot power-on
  *
- * <p>init() functions -- each of the following functions is called once when the
- * appropriate mode is entered:
- *   - disabledInit()   -- called each and every time disabled is entered from
- *                         another mode
- *   - autonomousInit() -- called each and every time autonomous is entered from
- *                         another mode
- *   - teleopInit()     -- called each and every time teleop is entered from
- *                         another mode
- *   - testInit()       -- called each and every time test is entered from
- *                         another mode
+ * <p>init() functions -- each of the following functions is called once when the appropriate mode
+ * is entered:
+ *
+ * <ul>
+ *   <li>disabledInit() -- called each and every time disabled is entered from another mode
+ *   <li>autonomousInit() -- called each and every time autonomous is entered from another mode
+ *   <li>teleopInit() -- called each and every time teleop is entered from another mode
+ *   <li>testInit() -- called each and every time test is entered from another mode
+ * </ul>
  *
  * <p>periodic() functions -- each of these functions is called on an interval:
- *   - robotPeriodic()
- *   - disabledPeriodic()
- *   - autonomousPeriodic()
- *   - teleopPeriodic()
- *   - testPeriodic()
+ *
+ * <ul>
+ *   <li>robotPeriodic()
+ *   <li>disabledPeriodic()
+ *   <li>autonomousPeriodic()
+ *   <li>teleopPeriodic()
+ *   <li>testPeriodic()
+ * </ul>
+ *
+ * <p>final() functions -- each of the following functions is called once when the appropriate mode
+ * is exited:
+ *
+ * <ul>
+ *   <li>disabledExit() -- called each and every time disabled is exited
+ *   <li>autonomousExit() -- called each and every time autonomous is exited
+ *   <li>teleopExit() -- called each and every time teleop is exited
+ *   <li>testExit() -- called each and every time test is exited
+ * </ul>
  */
 public abstract class IterativeRobotBase extends RobotBase {
-  protected double m_period;
-
   private enum Mode {
     kNone,
     kDisabled,
@@ -53,8 +61,11 @@
     kTest
   }
 
+  private final DSControlWord m_word = new DSControlWord();
   private Mode m_lastMode = Mode.kNone;
+  private final double m_period;
   private final Watchdog m_watchdog;
+  private boolean m_ntFlushEnabled;
 
   /**
    * Constructor for IterativeRobotBase.
@@ -66,9 +77,7 @@
     m_watchdog = new Watchdog(period, this::printLoopOverrunMessage);
   }
 
-  /**
-   * Provide an alternate "main loop" via startCompetition().
-   */
+  /** Provide an alternate "main loop" via startCompetition(). */
   @Override
   public abstract void startCompetition();
 
@@ -84,21 +93,16 @@
    * until RobotInit() exits. Code in RobotInit() that waits for enable will cause the robot to
    * never indicate that the code is ready, causing the robot to be bypassed in a match.
    */
-  public void robotInit() {
-    System.out.println("Default robotInit() method... Override me!");
-  }
+  public void robotInit() {}
 
   /**
    * Robot-wide simulation initialization code should go here.
    *
-   * <p>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.
+   * <p>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.
    */
-  public void simulationInit() {
-    System.out.println("Default simulationInit() method... Override me!");
-  }
+  public void simulationInit() {}
 
   /**
    * Initialization code for disabled mode should go here.
@@ -106,9 +110,7 @@
    * <p>Users should override this method for initialization code which will be called each time the
    * robot enters disabled mode.
    */
-  public void disabledInit() {
-    System.out.println("Default disabledInit() method... Override me!");
-  }
+  public void disabledInit() {}
 
   /**
    * Initialization code for autonomous mode should go here.
@@ -116,9 +118,7 @@
    * <p>Users should override this method for initialization code which will be called each time the
    * robot enters autonomous mode.
    */
-  public void autonomousInit() {
-    System.out.println("Default autonomousInit() method... Override me!");
-  }
+  public void autonomousInit() {}
 
   /**
    * Initialization code for teleop mode should go here.
@@ -126,9 +126,7 @@
    * <p>Users should override this method for initialization code which will be called each time the
    * robot enters teleop mode.
    */
-  public void teleopInit() {
-    System.out.println("Default teleopInit() method... Override me!");
-  }
+  public void teleopInit() {}
 
   /**
    * Initialization code for test mode should go here.
@@ -137,17 +135,13 @@
    * robot enters test mode.
    */
   @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
-  public void testInit() {
-    System.out.println("Default testInit() method... Override me!");
-  }
+  public void testInit() {}
 
   /* ----------- Overridable periodic code ----------------- */
 
   private boolean m_rpFirstRun = true;
 
-  /**
-   * Periodic code for all robot modes should go here.
-   */
+  /** Periodic code for all robot modes should go here. */
   public void robotPeriodic() {
     if (m_rpFirstRun) {
       System.out.println("Default robotPeriodic() method... Override me!");
@@ -171,9 +165,7 @@
 
   private boolean m_dpFirstRun = true;
 
-  /**
-   * Periodic code for disabled mode should go here.
-   */
+  /** Periodic code for disabled mode should go here. */
   public void disabledPeriodic() {
     if (m_dpFirstRun) {
       System.out.println("Default disabledPeriodic() method... Override me!");
@@ -183,9 +175,7 @@
 
   private boolean m_apFirstRun = true;
 
-  /**
-   * Periodic code for autonomous mode should go here.
-   */
+  /** Periodic code for autonomous mode should go here. */
   public void autonomousPeriodic() {
     if (m_apFirstRun) {
       System.out.println("Default autonomousPeriodic() method... Override me!");
@@ -195,9 +185,7 @@
 
   private boolean m_tpFirstRun = true;
 
-  /**
-   * Periodic code for teleop mode should go here.
-   */
+  /** Periodic code for teleop mode should go here. */
   public void teleopPeriodic() {
     if (m_tpFirstRun) {
       System.out.println("Default teleopPeriodic() method... Override me!");
@@ -207,9 +195,7 @@
 
   private boolean m_tmpFirstRun = true;
 
-  /**
-   * Periodic code for test mode should go here.
-   */
+  /** Periodic code for test mode should go here. */
   @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
   public void testPeriodic() {
     if (m_tmpFirstRun) {
@@ -218,64 +204,122 @@
     }
   }
 
-  @SuppressWarnings("PMD.CyclomaticComplexity")
+  /**
+   * Exit code for disabled mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * disabled mode.
+   */
+  public void disabledExit() {}
+
+  /**
+   * Exit code for autonomous mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * autonomous mode.
+   */
+  public void autonomousExit() {}
+
+  /**
+   * Exit code for teleop mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * teleop mode.
+   */
+  public void teleopExit() {}
+
+  /**
+   * Exit code for test mode should go here.
+   *
+   * <p>Users should override this method for code which will be called each time the robot exits
+   * test mode.
+   */
+  @SuppressWarnings("PMD.JUnit4TestShouldUseTestAnnotation")
+  public void testExit() {}
+
+  /**
+   * Enables or disables flushing NetworkTables every loop iteration. By default, this is disabled.
+   *
+   * @param enabled True to enable, false to disable
+   */
+  public void setNetworkTablesFlushEnabled(boolean enabled) {
+    m_ntFlushEnabled = enabled;
+  }
+
+  /**
+   * Gets time period between calls to Periodic() functions.
+   *
+   * @return The time period between calls to Periodic() functions.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
+
   protected void loopFunc() {
     m_watchdog.reset();
 
-    // Call the appropriate function depending upon the current robot mode
-    if (isDisabled()) {
-      // Call DisabledInit() if we are now just entering disabled mode from either a different mode
-      // or from power-on.
-      if (m_lastMode != Mode.kDisabled) {
+    // Get current mode
+    m_word.update();
+    Mode mode = Mode.kNone;
+    if (m_word.isDisabled()) {
+      mode = Mode.kDisabled;
+    } else if (m_word.isAutonomous()) {
+      mode = Mode.kAutonomous;
+    } else if (m_word.isTeleop()) {
+      mode = Mode.kTeleop;
+    } else if (m_word.isTest()) {
+      mode = Mode.kTest;
+    }
+
+    // If mode changed, call mode exit and entry functions
+    if (m_lastMode != mode) {
+      // Call last mode's exit function
+      if (m_lastMode == Mode.kDisabled) {
+        disabledExit();
+      } else if (m_lastMode == Mode.kAutonomous) {
+        autonomousExit();
+      } else if (m_lastMode == Mode.kTeleop) {
+        teleopExit();
+      } else if (m_lastMode == Mode.kTest) {
         LiveWindow.setEnabled(false);
         Shuffleboard.disableActuatorWidgets();
+        testExit();
+      }
+
+      // Call current mode's entry function
+      if (mode == Mode.kDisabled) {
         disabledInit();
         m_watchdog.addEpoch("disabledInit()");
-        m_lastMode = Mode.kDisabled;
-      }
-
-      HAL.observeUserProgramDisabled();
-      disabledPeriodic();
-      m_watchdog.addEpoch("disablePeriodic()");
-    } else if (isAutonomous()) {
-      // Call AutonomousInit() if we are now just entering autonomous mode from either a different
-      // mode or from power-on.
-      if (m_lastMode != Mode.kAutonomous) {
-        LiveWindow.setEnabled(false);
-        Shuffleboard.disableActuatorWidgets();
+      } else if (mode == Mode.kAutonomous) {
         autonomousInit();
         m_watchdog.addEpoch("autonomousInit()");
-        m_lastMode = Mode.kAutonomous;
-      }
-
-      HAL.observeUserProgramAutonomous();
-      autonomousPeriodic();
-      m_watchdog.addEpoch("autonomousPeriodic()");
-    } else if (isOperatorControl()) {
-      // Call TeleopInit() if we are now just entering teleop mode from either a different mode or
-      // from power-on.
-      if (m_lastMode != Mode.kTeleop) {
-        LiveWindow.setEnabled(false);
-        Shuffleboard.disableActuatorWidgets();
+      } else if (mode == Mode.kTeleop) {
         teleopInit();
         m_watchdog.addEpoch("teleopInit()");
-        m_lastMode = Mode.kTeleop;
-      }
-
-      HAL.observeUserProgramTeleop();
-      teleopPeriodic();
-      m_watchdog.addEpoch("teleopPeriodic()");
-    } else {
-      // Call TestInit() if we are now just entering test mode from either a different mode or from
-      // power-on.
-      if (m_lastMode != Mode.kTest) {
+      } else if (mode == Mode.kTest) {
         LiveWindow.setEnabled(true);
         Shuffleboard.enableActuatorWidgets();
         testInit();
         m_watchdog.addEpoch("testInit()");
-        m_lastMode = Mode.kTest;
       }
 
+      m_lastMode = mode;
+    }
+
+    // Call the appropriate function depending upon the current robot mode
+    if (mode == Mode.kDisabled) {
+      HAL.observeUserProgramDisabled();
+      disabledPeriodic();
+      m_watchdog.addEpoch("disabledPeriodic()");
+    } else if (mode == Mode.kAutonomous) {
+      HAL.observeUserProgramAutonomous();
+      autonomousPeriodic();
+      m_watchdog.addEpoch("autonomousPeriodic()");
+    } else if (mode == Mode.kTeleop) {
+      HAL.observeUserProgramTeleop();
+      teleopPeriodic();
+      m_watchdog.addEpoch("teleopPeriodic()");
+    } else {
       HAL.observeUserProgramTest();
       testPeriodic();
       m_watchdog.addEpoch("testPeriodic()");
@@ -292,12 +336,19 @@
     m_watchdog.addEpoch("Shuffleboard.update()");
 
     if (isSimulation()) {
+      HAL.simPeriodicBefore();
       simulationPeriodic();
+      HAL.simPeriodicAfter();
       m_watchdog.addEpoch("simulationPeriodic()");
     }
 
     m_watchdog.disable();
 
+    // Flush NetworkTables
+    if (m_ntFlushEnabled) {
+      NetworkTableInstance.getDefault().flush();
+    }
+
     // Warn on loop time overruns
     if (m_watchdog.isExpired()) {
       m_watchdog.printEpochs();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
deleted file mode 100644
index 8ce7b7d..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Jaguar.java
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Texas Instruments / Vex Robotics Jaguar Speed Controller as a PWM device.
- *
- * <p>Note that the Jaguar uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the Jaguar User Manual
- * available from Vex.
- *
- * <p><ul>
- * <li>2.310ms = full "forward"
- * <li>1.550ms = the "high end" of the deadband range
- * <li>1.507ms = center of the deadband range (off)
- * <li>1.454ms = the "low end" of the deadband range
- * <li>0.697ms = full "reverse"
- * </ul>
- */
-public class Jaguar extends PWMSpeedController {
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the Jaguar is attached to. 0-9 are on-board, 10-19 are on
-   *                the MXP port
-   */
-  public Jaguar(final int channel) {
-    super(channel);
-
-    setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_Jaguar, getChannel() + 1);
-    SendableRegistry.setName(this, "Jaguar", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
index a83a346..e9b09a9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Joystick.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,7 +8,7 @@
 import edu.wpi.first.hal.HAL;
 
 /**
- * Handle input from standard Joysticks connected to the Driver Station.
+ * Handle input from Flight Joysticks connected to the Driver Station.
  *
  * <p>This class handles standard input that comes from the Driver Station. Each time a value is
  * requested the most recent value is returned. There is a single class instance for each joystick
@@ -24,13 +21,14 @@
   public static final byte kDefaultTwistChannel = 2;
   public static final byte kDefaultThrottleChannel = 3;
 
-  /**
-   * Represents an analog axis on a joystick.
-   */
+  /** Represents an analog axis on a joystick. */
   public enum AxisType {
-    kX(0), kY(1), kZ(2), kTwist(3), kThrottle(4);
+    kX(0),
+    kY(1),
+    kZ(2),
+    kTwist(3),
+    kThrottle(4);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     AxisType(int value) {
@@ -38,13 +36,11 @@
     }
   }
 
-  /**
-   * Represents a digital button on a joystick.
-   */
+  /** Represents a digital button on a joystick. */
   public enum ButtonType {
-    kTrigger(1), kTop(2);
+    kTrigger(1),
+    kTop(2);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     ButtonType(int value) {
@@ -52,50 +48,21 @@
     }
   }
 
-  /**
-   * Represents a digital button on a joystick.
-   */
-  private enum Button {
-    kTrigger(1), kTop(2);
-
-    @SuppressWarnings("MemberName")
-    public final int value;
-
-    Button(int value) {
-      this.value = value;
-    }
-  }
+  private final byte[] m_axes = new byte[AxisType.values().length];
 
   /**
-   * Represents an analog axis on a joystick.
-   */
-  private enum Axis {
-    kX(0), kY(1), kZ(2), kTwist(3), kThrottle(4), kNumAxes(5);
-
-    @SuppressWarnings("MemberName")
-    public final int value;
-
-    Axis(int value) {
-      this.value = value;
-    }
-  }
-
-  private final byte[] m_axes = new byte[Axis.kNumAxes.value];
-
-  /**
-   * Construct an instance of a joystick. The joystick index is the USB port on the drivers
-   * station.
+   * Construct an instance of a joystick.
    *
-   * @param port The port on the Driver Station that the joystick is plugged into.
+   * @param port The port index on the Driver Station that the joystick is plugged into.
    */
   public Joystick(final int port) {
     super(port);
 
-    m_axes[Axis.kX.value] = kDefaultXChannel;
-    m_axes[Axis.kY.value] = kDefaultYChannel;
-    m_axes[Axis.kZ.value] = kDefaultZChannel;
-    m_axes[Axis.kTwist.value] = kDefaultTwistChannel;
-    m_axes[Axis.kThrottle.value] = kDefaultThrottleChannel;
+    m_axes[AxisType.kX.value] = kDefaultXChannel;
+    m_axes[AxisType.kY.value] = kDefaultYChannel;
+    m_axes[AxisType.kZ.value] = kDefaultZChannel;
+    m_axes[AxisType.kTwist.value] = kDefaultTwistChannel;
+    m_axes[AxisType.kThrottle.value] = kDefaultThrottleChannel;
 
     HAL.report(tResourceType.kResourceType_Joystick, port + 1);
   }
@@ -106,7 +73,7 @@
    * @param channel The channel to set the axis to.
    */
   public void setXChannel(int channel) {
-    m_axes[Axis.kX.value] = (byte) channel;
+    m_axes[AxisType.kX.value] = (byte) channel;
   }
 
   /**
@@ -115,7 +82,7 @@
    * @param channel The channel to set the axis to.
    */
   public void setYChannel(int channel) {
-    m_axes[Axis.kY.value] = (byte) channel;
+    m_axes[AxisType.kY.value] = (byte) channel;
   }
 
   /**
@@ -124,7 +91,7 @@
    * @param channel The channel to set the axis to.
    */
   public void setZChannel(int channel) {
-    m_axes[Axis.kZ.value] = (byte) channel;
+    m_axes[AxisType.kZ.value] = (byte) channel;
   }
 
   /**
@@ -133,7 +100,7 @@
    * @param channel The channel to set the axis to.
    */
   public void setThrottleChannel(int channel) {
-    m_axes[Axis.kThrottle.value] = (byte) channel;
+    m_axes[AxisType.kThrottle.value] = (byte) channel;
   }
 
   /**
@@ -142,7 +109,7 @@
    * @param channel The channel to set the axis to.
    */
   public void setTwistChannel(int channel) {
-    m_axes[Axis.kTwist.value] = (byte) channel;
+    m_axes[AxisType.kTwist.value] = (byte) channel;
   }
 
   /**
@@ -151,7 +118,7 @@
    * @return The channel for the axis.
    */
   public int getXChannel() {
-    return m_axes[Axis.kX.value];
+    return m_axes[AxisType.kX.value];
   }
 
   /**
@@ -160,7 +127,7 @@
    * @return The channel for the axis.
    */
   public int getYChannel() {
-    return m_axes[Axis.kY.value];
+    return m_axes[AxisType.kY.value];
   }
 
   /**
@@ -169,7 +136,7 @@
    * @return The channel for the axis.
    */
   public int getZChannel() {
-    return m_axes[Axis.kZ.value];
+    return m_axes[AxisType.kZ.value];
   }
 
   /**
@@ -178,7 +145,7 @@
    * @return The channel for the axis.
    */
   public int getTwistChannel() {
-    return m_axes[Axis.kTwist.value];
+    return m_axes[AxisType.kTwist.value];
   }
 
   /**
@@ -187,31 +154,27 @@
    * @return The channel for the axis.
    */
   public int getThrottleChannel() {
-    return m_axes[Axis.kThrottle.value];
+    return m_axes[AxisType.kThrottle.value];
   }
 
   /**
    * Get the X value of the joystick. This depends on the mapping of the joystick connected to the
    * current port.
    *
-   * @param hand Unused
    * @return The X value of the joystick.
    */
-  @Override
-  public final double getX(Hand hand) {
-    return getRawAxis(m_axes[Axis.kX.value]);
+  public final double getX() {
+    return getRawAxis(m_axes[AxisType.kX.value]);
   }
 
   /**
    * Get the Y value of the joystick. This depends on the mapping of the joystick connected to the
    * current port.
    *
-   * @param hand Unused
    * @return The Y value of the joystick.
    */
-  @Override
-  public final double getY(Hand hand) {
-    return getRawAxis(m_axes[Axis.kY.value]);
+  public final double getY() {
+    return getRawAxis(m_axes[AxisType.kY.value]);
   }
 
   /**
@@ -220,7 +183,7 @@
    * @return the z position
    */
   public double getZ() {
-    return getRawAxis(m_axes[Axis.kZ.value]);
+    return getRawAxis(m_axes[AxisType.kZ.value]);
   }
 
   /**
@@ -230,7 +193,7 @@
    * @return The Twist value of the joystick.
    */
   public double getTwist() {
-    return getRawAxis(m_axes[Axis.kTwist.value]);
+    return getRawAxis(m_axes[AxisType.kTwist.value]);
   }
 
   /**
@@ -240,7 +203,7 @@
    * @return The Throttle value of the joystick.
    */
   public double getThrottle() {
-    return getRawAxis(m_axes[Axis.kThrottle.value]);
+    return getRawAxis(m_axes[AxisType.kThrottle.value]);
   }
 
   /**
@@ -249,7 +212,7 @@
    * @return The state of the trigger.
    */
   public boolean getTrigger() {
-    return getRawButton(Button.kTrigger.value);
+    return getRawButton(ButtonType.kTrigger.value);
   }
 
   /**
@@ -258,7 +221,7 @@
    * @return Whether the button was pressed since the last check.
    */
   public boolean getTriggerPressed() {
-    return getRawButtonPressed(Button.kTrigger.value);
+    return getRawButtonPressed(ButtonType.kTrigger.value);
   }
 
   /**
@@ -267,7 +230,7 @@
    * @return Whether the button was released since the last check.
    */
   public boolean getTriggerReleased() {
-    return getRawButtonReleased(Button.kTrigger.value);
+    return getRawButtonReleased(ButtonType.kTrigger.value);
   }
 
   /**
@@ -276,7 +239,7 @@
    * @return The state of the top button.
    */
   public boolean getTop() {
-    return getRawButton(Button.kTop.value);
+    return getRawButton(ButtonType.kTop.value);
   }
 
   /**
@@ -285,7 +248,7 @@
    * @return Whether the button was pressed since the last check.
    */
   public boolean getTopPressed() {
-    return getRawButtonPressed(Button.kTop.value);
+    return getRawButtonPressed(ButtonType.kTop.value);
   }
 
   /**
@@ -294,7 +257,7 @@
    * @return Whether the button was released since the last check.
    */
   public boolean getTopReleased() {
-    return getRawButtonReleased(Button.kTop.value);
+    return getRawButtonReleased(ButtonType.kTop.value);
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
index 5dd3a4f..4e76936 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/MotorSafety.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,8 +8,8 @@
 import java.util.Set;
 
 /**
- * This base class runs a watchdog timer and calls the subclass's StopMotor()
- * function if the timeout expires.
+ * This base class runs a watchdog timer and calls the subclass's StopMotor() function if the
+ * timeout expires.
  *
  * <p>The subclass should call feed() whenever the motor value is updated.
  */
@@ -26,9 +23,7 @@
   private static final Set<MotorSafety> m_instanceList = new LinkedHashSet<>();
   private static final Object m_listMutex = new Object();
 
-  /**
-   * MotorSafety constructor.
-   */
+  /** MotorSafety constructor. */
   public MotorSafety() {
     synchronized (m_listMutex) {
       m_instanceList.add(this);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
deleted file mode 100644
index 9d1011b..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/NidecBrushless.java
+++ /dev/null
@@ -1,157 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Nidec Brushless Motor.
- */
-public class NidecBrushless extends MotorSafety implements SpeedController, Sendable,
-    AutoCloseable {
-  private boolean m_isInverted;
-  private final DigitalOutput m_dio;
-  private final PWM m_pwm;
-  private volatile double m_speed;
-  private volatile boolean m_disabled;
-
-  /**
-   * Constructor.
-   *
-   * @param pwmChannel The PWM channel that the Nidec Brushless controller is attached to.
-   *                   0-9 are on-board, 10-19 are on the MXP port
-   * @param dioChannel The DIO channel that the Nidec Brushless controller is attached to.
-   *                   0-9 are on-board, 10-25 are on the MXP port
-   */
-  public NidecBrushless(final int pwmChannel, final int dioChannel) {
-    setSafetyEnabled(false);
-
-    // the dio controls the output (in PWM mode)
-    m_dio = new DigitalOutput(dioChannel);
-    SendableRegistry.addChild(this, m_dio);
-    m_dio.setPWMRate(15625);
-    m_dio.enablePWM(0.5);
-
-    // the pwm enables the controller
-    m_pwm = new PWM(pwmChannel);
-    SendableRegistry.addChild(this, m_pwm);
-
-    HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
-    SendableRegistry.addLW(this, "Nidec Brushless", pwmChannel);
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-    m_dio.close();
-    m_pwm.close();
-  }
-
-  /**
-   * Set the PWM value.
-   *
-   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
-   * FPGA.
-   *
-   * @param speed The speed value between -1.0 and 1.0 to set.
-   */
-  @Override
-  public void set(double speed) {
-    if (!m_disabled) {
-      m_speed = speed;
-      m_dio.updateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
-      m_pwm.setRaw(0xffff);
-    }
-
-    feed();
-  }
-
-  /**
-   * Get the recently set value of the PWM.
-   *
-   * @return The most recently set value for the PWM between -1.0 and 1.0.
-   */
-  @Override
-  public double get() {
-    return m_speed;
-  }
-
-  @Override
-  public void setInverted(boolean isInverted) {
-    m_isInverted = isInverted;
-  }
-
-  @Override
-  public boolean getInverted() {
-    return m_isInverted;
-  }
-
-  /**
-   * Write out the PID value as seen in the PIDOutput base object.
-   *
-   * @param output Write out the PWM value as was found in the PIDController
-   */
-  @Override
-  public void pidWrite(double output) {
-    set(output);
-  }
-
-  /**
-   * Stop the motor. This is called by the MotorSafety object when it has a timeout for this PWM and
-   * needs to stop it from running. Calling set() will re-enable the motor.
-   */
-  @Override
-  public void stopMotor() {
-    m_dio.updateDutyCycle(0.5);
-    m_pwm.setDisabled();
-  }
-
-  @Override
-  public String getDescription() {
-    return "Nidec " + getChannel();
-  }
-
-  /**
-   * Disable the motor.  The enable() function must be called to re-enable
-   * the motor.
-   */
-  @Override
-  public void disable() {
-    m_disabled = true;
-    m_dio.updateDutyCycle(0.5);
-    m_pwm.setDisabled();
-  }
-
-  /**
-   * Re-enable the motor after disable() has been called.  The set()
-   * function must be called to set a new motor speed.
-   */
-  public void enable() {
-    m_disabled = false;
-  }
-
-  /**
-   * Gets the channel number associated with the object.
-   *
-   * @return The channel number.
-   */
-  public int getChannel() {
-    return m_pwm.getChannel();
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Nidec Brushless");
-    builder.setActuator(true);
-    builder.setSafeState(this::stopMotor);
-    builder.addDoubleProperty("Value", this::get, this::set);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
index 546da5b..40384ba 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Notifier.java
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.concurrent.atomic.AtomicInteger;
-import java.util.concurrent.locks.ReentrantLock;
+import static java.util.Objects.requireNonNull;
 
 import edu.wpi.first.hal.NotifierJNI;
-
-import static java.util.Objects.requireNonNull;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.locks.ReentrantLock;
 
 public class Notifier implements AutoCloseable {
   // The thread waiting on the HAL alarm.
@@ -22,9 +18,9 @@
   // The C pointer to the notifier object. We don't use it directly, it is
   // just passed to the JNI bindings.
   private final AtomicInteger m_notifier = new AtomicInteger();
-  // The time, in microseconds, at which the corresponding handler should be
-  // called. Has the same zero as Utility.getFPGATime().
-  private double m_expirationTime;
+  // The time, in seconds, at which the corresponding handler should be
+  // called. Has the same zero as RobotController.getFPGATime().
+  private double m_expirationTimeSeconds;
   // The handler passed in by the user which should be called at the
   // appropriate interval.
   private Runnable m_handler;
@@ -32,7 +28,7 @@
   private boolean m_periodic;
   // If periodic, the period of the calling; if just once, stores how long it
   // is until we call the handler.
-  private double m_period;
+  private double m_periodSeconds;
 
   @Override
   @SuppressWarnings("NoFinalizer")
@@ -63,28 +59,26 @@
   /**
    * Update the alarm hardware to reflect the next alarm.
    *
-   * @param triggerTime the time at which the next alarm will be triggered
+   * @param triggerTimeMicroS the time in microseconds at which the next alarm will be triggered
    */
-  private void updateAlarm(long triggerTime) {
+  private void updateAlarm(long triggerTimeMicroS) {
     int notifier = m_notifier.get();
     if (notifier == 0) {
       return;
     }
-    NotifierJNI.updateNotifierAlarm(notifier, triggerTime);
+    NotifierJNI.updateNotifierAlarm(notifier, triggerTimeMicroS);
   }
 
-  /**
-   * Update the alarm hardware to reflect the next alarm.
-   */
+  /** Update the alarm hardware to reflect the next alarm. */
   private void updateAlarm() {
-    updateAlarm((long) (m_expirationTime * 1e6));
+    updateAlarm((long) (m_expirationTimeSeconds * 1e6));
   }
 
   /**
    * Create a Notifier for timer event notification.
    *
-   * @param run The handler that is called at the notification time which is set
-   *            using StartSingle or StartPeriodic.
+   * @param run The handler that is called at the notification time which is set using StartSingle
+   *     or StartPeriodic.
    */
   public Notifier(Runnable run) {
     requireNonNull(run);
@@ -92,54 +86,59 @@
     m_handler = run;
     m_notifier.set(NotifierJNI.initializeNotifier());
 
-    m_thread = new Thread(() -> {
-      while (!Thread.interrupted()) {
-        int notifier = m_notifier.get();
-        if (notifier == 0) {
-          break;
-        }
-        long curTime = NotifierJNI.waitForNotifierAlarm(notifier);
-        if (curTime == 0) {
-          break;
-        }
+    m_thread =
+        new Thread(
+            () -> {
+              while (!Thread.interrupted()) {
+                int notifier = m_notifier.get();
+                if (notifier == 0) {
+                  break;
+                }
+                long curTime = NotifierJNI.waitForNotifierAlarm(notifier);
+                if (curTime == 0) {
+                  break;
+                }
 
-        Runnable handler = null;
-        m_processLock.lock();
-        try {
-          handler = m_handler;
-          if (m_periodic) {
-            m_expirationTime += m_period;
-            updateAlarm();
-          } else {
-            // need to update the alarm to cause it to wait again
-            updateAlarm((long) -1);
-          }
-        } finally {
-          m_processLock.unlock();
-        }
+                Runnable handler = null;
+                m_processLock.lock();
+                try {
+                  handler = m_handler;
+                  if (m_periodic) {
+                    m_expirationTimeSeconds += m_periodSeconds;
+                    updateAlarm();
+                  } else {
+                    // need to update the alarm to cause it to wait again
+                    updateAlarm((long) -1);
+                  }
+                } finally {
+                  m_processLock.unlock();
+                }
 
-        if (handler != null) {
-          handler.run();
-        }
-      }
-    });
+                if (handler != null) {
+                  handler.run();
+                }
+              }
+            });
     m_thread.setName("Notifier");
     m_thread.setDaemon(true);
-    m_thread.setUncaughtExceptionHandler((thread, error) -> {
-      Throwable cause = error.getCause();
-      if (cause != null) {
-        error = cause;
-      }
-      DriverStation.reportError("Unhandled exception: " + error.toString(), error.getStackTrace());
-      DriverStation.reportError(
-          "The loopFunc() method (or methods called by it) should have handled "
-              + "the exception above.", false);
-    });
+    m_thread.setUncaughtExceptionHandler(
+        (thread, error) -> {
+          Throwable cause = error.getCause();
+          if (cause != null) {
+            error = cause;
+          }
+          DriverStation.reportError(
+              "Unhandled exception: " + error.toString(), error.getStackTrace());
+          DriverStation.reportError(
+              "The loopFunc() method (or methods called by it) should have handled "
+                  + "the exception above.",
+              false);
+        });
     m_thread.start();
   }
 
   /**
-   * Sets the name of the notifier.  Used for debugging purposes only.
+   * Sets the name of the notifier. Used for debugging purposes only.
    *
    * @param name Name
    */
@@ -163,17 +162,17 @@
   }
 
   /**
-   * Register for single event notification. A timer event is queued for a single
-   * event after the specified delay.
+   * Register for single event notification. A timer event is queued for a single event after the
+   * specified delay.
    *
-   * @param delay Seconds to wait before the handler is called.
+   * @param delaySeconds Seconds to wait before the handler is called.
    */
-  public void startSingle(double delay) {
+  public void startSingle(double delaySeconds) {
     m_processLock.lock();
     try {
       m_periodic = false;
-      m_period = delay;
-      m_expirationTime = RobotController.getFPGATime() * 1e-6 + delay;
+      m_periodSeconds = delaySeconds;
+      m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + delaySeconds;
       updateAlarm();
     } finally {
       m_processLock.unlock();
@@ -181,19 +180,19 @@
   }
 
   /**
-   * Register for periodic event notification. A timer event is queued for
-   * periodic event notification. Each time the interrupt occurs, the event will
-   * be immediately requeued for the same time interval.
+   * Register for periodic event notification. A timer event is queued for periodic event
+   * notification. Each time the interrupt occurs, the event will be immediately requeued for the
+   * same time interval.
    *
-   * @param period Period in seconds to call the handler starting one period after
-   *               the call to this method.
+   * @param periodSeconds Period in seconds to call the handler starting one period after the call
+   *     to this method.
    */
-  public void startPeriodic(double period) {
+  public void startPeriodic(double periodSeconds) {
     m_processLock.lock();
     try {
       m_periodic = true;
-      m_period = period;
-      m_expirationTime = RobotController.getFPGATime() * 1e-6 + period;
+      m_periodSeconds = periodSeconds;
+      m_expirationTimeSeconds = RobotController.getFPGATime() * 1e-6 + periodSeconds;
       updateAlarm();
     } finally {
       m_processLock.unlock();
@@ -201,10 +200,9 @@
   }
 
   /**
-   * Stop timer events from occurring. 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
-   * function will block until the handler call is complete.
+   * Stop timer events from occurring. 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 function will block until the handler call is complete.
    */
   public void stop() {
     m_processLock.lock();
@@ -215,4 +213,21 @@
       m_processLock.unlock();
     }
   }
+
+  /**
+   * Sets the HAL notifier thread priority.
+   *
+   * <p>The HAL notifier thread is responsible for managing the FPGA's notifier interrupt and waking
+   * up user's Notifiers when it's their time to run. Giving the HAL notifier thread real-time
+   * priority helps ensure the user's real-time Notifiers, if any, are notified to run in a timely
+   * manner.
+   *
+   * @param realTime Set to true to set a real-time priority, false for standard priority.
+   * @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being
+   *     highest. For non-real-time, this is forced to 0. See "man 7 sched" for more details.
+   * @return True on success.
+   */
+  public static boolean setHALThreadPriority(boolean realTime, int priority) {
+    return NotifierJNI.setHALThreadPriority(realTime, priority);
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
deleted file mode 100644
index 40eb42f..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDBase.java
+++ /dev/null
@@ -1,829 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.concurrent.locks.ReentrantLock;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.util.BoundaryException;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
-/**
- * Class implements a PID Control Loop.
- *
- * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
- * calculations, as well as writing the given PIDOutput.
- *
- * <p>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.
- */
-@Deprecated(since = "2020", forRemoval = true)
-@SuppressWarnings("PMD.TooManyFields")
-public class PIDBase implements PIDInterface, PIDOutput, Sendable, AutoCloseable {
-  public static final double kDefaultPeriod = 0.05;
-  private static int instances;
-
-  // Factor for "proportional" control
-  @SuppressWarnings("MemberName")
-  private double m_P;
-
-  // Factor for "integral" control
-  @SuppressWarnings("MemberName")
-  private double m_I;
-
-  // Factor for "derivative" control
-  @SuppressWarnings("MemberName")
-  private double m_D;
-
-  // Factor for "feed forward" control
-  @SuppressWarnings("MemberName")
-  private double m_F;
-
-  // |maximum output|
-  private double m_maximumOutput = 1.0;
-
-  // |minimum output|
-  private double m_minimumOutput = -1.0;
-
-  // Maximum input - limit setpoint to this
-  private double m_maximumInput;
-
-  // Minimum input - limit setpoint to this
-  private double m_minimumInput;
-
-  // Input range - difference between maximum and minimum
-  private double m_inputRange;
-
-  // Do the endpoints wrap around? (e.g., absolute encoder)
-  private boolean m_continuous;
-
-  // Is the PID controller enabled
-  protected boolean m_enabled;
-
-  // The prior error (used to compute velocity)
-  private double m_prevError;
-
-  // The sum of the errors for use in the integral calc
-  private double m_totalError;
-
-  // The tolerance object used to check if on target
-  private Tolerance m_tolerance;
-
-  private double m_setpoint;
-  private double m_prevSetpoint;
-  @SuppressWarnings("PMD.UnusedPrivateField")
-  private double m_error;
-  private double m_result;
-
-  private LinearFilter m_filter;
-
-  protected ReentrantLock m_thisMutex = new ReentrantLock();
-
-  // Ensures when disable() is called, pidWrite() won't run if calculate()
-  // is already running at that time.
-  protected ReentrantLock m_pidWriteMutex = new ReentrantLock();
-
-  protected PIDSource m_pidInput;
-  protected PIDOutput m_pidOutput;
-  protected Timer m_setpointTimer;
-
-  /**
-   * Tolerance is the type of tolerance used to specify if the PID controller is on target.
-   *
-   * <p>The various implementations of this class such as PercentageTolerance and AbsoluteTolerance
-   * specify types of tolerance specifications to use.
-   */
-  public interface Tolerance {
-    boolean onTarget();
-  }
-
-  /**
-   * Used internally for when Tolerance hasn't been set.
-   */
-  public static class NullTolerance implements Tolerance {
-    @Override
-    public boolean onTarget() {
-      throw new IllegalStateException("No tolerance value set when calling onTarget().");
-    }
-  }
-
-  public class PercentageTolerance implements Tolerance {
-    private final double m_percentage;
-
-    PercentageTolerance(double value) {
-      m_percentage = value;
-    }
-
-    @Override
-    public boolean onTarget() {
-      return Math.abs(getError()) < m_percentage / 100 * m_inputRange;
-    }
-  }
-
-  public class AbsoluteTolerance implements Tolerance {
-    private final double m_value;
-
-    AbsoluteTolerance(double value) {
-      m_value = value;
-    }
-
-    @Override
-    public boolean onTarget() {
-      return Math.abs(getError()) < m_value;
-    }
-  }
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D, and F.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param Kf     the feed forward term
-   * @param source The PIDSource object that is used to get values
-   * @param output The PIDOutput object that is set to the output percentage
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDBase(double Kp, double Ki, double Kd, double Kf, PIDSource source,
-                 PIDOutput output) {
-    requireNonNullParam(source, "PIDSource", "PIDBase");
-    requireNonNullParam(output, "output", "PIDBase");
-
-    m_setpointTimer = new Timer();
-    m_setpointTimer.start();
-
-    m_P = Kp;
-    m_I = Ki;
-    m_D = Kd;
-    m_F = Kf;
-
-    m_pidInput = source;
-    m_filter = LinearFilter.movingAverage(1);
-
-    m_pidOutput = output;
-
-    instances++;
-    HAL.report(tResourceType.kResourceType_PIDController, instances);
-    m_tolerance = new NullTolerance();
-    SendableRegistry.add(this, "PIDController", instances);
-  }
-
-  /**
-   * Allocate a PID object with the given constants for P, I, and D.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source the PIDSource object that is used to get values
-   * @param output the PIDOutput object that is set to the output percentage
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDBase(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
-    this(Kp, Ki, Kd, 0.0, source, output);
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-  }
-
-  /**
-   * Read the input, calculate the output accordingly, and write to the output. This should only be
-   * called by the PIDTask and is created during initialization.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength", "PMD.NPathComplexity"})
-  protected void calculate() {
-    if (m_pidInput == null || m_pidOutput == null) {
-      return;
-    }
-
-    boolean enabled;
-
-    m_thisMutex.lock();
-    try {
-      enabled = m_enabled;
-    } finally {
-      m_thisMutex.unlock();
-    }
-
-    if (enabled) {
-      double input;
-
-      // Storage for function inputs
-      PIDSourceType pidSourceType;
-      double P;
-      double I;
-      double D;
-      double feedForward = calculateFeedForward();
-      double minimumOutput;
-      double maximumOutput;
-
-      // Storage for function input-outputs
-      double prevError;
-      double error;
-      double totalError;
-
-      m_thisMutex.lock();
-      try {
-        input = m_filter.calculate(m_pidInput.pidGet());
-
-        pidSourceType = m_pidInput.getPIDSourceType();
-        P = m_P;
-        I = m_I;
-        D = m_D;
-        minimumOutput = m_minimumOutput;
-        maximumOutput = m_maximumOutput;
-
-        prevError = m_prevError;
-        error = getContinuousError(m_setpoint - input);
-        totalError = m_totalError;
-      } finally {
-        m_thisMutex.unlock();
-      }
-
-      // Storage for function outputs
-      double result;
-
-      if (pidSourceType.equals(PIDSourceType.kRate)) {
-        if (P != 0) {
-          totalError = clamp(totalError + error, minimumOutput / P,
-              maximumOutput / P);
-        }
-
-        result = P * totalError + D * error + feedForward;
-      } else {
-        if (I != 0) {
-          totalError = clamp(totalError + error, minimumOutput / I,
-              maximumOutput / I);
-        }
-
-        result = P * error + I * totalError + D * (error - prevError)
-            + feedForward;
-      }
-
-      result = clamp(result, minimumOutput, maximumOutput);
-
-      // Ensures m_enabled check and pidWrite() call occur atomically
-      m_pidWriteMutex.lock();
-      try {
-        m_thisMutex.lock();
-        try {
-          if (m_enabled) {
-            // Don't block other PIDController operations on pidWrite()
-            m_thisMutex.unlock();
-
-            m_pidOutput.pidWrite(result);
-          }
-        } finally {
-          if (m_thisMutex.isHeldByCurrentThread()) {
-            m_thisMutex.unlock();
-          }
-        }
-      } finally {
-        m_pidWriteMutex.unlock();
-      }
-
-      m_thisMutex.lock();
-      try {
-        m_prevError = error;
-        m_error = error;
-        m_totalError = totalError;
-        m_result = result;
-      } finally {
-        m_thisMutex.unlock();
-      }
-    }
-  }
-
-  /**
-   * Calculate the feed forward term.
-   *
-   * <p>Both of the provided feed forward calculations are velocity feed forwards. If a different
-   * feed forward calculation is desired, the user can override this function and provide his or
-   * her own. This function  does no synchronization because the PIDController class only calls it
-   * in synchronized code, so be careful if calling it oneself.
-   *
-   * <p>If a velocity PID controller is being used, the F term should be set to 1 over the maximum
-   * setpoint for the output. If a position PID controller is being used, the F term should be set
-   * to 1 over the maximum speed for the output measured in setpoint units per this controller's
-   * update period (see the default period in this class's constructor).
-   */
-  protected double calculateFeedForward() {
-    if (m_pidInput.getPIDSourceType().equals(PIDSourceType.kRate)) {
-      return m_F * getSetpoint();
-    } else {
-      double temp = m_F * getDeltaSetpoint();
-      m_prevSetpoint = m_setpoint;
-      m_setpointTimer.reset();
-      return temp;
-    }
-  }
-
-  /**
-   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
-   * coefficients.
-   *
-   * @param p Proportional coefficient
-   * @param i Integral coefficient
-   * @param d Differential coefficient
-   */
-  @Override
-  @SuppressWarnings("ParameterName")
-  public void setPID(double p, double i, double d) {
-    m_thisMutex.lock();
-    try {
-      m_P = p;
-      m_I = i;
-      m_D = d;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the PID Controller gain parameters. Set the proportional, integral, and differential
-   * coefficients.
-   *
-   * @param p Proportional coefficient
-   * @param i Integral coefficient
-   * @param d Differential coefficient
-   * @param f Feed forward coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setPID(double p, double i, double d, double f) {
-    m_thisMutex.lock();
-    try {
-      m_P = p;
-      m_I = i;
-      m_D = d;
-      m_F = f;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the Proportional coefficient of the PID controller gain.
-   *
-   * @param p Proportional coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setP(double p) {
-    m_thisMutex.lock();
-    try {
-      m_P = p;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the Integral coefficient of the PID controller gain.
-   *
-   * @param i Integral coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setI(double i) {
-    m_thisMutex.lock();
-    try {
-      m_I = i;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the Differential coefficient of the PID controller gain.
-   *
-   * @param d differential coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setD(double d) {
-    m_thisMutex.lock();
-    try {
-      m_D = d;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the Feed forward coefficient of the PID controller gain.
-   *
-   * @param f feed forward coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setF(double f) {
-    m_thisMutex.lock();
-    try {
-      m_F = f;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Get the Proportional coefficient.
-   *
-   * @return proportional coefficient
-   */
-  @Override
-  public double getP() {
-    m_thisMutex.lock();
-    try {
-      return m_P;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Get the Integral coefficient.
-   *
-   * @return integral coefficient
-   */
-  @Override
-  public double getI() {
-    m_thisMutex.lock();
-    try {
-      return m_I;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Get the Differential coefficient.
-   *
-   * @return differential coefficient
-   */
-  @Override
-  public double getD() {
-    m_thisMutex.lock();
-    try {
-      return m_D;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Get the Feed forward coefficient.
-   *
-   * @return feed forward coefficient
-   */
-  public double getF() {
-    m_thisMutex.lock();
-    try {
-      return m_F;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Return the current PID result This is always centered on zero and constrained the the max and
-   * min outs.
-   *
-   * @return the latest calculated output
-   */
-  public double get() {
-    m_thisMutex.lock();
-    try {
-      return m_result;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the PID controller to consider the input to be continuous, Rather then using the max and
-   * min input range as constraints, it considers them to be the same point and automatically
-   * calculates the shortest route to the setpoint.
-   *
-   * @param continuous Set to true turns on continuous, false turns off continuous
-   */
-  public void setContinuous(boolean continuous) {
-    if (continuous && m_inputRange <= 0) {
-      throw new IllegalStateException("No input range set when calling setContinuous().");
-    }
-    m_thisMutex.lock();
-    try {
-      m_continuous = continuous;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the PID controller to consider the input to be continuous, Rather then using the max and
-   * min input range as constraints, it considers them to be the same point and automatically
-   * calculates the shortest route to the setpoint.
-   */
-  public void setContinuous() {
-    setContinuous(true);
-  }
-
-  /**
-   * Sets the maximum and minimum values expected from the input and setpoint.
-   *
-   * @param minimumInput the minimum value expected from the input
-   * @param maximumInput the maximum value expected from the input
-   */
-  public void setInputRange(double minimumInput, double maximumInput) {
-    m_thisMutex.lock();
-    try {
-      if (minimumInput > maximumInput) {
-        throw new BoundaryException("Lower bound is greater than upper bound");
-      }
-      m_minimumInput = minimumInput;
-      m_maximumInput = maximumInput;
-      m_inputRange = maximumInput - minimumInput;
-    } finally {
-      m_thisMutex.unlock();
-    }
-
-    setSetpoint(m_setpoint);
-  }
-
-  /**
-   * Sets the minimum and maximum values to write.
-   *
-   * @param minimumOutput the minimum percentage to write to the output
-   * @param maximumOutput the maximum percentage to write to the output
-   */
-  public void setOutputRange(double minimumOutput, double maximumOutput) {
-    m_thisMutex.lock();
-    try {
-      if (minimumOutput > maximumOutput) {
-        throw new BoundaryException("Lower bound is greater than upper bound");
-      }
-      m_minimumOutput = minimumOutput;
-      m_maximumOutput = maximumOutput;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the setpoint for the PIDController.
-   *
-   * @param setpoint the desired setpoint
-   */
-  @Override
-  public void setSetpoint(double setpoint) {
-    m_thisMutex.lock();
-    try {
-      if (m_maximumInput > m_minimumInput) {
-        if (setpoint > m_maximumInput) {
-          m_setpoint = m_maximumInput;
-        } else if (setpoint < m_minimumInput) {
-          m_setpoint = m_minimumInput;
-        } else {
-          m_setpoint = setpoint;
-        }
-      } else {
-        m_setpoint = setpoint;
-      }
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Returns the current setpoint of the PIDController.
-   *
-   * @return the current setpoint
-   */
-  @Override
-  public double getSetpoint() {
-    m_thisMutex.lock();
-    try {
-      return m_setpoint;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Returns the change in setpoint over time of the PIDController.
-   *
-   * @return the change in setpoint over time
-   */
-  public double getDeltaSetpoint() {
-    m_thisMutex.lock();
-    try {
-      return (m_setpoint - m_prevSetpoint) / m_setpointTimer.get();
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Returns the current difference of the input from the setpoint.
-   *
-   * @return the current error
-   */
-  @Override
-  public double getError() {
-    m_thisMutex.lock();
-    try {
-      return getContinuousError(getSetpoint() - m_filter.calculate(m_pidInput.pidGet()));
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Returns the current difference of the error over the past few iterations. You can specify the
-   * number of iterations to average with setToleranceBuffer() (defaults to 1). getAvgError() is
-   * used for the onTarget() function.
-   *
-   * @deprecated Use getError(), which is now already filtered.
-   * @return     the current average of the error
-   */
-  @Deprecated
-  public double getAvgError() {
-    m_thisMutex.lock();
-    try {
-      return getError();
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Sets what type of input the PID controller will use.
-   *
-   * @param pidSource the type of input
-   */
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_pidInput.setPIDSourceType(pidSource);
-  }
-
-  /**
-   * Returns the type of input the PID controller is using.
-   *
-   * @return the PID controller input type
-   */
-  public PIDSourceType getPIDSourceType() {
-    return m_pidInput.getPIDSourceType();
-  }
-
-  /**
-   * Set the PID tolerance using a Tolerance object. Tolerance can be specified as a percentage of
-   * the range or as an absolute value. The Tolerance object encapsulates those options in an
-   * object. Use it by creating the type of tolerance that you want to use: setTolerance(new
-   * PIDController.AbsoluteTolerance(0.1))
-   *
-   * @deprecated      Use setPercentTolerance() instead.
-   * @param tolerance A tolerance object of the right type, e.g. PercentTolerance or
-   *                  AbsoluteTolerance
-   */
-  @Deprecated
-  public void setTolerance(Tolerance tolerance) {
-    m_tolerance = tolerance;
-  }
-
-  /**
-   * Set the absolute error which is considered tolerable for use with OnTarget.
-   *
-   * @param absvalue absolute error which is tolerable in the units of the input object
-   */
-  public void setAbsoluteTolerance(double absvalue) {
-    m_thisMutex.lock();
-    try {
-      m_tolerance = new AbsoluteTolerance(absvalue);
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the percentage error which is considered tolerable for use with OnTarget. (Input of 15.0 =
-   * 15 percent)
-   *
-   * @param percentage percent error which is tolerable
-   */
-  public void setPercentTolerance(double percentage) {
-    m_thisMutex.lock();
-    try {
-      m_tolerance = new PercentageTolerance(percentage);
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the number of previous error samples to average for tolerancing. When determining whether a
-   * mechanism is on target, the user may want to use a rolling average of previous measurements
-   * instead of a precise position or velocity. This is useful for noisy sensors which return a few
-   * erroneous measurements when the mechanism is on target. However, the mechanism will not
-   * register as on target for at least the specified bufLength cycles.
-   *
-   * @deprecated      Use a LinearFilter as the input.
-   * @param bufLength Number of previous cycles to average.
-   */
-  @Deprecated
-  public void setToleranceBuffer(int bufLength) {
-    m_thisMutex.lock();
-    try {
-      m_filter = LinearFilter.movingAverage(bufLength);
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Return true if the error is within the percentage of the total input range, determined by
-   * setTolerance. This assumes that the maximum and minimum input were set using setInput.
-   *
-   * @return true if the error is less than the tolerance
-   */
-  public boolean onTarget() {
-    m_thisMutex.lock();
-    try {
-      return m_tolerance.onTarget();
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Reset the previous error, the integral term, and disable the controller.
-   */
-  @Override
-  public void reset() {
-    m_thisMutex.lock();
-    try {
-      m_prevError = 0;
-      m_totalError = 0;
-      m_result = 0;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Passes the output directly to setSetpoint().
-   *
-   * <p>PIDControllers can be nested by passing a PIDController as another PIDController's output.
-   * In that case, the output of the parent controller becomes the input (i.e., the reference) of
-   * the child.
-   *
-   * <p>It is the caller's responsibility to put the data into a valid form for setSetpoint().
-   */
-  @Override
-  public void pidWrite(double output) {
-    setSetpoint(output);
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("PIDController");
-    builder.setSafeState(this::reset);
-    builder.addDoubleProperty("p", this::getP, this::setP);
-    builder.addDoubleProperty("i", this::getI, this::setI);
-    builder.addDoubleProperty("d", this::getD, this::setD);
-    builder.addDoubleProperty("f", this::getF, this::setF);
-    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
-  }
-
-  /**
-   * Wraps error around for continuous inputs. The original error is returned if continuous mode is
-   * disabled. This is an unsynchronized function.
-   *
-   * @param error The current error of the PID controller.
-   * @return Error for continuous inputs.
-   */
-  protected double getContinuousError(double error) {
-    if (m_continuous && m_inputRange > 0) {
-      error %= m_inputRange;
-      if (Math.abs(error) > m_inputRange / 2) {
-        if (error > 0) {
-          return error - m_inputRange;
-        } else {
-          return error + m_inputRange;
-        }
-      }
-    }
-
-    return error;
-  }
-
-  private static double clamp(double value, double low, double high) {
-    return Math.max(low, Math.min(value, high));
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
deleted file mode 100644
index 881723e..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDController.java
+++ /dev/null
@@ -1,183 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-
-/**
- * Class implements a PID Control Loop.
- *
- * <p>Creates a separate thread which reads the given PIDSource and takes care of the integral
- * calculations, as well as writing the given PIDOutput.
- *
- * <p>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 {@link edu.wpi.first.wpilibj.controller.PIDController} instead.
- */
-@Deprecated(since = "2020", forRemoval = true)
-public class PIDController extends PIDBase implements Controller, AutoCloseable {
-  Notifier m_controlLoop = new Notifier(this::calculate);
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D, and F.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param Kf     the feed forward term
-   * @param source The PIDSource object that is used to get values
-   * @param output The PIDOutput object that is set to the output percentage
-   * @param period the loop time for doing calculations in seconds.
-   *               This particularly affects calculations of
-   *               the integral and differential terms.
-   *               The default is 0.05 (50ms).
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source,
-                       PIDOutput output, double period) {
-    super(Kp, Ki, Kd, Kf, source, output);
-    m_controlLoop.startPeriodic(period);
-  }
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D and period.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source the PIDSource object that is used to get values
-   * @param output the PIDOutput object that is set to the output percentage
-   * @param period the loop time for doing calculations in seconds.
-   *               This particularly affects calculations of
-   *               the integral and differential terms.
-   *               The default is 0.05 (50ms).
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output,
-                       double period) {
-    this(Kp, Ki, Kd, 0.0, source, output, period);
-  }
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param source The PIDSource object that is used to get values
-   * @param output The PIDOutput object that is set to the output percentage
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDController(double Kp, double Ki, double Kd, PIDSource source, PIDOutput output) {
-    this(Kp, Ki, Kd, source, output, kDefaultPeriod);
-  }
-
-  /**
-   * Allocate a PID object with the given constants for P, I, D, using a 50ms period.
-   *
-   * @param Kp     the proportional coefficient
-   * @param Ki     the integral coefficient
-   * @param Kd     the derivative coefficient
-   * @param Kf     the feed forward term
-   * @param source The PIDSource object that is used to get values
-   * @param output The PIDOutput object that is set to the output percentage
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDController(double Kp, double Ki, double Kd, double Kf, PIDSource source,
-                       PIDOutput output) {
-    this(Kp, Ki, Kd, Kf, source, output, kDefaultPeriod);
-  }
-
-  @Override
-  public void close() {
-    m_controlLoop.close();
-    m_thisMutex.lock();
-    try {
-      m_pidOutput = null;
-      m_pidInput = null;
-      m_controlLoop = null;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Begin running the PIDController.
-   */
-  @Override
-  public void enable() {
-    m_thisMutex.lock();
-    try {
-      m_enabled = true;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Stop running the PIDController, this sets the output to zero before stopping.
-   */
-  @Override
-  public void disable() {
-    // Ensures m_enabled check and pidWrite() call occur atomically
-    m_pidWriteMutex.lock();
-    try {
-      m_thisMutex.lock();
-      try {
-        m_enabled = false;
-      } finally {
-        m_thisMutex.unlock();
-      }
-
-      m_pidOutput.pidWrite(0);
-    } finally {
-      m_pidWriteMutex.unlock();
-    }
-  }
-
-  /**
-   * Set the enabled state of the PIDController.
-   */
-  public void setEnabled(boolean enable) {
-    if (enable) {
-      enable();
-    } else {
-      disable();
-    }
-  }
-
-  /**
-   * Return true if PIDController is enabled.
-   */
-  public boolean isEnabled() {
-    m_thisMutex.lock();
-    try {
-      return m_enabled;
-    } finally {
-      m_thisMutex.unlock();
-    }
-  }
-
-  /**
-   * Reset the previous error, the integral term, and disable the controller.
-   */
-  @Override
-  public void reset() {
-    disable();
-
-    super.reset();
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    super.initSendable(builder);
-    builder.addBooleanProperty("enabled", this::isEnabled, this::setEnabled);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
deleted file mode 100644
index 10e60d3..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDInterface.java
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-@Deprecated(since = "2020", forRemoval = true)
-@SuppressWarnings("SummaryJavadoc")
-public interface PIDInterface {
-  @SuppressWarnings("ParameterName")
-  void setPID(double p, double i, double d);
-
-  double getP();
-
-  double getI();
-
-  double getD();
-
-  void setSetpoint(double setpoint);
-
-  double getSetpoint();
-
-  double getError();
-
-  void reset();
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
deleted file mode 100644
index e9af2ff..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDOutput.java
+++ /dev/null
@@ -1,24 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-/**
- * This interface allows PIDController to write it's results to its output.
- *
- * @deprecated Use DoubleConsumer and new PIDController class.
- */
-@FunctionalInterface
-@Deprecated(since = "2020", forRemoval = true)
-public interface PIDOutput {
-  /**
-   * Set the output to the value calculated by PIDController.
-   *
-   * @param output the value calculated by PIDController
-   */
-  void pidWrite(double output);
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
deleted file mode 100644
index 3c4f1f5..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSource.java
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-/**
- * This interface allows for PIDController to automatically read from this object.
- *
- * @deprecated Use DoubleSupplier and new PIDController class.
- */
-@Deprecated(since = "2020", forRemoval = true)
-public interface PIDSource {
-  /**
-   * Set which parameter of the device you are using as a process control variable.
-   *
-   * @param pidSource An enum to select the parameter.
-   */
-  void setPIDSourceType(PIDSourceType pidSource);
-
-  /**
-   * Get which parameter of the device you are using as a process control variable.
-   *
-   * @return the currently selected PID source parameter
-   */
-  PIDSourceType getPIDSourceType();
-
-  /**
-   * Get the result to use in PIDController.
-   *
-   * @return the result to use in PIDController
-   */
-  double pidGet();
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
deleted file mode 100644
index 7508be5..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PIDSourceType.java
+++ /dev/null
@@ -1,17 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-/**
- * A description for the type of output value to provide to a PIDController.
- */
-@Deprecated(since = "2020", forRemoval = true)
-public enum PIDSourceType {
-  kDisplacement,
-  kRate
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java
new file mode 100644
index 0000000..e3eb052
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PS4Controller.java
@@ -0,0 +1,535 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+
+/**
+ * Handle input from PS4 controllers connected to the Driver Station.
+ *
+ * <p>This class handles PS4 input that comes from the Driver Station. Each time a value is
+ * requested the most recent value is returned. There is a single class instance for each controller
+ * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
+ */
+public class PS4Controller extends GenericHID {
+  /**
+   * Construct an instance of a device.
+   *
+   * @param port The port index on the Driver Station that the device is plugged into.
+   */
+  public PS4Controller(int port) {
+    super(port);
+    HAL.report(tResourceType.kResourceType_PS4Controller, port + 1);
+  }
+
+  /** Represents a digital button on a PS4Controller. */
+  public enum Button {
+    kSquare(1),
+    kCross(2),
+    kCircle(3),
+    kTriangle(4),
+    kL1(5),
+    kR1(6),
+    kL2(7),
+    kR2(8),
+    kShare(9),
+    kOptions(10),
+    kL3(11),
+    kR3(12),
+    kPS(13),
+    kTouchpad(14);
+
+    public final int value;
+
+    Button(int index) {
+      this.value = index;
+    }
+
+    /**
+     * Get the human-friendly name of the button, matching the relevant methods. This is done by
+     * stripping the leading `k`, and if not the touchpad append `Button`.
+     *
+     * <p>Primarily used for automated unit tests.
+     *
+     * @return the human-friendly name of the button.
+     */
+    @Override
+    public String toString() {
+      var name = this.name().substring(1); // Remove leading `k`
+      if (this == kTouchpad) {
+        return name;
+      }
+      return name + "Button";
+    }
+  }
+
+  /** Represents an axis on a PS4Controller. */
+  public enum Axis {
+    kLeftX(0),
+    kLeftY(1),
+    kRightX(2),
+    kRightY(5),
+    kL2(3),
+    kR2(4);
+
+    public final int value;
+
+    Axis(int index) {
+      value = index;
+    }
+
+    /**
+     * Get the human-friendly name of the axis, matching the relevant methods. This is done by
+     * stripping the leading `k`, and if one of L2/R2 append `Axis`.
+     *
+     * <p>Primarily used for automated unit tests.
+     *
+     * @return the human-friendly name of the axis.
+     */
+    @Override
+    public String toString() {
+      var name = this.name().substring(1); // Remove leading `k`
+      if (name.endsWith("2")) {
+        return name + "Axis";
+      }
+      return name;
+    }
+  }
+
+  /**
+   * Get the X axis value of left side of the controller.
+   *
+   * @return the axis value.
+   */
+  public double getLeftX() {
+    return getRawAxis(Axis.kLeftX.value);
+  }
+
+  /**
+   * Get the X axis value of right side of the controller.
+   *
+   * @return the axis value.
+   */
+  public double getRightX() {
+    return getRawAxis(Axis.kRightX.value);
+  }
+
+  /**
+   * Get the Y axis value of left side of the controller.
+   *
+   * @return the axis value.
+   */
+  public double getLeftY() {
+    return getRawAxis(Axis.kLeftY.value);
+  }
+
+  /**
+   * Get the Y axis value of right side of the controller.
+   *
+   * @return the axis value.
+   */
+  public double getRightY() {
+    return getRawAxis(Axis.kRightY.value);
+  }
+
+  /**
+   * Get the L2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as
+   * opposed to the usual [-1, 1].
+   *
+   * @return the axis value.
+   */
+  public double getL2Axis() {
+    return getRawAxis(Axis.kL2.value);
+  }
+
+  /**
+   * Get the R2 axis value of the controller. Note that this axis is bound to the range of [0, 1] as
+   * opposed to the usual [-1, 1].
+   *
+   * @return the axis value.
+   */
+  public double getR2Axis() {
+    return getRawAxis(Axis.kR2.value);
+  }
+
+  /**
+   * Read the value of the left trigger button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getL2Button() {
+    return getRawButton(Button.kL2.value);
+  }
+
+  /**
+   * Read the value of the right trigger button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getR2Button() {
+    return getRawButton(Button.kR2.value);
+  }
+
+  /**
+   * Whether the L2 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getL2ButtonPressed() {
+    return getRawButtonPressed(Button.kL2.value);
+  }
+
+  /**
+   * Whether the R2 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getR2ButtonPressed() {
+    return getRawButtonPressed(Button.kR2.value);
+  }
+
+  /**
+   * Whether the L2 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getL2ButtonReleased() {
+    return getRawButtonReleased(Button.kL2.value);
+  }
+
+  /**
+   * Whether the R2 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getR2ButtonReleased() {
+    return getRawButtonReleased(Button.kR2.value);
+  }
+
+  /**
+   * Read the value of the L1 button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getL1Button() {
+    return getRawButton(Button.kL1.value);
+  }
+
+  /**
+   * Read the value of the R1 button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getR1Button() {
+    return getRawButton(Button.kR1.value);
+  }
+
+  /**
+   * Whether the L1 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getL1ButtonPressed() {
+    return getRawButtonPressed(Button.kL1.value);
+  }
+
+  /**
+   * Whether the R1 button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getR1ButtonPressed() {
+    return getRawButtonPressed(Button.kR1.value);
+  }
+
+  /**
+   * Whether the L1 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getL1ButtonReleased() {
+    return getRawButtonReleased(Button.kL1.value);
+  }
+
+  /**
+   * Whether the R1 button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getR1ButtonReleased() {
+    return getRawButtonReleased(Button.kR1.value);
+  }
+
+  /**
+   * Read the value of the L3 button (pressing the left analog stick) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getL3Button() {
+    return getRawButton(Button.kL3.value);
+  }
+
+  /**
+   * Read the value of the R3 button (pressing the right analog stick) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getR3Button() {
+    return getRawButton(Button.kR3.value);
+  }
+
+  /**
+   * Whether the L3 (left stick) button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getL3ButtonPressed() {
+    return getRawButtonPressed(Button.kL3.value);
+  }
+
+  /**
+   * Whether the R3 (right stick) button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getR3ButtonPressed() {
+    return getRawButtonPressed(Button.kR3.value);
+  }
+
+  /**
+   * Whether the L3 (left stick) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getL3ButtonReleased() {
+    return getRawButtonReleased(Button.kL3.value);
+  }
+
+  /**
+   * Whether the R3 (right stick) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getR3ButtonReleased() {
+    return getRawButtonReleased(Button.kR3.value);
+  }
+
+  /**
+   * Read the value of the Square button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getSquareButton() {
+    return getRawButton(Button.kSquare.value);
+  }
+
+  /**
+   * Whether the Square button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getSquareButtonPressed() {
+    return getRawButtonPressed(Button.kSquare.value);
+  }
+
+  /**
+   * Whether the Square button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getSquareButtonReleased() {
+    return getRawButtonReleased(Button.kSquare.value);
+  }
+
+  /**
+   * Read the value of the Cross button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getCrossButton() {
+    return getRawButton(Button.kCross.value);
+  }
+
+  /**
+   * Whether the Cross button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getCrossButtonPressed() {
+    return getRawButtonPressed(Button.kCross.value);
+  }
+
+  /**
+   * Whether the Cross button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getCrossButtonReleased() {
+    return getRawButtonReleased(Button.kCross.value);
+  }
+
+  /**
+   * Read the value of the Triangle button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getTriangleButton() {
+    return getRawButton(Button.kTriangle.value);
+  }
+
+  /**
+   * Whether the Triangle button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getTriangleButtonPressed() {
+    return getRawButtonPressed(Button.kTriangle.value);
+  }
+
+  /**
+   * Whether the Triangle button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getTriangleButtonReleased() {
+    return getRawButtonReleased(Button.kTriangle.value);
+  }
+
+  /**
+   * Read the value of the Circle button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getCircleButton() {
+    return getRawButton(Button.kCircle.value);
+  }
+
+  /**
+   * Whether the Circle button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getCircleButtonPressed() {
+    return getRawButtonPressed(Button.kCircle.value);
+  }
+
+  /**
+   * Whether the Circle button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getCircleButtonReleased() {
+    return getRawButtonReleased(Button.kCircle.value);
+  }
+
+  /**
+   * Read the value of the share button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getShareButton() {
+    return getRawButton(Button.kShare.value);
+  }
+
+  /**
+   * Whether the share button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getShareButtonPressed() {
+    return getRawButtonPressed(Button.kShare.value);
+  }
+
+  /**
+   * Whether the share button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getShareButtonReleased() {
+    return getRawButtonReleased(Button.kShare.value);
+  }
+
+  /**
+   * Read the value of the PS button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getPSButton() {
+    return getRawButton(Button.kPS.value);
+  }
+
+  /**
+   * Whether the PS button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getPSButtonPressed() {
+    return getRawButtonPressed(Button.kPS.value);
+  }
+
+  /**
+   * Whether the PS button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getPSButtonReleased() {
+    return getRawButtonReleased(Button.kPS.value);
+  }
+
+  /**
+   * Read the value of the options button on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getOptionsButton() {
+    return getRawButton(Button.kOptions.value);
+  }
+
+  /**
+   * Whether the options button was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getOptionsButtonPressed() {
+    return getRawButtonPressed(Button.kOptions.value);
+  }
+
+  /**
+   * Whether the options button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getOptionsButtonReleased() {
+    return getRawButtonReleased(Button.kOptions.value);
+  }
+
+  /**
+   * Read the value of the touchpad on the controller.
+   *
+   * @return The state of the touchpad.
+   */
+  public boolean getTouchpad() {
+    return getRawButton(Button.kTouchpad.value);
+  }
+
+  /**
+   * Whether the touchpad was pressed since the last check.
+   *
+   * @return Whether the touchpad was pressed since the last check.
+   */
+  public boolean getTouchpadPressed() {
+    return getRawButtonPressed(Button.kTouchpad.value);
+  }
+
+  /**
+   * Whether the touchpad was released since the last check.
+   *
+   * @return Whether the touchpad was released since the last check.
+   */
+  public boolean getTouchpadReleased() {
+    return getRawButtonReleased(Button.kTouchpad.value);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
index 8febd2c..9d86ab8 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWM.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,8 +8,9 @@
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.PWMConfigDataResult;
 import edu.wpi.first.hal.PWMJNI;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Class implements the PWM generation in the FPGA.
@@ -26,36 +24,42 @@
  * center value - 999 to 2 = linear scaling from "center" to "full reverse" - 1 = minimum pulse
  * width (currently .5ms) - 0 = disabled (i.e. PWM output is held low)
  */
-public class PWM extends MotorSafety implements Sendable, AutoCloseable {
-  /**
-   * Represents the amount to multiply the minimum servo-pulse pwm period by.
-   */
+public class PWM implements Sendable, AutoCloseable {
+  /** Represents the amount to multiply the minimum servo-pulse pwm period by. */
   public enum PeriodMultiplier {
-    /**
-     * Period Multiplier: don't skip pulses. PWM pulses occur every 5.005 ms
-     */
+    /** Period Multiplier: don't skip pulses. PWM pulses occur every 5.005 ms */
     k1X,
-    /**
-     * Period Multiplier: skip every other pulse. PWM pulses occur every 10.010 ms
-     */
+    /** Period Multiplier: skip every other pulse. PWM pulses occur every 10.010 ms */
     k2X,
-    /**
-     * Period Multiplier: skip three out of four pulses. PWM pulses occur every 20.020 ms
-     */
+    /** Period Multiplier: skip three out of four pulses. PWM pulses occur every 20.020 ms */
     k4X
   }
 
   private final int m_channel;
 
-  // Package private to use from AddressableLED
-  int m_handle;
+  private int m_handle;
+
+  /**
+   * Allocate a PWM given a channel.
+   *
+   * <p>Checks channel value range and allocates the appropriate channel. The allocation is only
+   * done to help users ensure that they don't double assign channels.
+   *
+   * <p>By default, adds itself to SendableRegistry and LiveWindow.
+   *
+   * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+   */
+  public PWM(final int channel) {
+    this(channel, true);
+  }
 
   /**
    * Allocate a PWM given a channel.
    *
    * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+   * @param registerSendable If true, adds this instance to SendableRegistry and LiveWindow
    */
-  public PWM(final int channel) {
+  public PWM(final int channel, final boolean registerSendable) {
     SensorUtil.checkPWMChannel(channel);
     m_channel = channel;
 
@@ -66,14 +70,12 @@
     PWMJNI.setPWMEliminateDeadband(m_handle, false);
 
     HAL.report(tResourceType.kResourceType_PWM, channel + 1);
-    SendableRegistry.addLW(this, "PWM", channel);
-
-    setSafetyEnabled(false);
+    if (registerSendable) {
+      SendableRegistry.addLW(this, "PWM", channel);
+    }
   }
 
-  /**
-   * Free the resource associated with the PWM channel and set the value to 0.
-   */
+  /** Free the resource associated with the PWM channel and set the value to 0. */
   @Override
   public void close() {
     SendableRegistry.remove(this);
@@ -85,22 +87,12 @@
     m_handle = 0;
   }
 
-  @Override
-  public void stopMotor() {
-    setDisabled();
-  }
-
-  @Override
-  public String getDescription() {
-    return "PWM " + getChannel();
-  }
-
   /**
-   * Optionally eliminate the deadband from a speed controller.
+   * Optionally eliminate the deadband from a motor controller.
    *
-   * @param eliminateDeadband If true, set the motor curve on the Jaguar to eliminate the deadband
-   *                          in the middle of the range. Otherwise, keep the full range without
-   *                          modifying any values.
+   * @param eliminateDeadband If true, set the motor curve for the motor controller to eliminate the
+   *     deadband in the middle of the range. Otherwise, keep the full range without modifying any
+   *     values.
    */
   public void enableDeadbandElimination(boolean eliminateDeadband) {
     PWMJNI.setPWMEliminateDeadband(m_handle, eliminateDeadband);
@@ -111,21 +103,23 @@
    * type of controller. The values determine the upper and lower speeds as well as the deadband
    * bracket.
    *
-   * @param max         The max PWM pulse width in ms
+   * @param max The max PWM pulse width in ms
    * @param deadbandMax The high end of the deadband range pulse width in ms
-   * @param center      The center (off) pulse width in ms
+   * @param center The center (off) pulse width in ms
    * @param deadbandMin The low end of the deadband pulse width in ms
-   * @param min         The minimum pulse width in ms
+   * @param min The minimum pulse width in ms
    */
-  public void setBounds(double max, double deadbandMax, double center, double deadbandMin,
-                           double min) {
+  public void setBounds(
+      double max, double deadbandMax, double center, double deadbandMin, double min) {
     PWMJNI.setPWMConfig(m_handle, max, deadbandMax, center, deadbandMin, min);
   }
 
   /**
-   * Gets the bounds on the PWM pulse widths. This Gets the bounds on the PWM values for a
-   * particular type of controller. The values determine the upper and lower speeds as well
-   * as the deadband bracket.
+   * Gets the bounds on the PWM pulse widths. This gets the bounds on the PWM values for a
+   * particular type of controller. The values determine the upper and lower speeds as well as the
+   * deadband bracket.
+   *
+   * @return The bounds on the PWM pulse widths.
    */
   public PWMConfigDataResult getRawBounds() {
     return PWMJNI.getPWMConfigRaw(m_handle);
@@ -169,9 +163,9 @@
   /**
    * Set the PWM value based on a speed.
    *
-   * <p>This is intended to be used by speed controllers.
+   * <p>This is intended to be used by motor controllers.
    *
-   * @param speed The speed to set the speed controller between -1.0 and 1.0.
+   * @param speed The speed to set the motor controller between -1.0 and 1.0.
    * @pre SetMaxPositivePwm() called.
    * @pre SetMinPositivePwm() called.
    * @pre SetCenterPwm() called.
@@ -185,7 +179,7 @@
   /**
    * Get the PWM value in terms of speed.
    *
-   * <p>This is intended to be used by speed controllers.
+   * <p>This is intended to be used by motor controllers.
    *
    * @return The most recently set speed between -1.0 and 1.0.
    * @pre SetMaxPositivePwm() called.
@@ -219,10 +213,7 @@
     return PWMJNI.getPWMRaw(m_handle);
   }
 
-  /**
-   * Temporarily disables the PWM output. The next set call will reenable
-   * the output.
-   */
+  /** Temporarily disables the PWM output. The next set call will reenable the output. */
   public void setDisabled() {
     PWMJNI.setPWMDisabled(m_handle);
   }
@@ -251,10 +242,19 @@
     }
   }
 
-  protected void setZeroLatch() {
+  public void setZeroLatch() {
     PWMJNI.latchPWMZero(m_handle);
   }
 
+  /**
+   * Get the underlying handle.
+   *
+   * @return Underlying PWM handle
+   */
+  public int getHandle() {
+    return m_handle;
+  }
+
   @Override
   public void initSendable(SendableBuilder builder) {
     builder.setSmartDashboardType("PWM");
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
deleted file mode 100644
index 0f97ea5..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSparkMax.java
+++ /dev/null
@@ -1,47 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * REV Robotics SPARK MAX Speed Controller with PWM control.
- *
- * <P>Note that the SPARK MAX uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the SPARK MAX User Manual
- * available from REV Robotics.
- *
- * <p><ul>
- * <li> 2.003ms = full "forward"
- * <li> 1.550ms = the "high end" of the deadband range
- * <li> 1.500ms = center of the deadband range (off)
- * <li> 1.460ms = the "low end" of the deadband range
- * <li> 0.999ms = full "reverse"
- * </ul>
- */
-public class PWMSparkMax extends PWMSpeedController {
-  /**
-   * Common initialization code called by all constructors.
-   *
-   */
-  public PWMSparkMax(final int channel) {
-    super(channel);
-
-    setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel() + 1);
-    SendableRegistry.setName(this, "PWMSparkMax", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
deleted file mode 100644
index 0a33423..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMSpeedController.java
+++ /dev/null
@@ -1,89 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-
-/**
- * Common base class for all PWM Speed Controllers.
- */
-public abstract class PWMSpeedController extends PWM implements SpeedController {
-  private boolean m_isInverted;
-
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
-   *                on the MXP port
-   */
-  protected PWMSpeedController(int channel) {
-    super(channel);
-  }
-
-  @Override
-  public String getDescription() {
-    return "PWM " + getChannel();
-  }
-
-  /**
-   * Set the PWM value.
-   *
-   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
-   * FPGA.
-   *
-   * @param speed The speed value between -1.0 and 1.0 to set.
-   */
-  @Override
-  public void set(double speed) {
-    setSpeed(m_isInverted ? -speed : speed);
-    feed();
-  }
-
-  /**
-   * Get the recently set value of the PWM.
-   *
-   * @return The most recently set value for the PWM between -1.0 and 1.0.
-   */
-  @Override
-  public double get() {
-    return getSpeed();
-  }
-
-  @Override
-  public void setInverted(boolean isInverted) {
-    m_isInverted = isInverted;
-  }
-
-  @Override
-  public boolean getInverted() {
-    return m_isInverted;
-  }
-
-  @Override
-  public void disable() {
-    setDisabled();
-  }
-
-  /**
-   * Write out the PID value as seen in the PIDOutput base object.
-   *
-   * @param output Write out the PWM value as was found in the PIDController
-   */
-  @Override
-  public void pidWrite(double output) {
-    set(output);
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Speed Controller");
-    builder.setActuator(true);
-    builder.setSafeState(this::setDisabled);
-    builder.addDoubleProperty("Value", this::getSpeed, this::setSpeed);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
deleted file mode 100644
index 03acab2..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonFX.java
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Cross the Road Electronics (CTRE) Talon FX Speed Controller with PWM control.
- *
- * <p>Note that the TalonFX uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the TalonFX User Manual
- * available from CTRE.
- *
- * <p><ul>
- * <li>2.004ms = full "forward"
- * <li>1.520ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.480ms = the "low end" of the deadband range
- * <li>0.997ms = full "reverse"
- * </ul>
- */
-public class PWMTalonFX extends PWMSpeedController {
-  /**
-   * Constructor for a TalonFX connected via PWM.
-   *
-   * @param channel The PWM channel that the Talon FX is attached to. 0-9 are on-board, 10-19 are
-   *                on the MXP port
-   */
-  public PWMTalonFX(final int channel) {
-    super(channel);
-
-    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_TalonFX, getChannel() + 1);
-    SendableRegistry.setName(this, "PWMTalonFX", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
deleted file mode 100644
index 1859e7f..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMTalonSRX.java
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Cross the Road Electronics (CTRE) Talon SRX Speed Controller with PWM control.
- *
- * <p>Note that the TalonSRX uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the TalonSRX User Manual
- * available from CTRE.
- *
- * <p><ul>
- * <li>2.004ms = full "forward"
- * <li>1.520ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.480ms = the "low end" of the deadband range
- * <li>0.997ms = full "reverse"
- * </ul>
- */
-public class PWMTalonSRX extends PWMSpeedController {
-  /**
-   * Constructor for a TalonSRX connected via PWM.
-   *
-   * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are on-board, 10-19 are
-   *                on the MXP port
-   */
-  public PWMTalonSRX(final int channel) {
-    super(channel);
-
-    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel() + 1);
-    SendableRegistry.setName(this, "PWMTalonSRX", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
deleted file mode 100644
index 9a1116d..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVenom.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Playing with Fusion Venom Smart Motor with PWM control.
- *
- * <p>Note that the Venom uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended.
- *
- * <p><ul>
- * <li>2.004ms = full "forward"
- * <li>1.520ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.480ms = the "low end" of the deadband range
- * <li>0.997ms = full "reverse"
- * </ul>
- */
-public class PWMVenom extends PWMSpeedController {
-  /**
-   * Constructor for a Venom connected via PWM.
-   *
-   * @param channel The PWM channel that the Venom is attached to. 0-9 are on-board, 10-19 are
-   *                on the MXP port
-   */
-  public PWMVenom(final int channel) {
-    super(channel);
-
-    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_FusionVenom, getChannel() + 1);
-    SendableRegistry.setName(this, "PWMVenom", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
deleted file mode 100644
index b51686a..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PWMVictorSPX.java
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Cross the Road Electronics (CTRE) Victor SPX Speed Controller with PWM control.
- *
- * <p>Note that the Victor SPX uses the following bounds for PWM values. These values should
- * work reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the Victor SPX User
- * Manual available from CTRE.
- *
- * <p><ul>
- * <li>2.004ms = full "forward"
- * <li>1.520ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.480ms = the "low end" of the deadband range
- * <li>0.997ms = full "reverse"
- * </ul>
- */
-public class PWMVictorSPX extends PWMSpeedController {
-  /**
-   * Constructor for a Victor SPX connected via PWM.
-   *
-   * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9 are on-board, 10-19
-   *                are on the MXP port
-   */
-  public PWMVictorSPX(final int channel) {
-    super(channel);
-
-    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel() + 1);
-    SendableRegistry.setName(this, "PWMVictorSPX", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java
new file mode 100644
index 0000000..546c7c0
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticHub.java
@@ -0,0 +1,203 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.REVPHJNI;
+import java.util.HashMap;
+import java.util.Map;
+
+/** Module class for controlling a REV Robotics Pneumatic Hub. */
+public class PneumaticHub implements PneumaticsBase {
+  private static class DataStore implements AutoCloseable {
+    public final int m_module;
+    public final int m_handle;
+    private int m_refCount;
+    private int m_reservedMask;
+    private boolean m_compressorReserved;
+    private final Object m_reserveLock = new Object();
+
+    DataStore(int module) {
+      m_handle = REVPHJNI.initialize(module);
+      m_module = module;
+      m_handleMap.put(module, this);
+    }
+
+    @Override
+    public void close() {
+      REVPHJNI.free(m_handle);
+      m_handleMap.remove(m_module);
+    }
+
+    public void addRef() {
+      m_refCount++;
+    }
+
+    public void removeRef() {
+      m_refCount--;
+      if (m_refCount == 0) {
+        this.close();
+      }
+    }
+  }
+
+  private static final Map<Integer, DataStore> m_handleMap = new HashMap<>();
+  private static final Object m_handleLock = new Object();
+
+  private static DataStore getForModule(int module) {
+    synchronized (m_handleLock) {
+      Integer moduleBoxed = module;
+      DataStore pcm = m_handleMap.get(moduleBoxed);
+      if (pcm == null) {
+        pcm = new DataStore(module);
+      }
+      pcm.addRef();
+      return pcm;
+    }
+  }
+
+  private static void freeModule(DataStore store) {
+    synchronized (m_handleLock) {
+      store.removeRef();
+    }
+  }
+
+  private final DataStore m_dataStore;
+  private final int m_handle;
+
+  /** Constructs a PneumaticHub with the default id (1). */
+  public PneumaticHub() {
+    this(SensorUtil.getDefaultREVPHModule());
+  }
+
+  /**
+   * Constructs a PneumaticHub.
+   *
+   * @param module module number to construct
+   */
+  public PneumaticHub(int module) {
+    m_dataStore = getForModule(module);
+    m_handle = m_dataStore.m_handle;
+  }
+
+  @Override
+  public void close() {
+    freeModule(m_dataStore);
+  }
+
+  @Override
+  public boolean getCompressor() {
+    return REVPHJNI.getCompressor(m_handle);
+  }
+
+  @Override
+  public void setClosedLoopControl(boolean enabled) {
+    REVPHJNI.setClosedLoopControl(m_handle, enabled);
+  }
+
+  @Override
+  public boolean getClosedLoopControl() {
+    return REVPHJNI.getClosedLoopControl(m_handle);
+  }
+
+  @Override
+  public boolean getPressureSwitch() {
+    return REVPHJNI.getPressureSwitch(m_handle);
+  }
+
+  @Override
+  public double getCompressorCurrent() {
+    return REVPHJNI.getCompressorCurrent(m_handle);
+  }
+
+  @Override
+  public void setSolenoids(int mask, int values) {
+    REVPHJNI.setSolenoids(m_handle, mask, values);
+  }
+
+  @Override
+  public int getSolenoids() {
+    return REVPHJNI.getSolenoids(m_handle);
+  }
+
+  @Override
+  public int getModuleNumber() {
+    return m_dataStore.m_module;
+  }
+
+  @Override
+  public void fireOneShot(int index) {
+    // TODO Combine APIs
+    // REVPHJNI.fireOneShot(m_handle, index, durMs);
+  }
+
+  @Override
+  public void setOneShotDuration(int index, int durMs) {
+    // TODO Combine APIs
+    // REVPHJNI.setOneShotDuration(m_handle, index, durMs);
+  }
+
+  @Override
+  public boolean checkSolenoidChannel(int channel) {
+    return REVPHJNI.checkSolenoidChannel(channel);
+  }
+
+  @Override
+  public int checkAndReserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      if ((m_dataStore.m_reservedMask & mask) != 0) {
+        return m_dataStore.m_reservedMask & mask;
+      }
+      m_dataStore.m_reservedMask |= mask;
+      return 0;
+    }
+  }
+
+  @Override
+  public void unreserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_reservedMask &= ~mask;
+    }
+  }
+
+  @Override
+  public Solenoid makeSolenoid(int channel) {
+    return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.REVPH, channel);
+  }
+
+  @Override
+  public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) {
+    return new DoubleSolenoid(
+        m_dataStore.m_module, PneumaticsModuleType.REVPH, forwardChannel, reverseChannel);
+  }
+
+  @Override
+  public Compressor makeCompressor() {
+    return new Compressor(m_dataStore.m_module, PneumaticsModuleType.REVPH);
+  }
+
+  @Override
+  public boolean reserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      if (m_dataStore.m_compressorReserved) {
+        return false;
+      }
+      m_dataStore.m_compressorReserved = true;
+      return true;
+    }
+  }
+
+  @Override
+  public void unreserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_compressorReserved = false;
+    }
+  }
+
+  @Override
+  public int getSolenoidDisabledList() {
+    // TODO Get this working
+    return 0;
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java
new file mode 100644
index 0000000..67c0f25
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsBase.java
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+public interface PneumaticsBase extends AutoCloseable {
+  /**
+   * For internal use to get a module for a specific type.
+   *
+   * @param module module number
+   * @param type module type
+   * @return module
+   */
+  static PneumaticsBase getForType(int module, PneumaticsModuleType type) {
+    if (type == PneumaticsModuleType.CTREPCM) {
+      return new PneumaticsControlModule(module);
+    } else if (type == PneumaticsModuleType.REVPH) {
+      return new PneumaticHub(module);
+    }
+    throw new IllegalArgumentException("Unknown module type");
+  }
+
+  /**
+   * For internal use to get the default for a specific type.
+   *
+   * @param type module type
+   * @return module default
+   */
+  static int getDefaultForType(PneumaticsModuleType type) {
+    if (type == PneumaticsModuleType.CTREPCM) {
+      return SensorUtil.getDefaultCTREPCMModule();
+    } else if (type == PneumaticsModuleType.REVPH) {
+      return SensorUtil.getDefaultREVPHModule();
+    }
+    throw new IllegalArgumentException("Unknown module type");
+  }
+
+  /**
+   * Sets solenoids on a pneumatics module.
+   *
+   * @param mask mask
+   * @param values values
+   */
+  void setSolenoids(int mask, int values);
+
+  /**
+   * Gets solenoid values.
+   *
+   * @return values
+   */
+  int getSolenoids();
+
+  /**
+   * Get module number for this module.
+   *
+   * @return module number
+   */
+  int getModuleNumber();
+
+  /**
+   * Get the disabled solenoids.
+   *
+   * @return disabled list
+   */
+  int getSolenoidDisabledList();
+
+  /**
+   * Fire a single solenoid shot.
+   *
+   * @param index solenoid index
+   */
+  void fireOneShot(int index);
+
+  /**
+   * Set the duration for a single solenoid shot.
+   *
+   * @param index solenoid index
+   * @param durMs shot duration
+   */
+  void setOneShotDuration(int index, int durMs);
+
+  boolean getCompressor();
+
+  boolean getPressureSwitch();
+
+  double getCompressorCurrent();
+
+  void setClosedLoopControl(boolean on);
+
+  boolean getClosedLoopControl();
+
+  /**
+   * Check if a solenoid channel is valid.
+   *
+   * @param channel Channel to check
+   * @return True if channel exists
+   */
+  boolean checkSolenoidChannel(int channel);
+
+  /**
+   * Check to see if the masked solenoids can be reserved, and if not reserve them.
+   *
+   * @param mask The solenoid mask to reserve
+   * @return 0 if successful, mask of solenoids that couldn't be allocated otherwise
+   */
+  int checkAndReserveSolenoids(int mask);
+
+  /**
+   * Unreserve the masked solenoids.
+   *
+   * @param mask The solenoid mask to unreserve
+   */
+  void unreserveSolenoids(int mask);
+
+  boolean reserveCompressor();
+
+  void unreserveCompressor();
+
+  @Override
+  void close();
+
+  Solenoid makeSolenoid(int channel);
+
+  DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel);
+
+  Compressor makeCompressor();
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java
new file mode 100644
index 0000000..79f245b
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsControlModule.java
@@ -0,0 +1,236 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.CTREPCMJNI;
+import java.util.HashMap;
+import java.util.Map;
+
+/** Module class for controlling a Cross The Road Electronics Pneumatics Control Module. */
+public class PneumaticsControlModule implements PneumaticsBase {
+  private static class DataStore implements AutoCloseable {
+    public final int m_module;
+    public final int m_handle;
+    private int m_refCount;
+    private int m_reservedMask;
+    private boolean m_compressorReserved;
+    private final Object m_reserveLock = new Object();
+
+    DataStore(int module) {
+      m_handle = CTREPCMJNI.initialize(module);
+      m_module = module;
+      m_handleMap.put(module, this);
+    }
+
+    @Override
+    public void close() {
+      CTREPCMJNI.free(m_handle);
+      m_handleMap.remove(m_module);
+    }
+
+    public void addRef() {
+      m_refCount++;
+    }
+
+    public void removeRef() {
+      m_refCount--;
+      if (m_refCount == 0) {
+        this.close();
+      }
+    }
+  }
+
+  private static final Map<Integer, DataStore> m_handleMap = new HashMap<>();
+  private static final Object m_handleLock = new Object();
+
+  private static DataStore getForModule(int module) {
+    synchronized (m_handleLock) {
+      Integer moduleBoxed = module;
+      DataStore pcm = m_handleMap.get(moduleBoxed);
+      if (pcm == null) {
+        pcm = new DataStore(module);
+      }
+      pcm.addRef();
+      return pcm;
+    }
+  }
+
+  private static void freeModule(DataStore store) {
+    synchronized (m_handleLock) {
+      store.removeRef();
+    }
+  }
+
+  private final DataStore m_dataStore;
+  private final int m_handle;
+
+  /** Constructs a PneumaticsControlModule with the default id (0). */
+  public PneumaticsControlModule() {
+    this(SensorUtil.getDefaultCTREPCMModule());
+  }
+
+  /**
+   * Constructs a PneumaticsControlModule.
+   *
+   * @param module module number to construct
+   */
+  public PneumaticsControlModule(int module) {
+    m_dataStore = getForModule(module);
+    m_handle = m_dataStore.m_handle;
+  }
+
+  @Override
+  public void close() {
+    freeModule(m_dataStore);
+  }
+
+  @Override
+  public boolean getCompressor() {
+    return CTREPCMJNI.getCompressor(m_handle);
+  }
+
+  @Override
+  public void setClosedLoopControl(boolean enabled) {
+    CTREPCMJNI.setClosedLoopControl(m_handle, enabled);
+  }
+
+  @Override
+  public boolean getClosedLoopControl() {
+    return CTREPCMJNI.getClosedLoopControl(m_handle);
+  }
+
+  @Override
+  public boolean getPressureSwitch() {
+    return CTREPCMJNI.getPressureSwitch(m_handle);
+  }
+
+  @Override
+  public double getCompressorCurrent() {
+    return CTREPCMJNI.getCompressorCurrent(m_handle);
+  }
+
+  public boolean getCompressorCurrentTooHighFault() {
+    return CTREPCMJNI.getCompressorCurrentTooHighFault(m_handle);
+  }
+
+  public boolean getCompressorCurrentTooHighStickyFault() {
+    return CTREPCMJNI.getCompressorCurrentTooHighStickyFault(m_handle);
+  }
+
+  public boolean getCompressorShortedFault() {
+    return CTREPCMJNI.getCompressorShortedFault(m_handle);
+  }
+
+  public boolean getCompressorShortedStickyFault() {
+    return CTREPCMJNI.getCompressorShortedStickyFault(m_handle);
+  }
+
+  public boolean getCompressorNotConnectedFault() {
+    return CTREPCMJNI.getCompressorNotConnectedFault(m_handle);
+  }
+
+  public boolean getCompressorNotConnectedStickyFault() {
+    return CTREPCMJNI.getCompressorNotConnectedStickyFault(m_handle);
+  }
+
+  @Override
+  public void setSolenoids(int mask, int values) {
+    CTREPCMJNI.setSolenoids(m_handle, mask, values);
+  }
+
+  @Override
+  public int getSolenoids() {
+    return CTREPCMJNI.getSolenoids(m_handle);
+  }
+
+  @Override
+  public int getModuleNumber() {
+    return m_dataStore.m_module;
+  }
+
+  @Override
+  public int getSolenoidDisabledList() {
+    return CTREPCMJNI.getSolenoidDisabledList(m_handle);
+  }
+
+  public boolean getSolenoidVoltageFault() {
+    return CTREPCMJNI.getSolenoidVoltageFault(m_handle);
+  }
+
+  public boolean getSolenoidVoltageStickyFault() {
+    return CTREPCMJNI.getSolenoidVoltageStickyFault(m_handle);
+  }
+
+  public void clearAllStickyFaults() {
+    CTREPCMJNI.clearAllStickyFaults(m_handle);
+  }
+
+  @Override
+  public void fireOneShot(int index) {
+    CTREPCMJNI.fireOneShot(m_handle, index);
+  }
+
+  @Override
+  public void setOneShotDuration(int index, int durMs) {
+    CTREPCMJNI.setOneShotDuration(m_handle, index, durMs);
+  }
+
+  @Override
+  public boolean checkSolenoidChannel(int channel) {
+    return CTREPCMJNI.checkSolenoidChannel(channel);
+  }
+
+  @Override
+  public int checkAndReserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      if ((m_dataStore.m_reservedMask & mask) != 0) {
+        return m_dataStore.m_reservedMask & mask;
+      }
+      m_dataStore.m_reservedMask |= mask;
+      return 0;
+    }
+  }
+
+  @Override
+  public void unreserveSolenoids(int mask) {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_reservedMask &= ~mask;
+    }
+  }
+
+  @Override
+  public Solenoid makeSolenoid(int channel) {
+    return new Solenoid(m_dataStore.m_module, PneumaticsModuleType.CTREPCM, channel);
+  }
+
+  @Override
+  public DoubleSolenoid makeDoubleSolenoid(int forwardChannel, int reverseChannel) {
+    return new DoubleSolenoid(
+        m_dataStore.m_module, PneumaticsModuleType.CTREPCM, forwardChannel, reverseChannel);
+  }
+
+  @Override
+  public Compressor makeCompressor() {
+    return new Compressor(m_dataStore.m_module, PneumaticsModuleType.CTREPCM);
+  }
+
+  @Override
+  public boolean reserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      if (m_dataStore.m_compressorReserved) {
+        return false;
+      }
+      m_dataStore.m_compressorReserved = true;
+      return true;
+    }
+  }
+
+  @Override
+  public void unreserveCompressor() {
+    synchronized (m_dataStore.m_reserveLock) {
+      m_dataStore.m_compressorReserved = false;
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java
new file mode 100644
index 0000000..a7951e8
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PneumaticsModuleType.java
@@ -0,0 +1,10 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+public enum PneumaticsModuleType {
+  CTREPCM,
+  REVPH;
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java
new file mode 100644
index 0000000..dd97efa
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistribution.java
@@ -0,0 +1,170 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.PowerDistributionJNI;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/**
+ * Class for getting voltage, current, temperature, power and energy from the Power Distribution
+ * Panel over CAN.
+ */
+public class PowerDistribution implements Sendable, AutoCloseable {
+  private final int m_handle;
+  private final int m_module;
+
+  public static final int kDefaultModule = PowerDistributionJNI.DEFAULT_MODULE;
+
+  public enum ModuleType {
+    kAutomatic(PowerDistributionJNI.AUTOMATIC_TYPE),
+    kCTRE(PowerDistributionJNI.CTRE_TYPE),
+    kRev(PowerDistributionJNI.REV_TYPE);
+
+    public final int value;
+
+    ModuleType(int value) {
+      this.value = value;
+    }
+  }
+
+  /**
+   * Constructs a PowerDistribution.
+   *
+   * @param module The CAN ID of the PDP.
+   * @param moduleType Module type (automatic, CTRE, or REV).
+   */
+  public PowerDistribution(int module, ModuleType moduleType) {
+    m_handle = PowerDistributionJNI.initialize(module, moduleType.value);
+    m_module = PowerDistributionJNI.getModuleNumber(m_handle);
+
+    HAL.report(tResourceType.kResourceType_PDP, m_module + 1);
+    SendableRegistry.addLW(this, "PowerDistribution", m_module);
+  }
+
+  /**
+   * Constructs a PowerDistribution.
+   *
+   * <p>Uses the default CAN ID.
+   */
+  public PowerDistribution() {
+    this(kDefaultModule, ModuleType.kAutomatic);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Gets the number of channel for this power distribution.
+   *
+   * @return Number of output channels.
+   */
+  public int getNumChannels() {
+    return PowerDistributionJNI.getNumChannels(m_handle);
+  }
+
+  /**
+   * Query the input voltage of the PDP.
+   *
+   * @return The voltage of the PDP in volts
+   */
+  public double getVoltage() {
+    return PowerDistributionJNI.getVoltage(m_handle);
+  }
+
+  /**
+   * Query the temperature of the PDP.
+   *
+   * @return The temperature of the PDP in degrees Celsius
+   */
+  public double getTemperature() {
+    return PowerDistributionJNI.getTemperature(m_handle);
+  }
+
+  /**
+   * Query the current of a single channel of the PDP.
+   *
+   * @param channel The PDP channel to query.
+   * @return The current of one of the PDP channels (channels 0-15) in Amperes
+   */
+  public double getCurrent(int channel) {
+    double current = PowerDistributionJNI.getChannelCurrent(m_handle, channel);
+
+    return current;
+  }
+
+  /**
+   * Query the current of all monitored PDP channels (0-15).
+   *
+   * @return The current of all the channels in Amperes
+   */
+  public double getTotalCurrent() {
+    return PowerDistributionJNI.getTotalCurrent(m_handle);
+  }
+
+  /**
+   * Query the total power drawn from the monitored PDP channels.
+   *
+   * @return the total power in Watts
+   */
+  public double getTotalPower() {
+    return PowerDistributionJNI.getTotalPower(m_handle);
+  }
+
+  /**
+   * Query the total energy drawn from the monitored PDP channels.
+   *
+   * @return the total energy in Joules
+   */
+  public double getTotalEnergy() {
+    return PowerDistributionJNI.getTotalEnergy(m_handle);
+  }
+
+  /** Reset the total energy to 0. */
+  public void resetTotalEnergy() {
+    PowerDistributionJNI.resetTotalEnergy(m_handle);
+  }
+
+  /** Clear all PDP sticky faults. */
+  public void clearStickyFaults() {
+    PowerDistributionJNI.clearStickyFaults(m_handle);
+  }
+
+  /**
+   * Gets module number (CAN ID).
+   *
+   * @return The module number (CAN ID).
+   */
+  public int getModule() {
+    return m_module;
+  }
+
+  public boolean getSwitchableChannel() {
+    return PowerDistributionJNI.getSwitchableChannel(m_handle);
+  }
+
+  public void setSwitchableChannel(boolean enabled) {
+    PowerDistributionJNI.setSwitchableChannel(m_handle, enabled);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PowerDistribution");
+    int numChannels = getNumChannels();
+    for (int i = 0; i < numChannels; ++i) {
+      final int chan = i;
+      builder.addDoubleProperty("Chan" + i, () -> getCurrent(chan), null);
+    }
+    builder.addDoubleProperty("Voltage", this::getVoltage, null);
+    builder.addDoubleProperty("TotalCurrent", this::getTotalCurrent, null);
+    builder.addBooleanProperty(
+        "SwitchableChannel", this::getSwitchableChannel, this::setSwitchableChannel);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
deleted file mode 100644
index ef46028..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/PowerDistributionPanel.java
+++ /dev/null
@@ -1,139 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.PDPJNI;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Class for getting voltage, current, temperature, power and energy from the Power Distribution
- * Panel over CAN.
- */
-public class PowerDistributionPanel implements Sendable, AutoCloseable {
-  private final int m_handle;
-  private final int m_module;
-
-  /**
-   * Constructor.
-   *
-   * @param module The CAN ID of the PDP
-   */
-  public PowerDistributionPanel(int module) {
-    SensorUtil.checkPDPModule(module);
-    m_handle = PDPJNI.initializePDP(module);
-    m_module = module;
-
-    HAL.report(tResourceType.kResourceType_PDP, module + 1);
-    SendableRegistry.addLW(this, "PowerDistributionPanel", module);
-  }
-
-  /**
-   * Constructor.  Uses the default CAN ID (0).
-   */
-  public PowerDistributionPanel() {
-    this(0);
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-  }
-
-  /**
-   * Query the input voltage of the PDP.
-   *
-   * @return The voltage of the PDP in volts
-   */
-  public double getVoltage() {
-    return PDPJNI.getPDPVoltage(m_handle);
-  }
-
-  /**
-   * Query the temperature of the PDP.
-   *
-   * @return The temperature of the PDP in degrees Celsius
-   */
-  public double getTemperature() {
-    return PDPJNI.getPDPTemperature(m_handle);
-  }
-
-  /**
-   * Query the current of a single channel of the PDP.
-   *
-   * @return The current of one of the PDP channels (channels 0-15) in Amperes
-   */
-  public double getCurrent(int channel) {
-    double current = PDPJNI.getPDPChannelCurrent((byte) channel, m_handle);
-
-    SensorUtil.checkPDPChannel(channel);
-
-    return current;
-  }
-
-  /**
-   * Query the current of all monitored PDP channels (0-15).
-   *
-   * @return The current of all the channels in Amperes
-   */
-  public double getTotalCurrent() {
-    return PDPJNI.getPDPTotalCurrent(m_handle);
-  }
-
-  /**
-   * Query the total power drawn from the monitored PDP channels.
-   *
-   * @return the total power in Watts
-   */
-  public double getTotalPower() {
-    return PDPJNI.getPDPTotalPower(m_handle);
-  }
-
-  /**
-   * Query the total energy drawn from the monitored PDP channels.
-   *
-   * @return the total energy in Joules
-   */
-  public double getTotalEnergy() {
-    return PDPJNI.getPDPTotalEnergy(m_handle);
-  }
-
-  /**
-   * Reset the total energy to 0.
-   */
-  public void resetTotalEnergy() {
-    PDPJNI.resetPDPTotalEnergy(m_handle);
-  }
-
-  /**
-   * Clear all PDP sticky faults.
-   */
-  public void clearStickyFaults() {
-    PDPJNI.clearPDPStickyFaults(m_handle);
-  }
-
-  /**
-   * Gets module number (CAN ID).
-   */
-  public int getModule() {
-    return m_module;
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("PowerDistributionPanel");
-    for (int i = 0; i < SensorUtil.kPDPChannels; ++i) {
-      final int chan = i;
-      builder.addDoubleProperty("Chan" + i, () -> getCurrent(chan), null);
-    }
-    builder.addDoubleProperty("Voltage", this::getVoltage, null);
-    builder.addDoubleProperty("TotalCurrent", this::getTotalCurrent, null);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
index 0edbbf3..a15fb70 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Preferences.java
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.Collection;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -15,41 +12,36 @@
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import java.util.Collection;
 
 /**
  * The preferences class provides a relatively simple way to save important values to the roboRIO to
  * access the next time the roboRIO is booted.
  *
- * <p> This class loads and saves from a file inside the roboRIO. The user can not access the file
+ * <p>This class loads and saves from a file inside the roboRIO. The user can not access the file
  * directly, but may modify values at specific fields which will then be automatically saved to the
- * file by the NetworkTables server. </p>
+ * file by the NetworkTables server.
  *
- * <p> This class is thread safe. </p>
+ * <p>This class is thread safe.
  *
- * <p> This will also interact with {@link NetworkTable} by creating a table called "Preferences"
- * with all the key-value pairs. </p>
+ * <p>This will also interact with {@link NetworkTable} by creating a table called "Preferences"
+ * with all the key-value pairs.
  */
 public final class Preferences {
-  /**
-   * The Preferences table name.
-   */
+  /** The Preferences table name. */
   private static final String TABLE_NAME = "Preferences";
-  /**
-   * The singleton instance.
-   */
+  /** The singleton instance. */
   private static Preferences instance;
-  /**
-   * The network table.
-   */
-  private final NetworkTable m_table;
+  /** The network table. */
+  private static final NetworkTable m_table;
 
   /**
    * Returns the preferences instance.
    *
    * @return the preferences instance
+   * @deprecated Use the static methods
    */
+  @Deprecated
   public static synchronized Preferences getInstance() {
     if (instance == null) {
       instance = new Preferences();
@@ -57,10 +49,10 @@
     return instance;
   }
 
-  /**
-   * Creates a preference class.
-   */
-  private Preferences() {
+  /** Creates a preference class. */
+  private Preferences() {}
+
+  static {
     m_table = NetworkTableInstance.getDefault().getTable(TABLE_NAME);
     m_table.getEntry(".type").setString("RobotPreferences");
     // Listener to set all Preferences values to persistent
@@ -73,21 +65,22 @@
 
   /**
    * Gets the preferences keys.
+   *
    * @return a collection of the keys
    */
-  public Collection<String> getKeys() {
+  public static Collection<String> getKeys() {
     return m_table.getKeys();
   }
 
   /**
    * Puts the given string into the preferences table.
    *
-   * @param key   the key
+   * @param key the key
    * @param value the value
    * @throws NullPointerException if value is null
    */
-  public void putString(String key, String value) {
-    requireNonNullParam(value, "value", "putString");
+  public static void setString(String key, String value) {
+    requireNonNullParam(value, "value", "setString");
 
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setString(value);
@@ -95,12 +88,25 @@
   }
 
   /**
+   * Puts the given string into the preferences table.
+   *
+   * @param key the key
+   * @param value the value
+   * @throws NullPointerException if value is null
+   * @deprecated Use {@link #setString(String, String)}
+   */
+  @Deprecated
+  public static void putString(String key, String value) {
+    setString(key, value);
+  }
+
+  /**
    * Puts the given string into the preferences table if it doesn't already exist.
    *
-   * @param key   The key
+   * @param key The key
    * @param value The value
    */
-  public void initString(String key, String value) {
+  public static void initString(String key, String value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDefaultString(value);
   }
@@ -108,22 +114,34 @@
   /**
    * Puts the given int into the preferences table.
    *
-   * @param key   the key
+   * @param key the key
    * @param value the value
    */
-  public void putInt(String key, int value) {
+  public static void setInt(String key, int value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDouble(value);
     entry.setPersistent();
   }
 
   /**
+   * Puts the given int into the preferences table.
+   *
+   * @param key the key
+   * @param value the value
+   * @deprecated Use {@link #setInt(String, int)}
+   */
+  @Deprecated
+  public static void putInt(String key, int value) {
+    setInt(key, value);
+  }
+
+  /**
    * Puts the given int into the preferences table if it doesn't already exist.
    *
-   * @param key   The key
+   * @param key The key
    * @param value The value
    */
-  public void initInt(String key, int value) {
+  public static void initInt(String key, int value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDefaultDouble(value);
   }
@@ -131,22 +149,34 @@
   /**
    * Puts the given double into the preferences table.
    *
-   * @param key   the key
+   * @param key the key
    * @param value the value
    */
-  public void putDouble(String key, double value) {
+  public static void setDouble(String key, double value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDouble(value);
     entry.setPersistent();
   }
 
   /**
+   * Puts the given double into the preferences table.
+   *
+   * @param key the key
+   * @param value the value
+   * @deprecated Use {@link #setDouble(String, double)}
+   */
+  @Deprecated
+  public static void putDouble(String key, double value) {
+    setDouble(key, value);
+  }
+
+  /**
    * Puts the given double into the preferences table if it doesn't already exist.
    *
-   * @param key   The key
+   * @param key The key
    * @param value The value
    */
-  public void initDouble(String key, double value) {
+  public static void initDouble(String key, double value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDefaultDouble(value);
   }
@@ -154,22 +184,34 @@
   /**
    * Puts the given float into the preferences table.
    *
-   * @param key   the key
+   * @param key the key
    * @param value the value
    */
-  public void putFloat(String key, float value) {
+  public static void setFloat(String key, float value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDouble(value);
     entry.setPersistent();
   }
 
   /**
+   * Puts the given float into the preferences table.
+   *
+   * @param key the key
+   * @param value the value
+   * @deprecated Use {@link #setFloat(String, float)}
+   */
+  @Deprecated
+  public static void putFloat(String key, float value) {
+    setFloat(key, value);
+  }
+
+  /**
    * Puts the given float into the preferences table if it doesn't already exist.
    *
-   * @param key   The key
+   * @param key The key
    * @param value The value
    */
-  public void initFloat(String key, float value) {
+  public static void initFloat(String key, float value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDefaultDouble(value);
   }
@@ -177,22 +219,34 @@
   /**
    * Puts the given boolean into the preferences table.
    *
-   * @param key   the key
+   * @param key the key
    * @param value the value
    */
-  public void putBoolean(String key, boolean value) {
+  public static void setBoolean(String key, boolean value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setBoolean(value);
     entry.setPersistent();
   }
 
   /**
+   * Puts the given boolean into the preferences table.
+   *
+   * @param key the key
+   * @param value the value
+   * @deprecated Use {@link #setBoolean(String, boolean)}
+   */
+  @Deprecated
+  public static void putBoolean(String key, boolean value) {
+    setBoolean(key, value);
+  }
+
+  /**
    * Puts the given boolean into the preferences table if it doesn't already exist.
    *
-   * @param key   The key
+   * @param key The key
    * @param value The value
    */
-  public void initBoolean(String key, boolean value) {
+  public static void initBoolean(String key, boolean value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDefaultBoolean(value);
   }
@@ -200,22 +254,34 @@
   /**
    * Puts the given long into the preferences table.
    *
-   * @param key   the key
+   * @param key the key
    * @param value the value
    */
-  public void putLong(String key, long value) {
+  public static void setLong(String key, long value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDouble(value);
     entry.setPersistent();
   }
 
   /**
+   * Puts the given long into the preferences table.
+   *
+   * @param key the key
+   * @param value the value
+   * @deprecated Use {@link #setLong(String, long)}
+   */
+  @Deprecated
+  public static void putLong(String key, long value) {
+    setLong(key, value);
+  }
+
+  /**
    * Puts the given long into the preferences table if it doesn't already exist.
    *
-   * @param key   The key
+   * @param key The key
    * @param value The value
    */
-  public void initLong(String key, long value) {
+  public static void initLong(String key, long value) {
     NetworkTableEntry entry = m_table.getEntry(key);
     entry.setDefaultDouble(value);
   }
@@ -226,7 +292,7 @@
    * @param key the key
    * @return if there is a value at the given key
    */
-  public boolean containsKey(String key) {
+  public static boolean containsKey(String key) {
     return m_table.containsKey(key);
   }
 
@@ -235,14 +301,12 @@
    *
    * @param key the key
    */
-  public void remove(String key) {
+  public static void remove(String key) {
     m_table.delete(key);
   }
 
-  /**
-   * Remove all preferences.
-   */
-  public void removeAll() {
+  /** Remove all preferences. */
+  public static void removeAll() {
     for (String key : m_table.getKeys()) {
       if (!".type".equals(key)) {
         remove(key);
@@ -254,11 +318,11 @@
    * Returns the string at the given key. If this table does not have a value for that position,
    * then the given backup value will be returned.
    *
-   * @param key    the key
+   * @param key the key
    * @param backup the value to return if none exists in the table
    * @return either the value in the table, or the backup
    */
-  public String getString(String key, String backup) {
+  public static String getString(String key, String backup) {
     return m_table.getEntry(key).getString(backup);
   }
 
@@ -266,11 +330,11 @@
    * Returns the int at the given key. If this table does not have a value for that position, then
    * the given backup value will be returned.
    *
-   * @param key    the key
+   * @param key the key
    * @param backup the value to return if none exists in the table
    * @return either the value in the table, or the backup
    */
-  public int getInt(String key, int backup) {
+  public static int getInt(String key, int backup) {
     return (int) m_table.getEntry(key).getDouble(backup);
   }
 
@@ -278,11 +342,11 @@
    * Returns the double at the given key. If this table does not have a value for that position,
    * then the given backup value will be returned.
    *
-   * @param key    the key
+   * @param key the key
    * @param backup the value to return if none exists in the table
    * @return either the value in the table, or the backup
    */
-  public double getDouble(String key, double backup) {
+  public static double getDouble(String key, double backup) {
     return m_table.getEntry(key).getDouble(backup);
   }
 
@@ -290,11 +354,11 @@
    * Returns the boolean at the given key. If this table does not have a value for that position,
    * then the given backup value will be returned.
    *
-   * @param key    the key
+   * @param key the key
    * @param backup the value to return if none exists in the table
    * @return either the value in the table, or the backup
    */
-  public boolean getBoolean(String key, boolean backup) {
+  public static boolean getBoolean(String key, boolean backup) {
     return m_table.getEntry(key).getBoolean(backup);
   }
 
@@ -302,11 +366,11 @@
    * Returns the float at the given key. If this table does not have a value for that position, then
    * the given backup value will be returned.
    *
-   * @param key    the key
+   * @param key the key
    * @param backup the value to return if none exists in the table
    * @return either the value in the table, or the backup
    */
-  public float getFloat(String key, float backup) {
+  public static float getFloat(String key, float backup) {
     return (float) m_table.getEntry(key).getDouble(backup);
   }
 
@@ -314,11 +378,11 @@
    * Returns the long at the given key. If this table does not have a value for that position, then
    * the given backup value will be returned.
    *
-   * @param key    the key
+   * @param key the key
    * @param backup the value to return if none exists in the table
    * @return either the value in the table, or the backup
    */
-  public long getLong(String key, long backup) {
+  public static long getLong(String key, long backup) {
     return (long) m_table.getEntry(key).getDouble(backup);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
index f379de4..05efebd 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Relay.java
@@ -1,32 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.Arrays;
-import java.util.Optional;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.RelayJNI;
+import edu.wpi.first.hal.util.HalHandleException;
 import edu.wpi.first.hal.util.UncleanStatusException;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.Arrays;
+import java.util.Optional;
 
 /**
  * Class for VEX Robotics Spike style relay outputs. Relays are intended to be connected to Spikes
  * or similar relays. The relay channels controls a pair of channels that are either both off, one
- * on, the other on, or both on. This translates into two Spike outputs at 0v, one at 12v and one
- * at 0v, one at 0v and the other at 12v, or two Spike outputs at 12V. This allows off, full
- * forward, or full reverse control of motors without variable speed. It also allows the two
- * channels (forward and reverse) to be used independently for something that does not care about
- * voltage polarity (like a solenoid).
+ * on, the other on, or both on. This translates into two Spike outputs at 0v, one at 12v and one at
+ * 0v, one at 0v and the other at 12v, or two Spike outputs at 12V. This allows off, full forward,
+ * or full reverse control of motors without variable speed. It also allows the two channels
+ * (forward and reverse) to be used independently for something that does not care about voltage
+ * polarity (like a solenoid).
  */
 public class Relay extends MotorSafety implements Sendable, AutoCloseable {
   /**
@@ -45,9 +43,7 @@
     }
   }
 
-  /**
-   * The state to drive a Relay to.
-   */
+  /** The state to drive a Relay to. */
   public enum Value {
     kOff("Off"),
     kOn("On"),
@@ -69,22 +65,13 @@
     }
   }
 
-  /**
-   * The Direction(s) that a relay is configured to operate in.
-   */
+  /** The Direction(s) that a relay is configured to operate in. */
   public enum Direction {
-    /**
-     * direction: both directions are valid.
-     */
-
+    /** direction: both directions are valid. */
     kBoth,
-    /**
-     * direction: Only forward is valid.
-     */
+    /** direction: Only forward is valid. */
     kForward,
-    /**
-     * direction: only reverse is valid.
-     */
+    /** direction: only reverse is valid. */
     kReverse
   }
 
@@ -149,12 +136,12 @@
   private void freeRelay() {
     try {
       RelayJNI.setRelay(m_forwardHandle, false);
-    } catch (UncleanStatusException ignored) {
+    } catch (UncleanStatusException | HalHandleException ignored) {
       // do nothing. Ignore
     }
     try {
       RelayJNI.setRelay(m_reverseHandle, false);
-    } catch (UncleanStatusException ignored) {
+    } catch (UncleanStatusException | HalHandleException ignored) {
       // do nothing. Ignore
     }
 
@@ -178,7 +165,6 @@
    *
    * @param value The state to set the relay.
    */
-  @SuppressWarnings("PMD.CyclomaticComplexity")
   public void set(Value value) {
     switch (value) {
       case kOff:
@@ -199,8 +185,8 @@
         break;
       case kForward:
         if (m_direction == Direction.kReverse) {
-          throw new InvalidValueException("A relay configured for reverse cannot be set to "
-              + "forward");
+          throw new InvalidValueException(
+              "A relay configured for reverse cannot be set to " + "forward");
         }
         if (m_direction == Direction.kBoth || m_direction == Direction.kForward) {
           RelayJNI.setRelay(m_forwardHandle, true);
@@ -211,8 +197,8 @@
         break;
       case kReverse:
         if (m_direction == Direction.kForward) {
-          throw new InvalidValueException("A relay configured for forward cannot be set to "
-              + "reverse");
+          throw new InvalidValueException(
+              "A relay configured for forward cannot be set to " + "reverse");
         }
         if (m_direction == Direction.kBoth) {
           RelayJNI.setRelay(m_forwardHandle, false);
@@ -310,7 +296,9 @@
     builder.setSmartDashboardType("Relay");
     builder.setActuator(true);
     builder.setSafeState(() -> set(Value.kOff));
-    builder.addStringProperty("Value", () -> get().getPrettyValue(),
+    builder.addStringProperty(
+        "Value",
+        () -> get().getPrettyValue(),
         value -> set(Value.getValueOf(value).orElse(Value.kOff)));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
index 39a0eac..09137d6 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Resource.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -29,9 +26,7 @@
   private final int m_size;
   private final Resource m_nextResource;
 
-  /**
-   * Clears all allocated resources.
-   */
+  /** Clears all allocated resources. */
   public static void restartProgram() {
     for (Resource r = Resource.resourceList; r != null; r = r.m_nextResource) {
       for (int i = 0; i < r.m_size; i++) {
@@ -42,8 +37,8 @@
 
   /**
    * Allocate storage for a new instance of Resource. Allocate a bool array of values that will get
-   * initialized to indicate that no resources have been allocated yet. The indices of the
-   * resources are 0..size-1.
+   * initialized to indicate that no resources have been allocated yet. The indices of the resources
+   * are 0..size-1.
    *
    * @param size The number of blocks to allocate
    */
@@ -106,5 +101,4 @@
     }
     m_numAllocated[index] = false;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
index 9c3e45f..8d1fb07 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotBase.java
@@ -1,23 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.io.File;
-import java.io.IOException;
-import java.io.OutputStream;
-import java.nio.charset.StandardCharsets;
-import java.nio.file.Files;
-import java.util.concurrent.locks.ReentrantLock;
-import java.util.function.Supplier;
-
-import edu.wpi.cscore.CameraServerJNI;
 import edu.wpi.first.cameraserver.CameraServerShared;
 import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.cscore.CameraServerJNI;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -29,6 +18,13 @@
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.util.WPILibVersion;
+import java.io.File;
+import java.io.IOException;
+import java.io.OutputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.util.concurrent.locks.ReentrantLock;
+import java.util.function.Supplier;
 
 /**
  * Implement a Robot Program framework. The RobotBase class is intended to be subclassed by a user
@@ -38,106 +34,107 @@
  * might be spawned as a task, then killed at the end of the Autonomous period.
  */
 public abstract class RobotBase implements AutoCloseable {
-  /**
-   * The ID of the main Java thread.
-   */
+  /** The ID of the main Java thread. */
   // This is usually 1, but it is best to make sure
   private static long m_threadId = -1;
 
   private static void setupCameraServerShared() {
-    CameraServerShared shared = new CameraServerShared() {
+    CameraServerShared shared =
+        new CameraServerShared() {
+          @Override
+          public void reportVideoServer(int id) {
+            HAL.report(tResourceType.kResourceType_PCVideoServer, id + 1);
+          }
 
-      @Override
-      public void reportVideoServer(int id) {
-        HAL.report(tResourceType.kResourceType_PCVideoServer, id + 1);
-      }
+          @Override
+          public void reportUsbCamera(int id) {
+            HAL.report(tResourceType.kResourceType_UsbCamera, id + 1);
+          }
 
-      @Override
-      public void reportUsbCamera(int id) {
-        HAL.report(tResourceType.kResourceType_UsbCamera, id + 1);
-      }
+          @Override
+          public void reportDriverStationError(String error) {
+            DriverStation.reportError(error, true);
+          }
 
-      @Override
-      public void reportDriverStationError(String error) {
-        DriverStation.reportError(error, true);
-      }
+          @Override
+          public void reportAxisCamera(int id) {
+            HAL.report(tResourceType.kResourceType_AxisCamera, id + 1);
+          }
 
-      @Override
-      public void reportAxisCamera(int id) {
-        HAL.report(tResourceType.kResourceType_AxisCamera, id + 1);
-      }
+          @Override
+          public Long getRobotMainThreadId() {
+            return RobotBase.getMainThreadId();
+          }
 
-      @Override
-      public Long getRobotMainThreadId() {
-        return RobotBase.getMainThreadId();
-      }
-
-      @Override
-      public boolean isRoboRIO() {
-        return RobotBase.isReal();
-      }
-    };
+          @Override
+          public boolean isRoboRIO() {
+            return RobotBase.isReal();
+          }
+        };
 
     CameraServerSharedStore.setCameraServerShared(shared);
   }
 
   private static void setupMathShared() {
-    MathSharedStore.setMathShared(new MathShared() {
-      @Override
-      public void reportError(String error, StackTraceElement[] stackTrace) {
-        DriverStation.reportError(error, stackTrace);
-      }
+    MathSharedStore.setMathShared(
+        new MathShared() {
+          @Override
+          public void reportError(String error, StackTraceElement[] stackTrace) {
+            DriverStation.reportError(error, stackTrace);
+          }
 
-      @Override
-      public void reportUsage(MathUsageId id, int count) {
-        switch (id) {
-          case kKinematics_DifferentialDrive:
-            HAL.report(tResourceType.kResourceType_Kinematics,
-                tInstances.kKinematics_DifferentialDrive);
-            break;
-          case kKinematics_MecanumDrive:
-            HAL.report(tResourceType.kResourceType_Kinematics,
-                tInstances.kKinematics_MecanumDrive);
-            break;
-          case kKinematics_SwerveDrive:
-            HAL.report(tResourceType.kResourceType_Kinematics,
-                tInstances.kKinematics_SwerveDrive);
-            break;
-          case kTrajectory_TrapezoidProfile:
-            HAL.report(tResourceType.kResourceType_TrapezoidProfile, count);
-            break;
-          case kFilter_Linear:
-            HAL.report(tResourceType.kResourceType_LinearFilter, count);
-            break;
-          case kOdometry_DifferentialDrive:
-            HAL.report(tResourceType.kResourceType_Odometry,
-                tInstances.kOdometry_DifferentialDrive);
-            break;
-          case kOdometry_SwerveDrive:
-            HAL.report(tResourceType.kResourceType_Odometry,
-                tInstances.kOdometry_SwerveDrive);
-            break;
-          case kOdometry_MecanumDrive:
-            HAL.report(tResourceType.kResourceType_Odometry,
-                tInstances.kOdometry_MecanumDrive);
-            break;
-          default:
-            break;
-        }
-      }
-    });
+          @Override
+          public void reportUsage(MathUsageId id, int count) {
+            switch (id) {
+              case kKinematics_DifferentialDrive:
+                HAL.report(
+                    tResourceType.kResourceType_Kinematics,
+                    tInstances.kKinematics_DifferentialDrive);
+                break;
+              case kKinematics_MecanumDrive:
+                HAL.report(
+                    tResourceType.kResourceType_Kinematics, tInstances.kKinematics_MecanumDrive);
+                break;
+              case kKinematics_SwerveDrive:
+                HAL.report(
+                    tResourceType.kResourceType_Kinematics, tInstances.kKinematics_SwerveDrive);
+                break;
+              case kTrajectory_TrapezoidProfile:
+                HAL.report(tResourceType.kResourceType_TrapezoidProfile, count);
+                break;
+              case kFilter_Linear:
+                HAL.report(tResourceType.kResourceType_LinearFilter, count);
+                break;
+              case kOdometry_DifferentialDrive:
+                HAL.report(
+                    tResourceType.kResourceType_Odometry, tInstances.kOdometry_DifferentialDrive);
+                break;
+              case kOdometry_SwerveDrive:
+                HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_SwerveDrive);
+                break;
+              case kOdometry_MecanumDrive:
+                HAL.report(tResourceType.kResourceType_Odometry, tInstances.kOdometry_MecanumDrive);
+                break;
+              case kController_PIDController2:
+                HAL.report(tResourceType.kResourceType_PIDController2, count);
+                break;
+              case kController_ProfiledPIDController:
+                HAL.report(tResourceType.kResourceType_ProfiledPIDController, count);
+                break;
+              default:
+                break;
+            }
+          }
+        });
   }
 
-  protected final DriverStation m_ds;
-
   /**
    * Constructor for a generic robot program. User code should be placed in the constructor that
    * runs before the Autonomous or Operator Control period starts. The constructor will run to
    * completion before Autonomous is entered.
    *
    * <p>This must be used to ensure that the communications code starts. In the future it would be
-   * nice
-   * to put this code into it's own task that loads on boot so ensure that it runs.
+   * nice to put this code into it's own task that loads on boot so ensure that it runs.
    */
   protected RobotBase() {
     final NetworkTableInstance inst = NetworkTableInstance.getDefault();
@@ -150,7 +147,6 @@
     } else {
       inst.startServer();
     }
-    m_ds = DriverStation.getInstance();
     inst.getTable("LiveWindow").getSubTable(".status").getEntry("LW Enabled").setBoolean(false);
 
     LiveWindow.setEnabled(false);
@@ -162,7 +158,15 @@
   }
 
   @Override
-  public void close() {
+  public void close() {}
+
+  /**
+   * Get the current runtime type.
+   *
+   * @return Current runtime type.
+   */
+  public static RuntimeType getRuntimeType() {
+    return RuntimeType.getValue(HALUtil.getHALRuntimeType());
   }
 
   /**
@@ -180,7 +184,8 @@
    * @return If the robot is running in the real world.
    */
   public static boolean isReal() {
-    return HALUtil.getHALRuntimeType() == 0;
+    RuntimeType runtimeType = getRuntimeType();
+    return runtimeType == RuntimeType.kRoboRIO || runtimeType == RuntimeType.kRoboRIO2;
   }
 
   /**
@@ -189,7 +194,7 @@
    * @return True if the Robot is currently disabled by the field controls.
    */
   public boolean isDisabled() {
-    return m_ds.isDisabled();
+    return DriverStation.isDisabled();
   }
 
   /**
@@ -198,37 +203,47 @@
    * @return True if the Robot is currently enabled by the field controls.
    */
   public boolean isEnabled() {
-    return m_ds.isEnabled();
+    return DriverStation.isEnabled();
   }
 
   /**
-   * Determine if the robot is currently in Autonomous mode as determined by the field
-   * controls.
+   * Determine if the robot is currently in Autonomous mode as determined by the field controls.
    *
    * @return True if the robot is currently operating Autonomously.
    */
   public boolean isAutonomous() {
-    return m_ds.isAutonomous();
+    return DriverStation.isAutonomous();
   }
 
   /**
-   * Determine if the robot is current in Autonomous mode and enabled as determined by
-   * the field controls.
+   * Determine if the robot is current in Autonomous mode and enabled as determined by the field
+   * controls.
    *
    * @return True if the robot is currently operating autonomously while enabled.
    */
   public boolean isAutonomousEnabled() {
-    return m_ds.isAutonomousEnabled();
+    return DriverStation.isAutonomousEnabled();
   }
 
   /**
-   * Determine if the robot is currently in Test mode as determined by the driver
-   * station.
+   * Determine if the robot is currently in Test mode as determined by the driver station.
    *
    * @return True if the robot is currently operating in Test mode.
    */
   public boolean isTest() {
-    return m_ds.isTest();
+    return DriverStation.isTest();
+  }
+
+  /**
+   * Determine if the robot is currently in Operator Control mode as determined by the field
+   * controls.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode.
+   * @deprecated Use isTeleop() instead.
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public boolean isOperatorControl() {
+    return DriverStation.isTeleop();
   }
 
   /**
@@ -237,18 +252,30 @@
    *
    * @return True if the robot is currently operating in Tele-Op mode.
    */
-  public boolean isOperatorControl() {
-    return m_ds.isOperatorControl();
+  public boolean isTeleop() {
+    return DriverStation.isTeleop();
   }
 
   /**
-   * Determine if the robot is current in Operator Control mode and enabled as determined by
-   * the field controls.
+   * Determine if the robot is current in Operator Control mode and enabled as determined by the
+   * field controls.
+   *
+   * @return True if the robot is currently operating in Tele-Op mode while enabled.
+   * @deprecated Use isTeleopEnabled() instead.
+   */
+  @Deprecated(since = "2022", forRemoval = true)
+  public boolean isOperatorControlEnabled() {
+    return DriverStation.isTeleopEnabled();
+  }
+
+  /**
+   * Determine if the robot is current in Operator Control mode and enabled as determined by the
+   * field controls.
    *
    * @return True if the robot is currently operating in Tele-Op mode while enabled.
    */
-  public boolean isOperatorControlEnabled() {
-    return m_ds.isOperatorControlEnabled();
+  public boolean isTeleopEnabled() {
+    return DriverStation.isTeleopEnabled();
   }
 
   /**
@@ -257,20 +284,16 @@
    * @return Has new data arrived over the network since the last time this function was called?
    */
   public boolean isNewDataAvailable() {
-    return m_ds.isNewControlData();
+    return DriverStation.isNewControlData();
   }
 
-  /**
-   * Provide an alternate "main loop" via startCompetition().
-   */
+  /** Provide an alternate "main loop" via startCompetition(). */
   public abstract void startCompetition();
 
-  /**
-   * Ends the main loop in startCompetition().
-   */
+  /** Ends the main loop in startCompetition(). */
   public abstract void endCompetition();
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static boolean getBooleanProperty(String name, boolean defaultValue) {
     String propVal = System.getProperty(name);
     if (propVal == null) {
@@ -289,11 +312,8 @@
   private static RobotBase m_robotCopy;
   private static boolean m_suppressExitWarning;
 
-  /**
-   * Run the robot main loop.
-   */
-  @SuppressWarnings({"PMD.AvoidInstantiatingObjectsInLoops", "PMD.AvoidCatchingThrowable",
-                     "PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
+  /** Run the robot main loop. */
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
   private static <T extends RobotBase> void runRobot(Supplier<T> robotSupplier) {
     System.out.println("********** Robot program starting **********");
 
@@ -310,9 +330,15 @@
       if (elements.length > 0) {
         robotName = elements[0].getClassName();
       }
-      DriverStation.reportError("Unhandled exception instantiating robot " + robotName + " "
-          + throwable.toString(), elements);
-      DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+      DriverStation.reportError(
+          "Unhandled exception instantiating robot " + robotName + " " + throwable.toString(),
+          elements);
+      DriverStation.reportError(
+          "The robot program quit unexpectedly."
+              + " This is usually due to a code error.\n"
+              + "  The above stacktrace can help determine where the error occurred.\n"
+              + "  See https://wpilib.org/stacktrace for more information.\n",
+          false);
       DriverStation.reportError("Could not instantiate robot " + robotName + "!", false);
       return;
     }
@@ -322,23 +348,23 @@
     m_runMutex.unlock();
 
     if (isReal()) {
+      final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini");
       try {
-        final File file = new File("/tmp/frc_versions/FRC_Lib_Version.ini");
-
-        if (file.exists()) {
-          file.delete();
+        if (file.exists() && !file.delete()) {
+          throw new IOException("Failed to delete FRC_Lib_Version.ini");
         }
 
-        file.createNewFile();
+        if (!file.createNewFile()) {
+          throw new IOException("Failed to create new FRC_Lib_Version.ini");
+        }
 
         try (OutputStream output = Files.newOutputStream(file.toPath())) {
           output.write("Java ".getBytes(StandardCharsets.UTF_8));
           output.write(WPILibVersion.Version.getBytes(StandardCharsets.UTF_8));
         }
-
       } catch (IOException ex) {
-        DriverStation.reportError("Could not write FRC_Lib_Version.ini: " + ex.toString(),
-                ex.getStackTrace());
+        DriverStation.reportError(
+            "Could not write FRC_Lib_Version.ini: " + ex.toString(), ex.getStackTrace());
       }
     }
 
@@ -350,8 +376,8 @@
       if (cause != null) {
         throwable = cause;
       }
-      DriverStation.reportError("Unhandled exception: " + throwable.toString(),
-          throwable.getStackTrace());
+      DriverStation.reportError(
+          "Unhandled exception: " + throwable.toString(), throwable.getStackTrace());
       errorOnExit = true;
     } finally {
       m_runMutex.lock();
@@ -359,11 +385,17 @@
       m_runMutex.unlock();
       if (!suppressExitWarning) {
         // startCompetition never returns unless exception occurs....
-        DriverStation.reportWarning("Robots should not quit, but yours did!", false);
+        DriverStation.reportWarning(
+            "The robot program quit unexpectedly."
+                + " This is usually due to a code error.\n"
+                + "  The above stacktrace can help determine where the error occurred.\n"
+                + "  See https://wpilib.org/stacktrace for more information.",
+            false);
         if (errorOnExit) {
           DriverStation.reportError(
               "The startCompetition() method (or methods called by it) should have "
-                  + "handled the exception above.", false);
+                  + "handled the exception above.",
+              false);
         } else {
           DriverStation.reportError("Unexpected return from startCompetition() method.", false);
         }
@@ -372,7 +404,9 @@
   }
 
   /**
-   * Suppress the "Robots should not quit" message.
+   * Suppress the "The robot program quit unexpectedly." message.
+   *
+   * @param value True if exit warning should be suppressed.
    */
   public static void suppressExitWarning(boolean value) {
     m_runMutex.lock();
@@ -382,6 +416,9 @@
 
   /**
    * Starting point for the applications.
+   *
+   * @param <T> Robot subclass.
+   * @param robotSupplier Function that returns an instance of the robot subclass.
    */
   public static <T extends RobotBase> void startRobot(Supplier<T> robotSupplier) {
     if (!HAL.initialize(500, 0)) {
@@ -392,14 +429,21 @@
     // Needed because all the OpenCV JNI functions don't have built in loading
     CameraServerJNI.enumerateSinks();
 
-    HAL.report(tResourceType.kResourceType_Language, tInstances.kLanguage_Java, 0,
-        WPILibVersion.Version);
+    HAL.report(
+        tResourceType.kResourceType_Language, tInstances.kLanguage_Java, 0, WPILibVersion.Version);
+
+    if (!Notifier.setHALThreadPriority(true, 40)) {
+      DriverStation.reportWarning("Setting HAL Notifier RT priority to 40 failed", false);
+    }
 
     if (HAL.hasMain()) {
-      Thread thread = new Thread(() -> {
-        runRobot(robotSupplier);
-        HAL.exitMain();
-      }, "robot main");
+      Thread thread =
+          new Thread(
+              () -> {
+                runRobot(robotSupplier);
+                HAL.exitMain();
+              },
+              "robot main");
       thread.setDaemon(true);
       thread.start();
       HAL.runMain();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
index 0623812..8a7460e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotController.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -13,17 +10,14 @@
 import edu.wpi.first.hal.can.CANJNI;
 import edu.wpi.first.hal.can.CANStatus;
 
-/**
- * Contains functions for roboRIO functionality.
- */
+/** Contains functions for roboRIO functionality. */
 public final class RobotController {
   private RobotController() {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
   /**
-   * Return the FPGA Version number. For now, expect this to be the current
-   * year.
+   * Return the FPGA Version number. For now, expect this to be the current year.
    *
    * @return FPGA Version number.
    */
@@ -120,7 +114,7 @@
   /**
    * Get the current output of the 3.3V rail.
    *
-   * @return The controller 3.3V rail output current value in Volts
+   * @return The controller 3.3V rail output current value in Amps
    */
   public static double getCurrent3V3() {
     return PowerJNI.getUserCurrent3V3();
@@ -220,13 +214,33 @@
   }
 
   /**
+   * Get the current brownout voltage setting.
+   *
+   * @return The brownout voltage
+   */
+  public static double getBrownoutVoltage() {
+    return PowerJNI.getBrownoutVoltage();
+  }
+
+  /**
+   * Set the voltage the roboRIO will brownout and disable all outputs.
+   *
+   * <p>Note that this only does anything on the roboRIO 2. On the roboRIO it is a no-op.
+   *
+   * @param brownoutVoltage The brownout voltage
+   */
+  public static void setBrownoutVoltage(double brownoutVoltage) {
+    PowerJNI.setBrownoutVoltage(brownoutVoltage);
+  }
+
+  /**
    * Get the current status of the CAN bus.
    *
    * @return The status of the CAN bus
    */
   public static CANStatus getCANStatus() {
     CANStatus status = new CANStatus();
-    CANJNI.GetCANStatus(status);
+    CANJNI.getCANStatus(status);
     return status;
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
deleted file mode 100644
index f79d74a..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotDrive.java
+++ /dev/null
@@ -1,716 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tInstances;
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
-/**
- * Utility class for handling Robot drive based on a definition of the motor configuration. The
- * robot drive class handles basic driving for a robot. Currently, 2 and 4 motor tank and mecanum
- * drive trains are supported. In the future other drive types like swerve might be implemented.
- * Motor channel numbers are 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 {@link edu.wpi.first.wpilibj.drive.DifferentialDrive}
- *             or {@link edu.wpi.first.wpilibj.drive.MecanumDrive} classes instead.
- */
-@Deprecated
-@SuppressWarnings("PMD.GodClass")
-public class RobotDrive extends MotorSafety implements AutoCloseable {
-  /**
-   * The location of a motor on the robot for the purpose of driving.
-   */
-  public enum MotorType {
-    kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3);
-
-    @SuppressWarnings("MemberName")
-    public final int value;
-
-    MotorType(int value) {
-      this.value = value;
-    }
-  }
-
-  public static final double kDefaultExpirationTime = 0.1;
-  public static final double kDefaultSensitivity = 0.5;
-  public static final double kDefaultMaxOutput = 1.0;
-  protected static final int kMaxNumberOfMotors = 4;
-  protected double m_sensitivity;
-  protected double m_maxOutput;
-  protected SpeedController m_frontLeftMotor;
-  protected SpeedController m_frontRightMotor;
-  protected SpeedController m_rearLeftMotor;
-  protected SpeedController m_rearRightMotor;
-  protected boolean m_allocatedSpeedControllers;
-  protected static boolean kArcadeRatioCurve_Reported;
-  protected static boolean kTank_Reported;
-  protected static boolean kArcadeStandard_Reported;
-  protected static boolean kMecanumCartesian_Reported;
-  protected static boolean kMecanumPolar_Reported;
-
-  /**
-   * Constructor for RobotDrive with 2 motors specified with channel numbers. Set up parameters for
-   * a two wheel drive system where the left and right motor pwm channels are specified in the call.
-   * This call assumes Talons for controlling the motors.
-   *
-   * @param leftMotorChannel  The PWM channel number that drives the left motor.
-   * @param rightMotorChannel The PWM channel number that drives the right motor.
-   */
-  public RobotDrive(final int leftMotorChannel, final int rightMotorChannel) {
-    m_sensitivity = kDefaultSensitivity;
-    m_maxOutput = kDefaultMaxOutput;
-    m_frontLeftMotor = null;
-    m_rearLeftMotor = new Talon(leftMotorChannel);
-    m_frontRightMotor = null;
-    m_rearRightMotor = new Talon(rightMotorChannel);
-    m_allocatedSpeedControllers = true;
-    setSafetyEnabled(true);
-    drive(0, 0);
-  }
-
-  /**
-   * Constructor for RobotDrive with 4 motors specified with channel numbers. Set up parameters for
-   * a four wheel drive system where all four motor pwm channels are specified in the call. This
-   * call assumes Talons for controlling the motors.
-   *
-   * @param frontLeftMotor  Front left motor channel number
-   * @param rearLeftMotor   Rear Left motor channel number
-   * @param frontRightMotor Front right motor channel number
-   * @param rearRightMotor  Rear Right motor channel number
-   */
-  public RobotDrive(final int frontLeftMotor, final int rearLeftMotor, final int frontRightMotor,
-                    final int rearRightMotor) {
-    m_sensitivity = kDefaultSensitivity;
-    m_maxOutput = kDefaultMaxOutput;
-    m_rearLeftMotor = new Talon(rearLeftMotor);
-    m_rearRightMotor = new Talon(rearRightMotor);
-    m_frontLeftMotor = new Talon(frontLeftMotor);
-    m_frontRightMotor = new Talon(frontRightMotor);
-    m_allocatedSpeedControllers = true;
-    setSafetyEnabled(true);
-    drive(0, 0);
-  }
-
-  /**
-   * Constructor for RobotDrive with 2 motors specified as SpeedController objects. The
-   * SpeedController version of the constructor enables programs to use the RobotDrive classes with
-   * subclasses of the SpeedController objects, for example, versions with ramping or reshaping of
-   * the curve to suit motor bias or dead-band elimination.
-   *
-   * @param leftMotor  The left SpeedController object used to drive the robot.
-   * @param rightMotor the right SpeedController object used to drive the robot.
-   */
-  public RobotDrive(SpeedController leftMotor, SpeedController rightMotor) {
-    requireNonNullParam(leftMotor, "leftMotor", "RobotDrive");
-    requireNonNullParam(rightMotor, "rightMotor", "RobotDrive");
-
-    m_frontLeftMotor = null;
-    m_rearLeftMotor = leftMotor;
-    m_frontRightMotor = null;
-    m_rearRightMotor = rightMotor;
-    m_sensitivity = kDefaultSensitivity;
-    m_maxOutput = kDefaultMaxOutput;
-    m_allocatedSpeedControllers = false;
-    setSafetyEnabled(true);
-    drive(0, 0);
-  }
-
-  /**
-   * Constructor for RobotDrive with 4 motors specified as SpeedController objects. Speed controller
-   * input version of RobotDrive (see previous comments).
-   *
-   * @param frontLeftMotor  The front left SpeedController object used to drive the robot
-   * @param rearLeftMotor   The back left SpeedController object used to drive the robot.
-   * @param frontRightMotor The front right SpeedController object used to drive the robot.
-   * @param rearRightMotor  The back right SpeedController object used to drive the robot.
-   */
-  public RobotDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
-                    SpeedController frontRightMotor, SpeedController rearRightMotor) {
-    m_frontLeftMotor = requireNonNullParam(frontLeftMotor, "frontLeftMotor", "RobotDrive");
-    m_rearLeftMotor = requireNonNullParam(rearLeftMotor, "rearLeftMotor", "RobotDrive");
-    m_frontRightMotor = requireNonNullParam(frontRightMotor, "frontRightMotor", "RobotDrive");
-    m_rearRightMotor = requireNonNullParam(rearRightMotor, "rearRightMotor", "RobotDrive");
-    m_sensitivity = kDefaultSensitivity;
-    m_maxOutput = kDefaultMaxOutput;
-    m_allocatedSpeedControllers = false;
-    setSafetyEnabled(true);
-    drive(0, 0);
-  }
-
-  /**
-   * Drive the motors at "outputMagnitude" and "curve". Both outputMagnitude and curve are -1.0 to
-   * +1.0 values, where 0.0 represents stopped and not turning. {@literal curve < 0 will turn left
-   * and curve > 0} will turn right.
-   *
-   * <p>The algorithm for steering provides a constant turn radius for any normal speed range, both
-   * forward and backward. Increasing sensitivity causes sharper turns for fixed values of curve.
-   *
-   * <p>This function will most likely be used in an autonomous routine.
-   *
-   * @param outputMagnitude The speed setting for the outside wheel in a turn, forward or backwards,
-   *                        +1 to -1.
-   * @param curve           The rate of turn, constant for different forward speeds. Set {@literal
-   *                        curve < 0 for left turn or curve > 0 for right turn.} Set curve =
-   *                        e^(-r/w) to get a turn radius r for wheelbase w of your robot.
-   *                        Conversely, turn radius r = -ln(curve)*w for a given value of curve and
-   *                        wheelbase w.
-   */
-  public void drive(double outputMagnitude, double curve) {
-    final double leftOutput;
-    final double rightOutput;
-
-    if (!kArcadeRatioCurve_Reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_ArcadeRatioCurve,
-          getNumMotors());
-      kArcadeRatioCurve_Reported = true;
-    }
-    if (curve < 0) {
-      double value = Math.log(-curve);
-      double ratio = (value - m_sensitivity) / (value + m_sensitivity);
-      if (ratio == 0) {
-        ratio = 0.0000000001;
-      }
-      leftOutput = outputMagnitude / ratio;
-      rightOutput = outputMagnitude;
-    } else if (curve > 0) {
-      double value = Math.log(curve);
-      double ratio = (value - m_sensitivity) / (value + m_sensitivity);
-      if (ratio == 0) {
-        ratio = 0.0000000001;
-      }
-      leftOutput = outputMagnitude;
-      rightOutput = outputMagnitude / ratio;
-    } else {
-      leftOutput = outputMagnitude;
-      rightOutput = outputMagnitude;
-    }
-    setLeftRightMotorOutputs(leftOutput, rightOutput);
-  }
-
-  /**
-   * Provide tank steering using the stored robot configuration. drive the robot using two joystick
-   * inputs. The Y-axis will be selected from each Joystick object. The calculated values will be
-   * squared to decrease sensitivity at low speeds.
-   *
-   * @param leftStick  The joystick to control the left side of the robot.
-   * @param rightStick The joystick to control the right side of the robot.
-   */
-  public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
-    requireNonNullParam(leftStick, "leftStick", "tankDrive");
-    requireNonNullParam(rightStick, "rightStick", "tankDrive");
-
-    tankDrive(leftStick.getY(), rightStick.getY(), true);
-  }
-
-  /**
-   * Provide tank steering using the stored robot configuration. drive the robot using two joystick
-   * inputs. The Y-axis will be selected from each Joystick object.
-   *
-   * @param leftStick     The joystick to control the left side of the robot.
-   * @param rightStick    The joystick to control the right side of the robot.
-   * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
-   */
-  public void tankDrive(GenericHID leftStick, GenericHID rightStick, boolean squaredInputs) {
-    requireNonNullParam(leftStick, "leftStick", "tankDrive");
-    requireNonNullParam(rightStick, "rightStick", "tankDrive");
-
-    tankDrive(leftStick.getY(), rightStick.getY(), squaredInputs);
-  }
-
-  /**
-   * Provide tank steering using the stored robot configuration. This function lets you pick the
-   * axis to be used on each Joystick object for the left and right sides of the robot. The
-   * calculated values will be squared to decrease sensitivity at low speeds.
-   *
-   * @param leftStick  The Joystick object to use for the left side of the robot.
-   * @param leftAxis   The axis to select on the left side Joystick object.
-   * @param rightStick The Joystick object to use for the right side of the robot.
-   * @param rightAxis  The axis to select on the right side Joystick object.
-   */
-  public void tankDrive(GenericHID leftStick, final int leftAxis, GenericHID rightStick,
-                        final int rightAxis) {
-    requireNonNullParam(leftStick, "leftStick", "tankDrive");
-    requireNonNullParam(rightStick, "rightStick", "tankDrive");
-
-    tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), true);
-  }
-
-  /**
-   * Provide tank steering using the stored robot configuration. This function lets you pick the
-   * axis to be used on each Joystick object for the left and right sides of the robot.
-   *
-   * @param leftStick     The Joystick object to use for the left side of the robot.
-   * @param leftAxis      The axis to select on the left side Joystick object.
-   * @param rightStick    The Joystick object to use for the right side of the robot.
-   * @param rightAxis     The axis to select on the right side Joystick object.
-   * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
-   */
-  public void tankDrive(GenericHID leftStick, final int leftAxis, GenericHID rightStick,
-                        final int rightAxis, boolean squaredInputs) {
-    requireNonNullParam(leftStick, "leftStick", "tankDrive");
-    requireNonNullParam(rightStick, "rightStick", "tankDrive");
-
-    tankDrive(leftStick.getRawAxis(leftAxis), rightStick.getRawAxis(rightAxis), squaredInputs);
-  }
-
-  /**
-   * Provide tank steering using the stored robot configuration. This function lets you directly
-   * provide joystick values from any source.
-   *
-   * @param leftValue     The value of the left stick.
-   * @param rightValue    The value of the right stick.
-   * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
-   */
-  public void tankDrive(double leftValue, double rightValue, boolean squaredInputs) {
-    if (!kTank_Reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_Tank,
-          getNumMotors());
-      kTank_Reported = true;
-    }
-
-    leftValue = limit(leftValue);
-    rightValue = limit(rightValue);
-
-    // square the inputs (while preserving the sign) to increase fine control
-    // while permitting full power
-    if (squaredInputs) {
-      leftValue = Math.copySign(leftValue * leftValue, leftValue);
-      rightValue = Math.copySign(rightValue * rightValue, rightValue);
-    }
-    setLeftRightMotorOutputs(leftValue, rightValue);
-  }
-
-  /**
-   * Provide tank steering using the stored robot configuration. This function lets you directly
-   * provide joystick values from any source. The calculated values will be squared to decrease
-   * sensitivity at low speeds.
-   *
-   * @param leftValue  The value of the left stick.
-   * @param rightValue The value of the right stick.
-   */
-  public void tankDrive(double leftValue, double rightValue) {
-    tankDrive(leftValue, rightValue, true);
-  }
-
-  /**
-   * Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
-   * axis for the move value and the X axis for the rotate value. (Should add more information here
-   * regarding the way that arcade drive works.)
-   *
-   * @param stick         The joystick to use for Arcade single-stick driving. The Y-axis will be
-   *                      selected for forwards/backwards and the X-axis will be selected for
-   *                      rotation rate.
-   * @param squaredInputs If true, the sensitivity will be decreased for small values
-   */
-  public void arcadeDrive(GenericHID stick, boolean squaredInputs) {
-    // simply call the full-featured arcadeDrive with the appropriate values
-    arcadeDrive(stick.getY(), stick.getX(), squaredInputs);
-  }
-
-  /**
-   * Arcade drive implements single stick driving. Given a single Joystick, the class assumes the Y
-   * axis for the move value and the X axis for the rotate value. (Should add more information here
-   * regarding the way that arcade drive works.) The calculated values will be squared to decrease
-   * sensitivity at low speeds.
-   *
-   * @param stick The joystick to use for Arcade single-stick driving. The Y-axis will be selected
-   *              for forwards/backwards and the X-axis will be selected for rotation rate.
-   */
-  public void arcadeDrive(GenericHID stick) {
-    arcadeDrive(stick, true);
-  }
-
-  /**
-   * Arcade drive implements single stick driving. Given two joystick instances and two axis,
-   * compute the values to send to either two or four motors.
-   *
-   * @param moveStick     The Joystick object that represents the forward/backward direction
-   * @param moveAxis      The axis on the moveStick object to use for forwards/backwards (typically
-   *                      Y_AXIS)
-   * @param rotateStick   The Joystick object that represents the rotation value
-   * @param rotateAxis    The axis on the rotation object to use for the rotate right/left
-   *                      (typically X_AXIS)
-   * @param squaredInputs Setting this parameter to true decreases the sensitivity at lower speeds
-   */
-  public void arcadeDrive(GenericHID moveStick, final int moveAxis, GenericHID rotateStick,
-                          final int rotateAxis, boolean squaredInputs) {
-    double moveValue = moveStick.getRawAxis(moveAxis);
-    double rotateValue = rotateStick.getRawAxis(rotateAxis);
-
-    arcadeDrive(moveValue, rotateValue, squaredInputs);
-  }
-
-  /**
-   * Arcade drive implements single stick driving. Given two joystick instances and two axis,
-   * compute the values to send to either two or four motors. The calculated values will be
-   * squared to decrease sensitivity at low speeds.
-   *
-   * @param moveStick   The Joystick object that represents the forward/backward direction
-   * @param moveAxis    The axis on the moveStick object to use for forwards/backwards (typically
-   *                    Y_AXIS)
-   * @param rotateStick The Joystick object that represents the rotation value
-   * @param rotateAxis  The axis on the rotation object to use for the rotate right/left (typically
-   *                    X_AXIS)
-   */
-  public void arcadeDrive(GenericHID moveStick, final int moveAxis, GenericHID rotateStick,
-                          final int rotateAxis) {
-    arcadeDrive(moveStick, moveAxis, rotateStick, rotateAxis, true);
-  }
-
-  /**
-   * Arcade drive implements single stick driving. This function lets you directly provide
-   * joystick values from any source.
-   *
-   * @param moveValue     The value to use for forwards/backwards
-   * @param rotateValue   The value to use for the rotate right/left
-   * @param squaredInputs If set, decreases the sensitivity at low speeds
-   */
-  public void arcadeDrive(double moveValue, double rotateValue, boolean squaredInputs) {
-    // local variables to hold the computed PWM values for the motors
-    if (!kArcadeStandard_Reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_ArcadeStandard,
-          getNumMotors());
-      kArcadeStandard_Reported = true;
-    }
-
-    double leftMotorSpeed;
-    double rightMotorSpeed;
-
-    moveValue = limit(moveValue);
-    rotateValue = limit(rotateValue);
-
-    // square the inputs (while preserving the sign) to increase fine control
-    // while permitting full power
-    if (squaredInputs) {
-      // square the inputs (while preserving the sign) to increase fine control
-      // while permitting full power
-      moveValue = Math.copySign(moveValue * moveValue, moveValue);
-      rotateValue = Math.copySign(rotateValue * rotateValue, rotateValue);
-    }
-
-    if (moveValue > 0.0) {
-      if (rotateValue > 0.0) {
-        leftMotorSpeed = moveValue - rotateValue;
-        rightMotorSpeed = Math.max(moveValue, rotateValue);
-      } else {
-        leftMotorSpeed = Math.max(moveValue, -rotateValue);
-        rightMotorSpeed = moveValue + rotateValue;
-      }
-    } else {
-      if (rotateValue > 0.0) {
-        leftMotorSpeed = -Math.max(-moveValue, rotateValue);
-        rightMotorSpeed = moveValue + rotateValue;
-      } else {
-        leftMotorSpeed = moveValue - rotateValue;
-        rightMotorSpeed = -Math.max(-moveValue, -rotateValue);
-      }
-    }
-
-    setLeftRightMotorOutputs(leftMotorSpeed, rightMotorSpeed);
-  }
-
-  /**
-   * Arcade drive implements single stick driving. This function lets you directly provide
-   * joystick values from any source. The calculated values will be squared to decrease
-   * sensitivity at low speeds.
-   *
-   * @param moveValue   The value to use for forwards/backwards
-   * @param rotateValue The value to use for the rotate right/left
-   */
-  public void arcadeDrive(double moveValue, double rotateValue) {
-    arcadeDrive(moveValue, rotateValue, true);
-  }
-
-  /**
-   * Drive method for Mecanum wheeled robots.
-   *
-   * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
-   * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
-   * top, the roller axles should form an X across the robot.
-   *
-   * <p>This is designed to be directly driven by joystick axes.
-   *
-   * @param x         The speed that the robot should drive in the X direction. [-1.0..1.0]
-   * @param y         The speed that the robot should drive in the Y direction. This input is
-   *                  inverted to match the forward == -1.0 that joysticks produce. [-1.0..1.0]
-   * @param rotation  The rate of rotation for the robot that is completely independent of the
-   *                  translation. [-1.0..1.0]
-   * @param gyroAngle The current angle reading from the gyro. Use this to implement field-oriented
-   *                  controls.
-   */
-  @SuppressWarnings("ParameterName")
-  public void mecanumDrive_Cartesian(double x, double y, double rotation, double gyroAngle) {
-    if (!kMecanumCartesian_Reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_MecanumCartesian,
-          getNumMotors());
-      kMecanumCartesian_Reported = true;
-    }
-    @SuppressWarnings("LocalVariableName")
-    double xIn = x;
-    @SuppressWarnings("LocalVariableName")
-    double yIn = y;
-    // Negate y for the joystick.
-    yIn = -yIn;
-    // Compensate for gyro angle.
-    double[] rotated = rotateVector(xIn, yIn, gyroAngle);
-    xIn = rotated[0];
-    yIn = rotated[1];
-
-    double[] wheelSpeeds = new double[kMaxNumberOfMotors];
-    wheelSpeeds[MotorType.kFrontLeft.value] = xIn + yIn + rotation;
-    wheelSpeeds[MotorType.kFrontRight.value] = -xIn + yIn - rotation;
-    wheelSpeeds[MotorType.kRearLeft.value] = -xIn + yIn + rotation;
-    wheelSpeeds[MotorType.kRearRight.value] = xIn + yIn - rotation;
-
-    normalize(wheelSpeeds);
-    m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
-    m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
-    m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
-    m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
-
-    feed();
-  }
-
-  /**
-   * Drive method for Mecanum wheeled robots.
-   *
-   * <p>A method for driving with Mecanum wheeled robots. There are 4 wheels on the robot, arranged
-   * so that the front and back wheels are toed in 45 degrees. When looking at the wheels from the
-   * top, the roller axles should form an X across the robot.
-   *
-   * @param magnitude The speed that the robot should drive in a given direction. [-1.0..1.0]
-   * @param direction The angle the robot should drive in degrees. The direction and magnitude
-   *                  are independent of the rotation rate. [-180.0..180.0]
-   * @param rotation  The rate of rotation for the robot that is completely independent of the
-   *                  magnitude or direction. [-1.0..1.0]
-   */
-  public void mecanumDrive_Polar(double magnitude, double direction, double rotation) {
-    if (!kMecanumPolar_Reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive_MecanumPolar,
-          getNumMotors());
-      kMecanumPolar_Reported = true;
-    }
-    // Normalized for full power along the Cartesian axes.
-    magnitude = limit(magnitude) * Math.sqrt(2.0);
-    // The rollers are at 45 degree angles.
-    double dirInRad = (direction + 45.0) * Math.PI / 180.0;
-    double cosD = Math.cos(dirInRad);
-    double sinD = Math.sin(dirInRad);
-
-    double[] wheelSpeeds = new double[kMaxNumberOfMotors];
-    wheelSpeeds[MotorType.kFrontLeft.value] = sinD * magnitude + rotation;
-    wheelSpeeds[MotorType.kFrontRight.value] = cosD * magnitude - rotation;
-    wheelSpeeds[MotorType.kRearLeft.value] = cosD * magnitude + rotation;
-    wheelSpeeds[MotorType.kRearRight.value] = sinD * magnitude - rotation;
-
-    normalize(wheelSpeeds);
-
-    m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
-    m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput);
-    m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
-    m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput);
-
-    feed();
-  }
-
-  /**
-   * Holonomic Drive method for Mecanum wheeled robots.
-   *
-   * <p>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]
-   * @param direction The direction the robot should drive. The direction and maginitude are
-   *                  independent of the rotation rate.
-   * @param rotation  The rate of rotation for the robot that is completely independent of the
-   *                  magnitute or direction. [-1.0..1.0]
-   */
-  void holonomicDrive(double magnitude, double direction, double rotation) {
-    mecanumDrive_Polar(magnitude, direction, rotation);
-  }
-
-  /**
-   * Set the speed of the right and left motors. This is used once an appropriate drive setup
-   * function is called such as twoWheelDrive(). The motors are set to "leftSpeed" and
-   * "rightSpeed" and includes flipping the direction of one side for opposing motors.
-   *
-   * @param leftOutput  The speed to send to the left side of the robot.
-   * @param rightOutput The speed to send to the right side of the robot.
-   */
-  public void setLeftRightMotorOutputs(double leftOutput, double rightOutput) {
-
-    if (m_frontLeftMotor != null) {
-      m_frontLeftMotor.set(limit(leftOutput) * m_maxOutput);
-    }
-    m_rearLeftMotor.set(limit(leftOutput) * m_maxOutput);
-
-    if (m_frontRightMotor != null) {
-      m_frontRightMotor.set(-limit(rightOutput) * m_maxOutput);
-    }
-    m_rearRightMotor.set(-limit(rightOutput) * m_maxOutput);
-
-    feed();
-  }
-
-  /**
-   * Limit motor values to the -1.0 to +1.0 range.
-   */
-  protected static double limit(double number) {
-    if (number > 1.0) {
-      return 1.0;
-    }
-    if (number < -1.0) {
-      return -1.0;
-    }
-    return number;
-  }
-
-  /**
-   * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
-   */
-  protected static void normalize(double[] wheelSpeeds) {
-    double maxMagnitude = Math.abs(wheelSpeeds[0]);
-    for (int i = 1; i < kMaxNumberOfMotors; i++) {
-      double temp = Math.abs(wheelSpeeds[i]);
-      if (maxMagnitude < temp) {
-        maxMagnitude = temp;
-      }
-    }
-    if (maxMagnitude > 1.0) {
-      for (int i = 0; i < kMaxNumberOfMotors; i++) {
-        wheelSpeeds[i] = wheelSpeeds[i] / maxMagnitude;
-      }
-    }
-  }
-
-  /**
-   * Rotate a vector in Cartesian space.
-   */
-  @SuppressWarnings("ParameterName")
-  protected static double[] rotateVector(double x, double y, double angle) {
-    double cosA = Math.cos(angle * (Math.PI / 180.0));
-    double sinA = Math.sin(angle * (Math.PI / 180.0));
-    double[] out = new double[2];
-    out[0] = x * cosA - y * sinA;
-    out[1] = x * sinA + y * cosA;
-    return out;
-  }
-
-  /**
-   * Invert a motor direction. This is used when a motor should run in the opposite direction as the
-   * drive code would normally run it. Motors that are direct drive would be inverted, the drive
-   * code assumes that the motors are geared with one reversal.
-   *
-   * @param motor      The motor index to invert.
-   * @param isInverted True if the motor should be inverted when operated.
-   */
-  public void setInvertedMotor(MotorType motor, boolean isInverted) {
-    switch (motor) {
-      case kFrontLeft:
-        m_frontLeftMotor.setInverted(isInverted);
-        break;
-      case kFrontRight:
-        m_frontRightMotor.setInverted(isInverted);
-        break;
-      case kRearLeft:
-        m_rearLeftMotor.setInverted(isInverted);
-        break;
-      case kRearRight:
-        m_rearRightMotor.setInverted(isInverted);
-        break;
-      default:
-        throw new IllegalArgumentException("Illegal motor type: " + motor);
-    }
-  }
-
-  /**
-   * Set the turning sensitivity.
-   *
-   * <p>This only impacts the drive() entry-point.
-   *
-   * @param sensitivity Effectively sets the turning sensitivity (or turn radius for a given value)
-   */
-  public void setSensitivity(double sensitivity) {
-    m_sensitivity = sensitivity;
-  }
-
-  /**
-   * Configure the scaling factor for using RobotDrive with motor controllers in a mode other than
-   * PercentVbus.
-   *
-   * @param maxOutput Multiplied with the output percentage computed by the drive functions.
-   */
-  public void setMaxOutput(double maxOutput) {
-    m_maxOutput = maxOutput;
-  }
-
-  /**
-   * Free the speed controllers if they were allocated locally.
-   */
-  @Override
-  public void close() {
-    if (m_allocatedSpeedControllers) {
-      if (m_frontLeftMotor != null) {
-        ((PWM) m_frontLeftMotor).close();
-      }
-      if (m_frontRightMotor != null) {
-        ((PWM) m_frontRightMotor).close();
-      }
-      if (m_rearLeftMotor != null) {
-        ((PWM) m_rearLeftMotor).close();
-      }
-      if (m_rearRightMotor != null) {
-        ((PWM) m_rearRightMotor).close();
-      }
-    }
-  }
-
-  @Override
-  public String getDescription() {
-    return "Robot Drive";
-  }
-
-  @Override
-  public void stopMotor() {
-    if (m_frontLeftMotor != null) {
-      m_frontLeftMotor.stopMotor();
-    }
-    if (m_frontRightMotor != null) {
-      m_frontRightMotor.stopMotor();
-    }
-    if (m_rearLeftMotor != null) {
-      m_rearLeftMotor.stopMotor();
-    }
-    if (m_rearRightMotor != null) {
-      m_rearRightMotor.stopMotor();
-    }
-
-    feed();
-  }
-
-  protected int getNumMotors() {
-    int motors = 0;
-    if (m_frontLeftMotor != null) {
-      motors++;
-    }
-    if (m_frontRightMotor != null) {
-      motors++;
-    }
-    if (m_rearLeftMotor != null) {
-      motors++;
-    }
-    if (m_rearRightMotor != null) {
-      motors++;
-    }
-    return motors;
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
index 58004d5..462051a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RobotState.java
@@ -1,38 +1,39 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-@SuppressWarnings("JavadocMethod")
+@SuppressWarnings("MissingJavadocMethod")
 public final class RobotState {
   public static boolean isDisabled() {
-    return DriverStation.getInstance().isDisabled();
+    return DriverStation.isDisabled();
   }
 
   public static boolean isEnabled() {
-    return DriverStation.getInstance().isEnabled();
+    return DriverStation.isEnabled();
   }
 
   public static boolean isEStopped() {
-    return DriverStation.getInstance().isEStopped();
+    return DriverStation.isEStopped();
   }
 
+  @Deprecated
   public static boolean isOperatorControl() {
-    return DriverStation.getInstance().isOperatorControl();
+    return isTeleop();
+  }
+
+  public static boolean isTeleop() {
+    return DriverStation.isTeleop();
   }
 
   public static boolean isAutonomous() {
-    return DriverStation.getInstance().isAutonomous();
+    return DriverStation.isAutonomous();
   }
 
   public static boolean isTest() {
-    return DriverStation.getInstance().isTest();
+    return DriverStation.isTest();
   }
 
-  private RobotState() {
-  }
+  private RobotState() {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java
new file mode 100644
index 0000000..e7af118
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/RuntimeType.java
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import edu.wpi.first.hal.HALUtil;
+
+public enum RuntimeType {
+  kRoboRIO(HALUtil.RUNTIME_ROBORIO),
+  kRoboRIO2(HALUtil.RUNTIME_ROBORIO2),
+  kSimulation(HALUtil.RUNTIME_SIMULATION);
+
+  public final int value;
+
+  RuntimeType(int value) {
+    this.value = value;
+  }
+
+  /**
+   * Construct a runtime type from an int value.
+   *
+   * @param type Runtime type as int
+   * @return Matching runtime type
+   */
+  public static RuntimeType getValue(int type) {
+    if (type == HALUtil.RUNTIME_ROBORIO) {
+      return RuntimeType.kRoboRIO;
+    } else if (type == HALUtil.RUNTIME_ROBORIO2) {
+      return RuntimeType.kRoboRIO2;
+    }
+    return RuntimeType.kSimulation;
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
deleted file mode 100644
index b2e98b6..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SD540.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Mindsensors SD540 Speed Controller.
- *
- * <p>Note that the SD540 uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the SD540 User Manual
- * available from Mindsensors.
- *
- * <p><ul>
- * <li>2.05ms = full "forward"
- * <li>1.55ms = the "high end" of the deadband range
- * <li>1.50ms = center of the deadband range (off)
- * <li>1.44ms = the "low end" of the deadband range
- * <li>0.94ms = full "reverse"
- * </ul>
- */
-public class SD540 extends PWMSpeedController {
-  /**
-   * Common initialization code called by all constructors.
-   */
-  protected void initSD540() {
-    setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
-    SendableRegistry.setName(this, "SD540", getChannel());
-  }
-
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on
-   *                the MXP port
-   */
-  public SD540(final int channel) {
-    super(channel);
-    initSD540();
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
index aa3c679..2243140 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
@@ -1,30 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.ByteBuffer;
-import java.nio.ByteOrder;
-import java.nio.IntBuffer;
-
 import edu.wpi.first.hal.AccumulatorResult;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SPIJNI;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.nio.IntBuffer;
 
-/**
- * Represents a SPI bus port.
- */
-@SuppressWarnings("PMD.CyclomaticComplexity")
+/** Represents a SPI bus port. */
 public class SPI implements AutoCloseable {
   public enum Port {
-    kOnboardCS0(0), kOnboardCS1(1), kOnboardCS2(2), kOnboardCS3(3), kMXP(4);
+    kOnboardCS0(0),
+    kOnboardCS1(1),
+    kOnboardCS2(2),
+    kOnboardCS3(3),
+    kMXP(4);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     Port(int value) {
@@ -50,6 +46,10 @@
     HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
   }
 
+  public int getPort() {
+    return m_port;
+  }
+
   @Override
   public void close() {
     if (m_accum != null) {
@@ -126,6 +126,7 @@
   /**
    * Configure that the data is stable on the falling edge and the data changes on the rising edge.
    * Note this gets reversed is setClockActiveLow is set.
+   *
    * @deprecated use {@link #setSampleDataOnTrailingEdge()} in most cases.
    */
   @Deprecated
@@ -137,6 +138,7 @@
   /**
    * Configure that the data is stable on the rising edge and the data changes on the falling edge.
    * Note this gets reversed is setClockActiveLow is set.
+   *
    * @deprecated use {@link #setSampleDataOnLeadingEdge()} in most cases.
    */
   @Deprecated
@@ -145,18 +147,12 @@
     SPIJNI.spiSetOpts(m_port, m_msbFirst, m_sampleOnTrailing, m_clockIdleHigh);
   }
 
-
-
-  /**
-   * Configure the chip select line to be active high.
-   */
+  /** Configure the chip select line to be active high. */
   public final void setChipSelectActiveHigh() {
     SPIJNI.spiSetChipSelectActiveHigh(m_port);
   }
 
-  /**
-   * Configure the chip select line to be active low.
-   */
+  /** Configure the chip select line to be active low. */
   public final void setChipSelectActiveLow() {
     SPIJNI.spiSetChipSelectActiveLow(m_port);
   }
@@ -166,6 +162,10 @@
    *
    * <p>If not running in output only mode, also saves the data received on the CIPO input during
    * the transfer into the receive FIFO.
+   *
+   * @param dataToSend The buffer containing the data to send.
+   * @param size The number of bytes to send.
+   * @return Number of bytes written or -1 on error.
    */
   public int write(byte[] dataToSend, int size) {
     if (dataToSend.length < size) {
@@ -181,6 +181,8 @@
    * the transfer into the receive FIFO.
    *
    * @param dataToSend The buffer containing the data to send.
+   * @param size The number of bytes to send.
+   * @return Number of bytes written or -1 on error.
    */
   @SuppressWarnings("ByteBufferBackingArray")
   public int write(ByteBuffer dataToSend, int size) {
@@ -204,8 +206,11 @@
    * <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
    *
    * @param initiate If true, this function pushes "0" into the transmit buffer and initiates a
-   *                 transfer. If false, this function assumes that data is already in the receive
-   *                 FIFO from a previous write.
+   *     transfer. If false, this function assumes that data is already in the receive FIFO from a
+   *     previous write.
+   * @param dataReceived Buffer in which to store bytes read.
+   * @param size Number of bytes to read.
+   * @return Number of bytes read or -1 on error.
    */
   public int read(boolean initiate, byte[] dataReceived, int size) {
     if (dataReceived.length < size) {
@@ -221,11 +226,12 @@
    *
    * <p>If the receive FIFO is empty, there is no active transfer, and initiate is false, errors.
    *
-   * @param initiate     If true, this function pushes "0" into the transmit buffer and initiates
-   *                     a transfer. If false, this function assumes that data is already in the
-   *                     receive FIFO from a previous write.
+   * @param initiate If true, this function pushes "0" into the transmit buffer and initiates a
+   *     transfer. If false, this function assumes that data is already in the receive FIFO from a
+   *     previous write.
    * @param dataReceived The buffer to be filled with the received data.
-   * @param size         The length of the transaction, in bytes
+   * @param size The length of the transaction, in bytes
+   * @return Number of bytes read or -1 on error.
    */
   @SuppressWarnings("ByteBufferBackingArray")
   public int read(boolean initiate, ByteBuffer dataReceived, int size) {
@@ -244,9 +250,10 @@
   /**
    * Perform a simultaneous read/write transaction with the device.
    *
-   * @param dataToSend   The data to be written out to the device
+   * @param dataToSend The data to be written out to the device
    * @param dataReceived Buffer to receive data from the device
-   * @param size         The length of the transaction, in bytes
+   * @param size The length of the transaction, in bytes
+   * @return TODO
    */
   public int transaction(byte[] dataToSend, byte[] dataReceived, int size) {
     if (dataToSend.length < size) {
@@ -261,11 +268,12 @@
   /**
    * Perform a simultaneous read/write transaction with the device.
    *
-   * @param dataToSend   The data to be written out to the device.
+   * @param dataToSend The data to be written out to the device.
    * @param dataReceived Buffer to receive data from the device.
-   * @param size         The length of the transaction, in bytes
+   * @param size The length of the transaction, in bytes
+   * @return TODO
    */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "ByteBufferBackingArray"})
+  @SuppressWarnings("ByteBufferBackingArray")
   public int transaction(ByteBuffer dataToSend, ByteBuffer dataReceived, int size) {
     if (dataToSend.hasArray() && dataReceived.hasArray()) {
       return transaction(dataToSend.array(), dataReceived.array(), size);
@@ -288,8 +296,8 @@
   /**
    * Initialize automatic SPI transfer engine.
    *
-   * <p>Only a single engine is available, and use of it blocks use of all other
-   * chip select usage on the same physical SPI port while it is running.
+   * <p>Only a single engine is available, and use of it blocks use of all other chip select usage
+   * on the same physical SPI port while it is running.
    *
    * @param bufferSize buffer size in bytes
    */
@@ -297,9 +305,7 @@
     SPIJNI.spiInitAuto(m_port, bufferSize);
   }
 
-  /**
-   * Frees the automatic SPI transfer engine.
-   */
+  /** Frees the automatic SPI transfer engine. */
   public void freeAuto() {
     SPIJNI.spiFreeAuto(m_port);
   }
@@ -307,8 +313,7 @@
   /**
    * Set the data to be transmitted by the engine.
    *
-   * <p>Up to 16 bytes are configurable, and may be followed by up to 127 zero
-   * bytes.
+   * <p>Up to 16 bytes are configurable, and may be followed by up to 127 zero bytes.
    *
    * @param dataToSend data to send (maximum 16 bytes)
    * @param zeroSize number of zeros to send after the data
@@ -320,8 +325,8 @@
   /**
    * Start running the automatic SPI transfer engine at a periodic rate.
    *
-   * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must
-   * be called before calling this function.
+   * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must be called before
+   * calling this function.
    *
    * @param period period between transfers, in seconds (us resolution)
    */
@@ -332,28 +337,28 @@
   /**
    * Start running the automatic SPI transfer engine when a trigger occurs.
    *
-   * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must
-   * be called before calling this function.
+   * <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must be called before
+   * calling this function.
    *
    * @param source digital source for the trigger (may be an analog trigger)
    * @param rising trigger on the rising edge
    * @param falling trigger on the falling edge
    */
   public void startAutoTrigger(DigitalSource source, boolean rising, boolean falling) {
-    SPIJNI.spiStartAutoTrigger(m_port, source.getPortHandleForRouting(),
-                               source.getAnalogTriggerTypeForRouting(), rising, falling);
+    SPIJNI.spiStartAutoTrigger(
+        m_port,
+        source.getPortHandleForRouting(),
+        source.getAnalogTriggerTypeForRouting(),
+        rising,
+        falling);
   }
 
-  /**
-   * Stop running the automatic SPI transfer engine.
-   */
+  /** Stop running the automatic SPI transfer engine. */
   public void stopAuto() {
     SPIJNI.spiStopAuto(m_port);
   }
 
-  /**
-   * Force the engine to make a single transfer.
-   */
+  /** Force the engine to make a single transfer. */
   public void forceAutoRead() {
     SPIJNI.spiForceAutoRead(m_port);
   }
@@ -361,16 +366,15 @@
   /**
    * Read data that has been transferred by the automatic SPI transfer engine.
    *
-   * <p>Transfers may be made a byte at a time, so it's necessary for the caller
-   * to handle cases where an entire transfer has not been completed.
+   * <p>Transfers may be made a byte at a time, so it's necessary for the caller to handle cases
+   * where an entire transfer has not been completed.
    *
-   * <p>Each received data sequence consists of a timestamp followed by the
-   * received data bytes, one byte per word (in the least significant byte).
-   * The length of each received data sequence is the same as the combined
-   * size of the data and zeroSize set in setAutoTransmitData().
+   * <p>Each received data sequence consists of a timestamp followed by the received data bytes, one
+   * byte per word (in the least significant byte). The length of each received data sequence is the
+   * same as the combined size of the data and zeroSize set in setAutoTransmitData().
    *
-   * <p>Blocks until numToRead words have been read or timeout expires.
-   * May be called with numToRead=0 to retrieve how many words are available.
+   * <p>Blocks until numToRead words have been read or timeout expires. May be called with
+   * numToRead=0 to retrieve how many words are available.
    *
    * @param buffer buffer where read words are stored
    * @param numToRead number of words to read
@@ -382,8 +386,8 @@
       throw new IllegalArgumentException("must be a direct buffer");
     }
     if (buffer.capacity() < numToRead * 4) {
-      throw new IllegalArgumentException("buffer is too small, must be at least "
-          + (numToRead * 4));
+      throw new IllegalArgumentException(
+          "buffer is too small, must be at least " + (numToRead * 4));
     }
     return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
   }
@@ -391,16 +395,15 @@
   /**
    * Read data that has been transferred by the automatic SPI transfer engine.
    *
-   * <p>Transfers may be made a byte at a time, so it's necessary for the caller
-   * to handle cases where an entire transfer has not been completed.
+   * <p>Transfers may be made a byte at a time, so it's necessary for the caller to handle cases
+   * where an entire transfer has not been completed.
    *
-   * <p>Each received data sequence consists of a timestamp followed by the
-   * received data bytes, one byte per word (in the least significant byte).
-   * The length of each received data sequence is the same as the combined
-   * size of the data and zeroSize set in setAutoTransmitData().
+   * <p>Each received data sequence consists of a timestamp followed by the received data bytes, one
+   * byte per word (in the least significant byte). The length of each received data sequence is the
+   * same as the combined size of the data and zeroSize set in setAutoTransmitData().
    *
-   * <p>Blocks until numToRead words have been read or timeout expires.
-   * May be called with numToRead=0 to retrieve how many words are available.
+   * <p>Blocks until numToRead words have been read or timeout expires. May be called with
+   * numToRead=0 to retrieve how many words are available.
    *
    * @param buffer array where read words are stored
    * @param numToRead number of words to read
@@ -415,8 +418,8 @@
   }
 
   /**
-   * Get the number of bytes dropped by the automatic SPI transfer engine due
-   * to the receive buffer being full.
+   * Get the number of bytes dropped by the automatic SPI transfer engine due to the receive buffer
+   * being full.
    *
    * @return Number of bytes dropped
    */
@@ -437,15 +440,22 @@
 
   private static final int kAccumulateDepth = 2048;
 
-  @SuppressWarnings("PMD.TooManyFields")
   private static class Accumulator implements AutoCloseable {
-    Accumulator(int port, int xferSize, int validMask, int validValue, int dataShift,
-                int dataSize, boolean isSigned, boolean bigEndian) {
+    Accumulator(
+        int port,
+        int xferSize,
+        int validMask,
+        int validValue,
+        int dataShift,
+        int dataSize,
+        boolean isSigned,
+        boolean bigEndian) {
       m_notifier = new Notifier(this::update);
-      m_buf = ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4)
-          .order(ByteOrder.nativeOrder());
+      m_buf =
+          ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4)
+              .order(ByteOrder.nativeOrder());
       m_intBuf = m_buf.asIntBuffer();
-      m_xferSize = xferSize + 1;  // +1 for timestamp
+      m_xferSize = xferSize + 1; // +1 for timestamp
       m_validMask = validMask;
       m_validValue = validValue;
       m_dataShift = dataShift;
@@ -478,15 +488,14 @@
 
     final int m_validMask;
     final int m_validValue;
-    final int m_dataMax;        // one more than max data value
-    final int m_dataMsbMask;    // data field MSB mask (for signed)
-    final int m_dataShift;      // data field shift right amount, in bits
-    final int m_xferSize;       // SPI transfer size, in bytes
-    final boolean m_isSigned;   // is data field signed?
-    final boolean m_bigEndian;  // is response big endian?
+    final int m_dataMax; // one more than max data value
+    final int m_dataMsbMask; // data field MSB mask (for signed)
+    final int m_dataShift; // data field shift right amount, in bits
+    final int m_xferSize; // SPI transfer size, in bytes
+    final boolean m_isSigned; // is data field signed?
+    final boolean m_bigEndian; // is response big endian?
     final int m_port;
 
-    @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
     void update() {
       synchronized (m_mutex) {
         boolean done = false;
@@ -503,7 +512,7 @@
             done = false;
           }
           if (numToRead == 0) {
-            return;  // no samples
+            return; // no samples
           }
 
           // read buffered data
@@ -546,11 +555,12 @@
                 if (m_count != 0) {
                   // timestamps use the 1us FPGA clock; also handle rollover
                   if (timestamp >= m_lastTimestamp) {
-                    m_integratedValue += dataNoCenter * (timestamp - m_lastTimestamp)
-                        * 1e-6 - m_integratedCenter;
+                    m_integratedValue +=
+                        dataNoCenter * (timestamp - m_lastTimestamp) * 1e-6 - m_integratedCenter;
                   } else {
-                    m_integratedValue += dataNoCenter * ((1L << 32) - m_lastTimestamp + timestamp)
-                        * 1e-6 - m_integratedCenter;
+                    m_integratedValue +=
+                        dataNoCenter * ((1L << 32) - m_lastTimestamp + timestamp) * 1e-6
+                            - m_integratedCenter;
                   }
                 }
               }
@@ -572,20 +582,26 @@
   /**
    * Initialize the accumulator.
    *
-   * @param period     Time between reads
-   * @param cmd        SPI command to send to request data
-   * @param xferSize   SPI transfer size, in bytes
-   * @param validMask  Mask to apply to received data for validity checking
+   * @param period Time between reads
+   * @param cmd SPI command to send to request data
+   * @param xferSize SPI transfer size, in bytes
+   * @param validMask Mask to apply to received data for validity checking
    * @param validValue After validMask is applied, required matching value for validity checking
-   * @param dataShift  Bit shift to apply to received data to get actual data value
-   * @param dataSize   Size (in bits) of data field
-   * @param isSigned   Is data field signed?
-   * @param bigEndian  Is device big endian?
+   * @param dataShift Bit shift to apply to received data to get actual data value
+   * @param dataSize Size (in bits) of data field
+   * @param isSigned Is data field signed?
+   * @param bigEndian Is device big endian?
    */
-  public void initAccumulator(double period, int cmd, int xferSize,
-                              int validMask, int validValue,
-                              int dataShift, int dataSize,
-                              boolean isSigned, boolean bigEndian) {
+  public void initAccumulator(
+      double period,
+      int cmd,
+      int xferSize,
+      int validMask,
+      int validValue,
+      int dataShift,
+      int dataSize,
+      boolean isSigned,
+      boolean bigEndian) {
     initAuto(xferSize * 2048);
     byte[] cmdBytes = new byte[] {0, 0, 0, 0};
     if (bigEndian) {
@@ -605,14 +621,13 @@
     setAutoTransmitData(cmdBytes, xferSize - 4);
     startAutoRate(period);
 
-    m_accum = new Accumulator(m_port, xferSize, validMask, validValue, dataShift, dataSize,
-                              isSigned, bigEndian);
+    m_accum =
+        new Accumulator(
+            m_port, xferSize, validMask, validValue, dataShift, dataSize, isSigned, bigEndian);
     m_accum.m_notifier.startPeriodic(period * 1024);
   }
 
-  /**
-   * Frees the accumulator.
-   */
+  /** Frees the accumulator. */
   public void freeAccumulator() {
     if (m_accum != null) {
       m_accum.close();
@@ -621,9 +636,7 @@
     freeAuto();
   }
 
-  /**
-   * Resets the accumulator to zero.
-   */
+  /** Resets the accumulator to zero. */
   public void resetAccumulator() {
     if (m_accum == null) {
       return;
@@ -643,6 +656,8 @@
    * <p>The center value is subtracted from each value before it is added to the accumulator. This
    * is used for the center value of devices like gyros and accelerometers to make integration work
    * and to take the device offset into account when integrating.
+   *
+   * @param center The accumulator's center value.
    */
   public void setAccumulatorCenter(int center) {
     if (m_accum == null) {
@@ -655,6 +670,8 @@
 
   /**
    * Set the accumulator's deadband.
+   *
+   * @param deadband The accumulator's deadband.
    */
   public void setAccumulatorDeadband(int deadband) {
     if (m_accum == null) {
@@ -667,6 +684,8 @@
 
   /**
    * Read the last value read by the accumulator engine.
+   *
+   * @return The last value read by the accumulator engine.
    */
   public int getAccumulatorLastValue() {
     if (m_accum == null) {
@@ -754,9 +773,11 @@
   /**
    * Set the center value of the accumulator integrator.
    *
-   * <p>The center value is subtracted from each value*dt before it is added to the
-   * integrated value. This is used for the center value of devices like gyros
-   * and accelerometers to take the device offset into account when integrating.
+   * <p>The center value is subtracted from each value*dt before it is added to the integrated
+   * value. This is used for the center value of devices like gyros and accelerometers to take the
+   * device offset into account when integrating.
+   *
+   * @param center The accumulator integrator's center value.
    */
   public void setAccumulatorIntegratedCenter(double center) {
     if (m_accum == null) {
@@ -768,8 +789,7 @@
   }
 
   /**
-   * Read the integrated value.  This is the sum of (each value * time between
-   * values).
+   * Read the integrated value. This is the sum of (each value * time between values).
    *
    * @return The integrated value accumulated since the last Reset().
    */
@@ -784,11 +804,10 @@
   }
 
   /**
-   * Read the average of the integrated value.  This is the sum of (each value
-   * times the time between values), divided by the count.
+   * Read the average of the integrated value. This is the sum of (each value times the time between
+   * values), divided by the count.
    *
-   * @return The average of the integrated value accumulated since the last
-   *         Reset().
+   * @return The average of the integrated value accumulated since the last Reset().
    */
   public double getAccumulatorIntegratedAverage() {
     if (m_accum == null) {
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
deleted file mode 100644
index 138f7f4..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Sendable.java
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-
-/**
- * The base interface for objects that can be sent over the network through network tables.
- */
-public interface Sendable {
-  /**
-   * Gets the name of this {@link Sendable} object.
-   *
-   * @return Name
-   * @deprecated Use SendableRegistry.getName()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default String getName() {
-    return SendableRegistry.getName(this);
-  }
-
-  /**
-   * Sets the name of this {@link Sendable} object.
-   *
-   * @param name name
-   * @deprecated Use SendableRegistry.setName()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default void setName(String name) {
-    SendableRegistry.setName(this, name);
-  }
-
-  /**
-   * Sets both the subsystem name and device name of this {@link Sendable} object.
-   *
-   * @param subsystem subsystem name
-   * @param name device name
-   * @deprecated Use SendableRegistry.setName()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default void setName(String subsystem, String name) {
-    SendableRegistry.setName(this, subsystem, name);
-  }
-
-  /**
-   * Sets the name of the sensor with a channel number.
-   *
-   * @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
-   * @deprecated Use SendableRegistry.setName()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default void setName(String moduleType, int channel) {
-    SendableRegistry.setName(this, moduleType, channel);
-  }
-
-  /**
-   * Sets the name of the sensor with a module and channel number.
-   *
-   * @param moduleType   A string that defines the module name in the label for the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into (usually PWM)
-   * @deprecated Use SendableRegistry.setName()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default void setName(String moduleType, int moduleNumber, int channel) {
-    SendableRegistry.setName(this, moduleType, moduleNumber, channel);
-  }
-
-  /**
-   * Gets the subsystem name of this {@link Sendable} object.
-   *
-   * @return Subsystem name
-   * @deprecated Use SendableRegistry.getSubsystem()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default String getSubsystem() {
-    return SendableRegistry.getSubsystem(this);
-  }
-
-  /**
-   * Sets the subsystem name of this {@link Sendable} object.
-   *
-   * @param subsystem subsystem name
-   * @deprecated Use SendableRegistry.setSubsystem()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default void setSubsystem(String subsystem) {
-    SendableRegistry.setSubsystem(this, subsystem);
-  }
-
-  /**
-   * Add a child component.
-   *
-   * @param child child component
-   * @deprecated Use SendableRegistry.addChild()
-   */
-  @Deprecated(since = "2020", forRemoval = true)
-  default void addChild(Object child) {
-    SendableRegistry.addChild(this, child);
-  }
-
-  /**
-   * Initializes this {@link Sendable} object.
-   *
-   * @param builder sendable builder
-   */
-  void initSendable(SendableBuilder builder);
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
deleted file mode 100644
index 4e2116c..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SendableBase.java
+++ /dev/null
@@ -1,43 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Base class for all sensors. Stores most recent status information as well as containing utility
- * functions for checking channels and error processing.
- * @deprecated Use Sendable and SendableRegistry
- */
-@Deprecated(since = "2020", forRemoval = true)
-public abstract class SendableBase implements Sendable, AutoCloseable {
-  /**
-   * Creates an instance of the sensor base.
-   */
-  public SendableBase() {
-    this(true);
-  }
-
-  /**
-   * Creates an instance of the sensor base.
-   *
-   * @param addLiveWindow if true, add this Sendable to LiveWindow
-   */
-  public SendableBase(boolean addLiveWindow) {
-    if (addLiveWindow) {
-      SendableRegistry.addLW(this, "");
-    } else {
-      SendableRegistry.add(this, "");
-    }
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
index 22aa7bd..0ac24de 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SensorUtil.java
@@ -1,92 +1,55 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.AnalogJNI;
 import edu.wpi.first.hal.ConstantsJNI;
 import edu.wpi.first.hal.DIOJNI;
-import edu.wpi.first.hal.PDPJNI;
 import edu.wpi.first.hal.PWMJNI;
 import edu.wpi.first.hal.PortsJNI;
 import edu.wpi.first.hal.RelayJNI;
-import edu.wpi.first.hal.SolenoidJNI;
 
 /**
  * Stores most recent status information as well as containing utility functions for checking
  * channels and error processing.
  */
 public final class SensorUtil {
-  /**
-   * Ticks per microsecond.
-   */
-  public static final int kSystemClockTicksPerMicrosecond
-      = ConstantsJNI.getSystemClockTicksPerMicrosecond();
+  /** Ticks per microsecond. */
+  public static final int kSystemClockTicksPerMicrosecond =
+      ConstantsJNI.getSystemClockTicksPerMicrosecond();
 
-  /**
-   * Number of digital channels per roboRIO.
-   */
+  /** Number of digital channels per roboRIO. */
   public static final int kDigitalChannels = PortsJNI.getNumDigitalChannels();
 
-  /**
-   * Number of analog input channels per roboRIO.
-   */
+  /** Number of analog input channels per roboRIO. */
   public static final int kAnalogInputChannels = PortsJNI.getNumAnalogInputs();
 
-  /**
-   * Number of analog output channels per roboRIO.
-   */
+  /** Number of analog output channels per roboRIO. */
   public static final int kAnalogOutputChannels = PortsJNI.getNumAnalogOutputs();
 
-  /**
-   * Number of solenoid channels per module.
-   */
-  public static final int kSolenoidChannels = PortsJNI.getNumSolenoidChannels();
+  /** Number of solenoid channels per module. */
+  public static final int kCTRESolenoidChannels = PortsJNI.getNumCTRESolenoidChannels();
 
-  /**
-   * Number of PWM channels per roboRIO.
-   */
+  /** Number of PWM channels per roboRIO. */
   public static final int kPwmChannels = PortsJNI.getNumPWMChannels();
 
-  /**
-   * Number of relay channels per roboRIO.
-   */
+  /** Number of relay channels per roboRIO. */
   public static final int kRelayChannels = PortsJNI.getNumRelayHeaders();
 
-  /**
-   * Number of power distribution channels per PDP.
-   */
-  public static final int kPDPChannels = PortsJNI.getNumPDPChannels();
+  /** Number of power distribution channels per PDP. */
+  public static final int kCTREPDPChannels = PortsJNI.getNumCTREPDPChannels();
 
-  /**
-   * Number of power distribution modules per PDP.
-   */
-  public static final int kPDPModules = PortsJNI.getNumPDPModules();
+  /** Number of power distribution modules per PDP. */
+  public static final int kCTREPDPModules = PortsJNI.getNumCTREPDPModules();
 
-  /**
-   * Number of PCM Modules.
-   */
-  public static final int kPCMModules = PortsJNI.getNumPCMModules();
+  /** Number of PCM Modules. */
+  public static final int kCTREPCMModules = PortsJNI.getNumCTREPCMModules();
 
-  /**
-   * Verify that the solenoid module is correct.
-   *
-   * @param moduleNumber The solenoid module module number to check.
-   */
-  public static void checkSolenoidModule(final int moduleNumber) {
-    if (!SolenoidJNI.checkSolenoidModule(moduleNumber)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested solenoid module is out of range. Minimum: 0, Maximum: ")
-        .append(kPCMModules)
-        .append(", Requested: ")
-        .append(moduleNumber);
-      throw new IllegalArgumentException(buf.toString());
-    }
-  }
+  public static final int kREVPHChannels = PortsJNI.getNumREVPHChannels();
+
+  public static final int kREVPHModules = PortsJNI.getNumREVPHModules();
 
   /**
    * Check that the digital channel number is valid. Verify that the channel number is one of the
@@ -98,9 +61,9 @@
     if (!DIOJNI.checkDIOChannel(channel)) {
       StringBuilder buf = new StringBuilder();
       buf.append("Requested DIO channel is out of range. Minimum: 0, Maximum: ")
-        .append(kDigitalChannels)
-        .append(", Requested: ")
-        .append(channel);
+          .append(kDigitalChannels)
+          .append(", Requested: ")
+          .append(channel);
       throw new IllegalArgumentException(buf.toString());
     }
   }
@@ -115,9 +78,9 @@
     if (!RelayJNI.checkRelayChannel(channel)) {
       StringBuilder buf = new StringBuilder();
       buf.append("Requested relay channel is out of range. Minimum: 0, Maximum: ")
-        .append(kRelayChannels)
-        .append(", Requested: ")
-        .append(channel);
+          .append(kRelayChannels)
+          .append(", Requested: ")
+          .append(channel);
       throw new IllegalArgumentException(buf.toString());
     }
   }
@@ -132,9 +95,9 @@
     if (!PWMJNI.checkPWMChannel(channel)) {
       StringBuilder buf = new StringBuilder();
       buf.append("Requested PWM channel is out of range. Minimum: 0, Maximum: ")
-        .append(kPwmChannels)
-        .append(", Requested: ")
-        .append(channel);
+          .append(kPwmChannels)
+          .append(", Requested: ")
+          .append(channel);
       throw new IllegalArgumentException(buf.toString());
     }
   }
@@ -149,9 +112,9 @@
     if (!AnalogJNI.checkAnalogInputChannel(channel)) {
       StringBuilder buf = new StringBuilder();
       buf.append("Requested analog input channel is out of range. Minimum: 0, Maximum: ")
-        .append(kAnalogInputChannels)
-        .append(", Requested: ")
-        .append(channel);
+          .append(kAnalogInputChannels)
+          .append(", Requested: ")
+          .append(channel);
       throw new IllegalArgumentException(buf.toString());
     }
   }
@@ -166,58 +129,9 @@
     if (!AnalogJNI.checkAnalogOutputChannel(channel)) {
       StringBuilder buf = new StringBuilder();
       buf.append("Requested analog output channel is out of range. Minimum: 0, Maximum: ")
-        .append(kAnalogOutputChannels)
-        .append(", Requested: ")
-        .append(channel);
-      throw new IllegalArgumentException(buf.toString());
-    }
-  }
-
-  /**
-   * Verify that the solenoid channel number is within limits. Channel numbers are 0-based.
-   *
-   * @param channel The channel number to check.
-   */
-  public static void checkSolenoidChannel(final int channel) {
-    if (!SolenoidJNI.checkSolenoidChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested solenoid channel is out of range. Minimum: 0, Maximum: ")
-        .append(kSolenoidChannels)
-        .append(", Requested: ")
-        .append(channel);
-      throw new IllegalArgumentException(buf.toString());
-    }
-  }
-
-  /**
-   * Verify that the power distribution channel number is within limits. Channel numbers are
-   * 0-based.
-   *
-   * @param channel The channel number to check.
-   */
-  public static void checkPDPChannel(final int channel) {
-    if (!PDPJNI.checkPDPChannel(channel)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested PDP channel is out of range. Minimum: 0, Maximum: ")
-        .append(kPDPChannels)
-        .append(", Requested: ")
-        .append(channel);
-      throw new IllegalArgumentException(buf.toString());
-    }
-  }
-
-  /**
-   * Verify that the PDP module number is within limits. module numbers are 0-based.
-   *
-   * @param module The module number to check.
-   */
-  public static void checkPDPModule(final int module) {
-    if (!PDPJNI.checkPDPModule(module)) {
-      StringBuilder buf = new StringBuilder();
-      buf.append("Requested PDP module is out of range. Minimum: 0, Maximum: ")
-        .append(kPDPModules)
-        .append(", Requested: ")
-        .append(module);
+          .append(kAnalogOutputChannels)
+          .append(", Requested: ")
+          .append(channel);
       throw new IllegalArgumentException(buf.toString());
     }
   }
@@ -227,10 +141,20 @@
    *
    * @return The number of the default solenoid module.
    */
-  public static int getDefaultSolenoidModule() {
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public static int getDefaultCTREPCMModule() {
     return 0;
   }
 
-  private SensorUtil() {
+  /**
+   * Get the number of the default solenoid module.
+   *
+   * @return The number of the default solenoid module.
+   */
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public static int getDefaultREVPHModule() {
+    return 1;
   }
+
+  private SensorUtil() {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
index 0f509c7..331bbf1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SerialPort.java
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.nio.charset.StandardCharsets;
-
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SerialPortJNI;
+import java.nio.charset.StandardCharsets;
 
-/**
- * Driver for the serial ports (USB, MXP, Onboard) on the roboRIO.
- */
+/** Driver for the serial ports (USB, MXP, Onboard) on the roboRIO. */
 public class SerialPort implements AutoCloseable {
   private int m_portHandle;
 
   public enum Port {
-    kOnboard(0), kMXP(1), kUSB(2), kUSB1(2), kUSB2(3);
+    kOnboard(0),
+    kMXP(1),
+    kUSB(2),
+    kUSB1(2),
+    kUSB2(3);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     Port(int value) {
@@ -30,13 +27,14 @@
     }
   }
 
-  /**
-   * Represents the parity to use for serial communications.
-   */
+  /** Represents the parity to use for serial communications. */
   public enum Parity {
-    kNone(0), kOdd(1), kEven(2), kMark(3), kSpace(4);
+    kNone(0),
+    kOdd(1),
+    kEven(2),
+    kMark(3),
+    kSpace(4);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     Parity(int value) {
@@ -44,13 +42,12 @@
     }
   }
 
-  /**
-   * Represents the number of stop bits to use for Serial Communication.
-   */
+  /** Represents the number of stop bits to use for Serial Communication. */
   public enum StopBits {
-    kOne(10), kOnePointFive(15), kTwo(20);
+    kOne(10),
+    kOnePointFive(15),
+    kTwo(20);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     StopBits(int value) {
@@ -58,13 +55,13 @@
     }
   }
 
-  /**
-   * Represents what type of flow control to use for serial communication.
-   */
+  /** Represents what type of flow control to use for serial communication. */
   public enum FlowControl {
-    kNone(0), kXonXoff(1), kRtsCts(2), kDtsDsr(4);
+    kNone(0),
+    kXonXoff(1),
+    kRtsCts(2),
+    kDtsDsr(4);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     FlowControl(int value) {
@@ -72,13 +69,11 @@
     }
   }
 
-  /**
-   * Represents which type of buffer mode to use when writing to a serial port.
-   */
+  /** Represents which type of buffer mode to use when writing to a serial port. */
   public enum WriteBufferMode {
-    kFlushOnAccess(1), kFlushWhenFull(2);
+    kFlushOnAccess(1),
+    kFlushWhenFull(2);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     WriteBufferMode(int value) {
@@ -89,19 +84,25 @@
   /**
    * Create an instance of a Serial Port class.
    *
-   * <p>Prefer to use the constructor that doesn't take a port name, but in some
-   * cases the automatic detection might not work correctly.
+   * <p>Prefer to use the constructor that doesn't take a port name, but in some cases the automatic
+   * detection might not work correctly.
    *
    * @param baudRate The baud rate to configure the serial port.
-   * @param port     The Serial port to use
+   * @param port The Serial port to use
    * @param portName The direct portName to use
    * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
-   * @param parity   Select the type of parity checking to use.
+   * @param parity Select the type of parity checking to use.
    * @param stopBits The number of stop bits to use as defined by the enum StopBits.
-   * @deprecated     Will be removed for 2019
+   * @deprecated Will be removed for 2019
    */
-  public SerialPort(final int baudRate, String portName, Port port, final int dataBits,
-                    Parity parity, StopBits stopBits) {
+  @Deprecated
+  public SerialPort(
+      final int baudRate,
+      String portName,
+      Port port,
+      final int dataBits,
+      Parity parity,
+      StopBits stopBits) {
     m_portHandle = SerialPortJNI.serialInitializePortDirect((byte) port.value, portName);
     SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
     SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
@@ -126,13 +127,13 @@
    * Create an instance of a Serial Port class.
    *
    * @param baudRate The baud rate to configure the serial port.
-   * @param port     The Serial port to use
+   * @param port The Serial port to use
    * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
-   * @param parity   Select the type of parity checking to use.
+   * @param parity Select the type of parity checking to use.
    * @param stopBits The number of stop bits to use as defined by the enum StopBits.
    */
-  public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity,
-                    StopBits stopBits) {
+  public SerialPort(
+      final int baudRate, Port port, final int dataBits, Parity parity, StopBits stopBits) {
     m_portHandle = SerialPortJNI.serialInitializePort((byte) port.value);
     SerialPortJNI.serialSetBaudRate(m_portHandle, baudRate);
     SerialPortJNI.serialSetDataBits(m_portHandle, (byte) dataBits);
@@ -157,8 +158,9 @@
    * Create an instance of a Serial Port class. Defaults to one stop bit.
    *
    * @param baudRate The baud rate to configure the serial port.
+   * @param port The serial port to use.
    * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
-   * @param parity   Select the type of parity checking to use.
+   * @param parity Select the type of parity checking to use.
    */
   public SerialPort(final int baudRate, Port port, final int dataBits, Parity parity) {
     this(baudRate, port, dataBits, parity, StopBits.kOne);
@@ -168,6 +170,7 @@
    * Create an instance of a Serial Port class. Defaults to no parity and one stop bit.
    *
    * @param baudRate The baud rate to configure the serial port.
+   * @param port The serial port to use.
    * @param dataBits The number of data bits per transfer. Valid values are between 5 and 8 bits.
    */
   public SerialPort(final int baudRate, Port port, final int dataBits) {
@@ -175,10 +178,10 @@
   }
 
   /**
-   * Create an instance of a Serial Port class. Defaults to 8 databits, no parity, and one stop
-   * bit.
+   * Create an instance of a Serial Port class. Defaults to 8 databits, no parity, and one stop bit.
    *
    * @param baudRate The baud rate to configure the serial port.
+   * @param port The serial port to use.
    */
   public SerialPort(final int baudRate, Port port) {
     this(baudRate, port, 8, Parity.kNone, StopBits.kOne);
@@ -226,9 +229,7 @@
     enableTermination('\n');
   }
 
-  /**
-   * Disable termination behavior.
-   */
+  /** Disable termination behavior. */
   public void disableTermination() {
     SerialPortJNI.serialDisableTermination(m_portHandle);
   }
@@ -283,7 +284,7 @@
    * Write raw bytes to the serial port.
    *
    * @param buffer The buffer of bytes to write.
-   * @param count  The maximum number of bytes to write.
+   * @param count The maximum number of bytes to write.
    * @return The number of bytes actually written into the port.
    */
   public int write(byte[] buffer, int count) {
@@ -344,11 +345,11 @@
   /**
    * Specify the flushing behavior of the output buffer.
    *
-   * <p>When set to kFlushOnAccess, data is synchronously written to the serial port after each
-   * call to either print() or write().
+   * <p>When set to kFlushOnAccess, data is synchronously written to the serial port after each call
+   * to either print() or write().
    *
-   * <p>When set to kFlushWhenFull, data will only be written to the serial port when the buffer
-   * is full or when flush() is called.
+   * <p>When set to kFlushWhenFull, data will only be written to the serial port when the buffer is
+   * full or when flush() is called.
    *
    * @param mode The write buffer mode.
    */
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
index 6b2eab6..f11f8f9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Servo.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
  * Standard hobby style servo.
@@ -28,11 +25,11 @@
   /**
    * Constructor.<br>
    *
-   * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br> By default
-   * {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
+   * <p>By default {@value #kDefaultMaxServoPWM} ms is used as the maxPWM value<br>
+   * By default {@value #kDefaultMinServoPWM} ms is used as the minPWM value<br>
    *
    * @param channel The PWM channel to which the servo is attached. 0-9 are on-board, 10-19 are on
-   *                the MXP port
+   *     the MXP port
    */
   public Servo(final int channel) {
     super(channel);
@@ -43,7 +40,6 @@
     SendableRegistry.setName(this, "Servo", getChannel());
   }
 
-
   /**
    * Set the servo position.
    *
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
deleted file mode 100644
index fe4dda7..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SlewRateLimiter.java
+++ /dev/null
@@ -1,69 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.wpiutil.math.MathUtil;
-
-/**
- * A class that limits the rate of change of an input value.  Useful for implementing voltage,
- * setpoint, and/or output ramps.  A slew-rate limit is most appropriate when the quantity being
- * controlled is a velocity or a voltage; when controlling a position, consider using a
- * {@link edu.wpi.first.wpilibj.trajectory.TrapezoidProfile} instead.
- */
-public class SlewRateLimiter {
-  private final double m_rateLimit;
-  private double m_prevVal;
-  private double m_prevTime;
-
-  /**
-   * Creates a new SlewRateLimiter with the given rate limit and initial value.
-   *
-   * @param rateLimit The rate-of-change limit, in units per second.
-   * @param initialValue The initial value of the input.
-   */
-  public SlewRateLimiter(double rateLimit, double initialValue) {
-    m_rateLimit = rateLimit;
-    m_prevVal = initialValue;
-    m_prevTime = Timer.getFPGATimestamp();
-  }
-
-  /**
-   * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
-   *
-   * @param rateLimit The rate-of-change limit, in units per second.
-   */
-  public SlewRateLimiter(double rateLimit) {
-    this(rateLimit, 0);
-  }
-
-  /**
-   * Filters the input to limit its slew rate.
-   *
-   * @param input The input value whose slew rate is to be limited.
-   * @return The filtered value, which will not change faster than the slew rate.
-   */
-  public double calculate(double input) {
-    double currentTime = Timer.getFPGATimestamp();
-    double elapsedTime = currentTime - m_prevTime;
-    m_prevVal += MathUtil.clamp(input - m_prevVal,
-                                -m_rateLimit * elapsedTime,
-                                m_rateLimit * elapsedTime);
-    m_prevTime = currentTime;
-    return m_prevVal;
-  }
-
-  /**
-   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
-   *
-   * @param value The value to reset to.
-   */
-  public void reset(double value) {
-    m_prevVal = value;
-    m_prevTime = Timer.getFPGATimestamp();
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
index 6245c2b..e86042b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Solenoid.java
@@ -1,62 +1,83 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.hal.SolenoidJNI;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
 /**
- * Solenoid class for running high voltage Digital Output on the PCM.
+ * Solenoid class for running high voltage Digital Output on a pneumatics module.
  *
- * <p>The Solenoid class is typically used for pneumatic solenoids, but could be used for any
- * device within the current spec of the PCM.
+ * <p>The Solenoid class is typically used for pneumatic solenoids, but could be used for any device
+ * within the current spec of the module.
  */
-public class Solenoid extends SolenoidBase implements Sendable, AutoCloseable {
-  private final int m_channel; // The channel to control.
-  private int m_solenoidHandle;
+public class Solenoid implements Sendable, AutoCloseable {
+  private final int m_mask; // The channel mask
+  private final int m_channel;
+  private PneumaticsBase m_module;
 
   /**
-   * Constructor using the default PCM ID (defaults to 0).
+   * Constructs a solenoid for a default module and specified type.
    *
-   * @param channel The channel on the PCM to control (0..7).
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
    */
-  public Solenoid(final int channel) {
-    this(SensorUtil.getDefaultSolenoidModule(), channel);
+  public Solenoid(final PneumaticsModuleType moduleType, final int channel) {
+    this(PneumaticsBase.getDefaultForType(moduleType), moduleType, channel);
   }
 
   /**
-   * Constructor.
+   * Constructs a solenoid for a specified module and type.
    *
-   * @param moduleNumber The CAN ID of the PCM the solenoid is attached to.
-   * @param channel      The channel on the PCM to control (0..7).
+   * @param module The module ID to use.
+   * @param moduleType The module type to use.
+   * @param channel The channel the solenoid is on.
    */
-  public Solenoid(final int moduleNumber, final int channel) {
-    super(moduleNumber);
+  public Solenoid(final int module, final PneumaticsModuleType moduleType, final int channel) {
+    m_module = PneumaticsBase.getForType(module, moduleType);
+    boolean allocatedSolenoids = false;
+    boolean successfulCompletion = false;
+
+    m_mask = 1 << channel;
     m_channel = channel;
 
-    SensorUtil.checkSolenoidModule(m_moduleNumber);
-    SensorUtil.checkSolenoidChannel(m_channel);
+    try {
+      if (!m_module.checkSolenoidChannel(channel)) {
+        throw new IllegalArgumentException("Channel " + channel + " out of range");
+      }
 
-    int portHandle = HAL.getPortWithModule((byte) m_moduleNumber, (byte) m_channel);
-    m_solenoidHandle = SolenoidJNI.initializeSolenoidPort(portHandle);
+      if (m_module.checkAndReserveSolenoids(m_mask) != 0) {
+        throw new AllocationException("Solenoid already allocated");
+      }
 
-    HAL.report(tResourceType.kResourceType_Solenoid, m_channel + 1, m_moduleNumber + 1);
-    SendableRegistry.addLW(this, "Solenoid", m_moduleNumber, m_channel);
+      allocatedSolenoids = true;
+
+      HAL.report(tResourceType.kResourceType_Solenoid, channel + 1, m_module.getModuleNumber() + 1);
+      SendableRegistry.addLW(this, "Solenoid", m_module.getModuleNumber(), channel);
+      successfulCompletion = true;
+
+    } finally {
+      if (!successfulCompletion) {
+        if (allocatedSolenoids) {
+          m_module.unreserveSolenoids(m_mask);
+        }
+        m_module.close();
+      }
+    }
   }
 
   @Override
   public void close() {
     SendableRegistry.remove(this);
-    SolenoidJNI.freeSolenoidPort(m_solenoidHandle);
-    m_solenoidHandle = 0;
+    m_module.unreserveSolenoids(m_mask);
+    m_module.close();
+    m_module = null;
   }
 
   /**
@@ -65,7 +86,8 @@
    * @param on True will turn the solenoid output on. False will turn the solenoid output off.
    */
   public void set(boolean on) {
-    SolenoidJNI.setSolenoid(m_solenoidHandle, on);
+    int value = on ? (0xFFFF & m_mask) : 0;
+    m_module.setSolenoids(m_mask, value);
   }
 
   /**
@@ -74,7 +96,8 @@
    * @return True if the solenoid output is on or false if the solenoid output is off.
    */
   public boolean get() {
-    return SolenoidJNI.getSolenoid(m_solenoidHandle);
+    int currentAll = m_module.getSolenoids();
+    return (currentAll & m_mask) != 0;
   }
 
   /**
@@ -88,39 +111,44 @@
   }
 
   /**
-   * Check if solenoid is blacklisted. If a solenoid is shorted, it is added to the blacklist and
-   * disabled until power cycle, or until faults are cleared.
+   * Get the channel this solenoid is connected to.
    *
-   * @return If solenoid is disabled due to short.
-   * @see #clearAllPCMStickyFaults()
+   * @return The channel this solenoid is connected to.
    */
-  public boolean isBlackListed() {
-    int value = getPCMSolenoidBlackList() & (1 << m_channel);
-    return value != 0;
+  public int getChannel() {
+    return m_channel;
   }
 
   /**
-   * Set the pulse duration in the PCM. This is used in conjunction with
-   * the startPulse method to allow the PCM to control the timing of a pulse.
-   * The timing can be controlled in 0.01 second increments.
+   * Check if solenoid is DisabledListed. If a solenoid is shorted, it is added to the Disabled List
+   * and disabled until power cycle, or until faults are cleared.
+   *
+   * @return If solenoid is disabled due to short.
+   */
+  public boolean isDisabled() {
+    return (m_module.getSolenoidDisabledList() & m_mask) != 0;
+  }
+
+  /**
+   * Set the pulse duration in the PCM. This is used in conjunction with the startPulse method to
+   * allow the PCM to control the timing of a pulse. The timing can be controlled in 0.01 second
+   * increments.
    *
    * @param durationSeconds The duration of the pulse, from 0.01 to 2.55 seconds.
-   *
    * @see #startPulse()
    */
   public void setPulseDuration(double durationSeconds) {
     long durationMS = (long) (durationSeconds * 1000);
-    SolenoidJNI.setOneShotDuration(m_solenoidHandle, durationMS);
+    m_module.setOneShotDuration(m_channel, (int) durationMS);
   }
 
   /**
-   * Trigger the PCM to generate a pulse of the duration set in
-   * setPulseDuration.
+   * Trigger the PCM to generate a pulse of the duration set in setPulseDuration.
    *
    * @see #setPulseDuration(double)
    */
   public void startPulse() {
-    SolenoidJNI.fireOneShot(m_solenoidHandle);
+    m_module.fireOneShot(m_channel);
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
deleted file mode 100644
index a778d8c..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SolenoidBase.java
+++ /dev/null
@@ -1,141 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.SolenoidJNI;
-
-/**
- * SolenoidBase class is the common base class for the {@link Solenoid} and {@link DoubleSolenoid}
- * classes.
- */
-public class SolenoidBase {
-  protected final int m_moduleNumber; // The number of the solenoid module being used.
-
-  /**
-   * Constructor.
-   *
-   * @param moduleNumber The PCM CAN ID
-   */
-  protected SolenoidBase(final int moduleNumber) {
-    m_moduleNumber = moduleNumber;
-  }
-
-  /**
-   * Read all 8 solenoids from the specified module as a single byte.
-   *
-   * @param moduleNumber the module number to read
-   * @return The current value of all 8 solenoids on the module.
-   */
-  public static int getAll(int moduleNumber) {
-    return SolenoidJNI.getAllSolenoids(moduleNumber);
-  }
-
-  /**
-   * Read all 8 solenoids from the module used by this solenoid as a single byte.
-   *
-   * @return The current value of all 8 solenoids on this module.
-   */
-  public int getAll() {
-    return SolenoidBase.getAll(m_moduleNumber);
-  }
-
-  /**
-   * Reads complete solenoid blacklist for all 8 solenoids as a single byte. If a solenoid is
-   * shorted, it is added to the blacklist and disabled until power cycle, or until faults are
-   * cleared.
-   *
-   * @param moduleNumber the module number to read
-   * @return The solenoid blacklist of all 8 solenoids on the module.
-   * @see #clearAllPCMStickyFaults()
-   */
-  public static int getPCMSolenoidBlackList(int moduleNumber) {
-    return SolenoidJNI.getPCMSolenoidBlackList(moduleNumber);
-  }
-
-  /**
-   * Reads complete solenoid blacklist for all 8 solenoids as a single byte. If a solenoid is
-   * shorted, it is added to the blacklist and disabled until power cycle, or until faults are
-   * cleared.
-   *
-   * @return The solenoid blacklist of all 8 solenoids on the module.
-   * @see #clearAllPCMStickyFaults()
-   */
-  public int getPCMSolenoidBlackList() {
-    return SolenoidBase.getPCMSolenoidBlackList(m_moduleNumber);
-  }
-
-  /**
-   * If true, the common highside solenoid voltage rail is too low, most likely a solenoid channel
-   * is shorted.
-   *
-   * @param moduleNumber the module number to read
-   * @return true if PCM sticky fault is set
-   */
-  public static boolean getPCMSolenoidVoltageStickyFault(int moduleNumber) {
-    return SolenoidJNI.getPCMSolenoidVoltageStickyFault(moduleNumber);
-  }
-
-  /**
-   * If true, the common highside solenoid voltage rail is too low, most likely a solenoid channel
-   * is shorted.
-   *
-   * @return true if PCM sticky fault is set
-   */
-  public boolean getPCMSolenoidVoltageStickyFault() {
-    return SolenoidBase.getPCMSolenoidVoltageStickyFault(m_moduleNumber);
-  }
-
-  /**
-   * The common highside solenoid voltage rail is too low, most likely a solenoid channel is
-   * shorted.
-   *
-   * @param moduleNumber the module number to read
-   * @return true if PCM is in fault state.
-   */
-  public static boolean getPCMSolenoidVoltageFault(int moduleNumber) {
-    return SolenoidJNI.getPCMSolenoidVoltageFault(moduleNumber);
-  }
-
-  /**
-   * The common highside solenoid voltage rail is too low, most likely a solenoid channel is
-   * shorted.
-   *
-   * @return true if PCM is in fault state.
-   */
-  public boolean getPCMSolenoidVoltageFault() {
-    return SolenoidBase.getPCMSolenoidVoltageFault(m_moduleNumber);
-  }
-
-  /**
-   * Clear ALL sticky faults inside PCM that Compressor is wired to.
-   *
-   * <p>If a sticky fault is set, then it will be persistently cleared. Compressor drive maybe
-   * momentarily disable while flags are being cleared. Care should be taken to not call this too
-   * frequently, otherwise normal compressor functionality may be prevented.
-   *
-   * <p>If no sticky faults are set then this call will have no effect.
-   *
-   * @param moduleNumber the module number to read
-   */
-  public static void clearAllPCMStickyFaults(int moduleNumber) {
-    SolenoidJNI.clearAllPCMStickyFaults(moduleNumber);
-  }
-
-  /**
-   * Clear ALL sticky faults inside PCM that Compressor is wired to.
-   *
-   * <p>If a sticky fault is set, then it will be persistently cleared. Compressor drive maybe
-   * momentarily disable while flags are being cleared. Care should be taken to not call this too
-   * frequently, otherwise normal compressor functionality may be prevented.
-   *
-   * <p>If no sticky faults are set then this call will have no effect.
-   */
-  public void clearAllPCMStickyFaults() {
-    SolenoidBase.clearAllPCMStickyFaults(m_moduleNumber);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
deleted file mode 100644
index c8bacbf..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Spark.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * REV Robotics SPARK Speed Controller.
- *
- * <p>Note that the SPARK uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the Spark User Manual
- * available from REV Robotics.
- *
- * <p><ul>
- * <li>2.003ms = full "forward"
- * <li>1.550ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.460ms = the "low end" of the deadband range
- * <li>0.999ms = full "reverse"
- * </ul>
- */
-public class Spark extends PWMSpeedController {
-  /**
-   * Common initialization code called by all constructors.
-   */
-  protected void initSpark() {
-    setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
-    SendableRegistry.setName(this, "Spark", getChannel());
-  }
-
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the SPARK is attached to. 0-9 are on-board, 10-19 are on
-   *                the MXP port
-   */
-  public Spark(final int channel) {
-    super(channel);
-    initSpark();
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
index 4407ff0..490455c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedController.java
@@ -1,28 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
 /**
- * Interface for speed controlling devices.
+ * Interface for motor controlling devices.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.motorcontrol.MotorController}.
  */
-public interface SpeedController extends PIDOutput {
+@Deprecated(since = "2022", forRemoval = true)
+public interface SpeedController {
   /**
-   * Common interface for setting the speed of a speed controller.
+   * Common interface for setting the speed of a motor controller.
    *
    * @param speed The speed to set. Value should be between -1.0 and 1.0.
    */
   void set(double speed);
 
   /**
-   * Sets the voltage output of the SpeedController.  Compensates for the current bus
-   * voltage to ensure that the desired voltage is output even if the battery voltage is below
-   * 12V - highly useful when the voltage outputs are "meaningful" (e.g. they come from a
-   * feedforward calculation).
+   * Sets the voltage output of the MotorController. Compensates for the current bus voltage to
+   * ensure that the desired voltage is output even if the battery voltage is below 12V - highly
+   * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
+   * calculation).
    *
    * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
    * properly - unlike the ordinary set function, it is not "set it and forget it."
@@ -34,29 +34,27 @@
   }
 
   /**
-   * Common interface for getting the current set speed of a speed controller.
+   * Common interface for getting the current set speed of a motor controller.
    *
    * @return The current set speed. Value is between -1.0 and 1.0.
    */
   double get();
 
   /**
-   * Common interface for inverting direction of a speed controller.
+   * Common interface for inverting direction of a motor controller.
    *
    * @param isInverted The state of inversion true is inverted.
    */
   void setInverted(boolean isInverted);
 
   /**
-   * Common interface for returning if a speed controller is in the inverted state or not.
+   * Common interface for returning if a motor controller is in the inverted state or not.
    *
    * @return isInverted The state of the inversion true is inverted.
    */
   boolean getInverted();
 
-  /**
-   * Disable the speed controller.
-   */
+  /** Disable the motor controller. */
   void disable();
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
index 81b5470..d2217b7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SpeedControllerGroup.java
@@ -1,21 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import java.util.Arrays;
 
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
 /**
  * Allows multiple {@link SpeedController} objects to be linked together.
+ *
+ * @deprecated Use {@link edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}.
  */
-public class SpeedControllerGroup implements SpeedController, Sendable, AutoCloseable {
+@Deprecated(since = "2022", forRemoval = true)
+@SuppressWarnings("removal")
+public class SpeedControllerGroup implements MotorController, Sendable, AutoCloseable {
   private boolean m_isInverted;
   private final SpeedController[] m_speedControllers;
   private static int instances;
@@ -23,16 +25,14 @@
   /**
    * Create a new SpeedControllerGroup with the provided SpeedControllers.
    *
+   * @param speedController The first SpeedController to add.
    * @param speedControllers The SpeedControllers to add
    */
-  @SuppressWarnings("PMD.AvoidArrayLoops")
-  public SpeedControllerGroup(SpeedController speedController,
-                              SpeedController... speedControllers) {
+  public SpeedControllerGroup(
+      SpeedController speedController, SpeedController... speedControllers) {
     m_speedControllers = new SpeedController[speedControllers.length + 1];
     m_speedControllers[0] = speedController;
-    for (int i = 0; i < speedControllers.length; i++) {
-      m_speedControllers[i + 1] = speedControllers[i];
-    }
+    System.arraycopy(speedControllers, 0, m_speedControllers, 1, speedControllers.length);
     init();
   }
 
@@ -46,7 +46,7 @@
       SendableRegistry.addChild(this, controller);
     }
     instances++;
-    SendableRegistry.addLW(this, "SpeedControllerGroup", instances);
+    SendableRegistry.addLW(this, "MotorControllerGroup", instances);
   }
 
   @Override
@@ -94,13 +94,8 @@
   }
 
   @Override
-  public void pidWrite(double output) {
-    set(output);
-  }
-
-  @Override
   public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("Speed Controller");
+    builder.setSmartDashboardType("Motor Controller");
     builder.setActuator(true);
     builder.setSafeState(this::stopMotor);
     builder.addDoubleProperty("Value", this::get, this::set);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java
new file mode 100644
index 0000000..1e7a54d
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/SynchronousInterrupt.java
@@ -0,0 +1,165 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.hal.InterruptJNI;
+
+/**
+ * Class for handling synchronous (blocking) interrupts.
+ *
+ * <p>By default, interrupts will occur on rising edge.
+ *
+ * <p>Asynchronous interrupts are handled by the AsynchronousInterrupt class.
+ */
+public class SynchronousInterrupt implements AutoCloseable {
+  @SuppressWarnings("PMD.SingularField")
+  private final DigitalSource m_source;
+
+  private final int m_handle;
+
+  @SuppressWarnings("JavadocMethod")
+  public enum WaitResult {
+    kTimeout(0x0),
+    kRisingEdge(0x1),
+    kFallingEdge(0x100),
+    kBoth(0x101);
+
+    @SuppressWarnings("MemberName")
+    public final int value;
+
+    WaitResult(int value) {
+      this.value = value;
+    }
+
+    /**
+     * Create a wait result enum.
+     *
+     * @param rising True if a rising edge occurred.
+     * @param falling True if a falling edge occurred.
+     * @return A wait result enum.
+     */
+    public static WaitResult getValue(boolean rising, boolean falling) {
+      if (rising && falling) {
+        return kBoth;
+      } else if (rising) {
+        return kRisingEdge;
+      } else if (falling) {
+        return kFallingEdge;
+      } else {
+        return kTimeout;
+      }
+    }
+  }
+
+  /**
+   * Constructs a new synchronous interrupt using a DigitalSource.
+   *
+   * <p>At construction, the interrupt will trigger on the rising edge.
+   *
+   * @param source The digital source to use.
+   */
+  public SynchronousInterrupt(DigitalSource source) {
+    m_source = requireNonNullParam(source, "source", "SynchronousInterrupt");
+    m_handle = InterruptJNI.initializeInterrupts();
+    InterruptJNI.requestInterrupts(
+        m_handle, m_source.getPortHandleForRouting(), m_source.getAnalogTriggerTypeForRouting());
+    InterruptJNI.setInterruptUpSourceEdge(m_handle, true, false);
+  }
+
+  /**
+   * Closes the interrupt.
+   *
+   * <p>This does not close the associated digital source.
+   */
+  @Override
+  public void close() {
+    InterruptJNI.cleanInterrupts(m_handle);
+  }
+
+  /**
+   * Wait for interrupt that returns the raw result value from the hardware.
+   *
+   * <p>Used by AsynchronousInterrupt. Users should use waitForInterrupt.
+   *
+   * @param timeoutSeconds The timeout in seconds. 0 or less will return immediately.
+   * @param ignorePrevious True to ignore if a previous interrupt has occurred, and only wait for a
+   *     new trigger. False will consider if an interrupt has occurred since the last time the
+   *     interrupt was read.
+   * @return The raw hardware interrupt result
+   */
+  int waitForInterruptRaw(double timeoutSeconds, boolean ignorePrevious) {
+    return InterruptJNI.waitForInterrupt(m_handle, timeoutSeconds, ignorePrevious);
+  }
+
+  /**
+   * Wait for an interrupt.
+   *
+   * @param timeoutSeconds The timeout in seconds. 0 or less will return immediately.
+   * @param ignorePrevious True to ignore if a previous interrupt has occurred, and only wait for a
+   *     new trigger. False will consider if an interrupt has occurred since the last time the
+   *     interrupt was read.
+   * @return Result of which edges were triggered, or if an timeout occurred.
+   */
+  public WaitResult waitForInterrupt(double timeoutSeconds, boolean ignorePrevious) {
+    int result = InterruptJNI.waitForInterrupt(m_handle, timeoutSeconds, ignorePrevious);
+
+    // Rising edge result is the interrupt bit set in the byte 0xFF
+    // Falling edge result is the interrupt bit set in the byte 0xFF00
+    // Set any bit set to be true for that edge, and AND the 2 results
+    // together to match the existing enum for all interrupts
+    boolean rising = (result & 0xFF) != 0;
+    boolean falling = (result & 0xFF00) != 0;
+    return WaitResult.getValue(rising, falling);
+  }
+
+  /**
+   * Wait for an interrupt, ignoring any previously occurring interrupts.
+   *
+   * @param timeoutSeconds The timeout in seconds. 0 or less will return immediately.
+   * @return Result of which edges were triggered, or if an timeout occurred.
+   */
+  public WaitResult waitForInterrupt(double timeoutSeconds) {
+    return waitForInterrupt(timeoutSeconds, true);
+  }
+
+  /**
+   * Set which edges to trigger the interrupt on.
+   *
+   * @param risingEdge Trigger on rising edge
+   * @param fallingEdge Trigger on falling edge
+   */
+  public void setInterruptEdges(boolean risingEdge, boolean fallingEdge) {
+    InterruptJNI.setInterruptUpSourceEdge(m_handle, risingEdge, fallingEdge);
+  }
+
+  /**
+   * Get the timestamp of the last rising edge.
+   *
+   * <p>This only works if rising edge was configured using setInterruptEdges.
+   *
+   * @return the timestamp in seconds relative to getFPGATime
+   */
+  public double getRisingTimestamp() {
+    return InterruptJNI.readInterruptRisingTimestamp(m_handle) * 1e-6;
+  }
+
+  /**
+   * Get the timestamp of the last falling edge.
+   *
+   * <p>This only works if falling edge was configured using setInterruptEdges.
+   *
+   * @return the timestamp in seconds relative to getFPGATime
+   */
+  public double getFallingTimestamp() {
+    return InterruptJNI.readInterruptFallingTimestamp(m_handle) * 1e-6;
+  }
+
+  /** Force triggering of any waiting interrupt, which will be seen as a timeout. */
+  public void wakeupWaitingInterrupt() {
+    InterruptJNI.releaseWaitingInterrupt(m_handle);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
deleted file mode 100644
index 6f20869..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Talon.java
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * Cross the Road Electronics (CTRE) Talon and Talon SR Speed Controller.
- *
- * <p>Note that the Talon uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the Talon User Manual
- * available from CTRE.
- *
- * <p><ul>
- * <li>2.037ms = full "forward"
- * <li>1.539ms = the "high end" of the deadband range
- * <li>1.513ms = center of the deadband range (off)
- * <li>1.487ms = the "low end" of the deadband range
- * <li>0.989ms = full "reverse"
- * </ul>
- */
-public class Talon extends PWMSpeedController {
-  /**
-   * Constructor for a Talon (original or Talon SR).
-   *
-   * @param channel The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on
-   *                the MXP port
-   */
-  public Talon(final int channel) {
-    super(channel);
-
-    setBounds(2.037, 1.539, 1.513, 1.487, 0.989);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_Talon, getChannel() + 1);
-    SendableRegistry.setName(this, "Talon", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
index 7b49d4d..8b0d167 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Threads.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -11,35 +8,35 @@
 
 public final class Threads {
   /**
-  * Get the thread priority for the current thread.
-  * @return The current thread priority. Scaled 1-99.
-  */
+   * Get the thread priority for the current thread.
+   *
+   * @return The current thread priority. For real-time, this is 1-99 with 99 being highest. For
+   *     non-real-time, this is 0. See "man 7 sched" for details.
+   */
   public static int getCurrentThreadPriority() {
     return ThreadsJNI.getCurrentThreadPriority();
   }
 
   /**
-  * Get if the current thread is realtime.
-  * @return If the current thread is realtime
-  */
+   * Get if the current thread is real-time.
+   *
+   * @return If the current thread is real-time.
+   */
   public static boolean getCurrentThreadIsRealTime() {
     return ThreadsJNI.getCurrentThreadIsRealTime();
   }
 
   /**
-  * Sets the thread priority for the current thread.
-  *
-  * @param realTime Set to true to set a realtime priority, false for standard
-  *     priority
-  * @param priority Priority to set the thread to. Scaled 1-99, with 1 being
-  *     highest. On RoboRIO, priority is ignored for non realtime setting
-  *
-  * @return The success state of setting the priority
-  */
+   * Sets the thread priority for the current thread.
+   *
+   * @param realTime Set to true to set a real-time priority, false for standard priority.
+   * @param priority Priority to set the thread to. For real-time, this is 1-99 with 99 being
+   *     highest. For non-real-time, this is forced to 0. See "man 7 sched" for details.
+   * @return True on success.
+   */
   public static boolean setCurrentThreadPriority(boolean realTime, int priority) {
     return ThreadsJNI.setCurrentThreadPriority(realTime, priority);
   }
 
-  private Threads() {
-  }
+  private Threads() {}
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
index 99bce3d..dda62d2 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimedRobot.java
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.PriorityQueue;
-
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.NotifierJNI;
+import java.util.PriorityQueue;
 
 /**
  * TimedRobot implements the IterativeRobotBase robot program framework.
@@ -23,7 +19,7 @@
  */
 public class TimedRobot extends IterativeRobotBase {
   @SuppressWarnings("MemberName")
-  class Callback implements Comparable<Callback> {
+  static class Callback implements Comparable<Callback> {
     public Runnable func;
     public double period;
     public double expirationTime;
@@ -31,20 +27,33 @@
     /**
      * Construct a callback container.
      *
-     * @param func             The callback to run.
-     * @param startTimeSeconds The common starting point for all callback
-     *                         scheduling in seconds.
-     * @param periodSeconds    The period at which to run the callback in
-     *                         seconds.
-     * @param offsetSeconds    The offset from the common starting time in
-     *                         seconds.
+     * @param func The callback to run.
+     * @param startTimeSeconds The common starting point for all callback scheduling in seconds.
+     * @param periodSeconds The period at which to run the callback in seconds.
+     * @param offsetSeconds The offset from the common starting time in seconds.
      */
     Callback(Runnable func, double startTimeSeconds, double periodSeconds, double offsetSeconds) {
       this.func = func;
       this.period = periodSeconds;
-      this.expirationTime = startTimeSeconds + offsetSeconds
-        + Math.floor((Timer.getFPGATimestamp() - startTimeSeconds)
-            / this.period) * this.period + this.period;
+      this.expirationTime =
+          startTimeSeconds
+              + offsetSeconds
+              + Math.floor((Timer.getFPGATimestamp() - startTimeSeconds) / this.period)
+                  * this.period
+              + this.period;
+    }
+
+    @Override
+    public boolean equals(Object rhs) {
+      if (rhs instanceof Callback) {
+        return Double.compare(expirationTime, ((Callback) rhs).expirationTime) == 0;
+      }
+      return false;
+    }
+
+    @Override
+    public int hashCode() {
+      return Double.hashCode(expirationTime);
     }
 
     @Override
@@ -65,9 +74,7 @@
 
   private final PriorityQueue<Callback> m_callbacks = new PriorityQueue<>();
 
-  /**
-   * Constructor for TimedRobot.
-   */
+  /** Constructor for TimedRobot. */
   protected TimedRobot() {
     this(kDefaultPeriod);
   }
@@ -93,9 +100,7 @@
     NotifierJNI.cleanNotifier(m_notifier);
   }
 
-  /**
-   * Provide an alternate "main loop" via startCompetition().
-   */
+  /** Provide an alternate "main loop" via startCompetition(). */
   @Override
   @SuppressWarnings("UnsafeFinalization")
   public void startCompetition() {
@@ -106,6 +111,7 @@
     }
 
     // Tell the DS that the robot is ready to be enabled
+    System.out.println("********** Robot program startup complete **********");
     HAL.observeUserProgramStarting();
 
     // Loop forever, calling the appropriate mode-dependent function
@@ -139,28 +145,19 @@
     }
   }
 
-  /**
-   * Ends the main loop in startCompetition().
-   */
+  /** Ends the main loop in startCompetition(). */
   @Override
   public void endCompetition() {
     NotifierJNI.stopNotifier(m_notifier);
   }
 
   /**
-   * Get time period between calls to Periodic() functions.
-   */
-  public double getPeriod() {
-    return m_period;
-  }
-
-  /**
    * Add a callback to run at a specific period.
    *
    * <p>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 callback The callback to run.
    * @param periodSeconds The period at which to run the callback in seconds.
    */
   public void addPeriodic(Runnable callback, double periodSeconds) {
@@ -173,11 +170,10 @@
    * <p>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 callback The callback to run.
    * @param periodSeconds The period at which to run the callback in seconds.
-   * @param offsetSeconds The offset from the common starting time in seconds.
-   *                      This is useful for scheduling a callback in a
-   *                      different timeslot relative to TimedRobot.
+   * @param offsetSeconds The offset from the common starting time in seconds. This is useful for
+   *     scheduling a callback in a different timeslot relative to TimedRobot.
    */
   public void addPeriodic(Runnable callback, double periodSeconds, double offsetSeconds) {
     m_callbacks.add(new Callback(callback, m_startTime, periodSeconds, offsetSeconds));
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
index 5d71e72..939e255 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Timer.java
@@ -1,12 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+/**
+ * A timer class.
+ *
+ * <p>Note that if the user calls SimHooks.restartTiming(), they should also reset the timer so
+ * get() won't return a negative duration.
+ */
 public class Timer {
   /**
    * Return the system clock time in seconds. Return the time from the FPGA hardware clock in
@@ -23,13 +26,13 @@
    * Return the approximate match time. The FMS does not send an official match time to the robots,
    * but does send an approximate match time. The value will count down the time remaining in the
    * current period (auto or teleop). 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 behavior seen on the field.
+   * dispute ref calls or guarantee that a function will trigger before the match ends) The Practice
+   * Match function of the DS approximates the behavior seen on the field.
    *
    * @return Time remaining in current match period (auto or teleop) in seconds
    */
   public static double getMatchTime() {
-    return DriverStation.getInstance().getMatchTime();
+    return DriverStation.getMatchTime();
   }
 
   /**
@@ -52,12 +55,7 @@
   private double m_accumulatedTime;
   private boolean m_running;
 
-  /**
-   * Lock for synchronization.
-   */
-  private final Object m_lock = new Object();
-
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public Timer() {
     reset();
   }
@@ -74,24 +72,21 @@
    * @return Current time value for this timer in seconds
    */
   public double get() {
-    synchronized (m_lock) {
-      if (m_running) {
-        return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
-      } else {
-        return m_accumulatedTime;
-      }
+    if (m_running) {
+      return m_accumulatedTime + (getMsClock() - m_startTime) / 1000.0;
+    } else {
+      return m_accumulatedTime;
     }
   }
 
   /**
-   * Reset the timer by setting the time to 0. Make the timer startTime the current time so new
-   * requests will be relative now
+   * Reset the timer by setting the time to 0.
+   *
+   * <p>Make the timer startTime the current time so new requests will be relative now.
    */
   public void reset() {
-    synchronized (m_lock) {
-      m_accumulatedTime = 0;
-      m_startTime = getMsClock();
-    }
+    m_accumulatedTime = 0;
+    m_startTime = getMsClock();
   }
 
   /**
@@ -100,11 +95,9 @@
    * already running.
    */
   public void start() {
-    synchronized (m_lock) {
-      if (!m_running) {
-        m_startTime = getMsClock();
-        m_running = true;
-      }
+    if (!m_running) {
+      m_startTime = getMsClock();
+      m_running = true;
     }
   }
 
@@ -114,10 +107,8 @@
    * clock.
    */
   public void stop() {
-    synchronized (m_lock) {
-      m_accumulatedTime = get();
-      m_running = false;
-    }
+    m_accumulatedTime = get();
+    m_running = false;
   }
 
   /**
@@ -127,9 +118,7 @@
    * @return Whether the period has passed.
    */
   public boolean hasElapsed(double seconds) {
-    synchronized (m_lock) {
-      return get() > seconds;
-    }
+    return get() >= seconds;
   }
 
   /**
@@ -139,7 +128,9 @@
    *
    * @param period The period to check for (in seconds).
    * @return Whether the period has passed.
+   * @deprecated Use advanceIfElapsed() instead.
    */
+  @Deprecated(since = "2022", forRemoval = true)
   public boolean hasPeriodPassed(double period) {
     return advanceIfElapsed(period);
   }
@@ -153,15 +144,13 @@
    * @return Whether the period has passed.
    */
   public boolean advanceIfElapsed(double seconds) {
-    synchronized (m_lock) {
-      if (get() > seconds) {
-        // Advance the start time by the period.
-        // Don't set it to the current time... we want to avoid drift.
-        m_startTime += seconds * 1000;
-        return true;
-      } else {
-        return false;
-      }
+    if (get() >= seconds) {
+      // Advance the start time by the period.
+      // Don't set it to the current time... we want to avoid drift.
+      m_startTime += seconds * 1000;
+      return true;
+    } else {
+      return false;
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java
new file mode 100644
index 0000000..5d10bb5
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/TimesliceRobot.java
@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+/**
+ * TimesliceRobot extends the TimedRobot robot program framework to provide timeslice scheduling of
+ * periodic functions.
+ *
+ * <p>The TimesliceRobot class is intended to be subclassed by a user creating a robot program.
+ *
+ * <p>This class schedules robot operations serially in a timeslice format. TimedRobot's periodic
+ * functions are the first in the timeslice table with 0 ms offset and 20 ms period. You can
+ * schedule additional controller periodic functions at a shorter period (5 ms by default). You give
+ * each one a timeslice duration, then they're run sequentially. The main benefit of this approach
+ * is consistent starting times for each controller periodic, which can make odometry and estimators
+ * more accurate and controller outputs change more consistently.
+ *
+ * <p>Here's an example of measured subsystem durations and their timeslice allocations:
+ *
+ * <table>
+ *   <tr>
+ *     <td><b>Subsystem</b></td>
+ *     <td><b>Duration (ms)</b></td>
+ *     <td><b>Allocation (ms)</b></td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Total</b></td>
+ *     <td>5.0</td>
+ *     <td>5.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>TimedRobot</td>
+ *     <td>?</td>
+ *     <td>2.0</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Drivetrain</td>
+ *     <td>1.32</td>
+ *     <td>1.5</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Flywheel</td>
+ *     <td>0.6</td>
+ *     <td>0.7</td>
+ *   </tr>
+ *   <tr>
+ *     <td>Turret</td>
+ *     <td>0.6</td>
+ *     <td>0.8</td>
+ *   </tr>
+ *   <tr>
+ *     <td><b>Free</b></td>
+ *     <td>0.0</td>
+ *     <td>N/A</td>
+ *   </tr>
+ * </table>
+ *
+ * <p>Since TimedRobot periodic functions only run every 20ms, that leaves a 2 ms empty spot in the
+ * allocation table for three of the four 5 ms cycles comprising 20 ms. That's OK because the OS
+ * needs time to do other things.
+ *
+ * <p>If the robot periodic functions and the controller periodic functions have a lot of scheduling
+ * jitter that cause them to occasionally overlap with later timeslices, consider giving the main
+ * robot thread a real-time priority using {@link Threads#setCurrentThreadPriority(boolean,int)}. An
+ * RT priority of 15 is a reasonable choice.
+ *
+ * <p>If you do enable RT though, <i>make sure your periodic functions do not block</i>. If they do,
+ * the operating system will lock up, and you'll have to boot the roboRIO into safe mode and delete
+ * the robot program to recover.
+ */
+public class TimesliceRobot extends TimedRobot {
+  /**
+   * Constructor for TimesliceRobot.
+   *
+   * @param robotPeriodicAllocation The allocation in seconds to give the TimesliceRobot periodic
+   *     functions.
+   * @param controllerPeriod The controller period in seconds. The sum of all scheduler allocations
+   *     should be less than or equal to this value.
+   */
+  public TimesliceRobot(double robotPeriodicAllocation, double controllerPeriod) {
+    m_nextOffset = robotPeriodicAllocation;
+    m_controllerPeriod = controllerPeriod;
+  }
+
+  /**
+   * Schedule a periodic function with the constructor's controller period and the given allocation.
+   * The function's runtime allocation will be placed after the end of the previous one's.
+   *
+   * <p>If a call to this function makes the allocations exceed the controller period, an exception
+   * will be thrown since that means the TimesliceRobot periodic functions and the given function
+   * will have conflicting timeslices.
+   *
+   * @param func Function to schedule.
+   * @param allocation The function's runtime allocation in seconds out of the controller period.
+   */
+  public void schedule(Runnable func, double allocation) {
+    if (m_nextOffset + allocation > m_controllerPeriod) {
+      throw new IllegalStateException(
+          "Function scheduled at offset "
+              + m_nextOffset
+              + " with allocation "
+              + allocation
+              + " exceeded controller period of "
+              + m_controllerPeriod
+              + "\n");
+    }
+
+    addPeriodic(func, m_controllerPeriod, m_nextOffset);
+    m_nextOffset += allocation;
+  }
+
+  private double m_nextOffset;
+  private final double m_controllerPeriod;
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
index 3026e90..ea04192 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Tracer.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -12,12 +9,12 @@
 import java.util.function.Consumer;
 
 /**
- * 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 by calls to {@link #addEpoch(String)},
- * and can be printed with a call to {@link #printEpochs()}.
+ * 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 by calls to {@link #addEpoch(String)}, and can be printed
+ * with a call to {@link #printEpochs()}.
  *
- * <p>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.
+ * <p>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.
  */
 public class Tracer {
   private static final long kMinPrintPeriod = 1000000; // microseconds
@@ -28,24 +25,18 @@
   @SuppressWarnings("PMD.UseConcurrentHashMap")
   private final Map<String, Long> m_epochs = new HashMap<>(); // microseconds
 
-  /**
-   * Tracer constructor.
-   */
+  /** Tracer constructor. */
   public Tracer() {
     resetTimer();
   }
 
-  /**
-   * Clears all epochs.
-   */
+  /** Clears all epochs. */
   public void clearEpochs() {
     m_epochs.clear();
     resetTimer();
   }
 
-  /**
-   * Restarts the epoch timer.
-   */
+  /** Restarts the epoch timer. */
   public void resetTimer() {
     m_startTime = RobotController.getFPGATime();
   }
@@ -56,8 +47,8 @@
    * <p>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.
    *
-   * <p>This should be called immediately after execution has finished,
-   * with a call to this method or {@link #resetTimer()} before execution.
+   * <p>This should be called immediately after execution has finished, with a call to this method
+   * or {@link #resetTimer()} before execution.
    *
    * @param epochName The name to associate with the epoch.
    */
@@ -67,9 +58,7 @@
     m_startTime = currentTime;
   }
 
-  /**
-   * Prints list of epochs added so far and their times to the DriverStation.
-   */
+  /** Prints list of epochs added so far and their times to the DriverStation. */
   public void printEpochs() {
     printEpochs(out -> DriverStation.reportWarning(out, false));
   }
@@ -86,9 +75,10 @@
     if (now - m_lastEpochsPrintTime > kMinPrintPeriod) {
       StringBuilder sb = new StringBuilder();
       m_lastEpochsPrintTime = now;
-      m_epochs.forEach((key, value) -> {
-        sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6));
-      });
+      m_epochs.forEach(
+          (key, value) -> {
+            sb.append(String.format("\t%s: %.6fs\n", key, value / 1.0e6));
+          });
       if (sb.length() > 0) {
         output.accept(sb.toString());
       }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
index 0e14c96..2d3fb4e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
@@ -1,24 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.ArrayList;
-import java.util.List;
+import static java.util.Objects.requireNonNull;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
 import edu.wpi.first.hal.SimDouble;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-import static java.util.Objects.requireNonNull;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.ArrayList;
+import java.util.List;
 
 /**
  * Ultrasonic rangefinder class. The Ultrasonic rangefinder measures absolute distance based on the
@@ -29,21 +27,7 @@
  * echo is received. The time that the line is high determines the round trip distance (time of
  * flight).
  */
-public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
-  /**
-   * The units to return when PIDGet is called.
-   */
-  public enum Unit {
-    /**
-     * Use inches for PIDGet.
-     */
-    kInches,
-    /**
-     * Use millimeters for PIDGet.
-     */
-    kMillimeters
-  }
-
+public class Ultrasonic implements Sendable, AutoCloseable {
   // Time (sec) for the ping trigger pulse.
   private static final double kPingTime = 10 * 1e-6;
   private static final double kSpeedOfSoundInchesPerSec = 1130.0 * 12.0;
@@ -53,14 +37,12 @@
   private static volatile boolean m_automaticEnabled;
   private DigitalInput m_echoChannel;
   private DigitalOutput m_pingChannel;
-  private boolean m_allocatedChannels;
+  private final boolean m_allocatedChannels;
   private boolean m_enabled;
   private Counter m_counter;
   // task doing the round-robin automatic sensing
   private static Thread m_task;
-  private Unit m_units;
   private static int m_instances;
-  protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
 
   private SimDevice m_simDevice;
   private SimBoolean m_simRangeValid;
@@ -79,16 +61,16 @@
     @Override
     public synchronized void run() {
       while (m_automaticEnabled) {
-        for (Ultrasonic sensor: m_sensors) {
+        for (Ultrasonic sensor : m_sensors) {
           if (!m_automaticEnabled) {
             break;
           }
 
           if (sensor.isEnabled()) {
-            sensor.m_pingChannel.pulse(kPingTime);  // do the ping
+            sensor.m_pingChannel.pulse(kPingTime); // do the ping
           }
 
-          Timer.delay(0.1);  // wait for ping to return
+          Timer.delay(0.1); // wait for ping to return
         }
       }
     }
@@ -103,8 +85,8 @@
   private synchronized void initialize() {
     m_simDevice = SimDevice.create("Ultrasonic", m_echoChannel.getChannel());
     if (m_simDevice != null) {
-      m_simRangeValid = m_simDevice.createBoolean("Range Valid", false, true);
-      m_simRange = m_simDevice.createDouble("Range (in)", false, 0.0);
+      m_simRangeValid = m_simDevice.createBoolean("Range Valid", Direction.kInput, true);
+      m_simRange = m_simDevice.createDouble("Range (in)", Direction.kInput, 0.0);
       m_pingChannel.setSimDevice(m_simDevice);
       m_echoChannel.setSimDevice(m_simDevice);
     }
@@ -129,76 +111,47 @@
     SendableRegistry.addLW(this, "Ultrasonic", m_echoChannel.getChannel());
   }
 
+  public int getEchoChannel() {
+    return m_echoChannel.getChannel();
+  }
+
   /**
    * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
    * and Vex ultrasonic sensors.
    *
    * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
-   *                    sending the ping.
+   *     sending the ping.
    * @param echoChannel The digital input channel that receives the echo. The length of time that
-   *                    the echo is high represents the round trip time of the ping, and the
-   *                    distance.
-   * @param units       The units returned in either kInches or kMilliMeters
+   *     the echo is high represents the round trip time of the ping, and the distance.
    */
-  public Ultrasonic(final int pingChannel, final int echoChannel, Unit units) {
+  public Ultrasonic(final int pingChannel, final int echoChannel) {
     m_pingChannel = new DigitalOutput(pingChannel);
     m_echoChannel = new DigitalInput(echoChannel);
     SendableRegistry.addChild(this, m_pingChannel);
     SendableRegistry.addChild(this, m_echoChannel);
     m_allocatedChannels = true;
-    m_units = units;
     initialize();
   }
 
   /**
-   * Create an instance of the Ultrasonic Sensor. This is designed to supchannel the Daventech SRF04
-   * and Vex ultrasonic sensors. Default unit is inches.
-   *
-   * @param pingChannel The digital output channel that sends the pulse to initiate the sensor
-   *                    sending the ping.
-   * @param echoChannel The digital input channel that receives the echo. The length of time that
-   *                    the echo is high represents the round trip time of the ping, and the
-   *                    distance.
-   */
-  public Ultrasonic(final int pingChannel, final int echoChannel) {
-    this(pingChannel, echoChannel, Unit.kInches);
-  }
-
-  /**
    * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
    * DigitalOutput for the ping channel.
    *
    * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
-   *                    10uS pulse to start.
-   * @param echoChannel The digital input object that times the return pulse to determine the
-   *                    range.
-   * @param units       The units returned in either kInches or kMilliMeters
+   *     10uS pulse to start.
+   * @param echoChannel The digital input object that times the return pulse to determine the range.
    */
-  public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel, Unit units) {
+  public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
     requireNonNull(pingChannel, "Provided ping channel was null");
     requireNonNull(echoChannel, "Provided echo channel was null");
 
     m_allocatedChannels = false;
     m_pingChannel = pingChannel;
     m_echoChannel = echoChannel;
-    m_units = units;
     initialize();
   }
 
   /**
-   * Create an instance of an Ultrasonic Sensor from a DigitalInput for the echo channel and a
-   * DigitalOutput for the ping channel. Default unit is inches.
-   *
-   * @param pingChannel The digital output object that starts the sensor doing a ping. Requires a
-   *                    10uS pulse to start.
-   * @param echoChannel The digital input object that times the return pulse to determine the
-   *                    range.
-   */
-  public Ultrasonic(DigitalOutput pingChannel, DigitalInput echoChannel) {
-    this(pingChannel, echoChannel, Unit.kInches);
-  }
-
-  /**
    * Destructor for the ultrasonic sensor. Delete the instance of the ultrasonic sensor by freeing
    * the allocated digital channels. If the system was in automatic mode (round robin), then it is
    * stopped, then started again after this sensor is removed (provided this wasn't the last
@@ -245,10 +198,9 @@
    * each sensor.
    *
    * @param enabling Set to true if round robin scheduling should start for all 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 preferred, it can be implemented by pinging the sensors manually and waiting
-   *                 for the results to come back.
+   *     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 preferred, it can be
+   *     implemented by pinging the sensors manually and waiting for the results to come back.
    */
   public static void setAutomaticMode(boolean enabling) {
     if (enabling == m_automaticEnabled) {
@@ -340,54 +292,6 @@
     return getRangeInches() * 25.4;
   }
 
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    if (!pidSource.equals(PIDSourceType.kDisplacement)) {
-      throw new IllegalArgumentException("Only displacement PID is allowed for ultrasonics.");
-    }
-    m_pidSource = pidSource;
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_pidSource;
-  }
-
-  /**
-   * Get the range in the current DistanceUnit for the PIDSource base object.
-   *
-   * @return The range in DistanceUnit
-   */
-  @Override
-  public double pidGet() {
-    switch (m_units) {
-      case kInches:
-        return getRangeInches();
-      case kMillimeters:
-        return getRangeMM();
-      default:
-        return 0.0;
-    }
-  }
-
-  /**
-   * Set the current DistanceUnit that should be used for the PIDSource base object.
-   *
-   * @param units The DistanceUnit that should be used.
-   */
-  public void setDistanceUnits(Unit units) {
-    m_units = units;
-  }
-
-  /**
-   * Get the current DistanceUnit that is used for the PIDSource base object.
-   *
-   * @return The type of DistanceUnit that is being used.
-   */
-  public Unit getDistanceUnits() {
-    return m_units;
-  }
-
   /**
    * Is the ultrasonic enabled.
    *
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
deleted file mode 100644
index 29c1b27..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Victor.java
+++ /dev/null
@@ -1,51 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * VEX Robotics Victor 888 Speed Controller The Vex Robotics Victor 884 Speed Controller can also
- * be used with this class but may need to be calibrated per the Victor 884 user manual.
- *
- * <p>Note that the Victor uses the following bounds for PWM values. These values were determined
- * empirically and optimized for the Victor 888. These values should work reasonably well for
- * Victor 884 controllers also but if users experience issues such as asymmetric behavior around
- * the deadband or inability to saturate the controller in either direction, calibration is
- * recommended. The calibration procedure can be found in the Victor 884 User Manual available
- * from VEX Robotics: http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf
- *
- * <p><ul>
- * <li>2.027ms = full "forward"
- * <li>1.525ms = the "high end" of the deadband range
- * <li>1.507ms = center of the deadband range (off)
- * <li>1.490ms = the "low end" of the deadband range
- * <li>1.026ms = full "reverse"
- * </ul>
- */
-public class Victor extends PWMSpeedController {
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the Victor is attached to. 0-9 are
-   *        on-board, 10-19 are on the MXP port
-   */
-  public Victor(final int channel) {
-    super(channel);
-
-    setBounds(2.027, 1.525, 1.507, 1.49, 1.026);
-    setPeriodMultiplier(PeriodMultiplier.k2X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_Victor, getChannel() + 1);
-    SendableRegistry.setName(this, "Victor", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
deleted file mode 100644
index 22b3513..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/VictorSP.java
+++ /dev/null
@@ -1,49 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * VEX Robotics Victor SP Speed Controller.
- *
- * <p>Note that the VictorSP uses the following bounds for PWM values. These values should work
- * reasonably well for most controllers, but if users experience issues such as asymmetric
- * behavior around the deadband or inability to saturate the controller in either direction,
- * calibration is recommended. The calibration procedure can be found in the VictorSP User Manual
- * available from CTRE.
- *
- * <p><ul>
- * <li>2.004ms = full "forward"
- * <li>1.520ms = the "high end" of the deadband range
- * <li>1.500ms = center of the deadband range (off)
- * <li>1.480ms = the "low end" of the deadband range
- * <li>0.997ms = full "reverse"
- * </ul>
- */
-public class VictorSP extends PWMSpeedController {
-  /**
-   * Constructor.
-   *
-   * @param channel The PWM channel that the VictorSP is attached to. 0-9 are
-   *        on-board, 10-19 are on the MXP port
-   */
-  public VictorSP(final int channel) {
-    super(channel);
-
-    setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
-    setPeriodMultiplier(PeriodMultiplier.k1X);
-    setSpeed(0.0);
-    setZeroLatch();
-
-    HAL.report(tResourceType.kResourceType_VictorSP, getChannel() + 1);
-    SendableRegistry.setName(this, "VictorSP", getChannel());
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
index aaf5671..22581a5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/Watchdog.java
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.NotifierJNI;
 import java.io.Closeable;
 import java.util.PriorityQueue;
 import java.util.concurrent.locks.ReentrantLock;
 
-import edu.wpi.first.hal.NotifierJNI;
-
 /**
  * A class that's a wrapper around a watchdog timer.
  *
@@ -23,13 +19,13 @@
  */
 public class Watchdog implements Closeable, Comparable<Watchdog> {
   // Used for timeout print rate-limiting
-  private static final long kMinPrintPeriod = 1000000; // microseconds
+  private static final long kMinPrintPeriodMicroS = (long) 1e6;
 
-  private double m_startTime; // seconds
-  private double m_timeout; // seconds
-  private double m_expirationTime; // seconds
+  private double m_startTimeSeconds;
+  private double m_timeoutSeconds;
+  private double m_expirationTimeSeconds;
   private final Runnable m_callback;
-  private double m_lastTimeoutPrintTime; // seconds
+  private double m_lastTimeoutPrintSeconds;
 
   boolean m_isExpired;
 
@@ -50,11 +46,11 @@
   /**
    * Watchdog constructor.
    *
-   * @param timeout  The watchdog's timeout in seconds with microsecond resolution.
+   * @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
    * @param callback This function is called when the timeout expires.
    */
-  public Watchdog(double timeout, Runnable callback) {
-    m_timeout = timeout;
+  public Watchdog(double timeoutSeconds, Runnable callback) {
+    m_timeoutSeconds = timeoutSeconds;
     m_callback = callback;
     m_tracer = new Tracer();
   }
@@ -65,50 +61,50 @@
   }
 
   @Override
-  public int compareTo(Watchdog rhs) {
-    // Elements with sooner expiration times are sorted as lesser. The head of
-    // Java's PriorityQueue is the least element.
-    return Double.compare(m_expirationTime, rhs.m_expirationTime);
-  }
-
-  @Override
   public boolean equals(Object obj) {
-    if (!(obj instanceof Watchdog)) {
-      return false;
+    if (obj instanceof Watchdog) {
+      return Double.compare(m_expirationTimeSeconds, ((Watchdog) obj).m_expirationTimeSeconds) == 0;
     }
-    Watchdog oth = (Watchdog) obj;
-    return oth.m_expirationTime == m_expirationTime;
+    return false;
   }
 
   @Override
   public int hashCode() {
-    return Double.hashCode(m_expirationTime);
+    return Double.hashCode(m_expirationTimeSeconds);
+  }
+
+  @Override
+  public int compareTo(Watchdog rhs) {
+    // Elements with sooner expiration times are sorted as lesser. The head of
+    // Java's PriorityQueue is the least element.
+    return Double.compare(m_expirationTimeSeconds, rhs.m_expirationTimeSeconds);
   }
 
   /**
    * Returns the time in seconds since the watchdog was last fed.
+   *
+   * @return The time in seconds since the watchdog was last fed.
    */
   public double getTime() {
-    return Timer.getFPGATimestamp() - m_startTime;
+    return Timer.getFPGATimestamp() - m_startTimeSeconds;
   }
 
   /**
    * Sets the watchdog's timeout.
    *
-   * @param timeout The watchdog's timeout in seconds with microsecond
-   *                resolution.
+   * @param timeoutSeconds The watchdog's timeout in seconds with microsecond resolution.
    */
-  public void setTimeout(double timeout) {
-    m_startTime = Timer.getFPGATimestamp();
+  public void setTimeout(double timeoutSeconds) {
+    m_startTimeSeconds = Timer.getFPGATimestamp();
     m_tracer.clearEpochs();
 
     m_queueMutex.lock();
     try {
-      m_timeout = timeout;
+      m_timeoutSeconds = timeoutSeconds;
       m_isExpired = false;
 
       m_watchdogs.remove(this);
-      m_expirationTime = m_startTime + m_timeout;
+      m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
       m_watchdogs.add(this);
       updateAlarm();
     } finally {
@@ -118,11 +114,13 @@
 
   /**
    * Returns the watchdog's timeout in seconds.
+   *
+   * @return The watchdog's timeout in seconds.
    */
   public double getTimeout() {
     m_queueMutex.lock();
     try {
-      return m_timeout;
+      return m_timeoutSeconds;
     } finally {
       m_queueMutex.unlock();
     }
@@ -130,6 +128,8 @@
 
   /**
    * Returns true if the watchdog timer has expired.
+   *
+   * @return True if the watchdog timer has expired.
    */
   public boolean isExpired() {
     m_queueMutex.lock();
@@ -144,7 +144,6 @@
    * Adds time since last epoch to the list printed by printEpochs().
    *
    * @see Tracer#addEpoch(String)
-   *
    * @param epochName The name to associate with the epoch.
    */
   public void addEpoch(String epochName) {
@@ -153,6 +152,7 @@
 
   /**
    * Prints list of epochs added so far and their times.
+   *
    * @see Tracer#printEpochs()
    */
   public void printEpochs() {
@@ -168,11 +168,9 @@
     enable();
   }
 
-  /**
-   * Enables the watchdog timer.
-   */
+  /** Enables the watchdog timer. */
   public void enable() {
-    m_startTime = Timer.getFPGATimestamp();
+    m_startTimeSeconds = Timer.getFPGATimestamp();
     m_tracer.clearEpochs();
 
     m_queueMutex.lock();
@@ -180,7 +178,7 @@
       m_isExpired = false;
 
       m_watchdogs.remove(this);
-      m_expirationTime = m_startTime + m_timeout;
+      m_expirationTimeSeconds = m_startTimeSeconds + m_timeoutSeconds;
       m_watchdogs.add(this);
       updateAlarm();
     } finally {
@@ -188,9 +186,7 @@
     }
   }
 
-  /**
-   * Disables the watchdog timer.
-   */
+  /** Disables the watchdog timer. */
   public void disable() {
     m_queueMutex.lock();
     try {
@@ -212,12 +208,13 @@
     m_suppressTimeoutMessage = suppress;
   }
 
+  @SuppressWarnings("resource")
   private static void updateAlarm() {
     if (m_watchdogs.size() == 0) {
       NotifierJNI.cancelNotifierAlarm(m_notifier);
     } else {
-      NotifierJNI.updateNotifierAlarm(m_notifier,
-          (long) (m_watchdogs.peek().m_expirationTime * 1e6));
+      NotifierJNI.updateNotifierAlarm(
+          m_notifier, (long) (m_watchdogs.peek().m_expirationTimeSeconds * 1e6));
     }
   }
 
@@ -247,12 +244,11 @@
         Watchdog watchdog = m_watchdogs.poll();
 
         double now = curTime * 1e-6;
-        if (now - watchdog.m_lastTimeoutPrintTime > kMinPrintPeriod) {
-          watchdog.m_lastTimeoutPrintTime = now;
+        if (now - watchdog.m_lastTimeoutPrintSeconds > kMinPrintPeriodMicroS) {
+          watchdog.m_lastTimeoutPrintSeconds = now;
           if (!watchdog.m_suppressTimeoutMessage) {
             DriverStation.reportWarning(
-                String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeout),
-                false);
+                String.format("Watchdog not fed within %.6fs\n", watchdog.m_timeoutSeconds), false);
           }
         }
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
index 6248a76..5de3503 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/XboxController.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -18,14 +15,12 @@
  * and the mapping of ports to hardware buttons depends on the code in the Driver Station.
  */
 public class XboxController extends GenericHID {
-  /**
-   * Represents a digital button on an XboxController.
-   */
+  /** Represents a digital button on an XboxController. */
   public enum Button {
-    kBumperLeft(5),
-    kBumperRight(6),
-    kStickLeft(9),
-    kStickRight(10),
+    kLeftBumper(5),
+    kRightBumper(6),
+    kLeftStick(9),
+    kRightStick(10),
     kA(1),
     kB(2),
     kX(3),
@@ -33,17 +28,32 @@
     kBack(7),
     kStart(8);
 
-    @SuppressWarnings({"MemberName", "PMD.SingularField"})
+    @SuppressWarnings("MemberName")
     public final int value;
 
     Button(int value) {
       this.value = value;
     }
+
+    /**
+     * Get the human-friendly name of the button, matching the relevant methods. This is done by
+     * stripping the leading `k`, and if not a Bumper button append `Button`.
+     *
+     * <p>Primarily used for automated unit tests.
+     *
+     * @return the human-friendly name of the button.
+     */
+    @Override
+    public String toString() {
+      var name = this.name().substring(1); // Remove leading `k`
+      if (name.endsWith("Bumper")) {
+        return name;
+      }
+      return name + "Button";
+    }
   }
 
-  /**
-   * Represents an axis on an XboxController.
-   */
+  /** Represents an axis on an XboxController. */
   public enum Axis {
     kLeftX(0),
     kRightX(4),
@@ -52,19 +62,35 @@
     kLeftTrigger(2),
     kRightTrigger(3);
 
-    @SuppressWarnings({"MemberName", "PMD.SingularField"})
+    @SuppressWarnings("MemberName")
     public final int value;
 
     Axis(int value) {
       this.value = value;
     }
+
+    /**
+     * Get the human-friendly name of the axis, matching the relevant methods. This is done by
+     * stripping the leading `k`, and if a trigger axis append `Axis`.
+     *
+     * <p>Primarily used for automated unit tests.
+     *
+     * @return the human-friendly name of the axis.
+     */
+    @Override
+    public String toString() {
+      var name = this.name().substring(1); // Remove leading `k`
+      if (name.endsWith("Trigger")) {
+        return name + "Axis";
+      }
+      return name;
+    }
   }
 
   /**
-   * Construct an instance of a joystick. The joystick index is the USB port on the drivers
-   * station.
+   * Construct an instance of a controller.
    *
-   * @param port The port on the Driver Station that the joystick is plugged into.
+   * @param port The port index on the Driver Station that the controller is plugged into.
    */
   public XboxController(final int port) {
     super(port);
@@ -73,131 +99,167 @@
   }
 
   /**
-   * Get the X axis value of the controller.
+   * Get the X axis value of left side of the controller.
    *
-   * @param hand Side of controller whose value should be returned.
-   * @return The X axis value of the controller.
+   * @return The axis value.
    */
-  @Override
-  public double getX(Hand hand) {
-    if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(Axis.kLeftX.value);
-    } else {
-      return getRawAxis(Axis.kRightX.value);
-    }
+  public double getLeftX() {
+    return getRawAxis(Axis.kLeftX.value);
   }
 
   /**
-   * Get the Y axis value of the controller.
+   * Get the X axis value of right side of the controller.
    *
-   * @param hand Side of controller whose value should be returned.
-   * @return The Y axis value of the controller.
+   * @return The axis value.
    */
-  @Override
-  public double getY(Hand hand) {
-    if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(Axis.kLeftY.value);
-    } else {
-      return getRawAxis(Axis.kRightY.value);
-    }
+  public double getRightX() {
+    return getRawAxis(Axis.kRightX.value);
   }
 
   /**
-   * Get the trigger axis value of the controller.
+   * Get the Y axis value of left side of the controller.
    *
-   * @param hand Side of controller whose value should be returned.
-   * @return The trigger axis value of the controller.
+   * @return The axis value.
    */
-  public double getTriggerAxis(Hand hand) {
-    if (hand.equals(Hand.kLeft)) {
-      return getRawAxis(Axis.kLeftTrigger.value);
-    } else {
-      return getRawAxis(Axis.kRightTrigger.value);
-    }
+  public double getLeftY() {
+    return getRawAxis(Axis.kLeftY.value);
   }
 
   /**
-   * Read the value of the bumper button on the controller.
+   * Get the Y axis value of right side of the controller.
    *
-   * @param hand Side of controller whose value should be returned.
+   * @return The axis value.
+   */
+  public double getRightY() {
+    return getRawAxis(Axis.kRightY.value);
+  }
+
+  /**
+   * Get the left trigger (LT) axis value of the controller. Note that this axis is bound to the
+   * range of [0, 1] as opposed to the usual [-1, 1].
+   *
+   * @return The axis value.
+   */
+  public double getLeftTriggerAxis() {
+    return getRawAxis(Axis.kLeftTrigger.value);
+  }
+
+  /**
+   * Get the right trigger (RT) axis value of the controller. Note that this axis is bound to the
+   * range of [0, 1] as opposed to the usual [-1, 1].
+   *
+   * @return The axis value.
+   */
+  public double getRightTriggerAxis() {
+    return getRawAxis(Axis.kRightTrigger.value);
+  }
+
+  /**
+   * Read the value of the left bumper (LB) button on the controller.
+   *
    * @return The state of the button.
    */
-  public boolean getBumper(Hand hand) {
-    if (hand.equals(Hand.kLeft)) {
-      return getRawButton(Button.kBumperLeft.value);
-    } else {
-      return getRawButton(Button.kBumperRight.value);
-    }
+  public boolean getLeftBumper() {
+    return getRawButton(Button.kLeftBumper.value);
   }
 
   /**
-   * Whether the bumper was pressed since the last check.
+   * Read the value of the right bumper (RB) button on the controller.
    *
-   * @param hand Side of controller whose value should be returned.
-   * @return Whether the button was pressed since the last check.
-   */
-  public boolean getBumperPressed(Hand hand) {
-    if (hand == Hand.kLeft) {
-      return getRawButtonPressed(Button.kBumperLeft.value);
-    } else {
-      return getRawButtonPressed(Button.kBumperRight.value);
-    }
-  }
-
-  /**
-   * Whether the bumper was released since the last check.
-   *
-   * @param hand Side of controller whose value should be returned.
-   * @return Whether the button was released since the last check.
-   */
-  public boolean getBumperReleased(Hand hand) {
-    if (hand == Hand.kLeft) {
-      return getRawButtonReleased(Button.kBumperLeft.value);
-    } else {
-      return getRawButtonReleased(Button.kBumperRight.value);
-    }
-  }
-
-  /**
-   * Read the value of the stick button on the controller.
-   *
-   * @param hand Side of controller whose value should be returned.
    * @return The state of the button.
    */
-  public boolean getStickButton(Hand hand) {
-    if (hand.equals(Hand.kLeft)) {
-      return getRawButton(Button.kStickLeft.value);
-    } else {
-      return getRawButton(Button.kStickRight.value);
-    }
+  public boolean getRightBumper() {
+    return getRawButton(Button.kRightBumper.value);
   }
 
   /**
-   * Whether the stick button was pressed since the last check.
+   * Whether the left bumper (LB) was pressed since the last check.
    *
-   * @param hand Side of controller whose value should be returned.
    * @return Whether the button was pressed since the last check.
    */
-  public boolean getStickButtonPressed(Hand hand) {
-    if (hand == Hand.kLeft) {
-      return getRawButtonPressed(Button.kStickLeft.value);
-    } else {
-      return getRawButtonPressed(Button.kStickRight.value);
-    }
+  public boolean getLeftBumperPressed() {
+    return getRawButtonPressed(Button.kLeftBumper.value);
   }
 
   /**
-   * Whether the stick button was released since the last check.
+   * Whether the right bumper (RB) was pressed since the last check.
    *
-   * @param hand Side of controller whose value should be returned.
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getRightBumperPressed() {
+    return getRawButtonPressed(Button.kRightBumper.value);
+  }
+
+  /**
+   * Whether the left bumper (LB) was released since the last check.
+   *
    * @return Whether the button was released since the last check.
    */
-  public boolean getStickButtonReleased(Hand hand) {
-    if (hand == Hand.kLeft) {
-      return getRawButtonReleased(Button.kStickLeft.value);
-    } else {
-      return getRawButtonReleased(Button.kStickRight.value);
-    }
+  public boolean getLeftBumperReleased() {
+    return getRawButtonReleased(Button.kLeftBumper.value);
+  }
+
+  /**
+   * Whether the right bumper (RB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getRightBumperReleased() {
+    return getRawButtonReleased(Button.kRightBumper.value);
+  }
+
+  /**
+   * Read the value of the left stick button (LSB) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getLeftStickButton() {
+    return getRawButton(Button.kLeftStick.value);
+  }
+
+  /**
+   * Read the value of the right stick button (RSB) on the controller.
+   *
+   * @return The state of the button.
+   */
+  public boolean getRightStickButton() {
+    return getRawButton(Button.kRightStick.value);
+  }
+
+  /**
+   * Whether the left stick button (LSB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getLeftStickButtonPressed() {
+    return getRawButtonPressed(Button.kLeftStick.value);
+  }
+
+  /**
+   * Whether the right stick button (RSB) was pressed since the last check.
+   *
+   * @return Whether the button was pressed since the last check.
+   */
+  public boolean getRightStickButtonPressed() {
+    return getRawButtonPressed(Button.kRightStick.value);
+  }
+
+  /**
+   * Whether the left stick button (LSB) was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getLeftStickButtonReleased() {
+    return getRawButtonReleased(Button.kLeftStick.value);
+  }
+
+  /**
+   * Whether the right stick (RSB) button was released since the last check.
+   *
+   * @return Whether the button was released since the last check.
+   */
+  public boolean getRightStickButtonReleased() {
+    return getRawButtonReleased(Button.kRightStick.value);
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
deleted file mode 100644
index 0896ac6..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ControllerUtil.java
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-public final class ControllerUtil {
-  /**
-   * 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.
-   */
-  public static double getModulusError(double reference, double measurement, double minimumInput,
-      double maximumInput) {
-    double error = reference - measurement;
-    double modulus = maximumInput - minimumInput;
-
-    // Wrap error above maximum input
-    int numMax = (int) ((error + maximumInput) / modulus);
-    error -= numMax * modulus;
-
-    // Wrap error below minimum input
-    int numMin = (int) ((error + minimumInput) / modulus);
-    error -= numMin * modulus;
-
-    return error;
-  }
-
-  private ControllerUtil() {
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
deleted file mode 100644
index 4a98232..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/HolonomicDriveController.java
+++ /dev/null
@@ -1,134 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-
-/**
- * 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.
- *
- * <p>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.
- */
-@SuppressWarnings("MemberName")
-public class HolonomicDriveController {
-  private Pose2d m_poseError = new Pose2d();
-  private Pose2d m_poseTolerance = new Pose2d();
-  private boolean m_enabled = true;
-
-  private final PIDController m_xController;
-  private final PIDController m_yController;
-  private final ProfiledPIDController m_thetaController;
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  public HolonomicDriveController(PIDController xController,
-                                  PIDController yController,
-                                  ProfiledPIDController thetaController) {
-    m_xController = xController;
-    m_yController = yController;
-    m_thetaController = thetaController;
-  }
-
-  /**
-   * Returns true if the pose error is within tolerance of the reference.
-   *
-   * @return True if the pose error is within tolerance of the reference.
-   */
-  public boolean atReference() {
-    final var eTranslate = m_poseError.getTranslation();
-    final var eRotate = m_poseError.getRotation();
-    final var tolTranslate = m_poseTolerance.getTranslation();
-    final var tolRotate = m_poseTolerance.getRotation();
-    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
-        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
-        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
-  }
-
-  /**
-   * Sets the pose error which is considered tolerance for use with atReference().
-   *
-   * @param tolerance The pose error which is tolerable.
-   */
-  public void setTolerance(Pose2d tolerance) {
-    m_poseTolerance = tolerance;
-  }
-
-  /**
-   * Returns the next output of the holonomic drive controller.
-   *
-   * @param currentPose             The current pose.
-   * @param poseRef                 The desired pose.
-   * @param linearVelocityRefMeters The linear velocity reference.
-   * @param angleRef                The angular reference.
-   * @return The next output of the holonomic drive controller.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public ChassisSpeeds calculate(Pose2d currentPose,
-                                 Pose2d poseRef,
-                                 double linearVelocityRefMeters,
-                                 Rotation2d angleRef) {
-    // Calculate feedforward velocities (field-relative).
-    double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
-    double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
-    double thetaFF = m_thetaController.calculate(currentPose.getRotation().getRadians(),
-        angleRef.getRadians());
-
-    m_poseError = poseRef.relativeTo(currentPose);
-
-    if (!m_enabled) {
-      return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
-    }
-
-    // Calculate feedback velocities (based on position error).
-    double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
-    double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
-
-    // Return next output.
-    return ChassisSpeeds.fromFieldRelativeSpeeds(xFF + xFeedback,
-        yFF + yFeedback, thetaFF, currentPose.getRotation());
-  }
-
-  /**
-   * Returns the next output of the holonomic drive controller.
-   *
-   * @param currentPose  The current pose.
-   * @param desiredState The desired trajectory state.
-   * @param angleRef     The desired end-angle.
-   * @return The next output of the holonomic drive controller.
-   */
-  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState,
-                                 Rotation2d angleRef) {
-    return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond,
-        angleRef);
-  }
-
-  /**
-   * Enables and disables the controller for troubleshooting problems. When calculate()
-   * is called on a disabled controller, only feedforward values are returned.
-   *
-   * @param enabled If the controller is enabled or not.
-   */
-  public void setEnabled(boolean enabled) {
-    m_enabled = enabled;
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
deleted file mode 100644
index 526e91e..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/PIDController.java
+++ /dev/null
@@ -1,351 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
-
-/**
- * Implements a PID control loop.
- */
-@SuppressWarnings("PMD.TooManyFields")
-public class PIDController implements Sendable, AutoCloseable {
-  private static int instances;
-
-  // Factor for "proportional" control
-  @SuppressWarnings("MemberName")
-  private double m_Kp;
-
-  // Factor for "integral" control
-  @SuppressWarnings("MemberName")
-  private double m_Ki;
-
-  // Factor for "derivative" control
-  @SuppressWarnings("MemberName")
-  private double m_Kd;
-
-  // The period (in seconds) of the loop that calls the controller
-  private final double m_period;
-
-  private double m_maximumIntegral = 1.0;
-
-  private double m_minimumIntegral = -1.0;
-
-  private double m_maximumInput;
-
-  private double m_minimumInput;
-
-  // Do the endpoints wrap around? eg. Absolute encoder
-  private boolean m_continuous;
-
-  // The error at the time of the most recent call to calculate()
-  private double m_positionError;
-  private double m_velocityError;
-
-  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
-  private double m_prevError;
-
-  // The sum of the errors for use in the integral calc
-  private double m_totalError;
-
-  // The error that is considered at setpoint.
-  private double m_positionTolerance = 0.05;
-  private double m_velocityTolerance = Double.POSITIVE_INFINITY;
-
-  private double m_setpoint;
-
-  /**
-   * Allocates a PIDController with the given constants for Kp, Ki, and Kd and a default period of
-   * 0.02 seconds.
-   *
-   * @param Kp The proportional coefficient.
-   * @param Ki The integral coefficient.
-   * @param Kd The derivative coefficient.
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDController(double Kp, double Ki, double Kd) {
-    this(Kp, Ki, Kd, 0.02);
-  }
-
-  /**
-   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
-   *
-   * @param Kp     The proportional coefficient.
-   * @param Ki     The integral coefficient.
-   * @param Kd     The derivative coefficient.
-   * @param period The period between controller updates in seconds.
-   */
-  @SuppressWarnings("ParameterName")
-  public PIDController(double Kp, double Ki, double Kd, double period) {
-    m_Kp = Kp;
-    m_Ki = Ki;
-    m_Kd = Kd;
-
-    m_period = period;
-
-    instances++;
-    SendableRegistry.addLW(this, "PIDController", instances);
-
-    HAL.report(tResourceType.kResourceType_PIDController2, instances);
-  }
-
-  @Override
-  public void close() {
-    SendableRegistry.remove(this);
-  }
-
-  /**
-   * Sets the PID Controller gain parameters.
-   *
-   * <p>Set the proportional, integral, and differential coefficients.
-   *
-   * @param Kp The proportional coefficient.
-   * @param Ki The integral coefficient.
-   * @param Kd The derivative coefficient.
-   */
-  @SuppressWarnings("ParameterName")
-  public void setPID(double Kp, double Ki, double Kd) {
-    m_Kp = Kp;
-    m_Ki = Ki;
-    m_Kd = Kd;
-  }
-
-  /**
-   * Sets the Proportional coefficient of the PID controller gain.
-   *
-   * @param Kp proportional coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setP(double Kp) {
-    m_Kp = Kp;
-  }
-
-  /**
-   * Sets the Integral coefficient of the PID controller gain.
-   *
-   * @param Ki integral coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setI(double Ki) {
-    m_Ki = Ki;
-  }
-
-  /**
-   * Sets the Differential coefficient of the PID controller gain.
-   *
-   * @param Kd differential coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setD(double Kd) {
-    m_Kd = Kd;
-  }
-
-  /**
-   * Get the Proportional coefficient.
-   *
-   * @return proportional coefficient
-   */
-  public double getP() {
-    return m_Kp;
-  }
-
-  /**
-   * Get the Integral coefficient.
-   *
-   * @return integral coefficient
-   */
-  public double getI() {
-    return m_Ki;
-  }
-
-  /**
-   * Get the Differential coefficient.
-   *
-   * @return differential coefficient
-   */
-  public double getD() {
-    return m_Kd;
-  }
-
-  /**
-   * Returns the period of this controller.
-   *
-   * @return the period of the controller.
-   */
-  public double getPeriod() {
-    return m_period;
-  }
-
-  /**
-   * Sets the setpoint for the PIDController.
-   *
-   * @param setpoint The desired setpoint.
-   */
-  public void setSetpoint(double setpoint) {
-    m_setpoint = setpoint;
-  }
-
-  /**
-   * Returns the current setpoint of the PIDController.
-   *
-   * @return The current setpoint.
-   */
-  public double getSetpoint() {
-    return m_setpoint;
-  }
-
-  /**
-   * Returns true if the error is within the tolerance of the setpoint.
-   *
-   * <p>This will return false until at least one input value has been computed.
-   *
-   * @return Whether the error is within the acceptable bounds.
-   */
-  public boolean atSetpoint() {
-    return Math.abs(m_positionError) < m_positionTolerance
-        && Math.abs(m_velocityError) < m_velocityTolerance;
-  }
-
-  /**
-   * Enables continuous input.
-   *
-   * <p>Rather then using the max and min input range as constraints, it considers
-   * them to be the same point and automatically calculates the shortest route
-   * to the setpoint.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  public void enableContinuousInput(double minimumInput, double maximumInput) {
-    m_continuous = true;
-    m_minimumInput = minimumInput;
-    m_maximumInput = maximumInput;
-  }
-
-  /**
-   * Disables continuous input.
-   */
-  public void disableContinuousInput() {
-    m_continuous = false;
-  }
-
-  /**
-   * Returns true if continuous input is enabled.
-   */
-  public boolean isContinuousInputEnabled() {
-    return m_continuous;
-  }
-
-  /**
-   * Sets the minimum and maximum values for the integrator.
-   *
-   * <p>When the cap is reached, the integrator value is added to the controller
-   * output rather than the integrator value times the integral gain.
-   *
-   * @param minimumIntegral The minimum value of the integrator.
-   * @param maximumIntegral The maximum value of the integrator.
-   */
-  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
-    m_minimumIntegral = minimumIntegral;
-    m_maximumIntegral = maximumIntegral;
-  }
-
-  /**
-   * Sets the error which is considered tolerable for use with atSetpoint().
-   *
-   * @param positionTolerance Position error which is tolerable.
-   */
-  public void setTolerance(double positionTolerance) {
-    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
-  }
-
-  /**
-   * Sets the error which is considered tolerable for use with atSetpoint().
-   *
-   * @param positionTolerance Position error which is tolerable.
-   * @param velocityTolerance Velocity error which is tolerable.
-   */
-  public void setTolerance(double positionTolerance, double velocityTolerance) {
-    m_positionTolerance = positionTolerance;
-    m_velocityTolerance = velocityTolerance;
-  }
-
-  /**
-   * Returns the difference between the setpoint and the measurement.
-   *
-   * @return The error.
-   */
-  public double getPositionError() {
-    return m_positionError;
-  }
-
-  /**
-   * Returns the velocity error.
-   */
-  public double getVelocityError() {
-    return m_velocityError;
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param setpoint    The new setpoint of the controller.
-   */
-  public double calculate(double measurement, double setpoint) {
-    // Set setpoint to provided value
-    setSetpoint(setpoint);
-    return calculate(measurement);
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   */
-  public double calculate(double measurement) {
-    m_prevError = m_positionError;
-
-    if (m_continuous) {
-      m_positionError = ControllerUtil.getModulusError(m_setpoint, measurement, m_minimumInput,
-          m_maximumInput);
-    } else {
-      m_positionError = m_setpoint - measurement;
-    }
-
-    m_velocityError = (m_positionError - m_prevError) / m_period;
-
-    if (m_Ki != 0) {
-      m_totalError = MathUtil.clamp(m_totalError + m_positionError * m_period,
-          m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
-    }
-
-    return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
-  }
-
-  /**
-   * Resets the previous error and the integral term.
-   */
-  public void reset() {
-    m_prevError = 0;
-    m_totalError = 0;
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("PIDController");
-    builder.addDoubleProperty("p", this::getP, this::setP);
-    builder.addDoubleProperty("i", this::getI, this::setI);
-    builder.addDoubleProperty("d", this::getD, this::setD);
-    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
deleted file mode 100644
index 3955c5b..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/ProfiledPIDController.java
+++ /dev/null
@@ -1,378 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-/**
- * Implements a PID control loop whose setpoint is constrained by a trapezoid
- * profile. Users should call reset() when they first start running the controller
- * to avoid unwanted behavior.
- */
-public class ProfiledPIDController implements Sendable {
-  private static int instances;
-
-  private PIDController m_controller;
-  private double m_minimumInput;
-  private double m_maximumInput;
-  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
-  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
-  private TrapezoidProfile.Constraints m_constraints;
-
-  /**
-   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
-   * Kd.
-   *
-   * @param Kp          The proportional coefficient.
-   * @param Ki          The integral coefficient.
-   * @param Kd          The derivative coefficient.
-   * @param constraints Velocity and acceleration constraints for goal.
-   */
-  @SuppressWarnings("ParameterName")
-  public ProfiledPIDController(double Kp, double Ki, double Kd,
-                        TrapezoidProfile.Constraints constraints) {
-    this(Kp, Ki, Kd, constraints, 0.02);
-  }
-
-  /**
-   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
-   * Kd.
-   *
-   * @param Kp          The proportional coefficient.
-   * @param Ki          The integral coefficient.
-   * @param Kd          The derivative coefficient.
-   * @param constraints Velocity and acceleration constraints for goal.
-   * @param period      The period between controller updates in seconds. The
-   *                    default is 0.02 seconds.
-   */
-  @SuppressWarnings("ParameterName")
-  public ProfiledPIDController(double Kp, double Ki, double Kd,
-                        TrapezoidProfile.Constraints constraints,
-                        double period) {
-    m_controller = new PIDController(Kp, Ki, Kd, period);
-    m_constraints = constraints;
-    instances++;
-    HAL.report(tResourceType.kResourceType_ProfiledPIDController, instances);
-  }
-
-  /**
-   * Sets the PID Controller gain parameters.
-   *
-   * <p>Sets the proportional, integral, and differential coefficients.
-   *
-   * @param Kp Proportional coefficient
-   * @param Ki Integral coefficient
-   * @param Kd Differential coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setPID(double Kp, double Ki, double Kd) {
-    m_controller.setPID(Kp, Ki, Kd);
-  }
-
-  /**
-   * Sets the proportional coefficient of the PID controller gain.
-   *
-   * @param Kp proportional coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setP(double Kp) {
-    m_controller.setP(Kp);
-  }
-
-  /**
-   * Sets the integral coefficient of the PID controller gain.
-   *
-   * @param Ki integral coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setI(double Ki) {
-    m_controller.setI(Ki);
-  }
-
-  /**
-   * Sets the differential coefficient of the PID controller gain.
-   *
-   * @param Kd differential coefficient
-   */
-  @SuppressWarnings("ParameterName")
-  public void setD(double Kd) {
-    m_controller.setD(Kd);
-  }
-
-  /**
-   * Gets the proportional coefficient.
-   *
-   * @return proportional coefficient
-   */
-  public double getP() {
-    return m_controller.getP();
-  }
-
-  /**
-   * Gets the integral coefficient.
-   *
-   * @return integral coefficient
-   */
-  public double getI() {
-    return m_controller.getI();
-  }
-
-  /**
-   * Gets the differential coefficient.
-   *
-   * @return differential coefficient
-   */
-  public double getD() {
-    return m_controller.getD();
-  }
-
-  /**
-   * Gets the period of this controller.
-   *
-   * @return The period of the controller.
-   */
-  public double getPeriod() {
-    return m_controller.getPeriod();
-  }
-
-  /**
-   * Sets the goal for the ProfiledPIDController.
-   *
-   * @param goal The desired goal state.
-   */
-  public void setGoal(TrapezoidProfile.State goal) {
-    m_goal = goal;
-  }
-
-  /**
-   * Sets the goal for the ProfiledPIDController.
-   *
-   * @param goal The desired goal position.
-   */
-  public void setGoal(double goal) {
-    m_goal = new TrapezoidProfile.State(goal, 0);
-  }
-
-  /**
-   * Gets the goal for the ProfiledPIDController.
-   */
-  public TrapezoidProfile.State getGoal() {
-    return m_goal;
-  }
-
-  /**
-   * Returns true if the error is within the tolerance of the error.
-   *
-   * <p>This will return false until at least one input value has been computed.
-   */
-  public boolean atGoal() {
-    return atSetpoint() && m_goal.equals(m_setpoint);
-  }
-
-  /**
-   * Set velocity and acceleration constraints for goal.
-   *
-   * @param constraints Velocity and acceleration constraints for goal.
-   */
-  public void setConstraints(TrapezoidProfile.Constraints constraints) {
-    m_constraints = constraints;
-  }
-
-  /**
-   * Returns the current setpoint of the ProfiledPIDController.
-   *
-   * @return The current setpoint.
-   */
-  public TrapezoidProfile.State getSetpoint() {
-    return m_setpoint;
-  }
-
-  /**
-   * Returns true if the error is within the tolerance of the error.
-   *
-   * <p>This will return false until at least one input value has been computed.
-   */
-  public boolean atSetpoint() {
-    return m_controller.atSetpoint();
-  }
-
-  /**
-   * Enables continuous input.
-   *
-   * <p>Rather then using the max and min input range as constraints, it considers
-   * them to be the same point and automatically calculates the shortest route
-   * to the setpoint.
-   *
-   * @param minimumInput The minimum value expected from the input.
-   * @param maximumInput The maximum value expected from the input.
-   */
-  public void enableContinuousInput(double minimumInput, double maximumInput) {
-    m_controller.enableContinuousInput(minimumInput, maximumInput);
-    m_minimumInput = minimumInput;
-    m_maximumInput = maximumInput;
-  }
-
-  /**
-   * Disables continuous input.
-   */
-  public void disableContinuousInput() {
-    m_controller.disableContinuousInput();
-  }
-
-  /**
-   * Sets the minimum and maximum values for the integrator.
-   *
-   * <p>When the cap is reached, the integrator value is added to the controller
-   * output rather than the integrator value times the integral gain.
-   *
-   * @param minimumIntegral The minimum value of the integrator.
-   * @param maximumIntegral The maximum value of the integrator.
-   */
-  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
-    m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
-  }
-
-  /**
-   * Sets the error which is considered tolerable for use with atSetpoint().
-   *
-   * @param positionTolerance Position error which is tolerable.
-   */
-  public void setTolerance(double positionTolerance) {
-    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
-  }
-
-  /**
-   * Sets the error which is considered tolerable for use with atSetpoint().
-   *
-   * @param positionTolerance Position error which is tolerable.
-   * @param velocityTolerance Velocity error which is tolerable.
-   */
-  public void setTolerance(double positionTolerance, double velocityTolerance) {
-    m_controller.setTolerance(positionTolerance, velocityTolerance);
-  }
-
-  /**
-   * Returns the difference between the setpoint and the measurement.
-   *
-   * @return The error.
-   */
-  public double getPositionError() {
-    return m_controller.getPositionError();
-  }
-
-  /**
-   * Returns the change in error per second.
-   */
-  public double getVelocityError() {
-    return m_controller.getVelocityError();
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   */
-  public double calculate(double measurement) {
-    if (m_controller.isContinuousInputEnabled()) {
-      // Get error which is smallest distance between goal and measurement
-      double goalMinDistance = ControllerUtil.getModulusError(m_goal.position, measurement,
-          m_minimumInput, m_maximumInput);
-      double setpointMinDistance = ControllerUtil.getModulusError(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;
-    }
-
-    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
-    m_setpoint = profile.calculate(getPeriod());
-    return m_controller.calculate(measurement, m_setpoint.position);
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param goal The new goal of the controller.
-   */
-  public double calculate(double measurement, TrapezoidProfile.State goal) {
-    setGoal(goal);
-    return calculate(measurement);
-  }
-
-  /**
-   * Returns the next output of the PIDController.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param goal The new goal of the controller.
-   */
-  public double calculate(double measurement, double goal) {
-    setGoal(goal);
-    return calculate(measurement);
-  }
-
-  /**
-   * Returns the next output of the PID controller.
-   *
-   * @param measurement The current measurement of the process variable.
-   * @param goal        The new goal of the controller.
-   * @param constraints Velocity and acceleration constraints for goal.
-   */
-  public double calculate(double measurement, TrapezoidProfile.State goal,
-                   TrapezoidProfile.Constraints constraints) {
-    setConstraints(constraints);
-    return calculate(measurement, goal);
-  }
-
-  /**
-   * Reset the previous error and the integral term.
-   *
-   * @param measurement The current measured State of the system.
-   */
-  public void reset(TrapezoidProfile.State measurement) {
-    m_controller.reset();
-    m_setpoint = measurement;
-  }
-
-  /**
-   * Reset the previous error and the integral term.
-   *
-   * @param measuredPosition The current measured position of the system.
-   * @param measuredVelocity The current measured velocity of the system.
-   */
-  public void reset(double measuredPosition, double measuredVelocity) {
-    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
-  }
-
-  /**
-   * Reset the previous error and the integral term.
-   *
-   * @param measuredPosition The current measured position of the system. The velocity is
-   *     assumed to be zero.
-   */
-  public void reset(double measuredPosition) {
-    reset(measuredPosition, 0.0);
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    builder.setSmartDashboardType("ProfiledPIDController");
-    builder.addDoubleProperty("p", this::getP, this::setP);
-    builder.addDoubleProperty("i", this::getI, this::setI);
-    builder.addDoubleProperty("d", this::getD, this::setD);
-    builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
deleted file mode 100644
index b597352..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/controller/RamseteController.java
+++ /dev/null
@@ -1,173 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-
-/**
- * Ramsete is a nonlinear time-varying feedback controller for unicycle models
- * that drives the model to a desired pose along a two-dimensional trajectory.
- * Why would we need a nonlinear control law in addition to the linear ones we
- * have used so far like PID? If we use the original approach with PID
- * controllers for left and right position and velocity states, the controllers
- * only deal with the local pose. If the robot deviates from the path, there is
- * no way for the controllers to correct and the robot may not reach the desired
- * global pose. This is due to multiple endpoints existing for the robot which
- * have the same encoder path arc lengths.
- *
- * <p>Instead of using wheel path arc lengths (which are in the robot's local
- * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
- * global pose. The controller uses this extra information to guide a linear
- * reference tracker like the PID controllers back in by adjusting the
- * references of the PID controllers.
- *
- * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
- * describes a nonlinear controller for a wheeled vehicle with unicycle-like
- * kinematics; a global pose consisting of x, y, and theta; and a desired pose
- * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
- * acronym for the title of the book it came from in Italian ("Robotica
- * Articolata e Mobile per i SErvizi e le TEcnologie").
- *
- * <p>See
- * <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">
- * Controls Engineering in the FIRST Robotics Competition</a> section on Ramsete
- * unicycle controller for a derivation and analysis.
- */
-public class RamseteController {
-  @SuppressWarnings("MemberName")
-  private final double m_b;
-  @SuppressWarnings("MemberName")
-  private final double m_zeta;
-
-  private Pose2d m_poseError = new Pose2d();
-  private Pose2d m_poseTolerance = new Pose2d();
-  private boolean m_enabled = true;
-
-  /**
-   * Construct a Ramsete unicycle controller.
-   *
-   * @param b    Tuning parameter (b &gt; 0) for which larger values make convergence more
-   *             aggressive like a proportional term.
-   * @param zeta Tuning parameter (0 &lt; zeta &lt; 1) for which larger values provide more damping
-   *             in response.
-   */
-  @SuppressWarnings("ParameterName")
-  public RamseteController(double b, double zeta) {
-    m_b = b;
-    m_zeta = zeta;
-  }
-
-  /**
-   * Construct a Ramsete unicycle controller. The default arguments for
-   * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
-   * results.
-   */
-  public RamseteController() {
-    this(2.0, 0.7);
-  }
-
-  /**
-   * Returns true if the pose error is within tolerance of the reference.
-   */
-  public boolean atReference() {
-    final var eTranslate = m_poseError.getTranslation();
-    final var eRotate = m_poseError.getRotation();
-    final var tolTranslate = m_poseTolerance.getTranslation();
-    final var tolRotate = m_poseTolerance.getRotation();
-    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
-           && Math.abs(eTranslate.getY()) < tolTranslate.getY()
-           && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
-  }
-
-  /**
-   * Sets the pose error which is considered tolerable for use with
-   * atReference().
-   *
-   * @param poseTolerance Pose error which is tolerable.
-   */
-  public void setTolerance(Pose2d poseTolerance) {
-    m_poseTolerance = poseTolerance;
-  }
-
-  /**
-   * Returns the next output of the Ramsete controller.
-   *
-   * <p>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 linearVelocityRefMeters            The desired linear velocity in meters per second.
-   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public ChassisSpeeds calculate(Pose2d currentPose,
-                                 Pose2d poseRef,
-                                 double linearVelocityRefMeters,
-                                 double angularVelocityRefRadiansPerSecond) {
-    if (!m_enabled) {
-      return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
-    }
-
-    m_poseError = poseRef.relativeTo(currentPose);
-
-    // Aliases for equation readability
-    final double eX = m_poseError.getX();
-    final double eY = m_poseError.getY();
-    final double eTheta = m_poseError.getRotation().getRadians();
-    final double vRef = linearVelocityRefMeters;
-    final double omegaRef = angularVelocityRefRadiansPerSecond;
-
-    double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
-
-    return new ChassisSpeeds(vRef * m_poseError.getRotation().getCos() + k * eX,
-                             0.0,
-                             omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
-  }
-
-  /**
-   * Returns the next output of the Ramsete controller.
-   *
-   * <p>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.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
-    return calculate(currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond,
-        desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
-  }
-
-  /**
-   * Enables and disables the controller for troubleshooting purposes.
-   *
-   * @param enabled If the controller is enabled or not.
-   */
-  public void setEnabled(boolean enabled) {
-    m_enabled = enabled;
-  }
-
-  /**
-   * Returns sin(x) / x.
-   *
-   * @param x Value of which to take sinc(x).
-   */
-  @SuppressWarnings("ParameterName")
-  private static double sinc(double x) {
-    if (Math.abs(x) < 1e-9) {
-      return 1.0 - 1.0 / 6.0 * x * x;
-    } else {
-      return Math.sin(x) / x;
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
index 88a6ff9..57fb365 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/DifferentialDrive.java
@@ -1,60 +1,58 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.drive;
 
-import java.util.StringJoiner;
+import static java.util.Objects.requireNonNull;
 
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 
 /**
  * A class for driving differential drive/skid-steer drive platforms such as the Kit of Parts drive
  * base, "tank drive", or West Coast Drive.
  *
  * <p>These drive bases typically have drop-center / skid-steer with two or more wheels per side
- * (e.g., 6WD or 8WD). This class takes a SpeedController per side. For four and
- * six motor drivetrains, construct and pass in {@link edu.wpi.first.wpilibj.SpeedControllerGroup}
- * instances as follows.
+ * (e.g., 6WD or 8WD). This class takes a MotorController per side. For four and six motor
+ * drivetrains, construct and pass in {@link
+ * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup} instances as follows.
  *
  * <p>Four motor drivetrain:
+ *
  * <pre><code>
  * public class Robot {
- *   SpeedController m_frontLeft = new PWMVictorSPX(1);
- *   SpeedController m_rearLeft = new PWMVictorSPX(2);
- *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_rearLeft);
+ *   MotorController m_frontLeft = new PWMVictorSPX(1);
+ *   MotorController m_rearLeft = new PWMVictorSPX(2);
+ *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_rearLeft);
  *
- *   SpeedController m_frontRight = new PWMVictorSPX(3);
- *   SpeedController m_rearRight = new PWMVictorSPX(4);
- *   SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_rearRight);
+ *   MotorController m_frontRight = new PWMVictorSPX(3);
+ *   MotorController m_rearRight = new PWMVictorSPX(4);
+ *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_rearRight);
  *
  *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
  * }
  * </code></pre>
  *
  * <p>Six motor drivetrain:
+ *
  * <pre><code>
  * public class Robot {
- *   SpeedController m_frontLeft = new PWMVictorSPX(1);
- *   SpeedController m_midLeft = new PWMVictorSPX(2);
- *   SpeedController m_rearLeft = new PWMVictorSPX(3);
- *   SpeedControllerGroup m_left = new SpeedControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
+ *   MotorController m_frontLeft = new PWMVictorSPX(1);
+ *   MotorController m_midLeft = new PWMVictorSPX(2);
+ *   MotorController m_rearLeft = new PWMVictorSPX(3);
+ *   MotorControllerGroup m_left = new MotorControllerGroup(m_frontLeft, m_midLeft, m_rearLeft);
  *
- *   SpeedController m_frontRight = new PWMVictorSPX(4);
- *   SpeedController m_midRight = new PWMVictorSPX(5);
- *   SpeedController m_rearRight = new PWMVictorSPX(6);
- *   SpeedControllerGroup m_right = new SpeedControllerGroup(m_frontRight, m_midRight, m_rearRight);
+ *   MotorController m_frontRight = new PWMVictorSPX(4);
+ *   MotorController m_midRight = new PWMVictorSPX(5);
+ *   MotorController m_rearRight = new PWMVictorSPX(6);
+ *   MotorControllerGroup m_right = new MotorControllerGroup(m_frontRight, m_midRight, m_rearRight);
  *
  *   DifferentialDrive m_drive = new DifferentialDrive(m_left, m_right);
  * }
@@ -63,6 +61,7 @@
  * <p>A differential drive robot has left and right wheels separated by an arbitrary width.
  *
  * <p>Drive base diagram:
+ *
  * <pre>
  * |_______|
  * | |   | |
@@ -83,42 +82,52 @@
  * positive.
  *
  * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
- * be set to 0, and larger values will be scaled so that the full range is still used. This
- * deadband value can be changed with {@link #setDeadband}.
- *
- * <p>RobotDrive porting guide:
- * <br>{@link #tankDrive(double, double)} is equivalent to
- * {@link edu.wpi.first.wpilibj.RobotDrive#tankDrive(double, double)} if a deadband of 0 is used.
- * <br>{@link #arcadeDrive(double, double)} is equivalent to
- * {@link edu.wpi.first.wpilibj.RobotDrive#arcadeDrive(double, double)} if a deadband of 0 is used
- * and the the rotation input is inverted eg arcadeDrive(y, -rotation)
- * <br>{@link #curvatureDrive(double, double, boolean)} is similar in concept to
- * {@link edu.wpi.first.wpilibj.RobotDrive#drive(double, double)} with the addition of a quick turn
- * mode. However, it is not designed to give exactly the same response.
+ * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
+ * value can be changed with {@link #setDeadband}.
  */
+@SuppressWarnings("removal")
 public class DifferentialDrive extends RobotDriveBase implements Sendable, AutoCloseable {
-  public static final double kDefaultQuickStopThreshold = 0.2;
-  public static final double kDefaultQuickStopAlpha = 0.1;
-
   private static int instances;
 
   private final SpeedController m_leftMotor;
   private final SpeedController m_rightMotor;
 
-  private double m_quickStopThreshold = kDefaultQuickStopThreshold;
-  private double m_quickStopAlpha = kDefaultQuickStopAlpha;
-  private double m_quickStopAccumulator;
-  private double m_rightSideInvertMultiplier = -1.0;
   private boolean m_reported;
 
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    public double left;
+    public double right;
+
+    /** Constructs a WheelSpeeds with zeroes for left and right speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param left The left speed.
+     * @param right The right speed.
+     */
+    public WheelSpeeds(double left, double right) {
+      this.left = left;
+      this.right = right;
+    }
+  }
+
   /**
    * Construct a DifferentialDrive.
    *
-   * <p>To pass multiple motors per side, use a {@link SpeedControllerGroup}. If a motor needs to be
-   * inverted, do so before passing it in.
+   * <p>To pass multiple motors per side, use a {@link
+   * edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}. If a motor needs to be inverted, do
+   * so before passing it in.
+   *
+   * @param leftMotor Left motor.
+   * @param rightMotor Right motor.
    */
   public DifferentialDrive(SpeedController leftMotor, SpeedController rightMotor) {
-    verify(leftMotor, rightMotor);
+    requireNonNull(leftMotor, "Left motor cannot be null");
+    requireNonNull(rightMotor, "Right motor cannot be null");
+
     m_leftMotor = leftMotor;
     m_rightMotor = rightMotor;
     SendableRegistry.addChild(this, m_leftMotor);
@@ -133,35 +142,12 @@
   }
 
   /**
-   * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
-   * The exception's error message will specify all null motors, e.g. {@code
-   * NullPointerException("leftMotor, rightMotor")}, to give as much information as possible to
-   * the programmer.
+   * Arcade drive method for differential drive platform. The calculated values will be squared to
+   * decrease sensitivity at low speeds.
    *
-   * @throws NullPointerException if any of the given motors are null
-   */
-  @SuppressWarnings("PMD.AvoidThrowingNullPointerException")
-  private void verify(SpeedController leftMotor, SpeedController rightMotor) {
-    if (leftMotor != null && rightMotor != null) {
-      return;
-    }
-    StringJoiner joiner = new StringJoiner(", ");
-    if (leftMotor == null) {
-      joiner.add("leftMotor");
-    }
-    if (rightMotor == null) {
-      joiner.add("rightMotor");
-    }
-    throw new NullPointerException(joiner.toString());
-  }
-
-  /**
-   * Arcade drive method for differential drive platform.
-   * The calculated values will be squared to decrease sensitivity at low speeds.
-   *
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
+   *     positive.
    */
   @SuppressWarnings("ParameterName")
   public void arcadeDrive(double xSpeed, double zRotation) {
@@ -171,60 +157,26 @@
   /**
    * Arcade drive method for differential drive platform.
    *
-   * @param xSpeed        The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
-   * @param zRotation     The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                      positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
    * @param squareInputs If set, decreases the input sensitivity at low speeds.
    */
   @SuppressWarnings("ParameterName")
   public void arcadeDrive(double xSpeed, double zRotation, boolean squareInputs) {
     if (!m_reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive,
-                 tInstances.kRobotDrive2_DifferentialArcade, 2);
+      HAL.report(
+          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialArcade, 2);
       m_reported = true;
     }
 
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
-    xSpeed = applyDeadband(xSpeed, m_deadband);
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
+    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
 
-    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
-    zRotation = applyDeadband(zRotation, m_deadband);
+    var speeds = arcadeDriveIK(xSpeed, zRotation, squareInputs);
 
-    // Square the inputs (while preserving the sign) to increase fine control
-    // while permitting full power.
-    if (squareInputs) {
-      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
-      zRotation = Math.copySign(zRotation * zRotation, zRotation);
-    }
-
-    double leftMotorOutput;
-    double rightMotorOutput;
-
-    double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
-
-    if (xSpeed >= 0.0) {
-      // First quadrant, else second quadrant
-      if (zRotation >= 0.0) {
-        leftMotorOutput = maxInput;
-        rightMotorOutput = xSpeed - zRotation;
-      } else {
-        leftMotorOutput = xSpeed + zRotation;
-        rightMotorOutput = maxInput;
-      }
-    } else {
-      // Third quadrant, else fourth quadrant
-      if (zRotation >= 0.0) {
-        leftMotorOutput = xSpeed + zRotation;
-        rightMotorOutput = maxInput;
-      } else {
-        leftMotorOutput = maxInput;
-        rightMotorOutput = xSpeed - zRotation;
-      }
-    }
-
-    m_leftMotor.set(MathUtil.clamp(leftMotorOutput, -1.0, 1.0) * m_maxOutput);
-    double maxOutput = m_maxOutput * m_rightSideInvertMultiplier;
-    m_rightMotor.set(MathUtil.clamp(rightMotorOutput, -1.0, 1.0) * maxOutput);
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
 
     feed();
   }
@@ -233,94 +185,40 @@
    * Curvature drive method for differential drive platform.
    *
    * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
-   * heading change. This makes the robot more controllable at high speeds. Also handles the
-   * robot's quick turn functionality - "quick turn" overrides constant-curvature turning for
-   * turn-in-place maneuvers.
+   * heading change. This makes the robot more controllable at high speeds.
    *
-   * @param xSpeed      The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
-   * @param zRotation   The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                    positive.
-   * @param isQuickTurn If set, overrides constant-curvature turning for
-   *                    turn-in-place maneuvers.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
+   *     maneuvers.
    */
-  @SuppressWarnings({"ParameterName", "PMD.CyclomaticComplexity"})
-  public void curvatureDrive(double xSpeed, double zRotation, boolean isQuickTurn) {
+  @SuppressWarnings("ParameterName")
+  public void curvatureDrive(double xSpeed, double zRotation, boolean allowTurnInPlace) {
     if (!m_reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive,
-                 tInstances.kRobotDrive2_DifferentialCurvature, 2);
+      HAL.report(
+          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialCurvature, 2);
       m_reported = true;
     }
 
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
-    xSpeed = applyDeadband(xSpeed, m_deadband);
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
+    zRotation = MathUtil.applyDeadband(zRotation, m_deadband);
 
-    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
-    zRotation = applyDeadband(zRotation, m_deadband);
+    var speeds = curvatureDriveIK(xSpeed, zRotation, allowTurnInPlace);
 
-    double angularPower;
-    boolean overPower;
-
-    if (isQuickTurn) {
-      if (Math.abs(xSpeed) < m_quickStopThreshold) {
-        m_quickStopAccumulator = (1 - m_quickStopAlpha) * m_quickStopAccumulator
-            + m_quickStopAlpha * MathUtil.clamp(zRotation, -1.0, 1.0) * 2;
-      }
-      overPower = true;
-      angularPower = zRotation;
-    } else {
-      overPower = false;
-      angularPower = Math.abs(xSpeed) * zRotation - m_quickStopAccumulator;
-
-      if (m_quickStopAccumulator > 1) {
-        m_quickStopAccumulator -= 1;
-      } else if (m_quickStopAccumulator < -1) {
-        m_quickStopAccumulator += 1;
-      } else {
-        m_quickStopAccumulator = 0.0;
-      }
-    }
-
-    double leftMotorOutput = xSpeed + angularPower;
-    double rightMotorOutput = xSpeed - angularPower;
-
-    // If rotation is overpowered, reduce both outputs to within acceptable range
-    if (overPower) {
-      if (leftMotorOutput > 1.0) {
-        rightMotorOutput -= leftMotorOutput - 1.0;
-        leftMotorOutput = 1.0;
-      } else if (rightMotorOutput > 1.0) {
-        leftMotorOutput -= rightMotorOutput - 1.0;
-        rightMotorOutput = 1.0;
-      } else if (leftMotorOutput < -1.0) {
-        rightMotorOutput -= leftMotorOutput + 1.0;
-        leftMotorOutput = -1.0;
-      } else if (rightMotorOutput < -1.0) {
-        leftMotorOutput -= rightMotorOutput + 1.0;
-        rightMotorOutput = -1.0;
-      }
-    }
-
-    // Normalize the wheel speeds
-    double maxMagnitude = Math.max(Math.abs(leftMotorOutput), Math.abs(rightMotorOutput));
-    if (maxMagnitude > 1.0) {
-      leftMotorOutput /= maxMagnitude;
-      rightMotorOutput /= maxMagnitude;
-    }
-
-    m_leftMotor.set(leftMotorOutput * m_maxOutput);
-    m_rightMotor.set(rightMotorOutput * m_maxOutput * m_rightSideInvertMultiplier);
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
 
     feed();
   }
 
   /**
-   * Tank drive method for differential drive platform.
-   * The calculated values will be squared to decrease sensitivity at low speeds.
+   * Tank drive method for differential drive platform. The calculated values will be squared to
+   * decrease sensitivity at low speeds.
    *
-   * @param leftSpeed  The robot's left side speed along the X axis [-1.0..1.0]. Forward is
-   *                   positive.
+   * @param leftSpeed The robot's left side speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param rightSpeed The robot's right side speed along the X axis [-1.0..1.0]. Forward is
-   *                   positive.
+   *     positive.
    */
   public void tankDrive(double leftSpeed, double rightSpeed) {
     tankDrive(leftSpeed, rightSpeed, true);
@@ -329,24 +227,137 @@
   /**
    * Tank drive method for differential drive platform.
    *
-   * @param leftSpeed     The robot left side's speed along the X axis [-1.0..1.0]. Forward is
-   *                      positive.
-   * @param rightSpeed    The robot right side's speed along the X axis [-1.0..1.0]. Forward is
-   *                      positive.
+   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+   *     positive.
    * @param squareInputs If set, decreases the input sensitivity at low speeds.
    */
   public void tankDrive(double leftSpeed, double rightSpeed, boolean squareInputs) {
     if (!m_reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive,
-                 tInstances.kRobotDrive2_DifferentialTank, 2);
+      HAL.report(
+          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_DifferentialTank, 2);
       m_reported = true;
     }
 
-    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
-    leftSpeed = applyDeadband(leftSpeed, m_deadband);
+    leftSpeed = MathUtil.applyDeadband(leftSpeed, m_deadband);
+    rightSpeed = MathUtil.applyDeadband(rightSpeed, m_deadband);
 
+    var speeds = tankDriveIK(leftSpeed, rightSpeed, squareInputs);
+
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
+
+    feed();
+  }
+
+  /**
+   * Arcade drive inverse kinematics for differential drive platform.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds.
+   */
+  @SuppressWarnings("ParameterName")
+  public static WheelSpeeds arcadeDriveIK(double xSpeed, double zRotation, boolean squareInputs) {
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+
+    // Square the inputs (while preserving the sign) to increase fine control
+    // while permitting full power.
+    if (squareInputs) {
+      xSpeed = Math.copySign(xSpeed * xSpeed, xSpeed);
+      zRotation = Math.copySign(zRotation * zRotation, zRotation);
+    }
+
+    double leftSpeed;
+    double rightSpeed;
+
+    double maxInput = Math.copySign(Math.max(Math.abs(xSpeed), Math.abs(zRotation)), xSpeed);
+
+    if (xSpeed >= 0.0) {
+      // First quadrant, else second quadrant
+      if (zRotation >= 0.0) {
+        leftSpeed = maxInput;
+        rightSpeed = xSpeed - zRotation;
+      } else {
+        leftSpeed = xSpeed + zRotation;
+        rightSpeed = maxInput;
+      }
+    } else {
+      // Third quadrant, else fourth quadrant
+      if (zRotation >= 0.0) {
+        leftSpeed = xSpeed + zRotation;
+        rightSpeed = maxInput;
+      } else {
+        leftSpeed = maxInput;
+        rightSpeed = xSpeed - zRotation;
+      }
+    }
+
+    // Normalize the wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+    if (maxMagnitude > 1.0) {
+      leftSpeed /= maxMagnitude;
+      rightSpeed /= maxMagnitude;
+    }
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  /**
+   * Curvature drive inverse kinematics for differential drive platform.
+   *
+   * <p>The rotation argument controls the curvature of the robot's path rather than its rate of
+   * heading change. This makes the robot more controllable at high speeds.
+   *
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param allowTurnInPlace If set, overrides constant-curvature turning for turn-in-place
+   *     maneuvers.
+   * @return Wheel speeds.
+   */
+  @SuppressWarnings("ParameterName")
+  public static WheelSpeeds curvatureDriveIK(
+      double xSpeed, double zRotation, boolean allowTurnInPlace) {
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
+    zRotation = MathUtil.clamp(zRotation, -1.0, 1.0);
+
+    double leftSpeed = 0.0;
+    double rightSpeed = 0.0;
+
+    if (allowTurnInPlace) {
+      leftSpeed = xSpeed + zRotation;
+      rightSpeed = xSpeed - zRotation;
+    } else {
+      leftSpeed = xSpeed + Math.abs(xSpeed) * zRotation;
+      rightSpeed = xSpeed - Math.abs(xSpeed) * zRotation;
+    }
+
+    // Normalize wheel speeds
+    double maxMagnitude = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+    if (maxMagnitude > 1.0) {
+      leftSpeed /= maxMagnitude;
+      rightSpeed /= maxMagnitude;
+    }
+
+    return new WheelSpeeds(leftSpeed, rightSpeed);
+  }
+
+  /**
+   * Tank drive inverse kinematics for differential drive platform.
+   *
+   * @param leftSpeed The robot left side's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param rightSpeed The robot right side's speed along the X axis [-1.0..1.0]. Forward is
+   *     positive.
+   * @param squareInputs If set, decreases the input sensitivity at low speeds.
+   * @return Wheel speeds.
+   */
+  public static WheelSpeeds tankDriveIK(double leftSpeed, double rightSpeed, boolean squareInputs) {
+    leftSpeed = MathUtil.clamp(leftSpeed, -1.0, 1.0);
     rightSpeed = MathUtil.clamp(rightSpeed, -1.0, 1.0);
-    rightSpeed = applyDeadband(rightSpeed, m_deadband);
 
     // Square the inputs (while preserving the sign) to increase fine control
     // while permitting full power.
@@ -355,59 +366,7 @@
       rightSpeed = Math.copySign(rightSpeed * rightSpeed, rightSpeed);
     }
 
-    m_leftMotor.set(leftSpeed * m_maxOutput);
-    m_rightMotor.set(rightSpeed * m_maxOutput * m_rightSideInvertMultiplier);
-
-    feed();
-  }
-
-  /**
-   * Sets the QuickStop speed threshold in curvature drive.
-   *
-   * <p>QuickStop compensates for the robot's moment of inertia when stopping after a QuickTurn.
-   *
-   * <p>While QuickTurn is enabled, the QuickStop accumulator takes on the rotation rate value
-   * outputted by the low-pass filter when the robot's speed along the X axis is below the
-   * threshold. When QuickTurn is disabled, the accumulator's value is applied against the computed
-   * angular power request to slow the robot's rotation.
-   *
-   * @param threshold X speed below which quick stop accumulator will receive rotation rate values
-   *                  [0..1.0].
-   */
-  public void setQuickStopThreshold(double threshold) {
-    m_quickStopThreshold = threshold;
-  }
-
-  /**
-   * Sets the low-pass filter gain for QuickStop in curvature drive.
-   *
-   * <p>The low-pass filter filters incoming rotation rate commands to smooth out high frequency
-   * changes.
-   *
-   * @param alpha Low-pass filter gain [0.0..2.0]. Smaller values result in slower output changes.
-   *              Values between 1.0 and 2.0 result in output oscillation. Values below 0.0 and
-   *              above 2.0 are unstable.
-   */
-  public void setQuickStopAlpha(double alpha) {
-    m_quickStopAlpha = alpha;
-  }
-
-  /**
-   * Gets if the power sent to the right side of the drivetrain is multiplied by -1.
-   *
-   * @return true if the right side is inverted
-   */
-  public boolean isRightSideInverted() {
-    return m_rightSideInvertMultiplier == -1.0;
-  }
-
-  /**
-   * Sets if the power sent to the right side of the drivetrain should be multiplied by -1.
-   *
-   * @param rightSideInverted true if right side power should be multiplied by -1
-   */
-  public void setRightSideInverted(boolean rightSideInverted) {
-    m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+    return new WheelSpeeds(leftSpeed, rightSpeed);
   }
 
   @Override
@@ -429,8 +388,6 @@
     builder.setSafeState(this::stopMotor);
     builder.addDoubleProperty("Left Motor Speed", m_leftMotor::get, m_leftMotor::set);
     builder.addDoubleProperty(
-        "Right Motor Speed",
-        () -> m_rightMotor.get() * m_rightSideInvertMultiplier,
-        x -> m_rightMotor.set(x * m_rightSideInvertMultiplier));
+        "Right Motor Speed", () -> m_rightMotor.get(), x -> m_rightMotor.set(x));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
index 45da78a..3c49989 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/KilloughDrive.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.drive;
 
-import java.util.StringJoiner;
+import static java.util.Objects.requireNonNull;
 
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 
 /**
  * A class for driving Killough drive platforms.
@@ -24,6 +21,7 @@
  * <p>Killough drives are triangular with one omni wheel on each corner.
  *
  * <p>Drive base diagram:
+ *
  * <pre>
  *  /_____\
  * / \   / \
@@ -42,6 +40,7 @@
  * points down. Rotations follow the right-hand rule, so clockwise rotation around the Z axis is
  * positive.
  */
+@SuppressWarnings("removal")
 public class KilloughDrive extends RobotDriveBase implements Sendable, AutoCloseable {
   public static final double kDefaultLeftMotorAngle = 60.0;
   public static final double kDefaultRightMotorAngle = 120.0;
@@ -59,6 +58,29 @@
 
   private boolean m_reported;
 
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    public double left;
+    public double right;
+    public double back;
+
+    /** Constructs a WheelSpeeds with zeroes for left, right, and back speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param left The left speed.
+     * @param right The right speed.
+     * @param back The back speed.
+     */
+    public WheelSpeeds(double left, double right, double back) {
+      this.left = left;
+      this.right = right;
+      this.back = back;
+    }
+  }
+
   /**
    * Construct a Killough drive with the given motors and default motor angles.
    *
@@ -67,13 +89,18 @@
    *
    * <p>If a motor needs to be inverted, do so before passing it in.
    *
-   * @param leftMotor  The motor on the left corner.
+   * @param leftMotor The motor on the left corner.
    * @param rightMotor The motor on the right corner.
-   * @param backMotor  The motor on the back corner.
+   * @param backMotor The motor on the back corner.
    */
-  public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
-                       SpeedController backMotor) {
-    this(leftMotor, rightMotor, backMotor, kDefaultLeftMotorAngle, kDefaultRightMotorAngle,
+  public KilloughDrive(
+      SpeedController leftMotor, SpeedController rightMotor, SpeedController backMotor) {
+    this(
+        leftMotor,
+        rightMotor,
+        backMotor,
+        kDefaultLeftMotorAngle,
+        kDefaultRightMotorAngle,
         kDefaultBackMotorAngle);
   }
 
@@ -82,26 +109,39 @@
    *
    * <p>Angles are measured in degrees clockwise from the positive X axis.
    *
-   * @param leftMotor       The motor on the left corner.
-   * @param rightMotor      The motor on the right corner.
-   * @param backMotor       The motor on the back corner.
-   * @param leftMotorAngle  The angle of the left wheel's forward direction of travel.
+   * @param leftMotor The motor on the left corner.
+   * @param rightMotor The motor on the right corner.
+   * @param backMotor The motor on the back corner.
+   * @param leftMotorAngle The angle of the left wheel's forward direction of travel.
    * @param rightMotorAngle The angle of the right wheel's forward direction of travel.
-   * @param backMotorAngle  The angle of the back wheel's forward direction of travel.
+   * @param backMotorAngle The angle of the back wheel's forward direction of travel.
    */
-  public KilloughDrive(SpeedController leftMotor, SpeedController rightMotor,
-                       SpeedController backMotor, double leftMotorAngle, double rightMotorAngle,
-                       double backMotorAngle) {
-    verify(leftMotor, rightMotor, backMotor);
+  public KilloughDrive(
+      SpeedController leftMotor,
+      SpeedController rightMotor,
+      SpeedController backMotor,
+      double leftMotorAngle,
+      double rightMotorAngle,
+      double backMotorAngle) {
+    requireNonNull(leftMotor, "Left motor cannot be null");
+    requireNonNull(rightMotor, "Right motor cannot be null");
+    requireNonNull(backMotor, "Back motor cannot be null");
+
     m_leftMotor = leftMotor;
     m_rightMotor = rightMotor;
     m_backMotor = backMotor;
-    m_leftVec = new Vector2d(Math.cos(leftMotorAngle * (Math.PI / 180.0)),
-                             Math.sin(leftMotorAngle * (Math.PI / 180.0)));
-    m_rightVec = new Vector2d(Math.cos(rightMotorAngle * (Math.PI / 180.0)),
-                              Math.sin(rightMotorAngle * (Math.PI / 180.0)));
-    m_backVec = new Vector2d(Math.cos(backMotorAngle * (Math.PI / 180.0)),
-                             Math.sin(backMotorAngle * (Math.PI / 180.0)));
+    m_leftVec =
+        new Vector2d(
+            Math.cos(leftMotorAngle * (Math.PI / 180.0)),
+            Math.sin(leftMotorAngle * (Math.PI / 180.0)));
+    m_rightVec =
+        new Vector2d(
+            Math.cos(rightMotorAngle * (Math.PI / 180.0)),
+            Math.sin(rightMotorAngle * (Math.PI / 180.0)));
+    m_backVec =
+        new Vector2d(
+            Math.cos(backMotorAngle * (Math.PI / 180.0)),
+            Math.sin(backMotorAngle * (Math.PI / 180.0)));
     SendableRegistry.addChild(this, m_leftMotor);
     SendableRegistry.addChild(this, m_rightMotor);
     SendableRegistry.addChild(this, m_backMotor);
@@ -115,42 +155,15 @@
   }
 
   /**
-   * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
-   * The exception's error message will specify all null motors, e.g. {@code
-   * NullPointerException("leftMotor, rightMotor")}, to give as much information as possible to
-   * the programmer.
-   *
-   * @throws NullPointerException if any of the given motors are null
-   */
-  @SuppressWarnings("PMD.AvoidThrowingNullPointerException")
-  private void verify(SpeedController leftMotor, SpeedController rightMotor,
-                      SpeedController backMotor) {
-    if (leftMotor != null && rightMotor != null && backMotor != null) {
-      return;
-    }
-    StringJoiner joiner = new StringJoiner(", ");
-    if (leftMotor == null) {
-      joiner.add("leftMotor");
-    }
-    if (rightMotor == null) {
-      joiner.add("rightMotor");
-    }
-    if (backMotor == null) {
-      joiner.add("backMotor");
-    }
-    throw new NullPointerException(joiner.toString());
-  }
-
-  /**
    * Drive method for Killough platform.
    *
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
+   *     positive.
    */
   @SuppressWarnings("ParameterName")
   public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
@@ -163,27 +176,77 @@
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
-   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use
-   *                  this to implement field-oriented controls.
+   *     positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
+   *     to implement field-oriented controls.
    */
   @SuppressWarnings("ParameterName")
-  public void driveCartesian(double ySpeed, double xSpeed, double zRotation,
-                             double gyroAngle) {
+  public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
     if (!m_reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive,
-                 tInstances.kRobotDrive2_KilloughCartesian, 3);
+      HAL.report(
+          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughCartesian, 3);
       m_reported = true;
     }
 
-    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
-    ySpeed = applyDeadband(ySpeed, m_deadband);
+    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
 
+    var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
+
+    m_leftMotor.set(speeds.left * m_maxOutput);
+    m_rightMotor.set(speeds.right * m_maxOutput);
+    m_backMotor.set(speeds.back * m_maxOutput);
+
+    feed();
+  }
+
+  /**
+   * Drive method for Killough platform.
+   *
+   * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
+   * drives (translation) is independent from its angle or rotation rate.
+   *
+   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
+   * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drivePolar(double magnitude, double angle, double zRotation) {
+    if (!m_reported) {
+      HAL.report(tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_KilloughPolar, 3);
+      m_reported = true;
+    }
+
+    driveCartesian(
+        magnitude * Math.sin(angle * (Math.PI / 180.0)),
+        magnitude * Math.cos(angle * (Math.PI / 180.0)),
+        zRotation,
+        0.0);
+  }
+
+  /**
+   * Cartesian inverse kinematics for Killough platform.
+   *
+   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+   * from its angle or rotation rate.
+   *
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
+   *     to implement field-oriented controls.
+   * @return Wheel speeds.
+   */
+  @SuppressWarnings("ParameterName")
+  public WheelSpeeds driveCartesianIK(
+      double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
+    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
     xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
-    xSpeed = applyDeadband(xSpeed, m_deadband);
 
     // Compensate for gyro angle.
     Vector2d input = new Vector2d(ySpeed, xSpeed);
@@ -196,34 +259,10 @@
 
     normalize(wheelSpeeds);
 
-    m_leftMotor.set(wheelSpeeds[MotorType.kLeft.value] * m_maxOutput);
-    m_rightMotor.set(wheelSpeeds[MotorType.kRight.value] * m_maxOutput);
-    m_backMotor.set(wheelSpeeds[MotorType.kBack.value] * m_maxOutput);
-
-    feed();
-  }
-
-  /**
-   * Drive method for Killough platform.
-   *
-   * <p>Angles are measured counter-clockwise from straight ahead. The speed at which the robot
-   * drives (translation) is independent from its angle or rotation rate.
-   *
-   * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
-   * @param angle     The angle around the Z axis at which the robot drives in degrees [-180..180].
-   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
-   */
-  @SuppressWarnings("ParameterName")
-  public void drivePolar(double magnitude, double angle, double zRotation) {
-    if (!m_reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive,
-                 tInstances.kRobotDrive2_KilloughPolar, 3);
-      m_reported = true;
-    }
-
-    driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
-                   magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
+    return new WheelSpeeds(
+        wheelSpeeds[MotorType.kLeft.value],
+        wheelSpeeds[MotorType.kRight.value],
+        wheelSpeeds[MotorType.kBack.value]);
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
index d709004..b672fa8 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/MecanumDrive.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.drive;
 
-import java.util.StringJoiner;
+import static java.util.Objects.requireNonNull;
 
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-import edu.wpi.first.wpiutil.math.MathUtil;
 
 /**
  * A class for driving Mecanum drive platforms.
@@ -27,6 +24,7 @@
  * relations for a Mecanum drive robot.
  *
  * <p>Drive base diagram:
+ *
  * <pre>
  * \\_______/
  * \\ |   | /
@@ -47,20 +45,20 @@
  * positive.
  *
  * <p>Inputs smaller then {@value edu.wpi.first.wpilibj.drive.RobotDriveBase#kDefaultDeadband} will
- * be set to 0, and larger values will be scaled so that the full range is still used. This
- * deadband value can be changed with {@link #setDeadband}.
+ * be set to 0, and larger values will be scaled so that the full range is still used. This deadband
+ * value can be changed with {@link #setDeadband}.
  *
- * <p>RobotDrive porting guide:
- * <br>In MecanumDrive, the right side speed controllers are automatically inverted, while in
- * RobotDrive, no speed controllers are automatically inverted.
- * <br>{@link #driveCartesian(double, double, double, double)} is equivalent to
- * {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Cartesian(double, double, double, double)}
- * if a deadband of 0 is used, and the ySpeed and gyroAngle values are inverted compared to
- * RobotDrive (eg driveCartesian(xSpeed, -ySpeed, zRotation, -gyroAngle).
- * <br>{@link #drivePolar(double, double, double)} is equivalent to
- * {@link edu.wpi.first.wpilibj.RobotDrive#mecanumDrive_Polar(double, double, double)} if a
- * deadband of 0 is used.
+ * <p>RobotDrive porting guide: <br>
+ * In MecanumDrive, the right side motor controllers are automatically inverted, while in
+ * RobotDrive, no motor controllers are automatically inverted. <br>
+ * {@link #driveCartesian(double, double, double, double)} is equivalent to RobotDrive's
+ * mecanumDrive_Cartesian(double, double, double, double) if a deadband of 0 is used, and the ySpeed
+ * and gyroAngle values are inverted compared to RobotDrive (eg driveCartesian(xSpeed, -ySpeed,
+ * zRotation, -gyroAngle). <br>
+ * {@link #drivePolar(double, double, double)} is equivalent to RobotDrive's
+ * mecanumDrive_Polar(double, double, double)} if a deadband of 0 is used.
  */
+@SuppressWarnings("removal")
 public class MecanumDrive extends RobotDriveBase implements Sendable, AutoCloseable {
   private static int instances;
 
@@ -69,17 +67,54 @@
   private final SpeedController m_frontRightMotor;
   private final SpeedController m_rearRightMotor;
 
-  private double m_rightSideInvertMultiplier = -1.0;
   private boolean m_reported;
 
+  @SuppressWarnings("MemberName")
+  public static class WheelSpeeds {
+    public double frontLeft;
+    public double frontRight;
+    public double rearLeft;
+    public double rearRight;
+
+    /** Constructs a WheelSpeeds with zeroes for all four speeds. */
+    public WheelSpeeds() {}
+
+    /**
+     * Constructs a WheelSpeeds.
+     *
+     * @param frontLeft The front left speed.
+     * @param frontRight The front right speed.
+     * @param rearLeft The rear left speed.
+     * @param rearRight The rear right speed.
+     */
+    public WheelSpeeds(double frontLeft, double frontRight, double rearLeft, double rearRight) {
+      this.frontLeft = frontLeft;
+      this.frontRight = frontRight;
+      this.rearLeft = rearLeft;
+      this.rearRight = rearRight;
+    }
+  }
+
   /**
    * Construct a MecanumDrive.
    *
    * <p>If a motor needs to be inverted, do so before passing it in.
+   *
+   * @param frontLeftMotor The motor on the front-left corner.
+   * @param rearLeftMotor The motor on the rear-left corner.
+   * @param frontRightMotor The motor on the front-right corner.
+   * @param rearRightMotor The motor on the rear-right corner.
    */
-  public MecanumDrive(SpeedController frontLeftMotor, SpeedController rearLeftMotor,
-                      SpeedController frontRightMotor, SpeedController rearRightMotor) {
-    verify(frontLeftMotor, rearLeftMotor, frontRightMotor, rearRightMotor);
+  public MecanumDrive(
+      SpeedController frontLeftMotor,
+      SpeedController rearLeftMotor,
+      SpeedController frontRightMotor,
+      SpeedController rearRightMotor) {
+    requireNonNull(frontLeftMotor, "Front-left motor cannot be null");
+    requireNonNull(rearLeftMotor, "Rear-left motor cannot be null");
+    requireNonNull(frontRightMotor, "Front-right motor cannot be null");
+    requireNonNull(rearRightMotor, "Rear-right motor cannot be null");
+
     m_frontLeftMotor = frontLeftMotor;
     m_rearLeftMotor = rearLeftMotor;
     m_frontRightMotor = frontRightMotor;
@@ -98,45 +133,15 @@
   }
 
   /**
-   * Verifies that all motors are nonnull, throwing a NullPointerException if any of them are.
-   * The exception's error message will specify all null motors, e.g. {@code
-   * NullPointerException("frontLeftMotor, rearRightMotor")}, to give as much information as
-   * possible to the programmer.
-   *
-   * @throws NullPointerException if any of the given motors are null
-   */
-  @SuppressWarnings({"PMD.AvoidThrowingNullPointerException", "PMD.CyclomaticComplexity"})
-  private void verify(SpeedController frontLeft, SpeedController rearLeft,
-                      SpeedController frontRight, SpeedController rearRightMotor) {
-    if (frontLeft != null && rearLeft != null && frontRight != null && rearRightMotor != null) {
-      return;
-    }
-    StringJoiner joiner = new StringJoiner(", ");
-    if (frontLeft == null) {
-      joiner.add("frontLeftMotor");
-    }
-    if (rearLeft == null) {
-      joiner.add("rearLeftMotor");
-    }
-    if (frontRight == null) {
-      joiner.add("frontRightMotor");
-    }
-    if (rearRightMotor == null) {
-      joiner.add("rearRightMotor");
-    }
-    throw new NullPointerException(joiner.toString());
-  }
-
-  /**
    * Drive method for Mecanum platform.
    *
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
+   *     positive.
    */
   @SuppressWarnings("ParameterName")
   public void driveCartesian(double ySpeed, double xSpeed, double zRotation) {
@@ -149,45 +154,30 @@
    * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
    * from its angle or rotation rate.
    *
-   * @param ySpeed    The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
-   * @param xSpeed    The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
-   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use
-   *                  this to implement field-oriented controls.
+   *     positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
+   *     to implement field-oriented controls.
    */
   @SuppressWarnings("ParameterName")
   public void driveCartesian(double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
     if (!m_reported) {
-      HAL.report(tResourceType.kResourceType_RobotDrive,
-                 tInstances.kRobotDrive2_MecanumCartesian, 4);
+      HAL.report(
+          tResourceType.kResourceType_RobotDrive, tInstances.kRobotDrive2_MecanumCartesian, 4);
       m_reported = true;
     }
 
-    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
-    ySpeed = applyDeadband(ySpeed, m_deadband);
+    ySpeed = MathUtil.applyDeadband(ySpeed, m_deadband);
+    xSpeed = MathUtil.applyDeadband(xSpeed, m_deadband);
 
-    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
-    xSpeed = applyDeadband(xSpeed, m_deadband);
+    var speeds = driveCartesianIK(ySpeed, xSpeed, zRotation, gyroAngle);
 
-    // Compensate for gyro angle.
-    Vector2d input = new Vector2d(ySpeed, xSpeed);
-    input.rotate(-gyroAngle);
-
-    double[] wheelSpeeds = new double[4];
-    wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
-    wheelSpeeds[MotorType.kFrontRight.value] = -input.x + input.y - zRotation;
-    wheelSpeeds[MotorType.kRearLeft.value] = -input.x + input.y + zRotation;
-    wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
-
-    normalize(wheelSpeeds);
-
-    m_frontLeftMotor.set(wheelSpeeds[MotorType.kFrontLeft.value] * m_maxOutput);
-    m_frontRightMotor.set(wheelSpeeds[MotorType.kFrontRight.value] * m_maxOutput
-        * m_rightSideInvertMultiplier);
-    m_rearLeftMotor.set(wheelSpeeds[MotorType.kRearLeft.value] * m_maxOutput);
-    m_rearRightMotor.set(wheelSpeeds[MotorType.kRearRight.value] * m_maxOutput
-        * m_rightSideInvertMultiplier);
+    m_frontLeftMotor.set(speeds.frontLeft * m_maxOutput);
+    m_frontRightMotor.set(speeds.frontRight * m_maxOutput);
+    m_rearLeftMotor.set(speeds.rearLeft * m_maxOutput);
+    m_rearRightMotor.set(speeds.rearRight * m_maxOutput);
 
     feed();
   }
@@ -199,9 +189,9 @@
    * drives (translation) is independent from its angle or rotation rate.
    *
    * @param magnitude The robot's speed at a given angle [-1.0..1.0]. Forward is positive.
-   * @param angle     The angle around the Z axis at which the robot drives in degrees [-180..180].
+   * @param angle The angle around the Z axis at which the robot drives in degrees [-180..180].
    * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
-   *                  positive.
+   *     positive.
    */
   @SuppressWarnings("ParameterName")
   public void drivePolar(double magnitude, double angle, double zRotation) {
@@ -210,26 +200,50 @@
       m_reported = true;
     }
 
-    driveCartesian(magnitude * Math.sin(angle * (Math.PI / 180.0)),
-                   magnitude * Math.cos(angle * (Math.PI / 180.0)), zRotation, 0.0);
+    driveCartesian(
+        magnitude * Math.cos(angle * (Math.PI / 180.0)),
+        magnitude * Math.sin(angle * (Math.PI / 180.0)),
+        zRotation,
+        0.0);
   }
 
   /**
-   * Gets if the power sent to the right side of the drivetrain is multiplied by -1.
+   * Cartesian inverse kinematics for Mecanum platform.
    *
-   * @return true if the right side is inverted
+   * <p>Angles are measured clockwise from the positive X axis. The robot's speed is independent
+   * from its angle or rotation rate.
+   *
+   * @param ySpeed The robot's speed along the Y axis [-1.0..1.0]. Right is positive.
+   * @param xSpeed The robot's speed along the X axis [-1.0..1.0]. Forward is positive.
+   * @param zRotation The robot's rotation rate around the Z axis [-1.0..1.0]. Clockwise is
+   *     positive.
+   * @param gyroAngle The current angle reading from the gyro in degrees around the Z axis. Use this
+   *     to implement field-oriented controls.
+   * @return Wheel speeds.
    */
-  public boolean isRightSideInverted() {
-    return m_rightSideInvertMultiplier == -1.0;
-  }
+  @SuppressWarnings("ParameterName")
+  public static WheelSpeeds driveCartesianIK(
+      double ySpeed, double xSpeed, double zRotation, double gyroAngle) {
+    ySpeed = MathUtil.clamp(ySpeed, -1.0, 1.0);
+    xSpeed = MathUtil.clamp(xSpeed, -1.0, 1.0);
 
-  /**
-   * Sets if the power sent to the right side of the drivetrain should be multiplied by -1.
-   *
-   * @param rightSideInverted true if right side power should be multiplied by -1
-   */
-  public void setRightSideInverted(boolean rightSideInverted) {
-    m_rightSideInvertMultiplier = rightSideInverted ? -1.0 : 1.0;
+    // Compensate for gyro angle.
+    Vector2d input = new Vector2d(ySpeed, xSpeed);
+    input.rotate(-gyroAngle);
+
+    double[] wheelSpeeds = new double[4];
+    wheelSpeeds[MotorType.kFrontLeft.value] = input.x + input.y + zRotation;
+    wheelSpeeds[MotorType.kFrontRight.value] = input.x - input.y - zRotation;
+    wheelSpeeds[MotorType.kRearLeft.value] = input.x - input.y + zRotation;
+    wheelSpeeds[MotorType.kRearRight.value] = input.x + input.y - zRotation;
+
+    normalize(wheelSpeeds);
+
+    return new WheelSpeeds(
+        wheelSpeeds[MotorType.kFrontLeft.value],
+        wheelSpeeds[MotorType.kFrontRight.value],
+        wheelSpeeds[MotorType.kRearLeft.value],
+        wheelSpeeds[MotorType.kRearRight.value]);
   }
 
   @Override
@@ -251,17 +265,16 @@
     builder.setSmartDashboardType("MecanumDrive");
     builder.setActuator(true);
     builder.setSafeState(this::stopMotor);
-    builder.addDoubleProperty("Front Left Motor Speed",
-        m_frontLeftMotor::get,
-        m_frontLeftMotor::set);
-    builder.addDoubleProperty("Front Right Motor Speed",
-        () -> m_frontRightMotor.get() * m_rightSideInvertMultiplier,
-        value -> m_frontRightMotor.set(value * m_rightSideInvertMultiplier));
-    builder.addDoubleProperty("Rear Left Motor Speed",
-        m_rearLeftMotor::get,
-        m_rearLeftMotor::set);
-    builder.addDoubleProperty("Rear Right Motor Speed",
-        () -> m_rearRightMotor.get() * m_rightSideInvertMultiplier,
-        value -> m_rearRightMotor.set(value * m_rightSideInvertMultiplier));
+    builder.addDoubleProperty(
+        "Front Left Motor Speed", m_frontLeftMotor::get, m_frontLeftMotor::set);
+    builder.addDoubleProperty(
+        "Front Right Motor Speed",
+        () -> m_frontRightMotor.get(),
+        value -> m_frontRightMotor.set(value));
+    builder.addDoubleProperty("Rear Left Motor Speed", m_rearLeftMotor::get, m_rearLeftMotor::set);
+    builder.addDoubleProperty(
+        "Rear Right Motor Speed",
+        () -> m_rearRightMotor.get(),
+        value -> m_rearRightMotor.set(value));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
index 785a0c5..569f94c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/RobotDriveBase.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.drive;
 
+import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.wpilibj.MotorSafety;
 
-/**
- * Common base class for drive platforms.
- */
+/** Common base class for drive platforms. */
 public abstract class RobotDriveBase extends MotorSafety {
   public static final double kDefaultDeadband = 0.02;
   public static final double kDefaultMaxOutput = 1.0;
@@ -19,14 +15,16 @@
   protected double m_deadband = kDefaultDeadband;
   protected double m_maxOutput = kDefaultMaxOutput;
 
-  /**
-   * The location of a motor on the robot for the purpose of driving.
-   */
+  /** The location of a motor on the robot for the purpose of driving. */
   public enum MotorType {
-    kFrontLeft(0), kFrontRight(1), kRearLeft(2), kRearRight(3), kLeft(0),
-    kRight(1), kBack(2);
+    kFrontLeft(0),
+    kFrontRight(1),
+    kRearLeft(2),
+    kRearRight(3),
+    kLeft(0),
+    kRight(1),
+    kBack(2);
 
-    @SuppressWarnings("MemberName")
     public final int value;
 
     MotorType(int value) {
@@ -34,9 +32,7 @@
     }
   }
 
-  /**
-   * RobotDriveBase constructor.
-   */
+  /** RobotDriveBase constructor. */
   public RobotDriveBase() {
     setSafetyEnabled(true);
   }
@@ -45,8 +41,8 @@
    * Sets the deadband applied to the drive inputs (e.g., joystick values).
    *
    * <p>The default value is {@value #kDefaultDeadband}. Inputs smaller than the deadband are set to
-   * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See
-   * {@link #applyDeadband}.
+   * 0.0 while inputs larger than the deadband are scaled from 0.0 to 1.0. See {@link
+   * edu.wpi.first.math.MathUtil#applyDeadband}.
    *
    * @param deadband The deadband to set.
    */
@@ -85,25 +81,22 @@
    * Returns 0.0 if the given value is within the specified range around zero. The remaining range
    * between the deadband and 1.0 is scaled from 0.0 to 1.0.
    *
-   * @param value    value to clip
+   * @param value value to clip
    * @param deadband range around zero
+   * @return The value after the deadband is applied.
+   * @deprecated Use MathUtil.applyDeadband(double,double).
    */
-  protected double applyDeadband(double value, double deadband) {
-    if (Math.abs(value) > deadband) {
-      if (value > 0.0) {
-        return (value - deadband) / (1.0 - deadband);
-      } else {
-        return (value + deadband) / (1.0 - deadband);
-      }
-    } else {
-      return 0.0;
-    }
+  @Deprecated(since = "2021", forRemoval = true)
+  protected static double applyDeadband(double value, double deadband) {
+    return MathUtil.applyDeadband(value, deadband);
   }
 
   /**
    * Normalize all wheel speeds if the magnitude of any wheel is greater than 1.0.
+   *
+   * @param wheelSpeeds List of wheel speeds to normalize.
    */
-  protected void normalize(double[] wheelSpeeds) {
+  protected static void normalize(double[] wheelSpeeds) {
     double maxMagnitude = Math.abs(wheelSpeeds[0]);
     for (int i = 1; i < wheelSpeeds.length; i++) {
       double temp = Math.abs(wheelSpeeds[i]);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
index 2b1f839..728cc83 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/drive/Vector2d.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.drive;
 
-/**
- * This is a 2D vector struct that supports basic vector operations.
- */
-@SuppressWarnings("MemberName")
+/** This is a 2D vector struct that supports basic vector operations. */
 public class Vector2d {
+  @SuppressWarnings("MemberName")
   public double x;
+
+  @SuppressWarnings("MemberName")
   public double y;
 
   public Vector2d() {}
 
-  @SuppressWarnings("ParameterName")
   public Vector2d(double x, double y) {
     this.x = x;
     this.y = y;
@@ -42,6 +38,7 @@
    * Returns dot product of this vector with argument.
    *
    * @param vec Vector with which to perform dot product.
+   * @return Dot product of this vector with argument.
    */
   public double dot(Vector2d vec) {
     return x * vec.x + y * vec.y;
@@ -49,6 +46,8 @@
 
   /**
    * Returns magnitude of vector.
+   *
+   * @return Magnitude of vector.
    */
   public double magnitude() {
     return Math.sqrt(x * x + y * y);
@@ -58,6 +57,7 @@
    * Returns scalar projection of this vector onto argument.
    *
    * @param vec Vector onto which to project this vector.
+   * @return scalar projection of this vector onto argument.
    */
   public double scalarProject(Vector2d vec) {
     return dot(vec) / vec.magnitude();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
deleted file mode 100644
index 257d36a..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/Filter.java
+++ /dev/null
@@ -1,59 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.filters;
-
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpilibj.PIDSourceType;
-
-/**
- * Superclass for filters.
- *
- * @deprecated This class is no longer used.
- */
-@Deprecated(since = "2020", forRemoval = true)
-public abstract class Filter implements PIDSource {
-  private final PIDSource m_source;
-
-  public Filter(PIDSource source) {
-    m_source = source;
-  }
-
-  @Override
-  public void setPIDSourceType(PIDSourceType pidSource) {
-    m_source.setPIDSourceType(pidSource);
-  }
-
-  @Override
-  public PIDSourceType getPIDSourceType() {
-    return m_source.getPIDSourceType();
-  }
-
-  @Override
-  public abstract double pidGet();
-
-  /**
-   * Returns the current filter estimate without also inserting new data as pidGet() would do.
-   *
-   * @return The current filter estimate
-   */
-  public abstract double get();
-
-  /**
-   * Reset the filter state.
-   */
-  public abstract void reset();
-
-  /**
-   * Calls PIDGet() of source.
-   *
-   * @return Current value of source
-   */
-  protected double pidGetSource() {
-    return m_source.pidGet();
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
deleted file mode 100644
index cbeb4aa..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/filters/LinearDigitalFilter.java
+++ /dev/null
@@ -1,195 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.filters;
-
-import java.util.Arrays;
-
-import edu.wpi.first.hal.FRCNetComm.tResourceType;
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.PIDSource;
-import edu.wpi.first.wpiutil.CircularBuffer;
-
-/**
- * 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.
- *
- * <p>Filters are of the form: 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])
- *
- * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
- * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
- * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
- * front of the feedback term! This is a common convention in signal processing.
- *
- * <p>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.
- *
- * <p>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!
- *
- * <p>For more on filters, I highly recommend the following articles: http://en.wikipedia
- * .org/wiki/Linear_filter http://en.wikipedia.org/wiki/Iir_filter http://en.wikipedia
- * .org/wiki/Fir_filter
- *
- * <p>Note 1: PIDGet() should be called by the user on a known, regular period. You can set up a
- * Notifier to do this (look at the WPILib PIDController class), or do it "inline" with code in a
- * periodic function.
- *
- * <p>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 PIDGet() gets called at the desired, constant frequency!
- *
- * @deprecated Use LinearFilter class instead.
- */
-@Deprecated
-public class LinearDigitalFilter extends Filter {
-  private static int instances;
-
-  private final CircularBuffer m_inputs;
-  private final CircularBuffer m_outputs;
-  private final double[] m_inputGains;
-  private final double[] m_outputGains;
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param source  The PIDSource object that is used to get values
-   * @param ffGains The "feed forward" or FIR gains
-   * @param fbGains The "feed back" or IIR gains
-   */
-  public LinearDigitalFilter(PIDSource source, double[] ffGains,
-                             double[] fbGains) {
-    super(source);
-    m_inputs = new CircularBuffer(ffGains.length);
-    m_outputs = new CircularBuffer(fbGains.length);
-    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
-    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
-
-    instances++;
-    HAL.report(tResourceType.kResourceType_LinearFilter, instances);
-  }
-
-  /**
-   * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
-   * gain = e^(-dt / T), T is the time constant in seconds.
-   *
-   * <p>This filter is stable for time constants greater than zero.
-   *
-   * @param source       The PIDSource object that is used to get values
-   * @param timeConstant The discrete-time time constant in seconds
-   * @param period       The period in seconds between samples taken by the user
-   */
-  public static LinearDigitalFilter singlePoleIIR(PIDSource source,
-                                                  double timeConstant,
-                                                  double period) {
-    double gain = Math.exp(-period / timeConstant);
-    double[] ffGains = {1.0 - gain};
-    double[] fbGains = {-gain};
-
-    return new LinearDigitalFilter(source, ffGains, fbGains);
-  }
-
-  /**
-   * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
-   * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
-   *
-   * <p>This filter is stable for time constants greater than zero.
-   *
-   * @param source       The PIDSource object that is used to get values
-   * @param timeConstant The discrete-time time constant in seconds
-   * @param period       The period in seconds between samples taken by the user
-   */
-  public static LinearDigitalFilter highPass(PIDSource source,
-                                             double timeConstant,
-                                             double period) {
-    double gain = Math.exp(-period / timeConstant);
-    double[] ffGains = {gain, -gain};
-    double[] fbGains = {-gain};
-
-    return new LinearDigitalFilter(source, ffGains, fbGains);
-  }
-
-  /**
-   * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
-   * x[0]).
-   *
-   * <p>This filter is always stable.
-   *
-   * @param source The PIDSource object that is used to get values
-   * @param taps   The number of samples to average over. Higher = smoother but slower
-   * @throws IllegalArgumentException if number of taps is less than 1
-   */
-  public static LinearDigitalFilter movingAverage(PIDSource source, int taps) {
-    if (taps <= 0) {
-      throw new IllegalArgumentException("Number of taps was not at least 1");
-    }
-
-    double[] ffGains = new double[taps];
-    for (int i = 0; i < ffGains.length; i++) {
-      ffGains[i] = 1.0 / taps;
-    }
-
-    double[] fbGains = new double[0];
-
-    return new LinearDigitalFilter(source, ffGains, fbGains);
-  }
-
-  @Override
-  public double get() {
-    double retVal = 0.0;
-
-    // Calculate the new value
-    for (int i = 0; i < m_inputGains.length; i++) {
-      retVal += m_inputs.get(i) * m_inputGains[i];
-    }
-    for (int i = 0; i < m_outputGains.length; i++) {
-      retVal -= m_outputs.get(i) * m_outputGains[i];
-    }
-
-    return retVal;
-  }
-
-  @Override
-  public void reset() {
-    m_inputs.clear();
-    m_outputs.clear();
-  }
-
-  /**
-   * Calculates the next value of the filter.
-   *
-   * @return The filtered value at this step
-   */
-  @Override
-  public double pidGet() {
-    double retVal = 0.0;
-
-    // Rotate the inputs
-    m_inputs.addFirst(pidGetSource());
-
-    // Calculate the new value
-    for (int i = 0; i < m_inputGains.length; i++) {
-      retVal += m_inputs.get(i) * m_inputGains[i];
-    }
-    for (int i = 0; i < m_outputGains.length; i++) {
-      retVal -= m_outputs.get(i) * m_outputGains[i];
-    }
-
-    // Rotate the outputs
-    m_outputs.addFirst(retVal);
-
-    return retVal;
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
index 81086d4..de2780f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Accelerometer.java
@@ -1,25 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.interfaces;
 
-/**
- * Interface for 3-axis accelerometers.
- */
+/** Interface for 3-axis accelerometers. */
 public interface Accelerometer {
   enum Range {
-    k2G, k4G, k8G, k16G
+    k2G,
+    k4G,
+    k8G,
+    k16G
   }
 
   /**
    * Common interface for setting the measuring range of an accelerometer.
    *
    * @param range The maximum acceleration, positive or negative, that the accelerometer will
-   *              measure. Not all accelerometers support all ranges.
+   *     measure. Not all accelerometers support all ranges.
    */
   void setRange(Range range);
 
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
index 3bac20f..27377be 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Gyro.java
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.interfaces;
 
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 
-/**
- * Interface for yaw rate gyros.
- */
+/** Interface for yaw rate gyros. */
 public interface Gyro extends AutoCloseable {
   /**
    * 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 match starts.
+   * calibration is in progress, this is typically done when the robot is first turned on while it's
+   * sitting at rest before the match starts.
    */
   void calibrate();
 
@@ -30,11 +25,11 @@
    * Return the heading of the robot in degrees.
    *
    * <p>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.
+   * 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.
    *
-   * <p>The angle is expected to increase as the gyro turns clockwise when looked
-   * at from the top. It needs to follow the NED axis convention.
+   * <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
+   * It needs to follow the NED axis convention.
    *
    * <p>This heading is based on integration of the returned rate from the gyro.
    *
@@ -47,27 +42,26 @@
    *
    * <p>The rate is based on the most recent reading of the gyro analog value
    *
-   * <p>The rate is expected to be positive as the gyro turns clockwise when looked
-   * at from the top. It needs to follow the NED axis convention.
+   * <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
+   * It needs to follow the NED axis convention.
    *
    * @return the current rate in degrees per second
    */
   double getRate();
 
   /**
-   * Return the heading of the robot as a {@link edu.wpi.first.wpilibj.geometry.Rotation2d}.
+   * Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
    *
    * <p>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.
+   * 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.
    *
-   * <p>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.
+   * <p>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.
    *
    * <p>This heading is based on integration of the returned rate from the gyro.
    *
-   * @return the current heading of the robot as a
-   *         {@link edu.wpi.first.wpilibj.geometry.Rotation2d}.
+   * @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
    */
   default Rotation2d getRotation2d() {
     return Rotation2d.fromDegrees(-getAngle());
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
deleted file mode 100644
index ea5c1e6..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/interfaces/Potentiometer.java
+++ /dev/null
@@ -1,17 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.interfaces;
-
-import edu.wpi.first.wpilibj.PIDSource;
-
-/**
- * Interface for a Potentiometer.
- */
-public interface Potentiometer extends PIDSource {
-  double get();
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
index 6570780..ff3e8ef 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/livewindow/LiveWindow.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.livewindow;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
 
 /**
- * The LiveWindow class is the public interface for putting sensors and actuators on the
- * LiveWindow.
+ * The LiveWindow class is the public interface for putting sensors and actuators on the LiveWindow.
  */
 public class LiveWindow {
   private static class Component {
@@ -36,6 +32,10 @@
   private static Runnable enabledListener;
   private static Runnable disabledListener;
 
+  static {
+    SendableRegistry.setLiveWindowBuilderFactory(() -> new SendableBuilderImpl());
+  }
+
   private static Component getOrAdd(Sendable sendable) {
     Component data = (Component) SendableRegistry.getData(sendable, dataHandle);
     if (data == null) {
@@ -62,12 +62,17 @@
   }
 
   /**
-   * Set the enabled state of LiveWindow. If it's being enabled, turn off the scheduler and remove
-   * all the commands from the queue and enable all the components registered for LiveWindow. If
-   * it's being disabled, stop all the registered components and reenable the scheduler. TODO: add
-   * code to disable PID loops when enabling LiveWindow. The commands should reenable the PID loops
-   * themselves when they get rescheduled. This prevents arms from starting to move around, etc.
-   * after a period of adjusting them in LiveWindow mode.
+   * Set the enabled state of LiveWindow.
+   *
+   * <p>If it's being enabled, turn off the scheduler and remove all the commands from the queue and
+   * enable all the components registered for LiveWindow. If it's being disabled, stop all the
+   * registered components and reenable the scheduler.
+   *
+   * <p>TODO: add code to disable PID loops when enabling LiveWindow. The commands should reenable
+   * the PID loops themselves when they get rescheduled. This prevents arms from starting to move
+   * around, etc. after a period of adjusting them in LiveWindow mode.
+   *
+   * @param enabled True to enable LiveWindow.
    */
   public static synchronized void setEnabled(boolean enabled) {
     if (liveWindowEnabled != enabled) {
@@ -81,9 +86,11 @@
         }
       } else {
         System.out.println("stopping live window mode.");
-        SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
-          cbdata.builder.stopLiveWindowMode();
-        });
+        SendableRegistry.foreachLiveWindow(
+            dataHandle,
+            cbdata -> {
+              ((SendableBuilderImpl) cbdata.builder).stopLiveWindowMode();
+            });
         if (disabledListener != null) {
           disabledListener.run();
         }
@@ -112,76 +119,77 @@
     getOrAdd(sendable).m_telemetryEnabled = false;
   }
 
-  /**
-   * Disable ALL telemetry.
-   */
+  /** Disable ALL telemetry. */
   public static synchronized void disableAllTelemetry() {
     telemetryEnabled = false;
-    SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
-      if (cbdata.data == null) {
-        cbdata.data = new Component();
-      }
-      ((Component) cbdata.data).m_telemetryEnabled = false;
-    });
+    SendableRegistry.foreachLiveWindow(
+        dataHandle,
+        cbdata -> {
+          if (cbdata.data == null) {
+            cbdata.data = new Component();
+          }
+          ((Component) cbdata.data).m_telemetryEnabled = false;
+        });
   }
 
   /**
    * Tell all the sensors to update (send) their values.
    *
-   * <p>Actuators are handled through callbacks on their value changing from the
-   * SmartDashboard widgets.
+   * <p>Actuators are handled through callbacks on their value changing from the SmartDashboard
+   * widgets.
    */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.NPathComplexity"})
   public static synchronized void updateValues() {
     // Only do this if either LiveWindow mode or telemetry is enabled.
     if (!liveWindowEnabled && !telemetryEnabled) {
       return;
     }
 
-    SendableRegistry.foreachLiveWindow(dataHandle, cbdata -> {
-      if (cbdata.sendable == null || cbdata.parent != null) {
-        return;
-      }
+    SendableRegistry.foreachLiveWindow(
+        dataHandle,
+        cbdata -> {
+          if (cbdata.sendable == null || cbdata.parent != null) {
+            return;
+          }
 
-      if (cbdata.data == null) {
-        cbdata.data = new Component();
-      }
+          if (cbdata.data == null) {
+            cbdata.data = new Component();
+          }
 
-      Component component = (Component) cbdata.data;
+          Component component = (Component) cbdata.data;
 
-      if (!liveWindowEnabled && !component.m_telemetryEnabled) {
-        return;
-      }
+          if (!liveWindowEnabled && !component.m_telemetryEnabled) {
+            return;
+          }
 
-      if (component.m_firstTime) {
-        // By holding off creating the NetworkTable entries, it allows the
-        // components to be redefined. This allows default sensor and actuator
-        // values to be created that are replaced with the custom names from
-        // users calling setName.
-        if (cbdata.name.isEmpty()) {
-          return;
-        }
-        NetworkTable ssTable = liveWindowTable.getSubTable(cbdata.subsystem);
-        NetworkTable table;
-        // Treat name==subsystem as top level of subsystem
-        if (cbdata.name.equals(cbdata.subsystem)) {
-          table = ssTable;
-        } else {
-          table = ssTable.getSubTable(cbdata.name);
-        }
-        table.getEntry(".name").setString(cbdata.name);
-        cbdata.builder.setTable(table);
-        cbdata.sendable.initSendable(cbdata.builder);
-        ssTable.getEntry(".type").setString("LW Subsystem");
+          if (component.m_firstTime) {
+            // By holding off creating the NetworkTable entries, it allows the
+            // components to be redefined. This allows default sensor and actuator
+            // values to be created that are replaced with the custom names from
+            // users calling setName.
+            if (cbdata.name.isEmpty()) {
+              return;
+            }
+            NetworkTable ssTable = liveWindowTable.getSubTable(cbdata.subsystem);
+            NetworkTable table;
+            // Treat name==subsystem as top level of subsystem
+            if (cbdata.name.equals(cbdata.subsystem)) {
+              table = ssTable;
+            } else {
+              table = ssTable.getSubTable(cbdata.name);
+            }
+            table.getEntry(".name").setString(cbdata.name);
+            ((SendableBuilderImpl) cbdata.builder).setTable(table);
+            cbdata.sendable.initSendable(cbdata.builder);
+            ssTable.getEntry(".type").setString("LW Subsystem");
 
-        component.m_firstTime = false;
-      }
+            component.m_firstTime = false;
+          }
 
-      if (startLiveWindow) {
-        cbdata.builder.startLiveWindowMode();
-      }
-      cbdata.builder.updateTable();
-    });
+          if (startLiveWindow) {
+            ((SendableBuilderImpl) cbdata.builder).startLiveWindowMode();
+          }
+          cbdata.builder.update();
+        });
 
     startLiveWindow = false;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java
new file mode 100644
index 0000000..c003b56
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/DMC60.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Digilent DMC 60 Motor Controller.
+ *
+ * <p>Note that the DMC uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the DMC 60 User Manual available from
+ * Digilent.
+ *
+ * <ul>
+ *   <li>2.004ms = full "forward"
+ *   <li>1.520ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.480ms = the "low end" of the deadband range
+ *   <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class DMC60 extends PWMMotorController {
+  /**
+   * Constructor.
+   *
+   * @param channel The PWM channel that the DMC60 is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public DMC60(final int channel) {
+    super("DMC60", channel);
+
+    m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_DigilentDMC60, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java
new file mode 100644
index 0000000..32e6417
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Jaguar.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Texas Instruments / Vex Robotics Jaguar Motor Controller as a PWM device.
+ *
+ * <p>Note that the Jaguar uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Jaguar User Manual available from Vex.
+ *
+ * <ul>
+ *   <li>2.310ms = full "forward"
+ *   <li>1.550ms = the "high end" of the deadband range
+ *   <li>1.507ms = center of the deadband range (off)
+ *   <li>1.454ms = the "low end" of the deadband range
+ *   <li>0.697ms = full "reverse"
+ * </ul>
+ */
+public class Jaguar extends PWMMotorController {
+  /**
+   * Constructor.
+   *
+   * @param channel The PWM channel that the Jaguar is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public Jaguar(final int channel) {
+    super("Jaguar", channel);
+
+    m_pwm.setBounds(2.31, 1.55, 1.507, 1.454, 0.697);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_Jaguar, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java
new file mode 100644
index 0000000..ac82afa
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorController.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.SpeedController;
+
+/** Interface for motor controlling devices. */
+@SuppressWarnings("removal")
+public interface MotorController extends SpeedController {
+  /**
+   * Common interface for setting the speed of a motor controller.
+   *
+   * @param speed The speed to set. Value should be between -1.0 and 1.0.
+   */
+  @Override
+  void set(double speed);
+
+  /**
+   * Sets the voltage output of the MotorController. Compensates for the current bus voltage to
+   * ensure that the desired voltage is output even if the battery voltage is below 12V - highly
+   * useful when the voltage outputs are "meaningful" (e.g. they come from a feedforward
+   * calculation).
+   *
+   * <p>NOTE: This function *must* be called regularly in order for voltage compensation to work
+   * properly - unlike the ordinary set function, it is not "set it and forget it."
+   *
+   * @param outputVolts The voltage to output.
+   */
+  @Override
+  default void setVoltage(double outputVolts) {
+    set(outputVolts / RobotController.getBatteryVoltage());
+  }
+
+  /**
+   * Common interface for getting the current set speed of a motor controller.
+   *
+   * @return The current set speed. Value is between -1.0 and 1.0.
+   */
+  @Override
+  double get();
+
+  /**
+   * Common interface for inverting direction of a motor controller.
+   *
+   * @param isInverted The state of inversion true is inverted.
+   */
+  @Override
+  void setInverted(boolean isInverted);
+
+  /**
+   * Common interface for returning if a motor controller is in the inverted state or not.
+   *
+   * @return isInverted The state of the inversion true is inverted.
+   */
+  @Override
+  boolean getInverted();
+
+  /** Disable the motor controller. */
+  @Override
+  void disable();
+
+  /**
+   * Stops motor movement. Motor can be moved again by calling set without having to re-enable the
+   * motor.
+   */
+  @Override
+  void stopMotor();
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
new file mode 100644
index 0000000..714aac5
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroup.java
@@ -0,0 +1,96 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.Arrays;
+
+/** Allows multiple {@link MotorController} objects to be linked together. */
+public class MotorControllerGroup implements MotorController, Sendable, AutoCloseable {
+  private boolean m_isInverted;
+  private final MotorController[] m_motorControllers;
+  private static int instances;
+
+  /**
+   * Create a new MotorControllerGroup with the provided MotorControllers.
+   *
+   * @param motorController The first MotorController to add
+   * @param motorControllers The MotorControllers to add
+   */
+  public MotorControllerGroup(
+      MotorController motorController, MotorController... motorControllers) {
+    m_motorControllers = new MotorController[motorControllers.length + 1];
+    m_motorControllers[0] = motorController;
+    System.arraycopy(motorControllers, 0, m_motorControllers, 1, motorControllers.length);
+    init();
+  }
+
+  public MotorControllerGroup(MotorController[] motorControllers) {
+    m_motorControllers = Arrays.copyOf(motorControllers, motorControllers.length);
+    init();
+  }
+
+  private void init() {
+    for (MotorController controller : m_motorControllers) {
+      SendableRegistry.addChild(this, controller);
+    }
+    instances++;
+    SendableRegistry.addLW(this, "MotorControllerGroup", instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  @Override
+  public void set(double speed) {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.set(m_isInverted ? -speed : speed);
+    }
+  }
+
+  @Override
+  public double get() {
+    if (m_motorControllers.length > 0) {
+      return m_motorControllers[0].get() * (m_isInverted ? -1 : 1);
+    }
+    return 0.0;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.disable();
+    }
+  }
+
+  @Override
+  public void stopMotor() {
+    for (MotorController motorController : m_motorControllers) {
+      motorController.stopMotor();
+    }
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Motor Controller");
+    builder.setActuator(true);
+    builder.setSafeState(this::stopMotor);
+    builder.addDoubleProperty("Value", this::get, this::set);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java
new file mode 100644
index 0000000..13daf68
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/NidecBrushless.java
@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.MotorSafety;
+import edu.wpi.first.wpilibj.PWM;
+
+/** Nidec Brushless Motor. */
+public class NidecBrushless extends MotorSafety
+    implements MotorController, Sendable, AutoCloseable {
+  private boolean m_isInverted;
+  private final DigitalOutput m_dio;
+  private final PWM m_pwm;
+  private volatile double m_speed;
+  private volatile boolean m_disabled;
+
+  /**
+   * Constructor.
+   *
+   * @param pwmChannel The PWM channel that the Nidec Brushless controller is attached to. 0-9 are
+   *     on-board, 10-19 are on the MXP port
+   * @param dioChannel The DIO channel that the Nidec Brushless controller is attached to. 0-9 are
+   *     on-board, 10-25 are on the MXP port
+   */
+  public NidecBrushless(final int pwmChannel, final int dioChannel) {
+    setSafetyEnabled(false);
+
+    // the dio controls the output (in PWM mode)
+    m_dio = new DigitalOutput(dioChannel);
+    SendableRegistry.addChild(this, m_dio);
+    m_dio.setPWMRate(15625);
+    m_dio.enablePWM(0.5);
+
+    // the pwm enables the controller
+    m_pwm = new PWM(pwmChannel);
+    SendableRegistry.addChild(this, m_pwm);
+
+    HAL.report(tResourceType.kResourceType_NidecBrushless, pwmChannel + 1);
+    SendableRegistry.addLW(this, "Nidec Brushless", pwmChannel);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_dio.close();
+    m_pwm.close();
+  }
+
+  /**
+   * Set the PWM value.
+   *
+   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+   * FPGA.
+   *
+   * @param speed The speed value between -1.0 and 1.0 to set.
+   */
+  @Override
+  public void set(double speed) {
+    if (!m_disabled) {
+      m_speed = speed;
+      m_dio.updateDutyCycle(0.5 + 0.5 * (m_isInverted ? -speed : speed));
+      m_pwm.setRaw(0xffff);
+    }
+
+    feed();
+  }
+
+  /**
+   * Get the recently set value of the PWM.
+   *
+   * @return The most recently set value for the PWM between -1.0 and 1.0.
+   */
+  @Override
+  public double get() {
+    return m_speed;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  /**
+   * Stop the motor. This is called by the MotorSafety object when it has a timeout for this PWM and
+   * needs to stop it from running. Calling set() will re-enable the motor.
+   */
+  @Override
+  public void stopMotor() {
+    m_dio.updateDutyCycle(0.5);
+    m_pwm.setDisabled();
+  }
+
+  @Override
+  public String getDescription() {
+    return "Nidec " + getChannel();
+  }
+
+  /** Disable the motor. The enable() function must be called to re-enable the motor. */
+  @Override
+  public void disable() {
+    m_disabled = true;
+    m_dio.updateDutyCycle(0.5);
+    m_pwm.setDisabled();
+  }
+
+  /**
+   * Re-enable the motor after disable() has been called. The set() function must be called to set a
+   * new motor speed.
+   */
+  public void enable() {
+    m_disabled = false;
+  }
+
+  /**
+   * Gets the channel number associated with the object.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_pwm.getChannel();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Nidec Brushless");
+    builder.setActuator(true);
+    builder.setSafeState(this::stopMotor);
+    builder.addDoubleProperty("Value", this::get, this::set);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
new file mode 100644
index 0000000..d33fb01
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMMotorController.java
@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj.MotorSafety;
+import edu.wpi.first.wpilibj.PWM;
+
+/** Common base class for all PWM Motor Controllers. */
+public abstract class PWMMotorController extends MotorSafety
+    implements MotorController, Sendable, AutoCloseable {
+  private boolean m_isInverted;
+  protected PWM m_pwm;
+
+  /**
+   * Constructor.
+   *
+   * @param name Name to use for SendableRegistry
+   * @param channel The PWM channel that the controller is attached to. 0-9 are on-board, 10-19 are
+   *     on the MXP port
+   */
+  protected PWMMotorController(final String name, final int channel) {
+    m_pwm = new PWM(channel, false);
+    SendableRegistry.addLW(this, name, channel);
+  }
+
+  /** Free the resource associated with the PWM channel and set the value to 0. */
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+    m_pwm.close();
+  }
+
+  /**
+   * Set the PWM value.
+   *
+   * <p>The PWM value is set using a range of -1.0 to 1.0, appropriately scaling the value for the
+   * FPGA.
+   *
+   * @param speed The speed value between -1.0 and 1.0 to set.
+   */
+  @Override
+  public void set(double speed) {
+    m_pwm.setSpeed(m_isInverted ? -speed : speed);
+    feed();
+  }
+
+  /**
+   * Get the recently set value of the PWM. This value is affected by the inversion property. If you
+   * want the value that is sent directly to the MotorController, use {@link
+   * edu.wpi.first.wpilibj.PWM#getSpeed()} instead.
+   *
+   * @return The most recently set value for the PWM between -1.0 and 1.0.
+   */
+  @Override
+  public double get() {
+    return m_pwm.getSpeed() * (m_isInverted ? -1.0 : 1.0);
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    m_pwm.setDisabled();
+  }
+
+  @Override
+  public void stopMotor() {
+    disable();
+  }
+
+  @Override
+  public String getDescription() {
+    return "PWM " + getChannel();
+  }
+
+  /**
+   * Gets the backing PWM handle.
+   *
+   * @return The pwm handle.
+   */
+  public int getPwmHandle() {
+    return m_pwm.getHandle();
+  }
+
+  /**
+   * Gets the PWM channel number.
+   *
+   * @return The channel number.
+   */
+  public int getChannel() {
+    return m_pwm.getChannel();
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("Motor Controller");
+    builder.setActuator(true);
+    builder.setSafeState(this::disable);
+    builder.addDoubleProperty("Value", this::get, this::set);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java
new file mode 100644
index 0000000..e90f60d
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMSparkMax.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * REV Robotics SPARK MAX Motor Controller with PWM control.
+ *
+ * <p>Note that the SPARK MAX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SPARK MAX User Manual available from
+ * REV Robotics.
+ *
+ * <ul>
+ *   <li>2.003ms = full "forward"
+ *   <li>1.550ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.460ms = the "low end" of the deadband range
+ *   <li>0.999ms = full "reverse"
+ * </ul>
+ */
+public class PWMSparkMax extends PWMMotorController {
+  /**
+   * Common initialization code called by all constructors.
+   *
+   * @param channel The PWM channel number. 0-9 are on-board, 10-19 are on the MXP port
+   */
+  public PWMSparkMax(final int channel) {
+    super("PWMSparkMax", channel);
+
+    m_pwm.setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_RevSparkMaxPWM, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java
new file mode 100644
index 0000000..d521745
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonFX.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon FX Motor Controller with PWM control.
+ *
+ * <p>Note that the TalonFX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the TalonFX User Manual available from
+ * CTRE.
+ *
+ * <ul>
+ *   <li>2.004ms = full "forward"
+ *   <li>1.520ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.480ms = the "low end" of the deadband range
+ *   <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonFX extends PWMMotorController {
+  /**
+   * Constructor for a TalonFX connected via PWM.
+   *
+   * @param channel The PWM channel that the Talon FX is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public PWMTalonFX(final int channel) {
+    super("PWMTalonFX", channel);
+
+    m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_TalonFX, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java
new file mode 100644
index 0000000..81f1834
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMTalonSRX.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon SRX Motor Controller with PWM control.
+ *
+ * <p>Note that the TalonSRX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the TalonSRX User Manual available from
+ * CTRE.
+ *
+ * <ul>
+ *   <li>2.004ms = full "forward"
+ *   <li>1.520ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.480ms = the "low end" of the deadband range
+ *   <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMTalonSRX extends PWMMotorController {
+  /**
+   * Constructor for a TalonSRX connected via PWM.
+   *
+   * @param channel The PWM channel that the Talon SRX is attached to. 0-9 are on-board, 10-19 are
+   *     on the MXP port
+   */
+  public PWMTalonSRX(final int channel) {
+    super("PWMTalonSRX", channel);
+
+    m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_PWMTalonSRX, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java
new file mode 100644
index 0000000..9f7a885
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVenom.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Playing with Fusion Venom Smart Motor with PWM control.
+ *
+ * <p>Note that the Venom uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended.
+ *
+ * <ul>
+ *   <li>2.004ms = full "forward"
+ *   <li>1.520ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.480ms = the "low end" of the deadband range
+ *   <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVenom extends PWMMotorController {
+  /**
+   * Constructor for a Venom connected via PWM.
+   *
+   * @param channel The PWM channel that the Venom is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public PWMVenom(final int channel) {
+    super("PWMVenom", channel);
+
+    m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_FusionVenom, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java
new file mode 100644
index 0000000..9880464
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/PWMVictorSPX.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Cross the Road Electronics (CTRE) Victor SPX Motor Controller with PWM control.
+ *
+ * <p>Note that the Victor SPX uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Victor SPX User Manual available from
+ * CTRE.
+ *
+ * <ul>
+ *   <li>2.004ms = full "forward"
+ *   <li>1.520ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.480ms = the "low end" of the deadband range
+ *   <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class PWMVictorSPX extends PWMMotorController {
+  /**
+   * Constructor for a Victor SPX connected via PWM.
+   *
+   * @param channel The PWM channel that the PWMVictorSPX is attached to. 0-9 are on-board, 10-19
+   *     are on the MXP port
+   */
+  public PWMVictorSPX(final int channel) {
+    super("PWMVictorSPX", channel);
+
+    m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_PWMVictorSPX, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java
new file mode 100644
index 0000000..3876dfc
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/SD540.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Mindsensors SD540 Motor Controller.
+ *
+ * <p>Note that the SD540 uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the SD540 User Manual available from
+ * Mindsensors.
+ *
+ * <ul>
+ *   <li>2.05ms = full "forward"
+ *   <li>1.55ms = the "high end" of the deadband range
+ *   <li>1.50ms = center of the deadband range (off)
+ *   <li>1.44ms = the "low end" of the deadband range
+ *   <li>0.94ms = full "reverse"
+ * </ul>
+ */
+public class SD540 extends PWMMotorController {
+  /**
+   * Constructor.
+   *
+   * @param channel The PWM channel that the SD540 is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public SD540(final int channel) {
+    super("SD540", channel);
+
+    m_pwm.setBounds(2.05, 1.55, 1.50, 1.44, 0.94);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_MindsensorsSD540, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java
new file mode 100644
index 0000000..1b99228
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Spark.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * REV Robotics SPARK Motor Controller.
+ *
+ * <p>Note that the SPARK uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Spark User Manual available from REV
+ * Robotics.
+ *
+ * <ul>
+ *   <li>2.003ms = full "forward"
+ *   <li>1.550ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.460ms = the "low end" of the deadband range
+ *   <li>0.999ms = full "reverse"
+ * </ul>
+ */
+public class Spark extends PWMMotorController {
+  /**
+   * Constructor.
+   *
+   * @param channel The PWM channel that the SPARK is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public Spark(final int channel) {
+    super("Spark", channel);
+
+    m_pwm.setBounds(2.003, 1.55, 1.50, 1.46, 0.999);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_RevSPARK, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java
new file mode 100644
index 0000000..576ba6a
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Talon.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * Cross the Road Electronics (CTRE) Talon and Talon SR Motor Controller.
+ *
+ * <p>Note that the Talon uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the Talon User Manual available from CTRE.
+ *
+ * <ul>
+ *   <li>2.037ms = full "forward"
+ *   <li>1.539ms = the "high end" of the deadband range
+ *   <li>1.513ms = center of the deadband range (off)
+ *   <li>1.487ms = the "low end" of the deadband range
+ *   <li>0.989ms = full "reverse"
+ * </ul>
+ */
+public class Talon extends PWMMotorController {
+  /**
+   * Constructor for a Talon (original or Talon SR).
+   *
+   * @param channel The PWM channel that the Talon is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public Talon(final int channel) {
+    super("Talon", channel);
+
+    m_pwm.setBounds(2.037, 1.539, 1.513, 1.487, 0.989);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_Talon, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java
new file mode 100644
index 0000000..fac6c4f
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/Victor.java
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * VEX Robotics Victor 888 Motor Controller The Vex Robotics Victor 884 Motor Controller can also be
+ * used with this class but may need to be calibrated per the Victor 884 user manual.
+ *
+ * <p>Note that the Victor uses the following bounds for PWM values. These values were determined
+ * empirically and optimized for the Victor 888. These values should work reasonably well for Victor
+ * 884 controllers also but if users experience issues such as asymmetric behavior around the
+ * deadband or inability to saturate the controller in either direction, calibration is recommended.
+ * The calibration procedure can be found in the Victor 884 User Manual available from VEX Robotics:
+ * http://content.vexrobotics.com/docs/ifi-v884-users-manual-9-25-06.pdf
+ *
+ * <ul>
+ *   <li>2.027ms = full "forward"
+ *   <li>1.525ms = the "high end" of the deadband range
+ *   <li>1.507ms = center of the deadband range (off)
+ *   <li>1.490ms = the "low end" of the deadband range
+ *   <li>1.026ms = full "reverse"
+ * </ul>
+ */
+public class Victor extends PWMMotorController {
+  /**
+   * Constructor.
+   *
+   * @param channel The PWM channel that the Victor is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public Victor(final int channel) {
+    super("Victor", channel);
+
+    m_pwm.setBounds(2.027, 1.525, 1.507, 1.49, 1.026);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k2X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_Victor, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java
new file mode 100644
index 0000000..10dba67
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/motorcontrol/VictorSP.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+
+/**
+ * VEX Robotics Victor SP Motor Controller.
+ *
+ * <p>Note that the VictorSP uses the following bounds for PWM values. These values should work
+ * reasonably well for most controllers, but if users experience issues such as asymmetric behavior
+ * around the deadband or inability to saturate the controller in either direction, calibration is
+ * recommended. The calibration procedure can be found in the VictorSP User Manual available from
+ * CTRE.
+ *
+ * <ul>
+ *   <li>2.004ms = full "forward"
+ *   <li>1.520ms = the "high end" of the deadband range
+ *   <li>1.500ms = center of the deadband range (off)
+ *   <li>1.480ms = the "low end" of the deadband range
+ *   <li>0.997ms = full "reverse"
+ * </ul>
+ */
+public class VictorSP extends PWMMotorController {
+  /**
+   * Constructor.
+   *
+   * @param channel The PWM channel that the VictorSP is attached to. 0-9 are on-board, 10-19 are on
+   *     the MXP port
+   */
+  public VictorSP(final int channel) {
+    super("VictorSP", channel);
+
+    m_pwm.setBounds(2.004, 1.52, 1.50, 1.48, 0.997);
+    m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X);
+    m_pwm.setSpeed(0.0);
+    m_pwm.setZeroLatch();
+
+    HAL.report(tResourceType.kResourceType_VictorSP, getChannel() + 1);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
index 27c3e8c..fb642ae 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInLayouts.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -18,8 +15,9 @@
 public enum BuiltInLayouts implements LayoutType {
   /**
    * Groups components in a vertical list. New widgets added to the layout will be placed at the
-   * bottom of the list.
-   * <br>Custom properties:
+   * bottom of the list. <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Label position</td><td>String</td><td>"BOTTOM"</td>
@@ -30,8 +28,9 @@
   kList("List Layout"),
 
   /**
-   * Groups components in an <i>n</i> x <i>m</i> grid. Grid layouts default to 3x3.
-   * <br>Custom properties:
+   * Groups components in an <i>n</i> x <i>m</i> grid. Grid layouts default to 3x3. <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Number of columns</td><td>Number</td><td>3</td><td>Must be in the range [1,15]</td>
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
index fa98a0e..82f2745 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -13,6 +10,7 @@
  * The types of the widgets bundled with Shuffleboard.
  *
  * <p>For example, setting a number to be displayed with a slider:
+ *
  * <pre>{@code
  * NetworkTableEntry example = Shuffleboard.getTab("My Tab")
  *   .add("My Number", 0)
@@ -26,23 +24,30 @@
  */
 public enum BuiltInWidgets implements WidgetType {
   /**
-   * Displays a value with a simple text field.
-   * <br>Supported types:
+   * Displays a value with a simple text field. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>String</li>
-   * <li>Number</li>
-   * <li>Boolean</li>
+   *   <li>String
+   *   <li>Number
+   *   <li>Boolean
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kTextView("Text View"),
   /**
-   * Displays a number with a controllable slider.
-   * <br>Supported types:
+   * Displays a number with a controllable slider. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Number</li>
+   *   <li>Number
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Min</td><td>Number</td><td>-1.0</td><td>The minimum value of the slider</td></tr>
@@ -53,12 +58,16 @@
    */
   kNumberSlider("Number Slider"),
   /**
-   * Displays a number with a view-only bar.
-   * <br>Supported types:
+   * Displays a number with a view-only bar. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Number</li>
+   *   <li>Number
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Min</td><td>Number</td><td>-1.0</td><td>The minimum value of the bar</td></tr>
@@ -69,11 +78,16 @@
   kNumberBar("Number Bar"),
   /**
    * Displays a number with a view-only dial. Displayed values are rounded to the nearest integer.
-   * <br>Supported types:
+   * <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Number</li>
+   *   <li>Number
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Min</td><td>Number</td><td>0</td><td>The minimum value of the dial</td></tr>
@@ -86,13 +100,17 @@
   /**
    * Displays a number with a graph. <strong>NOTE:</strong> graphs can be taxing on the computer
    * running the dashboard. Keep the number of visible data points to a minimum. Making the widget
-   * smaller also helps with performance, but may cause the graph to become difficult to read.
-   * <br>Supported types:
+   * smaller also helps with performance, but may cause the graph to become difficult to read. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Number</li>
-   * <li>Number array</li>
+   *   <li>Number
+   *   <li>Number array
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Visible time</td><td>Number</td><td>30</td>
@@ -101,12 +119,16 @@
    */
   kGraph("Graph"),
   /**
-   * Displays a boolean value as a large colored box.
-   * <br>Supported types:
+   * Displays a boolean value as a large colored box. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Boolean</li>
+   *   <li>Boolean
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Color when true</td><td>Color</td><td>"green"</td>
@@ -118,31 +140,41 @@
    */
   kBooleanBox("Boolean Box"),
   /**
-   * Displays a boolean with a large interactive toggle button.
-   * <br>Supported types:
+   * Displays a boolean with a large interactive toggle button. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Boolean</li>
+   *   <li>Boolean
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kToggleButton("Toggle Button"),
   /**
-   * Displays a boolean with a fixed-size toggle switch.
-   * <br>Supported types:
+   * Displays a boolean with a fixed-size toggle switch. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Boolean</li>
+   *   <li>Boolean
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kToggleSwitch("Toggle Switch"),
   /**
-   * Displays an analog input or a raw number with a number bar.
-   * <br>Supported types:
+   * Displays an analog input or a raw number with a number bar. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>Number</li>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogInput}</li>
+   *   <li>Number
+   *   <li>{@link edu.wpi.first.wpilibj.AnalogInput}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Min</td><td>Number</td><td>0</td><td>The minimum value of the bar</td></tr>
@@ -156,121 +188,151 @@
    */
   kVoltageView("Voltage View"),
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.PowerDistributionPanel PowerDistributionPanel}.
-   * <br>Supported types:
+   * Displays a {@link edu.wpi.first.wpilibj.PowerDistribution PowerDistribution}. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.PowerDistributionPanel}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.PowerDistribution}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Show voltage and current values</td><td>Boolean</td><td>true</td>
    * <td>Whether or not to display the voltage and current draw</td></tr>
    * </table>
    */
-  kPowerDistributionPanel("PDP"),
+  kPowerDistribution("PDP"),
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with
-   * a dropdown combo box with a list of options.
-   * <br>Supported types:
+   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser 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>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kComboBoxChooser("ComboBox Chooser"),
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with
-   * a toggle button for each available option.
-   * <br>Supported types:
+   * Displays a {@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser SendableChooser} with a
+   * toggle button for each available option. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.smartdashboard.SendableChooser}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kSplitButtonChooser("Split Button Chooser"),
   /**
    * Displays an {@link edu.wpi.first.wpilibj.Encoder} displaying its speed, total traveled
-   * distance, and its distance per tick.
-   * <br>Supported types:
+   * distance, and its distance per tick. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.Encoder}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.Encoder}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kEncoder("Encoder"),
   /**
-   * Displays a {@link edu.wpi.first.wpilibj.SpeedController SpeedController}. The speed controller
-   * will be controllable from the dashboard when test mode is enabled, but will otherwise be
-   * view-only.
-   * <br>Supported types:
+   * Displays a {@link edu.wpi.first.wpilibj.motorcontrol.MotorController MotorController}. The
+   * motor 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.PWMSparkMax}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMTalonFX}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMTalonSRX}</li>
-   * <li>{@link edu.wpi.first.wpilibj.PWMVenom}</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>Any custom subclass of {@code SpeedController}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMMotorController}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.DMC60}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.Jaguar}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMTalonFX}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMVenom}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.SD540}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.Spark}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.Talon}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.Victor}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.VictorSP}
+   *   <li>{@link edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup}
+   *   <li>Any custom subclass of {@code MotorController}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Orientation</td><td>String</td><td>"HORIZONTAL"</td>
    * <td>One of {@code ["HORIZONTAL", "VERTICAL"]}</td></tr>
    * </table>
    */
-  kSpeedController("Speed Controller"),
+  kMotorController("Motor Controller"),
   /**
    * 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:
+   * 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>
+   *   <li>{@link edu.wpi.first.wpilibj.command.Command}
+   *   <li>{@link edu.wpi.first.wpilibj.command.CommandGroup}
+   *   <li>Any custom subclass of {@code Command} or {@code CommandGroup}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kCommand("Command"),
   /**
    * 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:
+   * completes. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.command.PIDCommand}</li>
-   * <li>Any custom subclass of {@code PIDCommand}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.command.PIDCommand}
+   *   <li>Any custom subclass of {@code PIDCommand}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kPIDCommand("PID Command"),
   /**
    * Displays a PID controller with an editor for the PIDF constants and a toggle switch for
-   * enabling and disabling the controller.
-   * <br>Supported types:
+   * enabling and disabling the controller. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.PIDController}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.PIDController}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kPIDController("PID Controller"),
   /**
    * Displays an accelerometer with a number bar displaying the magnitude of the acceleration and
-   * text displaying the exact value.
-   * <br>Supported types:
+   * text displaying the exact value. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.AnalogAccelerometer}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.AnalogAccelerometer}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Min</td><td>Number</td><td>-1</td>
@@ -287,14 +349,18 @@
    */
   kAccelerometer("Accelerometer"),
   /**
-   * Displays a 3-axis accelerometer with a number bar for each axis' acceleration.
-   * <br>Supported types:
+   * Displays a 3-axis accelerometer with a number bar for each axis' acceleration. <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>
+   *   <li>{@link edu.wpi.first.wpilibj.ADXL345_I2C}
+   *   <li>{@link edu.wpi.first.wpilibj.ADXL345_SPI}
+   *   <li>{@link edu.wpi.first.wpilibj.ADXL362}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Range</td><td>{@link Range}</td><td>k16G</td><td>The accelerometer range</td></tr>
@@ -308,14 +374,18 @@
    */
   k3AxisAccelerometer("3-Axis Accelerometer"),
   /**
-   * Displays a gyro with a dial from 0 to 360 degrees.
-   * <br>Supported types:
+   * 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>Any custom subclass of {@code GyroBase} (such as a MXP gyro)</li>
+   *   <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}
+   *   <li>{@link edu.wpi.first.wpilibj.AnalogGyro}
+   *   <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Major tick spacing</td><td>Number</td><td>45</td><td>Degrees</td></tr>
@@ -326,23 +396,30 @@
    */
   kGyro("Gyro"),
   /**
-   * Displays a relay with toggle buttons for each supported mode (off, on, forward, reverse).
-   * <br>Supported types:
+   * 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>
+   *   <li>{@link edu.wpi.first.wpilibj.Relay}
    * </ul>
-   * <br>This widget has no custom properties.
+   *
+   * <br>
+   * This widget has no custom properties.
    */
   kRelay("Relay"),
   /**
    * Displays a differential drive with a widget that displays the speed of each side of the
    * drivebase and a vector for the direction and rotation of the drivebase. The widget will be
-   * controllable if the robot is in test mode.
-   * <br>Supported types:
+   * controllable if the robot is in test mode. <br>
+   * Supported types:
+   *
    * <ul>
-   * <li>{@link edu.wpi.first.wpilibj.drive.DifferentialDrive}</li>
+   *   <li>{@link edu.wpi.first.wpilibj.drive.DifferentialDrive}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Number of wheels</td><td>Number</td><td>4</td><td>Must be a positive even integer
@@ -354,13 +431,17 @@
   kDifferentialDrive("Differential Drivebase"),
   /**
    * 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:
+   * 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>
+   *   <li>{@link edu.wpi.first.wpilibj.drive.MecanumDrive}
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Show velocity vectors</td><td>Boolean</td><td>true</td></tr>
@@ -368,12 +449,16 @@
    */
   kMecanumDrive("Mecanum Drivebase"),
   /**
-   * Displays a camera stream.
-   * <br>Supported types:
+   * 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>{@link edu.wpi.first.cscore.VideoSource} (as long as it is streaming on an MJPEG server)
    * </ul>
-   * <br>Custom properties:
+   *
+   * <br>
+   * Custom properties:
+   *
    * <table>
    * <tr><th>Name</th><th>Type</th><th>Default Value</th><th>Notes</th></tr>
    * <tr><td>Show crosshair</td><td>Boolean</td><td>true</td>
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
index d56fde8..cef9c07 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ComplexWidget.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
 import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
+import edu.wpi.first.util.sendable.Sendable;
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
 
 /**
- * A Shuffleboard widget that handles a {@link Sendable} object such as a speed controller or
+ * A Shuffleboard widget that handles a {@link Sendable} object such as a motor controller or
  * sensor.
  */
 public final class ComplexWidget extends ShuffleboardWidget<ComplexWidget> {
@@ -34,7 +30,7 @@
       m_sendable.initSendable(m_builder);
       m_builder.startListeners();
     }
-    m_builder.updateTable();
+    m_builder.update();
   }
 
   /**
@@ -58,5 +54,4 @@
       m_builder.stopLiveWindowMode();
     }
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
index 26649d0..58ac678 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ContainerHelper.java
@@ -1,12 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.ArrayList;
 import java.util.HashSet;
 import java.util.LinkedHashMap;
@@ -20,13 +20,7 @@
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * A helper class for Shuffleboard containers to handle common child operations.
- */
+/** A helper class for Shuffleboard containers to handle common child operations. */
 final class ContainerHelper {
   private final ShuffleboardContainer m_container;
   private final Set<String> m_usedTitles = new HashSet<>();
@@ -126,9 +120,8 @@
     checkTitle(title);
   }
 
-  private <T> SuppliedValueWidget<T> addSupplied(String title,
-                                                 Supplier<T> supplier,
-                                                 BiConsumer<NetworkTableEntry, T> setter) {
+  private <T> SuppliedValueWidget<T> addSupplied(
+      String title, Supplier<T> supplier, BiConsumer<NetworkTableEntry, T> setter) {
     SuppliedValueWidget<T> widget = new SuppliedValueWidget<>(m_container, title, supplier, setter);
     m_components.add(widget);
     return widget;
@@ -147,5 +140,4 @@
     }
     m_usedTitles.add(title);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
index 08f9712..4d64efe 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/EventImportance.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
 /**
- * The importance of an event marker in Shuffleboard.  The exact meaning of each importance level is
- * up for interpretation on a team-to-team basis, but users should follow the general guidelines
- * of the various importance levels.  The examples given are for reference and may be ignored or
+ * The importance of an event marker in Shuffleboard. The exact meaning of each importance level is
+ * up for interpretation on a team-to-team basis, but users should follow the general guidelines of
+ * the various importance levels. The examples given are for reference and may be ignored or
  * considered to be more or less important from team to team.
  */
 public enum EventImportance {
@@ -18,14 +15,10 @@
   // Modifying the enum or enum strings requires a corresponding change to the C++ enum
   // and the enum in Shuffleboard
 
-  /**
-   * A trivial event such as a change in command state.
-   */
+  /** A trivial event such as a change in command state. */
   kTrivial("TRIVIAL"),
 
-  /**
-   * A low importance event such as acquisition of a game piece.
-   */
+  /** A low importance event such as acquisition of a game piece. */
   kLow("LOW"),
 
   /**
@@ -33,14 +26,10 @@
    */
   kNormal("NORMAL"),
 
-  /**
-   * A high-importance event such as scoring a game piece.
-   */
+  /** A high-importance event such as scoring a game piece. */
   kHigh("HIGH"),
 
-  /**
-   * A critically important event such as a brownout, component failure, or software deadlock.
-   */
+  /** A critically important event such as a brownout, component failure, or software deadlock. */
   kCritical("CRITICAL");
 
   private final String m_simpleName;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
index 2d7b565..82ffcbe 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/LayoutType.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -16,6 +13,8 @@
 public interface LayoutType {
   /**
    * Gets the string type of the layout as defined by that layout in Shuffleboard.
+   *
+   * @return The string type of the layout.
    */
   String getLayoutName();
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
index 9f04274..a7be3c9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/RecordingController.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -12,9 +9,7 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.DriverStation;
 
-/**
- * Controls Shuffleboard recordings via NetworkTables.
- */
+/** Controls Shuffleboard recordings via NetworkTables. */
 final class RecordingController {
   private static final String kRecordingTableName = "/Shuffleboard/.recording/";
   private static final String kRecordingControlKey = kRecordingTableName + "RecordData";
@@ -49,21 +44,20 @@
 
   public void addEventMarker(String name, String description, EventImportance importance) {
     if (name == null || name.isEmpty()) {
-      DriverStation.reportError(
-          "Shuffleboard event name was not specified", true);
+      DriverStation.reportError("Shuffleboard event name was not specified", true);
       return;
     }
 
     if (importance == null) {
-      DriverStation.reportError(
-          "Shuffleboard event importance was null", true);
+      DriverStation.reportError("Shuffleboard event importance was null", true);
       return;
     }
 
     String eventDescription = description == null ? "" : description;
 
-    m_eventsTable.getSubTable(name)
+    m_eventsTable
+        .getSubTable(name)
         .getEntry("Info")
-        .setStringArray(new String[]{eventDescription, importance.getSimpleName()});
+        .setStringArray(new String[] {eventDescription, importance.getSimpleName()});
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
index bb7738f..ee72303 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SendableCameraWrapper.java
@@ -1,23 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
+import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.Map;
 import java.util.WeakHashMap;
 
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
-
-/**
- * A wrapper to make video sources sendable and usable from Shuffleboard.
- */
+/** A wrapper to make video sources sendable and usable from Shuffleboard. */
 public final class SendableCameraWrapper implements Sendable, AutoCloseable {
   private static final String kProtocol = "camera_server://";
 
@@ -26,8 +20,8 @@
   private final String m_uri;
 
   /**
-   * Creates a new sendable wrapper. Private constructor to avoid direct instantiation with
-   * multiple wrappers floating around for the same camera.
+   * Creates a new sendable wrapper. Private constructor to avoid direct instantiation with multiple
+   * wrappers floating around for the same camera.
    *
    * @param source the source to wrap
    */
@@ -43,12 +37,12 @@
   }
 
   /**
-   * Gets a sendable wrapper object for the given video source, creating the wrapper if one does
-   * not already exist for the source.
+   * Gets a sendable wrapper object for the given video source, creating the wrapper if one does not
+   * already exist for the source.
    *
    * @param source the video source to wrap
-   * @return a sendable wrapper object for the video source, usable in Shuffleboard via
-   * {@link ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
+   * @return a sendable wrapper object for the video source, usable in Shuffleboard via {@link
+   *     ShuffleboardTab#add(Sendable)} and {@link ShuffleboardLayout#add(Sendable)}
    */
   public static SendableCameraWrapper wrap(VideoSource source) {
     return m_wrappers.computeIfAbsent(source, SendableCameraWrapper::new);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
index c88256a..1bea28e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/Shuffleboard.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -16,7 +13,8 @@
  * example, programmers can specify a specific {@code boolean} value to be displayed with a toggle
  * button instead of the default colored box, or set custom colors for that box.
  *
- * <p>For example, displaying a boolean entry with a toggle button:</p>
+ * <p>For example, displaying a boolean entry with a toggle button:
+ *
  * <pre>{@code
  * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
  *   .add("My Boolean", false)
@@ -24,7 +22,8 @@
  *   .getEntry();
  * }</pre>
  *
- * <p>Changing the colors of the boolean box:</p>
+ * <p>Changing the colors of the boolean box:
+ *
  * <pre>{@code
  * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
  *   .add("My Boolean", false)
@@ -34,7 +33,8 @@
  * }</pre>
  *
  * <p>Specifying a parent layout. Note that the layout type must <i>always</i> be specified, even if
- * the layout has already been generated by a previously defined entry.</p>
+ * the layout has already been generated by a previously defined entry.
+ *
  * <pre>{@code
  * NetworkTableEntry myBoolean = Shuffleboard.getTab("Example Tab")
  *   .getLayout("List", "Example List")
@@ -43,12 +43,10 @@
  *   .getEntry();
  * }</pre>
  *
- * <p>Teams are encouraged to set up shuffleboard layouts at the start of the robot program.</p>
+ * <p>Teams are encouraged to set up shuffleboard layouts at the start of the robot program.
  */
 public final class Shuffleboard {
-  /**
-   * The name of the base NetworkTable into which all Shuffleboard data will be added.
-   */
+  /** The name of the base NetworkTable into which all Shuffleboard data will be added. */
   public static final String kBaseTableName = "/Shuffleboard";
 
   private static final ShuffleboardRoot root =
@@ -80,8 +78,8 @@
   }
 
   /**
-   * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i>
-   * is the number of tabs in the dashboard at the time this method is called.
+   * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i> is
+   * the number of tabs in the dashboard at the time this method is called.
    *
    * @param index the index of the tab to select
    */
@@ -99,10 +97,10 @@
   }
 
   /**
-   * Enables user control of widgets containing actuators: speed controllers, relays, etc. This
-   * should only be used when the robot is in test mode. IterativeRobotBase and SampleRobot are
-   * both configured to call this method when entering test mode; most users should not need to use
-   * this method directly.
+   * Enables user control of widgets containing actuators: motor controllers, relays, etc. This
+   * should only be used when the robot is in test mode. IterativeRobotBase and SampleRobot are both
+   * configured to call this method when entering test mode; most users should not need to use this
+   * method directly.
    */
   public static void enableActuatorWidgets() {
     root.enableActuatorWidgets();
@@ -111,8 +109,8 @@
   /**
    * Disables user control of widgets containing actuators. For safety reasons, actuators should
    * only be controlled while in test mode. IterativeRobotBase and SampleRobot are both configured
-   * to call this method when exiting in test mode; most users should not need to use
-   * this method directly.
+   * to call this method when exiting in test mode; most users should not need to use this method
+   * directly.
    */
   public static void disableActuatorWidgets() {
     update(); // Need to update to make sure the sendable builders are initialized
@@ -143,11 +141,10 @@
    *
    * <p>To avoid recording files overwriting each other, make sure to use unique recording file
    * names. File name formats accept templates for inserting the date and time when the recording
-   * started with the {@code ${date}} and {@code ${time}} templates, respectively. For example,
-   * the default format is {@code "recording-${time}"} and recording files created with it will have
+   * started with the {@code ${date}} and {@code ${time}} templates, respectively. For example, the
+   * default format is {@code "recording-${time}"} and recording files created with it will have
    * names like {@code "recording-2018.01.15.sbr"}. Users are <strong>strongly</strong> recommended
    * to use the {@code ${time}} template to ensure unique file names.
-   * </p>
    *
    * @param format the format for the
    * @see #clearRecordingFileNameFormat()
@@ -170,12 +167,12 @@
    * state to as critical as a total power loss or component failure. If Shuffleboard is recording,
    * the event will also be recorded.
    *
-   * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then
-   * no event will be sent and an error will be printed to the driver station.
+   * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then no
+   * event will be sent and an error will be printed to the driver station.
    *
-   * @param name        the name of the event
+   * @param name the name of the event
    * @param description a description of the event
-   * @param importance  the importance of the event
+   * @param importance the importance of the event
    */
   public static void addEventMarker(String name, String description, EventImportance importance) {
     recordingController.addEventMarker(name, description, importance);
@@ -186,11 +183,11 @@
    * state to as critical as a total power loss or component failure. If Shuffleboard is recording,
    * the event will also be recorded.
    *
-   * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then
-   * no event will be sent and an error will be printed to the driver station.
+   * <p>If {@code name} is {@code null} or empty, or {@code importance} is {@code null}, then no
+   * event will be sent and an error will be printed to the driver station.
    *
-   * @param name        the name of the event
-   * @param importance  the importance of the event
+   * @param name the name of the event
+   * @param importance the importance of the event
    */
   public static void addEventMarker(String name, EventImportance importance) {
     addEventMarker(name, "", importance);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
index 5d90396..fd7d541 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardComponent.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import java.util.Map;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.networktables.NetworkTable;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import java.util.Map;
 
 /**
  * A generic component in Shuffleboard.
@@ -58,9 +54,7 @@
     return m_title;
   }
 
-  /**
-   * Gets the custom properties for this component. May be null.
-   */
+  /** Gets the custom properties for this component. May be null. */
   final Map<String, Object> getProperties() {
     return m_properties;
   }
@@ -72,6 +66,7 @@
    * @param properties the properties for this component
    * @return this component
    */
+  @SuppressWarnings("unchecked")
   public final C withProperties(Map<String, Object> properties) {
     m_properties = properties;
     m_metadataDirty = true;
@@ -87,9 +82,10 @@
    * component there before the one with the specific position is sent.
    *
    * @param columnIndex the column in the tab to place this component
-   * @param rowIndex    the row in the tab to place this component
+   * @param rowIndex the row in the tab to place this component
    * @return this component
    */
+  @SuppressWarnings("unchecked")
   public final C withPosition(int columnIndex, int rowIndex) {
     m_column = columnIndex;
     m_row = rowIndex;
@@ -101,10 +97,11 @@
    * Sets the size of this component in the tab. This has no effect if this component is inside a
    * layout.
    *
-   * @param width  how many columns wide the component should be
+   * @param width how many columns wide the component should be
    * @param height how many rows high the component should be
    * @return this component
    */
+  @SuppressWarnings("unchecked")
   public final C withSize(int width, int height) {
     m_width = width;
     m_height = height;
@@ -127,14 +124,14 @@
     if (m_width <= 0 || m_height <= 0) {
       metaTable.getEntry("Size").delete();
     } else {
-      metaTable.getEntry("Size").setDoubleArray(new double[]{m_width, m_height});
+      metaTable.getEntry("Size").setDoubleArray(new double[] {m_width, m_height});
     }
 
     // Tile position
     if (m_column < 0 || m_row < 0) {
       metaTable.getEntry("Position").delete();
     } else {
-      metaTable.getEntry("Position").setDoubleArray(new double[]{m_column, m_row});
+      metaTable.getEntry("Position").setDoubleArray(new double[] {m_column, m_row});
     }
 
     // Custom properties
@@ -144,5 +141,4 @@
     }
     m_metadataDirty = false;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
index 9a89404..8b4e009 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardContainer.java
@@ -1,39 +1,34 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
+import edu.wpi.first.cscore.VideoSource;
+import edu.wpi.first.util.sendable.Sendable;
 import java.util.List;
 import java.util.NoSuchElementException;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.wpilibj.Sendable;
-
-/**
- * Common interface for objects that can contain shuffleboard components.
- */
+/** Common interface for objects that can contain shuffleboard components. */
 public interface ShuffleboardContainer extends ShuffleboardValue {
-
   /**
    * Gets the components that are direct children of this container.
+   *
+   * @return The components that are direct children of this container.
    */
   List<ShuffleboardComponent<?>> getComponents();
 
   /**
    * Gets the layout with the given type and title, creating it if it does not already exist at the
-   * time this method is called. Note: this method should only be used to use a layout type that
-   * is not already built into Shuffleboard. To use a layout built into Shuffleboard, use
-   * {@link #getLayout(String, LayoutType)} and the layouts in {@link BuiltInLayouts}.
+   * time this method is called. Note: this method should only be used to use a layout type that is
+   * not already built into Shuffleboard. To use a layout built into Shuffleboard, use {@link
+   * #getLayout(String, LayoutType)} and the layouts in {@link BuiltInLayouts}.
    *
    * @param title the title of the layout
-   * @param type  the type of the layout, eg "List Layout" or "Grid Layout"
+   * @param type the type of the layout, eg "List Layout" or "Grid Layout"
    * @return the layout
    * @see #getLayout(String, LayoutType)
    */
@@ -43,7 +38,7 @@
    * Gets the layout with the given type and title, creating it if it does not already exist at the
    * time this method is called.
    *
-   * @param title      the title of the layout
+   * @param title the title of the layout
    * @param layoutType the type of the layout, eg "List" or "Grid"
    * @return the layout
    */
@@ -72,11 +67,11 @@
   /**
    * Adds a widget to this container to display the given sendable.
    *
-   * @param title    the title of the widget
+   * @param title the title of the widget
    * @param sendable the sendable to display
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   ComplexWidget add(String title, Sendable sendable) throws IllegalArgumentException;
 
@@ -87,7 +82,7 @@
    * @param video the video stream to display
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   default ComplexWidget add(String title, VideoSource video) throws IllegalArgumentException {
     return add(title, SendableCameraWrapper.wrap(video));
@@ -99,7 +94,7 @@
    * @param sendable the sendable to display
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title, or if the sendable's name has not been specified
+   *     title, or if the sendable's name has not been specified
    */
   ComplexWidget add(Sendable sendable);
 
@@ -109,7 +104,7 @@
    * @param video the video to display
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this container with the same
-   *                                  title as the video source
+   *     title as the video source
    */
   default ComplexWidget add(VideoSource video) {
     return add(SendableCameraWrapper.wrap(video));
@@ -118,11 +113,11 @@
   /**
    * Adds a widget to this container to display the given data.
    *
-   * @param title        the title of the widget
+   * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    * @see #addPersistent(String, Object) add(String title, Object defaultValue)
    */
   SimpleWidget add(String title, Object defaultValue) throws IllegalArgumentException;
@@ -136,7 +131,7 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier)
       throws IllegalArgumentException;
@@ -150,7 +145,7 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier)
       throws IllegalArgumentException;
@@ -164,7 +159,7 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier)
       throws IllegalArgumentException;
@@ -178,7 +173,7 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<String[]> addStringArray(String title, Supplier<String[]> valueSupplier)
       throws IllegalArgumentException;
@@ -192,7 +187,7 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<double[]> addDoubleArray(String title, Supplier<double[]> valueSupplier)
       throws IllegalArgumentException;
@@ -206,7 +201,7 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<boolean[]> addBooleanArray(String title, Supplier<boolean[]> valueSupplier)
       throws IllegalArgumentException;
@@ -220,21 +215,21 @@
    * @param valueSupplier the supplier for values
    * @return a widget to display data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    */
   SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier)
       throws IllegalArgumentException;
 
   /**
-   * Adds a widget to this container to display a simple piece of data. Unlike
-   * {@link #add(String, Object)}, the value in the widget will be saved on the robot and will be
-   * used when the robot program next starts rather than {@code defaultValue}.
+   * Adds a widget to this container to display a simple piece of data. Unlike {@link #add(String,
+   * Object)}, the value in the widget will be saved on the robot and will be used when the robot
+   * program next starts rather than {@code defaultValue}.
    *
-   * @param title        the title of the widget
+   * @param title the title of the widget
    * @param defaultValue the default value of the widget
    * @return a widget to display the sendable data
    * @throws IllegalArgumentException if a widget already exists in this container with the given
-   *                                  title
+   *     title
    * @see #add(String, Object) add(String title, Object defaultValue)
    */
   default SimpleWidget addPersistent(String title, Object defaultValue)
@@ -243,5 +238,4 @@
     widget.getEntry().setPersistent();
     return widget;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
index 8af1d78..a864433 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstance.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import java.util.LinkedHashMap;
-import java.util.Map;
-import java.util.function.Consumer;
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
 
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import java.util.LinkedHashMap;
+import java.util.Map;
+import java.util.function.Consumer;
 
 final class ShuffleboardInstance implements ShuffleboardRoot {
   private final Map<String, ShuffleboardTab> m_tabs = new LinkedHashMap<>();
@@ -53,10 +49,8 @@
   @Override
   public void update() {
     if (m_tabsChanged) {
-      String[] tabTitles = m_tabs.values()
-          .stream()
-          .map(ShuffleboardTab::getTitle)
-          .toArray(String[]::new);
+      String[] tabTitles =
+          m_tabs.values().stream().map(ShuffleboardTab::getTitle).toArray(String[]::new);
       m_rootMetaTable.getEntry("Tabs").forceSetStringArray(tabTitles);
       m_tabsChanged = false;
     }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
index b6ed776..726f9cc 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardLayout.java
@@ -1,26 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.util.sendable.Sendable;
 import java.util.List;
 import java.util.NoSuchElementException;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.wpilibj.Sendable;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
-/**
- * A layout in a Shuffleboard tab. Layouts can contain widgets and other layouts.
- */
+/** A layout in a Shuffleboard tab. Layouts can contain widgets and other layouts. */
 public class ShuffleboardLayout extends ShuffleboardComponent<ShuffleboardLayout>
     implements ShuffleboardContainer {
   private final ContainerHelper m_helper = new ContainerHelper(this);
@@ -60,50 +54,43 @@
   }
 
   @Override
-  public SuppliedValueWidget<String> addString(String title,
-                                               Supplier<String> valueSupplier)
+  public SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addString(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<Double> addNumber(String title,
-                                               DoubleSupplier valueSupplier)
+  public SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addNumber(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<Boolean> addBoolean(String title,
-                                                 BooleanSupplier valueSupplier)
+  public SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addBoolean(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<String[]> addStringArray(String title,
-                                                      Supplier<String[]> valueSupplier)
-      throws IllegalArgumentException {
+  public SuppliedValueWidget<String[]> addStringArray(
+      String title, Supplier<String[]> valueSupplier) throws IllegalArgumentException {
     return m_helper.addStringArray(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<double[]> addDoubleArray(String title,
-                                                      Supplier<double[]> valueSupplier)
-      throws IllegalArgumentException {
+  public SuppliedValueWidget<double[]> addDoubleArray(
+      String title, Supplier<double[]> valueSupplier) throws IllegalArgumentException {
     return m_helper.addDoubleArray(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<boolean[]> addBooleanArray(String title,
-                                                        Supplier<boolean[]> valueSupplier)
-      throws IllegalArgumentException {
+  public SuppliedValueWidget<boolean[]> addBooleanArray(
+      String title, Supplier<boolean[]> valueSupplier) throws IllegalArgumentException {
     return m_helper.addBooleanArray(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<byte[]> addRaw(String title,
-                                            Supplier<byte[]> valueSupplier)
+  public SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addRaw(title, valueSupplier);
   }
@@ -117,5 +104,4 @@
       component.buildInto(table, metaTable.getSubTable(component.getTitle()));
     }
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
index a32af69..fa6cd08 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardRoot.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
 /**
- * The root of the data placed in Shuffleboard. It contains the tabs, but no data is placed
- * directly in the root.
+ * The root of the data placed in Shuffleboard. It contains the tabs, but no data is placed directly
+ * in the root.
  *
  * <p>This class is package-private to minimize API surface area.
  */
 interface ShuffleboardRoot {
-
   /**
    * Gets the tab with the given title, creating it if it does not already exist.
    *
@@ -23,24 +19,18 @@
    */
   ShuffleboardTab getTab(String title);
 
-  /**
-   * Updates all tabs.
-   */
+  /** Updates all tabs. */
   void update();
 
-  /**
-   * Enables all widgets in Shuffleboard that offer user control over actuators.
-   */
+  /** Enables all widgets in Shuffleboard that offer user control over actuators. */
   void enableActuatorWidgets();
 
-  /**
-   * Disables all widgets in Shuffleboard that offer user control over actuators.
-   */
+  /** Disables all widgets in Shuffleboard that offer user control over actuators. */
   void disableActuatorWidgets();
 
   /**
-   * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i>
-   * is the number of tabs in the dashboard at the time this method is called.
+   * Selects the tab in the dashboard with the given index in the range [0..n-1], where <i>n</i> is
+   * the number of tabs in the dashboard at the time this method is called.
    *
    * @param index the index of the tab to select
    */
@@ -52,5 +42,4 @@
    * @param title the title of the tab to select
    */
   void selectTab(String title);
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
index 5d575df..172d86f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTab.java
@@ -1,25 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.util.sendable.Sendable;
 import java.util.List;
 import java.util.NoSuchElementException;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 import java.util.function.Supplier;
 
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.wpilibj.Sendable;
-
 /**
- * Represents a tab in the Shuffleboard dashboard. Widgets can be added to the tab with
- * {@link #add(Sendable)}, {@link #add(String, Object)}, and {@link #add(String, Sendable)}. Widgets
- * can also be added to layouts with {@link #getLayout(String, String)}; layouts can be nested
+ * Represents a tab in the Shuffleboard dashboard. Widgets can be added to the tab with {@link
+ * #add(Sendable)}, {@link #add(String, Object)}, and {@link #add(String, Sendable)}. Widgets can
+ * also be added to layouts with {@link #getLayout(String, String)}; layouts can be nested
  * arbitrarily deep (note that too many levels may make deeper components unusable).
  */
 public final class ShuffleboardTab implements ShuffleboardContainer {
@@ -72,50 +68,43 @@
   }
 
   @Override
-  public SuppliedValueWidget<String> addString(String title,
-                                               Supplier<String> valueSupplier)
+  public SuppliedValueWidget<String> addString(String title, Supplier<String> valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addString(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<Double> addNumber(String title,
-                                               DoubleSupplier valueSupplier)
+  public SuppliedValueWidget<Double> addNumber(String title, DoubleSupplier valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addNumber(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<Boolean> addBoolean(String title,
-                                                 BooleanSupplier valueSupplier)
+  public SuppliedValueWidget<Boolean> addBoolean(String title, BooleanSupplier valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addBoolean(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<String[]> addStringArray(String title,
-                                                      Supplier<String[]> valueSupplier)
-      throws IllegalArgumentException {
+  public SuppliedValueWidget<String[]> addStringArray(
+      String title, Supplier<String[]> valueSupplier) throws IllegalArgumentException {
     return m_helper.addStringArray(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<double[]> addDoubleArray(String title,
-                                                      Supplier<double[]> valueSupplier)
-      throws IllegalArgumentException {
+  public SuppliedValueWidget<double[]> addDoubleArray(
+      String title, Supplier<double[]> valueSupplier) throws IllegalArgumentException {
     return m_helper.addDoubleArray(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<boolean[]> addBooleanArray(String title,
-                                                        Supplier<boolean[]> valueSupplier)
-      throws IllegalArgumentException {
+  public SuppliedValueWidget<boolean[]> addBooleanArray(
+      String title, Supplier<boolean[]> valueSupplier) throws IllegalArgumentException {
     return m_helper.addBooleanArray(title, valueSupplier);
   }
 
   @Override
-  public SuppliedValueWidget<byte[]> addRaw(String title,
-                                            Supplier<byte[]> valueSupplier)
+  public SuppliedValueWidget<byte[]> addRaw(String title, Supplier<byte[]> valueSupplier)
       throws IllegalArgumentException {
     return m_helper.addRaw(title, valueSupplier);
   }
@@ -128,5 +117,4 @@
       component.buildInto(tabTable, metaTable.getSubTable(component.getTitle()));
     }
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
index 8bfa6c3..88f4389 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardValue.java
@@ -1,18 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
 import edu.wpi.first.networktables.NetworkTable;
 
 interface ShuffleboardValue {
-
   /**
    * Gets the title of this Shuffleboard value.
+   *
+   * @return The title of this Shuffleboard value.
    */
   String getTitle();
 
@@ -20,12 +18,10 @@
    * Builds the entries for this value.
    *
    * @param parentTable the table containing all the data for the parent. Values that require a
-   *                    complex entry or table structure should call {@code
-   *                    parentTable.getSubTable(getTitle())} to get the table to put data into.
-   *                    Values that only use a single entry should call {@code
-   *                    parentTable.getEntry(getTitle())} to get that entry.
-   * @param metaTable   the table containing all the metadata for this value and its sub-values
+   *     complex entry or table structure should call {@code parentTable.getSubTable(getTitle())} to
+   *     get the table to put data into. Values that only use a single entry should call {@code
+   *     parentTable.getEntry(getTitle())} to get that entry.
+   * @param metaTable the table containing all the metadata for this value and its sub-values
    */
   void buildInto(NetworkTable parentTable, NetworkTable metaTable);
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
index c929f4a..39ce036 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardWidget.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -16,7 +13,6 @@
  */
 abstract class ShuffleboardWidget<W extends ShuffleboardWidget<W>>
     extends ShuffleboardComponent<W> {
-
   ShuffleboardWidget(ShuffleboardContainer parent, String title) {
     super(parent, title);
   }
@@ -36,15 +32,15 @@
   /**
    * 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 third-party plugin). To use a widget that
-   * is built into Shuffleboard, use {@link #withWidget(WidgetType)} and {@link BuiltInWidgets}.
+   * Shuffleboard (i.e. one that comes with a custom or third-party plugin). To use a widget that is
+   * built into Shuffleboard, use {@link #withWidget(WidgetType)} and {@link BuiltInWidgets}.
    *
    * @param widgetType the type of the widget used to display the data
    * @return this widget object
    */
+  @SuppressWarnings("unchecked")
   public final W withWidget(String widgetType) {
     setType(widgetType);
     return (W) this;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
index c645baa..2043432 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SimpleWidget.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 
-/**
- * A Shuffleboard widget that handles a single data point such as a number or string.
- */
+/** A Shuffleboard widget that handles a single data point such as a number or string. */
 public final class SimpleWidget extends ShuffleboardWidget<SimpleWidget> {
   private NetworkTableEntry m_entry;
 
@@ -22,6 +17,8 @@
 
   /**
    * Gets the NetworkTable entry that contains the data for this widget.
+   *
+   * @return The NetworkTable entry that contains the data for this widget.
    */
   public NetworkTableEntry getEntry() {
     if (m_entry == null) {
@@ -46,5 +43,4 @@
     ShuffleboardTab tab = (ShuffleboardTab) parent;
     tab.getRoot().update();
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
index c7c9e8b..497b30f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidget.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import java.util.function.BiConsumer;
-import java.util.function.Supplier;
-
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
+import java.util.function.BiConsumer;
+import java.util.function.Supplier;
 
 /**
  * A Shuffleboard widget whose value is provided by user code.
@@ -25,15 +21,16 @@
   /**
    * Package-private constructor for use by the Shuffleboard API.
    *
-   * @param parent   the parent container for the widget
-   * @param title    the title of the widget
+   * @param parent the parent container for the widget
+   * @param title the title of the widget
    * @param supplier the supplier for values to place in the NetworkTable entry
-   * @param setter   the function for placing values in the NetworkTable entry
+   * @param setter the function for placing values in the NetworkTable entry
    */
-  SuppliedValueWidget(ShuffleboardContainer parent,
-                      String title,
-                      Supplier<T> supplier,
-                      BiConsumer<NetworkTableEntry, T> setter) {
+  SuppliedValueWidget(
+      ShuffleboardContainer parent,
+      String title,
+      Supplier<T> supplier,
+      BiConsumer<NetworkTableEntry, T> setter) {
     super(parent, title);
     this.m_supplier = supplier;
     this.m_setter = setter;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
index 1e8242f..3033f8e 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/WidgetType.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
@@ -16,6 +13,8 @@
 public interface WidgetType {
   /**
    * Gets the string type of the widget as defined by that widget in Shuffleboard.
+   *
+   * @return The string type of the widget.
    */
   String getWidgetName();
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
new file mode 100644
index 0000000..c50c2f8
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADXL345_I2C;
+import edu.wpi.first.wpilibj.ADXL345_SPI;
+import java.util.Objects;
+
+public class ADXL345Sim {
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL345Sim(ADXL345_SPI device) {
+    SimDeviceSim simDevice = new SimDeviceSim("Accel:ADXL345_SPI" + "[" + device.getPort() + "]");
+    initSim(simDevice);
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL345Sim(ADXL345_I2C device) {
+    SimDeviceSim simDevice =
+        new SimDeviceSim(
+            "Accel:ADXL345_I2C" + "[" + device.getPort() + "," + device.getDeviceAddress() + "]");
+    initSim(simDevice);
+  }
+
+  private void initSim(SimDeviceSim simDevice) {
+    Objects.requireNonNull(simDevice);
+
+    m_simX = simDevice.getDouble("x");
+    m_simY = simDevice.getDouble("y");
+    m_simZ = simDevice.getDouble("z");
+
+    Objects.requireNonNull(m_simX);
+    Objects.requireNonNull(m_simY);
+    Objects.requireNonNull(m_simZ);
+  }
+
+  public void setX(double x) {
+    m_simX.set(x);
+  }
+
+  public void setY(double y) {
+    m_simY.set(y);
+  }
+
+  public void setZ(double z) {
+    m_simZ.set(z);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
new file mode 100644
index 0000000..76df8bd
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.wpilibj.ADXL362;
+import java.util.Objects;
+
+public class ADXL362Sim {
+  protected SimDouble m_simX;
+  protected SimDouble m_simY;
+  protected SimDouble m_simZ;
+
+  /**
+   * Constructor.
+   *
+   * @param device The device to simulate
+   */
+  public ADXL362Sim(ADXL362 device) {
+    SimDeviceSim wrappedSimDevice =
+        new SimDeviceSim("Accel:ADXL362" + "[" + device.getPort() + "]");
+    initSim(wrappedSimDevice);
+  }
+
+  private void initSim(SimDeviceSim wrappedSimDevice) {
+    m_simX = wrappedSimDevice.getDouble("x");
+    m_simY = wrappedSimDevice.getDouble("y");
+    m_simZ = wrappedSimDevice.getDouble("z");
+
+    Objects.requireNonNull(m_simX);
+    Objects.requireNonNull(m_simY);
+    Objects.requireNonNull(m_simZ);
+  }
+
+  public void setX(double x) {
+    m_simX.set(x);
+  }
+
+  public void setY(double y) {
+    m_simY.set(y);
+  }
+
+  public void setZ(double z) {
+    m_simZ.set(z);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
index 950efef..27e06a3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 
-/**
- * Class to control a simulated ADXRS450 gyroscope.
- */
-@SuppressWarnings("TypeName")
+/** Class to control a simulated ADXRS450 gyroscope. */
+@SuppressWarnings({"TypeName", "AbbreviationAsWordInName"})
 public class ADXRS450_GyroSim {
   private final SimDouble m_simAngle;
   private final SimDouble m_simRate;
@@ -24,9 +19,9 @@
    * @param gyro ADXRS450_Gyro to simulate
    */
   public ADXRS450_GyroSim(ADXRS450_Gyro gyro) {
-    SimDeviceSim wrappedSimDevice = new SimDeviceSim("ADXRS450_Gyro" + "[" + gyro.getPort() + "]");
-    m_simAngle = wrappedSimDevice.getDouble("Angle");
-    m_simRate = wrappedSimDevice.getDouble("Rate");
+    SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADXRS450" + "[" + gyro.getPort() + "]");
+    m_simAngle = wrappedSimDevice.getDouble("angle_x");
+    m_simRate = wrappedSimDevice.getDouble("rate_x");
   }
 
   /**
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java
index 81c9a7b..bf543c0 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -13,15 +10,11 @@
 import edu.wpi.first.wpilibj.AddressableLED;
 import java.util.NoSuchElementException;
 
-/**
- * Class to control a simulated addressable LED.
- */
+/** Class to control a simulated addressable LED. */
 public class AddressableLEDSim {
   private final int m_index;
 
-  /**
-   * Constructs for the first addressable LED.
-   */
+  /** Constructs for the first addressable LED. */
   public AddressableLEDSim() {
     m_index = 0;
   }
@@ -57,8 +50,8 @@
   }
 
   /**
-   * Creates an AddressableLEDSim for a simulated index.
-   * The index is incremented for each simulated AddressableLED.
+   * Creates an AddressableLEDSim for a simulated index. The index is incremented for each simulated
+   * AddressableLED.
    *
    * @param index simulator index
    * @return Simulated object
@@ -67,61 +60,161 @@
     return new AddressableLEDSim(index);
   }
 
+  /**
+   * Register a callback on the Initialized property.
+   *
+   * @param callback the callback that will be called whenever the Initialized property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AddressableLEDDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check if initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return AddressableLEDDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Change the Initialized value of the LED strip.
+   *
+   * @param initialized the new value
+   */
   public void setInitialized(boolean initialized) {
     AddressableLEDDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback on the output port.
+   *
+   * @param callback the callback that will be called whenever the output port is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerOutputPortCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AddressableLEDDataJNI.registerOutputPortCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelOutputPortCallback);
   }
+
+  /**
+   * Get the output port.
+   *
+   * @return the output port
+   */
   public int getOutputPort() {
     return AddressableLEDDataJNI.getOutputPort(m_index);
   }
+
+  /**
+   * Change the output port.
+   *
+   * @param outputPort the new output port
+   */
   public void setOutputPort(int outputPort) {
     AddressableLEDDataJNI.setOutputPort(m_index, outputPort);
   }
 
+  /**
+   * Register a callback on the length.
+   *
+   * @param callback the callback that will be called whenever the length is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerLengthCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AddressableLEDDataJNI.registerLengthCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelLengthCallback);
   }
+
+  /**
+   * Get the length of the LED strip.
+   *
+   * @return the length
+   */
   public int getLength() {
     return AddressableLEDDataJNI.getLength(m_index);
   }
+
+  /**
+   * Change the length of the LED strip.
+   *
+   * @param length the new value
+   */
   public void setLength(int length) {
     AddressableLEDDataJNI.setLength(m_index, length);
   }
 
+  /**
+   * Register a callback on whether the LEDs are running.
+   *
+   * @param callback the callback that will be called whenever the LED state is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerRunningCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AddressableLEDDataJNI.registerRunningCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelRunningCallback);
   }
+
+  /**
+   * Check if the LEDs are running.
+   *
+   * @return true if they are
+   */
   public boolean getRunning() {
     return AddressableLEDDataJNI.getRunning(m_index);
   }
+
+  /**
+   * Change whether the LEDs are active.
+   *
+   * @param running the new value
+   */
   public void setRunning(boolean running) {
     AddressableLEDDataJNI.setRunning(m_index, running);
   }
 
+  /**
+   * Register a callback on the LED data.
+   *
+   * @param callback the callback that will be called whenever the LED data is changed
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerDataCallback(ConstBufferCallback callback) {
     int uid = AddressableLEDDataJNI.registerDataCallback(m_index, callback);
     return new CallbackStore(m_index, uid, AddressableLEDDataJNI::cancelDataCallback);
   }
+
+  /**
+   * Get the LED data.
+   *
+   * @return the LED data
+   */
   public byte[] getData() {
     return AddressableLEDDataJNI.getData(m_index);
   }
+
+  /**
+   * Change the LED data.
+   *
+   * @param data the new data
+   */
   public void setData(byte[] data) {
     AddressableLEDDataJNI.setData(m_index, data);
   }
 
+  /** Reset all simulation data for this LED object. */
   public void resetData() {
     AddressableLEDDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
index e1b541f..6b11673 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSim.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.AnalogEncoder;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 
-/**
- * Class to control a simulated analog encoder.
- */
+/** Class to control a simulated analog encoder. */
 public class AnalogEncoderSim {
   private final SimDouble m_simPosition;
 
@@ -23,7 +18,8 @@
    * @param encoder AnalogEncoder to simulate
    */
   public AnalogEncoderSim(AnalogEncoder encoder) {
-    SimDeviceSim wrappedSimDevice = new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]");
+    SimDeviceSim wrappedSimDevice =
+        new SimDeviceSim("AnalogEncoder" + "[" + encoder.getChannel() + "]");
     m_simPosition = wrappedSimDevice.getDouble("Position");
   }
 
@@ -47,6 +43,8 @@
 
   /**
    * Get the simulated position.
+   *
+   * @return The simulated position.
    */
   public double getTurns() {
     return m_simPosition.get();
@@ -54,6 +52,8 @@
 
   /**
    * Get the position as a {@link Rotation2d}.
+   *
+   * @return The position as a {@link Rotation2d}.
    */
   public Rotation2d getPosition() {
     return Rotation2d.fromDegrees(getTurns() * 360.0);
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java
index b204fae..87217aa 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.wpilibj.AnalogGyro;
 
-/**
- * Class to control a simulated analog gyro.
- */
+/** Class to control a simulated analog gyro. */
 public class AnalogGyroSim {
   private final int m_index;
 
@@ -35,39 +30,100 @@
     m_index = channel;
   }
 
+  /**
+   * Register a callback on the angle.
+   *
+   * @param callback the callback that will be called whenever the angle changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerAngleCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogGyroDataJNI.registerAngleCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelAngleCallback);
   }
+
+  /**
+   * Get the current angle of the gyro.
+   *
+   * @return the angle measured by the gyro
+   */
   public double getAngle() {
     return AnalogGyroDataJNI.getAngle(m_index);
   }
+
+  /**
+   * Change the angle measured by the gyro.
+   *
+   * @param angle the new value
+   */
   public void setAngle(double angle) {
     AnalogGyroDataJNI.setAngle(m_index, angle);
   }
 
+  /**
+   * Register a callback on the rate.
+   *
+   * @param callback the callback that will be called whenever the rate changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerRateCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogGyroDataJNI.registerRateCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelRateCallback);
   }
+
+  /**
+   * Get the rate of angle change on this gyro.
+   *
+   * @return the rate
+   */
   public double getRate() {
     return AnalogGyroDataJNI.getRate(m_index);
   }
+
+  /**
+   * Change the rate of the gyro.
+   *
+   * @param rate the new rate
+   */
   public void setRate(double rate) {
     AnalogGyroDataJNI.setRate(m_index, rate);
   }
 
+  /**
+   * Register a callback on whether the gyro is initialized.
+   *
+   * @param callback the callback that will be called whenever the gyro is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogGyroDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogGyroDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check if the gyro is initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return AnalogGyroDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Set whether this gyro is initialized.
+   *
+   * @param initialized the new value
+   */
   public void setInitialized(boolean initialized) {
     AnalogGyroDataJNI.setInitialized(m_index, initialized);
   }
 
+  /** Reset all simulation data for this object. */
   public void resetData() {
     AnalogGyroDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java
index 1709d6a..87dce71 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogInputSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.wpilibj.AnalogInput;
 
-/**
- * Class to control a simulated analog input.
- */
+/** Class to control a simulated analog input. */
 public class AnalogInputSim {
   private final int m_index;
 
@@ -35,105 +30,293 @@
     m_index = channel;
   }
 
+  /**
+   * Register a callback on whether the analog input is initialized.
+   *
+   * @param callback the callback that will be called whenever the analog input is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check if this analog input has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return AnalogInDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Change whether this analog input has been initialized.
+   *
+   * @param initialized the new value
+   */
   public void setInitialized(boolean initialized) {
     AnalogInDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback on the number of average bits.
+   *
+   * @param callback the callback that will be called whenever the number of average bits is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerAverageBitsCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerAverageBitsCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAverageBitsCallback);
   }
+
+  /**
+   * Get the number of average bits.
+   *
+   * @return the number of average bits
+   */
   public int getAverageBits() {
     return AnalogInDataJNI.getAverageBits(m_index);
   }
+
+  /**
+   * Change the number of average bits.
+   *
+   * @param averageBits the new value
+   */
   public void setAverageBits(int averageBits) {
     AnalogInDataJNI.setAverageBits(m_index, averageBits);
   }
 
-  public CallbackStore registerOversampleBitsCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the amount of oversampling bits.
+   *
+   * @param callback the callback that will be called whenever the oversampling bits are changed.
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerOversampleBitsCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerOversampleBitsCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelOversampleBitsCallback);
   }
+
+  /**
+   * Get the amount of oversampling bits.
+   *
+   * @return the amount of oversampling bits
+   */
   public int getOversampleBits() {
     return AnalogInDataJNI.getOversampleBits(m_index);
   }
+
+  /**
+   * Change the amount of oversampling bits.
+   *
+   * @param oversampleBits the new value
+   */
   public void setOversampleBits(int oversampleBits) {
     AnalogInDataJNI.setOversampleBits(m_index, oversampleBits);
   }
 
+  /**
+   * Register a callback on the voltage.
+   *
+   * @param callback the callback that will be called whenever the voltage is changed.
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelVoltageCallback);
   }
+
+  /**
+   * Get the voltage.
+   *
+   * @return the voltage
+   */
   public double getVoltage() {
     return AnalogInDataJNI.getVoltage(m_index);
   }
+
+  /**
+   * Change the voltage.
+   *
+   * @param voltage the new value
+   */
   public void setVoltage(double voltage) {
     AnalogInDataJNI.setVoltage(m_index, voltage);
   }
 
-  public CallbackStore registerAccumulatorInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogInDataJNI.registerAccumulatorInitializedCallback(m_index, callback, initialNotify);
+  /**
+   * Register a callback on whether the accumulator is initialized.
+   *
+   * @param callback the callback that will be called whenever the accumulator is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerAccumulatorInitializedCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        AnalogInDataJNI.registerAccumulatorInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorInitializedCallback);
   }
+
+  /**
+   * Check if the accumulator has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getAccumulatorInitialized() {
     return AnalogInDataJNI.getAccumulatorInitialized(m_index);
   }
+
+  /**
+   * Change whether the accumulator has been initialized.
+   *
+   * @param accumulatorInitialized the new value
+   */
   public void setAccumulatorInitialized(boolean accumulatorInitialized) {
     AnalogInDataJNI.setAccumulatorInitialized(m_index, accumulatorInitialized);
   }
 
-  public CallbackStore registerAccumulatorValueCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the accumulator value.
+   *
+   * @param callback the callback that will be called whenever the accumulator value is changed.
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerAccumulatorValueCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerAccumulatorValueCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorValueCallback);
   }
+
+  /**
+   * Get the accumulator value.
+   *
+   * @return the accumulator value
+   */
   public long getAccumulatorValue() {
     return AnalogInDataJNI.getAccumulatorValue(m_index);
   }
+
+  /**
+   * Change the accumulator value.
+   *
+   * @param accumulatorValue the new value
+   */
   public void setAccumulatorValue(long accumulatorValue) {
     AnalogInDataJNI.setAccumulatorValue(m_index, accumulatorValue);
   }
 
-  public CallbackStore registerAccumulatorCountCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the accumulator count.
+   *
+   * @param callback the callback that will be called whenever the accumulator count is changed.
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerAccumulatorCountCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerAccumulatorCountCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorCountCallback);
   }
+
+  /**
+   * Get the accumulator count.
+   *
+   * @return the accumulator count.
+   */
   public long getAccumulatorCount() {
     return AnalogInDataJNI.getAccumulatorCount(m_index);
   }
+
+  /**
+   * Change the accumulator count.
+   *
+   * @param accumulatorCount the new count.
+   */
   public void setAccumulatorCount(long accumulatorCount) {
     AnalogInDataJNI.setAccumulatorCount(m_index, accumulatorCount);
   }
 
-  public CallbackStore registerAccumulatorCenterCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the accumulator center.
+   *
+   * @param callback the callback that will be called whenever the accumulator center is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerAccumulatorCenterCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerAccumulatorCenterCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorCenterCallback);
   }
+
+  /**
+   * Get the accumulator center.
+   *
+   * @return the accumulator center
+   */
   public int getAccumulatorCenter() {
     return AnalogInDataJNI.getAccumulatorCenter(m_index);
   }
+
+  /**
+   * Change the accumulator center.
+   *
+   * @param accumulatorCenter the new center
+   */
   public void setAccumulatorCenter(int accumulatorCenter) {
     AnalogInDataJNI.setAccumulatorCenter(m_index, accumulatorCenter);
   }
 
-  public CallbackStore registerAccumulatorDeadbandCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the accumulator deadband.
+   *
+   * @param callback the callback that will be called whenever the accumulator deadband is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerAccumulatorDeadbandCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogInDataJNI.registerAccumulatorDeadbandCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogInDataJNI::cancelAccumulatorDeadbandCallback);
   }
+
+  /**
+   * Get the accumulator deadband.
+   *
+   * @return the accumulator deadband
+   */
   public int getAccumulatorDeadband() {
     return AnalogInDataJNI.getAccumulatorDeadband(m_index);
   }
+
+  /**
+   * Change the accumulator deadband.
+   *
+   * @param accumulatorDeadband the new deadband
+   */
   public void setAccumulatorDeadband(int accumulatorDeadband) {
     AnalogInDataJNI.setAccumulatorDeadband(m_index, accumulatorDeadband);
   }
 
+  /** Reset all simulation data for this object. */
   public void resetData() {
     AnalogInDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java
index 077a2eb..985fcd9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.wpilibj.AnalogOutput;
 
-/**
- * Class to control a simulated analog output.
- */
+/** Class to control a simulated analog output. */
 public class AnalogOutputSim {
   private final int m_index;
 
@@ -35,28 +30,69 @@
     m_index = channel;
   }
 
+  /**
+   * Register a callback to be run whenever the voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogOutDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogOutDataJNI::cancelVoltageCallback);
   }
+
+  /**
+   * Read the analog output voltage.
+   *
+   * @return the voltage on this analog output
+   */
   public double getVoltage() {
     return AnalogOutDataJNI.getVoltage(m_index);
   }
+
+  /**
+   * Set the analog output voltage.
+   *
+   * @param voltage the new voltage on this analog output
+   */
   public void setVoltage(double voltage) {
     AnalogOutDataJNI.setVoltage(m_index, voltage);
   }
 
+  /**
+   * Register a callback to be run when this analog output is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogOutDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogOutDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether this analog output has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return AnalogOutDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether this analog output has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
     AnalogOutDataJNI.setInitialized(m_index, initialized);
   }
 
+  /** Reset all simulation data on this object. */
   public void resetData() {
     AnalogOutDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java
index 6916fdc..58d51b1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -12,9 +9,7 @@
 import edu.wpi.first.wpilibj.AnalogTrigger;
 import java.util.NoSuchElementException;
 
-/**
- * Class to control a simulated analog trigger.
- */
+/** Class to control a simulated analog trigger. */
 public class AnalogTriggerSim {
   private final int m_index;
 
@@ -47,8 +42,8 @@
   }
 
   /**
-   * Creates an AnalogTriggerSim for a simulated index.
-   * The index is incremented for each simulated AnalogTrigger.
+   * Creates an AnalogTriggerSim for a simulated index. The index is incremented for each simulated
+   * AnalogTrigger.
    *
    * @param index simulator index
    * @return Simulated object
@@ -57,39 +52,104 @@
     return new AnalogTriggerSim(index);
   }
 
+  /**
+   * Register a callback on whether the analog trigger is initialized.
+   *
+   * @param callback the callback that will be called whenever the analog trigger is initialized
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AnalogTriggerDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check if this analog trigger has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return AnalogTriggerDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Change whether this analog trigger has been initialized.
+   *
+   * @param initialized the new value
+   */
   public void setInitialized(boolean initialized) {
     AnalogTriggerDataJNI.setInitialized(m_index, initialized);
   }
 
-  public CallbackStore registerTriggerLowerBoundCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogTriggerDataJNI.registerTriggerLowerBoundCallback(m_index, callback, initialNotify);
+  /**
+   * Register a callback on the lower bound.
+   *
+   * @param callback the callback that will be called whenever the lower bound is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerTriggerLowerBoundCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        AnalogTriggerDataJNI.registerTriggerLowerBoundCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelTriggerLowerBoundCallback);
   }
+
+  /**
+   * Get the lower bound.
+   *
+   * @return the lower bound
+   */
   public double getTriggerLowerBound() {
     return AnalogTriggerDataJNI.getTriggerLowerBound(m_index);
   }
+
+  /**
+   * Change the lower bound.
+   *
+   * @param triggerLowerBound the new lower bound
+   */
   public void setTriggerLowerBound(double triggerLowerBound) {
     AnalogTriggerDataJNI.setTriggerLowerBound(m_index, triggerLowerBound);
   }
 
-  public CallbackStore registerTriggerUpperBoundCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = AnalogTriggerDataJNI.registerTriggerUpperBoundCallback(m_index, callback, initialNotify);
+  /**
+   * Register a callback on the upper bound.
+   *
+   * @param callback the callback that will be called whenever the upper bound is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerTriggerUpperBoundCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        AnalogTriggerDataJNI.registerTriggerUpperBoundCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AnalogTriggerDataJNI::cancelTriggerUpperBoundCallback);
   }
+
+  /**
+   * Get the upper bound.
+   *
+   * @return the upper bound
+   */
   public double getTriggerUpperBound() {
     return AnalogTriggerDataJNI.getTriggerUpperBound(m_index);
   }
+
+  /**
+   * Change the upper bound.
+   *
+   * @param triggerUpperBound the new upper bound
+   */
   public void setTriggerUpperBound(double triggerUpperBound) {
     AnalogTriggerDataJNI.setTriggerUpperBound(m_index, triggerUpperBound);
   }
 
+  /** Reset all simulation data for this object. */
   public void resetData() {
     AnalogTriggerDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java
index 0150ed4..78c9888 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BatterySim.java
@@ -1,34 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.RobotController;
-
+/** A utility class to simulate the robot battery. */
 public final class BatterySim {
   private BatterySim() {
     // Utility class
   }
 
   /**
-   * 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.
+   * 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
+   * edu.wpi.first.wpilibj.RobotController#getBatteryVoltage()} method.
    *
    * @param nominalVoltage The nominal battery voltage. Usually 12v.
-   * @param resistanceOhms The forward resistance of the battery. Most batteries are at or
-   *                       below 20 milliohms.
-   * @param currents       The currents drawn from the battery.
+   * @param resistanceOhms 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.
    */
-  public static double calculateLoadedBatteryVoltage(double nominalVoltage,
-                                                     double resistanceOhms,
-                                                     double... currents) {
+  public static double calculateLoadedBatteryVoltage(
+      double nominalVoltage, double resistanceOhms, double... currents) {
     double retval = nominalVoltage;
     for (var current : currents) {
       retval -= current * resistanceOhms;
@@ -37,11 +31,10 @@
   }
 
   /**
-   * 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)
+   * 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
+   * edu.wpi.first.wpilibj.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.
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java
index 8905542..5714a2d 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/BuiltInAccelerometerSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -11,15 +8,11 @@
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.wpilibj.BuiltInAccelerometer;
 
-/**
- * Class to control a simulated built-in accelerometer.
- */
+/** Class to control a simulated built-in accelerometer. */
 public class BuiltInAccelerometerSim {
   private final int m_index;
 
-  /**
-   * Constructs for the first built-in accelerometer.
-   */
+  /** Constructs for the first built-in accelerometer. */
   public BuiltInAccelerometerSim() {
     m_index = 0;
   }
@@ -34,61 +27,165 @@
     m_index = 0;
   }
 
+  /**
+   * Register a callback to be run when this accelerometer activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelActiveCallback);
   }
+
+  /**
+   * Check whether the accelerometer is active.
+   *
+   * @return true if active
+   */
   public boolean getActive() {
     return AccelerometerDataJNI.getActive(m_index);
   }
+
+  /**
+   * Define whether this accelerometer is active.
+   *
+   * @param active the new state
+   */
   public void setActive(boolean active) {
     AccelerometerDataJNI.setActive(m_index, active);
   }
 
+  /**
+   * Register a callback to be run whenever the range changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelRangeCallback);
   }
+
+  /**
+   * Check the range of this accelerometer.
+   *
+   * @return the accelerometer range
+   */
   public int getRange() {
     return AccelerometerDataJNI.getRange(m_index);
   }
+
+  /**
+   * Change the range of this accelerometer.
+   *
+   * @param range the new accelerometer range
+   */
   public void setRange(int range) {
     AccelerometerDataJNI.setRange(m_index, range);
   }
 
+  /**
+   * Register a callback to be run whenever the X axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelXCallback);
   }
+
+  /**
+   * Measure the X axis value.
+   *
+   * @return the X axis measurement
+   */
   public double getX() {
     return AccelerometerDataJNI.getX(m_index);
   }
+
+  /**
+   * Change the X axis value of the accelerometer.
+   *
+   * @param x the new reading of the X axis
+   */
+  @SuppressWarnings("ParameterName")
   public void setX(double x) {
     AccelerometerDataJNI.setX(m_index, x);
   }
 
+  /**
+   * Register a callback to be run whenever the Y axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelYCallback);
   }
+
+  /**
+   * Measure the Y axis value.
+   *
+   * @return the Y axis measurement
+   */
   public double getY() {
     return AccelerometerDataJNI.getY(m_index);
   }
+
+  /**
+   * Change the Y axis value of the accelerometer.
+   *
+   * @param y the new reading of the Y axis
+   */
+  @SuppressWarnings("ParameterName")
   public void setY(double y) {
     AccelerometerDataJNI.setY(m_index, y);
   }
 
+  /**
+   * Register a callback to be run whenever the Z axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = AccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, AccelerometerDataJNI::cancelZCallback);
   }
+
+  /**
+   * Measure the Z axis value.
+   *
+   * @return the Z axis measurement
+   */
   public double getZ() {
     return AccelerometerDataJNI.getZ(m_index);
   }
+
+  /**
+   * Change the Z axis value of the accelerometer.
+   *
+   * @param z the new reading of the Z axis
+   */
+  @SuppressWarnings("ParameterName")
   public void setZ(double z) {
     AccelerometerDataJNI.setZ(m_index, z);
   }
 
+  /** Reset all simulation data of this object. */
   public void resetData() {
     AccelerometerDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CTREPCMSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CTREPCMSim.java
new file mode 100644
index 0000000..12ae153
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CTREPCMSim.java
@@ -0,0 +1,239 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.CTREPCMDataJNI;
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.wpilibj.PneumaticsBase;
+import edu.wpi.first.wpilibj.SensorUtil;
+
+/** Class to control a simulated Pneumatic Control Module (PCM). */
+@SuppressWarnings("AbbreviationAsWordInName")
+public class CTREPCMSim {
+  private final int m_index;
+
+  /** Constructs for the default PCM. */
+  public CTREPCMSim() {
+    m_index = SensorUtil.getDefaultCTREPCMModule();
+  }
+
+  /**
+   * Constructs from a PCM module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public CTREPCMSim(int module) {
+    m_index = module;
+  }
+
+  /**
+   * Constructs from a Compressor object.
+   *
+   * @param module PCM module to simulate
+   */
+  public CTREPCMSim(PneumaticsBase module) {
+    m_index = module.getModuleNumber();
+  }
+
+  /**
+   * Register a callback to be run when the solenoid output on a channel changes.
+   *
+   * @param channel the channel to monitor
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerSolenoidOutputCallback(
+      int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        CTREPCMDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, CTREPCMDataJNI::cancelSolenoidOutputCallback);
+  }
+
+  /**
+   * Check the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @return the solenoid output
+   */
+  public boolean getSolenoidOutput(int channel) {
+    return CTREPCMDataJNI.getSolenoidOutput(m_index, channel);
+  }
+
+  /**
+   * Change the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @param solenoidOutput the new solenoid output
+   */
+  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
+    CTREPCMDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
+  }
+
+  /**
+   * Register a callback to be run when the compressor is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether the compressor has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return CTREPCMDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether the compressor has been initialized.
+   *
+   * @param initialized whether the compressor is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    CTREPCMDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run when the compressor activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerCompressorOnCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelCompressorOnCallback);
+  }
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  public boolean getCompressorOn() {
+    return CTREPCMDataJNI.getCompressorOn(m_index);
+  }
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  public void setCompressorOn(boolean compressorOn) {
+    CTREPCMDataJNI.setCompressorOn(m_index, compressorOn);
+  }
+
+  /**
+   * Register a callback to be run whenever the closed loop state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerClosedLoopEnabledCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelClosedLoopEnabledCallback);
+  }
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return true if active
+   */
+  public boolean getClosedLoopEnabled() {
+    return CTREPCMDataJNI.getClosedLoopEnabled(m_index);
+  }
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param closedLoopEnabled whether the control loop is active
+   */
+  public void setClosedLoopEnabled(boolean closedLoopEnabled) {
+    CTREPCMDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled);
+  }
+
+  /**
+   * Register a callback to be run whenever the pressure switch value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerPressureSwitchCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelPressureSwitchCallback);
+  }
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  public boolean getPressureSwitch() {
+    return CTREPCMDataJNI.getPressureSwitch(m_index);
+  }
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  public void setPressureSwitch(boolean pressureSwitch) {
+    CTREPCMDataJNI.setPressureSwitch(m_index, pressureSwitch);
+  }
+
+  /**
+   * Register a callback to be run whenever the compressor current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerCompressorCurrentCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = CTREPCMDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, CTREPCMDataJNI::cancelCompressorCurrentCallback);
+  }
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  public double getCompressorCurrent() {
+    return CTREPCMDataJNI.getCompressorCurrent(m_index);
+  }
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  public void setCompressorCurrent(double compressorCurrent) {
+    CTREPCMDataJNI.setCompressorCurrent(m_index, compressorCurrent);
+  }
+
+  /** Reset all simulation data for this object. */
+  public void resetData() {
+    CTREPCMDataJNI.resetData(m_index);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java
index 3df142f..048d3c1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/CallbackStore.java
@@ -1,25 +1,33 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
+/** Manages simulation callbacks; each object is associated with a callback. */
 public class CallbackStore implements AutoCloseable {
+  /** <b>Note: This interface is for simulation classes only. It should not be used by teams!</b> */
   interface CancelCallbackFunc {
     void cancel(int index, int uid);
   }
 
+  /** <b>Note: This interface is for simulation classes only. It should not be used by teams!</b> */
   interface CancelCallbackChannelFunc {
     void cancel(int index, int channel, int uid);
   }
 
+  /** <b>Note: This interface is for simulation classes only. It should not be used by teams!</b> */
   interface CancelCallbackNoIndexFunc {
     void cancel(int uid);
   }
 
+  /**
+   * <b>Note: This constructor is for simulation classes only. It should not be called by teams!</b>
+   *
+   * @param index TODO
+   * @param uid TODO
+   * @param ccf TODO
+   */
   public CallbackStore(int index, int uid, CancelCallbackFunc ccf) {
     this.m_cancelType = kNormalCancel;
     this.m_index = index;
@@ -27,6 +35,14 @@
     this.m_cancelCallback = ccf;
   }
 
+  /**
+   * <b>Note: This constructor is for simulation classes only. It should not be called by teams!</b>
+   *
+   * @param index TODO
+   * @param channel TODO
+   * @param uid TODO
+   * @param ccf TODO
+   */
   public CallbackStore(int index, int channel, int uid, CancelCallbackChannelFunc ccf) {
     this.m_cancelType = kChannelCancel;
     this.m_index = index;
@@ -35,6 +51,12 @@
     this.m_cancelCallbackChannel = ccf;
   }
 
+  /**
+   * <b>Note: This constructor is for simulation classes only. It should not be called by teams!</b>
+   *
+   * @param uid TODO
+   * @param ccf TODO
+   */
   public CallbackStore(int uid, CancelCallbackNoIndexFunc ccf) {
     this.m_cancelType = kNoIndexCancel;
     this.m_uid = uid;
@@ -52,6 +74,7 @@
   private static final int kNoIndexCancel = 2;
   private int m_cancelType;
 
+  /** Cancel the callback associated with this object. */
   @Override
   public void close() {
     switch (m_cancelType) {
@@ -71,11 +94,12 @@
     m_cancelType = -1;
   }
 
+  @SuppressWarnings({"NoFinalizer", "deprecation"})
   @Override
   protected void finalize() throws Throwable {
     try {
       if (m_cancelType >= 0) {
-        close();        // close open files
+        close(); // close open files
       }
     } finally {
       super.finalize();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java
index 758d7e4..83afb74 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DIOSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -12,9 +9,7 @@
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.DigitalOutput;
 
-/**
- * Class to control a simulated digital input or output.
- */
+/** Class to control a simulated digital input or output. */
 public class DIOSim {
   private final int m_index;
 
@@ -45,61 +40,162 @@
     m_index = channel;
   }
 
+  /**
+   * Register a callback to be run when this DIO is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DIODataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DIODataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether this DIO has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return DIODataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether this DIO has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
     DIODataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run whenever the DIO value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerValueCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DIODataJNI.registerValueCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DIODataJNI::cancelValueCallback);
   }
+
+  /**
+   * Read the value of the DIO port.
+   *
+   * @return the DIO value
+   */
   public boolean getValue() {
     return DIODataJNI.getValue(m_index);
   }
+
+  /**
+   * Change the DIO value.
+   *
+   * @param value the new value
+   */
   public void setValue(boolean value) {
     DIODataJNI.setValue(m_index, value);
   }
 
+  /**
+   * Register a callback to be run whenever the pulse length changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerPulseLengthCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DIODataJNI.registerPulseLengthCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DIODataJNI::cancelPulseLengthCallback);
   }
+
+  /**
+   * Read the pulse length.
+   *
+   * @return the pulse length of this DIO port
+   */
   public double getPulseLength() {
     return DIODataJNI.getPulseLength(m_index);
   }
+
+  /**
+   * Change the pulse length of this DIO port.
+   *
+   * @param pulseLength the new pulse length
+   */
   public void setPulseLength(double pulseLength) {
     DIODataJNI.setPulseLength(m_index, pulseLength);
   }
 
+  /**
+   * Register a callback to be run whenever this DIO changes to be an input.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerIsInputCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DIODataJNI.registerIsInputCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DIODataJNI::cancelIsInputCallback);
   }
+
+  /**
+   * Check whether this DIO port is currently an Input.
+   *
+   * @return true if Input
+   */
   public boolean getIsInput() {
     return DIODataJNI.getIsInput(m_index);
   }
+
+  /**
+   * Define whether this DIO port is an Input.
+   *
+   * @param isInput whether this DIO should be an Input
+   */
   public void setIsInput(boolean isInput) {
     DIODataJNI.setIsInput(m_index, isInput);
   }
 
+  /**
+   * Register a callback to be run whenever the filter index changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerFilterIndexCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DIODataJNI.registerFilterIndexCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DIODataJNI::cancelFilterIndexCallback);
   }
+
+  /**
+   * Read the filter index.
+   *
+   * @return the filter index of this DIO port
+   */
   public int getFilterIndex() {
     return DIODataJNI.getFilterIndex(m_index);
   }
+
+  /**
+   * Change the filter index of this DIO port.
+   *
+   * @param filterIndex the new filter index
+   */
   public void setFilterIndex(int filterIndex) {
     DIODataJNI.setFilterIndex(m_index, filterIndex);
   }
 
+  /** Reset all simulation data of this object. */
   public void resetData() {
     DIODataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
index ab3d003..b123eac 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSim.java
@@ -1,173 +1,292 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N7;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.RobotController;
 
 /**
- * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set inputs from motors with
- * {@link #setInputs(double, double)}, call {@link #update(double)} to update the simulation,
- * and set estimated encoder and gyro positions, as well as estimated odometry pose. Teams can use {@link edu.wpi.first.wpilibj.simulation.Field2d} to
- * visualize their robot on the Sim GUI's field.
+ * This class simulates the state of the drivetrain. In simulationPeriodic, users should first set
+ * inputs from motors with {@link #setInputs(double, double)}, call {@link #update(double)} to
+ * update the simulation, and set estimated encoder and gyro positions, as well as estimated
+ * odometry pose. Teams can use {@link edu.wpi.first.wpilibj.smartdashboard.Field2d} to visualize
+ * their robot on the Sim GUI's field.
  *
- *  <p>Our state-space system is:
+ * <p>Our state-space system is:
  *
- *  <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r, voltError_l, voltError_r, headingError]]^T
- *  in the field coordinate system (dist_* are wheel distances.)
+ * <p>x = [[x, y, theta, vel_l, vel_r, dist_l, dist_r]]ᵀ in the field coordinate system (dist_* are
+ * wheel distances.)
  *
- *  <p>u = [[voltage_l, voltage_r]]^T This is typically the control input of the last timestep
- *  from a LTVDiffDriveController.
+ * <p>u = [[voltage_l, voltage_r]]ᵀ This is typically the control input of the last timestep from a
+ * LTVDiffDriveController.
  *
- *  <p>y = [[x, y, theta]]^T from a global measurement source(ex. vision),
- *  or y = [[dist_l, dist_r, theta]] from encoders and gyro.
- *
+ * <p>y = x
  */
 public class DifferentialDrivetrainSim {
   private final DCMotor m_motor;
   private final double m_originalGearing;
+  private final Matrix<N7, N1> m_measurementStdDevs;
   private double m_currentGearing;
   private final double m_wheelRadiusMeters;
+
   @SuppressWarnings("MemberName")
   private Matrix<N2, N1> m_u;
+
   @SuppressWarnings("MemberName")
   private Matrix<N7, N1> m_x;
 
+  @SuppressWarnings("MemberName")
+  private Matrix<N7, N1> m_y;
+
   private final double m_rb;
   private final LinearSystem<N2, N2, N2> m_plant;
 
   /**
    * 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 jKgMetersSquared  The moment of inertia of the drivetrain about its center.
-   * @param massKg            The mass of the drivebase.
+   * @param driveMotor A {@link DCMotor} representing the left side of the drivetrain.
+   * @param gearing The gearing ratio 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 jKgMetersSquared The moment of inertia of the drivetrain about its center.
+   * @param massKg The mass of the drivebase.
    * @param wheelRadiusMeters The radius of the wheels on the drivetrain.
-   * @param trackWidthMeters  The robot's track width, or distance between left and right wheels.
+   * @param trackWidthMeters The robot's track width, or distance between left and right wheels.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
    */
-  public DifferentialDrivetrainSim(DCMotor driveMotor, double gearing,
-                                   double jKgMetersSquared, double massKg,
-                                   double wheelRadiusMeters, double trackWidthMeters) {
-    this(LinearSystemId.createDrivetrainVelocitySystem(driveMotor, massKg, wheelRadiusMeters,
-        trackWidthMeters / 2.0, jKgMetersSquared, gearing),
-        driveMotor, gearing, trackWidthMeters, wheelRadiusMeters);
+  @SuppressWarnings("ParameterName")
+  public DifferentialDrivetrainSim(
+      DCMotor driveMotor,
+      double gearing,
+      double jKgMetersSquared,
+      double massKg,
+      double wheelRadiusMeters,
+      double trackWidthMeters,
+      Matrix<N7, N1> measurementStdDevs) {
+    this(
+        LinearSystemId.createDrivetrainVelocitySystem(
+            driveMotor,
+            massKg,
+            wheelRadiusMeters,
+            trackWidthMeters / 2.0,
+            jKgMetersSquared,
+            gearing),
+        driveMotor,
+        gearing,
+        trackWidthMeters,
+        wheelRadiusMeters,
+        measurementStdDevs);
   }
 
   /**
-   * Create a SimDrivetrain
-   * .
-   * @param drivetrainPlant   The {@link LinearSystem} representing the robot's drivetrain. This
-   *                          system can be created with {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor, double, double, double, double, double)}
-   *                          or {@link edu.wpi.first.wpilibj.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double, double, double)}.
-   * @param driveMotor        A {@link DCMotor} representing the drivetrain.
-   * @param gearing           The gearingRatio ratio of the robot, as output over input.
-   *                          This must be the same ratio as the ratio used to identify or
-   *                          create the drivetrainPlant.
-   * @param trackWidthMeters  The distance between the two sides of the drivetrian. Can be
-   *                          found with frc-characterization.
+   * Create a SimDrivetrain .
+   *
+   * @param drivetrainPlant The {@link LinearSystem} representing the robot's drivetrain. This
+   *     system can be created with {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#createDrivetrainVelocitySystem(DCMotor,
+   *     double, double, double, double, double)} or {@link
+   *     edu.wpi.first.math.system.plant.LinearSystemId#identifyDrivetrainSystem(double, double,
+   *     double, double)}.
+   * @param driveMotor A {@link DCMotor} representing the drivetrain.
+   * @param gearing The gearingRatio ratio of the robot, as output over input. This must be the same
+   *     ratio as the ratio used to identify or create the drivetrainPlant.
+   * @param trackWidthMeters The distance between the two sides of the drivetrian. Can be found with
+   *     SysId.
    * @param wheelRadiusMeters The radius of the wheels on the drivetrain, in meters.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
    */
-  public DifferentialDrivetrainSim(LinearSystem<N2, N2, N2> drivetrainPlant,
-                                   DCMotor driveMotor, double gearing,
-                                   double trackWidthMeters,
-                                   double wheelRadiusMeters) {
+  public DifferentialDrivetrainSim(
+      LinearSystem<N2, N2, N2> drivetrainPlant,
+      DCMotor driveMotor,
+      double gearing,
+      double trackWidthMeters,
+      double wheelRadiusMeters,
+      Matrix<N7, N1> measurementStdDevs) {
     this.m_plant = drivetrainPlant;
     this.m_rb = trackWidthMeters / 2.0;
     this.m_motor = driveMotor;
     this.m_originalGearing = gearing;
+    this.m_measurementStdDevs = measurementStdDevs;
     m_wheelRadiusMeters = wheelRadiusMeters;
     m_currentGearing = m_originalGearing;
 
     m_x = new Matrix<>(Nat.N7(), Nat.N1());
     m_u = VecBuilder.fill(0, 0);
+    m_y = new Matrix<>(Nat.N7(), Nat.N1());
   }
 
   /**
-   * Sets the applied voltage to the drivetrain. Note that positive voltage must make that
-   * side of the drivetrain travel forward (+X).
+   * Sets the applied voltage to the drivetrain. Note that positive voltage must make that side of
+   * the drivetrain travel forward (+X).
    *
-   * @param leftVoltageVolts  The left voltage.
+   * @param leftVoltageVolts The left voltage.
    * @param rightVoltageVolts The right voltage.
    */
   public void setInputs(double leftVoltageVolts, double rightVoltageVolts) {
-    m_u = VecBuilder.fill(leftVoltageVolts, rightVoltageVolts);
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  public void update(double dtSeconds) {
-
-    // Update state estimate with RK4
-    m_x = RungeKutta.rungeKutta(this::getDynamics, m_x, m_u, dtSeconds);
-  }
-
-  public double getState(State state) {
-    return m_x.get(state.value, 0);
+    m_u = clampInput(VecBuilder.fill(leftVoltageVolts, rightVoltageVolts));
   }
 
   /**
-   * Returns the full simulated state of the drivetrain.
+   * Update the drivetrain states with the current time difference.
+   *
+   * @param dtSeconds the time difference
    */
-  public Matrix<N7, N1> getState() {
+  @SuppressWarnings("LocalVariableName")
+  public void update(double dtSeconds) {
+    // Update state estimate with RK4
+    m_x = NumericalIntegration.rk4(this::getDynamics, m_x, m_u, dtSeconds);
+    m_y = m_x;
+    if (m_measurementStdDevs != null) {
+      m_y = m_y.plus(StateSpaceUtil.makeWhiteNoiseVector(m_measurementStdDevs));
+    }
+  }
+
+  /** Returns the full simulated state of the drivetrain. */
+  Matrix<N7, N1> getState() {
     return m_x;
   }
 
   /**
+   * Get one of the drivetrain states.
+   *
+   * @param state the state to get
+   * @return the state
+   */
+  double getState(State state) {
+    return m_x.get(state.value, 0);
+  }
+
+  private double getOutput(State output) {
+    return m_y.get(output.value, 0);
+  }
+
+  /**
    * Returns the direction the robot is pointing.
    *
-   * <p>Note that this angle is counterclockwise-positive, while most gyros are
-   * clockwise positive.
+   * <p>Note that this angle is counterclockwise-positive, while most gyros are clockwise positive.
+   *
+   * @return The direction the robot is pointing.
    */
   public Rotation2d getHeading() {
-    return new Rotation2d(getState(State.kHeading));
+    return new Rotation2d(getOutput(State.kHeading));
   }
 
   /**
    * Returns the current pose.
+   *
+   * @return The current pose.
    */
   public Pose2d getPose() {
-    return new Pose2d(m_x.get(0, 0),
-      m_x.get(1, 0),
-      new Rotation2d(m_x.get(2, 0)));
+    return new Pose2d(getOutput(State.kX), getOutput(State.kY), getHeading());
   }
 
+  /**
+   * Get the right encoder position in meters.
+   *
+   * @return The encoder position.
+   */
+  public double getRightPositionMeters() {
+    return getOutput(State.kRightPosition);
+  }
+
+  /**
+   * Get the right encoder velocity in meters per second.
+   *
+   * @return The encoder velocity.
+   */
+  public double getRightVelocityMetersPerSecond() {
+    return getOutput(State.kRightVelocity);
+  }
+
+  /**
+   * Get the left encoder position in meters.
+   *
+   * @return The encoder position.
+   */
+  public double getLeftPositionMeters() {
+    return getOutput(State.kLeftPosition);
+  }
+
+  /**
+   * Get the left encoder velocity in meters per second.
+   *
+   * @return The encoder velocity.
+   */
+  public double getLeftVelocityMetersPerSecond() {
+    return getOutput(State.kLeftVelocity);
+  }
+
+  /**
+   * Get the current draw of the left side of the drivetrain.
+   *
+   * @return the drivetrain's left side current draw, in amps
+   */
+  public double getLeftCurrentDrawAmps() {
+    var loadIleft =
+        m_motor.getCurrent(
+                getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters,
+                m_u.get(0, 0))
+            * Math.signum(m_u.get(0, 0));
+    return loadIleft;
+  }
+
+  /**
+   * Get the current draw of the right side of the drivetrain.
+   *
+   * @return the drivetrain's right side current draw, in amps
+   */
+  public double getRightCurrentDrawAmps() {
+    var loadIright =
+        m_motor.getCurrent(
+                getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters,
+                m_u.get(1, 0))
+            * Math.signum(m_u.get(1, 0));
+
+    return loadIright;
+  }
+
+  /**
+   * Get the current draw of the drivetrain.
+   *
+   * @return the current draw, in amps
+   */
   public double getCurrentDrawAmps() {
-    var loadIleft = m_motor.getCurrent(
-        getState(State.kLeftVelocity) * m_currentGearing / m_wheelRadiusMeters,
-        m_u.get(0, 0)) * Math.signum(m_u.get(0, 0));
-
-    var loadIright = m_motor.getCurrent(
-        getState(State.kRightVelocity) * m_currentGearing / m_wheelRadiusMeters,
-        m_u.get(1, 0)) * Math.signum(m_u.get(1, 0));
-
-    return loadIleft + loadIright;
+    return getLeftCurrentDrawAmps() + getRightCurrentDrawAmps();
   }
 
+  /**
+   * Get the drivetrain gearing.
+   *
+   * @return the gearing ration
+   */
   public double getCurrentGearing() {
     return m_currentGearing;
   }
 
   /**
-   * Sets the gearing reduction on the drivetrain. This is commonly used for
-   * shifting drivetrains.
+   * Sets the gearing reduction on the drivetrain. This is commonly used for shifting drivetrains.
    *
    * @param newGearRatio The new gear ratio, as output over input.
    */
@@ -197,9 +316,8 @@
     m_x.set(State.kRightPosition.value, 0, 0);
   }
 
-  @SuppressWarnings({"DuplicatedCode", "LocalVariableName"})
+  @SuppressWarnings({"DuplicatedCode", "LocalVariableName", "ParameterName"})
   protected Matrix<N7, N1> getDynamics(Matrix<N7, N1> x, Matrix<N2, N1> u) {
-
     // 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.
     var B = new Matrix<>(Nat.N4(), Nat.N2());
@@ -208,8 +326,14 @@
     // 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.
     var A = new Matrix<>(Nat.N4(), Nat.N4());
-    A.assignBlock(0, 0, m_plant.getA().times((this.m_currentGearing * this.m_currentGearing)
-        / (this.m_originalGearing * this.m_originalGearing)));
+    A.assignBlock(
+        0,
+        0,
+        m_plant
+            .getA()
+            .times(
+                (this.m_currentGearing * this.m_currentGearing)
+                    / (this.m_originalGearing * this.m_originalGearing)));
 
     A.assignBlock(2, 0, Matrix.eye(Nat.N2()));
 
@@ -218,16 +342,29 @@
     var xdot = new Matrix<>(Nat.N7(), Nat.N1());
     xdot.set(0, 0, v * Math.cos(x.get(State.kHeading.value, 0)));
     xdot.set(1, 0, v * Math.sin(x.get(State.kHeading.value, 0)));
-    xdot.set(2, 0, (x.get(State.kRightVelocity.value, 0)
-        - x.get(State.kLeftVelocity.value, 0)) / (2.0 * m_rb));
-    xdot.assignBlock(3, 0,
-        A.times(x.block(Nat.N4(), Nat.N1(), 3, 0))
-        .plus(B.times(u)));
+    xdot.set(
+        2,
+        0,
+        (x.get(State.kRightVelocity.value, 0) - x.get(State.kLeftVelocity.value, 0))
+            / (2.0 * m_rb));
+    xdot.assignBlock(3, 0, A.times(x.block(Nat.N4(), Nat.N1(), 3, 0)).plus(B.times(u)));
 
     return xdot;
   }
 
-  public enum State {
+  /**
+   * Clamp the input vector such that no element exceeds the battery voltage. If any does, the
+   * relative magnitudes of the input will be maintained.
+   *
+   * @param u The input vector.
+   * @return The normalized input.
+   */
+  protected Matrix<N2, N1> clampInput(Matrix<N2, N1> u) {
+    return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
+  }
+
+  /** Represents the different states of the drivetrain. */
+  enum State {
     kX(0),
     kY(1),
     kHeading(2),
@@ -239,18 +376,15 @@
     @SuppressWarnings("MemberName")
     public final int value;
 
+    @SuppressWarnings("ParameterName")
     State(int i) {
       this.value = i;
     }
   }
 
   /**
-   * 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
+   * 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
    */
   public enum KitbotGearing {
     k12p75(12.75),
@@ -262,11 +396,13 @@
     @SuppressWarnings("MemberName")
     public final double value;
 
+    @SuppressWarnings("ParameterName")
     KitbotGearing(double i) {
       this.value = i;
     }
   }
 
+  /** Represents common motor layouts of the kit drivetrain. */
   public enum KitbotMotor {
     kSingleCIMPerSide(DCMotor.getCIM(1)),
     kDualCIMPerSide(DCMotor.getCIM(2)),
@@ -276,19 +412,29 @@
     @SuppressWarnings("MemberName")
     public final DCMotor value;
 
+    @SuppressWarnings("ParameterName")
     KitbotMotor(DCMotor i) {
       this.value = i;
     }
   }
 
+  /** Represents common wheel sizes of the kit drivetrain. */
   public enum KitbotWheelSize {
+    kSixInch(Units.inchesToMeters(6)),
+    kEightInch(Units.inchesToMeters(8)),
+    kTenInch(Units.inchesToMeters(10)),
+
+    @Deprecated
     SixInch(Units.inchesToMeters(6)),
+    @Deprecated
     EightInch(Units.inchesToMeters(8)),
+    @Deprecated
     TenInch(Units.inchesToMeters(10));
 
     @SuppressWarnings("MemberName")
     public final double value;
 
+    @SuppressWarnings("ParameterName")
     KitbotWheelSize(double i) {
       this.value = i;
     }
@@ -297,32 +443,59 @@
   /**
    * Create a sim for the standard FRC kitbot.
    *
-   * @param motor     The motors installed in the bot.
-   * @param gearing   The gearing reduction used.
+   * @param motor The motors installed in the bot.
+   * @param gearing The gearing reduction used.
    * @param wheelSize The wheel size.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
+   * @return A sim for the standard FRC kitbot.
    */
-  public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
-                                                          KitbotWheelSize wheelSize) {
+  public static DifferentialDrivetrainSim createKitbotSim(
+      KitbotMotor motor,
+      KitbotGearing gearing,
+      KitbotWheelSize wheelSize,
+      Matrix<N7, N1> measurementStdDevs) {
     // MOI estimation -- note that I = m r^2 for point masses
     var batteryMoi = 12.5 / 2.2 * Math.pow(Units.inchesToMeters(10), 2);
-    var gearboxMoi = (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */)
-        * Math.pow(Units.inchesToMeters(26.0 / 2.0), 2);
+    var gearboxMoi =
+        (2.8 /* CIM motor */ * 2 / 2.2 + 2.0 /* Toughbox Mini- ish */)
+            * Math.pow(Units.inchesToMeters(26.0 / 2.0), 2);
 
-    return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi);
+    return createKitbotSim(motor, gearing, wheelSize, batteryMoi + gearboxMoi, measurementStdDevs);
   }
 
   /**
    * 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 motor The motors installed in the bot.
+   * @param gearing The gearing reduction used.
+   * @param wheelSize The wheel size.
    * @param jKgMetersSquared The moment of inertia of the drivebase. This can be calculated using
-   *                         frc-characterization.
+   *     SysId.
+   * @param measurementStdDevs Standard deviations for measurements, in the form [x, y, heading,
+   *     left velocity, right velocity, left distance, right distance]ᵀ. Can be null if no noise is
+   *     desired. Gyro standard deviations of 0.0001 radians, velocity standard deviations of 0.05
+   *     m/s, and position measurement standard deviations of 0.005 meters are a reasonable starting
+   *     point.
+   * @return A sim for the standard FRC kitbot.
    */
-  public static DifferentialDrivetrainSim createKitbotSim(KitbotMotor motor, KitbotGearing gearing,
-                                                          KitbotWheelSize wheelSize, double jKgMetersSquared) {
-    return new DifferentialDrivetrainSim(motor.value, gearing.value, jKgMetersSquared, 25 / 2.2,
-        wheelSize.value / 2.0, Units.inchesToMeters(26));
+  @SuppressWarnings("ParameterName")
+  public static DifferentialDrivetrainSim createKitbotSim(
+      KitbotMotor motor,
+      KitbotGearing gearing,
+      KitbotWheelSize wheelSize,
+      double jKgMetersSquared,
+      Matrix<N7, N1> measurementStdDevs) {
+    return new DifferentialDrivetrainSim(
+        motor.value,
+        gearing.value,
+        jKgMetersSquared,
+        Units.lbsToKilograms(60),
+        wheelSize.value / 2.0,
+        Units.inchesToMeters(26),
+        measurementStdDevs);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java
index db942eb..f18f092 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -15,8 +12,8 @@
 /**
  * Class to control a simulated digital PWM output.
  *
- * <p>This is for duty cycle PWM outputs on a DigitalOutput, not for the servo
- * style PWM outputs on a PWM channel.
+ * <p>This is for duty cycle PWM outputs on a DigitalOutput, not for the servo style PWM outputs on
+ * a PWM channel.
  */
 public class DigitalPWMSim {
   private final int m_index;
@@ -50,8 +47,8 @@
   }
 
   /**
-   * Creates an DigitalPWMSim for a simulated index.
-   * The index is incremented for each simulated DigitalPWM.
+   * Creates an DigitalPWMSim for a simulated index. The index is incremented for each simulated
+   * DigitalPWM.
    *
    * @param index simulator index
    * @return Simulated object
@@ -60,39 +57,100 @@
     return new DigitalPWMSim(index);
   }
 
+  /**
+   * Register a callback to be run when this PWM output is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DigitalPWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether this PWM output has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return DigitalPWMDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether this PWM output has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
     DigitalPWMDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run whenever the duty cycle value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerDutyCycleCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DigitalPWMDataJNI.registerDutyCycleCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelDutyCycleCallback);
   }
+
+  /**
+   * Read the duty cycle value.
+   *
+   * @return the duty cycle value of this PWM output
+   */
   public double getDutyCycle() {
     return DigitalPWMDataJNI.getDutyCycle(m_index);
   }
+
+  /**
+   * Set the duty cycle value of this PWM output.
+   *
+   * @param dutyCycle the new value
+   */
   public void setDutyCycle(double dutyCycle) {
     DigitalPWMDataJNI.setDutyCycle(m_index, dutyCycle);
   }
 
+  /**
+   * Register a callback to be run whenever the pin changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerPinCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DigitalPWMDataJNI.registerPinCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DigitalPWMDataJNI::cancelPinCallback);
   }
+
+  /**
+   * Check the pin number.
+   *
+   * @return the pin number
+   */
   public int getPin() {
     return DigitalPWMDataJNI.getPin(m_index);
   }
+
+  /**
+   * Change the pin number.
+   *
+   * @param pin the new pin number
+   */
   public void setPin(int pin) {
     DigitalPWMDataJNI.setPin(m_index, pin);
   }
 
+  /** Reset all simulation data. */
   public void resetData() {
     DigitalPWMDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java
index 40b1017..f6da659 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DriverStationSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -12,81 +9,223 @@
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.wpilibj.DriverStation;
 
-/**
- * Class to control a simulated driver station.
- */
-@SuppressWarnings({"PMD.UseUtilityClass", "PMD.GodClass", "PMD.ExcessivePublicCount"})
-public class DriverStationSim {
-  public static CallbackStore registerEnabledCallback(NotifyCallback callback, boolean initialNotify) {
+/** Class to control a simulated driver station. */
+public final class DriverStationSim {
+  private DriverStationSim() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Register a callback on whether the DS is enabled.
+   *
+   * @param callback the callback that will be called whenever the enabled state is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerEnabledCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerEnabledCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelEnabledCallback);
   }
+
+  /**
+   * Check if the DS is enabled.
+   *
+   * @return true if enabled
+   */
   public static boolean getEnabled() {
     return DriverStationDataJNI.getEnabled();
   }
+
+  /**
+   * Change whether the DS is enabled.
+   *
+   * @param enabled the new value
+   */
   public static void setEnabled(boolean enabled) {
     DriverStationDataJNI.setEnabled(enabled);
   }
 
-  public static CallbackStore registerAutonomousCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on whether the DS is in autonomous mode.
+   *
+   * @param callback the callback that will be called on autonomous mode entrance/exit
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerAutonomousCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerAutonomousCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelAutonomousCallback);
   }
+
+  /**
+   * Check if the DS is in autonomous.
+   *
+   * @return true if autonomous
+   */
   public static boolean getAutonomous() {
     return DriverStationDataJNI.getAutonomous();
   }
+
+  /**
+   * Change whether the DS is in autonomous.
+   *
+   * @param autonomous the new value
+   */
   public static void setAutonomous(boolean autonomous) {
     DriverStationDataJNI.setAutonomous(autonomous);
   }
 
+  /**
+   * Register a callback on whether the DS is in test mode.
+   *
+   * @param callback the callback that will be called whenever the test mode is entered or left
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public static CallbackStore registerTestCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerTestCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelTestCallback);
   }
+
+  /**
+   * Check if the DS is in test.
+   *
+   * @return true if test
+   */
   public static boolean getTest() {
     return DriverStationDataJNI.getTest();
   }
+
+  /**
+   * Change whether the DS is in test.
+   *
+   * @param test the new value
+   */
   public static void setTest(boolean test) {
     DriverStationDataJNI.setTest(test);
   }
 
-  public static CallbackStore registerEStopCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the eStop state.
+   *
+   * @param callback the callback that will be called whenever the eStop state changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerEStopCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerEStopCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelEStopCallback);
   }
+
+  /**
+   * Check if eStop has been activated.
+   *
+   * @return true if eStopped
+   */
   public static boolean getEStop() {
     return DriverStationDataJNI.getEStop();
   }
+
+  /**
+   * Set whether eStop is active.
+   *
+   * @param eStop true to activate
+   */
+  @SuppressWarnings("ParameterName")
   public static void setEStop(boolean eStop) {
     DriverStationDataJNI.setEStop(eStop);
   }
 
-  public static CallbackStore registerFmsAttachedCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on whether the FMS is connected.
+   *
+   * @param callback the callback that will be called whenever the FMS connection changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerFmsAttachedCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerFmsAttachedCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelFmsAttachedCallback);
   }
+
+  /**
+   * Check if the FMS is connected.
+   *
+   * @return true if FMS is connected
+   */
   public static boolean getFmsAttached() {
     return DriverStationDataJNI.getFmsAttached();
   }
+
+  /**
+   * Change whether the FMS is connected.
+   *
+   * @param fmsAttached the new value
+   */
   public static void setFmsAttached(boolean fmsAttached) {
     DriverStationDataJNI.setFmsAttached(fmsAttached);
   }
 
-  public static CallbackStore registerDsAttachedCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on whether the DS is connected.
+   *
+   * @param callback the callback that will be called whenever the DS connection changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerDsAttachedCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerDsAttachedCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelDsAttachedCallback);
   }
+
+  /**
+   * Check if the DS is attached.
+   *
+   * @return true if attached
+   */
   public static boolean getDsAttached() {
     return DriverStationDataJNI.getDsAttached();
   }
+
+  /**
+   * Change whether the DS is attached.
+   *
+   * @param dsAttached the new value
+   */
   public static void setDsAttached(boolean dsAttached) {
     DriverStationDataJNI.setDsAttached(dsAttached);
   }
 
-  public static CallbackStore registerAllianceStationIdCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the alliance station ID.
+   *
+   * @param callback the callback that will be called whenever the alliance station changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerAllianceStationIdCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerAllianceStationIdCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelAllianceStationIdCallback);
   }
+
+  /**
+   * Get the alliance station ID (color + number).
+   *
+   * @return the alliance station color and number
+   */
   public static AllianceStationID getAllianceStationId() {
     switch (DriverStationDataJNI.getAllianceStationId()) {
       case 0:
@@ -105,6 +244,12 @@
         return null;
     }
   }
+
+  /**
+   * Change the alliance station.
+   *
+   * @param allianceStationId the new alliance station
+   */
   public static void setAllianceStationId(AllianceStationID allianceStationId) {
     int allianceStation;
     switch (allianceStationId) {
@@ -132,24 +277,42 @@
     DriverStationDataJNI.setAllianceStationId(allianceStation);
   }
 
-  public static CallbackStore registerMatchTimeCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on match time.
+   *
+   * @param callback the callback that will be called whenever match time changes
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerMatchTimeCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = DriverStationDataJNI.registerMatchTimeCallback(callback, initialNotify);
     return new CallbackStore(uid, DriverStationDataJNI::cancelMatchTimeCallback);
   }
+
+  /**
+   * Get the current value of the match timer.
+   *
+   * @return the current match time
+   */
   public static double getMatchTime() {
     return DriverStationDataJNI.getMatchTime();
   }
+
+  /**
+   * Sets the match timer.
+   *
+   * @param matchTime the new match time
+   */
   public static void setMatchTime(double matchTime) {
     DriverStationDataJNI.setMatchTime(matchTime);
   }
 
-  /**
-   * Updates DriverStation data so that new values are visible to the user
-   * program.
-   */
+  /** Updates DriverStation data so that new values are visible to the user program. */
   public static void notifyNewData() {
     DriverStationDataJNI.notifyNewData();
-    DriverStation.getInstance().waitForData();
+    DriverStation.waitForData();
   }
 
   /**
@@ -194,7 +357,7 @@
   /**
    * Sets the state of one joystick button. Button indexes begin at 1.
    *
-   * @param stick  The joystick number
+   * @param stick The joystick number
    * @param button The button index, beginning at 1
    * @param state The state of the joystick button
    */
@@ -206,7 +369,7 @@
    * Gets the value of the axis on a joystick.
    *
    * @param stick The joystick number
-   * @param axis  The analog axis number
+   * @param axis The analog axis number
    * @param value The value of the axis on the joystick
    */
   public static void setJoystickAxis(int stick, int axis, double value) {
@@ -367,6 +530,7 @@
     DriverStationDataJNI.setReplayNumber(replayNumber);
   }
 
+  /** Reset all simulation data for the Driver Station. */
   public static void resetData() {
     DriverStationDataJNI.resetData();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
index 92b0883..f5c900c 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSim.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.wpilibj.DutyCycleEncoder;
 
-/**
- * Class to control a simulated duty cycle encoder.
- */
+/** Class to control a simulated duty cycle encoder. */
 public class DutyCycleEncoderSim {
   private final SimDouble m_simPosition;
   private final SimDouble m_simDistancePerRotation;
@@ -23,9 +18,10 @@
    * @param encoder DutyCycleEncoder to simulate
    */
   public DutyCycleEncoderSim(DutyCycleEncoder encoder) {
-    SimDeviceSim wrappedSimDevice = new SimDeviceSim("DutyCycleEncoder" + "[" + encoder.getFPGAIndex() + "]");
-    m_simPosition = wrappedSimDevice.getDouble("Position");
-    m_simDistancePerRotation = wrappedSimDevice.getDouble("DistancePerRotation");
+    SimDeviceSim wrappedSimDevice =
+        new SimDeviceSim("DutyCycle:DutyCycleEncoder" + "[" + encoder.getSourceChannel() + "]");
+    m_simPosition = wrappedSimDevice.getDouble("position");
+    m_simDistancePerRotation = wrappedSimDevice.getDouble("distance_per_rot");
   }
 
   /**
@@ -39,6 +35,8 @@
 
   /**
    * Set the position.
+   *
+   * @param distance The position.
    */
   public void setDistance(double distance) {
     m_simPosition.set(distance / m_simDistancePerRotation.get());
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java
index 726e08c..e893954 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/DutyCycleSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -12,9 +9,7 @@
 import edu.wpi.first.wpilibj.DutyCycle;
 import java.util.NoSuchElementException;
 
-/**
- * Class to control a simulated duty cycle digital input.
- */
+/** Class to control a simulated duty cycle digital input. */
 public class DutyCycleSim {
   private final int m_index;
 
@@ -47,8 +42,8 @@
   }
 
   /**
-   * Creates a DutyCycleSim for a simulated index.
-   * The index is incremented for each simulated DutyCycle.
+   * Creates a DutyCycleSim for a simulated index. The index is incremented for each simulated
+   * DutyCycle.
    *
    * @param index simulator index
    * @return Simulated object
@@ -57,36 +52,101 @@
     return new DutyCycleSim(index);
   }
 
+  /**
+   * Register a callback to be run when this duty cycle input is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DutyCycleDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether this duty cycle input has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return DutyCycleDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether this duty cycle input has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
     DutyCycleDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run whenever the frequency changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerFrequencyCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DutyCycleDataJNI.registerFrequencyCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelFrequencyCallback);
   }
+
+  /**
+   * Measure the frequency.
+   *
+   * @return the duty cycle frequency
+   */
   public int getFrequency() {
     return DutyCycleDataJNI.getFrequency(m_index);
   }
+
+  /**
+   * Change the duty cycle frequency.
+   *
+   * @param frequency the new frequency
+   */
   public void setFrequency(int frequency) {
     DutyCycleDataJNI.setFrequency(m_index, frequency);
   }
 
+  /**
+   * Register a callback to be run whenever the output changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerOutputCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = DutyCycleDataJNI.registerOutputCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, DutyCycleDataJNI::cancelOutputCallback);
   }
+
+  /**
+   * Measure the output from this duty cycle port.
+   *
+   * @return the output value
+   */
   public double getOutput() {
     return DutyCycleDataJNI.getOutput(m_index);
   }
+
+  /**
+   * Change the duty cycle output.
+   *
+   * @param output the new output value
+   */
   public void setOutput(double output) {
     DutyCycleDataJNI.setOutput(m_index, output);
   }
+
+  /** Reset all simulation data for the duty cycle output. */
+  public void resetData() {
+    DutyCycleDataJNI.resetData(m_index);
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
index 1610089..ecd2379 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ElevatorSim.java
@@ -1,24 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 
-/**
- * Represents a simulated elevator mechanism.
- */
+/** Represents a simulated elevator mechanism. */
 public class ElevatorSim extends LinearSystemSim<N2, N1, N1> {
   // Gearbox for the elevator.
   private final DCMotor m_gearbox;
@@ -38,32 +33,42 @@
   /**
    * Creates a simulated elevator mechanism.
    *
-   * @param plant            The linear system that represents the elevator.
-   * @param gearbox          The type of and number of motors in the elevator gearbox.
-   * @param gearing          The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param plant The linear system that represents the elevator.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
    * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
-   * @param minHeightMeters  The min allowable height of the elevator.
-   * @param maxHeightMeters  The max allowable height of the elevator.
+   * @param minHeightMeters The min allowable height of the elevator.
+   * @param maxHeightMeters The max allowable height of the elevator.
    */
-  public ElevatorSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
-                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) {
+  public ElevatorSim(
+      LinearSystem<N2, N1, N1> plant,
+      DCMotor gearbox,
+      double gearing,
+      double drumRadiusMeters,
+      double minHeightMeters,
+      double maxHeightMeters) {
     this(plant, gearbox, gearing, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
   }
 
   /**
    * Creates a simulated elevator mechanism.
    *
-   * @param plant              The linear system that represents the elevator.
-   * @param gearbox            The type of and number of motors in the elevator gearbox.
-   * @param gearing            The gearing of the elevator (numbers greater than 1 represent reductions).
-   * @param drumRadiusMeters   The radius of the drum that the elevator spool is wrapped around.
-   * @param minHeightMeters    The min allowable height of the elevator.
-   * @param maxHeightMeters    The max allowable height of the elevator.
+   * @param plant The linear system that represents the elevator.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeightMeters The min allowable height of the elevator.
+   * @param maxHeightMeters The max allowable height of the elevator.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
-  public ElevatorSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
-                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters,
-                     Matrix<N1, N1> measurementStdDevs) {
+  public ElevatorSim(
+      LinearSystem<N2, N1, N1> plant,
+      DCMotor gearbox,
+      double gearing,
+      double drumRadiusMeters,
+      double minHeightMeters,
+      double maxHeightMeters,
+      Matrix<N1, N1> measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
@@ -75,34 +80,45 @@
   /**
    * Creates a simulated elevator mechanism.
    *
-   * @param gearbox          The type of and number of motors in the elevator gearbox.
-   * @param gearing          The gearing of the elevator (numbers greater than 1 represent reductions).
-   * @param carriageMassKg   The mass of the elevator carriage.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param carriageMassKg The mass of the elevator carriage.
    * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
-   * @param minHeightMeters  The min allowable height of the elevator.
-   * @param maxHeightMeters  The max allowable height of the elevator.
+   * @param minHeightMeters The min allowable height of the elevator.
+   * @param maxHeightMeters The max allowable height of the elevator.
    */
-  public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg,
-                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters) {
-    this(gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters,
-        null);
+  public ElevatorSim(
+      DCMotor gearbox,
+      double gearing,
+      double carriageMassKg,
+      double drumRadiusMeters,
+      double minHeightMeters,
+      double maxHeightMeters) {
+    this(
+        gearbox, gearing, carriageMassKg, drumRadiusMeters, minHeightMeters, maxHeightMeters, null);
   }
 
   /**
    * Creates a simulated elevator mechanism.
    *
-   * @param gearbox            The type of and number of motors in the elevator gearbox.
-   * @param gearing            The gearing of the elevator (numbers greater than 1 represent reductions).
-   * @param carriageMassKg     The mass of the elevator carriage.
-   * @param drumRadiusMeters   The radius of the drum that the elevator spool is wrapped around.
-   * @param minHeightMeters    The min allowable height of the elevator.
-   * @param maxHeightMeters    The max allowable height of the elevator.
+   * @param gearbox The type of and number of motors in the elevator gearbox.
+   * @param gearing The gearing of the elevator (numbers greater than 1 represent reductions).
+   * @param carriageMassKg The mass of the elevator carriage.
+   * @param drumRadiusMeters The radius of the drum that the elevator spool is wrapped around.
+   * @param minHeightMeters The min allowable height of the elevator.
+   * @param maxHeightMeters The max allowable height of the elevator.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
-  public ElevatorSim(DCMotor gearbox, double gearing, double carriageMassKg,
-                     double drumRadiusMeters, double minHeightMeters, double maxHeightMeters,
-                     Matrix<N1, N1> measurementStdDevs) {
-    super(LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
+  public ElevatorSim(
+      DCMotor gearbox,
+      double gearing,
+      double carriageMassKg,
+      double drumRadiusMeters,
+      double minHeightMeters,
+      double maxHeightMeters,
+      Matrix<N1, N1> measurementStdDevs) {
+    super(
+        LinearSystemId.createElevatorSystem(gearbox, carriageMassKg, drumRadiusMeters, gearing),
         measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
@@ -112,23 +128,41 @@
   }
 
   /**
+   * Returns whether the elevator would hit the lower limit.
+   *
+   * @param elevatorHeightMeters The elevator height.
+   * @return Whether the elevator would hit the lower limit.
+   */
+  public boolean wouldHitLowerLimit(double elevatorHeightMeters) {
+    return elevatorHeightMeters < this.m_minHeight;
+  }
+
+  /**
+   * Returns whether the elevator would hit the upper limit.
+   *
+   * @param elevatorHeightMeters The elevator height.
+   * @return Whether the elevator would hit the upper limit.
+   */
+  public boolean wouldHitUpperLimit(double elevatorHeightMeters) {
+    return elevatorHeightMeters > this.m_maxHeight;
+  }
+
+  /**
    * Returns whether the elevator has hit the lower limit.
    *
-   * @param x The current elevator state.
    * @return Whether the elevator has hit the lower limit.
    */
-  public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
-    return x.get(0, 0) < this.m_minHeight;
+  public boolean hasHitLowerLimit() {
+    return wouldHitLowerLimit(getPositionMeters());
   }
 
   /**
    * Returns whether the elevator has hit the upper limit.
    *
-   * @param x The current elevator state.
    * @return Whether the elevator has hit the upper limit.
    */
-  public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
-    return x.get(0, 0) > this.m_maxHeight;
+  public boolean hasHitUpperLimit() {
+    return wouldHitUpperLimit(getPositionMeters());
   }
 
   /**
@@ -146,7 +180,7 @@
    * @return The velocity of the elevator.
    */
   public double getVelocityMetersPerSecond() {
-    return m_x.get(0, 1);
+    return m_x.get(1, 0);
   }
 
   /**
@@ -162,7 +196,8 @@
     // v = r w, so w = v/r
     double motorVelocityRadPerSec = getVelocityMetersPerSecond() / m_drumRadius * m_gearing;
     var appliedVoltage = m_u.get(0, 0);
-    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage) * Math.signum(appliedVoltage);
+    return m_gearbox.getCurrent(motorVelocityRadPerSec, appliedVoltage)
+        * Math.signum(appliedVoltage);
   }
 
   /**
@@ -178,22 +213,28 @@
    * Updates the state of the elevator.
    *
    * @param currentXhat The current state estimate.
-   * @param u           The system inputs (voltage).
-   * @param dtSeconds   The time difference between controller updates.
+   * @param u The system inputs (voltage).
+   * @param dtSeconds The time difference between controller updates.
    */
+  @SuppressWarnings({"ParameterName", "LambdaParameterName"})
   @Override
-  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat,
-                                   Matrix<N1, N1> u, double dtSeconds) {
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
     // Calculate updated x-hat from Runge-Kutta.
-    var updatedXhat = RungeKutta.rungeKutta(
-        (x, u_) -> (m_plant.getA().times(x)).plus(m_plant.getB().times(u_))
-            .plus(VecBuilder.fill(0, -9.8)), currentXhat, u, dtSeconds);
+    var updatedXhat =
+        NumericalIntegration.rkdp(
+            (x, u_) ->
+                (m_plant.getA().times(x))
+                    .plus(m_plant.getB().times(u_))
+                    .plus(VecBuilder.fill(0, -9.8)),
+            currentXhat,
+            u,
+            dtSeconds);
 
     // We check for collisions after updating x-hat.
-    if (hasHitLowerLimit(updatedXhat)) {
+    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
       return VecBuilder.fill(m_minHeight, 0);
     }
-    if (hasHitUpperLimit(updatedXhat)) {
+    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
       return VecBuilder.fill(m_maxHeight, 0);
     }
     return updatedXhat;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java
index 9df5187..924ba99 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/EncoderSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -12,9 +9,7 @@
 import edu.wpi.first.wpilibj.Encoder;
 import java.util.NoSuchElementException;
 
-/**
- * Class to control a simulated encoder.
- */
+/** Class to control a simulated encoder. */
 public class EncoderSim {
   private final int m_index;
 
@@ -32,8 +27,8 @@
   }
 
   /**
-   * Creates an EncoderSim for a digital input channel.  Encoders take two
-   * channels, so either one may be specified.
+   * 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
@@ -48,8 +43,8 @@
   }
 
   /**
-   * Creates an EncoderSim for a simulated index.
-   * The index is incremented for each simulated Encoder.
+   * Creates an EncoderSim for a simulated index. The index is incremented for each simulated
+   * Encoder.
    *
    * @param index simulator index
    * @return Simulated object
@@ -58,110 +53,293 @@
     return new EncoderSim(index);
   }
 
+  /**
+   * Register a callback on the Initialized property of the encoder.
+   *
+   * @param callback the callback that will be called whenever the Initialized property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Read the Initialized value of the encoder.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return EncoderDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Change the Initialized value of the encoder.
+   *
+   * @param initialized the new value
+   */
   public void setInitialized(boolean initialized) {
     EncoderDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback on the count property of the encoder.
+   *
+   * @param callback the callback that will be called whenever the count property is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerCountCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerCountCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelCountCallback);
   }
+
+  /**
+   * Read the count of the encoder.
+   *
+   * @return the count
+   */
   public int getCount() {
     return EncoderDataJNI.getCount(m_index);
   }
+
+  /**
+   * Change the count of the encoder.
+   *
+   * @param count the new count
+   */
   public void setCount(int count) {
     EncoderDataJNI.setCount(m_index, count);
   }
 
+  /**
+   * Register a callback on the period of the encoder.
+   *
+   * @param callback the callback that will be called whenever the period is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerPeriodCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerPeriodCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelPeriodCallback);
   }
+
+  /**
+   * Read the period of the encoder.
+   *
+   * @return the encoder period
+   */
   public double getPeriod() {
     return EncoderDataJNI.getPeriod(m_index);
   }
+
+  /**
+   * Change the encoder period.
+   *
+   * @param period the new period
+   */
   public void setPeriod(double period) {
     EncoderDataJNI.setPeriod(m_index, period);
   }
 
+  /**
+   * Register a callback to be called whenever the encoder is reset.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerResetCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerResetCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelResetCallback);
   }
+
+  /**
+   * Check if the encoder has been reset.
+   *
+   * @return true if reset
+   */
   public boolean getReset() {
     return EncoderDataJNI.getReset(m_index);
   }
+
+  /**
+   * Change the reset property of the encoder.
+   *
+   * @param reset the new value
+   */
   public void setReset(boolean reset) {
     EncoderDataJNI.setReset(m_index, reset);
   }
 
+  /**
+   * Register a callback to be run whenever the max period of the encoder is changed.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerMaxPeriodCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerMaxPeriodCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelMaxPeriodCallback);
   }
+
+  /**
+   * Get the max period of the encoder.
+   *
+   * @return the max period of the encoder
+   */
   public double getMaxPeriod() {
     return EncoderDataJNI.getMaxPeriod(m_index);
   }
+
+  /**
+   * Change the max period of the encoder.
+   *
+   * @param maxPeriod the new value
+   */
   public void setMaxPeriod(double maxPeriod) {
     EncoderDataJNI.setMaxPeriod(m_index, maxPeriod);
   }
 
+  /**
+   * Register a callback on the direction of the encoder.
+   *
+   * @param callback the callback that will be called whenever the direction is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerDirectionCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerDirectionCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelDirectionCallback);
   }
+
+  /**
+   * Get the direction of the encoder.
+   *
+   * @return the direction of the encoder
+   */
   public boolean getDirection() {
     return EncoderDataJNI.getDirection(m_index);
   }
+
+  /**
+   * Set the direction of the encoder.
+   *
+   * @param direction the new direction
+   */
   public void setDirection(boolean direction) {
     EncoderDataJNI.setDirection(m_index, direction);
   }
 
-  public CallbackStore registerReverseDirectionCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the reverse direction.
+   *
+   * @param callback the callback that will be called whenever the reverse direction is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerReverseDirectionCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerReverseDirectionCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelReverseDirectionCallback);
   }
+
+  /**
+   * Get the reverse direction of the encoder.
+   *
+   * @return the reverse direction of the encoder
+   */
   public boolean getReverseDirection() {
     return EncoderDataJNI.getReverseDirection(m_index);
   }
+
+  /**
+   * Set the reverse direction.
+   *
+   * @param reverseDirection the new value
+   */
   public void setReverseDirection(boolean reverseDirection) {
     EncoderDataJNI.setReverseDirection(m_index, reverseDirection);
   }
 
-  public CallbackStore registerSamplesToAverageCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback on the samples-to-average value of this encoder.
+   *
+   * @param callback the callback that will be called whenever the samples-to-average is changed
+   * @param initialNotify if true, the callback will be run on the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerSamplesToAverageCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = EncoderDataJNI.registerSamplesToAverageCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, EncoderDataJNI::cancelSamplesToAverageCallback);
   }
+
+  /**
+   * Get the samples-to-average value.
+   *
+   * @return the samples-to-average value
+   */
   public int getSamplesToAverage() {
     return EncoderDataJNI.getSamplesToAverage(m_index);
   }
+
+  /**
+   * Set the samples-to-average value.
+   *
+   * @param samplesToAverage the new value
+   */
   public void setSamplesToAverage(int samplesToAverage) {
     EncoderDataJNI.setSamplesToAverage(m_index, samplesToAverage);
   }
 
+  /**
+   * Change the encoder distance.
+   *
+   * @param distance the new distance
+   */
   public void setDistance(double distance) {
     EncoderDataJNI.setDistance(m_index, distance);
   }
 
+  /**
+   * Read the distance of the encoder.
+   *
+   * @return the encoder distance
+   */
   public double getDistance() {
     return EncoderDataJNI.getDistance(m_index);
   }
 
+  /**
+   * Change the rate of the encoder.
+   *
+   * @param rate the new rate
+   */
   public void setRate(double rate) {
     EncoderDataJNI.setRate(m_index, rate);
   }
 
+  /**
+   * Get the rate of the encoder.
+   *
+   * @return the rate of change
+   */
   public double getRate() {
     return EncoderDataJNI.getRate(m_index);
   }
 
+  /** Resets all simulation data for this encoder. */
   public void resetData() {
     EncoderDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java
deleted file mode 100644
index bbda95a..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Field2d.java
+++ /dev/null
@@ -1,100 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.simulation;
-
-import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.hal.SimDouble;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * 2D representation of game field (for simulation).
- *
- * <p>In non-simulation mode this simply stores and returns the robot pose.
- *
- * <p>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).
- *
- * <p>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.
- */
-public class Field2d {
-  public Field2d() {
-    m_device = SimDevice.create("Field2D");
-    if (m_device != null) {
-      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);
-    }
-  }
-
-  /**
-   * Set the robot pose from a Pose object.
-   *
-   * @param pose 2D pose
-   */
-  public void setRobotPose(Pose2d pose) {
-    if (m_device != null) {
-      Translation2d translation = pose.getTranslation();
-      m_x.set(translation.getX());
-      m_y.set(translation.getY());
-      m_rot.set(pose.getRotation().getDegrees());
-    } else {
-      m_pose = pose;
-    }
-  }
-
-  /**
-   * Set the robot pose from x, y, and rotation.
-   *
-   * @param xMeters X location, in meters
-   * @param yMeters Y location, in meters
-   * @param rotation rotation
-   */
-  public void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
-    if (m_device != null) {
-      m_x.set(xMeters);
-      m_y.set(yMeters);
-      m_rot.set(rotation.getDegrees());
-    } else {
-      m_pose = new Pose2d(xMeters, yMeters, rotation);
-    }
-  }
-
-  /**
-   * Get the robot pose.
-   *
-   * @return 2D pose
-   */
-  public Pose2d getRobotPose() {
-    if (m_device != null) {
-      return new Pose2d(m_x.get(), m_y.get(), Rotation2d.fromDegrees(m_rot.get()));
-    } else {
-      return m_pose;
-    }
-  }
-
-  private Pose2d m_pose;
-
-  @SuppressWarnings("MemberName")
-  private final SimDevice m_device;
-
-  @SuppressWarnings("MemberName")
-  private SimDouble m_x;
-
-  @SuppressWarnings("MemberName")
-  private SimDouble m_y;
-
-  private SimDouble m_rot;
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
index 785facc..aaf13de 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/FlywheelSim.java
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
 
-/**
- * Represents a simulated flywheel mechanism.
- */
+/** Represents a simulated flywheel mechanism. */
 public class FlywheelSim extends LinearSystemSim<N1, N1, N1> {
   // Gearbox for the flywheel.
   private final DCMotor m_gearbox;
@@ -27,10 +22,9 @@
   /**
    * Creates a simulated flywheel mechanism.
    *
-   * @param plant   The linear system that represents the flywheel.
+   * @param plant The linear system that represents 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 gearing The gearing of the flywheel (numbers greater than 1 represent reductions).
    */
   public FlywheelSim(LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double gearing) {
     super(plant);
@@ -41,15 +35,16 @@
   /**
    * Creates a simulated flywheel mechanism.
    *
-   * @param plant              The linear system that represents 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 plant The linear system that represents 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 deviations of the measurements.
    */
-  public FlywheelSim(LinearSystem<N1, N1, N1> plant, DCMotor gearbox, double gearing,
-                     Matrix<N1, N1> measurementStdDevs) {
+  public FlywheelSim(
+      LinearSystem<N1, N1, N1> plant,
+      DCMotor gearbox,
+      double gearing,
+      Matrix<N1, N1> measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
@@ -58,15 +53,14 @@
   /**
    * Creates a simulated flywheel 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 jKgMetersSquared The moment of inertia of the flywheel. If this is unknown,
-   *                         use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
-   *                         constructor.
+   * @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 jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the
+   *     {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} constructor.
    */
+  @SuppressWarnings("ParameterName")
   public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared) {
-    super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared));
+    super(LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing));
     m_gearbox = gearbox;
     m_gearing = gearing;
   }
@@ -74,18 +68,17 @@
   /**
    * Creates a simulated flywheel 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 jKgMetersSquared   The moment of inertia of the flywheel. If this is unknown,
-   *                           use the {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)}
-   *                           constructor.
+   * @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 jKgMetersSquared The moment of inertia of the flywheel. If this is unknown, use the
+   *     {@link #FlywheelSim(LinearSystem, DCMotor, double, Matrix)} constructor.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
-  public FlywheelSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
-                     Matrix<N1, N1> measurementStdDevs) {
-    super(LinearSystemId.createFlywheelSystem(gearbox, gearing, jKgMetersSquared),
+  @SuppressWarnings("ParameterName")
+  public FlywheelSim(
+      DCMotor gearbox, double gearing, double jKgMetersSquared, Matrix<N1, N1> measurementStdDevs) {
+    super(
+        LinearSystemId.createFlywheelSystem(gearbox, jKgMetersSquared, gearing),
         measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
index 506be52..c5878ca 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/GenericHIDSim.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.wpilibj.GenericHID;
 
-/**
- * Class to control a simulated generic joystick.
- */
+/** Class to control a simulated generic joystick. */
 public class GenericHIDSim {
   protected final int m_port;
 
@@ -33,65 +28,135 @@
     m_port = port;
   }
 
-  /**
-   * Updates joystick data so that new values are visible to the user program.
-   */
+  /** Updates joystick data so that new values are visible to the user program. */
   public void notifyNewData() {
     DriverStationSim.notifyNewData();
   }
 
+  /**
+   * Set the value of a given button.
+   *
+   * @param button the button to set
+   * @param value the new value
+   */
   public void setRawButton(int button, boolean value) {
     DriverStationSim.setJoystickButton(m_port, button, value);
   }
 
+  /**
+   * Set the value of a given axis.
+   *
+   * @param axis the axis to set
+   * @param value the new value
+   */
   public void setRawAxis(int axis, double value) {
     DriverStationSim.setJoystickAxis(m_port, axis, value);
   }
 
+  /**
+   * Set the value of a given POV.
+   *
+   * @param pov the POV to set
+   * @param value the new value
+   */
   public void setPOV(int pov, int value) {
     DriverStationSim.setJoystickPOV(m_port, pov, value);
   }
 
+  /**
+   * Set the value of the default POV (port 0).
+   *
+   * @param value the new value
+   */
   public void setPOV(int value) {
     setPOV(0, value);
   }
 
+  /**
+   * Set the axis count of this device.
+   *
+   * @param count the new axis count
+   */
   public void setAxisCount(int count) {
     DriverStationSim.setJoystickAxisCount(m_port, count);
   }
 
+  /**
+   * Set the POV count of this device.
+   *
+   * @param count the new POV count
+   */
   public void setPOVCount(int count) {
     DriverStationSim.setJoystickPOVCount(m_port, count);
   }
 
+  /**
+   * Set the button count of this device.
+   *
+   * @param count the new button count
+   */
   public void setButtonCount(int count) {
     DriverStationSim.setJoystickButtonCount(m_port, count);
   }
 
+  /**
+   * Set the type of this device.
+   *
+   * @param type the new device type
+   */
   public void setType(GenericHID.HIDType type) {
     DriverStationSim.setJoystickType(m_port, type.value);
   }
 
+  /**
+   * Set the name of this device.
+   *
+   * @param name the new device name
+   */
   public void setName(String name) {
     DriverStationSim.setJoystickName(m_port, name);
   }
 
+  /**
+   * Set the type of an axis.
+   *
+   * @param axis the axis
+   * @param type the type
+   */
   public void setAxisType(int axis, int type) {
     DriverStationSim.setJoystickAxisType(m_port, axis, type);
   }
 
+  /**
+   * Read the output of a button.
+   *
+   * @param outputNumber the button number
+   * @return the value of the button (true = pressed)
+   */
   public boolean getOutput(int outputNumber) {
     long outputs = getOutputs();
     return (outputs & (1 << (outputNumber - 1))) != 0;
   }
 
+  /**
+   * Get the encoded 16-bit integer that passes button values.
+   *
+   * @return the button values
+   */
   public long getOutputs() {
     return DriverStationSim.getJoystickOutputs(m_port);
   }
 
+  /**
+   * Get the joystick rumble.
+   *
+   * @param type the rumble to read
+   * @return the rumble value
+   */
   public double getRumble(GenericHID.RumbleType type) {
-    int value = DriverStationSim.getJoystickRumble(
-        m_port, type == GenericHID.RumbleType.kLeftRumble ? 0 : 1);
+    int value =
+        DriverStationSim.getJoystickRumble(
+            m_port, type == GenericHID.RumbleType.kLeftRumble ? 0 : 1);
     return value / 65535.0;
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java
index 1252eeb..986e310 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/I2CSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -12,34 +9,75 @@
 import edu.wpi.first.hal.simulation.I2CDataJNI;
 import edu.wpi.first.hal.simulation.NotifyCallback;
 
+/** A class to control a simulated I2C device. */
 public class I2CSim {
   private final int m_index;
 
+  /**
+   * Construct a new I2C simulation object.
+   *
+   * @param index the HAL index of the I2C object
+   */
   public I2CSim(int index) {
     m_index = index;
   }
 
+  /**
+   * Register a callback to be run when this I2C device is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = I2CDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, I2CDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether this I2C device has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return I2CDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether this I2C device has been initialized.
+   *
+   * @param initialized whether this device is initialized
+   */
   public void setInitialized(boolean initialized) {
     I2CDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run whenever a `read` operation is done.
+   *
+   * @param callback the callback that is run on `read` operations
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerReadCallback(BufferCallback callback) {
     int uid = I2CDataJNI.registerReadCallback(m_index, callback);
     return new CallbackStore(m_index, uid, I2CDataJNI::cancelReadCallback);
   }
 
+  /**
+   * Register a callback to be run whenever a `write` operation is done.
+   *
+   * @param callback the callback that is run on `write` operations
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
     int uid = I2CDataJNI.registerWriteCallback(m_index, callback);
     return new CallbackStore(m_index, uid, I2CDataJNI::cancelWriteCallback);
   }
 
+  /** Reset all I2C simulation data. */
   public void resetData() {
     I2CDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
index d1d7100..f55b038 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/JoystickSim.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.wpilibj.Joystick;
 
-/**
- * Class to control a simulated joystick.
- */
+/** Class to control a simulated joystick. */
 public class JoystickSim extends GenericHIDSim {
   private Joystick m_joystick;
 
@@ -42,34 +37,68 @@
     setPOVCount(1);
   }
 
+  /**
+   * Set the X value of the joystick.
+   *
+   * @param value the new X value
+   */
   public void setX(double value) {
     setRawAxis(m_joystick != null ? m_joystick.getXChannel() : Joystick.kDefaultXChannel, value);
   }
 
+  /**
+   * Set the Y value of the joystick.
+   *
+   * @param value the new Y value
+   */
   public void setY(double value) {
     setRawAxis(m_joystick != null ? m_joystick.getYChannel() : Joystick.kDefaultYChannel, value);
   }
 
+  /**
+   * Set the Z value of the joystick.
+   *
+   * @param value the new Z value
+   */
   public void setZ(double value) {
     setRawAxis(m_joystick != null ? m_joystick.getZChannel() : Joystick.kDefaultZChannel, value);
   }
 
+  /**
+   * Set the twist value of the joystick.
+   *
+   * @param value the new twist value
+   */
   public void setTwist(double value) {
     setRawAxis(
-        m_joystick != null ? m_joystick.getTwistChannel() : Joystick.kDefaultTwistChannel,
-        value);
+        m_joystick != null ? m_joystick.getTwistChannel() : Joystick.kDefaultTwistChannel, value);
   }
 
+  /**
+   * Set the throttle value of the joystick.
+   *
+   * @param value the new throttle value
+   */
   public void setThrottle(double value) {
     setRawAxis(
         m_joystick != null ? m_joystick.getThrottleChannel() : Joystick.kDefaultThrottleChannel,
         value);
   }
 
+  /**
+   * Set the trigger value of the joystick.
+   *
+   * @param state the new value
+   */
   public void setTrigger(boolean state) {
     setRawButton(1, state);
   }
 
+  /**
+   * Set the top state of the joystick.
+   *
+   * @param state the new state
+   */
   public void setTop(boolean state) {
     setRawButton(2, state);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
index dc69ffa..2b4e9a9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/LinearSystemSim.java
@@ -1,24 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.wpilibj.RobotController;
 import org.ejml.MatrixDimensionException;
 import org.ejml.simple.SimpleMatrix;
 
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
 /**
- * This class helps simulate linear systems. To use this class, do the following in the
- * {@link edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method.
+ * This class helps simulate linear systems. To use this class, do the following in the {@link
+ * edu.wpi.first.wpilibj.IterativeRobotBase#simulationPeriodic} method.
  *
  * <p>Call {@link #setInput(double...)} with the inputs to the system (usually voltage).
  *
@@ -26,8 +23,8 @@
  *
  * <p>Set simulated sensor readings with the simulated positions in {@link #getOutput()}
  *
- * @param <States>  The number of states of the system.
- * @param <Inputs>  The number of inputs to the system.
+ * @param <States> The number of states of the system.
+ * @param <Inputs> The number of inputs to the system.
  * @param <Outputs> The number of outputs of the system.
  */
 @SuppressWarnings("ClassTypeParameterName")
@@ -38,8 +35,10 @@
   // Variables for state, output, and input.
   @SuppressWarnings("MemberName")
   protected Matrix<States, N1> m_x;
+
   @SuppressWarnings("MemberName")
   protected Matrix<Outputs, N1> m_y;
+
   @SuppressWarnings("MemberName")
   protected Matrix<Inputs, N1> m_u;
 
@@ -59,11 +58,12 @@
   /**
    * Creates a simulated generic linear system with measurement noise.
    *
-   * @param system             The system being controlled.
-   * @param measurementStdDevs Standard deviations of measurements.
+   * @param system The system being controlled.
+   * @param measurementStdDevs Standard deviations of measurements. Can be null if no noise is
+   *     desired.
    */
-  public LinearSystemSim(LinearSystem<States, Inputs, Outputs> system,
-                         Matrix<Outputs, N1> measurementStdDevs) {
+  public LinearSystemSim(
+      LinearSystem<States, Inputs, Outputs> system, Matrix<Outputs, N1> measurementStdDevs) {
     this.m_plant = system;
     this.m_measurementStdDevs = measurementStdDevs;
 
@@ -115,18 +115,20 @@
    *
    * @param u The system inputs.
    */
+  @SuppressWarnings("ParameterName")
   public void setInput(Matrix<Inputs, N1> u) {
-    this.m_u = u;
+    this.m_u = clampInput(u);
   }
 
   /**
    * Sets the system inputs.
    *
-   * @param row   The row in the input matrix to set.
+   * @param row The row in the input matrix to set.
    * @param value The value to set the row to.
    */
   public void setInput(int row, double value) {
     m_u.set(row, 0, value);
+    m_u = clampInput(m_u);
   }
 
   /**
@@ -134,10 +136,11 @@
    *
    * @param u An array of doubles that represent the inputs of the system.
    */
+  @SuppressWarnings("ParameterName")
   public void setInput(double... u) {
     if (u.length != m_u.getNumRows()) {
-      throw new MatrixDimensionException("Malformed input! Got " + u.length
-          + " elements instead of " + m_u.getNumRows());
+      throw new MatrixDimensionException(
+          "Malformed input! Got " + u.length + " elements instead of " + m_u.getNumRows());
     }
     m_u = new Matrix<>(new SimpleMatrix(m_u.getNumRows(), 1, true, u));
   }
@@ -165,12 +168,24 @@
    * Updates the state estimate of the system.
    *
    * @param currentXhat The current state estimate.
-   * @param u           The system inputs (usually voltage).
-   * @param dtSeconds   The time difference between controller updates.
+   * @param u The system inputs (usually voltage).
+   * @param dtSeconds The time difference between controller updates.
    * @return The new state.
    */
-  protected Matrix<States, N1> updateX(Matrix<States, N1> currentXhat,
-                                       Matrix<Inputs, N1> u, double dtSeconds) {
+  @SuppressWarnings("ParameterName")
+  protected Matrix<States, N1> updateX(
+      Matrix<States, N1> currentXhat, Matrix<Inputs, N1> u, double dtSeconds) {
     return m_plant.calculateX(currentXhat, u, dtSeconds);
   }
+
+  /**
+   * Clamp the input vector such that no element exceeds the given voltage. If any does, the
+   * relative magnitudes of the input will be maintained.
+   *
+   * @param u The input vector.
+   * @return The normalized input.
+   */
+  protected Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> u) {
+    return StateSpaceUtil.normalizeInputVector(u, RobotController.getBatteryVoltage());
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java
deleted file mode 100644
index 7006c91..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/Mechanism2D.java
+++ /dev/null
@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.simulation;
-
-import edu.wpi.first.hal.SimDevice;
-import edu.wpi.first.hal.SimDouble;
-import java.util.HashMap;
-import java.util.Map;
-
-public class Mechanism2D {
-  private static final SimDevice m_device = SimDevice.create("Mechanism2D");
-  private static final Map<String, SimDouble> m_createdItems = new HashMap<String, SimDouble>();
-
-  /**
-     * Set/Create the angle of a ligament.
-     *
-     * @param ligamentPath json path to the ligament
-     * @param angle        to set the ligament
-     */
-  public void setLigamentAngle(String ligamentPath, float angle) {
-    ligamentPath = ligamentPath + "/angle";
-    if (m_device != null) {
-      if (!m_createdItems.containsKey(ligamentPath)) {
-        m_createdItems.put(ligamentPath, m_device.createDouble(ligamentPath, false, angle));
-      }
-      m_createdItems.get(ligamentPath).set(angle);
-    }
-  }
-
-  /**
-   * Set/Create the length of a ligament.
-   *
-   * @param ligamentPath json path to the ligament
-   * @param length       to set the ligament
-   */
-  public void setLigamentLength(String ligamentPath, float length) {
-    ligamentPath = ligamentPath + "/length";
-    if (m_device != null) {
-      if (!m_createdItems.containsKey(ligamentPath)) {
-        m_createdItems.put(ligamentPath, m_device.createDouble(ligamentPath, false, length));
-      }
-      m_createdItems.get(ligamentPath).set(length);
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java
index 3778e36..a5be97f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/NotifierSim.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.simulation.NotifierDataJNI;
 
-/**
- * Class to control simulated notifiers.
- */
+/** Class to control simulated notifiers. */
 public final class NotifierSim {
-  private NotifierSim() {
-  }
+  private NotifierSim() {}
 
   /**
    * Gets the timeout of the next notifier.
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java
deleted file mode 100644
index 7881f0c..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PCMSim.java
+++ /dev/null
@@ -1,126 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.simulation;
-
-import edu.wpi.first.hal.simulation.NotifyCallback;
-import edu.wpi.first.hal.simulation.PCMDataJNI;
-import edu.wpi.first.wpilibj.Compressor;
-import edu.wpi.first.wpilibj.SensorUtil;
-
-/**
- * Class to control a simulated Pneumatic Control Module (PCM).
- */
-public class PCMSim {
-  private final int m_index;
-
-  /**
-   * Constructs for the default PCM.
-   */
-  public PCMSim() {
-    m_index = SensorUtil.getDefaultSolenoidModule();
-  }
-
-  /**
-   * Constructs from a PCM module number (CAN ID).
-   *
-   * @param module module number
-   */
-  public PCMSim(int module) {
-    m_index = module;
-  }
-
-  /**
-   * Constructs from a Compressor object.
-   *
-   * @param compressor Compressor connected to PCM to simulate
-   */
-  public PCMSim(Compressor compressor) {
-    m_index = compressor.getModule();
-  }
-
-  public CallbackStore registerSolenoidInitializedCallback(int channel, NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerSolenoidInitializedCallback(m_index, channel, callback, initialNotify);
-    return new CallbackStore(m_index, channel, uid, PCMDataJNI::cancelSolenoidInitializedCallback);
-  }
-  public boolean getSolenoidInitialized(int channel) {
-    return PCMDataJNI.getSolenoidInitialized(m_index, channel);
-  }
-  public void setSolenoidInitialized(int channel, boolean solenoidInitialized) {
-    PCMDataJNI.setSolenoidInitialized(m_index, channel, solenoidInitialized);
-  }
-
-  public CallbackStore registerSolenoidOutputCallback(int channel, NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
-    return new CallbackStore(m_index, channel, uid, PCMDataJNI::cancelSolenoidOutputCallback);
-  }
-  public boolean getSolenoidOutput(int channel) {
-    return PCMDataJNI.getSolenoidOutput(m_index, channel);
-  }
-  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
-    PCMDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
-  }
-
-  public CallbackStore registerCompressorInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerCompressorInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorInitializedCallback);
-  }
-  public boolean getCompressorInitialized() {
-    return PCMDataJNI.getCompressorInitialized(m_index);
-  }
-  public void setCompressorInitialized(boolean compressorInitialized) {
-    PCMDataJNI.setCompressorInitialized(m_index, compressorInitialized);
-  }
-
-  public CallbackStore registerCompressorOnCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorOnCallback);
-  }
-  public boolean getCompressorOn() {
-    return PCMDataJNI.getCompressorOn(m_index);
-  }
-  public void setCompressorOn(boolean compressorOn) {
-    PCMDataJNI.setCompressorOn(m_index, compressorOn);
-  }
-
-  public CallbackStore registerClosedLoopEnabledCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelClosedLoopEnabledCallback);
-  }
-  public boolean getClosedLoopEnabled() {
-    return PCMDataJNI.getClosedLoopEnabled(m_index);
-  }
-  public void setClosedLoopEnabled(boolean closedLoopEnabled) {
-    PCMDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled);
-  }
-
-  public CallbackStore registerPressureSwitchCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelPressureSwitchCallback);
-  }
-  public boolean getPressureSwitch() {
-    return PCMDataJNI.getPressureSwitch(m_index);
-  }
-  public void setPressureSwitch(boolean pressureSwitch) {
-    PCMDataJNI.setPressureSwitch(m_index, pressureSwitch);
-  }
-
-  public CallbackStore registerCompressorCurrentCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PCMDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PCMDataJNI::cancelCompressorCurrentCallback);
-  }
-  public double getCompressorCurrent() {
-    return PCMDataJNI.getCompressorCurrent(m_index);
-  }
-  public void setCompressorCurrent(double compressorCurrent) {
-    PCMDataJNI.setCompressorCurrent(m_index, compressorCurrent);
-  }
-
-  public void resetData() {
-    PCMDataJNI.resetData(m_index);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java
index ad57973..eef8ccf 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PDPSim.java
@@ -1,25 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.simulation.NotifyCallback;
-import edu.wpi.first.hal.simulation.PDPDataJNI;
-import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.hal.simulation.PowerDistributionDataJNI;
+import edu.wpi.first.wpilibj.PowerDistribution;
 
-/**
- * Class to control a simulated Power Distribution Panel (PDP).
- */
+/** Class to control a simulated Power Distribution Panel (PDP). */
 public class PDPSim {
   private final int m_index;
 
-  /**
-   * Constructs for the default PDP.
-   */
+  /** Constructs for the default PDP. */
   public PDPSim() {
     m_index = 0;
   }
@@ -34,59 +27,148 @@
   }
 
   /**
-   * Constructs from a PowerDistributionPanel object.
+   * Constructs from a PowerDistribution object.
    *
-   * @param pdp PowerDistributionPanel to simulate
+   * @param pdp PowerDistribution to simulate
    */
-  public PDPSim(PowerDistributionPanel pdp) {
+  public PDPSim(PowerDistribution pdp) {
     m_index = pdp.getModule();
   }
 
+  /**
+   * Register a callback to be run when the PDP is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PDPDataJNI::cancelInitializedCallback);
+    int uid =
+        PowerDistributionDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PowerDistributionDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether the PDP has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
-    return PDPDataJNI.getInitialized(m_index);
+    return PowerDistributionDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether the PDP has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
-    PDPDataJNI.setInitialized(m_index, initialized);
+    PowerDistributionDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run whenever the PDP temperature changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerTemperatureCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerTemperatureCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PDPDataJNI::cancelTemperatureCallback);
+    int uid =
+        PowerDistributionDataJNI.registerTemperatureCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PowerDistributionDataJNI::cancelTemperatureCallback);
   }
+
+  /**
+   * Check the temperature of the PDP.
+   *
+   * @return the PDP temperature
+   */
   public double getTemperature() {
-    return PDPDataJNI.getTemperature(m_index);
+    return PowerDistributionDataJNI.getTemperature(m_index);
   }
+
+  /**
+   * Define the PDP temperature.
+   *
+   * @param temperature the new PDP temperature
+   */
   public void setTemperature(double temperature) {
-    PDPDataJNI.setTemperature(m_index, temperature);
+    PowerDistributionDataJNI.setTemperature(m_index, temperature);
   }
 
+  /**
+   * Register a callback to be run whenever the PDP voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerVoltageCallback(NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
-    return new CallbackStore(m_index, uid, PDPDataJNI::cancelVoltageCallback);
+    int uid = PowerDistributionDataJNI.registerVoltageCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, PowerDistributionDataJNI::cancelVoltageCallback);
   }
+
+  /**
+   * Check the PDP voltage.
+   *
+   * @return the PDP voltage.
+   */
   public double getVoltage() {
-    return PDPDataJNI.getVoltage(m_index);
+    return PowerDistributionDataJNI.getVoltage(m_index);
   }
+
+  /**
+   * Set the PDP voltage.
+   *
+   * @param voltage the new PDP voltage
+   */
   public void setVoltage(double voltage) {
-    PDPDataJNI.setVoltage(m_index, voltage);
+    PowerDistributionDataJNI.setVoltage(m_index, voltage);
   }
 
-  public CallbackStore registerCurrentCallback(int channel, NotifyCallback callback, boolean initialNotify) {
-    int uid = PDPDataJNI.registerCurrentCallback(m_index, channel, callback, initialNotify);
-    return new CallbackStore(m_index, channel, uid, PDPDataJNI::cancelCurrentCallback);
+  /**
+   * Register a callback to be run whenever the current of a specific channel changes.
+   *
+   * @param channel the channel
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerCurrentCallback(
+      int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        PowerDistributionDataJNI.registerCurrentCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(
+        m_index, channel, uid, PowerDistributionDataJNI::cancelCurrentCallback);
   }
+
+  /**
+   * Read the current in one of the PDP channels.
+   *
+   * @param channel the channel to check
+   * @return the current in the given channel
+   */
   public double getCurrent(int channel) {
-    return PDPDataJNI.getCurrent(m_index, channel);
-  }
-  public void setCurrent(int channel, double current) {
-    PDPDataJNI.setCurrent(m_index, channel, current);
+    return PowerDistributionDataJNI.getCurrent(m_index, channel);
   }
 
+  /**
+   * Change the current in the given channel.
+   *
+   * @param channel the channel to edit
+   * @param current the new current for the channel
+   */
+  public void setCurrent(int channel, double current) {
+    PowerDistributionDataJNI.setCurrent(m_index, channel, current);
+  }
+
+  /** Reset all PDP simulation data. */
   public void resetData() {
-    PDPDataJNI.resetData(m_index);
+    PowerDistributionDataJNI.resetData(m_index);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java
new file mode 100644
index 0000000..69109b8
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PS4ControllerSim.java
@@ -0,0 +1,212 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.wpilibj.PS4Controller;
+
+/** Class to control a simulated PS4 controller. */
+public class PS4ControllerSim extends GenericHIDSim {
+  /**
+   * Constructs from a PS4Controller object.
+   *
+   * @param joystick controller to simulate
+   */
+  public PS4ControllerSim(PS4Controller joystick) {
+    super(joystick);
+    setAxisCount(6);
+    setButtonCount(14);
+  }
+
+  /**
+   * Constructs from a joystick port number.
+   *
+   * @param port port number
+   */
+  public PS4ControllerSim(int port) {
+    super(port);
+    setAxisCount(6);
+    setButtonCount(14);
+  }
+
+  /**
+   * Change the X axis value of the controller's left stick.
+   *
+   * @param value the new value
+   */
+  public void setLeftX(double value) {
+    setRawAxis(PS4Controller.Axis.kLeftX.value, value);
+  }
+
+  /**
+   * Change the X axis value of the controller's right stick.
+   *
+   * @param value the new value
+   */
+  public void setRightX(double value) {
+    setRawAxis(PS4Controller.Axis.kRightX.value, value);
+  }
+
+  /**
+   * Change the Y axis value of the controller's left stick.
+   *
+   * @param value the new value
+   */
+  public void setLeftY(double value) {
+    setRawAxis(PS4Controller.Axis.kLeftY.value, value);
+  }
+
+  /**
+   * Change the Y axis value of the controller's right stick.
+   *
+   * @param value the new value
+   */
+  public void setRightY(double value) {
+    setRawAxis(PS4Controller.Axis.kRightY.value, value);
+  }
+
+  /**
+   * Change the L2 axis axis value of the controller.
+   *
+   * @param value the new value
+   */
+  public void setL2Axis(double value) {
+    setRawAxis(PS4Controller.Axis.kL2.value, value);
+  }
+
+  /**
+   * Change the R2 axis value of the controller.
+   *
+   * @param value the new value
+   */
+  public void setR2Axis(double value) {
+    setRawAxis(PS4Controller.Axis.kR2.value, value);
+  }
+
+  /**
+   * Change the value of the Square button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setSquareButton(boolean value) {
+    setRawButton(PS4Controller.Button.kSquare.value, value);
+  }
+
+  /**
+   * Change the value of the Cross button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setCrossButton(boolean value) {
+    setRawButton(PS4Controller.Button.kCross.value, value);
+  }
+
+  /**
+   * Change the value of the Circle button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setCircleButton(boolean value) {
+    setRawButton(PS4Controller.Button.kCircle.value, value);
+  }
+
+  /**
+   * Change the value of the Triangle button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setTriangleButton(boolean value) {
+    setRawButton(PS4Controller.Button.kTriangle.value, value);
+  }
+
+  /**
+   * Change the value of the L1 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setL1Button(boolean value) {
+    setRawButton(PS4Controller.Button.kL1.value, value);
+  }
+
+  /**
+   * Change the value of the R1 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setR1Button(boolean value) {
+    setRawButton(PS4Controller.Button.kR1.value, value);
+  }
+
+  /**
+   * Change the value of the L2 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setL2Button(boolean value) {
+    setRawButton(PS4Controller.Button.kL2.value, value);
+  }
+
+  /**
+   * Change the value of the R2 button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setR2Button(boolean value) {
+    setRawButton(PS4Controller.Button.kR2.value, value);
+  }
+
+  /**
+   * Change the value of the Share button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setShareButton(boolean value) {
+    setRawButton(PS4Controller.Button.kShare.value, value);
+  }
+
+  /**
+   * Change the value of the Options button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setOptionsButton(boolean value) {
+    setRawButton(PS4Controller.Button.kOptions.value, value);
+  }
+
+  /**
+   * Change the value of the L3 (left stick) button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setL3Button(boolean value) {
+    setRawButton(PS4Controller.Button.kL3.value, value);
+  }
+
+  /**
+   * Change the value of the R3 (right stick) button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setR3Button(boolean value) {
+    setRawButton(PS4Controller.Button.kR3.value, value);
+  }
+
+  /**
+   * Change the value of the PS button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setPSButton(boolean value) {
+    setRawButton(PS4Controller.Button.kPS.value, value);
+  }
+
+  /**
+   * Change the value of the touchpad button on the controller.
+   *
+   * @param value the new value
+   */
+  public void setTouchpad(boolean value) {
+    setRawButton(PS4Controller.Button.kTouchpad.value, value);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java
index d0cca92..8a60e52 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/PWMSim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.hal.simulation.PWMDataJNI;
 import edu.wpi.first.wpilibj.PWM;
 
-/**
- * Class to control a simulated PWM output.
- */
+/** Class to control a simulated PWM output. */
 public class PWMSim {
   private final int m_index;
 
@@ -35,72 +30,193 @@
     m_index = channel;
   }
 
+  /**
+   * Register a callback to be run when the PWM is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = PWMDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, PWMDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether the PWM has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return PWMDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether the PWM has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
     PWMDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run when the PWM raw value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerRawValueCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = PWMDataJNI.registerRawValueCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, PWMDataJNI::cancelRawValueCallback);
   }
+
+  /**
+   * Get the PWM raw value.
+   *
+   * @return the PWM raw value
+   */
   public int getRawValue() {
     return PWMDataJNI.getRawValue(m_index);
   }
+
+  /**
+   * Set the PWM raw value.
+   *
+   * @param rawValue the PWM raw value
+   */
   public void setRawValue(int rawValue) {
     PWMDataJNI.setRawValue(m_index, rawValue);
   }
 
+  /**
+   * Register a callback to be run when the PWM speed changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerSpeedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = PWMDataJNI.registerSpeedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, PWMDataJNI::cancelSpeedCallback);
   }
+
+  /**
+   * Get the PWM speed.
+   *
+   * @return the PWM speed (-1.0 to 1.0)
+   */
   public double getSpeed() {
     return PWMDataJNI.getSpeed(m_index);
   }
+
+  /**
+   * Set the PWM speed.
+   *
+   * @param speed the PWM speed (-1.0 to 1.0)
+   */
   public void setSpeed(double speed) {
     PWMDataJNI.setSpeed(m_index, speed);
   }
 
+  /**
+   * Register a callback to be run when the PWM position changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerPositionCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = PWMDataJNI.registerPositionCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, PWMDataJNI::cancelPositionCallback);
   }
+
+  /**
+   * Get the PWM position.
+   *
+   * @return the PWM position (0.0 to 1.0)
+   */
   public double getPosition() {
     return PWMDataJNI.getPosition(m_index);
   }
+
+  /**
+   * Set the PWM position.
+   *
+   * @param position the PWM position (0.0 to 1.0)
+   */
   public void setPosition(double position) {
     PWMDataJNI.setPosition(m_index, position);
   }
 
+  /**
+   * Register a callback to be run when the PWM period scale changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerPeriodScaleCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = PWMDataJNI.registerPeriodScaleCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, PWMDataJNI::cancelPeriodScaleCallback);
   }
+
+  /**
+   * Get the PWM period scale.
+   *
+   * @return the PWM period scale
+   */
   public int getPeriodScale() {
     return PWMDataJNI.getPeriodScale(m_index);
   }
+
+  /**
+   * Set the PWM period scale.
+   *
+   * @param periodScale the PWM period scale
+   */
   public void setPeriodScale(int periodScale) {
     PWMDataJNI.setPeriodScale(m_index, periodScale);
   }
 
+  /**
+   * Register a callback to be run when the PWM zero latch state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerZeroLatchCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = PWMDataJNI.registerZeroLatchCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, PWMDataJNI::cancelZeroLatchCallback);
   }
+
+  /**
+   * Check whether the PWM is zero latched.
+   *
+   * @return true if zero latched
+   */
   public boolean getZeroLatch() {
     return PWMDataJNI.getZeroLatch(m_index);
   }
+
+  /**
+   * Define whether the PWM has been zero latched.
+   *
+   * @param zeroLatch true to indicate zero latched
+   */
   public void setZeroLatch(boolean zeroLatch) {
     PWMDataJNI.setZeroLatch(m_index, zeroLatch);
   }
 
+  /** Reset all simulation data. */
   public void resetData() {
     PWMDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/REVPHSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/REVPHSim.java
new file mode 100644
index 0000000..8a5f0f8
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/REVPHSim.java
@@ -0,0 +1,239 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+import edu.wpi.first.hal.simulation.REVPHDataJNI;
+import edu.wpi.first.wpilibj.PneumaticsBase;
+import edu.wpi.first.wpilibj.SensorUtil;
+
+/** Class to control a simulated PneumaticHub (PH). */
+@SuppressWarnings("AbbreviationAsWordInName")
+public class REVPHSim {
+  private final int m_index;
+
+  /** Constructs for the default PH. */
+  public REVPHSim() {
+    m_index = SensorUtil.getDefaultREVPHModule();
+  }
+
+  /**
+   * Constructs from a PH module number (CAN ID).
+   *
+   * @param module module number
+   */
+  public REVPHSim(int module) {
+    m_index = module;
+  }
+
+  /**
+   * Constructs from a Compressor object.
+   *
+   * @param module PCM module to simulate
+   */
+  public REVPHSim(PneumaticsBase module) {
+    m_index = module.getModuleNumber();
+  }
+
+  /**
+   * Register a callback to be run when the solenoid output on a channel changes.
+   *
+   * @param channel the channel to monitor
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerSolenoidOutputCallback(
+      int channel, NotifyCallback callback, boolean initialNotify) {
+    int uid =
+        REVPHDataJNI.registerSolenoidOutputCallback(m_index, channel, callback, initialNotify);
+    return new CallbackStore(m_index, channel, uid, REVPHDataJNI::cancelSolenoidOutputCallback);
+  }
+
+  /**
+   * Check the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @return the solenoid output
+   */
+  public boolean getSolenoidOutput(int channel) {
+    return REVPHDataJNI.getSolenoidOutput(m_index, channel);
+  }
+
+  /**
+   * Change the solenoid output on a specific channel.
+   *
+   * @param channel the channel to check
+   * @param solenoidOutput the new solenoid output
+   */
+  public void setSolenoidOutput(int channel, boolean solenoidOutput) {
+    REVPHDataJNI.setSolenoidOutput(m_index, channel, solenoidOutput);
+  }
+
+  /**
+   * Register a callback to be run when the compressor is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelInitializedCallback);
+  }
+
+  /**
+   * Check whether the compressor has been initialized.
+   *
+   * @return true if initialized
+   */
+  public boolean getInitialized() {
+    return REVPHDataJNI.getInitialized(m_index);
+  }
+
+  /**
+   * Define whether the compressor has been initialized.
+   *
+   * @param initialized whether the compressor is initialized
+   */
+  public void setInitialized(boolean initialized) {
+    REVPHDataJNI.setInitialized(m_index, initialized);
+  }
+
+  /**
+   * Register a callback to be run when the compressor activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerCompressorOnCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerCompressorOnCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorOnCallback);
+  }
+
+  /**
+   * Check if the compressor is on.
+   *
+   * @return true if the compressor is active
+   */
+  public boolean getCompressorOn() {
+    return REVPHDataJNI.getCompressorOn(m_index);
+  }
+
+  /**
+   * Set whether the compressor is active.
+   *
+   * @param compressorOn the new value
+   */
+  public void setCompressorOn(boolean compressorOn) {
+    REVPHDataJNI.setCompressorOn(m_index, compressorOn);
+  }
+
+  /**
+   * Register a callback to be run whenever the closed loop state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerClosedLoopEnabledCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerClosedLoopEnabledCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelClosedLoopEnabledCallback);
+  }
+
+  /**
+   * Check whether the closed loop compressor control is active.
+   *
+   * @return true if active
+   */
+  public boolean getClosedLoopEnabled() {
+    return REVPHDataJNI.getClosedLoopEnabled(m_index);
+  }
+
+  /**
+   * Turn on/off the closed loop control of the compressor.
+   *
+   * @param closedLoopEnabled whether the control loop is active
+   */
+  public void setClosedLoopEnabled(boolean closedLoopEnabled) {
+    REVPHDataJNI.setClosedLoopEnabled(m_index, closedLoopEnabled);
+  }
+
+  /**
+   * Register a callback to be run whenever the pressure switch value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerPressureSwitchCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerPressureSwitchCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelPressureSwitchCallback);
+  }
+
+  /**
+   * Check the value of the pressure switch.
+   *
+   * @return the pressure switch value
+   */
+  public boolean getPressureSwitch() {
+    return REVPHDataJNI.getPressureSwitch(m_index);
+  }
+
+  /**
+   * Set the value of the pressure switch.
+   *
+   * @param pressureSwitch the new value
+   */
+  public void setPressureSwitch(boolean pressureSwitch) {
+    REVPHDataJNI.setPressureSwitch(m_index, pressureSwitch);
+  }
+
+  /**
+   * Register a callback to be run whenever the compressor current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerCompressorCurrentCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = REVPHDataJNI.registerCompressorCurrentCallback(m_index, callback, initialNotify);
+    return new CallbackStore(m_index, uid, REVPHDataJNI::cancelCompressorCurrentCallback);
+  }
+
+  /**
+   * Read the compressor current.
+   *
+   * @return the current of the compressor connected to this module
+   */
+  public double getCompressorCurrent() {
+    return REVPHDataJNI.getCompressorCurrent(m_index);
+  }
+
+  /**
+   * Set the compressor current.
+   *
+   * @param compressorCurrent the new compressor current
+   */
+  public void setCompressorCurrent(double compressorCurrent) {
+    REVPHDataJNI.setCompressorCurrent(m_index, compressorCurrent);
+  }
+
+  /** Reset all simulation data for this object. */
+  public void resetData() {
+    REVPHDataJNI.resetData(m_index);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java
index dc7441e..b6f2542 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RelaySim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.hal.simulation.RelayDataJNI;
 import edu.wpi.first.wpilibj.Relay;
 
-/**
- * Class to control a simulated relay.
- */
+/** Class to control a simulated relay. */
 public class RelaySim {
   private final int m_index;
 
@@ -35,50 +30,133 @@
     m_index = channel;
   }
 
-  public CallbackStore registerInitializedForwardCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback to be run when the forward direction is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerInitializedForwardCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RelayDataJNI.registerInitializedForwardCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, RelayDataJNI::cancelInitializedForwardCallback);
   }
+
+  /**
+   * Check whether the forward direction has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitializedForward() {
     return RelayDataJNI.getInitializedForward(m_index);
   }
+
+  /**
+   * Define whether the forward direction has been initialized.
+   *
+   * @param initializedForward whether this object is initialized
+   */
   public void setInitializedForward(boolean initializedForward) {
     RelayDataJNI.setInitializedForward(m_index, initializedForward);
   }
 
-  public CallbackStore registerInitializedReverseCallback(NotifyCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback to be run when the reverse direction is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerInitializedReverseCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RelayDataJNI.registerInitializedReverseCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, RelayDataJNI::cancelInitializedReverseCallback);
   }
+
+  /**
+   * Check whether the reverse direction has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitializedReverse() {
     return RelayDataJNI.getInitializedReverse(m_index);
   }
+
+  /**
+   * Define whether the reverse direction has been initialized.
+   *
+   * @param initializedReverse whether this object is initialized
+   */
   public void setInitializedReverse(boolean initializedReverse) {
     RelayDataJNI.setInitializedReverse(m_index, initializedReverse);
   }
 
+  /**
+   * Register a callback to be run when the forward direction changes state.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerForwardCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = RelayDataJNI.registerForwardCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, RelayDataJNI::cancelForwardCallback);
   }
+
+  /**
+   * Check whether the forward direction is active.
+   *
+   * @return true if active
+   */
   public boolean getForward() {
     return RelayDataJNI.getForward(m_index);
   }
+
+  /**
+   * Set whether the forward direction is active.
+   *
+   * @param forward true to make active
+   */
   public void setForward(boolean forward) {
     RelayDataJNI.setForward(m_index, forward);
   }
 
+  /**
+   * Register a callback to be run when the reverse direction changes state.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerReverseCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = RelayDataJNI.registerReverseCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, RelayDataJNI::cancelReverseCallback);
   }
+
+  /**
+   * Check whether the reverse direction is active.
+   *
+   * @return true if active
+   */
   public boolean getReverse() {
     return RelayDataJNI.getReverse(m_index);
   }
+
+  /**
+   * Set whether the reverse direction is active.
+   *
+   * @param reverse true to make active
+   */
   public void setReverse(boolean reverse) {
     RelayDataJNI.setReverse(m_index, reverse);
   }
 
+  /** Reset all simulation data. */
   public void resetData() {
     RelayDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
index f90ce0f..976ba4a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/RoboRioSim.java
@@ -1,200 +1,537 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.hal.simulation.RoboRioDataJNI;
 
-/**
- * Class to control a simulated RoboRIO.
- */
-@SuppressWarnings({"PMD.ExcessivePublicCount", "PMD.UseUtilityClass"})
-public class RoboRioSim {
-  public static CallbackStore registerFPGAButtonCallback(NotifyCallback callback,
-                                                         boolean initialNotify) {
+/** A utility class to control a simulated RoboRIO. */
+public final class RoboRioSim {
+  private RoboRioSim() {
+    // Utility class
+  }
+
+  /**
+   * Register a callback to be run when the FPGA button state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public static CallbackStore registerFPGAButtonCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerFPGAButtonCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelFPGAButtonCallback);
   }
+
+  /**
+   * Query the state of the FPGA button.
+   *
+   * @return the FPGA button state
+   */
+  @SuppressWarnings("AbbreviationAsWordInName")
   public static boolean getFPGAButton() {
     return RoboRioDataJNI.getFPGAButton();
   }
-  public static void setFPGAButton(boolean fPGAButton) {
-    RoboRioDataJNI.setFPGAButton(fPGAButton);
+
+  /**
+   * Define the state of the FPGA button.
+   *
+   * @param fpgaButton the new state
+   */
+  @SuppressWarnings("AbbreviationAsWordInName")
+  public static void setFPGAButton(boolean fpgaButton) {
+    RoboRioDataJNI.setFPGAButton(fpgaButton);
   }
 
-  public static CallbackStore registerVInVoltageCallback(NotifyCallback callback,
-                                                         boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the Vin voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerVInVoltageCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerVInVoltageCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelVInVoltageCallback);
   }
+
+  /**
+   * Measure the Vin voltage.
+   *
+   * @return the Vin voltage
+   */
   public static double getVInVoltage() {
     return RoboRioDataJNI.getVInVoltage();
   }
+
+  /**
+   * Define the Vin voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  @SuppressWarnings("ParameterName")
   public static void setVInVoltage(double vInVoltage) {
     RoboRioDataJNI.setVInVoltage(vInVoltage);
   }
 
-  public static CallbackStore registerVInCurrentCallback(NotifyCallback callback,
-                                                         boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the Vin current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerVInCurrentCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerVInCurrentCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelVInCurrentCallback);
   }
+
+  /**
+   * Measure the Vin current.
+   *
+   * @return the Vin current
+   */
   public static double getVInCurrent() {
     return RoboRioDataJNI.getVInCurrent();
   }
+
+  /**
+   * Define the Vin current.
+   *
+   * @param vInCurrent the new current
+   */
+  @SuppressWarnings("ParameterName")
   public static void setVInCurrent(double vInCurrent) {
     RoboRioDataJNI.setVInCurrent(vInCurrent);
   }
 
-  public static CallbackStore registerUserVoltage6VCallback(NotifyCallback callback,
-                                                            boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 6V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserVoltage6VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserVoltage6VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage6VCallback);
   }
+
+  /**
+   * Measure the 6V rail voltage.
+   *
+   * @return the 6V rail voltage
+   */
   public static double getUserVoltage6V() {
     return RoboRioDataJNI.getUserVoltage6V();
   }
+
+  /**
+   * Define the 6V rail voltage.
+   *
+   * @param userVoltage6V the new voltage
+   */
   public static void setUserVoltage6V(double userVoltage6V) {
     RoboRioDataJNI.setUserVoltage6V(userVoltage6V);
   }
 
-  public static CallbackStore registerUserCurrent6VCallback(NotifyCallback callback,
-                                                            boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 6V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserCurrent6VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserCurrent6VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent6VCallback);
   }
+
+  /**
+   * Measure the 6V rail current.
+   *
+   * @return the 6V rail current
+   */
   public static double getUserCurrent6V() {
     return RoboRioDataJNI.getUserCurrent6V();
   }
+
+  /**
+   * Define the 6V rail current.
+   *
+   * @param userCurrent6V the new current
+   */
   public static void setUserCurrent6V(double userCurrent6V) {
     RoboRioDataJNI.setUserCurrent6V(userCurrent6V);
   }
 
-  public static CallbackStore registerUserActive6VCallback(NotifyCallback callback,
-                                                           boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 6V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserActive6VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserActive6VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive6VCallback);
   }
+
+  /**
+   * Get the 6V rail active state.
+   *
+   * @return true if the 6V rail is active
+   */
   public static boolean getUserActive6V() {
     return RoboRioDataJNI.getUserActive6V();
   }
+
+  /**
+   * Set the 6V rail active state.
+   *
+   * @param userActive6V true to make rail active
+   */
   public static void setUserActive6V(boolean userActive6V) {
     RoboRioDataJNI.setUserActive6V(userActive6V);
   }
 
-  public static CallbackStore registerUserVoltage5VCallback(NotifyCallback callback,
-                                                            boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 5V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserVoltage5VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserVoltage5VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage5VCallback);
   }
+
+  /**
+   * Measure the 5V rail voltage.
+   *
+   * @return the 5V rail voltage
+   */
   public static double getUserVoltage5V() {
     return RoboRioDataJNI.getUserVoltage5V();
   }
+
+  /**
+   * Define the 5V rail voltage.
+   *
+   * @param userVoltage5V the new voltage
+   */
   public static void setUserVoltage5V(double userVoltage5V) {
     RoboRioDataJNI.setUserVoltage5V(userVoltage5V);
   }
 
-  public static CallbackStore registerUserCurrent5VCallback(NotifyCallback callback,
-                                                            boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 5V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserCurrent5VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserCurrent5VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent5VCallback);
   }
+
+  /**
+   * Measure the 5V rail current.
+   *
+   * @return the 5V rail current
+   */
   public static double getUserCurrent5V() {
     return RoboRioDataJNI.getUserCurrent5V();
   }
+
+  /**
+   * Define the 5V rail current.
+   *
+   * @param userCurrent5V the new current
+   */
   public static void setUserCurrent5V(double userCurrent5V) {
     RoboRioDataJNI.setUserCurrent5V(userCurrent5V);
   }
 
-  public static CallbackStore registerUserActive5VCallback(NotifyCallback callback,
-                                                           boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 5V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserActive5VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserActive5VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive5VCallback);
   }
+
+  /**
+   * Get the 5V rail active state.
+   *
+   * @return true if the 5V rail is active
+   */
   public static boolean getUserActive5V() {
     return RoboRioDataJNI.getUserActive5V();
   }
+
+  /**
+   * Set the 5V rail active state.
+   *
+   * @param userActive5V true to make rail active
+   */
   public static void setUserActive5V(boolean userActive5V) {
     RoboRioDataJNI.setUserActive5V(userActive5V);
   }
 
-  public static CallbackStore registerUserVoltage3V3Callback(NotifyCallback callback,
-                                                             boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 3.3V rail voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserVoltage3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserVoltage3V3Callback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserVoltage3V3Callback);
   }
+
+  /**
+   * Measure the 3.3V rail voltage.
+   *
+   * @return the 3.3V rail voltage
+   */
   public static double getUserVoltage3V3() {
     return RoboRioDataJNI.getUserVoltage3V3();
   }
+
+  /**
+   * Define the 3.3V rail voltage.
+   *
+   * @param userVoltage3V3 the new voltage
+   */
   public static void setUserVoltage3V3(double userVoltage3V3) {
     RoboRioDataJNI.setUserVoltage3V3(userVoltage3V3);
   }
 
-  public static CallbackStore registerUserCurrent3V3Callback(NotifyCallback callback,
-                                                             boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 3.3V rail current changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserCurrent3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserCurrent3V3Callback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserCurrent3V3Callback);
   }
+
+  /**
+   * Measure the 3.3V rail current.
+   *
+   * @return the 3.3V rail current
+   */
   public static double getUserCurrent3V3() {
     return RoboRioDataJNI.getUserCurrent3V3();
   }
+
+  /**
+   * Define the 3.3V rail current.
+   *
+   * @param userCurrent3V3 the new current
+   */
   public static void setUserCurrent3V3(double userCurrent3V3) {
     RoboRioDataJNI.setUserCurrent3V3(userCurrent3V3);
   }
 
-  public static CallbackStore registerUserActive3V3Callback(NotifyCallback callback,
-                                                            boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 3.3V rail active state changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserActive3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserActive3V3Callback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserActive3V3Callback);
   }
+
+  /**
+   * Get the 3.3V rail active state.
+   *
+   * @return true if the 3.3V rail is active
+   */
   public static boolean getUserActive3V3() {
     return RoboRioDataJNI.getUserActive3V3();
   }
+
+  /**
+   * Set the 3.3V rail active state.
+   *
+   * @param userActive3V3 true to make rail active
+   */
   public static void setUserActive3V3(boolean userActive3V3) {
     RoboRioDataJNI.setUserActive3V3(userActive3V3);
   }
 
-  public static CallbackStore registerUserFaults6VCallback(NotifyCallback callback,
-                                                           boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 6V rail number of faults changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserFaults6VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserFaults6VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults6VCallback);
   }
+
+  /**
+   * Get the 6V rail number of faults.
+   *
+   * @return number of faults
+   */
   public static int getUserFaults6V() {
     return RoboRioDataJNI.getUserFaults6V();
   }
+
+  /**
+   * Set the 6V rail number of faults.
+   *
+   * @param userFaults6V number of faults
+   */
   public static void setUserFaults6V(int userFaults6V) {
     RoboRioDataJNI.setUserFaults6V(userFaults6V);
   }
 
-  public static CallbackStore registerUserFaults5VCallback(NotifyCallback callback,
-                                                           boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 5V rail number of faults changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserFaults5VCallback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserFaults5VCallback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults5VCallback);
   }
+
+  /**
+   * Get the 5V rail number of faults.
+   *
+   * @return number of faults
+   */
   public static int getUserFaults5V() {
     return RoboRioDataJNI.getUserFaults5V();
   }
+
+  /**
+   * Set the 5V rail number of faults.
+   *
+   * @param userFaults5V number of faults
+   */
   public static void setUserFaults5V(int userFaults5V) {
     RoboRioDataJNI.setUserFaults5V(userFaults5V);
   }
 
-  public static CallbackStore registerUserFaults3V3Callback(NotifyCallback callback,
-                                                            boolean initialNotify) {
+  /**
+   * Register a callback to be run whenever the 3.3V rail number of faults changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether the callback should be called with the initial value
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerUserFaults3V3Callback(
+      NotifyCallback callback, boolean initialNotify) {
     int uid = RoboRioDataJNI.registerUserFaults3V3Callback(callback, initialNotify);
     return new CallbackStore(uid, RoboRioDataJNI::cancelUserFaults3V3Callback);
   }
+
+  /**
+   * Get the 3.3V rail number of faults.
+   *
+   * @return number of faults
+   */
   public static int getUserFaults3V3() {
     return RoboRioDataJNI.getUserFaults3V3();
   }
+
+  /**
+   * Set the 3.3V rail number of faults.
+   *
+   * @param userFaults3V3 number of faults
+   */
   public static void setUserFaults3V3(int userFaults3V3) {
     RoboRioDataJNI.setUserFaults3V3(userFaults3V3);
   }
 
+  /**
+   * Register a callback to be run whenever the Brownout voltage changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerBrownoutVoltageCallback(
+      NotifyCallback callback, boolean initialNotify) {
+    int uid = RoboRioDataJNI.registerBrownoutVoltageCallback(callback, initialNotify);
+    return new CallbackStore(uid, RoboRioDataJNI::cancelBrownoutVoltageCallback);
+  }
+
+  /**
+   * Measure the Brownout voltage.
+   *
+   * @return the Brownout voltage
+   */
+  public static double getBrownoutVoltage() {
+    return RoboRioDataJNI.getBrownoutVoltage();
+  }
+
+  /**
+   * Define the Brownout voltage.
+   *
+   * @param vInVoltage the new voltage
+   */
+  @SuppressWarnings("ParameterName")
+  public static void setBrownoutVoltage(double vInVoltage) {
+    RoboRioDataJNI.setBrownoutVoltage(vInVoltage);
+  }
+
+  /** Reset all simulation data. */
   public static void resetData() {
     RoboRioDataJNI.resetData();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
index ce6b5c1..da1213a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
@@ -1,77 +1,184 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.simulation.NotifyCallback;
 import edu.wpi.first.hal.simulation.SPIAccelerometerDataJNI;
 
+/** A class to control a simulated accelerometer over SPI. */
 public class SPIAccelerometerSim {
   private final int m_index;
 
+  /**
+   * Construct a new simulation object.
+   *
+   * @param index the HAL index of the accelerometer
+   */
   public SPIAccelerometerSim(int index) {
     m_index = index;
   }
 
+  /**
+   * Register a callback to be run when this accelerometer activates.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = SPIAccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelActiveCallback);
   }
+
+  /**
+   * Check whether the accelerometer is active.
+   *
+   * @return true if active
+   */
   public boolean getActive() {
     return SPIAccelerometerDataJNI.getActive(m_index);
   }
+
+  /**
+   * Define whether this accelerometer is active.
+   *
+   * @param active the new state
+   */
   public void setActive(boolean active) {
     SPIAccelerometerDataJNI.setActive(m_index, active);
   }
 
+  /**
+   * Register a callback to be run whenever the range changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = SPIAccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelRangeCallback);
   }
+
+  /**
+   * Check the range of this accelerometer.
+   *
+   * @return the accelerometer range
+   */
   public int getRange() {
     return SPIAccelerometerDataJNI.getRange(m_index);
   }
+
+  /**
+   * Change the range of this accelerometer.
+   *
+   * @param range the new accelerometer range
+   */
   public void setRange(int range) {
     SPIAccelerometerDataJNI.setRange(m_index, range);
   }
 
+  /**
+   * Register a callback to be run whenever the X axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = SPIAccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelXCallback);
   }
+
+  /**
+   * Measure the X axis value.
+   *
+   * @return the X axis measurement
+   */
   public double getX() {
     return SPIAccelerometerDataJNI.getX(m_index);
   }
+
+  /**
+   * Change the X axis value of the accelerometer.
+   *
+   * @param x the new reading of the X axis
+   */
+  @SuppressWarnings("ParameterName")
   public void setX(double x) {
     SPIAccelerometerDataJNI.setX(m_index, x);
   }
 
+  /**
+   * Register a callback to be run whenever the Y axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = SPIAccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelYCallback);
   }
+
+  /**
+   * Measure the Y axis value.
+   *
+   * @return the Y axis measurement
+   */
   public double getY() {
     return SPIAccelerometerDataJNI.getY(m_index);
   }
+
+  /**
+   * Change the Y axis value of the accelerometer.
+   *
+   * @param y the new reading of the Y axis
+   */
+  @SuppressWarnings("ParameterName")
   public void setY(double y) {
     SPIAccelerometerDataJNI.setY(m_index, y);
   }
 
+  /**
+   * Register a callback to be run whenever the Z axis value changes.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to call the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = SPIAccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelZCallback);
   }
+
+  /**
+   * Measure the Z axis value.
+   *
+   * @return the Z axis measurement
+   */
   public double getZ() {
     return SPIAccelerometerDataJNI.getZ(m_index);
   }
+
+  /**
+   * Change the Z axis value of the accelerometer.
+   *
+   * @param z the new reading of the Z axis
+   */
+  @SuppressWarnings("ParameterName")
   public void setZ(double z) {
     SPIAccelerometerDataJNI.setZ(m_index, z);
   }
 
+  /** Reset all simulation data of this object. */
   public void resetData() {
     SPIAccelerometerDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
index 71c197a..6c0b9e9 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
@@ -13,39 +10,77 @@
 import edu.wpi.first.hal.simulation.SPIDataJNI;
 import edu.wpi.first.hal.simulation.SpiReadAutoReceiveBufferCallback;
 
+/** A class for controlling a simulated SPI device. */
 public class SPISim {
   private final int m_index;
 
+  /** Create a new simulated SPI device. */
   public SPISim() {
     m_index = 0;
   }
 
+  /**
+   * Register a callback to be run when this device is initialized.
+   *
+   * @param callback the callback
+   * @param initialNotify whether to run the callback with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
     int uid = SPIDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
     return new CallbackStore(m_index, uid, SPIDataJNI::cancelInitializedCallback);
   }
+
+  /**
+   * Check whether this device has been initialized.
+   *
+   * @return true if initialized
+   */
   public boolean getInitialized() {
     return SPIDataJNI.getInitialized(m_index);
   }
+
+  /**
+   * Define whether this device has been initialized.
+   *
+   * @param initialized whether this object is initialized
+   */
   public void setInitialized(boolean initialized) {
     SPIDataJNI.setInitialized(m_index, initialized);
   }
 
+  /**
+   * Register a callback to be run whenever a `read` operation is executed.
+   *
+   * @param callback the callback
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerReadCallback(BufferCallback callback) {
     int uid = SPIDataJNI.registerReadCallback(m_index, callback);
     return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadCallback);
   }
 
+  /**
+   * Register a callback to be run whenever a `write` operation is executed.
+   *
+   * @param callback the callback
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
   public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
     int uid = SPIDataJNI.registerWriteCallback(m_index, callback);
     return new CallbackStore(m_index, uid, SPIDataJNI::cancelWriteCallback);
   }
 
-  public CallbackStore registerReadAutoReceiveBufferCallback(SpiReadAutoReceiveBufferCallback callback) {
+  public CallbackStore registerReadAutoReceiveBufferCallback(
+      SpiReadAutoReceiveBufferCallback callback) {
     int uid = SPIDataJNI.registerReadAutoReceiveBufferCallback(m_index, callback);
     return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadAutoReceiveBufferCallback);
   }
 
+  /** Reset all simulation data. */
   public void resetData() {
     SPIDataJNI.resetData(m_index);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java
index 713c112..bca1f6f 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimDeviceSim.java
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDouble;
 import edu.wpi.first.hal.SimEnum;
+import edu.wpi.first.hal.SimInt;
+import edu.wpi.first.hal.SimLong;
 import edu.wpi.first.hal.SimValue;
 import edu.wpi.first.hal.simulation.SimDeviceCallback;
 import edu.wpi.first.hal.simulation.SimDeviceDataJNI;
 import edu.wpi.first.hal.simulation.SimValueCallback;
 
-/**
- * Class to control the simulation side of a SimDevice.
- */
+/** Class to control the simulation side of a SimDevice. */
 public class SimDeviceSim {
   private final int m_handle;
 
@@ -30,6 +27,33 @@
     m_handle = SimDeviceDataJNI.getSimDeviceHandle(name);
   }
 
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   */
+  public SimDeviceSim(String name, int index) {
+    this(name + "[" + index + "]");
+  }
+
+  /**
+   * Constructs a SimDeviceSim.
+   *
+   * @param name name of the SimDevice
+   * @param index device index number to append to name
+   * @param channel device channel number to append to name
+   */
+  public SimDeviceSim(String name, int index, int channel) {
+    this(name + "[" + index + "," + channel + "]");
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   public SimValue getValue(String name) {
     int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
     if (handle <= 0) {
@@ -38,6 +62,40 @@
     return new SimValue(handle);
   }
 
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimInt getInt(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimInt(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
+  public SimLong getLong(String name) {
+    int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
+    if (handle <= 0) {
+      return null;
+    }
+    return new SimLong(handle);
+  }
+
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   public SimDouble getDouble(String name) {
     int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
     if (handle <= 0) {
@@ -46,6 +104,12 @@
     return new SimDouble(handle);
   }
 
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   public SimEnum getEnum(String name) {
     int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
     if (handle <= 0) {
@@ -54,6 +118,12 @@
     return new SimEnum(handle);
   }
 
+  /**
+   * Get the property object with the given name.
+   *
+   * @param name the property name
+   * @return the property object
+   */
   public SimBoolean getBoolean(String name) {
     int handle = SimDeviceDataJNI.getSimValueHandle(m_handle, name);
     if (handle <= 0) {
@@ -62,42 +132,125 @@
     return new SimBoolean(handle);
   }
 
+  /**
+   * Get all options for the given enum.
+   *
+   * @param val the enum
+   * @return names of the different values for that enum
+   */
   public static String[] getEnumOptions(SimEnum val) {
     return SimDeviceDataJNI.getSimValueEnumOptions(val.getNativeHandle());
   }
 
+  /**
+   * Get all data of this object.
+   *
+   * @return all data and fields of this object
+   */
   public SimDeviceDataJNI.SimValueInfo[] enumerateValues() {
     return SimDeviceDataJNI.enumerateSimValues(m_handle);
   }
 
+  /**
+   * Get the native handle of this object.
+   *
+   * @return the handle used to refer to this object through JNI
+   */
   public int getNativeHandle() {
     return m_handle;
   }
 
-  public CallbackStore registerValueCreatedCallback(SimValueCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback to be run every time a new value is added to this device.
+   *
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerValueCreatedCallback(
+      SimValueCallback callback, boolean initialNotify) {
     int uid = SimDeviceDataJNI.registerSimValueCreatedCallback(m_handle, callback, initialNotify);
     return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueCreatedCallback);
   }
 
-  public CallbackStore registerValueChangedCallback(SimValueCallback callback, boolean initialNotify) {
-    int uid = SimDeviceDataJNI.registerSimValueChangedCallback(m_handle, callback, initialNotify);
+  /**
+   * Register a callback to be run every time a value is changed on this device.
+   *
+   * @param value simulated value
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerValueChangedCallback(
+      SimValue value, SimValueCallback callback, boolean initialNotify) {
+    int uid =
+        SimDeviceDataJNI.registerSimValueChangedCallback(
+            value.getNativeHandle(), callback, initialNotify);
     return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueChangedCallback);
   }
 
+  /**
+   * Register a callback for SimDouble.reset() and similar functions. The callback is called with
+   * the old value.
+   *
+   * @param value simulated value
+   * @param callback callback
+   * @param initialNotify ignored (present for consistency)
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public CallbackStore registerValueResetCallback(
+      SimValue value, SimValueCallback callback, boolean initialNotify) {
+    int uid =
+        SimDeviceDataJNI.registerSimValueResetCallback(
+            value.getNativeHandle(), callback, initialNotify);
+    return new CallbackStore(uid, SimDeviceDataJNI::cancelSimValueResetCallback);
+  }
+
+  /**
+   * Get all sim devices with the given prefix.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @return all sim devices
+   */
   public static SimDeviceDataJNI.SimDeviceInfo[] enumerateDevices(String prefix) {
     return SimDeviceDataJNI.enumerateSimDevices(prefix);
   }
 
-  public static CallbackStore registerDeviceCreatedCallback(String prefix, SimDeviceCallback callback, boolean initialNotify) {
+  /**
+   * Register a callback to be run every time a new {@link edu.wpi.first.hal.SimDevice} is created.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerDeviceCreatedCallback(
+      String prefix, SimDeviceCallback callback, boolean initialNotify) {
     int uid = SimDeviceDataJNI.registerSimDeviceCreatedCallback(prefix, callback, initialNotify);
     return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceCreatedCallback);
   }
 
-  public static CallbackStore registerDeviceFreedCallback(String prefix, SimDeviceCallback callback) {
-    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback);
+  /**
+   * Register a callback to be run every time a {@link edu.wpi.first.hal.SimDevice} is
+   * freed/destroyed.
+   *
+   * @param prefix the prefix to filter sim devices
+   * @param callback the callback
+   * @param initialNotify should the callback be run with the initial state
+   * @return the {@link CallbackStore} object associated with this callback. Save a reference to
+   *     this object so GC doesn't cancel the callback.
+   */
+  public static CallbackStore registerDeviceFreedCallback(
+      String prefix, SimDeviceCallback callback, boolean initialNotify) {
+    int uid = SimDeviceDataJNI.registerSimDeviceFreedCallback(prefix, callback, initialNotify);
     return new CallbackStore(uid, SimDeviceDataJNI::cancelSimDeviceFreedCallback);
   }
 
+  /** Reset all SimDevice data. */
   public static void resetData() {
     SimDeviceDataJNI.resetSimDeviceData();
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
index 9c17e1c..1664081 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SimHooks.java
@@ -1,18 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
 import edu.wpi.first.hal.simulation.SimulatorJNI;
 
 public final class SimHooks {
-  private SimHooks() {
-  }
+  private SimHooks() {}
 
+  /**
+   * Override the HAL runtime type (simulated/real).
+   *
+   * @param type runtime type
+   */
   public static void setHALRuntimeType(int type) {
     SimulatorJNI.setRuntimeType(type);
   }
@@ -29,26 +30,44 @@
     return SimulatorJNI.getProgramStarted();
   }
 
+  /** Restart the simulator time. */
   public static void restartTiming() {
     SimulatorJNI.restartTiming();
   }
 
+  /** Pause the simulator time. */
   public static void pauseTiming() {
     SimulatorJNI.pauseTiming();
   }
 
+  /** Resume the simulator time. */
   public static void resumeTiming() {
     SimulatorJNI.resumeTiming();
   }
 
+  /**
+   * Check if the simulator time is paused.
+   *
+   * @return true if paused
+   */
   public static boolean isTimingPaused() {
     return SimulatorJNI.isTimingPaused();
   }
 
+  /**
+   * Advance the simulator time and wait for all notifiers to run.
+   *
+   * @param deltaSeconds the amount to advance (in seconds)
+   */
   public static void stepTiming(double deltaSeconds) {
     SimulatorJNI.stepTiming((long) (deltaSeconds * 1e6));
   }
 
+  /**
+   * Advance the simulator time and return immediately.
+   *
+   * @param deltaSeconds the amount to advance (in seconds)
+   */
   public static void stepTimingAsync(double deltaSeconds) {
     SimulatorJNI.stepTimingAsync((long) (deltaSeconds * 1e6));
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
index f3188a0..87981f7 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSim.java
@@ -1,24 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 
-/**
- * Represents a simulated single jointed arm mechanism.
- */
+/** Represents a simulated single jointed arm mechanism. */
 public class SingleJointedArmSim extends LinearSystemSim<N2, N1, N1> {
   // The gearbox for the arm.
   private final DCMotor m_gearbox;
@@ -45,39 +40,59 @@
   /**
    * Creates a simulated arm mechanism.
    *
-   * @param plant           The linear system that represents the arm.
-   * @param gearbox         The type of and number of motors in the arm gearbox.
-   * @param gearing         The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param plant The linear system that represents the arm.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
    * @param armLengthMeters The length of the arm.
-   * @param minAngleRads    The minimum angle that the arm is capable of.
-   * @param maxAngleRads    The maximum angle that the arm is capable of.
-   * @param armMassKg       The mass of the arm.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param armMassKg The mass of the arm.
    * @param simulateGravity Whether gravity should be simulated or not.
    */
-  public SingleJointedArmSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
-                             double armLengthMeters, double minAngleRads, double maxAngleRads,
-                             double armMassKg, boolean simulateGravity) {
-    this(plant, gearbox, gearing, armLengthMeters, minAngleRads, maxAngleRads, armMassKg,
-        simulateGravity, null);
+  public SingleJointedArmSim(
+      LinearSystem<N2, N1, N1> plant,
+      DCMotor gearbox,
+      double gearing,
+      double armLengthMeters,
+      double minAngleRads,
+      double maxAngleRads,
+      double armMassKg,
+      boolean simulateGravity) {
+    this(
+        plant,
+        gearbox,
+        gearing,
+        armLengthMeters,
+        minAngleRads,
+        maxAngleRads,
+        armMassKg,
+        simulateGravity,
+        null);
   }
 
   /**
    * Creates a simulated arm mechanism.
    *
-   * @param plant              The linear system that represents the arm.
-   * @param gearbox            The type of and number of motors in the arm gearbox.
-   * @param gearing            The gearing of the arm (numbers greater than 1 represent reductions).
-   * @param armLengthMeters    The length of the arm.
-   * @param minAngleRads       The minimum angle that the arm is capable of.
-   * @param maxAngleRads       The maximum angle that the arm is capable of.
-   * @param armMassKg          The mass of the arm.
-   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param plant The linear system that represents the arm.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param armLengthMeters The length of the arm.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param armMassKg The mass of the arm.
+   * @param simulateGravity Whether gravity should be simulated or not.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
-  public SingleJointedArmSim(LinearSystem<N2, N1, N1> plant, DCMotor gearbox, double gearing,
-                             double armLengthMeters, double minAngleRads, double maxAngleRads,
-                             double armMassKg, boolean simulateGravity,
-                             Matrix<N1, N1> measurementStdDevs) {
+  public SingleJointedArmSim(
+      LinearSystem<N2, N1, N1> plant,
+      DCMotor gearbox,
+      double gearing,
+      double armLengthMeters,
+      double minAngleRads,
+      double maxAngleRads,
+      double armMassKg,
+      boolean simulateGravity,
+      Matrix<N1, N1> measurementStdDevs) {
     super(plant, measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
@@ -91,40 +106,63 @@
   /**
    * Creates a simulated arm mechanism.
    *
-   * @param gearbox          The type of and number of motors in the arm gearbox.
-   * @param gearing          The gearing of the arm (numbers greater than 1 represent reductions).
-   * @param jKgMetersSquared The moment of inertia of the arm. This can be calculated from CAD software.
-   * @param armLengthMeters  The length of the arm.
-   * @param minAngleRads     The minimum angle that the arm is capable of.
-   * @param maxAngleRads     The maximum angle that the arm is capable of.
-   * @param armMassKg        The mass of the arm.
-   * @param simulateGravity  Whether gravity should be simulated or not.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the arm, can be calculated from CAD software.
+   * @param armLengthMeters The length of the arm.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param armMassKg The mass of the arm.
+   * @param simulateGravity Whether gravity should be simulated or not.
    */
-  public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
-                             double armLengthMeters, double minAngleRads, double maxAngleRads,
-                             double armMassKg, boolean simulateGravity) {
-    this(gearbox, gearing, jKgMetersSquared, armLengthMeters, minAngleRads, maxAngleRads,
-        armMassKg, simulateGravity, null);
+  @SuppressWarnings("ParameterName")
+  public SingleJointedArmSim(
+      DCMotor gearbox,
+      double gearing,
+      double jKgMetersSquared,
+      double armLengthMeters,
+      double minAngleRads,
+      double maxAngleRads,
+      double armMassKg,
+      boolean simulateGravity) {
+    this(
+        gearbox,
+        gearing,
+        jKgMetersSquared,
+        armLengthMeters,
+        minAngleRads,
+        maxAngleRads,
+        armMassKg,
+        simulateGravity,
+        null);
   }
 
   /**
    * Creates a simulated arm mechanism.
    *
-   * @param gearbox            The type of and number of motors in the arm gearbox.
-   * @param gearing            The gearing of the arm (numbers greater than 1 represent reductions).
-   * @param jKgMetersSquared   The moment of inertia of the arm. This can be calculated from CAD software.
-   * @param armLengthMeters    The length of the arm.
-   * @param minAngleRads       The minimum angle that the arm is capable of.
-   * @param maxAngleRads       The maximum angle that the arm is capable of.
-   * @param armMassKg          The mass of the arm.
-   * @param simulateGravity    Whether gravity should be simulated or not.
+   * @param gearbox The type of and number of motors in the arm gearbox.
+   * @param gearing The gearing of the arm (numbers greater than 1 represent reductions).
+   * @param jKgMetersSquared The moment of inertia of the arm; can be calculated from CAD software.
+   * @param armLengthMeters The length of the arm.
+   * @param minAngleRads The minimum angle that the arm is capable of.
+   * @param maxAngleRads The maximum angle that the arm is capable of.
+   * @param armMassKg The mass of the arm.
+   * @param simulateGravity Whether gravity should be simulated or not.
    * @param measurementStdDevs The standard deviations of the measurements.
    */
-  public SingleJointedArmSim(DCMotor gearbox, double gearing, double jKgMetersSquared,
-                             double armLengthMeters, double minAngleRads, double maxAngleRads,
-                             double armMassKg, boolean simulateGravity,
-                             Matrix<N1, N1> measurementStdDevs) {
-    super(LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
+  @SuppressWarnings("ParameterName")
+  public SingleJointedArmSim(
+      DCMotor gearbox,
+      double gearing,
+      double jKgMetersSquared,
+      double armLengthMeters,
+      double minAngleRads,
+      double maxAngleRads,
+      double armMassKg,
+      boolean simulateGravity,
+      Matrix<N1, N1> measurementStdDevs) {
+    super(
+        LinearSystemId.createSingleJointedArmSystem(gearbox, jKgMetersSquared, gearing),
         measurementStdDevs);
     m_gearbox = gearbox;
     m_gearing = gearing;
@@ -136,23 +174,41 @@
   }
 
   /**
+   * Returns whether the arm would hit the lower limit.
+   *
+   * @param currentAngleRads The current arm height.
+   * @return Whether the arm would hit the lower limit.
+   */
+  public boolean wouldHitLowerLimit(double currentAngleRads) {
+    return currentAngleRads < this.m_minAngle;
+  }
+
+  /**
+   * Returns whether the arm would hit the upper limit.
+   *
+   * @param currentAngleRads The current arm height.
+   * @return Whether the arm would hit the upper limit.
+   */
+  public boolean wouldHitUpperLimit(double currentAngleRads) {
+    return currentAngleRads > this.m_maxAngle;
+  }
+
+  /**
    * Returns whether the arm has hit the lower limit.
    *
-   * @param x The current arm state.
    * @return Whether the arm has hit the lower limit.
    */
-  public boolean hasHitLowerLimit(Matrix<N2, N1> x) {
-    return x.get(0, 0) < this.m_minAngle;
+  public boolean hasHitLowerLimit() {
+    return wouldHitLowerLimit(getAngleRads());
   }
 
   /**
    * Returns whether the arm has hit the upper limit.
    *
-   * @param x The current arm state.
    * @return Whether the arm has hit the upper limit.
    */
-  public boolean hasHitUpperLimit(Matrix<N2, N1> x) {
-    return x.get(0, 0) > this.m_maxAngle;
+  public boolean hasHitUpperLimit() {
+    return wouldHitUpperLimit(getAngleRads());
   }
 
   /**
@@ -161,7 +217,7 @@
    * @return The current arm angle.
    */
   public double getAngleRads() {
-    return m_x.get(0, 0);
+    return m_y.get(0, 0);
   }
 
   /**
@@ -187,7 +243,7 @@
   }
 
   /**
-   * Sets the input voltage for the elevator.
+   * Sets the input voltage for the arm.
    *
    * @param volts The input voltage.
    */
@@ -196,12 +252,10 @@
   }
 
   /**
-   * Calculates a rough estimate of the moment of inertia of an arm given its
-   * length and mass.
+   * Calculates a rough estimate of the moment of inertia of an arm given its length and mass.
    *
    * @param lengthMeters The length of the arm.
-   * @param massKg       The mass of the arm.
-   *
+   * @param massKg The mass of the arm.
    * @return The calculated moment of inertia.
    */
   public static double estimateMOI(double lengthMeters, double massKg) {
@@ -212,12 +266,12 @@
    * Updates the state of the arm.
    *
    * @param currentXhat The current state estimate.
-   * @param u           The system inputs (voltage).
-   * @param dtSeconds   The time difference between controller updates.
+   * @param u The system inputs (voltage).
+   * @param dtSeconds The time difference between controller updates.
    */
   @Override
-  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat,
-                                   Matrix<N1, N1> u, double dtSeconds) {
+  @SuppressWarnings({"ParameterName", "LambdaParameterName"})
+  protected Matrix<N2, N1> updateX(Matrix<N2, N1> currentXhat, Matrix<N1, N1> u, double dtSeconds) {
     // Horizontal case:
     // Torque = F * r = I * alpha
     // alpha = F * r / I
@@ -227,22 +281,33 @@
     // 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 *
     // cos(theta)]]
-    Matrix<N2, N1> updatedXhat = RungeKutta.rungeKutta(
-        (Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
-          Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
-          if (m_simulateGravity) {
-            xdot = xdot.plus(VecBuilder.fill(0,
-                m_armMass * m_r * -9.8 * 3.0 / (m_armMass * m_r * m_r) * Math.cos(x.get(0, 0))));
-          }
-          return xdot;
-        },
-        currentXhat, u, dtSeconds);
+    Matrix<N2, N1> updatedXhat =
+        NumericalIntegration.rkdp(
+            (Matrix<N2, N1> x, Matrix<N1, N1> u_) -> {
+              Matrix<N2, N1> xdot = m_plant.getA().times(x).plus(m_plant.getB().times(u_));
+              if (m_simulateGravity) {
+                xdot =
+                    xdot.plus(
+                        VecBuilder.fill(
+                            0,
+                            m_armMass
+                                * m_r
+                                * -9.8
+                                * 3.0
+                                / (m_armMass * m_r * m_r)
+                                * Math.cos(x.get(0, 0))));
+              }
+              return xdot;
+            },
+            currentXhat,
+            u,
+            dtSeconds);
 
     // We check for collision after updating xhat
-    if (hasHitLowerLimit(updatedXhat)) {
+    if (wouldHitLowerLimit(updatedXhat.get(0, 0))) {
       return VecBuilder.fill(m_minAngle, 0);
     }
-    if (hasHitUpperLimit(updatedXhat)) {
+    if (wouldHitUpperLimit(updatedXhat.get(0, 0))) {
       return VecBuilder.fill(m_maxAngle, 0);
     }
     return updatedXhat;
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java
new file mode 100644
index 0000000..3781281
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/UltrasonicSim.java
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import edu.wpi.first.hal.SimBoolean;
+import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Ultrasonic;
+
+public class UltrasonicSim {
+  private final SimBoolean m_simRangeValid;
+  private final SimDouble m_simRange;
+
+  /**
+   * Constructor.
+   *
+   * @param ultrasonic The real ultrasonic to simulate
+   */
+  public UltrasonicSim(Ultrasonic ultrasonic) {
+    SimDeviceSim simDevice = new SimDeviceSim("Ultrasonic", ultrasonic.getEchoChannel());
+    m_simRangeValid = simDevice.getBoolean("Range Valid");
+    m_simRange = simDevice.getDouble("Range (in)");
+  }
+
+  public void setRangeValid(boolean valid) {
+    m_simRangeValid.set(valid);
+  }
+
+  public void setRangeInches(double inches) {
+    m_simRange.set(inches);
+  }
+
+  public void setRangeMeters(double meters) {
+    m_simRange.set(Units.metersToInches(meters));
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
index 40a02bf..625ba81 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/XboxControllerSim.java
@@ -1,18 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 
-/**
- * Class to control a simulated Xbox 360 or Xbox One controller.
- */
+/** Class to control a simulated Xbox 360 or Xbox One controller. */
 public class XboxControllerSim extends GenericHIDSim {
   /**
    * Constructs from a XboxController object.
@@ -36,66 +30,146 @@
     setButtonCount(10);
   }
 
-  public void setX(GenericHID.Hand hand, double value) {
-    if (hand.equals(GenericHID.Hand.kLeft)) {
-      setRawAxis(XboxController.Axis.kLeftX.value, value);
-    } else {
-      setRawAxis(XboxController.Axis.kRightX.value, value);
-    }
+  /**
+   * Change the left X value of the joystick.
+   *
+   * @param value the new value
+   */
+  public void setLeftX(double value) {
+    setRawAxis(XboxController.Axis.kLeftX.value, value);
   }
 
-  public void setY(GenericHID.Hand hand, double value) {
-    if (hand.equals(GenericHID.Hand.kLeft)) {
-      setRawAxis(XboxController.Axis.kLeftY.value, value);
-    } else {
-      setRawAxis(XboxController.Axis.kRightY.value, value);
-    }
+  /**
+   * Change the right X value of the joystick.
+   *
+   * @param value the new value
+   */
+  public void setRightX(double value) {
+    setRawAxis(XboxController.Axis.kRightX.value, value);
   }
 
-  public void setTriggerAxis(GenericHID.Hand hand, double value) {
-    if (hand.equals(GenericHID.Hand.kLeft)) {
-      setRawAxis(XboxController.Axis.kLeftTrigger.value, value);
-    } else {
-      setRawAxis(XboxController.Axis.kRightTrigger.value, value);
-    }
+  /**
+   * Change the left Y value of the joystick.
+   *
+   * @param value the new value
+   */
+  public void setLeftY(double value) {
+    setRawAxis(XboxController.Axis.kLeftY.value, value);
   }
 
-  public void setBumper(GenericHID.Hand hand, boolean state) {
-    if (hand.equals(GenericHID.Hand.kLeft)) {
-      setRawButton(XboxController.Button.kBumperLeft.value, state);
-    } else {
-      setRawButton(XboxController.Button.kBumperRight.value, state);
-    }
+  /**
+   * Change the right Y value of the joystick.
+   *
+   * @param value the new value
+   */
+  public void setRightY(double value) {
+    setRawAxis(XboxController.Axis.kRightY.value, value);
   }
 
-  public void setStickButton(GenericHID.Hand hand, boolean state) {
-    if (hand.equals(GenericHID.Hand.kLeft)) {
-      setRawButton(XboxController.Button.kStickLeft.value, state);
-    } else {
-      setRawButton(XboxController.Button.kStickRight.value, state);
-    }
+  /**
+   * Change the value of the left trigger axis on the joystick.
+   *
+   * @param value the new value
+   */
+  public void setLeftTriggerAxis(double value) {
+    setRawAxis(XboxController.Axis.kLeftTrigger.value, value);
   }
 
+  /**
+   * Change the value of the right trigger axis on the joystick.
+   *
+   * @param value the new value
+   */
+  public void setRightTriggerAxis(double value) {
+    setRawAxis(XboxController.Axis.kRightTrigger.value, value);
+  }
+
+  /**
+   * Change the value of the left bumper on the joystick.
+   *
+   * @param state the new value
+   */
+  public void setLeftBumper(boolean state) {
+    setRawButton(XboxController.Button.kLeftBumper.value, state);
+  }
+
+  /**
+   * Change the value of the right bumper on the joystick.
+   *
+   * @param state the new value
+   */
+  public void setRightBumper(boolean state) {
+    setRawButton(XboxController.Button.kRightBumper.value, state);
+  }
+
+  /**
+   * Change the value of the left stick button on the joystick.
+   *
+   * @param state the new value
+   */
+  public void setLeftStickButton(boolean state) {
+    setRawButton(XboxController.Button.kLeftStick.value, state);
+  }
+
+  /**
+   * Change the value of the right stick button on the joystick.
+   *
+   * @param state the new value
+   */
+  public void setRightStickButton(boolean state) {
+    setRawButton(XboxController.Button.kRightStick.value, state);
+  }
+
+  /**
+   * Change the value of the A button.
+   *
+   * @param state the new value
+   */
   public void setAButton(boolean state) {
     setRawButton(XboxController.Button.kA.value, state);
   }
 
+  /**
+   * Change the value of the B button.
+   *
+   * @param state the new value
+   */
   public void setBButton(boolean state) {
     setRawButton(XboxController.Button.kB.value, state);
   }
 
+  /**
+   * Change the value of the X button.
+   *
+   * @param state the new value
+   */
   public void setXButton(boolean state) {
     setRawButton(XboxController.Button.kX.value, state);
   }
 
+  /**
+   * Change the value of the Y button.
+   *
+   * @param state the new value
+   */
   public void setYButton(boolean state) {
     setRawButton(XboxController.Button.kY.value, state);
   }
 
+  /**
+   * Change the value of the Back button.
+   *
+   * @param state the new value
+   */
   public void setBackButton(boolean state) {
     setRawButton(XboxController.Button.kBack.value, state);
   }
 
+  /**
+   * Change the value of the Start button.
+   *
+   * @param state the new value
+   */
   public void setStartButton(boolean state) {
     setRawButton(XboxController.Button.kStart.value, state);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
new file mode 100644
index 0000000..b76f056
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Field2d.java
@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * 2D representation of game field for dashboards.
+ *
+ * <p>An object's pose is the location shown on the dashboard view. Note that for the robot, this
+ * may or may not match the internal odometry. For example, 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).
+ *
+ * <p>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 wheel odometry), and set the new pose.
+ *
+ * <p>This class provides methods to set the robot pose, but other objects can also be shown by
+ * using the getObject() function. Other objects can also have multiple poses (which will show the
+ * object at multiple locations).
+ */
+public class Field2d implements NTSendable {
+  /** Constructor. */
+  public Field2d() {
+    FieldObject2d obj = new FieldObject2d("Robot");
+    obj.setPose(new Pose2d());
+    m_objects.add(obj);
+    SendableRegistry.add(this, "Field");
+  }
+
+  /**
+   * Set the robot pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public synchronized void setRobotPose(Pose2d pose) {
+    m_objects.get(0).setPose(pose);
+  }
+
+  /**
+   * Set the robot pose from x, y, and rotation.
+   *
+   * @param xMeters X location, in meters
+   * @param yMeters Y location, in meters
+   * @param rotation rotation
+   */
+  @SuppressWarnings("ParameterName")
+  public synchronized void setRobotPose(double xMeters, double yMeters, Rotation2d rotation) {
+    m_objects.get(0).setPose(xMeters, yMeters, rotation);
+  }
+
+  /**
+   * Get the robot pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getRobotPose() {
+    return m_objects.get(0).getPose();
+  }
+
+  /**
+   * Get or create a field object.
+   *
+   * @param name The field object's name.
+   * @return Field object
+   */
+  public synchronized FieldObject2d getObject(String name) {
+    for (FieldObject2d obj : m_objects) {
+      if (obj.m_name.equals(name)) {
+        return obj;
+      }
+    }
+    FieldObject2d obj = new FieldObject2d(name);
+    m_objects.add(obj);
+    if (m_table != null) {
+      synchronized (obj) {
+        obj.m_entry = m_table.getEntry(name);
+      }
+    }
+    return obj;
+  }
+
+  /**
+   * Get the robot object.
+   *
+   * @return Field object for robot
+   */
+  public synchronized FieldObject2d getRobotObject() {
+    return m_objects.get(0);
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("Field2d");
+
+    synchronized (this) {
+      m_table = builder.getTable();
+      for (FieldObject2d obj : m_objects) {
+        synchronized (obj) {
+          obj.m_entry = m_table.getEntry(obj.m_name);
+          obj.updateEntry(true);
+        }
+      }
+    }
+  }
+
+  private NetworkTable m_table;
+  private final List<FieldObject2d> m_objects = new ArrayList<>();
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
new file mode 100644
index 0000000..d6c7a5e
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/FieldObject2d.java
@@ -0,0 +1,198 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.ArrayList;
+import java.util.List;
+
+/** Game field object on a Field2d. */
+public class FieldObject2d {
+  /**
+   * Package-local constructor.
+   *
+   * @param name name
+   */
+  FieldObject2d(String name) {
+    m_name = name;
+  }
+
+  /**
+   * Set the pose from a Pose object.
+   *
+   * @param pose 2D pose
+   */
+  public synchronized void setPose(Pose2d pose) {
+    setPoses(pose);
+  }
+
+  /**
+   * Set the pose from x, y, and rotation.
+   *
+   * @param xMeters X location, in meters
+   * @param yMeters Y location, in meters
+   * @param rotation rotation
+   */
+  @SuppressWarnings("ParameterName")
+  public synchronized void setPose(double xMeters, double yMeters, Rotation2d rotation) {
+    setPose(new Pose2d(xMeters, yMeters, rotation));
+  }
+
+  /**
+   * Get the pose.
+   *
+   * @return 2D pose
+   */
+  public synchronized Pose2d getPose() {
+    updateFromEntry();
+    if (m_poses.isEmpty()) {
+      return new Pose2d();
+    }
+    return m_poses.get(0);
+  }
+
+  /**
+   * Set multiple poses from an list of Pose objects. The total number of poses is limited to 85.
+   *
+   * @param poses list of 2D poses
+   */
+  public synchronized void setPoses(List<Pose2d> poses) {
+    m_poses.clear();
+    for (Pose2d pose : poses) {
+      m_poses.add(pose);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Set multiple poses from an list of Pose objects. The total number of poses is limited to 85.
+   *
+   * @param poses list of 2D poses
+   */
+  public synchronized void setPoses(Pose2d... poses) {
+    m_poses.clear();
+    for (Pose2d pose : poses) {
+      m_poses.add(pose);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Sets poses from a trajectory.
+   *
+   * @param trajectory The trajectory from which the poses should be added.
+   */
+  public synchronized void setTrajectory(Trajectory trajectory) {
+    m_poses.clear();
+    for (Trajectory.State state : trajectory.getStates()) {
+      m_poses.add(state.poseMeters);
+    }
+    updateEntry();
+  }
+
+  /**
+   * Get multiple poses.
+   *
+   * @return list of 2D poses
+   */
+  public synchronized List<Pose2d> getPoses() {
+    updateFromEntry();
+    return new ArrayList<Pose2d>(m_poses);
+  }
+
+  void updateEntry() {
+    updateEntry(false);
+  }
+
+  synchronized void updateEntry(boolean setDefault) {
+    if (m_entry == null) {
+      return;
+    }
+
+    if (m_poses.size() < (255 / 3)) {
+      double[] arr = new double[m_poses.size() * 3];
+      int ndx = 0;
+      for (Pose2d pose : m_poses) {
+        Translation2d translation = pose.getTranslation();
+        arr[ndx + 0] = translation.getX();
+        arr[ndx + 1] = translation.getY();
+        arr[ndx + 2] = pose.getRotation().getDegrees();
+        ndx += 3;
+      }
+
+      if (setDefault) {
+        m_entry.setDefaultDoubleArray(arr);
+      } else {
+        m_entry.setDoubleArray(arr);
+      }
+    } else {
+      // send as raw array of doubles if too big for NT array
+      ByteBuffer output = ByteBuffer.allocate(m_poses.size() * 3 * 8);
+      output.order(ByteOrder.BIG_ENDIAN);
+
+      for (Pose2d pose : m_poses) {
+        Translation2d translation = pose.getTranslation();
+        output.putDouble(translation.getX());
+        output.putDouble(translation.getY());
+        output.putDouble(pose.getRotation().getDegrees());
+      }
+
+      if (setDefault) {
+        m_entry.setDefaultRaw(output.array());
+      } else {
+        m_entry.forceSetRaw(output.array());
+      }
+    }
+  }
+
+  private synchronized void updateFromEntry() {
+    if (m_entry == null) {
+      return;
+    }
+
+    double[] arr = m_entry.getDoubleArray((double[]) null);
+    if (arr != null) {
+      if ((arr.length % 3) != 0) {
+        return;
+      }
+
+      m_poses.clear();
+      for (int i = 0; i < arr.length; i += 3) {
+        m_poses.add(new Pose2d(arr[i], arr[i + 1], Rotation2d.fromDegrees(arr[i + 2])));
+      }
+    } else {
+      // read as raw array of doubles
+      byte[] data = m_entry.getRaw((byte[]) null);
+      if (data == null) {
+        return;
+      }
+
+      // must be triples of doubles
+      if ((data.length % (3 * 8)) != 0) {
+        return;
+      }
+      ByteBuffer input = ByteBuffer.wrap(data);
+      input.order(ByteOrder.BIG_ENDIAN);
+
+      m_poses.clear();
+      for (int i = 0; i < (data.length / (3 * 8)); i++) {
+        double x = input.getDouble();
+        double y = input.getDouble();
+        double rot = input.getDouble();
+        m_poses.add(new Pose2d(x, y, Rotation2d.fromDegrees(rot)));
+      }
+    }
+  }
+
+  String m_name;
+  NetworkTableEntry m_entry;
+  private final List<Pose2d> m_poses = new ArrayList<>();
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
index 274c7a8..19a3fc1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/ListenerExecutor.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
@@ -31,9 +28,7 @@
     }
   }
 
-  /**
-   * Runs all posted tasks.  Called periodically from main thread.
-   */
+  /** Runs all posted tasks. Called periodically from main thread. */
   public void runListenerTasks() {
     // Locally copy tasks from internal list; minimizes blocking time
     Collection<Runnable> tasks = new ArrayList<>();
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java
new file mode 100644
index 0000000..8967944
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/Mechanism2d.java
@@ -0,0 +1,113 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Map.Entry;
+
+/**
+ * Visual 2D representation of arms, elevators, and general mechanisms through a node-based API.
+ *
+ * <p>A Mechanism2d object is published and contains at least one root node. A root is the anchor
+ * point of other nodes (such as ligaments). Other nodes are recursively appended based on other
+ * nodes.
+ *
+ * @see MechanismObject2d
+ * @see MechanismLigament2d
+ * @see MechanismRoot2d
+ */
+public final class Mechanism2d implements NTSendable {
+  private static final String kBackgroundColor = "backgroundColor";
+  private NetworkTable m_table;
+  private final Map<String, MechanismRoot2d> m_roots;
+  private final double[] m_dims = new double[2];
+  private String m_color;
+
+  /**
+   * Create a new Mechanism2d with the given dimensions and default color (dark blue).
+   *
+   * <p>The dimensions represent the canvas that all the nodes are drawn on.
+   *
+   * @param width the width
+   * @param height the height
+   */
+  public Mechanism2d(double width, double height) {
+    this(width, height, new Color8Bit(0, 0, 32));
+  }
+
+  /**
+   * Create a new Mechanism2d with the given dimensions.
+   *
+   * <p>The dimensions represent the canvas that all the nodes are drawn on.
+   *
+   * @param width the width
+   * @param height the height
+   * @param backgroundColor the background color. Defaults to dark blue.
+   */
+  public Mechanism2d(double width, double height, Color8Bit backgroundColor) {
+    m_roots = new HashMap<>();
+    m_dims[0] = width;
+    m_dims[1] = height;
+    setBackgroundColor(backgroundColor);
+  }
+
+  /**
+   * Get or create a root in this Mechanism2d with the given name and position.
+   *
+   * <p>If a root with the given name already exists, the given x and y coordinates are not used.
+   *
+   * @param name the root name
+   * @param x the root x coordinate
+   * @param y the root y coordinate
+   * @return a new root joint object, or the existing one with the given name.
+   */
+  public synchronized MechanismRoot2d getRoot(String name, double x, double y) {
+    var existing = m_roots.get(name);
+    if (existing != null) {
+      return existing;
+    }
+
+    var root = new MechanismRoot2d(name, x, y);
+    m_roots.put(name, root);
+    if (m_table != null) {
+      root.update(m_table.getSubTable(name));
+    }
+    return root;
+  }
+
+  /**
+   * Set the Mechanism2d background color.
+   *
+   * @param color the new color
+   */
+  public synchronized void setBackgroundColor(Color8Bit color) {
+    this.m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
+    if (m_table != null) {
+      m_table.getEntry(kBackgroundColor).setString(m_color);
+    }
+  }
+
+  @Override
+  public void initSendable(NTSendableBuilder builder) {
+    builder.setSmartDashboardType("Mechanism2d");
+    synchronized (this) {
+      m_table = builder.getTable();
+      m_table.getEntry("dims").setDoubleArray(m_dims);
+      m_table.getEntry(kBackgroundColor).setString(m_color);
+      for (Entry<String, MechanismRoot2d> entry : m_roots.entrySet()) {
+        String name = entry.getKey();
+        MechanismRoot2d root = entry.getValue();
+        synchronized (root) {
+          root.update(m_table.getSubTable(name));
+        }
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
new file mode 100644
index 0000000..8579936
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismLigament2d.java
@@ -0,0 +1,155 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * Ligament node on a Mechanism2d. A ligament can have its length changed (like an elevator) or
+ * angle changed, like an arm.
+ *
+ * @see Mechanism2d
+ */
+public class MechanismLigament2d extends MechanismObject2d {
+  private double m_angle;
+  private NetworkTableEntry m_angleEntry;
+  private String m_color;
+  private NetworkTableEntry m_colorEntry;
+  private double m_length;
+  private NetworkTableEntry m_lengthEntry;
+  private double m_weight;
+  private NetworkTableEntry m_weightEntry;
+
+  /**
+   * Create a new ligament.
+   *
+   * @param name The ligament name.
+   * @param length The ligament length.
+   * @param angle The ligament angle.
+   * @param lineWidth The ligament's line width.
+   * @param color The ligament's color.
+   */
+  public MechanismLigament2d(
+      String name, double length, double angle, double lineWidth, Color8Bit color) {
+    super(name);
+    setColor(color);
+    setLength(length);
+    setAngle(angle);
+    setLineWeight(lineWidth);
+  }
+
+  /**
+   * Create a new ligament with the default color (orange) and thickness (6).
+   *
+   * @param name The ligament's name.
+   * @param length The ligament's length.
+   * @param angle The ligament's angle relative to its parent.
+   */
+  public MechanismLigament2d(String name, double length, double angle) {
+    this(name, length, angle, 10, new Color8Bit(235, 137, 52));
+  }
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param degrees the angle, in degrees
+   */
+  public synchronized void setAngle(double degrees) {
+    m_angle = degrees;
+    flush();
+  }
+
+  /**
+   * Set the ligament's angle relative to its parent.
+   *
+   * @param angle the angle
+   */
+  public synchronized void setAngle(Rotation2d angle) {
+    setAngle(angle.getDegrees());
+  }
+
+  /**
+   * Get the ligament's angle relative to its parent.
+   *
+   * @return the angle, in degrees
+   */
+  public synchronized double getAngle() {
+    if (m_angleEntry != null) {
+      m_angle = m_angleEntry.getDouble(0.0);
+    }
+    return m_angle;
+  }
+
+  /**
+   * Set the ligament's length.
+   *
+   * @param length the line length
+   */
+  public synchronized void setLength(double length) {
+    m_length = length;
+    flush();
+  }
+
+  /**
+   * Get the ligament length.
+   *
+   * @return the line length
+   */
+  public synchronized double getLength() {
+    if (m_lengthEntry != null) {
+      m_length = m_lengthEntry.getDouble(0.0);
+    }
+    return m_length;
+  }
+
+  /**
+   * Set the ligament color.
+   *
+   * @param color the color of the line
+   */
+  public synchronized void setColor(Color8Bit color) {
+    m_color = String.format("#%02X%02X%02X", color.red, color.green, color.blue);
+    flush();
+  }
+
+  /**
+   * Set the line thickness.
+   *
+   * @param weight the line thickness
+   */
+  public synchronized void setLineWeight(double weight) {
+    m_weight = weight;
+    flush();
+  }
+
+  @Override
+  protected synchronized void updateEntries(NetworkTable table) {
+    table.getEntry(".type").setString("line");
+    m_angleEntry = table.getEntry("angle");
+    m_lengthEntry = table.getEntry("length");
+    m_colorEntry = table.getEntry("color");
+    m_weightEntry = table.getEntry("weight");
+    flush();
+  }
+
+  /** Flush latest data to NT. */
+  private synchronized void flush() {
+    if (m_angleEntry != null) {
+      m_angleEntry.setDouble(m_angle);
+    }
+    if (m_lengthEntry != null) {
+      m_lengthEntry.setDouble(m_length);
+    }
+    if (m_colorEntry != null) {
+      m_colorEntry.setString(m_color);
+    }
+    if (m_weightEntry != null) {
+      m_weightEntry.setDouble(m_weight);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java
new file mode 100644
index 0000000..4598f7e
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismObject2d.java
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Common base class for all Mechanism2d node types.
+ *
+ * <p>To append another node, call {@link #append(MechanismObject2d)}. Objects that aren't appended
+ * to a published {@link Mechanism2d} container are nonfunctional.
+ *
+ * @see Mechanism2d
+ */
+public abstract class MechanismObject2d {
+  /** Relative to parent. */
+  private final String m_name;
+
+  private NetworkTable m_table;
+  private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
+
+  /**
+   * Create a new Mechanism node object.
+   *
+   * @param name the node's name, must be unique.
+   */
+  protected MechanismObject2d(String name) {
+    m_name = name;
+  }
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param <T> The object type.
+   * @param object the object to add.
+   * @return the object given as a parameter, useful for variable assignments and call chaining.
+   * @throws UnsupportedOperationException if the object's name is already used - object names must
+   *     be unique.
+   */
+  public final synchronized <T extends MechanismObject2d> T append(T object) {
+    if (m_objects.containsKey(object.getName())) {
+      throw new UnsupportedOperationException("Mechanism object names must be unique!");
+    }
+    m_objects.put(object.getName(), object);
+    if (m_table != null) {
+      object.update(m_table.getSubTable(object.getName()));
+    }
+    return object;
+  }
+
+  final void update(NetworkTable table) {
+    m_table = table;
+    updateEntries(m_table);
+    for (MechanismObject2d obj : m_objects.values()) {
+      obj.update(m_table.getSubTable(obj.m_name));
+    }
+  }
+
+  /**
+   * Update all entries with new ones from a new table.
+   *
+   * @param table the new table.
+   */
+  protected abstract void updateEntries(NetworkTable table);
+
+  public final String getName() {
+    return m_name;
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java
new file mode 100644
index 0000000..39f4515
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/MechanismRoot2d.java
@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.smartdashboard;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Root Mechanism2d node.
+ *
+ * <p>A root is the anchor point of other nodes (such as ligaments).
+ *
+ * <p>Do not create objects of this class directly! Obtain instances from the {@link
+ * Mechanism2d#getRoot(String, double, double)} factory method.
+ *
+ * <p>Append other nodes by using {@link #append(MechanismObject2d)}.
+ */
+public final class MechanismRoot2d {
+  private final String m_name;
+  private NetworkTable m_table;
+  private final Map<String, MechanismObject2d> m_objects = new HashMap<>(1);
+  private double m_x;
+  private NetworkTableEntry m_xEntry;
+  private double m_y;
+  private NetworkTableEntry m_yEntry;
+
+  /**
+   * Package-private constructor for roots.
+   *
+   * @param name name
+   * @param x x coordinate of root (provide only when constructing a root node)
+   * @param y y coordinate of root (provide only when constructing a root node)
+   */
+  MechanismRoot2d(String name, double x, double y) {
+    m_name = name;
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Append a Mechanism object that is based on this one.
+   *
+   * @param <T> The object type.
+   * @param object the object to add.
+   * @return the object given as a parameter, useful for variable assignments and call chaining.
+   * @throws UnsupportedOperationException if the object's name is already used - object names must
+   *     be unique.
+   */
+  public synchronized <T extends MechanismObject2d> T append(T object) {
+    if (m_objects.containsKey(object.getName())) {
+      throw new UnsupportedOperationException("Mechanism object names must be unique!");
+    }
+    m_objects.put(object.getName(), object);
+    if (m_table != null) {
+      object.update(m_table.getSubTable(object.getName()));
+    }
+    return object;
+  }
+
+  /**
+   * Set the root's position.
+   *
+   * @param x new x coordinate
+   * @param y new y coordinate
+   */
+  public synchronized void setPosition(double x, double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  synchronized void update(NetworkTable table) {
+    m_table = table;
+    m_xEntry = m_table.getEntry("x");
+    m_yEntry = m_table.getEntry("y");
+    flush();
+    for (MechanismObject2d obj : m_objects.values()) {
+      obj.update(m_table.getSubTable(obj.getName()));
+    }
+  }
+
+  public String getName() {
+    return m_name;
+  }
+
+  private synchronized void flush() {
+    if (m_xEntry != null) {
+      m_xEntry.setDouble(m_x);
+    }
+    if (m_yEntry != null) {
+      m_yEntry.setDouble(m_y);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java
deleted file mode 100644
index 35a8b54..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilder.java
+++ /dev/null
@@ -1,152 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.smartdashboard;
-
-import java.util.function.BooleanSupplier;
-import java.util.function.Consumer;
-import java.util.function.DoubleConsumer;
-import java.util.function.DoubleSupplier;
-import java.util.function.Supplier;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableValue;
-
-public interface SendableBuilder {
-  /**
-   * Set the string representation of the named data type that will be used
-   * by the smart dashboard for this sendable.
-   *
-   * @param type    data type
-   */
-  void setSmartDashboardType(String type);
-
-  /**
-   * Set a flag indicating if this sendable should be treated as an actuator.
-   * By default this flag is false.
-   *
-   * @param value   true if actuator, false if not
-   */
-  void setActuator(boolean value);
-
-  /**
-   * Set the function that should be called to set the Sendable into a safe
-   * state.  This is called when entering and exiting Live Window mode.
-   *
-   * @param func    function
-   */
-  void setSafeState(Runnable func);
-
-  /**
-   * Set the function that should be called to update the network table
-   * for things other than properties.  Note this function is not passed
-   * the network table object; instead it should use the entry handles
-   * returned by getEntry().
-   *
-   * @param func    function
-   */
-  void setUpdateTable(Runnable func);
-
-  /**
-   * Add a property without getters or setters.  This can be used to get
-   * entry handles for the function called by setUpdateTable().
-   *
-   * @param key   property name
-   * @return Network table entry
-   */
-  NetworkTableEntry getEntry(String key);
-
-  /**
-   * Represents an operation that accepts a single boolean-valued argument and
-   * returns no result. This is the primitive type specialization of Consumer
-   * for boolean. Unlike most other functional interfaces, BooleanConsumer is
-   * expected to operate via side-effects.
-   *
-   * <p>This is a functional interface whose functional method is accept(boolean).
-   */
-  @FunctionalInterface
-  interface BooleanConsumer {
-    /**
-     * Performs the operation on the given value.
-     * @param value the value
-     */
-    void accept(boolean value);
-  }
-
-  /**
-   * Add a boolean property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter);
-
-  /**
-   * Add a double property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter);
-
-  /**
-   * Add a string property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter);
-
-  /**
-   * Add a boolean array property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addBooleanArrayProperty(String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter);
-
-  /**
-   * Add a double array property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addDoubleArrayProperty(String key, Supplier<double[]> getter, Consumer<double[]> setter);
-
-  /**
-   * Add a string array property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addStringArrayProperty(String key, Supplier<String[]> getter, Consumer<String[]> setter);
-
-  /**
-   * Add a raw property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter);
-
-  /**
-   * Add a NetworkTableValue property.
-   *
-   * @param key     property name
-   * @param getter  getter function (returns current value)
-   * @param setter  setter function (sets new value)
-   */
-  void addValueProperty(String key, Supplier<NetworkTableValue> getter,
-                        Consumer<NetworkTableValue> setter);
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
index a82f26b..6c07fe3 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableBuilderImpl.java
@@ -1,12 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
+import edu.wpi.first.networktables.EntryListenerFlags;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableValue;
+import edu.wpi.first.util.function.BooleanConsumer;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.BooleanSupplier;
@@ -16,12 +19,7 @@
 import java.util.function.Function;
 import java.util.function.Supplier;
 
-import edu.wpi.first.networktables.EntryListenerFlags;
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableValue;
-
-public class SendableBuilderImpl implements SendableBuilder {
+public class SendableBuilderImpl implements NTSendableBuilder {
   private static class Property {
     Property(NetworkTable table, String key) {
       m_entry = table.getEntry(key);
@@ -60,7 +58,7 @@
   private boolean m_actuator;
 
   /**
-   * Set the network table.  Must be called prior to any Add* functions being called.
+   * Set the network table. Must be called prior to any Add* functions being called.
    *
    * @param table Network table
    */
@@ -74,15 +72,18 @@
    *
    * @return The network table
    */
+  @Override
   public NetworkTable getTable() {
     return m_table;
   }
 
   /**
    * Return whether this sendable has an associated table.
+   *
    * @return True if it has a table, false if not.
    */
-  public boolean hasTable() {
+  @Override
+  public boolean isPublished() {
     return m_table != null;
   }
 
@@ -95,10 +96,9 @@
     return m_actuator;
   }
 
-  /**
-   * Update the network table values by calling the getters for all properties.
-   */
-  public void updateTable() {
+  /** Update the network table values by calling the getters for all properties. */
+  @Override
+  public void update() {
     for (Property property : m_properties) {
       if (property.m_update != null) {
         property.m_update.accept(property.m_entry);
@@ -109,9 +109,7 @@
     }
   }
 
-  /**
-   * Hook setters for all properties.
-   */
+  /** Hook setters for all properties. */
   public void startListeners() {
     for (Property property : m_properties) {
       property.startListener();
@@ -121,9 +119,7 @@
     }
   }
 
-  /**
-   * Unhook setters for all properties.
-   */
+  /** Unhook setters for all properties. */
   public void stopListeners() {
     for (Property property : m_properties) {
       property.stopListener();
@@ -134,7 +130,7 @@
   }
 
   /**
-   * Start LiveWindow mode by hooking the setters for all properties.  Also calls the safeState
+   * Start LiveWindow mode by hooking the setters for all properties. Also calls the safeState
    * function if one was provided.
    */
   public void startLiveWindowMode() {
@@ -145,7 +141,7 @@
   }
 
   /**
-   * Stop LiveWindow mode by unhooking the setters for all properties.  Also calls the safeState
+   * Stop LiveWindow mode by unhooking the setters for all properties. Also calls the safeState
    * function if one was provided.
    */
   public void stopLiveWindowMode() {
@@ -155,9 +151,8 @@
     }
   }
 
-  /**
-   * Clear properties.
-   */
+  /** Clear properties. */
+  @Override
   public void clearProperties() {
     stopListeners();
     m_properties.clear();
@@ -187,7 +182,7 @@
   }
 
   /**
-   * Set the function that should be called to set the Sendable into a safe state.  This is called
+   * Set the function that should be called to set the Sendable into a safe state. This is called
    * when entering and exiting Live Window mode.
    *
    * @param func function
@@ -199,7 +194,7 @@
 
   /**
    * Set the function that should be called to update the network table for things other than
-   * properties.  Note this function is not passed the network table object; instead it should use
+   * properties. Note this function is not passed the network table object; instead it should use
    * the entry handles returned by getEntry().
    *
    * @param func function
@@ -210,7 +205,7 @@
   }
 
   /**
-   * Add a property without getters or setters.  This can be used to get entry handles for the
+   * Add a property without getters or setters. This can be used to get entry handles for the
    * function called by setUpdateTable().
    *
    * @param key property name
@@ -224,7 +219,7 @@
   /**
    * Add a boolean property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
@@ -235,11 +230,18 @@
       property.m_update = entry -> entry.setBoolean(getter.getAsBoolean());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isBoolean()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getBoolean()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isBoolean()) {
+                      SmartDashboard.postListenerTask(
+                          () -> setter.accept(event.value.getBoolean()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -247,7 +249,7 @@
   /**
    * Add a double property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
@@ -258,11 +260,17 @@
       property.m_update = entry -> entry.setDouble(getter.getAsDouble());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isDouble()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getDouble()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isDouble()) {
+                      SmartDashboard.postListenerTask(() -> setter.accept(event.value.getDouble()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -270,7 +278,7 @@
   /**
    * Add a string property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
@@ -281,11 +289,17 @@
       property.m_update = entry -> entry.setString(getter.get());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isString()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getString()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isString()) {
+                      SmartDashboard.postListenerTask(() -> setter.accept(event.value.getString()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -293,23 +307,30 @@
   /**
    * Add a boolean array property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
   @Override
-  public void addBooleanArrayProperty(String key, Supplier<boolean[]> getter,
-                                      Consumer<boolean[]> setter) {
+  public void addBooleanArrayProperty(
+      String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter) {
     Property property = new Property(m_table, key);
     if (getter != null) {
       property.m_update = entry -> entry.setBooleanArray(getter.get());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isBooleanArray()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getBooleanArray()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isBooleanArray()) {
+                      SmartDashboard.postListenerTask(
+                          () -> setter.accept(event.value.getBooleanArray()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -317,23 +338,30 @@
   /**
    * Add a double array property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
   @Override
-  public void addDoubleArrayProperty(String key, Supplier<double[]> getter,
-                                     Consumer<double[]> setter) {
+  public void addDoubleArrayProperty(
+      String key, Supplier<double[]> getter, Consumer<double[]> setter) {
     Property property = new Property(m_table, key);
     if (getter != null) {
       property.m_update = entry -> entry.setDoubleArray(getter.get());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isDoubleArray()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getDoubleArray()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isDoubleArray()) {
+                      SmartDashboard.postListenerTask(
+                          () -> setter.accept(event.value.getDoubleArray()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -341,23 +369,30 @@
   /**
    * Add a string array property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
   @Override
-  public void addStringArrayProperty(String key, Supplier<String[]> getter,
-                                     Consumer<String[]> setter) {
+  public void addStringArrayProperty(
+      String key, Supplier<String[]> getter, Consumer<String[]> setter) {
     Property property = new Property(m_table, key);
     if (getter != null) {
       property.m_update = entry -> entry.setStringArray(getter.get());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isStringArray()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getStringArray()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isStringArray()) {
+                      SmartDashboard.postListenerTask(
+                          () -> setter.accept(event.value.getStringArray()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -365,7 +400,7 @@
   /**
    * Add a raw property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
@@ -376,11 +411,17 @@
       property.m_update = entry -> entry.setRaw(getter.get());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        if (event.value.isRaw()) {
-          SmartDashboard.postListenerTask(() -> setter.accept(event.value.getRaw()));
-        }
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    if (event.value.isRaw()) {
+                      SmartDashboard.postListenerTask(() -> setter.accept(event.value.getRaw()));
+                    }
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
@@ -388,21 +429,27 @@
   /**
    * Add a NetworkTableValue property.
    *
-   * @param key    property name
+   * @param key property name
    * @param getter getter function (returns current value)
    * @param setter setter function (sets new value)
    */
   @Override
-  public void addValueProperty(String key, Supplier<NetworkTableValue> getter,
-                               Consumer<NetworkTableValue> setter) {
+  public void addValueProperty(
+      String key, Supplier<NetworkTableValue> getter, Consumer<NetworkTableValue> setter) {
     Property property = new Property(m_table, key);
     if (getter != null) {
       property.m_update = entry -> entry.setValue(getter.get());
     }
     if (setter != null) {
-      property.m_createListener = entry -> entry.addListener(event -> {
-        SmartDashboard.postListenerTask(() -> setter.accept(event.value));
-      }, EntryListenerFlags.kImmediate | EntryListenerFlags.kNew | EntryListenerFlags.kUpdate);
+      property.m_createListener =
+          entry ->
+              entry.addListener(
+                  event -> {
+                    SmartDashboard.postListenerTask(() -> setter.accept(event.value));
+                  },
+                  EntryListenerFlags.kImmediate
+                      | EntryListenerFlags.kNew
+                      | EntryListenerFlags.kUpdate);
     }
     m_properties.add(property);
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
index cdf1ffb..2614edf 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableChooser.java
@@ -1,12 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.networktables.NTSendable;
+import edu.wpi.first.networktables.NTSendableBuilder;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.util.sendable.SendableRegistry;
 import java.util.ArrayList;
 import java.util.LinkedHashMap;
 import java.util.List;
@@ -14,11 +17,6 @@
 import java.util.concurrent.atomic.AtomicInteger;
 import java.util.concurrent.locks.ReentrantLock;
 
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.wpilibj.Sendable;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-
 /**
  * The {@link SendableChooser} class is a useful tool for presenting a selection of options to the
  * {@link SmartDashboard}.
@@ -31,38 +29,25 @@
  *
  * @param <V> The type of the values to be stored
  */
-public class SendableChooser<V> implements Sendable, AutoCloseable {
-  /**
-   * The key for the default value.
-   */
+public class SendableChooser<V> implements NTSendable, AutoCloseable {
+  /** The key for the default value. */
   private static final String DEFAULT = "default";
-  /**
-   * The key for the selected option.
-   */
+  /** The key for the selected option. */
   private static final String SELECTED = "selected";
-  /**
-   * The key for the active option.
-   */
+  /** The key for the active option. */
   private static final String ACTIVE = "active";
-  /**
-   * The key for the option array.
-   */
+  /** The key for the option array. */
   private static final String OPTIONS = "options";
-  /**
-   * The key for the instance number.
-   */
+  /** The key for the instance number. */
   private static final String INSTANCE = ".instance";
-  /**
-   * A map linking strings to the objects the represent.
-   */
+  /** A map linking strings to the objects the represent. */
   private final Map<String, V> m_map = new LinkedHashMap<>();
+
   private String m_defaultChoice = "";
   private final int m_instance;
   private static final AtomicInteger s_instances = new AtomicInteger();
 
-  /**
-   * Instantiates a {@link SendableChooser}.
-   */
+  /** Instantiates a {@link SendableChooser}. */
   public SendableChooser() {
     m_instance = s_instances.getAndIncrement();
     SendableRegistry.add(this, "SendableChooser", m_instance);
@@ -77,7 +62,7 @@
    * Adds the given object to the list of options. On the {@link SmartDashboard} on the desktop, the
    * object will appear as the given name.
    *
-   * @param name   the name of the option
+   * @param name the name of the option
    * @param object the option
    */
   public void addOption(String name, V object) {
@@ -88,8 +73,7 @@
    * Adds the given object to the list of options.
    *
    * @deprecated Use {@link #addOption(String, Object)} instead.
-   *
-   * @param name   the name of the option
+   * @param name the name of the option
    * @param object the option
    */
   @Deprecated
@@ -102,7 +86,7 @@
    * very close to {@link #addOption(String, Object)} except that it will use this as the default
    * option if none other is explicitly selected.
    *
-   * @param name   the name of the option
+   * @param name the name of the option
    * @param object the option
    */
   public void setDefaultOption(String name, V object) {
@@ -116,8 +100,7 @@
    * Adds the given object to the list of options and marks it as the default.
    *
    * @deprecated Use {@link #setDefaultOption(String, Object)} instead.
-   *
-   * @param name   the name of the option
+   * @param name the name of the option
    * @param object the option
    */
   @Deprecated
@@ -149,39 +132,45 @@
   private final ReentrantLock m_mutex = new ReentrantLock();
 
   @Override
-  public void initSendable(SendableBuilder builder) {
+  public void initSendable(NTSendableBuilder builder) {
     builder.setSmartDashboardType("String Chooser");
     builder.getEntry(INSTANCE).setDouble(m_instance);
     builder.addStringProperty(DEFAULT, () -> m_defaultChoice, null);
     builder.addStringArrayProperty(OPTIONS, () -> m_map.keySet().toArray(new String[0]), null);
-    builder.addStringProperty(ACTIVE, () -> {
-      m_mutex.lock();
-      try {
-        if (m_selected != null) {
-          return m_selected;
-        } else {
-          return m_defaultChoice;
-        }
-      } finally {
-        m_mutex.unlock();
-      }
-    }, null);
+    builder.addStringProperty(
+        ACTIVE,
+        () -> {
+          m_mutex.lock();
+          try {
+            if (m_selected != null) {
+              return m_selected;
+            } else {
+              return m_defaultChoice;
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        },
+        null);
     m_mutex.lock();
     try {
       m_activeEntries.add(builder.getEntry(ACTIVE));
     } finally {
       m_mutex.unlock();
     }
-    builder.addStringProperty(SELECTED, null, val -> {
-      m_mutex.lock();
-      try {
-        m_selected = val;
-        for (NetworkTableEntry entry : m_activeEntries) {
-          entry.setString(val);
-        }
-      } finally {
-        m_mutex.unlock();
-      }
-    });
+    builder.addStringProperty(
+        SELECTED,
+        null,
+        val -> {
+          m_mutex.lock();
+          try {
+            m_selected = val;
+            for (NetworkTableEntry entry : m_activeEntries) {
+              entry.setString(val);
+            }
+          } finally {
+            m_mutex.unlock();
+          }
+        });
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
deleted file mode 100644
index 3cafbf7..0000000
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SendableRegistry.java
+++ /dev/null
@@ -1,520 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.smartdashboard;
-
-import java.lang.ref.WeakReference;
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-import java.util.Map;
-import java.util.WeakHashMap;
-import java.util.function.Consumer;
-
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.Sendable;
-
-
-/**
- * The SendableRegistry class is the public interface for registering sensors
- * and actuators for use on dashboards and LiveWindow.
- */
-public class SendableRegistry {
-  private static class Component {
-    Component() {}
-
-    Component(Sendable sendable) {
-      m_sendable = new WeakReference<>(sendable);
-    }
-
-    WeakReference<Sendable> m_sendable;
-    SendableBuilderImpl m_builder = new SendableBuilderImpl();
-    String m_name;
-    String m_subsystem = "Ungrouped";
-    WeakReference<Sendable> m_parent;
-    boolean m_liveWindow;
-    Object[] m_data;
-
-    void setName(String moduleType, int channel) {
-      m_name = moduleType + "[" + channel + "]";
-    }
-
-    void setName(String moduleType, int moduleNumber, int channel) {
-      m_name = moduleType + "[" + moduleNumber + "," + channel + "]";
-    }
-  }
-
-  private static final Map<Object, Component> components = new WeakHashMap<>();
-  private static int nextDataHandle;
-
-  private static Component getOrAdd(Sendable sendable) {
-    Component comp = components.get(sendable);
-    if (comp == null) {
-      comp = new Component(sendable);
-      components.put(sendable, comp);
-    } else {
-      if (comp.m_sendable == null) {
-        comp.m_sendable = new WeakReference<>(sendable);
-      }
-    }
-    return comp;
-  }
-
-  private SendableRegistry() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable object to add
-   * @param name component name
-   */
-  public static synchronized void add(Sendable sendable, String name) {
-    Component comp = getOrAdd(sendable);
-    comp.m_name = name;
-  }
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable     object to add
-   * @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
-   */
-  public static synchronized void add(Sendable sendable, String moduleType, int channel) {
-    Component comp = getOrAdd(sendable);
-    comp.setName(moduleType, channel);
-  }
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable     object to add
-   * @param moduleType   A string that defines the module name in the label for
-   *                     the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into
-   */
-  public static synchronized void add(Sendable sendable, String moduleType, int moduleNumber,
-      int channel) {
-    Component comp = getOrAdd(sendable);
-    comp.setName(moduleType, moduleNumber, channel);
-  }
-
-  /**
-   * Adds an object to the registry.
-   *
-   * @param sendable object to add
-   * @param subsystem subsystem name
-   * @param name component name
-   */
-  public static synchronized void add(Sendable sendable, String subsystem, String name) {
-    Component comp = getOrAdd(sendable);
-    comp.m_name = name;
-    comp.m_subsystem = subsystem;
-  }
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable object to add
-   * @param name component name
-   */
-  public static synchronized void addLW(Sendable sendable, String name) {
-    Component comp = getOrAdd(sendable);
-    comp.m_liveWindow = true;
-    comp.m_name = name;
-  }
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable     object to add
-   * @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
-   */
-  public static synchronized void addLW(Sendable sendable, String moduleType, int channel) {
-    Component comp = getOrAdd(sendable);
-    comp.m_liveWindow = true;
-    comp.setName(moduleType, channel);
-  }
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable     object to add
-   * @param moduleType   A string that defines the module name in the label for
-   *                     the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into
-   */
-  public static synchronized void addLW(Sendable sendable, String moduleType, int moduleNumber,
-      int channel) {
-    Component comp = getOrAdd(sendable);
-    comp.m_liveWindow = true;
-    comp.setName(moduleType, moduleNumber, channel);
-  }
-
-  /**
-   * Adds an object to the registry and LiveWindow.
-   *
-   * @param sendable object to add
-   * @param subsystem subsystem name
-   * @param name component name
-   */
-  public static synchronized void addLW(Sendable sendable, String subsystem, String name) {
-    Component comp = getOrAdd(sendable);
-    comp.m_liveWindow = true;
-    comp.m_name = name;
-    comp.m_subsystem = subsystem;
-  }
-
-  /**
-   * Adds a child object to an object.  Adds the child object to the registry
-   * if it's not already present.
-   *
-   * @param parent parent object
-   * @param child child object
-   */
-  public static synchronized void addChild(Sendable parent, Object child) {
-    Component comp = components.get(child);
-    if (comp == null) {
-      comp = new Component();
-      components.put(child, comp);
-    }
-    comp.m_parent = new WeakReference<>(parent);
-  }
-
-  /**
-   * Removes an object from the registry.
-   *
-   * @param sendable object to remove
-   * @return true if the object was removed; false if it was not present
-   */
-  public static synchronized boolean remove(Sendable sendable) {
-    return components.remove(sendable) != null;
-  }
-
-  /**
-   * Determines if an object is in the registry.
-   *
-   * @param sendable object to check
-   * @return True if in registry, false if not.
-   */
-  public static synchronized boolean contains(Sendable sendable) {
-    return components.containsKey(sendable);
-  }
-
-  /**
-   * Gets the name of an object.
-   *
-   * @param sendable object
-   * @return Name (empty if object is not in registry)
-   */
-  public static synchronized String getName(Sendable sendable) {
-    Component comp = components.get(sendable);
-    if (comp == null) {
-      return "";
-    }
-    return comp.m_name;
-  }
-
-  /**
-   * Sets the name of an object.
-   *
-   * @param sendable object
-   * @param name name
-   */
-  public static synchronized void setName(Sendable sendable, String name) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.m_name = name;
-    }
-  }
-
-  /**
-   * Sets the name of an object with a channel number.
-   *
-   * @param sendable   object
-   * @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
-   */
-  public static synchronized void setName(Sendable sendable, String moduleType, int channel) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.setName(moduleType, channel);
-    }
-  }
-
-  /**
-   * Sets the name of an object with a module and channel number.
-   *
-   * @param sendable     object
-   * @param moduleType   A string that defines the module name in the label for
-   *                     the value
-   * @param moduleNumber The number of the particular module type
-   * @param channel      The channel number the device is plugged into
-   */
-  public static synchronized void setName(Sendable sendable, String moduleType, int moduleNumber,
-      int channel) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.setName(moduleType, moduleNumber, channel);
-    }
-  }
-
-  /**
-   * Sets both the subsystem name and device name of an object.
-   *
-   * @param sendable object
-   * @param subsystem subsystem name
-   * @param name device name
-   */
-  public static synchronized void setName(Sendable sendable, String subsystem, String name) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.m_name = name;
-      comp.m_subsystem = subsystem;
-    }
-  }
-
-  /**
-   * Gets the subsystem name of an object.
-   *
-   * @param sendable object
-   * @return Subsystem name (empty if object is not in registry)
-   */
-  public static synchronized String getSubsystem(Sendable sendable) {
-    Component comp = components.get(sendable);
-    if (comp == null) {
-      return "";
-    }
-    return comp.m_subsystem;
-  }
-
-  /**
-   * Sets the subsystem name of an object.
-   *
-   * @param sendable object
-   * @param subsystem subsystem name
-   */
-  public static synchronized void setSubsystem(Sendable sendable, String subsystem) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.m_subsystem = subsystem;
-    }
-  }
-
-  /**
-   * Gets a unique handle for setting/getting data with setData() and getData().
-   *
-   * @return Handle
-   */
-  public static synchronized int getDataHandle() {
-    return nextDataHandle++;
-  }
-
-  /**
-   * Associates arbitrary data with an object in the registry.
-   *
-   * @param sendable object
-   * @param handle data handle returned by getDataHandle()
-   * @param data data to set
-   * @return Previous data (may be null)
-   */
-  public static synchronized Object setData(Sendable sendable, int handle, Object data) {
-    Component comp = components.get(sendable);
-    if (comp == null) {
-      return null;
-    }
-    Object rv = null;
-    if (comp.m_data == null) {
-      comp.m_data = new Object[handle + 1];
-    } else if (handle < comp.m_data.length) {
-      rv = comp.m_data[handle];
-    } else {
-      comp.m_data = Arrays.copyOf(comp.m_data, handle + 1);
-    }
-    comp.m_data[handle] = data;
-    return rv;
-  }
-
-  /**
-   * Gets arbitrary data associated with an object in the registry.
-   *
-   * @param sendable object
-   * @param handle data handle returned by getDataHandle()
-   * @return data (may be null if none associated)
-   */
-  public static synchronized Object getData(Sendable sendable, int handle) {
-    Component comp = components.get(sendable);
-    if (comp == null || comp.m_data == null || handle >= comp.m_data.length) {
-      return null;
-    }
-    return comp.m_data[handle];
-  }
-
-  /**
-   * Enables LiveWindow for an object.
-   *
-   * @param sendable object
-   */
-  public static synchronized void enableLiveWindow(Sendable sendable) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.m_liveWindow = true;
-    }
-  }
-
-  /**
-   * Disables LiveWindow for an object.
-   *
-   * @param sendable object
-   */
-  public static synchronized void disableLiveWindow(Sendable sendable) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.m_liveWindow = false;
-    }
-  }
-
-  /**
-   * Publishes an object in the registry to a network table.
-   *
-   * @param sendable object
-   * @param table network table
-   */
-  public static synchronized void publish(Sendable sendable, NetworkTable table) {
-    Component comp = getOrAdd(sendable);
-    comp.m_builder.clearProperties();
-    comp.m_builder.setTable(table);
-    sendable.initSendable(comp.m_builder);
-    comp.m_builder.updateTable();
-    comp.m_builder.startListeners();
-  }
-
-  /**
-   * Updates network table information from an object.
-   *
-   * @param sendable object
-   */
-  public static synchronized void update(Sendable sendable) {
-    Component comp = components.get(sendable);
-    if (comp != null) {
-      comp.m_builder.updateTable();
-    }
-  }
-
-  /**
-   * Data passed to foreachLiveWindow() callback function.
-   */
-  public static class CallbackData {
-    /**
-     * Sendable object.
-     */
-    @SuppressWarnings("MemberName")
-    public Sendable sendable;
-
-    /**
-     * Name.
-     */
-    @SuppressWarnings("MemberName")
-    public String name;
-
-    /**
-     * Subsystem.
-     */
-    @SuppressWarnings("MemberName")
-    public String subsystem;
-
-    /**
-     * Parent sendable object.
-     */
-    @SuppressWarnings("MemberName")
-    public Sendable parent;
-
-    /**
-     * Data stored in object with setData().  Update this to change the data.
-     */
-    @SuppressWarnings("MemberName")
-    public Object data;
-
-    /**
-     * Sendable builder for the sendable.
-     */
-    @SuppressWarnings("MemberName")
-    public SendableBuilderImpl builder;
-  }
-
-  // As foreachLiveWindow is single threaded, cache the components it
-  // iterates over to avoid risk of ConcurrentModificationException
-  private static List<Component> foreachComponents = new ArrayList<>();
-
-  /**
-   * Iterates over LiveWindow-enabled objects in the registry.
-   * It is *not* safe to call other SendableRegistry functions from the
-   * callback.
-   *
-   * @param dataHandle data handle to get data object passed to callback
-   * @param callback function to call for each object
-   */
-  @SuppressWarnings({"PMD.CyclomaticComplexity", "PMD.AvoidInstantiatingObjectsInLoops",
-                     "PMD.AvoidCatchingThrowable"})
-  public static synchronized void foreachLiveWindow(int dataHandle,
-      Consumer<CallbackData> callback) {
-    CallbackData cbdata = new CallbackData();
-    foreachComponents.clear();
-    foreachComponents.addAll(components.values());
-    for (Component comp : foreachComponents) {
-      if (comp.m_sendable == null) {
-        continue;
-      }
-      cbdata.sendable = comp.m_sendable.get();
-      if (cbdata.sendable != null && comp.m_liveWindow) {
-        cbdata.name = comp.m_name;
-        cbdata.subsystem = comp.m_subsystem;
-        if (comp.m_parent != null) {
-          cbdata.parent = comp.m_parent.get();
-        } else {
-          cbdata.parent = null;
-        }
-        if (comp.m_data != null && dataHandle < comp.m_data.length) {
-          cbdata.data = comp.m_data[dataHandle];
-        } else {
-          cbdata.data = null;
-        }
-        cbdata.builder = comp.m_builder;
-        try {
-          callback.accept(cbdata);
-        } catch (Throwable throwable) {
-          Throwable cause = throwable.getCause();
-          if (cause != null) {
-            throwable = cause;
-          }
-          DriverStation.reportError(
-              "Unhandled exception calling LiveWindow for " + comp.m_name + ": "
-                  + throwable.toString(), false);
-          comp.m_liveWindow = false;
-        }
-        if (cbdata.data != null) {
-          if (comp.m_data == null) {
-            comp.m_data = new Object[dataHandle + 1];
-          } else if (dataHandle >= comp.m_data.length) {
-            comp.m_data = Arrays.copyOf(comp.m_data, dataHandle + 1);
-          }
-          comp.m_data[dataHandle] = cbdata.data;
-        }
-      }
-    }
-    foreachComponents.clear();
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
index 1b022c1..be5684b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboard.java
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
-import java.nio.ByteBuffer;
-import java.util.HashMap;
-import java.util.Map;
-import java.util.Set;
-
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.Sendable;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import java.nio.ByteBuffer;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.Set;
 
 /**
  * The {@link SmartDashboard} class is the bridge between robot programs and the SmartDashboard on
@@ -26,24 +23,18 @@
  * <p>When a value is put into the SmartDashboard here, it pops up on the SmartDashboard on the
  * laptop. Users can put values into and get values from the SmartDashboard.
  */
-@SuppressWarnings("PMD.GodClass")
 public final class SmartDashboard {
-  /**
-   * The {@link NetworkTable} used by {@link SmartDashboard}.
-   */
+  /** The {@link NetworkTable} used by {@link SmartDashboard}. */
   private static final NetworkTable table =
       NetworkTableInstance.getDefault().getTable("SmartDashboard");
 
   /**
-   * A table linking tables in the SmartDashboard to the {@link Sendable} objects they
-   * came from.
+   * A table linking tables in the SmartDashboard to the {@link Sendable} objects they came from.
    */
   @SuppressWarnings("PMD.UseConcurrentHashMap")
   private static final Map<String, Sendable> tablesToData = new HashMap<>();
 
-  /**
-   * The executor for listener tasks; calls listener tasks synchronously from main thread.
-   */
+  /** The executor for listener tasks; calls listener tasks synchronously from main thread. */
   private static final ListenerExecutor listenerExecutor = new ListenerExecutor();
 
   static {
@@ -58,7 +49,7 @@
    * Maps the specified key to the specified value in this table. The key can not be null. The value
    * can be retrieved by calling the get method with a key that is equal to the original key.
    *
-   * @param key  the key
+   * @param key the key
    * @param data the value
    * @throws IllegalArgumentException If key is null
    */
@@ -68,15 +59,18 @@
     if (sddata == null || sddata != data) {
       tablesToData.put(key, data);
       NetworkTable dataTable = table.getSubTable(key);
-      SendableRegistry.publish(data, dataTable);
+      SendableBuilderImpl builder = new SendableBuilderImpl();
+      builder.setTable(dataTable);
+      SendableRegistry.publish(data, builder);
+      builder.startListeners();
       dataTable.getEntry(".name").setString(key);
     }
   }
 
   /**
-   * Maps the specified key (where the key is the name of the {@link Sendable}
-   * to the specified value in this table. The value can be retrieved by
-   * calling the get method with a key that is equal to the original key.
+   * Maps the specified key (where the key is the name of the {@link Sendable} to the specified
+   * value in this table. The value can be retrieved by calling the get method with a key that is
+   * equal to the original key.
    *
    * @param value the value
    * @throws IllegalArgumentException If key is null
@@ -93,7 +87,7 @@
    *
    * @param key the key
    * @return the value
-   * @throws IllegalArgumentException  if the key is null
+   * @throws IllegalArgumentException if the key is null
    */
   public static synchronized Sendable getData(String key) {
     Sendable data = tablesToData.get(key);
@@ -106,6 +100,7 @@
 
   /**
    * Gets the entry for the specified key.
+   *
    * @param key the key name
    * @return Network table entry.
    */
@@ -143,8 +138,7 @@
   }
 
   /**
-   * Makes a key's value persistent through program restarts.
-   * The key cannot be null.
+   * Makes a key's value persistent through program restarts. The key cannot be null.
    *
    * @param key the key name
    */
@@ -153,8 +147,7 @@
   }
 
   /**
-   * Stop making a key's value persistent through program restarts.
-   * The key cannot be null.
+   * Stop making a key's value persistent through program restarts. The key cannot be null.
    *
    * @param key the key name
    */
@@ -163,8 +156,7 @@
   }
 
   /**
-   * Returns whether the value is persistent through program restarts.
-   * The key cannot be null.
+   * Returns whether the value is persistent through program restarts. The key cannot be null.
    *
    * @param key the key name
    * @return True if the value is persistent.
@@ -174,8 +166,7 @@
   }
 
   /**
-   * Sets flags on the specified key in this table. The key can
-   * not be null.
+   * Sets flags on the specified key in this table. The key can not be null.
    *
    * @param key the key name
    * @param flags the flags to set (bitmask)
@@ -185,8 +176,7 @@
   }
 
   /**
-   * Clears flags on the specified key in this table. The key can
-   * not be null.
+   * Clears flags on the specified key in this table. The key can not be null.
    *
    * @param key the key name
    * @param flags the flags to clear (bitmask)
@@ -206,8 +196,7 @@
   }
 
   /**
-   * Deletes the specified key in this table. The key can
-   * not be null.
+   * Deletes the specified key in this table. The key can not be null.
    *
    * @param key the key name
    */
@@ -217,6 +206,7 @@
 
   /**
    * Put a boolean in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -227,6 +217,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -236,12 +227,13 @@
   }
 
   /**
-   * Returns the boolean the key maps to. If the key does not exist or is of
-   *     different type, it will return the default value.
+   * Returns the boolean the key maps to. If the key does not exist or is of different type, it will
+   * return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static boolean getBoolean(String key, boolean defaultValue) {
     return getEntry(key).getBoolean(defaultValue);
@@ -249,6 +241,7 @@
 
   /**
    * Put a number in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -259,6 +252,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -268,12 +262,13 @@
   }
 
   /**
-   * Returns the number the key maps to. If the key does not exist or is of
-   *     different type, it will return the default value.
+   * Returns the number the key maps to. If the key does not exist or is of different type, it will
+   * return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static double getNumber(String key, double defaultValue) {
     return getEntry(key).getDouble(defaultValue);
@@ -281,6 +276,7 @@
 
   /**
    * Put a string in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -291,6 +287,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -300,12 +297,13 @@
   }
 
   /**
-   * Returns the string the key maps to. If the key does not exist or is of
-   *     different type, it will return the default value.
+   * Returns the string the key maps to. If the key does not exist or is of different type, it will
+   * return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static String getString(String key, String defaultValue) {
     return getEntry(key).getString(defaultValue);
@@ -313,6 +311,7 @@
 
   /**
    * Put a boolean array in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -323,6 +322,7 @@
 
   /**
    * Put a boolean array in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -333,6 +333,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -343,6 +344,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -352,24 +354,26 @@
   }
 
   /**
-   * Returns the boolean array the key maps to. If the key does not exist or is
-   *     of different type, it will return the default value.
+   * Returns the boolean array the key maps to. If the key does not exist or is of different type,
+   * it will return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static boolean[] getBooleanArray(String key, boolean[] defaultValue) {
     return getEntry(key).getBooleanArray(defaultValue);
   }
 
   /**
-   * Returns the boolean array the key maps to. If the key does not exist or is
-   *     of different type, it will return the default value.
+   * Returns the boolean array the key maps to. If the key does not exist or is of different type,
+   * it will return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static Boolean[] getBooleanArray(String key, Boolean[] defaultValue) {
     return getEntry(key).getBooleanArray(defaultValue);
@@ -377,6 +381,7 @@
 
   /**
    * Put a number array in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -387,6 +392,7 @@
 
   /**
    * Put a number array in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -397,6 +403,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -407,6 +414,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -416,24 +424,26 @@
   }
 
   /**
-   * Returns the number array the key maps to. If the key does not exist or is
-   *     of different type, it will return the default value.
+   * Returns the number array the key maps to. If the key does not exist or is of different type, it
+   * will return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static double[] getNumberArray(String key, double[] defaultValue) {
     return getEntry(key).getDoubleArray(defaultValue);
   }
 
   /**
-   * Returns the number array the key maps to. If the key does not exist or is
-   *     of different type, it will return the default value.
+   * Returns the number array the key maps to. If the key does not exist or is of different type, it
+   * will return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static Double[] getNumberArray(String key, Double[] defaultValue) {
     return getEntry(key).getDoubleArray(defaultValue);
@@ -441,6 +451,7 @@
 
   /**
    * Put a string array in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -451,6 +462,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -460,12 +472,13 @@
   }
 
   /**
-   * Returns the string array the key maps to. If the key does not exist or is
-   *     of different type, it will return the default value.
+   * Returns the string array the key maps to. If the key does not exist or is of different type, it
+   * will return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static String[] getStringArray(String key, String[] defaultValue) {
     return getEntry(key).getStringArray(defaultValue);
@@ -473,6 +486,7 @@
 
   /**
    * Put a raw value (byte array) in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @return False if the table key already exists with a different type
@@ -483,6 +497,7 @@
 
   /**
    * Put a raw value (bytes from a byte buffer) in the table.
+   *
    * @param key the key to be assigned to
    * @param value the value that will be assigned
    * @param len the length of the value
@@ -494,6 +509,7 @@
 
   /**
    * Gets the current value in the table, setting it if it does not exist.
+   *
    * @param key the key
    * @param defaultValue the default value to set if key does not exist.
    * @return False if the table key exists with a different type
@@ -503,20 +519,21 @@
   }
 
   /**
-   * Returns the raw value (byte array) the key maps to. If the key does not
-   *     exist or is of different type, it will return the default value.
+   * Returns the raw value (byte array) the key maps to. If the key does not exist or is of
+   * different type, it will return the default value.
+   *
    * @param key the key to look up
    * @param defaultValue the value to be returned if no value is found
-   * @return the value associated with the given key or the given default value
-   *     if there is no value associated with the key
+   * @return the value associated with the given key or the given default value if there is no value
+   *     associated with the key
    */
   public static byte[] getRaw(String key, byte[] defaultValue) {
     return getEntry(key).getRaw(defaultValue);
   }
 
   /**
-   * Posts a task from a listener to the ListenerExecutor, so that it can be run synchronously
-   * from the main loop on the next call to {@link SmartDashboard#updateValues()}.
+   * Posts a task from a listener to the ListenerExecutor, so that it can be run synchronously from
+   * the main loop on the next call to {@link SmartDashboard#updateValues()}.
    *
    * @param task The task to run synchronously from the main thread.
    */
@@ -524,14 +541,12 @@
     listenerExecutor.execute(task);
   }
 
-  /**
-   * Puts all sendable data to the dashboard.
-   */
+  /** Puts all sendable data to the dashboard. */
   public static synchronized void updateValues() {
+    // Execute posted listener tasks
+    listenerExecutor.runListenerTasks();
     for (Sendable data : tablesToData.values()) {
       SendableRegistry.update(data);
     }
-    // Execute posted listener tasks
-    listenerExecutor.runListenerTasks();
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
index a576991..1287f4a 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color.java
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.util;
 
+import edu.wpi.first.math.MathUtil;
 import java.util.Objects;
 
-import edu.wpi.first.wpiutil.math.MathUtil;
-
 /**
  * Represents colors.
  *
@@ -27,9 +23,9 @@
   /**
    * Constructs a Color.
    *
-   * @param red   Red value (0-1)
+   * @param red Red value (0-1)
    * @param green Green value (0-1)
-   * @param blue  Blue value (0-1)
+   * @param blue Blue value (0-1)
    */
   public Color(double red, double green, double blue) {
     this.red = roundAndClamp(red);
@@ -43,9 +39,7 @@
    * @param color The color
    */
   public Color(Color8Bit color) {
-    this(color.red / 255.0,
-        color.green / 255.0,
-        color.blue / 255.0);
+    this(color.red / 255.0, color.green / 255.0, color.blue / 255.0);
   }
 
   /**
@@ -114,722 +108,436 @@
    * FIRST Colors
    */
 
-  /**
-   * #1560BD.
-   */
+  /** 0x1560BD. */
   public static final Color kDenim = new Color(0.0823529412, 0.376470589, 0.7411764706);
 
-  /**
-   * #0066B3.
-   */
+  /** 0x0066B3. */
   public static final Color kFirstBlue = new Color(0.0, 0.4, 0.7019607844);
 
-  /**
-   * #ED1C24.
-   */
+  /** 0xED1C24. */
   public static final Color kFirstRed = new Color(0.9294117648, 0.1098039216, 0.1411764706);
 
   /*
    * Standard Colors
    */
 
-  /**
-   * #F0F8FF.
-   */
+  /** 0xF0F8FF. */
   public static final Color kAliceBlue = new Color(0.9411765f, 0.972549f, 1.0f);
 
-  /**
-   * #FAEBD7.
-   */
+  /** 0xFAEBD7. */
   public static final Color kAntiqueWhite = new Color(0.98039216f, 0.92156863f, 0.84313726f);
 
-  /**
-   * #00FFFF.
-   */
+  /** 0x00FFFF. */
   public static final Color kAqua = new Color(0.0f, 1.0f, 1.0f);
 
-  /**
-   * #7FFFD4.
-    */
+  /** 0x7FFFD4. */
   public static final Color kAquamarine = new Color(0.49803922f, 1.0f, 0.83137256f);
 
-  /**
-   * #F0FFFF.
-   */
+  /** 0xF0FFFF. */
   public static final Color kAzure = new Color(0.9411765f, 1.0f, 1.0f);
 
-  /**
-   * #F5F5DC.
-   */
+  /** 0xF5F5DC. */
   public static final Color kBeige = new Color(0.9607843f, 0.9607843f, 0.8627451f);
 
-  /**
-   * #FFE4C4.
-   */
+  /** 0xFFE4C4. */
   public static final Color kBisque = new Color(1.0f, 0.89411765f, 0.76862746f);
 
-  /**
-   * #000000.
-   */
+  /** 0x000000. */
   public static final Color kBlack = new Color(0.0f, 0.0f, 0.0f);
 
-  /**
-   * #FFEBCD.
-   */
+  /** 0xFFEBCD. */
   public static final Color kBlanchedAlmond = new Color(1.0f, 0.92156863f, 0.8039216f);
 
-  /**
-   * #0000FF.
-   */
+  /** 0x0000FF. */
   public static final Color kBlue = new Color(0.0f, 0.0f, 1.0f);
 
-  /**
-   * #8A2BE2.
-   */
+  /** 0x8A2BE2. */
   public static final Color kBlueViolet = new Color(0.5411765f, 0.16862746f, 0.8862745f);
 
-  /**
-   * #A52A2A.
-   */
+  /** 0xA52A2A. */
   public static final Color kBrown = new Color(0.64705884f, 0.16470589f, 0.16470589f);
 
-  /**
-   * #DEB887.
-   */
+  /** 0xDEB887. */
   public static final Color kBurlywood = new Color(0.87058824f, 0.72156864f, 0.5294118f);
 
-  /**
-   * #5F9EA0.
-   */
+  /** 0x5F9EA0. */
   public static final Color kCadetBlue = new Color(0.37254903f, 0.61960787f, 0.627451f);
 
-  /**
-   * #7FFF00.
-   */
+  /** 0x7FFF00. */
   public static final Color kChartreuse = new Color(0.49803922f, 1.0f, 0.0f);
 
-  /**
-   * #D2691E.
-   */
+  /** 0xD2691E. */
   public static final Color kChocolate = new Color(0.8235294f, 0.4117647f, 0.11764706f);
 
-  /**
-   * #FF7F50.
-   */
+  /** 0xFF7F50. */
   public static final Color kCoral = new Color(1.0f, 0.49803922f, 0.3137255f);
 
-  /**
-   * #6495ED.
-   */
+  /** 0x6495ED. */
   public static final Color kCornflowerBlue = new Color(0.39215687f, 0.58431375f, 0.92941177f);
 
-  /**
-   * #FFF8DC.
-   */
+  /** 0xFFF8DC. */
   public static final Color kCornsilk = new Color(1.0f, 0.972549f, 0.8627451f);
 
-  /**
-   * #DC143C.
-   */
+  /** 0xDC143C. */
   public static final Color kCrimson = new Color(0.8627451f, 0.078431375f, 0.23529412f);
 
-  /**
-   * #00FFFF.
-   */
+  /** 0x00FFFF. */
   public static final Color kCyan = new Color(0.0f, 1.0f, 1.0f);
 
-  /**
-   * #00008B.
-   */
+  /** 0x00008B. */
   public static final Color kDarkBlue = new Color(0.0f, 0.0f, 0.54509807f);
 
-  /**
-   * #008B8B.
-   */
+  /** 0x008B8B. */
   public static final Color kDarkCyan = new Color(0.0f, 0.54509807f, 0.54509807f);
 
-  /**
-   * #B8860B.
-   */
+  /** 0xB8860B. */
   public static final Color kDarkGoldenrod = new Color(0.72156864f, 0.5254902f, 0.043137256f);
 
-  /**
-   * #A9A9A9.
-   */
+  /** 0xA9A9A9. */
   public static final Color kDarkGray = new Color(0.6627451f, 0.6627451f, 0.6627451f);
 
-  /**
-   * #006400.
-   */
+  /** 0x006400. */
   public static final Color kDarkGreen = new Color(0.0f, 0.39215687f, 0.0f);
 
-  /**
-   * #BDB76B.
-   */
+  /** 0xBDB76B. */
   public static final Color kDarkKhaki = new Color(0.7411765f, 0.7176471f, 0.41960785f);
 
-  /**
-   * #8B008B.
-   */
+  /** 0x8B008B. */
   public static final Color kDarkMagenta = new Color(0.54509807f, 0.0f, 0.54509807f);
 
-  /**
-   * #556B2F.
-   */
+  /** 0x556B2F. */
   public static final Color kDarkOliveGreen = new Color(0.33333334f, 0.41960785f, 0.18431373f);
 
-  /**
-   * #FF8C00.
-   */
+  /** 0xFF8C00. */
   public static final Color kDarkOrange = new Color(1.0f, 0.54901963f, 0.0f);
 
-  /**
-   * #9932CC.
-   */
+  /** 0x9932CC. */
   public static final Color kDarkOrchid = new Color(0.6f, 0.19607843f, 0.8f);
 
-  /**
-   * #8B0000.
-   */
+  /** 0x8B0000. */
   public static final Color kDarkRed = new Color(0.54509807f, 0.0f, 0.0f);
 
-  /**
-   * #E9967A.
-   */
+  /** 0xE9967A. */
   public static final Color kDarkSalmon = new Color(0.9137255f, 0.5882353f, 0.47843137f);
 
-  /**
-   * #8FBC8F.
-   */
+  /** 0x8FBC8F. */
   public static final Color kDarkSeaGreen = new Color(0.56078434f, 0.7372549f, 0.56078434f);
 
-  /**
-   * #483D8B.
-   */
+  /** 0x483D8B. */
   public static final Color kDarkSlateBlue = new Color(0.28235295f, 0.23921569f, 0.54509807f);
 
-  /**
-   * #2F4F4F.
-   */
+  /** 0x2F4F4F. */
   public static final Color kDarkSlateGray = new Color(0.18431373f, 0.30980393f, 0.30980393f);
 
-  /**
-   * #00CED1.
-   */
+  /** 0x00CED1. */
   public static final Color kDarkTurquoise = new Color(0.0f, 0.80784315f, 0.81960785f);
 
-  /**
-   * #9400D3.
-   */
+  /** 0x9400D3. */
   public static final Color kDarkViolet = new Color(0.5803922f, 0.0f, 0.827451f);
 
-  /**
-   * #FF1493.
-   */
+  /** 0xFF1493. */
   public static final Color kDeepPink = new Color(1.0f, 0.078431375f, 0.5764706f);
 
-  /**
-   * #00BFFF.
-   */
+  /** 0x00BFFF. */
   public static final Color kDeepSkyBlue = new Color(0.0f, 0.7490196f, 1.0f);
 
-  /**
-   * #696969.
-   */
+  /** 0x696969. */
   public static final Color kDimGray = new Color(0.4117647f, 0.4117647f, 0.4117647f);
 
-  /**
-   * #1E90FF.
-   */
+  /** 0x1E90FF. */
   public static final Color kDodgerBlue = new Color(0.11764706f, 0.5647059f, 1.0f);
 
-  /**
-   * #B22222.
-   */
+  /** 0xB22222. */
   public static final Color kFirebrick = new Color(0.69803923f, 0.13333334f, 0.13333334f);
 
-  /**
-   * #FFFAF0.
-   */
+  /** 0xFFFAF0. */
   public static final Color kFloralWhite = new Color(1.0f, 0.98039216f, 0.9411765f);
 
-  /**
-   * #228B22.
-   */
+  /** 0x228B22. */
   public static final Color kForestGreen = new Color(0.13333334f, 0.54509807f, 0.13333334f);
 
-  /**
-   * #FF00FF.
-   */
+  /** 0xFF00FF. */
   public static final Color kFuchsia = new Color(1.0f, 0.0f, 1.0f);
 
-  /**
-   * #DCDCDC.
-   */
+  /** 0xDCDCDC. */
   public static final Color kGainsboro = new Color(0.8627451f, 0.8627451f, 0.8627451f);
 
-  /**
-   * #F8F8FF.
-   */
+  /** 0xF8F8FF. */
   public static final Color kGhostWhite = new Color(0.972549f, 0.972549f, 1.0f);
 
-  /**
-   * #FFD700.
-   */
+  /** 0xFFD700. */
   public static final Color kGold = new Color(1.0f, 0.84313726f, 0.0f);
 
-  /**
-   * #DAA520.
-   */
+  /** 0xDAA520. */
   public static final Color kGoldenrod = new Color(0.85490197f, 0.64705884f, 0.1254902f);
 
-  /**
-   * #808080.
-   */
+  /** 0x808080. */
   public static final Color kGray = new Color(0.5019608f, 0.5019608f, 0.5019608f);
 
-  /**
-   * #008000.
-   */
+  /** 0x008000. */
   public static final Color kGreen = new Color(0.0f, 0.5019608f, 0.0f);
 
-  /**
-   * #ADFF2F.
-   */
+  /** 0xADFF2F. */
   public static final Color kGreenYellow = new Color(0.6784314f, 1.0f, 0.18431373f);
 
-  /**
-   * #F0FFF0.
-   */
+  /** 0xF0FFF0. */
   public static final Color kHoneydew = new Color(0.9411765f, 1.0f, 0.9411765f);
 
-  /**
-   * #FF69B4.
-   */
+  /** 0xFF69B4. */
   public static final Color kHotPink = new Color(1.0f, 0.4117647f, 0.7058824f);
 
-  /**
-   * #CD5C5C.
-   */
+  /** 0xCD5C5C. */
   public static final Color kIndianRed = new Color(0.8039216f, 0.36078432f, 0.36078432f);
 
-  /**
-   * #4B0082.
-   */
+  /** 0x4B0082. */
   public static final Color kIndigo = new Color(0.29411766f, 0.0f, 0.50980395f);
 
-  /**
-   * #FFFFF0.
-   */
+  /** 0xFFFFF0. */
   public static final Color kIvory = new Color(1.0f, 1.0f, 0.9411765f);
 
-  /**
-   * #F0E68C.
-   */
+  /** 0xF0E68C. */
   public static final Color kKhaki = new Color(0.9411765f, 0.9019608f, 0.54901963f);
 
-  /**
-   * #E6E6FA.
-   */
+  /** 0xE6E6FA. */
   public static final Color kLavender = new Color(0.9019608f, 0.9019608f, 0.98039216f);
 
-  /**
-   * #FFF0F5.
-   */
+  /** 0xFFF0F5. */
   public static final Color kLavenderBlush = new Color(1.0f, 0.9411765f, 0.9607843f);
 
-  /**
-   * #7CFC00.
-   */
+  /** 0x7CFC00. */
   public static final Color kLawnGreen = new Color(0.4862745f, 0.9882353f, 0.0f);
 
-  /**
-   * #FFFACD.
-   */
+  /** 0xFFFACD. */
   public static final Color kLemonChiffon = new Color(1.0f, 0.98039216f, 0.8039216f);
 
-  /**
-   * #ADD8E6.
-   */
+  /** 0xADD8E6. */
   public static final Color kLightBlue = new Color(0.6784314f, 0.84705883f, 0.9019608f);
 
-  /**
-   * #F08080.
-   */
+  /** 0xF08080. */
   public static final Color kLightCoral = new Color(0.9411765f, 0.5019608f, 0.5019608f);
 
-  /**
-   * #E0FFFF.
-   */
+  /** 0xE0FFFF. */
   public static final Color kLightCyan = new Color(0.8784314f, 1.0f, 1.0f);
 
-  /**
-   * #FAFAD2.
-   */
+  /** 0xFAFAD2. */
   public static final Color kLightGoldenrodYellow = new Color(0.98039216f, 0.98039216f, 0.8235294f);
 
-  /**
-   * #D3D3D3.
-   */
+  /** 0xD3D3D3. */
   public static final Color kLightGray = new Color(0.827451f, 0.827451f, 0.827451f);
 
-  /**
-   * #90EE90.
-   */
+  /** 0x90EE90. */
   public static final Color kLightGreen = new Color(0.5647059f, 0.93333334f, 0.5647059f);
 
-  /**
-   * #FFB6C1.
-   */
+  /** 0xFFB6C1. */
   public static final Color kLightPink = new Color(1.0f, 0.7137255f, 0.75686276f);
 
-  /**
-   * #FFA07A.
-   */
+  /** 0xFFA07A. */
   public static final Color kLightSalmon = new Color(1.0f, 0.627451f, 0.47843137f);
 
-  /**
-   * #20B2AA.
-   */
+  /** 0x20B2AA. */
   public static final Color kLightSeaGreen = new Color(0.1254902f, 0.69803923f, 0.6666667f);
 
-  /**
-   * #87CEFA.
-   */
+  /** 0x87CEFA. */
   public static final Color kLightSkyBlue = new Color(0.5294118f, 0.80784315f, 0.98039216f);
 
-  /**
-   * #778899.
-   */
+  /** 0x778899. */
   public static final Color kLightSlateGray = new Color(0.46666667f, 0.53333336f, 0.6f);
 
-  /**
-   * #B0C4DE.
-   */
+  /** 0xB0C4DE. */
   public static final Color kLightSteelBlue = new Color(0.6901961f, 0.76862746f, 0.87058824f);
 
-  /**
-   * #FFFFE0.
-   */
+  /** 0xFFFFE0. */
   public static final Color kLightYellow = new Color(1.0f, 1.0f, 0.8784314f);
 
-  /**
-   * #00FF00.
-   */
+  /** 0x00FF00. */
   public static final Color kLime = new Color(0.0f, 1.0f, 0.0f);
 
-  /**
-   * #32CD32.
-   */
+  /** 0x32CD32. */
   public static final Color kLimeGreen = new Color(0.19607843f, 0.8039216f, 0.19607843f);
 
-  /**
-   * #FAF0E6.
-   */
+  /** 0xFAF0E6. */
   public static final Color kLinen = new Color(0.98039216f, 0.9411765f, 0.9019608f);
 
-  /**
-   * #FF00FF.
-   */
+  /** 0xFF00FF. */
   public static final Color kMagenta = new Color(1.0f, 0.0f, 1.0f);
 
-  /**
-   * #800000.
-   */
+  /** 0x800000. */
   public static final Color kMaroon = new Color(0.5019608f, 0.0f, 0.0f);
 
-  /**
-   * #66CDAA.
-   */
+  /** 0x66CDAA. */
   public static final Color kMediumAquamarine = new Color(0.4f, 0.8039216f, 0.6666667f);
 
-  /**
-   * #0000CD.
-   */
+  /** 0x0000CD. */
   public static final Color kMediumBlue = new Color(0.0f, 0.0f, 0.8039216f);
 
-  /**
-   * #BA55D3.
-   */
+  /** 0xBA55D3. */
   public static final Color kMediumOrchid = new Color(0.7294118f, 0.33333334f, 0.827451f);
 
-  /**
-   * #9370DB.
-   */
+  /** 0x9370DB. */
   public static final Color kMediumPurple = new Color(0.5764706f, 0.4392157f, 0.85882354f);
 
-  /**
-   * #3CB371.
-   */
+  /** 0x3CB371. */
   public static final Color kMediumSeaGreen = new Color(0.23529412f, 0.7019608f, 0.44313726f);
 
-  /**
-   * #7B68EE.
-   */
+  /** 0x7B68EE. */
   public static final Color kMediumSlateBlue = new Color(0.48235294f, 0.40784314f, 0.93333334f);
 
-  /**
-   * #00FA9A.
-   */
+  /** 0x00FA9A. */
   public static final Color kMediumSpringGreen = new Color(0.0f, 0.98039216f, 0.6039216f);
 
-  /**
-   * #48D1CC.
-   */
+  /** 0x48D1CC. */
   public static final Color kMediumTurquoise = new Color(0.28235295f, 0.81960785f, 0.8f);
 
-  /**
-   * #C71585.
-   */
+  /** 0xC71585. */
   public static final Color kMediumVioletRed = new Color(0.78039217f, 0.08235294f, 0.52156866f);
 
-  /**
-   * #191970.
-   */
+  /** 0x191970. */
   public static final Color kMidnightBlue = new Color(0.09803922f, 0.09803922f, 0.4392157f);
 
-  /**
-   * #F5FFFA.
-   */
+  /** 0xF5FFFA. */
   public static final Color kMintcream = new Color(0.9607843f, 1.0f, 0.98039216f);
 
-  /**
-   * #FFE4E1.
-   */
+  /** 0xFFE4E1. */
   public static final Color kMistyRose = new Color(1.0f, 0.89411765f, 0.88235295f);
 
-  /**
-   * #FFE4B5.
-   */
+  /** 0xFFE4B5. */
   public static final Color kMoccasin = new Color(1.0f, 0.89411765f, 0.70980394f);
 
-  /**
-   * #FFDEAD.
-   */
+  /** 0xFFDEAD. */
   public static final Color kNavajoWhite = new Color(1.0f, 0.87058824f, 0.6784314f);
 
-  /**
-   * #000080.
-   */
+  /** 0x000080. */
   public static final Color kNavy = new Color(0.0f, 0.0f, 0.5019608f);
 
-  /**
-   * #FDF5E6.
-   */
+  /** 0xFDF5E6. */
   public static final Color kOldLace = new Color(0.99215686f, 0.9607843f, 0.9019608f);
 
-  /**
-   * #808000.
-   */
+  /** 0x808000. */
   public static final Color kOlive = new Color(0.5019608f, 0.5019608f, 0.0f);
 
-  /**
-   * #6B8E23.
-   */
+  /** 0x6B8E23. */
   public static final Color kOliveDrab = new Color(0.41960785f, 0.5568628f, 0.13725491f);
 
-  /**
-   * #FFA500.
-   */
+  /** 0xFFA500. */
   public static final Color kOrange = new Color(1.0f, 0.64705884f, 0.0f);
 
-  /**
-   * #FF4500.
-   */
+  /** 0xFF4500. */
   public static final Color kOrangeRed = new Color(1.0f, 0.27058825f, 0.0f);
 
-  /**
-   * #DA70D6.
-   */
+  /** 0xDA70D6. */
   public static final Color kOrchid = new Color(0.85490197f, 0.4392157f, 0.8392157f);
 
-  /**
-   * #EEE8AA.
-   */
+  /** 0xEEE8AA. */
   public static final Color kPaleGoldenrod = new Color(0.93333334f, 0.9098039f, 0.6666667f);
 
-  /**
-   * #98FB98.
-   */
+  /** 0x98FB98. */
   public static final Color kPaleGreen = new Color(0.59607846f, 0.9843137f, 0.59607846f);
 
-  /**
-   * #AFEEEE.
-   */
+  /** 0xAFEEEE. */
   public static final Color kPaleTurquoise = new Color(0.6862745f, 0.93333334f, 0.93333334f);
 
-  /**
-   * #DB7093.
-   */
+  /** 0xDB7093. */
   public static final Color kPaleVioletRed = new Color(0.85882354f, 0.4392157f, 0.5764706f);
 
-  /**
-   * #FFEFD5.
-   */
+  /** 0xFFEFD5. */
   public static final Color kPapayaWhip = new Color(1.0f, 0.9372549f, 0.8352941f);
 
-  /**
-   * #FFDAB9.
-   */
+  /** 0xFFDAB9. */
   public static final Color kPeachPuff = new Color(1.0f, 0.85490197f, 0.7254902f);
 
-  /**
-   * #CD853F.
-   */
+  /** 0xCD853F. */
   public static final Color kPeru = new Color(0.8039216f, 0.52156866f, 0.24705882f);
 
-  /**
-   * #FFC0CB.
-   */
+  /** 0xFFC0CB. */
   public static final Color kPink = new Color(1.0f, 0.7529412f, 0.79607844f);
 
-  /**
-   * #DDA0DD.
-   */
+  /** 0xDDA0DD. */
   public static final Color kPlum = new Color(0.8666667f, 0.627451f, 0.8666667f);
 
-  /**
-   * #B0E0E6.
-   */
+  /** 0xB0E0E6. */
   public static final Color kPowderBlue = new Color(0.6901961f, 0.8784314f, 0.9019608f);
 
-  /**
-   * #800080.
-   */
+  /** 0x800080. */
   public static final Color kPurple = new Color(0.5019608f, 0.0f, 0.5019608f);
 
-  /**
-   * #FF0000.
-   */
+  /** 0xFF0000. */
   public static final Color kRed = new Color(1.0f, 0.0f, 0.0f);
 
-  /**
-   * #BC8F8F.
-   */
+  /** 0xBC8F8F. */
   public static final Color kRosyBrown = new Color(0.7372549f, 0.56078434f, 0.56078434f);
 
-  /**
-   * #4169E1.
-   */
+  /** 0x4169E1. */
   public static final Color kRoyalBlue = new Color(0.25490198f, 0.4117647f, 0.88235295f);
 
-  /**
-   * #8B4513.
-   */
+  /** 0x8B4513. */
   public static final Color kSaddleBrown = new Color(0.54509807f, 0.27058825f, 0.07450981f);
 
-  /**
-   * #FA8072.
-   */
+  /** 0xFA8072. */
   public static final Color kSalmon = new Color(0.98039216f, 0.5019608f, 0.44705883f);
 
-  /**
-   * #F4A460.
-   */
+  /** 0xF4A460. */
   public static final Color kSandyBrown = new Color(0.95686275f, 0.6431373f, 0.3764706f);
 
-  /**
-   * #2E8B57.
-   */
+  /** 0x2E8B57. */
   public static final Color kSeaGreen = new Color(0.18039216f, 0.54509807f, 0.34117648f);
 
-  /**
-   * #FFF5EE.
-   */
+  /** 0xFFF5EE. */
   public static final Color kSeashell = new Color(1.0f, 0.9607843f, 0.93333334f);
 
-  /**
-   * #A0522D.
-   */
+  /** 0xA0522D. */
   public static final Color kSienna = new Color(0.627451f, 0.32156864f, 0.1764706f);
 
-  /**
-   * #C0C0C0.
-   */
+  /** 0xC0C0C0. */
   public static final Color kSilver = new Color(0.7529412f, 0.7529412f, 0.7529412f);
 
-  /**
-   * #87CEEB.
-   */
+  /** 0x87CEEB. */
   public static final Color kSkyBlue = new Color(0.5294118f, 0.80784315f, 0.92156863f);
 
-  /**
-   * #6A5ACD.
-   */
+  /** 0x6A5ACD. */
   public static final Color kSlateBlue = new Color(0.41568628f, 0.3529412f, 0.8039216f);
 
-  /**
-   * #708090.
-   */
+  /** 0x708090. */
   public static final Color kSlateGray = new Color(0.4392157f, 0.5019608f, 0.5647059f);
 
-  /**
-   * #FFFAFA.
-   */
+  /** 0xFFFAFA. */
   public static final Color kSnow = new Color(1.0f, 0.98039216f, 0.98039216f);
 
-  /**
-   * #00FF7F.
-   */
+  /** 0x00FF7F. */
   public static final Color kSpringGreen = new Color(0.0f, 1.0f, 0.49803922f);
 
-  /**
-   * #4682B4.
-   */
+  /** 0x4682B4. */
   public static final Color kSteelBlue = new Color(0.27450982f, 0.50980395f, 0.7058824f);
 
-  /**
-   * #D2B48C.
-   */
+  /** 0xD2B48C. */
   public static final Color kTan = new Color(0.8235294f, 0.7058824f, 0.54901963f);
 
-  /**
-   * #008080.
-   */
+  /** 0x008080. */
   public static final Color kTeal = new Color(0.0f, 0.5019608f, 0.5019608f);
 
-  /**
-   * #D8BFD8.
-   */
+  /** 0xD8BFD8. */
   public static final Color kThistle = new Color(0.84705883f, 0.7490196f, 0.84705883f);
 
-  /**
-   * #FF6347.
-   */
+  /** 0xFF6347. */
   public static final Color kTomato = new Color(1.0f, 0.3882353f, 0.2784314f);
 
-  /**
-   * #40E0D0.
-   */
+  /** 0x40E0D0. */
   public static final Color kTurquoise = new Color(0.2509804f, 0.8784314f, 0.8156863f);
 
-  /**
-   * #EE82EE.
-   */
+  /** 0xEE82EE. */
   public static final Color kViolet = new Color(0.93333334f, 0.50980395f, 0.93333334f);
 
-  /**
-   * #F5DEB3.
-   */
+  /** 0xF5DEB3. */
   public static final Color kWheat = new Color(0.9607843f, 0.87058824f, 0.7019608f);
 
-  /**
-   * #FFFFFF.
-   */
+  /** 0xFFFFFF. */
   public static final Color kWhite = new Color(1.0f, 1.0f, 1.0f);
 
-  /**
-   * #F5F5F5.
-   */
+  /** 0xF5F5F5. */
   public static final Color kWhiteSmoke = new Color(0.9607843f, 0.9607843f, 0.9607843f);
 
-  /**
-   * #FFFF00.
-   */
+  /** 0xFFFF00. */
   public static final Color kYellow = new Color(1.0f, 1.0f, 0.0f);
 
-  /**
-   * #9ACD32.
-   */
+  /** 0x9ACD32. */
   public static final Color kYellowGreen = new Color(0.6039216f, 0.8039216f, 0.19607843f);
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
index 50d42fc..cd7154d 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/Color8Bit.java
@@ -1,19 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.util;
 
+import edu.wpi.first.math.MathUtil;
 import java.util.Objects;
 
-import edu.wpi.first.wpiutil.math.MathUtil;
-
-/**
- * Represents colors with 8 bits of precision.
- */
+/** Represents colors with 8 bits of precision. */
 @SuppressWarnings("MemberName")
 public class Color8Bit {
   public final int red;
@@ -23,9 +17,9 @@
   /**
    * Constructs a Color8Bit.
    *
-   * @param red   Red value (0-255)
+   * @param red Red value (0-255)
    * @param green Green value (0-255)
-   * @param blue  Blue value (0-255)
+   * @param blue Blue value (0-255)
    */
   public Color8Bit(int red, int green, int blue) {
     this.red = MathUtil.clamp(red, 0, 255);
@@ -39,9 +33,7 @@
    * @param color The color
    */
   public Color8Bit(Color color) {
-    this((int) (color.red * 255),
-        (int) (color.green * 255),
-        (int) (color.blue * 255));
+    this((int) (color.red * 255), (int) (color.green * 255), (int) (color.blue * 255));
   }
 
   @Override
@@ -54,9 +46,7 @@
     }
 
     Color8Bit color8Bit = (Color8Bit) other;
-    return red == color8Bit.red
-        && green == color8Bit.green
-        && blue == color8Bit.blue;
+    return red == color8Bit.red && green == color8Bit.green && blue == color8Bit.blue;
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
index 8bc86b2..c5b5832 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/util/ErrorMessages.java
@@ -1,36 +1,36 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.util;
 
 import static java.util.Objects.requireNonNull;
 
-/**
- * Utility class for common WPILib error messages.
- */
+/** Utility class for common WPILib error messages. */
 public final class ErrorMessages {
-  /**
-   * Utility class, so constructor is private.
-   */
+  /** Utility class, so constructor is private. */
   private ErrorMessages() {
     throw new UnsupportedOperationException("This is a utility class!");
   }
 
   /**
-   * Requires that a parameter of a method not be null; prints an error message with
-   * helpful debugging instructions if the parameter is null.
+   * Requires that a parameter of a method not be null; prints an error message with helpful
+   * debugging instructions if the parameter is null.
    *
+   * @param <T> Type of object.
    * @param obj The parameter that must not be null.
    * @param paramName The name of the parameter.
    * @param methodName The name of the method.
+   * @return The object parameter confirmed not to be null.
    */
   public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
-    return requireNonNull(obj,
-        "Parameter " + paramName + " in method " + methodName + " was null when it"
+    return requireNonNull(
+        obj,
+        "Parameter "
+            + paramName
+            + " in method "
+            + methodName
+            + " was null when it"
             + " should not have been!  Check the stacktrace to find the responsible line of code - "
             + "usually, it is the first line of user-written code indicated in the stacktrace.  "
             + "Make sure all objects passed to the method in question were properly initialized -"
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
index 2c0f8f2..d888d3b 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionPipeline.java
@@ -1,29 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.vision;
 
 import org.opencv.core.Mat;
 
 /**
- * A vision pipeline is responsible for running a group of
- * OpenCV algorithms to extract data from an image.
+ * A vision pipeline is responsible for running a group of OpenCV algorithms to extract data from an
+ * image.
  *
  * @see VisionRunner
  * @see VisionThread
- *
  * @deprecated Replaced with edu.wpi.first.vision.VisionPipeline
  */
 @Deprecated
 public interface VisionPipeline {
   /**
-   * Processes the image input and sets the result objects.
-   * Implementations should make these objects accessible.
+   * Processes the image input and sets the result objects. Implementations should make these
+   * objects accessible.
+   *
+   * @param image The image to process.
    */
   void process(Mat image);
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
index 328686a..a1002f5 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionRunner.java
@@ -1,27 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.vision;
 
+import edu.wpi.first.cameraserver.CameraServerSharedStore;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.VideoSource;
 import org.opencv.core.Mat;
 
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.VideoSource;
-import edu.wpi.first.cameraserver.CameraServerSharedStore;
-
 /**
- * A vision runner is a convenient wrapper object to make it easy to run vision pipelines
- * from robot code. The easiest  way to use this is to run it in a {@link VisionThread}
- * and use the listener to take snapshots of the pipeline's outputs.
+ * A vision runner is a convenient wrapper object to make it easy to run vision pipelines from robot
+ * code. The easiest way to use this is to run it in a {@link VisionThread} and use the listener to
+ * take snapshots of the pipeline's outputs.
  *
  * @see VisionPipeline
  * @see VisionThread
  * @see <a href="package-summary.html">vision</a>
- *
  * @deprecated Replaced with edu.wpi.first.vision.VisionRunner
  */
 @Deprecated
@@ -48,17 +43,16 @@
      * @param pipeline the vision pipeline that ran
      */
     void copyPipelineOutputs(P pipeline);
-
   }
 
   /**
-   * Creates a new vision runner. It will take images from the {@code videoSource}, send them to
-   * the {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert
-   * user code when it is safe to access the pipeline's outputs.
+   * Creates a new vision runner. It will take images from the {@code videoSource}, send them to the
+   * {@code pipeline}, and call the {@code listener} when the pipeline has finished to alert user
+   * code when it is safe to access the pipeline's outputs.
    *
    * @param videoSource the video source to use to supply images for the pipeline
-   * @param pipeline    the vision pipeline to run
-   * @param listener    a function to call after the pipeline has finished running
+   * @param pipeline the vision pipeline to run
+   * @param listener a function to call after the pipeline has finished running
    */
   public VisionRunner(VideoSource videoSource, P pipeline, Listener<? super P> listener) {
     this.m_pipeline = pipeline;
@@ -67,15 +61,15 @@
   }
 
   /**
-   * Runs the pipeline one time, giving it the next image from the video source specified
-   * in the constructor. This will block until the source either has an image or throws an error.
-   * If the source successfully supplied a frame, the pipeline's image input will be set,
-   * the pipeline will run, and the listener specified in the constructor will be called to notify
-   * it that the pipeline ran.
+   * Runs the pipeline one time, giving it the next image from the video source specified in the
+   * constructor. This will block until the source either has an image or throws an error. If the
+   * source successfully supplied a frame, the pipeline's image input will be set, the pipeline will
+   * run, and the listener specified in the constructor will be called to notify it that the
+   * pipeline ran.
    *
-   * <p>This method is exposed to allow teams to add additional functionality or have their own
-   * ways to run the pipeline. Most teams, however, should just use {@link #runForever} in its own
-   * thread using a {@link VisionThread}.</p>
+   * <p>This method is exposed to allow teams to add additional functionality or have their own ways
+   * to run the pipeline. Most teams, however, should just use {@link #runForever} in its own thread
+   * using a {@link VisionThread}.
    */
   public void runOnce() {
     Long id = CameraServerSharedStore.getCameraServerShared().getRobotMainThreadId();
@@ -101,11 +95,11 @@
   }
 
   /**
-   * A convenience method that calls {@link #runOnce()} in an infinite loop. This must
-   * be run in a dedicated thread, and cannot be used in the main robot thread because
-   * it will freeze the robot program.
+   * A convenience method that calls {@link #runOnce()} in an infinite loop. This must be run in a
+   * dedicated thread, and cannot be used in the main robot thread because it will freeze the robot
+   * program.
    *
-   * <p><strong>Do not call this method directly from the main thread.</strong></p>
+   * <p><strong>Do not call this method directly from the main thread.</strong>
    *
    * @throws IllegalStateException if this is called from the main robot thread
    * @see VisionThread
@@ -122,9 +116,7 @@
     }
   }
 
-  /**
-   * Stop a RunForever() loop.
-   */
+  /** Stop a RunForever() loop. */
   public void stop() {
     m_enabled = false;
   }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
index 5184ba0..5f23fe1 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/VisionThread.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.vision;
 
-import edu.wpi.cscore.VideoSource;
+import edu.wpi.first.cscore.VideoSource;
 
 /**
- * A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread;
- * it does not prevent the program from exiting when all other non-daemon threads
- * have finished running.
+ * A vision thread is a special thread that runs a vision pipeline. It is a <i>daemon</i> thread; it
+ * does not prevent the program from exiting when all other non-daemon threads have finished
+ * running.
  *
  * @see VisionPipeline
  * @see VisionRunner
  * @see Thread#setDaemon(boolean)
- *
  * @deprecated Replaced with edu.wpi.first.vision.VisionThread
  */
 @Deprecated
@@ -37,14 +33,12 @@
    * equivalent to {@code new VisionThread(new VisionRunner<>(videoSource, pipeline, listener))}.
    *
    * @param videoSource the source for images the pipeline should process
-   * @param pipeline    the pipeline to run
-   * @param listener    the listener to copy outputs from the pipeline after it runs
-   * @param <P>         the type of the pipeline
+   * @param pipeline the pipeline to run
+   * @param listener the listener to copy outputs from the pipeline after it runs
+   * @param <P> the type of the pipeline
    */
-  public <P extends VisionPipeline> VisionThread(VideoSource videoSource,
-                                                 P pipeline,
-                                                 VisionRunner.Listener<? super P> listener) {
+  public <P extends VisionPipeline> VisionThread(
+      VideoSource videoSource, P pipeline, VisionRunner.Listener<? super P> listener) {
     this(new VisionRunner<>(videoSource, pipeline, listener));
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
index 01c8d73..b52f4ed 100644
--- a/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
+++ b/third_party/allwpilib/wpilibj/src/main/java/edu/wpi/first/wpilibj/vision/package-info.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /**
- * Classes in the {@code edu.wpi.first.wpilibj.vision} package are designed to
- * simplify using OpenCV vision processing code from a robot program.
+ * Classes in the {@code edu.wpi.first.wpilibj.vision} package are designed to simplify using OpenCV
+ * vision processing code from a robot program.
  *
- * <p>An example use case for grabbing a yellow tote from 2015 in autonomous:
- * <br>
+ * <p>An example use case for grabbing a yellow tote from 2015 in autonomous: <br>
+ *
  * <pre><code>
  * public class Robot extends IterativeRobot
  *     implements VisionRunner.Listener&lt;MyFindTotePipeline&gt; {
  *
  *      // A USB camera connected to the roboRIO.
- *      private {@link edu.wpi.cscore.VideoSource VideoSource} usbCamera;
+ *      private {@link edu.wpi.first.cscore.VideoSource VideoSource} usbCamera;
  *
  *      // A vision pipeline. This could be handwritten or generated by GRIP.
  *      // This has to implement {@link edu.wpi.first.wpilibj.vision.VisionPipeline}.
@@ -47,7 +44,7 @@
  *
  *     {@literal @}Override
  *      public void robotInit() {
- *          usbCamera = CameraServer.getInstance().startAutomaticCapture(0);
+ *          usbCamera = CameraServer.startAutomaticCapture(0);
  *          findTotePipeline = new MyFindTotePipeline();
  *          findToteThread = new VisionThread(usbCamera, findTotePipeline, this);
  *      }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/math/util/ColorTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/math/util/ColorTest.java
new file mode 100644
index 0000000..6685314
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/math/util/ColorTest.java
@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.util;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import edu.wpi.first.wpilibj.util.Color;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class ColorTest {
+  private static final double kEpsilon = 1e-3;
+
+  void assertColorMatches(double red, double green, double blue, Color color) {
+    assertAll(
+        () -> assertEquals(red, color.red, kEpsilon),
+        () -> assertEquals(green, color.green, kEpsilon),
+        () -> assertEquals(blue, color.blue, kEpsilon));
+  }
+
+  @ParameterizedTest
+  @MethodSource("staticColorProvider")
+  void staticColorTest(double red, double green, double blue, Color color) {
+    assertColorMatches(red, green, blue, color);
+  }
+
+  @ParameterizedTest
+  @MethodSource("staticColorProvider")
+  void colorEqualsTest(double red, double green, double blue, Color color) {
+    assertEquals(color, new Color(red, green, blue));
+  }
+
+  static Stream<Arguments> staticColorProvider() {
+    return Stream.of(
+        arguments(0.0823529412, 0.376470589, 0.7411764706, Color.kDenim),
+        arguments(0.0, 0.4, 0.7019607844, Color.kFirstBlue),
+        arguments(0.9294117648, 0.1098039216, 0.1411764706, Color.kFirstRed));
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/math/util/ErrorMessagesTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/math/util/ErrorMessagesTest.java
new file mode 100644
index 0000000..5153b00
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/math/util/ErrorMessagesTest.java
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.util;
+
+import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+import edu.wpi.first.wpilibj.util.ErrorMessages;
+import org.junit.jupiter.api.Test;
+
+class ErrorMessagesTest extends UtilityClassTest<ErrorMessages> {
+  ErrorMessagesTest() {
+    super(ErrorMessages.class);
+  }
+
+  @Test
+  void requireNonNullParamNullTest() {
+    assertThrows(
+        NullPointerException.class, () -> requireNonNullParam(null, "testParam", "testMethod"));
+  }
+
+  @Test
+  void requireNonNullParamNotNullTest() {
+    assertDoesNotThrow(() -> requireNonNullParam("null", "testParam", "testMethod"));
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
index 0bd0113..2a24dbe 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AddressableLEDBufferTest.java
@@ -1,27 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.stream.Stream;
-
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.Arguments;
-import org.junit.jupiter.params.provider.MethodSource;
-
-import edu.wpi.first.wpilibj.util.Color;
-import edu.wpi.first.wpilibj.util.Color8Bit;
-
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.params.provider.Arguments.arguments;
 
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
 class AddressableLEDBufferTest {
   @ParameterizedTest
   @MethodSource("hsvToRgbProvider")
@@ -32,8 +27,7 @@
     assertAll(
         () -> assertEquals((byte) r, buffer.m_buffer[2], "R value didn't match"),
         () -> assertEquals((byte) g, buffer.m_buffer[1], "G value didn't match"),
-        () -> assertEquals((byte) b, buffer.m_buffer[0], "B value didn't match")
-    );
+        () -> assertEquals((byte) b, buffer.m_buffer[0], "B value didn't match"));
   }
 
   static Stream<Arguments> hsvToRgbProvider() {
@@ -54,7 +48,7 @@
         arguments(150, 255, 128, 128, 0, 127), // Purple (ish)
         arguments(90, 255, 128, 0, 127, 128), // Teal (ish)
         arguments(120, 255, 128, 0, 0, 128) // Navy
-    );
+        );
   }
 
   @Test
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AnalogGyroTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AnalogGyroTest.java
new file mode 100644
index 0000000..c8eaad9
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AnalogGyroTest.java
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNotNull;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import org.junit.jupiter.api.Test;
+
+public class AnalogGyroTest {
+  @Test
+  void testInitializeWithAnalogInput() {
+    HAL.initialize(500, 0);
+    AnalogGyroSim sim = new AnalogGyroSim(0);
+    assertFalse(sim.getInitialized());
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogGyro gyro = new AnalogGyro(ai, 229, 17.4)) {
+      assertEquals(ai, gyro.getAnalogInput());
+    }
+  }
+
+  @Test
+  void testInitializeWithChannel() {
+    HAL.initialize(500, 0);
+    AnalogGyroSim sim = new AnalogGyroSim(0);
+    assertFalse(sim.getInitialized());
+
+    try (AnalogGyro gyro = new AnalogGyro(1, 191, 35.04)) {
+      assertNotNull(gyro.getAnalogInput());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
new file mode 100644
index 0000000..fa299a5
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.AnalogInputSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import org.junit.jupiter.api.Test;
+
+public class AnalogPotentiometerTest {
+  @Test
+  void testInitializeWithAnalogInput() {
+    HAL.initialize(500, 0);
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogPotentiometer pot = new AnalogPotentiometer(ai)) {
+      AnalogInputSim sim = new AnalogInputSim(ai);
+
+      RoboRioSim.resetData();
+      sim.setVoltage(4.0);
+      assertEquals(0.8, pot.get());
+    }
+  }
+
+  @Test
+  void testInitializeWithAnalogInputAndScale() {
+    HAL.initialize(500, 0);
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogPotentiometer pot = new AnalogPotentiometer(ai, 270.0)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(ai);
+
+      sim.setVoltage(5.0);
+      assertEquals(270.0, pot.get());
+
+      sim.setVoltage(2.5);
+      assertEquals(135, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(0.0, pot.get());
+    }
+  }
+
+  @Test
+  void testInitializeWithChannel() {
+    HAL.initialize(500, 0);
+
+    try (AnalogPotentiometer pot = new AnalogPotentiometer(1)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(1);
+
+      sim.setVoltage(5.0);
+      assertEquals(1.0, pot.get());
+    }
+  }
+
+  @Test
+  void testInitializeWithChannelAndScale() {
+    HAL.initialize(500, 0);
+
+    try (AnalogPotentiometer pot = new AnalogPotentiometer(1, 180.0)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(1);
+
+      sim.setVoltage(5.0);
+      assertEquals(180.0, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(0.0, pot.get());
+    }
+  }
+
+  @Test
+  void testWithModifiedBatteryVoltage() {
+    try (AnalogPotentiometer pot = new AnalogPotentiometer(1, 180.0, 90.0)) {
+      RoboRioSim.resetData();
+      AnalogInputSim sim = new AnalogInputSim(1);
+
+      // Test at 5v
+      sim.setVoltage(5.0);
+      assertEquals(270, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(90, pot.get());
+
+      // Simulate a lower battery voltage
+      RoboRioSim.setUserVoltage5V(2.5);
+
+      sim.setVoltage(2.5);
+      assertEquals(270, pot.get());
+
+      sim.setVoltage(2.0);
+      assertEquals(234, pot.get());
+
+      sim.setVoltage(0.0);
+      assertEquals(90, pot.get());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java
new file mode 100644
index 0000000..5a51f6c
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DebouncerTest.java
@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.Test;
+
+public class DebouncerTest {
+  @Test
+  void debounceRisingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kRising);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    SimHooks.stepTiming(0.1);
+
+    assertTrue(debouncer.calculate(true));
+  }
+
+  @Test
+  void debounceFallingTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kFalling);
+
+    debouncer.calculate(true);
+    assertTrue(debouncer.calculate(false));
+
+    SimHooks.stepTiming(0.1);
+
+    assertFalse(debouncer.calculate(false));
+  }
+
+  @Test
+  void debounceBothTest() {
+    var debouncer = new Debouncer(0.02, Debouncer.DebounceType.kBoth);
+
+    debouncer.calculate(false);
+    assertFalse(debouncer.calculate(true));
+
+    SimHooks.stepTiming(0.1);
+
+    assertTrue(debouncer.calculate(true));
+    assertTrue(debouncer.calculate(false));
+
+    SimHooks.stepTiming(0.1);
+
+    assertFalse(debouncer.calculate(false));
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DigitalOutputTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DigitalOutputTest.java
new file mode 100644
index 0000000..47b7eda
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DigitalOutputTest.java
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import org.junit.jupiter.api.Test;
+
+public class DigitalOutputTest {
+  @Test
+  void testDefaultFunctions() {
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      assertFalse(output.isAnalogTrigger());
+      assertEquals(0, output.getAnalogTriggerTypeForRouting());
+      assertFalse(output.isPulsing());
+    }
+  }
+
+  @Test
+  void testPwmFunctionsWithoutInitialization() {
+    HAL.initialize(500, 0);
+
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      assertDoesNotThrow(() -> output.updateDutyCycle(0.6));
+      assertDoesNotThrow(output::disablePWM);
+    }
+  }
+
+  @Test
+  void testPwmFunctionsWithInitialization() {
+    HAL.initialize(500, 0);
+
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DIOSim sim = new DIOSim(output);
+      assertEquals(0, sim.getPulseLength());
+
+      output.enablePWM(0.5);
+      output.updateDutyCycle(0.6);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestCTRE.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestCTRE.java
new file mode 100644
index 0000000..72a3e89
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestCTRE.java
@@ -0,0 +1,94 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+public class DoubleSolenoidTestCTRE {
+  @Test
+  void testValidInitialization() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.CTREPCM, 2, 3)) {
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kForward);
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testThrowForwardPortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(5, PneumaticsModuleType.CTREPCM, 2)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(5, PneumaticsModuleType.CTREPCM, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowReversePortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(6, PneumaticsModuleType.CTREPCM, 3)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(6, PneumaticsModuleType.CTREPCM, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowBothPortsAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid0 = new Solenoid(6, PneumaticsModuleType.CTREPCM, 2);
+        Solenoid solenoid1 = new Solenoid(6, PneumaticsModuleType.CTREPCM, 3)) {
+      assertThrows(
+          AllocationException.class,
+          () -> new DoubleSolenoid(6, PneumaticsModuleType.CTREPCM, 2, 3));
+    }
+  }
+
+  @Test
+  void testToggle() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(4, PneumaticsModuleType.CTREPCM, 2, 3)) {
+      // Bootstrap it into reverse
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      // Of shouldn't do anything on toggle
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testInvalidForwardPort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 100, 1));
+  }
+
+  @Test
+  void testInvalidReversePort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 0, 100));
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestREV.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestREV.java
new file mode 100644
index 0000000..888b677
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DoubleSolenoidTestREV.java
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+public class DoubleSolenoidTestREV {
+  @Test
+  void testValidInitialization() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.REVPH, 2, 3)) {
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kForward);
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testThrowForwardPortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(5, PneumaticsModuleType.REVPH, 2)) {
+      assertThrows(
+          AllocationException.class, () -> new DoubleSolenoid(5, PneumaticsModuleType.REVPH, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowReversePortAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid = new Solenoid(6, PneumaticsModuleType.REVPH, 3)) {
+      assertThrows(
+          AllocationException.class, () -> new DoubleSolenoid(6, PneumaticsModuleType.REVPH, 2, 3));
+    }
+  }
+
+  @Test
+  void testThrowBothPortsAlreadyInitialized() {
+    try (
+    // Single solenoid that is reused for forward port
+    Solenoid solenoid0 = new Solenoid(6, PneumaticsModuleType.REVPH, 2);
+        Solenoid solenoid1 = new Solenoid(6, PneumaticsModuleType.REVPH, 3)) {
+      assertThrows(
+          AllocationException.class, () -> new DoubleSolenoid(6, PneumaticsModuleType.REVPH, 2, 3));
+    }
+  }
+
+  @Test
+  void testToggle() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(4, PneumaticsModuleType.REVPH, 2, 3)) {
+      // Bootstrap it into reverse
+      solenoid.set(DoubleSolenoid.Value.kReverse);
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kForward, solenoid.get());
+
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kReverse, solenoid.get());
+
+      // Of shouldn't do anything on toggle
+      solenoid.set(DoubleSolenoid.Value.kOff);
+      solenoid.toggle();
+      assertEquals(DoubleSolenoid.Value.kOff, solenoid.get());
+    }
+  }
+
+  @Test
+  void testInvalidForwardPort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 100, 1));
+  }
+
+  @Test
+  void testInvalidReversePort() {
+    assertThrows(
+        IllegalArgumentException.class,
+        () -> new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 0, 100));
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java
index 50e16c8..5a44615 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/DriverStationTest.java
@@ -1,23 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.stream.Stream;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
 
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import java.util.stream.Stream;
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.Arguments;
 import org.junit.jupiter.params.provider.MethodSource;
 
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.params.provider.Arguments.arguments;
-
 class DriverStationTest {
   @ParameterizedTest
   @MethodSource("isConnectedProvider")
@@ -28,18 +23,17 @@
 
     DriverStationSim.notifyNewData();
 
-    assertEquals(expected, DriverStation.getInstance().isJoystickConnected(1));
+    assertEquals(expected, DriverStation.isJoystickConnected(1));
   }
 
   static Stream<Arguments> isConnectedProvider() {
     return Stream.of(
-      arguments(0, 0, 0, false),
-      arguments(1, 0, 0, true),
-      arguments(0, 1, 0, true),
-      arguments(0, 0, 1, true),
-      arguments(1, 1, 1, true),
-      arguments(4, 10, 1, true)
-    );
+        arguments(0, 0, 0, false),
+        arguments(1, 0, 0, true),
+        arguments(0, 1, 0, true),
+        arguments(0, 0, 1, true),
+        arguments(1, 1, 1, true),
+        arguments(4, 10, 1, true));
   }
 
   @MethodSource("connectionWarningProvider")
@@ -47,17 +41,15 @@
     DriverStationSim.setFmsAttached(fms);
     DriverStationSim.notifyNewData();
 
-    DriverStation.getInstance().silenceJoystickConnectionWarning(silence);
-    assertEquals(expected,
-        DriverStation.getInstance().isJoystickConnectionWarningSilenced());
+    DriverStation.silenceJoystickConnectionWarning(silence);
+    assertEquals(expected, DriverStation.isJoystickConnectionWarningSilenced());
   }
 
   static Stream<Arguments> connectionWarningProvider() {
     return Stream.of(
-      arguments(false, true, true),
-      arguments(false, false, false),
-      arguments(true, true, false),
-      arguments(true, false, false)
-    );
+        arguments(false, true, true),
+        arguments(false, false, false),
+        arguments(true, true, false),
+        arguments(true, false, false));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/InterruptTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/InterruptTest.java
new file mode 100644
index 0000000..2eecd66
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/InterruptTest.java
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.simulation.DIOSim;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
+
+public class InterruptTest {
+  @Test
+  void testAsynchronousInterrupt() {
+    AtomicBoolean hasFired = new AtomicBoolean(false);
+    AtomicInteger counter = new AtomicInteger(0);
+
+    try (DigitalInput di = new DigitalInput(0);
+        AsynchronousInterrupt interrupt =
+            new AsynchronousInterrupt(
+                di,
+                (a, b) -> {
+                  counter.incrementAndGet();
+                  hasFired.set(true);
+                })) {
+      interrupt.enable();
+      Timer.delay(0.5);
+      DIOSim digitalSim = new DIOSim(di);
+      digitalSim.setValue(false);
+      Timer.delay(0.01);
+      digitalSim.setValue(true);
+      Timer.delay(0.01);
+
+      int count = 0;
+      while (!hasFired.get()) {
+        Timer.delay(0.005);
+        count++;
+        assertTrue(count < 1000);
+      }
+      assertEquals(1, counter.get(), "The interrupt did not fire the expected number of times");
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java
index 7b1cfbb..bc79d0d 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/JoystickTest.java
@@ -1,18 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.simulation.JoystickSim;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
+import org.junit.jupiter.api.Test;
 
 class JoystickTest {
   @Test
@@ -44,4 +42,132 @@
     joysim.notifyNewData();
     assertEquals(0.0, joy.getY(), 0.001);
   }
+
+  @Test
+  void testGetZ() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setZ(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getZ(), 0.001);
+
+    joysim.setZ(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getZ(), 0.001);
+  }
+
+  @Test
+  void testGetTwist() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setTwist(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getTwist(), 0.001);
+
+    joysim.setTwist(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getTwist(), 0.001);
+  }
+
+  @Test
+  void testGetThrottle() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setThrottle(0.25);
+    joysim.notifyNewData();
+    assertEquals(0.25, joy.getThrottle(), 0.001);
+
+    joysim.setThrottle(0);
+    joysim.notifyNewData();
+    assertEquals(0.0, joy.getThrottle(), 0.001);
+  }
+
+  @Test
+  void testGetTrigger() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setTrigger(true);
+    joysim.notifyNewData();
+    assertTrue(joy.getTrigger());
+
+    joysim.setTrigger(false);
+    joysim.notifyNewData();
+    assertFalse(joy.getTrigger());
+  }
+
+  @Test
+  void testGetTop() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    joysim.setTop(true);
+    joysim.notifyNewData();
+    assertTrue(joy.getTop());
+
+    joysim.setTop(false);
+    joysim.notifyNewData();
+    assertFalse(joy.getTop());
+  }
+
+  @Test
+  void testGetMagnitude() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    // X Only
+    joysim.setX(0.5);
+    joysim.setY(0.0);
+    joysim.notifyNewData();
+    assertEquals(0.5, joy.getMagnitude(), 0.001);
+
+    // Y Only
+    joysim.setX(0.0);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(0.5, joy.getMagnitude(), 0.001);
+
+    // Both
+    joysim.setX(0.5);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(0.70710678118, joy.getMagnitude(), 0.001);
+  }
+
+  @Test
+  void testGetDirection() {
+    HAL.initialize(500, 0);
+    Joystick joy = new Joystick(1);
+    JoystickSim joysim = new JoystickSim(joy);
+
+    // X Only
+    joysim.setX(0.5);
+    joysim.setY(0.0);
+    joysim.notifyNewData();
+    assertEquals(90, joy.getDirectionDegrees(), 0.001);
+    assertEquals(Math.toRadians(90), joy.getDirectionRadians(), 0.001);
+
+    // Y Only
+    joysim.setX(0.0);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(0, joy.getDirectionDegrees(), 0.001);
+    assertEquals(Math.toRadians(0), joy.getDirectionRadians(), 0.001);
+
+    // Both
+    joysim.setX(0.5);
+    joysim.setY(-0.5);
+    joysim.notifyNewData();
+    assertEquals(45, joy.getDirectionDegrees(), 0.001);
+    assertEquals(Math.toRadians(45), joy.getDirectionRadians(), 0.001);
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
index 7406ca9..e70f182 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockHardwareExtension.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
 import org.junit.jupiter.api.extension.BeforeAllCallback;
 import org.junit.jupiter.api.extension.ExtensionContext;
 import org.junit.jupiter.api.extension.ExtensionContext.Namespace;
 
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-
 public final class MockHardwareExtension implements BeforeAllCallback {
   private static ExtensionContext getRoot(ExtensionContext context) {
     return context.getParent().map(MockHardwareExtension::getRoot).orElse(context);
@@ -21,10 +17,15 @@
 
   @Override
   public void beforeAll(ExtensionContext context) {
-    getRoot(context).getStore(Namespace.GLOBAL).getOrComputeIfAbsent("HAL Initialized", key -> {
-      initializeHardware();
-      return true;
-    }, Boolean.class);
+    getRoot(context)
+        .getStore(Namespace.GLOBAL)
+        .getOrComputeIfAbsent(
+            "HAL Initialized",
+            key -> {
+              initializeHardware();
+              return true;
+            },
+            Boolean.class);
   }
 
   private void initializeHardware() {
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java
deleted file mode 100644
index d183723..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/MockSpeedController.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-public class MockSpeedController implements SpeedController {
-  private double m_speed;
-  private boolean m_isInverted;
-
-  @Override
-  public void set(double speed) {
-    m_speed = m_isInverted ? -speed : speed;
-  }
-
-  @Override
-  public double get() {
-    return m_speed;
-  }
-
-  @Override
-  public void setInverted(boolean isInverted) {
-    m_isInverted = isInverted;
-  }
-
-  @Override
-  public boolean getInverted() {
-    return m_isInverted;
-  }
-
-  @Override
-  public void disable() {
-    m_speed = 0;
-  }
-
-  @Override
-  public void stopMotor() {
-    disable();
-  }
-
-  @Override
-  public void pidWrite(double output) {
-    set(output);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PS4ControllerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PS4ControllerTest.java
new file mode 100644
index 0000000..e969cb3
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PS4ControllerTest.java
@@ -0,0 +1,81 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.PS4ControllerSim;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+class PS4ControllerTest {
+  @ParameterizedTest
+  @EnumSource(value = PS4Controller.Button.class)
+  @SuppressWarnings({"VariableDeclarationUsageDistance"})
+  public void testButtons(PS4Controller.Button button)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    PS4Controller joy = new PS4Controller(2);
+    PS4ControllerSim joysim = new PS4ControllerSim(joy);
+
+    var buttonName = button.toString();
+
+    String simSetMethodName = "set" + buttonName;
+    String joyGetMethodName = "get" + buttonName;
+    String joyPressedMethodName = "get" + buttonName + "Pressed";
+    String joyReleasedMethodName = "get" + buttonName + "Released";
+
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, boolean.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+    Method joyPressedMethod = joy.getClass().getMethod(joyPressedMethodName);
+    Method joyReleasedMethod = joy.getClass().getMethod(joyReleasedMethodName);
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    // need to call pressed and released to clear flags
+    joyPressedMethod.invoke(joy);
+    joyReleasedMethod.invoke(joy);
+
+    simSetMethod.invoke(joysim, true);
+    joysim.notifyNewData();
+    assertTrue((Boolean) joyGetMethod.invoke(joy));
+    assertTrue((Boolean) joyPressedMethod.invoke(joy));
+    assertFalse((Boolean) joyReleasedMethod.invoke(joy));
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    assertFalse((Boolean) joyPressedMethod.invoke(joy));
+    assertTrue((Boolean) joyReleasedMethod.invoke(joy));
+  }
+
+  @ParameterizedTest
+  @EnumSource(value = PS4Controller.Axis.class)
+  @SuppressWarnings({"VariableDeclarationUsageDistance"})
+  public void testAxes(PS4Controller.Axis axis)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
+    HAL.initialize(500, 0);
+    PS4Controller joy = new PS4Controller(2);
+    PS4ControllerSim joysim = new PS4ControllerSim(joy);
+
+    var axisName = axis.toString();
+
+    String simSetMethodName = "set" + axisName;
+    String joyGetMethodName = "get" + axisName;
+
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, double.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+
+    simSetMethod.invoke(joysim, 0.35);
+    joysim.notifyNewData();
+    assertEquals(0.35, (Double) joyGetMethod.invoke(joy), 0.001);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
index 7b3735f..2603d5b 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/PreferencesTest.java
@@ -1,12 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.fail;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
 import java.io.IOException;
 import java.io.InputStream;
 import java.nio.file.Files;
@@ -14,7 +19,6 @@
 import java.util.Arrays;
 import java.util.Set;
 import java.util.stream.Stream;
-
 import org.junit.jupiter.api.AfterAll;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeAll;
@@ -25,15 +29,6 @@
 import org.junit.jupiter.params.ParameterizedTest;
 import org.junit.jupiter.params.provider.MethodSource;
 
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-import static org.junit.jupiter.api.Assertions.fail;
-
 class PreferencesTest {
   private final Preferences m_prefs = Preferences.getInstance();
   private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("Preferences");
@@ -76,8 +71,9 @@
     Set<String> keys = m_table.getKeys();
     keys.remove(".type");
 
-    assertTrue(keys.isEmpty(), "Preferences was not empty!  Preferences in table: "
-        + Arrays.toString(keys.toArray()));
+    assertTrue(
+        keys.isEmpty(),
+        "Preferences was not empty!  Preferences in table: " + Arrays.toString(keys.toArray()));
   }
 
   @ParameterizedTest
@@ -99,9 +95,8 @@
         () -> assertEquals(0.2, m_prefs.getDouble("checkedValueDouble", 0), 1e-6),
         () -> assertEquals("Hello. How are you?", m_prefs.getString("checkedValueString", "")),
         () -> assertEquals(2, m_prefs.getInt("checkedValueInt", 0)),
-        () -> assertEquals(3.14, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
-        () -> assertFalse(m_prefs.getBoolean("checkedValueBoolean", true))
-    );
+        () -> assertEquals(3.4, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
+        () -> assertFalse(m_prefs.getBoolean("checkedValueBoolean", true)));
   }
 
   @Test
@@ -114,8 +109,7 @@
         () -> assertEquals("", m_prefs.getString("checkedValueString", "")),
         () -> assertEquals(0, m_prefs.getInt("checkedValueInt", 0)),
         () -> assertEquals(0, m_prefs.getFloat("checkedValueFloat", 0), 1e-6),
-        () -> assertTrue(m_prefs.getBoolean("checkedValueBoolean", true))
-    );
+        () -> assertTrue(m_prefs.getBoolean("checkedValueBoolean", true)));
   }
 
   @Nested
@@ -129,8 +123,7 @@
 
       assertAll(
           () -> assertEquals(value, m_prefs.getInt(key, -1)),
-          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).intValue())
-      );
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).intValue()));
     }
 
     @Test
@@ -142,8 +135,7 @@
 
       assertAll(
           () -> assertEquals(value, m_prefs.getLong(key, -1)),
-          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).longValue())
-      );
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).longValue()));
     }
 
     @Test
@@ -155,8 +147,7 @@
 
       assertAll(
           () -> assertEquals(value, m_prefs.getFloat(key, -1), 1e-6),
-          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).floatValue(), 1e-6)
-      );
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).floatValue(), 1e-6));
     }
 
     @Test
@@ -168,8 +159,7 @@
 
       assertAll(
           () -> assertEquals(value, m_prefs.getDouble(key, -1), 1e-6),
-          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).doubleValue(), 1e-6)
-      );
+          () -> assertEquals(value, m_table.getEntry(key).getNumber(-1).doubleValue(), 1e-6));
     }
 
     @Test
@@ -181,8 +171,7 @@
 
       assertAll(
           () -> assertEquals(value, m_prefs.getString(key, "")),
-          () -> assertEquals(value, m_table.getEntry(key).getString(""))
-      );
+          () -> assertEquals(value, m_table.getEntry(key).getString("")));
     }
 
     @Test
@@ -194,8 +183,7 @@
 
       assertAll(
           () -> assertEquals(value, m_prefs.getBoolean(key, false)),
-          () -> assertEquals(value, m_table.getEntry(key).getBoolean(false))
-      );
+          () -> assertEquals(value, m_table.getEntry(key).getBoolean(false)));
     }
   }
 
@@ -206,7 +194,6 @@
         "checkedValueString",
         "checkedValueInt",
         "checkedValueFloat",
-        "checkedValueBoolean"
-    );
+        "checkedValueBoolean");
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
index 4bce18f..3c5647c 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/RobotControllerTest.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SensorUtilTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SensorUtilTest.java
new file mode 100644
index 0000000..a33716c
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SensorUtilTest.java
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import org.junit.jupiter.api.Test;
+
+public class SensorUtilTest {
+  @Test
+  void checkAnalogInputChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkAnalogInputChannel(100));
+  }
+
+  @Test
+  void checkAnalogOutputChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkAnalogOutputChannel(100));
+  }
+
+  @Test
+  void testInvalidDigitalChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkDigitalChannel(100));
+  }
+
+  @Test
+  void testInvalidPwmChannel() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkPWMChannel(100));
+  }
+
+  @Test
+  void testInvalidRelayModule() {
+    assertThrows(IllegalArgumentException.class, () -> SensorUtil.checkRelayChannel(100));
+  }
+
+  @Test
+  void testgetDefaultCtrePcmModule() {
+    assertEquals(0, SensorUtil.getDefaultCTREPCMModule());
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
deleted file mode 100644
index e47b70b..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SlewRateLimiterTest.java
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class SlewRateLimiterTest {
-  @Test
-  void slewRateLimitTest() {
-    SlewRateLimiter limiter = new SlewRateLimiter(1);
-    Timer.delay(1);
-    assertTrue(limiter.calculate(2) < 2);
-  }
-
-  @Test
-  void slewRateNoLimitTest() {
-    SlewRateLimiter limiter = new SlewRateLimiter(1);
-    Timer.delay(1);
-    assertEquals(limiter.calculate(0.5), 0.5);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestCTRE.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestCTRE.java
new file mode 100644
index 0000000..0077650
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestCTRE.java
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+public class SolenoidTestCTRE {
+  @Test
+  void testValidInitialization() {
+    try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) {
+      assertEquals(2, solenoid.getChannel());
+
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.set(false);
+      assertFalse(solenoid.get());
+    }
+  }
+
+  @Test
+  void testDoubleInitialization() {
+    try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) {
+      assertThrows(
+          AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.CTREPCM, 2));
+    }
+  }
+
+  @Test
+  void testDoubleInitializationFromDoubleSolenoid() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.CTREPCM, 2, 3)) {
+      assertThrows(
+          AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.CTREPCM, 2));
+    }
+  }
+
+  @Test
+  void testInvalidChannel() {
+    assertThrows(
+        IllegalArgumentException.class, () -> new Solenoid(3, PneumaticsModuleType.CTREPCM, 100));
+  }
+
+  @Test
+  void testToggle() {
+    try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.CTREPCM, 2)) {
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.toggle();
+      assertFalse(solenoid.get());
+
+      solenoid.toggle();
+      assertTrue(solenoid.get());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestREV.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestREV.java
new file mode 100644
index 0000000..3f07406
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SolenoidTestREV.java
@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.util.AllocationException;
+import org.junit.jupiter.api.Test;
+
+public class SolenoidTestREV {
+  @Test
+  void testValidInitialization() {
+    try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.REVPH, 2)) {
+      assertEquals(2, solenoid.getChannel());
+
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.set(false);
+      assertFalse(solenoid.get());
+    }
+  }
+
+  @Test
+  void testDoubleInitialization() {
+    try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.REVPH, 2)) {
+      assertThrows(AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.REVPH, 2));
+    }
+  }
+
+  @Test
+  void testDoubleInitializationFromDoubleSolenoid() {
+    try (DoubleSolenoid solenoid = new DoubleSolenoid(3, PneumaticsModuleType.REVPH, 2, 3)) {
+      assertThrows(AllocationException.class, () -> new Solenoid(3, PneumaticsModuleType.REVPH, 2));
+    }
+  }
+
+  @Test
+  void testInvalidChannel() {
+    assertThrows(
+        IllegalArgumentException.class, () -> new Solenoid(3, PneumaticsModuleType.REVPH, 100));
+  }
+
+  @Test
+  void testToggle() {
+    try (Solenoid solenoid = new Solenoid(3, PneumaticsModuleType.REVPH, 2)) {
+      solenoid.set(true);
+      assertTrue(solenoid.get());
+
+      solenoid.toggle();
+      assertFalse(solenoid.get());
+
+      solenoid.toggle();
+      assertTrue(solenoid.get());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java
deleted file mode 100644
index 62e2f46..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/SpeedControllerGroupTest.java
+++ /dev/null
@@ -1,109 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.Arrays;
-import java.util.stream.DoubleStream;
-import java.util.stream.IntStream;
-import java.util.stream.Stream;
-
-import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.Arguments;
-import org.junit.jupiter.params.provider.MethodSource;
-
-import static org.junit.jupiter.api.Assertions.assertArrayEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class SpeedControllerGroupTest {
-  private static Stream<Arguments> speedControllerArguments() {
-    return IntStream.of(1, 2, 3).mapToObj(number -> {
-      SpeedController[] speedControllers = Stream.generate(MockSpeedController::new)
-          .limit(number)
-          .toArray(SpeedController[]::new);
-      SpeedControllerGroup group = new SpeedControllerGroup(speedControllers[0],
-          Arrays.copyOfRange(speedControllers, 1, speedControllers.length));
-      return Arguments.of(group, speedControllers);
-    });
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void setTest(final SpeedControllerGroup group, final SpeedController[] speedControllers) {
-    group.set(1.0);
-
-    assertArrayEquals(DoubleStream.generate(() -> 1.0).limit(speedControllers.length).toArray(),
-        Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
-        0.00005);
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void getInvertedTest(final SpeedControllerGroup group,
-                       final SpeedController[] speedControllers) {
-    group.setInverted(true);
-
-    assertTrue(group.getInverted());
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void setInvertedDoesNotModifySpeedControllersTest(final SpeedControllerGroup group,
-                                                    final SpeedController[] speedControllers) {
-    group.setInverted(true);
-
-    assertArrayEquals(Stream.generate(() -> false).limit(speedControllers.length).toArray(),
-        Arrays.stream(speedControllers).map(SpeedController::getInverted).toArray());
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void setInvertedDoesInvertTest(final SpeedControllerGroup group,
-                                 final SpeedController[] speedControllers) {
-    group.setInverted(true);
-    group.set(1.0);
-
-    assertArrayEquals(DoubleStream.generate(() -> -1.0).limit(speedControllers.length).toArray(),
-        Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
-        0.00005);
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void disableTest(final SpeedControllerGroup group,
-                   final SpeedController[] speedControllers) {
-    group.set(1.0);
-    group.disable();
-
-    assertArrayEquals(DoubleStream.generate(() -> 0.0).limit(speedControllers.length).toArray(),
-        Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
-        0.00005);
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void stopMotorTest(final SpeedControllerGroup group,
-                     final SpeedController[] speedControllers) {
-    group.set(1.0);
-    group.stopMotor();
-
-    assertArrayEquals(DoubleStream.generate(() -> 0.0).limit(speedControllers.length).toArray(),
-        Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
-        0.00005);
-  }
-
-  @ParameterizedTest
-  @MethodSource("speedControllerArguments")
-  void pidWriteTest(final SpeedControllerGroup group,
-                    final SpeedController[] speedControllers) {
-    group.pidWrite(1.0);
-
-    assertArrayEquals(DoubleStream.generate(() -> 1.0).limit(speedControllers.length).toArray(),
-        Arrays.stream(speedControllers).mapToDouble(SpeedController::get).toArray(),
-        0.00005);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
index 6942ca0..de5fd1c 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimedRobotTest.java
@@ -1,26 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.concurrent.atomic.AtomicInteger;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
 import org.junit.jupiter.api.AfterEach;
 import org.junit.jupiter.api.BeforeEach;
 import org.junit.jupiter.api.Test;
 import org.junit.jupiter.api.parallel.ResourceLock;
 
-import edu.wpi.first.wpilibj.simulation.DriverStationSim;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 class TimedRobotTest {
-  class MockRobot extends TimedRobot {
+  static class MockRobot extends TimedRobot {
     public final AtomicInteger m_robotInitCount = new AtomicInteger(0);
     public final AtomicInteger m_simulationInitCount = new AtomicInteger(0);
     public final AtomicInteger m_disabledInitCount = new AtomicInteger(0);
@@ -35,6 +30,11 @@
     public final AtomicInteger m_teleopPeriodicCount = new AtomicInteger(0);
     public final AtomicInteger m_testPeriodicCount = new AtomicInteger(0);
 
+    public final AtomicInteger m_disabledExitCount = new AtomicInteger(0);
+    public final AtomicInteger m_autonomousExitCount = new AtomicInteger(0);
+    public final AtomicInteger m_teleopExitCount = new AtomicInteger(0);
+    public final AtomicInteger m_testExitCount = new AtomicInteger(0);
+
     @Override
     public void robotInit() {
       m_robotInitCount.addAndGet(1);
@@ -94,6 +94,26 @@
     public void testPeriodic() {
       m_testPeriodicCount.addAndGet(1);
     }
+
+    @Override
+    public void disabledExit() {
+      m_disabledExitCount.addAndGet(1);
+    }
+
+    @Override
+    public void autonomousExit() {
+      m_autonomousExitCount.addAndGet(1);
+    }
+
+    @Override
+    public void teleopExit() {
+      m_teleopExitCount.addAndGet(1);
+    }
+
+    @Override
+    public void testExit() {
+      m_testExitCount.addAndGet(1);
+    }
   }
 
   @BeforeEach
@@ -108,17 +128,19 @@
 
   @Test
   @ResourceLock("timing")
-  void disabledTest() {
+  void disabledModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread = new Thread(() -> {
-      robot.startCompetition();
-    });
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
     DriverStationSim.notifyNewData();
-    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -134,6 +156,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -150,6 +177,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -166,6 +198,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     robot.endCompetition();
     try {
       robotThread.interrupt();
@@ -178,19 +215,21 @@
 
   @Test
   @ResourceLock("timing")
-  void autonomousTest() {
+  void autonomousModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread = new Thread(() -> {
-      robot.startCompetition();
-    });
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
     robotThread.start();
 
     DriverStationSim.setEnabled(true);
     DriverStationSim.setAutonomous(true);
     DriverStationSim.setTest(false);
     DriverStationSim.notifyNewData();
-    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -206,6 +245,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -222,6 +266,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -238,6 +287,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     robot.endCompetition();
     try {
       robotThread.interrupt();
@@ -250,19 +304,21 @@
 
   @Test
   @ResourceLock("timing")
-  void teleopTest() {
+  void teleopModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread = new Thread(() -> {
-      robot.startCompetition();
-    });
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
     robotThread.start();
 
     DriverStationSim.setEnabled(true);
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(false);
     DriverStationSim.notifyNewData();
-    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -278,6 +334,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -294,6 +355,11 @@
     assertEquals(1, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -310,6 +376,11 @@
     assertEquals(2, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     robot.endCompetition();
     try {
       robotThread.interrupt();
@@ -322,19 +393,21 @@
 
   @Test
   @ResourceLock("timing")
-  void testTest() {
+  void testModeTest() {
     MockRobot robot = new MockRobot();
 
-    Thread robotThread = new Thread(() -> {
-      robot.startCompetition();
-    });
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
     robotThread.start();
 
     DriverStationSim.setEnabled(true);
     DriverStationSim.setAutonomous(false);
     DriverStationSim.setTest(true);
     DriverStationSim.notifyNewData();
-    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
 
     assertEquals(1, robot.m_robotInitCount.get());
     assertEquals(1, robot.m_simulationInitCount.get());
@@ -350,6 +423,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(0, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -366,6 +444,11 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(1, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
     SimHooks.stepTiming(0.02);
 
     assertEquals(1, robot.m_robotInitCount.get());
@@ -382,6 +465,134 @@
     assertEquals(0, robot.m_teleopPeriodicCount.get());
     assertEquals(2, robot.m_testPeriodicCount.get());
 
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void modeChangeTest() {
+    MockRobot robot = new MockRobot();
+
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
+    robotThread.start();
+
+    // Start in disabled
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    assertEquals(0, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(0, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(0, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to autonomous
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(true);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(0, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(0, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to teleop
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(0, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(1, robot.m_autonomousExitCount.get());
+    assertEquals(0, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to test
+    DriverStationSim.setEnabled(true);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(true);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(1, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(1, robot.m_autonomousExitCount.get());
+    assertEquals(1, robot.m_teleopExitCount.get());
+    assertEquals(0, robot.m_testExitCount.get());
+
+    // Transition to disabled
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.setAutonomous(false);
+    DriverStationSim.setTest(false);
+    DriverStationSim.notifyNewData();
+
+    SimHooks.stepTiming(0.02);
+
+    assertEquals(2, robot.m_disabledInitCount.get());
+    assertEquals(1, robot.m_autonomousInitCount.get());
+    assertEquals(1, robot.m_teleopInitCount.get());
+    assertEquals(1, robot.m_testInitCount.get());
+
+    assertEquals(1, robot.m_disabledExitCount.get());
+    assertEquals(1, robot.m_autonomousExitCount.get());
+    assertEquals(1, robot.m_teleopExitCount.get());
+    assertEquals(1, robot.m_testExitCount.get());
+
     robot.endCompetition();
     try {
       robotThread.interrupt();
@@ -398,18 +609,22 @@
     MockRobot robot = new MockRobot();
 
     final AtomicInteger callbackCount = new AtomicInteger(0);
-    robot.addPeriodic(() -> {
-      callbackCount.addAndGet(1);
-    }, 0.01);
+    robot.addPeriodic(
+        () -> {
+          callbackCount.addAndGet(1);
+        },
+        0.01);
 
-    Thread robotThread = new Thread(() -> {
-      robot.startCompetition();
-    });
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
     DriverStationSim.notifyNewData();
-    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
 
     assertEquals(0, robot.m_disabledInitCount.get());
     assertEquals(0, robot.m_disabledPeriodicCount.get());
@@ -443,9 +658,12 @@
     MockRobot robot = new MockRobot();
 
     final AtomicInteger callbackCount = new AtomicInteger(0);
-    robot.addPeriodic(() -> {
-      callbackCount.addAndGet(1);
-    }, 0.01, 0.005);
+    robot.addPeriodic(
+        () -> {
+          callbackCount.addAndGet(1);
+        },
+        0.01,
+        0.005);
 
     // Expirations in this test (ms)
     //
@@ -454,14 +672,16 @@
     //    20 |      15
     //    40 |      25
 
-    Thread robotThread = new Thread(() -> {
-      robot.startCompetition();
-    });
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
     robotThread.start();
 
     DriverStationSim.setEnabled(false);
     DriverStationSim.notifyNewData();
-    SimHooks.stepTiming(0.0);  // Wait for Notifiers
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
 
     assertEquals(0, robot.m_disabledInitCount.get());
     assertEquals(0, robot.m_disabledPeriodicCount.get());
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimerTest.java
new file mode 100644
index 0000000..89619dd
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimerTest.java
@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+class TimerTest {
+  @BeforeEach
+  void setup() {
+    HAL.initialize(500, 0);
+    SimHooks.pauseTiming();
+    SimHooks.restartTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void startStopTest() {
+    var timer = new Timer();
+
+    // Verify timer is initialized as stopped
+    assertEquals(timer.get(), 0.0);
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.0);
+
+    // Verify timer increments after it's started
+    timer.start();
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+
+    // Verify timer stops incrementing after it's stopped
+    timer.stop();
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void resetTest() {
+    var timer = new Timer();
+    timer.start();
+
+    // Advance timer to 500 ms
+    assertEquals(timer.get(), 0.0);
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+
+    // Verify timer reports 0 ms after reset
+    timer.reset();
+    assertEquals(timer.get(), 0.0);
+
+    // Verify timer continues incrementing
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.5, 1e-9);
+
+    // Verify timer doesn't start incrementing after reset if it was stopped
+    timer.stop();
+    timer.reset();
+    SimHooks.stepTiming(0.5);
+    assertEquals(timer.get(), 0.0);
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void hasElapsedTest() {
+    var timer = new Timer();
+
+    // Verify 0 ms has elapsed since timer hasn't started
+    assertTrue(timer.hasElapsed(0.0));
+
+    // Verify timer doesn't report elapsed time when stopped
+    SimHooks.stepTiming(0.5);
+    assertFalse(timer.hasElapsed(0.4));
+
+    timer.start();
+
+    // Verify timer reports >= 400 ms has elapsed after multiple calls
+    SimHooks.stepTiming(0.5);
+    assertTrue(timer.hasElapsed(0.4));
+    assertTrue(timer.hasElapsed(0.4));
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void advanceIfElapsedTest() {
+    var timer = new Timer();
+
+    // Verify 0 ms has elapsed since timer hasn't started
+    assertTrue(timer.advanceIfElapsed(0.0));
+
+    // Verify timer doesn't report elapsed time when stopped
+    SimHooks.stepTiming(0.5);
+    assertFalse(timer.advanceIfElapsed(0.4));
+
+    timer.start();
+
+    // Verify timer reports >= 400 ms has elapsed for only first call
+    SimHooks.stepTiming(0.5);
+    assertTrue(timer.advanceIfElapsed(0.4));
+    assertFalse(timer.advanceIfElapsed(0.4));
+
+    // Verify timer reports >= 400 ms has elapsed for two calls
+    SimHooks.stepTiming(1.0);
+    assertTrue(timer.advanceIfElapsed(0.4));
+    assertTrue(timer.advanceIfElapsed(0.4));
+    assertFalse(timer.advanceIfElapsed(0.4));
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void getFPGATimestampTest() {
+    double start = Timer.getFPGATimestamp();
+    SimHooks.stepTiming(0.5);
+    double end = Timer.getFPGATimestamp();
+    assertEquals(start + 0.5, end, 1e-9);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java
new file mode 100644
index 0000000..ee4860d
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/TimesliceRobotTest.java
@@ -0,0 +1,131 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+
+import edu.wpi.first.wpilibj.simulation.DriverStationSim;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
+class TimesliceRobotTest {
+  static class MockRobot extends TimesliceRobot {
+    public final AtomicInteger m_robotPeriodicCount = new AtomicInteger(0);
+
+    MockRobot() {
+      super(0.002, 0.005);
+    }
+
+    @Override
+    public void robotPeriodic() {
+      m_robotPeriodicCount.addAndGet(1);
+    }
+  }
+
+  @BeforeEach
+  void setup() {
+    SimHooks.pauseTiming();
+  }
+
+  @AfterEach
+  void cleanup() {
+    SimHooks.resumeTiming();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void scheduleTest() {
+    MockRobot robot = new MockRobot();
+
+    final AtomicInteger callbackCount1 = new AtomicInteger(0);
+    final AtomicInteger callbackCount2 = new AtomicInteger(0);
+
+    // Timeslice allocation table
+    //
+    // |       Name      | Offset (ms) | Allocation (ms)|
+    // |-----------------|-------------|----------------|
+    // | RobotPeriodic() |           0 |              2 |
+    // | Callback 1      |           2 |            0.5 |
+    // | Callback 2      |         2.5 |              1 |
+    robot.schedule(
+        () -> {
+          callbackCount1.addAndGet(1);
+        },
+        0.0005);
+    robot.schedule(
+        () -> {
+          callbackCount2.addAndGet(1);
+        },
+        0.001);
+
+    Thread robotThread =
+        new Thread(
+            () -> {
+              robot.startCompetition();
+            });
+    robotThread.start();
+
+    DriverStationSim.setEnabled(false);
+    DriverStationSim.notifyNewData();
+    SimHooks.stepTiming(0.0); // Wait for Notifiers
+
+    // Functions scheduled with addPeriodic() are delayed by one period before
+    // their first run (5 ms for this test's callbacks here and 20 ms for
+    // robotPeriodic()).
+    SimHooks.stepTiming(0.005);
+
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, callbackCount1.get());
+    assertEquals(0, callbackCount2.get());
+
+    // Step to 1.5 ms
+    SimHooks.stepTiming(0.0015);
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(0, callbackCount1.get());
+    assertEquals(0, callbackCount2.get());
+
+    // Step to 2.25 ms
+    SimHooks.stepTiming(0.00075);
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(1, callbackCount1.get());
+    assertEquals(0, callbackCount2.get());
+
+    // Step to 2.75 ms
+    SimHooks.stepTiming(0.0005);
+    assertEquals(0, robot.m_robotPeriodicCount.get());
+    assertEquals(1, callbackCount1.get());
+    assertEquals(1, callbackCount2.get());
+
+    robot.endCompetition();
+    try {
+      robotThread.interrupt();
+      robotThread.join();
+    } catch (InterruptedException ex) {
+      Thread.currentThread().interrupt();
+    }
+    robot.close();
+  }
+
+  @Test
+  @ResourceLock("timing")
+  void scheduleOverrunTest() {
+    MockRobot robot = new MockRobot();
+
+    robot.schedule(() -> {}, 0.0005);
+    robot.schedule(() -> {}, 0.001);
+
+    // offset = 2 ms + 0.5 ms + 1 ms = 3.5 ms
+    // 3.5 ms + 3 ms allocation = 6.5 ms > max of 5 ms
+    assertThrows(IllegalStateException.class, () -> robot.schedule(() -> {}, 0.003));
+
+    robot.endCompetition();
+    robot.close();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java
new file mode 100644
index 0000000..1b50572
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UltrasonicTest.java
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.wpilibj.simulation.UltrasonicSim;
+import org.junit.jupiter.api.Test;
+
+public class UltrasonicTest {
+  @Test
+  public void testUltrasonic() {
+    try (Ultrasonic ultrasonic = new Ultrasonic(0, 1)) {
+      UltrasonicSim sim = new UltrasonicSim(ultrasonic);
+
+      assertEquals(0, ultrasonic.getRangeInches());
+      assertTrue(ultrasonic.isRangeValid());
+
+      sim.setRangeInches(35.04);
+      assertEquals(35.04, ultrasonic.getRangeInches());
+
+      sim.setRangeValid(false);
+      assertFalse(ultrasonic.isRangeValid());
+      assertEquals(0, ultrasonic.getRangeInches());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
index a66cd13..8eed93f 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -1,28 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.lang.reflect.Constructor;
-import java.lang.reflect.InvocationTargetException;
-import java.lang.reflect.Modifier;
-import java.util.Arrays;
-import java.util.stream.Stream;
-
-import org.junit.jupiter.api.DynamicTest;
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.TestFactory;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.DynamicTest.dynamicTest;
 
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+// Declaring this class abstract prevents UtilityClassTest from running on itself and throwing the
+// following exception:
+//
+// org.junit.jupiter.api.extension.ParameterResolutionException: No ParameterResolver registered
+// for parameter [java.lang.Class<T> arg0] in constructor [protected
+// edu.wpi.first.wpilibj.UtilityClassTest(java.lang.Class<T>)].
 @SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
 public abstract class UtilityClassTest<T> {
   private final Class<T> m_clazz;
@@ -33,8 +35,7 @@
 
   @Test
   public void singleConstructorTest() {
-    assertEquals(1, m_clazz.getDeclaredConstructors().length,
-        "More than one constructor defined");
+    assertEquals(1, m_clazz.getDeclaredConstructors().length, "More than one constructor defined");
   }
 
   @Test
@@ -55,7 +56,9 @@
   Stream<DynamicTest> publicMethodsStaticTestFactory() {
     return Arrays.stream(m_clazz.getDeclaredMethods())
         .filter(method -> Modifier.isPublic(method.getModifiers()))
-        .map(method -> dynamicTest(method.getName(),
-            () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+        .map(
+            method ->
+                dynamicTest(
+                    method.getName(), () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
index d99d7db..c1676e0 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/WatchdogTest.java
@@ -1,26 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.concurrent.atomic.AtomicInteger;
-
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.parallel.ResourceLock;
-
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.SimHooks;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.SimHooks;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.parallel.ResourceLock;
+
 class WatchdogTest {
   @BeforeEach
   void setup() {
@@ -38,9 +33,12 @@
   void enableDisableTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(0.4, () -> {
-      watchdogCounter.addAndGet(1);
-    })) {
+    try (Watchdog watchdog =
+        new Watchdog(
+            0.4,
+            () -> {
+              watchdogCounter.addAndGet(1);
+            })) {
       // Run 1
       watchdog.enable();
       SimHooks.stepTiming(0.2);
@@ -54,8 +52,8 @@
       SimHooks.stepTiming(0.4);
       watchdog.disable();
 
-      assertEquals(1, watchdogCounter.get(),
-          "Watchdog either didn't trigger or triggered more than once");
+      assertEquals(
+          1, watchdogCounter.get(), "Watchdog either didn't trigger or triggered more than once");
 
       // Run 3
       watchdogCounter.set(0);
@@ -63,8 +61,8 @@
       SimHooks.stepTiming(1.0);
       watchdog.disable();
 
-      assertEquals(1, watchdogCounter.get(),
-          "Watchdog either didn't trigger or triggered more than once");
+      assertEquals(
+          1, watchdogCounter.get(), "Watchdog either didn't trigger or triggered more than once");
     }
   }
 
@@ -73,9 +71,12 @@
   void resetTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(0.4, () -> {
-      watchdogCounter.addAndGet(1);
-    })) {
+    try (Watchdog watchdog =
+        new Watchdog(
+            0.4,
+            () -> {
+              watchdogCounter.addAndGet(1);
+            })) {
       watchdog.enable();
       SimHooks.stepTiming(0.2);
       watchdog.reset();
@@ -91,9 +92,12 @@
   void setTimeoutTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(1.0, () -> {
-      watchdogCounter.addAndGet(1);
-    })) {
+    try (Watchdog watchdog =
+        new Watchdog(
+            1.0,
+            () -> {
+              watchdogCounter.addAndGet(1);
+            })) {
       watchdog.enable();
       SimHooks.stepTiming(0.2);
       watchdog.setTimeout(0.2);
@@ -104,16 +108,15 @@
       SimHooks.stepTiming(0.3);
       watchdog.disable();
 
-      assertEquals(1, watchdogCounter.get(),
-          "Watchdog either didn't trigger or triggered more than once");
+      assertEquals(
+          1, watchdogCounter.get(), "Watchdog either didn't trigger or triggered more than once");
     }
   }
 
   @Test
   @ResourceLock("timing")
   void isExpiredTest() {
-    try (Watchdog watchdog = new Watchdog(0.2, () -> {
-    })) {
+    try (Watchdog watchdog = new Watchdog(0.2, () -> {})) {
       assertFalse(watchdog.isExpired());
       watchdog.enable();
 
@@ -134,9 +137,12 @@
   void epochsTest() {
     final AtomicInteger watchdogCounter = new AtomicInteger(0);
 
-    try (Watchdog watchdog = new Watchdog(0.4, () -> {
-      watchdogCounter.addAndGet(1);
-    })) {
+    try (Watchdog watchdog =
+        new Watchdog(
+            0.4,
+            () -> {
+              watchdogCounter.addAndGet(1);
+            })) {
       // Run 1
       watchdog.enable();
       watchdog.addEpoch("Epoch 1");
@@ -167,12 +173,18 @@
     final AtomicInteger watchdogCounter1 = new AtomicInteger(0);
     final AtomicInteger watchdogCounter2 = new AtomicInteger(0);
 
-    try (Watchdog watchdog1 = new Watchdog(0.2, () -> {
-      watchdogCounter1.addAndGet(1);
-    });
-        Watchdog watchdog2 = new Watchdog(0.6, () -> {
-          watchdogCounter2.addAndGet(1);
-        })) {
+    try (Watchdog watchdog1 =
+            new Watchdog(
+                0.2,
+                () -> {
+                  watchdogCounter1.addAndGet(1);
+                });
+        Watchdog watchdog2 =
+            new Watchdog(
+                0.6,
+                () -> {
+                  watchdogCounter2.addAndGet(1);
+                })) {
       watchdog2.enable();
       SimHooks.stepTiming(0.25);
       assertEquals(0, watchdogCounter1.get(), "Watchdog triggered early");
@@ -184,8 +196,8 @@
       watchdog1.disable();
       watchdog2.disable();
 
-      assertEquals(1, watchdogCounter1.get(),
-          "Watchdog either didn't trigger or triggered more than once");
+      assertEquals(
+          1, watchdogCounter1.get(), "Watchdog either didn't trigger or triggered more than once");
       assertEquals(0, watchdogCounter2.get(), "Watchdog triggered early");
     }
   }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java
index 34fce9a..d472377 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/XboxControllerTest.java
@@ -1,86 +1,81 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.hal.HAL;
-import edu.wpi.first.wpilibj.simulation.XboxControllerSim;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.simulation.XboxControllerSim;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
 class XboxControllerTest {
-  @Test
-  void testGetX() {
+  @ParameterizedTest
+  @EnumSource(value = XboxController.Button.class)
+  @SuppressWarnings({"VariableDeclarationUsageDistance"})
+  public void testButtons(XboxController.Button button)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
     HAL.initialize(500, 0);
     XboxController joy = new XboxController(2);
     XboxControllerSim joysim = new XboxControllerSim(joy);
 
-    joysim.setX(XboxController.Hand.kLeft, 0.35);
-    joysim.setX(XboxController.Hand.kRight, 0.45);
+    var buttonName = button.toString();
+
+    String simSetMethodName = "set" + buttonName;
+    String joyGetMethodName = "get" + buttonName;
+    String joyPressedMethodName = "get" + buttonName + "Pressed";
+    String joyReleasedMethodName = "get" + buttonName + "Released";
+
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, boolean.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
+    Method joyPressedMethod = joy.getClass().getMethod(joyPressedMethodName);
+    Method joyReleasedMethod = joy.getClass().getMethod(joyReleasedMethodName);
+
+    simSetMethod.invoke(joysim, false);
     joysim.notifyNewData();
-    assertEquals(0.35, joy.getX(XboxController.Hand.kLeft), 0.001);
-    assertEquals(0.45, joy.getX(XboxController.Hand.kRight), 0.001);
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    // need to call pressed and released to clear flags
+    joyPressedMethod.invoke(joy);
+    joyReleasedMethod.invoke(joy);
+
+    simSetMethod.invoke(joysim, true);
+    joysim.notifyNewData();
+    assertTrue((Boolean) joyGetMethod.invoke(joy));
+    assertTrue((Boolean) joyPressedMethod.invoke(joy));
+    assertFalse((Boolean) joyReleasedMethod.invoke(joy));
+
+    simSetMethod.invoke(joysim, false);
+    joysim.notifyNewData();
+    assertFalse((Boolean) joyGetMethod.invoke(joy));
+    assertFalse((Boolean) joyPressedMethod.invoke(joy));
+    assertTrue((Boolean) joyReleasedMethod.invoke(joy));
   }
 
-  @Test
-  void testGetBumper() {
+  @ParameterizedTest
+  @EnumSource(value = XboxController.Axis.class)
+  @SuppressWarnings({"VariableDeclarationUsageDistance"})
+  public void testAxes(XboxController.Axis axis)
+      throws NoSuchMethodException, InvocationTargetException, IllegalAccessException {
     HAL.initialize(500, 0);
     XboxController joy = new XboxController(2);
     XboxControllerSim joysim = new XboxControllerSim(joy);
 
-    joysim.setBumper(XboxController.Hand.kLeft, false);
-    joysim.setBumper(XboxController.Hand.kRight, true);
-    joysim.notifyNewData();
-    assertFalse(joy.getBumper(XboxController.Hand.kLeft));
-    assertTrue(joy.getBumper(XboxController.Hand.kRight));
-    // need to call pressed and released to clear flags
-    joy.getBumperPressed(XboxController.Hand.kLeft);
-    joy.getBumperReleased(XboxController.Hand.kLeft);
-    joy.getBumperPressed(XboxController.Hand.kRight);
-    joy.getBumperReleased(XboxController.Hand.kRight);
+    var axisName = axis.toString();
 
-    joysim.setBumper(XboxController.Hand.kLeft, true);
-    joysim.setBumper(XboxController.Hand.kRight, false);
-    joysim.notifyNewData();
-    assertTrue(joy.getBumper(XboxController.Hand.kLeft));
-    assertTrue(joy.getBumperPressed(XboxController.Hand.kLeft));
-    assertFalse(joy.getBumperReleased(XboxController.Hand.kLeft));
-    assertFalse(joy.getBumper(XboxController.Hand.kRight));
-    assertFalse(joy.getBumperPressed(XboxController.Hand.kRight));
-    assertTrue(joy.getBumperReleased(XboxController.Hand.kRight));
-  }
+    String simSetMethodName = "set" + axisName;
+    String joyGetMethodName = "get" + axisName;
 
-  @Test
-  void testGetAButton() {
-    HAL.initialize(500, 0);
-    XboxController joy = new XboxController(2);
-    XboxControllerSim joysim = new XboxControllerSim(joy);
+    Method simSetMethod = joysim.getClass().getMethod(simSetMethodName, double.class);
+    Method joyGetMethod = joy.getClass().getMethod(joyGetMethodName);
 
-    joysim.setAButton(false);
+    simSetMethod.invoke(joysim, 0.35);
     joysim.notifyNewData();
-    assertFalse(joy.getAButton());
-    // need to call pressed and released to clear flags
-    joy.getAButtonPressed();
-    joy.getAButtonReleased();
-
-    joysim.setAButton(true);
-    joysim.notifyNewData();
-    assertTrue(joy.getAButton());
-    assertTrue(joy.getAButtonPressed());
-    assertFalse(joy.getAButtonReleased());
-
-    joysim.setAButton(false);
-    joysim.notifyNewData();
-    assertFalse(joy.getAButton());
-    assertFalse(joy.getAButtonPressed());
-    assertTrue(joy.getAButtonReleased());
+    assertEquals(0.35, (Double) joyGetMethod.invoke(joy), 0.001);
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
index e804d0c..71104fc 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
@@ -1,25 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.can;
 
-import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.can.CANJNI;
 import edu.wpi.first.hal.can.CANStatus;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import org.junit.jupiter.api.Test;
 
 class CANStatusTest {
   @Test
   void canStatusGetDoesntThrow() {
     HAL.initialize(500, 0);
     CANStatus status = new CANStatus();
-    assertDoesNotThrow(() -> CANJNI.GetCANStatus(status));
+    assertDoesNotThrow(() -> CANJNI.getCANStatus(status));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
deleted file mode 100644
index 307eccd..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ControllerUtilTest.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class ControllerUtilTest {
-  @Test
-  void testGetModulusError() {
-    // Test symmetric range
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -180.0, 180.0));
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
-    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0, -180.0, 180.0));
-    assertEquals(20.0, ControllerUtil.getModulusError(-170.0 + 360.0, 170.0, -180.0, 180.0));
-    assertEquals(20.0, ControllerUtil.getModulusError(-170.0, 170.0 + 360.0, -180.0, 180.0));
-
-    // Test range start at zero
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0, 0.0, 360.0));
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, 190.0 + 360, 0.0, 360.0));
-
-    // Test asymmetric range that doesn't start at zero
-    assertEquals(-20.0, ControllerUtil.getModulusError(170.0, -170.0, -170.0, 190.0));
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
deleted file mode 100644
index 2bf7900..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/HolonomicDriveControllerTest.java
+++ /dev/null
@@ -1,78 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpiutil.math.MathUtil;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class HolonomicDriveControllerTest {
-  private static final double kTolerance = 1 / 12.0;
-  private static final double kAngularTolerance = Math.toRadians(2);
-
-  @Test
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  void testReachesReference() {
-    HolonomicDriveController controller = new HolonomicDriveController(
-        new PIDController(1.0, 0.0, 0.0),
-        new PIDController(1.0, 0.0, 0.0),
-        new ProfiledPIDController(1.0, 0.0, 0.0,
-            new TrapezoidProfile.Constraints(6.28, 3.14))
-    );
-    Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
-
-    List<Pose2d> waypoints = new ArrayList<>();
-    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
-    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
-
-    TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
-    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
-
-    final double kDt = 0.02;
-    final double kTotalTime = trajectory.getTotalTimeSeconds();
-
-    for (int i = 0; i < (kTotalTime / kDt); i++) {
-      Trajectory.State state = trajectory.sample(kDt * i);
-      ChassisSpeeds output = controller.calculate(robotPose, state, new Rotation2d());
-
-      robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt,
-          output.vyMetersPerSecond * kDt, output.omegaRadiansPerSecond * kDt));
-    }
-
-    final List<Trajectory.State> states = trajectory.getStates();
-    final Pose2d endPose = states.get(states.size() - 1).poseMeters;
-
-    // Java lambdas require local variables referenced from a lambda expression
-    // must be final or effectively final.
-    final Pose2d finalRobotPose = robotPose;
-
-    assertAll(
-        () -> assertEquals(endPose.getX(), finalRobotPose.getX(),
-            kTolerance),
-        () -> assertEquals(endPose.getY(), finalRobotPose.getY(),
-            kTolerance),
-        () -> assertEquals(0.0,
-            MathUtil.normalizeAngle(finalRobotPose.getRotation().getRadians()),
-            kAngularTolerance)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
deleted file mode 100644
index 873d03f..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDInputOutputTest.java
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class PIDInputOutputTest {
-  private PIDController m_controller;
-
-  @BeforeEach
-  void setUp() {
-    m_controller = new PIDController(0, 0, 0);
-  }
-
-  @Test
-  void continuousInputTest() {
-    m_controller.setP(1);
-    m_controller.enableContinuousInput(-180, 180);
-
-    assertTrue(m_controller.calculate(-179, 179) < 0.0);
-  }
-
-  @Test
-  void proportionalGainOutputTest() {
-    m_controller.setP(4);
-
-    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
-  }
-
-  @Test
-  void integralGainOutputTest() {
-    m_controller.setI(4);
-
-    double out = 0;
-
-    for (int i = 0; i < 5; i++) {
-      out = m_controller.calculate(0.025, 0);
-    }
-
-    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
-  }
-
-  @Test
-  void derivativeGainOutputTest() {
-    m_controller.setD(4);
-
-    m_controller.calculate(0, 0);
-
-    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
deleted file mode 100644
index b2d43c6..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/PIDToleranceTest.java
+++ /dev/null
@@ -1,56 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class PIDToleranceTest {
-  private static final double kSetpoint = 50.0;
-  private static final double kTolerance = 10.0;
-  private static final double kRange = 200;
-
-  @Test
-  void initialToleranceTest() {
-    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
-      controller.enableContinuousInput(-kRange / 2, kRange / 2);
-
-      assertTrue(controller.atSetpoint());
-    }
-  }
-
-  @Test
-  void absoluteToleranceTest() {
-    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
-      controller.enableContinuousInput(-kRange / 2, kRange / 2);
-
-      controller.setTolerance(kTolerance);
-      controller.setSetpoint(kSetpoint);
-
-      controller.calculate(0.0);
-
-      assertFalse(controller.atSetpoint(),
-          "Error was in tolerance when it should not have been. Error was "
-          + controller.getPositionError());
-
-      controller.calculate(kSetpoint + kTolerance / 2);
-
-      assertTrue(controller.atSetpoint(),
-          "Error was not in tolerance when it should have been. Error was "
-          + controller.getPositionError());
-
-      controller.calculate(kSetpoint + 10 * kTolerance);
-
-      assertFalse(controller.atSetpoint(),
-          "Error was in tolerance when it should not have been. Error was "
-          + controller.getPositionError());
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
deleted file mode 100644
index 6babcc2..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDControllerTest.java
+++ /dev/null
@@ -1,26 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class ProfiledPIDControllerTest {
-  @Test
-  void testStartFromNonZeroPosition() {
-    ProfiledPIDController controller = new ProfiledPIDController(1.0, 0.0, 0.0,
-        new TrapezoidProfile.Constraints(1.0, 1.0));
-
-    controller.reset(20);
-
-    assertEquals(0.0, controller.calculate(20), 0.05);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
deleted file mode 100644
index 9fca2a4..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/ProfiledPIDInputOutputTest.java
+++ /dev/null
@@ -1,103 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class ProfiledPIDInputOutputTest {
-  private ProfiledPIDController m_controller;
-
-  @BeforeEach
-  void setUp() {
-    m_controller = new ProfiledPIDController(0, 0, 0,
-        new TrapezoidProfile.Constraints(360, 180));
-  }
-
-  @Test
-  void continuousInputTest1() {
-    m_controller.setP(1);
-    m_controller.enableContinuousInput(-180, 180);
-
-    final double kSetpoint = -179.0;
-    final double kMeasurement = -179.0;
-    final double kGoal = 179.0;
-
-    m_controller.reset(kSetpoint);
-    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
-
-    // Error must be less than half the input range at all times
-    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < 180.0);
-  }
-
-  @Test
-  void continuousInputTest2() {
-    m_controller.setP(1);
-    m_controller.enableContinuousInput(-Math.PI, Math.PI);
-
-    final double kSetpoint = -3.4826633343199735;
-    final double kMeasurement = -3.1352207333939606;
-    final double kGoal = -3.534162788601621;
-
-    m_controller.reset(kSetpoint);
-    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
-
-    // Error must be less than half the input range at all times
-    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
-  }
-
-  @Test
-  void continuousInputTest3() {
-    m_controller.setP(1);
-    m_controller.enableContinuousInput(-Math.PI, Math.PI);
-
-    final double kSetpoint = -3.5176604690006377;
-    final double kMeasurement = 3.1191729343822456;
-    final double kGoal = 2.709680418117445;
-
-    m_controller.reset(kSetpoint);
-    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
-
-    // Error must be less than half the input range at all times
-    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
-  }
-
-  @Test
-  void proportionalGainOutputTest() {
-    m_controller.setP(4);
-
-    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
-  }
-
-  @Test
-  void integralGainOutputTest() {
-    m_controller.setI(4);
-
-    double out = 0;
-
-    for (int i = 0; i < 5; i++) {
-      out = m_controller.calculate(0.025, 0);
-    }
-
-    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
-  }
-
-  @Test
-  void derivativeGainOutputTest() {
-    m_controller.setD(4);
-
-    m_controller.calculate(0, 0);
-
-    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
deleted file mode 100644
index a2b6e22..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/controller/RamseteControllerTest.java
+++ /dev/null
@@ -1,67 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import java.util.ArrayList;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpiutil.math.MathUtil;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class RamseteControllerTest {
-  private static final double kTolerance = 1 / 12.0;
-  private static final double kAngularTolerance = Math.toRadians(2);
-
-  @Test
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  void testReachesReference() {
-    final var controller = new RamseteController(2.0, 0.7);
-    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
-
-    final var waypoints = new ArrayList<Pose2d>();
-    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
-    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
-    var config = new TrajectoryConfig(8.8, 0.1);
-    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
-
-    final double kDt = 0.02;
-    final var totalTime = trajectory.getTotalTimeSeconds();
-    for (int i = 0; i < (totalTime / kDt); ++i) {
-      var state = trajectory.sample(kDt * i);
-
-      var output = controller.calculate(robotPose, state);
-      robotPose = robotPose.exp(new Twist2d(output.vxMetersPerSecond * kDt, 0,
-          output.omegaRadiansPerSecond * kDt));
-    }
-
-    final var states = trajectory.getStates();
-    final var endPose = states.get(states.size() - 1).poseMeters;
-
-    // Java lambdas require local variables referenced from a lambda expression
-    // must be final or effectively final.
-    final var finalRobotPose = robotPose;
-    assertAll(
-        () -> assertEquals(endPose.getX(), finalRobotPose.getX(),
-            kTolerance),
-        () -> assertEquals(endPose.getY(), finalRobotPose.getY(),
-            kTolerance),
-        () -> assertEquals(0.0,
-            MathUtil.normalizeAngle(endPose.getRotation().getRadians()
-                - finalRobotPose.getRotation().getRadians()),
-            kAngularTolerance)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java
new file mode 100644
index 0000000..e7f2e31
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DifferentialDriveTest.java
@@ -0,0 +1,240 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.drive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveTest {
+  @Test
+  void testArcadeDrive() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var drive = new DifferentialDrive(left, right);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.arcadeDrive(1.0, 0.0, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.arcadeDrive(0.5, -0.5, false);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(0.5, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.arcadeDrive(0.5, 0.5, false);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward
+    drive.arcadeDrive(-1.0, 0.0, false);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.arcadeDrive(-0.5, -0.5, false);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.arcadeDrive(-0.5, 0.5, false);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(-0.5, right.get(), 1e-9);
+  }
+
+  @Test
+  void testArcadeDriveSquared() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var drive = new DifferentialDrive(left, right);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.arcadeDrive(1.0, 0.0, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.arcadeDrive(0.5, -0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(0.25, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.arcadeDrive(0.5, 0.5, true);
+    assertEquals(0.25, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward
+    drive.arcadeDrive(-1.0, 0.0, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.arcadeDrive(-0.5, -0.5, true);
+    assertEquals(-0.25, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.arcadeDrive(-0.5, 0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(-0.25, right.get(), 1e-9);
+  }
+
+  @Test
+  void testCurvatureDrive() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var drive = new DifferentialDrive(left, right);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.curvatureDrive(1.0, 0.0, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.curvatureDrive(0.5, -0.5, false);
+    assertEquals(0.25, left.get(), 1e-9);
+    assertEquals(0.75, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.curvatureDrive(0.5, 0.5, false);
+    assertEquals(0.75, left.get(), 1e-9);
+    assertEquals(0.25, right.get(), 1e-9);
+
+    // Backward
+    drive.curvatureDrive(-1.0, 0.0, false);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.curvatureDrive(-0.5, -0.5, false);
+    assertEquals(-0.75, left.get(), 1e-9);
+    assertEquals(-0.25, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.curvatureDrive(-0.5, 0.5, false);
+    assertEquals(-0.25, left.get(), 1e-9);
+    assertEquals(-0.75, right.get(), 1e-9);
+  }
+
+  @Test
+  void testCurvatureDriveTurnInPlace() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var drive = new DifferentialDrive(left, right);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.curvatureDrive(1.0, 0.0, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.curvatureDrive(0.5, -0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.curvatureDrive(0.5, 0.5, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward
+    drive.curvatureDrive(-1.0, 0.0, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.curvatureDrive(-0.5, -0.5, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(0.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.curvatureDrive(-0.5, 0.5, true);
+    assertEquals(0.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+  }
+
+  @Test
+  void testTankDrive() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var drive = new DifferentialDrive(left, right);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.tankDrive(1.0, 1.0, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.tankDrive(0.5, 1.0, false);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.tankDrive(1.0, 0.5, false);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(0.5, right.get(), 1e-9);
+
+    // Backward
+    drive.tankDrive(-1.0, -1.0, false);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.tankDrive(-0.5, -1.0, false);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.tankDrive(-0.5, 1.0, false);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+  }
+
+  @Test
+  void testTankDriveSquared() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var drive = new DifferentialDrive(left, right);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.tankDrive(1.0, 1.0, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward left turn
+    drive.tankDrive(0.5, 1.0, true);
+    assertEquals(0.25, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+
+    // Forward right turn
+    drive.tankDrive(1.0, 0.5, true);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(0.25, right.get(), 1e-9);
+
+    // Backward
+    drive.tankDrive(-1.0, -1.0, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward left turn
+    drive.tankDrive(-0.5, -1.0, true);
+    assertEquals(-0.25, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+
+    // Backward right turn
+    drive.tankDrive(-1.0, -0.5, true);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-0.25, right.get(), 1e-9);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java
deleted file mode 100644
index 14e9401..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/DriveTest.java
+++ /dev/null
@@ -1,170 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.drive;
-
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.MockSpeedController;
-import edu.wpi.first.wpilibj.RobotDrive;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-/**
- * Tests DifferentialDrive and MecanumDrive.
- */
-public class DriveTest {
-  private final MockSpeedController m_rdFrontLeft = new MockSpeedController();
-  private final MockSpeedController m_rdRearLeft = new MockSpeedController();
-  private final MockSpeedController m_rdFrontRight = new MockSpeedController();
-  private final MockSpeedController m_rdRearRight = new MockSpeedController();
-  private final MockSpeedController m_frontLeft = new MockSpeedController();
-  private final MockSpeedController m_rearLeft = new MockSpeedController();
-  private final MockSpeedController m_frontRight = new MockSpeedController();
-  private final MockSpeedController m_rearRight = new MockSpeedController();
-  private final RobotDrive m_robotDrive =
-      new RobotDrive(m_rdFrontLeft, m_rdRearLeft, m_rdFrontRight, m_rdRearRight);
-  private final DifferentialDrive m_differentialDrive =
-      new DifferentialDrive(m_frontLeft, m_frontRight);
-  private final MecanumDrive m_mecanumDrive =
-      new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
-
-  private final double[] m_testJoystickValues = {1.0, 0.9, 0.5, 0.01, 0.0, -0.01, -0.5, -0.9,
-                                                 -1.0};
-  private final double[] m_testGyroValues = {0, 30, 45, 90, 135, 180, 225, 270, 305, 360, 540,
-                                             -45, -90, -135, -180, -225, -270, -305, -360, -540};
-
-  @BeforeEach
-  void setUp() {
-    m_differentialDrive.setDeadband(0.0);
-    m_differentialDrive.setSafetyEnabled(false);
-    m_mecanumDrive.setDeadband(0.0);
-    m_mecanumDrive.setSafetyEnabled(false);
-    m_robotDrive.setSafetyEnabled(false);
-  }
-
-  @Test
-  public void testTankDriveSquared() {
-    for (double leftJoystick : m_testJoystickValues) {
-      for (double rightJoystick : m_testJoystickValues) {
-        m_robotDrive.tankDrive(leftJoystick, rightJoystick);
-        m_differentialDrive.tankDrive(leftJoystick, rightJoystick);
-        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
-            "Left Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
-            + rightJoystick);
-        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
-            "Right Motor squared didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
-            + rightJoystick);
-      }
-    }
-  }
-
-  @Test
-  void testTankDrive() {
-    for (double leftJoystick : m_testJoystickValues) {
-      for (double rightJoystick : m_testJoystickValues) {
-        m_robotDrive.tankDrive(leftJoystick, rightJoystick, false);
-        m_differentialDrive.tankDrive(leftJoystick, rightJoystick, false);
-        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
-            "Left Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
-            + rightJoystick);
-        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
-            "Right Motor didn't match. Left Joystick: " + leftJoystick + " Right Joystick: "
-            + rightJoystick);
-      }
-    }
-  }
-
-  @Test
-  void testArcadeDriveSquared() {
-    for (double moveJoystick : m_testJoystickValues) {
-      for (double rotateJoystick : m_testJoystickValues) {
-        m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick);
-        m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick);
-        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
-            "Left Motor squared didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
-            + rotateJoystick);
-        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
-            "Right Motor squared didn't match. Move Joystick: " + moveJoystick
-            + " Rotate Joystick: " + rotateJoystick);
-      }
-    }
-  }
-
-  @Test
-  void testArcadeDrive() {
-    for (double moveJoystick : m_testJoystickValues) {
-      for (double rotateJoystick : m_testJoystickValues) {
-        m_robotDrive.arcadeDrive(moveJoystick, rotateJoystick, false);
-        m_differentialDrive.arcadeDrive(moveJoystick, -rotateJoystick, false);
-        assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
-            "Left Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
-            + rotateJoystick);
-        assertEquals(m_rdFrontRight.get(), m_frontRight.get(), 0.01,
-            "Right Motor didn't match. Move Joystick: " + moveJoystick + " Rotate Joystick: "
-            + rotateJoystick);
-      }
-    }
-  }
-
-  @Test
-  void testMecanumPolar() {
-    for (double magnitudeJoystick : m_testJoystickValues) {
-      for (double directionJoystick : m_testGyroValues) {
-        for (double rotationJoystick : m_testJoystickValues) {
-          m_robotDrive.mecanumDrive_Polar(magnitudeJoystick, directionJoystick, rotationJoystick);
-          m_mecanumDrive.drivePolar(magnitudeJoystick, directionJoystick, rotationJoystick);
-          assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
-              "Left Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
-              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
-              + rotationJoystick);
-          assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01,
-              "Right Front Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
-              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
-              + rotationJoystick);
-          assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01,
-              "Left Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
-              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
-              + rotationJoystick);
-          assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01,
-              "Right Rear Motor didn't match. Magnitude Joystick: " + magnitudeJoystick
-              + " Direction Joystick: " + directionJoystick + " RotationJoystick: "
-              + rotationJoystick);
-        }
-      }
-    }
-  }
-
-  @Test
-  @SuppressWarnings("checkstyle:LocalVariableName")
-  void testMecanumCartesian() {
-    for (double x_Joystick : m_testJoystickValues) {
-      for (double y_Joystick : m_testJoystickValues) {
-        for (double rotationJoystick : m_testJoystickValues) {
-          for (double gyroValue : m_testGyroValues) {
-            m_robotDrive.mecanumDrive_Cartesian(x_Joystick, y_Joystick, rotationJoystick,
-                                                gyroValue);
-            m_mecanumDrive.driveCartesian(x_Joystick, -y_Joystick, rotationJoystick, -gyroValue);
-            assertEquals(m_rdFrontLeft.get(), m_frontLeft.get(), 0.01,
-                "Left Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
-                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
-            assertEquals(m_rdFrontRight.get(), -m_frontRight.get(), 0.01,
-                "Right Front Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
-                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
-            assertEquals(m_rdRearLeft.get(), m_rearLeft.get(), 0.01,
-                "Left Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
-                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
-            assertEquals(m_rdRearRight.get(), -m_rearRight.get(), 0.01,
-                "Right Rear Motor didn't match. X Joystick: " + x_Joystick + " Y Joystick: "
-                + y_Joystick + " RotationJoystick: " + rotationJoystick + " Gyro: " + gyroValue);
-          }
-        }
-      }
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java
new file mode 100644
index 0000000..7d06d9f
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/KilloughDriveTest.java
@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.drive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
+import org.junit.jupiter.api.Test;
+
+class KilloughDriveTest {
+  @Test
+  void testCartesian() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var back = new MockMotorController();
+    var drive = new KilloughDrive(left, right, back);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.driveCartesian(1.0, 0.0, 0.0);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(-0.5, right.get(), 1e-9);
+    assertEquals(0.0, back.get(), 1e-9);
+
+    // Left
+    drive.driveCartesian(0.0, -1.0, 0.0);
+    assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9);
+    assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9);
+    assertEquals(1.0, back.get(), 1e-9);
+
+    // Right
+    drive.driveCartesian(0.0, 1.0, 0.0);
+    assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9);
+    assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9);
+    assertEquals(-1.0, back.get(), 1e-9);
+
+    // Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+    assertEquals(-1.0, back.get(), 1e-9);
+
+    // Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+    assertEquals(1.0, back.get(), 1e-9);
+  }
+
+  @Test
+  void testCartesiangyro90CW() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var back = new MockMotorController();
+    var drive = new KilloughDrive(left, right, back);
+    drive.setDeadband(0.0);
+
+    // Forward in global frame; left in robot frame
+    drive.driveCartesian(1.0, 0.0, 0.0, 90.0);
+    assertEquals(-Math.sqrt(3) / 2, left.get(), 1e-9);
+    assertEquals(-Math.sqrt(3) / 2, right.get(), 1e-9);
+    assertEquals(1.0, back.get(), 1e-9);
+
+    // Left in global frame; backward in robot frame
+    drive.driveCartesian(0.0, -1.0, 0.0, 90.0);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(0.5, right.get(), 1e-9);
+    assertEquals(0.0, back.get(), 1e-9);
+
+    // Right in global frame; forward in robot frame
+    drive.driveCartesian(0.0, 1.0, 0.0, 90.0);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(-0.5, right.get(), 1e-9);
+    assertEquals(0.0, back.get(), 1e-9);
+
+    // Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0, 90.0);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+    assertEquals(-1.0, back.get(), 1e-9);
+
+    // Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0, 90.0);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+    assertEquals(1.0, back.get(), 1e-9);
+  }
+
+  @Test
+  void testPolar() {
+    var left = new MockMotorController();
+    var right = new MockMotorController();
+    var back = new MockMotorController();
+    var drive = new KilloughDrive(left, right, back);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.drivePolar(1.0, 0.0, 0.0);
+    assertEquals(Math.sqrt(3) / 2, left.get(), 1e-9);
+    assertEquals(Math.sqrt(3) / 2, right.get(), 1e-9);
+    assertEquals(-1.0, back.get(), 1e-9);
+
+    // Left
+    drive.drivePolar(1.0, -90.0, 0.0);
+    assertEquals(-0.5, left.get(), 1e-9);
+    assertEquals(0.5, right.get(), 1e-9);
+    assertEquals(0.0, back.get(), 1e-9);
+
+    // Right
+    drive.drivePolar(1.0, 90.0, 0.0);
+    assertEquals(0.5, left.get(), 1e-9);
+    assertEquals(-0.5, right.get(), 1e-9);
+    assertEquals(0.0, back.get(), 1e-9);
+
+    // Rotate CCW
+    drive.drivePolar(0.0, 0.0, -1.0);
+    assertEquals(-1.0, left.get(), 1e-9);
+    assertEquals(-1.0, right.get(), 1e-9);
+    assertEquals(-1.0, back.get(), 1e-9);
+
+    // Rotate CW
+    drive.drivePolar(0.0, 0.0, 1.0);
+    assertEquals(1.0, left.get(), 1e-9);
+    assertEquals(1.0, right.get(), 1e-9);
+    assertEquals(1.0, back.get(), 1e-9);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
new file mode 100644
index 0000000..70a07a2
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/drive/MecanumDriveTest.java
@@ -0,0 +1,147 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.drive;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.motorcontrol.MockMotorController;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveTest {
+  @Test
+  void testCartesian() {
+    var fl = new MockMotorController();
+    var fr = new MockMotorController();
+    var rl = new MockMotorController();
+    var rr = new MockMotorController();
+    var drive = new MecanumDrive(fl, fr, rl, rr);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.driveCartesian(1.0, 0.0, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Left
+    drive.driveCartesian(0.0, -1.0, 0.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Right
+    drive.driveCartesian(0.0, 1.0, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+  }
+
+  @Test
+  void testCartesiangyro90CW() {
+    var fl = new MockMotorController();
+    var fr = new MockMotorController();
+    var rl = new MockMotorController();
+    var rr = new MockMotorController();
+    var drive = new MecanumDrive(fl, fr, rl, rr);
+    drive.setDeadband(0.0);
+
+    // Forward in global frame; left in robot frame
+    drive.driveCartesian(1.0, 0.0, 0.0, 90.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Left in global frame; backward in robot frame
+    drive.driveCartesian(0.0, -1.0, 0.0, 90.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Right in global frame; forward in robot frame
+    drive.driveCartesian(0.0, 1.0, 0.0, 90.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CCW
+    drive.driveCartesian(0.0, 0.0, -1.0, 90.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CW
+    drive.driveCartesian(0.0, 0.0, 1.0, 90.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+  }
+
+  @Test
+  void testPolar() {
+    var fl = new MockMotorController();
+    var fr = new MockMotorController();
+    var rl = new MockMotorController();
+    var rr = new MockMotorController();
+    var drive = new MecanumDrive(fl, fr, rl, rr);
+    drive.setDeadband(0.0);
+
+    // Forward
+    drive.drivePolar(1.0, 0.0, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Left
+    drive.drivePolar(1.0, -90.0, 0.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+
+    // Right
+    drive.drivePolar(1.0, 90.0, 0.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CCW
+    drive.drivePolar(0.0, 0.0, -1.0);
+    assertEquals(-1.0, fl.get(), 1e-9);
+    assertEquals(-1.0, fr.get(), 1e-9);
+    assertEquals(1.0, rl.get(), 1e-9);
+    assertEquals(1.0, rr.get(), 1e-9);
+
+    // Rotate CW
+    drive.drivePolar(0.0, 0.0, 1.0);
+    assertEquals(1.0, fl.get(), 1e-9);
+    assertEquals(1.0, fr.get(), 1e-9);
+    assertEquals(-1.0, rl.get(), 1e-9);
+    assertEquals(-1.0, rr.get(), 1e-9);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
index f70f3f8..083e82c 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/JNITest.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.hal;
 
-import org.junit.jupiter.api.Test;
-
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.HALUtil;
 import edu.wpi.first.networktables.NetworkTablesJNI;
+import org.junit.jupiter.api.Test;
 
 class JNITest {
   @Test
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
index 3887d91..855ace5 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/hal/MatchInfoDataTest.java
@@ -1,26 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.hal;
 
-import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
 
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.hal.MatchInfoData;
 import edu.wpi.first.hal.simulation.DriverStationDataJNI;
 import edu.wpi.first.wpilibj.DriverStation.MatchType;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
+import org.junit.jupiter.api.Test;
 
 class MatchInfoDataTest {
   @Test
   void testSetMatchInfo() {
-
     MatchType matchType = MatchType.Qualification;
     DriverStationDataJNI.setMatchInfo("Event Name", "Game Message", 174, 191, matchType.ordinal());
 
@@ -32,7 +27,6 @@
         () -> assertEquals(matchType.ordinal(), outMatchInfo.matchType),
         () -> assertEquals(174, outMatchInfo.matchNumber),
         () -> assertEquals(191, outMatchInfo.replayNumber),
-        () -> assertEquals("Game Message", outMatchInfo.gameSpecificMessage)
-    );
+        () -> assertEquals("Game Message", outMatchInfo.gameSpecificMessage));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
index dd33945..6d4a666 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/livewindow/LiveWindowTest.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.livewindow;
 
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
new file mode 100644
index 0000000..e7a6b8e
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+public class MockMotorController implements MotorController {
+  private double m_speed;
+  private boolean m_isInverted;
+
+  @Override
+  public void set(double speed) {
+    m_speed = m_isInverted ? -speed : speed;
+  }
+
+  @Override
+  public double get() {
+    return m_speed;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    m_speed = 0;
+  }
+
+  @Override
+  public void stopMotor() {
+    disable();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java
new file mode 100644
index 0000000..6efb3de
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/motorcontrol/MotorControllerGroupTest.java
@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+import static org.junit.jupiter.api.Assertions.assertArrayEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.Arrays;
+import java.util.stream.DoubleStream;
+import java.util.stream.IntStream;
+import java.util.stream.Stream;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class MotorControllerGroupTest {
+  private static Stream<Arguments> motorControllerArguments() {
+    return IntStream.of(1, 2, 3)
+        .mapToObj(
+            number -> {
+              MotorController[] motorControllers =
+                  Stream.generate(MockMotorController::new)
+                      .limit(number)
+                      .toArray(MotorController[]::new);
+              MotorControllerGroup group =
+                  new MotorControllerGroup(
+                      motorControllers[0],
+                      Arrays.copyOfRange(motorControllers, 1, motorControllers.length));
+              return Arguments.of(group, motorControllers);
+            });
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void setTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.set(1.0);
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> 1.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void getInvertedTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.setInverted(true);
+
+    assertTrue(group.getInverted());
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void setInvertedDoesNotModifyMotorControllersTest(
+      final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.setInverted(true);
+
+    assertArrayEquals(
+        Stream.generate(() -> false).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).map(MotorController::getInverted).toArray());
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void setInvertedDoesInvertTest(
+      final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.setInverted(true);
+    group.set(1.0);
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> -1.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void disableTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.set(1.0);
+    group.disable();
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> 0.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+
+  @ParameterizedTest
+  @MethodSource("motorControllerArguments")
+  void stopMotorTest(final MotorControllerGroup group, final MotorController[] motorControllers) {
+    group.set(1.0);
+    group.stopMotor();
+
+    assertArrayEquals(
+        DoubleStream.generate(() -> 0.0).limit(motorControllers.length).toArray(),
+        Arrays.stream(motorControllers).mapToDouble(MotorController::get).toArray(),
+        0.00005);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
index 89ca3ec..04397c2 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/MockActuatorSendable.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import edu.wpi.first.wpilibj.Sendable;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
-import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
 
-/**
- * A mock sendable that marks itself as an actuator.
- */
+/** A mock sendable that marks itself as an actuator. */
 public class MockActuatorSendable implements Sendable {
   public MockActuatorSendable(String name) {
     SendableRegistry.add(this, name);
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
index 9824f11..61db14a 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardInstanceTest.java
@@ -1,24 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import org.junit.jupiter.api.AfterEach;
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
 import static org.junit.jupiter.api.Assertions.assertAll;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import org.junit.jupiter.api.AfterEach;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
 public class ShuffleboardInstanceTest {
   private NetworkTableInstance m_ntInstance;
   private ShuffleboardInstance m_shuffleboardInstance;
@@ -36,32 +32,42 @@
 
   @Test
   void testPathFluent() {
-    NetworkTableEntry entry = m_shuffleboardInstance.getTab("Tab Title")
-                                                  .getLayout("Layout Title", "List Layout")
-                                                  .add("Data", "string")
-                                                  .withWidget("Text View")
-                                                  .getEntry();
+    NetworkTableEntry entry =
+        m_shuffleboardInstance
+            .getTab("Tab Title")
+            .getLayout("Layout Title", "List Layout")
+            .add("Data", "string")
+            .withWidget("Text View")
+            .getEntry();
 
     assertAll(
         () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
-        () -> assertEquals("/Shuffleboard/Tab Title/Layout Title/Data", entry.getName(),
-                           "Entry path generated incorrectly"));
+        () ->
+            assertEquals(
+                "/Shuffleboard/Tab Title/Layout Title/Data",
+                entry.getName(),
+                "Entry path generated incorrectly"));
   }
 
   @Test
   void testNestedLayoutsFluent() {
-    NetworkTableEntry entry = m_shuffleboardInstance.getTab("Tab")
-                                                  .getLayout("First", "List")
-                                                  .getLayout("Second", "List")
-                                                  .getLayout("Third", "List")
-                                                  .getLayout("Fourth", "List")
-                                                  .add("Value", "string")
-                                                  .getEntry();
+    NetworkTableEntry entry =
+        m_shuffleboardInstance
+            .getTab("Tab")
+            .getLayout("First", "List")
+            .getLayout("Second", "List")
+            .getLayout("Third", "List")
+            .getLayout("Fourth", "List")
+            .add("Value", "string")
+            .getEntry();
 
     assertAll(
         () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
-        () -> assertEquals("/Shuffleboard/Tab/First/Second/Third/Fourth/Value", entry.getName(),
-                           "Entry path generated incorrectly"));
+        () ->
+            assertEquals(
+                "/Shuffleboard/Tab/First/Second/Third/Fourth/Value",
+                entry.getName(),
+                "Entry path generated incorrectly"));
   }
 
   @Test
@@ -76,26 +82,29 @@
 
     assertAll(
         () -> assertEquals("string", entry.getString(null), "Wrong entry value"),
-        () -> assertEquals("/Shuffleboard/Tab/First/Second/Third/Fourth/Value", entry.getName(),
-                           "Entry path generated incorrectly"));
+        () ->
+            assertEquals(
+                "/Shuffleboard/Tab/First/Second/Third/Fourth/Value",
+                entry.getName(),
+                "Entry path generated incorrectly"));
   }
 
   @Test
   void testLayoutTypeIsSet() {
     String layoutType = "Type";
-    m_shuffleboardInstance.getTab("Tab")
-                          .getLayout("Title", layoutType);
+    m_shuffleboardInstance.getTab("Tab").getLayout("Title", layoutType);
     m_shuffleboardInstance.update();
-    NetworkTableEntry entry = m_ntInstance.getEntry(
-        "/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
+    NetworkTableEntry entry =
+        m_ntInstance.getEntry("/Shuffleboard/.metadata/Tab/Title/PreferredComponent");
     assertEquals(layoutType, entry.getString("Not Set"), "Layout type not set");
   }
 
   @Test
   void testNestedActuatorWidgetsAreDisabled() {
-    m_shuffleboardInstance.getTab("Tab")
-                          .getLayout("Title", "Layout")
-                          .add(new MockActuatorSendable("Actuator"));
+    m_shuffleboardInstance
+        .getTab("Tab")
+        .getLayout("Title", "Layout")
+        .add(new MockActuatorSendable("Actuator"));
     NetworkTableEntry controllableEntry =
         m_ntInstance.getEntry("/Shuffleboard/Tab/Title/Actuator/.controllable");
 
@@ -111,5 +120,4 @@
     controllable = controllableEntry.getValue().getBoolean();
     assertFalse(controllable, "The nested actuator widget should have been disabled");
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
index 1469170..963d262 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/ShuffleboardTest.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertSame;
 
 import edu.wpi.first.wpilibj.UtilityClassTest;
-
-import static org.junit.jupiter.api.Assertions.assertSame;
+import org.junit.jupiter.api.Test;
 
 public class ShuffleboardTest extends UtilityClassTest<Shuffleboard> {
   public ShuffleboardTest() {
@@ -26,5 +22,4 @@
     ShuffleboardTab tab2 = Shuffleboard.getTab("testTabObjectsCached");
     assertSame(tab1, tab2, "Tab objects were not cached");
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
index 6e0da8e..036c73d 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/shuffleboard/SuppliedValueWidgetTest.java
@@ -1,25 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.shuffleboard;
 
-import java.util.concurrent.atomic.AtomicInteger;
-
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.networktables.NetworkTableEntry;
-import edu.wpi.first.networktables.NetworkTableInstance;
-
 import static org.junit.jupiter.api.Assertions.assertArrayEquals;
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
 class SuppliedValueWidgetTest {
   private NetworkTableInstance m_ntInstance;
   private ShuffleboardInstance m_instance;
@@ -33,8 +28,7 @@
   @Test
   void testAddString() {
     AtomicInteger count = new AtomicInteger(0);
-    m_instance.getTab("Tab")
-        .addString("Title", () -> Integer.toString(count.incrementAndGet()));
+    m_instance.getTab("Tab").addString("Title", () -> Integer.toString(count.incrementAndGet()));
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
@@ -47,8 +41,7 @@
   @Test
   void testAddDouble() {
     AtomicInteger num = new AtomicInteger(0);
-    m_instance.getTab("Tab")
-        .addNumber("Title", num::incrementAndGet);
+    m_instance.getTab("Tab").addNumber("Title", num::incrementAndGet);
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
@@ -61,8 +54,7 @@
   @Test
   void testAddBoolean() {
     boolean[] bool = {false};
-    m_instance.getTab("Tab")
-        .addBoolean("Title", () -> bool[0] = !bool[0]);
+    m_instance.getTab("Tab").addBoolean("Title", () -> bool[0] = !bool[0]);
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
@@ -75,8 +67,7 @@
   @Test
   void testAddStringArray() {
     String[] arr = {"foo", "bar"};
-    m_instance.getTab("Tab")
-        .addStringArray("Title", () -> arr);
+    m_instance.getTab("Tab").addStringArray("Title", () -> arr);
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
@@ -86,8 +77,7 @@
   @Test
   void testAddDoubleArray() {
     double[] arr = {0, 1};
-    m_instance.getTab("Tab")
-        .addDoubleArray("Title", () -> arr);
+    m_instance.getTab("Tab").addDoubleArray("Title", () -> arr);
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
@@ -97,8 +87,7 @@
   @Test
   void testAddBooleanArray() {
     boolean[] arr = {true, false};
-    m_instance.getTab("Tab")
-        .addBooleanArray("Title", () -> arr);
+    m_instance.getTab("Tab").addBooleanArray("Title", () -> arr);
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
@@ -108,12 +97,10 @@
   @Test
   void testAddRawBytes() {
     byte[] arr = {0, 1, 2, 3};
-    m_instance.getTab("Tab")
-        .addRaw("Title", () -> arr);
+    m_instance.getTab("Tab").addRaw("Title", () -> arr);
     NetworkTableEntry entry = m_ntInstance.getEntry("/Shuffleboard/Tab/Title");
 
     m_instance.update();
     assertArrayEquals(arr, entry.getRaw(new byte[0]));
   }
-
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java
new file mode 100644
index 0000000..7f41038
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADXL345_I2C;
+import edu.wpi.first.wpilibj.ADXL345_SPI;
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+public class ADXL345SimTest {
+  @ParameterizedTest
+  @EnumSource(Accelerometer.Range.class)
+  void testInitI2C(Accelerometer.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL345_I2C accel = new ADXL345_I2C(I2C.Port.kMXP, range)) {
+      ADXL345Sim sim = new ADXL345Sim(accel);
+
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL345_I2C.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+
+  @ParameterizedTest
+  @EnumSource(Accelerometer.Range.class)
+  void testInitSPi(Accelerometer.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL345_SPI accel = new ADXL345_SPI(SPI.Port.kMXP, range)) {
+      ADXL345Sim sim = new ADXL345Sim(accel);
+
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL345_SPI.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java
new file mode 100644
index 0000000..f361381
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADXL362;
+import edu.wpi.first.wpilibj.SPI;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+public class ADXL362SimTest {
+  @ParameterizedTest
+  @EnumSource(Accelerometer.Range.class)
+  void testAccel(Accelerometer.Range range) {
+    HAL.initialize(500, 0);
+
+    try (ADXL362 accel = new ADXL362(SPI.Port.kMXP, range)) {
+      assertEquals(0, accel.getX());
+      assertEquals(0, accel.getY());
+      assertEquals(0, accel.getZ());
+
+      ADXL362Sim sim = new ADXL362Sim(accel);
+      sim.setX(1.91);
+      sim.setY(-3.405);
+      sim.setZ(2.29);
+
+      assertEquals(1.91, accel.getX());
+      assertEquals(-3.405, accel.getY());
+      assertEquals(2.29, accel.getZ());
+
+      ADXL362.AllAxes allAccel = accel.getAccelerations();
+      assertEquals(1.91, allAccel.XAxis);
+      assertEquals(-3.405, allAccel.YAxis);
+      assertEquals(2.29, allAccel.ZAxis);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java
new file mode 100644
index 0000000..301cef1
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings({"TypeName"})
+class ADXRS450_GyroSimTest {
+  @Test
+  void testCallbacks() {
+    HAL.initialize(500, 0);
+
+    try (ADXRS450_Gyro gyro = new ADXRS450_Gyro()) {
+      ADXRS450_GyroSim sim = new ADXRS450_GyroSim(gyro);
+
+      assertEquals(0, gyro.getAngle());
+      assertEquals(0, gyro.getRate());
+
+      sim.setAngle(123.456);
+      sim.setRate(229.3504);
+
+      assertEquals(123.456, gyro.getAngle());
+      assertEquals(229.3504, gyro.getRate());
+
+      gyro.reset();
+      assertEquals(0, gyro.getAngle());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java
index 05ea290..998b66a 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AccelerometerSimTest.java
@@ -1,42 +1,142 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.hal.AccelerometerJNI;
-import edu.wpi.first.hal.HAL;
-
+import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
-class AccelerometerSimTest {
-  static class TriggeredStore {
-    public boolean m_wasTriggered;
-    public boolean m_setValue = true;
-  }
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.BuiltInAccelerometer;
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.EnumCallback;
+import org.junit.jupiter.api.Test;
 
+class AccelerometerSimTest {
   @Test
   void testCallbacks() {
     HAL.initialize(500, 0);
     BuiltInAccelerometerSim sim = new BuiltInAccelerometerSim();
     sim.resetData();
 
-    TriggeredStore store = new TriggeredStore();
+    BooleanCallback store = new BooleanCallback();
 
-    try (CallbackStore cb = sim.registerActiveCallback((s, v) -> {
-      store.m_wasTriggered = true;
-      store.m_setValue = v.getBoolean();
-    }, false)) {
-      assertFalse(store.m_wasTriggered);
-      AccelerometerJNI.setAccelerometerActive(true);
-      assertTrue(store.m_wasTriggered);
-      assertTrue(store.m_setValue);
+    try (CallbackStore cb = sim.registerActiveCallback(store, false)) {
+      assertFalse(store.wasTriggered());
+      sim.setActive(true);
+      assertTrue(sim.getActive());
+      assertTrue(store.wasTriggered());
+      assertTrue(store.getSetValue());
+    }
+  }
+
+  @Test
+  void testX() {
+    HAL.initialize(500, 0);
+    BuiltInAccelerometerSim sim = new BuiltInAccelerometerSim();
+    sim.resetData();
+
+    DoubleCallback callback = new DoubleCallback();
+    final double kTestValue = 1.91;
+
+    try (BuiltInAccelerometer accel = new BuiltInAccelerometer();
+        CallbackStore cb = sim.registerXCallback(callback, false)) {
+      sim.setX(kTestValue);
+      assertEquals(kTestValue, accel.getX());
+      assertEquals(kTestValue, sim.getX());
+      assertTrue(callback.wasTriggered());
+      assertEquals(kTestValue, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testY() {
+    HAL.initialize(500, 0);
+    BuiltInAccelerometerSim sim = new BuiltInAccelerometerSim();
+    sim.resetData();
+
+    DoubleCallback callback = new DoubleCallback();
+    final double kTestValue = 2.29;
+
+    try (BuiltInAccelerometer accel = new BuiltInAccelerometer();
+        CallbackStore cb = sim.registerYCallback(callback, false)) {
+      sim.setY(kTestValue);
+      assertEquals(kTestValue, accel.getY());
+      assertEquals(kTestValue, sim.getY());
+      assertTrue(callback.wasTriggered());
+      assertEquals(kTestValue, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testZ() {
+    HAL.initialize(500, 0);
+
+    BuiltInAccelerometerSim sim = new BuiltInAccelerometerSim();
+    sim.resetData();
+
+    DoubleCallback callback = new DoubleCallback();
+    final double kTestValue = 3.405;
+
+    try (BuiltInAccelerometer accel = new BuiltInAccelerometer();
+        CallbackStore cb = sim.registerZCallback(callback, false)) {
+      sim.setZ(kTestValue);
+      assertEquals(kTestValue, accel.getZ());
+      assertEquals(kTestValue, sim.getZ());
+      assertTrue(callback.wasTriggered());
+      assertEquals(kTestValue, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testRange() {
+    HAL.initialize(500, 0);
+
+    BuiltInAccelerometerSim sim = new BuiltInAccelerometerSim();
+    sim.resetData();
+
+    EnumCallback callback = new EnumCallback();
+
+    Accelerometer.Range range = Accelerometer.Range.k4G;
+    try (CallbackStore cb = sim.registerRangeCallback(callback, false);
+        BuiltInAccelerometer accel = new BuiltInAccelerometer(range)) {
+      assertTrue(callback.wasTriggered());
+      assertEquals(range.ordinal(), sim.getRange());
+      assertEquals(range.ordinal(), callback.getSetValue());
+
+      // 2G
+      callback.reset();
+      range = Accelerometer.Range.k2G;
+      accel.setRange(range);
+      assertTrue(callback.wasTriggered());
+      assertEquals(range.ordinal(), sim.getRange());
+      assertEquals(range.ordinal(), callback.getSetValue());
+
+      // 4G
+      callback.reset();
+      range = Accelerometer.Range.k4G;
+      accel.setRange(range);
+      assertTrue(callback.wasTriggered());
+      assertEquals(range.ordinal(), sim.getRange());
+      assertEquals(range.ordinal(), callback.getSetValue());
+
+      // 8G
+      callback.reset();
+      range = Accelerometer.Range.k8G;
+      accel.setRange(range);
+      assertTrue(callback.wasTriggered());
+      assertEquals(range.ordinal(), sim.getRange());
+      assertEquals(range.ordinal(), callback.getSetValue());
+
+      // 16G - Not supported
+      callback.reset();
+      assertThrows(IllegalArgumentException.class, () -> accel.setRange(Accelerometer.Range.k16G));
+      assertFalse(callback.wasTriggered());
     }
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java
new file mode 100644
index 0000000..096f77e
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AddressableLEDSimTest.java
@@ -0,0 +1,133 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.BufferCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import java.util.Arrays;
+import org.junit.jupiter.api.Test;
+
+class AddressableLEDSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+    AddressableLEDSim sim = new AddressableLEDSim();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback initializedCallback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(initializedCallback, false);
+        AddressableLED led = new AddressableLED(0)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(initializedCallback.wasTriggered());
+      assertTrue(initializedCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testLength() {
+    AddressableLEDSim sim = new AddressableLEDSim();
+    IntCallback callback = new IntCallback();
+    try (CallbackStore cb = sim.registerLengthCallback(callback, false);
+        AddressableLED led = new AddressableLED(0)) {
+      assertEquals(1, sim.getLength()); // Defaults to 1 led
+
+      AddressableLEDBuffer ledData = new AddressableLEDBuffer(50);
+      led.setLength(ledData.getLength());
+      led.setData(ledData);
+
+      assertEquals(50, sim.getLength());
+      assertTrue(callback.wasTriggered());
+      assertEquals(50, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetRunning() {
+    AddressableLEDSim sim = AddressableLEDSim.createForIndex(0);
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = sim.registerRunningCallback(callback, false);
+        AddressableLED led = new AddressableLED(0)) {
+      assertFalse(sim.getRunning());
+
+      led.start();
+      assertTrue(sim.getRunning());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+
+      callback.reset();
+      led.stop();
+      assertFalse(sim.getRunning());
+      assertTrue(callback.wasTriggered());
+      assertFalse(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetData() {
+    AddressableLEDSim sim = new AddressableLEDSim();
+    BufferCallback callback = new BufferCallback();
+
+    try (AddressableLED led = new AddressableLED(0);
+        CallbackStore cb = sim.registerDataCallback(callback); ) {
+      assertFalse(sim.getRunning());
+
+      assertEquals(1, sim.getLength()); // Defaults to 1 led
+
+      AddressableLEDBuffer ledData = new AddressableLEDBuffer(3);
+      led.setLength(ledData.getLength());
+
+      ledData.setRGB(0, 255, 0, 0);
+      ledData.setRGB(1, 0, 255, 0);
+      ledData.setRGB(2, 0, 0, 255);
+      led.setData(ledData);
+
+      byte[] data = sim.getData();
+      System.out.println(Arrays.toString(data));
+      assertEquals(12, data.length);
+
+      assertEquals((byte) 0, data[0]);
+      assertEquals((byte) 0, data[1]);
+      assertEquals((byte) 255, data[2]);
+      assertEquals((byte) 0, data[3]);
+
+      assertEquals((byte) 0, data[4]);
+      assertEquals((byte) 255, data[5]);
+      assertEquals((byte) 0, data[6]);
+      assertEquals((byte) 0, data[7]);
+
+      assertEquals((byte) 255, data[8]);
+      assertEquals((byte) 0, data[9]);
+      assertEquals((byte) 0, data[10]);
+      assertEquals((byte) 0, data[11]);
+
+      assertTrue(callback.wasTriggered());
+      data = callback.getSetValue();
+
+      assertEquals((byte) 0, data[0]);
+      assertEquals((byte) 0, data[1]);
+      assertEquals((byte) 255, data[2]);
+      assertEquals((byte) 0, data[3]);
+
+      assertEquals((byte) 0, data[4]);
+      assertEquals((byte) 255, data[5]);
+      assertEquals((byte) 0, data[6]);
+      assertEquals((byte) 0, data[7]);
+
+      assertEquals((byte) 255, data[8]);
+      assertEquals((byte) 0, data[9]);
+      assertEquals((byte) 0, data[10]);
+      assertEquals((byte) 0, data[11]);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
index 677c5eb..5a04441 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogEncoderSimTest.java
@@ -1,29 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.AnalogEncoder;
 import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
 import org.junit.jupiter.api.Test;
 
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 public class AnalogEncoderSimTest {
   @Test
   public void testBasic() {
-    var analogInput = new AnalogInput(0);
-    var analogEncoder = new AnalogEncoder(analogInput);
-    var encoderSim = new AnalogEncoderSim(analogEncoder);
+    try (var analogInput = new AnalogInput(0);
+        var analogEncoder = new AnalogEncoder(analogInput)) {
+      var encoderSim = new AnalogEncoderSim(analogEncoder);
 
-    encoderSim.setPosition(Rotation2d.fromDegrees(180));
-    assertEquals(analogEncoder.get(), 0.5, 1E-8);
-    assertEquals(encoderSim.getTurns(), 0.5, 1E-8);
-    assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8);
+      encoderSim.setPosition(Rotation2d.fromDegrees(180));
+      assertEquals(analogEncoder.get(), 0.5, 1E-8);
+      assertEquals(encoderSim.getTurns(), 0.5, 1E-8);
+      assertEquals(encoderSim.getPosition().getRadians(), Math.PI, 1E-8);
+    }
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSimTest.java
new file mode 100644
index 0000000..5ce5093
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogGyroSimTest.java
@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+class AnalogGyroSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+    AnalogGyroSim sim = new AnalogGyroSim(0);
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback initializedCallback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(initializedCallback, false);
+        AnalogInput ai = new AnalogInput(0);
+        AnalogGyro gyro = new AnalogGyro(ai)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(initializedCallback.wasTriggered());
+      assertTrue(initializedCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetAngle() {
+    HAL.initialize(500, 0);
+
+    AnalogGyroSim sim = new AnalogGyroSim(0);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogGyro gyro = new AnalogGyro(ai);
+        CallbackStore cb = sim.registerAngleCallback(callback, false)) {
+      assertEquals(0, gyro.getAngle());
+
+      final double TEST_ANGLE = 35.04;
+      sim.setAngle(TEST_ANGLE);
+      assertEquals(TEST_ANGLE, sim.getAngle());
+      assertEquals(TEST_ANGLE, gyro.getAngle());
+      assertEquals(-TEST_ANGLE, gyro.getRotation2d().getDegrees());
+      assertTrue(callback.wasTriggered());
+      assertEquals(TEST_ANGLE, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetRate() {
+    HAL.initialize(500, 0);
+
+    AnalogGyroSim sim = new AnalogGyroSim(0);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogGyro gyro = new AnalogGyro(ai);
+        CallbackStore cb = sim.registerRateCallback(callback, false)) {
+      assertEquals(0, gyro.getRate());
+
+      final double TEST_RATE = -19.1;
+      sim.setRate(TEST_RATE);
+      assertEquals(TEST_RATE, sim.getRate());
+      assertEquals(TEST_RATE, gyro.getRate());
+
+      assertTrue(callback.wasTriggered());
+      assertEquals(TEST_RATE, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testReset() {
+    HAL.initialize(500, 0);
+
+    try (AnalogInput ai = new AnalogInput(0);
+        AnalogGyro gyro = new AnalogGyro(ai)) {
+      AnalogGyroSim sim = new AnalogGyroSim(gyro);
+      sim.setAngle(12.34);
+      sim.setRate(43.21);
+
+      sim.resetData();
+      assertEquals(0, sim.getAngle());
+      assertEquals(0, sim.getRate());
+      assertEquals(0, gyro.getAngle());
+      assertEquals(0, gyro.getRate());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java
index a707725..ada4ebf 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogInputSimTest.java
@@ -1,42 +1,86 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.hal.util.AllocationException;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
 import org.junit.jupiter.api.Test;
 
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.hal.HAL;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
 class AnalogInputSimTest {
-  static class DoubleStore {
-    public boolean m_wasTriggered;
-    public boolean m_wasCorrectType;
-    public double m_setValue0;
+  @Test
+  void setInitializeTest() {
+    HAL.initialize(500, 0);
+
+    AnalogInputSim sim = new AnalogInputSim(5);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        AnalogInput input = new AnalogInput(5)) {
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
   }
 
   @Test
-  void setCallbackTest() {
+  void testSetVoltage() {
     HAL.initialize(500, 0);
 
-    try (AnalogInput input = new AnalogInput(5)) {
-      AnalogInputSim inputSim = new AnalogInputSim(input);
+    AnalogInputSim sim = new AnalogInputSim(5);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (CallbackStore cb = sim.registerVoltageCallback(callback, false);
+        AnalogInput input = new AnalogInput(5)) {
+      // Bootstrap the voltage to be non-zero
+      sim.setVoltage(1.0);
 
       for (double i = 0; i < 5.0; i += 0.1) {
-        inputSim.setVoltage(0);
+        callback.reset();
 
+        sim.setVoltage(0);
         assertEquals(input.getVoltage(), 0, 0.001);
 
-        inputSim.setVoltage(i);
-
+        callback.reset();
+        sim.setVoltage(i);
         assertEquals(input.getVoltage(), i, 0.001);
       }
     }
   }
+
+  @Test
+  void testSetOverSampleBits() {
+    HAL.initialize(500, 0);
+    try (AnalogInput input = new AnalogInput(5)) {
+      input.setOversampleBits(3504);
+      assertEquals(3504, input.getOversampleBits());
+    }
+  }
+
+  @Test
+  void tesInitAccumulator() {
+    HAL.initialize(500, 0);
+    try (AnalogInput input = new AnalogInput(0)) {
+      // First initialization works fine
+      assertDoesNotThrow(input::initAccumulator);
+
+      input.resetAccumulator();
+    }
+  }
+
+  @Test
+  void tesInitAccumulatorOnInvalidPort() {
+    HAL.initialize(500, 0);
+    try (AnalogInput input = new AnalogInput(5)) {
+      assertThrows(AllocationException.class, input::initAccumulator);
+    }
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java
index fe1bba8..85a7494 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogOutputSimTest.java
@@ -1,31 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.AnalogOutput;
-import edu.wpi.first.hal.HAL;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.AnalogOutput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
 import org.junit.jupiter.api.Test;
 
 class AnalogOutputSimTest {
-  static class DoubleStore {
-    public boolean m_wasTriggered;
-    public boolean m_wasCorrectType;
-    public double m_setValue = -1;
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
 
-    public void reset() {
-      m_wasCorrectType = false;
-      m_wasTriggered = false;
-      m_setValue = -1;
+    AnalogOutputSim outputSim = new AnalogOutputSim(0);
+    assertFalse(outputSim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = outputSim.registerInitializedCallback(callback, false);
+        AnalogOutput output = new AnalogOutput(0)) {
+      assertTrue(outputSim.getInitialized());
     }
   }
 
@@ -33,37 +34,51 @@
   void setCallbackTest() {
     HAL.initialize(500, 0);
 
-
     try (AnalogOutput output = new AnalogOutput(0)) {
       output.setVoltage(0.5);
 
       AnalogOutputSim outputSim = new AnalogOutputSim(output);
 
-      DoubleStore store = new DoubleStore();
+      DoubleCallback voltageCallback = new DoubleCallback();
 
-      try (CallbackStore cb = outputSim.registerVoltageCallback((name, value) -> {
-        store.m_wasTriggered = true;
-        store.m_wasCorrectType = true;
-        store.m_setValue = value.getDouble();
-      }, false)) {
-        assertFalse(store.m_wasTriggered);
+      try (CallbackStore cb = outputSim.registerVoltageCallback(voltageCallback, false)) {
+        assertFalse(voltageCallback.wasTriggered());
 
         for (double i = 0.1; i < 5.0; i += 0.1) {
-          store.reset();
+          voltageCallback.reset();
 
           output.setVoltage(0);
 
-          assertTrue(store.m_wasTriggered);
-          assertEquals(store.m_setValue, 0, 0.001);
+          assertEquals(0, output.getVoltage());
+          assertEquals(0, outputSim.getVoltage());
+          assertTrue(voltageCallback.wasTriggered());
+          assertEquals(voltageCallback.getSetValue(), 0, 0.001);
 
-          store.reset();
+          voltageCallback.reset();
 
           output.setVoltage(i);
 
-          assertTrue(store.m_wasTriggered);
-          assertEquals(store.m_setValue, i, 0.001);
+          assertEquals(i, output.getVoltage());
+          assertEquals(i, outputSim.getVoltage());
+          assertTrue(voltageCallback.wasTriggered());
+          assertEquals(voltageCallback.getSetValue(), i, 0.001);
         }
       }
     }
   }
+
+  @Test
+  void testReset() {
+    HAL.initialize(500, 0);
+
+    AnalogOutputSim outputSim = new AnalogOutputSim(0);
+
+    try (AnalogOutput output = new AnalogOutput(0)) {
+      output.setVoltage(1.2);
+
+      outputSim.resetData();
+      assertEquals(0, output.getVoltage());
+      assertEquals(0, outputSim.getVoltage());
+    }
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSimTest.java
new file mode 100644
index 0000000..1142914
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/AnalogTriggerSimTest.java
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.AnalogTrigger;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+class AnalogTriggerSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+
+    AnalogTriggerSim sim = AnalogTriggerSim.createForIndex(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        AnalogTrigger trigger = new AnalogTrigger(0)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void triggerLowerBoundTest() {
+    HAL.initialize(500, 0);
+
+    try (AnalogTrigger trigger = new AnalogTrigger(0)) {
+      AnalogTriggerSim sim = new AnalogTriggerSim(trigger);
+      DoubleCallback lowerCallback = new DoubleCallback();
+      DoubleCallback upperCallback = new DoubleCallback();
+      try (CallbackStore lowerCb = sim.registerTriggerLowerBoundCallback(lowerCallback, false);
+          CallbackStore upperCb = sim.registerTriggerUpperBoundCallback(upperCallback, false)) {
+        trigger.setLimitsVoltage(0.299, 1.91);
+
+        assertEquals(0.299, sim.getTriggerLowerBound());
+        assertEquals(1.91, sim.getTriggerUpperBound());
+
+        assertTrue(lowerCallback.wasTriggered());
+        assertEquals(0.299, lowerCallback.getSetValue());
+
+        assertTrue(upperCallback.wasTriggered());
+        assertEquals(1.91, upperCallback.getSetValue());
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java
new file mode 100644
index 0000000..29cac3a
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/CTREPCMSimTest.java
@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsControlModule;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+class CTREPCMSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        PneumaticsControlModule pcm = new PneumaticsControlModule(0)) {
+      assertTrue(sim.getInitialized());
+    }
+    assertFalse(sim.getInitialized());
+  }
+
+  @Test
+  void solenoidOutputTest() {
+    HAL.initialize(500, 0);
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        DoubleSolenoid doubleSolenoid = new DoubleSolenoid(0, PneumaticsModuleType.CTREPCM, 3, 4)) {
+      CTREPCMSim sim = new CTREPCMSim(0);
+      sim.resetData();
+
+      BooleanCallback callback3 = new BooleanCallback();
+      BooleanCallback callback4 = new BooleanCallback();
+
+      try (CallbackStore cb3 = sim.registerSolenoidOutputCallback(3, callback3, false);
+          CallbackStore cb4 = sim.registerSolenoidOutputCallback(4, callback4, false)) {
+        // Reverse
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
+        assertFalse(callback3.wasTriggered());
+        assertNull(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertTrue(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertTrue(sim.getSolenoidOutput(4));
+        assertEquals(0x10, pcm.getSolenoids());
+
+        // Forward
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kForward);
+        assertTrue(callback3.wasTriggered());
+        assertTrue(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertFalse(callback4.getSetValue());
+        assertTrue(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x8, pcm.getSolenoids());
+
+        // Off
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kOff);
+        assertTrue(callback3.wasTriggered());
+        assertFalse(callback3.getSetValue());
+        assertFalse(callback4.wasTriggered());
+        assertNull(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x0, pcm.getSolenoids());
+      }
+    }
+  }
+
+  @Test
+  void setCompressorOnTest() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerCompressorOnCallback(callback, false)) {
+      assertFalse(pcm.getCompressor());
+      assertFalse(sim.getCompressorOn());
+      sim.setCompressorOn(true);
+      assertTrue(pcm.getCompressor());
+      assertTrue(sim.getCompressorOn());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setClosedLoopEnabled() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerClosedLoopEnabledCallback(callback, false)) {
+      pcm.setClosedLoopControl(false);
+      assertFalse(pcm.getClosedLoopControl());
+
+      pcm.setClosedLoopControl(true);
+      assertTrue(sim.getClosedLoopEnabled());
+      assertTrue(pcm.getClosedLoopControl());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setPressureSwitchEnabledTest() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerPressureSwitchCallback(callback, false)) {
+      assertFalse(pcm.getPressureSwitch());
+
+      sim.setPressureSwitch(true);
+      assertTrue(sim.getPressureSwitch());
+      assertTrue(pcm.getPressureSwitch());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setCompressorCurrentTest() {
+    HAL.initialize(500, 0);
+
+    CTREPCMSim sim = new CTREPCMSim(0);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (PneumaticsControlModule pcm = new PneumaticsControlModule(0);
+        CallbackStore cb = sim.registerCompressorCurrentCallback(callback, false)) {
+      assertFalse(pcm.getPressureSwitch());
+
+      sim.setCompressorCurrent(35.04);
+      assertEquals(35.04, sim.getCompressorCurrent());
+      assertEquals(35.04, pcm.getCompressorCurrent());
+      assertTrue(callback.wasTriggered());
+      assertEquals(35.04, callback.getSetValue());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DIOSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DIOSimTest.java
new file mode 100644
index 0000000..1c742fe
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DIOSimTest.java
@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import org.junit.jupiter.api.Test;
+
+class DIOSimTest {
+  @Test
+  void testInitialization() {
+    DIOSim sim = new DIOSim(2);
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback initializedCallback = new BooleanCallback();
+    BooleanCallback isInputCallback = new BooleanCallback();
+
+    try (CallbackStore initializeCb = sim.registerInitializedCallback(initializedCallback, false);
+        CallbackStore inputCb = sim.registerIsInputCallback(isInputCallback, false);
+        DigitalOutput output = new DigitalOutput(2)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(initializedCallback.wasTriggered());
+      assertTrue(initializedCallback.getSetValue());
+
+      assertFalse(sim.getIsInput());
+      assertTrue(isInputCallback.wasTriggered());
+      assertFalse(isInputCallback.getSetValue());
+
+      initializedCallback.reset();
+      sim.setInitialized(false);
+      assertTrue(initializedCallback.wasTriggered());
+      assertFalse(initializedCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testInput() {
+    HAL.initialize(500, 0);
+
+    try (DigitalInput input = new DigitalInput(0)) {
+      DIOSim sim = new DIOSim(input);
+      assertTrue(sim.getIsInput());
+
+      BooleanCallback valueCallback = new BooleanCallback();
+
+      try (CallbackStore cb = sim.registerValueCallback(valueCallback, false)) {
+        assertTrue(input.get());
+        assertTrue(sim.getValue());
+
+        assertFalse(valueCallback.wasTriggered());
+        sim.setValue(false);
+        assertTrue(valueCallback.wasTriggered());
+        assertFalse(valueCallback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void testOutput() {
+    HAL.initialize(500, 0);
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DIOSim sim = new DIOSim(output);
+      assertFalse(sim.getIsInput());
+
+      BooleanCallback valueCallback = new BooleanCallback();
+
+      try (CallbackStore cb = sim.registerValueCallback(valueCallback, false)) {
+        assertFalse(output.get());
+        assertFalse(sim.getValue());
+
+        assertFalse(valueCallback.wasTriggered());
+        output.set(true);
+        assertTrue(valueCallback.wasTriggered());
+        assertTrue(valueCallback.getSetValue());
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
index 4a0eab3..62fdcc9 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DifferentialDrivetrainSimTest.java
@@ -1,53 +1,50 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.Vector;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N7;
-
-import org.junit.jupiter.api.Test;
-
-import java.util.List;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
 class DifferentialDrivetrainSimTest {
   @Test
   public void testConvergence() {
     var motor = DCMotor.getNEO(2);
-    var plant = LinearSystemId.createDrivetrainVelocitySystem(
-        motor,
-        50,
-        Units.inchesToMeters(2),
-        Units.inchesToMeters(12),
-        0.5,
-        1.0);
+    var plant =
+        LinearSystemId.createDrivetrainVelocitySystem(
+            motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0);
 
     var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
-    var sim = new DifferentialDrivetrainSim(plant,
-        motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+    var sim =
+        new DifferentialDrivetrainSim(
+            plant,
+            motor,
+            1,
+            kinematics.trackWidthMeters,
+            Units.inchesToMeters(2),
+            VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
 
     var feedforward = new LinearPlantInversionFeedforward<>(plant, 0.020);
     var ramsete = new RamseteController();
@@ -56,12 +53,13 @@
     // ground truth
     Matrix<N7, N1> groundTruthX = new Vector<>(Nat.N7());
 
-    var traj = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(),
-        List.of(),
-        new Pose2d(2, 2, new Rotation2d()),
-        new TrajectoryConfig(1, 1)
-            .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(),
+            List.of(),
+            new Pose2d(2, 2, new Rotation2d()),
+            new TrajectoryConfig(1, 1)
+                .addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, 1)));
 
     for (double t = 0; t < traj.getTotalTimeSeconds(); t += 0.020) {
       var state = traj.sample(0.020);
@@ -69,37 +67,44 @@
 
       var wheelSpeeds = kinematics.toWheelSpeeds(ramseteOut);
 
-      var voltages = feedforward.calculate(
-          VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond));
+      var voltages =
+          feedforward.calculate(
+              VecBuilder.fill(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond));
 
       // Sim periodic code
       sim.setInputs(voltages.get(0, 0), voltages.get(1, 0));
       sim.update(0.020);
 
       // Update our ground truth
-      groundTruthX = RungeKutta.rungeKutta(sim::getDynamics, groundTruthX, voltages, 0.020);
+      groundTruthX = NumericalIntegration.rk4(sim::getDynamics, groundTruthX, voltages, 0.020);
     }
 
-    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
-        sim.getState(DifferentialDrivetrainSim.State.kX));
-    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0),
-        sim.getState(DifferentialDrivetrainSim.State.kY));
-    assertEquals(groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0),
-        sim.getState(DifferentialDrivetrainSim.State.kHeading));
+    // 2 inch tolerance is OK since our ground truth is an approximation of the
+    // ODE solution using RK4 anyway
+    assertEquals(
+        groundTruthX.get(DifferentialDrivetrainSim.State.kX.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kX),
+        0.05);
+    assertEquals(
+        groundTruthX.get(DifferentialDrivetrainSim.State.kY.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kY),
+        0.05);
+    assertEquals(
+        groundTruthX.get(DifferentialDrivetrainSim.State.kHeading.value, 0),
+        sim.getState(DifferentialDrivetrainSim.State.kHeading),
+        0.01);
   }
 
   @Test
   public void testCurrent() {
     var motor = DCMotor.getNEO(2);
-    var plant = LinearSystemId.createDrivetrainVelocitySystem(
-        motor,
-        50,
-        Units.inchesToMeters(2),
-        Units.inchesToMeters(12),
-        0.5,
-        1.0);
+    var plant =
+        LinearSystemId.createDrivetrainVelocitySystem(
+            motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 0.5, 1.0);
     var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
-    var sim = new DifferentialDrivetrainSim(plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+    var sim =
+        new DifferentialDrivetrainSim(
+            plant, motor, 1, kinematics.trackWidthMeters, Units.inchesToMeters(2), null);
 
     sim.setInputs(-12, -12);
     for (int i = 0; i < 10; i++) {
@@ -123,16 +128,19 @@
   @Test
   public void testModelStability() {
     var motor = DCMotor.getNEO(2);
-    var plant = LinearSystemId.createDrivetrainVelocitySystem(
-        motor,
-        50,
-        Units.inchesToMeters(2),
-        Units.inchesToMeters(12),
-        2.0,
-        5.0);
+    var plant =
+        LinearSystemId.createDrivetrainVelocitySystem(
+            motor, 50, Units.inchesToMeters(2), Units.inchesToMeters(12), 2.0, 5.0);
 
     var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(24));
-    var sim = new DifferentialDrivetrainSim(plant, motor, 5, kinematics.trackWidthMeters, Units.inchesToMeters(2));
+    var sim =
+        new DifferentialDrivetrainSim(
+            plant,
+            motor,
+            5,
+            kinematics.trackWidthMeters,
+            Units.inchesToMeters(2),
+            VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
 
     sim.setInputs(2, 4);
 
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java
new file mode 100644
index 0000000..9f29aac
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DigitalPWMSimTest.java
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class DigitalPWMSimTest {
+  @Test
+  void testInitialization() {
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DigitalPWMSim sim = new DigitalPWMSim(output);
+      assertFalse(sim.getInitialized());
+
+      BooleanCallback initializeCallback = new BooleanCallback();
+      DoubleCallback dutyCycleCallback = new DoubleCallback();
+      try (CallbackStore initCb = sim.registerInitializedCallback(initializeCallback, false);
+          CallbackStore dutyCycleCb = sim.registerDutyCycleCallback(dutyCycleCallback, false); ) {
+        final double kTestDutyCycle = 0.191;
+        output.enablePWM(kTestDutyCycle);
+
+        assertTrue(sim.getInitialized());
+        assertTrue(initializeCallback.wasTriggered());
+        assertTrue(initializeCallback.getSetValue());
+
+        assertEquals(kTestDutyCycle, sim.getDutyCycle());
+        assertTrue(dutyCycleCallback.wasTriggered());
+        assertEquals(kTestDutyCycle, dutyCycleCallback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void setPinTest() {
+    HAL.initialize(500, 0);
+
+    try (DigitalOutput output = new DigitalOutput(0)) {
+      DigitalPWMSim sim = new DigitalPWMSim(output);
+      IntCallback callback = new IntCallback();
+      try (CallbackStore cb = sim.registerPinCallback(callback, false)) {
+        sim.setPin(191);
+        assertEquals(191, sim.getPin());
+        assertTrue(callback.wasTriggered());
+        assertEquals(191, callback.getSetValue());
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java
new file mode 100644
index 0000000..ce50afc
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java
@@ -0,0 +1,261 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.AllianceStationID;
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.EnumCallback;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.EnumSource;
+
+public class DriverStationSimTest {
+  @Test
+  void testEnabled() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isEnabled());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerEnabledCallback(callback, false)) {
+      DriverStationSim.setEnabled(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getEnabled());
+      assertTrue(DriverStation.isEnabled());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testAutonomus() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isAutonomous());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerAutonomousCallback(callback, false)) {
+      DriverStationSim.setAutonomous(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getAutonomous());
+      assertTrue(DriverStation.isAutonomous());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testTest() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isTest());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerTestCallback(callback, false)) {
+      DriverStationSim.setTest(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getTest());
+      assertTrue(DriverStation.isTest());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testEstop() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isEStopped());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerEStopCallback(callback, false)) {
+      DriverStationSim.setEStop(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getEStop());
+      assertTrue(DriverStation.isEStopped());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testFmsAttached() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    assertFalse(DriverStation.isFMSAttached());
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerFmsAttachedCallback(callback, false)) {
+      DriverStationSim.setFmsAttached(true);
+      DriverStationSim.notifyNewData();
+      assertTrue(DriverStationSim.getFmsAttached());
+      assertTrue(DriverStation.isFMSAttached());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testDsAttached() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.notifyNewData();
+    assertTrue(DriverStation.isDSAttached());
+
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = DriverStationSim.registerDsAttachedCallback(callback, false)) {
+      DriverStationSim.setDsAttached(false);
+      DriverStationSim.notifyNewData();
+      assertFalse(DriverStationSim.getDsAttached());
+      assertFalse(DriverStation.isDSAttached());
+      assertTrue(callback.wasTriggered());
+      assertFalse(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testAllianceStationId() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    EnumCallback callback = new EnumCallback();
+
+    AllianceStationID allianceStation = AllianceStationID.Blue2;
+    DriverStationSim.setAllianceStationId(allianceStation);
+
+    try (CallbackStore cb = DriverStationSim.registerAllianceStationIdCallback(callback, false)) {
+      // B1
+      allianceStation = AllianceStationID.Blue1;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
+      assertEquals(1, DriverStation.getLocation());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // B2
+      allianceStation = AllianceStationID.Blue2;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
+      assertEquals(2, DriverStation.getLocation());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // B3
+      allianceStation = AllianceStationID.Blue3;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Blue, DriverStation.getAlliance());
+      assertEquals(3, DriverStation.getLocation());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // R1
+      allianceStation = AllianceStationID.Red1;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
+      assertEquals(1, DriverStation.getLocation());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // R2
+      allianceStation = AllianceStationID.Red2;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
+      assertEquals(2, DriverStation.getLocation());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+
+      // R3
+      allianceStation = AllianceStationID.Red3;
+      DriverStationSim.setAllianceStationId(allianceStation);
+      assertEquals(allianceStation, DriverStationSim.getAllianceStationId());
+      assertEquals(DriverStation.Alliance.Red, DriverStation.getAlliance());
+      assertEquals(3, DriverStation.getLocation());
+      assertTrue(callback.wasTriggered());
+      assertEquals(allianceStation.ordinal(), callback.getSetValue());
+    }
+  }
+
+  @ParameterizedTest
+  @EnumSource(DriverStation.MatchType.class)
+  public void testMatchType(DriverStation.MatchType matchType) {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.setMatchType(matchType);
+    DriverStationSim.notifyNewData();
+    assertEquals(matchType, DriverStation.getMatchType());
+  }
+
+  @Test
+  public void testReplayNumber() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.setReplayNumber(4);
+    DriverStationSim.notifyNewData();
+    assertEquals(4, DriverStation.getReplayNumber());
+  }
+
+  @Test
+  public void testMatchNumber() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DriverStationSim.setMatchNumber(3);
+    DriverStationSim.notifyNewData();
+    assertEquals(3, DriverStation.getMatchNumber());
+  }
+
+  @Test
+  public void testMatchTime() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    DoubleCallback callback = new DoubleCallback();
+    try (CallbackStore cb = DriverStationSim.registerMatchTimeCallback(callback, false)) {
+      final double testTime = 19.174;
+      DriverStationSim.setMatchTime(testTime);
+      assertEquals(testTime, DriverStationSim.getMatchTime());
+      assertEquals(testTime, DriverStation.getMatchTime());
+      assertTrue(callback.wasTriggered());
+      assertEquals(testTime, callback.getSetValue());
+    }
+  }
+
+  @Test
+  public void testSetGameSpecificMessage() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    final String message = "Hello World!";
+    DriverStationSim.setGameSpecificMessage(message);
+    DriverStationSim.notifyNewData();
+    assertEquals(message, DriverStation.getGameSpecificMessage());
+  }
+
+  @Test
+  public void testSetEventName() {
+    HAL.initialize(500, 0);
+    DriverStationSim.resetData();
+
+    final String message = "The Best Event";
+    DriverStationSim.setEventName(message);
+    DriverStationSim.notifyNewData();
+    assertEquals(message, DriverStation.getEventName());
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java
new file mode 100644
index 0000000..49f8e77
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleEncoderSimTest.java
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import org.junit.jupiter.api.Test;
+
+class DutyCycleEncoderSimTest {
+  @Test
+  void setTest() {
+    try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) {
+      DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder);
+
+      sim.set(5.67);
+      assertEquals(5.67, encoder.get());
+    }
+  }
+
+  @Test
+  void setDistanceTest() {
+    HAL.initialize(500, 0);
+
+    try (DutyCycleEncoder encoder = new DutyCycleEncoder(0)) {
+      DutyCycleEncoderSim sim = new DutyCycleEncoderSim(encoder);
+
+      sim.setDistance(19.1);
+      assertEquals(19.1, encoder.getDistance());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleSimTest.java
new file mode 100644
index 0000000..3c2a7fe
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DutyCycleSimTest.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class DutyCycleSimTest {
+  @Test
+  void testInitialization() {
+    DutyCycleSim sim = DutyCycleSim.createForIndex(0);
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        DigitalInput di = new DigitalInput(2);
+        DutyCycle dc = new DutyCycle(di)) {
+      assertTrue(sim.getInitialized());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setFrequencyTest() {
+    HAL.initialize(500, 0);
+
+    try (DigitalInput di = new DigitalInput(2);
+        DutyCycle dc = new DutyCycle(di)) {
+      IntCallback callback = new IntCallback();
+      DutyCycleSim sim = new DutyCycleSim(dc);
+      try (CallbackStore cb = sim.registerFrequencyCallback(callback, false)) {
+        sim.setFrequency(191);
+        assertEquals(191, sim.getFrequency());
+        assertEquals(191, dc.getFrequency());
+        assertTrue(callback.wasTriggered());
+        assertEquals(191, callback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void setOutputTest() {
+    HAL.initialize(500, 0);
+
+    try (DigitalInput di = new DigitalInput(2);
+        DutyCycle dc = new DutyCycle(di)) {
+      DoubleCallback callback = new DoubleCallback();
+      DutyCycleSim sim = new DutyCycleSim(dc);
+      try (CallbackStore cb = sim.registerOutputCallback(callback, false)) {
+        sim.setOutput(229.174);
+        assertEquals(229.174, sim.getOutput());
+        assertEquals(229.174, dc.getOutput());
+        assertTrue(callback.wasTriggered());
+        assertEquals(229.174, callback.getSetValue());
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
index c4d78d0..d655a90 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ElevatorSimTest.java
@@ -1,70 +1,80 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMVictorSPX;
+import org.junit.jupiter.api.Test;
+
 public class ElevatorSimTest {
   @Test
-  @SuppressWarnings("LocalVariableName")
+  @SuppressWarnings({"LocalVariableName", "resource"})
   public void testStateSpaceSimWithElevator() {
+    RoboRioSim.resetData();
 
     var controller = new PIDController(10, 0, 0);
 
-    var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 14.67, 8, 0.75 * 25.4 / 1000.0,
-        0.0, 3.0, VecBuilder.fill(0.01));
+    var sim =
+        new ElevatorSim(
+            DCMotor.getVex775Pro(4),
+            14.67,
+            8,
+            0.75 * 25.4 / 1000.0,
+            0.0,
+            3.0,
+            VecBuilder.fill(0.01));
 
-    var motor = new PWMVictorSPX(0);
-    var encoder = new Encoder(0, 1);
-    var encoderSim = new EncoderSim(encoder);
+    try (var motor = new PWMVictorSPX(0);
+        var encoder = new Encoder(0, 1)) {
+      var encoderSim = new EncoderSim(encoder);
 
-    for (int i = 0; i < 100; i++) {
-      controller.setSetpoint(2.0);
+      for (int i = 0; i < 100; i++) {
+        controller.setSetpoint(2.0);
 
-      double nextVoltage = controller.calculate(encoderSim.getDistance());
+        double nextVoltage = controller.calculate(encoderSim.getDistance());
 
-      double currentBatteryVoltage = RobotController.getBatteryVoltage();
-      motor.set(nextVoltage / currentBatteryVoltage);
+        double currentBatteryVoltage = RobotController.getBatteryVoltage();
+        motor.set(nextVoltage / currentBatteryVoltage);
 
-      // ------ SimulationPeriodic() happens after user code -------
+        // ------ SimulationPeriodic() happens after user code -------
 
-      var u = VecBuilder.fill(motor.get() * currentBatteryVoltage);
-      sim.setInput(u);
-      sim.update(0.020);
-      var y = sim.getOutput();
-      encoderSim.setDistance(y.get(0, 0));
+        var u = VecBuilder.fill(motor.get() * currentBatteryVoltage);
+        sim.setInput(u);
+        sim.update(0.020);
+        var y = sim.getOutput();
+        encoderSim.setDistance(y.get(0, 0));
+      }
+
+      assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2);
     }
-
-    assertEquals(controller.getSetpoint(), sim.getPositionMeters(), 0.2);
   }
 
   @Test
   public void testMinMax() {
-    var plant = LinearSystemId.createElevatorSystem(
-        DCMotor.getVex775Pro(4),
-        8.0,
-        0.75 * 25.4 / 1000.0,
-        14.67);
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
 
-    var sim = new ElevatorSim(plant,
-        DCMotor.getVex775Pro(4),
-        14.67, 0.75 * 25.4 / 1000.0, 0.0, 1.0, VecBuilder.fill(0.01));
+    var sim =
+        new ElevatorSim(
+            plant,
+            DCMotor.getVex775Pro(4),
+            14.67,
+            0.75 * 25.4 / 1000.0,
+            0.0,
+            1.0,
+            VecBuilder.fill(0.01));
 
     for (int i = 0; i < 100; i++) {
       sim.setInput(VecBuilder.fill(0));
@@ -80,4 +90,20 @@
       assertTrue(height <= 1.05);
     }
   }
+
+  @Test
+  public void testStability() {
+    var sim = new ElevatorSim(DCMotor.getVex775Pro(4), 100, 4, Units.inchesToMeters(0.5), 0, 10);
+
+    sim.setState(VecBuilder.fill(0, 0));
+    sim.setInput(12);
+    for (int i = 0; i < 50; i++) {
+      sim.update(0.02);
+    }
+
+    assertEquals(
+        sim.m_plant.calculateX(VecBuilder.fill(0, 0), VecBuilder.fill(12), 0.02 * 50.0).get(0, 0),
+        sim.getPositionMeters(),
+        0.1);
+  }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/EncoderSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/EncoderSimTest.java
new file mode 100644
index 0000000..931c0fe
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/EncoderSimTest.java
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class EncoderSimTest {
+  private static final double DEFAULT_DISTANCE_PER_PULSE = 0.0005;
+
+  @Test
+  void testRate() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      sim.setRate(1.91);
+      assertEquals(1.91, sim.getRate());
+    }
+  }
+
+  @Test
+  void testCount() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      IntCallback callback = new IntCallback();
+      try (CallbackStore cb = sim.registerCountCallback(callback, false)) {
+        sim.setCount(3504);
+        assertEquals(3504, sim.getCount());
+
+        assertTrue(callback.wasTriggered());
+        assertEquals(3504, encoder.get());
+        assertEquals(3504, callback.getSetValue());
+      }
+    }
+  }
+
+  @Test
+  void testDistance() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      sim.setDistance(229.174);
+      assertEquals(229.174, sim.getDistance());
+      assertEquals(229.174, encoder.getDistance());
+    }
+  }
+
+  @Test
+  void testPeriod() {
+    HAL.initialize(500, 0);
+
+    try (Encoder encoder = new Encoder(0, 1)) {
+      EncoderSim sim = new EncoderSim(encoder);
+      sim.resetData();
+
+      encoder.setDistancePerPulse(DEFAULT_DISTANCE_PER_PULSE);
+
+      DoubleCallback callback = new DoubleCallback();
+      try (CallbackStore cb = sim.registerPeriodCallback(callback, false)) {
+        sim.setPeriod(123.456);
+        assertEquals(123.456, sim.getPeriod());
+        assertEquals(123.456, encoder.getPeriod());
+        assertEquals(DEFAULT_DISTANCE_PER_PULSE / 123.456, encoder.getRate());
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java
new file mode 100644
index 0000000..2e76aa2
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/PWMSimTest.java
@@ -0,0 +1,135 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+class PWMSimTest {
+  @Test
+  void testInitialize() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      assertTrue(sim.getInitialized());
+    }
+  }
+
+  @Test
+  void testSetRawValue() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    IntCallback callback = new IntCallback();
+
+    try (CallbackStore cb = sim.registerRawValueCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      sim.setRawValue(229);
+      assertEquals(229, sim.getRawValue());
+      assertEquals(229, pwm.getRaw());
+      assertTrue(callback.wasTriggered());
+      assertEquals(229, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetSpeed() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    DoubleCallback callback = new DoubleCallback();
+
+    try (CallbackStore cb = sim.registerSpeedCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      final double kTestValue = 0.3504;
+      pwm.setSpeed(kTestValue);
+
+      assertEquals(kTestValue, sim.getSpeed());
+      assertEquals(kTestValue, pwm.getSpeed());
+      assertTrue(callback.wasTriggered());
+      assertEquals(kTestValue, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetPosition() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    DoubleCallback callback = new DoubleCallback();
+
+    try (CallbackStore cb = sim.registerPositionCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      final double kTestValue = 0.3504;
+      pwm.setPosition(kTestValue);
+
+      assertEquals(kTestValue, sim.getPosition());
+      assertEquals(kTestValue, pwm.getPosition());
+      assertTrue(callback.wasTriggered());
+      assertEquals(kTestValue, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetPeriodScale() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    IntCallback callback = new IntCallback();
+
+    try (CallbackStore cb = sim.registerPeriodScaleCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      sim.setPeriodScale(3504);
+      assertEquals(3504, sim.getPeriodScale());
+      assertTrue(callback.wasTriggered());
+      assertEquals(3504, callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetZeroLatch() {
+    HAL.initialize(500, 0);
+
+    PWMSim sim = new PWMSim(0);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerZeroLatchCallback(callback, false);
+        PWM pwm = new PWM(0)) {
+      pwm.setZeroLatch();
+
+      assertTrue(callback.wasTriggered());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/REVPHSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/REVPHSimTest.java
new file mode 100644
index 0000000..8bdf23e
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/REVPHSimTest.java
@@ -0,0 +1,168 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticHub;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings("AbbreviationAsWordInName")
+class REVPHSimTest {
+  @Test
+  void testInitialization() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    sim.resetData();
+    assertFalse(sim.getInitialized());
+
+    BooleanCallback callback = new BooleanCallback();
+
+    try (CallbackStore cb = sim.registerInitializedCallback(callback, false);
+        PneumaticHub ph = new PneumaticHub(1)) {
+      assertTrue(sim.getInitialized());
+    }
+    assertFalse(sim.getInitialized());
+  }
+
+  @Test
+  void solenoidOutputTest() {
+    HAL.initialize(500, 0);
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        DoubleSolenoid doubleSolenoid = new DoubleSolenoid(1, PneumaticsModuleType.REVPH, 3, 4)) {
+      REVPHSim sim = new REVPHSim(ph);
+      sim.resetData();
+
+      BooleanCallback callback3 = new BooleanCallback();
+      BooleanCallback callback4 = new BooleanCallback();
+
+      try (CallbackStore cb3 = sim.registerSolenoidOutputCallback(3, callback3, false);
+          CallbackStore cb4 = sim.registerSolenoidOutputCallback(4, callback4, false)) {
+        // Reverse
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kReverse);
+        assertFalse(callback3.wasTriggered());
+        assertNull(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertTrue(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertTrue(sim.getSolenoidOutput(4));
+        assertEquals(0x10, ph.getSolenoids());
+
+        // Forward
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kForward);
+        assertTrue(callback3.wasTriggered());
+        assertTrue(callback3.getSetValue());
+        assertTrue(callback4.wasTriggered());
+        assertFalse(callback4.getSetValue());
+        assertTrue(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x8, ph.getSolenoids());
+
+        // Off
+        callback3.reset();
+        callback4.reset();
+        doubleSolenoid.set(DoubleSolenoid.Value.kOff);
+        assertTrue(callback3.wasTriggered());
+        assertFalse(callback3.getSetValue());
+        assertFalse(callback4.wasTriggered());
+        assertNull(callback4.getSetValue());
+        assertFalse(sim.getSolenoidOutput(3));
+        assertFalse(sim.getSolenoidOutput(4));
+        assertEquals(0x0, ph.getSolenoids());
+      }
+    }
+  }
+
+  @Test
+  void setCompressorOnTest() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorOnCallback(callback, false)) {
+      assertFalse(ph.getCompressor());
+      assertFalse(sim.getCompressorOn());
+      sim.setCompressorOn(true);
+      assertTrue(ph.getCompressor());
+      assertTrue(sim.getCompressorOn());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setClosedLoopEnabled() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerClosedLoopEnabledCallback(callback, false)) {
+      ph.setClosedLoopControl(false);
+      assertFalse(ph.getClosedLoopControl());
+
+      ph.setClosedLoopControl(true);
+      assertTrue(sim.getClosedLoopEnabled());
+      assertTrue(ph.getClosedLoopControl());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setPressureSwitchEnabledTest() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    BooleanCallback callback = new BooleanCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerPressureSwitchCallback(callback, false)) {
+      assertFalse(ph.getPressureSwitch());
+
+      sim.setPressureSwitch(true);
+      assertTrue(sim.getPressureSwitch());
+      assertTrue(ph.getPressureSwitch());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void setCompressorCurrentTest() {
+    HAL.initialize(500, 0);
+
+    REVPHSim sim = new REVPHSim(1);
+    DoubleCallback callback = new DoubleCallback();
+
+    try (PneumaticHub ph = new PneumaticHub(1);
+        CallbackStore cb = sim.registerCompressorCurrentCallback(callback, false)) {
+      assertFalse(ph.getPressureSwitch());
+
+      sim.setCompressorCurrent(35.04);
+      assertEquals(35.04, sim.getCompressorCurrent());
+      assertEquals(35.04, ph.getCompressorCurrent());
+      assertTrue(callback.wasTriggered());
+      assertEquals(35.04, callback.getSetValue());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java
new file mode 100644
index 0000000..ba84fc5
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RelaySimTest.java
@@ -0,0 +1,258 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertNull;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import org.junit.jupiter.api.Test;
+
+class RelaySimTest {
+  @Test
+  void testInitializationBidrectional() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    sim.resetData();
+
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    assertFalse(sim.getInitializedForward());
+    assertFalse(sim.getInitializedReverse());
+
+    try (CallbackStore fwdCb = sim.registerInitializedForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerInitializedReverseCallback(reverseCallback, false);
+        Relay relay = new Relay(0)) {
+      assertTrue(sim.getInitializedForward());
+      assertTrue(sim.getInitializedReverse());
+
+      assertTrue(forwardCallback.wasTriggered());
+      assertTrue(forwardCallback.getSetValue());
+      assertTrue(reverseCallback.wasTriggered());
+      assertTrue(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testInitializationForwardOnly() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    sim.resetData();
+
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    assertFalse(sim.getInitializedForward());
+    assertFalse(sim.getInitializedReverse());
+
+    try (CallbackStore fwdCb = sim.registerInitializedForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerInitializedReverseCallback(reverseCallback, false);
+        Relay relay = new Relay(0, Relay.Direction.kForward); ) {
+      assertTrue(sim.getInitializedForward());
+      assertFalse(sim.getInitializedReverse());
+
+      assertTrue(forwardCallback.wasTriggered());
+      assertTrue(forwardCallback.getSetValue());
+      assertFalse(reverseCallback.wasTriggered());
+      assertNull(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testInitializationReverseOnly() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    sim.resetData();
+
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    assertFalse(sim.getInitializedForward());
+    assertFalse(sim.getInitializedReverse());
+
+    try (CallbackStore fwdCb = sim.registerInitializedForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerInitializedReverseCallback(reverseCallback, false);
+        Relay relay = new Relay(0, Relay.Direction.kReverse); ) {
+      assertFalse(sim.getInitializedForward());
+      assertTrue(sim.getInitializedReverse());
+
+      assertFalse(forwardCallback.wasTriggered());
+      assertNull(forwardCallback.getSetValue());
+      assertTrue(reverseCallback.wasTriggered());
+      assertTrue(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testBidirectionalSetForward() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    try (Relay relay = new Relay(0);
+        CallbackStore fwdCb = sim.registerForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerReverseCallback(reverseCallback, false)) {
+      relay.set(Relay.Value.kForward);
+      assertEquals(Relay.Value.kForward, relay.get());
+      assertTrue(sim.getForward());
+      assertFalse(sim.getReverse());
+
+      assertTrue(forwardCallback.wasTriggered());
+      assertTrue(forwardCallback.getSetValue());
+      assertFalse(reverseCallback.wasTriggered());
+      assertNull(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testBidirectionalSetReverse() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    try (Relay relay = new Relay(0);
+        CallbackStore fwdCb = sim.registerForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerReverseCallback(reverseCallback, false)) {
+      relay.set(Relay.Value.kReverse);
+      assertEquals(Relay.Value.kReverse, relay.get());
+      assertFalse(sim.getForward());
+      assertTrue(sim.getReverse());
+
+      assertFalse(forwardCallback.wasTriggered());
+      assertNull(forwardCallback.getSetValue());
+      assertTrue(reverseCallback.wasTriggered());
+      assertTrue(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testBidirectionalSetOn() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    try (Relay relay = new Relay(0);
+        CallbackStore fwdCb = sim.registerForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerReverseCallback(reverseCallback, false)) {
+      relay.set(Relay.Value.kOn);
+      assertEquals(Relay.Value.kOn, relay.get());
+      assertTrue(sim.getForward());
+      assertTrue(sim.getReverse());
+
+      assertTrue(forwardCallback.wasTriggered());
+      assertTrue(forwardCallback.getSetValue());
+      assertTrue(reverseCallback.wasTriggered());
+      assertTrue(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testBidirectionalSetOff() {
+    HAL.initialize(500, 0);
+
+    RelaySim sim = new RelaySim(0);
+    BooleanCallback forwardCallback = new BooleanCallback();
+    BooleanCallback reverseCallback = new BooleanCallback();
+
+    try (Relay relay = new Relay(0);
+        CallbackStore fwdCb = sim.registerForwardCallback(forwardCallback, false);
+        CallbackStore revCb = sim.registerReverseCallback(reverseCallback, false)) {
+      // Bootstrap into a non-off state to verify the callbacks
+      relay.set(Relay.Value.kOn);
+      forwardCallback.reset();
+      reverseCallback.reset();
+
+      relay.set(Relay.Value.kOff);
+      assertEquals(Relay.Value.kOff, relay.get());
+      assertFalse(sim.getForward());
+      assertFalse(sim.getReverse());
+
+      assertTrue(forwardCallback.wasTriggered());
+      assertFalse(forwardCallback.getSetValue());
+      assertTrue(reverseCallback.wasTriggered());
+      assertFalse(reverseCallback.getSetValue());
+    }
+  }
+
+  @Test
+  void testStopMotor() {
+    try (Relay relay = new Relay(0)) {
+      // Bootstrap into non-off state
+      relay.set(Relay.Value.kOn);
+
+      relay.stopMotor();
+      assertEquals(Relay.Value.kOff, relay.get());
+    }
+  }
+
+  @Test
+  void testForwardOnlyCannotGoReverse() {
+    try (Relay relay = new Relay(0, Relay.Direction.kForward)) {
+      relay.set(Relay.Value.kForward);
+      assertEquals(Relay.Value.kOn, relay.get());
+
+      relay.set(Relay.Value.kOff);
+      assertEquals(Relay.Value.kOff, relay.get());
+
+      assertThrows(Relay.InvalidValueException.class, () -> relay.set(Relay.Value.kReverse));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOn));
+    }
+  }
+
+  @Test
+  void testReverseOnlyCannotGoForwards() {
+    try (Relay relay = new Relay(0, Relay.Direction.kReverse)) {
+      relay.set(Relay.Value.kReverse);
+      assertEquals(Relay.Value.kOn, relay.get());
+
+      relay.set(Relay.Value.kOff);
+      assertEquals(Relay.Value.kOff, relay.get());
+
+      assertThrows(Relay.InvalidValueException.class, () -> relay.set(Relay.Value.kForward));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOn));
+    }
+  }
+
+  @Test
+  void testSwitchDirections() {
+    try (Relay relay = new Relay(0, Relay.Direction.kBoth)) {
+      // Start with both. Should be able to set all 4 values
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOff));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kForward));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kReverse));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOn));
+
+      // Switch it to forward only
+      relay.setDirection(Relay.Direction.kForward);
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOff));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kForward));
+      assertThrows(Relay.InvalidValueException.class, () -> relay.set(Relay.Value.kReverse));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOn));
+
+      // Switch it to Reverse only
+      relay.setDirection(Relay.Direction.kReverse);
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOff));
+      assertThrows(Relay.InvalidValueException.class, () -> relay.set(Relay.Value.kForward));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kReverse));
+      assertDoesNotThrow(() -> relay.set(Relay.Value.kOn));
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java
new file mode 100644
index 0000000..e9ea9f1
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/RoboRioSimTest.java
@@ -0,0 +1,211 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.hal.HALUtil;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.simulation.testutils.BooleanCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.DoubleCallback;
+import edu.wpi.first.wpilibj.simulation.testutils.IntCallback;
+import org.junit.jupiter.api.Test;
+
+public class RoboRioSimTest {
+  @Test
+  void testFPGAButton() {
+    RoboRioSim.resetData();
+
+    BooleanCallback callback = new BooleanCallback();
+    try (CallbackStore cb = RoboRioSim.registerFPGAButtonCallback(callback, false)) {
+      RoboRioSim.setFPGAButton(true);
+      assertTrue(RoboRioSim.getFPGAButton());
+      assertTrue(HALUtil.getFPGAButton());
+      assertTrue(callback.wasTriggered());
+      assertTrue(callback.getSetValue());
+
+      callback.reset();
+      RoboRioSim.setFPGAButton(false);
+      assertFalse(RoboRioSim.getFPGAButton());
+      assertFalse(HALUtil.getFPGAButton());
+      assertTrue(callback.wasTriggered());
+      assertFalse(callback.getSetValue());
+    }
+  }
+
+  @Test
+  void testSetVin() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    DoubleCallback currentCallback = new DoubleCallback();
+    try (CallbackStore voltageCb = RoboRioSim.registerVInVoltageCallback(voltageCallback, false);
+        CallbackStore currentCb = RoboRioSim.registerVInCurrentCallback(currentCallback, false)) {
+      final double kTestVoltage = 1.91;
+      final double kTestCurrent = 35.04;
+
+      RoboRioSim.setVInVoltage(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getVInVoltage());
+      assertEquals(kTestVoltage, RobotController.getInputVoltage());
+
+      RoboRioSim.setVInCurrent(kTestCurrent);
+      assertTrue(currentCallback.wasTriggered());
+      assertEquals(kTestCurrent, currentCallback.getSetValue());
+      assertEquals(kTestCurrent, RoboRioSim.getVInCurrent());
+      assertEquals(kTestCurrent, RobotController.getInputCurrent());
+    }
+  }
+
+  @Test
+  void testSetBrownout() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    try (CallbackStore voltageCb =
+        RoboRioSim.registerBrownoutVoltageCallback(voltageCallback, false)) {
+      final double kTestVoltage = 1.91;
+
+      RoboRioSim.setBrownoutVoltage(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getBrownoutVoltage());
+      assertEquals(kTestVoltage, RobotController.getBrownoutVoltage());
+    }
+  }
+
+  @Test
+  void test6V() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    DoubleCallback currentCallback = new DoubleCallback();
+    BooleanCallback activeCallback = new BooleanCallback();
+    IntCallback faultCallback = new IntCallback();
+    try (CallbackStore voltageCb =
+            RoboRioSim.registerUserVoltage6VCallback(voltageCallback, false);
+        CallbackStore currentCb = RoboRioSim.registerUserCurrent6VCallback(currentCallback, false);
+        CallbackStore activeCb = RoboRioSim.registerUserActive6VCallback(activeCallback, false);
+        CallbackStore faultsCb = RoboRioSim.registerUserFaults6VCallback(faultCallback, false)) {
+      final double kTestVoltage = 22.9;
+      final double kTestCurrent = 174;
+      final int kTestFaults = 229;
+
+      RoboRioSim.setUserVoltage6V(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getUserVoltage6V());
+      assertEquals(kTestVoltage, RobotController.getVoltage6V());
+
+      RoboRioSim.setUserCurrent6V(kTestCurrent);
+      assertTrue(currentCallback.wasTriggered());
+      assertEquals(kTestCurrent, currentCallback.getSetValue());
+      assertEquals(kTestCurrent, RoboRioSim.getUserCurrent6V());
+      assertEquals(kTestCurrent, RobotController.getCurrent6V());
+
+      RoboRioSim.setUserActive6V(false);
+      assertTrue(activeCallback.wasTriggered());
+      assertFalse(activeCallback.getSetValue());
+      assertFalse(RoboRioSim.getUserActive6V());
+      assertFalse(RobotController.getEnabled6V());
+
+      RoboRioSim.setUserFaults6V(kTestFaults);
+      assertTrue(faultCallback.wasTriggered());
+      assertEquals(kTestFaults, faultCallback.getSetValue());
+      assertEquals(kTestFaults, RoboRioSim.getUserFaults6V());
+      assertEquals(kTestFaults, RobotController.getFaultCount6V());
+    }
+  }
+
+  @Test
+  void test5V() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    DoubleCallback currentCallback = new DoubleCallback();
+    BooleanCallback activeCallback = new BooleanCallback();
+    IntCallback faultCallback = new IntCallback();
+    try (CallbackStore voltageCb =
+            RoboRioSim.registerUserVoltage5VCallback(voltageCallback, false);
+        CallbackStore currentCb = RoboRioSim.registerUserCurrent5VCallback(currentCallback, false);
+        CallbackStore activeCb = RoboRioSim.registerUserActive5VCallback(activeCallback, false);
+        CallbackStore faultsCb = RoboRioSim.registerUserFaults5VCallback(faultCallback, false)) {
+      final double kTestVoltage = 22.9;
+      final double kTestCurrent = 174;
+      final int kTestFaults = 229;
+
+      RoboRioSim.setUserVoltage5V(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getUserVoltage5V());
+      assertEquals(kTestVoltage, RobotController.getVoltage5V());
+
+      RoboRioSim.setUserCurrent5V(kTestCurrent);
+      assertTrue(currentCallback.wasTriggered());
+      assertEquals(kTestCurrent, currentCallback.getSetValue());
+      assertEquals(kTestCurrent, RoboRioSim.getUserCurrent5V());
+      assertEquals(kTestCurrent, RobotController.getCurrent5V());
+
+      RoboRioSim.setUserActive5V(false);
+      assertTrue(activeCallback.wasTriggered());
+      assertFalse(activeCallback.getSetValue());
+      assertFalse(RoboRioSim.getUserActive5V());
+      assertFalse(RobotController.getEnabled5V());
+
+      RoboRioSim.setUserFaults5V(kTestFaults);
+      assertTrue(faultCallback.wasTriggered());
+      assertEquals(kTestFaults, faultCallback.getSetValue());
+      assertEquals(kTestFaults, RoboRioSim.getUserFaults5V());
+      assertEquals(kTestFaults, RobotController.getFaultCount5V());
+    }
+  }
+
+  @Test
+  void test3V3() {
+    RoboRioSim.resetData();
+
+    DoubleCallback voltageCallback = new DoubleCallback();
+    DoubleCallback currentCallback = new DoubleCallback();
+    BooleanCallback activeCallback = new BooleanCallback();
+    IntCallback faultCallback = new IntCallback();
+    try (CallbackStore voltageCb =
+            RoboRioSim.registerUserVoltage3V3Callback(voltageCallback, false);
+        CallbackStore currentCb =
+            RoboRioSim.registerUserCurrent3V3Callback(currentCallback, false);
+        CallbackStore activeCb = RoboRioSim.registerUserActive3V3Callback(activeCallback, false);
+        CallbackStore faultsCb = RoboRioSim.registerUserFaults3V3Callback(faultCallback, false)) {
+      final double kTestVoltage = 22.9;
+      final double kTestCurrent = 174;
+      final int kTestFaults = 229;
+
+      RoboRioSim.setUserVoltage3V3(kTestVoltage);
+      assertTrue(voltageCallback.wasTriggered());
+      assertEquals(kTestVoltage, voltageCallback.getSetValue());
+      assertEquals(kTestVoltage, RoboRioSim.getUserVoltage3V3());
+      assertEquals(kTestVoltage, RobotController.getVoltage3V3());
+
+      RoboRioSim.setUserCurrent3V3(kTestCurrent);
+      assertTrue(currentCallback.wasTriggered());
+      assertEquals(kTestCurrent, currentCallback.getSetValue());
+      assertEquals(kTestCurrent, RoboRioSim.getUserCurrent3V3());
+      assertEquals(kTestCurrent, RobotController.getCurrent3V3());
+
+      RoboRioSim.setUserActive3V3(false);
+      assertTrue(activeCallback.wasTriggered());
+      assertFalse(activeCallback.getSetValue());
+      assertFalse(RoboRioSim.getUserActive3V3());
+      assertFalse(RobotController.getEnabled3V3());
+
+      RoboRioSim.setUserFaults3V3(kTestFaults);
+      assertTrue(faultCallback.wasTriggered());
+      assertEquals(kTestFaults, faultCallback.getSetValue());
+      assertEquals(kTestFaults, RoboRioSim.getUserFaults3V3());
+      assertEquals(kTestFaults, RobotController.getFaultCount3V3());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
index 6b9fe61..1daea96 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SimDeviceSimTest.java
@@ -1,31 +1,178 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
 
 import edu.wpi.first.hal.SimBoolean;
 import edu.wpi.first.hal.SimDevice;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimValue;
+import java.util.concurrent.atomic.AtomicInteger;
+import org.junit.jupiter.api.Test;
 
 class SimDeviceSimTest {
   @Test
   void testBasic() {
-    SimDevice dev = SimDevice.create("test");
-    SimBoolean devBool = dev.createBoolean("bool", false, false);
+    try (SimDevice dev = SimDevice.create("test")) {
+      SimBoolean devBool = dev.createBoolean("bool", Direction.kBidir, false);
 
-    SimDeviceSim sim = new SimDeviceSim("test");
-    SimBoolean simBool = sim.getBoolean("bool");
+      SimDeviceSim sim = new SimDeviceSim("test");
+      SimBoolean simBool = sim.getBoolean("bool");
 
-    assertFalse(simBool.get());
-    simBool.set(true);
-    assertTrue(devBool.get());
+      assertFalse(simBool.get());
+      simBool.set(true);
+      assertTrue(devBool.get());
+    }
+  }
+
+  @Test
+  void testDeviceCreatedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice otherdev = SimDevice.create("testnotDC");
+        SimDevice dev1 = SimDevice.create("testDC1")) {
+      try (CallbackStore callback1 =
+              SimDeviceSim.registerDeviceCreatedCallback(
+                  "testDC",
+                  (name, handle) -> {
+                    callback1Counter.addAndGet(1);
+                  },
+                  false);
+          CallbackStore callback2 =
+              SimDeviceSim.registerDeviceCreatedCallback(
+                  "testDC",
+                  (name, handle) -> {
+                    callback2Counter.addAndGet(1);
+                  },
+                  true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(
+            1,
+            callback2Counter.get(),
+            "Callback 2 called early or not initialized with existing devices");
+
+        SimDevice dev2 = SimDevice.create("testDC2");
+        dev2.close();
+
+        assertEquals(
+            1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+
+      SimDevice dev3 = SimDevice.create("testDC3");
+      dev3.close();
+
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
+  }
+
+  @Test
+  void testDeviceFreedCallback() {
+    AtomicInteger counter = new AtomicInteger(0);
+
+    SimDevice dev1 = SimDevice.create("testDF1");
+    try (CallbackStore callback =
+        SimDeviceSim.registerDeviceFreedCallback(
+            "testDF",
+            (name, handle) -> {
+              counter.addAndGet(1);
+            },
+            false)) {
+      assertEquals(0, counter.get(), "Callback called early");
+      dev1.close();
+      assertEquals(1, counter.get(), "Callback called either more than once or not at all");
+    }
+
+    SimDevice dev2 = SimDevice.create("testDF2");
+    dev2.close();
+
+    assertEquals(1, counter.get(), "Callback called after closure");
+  }
+
+  @Test
+  void testValueCreatedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice dev1 = SimDevice.create("testVM1")) {
+      dev1.createBoolean("v1", Direction.kBidir, false);
+      SimDeviceSim sim = new SimDeviceSim("testVM1");
+      try (CallbackStore callback1 =
+              sim.registerValueCreatedCallback(
+                  (name, handle, readonly, value) -> {
+                    callback1Counter.addAndGet(1);
+                  },
+                  false);
+          CallbackStore callback2 =
+              sim.registerValueCreatedCallback(
+                  (name, handle, readonly, value) -> {
+                    callback2Counter.addAndGet(1);
+                  },
+                  true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(
+            1,
+            callback2Counter.get(),
+            "Callback 2 called early or not initialized with existing devices");
+
+        dev1.createDouble("v2", Direction.kBidir, 0);
+
+        assertEquals(
+            1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+      dev1.createBoolean("v3", Direction.kBidir, false);
+
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
+  }
+
+  @Test
+  void testValueChangedCallback() {
+    AtomicInteger callback1Counter = new AtomicInteger(0);
+    AtomicInteger callback2Counter = new AtomicInteger(0);
+
+    try (SimDevice dev1 = SimDevice.create("testVM1")) {
+      SimBoolean val = dev1.createBoolean("v1", Direction.kBidir, false);
+      SimDeviceSim sim = new SimDeviceSim("testVM1");
+      SimValue simVal = sim.getValue("v1");
+      try (CallbackStore callback1 =
+              sim.registerValueChangedCallback(
+                  simVal,
+                  (name, handle, readonly, value) -> {
+                    callback1Counter.addAndGet(1);
+                  },
+                  false);
+          CallbackStore callback2 =
+              sim.registerValueChangedCallback(
+                  simVal,
+                  (name, handle, readonly, value) -> {
+                    callback2Counter.addAndGet(1);
+                  },
+                  true)) {
+        assertEquals(0, callback1Counter.get(), "Callback 1 called early");
+        assertEquals(
+            1,
+            callback2Counter.get(),
+            "Callback 2 called early or not initialized with existing devices");
+
+        val.set(true);
+
+        assertEquals(
+            1, callback1Counter.get(), "Callback 1 called either more than once or not at all");
+        assertEquals(2, callback2Counter.get(), "Callback 2 called either more or less than twice");
+      }
+      val.set(false);
+      assertEquals(1, callback1Counter.get(), "Callback 1 called after closure");
+      assertEquals(2, callback2Counter.get(), "Callback 2 called after closure");
+    }
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
index f1b4bc8..26aa6b9 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/SingleJointedArmSimTest.java
@@ -1,28 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.simulation;
 
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
 public class SingleJointedArmSimTest {
-  SingleJointedArmSim m_sim = new SingleJointedArmSim(
-      DCMotor.getVex775Pro(2),
-      100,
-      3.0,
-      Units.inchesToMeters(19.0 / 2.0),
-      -Math.PI,
-      0.0, 10.0 / 2.2, true);
+  SingleJointedArmSim m_sim =
+      new SingleJointedArmSim(
+          DCMotor.getVex775Pro(2),
+          100,
+          3.0,
+          Units.inchesToMeters(19.0 / 2.0),
+          -Math.PI,
+          0.0,
+          10.0 / 2.2,
+          true);
 
   @Test
   public void testArmDisabled() {
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/BooleanCallback.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/BooleanCallback.java
new file mode 100644
index 0000000..f368d7f
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/BooleanCallback.java
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class BooleanCallback extends CallbackHelperBase<Boolean> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kBoolean) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = value.getBoolean();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/BufferCallback.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/BufferCallback.java
new file mode 100644
index 0000000..fdd8c06
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/BufferCallback.java
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation.testutils;
+
+import edu.wpi.first.hal.simulation.ConstBufferCallback;
+import java.util.Arrays;
+
+public class BufferCallback implements ConstBufferCallback {
+  private boolean m_wasTriggered;
+  private byte[] m_setValue;
+
+  @Override
+  public void callback(String name, byte[] buffer, int count) {
+    m_wasTriggered = true;
+    m_setValue = Arrays.copyOf(buffer, buffer.length);
+  }
+
+  public boolean wasTriggered() {
+    return m_wasTriggered;
+  }
+
+  public byte[] getSetValue() {
+    return Arrays.copyOf(m_setValue, m_setValue.length);
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/CallbackHelperBase.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/CallbackHelperBase.java
new file mode 100644
index 0000000..4fb455e
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/CallbackHelperBase.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation.testutils;
+
+import edu.wpi.first.hal.simulation.NotifyCallback;
+
+public abstract class CallbackHelperBase<T> implements NotifyCallback {
+  protected boolean m_wasTriggered;
+  protected T m_setValue;
+
+  public final boolean wasTriggered() {
+    return m_wasTriggered;
+  }
+
+  public final T getSetValue() {
+    return m_setValue;
+  }
+
+  public final void reset() {
+    m_wasTriggered = false;
+    m_setValue = null;
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/DoubleCallback.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/DoubleCallback.java
new file mode 100644
index 0000000..ccf53f3
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/DoubleCallback.java
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class DoubleCallback extends CallbackHelperBase<Double> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kDouble) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = value.getDouble();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/EnumCallback.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/EnumCallback.java
new file mode 100644
index 0000000..2c585ac
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/EnumCallback.java
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class EnumCallback extends CallbackHelperBase<Long> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kEnum) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = value.getLong();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/IntCallback.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/IntCallback.java
new file mode 100644
index 0000000..a1f65c4
--- /dev/null
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/testutils/IntCallback.java
@@ -0,0 +1,19 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.simulation.testutils;
+
+import edu.wpi.first.hal.HALValue;
+
+public class IntCallback extends CallbackHelperBase<Integer> {
+  @Override
+  public void callback(String name, HALValue value) {
+    if (value.getType() != HALValue.kInt) {
+      throw new IllegalArgumentException("Wrong callback for type " + value.getType());
+    }
+
+    m_wasTriggered = true;
+    m_setValue = (int) value.getLong();
+  }
+}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
index cc92ed0..dfefddb 100644
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
+++ b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/smartdashboard/SmartDashboardTest.java
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.smartdashboard;
 
-import org.junit.jupiter.api.BeforeEach;
-import org.junit.jupiter.api.Test;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.wpilibj.UtilityClassTest;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
 
 class SmartDashboardTest extends UtilityClassTest<SmartDashboard> {
   private final NetworkTable m_table = NetworkTableInstance.getDefault().getTable("SmartDashboard");
@@ -107,13 +103,14 @@
 
   @Test
   void putStringNullKeyTest() {
-    assertThrows(NullPointerException.class,
-        () -> SmartDashboard.putString(null, "This should not work"));
+    assertThrows(
+        NullPointerException.class, () -> SmartDashboard.putString(null, "This should not work"));
   }
 
   @Test
   void putStringNullValueTest() {
-    assertThrows(NullPointerException.class,
+    assertThrows(
+        NullPointerException.class,
         () -> SmartDashboard.putString("KEY_SHOULD_NOT_BE_STORED", null));
   }
 }
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
deleted file mode 100644
index 231eab6..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ColorTest.java
+++ /dev/null
@@ -1,50 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.util;
-
-import java.util.stream.Stream;
-
-import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.Arguments;
-import org.junit.jupiter.params.provider.MethodSource;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.params.provider.Arguments.arguments;
-
-class ColorTest {
-  private static final double kEpsilon = 1e-3;
-
-  void assertColorMatches(double red, double green, double blue, Color color) {
-    assertAll(
-        () -> assertEquals(red, color.red, kEpsilon),
-        () -> assertEquals(green, color.green, kEpsilon),
-        () -> assertEquals(blue, color.blue, kEpsilon)
-    );
-  }
-
-  @ParameterizedTest
-  @MethodSource("staticColorProvider")
-  void staticColorTest(double red, double green, double blue, Color color) {
-    assertColorMatches(red, green, blue, color);
-  }
-
-  @ParameterizedTest
-  @MethodSource("staticColorProvider")
-  void colorEqualsTest(double red, double green, double blue, Color color) {
-    assertEquals(color, new Color(red, green, blue));
-  }
-
-  static Stream<Arguments> staticColorProvider() {
-    return Stream.of(
-      arguments(0.0823529412, 0.376470589, 0.7411764706, Color.kDenim),
-      arguments(0.0, 0.4, 0.7019607844, Color.kFirstBlue),
-      arguments(0.9294117648, 0.1098039216, 0.1411764706, Color.kFirstRed)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java b/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java
deleted file mode 100644
index 839e2e3..0000000
--- a/third_party/allwpilib/wpilibj/src/test/java/edu/wpi/first/wpilibj/util/ErrorMessagesTest.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.util;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.UtilityClassTest;
-
-import static edu.wpi.first.wpilibj.util.ErrorMessages.requireNonNullParam;
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-
-class ErrorMessagesTest extends UtilityClassTest<ErrorMessages> {
-  ErrorMessagesTest() {
-    super(ErrorMessages.class);
-  }
-
-  @Test
-  void requireNonNullParamNullTest() {
-    assertThrows(NullPointerException.class, () -> requireNonNullParam(null, "testParam",
-        "testMethod"));
-  }
-
-  @Test
-  void requireNonNullParamNotNullTest() {
-    assertDoesNotThrow(() -> requireNonNullParam("null", "testParam", "testMethod"));
-  }
-}
diff --git a/third_party/allwpilib/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini b/third_party/allwpilib/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
index a685de0..f271944 100644
--- a/third_party/allwpilib/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
+++ b/third_party/allwpilib/wpilibj/src/test/resources/edu/wpi/first/wpilibj/PreferencesTestDefault.ini
@@ -1,7 +1,8 @@
 [NetworkTables Storage 3.0]
 double "/Preferences/checkedValueInt"=2
+; The omission of a leading zero is intentional for testing purposes
 double "/Preferences/checkedValueDouble"=.2
-double "/Preferences/checkedValueFloat"=3.14
+double "/Preferences/checkedValueFloat"=3.4
 double "/Preferences/checkedValueLong"=172
 string "/Preferences/checkedValueString"="Hello. How are you?"
 boolean "/Preferences/checkedValueBoolean"=false
diff --git a/third_party/allwpilib/wpilibjExamples/build.gradle b/third_party/allwpilib/wpilibjExamples/build.gradle
index 73185ba..518b1e8 100644
--- a/third_party/allwpilib/wpilibjExamples/build.gradle
+++ b/third_party/allwpilib/wpilibjExamples/build.gradle
@@ -1,4 +1,7 @@
+import edu.wpi.first.toolchain.NativePlatforms
+
 apply plugin: 'java'
+apply plugin: 'jacoco'
 
 ext {
     useJava = true
@@ -8,7 +11,19 @@
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
+test {
+    useJUnitPlatform()
+    systemProperty 'junit.jupiter.extensions.autodetection.enabled', 'true'
+    testLogging {
+        events "failed"
+        exceptionFormat "full"
+    }
+    finalizedBy jacocoTestReport
+}
 
+if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxraspbian') || project.hasProperty('onlylinuxaarch64bionic')) {
+    test.enabled = false
+}
 
 dependencies {
     implementation project(':wpilibj')
@@ -21,9 +36,24 @@
     implementation project(':cameraserver')
     implementation project(':wpilibOldCommands')
     implementation project(':wpilibNewCommands')
+
+    testImplementation 'org.junit.jupiter:junit-jupiter-api:5.4.2'
+    testImplementation 'org.junit.jupiter:junit-jupiter-params:5.4.2'
+    testRuntimeOnly 'org.junit.jupiter:junit-jupiter-engine:5.4.2'
 }
 
-if (!project.hasProperty('skipPMD')) {
+jacoco {
+    toolVersion = "0.8.4"
+}
+
+jacocoTestReport {
+    reports {
+        xml.required = true
+        html.required = true
+    }
+}
+
+if (!project.hasProperty('skipJavaFormat')) {
     apply plugin: 'pmd'
 
     pmd {
@@ -55,6 +85,116 @@
     commandFile = new File("$projectDir/src/main/java/edu/wpi/first/wpilibj/commands/commands.json")
 }
 
+apply plugin: 'cpp'
+apply plugin: 'edu.wpi.first.NativeUtils'
+
+apply from: '../shared/config.gradle'
+
+
+model {
+    components {
+        wpilibjExamplesDev(NativeExecutableSpec) {
+            targetBuildTypes 'debug'
+            sources {
+                cpp {
+                    source {
+                        srcDirs 'src/dev/native/cpp'
+                        include '**/*.cpp'
+                    }
+                    exportedHeaders {
+                        srcDirs 'src/dev/native/include'
+                    }
+                }
+            }
+            binaries.all { binary ->
+                lib project: ':wpilibNewCommands', library: 'wpilibNewCommands', linkage: 'shared'
+                lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'shared'
+                lib project: ':wpimath', library: 'wpimathJNI', linkage: 'shared'
+                lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
+                lib project: ':cscore', library: 'cscore', linkage: 'shared'
+                lib project: ':ntcore', library: 'ntcoreJNIShared', linkage: 'shared'
+                lib project: ':cscore', library: 'cscoreJNIShared', linkage: 'shared'
+                project(':hal').addHalDependency(binary, 'shared')
+                project(':hal').addHalJniDependency(binary)
+                lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
+                lib project: ':wpiutil', library: 'wpiutilJNI', linkage: 'shared'
+                lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
+                if (binary.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
+                    nativeUtils.useRequiredLibrary(binary, 'ni_link_libraries', 'ni_runtime_libraries')
+                } else {
+                    def systemArch = getCurrentArch()
+                    if (binary.targetPlatform.name == systemArch) {
+                        lib project: ":simulation:halsim_gui", library: 'halsim_gui', linkage: 'shared'
+                    }
+                }
+                nativeUtils.useRequiredLibrary(binary, 'opencv_shared')
+            }
+        }
+    }
+    tasks {
+        def c = $.components
+        def found = false
+        c.each {
+            if (it in NativeExecutableSpec && it.name == "wpilibjExamplesDev") {
+                it.binaries.each {
+                    if (!found) {
+                        def arch = it.targetPlatform.name
+                        if (arch == NativePlatforms.desktop) {
+                            found = true
+                            def filePath = it.tasks.install.installDirectory.get().toString() + File.separatorChar + 'lib'
+
+                            def doFirstTask = { task ->
+                                def extensions = ''
+                                it.tasks.install.installDirectory.get().getAsFile().eachFileRecurse {
+                                    def name = it.name
+                                    if (!(name.endsWith('.dll') || name.endsWith('.so') || name.endsWith('.dylib'))) {
+                                        return
+                                    }
+                                    def file = it
+                                    if (name.startsWith("halsim_gui") || name.startsWith("libhalsim_gui".toString())) {
+                                        extensions += file.absolutePath + File.pathSeparator
+                                    }
+                                }
+                                if (extensions != '') {
+                                    task.environment 'HALSIM_EXTENSIONS', extensions
+                                }
+                            }
+
+                            project.tasks.create("runCpp", Exec) { task ->
+                                dependsOn it.tasks.install
+                                commandLine it.tasks.install.runScriptFile.get().asFile.toString()
+                                test.dependsOn it.tasks.install
+                                test.systemProperty 'java.library.path', filePath
+                                test.environment 'LD_LIBRARY_PATH', filePath
+                                test.workingDir filePath
+                            }
+
+                            new groovy.json.JsonSlurper().parseText(exampleFile.text).each { entry ->
+                                project.tasks.create("run${entry.foldername}", JavaExec) { run ->
+                                    mainClass = "edu.wpi.first.wpilibj.examples." + entry.foldername + ".Main"
+                                    classpath = sourceSets.main.runtimeClasspath
+                                    run.dependsOn it.tasks.install
+                                    run.systemProperty 'java.library.path', filePath
+                                    run.environment 'LD_LIBRARY_PATH', filePath
+                                    run.workingDir filePath
+                                    doFirst { doFirstTask(run) }
+
+                                    if (org.gradle.internal.os.OperatingSystem.current().isMacOsX()) {
+                                        run.jvmArgs = ['-XstartOnFirstThread']
+                                    }
+                                }
+                            }
+
+                            found = true
+                        }
+                    }
+                }
+            }
+        }
+    }
+}
+
 ext {
     isCppCommands = false
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpilibjExamples/src/dev/native/cpp/main.cpp
new file mode 100644
index 0000000..a3e363e
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/dev/native/cpp/main.cpp
@@ -0,0 +1,5 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+int main() {}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java
index c17ea2d..5ca1ca1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command/ReplaceMeCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.command;
 
@@ -17,13 +14,11 @@
 
   // Called just before this Command runs the first time
   @Override
-  protected void initialize() {
-  }
+  protected void initialize() {}
 
   // Called repeatedly when this Command is scheduled to run
   @Override
-  protected void execute() {
-  }
+  protected void execute() {}
 
   // Make this return true when this Command no longer needs to run execute()
   @Override
@@ -33,12 +28,10 @@
 
   // Called once after isFinished returns true
   @Override
-  protected void end() {
-  }
+  protected void end() {}
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
-  protected void interrupted() {
-  }
+  protected void interrupted() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
index 5d3a501..a9a96ef 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/command2/ReplaceMeCommand.java
@@ -1,36 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.command2;
 
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class ReplaceMeCommand extends CommandBase {
-  /**
-   * Creates a new ReplaceMeCommand.
-   */
+  /** Creates a new ReplaceMeCommand. */
   public ReplaceMeCommand() {
     // Use addRequirements() here to declare subsystem dependencies.
   }
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {
-  }
+  public void end(boolean interrupted) {}
 
   // Returns true when the command should end.
   @Override
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java
index d1dae8c..ba8e273 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commandgroup/ReplaceMeCommandGroup.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.commandgroup;
 
 import edu.wpi.first.wpilibj.command.CommandGroup;
 
 public class ReplaceMeCommandGroup extends CommandGroup {
-  /**
-   * Add your docs here.
-   */
+  /** Add your docs here. */
   public ReplaceMeCommandGroup() {
     // Add Commands here:
     // e.g. addSequential(new Command1());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
index a26d15b..8165a3c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/commands.json
@@ -202,7 +202,7 @@
   },
   {
     "name": "TrapezoidProfileSubsystem (New)",
-    "description": "A command that executes a trapezoidal motion profile.",
+    "description": "A subystem that executes a trapezoidal motion profile.",
     "tags": [
       "trapezoidprofilesubsystem"
     ],
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
index e2d866e..2e8a5ff 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/emptyclass/ReplaceMeEmptyClass.java
@@ -1,14 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.emptyclass;
 
-/**
- * Add your docs here.
- */
-public class ReplaceMeEmptyClass {
-}
+/** Add your docs here. */
+public class ReplaceMeEmptyClass {}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java
index bf24a14..769b595 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instant/ReplaceMeInstantCommand.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.instant;
 
 import edu.wpi.first.wpilibj.command.InstantCommand;
 
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
 public class ReplaceMeInstantCommand extends InstantCommand {
-  /**
-   * Add your docs here.
-   */
+  /** Add your docs here. */
   public ReplaceMeInstantCommand() {
     super();
     // Use requires() here to declare subsystem dependencies
@@ -24,7 +17,5 @@
 
   // Called once when the command executes
   @Override
-  protected void initialize() {
-  }
-
+  protected void initialize() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
index 4a8b0fc..fe7fe9b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/instantcommand/ReplaceMeInstantCommand.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.instantcommand;
 
@@ -19,6 +16,5 @@
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-  }
+  public void initialize() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
index 92958b3..ab4d0cc 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelcommandgroup/ReplaceMeParallelCommandGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.parallelcommandgroup;
 
@@ -13,11 +10,10 @@
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeParallelCommandGroup extends ParallelCommandGroup {
-  /**
-   * Creates a new ReplaceMeParallelCommandGroup.
-   */
+  /** Creates a new ReplaceMeParallelCommandGroup. */
   public ReplaceMeParallelCommandGroup() {
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());super();
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands();
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
index 82c000e..95c902d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/paralleldeadlinegroup/ReplaceMeParallelDeadlineGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.paralleldeadlinegroup;
 
@@ -14,13 +11,11 @@
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeParallelDeadlineGroup extends ParallelDeadlineGroup {
-  /**
-   * Creates a new ReplaceMeParallelDeadlineGroup.
-   */
+  /** Creates a new ReplaceMeParallelDeadlineGroup. */
   public ReplaceMeParallelDeadlineGroup() {
-    // Add your commands in the super() call.  Add the deadline first.
-    super(
-        new InstantCommand()
-    );
+    // Add the deadline command in the super() call. Add other commands using
+    // addCommands().
+    super(new InstantCommand());
+    // addCommands(new FooCommand(), new BarCommand());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
index f710ee4..3de0c7f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/parallelracegroup/ReplaceMeParallelRaceGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.parallelracegroup;
 
@@ -13,12 +10,10 @@
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeParallelRaceGroup extends ParallelRaceGroup {
-  /**
-   * Creates a new ReplaceMeParallelRaceGroup.
-   */
+  /** Creates a new ReplaceMeParallelRaceGroup. */
   public ReplaceMeParallelRaceGroup() {
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());
-    super();
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands();
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
index db1584b..ff39b12 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidcommand/ReplaceMePIDCommand.java
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.pidcommand;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMePIDCommand extends PIDCommand {
-  /**
-   * Creates a new ReplaceMePIDCommand.
-   */
+  /** Creates a new ReplaceMePIDCommand. */
   public ReplaceMePIDCommand() {
     super(
         // The controller that the command will use
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java
index 46d75f6..01e7987 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem/ReplaceMePIDSubsystem.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.pidsubsystem;
 
 import edu.wpi.first.wpilibj.command.PIDSubsystem;
 
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
 public class ReplaceMePIDSubsystem extends PIDSubsystem {
-  /**
-   * Add your docs here.
-   */
+  /** Add your docs here. */
   public ReplaceMePIDSubsystem() {
     // Intert a subsystem name and PID values here
     super("SubsystemName", 1, 2, 3);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
index 7d5e7b3..0876d94 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/pidsubsystem2/ReplaceMePIDSubsystem.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.pidsubsystem2;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 
 public class ReplaceMePIDSubsystem extends PIDSubsystem {
-  /**
-   * Creates a new ReplaceMePIDSubsystem.
-   */
+  /** Creates a new ReplaceMePIDSubsystem. */
   public ReplaceMePIDSubsystem() {
     super(
         // The PIDController used by the subsystem
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
index 1fc4c2d..f3d4410 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidcommand/ReplaceMeProfiledPIDCommand.java
@@ -1,29 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.profiledpidcommand;
 
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeProfiledPIDCommand extends ProfiledPIDCommand {
-  /**
-   * Creates a new ReplaceMeProfiledPIDCommand.
-   */
+  /** Creates a new ReplaceMeProfiledPIDCommand. */
   public ReplaceMeProfiledPIDCommand() {
     super(
         // The ProfiledPIDController used by the command
         new ProfiledPIDController(
             // The PID gains
-            0, 0, 0,
+            0,
+            0,
+            0,
             // The motion profile constraints
             new TrapezoidProfile.Constraints(0, 0)),
         // This should return the measurement
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
index b380d4e..60eda35 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/profiledpidsubsystem/ReplaceMeProfiledPIDSubsystem.java
@@ -1,26 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.profiledpidsubsystem;
 
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 
 public class ReplaceMeProfiledPIDSubsystem extends ProfiledPIDSubsystem {
-  /**
-   * Creates a new ReplaceMeProfiledPIDSubsystem.
-   */
+  /** Creates a new ReplaceMeProfiledPIDSubsystem. */
   public ReplaceMeProfiledPIDSubsystem() {
     super(
         // The ProfiledPIDController used by the subsystem
-        new ProfiledPIDController(0, 0, 0,
-                                  // The motion profile constraints
-                                  new TrapezoidProfile.Constraints(0, 0)));
+        new ProfiledPIDController(
+            0,
+            0,
+            0,
+            // The motion profile constraints
+            new TrapezoidProfile.Constraints(0, 0)));
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
index 1038b04..ca4488b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/sequentialcommandgroup/ReplaceMeSequentialCommandGroup.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.sequentialcommandgroup;
 
@@ -13,12 +10,10 @@
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeSequentialCommandGroup extends SequentialCommandGroup {
-  /**
-   * Creates a new ReplaceMeSequentialCommandGroup.
-   */
+  /** Creates a new ReplaceMeSequentialCommandGroup. */
   public ReplaceMeSequentialCommandGroup() {
-    // Add your commands in the super() call, e.g.
-    // super(new FooCommand(), new BarCommand());
-    super();
+    // Add your commands in the addCommands() call, e.g.
+    // addCommands(new FooCommand(), new BarCommand());
+    addCommands();
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java
index 1c7fe6d..671daf1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem/ReplaceMeSubsystem.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.subsystem;
 
 import edu.wpi.first.wpilibj.command.Subsystem;
 
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
 public class ReplaceMeSubsystem extends Subsystem {
   // Put methods for controlling this subsystem
   // here. Call these from Commands.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
index 0fa5838..f72539d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/subsystem2/ReplaceMeSubsystem.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.subsystem2;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class ReplaceMeSubsystem extends SubsystemBase {
-  /**
-   * Creates a new ReplaceMeSubsystem.
-   */
-  public ReplaceMeSubsystem() {
-
-  }
+  /** Creates a new ReplaceMeSubsystem. */
+  public ReplaceMeSubsystem() {}
 
   @Override
   public void periodic() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java
index 41ba965..6021382 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/timed/ReplaceMeTimedCommand.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.timed;
 
 import edu.wpi.first.wpilibj.command.TimedCommand;
 
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
 public class ReplaceMeTimedCommand extends TimedCommand {
-  /**
-   * Add your docs here.
-   */
+  /** Add your docs here. */
   public ReplaceMeTimedCommand(double timeout) {
     super(timeout);
     // Use requires() here to declare subsystem dependencies
@@ -24,22 +17,18 @@
 
   // Called just before this Command runs the first time
   @Override
-  protected void initialize() {
-  }
+  protected void initialize() {}
 
   // Called repeatedly when this Command is scheduled to run
   @Override
-  protected void execute() {
-  }
+  protected void execute() {}
 
   // Called once after timeout
   @Override
-  protected void end() {
-  }
+  protected void end() {}
 
   // Called when another command which requires one or more of the same
   // subsystems is scheduled to run
   @Override
-  protected void interrupted() {
-  }
+  protected void interrupted() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
index 646817f..7a55719 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilecommand/ReplaceMeTrapezoidProfileCommand.java
@@ -1,22 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.trapezoidprofilecommand;
 
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
 // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class ReplaceMeTrapezoidProfileCommand extends TrapezoidProfileCommand {
-  /**
-   * Creates a new ReplaceMeTrapezoidProfileCommand.
-   */
+  /** Creates a new ReplaceMeTrapezoidProfileCommand. */
   public ReplaceMeTrapezoidProfileCommand() {
     super(
         // The motion profile to be executed
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
index 46f1b44..e95f957 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trapezoidprofilesubsystem/ReplaceMeTrapezoidProfileSubsystem.java
@@ -1,19 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.trapezoidprofilesubsystem;
 
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
 
 public class ReplaceMeTrapezoidProfileSubsystem extends TrapezoidProfileSubsystem {
-  /**
-   * Creates a new ReplaceMeTrapezoidProfileSubsystem.
-   */
+  /** Creates a new ReplaceMeTrapezoidProfileSubsystem. */
   public ReplaceMeTrapezoidProfileSubsystem() {
     super(
         // The constraints for the generated profiles
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java
index 713ac6d..c1343d9 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/commands/trigger/ReplaceMeTrigger.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.commands.trigger;
 
 import edu.wpi.first.wpilibj.buttons.Trigger;
 
-/**
- * Add your docs here.
- */
+/** Add your docs here. */
 public class ReplaceMeTrigger extends Trigger {
   @Override
   public boolean get() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
index 2feaca2..be4ecff 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Main.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.addressableled;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
-  private Main() {
-  }
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.addressableled;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
index a26031b..5995cd0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/addressableled/Robot.java
@@ -1,59 +1,56 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.addressableled;
-
-import edu.wpi.first.wpilibj.AddressableLED;
-import edu.wpi.first.wpilibj.AddressableLEDBuffer;
-import edu.wpi.first.wpilibj.TimedRobot;
-
-public class Robot extends TimedRobot {
-  private AddressableLED m_led;
-  private AddressableLEDBuffer m_ledBuffer;
-  // Store what the last hue of the first pixel is
-  private int m_rainbowFirstPixelHue;
-
-  @Override
-  public void robotInit() {
-    // PWM port 9
-    // Must be a PWM header, not MXP or DIO
-    m_led = new AddressableLED(9);
-
-    // Reuse buffer
-    // Default to a length of 60, start empty output
-    // Length is expensive to set, so only set it once, then just update data
-    m_ledBuffer = new AddressableLEDBuffer(60);
-    m_led.setLength(m_ledBuffer.getLength());
-
-    // Set the data
-    m_led.setData(m_ledBuffer);
-    m_led.start();
-  }
-
-  @Override
-  public void robotPeriodic() {
-    // Fill the buffer with a rainbow
-    rainbow();
-    // Set the LEDs
-    m_led.setData(m_ledBuffer);
-  }
-
-  private void rainbow() {
-    // For every pixel
-    for (var i = 0; i < m_ledBuffer.getLength(); i++) {
-      // Calculate the hue - hue is easier for rainbows because the color
-      // shape is a circle so only one value needs to precess
-      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
-      // Set the value
-      m_ledBuffer.setHSV(i, hue, 255, 128);
-    }
-    // Increase by to make the rainbow "move"
-    m_rainbowFirstPixelHue += 3;
-    // Check bounds
-    m_rainbowFirstPixelHue %= 180;
-  }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.addressableled;
+
+import edu.wpi.first.wpilibj.AddressableLED;
+import edu.wpi.first.wpilibj.AddressableLEDBuffer;
+import edu.wpi.first.wpilibj.TimedRobot;
+
+public class Robot extends TimedRobot {
+  private AddressableLED m_led;
+  private AddressableLEDBuffer m_ledBuffer;
+  // Store what the last hue of the first pixel is
+  private int m_rainbowFirstPixelHue;
+
+  @Override
+  public void robotInit() {
+    // PWM port 9
+    // Must be a PWM header, not MXP or DIO
+    m_led = new AddressableLED(9);
+
+    // Reuse buffer
+    // Default to a length of 60, start empty output
+    // Length is expensive to set, so only set it once, then just update data
+    m_ledBuffer = new AddressableLEDBuffer(60);
+    m_led.setLength(m_ledBuffer.getLength());
+
+    // Set the data
+    m_led.setData(m_ledBuffer);
+    m_led.start();
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Fill the buffer with a rainbow
+    rainbow();
+    // Set the LEDs
+    m_led.setData(m_ledBuffer);
+  }
+
+  private void rainbow() {
+    // For every pixel
+    for (var i = 0; i < m_ledBuffer.getLength(); i++) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      final var hue = (m_rainbowFirstPixelHue + (i * 180 / m_ledBuffer.getLength())) % 180;
+      // Set the value
+      m_ledBuffer.setHSV(i, hue, 255, 128);
+    }
+    // Increase by to make the rainbow "move"
+    m_rainbowFirstPixelHue += 3;
+    // Check bounds
+    m_rainbowFirstPixelHue %= 180;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
index 438697e..b451bc3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.arcadedrive;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
index f002618..e35243d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrive/Robot.java
@@ -1,24 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.arcadedrive;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a demo program showing the use of the DifferentialDrive class.
- * Runs the motors with arcade steering.
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with
+ * arcade steering.
  */
 public class Robot extends TimedRobot {
-  private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
-  private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
   private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
   private final Joystick m_stick = new Joystick(0);
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
index fb5c2ab..2088c7b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
index 915d77e..0acc8dc 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/arcadedrivexboxcontroller/Robot.java
@@ -1,25 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.arcadedrivexboxcontroller;
 
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a demo program showing the use of the DifferentialDrive class.
- * Runs the motors with split arcade steering and an Xbox controller.
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with split
+ * arcade steering and an Xbox controller.
  */
 public class Robot extends TimedRobot {
-  private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
-  private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
   private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
   private final XboxController m_driverController = new XboxController(0);
 
@@ -28,7 +24,6 @@
     // Drive with split arcade drive.
     // That means that the Y axis of the left stick moves forward
     // and backward, and the X of the right stick turns left and right.
-    m_robotDrive.arcadeDrive(m_driverController.getY(Hand.kLeft),
-        m_driverController.getX(Hand.kRight));
+    m_robotDrive.arcadeDrive(m_driverController.getLeftY(), m_driverController.getRightX());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
index fda1290..c7f23de 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbot;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -22,8 +19,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -48,7 +45,7 @@
     public static final double kMaxVelocityRadPerSecond = 3;
     public static final double kMaxAccelerationRadPerSecSquared = 10;
 
-    public static final int[] kEncoderPorts = new int[]{4, 5};
+    public static final int[] kEncoderPorts = new int[] {4, 5};
     public static final int kEncoderPPR = 256;
     public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR;
 
@@ -63,6 +60,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
index 681a38e..4d353a6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbot;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
index 8c9cef6..3f91253 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbot;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,21 +46,16 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
   public void disabledInit() {
     m_robotContainer.disablePIDSubsystems();
   }
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -81,12 +73,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -99,13 +88,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -113,10 +98,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
index afe7317..b0d019e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/RobotContainer.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbot;
 
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import edu.wpi.first.wpilibj.examples.armbot.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.armbot.subsystems.ArmSubsystem;
-import edu.wpi.first.wpilibj.examples.armbot.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -34,9 +29,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -46,47 +39,50 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * JoystickButton}.
    */
   private void configureButtonBindings() {
-
     // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
     new JoystickButton(m_driverController, Button.kA.value)
-        .whenPressed(() -> {
-          m_robotArm.setGoal(2);
-          m_robotArm.enable();
-        }, m_robotArm);
+        .whenPressed(
+            () -> {
+              m_robotArm.setGoal(2);
+              m_robotArm.enable();
+            },
+            m_robotArm);
 
     // Move the arm to neutral position when the 'B' button is pressed.
     new JoystickButton(m_driverController, Button.kB.value)
-        .whenPressed(() -> {
-          m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
-          m_robotArm.enable();
-        }, m_robotArm);
+        .whenPressed(
+            () -> {
+              m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads);
+              m_robotArm.enable();
+            },
+            m_robotArm);
 
     // Disable the arm controller when Y is pressed.
-    new JoystickButton(m_driverController, Button.kY.value)
-        .whenPressed(m_robotArm::disable);
+    new JoystickButton(m_driverController, Button.kY.value).whenPressed(m_robotArm::disable);
 
     // Drive at half speed when the bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
   }
 
   /**
-   * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This
-   * should be called on robot disable to prevent integral windup.
+   * Disables all ProfiledPIDSubsystem and PIDSubsystem instances. This should be called on robot
+   * disable to prevent integral windup.
    */
   public void disablePIDSubsystems() {
     m_robotArm.disable();
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
index d9fd206..298b2d5 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/ArmSubsystem.java
@@ -1,38 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbot.subsystems;
 
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.controller.ArmFeedforward;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
 
-import edu.wpi.first.wpilibj.examples.armbot.Constants.ArmConstants;
-
-/**
- * A robot arm subsystem that moves with a motion profile.
- */
+/** A robot arm subsystem that moves with a motion profile. */
 public class ArmSubsystem extends ProfiledPIDSubsystem {
-  private final PWMVictorSPX m_motor = new PWMVictorSPX(ArmConstants.kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(ArmConstants.kMotorPort);
   private final Encoder m_encoder =
       new Encoder(ArmConstants.kEncoderPorts[0], ArmConstants.kEncoderPorts[1]);
   private final ArmFeedforward m_feedforward =
-      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
-                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
+      new ArmFeedforward(
+          ArmConstants.kSVolts, ArmConstants.kCosVolts,
+          ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
 
-  /**
-   * Create a new ArmSubsystem.
-   */
+  /** Create a new ArmSubsystem. */
   public ArmSubsystem() {
-    super(new ProfiledPIDController(ArmConstants.kP, 0, 0, new TrapezoidProfile.Constraints(
-        ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared)), 0);
+    super(
+        new ProfiledPIDController(
+            ArmConstants.kP,
+            0,
+            0,
+            new TrapezoidProfile.Constraints(
+                ArmConstants.kMaxVelocityRadPerSecond,
+                ArmConstants.kMaxAccelerationRadPerSecSquared)),
+        0);
     m_encoder.setDistancePerPulse(ArmConstants.kEncoderDistancePerPulse);
     // Start arm at rest in neutral position
     setGoal(ArmConstants.kArmOffsetRads);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
index 9404013..9338f30 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbot/subsystems/DriveSubsystem.java
@@ -1,47 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbot.subsystems;
 
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.armbot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +58,7 @@
     m_drive.arcadeDrive(fwd, rot);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -94,7 +92,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
index baa15b6..4fd17f1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -22,8 +19,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -54,6 +51,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
index 153a254..4f95b40 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/ExampleSmartMotorController.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard;
 
-import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements SpeedController {
+public class ExampleSmartMotorController implements MotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
@@ -27,8 +24,7 @@
    * @param port The port for the controller.
    */
   @SuppressWarnings("PMD.UnusedFormalParameter")
-  public ExampleSmartMotorController(int port) {
-  }
+  public ExampleSmartMotorController(int port) {}
 
   /**
    * Example method for setting the PID gains of the smart controller.
@@ -37,8 +33,7 @@
    * @param ki The integral gain.
    * @param kd The derivative gain.
    */
-  public void setPID(double kp, double ki, double kd) {
-  }
+  public void setPID(double kp, double ki, double kd) {}
 
   /**
    * Example method for setting the setpoint of the smart controller in PID mode.
@@ -47,16 +42,14 @@
    * @param setpoint The controller setpoint.
    * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
    */
-  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
-  }
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
 
   /**
    * Places this motor controller in follower mode.
    *
    * @param leader The leader to follow.
    */
-  public void follow(ExampleSmartMotorController leader) {
-  }
+  public void follow(ExampleSmartMotorController leader) {}
 
   /**
    * Returns the encoder distance.
@@ -76,15 +69,11 @@
     return 0;
   }
 
-  /**
-   * Resets the encoder to zero distance.
-   */
-  public void resetEncoder() {
-  }
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
 
   @Override
-  public void set(double speed) {
-  }
+  public void set(double speed) {}
 
   @Override
   public double get() {
@@ -92,9 +81,7 @@
   }
 
   @Override
-  public void setInverted(boolean isInverted) {
-
-  }
+  public void setInverted(boolean isInverted) {}
 
   @Override
   public boolean getInverted() {
@@ -102,14 +89,8 @@
   }
 
   @Override
-  public void disable() {
-  }
+  public void disable() {}
 
   @Override
-  public void stopMotor() {
-  }
-
-  @Override
-  public void pidWrite(double output) {
-  }
+  public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
index ff9e928..ad36fce 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
index 8a7f99b..e0c0c7b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
index 6bca84a..b0c3f76 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/RobotContainer.java
@@ -1,30 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard;
 
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
+import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.ArmSubsystem;
-import edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -34,9 +29,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -46,20 +39,20 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * JoystickButton}.
    */
   private void configureButtonBindings() {
-
     // Move the arm to 2 radians above horizontal when the 'A' button is pressed.
     new JoystickButton(m_driverController, Button.kA.value)
         .whenPressed(() -> m_robotArm.setGoal(2), m_robotArm);
@@ -69,12 +62,11 @@
         .whenPressed(() -> m_robotArm.setGoal(Constants.ArmConstants.kArmOffsetRads), m_robotArm);
 
     // Drive at half speed when the bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
index 92fd43c..152700e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/ArmSubsystem.java
@@ -1,36 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
 
-import edu.wpi.first.wpilibj.controller.ArmFeedforward;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
-
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.ArmConstants;
 import edu.wpi.first.wpilibj.examples.armbotoffboard.ExampleSmartMotorController;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileSubsystem;
 
-/**
- * A robot arm subsystem that moves with a motion profile.
- */
+/** A robot arm subsystem that moves with a motion profile. */
 public class ArmSubsystem extends TrapezoidProfileSubsystem {
   private final ExampleSmartMotorController m_motor =
       new ExampleSmartMotorController(ArmConstants.kMotorPort);
   private final ArmFeedforward m_feedforward =
-      new ArmFeedforward(ArmConstants.kSVolts, ArmConstants.kCosVolts,
-                         ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
+      new ArmFeedforward(
+          ArmConstants.kSVolts, ArmConstants.kCosVolts,
+          ArmConstants.kVVoltSecondPerRad, ArmConstants.kAVoltSecondSquaredPerRad);
 
-  /**
-   * Create a new ArmSubsystem.
-   */
+  /** Create a new ArmSubsystem. */
   public ArmSubsystem() {
-    super(new TrapezoidProfile.Constraints(ArmConstants.kMaxVelocityRadPerSecond,
-                                           ArmConstants.kMaxAccelerationRadPerSecSquared),
-          ArmConstants.kArmOffsetRads);
+    super(
+        new TrapezoidProfile.Constraints(
+            ArmConstants.kMaxVelocityRadPerSecond, ArmConstants.kMaxAccelerationRadPerSecSquared),
+        ArmConstants.kArmOffsetRads);
     m_motor.setPID(ArmConstants.kP, 0, 0);
   }
 
@@ -39,7 +33,7 @@
     // Calculate the feedforward from the sepoint
     double feedforward = m_feedforward.calculate(setpoint.position, setpoint.velocity);
     // Add the feedforward to the PID output to get the motor output
-    m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, setpoint.position,
-                        feedforward / 12.0);
+    m_motor.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition, setpoint.position, feedforward / 12.0);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
index 433dba4..8017719 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armbotoffboard/subsystems/DriveSubsystem.java
@@ -1,47 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armbotoffboard.subsystems;
 
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.armbotoffboard.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +58,7 @@
     m_drive.arcadeDrive(fwd, rot);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -94,7 +92,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
index 31777a6..b980695 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armsimulation;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
index c906381..a7e4d7d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/armsimulation/Robot.java
@@ -1,29 +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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.armsimulation;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
-import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
 
-/**
- * This is a sample program to demonstrate the use of elevator simulation with existing code.
- */
+/** This is a sample program to demonstrate the use of arm simulation with existing code. */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
   private static final int kEncoderAChannel = 0;
@@ -31,47 +32,67 @@
   private static final int kJoystickPort = 0;
 
   // The P gain for the PID controller that drives this arm.
-  private static final double kArmKp = 5.0;
+  private static final double kArmKp = 50.0;
 
   // distance per pulse = (angle per revolution) / (pulses per revolution)
   //  = (2 * PI rads) / (4096 pulses)
   private static final double kArmEncoderDistPerPulse = 2.0 * Math.PI / 4096;
 
-  // The arm gearbox represents a gerbox containing two Vex 775pro motors.
+  // The arm gearbox represents a gearbox containing two Vex 775pro motors.
   private final DCMotor m_armGearbox = DCMotor.getVex775Pro(2);
 
-  // Standard classes for controlling our elevator
+  // Standard classes for controlling our arm
   private final PIDController m_controller = new PIDController(kArmKp, 0, 0);
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
-  private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
   private final Joystick m_joystick = new Joystick(kJoystickPort);
 
   // Simulation classes help us simulate what's going on, including gravity.
-  private static final double m_armReduction = 100;
+  private static final double m_armReduction = 600;
   private static final double m_armMass = 5.0; // Kilograms
   private static final double m_armLength = Units.inchesToMeters(30);
-  // This arm sim represents an arm that can travel from -180 degrees (rotated straight backwards)
-  // to 0 degrees (rotated straight forwards).
-  private final SingleJointedArmSim m_armSim = new SingleJointedArmSim(m_armGearbox,
-      m_armReduction,
-      SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
-      m_armLength,
-      Units.degreesToRadians(-180),
-      Units.degreesToRadians(0),
-      m_armMass,
-      true,
-      VecBuilder.fill(Units.degreesToRadians(0.5)) // Add noise with a std-dev of 0.5 degrees
-      );
+  // This arm sim represents an arm that can travel from -75 degrees (rotated down front)
+  // to 255 degrees (rotated down in the back).
+  private final SingleJointedArmSim m_armSim =
+      new SingleJointedArmSim(
+          m_armGearbox,
+          m_armReduction,
+          SingleJointedArmSim.estimateMOI(m_armLength, m_armMass),
+          m_armLength,
+          Units.degreesToRadians(-75),
+          Units.degreesToRadians(255),
+          m_armMass,
+          true,
+          VecBuilder.fill(kArmEncoderDistPerPulse) // Add noise with a std-dev of 1 tick
+          );
   private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
 
+  // Create a Mechanism2d display of an Arm with a fixed ArmTower and moving Arm.
+  private final Mechanism2d m_mech2d = new Mechanism2d(60, 60);
+  private final MechanismRoot2d m_armPivot = m_mech2d.getRoot("ArmPivot", 30, 30);
+  private final MechanismLigament2d m_armTower =
+      m_armPivot.append(new MechanismLigament2d("ArmTower", 30, -90));
+  private final MechanismLigament2d m_arm =
+      m_armPivot.append(
+          new MechanismLigament2d(
+              "Arm",
+              30,
+              Units.radiansToDegrees(m_armSim.getAngleRads()),
+              6,
+              new Color8Bit(Color.kYellow)));
+
   @Override
   public void robotInit() {
     m_encoder.setDistancePerPulse(kArmEncoderDistPerPulse);
+
+    // Put Mechanism 2d to SmartDashboard
+    SmartDashboard.putData("Arm Sim", m_mech2d);
+    m_armTower.setColor(new Color8Bit(Color.kBlue));
   }
 
   @Override
   public void simulationPeriodic() {
-    // In this method, we update our simulation of what our elevator is doing
+    // In this method, we update our simulation of what our arm is doing
     // First, we set our "inputs" (voltages)
     m_armSim.setInput(m_motor.get() * RobotController.getBatteryVoltage());
 
@@ -81,14 +102,18 @@
     // Finally, we set our simulated encoder's readings and simulated battery voltage
     m_encoderSim.setDistance(m_armSim.getAngleRads());
     // SimBattery estimates loaded battery voltages
-    RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_armSim.getCurrentDrawAmps()));
+
+    // Update the Mechanism Arm angle based on the simulated arm angle
+    m_arm.setAngle(Units.radiansToDegrees(m_armSim.getAngleRads()));
   }
 
   @Override
   public void teleopPeriodic() {
     if (m_joystick.getTrigger()) {
-      // Here, we run PID control like normal, with a constant setpoint of 30in.
-      var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(0));
+      // Here, we run PID control like normal, with a constant setpoint of 75 degrees.
+      var pidOutput = m_controller.calculate(m_encoder.getDistance(), Units.degreesToRadians(75));
       m_motor.setVoltage(pidOutput);
     } else {
       // Otherwise, we disable the motor.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java
index 1c21a4a..cb1423d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.axiscamera;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
index ee4b424..65eafaa 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/axiscamera/Robot.java
@@ -1,69 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.axiscamera;
 
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.AxisCamera;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.wpilibj.TimedRobot;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
 import org.opencv.core.Scalar;
 import org.opencv.imgproc.Imgproc;
 
-import edu.wpi.cscore.AxisCamera;
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.CvSource;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.wpilibj.TimedRobot;
-
 /**
- * This is a demo program showing the use of OpenCV to do vision processing. The
- * image is acquired from the Axis camera, then a rectangle is put on the image
- * and sent to the dashboard. OpenCV has many methods for different types of
- * processing.
+ * This is a demo program showing the use of OpenCV to do vision processing. The image is acquired
+ * from the Axis camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+ * many methods for different types of processing.
  */
 public class Robot extends TimedRobot {
   Thread m_visionThread;
 
   @Override
   public void robotInit() {
-    m_visionThread = new Thread(() -> {
-      // Get the Axis camera from CameraServer
-      AxisCamera camera
-          = CameraServer.getInstance().addAxisCamera("axis-camera.local");
-      // Set the resolution
-      camera.setResolution(640, 480);
+    m_visionThread =
+        new Thread(
+            () -> {
+              // Get the Axis camera from CameraServer
+              AxisCamera camera = CameraServer.addAxisCamera("axis-camera.local");
+              // Set the resolution
+              camera.setResolution(640, 480);
 
-      // Get a CvSink. This will capture Mats from the camera
-      CvSink cvSink = CameraServer.getInstance().getVideo();
-      // Setup a CvSource. This will send images back to the Dashboard
-      CvSource outputStream
-          = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
+              // Get a CvSink. This will capture Mats from the camera
+              CvSink cvSink = CameraServer.getVideo();
+              // Setup a CvSource. This will send images back to the Dashboard
+              CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480);
 
-      // Mats are very memory expensive. Lets reuse this Mat.
-      Mat mat = new Mat();
+              // Mats are very memory expensive. Lets reuse this Mat.
+              Mat mat = new Mat();
 
-      // This cannot be 'true'. The program will never exit if it is. This
-      // lets the robot stop this thread when restarting robot code or
-      // deploying.
-      while (!Thread.interrupted()) {
-        // Tell the CvSink to grab a frame from the camera and put it
-        // in the source mat.  If there is an error notify the output.
-        if (cvSink.grabFrame(mat) == 0) {
-          // Send the output the error.
-          outputStream.notifyError(cvSink.getError());
-          // skip the rest of the current iteration
-          continue;
-        }
-        // Put a rectangle on the image
-        Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400),
-            new Scalar(255, 255, 255), 5);
-        // Give the output stream a new image to display
-        outputStream.putFrame(mat);
-      }
-    });
+              // This cannot be 'true'. The program will never exit if it is. This
+              // lets the robot stop this thread when restarting robot code or
+              // deploying.
+              while (!Thread.interrupted()) {
+                // Tell the CvSink to grab a frame from the camera and put it
+                // in the source mat.  If there is an error notify the output.
+                if (cvSink.grabFrame(mat) == 0) {
+                  // Send the output the error.
+                  outputStream.notifyError(cvSink.getError());
+                  // skip the rest of the current iteration
+                  continue;
+                }
+                // Put a rectangle on the image
+                Imgproc.rectangle(
+                    mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5);
+                // Give the output stream a new image to display
+                outputStream.putFrame(mat);
+              }
+            });
     m_visionThread.setDaemon(true);
     m_visionThread.start();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
index ff5c19c..bafb3af 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.canpdp;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
index 7a0831b..1d5e24f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/canpdp/Robot.java
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.canpdp;
 
-import edu.wpi.first.wpilibj.PowerDistributionPanel;
+import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
@@ -16,9 +13,7 @@
  * via CAN. The information will be displayed under variables through the SmartDashboard.
  */
 public class Robot extends TimedRobot {
-  private static final int kPDPId = 0;
-
-  private final PowerDistributionPanel m_pdp = new PowerDistributionPanel(kPDPId);
+  private final PowerDistribution m_pdp = new PowerDistribution();
 
   @Override
   public void robotPeriodic() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
index b9c69a8..07b4881 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Drivetrain.java
@@ -1,27 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.differentialdrivebot;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
-/**
- * Represents a differential drive style drivetrain.
- */
+/** Represents a differential drive style drivetrain. */
 public class Drivetrain {
   public static final double kMaxSpeed = 3.0; // meters per second
   public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
@@ -30,26 +25,26 @@
   private static final double kWheelRadius = 0.0508; // meters
   private static final int kEncoderResolution = 4096;
 
-  private final SpeedController m_leftLeader = new PWMVictorSPX(1);
-  private final SpeedController m_leftFollower = new PWMVictorSPX(2);
-  private final SpeedController m_rightLeader = new PWMVictorSPX(3);
-  private final SpeedController m_rightFollower = new PWMVictorSPX(4);
+  private final MotorController m_leftLeader = new PWMSparkMax(1);
+  private final MotorController m_leftFollower = new PWMSparkMax(2);
+  private final MotorController m_rightLeader = new PWMSparkMax(3);
+  private final MotorController m_rightFollower = new PWMSparkMax(4);
 
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
-  private final SpeedControllerGroup m_leftGroup
-      = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
-  private final SpeedControllerGroup m_rightGroup
-      = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
+  private final MotorControllerGroup m_leftGroup =
+      new MotorControllerGroup(m_leftLeader, m_leftFollower);
+  private final MotorControllerGroup m_rightGroup =
+      new MotorControllerGroup(m_rightLeader, m_rightFollower);
 
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
   private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
   private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
 
-  private final DifferentialDriveKinematics m_kinematics
-      = new DifferentialDriveKinematics(kTrackWidth);
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(kTrackWidth);
 
   private final DifferentialDriveOdometry m_odometry;
 
@@ -57,12 +52,17 @@
   private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
 
   /**
-   * Constructs a differential drive object.
-   * Sets the encoder distance per pulse and resets the gyro.
+   * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+   * gyro.
    */
   public Drivetrain() {
     m_gyro.reset();
 
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.setInverted(true);
+
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
     // resolution.
@@ -84,10 +84,10 @@
     final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
     final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
 
-    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
-        speeds.leftMetersPerSecond);
-    final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
-        speeds.rightMetersPerSecond);
+    final double leftOutput =
+        m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
+    final double rightOutput =
+        m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
     m_leftGroup.setVoltage(leftOutput + leftFeedforward);
     m_rightGroup.setVoltage(rightOutput + rightFeedforward);
   }
@@ -96,7 +96,7 @@
    * Drives the robot with the given linear velocity and angular velocity.
    *
    * @param xSpeed Linear velocity in m/s.
-   * @param rot    Angular velocity in rad/s.
+   * @param rot Angular velocity in rad/s.
    */
   @SuppressWarnings("ParameterName")
   public void drive(double xSpeed, double rot) {
@@ -104,11 +104,9 @@
     setSpeeds(wheelSpeeds);
   }
 
-  /**
-   * Updates the field-relative position.
-   */
+  /** Updates the field-relative position. */
   public void updateOdometry() {
-    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
-        m_rightEncoder.getDistance());
+    m_odometry.update(
+        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
index 1f2b319..89f8dae 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.differentialdrivebot;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
index 6e7416d..8cd5022 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdrivebot/Robot.java
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.differentialdrivebot;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
@@ -20,7 +16,6 @@
   private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
   private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
 
-
   @Override
   public void autonomousPeriodic() {
     teleopPeriodic();
@@ -31,17 +26,13 @@
   public void teleopPeriodic() {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    final var xSpeed =
-        -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
-            * Drivetrain.kMaxSpeed;
+    final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    final var rot =
-        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
-            * Drivetrain.kMaxAngularSpeed;
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
 
     m_drive.drive(xSpeed, rot);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
new file mode 100644
index 0000000..3b5d9e2
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Drivetrain.java
@@ -0,0 +1,132 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/** Represents a differential drive style drivetrain. */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // meters per second
+  public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
+
+  private static final double kTrackWidth = 0.381 * 2; // meters
+  private static final double kWheelRadius = 0.0508; // meters
+  private static final int kEncoderResolution = 4096;
+
+  private final MotorController m_leftLeader = new PWMSparkMax(1);
+  private final MotorController m_leftFollower = new PWMSparkMax(2);
+  private final MotorController m_rightLeader = new PWMSparkMax(3);
+  private final MotorController m_rightFollower = new PWMSparkMax(4);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+  private final MotorControllerGroup m_leftGroup =
+      new MotorControllerGroup(m_leftLeader, m_leftFollower);
+  private final MotorControllerGroup m_rightGroup =
+      new MotorControllerGroup(m_rightLeader, m_rightFollower);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
+
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(kTrackWidth);
+
+  /* Here we use DifferentialDrivePoseEstimator so that we can fuse odometry readings. The
+  numbers used  below are robot specific, and should be tuned. */
+  private final DifferentialDrivePoseEstimator m_poseEstimator =
+      new DifferentialDrivePoseEstimator(
+          m_gyro.getRotation2d(),
+          new Pose2d(),
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5), 0.01, 0.01),
+          VecBuilder.fill(0.02, 0.02, Units.degreesToRadians(1)),
+          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /**
+   * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+   * gyro.
+   */
+  public Drivetrain() {
+    m_gyro.reset();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.setInverted(true);
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Sets the desired wheel speeds.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+    final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+
+    final double leftOutput =
+        m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
+    final double rightOutput =
+        m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
+    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+  }
+
+  /**
+   * Drives the robot with the given linear velocity and angular velocity.
+   *
+   * @param xSpeed Linear velocity in m/s.
+   * @param rot Angular velocity in rad/s.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double rot) {
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0.0, rot));
+    setSpeeds(wheelSpeeds);
+  }
+
+  /** Updates the field-relative position. */
+  public void updateOdometry() {
+    m_poseEstimator.update(
+        m_gyro.getRotation2d(),
+        new DifferentialDriveWheelSpeeds(m_leftEncoder.getRate(), m_rightEncoder.getRate()),
+        m_leftEncoder.getDistance(),
+        m_rightEncoder.getDistance());
+
+    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+    // a real robot, this must be calculated based either on latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(
+        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+            m_poseEstimator.getEstimatedPosition()),
+        Timer.getFPGATimestamp() - 0.3);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java
new file mode 100644
index 0000000..4697bba
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/ExampleGlobalMeasurementSensor.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+  private ExampleGlobalMeasurementSensor() {
+    // Utility class
+  }
+
+  /**
+   * Get a "noisy" fake global pose reading.
+   *
+   * @param estimatedRobotPose The robot pose.
+   */
+  public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+    var rand =
+        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    return new Pose2d(
+        estimatedRobotPose.getX() + rand.get(0, 0),
+        estimatedRobotPose.getY() + rand.get(1, 0),
+        estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java
new file mode 100644
index 0000000..046c01a
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java
new file mode 100644
index 0000000..1011724
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/differentialdriveposeestimator/Robot.java
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.differentialdriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_drive = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    teleopPeriodic();
+    m_drive.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+    m_drive.drive(xSpeed, rot);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java
new file mode 100644
index 0000000..97f67d1
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.dma;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java
new file mode 100644
index 0000000..a7b088c
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dma/Robot.java
@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.dma;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.DMA;
+import edu.wpi.first.wpilibj.DMASample;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/** This is a sample program showing how to to use DMA to read a sensor. */
+public class Robot extends TimedRobot {
+  private DMA m_dma;
+  private DMASample m_dmaSample;
+
+  // DMA needs a trigger, can use an output as trigger.
+  // 8 Triggers exist per DMA object, can be triggered on any
+  // DigitalSource.
+  private DigitalOutput m_dmaTrigger;
+
+  // Analog input to read with DMA
+  private AnalogInput m_analogInput;
+
+  // Encoder to read with DMA
+  private Encoder m_encoder;
+
+  @Override
+  public void robotInit() {
+    m_dma = new DMA();
+    m_dmaSample = new DMASample();
+    m_dmaTrigger = new DigitalOutput(2);
+    m_analogInput = new AnalogInput(0);
+    m_encoder = new Encoder(0, 1);
+
+    // Trigger on falling edge of dma trigger output
+    m_dma.setExternalTrigger(m_dmaTrigger, false, true);
+
+    // Add inputs we want to read via DMA
+    m_dma.addAnalogInput(m_analogInput);
+    m_dma.addEncoder(m_encoder);
+    m_dma.addEncoderPeriod(m_encoder);
+
+    // Make sure trigger is set to off.
+    m_dmaTrigger.set(true);
+
+    // Start DMA. No triggers or inputs can be added after this call
+    // unless DMA is stopped.
+    m_dma.start(1024);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Manually Trigger DMA read
+    m_dmaTrigger.set(false);
+
+    // Update our sample. remaining is the number of samples remaining in the
+    // buffer status is more specific error messages if readStatus is not OK.
+    // Wait 1ms if buffer is empty
+    DMASample.DMAReadStatus readStatus = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
+
+    // Unset trigger
+    m_dmaTrigger.set(true);
+
+    if (readStatus == DMASample.DMAReadStatus.kOk) {
+      // Status value in all these reads should be checked, a non 0 value means
+      // value could not be read
+
+      // If DMA is good, values exist
+      double encoderDistance = m_dmaSample.getEncoderDistance(m_encoder);
+      // Period is not scaled, and is a raw value
+      int encoderPeriod = m_dmaSample.getEncoderPeriodRaw(m_encoder);
+      double analogVoltage = m_dmaSample.getAnalogInputVoltage(m_analogInput);
+
+      SmartDashboard.putNumber("Distance", encoderDistance);
+      SmartDashboard.putNumber("Period", encoderPeriod);
+      SmartDashboard.putNumber("Input", analogVoltage);
+      SmartDashboard.putNumber("Timestamp", m_dmaSample.getTimeStamp());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
index 5f4bbeb..64ba267 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -38,6 +35,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
index e2fb7c8..487974c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/ExampleSmartMotorController.java
@@ -1,34 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
-import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements SpeedController {
+public class ExampleSmartMotorController implements MotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
     kMovementWitchcraft
   }
 
+  double m_value;
+
   /**
    * Creates a new ExampleSmartMotorController.
    *
    * @param port The port for the controller.
    */
   @SuppressWarnings("PMD.UnusedFormalParameter")
-  public ExampleSmartMotorController(int port) {
-  }
+  public ExampleSmartMotorController(int port) {}
 
   /**
    * Example method for setting the PID gains of the smart controller.
@@ -37,8 +35,7 @@
    * @param ki The integral gain.
    * @param kd The derivative gain.
    */
-  public void setPID(double kp, double ki, double kd) {
-  }
+  public void setPID(double kp, double ki, double kd) {}
 
   /**
    * Example method for setting the setpoint of the smart controller in PID mode.
@@ -47,16 +44,14 @@
    * @param setpoint The controller setpoint.
    * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
    */
-  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
-  }
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
 
   /**
    * Places this motor controller in follower mode.
    *
    * @param leader The leader to follow.
    */
-  public void follow(ExampleSmartMotorController leader) {
-  }
+  public void follow(ExampleSmartMotorController leader) {}
 
   /**
    * Returns the encoder distance.
@@ -76,25 +71,21 @@
     return 0;
   }
 
-  /**
-   * Resets the encoder to zero distance.
-   */
-  public void resetEncoder() {
-  }
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
 
   @Override
   public void set(double speed) {
+    m_value = speed;
   }
 
   @Override
   public double get() {
-    return 0;
+    return m_value;
   }
 
   @Override
-  public void setInverted(boolean isInverted) {
-
-  }
+  public void setInverted(boolean isInverted) {}
 
   @Override
   public boolean getInverted() {
@@ -102,14 +93,8 @@
   }
 
   @Override
-  public void disable() {
-  }
+  public void disable() {}
 
   @Override
-  public void stopMotor() {
-  }
-
-  @Override
-  public void pidWrite(double output) {
-  }
+  public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
index 89ab553..3566c19 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
index e8ac285..87c9d52 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
index f618b9d..78b7f81 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/RobotContainer.java
@@ -1,33 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard;
 
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
+import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands.DriveDistanceProfiled;
-import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -36,9 +31,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -48,17 +41,18 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                         m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * JoystickButton}.
    */
   private void configureButtonBindings() {
     // Drive forward by 3 meters when the 'A' button is pressed, with a timeout of 10 seconds
@@ -69,29 +63,26 @@
     new JoystickButton(m_driverController, Button.kB.value)
         .whenPressed(
             new TrapezoidProfileCommand(
-                new TrapezoidProfile(
-                    // Limit the max acceleration and velocity
-                    new TrapezoidProfile.Constraints(
-                        DriveConstants.kMaxSpeedMetersPerSecond,
-                        DriveConstants.kMaxAccelerationMetersPerSecondSquared),
-                    // End at desired position in meters; implicitly starts at 0
-                    new TrapezoidProfile.State(3, 0)),
-                // Pipe the profile state to the drive
-                setpointState -> m_robotDrive.setDriveStates(
-                    setpointState,
-                    setpointState),
-                // Require the drive
-                m_robotDrive)
+                    new TrapezoidProfile(
+                        // Limit the max acceleration and velocity
+                        new TrapezoidProfile.Constraints(
+                            DriveConstants.kMaxSpeedMetersPerSecond,
+                            DriveConstants.kMaxAccelerationMetersPerSecondSquared),
+                        // End at desired position in meters; implicitly starts at 0
+                        new TrapezoidProfile.State(3, 0)),
+                    // Pipe the profile state to the drive
+                    setpointState -> m_robotDrive.setDriveStates(setpointState, setpointState),
+                    // Require the drive
+                    m_robotDrive)
                 .beforeStarting(m_robotDrive::resetEncoders)
                 .withTimeout(10));
 
     // Drive at half speed when the bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
index 280acd3..5bfa10d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/commands/DriveDistanceProfiled.java
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.commands;
 
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
-
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.TrapezoidProfileCommand;
 
-/**
- * Drives a set distance using a motion profile.
- */
+/** Drives a set distance using a motion profile. */
 public class DriveDistanceProfiled extends TrapezoidProfileCommand {
   /**
    * Creates a new DriveDistanceProfiled command.
@@ -27,8 +21,9 @@
     super(
         new TrapezoidProfile(
             // Limit the max acceleration and velocity
-            new TrapezoidProfile.Constraints(DriveConstants.kMaxSpeedMetersPerSecond,
-                                             DriveConstants.kMaxAccelerationMetersPerSecondSquared),
+            new TrapezoidProfile.Constraints(
+                DriveConstants.kMaxSpeedMetersPerSecond,
+                DriveConstants.kMaxAccelerationMetersPerSecondSquared),
             // End at desired position in meters; implicitly starts at 0
             new TrapezoidProfile.State(meters, 0)),
         // Pipe the profile state to the drive
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
index 3d52adb..124c1fe 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/drivedistanceoffboard/subsystems/DriveSubsystem.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.drivedistanceoffboard.subsystems;
 
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.drivedistanceoffboard.ExampleSmartMotorController;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
@@ -30,17 +26,26 @@
       new ExampleSmartMotorController(DriveConstants.kRightMotor2Port);
 
   private final SimpleMotorFeedforward m_feedforward =
-      new SimpleMotorFeedforward(DriveConstants.ksVolts,
-                                 DriveConstants.kvVoltSecondsPerMeter,
-                                 DriveConstants.kaVoltSecondsSquaredPerMeter);
+      new SimpleMotorFeedforward(
+          DriveConstants.ksVolts,
+          DriveConstants.kvVoltSecondsPerMeter,
+          DriveConstants.kaVoltSecondsSquaredPerMeter);
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftLeader, m_rightLeader);
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightLeader.setInverted(true);
+
+    // You might need to not do this depending on the specific motor controller
+    // that you are using -- contact the respective vendor's documentation for
+    // more details.
+    m_rightFollower.setInverted(true);
+
     m_leftFollower.follow(m_leftLeader);
     m_rightFollower.follow(m_rightLeader);
 
@@ -65,12 +70,14 @@
    * @param right The right wheel state.
    */
   public void setDriveStates(TrapezoidProfile.State left, TrapezoidProfile.State right) {
-    m_leftLeader.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
-                             left.position,
-                             m_feedforward.calculate(left.velocity));
-    m_rightLeader.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition,
-                              right.position,
-                              m_feedforward.calculate(right.velocity));
+    m_leftLeader.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        left.position,
+        m_feedforward.calculate(left.velocity));
+    m_rightLeader.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        right.position,
+        m_feedforward.calculate(right.velocity));
   }
 
   /**
@@ -91,16 +98,14 @@
     return m_rightLeader.getEncoderDistance();
   }
 
-  /**
-   * Resets the drive encoders.
-   */
+  /** Resets the drive encoders. */
   public void resetEncoders() {
     m_leftLeader.resetEncoder();
     m_rightLeader.resetEncoder();
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
index a051005..1ba73f5 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Main.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleencoder;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
-  private Main() {
-  }
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
index 5755f72..a12a6fd 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleencoder/Robot.java
@@ -1,46 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleencoder;
-
-import edu.wpi.first.wpilibj.DutyCycleEncoder;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-@SuppressWarnings({"PMD.SingularField"})
-public class Robot extends TimedRobot {
-  private DutyCycleEncoder m_dutyCycleEncoder;
-
-  @Override
-  public void robotInit() {
-    m_dutyCycleEncoder = new DutyCycleEncoder(0);
-
-    // Set to 0.5 units per rotation
-    m_dutyCycleEncoder.setDistancePerRotation(0.5);
-  }
-
-  @Override
-  public void robotPeriodic() {
-    // Connected can be checked, and uses the frequency of the encoder
-    boolean connected = m_dutyCycleEncoder.isConnected();
-
-    // Duty Cycle Frequency in Hz
-    int frequency = m_dutyCycleEncoder.getFrequency();
-
-    // Output of encoder
-    double output = m_dutyCycleEncoder.get();
-
-    // Output scaled by DistancePerPulse
-    double distance = m_dutyCycleEncoder.getDistance();
-
-    SmartDashboard.putBoolean("Connected", connected);
-    SmartDashboard.putNumber("Frequency", frequency);
-    SmartDashboard.putNumber("Output", output);
-    SmartDashboard.putNumber("Distance", distance);
-  }
-
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.dutycycleencoder;
+
+import edu.wpi.first.wpilibj.DutyCycleEncoder;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings("PMD.SingularField")
+public class Robot extends TimedRobot {
+  private DutyCycleEncoder m_dutyCycleEncoder;
+
+  @Override
+  public void robotInit() {
+    m_dutyCycleEncoder = new DutyCycleEncoder(0);
+
+    // Set to 0.5 units per rotation
+    m_dutyCycleEncoder.setDistancePerRotation(0.5);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Connected can be checked, and uses the frequency of the encoder
+    boolean connected = m_dutyCycleEncoder.isConnected();
+
+    // Duty Cycle Frequency in Hz
+    int frequency = m_dutyCycleEncoder.getFrequency();
+
+    // Output of encoder
+    double output = m_dutyCycleEncoder.get();
+
+    // Output scaled by DistancePerPulse
+    double distance = m_dutyCycleEncoder.getDistance();
+
+    SmartDashboard.putBoolean("Connected", connected);
+    SmartDashboard.putNumber("Frequency", frequency);
+    SmartDashboard.putNumber("Output", output);
+    SmartDashboard.putNumber("Distance", distance);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
index a7cbfc0..4558281 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Main.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleinput;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
-  private Main() {
-  }
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
index 657dc9b..584fbc6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/dutycycleinput/Robot.java
@@ -1,39 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.dutycycleinput;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.DutyCycle;
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-@SuppressWarnings({"PMD.SingularField"})
-public class Robot extends TimedRobot {
-  private DigitalInput m_input;
-  private DutyCycle m_dutyCycle;
-
-  @Override
-  public void robotInit() {
-    m_input = new DigitalInput(0);
-    m_dutyCycle = new DutyCycle(m_input);
-  }
-
-  @Override
-  public void robotPeriodic() {
-    // Duty Cycle Frequency in Hz
-    int frequency = m_dutyCycle.getFrequency();
-
-    // Output of duty cycle, ranging from 0 to 1
-    // 1 is fully on, 0 is fully off
-    double output = m_dutyCycle.getOutput();
-
-    SmartDashboard.putNumber("Frequency", frequency);
-    SmartDashboard.putNumber("Duty Cycle", output);
-  }
-
-}
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.dutycycleinput;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DutyCycle;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+@SuppressWarnings("PMD.SingularField")
+public class Robot extends TimedRobot {
+  private DigitalInput m_input;
+  private DutyCycle m_dutyCycle;
+
+  @Override
+  public void robotInit() {
+    m_input = new DigitalInput(0);
+    m_dutyCycle = new DutyCycle(m_input);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // Duty Cycle Frequency in Hz
+    int frequency = m_dutyCycle.getFrequency();
+
+    // Output of duty cycle, ranging from 0 to 1
+    // 1 is fully on, 0 is fully off
+    double output = m_dutyCycle.getOutput();
+
+    SmartDashboard.putNumber("Frequency", frequency);
+    SmartDashboard.putNumber("Duty Cycle", output);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
index 5df92a6..223b40d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
index 5d3d546..15d5e23 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
@@ -1,26 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatorprofiledpid;
 
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 public class Robot extends TimedRobot {
   private static double kDt = 0.02;
 
   private final Joystick m_joystick = new Joystick(1);
   private final Encoder m_encoder = new Encoder(1, 2);
-  private final SpeedController m_motor = new PWMVictorSPX(1);
+  private final MotorController m_motor = new PWMSparkMax(1);
 
   // Create a PID controller whose setpoint's change is subject to maximum
   // velocity and acceleration constraints.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
index 22cde2e..ab930d3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatorsimulation;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
index 600437a..3f2bae8 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorsimulation/Robot.java
@@ -1,29 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatorsimulation;
 
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.ElevatorSim;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.VecBuilder;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-/**
- * This is a sample program to demonstrate the use of elevator simulation with existing code.
- */
+/** This is a sample program to demonstrate the use of elevator simulation with existing code. */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
   private static final int kEncoderAChannel = 0;
@@ -35,34 +34,48 @@
   private static final double kElevatorDrumRadius = Units.inchesToMeters(2.0);
   private static final double kCarriageMass = 4.0; // kg
 
-  private static final double kMinElevatorHeight = 0.0;
+  private static final double kMinElevatorHeight = Units.inchesToMeters(2);
   private static final double kMaxElevatorHeight = Units.inchesToMeters(50);
 
   // distance per pulse = (distance per revolution) / (pulses per revolution)
   //  = (Pi * D) / ppr
-  private static final double kElevatorEncoderDistPerPulse = 2.0 * Math.PI * kElevatorDrumRadius / 4096;
+  private static final double kElevatorEncoderDistPerPulse =
+      2.0 * Math.PI * kElevatorDrumRadius / 4096;
 
   private final DCMotor m_elevatorGearbox = DCMotor.getVex775Pro(4);
 
   // Standard classes for controlling our elevator
   private final PIDController m_controller = new PIDController(kElevatorKp, 0, 0);
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
-  private final PWMVictorSPX m_motor = new PWMVictorSPX(kMotorPort);
+  private final PWMSparkMax m_motor = new PWMSparkMax(kMotorPort);
   private final Joystick m_joystick = new Joystick(kJoystickPort);
 
   // Simulation classes help us simulate what's going on, including gravity.
-  private final ElevatorSim m_elevatorSim = new ElevatorSim(m_elevatorGearbox,
-      kElevatorGearing,
-      kCarriageMass,
-      kElevatorDrumRadius,
-      kMinElevatorHeight,
-      kMaxElevatorHeight,
-      VecBuilder.fill(0.01));
+  private final ElevatorSim m_elevatorSim =
+      new ElevatorSim(
+          m_elevatorGearbox,
+          kElevatorGearing,
+          kCarriageMass,
+          kElevatorDrumRadius,
+          kMinElevatorHeight,
+          kMaxElevatorHeight,
+          VecBuilder.fill(0.01));
   private final EncoderSim m_encoderSim = new EncoderSim(m_encoder);
 
+  // Create a Mechanism2d visualization of the elevator
+  private final Mechanism2d m_mech2d = new Mechanism2d(20, 50);
+  private final MechanismRoot2d m_mech2dRoot = m_mech2d.getRoot("Elevator Root", 10, 0);
+  private final MechanismLigament2d m_elevatorMech2d =
+      m_mech2dRoot.append(
+          new MechanismLigament2d(
+              "Elevator", Units.metersToInches(m_elevatorSim.getPositionMeters()), 90));
+
   @Override
   public void robotInit() {
     m_encoder.setDistancePerPulse(kElevatorEncoderDistPerPulse);
+
+    // Publish Mechanism2d to SmartDashboard
+    SmartDashboard.putData("Elevator Sim", m_mech2d);
   }
 
   @Override
@@ -77,7 +90,11 @@
     // Finally, we set our simulated encoder's readings and simulated battery voltage
     m_encoderSim.setDistance(m_elevatorSim.getPositionMeters());
     // SimBattery estimates loaded battery voltages
-    RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(m_elevatorSim.getCurrentDrawAmps()));
+
+    // Update elevator visualization with simulated position
+    m_elevatorMech2d.setLength(Units.metersToInches(m_elevatorSim.getPositionMeters()));
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
index 94e3c77..a1366e6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/ExampleSmartMotorController.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
 
-import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 
 /**
  * A simplified stub class that simulates the API of a common "smart" motor controller.
  *
  * <p>Has no actual functionality.
  */
-public class ExampleSmartMotorController implements SpeedController {
+public class ExampleSmartMotorController implements MotorController {
   public enum PIDMode {
     kPosition,
     kVelocity,
@@ -27,8 +24,7 @@
    * @param port The port for the controller.
    */
   @SuppressWarnings("PMD.UnusedFormalParameter")
-  public ExampleSmartMotorController(int port) {
-  }
+  public ExampleSmartMotorController(int port) {}
 
   /**
    * Example method for setting the PID gains of the smart controller.
@@ -37,8 +33,7 @@
    * @param ki The integral gain.
    * @param kd The derivative gain.
    */
-  public void setPID(double kp, double ki, double kd) {
-  }
+  public void setPID(double kp, double ki, double kd) {}
 
   /**
    * Example method for setting the setpoint of the smart controller in PID mode.
@@ -47,16 +42,14 @@
    * @param setpoint The controller setpoint.
    * @param arbFeedforward An arbitrary feedforward output (from -1 to 1).
    */
-  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {
-  }
+  public void setSetpoint(PIDMode mode, double setpoint, double arbFeedforward) {}
 
   /**
    * Places this motor controller in follower mode.
    *
    * @param leader The leader to follow.
    */
-  public void follow(ExampleSmartMotorController leader) {
-  }
+  public void follow(ExampleSmartMotorController leader) {}
 
   /**
    * Returns the encoder distance.
@@ -76,15 +69,11 @@
     return 0;
   }
 
-  /**
-   * Resets the encoder to zero distance.
-   */
-  public void resetEncoder() {
-  }
+  /** Resets the encoder to zero distance. */
+  public void resetEncoder() {}
 
   @Override
-  public void set(double speed) {
-  }
+  public void set(double speed) {}
 
   @Override
   public double get() {
@@ -92,9 +81,7 @@
   }
 
   @Override
-  public void setInverted(boolean isInverted) {
-
-  }
+  public void setInverted(boolean isInverted) {}
 
   @Override
   public boolean getInverted() {
@@ -102,14 +89,8 @@
   }
 
   @Override
-  public void disable() {
-  }
+  public void disable() {}
 
   @Override
-  public void stopMotor() {
-  }
-
-  @Override
-  public void pidWrite(double output) {
-  }
+  public void stopMotor() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
index 12255cd..5428143 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
index 918d3d3..d459eeb 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatortrapezoidprofile/Robot.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.elevatortrapezoidprofile;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
 
 public class Robot extends TimedRobot {
   private static double kDt = 0.02;
@@ -49,7 +46,9 @@
     m_setpoint = profile.calculate(kDt);
 
     // Send setpoint to offboard controller PID
-    m_motor.setSetpoint(ExampleSmartMotorController.PIDMode.kPosition, m_setpoint.position,
-                        m_feedforward.calculate(m_setpoint.velocity) / 12.0);
+    m_motor.setSetpoint(
+        ExampleSmartMotorController.PIDMode.kPosition,
+        m_setpoint.position,
+        m_feedforward.calculate(m_setpoint.velocity) / 12.0);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
index da0be79..4f723cc 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.encoder;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
index ef34fe5..3b715eb 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/encoder/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.encoder;
 
@@ -29,14 +26,13 @@
    * The Encoder object is constructed with 4 parameters, the last two being optional. The first two
    * parameters (1, 2 in this case) refer to the ports on the roboRIO which the encoder uses.
    * Because a quadrature encoder has two signal wires, the signal from two DIO ports on the roboRIO
-   * are used. The third (optional)  parameter is a boolean which defaults to false. If you set this
-   *  parameter to true, the direction of the encoder will be reversed, in case it makes more sense
+   * are used. The third (optional) parameter is a boolean which defaults to false. If you set this
+   * parameter to true, the direction of the encoder will be reversed, in case it makes more sense
    * mechanically. The final (optional) parameter specifies encoding rate (k1X, k2X, or k4X) and
    * defaults to k4X. Faster (k4X) encoding gives greater positional precision but more noise in the
    * rate.
    */
-  private final Encoder m_encoder =
-      new Encoder(1, 2, false, CounterBase.EncodingType.k4X);
+  private final Encoder m_encoder = new Encoder(1, 2, false, CounterBase.EncodingType.k4X);
 
   @Override
   public void robotInit() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
index 5bc23ab..c270fc4 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/examples.json
@@ -12,7 +12,7 @@
   },
   {
     "name": "Tank Drive",
-    "description": "Demonstrate the use of the RobotDrive class doing teleop driving with tank steering",
+    "description": "Demonstrate the use of the DifferentialDrive class doing teleop driving with tank steering",
     "tags": [
       "Actuators",
       "Joystick",
@@ -37,7 +37,7 @@
   },
   {
     "name": "Mecanum Drive",
-    "description": "Demonstrate the use of the RobotDrive class doing teleop driving with Mecanum steering",
+    "description": "Demonstrate the use of the MecanumDrive class doing teleop driving with Mecanum steering",
     "tags": [
       "Actuators",
       "Joystick",
@@ -209,6 +209,14 @@
     "mainclass": "Main",
     "commandversion": 2
   },
+  {"name": "Mechanism2d",
+    "description": "An example usage of Mechanism2d to display mechanism states on a dashboard.",
+    "tags": ["Mechanism2d"],
+    "foldername": "mechanism2d",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
   {
     "name": "Motor Controller",
     "description": "Demonstrate controlling a single motor with a joystick",
@@ -250,17 +258,6 @@
     "commandversion": 2
   },
   {
-    "name": "PacGoat",
-    "description": "A fully functional example CommandBased program for FRC Team 190's 2014 robot. This code can run on your computer if it supports simulation.",
-    "tags": [
-      "Complete Robot"
-    ],
-    "foldername": "pacgoat",
-    "gradlebase": "java",
-    "mainclass": "Main",
-    "commandversion": 1
-  },
-  {
     "name": "Simple Vision",
     "description": "Demonstrate the use of the CameraServer class to stream from a USB Webcam without processing the images.",
     "tags": [
@@ -274,7 +271,7 @@
   },
   {
     "name": "Intermediate Vision",
-    "description": "Demonstrate the use of the NIVision class to capture image from a Webcam, process them, and then send them to the dashboard.",
+    "description": "An example program that acquires images from an attached USB camera and adds some annotation to the image as you might do for showing operators the result of some image recognition, and sends it to the dashboard for display.",
     "tags": [
       "Vision",
       "Complete List"
@@ -484,6 +481,17 @@
     "commandversion": 2
   },
   {
+    "name": "DMA",
+    "description": "Demonstrates the use of the DMA class",
+    "tags": [
+      "Advanced Java"
+    ],
+    "foldername": "dma",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
     "name": "ArmBot",
     "description": "An example command-based robot demonstrating the use of a ProfiledPIDSubsystem to control an arm.",
     "tags": [
@@ -633,6 +641,23 @@
     "commandversion": 2
   },
   {
+    "name": "SimpleDifferentialDriveSimulation",
+    "description": "An example of a minimal drivetrain simulation project without the command-based library.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Digital",
+      "Sensors",
+      "Actuators",
+      "Joystick",
+      "Simulation"
+    ],
+    "foldername": "simpledifferentialdrivesimulation",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
     "name": "StateSpaceDriveSimulation",
     "description": "An example of drivetrain simulation in combination with a RAMSETE path following controller and the Field2d class.",
     "tags": [
@@ -658,7 +683,8 @@
       "Digital",
       "Sensors",
       "Simulation",
-      "Physics"
+      "Physics",
+      "Mechanism2d"
     ],
     "foldername": "elevatorsimulation",
     "gradlebase": "java",
@@ -674,11 +700,75 @@
       "Digital",
       "Sensors",
       "Simulation",
-      "Physics"
+      "Physics",
+      "Mechanism2d"
     ],
     "foldername": "armsimulation",
     "gradlebase": "java",
     "mainclass": "Main",
     "commandversion": 2
+  },
+  {
+    "name": "DifferentialDrivePoseEstimator",
+    "description": "Demonstrates the use of the DifferentialDrivePoseEstimator as a replacement for differential drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose",
+      "Differential drive"
+    ],
+    "foldername": "differentialdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "MecanumDrivePoseEstimator",
+    "description": "Demonstrates the use of the MecanumDrivePoseEstimator as a replacement for mecanum drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose",
+      "Mecanum"
+    ],
+    "foldername": "mecanumdriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "SwerveDrivePoseEstimator",
+    "description": "Demonstrates the use of the SwerveDrivePoseEstimator as a replacement for swerve drive odometry.",
+    "tags": [
+      "Drivetrain",
+      "State space",
+      "Vision",
+      "Filter",
+      "Odometry",
+      "Pose",
+      "Swerve"
+    ],
+    "foldername": "swervedriveposeestimator",
+    "gradlebase": "java",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "RomiReference",
+    "description": "An example command-based robot program that can be used with the Romi reference robot design.",
+    "tags": [
+      "Drivetrain",
+      "Romi"
+    ],
+    "foldername": "romireference",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2
   }
 ]
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
index c8c422c..3fe16cf 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.frisbeebot;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -22,8 +19,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -35,7 +32,7 @@
   }
 
   public static final class ShooterConstants {
-    public static final int[] kEncoderPorts = new int[]{4, 5};
+    public static final int[] kEncoderPorts = new int[] {4, 5};
     public static final boolean kEncoderReversed = false;
     public static final int kEncoderCPR = 1024;
     public static final double kEncoderDistancePerPulse =
@@ -70,6 +67,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
index 1fdb348..48c3f40 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.frisbeebot;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
index 83a289d..a6b0023 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.frisbeebot;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
index 957f11f..5d228d0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/RobotContainer.java
@@ -1,14 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.frisbeebot;
 
-import edu.wpi.first.wpilibj.GenericHID;
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -17,18 +19,11 @@
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.AutoConstants;
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.DriveSubsystem;
-import edu.wpi.first.wpilibj.examples.frisbeebot.subsystems.ShooterSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -38,28 +33,28 @@
   // A simple autonomous routine that shoots the loaded frisbees
   private final Command m_autoCommand =
       // Start the command by spinning up the shooter...
-      new InstantCommand(m_shooter::enable, m_shooter).andThen(
-          // Wait until the shooter is at speed before feeding the frisbees
-          new WaitUntilCommand(m_shooter::atSetpoint),
-          // Start running the feeder
-          new InstantCommand(m_shooter::runFeeder, m_shooter),
-          // Shoot for the specified time
-          new WaitCommand(AutoConstants.kAutoShootTimeSeconds))
+      new InstantCommand(m_shooter::enable, m_shooter)
+          .andThen(
+              // Wait until the shooter is at speed before feeding the frisbees
+              new WaitUntilCommand(m_shooter::atSetpoint),
+              // Start running the feeder
+              new InstantCommand(m_shooter::runFeeder, m_shooter),
+              // Shoot for the specified time
+              new WaitCommand(AutoConstants.kAutoShootTimeSeconds))
           // Add a timeout (will end the command if, for instance, the shooter never gets up to
           // speed)
           .withTimeout(AutoConstants.kAutoTimeoutSeconds)
           // When the command ends, turn off the shooter and the feeder
-          .andThen(() -> {
-            m_shooter.disable();
-            m_shooter.stopFeeder();
-          });
+          .andThen(
+              () -> {
+                m_shooter.disable();
+                m_shooter.stopFeeder();
+              });
 
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -69,17 +64,18 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                         m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
     // Spin up the shooter when the 'A' button is pressed
@@ -91,22 +87,24 @@
         .whenPressed(new InstantCommand(m_shooter::disable, m_shooter));
 
     // Run the feeder when the 'X' button is held, but only if the shooter is at speed
-    new JoystickButton(m_driverController, Button.kX.value).whenPressed(new ConditionalCommand(
-        // Run the feeder
-        new InstantCommand(m_shooter::runFeeder, m_shooter),
-        // Do nothing
-        new InstantCommand(),
-        // Determine which of the above to do based on whether the shooter has reached the
-        // desired speed
-        m_shooter::atSetpoint)).whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
+    new JoystickButton(m_driverController, Button.kX.value)
+        .whenPressed(
+            new ConditionalCommand(
+                // Run the feeder
+                new InstantCommand(m_shooter::runFeeder, m_shooter),
+                // Do nothing
+                new InstantCommand(),
+                // Determine which of the above to do based on whether the shooter has reached the
+                // desired speed
+                m_shooter::atSetpoint))
+        .whenReleased(new InstantCommand(m_shooter::stopFeeder, m_shooter));
 
     // Drive at half speed when the bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
index b82cb75..5773da1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/DriveSubsystem.java
@@ -1,48 +1,53 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
 
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotors.setInverted(true);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +63,7 @@
     m_drive.arcadeDrive(fwd, rot);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -94,7 +97,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
index 8b0f3ca..f6f85f7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/frisbeebot/subsystems/ShooterSubsystem.java
@@ -1,33 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.frisbeebot.subsystems;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 
-import edu.wpi.first.wpilibj.examples.frisbeebot.Constants.ShooterConstants;
-
 public class ShooterSubsystem extends PIDSubsystem {
-  private final PWMVictorSPX m_shooterMotor = new PWMVictorSPX(ShooterConstants.kShooterMotorPort);
-  private final PWMVictorSPX m_feederMotor = new PWMVictorSPX(ShooterConstants.kFeederMotorPort);
+  private final PWMSparkMax m_shooterMotor = new PWMSparkMax(ShooterConstants.kShooterMotorPort);
+  private final PWMSparkMax m_feederMotor = new PWMSparkMax(ShooterConstants.kFeederMotorPort);
   private final Encoder m_shooterEncoder =
-      new Encoder(ShooterConstants.kEncoderPorts[0], ShooterConstants.kEncoderPorts[1],
-                  ShooterConstants.kEncoderReversed);
+      new Encoder(
+          ShooterConstants.kEncoderPorts[0],
+          ShooterConstants.kEncoderPorts[1],
+          ShooterConstants.kEncoderReversed);
   private final SimpleMotorFeedforward m_shooterFeedforward =
-      new SimpleMotorFeedforward(ShooterConstants.kSVolts,
-                                 ShooterConstants.kVVoltSecondsPerRotation);
+      new SimpleMotorFeedforward(
+          ShooterConstants.kSVolts, ShooterConstants.kVVoltSecondsPerRotation);
 
-  /**
-   * The shooter subsystem for the robot.
-   */
+  /** The shooter subsystem for the robot. */
   public ShooterSubsystem() {
     super(new PIDController(ShooterConstants.kP, ShooterConstants.kI, ShooterConstants.kD));
     getController().setTolerance(ShooterConstants.kShooterToleranceRPS);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
index 60aea50..8bf98fd 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
index ea26e28..12fddb3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -73,12 +64,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -91,12 +79,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -104,10 +89,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
index 40d3f11..e4c7412 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/RobotContainer.java
@@ -1,21 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.Joystick;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandBase;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.commands.Autonomous;
 import edu.wpi.first.wpilibj.examples.gearsbot.commands.CloseClaw;
 import edu.wpi.first.wpilibj.examples.gearsbot.commands.OpenClaw;
@@ -26,36 +15,37 @@
 import edu.wpi.first.wpilibj.examples.gearsbot.commands.SetWristSetpoint;
 import edu.wpi.first.wpilibj.examples.gearsbot.commands.TankDrive;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-  private final DriveTrain m_drivetrain = new DriveTrain();
+  private final Drivetrain m_drivetrain = new Drivetrain();
   private final Elevator m_elevator = new Elevator();
   private final Wrist m_wrist = new Wrist();
   private final Claw m_claw = new Claw();
 
-  private final Joystick m_joystick = new Joystick(0);
+  private final XboxController m_joystick = new XboxController(0);
 
   private final CommandBase m_autonomousCommand =
       new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Put Some buttons on the SmartDashboard
     SmartDashboard.putData("Elevator Bottom", new SetElevatorSetpoint(0, m_elevator));
-    SmartDashboard.putData("Elevator Platform", new SetElevatorSetpoint(0.2, m_elevator));
-    SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.3, m_elevator));
+    SmartDashboard.putData("Elevator Top", new SetElevatorSetpoint(0.25, m_elevator));
 
     SmartDashboard.putData("Wrist Horizontal", new SetWristSetpoint(0, m_wrist));
     SmartDashboard.putData("Raise Wrist", new SetWristSetpoint(-45, m_wrist));
@@ -63,12 +53,12 @@
     SmartDashboard.putData("Open Claw", new OpenClaw(m_claw));
     SmartDashboard.putData("Close Claw", new CloseClaw(m_claw));
 
-    SmartDashboard
-        .putData("Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
+    SmartDashboard.putData(
+        "Deliver Soda", new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
 
     // Assign default commands
-    m_drivetrain.setDefaultCommand(new TankDrive(() -> m_joystick.getY(Hand.kLeft),
-        () -> m_joystick.getY(Hand.kRight), m_drivetrain));
+    m_drivetrain.setDefaultCommand(
+        new TankDrive(m_joystick::getLeftY, m_joystick::getRightY, m_drivetrain));
 
     // Show what command your subsystem is running on the SmartDashboard
     SmartDashboard.putData(m_drivetrain);
@@ -81,10 +71,10 @@
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
     // Create some buttons
@@ -98,8 +88,8 @@
     final JoystickButton r1 = new JoystickButton(m_joystick, 12);
 
     // Connect the buttons to commands
-    dpadUp.whenPressed(new SetElevatorSetpoint(0.2, m_elevator));
-    dpadDown.whenPressed(new SetElevatorSetpoint(-0.2, m_elevator));
+    dpadUp.whenPressed(new SetElevatorSetpoint(0.25, m_elevator));
+    dpadDown.whenPressed(new SetElevatorSetpoint(0.0, m_elevator));
     dpadRight.whenPressed(new CloseClaw(m_claw));
     dpadLeft.whenPressed(new OpenClaw(m_claw));
 
@@ -109,7 +99,6 @@
     l2.whenPressed(new Autonomous(m_drivetrain, m_claw, m_wrist, m_elevator));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
index 0f0db70..599ed69 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Autonomous.java
@@ -1,27 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-/**
- * The main autonomous command to pickup and deliver the soda to the box.
- */
+/** The main autonomous command to pickup and deliver the soda to the box. */
 public class Autonomous extends SequentialCommandGroup {
-  /**
-   * Create a new autonomous command.
-   */
-  public Autonomous(DriveTrain drive, Claw claw, Wrist wrist, Elevator elevator) {
+  /** Create a new autonomous command. */
+  public Autonomous(Drivetrain drive, Claw claw, Wrist wrist, Elevator elevator) {
     addCommands(
         new PrepareToPickup(claw, wrist, elevator),
         new Pickup(claw, wrist, elevator),
@@ -30,10 +22,6 @@
         new Place(claw, wrist, elevator),
         new SetDistanceToBox(0.60, drive),
         // new DriveStraight(-2), // Use Encoders if ultrasonic is broken
-        parallel(
-            new SetWristSetpoint(-45, wrist),
-            new CloseClaw(claw)
-        )
-    );
+        parallel(new SetWristSetpoint(-45, wrist), new CloseClaw(claw)));
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
index 0ebce9a..8ef1deb 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/CloseClaw.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
-/**
- * Closes the claw for one second. Real robots should use sensors, stalling motors is BAD!
- */
+/** Closes the claw until the limit switch is tripped. */
 public class CloseClaw extends CommandBase {
   private final Claw m_claw;
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
index afed30d..ed47cc1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/DriveStraight.java
@@ -1,35 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-
 /**
- * Drive the given distance straight (negative values go backwards). Uses a
- * local PID controller to run a simple PID loop that is only enabled while this
- * command is running. The input is the averaged values of the left and right
- * encoders.
+ * Drive the given distance straight (negative values go backwards). Uses a local PID controller to
+ * run a simple PID loop that is only enabled while this command is running. The input is the
+ * averaged values of the left and right encoders.
  */
 public class DriveStraight extends PIDCommand {
-  private final DriveTrain m_drivetrain;
+  private final Drivetrain m_drivetrain;
 
   /**
    * Create a new DriveStraight command.
+   *
    * @param distance The distance to drive
    */
-  public DriveStraight(double distance, DriveTrain drivetrain) {
-    super(new PIDController(4, 0, 0),
-        drivetrain::getDistance,
-        distance,
-        d -> drivetrain.drive(d, d));
+  public DriveStraight(double distance, Drivetrain drivetrain) {
+    super(
+        new PIDController(4, 0, 0), drivetrain::getDistance, distance, d -> drivetrain.drive(d, d));
 
     m_drivetrain = drivetrain;
     addRequirements(m_drivetrain);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
index 9d944b4..cf1754e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/OpenClaw.java
@@ -1,19 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
-
-/**
- * Opens the claw for one second. Real robots should use sensors, stalling motors is BAD!
- */
+/** Opens the claw for one second. Real robots should use sensors, stalling motors is BAD! */
 public class OpenClaw extends WaitCommand {
   private final Claw m_claw;
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
index b51d106..4c7d267 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Pickup.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 /**
  * Pickup a soda can (if one is between the open claws) and get it in a safe state to drive around.
@@ -21,15 +16,13 @@
   /**
    * Create a new pickup command.
    *
-   * @param claw     The claw subsystem to use
-   * @param wrist    The wrist subsystem to use
+   * @param claw The claw subsystem to use
+   * @param wrist The wrist subsystem to use
    * @param elevator The elevator subsystem to use
    */
   public Pickup(Claw claw, Wrist wrist, Elevator elevator) {
     addCommands(
         new CloseClaw(claw),
-        parallel(
-            new SetWristSetpoint(-45, wrist),
-            new SetElevatorSetpoint(0.25, elevator)));
+        parallel(new SetWristSetpoint(-45, wrist), new SetElevatorSetpoint(0.25, elevator)));
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
index c57ffe2..442744c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/Place.java
@@ -1,28 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-/**
- * Place a held soda can onto the platform.
- */
+/** Place a held soda can onto the platform. */
 public class Place extends SequentialCommandGroup {
   /**
    * Create a new place command.
    *
-   * @param claw     The claw subsystem to use
-   * @param wrist    The wrist subsystem to use
+   * @param claw The claw subsystem to use
+   * @param wrist The wrist subsystem to use
    * @param elevator The elevator subsystem to use
    */
   public Place(Claw claw, Wrist wrist, Elevator elevator) {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
index 620c184..1ae5992 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/PrepareToPickup.java
@@ -1,34 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Claw;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-/**
- * Make sure the robot is in a state to pickup soda cans.
- */
+/** Make sure the robot is in a state to pickup soda cans. */
 public class PrepareToPickup extends SequentialCommandGroup {
   /**
    * Create a new prepare to pickup command.
    *
-   * @param claw     The claw subsystem to use
-   * @param wrist    The wrist subsystem to use
+   * @param claw The claw subsystem to use
+   * @param wrist The wrist subsystem to use
    * @param elevator The elevator subsystem to use
    */
   public PrepareToPickup(Claw claw, Wrist wrist, Elevator elevator) {
     addCommands(
         new OpenClaw(claw),
-        parallel(
-            new SetWristSetpoint(0, wrist),
-            new SetElevatorSetpoint(0, elevator)));
+        parallel(new SetWristSetpoint(0, wrist), new SetElevatorSetpoint(0, elevator)));
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
index b14ddc9..f8b291c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetDistanceToBox.java
@@ -1,35 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-
-
 /**
- * Drive until the robot is the given distance away from the box. Uses a local
- * PID controller to run a simple PID loop that is only enabled while this
- * command is running. The input is the averaged values of the left and right
- * encoders.
+ * Drive until the robot is the given distance away from the box. Uses a local PID controller to run
+ * a simple PID loop that is only enabled while this command is running. The input is the averaged
+ * values of the left and right encoders.
  */
 public class SetDistanceToBox extends PIDCommand {
-  private final DriveTrain m_drivetrain;
+  private final Drivetrain m_drivetrain;
 
   /**
    * Create a new set distance to box command.
    *
    * @param distance The distance away from the box to drive to
    */
-  public SetDistanceToBox(double distance, DriveTrain drivetrain) {
-    super(new PIDController(-2, 0, 0),
-        drivetrain::getDistanceToObstacle, distance,
+  public SetDistanceToBox(double distance, Drivetrain drivetrain) {
+    super(
+        new PIDController(-2, 0, 0),
+        drivetrain::getDistanceToObstacle,
+        distance,
         d -> drivetrain.drive(d, d));
 
     m_drivetrain = drivetrain;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
index 44c6677..a323305 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetElevatorSetpoint.java
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Elevator;
-
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * Move the elevator to a given location. This command finishes when it is within the tolerance, but
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
index d03211a..4c45ddf 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/SetWristSetpoint.java
@@ -1,16 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
 import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Wrist;
-
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 /**
  * Move the wrist to a given angle. This command finishes when it is within the tolerance, but
@@ -25,7 +20,7 @@
    * Create a new SetWristSetpoint command.
    *
    * @param setpoint The setpoint to set the wrist to
-   * @param wrist    The wrist to use
+   * @param wrist The wrist to use
    */
   public SetWristSetpoint(double setpoint, Wrist wrist) {
     m_wrist = wrist;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
index b5c24f2..009daad 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/commands/TankDrive.java
@@ -1,35 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.commands;
 
-
+import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 import java.util.function.DoubleSupplier;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
-import edu.wpi.first.wpilibj.examples.gearsbot.subsystems.DriveTrain;
-
-/**
- * Have the robot drive tank style.
- */
+/** Have the robot drive tank style. */
 public class TankDrive extends CommandBase {
-  private final DriveTrain m_drivetrain;
+  private final Drivetrain m_drivetrain;
   private final DoubleSupplier m_left;
   private final DoubleSupplier m_right;
 
   /**
    * Creates a new TankDrive command.
    *
-   * @param left       The control input for the left side of the drive
-   * @param right      The control input for the right sight of the drive
+   * @param left The control input for the left side of the drive
+   * @param right The control input for the right sight of the drive
    * @param drivetrain The drivetrain subsystem to drive
    */
-  public TankDrive(DoubleSupplier left, DoubleSupplier right, DriveTrain drivetrain) {
+  public TankDrive(DoubleSupplier left, DoubleSupplier right, Drivetrain drivetrain) {
     m_drivetrain = drivetrain;
     m_left = left;
     m_right = right;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
index 86859f1..d34d12a 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Claw.java
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
 import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Victor;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -20,9 +17,7 @@
   private final Victor m_motor = new Victor(7);
   private final DigitalInput m_contact = new DigitalInput(5);
 
-  /**
-   * Create a new claw subsystem.
-   */
+  /** Create a new claw subsystem. */
   public Claw() {
     // Let's name everything on the LiveWindow
     addChild("Motor", m_motor);
@@ -33,37 +28,27 @@
     SmartDashboard.putData("Claw switch", m_contact);
   }
 
-  /**
-   * Set the claw motor to move in the open direction.
-   */
+  /** Set the claw motor to move in the open direction. */
   public void open() {
     m_motor.set(-1);
   }
 
-  /**
-   * Set the claw motor to move in the close direction.
-   */
+  /** Set the claw motor to move in the close direction. */
   public void close() {
     m_motor.set(1);
   }
 
-  /**
-   * Stops the claw motor from moving.
-   */
+  /** Stops the claw motor from moving. */
   public void stop() {
     m_motor.set(0);
   }
 
-  /**
-   * Return true when the robot is grabbing an object hard enough to trigger the limit switch.
-   */
+  /** Return true when the robot is grabbing an object hard enough to trigger the limit switch. */
   public boolean isGrabbing() {
     return m_contact.get();
   }
 
-  /**
-   * Call log method every loop.
-   */
+  /** Call log method every loop. */
   @Override
   public void periodic() {
     log();
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
deleted file mode 100644
index 9c85429..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/DriveTrain.java
+++ /dev/null
@@ -1,132 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
-public class DriveTrain extends SubsystemBase {
-  /**
-   * The DriveTrain subsystem incorporates the sensors and actuators attached to the robots chassis.
-   * These include four drive motors, a left and right encoder and a gyro.
-   */
-  private final SpeedController m_leftMotor =
-      new SpeedControllerGroup(new PWMVictorSPX(0), new PWMVictorSPX(1));
-  private final SpeedController m_rightMotor =
-      new SpeedControllerGroup(new PWMVictorSPX(2), new PWMVictorSPX(3));
-
-  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
-
-  private final Encoder m_leftEncoder = new Encoder(1, 2);
-  private final Encoder m_rightEncoder = new Encoder(3, 4);
-  private final AnalogInput m_rangefinder = new AnalogInput(6);
-  private final AnalogGyro m_gyro = new AnalogGyro(1);
-
-  /**
-   * Create a new drive train subsystem.
-   */
-  public DriveTrain() {
-    super();
-
-    // Encoders may measure differently in the real world and in
-    // simulation. In this example the robot moves 0.042 barleycorns
-    // per tick in the real world, but the simulated encoders
-    // simulate 360 tick encoders. This if statement allows for the
-    // real robot to handle this difference in devices.
-    if (Robot.isReal()) {
-      m_leftEncoder.setDistancePerPulse(0.042);
-      m_rightEncoder.setDistancePerPulse(0.042);
-    } else {
-      // Circumference in ft = 4in/12(in/ft)*PI
-      m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
-      m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
-    }
-
-    // Let's name the sensors on the LiveWindow
-    addChild("Drive", m_drive);
-    addChild("Left Encoder", m_leftEncoder);
-    addChild("Right Encoder", m_rightEncoder);
-    addChild("Rangefinder", m_rangefinder);
-    addChild("Gyro", m_gyro);
-  }
-
-  /**
-   * The log method puts interesting information to the SmartDashboard.
-   */
-  public void log() {
-    SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
-    SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
-    SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
-    SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
-    SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
-  }
-
-  /**
-   * Tank style driving for the DriveTrain.
-   *
-   * @param left  Speed in range [-1,1]
-   * @param right Speed in range [-1,1]
-   */
-  public void drive(double left, double right) {
-    m_drive.tankDrive(left, right);
-  }
-
-  /**
-   * Get the robot's heading.
-   *
-   * @return The robots heading in degrees.
-   */
-  public double getHeading() {
-    return m_gyro.getAngle();
-  }
-
-  /**
-   * Reset the robots sensors to the zero states.
-   */
-  public void reset() {
-    m_gyro.reset();
-    m_leftEncoder.reset();
-    m_rightEncoder.reset();
-  }
-
-  /**
-   * Get the average distance of the encoders since the last reset.
-   *
-   * @return The distance driven (average of left and right encoders).
-   */
-  public double getDistance() {
-    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
-  }
-
-  /**
-   * Get the distance to the obstacle.
-   *
-   * @return The distance to the obstacle detected by the rangefinder.
-   */
-  public double getDistanceToObstacle() {
-    // Really meters in simulation since it's a rangefinder...
-    return m_rangefinder.getAverageVoltage();
-  }
-
-  /**
-   * Call log method every loop.
-   */
-  @Override
-  public void periodic() {
-    log();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
new file mode 100644
index 0000000..44f455d
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Drivetrain.java
@@ -0,0 +1,126 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
+
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Drivetrain extends SubsystemBase {
+  /**
+   * The Drivetrain subsystem incorporates the sensors and actuators attached to the robots chassis.
+   * These include four drive motors, a left and right encoder and a gyro.
+   */
+  private final MotorController m_leftMotor =
+      new MotorControllerGroup(new PWMSparkMax(0), new PWMSparkMax(1));
+
+  private final MotorController m_rightMotor =
+      new MotorControllerGroup(new PWMSparkMax(2), new PWMSparkMax(3));
+
+  private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  private final Encoder m_leftEncoder = new Encoder(1, 2);
+  private final Encoder m_rightEncoder = new Encoder(3, 4);
+  private final AnalogInput m_rangefinder = new AnalogInput(6);
+  private final AnalogGyro m_gyro = new AnalogGyro(1);
+
+  /** Create a new drivetrain subsystem. */
+  public Drivetrain() {
+    super();
+
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+
+    // Encoders may measure differently in the real world and in
+    // simulation. In this example the robot moves 0.042 barleycorns
+    // per tick in the real world, but the simulated encoders
+    // simulate 360 tick encoders. This if statement allows for the
+    // real robot to handle this difference in devices.
+    if (Robot.isReal()) {
+      m_leftEncoder.setDistancePerPulse(0.042);
+      m_rightEncoder.setDistancePerPulse(0.042);
+    } else {
+      // Circumference = diameter in feet * pi. 360 tick simulated encoders.
+      m_leftEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
+      m_rightEncoder.setDistancePerPulse((4.0 / 12.0 * Math.PI) / 360.0);
+    }
+
+    // Let's name the sensors on the LiveWindow
+    addChild("Drive", m_drive);
+    addChild("Left Encoder", m_leftEncoder);
+    addChild("Right Encoder", m_rightEncoder);
+    addChild("Rangefinder", m_rangefinder);
+    addChild("Gyro", m_gyro);
+  }
+
+  /** The log method puts interesting information to the SmartDashboard. */
+  public void log() {
+    SmartDashboard.putNumber("Left Distance", m_leftEncoder.getDistance());
+    SmartDashboard.putNumber("Right Distance", m_rightEncoder.getDistance());
+    SmartDashboard.putNumber("Left Speed", m_leftEncoder.getRate());
+    SmartDashboard.putNumber("Right Speed", m_rightEncoder.getRate());
+    SmartDashboard.putNumber("Gyro", m_gyro.getAngle());
+  }
+
+  /**
+   * Tank style driving for the Drivetrain.
+   *
+   * @param left Speed in range [-1,1]
+   * @param right Speed in range [-1,1]
+   */
+  public void drive(double left, double right) {
+    m_drive.tankDrive(left, right);
+  }
+
+  /**
+   * Get the robot's heading.
+   *
+   * @return The robots heading in degrees.
+   */
+  public double getHeading() {
+    return m_gyro.getAngle();
+  }
+
+  /** Reset the robots sensors to the zero states. */
+  public void reset() {
+    m_gyro.reset();
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  /**
+   * Get the average distance of the encoders since the last reset.
+   *
+   * @return The distance driven (average of left and right encoders).
+   */
+  public double getDistance() {
+    return (m_leftEncoder.getDistance() + m_rightEncoder.getDistance()) / 2;
+  }
+
+  /**
+   * Get the distance to the obstacle.
+   *
+   * @return The distance to the obstacle detected by the rangefinder.
+   */
+  public double getDistanceToObstacle() {
+    // Really meters in simulation since it's a rangefinder...
+    return m_rangefinder.getAverageVoltage();
+  }
+
+  /** Call log method every loop. */
+  @Override
+  public void periodic() {
+    log();
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
index 5492046..e174595 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Elevator.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
 /**
  * The elevator subsystem uses PID to go to a given height. Unfortunately, in it's current state PID
  * values for simulation are different than in the real world do to minor differences.
@@ -28,9 +24,7 @@
   private static final double kP_simulation = 18;
   private static final double kI_simulation = 0.2;
 
-  /**
-   * Create a new elevator subsystem.
-   */
+  /** Create a new elevator subsystem. */
   public Elevator() {
     super(new PIDController(kP_real, kI_real, 0));
     if (Robot.isSimulation()) { // Check for simulation and update PID values
@@ -53,9 +47,7 @@
     addChild("Pot", m_pot);
   }
 
-  /**
-   * The log method puts interesting information to the SmartDashboard.
-   */
+  /** The log method puts interesting information to the SmartDashboard. */
   public void log() {
     SmartDashboard.putData("Elevator Pot", m_pot);
   }
@@ -68,17 +60,13 @@
     return m_pot.get();
   }
 
-  /**
-   * Use the motor as the PID output. This method is automatically called by the subsystem.
-   */
+  /** Use the motor as the PID output. This method is automatically called by the subsystem. */
   @Override
   public void useOutput(double output, double setpoint) {
     m_motor.set(output);
   }
 
-  /**
-   * Call log method every loop.
-   */
+  /** Call log method every loop. */
   @Override
   public void periodic() {
     log();
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
index 1b32139..f176316 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gearsbot/subsystems/Wrist.java
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gearsbot.subsystems;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.PIDSubsystem;
 
-import edu.wpi.first.wpilibj.examples.gearsbot.Robot;
-
 /**
  * The wrist subsystem is like the elevator, but with a rotational joint instead of a linear joint.
  */
@@ -25,9 +21,7 @@
   private static final double kP_real = 1;
   private static final double kP_simulation = 0.05;
 
-  /**
-   * Create a new wrist subsystem.
-   */
+  /** Create a new wrist subsystem. */
   public Wrist() {
     super(new PIDController(kP_real, 0, 0));
     if (Robot.isSimulation()) { // Check for simulation and update PID values
@@ -50,9 +44,7 @@
     addChild("Pot", m_pot);
   }
 
-  /**
-   * The log method puts interesting information to the SmartDashboard.
-   */
+  /** The log method puts interesting information to the SmartDashboard. */
   public void log() {
     SmartDashboard.putData("Wrist Angle", m_pot);
   }
@@ -65,17 +57,13 @@
     return m_pot.get();
   }
 
-  /**
-   * Use the motor as the PID output. This method is automatically called by the subsystem.
-   */
+  /** Use the motor as the PID output. This method is automatically called by the subsystem. */
   @Override
   public void useOutput(double output, double setpoint) {
     m_motor.set(output);
   }
 
-  /**
-   * Call log method every loop.
-   */
+  /** Call log method every loop. */
   @Override
   public void periodic() {
     log();
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java
index 282f922..dd467ee 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gettingstarted;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
index 8b2f249..0a9a2b0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gettingstarted/Robot.java
@@ -1,51 +1,42 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gettingstarted;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the manifest file in the resource
  * directory.
  */
 public class Robot extends TimedRobot {
-  private final DifferentialDrive m_robotDrive
-      = new DifferentialDrive(new PWMVictorSPX(0), new PWMVictorSPX(1));
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1));
   private final Joystick m_stick = new Joystick(0);
   private final Timer m_timer = new Timer();
 
   /**
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
    */
   @Override
-  public void robotInit() {
-  }
+  public void robotInit() {}
 
-  /**
-   * This function is run once each time the robot enters autonomous mode.
-   */
+  /** This function is run once each time the robot enters autonomous mode. */
   @Override
   public void autonomousInit() {
     m_timer.reset();
     m_timer.start();
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
     // Drive for 2 seconds
@@ -56,25 +47,21 @@
     }
   }
 
-  /**
-   * This function is called once each time the robot enters teleoperated mode.
-   */
+  /** This function is called once each time the robot enters teleoperated mode. */
   @Override
-  public void teleopInit() {
-  }
+  public void teleopInit() {}
 
-  /**
-   * This function is called periodically during teleoperated mode.
-   */
+  /** This function is called periodically during teleoperated mode. */
   @Override
   public void teleopPeriodic() {
     m_robotDrive.arcadeDrive(m_stick.getY(), m_stick.getX());
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called once each time the robot enters test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testInit() {}
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java
index 4c5574e..e8f2df6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyro;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
index 39b2ecb..3d8353b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyro/Robot.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyro;
 
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate how to use a gyro sensor to make a
- * robot drive straight. This program uses a joystick to drive forwards and
- * backwards while the gyro is used for direction keeping.
+ * This is a sample program to demonstrate how to use a gyro sensor to make a robot drive straight.
+ * This program uses a joystick to drive forwards and backwards while the gyro is used for direction
+ * keeping.
  */
 public class Robot extends TimedRobot {
   private static final double kAngleSetpoint = 0.0;
@@ -31,9 +28,8 @@
   private static final int kGyroPort = 0;
   private static final int kJoystickPort = 0;
 
-  private final DifferentialDrive m_myRobot
-      = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
-      new PWMVictorSPX(kRightMotorPort));
+  private final DifferentialDrive m_myRobot =
+      new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
   private final AnalogGyro m_gyro = new AnalogGyro(kGyroPort);
   private final Joystick m_joystick = new Joystick(kJoystickPort);
 
@@ -43,8 +39,8 @@
   }
 
   /**
-   * The motor speed is set from the joystick while the RobotDrive turning
-   * value is assigned from the error between the setpoint and the gyro angle.
+   * The motor speed is set from the joystick while the DifferentialDrive turning value is assigned
+   * from the error between the setpoint and the gyro angle.
    */
   @Override
   public void teleopPeriodic() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
index 473d6e1..b59c23d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -22,8 +19,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -51,6 +48,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
index f18e95c..9e6bfe7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
index 40c6db9..fff4d8c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
index 1b94fea..1356a37 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/RobotContainer.java
@@ -1,45 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import static edu.wpi.first.wpilibj.PS4Controller.Button;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj.PS4Controller;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
+import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.PIDCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.OIConstants;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngle;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands.TurnToAngleProfiled;
-import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
 
   // The driver's controller
-  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+  PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -49,47 +42,51 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                         m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link PS4Controller}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
     // Drive at half speed when the right bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kR1.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
 
     // Stabilize robot to drive straight with gyro when left bumper is held
-    new JoystickButton(m_driverController, Button.kBumperLeft.value).whenHeld(new PIDCommand(
-        new PIDController(DriveConstants.kStabilizationP, DriveConstants.kStabilizationI,
-                          DriveConstants.kStabilizationD),
-        // Close the loop on the turn rate
-        m_robotDrive::getTurnRate,
-        // Setpoint is 0
-        0,
-        // Pipe the output to the turning controls
-        output -> m_robotDrive.arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft), output),
-        // Require the robot drive
-        m_robotDrive));
+    new JoystickButton(m_driverController, Button.kL1.value)
+        .whenHeld(
+            new PIDCommand(
+                new PIDController(
+                    DriveConstants.kStabilizationP,
+                    DriveConstants.kStabilizationI,
+                    DriveConstants.kStabilizationD),
+                // Close the loop on the turn rate
+                m_robotDrive::getTurnRate,
+                // Setpoint is 0
+                0,
+                // Pipe the output to the turning controls
+                output -> m_robotDrive.arcadeDrive(m_driverController.getLeftY(), output),
+                // Require the robot drive
+                m_robotDrive));
 
     // Turn to 90 degrees when the 'X' button is pressed, with a 5 second timeout
-    new JoystickButton(m_driverController, Button.kX.value)
+    new JoystickButton(m_driverController, Button.kCross.value)
         .whenPressed(new TurnToAngle(90, m_robotDrive).withTimeout(5));
 
-    // Turn to -90 degrees with a profile when the 'A' button is pressed, with a 5 second timeout
-    new JoystickButton(m_driverController, Button.kA.value)
+    // Turn to -90 degrees with a profile when the Circle button is pressed, with a 5 second timeout
+    new JoystickButton(m_driverController, Button.kCircle.value)
         .whenPressed(new TurnToAngleProfiled(-90, m_robotDrive).withTimeout(5));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
index 6c8ea36..053c644 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngle.java
@@ -1,27 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj2.command.PIDCommand;
-
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
 
-/**
- * A command that will turn the robot to the specified angle.
- */
+/** A command that will turn the robot to the specified angle. */
 public class TurnToAngle extends PIDCommand {
   /**
    * Turns to robot to the specified angle.
    *
    * @param targetAngleDegrees The angle to turn to
-   * @param drive              The drive subsystem to use
+   * @param drive The drive subsystem to use
    */
   public TurnToAngle(double targetAngleDegrees, DriveSubsystem drive) {
     super(
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
index 44f24d3..1481fab 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/commands/TurnToAngleProfiled.java
@@ -1,35 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands.commands;
 
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
-
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.ProfiledPIDCommand;
 
-/**
- * A command that will turn the robot to the specified angle using a motion profile.
- */
+/** A command that will turn the robot to the specified angle using a motion profile. */
 public class TurnToAngleProfiled extends ProfiledPIDCommand {
   /**
    * Turns to robot to the specified angle using a motion profile.
    *
    * @param targetAngleDegrees The angle to turn to
-   * @param drive              The drive subsystem to use
+   * @param drive The drive subsystem to use
    */
   public TurnToAngleProfiled(double targetAngleDegrees, DriveSubsystem drive) {
     super(
-        new ProfiledPIDController(DriveConstants.kTurnP, DriveConstants.kTurnI,
-                                  DriveConstants.kTurnD, new TrapezoidProfile.Constraints(
-            DriveConstants.kMaxTurnRateDegPerS,
-            DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
+        new ProfiledPIDController(
+            DriveConstants.kTurnP,
+            DriveConstants.kTurnI,
+            DriveConstants.kTurnD,
+            new TrapezoidProfile.Constraints(
+                DriveConstants.kMaxTurnRateDegPerS,
+                DriveConstants.kMaxTurnAccelerationDegPerSSquared)),
         // Close loop on heading
         drive::getHeading,
         // Set reference to target
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
index 7bdeb24..a2fdf6e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyrodrivecommands/subsystems/DriveSubsystem.java
@@ -1,53 +1,58 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyrodrivecommands.subsystems;
 
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.gyrodrivecommands.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
   // The gyro sensor
   private final Gyro m_gyro = new ADXRS450_Gyro();
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotors.setInverted(true);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -63,9 +68,7 @@
     m_drive.arcadeDrive(fwd, rot);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -99,7 +102,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
@@ -107,9 +110,7 @@
     m_drive.setMaxOutput(maxOutput);
   }
 
-  /**
-   * Zeroes the heading of the robot.
-   */
+  /** Zeroes the heading of the robot. */
   public void zeroHeading() {
     m_gyro.reset();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java
index 63300b6..8c952f1 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyromecanum;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
index 622fda4..f4b9a97 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/gyromecanum/Robot.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.gyromecanum;
 
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program that uses mecanum drive with a gyro sensor to
- * maintain rotation vectorsin relation to the starting orientation of the robot
- * (field-oriented controls).
+ * This is a sample program that uses mecanum drive with a gyro sensor to maintain rotation
+ * vectorsin relation to the starting orientation of the robot (field-oriented controls).
  */
 public class Robot extends TimedRobot {
   // gyro calibration constant, may need to be adjusted;
@@ -36,10 +32,10 @@
 
   @Override
   public void robotInit() {
-    PWMVictorSPX frontLeft = new PWMVictorSPX(kFrontLeftChannel);
-    PWMVictorSPX rearLeft = new PWMVictorSPX(kRearLeftChannel);
-    PWMVictorSPX frontRight = new PWMVictorSPX(kFrontRightChannel);
-    PWMVictorSPX rearRight = new PWMVictorSPX(kRearRightChannel);
+    PWMSparkMax frontLeft = new PWMSparkMax(kFrontLeftChannel);
+    PWMSparkMax rearLeft = new PWMSparkMax(kRearLeftChannel);
+    PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
+    PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
 
     // Invert the left side motors.
     // You may need to change or remove this to match your robot.
@@ -51,12 +47,10 @@
     m_gyro.setSensitivity(kVoltsPerDegreePerSecond);
   }
 
-  /**
-   * Mecanum drive is used with the gyro angle as an input.
-   */
+  /** Mecanum drive is used with the gyro angle as an input. */
   @Override
   public void teleopPeriodic() {
-    m_robotDrive.driveCartesian(m_joystick.getX(), m_joystick.getY(),
-        m_joystick.getZ(), m_gyro.getAngle());
+    m_robotDrive.driveCartesian(
+        m_joystick.getX(), m_joystick.getY(), m_joystick.getZ(), m_gyro.getAngle());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
index 8029888..30e8aad 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -22,8 +19,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -36,7 +33,7 @@
 
   public static final class HatchConstants {
     public static final int kHatchSolenoidModule = 0;
-    public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+    public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
   }
 
   public static final class AutoConstants {
@@ -46,6 +43,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
index 3852d41..114bc14 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
index c0c78ac..8f16ce5 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -73,12 +64,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -91,12 +79,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -104,10 +89,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
index d49fba4..2207379 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/RobotContainer.java
@@ -1,35 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import static edu.wpi.first.wpilibj.PS4Controller.Button;
 
+import edu.wpi.first.wpilibj.PS4Controller;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.commands.ComplexAutoCommand;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -39,18 +34,18 @@
   // The autonomous routines
 
   // A simple auto routine that drives forward a specified distance, and then stops.
-  private final Command m_simpleAuto = new StartEndCommand(
-      // Start driving forward at the start of the command
-      () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
-      // Stop driving at the end of the command
-      () -> m_robotDrive.arcadeDrive(0, 0),
-      // Requires the drive subsystem
-      m_robotDrive)
-      // Reset the encoders before starting
-      .beforeStarting(m_robotDrive::resetEncoders)
-      // End the command when the robot's driven distance exceeds the desired value
-      .withInterrupt(
-          () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches);
+  private final Command m_simpleAuto =
+      new FunctionalCommand(
+          // Reset encoders on command start
+          m_robotDrive::resetEncoders,
+          // Drive forward while the command is executing
+          () -> m_robotDrive.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
+          // Stop driving at the end of the command
+          interrupt -> m_robotDrive.arcadeDrive(0, 0),
+          // End the command when the robot's driven distance exceeds the desired value
+          () -> m_robotDrive.getAverageEncoderDistance() >= AutoConstants.kAutoDriveDistanceInches,
+          // Require the drive subsystem
+          m_robotDrive);
 
   // A complex auto routine that drives forward, drops a hatch, and then drives backward.
   private final Command m_complexAuto = new ComplexAutoCommand(m_robotDrive, m_hatchSubsystem);
@@ -59,11 +54,9 @@
   SendableChooser<Command> m_chooser = new SendableChooser<>();
 
   // The driver's controller
-  XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
+  PS4Controller m_driverController = new PS4Controller(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -73,9 +66,11 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                         m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
 
     // Add commands to the autonomous command chooser
     m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
@@ -86,25 +81,24 @@
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link PS4Controller}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
-    // Grab the hatch when the 'A' button is pressed.
-    new JoystickButton(m_driverController, Button.kA.value)
+    // Grab the hatch when the Circle button is pressed.
+    new JoystickButton(m_driverController, Button.kCircle.value)
         .whenPressed(new InstantCommand(m_hatchSubsystem::grabHatch, m_hatchSubsystem));
-    // Release the hatch when the 'B' button is pressed.
-    new JoystickButton(m_driverController, Button.kB.value)
+    // Release the hatch when the Square button is pressed.
+    new JoystickButton(m_driverController, Button.kSquare.value)
         .whenPressed(new InstantCommand(m_hatchSubsystem::releaseHatch, m_hatchSubsystem));
-    // While holding the shoulder button, drive at half speed
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    // While holding R1, drive at half speed
+    new JoystickButton(m_driverController, Button.kR1.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
index 45674da..1faf45a 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/commands/ComplexAutoCommand.java
@@ -1,23 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined.commands;
 
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.StartEndCommand;
-
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.FunctionalCommand;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-/**
- * A complex auto command that drives forward, releases a hatch, and then drives backward.
- */
+/** A complex auto command that drives forward, releases a hatch, and then drives backward. */
 public class ComplexAutoCommand extends SequentialCommandGroup {
   /**
    * Creates a new ComplexAutoCommand.
@@ -28,28 +22,36 @@
   public ComplexAutoCommand(DriveSubsystem driveSubsystem, HatchSubsystem hatchSubsystem) {
     addCommands(
         // Drive forward up to the front of the cargo ship
-        new StartEndCommand(
-            // Start driving forward at the start of the command
+        new FunctionalCommand(
+            // Reset encoders on command start
+            driveSubsystem::resetEncoders,
+            // Drive forward while the command is executing
             () -> driveSubsystem.arcadeDrive(AutoConstants.kAutoDriveSpeed, 0),
             // Stop driving at the end of the command
-            () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
-            // Reset the encoders before starting
-            .beforeStarting(driveSubsystem::resetEncoders)
+            interrupt -> driveSubsystem.arcadeDrive(0, 0),
             // End the command when the robot's driven distance exceeds the desired value
-            .withInterrupt(() -> driveSubsystem.getAverageEncoderDistance()
-                >= AutoConstants.kAutoDriveDistanceInches),
+            () ->
+                driveSubsystem.getAverageEncoderDistance()
+                    >= AutoConstants.kAutoDriveDistanceInches,
+            // Require the drive subsystem
+            driveSubsystem),
 
         // Release the hatch
         new InstantCommand(hatchSubsystem::releaseHatch, hatchSubsystem),
 
         // Drive backward the specified distance
-        new StartEndCommand(
+        new FunctionalCommand(
+            // Reset encoders on command start
+            driveSubsystem::resetEncoders,
+            // Drive backward while the command is executing
             () -> driveSubsystem.arcadeDrive(-AutoConstants.kAutoDriveSpeed, 0),
-            () -> driveSubsystem.arcadeDrive(0, 0), driveSubsystem)
-            .beforeStarting(driveSubsystem::resetEncoders)
-            .withInterrupt(
-                () -> driveSubsystem.getAverageEncoderDistance()
-                    <= -AutoConstants.kAutoBackupDistanceInches));
+            // Stop driving at the end of the command
+            interrupt -> driveSubsystem.arcadeDrive(0, 0),
+            // End the command when the robot's driven distance exceeds the desired value
+            () ->
+                driveSubsystem.getAverageEncoderDistance()
+                    <= AutoConstants.kAutoBackupDistanceInches,
+            // Require the drive subsystem
+            driveSubsystem));
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
index 5d16a44..115ee02 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/DriveSubsystem.java
@@ -1,48 +1,53 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
 
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotors.setInverted(true);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +63,7 @@
     m_drive.arcadeDrive(fwd, rot);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -94,7 +97,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
index 92e4979..fc508f3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbotinlined/subsystems/HatchSubsystem.java
@@ -1,38 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbotinlined.subsystems;
 
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
-
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
 
-/**
- * A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}.
- */
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.hatchbotinlined.Constants.HatchConstants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** A hatch mechanism actuated by a single {@link edu.wpi.first.wpilibj.DoubleSolenoid}. */
 public class HatchSubsystem extends SubsystemBase {
   private final DoubleSolenoid m_hatchSolenoid =
-      new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0],
-                         HatchConstants.kHatchSolenoidPorts[1]);
+      new DoubleSolenoid(
+          PneumaticsModuleType.CTREPCM,
+          HatchConstants.kHatchSolenoidPorts[0],
+          HatchConstants.kHatchSolenoidPorts[1]);
 
-  /**
-   * Grabs the hatch.
-   */
+  /** Grabs the hatch. */
   public void grabHatch() {
     m_hatchSolenoid.set(kForward);
   }
 
-  /**
-   * Releases the hatch.
-   */
+  /** Releases the hatch. */
   public void releaseHatch() {
     m_hatchSolenoid.set(kReverse);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
index 2674b50..f02fa12 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Constants.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -22,8 +19,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -36,7 +33,7 @@
 
   public static final class HatchConstants {
     public static final int kHatchSolenoidModule = 0;
-    public static final int[] kHatchSolenoidPorts = new int[]{0, 1};
+    public static final int[] kHatchSolenoidPorts = new int[] {0, 1};
   }
 
   public static final class AutoConstants {
@@ -46,6 +43,6 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
index f09858c..0d084c3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
index 8747f35..f6bcdcd 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,12 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -111,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
index 0784dde..1f1024f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/RobotContainer.java
@@ -1,19 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import static edu.wpi.first.wpilibj.XboxController.Button;
 
+import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ComplexAuto;
@@ -24,14 +17,16 @@
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.commands.ReleaseHatch;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -42,8 +37,8 @@
 
   // A simple auto routine that drives forward a specified distance, and then stops.
   private final Command m_simpleAuto =
-      new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed,
-                        m_robotDrive);
+      new DriveDistance(
+          AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, m_robotDrive);
 
   // A complex auto routine that drives forward, drops a hatch, and then drives backward.
   private final Command m_complexAuto = new ComplexAuto(m_robotDrive, m_hatchSubsystem);
@@ -54,9 +49,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -67,9 +60,7 @@
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
         new DefaultDrive(
-            m_robotDrive,
-            () -> m_driverController.getY(GenericHID.Hand.kLeft),
-            () -> m_driverController.getX(GenericHID.Hand.kRight)));
+            m_robotDrive, m_driverController::getLeftY, m_driverController::getRightX));
 
     // Add commands to the autonomous command chooser
     m_chooser.setDefaultOption("Simple Auto", m_simpleAuto);
@@ -80,10 +71,10 @@
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
     // Grab the hatch when the 'A' button is pressed.
@@ -93,11 +84,10 @@
     new JoystickButton(m_driverController, Button.kB.value)
         .whenPressed(new ReleaseHatch(m_hatchSubsystem));
     // While holding the shoulder button, drive at half speed
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenHeld(new HalveDriveSpeed(m_robotDrive));
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
index 5f776a5..736a8d9 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ComplexAuto.java
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
-/**
- * A complex auto command that drives forward, releases a hatch, and then drives backward.
- */
+/** A complex auto command that drives forward, releases a hatch, and then drives backward. */
 public class ComplexAuto extends SequentialCommandGroup {
   /**
    * Creates a new ComplexAuto.
@@ -26,15 +20,14 @@
   public ComplexAuto(DriveSubsystem drive, HatchSubsystem hatch) {
     addCommands(
         // Drive forward the specified distance
-        new DriveDistance(AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed,
-                          drive),
+        new DriveDistance(
+            AutoConstants.kAutoDriveDistanceInches, AutoConstants.kAutoDriveSpeed, drive),
 
         // Release the hatch
         new ReleaseHatch(hatch),
 
         // Drive backward the specified distance
-        new DriveDistance(AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed,
-                          drive));
+        new DriveDistance(
+            AutoConstants.kAutoBackupDistanceInches, -AutoConstants.kAutoDriveSpeed, drive));
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
index 9399c30..460e787 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DefaultDrive.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 
-import java.util.function.DoubleSupplier;
-
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import java.util.function.DoubleSupplier;
 
 /**
  * A command to drive the robot with joystick input (passed in as {@link DoubleSupplier}s). Written
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
index d4abd71..04a3c0b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/DriveDistance.java
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class DriveDistance extends CommandBase {
   private final DriveSubsystem m_drive;
@@ -27,6 +23,7 @@
     m_distance = inches;
     m_speed = speed;
     m_drive = drive;
+    addRequirements(m_drive);
   }
 
   @Override
@@ -36,6 +33,11 @@
   }
 
   @Override
+  public void execute() {
+    m_drive.arcadeDrive(m_speed, 0);
+  }
+
+  @Override
   public void end(boolean interrupted) {
     m_drive.arcadeDrive(0, 0);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
index a30da45..9cd1550 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/GrabHatch.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
-import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-
 /**
- * A simple command that grabs a hatch with the {@link HatchSubsystem}.  Written explicitly for
- * pedagogical purposes.  Actual code should inline a command this simple with {@link
+ * A simple command that grabs a hatch with the {@link HatchSubsystem}. Written explicitly for
+ * pedagogical purposes. Actual code should inline a command this simple with {@link
  * edu.wpi.first.wpilibj2.command.InstantCommand}.
  */
 public class GrabHatch extends CommandBase {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
index bd6c093..c34d951 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/HalveDriveSpeed.java
@@ -1,15 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 
-import edu.wpi.first.wpilibj2.command.CommandBase;
-
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.CommandBase;
 
 public class HalveDriveSpeed extends CommandBase {
   private final DriveSubsystem m_drive;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
index 1e6f0a0..6320486 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/commands/ReleaseHatch.java
@@ -1,19 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.commands;
 
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 
-import edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems.HatchSubsystem;
-
-/**
- * A command that releases the hatch.
- */
+/** A command that releases the hatch. */
 public class ReleaseHatch extends InstantCommand {
   public ReleaseHatch(HatchSubsystem subsystem) {
     super(subsystem::releaseHatch, subsystem);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
index f49abb9..d1236e2 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/DriveSubsystem.java
@@ -1,48 +1,53 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
 
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotors.setInverted(true);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -58,9 +63,7 @@
     m_drive.arcadeDrive(fwd, rot);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -94,7 +97,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
index 47da829..3526d17 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hatchbottraditional/subsystems/HatchSubsystem.java
@@ -1,38 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hatchbottraditional.subsystems;
 
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
-import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
-
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kForward;
 import static edu.wpi.first.wpilibj.DoubleSolenoid.Value.kReverse;
 
-/**
- * A hatch mechanism actuated by a single {@link DoubleSolenoid}.
- */
+import edu.wpi.first.wpilibj.DoubleSolenoid;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import edu.wpi.first.wpilibj.examples.hatchbottraditional.Constants.HatchConstants;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/** A hatch mechanism actuated by a single {@link DoubleSolenoid}. */
 public class HatchSubsystem extends SubsystemBase {
   private final DoubleSolenoid m_hatchSolenoid =
-      new DoubleSolenoid(HatchConstants.kHatchSolenoidModule, HatchConstants.kHatchSolenoidPorts[0],
-                         HatchConstants.kHatchSolenoidPorts[1]);
+      new DoubleSolenoid(
+          PneumaticsModuleType.CTREPCM,
+          HatchConstants.kHatchSolenoidPorts[0],
+          HatchConstants.kHatchSolenoidPorts[1]);
 
-  /**
-   * Grabs the hatch.
-   */
+  /** Grabs the hatch. */
   public void grabHatch() {
     m_hatchSolenoid.set(kForward);
   }
 
-  /**
-   * Releases the hatch.
-   */
+  /** Releases the hatch. */
   public void releaseHatch() {
     m_hatchSolenoid.set(kReverse);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java
index 297bbf1..5d139ca 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hidrumble;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java
old mode 100755
new mode 100644
index c7f6029..1c07516
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/hidrumble/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.hidrumble;
 
@@ -11,9 +8,7 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
-/**
- * This is a demo program showing the use of GenericHID's rumble feature.
- */
+/** This is a demo program showing the use of GenericHID's rumble feature. */
 public class Robot extends TimedRobot {
   private final XboxController m_hid = new XboxController(0);
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java
index a0210b4..f2fb11f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.intermediatevision;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
index 526e849..e6e8b65 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/intermediatevision/Robot.java
@@ -1,68 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.intermediatevision;
 
+import edu.wpi.first.cameraserver.CameraServer;
+import edu.wpi.first.cscore.CvSink;
+import edu.wpi.first.cscore.CvSource;
+import edu.wpi.first.cscore.UsbCamera;
+import edu.wpi.first.wpilibj.TimedRobot;
 import org.opencv.core.Mat;
 import org.opencv.core.Point;
 import org.opencv.core.Scalar;
 import org.opencv.imgproc.Imgproc;
 
-import edu.wpi.cscore.CvSink;
-import edu.wpi.cscore.CvSource;
-import edu.wpi.cscore.UsbCamera;
-import edu.wpi.first.cameraserver.CameraServer;
-import edu.wpi.first.wpilibj.TimedRobot;
-
 /**
- * This is a demo program showing the use of OpenCV to do vision processing. The
- * image is acquired from the USB camera, then a rectangle is put on the image
- * and sent to the dashboard. OpenCV has many methods for different types of
- * processing.
+ * This is a demo program showing the use of OpenCV to do vision processing. The image is acquired
+ * from the USB camera, then a rectangle is put on the image and sent to the dashboard. OpenCV has
+ * many methods for different types of processing.
  */
 public class Robot extends TimedRobot {
   Thread m_visionThread;
 
   @Override
   public void robotInit() {
-    m_visionThread = new Thread(() -> {
-      // Get the UsbCamera from CameraServer
-      UsbCamera camera = CameraServer.getInstance().startAutomaticCapture();
-      // Set the resolution
-      camera.setResolution(640, 480);
+    m_visionThread =
+        new Thread(
+            () -> {
+              // Get the UsbCamera from CameraServer
+              UsbCamera camera = CameraServer.startAutomaticCapture();
+              // Set the resolution
+              camera.setResolution(640, 480);
 
-      // Get a CvSink. This will capture Mats from the camera
-      CvSink cvSink = CameraServer.getInstance().getVideo();
-      // Setup a CvSource. This will send images back to the Dashboard
-      CvSource outputStream
-          = CameraServer.getInstance().putVideo("Rectangle", 640, 480);
+              // Get a CvSink. This will capture Mats from the camera
+              CvSink cvSink = CameraServer.getVideo();
+              // Setup a CvSource. This will send images back to the Dashboard
+              CvSource outputStream = CameraServer.putVideo("Rectangle", 640, 480);
 
-      // Mats are very memory expensive. Lets reuse this Mat.
-      Mat mat = new Mat();
+              // Mats are very memory expensive. Lets reuse this Mat.
+              Mat mat = new Mat();
 
-      // This cannot be 'true'. The program will never exit if it is. This
-      // lets the robot stop this thread when restarting robot code or
-      // deploying.
-      while (!Thread.interrupted()) {
-        // Tell the CvSink to grab a frame from the camera and put it
-        // in the source mat.  If there is an error notify the output.
-        if (cvSink.grabFrame(mat) == 0) {
-          // Send the output the error.
-          outputStream.notifyError(cvSink.getError());
-          // skip the rest of the current iteration
-          continue;
-        }
-        // Put a rectangle on the image
-        Imgproc.rectangle(mat, new Point(100, 100), new Point(400, 400),
-            new Scalar(255, 255, 255), 5);
-        // Give the output stream a new image to display
-        outputStream.putFrame(mat);
-      }
-    });
+              // This cannot be 'true'. The program will never exit if it is. This
+              // lets the robot stop this thread when restarting robot code or
+              // deploying.
+              while (!Thread.interrupted()) {
+                // Tell the CvSink to grab a frame from the camera and put it
+                // in the source mat.  If there is an error notify the output.
+                if (cvSink.grabFrame(mat) == 0) {
+                  // Send the output the error.
+                  outputStream.notifyError(cvSink.getError());
+                  // skip the rest of the current iteration
+                  continue;
+                }
+                // Put a rectangle on the image
+                Imgproc.rectangle(
+                    mat, new Point(100, 100), new Point(400, 400), new Scalar(255, 255, 255), 5);
+                // Give the output stream a new image to display
+                outputStream.putFrame(mat);
+              }
+            });
     m_visionThread.setDaemon(true);
     m_visionThread.start();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
index 2a081ff..c660ae2 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Drivetrain.java
@@ -1,36 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumbot;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
-/**
- * Represents a mecanum drive style drivetrain.
- */
-@SuppressWarnings("PMD.TooManyFields")
+/** Represents a mecanum drive style drivetrain. */
 public class Drivetrain {
   public static final double kMaxSpeed = 3.0; // 3 meters per second
   public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
 
-  private final SpeedController m_frontLeftMotor = new PWMVictorSPX(1);
-  private final SpeedController m_frontRightMotor = new PWMVictorSPX(2);
-  private final SpeedController m_backLeftMotor = new PWMVictorSPX(3);
-  private final SpeedController m_backRightMotor = new PWMVictorSPX(4);
+  private final MotorController m_frontLeftMotor = new PWMSparkMax(1);
+  private final MotorController m_frontRightMotor = new PWMSparkMax(2);
+  private final MotorController m_backLeftMotor = new PWMSparkMax(3);
+  private final MotorController m_backRightMotor = new PWMSparkMax(4);
 
   private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
   private final Encoder m_frontRightEncoder = new Encoder(2, 3);
@@ -49,19 +43,17 @@
 
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
-  private final MecanumDriveKinematics m_kinematics = new MecanumDriveKinematics(
-      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
-  );
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(
+          m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
 
-  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
-      m_gyro.getRotation2d());
+  private final MecanumDriveOdometry m_odometry =
+      new MecanumDriveOdometry(m_kinematics, m_gyro.getRotation2d());
 
   // Gains are for example purposes only - must be determined for your own robot!
   private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
 
-  /**
-   * Constructs a MecanumDrive and resets the gyro.
-   */
+  /** Constructs a MecanumDrive and resets the gyro. */
   public Drivetrain() {
     m_gyro.reset();
   }
@@ -76,8 +68,7 @@
         m_frontLeftEncoder.getRate(),
         m_frontRightEncoder.getRate(),
         m_backLeftEncoder.getRate(),
-        m_backRightEncoder.getRate()
-    );
+        m_backRightEncoder.getRate());
   }
 
   /**
@@ -91,18 +82,18 @@
     final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
     final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
 
-    final double frontLeftOutput = m_frontLeftPIDController.calculate(
-        m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond
-    );
-    final double frontRightOutput = m_frontRightPIDController.calculate(
-        m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond
-    );
-    final double backLeftOutput = m_backLeftPIDController.calculate(
-        m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond
-    );
-    final double backRightOutput = m_backRightPIDController.calculate(
-        m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond
-    );
+    final double frontLeftOutput =
+        m_frontLeftPIDController.calculate(
+            m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond);
+    final double frontRightOutput =
+        m_frontRightPIDController.calculate(
+            m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond);
+    final double backLeftOutput =
+        m_backLeftPIDController.calculate(
+            m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond);
+    final double backRightOutput =
+        m_backRightPIDController.calculate(
+            m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond);
 
     m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
     m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
@@ -113,25 +104,23 @@
   /**
    * Method to drive the robot using joystick info.
    *
-   * @param xSpeed        Speed of the robot in the x direction (forward).
-   * @param ySpeed        Speed of the robot in the y direction (sideways).
-   * @param rot           Angular rate of the robot.
+   * @param xSpeed Speed of the robot in the x direction (forward).
+   * @param ySpeed Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
    * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
   @SuppressWarnings("ParameterName")
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    var mecanumDriveWheelSpeeds = m_kinematics.toWheelSpeeds(
-        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xSpeed, ySpeed, rot, m_gyro.getRotation2d()
-        ) : new ChassisSpeeds(xSpeed, ySpeed, rot)
-    );
+    var mecanumDriveWheelSpeeds =
+        m_kinematics.toWheelSpeeds(
+            fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                : new ChassisSpeeds(xSpeed, ySpeed, rot));
     mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
     setSpeeds(mecanumDriveWheelSpeeds);
   }
 
-  /**
-   * Updates the field relative position of the robot.
-   */
+  /** Updates the field relative position of the robot. */
   public void updateOdometry() {
     m_odometry.update(m_gyro.getRotation2d(), getCurrentState());
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
index 27a4b32..338365e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumbot;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
index d41a947..e1d9a08 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumbot/Robot.java
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumbot;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
@@ -35,24 +31,18 @@
   private void driveWithJoystick(boolean fieldRelative) {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    final var xSpeed =
-        -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
-            * Drivetrain.kMaxSpeed;
+    final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
-    final var ySpeed =
-        -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
-            * Drivetrain.kMaxSpeed;
+    final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    final var rot =
-        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
-            * Drivetrain.kMaxAngularSpeed;
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
 
     m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
index b402109..e959ac2 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Constants.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
 
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -27,10 +24,10 @@
     public static final int kFrontRightMotorPort = 2;
     public static final int kRearRightMotorPort = 3;
 
-    public static final int[] kFrontLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRearLeftEncoderPorts = new int[]{2, 3};
-    public static final int[] kFrontRightEncoderPorts = new int[]{4, 5};
-    public static final int[] kRearRightEncoderPorts = new int[]{5, 6};
+    public static final int[] kFrontLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRearLeftEncoderPorts = new int[] {2, 3};
+    public static final int[] kFrontRightEncoderPorts = new int[] {4, 5};
+    public static final int[] kRearRightEncoderPorts = new int[] {6, 7};
 
     public static final boolean kFrontLeftEncoderReversed = false;
     public static final boolean kRearLeftEncoderReversed = true;
@@ -44,10 +41,10 @@
 
     public static final MecanumDriveKinematics kDriveKinematics =
         new MecanumDriveKinematics(
-          new Translation2d(kWheelBase / 2, kTrackWidth / 2),
-          new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
-          new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
-          new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+            new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+            new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+            new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+            new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     public static final int kEncoderCPR = 1024;
     public static final double kWheelDiameterMeters = 0.15;
@@ -68,12 +65,10 @@
     public static final double kPRearLeftVel = 0.5;
     public static final double kPFrontRightVel = 0.5;
     public static final double kPRearRightVel = 0.5;
-
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
-
+    public static final int kDriverControllerPort = 0;
   }
 
   public static final class AutoConstants {
@@ -86,10 +81,9 @@
     public static final double kPYController = 0.5;
     public static final double kPThetaController = 0.5;
 
-    //Constraint for the motion profilied robot angle controller
+    // Constraint for the motion profilied robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
-        new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
-        kMaxAngularSpeedRadiansPerSecondSquared);
-
+        new TrapezoidProfile.Constraints(
+            kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
index 9670d3d..3398c21 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
index 6f2ccbf..3913f5d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
index 91055c6..0d36a94 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/RobotContainer.java
@@ -1,34 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand;
 
-import java.util.List;
-
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.MecanumControllerCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.List;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -36,7 +30,6 @@
  * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
  * (including subsystems, commands, and button mappings) should be declared here.
  */
-@SuppressWarnings("PMD.ExcessiveImports")
 public class RobotContainer {
   // The robot's subsystems
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
@@ -44,9 +37,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -56,25 +47,27 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive.drive(
-            m_driverController.getY(GenericHID.Hand.kLeft),
-            m_driverController.getX(GenericHID.Hand.kRight),
-            m_driverController.getX(GenericHID.Hand.kLeft), false)));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.drive(
+                    m_driverController.getLeftY(),
+                    m_driverController.getRightX(),
+                    m_driverController.getLeftX(),
+                    false),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
     // Drive at half speed when the right bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
-
   }
 
   /**
@@ -85,53 +78,47 @@
   public Command getAutonomousCommand() {
     // Create config for trajectory
     TrajectoryConfig config =
-        new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
-                             AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+        new TrajectoryConfig(
+                AutoConstants.kMaxSpeedMetersPerSecond,
+                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
             // Add kinematics to ensure max speed is actually obeyed
             .setKinematics(DriveConstants.kDriveKinematics);
 
     // An example trajectory to follow.  All units in meters.
-    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-        // Start at the origin facing the +X direction
-        new Pose2d(0, 0, new Rotation2d(0)),
-        // Pass through these two interior waypoints, making an 's' curve path
-        List.of(
-            new Translation2d(1, 1),
-            new Translation2d(2, -1)
-        ),
-        // End 3 meters straight ahead of where we started, facing forward
-        new Pose2d(3, 0, new Rotation2d(0)),
-        config
-    );
+    Trajectory exampleTrajectory =
+        TrajectoryGenerator.generateTrajectory(
+            // Start at the origin facing the +X direction
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(3, 0, new Rotation2d(0)),
+            config);
 
-    MecanumControllerCommand mecanumControllerCommand = new MecanumControllerCommand(
-        exampleTrajectory,
-        m_robotDrive::getPose,
+    MecanumControllerCommand mecanumControllerCommand =
+        new MecanumControllerCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose,
+            DriveConstants.kFeedforward,
+            DriveConstants.kDriveKinematics,
 
-        DriveConstants.kFeedforward,
-        DriveConstants.kDriveKinematics,
+            // Position contollers
+            new PIDController(AutoConstants.kPXController, 0, 0),
+            new PIDController(AutoConstants.kPYController, 0, 0),
+            new ProfiledPIDController(
+                AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints),
 
-        //Position contollers
-        new PIDController(AutoConstants.kPXController, 0, 0),
-        new PIDController(AutoConstants.kPYController, 0, 0),
-        new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
-                                  AutoConstants.kThetaControllerConstraints),
+            // Needed for normalizing wheel speeds
+            AutoConstants.kMaxSpeedMetersPerSecond,
 
-        //Needed for normalizing wheel speeds
-        AutoConstants.kMaxSpeedMetersPerSecond,
-
-        //Velocity PID's
-        new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
-        new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
-        new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
-        new PIDController(DriveConstants.kPRearRightVel, 0, 0),
-
-        m_robotDrive::getCurrentWheelSpeeds,
-
-        m_robotDrive::setDriveSpeedControllersVolts, //Consumer for the output motor voltages
-
-        m_robotDrive
-    );
+            // Velocity PID's
+            new PIDController(DriveConstants.kPFrontLeftVel, 0, 0),
+            new PIDController(DriveConstants.kPRearLeftVel, 0, 0),
+            new PIDController(DriveConstants.kPFrontRightVel, 0, 0),
+            new PIDController(DriveConstants.kPRearRightVel, 0, 0),
+            m_robotDrive::getCurrentWheelSpeeds,
+            m_robotDrive::setDriveMotorControllersVolts, // Consumer for the output motor voltages
+            m_robotDrive);
 
     // Reset odometry to the starting pose of the trajectory.
     m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
index 1fac67e..23da867 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
@@ -1,60 +1,57 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.subsystems;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.MecanumDriveMotorVoltages;
+import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveMotorVoltages;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
-
 public class DriveSubsystem extends SubsystemBase {
-  private final PWMVictorSPX m_frontLeft = new PWMVictorSPX(DriveConstants.kFrontLeftMotorPort);
-  private final PWMVictorSPX m_rearLeft = new PWMVictorSPX(DriveConstants.kRearLeftMotorPort);
-  private final PWMVictorSPX m_frontRight = new PWMVictorSPX(DriveConstants.kFrontRightMotorPort);
-  private final PWMVictorSPX m_rearRight = new PWMVictorSPX(DriveConstants.kRearRightMotorPort);
+  private final PWMSparkMax m_frontLeft = new PWMSparkMax(DriveConstants.kFrontLeftMotorPort);
+  private final PWMSparkMax m_rearLeft = new PWMSparkMax(DriveConstants.kRearLeftMotorPort);
+  private final PWMSparkMax m_frontRight = new PWMSparkMax(DriveConstants.kFrontRightMotorPort);
+  private final PWMSparkMax m_rearRight = new PWMSparkMax(DriveConstants.kRearRightMotorPort);
 
-  private final MecanumDrive m_drive = new MecanumDrive(
-      m_frontLeft,
-      m_rearLeft,
-      m_frontRight,
-      m_rearRight);
+  private final MecanumDrive m_drive =
+      new MecanumDrive(m_frontLeft, m_rearLeft, m_frontRight, m_rearRight);
 
   // The front-left-side drive encoder
   private final Encoder m_frontLeftEncoder =
-      new Encoder(DriveConstants.kFrontLeftEncoderPorts[0],
-                  DriveConstants.kFrontLeftEncoderPorts[1],
-                  DriveConstants.kFrontLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kFrontLeftEncoderPorts[0],
+          DriveConstants.kFrontLeftEncoderPorts[1],
+          DriveConstants.kFrontLeftEncoderReversed);
 
   // The rear-left-side drive encoder
   private final Encoder m_rearLeftEncoder =
-      new Encoder(DriveConstants.kRearLeftEncoderPorts[0],
-                  DriveConstants.kRearLeftEncoderPorts[1],
-                  DriveConstants.kRearLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kRearLeftEncoderPorts[0],
+          DriveConstants.kRearLeftEncoderPorts[1],
+          DriveConstants.kRearLeftEncoderReversed);
 
   // The front-right--side drive encoder
   private final Encoder m_frontRightEncoder =
-      new Encoder(DriveConstants.kFrontRightEncoderPorts[0],
-                  DriveConstants.kFrontRightEncoderPorts[1],
-                  DriveConstants.kFrontRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kFrontRightEncoderPorts[0],
+          DriveConstants.kFrontRightEncoderPorts[1],
+          DriveConstants.kFrontRightEncoderReversed);
 
   // The rear-right-side drive encoder
   private final Encoder m_rearRightEncoder =
-      new Encoder(DriveConstants.kRearRightEncoderPorts[0],
-                  DriveConstants.kRearRightEncoderPorts[1],
-                  DriveConstants.kRearRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRearRightEncoderPorts[0],
+          DriveConstants.kRearRightEncoderPorts[1],
+          DriveConstants.kRearRightEncoderReversed);
 
   // The gyro sensor
   private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -63,9 +60,7 @@
   MecanumDriveOdometry m_odometry =
       new MecanumDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
     // Sets the distance per pulse for the encoders
     m_frontLeftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -77,12 +72,13 @@
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
-    m_odometry.update(m_gyro.getRotation2d(),
-                      new MecanumDriveWheelSpeeds(
-                          m_frontLeftEncoder.getRate(),
-                          m_rearLeftEncoder.getRate(),
-                          m_frontRightEncoder.getRate(),
-                          m_rearRightEncoder.getRate()));
+    m_odometry.update(
+        m_gyro.getRotation2d(),
+        new MecanumDriveWheelSpeeds(
+            m_frontLeftEncoder.getRate(),
+            m_rearLeftEncoder.getRate(),
+            m_frontRightEncoder.getRate(),
+            m_rearRightEncoder.getRate()));
   }
 
   /**
@@ -107,9 +103,9 @@
    * Drives the robot at given x, y and theta speeds. Speeds range from [-1, 1] and the linear
    * speeds have no effect on the angular speed.
    *
-   * @param xSpeed        Speed of the robot in the x direction (forward/backwards).
-   * @param ySpeed        Speed of the robot in the y direction (sideways).
-   * @param rot           Angular rate of the robot.
+   * @param xSpeed Speed of the robot in the x direction (forward/backwards).
+   * @param ySpeed Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
    * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
   @SuppressWarnings("ParameterName")
@@ -119,23 +115,17 @@
     } else {
       m_drive.driveCartesian(ySpeed, xSpeed, rot);
     }
-
   }
 
-  /**
-   * Sets the front left drive SpeedController to a voltage.
-   */
-  public void setDriveSpeedControllersVolts(MecanumDriveMotorVoltages volts) {
+  /** Sets the front left drive MotorController to a voltage. */
+  public void setDriveMotorControllersVolts(MecanumDriveMotorVoltages volts) {
     m_frontLeft.setVoltage(volts.frontLeftVoltage);
     m_rearLeft.setVoltage(volts.rearLeftVoltage);
     m_frontRight.setVoltage(volts.frontRightVoltage);
     m_rearRight.setVoltage(volts.rearRightVoltage);
   }
 
-
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_frontLeftEncoder.reset();
     m_rearLeftEncoder.reset();
@@ -148,7 +138,6 @@
    *
    * @return the front left drive encoder
    */
-
   public Encoder getFrontLeftEncoder() {
     return m_frontLeftEncoder;
   }
@@ -158,7 +147,6 @@
    *
    * @return the rear left drive encoder
    */
-
   public Encoder getRearLeftEncoder() {
     return m_rearLeftEncoder;
   }
@@ -168,7 +156,6 @@
    *
    * @return the front right drive encoder
    */
-
   public Encoder getFrontRightEncoder() {
     return m_frontRightEncoder;
   }
@@ -178,7 +165,6 @@
    *
    * @return the rear right encoder
    */
-
   public Encoder getRearRightEncoder() {
     return m_rearRightEncoder;
   }
@@ -188,15 +174,14 @@
    *
    * @return the current wheel speeds in a MecanumDriveWheelSpeeds object.
    */
-
   public MecanumDriveWheelSpeeds getCurrentWheelSpeeds() {
-    return new MecanumDriveWheelSpeeds(m_frontLeftEncoder.getRate(),
-                                       m_rearLeftEncoder.getRate(),
-                                       m_frontRightEncoder.getRate(),
-                                       m_rearRightEncoder.getRate());
+    return new MecanumDriveWheelSpeeds(
+        m_frontLeftEncoder.getRate(),
+        m_rearLeftEncoder.getRate(),
+        m_frontRightEncoder.getRate(),
+        m_rearRightEncoder.getRate());
   }
 
-
   /**
    * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
@@ -206,9 +191,7 @@
     m_drive.setMaxOutput(maxOutput);
   }
 
-  /**
-   * Zeroes the heading of the robot.
-   */
+  /** Zeroes the heading of the robot. */
   public void zeroHeading() {
     m_gyro.reset();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java
index 4a8f2e1..b44186c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumdrive;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
old mode 100755
new mode 100644
index 35c975c..80c16b5
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdrive/Robot.java
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.mecanumdrive;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.MecanumDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
-/**
- * This is a demo program showing how to use Mecanum control with the RobotDrive
- * class.
- */
+/** This is a demo program showing how to use Mecanum control with the MecanumDrive class. */
 public class Robot extends TimedRobot {
   private static final int kFrontLeftChannel = 2;
   private static final int kRearLeftChannel = 3;
@@ -29,10 +23,10 @@
 
   @Override
   public void robotInit() {
-    PWMVictorSPX frontLeft = new PWMVictorSPX(kFrontLeftChannel);
-    PWMVictorSPX rearLeft = new PWMVictorSPX(kRearLeftChannel);
-    PWMVictorSPX frontRight = new PWMVictorSPX(kFrontRightChannel);
-    PWMVictorSPX rearRight = new PWMVictorSPX(kRearRightChannel);
+    PWMSparkMax frontLeft = new PWMSparkMax(kFrontLeftChannel);
+    PWMSparkMax rearLeft = new PWMSparkMax(kRearLeftChannel);
+    PWMSparkMax frontRight = new PWMSparkMax(kFrontRightChannel);
+    PWMSparkMax rearRight = new PWMSparkMax(kRearRightChannel);
 
     // Invert the left side motors.
     // You may need to change or remove this to match your robot.
@@ -48,7 +42,6 @@
   public void teleopPeriodic() {
     // Use the joystick X axis for lateral movement, Y axis for forward
     // movement, and Z axis for rotation.
-    m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(),
-        m_stick.getZ(), 0.0);
+    m_robotDrive.driveCartesian(m_stick.getX(), m_stick.getY(), m_stick.getZ(), 0.0);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
new file mode 100644
index 0000000..ba544b2
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Drivetrain.java
@@ -0,0 +1,146 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+/** Represents a mecanum drive style drivetrain. */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final MotorController m_frontLeftMotor = new PWMSparkMax(1);
+  private final MotorController m_frontRightMotor = new PWMSparkMax(2);
+  private final MotorController m_backLeftMotor = new PWMSparkMax(3);
+  private final MotorController m_backRightMotor = new PWMSparkMax(4);
+
+  private final Encoder m_frontLeftEncoder = new Encoder(0, 1);
+  private final Encoder m_frontRightEncoder = new Encoder(2, 3);
+  private final Encoder m_backLeftEncoder = new Encoder(4, 5);
+  private final Encoder m_backRightEncoder = new Encoder(6, 7);
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final PIDController m_frontLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_frontRightPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backLeftPIDController = new PIDController(1, 0, 0);
+  private final PIDController m_backRightPIDController = new PIDController(1, 0, 0);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(
+          m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+
+  /* Here we use MecanumDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+  below are robot specific, and should be tuned. */
+  private final MecanumDrivePoseEstimator m_poseEstimator =
+      new MecanumDrivePoseEstimator(
+          m_gyro.getRotation2d(),
+          new Pose2d(),
+          m_kinematics,
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+          VecBuilder.fill(Units.degreesToRadians(0.01)),
+          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  /** Constructs a MecanumDrive and resets the gyro. */
+  public Drivetrain() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Returns the current state of the drivetrain.
+   *
+   * @return The current state of the drivetrain.
+   */
+  public MecanumDriveWheelSpeeds getCurrentState() {
+    return new MecanumDriveWheelSpeeds(
+        m_frontLeftEncoder.getRate(),
+        m_frontRightEncoder.getRate(),
+        m_backLeftEncoder.getRate(),
+        m_backRightEncoder.getRate());
+  }
+
+  /**
+   * Set the desired speeds for each wheel.
+   *
+   * @param speeds The desired wheel speeds.
+   */
+  public void setSpeeds(MecanumDriveWheelSpeeds speeds) {
+    final double frontLeftFeedforward = m_feedforward.calculate(speeds.frontLeftMetersPerSecond);
+    final double frontRightFeedforward = m_feedforward.calculate(speeds.frontRightMetersPerSecond);
+    final double backLeftFeedforward = m_feedforward.calculate(speeds.rearLeftMetersPerSecond);
+    final double backRightFeedforward = m_feedforward.calculate(speeds.rearRightMetersPerSecond);
+
+    final double frontLeftOutput =
+        m_frontLeftPIDController.calculate(
+            m_frontLeftEncoder.getRate(), speeds.frontLeftMetersPerSecond);
+    final double frontRightOutput =
+        m_frontRightPIDController.calculate(
+            m_frontRightEncoder.getRate(), speeds.frontRightMetersPerSecond);
+    final double backLeftOutput =
+        m_backLeftPIDController.calculate(
+            m_backLeftEncoder.getRate(), speeds.rearLeftMetersPerSecond);
+    final double backRightOutput =
+        m_backRightPIDController.calculate(
+            m_backRightEncoder.getRate(), speeds.rearRightMetersPerSecond);
+
+    m_frontLeftMotor.setVoltage(frontLeftOutput + frontLeftFeedforward);
+    m_frontRightMotor.setVoltage(frontRightOutput + frontRightFeedforward);
+    m_backLeftMotor.setVoltage(backLeftOutput + backLeftFeedforward);
+    m_backRightMotor.setVoltage(backRightOutput + backRightFeedforward);
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed Speed of the robot in the x direction (forward).
+   * @param ySpeed Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    var mecanumDriveWheelSpeeds =
+        m_kinematics.toWheelSpeeds(
+            fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                : new ChassisSpeeds(xSpeed, ySpeed, rot));
+    mecanumDriveWheelSpeeds.normalize(kMaxSpeed);
+    setSpeeds(mecanumDriveWheelSpeeds);
+  }
+
+  /** Updates the field relative position of the robot. */
+  public void updateOdometry() {
+    m_poseEstimator.update(m_gyro.getRotation2d(), getCurrentState());
+
+    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+    // a real robot, this must be calculated based either on latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(
+        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+            m_poseEstimator.getEstimatedPosition()),
+        Timer.getFPGATimestamp() - 0.3);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java
new file mode 100644
index 0000000..8b7a635
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/ExampleGlobalMeasurementSensor.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+  private ExampleGlobalMeasurementSensor() {
+    // Utility class
+  }
+
+  /**
+   * Get a "noisy" fake global pose reading.
+   *
+   * @param estimatedRobotPose The robot pose.
+   */
+  public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+    var rand =
+        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    return new Pose2d(
+        estimatedRobotPose.getX() + rand.get(0, 0),
+        estimatedRobotPose.getY() + rand.get(1, 0),
+        estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java
new file mode 100644
index 0000000..a6c5ccc
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java
new file mode 100644
index 0000000..fb22388
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumdriveposeestimator/Robot.java
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.mecanumdriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_mecanum = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    driveWithJoystick(false);
+    m_mecanum.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    driveWithJoystick(true);
+  }
+
+  private void driveWithJoystick(boolean fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+    m_mecanum.drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java
new file mode 100644
index 0000000..a477a03
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.mechanism2d;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java
new file mode 100644
index 0000000..264b8ad
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mechanism2d/Robot.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.mechanism2d;
+
+import edu.wpi.first.wpilibj.AnalogPotentiometer;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+/**
+ * This sample program shows how to use Mechanism2d - a visual representation of arms, elevators,
+ * and other mechanisms on dashboards; driven by a node-based API.
+ *
+ * <p>Ligaments are based on other ligaments or roots, and roots are contained in the base
+ * Mechanism2d object.
+ */
+public class Robot extends TimedRobot {
+  private static final double kMetersPerPulse = 0.01;
+  private static final double kElevatorMinimumLength = 0.5;
+
+  private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_wristMotor = new PWMSparkMax(1);
+  private final AnalogPotentiometer m_wristPot = new AnalogPotentiometer(1, 90);
+  private final Encoder m_elevatorEncoder = new Encoder(0, 1);
+  private final Joystick m_joystick = new Joystick(0);
+
+  private MechanismLigament2d m_elevator;
+  private MechanismLigament2d m_wrist;
+
+  @Override
+  public void robotInit() {
+    m_elevatorEncoder.setDistancePerPulse(kMetersPerPulse);
+
+    // the main mechanism object
+    Mechanism2d mech = new Mechanism2d(3, 3);
+    // the mechanism root node
+    MechanismRoot2d root = mech.getRoot("climber", 2, 0);
+
+    // MechanismLigament2d objects represent each "section"/"stage" of the mechanism, and are based
+    // off the root node or another ligament object
+    m_elevator = root.append(new MechanismLigament2d("elevator", kElevatorMinimumLength, 90));
+    m_wrist =
+        m_elevator.append(
+            new MechanismLigament2d("wrist", 0.5, 90, 6, new Color8Bit(Color.kPurple)));
+
+    // post the mechanism to the dashboard
+    SmartDashboard.putData("Mech2d", mech);
+  }
+
+  @Override
+  public void robotPeriodic() {
+    // update the dashboard mechanism's state
+    m_elevator.setLength(kElevatorMinimumLength + m_elevatorEncoder.getDistance());
+    m_wrist.setAngle(m_wristPot.get());
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    m_elevatorMotor.set(m_joystick.getRawAxis(0));
+    m_wristMotor.set(m_joystick.getRawAxis(1));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java
index f817e8f..abd3c94 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.motorcontrol;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
old mode 100755
new mode 100644
index 7dfb49b..9976381
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrol/Robot.java
@@ -1,35 +1,31 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.motorcontrol;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This sample program shows how to control a motor using a joystick. In the
- * operator control part of the program, the joystick is read and the value is
- * written to the motor.
+ * This sample program shows how to control a motor using a joystick. In the operator control part
+ * of the program, the joystick is read and the value is written to the motor.
  *
- * <p>Joystick analog values range from -1 to 1 and speed controller inputs also
- * range from -1 to 1 making it easy to work together.
+ * <p>Joystick analog values range from -1 to 1 and speed controller inputs also range from -1 to 1
+ * making it easy to work together.
  */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
   private static final int kJoystickPort = 0;
 
-  private SpeedController m_motor;
+  private MotorController m_motor;
   private Joystick m_joystick;
 
   @Override
   public void robotInit() {
-    m_motor = new PWMVictorSPX(kMotorPort);
+    m_motor = new PWMSparkMax(kMotorPort);
     m_joystick = new Joystick(kJoystickPort);
   }
 
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java
index 5b03eed..0ca48f6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.motorcontrolencoder;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java
index b0cc217..9b68965 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/motorcontrolencoder/Robot.java
@@ -1,29 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.motorcontrolencoder;
 
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
- * This sample program shows how to control a motor using a joystick. In the
- * operator control part of the program, the joystick is read and the value is
- * written to the motor.
+ * This sample program shows how to control a motor using a joystick. In the operator control part
+ * of the program, the joystick is read and the value is written to the motor.
  *
- * <p>Joystick analog values range from -1 to 1 and speed controller inputs also
- * range from -1 to 1 making it easy to work together.
+ * <p>Joystick analog values range from -1 to 1 and speed controller inputs also range from -1 to 1
+ * making it easy to work together.
  *
- * <p>In addition, the encoder value of an encoder connected to ports 0 and 1 is
- * consistently sent to the Dashboard.
+ * <p>In addition, the encoder value of an encoder connected to ports 0 and 1 is consistently sent
+ * to the Dashboard.
  */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
@@ -31,13 +27,13 @@
   private static final int kEncoderPortA = 0;
   private static final int kEncoderPortB = 1;
 
-  private SpeedController m_motor;
+  private MotorController m_motor;
   private Joystick m_joystick;
   private Encoder m_encoder;
 
   @Override
   public void robotInit() {
-    m_motor = new PWMVictorSPX(kMotorPort);
+    m_motor = new PWMSparkMax(kMotorPort);
     m_joystick = new Joystick(kJoystickPort);
     m_encoder = new Encoder(kEncoderPortA, kEncoderPortB);
     // Use SetDistancePerPulse to set the multiplier for GetDistance
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Main.java
deleted file mode 100644
index 04a26be..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Main.java
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
-  private Main() {
-  }
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java
deleted file mode 100644
index c1646e1..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/OI.java
+++ /dev/null
@@ -1,60 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.buttons.JoystickButton;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.Collect;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.LowGoal;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetCollectionSpeed;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.SetPivotSetpoint;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.Shoot;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-import edu.wpi.first.wpilibj.examples.pacgoat.triggers.DoubleButton;
-
-/**
- * The operator interface of the robot, it has been simplified from the real
- * robot to allow control with a single PS3 joystick. As a result, not all
- * functionality from the real robot is available.
- */
-public class OI {
-  public Joystick m_joystick = new Joystick(0);
-
-  /**
-   * Create a new OI and all of the buttons on it.
-   */
-  public OI() {
-    new JoystickButton(m_joystick, 12).whenPressed(new LowGoal());
-    new JoystickButton(m_joystick, 10).whenPressed(new Collect());
-
-    new JoystickButton(m_joystick, 11).whenPressed(
-        new SetPivotSetpoint(Pivot.kShoot));
-    new JoystickButton(m_joystick, 9).whenPressed(
-        new SetPivotSetpoint(Pivot.kShootNear));
-
-    new DoubleButton(m_joystick, 2, 3).whenActive(new Shoot());
-
-    // SmartDashboard Buttons
-    SmartDashboard.putData("Drive Forward", new DriveForward(2.25));
-    SmartDashboard.putData("Drive Backward", new DriveForward(-2.25));
-    SmartDashboard.putData("Start Rollers",
-        new SetCollectionSpeed(Collector.kForward));
-    SmartDashboard.putData("Stop Rollers",
-        new SetCollectionSpeed(Collector.kStop));
-    SmartDashboard.putData("Reverse Rollers",
-        new SetCollectionSpeed(Collector.kReverse));
-  }
-
-  public Joystick getJoystick() {
-    return m_joystick;
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
deleted file mode 100644
index 0f4da55..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/Robot.java
+++ /dev/null
@@ -1,128 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat;
-
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveAndShootAutonomous;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveForward;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.DriveTrain;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pneumatics;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Shooter;
-
-/**
- * This is the main class for running the PacGoat code.
- *
- * <p>The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the manifest file in the resource
- * directory.
- */
-public class Robot extends TimedRobot {
-  Command m_autonomousCommand;
-  public static OI oi;
-
-  // Initialize the subsystems
-  public static DriveTrain drivetrain = new DriveTrain();
-  public static Collector collector = new Collector();
-  public static Shooter shooter = new Shooter();
-  public static Pneumatics pneumatics = new Pneumatics();
-  public static Pivot pivot = new Pivot();
-
-  public SendableChooser<Command> m_autoChooser;
-
-  // This function is run when the robot is first started up and should be
-  // used for any initialization code.
-  @Override
-  public void robotInit() {
-    SmartDashboard.putData(drivetrain);
-    SmartDashboard.putData(collector);
-    SmartDashboard.putData(shooter);
-    SmartDashboard.putData(pneumatics);
-    SmartDashboard.putData(pivot);
-
-    // This MUST be here. If the OI creates Commands (which it very likely
-    // will), constructing it during the construction of CommandBase (from
-    // which commands extend), subsystems are not guaranteed to be
-    // yet. Thus, their requires() statements may grab null pointers. Bad
-    // news. Don't move it.
-    oi = new OI();
-
-    // instantiate the command used for the autonomous period
-    m_autoChooser = new SendableChooser<>();
-    m_autoChooser.setDefaultOption("Drive and Shoot", new DriveAndShootAutonomous());
-    m_autoChooser.addOption("Drive Forward", new DriveForward());
-    SmartDashboard.putData("Auto Mode", m_autoChooser);
-  }
-
-  @Override
-  public void autonomousInit() {
-    m_autonomousCommand = m_autoChooser.getSelected();
-    m_autonomousCommand.start();
-  }
-
-  // This function is called periodically during autonomous
-  @Override
-  public void autonomousPeriodic() {
-    Scheduler.getInstance().run();
-    log();
-  }
-
-  @Override
-  public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
-  }
-
-  // This function is called periodically during operator control
-  @Override
-  public void teleopPeriodic() {
-    Scheduler.getInstance().run();
-    log();
-  }
-
-  // This function called periodically during test mode
-  @Override
-  public void testPeriodic() {
-  }
-
-  @Override
-  public void disabledInit() {
-    Robot.shooter.unlatch();
-  }
-
-  // This function is called periodically while disabled
-  @Override
-  public void disabledPeriodic() {
-    log();
-  }
-
-  /**
-   * Log interesting values to the SmartDashboard.
-   */
-  private void log() {
-    Robot.pneumatics.writePressure();
-    SmartDashboard.putNumber("Pivot Pot Value", Robot.pivot.getAngle());
-    SmartDashboard.putNumber("Left Distance",
-        drivetrain.getLeftEncoder().getDistance());
-    SmartDashboard.putNumber("Right Distance",
-        drivetrain.getRightEncoder().getDistance());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java
deleted file mode 100644
index 4136ed1..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CheckForHotGoal.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command looks for the hot goal and waits until it's detected or timed
- * out. The timeout is because it's better to shoot and get some autonomous
- * points than get none. When called sequentially, this command will block until
- * the hot goal is detected or until it is timed out.
- */
-public class CheckForHotGoal extends Command {
-  public CheckForHotGoal(double time) {
-    setTimeout(time);
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return isTimedOut() || Robot.shooter.goalIsHot();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java
deleted file mode 100644
index e9c154a..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/CloseClaw.java
+++ /dev/null
@@ -1,30 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.InstantCommand;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Close the claw.
- *
- * <p>NOTE: It doesn't wait for the claw to close since there is no sensor to
- * detect that.
- */
-public class CloseClaw extends InstantCommand {
-  public CloseClaw() {
-    requires(Robot.collector);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    Robot.collector.close();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
deleted file mode 100644
index d9db916..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Collect.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-
-/**
- * Get the robot set to collect balls.
- */
-public class Collect extends CommandGroup {
-  /**
-   * Create a new collect command.
-   */
-  public Collect() {
-    addSequential(new SetCollectionSpeed(Collector.kForward));
-    addParallel(new CloseClaw());
-    addSequential(new SetPivotSetpoint(Pivot.kCollect));
-    addSequential(new WaitForBall());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
deleted file mode 100644
index f614e8f..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveAndShootAutonomous.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Drive over the line and then shoot the ball. If the hot goal is not detected,
- * it will wait briefly.
- */
-public class DriveAndShootAutonomous extends CommandGroup {
-  /**
-   * Create a new drive and shoot autonomous.
-   */
-  public DriveAndShootAutonomous() {
-    addSequential(new CloseClaw());
-    addSequential(new WaitForPressure(), 2);
-    if (Robot.isReal()) {
-      // NOTE: Simulation doesn't currently have the concept of hot.
-      addSequential(new CheckForHotGoal(2));
-    }
-    addSequential(new SetPivotSetpoint(45));
-    addSequential(new DriveForward(8, 0.3));
-    addSequential(new Shoot());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
deleted file mode 100644
index 219233b..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveForward.java
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command drives the robot over a given distance with simple proportional
- * control This command will drive a given distance limiting to a maximum speed.
- */
-public class DriveForward extends Command {
-  private final double m_driveForwardSpeed;
-  private final double m_distance;
-  private double m_error;
-  private static final double kTolerance = 0.1;
-  private static final double kP = -1.0 / 5.0;
-
-  public DriveForward() {
-    this(10, 0.5);
-  }
-
-  public DriveForward(double dist) {
-    this(dist, 0.5);
-  }
-
-  /**
-   * Create a new drive forward command.
-   * @param dist The distance to drive
-   * @param maxSpeed The maximum speed to drive at
-   */
-  public DriveForward(double dist, double maxSpeed) {
-    requires(Robot.drivetrain);
-    m_distance = dist;
-    m_driveForwardSpeed = maxSpeed;
-  }
-
-  @Override
-  protected void initialize() {
-    Robot.drivetrain.getRightEncoder().reset();
-    setTimeout(2);
-  }
-
-  @Override
-  protected void execute() {
-    m_error = m_distance - Robot.drivetrain.getRightEncoder().getDistance();
-    if (m_driveForwardSpeed * kP * m_error >= m_driveForwardSpeed) {
-      Robot.drivetrain.tankDrive(m_driveForwardSpeed, m_driveForwardSpeed);
-    } else {
-      Robot.drivetrain.tankDrive(m_driveForwardSpeed * kP * m_error,
-          m_driveForwardSpeed * kP * m_error);
-    }
-  }
-
-  @Override
-  protected boolean isFinished() {
-    return Math.abs(m_error) <= kTolerance || isTimedOut();
-  }
-
-  @Override
-  protected void end() {
-    Robot.drivetrain.stop();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
deleted file mode 100644
index 883ff62..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/DriveWithJoystick.java
+++ /dev/null
@@ -1,37 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command allows PS3 joystick to drive the robot. It is always running
- * except when interrupted by another command.
- */
-public class DriveWithJoystick extends Command {
-  public DriveWithJoystick() {
-    requires(Robot.drivetrain);
-  }
-
-  @Override
-  protected void execute() {
-    Robot.drivetrain.tankDrive(Robot.oi.getJoystick());
-  }
-
-  @Override
-  protected boolean isFinished() {
-    return false;
-  }
-
-  @Override
-  protected void end() {
-    Robot.drivetrain.stop();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
deleted file mode 100644
index cfad93d..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/ExtendShooter.java
+++ /dev/null
@@ -1,34 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.TimedCommand;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Extend the shooter and then retract it after a second.
- */
-public class ExtendShooter extends TimedCommand {
-  public ExtendShooter() {
-    super(1);
-    requires(Robot.shooter);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    Robot.shooter.extendBoth();
-  }
-
-  // Called once after isFinished returns true
-  @Override
-  protected void end() {
-    Robot.shooter.retractBoth();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
deleted file mode 100644
index 08214e1..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/LowGoal.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Pivot;
-
-/**
- * Spit the ball out into the low goal assuming that the robot is in front of
- * it.
- */
-public class LowGoal extends CommandGroup {
-  /**
-   * Create a new low goal command.
-   */
-  public LowGoal() {
-    addSequential(new SetPivotSetpoint(Pivot.kLowGoal));
-    addSequential(new SetCollectionSpeed(Collector.kReverse));
-    addSequential(new ExtendShooter());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
deleted file mode 100644
index f59a6f6..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/OpenClaw.java
+++ /dev/null
@@ -1,33 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Opens the claw.
- */
-public class OpenClaw extends Command {
-  public OpenClaw() {
-    requires(Robot.collector);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    Robot.collector.open();
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return Robot.collector.isOpen();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
deleted file mode 100644
index 172905d..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetCollectionSpeed.java
+++ /dev/null
@@ -1,32 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.InstantCommand;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * This command sets the collector rollers spinning at the given speed. Since
- * there is no sensor for detecting speed, it finishes immediately. As a result,
- * the spinners may still be adjusting their speed.
- */
-public class SetCollectionSpeed extends InstantCommand {
-  private final double m_speed;
-
-  public SetCollectionSpeed(double speed) {
-    requires(Robot.collector);
-    this.m_speed = speed;
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    Robot.collector.setSpeed(m_speed);
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
deleted file mode 100644
index 74a81a0..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/SetPivotSetpoint.java
+++ /dev/null
@@ -1,39 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Moves the pivot to a given angle. This command finishes when it is within the
- * tolerance, but leaves the PID loop running to maintain the position. Other
- * commands using the pivot should make sure they disable PID!
- */
-public class SetPivotSetpoint extends Command {
-  private final double m_setpoint;
-
-  public SetPivotSetpoint(double setpoint) {
-    this.m_setpoint = setpoint;
-    requires(Robot.pivot);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-    Robot.pivot.enable();
-    Robot.pivot.setSetpoint(m_setpoint);
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return Robot.pivot.onTarget();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
deleted file mode 100644
index 40ccb25..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/Shoot.java
+++ /dev/null
@@ -1,27 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.CommandGroup;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.subsystems.Collector;
-
-/**
- * Shoot the ball at the current angle.
- */
-public class Shoot extends CommandGroup {
-  /**
-   * Create a new shoot command.
-   */
-  public Shoot() {
-    addSequential(new WaitForPressure());
-    addSequential(new SetCollectionSpeed(Collector.kStop));
-    addSequential(new OpenClaw());
-    addSequential(new ExtendShooter());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
deleted file mode 100644
index bf1d506..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForBall.java
+++ /dev/null
@@ -1,29 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Wait until the collector senses that it has the ball. This command does
- * nothing and is intended to be used in command groups to wait for this
- * condition.
- */
-public class WaitForBall extends Command {
-  public WaitForBall() {
-    requires(Robot.collector);
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return Robot.collector.hasBall();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
deleted file mode 100644
index 39bac0a..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/commands/WaitForPressure.java
+++ /dev/null
@@ -1,28 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * Wait until the pneumatics are fully pressurized. This command does nothing
- * and is intended to be used in command groups to wait for this condition.
- */
-public class WaitForPressure extends Command {
-  public WaitForPressure() {
-    requires(Robot.pneumatics);
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return Robot.pneumatics.isPressurized();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
deleted file mode 100644
index 853b51a..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Collector.java
+++ /dev/null
@@ -1,103 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Solenoid;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * The Collector subsystem has one motor for the rollers, a limit switch for
- * ball detection, a piston for opening and closing the claw, and a reed switch
- * to check if the piston is open.
- */
-public class Collector extends Subsystem implements AutoCloseable {
-  // Constants for some useful speeds
-  public static final double kForward = 1;
-  public static final double kStop = 0;
-  public static final double kReverse = -1;
-
-  // Subsystem devices
-  private final SpeedController m_rollerMotor = new Victor(6);
-  private final DigitalInput m_ballDetector = new DigitalInput(10);
-  private final DigitalInput m_openDetector = new DigitalInput(6);
-  private final Solenoid m_piston = new Solenoid(1, 1);
-
-  /**
-   * Create a new collector subsystem.
-   */
-  public Collector() {
-    // Put everything to the LiveWindow for testing.
-    addChild("Roller Motor", (Victor) m_rollerMotor);
-    addChild("Ball Detector", m_ballDetector);
-    addChild("Claw Open Detector", m_openDetector);
-    addChild("Piston", m_piston);
-  }
-
-  /**
-   * Whether or not the robot has the ball.
-   *
-   * <p>NOTE: The current simulation model uses the the lower part of the claw
-   * since the limit switch wasn't exported. At some point, this will be
-   * updated.
-   *
-   * @return Whether or not the robot has the ball.
-   */
-  public boolean hasBall() {
-    return m_ballDetector.get(); // TODO: prepend ! to reflect real robot
-  }
-
-  /**
-   * Set the speed to spin the collector rollers.
-   *
-   * @param speed The speed to spin the rollers.
-   */
-  public void setSpeed(double speed) {
-    m_rollerMotor.set(-speed);
-  }
-
-  /**
-   * Stop the rollers from spinning.
-   */
-  public void stop() {
-    m_rollerMotor.set(0);
-  }
-
-  /**
-   * Wether or not the claw is open.
-   *
-   * @return Whether or not the claw is open.
-   */
-  public boolean isOpen() {
-    return m_openDetector.get(); // TODO: prepend ! to reflect real robot
-  }
-
-  /**
-   * Open the claw up (For shooting).
-   */
-  public void open() {
-    m_piston.set(true);
-  }
-
-  /**
-   * Close the claw (For collecting and driving).
-   */
-  @Override
-  public void close() {
-    m_piston.set(false);
-  }
-
-  /**
-   * No default command.
-   */
-  @Override
-  protected void initDefaultCommand() {
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
deleted file mode 100644
index 9b165de..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/DriveTrain.java
+++ /dev/null
@@ -1,138 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.CounterBase.EncodingType;
-import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-import edu.wpi.first.wpilibj.examples.pacgoat.commands.DriveWithJoystick;
-
-/**
- * The DriveTrain subsystem controls the robot's chassis and reads in
- * information about it's speed and position.
- */
-public class DriveTrain extends Subsystem {
-  // Subsystem devices
-  private final SpeedController m_frontLeftCIM = new Victor(1);
-  private final SpeedController m_frontRightCIM = new Victor(2);
-  private final SpeedController m_rearLeftCIM = new Victor(3);
-  private final SpeedController m_rearRightCIM = new Victor(4);
-  private final SpeedControllerGroup m_leftCIMs = new SpeedControllerGroup(
-      m_frontLeftCIM, m_rearLeftCIM);
-  private final SpeedControllerGroup m_rightCIMs = new SpeedControllerGroup(
-      m_frontRightCIM, m_rearRightCIM);
-  private final DifferentialDrive m_drive;
-  private final Encoder m_rightEncoder = new Encoder(1, 2, true, EncodingType.k4X);
-  private final Encoder m_leftEncoder = new Encoder(3, 4, false, EncodingType.k4X);
-  private final AnalogGyro m_gyro = new AnalogGyro(0);
-
-  /**
-   * Create a new drive train subsystem.
-   */
-  public DriveTrain() {
-    // Configure drive motors
-    addChild("Front Left CIM", (Victor) m_frontLeftCIM);
-    addChild("Front Right CIM", (Victor) m_frontRightCIM);
-    addChild("Back Left CIM", (Victor) m_rearLeftCIM);
-    addChild("Back Right CIM", (Victor) m_rearRightCIM);
-
-    // Configure the DifferentialDrive to reflect the fact that all motors
-    // are wired backwards (right is inverted in DifferentialDrive).
-    m_leftCIMs.setInverted(true);
-    m_drive = new DifferentialDrive(m_leftCIMs, m_rightCIMs);
-    m_drive.setSafetyEnabled(true);
-    m_drive.setExpiration(0.1);
-    m_drive.setMaxOutput(1.0);
-
-    if (Robot.isReal()) { // Converts to feet
-      m_rightEncoder.setDistancePerPulse(0.0785398);
-      m_leftEncoder.setDistancePerPulse(0.0785398);
-    } else {
-      // Convert to feet 4in diameter wheels with 360 tick sim encoders
-      m_rightEncoder.setDistancePerPulse(
-          (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
-      m_leftEncoder.setDistancePerPulse(
-          (4.0/* in */ * Math.PI) / (360.0 * 12.0/* in/ft */));
-    }
-
-    addChild("Right Encoder", m_rightEncoder);
-    addChild("Left Encoder", m_leftEncoder);
-
-    // Configure gyro
-    if (Robot.isReal()) {
-      m_gyro.setSensitivity(0.007); // TODO: Handle more gracefully?
-    }
-    addChild("Gyro", m_gyro);
-  }
-
-  /**
-   * When other commands aren't using the drivetrain, allow tank drive with
-   * the joystick.
-   */
-  @Override
-  public void initDefaultCommand() {
-    setDefaultCommand(new DriveWithJoystick());
-  }
-
-  /**
-   * Tank drive using a PS3 joystick.
-   *
-   * @param joy PS3 style joystick to use as the input for tank drive.
-   */
-  public void tankDrive(Joystick joy) {
-    m_drive.tankDrive(joy.getY(), joy.getRawAxis(4));
-  }
-
-  /**
-   * Tank drive using individual joystick axes.
-   *
-   * @param leftAxis Left sides value
-   * @param rightAxis Right sides value
-   */
-  public void tankDrive(double leftAxis, double rightAxis) {
-    m_drive.tankDrive(leftAxis, rightAxis);
-  }
-
-  /**
-   * Stop the drivetrain from moving.
-   */
-  public void stop() {
-    m_drive.tankDrive(0, 0);
-  }
-
-  /**
-   * The encoder getting the distance and speed of left side of the
-   * drivetrain.
-   */
-  public Encoder getLeftEncoder() {
-    return m_leftEncoder;
-  }
-
-  /**
-   * The encoder getting the distance and speed of right side of the
-   * drivetrain.
-   */
-  public Encoder getRightEncoder() {
-    return m_rightEncoder;
-  }
-
-  /**
-   * The current angle of the drivetrain as measured by the Gyro.
-   */
-  public double getAngle() {
-    return m_gyro.getAngle();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
deleted file mode 100644
index 44e197c..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pivot.java
+++ /dev/null
@@ -1,107 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogPotentiometer;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.command.PIDSubsystem;
-import edu.wpi.first.wpilibj.interfaces.Potentiometer;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * The Pivot subsystem contains the Van-door motor and the pot for PID control
- * of angle of the pivot and claw.
- */
-public class Pivot extends PIDSubsystem {
-  // Constants for some useful angles
-  public static final double kCollect = 105;
-  public static final double kLowGoal = 90;
-  public static final double kShoot = 45;
-  public static final double kShootNear = 30;
-
-  // Sensors for measuring the position of the pivot.
-  private final DigitalInput m_upperLimitSwitch = new DigitalInput(13);
-  private final DigitalInput m_lowerLimitSwitch = new DigitalInput(12);
-
-  // 0 degrees is vertical facing up.
-  // Angle increases the more forward the pivot goes.
-  private final Potentiometer m_pot = new AnalogPotentiometer(1);
-
-  // Motor to move the pivot.
-  private final SpeedController m_motor = new Victor(5);
-
-  /**
-   * Create a new pivot subsystem.
-   */
-  public Pivot() {
-    super("Pivot", 7.0, 0.0, 8.0);
-    setAbsoluteTolerance(0.005);
-    getPIDController().setContinuous(false);
-    if (Robot.isSimulation()) { // PID is different in simulation.
-      getPIDController().setPID(0.5, 0.001, 2);
-      setAbsoluteTolerance(5);
-    }
-
-    // Put everything to the LiveWindow for testing.
-    addChild("Upper Limit Switch", m_upperLimitSwitch);
-    addChild("Lower Limit Switch", m_lowerLimitSwitch);
-    addChild("Pot", (AnalogPotentiometer) m_pot);
-    addChild("Motor", (Victor) m_motor);
-    addChild("PIDSubsystem Controller", getPIDController());
-  }
-
-  /**
-   * No default command, if PID is enabled, the current setpoint will be
-   * maintained.
-   */
-  @Override
-  public void initDefaultCommand() {
-  }
-
-  /**
-   * The angle read in by the potentiometer.
-   */
-  @Override
-  protected double returnPIDInput() {
-    return m_pot.get();
-  }
-
-  /**
-   * Set the motor speed based off of the PID output.
-   */
-  @Override
-  protected void usePIDOutput(double output) {
-    m_motor.pidWrite(output);
-  }
-
-  /**
-   * If the pivot is at its upper limit.
-   */
-  public boolean isAtUpperLimit() {
-    // TODO: inverted from real robot (prefix with !)
-    return m_upperLimitSwitch.get();
-  }
-
-  /**
-   * If the pivot is at its lower limit.
-   */
-  public boolean isAtLowerLimit() {
-    // TODO: inverted from real robot (prefix with !)
-    return m_lowerLimitSwitch.get();
-  }
-
-  /**
-   * The current angle of the pivot.
-   */
-  public double getAngle() {
-    return m_pot.get();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
deleted file mode 100644
index 88dc838..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Pneumatics.java
+++ /dev/null
@@ -1,55 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.command.Subsystem;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-import edu.wpi.first.wpilibj.examples.pacgoat.Robot;
-
-/**
- * The Pneumatics subsystem contains a pressure sensor.
- *
- * <p>NOTE: The simulator currently doesn't support the compressor or pressure
- * sensors.
- */
-public class Pneumatics extends Subsystem {
-  AnalogInput m_pressureSensor = new AnalogInput(3);
-
-  private static final double kMaxPressure = 2.55;
-
-  public Pneumatics() {
-    addChild("Pressure Sensor", m_pressureSensor);
-  }
-
-  /**
-   * No default command.
-   */
-  @Override
-  public void initDefaultCommand() {
-  }
-
-  /**
-   * Whether or not the system is fully pressurized.
-   */
-  public boolean isPressurized() {
-    if (Robot.isReal()) {
-      return kMaxPressure <= m_pressureSensor.getVoltage();
-    } else {
-      return true; // NOTE: Simulation always has full pressure
-    }
-  }
-
-  /**
-   * Puts the pressure on the SmartDashboard.
-   */
-  public void writePressure() {
-    SmartDashboard.putNumber("Pressure", m_pressureSensor.getVoltage());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
deleted file mode 100644
index 6fa7e68..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/subsystems/Shooter.java
+++ /dev/null
@@ -1,176 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.subsystems;
-
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.DoubleSolenoid;
-import edu.wpi.first.wpilibj.Solenoid;
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * The Shooter subsystem handles shooting. The mechanism for shooting is
- * slightly complicated because it has to pneumatic cylinders for shooting, and
- * a third latch to allow the pressure to partially build up and reduce the
- * effect of the airflow. For shorter shots, when full power isn't needed, only
- * one cylinder fires.
- *
- * <p>NOTE: Simulation currently approximates this as as single pneumatic
- * cylinder and ignores the latch.
- */
-public class Shooter extends Subsystem {
-  // Devices
-  DoubleSolenoid m_piston1 = new DoubleSolenoid(1, 3, 4);
-  DoubleSolenoid m_piston2 = new DoubleSolenoid(1, 5, 6);
-  Solenoid m_latchPiston = new Solenoid(1, 2);
-  DigitalInput m_piston1ReedSwitchFront = new DigitalInput(9);
-  DigitalInput m_piston1ReedSwitchBack = new DigitalInput(11);
-  //NOTE: currently ignored in simulation
-  DigitalInput m_hotGoalSensor = new DigitalInput(7);
-
-  /**
-   * Create a new shooter subsystem.
-   */
-  public Shooter() {
-    // Put everything to the LiveWindow for testing.
-    addChild("Hot Goal Sensor", m_hotGoalSensor);
-    addChild("Piston1 Reed Switch Front ", m_piston1ReedSwitchFront);
-    addChild("Piston1 Reed Switch Back ", m_piston1ReedSwitchBack);
-    addChild("Latch Piston", m_latchPiston);
-  }
-
-  /**
-   * No default command.
-   */
-  @Override
-  public void initDefaultCommand() {
-  }
-
-  /**
-   * Extend both solenoids to shoot.
-   */
-  public void extendBoth() {
-    m_piston1.set(DoubleSolenoid.Value.kForward);
-    m_piston2.set(DoubleSolenoid.Value.kForward);
-  }
-
-  /**
-   * Retract both solenoids to prepare to shoot.
-   */
-  public void retractBoth() {
-    m_piston1.set(DoubleSolenoid.Value.kReverse);
-    m_piston2.set(DoubleSolenoid.Value.kReverse);
-  }
-
-  /**
-   * Extend solenoid 1 to shoot.
-   */
-  public void extend1() {
-    m_piston1.set(DoubleSolenoid.Value.kForward);
-  }
-
-  /**
-   * Retract solenoid 1 to prepare to shoot.
-   */
-  public void retract1() {
-    m_piston1.set(DoubleSolenoid.Value.kReverse);
-  }
-
-  /**
-   * Extend solenoid 2 to shoot.
-   */
-  public void extend2() {
-    m_piston2.set(DoubleSolenoid.Value.kReverse);
-  }
-
-  /**
-   * Retract solenoid 2 to prepare to shoot.
-   */
-  public void retract2() {
-    m_piston2.set(DoubleSolenoid.Value.kForward);
-  }
-
-  /**
-   * Turns off the piston1 double solenoid. This won't actuate anything
-   * because double solenoids preserve their state when turned off. This
-   * should be called in order to reduce the amount of time that the coils
-   * are powered.
-   */
-  public void off1() {
-    m_piston1.set(DoubleSolenoid.Value.kOff);
-  }
-
-  /**
-   * Turns off the piston2 double solenoid. This won't actuate anything
-   * because double solenoids preserve their state when turned off. This
-   * should be called in order to reduce the amount of time that the coils
-   * are powered.
-   */
-  public void off2() {
-    m_piston2.set(DoubleSolenoid.Value.kOff);
-  }
-
-  /**
-   * Release the latch so that we can shoot.
-   */
-  public void unlatch() {
-    m_latchPiston.set(true);
-  }
-
-  /**
-   * Latch so that pressure can build up and we aren't limited by air flow.
-   */
-  public void latch() {
-    m_latchPiston.set(false);
-  }
-
-  /**
-   * Toggles the latch postions.
-   */
-  public void toggleLatchPosition() {
-    m_latchPiston.set(!m_latchPiston.get());
-  }
-
-  /**
-   * Is Piston 1 extended (after shooting).
-   *
-   * @return Whether or not piston 1 is fully extended.
-   */
-  public boolean piston1IsExtended() {
-    return !m_piston1ReedSwitchFront.get();
-  }
-
-  /**
-   * Is Piston 1 retracted (before shooting).
-   *
-   * @return Whether or not piston 1 is fully retracted.
-   */
-  public boolean piston1IsRetracted() {
-    return !m_piston1ReedSwitchBack.get();
-  }
-
-  /**
-   * Turns off all double solenoids. Double solenoids hold their position when
-   * they are turned off. We should turn them off whenever possible to extend
-   * the life of the coils.
-   */
-  public void offBoth() {
-    m_piston1.set(DoubleSolenoid.Value.kOff);
-    m_piston2.set(DoubleSolenoid.Value.kOff);
-  }
-
-  /**
-   * Return whether the goal is hot as read by the banner sensor.
-   *
-   * <p>NOTE: doesn't work in simulation.
-   *
-   * @return Whether or not the goal is hot
-   */
-  public boolean goalIsHot() {
-    return m_hotGoalSensor.get();
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
deleted file mode 100644
index 4fc8055..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/pacgoat/triggers/DoubleButton.java
+++ /dev/null
@@ -1,38 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.examples.pacgoat.triggers;
-
-import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.buttons.Trigger;
-
-/**
- * A custom button that is triggered when TWO buttons on a Joystick are
- * simultaneously pressed.
- */
-public class DoubleButton extends Trigger {
-  private final Joystick m_joy;
-  private final int m_button1;
-  private final int m_button2;
-
-  /**
-   * Create a new double button trigger.
-   * @param joy The joystick
-   * @param button1 The first button
-   * @param button2 The second button
-   */
-  public DoubleButton(Joystick joy, int button1, int button2) {
-    this.m_joy = joy;
-    this.m_button1 = button1;
-    this.m_button2 = button2;
-  }
-
-  @Override
-  public boolean get() {
-    return m_joy.getRawButton(m_button1) && m_joy.getRawButton(m_button2);
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java
index 72b9454..21bcb5f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.potentiometerpid;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
index cef0916..a1bb38f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/potentiometerpid/Robot.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.potentiometerpid;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate how to use a soft potentiometer and a
- * PID controller to reach and maintain position setpoints on an elevator
- * mechanism.
+ * This is a sample program to demonstrate how to use a soft potentiometer and a PID controller to
+ * reach and maintain position setpoints on an elevator mechanism.
  */
 public class Robot extends TimedRobot {
   private static final int kPotChannel = 1;
@@ -37,7 +33,7 @@
 
   private PIDController m_pidController;
   private AnalogInput m_potentiometer;
-  private SpeedController m_elevatorMotor;
+  private MotorController m_elevatorMotor;
   private Joystick m_joystick;
 
   private int m_index;
@@ -46,17 +42,17 @@
   @Override
   public void robotInit() {
     m_potentiometer = new AnalogInput(kPotChannel);
-    m_elevatorMotor = new PWMVictorSPX(kMotorChannel);
+    m_elevatorMotor = new PWMSparkMax(kMotorChannel);
     m_joystick = new Joystick(kJoystickChannel);
 
     m_pidController = new PIDController(kP, kI, kD);
+    m_pidController.setSetpoint(kSetPoints[m_index]);
   }
 
   @Override
   public void teleopPeriodic() {
     // Run the PID Controller
-    double pidOut
-        = m_pidController.calculate(m_potentiometer.getAverageVoltage());
+    double pidOut = m_pidController.calculate(m_potentiometer.getAverageVoltage());
     m_elevatorMotor.set(pidOut);
 
     // when the button is pressed once, the selected elevator setpoint
@@ -65,9 +61,8 @@
     if (currentButtonValue && !m_previousButtonValue) {
       // index of the elevator setpoint wraps around.
       m_index = (m_index + 1) % kSetPoints.length;
+      m_pidController.setSetpoint(kSetPoints[m_index]);
     }
     m_previousButtonValue = currentButtonValue;
-
-    m_pidController.setSetpoint(kSetPoints[m_index]);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java
index be7edc6..f3925a0 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.quickvision;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
index 9816f13..a025c56 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/quickvision/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.quickvision;
 
@@ -11,14 +8,13 @@
 import edu.wpi.first.wpilibj.TimedRobot;
 
 /**
- * Uses the CameraServer class to automatically capture video from a USB webcam
- * and send it to the FRC dashboard without doing any vision processing. This
- * is the easiest way to get camera images to the dashboard. Just add this to
- * the robotInit() method in your program.
+ * Uses the CameraServer class to automatically capture video from a USB webcam and send it to the
+ * FRC dashboard without doing any vision processing. This is the easiest way to get camera images
+ * to the dashboard. Just add this to the robotInit() method in your program.
  */
 public class Robot extends TimedRobot {
   @Override
   public void robotInit() {
-    CameraServer.getInstance().startAutomaticCapture();
+    CameraServer.startAutomaticCapture();
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
index 49044a2..b63766c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Constants.java
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecommand;
 
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -24,8 +21,8 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
@@ -53,7 +50,7 @@
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 
   public static final class AutoConstants {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
index 4cce085..462edd4 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecommand;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
index 18dc814..49f1975 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecommand;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
index bb8dd28..680af28 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/RobotContainer.java
@@ -1,43 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecommand;
 
-import java.util.List;
+import static edu.wpi.first.wpilibj.XboxController.Button;
 
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.RamseteCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems.DriveSubsystem;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.List;
 
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
@@ -46,9 +40,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -58,82 +50,80 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(m_driverController.getY(GenericHID.Hand.kLeft),
-                         m_driverController.getX(GenericHID.Hand.kRight)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    m_driverController.getLeftY(), m_driverController.getRightX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
     // Drive at half speed when the right bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
-
   }
 
-
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-
     // Create a voltage constraint to ensure we don't accelerate too fast
     var autoVoltageConstraint =
         new DifferentialDriveVoltageConstraint(
-            new SimpleMotorFeedforward(DriveConstants.ksVolts,
-                                       DriveConstants.kvVoltSecondsPerMeter,
-                                       DriveConstants.kaVoltSecondsSquaredPerMeter),
+            new SimpleMotorFeedforward(
+                DriveConstants.ksVolts,
+                DriveConstants.kvVoltSecondsPerMeter,
+                DriveConstants.kaVoltSecondsSquaredPerMeter),
             DriveConstants.kDriveKinematics,
             10);
 
     // Create config for trajectory
     TrajectoryConfig config =
-        new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
-                             AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+        new TrajectoryConfig(
+                AutoConstants.kMaxSpeedMetersPerSecond,
+                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
             // Add kinematics to ensure max speed is actually obeyed
             .setKinematics(DriveConstants.kDriveKinematics)
             // Apply the voltage constraint
             .addConstraint(autoVoltageConstraint);
 
     // An example trajectory to follow.  All units in meters.
-    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-        // Start at the origin facing the +X direction
-        new Pose2d(0, 0, new Rotation2d(0)),
-        // Pass through these two interior waypoints, making an 's' curve path
-        List.of(
-            new Translation2d(1, 1),
-            new Translation2d(2, -1)
-        ),
-        // End 3 meters straight ahead of where we started, facing forward
-        new Pose2d(3, 0, new Rotation2d(0)),
-        // Pass config
-        config
-    );
+    Trajectory exampleTrajectory =
+        TrajectoryGenerator.generateTrajectory(
+            // Start at the origin facing the +X direction
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(3, 0, new Rotation2d(0)),
+            // Pass config
+            config);
 
-    RamseteCommand ramseteCommand = new RamseteCommand(
-        exampleTrajectory,
-        m_robotDrive::getPose,
-        new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
-        new SimpleMotorFeedforward(DriveConstants.ksVolts,
-                                   DriveConstants.kvVoltSecondsPerMeter,
-                                   DriveConstants.kaVoltSecondsSquaredPerMeter),
-        DriveConstants.kDriveKinematics,
-        m_robotDrive::getWheelSpeeds,
-        new PIDController(DriveConstants.kPDriveVel, 0, 0),
-        new PIDController(DriveConstants.kPDriveVel, 0, 0),
-        // RamseteCommand passes volts to the callback
-        m_robotDrive::tankDriveVolts,
-        m_robotDrive
-    );
+    RamseteCommand ramseteCommand =
+        new RamseteCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose,
+            new RamseteController(AutoConstants.kRamseteB, AutoConstants.kRamseteZeta),
+            new SimpleMotorFeedforward(
+                DriveConstants.ksVolts,
+                DriveConstants.kvVoltSecondsPerMeter,
+                DriveConstants.kaVoltSecondsSquaredPerMeter),
+            DriveConstants.kDriveKinematics,
+            m_robotDrive::getWheelSpeeds,
+            new PIDController(DriveConstants.kPDriveVel, 0, 0),
+            new PIDController(DriveConstants.kPDriveVel, 0, 0),
+            // RamseteCommand passes volts to the callback
+            m_robotDrive::tankDriveVolts,
+            m_robotDrive);
 
     // Reset odometry to the starting pose of the trajectory.
     m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
index 396d5df..8ca929d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecommand/subsystems/DriveSubsystem.java
@@ -1,48 +1,50 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecommand.subsystems;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj2.command.SubsystemBase;
-
 import edu.wpi.first.wpilibj.examples.ramsetecommand.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kLeftMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-      new SpeedControllerGroup(new PWMVictorSPX(DriveConstants.kRightMotor1Port),
-                               new PWMVictorSPX(DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-      new Encoder(DriveConstants.kLeftEncoderPorts[0], DriveConstants.kLeftEncoderPorts[1],
-                  DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-      new Encoder(DriveConstants.kRightEncoderPorts[0], DriveConstants.kRightEncoderPorts[1],
-                  DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
   // The gyro sensor
   private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -50,10 +52,13 @@
   // Odometry class for tracking robot pose
   private final DifferentialDriveOdometry m_odometry;
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotors.setInverted(true);
+
     // Sets the distance per pulse for the encoders
     m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
     m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
@@ -65,8 +70,8 @@
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
-    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
-                      m_rightEncoder.getDistance());
+    m_odometry.update(
+        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
   }
 
   /**
@@ -110,7 +115,7 @@
   /**
    * Controls the left and right sides of the drive directly with voltages.
    *
-   * @param leftVolts  the commanded left output
+   * @param leftVolts the commanded left output
    * @param rightVolts the commanded right output
    */
   public void tankDriveVolts(double leftVolts, double rightVolts) {
@@ -119,9 +124,7 @@
     m_drive.feed();
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -155,7 +158,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
@@ -163,9 +166,7 @@
     m_drive.setMaxOutput(maxOutput);
   }
 
-  /**
-   * Zeroes the heading of the robot.
-   */
+  /** Zeroes the heading of the robot. */
   public void zeroHeading() {
     m_gyro.reset();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
index 388b32d..e119080 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Drivetrain.java
@@ -1,28 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecontroller;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
-/**
- * Represents a differential drive style drivetrain.
- */
+/** Represents a differential drive style drivetrain. */
 public class Drivetrain {
   public static final double kMaxSpeed = 3.0; // meters per second
   public static final double kMaxAngularSpeed = 2 * Math.PI; // one rotation per second
@@ -31,26 +26,26 @@
   private static final double kWheelRadius = 0.0508; // meters
   private static final int kEncoderResolution = 4096;
 
-  private final SpeedController m_leftLeader = new PWMVictorSPX(1);
-  private final SpeedController m_leftFollower = new PWMVictorSPX(2);
-  private final SpeedController m_rightLeader = new PWMVictorSPX(3);
-  private final SpeedController m_rightFollower = new PWMVictorSPX(4);
+  private final MotorController m_leftLeader = new PWMSparkMax(1);
+  private final MotorController m_leftFollower = new PWMSparkMax(2);
+  private final MotorController m_rightLeader = new PWMSparkMax(3);
+  private final MotorController m_rightFollower = new PWMSparkMax(4);
 
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
-  private final SpeedControllerGroup m_leftGroup
-      = new SpeedControllerGroup(m_leftLeader, m_leftFollower);
-  private final SpeedControllerGroup m_rightGroup
-      = new SpeedControllerGroup(m_rightLeader, m_rightFollower);
+  private final MotorControllerGroup m_leftGroup =
+      new MotorControllerGroup(m_leftLeader, m_leftFollower);
+  private final MotorControllerGroup m_rightGroup =
+      new MotorControllerGroup(m_rightLeader, m_rightFollower);
 
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
   private final PIDController m_leftPIDController = new PIDController(1, 0, 0);
   private final PIDController m_rightPIDController = new PIDController(1, 0, 0);
 
-  private final DifferentialDriveKinematics m_kinematics
-      = new DifferentialDriveKinematics(kTrackWidth);
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(kTrackWidth);
 
   private final DifferentialDriveOdometry m_odometry;
 
@@ -58,12 +53,17 @@
   private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
 
   /**
-   * Constructs a differential drive object.
-   * Sets the encoder distance per pulse and resets the gyro.
+   * Constructs a differential drive object. Sets the encoder distance per pulse and resets the
+   * gyro.
    */
   public Drivetrain() {
     m_gyro.reset();
 
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.setInverted(true);
+
     // Set the distance per pulse for the drive encoders. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
     // resolution.
@@ -85,10 +85,10 @@
     final double leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
     final double rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
 
-    final double leftOutput = m_leftPIDController.calculate(m_leftEncoder.getRate(),
-        speeds.leftMetersPerSecond);
-    final double rightOutput = m_rightPIDController.calculate(m_rightEncoder.getRate(),
-        speeds.rightMetersPerSecond);
+    final double leftOutput =
+        m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
+    final double rightOutput =
+        m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
     m_leftGroup.setVoltage(leftOutput + leftFeedforward);
     m_rightGroup.setVoltage(rightOutput + rightFeedforward);
   }
@@ -97,7 +97,7 @@
    * Drives the robot with the given linear velocity and angular velocity.
    *
    * @param xSpeed Linear velocity in m/s.
-   * @param rot    Angular velocity in rad/s.
+   * @param rot Angular velocity in rad/s.
    */
   @SuppressWarnings("ParameterName")
   public void drive(double xSpeed, double rot) {
@@ -105,16 +105,15 @@
     setSpeeds(wheelSpeeds);
   }
 
-  /**
-   * Updates the field-relative position.
-   */
+  /** Updates the field-relative position. */
   public void updateOdometry() {
-    m_odometry.update(m_gyro.getRotation2d(), m_leftEncoder.getDistance(),
-        m_rightEncoder.getDistance());
+    m_odometry.update(
+        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
   }
 
   /**
    * Resets the field-relative position to a specific location.
+   *
    * @param pose The position to reset to.
    */
   public void resetOdometry(Pose2d pose) {
@@ -123,6 +122,7 @@
 
   /**
    * Returns the pose of the robot.
+   *
    * @return The pose of the robot.
    */
   public Pose2d getPose() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
index bd07c2e..0b59d82 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecontroller;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
index 6a95a24..ef73d10 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ramsetecontroller/Robot.java
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ramsetecontroller;
 
-import java.util.List;
-
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import java.util.List;
 
 public class Robot extends TimedRobot {
   private final XboxController m_controller = new XboxController(0);
@@ -35,21 +32,31 @@
   private Trajectory m_trajectory;
 
   // The Ramsete Controller to follow the trajectory.
-  private RamseteController m_ramseteController;
+  private final RamseteController m_ramseteController = new RamseteController();
 
   // The timer to use during the autonomous period.
   private Timer m_timer;
 
+  // Create Field2d for robot and trajectory visualizations.
+  private Field2d m_field;
+
   @Override
   public void robotInit() {
     // Create the trajectory to follow in autonomous. It is best to initialize
     // trajectories here to avoid wasting time in autonomous.
-    m_trajectory = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-        List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
-        new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
-        new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0))
-    );
+    m_trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+            new Pose2d(3, 0, Rotation2d.fromDegrees(0)),
+            new TrajectoryConfig(Units.feetToMeters(3.0), Units.feetToMeters(3.0)));
+
+    // Create and push Field2d to SmartDashboard.
+    m_field = new Field2d();
+    SmartDashboard.putData(m_field);
+
+    // Push the trajectory to Field2d.
+    m_field.getObject("traj").setTrajectory(m_trajectory);
   }
 
   @Override
@@ -67,6 +74,9 @@
     // Update odometry.
     m_drive.updateOdometry();
 
+    // Update robot position on Field2d.
+    m_field.setRobotPose(m_drive.getPose());
+
     if (m_timer.get() < m_trajectory.getTotalTimeSeconds()) {
       // Get the desired pose from the trajectory.
       var desiredPose = m_trajectory.sample(m_timer.get());
@@ -85,17 +95,13 @@
   public void teleopPeriodic() {
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
-    final var xSpeed =
-        -m_speedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
-            * Drivetrain.kMaxSpeed;
+    final var xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
-    final var rot =
-        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
-            * Drivetrain.kMaxAngularSpeed;
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
 
     m_drive.drive(xSpeed, rot);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
index 5cd6a00..c005ec7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.relay;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
index 18da837..39e505e 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/relay/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.relay;
 
@@ -18,7 +15,6 @@
  * compressor. This program uses two buttons on a joystick and each button corresponds to one
  * output; pressing the button sets the output to 12V and releasing sets it to 0V.
  */
-
 public class Robot extends TimedRobot {
   private final Joystick m_stick = new Joystick(0);
   private final Relay m_relay = new Relay(0);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java
new file mode 100644
index 0000000..2ebfb92
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Constants.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Main.java
new file mode 100644
index 0000000..aa21a58
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java
new file mode 100644
index 0000000..0cd83a9
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/Robot.java
@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    // Get selected routine from the SmartDashboard
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running which will
+    // use the default command which is ArcadeDrive. If you want the autonomous
+    // to continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java
new file mode 100644
index 0000000..8ad7bc3
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/RobotContainer.java
@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference;
+
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.romireference.commands.ArcadeDrive;
+import edu.wpi.first.wpilibj.examples.romireference.commands.AutonomousDistance;
+import edu.wpi.first.wpilibj.examples.romireference.commands.AutonomousTime;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO;
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.OnBoardIO.ChannelMode;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PrintCommand;
+import edu.wpi.first.wpilibj2.command.button.Button;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems and commands are defined here...
+  private final Drivetrain m_drivetrain = new Drivetrain();
+  private final OnBoardIO m_onboardIO = new OnBoardIO(ChannelMode.INPUT, ChannelMode.INPUT);
+
+  // Assumes a gamepad plugged into channnel 0
+  private final Joystick m_controller = new Joystick(0);
+
+  // Create SmartDashboard chooser for autonomous routines
+  private final SendableChooser<Command> m_chooser = new SendableChooser<>();
+
+  // NOTE: The I/O pin functionality of the 5 exposed I/O pins depends on the hardware "overlay"
+  // that is specified when launching the wpilib-ws server on the Romi raspberry pi.
+  // By default, the following are available (listed in order from inside of the board to outside):
+  // - DIO 8 (mapped to Arduino pin 11, closest to the inside of the board)
+  // - Analog In 0 (mapped to Analog Channel 6 / Arduino Pin 4)
+  // - Analog In 1 (mapped to Analog Channel 2 / Arduino Pin 20)
+  // - PWM 2 (mapped to Arduino Pin 21)
+  // - PWM 3 (mapped to Arduino Pin 22)
+  //
+  // Your subsystem configuration should take the overlays into account
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {
+    // Default command is arcade drive. This will run unless another command
+    // is scheduled over it.
+    m_drivetrain.setDefaultCommand(getArcadeDriveCommand());
+
+    // Example of how to use the onboard IO
+    Button onboardButtonA = new Button(m_onboardIO::getButtonAPressed);
+    onboardButtonA
+        .whenActive(new PrintCommand("Button A Pressed"))
+        .whenInactive(new PrintCommand("Button A Released"));
+
+    // Setup SmartDashboard options
+    m_chooser.setDefaultOption("Auto Routine Distance", new AutonomousDistance(m_drivetrain));
+    m_chooser.addOption("Auto Routine Time", new AutonomousTime(m_drivetrain));
+    SmartDashboard.putData(m_chooser);
+  }
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    return m_chooser.getSelected();
+  }
+
+  /**
+   * Use this to pass the teleop command to the main {@link Robot} class.
+   *
+   * @return the command to run in teleop
+   */
+  public Command getArcadeDriveCommand() {
+    return new ArcadeDrive(
+        m_drivetrain, () -> -m_controller.getRawAxis(1), () -> m_controller.getRawAxis(2));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java
new file mode 100644
index 0000000..4a82b38
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/ArcadeDrive.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import java.util.function.Supplier;
+
+public class ArcadeDrive extends CommandBase {
+  private final Drivetrain m_drivetrain;
+  private final Supplier<Double> m_xaxisSpeedSupplier;
+  private final Supplier<Double> m_zaxisRotateSupplier;
+
+  /**
+   * Creates a new ArcadeDrive. This command will drive your robot according to the speed supplier
+   * lambdas. This command does not terminate.
+   *
+   * @param drivetrain The drivetrain subsystem on which this command will run
+   * @param xaxisSpeedSupplier Lambda supplier of forward/backward speed
+   * @param zaxisRotateSupplier Lambda supplier of rotational speed
+   */
+  public ArcadeDrive(
+      Drivetrain drivetrain,
+      Supplier<Double> xaxisSpeedSupplier,
+      Supplier<Double> zaxisRotateSupplier) {
+    m_drivetrain = drivetrain;
+    m_xaxisSpeedSupplier = xaxisSpeedSupplier;
+    m_zaxisRotateSupplier = zaxisRotateSupplier;
+    addRequirements(drivetrain);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drivetrain.arcadeDrive(m_xaxisSpeedSupplier.get(), m_zaxisRotateSupplier.get());
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java
new file mode 100644
index 0000000..d48b2b4
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousDistance.java
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AutonomousDistance extends SequentialCommandGroup {
+  /**
+   * Creates a new Autonomous Drive based on distance. This will drive out for a specified distance,
+   * turn around and drive back.
+   *
+   * @param drivetrain The drivetrain subsystem on which this command will run
+   */
+  public AutonomousDistance(Drivetrain drivetrain) {
+    addCommands(
+        new DriveDistance(-0.5, 10, drivetrain),
+        new TurnDegrees(-0.5, 180, drivetrain),
+        new DriveDistance(-0.5, 10, drivetrain),
+        new TurnDegrees(0.5, 180, drivetrain));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java
new file mode 100644
index 0000000..4cc9afa
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/AutonomousTime.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+public class AutonomousTime extends SequentialCommandGroup {
+  /**
+   * Creates a new Autonomous Drive based on time. This will drive out for a period of time, turn
+   * around for time (equivalent to time to turn around) and drive forward again. This should mimic
+   * driving out, turning around and driving back.
+   *
+   * @param drivetrain The drive subsystem on which this command will run
+   */
+  public AutonomousTime(Drivetrain drivetrain) {
+    addCommands(
+        new DriveTime(-0.6, 2.0, drivetrain),
+        new TurnTime(-0.5, 1.3, drivetrain),
+        new DriveTime(-0.6, 2.0, drivetrain),
+        new TurnTime(0.5, 1.3, drivetrain));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java
new file mode 100644
index 0000000..f316ce6
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveDistance.java
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveDistance extends CommandBase {
+  private final Drivetrain m_drive;
+  private final double m_distance;
+  private final double m_speed;
+
+  /**
+   * Creates a new DriveDistance. This command will drive your your robot for a desired distance at
+   * a desired speed.
+   *
+   * @param speed The speed at which the robot will drive
+   * @param inches The number of inches the robot will drive
+   * @param drive The drivetrain subsystem on which this command will run
+   */
+  public DriveDistance(double speed, double inches, Drivetrain drive) {
+    m_distance = inches;
+    m_speed = speed;
+    m_drive = drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_drive.arcadeDrive(0, 0);
+    m_drive.resetEncoders();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(m_speed, 0);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    // Compare distance travelled from start to desired distance
+    return Math.abs(m_drive.getAverageDistanceInch()) >= m_distance;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java
new file mode 100644
index 0000000..80636f0
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/DriveTime.java
@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class DriveTime extends CommandBase {
+  private final double m_duration;
+  private final double m_speed;
+  private final Drivetrain m_drive;
+  private long m_startTime;
+
+  /**
+   * Creates a new DriveTime. This command will drive your robot for a desired speed and time.
+   *
+   * @param speed The speed which the robot will drive. Negative is in reverse.
+   * @param time How much time to drive in seconds
+   * @param drive The drivetrain subsystem on which this command will run
+   */
+  public DriveTime(double speed, double time, Drivetrain drive) {
+    m_speed = speed;
+    m_duration = time * 1000;
+    m_drive = drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_startTime = System.currentTimeMillis();
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(m_speed, 0);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (System.currentTimeMillis() - m_startTime) >= m_duration;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java
new file mode 100644
index 0000000..78ffa6c
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnDegrees.java
@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class TurnDegrees extends CommandBase {
+  private final Drivetrain m_drive;
+  private final double m_degrees;
+  private final double m_speed;
+
+  /**
+   * Creates a new TurnDegrees. This command will turn your robot for a desired rotation (in
+   * degrees) and rotational speed.
+   *
+   * @param speed The speed which the robot will drive. Negative is in reverse.
+   * @param degrees Degrees to turn. Leverages encoders to compare distance.
+   * @param drive The drive subsystem on which this command will run
+   */
+  public TurnDegrees(double speed, double degrees, Drivetrain drive) {
+    m_degrees = degrees;
+    m_speed = speed;
+    m_drive = drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    // Set motors to stop, read encoder values for starting point
+    m_drive.arcadeDrive(0, 0);
+    m_drive.resetEncoders();
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(0, m_speed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    /* Need to convert distance travelled to degrees. The Standard
+       Romi Chassis found here, https://www.pololu.com/category/203/romi-chassis-kits,
+       has a wheel placement diameter (149 mm) - width of the wheel (8 mm) = 141 mm
+       or 5.551 inches. We then take into consideration the width of the tires.
+    */
+    double inchPerDegree = Math.PI * 5.551 / 360;
+    // Compare distance travelled from start to distance based on degree turn
+    return getAverageTurningDistance() >= (inchPerDegree * m_degrees);
+  }
+
+  private double getAverageTurningDistance() {
+    double leftDistance = Math.abs(m_drive.getLeftDistanceInch());
+    double rightDistance = Math.abs(m_drive.getRightDistanceInch());
+    return (leftDistance + rightDistance) / 2.0;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java
new file mode 100644
index 0000000..10341f9
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/commands/TurnTime.java
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.commands;
+
+import edu.wpi.first.wpilibj.examples.romireference.subsystems.Drivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/*
+ * Creates a new TurnTime command. This command will turn your robot for a
+ * desired rotational speed and time.
+ */
+public class TurnTime extends CommandBase {
+  private final double m_duration;
+  private final double m_rotationalSpeed;
+  private final Drivetrain m_drive;
+  private long m_startTime;
+
+  /**
+   * Creates a new TurnTime.
+   *
+   * @param speed The speed which the robot will turn. Negative is in reverse.
+   * @param time How much time to turn in seconds
+   * @param drive The drive subsystem on which this command will run
+   */
+  public TurnTime(double speed, double time, Drivetrain drive) {
+    m_rotationalSpeed = speed;
+    m_duration = time * 1000;
+    m_drive = drive;
+    addRequirements(drive);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    m_startTime = System.currentTimeMillis();
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    m_drive.arcadeDrive(0, m_rotationalSpeed);
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    m_drive.arcadeDrive(0, 0);
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return (System.currentTimeMillis() - m_startTime) >= m_duration;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
new file mode 100644
index 0000000..c21f623
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/sensors/RomiGyro.java
@@ -0,0 +1,124 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.sensors;
+
+import edu.wpi.first.hal.SimDevice;
+import edu.wpi.first.hal.SimDevice.Direction;
+import edu.wpi.first.hal.SimDouble;
+
+public class RomiGyro {
+  private SimDouble m_simRateX;
+  private SimDouble m_simRateY;
+  private SimDouble m_simRateZ;
+  private SimDouble m_simAngleX;
+  private SimDouble m_simAngleY;
+  private SimDouble m_simAngleZ;
+
+  private double m_angleXOffset;
+  private double m_angleYOffset;
+  private double m_angleZOffset;
+
+  /** Create a new RomiGyro. */
+  public RomiGyro() {
+    SimDevice gyroSimDevice = SimDevice.create("Gyro:RomiGyro");
+    if (gyroSimDevice != null) {
+      gyroSimDevice.createBoolean("init", Direction.kOutput, true);
+      m_simRateX = gyroSimDevice.createDouble("rate_x", Direction.kInput, 0.0);
+      m_simRateY = gyroSimDevice.createDouble("rate_y", Direction.kInput, 0.0);
+      m_simRateZ = gyroSimDevice.createDouble("rate_z", Direction.kInput, 0.0);
+
+      m_simAngleX = gyroSimDevice.createDouble("angle_x", Direction.kInput, 0.0);
+      m_simAngleY = gyroSimDevice.createDouble("angle_y", Direction.kInput, 0.0);
+      m_simAngleZ = gyroSimDevice.createDouble("angle_z", Direction.kInput, 0.0);
+    }
+  }
+
+  /**
+   * Get the rate of turn in degrees-per-second around the X-axis.
+   *
+   * @return rate of turn in degrees-per-second
+   */
+  public double getRateX() {
+    if (m_simRateX != null) {
+      return m_simRateX.get();
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the rate of turn in degrees-per-second around the Y-axis.
+   *
+   * @return rate of turn in degrees-per-second
+   */
+  public double getRateY() {
+    if (m_simRateY != null) {
+      return m_simRateY.get();
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the rate of turn in degrees-per-second around the Z-axis.
+   *
+   * @return rate of turn in degrees-per-second
+   */
+  public double getRateZ() {
+    if (m_simRateZ != null) {
+      return m_simRateZ.get();
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the currently reported angle around the X-axis.
+   *
+   * @return current angle around X-axis in degrees
+   */
+  public double getAngleX() {
+    if (m_simAngleX != null) {
+      return m_simAngleX.get() - m_angleXOffset;
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the currently reported angle around the X-axis.
+   *
+   * @return current angle around Y-axis in degrees
+   */
+  public double getAngleY() {
+    if (m_simAngleY != null) {
+      return m_simAngleY.get() - m_angleYOffset;
+    }
+
+    return 0.0;
+  }
+
+  /**
+   * Get the currently reported angle around the Z-axis.
+   *
+   * @return current angle around Z-axis in degrees
+   */
+  public double getAngleZ() {
+    if (m_simAngleZ != null) {
+      return m_simAngleZ.get() - m_angleZOffset;
+    }
+
+    return 0.0;
+  }
+
+  /** Reset the gyro angles to 0. */
+  public void reset() {
+    if (m_simAngleX != null) {
+      m_angleXOffset = m_simAngleX.get();
+      m_angleYOffset = m_simAngleY.get();
+      m_angleZOffset = m_simAngleZ.get();
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
new file mode 100644
index 0000000..a7c8432
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/Drivetrain.java
@@ -0,0 +1,142 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.subsystems;
+
+import edu.wpi.first.wpilibj.BuiltInAccelerometer;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.examples.romireference.sensors.RomiGyro;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Drivetrain extends SubsystemBase {
+  private static final double kCountsPerRevolution = 1440.0;
+  private static final double kWheelDiameterInch = 2.75591; // 70 mm
+
+  // The Romi has the left and right motors set to
+  // PWM channels 0 and 1 respectively
+  private final Spark m_leftMotor = new Spark(0);
+  private final Spark m_rightMotor = new Spark(1);
+
+  // The Romi has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  // Set up the RomiGyro
+  private final RomiGyro m_gyro = new RomiGyro();
+
+  // Set up the BuiltInAccelerometer
+  private final BuiltInAccelerometer m_accelerometer = new BuiltInAccelerometer();
+
+  /** Creates a new Drivetrain. */
+  public Drivetrain() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+
+    // Use inches as unit for encoder distances
+    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    resetEncoders();
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public int getLeftEncoderCount() {
+    return m_leftEncoder.get();
+  }
+
+  public int getRightEncoderCount() {
+    return m_rightEncoder.get();
+  }
+
+  public double getLeftDistanceInch() {
+    return m_leftEncoder.getDistance();
+  }
+
+  public double getRightDistanceInch() {
+    return m_rightEncoder.getDistance();
+  }
+
+  public double getAverageDistanceInch() {
+    return (getLeftDistanceInch() + getRightDistanceInch()) / 2.0;
+  }
+
+  /**
+   * The acceleration in the X-axis.
+   *
+   * @return The acceleration of the Romi along the X-axis in Gs
+   */
+  public double getAccelX() {
+    return m_accelerometer.getX();
+  }
+
+  /**
+   * The acceleration in the Y-axis.
+   *
+   * @return The acceleration of the Romi along the Y-axis in Gs
+   */
+  public double getAccelY() {
+    return m_accelerometer.getY();
+  }
+
+  /**
+   * The acceleration in the Z-axis.
+   *
+   * @return The acceleration of the Romi along the Z-axis in Gs
+   */
+  public double getAccelZ() {
+    return m_accelerometer.getZ();
+  }
+
+  /**
+   * Current angle of the Romi around the X-axis.
+   *
+   * @return The current angle of the Romi in degrees
+   */
+  public double getGyroAngleX() {
+    return m_gyro.getAngleX();
+  }
+
+  /**
+   * Current angle of the Romi around the Y-axis.
+   *
+   * @return The current angle of the Romi in degrees
+   */
+  public double getGyroAngleY() {
+    return m_gyro.getAngleY();
+  }
+
+  /**
+   * Current angle of the Romi around the Z-axis.
+   *
+   * @return The current angle of the Romi in degrees
+   */
+  public double getGyroAngleZ() {
+    return m_gyro.getAngleZ();
+  }
+
+  /** Reset the gyro. */
+  public void resetGyro() {
+    m_gyro.reset();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java
new file mode 100644
index 0000000..848aff5
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/romireference/subsystems/OnBoardIO.java
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.romireference.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+/**
+ * This class represents the onboard IO of the Romi reference robot. This includes the pushbuttons
+ * and LEDs.
+ *
+ * <p>DIO 0 - Button A (input only) DIO 1 - Button B (input) or Green LED (output) DIO 2 - Button C
+ * (input) or Red LED (output) DIO 3 - Yellow LED (output only)
+ */
+public class OnBoardIO extends SubsystemBase {
+  private final DigitalInput m_buttonA = new DigitalInput(0);
+  private final DigitalOutput m_yellowLed = new DigitalOutput(3);
+
+  // DIO 1
+  private DigitalInput m_buttonB;
+  private DigitalOutput m_greenLed;
+
+  // DIO 2
+  private DigitalInput m_buttonC;
+  private DigitalOutput m_redLed;
+
+  private static final double MESSAGE_INTERVAL = 1.0;
+  private double m_nextMessageTime;
+
+  public enum ChannelMode {
+    INPUT,
+    OUTPUT
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param dio1 Mode for DIO 1 (input = Button B, output = green LED)
+   * @param dio2 Mode for DIO 2 (input = Button C, output = red LED)
+   */
+  public OnBoardIO(ChannelMode dio1, ChannelMode dio2) {
+    if (dio1 == ChannelMode.INPUT) {
+      m_buttonB = new DigitalInput(1);
+    } else {
+      m_greenLed = new DigitalOutput(1);
+    }
+
+    if (dio2 == ChannelMode.INPUT) {
+      m_buttonC = new DigitalInput(2);
+    } else {
+      m_redLed = new DigitalOutput(2);
+    }
+  }
+
+  /** Gets if the A button is pressed. */
+  public boolean getButtonAPressed() {
+    return m_buttonA.get();
+  }
+
+  /** Gets if the B button is pressed. */
+  public boolean getButtonBPressed() {
+    if (m_buttonB != null) {
+      return m_buttonB.get();
+    }
+
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      DriverStation.reportError("Button B was not configured", true);
+      m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+    }
+    return false;
+  }
+
+  /** Gets if the C button is pressed. */
+  public boolean getButtonCPressed() {
+    if (m_buttonC != null) {
+      return m_buttonC.get();
+    }
+
+    double currentTime = Timer.getFPGATimestamp();
+    if (currentTime > m_nextMessageTime) {
+      DriverStation.reportError("Button C was not configured", true);
+      m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+    }
+    return false;
+  }
+
+  /** Sets the green LED. */
+  public void setGreenLed(boolean value) {
+    if (m_greenLed != null) {
+      m_greenLed.set(value);
+    } else {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        DriverStation.reportError("Green LED was not configured", true);
+        m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+      }
+    }
+  }
+
+  /** Sets the red LED. */
+  public void setRedLed(boolean value) {
+    if (m_redLed != null) {
+      m_redLed.set(value);
+    } else {
+      double currentTime = Timer.getFPGATimestamp();
+      if (currentTime > m_nextMessageTime) {
+        DriverStation.reportError("Red LED was not configured", true);
+        m_nextMessageTime = currentTime + MESSAGE_INTERVAL;
+      }
+    }
+  }
+
+  /** Sets the yellow LED. */
+  public void setYellowLed(boolean value) {
+    m_yellowLed.set(value);
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
index 899d074..38217cd 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Constants.java
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.schedulereventlogging;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
   /**
-   * Example of an inner class.  One can "import static [...].Constants.OIConstants.*" to gain
-   * access to the constants contained within without having to preface the names with the class,
-   * greatly reducing the amount of text required.
+   * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain access
+   * to the constants contained within without having to preface the names with the class, greatly
+   * reducing the amount of text required.
    */
   public static final class OIConstants {
     // Example: the port of the driver's controller
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
index fda3a44..8e7a679 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.schedulereventlogging;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
index bfdf0d7..b61794c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.schedulereventlogging;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,12 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -111,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
index 570a0ad..c798600 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/schedulereventlogging/RobotContainer.java
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.schedulereventlogging;
 
+import static edu.wpi.first.wpilibj.XboxController.Button;
+
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants;
 import edu.wpi.first.wpilibj.shuffleboard.EventImportance;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -18,15 +18,11 @@
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 
-import edu.wpi.first.wpilibj.examples.schedulereventlogging.Constants.OIConstants;
-
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The driver's controller
@@ -38,9 +34,7 @@
   private final CommandBase m_instantCommand2 = new InstantCommand();
   private final CommandBase m_waitCommand = new WaitCommand(5);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Set names of commands
     m_instantCommand1.setName("Instant Command 1");
@@ -48,22 +42,31 @@
     m_waitCommand.setName("Wait 5 Seconds Command");
 
     // Set the scheduler to log Shuffleboard events for command initialize, interrupt, finish
-    CommandScheduler.getInstance().onCommandInitialize(command -> Shuffleboard.addEventMarker(
-        "Command initialized", command.getName(), EventImportance.kNormal));
-    CommandScheduler.getInstance().onCommandInterrupt(command -> Shuffleboard.addEventMarker(
-        "Command interrupted", command.getName(), EventImportance.kNormal));
-    CommandScheduler.getInstance().onCommandFinish(command -> Shuffleboard.addEventMarker(
-        "Command finished", command.getName(), EventImportance.kNormal));
+    CommandScheduler.getInstance()
+        .onCommandInitialize(
+            command ->
+                Shuffleboard.addEventMarker(
+                    "Command initialized", command.getName(), EventImportance.kNormal));
+    CommandScheduler.getInstance()
+        .onCommandInterrupt(
+            command ->
+                Shuffleboard.addEventMarker(
+                    "Command interrupted", command.getName(), EventImportance.kNormal));
+    CommandScheduler.getInstance()
+        .onCommandFinish(
+            command ->
+                Shuffleboard.addEventMarker(
+                    "Command finished", command.getName(), EventImportance.kNormal));
 
     // Configure the button bindings
     configureButtonBindings();
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
+   * Use this method to define your button->command mappings. Buttons can be created by
    * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
   private void configureButtonBindings() {
     // Run instant command 1 when the 'A' button is pressed
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
index 2b73deb..5a76db6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Constants.java
@@ -1,28 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.selectcommand;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
 public final class Constants {
   /**
-   * Example of an inner class.  One can "import static [...].Constants.OIConstants.*" to gain
-   * access to the constants contained within without having to preface the names with the class,
-   * greatly reducing the amount of text required.
+   * Example of an inner class. One can "import static [...].Constants.OIConstants.*" to gain access
+   * to the constants contained within without having to preface the names with the class, greatly
+   * reducing the amount of text required.
    */
   public static final class OIConstants {
     // Example: the port of the driver's controller
-    public static final int kDriverControllerPort = 1;
+    public static final int kDriverControllerPort = 0;
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
index a1104ac..1ad2aff 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.selectcommand;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
index 8d00173..8bd49e8 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.selectcommand;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,12 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -111,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
index eb26912..b6aed81 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/selectcommand/RobotContainer.java
@@ -1,30 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.selectcommand;
 
-import java.util.Map;
-
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.SelectCommand;
+import java.util.Map;
 
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The enum used as keys for selecting the command to run.
   private enum CommandSelector {
-    ONE, TWO, THREE
+    ONE,
+    TWO,
+    THREE
   }
 
   // An example selector method for the selectcommand.  Returns the selector that will select
@@ -43,10 +41,8 @@
           Map.ofEntries(
               Map.entry(CommandSelector.ONE, new PrintCommand("Command one was selected!")),
               Map.entry(CommandSelector.TWO, new PrintCommand("Command two was selected!")),
-              Map.entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))
-          ),
-          this::select
-      );
+              Map.entry(CommandSelector.THREE, new PrintCommand("Command three was selected!"))),
+          this::select);
 
   public RobotContainer() {
     // Configure the button bindings
@@ -54,13 +50,12 @@
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
+   * Use this method to define your button->command mappings. Buttons can be created by
    * instantiating a {@link GenericHID} or one of its subclasses ({@link
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
    * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
-  private void configureButtonBindings() {
-  }
+  private void configureButtonBindings() {}
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java
index d2283df..8a7a1c8 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.shuffleboard;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
index adc713d..eb6a6d9 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/shuffleboard/Robot.java
@@ -1,29 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.shuffleboard;
 
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.wpilibj.AnalogPotentiometer;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 
 public class Robot extends TimedRobot {
-  private final DifferentialDrive m_tankDrive = new DifferentialDrive(new PWMVictorSPX(0),
-                                                                      new PWMVictorSPX(1));
+  private final DifferentialDrive m_tankDrive =
+      new DifferentialDrive(new PWMSparkMax(0), new PWMSparkMax(1));
   private final Encoder m_leftEncoder = new Encoder(0, 1);
   private final Encoder m_rightEncoder = new Encoder(2, 3);
 
-  private final PWMVictorSPX m_elevatorMotor = new PWMVictorSPX(2);
+  private final PWMSparkMax m_elevatorMotor = new PWMSparkMax(2);
   private final AnalogPotentiometer m_elevatorPot = new AnalogPotentiometer(0);
   private NetworkTableEntry m_maxSpeed;
 
@@ -31,20 +28,20 @@
   public void robotInit() {
     // Add a 'max speed' widget to a tab named 'Configuration', using a number slider
     // The widget will be placed in the second column and row and will be TWO columns wide
-    m_maxSpeed = Shuffleboard.getTab("Configuration")
-                           .add("Max Speed", 1)
-                           .withWidget("Number Slider")
-                           .withPosition(1, 1)
-                           .withSize(2, 1)
-                           .getEntry();
+    m_maxSpeed =
+        Shuffleboard.getTab("Configuration")
+            .add("Max Speed", 1)
+            .withWidget("Number Slider")
+            .withPosition(1, 1)
+            .withSize(2, 1)
+            .getEntry();
 
     // Add the tank drive and encoders to a 'Drivebase' tab
     ShuffleboardTab driveBaseTab = Shuffleboard.getTab("Drivebase");
     driveBaseTab.add("Tank Drive", m_tankDrive);
     // Put both encoders in a list layout
-    ShuffleboardLayout encoders = driveBaseTab.getLayout("List Layout", "Encoders")
-                                              .withPosition(0, 0)
-                                              .withSize(2, 2);
+    ShuffleboardLayout encoders =
+        driveBaseTab.getLayout("List Layout", "Encoders").withPosition(0, 0).withSize(2, 2);
     encoders.add("Left Encoder", m_leftEncoder);
     encoders.add("Right Encoder", m_rightEncoder);
 
@@ -59,5 +56,4 @@
     // Read the value of the 'max speed' widget from the dashboard
     m_tankDrive.setMaxOutput(m_maxSpeed.getDouble(1.0));
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
new file mode 100644
index 0000000..ea80fd2
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Drivetrain.java
@@ -0,0 +1,163 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.AnalogGyroSim;
+import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class Drivetrain {
+  // 3 meters per second.
+  public static final double kMaxSpeed = 3.0;
+  // 1/2 rotation per second.
+  public static final double kMaxAngularSpeed = Math.PI;
+
+  private static final double kTrackWidth = 0.381 * 2;
+  private static final double kWheelRadius = 0.0508;
+  private static final int kEncoderResolution = -4096;
+
+  private final PWMSparkMax m_leftLeader = new PWMSparkMax(1);
+  private final PWMSparkMax m_leftFollower = new PWMSparkMax(2);
+  private final PWMSparkMax m_rightLeader = new PWMSparkMax(3);
+  private final PWMSparkMax m_rightFollower = new PWMSparkMax(4);
+
+  private final MotorControllerGroup m_leftGroup =
+      new MotorControllerGroup(m_leftLeader, m_leftFollower);
+  private final MotorControllerGroup m_rightGroup =
+      new MotorControllerGroup(m_rightLeader, m_rightFollower);
+
+  private final Encoder m_leftEncoder = new Encoder(0, 1);
+  private final Encoder m_rightEncoder = new Encoder(2, 3);
+
+  private final PIDController m_leftPIDController = new PIDController(8.5, 0, 0);
+  private final PIDController m_rightPIDController = new PIDController(8.5, 0, 0);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(kTrackWidth);
+  private final DifferentialDriveOdometry m_odometry =
+      new DifferentialDriveOdometry(m_gyro.getRotation2d());
+
+  // Gains are for example purposes only - must be determined for your own
+  // robot!
+  private final SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 3);
+
+  // Simulation classes help us simulate our robot
+  private final AnalogGyroSim m_gyroSim = new AnalogGyroSim(m_gyro);
+  private final EncoderSim m_leftEncoderSim = new EncoderSim(m_leftEncoder);
+  private final EncoderSim m_rightEncoderSim = new EncoderSim(m_rightEncoder);
+  private final Field2d m_fieldSim = new Field2d();
+  private final LinearSystem<N2, N2, N2> m_drivetrainSystem =
+      LinearSystemId.identifyDrivetrainSystem(1.98, 0.2, 1.5, 0.3);
+  private final DifferentialDrivetrainSim m_drivetrainSimulator =
+      new DifferentialDrivetrainSim(
+          m_drivetrainSystem, DCMotor.getCIM(2), 8, kTrackWidth, kWheelRadius, null);
+
+  /** Subsystem constructor. */
+  public Drivetrain() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightGroup.setInverted(true);
+
+    // Set the distance per pulse for the drive encoders. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_leftEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+    m_rightEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+
+    m_rightGroup.setInverted(true);
+    SmartDashboard.putData("Field", m_fieldSim);
+  }
+
+  /** Sets speeds to the drivetrain motors. */
+  public void setSpeeds(DifferentialDriveWheelSpeeds speeds) {
+    var leftFeedforward = m_feedforward.calculate(speeds.leftMetersPerSecond);
+    var rightFeedforward = m_feedforward.calculate(speeds.rightMetersPerSecond);
+    double leftOutput =
+        m_leftPIDController.calculate(m_leftEncoder.getRate(), speeds.leftMetersPerSecond);
+    double rightOutput =
+        m_rightPIDController.calculate(m_rightEncoder.getRate(), speeds.rightMetersPerSecond);
+
+    m_leftGroup.setVoltage(leftOutput + leftFeedforward);
+    m_rightGroup.setVoltage(rightOutput + rightFeedforward);
+  }
+
+  /**
+   * Controls the robot using arcade drive.
+   *
+   * @param xSpeed the speed for the x axis
+   * @param rot the rotation
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double rot) {
+    setSpeeds(m_kinematics.toWheelSpeeds(new ChassisSpeeds(xSpeed, 0, rot)));
+  }
+
+  /** Update robot odometry. */
+  public void updateOdometry() {
+    m_odometry.update(
+        m_gyro.getRotation2d(), m_leftEncoder.getDistance(), m_rightEncoder.getDistance());
+  }
+
+  /** Resets robot odometry. */
+  public void resetOdometry(Pose2d pose) {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+    m_drivetrainSimulator.setPose(pose);
+    m_odometry.resetPosition(pose, m_gyro.getRotation2d());
+  }
+
+  /** Check the current robot pose. */
+  public Pose2d getPose() {
+    return m_odometry.getPoseMeters();
+  }
+
+  /** Update our simulation. This should be run every robot loop in simulation. */
+  public void simulationPeriodic() {
+    // To update our simulation, we set motor voltage inputs, update the
+    // simulation, and write the simulated positions and velocities to our
+    // simulated encoder and gyro. We negate the right side so that positive
+    // voltages make the right side move forward.
+    m_drivetrainSimulator.setInputs(
+        m_leftLeader.get() * RobotController.getInputVoltage(),
+        -m_rightLeader.get() * RobotController.getInputVoltage());
+    m_drivetrainSimulator.update(0.02);
+
+    m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
+    m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
+    m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
+    m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
+    m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
+  }
+
+  /** Update odometry - this should be run every robot loop. */
+  public void periodic() {
+    updateOdometry();
+    m_fieldSim.setRobotPose(m_odometry.getPoseMeters());
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java
new file mode 100644
index 0000000..576b2d3
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
new file mode 100644
index 0000000..99178d4
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/simpledifferentialdrivesimulation/Robot.java
@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.simpledifferentialdrivesimulation;
+
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.XboxController;
+import java.util.List;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0
+  // to 1.
+  private final SlewRateLimiter m_speedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  private final Drivetrain m_drive = new Drivetrain();
+  private final RamseteController m_ramsete = new RamseteController();
+  private final Timer m_timer = new Timer();
+  private Trajectory m_trajectory;
+
+  @Override
+  public void robotInit() {
+    // Flush NetworkTables every loop. This ensures that robot pose and other values
+    // are sent during every iteration.
+    setNetworkTablesFlushEnabled(true);
+
+    m_trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(2, 2, new Rotation2d()),
+            List.of(),
+            new Pose2d(6, 4, new Rotation2d()),
+            new TrajectoryConfig(2, 2));
+  }
+
+  @Override
+  public void robotPeriodic() {
+    m_drive.periodic();
+  }
+
+  @Override
+  public void autonomousInit() {
+    m_timer.reset();
+    m_timer.start();
+    m_drive.resetOdometry(m_trajectory.getInitialPose());
+  }
+
+  @Override
+  public void autonomousPeriodic() {
+    double elapsed = m_timer.get();
+    Trajectory.State reference = m_trajectory.sample(elapsed);
+    ChassisSpeeds speeds = m_ramsete.calculate(m_drive.getPose(), reference);
+    m_drive.drive(speeds.vxMetersPerSecond, speeds.omegaRadiansPerSecond);
+  }
+
+  @Override
+  @SuppressWarnings("LocalVariableName")
+  public void teleopPeriodic() {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    double xSpeed = -m_speedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    double rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+    m_drive.drive(xSpeed, rot);
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    m_drive.simulationPeriodic();
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
index c62cb75..78218ad 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.solenoid;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
index 8fc49bb..98fb3b6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/solenoid/Robot.java
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.solenoid;
 
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.Solenoid;
 import edu.wpi.first.wpilibj.TimedRobot;
 
@@ -23,15 +21,15 @@
  * state. Additionally, double solenoids take up two channels on your PCM whereas single solenoids
  * only take a single channel.
  */
-
 public class Robot extends TimedRobot {
   private final Joystick m_stick = new Joystick(0);
 
   // Solenoid corresponds to a single solenoid.
-  private final Solenoid m_solenoid = new Solenoid(0);
+  private final Solenoid m_solenoid = new Solenoid(PneumaticsModuleType.CTREPCM, 0);
 
   // DoubleSolenoid corresponds to a double solenoid.
-  private final DoubleSolenoid m_doubleSolenoid = new DoubleSolenoid(1, 2);
+  private final DoubleSolenoid m_doubleSolenoid =
+      new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 1, 2);
 
   private static final int kSolenoidButton = 1;
   private static final int kDoubleSolenoidForward = 2;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
index c8a6fb1..204e8e3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacearm;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
index 508102d..9877906 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacearm/Robot.java
@@ -1,33 +1,29 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacearm;
 
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control an arm.
+ * This is a sample program to demonstrate how to use a state-space controller to control an arm.
  */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
@@ -45,8 +41,10 @@
   // the motors, this number should be greater than one.
   private static final double kArmGearing = 10.0;
 
-  private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
-        Units.degreesToRadians(45), Units.degreesToRadians(90)); // Max arm speed and acceleration.
+  private final TrapezoidProfile.Constraints m_constraints =
+      new TrapezoidProfile.Constraints(
+          Units.degreesToRadians(45),
+          Units.degreesToRadians(90)); // Max arm speed and acceleration.
   private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
 
   // The plant holds a state-space model of our arm. This system has the following properties:
@@ -55,47 +53,45 @@
   // Inputs (what we can "put in"): [voltage], in volts.
   // Outputs (what we can measure): [position], in radians.
   private final LinearSystem<N2, N1, N1> m_armPlant =
-      LinearSystemId.createSingleJointedArmSystem(
-        DCMotor.getNEO(2),
-        kArmMOI,
-        kArmGearing);
+      LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(2), kArmMOI, kArmGearing);
 
   // The observer fuses our encoder data and voltage inputs to reject noise.
-  private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
-        Nat.N2(), Nat.N1(),
-        m_armPlant,
-        VecBuilder.fill(0.015, 0.17), // How accurate we
-        // think our model is, in radians and radians/sec
-        VecBuilder.fill(0.01), // How accurate we think our encoder position
-        // data is. In this case we very highly trust our encoder position reading.
-        0.020);
+  private final KalmanFilter<N2, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N2(),
+          Nat.N1(),
+          m_armPlant,
+          VecBuilder.fill(0.015, 0.17), // How accurate we
+          // think our model is, in radians and radians/sec
+          VecBuilder.fill(0.01), // How accurate we think our encoder position
+          // data is. In this case we very highly trust our encoder position reading.
+          0.020);
 
   // A LQR uses feedback to create voltage commands.
-  private final LinearQuadraticRegulator<N2, N1, N1> m_controller
-        = new LinearQuadraticRegulator<>(m_armPlant,
-        VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
-        // Position and velocity error tolerances, in radians and radians per second. Decrease this
-        // to more heavily penalize state excursion, or make the controller behave more
-        // aggressively. In this example we weight position much more highly than velocity, but this
-        // can be tuned to balance the two.
-        VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
-        // heavily penalize control effort, or make the controller less aggressive. 12 is a good
-        // starting point because that is the (approximate) maximum voltage of a battery.
-        0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+  private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          m_armPlant,
+          VecBuilder.fill(Units.degreesToRadians(1.0), Units.degreesToRadians(10.0)), // qelms.
+          // Position and velocity error tolerances, in radians and radians per second. Decrease
+          // this
+          // to more heavily penalize state excursion, or make the controller behave more
+          // aggressively. In this example we weight position much more highly than velocity, but
+          // this
+          // can be tuned to balance the two.
+          VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+          // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+          // starting point because that is the (approximate) maximum voltage of a battery.
+          0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
   // lower if using notifiers.
 
   // The state-space loop combines a controller, observer, feedforward and plant for easy control.
-  private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
-      m_armPlant,
-        m_controller,
-        m_observer,
-        12.0,
-        0.020);
+  private final LinearSystemLoop<N2, N1, N1> m_loop =
+      new LinearSystemLoop<>(m_armPlant, m_controller, m_observer, 12.0, 0.020);
 
   // An encoder set up to measure arm position in radians.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -112,8 +108,8 @@
     m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
 
     // Reset our last reference to the current state.
-    m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
-          m_encoder.getRate());
+    m_lastProfiledReference =
+        new TrapezoidProfile.State(m_encoder.getDistance(), m_encoder.getRate());
   }
 
   @Override
@@ -129,8 +125,8 @@
       goal = new TrapezoidProfile.State(kLoweredPosition, 0.0);
     }
     // Step our TrapezoidalProfile forward 20ms and set it as our next reference
-    m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
-          .calculate(0.020);
+    m_lastProfiledReference =
+        (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
     m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
 
     // Correct our Kalman filter's state vector estimate with encoder data.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
index b86bd3f..2177bd7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Constants.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
 
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -28,20 +25,20 @@
     public static final int kRightMotor1Port = 2;
     public static final int kRightMotor2Port = 3;
 
-    public static final int[] kLeftEncoderPorts = new int[]{0, 1};
-    public static final int[] kRightEncoderPorts = new int[]{2, 3};
+    public static final int[] kLeftEncoderPorts = new int[] {0, 1};
+    public static final int[] kRightEncoderPorts = new int[] {2, 3};
     public static final boolean kLeftEncoderReversed = false;
     public static final boolean kRightEncoderReversed = true;
 
     public static final double kTrackwidthMeters = 0.69;
     public static final DifferentialDriveKinematics kDriveKinematics =
-          new DifferentialDriveKinematics(kTrackwidthMeters);
+        new DifferentialDriveKinematics(kTrackwidthMeters);
 
     public static final int kEncoderCPR = 1024;
     public static final double kWheelDiameterMeters = 0.15;
     public static final double kEncoderDistancePerPulse =
-          // Assumes the encoders are directly mounted on the wheel shafts
-          (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
+        // Assumes the encoders are directly mounted on the wheel shafts
+        (kWheelDiameterMeters * Math.PI) / (double) kEncoderCPR;
 
     public static final boolean kGyroReversed = true;
 
@@ -62,15 +59,18 @@
     public static final double kaVoltSecondsSquaredPerRadian = 0.3;
 
     public static final LinearSystem<N2, N2, N2> kDrivetrainPlant =
-          LinearSystemId.identifyDrivetrainSystem(kvVoltSecondsPerMeter, kaVoltSecondsSquaredPerMeter,
-                kvVoltSecondsPerRadian, kaVoltSecondsSquaredPerRadian);
+        LinearSystemId.identifyDrivetrainSystem(
+            kvVoltSecondsPerMeter,
+            kaVoltSecondsSquaredPerMeter,
+            kvVoltSecondsPerRadian,
+            kaVoltSecondsSquaredPerRadian);
 
     // Example values only -- use what's on your physical robot!
     public static final DCMotor kDriveGearbox = DCMotor.getCIM(2);
     public static final double kDriveGearing = 8;
 
     // Example value only - as above, this must be tuned for your drive!
-    public static final double kPDriveVel = 0.1;
+    public static final double kPDriveVel = 8.5;
   }
 
   public static final class OIConstants {
@@ -79,7 +79,7 @@
 
   public static final class AutoConstants {
     public static final double kMaxSpeedMetersPerSecond = 3;
-    public static final double kMaxAccelerationMetersPerSecondSquared = 6;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 3;
 
     // Reasonable baseline values for a RAMSETE follower in units of meters and seconds
     public static final double kRamseteB = 2;
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
index 5b041fb..bccc25a 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
index 485dfc3..75700c4 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/Robot.java
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
 
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 /**
- * This is a sample program to demonstrate the use of state-space classes in robot simulation.
- * This robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with
- * the sim GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class.
+ * This is a sample program to demonstrate the use of state-space classes in robot simulation. This
+ * robot has a flywheel, elevator, arm and differential drivetrain, and interfaces with the sim
+ * GUI's {@link edu.wpi.first.wpilibj.simulation.Field2d} class.
  */
 public class Robot extends TimedRobot {
   private RobotContainer m_robotContainer;
@@ -29,6 +26,10 @@
     // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
     // autonomous chooser on the dashboard.
     m_robotContainer = new RobotContainer();
+
+    // Flush NetworkTables every loop. This ensures that robot pose and other values
+    // are sent during every loop iteration.
+    setNetworkTablesFlushEnabled(true);
   }
 
   @Override
@@ -55,5 +56,6 @@
   @Override
   public void disabledInit() {
     CommandScheduler.getInstance().cancelAll();
+    m_robotContainer.zeroAllOutputs();
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
index 0790a40..5cbe38b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/RobotContainer.java
@@ -1,49 +1,43 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation;
 
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.RamseteController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.XboxController.Button;
 import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems.DriveSubsystem;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import java.util.List;
 
-import static edu.wpi.first.wpilibj.XboxController.Button;
-
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
- * "declarative" paradigm, very little robot logic should actually be handled in the Robot
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the Robot periodic
+ * methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems
   private final DriveSubsystem m_robotDrive = new DriveSubsystem();
 
   // The driver's controller
-  XboxController m_driverController = new XboxController(Constants.OIConstants.kDriverControllerPort);
+  XboxController m_driverController =
+      new XboxController(Constants.OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -53,41 +47,46 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive
-            .arcadeDrive(-m_driverController.getY(GenericHID.Hand.kRight),
-                m_driverController.getX(GenericHID.Hand.kLeft)), m_robotDrive));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.arcadeDrive(
+                    -m_driverController.getRightY(), m_driverController.getLeftX()),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
    * {@link JoystickButton}.
    */
   private void configureButtonBindings() {
     // Drive at half speed when the right bumper is held
-    new JoystickButton(m_driverController, Button.kBumperRight.value)
+    new JoystickButton(m_driverController, Button.kRightBumper.value)
         .whenPressed(() -> m_robotDrive.setMaxOutput(0.5))
         .whenReleased(() -> m_robotDrive.setMaxOutput(1));
-
   }
 
   public DriveSubsystem getRobotDrive() {
     return m_robotDrive;
   }
 
+  /** Zeros the outputs of all subsystems. */
+  public void zeroAllOutputs() {
+    m_robotDrive.tankDriveVolts(0, 0);
+  }
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-
     // Create a voltage constraint to ensure we don't accelerate too fast
     var autoVoltageConstraint =
         new DifferentialDriveVoltageConstraint(
-            new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts,
+            new SimpleMotorFeedforward(
+                Constants.DriveConstants.ksVolts,
                 Constants.DriveConstants.kvVoltSecondsPerMeter,
                 Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
             Constants.DriveConstants.kDriveKinematics,
@@ -95,40 +94,43 @@
 
     // Create config for trajectory
     TrajectoryConfig config =
-        new TrajectoryConfig(Constants.AutoConstants.kMaxSpeedMetersPerSecond,
-            Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+        new TrajectoryConfig(
+                Constants.AutoConstants.kMaxSpeedMetersPerSecond,
+                Constants.AutoConstants.kMaxAccelerationMetersPerSecondSquared)
             // Add kinematics to ensure max speed is actually obeyed
             .setKinematics(Constants.DriveConstants.kDriveKinematics)
             // Apply the voltage constraint
             .addConstraint(autoVoltageConstraint);
 
     // An example trajectory to follow.  All units in meters.
-    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-        // Start at the origin facing the +X direction
-        new Pose2d(0, 0, new Rotation2d(0)),
-        // Pass through these two interior waypoints, making an 's' curve path
-        List.of(),
-        // End 3 meters straight ahead of where we started, facing forward
-        new Pose2d(6, 6, new Rotation2d(0)),
-        // Pass config
-        config
-    );
+    Trajectory exampleTrajectory =
+        TrajectoryGenerator.generateTrajectory(
+            // Start at (1, 2) facing the +X direction
+            new Pose2d(1, 2, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(new Translation2d(2, 3), new Translation2d(3, 1)),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(4, 2, new Rotation2d(0)),
+            // Pass config
+            config);
 
-    RamseteCommand ramseteCommand = new RamseteCommand(
-        exampleTrajectory,
-        m_robotDrive::getPose,
-        new RamseteController(Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
-        new SimpleMotorFeedforward(Constants.DriveConstants.ksVolts,
-            Constants.DriveConstants.kvVoltSecondsPerMeter,
-            Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
-        Constants.DriveConstants.kDriveKinematics,
-        m_robotDrive::getWheelSpeeds,
-        new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
-        new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
-        // RamseteCommand passes volts to the callback
-        m_robotDrive::tankDriveVolts,
-        m_robotDrive
-    );
+    RamseteCommand ramseteCommand =
+        new RamseteCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose,
+            new RamseteController(
+                Constants.AutoConstants.kRamseteB, Constants.AutoConstants.kRamseteZeta),
+            new SimpleMotorFeedforward(
+                Constants.DriveConstants.ksVolts,
+                Constants.DriveConstants.kvVoltSecondsPerMeter,
+                Constants.DriveConstants.kaVoltSecondsSquaredPerMeter),
+            Constants.DriveConstants.kDriveKinematics,
+            m_robotDrive::getWheelSpeeds,
+            new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
+            new PIDController(Constants.DriveConstants.kPDriveVel, 0, 0),
+            // RamseteCommand passes volts to the callback
+            m_robotDrive::tankDriveVolts,
+            m_robotDrive);
 
     // Reset odometry to starting pose of trajectory.
     m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
index aaa0d8c..98037c3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespacedifferentialdrivesimulation/subsystems/DriveSubsystem.java
@@ -1,61 +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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.subsystems;
 
-import edu.wpi.first.hal.SimDouble;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.RobotController;
-import edu.wpi.first.wpilibj.SPI;
-import edu.wpi.first.wpilibj.SpeedControllerGroup;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
-import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
-import edu.wpi.first.wpilibj.simulation.EncoderSim;
-import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
+import edu.wpi.first.wpilibj.examples.statespacedifferentialdrivesimulation.Constants.DriveConstants;
+import edu.wpi.first.wpilibj.motorcontrol.MotorControllerGroup;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
 import edu.wpi.first.wpilibj.simulation.DifferentialDrivetrainSim;
-import edu.wpi.first.wpilibj.simulation.Field2d;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class DriveSubsystem extends SubsystemBase {
   // The motors on the left side of the drive.
-  private final SpeedControllerGroup m_leftMotors =
-        new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kLeftMotor1Port),
-              new PWMVictorSPX(Constants.DriveConstants.kLeftMotor2Port));
+  private final MotorControllerGroup m_leftMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kLeftMotor1Port),
+          new PWMSparkMax(DriveConstants.kLeftMotor2Port));
 
   // The motors on the right side of the drive.
-  private final SpeedControllerGroup m_rightMotors =
-        new SpeedControllerGroup(new PWMVictorSPX(Constants.DriveConstants.kRightMotor1Port),
-              new PWMVictorSPX(Constants.DriveConstants.kRightMotor2Port));
+  private final MotorControllerGroup m_rightMotors =
+      new MotorControllerGroup(
+          new PWMSparkMax(DriveConstants.kRightMotor1Port),
+          new PWMSparkMax(DriveConstants.kRightMotor2Port));
 
   // The robot's drive
   private final DifferentialDrive m_drive = new DifferentialDrive(m_leftMotors, m_rightMotors);
 
   // The left-side drive encoder
   private final Encoder m_leftEncoder =
-        new Encoder(Constants.DriveConstants.kLeftEncoderPorts[0],
-              Constants.DriveConstants.kLeftEncoderPorts[1],
-              Constants.DriveConstants.kLeftEncoderReversed);
+      new Encoder(
+          DriveConstants.kLeftEncoderPorts[0],
+          DriveConstants.kLeftEncoderPorts[1],
+          DriveConstants.kLeftEncoderReversed);
 
   // The right-side drive encoder
   private final Encoder m_rightEncoder =
-        new Encoder(Constants.DriveConstants.kRightEncoderPorts[0],
-              Constants.DriveConstants.kRightEncoderPorts[1],
-              Constants.DriveConstants.kRightEncoderReversed);
+      new Encoder(
+          DriveConstants.kRightEncoderPorts[0],
+          DriveConstants.kRightEncoderPorts[1],
+          DriveConstants.kRightEncoderReversed);
 
   // The gyro sensor
-  private final Gyro m_gyro = new ADXRS450_Gyro();
+  private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
 
   // Odometry class for tracking robot pose
   private final DifferentialDriveOdometry m_odometry;
@@ -64,48 +64,54 @@
   public DifferentialDrivetrainSim m_drivetrainSimulator;
   private EncoderSim m_leftEncoderSim;
   private EncoderSim m_rightEncoderSim;
-  // The Field2d class simulates the field in the sim GUI. Note that we can have only one
-  // instance!
+  // The Field2d class shows the field in the sim GUI
   private Field2d m_fieldSim;
-  private SimDouble m_gyroAngleSim;
+  private ADXRS450_GyroSim m_gyroSim;
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
+  /** Creates a new DriveSubsystem. */
   public DriveSubsystem() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotors.setInverted(true);
+
     // Sets the distance per pulse for the encoders
-    m_leftEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
-    m_rightEncoder.setDistancePerPulse(Constants.DriveConstants.kEncoderDistancePerPulse);
+    m_leftEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
+    m_rightEncoder.setDistancePerPulse(DriveConstants.kEncoderDistancePerPulse);
 
     resetEncoders();
     m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
 
     if (RobotBase.isSimulation()) { // If our robot is simulated
       // This class simulates our drivetrain's motion around the field.
-      m_drivetrainSimulator = new DifferentialDrivetrainSim(
-            Constants.DriveConstants.kDrivetrainPlant,
-            Constants.DriveConstants.kDriveGearbox,
-            Constants.DriveConstants.kDriveGearing,
-            Constants.DriveConstants.kTrackwidthMeters,
-          Constants.DriveConstants.kWheelDiameterMeters / 2.0);
+      m_drivetrainSimulator =
+          new DifferentialDrivetrainSim(
+              DriveConstants.kDrivetrainPlant,
+              DriveConstants.kDriveGearbox,
+              DriveConstants.kDriveGearing,
+              DriveConstants.kTrackwidthMeters,
+              DriveConstants.kWheelDiameterMeters / 2.0,
+              VecBuilder.fill(0, 0, 0.0001, 0.1, 0.1, 0.005, 0.005));
 
       // The encoder and gyro angle sims let us set simulated sensor readings
       m_leftEncoderSim = new EncoderSim(m_leftEncoder);
       m_rightEncoderSim = new EncoderSim(m_rightEncoder);
-      m_gyroAngleSim =
-            new SimDeviceSim("ADXRS450_Gyro" + "[" + SPI.Port.kOnboardCS0.value + "]")
-                  .getDouble("Angle");
+      m_gyroSim = new ADXRS450_GyroSim(m_gyro);
 
       // the Field2d class lets us visualize our robot in the simulation GUI.
       m_fieldSim = new Field2d();
+      SmartDashboard.putData("Field", m_fieldSim);
     }
   }
 
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
-    m_odometry.update(Rotation2d.fromDegrees(getHeading()), m_leftEncoder.getDistance(),
-          m_rightEncoder.getDistance());
+    m_odometry.update(
+        Rotation2d.fromDegrees(getHeading()),
+        m_leftEncoder.getDistance(),
+        m_rightEncoder.getDistance());
+    m_fieldSim.setRobotPose(getPose());
   }
 
   @Override
@@ -114,23 +120,21 @@
     // and write the simulated positions and velocities to our simulated encoder and gyro.
     // We negate the right side so that positive voltages make the right side
     // move forward.
-    m_drivetrainSimulator.setInputs(m_leftMotors.get() * RobotController.getBatteryVoltage(),
-          -m_rightMotors.get() * RobotController.getBatteryVoltage());
+    m_drivetrainSimulator.setInputs(
+        m_leftMotors.get() * RobotController.getBatteryVoltage(),
+        -m_rightMotors.get() * RobotController.getBatteryVoltage());
     m_drivetrainSimulator.update(0.020);
 
-    m_leftEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftPosition));
-    m_leftEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kLeftVelocity));
-    m_rightEncoderSim.setDistance(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightPosition));
-    m_rightEncoderSim.setRate(m_drivetrainSimulator.getState(DifferentialDrivetrainSim.State.kRightVelocity));
-    m_gyroAngleSim.set(-m_drivetrainSimulator.getHeading().getDegrees());
-
-    m_fieldSim.setRobotPose(getPose());
+    m_leftEncoderSim.setDistance(m_drivetrainSimulator.getLeftPositionMeters());
+    m_leftEncoderSim.setRate(m_drivetrainSimulator.getLeftVelocityMetersPerSecond());
+    m_rightEncoderSim.setDistance(m_drivetrainSimulator.getRightPositionMeters());
+    m_rightEncoderSim.setRate(m_drivetrainSimulator.getRightVelocityMetersPerSecond());
+    m_gyroSim.setAngle(-m_drivetrainSimulator.getHeading().getDegrees());
   }
 
   /**
-   * Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY!
-   * If you want it to work elsewhere, use the code in
-   * {@link DifferentialDrivetrainSim#getCurrentDrawAmps()}
+   * Returns the current being drawn by the drivetrain. This works in SIMULATION ONLY! If you want
+   * it to work elsewhere, use the code in {@link DifferentialDrivetrainSim#getCurrentDrawAmps()}
    *
    * @return The drawn current in Amps.
    */
@@ -180,13 +184,12 @@
   /**
    * Controls the left and right sides of the drive directly with voltages.
    *
-   * @param leftVolts  the commanded left output
+   * @param leftVolts the commanded left output
    * @param rightVolts the commanded right output
    */
   public void tankDriveVolts(double leftVolts, double rightVolts) {
     var batteryVoltage = RobotController.getBatteryVoltage();
-    if (Math.max(Math.abs(leftVolts), Math.abs(rightVolts))
-          > batteryVoltage) {
+    if (Math.max(Math.abs(leftVolts), Math.abs(rightVolts)) > batteryVoltage) {
       leftVolts *= batteryVoltage / 12.0;
       rightVolts *= batteryVoltage / 12.0;
     }
@@ -195,9 +198,7 @@
     m_drive.feed();
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_leftEncoder.reset();
     m_rightEncoder.reset();
@@ -231,7 +232,7 @@
   }
 
   /**
-   * Sets the max output of the drive.  Useful for scaling the drive to drive more slowly.
+   * Sets the max output of the drive. Useful for scaling the drive to drive more slowly.
    *
    * @param maxOutput the maximum output to which the drive will be constrained
    */
@@ -239,9 +240,7 @@
     m_drive.setMaxOutput(maxOutput);
   }
 
-  /**
-   * Zeroes the heading of the robot.
-   */
+  /** Zeroes the heading of the robot. */
   public void zeroHeading() {
     m_gyro.reset();
   }
@@ -252,7 +251,6 @@
    * @return the robot's heading in degrees, from -180 to 180
    */
   public double getHeading() {
-    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (Constants.DriveConstants.kGyroReversed ? -1.0 : 1.0);
+    return Math.IEEEremainder(m_gyro.getAngle(), 360) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
index 03dff05..a4bad42 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespaceelevator;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
index 85cbff4..f1aca19 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceelevator/Robot.java
@@ -1,33 +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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespaceelevator;
 
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control an elevator.
+ * This is a sample program to demonstrate how to use a state-space controller to control an
+ * elevator.
  */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
@@ -46,8 +43,9 @@
   // the motors, this number should be greater than one.
   private static final double kElevatorGearing = 6.0;
 
-  private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(
-        Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
+  private final TrapezoidProfile.Constraints m_constraints =
+      new TrapezoidProfile.Constraints(
+          Units.feetToMeters(3.0), Units.feetToMeters(6.0)); // Max elevator speed and acceleration.
   private TrapezoidProfile.State m_lastProfiledReference = new TrapezoidProfile.State();
 
   /* The plant holds a state-space model of our elevator. This system has the following properties:
@@ -58,49 +56,45 @@
 
   This elevator is driven by two NEO motors.
    */
-  private final LinearSystem<N2, N1, N1> m_elevatorPlant = LinearSystemId.createElevatorSystem(
-        DCMotor.getNEO(2),
-        kCarriageMass,
-        kDrumRadius,
-        kElevatorGearing
-  );
+  private final LinearSystem<N2, N1, N1> m_elevatorPlant =
+      LinearSystemId.createElevatorSystem(
+          DCMotor.getNEO(2), kCarriageMass, kDrumRadius, kElevatorGearing);
 
   // The observer fuses our encoder data and voltage inputs to reject noise.
-  private final KalmanFilter<N2, N1, N1> m_observer = new KalmanFilter<>(
-        Nat.N2(), Nat.N1(),
-        m_elevatorPlant,
-        VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
-        // think our model is, in meters and meters/second.
-        VecBuilder.fill(0.001), // How accurate we think our encoder position
-        // data is. In this case we very highly trust our encoder position reading.
-        0.020);
+  private final KalmanFilter<N2, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N2(),
+          Nat.N1(),
+          m_elevatorPlant,
+          VecBuilder.fill(Units.inchesToMeters(2), Units.inchesToMeters(40)), // How accurate we
+          // think our model is, in meters and meters/second.
+          VecBuilder.fill(0.001), // How accurate we think our encoder position
+          // data is. In this case we very highly trust our encoder position reading.
+          0.020);
 
   // A LQR uses feedback to create voltage commands.
-  private final LinearQuadraticRegulator<N2, N1, N1> m_controller
-        = new LinearQuadraticRegulator<>(m_elevatorPlant,
-        VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
-        // and velocity error tolerances, in meters and meters per second. Decrease this to more
-        // heavily penalize state excursion, or make the controller behave more aggressively. In
-        // this example we weight position much more highly than velocity, but this can be
-        // tuned to balance the two.
-        VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
-        // heavily penalize control effort, or make the controller less aggressive. 12 is a good
-        // starting point because that is the (approximate) maximum voltage of a battery.
-        0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+  private final LinearQuadraticRegulator<N2, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          m_elevatorPlant,
+          VecBuilder.fill(Units.inchesToMeters(1.0), Units.inchesToMeters(10.0)), // qelms. Position
+          // and velocity error tolerances, in meters and meters per second. Decrease this to more
+          // heavily penalize state excursion, or make the controller behave more aggressively. In
+          // this example we weight position much more highly than velocity, but this can be
+          // tuned to balance the two.
+          VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+          // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+          // starting point because that is the (approximate) maximum voltage of a battery.
+          0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
   // lower if using notifiers.
 
   // The state-space loop combines a controller, observer, feedforward and plant for easy control.
-  private final LinearSystemLoop<N2, N1, N1> m_loop = new LinearSystemLoop<>(
-      m_elevatorPlant,
-        m_controller,
-        m_observer,
-        12.0,
-        0.020);
+  private final LinearSystemLoop<N2, N1, N1> m_loop =
+      new LinearSystemLoop<>(m_elevatorPlant, m_controller, m_observer, 12.0, 0.020);
 
   // An encoder set up to measure elevator height in meters.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -117,8 +111,8 @@
     m_loop.reset(VecBuilder.fill(m_encoder.getDistance(), m_encoder.getRate()));
 
     // Reset our last reference to the current state.
-    m_lastProfiledReference = new TrapezoidProfile.State(m_encoder.getDistance(),
-          m_encoder.getRate());
+    m_lastProfiledReference =
+        new TrapezoidProfile.State(m_encoder.getDistance(), m_encoder.getRate());
   }
 
   @Override
@@ -134,8 +128,8 @@
       goal = new TrapezoidProfile.State(kLowGoalPosition, 0.0);
     }
     // Step our TrapezoidalProfile forward 20ms and set it as our next reference
-    m_lastProfiledReference = (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference))
-          .calculate(0.020);
+    m_lastProfiledReference =
+        (new TrapezoidProfile(m_constraints, goal, m_lastProfiledReference)).calculate(0.020);
     m_loop.setNextR(m_lastProfiledReference.position, m_lastProfiledReference.velocity);
 
     // Correct our Kalman filter's state vector estimate with encoder data.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
index 3673388..9e48d52 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespaceflywheel;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
index f9d1131..06e8d29 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheel/Robot.java
@@ -1,31 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespaceflywheel;
 
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control a flywheel.
+ * This is a sample program to demonstrate how to use a state-space controller to control a
+ * flywheel.
  */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
@@ -45,44 +42,42 @@
   // States: [velocity], in radians per second.
   // Inputs (what we can "put in"): [voltage], in volts.
   // Outputs (what we can measure): [velocity], in radians per second.
-  private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.createFlywheelSystem(
-        DCMotor.getNEO(2),
-        kFlywheelMomentOfInertia,
-        kFlywheelGearing);
+  private final LinearSystem<N1, N1, N1> m_flywheelPlant =
+      LinearSystemId.createFlywheelSystem(
+          DCMotor.getNEO(2), kFlywheelMomentOfInertia, kFlywheelGearing);
 
   // The observer fuses our encoder data and voltage inputs to reject noise.
-  private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
-        Nat.N1(), Nat.N1(),
-        m_flywheelPlant,
-        VecBuilder.fill(3.0), // How accurate we think our model is
-        VecBuilder.fill(0.01), // How accurate we think our encoder
-        // data is
-        0.020);
+  private final KalmanFilter<N1, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N1(),
+          Nat.N1(),
+          m_flywheelPlant,
+          VecBuilder.fill(3.0), // How accurate we think our model is
+          VecBuilder.fill(0.01), // How accurate we think our encoder
+          // data is
+          0.020);
 
   // A LQR uses feedback to create voltage commands.
-  private final LinearQuadraticRegulator<N1, N1, N1> m_controller
-        = new LinearQuadraticRegulator<>(m_flywheelPlant,
-        VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
-        // this to more heavily penalize state excursion, or make the controller behave more
-        // aggressively.
-        VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
-        // heavily penalize control effort, or make the controller less aggressive. 12 is a good
-        // starting point because that is the (approximate) maximum voltage of a battery.
-        0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
+  private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          m_flywheelPlant,
+          VecBuilder.fill(8.0), // qelms. Velocity error tolerance, in radians per second. Decrease
+          // this to more heavily penalize state excursion, or make the controller behave more
+          // aggressively.
+          VecBuilder.fill(12.0), // relms. Control effort (voltage) tolerance. Decrease this to more
+          // heavily penalize control effort, or make the controller less aggressive. 12 is a good
+          // starting point because that is the (approximate) maximum voltage of a battery.
+          0.020); // Nominal time between loops. 0.020 for TimedRobot, but can be
   // lower if using notifiers.
 
   // The state-space loop combines a controller, observer, feedforward and plant for easy control.
-  private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
-      m_flywheelPlant,
-        m_controller,
-        m_observer,
-        12.0,
-        0.020);
+  private final LinearSystemLoop<N1, N1, N1> m_loop =
+      new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
 
   // An encoder set up to measure flywheel velocity in radians per second.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -90,8 +85,7 @@
   @Override
   public void robotInit() {
     // We go 2 pi radians per 4096 clicks.
-    m_encoder.setDistancePerPulse(
-          2.0 * Math.PI / 4096.0);
+    m_encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
index e487e9f..67941f6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
index 7cd773b..26ee8f7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/statespaceflywheelsysid/Robot.java
@@ -1,30 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.statespaceflywheelsysid;
 
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.util.Units;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate how to use a state-space controller
- * to control a flywheel.
+ * This is a sample program to demonstrate how to use a state-space controller to control a
+ * flywheel.
  */
 public class Robot extends TimedRobot {
   private static final int kMotorPort = 0;
@@ -46,37 +43,36 @@
   // Outputs (what we can measure): [velocity], in radians per second.
   //
   // The Kv and Ka constants are found using the FRC Characterization toolsuite.
-  private final LinearSystem<N1, N1, N1> m_flywheelPlant = LinearSystemId.identifyVelocitySystem(
-        kFlywheelKv, kFlywheelKa);
+  private final LinearSystem<N1, N1, N1> m_flywheelPlant =
+      LinearSystemId.identifyVelocitySystem(kFlywheelKv, kFlywheelKa);
 
   // The observer fuses our encoder data and voltage inputs to reject noise.
-  private final KalmanFilter<N1, N1, N1> m_observer = new KalmanFilter<>(
-        Nat.N1(), Nat.N1(),
-        m_flywheelPlant,
-        VecBuilder.fill(3.0), // How accurate we think our model is
-        VecBuilder.fill(0.01), // How accurate we think our encoder
-        // data is
-        0.020);
+  private final KalmanFilter<N1, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N1(),
+          Nat.N1(),
+          m_flywheelPlant,
+          VecBuilder.fill(3.0), // How accurate we think our model is
+          VecBuilder.fill(0.01), // How accurate we think our encoder
+          // data is
+          0.020);
 
   // A LQR uses feedback to create voltage commands.
-  private final LinearQuadraticRegulator<N1, N1, N1> m_controller
-        = new LinearQuadraticRegulator<>(m_flywheelPlant,
-        VecBuilder.fill(8.0), // Velocity error tolerance
-        VecBuilder.fill(12.0), // Control effort (voltage) tolerance
-        0.020);
+  private final LinearQuadraticRegulator<N1, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          m_flywheelPlant,
+          VecBuilder.fill(8.0), // Velocity error tolerance
+          VecBuilder.fill(12.0), // Control effort (voltage) tolerance
+          0.020);
 
   // The state-space loop combines a controller, observer, feedforward and plant for easy control.
-  private final LinearSystemLoop<N1, N1, N1> m_loop = new LinearSystemLoop<>(
-      m_flywheelPlant,
-        m_controller,
-        m_observer,
-        12.0,
-        0.020);
+  private final LinearSystemLoop<N1, N1, N1> m_loop =
+      new LinearSystemLoop<>(m_flywheelPlant, m_controller, m_observer, 12.0, 0.020);
 
   // An encoder set up to measure flywheel velocity in radians per second.
   private final Encoder m_encoder = new Encoder(kEncoderAChannel, kEncoderBChannel);
 
-  private final SpeedController m_motor = new PWMVictorSPX(kMotorPort);
+  private final MotorController m_motor = new PWMSparkMax(kMotorPort);
 
   // A joystick to read the trigger from.
   private final Joystick m_joystick = new Joystick(kJoystickPort);
@@ -84,8 +80,7 @@
   @Override
   public void robotInit() {
     // We go 2 pi radians per 4096 clicks.
-    m_encoder.setDistancePerPulse(
-          2.0 * Math.PI / 4096.0);
+    m_encoder.setDistancePerPulse(2.0 * Math.PI / 4096.0);
   }
 
   @Override
@@ -96,7 +91,6 @@
 
   @Override
   public void teleopPeriodic() {
-
     // Sets the target speed of our flywheel. This is similar to setting the setpoint of a
     // PID controller.
     if (m_joystick.getTriggerPressed()) {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
index 5f1227a..3575266 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Drivetrain.java
@@ -1,21 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervebot;
 
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
 
-/**
- * Represents a swerve drive style drivetrain.
- */
+/** Represents a swerve drive style drivetrain. */
 public class Drivetrain {
   public static final double kMaxSpeed = 3.0; // 3 meters per second
   public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
@@ -25,19 +20,19 @@
   private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
   private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
 
-  private final SwerveModule m_frontLeft = new SwerveModule(1, 2);
-  private final SwerveModule m_frontRight = new SwerveModule(3, 4);
-  private final SwerveModule m_backLeft = new SwerveModule(5, 6);
-  private final SwerveModule m_backRight = new SwerveModule(7, 8);
+  private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
+  private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
+  private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
+  private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
 
   private final AnalogGyro m_gyro = new AnalogGyro(0);
 
-  private final SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(
-      m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation
-  );
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(
+          m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
 
-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
-      m_gyro.getRotation2d());
+  private final SwerveDriveOdometry m_odometry =
+      new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
 
   public Drivetrain() {
     m_gyro.reset();
@@ -46,18 +41,18 @@
   /**
    * Method to drive the robot using joystick info.
    *
-   * @param xSpeed        Speed of the robot in the x direction (forward).
-   * @param ySpeed        Speed of the robot in the y direction (sideways).
-   * @param rot           Angular rate of the robot.
+   * @param xSpeed Speed of the robot in the x direction (forward).
+   * @param ySpeed Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
    * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
   @SuppressWarnings("ParameterName")
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    var swerveModuleStates = m_kinematics.toSwerveModuleStates(
-        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xSpeed, ySpeed, rot, m_gyro.getRotation2d())
-            : new ChassisSpeeds(xSpeed, ySpeed, rot)
-    );
+    var swerveModuleStates =
+        m_kinematics.toSwerveModuleStates(
+            fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                : new ChassisSpeeds(xSpeed, ySpeed, rot));
     SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
     m_frontLeft.setDesiredState(swerveModuleStates[0]);
     m_frontRight.setDesiredState(swerveModuleStates[1]);
@@ -65,16 +60,13 @@
     m_backRight.setDesiredState(swerveModuleStates[3]);
   }
 
-  /**
-   * Updates the field relative position of the robot.
-   */
+  /** Updates the field relative position of the robot. */
   public void updateOdometry() {
     m_odometry.update(
         m_gyro.getRotation2d(),
         m_frontLeft.getState(),
         m_frontRight.getState(),
         m_backLeft.getState(),
-        m_backRight.getState()
-    );
+        m_backRight.getState());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
index 52400e6..31707aa 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervebot;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
index b8c4b8f..b2d39ef 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/Robot.java
@@ -1,14 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervebot;
 
-import edu.wpi.first.wpilibj.GenericHID;
-import edu.wpi.first.wpilibj.SlewRateLimiter;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.SlewRateLimiter;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 
@@ -36,23 +33,23 @@
     // Get the x speed. We are inverting this because Xbox controllers return
     // negative values when we push forward.
     final var xSpeed =
-        -m_xspeedLimiter.calculate(m_controller.getY(GenericHID.Hand.kLeft))
-            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+        -m_xspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftY(), 0.02))
+            * Drivetrain.kMaxSpeed;
 
     // Get the y speed or sideways/strafe speed. We are inverting this because
     // we want a positive value when we pull to the left. Xbox controllers
     // return positive values when you pull to the right by default.
     final var ySpeed =
-        -m_yspeedLimiter.calculate(m_controller.getX(GenericHID.Hand.kLeft))
-            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxSpeed;
+        -m_yspeedLimiter.calculate(MathUtil.applyDeadband(m_controller.getLeftX(), 0.02))
+            * Drivetrain.kMaxSpeed;
 
     // Get the rate of angular rotation. We are inverting this because we want a
     // positive value when we pull to the left (remember, CCW is positive in
     // mathematics). Xbox controllers return positive values when you pull to
     // the right by default.
     final var rot =
-        -m_rotLimiter.calculate(m_controller.getX(GenericHID.Hand.kRight))
-            * edu.wpi.first.wpilibj.examples.mecanumbot.Drivetrain.kMaxAngularSpeed;
+        -m_rotLimiter.calculate(MathUtil.applyDeadband(m_controller.getRightX(), 0.02))
+            * Drivetrain.kMaxAngularSpeed;
 
     m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
index e13108f..9749c2d 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervebot/SwerveModule.java
@@ -1,55 +1,71 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervebot;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
-import edu.wpi.first.wpilibj.SpeedController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 public class SwerveModule {
   private static final double kWheelRadius = 0.0508;
   private static final int kEncoderResolution = 4096;
 
   private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
-  private static final double kModuleMaxAngularAcceleration
-      = 2 * Math.PI; // radians per second squared
+  private static final double kModuleMaxAngularAcceleration =
+      2 * Math.PI; // radians per second squared
 
-  private final SpeedController m_driveMotor;
-  private final SpeedController m_turningMotor;
+  private final MotorController m_driveMotor;
+  private final MotorController m_turningMotor;
 
-  private final Encoder m_driveEncoder = new Encoder(0, 1);
-  private final Encoder m_turningEncoder = new Encoder(2, 3);
+  private final Encoder m_driveEncoder;
+  private final Encoder m_turningEncoder;
 
+  // Gains are for example purposes only - must be determined for your own robot!
   private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
 
-  private final ProfiledPIDController m_turningPIDController
-      = new ProfiledPIDController(1, 0, 0,
-      new TrapezoidProfile.Constraints(kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final ProfiledPIDController m_turningPIDController =
+      new ProfiledPIDController(
+          1,
+          0,
+          0,
+          new TrapezoidProfile.Constraints(
+              kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
 
   // Gains are for example purposes only - must be determined for your own robot!
   private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
   private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
 
   /**
-   * Constructs a SwerveModule.
+   * Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
    *
-   * @param driveMotorChannel   ID for the drive motor.
-   * @param turningMotorChannel ID for the turning motor.
+   * @param driveMotorChannel PWM output for the drive motor.
+   * @param turningMotorChannel PWM output for the turning motor.
+   * @param driveEncoderChannelA DIO input for the drive encoder channel A
+   * @param driveEncoderChannelB DIO input for the drive encoder channel B
+   * @param turningEncoderChannelA DIO input for the turning encoder channel A
+   * @param turningEncoderChannelB DIO input for the turning encoder channel B
    */
-  public SwerveModule(int driveMotorChannel, int turningMotorChannel) {
-    m_driveMotor = new PWMVictorSPX(driveMotorChannel);
-    m_turningMotor = new PWMVictorSPX(turningMotorChannel);
+  public SwerveModule(
+      int driveMotorChannel,
+      int turningMotorChannel,
+      int driveEncoderChannelA,
+      int driveEncoderChannelB,
+      int turningEncoderChannelA,
+      int turningEncoderChannelB) {
+    m_driveMotor = new PWMSparkMax(driveMotorChannel);
+    m_turningMotor = new PWMSparkMax(turningMotorChannel);
+
+    m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
+    m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
 
     // Set the distance per pulse for the drive encoder. We can simply use the
     // distance traveled for one rotation of the wheel divided by the encoder
@@ -57,8 +73,8 @@
     m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
 
     // Set the distance (in this case, angle) per pulse for the turning encoder.
-    // This is the the angle through an entire rotation (2 * wpi::math::pi)
-    // divided by the encoder resolution.
+    // This is the the angle through an entire rotation (2 * pi) divided by the
+    // encoder resolution.
     m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
 
     // Limit the PID Controller's input range between -pi and pi and set the input
@@ -78,19 +94,22 @@
   /**
    * Sets the desired state for the module.
    *
-   * @param state Desired state with speed and angle.
+   * @param desiredState Desired state with speed and angle.
    */
-  public void setDesiredState(SwerveModuleState state) {
+  public void setDesiredState(SwerveModuleState desiredState) {
+    // Optimize the reference state to avoid spinning further than 90 degrees
+    SwerveModuleState state =
+        SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
     // Calculate the drive output from the drive PID controller.
-    final double driveOutput = m_drivePIDController.calculate(
-        m_driveEncoder.getRate(), state.speedMetersPerSecond);
+    final double driveOutput =
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
 
     final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
 
     // Calculate the turning motor output from the turning PID controller.
-    final double turnOutput = m_turningPIDController.calculate(
-        m_turningEncoder.get(), state.angle.getRadians()
-    );
+    final double turnOutput =
+        m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
 
     final double turnFeedforward =
         m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
index 706b15e..b4e8e54 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Constants.java
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
 
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
@@ -31,37 +28,36 @@
     public static final int kFrontRightTurningMotorPort = 5;
     public static final int kRearRightTurningMotorPort = 7;
 
-    public static final int[] kFrontLeftTurningEncoderPorts = new int[]{0, 1};
-    public static final int[] kRearLeftTurningEncoderPorts = new int[]{2, 3};
-    public static final int[] kFrontRightTurningEncoderPorts = new int[]{4, 5};
-    public static final int[] kRearRightTurningEncoderPorts = new int[]{5, 6};
+    public static final int[] kFrontLeftTurningEncoderPorts = new int[] {0, 1};
+    public static final int[] kRearLeftTurningEncoderPorts = new int[] {2, 3};
+    public static final int[] kFrontRightTurningEncoderPorts = new int[] {4, 5};
+    public static final int[] kRearRightTurningEncoderPorts = new int[] {6, 7};
 
     public static final boolean kFrontLeftTurningEncoderReversed = false;
     public static final boolean kRearLeftTurningEncoderReversed = true;
     public static final boolean kFrontRightTurningEncoderReversed = false;
     public static final boolean kRearRightTurningEncoderReversed = true;
 
-    public static final int[] kFrontLeftDriveEncoderPorts = new int[]{7, 8};
-    public static final int[] kRearLeftDriveEncoderPorts = new int[]{9, 10};
-    public static final int[] kFrontRightDriveEncoderPorts = new int[]{11, 12};
-    public static final int[] kRearRightDriveEncoderPorts = new int[]{13, 14};
+    public static final int[] kFrontLeftDriveEncoderPorts = new int[] {8, 9};
+    public static final int[] kRearLeftDriveEncoderPorts = new int[] {10, 11};
+    public static final int[] kFrontRightDriveEncoderPorts = new int[] {12, 13};
+    public static final int[] kRearRightDriveEncoderPorts = new int[] {14, 15};
 
     public static final boolean kFrontLeftDriveEncoderReversed = false;
     public static final boolean kRearLeftDriveEncoderReversed = true;
     public static final boolean kFrontRightDriveEncoderReversed = false;
     public static final boolean kRearRightDriveEncoderReversed = true;
 
-
     public static final double kTrackWidth = 0.5;
-    //Distance between centers of right and left wheels on robot
+    // Distance between centers of right and left wheels on robot
     public static final double kWheelBase = 0.7;
-    //Distance between front and back wheels on robot
+    // Distance between front and back wheels on robot
     public static final SwerveDriveKinematics kDriveKinematics =
         new SwerveDriveKinematics(
-          new Translation2d(kWheelBase / 2, kTrackWidth / 2),
-          new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
-          new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
-          new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
+            new Translation2d(kWheelBase / 2, kTrackWidth / 2),
+            new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
+            new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
+            new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
 
     public static final boolean kGyroReversed = false;
 
@@ -94,12 +90,10 @@
     public static final double kPModuleTurningController = 1;
 
     public static final double kPModuleDriveController = 1;
-
   }
 
   public static final class OIConstants {
-    public static final int kDriverControllerPort = 1;
-
+    public static final int kDriverControllerPort = 0;
   }
 
   public static final class AutoConstants {
@@ -112,10 +106,9 @@
     public static final double kPYController = 1;
     public static final double kPThetaController = 1;
 
-    //Constraint for the motion profilied robot angle controller
+    // Constraint for the motion profilied robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints =
-        new TrapezoidProfile.Constraints(kMaxAngularSpeedRadiansPerSecond,
-          kMaxAngularSpeedRadiansPerSecondSquared);
-
+        new TrapezoidProfile.Constraints(
+            kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
index 5c8a1da..af62a09 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
index 3d41ba4..800b70f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -80,12 +71,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -98,13 +86,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -112,10 +96,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
index 544cb1a..a1a299a 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/RobotContainer.java
@@ -1,33 +1,27 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervecontrollercommand;
 
-import java.util.List;
-
-import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.Trajectory;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.RunCommand;
-import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
-import edu.wpi.first.wpilibj2.command.button.JoystickButton;
-
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.AutoConstants;
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.OIConstants;
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems.DriveSubsystem;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SwerveControllerCommand;
+import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import java.util.List;
 
 /*
  * This class is where the bulk of the robot should be declared.  Since Command-based is a
@@ -42,9 +36,7 @@
   // The driver's controller
   XboxController m_driverController = new XboxController(OIConstants.kDriverControllerPort);
 
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
@@ -54,22 +46,23 @@
     m_robotDrive.setDefaultCommand(
         // A split-stick arcade command, with forward/backward controlled by the left
         // hand, and turning controlled by the right.
-        new RunCommand(() -> m_robotDrive.drive(
-            m_driverController.getY(GenericHID.Hand.kLeft),
-            m_driverController.getX(GenericHID.Hand.kRight),
-            m_driverController.getX(GenericHID.Hand.kLeft), false)));
-
+        new RunCommand(
+            () ->
+                m_robotDrive.drive(
+                    m_driverController.getLeftY(),
+                    m_driverController.getRightX(),
+                    m_driverController.getLeftX(),
+                    false),
+            m_robotDrive));
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
-   * instantiating a {@link GenericHID} or one of its subclasses ({@link
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
    * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then calling passing it to a
    * {@link JoystickButton}.
    */
-  private void configureButtonBindings() {
-  }
-
+  private void configureButtonBindings() {}
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
@@ -79,43 +72,40 @@
   public Command getAutonomousCommand() {
     // Create config for trajectory
     TrajectoryConfig config =
-        new TrajectoryConfig(AutoConstants.kMaxSpeedMetersPerSecond,
-                             AutoConstants.kMaxAccelerationMetersPerSecondSquared)
+        new TrajectoryConfig(
+                AutoConstants.kMaxSpeedMetersPerSecond,
+                AutoConstants.kMaxAccelerationMetersPerSecondSquared)
             // Add kinematics to ensure max speed is actually obeyed
             .setKinematics(DriveConstants.kDriveKinematics);
 
     // An example trajectory to follow.  All units in meters.
-    Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
-        // Start at the origin facing the +X direction
-        new Pose2d(0, 0, new Rotation2d(0)),
-        // Pass through these two interior waypoints, making an 's' curve path
-        List.of(
-            new Translation2d(1, 1),
-            new Translation2d(2, -1)
-        ),
-        // End 3 meters straight ahead of where we started, facing forward
-        new Pose2d(3, 0, new Rotation2d(0)),
-        config
-    );
+    Trajectory exampleTrajectory =
+        TrajectoryGenerator.generateTrajectory(
+            // Start at the origin facing the +X direction
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(3, 0, new Rotation2d(0)),
+            config);
 
-    var thetaController = new ProfiledPIDController(AutoConstants.kPThetaController, 0, 0,
-        AutoConstants.kThetaControllerConstraints);
+    var thetaController =
+        new ProfiledPIDController(
+            AutoConstants.kPThetaController, 0, 0, AutoConstants.kThetaControllerConstraints);
     thetaController.enableContinuousInput(-Math.PI, Math.PI);
 
-    SwerveControllerCommand swerveControllerCommand = new SwerveControllerCommand(
-        exampleTrajectory,
-        m_robotDrive::getPose, //Functional interface to feed supplier
-        DriveConstants.kDriveKinematics,
+    SwerveControllerCommand swerveControllerCommand =
+        new SwerveControllerCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose, // Functional interface to feed supplier
+            DriveConstants.kDriveKinematics,
 
-        //Position controllers
-        new PIDController(AutoConstants.kPXController, 0, 0),
-        new PIDController(AutoConstants.kPYController, 0, 0), thetaController,
-
-        m_robotDrive::setModuleStates,
-
-        m_robotDrive
-
-    );
+            // Position controllers
+            new PIDController(AutoConstants.kPXController, 0, 0),
+            new PIDController(AutoConstants.kPYController, 0, 0),
+            thetaController,
+            m_robotDrive::setModuleStates,
+            m_robotDrive);
 
     // Reset odometry to the starting pose of the trajectory.
     m_robotDrive.resetOdometry(exampleTrajectory.getInitialPose());
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
index 2baeec4..64d65b3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
@@ -1,59 +1,56 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
 
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.ADXRS450_Gyro;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveOdometry;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
-
-@SuppressWarnings("PMD.ExcessiveImports")
 public class DriveSubsystem extends SubsystemBase {
-  //Robot swerve modules
-  private final SwerveModule m_frontLeft
-      = new SwerveModule(DriveConstants.kFrontLeftDriveMotorPort,
-                         DriveConstants.kFrontLeftTurningMotorPort,
-                         DriveConstants.kFrontLeftDriveEncoderPorts,
-                         DriveConstants.kFrontLeftTurningEncoderPorts,
-                         DriveConstants.kFrontLeftDriveEncoderReversed,
-                         DriveConstants.kFrontLeftTurningEncoderReversed);
+  // Robot swerve modules
+  private final SwerveModule m_frontLeft =
+      new SwerveModule(
+          DriveConstants.kFrontLeftDriveMotorPort,
+          DriveConstants.kFrontLeftTurningMotorPort,
+          DriveConstants.kFrontLeftDriveEncoderPorts,
+          DriveConstants.kFrontLeftTurningEncoderPorts,
+          DriveConstants.kFrontLeftDriveEncoderReversed,
+          DriveConstants.kFrontLeftTurningEncoderReversed);
 
   private final SwerveModule m_rearLeft =
-      new SwerveModule(DriveConstants.kRearLeftDriveMotorPort,
-                       DriveConstants.kRearLeftTurningMotorPort,
-                       DriveConstants.kRearLeftDriveEncoderPorts,
-                       DriveConstants.kRearLeftTurningEncoderPorts,
-                       DriveConstants.kRearLeftDriveEncoderReversed,
-                       DriveConstants.kRearLeftTurningEncoderReversed);
-
+      new SwerveModule(
+          DriveConstants.kRearLeftDriveMotorPort,
+          DriveConstants.kRearLeftTurningMotorPort,
+          DriveConstants.kRearLeftDriveEncoderPorts,
+          DriveConstants.kRearLeftTurningEncoderPorts,
+          DriveConstants.kRearLeftDriveEncoderReversed,
+          DriveConstants.kRearLeftTurningEncoderReversed);
 
   private final SwerveModule m_frontRight =
-      new SwerveModule(DriveConstants.kFrontRightDriveMotorPort,
-                       DriveConstants.kFrontRightTurningMotorPort,
-                       DriveConstants.kFrontRightDriveEncoderPorts,
-                       DriveConstants.kFrontRightTurningEncoderPorts,
-                       DriveConstants.kFrontRightDriveEncoderReversed,
-                       DriveConstants.kFrontRightTurningEncoderReversed);
+      new SwerveModule(
+          DriveConstants.kFrontRightDriveMotorPort,
+          DriveConstants.kFrontRightTurningMotorPort,
+          DriveConstants.kFrontRightDriveEncoderPorts,
+          DriveConstants.kFrontRightTurningEncoderPorts,
+          DriveConstants.kFrontRightDriveEncoderReversed,
+          DriveConstants.kFrontRightTurningEncoderReversed);
 
   private final SwerveModule m_rearRight =
-      new SwerveModule(DriveConstants.kRearRightDriveMotorPort,
-                       DriveConstants.kRearRightTurningMotorPort,
-                       DriveConstants.kRearRightDriveEncoderPorts,
-                       DriveConstants.kRearRightTurningEncoderPorts,
-                       DriveConstants.kRearRightDriveEncoderReversed,
-                       DriveConstants.kRearRightTurningEncoderReversed);
+      new SwerveModule(
+          DriveConstants.kRearRightDriveMotorPort,
+          DriveConstants.kRearRightTurningMotorPort,
+          DriveConstants.kRearRightDriveEncoderPorts,
+          DriveConstants.kRearRightTurningEncoderPorts,
+          DriveConstants.kRearRightDriveEncoderReversed,
+          DriveConstants.kRearRightTurningEncoderReversed);
 
   // The gyro sensor
   private final Gyro m_gyro = new ADXRS450_Gyro();
@@ -62,17 +59,14 @@
   SwerveDriveOdometry m_odometry =
       new SwerveDriveOdometry(DriveConstants.kDriveKinematics, m_gyro.getRotation2d());
 
-  /**
-   * Creates a new DriveSubsystem.
-   */
-  public DriveSubsystem() {
-  }
+  /** Creates a new DriveSubsystem. */
+  public DriveSubsystem() {}
 
   @Override
   public void periodic() {
     // Update the odometry in the periodic block
     m_odometry.update(
-        new Rotation2d(getHeading()),
+        m_gyro.getRotation2d(),
         m_frontLeft.getState(),
         m_rearLeft.getState(),
         m_frontRight.getState(),
@@ -100,20 +94,20 @@
   /**
    * Method to drive the robot using joystick info.
    *
-   * @param xSpeed        Speed of the robot in the x direction (forward).
-   * @param ySpeed        Speed of the robot in the y direction (sideways).
-   * @param rot           Angular rate of the robot.
+   * @param xSpeed Speed of the robot in the x direction (forward).
+   * @param ySpeed Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
    * @param fieldRelative Whether the provided x and y speeds are relative to the field.
    */
   @SuppressWarnings("ParameterName")
   public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
-    var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
-        fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(
-            xSpeed, ySpeed, rot, m_gyro.getRotation2d())
-            : new ChassisSpeeds(xSpeed, ySpeed, rot)
-    );
-    SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates,
-                                               DriveConstants.kMaxSpeedMetersPerSecond);
+    var swerveModuleStates =
+        DriveConstants.kDriveKinematics.toSwerveModuleStates(
+            fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                : new ChassisSpeeds(xSpeed, ySpeed, rot));
+    SwerveDriveKinematics.normalizeWheelSpeeds(
+        swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
     m_frontLeft.setDesiredState(swerveModuleStates[0]);
     m_frontRight.setDesiredState(swerveModuleStates[1]);
     m_rearLeft.setDesiredState(swerveModuleStates[2]);
@@ -126,17 +120,15 @@
    * @param desiredStates The desired SwerveModule states.
    */
   public void setModuleStates(SwerveModuleState[] desiredStates) {
-    SwerveDriveKinematics.normalizeWheelSpeeds(desiredStates,
-                                               DriveConstants.kMaxSpeedMetersPerSecond);
+    SwerveDriveKinematics.normalizeWheelSpeeds(
+        desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
     m_frontLeft.setDesiredState(desiredStates[0]);
     m_frontRight.setDesiredState(desiredStates[1]);
     m_rearLeft.setDesiredState(desiredStates[2]);
     m_rearRight.setDesiredState(desiredStates[3]);
   }
 
-  /**
-   * Resets the drive encoders to currently read a position of 0.
-   */
+  /** Resets the drive encoders to currently read a position of 0. */
   public void resetEncoders() {
     m_frontLeft.resetEncoders();
     m_rearLeft.resetEncoders();
@@ -144,9 +136,7 @@
     m_rearRight.resetEncoders();
   }
 
-  /**
-   * Zeroes the heading of the robot.
-   */
+  /** Zeroes the heading of the robot. */
   public void zeroHeading() {
     m_gyro.reset();
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
index e70c6ba..fd4c0b7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/SwerveModule.java
@@ -1,21 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.swervecontrollercommand.subsystems;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
 import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.Spark;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-
 import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.ModuleConstants;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
 
 public class SwerveModule {
   private final Spark m_driveMotor;
@@ -27,10 +23,12 @@
   private final PIDController m_drivePIDController =
       new PIDController(ModuleConstants.kPModuleDriveController, 0, 0);
 
-  //Using a TrapezoidProfile PIDController to allow for smooth turning
-  private final ProfiledPIDController m_turningPIDController
-      = new ProfiledPIDController(
-          ModuleConstants.kPModuleTurningController, 0, 0,
+  // Using a TrapezoidProfile PIDController to allow for smooth turning
+  private final ProfiledPIDController m_turningPIDController =
+      new ProfiledPIDController(
+          ModuleConstants.kPModuleTurningController,
+          0,
+          0,
           new TrapezoidProfile.Constraints(
               ModuleConstants.kMaxModuleAngularSpeedRadiansPerSecond,
               ModuleConstants.kMaxModuleAngularAccelerationRadiansPerSecondSquared));
@@ -38,16 +36,16 @@
   /**
    * Constructs a SwerveModule.
    *
-   * @param driveMotorChannel   ID for the drive motor.
+   * @param driveMotorChannel ID for the drive motor.
    * @param turningMotorChannel ID for the turning motor.
    */
-  public SwerveModule(int driveMotorChannel,
-                      int turningMotorChannel,
-                      int[] driveEncoderPorts,
-                      int[] turningEncoderPorts,
-                      boolean driveEncoderReversed,
-                      boolean turningEncoderReversed) {
-
+  public SwerveModule(
+      int driveMotorChannel,
+      int turningMotorChannel,
+      int[] driveEncoderPorts,
+      int[] turningEncoderPorts,
+      boolean driveEncoderReversed,
+      boolean turningEncoderReversed) {
     m_driveMotor = new Spark(driveMotorChannel);
     m_turningMotor = new Spark(turningMotorChannel);
 
@@ -60,15 +58,15 @@
     // resolution.
     m_driveEncoder.setDistancePerPulse(ModuleConstants.kDriveEncoderDistancePerPulse);
 
-    //Set whether drive encoder should be reversed or not
+    // Set whether drive encoder should be reversed or not
     m_driveEncoder.setReverseDirection(driveEncoderReversed);
 
     // Set the distance (in this case, angle) per pulse for the turning encoder.
-    // This is the the angle through an entire rotation (2 * wpi::math::pi)
-    // divided by the encoder resolution.
+    // This is the the angle through an entire rotation (2 * pi) divided by the
+    // encoder resolution.
     m_turningEncoder.setDistancePerPulse(ModuleConstants.kTurningEncoderDistancePerPulse);
 
-    //Set whether turning encoder should be reversed or not
+    // Set whether turning encoder should be reversed or not
     m_turningEncoder.setReverseDirection(turningEncoderReversed);
 
     // Limit the PID Controller's input range between -pi and pi and set the input
@@ -88,27 +86,27 @@
   /**
    * Sets the desired state for the module.
    *
-   * @param state Desired state with speed and angle.
+   * @param desiredState Desired state with speed and angle.
    */
-  public void setDesiredState(SwerveModuleState state) {
+  public void setDesiredState(SwerveModuleState desiredState) {
+    // Optimize the reference state to avoid spinning further than 90 degrees
+    SwerveModuleState state =
+        SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
     // Calculate the drive output from the drive PID controller.
-    final var driveOutput = m_drivePIDController.calculate(
-        m_driveEncoder.getRate(), state.speedMetersPerSecond);
+    final double driveOutput =
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
 
     // Calculate the turning motor output from the turning PID controller.
-    final var turnOutput = m_turningPIDController.calculate(
-        m_turningEncoder.get(), state.angle.getRadians()
-    );
+    final var turnOutput =
+        m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
 
     // Calculate the turning motor output from the turning PID controller.
     m_driveMotor.set(driveOutput);
     m_turningMotor.set(turnOutput);
   }
 
-  /**
-   * Zeros all the SwerveModule encoders.
-   */
-
+  /** Zeros all the SwerveModule encoders. */
   public void resetEncoders() {
     m_driveEncoder.reset();
     m_turningEncoder.reset();
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
new file mode 100644
index 0000000..0e8feef
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Drivetrain.java
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.Timer;
+
+/** Represents a swerve drive style drivetrain. */
+public class Drivetrain {
+  public static final double kMaxSpeed = 3.0; // 3 meters per second
+  public static final double kMaxAngularSpeed = Math.PI; // 1/2 rotation per second
+
+  private final Translation2d m_frontLeftLocation = new Translation2d(0.381, 0.381);
+  private final Translation2d m_frontRightLocation = new Translation2d(0.381, -0.381);
+  private final Translation2d m_backLeftLocation = new Translation2d(-0.381, 0.381);
+  private final Translation2d m_backRightLocation = new Translation2d(-0.381, -0.381);
+
+  private final SwerveModule m_frontLeft = new SwerveModule(1, 2, 0, 1, 2, 3);
+  private final SwerveModule m_frontRight = new SwerveModule(3, 4, 4, 5, 6, 7);
+  private final SwerveModule m_backLeft = new SwerveModule(5, 6, 8, 9, 10, 11);
+  private final SwerveModule m_backRight = new SwerveModule(7, 8, 12, 13, 14, 15);
+
+  private final AnalogGyro m_gyro = new AnalogGyro(0);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(
+          m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
+
+  /* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
+  below are robot specific, and should be tuned. */
+  private final SwerveDrivePoseEstimator m_poseEstimator =
+      new SwerveDrivePoseEstimator(
+          m_gyro.getRotation2d(),
+          new Pose2d(),
+          m_kinematics,
+          VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
+          VecBuilder.fill(Units.degreesToRadians(0.01)),
+          VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+
+  public Drivetrain() {
+    m_gyro.reset();
+  }
+
+  /**
+   * Method to drive the robot using joystick info.
+   *
+   * @param xSpeed Speed of the robot in the x direction (forward).
+   * @param ySpeed Speed of the robot in the y direction (sideways).
+   * @param rot Angular rate of the robot.
+   * @param fieldRelative Whether the provided x and y speeds are relative to the field.
+   */
+  @SuppressWarnings("ParameterName")
+  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
+    var swerveModuleStates =
+        m_kinematics.toSwerveModuleStates(
+            fieldRelative
+                ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, m_gyro.getRotation2d())
+                : new ChassisSpeeds(xSpeed, ySpeed, rot));
+    SwerveDriveKinematics.normalizeWheelSpeeds(swerveModuleStates, kMaxSpeed);
+    m_frontLeft.setDesiredState(swerveModuleStates[0]);
+    m_frontRight.setDesiredState(swerveModuleStates[1]);
+    m_backLeft.setDesiredState(swerveModuleStates[2]);
+    m_backRight.setDesiredState(swerveModuleStates[3]);
+  }
+
+  /** Updates the field relative position of the robot. */
+  public void updateOdometry() {
+    m_poseEstimator.update(
+        m_gyro.getRotation2d(),
+        m_frontLeft.getState(),
+        m_frontRight.getState(),
+        m_backLeft.getState(),
+        m_backRight.getState());
+
+    // Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
+    // a real robot, this must be calculated based either on latency or timestamps.
+    m_poseEstimator.addVisionMeasurement(
+        ExampleGlobalMeasurementSensor.getEstimatedGlobalPose(
+            m_poseEstimator.getEstimatedPosition()),
+        Timer.getFPGATimestamp() - 0.3);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java
new file mode 100644
index 0000000..a92a4fe
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/ExampleGlobalMeasurementSensor.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
+
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+
+/** This dummy class represents a global measurement sensor, such as a computer vision solution. */
+public final class ExampleGlobalMeasurementSensor {
+  private ExampleGlobalMeasurementSensor() {
+    // Utility class
+  }
+
+  /**
+   * Get a "noisy" fake global pose reading.
+   *
+   * @param estimatedRobotPose The robot pose.
+   */
+  public static Pose2d getEstimatedGlobalPose(Pose2d estimatedRobotPose) {
+    var rand =
+        StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
+    return new Pose2d(
+        estimatedRobotPose.getX() + rand.get(0, 0),
+        estimatedRobotPose.getY() + rand.get(1, 0),
+        estimatedRobotPose.getRotation().plus(new Rotation2d(rand.get(2, 0))));
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Main.java
new file mode 100644
index 0000000..2221f14
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java
new file mode 100644
index 0000000..7fbee8d
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/Robot.java
@@ -0,0 +1,49 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
+
+import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.XboxController;
+
+public class Robot extends TimedRobot {
+  private final XboxController m_controller = new XboxController(0);
+  private final Drivetrain m_swerve = new Drivetrain();
+
+  // Slew rate limiters to make joystick inputs more gentle; 1/3 sec from 0 to 1.
+  private final SlewRateLimiter m_xspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_yspeedLimiter = new SlewRateLimiter(3);
+  private final SlewRateLimiter m_rotLimiter = new SlewRateLimiter(3);
+
+  @Override
+  public void autonomousPeriodic() {
+    driveWithJoystick(false);
+    m_swerve.updateOdometry();
+  }
+
+  @Override
+  public void teleopPeriodic() {
+    driveWithJoystick(true);
+  }
+
+  private void driveWithJoystick(boolean fieldRelative) {
+    // Get the x speed. We are inverting this because Xbox controllers return
+    // negative values when we push forward.
+    final var xSpeed = -m_xspeedLimiter.calculate(m_controller.getLeftY()) * Drivetrain.kMaxSpeed;
+
+    // Get the y speed or sideways/strafe speed. We are inverting this because
+    // we want a positive value when we pull to the left. Xbox controllers
+    // return positive values when you pull to the right by default.
+    final var ySpeed = -m_yspeedLimiter.calculate(m_controller.getLeftX()) * Drivetrain.kMaxSpeed;
+
+    // Get the rate of angular rotation. We are inverting this because we want a
+    // positive value when we pull to the left (remember, CCW is positive in
+    // mathematics). Xbox controllers return positive values when you pull to
+    // the right by default.
+    final var rot = -m_rotLimiter.calculate(m_controller.getRightX()) * Drivetrain.kMaxAngularSpeed;
+
+    m_swerve.drive(xSpeed, ySpeed, rot, fieldRelative);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
new file mode 100644
index 0000000..d7c43dd
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervedriveposeestimator/SwerveModule.java
@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.examples.swervedriveposeestimator;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+public class SwerveModule {
+  private static final double kWheelRadius = 0.0508;
+  private static final int kEncoderResolution = 4096;
+
+  private static final double kModuleMaxAngularVelocity = Drivetrain.kMaxAngularSpeed;
+  private static final double kModuleMaxAngularAcceleration =
+      2 * Math.PI; // radians per second squared
+
+  private final MotorController m_driveMotor;
+  private final MotorController m_turningMotor;
+
+  private final Encoder m_driveEncoder;
+  private final Encoder m_turningEncoder;
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final PIDController m_drivePIDController = new PIDController(1, 0, 0);
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final ProfiledPIDController m_turningPIDController =
+      new ProfiledPIDController(
+          1,
+          0,
+          0,
+          new TrapezoidProfile.Constraints(
+              kModuleMaxAngularVelocity, kModuleMaxAngularAcceleration));
+
+  // Gains are for example purposes only - must be determined for your own robot!
+  private final SimpleMotorFeedforward m_driveFeedforward = new SimpleMotorFeedforward(1, 3);
+  private final SimpleMotorFeedforward m_turnFeedforward = new SimpleMotorFeedforward(1, 0.5);
+
+  /**
+   * Constructs a SwerveModule with a drive motor, turning motor, drive encoder and turning encoder.
+   *
+   * @param driveMotorChannel PWM output for the drive motor.
+   * @param turningMotorChannel PWM output for the turning motor.
+   * @param driveEncoderChannelA DIO input for the drive encoder channel A
+   * @param driveEncoderChannelB DIO input for the drive encoder channel B
+   * @param turningEncoderChannelA DIO input for the turning encoder channel A
+   * @param turningEncoderChannelB DIO input for the turning encoder channel B
+   */
+  public SwerveModule(
+      int driveMotorChannel,
+      int turningMotorChannel,
+      int driveEncoderChannelA,
+      int driveEncoderChannelB,
+      int turningEncoderChannelA,
+      int turningEncoderChannelB) {
+    m_driveMotor = new PWMSparkMax(driveMotorChannel);
+    m_turningMotor = new PWMSparkMax(turningMotorChannel);
+
+    m_driveEncoder = new Encoder(driveEncoderChannelA, driveEncoderChannelB);
+    m_turningEncoder = new Encoder(turningEncoderChannelA, turningEncoderChannelB);
+
+    // Set the distance per pulse for the drive encoder. We can simply use the
+    // distance traveled for one rotation of the wheel divided by the encoder
+    // resolution.
+    m_driveEncoder.setDistancePerPulse(2 * Math.PI * kWheelRadius / kEncoderResolution);
+
+    // Set the distance (in this case, angle) per pulse for the turning encoder.
+    // This is the the angle through an entire rotation (2 * pi) divided by the
+    // encoder resolution.
+    m_turningEncoder.setDistancePerPulse(2 * Math.PI / kEncoderResolution);
+
+    // Limit the PID Controller's input range between -pi and pi and set the input
+    // to be continuous.
+    m_turningPIDController.enableContinuousInput(-Math.PI, Math.PI);
+  }
+
+  /**
+   * Returns the current state of the module.
+   *
+   * @return The current state of the module.
+   */
+  public SwerveModuleState getState() {
+    return new SwerveModuleState(m_driveEncoder.getRate(), new Rotation2d(m_turningEncoder.get()));
+  }
+
+  /**
+   * Sets the desired state for the module.
+   *
+   * @param desiredState Desired state with speed and angle.
+   */
+  public void setDesiredState(SwerveModuleState desiredState) {
+    // Optimize the reference state to avoid spinning further than 90 degrees
+    SwerveModuleState state =
+        SwerveModuleState.optimize(desiredState, new Rotation2d(m_turningEncoder.get()));
+
+    // Calculate the drive output from the drive PID controller.
+    final double driveOutput =
+        m_drivePIDController.calculate(m_driveEncoder.getRate(), state.speedMetersPerSecond);
+
+    final double driveFeedforward = m_driveFeedforward.calculate(state.speedMetersPerSecond);
+
+    // Calculate the turning motor output from the turning PID controller.
+    final double turnOutput =
+        m_turningPIDController.calculate(m_turningEncoder.get(), state.angle.getRadians());
+
+    final double turnFeedforward =
+        m_turnFeedforward.calculate(m_turningPIDController.getSetpoint().velocity);
+
+    m_driveMotor.setVoltage(driveOutput + driveFeedforward);
+    m_turningMotor.setVoltage(turnOutput + turnFeedforward);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java
index e1dc017..404d96b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.tankdrive;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
old mode 100755
new mode 100644
index b8a9202..c91767a
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrive/Robot.java
@@ -1,29 +1,35 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.tankdrive;
 
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a demo program showing the use of the RobotDrive class, specifically
- * it contains the code necessary to operate a robot with tank drive.
+ * This is a demo program showing the use of the DifferentialDrive class, specifically it contains
+ * the code necessary to operate a robot with tank drive.
  */
 public class Robot extends TimedRobot {
   private DifferentialDrive m_myRobot;
   private Joystick m_leftStick;
   private Joystick m_rightStick;
 
+  private final MotorController m_leftMotor = new PWMSparkMax(0);
+  private final MotorController m_rightMotor = new PWMSparkMax(1);
+
   @Override
   public void robotInit() {
-    m_myRobot = new DifferentialDrive(new PWMVictorSPX(0), new PWMVictorSPX(1));
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+
+    m_myRobot = new DifferentialDrive(m_leftMotor, m_rightMotor);
     m_leftStick = new Joystick(0);
     m_rightStick = new Joystick(1);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
index 3adcee9..ada70f2 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
index f0a9e76..f05e22c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/tankdrivexboxcontroller/Robot.java
@@ -1,35 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.tankdrivexboxcontroller;
 
-import edu.wpi.first.wpilibj.GenericHID.Hand;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a demo program showing the use of the DifferentialDrive class.
- * Runs the motors with tank steering and an Xbox controller.
+ * This is a demo program showing the use of the DifferentialDrive class. Runs the motors with tank
+ * steering and an Xbox controller.
  */
 public class Robot extends TimedRobot {
-  private final PWMVictorSPX m_leftMotor = new PWMVictorSPX(0);
-  private final PWMVictorSPX m_rightMotor = new PWMVictorSPX(1);
+  private final PWMSparkMax m_leftMotor = new PWMSparkMax(0);
+  private final PWMSparkMax m_rightMotor = new PWMSparkMax(1);
   private final DifferentialDrive m_robotDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
   private final XboxController m_driverController = new XboxController(0);
 
   @Override
+  public void robotInit() {
+    // We need to invert one side of the drivetrain so that positive voltages
+    // result in both sides moving forward. Depending on how your robot's
+    // gearbox is constructed, you might have to invert the left side instead.
+    m_rightMotor.setInverted(true);
+  }
+
+  @Override
   public void teleopPeriodic() {
     // Drive with tank drive.
     // That means that the Y axis of the left stick moves the left side
     // of the robot forward and backward, and the Y axis of the right stick
     // moves the right side of the robot forward and backward.
-    m_robotDrive.tankDrive(m_driverController.getY(Hand.kLeft),
-        m_driverController.getY(Hand.kRight));
+    m_robotDrive.tankDrive(m_driverController.getLeftY(), m_driverController.getRightY());
   }
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
index edf56f8..3f4de06 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ultrasonic;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
index 2cae288..199d6f6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonic/Robot.java
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ultrasonic;
 
+import edu.wpi.first.math.filter.MedianFilter;
 import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.MedianFilter;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program demonstrating how to use an ultrasonic sensor and
- * proportional control to maintain a set distance from an object.
+ * This is a sample program demonstrating how to use an ultrasonic sensor and proportional control
+ * to maintain a set distance from an object.
  */
-
 public class Robot extends TimedRobot {
   // distance in inches the robot wants to stay from an object
   private static final double kHoldDistance = 12.0;
@@ -36,13 +32,12 @@
   private final MedianFilter m_filter = new MedianFilter(10);
 
   private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
-  private final DifferentialDrive m_robotDrive
-      = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
-      new PWMVictorSPX(kRightMotorPort));
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
 
   /**
-   * Tells the robot to drive to a set distance (in inches) from an object
-   * using proportional control.
+   * Tells the robot to drive to a set distance (in inches) from an object using proportional
+   * control.
    */
   @Override
   public void teleopPeriodic() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
index b8bffc7..b31e376 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ultrasonicpid;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
index b023429..526ac53 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/ultrasonicpid/Robot.java
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.examples.ultrasonicpid;
 
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.MedianFilter;
 import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.MedianFilter;
-import edu.wpi.first.wpilibj.PWMVictorSPX;
 import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.controller.PIDController;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
 
 /**
- * This is a sample program to demonstrate the use of a PIDController with an
- * ultrasonic sensor to reach and maintain a set distance from an object.
+ * This is a sample program to demonstrate the use of a PIDController with an ultrasonic sensor to
+ * reach and maintain a set distance from an object.
  */
 public class Robot extends TimedRobot {
   // distance in inches the robot wants to stay from an object
@@ -42,9 +39,8 @@
   private final MedianFilter m_filter = new MedianFilter(5);
 
   private final AnalogInput m_ultrasonic = new AnalogInput(kUltrasonicPort);
-  private final DifferentialDrive m_robotDrive
-      = new DifferentialDrive(new PWMVictorSPX(kLeftMotorPort),
-      new PWMVictorSPX(kRightMotorPort));
+  private final DifferentialDrive m_robotDrive =
+      new DifferentialDrive(new PWMSparkMax(kLeftMotorPort), new PWMSparkMax(kRightMotorPort));
   private final PIDController m_pidController = new PIDController(kP, kI, kD);
 
   @Override
@@ -57,8 +53,7 @@
   public void teleopPeriodic() {
     // returned value is filtered with a rolling median filter, since ultrasonics
     // tend to be quite noisy and susceptible to sudden outliers
-    double pidOutput
-        = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
+    double pidOutput = m_pidController.calculate(m_filter.calculate(m_ultrasonic.getVoltage()));
 
     m_robotDrive.arcadeDrive(pidOutput, 0);
   }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
index 0184096..ecfe201 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Constants.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.commandbased;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
- * constants.  This class should not be used for any other purpose.  All constants should be
- * declared globally (i.e. public static).  Do not put anything functional in this class.
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
  *
  * <p>It is advised to statically import this class (or one of its inner classes) wherever the
  * constants are needed, to reduce verbosity.
  */
-public final class Constants {
-}
+public final class Constants {}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
index b529f34..7fa438b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Main.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.commandbased;
 
@@ -15,8 +12,7 @@
  * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
index c0bc795..34cd3ce 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.commandbased;
 
@@ -37,8 +34,8 @@
    * This function is called every robot packet, no matter the mode. Use this for items like
    * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
   public void robotPeriodic() {
@@ -49,20 +46,14 @@
     CommandScheduler.getInstance().run();
   }
 
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   */
+  /** This function is called once each time the robot enters Disabled mode. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This autonomous runs the autonomous command selected by your {@link RobotContainer} class.
-   */
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
   @Override
   public void autonomousInit() {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
@@ -73,12 +64,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
   public void teleopInit() {
@@ -91,12 +79,9 @@
     }
   }
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
   public void testInit() {
@@ -104,10 +89,7 @@
     CommandScheduler.getInstance().cancelAll();
   }
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
index 4592f2e..75f79ae 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/RobotContainer.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.commandbased;
 
@@ -14,10 +11,10 @@
 import edu.wpi.first.wpilibj2.command.Command;
 
 /**
- * This class is where the bulk of the robot should be declared.  Since Command-based is a
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
- * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
- * (including subsystems, commands, and button mappings) should be declared here.
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
  */
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
@@ -25,25 +22,19 @@
 
   private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem);
 
-
-
-  /**
-   * The container for the robot.  Contains subsystems, OI devices, and commands.
-   */
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
   public RobotContainer() {
     // Configure the button bindings
     configureButtonBindings();
   }
 
   /**
-   * Use this method to define your button->command mappings.  Buttons can be created by
+   * Use this method to define your button->command mappings. Buttons can be created by
    * instantiating a {@link GenericHID} or one of its subclasses ({@link
-   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a
-   * {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
    */
-  private void configureButtonBindings() {
-  }
-
+  private void configureButtonBindings() {}
 
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
index 7cfe642..194cbf6 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/commands/ExampleCommand.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.commandbased.commands;
 
 import edu.wpi.first.wpilibj.templates.commandbased.subsystems.ExampleSubsystem;
 import edu.wpi.first.wpilibj2.command.CommandBase;
 
-/**
- * An example command that uses an example subsystem.
- */
+/** An example command that uses an example subsystem. */
 public class ExampleCommand extends CommandBase {
   @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
   private final ExampleSubsystem m_subsystem;
@@ -30,18 +25,15 @@
 
   // Called when the command is initially scheduled.
   @Override
-  public void initialize() {
-  }
+  public void initialize() {}
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
-  public void execute() {
-  }
+  public void execute() {}
 
   // Called once the command ends or is interrupted.
   @Override
-  public void end(boolean interrupted) {
-  }
+  public void end(boolean interrupted) {}
 
   // Returns true when the command should end.
   @Override
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
index 76202fe..8a3594b 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/commandbased/subsystems/ExampleSubsystem.java
@@ -1,21 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.commandbased.subsystems;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class ExampleSubsystem extends SubsystemBase {
-  /**
-   * Creates a new ExampleSubsystem.
-   */
-  public ExampleSubsystem() {
-
-  }
+  /** Creates a new ExampleSubsystem. */
+  public ExampleSubsystem() {}
 
   @Override
   public void periodic() {
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java
new file mode 100644
index 0000000..76d13a4
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/EducationalRobot.java
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.educational;
+
+import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+
+/** Educational robot base class. */
+public class EducationalRobot extends RobotBase {
+  public void robotInit() {}
+
+  public void disabled() {}
+
+  public void run() {}
+
+  public void autonomous() {
+    run();
+  }
+
+  public void teleop() {
+    run();
+  }
+
+  public void test() {
+    run();
+  }
+
+  private volatile boolean m_exit;
+
+  @Override
+  public void startCompetition() {
+    robotInit();
+
+    // Tell the DS that the robot is ready to be enabled
+    HAL.observeUserProgramStarting();
+
+    while (!Thread.currentThread().isInterrupted() && !m_exit) {
+      if (isDisabled()) {
+        DriverStation.inDisabled(true);
+        disabled();
+        DriverStation.inDisabled(false);
+        while (isDisabled()) {
+          DriverStation.waitForData();
+        }
+      } else if (isAutonomous()) {
+        DriverStation.inAutonomous(true);
+        autonomous();
+        DriverStation.inAutonomous(false);
+        while (isAutonomousEnabled()) {
+          DriverStation.waitForData();
+        }
+      } else if (isTest()) {
+        DriverStation.inTest(true);
+        test();
+        DriverStation.inTest(false);
+        while (isTest() && isEnabled()) {
+          DriverStation.waitForData();
+        }
+      } else {
+        DriverStation.inTeleop(true);
+        teleop();
+        DriverStation.inTeleop(false);
+        while (isTeleopEnabled()) {
+          DriverStation.waitForData();
+        }
+      }
+    }
+  }
+
+  @Override
+  public void endCompetition() {
+    m_exit = true;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Main.java
new file mode 100644
index 0000000..80b9bc9
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.educational;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Robot.java
new file mode 100644
index 0000000..0643977
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/educational/Robot.java
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.educational;
+
+/**
+ * The VM is configured to automatically run this class, and to call the run() function when the
+ * robot is enabled. If you change the name of this class or the package after creating this
+ * project, you must also update the build.gradle file in the project.
+ */
+public class Robot extends EducationalRobot {
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {}
+
+  /** This function is run when the robot is enabled. */
+  @Override
+  public void run() {}
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
deleted file mode 100644
index 67d8377..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Main.java
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-import edu.wpi.first.wpilibj.RobotBase;
-
-/**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
- */
-public final class Main {
-  private Main() {
-  }
-
-  /**
-   * Main initialization function. Do not perform any initialization here.
-   *
-   * <p>If you change your main robot class, change the parameter type.
-   */
-  public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
deleted file mode 100644
index d4c7f91..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/OI.java
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-/**
- * This class is the glue that binds the controls on the physical operator
- * interface to the commands and command groups that allow control of the robot.
- */
-public class OI {
-  //// CREATING BUTTONS
-  // One type of button is a joystick button which is any button on a
-  //// joystick.
-  // You create one by telling it which joystick it's on and which button
-  // number it is.
-  // Joystick stick = new Joystick(port);
-  // Button button = new JoystickButton(stick, buttonNumber);
-
-  // There are a few additional built in buttons you can use. Additionally,
-  // by subclassing Button you can create custom triggers and bind those to
-  // commands the same as any other Button.
-
-  //// TRIGGERING COMMANDS WITH BUTTONS
-  // Once you have a button, it's trivial to bind it to a button in one of
-  // three ways:
-
-  // Start the command when the button is pressed and let it run the command
-  // until it is finished as determined by it's isFinished method.
-  // button.whenPressed(new ExampleCommand());
-
-  // Run the command while the button is being held down and interrupt it once
-  // the button is released.
-  // button.whileHeld(new ExampleCommand());
-
-  // Start the command when the button is released and let it run the command
-  // until it is finished as determined by it's isFinished method.
-  // button.whenReleased(new ExampleCommand());
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
deleted file mode 100644
index 8f000ac..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/Robot.java
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-import edu.wpi.first.wpilibj.TimedRobot;
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.command.Scheduler;
-import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.templates.oldcommandbased.commands.ExampleCommand;
-import edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems.ExampleSubsystem;
-
-/**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
- * project.
- */
-public class Robot extends TimedRobot {
-  public static ExampleSubsystem m_subsystem = new ExampleSubsystem();
-  public static OI m_oi;
-
-  Command m_autonomousCommand;
-  SendableChooser<Command> m_chooser = new SendableChooser<>();
-
-  /**
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
-   */
-  @Override
-  public void robotInit() {
-    m_oi = new OI();
-    m_chooser.setDefaultOption("Default Auto", new ExampleCommand());
-    // chooser.addOption("My Auto", new MyAutoCommand());
-    SmartDashboard.putData("Auto mode", m_chooser);
-  }
-
-  /**
-   * This function is called every robot packet, no matter the mode. Use
-   * this for items like diagnostics that you want ran during disabled,
-   * autonomous, teleoperated and test.
-   *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
-   */
-  @Override
-  public void robotPeriodic() {
-  }
-
-  /**
-   * This function is called once each time the robot enters Disabled mode.
-   * You can use it to reset any subsystem information you want to clear when
-   * the robot is disabled.
-   */
-  @Override
-  public void disabledInit() {
-  }
-
-  @Override
-  public void disabledPeriodic() {
-    Scheduler.getInstance().run();
-  }
-
-  /**
-   * This autonomous (along with the chooser code above) shows how to select
-   * between different autonomous modes using the dashboard. The sendable
-   * chooser code works with the Java SmartDashboard. If you prefer the
-   * LabVIEW Dashboard, remove all of the chooser code and uncomment the
-   * getString code to get the auto name from the text box below the Gyro
-   *
-   * <p>You can add additional auto modes by adding additional commands to the
-   * chooser code above (like the commented example) or additional comparisons
-   * to the switch structure below with additional strings & commands.
-   */
-  @Override
-  public void autonomousInit() {
-    m_autonomousCommand = m_chooser.getSelected();
-
-    /*
-     * String autoSelected = SmartDashboard.getString("Auto Selector",
-     * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
-     * = new MyAutoCommand(); break; case "Default Auto": default:
-     * autonomousCommand = new ExampleCommand(); break; }
-     */
-
-    // schedule the autonomous command (example)
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.start();
-    }
-  }
-
-  /**
-   * This function is called periodically during autonomous.
-   */
-  @Override
-  public void autonomousPeriodic() {
-    Scheduler.getInstance().run();
-  }
-
-  @Override
-  public void teleopInit() {
-    // This makes sure that the autonomous stops running when
-    // teleop starts running. If you want the autonomous to
-    // continue until interrupted by another command, remove
-    // this line or comment it out.
-    if (m_autonomousCommand != null) {
-      m_autonomousCommand.cancel();
-    }
-  }
-
-  /**
-   * This function is called periodically during operator control.
-   */
-  @Override
-  public void teleopPeriodic() {
-    Scheduler.getInstance().run();
-  }
-
-  /**
-   * This function is called periodically during test mode.
-   */
-  @Override
-  public void testPeriodic() {
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
deleted file mode 100644
index 9f33b02..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/RobotMap.java
+++ /dev/null
@@ -1,26 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased;
-
-/**
- * The RobotMap is a mapping from the ports sensors and actuators are wired into
- * to a variable name. This provides flexibility changing wiring, makes checking
- * the wiring easier and significantly reduces the number of magic numbers
- * floating around.
- */
-public class RobotMap {
-  // For example to map the left and right motors, you could define the
-  // following variables to use with your drivetrain subsystem.
-  // public static int leftMotor = 1;
-  // public static int rightMotor = 2;
-
-  // If you are using multiple modules, make sure to define both the port
-  // number and the module. For example you with a rangefinder:
-  // public static int rangefinderPort = 1;
-  // public static int rangefinderModule = 1;
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
deleted file mode 100644
index 0d84987..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/commands/ExampleCommand.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-import edu.wpi.first.wpilibj.templates.oldcommandbased.Robot;
-
-/**
- * An example command.  You can replace me with your own command.
- */
-public class ExampleCommand extends Command {
-  public ExampleCommand() {
-    // Use requires() here to declare subsystem dependencies
-    requires(Robot.m_subsystem);
-  }
-
-  // Called just before this Command runs the first time
-  @Override
-  protected void initialize() {
-  }
-
-  // Called repeatedly when this Command is scheduled to run
-  @Override
-  protected void execute() {
-  }
-
-  // Make this return true when this Command no longer needs to run execute()
-  @Override
-  protected boolean isFinished() {
-    return false;
-  }
-
-  // Called once after isFinished returns true
-  @Override
-  protected void end() {
-  }
-
-  // Called when another command which requires one or more of the same
-  // subsystems is scheduled to run
-  @Override
-  protected void interrupted() {
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
deleted file mode 100644
index f3d1de9..0000000
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/oldcommandbased/subsystems/ExampleSubsystem.java
+++ /dev/null
@@ -1,25 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.templates.oldcommandbased.subsystems;
-
-import edu.wpi.first.wpilibj.command.Subsystem;
-
-/**
- * An example subsystem. You can replace with me with your own subsystem.
- */
-public class ExampleSubsystem extends Subsystem {
-  // Put methods for controlling this subsystem
-  // here. Call these from Commands.
-
-
-  @Override
-  protected void initDefaultCommand() {
-    // Set the default command for a subsystem here.
-    // setDefaultCommand(new MySpecialCommand());
-  }
-}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
index 65c6268..e465d47 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.robotbaseskeleton;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
index bfd3c46..787395c 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/robotbaseskeleton/Robot.java
@@ -1,41 +1,32 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.robotbaseskeleton;
 
 import edu.wpi.first.hal.HAL;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 
 /**
- * The VM is configured to automatically run this class. If you change the name
- * of this class or the package after creating this project, you must also
- * update the build.gradle file in the project.
+ * The VM is configured to automatically run this class. If you change the name of this class or the
+ * package after creating this project, you must also update the build.gradle file in the project.
  */
 public class Robot extends RobotBase {
-  public void robotInit() {
-  }
+  public void robotInit() {}
 
-  public void disabled() {
-  }
+  public void disabled() {}
 
-  public void autonomous() {
-  }
+  public void autonomous() {}
 
-  public void teleop() {
-  }
+  public void teleop() {}
 
-  public void test() {
-  }
+  public void test() {}
 
   private volatile boolean m_exit;
 
-  @SuppressWarnings("PMD.CyclomaticComplexity")
   @Override
   public void startCompetition() {
     robotInit();
@@ -45,36 +36,36 @@
 
     while (!Thread.currentThread().isInterrupted() && !m_exit) {
       if (isDisabled()) {
-        m_ds.InDisabled(true);
+        DriverStation.inDisabled(true);
         disabled();
-        m_ds.InDisabled(false);
+        DriverStation.inDisabled(false);
         while (isDisabled()) {
-          m_ds.waitForData();
+          DriverStation.waitForData();
         }
       } else if (isAutonomous()) {
-        m_ds.InAutonomous(true);
+        DriverStation.inAutonomous(true);
         autonomous();
-        m_ds.InAutonomous(false);
+        DriverStation.inAutonomous(false);
         while (isAutonomousEnabled()) {
-          m_ds.waitForData();
+          DriverStation.waitForData();
         }
       } else if (isTest()) {
         LiveWindow.setEnabled(true);
         Shuffleboard.enableActuatorWidgets();
-        m_ds.InTest(true);
+        DriverStation.inTest(true);
         test();
-        m_ds.InTest(false);
+        DriverStation.inTest(false);
         while (isTest() && isEnabled()) {
-          m_ds.waitForData();
+          DriverStation.waitForData();
         }
         LiveWindow.setEnabled(false);
         Shuffleboard.disableActuatorWidgets();
       } else {
-        m_ds.InOperatorControl(true);
+        DriverStation.inTeleop(true);
         teleop();
-        m_ds.InOperatorControl(false);
-        while (isOperatorControlEnabled()) {
-          m_ds.waitForData();
+        DriverStation.inTeleop(false);
+        while (isTeleopEnabled()) {
+          DriverStation.waitForData();
         }
       }
     }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Constants.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Constants.java
new file mode 100644
index 0000000..9a1ab69
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Constants.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romicommandbased;
+
+/**
+ * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
+ * constants. This class should not be used for any other purpose. All constants should be declared
+ * globally (i.e. public static). Do not put anything functional in this class.
+ *
+ * <p>It is advised to statically import this class (or one of its inner classes) wherever the
+ * constants are needed, to reduce verbosity.
+ */
+public final class Constants {}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Main.java
new file mode 100644
index 0000000..b73f963
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romicommandbased;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Robot.java
new file mode 100644
index 0000000..2475ee6
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/Robot.java
@@ -0,0 +1,95 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romicommandbased;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandScheduler;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private Command m_autonomousCommand;
+
+  private RobotContainer m_robotContainer;
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    // Instantiate our RobotContainer.  This will perform all our button bindings, and put our
+    // autonomous chooser on the dashboard.
+    m_robotContainer = new RobotContainer();
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {
+    // Runs the Scheduler.  This is responsible for polling buttons, adding newly-scheduled
+    // commands, running already-scheduled commands, removing finished or interrupted commands,
+    // and running subsystem periodic() methods.  This must be called from the robot's periodic
+    // block in order for anything in the Command-based framework to work.
+    CommandScheduler.getInstance().run();
+  }
+
+  /** This function is called once each time the robot enters Disabled mode. */
+  @Override
+  public void disabledInit() {}
+
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This autonomous runs the autonomous command selected by your {@link RobotContainer} class. */
+  @Override
+  public void autonomousInit() {
+    m_autonomousCommand = m_robotContainer.getAutonomousCommand();
+
+    // schedule the autonomous command (example)
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.schedule();
+    }
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {}
+
+  @Override
+  public void teleopInit() {
+    // This makes sure that the autonomous stops running when
+    // teleop starts running. If you want the autonomous to
+    // continue until interrupted by another command, remove
+    // this line or comment it out.
+    if (m_autonomousCommand != null) {
+      m_autonomousCommand.cancel();
+    }
+  }
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  @Override
+  public void testInit() {
+    // Cancels all running commands at the start of test mode.
+    CommandScheduler.getInstance().cancelAll();
+  }
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/RobotContainer.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/RobotContainer.java
new file mode 100644
index 0000000..11596a5
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/RobotContainer.java
@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romicommandbased;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.templates.romicommandbased.commands.ExampleCommand;
+import edu.wpi.first.wpilibj.templates.romicommandbased.subsystems.RomiDrivetrain;
+import edu.wpi.first.wpilibj2.command.Command;
+
+/**
+ * This class is where the bulk of the robot should be declared. Since Command-based is a
+ * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
+ * periodic methods (other than the scheduler calls). Instead, the structure of the robot (including
+ * subsystems, commands, and button mappings) should be declared here.
+ */
+public class RobotContainer {
+  // The robot's subsystems and commands are defined here...
+  private final RomiDrivetrain m_romiDrivetrain = new RomiDrivetrain();
+
+  private final ExampleCommand m_autoCommand = new ExampleCommand(m_romiDrivetrain);
+
+  /** The container for the robot. Contains subsystems, OI devices, and commands. */
+  public RobotContainer() {
+    // Configure the button bindings
+    configureButtonBindings();
+  }
+
+  /**
+   * Use this method to define your button->command mappings. Buttons can be created by
+   * instantiating a {@link edu.wpi.first.wpilibj.GenericHID} or one of its subclasses ({@link
+   * edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then passing it to a {@link
+   * edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+   */
+  private void configureButtonBindings() {}
+
+  /**
+   * Use this to pass the autonomous command to the main {@link Robot} class.
+   *
+   * @return the command to run in autonomous
+   */
+  public Command getAutonomousCommand() {
+    // An ExampleCommand will run in autonomous
+    return m_autoCommand;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/commands/ExampleCommand.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/commands/ExampleCommand.java
new file mode 100644
index 0000000..0ebb5d2
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/commands/ExampleCommand.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romicommandbased.commands;
+
+import edu.wpi.first.wpilibj.templates.romicommandbased.subsystems.RomiDrivetrain;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+/** An example command that uses an example subsystem. */
+public class ExampleCommand extends CommandBase {
+  @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
+  private final RomiDrivetrain m_subsystem;
+
+  /**
+   * Creates a new ExampleCommand.
+   *
+   * @param subsystem The subsystem used by this command.
+   */
+  public ExampleCommand(RomiDrivetrain subsystem) {
+    m_subsystem = subsystem;
+    // Use addRequirements() here to declare subsystem dependencies.
+    addRequirements(subsystem);
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {}
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {}
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {}
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    return false;
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
new file mode 100644
index 0000000..80543c6
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romicommandbased/subsystems/RomiDrivetrain.java
@@ -0,0 +1,63 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romicommandbased.subsystems;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class RomiDrivetrain extends SubsystemBase {
+  private static final double kCountsPerRevolution = 1440.0;
+  private static final double kWheelDiameterInch = 2.75591; // 70 mm
+
+  // The Romi has the left and right motors set to
+  // PWM channels 0 and 1 respectively
+  private final Spark m_leftMotor = new Spark(0);
+  private final Spark m_rightMotor = new Spark(1);
+
+  // The Romi has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  /** Creates a new RomiDrivetrain. */
+  public RomiDrivetrain() {
+    // Use inches as unit for encoder distances
+    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    resetEncoders();
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public double getLeftDistanceInch() {
+    return m_leftEncoder.getDistance();
+  }
+
+  public double getRightDistanceInch() {
+    return m_rightEncoder.getDistance();
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+
+  @Override
+  public void simulationPeriodic() {
+    // This method will be called once per scheduler run during simulation
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Main.java
new file mode 100644
index 0000000..69c9ced
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Main.java
@@ -0,0 +1,25 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romitimed;
+
+import edu.wpi.first.wpilibj.RobotBase;
+
+/**
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
+ */
+public final class Main {
+  private Main() {}
+
+  /**
+   * Main initialization function. Do not perform any initialization here.
+   *
+   * <p>If you change your main robot class, change the parameter type.
+   */
+  public static void main(String... args) {
+    RobotBase.startRobot(Robot::new);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Robot.java
new file mode 100644
index 0000000..75197ec
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/Robot.java
@@ -0,0 +1,102 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romitimed;
+
+import edu.wpi.first.wpilibj.TimedRobot;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+/**
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
+ * project.
+ */
+public class Robot extends TimedRobot {
+  private static final String kDefaultAuto = "Default";
+  private static final String kCustomAuto = "My Auto";
+  private String m_autoSelected;
+  private final SendableChooser<String> m_chooser = new SendableChooser<>();
+
+  private final RomiDrivetrain m_drivetrain = new RomiDrivetrain();
+
+  /**
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
+   */
+  @Override
+  public void robotInit() {
+    m_chooser.setDefaultOption("Default Auto", kDefaultAuto);
+    m_chooser.addOption("My Auto", kCustomAuto);
+    SmartDashboard.putData("Auto choices", m_chooser);
+  }
+
+  /**
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
+   *
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
+   */
+  @Override
+  public void robotPeriodic() {}
+
+  /**
+   * This autonomous (along with the chooser code above) shows how to select between different
+   * autonomous modes using the dashboard. The sendable chooser code works with the Java
+   * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
+   * uncomment the getString line to get the auto name from the text box below the Gyro
+   *
+   * <p>You can add additional auto modes by adding additional comparisons to the switch structure
+   * below with additional strings. If using the SendableChooser make sure to add them to the
+   * chooser code above as well.
+   */
+  @Override
+  public void autonomousInit() {
+    m_autoSelected = m_chooser.getSelected();
+    // m_autoSelected = SmartDashboard.getString("Auto Selector", kDefaultAuto);
+    System.out.println("Auto selected: " + m_autoSelected);
+
+    m_drivetrain.resetEncoders();
+  }
+
+  /** This function is called periodically during autonomous. */
+  @Override
+  public void autonomousPeriodic() {
+    switch (m_autoSelected) {
+      case kCustomAuto:
+        // Put custom auto code here
+        break;
+      case kDefaultAuto:
+      default:
+        // Put default auto code here
+        break;
+    }
+  }
+
+  /** This function is called once when teleop is enabled. */
+  @Override
+  public void teleopInit() {}
+
+  /** This function is called periodically during operator control. */
+  @Override
+  public void teleopPeriodic() {}
+
+  /** This function is called once when the robot is disabled. */
+  @Override
+  public void disabledInit() {}
+
+  /** This function is called periodically when disabled. */
+  @Override
+  public void disabledPeriodic() {}
+
+  /** This function is called once when test mode is enabled. */
+  @Override
+  public void testInit() {}
+
+  /** This function is called periodically during test mode. */
+  @Override
+  public void testPeriodic() {}
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
new file mode 100644
index 0000000..96c81b1
--- /dev/null
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/romitimed/RomiDrivetrain.java
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.templates.romitimed;
+
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.drive.DifferentialDrive;
+import edu.wpi.first.wpilibj.motorcontrol.Spark;
+
+public class RomiDrivetrain {
+  private static final double kCountsPerRevolution = 1440.0;
+  private static final double kWheelDiameterInch = 2.75591; // 70 mm
+
+  // The Romi has the left and right motors set to
+  // PWM channels 0 and 1 respectively
+  private final Spark m_leftMotor = new Spark(0);
+  private final Spark m_rightMotor = new Spark(1);
+
+  // The Romi has onboard encoders that are hardcoded
+  // to use DIO pins 4/5 and 6/7 for the left and right
+  private final Encoder m_leftEncoder = new Encoder(4, 5);
+  private final Encoder m_rightEncoder = new Encoder(6, 7);
+
+  // Set up the differential drive controller
+  private final DifferentialDrive m_diffDrive = new DifferentialDrive(m_leftMotor, m_rightMotor);
+
+  /** Creates a new RomiDrivetrain. */
+  public RomiDrivetrain() {
+    // Use inches as unit for encoder distances
+    m_leftEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    m_rightEncoder.setDistancePerPulse((Math.PI * kWheelDiameterInch) / kCountsPerRevolution);
+    resetEncoders();
+  }
+
+  public void arcadeDrive(double xaxisSpeed, double zaxisRotate) {
+    m_diffDrive.arcadeDrive(xaxisSpeed, zaxisRotate);
+  }
+
+  public void resetEncoders() {
+    m_leftEncoder.reset();
+    m_rightEncoder.reset();
+  }
+
+  public double getLeftDistanceInch() {
+    return m_leftEncoder.getDistance();
+  }
+
+  public double getRightDistanceInch() {
+    return m_rightEncoder.getDistance();
+  }
+}
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
index e0ebce7..9dd408f 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/templates.json
@@ -8,7 +8,7 @@
     "foldername": "timed",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Timed Skeleton (Advanced)",
@@ -19,7 +19,7 @@
     "foldername": "timedskeleton",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "RobotBase Skeleton (Advanced)",
@@ -30,7 +30,7 @@
     "foldername": "robotbaseskeleton",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   },
   {
     "name": "Command Robot",
@@ -44,14 +44,36 @@
     "commandversion": 2
   },
   {
-    "name": "Old Command Robot",
-    "description": "Old-command style (deprecated)",
+    "name": "Romi - Timed Robot",
+    "description": "Romi - Timed style",
     "tags": [
-      "Command"
+      "Timed", "Romi"
     ],
-    "foldername": "oldcommandbased",
+    "foldername": "romitimed",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Romi - Command Robot",
+    "description": "Romi - Command style",
+    "tags": [
+      "Command", "Romi"
+    ],
+    "foldername": "romicommandbased",
+    "gradlebase": "javaromi",
+    "mainclass": "Main",
+    "commandversion": 2
+  },
+  {
+    "name": "Educational Robot",
+    "description": "Educational Robot - Not for competition use",
+    "tags": [
+      "Educational"
+    ],
+    "foldername": "educational",
     "gradlebase": "java",
     "mainclass": "Main",
-    "commandversion": 1
+    "commandversion": 2
   }
 ]
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java
index 4cc64e0..9a3f9c7 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.timed;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
index 5974b41..9141414 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timed/Robot.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.timed;
 
@@ -12,10 +9,9 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 /**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
@@ -25,8 +21,8 @@
   private final SendableChooser<String> m_chooser = new SendableChooser<>();
 
   /**
-   * This function is run when the robot is first started up and should be
-   * used for any initialization code.
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
    */
   @Override
   public void robotInit() {
@@ -36,27 +32,24 @@
   }
 
   /**
-   * This function is called every robot packet, no matter the mode. Use
-   * this for items like diagnostics that you want ran during disabled,
-   * autonomous, teleoperated and test.
+   * This function is called every robot packet, no matter the mode. Use this for items like
+   * diagnostics that you want ran during disabled, autonomous, teleoperated and test.
    *
-   * <p>This runs after the mode specific periodic functions, but before
-   * LiveWindow and SmartDashboard integrated updating.
+   * <p>This runs after the mode specific periodic functions, but before LiveWindow and
+   * SmartDashboard integrated updating.
    */
   @Override
-  public void robotPeriodic() {
-  }
+  public void robotPeriodic() {}
 
   /**
-   * This autonomous (along with the chooser code above) shows how to select
-   * between different autonomous modes using the dashboard. The sendable
-   * chooser code works with the Java SmartDashboard. If you prefer the
-   * LabVIEW Dashboard, remove all of the chooser code and uncomment the
-   * getString line to get the auto name from the text box below the Gyro
+   * This autonomous (along with the chooser code above) shows how to select between different
+   * autonomous modes using the dashboard. The sendable chooser code works with the Java
+   * SmartDashboard. If you prefer the LabVIEW Dashboard, remove all of the chooser code and
+   * uncomment the getString line to get the auto name from the text box below the Gyro
    *
-   * <p>You can add additional auto modes by adding additional comparisons to
-   * the switch structure below with additional strings. If using the
-   * SendableChooser make sure to add them to the chooser code above as well.
+   * <p>You can add additional auto modes by adding additional comparisons to the switch structure
+   * below with additional strings. If using the SendableChooser make sure to add them to the
+   * chooser code above as well.
    */
   @Override
   public void autonomousInit() {
@@ -65,9 +58,7 @@
     System.out.println("Auto selected: " + m_autoSelected);
   }
 
-  /**
-   * This function is called periodically during autonomous.
-   */
+  /** This function is called periodically during autonomous. */
   @Override
   public void autonomousPeriodic() {
     switch (m_autoSelected) {
@@ -81,45 +72,27 @@
     }
   }
 
-  /**
-   * This function is called once when teleop is enabled.
-   */
+  /** This function is called once when teleop is enabled. */
   @Override
-  public void teleopInit() {
-  }
+  public void teleopInit() {}
 
-  /**
-   * This function is called periodically during operator control.
-   */
+  /** This function is called periodically during operator control. */
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
-  /**
-   * This function is called once when the robot is disabled.
-   */
+  /** This function is called once when the robot is disabled. */
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
-  /**
-   * This function is called periodically when disabled.
-   */
+  /** This function is called periodically when disabled. */
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
-  /**
-   * This function is called once when test mode is enabled.
-   */
+  /** This function is called once when test mode is enabled. */
   @Override
-  public void testInit() {
-  }
+  public void testInit() {}
 
-  /**
-   * This function is called periodically during test mode.
-   */
+  /** This function is called periodically during test mode. */
   @Override
-  public void testPeriodic() {
-  }
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java
index 6a895ba..ebe7ba4 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Main.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.timedskeleton;
 
 import edu.wpi.first.wpilibj.RobotBase;
 
 /**
- * Do NOT add any static variables to this class, or any initialization at all.
- * Unless you know what you are doing, do not modify this file except to
- * change the parameter class to the startRobot call.
+ * Do NOT add any static variables to this class, or any initialization at all. Unless you know what
+ * you are doing, do not modify this file except to change the parameter class to the startRobot
+ * call.
  */
 public final class Main {
-  private Main() {
-  }
+  private Main() {}
 
   /**
    * Main initialization function. Do not perform any initialization here.
diff --git a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
index b4480c8..d3968f3 100644
--- a/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
+++ b/third_party/allwpilib/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/templates/timedskeleton/Robot.java
@@ -1,64 +1,49 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.templates.timedskeleton;
 
 import edu.wpi.first.wpilibj.TimedRobot;
 
 /**
- * The VM is configured to automatically run this class, and to call the
- * functions corresponding to each mode, as described in the TimedRobot
- * documentation. If you change the name of this class or the package after
- * creating this project, you must also update the build.gradle file in the
+ * The VM is configured to automatically run this class, and to call the functions corresponding to
+ * each mode, as described in the TimedRobot documentation. If you change the name of this class or
+ * the package after creating this project, you must also update the build.gradle file in the
  * project.
  */
 public class Robot extends TimedRobot {
   /**
-   * This function is run when the robot is first started up and should be used
-   * for any initialization code.
+   * This function is run when the robot is first started up and should be used for any
+   * initialization code.
    */
   @Override
-  public void robotInit() {
-  }
+  public void robotInit() {}
 
   @Override
-  public void robotPeriodic() {
-  }
+  public void robotPeriodic() {}
 
   @Override
-  public void autonomousInit() {
-  }
+  public void autonomousInit() {}
 
   @Override
-  public void autonomousPeriodic() {
-  }
+  public void autonomousPeriodic() {}
 
   @Override
-  public void teleopInit() {
-  }
+  public void teleopInit() {}
 
   @Override
-  public void teleopPeriodic() {
-  }
+  public void teleopPeriodic() {}
 
   @Override
-  public void disabledInit() {
-  }
+  public void disabledInit() {}
 
   @Override
-  public void disabledPeriodic() {
-  }
+  public void disabledPeriodic() {}
 
   @Override
-  public void testInit() {
-  }
+  public void testInit() {}
 
   @Override
-  public void testPeriodic() {
-  }
-
+  public void testPeriodic() {}
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/build.gradle b/third_party/allwpilib/wpilibjIntegrationTests/build.gradle
index 090d309..3224345 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/build.gradle
+++ b/third_party/allwpilib/wpilibjIntegrationTests/build.gradle
@@ -11,7 +11,7 @@
 
 apply from: "${rootDir}/shared/opencv.gradle"
 
-mainClassName = 'edu.wpi.first.wpilibj.test.AntJunitLanucher'
+mainClassName = 'edu.wpi.first.wpilibj.test.AntJunitLauncher'
 
 apply plugin: 'com.github.johnrengelman.shadow'
 
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
index de54234..219a611 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AbstractInterruptTest.java
@@ -1,311 +1,254 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.concurrent.atomic.AtomicBoolean;
-import java.util.concurrent.atomic.AtomicInteger;
-import java.util.concurrent.atomic.AtomicLong;
-
-import org.junit.After;
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
 import static org.hamcrest.Matchers.both;
 import static org.hamcrest.Matchers.greaterThan;
 import static org.hamcrest.Matchers.lessThan;
 import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertSame;
 import static org.junit.Assert.assertThat;
 
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.concurrent.atomic.AtomicBoolean;
+import java.util.concurrent.atomic.AtomicInteger;
+import java.util.concurrent.atomic.AtomicLong;
+import org.junit.After;
+import org.junit.Test;
+
 /**
  * This class should not be run as a test explicitly. Instead it should be extended by tests that
- * use the InterruptableSensorBase.
+ * use DigitalSource.
  */
 public abstract class AbstractInterruptTest extends AbstractComsSetup {
-  private InterruptableSensorBase m_interruptable = null;
+  private DigitalSource m_source = null;
 
-  private InterruptableSensorBase getInterruptable() {
-    if (m_interruptable == null) {
-      m_interruptable = giveInterruptableSensorBase();
+  private DigitalSource getSource() {
+    if (m_source == null) {
+      m_source = giveSource();
     }
-    return m_interruptable;
+    return m_source;
   }
 
-
   @After
   public void interruptTeardown() {
-    if (m_interruptable != null) {
-      freeInterruptableSensorBase();
-      m_interruptable = null;
+    if (m_source != null) {
+      freeSource();
+      m_source = null;
     }
   }
 
-  /**
-   * Give the interruptable sensor base that interrupts can be attached to.
-   */
-  abstract InterruptableSensorBase giveInterruptableSensorBase();
+  /** Give the sensor source that interrupts can be attached to. */
+  abstract DigitalSource giveSource();
 
-  /**
-   * Cleans up the interruptable sensor base. This is only called if {@link
-   * #giveInterruptableSensorBase()} is called.
-   */
-  abstract void freeInterruptableSensorBase();
+  /** Cleans up the sensor source. This is only called if {@link #giveSource()} is called. */
+  abstract void freeSource();
 
-  /**
-   * Perform whatever action is required to set the interrupt high.
-   */
+  /** Perform whatever action is required to set the interrupt high. */
   abstract void setInterruptHigh();
 
-  /**
-   * Perform whatever action is required to set the interrupt low.
-   */
+  /** Perform whatever action is required to set the interrupt low. */
   abstract void setInterruptLow();
 
-
-  private class InterruptCounter {
-    private final AtomicInteger m_count = new AtomicInteger();
-
-    void increment() {
-      m_count.addAndGet(1);
-    }
-
-    int getCount() {
-      return m_count.get();
-    }
-  }
-
-  private class TestInterruptHandlerFunction extends InterruptHandlerFunction<InterruptCounter> {
-    protected final AtomicBoolean m_exceptionThrown = new AtomicBoolean(false);
-    /**
-     * Stores the time that the interrupt fires.
-     */
-    final AtomicLong m_interruptFireTime = new AtomicLong();
-    /**
-     * Stores if the interrupt has completed at least once.
-     */
-    final AtomicBoolean m_interruptComplete = new AtomicBoolean(false);
-    protected Exception m_ex;
-    final InterruptCounter m_counter;
-
-    TestInterruptHandlerFunction(InterruptCounter counter) {
-      m_counter = counter;
-    }
-
-    @Override
-    public void interruptFired(int interruptAssertedMask, InterruptCounter param) {
-      m_interruptFireTime.set(RobotController.getFPGATime());
-      m_counter.increment();
-      try {
-        // This won't cause the test to fail
-        assertSame(m_counter, param);
-      } catch (Exception ex) {
-        // So we must throw the exception within the test
-        m_exceptionThrown.set(true);
-        m_ex = ex;
-      }
-      m_interruptComplete.set(true);
-    }
-
-    @Override
-    public InterruptCounter overridableParameter() {
-      return m_counter;
-    }
-  }
-
   @Test(timeout = 1000)
   public void testSingleInterruptsTriggering() throws Exception {
     // Given
-    final InterruptCounter counter = new InterruptCounter();
-    TestInterruptHandlerFunction function = new TestInterruptHandlerFunction(counter);
+    // final InterruptCounter counter = new InterruptCounter();
+    // TestInterruptHandlerFunction function = new
+    // TestInterruptHandlerFunction(counter);
 
-    // When
-    getInterruptable().requestInterrupts(function);
-    getInterruptable().enableInterrupts();
+    AtomicBoolean hasFired = new AtomicBoolean(false);
+    AtomicInteger counter = new AtomicInteger(0);
+    AtomicLong interruptFireTime = new AtomicLong();
 
-    setInterruptLow();
-    Timer.delay(0.01);
-    // Note: Utility.getFPGATime() is used because double values can turn over
-    // after the robot has been running for a long time
-    final long interruptTriggerTime = RobotController.getFPGATime();
-    setInterruptHigh();
+    try (AsynchronousInterrupt interrupt =
+        new AsynchronousInterrupt(
+            getSource(),
+            (a, b) -> {
+              interruptFireTime.set(RobotController.getFPGATime());
+              hasFired.set(true);
+              counter.incrementAndGet();
+            })) {
+      interrupt.enable();
+      setInterruptLow();
+      Timer.delay(0.01);
+      final long interruptTriggerTime = RobotController.getFPGATime();
+      setInterruptHigh();
+      while (!hasFired.get()) {
+        Timer.delay(0.005);
+      }
 
-    // Delay until the interrupt is complete
-    while (!function.m_interruptComplete.get()) {
-      Timer.delay(0.005);
+      assertEquals("The interrupt did not fire the expected number of times", 1, counter.get());
+
+      final long range = 10000; // in microseconds
+      assertThat(
+          "The interrupt did not fire within the expected time period (values in milliseconds)",
+          interruptFireTime.get(),
+          both(greaterThan(interruptTriggerTime - range))
+              .and(lessThan(interruptTriggerTime + range)));
+      assertThat(
+          "The readRisingTimestamp() did not return the correct value (values in seconds)",
+          interrupt.getRisingTimestamp(),
+          both(greaterThan((interruptTriggerTime - range) / 1e6))
+              .and(lessThan((interruptTriggerTime + range) / 1e6)));
     }
-
-
-    // Then
-    assertEquals("The interrupt did not fire the expected number of times", 1, counter.getCount());
-    // If the test within the interrupt failed
-    if (function.m_exceptionThrown.get()) {
-      throw function.m_ex;
-    }
-    final long range = 10000; // in microseconds
-    assertThat(
-        "The interrupt did not fire within the expected time period (values in milliseconds)",
-        function.m_interruptFireTime.get(),
-        both(greaterThan(interruptTriggerTime - range)).and(lessThan(interruptTriggerTime
-            + range)));
-    assertThat(
-        "The readRisingTimestamp() did not return the correct value (values in seconds)",
-        getInterruptable().readRisingTimestamp(),
-        both(greaterThan((interruptTriggerTime - range) / 1e6)).and(
-            lessThan((interruptTriggerTime + range) / 1e6)));
   }
 
   @Test(timeout = 2000)
   public void testMultipleInterruptsTriggering() {
-    // Given
-    final InterruptCounter counter = new InterruptCounter();
-    TestInterruptHandlerFunction function = new TestInterruptHandlerFunction(counter);
+    AtomicBoolean hasFired = new AtomicBoolean(false);
+    AtomicInteger counter = new AtomicInteger(0);
 
-    // When
-    getInterruptable().requestInterrupts(function);
-    getInterruptable().enableInterrupts();
+    try (AsynchronousInterrupt interrupt =
+        new AsynchronousInterrupt(
+            getSource(),
+            (a, b) -> {
+              hasFired.set(true);
+              counter.incrementAndGet();
+            })) {
+      interrupt.enable();
 
-    final int fireCount = 50;
-    for (int i = 0; i < fireCount; i++) {
-      setInterruptLow();
-      setInterruptHigh();
-      // Wait for the interrupt to complete before moving on
-      while (!function.m_interruptComplete.getAndSet(false)) {
-        Timer.delay(0.005);
+      final int fireCount = 50;
+      for (int i = 0; i < fireCount; i++) {
+        setInterruptLow();
+        setInterruptHigh();
+        // Wait for the interrupt to complete before moving on
+        while (!hasFired.getAndSet(false)) {
+          Timer.delay(0.005);
+        }
       }
+      // Then
+      assertEquals(
+          "The interrupt did not fire the expected number of times", fireCount, counter.get());
     }
-    // Then
-    assertEquals("The interrupt did not fire the expected number of times", fireCount,
-        counter.getCount());
   }
 
-  /**
-   * The timeout length for this test in seconds.
-   */
+  /** The timeout length for this test in seconds. */
   private static final int synchronousTimeout = 5;
 
   @Test(timeout = (long) (synchronousTimeout * 1e3))
   public void testSynchronousInterruptsTriggering() {
-    // Given
-    getInterruptable().requestInterrupts();
+    try (SynchronousInterrupt interrupt = new SynchronousInterrupt(getSource())) {
+      final double synchronousDelay = synchronousTimeout / 2.0;
+      final Runnable runnable =
+          () -> {
+            Timer.delay(synchronousDelay);
+            setInterruptLow();
+            setInterruptHigh();
+          };
 
-    final double synchronousDelay = synchronousTimeout / 2.0;
-    final Runnable runnable = () -> {
-      Timer.delay(synchronousDelay);
-      setInterruptLow();
-      setInterruptHigh();
-    };
+      // When
 
-    // When
+      // Note: the long time value is used because doubles can flip if the robot
+      // is left running for long enough
+      final long startTimeStamp = RobotController.getFPGATime();
+      new Thread(runnable).start();
+      // Delay for twice as long as the timeout so the test should fail first
+      interrupt.waitForInterrupt(synchronousTimeout * 2);
+      final long stopTimeStamp = RobotController.getFPGATime();
 
-    // Note: the long time value is used because doubles can flip if the robot
-    // is left running for long enough
-    final long startTimeStamp = RobotController.getFPGATime();
-    new Thread(runnable).start();
-    // Delay for twice as long as the timeout so the test should fail first
-    getInterruptable().waitForInterrupt(synchronousTimeout * 2);
-    final long stopTimeStamp = RobotController.getFPGATime();
-
-    // Then
-    // The test will not have timed out and:
-    final double interruptRunTime = (stopTimeStamp - startTimeStamp) * 1e-6;
-    assertEquals("The interrupt did not run for the expected amount of time (units in seconds)",
-        synchronousDelay, interruptRunTime, 0.1);
+      // Then
+      // The test will not have timed out and:
+      final double interruptRunTime = (stopTimeStamp - startTimeStamp) * 1e-6;
+      assertEquals(
+          "The interrupt did not run for the expected amount of time (units in seconds)",
+          synchronousDelay,
+          interruptRunTime,
+          0.1);
+    }
   }
 
   @Test(timeout = (long) (synchronousTimeout * 1e3))
   public void testSynchronousInterruptsWaitResultTimeout() {
-    // Given
-    getInterruptable().requestInterrupts();
+    try (SynchronousInterrupt interrupt = new SynchronousInterrupt(getSource())) {
+      SynchronousInterrupt.WaitResult result = interrupt.waitForInterrupt(synchronousTimeout / 2);
 
-    //Don't fire interrupt. Expect it to timeout.
-    InterruptableSensorBase.WaitResult result = getInterruptable()
-        .waitForInterrupt(synchronousTimeout / 2);
-
-    assertEquals("The interrupt did not time out correctly.", result, InterruptableSensorBase
-        .WaitResult.kTimeout);
+      assertEquals(
+          "The interrupt did not time out correctly.",
+          result,
+          SynchronousInterrupt.WaitResult.kTimeout);
+    }
   }
 
   @Test(timeout = (long) (synchronousTimeout * 1e3))
   public void testSynchronousInterruptsWaitResultRisingEdge() {
-    // Given
-    getInterruptable().requestInterrupts();
+    try (SynchronousInterrupt interrupt = new SynchronousInterrupt(getSource())) {
+      final double synchronousDelay = synchronousTimeout / 2.0;
+      final Runnable runnable =
+          () -> {
+            Timer.delay(synchronousDelay);
+            setInterruptLow();
+            setInterruptHigh();
+          };
 
-    final double synchronousDelay = synchronousTimeout / 2.0;
-    final Runnable runnable = () -> {
-      Timer.delay(synchronousDelay);
-      setInterruptLow();
-      setInterruptHigh();
-    };
+      new Thread(runnable).start();
+      // Delay for twice as long as the timeout so the test should fail first
+      SynchronousInterrupt.WaitResult result = interrupt.waitForInterrupt(synchronousTimeout * 2);
 
-    new Thread(runnable).start();
-    // Delay for twice as long as the timeout so the test should fail first
-    InterruptableSensorBase.WaitResult result = getInterruptable()
-        .waitForInterrupt(synchronousTimeout * 2);
-
-    assertEquals("The interrupt did not fire on the rising edge.", result,
-        InterruptableSensorBase.WaitResult.kRisingEdge);
+      assertEquals(
+          "The interrupt did not fire on the rising edge.",
+          result,
+          SynchronousInterrupt.WaitResult.kRisingEdge);
+    }
   }
 
   @Test(timeout = (long) (synchronousTimeout * 1e3))
   public void testSynchronousInterruptsWaitResultFallingEdge() {
-    // Given
-    getInterruptable().requestInterrupts();
-    getInterruptable().setUpSourceEdge(false, true);
+    try (SynchronousInterrupt interrupt = new SynchronousInterrupt(getSource())) {
+      // Given
+      interrupt.setInterruptEdges(false, true);
 
-    final double synchronousDelay = synchronousTimeout / 2.0;
-    final Runnable runnable = () -> {
-      Timer.delay(synchronousDelay);
-      setInterruptHigh();
-      setInterruptLow();
-    };
+      final double synchronousDelay = synchronousTimeout / 2.0;
+      final Runnable runnable =
+          () -> {
+            Timer.delay(synchronousDelay);
+            setInterruptHigh();
+            setInterruptLow();
+          };
 
-    new Thread(runnable).start();
-    // Delay for twice as long as the timeout so the test should fail first
-    InterruptableSensorBase.WaitResult result = getInterruptable()
-        .waitForInterrupt(synchronousTimeout * 2);
+      new Thread(runnable).start();
+      // Delay for twice as long as the timeout so the test should fail first
+      SynchronousInterrupt.WaitResult result = interrupt.waitForInterrupt(synchronousTimeout * 2);
 
-    assertEquals("The interrupt did not fire on the falling edge.", result,
-        InterruptableSensorBase.WaitResult.kFallingEdge);
+      assertEquals(
+          "The interrupt did not fire on the falling edge.",
+          result,
+          SynchronousInterrupt.WaitResult.kFallingEdge);
+    }
   }
 
-
   @Test(timeout = 4000)
   public void testDisableStopsInterruptFiring() {
-    final InterruptCounter counter = new InterruptCounter();
-    TestInterruptHandlerFunction function = new TestInterruptHandlerFunction(counter);
-
-    // When
-    getInterruptable().requestInterrupts(function);
-    getInterruptable().enableInterrupts();
-
-    final int fireCount = 50;
-    for (int i = 0; i < fireCount; i++) {
-      setInterruptLow();
-      setInterruptHigh();
-      // Wait for the interrupt to complete before moving on
-      while (!function.m_interruptComplete.getAndSet(false)) {
+    AtomicBoolean interruptComplete = new AtomicBoolean(false);
+    AtomicInteger counter = new AtomicInteger(0);
+    try (AsynchronousInterrupt interrupt =
+        new AsynchronousInterrupt(
+            getSource(),
+            (a, b) -> {
+              interruptComplete.set(true);
+              counter.incrementAndGet();
+            })) {
+      interrupt.enable();
+      final int fireCount = 50;
+      for (int i = 0; i < fireCount; i++) {
+        setInterruptLow();
+        setInterruptHigh();
+        // Wait for the interrupt to complete before moving on
+        while (!interruptComplete.getAndSet(false)) {
+          Timer.delay(0.005);
+        }
+      }
+      interrupt.disable();
+      for (int i = 0; i < fireCount; i++) {
+        setInterruptLow();
+        setInterruptHigh();
+        // Just wait because the interrupt should not fire
         Timer.delay(0.005);
       }
-    }
-    getInterruptable().disableInterrupts();
-    // TestBench.out().println("Finished disabling the robot");
 
-    for (int i = 0; i < fireCount; i++) {
-      setInterruptLow();
-      setInterruptHigh();
-      // Just wait because the interrupt should not fire
-      Timer.delay(0.005);
+      assertEquals(
+          "The interrupt did not fire the expected number of times", fireCount, counter.get());
     }
-
-    // Then
-    assertEquals("The interrupt did not fire the expected number of times", fireCount,
-        counter.getCount());
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogCrossConnectTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogCrossConnectTest.java
index 14b7034..34c57fb 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogCrossConnectTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogCrossConnectTest.java
@@ -1,30 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.AfterClass;
-import org.junit.Before;
-import org.junit.BeforeClass;
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
-import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
-import edu.wpi.first.wpilibj.test.TestBench;
-
 import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertFalse;
 import static org.junit.Assert.assertTrue;
 
-/**
- * Test that covers the {@link AnalogCrossConnectFixture}.
- */
+import edu.wpi.first.wpilibj.AnalogTriggerOutput.AnalogTriggerType;
+import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
+import edu.wpi.first.wpilibj.test.TestBench;
+import java.util.logging.Logger;
+import org.junit.AfterClass;
+import org.junit.Before;
+import org.junit.BeforeClass;
+import org.junit.Test;
+
+/** Test that covers the {@link AnalogCrossConnectFixture}. */
 public class AnalogCrossConnectTest extends AbstractInterruptTest {
   private static final Logger logger = Logger.getLogger(AnalogCrossConnectTest.class.getName());
 
@@ -37,7 +30,6 @@
     return logger;
   }
 
-
   @BeforeClass
   public static void setUpBeforeClass() {
     analogIO = TestBench.getAnalogCrossConnectFixture();
@@ -54,7 +46,6 @@
     analogIO.setup();
   }
 
-
   @Test
   public void testAnalogOuput() {
     for (int i = 0; i < 50; i++) {
@@ -148,34 +139,31 @@
    * (non-Javadoc)
    *
    * @see
-   * edu.wpi.first.wpilibj.AbstractInterruptTest#giveInterruptableSensorBase()
+   * edu.wpi.first.wpilibj.AbstractInterruptTest#giveSource()
    */
   @Override
-  InterruptableSensorBase giveInterruptableSensorBase() {
+  DigitalSource giveSource() {
     m_interruptTrigger = new AnalogTrigger(analogIO.getInput());
     m_interruptTrigger.setLimitsVoltage(2.0, 3.0);
-    m_interruptTriggerOutput = new AnalogTriggerOutput(m_interruptTrigger,
-        AnalogTriggerType.kState);
+    m_interruptTriggerOutput =
+        new AnalogTriggerOutput(m_interruptTrigger, AnalogTriggerType.kState);
     return m_interruptTriggerOutput;
   }
 
-
   /*
    * (non-Javadoc)
    *
    * @see
-   * edu.wpi.first.wpilibj.AbstractInterruptTest#freeInterruptableSensorBase()
+   * edu.wpi.first.wpilibj.AbstractInterruptTest#freeSource()
    */
   @Override
-  void freeInterruptableSensorBase() {
-    m_interruptTriggerOutput.cancelInterrupts();
+  void freeSource() {
     m_interruptTriggerOutput.close();
     m_interruptTriggerOutput = null;
     m_interruptTrigger.close();
     m_interruptTrigger = null;
   }
 
-
   /*
    * (non-Javadoc)
    *
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
index 29ac6e4..1bac2ca 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/AnalogPotentiometerTest.java
@@ -1,28 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.After;
-import org.junit.Before;
-import org.junit.Test;
+import static org.junit.Assert.assertEquals;
 
 import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
 import edu.wpi.first.wpilibj.mockhardware.FakePotentiometerSource;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
+import java.util.logging.Logger;
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
 
-import static org.junit.Assert.assertEquals;
-
-/**
- * Tests the {@link AnalogPotentiometer}.
- */
+/** Tests the {@link AnalogPotentiometer}. */
 public class AnalogPotentiometerTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(AnalogPotentiometerTest.class.getName());
   private AnalogCrossConnectFixture m_analogIO;
@@ -36,7 +29,6 @@
     m_analogIO = TestBench.getAnalogCrossConnectFixture();
     m_potSource = new FakePotentiometerSource(m_analogIO.getOutput(), 360);
     m_pot = new AnalogPotentiometer(m_analogIO.getInput(), 360.0, 0);
-
   }
 
   @After
@@ -65,6 +57,4 @@
       assertEquals(i, m_pot.get(), DOUBLE_COMPARISON_DELTA);
     }
   }
-
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java
index dbfddfa..8b1188d 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/BuiltInAccelerometerTest.java
@@ -1,27 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertEquals;
+
+import edu.wpi.first.wpilibj.interfaces.Accelerometer;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.BeforeClass;
 import org.junit.Test;
 import org.junit.runner.RunWith;
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.wpilibj.interfaces.Accelerometer;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
-import static org.junit.Assert.assertEquals;
-
 @RunWith(Parameterized.class)
 public class BuiltInAccelerometerTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(BuiltInAccelerometerTest.class.getName());
@@ -41,13 +36,13 @@
     Timer.delay(1.0);
   }
 
-  /**
-   * Test with all valid ranges to make sure unpacking is always done correctly.
-   */
+  /** Test with all valid ranges to make sure unpacking is always done correctly. */
   @Parameters
   public static Collection<Accelerometer.Range[]> generateData() {
-    return Arrays.asList(new Accelerometer.Range[][]{{Accelerometer.Range.k2G},
-        {Accelerometer.Range.k4G}, {Accelerometer.Range.k8G}});
+    return Arrays.asList(
+        new Accelerometer.Range[][] {
+          {Accelerometer.Range.k2G}, {Accelerometer.Range.k4G}, {Accelerometer.Range.k8G}
+        });
   }
 
   @Override
@@ -65,5 +60,4 @@
     assertEquals(1.0, m_accelerometer.getY(), kAccelerationTolerance);
     assertEquals(0.0, m_accelerometer.getZ(), kAccelerationTolerance);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java
index aae1661..63ea22a 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/ConstantsPortsTest.java
@@ -1,23 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
 import static org.junit.Assert.assertEquals;
 
-/**
- * Tests for checking our constant and port values.
- */
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
+
+/** Tests for checking our constant and port values. */
 public class ConstantsPortsTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(ConstantsPortsTest.class.getName());
 
@@ -26,75 +19,33 @@
     return logger;
   }
 
-  /**
-   * kDigitalChannels.
-   */
+  /** kDigitalChannels. */
   @Test
   public void testDigitalChannels() {
     assertEquals(31, SensorUtil.kDigitalChannels);
   }
 
-  /**
-   * kAnalogInputChannels.
-   */
+  /** kAnalogInputChannels. */
   @Test
   public void testAnalogInputChannels() {
     assertEquals(8, SensorUtil.kAnalogInputChannels);
   }
 
-  /**
-   * kAnalogOutputChannels.
-   */
+  /** kAnalogOutputChannels. */
   @Test
   public void testAnalogOutputChannels() {
     assertEquals(2, SensorUtil.kAnalogOutputChannels);
   }
 
-  /**
-   * kSolenoidChannels.
-   */
-  @Test
-  public void testSolenoidChannels() {
-    assertEquals(8, SensorUtil.kSolenoidChannels);
-  }
-
-  /**
-   * kPwmChannels.
-   */
+  /** kPwmChannels. */
   @Test
   public void testPwmChannels() {
     assertEquals(20, SensorUtil.kPwmChannels);
   }
 
-  /**
-   * kRelayChannels.
-   */
+  /** kRelayChannels. */
   @Test
   public void testRelayChannels() {
     assertEquals(4, SensorUtil.kRelayChannels);
   }
-
-  /**
-   * kPDPChannels.
-   */
-  @Test
-  public void testPDPChannels() {
-    assertEquals(16, SensorUtil.kPDPChannels);
-  }
-
-  /**
-   * kPDPModules.
-   */
-  @Test
-  public void testPDPModules() {
-    assertEquals(63, SensorUtil.kPDPModules);
-  }
-
-  /**
-   * kPCMModules.
-   */
-  @Test
-  public void testPCMModules() {
-    assertEquals(63, SensorUtil.kPCMModules);
-  }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java
index f6e3326..793c7e9 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/CounterTest.java
@@ -1,15 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.fixtures.FakeCounterFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.AfterClass;
 import org.junit.Before;
 import org.junit.BeforeClass;
@@ -18,16 +20,7 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.wpilibj.fixtures.FakeCounterFixture;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import edu.wpi.first.wpilibj.test.TestBench;
-
-import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertTrue;
-
-/**
- * Tests to see if the Counter is working properly.
- */
+/** Tests to see if the Counter is working properly. */
 @RunWith(Parameterized.class)
 public class CounterTest extends AbstractComsSetup {
   private static FakeCounterFixture counter = null;
@@ -44,7 +37,7 @@
   /**
    * Constructs a Counter Test with the given inputs.
    *
-   * @param input  The input Port
+   * @param input The input Port
    * @param output The output Port
    */
   public CounterTest(Integer input, Integer output) {
@@ -73,13 +66,11 @@
     // port}.
     // These data are hard-coded into the class, but they could be
     // generated or loaded in any way you like.
-    return TestBench.getInstance().getDIOCrossConnectCollection();
+    return TestBench.getDIOCrossConnectCollection();
   }
 
-
   @BeforeClass
-  public static void setUpBeforeClass() {
-  }
+  public static void setUpBeforeClass() {}
 
   @AfterClass
   public static void tearDownAfterClass() {
@@ -92,9 +83,7 @@
     counter.setup();
   }
 
-  /**
-   * Tests the default state of the counter immediately after reset.
-   */
+  /** Tests the default state of the counter immediately after reset. */
   @Test
   public void testDefault() {
     assertEquals("Counter did not reset to 0", 0, counter.getCounter().get());
@@ -108,8 +97,15 @@
 
     final int count = counter.getCounter().get();
 
-    assertTrue("Fake Counter, Input: " + m_input + ", Output: " + m_output + ", did not return "
-        + goal + " instead got: " + count, count == goal);
+    assertTrue(
+        "Fake Counter, Input: "
+            + m_input
+            + ", Output: "
+            + m_output
+            + ", did not return "
+            + goal
+            + " instead got: "
+            + count,
+        count == goal);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java
index 14eed0c..97405ce 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DIOCrossConnectTest.java
@@ -1,15 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.After;
 import org.junit.AfterClass;
 import org.junit.Test;
@@ -17,15 +18,7 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
-import edu.wpi.first.wpilibj.test.TestBench;
-
-import static org.junit.Assert.assertFalse;
-import static org.junit.Assert.assertTrue;
-
-/**
- * Tests to see if the Digital ports are working properly.
- */
+/** Tests to see if the Digital ports are working properly. */
 @RunWith(Parameterized.class)
 public class DIOCrossConnectTest extends AbstractInterruptTest {
   private static final Logger logger = Logger.getLogger(DIOCrossConnectTest.class.getName());
@@ -39,11 +32,11 @@
 
   /**
    * Default constructor for the DIOCrossConnectTest This test is parameterized in order to allow it
-   * to be tested using a variety of different input/output pairs without duplicate code.<br> This
-   * class takes Integer port values instead of DigitalClasses because it would force them to be
-   * instantiated at the same time which could (untested) cause port binding errors.
+   * to be tested using a variety of different input/output pairs without duplicate code.<br>
+   * This class takes Integer port values instead of DigitalClasses because it would force them to
+   * be instantiated at the same time which could (untested) cause port binding errors.
    *
-   * @param input  The port for the input wire
+   * @param input The port for the input wire
    * @param output The port for the output wire
    */
   public DIOCrossConnectTest(Integer input, Integer output) {
@@ -53,7 +46,6 @@
     dio = new DIOCrossConnectFixture(input, output);
   }
 
-
   /**
    * Test data generator. This method is called the the JUnit parameterized test runner and returns
    * a Collection of Arrays. For each Array in the Collection, each array element corresponds to a
@@ -66,7 +58,7 @@
     // port}.
     // These data are hard-coded into the class, but they could be
     // generated or loaded in any way you like.
-    return TestBench.getInstance().getDIOCrossConnectCollection();
+    return TestBench.getDIOCrossConnectCollection();
   }
 
   @AfterClass
@@ -80,9 +72,7 @@
     dio.reset();
   }
 
-  /**
-   * Tests to see if the DIO can create and recognize a high value.
-   */
+  /** Tests to see if the DIO can create and recognize a high value. */
   @Test
   public void testSetHigh() {
     dio.getOutput().set(true);
@@ -91,9 +81,7 @@
     assertTrue("DIO Not High after .05s delay", dio.getInput().get());
   }
 
-  /**
-   * Tests to see if the DIO can create and recognize a low value.
-   */
+  /** Tests to see if the DIO can create and recognize a low value. */
   @Test
   public void testSetLow() {
     dio.getOutput().set(false);
@@ -102,23 +90,22 @@
     assertFalse("DIO Not Low after .05s delay", dio.getInput().get());
   }
 
-  /**
-   * Tests to see if the DIO PWM functionality works.
-   */
+  /** Tests to see if the DIO PWM functionality works. */
   @Test
   public void testDIOpulseWidthModulation() {
     dio.getOutput().set(false);
     assertFalse("DIO Not Low after no delay", dio.getInput().get());
-    //Set frequency to 2.0 Hz
+    // Set frequency to 2.0 Hz
     dio.getOutput().setPWMRate(2.0);
-    //Enable PWM, but leave it off.
+    // Enable PWM, but leave it off.
     dio.getOutput().enablePWM(0.0);
     Timer.delay(0.5);
     dio.getOutput().updateDutyCycle(0.5);
-    dio.getInput().requestInterrupts();
-    dio.getInput().setUpSourceEdge(false, true);
-    //TODO: Add return value from WaitForInterrupt
-    dio.getInput().waitForInterrupt(3.0, true);
+    try (var interruptHandler = new SynchronousInterrupt(dio.getInput())) {
+      interruptHandler.setInterruptEdges(false, true);
+      // TODO: Add return value from WaitForInterrupt
+      interruptHandler.waitForInterrupt(3.0, true);
+    }
     Timer.delay(0.5);
     final boolean firstCycle = dio.getInput().get();
     Timer.delay(0.5);
@@ -154,10 +141,10 @@
    * (non-Javadoc)
    *
    * @see
-   * edu.wpi.first.wpilibj.AbstractInterruptTest#giveInterruptableSensorBase()
+   * edu.wpi.first.wpilibj.AbstractInterruptTest#giveSource()
    */
   @Override
-  InterruptableSensorBase giveInterruptableSensorBase() {
+  DigitalSource giveSource() {
     return dio.getInput();
   }
 
@@ -165,10 +152,10 @@
    * (non-Javadoc)
    *
    * @see
-   * edu.wpi.first.wpilibj.AbstractInterruptTest#freeInterruptableSensorBase()
+   * edu.wpi.first.wpilibj.AbstractInterruptTest#freeSource()
    */
   @Override
-  void freeInterruptableSensorBase() {
+  void freeSource() {
     // Handled in the fixture
   }
 
@@ -190,7 +177,5 @@
   @Override
   void setInterruptLow() {
     dio.getOutput().set(false);
-
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DMATest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DMATest.java
new file mode 100644
index 0000000..1da5c6f
--- /dev/null
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DMATest.java
@@ -0,0 +1,161 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
+import edu.wpi.first.wpilibj.motorcontrol.Jaguar;
+import edu.wpi.first.wpilibj.motorcontrol.PWMMotorController;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.logging.Logger;
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Ignore;
+import org.junit.Test;
+
+@Ignore
+public class DMATest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(DMATest.class.getName());
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+
+  private AnalogCrossConnectFixture m_analogIO;
+  private DigitalOutput m_manualTrigger;
+  private PWMMotorController m_pwm;
+  private DMA m_dma;
+  private DMASample m_dmaSample;
+
+  @Before
+  public void setUp() {
+    m_analogIO = TestBench.getAnalogCrossConnectFixture();
+    m_manualTrigger = new DigitalOutput(7);
+    m_pwm = new Jaguar(14);
+    m_dma = new DMA();
+    m_dmaSample = new DMASample();
+
+    m_dma.addAnalogInput(m_analogIO.getInput());
+    m_dma.setExternalTrigger(m_manualTrigger, false, true);
+    m_manualTrigger.set(true);
+  }
+
+  @After
+  public void tearDown() {
+    m_dma.close();
+    m_manualTrigger.close();
+    m_analogIO.teardown();
+    m_pwm.close();
+  }
+
+  @Test
+  public void testPausingWorks() {
+    m_dma.start(1024);
+    m_dma.setPause(true);
+    m_manualTrigger.set(false);
+
+    var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5));
+
+    assertEquals(DMASample.DMAReadStatus.kTimeout, timedOut);
+  }
+
+  @Test
+  public void testRemovingTriggersWorks() {
+    m_dma.clearExternalTriggers();
+    m_dma.start(1024);
+    m_manualTrigger.set(false);
+
+    var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5));
+
+    assertEquals(DMASample.DMAReadStatus.kTimeout, timedOut);
+  }
+
+  @Test
+  public void testManualTriggerOnlyHappensOnce() {
+    m_dma.start(1024);
+    m_manualTrigger.set(false);
+    var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5));
+    m_manualTrigger.set(true);
+
+    assertEquals(DMASample.DMAReadStatus.kOk, timedOut);
+    assertEquals(0, m_dmaSample.getRemaining());
+    timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(5));
+    assertEquals(DMASample.DMAReadStatus.kTimeout, timedOut);
+  }
+
+  @Test
+  public void testAnalogIndividualTriggers() {
+    m_dma.start(1024);
+    for (double i = 0; i < 5; i += 0.5) {
+      m_analogIO.getOutput().setVoltage(i);
+      // Need to sleep to ensure value sets
+      Timer.delay(AnalogCrossConnectTest.kDelayTime);
+      m_manualTrigger.set(false);
+      var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
+      m_manualTrigger.set(true);
+
+      assertEquals(DMASample.DMAReadStatus.kOk, timedOut);
+      assertEquals(0, m_dmaSample.getRemaining());
+      assertEquals(
+          m_analogIO.getInput().getVoltage(),
+          m_dmaSample.getAnalogInputVoltage(m_analogIO.getInput()),
+          0.01);
+    }
+  }
+
+  @Test
+  public void testAnalogMultipleTriggers() {
+    m_dma.start(1024);
+    List<Double> values = new ArrayList<>();
+    for (double i = 0; i < 5; i += 0.5) {
+      m_analogIO.getOutput().setVoltage(i);
+      values.add(i);
+      // Need to sleep to ensure value sets
+      Timer.delay(AnalogCrossConnectTest.kDelayTime);
+      m_manualTrigger.set(false);
+      Timer.delay(AnalogCrossConnectTest.kDelayTime);
+      m_manualTrigger.set(true);
+    }
+
+    for (int i = 0; i < values.size(); i++) {
+      var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
+      assertEquals(DMASample.DMAReadStatus.kOk, timedOut);
+      assertEquals(values.size() - i - 1, m_dmaSample.getRemaining());
+      assertEquals(values.get(i), m_dmaSample.getAnalogInputVoltage(m_analogIO.getInput()), 0.01);
+    }
+  }
+
+  @Test
+  public void testTimedTriggers() {
+    m_dma.setTimedTrigger(Units.millisecondsToSeconds(10));
+    m_dma.start(1024);
+    Timer.delay(Units.millisecondsToSeconds(100));
+    m_dma.setPause(true);
+
+    var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
+    assertEquals(DMASample.DMAReadStatus.kOk, timedOut);
+    assertTrue("Received more then 5 samples in 100 ms", m_dmaSample.getRemaining() > 5);
+  }
+
+  @Test
+  public void testPwmTimedTriggers() {
+    m_dma.clearExternalTriggers();
+    m_dma.setPwmEdgeTrigger(m_pwm, true, false);
+    m_dma.start(1024);
+    Timer.delay(Units.millisecondsToSeconds(100));
+    m_dma.setPause(true);
+
+    var timedOut = m_dmaSample.update(m_dma, Units.millisecondsToSeconds(1));
+    assertEquals(DMASample.DMAReadStatus.kOk, timedOut);
+    assertTrue("Received more then 5 samples in 100 ms", m_dmaSample.getRemaining() > 5);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilterTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilterTest.java
index 821f5cc..932195e 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilterTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DigitalGlitchFilterTest.java
@@ -1,26 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
 import static org.junit.Assert.assertEquals;
 
-/**
- * Test for the DigitalGlitchFilter class.
- */
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
+
+/** Test for the DigitalGlitchFilter class. */
 public class DigitalGlitchFilterTest extends AbstractComsSetup {
-  private static final Logger logger = Logger.getLogger(
-      DigitalGlitchFilterTest.class.getName());
+  private static final Logger logger = Logger.getLogger(DigitalGlitchFilterTest.class.getName());
 
   @Override
   protected Logger getClassLogger() {
@@ -29,9 +21,9 @@
 
   /**
    * This is a test to make sure that filters can be created and are consistent. This assumes that
-   * the FPGA implementation to actually implement the filter has been tested.  It does validate
-   * that we are successfully able to set and get the active filters for ports and makes sure that
-   * the FPGA filter is changed correctly, and does the same for the period.
+   * the FPGA implementation to actually implement the filter has been tested. It does validate that
+   * we are successfully able to set and get the active filters for ports and makes sure that the
+   * FPGA filter is changed correctly, and does the same for the period.
    */
   @Test
   public void testDigitalGlitchFilterBasic() {
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java
index 301a7ee..b2e727d 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/DriverStationTest.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
 import static org.junit.Assert.assertEquals;
 
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
 
 public class DriverStationTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(TimerTest.class.getName());
@@ -32,13 +26,15 @@
 
     // Wait for data 50 times
     for (int i = 0; i < 50; i++) {
-      DriverStation.getInstance().waitForData();
+      DriverStation.waitForData();
     }
     long endTime = RobotController.getFPGATime();
     long difference = endTime - startTime;
 
-    assertEquals("DriverStation waitForData did not wait long enough", TIMER_RUNTIME, difference,
+    assertEquals(
+        "DriverStation waitForData did not wait long enough",
+        TIMER_RUNTIME,
+        difference,
         TIMER_TOLERANCE * TIMER_RUNTIME);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/EncoderTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/EncoderTest.java
index 3a639ab..f1bfca5 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/EncoderTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/EncoderTest.java
@@ -1,15 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.fixtures.FakeEncoderFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.After;
 import org.junit.AfterClass;
 import org.junit.Before;
@@ -18,16 +19,7 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.wpilibj.fixtures.FakeEncoderFixture;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import edu.wpi.first.wpilibj.test.TestBench;
-
-import static org.junit.Assert.assertTrue;
-
-
-/**
- * Test to see if the FPGA properly recognizes a mock Encoder input.
- */
+/** Test to see if the FPGA properly recognizes a mock Encoder input. */
 @RunWith(Parameterized.class)
 public class EncoderTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(EncoderTest.class.getName());
@@ -45,23 +37,23 @@
   }
 
   /**
-   * Test data generator. This method is called the the JUnit parametrized test runner and returns
-   * a Collection of Arrays. For each Array in the Collection, each array element corresponds to a
+   * Test data generator. This method is called the the JUnit parametrized test runner and returns a
+   * Collection of Arrays. For each Array in the Collection, each array element corresponds to a
    * parameter in the constructor.
    */
   @Parameters
   public static Collection<Integer[]> generateData() {
-    return TestBench.getInstance().getEncoderDIOCrossConnectCollection();
+    return TestBench.getEncoderDIOCrossConnectCollection();
   }
 
   /**
    * Constructs a parametrized Encoder Test.
    *
-   * @param inputA  The port number for inputA
+   * @param inputA The port number for inputA
    * @param outputA The port number for outputA
-   * @param inputB  The port number for inputB
+   * @param inputB The port number for inputB
    * @param outputB The port number for outputB
-   * @param flip    whether or not these set of values require the encoder to be reversed (0 or 1)
+   * @param flip whether or not these set of values require the encoder to be reversed (0 or 1)
    */
   public EncoderTest(int inputA, int outputA, int inputB, int outputB, int flip) {
     m_inputA = inputA;
@@ -84,9 +76,7 @@
     encoder = null;
   }
 
-  /**
-   * Sets up the test and verifies that the test was reset to the default state.
-   */
+  /** Sets up the test and verifies that the test was reset to the default state. */
   @Before
   public void setUp() {
     encoder.setup();
@@ -98,18 +88,14 @@
     encoder.reset();
   }
 
-  /**
-   * Tests to see if Encoders initialize to zero.
-   */
+  /** Tests to see if Encoders initialize to zero. */
   @Test
   public void testDefaultState() {
     int value = encoder.getEncoder().get();
     assertTrue(errorMessage(0, value), value == 0);
   }
 
-  /**
-   * Tests to see if Encoders can count up successfully.
-   */
+  /** Tests to see if Encoders can count up successfully. */
   @Test
   public void testCountUp() {
     int goal = 100;
@@ -118,12 +104,9 @@
     encoder.getFakeEncoderSource().execute();
     int value = encoder.getEncoder().get();
     assertTrue(errorMessage(goal, value), value == goal);
-
   }
 
-  /**
-   * Tests to see if Encoders can count down successfully.
-   */
+  /** Tests to see if Encoders can count down successfully. */
   @Test
   public void testCountDown() {
     int goal = -100;
@@ -132,18 +115,27 @@
     encoder.getFakeEncoderSource().execute();
     int value = encoder.getEncoder().get();
     assertTrue(errorMessage(goal, value), value == goal);
-
   }
 
   /**
    * Creates a simple message with the error that was encountered for the Encoders.
    *
-   * @param goal      The goal that was trying to be reached
+   * @param goal The goal that was trying to be reached
    * @param trueValue The actual value that was reached by the test
    * @return A fully constructed message with data about where and why the the test failed.
    */
   private String errorMessage(int goal, int trueValue) {
-    return "Encoder ({In,Out}): {" + m_inputA + ", " + m_outputA + "},{" + m_inputB + ", "
-        + m_outputB + "} Returned: " + trueValue + ", Wanted: " + goal;
+    return "Encoder ({In,Out}): {"
+        + m_inputA
+        + ", "
+        + m_outputA
+        + "},{"
+        + m_inputB
+        + ", "
+        + m_outputB
+        + "} Returned: "
+        + trueValue
+        + ", Wanted: "
+        + goal;
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java
index 1fc828c..0182a00 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java
@@ -1,27 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.After;
-import org.junit.Before;
-import org.junit.Test;
+import static org.junit.Assert.assertEquals;
 
 import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
+import java.util.logging.Logger;
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
 
-import static org.junit.Assert.assertEquals;
-
-/**
- * Tests that the {@link TiltPanCameraFixture}.
- */
+/** Tests that the {@link TiltPanCameraFixture}. */
 public class GyroTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(GyroTest.class.getName());
 
@@ -37,7 +30,7 @@
   @Before
   public void setUp() {
     logger.fine("Setup: TiltPan camera");
-    m_tpcam = TestBench.getInstance().getTiltPanCam();
+    m_tpcam = TestBench.getTiltPanCam();
     m_tpcam.setup();
   }
 
@@ -92,7 +85,6 @@
     assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10);
   }
 
-
   protected void testDeviationOverTime(AnalogGyro gyro) {
     // Make sure that the test isn't influenced by any previous motions.
     Timer.delay(0.5);
@@ -125,5 +117,4 @@
   private String errorMessage(double difference, double target) {
     return "Gyro angle skewed " + difference + " deg away from target " + target;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockDS.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockDS.java
index 0998ee9..dfd5b6c 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockDS.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockDS.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
@@ -25,50 +22,52 @@
     data[5] = 0x00; // red 1 station
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public void start() {
-    m_thread = new Thread(() -> {
-      DatagramSocket socket;
-      try {
-        socket = new DatagramSocket();
-      } catch (SocketException e1) {
-        // TODO Auto-generated catch block
-        e1.printStackTrace();
-        return;
-      }
-      InetSocketAddress addr = new InetSocketAddress("127.0.0.1", 1110);
-      byte[] sendData = new byte[6];
-      DatagramPacket packet = new DatagramPacket(sendData, 0, 6, addr);
-      short sendCount = 0;
-      int initCount = 0;
-      while (!Thread.currentThread().isInterrupted()) {
-        try {
-          Thread.sleep(20);
-          generateEnabledDsPacket(sendData, sendCount++);
-          // ~50 disabled packets are required to make the robot actually enable
-          // 1 is definitely not enough.
-          if (initCount < 50) {
-            initCount++;
-            sendData[3] = 0;
-          }
-          packet.setData(sendData);
-          socket.send(packet);
-        } catch (InterruptedException ex) {
-          Thread.currentThread().interrupt();
-        } catch (IOException ex) {
-          // TODO Auto-generated catch block
-          ex.printStackTrace();
-        }
-      }
-      socket.close();
-    });
+    m_thread =
+        new Thread(
+            () -> {
+              DatagramSocket socket;
+              try {
+                socket = new DatagramSocket();
+              } catch (SocketException e1) {
+                // TODO Auto-generated catch block
+                e1.printStackTrace();
+                return;
+              }
+              InetSocketAddress addr = new InetSocketAddress("127.0.0.1", 1110);
+              byte[] sendData = new byte[6];
+              DatagramPacket packet = new DatagramPacket(sendData, 0, 6, addr);
+              short sendCount = 0;
+              int initCount = 0;
+              while (!Thread.currentThread().isInterrupted()) {
+                try {
+                  Thread.sleep(20);
+                  generateEnabledDsPacket(sendData, sendCount++);
+                  // ~50 disabled packets are required to make the robot actually enable
+                  // 1 is definitely not enough.
+                  if (initCount < 50) {
+                    initCount++;
+                    sendData[3] = 0;
+                  }
+                  packet.setData(sendData);
+                  socket.send(packet);
+                } catch (InterruptedException ex) {
+                  Thread.currentThread().interrupt();
+                } catch (IOException ex) {
+                  // TODO Auto-generated catch block
+                  ex.printStackTrace();
+                }
+              }
+              socket.close();
+            });
     // Because of the test setup in Java, this thread will not be stopped
     // So it must be a daemon thread
     m_thread.setDaemon(true);
     m_thread.start();
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public void stop() {
     if (m_thread == null) {
       return;
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockSpeedController.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockSpeedController.java
deleted file mode 100644
index d183723..0000000
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MockSpeedController.java
+++ /dev/null
@@ -1,48 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-public class MockSpeedController implements SpeedController {
-  private double m_speed;
-  private boolean m_isInverted;
-
-  @Override
-  public void set(double speed) {
-    m_speed = m_isInverted ? -speed : speed;
-  }
-
-  @Override
-  public double get() {
-    return m_speed;
-  }
-
-  @Override
-  public void setInverted(boolean isInverted) {
-    m_isInverted = isInverted;
-  }
-
-  @Override
-  public boolean getInverted() {
-    return m_isInverted;
-  }
-
-  @Override
-  public void disable() {
-    m_speed = 0;
-  }
-
-  @Override
-  public void stopMotor() {
-    disable();
-  }
-
-  @Override
-  public void pidWrite(double output) {
-    set(output);
-  }
-}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
index e6e48a6..72c2c55 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorEncoderTest.java
@@ -1,16 +1,22 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.filter.LinearFilter;
+import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.After;
 import org.junit.AfterClass;
 import org.junit.Before;
@@ -19,17 +25,6 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import edu.wpi.first.wpilibj.test.TestBench;
-import edu.wpi.first.wpiutil.math.MathUtil;
-
-import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertFalse;
-import static org.junit.Assert.assertTrue;
-
-
 @RunWith(Parameterized.class)
 public class MotorEncoderTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(MotorEncoderTest.class.getName());
@@ -61,8 +56,10 @@
   @Parameters(name = "{index}: {0}")
   public static Collection<MotorEncoderFixture<?>[]> generateData() {
     // logger.fine("Loading the MotorList");
-    return Arrays.asList(new MotorEncoderFixture<?>[][]{{TestBench.getInstance().getTalonPair()},
-        {TestBench.getInstance().getVictorPair()}, {TestBench.getInstance().getJaguarPair()}});
+    return Arrays.asList(
+        new MotorEncoderFixture<?>[][] {
+          {TestBench.getTalonPair()}, {TestBench.getVictorPair()}, {TestBench.getJaguarPair()}
+        });
   }
 
   @Before
@@ -72,7 +69,6 @@
         me.getType() + " Did not start with an initial speed of 0 instead got: " + initialSpeed,
         Math.abs(initialSpeed) < 0.001);
     me.setup();
-
   }
 
   @After
@@ -112,9 +108,13 @@
     me.getMotor().set(0.2);
     Timer.delay(MOTOR_RUNTIME);
     int currentValue = me.getEncoder().get();
-    assertTrue(me.getType() + " Encoder not incremented: start: " + startValue + "; current: "
-        + currentValue, startValue < currentValue);
-
+    assertTrue(
+        me.getType()
+            + " Encoder not incremented: start: "
+            + startValue
+            + "; current: "
+            + currentValue,
+        startValue < currentValue);
   }
 
   /**
@@ -128,13 +128,16 @@
     me.getMotor().set(-0.2);
     Timer.delay(MOTOR_RUNTIME);
     int currentValue = me.getEncoder().get();
-    assertTrue(me.getType() + " Encoder not decremented: start: " + startValue + "; current: "
-        + currentValue, startValue > currentValue);
+    assertTrue(
+        me.getType()
+            + " Encoder not decremented: start: "
+            + startValue
+            + "; current: "
+            + currentValue,
+        startValue > currentValue);
   }
 
-  /**
-   * This method test if the counters count when the motors rotate.
-   */
+  /** This method test if the counters count when the motors rotate. */
   @Test
   public void testCounter() {
     final int counter1Start = me.getCounters()[0].get();
@@ -144,10 +147,20 @@
     Timer.delay(MOTOR_RUNTIME);
     int counter1End = me.getCounters()[0].get();
     int counter2End = me.getCounters()[1].get();
-    assertTrue(me.getType() + " Counter not incremented: start: " + counter1Start + "; current: "
-        + counter1End, counter1Start < counter1End);
-    assertTrue(me.getType() + " Counter not incremented: start: " + counter1Start + "; current: "
-        + counter2End, counter2Start < counter2End);
+    assertTrue(
+        me.getType()
+            + " Counter not incremented: start: "
+            + counter1Start
+            + "; current: "
+            + counter1End,
+        counter1Start < counter1End);
+    assertTrue(
+        me.getType()
+            + " Counter not incremented: start: "
+            + counter1Start
+            + "; current: "
+            + counter2End,
+        counter2Start < counter2End);
     me.reset();
     encodersResetCheck(me);
   }
@@ -159,7 +172,8 @@
   @Test
   public void testSetHighForwardSpeed() {
     me.getMotor().set(15);
-    assertTrue(me.getType() + " Motor speed was not close to 1.0, was: " + me.getMotor().get(),
+    assertTrue(
+        me.getType() + " Motor speed was not close to 1.0, was: " + me.getMotor().get(),
         me.isMotorSpeedWithinRange(1.0, 0.001));
   }
 
@@ -170,11 +184,11 @@
   @Test
   public void testSetHighReverseSpeed() {
     me.getMotor().set(-15);
-    assertTrue(me.getType() + " Motor speed was not close to 1.0, was: " + me.getMotor().get(),
+    assertTrue(
+        me.getType() + " Motor speed was not close to 1.0, was: " + me.getMotor().get(),
         me.isMotorSpeedWithinRange(-1.0, 0.001));
   }
 
-
   @Test
   public void testPositionPIDController() {
     try (PIDController pidController = new PIDController(0.001, 0.0005, 0)) {
@@ -182,17 +196,20 @@
       pidController.setIntegratorRange(-0.2, 0.2);
       pidController.setSetpoint(1000);
 
-      try (Notifier pidRunner = new Notifier(() -> {
-        var speed = pidController.calculate(me.getEncoder().getDistance());
-        me.getMotor().set(MathUtil.clamp(speed, -0.2, 0.2));
-      })) {
+      try (Notifier pidRunner =
+          new Notifier(
+              () -> {
+                var speed = pidController.calculate(me.getEncoder().getDistance());
+                me.getMotor().set(MathUtil.clamp(speed, -0.2, 0.2));
+              })) {
         pidRunner.startPeriodic(pidController.getPeriod());
         Timer.delay(10.0);
         pidRunner.stop();
 
         assertTrue(
             "PID loop did not reach reference within 10 seconds. The current error was"
-            + pidController.getPositionError(), pidController.atSetpoint());
+                + pidController.getPositionError(),
+            pidController.atSetpoint());
       }
     }
   }
@@ -204,16 +221,20 @@
       pidController.setTolerance(200);
       pidController.setSetpoint(600);
 
-      try (Notifier pidRunner = new Notifier(() -> {
-        var speed = filter.calculate(me.getEncoder().getRate());
-        me.getMotor().set(MathUtil.clamp(speed, -0.3, 0.3));
-      })) {
+      try (Notifier pidRunner =
+          new Notifier(
+              () -> {
+                var speed = filter.calculate(me.getEncoder().getRate());
+                me.getMotor().set(MathUtil.clamp(speed, -0.3, 0.3));
+              })) {
         pidRunner.startPeriodic(pidController.getPeriod());
         Timer.delay(10.0);
         pidRunner.stop();
 
-        assertTrue("PID loop did not reach reference within 10 seconds. The error was: "
-            + pidController.getPositionError(), pidController.atSetpoint());
+        assertTrue(
+            "PID loop did not reach reference within 10 seconds. The error was: "
+                + pidController.getPositionError(),
+            pidController.atSetpoint());
       }
     }
   }
@@ -224,18 +245,18 @@
    * @param me The MotorEncoderFixture under test
    */
   private void encodersResetCheck(MotorEncoderFixture<?> me) {
-    assertEquals(me.getType() + " Encoder value was incorrect after reset.", me.getEncoder().get(),
-        0);
-    assertEquals(me.getType() + " Motor value was incorrect after reset.", me.getMotor().get(), 0,
-        0);
-    assertEquals(me.getType() + " Counter1 value was incorrect after reset.",
-        me.getCounters()[0].get(), 0);
-    assertEquals(me.getType() + " Counter2 value was incorrect after reset.",
-        me.getCounters()[1].get(), 0);
+    assertEquals(
+        me.getType() + " Encoder value was incorrect after reset.", me.getEncoder().get(), 0);
+    assertEquals(
+        me.getType() + " Motor value was incorrect after reset.", me.getMotor().get(), 0, 0);
+    assertEquals(
+        me.getType() + " Counter1 value was incorrect after reset.", me.getCounters()[0].get(), 0);
+    assertEquals(
+        me.getType() + " Counter2 value was incorrect after reset.", me.getCounters()[1].get(), 0);
     Timer.delay(0.5); // so this doesn't fail with the 0.5 second default
     // timeout on the encoders
-    assertTrue(me.getType() + " Encoder.getStopped() returned false after the motor was reset.",
+    assertTrue(
+        me.getType() + " Encoder.getStopped() returned false after the motor was reset.",
         me.getEncoder().getStopped());
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java
index 5857681..769fa7f 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/MotorInvertingTest.java
@@ -1,16 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.AfterClass;
 import org.junit.Before;
 import org.junit.Test;
@@ -18,23 +20,13 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import edu.wpi.first.wpilibj.test.TestBench;
-
-import static org.junit.Assert.assertFalse;
-import static org.junit.Assert.assertTrue;
-
-/**
- * Tests Inversion of motors using the SpeedController setInverted.
- */
+/** Tests Inversion of motors using the MotorController setInverted. */
 @RunWith(Parameterized.class)
 public class MotorInvertingTest extends AbstractComsSetup {
   static MotorEncoderFixture<?> fixture = null;
   private static final double motorspeed = 0.2;
   private static final double delaytime = 0.3;
 
-
   /**
    * Constructs the test.
    *
@@ -52,8 +44,10 @@
   @Parameters(name = "{index}: {0}")
   public static Collection<MotorEncoderFixture<?>[]> generateData() {
     // logger.fine("Loading the MotorList");
-    return Arrays.asList(new MotorEncoderFixture<?>[][]{{TestBench.getInstance().getTalonPair()},
-        {TestBench.getInstance().getVictorPair()}, {TestBench.getInstance().getJaguarPair()}});
+    return Arrays.asList(
+        new MotorEncoderFixture<?>[][] {
+          {TestBench.getTalonPair()}, {TestBench.getVictorPair()}, {TestBench.getJaguarPair()}
+        });
   }
 
   private static final Logger logger = Logger.getLogger(MotorInvertingTest.class.getName());
@@ -85,8 +79,9 @@
     fixture.getMotor().setInverted(true);
     fixture.getMotor().set(motorspeed);
     Timer.delay(delaytime);
-    assertFalse("Inverting with Positive value does not change direction", fixture.getEncoder()
-        .getDirection() == initDirection);
+    assertFalse(
+        "Inverting with Positive value does not change direction",
+        fixture.getEncoder().getDirection() == initDirection);
     fixture.getMotor().set(0);
   }
 
@@ -99,8 +94,9 @@
     fixture.getMotor().setInverted(true);
     fixture.getMotor().set(-motorspeed);
     Timer.delay(delaytime);
-    assertFalse("Inverting with Negative value does not change direction", fixture.getEncoder()
-        .getDirection() == initDirection);
+    assertFalse(
+        "Inverting with Negative value does not change direction",
+        fixture.getEncoder().getDirection() == initDirection);
     fixture.getMotor().set(0);
   }
 
@@ -113,8 +109,9 @@
     fixture.getMotor().setInverted(true);
     fixture.getMotor().set(-motorspeed);
     Timer.delay(delaytime);
-    assertTrue("Inverting with Switching value does change direction", fixture.getEncoder()
-        .getDirection() == initDirection);
+    assertTrue(
+        "Inverting with Switching value does change direction",
+        fixture.getEncoder().getDirection() == initDirection);
     fixture.getMotor().set(0);
   }
 
@@ -127,8 +124,9 @@
     fixture.getMotor().setInverted(true);
     fixture.getMotor().set(motorspeed);
     Timer.delay(delaytime);
-    assertTrue("Inverting with Switching value does change direction", fixture.getEncoder()
-        .getDirection() == initDirection);
+    assertTrue(
+        "Inverting with Switching value does change direction",
+        fixture.getEncoder().getDirection() == initDirection);
     fixture.getMotor().set(0);
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/NotifierTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/NotifierTest.java
new file mode 100644
index 0000000..2f2ca74
--- /dev/null
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/NotifierTest.java
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertEquals;
+
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
+
+/** Tests to see if the Notifier is working properly. */
+public class NotifierTest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(NotifierTest.class.getName());
+  private static int counter = 0;
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+
+  @Test
+  public void testStartPeriodicAndStop() {
+    counter = 0;
+    Notifier notifier = new Notifier(() -> ++counter);
+    notifier.startPeriodic(1.0);
+
+    Timer.delay(10.5);
+
+    notifier.stop();
+    assertEquals("Received " + counter + " notifications in 10.5 seconds\n", 10, counter);
+    System.out.println("Received " + counter + " notifications in 10.5 seconds");
+
+    Timer.delay(3.0);
+
+    assertEquals("Received " + (counter - 10) + " notifications in 3 seconds\n", 10, counter);
+    System.out.println("Received " + (counter - 10) + " notifications in 3 seconds");
+
+    notifier.close();
+  }
+
+  @Test
+  public void testStartSingle() {
+    counter = 0;
+    Notifier notifier = new Notifier(() -> ++counter);
+    notifier.startSingle(1.0);
+
+    Timer.delay(10.5);
+
+    assertEquals("Received " + counter + " notifications in 10.5 seconds\n", 1, counter);
+    System.out.println("Received " + counter + " notifications in 10.5 seconds");
+
+    notifier.close();
+  }
+}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java
index 2c1eac1..9c44983 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PCMTest.java
@@ -1,29 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.AfterClass;
-import org.junit.Before;
-import org.junit.BeforeClass;
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
 import static org.junit.Assert.assertEquals;
 import static org.junit.Assert.assertFalse;
 import static org.junit.Assert.assertTrue;
 
-/**
- * Test that covers the {@link Compressor}.
- */
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.AfterClass;
+import org.junit.Before;
+import org.junit.BeforeClass;
+import org.junit.Test;
 
+/** Test that covers the {@link Compressor}. */
 public class PCMTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(PCMTest.class.getName());
   /*
@@ -42,7 +34,7 @@
   protected static final double kCompressorOnVoltage = 5.00;
   protected static final double kCompressorOffVoltage = 1.68;
 
-  private static Compressor compressor;
+  private static PneumaticsControlModule pcm;
 
   private static DigitalOutput fakePressureSwitch;
   private static AnalogInput fakeCompressor;
@@ -52,7 +44,7 @@
 
   @BeforeClass
   public static void setUpBeforeClass() {
-    compressor = new Compressor();
+    pcm = new PneumaticsControlModule();
 
     fakePressureSwitch = new DigitalOutput(11);
     fakeCompressor = new AnalogInput(1);
@@ -72,41 +64,43 @@
 
   @Before
   public void reset() {
-    compressor.stop();
+    pcm.setClosedLoopControl(false);
     fakePressureSwitch.set(false);
   }
 
-  /**
-   * Test if the compressor turns on and off when the pressure switch is toggled.
-   */
+  /** Test if the compressor turns on and off when the pressure switch is toggled. */
   @Test
   public void testPressureSwitch() throws Exception {
     final double range = 0.5;
     reset();
-    compressor.setClosedLoopControl(true);
+    pcm.setClosedLoopControl(true);
 
     // Turn on the compressor
     fakePressureSwitch.set(true);
     Timer.delay(kCompressorDelayTime);
-    assertEquals("Compressor did not turn on when the pressure switch turned on.",
-        kCompressorOnVoltage, fakeCompressor.getVoltage(), range);
+    assertEquals(
+        "Compressor did not turn on when the pressure switch turned on.",
+        kCompressorOnVoltage,
+        fakeCompressor.getVoltage(),
+        range);
 
     // Turn off the compressor
     fakePressureSwitch.set(false);
     Timer.delay(kCompressorDelayTime);
-    assertEquals("Compressor did not turn off when the pressure switch turned off.",
-        kCompressorOffVoltage, fakeCompressor.getVoltage(), range);
+    assertEquals(
+        "Compressor did not turn off when the pressure switch turned off.",
+        kCompressorOffVoltage,
+        fakeCompressor.getVoltage(),
+        range);
   }
 
-  /**
-   * Test if the correct solenoids turn on and off when they should.
-   */
+  /** Test if the correct solenoids turn on and off when they should. */
   @Test
   public void testSolenoid() throws Exception {
     reset();
 
-    Solenoid solenoid1 = new Solenoid(0);
-    Solenoid solenoid2 = new Solenoid(1);
+    Solenoid solenoid1 = new Solenoid(PneumaticsModuleType.CTREPCM, 0);
+    Solenoid solenoid2 = new Solenoid(PneumaticsModuleType.CTREPCM, 1);
 
     solenoid1.set(false);
     solenoid2.set(false);
@@ -153,7 +147,7 @@
    */
   @Test
   public void doubleSolenoid() {
-    DoubleSolenoid solenoid = new DoubleSolenoid(0, 1);
+    DoubleSolenoid solenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, 0, 1);
 
     solenoid.set(DoubleSolenoid.Value.kOff);
     Timer.delay(kSolenoidDelayTime);
@@ -165,28 +159,26 @@
     Timer.delay(kSolenoidDelayTime);
     assertFalse("Solenoid #1 did not turn on", fakeSolenoid1.get());
     assertTrue("Solenoid #2 did not turn off", fakeSolenoid2.get());
-    assertTrue("DoubleSolenoid did not report Forward", solenoid.get() == DoubleSolenoid.Value
-        .kForward);
+    assertTrue(
+        "DoubleSolenoid did not report Forward", solenoid.get() == DoubleSolenoid.Value.kForward);
 
     solenoid.set(DoubleSolenoid.Value.kReverse);
     Timer.delay(kSolenoidDelayTime);
     assertTrue("Solenoid #1 did not turn off", fakeSolenoid1.get());
     assertFalse("Solenoid #2 did not turn on", fakeSolenoid2.get());
-    assertTrue("DoubleSolenoid did not report Reverse", solenoid.get() == DoubleSolenoid.Value
-        .kReverse);
+    assertTrue(
+        "DoubleSolenoid did not report Reverse", solenoid.get() == DoubleSolenoid.Value.kReverse);
 
     solenoid.close();
   }
 
-  /**
-   * Test if the correct solenoids turn on and off when they should.
-   */
+  /** Test if the correct solenoids turn on and off when they should. */
   @Test
   public void testOneShot() throws Exception {
     reset();
 
-    Solenoid solenoid1 = new Solenoid(0);
-    Solenoid solenoid2 = new Solenoid(1);
+    Solenoid solenoid1 = new Solenoid(PneumaticsModuleType.CTREPCM, 0);
+    Solenoid solenoid2 = new Solenoid(PneumaticsModuleType.CTREPCM, 1);
 
     solenoid1.set(false);
     solenoid2.set(false);
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
index b781060..548c2e0 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PDPTest.java
@@ -1,16 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.hamcrest.Matchers.greaterThan;
+import static org.hamcrest.Matchers.is;
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertThat;
+
+import edu.wpi.first.hal.can.CANMessageNotFoundException;
+import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.After;
 import org.junit.AfterClass;
 import org.junit.BeforeClass;
@@ -19,30 +24,18 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.hal.can.CANMessageNotFoundException;
-import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import edu.wpi.first.wpilibj.test.TestBench;
-
-import static org.hamcrest.Matchers.greaterThan;
-import static org.hamcrest.Matchers.is;
-import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertThat;
-
-/**
- * Test that covers the {@link PowerDistributionPanel}.
- */
+/** Test that covers the {@link PowerDistribution}. */
 @RunWith(Parameterized.class)
 public class PDPTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(PDPTest.class.getName());
 
-  private static PowerDistributionPanel pdp;
+  private static PowerDistribution pdp;
   private static MotorEncoderFixture<?> me;
   private final double m_expectedStoppedCurrentDraw;
 
   @BeforeClass
   public static void setUpBeforeClass() {
-    pdp = new PowerDistributionPanel();
+    pdp = new PowerDistribution();
   }
 
   @AfterClass
@@ -52,8 +45,7 @@
     me = null;
   }
 
-
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public PDPTest(MotorEncoderFixture<?> mef, Double expectedCurrentDraw) {
     logger.fine("Constructor with: " + mef.getType());
     if (me != null && !me.equals(mef)) {
@@ -68,8 +60,7 @@
   @Parameters(name = "{index}: {0}, Expected Stopped Current Draw: {1}")
   public static Collection<Object[]> generateData() {
     // logger.fine("Loading the MotorList");
-    return Arrays.asList(new Object[][]{
-        {TestBench.getInstance().getTalonPair(), 0.0}});
+    return Arrays.asList(new Object[][] {{TestBench.getTalonPair(), 0.0}});
   }
 
   @After
@@ -77,31 +68,31 @@
     me.reset();
   }
 
-
-  /**
-   * Test if the current changes when the motor is driven using a talon.
-   */
+  /** Test if the current changes when the motor is driven using a talon. */
   @Test
-  public void checkStoppedCurrentForSpeedController() throws CANMessageNotFoundException {
+  public void checkStoppedCurrentForMotorController() throws CANMessageNotFoundException {
     Timer.delay(0.25);
 
     /* The Current should be 0 */
-    assertEquals("The low current was not within the expected range.", m_expectedStoppedCurrentDraw,
-        pdp.getCurrent(me.getPDPChannel()), 0.001);
+    assertEquals(
+        "The low current was not within the expected range.",
+        m_expectedStoppedCurrentDraw,
+        pdp.getCurrent(me.getPDPChannel()),
+        0.001);
   }
 
-  /**
-   * Test if the current changes when the motor is driven using a talon.
-   */
+  /** Test if the current changes when the motor is driven using a talon. */
   @Test
-  public void checkRunningCurrentForSpeedController() throws CANMessageNotFoundException {
+  public void checkRunningCurrentForMotorController() throws CANMessageNotFoundException {
     /* Set the motor to full forward */
     me.getMotor().set(1.0);
     Timer.delay(2);
 
     /* The current should now be greater than the low current */
-    assertThat("The driven current is not greater than the resting current.",
-        pdp.getCurrent(me.getPDPChannel()), is(greaterThan(m_expectedStoppedCurrentDraw)));
+    assertThat(
+        "The driven current is not greater than the resting current.",
+        pdp.getCurrent(me.getPDPChannel()),
+        is(greaterThan(m_expectedStoppedCurrentDraw)));
   }
 
   @Override
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
index 601fff5..c8afb4b 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PIDTest.java
@@ -1,17 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
+import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import edu.wpi.first.wpilibj.test.TestBench;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.logging.Logger;
-
 import org.junit.After;
 import org.junit.AfterClass;
 import org.junit.Before;
@@ -21,23 +28,7 @@
 import org.junit.runners.Parameterized;
 import org.junit.runners.Parameterized.Parameters;
 
-import edu.wpi.first.networktables.NetworkTable;
-import edu.wpi.first.networktables.NetworkTableInstance;
-import edu.wpi.first.wpilibj.controller.PIDController;
-import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
-import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-import edu.wpi.first.wpilibj.test.TestBench;
-
-import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertFalse;
-import static org.junit.Assert.assertTrue;
-
-
-/**
- * Test that covers the {@link PIDController}.
- */
-
+/** Test that covers the {@link PIDController}. */
 @RunWith(Parameterized.class)
 public class PIDTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(PIDTest.class.getName());
@@ -58,8 +49,7 @@
     return logger;
   }
 
-
-  @SuppressWarnings({"ParameterName", "JavadocMethod"})
+  @SuppressWarnings({"ParameterName", "MissingJavadocMethod"})
   public PIDTest(Double p, Double i, Double d, MotorEncoderFixture<?> mef) {
     logger.fine("Constructor with: " + mef.getType());
     if (PIDTest.me != null && !PIDTest.me.equals(mef)) {
@@ -71,7 +61,6 @@
     this.k_d = d;
   }
 
-
   @Parameters
   public static Collection<Object[]> generateData() {
     // logger.fine("Loading the MotorList");
@@ -80,16 +69,19 @@
     double ki = 0.0005;
     double kd = 0.0;
     for (int i = 0; i < 1; i++) {
-      data.addAll(Arrays.asList(new Object[][]{{kp, ki, kd, TestBench.getInstance().getTalonPair()},
-          {kp, ki, kd, TestBench.getInstance().getVictorPair()},
-          {kp, ki, kd, TestBench.getInstance().getJaguarPair()}}));
+      data.addAll(
+          Arrays.asList(
+              new Object[][] {
+                {kp, ki, kd, TestBench.getTalonPair()},
+                {kp, ki, kd, TestBench.getVictorPair()},
+                {kp, ki, kd, TestBench.getJaguarPair()}
+              }));
     }
     return data;
   }
 
   @BeforeClass
-  public static void setUpBeforeClass() {
-  }
+  public static void setUpBeforeClass() {}
 
   @AfterClass
   public static void tearDownAfterClass() {
@@ -130,9 +122,12 @@
     setupIntegratorRange();
     double reference = 2500.0;
     m_controller.setSetpoint(reference);
-    assertEquals("PID.getPositionError() did not start at " + reference,
-        reference, m_controller.getPositionError(), 0);
-    m_builder.updateTable();
+    assertEquals(
+        "PID.getPositionError() did not start at " + reference,
+        reference,
+        m_controller.getPositionError(),
+        0);
+    m_builder.update();
     assertEquals(k_p, m_table.getEntry("Kp").getDouble(9999999), 0);
     assertEquals(k_i, m_table.getEntry("Ki").getDouble(9999999), 0);
     assertEquals(k_d, m_table.getEntry("Kd").getDouble(9999999), 0);
@@ -156,28 +151,38 @@
     double reference = 1000.0;
     assertEquals(pidData() + "did not start at 0", 0, me.getMotor().get(), 0);
     m_controller.setSetpoint(reference);
-    assertEquals(pidData() + "did not have an error of " + reference, reference,
-        m_controller.getPositionError(), 0);
-    Notifier pidRunner = new Notifier(
-        () -> me.getMotor().set(m_controller.calculate(me.getEncoder().getDistance())));
+    assertEquals(
+        pidData() + "did not have an error of " + reference,
+        reference,
+        m_controller.getPositionError(),
+        0);
+    Notifier pidRunner =
+        new Notifier(
+            () -> me.getMotor().set(m_controller.calculate(me.getEncoder().getDistance())));
     pidRunner.startPeriodic(m_controller.getPeriod());
     Timer.delay(5);
     pidRunner.stop();
-    assertTrue(pidData() + "Was not on Target. Controller Error: "
-        + m_controller.getPositionError(), m_controller.atSetpoint());
+    assertTrue(
+        pidData() + "Was not on Target. Controller Error: " + m_controller.getPositionError(),
+        m_controller.atSetpoint());
 
     pidRunner.close();
   }
 
   private String pidData() {
-    return me.getType() + " PID {P:" + m_controller.getP() + " I:" + m_controller.getI() + " D:"
-        + m_controller.getD() + "} ";
+    return me.getType()
+        + " PID {P:"
+        + m_controller.getP()
+        + " I:"
+        + m_controller.getI()
+        + " D:"
+        + m_controller.getD()
+        + "} ";
   }
 
-
   @Test(expected = RuntimeException.class)
   public void testOnTargetNoToleranceSet() {
     setupIntegratorRange();
-    m_controller.atSetpoint();
+    assertFalse(m_controller.atSetpoint());
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java
new file mode 100644
index 0000000..7c3455d
--- /dev/null
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/PriorityTest.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj;
+
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
+
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
+
+/** Tests to see if the thread and process priority functions work. */
+public class PriorityTest extends AbstractComsSetup {
+  private static final Logger logger = Logger.getLogger(PriorityTest.class.getName());
+
+  @Override
+  protected Logger getClassLogger() {
+    return logger;
+  }
+
+  @Test
+  public void testSetCurrentThreadPriority() {
+    // Non-RT thread has priority of zero
+    assertEquals(0, Threads.getCurrentThreadPriority());
+    assertFalse(Threads.getCurrentThreadIsRealTime());
+
+    // Set thread priority to 15
+    assertTrue(Threads.setCurrentThreadPriority(true, 15));
+
+    // Verify thread was given priority 15
+    assertEquals(15, Threads.getCurrentThreadPriority());
+    assertTrue(Threads.getCurrentThreadIsRealTime());
+
+    // Set thread back to non-RT
+    assertTrue(Threads.setCurrentThreadPriority(false, 0));
+
+    // Verify thread is now non-RT
+    assertEquals(0, Threads.getCurrentThreadPriority());
+    assertFalse(Threads.getCurrentThreadIsRealTime());
+  }
+}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java
index 29fad5f..cc520c3 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/RelayCrossConnectTest.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.After;
-import org.junit.Before;
-import org.junit.Test;
+import static org.junit.Assert.assertEquals;
+import static org.junit.Assert.assertFalse;
+import static org.junit.Assert.assertTrue;
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableInstance;
@@ -22,14 +17,12 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableBuilderImpl;
 import edu.wpi.first.wpilibj.test.AbstractComsSetup;
 import edu.wpi.first.wpilibj.test.TestBench;
+import java.util.logging.Logger;
+import org.junit.After;
+import org.junit.Before;
+import org.junit.Test;
 
-import static org.junit.Assert.assertEquals;
-import static org.junit.Assert.assertFalse;
-import static org.junit.Assert.assertTrue;
-
-/**
- * Tests the {@link RelayCrossConnectFixture}.
- */
+/** Tests the {@link RelayCrossConnectFixture}. */
 public class RelayCrossConnectTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(RelayCrossConnectTest.class.getName());
   private static final NetworkTable table =
@@ -37,7 +30,6 @@
   private RelayCrossConnectFixture m_relayFixture;
   private SendableBuilderImpl m_builder;
 
-
   @Before
   public void setUp() {
     m_relayFixture = TestBench.getRelayCrossConnectFixture();
@@ -57,11 +49,11 @@
   public void testBothHigh() {
     m_relayFixture.getRelay().setDirection(Direction.kBoth);
     m_relayFixture.getRelay().set(Value.kOn);
-    m_builder.updateTable();
-    assertTrue("Input one was not high when relay set both high", m_relayFixture.getInputOne()
-        .get());
-    assertTrue("Input two was not high when relay set both high", m_relayFixture.getInputTwo()
-        .get());
+    m_builder.update();
+    assertTrue(
+        "Input one was not high when relay set both high", m_relayFixture.getInputOne().get());
+    assertTrue(
+        "Input two was not high when relay set both high", m_relayFixture.getInputTwo().get());
     assertEquals(Value.kOn, m_relayFixture.getRelay().get());
     assertEquals("On", table.getEntry("Value").getString(""));
   }
@@ -70,12 +62,11 @@
   public void testFirstHigh() {
     m_relayFixture.getRelay().setDirection(Direction.kBoth);
     m_relayFixture.getRelay().set(Value.kForward);
-    m_builder.updateTable();
-    assertFalse("Input one was not low when relay set Value.kForward", m_relayFixture.getInputOne()
-        .get());
-    assertTrue("Input two was not high when relay set Value.kForward", m_relayFixture
-        .getInputTwo()
-        .get());
+    m_builder.update();
+    assertFalse(
+        "Input one was not low when relay set Value.kForward", m_relayFixture.getInputOne().get());
+    assertTrue(
+        "Input two was not high when relay set Value.kForward", m_relayFixture.getInputTwo().get());
     assertEquals(Value.kForward, m_relayFixture.getRelay().get());
     assertEquals("Forward", table.getEntry("Value").getString(""));
   }
@@ -84,12 +75,11 @@
   public void testSecondHigh() {
     m_relayFixture.getRelay().setDirection(Direction.kBoth);
     m_relayFixture.getRelay().set(Value.kReverse);
-    m_builder.updateTable();
-    assertTrue("Input one was not high when relay set Value.kReverse", m_relayFixture.getInputOne()
-        .get());
-    assertFalse("Input two was not low when relay set Value.kReverse", m_relayFixture
-        .getInputTwo()
-        .get());
+    m_builder.update();
+    assertTrue(
+        "Input one was not high when relay set Value.kReverse", m_relayFixture.getInputOne().get());
+    assertFalse(
+        "Input two was not low when relay set Value.kReverse", m_relayFixture.getInputTwo().get());
     assertEquals(Value.kReverse, m_relayFixture.getRelay().get());
     assertEquals("Reverse", table.getEntry("Value").getString(""));
   }
@@ -98,10 +88,12 @@
   public void testForwardDirection() {
     m_relayFixture.getRelay().setDirection(Direction.kForward);
     m_relayFixture.getRelay().set(Value.kOn);
-    m_builder.updateTable();
-    assertFalse("Input one was not low when relay set Value.kOn in kForward Direction",
+    m_builder.update();
+    assertFalse(
+        "Input one was not low when relay set Value.kOn in kForward Direction",
         m_relayFixture.getInputOne().get());
-    assertTrue("Input two was not high when relay set Value.kOn in kForward Direction",
+    assertTrue(
+        "Input two was not high when relay set Value.kOn in kForward Direction",
         m_relayFixture.getInputTwo().get());
     assertEquals(Value.kOn, m_relayFixture.getRelay().get());
     assertEquals("On", table.getEntry("Value").getString(""));
@@ -111,10 +103,12 @@
   public void testReverseDirection() {
     m_relayFixture.getRelay().setDirection(Direction.kReverse);
     m_relayFixture.getRelay().set(Value.kOn);
-    m_builder.updateTable();
-    assertTrue("Input one was not high when relay set Value.kOn in kReverse Direction",
+    m_builder.update();
+    assertTrue(
+        "Input one was not high when relay set Value.kOn in kReverse Direction",
         m_relayFixture.getInputOne().get());
-    assertFalse("Input two was not low when relay set Value.kOn in kReverse Direction",
+    assertFalse(
+        "Input two was not low when relay set Value.kOn in kReverse Direction",
         m_relayFixture.getInputTwo().get());
     assertEquals(Value.kOn, m_relayFixture.getRelay().get());
     assertEquals("On", table.getEntry("Value").getString(""));
@@ -134,7 +128,7 @@
 
   @Test
   public void testInitialSettings() {
-    m_builder.updateTable();
+    m_builder.update();
     assertEquals(Value.kOff, m_relayFixture.getRelay().get());
     // Initially both outputs should be off
     assertFalse(m_relayFixture.getInputOne().get());
@@ -146,5 +140,4 @@
   protected Logger getClassLogger() {
     return logger;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java
index efcea23..bb0fcc9 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/SampleTest.java
@@ -1,24 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
+import static org.junit.Assert.assertTrue;
 
+import edu.wpi.first.wpilibj.fixtures.SampleFixture;
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
 import org.junit.AfterClass;
 import org.junit.Before;
 import org.junit.BeforeClass;
 import org.junit.Test;
 
-import edu.wpi.first.wpilibj.fixtures.SampleFixture;
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
-import static org.junit.Assert.assertTrue;
-
 /**
  * Sample test for a sample PID controller. This demonstrates the general pattern of how to create a
  * test and use testing fixtures and categories. All tests must extend from {@link
@@ -62,5 +57,4 @@
     Timer.delay(0.5);
     assertTrue(true);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java
index 2acd5bb..8d0fc95 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/TimerTest.java
@@ -1,20 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.util.logging.Logger;
-
-import org.junit.Test;
-
-import edu.wpi.first.wpilibj.test.AbstractComsSetup;
-
 import static org.junit.Assert.assertEquals;
 
+import edu.wpi.first.wpilibj.test.AbstractComsSetup;
+import java.util.logging.Logger;
+import org.junit.Test;
 
 public class TimerTest extends AbstractComsSetup {
   private static final Logger logger = Logger.getLogger(TimerTest.class.getName());
@@ -38,8 +32,10 @@
 
     // Then
     long offset = difference - TIMER_RUNTIME;
-    assertEquals("Timer.delay ran " + offset + " microseconds too long", TIMER_RUNTIME, difference,
+    assertEquals(
+        "Timer.delay ran " + offset + " microseconds too long",
+        TIMER_RUNTIME,
+        difference,
         TIMER_TOLERANCE);
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java
index 9ab6850..e7d968f 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/WpiLibJTestSuite.java
@@ -1,28 +1,38 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
+import edu.wpi.first.wpilibj.test.AbstractTestSuite;
 import org.junit.runner.RunWith;
 import org.junit.runners.Suite;
 import org.junit.runners.Suite.SuiteClasses;
 
-import edu.wpi.first.wpilibj.test.AbstractTestSuite;
-
 /**
  * Holds all of the tests in the root wpilibj directory. Please list alphabetically so that it is
  * easy to determine if a test is missing from the list.
  */
 @RunWith(Suite.class)
-@SuiteClasses({AnalogCrossConnectTest.class, AnalogPotentiometerTest.class,
-    BuiltInAccelerometerTest.class, ConstantsPortsTest.class, CounterTest.class,
-    DigitalGlitchFilterTest.class, DIOCrossConnectTest.class,
-    DriverStationTest.class, EncoderTest.class, GyroTest.class, MotorEncoderTest.class,
-    MotorInvertingTest.class, PCMTest.class, PDPTest.class, PIDTest.class,
-    RelayCrossConnectTest.class, SampleTest.class, TimerTest.class})
-public class WpiLibJTestSuite extends AbstractTestSuite {
-}
+@SuiteClasses({
+  AnalogCrossConnectTest.class,
+  AnalogPotentiometerTest.class,
+  BuiltInAccelerometerTest.class,
+  ConstantsPortsTest.class,
+  CounterTest.class,
+  DigitalGlitchFilterTest.class,
+  DIOCrossConnectTest.class,
+  DMATest.class,
+  DriverStationTest.class,
+  EncoderTest.class,
+  GyroTest.class,
+  MotorEncoderTest.class,
+  MotorInvertingTest.class,
+  PCMTest.class,
+  PDPTest.class,
+  PIDTest.class,
+  RelayCrossConnectTest.class,
+  SampleTest.class,
+  TimerTest.class
+})
+public class WpiLibJTestSuite extends AbstractTestSuite {}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANStatusTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
index 6aa6c8e..d5c11e3 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/can/CANStatusTest.java
@@ -1,22 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.can;
 
-import org.junit.Test;
-
 import edu.wpi.first.hal.can.CANJNI;
 import edu.wpi.first.hal.can.CANStatus;
+import org.junit.Test;
 
 public class CANStatusTest {
   @Test
   public void canStatusGetDoesntThrow() {
     CANStatus status = new CANStatus();
-    CANJNI.GetCANStatus(status);
+    CANJNI.getCANStatus(status);
     // Nothing we can assert, so just make sure it didn't throw.
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/AnalogCrossConnectFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/AnalogCrossConnectFixture.java
index ece1dc9..1dd63da 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/AnalogCrossConnectFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/AnalogCrossConnectFixture.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
 import edu.wpi.first.wpilibj.AnalogInput;
 import edu.wpi.first.wpilibj.AnalogOutput;
 
-/**
- * A fixture that connects an {@link AnalogInput} and an {@link AnalogOutput}.
- */
+/** A fixture that connects an {@link AnalogInput} and an {@link AnalogOutput}. */
 public abstract class AnalogCrossConnectFixture implements ITestFixture {
   private boolean m_initialized = false;
 
@@ -23,7 +18,6 @@
 
   protected abstract AnalogOutput giveAnalogOutput();
 
-
   private void initialize() {
     synchronized (this) {
       if (!m_initialized) {
@@ -40,10 +34,9 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#setup()
    */
   @Override
-  public boolean setup() {
+  public void setup() {
     initialize();
     m_output.setVoltage(0);
-    return true;
   }
 
   /*
@@ -52,9 +45,8 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#reset()
    */
   @Override
-  public boolean reset() {
+  public void reset() {
     initialize();
-    return true;
   }
 
   /*
@@ -63,15 +55,12 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#teardown()
    */
   @Override
-  public boolean teardown() {
+  public void teardown() {
     m_input.close();
     m_output.close();
-    return true;
   }
 
-  /**
-   * Analog Output.
-   */
+  /** Analog Output. */
   public final AnalogOutput getOutput() {
     initialize();
     return m_output;
@@ -81,6 +70,4 @@
     initialize();
     return m_input;
   }
-
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java
index fa17baa..78880d8 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/DIOCrossConnectFixture.java
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.DigitalOutput;
 import java.util.logging.Level;
 import java.util.logging.Logger;
 
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.DigitalOutput;
-
-/**
- * Connects a digital input to a digital output.
- */
+/** Connects a digital input to a digital output. */
 public class DIOCrossConnectFixture implements ITestFixture {
   private static final Logger logger = Logger.getLogger(DIOCrossConnectFixture.class.getName());
 
@@ -26,7 +20,7 @@
   /**
    * Constructs using two pre-allocated digital objects.
    *
-   * @param input  The input
+   * @param input The input
    * @param output The output.
    */
   public DIOCrossConnectFixture(DigitalInput input, DigitalOutput output) {
@@ -40,7 +34,7 @@
   /**
    * Constructs a {@link DIOCrossConnectFixture} using the ports of the digital objects.
    *
-   * @param input  The port of the {@link DigitalInput}
+   * @param input The port of the {@link DigitalInput}
    * @param output The port of the {@link DigitalOutput}
    */
   public DIOCrossConnectFixture(Integer input, Integer output) {
@@ -61,30 +55,20 @@
   }
 
   @Override
-  public boolean setup() {
-    return true;
-  }
+  public void setup() {}
 
   @Override
-  public boolean reset() {
-    try {
-      m_input.cancelInterrupts();
-    } catch (IllegalStateException ex) {
-      // This will happen if the interrupt has not been allocated for this test.
-    }
+  public void reset() {
     m_output.set(false);
-    return true;
   }
 
   @Override
-  public boolean teardown() {
+  public void teardown() {
     logger.log(Level.FINE, "Beginning teardown");
     if (m_allocated) {
       m_input.close();
       m_output.close();
       m_allocated = false;
     }
-    return true;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java
index 8dbb4d3..38fbbfc 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeCounterFixture.java
@@ -1,21 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
+import edu.wpi.first.wpilibj.Counter;
+import edu.wpi.first.wpilibj.mockhardware.FakeCounterSource;
 import java.util.logging.Level;
 import java.util.logging.Logger;
 
-import edu.wpi.first.wpilibj.Counter;
-import edu.wpi.first.wpilibj.mockhardware.FakeCounterSource;
-
-/**
- * A fixture that can test the {@link Counter} using a {@link DIOCrossConnectFixture}.
- */
+/** A fixture that can test the {@link Counter} using a {@link DIOCrossConnectFixture}. */
 public class FakeCounterFixture implements ITestFixture {
   private static final Logger logger = Logger.getLogger(FakeEncoderFixture.class.getName());
 
@@ -36,11 +30,10 @@
     m_counter = new Counter(dio.getInput());
   }
 
-
   /**
    * Constructs a FakeCounterFixture using two port numbers.
    *
-   * @param input  the input port
+   * @param input the input port
    * @param output the output port
    */
   public FakeCounterFixture(int input, int output) {
@@ -68,17 +61,13 @@
     return m_counter;
   }
 
-
   /*
    * (non-Javadoc)
    *
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#setup()
    */
   @Override
-  public boolean setup() {
-    return true;
-
-  }
+  public void setup() {}
 
   /*
    * (non-Javadoc)
@@ -86,9 +75,8 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#reset()
    */
   @Override
-  public boolean reset() {
+  public void reset() {
     m_counter.reset();
-    return true;
   }
 
   /*
@@ -97,7 +85,7 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#teardown()
    */
   @Override
-  public boolean teardown() {
+  public void teardown() {
     logger.log(Level.FINE, "Beginning teardown");
     m_counter.close();
     m_source.close();
@@ -105,8 +93,5 @@
       m_dio.teardown();
       m_allocated = false;
     }
-    return true;
   }
-
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java
index ef9da0e..480185e 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/FakeEncoderFixture.java
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
-import java.util.logging.Logger;
-
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.mockhardware.FakeEncoderSource;
+import java.util.logging.Logger;
 
 /**
  * An encoder that uses two {@link DIOCrossConnectFixture DIOCrossConnectFixtures} to test the
@@ -28,9 +24,7 @@
   private final Encoder m_encoder;
   private int[] m_encoderPort = new int[2];
 
-  /**
-   * Constructs a FakeEncoderFixture from two DIOCrossConnectFixture.
-   */
+  /** Constructs a FakeEncoderFixture from two DIOCrossConnectFixture. */
   public FakeEncoderFixture(DIOCrossConnectFixture dio1, DIOCrossConnectFixture dio2) {
     assert dio1 != null;
     assert dio2 != null;
@@ -41,9 +35,7 @@
     m_encoder = new Encoder(dio1.getInput(), dio2.getInput());
   }
 
-  /**
-   * Construcst a FakeEncoderFixture from a set of Digital I/O ports.
-   */
+  /** Construcst a FakeEncoderFixture from a set of Digital I/O ports. */
   public FakeEncoderFixture(int inputA, int outputA, int inputB, int outputB) {
     assert outputA != outputB;
     assert outputA != inputA;
@@ -76,9 +68,7 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#setup()
    */
   @Override
-  public boolean setup() {
-    return true;
-  }
+  public void setup() {}
 
   /*
    * (non-Javadoc)
@@ -86,11 +76,10 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#reset()
    */
   @Override
-  public boolean reset() {
+  public void reset() {
     m_dio1.reset();
     m_dio2.reset();
     m_encoder.reset();
-    return true;
   }
 
   /*
@@ -99,7 +88,7 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#teardown()
    */
   @Override
-  public boolean teardown() {
+  public void teardown() {
     logger.fine("Beginning teardown");
     m_source.close();
     logger.finer("Source freed " + m_sourcePort[0] + ",  " + m_sourcePort[1]);
@@ -109,8 +98,5 @@
       m_dio1.teardown();
       m_dio2.teardown();
     }
-    return true;
   }
-
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java
index 3465915..527606e 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/ITestFixture.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
@@ -21,10 +18,8 @@
   /**
    * Performs any required setup for this fixture, ensuring that all fixture elements are ready for
    * testing.
-   *
-   * @return True if the fixture is ready for testing
    */
-  boolean setup();
+  void setup();
 
   /**
    * Resets the fixture back to test start state. This should be called by the test class in the
@@ -32,16 +27,12 @@
    * ITestFixture#setup()} as that is called once, before the class is constructed, so it may need
    * to start sensors. This method should not have to start anything, just reset sensors and ensure
    * that motors are stopped.
-   *
-   * @return True if the fixture is ready for testing
    */
-  boolean reset();
+  void reset();
 
   /**
    * Performs any required teardown after use of the fixture, ensuring that future tests will not
    * run into conflicts.
-   *
-   * @return True if the teardown succeeded
    */
-  boolean teardown();
+  void teardown();
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java
index fab3306..220db18 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/MotorEncoderFixture.java
@@ -1,31 +1,28 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
-import java.lang.reflect.ParameterizedType;
-import java.util.logging.Logger;
-
 import edu.wpi.first.wpilibj.Counter;
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Encoder;
 import edu.wpi.first.wpilibj.PWM;
-import edu.wpi.first.wpilibj.SpeedController;
 import edu.wpi.first.wpilibj.Timer;
+import edu.wpi.first.wpilibj.motorcontrol.MotorController;
 import edu.wpi.first.wpilibj.test.TestBench;
+import java.lang.reflect.ParameterizedType;
+import java.util.logging.Logger;
 
 /**
  * Represents a physically connected Motor and Encoder to allow for unit tests on these different
- * pairs<br> Designed to allow the user to easily setup and tear down the fixture to allow for
- * reuse. This class should be explicitly instantiated in the TestBed class to allow any test to
- * access this fixture. This allows tests to be mailable so that you can easily reconfigure the
- * physical testbed without breaking the tests.
+ * pairs<br>
+ * Designed to allow the user to easily setup and tear down the fixture to allow for reuse. This
+ * class should be explicitly instantiated in the TestBed class to allow any test to access this
+ * fixture. This allows tests to be mailable so that you can easily reconfigure the physical testbed
+ * without breaking the tests.
  */
-public abstract class MotorEncoderFixture<T extends SpeedController> implements ITestFixture {
+public abstract class MotorEncoderFixture<T extends MotorController> implements ITestFixture {
   private static final Logger logger = Logger.getLogger(MotorEncoderFixture.class.getName());
   private boolean m_initialized = false;
   private boolean m_tornDown = false;
@@ -35,22 +32,19 @@
   protected DigitalInput m_alphaSource; // Stored so it can be freed at tear down
   protected DigitalInput m_betaSource;
 
-  /**
-   * Default constructor for a MotorEncoderFixture.
-   */
-  public MotorEncoderFixture() {
-  }
+  /** Default constructor for a MotorEncoderFixture. */
+  public MotorEncoderFixture() {}
 
   public abstract int getPDPChannel();
 
   /**
    * Where the implementer of this class should pass the speed controller Constructor should only be
-   * called from outside this class if the Speed controller is not also an implementation of PWM
+   * called from outside this class if the Motor controller is not also an implementation of PWM
    * interface.
    *
-   * @return SpeedController
+   * @return MotorController
    */
-  protected abstract T giveSpeedController();
+  protected abstract T giveMotorController();
 
   /**
    * Where the implementer of this class should pass one of the digital inputs.
@@ -78,20 +72,18 @@
         m_alphaSource = giveDigitalInputA();
         m_betaSource = giveDigitalInputB();
 
-
         m_encoder = new Encoder(m_alphaSource, m_betaSource);
         m_counters[0] = new Counter(m_alphaSource);
         m_counters[1] = new Counter(m_betaSource);
         logger.fine("Creating the speed controller!");
-        m_motor = giveSpeedController();
+        m_motor = giveMotorController();
       }
     }
   }
 
   @Override
-  public boolean setup() {
+  public void setup() {
     initialize();
-    return true;
   }
 
   /**
@@ -133,7 +125,7 @@
    * Checks to see if the speed of the motor is within some range of a given value. This is used
    * instead of equals() because doubles can have inaccuracies.
    *
-   * @param value    The value to compare against
+   * @param value The value to compare against
    * @param accuracy The accuracy range to check against to see if the
    * @return true if the range of values between motors speed accuracy contains the 'value'.
    */
@@ -143,7 +135,7 @@
   }
 
   @Override
-  public boolean reset() {
+  public void reset() {
     initialize();
     m_motor.setInverted(false);
     m_motor.set(0);
@@ -152,18 +144,8 @@
     for (Counter c : m_counters) {
       c.reset();
     }
-
-    boolean wasReset = true;
-    wasReset = wasReset && m_motor.get() == 0;
-    wasReset = wasReset && m_encoder.get() == 0;
-    for (Counter c : m_counters) {
-      wasReset = wasReset && c.get() == 0;
-    }
-
-    return wasReset;
   }
 
-
   /**
    * Safely tears down the MotorEncoder Fixture in a way that makes sure that even if an object
    * fails to initialize the rest of the fixture can still be torn down and the resources
@@ -171,62 +153,37 @@
    */
   @Override
   @SuppressWarnings("Regexp")
-  public boolean teardown() {
-    String type;
-    if (m_motor != null) {
-      type = getType();
-    } else {
-      type = "null";
-    }
+  public void teardown() {
     if (!m_tornDown) {
-      boolean wasNull = false;
-      if (m_motor instanceof PWM && m_motor != null) {
-        ((PWM) m_motor).close();
+      if (m_motor != null) {
+        if (m_motor instanceof PWM) {
+          ((PWM) m_motor).close();
+        }
         m_motor = null;
-      } else if (m_motor == null) {
-        wasNull = true;
       }
       if (m_encoder != null) {
         m_encoder.close();
         m_encoder = null;
-      } else {
-        wasNull = true;
       }
       if (m_counters[0] != null) {
         m_counters[0].close();
         m_counters[0] = null;
-      } else {
-        wasNull = true;
       }
       if (m_counters[1] != null) {
         m_counters[1].close();
         m_counters[1] = null;
-      } else {
-        wasNull = true;
       }
       if (m_alphaSource != null) {
         m_alphaSource.close();
         m_alphaSource = null;
-      } else {
-        wasNull = true;
       }
       if (m_betaSource != null) {
         m_betaSource.close();
         m_betaSource = null;
-      } else {
-        wasNull = true;
       }
 
       m_tornDown = true;
-
-      if (wasNull) {
-        throw new NullPointerException("MotorEncoderFixture had null params at teardown");
-      }
-    } else {
-      throw new RuntimeException(type + " Motor Encoder torn down multiple times");
     }
-
-    return true;
   }
 
   @Override
@@ -235,11 +192,10 @@
     // Get the generic type as a class
     @SuppressWarnings("unchecked")
     Class<T> class1 =
-        (Class<T>) ((ParameterizedType) getClass().getGenericSuperclass())
-            .getActualTypeArguments()[0];
+        (Class<T>)
+            ((ParameterizedType) getClass().getGenericSuperclass()).getActualTypeArguments()[0];
     string.append(class1.getSimpleName());
     string.append(">");
     return string.toString();
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/RelayCrossConnectFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/RelayCrossConnectFixture.java
index 382b4bb..3076caf 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/RelayCrossConnectFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/RelayCrossConnectFixture.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
 import edu.wpi.first.wpilibj.DigitalInput;
 import edu.wpi.first.wpilibj.Relay;
 
-/**
- * A connection between a {@link Relay} and two {@link DigitalInput DigitalInputs}.
- */
+/** A connection between a {@link Relay} and two {@link DigitalInput DigitalInputs}. */
 public abstract class RelayCrossConnectFixture implements ITestFixture {
   private DigitalInput m_inputOne;
   private DigitalInput m_inputTwo;
@@ -21,7 +16,6 @@
   private boolean m_initialized = false;
   private boolean m_freed = false;
 
-
   protected abstract Relay giveRelay();
 
   protected abstract DigitalInput giveInputOne();
@@ -60,9 +54,8 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#setup()
    */
   @Override
-  public boolean setup() {
+  public void setup() {
     initialize();
-    return true;
   }
 
   /*
@@ -71,9 +64,8 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#reset()
    */
   @Override
-  public boolean reset() {
+  public void reset() {
     initialize();
-    return true;
   }
 
   /*
@@ -82,16 +74,17 @@
    * @see edu.wpi.first.wpilibj.fixtures.ITestFixture#teardown()
    */
   @Override
-  public boolean teardown() {
+  public void teardown() {
     if (!m_freed) {
       m_relay.close();
       m_inputOne.close();
       m_inputTwo.close();
       m_freed = true;
     } else {
-      throw new RuntimeException("You attempted to free the "
-          + RelayCrossConnectFixture.class.getSimpleName() + " multiple times");
+      throw new RuntimeException(
+          "You attempted to free the "
+              + RelayCrossConnectFixture.class.getSimpleName()
+              + " multiple times");
     }
-    return true;
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java
index 994c6ee..38ea638 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/SampleFixture.java
@@ -1,48 +1,41 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
-
 /**
  * This is an example of how to use the {@link ITestFixture} interface to create test fixtures for a
  * test.
  */
 public class SampleFixture implements ITestFixture {
   @Override
-  public boolean setup() {
+  public void setup() {
     /*
      * If this fixture actually accessed the hardware, here is where it would
      * set up the starting state of the test bench. For example, reseting
      * encoders, ensuring motors are stopped, reseting any serial communication
      * if necessary, etc.
      */
-    return true;
   }
 
   @Override
-  public boolean reset() {
+  public void reset() {
     /*
      * This is where the fixture would reset any sensors or motors used by the
      * fixture to test default state. This method should not worry about whether
      * or not the sensors have been allocated correctly, that is the job of the
      * setup function.
      */
-    return false;
   }
 
   @Override
-  public boolean teardown() {
+  public void teardown() {
     /*
      * This is where the fixture would deallocate and reset back to normal
      * conditions any necessary hardware. This includes ensuring motors are
      * stopped, stoppable sensors are actually stopped, ensuring serial
      * communications are ready for the next test, etc.
      */
-    return true;
   }
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java
index afeaf38..f41687d 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.fixtures;
 
-import java.util.logging.Logger;
-
 import edu.wpi.first.wpilibj.AnalogGyro;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.Timer;
+import java.util.logging.Logger;
 
 /**
  * A class to represent the a physical Camera with two servos (tilt and pan) designed to test to see
@@ -27,7 +23,6 @@
   private Servo m_pan;
   private boolean m_initialized = false;
 
-
   protected abstract AnalogGyro giveGyro();
 
   protected abstract AnalogGyro giveGyroParam(int center, double offset);
@@ -36,15 +31,11 @@
 
   protected abstract Servo givePan();
 
-  /**
-   * Constructs the TiltPanCamera.
-   */
-  public TiltPanCameraFixture() {
-  }
+  /** Constructs the TiltPanCamera. */
+  public TiltPanCameraFixture() {}
 
   @Override
-  public boolean setup() {
-    boolean wasSetup = false;
+  public void setup() {
     if (!m_initialized) {
       m_initialized = true;
       m_tilt = giveTilt();
@@ -56,17 +47,14 @@
       logger.fine("Initializing the gyro");
       m_gyro = giveGyro();
       m_gyro.reset();
-      wasSetup = true;
     }
-    return wasSetup;
   }
 
   @Override
-  public boolean reset() {
+  public void reset() {
     if (m_gyro != null) {
       m_gyro.reset();
     }
-    return true;
   }
 
   public Servo getTilt() {
@@ -97,20 +85,18 @@
   }
 
   @Override
-  public boolean teardown() {
+  public void teardown() {
     m_tilt.close();
     m_tilt = null;
     m_pan.close();
     m_pan = null;
-    if (m_gyro != null) { //in case not freed during gyro tests
+    if (m_gyro != null) { // in case not freed during gyro tests
       m_gyro.close();
       m_gyro = null;
     }
-    if (m_gyroParam != null) { //in case gyro tests failed before getting to this point
+    if (m_gyroParam != null) { // in case gyro tests failed before getting to this point
       m_gyroParam.close();
       m_gyroParam = null;
     }
-    return true;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java
index d67fbdb..ac1094c 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeCounterSource.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.mockhardware;
 
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.Timer;
 
-/**
- * Simulates an encoder for testing purposes.
- */
+/** Simulates an encoder for testing purposes. */
 public class FakeCounterSource implements AutoCloseable {
   private Thread m_task;
   private int m_count;
@@ -20,10 +15,8 @@
   private DigitalOutput m_output;
   private boolean m_allocated;
 
-  /**
-   * Thread object that allows emulation of an encoder.
-   */
-  private class EncoderThread extends Thread {
+  /** Thread object that allows emulation of an encoder. */
+  private static class EncoderThread extends Thread {
     FakeCounterSource m_encoder;
 
     EncoderThread(FakeCounterSource encode) {
@@ -68,9 +61,7 @@
     initEncoder();
   }
 
-  /**
-   * Destroy Object with minimum memory leak.
-   */
+  /** Destroy Object with minimum memory leak. */
   @Override
   public void close() {
     m_task = null;
@@ -81,25 +72,19 @@
     }
   }
 
-  /**
-   * Common initailization code.
-   */
+  /** Common initailization code. */
   private void initEncoder() {
     m_milliSec = 1;
     m_task = new EncoderThread(this);
     m_output.set(false);
   }
 
-  /**
-   * Starts the thread execution task.
-   */
+  /** Starts the thread execution task. */
   public void start() {
     m_task.start();
   }
 
-  /**
-   * Waits for the thread to complete.
-   */
+  /** Waits for the thread to complete. */
   public void complete() {
     try {
       m_task.join();
@@ -110,9 +95,7 @@
     Timer.delay(0.01);
   }
 
-  /**
-   * Starts and completes a task set - does not return until thred has finished its operations.
-   */
+  /** Starts and completes a task set - does not return until thred has finished its operations. */
   public void execute() {
     start();
     complete();
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java
index efc69a5..5b91b0c 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakeEncoderSource.java
@@ -1,18 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2014-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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.mockhardware;
 
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.Timer;
 
-/**
- * Emulates a quadrature encoder.
- */
+/** Emulates a quadrature encoder. */
 public class FakeEncoderSource implements AutoCloseable {
   private Thread m_task;
   private int m_count;
@@ -22,10 +17,8 @@
   private final DigitalOutput m_outputB;
   private final boolean m_allocatedOutputs;
 
-  /**
-   * Thread object that allows emulation of a quadrature encoder.
-   */
-  private class QuadEncoderThread extends Thread {
+  /** Thread object that allows emulation of a quadrature encoder. */
+  private static class QuadEncoderThread extends Thread {
     FakeEncoderSource m_encoder;
 
     QuadEncoderThread(FakeEncoderSource encode) {
@@ -91,9 +84,7 @@
     initQuadEncoder();
   }
 
-  /**
-   * Frees the resource.
-   */
+  /** Frees the resource. */
   @Override
   public void close() {
     m_task = null;
@@ -103,9 +94,7 @@
     }
   }
 
-  /**
-   * Common initialization code.
-   */
+  /** Common initialization code. */
   private void initQuadEncoder() {
     m_milliSec = 1;
     m_forward = true;
@@ -114,16 +103,12 @@
     m_outputB.set(false);
   }
 
-  /**
-   * Starts the thread.
-   */
+  /** Starts the thread. */
   public void start() {
     m_task.start();
   }
 
-  /**
-   * Waits for thread to end.
-   */
+  /** Waits for thread to end. */
   public void complete() {
     try {
       m_task.join();
@@ -134,9 +119,7 @@
     Timer.delay(0.01);
   }
 
-  /**
-   * Runs and waits for thread to end before returning.
-   */
+  /** Runs and waits for thread to end before returning. */
   public void execute() {
     start();
     complete();
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakePotentiometerSource.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakePotentiometerSource.java
index 63af674..59cc5c7 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakePotentiometerSource.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/mockhardware/FakePotentiometerSource.java
@@ -1,17 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.mockhardware;
 
 import edu.wpi.first.wpilibj.AnalogOutput;
 
-/**
- * A fake source to provide output to a {@link edu.wpi.first.wpilibj.interfaces.Potentiometer}.
- */
+/** A fake source to provide output to a {@link edu.wpi.first.wpilibj.interfaces.Potentiometer}. */
 public class FakePotentiometerSource implements AutoCloseable {
   private AnalogOutput m_output;
   private boolean m_initOutput;
@@ -22,7 +17,7 @@
   /**
    * Constructs the fake source.
    *
-   * @param output             The analog port to output the signal on
+   * @param output The analog port to output the signal on
    * @param defaultPotMaxAngle The default maximum angle the pot supports.
    */
   public FakePotentiometerSource(AnalogOutput output, double defaultPotMaxAngle) {
@@ -80,9 +75,7 @@
     return voltage * (m_potMaxAngle / m_potMaxVoltage);
   }
 
-  /**
-   * Frees the resouce.
-   */
+  /** Frees the resouce. */
   @Override
   public void close() {
     if (m_initOutput) {
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
new file mode 100644
index 0000000..e7a6b8e
--- /dev/null
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/motorcontrol/MockMotorController.java
@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.motorcontrol;
+
+public class MockMotorController implements MotorController {
+  private double m_speed;
+  private boolean m_isInverted;
+
+  @Override
+  public void set(double speed) {
+    m_speed = m_isInverted ? -speed : speed;
+  }
+
+  @Override
+  public double get() {
+    return m_speed;
+  }
+
+  @Override
+  public void setInverted(boolean isInverted) {
+    m_isInverted = isInverted;
+  }
+
+  @Override
+  public boolean getInverted() {
+    return m_isInverted;
+  }
+
+  @Override
+  public void disable() {
+    m_speed = 0;
+  }
+
+  @Override
+  public void stopMotor() {
+    disable();
+  }
+}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
index 7c49eef..734115c 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractComsSetup.java
@@ -1,26 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
-import java.util.logging.Level;
-import java.util.logging.Logger;
-
-import org.junit.BeforeClass;
-import org.junit.Rule;
-import org.junit.rules.TestWatcher;
-import org.junit.runner.Description;
-import org.junit.runners.model.MultipleFailureException;
-
 import edu.wpi.first.hal.HAL;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.MockDS;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import java.util.logging.Level;
+import java.util.logging.Logger;
+import org.junit.BeforeClass;
+import org.junit.Rule;
+import org.junit.rules.TestWatcher;
+import org.junit.runner.Description;
+import org.junit.runners.model.MultipleFailureException;
 
 /**
  * This class serves as a superclass for all tests that involve the hardware on the roboRIO. It uses
@@ -29,19 +24,16 @@
  * to run.
  */
 public abstract class AbstractComsSetup {
-  /**
-   * Stores whether network coms have been initialized.
-   */
+  /** Stores whether network coms have been initialized. */
   private static boolean initialized = false;
 
   // We have no way to stop the MockDS, so its thread is daemon.
-  private static  MockDS ds;
+  private static MockDS ds;
 
   /**
-   * This sets up the network communications library to enable the driver
-   * station. After starting network coms, it will loop until the driver station
-   * returns that the robot is enabled, to ensure that tests will be able to run
-   * on the hardware.
+   * This sets up the network communications library to enable the driver station. After starting
+   * network coms, it will loop until the driver station returns that the robot is enabled, to
+   * ensure that tests will be able to run on the hardware.
    */
   static {
     if (!initialized) {
@@ -49,7 +41,7 @@
         // Set some implementations so that the static methods work properly
         HAL.initialize(500, 0);
         HAL.observeUserProgramStarting();
-        DriverStation.getInstance().getAlliance();
+        DriverStation.getAlliance();
 
         ds = new MockDS();
         ds.start();
@@ -64,7 +56,7 @@
 
       // Wait until the robot is enabled before starting the tests
       int enableCounter = 0;
-      while (!DriverStation.getInstance().isEnabled()) {
+      while (!DriverStation.isEnabled()) {
         if (enableCounter > 50) {
           // Robot did not enable properly after 5 seconds.
           // Force exit
@@ -86,12 +78,9 @@
     }
   }
 
-
   protected abstract Logger getClassLogger();
 
-  /**
-   * This causes a stack trace to be printed as the test is running as well as at the end.
-   */
+  /** This causes a stack trace to be printed as the test is running as well as at the end. */
   @Rule
   public final TestWatcher getTestWatcher() {
     return getOverridenTestWatcher();
@@ -124,19 +113,32 @@
         int exceptionCount = 1; // Running exception count
         int failureCount = ((MultipleFailureException) throwable).getFailures().size();
         for (Throwable singleThrown : ((MultipleFailureException) throwable).getFailures()) {
-          getClassLogger().logp(
-              Level.SEVERE,
-              description.getClassName(),
-              description.getMethodName(),
-              (exceptionCount++) + "/" + failureCount + " " + description.getDisplayName() + " "
-                  + "failed " + singleThrown.getMessage() + "\n" + status, singleThrown);
+          getClassLogger()
+              .logp(
+                  Level.SEVERE,
+                  description.getClassName(),
+                  description.getMethodName(),
+                  (exceptionCount++)
+                      + "/"
+                      + failureCount
+                      + " "
+                      + description.getDisplayName()
+                      + " "
+                      + "failed "
+                      + singleThrown.getMessage()
+                      + "\n"
+                      + status,
+                  singleThrown);
         }
 
       } else {
-        getClassLogger().logp(Level.SEVERE, description.getClassName(),
-            description.getMethodName(),
-            description.getDisplayName() + " failed " + throwable.getMessage() + "\n" + status,
-            throwable);
+        getClassLogger()
+            .logp(
+                Level.SEVERE,
+                description.getClassName(),
+                description.getMethodName(),
+                description.getDisplayName() + " failed " + throwable.getMessage() + "\n" + status,
+                throwable);
       }
       super.failed(throwable, description);
     }
@@ -152,7 +154,6 @@
       failed(exception, description, "");
     }
 
-
     /*
      * (non-Javadoc)
      *
@@ -163,7 +164,7 @@
       TestBench.out().println();
       // Wait until the robot is enabled before starting the next tests
       int enableCounter = 0;
-      while (!DriverStation.getInstance().isEnabled()) {
+      while (!DriverStation.isEnabled()) {
         try {
           Thread.sleep(100);
         } catch (InterruptedException ex) {
@@ -172,8 +173,8 @@
         // Prints the message on one line overwriting itself each time
         TestBench.out().print("\rWaiting for enable: " + enableCounter++);
       }
-      getClassLogger().logp(Level.INFO, description.getClassName(), description.getMethodName(),
-          "Starting");
+      getClassLogger()
+          .logp(Level.INFO, description.getClassName(), description.getMethodName(), "Starting");
       super.starting(description);
     }
 
@@ -182,13 +183,12 @@
       simpleLog(Level.INFO, "TEST PASSED!");
       super.succeeded(description);
     }
-
   }
 
   /**
    * Logs a simple message without the logger formatting associated with it.
    *
-   * @param level   The level to log the message at
+   * @param level The level to log the message at
    * @param message The message to log
    */
   protected void simpleLog(Level level, String message) {
@@ -201,9 +201,8 @@
    * Provided as a replacement to lambda functions to allow for repeatable checks to see if a
    * correct state is reached.
    */
-  public abstract class BooleanCheck {
-    public BooleanCheck() {
-    }
+  public abstract static class BooleanCheck {
+    public BooleanCheck() {}
 
     /**
      * Runs the enclosed code and evaluates it to determine what state it should return.
@@ -216,22 +215,23 @@
   /**
    * Delays until either the correct state is reached or we reach the timeout.
    *
-   * @param level        The level to log the message at.
-   * @param timeout      How long the delay should run before it should timeout and allow the test
-   *                     to continue
-   * @param message      The message to accompany the delay. The message will display 'message' took
-   *                     'timeout' seconds if it passed.
+   * @param level The level to log the message at.
+   * @param timeout How long the delay should run before it should timeout and allow the test to
+   *     continue
+   * @param message The message to accompany the delay. The message will display 'message' took
+   *     'timeout' seconds if it passed.
    * @param correctState A method to determine if the test has reached a state where it is valid to
-   *                     continue
+   *     continue
    * @return a double representing how long the delay took to run in seconds.
    */
-  public double delayTillInCorrectStateWithMessage(Level level, double timeout, String message,
-                                                   BooleanCheck correctState) {
+  public double delayTillInCorrectStateWithMessage(
+      Level level, double timeout, String message, BooleanCheck correctState) {
     int timeoutIndex;
     // As long as we are not in the correct state and the timeout has not been
     // reached then continue to run this loop
-    for (timeoutIndex = 0; timeoutIndex < (timeout * 100) && !correctState.getAsBoolean();
-         timeoutIndex++) {
+    for (timeoutIndex = 0;
+        timeoutIndex < (timeout * 100) && !correctState.getAsBoolean();
+        timeoutIndex++) {
       Timer.delay(0.01);
     }
     if (correctState.getAsBoolean()) {
@@ -241,5 +241,4 @@
     }
     return timeoutIndex * 0.01;
   }
-
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
index 54aae9e..ff0542c 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuite.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
@@ -14,7 +11,6 @@
 import java.util.logging.Level;
 import java.util.logging.Logger;
 import java.util.regex.Pattern;
-
 import org.junit.Ignore;
 import org.junit.Test;
 import org.junit.runner.Request;
@@ -40,8 +36,8 @@
     SuiteClasses annotation = getClass().getAnnotation(SuiteClasses.class);
     List<Class<?>> classes = new ArrayList<>();
     if (annotation == null) {
-      throw new RuntimeException(String.format("class '%s' must have a SuiteClasses annotation",
-          getClass().getName()));
+      throw new RuntimeException(
+          String.format("class '%s' must have a SuiteClasses annotation", getClass().getName()));
     }
     for (Class<?> c : annotation.value()) {
       classes.add(c);
@@ -63,7 +59,7 @@
    * Stores a method name and method class pair. Used when searching for methods matching a given
    * regex text.
    */
-  protected class ClassMethodPair {
+  protected static class ClassMethodPair {
     public final Class<?> m_methodClass;
     public final String m_methodName;
 
@@ -84,7 +80,8 @@
       for (Method m : c.getMethods()) {
         // If this is a test method that is not trying to be ignored and it
         // matches the regex
-        if (m.getAnnotation(Test.class) != null && m.getAnnotation(Ignore.class) == null
+        if (m.getAnnotation(Test.class) != null
+            && m.getAnnotation(Ignore.class) == null
             && Pattern.matches(regex, m.getName())) {
           ClassMethodPair pair = new ClassMethodPair(c, m);
           classMethodPairs.add(pair);
@@ -94,7 +91,6 @@
     return classMethodPairs;
   }
 
-
   /**
    * Gets all of the test classes listed in this suite. Does not include any of the test suites. All
    * of these classes contain tests.
@@ -112,16 +108,20 @@
           // Add the tests from this suite that match the regex to the list of
           // tests to run
           runningList = suite.getAllContainedBaseTests(runningList);
-        } catch (NoSuchMethodException | InvocationTargetException | InstantiationException
+        } catch (NoSuchMethodException
+            | InvocationTargetException
+            | InstantiationException
             | IllegalAccessException ex) {
           // This shouldn't happen unless the constructor is changed in some
           // way.
-          logger.log(Level.SEVERE, "Test suites can not take parameters in their constructors.",
-              ex);
+          logger.log(
+              Level.SEVERE, "Test suites can not take parameters in their constructors.", ex);
         }
       } else if (c.getAnnotation(SuiteClasses.class) != null) {
-        logger.log(Level.SEVERE,
-            String.format("class '%s' must extend %s to be searchable using regex.",
+        logger.log(
+            Level.SEVERE,
+            String.format(
+                "class '%s' must extend %s to be searchable using regex.",
                 c.getName(), AbstractTestSuite.class.getName()));
       } else { // This is a class containing tests
         // so add it to the list
@@ -142,12 +142,11 @@
     return getAllContainedBaseTests(runningBaseTests);
   }
 
-
   /**
    * Retrieves all of the classes listed in the <code>@SuiteClasses</code> annotation that match the
    * given regex text.
    *
-   * @param regex       the text pattern to retrieve.
+   * @param regex the text pattern to retrieve.
    * @param runningList the running list of classes to prevent recursion
    * @return The list of classes matching the regex pattern
    */
@@ -181,12 +180,11 @@
    * @param regex the regex text to search for
    * @return the list of suite and/or test classes matching the regex.
    */
-  private List<Class<?>> getSuiteOrTestMatchingRegex(final String regex, List<Class<?>>
-      runningList) {
+  private List<Class<?>> getSuiteOrTestMatchingRegex(
+      final String regex, List<Class<?>> runningList) {
     // Get any test suites matching the regex using the superclass methods
     runningList = getAllClassMatching(regex, runningList);
 
-
     // Then search any test suites not retrieved already for test classes
     // matching the regex.
     List<Class<?>> unCollectedSuites = getAllClasses();
@@ -208,12 +206,14 @@
             runningList = suite.getSuiteOrTestMatchingRegex(regex, runningList);
           }
 
-        } catch (NoSuchMethodException | InvocationTargetException | InstantiationException
+        } catch (NoSuchMethodException
+            | InvocationTargetException
+            | InstantiationException
             | IllegalAccessException ex) {
           // This shouldn't happen unless the constructor is changed in some
           // way.
-          logger.log(Level.SEVERE, "Test suites can not take parameters in their constructors.",
-              ex);
+          logger.log(
+              Level.SEVERE, "Test suites can not take parameters in their constructors.", ex);
         }
       }
     }
@@ -233,7 +233,6 @@
     return getSuiteOrTestMatchingRegex(regex, matchingClasses);
   }
 
-
   /**
    * Retrieves all of the classes listed in the <code>@SuiteClasses</code> annotation.
    *
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuiteTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuiteTest.java
index d788b81..c41ff8a 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuiteTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AbstractTestSuiteTest.java
@@ -1,14 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
-import java.util.List;
+import static org.hamcrest.MatcherAssert.assertThat;
+import static org.hamcrest.Matchers.hasItems;
+import static org.hamcrest.Matchers.not;
+import static org.junit.Assert.assertEquals;
 
+import edu.wpi.first.wpilibj.test.AbstractTestSuite.ClassMethodPair;
+import java.util.List;
 import org.junit.Before;
 import org.junit.Ignore;
 import org.junit.Test;
@@ -16,13 +18,6 @@
 import org.junit.runners.Suite;
 import org.junit.runners.Suite.SuiteClasses;
 
-import edu.wpi.first.wpilibj.test.AbstractTestSuite.ClassMethodPair;
-
-import static org.hamcrest.MatcherAssert.assertThat;
-import static org.hamcrest.Matchers.hasItems;
-import static org.hamcrest.Matchers.not;
-import static org.junit.Assert.assertEquals;
-
 /**
  * Yes, this is the test system testing itself. Functionally, this is making sure that all tests get
  * run correctly when you use parametrized arguments.
@@ -31,10 +26,16 @@
 public class AbstractTestSuiteTest {
   @Ignore("Prevents ANT from trying to run these as tests")
   @RunWith(Suite.class)
-  @SuiteClasses({FirstSampleTest.class, SecondSampleTest.class, ThirdSampleTest.class,
-      FourthSampleTest.class, UnusualTest.class, ExampleSubSuite.class, EmptySuite.class})
-  class TestForAbstractTestSuite extends AbstractTestSuite {
-  }
+  @SuiteClasses({
+    FirstSampleTest.class,
+    SecondSampleTest.class,
+    ThirdSampleTest.class,
+    FourthSampleTest.class,
+    UnusualTest.class,
+    ExampleSubSuite.class,
+    EmptySuite.class
+  })
+  static class TestForAbstractTestSuite extends AbstractTestSuite {}
 
   TestForAbstractTestSuite m_testSuite;
 
@@ -82,10 +83,11 @@
     List<Class<?>> collectedTests = m_testSuite.getSuiteOrTestMatchingRegex(".*Test.*");
     // then
     assertEquals(7, collectedTests.size());
-    assertThat(collectedTests, hasItems(FirstSubSuiteTest.class,
-        SecondSubSuiteTest.class, UnusualTest.class));
-    assertThat(collectedTests,
-        not(hasItems(new Class<?>[]{ExampleSubSuite.class, EmptySuite.class})));
+    assertThat(
+        collectedTests,
+        hasItems(FirstSubSuiteTest.class, SecondSubSuiteTest.class, UnusualTest.class));
+    assertThat(
+        collectedTests, not(hasItems(new Class<?>[] {ExampleSubSuite.class, EmptySuite.class})));
   }
 
   @Test
@@ -96,31 +98,23 @@
     assertEquals(1, pairs.size());
     assertEquals(FirstSubSuiteTest.class, pairs.get(0).m_methodClass);
     assertEquals(FirstSubSuiteTest.METHODNAME, pairs.get(0).m_methodName);
-
   }
-
 }
 
 @SuppressWarnings("OneTopLevelClass")
-class FirstSampleTest {
-}
+class FirstSampleTest {}
 
 @SuppressWarnings("OneTopLevelClass")
-class SecondSampleTest {
-}
+class SecondSampleTest {}
 
 @SuppressWarnings("OneTopLevelClass")
-class ThirdSampleTest {
-}
+class ThirdSampleTest {}
 
 @SuppressWarnings("OneTopLevelClass")
-class FourthSampleTest {
-}
+class FourthSampleTest {}
 
 @SuppressWarnings("OneTopLevelClass")
-class UnusualTest {
-} // This is a member of both suites
-
+class UnusualTest {} // This is a member of both suites
 
 @Ignore("Prevents ANT from trying to run these as tests")
 @SuppressWarnings("OneTopLevelClass")
@@ -129,26 +123,20 @@
 
   @Test
   @SuppressWarnings("MethodName")
-  public void aTestMethod() {
-  }
+  public void aTestMethod() {}
 }
 
 @SuppressWarnings("OneTopLevelClass")
-class SecondSubSuiteTest {
-}
-
+class SecondSubSuiteTest {}
 
 @RunWith(Suite.class)
 @SuiteClasses({FirstSubSuiteTest.class, SecondSubSuiteTest.class, UnusualTest.class})
 @Ignore("Prevents ANT from trying to run these as tests")
 @SuppressWarnings("OneTopLevelClass")
-class ExampleSubSuite extends AbstractTestSuite {
-}
-
+class ExampleSubSuite extends AbstractTestSuite {}
 
 @Ignore("Prevents ANT from trying to run these as tests")
 @RunWith(Suite.class)
 @SuiteClasses({})
 @SuppressWarnings("OneTopLevelClass")
-class EmptySuite extends AbstractTestSuite {
-}
+class EmptySuite extends AbstractTestSuite {}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLanucher.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLanucher.java
deleted file mode 100644
index 0350ea4..0000000
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLanucher.java
+++ /dev/null
@@ -1,91 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.test;
-
-import java.io.File;
-
-import org.apache.tools.ant.BuildLogger;
-import org.apache.tools.ant.DefaultLogger;
-import org.apache.tools.ant.Project;
-import org.apache.tools.ant.taskdefs.optional.junit.FormatterElement;
-import org.apache.tools.ant.taskdefs.optional.junit.JUnitTask;
-import org.apache.tools.ant.taskdefs.optional.junit.JUnitTest;
-
-/**
- * Provides an entry point for tests to run with ANT. This allows ant to output JUnit XML test
- * results for Jenkins.
- */
-public class AntJunitLanucher {
-  /**
-   * Main entry point for jenkins.
-   *
-   * @param args Arguments passed to java.
-   */
-  public static void main(String... args) {
-    if (args.length == 0) {
-      String path =
-          String.format("%s/%s", System.getProperty("user.dir"), "/testResults/AntReports");
-      String pathToReports = path;
-      Project project = new Project();
-
-      try {
-        // Create the file to store the test output
-        new File(pathToReports).mkdirs();
-
-        project.setProperty("java.io.tmpdir", pathToReports);
-
-        /* Output to screen */
-        FormatterElement.TypeAttribute typeScreen = new FormatterElement.TypeAttribute();
-        typeScreen.setValue("plain");
-        FormatterElement formatToScreen = new FormatterElement();
-        formatToScreen.setType(typeScreen);
-        formatToScreen.setUseFile(false);
-        formatToScreen.setOutput(System.out);
-
-        JUnitTask task = new JUnitTask();
-        task.addFormatter(formatToScreen);
-
-        // add a build listener to the project
-        BuildLogger logger = new DefaultLogger();
-        logger.setOutputPrintStream(System.out);
-        logger.setErrorPrintStream(System.err);
-        logger.setMessageOutputLevel(Project.MSG_INFO);
-        logger.setEmacsMode(true);
-        project.addBuildListener(logger);
-
-        task.setProject(project);
-
-        // Set the output to the XML file
-        FormatterElement.TypeAttribute type = new FormatterElement.TypeAttribute();
-        type.setValue("xml");
-
-        FormatterElement formater = new FormatterElement();
-        formater.setType(type);
-        task.addFormatter(formater);
-
-        // Create the JUnitTest
-        JUnitTest test = new JUnitTest(TestSuite.class.getName());
-        test.setTodir(new File(pathToReports));
-        task.addTest(test);
-
-        TestBench.out().println("Beginning Test Execution With ANT");
-        task.execute();
-      } catch (Exception ex) {
-        ex.printStackTrace();
-      }
-    } else {
-      TestBench.out().println(
-          "Run will not output XML for Jenkins because " + "tests are not being run with ANT");
-      // This should never return as it makes its own call to
-      // System.exit();
-      TestSuite.main(args);
-    }
-    System.exit(0);
-  }
-
-}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java
new file mode 100644
index 0000000..4dfa6e1
--- /dev/null
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/AntJunitLauncher.java
@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.test;
+
+import java.io.File;
+import org.apache.tools.ant.BuildLogger;
+import org.apache.tools.ant.DefaultLogger;
+import org.apache.tools.ant.Project;
+import org.apache.tools.ant.taskdefs.optional.junit.FormatterElement;
+import org.apache.tools.ant.taskdefs.optional.junit.JUnitTask;
+import org.apache.tools.ant.taskdefs.optional.junit.JUnitTest;
+
+/**
+ * Provides an entry point for tests to run with ANT. This allows ant to output JUnit XML test
+ * results for Jenkins.
+ */
+public class AntJunitLauncher {
+  /**
+   * Main entry point for jenkins.
+   *
+   * @param args Arguments passed to java.
+   */
+  public static void main(String... args) {
+    if (args.length == 0) {
+      String path =
+          String.format("%s/%s", System.getProperty("user.dir"), "/testResults/AntReports");
+      String pathToReports = path;
+      Project project = new Project();
+
+      try {
+        // Create the file to store the test output
+        new File(pathToReports).mkdirs();
+
+        project.setProperty("java.io.tmpdir", pathToReports);
+
+        /* Output to screen */
+        FormatterElement.TypeAttribute typeScreen = new FormatterElement.TypeAttribute();
+        typeScreen.setValue("plain");
+        FormatterElement formatToScreen = new FormatterElement();
+        formatToScreen.setType(typeScreen);
+        formatToScreen.setUseFile(false);
+        formatToScreen.setOutput(System.out);
+
+        JUnitTask task = new JUnitTask();
+        task.addFormatter(formatToScreen);
+
+        // add a build listener to the project
+        BuildLogger logger = new DefaultLogger();
+        logger.setOutputPrintStream(System.out);
+        logger.setErrorPrintStream(System.err);
+        logger.setMessageOutputLevel(Project.MSG_INFO);
+        logger.setEmacsMode(true);
+        project.addBuildListener(logger);
+
+        task.setProject(project);
+
+        // Set the output to the XML file
+        FormatterElement.TypeAttribute type = new FormatterElement.TypeAttribute();
+        type.setValue("xml");
+
+        FormatterElement formater = new FormatterElement();
+        formater.setType(type);
+        task.addFormatter(formater);
+
+        // Create the JUnitTest
+        JUnitTest test = new JUnitTest(TestSuite.class.getName());
+        test.setTodir(new File(pathToReports));
+        task.addTest(test);
+
+        TestBench.out().println("Beginning Test Execution With ANT");
+        task.execute();
+      } catch (Exception ex) {
+        ex.printStackTrace();
+      }
+    } else {
+      TestBench.out()
+          .println(
+              "Run will not output XML for Jenkins because " + "tests are not being run with ANT");
+      // This should never return as it makes its own call to
+      // System.exit();
+      TestSuite.main(args);
+    }
+    System.exit(0);
+  }
+}
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/QuickTest.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/QuickTest.java
index 87b05d0..3dd2ea1 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/QuickTest.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/QuickTest.java
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
 import java.util.logging.Logger;
-
 import org.junit.Test;
 
 /**
@@ -30,8 +26,6 @@
     return logger;
   }
 
-
   @Test
-  public void test() {
-  }
+  public void test() {}
 }
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/RepeatRule.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/RepeatRule.java
index deb0da8..07f41f2 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/RepeatRule.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/RepeatRule.java
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
 import java.lang.annotation.Retention;
 import java.lang.annotation.RetentionPolicy;
 import java.lang.annotation.Target;
-
 import org.junit.rules.TestRule;
 import org.junit.runner.Description;
 import org.junit.runners.model.Statement;
@@ -20,21 +16,18 @@
  * is important if you have a test that fails only "sometimes" and needs to be rerun to find the
  * issue.
  *
- * <p>This code was originally found here:
- * <a href="http://www.codeaffine.com/2013/04/10/running-junit-tests-repeatedly-without-loops/">
- * Running JUnit Tests Repeatedly Without Loops</a>
+ * <p>This code was originally found here: <a
+ * href="http://www.codeaffine.com/2013/04/10/running-junit-tests-repeatedly-without-loops/">Running
+ * JUnit Tests Repeatedly Without Loops</a>
  */
 public class RepeatRule implements TestRule {
   @Retention(RetentionPolicy.RUNTIME)
   @Target({java.lang.annotation.ElementType.METHOD})
   public @interface Repeat {
-    /**
-     * The number of times to repeat the test.
-     */
+    /** The number of times to repeat the test. */
     int times();
   }
 
-
   private static class RepeatStatement extends Statement {
     private final int m_times;
     private final Statement m_statement;
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
index afd152f..8e63dc9 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java
@@ -1,38 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
+import edu.wpi.first.wpilibj.AnalogGyro;
+import edu.wpi.first.wpilibj.AnalogInput;
+import edu.wpi.first.wpilibj.AnalogOutput;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Relay;
+import edu.wpi.first.wpilibj.Servo;
+import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
+import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
+import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
+import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture;
+import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture;
+import edu.wpi.first.wpilibj.motorcontrol.Jaguar;
+import edu.wpi.first.wpilibj.motorcontrol.Talon;
+import edu.wpi.first.wpilibj.motorcontrol.Victor;
 import java.io.PrintStream;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.Collection;
 import java.util.List;
 
-import edu.wpi.first.wpilibj.AnalogGyro;
-import edu.wpi.first.wpilibj.AnalogInput;
-import edu.wpi.first.wpilibj.AnalogOutput;
-import edu.wpi.first.wpilibj.DigitalInput;
-import edu.wpi.first.wpilibj.Jaguar;
-import edu.wpi.first.wpilibj.Relay;
-import edu.wpi.first.wpilibj.Servo;
-import edu.wpi.first.wpilibj.Talon;
-import edu.wpi.first.wpilibj.Victor;
-import edu.wpi.first.wpilibj.fixtures.AnalogCrossConnectFixture;
-import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
-import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
-import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture;
-import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture;
-
-/**
- * This class provides access to all of the elements on the test bench, for use in fixtures. This
- * class is a singleton, you should use {@link #getInstance()} to obtain a reference to the {@code
- * TestBench}.
- */
+/** This class provides access to all of the elements on the test bench, for use in fixtures. */
 public final class TestBench {
   /**
    * The time that it takes to have a motor go from rotating at full speed to completely stopped.
@@ -49,8 +41,7 @@
   public static final int kTiltServoChannel = 9;
   public static final int kPanServoChannel = 8;
 
-
-  /* PowerDistributionPanel channels */
+  /* PowerDistribution channels */
   public static final int kJaguarPDPChannel = 6;
   public static final int kVictorPDPChannel = 8;
   public static final int kTalonPDPChannel = 10;
@@ -61,17 +52,14 @@
   public static final int DIOCrossConnectA2 = 7;
   public static final int DIOCrossConnectA1 = 6;
 
-  /**
-   * The Singleton instance of the Test Bench.
-   */
+  /** The Singleton instance of the Test Bench. */
   private static TestBench instance = null;
 
   /**
    * The single constructor for the TestBench. This method is private in order to prevent multiple
    * TestBench objects from being allocated.
    */
-  protected TestBench() {
-  }
+  protected TestBench() {}
 
   /**
    * Constructs a new set of objects representing a connected set of Talon controlled Motors and an
@@ -79,10 +67,10 @@
    *
    * @return a freshly allocated Talon, Encoder pair
    */
-  public MotorEncoderFixture<Talon> getTalonPair() {
+  public static MotorEncoderFixture<Talon> getTalonPair() {
     return new MotorEncoderFixture<Talon>() {
       @Override
-      protected Talon giveSpeedController() {
+      protected Talon giveMotorController() {
         return new Talon(kTalonChannel);
       }
 
@@ -109,10 +97,10 @@
    *
    * @return a freshly allocated Victor, Encoder pair
    */
-  public MotorEncoderFixture<Victor> getVictorPair() {
+  public static MotorEncoderFixture<Victor> getVictorPair() {
     return new MotorEncoderFixture<Victor>() {
       @Override
-      protected Victor giveSpeedController() {
+      protected Victor giveMotorController() {
         return new Victor(kVictorChannel);
       }
 
@@ -139,10 +127,10 @@
    *
    * @return a freshly allocated Jaguar, Encoder pair
    */
-  public MotorEncoderFixture<Jaguar> getJaguarPair() {
+  public static MotorEncoderFixture<Jaguar> getJaguarPair() {
     return new MotorEncoderFixture<Jaguar>() {
       @Override
-      protected Jaguar giveSpeedController() {
+      protected Jaguar giveMotorController() {
         return new Jaguar(kJaguarChannel);
       }
 
@@ -168,8 +156,7 @@
    *
    * @return a freshly allocated Servo's and a freshly allocated Gyroscope
    */
-  public TiltPanCameraFixture getTiltPanCam() {
-
+  public static TiltPanCameraFixture getTiltPanCam() {
     return new TiltPanCameraFixture() {
       @Override
       protected AnalogGyro giveGyro() {
@@ -197,31 +184,33 @@
     };
   }
 
-  public DIOCrossConnectFixture getDIOCrossConnectFixture(int inputPort, int outputPort) {
+  public static DIOCrossConnectFixture getDIOCrossConnectFixture(int inputPort, int outputPort) {
     return new DIOCrossConnectFixture(inputPort, outputPort);
   }
 
-  /**
-   * Gets two lists of possible DIO pairs for the two pairs.
-   */
-  private List<List<Integer[]>> getDIOCrossConnect() {
+  /** Gets two lists of possible DIO pairs for the two pairs. */
+  private static List<List<Integer[]>> getDIOCrossConnect() {
     List<List<Integer[]>> pairs = new ArrayList<List<Integer[]>>();
     List<Integer[]> setA =
-        Arrays.asList(new Integer[][]{
-            {DIOCrossConnectA1, DIOCrossConnectA2},
-            {DIOCrossConnectA2, DIOCrossConnectA1}});
+        Arrays.asList(
+            new Integer[][] {
+              {DIOCrossConnectA1, DIOCrossConnectA2},
+              {DIOCrossConnectA2, DIOCrossConnectA1}
+            });
     pairs.add(setA);
 
     List<Integer[]> setB =
-        Arrays.asList(new Integer[][]{
-            {DIOCrossConnectB1, DIOCrossConnectB2},
-            {DIOCrossConnectB2, DIOCrossConnectB1}});
+        Arrays.asList(
+            new Integer[][] {
+              {DIOCrossConnectB1, DIOCrossConnectB2},
+              {DIOCrossConnectB2, DIOCrossConnectB1}
+            });
     pairs.add(setB);
     // NOTE: IF MORE DIOCROSSCONNECT PAIRS ARE ADDED ADD THEM HERE
     return pairs;
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static AnalogCrossConnectFixture getAnalogCrossConnectFixture() {
     return new AnalogCrossConnectFixture() {
       @Override
@@ -236,7 +225,7 @@
     };
   }
 
-  @SuppressWarnings("JavadocMethod")
+  @SuppressWarnings("MissingJavadocMethod")
   public static RelayCrossConnectFixture getRelayCrossConnectFixture() {
     return new RelayCrossConnectFixture() {
       @Override
@@ -262,7 +251,7 @@
    *
    * @return pairs of DIOCrossConnectFixtures
    */
-  public Collection<Integer[]> getDIOCrossConnectCollection() {
+  public static Collection<Integer[]> getDIOCrossConnectCollection() {
     Collection<Integer[]> pairs = new ArrayList<Integer[]>();
     for (Collection<Integer[]> collection : getDIOCrossConnect()) {
       pairs.addAll(collection);
@@ -276,8 +265,8 @@
    * @param flip whether this encoder needs to be flipped
    * @return A list of different inputs to use for the tests
    */
-  private Collection<Integer[]> getPairArray(List<Integer[]> listA, List<Integer[]> listB,
-                                             boolean flip) {
+  private static Collection<Integer[]> getPairArray(
+      List<Integer[]> listA, List<Integer[]> listB, boolean flip) {
     Collection<Integer[]> encoderPortPairs = new ArrayList<Integer[]>();
     for (Integer[] portPairsA : listA) {
       Integer[] inputs = new Integer[5];
@@ -310,13 +299,13 @@
    *
    * @return A collection of different input pairs to use for the encoder
    */
-  public Collection<Integer[]> getEncoderDIOCrossConnectCollection() {
+  public static Collection<Integer[]> getEncoderDIOCrossConnectCollection() {
     Collection<Integer[]> encoderPortPairs = new ArrayList<Integer[]>();
     assert getDIOCrossConnect().size() == 2;
-    encoderPortPairs.addAll(getPairArray(getDIOCrossConnect().get(0), getDIOCrossConnect().get(1),
-        false));
-    encoderPortPairs.addAll(getPairArray(getDIOCrossConnect().get(1), getDIOCrossConnect().get(0),
-        false));
+    encoderPortPairs.addAll(
+        getPairArray(getDIOCrossConnect().get(0), getDIOCrossConnect().get(1), false));
+    encoderPortPairs.addAll(
+        getPairArray(getDIOCrossConnect().get(1), getDIOCrossConnect().get(0), false));
     assert encoderPortPairs.size() == 8;
     return encoderPortPairs;
   }
@@ -326,7 +315,9 @@
    * new instance of it. Otherwise it returns the existing instance.
    *
    * @return The Singleton instance of the TestBench
+   * @deprecated Use the static methods instead
    */
+  @Deprecated
   public static TestBench getInstance() {
     if (instance == null) {
       instance = new TestBench();
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java
index 31b3b0d..880a97c 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestSuite.java
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj.test;
 
+import edu.wpi.first.wpilibj.WpiLibJTestSuite;
 import java.io.IOException;
 import java.io.InputStream;
 import java.util.ArrayList;
@@ -16,7 +14,8 @@
 import java.util.logging.LogManager;
 import java.util.logging.Logger;
 import java.util.regex.Pattern;
-
+import junit.framework.JUnit4TestAdapter;
+import junit.runner.Version;
 import org.junit.runner.JUnitCore;
 import org.junit.runner.Result;
 import org.junit.runner.RunWith;
@@ -24,11 +23,6 @@
 import org.junit.runners.Suite;
 import org.junit.runners.Suite.SuiteClasses;
 
-import junit.framework.JUnit4TestAdapter;
-import junit.runner.Version;
-
-import edu.wpi.first.wpilibj.WpiLibJTestSuite;
-
 /**
  * The WPILibJ Integeration Test Suite collects all of the tests to be run by junit. In order for a
  * test to be run, it must be added the list of suite classes below. The tests will be run in the
@@ -48,14 +42,18 @@
       Logger.getAnonymousLogger().severe("Could not load default logging.properties file");
       Logger.getAnonymousLogger().severe(ex.getMessage());
     }
+    try {
+      inputStream.close();
+    } catch (IOException e) {
+      throw new RuntimeException(e.getMessage());
+    }
 
     TestBench.out().println("Starting Tests");
   }
 
   private static final Logger WPILIBJ_ROOT_LOGGER = Logger.getLogger("edu.wpi.first.wpilibj");
-  private static final Logger WPILIBJ_COMMAND_ROOT_LOGGER = Logger
-      .getLogger("edu.wpi.first.wpilibj.command");
-
+  private static final Logger WPILIBJ_COMMAND_ROOT_LOGGER =
+      Logger.getLogger("edu.wpi.first.wpilibj.command");
 
   private static final Class<?> QUICK_TEST = QuickTest.class;
   private static final String QUICK_TEST_FLAG = "--quick";
@@ -66,9 +64,7 @@
 
   private static TestSuite instance = null;
 
-  /**
-   * Get the singleton instance of the test suite.
-   */
+  /** Get the singleton instance of the test suite. */
   public static TestSuite getInstance() {
     if (instance == null) {
       instance = new TestSuite();
@@ -76,32 +72,41 @@
     return instance;
   }
 
-  /**
-   * This has to be public so that the JUnit4.
-   */
-  public TestSuite() {
-  }
+  /** This has to be public so that the JUnit4. */
+  public TestSuite() {}
 
-  /**
-   * Displays a help message for the user when they use the --help flag at runtime.
-   */
+  /** Displays a help message for the user when they use the --help flag at runtime. */
   protected static void displayHelp() {
     StringBuilder helpMessage = new StringBuilder("Test Parameters help: \n");
-    helpMessage.append("\t" + QUICK_TEST_FLAG
-        + " will cause the quick test to be run. Ignores other flags except for "
-        + METHOD_REPEAT_FILTER + "\n");
-    helpMessage.append("\t" + CLASS_NAME_FILTER
-        + " will use the supplied regex text to search for suite/test class names "
-        + "matching the regex and run them.\n");
-    helpMessage.append("\t" + METHOD_NAME_FILTER
-        + " will use the supplied regex text to search for test methods (excluding methods "
-        + "with the @Ignore annotation) and run only those methods. Can be paired with "
-        + METHOD_REPEAT_FILTER + " to " + "repeat the selected tests multiple times.\n");
-    helpMessage.append("\t" + METHOD_REPEAT_FILTER + " will repeat the tests selected with either "
-        + QUICK_TEST_FLAG + " or " + CLASS_NAME_FILTER
-        + " and run them the given number of times.\n");
-    helpMessage
-        .append("[NOTE] All regex uses the syntax defined by java.util.regex.Pattern. This "
+    helpMessage.append(
+        "\t"
+            + QUICK_TEST_FLAG
+            + " will cause the quick test to be run. Ignores other flags except for "
+            + METHOD_REPEAT_FILTER
+            + "\n");
+    helpMessage.append(
+        "\t"
+            + CLASS_NAME_FILTER
+            + " will use the supplied regex text to search for suite/test class names "
+            + "matching the regex and run them.\n");
+    helpMessage.append(
+        "\t"
+            + METHOD_NAME_FILTER
+            + " will use the supplied regex text to search for test methods (excluding methods "
+            + "with the @Ignore annotation) and run only those methods. Can be paired with "
+            + METHOD_REPEAT_FILTER
+            + " to "
+            + "repeat the selected tests multiple times.\n");
+    helpMessage.append(
+        "\t"
+            + METHOD_REPEAT_FILTER
+            + " will repeat the tests selected with either "
+            + QUICK_TEST_FLAG
+            + " or "
+            + CLASS_NAME_FILTER
+            + " and run them the given number of times.\n");
+    helpMessage.append(
+        "[NOTE] All regex uses the syntax defined by java.util.regex.Pattern. This "
             + "documentation can be found at "
             + "http://docs.oracle.com/javase/7/docs/api/java/util/regex/Pattern.html\n");
     helpMessage.append("\n");
@@ -121,12 +126,12 @@
       invalidMessage.append(a + " ");
     }
     invalidMessage.append("\n");
-    invalidMessage
-        .append("For details on proper usage of the runtime flags please run again with the "
-            + HELP_FLAG + " flag.\n\n");
+    invalidMessage.append(
+        "For details on proper usage of the runtime flags please run again with the "
+            + HELP_FLAG
+            + " flag.\n\n");
 
     TestBench.out().println(invalidMessage);
-
   }
 
   /**
@@ -147,7 +152,6 @@
     TestBench.out().println(loadedTestsMessage);
   }
 
-
   /**
    * Parses the arguments passed at runtime and runs the appropriate tests based upon these
    * arguments.
@@ -173,14 +177,14 @@
     for (String s : args) {
       if (Pattern.matches(METHOD_NAME_FILTER + ".*", s)) {
         methodFilter = true;
-        methodRegex = new String(s).replace(METHOD_NAME_FILTER, "");
+        methodRegex = s.replace(METHOD_NAME_FILTER, "");
       }
       if (Pattern.matches(METHOD_REPEAT_FILTER + ".*", s)) {
         try {
-          repeatCount = Integer.parseInt(new String(s).replace(METHOD_REPEAT_FILTER, ""));
+          repeatCount = Integer.parseInt(s.replace(METHOD_REPEAT_FILTER, ""));
         } catch (NumberFormatException ex) {
-          displayInvalidUsage("The argument passed to the repeat rule was not a valid integer.",
-              args);
+          displayInvalidUsage(
+              "The argument passed to the repeat rule was not a valid integer.", args);
         }
       }
       if (Pattern.matches(CLASS_NAME_FILTER + ".*", s)) {
@@ -189,7 +193,6 @@
       }
     }
 
-
     ArrayList<String> argsParsed = new ArrayList<String>(Arrays.asList(args));
     if (argsParsed.contains(HELP_FLAG)) {
       // If the user inputs the help flag then return the help message and exit
@@ -203,8 +206,8 @@
     }
 
     /**
-     * Stores the data from multiple {@link Result}s in one class that can be
-     * returned to display the results.
+     * Stores the data from multiple {@link Result}s in one class that can be returned to display
+     * the results.
      */
     class MultipleResult extends Result {
       private static final long serialVersionUID = 1L;
@@ -255,8 +258,8 @@
     if (methodFilter) {
       List<ClassMethodPair> pairs = (new TestSuite()).getMethodMatching(methodRegex);
       if (pairs.size() == 0) {
-        displayInvalidUsage("None of the arguments passed to the method name filter matched.",
-            args);
+        displayInvalidUsage(
+            "None of the arguments passed to the method name filter matched.", args);
         return null;
       }
       // Print out the list of tests before we run them
@@ -271,7 +274,6 @@
         TestBench.out().println("\t" + p.m_methodName);
       }
 
-
       // The test results will be saved here
       MultipleResult results = new MultipleResult();
       // Runs tests multiple times if the repeat rule is used
@@ -323,9 +325,17 @@
       TestBench.out().println();
       TestBench.out().println("FAILURES!!!");
       // Print the test statistics
-      TestBench.out().println(
-          "Tests run: " + result.getRunCount() + ", Failures: " + result.getFailureCount()
-              + ", Ignored: " + result.getIgnoreCount() + ", In " + result.getRunTime() + "ms");
+      TestBench.out()
+          .println(
+              "Tests run: "
+                  + result.getRunCount()
+                  + ", Failures: "
+                  + result.getFailureCount()
+                  + ", Ignored: "
+                  + result.getIgnoreCount()
+                  + ", In "
+                  + result.getRunTime()
+                  + "ms");
 
       // Prints out a list of test that failed
       TestBench.out().println("Failure List (short):");
@@ -340,9 +350,15 @@
       }
     } else {
       TestBench.out().println("SUCCESS!");
-      TestBench.out().println(
-          "Tests run: " + result.getRunCount() + ", Ignored: " + result.getIgnoreCount() + ", In "
-              + result.getRunTime() + "ms");
+      TestBench.out()
+          .println(
+              "Tests run: "
+                  + result.getRunCount()
+                  + ", Ignored: "
+                  + result.getIgnoreCount()
+                  + ", In "
+                  + result.getRunTime()
+                  + "ms");
     }
     TestBench.out().println();
   }
@@ -356,7 +372,6 @@
     return new JUnit4TestAdapter(TestSuite.class);
   }
 
-
   /**
    * The method called at runtime.
    *
diff --git a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/package-info.java b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/package-info.java
index 481cb08..8ce4285 100644
--- a/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/package-info.java
+++ b/third_party/allwpilib/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/package-info.java
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /**
  * This is the starting point for the integration testing framework. This package should contain
  * classes that are not explicitly for testing the library but instead provide the framework that
- * the tests can extend from. Every test should extend from
- * {@link edu.wpi.first.wpilibj.test.AbstractComsSetup}
- * to ensure that Network Communications is properly instantiated before the test is run. The
- * {@link edu.wpi.first.wpilibj.test.TestBench} should contain the port numbers for all of the
- * hardware and these values should not be explicitly defined anywhere else in the testing
- * framework.
+ * the tests can extend from. Every test should extend from {@link
+ * edu.wpi.first.wpilibj.test.AbstractComsSetup} to ensure that Network Communications is properly
+ * instantiated before the test is run. The {@link edu.wpi.first.wpilibj.test.TestBench} should
+ * contain the port numbers for all of the hardware and these values should not be explicitly
+ * defined anywhere else in the testing framework.
  */
 package edu.wpi.first.wpilibj.test;
diff --git a/third_party/allwpilib/wpimath/.styleguide b/third_party/allwpilib/wpimath/.styleguide
index c82141b..b9044c9 100644
--- a/third_party/allwpilib/wpimath/.styleguide
+++ b/third_party/allwpilib/wpimath/.styleguide
@@ -8,6 +8,10 @@
   \.cpp$
 }
 
+modifiableFileExclude {
+  \.patch$
+}
+
 generatedFileExclude {
   src/main/native/cpp/drake/
   src/main/native/eigeninclude/
@@ -31,6 +35,7 @@
 }
 
 includeOtherLibs {
+  ^fmt/
   ^wpi/
 }
 
diff --git a/third_party/allwpilib/wpimath/CMakeLists.txt b/third_party/allwpilib/wpimath/CMakeLists.txt
index a129376..2a4090f 100644
--- a/third_party/allwpilib/wpimath/CMakeLists.txt
+++ b/third_party/allwpilib/wpimath/CMakeLists.txt
@@ -11,11 +11,11 @@
   find_package(Java REQUIRED)
   find_package(JNI REQUIRED)
   include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.38.jar")
+  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/ejml-simple-0.38.jar")
       set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
-      set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml")
+      set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml")
 
       message(STATUS "Downloading EJML jarfiles...")
 
@@ -37,15 +37,15 @@
       message(STATUS "All files downloaded.")
   endif()
 
-  file(GLOB EJML_JARS "${CMAKE_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
-  file(GLOB JACKSON_JARS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
+  file(GLOB EJML_JARS "${WPILIB_BINARY_DIR}/wpimath/thirdparty/ejml/*.jar")
+  file(GLOB JACKSON_JARS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar")
 
   set(CMAKE_JAVA_INCLUDE_PATH wpimath.jar ${EJML_JARS} ${JACKSON_JARS})
 
-  execute_process(COMMAND python3 ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
+  execute_process(COMMAND python3 ${CMAKE_CURRENT_SOURCE_DIR}/generate_numbers.py ${WPILIB_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
   if(NOT (generateResult EQUAL "0"))
     # Try python
-    execute_process(COMMAND python ${CMAKE_SOURCE_DIR}/wpimath/generate_numbers.py ${CMAKE_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
+    execute_process(COMMAND python ${CMAKE_CURRENT_SOURCE_DIR}/generate_numbers.py ${WPILIB_BINARY_DIR}/wpimath RESULT_VARIABLE generateResult)
     if(NOT (generateResult EQUAL "0"))
       message(FATAL_ERROR "python and python3 generate_numbers.py failed")
     endif()
@@ -53,7 +53,7 @@
 
   set(CMAKE_JNI_TARGET true)
 
-  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${CMAKE_BINARY_DIR}/wpimath/generated/*.java)
+  file(GLOB_RECURSE JAVA_SOURCES src/main/java/*.java ${WPILIB_BINARY_DIR}/wpimath/generated/*.java)
 
   if(${CMAKE_VERSION} VERSION_LESS "3.11.0")
     set(CMAKE_JAVA_COMPILE_FLAGS "-h" "${CMAKE_CURRENT_BINARY_DIR}/jniheaders")
@@ -92,10 +92,13 @@
 file(GLOB_RECURSE wpimath_native_src src/main/native/cpp/*.cpp)
 list(REMOVE_ITEM wpimath_native_src ${wpimath_jni_src})
 
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS FALSE)
 add_library(wpimath ${wpimath_native_src})
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS TRUE)
 set_target_properties(wpimath PROPERTIES DEBUG_POSTFIX "d")
 
 set_property(TARGET wpimath PROPERTY FOLDER "libraries")
+target_compile_definitions(wpimath PRIVATE WPILIB_EXPORTS)
 
 target_compile_features(wpimath PUBLIC cxx_std_17)
 if (MSVC)
@@ -106,7 +109,7 @@
 
 if (NOT USE_VCPKG_EIGEN)
     install(DIRECTORY src/main/native/eigeninclude/ DESTINATION "${include_dest}/wpimath")
-    target_include_directories(wpimath PUBLIC
+    target_include_directories(wpimath SYSTEM PUBLIC
                             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/eigeninclude>
                             $<INSTALL_INTERFACE:${include_dest}/wpimath>)
 else()
@@ -131,8 +134,8 @@
     set (wpimath_config_dir share/wpimath)
 endif()
 
-configure_file(wpimath-config.cmake.in ${CMAKE_BINARY_DIR}/wpimath-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/wpimath-config.cmake DESTINATION ${wpimath_config_dir})
+configure_file(wpimath-config.cmake.in ${WPILIB_BINARY_DIR}/wpimath-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/wpimath-config.cmake DESTINATION ${wpimath_config_dir})
 install(EXPORT wpimath DESTINATION ${wpimath_config_dir})
 
 if (WITH_TESTS)
diff --git a/third_party/allwpilib/wpimath/build.gradle b/third_party/allwpilib/wpimath/build.gradle
index c023a1a..79db4bc 100644
--- a/third_party/allwpilib/wpimath/build.gradle
+++ b/third_party/allwpilib/wpimath/build.gradle
@@ -1,3 +1,6 @@
+import com.hubspot.jinjava.Jinjava;
+import com.hubspot.jinjava.JinjavaConfig;
+
 ext {
     useJava = true
     useCpp = true
@@ -5,24 +8,11 @@
     groupId = 'edu.wpi.first.wpimath'
 
     nativeName = 'wpimath'
-    devMain = 'edu.wpi.first.wpiutil.math.DevMain'
+    devMain = 'edu.wpi.first.math.DevMain'
 }
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
-nativeUtils.exportsConfigs {
-    wpimath {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-    }
-}
-
 cppHeadersZip {
     from('src/main/native/eigeninclude') {
         into '/'
@@ -48,11 +38,10 @@
     api "com.fasterxml.jackson.core:jackson-databind:2.10.0"
 }
 
-def wpilibNumberFileInput = file("src/generate/GenericNumber.java.in")
-def natFileInput = file("src/generate/Nat.java.in")
-def natGetterInput = file("src/generate/NatGetter.java.in")
-def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/numbers")
-def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/wpiutil/math/Nat.java")
+def wpilibNumberFileInput = file("src/generate/GenericNumber.java.jinja")
+def natFileInput = file("src/generate/Nat.java.jinja")
+def wpilibNumberFileOutputDir = file("$buildDir/generated/java/edu/wpi/first/math/numbers")
+def wpilibNatFileOutput = file("$buildDir/generated/java/edu/wpi/first/math/Nat.java")
 def maxNum = 20
 
 task generateNumbers() {
@@ -68,10 +57,17 @@
         }
         wpilibNumberFileOutputDir.mkdirs()
 
+        def config = new JinjavaConfig()
+        def jinjava = new Jinjava(config)
+
+        def template = wpilibNumberFileInput.text
+
         for(i in 0..maxNum) {
             def outputFile = new File(wpilibNumberFileOutputDir, "N${i}.java")
-            def read = wpilibNumberFileInput.text.replace('${num}', i.toString())
-            outputFile.write(read)
+            def replacements = new HashMap<String,?>()
+            replacements.put("num", i)
+            def output = jinjava.render(template, replacements)
+            outputFile.write(output)
         }
     }
 }
@@ -79,7 +75,7 @@
 task generateNat() {
     description = "Generates Nat.java"
     group = "WPILib"
-    inputs.files([natFileInput, natGetterInput])
+    inputs.file natFileInput
     outputs.file wpilibNatFileOutput
     dependsOn generateNumbers
 
@@ -88,19 +84,16 @@
             wpilibNatFileOutput.delete()
         }
 
-        def template = natFileInput.text + "\n"
+        def config = new JinjavaConfig()
+        def jinjava = new Jinjava(config)
 
-        def importsString = "";
+        def template = natFileInput.text
 
-        for(i in 0..maxNum) {
-            importsString += "import edu.wpi.first.wpiutil.math.numbers.N${i};\n"
-            template += natGetterInput.text.replace('${num}', i.toString()) + "\n"
-        }
-        template += "}\n" // Close the class body
+        def replacements = new HashMap<String,?>()
+        replacements.put("nums", 0..maxNum)
 
-        template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
-
-        wpilibNatFileOutput.write(template)
+        def output = jinjava.render(template, replacements)
+        wpilibNatFileOutput.write(output)
     }
 }
 
diff --git a/third_party/allwpilib/wpimath/generate_numbers.py b/third_party/allwpilib/wpimath/generate_numbers.py
index 701f3e6..c52ddb4 100644
--- a/third_party/allwpilib/wpimath/generate_numbers.py
+++ b/third_party/allwpilib/wpimath/generate_numbers.py
@@ -1,5 +1,26 @@
+# Copyright (c) FIRST and other WPILib contributors.
+# Open Source Software; you can modify and/or share it under the terms of
+# the WPILib BSD license file in the root directory of this project.
+
 import os
 import sys
+from jinja2 import Environment, FileSystemLoader
+
+
+def output(outPath, outfn, contents):
+    if not os.path.exists(outPath):
+        os.makedirs(outPath)
+
+    outpathname = f"{outPath}/{outfn}"
+
+    if os.path.exists(outpathname):
+        with open(outpathname, "r") as f:
+            if f.read() == contents:
+                return
+
+    # File either doesn't exist or has different contents
+    with open(outpathname, "w") as f:
+        f.write(contents)
 
 
 def main():
@@ -8,51 +29,21 @@
     dirname, _ = os.path.split(os.path.abspath(__file__))
     cmake_binary_dir = sys.argv[1]
 
-    with open(f"{dirname}/src/generate/GenericNumber.java.in",
-              "r") as templateFile:
-        template = templateFile.read()
-        rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/numbers"
+    env = Environment(loader=FileSystemLoader(f"{dirname}/src/generate"),
+                      autoescape=False,
+                      keep_trailing_newline=True)
 
-        if not os.path.exists(rootPath):
-            os.makedirs(rootPath)
+    template = env.get_template("GenericNumber.java.jinja")
+    rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/math/numbers"
 
-        for i in range(MAX_NUM + 1):
-            new_contents = template.replace("${num}", str(i))
+    for i in range(MAX_NUM + 1):
+        contents = template.render(num=i)
+        output(rootPath, f"N{i}.java", contents)
 
-            if os.path.exists(f"{rootPath}/N{i}.java"):
-                with open(f"{rootPath}/N{i}.java", "r") as f:
-                    if f.read() == new_contents:
-                        continue
-
-            # File either doesn't exist or has different contents
-            with open(f"{rootPath}/N{i}.java", "w") as f:
-                f.write(new_contents)
-
-    with open(f"{dirname}/src/generate/Nat.java.in", "r") as templateFile:
-        template = templateFile.read()
-        outputPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/wpiutil/math/Nat.java"
-        with open(f"{dirname}/src/generate/NatGetter.java.in",
-                  "r") as getterFile:
-            getter = getterFile.read()
-
-        importsString = ""
-
-        for i in range(MAX_NUM + 1):
-            importsString += f"import edu.wpi.first.wpiutil.math.numbers.N{i};\n"
-            template += getter.replace("${num}", str(i))
-
-        template += "}\n"
-
-        template = template.replace('{{REPLACEWITHIMPORTS}}', importsString)
-
-        if os.path.exists(outputPath):
-            with open(outputPath, "r") as f:
-                if f.read() == template:
-                    return 0
-
-        # File either doesn't exist or has different contents
-        with open(outputPath, "w") as f:
-            f.write(template)
+    template = env.get_template("Nat.java.jinja")
+    rootPath = f"{cmake_binary_dir}/generated/main/java/edu/wpi/first/math"
+    contents = template.render(nums=range(MAX_NUM + 1))
+    output(rootPath, "Nat.java", contents)
 
 
 if __name__ == "__main__":
diff --git a/third_party/allwpilib/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java b/third_party/allwpilib/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java
new file mode 100644
index 0000000..c7b779e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/dev/java/edu/wpi/first/math/DevMain.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+public final class DevMain {
+  /** Main entry point. */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    System.out.println(MathUtil.angleModulus(-5.0));
+  }
+
+  private DevMain() {}
+}
diff --git a/third_party/allwpilib/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java b/third_party/allwpilib/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java
deleted file mode 100644
index 9a5d378..0000000
--- a/third_party/allwpilib/wpimath/src/dev/java/edu/wpi/first/wpiutil/math/DevMain.java
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-public final class DevMain {
-  /**
-   * Main entry point.
-   */
-  public static void main(String[] args) {
-    System.out.println("Hello World!");
-    System.out.println(MathUtil.normalizeAngle(-5.0));
-  }
-
-  private DevMain() {
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpimath/src/dev/native/cpp/main.cpp
index 030c8f9..54952b7 100644
--- a/third_party/allwpilib/wpimath/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpimath/src/dev/native/cpp/main.cpp
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
+#include <fmt/core.h>
+#include <wpi/numbers>
 
-#include <wpi/math>
-
-int main() { std::cout << wpi::math::pi << std::endl; }
+int main() {
+  fmt::print("{}\n", wpi::numbers::pi);
+}
diff --git a/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.in b/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.in
deleted file mode 100644
index 5a36582..0000000
--- a/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.in
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math.numbers;
-
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-
-/**
- * A class representing the number ${num}.
-*/
-public final class N${num} extends Num implements Nat<N${num}> {
-  private N${num}() {
-  }
-
-  /**
-   * The integer this class represents.
-   *
-   * @return The literal number ${num}.
-  */
-  @Override
-  public int getNum() {
-    return ${num};
-  }
-
-  /**
-   * The singleton instance of this class.
-  */
-  public static final N${num} instance = new N${num}();
-}
diff --git a/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.jinja b/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.jinja
new file mode 100644
index 0000000..5e4be85
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generate/GenericNumber.java.jinja
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.numbers;
+
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+
+/**
+ * A class representing the number {{ num }}.
+*/
+public final class N{{ num }} extends Num implements Nat<N{{ num }}> {
+  private N{{ num }}() {
+  }
+
+  /**
+   * The integer this class represents.
+   *
+   * @return The literal number {{ num }}.
+  */
+  @Override
+  public int getNum() {
+    return {{ num }};
+  }
+
+  /**
+   * The singleton instance of this class.
+  */
+  public static final N{{ num }} instance = new N{{ num }}();
+}
diff --git a/third_party/allwpilib/wpimath/src/generate/Nat.java.in b/third_party/allwpilib/wpimath/src/generate/Nat.java.in
deleted file mode 100644
index 666bd1c..0000000
--- a/third_party/allwpilib/wpimath/src/generate/Nat.java.in
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-//CHECKSTYLE.OFF: ImportOrder
-{{REPLACEWITHIMPORTS}}
-//CHECKSTYLE.ON
-
-/**
- * A natural number expressed as a java class.
- * The counterpart to {@link Num} that should be used as a concrete value.
- *
- * @param <T> The {@link Num} this represents.
- */
-@SuppressWarnings({"MethodName", "unused"})
-public interface Nat<T extends Num> {
-  /**
-   * The number this interface represents.
-   *
-   * @return The number backing this value.
-   */
-  int getNum();
diff --git a/third_party/allwpilib/wpimath/src/generate/Nat.java.jinja b/third_party/allwpilib/wpimath/src/generate/Nat.java.jinja
new file mode 100644
index 0000000..31451d2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/generate/Nat.java.jinja
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+//CHECKSTYLE.OFF: ImportOrder
+{% for num in nums %}
+import edu.wpi.first.math.numbers.N{{ num }};
+{%- endfor %}
+//CHECKSTYLE.ON
+
+/**
+ * A natural number expressed as a java class.
+ * The counterpart to {@link Num} that should be used as a concrete value.
+ *
+ * @param <T> The {@link Num} this represents.
+ */
+@SuppressWarnings({"MethodName", "unused"})
+public interface Nat<T extends Num> {
+  /**
+   * The number this interface represents.
+   *
+   * @return The number backing this value.
+   */
+  int getNum();
+{% for num in nums %}
+  static Nat<N{{ num }}> N{{ num }}() {
+    return N{{ num }}.instance;
+  }
+{% endfor %}
+}
diff --git a/third_party/allwpilib/wpimath/src/generate/NatGetter.java.in b/third_party/allwpilib/wpimath/src/generate/NatGetter.java.in
deleted file mode 100644
index d268fab..0000000
--- a/third_party/allwpilib/wpimath/src/generate/NatGetter.java.in
+++ /dev/null
@@ -1,3 +0,0 @@
-  static Nat<N${num}> N${num}() {
-    return N${num}.instance;
-  }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Drake.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Drake.java
index 4d66102..766f7e9 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Drake.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Drake.java
@@ -1,20 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
 import org.ejml.simple.SimpleMatrix;
 
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-
 public final class Drake {
-  private Drake() {
-  }
+  private Drake() {}
 
   /**
    * Solves the discrete alegebraic Riccati equation.
@@ -27,33 +20,101 @@
    */
   @SuppressWarnings({"LocalVariableName", "ParameterName"})
   public static SimpleMatrix discreteAlgebraicRiccatiEquation(
-          SimpleMatrix A,
-          SimpleMatrix B,
-          SimpleMatrix Q,
-          SimpleMatrix R) {
+      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
     var S = new SimpleMatrix(A.numRows(), A.numCols());
-    WPIMathJNI.discreteAlgebraicRiccatiEquation(A.getDDRM().getData(), B.getDDRM().getData(),
-            Q.getDDRM().getData(), R.getDDRM().getData(), A.numCols(), B.numCols(),
-            S.getDDRM().getData());
+    WPIMathJNI.discreteAlgebraicRiccatiEquation(
+        A.getDDRM().getData(),
+        B.getDDRM().getData(),
+        Q.getDDRM().getData(),
+        R.getDDRM().getData(),
+        A.numCols(),
+        B.numCols(),
+        S.getDDRM().getData());
     return S;
   }
 
   /**
    * Solves the discrete alegebraic Riccati equation.
    *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @return Solution of DARE.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num, Inputs extends Num>
+      Matrix<States, States> discreteAlgebraicRiccatiEquation(
+          Matrix<States, States> A,
+          Matrix<States, Inputs> B,
+          Matrix<States, States> Q,
+          Matrix<Inputs, Inputs> R) {
+    return new Matrix<>(
+        discreteAlgebraicRiccatiEquation(
+            A.getStorage(), B.getStorage(), Q.getStorage(), R.getStorage()));
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
    * @param A System matrix.
    * @param B Input matrix.
    * @param Q State cost matrix.
    * @param R Input cost matrix.
+   * @param N State-input cross-term cost matrix.
+   * @return Solution of DARE.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static SimpleMatrix discreteAlgebraicRiccatiEquation(
+      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R, SimpleMatrix N) {
+    // See
+    // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
+    // for the change of variables used here.
+    var scrA = A.minus(B.mult(R.solve(N.transpose())));
+    var scrQ = Q.minus(N.mult(R.solve(N.transpose())));
+
+    var S = new SimpleMatrix(A.numRows(), A.numCols());
+    WPIMathJNI.discreteAlgebraicRiccatiEquation(
+        scrA.getDDRM().getData(),
+        B.getDDRM().getData(),
+        scrQ.getDDRM().getData(),
+        R.getDDRM().getData(),
+        A.numCols(),
+        B.numCols(),
+        S.getDDRM().getData());
+    return S;
+  }
+
+  /**
+   * Solves the discrete alegebraic Riccati equation.
+   *
+   * @param <States> Number of states.
+   * @param <Inputs> Number of inputs.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @param Q State cost matrix.
+   * @param R Input cost matrix.
+   * @param N State-input cross-term cost matrix.
    * @return Solution of DARE.
    */
   @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <States extends Num, Inputs extends Num> Matrix<States, States>
-      discreteAlgebraicRiccatiEquation(Matrix<States, States> A,
-                                       Matrix<States, Inputs> B,
-                                       Matrix<States, States> Q,
-                                       Matrix<Inputs, Inputs> R) {
-    return new Matrix<>(discreteAlgebraicRiccatiEquation(A.getStorage(), B.getStorage(),
-    Q.getStorage(), R.getStorage()));
+  public static <States extends Num, Inputs extends Num>
+      Matrix<States, States> discreteAlgebraicRiccatiEquation(
+          Matrix<States, States> A,
+          Matrix<States, Inputs> B,
+          Matrix<States, States> Q,
+          Matrix<Inputs, Inputs> R,
+          Matrix<States, Inputs> N) {
+    // See
+    // https://en.wikipedia.org/wiki/Linear%E2%80%93quadratic_regulator#Infinite-horizon,_discrete-time_LQR
+    // for the change of variables used here.
+    var scrA = A.minus(B.times(R.solve(N.transpose())));
+    var scrQ = Q.minus(N.times(R.solve(N.transpose())));
+
+    return new Matrix<>(
+        discreteAlgebraicRiccatiEquation(
+            scrA.getStorage(), B.getStorage(), scrQ.getStorage(), R.getStorage()));
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
new file mode 100644
index 0000000..e5b1952
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatBuilder.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import java.util.Objects;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A class for constructing arbitrary RxC matrices.
+ *
+ * @param <R> The number of rows of the desired matrix.
+ * @param <C> The number of columns of the desired matrix.
+ */
+public class MatBuilder<R extends Num, C extends Num> {
+  final Nat<R> m_rows;
+  final Nat<C> m_cols;
+
+  /**
+   * Fills the matrix with the given data, encoded in row major form. (The matrix is filled row by
+   * row, left to right with the given data).
+   *
+   * @param data The data to fill the matrix with.
+   * @return The constructed matrix.
+   */
+  @SuppressWarnings("LineLength")
+  public final Matrix<R, C> fill(double... data) {
+    if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
+      throw new IllegalArgumentException(
+          "Invalid matrix data provided. Wanted "
+              + this.m_rows.getNum()
+              + " x "
+              + this.m_cols.getNum()
+              + " matrix, but got "
+              + data.length
+              + " elements");
+    } else {
+      return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
+    }
+  }
+
+  /**
+   * Creates a new {@link MatBuilder} with the given dimensions.
+   *
+   * @param rows The number of rows of the matrix.
+   * @param cols The number of columns of the matrix.
+   */
+  public MatBuilder(Nat<R> rows, Nat<C> cols) {
+    this.m_rows = Objects.requireNonNull(rows);
+    this.m_cols = Objects.requireNonNull(cols);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
index 168dbb5..483dad3 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathShared.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
@@ -12,6 +9,7 @@
    * Report an error.
    *
    * @param error the error to set
+   * @param stackTrace array of stacktrace elements
    */
   void reportError(String error, StackTraceElement[] stackTrace);
 
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
index a4c8425..0dbc03d 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathSharedStore.java
@@ -1,38 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
 public final class MathSharedStore {
   private static MathShared mathShared;
 
-  private MathSharedStore() {
-  }
+  private MathSharedStore() {}
 
   /**
-   * get the MathShared object.
+   * Get the MathShared object.
+   *
+   * @return The MathShared object.
    */
   public static synchronized MathShared getMathShared() {
     if (mathShared == null) {
-      mathShared = new MathShared() {
-        @Override
-        public void reportError(String error, StackTraceElement[] stackTrace) {
-        }
+      mathShared =
+          new MathShared() {
+            @Override
+            public void reportError(String error, StackTraceElement[] stackTrace) {}
 
-        @Override
-        public void reportUsage(MathUsageId id, int count) {
-        }
-      };
+            @Override
+            public void reportUsage(MathUsageId id, int count) {}
+          };
     }
     return mathShared;
   }
 
   /**
-   * set the MathShared object.
+   * Set the MathShared object.
+   *
+   * @param shared The MathShared object.
    */
   public static synchronized void setMathShared(MathShared shared) {
     mathShared = shared;
@@ -42,6 +41,7 @@
    * Report an error.
    *
    * @param error the error to set
+   * @param stackTrace array of stacktrace elements
    */
   public static void reportError(String error, StackTraceElement[] stackTrace) {
     getMathShared().reportError(error, stackTrace);
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
index ed95e24..a3cc299 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUsageId.java
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
@@ -15,5 +12,7 @@
   kFilter_Linear,
   kOdometry_DifferentialDrive,
   kOdometry_SwerveDrive,
-  kOdometry_MecanumDrive
+  kOdometry_MecanumDrive,
+  kController_PIDController2,
+  kController_ProfiledPIDController,
 }
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
new file mode 100644
index 0000000..791de8a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MathUtil.java
@@ -0,0 +1,87 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+public final class MathUtil {
+  private MathUtil() {
+    throw new AssertionError("utility class");
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low The lower boundary to which to clamp value.
+   * @param high The higher boundary to which to clamp value.
+   * @return The clamped value.
+   */
+  public static int clamp(int value, int low, int high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Returns value clamped between low and high boundaries.
+   *
+   * @param value Value to clamp.
+   * @param low The lower boundary to which to clamp value.
+   * @param high The higher boundary to which to clamp value.
+   * @return The clamped value.
+   */
+  public static double clamp(double value, double low, double high) {
+    return Math.max(low, Math.min(value, high));
+  }
+
+  /**
+   * Returns 0.0 if the given value is within the specified range around zero. The remaining range
+   * between the deadband and 1.0 is scaled from 0.0 to 1.0.
+   *
+   * @param value Value to clip.
+   * @param deadband Range around zero.
+   * @return The value after the deadband is applied.
+   */
+  public static double applyDeadband(double value, double deadband) {
+    if (Math.abs(value) > deadband) {
+      if (value > 0.0) {
+        return (value - deadband) / (1.0 - deadband);
+      } else {
+        return (value + deadband) / (1.0 - deadband);
+      }
+    } else {
+      return 0.0;
+    }
+  }
+
+  /**
+   * Returns modulus of input.
+   *
+   * @param input Input value to wrap.
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   * @return The wrapped value.
+   */
+  public static double inputModulus(double input, double minimumInput, double maximumInput) {
+    double modulus = maximumInput - minimumInput;
+
+    // Wrap input if it's above the maximum input
+    int numMax = (int) ((input - minimumInput) / modulus);
+    input -= numMax * modulus;
+
+    // Wrap input if it's below the minimum input
+    int numMin = (int) ((input - maximumInput) / modulus);
+    input -= numMin * modulus;
+
+    return input;
+  }
+
+  /**
+   * Wraps an angle to the range -pi to pi radians.
+   *
+   * @param angleRadians Angle to wrap in radians.
+   * @return The wrapped angle.
+   */
+  public static double angleModulus(double angleRadians) {
+    return inputModulus(angleRadians, -Math.PI, Math.PI);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
new file mode 100644
index 0000000..113758b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Matrix.java
@@ -0,0 +1,713 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.numbers.N1;
+import java.util.Objects;
+import org.ejml.MatrixDimensionException;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.CommonOps_DDRM;
+import org.ejml.dense.row.MatrixFeatures_DDRM;
+import org.ejml.dense.row.NormOps_DDRM;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
+ *
+ * <p>This class is intended to be used alongside the state space library.
+ *
+ * @param <R> The number of rows in this matrix.
+ * @param <C> The number of columns in this matrix.
+ */
+public class Matrix<R extends Num, C extends Num> {
+  protected final SimpleMatrix m_storage;
+
+  /**
+   * Constructs an empty zero matrix of the given dimensions.
+   *
+   * @param rows The number of rows of the matrix.
+   * @param columns The number of columns of the matrix.
+   */
+  public Matrix(Nat<R> rows, Nat<C> columns) {
+    this.m_storage =
+        new SimpleMatrix(
+            Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(columns).getNum());
+  }
+
+  /**
+   * Constructs a new {@link Matrix} with the given storage. Caller should make sure that the
+   * provided generic bounds match the shape of the provided {@link Matrix}.
+   *
+   * <p>NOTE:It is not recommend to use this constructor unless the {@link SimpleMatrix} API is
+   * absolutely necessary due to the desired function not being accessible through the {@link
+   * Matrix} wrapper.
+   *
+   * @param storage The {@link SimpleMatrix} to back this value.
+   */
+  public Matrix(SimpleMatrix storage) {
+    this.m_storage = Objects.requireNonNull(storage);
+  }
+
+  /**
+   * Constructs a new matrix with the storage of the supplied matrix.
+   *
+   * @param other The {@link Matrix} to copy the storage of.
+   */
+  public Matrix(Matrix<R, C> other) {
+    this.m_storage = Objects.requireNonNull(other).getStorage().copy();
+  }
+
+  /**
+   * Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
+   *
+   * <p>NOTE:The use of this method is heavily discouraged as this removes any guarantee of type
+   * safety. This should only be called if the {@link SimpleMatrix} API is absolutely necessary due
+   * to the desired function not being accessible through the {@link Matrix} wrapper.
+   *
+   * @return The underlying {@link SimpleMatrix} storage.
+   */
+  public SimpleMatrix getStorage() {
+    return m_storage;
+  }
+
+  /**
+   * Gets the number of columns in this matrix.
+   *
+   * @return The number of columns, according to the internal storage.
+   */
+  public final int getNumCols() {
+    return this.m_storage.numCols();
+  }
+
+  /**
+   * Gets the number of rows in this matrix.
+   *
+   * @return The number of rows, according to the internal storage.
+   */
+  public final int getNumRows() {
+    return this.m_storage.numRows();
+  }
+
+  /**
+   * Get an element of this matrix.
+   *
+   * @param row The row of the element.
+   * @param col The column of the element.
+   * @return The element in this matrix at row,col.
+   */
+  public final double get(int row, int col) {
+    return this.m_storage.get(row, col);
+  }
+
+  /**
+   * Sets the value at the given indices.
+   *
+   * @param row The row of the element.
+   * @param col The column of the element.
+   * @param value The value to insert at the given location.
+   */
+  public final void set(int row, int col, double value) {
+    this.m_storage.set(row, col, value);
+  }
+
+  /**
+   * Sets a row to a given row vector.
+   *
+   * @param row The row to set.
+   * @param val The row vector to set the given row to.
+   */
+  public final void setRow(int row, Matrix<N1, C> val) {
+    this.m_storage.setRow(row, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
+  }
+
+  /**
+   * Sets a column to a given column vector.
+   *
+   * @param column The column to set.
+   * @param val The column vector to set the given row to.
+   */
+  public final void setColumn(int column, Matrix<R, N1> val) {
+    this.m_storage.setColumn(column, 0, Objects.requireNonNull(val).m_storage.getDDRM().getData());
+  }
+
+  /**
+   * Sets all the elements in "this" matrix equal to the specified value.
+   *
+   * @param value The value each element is set to.
+   */
+  public void fill(double value) {
+    this.m_storage.fill(value);
+  }
+
+  /**
+   * Returns the diagonal elements inside a vector or square matrix.
+   *
+   * <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this" {@link
+   * Matrix} is a matrix then a vector of diagonal elements is returned.
+   *
+   * @return The diagonal elements inside a vector or a square matrix.
+   */
+  public final Matrix<R, C> diag() {
+    return new Matrix<>(this.m_storage.diag());
+  }
+
+  /**
+   * Returns the largest element of this matrix.
+   *
+   * @return The largest element of this matrix.
+   */
+  public final double max() {
+    return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Returns the absolute value of the element in this matrix with the largest absolute value.
+   *
+   * @return The absolute value of the element with the largest absolute value.
+   */
+  public final double maxAbs() {
+    return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Returns the smallest element of this matrix.
+   *
+   * @return The smallest element of this matrix.
+   */
+  public final double minInternal() {
+    return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Calculates the mean of the elements in this matrix.
+   *
+   * @return The mean value of this matrix.
+   */
+  public final double mean() {
+    return this.elementSum() / (double) this.m_storage.getNumElements();
+  }
+
+  /**
+   * Multiplies this matrix with another that has C rows.
+   *
+   * <p>As matrix multiplication is only defined if the number of columns in the first matrix
+   * matches the number of rows in the second, this operation will fail to compile under any other
+   * circumstances.
+   *
+   * @param other The other matrix to multiply by.
+   * @param <C2> The number of columns in the second matrix.
+   * @return The result of the matrix multiplication between "this" and the given matrix.
+   */
+  public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
+    return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage));
+  }
+
+  /**
+   * Multiplies all the elements of this matrix by the given scalar.
+   *
+   * @param value The scalar value to multiply by.
+   * @return A new matrix with all the elements multiplied by the given value.
+   */
+  public Matrix<R, C> times(double value) {
+    return new Matrix<>(this.m_storage.scale(value));
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element multiplication of "this" and
+   * other.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
+   *
+   * @param other The other {@link Matrix} to preform element multiplication on.
+   * @return The element by element multiplication of "this" and other.
+   */
+  public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
+    return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
+  }
+
+  /**
+   * Subtracts the given value from all the elements of this matrix.
+   *
+   * @param value The value to subtract.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> minus(double value) {
+    return new Matrix<>(this.m_storage.minus(value));
+  }
+
+  /**
+   * Subtracts the given matrix from this matrix.
+   *
+   * @param value The matrix to subtract.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> minus(Matrix<R, C> value) {
+    return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Adds the given value to all the elements of this matrix.
+   *
+   * @param value The value to add.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> plus(double value) {
+    return new Matrix<>(this.m_storage.plus(value));
+  }
+
+  /**
+   * Adds the given matrix to this matrix.
+   *
+   * @param value The matrix to add.
+   * @return The resultant matrix.
+   */
+  public final Matrix<R, C> plus(Matrix<R, C> value) {
+    return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
+  }
+
+  /**
+   * Divides all elements of this matrix by the given value.
+   *
+   * @param value The value to divide by.
+   * @return The resultant matrix.
+   */
+  public Matrix<R, C> div(int value) {
+    return new Matrix<>(this.m_storage.divide((double) value));
+  }
+
+  /**
+   * Divides all elements of this matrix by the given value.
+   *
+   * @param value The value to divide by.
+   * @return The resultant matrix.
+   */
+  public Matrix<R, C> div(double value) {
+    return new Matrix<>(this.m_storage.divide(value));
+  }
+
+  /**
+   * Calculates the transpose, Mᵀ of this matrix.
+   *
+   * @return The transpose matrix.
+   */
+  public final Matrix<C, R> transpose() {
+    return new Matrix<>(this.m_storage.transpose());
+  }
+
+  /**
+   * Returns a copy of this matrix.
+   *
+   * @return A copy of this matrix.
+   */
+  public final Matrix<R, C> copy() {
+    return new Matrix<>(this.m_storage.copy());
+  }
+
+  /**
+   * Returns the inverse matrix of "this" matrix.
+   *
+   * @return The inverse of "this" matrix.
+   * @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable.
+   */
+  public final Matrix<R, C> inv() {
+    return new Matrix<>(this.m_storage.invert());
+  }
+
+  /**
+   * Returns the solution x to the equation Ax = b, where A is "this" matrix.
+   *
+   * <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the pseudo inverse
+   * is used if A is not square.
+   *
+   * @param <C2> Columns in b.
+   * @param b The right-hand side of the equation to solve.
+   * @return The solution to the linear system.
+   */
+  @SuppressWarnings("ParameterName")
+  public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
+    return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
+  }
+
+  /**
+   * Computes the matrix exponential using Eigen's solver. This method only works for square
+   * matrices, and will otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> exp() {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException(
+          "Non-square matrices cannot be exponentiated! "
+              + "This matrix is "
+              + this.getNumRows()
+              + " x "
+              + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.exp(
+        this.m_storage.getDDRM().getData(),
+        this.getNumRows(),
+        toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
+  /**
+   * Computes the matrix power using Eigen's solver. This method only works for square matrices, and
+   * will otherwise throw an {@link MatrixDimensionException}.
+   *
+   * @param exponent The exponent.
+   * @return The exponential of A.
+   */
+  public final Matrix<R, C> pow(double exponent) {
+    if (this.getNumRows() != this.getNumCols()) {
+      throw new MatrixDimensionException(
+          "Non-square matrices cannot be raised to a power! "
+              + "This matrix is "
+              + this.getNumRows()
+              + " x "
+              + this.getNumCols());
+    }
+    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
+    WPIMathJNI.pow(
+        this.m_storage.getDDRM().getData(),
+        this.getNumRows(),
+        exponent,
+        toReturn.m_storage.getDDRM().getData());
+    return toReturn;
+  }
+
+  /**
+   * Returns the determinant of this matrix.
+   *
+   * @return The determinant of this matrix.
+   */
+  public final double det() {
+    return this.m_storage.determinant();
+  }
+
+  /**
+   * Computes the Frobenius normal of the matrix.
+   *
+   * <p>normF = Sqrt{ &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>} }
+   *
+   * @return The matrix's Frobenius normal.
+   */
+  public final double normF() {
+    return this.m_storage.normF();
+  }
+
+  /**
+   * Computes the induced p = 1 matrix norm.
+   *
+   * <p>||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
+   *
+   * @return The norm.
+   */
+  public final double normIndP1() {
+    return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
+  }
+
+  /**
+   * Computes the sum of all the elements in the matrix.
+   *
+   * @return Sum of all the elements.
+   */
+  public final double elementSum() {
+    return this.m_storage.elementSum();
+  }
+
+  /**
+   * Computes the trace of the matrix.
+   *
+   * @return The trace of the matrix.
+   */
+  public final double trace() {
+    return this.m_storage.trace();
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element power of "this" and b.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
+   *
+   * @param b Scalar.
+   * @return The element by element power of "this" and b.
+   */
+  @SuppressWarnings("ParameterName")
+  public final Matrix<R, C> elementPower(double b) {
+    return new Matrix<>(this.m_storage.elementPower(b));
+  }
+
+  /**
+   * Returns a matrix which is the result of an element by element power of "this" and b.
+   *
+   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
+   *
+   * @param b Scalar.
+   * @return The element by element power of "this" and b.
+   */
+  @SuppressWarnings("ParameterName")
+  public final Matrix<R, C> elementPower(int b) {
+    return new Matrix<>(this.m_storage.elementPower((double) b));
+  }
+
+  /**
+   * Extracts a given row into a row vector with new underlying storage.
+   *
+   * @param row The row to extract a vector from.
+   * @return A row vector from the given row.
+   */
+  public final Matrix<N1, C> extractRowVector(int row) {
+    return new Matrix<>(this.m_storage.extractVector(true, row));
+  }
+
+  /**
+   * Extracts a given column into a column vector with new underlying storage.
+   *
+   * @param column The column to extract a vector from.
+   * @return A column vector from the given column.
+   */
+  public final Matrix<R, N1> extractColumnVector(int column) {
+    return new Matrix<>(this.m_storage.extractVector(false, column));
+  }
+
+  /**
+   * Extracts a matrix of a given size and start position with new underlying storage.
+   *
+   * @param <R2> Number of rows to extract.
+   * @param <C2> Number of columns to extract.
+   * @param height The number of rows of the extracted matrix.
+   * @param width The number of columns of the extracted matrix.
+   * @param startingRow The starting row of the extracted matrix.
+   * @param startingCol The starting column of the extracted matrix.
+   * @return The extracted matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
+      Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
+    return new Matrix<>(
+        this.m_storage.extractMatrix(
+            startingRow,
+            startingRow + Objects.requireNonNull(height).getNum(),
+            startingCol,
+            startingCol + Objects.requireNonNull(width).getNum()));
+  }
+
+  /**
+   * Extracts a matrix of a given size and start position with new underlying storage.
+   *
+   * @param <R2> Number of rows to extract.
+   * @param <C2> Number of columns to extract.
+   * @param height The number of rows of the extracted matrix.
+   * @param width The number of columns of the extracted matrix.
+   * @param startingRow The starting row of the extracted matrix.
+   * @param startingCol The starting column of the extracted matrix.
+   * @return The extracted matrix.
+   */
+  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
+      int height, int width, int startingRow, int startingCol) {
+    return new Matrix<R2, C2>(
+        this.m_storage.extractMatrix(
+            startingRow, startingRow + height, startingCol, startingCol + width));
+  }
+
+  /**
+   * Assign a matrix of a given size and start position.
+   *
+   * @param <R2> Rows in block assignment.
+   * @param <C2> Columns in block assignment.
+   * @param startingRow The row to start at.
+   * @param startingCol The column to start at.
+   * @param other The matrix to assign the block to.
+   */
+  public <R2 extends Num, C2 extends Num> void assignBlock(
+      int startingRow, int startingCol, Matrix<R2, C2> other) {
+    this.m_storage.insertIntoThis(
+        startingRow, startingCol, Objects.requireNonNull(other).m_storage);
+  }
+
+  /**
+   * Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The
+   * shape of "this" is used to determine the size of the matrix extracted.
+   *
+   * @param <R2> Number of rows to extract.
+   * @param <C2> Number of columns to extract.
+   * @param startingRow The starting row in the supplied matrix to extract the submatrix.
+   * @param startingCol The starting column in the supplied matrix to extract the submatrix.
+   * @param other The matrix to extract the submatrix from.
+   */
+  public <R2 extends Num, C2 extends Num> void extractFrom(
+      int startingRow, int startingCol, Matrix<R2, C2> other) {
+    CommonOps_DDRM.extract(
+        other.m_storage.getDDRM(), startingRow, startingCol, this.m_storage.getDDRM());
+  }
+
+  /**
+   * Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it will
+   * return the zero matrix.
+   *
+   * @param lowerTriangular Whether or not we want to decompose to the lower triangular Cholesky
+   *     matrix.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
+   *     semidefinite).
+   */
+  public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
+    SimpleMatrix temp = m_storage.copy();
+
+    CholeskyDecomposition_F64<DMatrixRMaj> chol =
+        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+    if (!chol.decompose(temp.getMatrix())) {
+      // check that the input is not all zeros -- if they are, we special case and return all
+      // zeros.
+      var matData = temp.getDDRM().data;
+      var isZeros = true;
+      for (double matDatum : matData) {
+        isZeros &= Math.abs(matDatum) < 1e-6;
+      }
+      if (isZeros) {
+        return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
+      }
+
+      throw new RuntimeException(
+          "Cholesky decomposition failed! Input matrix:\n" + m_storage.toString());
+    }
+
+    return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
+  }
+
+  /**
+   * Returns the row major data of this matrix as a double array.
+   *
+   * @return The row major data of this matrix as a double array.
+   */
+  public double[] getData() {
+    return m_storage.getDDRM().getData();
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix as a {@link Nat}.
+   * @param <D> The dimension of the desired matrix as a generic.
+   * @return The DxD identity matrix.
+   */
+  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix as a {@link Num}.
+   * @param <D> The dimension of the desired matrix as a generic.
+   * @return The DxD identity matrix.
+   */
+  public static <D extends Num> Matrix<D, D> eye(D dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Entrypoint to the {@link MatBuilder} class for creation of custom matrices with the given
+   * dimensions and contents.
+   *
+   * @param rows The number of rows of the desired matrix.
+   * @param cols The number of columns of the desired matrix.
+   * @param <R> The number of rows of the desired matrix as a generic.
+   * @param <C> The number of columns of the desired matrix as a generic.
+   * @return A builder to construct the matrix.
+   */
+  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
+    return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
+  }
+
+  /**
+   * Reassigns dimensions of a {@link Matrix} to allow for operations with other matrices that have
+   * wildcard dimensions.
+   *
+   * @param <R1> Row dimension to assign.
+   * @param <C1> Column dimension to assign.
+   * @param mat The {@link Matrix} to remove the dimensions from.
+   * @return The matrix with reassigned dimensions.
+   */
+  public static <R1 extends Num, C1 extends Num> Matrix<R1, C1> changeBoundsUnchecked(
+      Matrix<?, ?> mat) {
+    return new Matrix<>(mat.m_storage);
+  }
+
+  /**
+   * Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
+   *
+   * <p>This will check if each element is in tolerance of the corresponding element from the other
+   * {@link Matrix} or if the elements have the same symbolic meaning. For two elements to have the
+   * same symbolic meaning they both must be either Double.NaN, Double.POSITIVE_INFINITY, or
+   * Double.NEGATIVE_INFINITY.
+   *
+   * <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this method when
+   * checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)} will return false
+   * if an element is uncountable. This method should only be used when uncountable elements need to
+   * compared.
+   *
+   * @param other The {@link Matrix} to check against this one.
+   * @param tolerance The tolerance to check equality with.
+   * @return true if this matrix is identical to the one supplied.
+   */
+  public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
+    return MatrixFeatures_DDRM.isIdentical(
+        this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
+  }
+
+  /**
+   * Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
+   *
+   * <p>This will check if each element is in tolerance of the corresponding element from the other
+   * {@link Matrix}.
+   *
+   * <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
+   *
+   * @param other The {@link Matrix} to check against this one.
+   * @param tolerance The tolerance to check equality with.
+   * @return true if this matrix is equal to the one supplied.
+   */
+  public boolean isEqual(Matrix<?, ?> other, double tolerance) {
+    return MatrixFeatures_DDRM.isEquals(
+        this.m_storage.getDDRM(), other.m_storage.getDDRM(), tolerance);
+  }
+
+  @Override
+  public String toString() {
+    return m_storage.toString();
+  }
+
+  /**
+   * Checks if an object is equal to this {@link Matrix}.
+   *
+   * <p>a<sub>ij</sub> == b<sub>ij</sub>
+   *
+   * @param other The Object to check against this {@link Matrix}.
+   * @return true if the object supplied is a {@link Matrix} and is equal to this matrix.
+   */
+  @Override
+  public boolean equals(Object other) {
+    if (this == other) {
+      return true;
+    }
+    if (!(other instanceof Matrix)) {
+      return false;
+    }
+
+    Matrix<?, ?> matrix = (Matrix<?, ?>) other;
+    if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) {
+      return false;
+    }
+    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM());
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_storage);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java
new file mode 100644
index 0000000..7600e31
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/MatrixUtils.java
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.numbers.N1;
+import java.util.Objects;
+import org.ejml.simple.SimpleMatrix;
+
+@Deprecated
+public final class MatrixUtils {
+  private MatrixUtils() {
+    throw new AssertionError("utility class");
+  }
+
+  /**
+   * Creates a new matrix of zeros.
+   *
+   * @param rows The number of rows in the matrix.
+   * @param cols The number of columns in the matrix.
+   * @param <R> The number of rows in the matrix as a generic.
+   * @param <C> The number of columns in the matrix as a generic.
+   * @return An RxC matrix filled with zeros.
+   */
+  @SuppressWarnings("LineLength")
+  public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
+    return new Matrix<>(
+        new SimpleMatrix(
+            Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
+  }
+
+  /**
+   * Creates a new vector of zeros.
+   *
+   * @param nums The size of the desired vector.
+   * @param <N> The size of the desired vector as a generic.
+   * @return A vector of size N filled with zeros.
+   */
+  public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
+    return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
+  }
+
+  /**
+   * Creates the identity matrix of the given dimension.
+   *
+   * @param dim The dimension of the desired matrix.
+   * @param <D> The dimension of the desired matrix as a generic.
+   * @return The DxD identity matrix.
+   */
+  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
+    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
+  }
+
+  /**
+   * Entrypoint to the MatBuilder class for creation of custom matrices with the given dimensions
+   * and contents.
+   *
+   * @param rows The number of rows of the desired matrix.
+   * @param cols The number of columns of the desired matrix.
+   * @param <R> The number of rows of the desired matrix as a generic.
+   * @param <C> The number of columns of the desired matrix as a generic.
+   * @return A builder to construct the matrix.
+   */
+  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
+    return new MatBuilder<>(rows, cols);
+  }
+
+  /**
+   * Entrypoint to the VecBuilder class for creation of custom vectors with the given size and
+   * contents.
+   *
+   * @param dim The dimension of the vector.
+   * @param <D> The dimension of the vector as a generic.
+   * @return A builder to construct the vector.
+   */
+  public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
+    return new VecBuilder<>(dim);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java
new file mode 100644
index 0000000..ef0fd2d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Num.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+/** A number expressed as a java class. */
+public abstract class Num {
+  /**
+   * The number this is backing.
+   *
+   * @return The number represented by this class.
+   */
+  public abstract int getNum();
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java
new file mode 100644
index 0000000..d1a68c7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Pair.java
@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+public class Pair<A, B> {
+  private final A m_first;
+  private final B m_second;
+
+  public Pair(A first, B second) {
+    m_first = first;
+    m_second = second;
+  }
+
+  public A getFirst() {
+    return m_first;
+  }
+
+  public B getSecond() {
+    return m_second;
+  }
+
+  @SuppressWarnings("ParameterName")
+  public static <A, B> Pair<A, B> of(A a, B b) {
+    return new Pair<>(a, b);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
new file mode 100644
index 0000000..fc78dd1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/SimpleMatrixUtils.java
@@ -0,0 +1,254 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import java.util.function.BiFunction;
+import org.ejml.data.DMatrixRMaj;
+import org.ejml.dense.row.NormOps_DDRM;
+import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
+import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
+import org.ejml.simple.SimpleBase;
+import org.ejml.simple.SimpleMatrix;
+
+public final class SimpleMatrixUtils {
+  private SimpleMatrixUtils() {}
+
+  /**
+   * Compute the matrix exponential, e^M of the given matrix.
+   *
+   * @param matrix The matrix to compute the exponential of.
+   * @return The resultant matrix.
+   */
+  @SuppressWarnings({"LocalVariableName", "LineLength"})
+  public static SimpleMatrix expm(SimpleMatrix matrix) {
+    BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
+    SimpleMatrix A = matrix;
+    double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
+    int n_squarings = 0;
+
+    if (A_L1 < 1.495585217958292e-002) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 2.539398330063230e-001) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 9.504178996162932e-001) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else if (A_L1 < 2.097847961257068e+000) {
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    } else {
+      double maxNorm = 5.371920351148152;
+      double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
+      n_squarings = (int) Math.max(0, Math.ceil(log));
+      A = A.divide(Math.pow(2.0, n_squarings));
+      Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
+      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
+    }
+  }
+
+  @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
+  private static SimpleMatrix dispatchPade(
+      SimpleMatrix U,
+      SimpleMatrix V,
+      int nSquarings,
+      BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
+    SimpleMatrix P = U.plus(V);
+    SimpleMatrix Q = U.negative().plus(V);
+
+    SimpleMatrix R = solveProvider.apply(Q, P);
+
+    for (int i = 0; i < nSquarings; i++) {
+      R = R.mult(R);
+    }
+
+    return R;
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
+    double[] b = new double[] {120, 60, 12, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
+    SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
+    double[] b = new double[] {30240, 15120, 3360, 420, 30, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+
+    SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
+    double[] b = new double[] {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+
+    SimpleMatrix U =
+        A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
+    SimpleMatrix V =
+        A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
+    double[] b =
+        new double[] {
+          17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1
+        };
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+    SimpleMatrix A8 = A6.mult(A2);
+
+    SimpleMatrix U =
+        A.mult(
+            A8.scale(b[9])
+                .plus(A6.scale(b[7]))
+                .plus(A4.scale(b[5]))
+                .plus(A2.scale(b[3]))
+                .plus(ident.scale(b[1])));
+    SimpleMatrix V =
+        A8.scale(b[8])
+            .plus(A6.scale(b[6]))
+            .plus(A4.scale(b[4]))
+            .plus(A2.scale(b[2]))
+            .plus(ident.scale(b[0]));
+
+    return new Pair<>(U, V);
+  }
+
+  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
+  private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
+    double[] b =
+        new double[] {
+          64764752532480000.0,
+          32382376266240000.0,
+          7771770303897600.0,
+          1187353796428800.0,
+          129060195264000.0,
+          10559470521600.0,
+          670442572800.0,
+          33522128640.0,
+          1323241920,
+          40840800,
+          960960,
+          16380,
+          182,
+          1
+        };
+    SimpleMatrix ident = eye(A.numRows(), A.numCols());
+
+    SimpleMatrix A2 = A.mult(A);
+    SimpleMatrix A4 = A2.mult(A2);
+    SimpleMatrix A6 = A4.mult(A2);
+
+    SimpleMatrix U =
+        A.mult(
+            A6.scale(b[13])
+                .plus(A4.scale(b[11]))
+                .plus(A2.scale(b[9]))
+                .plus(A6.scale(b[7]))
+                .plus(A4.scale(b[5]))
+                .plus(A2.scale(b[3]))
+                .plus(ident.scale(b[1])));
+    SimpleMatrix V =
+        A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8])))
+            .plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
+
+    return new Pair<>(U, V);
+  }
+
+  private static SimpleMatrix eye(int rows, int cols) {
+    return SimpleMatrix.identity(Math.min(rows, cols));
+  }
+
+  /**
+   * The identy of a square matrix.
+   *
+   * @param rows the number of rows (and columns)
+   * @return the identiy matrix, rows x rows.
+   */
+  public static SimpleMatrix eye(int rows) {
+    return SimpleMatrix.identity(rows);
+  }
+
+  /**
+   * Decompose the given matrix using Cholesky Decomposition and return a view of the upper
+   * triangular matrix (if you want lower triangular see the other overload of this method.) If the
+   * input matrix is zeros, this will return the zero matrix.
+   *
+   * @param src The matrix to decompose.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   *     semidefinite).
+   */
+  public static SimpleMatrix lltDecompose(SimpleMatrix src) {
+    return lltDecompose(src, false);
+  }
+
+  /**
+   * Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this
+   * will return the zero matrix.
+   *
+   * @param src The matrix to decompose.
+   * @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
+   * @return The decomposed matrix.
+   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
+   *     semidefinite).
+   */
+  public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
+    SimpleMatrix temp = src.copy();
+
+    CholeskyDecomposition_F64<DMatrixRMaj> chol =
+        DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
+    if (!chol.decompose(temp.getMatrix())) {
+      // check that the input is not all zeros -- if they are, we special case and return all
+      // zeros.
+      var matData = temp.getDDRM().data;
+      var isZeros = true;
+      for (double matDatum : matData) {
+        isZeros &= Math.abs(matDatum) < 1e-6;
+      }
+      if (isZeros) {
+        return new SimpleMatrix(temp.numRows(), temp.numCols());
+      }
+
+      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
+    }
+
+    return SimpleMatrix.wrap(chol.getT(null));
+  }
+
+  /**
+   * Computes the matrix exponential using Eigen's solver.
+   *
+   * @param A the matrix to exponentiate.
+   * @return the exponential of A.
+   */
+  @SuppressWarnings("ParameterName")
+  public static SimpleMatrix exp(SimpleMatrix A) {
+    SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
+    WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
+    return toReturn;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
new file mode 100644
index 0000000..8baf401
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -0,0 +1,202 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import java.util.Random;
+import org.ejml.simple.SimpleMatrix;
+
+@SuppressWarnings("ParameterName")
+public final class StateSpaceUtil {
+  private static Random rand = new Random();
+
+  private StateSpaceUtil() {
+    // Utility class
+  }
+
+  /**
+   * Creates a covariance matrix from the given vector for use with Kalman filters.
+   *
+   * <p>Each element is squared and placed on the covariance matrix diagonal.
+   *
+   * @param <States> Num representing the states of the system.
+   * @param states A Nat representing the states of the system.
+   * @param stdDevs For a Q matrix, its elements are the standard deviations of each state from how
+   *     the model behaves. For an R matrix, its elements are the standard deviations for each
+   *     output measurement.
+   * @return Process noise or measurement noise covariance matrix.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
+      Nat<States> states, Matrix<States, N1> stdDevs) {
+    var result = new Matrix<>(states, states);
+    for (int i = 0; i < states.getNum(); i++) {
+      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
+    }
+    return result;
+  }
+
+  /**
+   * Creates a vector of normally distributed white noise with the given noise intensities for each
+   * element.
+   *
+   * @param <N> Num representing the dimensionality of the noise vector to create.
+   * @param stdDevs A matrix whose elements are the standard deviations of each element of the noise
+   *     vector.
+   * @return White noise vector.
+   */
+  public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(Matrix<N, N1> stdDevs) {
+    Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
+    for (int i = 0; i < stdDevs.getNumRows(); i++) {
+      result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
+    }
+    return result;
+  }
+
+  /**
+   * Creates a cost matrix from the given vector for use with LQR.
+   *
+   * <p>The cost matrix is constructed using Bryson's rule. The inverse square of each element in
+   * the input is taken and placed on the cost matrix diagonal.
+   *
+   * @param <States> Nat representing the states of the system.
+   * @param costs An array. For a Q matrix, its elements are the maximum allowed excursions of the
+   *     states from the reference. For an R matrix, its elements are the maximum allowed excursions
+   *     of the control inputs from no actuation.
+   * @return State excursion or control effort cost matrix.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num> Matrix<States, States> makeCostMatrix(
+      Matrix<States, N1> costs) {
+    Matrix<States, States> result =
+        new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
+    result.fill(0.0);
+
+    for (int i = 0; i < costs.getNumRows(); i++) {
+      result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns true if (A, B) is a stabilizable pair.
+   *
+   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank(λI - A, B) %3C n
+   * where n is the number of states.
+   *
+   * @param <States> Num representing the size of A.
+   * @param <Inputs> Num representing the columns of B.
+   * @param A System matrix.
+   * @param B Input matrix.
+   * @return If the system is stabilizable.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> boolean isStabilizable(
+      Matrix<States, States> A, Matrix<States, Inputs> B) {
+    return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(), A.getData(), B.getData());
+  }
+
+  /**
+   * Returns true if (A, C) is a detectable pair.
+   *
+   * <p>(A, C) is detectable if and only if the unobservable eigenvalues of A, if any, have absolute
+   * values less than one, where an eigenvalue is unobservable if rank(λI - A; C) %3C n where n is
+   * the number of states.
+   *
+   * @param <States> Num representing the size of A.
+   * @param <Outputs> Num representing the rows of C.
+   * @param A System matrix.
+   * @param C Output matrix.
+   * @return If the system is detectable.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Outputs extends Num> boolean isDetectable(
+      Matrix<States, States> A, Matrix<Outputs, States> C) {
+    return WPIMathJNI.isStabilizable(
+        A.getNumRows(), C.getNumRows(), A.transpose().getData(), C.transpose().getData());
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in vector form, with the third element, theta, in radians.
+   */
+  public static Matrix<N3, N1> poseToVector(Pose2d pose) {
+    return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param u The input to clamp.
+   * @param umin The minimum input magnitude.
+   * @param umax The maximum input magnitude.
+   * @param <I> The number of inputs.
+   * @return The clamped input.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(
+      Matrix<I, N1> u, Matrix<I, N1> umin, Matrix<I, N1> umax) {
+    var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
+    for (int i = 0; i < u.getNumRows(); i++) {
+      result.set(i, 0, MathUtil.clamp(u.get(i, 0), umin.get(i, 0), umax.get(i, 0)));
+    }
+    return result;
+  }
+
+  /**
+   * Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
+   * differential drivetrains.
+   *
+   * @param u The input vector.
+   * @param maxMagnitude The maximum magnitude any input can have.
+   * @param <I> The number of inputs.
+   * @return The normalizedInput
+   */
+  public static <I extends Num> Matrix<I, N1> normalizeInputVector(
+      Matrix<I, N1> u, double maxMagnitude) {
+    double maxValue = u.maxAbs();
+    boolean isCapped = maxValue > maxMagnitude;
+
+    if (isCapped) {
+      return u.times(maxMagnitude / maxValue);
+    }
+    return u;
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, cos(theta), sin(theta)], where theta is in
+   * radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta).
+   */
+  public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
+    return VecBuilder.fill(
+        pose.getTranslation().getX(),
+        pose.getTranslation().getY(),
+        pose.getRotation().getCos(),
+        pose.getRotation().getSin());
+  }
+
+  /**
+   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
+   *
+   * @param pose A pose to convert to a vector.
+   * @return The given pose in vector form, with the third element, theta, in radians.
+   */
+  public static Matrix<N3, N1> poseTo3dVector(Pose2d pose) {
+    return VecBuilder.fill(
+        pose.getTranslation().getX(),
+        pose.getTranslation().getY(),
+        pose.getRotation().getRadians());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
new file mode 100644
index 0000000..670611a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/VecBuilder.java
@@ -0,0 +1,197 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N10;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.numbers.N7;
+import edu.wpi.first.math.numbers.N8;
+import edu.wpi.first.math.numbers.N9;
+
+/**
+ * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
+ *
+ * @param <N> The dimension of the vector to be constructed.
+ */
+public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
+  public VecBuilder(Nat<N> rows) {
+    super(rows, Nat.N1());
+  }
+
+  private Vector<N> fillVec(double... data) {
+    return new Vector<>(fill(data));
+  }
+
+  /**
+   * Returns a 1x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @return 1x1 vector
+   */
+  public static Vector<N1> fill(double n1) {
+    return new VecBuilder<>(Nat.N1()).fillVec(n1);
+  }
+
+  /**
+   * Returns a 2x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @return 2x1 vector
+   */
+  public static Vector<N2> fill(double n1, double n2) {
+    return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
+  }
+
+  /**
+   * Returns a 3x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @return 3x1 vector
+   */
+  public static Vector<N3> fill(double n1, double n2, double n3) {
+    return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
+  }
+
+  /**
+   * Returns a 4x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @return 4x1 vector
+   */
+  public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
+    return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
+  }
+
+  /**
+   * Returns a 5x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @return 5x1 vector
+   */
+  public static Vector<N5> fill(double n1, double n2, double n3, double n4, double n5) {
+    return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5);
+  }
+
+  /**
+   * Returns a 6x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @return 6x1 vector
+   */
+  public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5, double n6) {
+    return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
+  }
+
+  /**
+   * Returns a 7x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @return 7x1 vector
+   */
+  public static Vector<N7> fill(
+      double n1, double n2, double n3, double n4, double n5, double n6, double n7) {
+    return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
+  }
+
+  /**
+   * Returns a 8x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @return 8x1 vector
+   */
+  public static Vector<N8> fill(
+      double n1, double n2, double n3, double n4, double n5, double n6, double n7, double n8) {
+    return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
+  }
+
+  /**
+   * Returns a 9x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @param n9 the ninth element.
+   * @return 9x1 vector
+   */
+  public static Vector<N9> fill(
+      double n1,
+      double n2,
+      double n3,
+      double n4,
+      double n5,
+      double n6,
+      double n7,
+      double n8,
+      double n9) {
+    return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
+  }
+
+  /**
+   * Returns a 10x1 vector containing the given elements.
+   *
+   * @param n1 the first element.
+   * @param n2 the second element.
+   * @param n3 the third element.
+   * @param n4 the fourth element.
+   * @param n5 the fifth element.
+   * @param n6 the sixth element.
+   * @param n7 the seventh element.
+   * @param n8 the eighth element.
+   * @param n9 the ninth element.
+   * @param n10 the tenth element.
+   * @return 10x1 vector
+   */
+  public static Vector<N10> fill(
+      double n1,
+      double n2,
+      double n3,
+      double n4,
+      double n5,
+      double n6,
+      double n7,
+      double n8,
+      double n9,
+      double n10) {
+    return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Vector.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Vector.java
new file mode 100644
index 0000000..9b06e71
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/Vector.java
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import edu.wpi.first.math.numbers.N1;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
+ *
+ * <p>This class is intended to be used alongside the state space library.
+ *
+ * @param <R> The number of rows in this matrix.
+ */
+public class Vector<R extends Num> extends Matrix<R, N1> {
+  /**
+   * Constructs an empty zero vector of the given dimensions.
+   *
+   * @param rows The number of rows of the vector.
+   */
+  public Vector(Nat<R> rows) {
+    super(rows, Nat.N1());
+  }
+
+  /**
+   * Constructs a new {@link Vector} with the given storage. Caller should make sure that the
+   * provided generic bounds match the shape of the provided {@link Vector}.
+   *
+   * <p>NOTE:It is not recommended to use this constructor unless the {@link SimpleMatrix} API is
+   * absolutely necessary due to the desired function not being accessible through the {@link
+   * Vector} wrapper.
+   *
+   * @param storage The {@link SimpleMatrix} to back this vector.
+   */
+  public Vector(SimpleMatrix storage) {
+    super(storage);
+  }
+
+  /**
+   * Constructs a new vector with the storage of the supplied matrix.
+   *
+   * @param other The {@link Vector} to copy the storage of.
+   */
+  public Vector(Matrix<R, N1> other) {
+    super(other);
+  }
+
+  @Override
+  public Vector<R> times(double value) {
+    return new Vector<>(this.m_storage.scale(value));
+  }
+
+  @Override
+  public Vector<R> div(int value) {
+    return new Vector<>(this.m_storage.divide(value));
+  }
+
+  @Override
+  public Vector<R> div(double value) {
+    return new Vector<>(this.m_storage.divide(value));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
index 30984ac..54445d3 100644
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
+import edu.wpi.first.util.RuntimeLoader;
 import java.io.IOException;
 import java.util.concurrent.atomic.AtomicBoolean;
 
-import edu.wpi.first.wpiutil.RuntimeLoader;
-
 public final class WPIMathJNI {
   static boolean libraryLoaded = false;
   static RuntimeLoader<WPIMathJNI> loader = null;
@@ -19,8 +15,9 @@
   static {
     if (Helper.getExtractOnStaticLoad()) {
       try {
-        loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
-                WPIMathJNI.class);
+        loader =
+            new RuntimeLoader<>(
+                "wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
         loader.loadLibrary();
       } catch (IOException ex) {
         ex.printStackTrace();
@@ -39,8 +36,9 @@
     if (libraryLoaded) {
       return;
     }
-    loader = new RuntimeLoader<>("wpimathjni", RuntimeLoader.getDefaultExtractionRoot(),
-            WPIMathJNI.class);
+    loader =
+        new RuntimeLoader<>(
+            "wpimathjni", RuntimeLoader.getDefaultExtractionRoot(), WPIMathJNI.class);
     loader.loadLibrary();
     libraryLoaded = true;
   }
@@ -48,47 +46,85 @@
   /**
    * Solves the discrete alegebraic Riccati equation.
    *
-   * @param A      Array containing elements of A in row-major order.
-   * @param B      Array containing elements of B in row-major order.
-   * @param Q      Array containing elements of Q in row-major order.
-   * @param R      Array containing elements of R in row-major order.
+   * @param A Array containing elements of A in row-major order.
+   * @param B Array containing elements of B in row-major order.
+   * @param Q Array containing elements of Q in row-major order.
+   * @param R Array containing elements of R in row-major order.
    * @param states Number of states in A matrix.
    * @param inputs Number of inputs in B matrix.
-   * @param S      Array storage for DARE solution.
+   * @param S Array storage for DARE solution.
    */
   public static native void discreteAlgebraicRiccatiEquation(
-          double[] A,
-          double[] B,
-          double[] Q,
-          double[] R,
-          int states,
-          int inputs,
-          double[] S);
+      double[] A, double[] B, double[] Q, double[] R, int states, int inputs, double[] S);
 
   /**
    * Computes the matrix exp.
    *
-   * @param src  Array of elements of the matrix to be exponentiated.
-   * @param rows how many rows there are.
-   * @param dst  Array where the result will be stored.
+   * @param src Array of elements of the matrix to be exponentiated.
+   * @param rows How many rows there are.
+   * @param dst Array where the result will be stored.
    */
   public static native void exp(double[] src, int rows, double[] dst);
 
   /**
+   * Computes the matrix pow.
+   *
+   * @param src Array of elements of the matrix to be raised to a power.
+   * @param rows How many rows there are.
+   * @param exponent The exponent.
+   * @param dst Array where the result will be stored.
+   */
+  public static native void pow(double[] src, int rows, double exponent, double[] dst);
+
+  /**
    * Returns true if (A, B) is a stabilizable pair.
    *
-   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
-   * any, have absolute values less than one, where an eigenvalue is
-   * uncontrollable if rank(lambda * I - A, B) &lt; n where n is number of states.
+   * <p>(A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if any, have
+   * absolute values less than one, where an eigenvalue is uncontrollable if rank(lambda * I - A, B)
+   * &lt; n where n is the number of states.
    *
    * @param states the number of states of the system.
    * @param inputs the number of inputs to the system.
-   * @param A      System matrix.
-   * @param B      Input matrix.
+   * @param A System matrix.
+   * @param B Input matrix.
    * @return If the system is stabilizable.
    */
   public static native boolean isStabilizable(int states, int inputs, double[] A, double[] B);
 
+  /**
+   * Loads a Pathweaver JSON.
+   *
+   * @param path The path to the JSON.
+   * @return A double array with the trajectory states from the JSON.
+   * @throws IOException if the JSON could not be read.
+   */
+  public static native double[] fromPathweaverJson(String path) throws IOException;
+
+  /**
+   * Converts a trajectory into a Pathweaver JSON and saves it.
+   *
+   * @param elements The elements of the trajectory.
+   * @param path The location to save the JSON to.
+   * @throws IOException if the JSON could not be written.
+   */
+  public static native void toPathweaverJson(double[] elements, String path) throws IOException;
+
+  /**
+   * Deserializes a trajectory JSON into a double[] of trajectory elements.
+   *
+   * @param json The JSON containing the serialized trajectory.
+   * @return A double array with the trajectory states.
+   */
+  public static native double[] deserializeTrajectory(String json);
+
+  /**
+   * Serializes the trajectory into a JSON string.
+   *
+   * @param elements The elements of the trajectory.
+   * @return A JSON containing the serialized trajectory.
+   */
+  public static native String serializeTrajectory(double[] elements);
+
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
new file mode 100644
index 0000000..2991511
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java
@@ -0,0 +1,138 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+/**
+ * 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).
+ */
+@SuppressWarnings("MemberName")
+public class ArmFeedforward {
+  public final double ks;
+  public final double kcos;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. Units of the gain values will dictate
+   * units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kcos The gravity gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
+    this.ks = ks;
+    this.kcos = kcos;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ArmFeedforward with the specified gains. Acceleration gain is defaulted to zero.
+   * Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kcos The gravity gain.
+   * @param kv The velocity gain.
+   */
+  public ArmFeedforward(double ks, double kcos, double kv) {
+    this(ks, kcos, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param positionRadians The position (angle) setpoint.
+   * @param velocityRadPerSec The velocity setpoint.
+   * @param accelRadPerSecSquared The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(
+      double positionRadians, double velocityRadPerSec, double accelRadPerSecSquared) {
+    return ks * Math.signum(velocityRadPerSec)
+        + kcos * Math.cos(positionRadians)
+        + kv * velocityRadPerSec
+        + ka * accelRadPerSecSquared;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
+   * zero).
+   *
+   * @param positionRadians The position (angle) setpoint.
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double positionRadians, double velocity) {
+    return calculate(positionRadians, velocity, 0);
+  }
+
+  // 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.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / 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.
+   */
+  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / 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.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / 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.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
new file mode 100644
index 0000000..d4beabd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforward.java
@@ -0,0 +1,193 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+/**
+ * Constructs a control-affine plant inversion model-based feedforward from given model dynamics.
+ *
+ * <p>If given the vector valued function as f(x, u) where x is the state vector and u is the input
+ * vector, the B matrix(continuous input matrix) is calculated through a {@link
+ * edu.wpi.first.math.system.NumericalJacobian}. In this case f has to be control-affine (of the
+ * form f(x) + Bu).
+ *
+ * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
+ * <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * <p>This feedforward does not account for a dynamic B matrix, B is either determined or supplied
+ * when the feedforward is created and remains constant.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
+public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
+  /** The current reference state. */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_r;
+
+  /** The computed feedforward. */
+  private Matrix<Inputs, N1> m_uff;
+
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Inputs> m_B;
+
+  private final Nat<Inputs> m_inputs;
+
+  private final double m_dt;
+
+  /** The model dynamics. */
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state and input.
+   *
+   * @param states A {@link Nat} representing the number of states.
+   * @param inputs A {@link Nat} representing the number of inputs.
+   * @param f A vector-valued function of x, the state, and u, the input, that returns the
+   *     derivative of the state vector. HAS to be control-affine (of the form f(x) + Bu).
+   * @param dtSeconds The timestep between calls of calculate().
+   */
+  public ControlAffinePlantInversionFeedforward(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dtSeconds) {
+    this.m_dt = dtSeconds;
+    this.m_f = f;
+    this.m_inputs = inputs;
+
+    this.m_B =
+        NumericalJacobian.numericalJacobianU(
+            states, inputs, m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_uff = new Matrix<>(inputs, Nat.N1());
+
+    reset();
+  }
+
+  /**
+   * Constructs a feedforward with given model dynamics as a function of state, and the plant's
+   * B(continuous input matrix) matrix.
+   *
+   * @param states A {@link Nat} representing the number of states.
+   * @param inputs A {@link Nat} representing the number of inputs.
+   * @param f A vector-valued function of x, the state, that returns the derivative of the state
+   *     vector.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param dtSeconds The timestep between calls of calculate().
+   */
+  public ControlAffinePlantInversionFeedforward(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Function<Matrix<States, N1>, Matrix<States, N1>> f,
+      Matrix<States, Inputs> B,
+      double dtSeconds) {
+    this.m_dt = dtSeconds;
+    this.m_inputs = inputs;
+
+    this.m_f = (x, u) -> f.apply(x);
+    this.m_B = B;
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_uff = new Matrix<>(inputs, Nat.N1());
+
+    reset();
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> getUff() {
+    return m_uff;
+  }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   * @return The row of the calculated feedforward.
+   */
+  public double getUff(int row) {
+    return m_uff.get(row, 0);
+  }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the current reference vector r.
+   *
+   * @param row Row of r.
+   * @return The row of the current reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /** Resets the feedforward with a zero initial state vector. */
+  public void reset() {
+    m_r.fill(0.0);
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Calculate the feedforward with only the desired future reference. This uses the internally
+   * stored "current" reference.
+   *
+   * <p>If this method is used the initial state of the system is the one set using {@link
+   * LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
+   * a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
+    return calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   * @return The calculated feedforward.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    var rDot = (nextR.minus(r)).div(m_dt);
+
+    m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
new file mode 100644
index 0000000..248015f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+/**
+ * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor acting
+ * against the force of gravity).
+ */
+@SuppressWarnings("MemberName")
+public class ElevatorFeedforward {
+  public final double ks;
+  public final double kg;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. Units of the gain values will
+   * dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
+    this.ks = ks;
+    this.kg = kg;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new ElevatorFeedforward with the specified gains. Acceleration gain is defaulted to
+   * zero. Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kg The gravity gain.
+   * @param kv The velocity gain.
+   */
+  public ElevatorFeedforward(double ks, double kg, double kv) {
+    this(ks, kg, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
+   * zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  // 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.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - kg - acceleration * ka) / 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.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - kg - acceleration * ka) / 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.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / 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.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
new file mode 100644
index 0000000..be813cc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/HolonomicDriveController.java
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * This holonomic drive controller can be used to follow trajectories using a holonomic drivetrain
+ * (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.
+ *
+ * <p>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.
+ */
+@SuppressWarnings("MemberName")
+public class HolonomicDriveController {
+  private Pose2d m_poseError = new Pose2d();
+  private Rotation2d m_rotationError = new Rotation2d();
+  private Pose2d m_poseTolerance = new Pose2d();
+  private boolean m_enabled = true;
+
+  private final PIDController m_xController;
+  private final PIDController m_yController;
+  private final ProfiledPIDController m_thetaController;
+
+  private boolean m_firstRun = true;
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("ParameterName")
+  public HolonomicDriveController(
+      PIDController xController, PIDController yController, ProfiledPIDController thetaController) {
+    m_xController = xController;
+    m_yController = yController;
+    m_thetaController = thetaController;
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    final var eTranslate = m_poseError.getTranslation();
+    final var eRotate = m_rotationError;
+    final var tolTranslate = m_poseTolerance.getTranslation();
+    final var tolRotate = m_poseTolerance.getRotation();
+    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerance for use with atReference().
+   *
+   * @param tolerance The pose error which is tolerable.
+   */
+  public void setTolerance(Pose2d tolerance) {
+    m_poseTolerance = tolerance;
+  }
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * @param currentPose The current pose.
+   * @param poseRef The desired pose.
+   * @param linearVelocityRefMeters The linear velocity reference.
+   * @param angleRef The angular reference.
+   * @return The next output of the holonomic drive controller.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(
+      Pose2d currentPose, Pose2d poseRef, double linearVelocityRefMeters, Rotation2d angleRef) {
+    // If this is the first run, then we need to reset the theta controller to the current pose's
+    // heading.
+    if (m_firstRun) {
+      m_thetaController.reset(currentPose.getRotation().getRadians());
+      m_firstRun = false;
+    }
+
+    // Calculate feedforward velocities (field-relative).
+    double xFF = linearVelocityRefMeters * poseRef.getRotation().getCos();
+    double yFF = linearVelocityRefMeters * poseRef.getRotation().getSin();
+    double thetaFF =
+        m_thetaController.calculate(currentPose.getRotation().getRadians(), angleRef.getRadians());
+
+    m_poseError = poseRef.relativeTo(currentPose);
+    m_rotationError = angleRef.minus(currentPose.getRotation());
+
+    if (!m_enabled) {
+      return ChassisSpeeds.fromFieldRelativeSpeeds(xFF, yFF, thetaFF, currentPose.getRotation());
+    }
+
+    // Calculate feedback velocities (based on position error).
+    double xFeedback = m_xController.calculate(currentPose.getX(), poseRef.getX());
+    double yFeedback = m_yController.calculate(currentPose.getY(), poseRef.getY());
+
+    // Return next output.
+    return ChassisSpeeds.fromFieldRelativeSpeeds(
+        xFF + xFeedback, yFF + yFeedback, thetaFF, currentPose.getRotation());
+  }
+
+  /**
+   * Returns the next output of the holonomic drive controller.
+   *
+   * @param currentPose The current pose.
+   * @param desiredState The desired trajectory state.
+   * @param angleRef The desired end-angle.
+   * @return The next output of the holonomic drive controller.
+   */
+  public ChassisSpeeds calculate(
+      Pose2d currentPose, Trajectory.State desiredState, Rotation2d angleRef) {
+    return calculate(
+        currentPose, desiredState.poseMeters, desiredState.velocityMetersPerSecond, angleRef);
+  }
+
+  /**
+   * Enables and disables the controller for troubleshooting problems. When calculate() is called on
+   * a disabled controller, only feedforward values are returned.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabled = enabled;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
new file mode 100644
index 0000000..627c272
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforward.java
@@ -0,0 +1,153 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
+ *
+ * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
+ * where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
+public class LinearPlantInversionFeedforward<
+    States extends Num, Inputs extends Num, Outputs extends Num> {
+  /** The current reference state. */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_r;
+
+  /** The computed feedforward. */
+  private Matrix<Inputs, N1> m_uff;
+
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Inputs> m_B;
+
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, States> m_A;
+
+  /**
+   * Constructs a feedforward with the given plant.
+   *
+   * @param plant The plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  public LinearPlantInversionFeedforward(
+      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds) {
+    this(plant.getA(), plant.getB(), dtSeconds);
+  }
+
+  /**
+   * Constructs a feedforward with the given coefficients.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearPlantInversionFeedforward(
+      Matrix<States, States> A, Matrix<States, Inputs> B, double dtSeconds) {
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    this.m_A = discABPair.getFirst();
+    this.m_B = discABPair.getSecond();
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_uff = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    reset();
+  }
+
+  /**
+   * Returns the previously calculated feedforward as an input vector.
+   *
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> getUff() {
+    return m_uff;
+  }
+
+  /**
+   * Returns an element of the previously calculated feedforward.
+   *
+   * @param row Row of uff.
+   * @return The row of the calculated feedforward.
+   */
+  public double getUff(int row) {
+    return m_uff.get(row, 0);
+  }
+
+  /**
+   * Returns the current reference vector r.
+   *
+   * @return The current reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the current reference vector r.
+   *
+   * @param row Row of r.
+   * @return The row of the current reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Resets the feedforward with a specified initial state vector.
+   *
+   * @param initialState The initial state vector.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_r = initialState;
+    m_uff.fill(0.0);
+  }
+
+  /** Resets the feedforward with a zero initial state vector. */
+  public void reset() {
+    m_r.fill(0.0);
+    m_uff.fill(0.0);
+  }
+
+  /**
+   * Calculate the feedforward with only the desired future reference. This uses the internally
+   * stored "current" reference.
+   *
+   * <p>If this method is used the initial state of the system is the one set using {@link
+   * LinearPlantInversionFeedforward#reset(Matrix)}. If the initial state is not set it defaults to
+   * a zero vector.
+   *
+   * @param nextR The reference state of the future timestep (k + dt).
+   * @return The calculated feedforward.
+   */
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
+    return calculate(m_r, nextR);
+  }
+
+  /**
+   * Calculate the feedforward with current and future reference vectors.
+   *
+   * @param r The reference state of the current timestep (k).
+   * @param nextR The reference state of the future timestep (k + dt).
+   * @return The calculated feedforward.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
+    m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
+
+    m_r = nextR;
+    return m_uff;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
new file mode 100644
index 0000000..cf4b26d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/LinearQuadraticRegulator.java
@@ -0,0 +1,281 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.LinearSystem;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Contains the controller coefficients and logic for a linear-quadratic regulator (LQR). LQRs use
+ * the control law u = K(r - x).
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearQuadraticRegulator<States extends Num, Inputs extends Num, Outputs extends Num> {
+  /** The current reference state. */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_r;
+
+  /** The computed and capped controller output. */
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, N1> m_u;
+
+  // Controller gain.
+  @SuppressWarnings("MemberName")
+  private Matrix<Inputs, States> m_K;
+
+  /**
+   * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
+   *
+   * @param plant The plant being controlled.
+   * @param qelms The maximum desired error tolerance for each state.
+   * @param relms The maximum desired control effort for each input.
+   * @param dtSeconds Discretization timestep.
+   */
+  public LinearQuadraticRegulator(
+      LinearSystem<States, Inputs, Outputs> plant,
+      Vector<States> qelms,
+      Vector<Inputs> relms,
+      double dtSeconds) {
+    this(
+        plant.getA(),
+        plant.getB(),
+        StateSpaceUtil.makeCostMatrix(qelms),
+        StateSpaceUtil.makeCostMatrix(relms),
+        dtSeconds);
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param qelms The maximum desired error tolerance for each state.
+   * @param relms The maximum desired control effort for each input.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Vector<States> qelms,
+      Vector<Inputs> relms,
+      double dtSeconds) {
+    this(
+        A,
+        B,
+        StateSpaceUtil.makeCostMatrix(qelms),
+        StateSpaceUtil.makeCostMatrix(relms),
+        dtSeconds);
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param Q The state cost matrix.
+   * @param R The input cost matrix.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      double dtSeconds) {
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    if (!StateSpaceUtil.isStabilizable(discA, discB)) {
+      var builder = new StringBuilder("The system passed to the LQR is uncontrollable!\n\nA =\n");
+      builder.append(discA.getStorage().toString());
+      builder.append("\nB =\n");
+      builder.append(discB.getStorage().toString());
+      builder.append("\n");
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    var temp = discB.transpose().times(S).times(discB).plus(R);
+    m_K = temp.solve(discB.transpose().times(S).times(discA));
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    reset();
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A Continuous system matrix of the plant being controlled.
+   * @param B Continuous input matrix of the plant being controlled.
+   * @param Q The state cost matrix.
+   * @param R The input cost matrix.
+   * @param N The state-input cross-term cost matrix.
+   * @param dtSeconds Discretization timestep.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public LinearQuadraticRegulator(
+      Matrix<States, States> A,
+      Matrix<States, Inputs> B,
+      Matrix<States, States> Q,
+      Matrix<Inputs, Inputs> R,
+      Matrix<States, Inputs> N,
+      double dtSeconds) {
+    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+
+    // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+    var temp = discB.transpose().times(S).times(discB).plus(R);
+    m_K = temp.solve(discB.transpose().times(S).times(discA).plus(N.transpose()));
+
+    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
+    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
+
+    reset();
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param states The number of states.
+   * @param inputs The number of inputs.
+   * @param k The gain matrix.
+   */
+  @SuppressWarnings("ParameterName")
+  public LinearQuadraticRegulator(
+      Nat<States> states, Nat<Inputs> inputs, Matrix<Inputs, States> k) {
+    m_K = k;
+
+    m_r = new Matrix<>(states, Nat.N1());
+    m_u = new Matrix<>(inputs, Nat.N1());
+
+    reset();
+  }
+
+  /**
+   * Returns the control input vector u.
+   *
+   * @return The control input.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return m_u;
+  }
+
+  /**
+   * Returns an element of the control input vector u.
+   *
+   * @param row Row of u.
+   * @return The row of the control input vector.
+   */
+  public double getU(int row) {
+    return m_u.get(row, 0);
+  }
+
+  /**
+   * Returns the reference vector r.
+   *
+   * @return The reference vector.
+   */
+  public Matrix<States, N1> getR() {
+    return m_r;
+  }
+
+  /**
+   * Returns an element of the reference vector r.
+   *
+   * @param row Row of r.
+   * @return The row of the reference vector.
+   */
+  public double getR(int row) {
+    return m_r.get(row, 0);
+  }
+
+  /**
+   * Returns the controller matrix K.
+   *
+   * @return the controller matrix K.
+   */
+  public Matrix<Inputs, States> getK() {
+    return m_K;
+  }
+
+  /** Resets the controller. */
+  public void reset() {
+    m_r.fill(0.0);
+    m_u.fill(0.0);
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @return The next controller output.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
+    m_u = m_K.times(m_r.minus(x));
+    return m_u;
+  }
+
+  /**
+   * Returns the next output of the controller.
+   *
+   * @param x The current state x.
+   * @param nextR the next reference vector r.
+   * @return The next controller output.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
+    m_r = nextR;
+    return calculate(x);
+  }
+
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the input.
+   *
+   * <p>Linear-Quadratic regulator controller gains tend to be aggressive. If sensor measurements
+   * are time-delayed too long, the LQR may be unstable. However, if we know the amount of delay, we
+   * can compute the control based on where the system will be after the time delay.
+   *
+   * <p>See https://file.tavsys.net/control/controls-engineering-in-frc.pdf appendix C.4 for a
+   * derivation.
+   *
+   * @param plant The plant being controlled.
+   * @param dtSeconds Discretization timestep in seconds.
+   * @param inputDelaySeconds Input time delay in seconds.
+   */
+  public void latencyCompensate(
+      LinearSystem<States, Inputs, Outputs> plant, double dtSeconds, double inputDelaySeconds) {
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dtSeconds);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    m_K = m_K.times((discA.minus(discB.times(m_K))).pow(inputDelaySeconds / dtSeconds));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
new file mode 100644
index 0000000..12c4175
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/PIDController.java
@@ -0,0 +1,355 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+import edu.wpi.first.util.sendable.SendableRegistry;
+
+/** Implements a PID control loop. */
+public class PIDController implements Sendable, AutoCloseable {
+  private static int instances;
+
+  // Factor for "proportional" control
+  private double m_kp;
+
+  // Factor for "integral" control
+  private double m_ki;
+
+  // Factor for "derivative" control
+  private double m_kd;
+
+  // The period (in seconds) of the loop that calls the controller
+  private final double m_period;
+
+  private double m_maximumIntegral = 1.0;
+
+  private double m_minimumIntegral = -1.0;
+
+  private double m_maximumInput;
+
+  private double m_minimumInput;
+
+  // Do the endpoints wrap around? eg. Absolute encoder
+  private boolean m_continuous;
+
+  // The error at the time of the most recent call to calculate()
+  private double m_positionError;
+  private double m_velocityError;
+
+  // The error at the time of the second-most-recent call to calculate() (used to compute velocity)
+  private double m_prevError;
+
+  // The sum of the errors for use in the integral calc
+  private double m_totalError;
+
+  // The error that is considered at setpoint.
+  private double m_positionTolerance = 0.05;
+  private double m_velocityTolerance = Double.POSITIVE_INFINITY;
+
+  private double m_setpoint;
+  private double m_measurement;
+
+  /**
+   * Allocates a PIDController with the given constants for kp, ki, and kd and a default period of
+   * 0.02 seconds.
+   *
+   * @param kp The proportional coefficient.
+   * @param ki The integral coefficient.
+   * @param kd The derivative coefficient.
+   */
+  public PIDController(double kp, double ki, double kd) {
+    this(kp, ki, kd, 0.02);
+  }
+
+  /**
+   * Allocates a PIDController with the given constants for kp, ki, and kd.
+   *
+   * @param kp The proportional coefficient.
+   * @param ki The integral coefficient.
+   * @param kd The derivative coefficient.
+   * @param period The period between controller updates in seconds. Must be non-zero and positive.
+   */
+  public PIDController(double kp, double ki, double kd, double period) {
+    m_kp = kp;
+    m_ki = ki;
+    m_kd = kd;
+
+    if (period <= 0) {
+      throw new IllegalArgumentException("Controller period must be a non-zero positive number!");
+    }
+    m_period = period;
+
+    instances++;
+    SendableRegistry.addLW(this, "PIDController", instances);
+
+    MathSharedStore.reportUsage(MathUsageId.kController_PIDController2, instances);
+  }
+
+  @Override
+  public void close() {
+    SendableRegistry.remove(this);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Set the proportional, integral, and differential coefficients.
+   *
+   * @param kp The proportional coefficient.
+   * @param ki The integral coefficient.
+   * @param kd The derivative coefficient.
+   */
+  public void setPID(double kp, double ki, double kd) {
+    m_kp = kp;
+    m_ki = ki;
+    m_kd = kd;
+  }
+
+  /**
+   * Sets the Proportional coefficient of the PID controller gain.
+   *
+   * @param kp proportional coefficient
+   */
+  public void setP(double kp) {
+    m_kp = kp;
+  }
+
+  /**
+   * Sets the Integral coefficient of the PID controller gain.
+   *
+   * @param ki integral coefficient
+   */
+  public void setI(double ki) {
+    m_ki = ki;
+  }
+
+  /**
+   * Sets the Differential coefficient of the PID controller gain.
+   *
+   * @param kd differential coefficient
+   */
+  public void setD(double kd) {
+    m_kd = kd;
+  }
+
+  /**
+   * Get the Proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_kp;
+  }
+
+  /**
+   * Get the Integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_ki;
+  }
+
+  /**
+   * Get the Differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_kd;
+  }
+
+  /**
+   * Returns the period of this controller.
+   *
+   * @return the period of the controller.
+   */
+  public double getPeriod() {
+    return m_period;
+  }
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  public void setSetpoint(double setpoint) {
+    m_setpoint = setpoint;
+  }
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  public double getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the setpoint.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return Whether the error is within the acceptable bounds.
+   */
+  public boolean atSetpoint() {
+    double positionError;
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      positionError = m_setpoint - m_measurement;
+    }
+
+    double velocityError = (positionError - m_prevError) / m_period;
+
+    return Math.abs(positionError) < m_positionTolerance
+        && Math.abs(velocityError) < m_velocityTolerance;
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_continuous = true;
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /** Disables continuous input. */
+  public void disableContinuousInput() {
+    m_continuous = false;
+  }
+
+  /**
+   * Returns true if continuous input is enabled.
+   *
+   * @return True if continuous input is enabled.
+   */
+  public boolean isContinuousInputEnabled() {
+    return m_continuous;
+  }
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * <p>When the cap is reached, the integrator value is added to the controller output rather than
+   * the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_minimumIntegral = minimumIntegral;
+    m_maximumIntegral = maximumIntegral;
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance) {
+    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance, double velocityTolerance) {
+    m_positionTolerance = positionTolerance;
+    m_velocityTolerance = velocityTolerance;
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getPositionError() {
+    return m_positionError;
+  }
+
+  /**
+   * Returns the velocity error.
+   *
+   * @return The velocity error.
+   */
+  public double getVelocityError() {
+    return m_velocityError;
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   * @return The next controller output.
+   */
+  public double calculate(double measurement, double setpoint) {
+    // Set setpoint to provided value
+    setSetpoint(setpoint);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @return The next controller output.
+   */
+  public double calculate(double measurement) {
+    m_measurement = measurement;
+    m_prevError = m_positionError;
+
+    if (m_continuous) {
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      m_positionError = MathUtil.inputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+    } else {
+      m_positionError = m_setpoint - measurement;
+    }
+
+    m_velocityError = (m_positionError - m_prevError) / m_period;
+
+    if (m_ki != 0) {
+      m_totalError =
+          MathUtil.clamp(
+              m_totalError + m_positionError * m_period,
+              m_minimumIntegral / m_ki,
+              m_maximumIntegral / m_ki);
+    }
+
+    return m_kp * m_positionError + m_ki * m_totalError + m_kd * m_velocityError;
+  }
+
+  /** Resets the previous error and the integral term. */
+  public void reset() {
+    m_prevError = 0;
+    m_totalError = 0;
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("PIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("setpoint", this::getSetpoint, this::setSetpoint);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
new file mode 100644
index 0000000..3ebcbd8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java
@@ -0,0 +1,381 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.util.sendable.Sendable;
+import edu.wpi.first.util.sendable.SendableBuilder;
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid profile. Users should
+ * call reset() when they first start running the controller to avoid unwanted behavior.
+ */
+public class ProfiledPIDController implements Sendable {
+  private static int instances;
+
+  private PIDController m_controller;
+  private double m_minimumInput;
+  private double m_maximumInput;
+  private TrapezoidProfile.State m_goal = new TrapezoidProfile.State();
+  private TrapezoidProfile.State m_setpoint = new TrapezoidProfile.State();
+  private TrapezoidProfile.Constraints m_constraints;
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  @SuppressWarnings("ParameterName")
+  public ProfiledPIDController(
+      double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints) {
+    this(Kp, Ki, Kd, constraints, 0.02);
+  }
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp The proportional coefficient.
+   * @param Ki The integral coefficient.
+   * @param Kd The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @param period The period between controller updates in seconds. The default is 0.02 seconds.
+   */
+  @SuppressWarnings("ParameterName")
+  public ProfiledPIDController(
+      double Kp, double Ki, double Kd, TrapezoidProfile.Constraints constraints, double period) {
+    m_controller = new PIDController(Kp, Ki, Kd, period);
+    m_constraints = constraints;
+    instances++;
+    MathSharedStore.reportUsage(MathUsageId.kController_ProfiledPIDController, instances);
+  }
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * <p>Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setPID(double Kp, double Ki, double Kd) {
+    m_controller.setPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setP(double Kp) {
+    m_controller.setP(Kp);
+  }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setI(double Ki) {
+    m_controller.setI(Ki);
+  }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  @SuppressWarnings("ParameterName")
+  public void setD(double Kd) {
+    m_controller.setD(Kd);
+  }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  public double getP() {
+    return m_controller.getP();
+  }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  public double getI() {
+    return m_controller.getI();
+  }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  public double getD() {
+    return m_controller.getD();
+  }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  public double getPeriod() {
+    return m_controller.getPeriod();
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal state.
+   */
+  public void setGoal(TrapezoidProfile.State goal) {
+    m_goal = goal;
+  }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired goal position.
+   */
+  public void setGoal(double goal) {
+    m_goal = new TrapezoidProfile.State(goal, 0);
+  }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   *
+   * @return The goal.
+   */
+  public TrapezoidProfile.State getGoal() {
+    return m_goal;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return True if the error is within the tolerance of the error.
+   */
+  public boolean atGoal() {
+    return atSetpoint() && m_goal.equals(m_setpoint);
+  }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  public void setConstraints(TrapezoidProfile.Constraints constraints) {
+    m_constraints = constraints;
+  }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  public TrapezoidProfile.State getSetpoint() {
+    return m_setpoint;
+  }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * <p>This will return false until at least one input value has been computed.
+   *
+   * @return True if the error is within the tolerance of the error.
+   */
+  public boolean atSetpoint() {
+    return m_controller.atSetpoint();
+  }
+
+  /**
+   * Enables continuous input.
+   *
+   * <p>Rather then using the max and min input range as constraints, it considers them to be the
+   * same point and automatically calculates the shortest route to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  public void enableContinuousInput(double minimumInput, double maximumInput) {
+    m_controller.enableContinuousInput(minimumInput, maximumInput);
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /** Disables continuous input. */
+  public void disableContinuousInput() {
+    m_controller.disableContinuousInput();
+  }
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * <p>When the cap is reached, the integrator value is added to the controller output rather than
+   * the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  public void setIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.setIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance) {
+    setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with atSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  public void setTolerance(double positionTolerance, double velocityTolerance) {
+    m_controller.setTolerance(positionTolerance, velocityTolerance);
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  public double getPositionError() {
+    return m_controller.getPositionError();
+  }
+
+  /**
+   * Returns the change in error per second.
+   *
+   * @return The change in error per second.
+   */
+  public double getVelocityError() {
+    return m_controller.getVelocityError();
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @return The controller's next output.
+   */
+  public double calculate(double measurement) {
+    if (m_controller.isContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      double goalMinDistance =
+          MathUtil.inputModulus(m_goal.position - measurement, -errorBound, errorBound);
+      double setpointMinDistance =
+          MathUtil.inputModulus(m_setpoint.position - measurement, -errorBound, errorBound);
+
+      // 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;
+    }
+
+    var profile = new TrapezoidProfile(m_constraints, m_goal, m_setpoint);
+    m_setpoint = profile.calculate(getPeriod());
+    return m_controller.calculate(measurement, m_setpoint.position);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @return The controller's next output.
+   */
+  public double calculate(double measurement, TrapezoidProfile.State goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PIDController.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @return The controller's next output.
+   */
+  public double calculate(double measurement, double goal) {
+    setGoal(goal);
+    return calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @return The controller's next output.
+   */
+  public double calculate(
+      double measurement, TrapezoidProfile.State goal, TrapezoidProfile.Constraints constraints) {
+    setConstraints(constraints);
+    return calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
+   */
+  public void reset(TrapezoidProfile.State measurement) {
+    m_controller.reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  public void reset(double measuredPosition, double measuredVelocity) {
+    reset(new TrapezoidProfile.State(measuredPosition, measuredVelocity));
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The velocity is assumed to
+   *     be zero.
+   */
+  public void reset(double measuredPosition) {
+    reset(measuredPosition, 0.0);
+  }
+
+  @Override
+  public void initSendable(SendableBuilder builder) {
+    builder.setSmartDashboardType("ProfiledPIDController");
+    builder.addDoubleProperty("p", this::getP, this::setP);
+    builder.addDoubleProperty("i", this::getI, this::setI);
+    builder.addDoubleProperty("d", this::getD, this::setD);
+    builder.addDoubleProperty("goal", () -> getGoal().position, this::setGoal);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
new file mode 100644
index 0000000..0be592e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/RamseteController.java
@@ -0,0 +1,172 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
+ * to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law
+ * in addition to the linear ones we have used so far like PID? If we use the original approach with
+ * PID controllers for left and right position and velocity states, the controllers only deal with
+ * the local pose. If the robot deviates from the path, there is no way for the controllers to
+ * correct and the robot may not reach the desired global pose. This is due to multiple endpoints
+ * existing for the robot which have the same encoder path arc lengths.
+ *
+ * <p>Instead of using wheel path arc lengths (which are in the robot's local coordinate frame),
+ * nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this
+ * extra information to guide a linear reference tracker like the PID controllers back in by
+ * adjusting the references of the PID controllers.
+ *
+ * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear
+ * controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y,
+ * and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because
+ * that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e
+ * Mobile per i SErvizi e le TEcnologie").
+ *
+ * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
+ * Engineering in the FIRST Robotics Competition</a> section on Ramsete unicycle controller for a
+ * derivation and analysis.
+ */
+public class RamseteController {
+  @SuppressWarnings("MemberName")
+  private final double m_b;
+
+  @SuppressWarnings("MemberName")
+  private final double m_zeta;
+
+  private Pose2d m_poseError = new Pose2d();
+  private Pose2d m_poseTolerance = new Pose2d();
+  private boolean m_enabled = true;
+
+  /**
+   * Construct a Ramsete unicycle controller.
+   *
+   * @param b Tuning parameter (b &gt; 0) for which larger values make convergence more aggressive
+   *     like a proportional term.
+   * @param zeta Tuning parameter (0 &lt; zeta &lt; 1) for which larger values provide more damping
+   *     in response.
+   */
+  @SuppressWarnings("ParameterName")
+  public RamseteController(double b, double zeta) {
+    m_b = b;
+    m_zeta = zeta;
+  }
+
+  /**
+   * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 and 0.7
+   * have been well-tested to produce desirable results.
+   */
+  public RamseteController() {
+    this(2.0, 0.7);
+  }
+
+  /**
+   * Returns true if the pose error is within tolerance of the reference.
+   *
+   * @return True if the pose error is within tolerance of the reference.
+   */
+  public boolean atReference() {
+    final var eTranslate = m_poseError.getTranslation();
+    final var eRotate = m_poseError.getRotation();
+    final var tolTranslate = m_poseTolerance.getTranslation();
+    final var tolRotate = m_poseTolerance.getRotation();
+    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
+        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
+        && Math.abs(eRotate.getRadians()) < tolRotate.getRadians();
+  }
+
+  /**
+   * Sets the pose error which is considered tolerable for use with atReference().
+   *
+   * @param poseTolerance Pose error which is tolerable.
+   */
+  public void setTolerance(Pose2d poseTolerance) {
+    m_poseTolerance = poseTolerance;
+  }
+
+  /**
+   * Returns the next output of the Ramsete controller.
+   *
+   * <p>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 linearVelocityRefMeters The desired linear velocity in meters per second.
+   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
+   * @return The next controller output.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(
+      Pose2d currentPose,
+      Pose2d poseRef,
+      double linearVelocityRefMeters,
+      double angularVelocityRefRadiansPerSecond) {
+    if (!m_enabled) {
+      return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
+    }
+
+    m_poseError = poseRef.relativeTo(currentPose);
+
+    // Aliases for equation readability
+    final double eX = m_poseError.getX();
+    final double eY = m_poseError.getY();
+    final double eTheta = m_poseError.getRotation().getRadians();
+    final double vRef = linearVelocityRefMeters;
+    final double omegaRef = angularVelocityRefRadiansPerSecond;
+
+    double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
+
+    return new ChassisSpeeds(
+        vRef * m_poseError.getRotation().getCos() + k * eX,
+        0.0,
+        omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
+  }
+
+  /**
+   * Returns the next output of the Ramsete controller.
+   *
+   * <p>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.
+   * @return The next controller output.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
+    return calculate(
+        currentPose,
+        desiredState.poseMeters,
+        desiredState.velocityMetersPerSecond,
+        desiredState.velocityMetersPerSecond * desiredState.curvatureRadPerMeter);
+  }
+
+  /**
+   * Enables and disables the controller for troubleshooting purposes.
+   *
+   * @param enabled If the controller is enabled or not.
+   */
+  public void setEnabled(boolean enabled) {
+    m_enabled = enabled;
+  }
+
+  /**
+   * Returns sin(x) / x.
+   *
+   * @param x Value of which to take sinc(x).
+   */
+  @SuppressWarnings("ParameterName")
+  private static double sinc(double x) {
+    if (Math.abs(x) < 1e-9) {
+      return 1.0 - 1.0 / 6.0 * x * x;
+    } else {
+      return Math.sin(x) / x;
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
new file mode 100644
index 0000000..f985960
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java
@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+/** A helper class that computes feedforward outputs for a simple permanent-magnet DC motor. */
+@SuppressWarnings("MemberName")
+public class SimpleMotorFeedforward {
+  public final double ks;
+  public final double kv;
+  public final double ka;
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains. Units of the gain values will
+   * dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   * @param ka The acceleration gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv, double ka) {
+    this.ks = ks;
+    this.kv = kv;
+    this.ka = ka;
+  }
+
+  /**
+   * Creates a new SimpleMotorFeedforward with the specified gains. Acceleration gain is defaulted
+   * to zero. Units of the gain values will dictate units of the computed feedforward.
+   *
+   * @param ks The static gain.
+   * @param kv The velocity gain.
+   */
+  public SimpleMotorFeedforward(double ks, double kv) {
+    this(ks, kv, 0);
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param velocity The velocity setpoint.
+   * @param acceleration The acceleration setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity, double acceleration) {
+    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
+  }
+
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint.
+   * @param nextVelocity The next velocity setpoint.
+   * @param dtSeconds Time between velocity setpoints in seconds.
+   * @return The computed feedforward.
+   */
+  public double calculate(double currentVelocity, double nextVelocity, double dtSeconds) {
+    var plant = LinearSystemId.identifyVelocitySystem(this.kv, this.ka);
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, dtSeconds);
+
+    var r = Matrix.mat(Nat.N1(), Nat.N1()).fill(currentVelocity);
+    var nextR = Matrix.mat(Nat.N1(), Nat.N1()).fill(nextVelocity);
+
+    return ks * Math.signum(currentVelocity) + feedforward.calculate(r, nextR).get(0, 0);
+  }
+
+  // Rearranging the main equation from the calculate() method yields the
+  // formulas for the methods below:
+
+  /**
+   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to be
+   * zero).
+   *
+   * @param velocity The velocity setpoint.
+   * @return The computed feedforward.
+   */
+  public double calculate(double velocity) {
+    return calculate(velocity, 0);
+  }
+
+  /**
+   * 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.
+   */
+  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume max velocity is positive
+    return (maxVoltage - ks - acceleration * ka) / 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.
+   */
+  public double minAchievableVelocity(double maxVoltage, double acceleration) {
+    // Assume min velocity is negative, ks flips sign
+    return (-maxVoltage + ks - acceleration * ka) / 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.
+   */
+  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
+    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
+  }
+
+  /**
+   * 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 minimum possible acceleration at the given velocity.
+   */
+  public double minAchievableAcceleration(double maxVoltage, double velocity) {
+    return maxAchievableAcceleration(-maxVoltage, velocity);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
new file mode 100644
index 0000000..a6d8b44
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/AngleStatistics.java
@@ -0,0 +1,123 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.BiFunction;
+import org.ejml.simple.SimpleMatrix;
+
+public final class AngleStatistics {
+  private AngleStatistics() {
+    // Utility class
+  }
+
+  /**
+   * Subtracts a and b while normalizing the resulting value in the selected row as if it were an
+   * angle.
+   *
+   * @param <S> Number of rows in vector.
+   * @param a A vector to subtract from.
+   * @param b A vector to subtract with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Difference of two vectors with angle at the given index normalized.
+   */
+  public static <S extends Num> Matrix<S, N1> angleResidual(
+      Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
+    Matrix<S, N1> ret = a.minus(b);
+    ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that subtracts two vectors while normalizing the resulting value in the
+   * selected row as if it were an angle.
+   *
+   * @param <S> Number of rows in vector.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Function returning difference of two vectors with angle at the given index normalized.
+   */
+  public static <S extends Num>
+      BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>> angleResidual(int angleStateIdx) {
+    return (a, b) -> angleResidual(a, b, angleStateIdx);
+  }
+
+  /**
+   * Adds a and b while normalizing the resulting value in the selected row as an angle.
+   *
+   * @param <S> Number of rows in vector.
+   * @param a A vector to add with.
+   * @param b A vector to add with.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Sum of two vectors with angle at the given index normalized.
+   */
+  public static <S extends Num> Matrix<S, N1> angleAdd(
+      Matrix<S, N1> a, Matrix<S, N1> b, int angleStateIdx) {
+    Matrix<S, N1> ret = a.plus(b);
+    ret.set(angleStateIdx, 0, MathUtil.angleModulus(ret.get(angleStateIdx, 0)));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that adds two vectors while normalizing the resulting value in the selected
+   * row as an angle.
+   *
+   * @param <S> Number of rows in vector.
+   * @param angleStateIdx The row containing angles to be normalized.
+   * @return Function returning of two vectors with angle at the given index normalized.
+   */
+  public static <S extends Num> BiFunction<Matrix<S, N1>, Matrix<S, N1>, Matrix<S, N1>> angleAdd(
+      int angleStateIdx) {
+    return (a, b) -> angleAdd(a, b, angleStateIdx);
+  }
+
+  /**
+   * Computes the mean of sigmas with the weights Wm while computing a special angle mean for a
+   * select row.
+   *
+   * @param <S> Number of rows in sigma point matrix.
+   * @param sigmas Sigma points.
+   * @param Wm Weights for the mean.
+   * @param angleStateIdx The row containing the angles.
+   * @return Mean of sigma points.
+   */
+  @SuppressWarnings("checkstyle:ParameterName")
+  public static <S extends Num> Matrix<S, N1> angleMean(
+      Matrix<S, ?> sigmas, Matrix<?, N1> Wm, int angleStateIdx) {
+    double[] angleSigmas = sigmas.extractRowVector(angleStateIdx).getData();
+    Matrix<N1, ?> sinAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
+    Matrix<N1, ?> cosAngleSigmas = new Matrix<>(new SimpleMatrix(1, sigmas.getNumCols()));
+    for (int i = 0; i < angleSigmas.length; i++) {
+      sinAngleSigmas.set(0, i, Math.sin(angleSigmas[i]));
+      cosAngleSigmas.set(0, i, Math.cos(angleSigmas[i]));
+    }
+
+    double sumSin = sinAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum();
+    double sumCos = cosAngleSigmas.times(Matrix.changeBoundsUnchecked(Wm)).elementSum();
+
+    Matrix<S, N1> ret = sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+    ret.set(angleStateIdx, 0, Math.atan2(sumSin, sumCos));
+
+    return ret;
+  }
+
+  /**
+   * Returns a function that computes the mean of sigmas with the weights Wm while computing a
+   * special angle mean for a select row.
+   *
+   * @param <S> Number of rows in sigma point matrix.
+   * @param angleStateIdx The row containing the angles.
+   * @return Function returning mean of sigma points.
+   */
+  @SuppressWarnings("LambdaParameterName")
+  public static <S extends Num> BiFunction<Matrix<S, ?>, Matrix<?, N1>, Matrix<S, N1>> angleMean(
+      int angleStateIdx) {
+    return (sigmas, Wm) -> angleMean(sigmas, Wm, angleStateIdx);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
new file mode 100644
index 0000000..074362e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimator.java
@@ -0,0 +1,372 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.util.function.BiConsumer;
+
+/**
+ * This class wraps an {@link edu.wpi.first.math.estimator.UnscentedKalmanFilter Unscented Kalman
+ * Filter} to fuse latency-compensated vision measurements with differential drive encoder
+ * measurements. It will correct for noisy vision measurements and encoder drift. It is intended to
+ * be an easy drop-in for {@link edu.wpi.first.math.kinematics.DifferentialDriveOdometry}; in fact,
+ * if you never call {@link DifferentialDrivePoseEstimator#addVisionMeasurement} and only call
+ * {@link DifferentialDrivePoseEstimator#update} then this will behave exactly the same as
+ * DifferentialDriveOdometry.
+ *
+ * <p>{@link DifferentialDrivePoseEstimator#update} should be called every robot loop (if your robot
+ * loops are faster than the default then you should change the {@link
+ * DifferentialDrivePoseEstimator#DifferentialDrivePoseEstimator(Rotation2d, Pose2d, Matrix, Matrix,
+ * Matrix, double) nominal delta time}.) {@link DifferentialDrivePoseEstimator#addVisionMeasurement}
+ * can be called as infrequently as you want; if you never call it then this class will behave
+ * exactly like regular encoder odometry.
+ *
+ * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
+ * (y):
+ *
+ * <p><strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate system
+ * containing x position, y position, heading, left encoder distance, and right encoder distance.
+ *
+ * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
+ * velocity, and change in gyro heading.
+ *
+ * <p>NB: Using velocities make things considerably easier, because it means that teams don't have
+ * to worry about getting an accurate model. Basically, we suspect that it's easier for teams to get
+ * good encoder data than it is for them to perform system identification well enough to get a good
+ * model.
+ *
+ * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
+ * heading; or <strong>y = [dist_l, dist_r, theta] </strong> containing left encoder position, right
+ * encoder position, and gyro heading.
+ */
+public class DifferentialDrivePoseEstimator {
+  final UnscentedKalmanFilter<N5, N3, N3> m_observer; // Package-private to allow for unit testing
+  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final KalmanFilterLatencyCompensator<N5, N3, N3> m_latencyCompensator;
+
+  private final double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private Matrix<N3, N3> m_visionContR;
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param initialPoseMeters The starting pose estimate.
+   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
+   *     model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
+   *     with units in meters and radians.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [dist_l, dist_r, theta]ᵀ, with units in meters and radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public DifferentialDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      Matrix<N5, N1> stateStdDevs,
+      Matrix<N3, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    this(
+        gyroAngle,
+        initialPoseMeters,
+        stateStdDevs,
+        localMeasurementStdDevs,
+        visionMeasurementStdDevs,
+        0.02);
+  }
+
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param initialPoseMeters The starting pose estimate.
+   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
+   *     model's state estimates less. This matrix is in the form [x, y, theta, dist_l, dist_r]ᵀ,
+   *     with units in meters and radians.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [dist_l, dist_r, theta]ᵀ, with units in meters and radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   * @param nominalDtSeconds The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public DifferentialDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      Matrix<N5, N1> stateStdDevs,
+      Matrix<N3, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs,
+      double nominalDtSeconds) {
+    m_nominalDt = nominalDtSeconds;
+
+    m_observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N5(),
+            Nat.N3(),
+            this::f,
+            (x, u) -> VecBuilder.fill(x.get(3, 0), x.get(4, 0), x.get(2, 0)),
+            stateStdDevs,
+            localMeasurementStdDevs,
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleAdd(2),
+            m_nominalDt);
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    // Initialize vision R
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+    m_visionCorrect =
+        (u, y) ->
+            m_observer.correct(
+                Nat.N3(),
+                u,
+                y,
+                (x, u1) -> new Matrix<>(x.getStorage().extractMatrix(0, 3, 0, 1)),
+                m_visionContR,
+                AngleStatistics.angleMean(2),
+                AngleStatistics.angleResidual(2),
+                AngleStatistics.angleResidual(2),
+                AngleStatistics.angleAdd(2));
+
+    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    m_observer.setXhat(fillStateVector(initialPoseMeters, 0.0, 0.0));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+  }
+
+  @SuppressWarnings({"ParameterName", "MethodName"})
+  private Matrix<N5, N1> f(Matrix<N5, N1> x, Matrix<N3, N1> u) {
+    // Apply a rotation matrix. Note that we do *not* add x--Runge-Kutta does that for us.
+    var theta = x.get(2, 0);
+    var toFieldRotation =
+        new MatBuilder<>(Nat.N5(), Nat.N5())
+            .fill(
+                Math.cos(theta),
+                -Math.sin(theta),
+                0,
+                0,
+                0,
+                Math.sin(theta),
+                Math.cos(theta),
+                0,
+                0,
+                0,
+                0,
+                0,
+                1,
+                0,
+                0,
+                0,
+                0,
+                0,
+                1,
+                0,
+                0,
+                0,
+                0,
+                0,
+                1);
+    return toFieldRotation.times(
+        VecBuilder.fill(u.get(0, 0), u.get(1, 0), u.get(2, 0), u.get(0, 0), u.get(1, 0)));
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>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 poseMeters The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    // Reset state estimate and error covariance
+    m_observer.reset();
+    m_latencyCompensator.reset();
+
+    m_observer.setXhat(fillStateVector(poseMeters, 0.0, 0.0));
+
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_previousAngle = poseMeters.getRotation();
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return new Pose2d(
+        m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * DifferentialDrivePoseEstimator#update} every loop.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link
+   *     DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
+   *     since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
+   *     Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
+   *     source in this case.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+        Nat.N3(),
+        m_observer,
+        m_nominalDt,
+        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+        m_visionCorrect,
+        timestampSeconds);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * DifferentialDrivePoseEstimator#update} every loop.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * DifferentialDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link
+   *     DifferentialDrivePoseEstimator#updateWithTime} then you must use a timestamp with an epoch
+   *     since FPGA startup (i.e. the epoch of this timestamp is the same epoch as
+   *     Timer.getFPGATimestamp.) This means that you should use Timer.getFPGATimestamp as your time
+   *     source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
+   * should be called every loop.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
+   * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
+   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
+   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(
+      Rotation2d gyroAngle,
+      DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
+      double distanceLeftMeters,
+      double distanceRightMeters) {
+    return updateWithTime(
+        WPIUtilJNI.now() * 1.0e-6,
+        gyroAngle,
+        wheelVelocitiesMetersPerSecond,
+        distanceLeftMeters,
+        distanceRightMeters);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. Note that this
+   * should be called every loop.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyro angle.
+   * @param wheelVelocitiesMetersPerSecond The velocities of the wheels in meters per second.
+   * @param distanceLeftMeters The total distance travelled by the left wheel in meters since the
+   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @param distanceRightMeters The total distance travelled by the right wheel in meters since the
+   *     last time you called {@link DifferentialDrivePoseEstimator#resetPosition}.
+   * @return The estimated pose of the robot in meters.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public Pose2d updateWithTime(
+      double currentTimeSeconds,
+      Rotation2d gyroAngle,
+      DifferentialDriveWheelSpeeds wheelVelocitiesMetersPerSecond,
+      double distanceLeftMeters,
+      double distanceRightMeters) {
+    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    // Diff drive forward kinematics:
+    // v_c = (v_l + v_r) / 2
+    var wheelVels = wheelVelocitiesMetersPerSecond;
+    var u =
+        VecBuilder.fill(
+            (wheelVels.leftMetersPerSecond + wheelVels.rightMetersPerSecond) / 2,
+            0,
+            angle.minus(m_previousAngle).getRadians() / dt);
+    m_previousAngle = angle;
+
+    var localY = VecBuilder.fill(distanceLeftMeters, distanceRightMeters, angle.getRadians());
+    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_observer.predict(u, dt);
+    m_observer.correct(u, localY);
+
+    return getEstimatedPosition();
+  }
+
+  private static Matrix<N5, N1> fillStateVector(Pose2d pose, double leftDist, double rightDist) {
+    return VecBuilder.fill(
+        pose.getTranslation().getX(),
+        pose.getTranslation().getY(),
+        pose.getRotation().getRadians(),
+        leftDist,
+        rightDist);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
new file mode 100644
index 0000000..d4f9e56
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/ExtendedKalmanFilter.java
@@ -0,0 +1,372 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
+ * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
+ * of squares error in the state estimate. This K gain is used to correct the state estimate by some
+ * amount of the difference between the actual measurements and the measurements predicted by the
+ * model.
+ *
+ * <p>An extended Kalman filter supports nonlinear state and measurement models. It propagates the
+ * error covariance by linearizing the models around the state estimate, then applying the linear
+ * Kalman filter equations.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
+ * theory".
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements KalmanTypeFilter<States, Inputs, Outputs> {
+  private final Nat<States> m_states;
+  private final Nat<Outputs> m_outputs;
+
+  @SuppressWarnings("MemberName")
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+
+  @SuppressWarnings("MemberName")
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
+
+  private BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> m_residualFuncY;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
+
+  private final Matrix<States, States> m_contQ;
+  private final Matrix<States, States> m_initP;
+  private final Matrix<Outputs, Outputs> m_contR;
+
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_xHat;
+
+  @SuppressWarnings("MemberName")
+  private Matrix<States, States> m_P;
+
+  private double m_dtSeconds;
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param states a Nat representing the number of states.
+   * @param inputs a Nat representing the number of inputs.
+   * @param outputs a Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("ParameterName")
+  public ExtendedKalmanFilter(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dtSeconds) {
+    this(
+        states,
+        inputs,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        Matrix::minus,
+        Matrix::plus,
+        dtSeconds);
+  }
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param states a Nat representing the number of states.
+   * @param inputs a Nat representing the number of inputs.
+   * @param outputs a Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
+   *     subtracts them.)
+   * @param addFuncX A function that adds two state vectors.
+   * @param dtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("ParameterName")
+  public ExtendedKalmanFilter(
+      Nat<States> states,
+      Nat<Inputs> inputs,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
+      double dtSeconds) {
+    m_states = states;
+    m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_residualFuncY = residualFuncY;
+    m_addFuncX = addFuncX;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+    m_dtSeconds = dtSeconds;
+
+    reset();
+
+    final var contA =
+        NumericalJacobian.numericalJacobianX(
+            states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
+    final var C =
+        NumericalJacobian.numericalJacobianX(
+            outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
+
+    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    final var discR = Discretization.discretizeR(m_contR, dtSeconds);
+
+    if (StateSpaceUtil.isDetectable(discA, C) && outputs.getNum() <= states.getNum()) {
+      m_initP =
+          Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR);
+    } else {
+      m_initP = new Matrix<>(states, states);
+    }
+
+    m_P = m_initP;
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the value of the state estimate x-hat at i.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  @Override
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = m_initP;
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    predict(u, m_f, dtSeconds);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param f The function used to linearlize the model.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  public void predict(
+      Matrix<Inputs, N1> u,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      double dtSeconds) {
+    // Find continuous A
+    final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
+
+    // Find discrete A and Q
+    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
+    final var discA = discPair.getFirst();
+    final var discQ = discPair.getSecond();
+
+    m_xHat = NumericalIntegration.rk4(f, m_xHat, u, dtSeconds);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
+    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
+
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    correct(m_outputs, u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Discrete measurement noise covariance matrix.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public <Rows extends Num> void correct(
+      Nat<Rows> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<Rows, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+      Matrix<Rows, Rows> R) {
+    correct(rows, u, y, h, R, Matrix::minus, Matrix::plus);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Discrete measurement noise covariance matrix.
+   * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
+   *     subtracts them.)
+   * @param addFuncX A function that adds two state vectors.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public <Rows extends Num> void correct(
+      Nat<Rows> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<Rows, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
+      Matrix<Rows, Rows> R,
+      BiFunction<Matrix<Rows, N1>, Matrix<Rows, N1>, Matrix<Rows, N1>> residualFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
+    final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
+
+    final var S = C.times(m_P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, h.apply(m_xHat, u))));
+
+    // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
+    m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
new file mode 100644
index 0000000..4aea69d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilter.java
@@ -0,0 +1,204 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Drake;
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.LinearSystem;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
+ * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
+ * of squares error in the state estimate. This K gain is used to correct the state estimate by some
+ * amount of the difference between the actual measurements and the measurements predicted by the
+ * model.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
+ * theory".
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class KalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+  private final Nat<States> m_states;
+
+  private final LinearSystem<States, Inputs, Outputs> m_plant;
+
+  /** The steady-state Kalman gain matrix. */
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Outputs> m_K;
+
+  /** The state estimate. */
+  @SuppressWarnings("MemberName")
+  private Matrix<States, N1> m_xHat;
+
+  /**
+   * Constructs a state-space observer with the given plant.
+   *
+   * @param states A Nat representing the states of the system.
+   * @param outputs A Nat representing the outputs of the system.
+   * @param plant The plant used for the prediction step.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param dtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public KalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      LinearSystem<States, Inputs, Outputs> plant,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double dtSeconds) {
+    this.m_states = states;
+
+    this.m_plant = plant;
+
+    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
+    var discA = pair.getFirst();
+    var discQ = pair.getSecond();
+
+    var discR = Discretization.discretizeR(contR, dtSeconds);
+
+    var C = plant.getC();
+
+    if (!StateSpaceUtil.isDetectable(discA, C)) {
+      var builder =
+          new StringBuilder("The system passed to the Kalman filter is unobservable!\n\nA =\n");
+      builder.append(discA.getStorage().toString());
+      builder.append("\nC =\n");
+      builder.append(C.getStorage().toString());
+      builder.append("\n");
+
+      var msg = builder.toString();
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    var P =
+        new Matrix<>(
+            Drake.discreteAlgebraicRiccatiEquation(discA.transpose(), C.transpose(), discQ, discR));
+
+    // S = CPCᵀ + R
+    var S = C.times(P).times(C.transpose()).plus(discR);
+
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
+    // efficiently.
+    //
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
+    //
+    // The solution of Ax = b can be found via x = A.solve(b).
+    //
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
+    m_K =
+        new Matrix<>(
+            S.transpose().getStorage().solve((C.times(P.transpose())).getStorage()).transpose());
+
+    reset();
+  }
+
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+  }
+
+  /**
+   * Returns the steady-state Kalman gain matrix K.
+   *
+   * @return The steady-state Kalman gain matrix K.
+   */
+  public Matrix<States, Outputs> getK() {
+    return m_K;
+  }
+
+  /**
+   * Returns an element of the steady-state Kalman gain matrix K.
+   *
+   * @param row Row of K.
+   * @param col Column of K.
+   * @return the element (i, j) of the steady-state Kalman gain matrix K.
+   */
+  public double getK(int row, int col) {
+    return m_K.get(row, col);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xhat The state estimate x-hat.
+   */
+  public void setXhat(Matrix<States, N1> xhat) {
+    this.m_xHat = xhat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return The state estimate x-hat.
+   */
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the state estimate x-hat at i.
+   */
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings("ParameterName")
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the last predict step.
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    final var C = m_plant.getC();
+    final var D = m_plant.getD();
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
+    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
new file mode 100644
index 0000000..50a83b5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanFilterLatencyCompensator.java
@@ -0,0 +1,161 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Map;
+import java.util.function.BiConsumer;
+
+public class KalmanFilterLatencyCompensator<S extends Num, I extends Num, O extends Num> {
+  private static final int kMaxPastObserverStates = 300;
+
+  private final List<Map.Entry<Double, ObserverSnapshot>> m_pastObserverSnapshots;
+
+  KalmanFilterLatencyCompensator() {
+    m_pastObserverSnapshots = new ArrayList<>();
+  }
+
+  /** Clears the observer snapshot buffer. */
+  public void reset() {
+    m_pastObserverSnapshots.clear();
+  }
+
+  /**
+   * Add past observer states to the observer snapshots list.
+   *
+   * @param observer The observer.
+   * @param u The input at the timestamp.
+   * @param localY The local output at the timestamp
+   * @param timestampSeconds The timesnap of the state.
+   */
+  @SuppressWarnings("ParameterName")
+  public void addObserverState(
+      KalmanTypeFilter<S, I, O> observer,
+      Matrix<I, N1> u,
+      Matrix<O, N1> localY,
+      double timestampSeconds) {
+    m_pastObserverSnapshots.add(
+        Map.entry(timestampSeconds, new ObserverSnapshot(observer, u, localY)));
+
+    if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
+      m_pastObserverSnapshots.remove(0);
+    }
+  }
+
+  /**
+   * Add past global measurements (such as from vision)to the estimator.
+   *
+   * @param <R> The rows in the global measurement vector.
+   * @param rows The rows in the global measurement vector.
+   * @param observer The observer to apply the past global measurement.
+   * @param nominalDtSeconds The nominal timestep.
+   * @param y The measurement.
+   * @param globalMeasurementCorrect The function take calls correct() on the observer.
+   * @param timestampSeconds The timestamp of the measurement.
+   */
+  @SuppressWarnings("ParameterName")
+  public <R extends Num> void applyPastGlobalMeasurement(
+      Nat<R> rows,
+      KalmanTypeFilter<S, I, O> observer,
+      double nominalDtSeconds,
+      Matrix<R, N1> y,
+      BiConsumer<Matrix<I, N1>, Matrix<R, N1>> globalMeasurementCorrect,
+      double timestampSeconds) {
+    if (m_pastObserverSnapshots.isEmpty()) {
+      // State map was empty, which means that we got a past measurement right at startup. The only
+      // thing we can really do is ignore the measurement.
+      return;
+    }
+
+    // This index starts at one because we use the previous state later on, and we always want to
+    // have a "previous state".
+    int maxIdx = m_pastObserverSnapshots.size() - 1;
+    int low = 1;
+    int high = Math.max(maxIdx, 1);
+
+    while (low != high) {
+      int mid = (low + high) / 2;
+      if (m_pastObserverSnapshots.get(mid).getKey() < timestampSeconds) {
+        // 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;
+      }
+    }
+
+    // We are simply assigning this index to a new variable to avoid confusion
+    // with variable names.
+    int index = low;
+    double timestamp = timestampSeconds;
+    int indexOfClosestEntry =
+        Math.abs(timestamp - m_pastObserverSnapshots.get(index - 1).getKey())
+                <= Math.abs(
+                    timestamp - m_pastObserverSnapshots.get(Math.min(index, maxIdx)).getKey())
+            ? index - 1
+            : index;
+
+    double lastTimestamp =
+        m_pastObserverSnapshots.get(indexOfClosestEntry).getKey() - nominalDtSeconds;
+
+    // We will now go back in time to the state of the system at the time when
+    // the measurement was captured. We will reset the observer to that state,
+    // and apply correction based on the measurement. Then, we will go back
+    // through all observer states until the present and apply past inputs to
+    // get the present estimated state.
+    for (int i = indexOfClosestEntry; i < m_pastObserverSnapshots.size(); i++) {
+      var key = m_pastObserverSnapshots.get(i).getKey();
+      var snapshot = m_pastObserverSnapshots.get(i).getValue();
+
+      if (i == indexOfClosestEntry) {
+        observer.setP(snapshot.errorCovariances);
+        observer.setXhat(snapshot.xHat);
+      }
+
+      observer.predict(snapshot.inputs, key - lastTimestamp);
+      observer.correct(snapshot.inputs, snapshot.localMeasurements);
+
+      if (i == indexOfClosestEntry) {
+        // Note that the measurement is at a timestep close but probably not exactly equal to the
+        // timestep for which we called predict.
+        // This makes the assumption that the dt is small enough that the difference between the
+        // measurement time and the time that the inputs were captured at is very small.
+        globalMeasurementCorrect.accept(snapshot.inputs, y);
+      }
+      lastTimestamp = key;
+
+      m_pastObserverSnapshots.set(
+          i,
+          Map.entry(
+              key, new ObserverSnapshot(observer, snapshot.inputs, snapshot.localMeasurements)));
+    }
+  }
+
+  /** This class contains all the information about our observer at a given time. */
+  @SuppressWarnings("MemberName")
+  public class ObserverSnapshot {
+    public final Matrix<S, N1> xHat;
+    public final Matrix<S, S> errorCovariances;
+    public final Matrix<I, N1> inputs;
+    public final Matrix<O, N1> localMeasurements;
+
+    @SuppressWarnings("ParameterName")
+    private ObserverSnapshot(
+        KalmanTypeFilter<S, I, O> observer, Matrix<I, N1> u, Matrix<O, N1> localY) {
+      this.xHat = observer.getXhat();
+      this.errorCovariances = observer.getP();
+
+      inputs = u;
+      localMeasurements = localY;
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
new file mode 100644
index 0000000..3fd3957
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/KalmanTypeFilter.java
@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+
+@SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"})
+interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
+  Matrix<States, States> getP();
+
+  double getP(int i, int j);
+
+  void setP(Matrix<States, States> newP);
+
+  Matrix<States, N1> getXhat();
+
+  double getXhat(int i);
+
+  void setXhat(Matrix<States, N1> xHat);
+
+  void setXhat(int i, double value);
+
+  void reset();
+
+  void predict(Matrix<Inputs, N1> u, double dtSeconds);
+
+  void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
new file mode 100644
index 0000000..42a6adb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimator.java
@@ -0,0 +1,305 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.util.function.BiConsumer;
+
+/**
+ * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
+ * latency-compensated vision measurements with mecanum drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
+ * drop-in for {@link edu.wpi.first.math.kinematics.MecanumDriveOdometry}.
+ *
+ * <p>{@link MecanumDrivePoseEstimator#update} should be called every robot loop. If your loops are
+ * faster or slower than the default of 0.02s, then you should change the nominal delta time using
+ * the secondary constructor: {@link MecanumDrivePoseEstimator#MecanumDrivePoseEstimator(Rotation2d,
+ * Pose2d, MecanumDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ *
+ * <p>{@link MecanumDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
+ * want; if you never call it, then this class will behave mostly like regular encoder odometry.
+ *
+ * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
+ * (y):
+ *
+ * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
+ * position, and heading.
+ *
+ * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
+ * velocity, and change in gyro heading.
+ *
+ * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
+ * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
+ */
+public class MecanumDrivePoseEstimator {
+  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
+  private final MecanumDriveKinematics m_kinematics;
+  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+
+  private final double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private Matrix<N3, N3> m_visionContR;
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param initialPoseMeters The starting pose estimate.
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
+   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+   *     meters and radians.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [theta], with units in radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public MecanumDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      MecanumDriveKinematics kinematics,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    this(
+        gyroAngle,
+        initialPoseMeters,
+        kinematics,
+        stateStdDevs,
+        localMeasurementStdDevs,
+        visionMeasurementStdDevs,
+        0.02);
+  }
+
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param initialPoseMeters The starting pose estimate.
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
+   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+   *     meters and radians.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [theta], with units in radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   * @param nominalDtSeconds The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public MecanumDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      MecanumDriveKinematics kinematics,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs,
+      double nominalDtSeconds) {
+    m_nominalDt = nominalDtSeconds;
+
+    m_observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N3(),
+            Nat.N1(),
+            (x, u) -> u,
+            (x, u) -> x.extractRowVector(2),
+            stateStdDevs,
+            localMeasurementStdDevs,
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(0),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(0),
+            AngleStatistics.angleAdd(2),
+            m_nominalDt);
+    m_kinematics = kinematics;
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    // Initialize vision R
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+    m_visionCorrect =
+        (u, y) ->
+            m_observer.correct(
+                Nat.N3(),
+                u,
+                y,
+                (x, u1) -> x,
+                m_visionContR,
+                AngleStatistics.angleMean(2),
+                AngleStatistics.angleResidual(2),
+                AngleStatistics.angleResidual(2),
+                AngleStatistics.angleAdd(2));
+
+    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    // Reset state estimate and error covariance
+    m_observer.reset();
+    m_latencyCompensator.reset();
+
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_previousAngle = poseMeters.getRotation();
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return new Pose2d(
+        m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * MecanumDrivePoseEstimator#update} every loop.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
+   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
+   *     Timer.getFPGATimestamp as your time source or sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+        Nat.N3(),
+        m_observer,
+        m_nominalDt,
+        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+        m_visionCorrect,
+        timestampSeconds);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * MecanumDrivePoseEstimator#update} every loop.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * MecanumDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link MecanumDrivePoseEstimator#updateWithTime}
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
+   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
+   *     Timer.getFPGATimestamp as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
+   * called every loop, and the correct loop period must be passed into the constructor of this
+   * class.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
+   * called every loop, and the correct loop period must be passed into the constructor of this
+   * class.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyroscope angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d updateWithTime(
+      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
+    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    var omega = angle.minus(m_previousAngle).getRadians() / dt;
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+    var fieldRelativeVelocities =
+        new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
+            .rotateBy(angle);
+
+    var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
+    m_previousAngle = angle;
+
+    var localY = VecBuilder.fill(angle.getRadians());
+    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_observer.predict(u, dt);
+    m_observer.correct(u, localY);
+
+    return getEstimatedPosition();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
new file mode 100644
index 0000000..fb0628b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/MerweScaledSigmaPoints.java
@@ -0,0 +1,158 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Generates sigma points and weights according to Van der Merwe's 2004 dissertation[1] for the
+ * UnscentedKalmanFilter class.
+ *
+ * <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the version seen in
+ * most publications. Unless you know better, this should be your default choice.
+ *
+ * <p>States is the dimensionality of the state. 2*States+1 weights will be generated.
+ *
+ * <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic Inference in Dynamic
+ * State-Space Models" (Doctoral dissertation)
+ */
+public class MerweScaledSigmaPoints<S extends Num> {
+  private final double m_alpha;
+  private final int m_kappa;
+  private final Nat<S> m_states;
+  private Matrix<?, N1> m_wm;
+  private Matrix<?, N1> m_wc;
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points.
+   *
+   * @param states an instance of Num that represents the number of states.
+   * @param alpha Determines the spread of the sigma points around the mean. Usually a small
+   *     positive value (1e-3).
+   * @param beta Incorporates prior knowledge of the distribution of the mean. For Gaussian
+   *     distributions, beta = 2 is optimal.
+   * @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
+   */
+  public MerweScaledSigmaPoints(Nat<S> states, double alpha, double beta, int kappa) {
+    this.m_states = states;
+    this.m_alpha = alpha;
+    this.m_kappa = kappa;
+
+    computeWeights(beta);
+  }
+
+  /**
+   * Constructs a generator for Van der Merwe scaled sigma points with default values for alpha,
+   * beta, and kappa.
+   *
+   * @param states an instance of Num that represents the number of states.
+   */
+  public MerweScaledSigmaPoints(Nat<S> states) {
+    this(states, 1e-3, 2, 3 - states.getNum());
+  }
+
+  /**
+   * Returns number of sigma points for each variable in the state x.
+   *
+   * @return The number of sigma points for each variable in the state x.
+   */
+  public int getNumSigmas() {
+    return 2 * m_states.getNum() + 1;
+  }
+
+  /**
+   * Computes the sigma points for an unscented Kalman filter given the mean (x) and covariance(P)
+   * of the filter.
+   *
+   * @param x An array of the means.
+   * @param P Covariance of the filter.
+   * @return Two dimensional array of sigma points. Each column contains all of the sigmas for one
+   *     dimension in the problem space. Ordered by Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public Matrix<S, ?> sigmaPoints(Matrix<S, N1> x, Matrix<S, S> P) {
+    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+
+    var intermediate = P.times(lambda + m_states.getNum());
+    var U = intermediate.lltDecompose(true); // Lower triangular
+
+    // 2 * states + 1 by states
+    Matrix<S, ?> sigmas =
+        new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+    sigmas.setColumn(0, x);
+    for (int k = 0; k < m_states.getNum(); k++) {
+      var xPlusU = x.plus(U.extractColumnVector(k));
+      var xMinusU = x.minus(U.extractColumnVector(k));
+      sigmas.setColumn(k + 1, xPlusU);
+      sigmas.setColumn(m_states.getNum() + k + 1, xMinusU);
+    }
+
+    return new Matrix<>(sigmas);
+  }
+
+  /**
+   * Computes the weights for the scaled unscented Kalman filter.
+   *
+   * @param beta Incorporates prior knowledge of the distribution of the mean.
+   */
+  @SuppressWarnings("LocalVariableName")
+  private void computeWeights(double beta) {
+    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
+    double c = 0.5 / (m_states.getNum() + lambda);
+
+    Matrix<?, N1> wM = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
+    Matrix<?, N1> wC = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
+    wM.fill(c);
+    wC.fill(c);
+
+    wM.set(0, 0, lambda / (m_states.getNum() + lambda));
+    wC.set(0, 0, lambda / (m_states.getNum() + lambda) + (1 - Math.pow(m_alpha, 2) + beta));
+
+    this.m_wm = wM;
+    this.m_wc = wC;
+  }
+
+  /**
+   * Returns the weight for each sigma point for the mean.
+   *
+   * @return the weight for each sigma point for the mean.
+   */
+  public Matrix<?, N1> getWm() {
+    return m_wm;
+  }
+
+  /**
+   * Returns an element of the weight for each sigma point for the mean.
+   *
+   * @param element Element of vector to return.
+   * @return the element i's weight for the mean.
+   */
+  public double getWm(int element) {
+    return m_wm.get(element, 0);
+  }
+
+  /**
+   * Returns the weight for each sigma point for the covariance.
+   *
+   * @return the weight for each sigma point for the covariance.
+   */
+  public Matrix<?, N1> getWc() {
+    return m_wc;
+  }
+
+  /**
+   * Returns an element of the weight for each sigma point for the covariance.
+   *
+   * @param element Element of vector to return.
+   * @return The element I's weight for the covariance.
+   */
+  public double getWc(int element) {
+    return m_wc.get(element, 0);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
new file mode 100644
index 0000000..9e48728
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimator.java
@@ -0,0 +1,305 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.util.function.BiConsumer;
+
+/**
+ * This class wraps an {@link UnscentedKalmanFilter Unscented Kalman Filter} to fuse
+ * latency-compensated vision measurements with swerve drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an easy but more accurate
+ * drop-in for {@link edu.wpi.first.math.kinematics.SwerveDriveOdometry}.
+ *
+ * <p>{@link SwerveDrivePoseEstimator#update} should be called every robot loop. If your loops are
+ * faster or slower than the default of 0.02s, then you should change the nominal delta time using
+ * the secondary constructor: {@link SwerveDrivePoseEstimator#SwerveDrivePoseEstimator(Rotation2d,
+ * Pose2d, SwerveDriveKinematics, Matrix, Matrix, Matrix, double)}.
+ *
+ * <p>{@link SwerveDrivePoseEstimator#addVisionMeasurement} can be called as infrequently as you
+ * want; if you never call it, then this class will behave mostly like regular encoder odometry.
+ *
+ * <p>The state-space system used internally has the following states (x), inputs (u), and outputs
+ * (y):
+ *
+ * <p><strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system containing x position, y
+ * position, and heading.
+ *
+ * <p><strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity, right wheel
+ * velocity, and change in gyro heading.
+ *
+ * <p><strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y position, and
+ * heading; or <strong> y = [theta]ᵀ </strong> containing gyro heading.
+ */
+public class SwerveDrivePoseEstimator {
+  private final UnscentedKalmanFilter<N3, N3, N1> m_observer;
+  private final SwerveDriveKinematics m_kinematics;
+  private final BiConsumer<Matrix<N3, N1>, Matrix<N3, N1>> m_visionCorrect;
+  private final KalmanFilterLatencyCompensator<N3, N3, N1> m_latencyCompensator;
+
+  private final double m_nominalDt; // Seconds
+  private double m_prevTimeSeconds = -1.0;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private Matrix<N3, N3> m_visionContR;
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param initialPoseMeters The starting pose estimate.
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
+   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+   *     meters and radians.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [theta], with units in radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public SwerveDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      SwerveDriveKinematics kinematics,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    this(
+        gyroAngle,
+        initialPoseMeters,
+        kinematics,
+        stateStdDevs,
+        localMeasurementStdDevs,
+        visionMeasurementStdDevs,
+        0.02);
+  }
+
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param initialPoseMeters The starting pose estimate.
+   * @param kinematics A correctly-configured kinematics object for your drivetrain.
+   * @param stateStdDevs Standard deviations of model states. Increase these numbers to trust your
+   *     model's state estimates less. This matrix is in the form [x, y, theta]ᵀ, with units in
+   *     meters and radians.
+   * @param localMeasurementStdDevs Standard deviations of the encoder and gyro measurements.
+   *     Increase these numbers to trust sensor readings from encoders and gyros less. This matrix
+   *     is in the form [theta], with units in radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   * @param nominalDtSeconds The time in seconds between each robot loop.
+   */
+  @SuppressWarnings("ParameterName")
+  public SwerveDrivePoseEstimator(
+      Rotation2d gyroAngle,
+      Pose2d initialPoseMeters,
+      SwerveDriveKinematics kinematics,
+      Matrix<N3, N1> stateStdDevs,
+      Matrix<N1, N1> localMeasurementStdDevs,
+      Matrix<N3, N1> visionMeasurementStdDevs,
+      double nominalDtSeconds) {
+    m_nominalDt = nominalDtSeconds;
+
+    m_observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N3(),
+            Nat.N1(),
+            (x, u) -> u,
+            (x, u) -> x.extractRowVector(2),
+            stateStdDevs,
+            localMeasurementStdDevs,
+            AngleStatistics.angleMean(2),
+            AngleStatistics.angleMean(0),
+            AngleStatistics.angleResidual(2),
+            AngleStatistics.angleResidual(0),
+            AngleStatistics.angleAdd(2),
+            m_nominalDt);
+    m_kinematics = kinematics;
+    m_latencyCompensator = new KalmanFilterLatencyCompensator<>();
+
+    // Initialize vision R
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+    m_visionCorrect =
+        (u, y) ->
+            m_observer.correct(
+                Nat.N3(),
+                u,
+                y,
+                (x, u1) -> x,
+                m_visionContR,
+                AngleStatistics.angleMean(2),
+                AngleStatistics.angleResidual(2),
+                AngleStatistics.angleResidual(2),
+                AngleStatistics.angleAdd(2));
+
+    m_gyroOffset = initialPoseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(initialPoseMeters));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used to change trust in
+   * vision measurements after the autonomous period, or to change trust as distance to a vision
+   * target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void setVisionMeasurementStdDevs(Matrix<N3, N1> visionMeasurementStdDevs) {
+    m_visionContR = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), visionMeasurementStdDevs);
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset in the user's robot code. The library
+   * automatically takes care of offsetting the gyro angle.
+   *
+   * @param poseMeters The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    // Reset state estimate and error covariance
+    m_observer.reset();
+    m_latencyCompensator.reset();
+
+    m_observer.setXhat(StateSpaceUtil.poseTo3dVector(poseMeters));
+
+    m_gyroOffset = getEstimatedPosition().getRotation().minus(gyroAngle);
+    m_previousAngle = poseMeters.getRotation();
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  public Pose2d getEstimatedPosition() {
+    return new Pose2d(
+        m_observer.getXhat(0), m_observer.getXhat(1), new Rotation2d(m_observer.getXhat(2)));
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * SwerveDrivePoseEstimator#update} every loop.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
+   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
+   *     Timer.getFPGATimestamp as your time source or sync the epochs.
+   */
+  public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+    m_latencyCompensator.applyPastGlobalMeasurement(
+        Nat.N3(),
+        m_observer,
+        m_nominalDt,
+        StateSpaceUtil.poseTo3dVector(visionRobotPoseMeters),
+        m_visionCorrect,
+        timestampSeconds);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct the odometry pose
+   * estimate while still accounting for measurement noise.
+   *
+   * <p>This method can be called as infrequently as you want, as long as you are calling {@link
+   * SwerveDrivePoseEstimator#update} every loop.
+   *
+   * <p>Note that the vision measurement standard deviations passed into this method will continue
+   * to apply to future measurements until a subsequent call to {@link
+   * SwerveDrivePoseEstimator#setVisionMeasurementStdDevs(Matrix)} or this method.
+   *
+   * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+   * @param timestampSeconds The timestamp of the vision measurement in seconds. Note that if you
+   *     don't use your own time source by calling {@link SwerveDrivePoseEstimator#updateWithTime}
+   *     then you must use a timestamp with an epoch since FPGA startup (i.e. the epoch of this
+   *     timestamp is the same epoch as Timer.getFPGATimestamp.) This means that you should use
+   *     Timer.getFPGATimestamp as your time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision measurements. Increase these
+   *     numbers to trust global measurements from vision less. This matrix is in the form [x, y,
+   *     theta]ᵀ, with units in meters and radians.
+   */
+  public void addVisionMeasurement(
+      Pose2d visionRobotPoseMeters,
+      double timestampSeconds,
+      Matrix<N3, N1> visionMeasurementStdDevs) {
+    setVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    addVisionMeasurement(visionRobotPoseMeters, timestampSeconds);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
+   * called every loop, and the correct loop period must be passed into the constructor of this
+   * class.
+   *
+   * @param gyroAngle The current gyro angle.
+   * @param moduleStates The current velocities and rotations of the swerve modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder information. This should be
+   * called every loop, and the correct loop period must be passed into the constructor of this
+   * class.
+   *
+   * @param currentTimeSeconds Time at which this method was called, in seconds.
+   * @param gyroAngle The current gyroscope angle.
+   * @param moduleStates The current velocities and rotations of the swerve modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public Pose2d updateWithTime(
+      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    double dt = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : m_nominalDt;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+    var omega = angle.minus(m_previousAngle).getRadians() / dt;
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(moduleStates);
+    var fieldRelativeVelocities =
+        new Translation2d(chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond)
+            .rotateBy(angle);
+
+    var u = VecBuilder.fill(fieldRelativeVelocities.getX(), fieldRelativeVelocities.getY(), omega);
+    m_previousAngle = angle;
+
+    var localY = VecBuilder.fill(angle.getRadians());
+    m_latencyCompensator.addObserverState(m_observer, u, localY, currentTimeSeconds);
+    m_observer.predict(u, dt);
+    m_observer.correct(u, localY);
+
+    return getEstimatedPosition();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
new file mode 100644
index 0000000..ffc2c15
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/estimator/UnscentedKalmanFilter.java
@@ -0,0 +1,436 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import java.util.function.BiFunction;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an estimate of the
+ * true system state. This is useful because many states cannot be measured directly as a result of
+ * sensor noise, or because the state is "hidden".
+ *
+ * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
+ * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
+ * of squares error in the state estimate. This K gain is used to correct the state estimate by some
+ * amount of the difference between the actual measurements and the measurements predicted by the
+ * model.
+ *
+ * <p>An unscented Kalman filter uses nonlinear state and measurement models. It propagates the
+ * error covariance using sigma points chosen to approximate the true probability distribution.
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
+ * theory".
+ */
+@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
+public class UnscentedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
+    implements KalmanTypeFilter<States, Inputs, Outputs> {
+  private final Nat<States> m_states;
+  private final Nat<Outputs> m_outputs;
+
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
+  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
+
+  private BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> m_meanFuncX;
+  private BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> m_meanFuncY;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_residualFuncX;
+  private BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> m_residualFuncY;
+  private BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> m_addFuncX;
+
+  private Matrix<States, N1> m_xHat;
+  private Matrix<States, States> m_P;
+  private final Matrix<States, States> m_contQ;
+  private final Matrix<Outputs, Outputs> m_contR;
+  private Matrix<States, ?> m_sigmasF;
+  private double m_dtSeconds;
+
+  private final MerweScaledSigmaPoints<States> m_pts;
+
+  /**
+   * Constructs an Unscented Kalman Filter.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param nominalDtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("LambdaParameterName")
+  public UnscentedKalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      double nominalDtSeconds) {
+    this(
+        states,
+        outputs,
+        f,
+        h,
+        stateStdDevs,
+        measurementStdDevs,
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm)),
+        Matrix::minus,
+        Matrix::minus,
+        Matrix::plus,
+        nominalDtSeconds);
+  }
+
+  /**
+   * Constructs an unscented Kalman filter with custom mean, residual, and addition functions. Using
+   * custom functions for arithmetic can be useful if you have angles in the state or measurements,
+   * because they allow you to correctly account for the modular nature of angle arithmetic.
+   *
+   * @param states A Nat representing the number of states.
+   * @param outputs A Nat representing the number of outputs.
+   * @param f A vector-valued function of x and u that returns the derivative of the state vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param stateStdDevs Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param meanFuncX A function that computes the mean of 2 * States + 1 state vectors using a
+   *     given set of weights.
+   * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
+   *     a given set of weights.
+   * @param residualFuncX A function that computes the residual of two state vectors (i.e. it
+   *     subtracts them.)
+   * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
+   *     subtracts them.)
+   * @param addFuncX A function that adds two state vectors.
+   * @param nominalDtSeconds Nominal discretization timestep.
+   */
+  @SuppressWarnings("ParameterName")
+  public UnscentedKalmanFilter(
+      Nat<States> states,
+      Nat<Outputs> outputs,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
+      Matrix<States, N1> stateStdDevs,
+      Matrix<Outputs, N1> measurementStdDevs,
+      BiFunction<Matrix<States, ?>, Matrix<?, N1>, Matrix<States, N1>> meanFuncX,
+      BiFunction<Matrix<Outputs, ?>, Matrix<?, N1>, Matrix<Outputs, N1>> meanFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
+      BiFunction<Matrix<Outputs, N1>, Matrix<Outputs, N1>, Matrix<Outputs, N1>> residualFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX,
+      double nominalDtSeconds) {
+    this.m_states = states;
+    this.m_outputs = outputs;
+
+    m_f = f;
+    m_h = h;
+
+    m_meanFuncX = meanFuncX;
+    m_meanFuncY = meanFuncY;
+    m_residualFuncX = residualFuncX;
+    m_residualFuncY = residualFuncY;
+    m_addFuncX = addFuncX;
+
+    m_dtSeconds = nominalDtSeconds;
+
+    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
+    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
+
+    m_pts = new MerweScaledSigmaPoints<>(states);
+
+    reset();
+  }
+
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  static <S extends Num, C extends Num> Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
+      Nat<S> s,
+      Nat<C> dim,
+      Matrix<C, ?> sigmas,
+      Matrix<?, N1> Wm,
+      Matrix<?, N1> Wc,
+      BiFunction<Matrix<C, ?>, Matrix<?, N1>, Matrix<C, N1>> meanFunc,
+      BiFunction<Matrix<C, N1>, Matrix<C, N1>, Matrix<C, N1>> residualFunc) {
+    if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
+      throw new IllegalArgumentException(
+          "Sigmas must be covDim by 2 * states + 1! Got "
+              + sigmas.getNumRows()
+              + " by "
+              + sigmas.getNumCols());
+    }
+
+    if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1) {
+      throw new IllegalArgumentException(
+          "Wm must be 2 * states + 1 by 1! Got " + Wm.getNumRows() + " by " + Wm.getNumCols());
+    }
+
+    if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) {
+      throw new IllegalArgumentException(
+          "Wc must be 2 * states + 1 by 1! Got " + Wc.getNumRows() + " by " + Wc.getNumCols());
+    }
+
+    // New mean is usually just the sum of the sigmas * weight:
+    //       n
+    // dot = Σ W[k] Xᵢ[k]
+    //      k=1
+    Matrix<C, N1> x = meanFunc.apply(sigmas, Wm);
+
+    // New covariance is the sum of the outer product of the residuals times the
+    // weights
+    Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
+    for (int i = 0; i < 2 * s.getNum() + 1; i++) {
+      // y[:, i] = sigmas[:, i] - x
+      y.setColumn(i, residualFunc.apply(sigmas.extractColumnVector(i), x));
+    }
+    Matrix<C, C> P =
+        y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
+            .times(Matrix.changeBoundsUnchecked(y.transpose()));
+
+    return new Pair<>(x, P);
+  }
+
+  /**
+   * Returns the error covariance matrix P.
+   *
+   * @return the error covariance matrix P.
+   */
+  @Override
+  public Matrix<States, States> getP() {
+    return m_P;
+  }
+
+  /**
+   * Returns an element of the error covariance matrix P.
+   *
+   * @param row Row of P.
+   * @param col Column of P.
+   * @return the value of the error covariance matrix P at (i, j).
+   */
+  @Override
+  public double getP(int row, int col) {
+    return m_P.get(row, col);
+  }
+
+  /**
+   * Sets the entire error covariance matrix P.
+   *
+   * @param newP The new value of P to use.
+   */
+  @Override
+  public void setP(Matrix<States, States> newP) {
+    m_P = newP;
+  }
+
+  /**
+   * Returns the state estimate x-hat.
+   *
+   * @return the state estimate x-hat.
+   */
+  @Override
+  public Matrix<States, N1> getXhat() {
+    return m_xHat;
+  }
+
+  /**
+   * Returns an element of the state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the value of the state estimate x-hat at i.
+   */
+  @Override
+  public double getXhat(int row) {
+    return m_xHat.get(row, 0);
+  }
+
+  /**
+   * Set initial state estimate x-hat.
+   *
+   * @param xHat The state estimate x-hat.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void setXhat(Matrix<States, N1> xHat) {
+    m_xHat = xHat;
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  @Override
+  public void setXhat(int row, double value) {
+    m_xHat.set(row, 0, value);
+  }
+
+  /** Resets the observer. */
+  @Override
+  public void reset() {
+    m_xHat = new Matrix<>(m_states, Nat.N1());
+    m_P = new Matrix<>(m_states, m_states);
+    m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
+  }
+
+  /**
+   * Project the model into the future with a new control input u.
+   *
+   * @param u New control input from controller.
+   * @param dtSeconds Timestep for prediction.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  @Override
+  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
+    // Discretize Q before projecting mean and covariance forward
+    Matrix<States, States> contA =
+        NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
+    var discQ = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
+
+    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+
+    for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
+      Matrix<States, N1> x = sigmas.extractColumnVector(i);
+
+      m_sigmasF.setColumn(i, NumericalIntegration.rk4(m_f, x, u, dtSeconds));
+    }
+
+    var ret =
+        unscentedTransform(
+            m_states,
+            m_states,
+            m_sigmasF,
+            m_pts.getWm(),
+            m_pts.getWc(),
+            m_meanFuncX,
+            m_residualFuncX);
+
+    m_xHat = ret.getFirst();
+    m_P = ret.getSecond().plus(discQ);
+    m_dtSeconds = dtSeconds;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  @Override
+  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
+    correct(
+        m_outputs, u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY, m_residualFuncX, m_addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <R> Number of measurements in y.
+   * @param rows Number of rows in y.
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Measurement noise covariance matrix (continuous-time).
+   */
+  @SuppressWarnings({"ParameterName", "LambdaParameterName", "LocalVariableName"})
+  public <R extends Num> void correct(
+      Nat<R> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<R, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
+      Matrix<R, R> R) {
+    BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY =
+        (sigmas, Wm) -> sigmas.times(Matrix.changeBoundsUnchecked(Wm));
+    BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX =
+        Matrix::minus;
+    BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY = Matrix::minus;
+    BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX = Matrix::plus;
+    correct(rows, u, y, h, R, meanFuncY, residualFuncY, residualFuncX, addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * <p>This is useful for when the measurements available during a timestep's Correct() call vary.
+   * The h(x, u) passed to the constructor is used if one is not provided (the two-argument version
+   * of this function).
+   *
+   * @param <R> Number of measurements in y.
+   * @param rows Number of rows in y.
+   * @param u Same control input used in the predict step.
+   * @param y Measurement vector.
+   * @param h A vector-valued function of x and u that returns the measurement vector.
+   * @param R Measurement noise covariance matrix (continuous-time).
+   * @param meanFuncY A function that computes the mean of 2 * States + 1 measurement vectors using
+   *     a given set of weights.
+   * @param residualFuncY A function that computes the residual of two measurement vectors (i.e. it
+   *     subtracts them.)
+   * @param residualFuncX A function that computes the residual of two state vectors (i.e. it
+   *     subtracts them.)
+   * @param addFuncX A function that adds two state vectors.
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  public <R extends Num> void correct(
+      Nat<R> rows,
+      Matrix<Inputs, N1> u,
+      Matrix<R, N1> y,
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
+      Matrix<R, R> R,
+      BiFunction<Matrix<R, ?>, Matrix<?, N1>, Matrix<R, N1>> meanFuncY,
+      BiFunction<Matrix<R, N1>, Matrix<R, N1>, Matrix<R, N1>> residualFuncY,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> residualFuncX,
+      BiFunction<Matrix<States, N1>, Matrix<States, N1>, Matrix<States, N1>> addFuncX) {
+    final var discR = Discretization.discretizeR(R, m_dtSeconds);
+
+    // Transform sigma points into measurement space
+    Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(rows.getNum(), 2 * m_states.getNum() + 1));
+    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      Matrix<R, N1> hRet = h.apply(sigmas.extractColumnVector(i), u);
+      sigmasH.setColumn(i, hRet);
+    }
+
+    // Mean and covariance of prediction passed through unscented transform
+    var transRet =
+        unscentedTransform(
+            m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc(), meanFuncY, residualFuncY);
+    var yHat = transRet.getFirst();
+    var Py = transRet.getSecond().plus(discR);
+
+    // Compute cross covariance of the state and the measurements
+    Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
+    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
+      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
+      var dx = residualFuncX.apply(m_sigmasF.extractColumnVector(i), m_xHat);
+      var dy = residualFuncY.apply(sigmasH.extractColumnVector(i), yHat).transpose();
+
+      Pxy = Pxy.plus(dx.times(dy).times(m_pts.getWc(i)));
+    }
+
+    // K = P_{xy} P_y⁻¹
+    // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
+    // P_yᵀKᵀ = P_{xy}ᵀ
+    // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
+    // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
+    Matrix<States, R> K = new Matrix<>(Py.transpose().solve(Pxy.transpose()).transpose());
+
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+    m_xHat = addFuncX.apply(m_xHat, K.times(residualFuncY.apply(y, yHat)));
+
+    // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
+    m_P = m_P.minus(K.times(Py).times(K.transpose()));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
new file mode 100644
index 0000000..93d6bea
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/LinearFilter.java
@@ -0,0 +1,267 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.util.CircularBuffer;
+import java.util.Arrays;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * 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.
+ *
+ * <p>Filters are of the form: 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])
+ *
+ * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
+ * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
+ * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
+ * front of the feedback term! This is a common convention in signal processing.
+ *
+ * <p>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.
+ *
+ * <p>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!
+ *
+ * <p>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>
+ *
+ * <p>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.
+ *
+ * <p>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!
+ */
+public class LinearFilter {
+  private final CircularBuffer m_inputs;
+  private final CircularBuffer m_outputs;
+  private final double[] m_inputGains;
+  private final double[] m_outputGains;
+
+  private static int instances;
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feedforward" or FIR gains.
+   * @param fbGains The "feedback" or IIR gains.
+   */
+  public LinearFilter(double[] ffGains, double[] fbGains) {
+    m_inputs = new CircularBuffer(ffGains.length);
+    m_outputs = new CircularBuffer(fbGains.length);
+    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
+    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
+
+    instances++;
+    MathSharedStore.reportUsage(MathUsageId.kFilter_Linear, instances);
+  }
+
+  /**
+   * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain) x[n] + gain y[n-1] where
+   * gain = e<sup>-dt / T</sup>, T is the time constant in seconds.
+   *
+   * <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency above which the
+   * input starts to attenuate.
+   *
+   * <p>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.
+   * @return Linear filter.
+   */
+  public static LinearFilter singlePoleIIR(double timeConstant, double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {1.0 - gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a first-order high-pass filter of the form: y[n] = gain x[n] + (-gain) x[n-1] + gain
+   * y[n-1] where gain = e<sup>-dt / T</sup>, T is the time constant in seconds.
+   *
+   * <p>Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency below which the
+   * input starts to attenuate.
+   *
+   * <p>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.
+   * @return Linear filter.
+   */
+  public static LinearFilter highPass(double timeConstant, double period) {
+    double gain = Math.exp(-period / timeConstant);
+    double[] ffGains = {gain, -gain};
+    double[] fbGains = {-gain};
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k (x[k] + x[k-1] + ... + x[0]).
+   *
+   * <p>This filter is always stable.
+   *
+   * @param taps The number of samples to average over. Higher = smoother but slower.
+   * @return Linear filter.
+   * @throws IllegalArgumentException if number of taps is less than 1.
+   */
+  public static LinearFilter movingAverage(int taps) {
+    if (taps <= 0) {
+      throw new IllegalArgumentException("Number of taps was not at least 1");
+    }
+
+    double[] ffGains = new double[taps];
+    for (int i = 0; i < ffGains.length; i++) {
+      ffGains[i] = 1.0 / taps;
+    }
+
+    double[] fbGains = new double[0];
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /**
+   * Creates a backward finite difference filter that computes the nth derivative of the input given
+   * the specified number of samples.
+   *
+   * <p>For example, a first derivative filter that uses two samples and a sample period of 20 ms
+   * would be
+   *
+   * <pre><code>
+   * LinearFilter.backwardFiniteDifference(1, 2, 0.02);
+   * </code></pre>
+   *
+   * @param derivative The order of the derivative to compute.
+   * @param samples The number of samples to use to compute the given derivative. This must be one
+   *     more than the order of derivative or higher.
+   * @param period The period in seconds between samples taken by the user.
+   * @return Linear filter.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static LinearFilter backwardFiniteDifference(int derivative, int samples, double period) {
+    // See
+    // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
+    //
+    // <p>For a given list of stencil points s of length n and the order of
+    // derivative d < n, the finite difference coefficients can be obtained by
+    // solving the following linear system for the vector a.
+    //
+    // <pre>
+    // [s₁⁰   ⋯  sₙ⁰ ][a₁]      [ δ₀,d ]
+    // [ ⋮    ⋱  ⋮   ][⋮ ] = d! [  ⋮   ]
+    // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ]      [δₙ₋₁,d]
+    // </pre>
+    //
+    // <p>where δᵢ,ⱼ are the Kronecker delta. For backward finite difference,
+    // the stencil points are the range [-n + 1, 0]. The FIR gains are the
+    // elements of the vector a in reverse order divided by hᵈ.
+    //
+    // <p>The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
+
+    if (derivative < 1) {
+      throw new IllegalArgumentException(
+          "Order of derivative must be greater than or equal to one.");
+    }
+
+    if (samples <= 0) {
+      throw new IllegalArgumentException("Number of samples must be greater than zero.");
+    }
+
+    if (derivative >= samples) {
+      throw new IllegalArgumentException(
+          "Order of derivative must be less than number of samples.");
+    }
+
+    var S = new SimpleMatrix(samples, samples);
+    for (int row = 0; row < samples; ++row) {
+      for (int col = 0; col < samples; ++col) {
+        double s = 1 - samples + col;
+        S.set(row, col, Math.pow(s, row));
+      }
+    }
+
+    // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
+    var d = new SimpleMatrix(samples, 1);
+    for (int i = 0; i < samples; ++i) {
+      d.set(i, 0, (i == derivative) ? factorial(derivative) : 0.0);
+    }
+
+    var a = S.solve(d).divide(Math.pow(period, derivative));
+
+    // Reverse gains list
+    double[] ffGains = new double[samples];
+    for (int i = 0; i < samples; ++i) {
+      ffGains[i] = a.get(samples - i - 1, 0);
+    }
+
+    double[] fbGains = new double[0];
+
+    return new LinearFilter(ffGains, fbGains);
+  }
+
+  /** Reset the filter state. */
+  public void reset() {
+    m_inputs.clear();
+    m_outputs.clear();
+  }
+
+  /**
+   * Calculates the next value of the filter.
+   *
+   * @param input Current input value.
+   * @return The filtered value at this step
+   */
+  public double calculate(double input) {
+    double retVal = 0.0;
+
+    // Rotate the inputs
+    if (m_inputGains.length > 0) {
+      m_inputs.addFirst(input);
+    }
+
+    // Calculate the new value
+    for (int i = 0; i < m_inputGains.length; i++) {
+      retVal += m_inputs.get(i) * m_inputGains[i];
+    }
+    for (int i = 0; i < m_outputGains.length; i++) {
+      retVal -= m_outputs.get(i) * m_outputGains[i];
+    }
+
+    // Rotate the outputs
+    if (m_outputGains.length > 0) {
+      m_outputs.addFirst(retVal);
+    }
+
+    return retVal;
+  }
+
+  /**
+   * Factorial of n.
+   *
+   * @param n Argument of which to take factorial.
+   */
+  private static int factorial(int n) {
+    if (n < 2) {
+      return 1;
+    } else {
+      return n * factorial(n - 1);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
new file mode 100644
index 0000000..c24f6e9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/MedianFilter.java
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.util.CircularBuffer;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * 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).
+ */
+public class MedianFilter {
+  private final CircularBuffer m_valueBuffer;
+  private final List<Double> m_orderedValues;
+  private final int m_size;
+
+  /**
+   * Creates a new MedianFilter.
+   *
+   * @param size The number of samples in the moving window.
+   */
+  public MedianFilter(int size) {
+    // Circular buffer of values currently in the window, ordered by time
+    m_valueBuffer = new CircularBuffer(size);
+    // List of values currently in the window, ordered by value
+    m_orderedValues = new ArrayList<>(size);
+    // Size of rolling window
+    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.
+   */
+  public double calculate(double next) {
+    // Find insertion point for next value
+    int index = Collections.binarySearch(m_orderedValues, next);
+
+    // Deal with binarySearch behavior for element not found
+    if (index < 0) {
+      index = Math.abs(index + 1);
+    }
+
+    // Place value at proper insertion point
+    m_orderedValues.add(index, next);
+
+    int 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.remove(m_valueBuffer.removeLast());
+      --curSize;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.addFirst(next);
+
+    if (curSize % 2 != 0) {
+      // If size is odd, return middle element of sorted list
+      return m_orderedValues.get(curSize / 2);
+    } else {
+      // If size is even, return average of middle elements
+      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
+    }
+  }
+
+  /** Resets the filter, clearing the window of all elements. */
+  public void reset() {
+    m_orderedValues.clear();
+    m_valueBuffer.clear();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
new file mode 100644
index 0000000..d3aa7d8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/filter/SlewRateLimiter.java
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * A class that limits the rate of change of an input value. Useful for implementing voltage,
+ * setpoint, and/or output ramps. A slew-rate limit is most appropriate when the quantity being
+ * controlled is a velocity or a voltage; when controlling a position, consider using a {@link
+ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead.
+ */
+public class SlewRateLimiter {
+  private final double m_rateLimit;
+  private double m_prevVal;
+  private double m_prevTime;
+
+  /**
+   * Creates a new SlewRateLimiter with the given rate limit and initial value.
+   *
+   * @param rateLimit The rate-of-change limit, in units per second.
+   * @param initialValue The initial value of the input.
+   */
+  public SlewRateLimiter(double rateLimit, double initialValue) {
+    m_rateLimit = rateLimit;
+    m_prevVal = initialValue;
+    m_prevTime = WPIUtilJNI.now() * 1e-6;
+  }
+
+  /**
+   * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero.
+   *
+   * @param rateLimit The rate-of-change limit, in units per second.
+   */
+  public SlewRateLimiter(double rateLimit) {
+    this(rateLimit, 0);
+  }
+
+  /**
+   * Filters the input to limit its slew rate.
+   *
+   * @param input The input value whose slew rate is to be limited.
+   * @return The filtered value, which will not change faster than the slew rate.
+   */
+  public double calculate(double input) {
+    double currentTime = WPIUtilJNI.now() * 1e-6;
+    double elapsedTime = currentTime - m_prevTime;
+    m_prevVal +=
+        MathUtil.clamp(input - m_prevVal, -m_rateLimit * elapsedTime, m_rateLimit * elapsedTime);
+    m_prevTime = currentTime;
+    return m_prevVal;
+  }
+
+  /**
+   * Resets the slew rate limiter to the specified value; ignores the rate limit when doing so.
+   *
+   * @param value The value to reset to.
+   */
+  public void reset(double value) {
+    m_prevVal = value;
+    m_prevTime = WPIUtilJNI.now() * 1e-6;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
new file mode 100644
index 0000000..6033b89
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -0,0 +1,245 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import java.util.Objects;
+
+/** Represents a 2d pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Pose2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs a pose at the origin facing toward the positive X axis. (Translation2d{0, 0} and
+   * Rotation{0})
+   */
+  public Pose2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * 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.
+   */
+  @JsonCreator
+  public Pose2d(
+      @JsonProperty(required = true, value = "translation") Translation2d translation,
+      @JsonProperty(required = true, value = "rotation") Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = 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.
+   */
+  public Pose2d(double x, double y, Rotation2d rotation) {
+    m_translation = new Translation2d(x, y);
+    m_rotation = rotation;
+  }
+
+  /**
+   * Transforms the pose by the given transformation and returns the new transformed pose.
+   *
+   * <p>The matrix multiplication is as follows [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.
+   */
+  public Pose2d plus(Transform2d other) {
+    return transformBy(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.
+   */
+  public Transform2d minus(Pose2d other) {
+    final var pose = this.relativeTo(other);
+    return new Transform2d(pose.getTranslation(), pose.getRotation());
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the pose.
+   */
+  @JsonProperty
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the pose's translation.
+   *
+   * @return The x component of the pose's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the pose's translation.
+   *
+   * @return The y component of the pose's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return The rotational component of the pose.
+   */
+  @JsonProperty
+  public Rotation2d getRotation() {
+    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.
+   */
+  public Pose2d transformBy(Transform2d other) {
+    return new Pose2d(
+        m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+        m_rotation.plus(other.getRotation()));
+  }
+
+  /**
+   * Returns the other pose relative to the current pose.
+   *
+   * <p>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.
+   */
+  public Pose2d relativeTo(Pose2d other) {
+    var transform = new Transform2d(other, this);
+    return new Pose2d(transform.getTranslation(), transform.getRotation());
+  }
+
+  /**
+   * Obtain a new Pose2d from a (constant curvature) velocity.
+   *
+   * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
+   * Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for a
+   * derivation.
+   *
+   * <p>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.
+   *
+   * <p>"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.
+   */
+  public Pose2d exp(Twist2d twist) {
+    double dx = twist.dx;
+    double dy = twist.dy;
+    double dtheta = twist.dtheta;
+
+    double sinTheta = Math.sin(dtheta);
+    double cosTheta = Math.cos(dtheta);
+
+    double s;
+    double c;
+    if (Math.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;
+    }
+    var transform =
+        new Transform2d(
+            new Translation2d(dx * s - dy * c, dx * c + dy * s),
+            new Rotation2d(cosTheta, sinTheta));
+
+    return this.plus(transform);
+  }
+
+  /**
+   * 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.
+   */
+  public Twist2d log(Pose2d end) {
+    final var transform = end.relativeTo(this);
+    final var dtheta = transform.getRotation().getRadians();
+    final var halfDtheta = dtheta / 2.0;
+
+    final var cosMinusOne = transform.getRotation().getCos() - 1;
+
+    double halfThetaByTanOfHalfDtheta;
+    if (Math.abs(cosMinusOne) < 1E-9) {
+      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+    } else {
+      halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
+    }
+
+    Translation2d translationPart =
+        transform
+            .getTranslation()
+            .rotateBy(new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta))
+            .times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Pose2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Pose2d) {
+      return ((Pose2d) obj).m_translation.equals(m_translation)
+          && ((Pose2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
new file mode 100644
index 0000000..74ef228
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -0,0 +1,201 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import java.util.Objects;
+
+/** A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine). */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Rotation2d {
+  private final double m_value;
+  private final double m_cos;
+  private final double m_sin;
+
+  /** Constructs a Rotation2d with a default angle of 0 degrees. */
+  public Rotation2d() {
+    m_value = 0.0;
+    m_cos = 1.0;
+    m_sin = 0.0;
+  }
+
+  /**
+   * Constructs a Rotation2d with the given radian value. The x and y don't have to be normalized.
+   *
+   * @param value The value of the angle in radians.
+   */
+  @JsonCreator
+  public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
+    m_value = value;
+    m_cos = Math.cos(value);
+    m_sin = Math.sin(value);
+  }
+
+  /**
+   * Constructs a Rotation2d with the given x and y (cosine and sine) components.
+   *
+   * @param x The x component or cosine of the rotation.
+   * @param y The y component or sine of the rotation.
+   */
+  public Rotation2d(double x, double y) {
+    double magnitude = Math.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 = Math.atan2(m_sin, m_cos);
+  }
+
+  /**
+   * Constructs and returns a Rotation2d with the given degree value.
+   *
+   * @param degrees The value of the angle in degrees.
+   * @return The rotation object with the desired angle value.
+   */
+  public static Rotation2d fromDegrees(double degrees) {
+    return new Rotation2d(Math.toRadians(degrees));
+  }
+
+  /**
+   * Adds two rotations together, with the result being bounded between -pi and pi.
+   *
+   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) = Rotation2d{-pi/2}
+   *
+   * @param other The rotation to add.
+   * @return The sum of the two rotations.
+   */
+  public Rotation2d plus(Rotation2d other) {
+    return rotateBy(other);
+  }
+
+  /**
+   * Subtracts the new rotation from the current rotation and returns the new rotation.
+   *
+   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) = Rotation2d{-pi/2}
+   *
+   * @param other The rotation to subtract.
+   * @return The difference between the two rotations.
+   */
+  public Rotation2d minus(Rotation2d other) {
+    return rotateBy(other.unaryMinus());
+  }
+
+  /**
+   * Takes the inverse of the current rotation. This is simply the negative of the current angular
+   * value.
+   *
+   * @return The inverse of the current rotation.
+   */
+  public Rotation2d unaryMinus() {
+    return new Rotation2d(-m_value);
+  }
+
+  /**
+   * Multiplies the current rotation by a scalar.
+   *
+   * @param scalar The scalar.
+   * @return The new scaled Rotation2d.
+   */
+  public Rotation2d times(double scalar) {
+    return new Rotation2d(m_value * scalar);
+  }
+
+  /**
+   * Adds the new rotation to the current rotation using a rotation matrix.
+   *
+   * <p>The matrix multiplication is as follows:
+   *
+   * <pre>
+   * [cos_new]   [other.cos, -other.sin][cos]
+   * [sin_new] = [other.sin,  other.cos][sin]
+   * value_new = atan2(sin_new, cos_new)
+   * </pre>
+   *
+   * @param other The rotation to rotate by.
+   * @return The new rotated Rotation2d.
+   */
+  public Rotation2d rotateBy(Rotation2d other) {
+    return new Rotation2d(
+        m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
+  }
+
+  /**
+   * Returns the radian value of the rotation.
+   *
+   * @return The radian value of the rotation.
+   */
+  @JsonProperty
+  public double getRadians() {
+    return m_value;
+  }
+
+  /**
+   * Returns the degree value of the rotation.
+   *
+   * @return The degree value of the rotation.
+   */
+  public double getDegrees() {
+    return Math.toDegrees(m_value);
+  }
+
+  /**
+   * Returns the cosine of the rotation.
+   *
+   * @return The cosine of the rotation.
+   */
+  public double getCos() {
+    return m_cos;
+  }
+
+  /**
+   * Returns the sine of the rotation.
+   *
+   * @return The sine of the rotation.
+   */
+  public double getSin() {
+    return m_sin;
+  }
+
+  /**
+   * Returns the tangent of the rotation.
+   *
+   * @return The tangent of the rotation.
+   */
+  public double getTan() {
+    return m_sin / m_cos;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+  }
+
+  /**
+   * Checks equality between this Rotation2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Rotation2d) {
+      var other = (Rotation2d) obj;
+      return Math.hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_value);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
new file mode 100644
index 0000000..dd35670
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
@@ -0,0 +1,143 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import java.util.Objects;
+
+/** Represents a transformation for a Pose2d. */
+public class Transform2d {
+  private final Translation2d m_translation;
+  private final Rotation2d m_rotation;
+
+  /**
+   * Constructs the transform that maps the initial pose to the final pose.
+   *
+   * @param initial The initial pose for the transformation.
+   * @param last The final pose for the transformation.
+   */
+  public Transform2d(Pose2d initial, Pose2d last) {
+    // 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 =
+        last.getTranslation()
+            .minus(initial.getTranslation())
+            .rotateBy(initial.getRotation().unaryMinus());
+
+    m_rotation = last.getRotation().minus(initial.getRotation());
+  }
+
+  /**
+   * Constructs a transform with the given translation and rotation components.
+   *
+   * @param translation Translational component of the transform.
+   * @param rotation Rotational component of the transform.
+   */
+  public Transform2d(Translation2d translation, Rotation2d rotation) {
+    m_translation = translation;
+    m_rotation = rotation;
+  }
+
+  /** Constructs the identity transform -- maps an initial pose to itself. */
+  public Transform2d() {
+    m_translation = new Translation2d();
+    m_rotation = new Rotation2d();
+  }
+
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  public Transform2d times(double scalar) {
+    return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
+  }
+
+  /**
+   * Composes two transformations.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  public Transform2d plus(Transform2d other) {
+    return new Transform2d(new Pose2d(), new Pose2d().transformBy(this).transformBy(other));
+  }
+
+  /**
+   * Returns the translation component of the transformation.
+   *
+   * @return The translational component of the transform.
+   */
+  public Translation2d getTranslation() {
+    return m_translation;
+  }
+
+  /**
+   * Returns the X component of the transformation's translation.
+   *
+   * @return The x component of the transformation's translation.
+   */
+  public double getX() {
+    return m_translation.getX();
+  }
+
+  /**
+   * Returns the Y component of the transformation's translation.
+   *
+   * @return The y component of the transformation's translation.
+   */
+  public double getY() {
+    return m_translation.getY();
+  }
+
+  /**
+   * Returns the rotational component of the transformation.
+   *
+   * @return Reference to the rotational component of the transform.
+   */
+  public Rotation2d getRotation() {
+    return m_rotation;
+  }
+
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  public Transform2d inverse() {
+    // 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).
+    return new Transform2d(
+        getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+        getRotation().unaryMinus());
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+  }
+
+  /**
+   * Checks equality between this Transform2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Transform2d) {
+      return ((Transform2d) obj).m_translation.equals(m_translation)
+          && ((Transform2d) obj).m_rotation.equals(m_rotation);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_translation, m_rotation);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
new file mode 100644
index 0000000..251c078
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -0,0 +1,199 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import java.util.Objects;
+
+/**
+ * Represents a translation in 2d space. This object can be used to represent a point or a vector.
+ *
+ * <p>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.
+ */
+@SuppressWarnings({"ParameterName", "MemberName"})
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class Translation2d {
+  private final double m_x;
+  private final double m_y;
+
+  /** Constructs a Translation2d with X and Y components equal to zero. */
+  public Translation2d() {
+    this(0.0, 0.0);
+  }
+
+  /**
+   * 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.
+   */
+  @JsonCreator
+  public Translation2d(
+      @JsonProperty(required = true, value = "x") double x,
+      @JsonProperty(required = true, value = "y") double y) {
+    m_x = x;
+    m_y = y;
+  }
+
+  /**
+   * Constructs a Translation2d with the provided distance and angle. This is essentially converting
+   * from polar coordinates to Cartesian coordinates.
+   *
+   * @param distance The distance from the origin to the end of the translation.
+   * @param angle The angle between the x-axis and the translation vector.
+   */
+  public Translation2d(double distance, Rotation2d angle) {
+    m_x = distance * angle.getCos();
+    m_y = distance * angle.getSin();
+  }
+
+  /**
+   * Calculates the distance between two translations in 2d space.
+   *
+   * <p>This function uses the pythagorean theorem to calculate the distance. distance = sqrt((x2 -
+   * x1)^2 + (y2 - y1)^2)
+   *
+   * @param other The translation to compute the distance to.
+   * @return The distance between the two translations.
+   */
+  public double getDistance(Translation2d other) {
+    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+  }
+
+  /**
+   * Returns the X component of the translation.
+   *
+   * @return The x component of the translation.
+   */
+  @JsonProperty
+  public double getX() {
+    return m_x;
+  }
+
+  /**
+   * Returns the Y component of the translation.
+   *
+   * @return The y component of the translation.
+   */
+  @JsonProperty
+  public double getY() {
+    return m_y;
+  }
+
+  /**
+   * Returns the norm, or distance from the origin to the translation.
+   *
+   * @return The norm of the translation.
+   */
+  public double getNorm() {
+    return Math.hypot(m_x, m_y);
+  }
+
+  /**
+   * Applies a rotation to the translation in 2d space.
+   *
+   * <p>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]
+   *
+   * <p>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.
+   */
+  public Translation2d rotateBy(Rotation2d other) {
+    return new Translation2d(
+        m_x * other.getCos() - m_y * other.getSin(), m_x * other.getSin() + m_y * other.getCos());
+  }
+
+  /**
+   * Adds two translations in 2d space and returns the sum. This is similar to vector addition.
+   *
+   * <p>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.
+   */
+  public Translation2d plus(Translation2d other) {
+    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+  }
+
+  /**
+   * Subtracts the other translation from the other translation and returns the difference.
+   *
+   * <p>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.
+   */
+  public Translation2d minus(Translation2d other) {
+    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+  }
+
+  /**
+   * 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.
+   */
+  public Translation2d unaryMinus() {
+    return new Translation2d(-m_x, -m_y);
+  }
+
+  /**
+   * Multiplies the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The scaled translation.
+   */
+  public Translation2d times(double scalar) {
+    return new Translation2d(m_x * scalar, m_y * scalar);
+  }
+
+  /**
+   * Divides the translation by a scalar and returns the new translation.
+   *
+   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+   *
+   * @param scalar The scalar to multiply by.
+   * @return The reference to the new mutated object.
+   */
+  public Translation2d div(double scalar) {
+    return new Translation2d(m_x / scalar, m_y / scalar);
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+  }
+
+  /**
+   * Checks equality between this Translation2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Translation2d) {
+      return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
+          && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_x, m_y);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
new file mode 100644
index 0000000..c73d236
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/geometry/Twist2d.java
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import java.util.Objects;
+
+/**
+ * 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.
+ *
+ * <p>A Twist can be used to represent a difference between two poses.
+ */
+@SuppressWarnings("MemberName")
+public class Twist2d {
+  /** Linear "dx" component. */
+  public double dx;
+
+  /** Linear "dy" component. */
+  public double dy;
+
+  /** Angular "dtheta" component (radians). */
+  public double dtheta;
+
+  public Twist2d() {}
+
+  /**
+   * Constructs a Twist2d with the given values.
+   *
+   * @param dx Change in x direction relative to robot.
+   * @param dy Change in y direction relative to robot.
+   * @param dtheta Change in angle relative to robot.
+   */
+  public Twist2d(double dx, double dy, double dtheta) {
+    this.dx = dx;
+    this.dy = dy;
+    this.dtheta = dtheta;
+  }
+
+  @Override
+  public String toString() {
+    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+  }
+
+  /**
+   * Checks equality between this Twist2d and another object.
+   *
+   * @param obj The other object.
+   * @return Whether the two objects are equal or not.
+   */
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof Twist2d) {
+      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
+          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
+          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(dx, dy, dtheta);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
new file mode 100644
index 0000000..451c008
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/ChassisSpeeds.java
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+
+/**
+ * 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.
+ *
+ * <p>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.
+ */
+@SuppressWarnings("MemberName")
+public class ChassisSpeeds {
+  /** Represents forward velocity w.r.t the robot frame of reference. (Fwd is +) */
+  public double vxMetersPerSecond;
+
+  /** Represents sideways velocity w.r.t the robot frame of reference. (Left is +) */
+  public double vyMetersPerSecond;
+
+  /** Represents the angular velocity of the robot frame. (CCW is +) */
+  public double omegaRadiansPerSecond;
+
+  /** Constructs a ChassisSpeeds with zeros for dx, dy, and theta. */
+  public ChassisSpeeds() {}
+
+  /**
+   * Constructs a ChassisSpeeds object.
+   *
+   * @param vxMetersPerSecond Forward velocity.
+   * @param vyMetersPerSecond Sideways velocity.
+   * @param omegaRadiansPerSecond Angular velocity.
+   */
+  public ChassisSpeeds(
+      double vxMetersPerSecond, double vyMetersPerSecond, double omegaRadiansPerSecond) {
+    this.vxMetersPerSecond = vxMetersPerSecond;
+    this.vyMetersPerSecond = vyMetersPerSecond;
+    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
+  }
+
+  /**
+   * Converts a user provided field-relative set of speeds into a robot-relative ChassisSpeeds
+   * object.
+   *
+   * @param vxMetersPerSecond The component of speed in the x direction relative to the field.
+   *     Positive x is away from your alliance wall.
+   * @param vyMetersPerSecond 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 omegaRadiansPerSecond 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.
+   */
+  public static ChassisSpeeds fromFieldRelativeSpeeds(
+      double vxMetersPerSecond,
+      double vyMetersPerSecond,
+      double omegaRadiansPerSecond,
+      Rotation2d robotAngle) {
+    return new ChassisSpeeds(
+        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
+        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
+        omegaRadiansPerSecond);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
+        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
new file mode 100644
index 0000000..7984e39
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+
+/**
+ * Helper class that converts a chassis velocity (dx and dtheta components) to left and right wheel
+ * velocities for a differential drive.
+ *
+ * <p>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.
+ */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveKinematics {
+  public final double trackWidthMeters;
+
+  /**
+   * Constructs a differential drive kinematics object.
+   *
+   * @param trackWidthMeters 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.
+   */
+  public DifferentialDriveKinematics(double trackWidthMeters) {
+    this.trackWidthMeters = trackWidthMeters;
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
+  }
+
+  /**
+   * Returns a chassis speed from left and right component velocities using forward kinematics.
+   *
+   * @param wheelSpeeds The left and right velocities.
+   * @return The chassis speed.
+   */
+  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
+    return new ChassisSpeeds(
+        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2,
+        0,
+        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond) / trackWidthMeters);
+  }
+
+  /**
+   * 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.
+   */
+  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return new DifferentialDriveWheelSpeeds(
+        chassisSpeeds.vxMetersPerSecond
+            - trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond,
+        chassisSpeeds.vxMetersPerSecond
+            + trackWidthMeters / 2 * chassisSpeeds.omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
new file mode 100644
index 0000000..0139573
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometry.java
@@ -0,0 +1,113 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+
+/**
+ * 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.
+ *
+ * <p>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.
+ *
+ * <p>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.
+ */
+public class DifferentialDriveOdometry {
+  private Pose2d m_poseMeters;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  private double m_prevLeftDistance;
+  private double m_prevRightDistance;
+
+  /**
+   * Constructs a DifferentialDriveOdometry object.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
+  }
+
+  /**
+   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
+   *
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
+    this(gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>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 poseMeters The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+
+    m_prevLeftDistance = 0.0;
+    m_prevRightDistance = 0.0;
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * 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 leftDistanceMeters The distance traveled by the left encoder.
+   * @param rightDistanceMeters The distance traveled by the right encoder.
+   * @return The new pose of the robot.
+   */
+  public Pose2d update(
+      Rotation2d gyroAngle, double leftDistanceMeters, double rightDistanceMeters) {
+    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
+    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
+
+    m_prevLeftDistance = leftDistanceMeters;
+    m_prevRightDistance = rightDistanceMeters;
+
+    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var newPose =
+        m_poseMeters.exp(
+            new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
new file mode 100644
index 0000000..b84eeba
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelSpeeds.java
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+/** Represents the wheel speeds for a differential drive drivetrain. */
+@SuppressWarnings("MemberName")
+public class DifferentialDriveWheelSpeeds {
+  /** Speed of the left side of the robot. */
+  public double leftMetersPerSecond;
+
+  /** Speed of the right side of the robot. */
+  public double rightMetersPerSecond;
+
+  /** Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds. */
+  public DifferentialDriveWheelSpeeds() {}
+
+  /**
+   * Constructs a DifferentialDriveWheelSpeeds.
+   *
+   * @param leftMetersPerSecond The left speed.
+   * @param rightMetersPerSecond The right speed.
+   */
+  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
+    this.leftMetersPerSecond = leftMetersPerSecond;
+    this.rightMetersPerSecond = rightMetersPerSecond;
+  }
+
+  /**
+   * 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      rightMetersPerSecond =
+          rightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
+        leftMetersPerSecond, rightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
new file mode 100644
index 0000000..3ea39e5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java
@@ -0,0 +1,171 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Translation2d;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
+ * wheel speeds.
+ *
+ * <p>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 maneuvers.
+ *
+ * <p>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.
+ *
+ * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds] We take the
+ * Moore-Penrose pseudoinverse of [wheelLocations] and then multiply by [wheelSpeeds] to get our
+ * chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
+ * field using encoders and a gyro.
+ */
+public class MecanumDriveKinematics {
+  private final SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final Translation2d m_frontLeftWheelMeters;
+  private final Translation2d m_frontRightWheelMeters;
+  private final Translation2d m_rearLeftWheelMeters;
+  private final Translation2d m_rearRightWheelMeters;
+
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * Constructs a mecanum drive kinematics object.
+   *
+   * @param frontLeftWheelMeters The location of the front-left wheel relative to the physical
+   *     center of the robot.
+   * @param frontRightWheelMeters The location of the front-right wheel relative to the physical
+   *     center of the robot.
+   * @param rearLeftWheelMeters The location of the rear-left wheel relative to the physical center
+   *     of the robot.
+   * @param rearRightWheelMeters The location of the rear-right wheel relative to the physical
+   *     center of the robot.
+   */
+  public MecanumDriveKinematics(
+      Translation2d frontLeftWheelMeters,
+      Translation2d frontRightWheelMeters,
+      Translation2d rearLeftWheelMeters,
+      Translation2d rearRightWheelMeters) {
+    m_frontLeftWheelMeters = frontLeftWheelMeters;
+    m_frontRightWheelMeters = frontRightWheelMeters;
+    m_rearLeftWheelMeters = rearLeftWheelMeters;
+    m_rearRightWheelMeters = rearRightWheelMeters;
+
+    m_inverseKinematics = new SimpleMatrix(4, 3);
+
+    setInverseKinematics(
+        frontLeftWheelMeters, frontRightWheelMeters, rearLeftWheelMeters, rearRightWheelMeters);
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
+  }
+
+  /**
+   * 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.
+   *
+   * <p>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 maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @param centerOfRotationMeters 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 {@link
+   *     MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(
+      ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    // We have a new center of rotation. We need to compute the matrix again.
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
+      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
+      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
+      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
+
+      setInverseKinematics(fl, fr, rl, rr);
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(
+        0,
+        0,
+        chassisSpeeds.vxMetersPerSecond,
+        chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var wheelsVector = m_inverseKinematics.mult(chassisSpeedsVector);
+    return new MecanumDriveWheelSpeeds(
+        wheelsVector.get(0, 0),
+        wheelsVector.get(1, 0),
+        wheelsVector.get(2, 0),
+        wheelsVector.get(3, 0));
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
+   * information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return The wheel speeds.
+   */
+  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
+    return toWheelSpeeds(chassisSpeeds, new 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.
+   */
+  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
+    var wheelSpeedsVector = new SimpleMatrix(4, 1);
+    wheelSpeedsVector.setColumn(
+        0,
+        0,
+        wheelSpeeds.frontLeftMetersPerSecond,
+        wheelSpeeds.frontRightMetersPerSecond,
+        wheelSpeeds.rearLeftMetersPerSecond,
+        wheelSpeeds.rearRightMetersPerSecond);
+    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsVector);
+
+    return new ChassisSpeeds(
+        chassisSpeedsVector.get(0, 0),
+        chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+  }
+
+  /**
+   * 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.
+   */
+  private void setInverseKinematics(
+      Translation2d fl, Translation2d fr, Translation2d rl, Translation2d rr) {
+    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
+    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
+    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
+    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
new file mode 100644
index 0000000..b504acc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveMotorVoltages.java
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+/** Represents the motor voltages for a mecanum drive drivetrain. */
+@SuppressWarnings("MemberName")
+public class MecanumDriveMotorVoltages {
+  /** Voltage of the front left motor. */
+  public double frontLeftVoltage;
+
+  /** Voltage of the front right motor. */
+  public double frontRightVoltage;
+
+  /** Voltage of the rear left motor. */
+  public double rearLeftVoltage;
+
+  /** Voltage of the rear right motor. */
+  public double rearRightVoltage;
+
+  /** Constructs a MecanumDriveMotorVoltages with zeros for all member fields. */
+  public MecanumDriveMotorVoltages() {}
+
+  /**
+   * Constructs a MecanumDriveMotorVoltages.
+   *
+   * @param frontLeftVoltage Voltage of the front left motor.
+   * @param frontRightVoltage Voltage of the front right motor.
+   * @param rearLeftVoltage Voltage of the rear left motor.
+   * @param rearRightVoltage Voltage of the rear right motor.
+   */
+  public MecanumDriveMotorVoltages(
+      double frontLeftVoltage,
+      double frontRightVoltage,
+      double rearLeftVoltage,
+      double rearRightVoltage) {
+    this.frontLeftVoltage = frontLeftVoltage;
+    this.frontRightVoltage = frontRightVoltage;
+    this.rearLeftVoltage = rearLeftVoltage;
+    this.rearRightVoltage = rearRightVoltage;
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
+            + "Rear Left: %.2f V, Rear Right: %.2f V)",
+        frontLeftVoltage, frontRightVoltage, rearLeftVoltage, rearRightVoltage);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
new file mode 100644
index 0000000..b3c79c9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveOdometry.java
@@ -0,0 +1,125 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * 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.
+ *
+ * <p>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.
+ */
+public class MecanumDriveOdometry {
+  private final MecanumDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * Constructs a MecanumDriveOdometry object.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param initialPoseMeters The starting position of the robot on the field.
+   */
+  public MecanumDriveOdometry(
+      MecanumDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPoseMeters) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPoseMeters;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPoseMeters.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
+  }
+
+  /**
+   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The mecanum drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>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 poseMeters The position on the field that your robot is at.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
+    m_poseMeters = poseMeters;
+    m_previousAngle = poseMeters.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * 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 currentTimeSeconds The current time in seconds.
+   * @param gyroAngle The angle reported by the gyroscope.
+   * @param wheelSpeeds The current wheel speeds.
+   * @return The new pose of the robot.
+   */
+  public Pose2d updateWithTime(
+      double currentTimeSeconds, Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
+    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
+    var newPose =
+        m_poseMeters.exp(
+            new Twist2d(
+                chassisState.vxMetersPerSecond * period,
+                chassisState.vyMetersPerSecond * period,
+                angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+    return m_poseMeters;
+  }
+
+  /**
+   * 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.
+   */
+  public Pose2d update(Rotation2d gyroAngle, MecanumDriveWheelSpeeds wheelSpeeds) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, wheelSpeeds);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
new file mode 100644
index 0000000..7a159fe
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelSpeeds.java
@@ -0,0 +1,86 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import java.util.stream.DoubleStream;
+
+@SuppressWarnings("MemberName")
+public class MecanumDriveWheelSpeeds {
+  /** Speed of the front left wheel. */
+  public double frontLeftMetersPerSecond;
+
+  /** Speed of the front right wheel. */
+  public double frontRightMetersPerSecond;
+
+  /** Speed of the rear left wheel. */
+  public double rearLeftMetersPerSecond;
+
+  /** Speed of the rear right wheel. */
+  public double rearRightMetersPerSecond;
+
+  /** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
+  public MecanumDriveWheelSpeeds() {}
+
+  /**
+   * Constructs a MecanumDriveWheelSpeeds.
+   *
+   * @param frontLeftMetersPerSecond Speed of the front left wheel.
+   * @param frontRightMetersPerSecond Speed of the front right wheel.
+   * @param rearLeftMetersPerSecond Speed of the rear left wheel.
+   * @param rearRightMetersPerSecond Speed of the rear right wheel.
+   */
+  public MecanumDriveWheelSpeeds(
+      double frontLeftMetersPerSecond,
+      double frontRightMetersPerSecond,
+      double rearLeftMetersPerSecond,
+      double rearRightMetersPerSecond) {
+    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
+    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
+    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
+    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
+  }
+
+  /**
+   * 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
+   */
+  public void normalize(double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed =
+        DoubleStream.of(
+                frontLeftMetersPerSecond,
+                frontRightMetersPerSecond,
+                rearLeftMetersPerSecond,
+                rearRightMetersPerSecond)
+            .max()
+            .getAsDouble();
+
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      frontLeftMetersPerSecond =
+          frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      frontRightMetersPerSecond =
+          frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      rearLeftMetersPerSecond =
+          rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      rearRightMetersPerSecond =
+          rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+    }
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
+            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
+        frontLeftMetersPerSecond,
+        frontRightMetersPerSecond,
+        rearLeftMetersPerSecond,
+        rearRightMetersPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
new file mode 100644
index 0000000..4c6d9cc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveKinematics.java
@@ -0,0 +1,194 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.Arrays;
+import java.util.Collections;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Helper class that converts a chassis velocity (dx, dy, and dtheta components) into individual
+ * module states (speed and angle).
+ *
+ * <p>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 maneuvers.
+ *
+ * <p>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.
+ *
+ * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds] We take the
+ * Moore-Penrose pseudoinverse of [moduleLocations] and then multiply by [moduleStates] to get our
+ * chassis speeds.
+ *
+ * <p>Forward kinematics is also used for odometry -- determining the position of the robot on the
+ * field using encoders and a gyro.
+ */
+public class SwerveDriveKinematics {
+  private final SimpleMatrix m_inverseKinematics;
+  private final SimpleMatrix m_forwardKinematics;
+
+  private final int m_numModules;
+  private final Translation2d[] m_modules;
+  private Translation2d m_prevCoR = new Translation2d();
+
+  /**
+   * 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 receive 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 wheelsMeters The locations of the wheels relative to the physical center of the robot.
+   */
+  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
+    if (wheelsMeters.length < 2) {
+      throw new IllegalArgumentException("A swerve drive requires at least two modules");
+    }
+    m_numModules = wheelsMeters.length;
+    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
+    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
+
+    for (int i = 0; i < m_numModules; i++) {
+      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
+      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
+    }
+    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
+
+    MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
+  }
+
+  /**
+   * 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.
+   *
+   * <p>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 maneuvers, vision alignment, or for any other use case, you can do so.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @param centerOfRotationMeters 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 {@link #normalizeWheelSpeeds(SwerveModuleState[], double)
+   *     normalizeWheelSpeeds} function to rectify this issue.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public SwerveModuleState[] toSwerveModuleStates(
+      ChassisSpeeds chassisSpeeds, Translation2d centerOfRotationMeters) {
+    if (!centerOfRotationMeters.equals(m_prevCoR)) {
+      for (int i = 0; i < m_numModules; i++) {
+        m_inverseKinematics.setRow(
+            i * 2 + 0,
+            0, /* Start Data */
+            1,
+            0,
+            -m_modules[i].getY() + centerOfRotationMeters.getY());
+        m_inverseKinematics.setRow(
+            i * 2 + 1,
+            0, /* Start Data */
+            0,
+            1,
+            +m_modules[i].getX() - centerOfRotationMeters.getX());
+      }
+      m_prevCoR = centerOfRotationMeters;
+    }
+
+    var chassisSpeedsVector = new SimpleMatrix(3, 1);
+    chassisSpeedsVector.setColumn(
+        0,
+        0,
+        chassisSpeeds.vxMetersPerSecond,
+        chassisSpeeds.vyMetersPerSecond,
+        chassisSpeeds.omegaRadiansPerSecond);
+
+    var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
+    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
+
+    for (int i = 0; i < m_numModules; i++) {
+      double x = moduleStatesMatrix.get(i * 2, 0);
+      double y = moduleStatesMatrix.get(i * 2 + 1, 0);
+
+      double speed = Math.hypot(x, y);
+      Rotation2d angle = new Rotation2d(x, y);
+
+      moduleStates[i] = new SwerveModuleState(speed, angle);
+    }
+
+    return moduleStates;
+  }
+
+  /**
+   * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
+   * toSwerveModuleStates for more information.
+   *
+   * @param chassisSpeeds The desired chassis speed.
+   * @return An array containing the module states.
+   */
+  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
+    return toSwerveModuleStates(chassisSpeeds, new 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.
+   */
+  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
+    if (wheelStates.length != m_numModules) {
+      throw new IllegalArgumentException(
+          "Number of modules is not consistent with number of wheel locations provided in "
+              + "constructor");
+    }
+    var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
+
+    for (int i = 0; i < m_numModules; i++) {
+      var module = wheelStates[i];
+      moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
+      moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
+    }
+
+    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
+    return new ChassisSpeeds(
+        chassisSpeedsVector.get(0, 0),
+        chassisSpeedsVector.get(1, 0),
+        chassisSpeedsVector.get(2, 0));
+  }
+
+  /**
+   * 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
+   */
+  public static void normalizeWheelSpeeds(
+      SwerveModuleState[] moduleStates, double attainableMaxSpeedMetersPerSecond) {
+    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
+    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
+      for (SwerveModuleState moduleState : moduleStates) {
+        moduleState.speedMetersPerSecond =
+            moduleState.speedMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
new file mode 100644
index 0000000..8b4161e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveDriveOdometry.java
@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * 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.
+ *
+ * <p>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.
+ */
+public class SwerveDriveOdometry {
+  private final SwerveDriveKinematics m_kinematics;
+  private Pose2d m_poseMeters;
+  private double m_prevTimeSeconds = -1;
+
+  private Rotation2d m_gyroOffset;
+  private Rotation2d m_previousAngle;
+
+  /**
+   * 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.
+   */
+  public SwerveDriveOdometry(
+      SwerveDriveKinematics kinematics, Rotation2d gyroAngle, Pose2d initialPose) {
+    m_kinematics = kinematics;
+    m_poseMeters = initialPose;
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+    m_previousAngle = initialPose.getRotation();
+    MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
+  }
+
+  /**
+   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
+   *
+   * @param kinematics The swerve drive kinematics for your drivetrain.
+   * @param gyroAngle The angle reported by the gyroscope.
+   */
+  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
+    this(kinematics, gyroAngle, new Pose2d());
+  }
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>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.
+   */
+  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
+    m_poseMeters = pose;
+    m_previousAngle = pose.getRotation();
+    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
+  }
+
+  /**
+   * Returns the position of the robot on the field.
+   *
+   * @return The pose of the robot (x and y are in meters).
+   */
+  public Pose2d getPoseMeters() {
+    return m_poseMeters;
+  }
+
+  /**
+   * 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 currentTimeSeconds The current time in seconds.
+   * @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.
+   */
+  public Pose2d updateWithTime(
+      double currentTimeSeconds, Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
+    m_prevTimeSeconds = currentTimeSeconds;
+
+    var angle = gyroAngle.plus(m_gyroOffset);
+
+    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
+    var newPose =
+        m_poseMeters.exp(
+            new Twist2d(
+                chassisState.vxMetersPerSecond * period,
+                chassisState.vyMetersPerSecond * period,
+                angle.minus(m_previousAngle).getRadians()));
+
+    m_previousAngle = angle;
+    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
+
+    return m_poseMeters;
+  }
+
+  /**
+   * 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.
+   */
+  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
+    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
new file mode 100644
index 0000000..6a9c48c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/kinematics/SwerveModuleState.java
@@ -0,0 +1,85 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.Objects;
+
+/** Represents the state of one swerve module. */
+@SuppressWarnings("MemberName")
+public class SwerveModuleState implements Comparable<SwerveModuleState> {
+  /** Speed of the wheel of the module. */
+  public double speedMetersPerSecond;
+
+  /** Angle of the module. */
+  public Rotation2d angle = Rotation2d.fromDegrees(0);
+
+  /** Constructs a SwerveModuleState with zeros for speed and angle. */
+  public SwerveModuleState() {}
+
+  /**
+   * Constructs a SwerveModuleState.
+   *
+   * @param speedMetersPerSecond The speed of the wheel of the module.
+   * @param angle The angle of the module.
+   */
+  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
+    this.speedMetersPerSecond = speedMetersPerSecond;
+    this.angle = angle;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof SwerveModuleState) {
+      return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
+          == 0;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(speedMetersPerSecond);
+  }
+
+  /**
+   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
+   * is higher than the other.
+   *
+   * @param other The other swerve module.
+   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
+   */
+  @Override
+  public int compareTo(SwerveModuleState other) {
+    return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond);
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
+  }
+
+  /**
+   * Minimize the change in heading the desired swerve module state would require by potentially
+   * reversing the direction the wheel spins. If this is used with the PIDController class's
+   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
+   *
+   * @param desiredState The desired state.
+   * @param currentAngle The current module angle.
+   * @return Optimized swerve module state.
+   */
+  public static SwerveModuleState optimize(
+      SwerveModuleState desiredState, Rotation2d currentAngle) {
+    var delta = desiredState.angle.minus(currentAngle);
+    if (Math.abs(delta.getDegrees()) > 90.0) {
+      return new SwerveModuleState(
+          -desiredState.speedMetersPerSecond,
+          desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
+    } else {
+      return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
new file mode 100644
index 0000000..9bbeaf6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/CubicHermiteSpline.java
@@ -0,0 +1,137 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class CubicHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("ParameterName")
+  public CubicHermiteSpline(
+      double[] xInitialControlVector,
+      double[] xFinalControlVector,
+      double[] yInitialControlVector,
+      double[] yFinalControlVector) {
+    super(3);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 4);
+
+    for (int i = 0; i < 4; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // Then populate row 4 and 5 with the second derivatives.
+      // 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.set(2, i, m_coefficients.get(0, i) * (3 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, 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.set(4, i, m_coefficients.get(2, i) * (2 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  private SimpleMatrix makeHermiteBasis() {
+    if (hermiteBasis == null) {
+      // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
+      // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+      //
+      // P(i)    = P(0)  = a0
+      // P'(i)   = P'(0) = a1
+      // P(i+1)  = P(1)  = a3 + a2 + a1 + a0
+      // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+      //
+      // [ P(i)    ] = [ 0 0 0 1 ][ a3 ]
+      // [ P'(i)   ] = [ 0 0 1 0 ][ a2 ]
+      // [ P(i+1)  ] = [ 1 1 1 1 ][ a1 ]
+      // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+      //
+      // To solve for the coefficients, we can invert the 4x4 matrix and move it
+      // to the other side of the equation.
+      //
+      // [ a3 ] = [  2  1 -2  1 ][ P(i)    ]
+      // [ a2 ] = [ -3 -2  3 -1 ][ P'(i)   ]
+      // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
+      // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
+      hermiteBasis =
+          new SimpleMatrix(
+              4,
+              4,
+              true,
+              new double[] {
+                +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
+              });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * 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.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 2 || finalVector.length != 2) {
+      throw new IllegalArgumentException("Size of vectors must be 2");
+    }
+    return new SimpleMatrix(
+        4,
+        1,
+        true,
+        new double[] {
+          initialVector[0], initialVector[1],
+          finalVector[0], finalVector[1]
+        });
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
new file mode 100644
index 0000000..8bad7b1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/PoseWithCurvature.java
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/** Represents a pair of a pose and a curvature. */
+@SuppressWarnings("MemberName")
+public class PoseWithCurvature {
+  // Represents the pose.
+  public Pose2d poseMeters;
+
+  // Represents the curvature.
+  public double curvatureRadPerMeter;
+
+  /**
+   * Constructs a PoseWithCurvature.
+   *
+   * @param poseMeters The pose.
+   * @param curvatureRadPerMeter The curvature.
+   */
+  public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
+    this.poseMeters = poseMeters;
+    this.curvatureRadPerMeter = curvatureRadPerMeter;
+  }
+
+  /** Constructs a PoseWithCurvature with default values. */
+  public PoseWithCurvature() {
+    poseMeters = new Pose2d();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
new file mode 100644
index 0000000..4017044
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/QuinticHermiteSpline.java
@@ -0,0 +1,145 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import org.ejml.simple.SimpleMatrix;
+
+public class QuinticHermiteSpline extends Spline {
+  private static SimpleMatrix hermiteBasis;
+  private final SimpleMatrix m_coefficients;
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("ParameterName")
+  public QuinticHermiteSpline(
+      double[] xInitialControlVector,
+      double[] xFinalControlVector,
+      double[] yInitialControlVector,
+      double[] yFinalControlVector) {
+    super(5);
+
+    // Populate the coefficients for the actual spline equations.
+    // Row 0 is x coefficients
+    // Row 1 is y coefficients
+    final var hermite = makeHermiteBasis();
+    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+    final var xCoeffs = (hermite.mult(x)).transpose();
+    final var yCoeffs = (hermite.mult(y)).transpose();
+
+    m_coefficients = new SimpleMatrix(6, 6);
+
+    for (int i = 0; i < 6; i++) {
+      m_coefficients.set(0, i, xCoeffs.get(0, i));
+      m_coefficients.set(1, i, yCoeffs.get(0, i));
+    }
+    for (int i = 0; i < 6; i++) {
+      // Populate Row 2 and Row 3 with the derivatives of the equations above.
+      // 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.set(2, i, m_coefficients.get(0, i) * (5 - i));
+      m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
+    }
+    for (int i = 0; i < 5; i++) {
+      // Then populate row 4 and 5 with the second derivatives.
+      // 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.set(4, i, m_coefficients.get(2, i) * (4 - i));
+      m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
+    }
+  }
+
+  /**
+   * Returns the coefficients matrix.
+   *
+   * @return The coefficients matrix.
+   */
+  @Override
+  protected SimpleMatrix getCoefficients() {
+    return m_coefficients;
+  }
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   *
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  private SimpleMatrix makeHermiteBasis() {
+    if (hermiteBasis == null) {
+      // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
+      // vectors, we want to find the coefficients of the spline
+      // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+      //
+      // P(i)     = P(0)   = a0
+      // P'(i)    = P'(0)  = a1
+      // P''(i)   = P''(0) = 2 * a2
+      // P(i+1)   = P(1)   = a5 + a4 + a3 + a2 + a1 + a0
+      // P'(i+1)  = P'(1)  = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
+      // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+      //
+      // [ P(i)     ] = [  0  0  0  0  0  1 ][ a5 ]
+      // [ P'(i)    ] = [  0  0  0  0  1  0 ][ a4 ]
+      // [ P''(i)   ] = [  0  0  0  2  0  0 ][ a3 ]
+      // [ P(i+1)   ] = [  1  1  1  1  1  1 ][ a2 ]
+      // [ P'(i+1)  ] = [  5  4  3  2  1  0 ][ a1 ]
+      // [ P''(i+1) ] = [ 20 12  6  2  0  0 ][ a0 ]
+      //
+      // To solve for the coefficients, we can invert the 6x6 matrix and move it
+      // to the other side of the equation.
+      //
+      // [ a5 ] = [  -6.0  -3.0  -0.5   6.0  -3.0   0.5 ][ P(i)     ]
+      // [ a4 ] = [  15.0   8.0   1.5 -15.0   7.0  -1.0 ][ P'(i)    ]
+      // [ a3 ] = [ -10.0  -6.0  -1.5  10.0  -4.0   0.5 ][ P''(i)   ]
+      // [ a2 ] = [   0.0   0.0   0.5   0.0   0.0   0.0 ][ P(i+1)   ]
+      // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
+      // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
+      hermiteBasis =
+          new SimpleMatrix(
+              6,
+              6,
+              true,
+              new double[] {
+                -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
+              });
+    }
+    return hermiteBasis;
+  }
+
+  /**
+   * 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.
+   */
+  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
+    if (initialVector.length != 3 || finalVector.length != 3) {
+      throw new IllegalArgumentException("Size of vectors must be 3");
+    }
+    return new SimpleMatrix(
+        6,
+        1,
+        true,
+        new double[] {
+          initialVector[0], initialVector[1], initialVector[2],
+          finalVector[0], finalVector[1], finalVector[2]
+        });
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
new file mode 100644
index 0000000..5451eea
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/Spline.java
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.Arrays;
+import org.ejml.simple.SimpleMatrix;
+
+/** Represents a two-dimensional parametric spline that interpolates between two points. */
+public abstract class Spline {
+  private final int m_degree;
+
+  /**
+   * Constructs a spline with the given degree.
+   *
+   * @param degree The degree of the spline.
+   */
+  Spline(int degree) {
+    m_degree = degree;
+  }
+
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  protected abstract SimpleMatrix getCoefficients();
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("ParameterName")
+  public PoseWithCurvature getPoint(double t) {
+    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
+    final var coefficients = getCoefficients();
+
+    // Populate the polynomial bases.
+    for (int i = 0; i <= m_degree; i++) {
+      polynomialBases.set(i, 0, Math.pow(t, m_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.
+    SimpleMatrix combined = coefficients.mult(polynomialBases);
+
+    // Get x and y
+    final double x = combined.get(0, 0);
+    final double y = combined.get(1, 0);
+
+    double dx;
+    double dy;
+    double ddx;
+    double ddy;
+
+    if (t == 0) {
+      dx = coefficients.get(2, m_degree - 1);
+      dy = coefficients.get(3, m_degree - 1);
+      ddx = coefficients.get(4, m_degree - 2);
+      ddy = coefficients.get(5, m_degree - 2);
+    } else {
+      // Divide out t once for first derivative.
+      dx = combined.get(2, 0) / t;
+      dy = combined.get(3, 0) / t;
+
+      // Divide out t twice for second derivative.
+      ddx = combined.get(4, 0) / t / t;
+      ddy = combined.get(5, 0) / t / t;
+    }
+
+    // Find the curvature.
+    final double curvature = (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
+
+    return new PoseWithCurvature(new Pose2d(x, y, new Rotation2d(dx, dy)), curvature);
+  }
+
+  /**
+   * Represents a control vector for a spline.
+   *
+   * <p>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.
+   */
+  @SuppressWarnings("MemberName")
+  public static class ControlVector {
+    public double[] x;
+    public double[] y;
+
+    /**
+     * Instantiates a control vector.
+     *
+     * @param x The x dimension of the control vector.
+     * @param y The y dimension of the control vector.
+     */
+    @SuppressWarnings("ParameterName")
+    public ControlVector(double[] x, double[] y) {
+      this.x = Arrays.copyOf(x, x.length);
+      this.y = Arrays.copyOf(y, y.length);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
new file mode 100644
index 0000000..e5c67f8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineHelper.java
@@ -0,0 +1,268 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.Arrays;
+import java.util.List;
+
+public final class SplineHelper {
+  /** Private constructor because this is a utility class. */
+  private SplineHelper() {}
+
+  /**
+   * 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.
+   */
+  public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
+      Pose2d start, Translation2d[] interiorWaypoints, Pose2d end) {
+    // Generate control vectors from poses.
+    Spline.ControlVector initialCV;
+    Spline.ControlVector endCV;
+
+    // Chooses a magnitude automatically that makes the splines look better.
+    if (interiorWaypoints.length < 1) {
+      double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      endCV = getCubicControlVector(scalar, end);
+    } else {
+      double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
+      initialCV = getCubicControlVector(scalar, start);
+      scalar =
+          end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1]) * 1.2;
+      endCV = getCubicControlVector(scalar, end);
+    }
+    return new Spline.ControlVector[] {initialCV, endCV};
+  }
+
+  /**
+   * Returns quintic splines from a set of waypoints.
+   *
+   * @param waypoints The waypoints
+   * @return List of splines.
+   */
+  public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
+    for (int i = 0; i < waypoints.size() - 1; ++i) {
+      var p0 = waypoints.get(i);
+      var p1 = waypoints.get(i + 1);
+
+      // This just makes the splines look better.
+      final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
+
+      var controlVecA = getQuinticControlVector(scalar, p0);
+      var controlVecB = getQuinticControlVector(scalar, p1);
+
+      splines[i] =
+          new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
+    }
+    return splines;
+  }
+
+  /**
+   * 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.
+   *
+   * @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 and
+   *     control vectors.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
+      Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
+    CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
+
+    double[] xInitial = start.x;
+    double[] yInitial = start.y;
+    double[] xFinal = end.x;
+    double[] yFinal = end.y;
+
+    if (waypoints.length > 1) {
+      Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
+
+      // Create an array of all waypoints, including the start and end.
+      newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
+      System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
+      newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], 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
+      final double[] a = new double[newWaypts.length - 2];
+
+      // Diagonal of tridiagonal matrix
+      final double[] b = new double[newWaypts.length - 2];
+      Arrays.fill(b, 4.0);
+
+      // Below-diagonal of tridiagonal matrix, zero-padded
+      final double[] c = new double[newWaypts.length - 2];
+
+      // rhs vectors
+      final double[] dx = new double[newWaypts.length - 2];
+      final double[] dy = new double[newWaypts.length - 2];
+
+      // solution vectors
+      final double[] fx = new double[newWaypts.length - 2];
+      final double[] fy = new double[newWaypts.length - 2];
+
+      // populate above-diagonal and below-diagonal vectors
+      a[0] = 0.0;
+      for (int i = 0; i < newWaypts.length - 3; i++) {
+        a[i + 1] = 1;
+        c[i] = 1;
+      }
+      c[c.length - 1] = 0.0;
+
+      // populate rhs vectors
+      dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
+      dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
+
+      if (newWaypts.length > 4) {
+        for (int i = 1; i <= newWaypts.length - 4; i++) {
+          // dx and dy represent the derivatives of the internal waypoints. The derivative
+          // of the second internal waypoint should involve the third and first internal waypoint,
+          // which have indices of 1 and 3 in the newWaypts list (which contains ALL waypoints).
+          dx[i] = 3 * (newWaypts[i + 2].getX() - newWaypts[i].getX());
+          dy[i] = 3 * (newWaypts[i + 2].getY() - newWaypts[i].getY());
+        }
+      }
+
+      dx[dx.length - 1] =
+          3 * (newWaypts[newWaypts.length - 1].getX() - newWaypts[newWaypts.length - 3].getX())
+              - xFinal[1];
+      dy[dy.length - 1] =
+          3 * (newWaypts[newWaypts.length - 1].getY() - newWaypts[newWaypts.length - 3].getY())
+              - yFinal[1];
+
+      // Compute solution to tridiagonal system
+      thomasAlgorithm(a, b, c, dx, fx);
+      thomasAlgorithm(a, b, c, dy, fy);
+
+      double[] newFx = new double[fx.length + 2];
+      double[] newFy = new double[fy.length + 2];
+
+      newFx[0] = xInitial[1];
+      newFy[0] = yInitial[1];
+      System.arraycopy(fx, 0, newFx, 1, fx.length);
+      System.arraycopy(fy, 0, newFy, 1, fy.length);
+      newFx[newFx.length - 1] = xFinal[1];
+      newFy[newFy.length - 1] = yFinal[1];
+
+      for (int i = 0; i < newFx.length - 1; i++) {
+        splines[i] =
+            new CubicHermiteSpline(
+                new double[] {newWaypts[i].getX(), newFx[i]},
+                new double[] {newWaypts[i + 1].getX(), newFx[i + 1]},
+                new double[] {newWaypts[i].getY(), newFy[i]},
+                new double[] {newWaypts[i + 1].getY(), newFy[i + 1]});
+      }
+    } else if (waypoints.length == 1) {
+      final var xDeriv = (3 * (xFinal[0] - xInitial[0]) - xFinal[1] - xInitial[1]) / 4.0;
+      final var yDeriv = (3 * (yFinal[0] - yInitial[0]) - yFinal[1] - yInitial[1]) / 4.0;
+
+      double[] midXControlVector = {waypoints[0].getX(), xDeriv};
+      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
+
+      splines[0] =
+          new CubicHermiteSpline(
+              xInitial, midXControlVector,
+              yInitial, midYControlVector);
+      splines[1] =
+          new CubicHermiteSpline(
+              midXControlVector, xFinal,
+              midYControlVector, yFinal);
+    } else {
+      splines[0] =
+          new CubicHermiteSpline(
+              xInitial, xFinal,
+              yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided control vectors. The user is
+   * free to set the direction of all control vectors. 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.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
+      Spline.ControlVector[] controlVectors) {
+    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
+    for (int i = 0; i < controlVectors.length - 1; i++) {
+      var xInitial = controlVectors[i].x;
+      var xFinal = controlVectors[i + 1].x;
+      var yInitial = controlVectors[i].y;
+      var yFinal = controlVectors[i + 1].y;
+      splines[i] =
+          new QuinticHermiteSpline(
+              xInitial, xFinal,
+              yInitial, yFinal);
+    }
+    return splines;
+  }
+
+  /**
+   * 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
+   */
+  @SuppressWarnings({"ParameterName", "LocalVariableName"})
+  private static void thomasAlgorithm(
+      double[] a, double[] b, double[] c, double[] d, double[] solutionVector) {
+    int N = d.length;
+
+    double[] cStar = new double[N];
+    double[] dStar = new double[N];
+
+    // This updates the coefficients in the first row
+    // Note that we should be checking for division by zero here
+    cStar[0] = c[0] / b[0];
+    dStar[0] = d[0] / b[0];
+
+    // Create the c_star and d_star coefficients in the forward sweep
+    for (int i = 1; i < N; i++) {
+      double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
+      cStar[i] = c[i] * m;
+      dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
+    }
+    solutionVector[N - 1] = dStar[N - 1];
+    // This is the reverse sweep, used to update the solution vector f
+    for (int i = N - 2; i >= 0; i--) {
+      solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
+    }
+  }
+
+  private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[] {point.getX(), scalar * point.getRotation().getCos()},
+        new double[] {point.getY(), scalar * point.getRotation().getSin()});
+  }
+
+  private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
+    return new Spline.ControlVector(
+        new double[] {point.getX(), scalar * point.getRotation().getCos(), 0.0},
+        new double[] {point.getY(), scalar * point.getRotation().getSin(), 0.0});
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
new file mode 100644
index 0000000..88afc6d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/spline/SplineParameterizer.java
@@ -0,0 +1,141 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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.
+ */
+
+package edu.wpi.first.math.spline;
+
+import java.util.ArrayDeque;
+import java.util.ArrayList;
+import java.util.List;
+
+/** Class used to parameterize a spline by its arc length. */
+public final class SplineParameterizer {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  /**
+   * 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.
+   */
+  private static final int kMaxIterations = 5000;
+
+  @SuppressWarnings("MemberName")
+  private static class StackContents {
+    final double t1;
+    final double t0;
+
+    StackContents(double t0, double t1) {
+      this.t0 = t0;
+      this.t1 = t1;
+    }
+  }
+
+  @SuppressWarnings("serial")
+  public static class MalformedSplineException extends RuntimeException {
+    /**
+     * Create a new exception with the given message.
+     *
+     * @param message the message to pass with the exception
+     */
+    private MalformedSplineException(String message) {
+      super(message);
+    }
+  }
+
+  /** Private constructor because this is a utility class. */
+  private SplineParameterizer() {}
+
+  /**
+   * 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.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *     with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> parameterize(Spline spline) {
+    return parameterize(spline, 0.0, 1.0);
+  }
+
+  /**
+   * 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 use 0.0.
+   * @param t1 Ending internal spline parameter. It is recommended to use 1.0.
+   * @return A list of poses and curvatures that represents various points on the spline.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *     with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // The parameterization does not add the initial point. Let's add that.
+    splinePoints.add(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
+    var stack = new ArrayDeque<StackContents>();
+    stack.push(new StackContents(t0, t1));
+
+    StackContents current;
+    PoseWithCurvature start;
+    PoseWithCurvature end;
+    int iterations = 0;
+
+    while (!stack.isEmpty()) {
+      current = stack.removeFirst();
+      start = spline.getPoint(current.t0);
+      end = spline.getPoint(current.t1);
+
+      final var twist = start.poseMeters.log(end.poseMeters);
+      if (Math.abs(twist.dy) > kMaxDy
+          || Math.abs(twist.dx) > kMaxDx
+          || Math.abs(twist.dtheta) > kMaxDtheta) {
+        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
+        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
+      } else {
+        splinePoints.add(spline.getPoint(current.t1));
+      }
+
+      iterations++;
+      if (iterations >= kMaxIterations) {
+        throw new 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;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
new file mode 100644
index 0000000..ffbd99e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/Discretization.java
@@ -0,0 +1,167 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.Pair;
+import org.ejml.simple.SimpleMatrix;
+
+@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+public final class Discretization {
+  private Discretization() {
+    // Utility class
+  }
+
+  /**
+   * Discretizes the given continuous A matrix.
+   *
+   * @param <States> Num representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return the discrete matrix system.
+   */
+  public static <States extends Num> Matrix<States, States> discretizeA(
+      Matrix<States, States> contA, double dtSeconds) {
+    return contA.times(dtSeconds).exp();
+  }
+
+  /**
+   * Discretizes the given continuous A and B matrices.
+   *
+   * @param <States> Nat representing the states of the system.
+   * @param <Inputs> Nat representing the inputs to the system.
+   * @param contA Continuous system matrix.
+   * @param contB Continuous input matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a Pair representing discA and diskB.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num, Inputs extends Num>
+      Pair<Matrix<States, States>, Matrix<States, Inputs>> discretizeAB(
+          Matrix<States, States> contA, Matrix<States, Inputs> contB, double dtSeconds) {
+    var scaledA = contA.times(dtSeconds);
+    var scaledB = contB.times(dtSeconds);
+
+    int states = contA.getNumRows();
+    int inputs = contB.getNumCols();
+    var M = new Matrix<>(new SimpleMatrix(states + inputs, states + inputs));
+    M.assignBlock(0, 0, scaledA);
+    M.assignBlock(0, scaledA.getNumCols(), scaledB);
+    var phi = M.exp();
+
+    var discA = new Matrix<States, States>(new SimpleMatrix(states, states));
+    var discB = new Matrix<States, Inputs>(new SimpleMatrix(states, inputs));
+
+    discA.extractFrom(0, 0, phi);
+    discB.extractFrom(0, contB.getNumRows(), phi);
+
+    return new Pair<>(discA, discB);
+  }
+
+  /**
+   * Discretizes the given continuous A and Q matrices.
+   *
+   * @param <States> Nat representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param contQ Continuous process noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a pair representing the discrete system matrix and process noise covariance matrix.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num>
+      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQ(
+          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
+    int states = contA.getNumRows();
+
+    // Make continuous Q symmetric if it isn't already
+    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
+
+    // Set up the matrix M = [[-A, Q], [0, A.T]]
+    final var M = new Matrix<>(new SimpleMatrix(2 * states, 2 * states));
+    M.assignBlock(0, 0, contA.times(-1.0));
+    M.assignBlock(0, states, Q);
+    M.assignBlock(states, 0, new Matrix<>(new SimpleMatrix(states, states)));
+    M.assignBlock(states, states, contA.transpose());
+
+    final var phi = M.times(dtSeconds).exp();
+
+    // Phi12 = phi[0:States,        States:2*States]
+    // Phi22 = phi[States:2*States, States:2*States]
+    final Matrix<States, States> phi12 = phi.block(states, states, 0, states);
+    final Matrix<States, States> phi22 = phi.block(states, states, states, states);
+
+    final var discA = phi22.transpose();
+
+    Q = discA.times(phi12);
+
+    // Make discrete Q symmetric if it isn't already
+    final var discQ = Q.plus(Q.transpose()).div(2.0);
+
+    return new Pair<>(discA, discQ);
+  }
+
+  /**
+   * Discretizes the given continuous A and Q matrices.
+   *
+   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which is expensive),
+   * we take advantage of the structure of the block matrix of A and Q.
+   *
+   * <p>The exponential of A*t, which is only N x N, is relatively cheap. 2) The upper-right quarter
+   * of the 2N x 2N matrix, which we can approximate using a taylor series to several terms and
+   * still be substantially cheaper than taking the big exponential.
+   *
+   * @param <States> Nat representing the number of states.
+   * @param contA Continuous system matrix.
+   * @param contQ Continuous process noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return a pair representing the discrete system matrix and process noise covariance matrix.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static <States extends Num>
+      Pair<Matrix<States, States>, Matrix<States, States>> discretizeAQTaylor(
+          Matrix<States, States> contA, Matrix<States, States> contQ, double dtSeconds) {
+    // Make continuous Q symmetric if it isn't already
+    Matrix<States, States> Q = contQ.plus(contQ.transpose()).div(2.0);
+
+    Matrix<States, States> lastTerm = Q.copy();
+    double lastCoeff = dtSeconds;
+
+    // Aᵀⁿ
+    Matrix<States, States> Atn = contA.transpose();
+    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
+
+    // i = 6 i.e. 5th order should be enough precision
+    for (int i = 2; i < 6; ++i) {
+      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
+      lastCoeff *= dtSeconds / ((double) i);
+
+      phi12 = phi12.plus(lastTerm.times(lastCoeff));
+
+      Atn = Atn.times(contA.transpose());
+    }
+
+    var discA = discretizeA(contA, dtSeconds);
+    Q = discA.times(phi12);
+
+    // Make Q symmetric if it isn't already
+    var discQ = Q.plus(Q.transpose()).div(2.0);
+
+    return new Pair<>(discA, discQ);
+  }
+
+  /**
+   * Returns a discretized version of the provided continuous measurement noise covariance matrix.
+   * Note that dt=0.0 divides R by zero.
+   *
+   * @param <O> Nat representing the number of outputs.
+   * @param R Continuous measurement noise covariance matrix.
+   * @param dtSeconds Discretization timestep.
+   * @return Discretized version of the provided continuous measurement noise covariance matrix.
+   */
+  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
+    return R.div(dtSeconds);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
new file mode 100644
index 0000000..9e35cfd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystem.java
@@ -0,0 +1,201 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearSystem<States extends Num, Inputs extends Num, Outputs extends Num> {
+  /** Continuous system matrix. */
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, States> m_A;
+
+  /** Continuous input matrix. */
+  @SuppressWarnings("MemberName")
+  private final Matrix<States, Inputs> m_B;
+
+  /** Output matrix. */
+  @SuppressWarnings("MemberName")
+  private final Matrix<Outputs, States> m_C;
+
+  /** Feedthrough matrix. */
+  @SuppressWarnings("MemberName")
+  private final Matrix<Outputs, Inputs> m_D;
+
+  /**
+   * Construct a new LinearSystem from the four system matrices.
+   *
+   * @param a The system matrix A.
+   * @param b The input matrix B.
+   * @param c The output matrix C.
+   * @param d The feedthrough matrix D.
+   * @throws IllegalArgumentException if any matrix element isn't finite.
+   */
+  @SuppressWarnings("ParameterName")
+  public LinearSystem(
+      Matrix<States, States> a,
+      Matrix<States, Inputs> b,
+      Matrix<Outputs, States> c,
+      Matrix<Outputs, Inputs> d) {
+    for (int row = 0; row < a.getNumRows(); ++row) {
+      for (int col = 0; col < a.getNumCols(); ++col) {
+        if (!Double.isFinite(a.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of A aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < b.getNumRows(); ++row) {
+      for (int col = 0; col < b.getNumCols(); ++col) {
+        if (!Double.isFinite(b.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of B aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < c.getNumRows(); ++row) {
+      for (int col = 0; col < c.getNumCols(); ++col) {
+        if (!Double.isFinite(c.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of C aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+    for (int row = 0; row < d.getNumRows(); ++row) {
+      for (int col = 0; col < d.getNumCols(); ++col) {
+        if (!Double.isFinite(d.get(row, col))) {
+          throw new IllegalArgumentException(
+              "Elements of D aren't finite. This is usually due to model implementation errors.");
+        }
+      }
+    }
+
+    this.m_A = a;
+    this.m_B = b;
+    this.m_C = c;
+    this.m_D = d;
+  }
+
+  /**
+   * Returns the system matrix A.
+   *
+   * @return the system matrix A.
+   */
+  public Matrix<States, States> getA() {
+    return m_A;
+  }
+
+  /**
+   * Returns an element of the system matrix A.
+   *
+   * @param row Row of A.
+   * @param col Column of A.
+   * @return the system matrix A at (i, j).
+   */
+  public double getA(int row, int col) {
+    return m_A.get(row, col);
+  }
+
+  /**
+   * Returns the input matrix B.
+   *
+   * @return the input matrix B.
+   */
+  public Matrix<States, Inputs> getB() {
+    return m_B;
+  }
+
+  /**
+   * Returns an element of the input matrix B.
+   *
+   * @param row Row of B.
+   * @param col Column of B.
+   * @return The value of the input matrix B at (i, j).
+   */
+  public double getB(int row, int col) {
+    return m_B.get(row, col);
+  }
+
+  /**
+   * Returns the output matrix C.
+   *
+   * @return Output matrix C.
+   */
+  public Matrix<Outputs, States> getC() {
+    return m_C;
+  }
+
+  /**
+   * Returns an element of the output matrix C.
+   *
+   * @param row Row of C.
+   * @param col Column of C.
+   * @return the double value of C at the given position.
+   */
+  public double getC(int row, int col) {
+    return m_C.get(row, col);
+  }
+
+  /**
+   * Returns the feedthrough matrix D.
+   *
+   * @return the feedthrough matrix D.
+   */
+  public Matrix<Outputs, Inputs> getD() {
+    return m_D;
+  }
+
+  /**
+   * Returns an element of the feedthrough matrix D.
+   *
+   * @param row Row of D.
+   * @param col Column of D.
+   * @return The feedthrough matrix D at (i, j).
+   */
+  public double getD(int row, int col) {
+    return m_D.get(row, col);
+  }
+
+  /**
+   * Computes the new x given the old x and the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   * @param dtSeconds Timestep for model update.
+   * @return the updated x.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<States, N1> calculateX(
+      Matrix<States, N1> x, Matrix<Inputs, N1> clampedU, double dtSeconds) {
+    var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
+
+    return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU));
+  }
+
+  /**
+   * Computes the new y given the control input.
+   *
+   * <p>This is used by state observers directly to run updates based on state estimate.
+   *
+   * @param x The current state.
+   * @param clampedU The control input.
+   * @return the updated output matrix Y.
+   */
+  @SuppressWarnings("ParameterName")
+  public Matrix<Outputs, N1> calculateY(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU) {
+    return m_C.times(x).plus(m_D.times(clampedU));
+  }
+
+  @Override
+  public String toString() {
+    return String.format(
+        "Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n",
+        m_A.toString(), m_B.toString(), m_C.toString(), m_D.toString());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
new file mode 100644
index 0000000..8f3da6a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/LinearSystemLoop.java
@@ -0,0 +1,349 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.controller.LinearPlantInversionFeedforward;
+import edu.wpi.first.math.controller.LinearQuadraticRegulator;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.Function;
+import org.ejml.MatrixDimensionException;
+import org.ejml.simple.SimpleMatrix;
+
+/**
+ * Combines a controller, feedforward, and observer for controlling a mechanism with full state
+ * feedback.
+ *
+ * <p>For everything in this file, "inputs" and "outputs" are defined from the perspective of the
+ * plant. This means U is an input and Y is an output (because you give the plant U (powers) and it
+ * gives you back a Y (sensor values). This is the opposite of what they mean from the perspective
+ * of the controller (U is an output because that's what goes to the motors and Y is an input
+ * because that's what comes back from the sensors).
+ *
+ * <p>For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ */
+@SuppressWarnings("ClassTypeParameterName")
+public class LinearSystemLoop<States extends Num, Inputs extends Num, Outputs extends Num> {
+  private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
+  private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
+  private final KalmanFilter<States, Inputs, Outputs> m_observer;
+  private Matrix<States, N1> m_nextR;
+  private Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> m_clampFunction;
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and observer. By default, the
+   * initial reference is all zeros. Users should call reset with the initial system state before
+   * enabling the loop. This constructor assumes that the input(s) to this system are voltage.
+   *
+   * @param plant State-space plant.
+   * @param controller State-space controller.
+   * @param observer State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
+   * @param dtSeconds The nominal timestep.
+   */
+  public LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs> plant,
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      double maxVoltageVolts,
+      double dtSeconds) {
+    this(
+        controller,
+        new LinearPlantInversionFeedforward<>(plant, dtSeconds),
+        observer,
+        u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given plant, controller, and observer. By default, the
+   * initial reference is all zeros. Users should call reset with the initial system state before
+   * enabling the loop.
+   *
+   * @param plant State-space plant.
+   * @param controller State-space controller.
+   * @param observer State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   * @param dtSeconds The nominal timestep.
+   */
+  public LinearSystemLoop(
+      LinearSystem<States, Inputs, Outputs> plant,
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
+      double dtSeconds) {
+    this(
+        controller,
+        new LinearPlantInversionFeedforward<>(plant, dtSeconds),
+        observer,
+        clampFunction);
+  }
+
+  /**
+   * Constructs a state-space loop with the given controller, feedforward and observer. By default,
+   * the initial reference is all zeros. Users should call reset with the initial system state
+   * before enabling the loop.
+   *
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer State-space observer.
+   * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the inputs are
+   *     voltages.
+   */
+  public LinearSystemLoop(
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      double maxVoltageVolts) {
+    this(
+        controller,
+        feedforward,
+        observer,
+        u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
+  }
+
+  /**
+   * Constructs a state-space loop with the given controller, feedforward, and observer. By default,
+   * the initial reference is all zeros. Users should call reset with the initial system state
+   * before enabling the loop.
+   *
+   * @param controller State-space controller.
+   * @param feedforward Plant inversion feedforward.
+   * @param observer State-space observer.
+   * @param clampFunction The function used to clamp the input U.
+   */
+  public LinearSystemLoop(
+      LinearQuadraticRegulator<States, Inputs, Outputs> controller,
+      LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
+      KalmanFilter<States, Inputs, Outputs> observer,
+      Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_controller = controller;
+    this.m_feedforward = feedforward;
+    this.m_observer = observer;
+    this.m_clampFunction = clampFunction;
+
+    m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
+    reset(m_nextR);
+  }
+
+  /**
+   * Returns the observer's state estimate x-hat.
+   *
+   * @return the observer's state estimate x-hat.
+   */
+  public Matrix<States, N1> getXHat() {
+    return getObserver().getXhat();
+  }
+
+  /**
+   * Returns an element of the observer's state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @return the i-th element of the observer's state estimate x-hat.
+   */
+  public double getXHat(int row) {
+    return getObserver().getXhat(row);
+  }
+
+  /**
+   * Set the initial state estimate x-hat.
+   *
+   * @param xhat The initial state estimate x-hat.
+   */
+  public void setXHat(Matrix<States, N1> xhat) {
+    getObserver().setXhat(xhat);
+  }
+
+  /**
+   * Set an element of the initial state estimate x-hat.
+   *
+   * @param row Row of x-hat.
+   * @param value Value for element of x-hat.
+   */
+  public void setXHat(int row, double value) {
+    getObserver().setXhat(row, value);
+  }
+
+  /**
+   * Returns an element of the controller's next reference r.
+   *
+   * @param row Row of r.
+   * @return the element i of the controller's next reference r.
+   */
+  public double getNextR(int row) {
+    return getNextR().get(row, 0);
+  }
+
+  /**
+   * Returns the controller's next reference r.
+   *
+   * @return the controller's next reference r.
+   */
+  public Matrix<States, N1> getNextR() {
+    return m_nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(Matrix<States, N1> nextR) {
+    m_nextR = nextR;
+  }
+
+  /**
+   * Set the next reference r.
+   *
+   * @param nextR Next reference.
+   */
+  public void setNextR(double... nextR) {
+    if (nextR.length != m_nextR.getNumRows()) {
+      throw new MatrixDimensionException(
+          String.format(
+              "The next reference does not have the "
+                  + "correct number of entries! Expected %s, but got %s.",
+              m_nextR.getNumRows(), nextR.length));
+    }
+    m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR));
+  }
+
+  /**
+   * Returns the controller's calculated control input u plus the calculated feedforward u_ff.
+   *
+   * @return the calculated control input u.
+   */
+  public Matrix<Inputs, N1> getU() {
+    return clampInput(m_controller.getU().plus(m_feedforward.getUff()));
+  }
+
+  /**
+   * Returns an element of the controller's calculated control input u.
+   *
+   * @param row Row of u.
+   * @return the calculated control input u at the row i.
+   */
+  public double getU(int row) {
+    return getU().get(row, 0);
+  }
+
+  /**
+   * Return the controller used internally.
+   *
+   * @return the controller used internally.
+   */
+  public LinearQuadraticRegulator<States, Inputs, Outputs> getController() {
+    return m_controller;
+  }
+
+  /**
+   * Return the feedforward used internally.
+   *
+   * @return the feedforward used internally.
+   */
+  public LinearPlantInversionFeedforward<States, Inputs, Outputs> getFeedforward() {
+    return m_feedforward;
+  }
+
+  /**
+   * Return the observer used internally.
+   *
+   * @return the observer used internally.
+   */
+  public KalmanFilter<States, Inputs, Outputs> getObserver() {
+    return m_observer;
+  }
+
+  /**
+   * Zeroes reference r and controller output u. The previous reference of the
+   * PlantInversionFeedforward and the initial state estimate of the KalmanFilter are set to the
+   * initial state provided.
+   *
+   * @param initialState The initial state.
+   */
+  public void reset(Matrix<States, N1> initialState) {
+    m_nextR.fill(0.0);
+    m_controller.reset();
+    m_feedforward.reset(initialState);
+    m_observer.setXhat(initialState);
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @return The state error matrix.
+   */
+  public Matrix<States, N1> getError() {
+    return getController().getR().minus(m_observer.getXhat());
+  }
+
+  /**
+   * Returns difference between reference r and current state x-hat.
+   *
+   * @param index The index of the error matrix to return.
+   * @return The error at that index.
+   */
+  public double getError(int index) {
+    return (getController().getR().minus(m_observer.getXhat())).get(index, 0);
+  }
+
+  /**
+   * Get the function used to clamp the input u.
+   *
+   * @return The clamping function.
+   */
+  public Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> getClampFunction() {
+    return m_clampFunction;
+  }
+
+  /**
+   * Set the clamping function used to clamp inputs.
+   *
+   * @param clampFunction The clamping function.
+   */
+  public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
+    this.m_clampFunction = clampFunction;
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * @param y Measurement vector.
+   */
+  @SuppressWarnings("ParameterName")
+  public void correct(Matrix<Outputs, N1> y) {
+    getObserver().correct(getU(), y);
+  }
+
+  /**
+   * Sets new controller output, projects model forward, and runs observer prediction.
+   *
+   * <p>After calling this, the user should send the elements of u to the actuators.
+   *
+   * @param dtSeconds Timestep for model update.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public void predict(double dtSeconds) {
+    var u =
+        clampInput(
+            m_controller
+                .calculate(getObserver().getXhat(), m_nextR)
+                .plus(m_feedforward.calculate(m_nextR)));
+    getObserver().predict(u, dtSeconds);
+  }
+
+  /**
+   * Clamp the input u to the min and max.
+   *
+   * @param unclampedU The input to clamp.
+   * @return The clamped input.
+   */
+  public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
+    return m_clampFunction.apply(unclampedU);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
new file mode 100644
index 0000000..274b10a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalIntegration.java
@@ -0,0 +1,383 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.BiFunction;
+import java.util.function.DoubleFunction;
+import java.util.function.Function;
+
+public final class NumericalIntegration {
+  private NumericalIntegration() {
+    // utility Class
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f The function to integrate, which takes one argument x.
+   * @param x The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rk4(DoubleFunction<Double> f, double x, double dtSeconds) {
+    final var h = dtSeconds;
+    final var k1 = f.apply(x);
+    final var k2 = f.apply(x + h * k1 * 0.5);
+    final var k3 = f.apply(x + h * k2 * 0.5);
+    final var k4 = f.apply(x + h * k3);
+
+    return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs Runge Kutta integration (4th order).
+   *
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return The result of Runge Kutta integration (4th order).
+   */
+  @SuppressWarnings("ParameterName")
+  public static double rk4(
+      BiFunction<Double, Double, Double> f, double x, Double u, double dtSeconds) {
+    final var h = dtSeconds;
+
+    final var k1 = f.apply(x, u);
+    final var k2 = f.apply(x + h * k1 * 0.5, u);
+    final var k3 = f.apply(x + h * k2 * 0.5, u);
+    final var k4 = f.apply(x + h * k3, u);
+
+    return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rk4(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+    final var h = dtSeconds;
+
+    Matrix<States, N1> k1 = f.apply(x, u);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)), u);
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)), u);
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)), u);
+
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   *
+   * @param <States> A Num prepresenting the states of the system.
+   * @param f The function to integrate. It must take one argument x.
+   * @param x The initial value of x.
+   * @param dtSeconds The time over which to integrate.
+   * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <States extends Num> Matrix<States, N1> rk4(
+      Function<Matrix<States, N1>, Matrix<States, N1>> f, Matrix<States, N1> x, double dtSeconds) {
+    final var h = dtSeconds;
+
+    Matrix<States, N1> k1 = f.apply(x);
+    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(h * 0.5)));
+    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(h * 0.5)));
+    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(h)));
+
+    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+
+  /**
+   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
+   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method. By default, the max
+   * error is 1e-6.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+    return rkf45(f, x, u, dtSeconds, 1e-6);
+  }
+
+  /**
+   * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described in
+   * https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkf45(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds,
+      double maxError) {
+    // See
+    // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+    // for the Butcher tableau the following arrays came from.
+
+    // final double[5][5]
+    final double[][] A = {
+      {1.0 / 4.0},
+      {3.0 / 32.0, 9.0 / 32.0},
+      {1932.0 / 2197.0, -7200.0 / 2197.0, 7296.0 / 2197.0},
+      {439.0 / 216.0, -8.0, 3680.0 / 513.0, -845.0 / 4104.0},
+      {-8.0 / 27.0, 2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}
+    };
+
+    // final double[6]
+    final double[] b1 = {
+      16.0 / 135.0, 0.0, 6656.0 / 12825.0, 28561.0 / 56430.0, -9.0 / 50.0, 2.0 / 55.0
+    };
+
+    // final double[6]
+    final double[] b2 = {25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
+
+    Matrix<States, N1> newX;
+    double truncationError;
+
+    double dtElapsed = 0.0;
+    double h = dtSeconds;
+
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dtSeconds) {
+      do {
+        // Only allow us to advance up to the dt remaining
+        h = Math.min(h, dtSeconds - dtElapsed);
+
+        // Notice how the derivative in the Wikipedia notation is dy/dx.
+        // That means their y is our x and their x is our t
+        var k1 = f.apply(x, u);
+        var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
+        var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
+        var k4 =
+            f.apply(
+                x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
+                u);
+        var k5 =
+            f.apply(
+                x.plus(
+                    k1.times(A[3][0])
+                        .plus(k2.times(A[3][1]))
+                        .plus(k3.times(A[3][2]))
+                        .plus(k4.times(A[3][3]))
+                        .times(h)),
+                u);
+        var k6 =
+            f.apply(
+                x.plus(
+                    k1.times(A[4][0])
+                        .plus(k2.times(A[4][1]))
+                        .plus(k3.times(A[4][2]))
+                        .plus(k4.times(A[4][3]))
+                        .plus(k5.times(A[4][4]))
+                        .times(h)),
+                u);
+
+        newX =
+            x.plus(
+                k1.times(b1[0])
+                    .plus(k2.times(b1[1]))
+                    .plus(k3.times(b1[2]))
+                    .plus(k4.times(b1[3]))
+                    .plus(k5.times(b1[4]))
+                    .plus(k6.times(b1[5]))
+                    .times(h));
+        truncationError =
+            (k1.times(b1[0] - b2[0])
+                    .plus(k2.times(b1[1] - b2[1]))
+                    .plus(k3.times(b1[2] - b2[2]))
+                    .plus(k4.times(b1[3] - b2[3]))
+                    .plus(k5.times(b1[4] - b2[4]))
+                    .plus(k6.times(b1[5] - b2[5]))
+                    .times(h))
+                .normF();
+
+        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+      } while (truncationError > maxError);
+
+      dtElapsed += h;
+      x = newX;
+    }
+
+    return x;
+  }
+
+  /**
+   * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt. By default, the max
+   * error is 1e-6.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds) {
+    return rkdp(f, x, u, dtSeconds, 1e-6);
+  }
+
+  /**
+   * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
+   *
+   * @param <States> A Num representing the states of the system to integrate.
+   * @param <Inputs> A Num representing the inputs of the system to integrate.
+   * @param f The function to integrate. It must take two arguments x and u.
+   * @param x The initial value of x.
+   * @param u The value u held constant over the integration period.
+   * @param dtSeconds The time over which to integrate.
+   * @param maxError The maximum acceptable truncation error. Usually a small number like 1e-6.
+   * @return the integration of dx/dt = f(x, u) for dt.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rkdp(
+      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+      Matrix<States, N1> x,
+      Matrix<Inputs, N1> u,
+      double dtSeconds,
+      double maxError) {
+    // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
+    // Butcher tableau the following arrays came from.
+
+    // final double[6][6]
+    final double[][] A = {
+      {1.0 / 5.0},
+      {3.0 / 40.0, 9.0 / 40.0},
+      {44.0 / 45.0, -56.0 / 15.0, 32.0 / 9.0},
+      {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0},
+      {9017.0 / 3168.0, -355.0 / 33.0, 46732.0 / 5247.0, 49.0 / 176.0, -5103.0 / 18656.0},
+      {35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0}
+    };
+
+    // final double[7]
+    final double[] b1 = {
+      35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0, 11.0 / 84.0, 0.0
+    };
+
+    // final double[7]
+    final double[] b2 = {
+      5179.0 / 57600.0,
+      0.0,
+      7571.0 / 16695.0,
+      393.0 / 640.0,
+      -92097.0 / 339200.0,
+      187.0 / 2100.0,
+      1.0 / 40.0
+    };
+
+    Matrix<States, N1> newX;
+    double truncationError;
+
+    double dtElapsed = 0.0;
+    double h = dtSeconds;
+
+    // Loop until we've gotten to our desired dt
+    while (dtElapsed < dtSeconds) {
+      do {
+        // Only allow us to advance up to the dt remaining
+        h = Math.min(h, dtSeconds - dtElapsed);
+
+        var k1 = f.apply(x, u);
+        var k2 = f.apply(x.plus(k1.times(A[0][0]).times(h)), u);
+        var k3 = f.apply(x.plus(k1.times(A[1][0]).plus(k2.times(A[1][1])).times(h)), u);
+        var k4 =
+            f.apply(
+                x.plus(k1.times(A[2][0]).plus(k2.times(A[2][1])).plus(k3.times(A[2][2])).times(h)),
+                u);
+        var k5 =
+            f.apply(
+                x.plus(
+                    k1.times(A[3][0])
+                        .plus(k2.times(A[3][1]))
+                        .plus(k3.times(A[3][2]))
+                        .plus(k4.times(A[3][3]))
+                        .times(h)),
+                u);
+        var k6 =
+            f.apply(
+                x.plus(
+                    k1.times(A[4][0])
+                        .plus(k2.times(A[4][1]))
+                        .plus(k3.times(A[4][2]))
+                        .plus(k4.times(A[4][3]))
+                        .plus(k5.times(A[4][4]))
+                        .times(h)),
+                u);
+
+        // Since the final row of A and the array b1 have the same coefficients
+        // and k7 has no effect on newX, we can reuse the calculation.
+        newX =
+            x.plus(
+                k1.times(A[5][0])
+                    .plus(k2.times(A[5][1]))
+                    .plus(k3.times(A[5][2]))
+                    .plus(k4.times(A[5][3]))
+                    .plus(k5.times(A[5][4]))
+                    .plus(k6.times(A[5][5]))
+                    .times(h));
+        var k7 = f.apply(newX, u);
+
+        truncationError =
+            (k1.times(b1[0] - b2[0])
+                    .plus(k2.times(b1[1] - b2[1]))
+                    .plus(k3.times(b1[2] - b2[2]))
+                    .plus(k4.times(b1[3] - b2[3]))
+                    .plus(k5.times(b1[4] - b2[4]))
+                    .plus(k6.times(b1[5] - b2[5]))
+                    .plus(k7.times(b1[6] - b2[6]))
+                    .times(h))
+                .normF();
+
+        h *= 0.9 * Math.pow(maxError / truncationError, 1.0 / 5.0);
+      } while (truncationError > maxError);
+
+      dtElapsed += h;
+      x = newX;
+    }
+
+    return x;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
new file mode 100644
index 0000000..6c2c896
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/NumericalJacobian.java
@@ -0,0 +1,104 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.Num;
+import edu.wpi.first.math.numbers.N1;
+import java.util.function.BiFunction;
+import java.util.function.Function;
+
+public final class NumericalJacobian {
+  private NumericalJacobian() {
+    // Utility Class.
+  }
+
+  private static final double kEpsilon = 1e-5;
+
+  /**
+   * Computes the numerical Jacobian with respect to x for f(x).
+   *
+   * @param <Rows> Number of rows in the result of f(x).
+   * @param <States> Num representing the number of rows in the output of f.
+   * @param <Cols> Number of columns in the result of f(x).
+   * @param rows Number of rows in the result of f(x).
+   * @param cols Number of columns in the result of f(x).
+   * @param f Vector-valued function from which to compute the Jacobian.
+   * @param x Vector argument.
+   * @return The numerical Jacobian with respect to x for f(x, u, ...).
+   */
+  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
+  public static <Rows extends Num, Cols extends Num, States extends Num>
+      Matrix<Rows, Cols> numericalJacobian(
+          Nat<Rows> rows,
+          Nat<Cols> cols,
+          Function<Matrix<Cols, N1>, Matrix<States, N1>> f,
+          Matrix<Cols, N1> x) {
+    var result = new Matrix<>(rows, cols);
+
+    for (int i = 0; i < cols.getNum(); i++) {
+      var dxPlus = x.copy();
+      var dxMinus = x.copy();
+      dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
+      dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
+      @SuppressWarnings("LocalVariableName")
+      var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
+
+      result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
+    }
+
+    return result;
+  }
+
+  /**
+   * Returns numerical Jacobian with respect to x for f(x, u, ...).
+   *
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param <States> Number of rows in x.
+   * @param <Inputs> Number of rows in the second input to f.
+   * @param <Outputs> Num representing the rows in the output of f.
+   * @param rows Number of rows in the result of f(x, u).
+   * @param states Number of rows in x.
+   * @param f Vector-valued function from which to compute Jacobian.
+   * @param x State vector.
+   * @param u Input vector.
+   * @return The numerical Jacobian with respect to x for f(x, u, ...).
+   */
+  @SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
+  public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
+      Matrix<Rows, States> numericalJacobianX(
+          Nat<Rows> rows,
+          Nat<States> states,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u) {
+    return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
+  }
+
+  /**
+   * Returns the numerical Jacobian with respect to u for f(x, u).
+   *
+   * @param <States> The states of the system.
+   * @param <Inputs> The inputs to the system.
+   * @param <Rows> Number of rows in the result of f(x, u).
+   * @param rows Number of rows in the result of f(x, u).
+   * @param inputs Number of rows in u.
+   * @param f Vector-valued function from which to compute the Jacobian.
+   * @param x State vector.
+   * @param u Input vector.
+   * @return the numerical Jacobian with respect to u for f(x, u).
+   */
+  @SuppressWarnings({"LambdaParameterName", "MethodTypeParameterName"})
+  public static <Rows extends Num, States extends Num, Inputs extends Num>
+      Matrix<Rows, Inputs> numericalJacobianU(
+          Nat<Rows> rows,
+          Nat<Inputs> inputs,
+          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
+          Matrix<States, N1> x,
+          Matrix<Inputs, N1> u) {
+    return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
new file mode 100644
index 0000000..94c117f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/DCMotor.java
@@ -0,0 +1,207 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system.plant;
+
+import edu.wpi.first.math.util.Units;
+
+/** Holds the constants for a DC motor. */
+public class DCMotor {
+  @SuppressWarnings("MemberName")
+  public final double nominalVoltageVolts;
+
+  @SuppressWarnings("MemberName")
+  public final double stallTorqueNewtonMeters;
+
+  @SuppressWarnings("MemberName")
+  public final double stallCurrentAmps;
+
+  @SuppressWarnings("MemberName")
+  public final double freeCurrentAmps;
+
+  @SuppressWarnings("MemberName")
+  public final double freeSpeedRadPerSec;
+
+  @SuppressWarnings("MemberName")
+  public final double rOhms;
+
+  @SuppressWarnings("MemberName")
+  public final double KvRadPerSecPerVolt;
+
+  @SuppressWarnings("MemberName")
+  public final double KtNMPerAmp;
+
+  /**
+   * Constructs a DC motor.
+   *
+   * @param nominalVoltageVolts Voltage at which the motor constants were measured.
+   * @param stallTorqueNewtonMeters Current draw when stalled.
+   * @param stallCurrentAmps Current draw when stalled.
+   * @param freeCurrentAmps Current draw under no load.
+   * @param freeSpeedRadPerSec Angular velocity under no load.
+   * @param numMotors Number of motors in a gearbox.
+   */
+  public DCMotor(
+      double nominalVoltageVolts,
+      double stallTorqueNewtonMeters,
+      double stallCurrentAmps,
+      double freeCurrentAmps,
+      double freeSpeedRadPerSec,
+      int numMotors) {
+    this.nominalVoltageVolts = nominalVoltageVolts;
+    this.stallTorqueNewtonMeters = stallTorqueNewtonMeters * numMotors;
+    this.stallCurrentAmps = stallCurrentAmps * numMotors;
+    this.freeCurrentAmps = freeCurrentAmps * numMotors;
+    this.freeSpeedRadPerSec = freeSpeedRadPerSec;
+
+    this.rOhms = nominalVoltageVolts / this.stallCurrentAmps;
+    this.KvRadPerSecPerVolt =
+        freeSpeedRadPerSec / (nominalVoltageVolts - rOhms * this.freeCurrentAmps);
+    this.KtNMPerAmp = this.stallTorqueNewtonMeters / this.stallCurrentAmps;
+  }
+
+  /**
+   * Estimate the current being drawn by this motor.
+   *
+   * @param speedRadiansPerSec The speed of the rotor.
+   * @param voltageInputVolts The input voltage.
+   * @return The estimated current.
+   */
+  public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
+    return -1.0 / KvRadPerSecPerVolt / rOhms * speedRadiansPerSec + 1.0 / rOhms * voltageInputVolts;
+  }
+
+  /**
+   * Return a gearbox of CIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of CIM motors.
+   */
+  public static DCMotor getCIM(int numMotors) {
+    return new DCMotor(
+        12, 2.42, 133, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310), numMotors);
+  }
+
+  /**
+   * Return a gearbox of 775Pro motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of 775Pro motors.
+   */
+  public static DCMotor getVex775Pro(int numMotors) {
+    return new DCMotor(
+        12, 0.71, 134, 0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730), numMotors);
+  }
+
+  /**
+   * Return a gearbox of NEO motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of NEO motors.
+   */
+  public static DCMotor getNEO(int numMotors) {
+    return new DCMotor(
+        12, 2.6, 105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676), numMotors);
+  }
+
+  /**
+   * Return a gearbox of MiniCIM motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of MiniCIM motors.
+   */
+  public static DCMotor getMiniCIM(int numMotors) {
+    return new DCMotor(
+        12, 1.41, 89, 3, Units.rotationsPerMinuteToRadiansPerSecond(5840), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Bag motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Bag motors.
+   */
+  public static DCMotor getBag(int numMotors) {
+    return new DCMotor(
+        12, 0.43, 53, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(13180), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark RS775-125 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Andymark RS775-125 motors.
+   */
+  public static DCMotor getAndymarkRs775_125(int numMotors) {
+    return new DCMotor(
+        12, 0.28, 18, 1.6, Units.rotationsPerMinuteToRadiansPerSecond(5800.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS775 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Banebots RS775 motors.
+   */
+  public static DCMotor getBanebotsRs775(int numMotors) {
+    return new DCMotor(
+        12, 0.72, 97, 2.7, Units.rotationsPerMinuteToRadiansPerSecond(13050.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Andymark 9015 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Andymark 9015 motors.
+   */
+  public static DCMotor getAndymark9015(int numMotors) {
+    return new DCMotor(
+        12, 0.36, 71, 3.7, Units.rotationsPerMinuteToRadiansPerSecond(14270.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Banebots RS 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Banebots RS 550 motors.
+   */
+  public static DCMotor getBanebotsRs550(int numMotors) {
+    return new DCMotor(
+        12, 0.38, 84, 0.4, Units.rotationsPerMinuteToRadiansPerSecond(19000.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of NEO 550 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of NEO 550 motors.
+   */
+  public static DCMotor getNeo550(int numMotors) {
+    return new DCMotor(
+        12, 0.97, 100, 1.4, Units.rotationsPerMinuteToRadiansPerSecond(11000.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Falcon 500 motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Falcon 500 motors.
+   */
+  public static DCMotor getFalcon500(int numMotors) {
+    return new DCMotor(
+        12, 4.69, 257, 1.5, Units.rotationsPerMinuteToRadiansPerSecond(6380.0), numMotors);
+  }
+
+  /**
+   * Return a gearbox of Romi/TI_RSLK MAX motors.
+   *
+   * @param numMotors Number of motors in the gearbox.
+   * @return A gearbox of Romi/TI_RSLK MAX motors.
+   */
+  public static DCMotor getRomiBuiltIn(int numMotors) {
+    // From https://www.pololu.com/product/1520/specs
+    return new DCMotor(
+        4.5, 0.1765, 1.25, 0.13, Units.rotationsPerMinuteToRadiansPerSecond(150.0), numMotors);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
new file mode 100644
index 0000000..2933e93
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/system/plant/LinearSystemId.java
@@ -0,0 +1,331 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system.plant;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+
+public final class LinearSystemId {
+  private LinearSystemId() {
+    // Utility class
+  }
+
+  /**
+   * Create a state-space model of an elevator system. The states of the system are [position,
+   * velocity]ᵀ, inputs are [voltage], and outputs are [position].
+   *
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @param massKg The mass of the elevator carriage, in kilograms.
+   * @param radiusMeters The radius of thd driving drum of the elevator, in meters.
+   * @param G The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if massKg &lt;= 0, radiusMeters &lt;= 0, or G &lt;= 0.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> createElevatorSystem(
+      DCMotor motor, double massKg, double radiusMeters, double G) {
+    if (massKg <= 0.0) {
+      throw new IllegalArgumentException("massKg must be greater than zero.");
+    }
+    if (radiusMeters <= 0.0) {
+      throw new IllegalArgumentException("radiusMeters must be greater than zero.");
+    }
+    if (G <= 0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2())
+            .fill(
+                0,
+                1,
+                0,
+                -Math.pow(G, 2)
+                    * motor.KtNMPerAmp
+                    / (motor.rOhms
+                        * radiusMeters
+                        * radiusMeters
+                        * massKg
+                        * motor.KvRadPerSecPerVolt)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * radiusMeters * massKg)),
+        Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+        new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a flywheel system. The states of the system are [angular
+   * velocity], inputs are [voltage], and outputs are [angular velocity].
+   *
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @param jKgMetersSquared The moment of inertia J of the flywheel.
+   * @param G The reduction between motor and drum, as a ratio of output to input.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if jKgMetersSquared &lt;= 0 or G &lt;= 0.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N1, N1, N1> createFlywheelSystem(
+      DCMotor motor, double jKgMetersSquared, double G) {
+    if (jKgMetersSquared <= 0.0) {
+      throw new IllegalArgumentException("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        VecBuilder.fill(
+            -G
+                * G
+                * motor.KtNMPerAmp
+                / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgMetersSquared)),
+        VecBuilder.fill(G * motor.KtNMPerAmp / (motor.rOhms * jKgMetersSquared)),
+        Matrix.eye(Nat.N1()),
+        new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Create a state-space model of a differential drive drivetrain. In this model, the states are
+   * [v_left, v_right]ᵀ, inputs are [V_left, V_right]ᵀ and outputs are [v_left, v_right]ᵀ.
+   *
+   * @param motor the gearbox representing the motors driving the drivetrain.
+   * @param massKg the mass of the robot.
+   * @param rMeters the radius of the wheels in meters.
+   * @param rbMeters the radius of the base (half the track width) in meters.
+   * @param JKgMetersSquared the moment of inertia of the robot.
+   * @param G the gearing reduction as output over input.
+   * @return A LinearSystem representing a differential drivetrain.
+   * @throws IllegalArgumentException if m &lt;= 0, r &lt;= 0, rb &lt;= 0, J &lt;= 0, or G &lt;= 0.
+   */
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(
+      DCMotor motor,
+      double massKg,
+      double rMeters,
+      double rbMeters,
+      double JKgMetersSquared,
+      double G) {
+    if (massKg <= 0.0) {
+      throw new IllegalArgumentException("massKg must be greater than zero.");
+    }
+    if (rMeters <= 0.0) {
+      throw new IllegalArgumentException("rMeters must be greater than zero.");
+    }
+    if (rbMeters <= 0.0) {
+      throw new IllegalArgumentException("rbMeters must be greater than zero.");
+    }
+    if (JKgMetersSquared <= 0.0) {
+      throw new IllegalArgumentException("JKgMetersSquared must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
+    var C1 =
+        -(G * G) * motor.KtNMPerAmp / (motor.KvRadPerSecPerVolt * motor.rOhms * rMeters * rMeters);
+    var C2 = G * motor.KtNMPerAmp / (motor.rOhms * rMeters);
+
+    final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
+    final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
+    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C1, C4 * C1, C4 * C1, C3 * C1);
+    var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(C3 * C2, C4 * C2, C4 * C2, C3 * C2);
+    var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
+    var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0);
+
+    return new LinearSystem<>(A, B, C, D);
+  }
+
+  /**
+   * Create a state-space model of a single jointed arm system. The states of the system are [angle,
+   * angular velocity], inputs are [voltage], and outputs are [angle].
+   *
+   * @param motor The motor (or gearbox) attached to the arm.
+   * @param jKgSquaredMeters The moment of inertia J of the arm.
+   * @param G The gearing between the motor and arm, in output over input. Most of the time this
+   *     will be greater than 1.
+   * @return A LinearSystem representing the given characterized constants.
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(
+      DCMotor motor, double jKgSquaredMeters, double G) {
+    if (jKgSquaredMeters <= 0.0) {
+      throw new IllegalArgumentException("jKgSquaredMeters must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw new IllegalArgumentException("G must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2())
+            .fill(
+                0,
+                1,
+                0,
+                -Math.pow(G, 2)
+                    * motor.KtNMPerAmp
+                    / (motor.KvRadPerSecPerVolt * motor.rOhms * jKgSquaredMeters)),
+        VecBuilder.fill(0, G * motor.KtNMPerAmp / (motor.rOhms * jKgSquaredMeters)),
+        Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
+        new Matrix<>(Nat.N1(), Nat.N1()));
+  }
+
+  /**
+   * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
+   * constants cam be found using SysId. The states of the system are [velocity], inputs are
+   * [voltage], and outputs are [velocity].
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * @param kV The velocity gain, in volts per (units per second)
+   * @param kA The acceleration gain, in volts per (units per second squared)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
+    if (kV <= 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        VecBuilder.fill(-kV / kA),
+        VecBuilder.fill(1.0 / kA),
+        VecBuilder.fill(1.0),
+        VecBuilder.fill(0.0));
+  }
+
+  /**
+   * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2). These
+   * constants cam be found using SysId. The states of the system are [position, velocity]ᵀ, inputs
+   * are [voltage], and outputs are [position].
+   *
+   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use the
+   * {@link edu.wpi.first.math.util.Units} class for converting between unit types.
+   *
+   * @param kV The velocity gain, in volts per (units per second)
+   * @param kA The acceleration gain, in volts per (units per second squared)
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kV &lt;= 0 or kA &lt;= 0.
+   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
+    if (kV <= 0.0) {
+      throw new IllegalArgumentException("Kv must be greater than zero.");
+    }
+    if (kA <= 0.0) {
+      throw new IllegalArgumentException("Ka must be greater than zero.");
+    }
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
+        VecBuilder.fill(0.0, 1.0 / kA),
+        Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
+        VecBuilder.fill(0.0));
+  }
+
+  /**
+   * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
+   * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(meter/sec) and
+   * volts/(meter/sec^2)) cases. This can be found using SysId. The states of the system are [left
+   * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
+   * velocity, right velocity]ᵀ.
+   *
+   * @param kVLinear The linear velocity gain, volts per (meter per second).
+   * @param kALinear The linear acceleration gain, volts per (meter per second squared).
+   * @param kVAngular The angular velocity gain, volts per (meter per second).
+   * @param kAAngular The angular acceleration gain, volts per (meter per second squared).
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0, or
+   *     kAAngular &lt;= 0.
+   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
+    if (kVLinear <= 0.0) {
+      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
+    }
+    if (kALinear <= 0.0) {
+      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
+    }
+    if (kVAngular <= 0.0) {
+      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
+    }
+    if (kAAngular <= 0.0) {
+      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
+    }
+
+    final double A1 = 0.5 * -(kVLinear / kALinear + kVAngular / kAAngular);
+    final double A2 = 0.5 * -(kVLinear / kALinear - kVAngular / kAAngular);
+    final double B1 = 0.5 * (1.0 / kALinear + 1.0 / kAAngular);
+    final double B2 = 0.5 * (1.0 / kALinear - 1.0 / kAAngular);
+
+    return new LinearSystem<>(
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
+        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
+  }
+
+  /**
+   * Identify a standard differential drive drivetrain, given the drivetrain's kV and kA in both
+   * linear (volts/(meter/sec) and volts/(meter/sec^2)) and angular (volts/(radian/sec) and
+   * volts/(radian/sec^2)) cases. This can be found using SysId. The states of the system are [left
+   * velocity, right velocity]ᵀ, inputs are [left voltage, right voltage]ᵀ, and outputs are [left
+   * velocity, right velocity]ᵀ.
+   *
+   * @param kVLinear The linear velocity gain, volts per (meter per second).
+   * @param kALinear The linear acceleration gain, volts per (meter per second squared).
+   * @param kVAngular The angular velocity gain, volts per (radians per second).
+   * @param kAAngular The angular acceleration gain, volts per (radians per second squared).
+   * @param trackwidth The width of the drivetrain in meters.
+   * @return A LinearSystem representing the given characterized constants.
+   * @throws IllegalArgumentException if kVLinear &lt;= 0, kALinear &lt;= 0, kVAngular &lt;= 0,
+   *     kAAngular &lt;= 0, or trackwidth &lt;= 0.
+   * @see <a href="https://github.com/wpilibsuite/sysid">https://github.com/wpilibsuite/sysid</a>
+   */
+  @SuppressWarnings("ParameterName")
+  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
+      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
+    if (kVLinear <= 0.0) {
+      throw new IllegalArgumentException("Kv,linear must be greater than zero.");
+    }
+    if (kALinear <= 0.0) {
+      throw new IllegalArgumentException("Ka,linear must be greater than zero.");
+    }
+    if (kVAngular <= 0.0) {
+      throw new IllegalArgumentException("Kv,angular must be greater than zero.");
+    }
+    if (kAAngular <= 0.0) {
+      throw new IllegalArgumentException("Ka,angular must be greater than zero.");
+    }
+    if (trackwidth <= 0.0) {
+      throw new IllegalArgumentException("trackwidth must be greater than zero.");
+    }
+
+    // We want to find a factor to include in Kv,angular that will convert
+    // `u = Kv,angular omega` to `u = Kv,angular v`.
+    //
+    // v = omega r
+    // omega = v/r
+    // omega = 1/r v
+    // omega = 1/(trackwidth/2) v
+    // omega = 2/trackwidth v
+    //
+    // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
+    // to V/m/s).
+    return identifyDrivetrainSystem(
+        kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
new file mode 100644
index 0000000..2ec4244
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/Trajectory.java
@@ -0,0 +1,409 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.stream.Collectors;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of various States that
+ * represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
+ */
+public class Trajectory {
+  private final double m_totalTimeSeconds;
+  private final List<State> m_states;
+
+  /** Constructs an empty trajectory. */
+  public Trajectory() {
+    m_states = new ArrayList<>();
+    m_totalTimeSeconds = 0.0;
+  }
+
+  /**
+   * Constructs a trajectory from a vector of states.
+   *
+   * @param states A vector of states.
+   */
+  public Trajectory(final List<State> states) {
+    m_states = states;
+    m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
+  }
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("ParameterName")
+  private static double lerp(double startValue, double endValue, double t) {
+    return startValue + (endValue - startValue) * t;
+  }
+
+  /**
+   * Linearly interpolates between two poses.
+   *
+   * @param startValue The start pose.
+   * @param endValue The end pose.
+   * @param t The fraction for interpolation.
+   * @return The interpolated pose.
+   */
+  @SuppressWarnings("ParameterName")
+  private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
+    return startValue.plus((endValue.minus(startValue)).times(t));
+  }
+
+  /**
+   * Returns the initial pose of the trajectory.
+   *
+   * @return The initial pose of the trajectory.
+   */
+  public Pose2d getInitialPose() {
+    return sample(0).poseMeters;
+  }
+
+  /**
+   * Returns the overall duration of the trajectory.
+   *
+   * @return The duration of the trajectory.
+   */
+  public double getTotalTimeSeconds() {
+    return m_totalTimeSeconds;
+  }
+
+  /**
+   * Return the states of the trajectory.
+   *
+   * @return The states of the trajectory.
+   */
+  public List<State> getStates() {
+    return m_states;
+  }
+
+  /**
+   * Sample the trajectory at a point in time.
+   *
+   * @param timeSeconds The point in time since the beginning of the trajectory to sample.
+   * @return The state at that point in time.
+   */
+  public State sample(double timeSeconds) {
+    if (timeSeconds <= m_states.get(0).timeSeconds) {
+      return m_states.get(0);
+    }
+    if (timeSeconds >= m_totalTimeSeconds) {
+      return m_states.get(m_states.size() - 1);
+    }
+
+    // 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.get(mid).timeSeconds < timeSeconds) {
+        // 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.
+    final State sample = m_states.get(low);
+    final State prevSample = m_states.get(low - 1);
+
+    // If the difference in states is negligible, then we are spot on!
+    if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
+      return sample;
+    }
+    // Interpolate between the two states for the state that we want.
+    return prevSample.interpolate(
+        sample,
+        (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
+  }
+
+  /**
+   * 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.
+   */
+  public Trajectory transformBy(Transform2d transform) {
+    var firstState = m_states.get(0);
+    var firstPose = firstState.poseMeters;
+
+    // Calculate the transformed first pose.
+    var newFirstPose = firstPose.plus(transform);
+    List<State> newStates = new ArrayList<>();
+
+    newStates.add(
+        new State(
+            firstState.timeSeconds,
+            firstState.velocityMetersPerSecond,
+            firstState.accelerationMetersPerSecondSq,
+            newFirstPose,
+            firstState.curvatureRadPerMeter));
+
+    for (int i = 1; i < m_states.size(); i++) {
+      var state = m_states.get(i);
+      // We are transforming relative to the coordinate frame of the new initial pose.
+      newStates.add(
+          new State(
+              state.timeSeconds,
+              state.velocityMetersPerSecond,
+              state.accelerationMetersPerSecondSq,
+              newFirstPose.plus(state.poseMeters.minus(firstPose)),
+              state.curvatureRadPerMeter));
+    }
+
+    return new Trajectory(newStates);
+  }
+
+  /**
+   * 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.
+   */
+  public Trajectory relativeTo(Pose2d pose) {
+    return new Trajectory(
+        m_states.stream()
+            .map(
+                state ->
+                    new State(
+                        state.timeSeconds,
+                        state.velocityMetersPerSecond,
+                        state.accelerationMetersPerSecondSq,
+                        state.poseMeters.relativeTo(pose),
+                        state.curvatureRadPerMeter))
+            .collect(Collectors.toList()));
+  }
+
+  /**
+   * Concatenates another trajectory to the current trajectory. The user is responsible for making
+   * sure that the end pose of this trajectory and the start pose of the other trajectory match (if
+   * that is the desired behavior).
+   *
+   * @param other The trajectory to concatenate.
+   * @return The concatenated trajectory.
+   */
+  public Trajectory concatenate(Trajectory other) {
+    // If this is a default constructed trajectory with no states, then we can
+    // simply return the rhs trajectory.
+    if (m_states.isEmpty()) {
+      return other;
+    }
+
+    // Deep copy the current states.
+    List<State> states =
+        m_states.stream()
+            .map(
+                state ->
+                    new State(
+                        state.timeSeconds,
+                        state.velocityMetersPerSecond,
+                        state.accelerationMetersPerSecondSq,
+                        state.poseMeters,
+                        state.curvatureRadPerMeter))
+            .collect(Collectors.toList());
+
+    // Here we omit the first state of the other trajectory because we don't want
+    // two time points with different states. Sample() will automatically
+    // interpolate between the end of this trajectory and the second state of the
+    // other trajectory.
+    for (int i = 1; i < other.getStates().size(); ++i) {
+      var s = other.getStates().get(i);
+      states.add(
+          new State(
+              s.timeSeconds + m_totalTimeSeconds,
+              s.velocityMetersPerSecond,
+              s.accelerationMetersPerSecondSq,
+              s.poseMeters,
+              s.curvatureRadPerMeter));
+    }
+    return new Trajectory(states);
+  }
+
+  /**
+   * Represents a time-parameterized trajectory. The trajectory contains of various States that
+   * represent the pose, curvature, time elapsed, velocity, and acceleration at that point.
+   */
+  @SuppressWarnings("MemberName")
+  public static class State {
+    // The time elapsed since the beginning of the trajectory.
+    @JsonProperty("time")
+    public double timeSeconds;
+
+    // The speed at that point of the trajectory.
+    @JsonProperty("velocity")
+    public double velocityMetersPerSecond;
+
+    // The acceleration at that point of the trajectory.
+    @JsonProperty("acceleration")
+    public double accelerationMetersPerSecondSq;
+
+    // The pose at that point of the trajectory.
+    @JsonProperty("pose")
+    public Pose2d poseMeters;
+
+    // The curvature at that point of the trajectory.
+    @JsonProperty("curvature")
+    public double curvatureRadPerMeter;
+
+    public State() {
+      poseMeters = new Pose2d();
+    }
+
+    /**
+     * Constructs a State with the specified parameters.
+     *
+     * @param timeSeconds The time elapsed since the beginning of the trajectory.
+     * @param velocityMetersPerSecond The speed at that point of the trajectory.
+     * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
+     * @param poseMeters The pose at that point of the trajectory.
+     * @param curvatureRadPerMeter The curvature at that point of the trajectory.
+     */
+    public State(
+        double timeSeconds,
+        double velocityMetersPerSecond,
+        double accelerationMetersPerSecondSq,
+        Pose2d poseMeters,
+        double curvatureRadPerMeter) {
+      this.timeSeconds = timeSeconds;
+      this.velocityMetersPerSecond = velocityMetersPerSecond;
+      this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
+      this.poseMeters = poseMeters;
+      this.curvatureRadPerMeter = curvatureRadPerMeter;
+    }
+
+    /**
+     * Interpolates between two States.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i The interpolant (fraction).
+     * @return The interpolated state.
+     */
+    @SuppressWarnings("ParameterName")
+    State interpolate(State endValue, double i) {
+      // Find the new t value.
+      final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
+
+      // Find the delta time between the current state and the interpolated state.
+      final double deltaT = newT - timeSeconds;
+
+      // If delta time is negative, flip the order of interpolation.
+      if (deltaT < 0) {
+        return endValue.interpolate(this, 1 - i);
+      }
+
+      // Check whether the robot is reversing at this stage.
+      final boolean reversing =
+          velocityMetersPerSecond < 0
+              || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
+
+      // Calculate the new velocity
+      // v_f = v_0 + at
+      final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
+
+      // Calculate the change in position.
+      // delta_s = v_0 t + 0.5 at^2
+      final double newS =
+          (velocityMetersPerSecond * deltaT
+                  + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2))
+              * (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.
+      final double interpolationFrac =
+          newS / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
+
+      return new State(
+          newT,
+          newV,
+          accelerationMetersPerSecondSq,
+          lerp(poseMeters, endValue.poseMeters, interpolationFrac),
+          lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac));
+    }
+
+    @Override
+    public String toString() {
+      return String.format(
+          "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
+          timeSeconds,
+          velocityMetersPerSecond,
+          accelerationMetersPerSecondSq,
+          poseMeters,
+          curvatureRadPerMeter);
+    }
+
+    @Override
+    public boolean equals(Object obj) {
+      if (this == obj) {
+        return true;
+      }
+      if (!(obj instanceof State)) {
+        return false;
+      }
+      State state = (State) obj;
+      return Double.compare(state.timeSeconds, timeSeconds) == 0
+          && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
+          && Double.compare(state.accelerationMetersPerSecondSq, accelerationMetersPerSecondSq) == 0
+          && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
+          && Objects.equals(poseMeters, state.poseMeters);
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(
+          timeSeconds,
+          velocityMetersPerSecond,
+          accelerationMetersPerSecondSq,
+          poseMeters,
+          curvatureRadPerMeter);
+    }
+  }
+
+  @Override
+  public String toString() {
+    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
+    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
+  }
+
+  @Override
+  public int hashCode() {
+    return m_states.hashCode();
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    return obj instanceof Trajectory && m_states.equals(((Trajectory) obj).getStates());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java
new file mode 100644
index 0000000..fbf734f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryConfig.java
@@ -0,0 +1,190 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.math.trajectory.constraint.MecanumDriveKinematicsConstraint;
+import edu.wpi.first.math.trajectory.constraint.SwerveDriveKinematicsConstraint;
+import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * 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.
+ *
+ * <p>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.
+ */
+public class TrajectoryConfig {
+  private final double m_maxVelocity;
+  private final double m_maxAcceleration;
+  private final List<TrajectoryConstraint> m_constraints;
+  private double m_startVelocity;
+  private double m_endVelocity;
+  private boolean m_reversed;
+
+  /**
+   * Constructs the trajectory configuration class.
+   *
+   * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   */
+  public TrajectoryConfig(
+      double maxVelocityMetersPerSecond, double maxAccelerationMetersPerSecondSq) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
+    m_constraints = new ArrayList<>();
+  }
+
+  /**
+   * Adds a user-defined constraint to the trajectory.
+   *
+   * @param constraint The user-defined constraint.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
+    m_constraints.add(constraint);
+    return this;
+  }
+
+  /**
+   * Adds all user-defined constraints from a list to the trajectory.
+   *
+   * @param constraints List of user-defined constraints.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
+    m_constraints.addAll(constraints);
+    return this;
+  }
+
+  /**
+   * 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.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
+    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+   * 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.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
+    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+   * 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.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
+    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
+    return this;
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   *
+   * @return The starting velocity of the trajectory.
+   */
+  public double getStartVelocity() {
+    return m_startVelocity;
+  }
+
+  /**
+   * Sets the start velocity of the trajectory.
+   *
+   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
+    m_startVelocity = startVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the starting velocity of the trajectory.
+   *
+   * @return The starting velocity of the trajectory.
+   */
+  public double getEndVelocity() {
+    return m_endVelocity;
+  }
+
+  /**
+   * Sets the end velocity of the trajectory.
+   *
+   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
+    m_endVelocity = endVelocityMetersPerSecond;
+    return this;
+  }
+
+  /**
+   * Returns the maximum velocity of the trajectory.
+   *
+   * @return The maximum velocity of the trajectory.
+   */
+  public double getMaxVelocity() {
+    return m_maxVelocity;
+  }
+
+  /**
+   * Returns the maximum acceleration of the trajectory.
+   *
+   * @return The maximum acceleration of the trajectory.
+   */
+  public double getMaxAcceleration() {
+    return m_maxAcceleration;
+  }
+
+  /**
+   * Returns the user-defined constraints of the trajectory.
+   *
+   * @return The user-defined constraints of the trajectory.
+   */
+  public List<TrajectoryConstraint> getConstraints() {
+    return m_constraints;
+  }
+
+  /**
+   * Returns whether the trajectory is reversed or not.
+   *
+   * @return whether the trajectory is reversed or not.
+   */
+  public boolean isReversed() {
+    return m_reversed;
+  }
+
+  /**
+   * Sets the reversed flag of the trajectory.
+   *
+   * @param reversed Whether the trajectory should be reversed or not.
+   * @return Instance of the current config object.
+   */
+  public TrajectoryConfig setReversed(boolean reversed) {
+    m_reversed = reversed;
+    return this;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
new file mode 100644
index 0000000..0827c15
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryGenerator.java
@@ -0,0 +1,280 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.spline.PoseWithCurvature;
+import edu.wpi.first.math.spline.Spline;
+import edu.wpi.first.math.spline.SplineHelper;
+import edu.wpi.first.math.spline.SplineParameterizer;
+import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collection;
+import java.util.List;
+import java.util.function.BiConsumer;
+
+public final class TrajectoryGenerator {
+  private static final Trajectory kDoNothingTrajectory =
+      new Trajectory(Arrays.asList(new Trajectory.State()));
+  private static BiConsumer<String, StackTraceElement[]> errorFunc;
+
+  /** Private constructor because this is a utility class. */
+  private TrajectoryGenerator() {}
+
+  private static void reportError(String error, StackTraceElement[] stackTrace) {
+    if (errorFunc != null) {
+      errorFunc.accept(error, stackTrace);
+    } else {
+      MathSharedStore.reportError(error, stackTrace);
+    }
+  }
+
+  /**
+   * Set error reporting function. By default, DriverStation.reportError() is used.
+   *
+   * @param func Error reporting function, arguments are error and stackTrace.
+   */
+  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
+    errorFunc = func;
+  }
+
+  /**
+   * 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.
+   */
+  public static Trajectory generateTrajectory(
+      Spline.ControlVector initial,
+      List<Translation2d> interiorWaypoints,
+      Spline.ControlVector end,
+      TrajectoryConfig config) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    // Clone the control vectors.
+    var newInitial = new Spline.ControlVector(initial.x, initial.y);
+    var newEnd = new Spline.ControlVector(end.x, end.y);
+
+    // Change the orientation if reversed.
+    if (config.isReversed()) {
+      newInitial.x[1] *= -1;
+      newInitial.y[1] *= -1;
+      newEnd.x[1] *= -1;
+      newEnd.y[1] *= -1;
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points =
+          splinePointsFromSplines(
+              SplineHelper.getCubicSplinesFromControlVectors(
+                  newInitial, interiorWaypoints.toArray(new Translation2d[0]), newEnd));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(
+        points,
+        config.getConstraints(),
+        config.getStartVelocity(),
+        config.getEndVelocity(),
+        config.getMaxVelocity(),
+        config.getMaxAcceleration(),
+        config.isReversed());
+  }
+
+  /**
+   * 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.
+   */
+  public static Trajectory generateTrajectory(
+      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end, TrajectoryConfig config) {
+    var controlVectors =
+        SplineHelper.getCubicControlVectorsFromWaypoints(
+            start, interiorWaypoints.toArray(new Translation2d[0]), end);
+
+    // Return the generated trajectory.
+    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], 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.
+   */
+  public static Trajectory generateTrajectory(
+      ControlVectorList controlVectors, TrajectoryConfig config) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
+
+    // Create a new control vector list, flipping the orientation if reversed.
+    for (final var vector : controlVectors) {
+      var newVector = new Spline.ControlVector(vector.x, vector.y);
+      if (config.isReversed()) {
+        newVector.x[1] *= -1;
+        newVector.y[1] *= -1;
+      }
+      newControlVectors.add(newVector);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points =
+          splinePointsFromSplines(
+              SplineHelper.getQuinticSplinesFromControlVectors(
+                  newControlVectors.toArray(new Spline.ControlVector[] {})));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(
+        points,
+        config.getConstraints(),
+        config.getStartVelocity(),
+        config.getEndVelocity(),
+        config.getMaxVelocity(),
+        config.getMaxAcceleration(),
+        config.isReversed());
+  }
+
+  /**
+   * 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.
+   */
+  @SuppressWarnings("LocalVariableName")
+  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
+    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
+
+    List<Pose2d> newWaypoints = new ArrayList<>();
+    if (config.isReversed()) {
+      for (Pose2d originalWaypoint : waypoints) {
+        newWaypoints.add(originalWaypoint.plus(flip));
+      }
+    } else {
+      newWaypoints.addAll(waypoints);
+    }
+
+    // Get the spline points
+    List<PoseWithCurvature> points;
+    try {
+      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
+    } catch (MalformedSplineException ex) {
+      reportError(ex.getMessage(), ex.getStackTrace());
+      return kDoNothingTrajectory;
+    }
+
+    // Change the points back to their original orientation.
+    if (config.isReversed()) {
+      for (var point : points) {
+        point.poseMeters = point.poseMeters.plus(flip);
+        point.curvatureRadPerMeter *= -1;
+      }
+    }
+
+    // Generate and return trajectory.
+    return TrajectoryParameterizer.timeParameterizeTrajectory(
+        points,
+        config.getConstraints(),
+        config.getStartVelocity(),
+        config.getEndVelocity(),
+        config.getMaxVelocity(),
+        config.getMaxAcceleration(),
+        config.isReversed());
+  }
+
+  /**
+   * 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.
+   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
+   *     with approximately opposing headings)
+   */
+  public static List<PoseWithCurvature> splinePointsFromSplines(Spline[] splines) {
+    // Create the vector of spline points.
+    var splinePoints = new ArrayList<PoseWithCurvature>();
+
+    // Add the first point to the vector.
+    splinePoints.add(splines[0].getPoint(0.0));
+
+    // Iterate through the vector and parameterize each spline, adding the
+    // parameterized points to the final vector.
+    for (final var spline : splines) {
+      var 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.addAll(points.subList(1, points.size()));
+    }
+    return splinePoints;
+  }
+
+  // Work around type erasure signatures
+  @SuppressWarnings("serial")
+  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
+    public ControlVectorList(int initialCapacity) {
+      super(initialCapacity);
+    }
+
+    public ControlVectorList() {
+      super();
+    }
+
+    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
+      super(collection);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
new file mode 100644
index 0000000..d7fbf59
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryParameterizer.java
@@ -0,0 +1,329 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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.
+ */
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.spline.PoseWithCurvature;
+import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
+import java.util.ArrayList;
+import java.util.List;
+
+/** Class used to parameterize a trajectory by time. */
+public final class TrajectoryParameterizer {
+  /** Private constructor because this is a utility class. */
+  private TrajectoryParameterizer() {}
+
+  /**
+   * Parameterize the trajectory by time. This is where the velocity profile is generated.
+   *
+   * <p>The derivation of the algorithm used can be found <a
+   * href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">here</a>.
+   *
+   * @param points Reference to the spline points.
+   * @param constraints A vector of various velocity and acceleration. constraints.
+   * @param startVelocityMetersPerSecond The start velocity for the trajectory.
+   * @param endVelocityMetersPerSecond The end velocity for the trajectory.
+   * @param maxVelocityMetersPerSecond The max velocity for the trajectory.
+   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
+   * @param reversed Whether the robot should move backwards. Note that the robot will still move
+   *     from a -&gt; b -&gt; ... -&gt; z as defined in the waypoints.
+   * @return The trajectory.
+   */
+  public static Trajectory timeParameterizeTrajectory(
+      List<PoseWithCurvature> points,
+      List<TrajectoryConstraint> constraints,
+      double startVelocityMetersPerSecond,
+      double endVelocityMetersPerSecond,
+      double maxVelocityMetersPerSecond,
+      double maxAccelerationMetersPerSecondSq,
+      boolean reversed) {
+    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
+    var predecessor =
+        new ConstrainedState(
+            points.get(0),
+            0,
+            startVelocityMetersPerSecond,
+            -maxAccelerationMetersPerSecondSq,
+            maxAccelerationMetersPerSecondSq);
+
+    // Forward pass
+    for (int i = 0; i < points.size(); i++) {
+      constrainedStates.add(new ConstrainedState());
+      var constrainedState = constrainedStates.get(i);
+      constrainedState.pose = points.get(i);
+
+      // Begin constraining based on predecessor.
+      double ds =
+          constrainedState
+              .pose
+              .poseMeters
+              .getTranslation()
+              .getDistance(predecessor.pose.poseMeters.getTranslation());
+      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
+
+      // 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.maxVelocityMetersPerSecond =
+            Math.min(
+                maxVelocityMetersPerSecond,
+                Math.sqrt(
+                    predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond
+                        + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0));
+
+        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
+        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+
+        // At this point, the constrained state is fully constructed apart from
+        // all the custom-defined user constraints.
+        for (final var constraint : constraints) {
+          constrainedState.maxVelocityMetersPerSecond =
+              Math.min(
+                  constrainedState.maxVelocityMetersPerSecond,
+                  constraint.getMaxVelocityMetersPerSecond(
+                      constrainedState.pose.poseMeters,
+                      constrainedState.pose.curvatureRadPerMeter,
+                      constrainedState.maxVelocityMetersPerSecond));
+        }
+
+        // Now enforce all acceleration limits.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds < 1E-6) {
+          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.
+        double actualAcceleration =
+            (constrainedState.maxVelocityMetersPerSecond
+                        * constrainedState.maxVelocityMetersPerSecond
+                    - predecessor.maxVelocityMetersPerSecond
+                        * predecessor.maxVelocityMetersPerSecond)
+                / (ds * 2.0);
+
+        // If we violate the max acceleration constraint, let's modify the
+        // predecessor.
+        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
+          predecessor.maxAccelerationMetersPerSecondSq =
+              constrainedState.maxAccelerationMetersPerSecondSq;
+        } else {
+          // Constrain the predecessor's max acceleration to the current
+          // acceleration.
+          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
+            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
+          }
+          // If the actual acceleration is less than the predecessor's min
+          // acceleration, it will be repaired in the backward pass.
+          break;
+        }
+      }
+      predecessor = constrainedState;
+    }
+
+    var successor =
+        new ConstrainedState(
+            points.get(points.size() - 1),
+            constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
+            endVelocityMetersPerSecond,
+            -maxAccelerationMetersPerSecondSq,
+            maxAccelerationMetersPerSecondSq);
+
+    // Backward pass
+    for (int i = points.size() - 1; i >= 0; i--) {
+      var constrainedState = constrainedStates.get(i);
+      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
+
+      while (true) {
+        // Enforce max velocity limit (reverse)
+        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+        double newMaxVelocity =
+            Math.sqrt(
+                successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
+                    + successor.minAccelerationMetersPerSecondSq * ds * 2.0);
+
+        // No more limits to impose! This state can be finalized.
+        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
+          break;
+        }
+
+        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
+
+        // Check all acceleration constraints with the new max velocity.
+        enforceAccelerationLimits(reversed, constraints, constrainedState);
+
+        if (ds > -1E-6) {
+          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.
+        double actualAcceleration =
+            (constrainedState.maxVelocityMetersPerSecond
+                        * constrainedState.maxVelocityMetersPerSecond
+                    - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
+                / (ds * 2.0);
+
+        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
+          successor.minAccelerationMetersPerSecondSq =
+              constrainedState.minAccelerationMetersPerSecondSq;
+        } else {
+          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
+          break;
+        }
+      }
+      successor = constrainedState;
+    }
+
+    // Now we can integrate the constrained states forward in time to obtain our
+    // trajectory states.
+    var states = new ArrayList<Trajectory.State>(points.size());
+    double timeSeconds = 0.0;
+    double distanceMeters = 0.0;
+    double velocityMetersPerSecond = 0.0;
+
+    for (int i = 0; i < constrainedStates.size(); i++) {
+      final var state = constrainedStates.get(i);
+
+      // Calculate the change in position between the current state and the previous
+      // state.
+      double ds = state.distanceMeters - distanceMeters;
+
+      // Calculate the acceleration between the current state and the previous
+      // state.
+      double accel =
+          (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
+                  - velocityMetersPerSecond * velocityMetersPerSecond)
+              / (ds * 2);
+
+      // Calculate dt
+      double dt = 0.0;
+      if (i > 0) {
+        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
+        if (Math.abs(accel) > 1E-6) {
+          // v_f = v_0 + a * t
+          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
+        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
+          // delta_x = v * t
+          dt = ds / velocityMetersPerSecond;
+        } else {
+          throw new TrajectoryGenerationException(
+              "Something went wrong at iteration " + i + " of time parameterization.");
+        }
+      }
+
+      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
+      distanceMeters = state.distanceMeters;
+
+      timeSeconds += dt;
+
+      states.add(
+          new Trajectory.State(
+              timeSeconds,
+              reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
+              reversed ? -accel : accel,
+              state.pose.poseMeters,
+              state.pose.curvatureRadPerMeter));
+    }
+
+    return new Trajectory(states);
+  }
+
+  private static void enforceAccelerationLimits(
+      boolean reverse, List<TrajectoryConstraint> constraints, ConstrainedState state) {
+    for (final var constraint : constraints) {
+      double factor = reverse ? -1.0 : 1.0;
+      final var minMaxAccel =
+          constraint.getMinMaxAccelerationMetersPerSecondSq(
+              state.pose.poseMeters,
+              state.pose.curvatureRadPerMeter,
+              state.maxVelocityMetersPerSecond * factor);
+
+      if (minMaxAccel.minAccelerationMetersPerSecondSq
+          > minMaxAccel.maxAccelerationMetersPerSecondSq) {
+        throw new TrajectoryGenerationException(
+            "The constraint's min acceleration "
+                + "was greater than its max acceleration.\n Offending Constraint: "
+                + constraint.getClass().getName()
+                + "\n If the offending constraint was packaged with WPILib, please file a bug"
+                + " report.");
+      }
+
+      state.minAccelerationMetersPerSecondSq =
+          Math.max(
+              state.minAccelerationMetersPerSecondSq,
+              reverse
+                  ? -minMaxAccel.maxAccelerationMetersPerSecondSq
+                  : minMaxAccel.minAccelerationMetersPerSecondSq);
+
+      state.maxAccelerationMetersPerSecondSq =
+          Math.min(
+              state.maxAccelerationMetersPerSecondSq,
+              reverse
+                  ? -minMaxAccel.minAccelerationMetersPerSecondSq
+                  : minMaxAccel.maxAccelerationMetersPerSecondSq);
+    }
+  }
+
+  @SuppressWarnings("MemberName")
+  private static class ConstrainedState {
+    PoseWithCurvature pose;
+    double distanceMeters;
+    double maxVelocityMetersPerSecond;
+    double minAccelerationMetersPerSecondSq;
+    double maxAccelerationMetersPerSecondSq;
+
+    ConstrainedState(
+        PoseWithCurvature pose,
+        double distanceMeters,
+        double maxVelocityMetersPerSecond,
+        double minAccelerationMetersPerSecondSq,
+        double maxAccelerationMetersPerSecondSq) {
+      this.pose = pose;
+      this.distanceMeters = distanceMeters;
+      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    ConstrainedState() {
+      pose = new PoseWithCurvature();
+    }
+  }
+
+  @SuppressWarnings("serial")
+  public static class TrajectoryGenerationException extends RuntimeException {
+    public TrajectoryGenerationException(String message) {
+      super(message);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
new file mode 100644
index 0000000..5fb2c34
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrajectoryUtil.java
@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.List;
+
+public final class TrajectoryUtil {
+  private TrajectoryUtil() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Creates a trajectory from a double[] of elements.
+   *
+   * @param elements A double[] containing the raw elements of the trajectory.
+   * @return A trajectory created from the elements.
+   */
+  private static Trajectory createTrajectoryFromElements(double[] elements) {
+    // Make sure that the elements have the correct length.
+    if (elements.length % 7 != 0) {
+      throw new TrajectorySerializationException(
+          "An error occurred when converting trajectory elements into a trajectory.");
+    }
+
+    // Create a list of states from the elements.
+    List<Trajectory.State> states = new ArrayList<>();
+    for (int i = 0; i < elements.length; i += 7) {
+      states.add(
+          new Trajectory.State(
+              elements[i],
+              elements[i + 1],
+              elements[i + 2],
+              new Pose2d(elements[i + 3], elements[i + 4], new Rotation2d(elements[i + 5])),
+              elements[i + 6]));
+    }
+    return new Trajectory(states);
+  }
+
+  /**
+   * Returns a double[] of elements from the given trajectory.
+   *
+   * @param trajectory The trajectory to retrieve raw elements from.
+   * @return A double[] of elements from the given trajectory.
+   */
+  private static double[] getElementsFromTrajectory(Trajectory trajectory) {
+    // Create a double[] of elements and fill it from the trajectory states.
+    double[] elements = new double[trajectory.getStates().size() * 7];
+
+    for (int i = 0; i < trajectory.getStates().size() * 7; i += 7) {
+      var state = trajectory.getStates().get(i / 7);
+      elements[i] = state.timeSeconds;
+      elements[i + 1] = state.velocityMetersPerSecond;
+      elements[i + 2] = state.accelerationMetersPerSecondSq;
+      elements[i + 3] = state.poseMeters.getX();
+      elements[i + 4] = state.poseMeters.getY();
+      elements[i + 5] = state.poseMeters.getRotation().getRadians();
+      elements[i + 6] = state.curvatureRadPerMeter;
+    }
+    return elements;
+  }
+
+  /**
+   * 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.
+   * @throws IOException if reading from the file fails.
+   */
+  public static Trajectory fromPathweaverJson(Path path) throws IOException {
+    return createTrajectoryFromElements(WPIMathJNI.fromPathweaverJson(path.toString()));
+  }
+
+  /**
+   * 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
+   * @throws IOException if writing to the file fails.
+   */
+  public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
+    WPIMathJNI.toPathweaverJson(getElementsFromTrajectory(trajectory), path.toString());
+  }
+
+  /**
+   * Deserializes a Trajectory from PathWeaver-style JSON.
+   *
+   * @param json The string containing the serialized JSON
+   * @return the trajectory represented by the JSON
+   * @throws TrajectorySerializationException if deserialization of the string fails.
+   */
+  public static Trajectory deserializeTrajectory(String json) {
+    return createTrajectoryFromElements(WPIMathJNI.deserializeTrajectory(json));
+  }
+
+  /**
+   * Serializes a Trajectory to PathWeaver-style JSON.
+   *
+   * @param trajectory The trajectory to export
+   * @return The string containing the serialized JSON
+   * @throws TrajectorySerializationException if serialization of the trajectory fails.
+   */
+  public static String serializeTrajectory(Trajectory trajectory) {
+    return WPIMathJNI.serializeTrajectory(getElementsFromTrajectory(trajectory));
+  }
+
+  public static class TrajectorySerializationException extends RuntimeException {
+    public TrajectorySerializationException(String message) {
+      super(message);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
new file mode 100644
index 0000000..35745b5
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/TrapezoidProfile.java
@@ -0,0 +1,305 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import edu.wpi.first.math.MathSharedStore;
+import edu.wpi.first.math.MathUsageId;
+import java.util.Objects;
+
+/**
+ * A trapezoid-shaped velocity profile.
+ *
+ * <p>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.
+ *
+ * <p>Initialization:
+ *
+ * <pre><code>
+ * TrapezoidProfile.Constraints constraints =
+ *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
+ * TrapezoidProfile.State previousProfiledReference =
+ *   new TrapezoidProfile.State(initialReference, 0.0);
+ * </code></pre>
+ *
+ * <p>Run on update:
+ *
+ * <pre><code>
+ * TrapezoidProfile profile =
+ *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
+ * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
+ * </code></pre>
+ *
+ * <p>where `unprofiledReference` is free to change between calls. Note that when the unprofiled
+ * reference is within the constraints, `calculate()` returns the unprofiled reference unchanged.
+ *
+ * <p>Otherwise, a timer can be started to provide monotonic values for `calculate()` and to
+ * determine when the profile has completed via `isFinished()`.
+ */
+public class TrapezoidProfile {
+  // The direction of the profile, either 1 for forwards or -1 for inverted
+  private int m_direction;
+
+  private Constraints m_constraints;
+  private State m_initial;
+  private State m_goal;
+
+  private double m_endAccel;
+  private double m_endFullSpeed;
+  private double m_endDeccel;
+
+  public static class Constraints {
+    @SuppressWarnings("MemberName")
+    public final double maxVelocity;
+
+    @SuppressWarnings("MemberName")
+    public final double maxAcceleration;
+
+    /**
+     * Construct constraints for a TrapezoidProfile.
+     *
+     * @param maxVelocity maximum velocity
+     * @param maxAcceleration maximum acceleration
+     */
+    public Constraints(double maxVelocity, double maxAcceleration) {
+      this.maxVelocity = maxVelocity;
+      this.maxAcceleration = maxAcceleration;
+      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
+    }
+  }
+
+  public static class State {
+    @SuppressWarnings("MemberName")
+    public double position;
+
+    @SuppressWarnings("MemberName")
+    public double velocity;
+
+    public State() {}
+
+    public State(double position, double velocity) {
+      this.position = position;
+      this.velocity = velocity;
+    }
+
+    @Override
+    public boolean equals(Object other) {
+      if (other instanceof State) {
+        State rhs = (State) other;
+        return this.position == rhs.position && this.velocity == rhs.velocity;
+      } else {
+        return false;
+      }
+    }
+
+    @Override
+    public int hashCode() {
+      return Objects.hash(position, velocity);
+    }
+  }
+
+  /**
+   * 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).
+   */
+  public 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
+    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
+    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
+
+    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
+    double 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
+
+    double fullTrapezoidDist =
+        cutoffDistBegin + (m_goal.position - m_initial.position) + cutoffDistEnd;
+    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
+
+    double fullSpeedDist =
+        fullTrapezoidDist - accelerationTime * accelerationTime * m_constraints.maxAcceleration;
+
+    // Handle the case where the profile never reaches full speed
+    if (fullSpeedDist < 0) {
+      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
+      fullSpeedDist = 0;
+    }
+
+    m_endAccel = accelerationTime - cutoffBegin;
+    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
+    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
+  }
+
+  /**
+   * Construct a TrapezoidProfile.
+   *
+   * @param constraints The constraints on the profile, like maximum velocity.
+   * @param goal The desired state when the profile is complete.
+   */
+  public TrapezoidProfile(Constraints constraints, State goal) {
+    this(constraints, goal, new State(0, 0));
+  }
+
+  /**
+   * 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.
+   * @return The position and velocity of the profile at time t.
+   */
+  public State calculate(double t) {
+    State result = new State(m_initial.position, m_initial.velocity);
+
+    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;
+      double 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);
+  }
+
+  /**
+   * Returns the time left until a target distance in the profile is reached.
+   *
+   * @param target The target distance.
+   * @return The time left until a target distance in the profile is reached.
+   */
+  public double timeLeftUntil(double target) {
+    double position = m_initial.position * m_direction;
+    double velocity = m_initial.velocity * m_direction;
+
+    double endAccel = m_endAccel * m_direction;
+    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
+
+    if (target < position) {
+      endAccel = -endAccel;
+      endFullSpeed = -endFullSpeed;
+      velocity = -velocity;
+    }
+
+    endAccel = Math.max(endAccel, 0);
+    endFullSpeed = Math.max(endFullSpeed, 0);
+    double endDeccel = m_endDeccel - endAccel - endFullSpeed;
+    endDeccel = Math.max(endDeccel, 0);
+
+    final double acceleration = m_constraints.maxAcceleration;
+    final double decceleration = -m_constraints.maxAcceleration;
+
+    double distToTarget = Math.abs(target - position);
+    if (distToTarget < 1e-6) {
+      return 0;
+    }
+
+    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
+
+    double deccelVelocity;
+    if (endAccel > 0) {
+      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
+    } else {
+      deccelVelocity = velocity;
+    }
+
+    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
+
+    deccelDist = Math.max(deccelDist, 0);
+
+    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
+
+    if (accelDist > distToTarget) {
+      accelDist = distToTarget;
+      fullSpeedDist = 0;
+      deccelDist = 0;
+    } else if (accelDist + fullSpeedDist > distToTarget) {
+      fullSpeedDist = distToTarget - accelDist;
+      deccelDist = 0;
+    } else {
+      deccelDist = distToTarget - fullSpeedDist - accelDist;
+    }
+
+    double accelTime =
+        (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist)))
+            / acceleration;
+
+    double deccelTime =
+        (-deccelVelocity
+                + Math.sqrt(
+                    Math.abs(deccelVelocity * deccelVelocity + 2 * decceleration * deccelDist)))
+            / decceleration;
+
+    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
+
+    return accelTime + fullSpeedTime + deccelTime;
+  }
+
+  /**
+   * Returns the total time the profile takes to reach the goal.
+   *
+   * @return The total time the profile takes to reach the goal.
+   */
+  public double totalTime() {
+    return m_endDeccel;
+  }
+
+  /**
+   * Returns true if the profile has reached the goal.
+   *
+   * <p>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.
+   * @return True if the profile has reached the goal.
+   */
+  public boolean isFinished(double t) {
+    return t >= totalTime();
+  }
+
+  /**
+   * Returns true if the profile inverted.
+   *
+   * <p>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.
+   */
+  private static boolean shouldFlipAcceleration(State initial, State goal) {
+    return initial.position > goal.position;
+  }
+
+  // Flip the sign of the velocity and position if the profile is inverted
+  private State direct(State in) {
+    State result = new State(in.position, in.velocity);
+    result.position = result.position * m_direction;
+    result.velocity = result.velocity * m_direction;
+    return result;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
new file mode 100644
index 0000000..13138e8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/CentripetalAccelerationConstraint.java
@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * 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.
+ *
+ * <p>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.
+ */
+public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
+  private final double m_maxCentripetalAccelerationMetersPerSecondSq;
+
+  /**
+   * Constructs a centripetal acceleration constraint.
+   *
+   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
+   */
+  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
+    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // ac = v^2 / r
+    // k (curvature) = 1 / r
+
+    // therefore, ac = v^2 * k
+    // ac / k = v^2
+    // v = std::sqrt(ac / k)
+
+    return Math.sqrt(
+        m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // The acceleration of the robot has no impact on the centripetal acceleration
+    // of the robot.
+    return new MinMax();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
new file mode 100644
index 0000000..37d8d68
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
@@ -0,0 +1,71 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+
+/**
+ * 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.
+ */
+public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final DifferentialDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a differential drive dynamics constraint.
+   *
+   * @param kinematics A kinematics component describing the drive geometry.
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public DifferentialDriveKinematicsConstraint(
+      final DifferentialDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds =
+        new ChassisSpeeds(
+            velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Return the new linear chassis speed.
+    return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
new file mode 100644
index 0000000..4c7e814
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/DifferentialDriveVoltageConstraint.java
@@ -0,0 +1,129 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+
+/**
+ * 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.
+ */
+public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
+  private final SimpleMotorFeedforward m_feedforward;
+  private final DifferentialDriveKinematics m_kinematics;
+  private final double m_maxVoltage;
+
+  /**
+   * 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.
+   */
+  public DifferentialDriveVoltageConstraint(
+      SimpleMotorFeedforward feedforward,
+      DifferentialDriveKinematics kinematics,
+      double maxVoltage) {
+    m_feedforward =
+        requireNonNullParam(feedforward, "feedforward", "DifferentialDriveVoltageConstraint");
+    m_kinematics =
+        requireNonNullParam(kinematics, "kinematics", "DifferentialDriveVoltageConstraint");
+    m_maxVoltage = maxVoltage;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return Double.POSITIVE_INFINITY;
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    var wheelSpeeds =
+        m_kinematics.toWheelSpeeds(
+            new ChassisSpeeds(
+                velocityMetersPerSecond, 0, velocityMetersPerSecond * curvatureRadPerMeter));
+
+    double maxWheelSpeed =
+        Math.max(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
+    double minWheelSpeed =
+        Math.min(wheelSpeeds.leftMetersPerSecond, wheelSpeeds.rightMetersPerSecond);
+
+    // Calculate maximum/minimum possible accelerations from motor dynamics
+    // and max/min wheel speeds
+    double maxWheelAcceleration =
+        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+    double 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
+
+    // When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
+    // turning on its center) - we have to treat this as a special case, as it breaks
+    // the signum function.  Both max and min acceleration are *reduced in magnitude*
+    // in this case.
+
+    double maxChassisAcceleration;
+    double minChassisAcceleration;
+
+    if (velocityMetersPerSecond == 0) {
+      maxChassisAcceleration =
+          maxWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
+      minChassisAcceleration =
+          minWheelAcceleration
+              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
+    } else {
+      maxChassisAcceleration =
+          maxWheelAcceleration
+              / (1
+                  + m_kinematics.trackWidthMeters
+                      * Math.abs(curvatureRadPerMeter)
+                      * Math.signum(velocityMetersPerSecond)
+                      / 2);
+      minChassisAcceleration =
+          minWheelAcceleration
+              / (1
+                  - m_kinematics.trackWidthMeters
+                      * Math.abs(curvatureRadPerMeter)
+                      * Math.signum(velocityMetersPerSecond)
+                      / 2);
+    }
+
+    // When turning about a point inside of the wheelbase (i.e. radius less than half
+    // the trackwidth), the inner wheel's direction changes, but the magnitude remains
+    // the same.  The formula above changes sign for the inner wheel when this happens.
+    // We can accurately account for this by simply negating the inner wheel.
+
+    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
+      if (velocityMetersPerSecond > 0) {
+        minChassisAcceleration = -minChassisAcceleration;
+      } else if (velocityMetersPerSecond < 0) {
+        maxChassisAcceleration = -maxChassisAcceleration;
+      }
+    }
+
+    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
new file mode 100644
index 0000000..c3bd226
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/EllipticalRegionConstraint.java
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/** Enforces a particular constraint only within an elliptical region. */
+public class EllipticalRegionConstraint implements TrajectoryConstraint {
+  private final Translation2d m_center;
+  private final Translation2d m_radii;
+  private final TrajectoryConstraint m_constraint;
+
+  /**
+   * Constructs a new EllipticalRegionConstraint.
+   *
+   * @param center The center of the ellipse in which to enforce the constraint.
+   * @param xWidth The width of the ellipse in which to enforce the constraint.
+   * @param yWidth The height of the ellipse in which to enforce the constraint.
+   * @param rotation The rotation to apply to all radii around the origin.
+   * @param constraint The constraint to enforce when the robot is within the region.
+   */
+  @SuppressWarnings("ParameterName")
+  public EllipticalRegionConstraint(
+      Translation2d center,
+      double xWidth,
+      double yWidth,
+      Rotation2d rotation,
+      TrajectoryConstraint constraint) {
+    m_center = center;
+    m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
+    m_constraint = constraint;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMaxVelocityMetersPerSecond(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return Double.POSITIVE_INFINITY;
+    }
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return new MinMax();
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the constraint is enforced
+   * in.
+   *
+   * @param robotPose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  public boolean isPoseInRegion(Pose2d robotPose) {
+    // The region (disk) bounded by the ellipse is given by the equation:
+    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+    // If the inequality is satisfied, then it is inside the ellipse; otherwise
+    // it is outside the ellipse.
+    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
+    return Math.pow(robotPose.getX() - m_center.getX(), 2) * Math.pow(m_radii.getY(), 2)
+            + Math.pow(robotPose.getY() - m_center.getY(), 2) * Math.pow(m_radii.getX(), 2)
+        <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java
new file mode 100644
index 0000000..d672295
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MaxVelocityConstraint.java
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * Represents a constraint that enforces a max velocity. This can be composed with the {@link
+ * EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce a max velocity in a
+ * region.
+ */
+public class MaxVelocityConstraint implements TrajectoryConstraint {
+  private final double m_maxVelocity;
+
+  /**
+   * Constructs a new MaxVelocityConstraint.
+   *
+   * @param maxVelocityMetersPerSecond The max velocity.
+   */
+  public MaxVelocityConstraint(double maxVelocityMetersPerSecond) {
+    m_maxVelocity = maxVelocityMetersPerSecond;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return m_maxVelocity;
+  }
+
+  @Override
+  public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
new file mode 100644
index 0000000..b26cdcf
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/MecanumDriveKinematicsConstraint.java
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+
+/**
+ * A class that enforces constraints on the mecanum drive kinematics. This can be used to ensure
+ * that the trajectory is constructed so that the commanded velocities for all 4 wheels of the
+ * drivetrain stay below a certain limit.
+ */
+public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final MecanumDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a mecanum drive kinematics constraint.
+   *
+   * @param kinematics Mecanum drive kinematics.
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public MecanumDriveKinematicsConstraint(
+      final MecanumDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds =
+        new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java
new file mode 100644
index 0000000..b29df5e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/RectangularRegionConstraint.java
@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+
+/** Enforces a particular constraint only within a rectangular region. */
+public class RectangularRegionConstraint implements TrajectoryConstraint {
+  private final Translation2d m_bottomLeftPoint;
+  private final Translation2d m_topRightPoint;
+  private final TrajectoryConstraint m_constraint;
+
+  /**
+   * Constructs a new RectangularRegionConstraint.
+   *
+   * @param bottomLeftPoint The bottom left point of the rectangular region in which to enforce the
+   *     constraint.
+   * @param topRightPoint The top right point of the rectangular region in which to enforce the
+   *     constraint.
+   * @param constraint The constraint to enforce when the robot is within the region.
+   */
+  public RectangularRegionConstraint(
+      Translation2d bottomLeftPoint, Translation2d topRightPoint, TrajectoryConstraint constraint) {
+    m_bottomLeftPoint = bottomLeftPoint;
+    m_topRightPoint = topRightPoint;
+    m_constraint = constraint;
+  }
+
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMaxVelocityMetersPerSecond(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return Double.POSITIVE_INFINITY;
+    }
+  }
+
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    if (isPoseInRegion(poseMeters)) {
+      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(
+          poseMeters, curvatureRadPerMeter, velocityMetersPerSecond);
+    } else {
+      return new MinMax();
+    }
+  }
+
+  /**
+   * Returns whether the specified robot pose is within the region that the constraint is enforced
+   * in.
+   *
+   * @param robotPose The robot pose.
+   * @return Whether the robot pose is within the constraint region.
+   */
+  public boolean isPoseInRegion(Pose2d robotPose) {
+    return robotPose.getX() >= m_bottomLeftPoint.getX()
+        && robotPose.getX() <= m_topRightPoint.getX()
+        && robotPose.getY() >= m_bottomLeftPoint.getY()
+        && robotPose.getY() <= m_topRightPoint.getY();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
new file mode 100644
index 0000000..5d95290
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/SwerveDriveKinematicsConstraint.java
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+
+/**
+ * A class that enforces constraints on the swerve drive kinematics. This can be used to ensure that
+ * the trajectory is constructed so that the commanded velocities for all 4 wheels of the drivetrain
+ * stay below a certain limit.
+ */
+public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
+  private final double m_maxSpeedMetersPerSecond;
+  private final SwerveDriveKinematics m_kinematics;
+
+  /**
+   * Constructs a swerve drive kinematics constraint.
+   *
+   * @param kinematics Swerve drive kinematics.
+   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
+   */
+  public SwerveDriveKinematicsConstraint(
+      final SwerveDriveKinematics kinematics, double maxSpeedMetersPerSecond) {
+    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
+    m_kinematics = kinematics;
+  }
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  @Override
+  public double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    // Represents the velocity of the chassis in the x direction
+    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
+
+    // Represents the velocity of the chassis in the y direction
+    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
+
+    // Create an object to represent the current chassis speeds.
+    var chassisSpeeds =
+        new ChassisSpeeds(xdVelocity, ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
+
+    // Get the wheel speeds and normalize them to within the max velocity.
+    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
+    SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
+
+    // Convert normalized wheel speeds back to chassis speeds
+    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    // Return the new linear chassis speed.
+    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
+  }
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  @Override
+  public MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
+    return new MinMax();
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
new file mode 100644
index 0000000..bbf30f7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/trajectory/constraint/TrajectoryConstraint.java
@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory.constraint;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+/**
+ * An interface for defining user-defined velocity and acceleration constraints while generating
+ * trajectories.
+ */
+public interface TrajectoryConstraint {
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
+   *     constraints are applied.
+   * @return The absolute maximum velocity.
+   */
+  double getMaxVelocityMetersPerSecond(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
+   * curvature, and speed.
+   *
+   * @param poseMeters The pose at the current point in the trajectory.
+   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
+   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
+   * @return The min and max acceleration bounds.
+   */
+  MinMax getMinMaxAccelerationMetersPerSecondSq(
+      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond);
+
+  /** Represents a minimum and maximum acceleration. */
+  @SuppressWarnings("MemberName")
+  class MinMax {
+    public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
+    public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
+
+    /**
+     * Constructs a MinMax.
+     *
+     * @param minAccelerationMetersPerSecondSq The minimum acceleration.
+     * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
+     */
+    public MinMax(
+        double minAccelerationMetersPerSecondSq, double maxAccelerationMetersPerSecondSq) {
+      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
+      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
+    }
+
+    /** Constructs a MinMax with default values. */
+    public MinMax() {}
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/util/Units.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
new file mode 100644
index 0000000..13adcd8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/math/util/Units.java
@@ -0,0 +1,139 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.util;
+
+/** Utility class that converts between commonly used units in FRC. */
+public final class Units {
+  private static final double kInchesPerFoot = 12.0;
+  private static final double kMetersPerInch = 0.0254;
+  private static final double kSecondsPerMinute = 60;
+  private static final double kMillisecondsPerSecond = 1000;
+  private static final double kKilogramsPerLb = 0.453592;
+
+  /** Utility class, so constructor is private. */
+  private Units() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Converts given meters to feet.
+   *
+   * @param meters The meters to convert to feet.
+   * @return Feet converted from meters.
+   */
+  public static double metersToFeet(double meters) {
+    return metersToInches(meters) / kInchesPerFoot;
+  }
+
+  /**
+   * Converts given feet to meters.
+   *
+   * @param feet The feet to convert to meters.
+   * @return Meters converted from feet.
+   */
+  public static double feetToMeters(double feet) {
+    return inchesToMeters(feet * kInchesPerFoot);
+  }
+
+  /**
+   * Converts given meters to inches.
+   *
+   * @param meters The meters to convert to inches.
+   * @return Inches converted from meters.
+   */
+  public static double metersToInches(double meters) {
+    return meters / kMetersPerInch;
+  }
+
+  /**
+   * Converts given inches to meters.
+   *
+   * @param inches The inches to convert to meters.
+   * @return Meters converted from inches.
+   */
+  public static double inchesToMeters(double inches) {
+    return inches * kMetersPerInch;
+  }
+
+  /**
+   * Converts given degrees to radians.
+   *
+   * @param degrees The degrees to convert to radians.
+   * @return Radians converted from degrees.
+   */
+  public static double degreesToRadians(double degrees) {
+    return Math.toRadians(degrees);
+  }
+
+  /**
+   * Converts given radians to degrees.
+   *
+   * @param radians The radians to convert to degrees.
+   * @return Degrees converted from radians.
+   */
+  public static double radiansToDegrees(double radians) {
+    return Math.toDegrees(radians);
+  }
+
+  /**
+   * Converts rotations per minute to radians per second.
+   *
+   * @param rpm The rotations per minute to convert to radians per second.
+   * @return Radians per second converted from rotations per minute.
+   */
+  public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
+    return rpm * Math.PI / (kSecondsPerMinute / 2);
+  }
+
+  /**
+   * Converts radians per second to rotations per minute.
+   *
+   * @param radiansPerSecond The radians per second to convert to from rotations per minute.
+   * @return Rotations per minute converted from radians per second.
+   */
+  public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
+    return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
+  }
+
+  /**
+   * Converts given milliseconds to seconds.
+   *
+   * @param milliseconds The milliseconds to convert to seconds.
+   * @return Seconds converted from milliseconds.
+   */
+  public static double millisecondsToSeconds(double milliseconds) {
+    return milliseconds / kMillisecondsPerSecond;
+  }
+
+  /**
+   * Converts given seconds to milliseconds.
+   *
+   * @param seconds The seconds to convert to milliseconds.
+   * @return Milliseconds converted from seconds.
+   */
+  public static double secondsToMilliseconds(double seconds) {
+    return seconds * kMillisecondsPerSecond;
+  }
+
+  /**
+   * Converts kilograms into lbs (pound-mass).
+   *
+   * @param kilograms The kilograms to convert to lbs (pound-mass).
+   * @return Lbs (pound-mass) converted from kilograms.
+   */
+  public static double kilogramsToLbs(double kilograms) {
+    return kilograms / kKilogramsPerLb;
+  }
+
+  /**
+   * Converts lbs (pound-mass) into kilograms.
+   *
+   * @param lbs The lbs (pound-mass) to convert to kilograms.
+   * @return Kilograms converted from lbs (pound-mass).
+   */
+  public static double lbsToKilograms(double lbs) {
+    return lbs * kKilogramsPerLb;
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
deleted file mode 100644
index 10897e8..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/LinearFilter.java
+++ /dev/null
@@ -1,171 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.Arrays;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.wpiutil.CircularBuffer;
-
-/**
- * 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.
- *
- * <p>Filters are of the form: 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])
- *
- * <p>Where: y[n] is the output at time "n" x[n] is the input at time "n" y[n-1] is the output from
- * the LAST time step ("n-1") x[n-1] is the input from the LAST time step ("n-1") b0...bP are the
- * "feedforward" (FIR) gains a0...aQ are the "feedback" (IIR) gains IMPORTANT! Note the "-" sign in
- * front of the feedback term! This is a common convention in signal processing.
- *
- * <p>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.
- *
- * <p>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!
- *
- * <p>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>
- *
- * <p>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.
- *
- * <p>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!
- */
-public class LinearFilter {
-  private final CircularBuffer m_inputs;
-  private final CircularBuffer m_outputs;
-  private final double[] m_inputGains;
-  private final double[] m_outputGains;
-
-  private static int instances;
-
-  /**
-   * Create a linear FIR or IIR filter.
-   *
-   * @param ffGains The "feed forward" or FIR gains.
-   * @param fbGains The "feed back" or IIR gains.
-   */
-  public LinearFilter(double[] ffGains, double[] fbGains) {
-    m_inputs = new CircularBuffer(ffGains.length);
-    m_outputs = new CircularBuffer(fbGains.length);
-    m_inputGains = Arrays.copyOf(ffGains, ffGains.length);
-    m_outputGains = Arrays.copyOf(fbGains, fbGains.length);
-
-    instances++;
-    MathSharedStore.reportUsage(MathUsageId.kFilter_Linear, instances);
-  }
-
-  /**
-   * Creates a one-pole IIR low-pass filter of the form: y[n] = (1-gain)*x[n] + gain*y[n-1] where
-   * gain = e^(-dt / T), T is the time constant in seconds.
-   *
-   * <p>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.
-   */
-  public static LinearFilter singlePoleIIR(double timeConstant,
-                                           double period) {
-    double gain = Math.exp(-period / timeConstant);
-    double[] ffGains = {1.0 - gain};
-    double[] fbGains = {-gain};
-
-    return new LinearFilter(ffGains, fbGains);
-  }
-
-  /**
-   * Creates a first-order high-pass filter of the form: y[n] = gain*x[n] + (-gain)*x[n-1] +
-   * gain*y[n-1] where gain = e^(-dt / T), T is the time constant in seconds.
-   *
-   * <p>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.
-   */
-  public static LinearFilter highPass(double timeConstant,
-                                      double period) {
-    double gain = Math.exp(-period / timeConstant);
-    double[] ffGains = {gain, -gain};
-    double[] fbGains = {-gain};
-
-    return new LinearFilter(ffGains, fbGains);
-  }
-
-  /**
-   * Creates a K-tap FIR moving average filter of the form: y[n] = 1/k * (x[k] + x[k-1] + ... +
-   * x[0]).
-   *
-   * <p>This filter is always stable.
-   *
-   * @param taps The number of samples to average over. Higher = smoother but slower.
-   * @throws IllegalArgumentException if number of taps is less than 1.
-   */
-  public static LinearFilter movingAverage(int taps) {
-    if (taps <= 0) {
-      throw new IllegalArgumentException("Number of taps was not at least 1");
-    }
-
-    double[] ffGains = new double[taps];
-    for (int i = 0; i < ffGains.length; i++) {
-      ffGains[i] = 1.0 / taps;
-    }
-
-    double[] fbGains = new double[0];
-
-    return new LinearFilter(ffGains, fbGains);
-  }
-
-  /**
-   * Reset the filter state.
-   */
-  public void reset() {
-    m_inputs.clear();
-    m_outputs.clear();
-  }
-
-  /**
-   * Calculates the next value of the filter.
-   *
-   * @param input Current input value.
-   *
-   * @return The filtered value at this step
-   */
-  public double calculate(double input) {
-    double retVal = 0.0;
-
-    // Rotate the inputs
-    m_inputs.addFirst(input);
-
-    // Calculate the new value
-    for (int i = 0; i < m_inputGains.length; i++) {
-      retVal += m_inputs.get(i) * m_inputGains[i];
-    }
-    for (int i = 0; i < m_outputGains.length; i++) {
-      retVal -= m_outputs.get(i) * m_outputGains[i];
-    }
-
-    // Rotate the outputs
-    m_outputs.addFirst(retVal);
-
-    return retVal;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
deleted file mode 100644
index 18998b0..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/MedianFilter.java
+++ /dev/null
@@ -1,86 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.ArrayList;
-import java.util.Collections;
-import java.util.List;
-
-import edu.wpi.first.wpiutil.CircularBuffer;
-
-/**
- * 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).
- */
-public class MedianFilter {
-  private final CircularBuffer m_valueBuffer;
-  private final List<Double> m_orderedValues;
-  private final int m_size;
-
-  /**
-   * Creates a new MedianFilter.
-   *
-   * @param size The number of samples in the moving window.
-   */
-  public MedianFilter(int size) {
-    // Circular buffer of values currently in the window, ordered by time
-    m_valueBuffer = new CircularBuffer(size);
-    // List of values currently in the window, ordered by value
-    m_orderedValues = new ArrayList<>(size);
-    // Size of rolling window
-    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.
-   */
-  public double calculate(double next) {
-    // Find insertion point for next value
-    int index = Collections.binarySearch(m_orderedValues, next);
-
-    // Deal with binarySearch behavior for element not found
-    if (index < 0) {
-      index = Math.abs(index + 1);
-    }
-
-    // Place value at proper insertion point
-    m_orderedValues.add(index, next);
-
-    int 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.remove(m_valueBuffer.removeLast());
-      curSize = curSize - 1;
-    }
-
-    // Add next value to circular buffer
-    m_valueBuffer.addFirst(next);
-
-    if (curSize % 2 == 1) {
-      // If size is odd, return middle element of sorted list
-      return m_orderedValues.get(curSize / 2);
-    } else {
-      // If size is even, return average of middle elements
-      return (m_orderedValues.get(curSize / 2 - 1) + m_orderedValues.get(curSize / 2)) / 2.0;
-    }
-  }
-
-  /**
-   * Resets the filter, clearing the window of all elements.
-   */
-  public void reset() {
-    m_orderedValues.clear();
-    m_valueBuffer.clear();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
deleted file mode 100644
index 59e927a..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ArmFeedforward.java
+++ /dev/null
@@ -1,144 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-/**
- * 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).
- */
-@SuppressWarnings("MemberName")
-public class ArmFeedforward {
-  public final double ks;
-  public final double kcos;
-  public final double kv;
-  public final double ka;
-
-  /**
-   * Creates a new ArmFeedforward with the specified gains.  Units of the gain values
-   * will dictate units of the computed feedforward.
-   *
-   * @param ks   The static gain.
-   * @param kcos The gravity gain.
-   * @param kv   The velocity gain.
-   * @param ka   The acceleration gain.
-   */
-  public ArmFeedforward(double ks, double kcos, double kv, double ka) {
-    this.ks = ks;
-    this.kcos = kcos;
-    this.kv = kv;
-    this.ka = ka;
-  }
-
-  /**
-   * Creates a new ArmFeedforward with the specified gains.  Acceleration gain is
-   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
-   *
-   * @param ks   The static gain.
-   * @param kcos The gravity gain.
-   * @param kv   The velocity gain.
-   */
-  public ArmFeedforward(double ks, double kcos, double kv) {
-    this(ks, kcos, kv, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param positionRadians       The position (angle) setpoint.
-   * @param velocityRadPerSec     The velocity setpoint.
-   * @param accelRadPerSecSquared The acceleration setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double positionRadians, double velocityRadPerSec,
-                          double accelRadPerSecSquared) {
-    return ks * Math.signum(velocityRadPerSec) + kcos * Math.cos(positionRadians)
-        + kv * velocityRadPerSec
-        + ka * accelRadPerSecSquared;
-  }
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
-   * be zero).
-   *
-   * @param positionRadians The position (angle) setpoint.
-   * @param velocity        The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double positionRadians, double velocity) {
-    return calculate(positionRadians, velocity, 0);
-  }
-
-  // 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.
-   */
-  public double maxAchievableVelocity(double maxVoltage, double angle, double acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - ks - Math.cos(angle) * kcos - acceleration * ka) / 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.
-   */
-  public double minAchievableVelocity(double maxVoltage, double angle, double acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - Math.cos(angle) * kcos - acceleration * ka) / 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.
-   */
-  public double maxAchievableAcceleration(double maxVoltage, double angle, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - Math.cos(angle) * kcos - velocity * kv) / 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.
-   */
-  public double minAchievableAcceleration(double maxVoltage, double angle, double velocity) {
-    return maxAchievableAcceleration(-maxVoltage, angle, velocity);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java
deleted file mode 100644
index 79a88cd..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforward.java
+++ /dev/null
@@ -1,215 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import java.util.function.BiFunction;
-import java.util.function.Function;
-
-import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * Constructs a control-affine plant inversion model-based feedforward from
- * given model dynamics.
- *
- * <p>If given the vector valued function as f(x, u) where x is the state
- * vector and u is the input vector, the B matrix(continuous input matrix)
- * is calculated through a {@link edu.wpi.first.wpilibj.system.NumericalJacobian}.
- * In this case f has to be control-affine (of the form f(x) + Bu).
- *
- * <p>The feedforward is calculated as
- * <strong> u_ff = B<sup>+</sup> (rDot - f(x))</strong>, where
- * <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
- *
- * <p>This feedforward does not account for a dynamic B matrix, B is either
- * determined or supplied when the feedforward is created and remains constant.
- *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
- */
-@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
-public class ControlAffinePlantInversionFeedforward<States extends Num, Inputs extends Num> {
-  /**
-   * The current reference state.
-   */
-  @SuppressWarnings("MemberName")
-  private Matrix<States, N1> m_r;
-
-  /**
-   * The computed feedforward.
-   */
-  private Matrix<Inputs, N1> m_uff;
-
-  @SuppressWarnings("MemberName")
-  private final Matrix<States, Inputs> m_B;
-
-  private final Nat<Inputs> m_inputs;
-
-  private final double m_dt;
-
-  /**
-   * The model dynamics.
-   */
-  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
-
-  /**
-   * Constructs a feedforward with given model dynamics as a function
-   * of state and input.
-   *
-   * @param states    A {@link Nat} representing the number of states.
-   * @param inputs    A {@link Nat} representing the number of inputs.
-   * @param f         A vector-valued function of x, the state, and
-   *                  u, the input, that returns the derivative of
-   *                  the state vector. HAS to be control-affine
-   *                  (of the form f(x) + Bu).
-   * @param dtSeconds The timestep between calls of calculate().
-   */
-  public ControlAffinePlantInversionFeedforward(
-        Nat<States> states,
-        Nat<Inputs> inputs,
-        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-        double dtSeconds) {
-    this.m_dt = dtSeconds;
-    this.m_f = f;
-    this.m_inputs = inputs;
-
-    this.m_B = NumericalJacobian.numericalJacobianU(states, inputs,
-            m_f, new Matrix<>(states, Nat.N1()), new Matrix<>(inputs, Nat.N1()));
-
-    m_r = new Matrix<>(states, Nat.N1());
-    m_uff = new Matrix<>(inputs, Nat.N1());
-
-    reset(m_r);
-  }
-
-  /**
-   * Constructs a feedforward with given model dynamics as a function of state,
-   * and the plant's B(continuous input matrix) matrix.
-   *
-   * @param states    A {@link Nat} representing the number of states.
-   * @param inputs    A {@link Nat} representing the number of inputs.
-   * @param f         A vector-valued function of x, the state,
-   *                  that returns the derivative of the state vector.
-   * @param B         Continuous input matrix of the plant being controlled.
-   * @param dtSeconds The timestep between calls of calculate().
-   */
-  public ControlAffinePlantInversionFeedforward(
-        Nat<States> states,
-        Nat<Inputs> inputs,
-        Function<Matrix<States, N1>, Matrix<States, N1>> f,
-        Matrix<States, Inputs> B,
-        double dtSeconds) {
-    this.m_dt = dtSeconds;
-    this.m_inputs = inputs;
-
-    this.m_f = (x, u) -> f.apply(x);
-    this.m_B = B;
-
-    m_r = new Matrix<>(states, Nat.N1());
-    m_uff = new Matrix<>(inputs, Nat.N1());
-
-    reset(m_r);
-  }
-
-
-  /**
-   * Returns the previously calculated feedforward as an input vector.
-   *
-   * @return The calculated feedforward.
-   */
-  public Matrix<Inputs, N1> getUff() {
-    return m_uff;
-  }
-
-  /**
-   * Returns an element of the previously calculated feedforward.
-   *
-   * @param row Row of uff.
-   *
-   * @return The row of the calculated feedforward.
-   */
-  public double getUff(int row) {
-    return m_uff.get(row, 0);
-  }
-
-  /**
-   * Returns the current reference vector r.
-   *
-   * @return The current reference vector.
-   */
-  public Matrix<States, N1> getR() {
-    return m_r;
-  }
-
-  /**
-   * Returns an element of the current reference vector r.
-   *
-   * @param row Row of r.
-   *
-   * @return The row of the current reference vector.
-   */
-  public double getR(int row) {
-    return m_r.get(row, 0);
-  }
-
-  /**
-   * Resets the feedforward with a specified initial state vector.
-   *
-   * @param initialState The initial state vector.
-   */
-  public void reset(Matrix<States, N1> initialState) {
-    m_r = initialState;
-    m_uff.fill(0.0);
-  }
-
-  /**
-   * Resets the feedforward with a zero initial state vector.
-   */
-  public void reset() {
-    m_r.fill(0.0);
-    m_uff.fill(0.0);
-  }
-
-  /**
-   * Calculate the feedforward with only the desired
-   * future reference. This uses the internally stored "current"
-   * reference.
-   *
-   * <p>If this method is used the initial state of the system is the one
-   * set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
-   * If the initial state is not set it defaults to a zero vector.
-   *
-   * @param nextR The reference state of the future timestep (k + dt).
-   *
-   * @return The calculated feedforward.
-   */
-  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
-    return calculate(m_r, nextR);
-  }
-
-  /**
-   * Calculate the feedforward with current and future reference vectors.
-   *
-   * @param r     The reference state of the current timestep (k).
-   * @param nextR The reference state of the future timestep (k + dt).
-   *
-   * @return The calculated feedforward.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
-    var rDot = (nextR.minus(r)).div(m_dt);
-
-    m_uff = m_B.solve(rDot.minus(m_f.apply(r, new Matrix<>(m_inputs, Nat.N1()))));
-
-    m_r = nextR;
-    return m_uff;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
deleted file mode 100644
index 0b52c14..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/ElevatorFeedforward.java
+++ /dev/null
@@ -1,135 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-/**
- * A helper class that computes feedforward outputs for a simple elevator (modeled as a motor
- * acting against the force of gravity).
- */
-@SuppressWarnings("MemberName")
-public class ElevatorFeedforward {
-  public final double ks;
-  public final double kg;
-  public final double kv;
-  public final double ka;
-
-  /**
-   * Creates a new ElevatorFeedforward with the specified gains.  Units of the gain values
-   * will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
-   */
-  public ElevatorFeedforward(double ks, double kg, double kv, double ka) {
-    this.ks = ks;
-    this.kg = kg;
-    this.kv = kv;
-    this.ka = ka;
-  }
-
-  /**
-   * Creates a new ElevatorFeedforward with the specified gains.  Acceleration gain is
-   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kg The gravity gain.
-   * @param kv The velocity gain.
-   */
-  public ElevatorFeedforward(double ks, double kg, double kv) {
-    this(ks, kg, kv, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param velocity     The velocity setpoint.
-   * @param acceleration The acceleration setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity, double acceleration) {
-    return ks * Math.signum(velocity) + kg + kv * velocity + ka * acceleration;
-  }
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
-   * be zero).
-   *
-   * @param velocity The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity) {
-    return calculate(velocity, 0);
-  }
-
-  // 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.
-   */
-  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - ks - kg - acceleration * ka) / 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.
-   */
-  public double minAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - kg - acceleration * ka) / 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.
-   */
-  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - kg - velocity * kv) / 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.
-   */
-  public double minAchievableAcceleration(double maxVoltage, double velocity) {
-    return maxAchievableAcceleration(-maxVoltage, velocity);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java
deleted file mode 100644
index a4a00ee..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforward.java
+++ /dev/null
@@ -1,170 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * Constructs a plant inversion model-based feedforward from a {@link LinearSystem}.
- *
- * <p>The feedforward is calculated as <strong> u_ff = B<sup>+</sup> (r_k+1 - A r_k) </strong>,
- * where <strong> B<sup>+</sup> </strong> is the pseudoinverse of B.
- *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
- */
-@SuppressWarnings({"ParameterName", "LocalVariableName", "MemberName", "ClassTypeParameterName"})
-public class LinearPlantInversionFeedforward<States extends Num, Inputs extends Num,
-        Outputs extends Num> {
-  /**
-   * The current reference state.
-   */
-  @SuppressWarnings("MemberName")
-  private Matrix<States, N1> m_r;
-
-  /**
-   * The computed feedforward.
-   */
-  private Matrix<Inputs, N1> m_uff;
-
-  @SuppressWarnings("MemberName")
-  private final Matrix<States, Inputs> m_B;
-
-  @SuppressWarnings("MemberName")
-  private final Matrix<States, States> m_A;
-
-  /**
-   * Constructs a feedforward with the given plant.
-   *
-   * @param plant     The plant being controlled.
-   * @param dtSeconds Discretization timestep.
-   */
-  public LinearPlantInversionFeedforward(
-          LinearSystem<States, Inputs, Outputs> plant,
-          double dtSeconds
-  ) {
-    this(plant.getA(), plant.getB(), dtSeconds);
-  }
-
-  /**
-   * Constructs a feedforward with the given coefficients.
-   *
-   * @param A         Continuous system matrix of the plant being controlled.
-   * @param B         Continuous input matrix of the plant being controlled.
-   * @param dtSeconds Discretization timestep.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public LinearPlantInversionFeedforward(Matrix<States, States> A, Matrix<States, Inputs> B,
-                                         double dtSeconds) {
-    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
-    this.m_A = discABPair.getFirst();
-    this.m_B = discABPair.getSecond();
-
-    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
-    m_uff = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
-
-    reset(m_r);
-  }
-
-  /**
-   * Returns the previously calculated feedforward as an input vector.
-   *
-   * @return The calculated feedforward.
-   */
-  public Matrix<Inputs, N1> getUff() {
-    return m_uff;
-  }
-
-  /**
-   * Returns an element of the previously calculated feedforward.
-   *
-   * @param row Row of uff.
-   *
-   * @return The row of the calculated feedforward.
-   */
-  public double getUff(int row) {
-    return m_uff.get(row, 0);
-  }
-
-  /**
-   * Returns the current reference vector r.
-   *
-   * @return The current reference vector.
-   */
-  public Matrix<States, N1> getR() {
-    return m_r;
-  }
-
-  /**
-   * Returns an element of the current reference vector r.
-   *
-   * @param row Row of r.
-   *
-   * @return The row of the current reference vector.
-   */
-  public double getR(int row) {
-    return m_r.get(row, 0);
-  }
-
-  /**
-   * Resets the feedforward with a specified initial state vector.
-   *
-   * @param initialState The initial state vector.
-   */
-  public void reset(Matrix<States, N1> initialState) {
-    m_r = initialState;
-    m_uff.fill(0.0);
-  }
-
-  /**
-   * Resets the feedforward with a zero initial state vector.
-   */
-  public void reset() {
-    m_r.fill(0.0);
-    m_uff.fill(0.0);
-  }
-
-  /**
-   * Calculate the feedforward with only the desired
-   * future reference. This uses the internally stored "current"
-   * reference.
-   *
-   * <p>If this method is used the initial state of the system is the one
-   * set using {@link LinearPlantInversionFeedforward#reset(Matrix)}.
-   * If the initial state is not set it defaults to a zero vector.
-   *
-   * @param nextR The reference state of the future timestep (k + dt).
-   *
-   * @return The calculated feedforward.
-   */
-  public Matrix<Inputs, N1> calculate(Matrix<States, N1> nextR) {
-    return calculate(m_r, nextR);
-  }
-
-  /**
-   * Calculate the feedforward with current and future reference vectors.
-   *
-   * @param r     The reference state of the current timestep (k).
-   * @param nextR The reference state of the future timestep (k + dt).
-   *
-   * @return The calculated feedforward.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public Matrix<Inputs, N1> calculate(Matrix<States, N1> r, Matrix<States, N1> nextR) {
-    m_uff = new Matrix<>(m_B.solve(nextR.minus(m_A.times(r))));
-
-    m_r = nextR;
-    return m_uff;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java
deleted file mode 100644
index 195ba4f..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulator.java
+++ /dev/null
@@ -1,214 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.math.Drake;
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.Vector;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-
-/**
- * Contains the controller coefficients and logic for a linear-quadratic
- * regulator (LQR).
- * LQRs use the control law u = K(r - x).
- *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
- */
-@SuppressWarnings("ClassTypeParameterName")
-public class LinearQuadraticRegulator<States extends Num, Inputs extends Num,
-      Outputs extends Num> {
-  /**
-   * The current reference state.
-   */
-  @SuppressWarnings("MemberName")
-  private Matrix<States, N1> m_r;
-
-  /**
-   * The computed and capped controller output.
-   */
-  @SuppressWarnings("MemberName")
-  private Matrix<Inputs, N1> m_u;
-
-  // Controller gain.
-  @SuppressWarnings("MemberName")
-  private Matrix<Inputs, States> m_K;
-
-  /**
-   * Constructs a controller with the given coefficients and plant. Rho is defaulted to 1.
-   *
-   * @param plant     The plant being controlled.
-   * @param qelms     The maximum desired error tolerance for each state.
-   * @param relms     The maximum desired control effort for each input.
-   * @param dtSeconds Discretization timestep.
-   */
-  public LinearQuadraticRegulator(
-        LinearSystem<States, Inputs, Outputs> plant,
-        Vector<States> qelms,
-        Vector<Inputs> relms,
-        double dtSeconds
-  ) {
-    this(plant.getA(), plant.getB(), StateSpaceUtil.makeCostMatrix(qelms),
-        StateSpaceUtil.makeCostMatrix(relms), dtSeconds);
-  }
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param A         Continuous system matrix of the plant being controlled.
-   * @param B         Continuous input matrix of the plant being controlled.
-   * @param qelms     The maximum desired error tolerance for each state.
-   * @param relms     The maximum desired control effort for each input.
-   * @param dtSeconds Discretization timestep.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
-                                  Vector<States> qelms, Vector<Inputs> relms,
-                                  double dtSeconds
-  ) {
-    this(A, B, StateSpaceUtil.makeCostMatrix(qelms), StateSpaceUtil.makeCostMatrix(relms),
-        dtSeconds);
-  }
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   * @param A         Continuous system matrix of the plant being controlled.
-   * @param B         Continuous input matrix of the plant being controlled.
-   * @param Q         The state cost matrix.
-   * @param R         The input cost matrix.
-   * @param dtSeconds Discretization timestep.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public LinearQuadraticRegulator(Matrix<States, States> A, Matrix<States, Inputs> B,
-                                  Matrix<States, States> Q, Matrix<Inputs, Inputs> R,
-                                  double dtSeconds
-  ) {
-    var discABPair = Discretization.discretizeAB(A, B, dtSeconds);
-    var discA = discABPair.getFirst();
-    var discB = discABPair.getSecond();
-
-    var S = Drake.discreteAlgebraicRiccatiEquation(discA, discB, Q, R);
-
-    var temp = discB.transpose().times(S).times(discB).plus(R);
-
-    m_K = temp.solve(discB.transpose().times(S).times(discA));
-
-    m_r = new Matrix<>(new SimpleMatrix(B.getNumRows(), 1));
-    m_u = new Matrix<>(new SimpleMatrix(B.getNumCols(), 1));
-
-    reset();
-  }
-
-  /**
-   * Constructs a controller with the given coefficients and plant.
-   *
-   * @param states The number of states.
-   * @param inputs The number of inputs.
-   * @param k The gain matrix.
-   */
-  @SuppressWarnings("ParameterName")
-  public LinearQuadraticRegulator(
-      Nat<States> states, Nat<Inputs> inputs,
-      Matrix<Inputs, States> k
-  ) {
-    m_K = k;
-
-    m_r = new Matrix<>(states, Nat.N1());
-    m_u = new Matrix<>(inputs, Nat.N1());
-
-    reset();
-  }
-
-  /**
-   * Returns the control input vector u.
-   *
-   * @return The control input.
-   */
-  public Matrix<Inputs, N1> getU() {
-    return m_u;
-  }
-
-  /**
-   * Returns an element of the control input vector u.
-   *
-   * @param row Row of u.
-   *
-   * @return The row of the control input vector.
-   */
-  public double getU(int row) {
-    return m_u.get(row, 0);
-  }
-
-  /**
-   * Returns the reference vector r.
-   *
-   * @return The reference vector.
-   */
-  public Matrix<States, N1> getR() {
-    return m_r;
-  }
-
-  /**
-   * Returns an element of the reference vector r.
-   *
-   * @param row Row of r.
-   *
-   * @return The row of the reference vector.
-   */
-  public double getR(int row) {
-    return m_r.get(row, 0);
-  }
-
-  /**
-   * Returns the controller matrix K.
-   *
-   * @return the controller matrix K.
-   */
-  public Matrix<Inputs, States> getK() {
-    return m_K;
-  }
-
-  /**
-   * Resets the controller.
-   */
-  public void reset() {
-    m_r.fill(0.0);
-    m_u.fill(0.0);
-  }
-
-  /**
-   * Returns the next output of the controller.
-   *
-   * @param x The current state x.
-   */
-  @SuppressWarnings("ParameterName")
-  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x) {
-    m_u = m_K.times(m_r.minus(x));
-    return m_u;
-  }
-
-  /**
-   * Returns the next output of the controller.
-   *
-   * @param x     The current state x.
-   * @param nextR the next reference vector r.
-   */
-  @SuppressWarnings("ParameterName")
-  public Matrix<Inputs, N1> calculate(Matrix<States, N1> x, Matrix<States, N1> nextR) {
-    m_r = nextR;
-    return calculate(x);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
deleted file mode 100644
index ec53d46..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/controller/SimpleMotorFeedforward.java
+++ /dev/null
@@ -1,130 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-/**
- * A helper class that computes feedforward outputs for a simple permanent-magnet DC motor.
- */
-@SuppressWarnings("MemberName")
-public class SimpleMotorFeedforward {
-  public final double ks;
-  public final double kv;
-  public final double ka;
-
-  /**
-   * Creates a new SimpleMotorFeedforward with the specified gains.  Units of the gain values
-   * will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
-   * @param ka The acceleration gain.
-   */
-  public SimpleMotorFeedforward(double ks, double kv, double ka) {
-    this.ks = ks;
-    this.kv = kv;
-    this.ka = ka;
-  }
-
-  /**
-   * Creates a new SimpleMotorFeedforward with the specified gains.  Acceleration gain is
-   * defaulted to zero.  Units of the gain values will dictate units of the computed feedforward.
-   *
-   * @param ks The static gain.
-   * @param kv The velocity gain.
-   */
-  public SimpleMotorFeedforward(double ks, double kv) {
-    this(ks, kv, 0);
-  }
-
-  /**
-   * Calculates the feedforward from the gains and setpoints.
-   *
-   * @param velocity     The velocity setpoint.
-   * @param acceleration The acceleration setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity, double acceleration) {
-    return ks * Math.signum(velocity) + kv * velocity + ka * acceleration;
-  }
-
-  // Rearranging the main equation from the calculate() method yields the
-  // formulas for the methods below:
-
-  /**
-   * Calculates the feedforward from the gains and velocity setpoint (acceleration is assumed to
-   * be zero).
-   *
-   * @param velocity The velocity setpoint.
-   * @return The computed feedforward.
-   */
-  public double calculate(double velocity) {
-    return calculate(velocity, 0);
-  }
-
-  /**
-   * 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.
-   */
-  public double maxAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume max velocity is positive
-    return (maxVoltage - ks - acceleration * ka) / 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.
-   */
-  public double minAchievableVelocity(double maxVoltage, double acceleration) {
-    // Assume min velocity is negative, ks flips sign
-    return (-maxVoltage + ks - acceleration * ka) / 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.
-   */
-  public double maxAchievableAcceleration(double maxVoltage, double velocity) {
-    return (maxVoltage - ks * Math.signum(velocity) - velocity * kv) / ka;
-  }
-
-  /**
-   * 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 minimum possible acceleration at the given velocity.
-   */
-  public double minAchievableAcceleration(double maxVoltage, double velocity) {
-    return maxAchievableAcceleration(-maxVoltage, velocity);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
deleted file mode 100644
index 7474b02..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilter.java
+++ /dev/null
@@ -1,288 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import java.util.function.BiFunction;
-
-import edu.wpi.first.math.Drake;
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * Kalman filters combine predictions from a model and measurements to give an estimate of the true
- * system state. This is useful because many states cannot be measured directly as a result of
- * sensor noise, or because the state is "hidden".
- *
- * <p>The Extended Kalman filter is just like the {@link KalmanFilter Kalman filter}, but we make a
- * linear approximation of nonlinear dynamics and/or nonlinear measurement models. This means that
- * the EKF works with nonlinear systems.
- */
-@SuppressWarnings("ClassTypeParameterName")
-public class ExtendedKalmanFilter<States extends Num, Inputs extends Num, Outputs extends Num>
-      implements KalmanTypeFilter<States, Inputs, Outputs> {
-  private final Nat<States> m_states;
-  private final Nat<Outputs> m_outputs;
-
-  @SuppressWarnings("MemberName")
-  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
-  @SuppressWarnings("MemberName")
-  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
-  private final Matrix<States, States> m_contQ;
-  private final Matrix<States, States> m_initP;
-  private final Matrix<Outputs, Outputs> m_contR;
-  @SuppressWarnings("MemberName")
-  private Matrix<States, N1> m_xHat;
-  @SuppressWarnings("MemberName")
-  private Matrix<States, States> m_P;
-  private double m_dtSeconds;
-
-  /**
-   * Constructs an extended Kalman filter.
-   *
-   * @param states             a Nat representing the number of states.
-   * @param inputs             a Nat representing the number of inputs.
-   * @param outputs            a Nat representing the number of outputs.
-   * @param f                  A vector-valued function of x and u that returns
-   *                           the derivative of the state vector.
-   * @param h                  A vector-valued function of x and u that returns
-   *                           the measurement vector.
-   * @param stateStdDevs       Standard deviations of model states.
-   * @param measurementStdDevs Standard deviations of measurements.
-   * @param dtSeconds          Nominal discretization timestep.
-   */
-  @SuppressWarnings("ParameterName")
-  public ExtendedKalmanFilter(
-        Nat<States> states,
-        Nat<Inputs> inputs,
-        Nat<Outputs> outputs,
-        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> h,
-        Matrix<States, N1> stateStdDevs,
-        Matrix<Outputs, N1> measurementStdDevs,
-        double dtSeconds
-  ) {
-    m_states = states;
-    m_outputs = outputs;
-
-    m_f = f;
-    m_h = h;
-
-    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    this.m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
-    m_dtSeconds = dtSeconds;
-
-    reset();
-
-    final var contA = NumericalJacobian
-          .numericalJacobianX(states, states, f, m_xHat, new Matrix<>(inputs, Nat.N1()));
-    final var C = NumericalJacobian
-          .numericalJacobianX(outputs, states, h, m_xHat, new Matrix<>(inputs, Nat.N1()));
-
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
-    final var discA = discPair.getFirst();
-    final var discQ = discPair.getSecond();
-
-    final var discR = Discretization.discretizeR(m_contR, dtSeconds);
-
-    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
-    boolean isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
-    if (isObservable && outputs.getNum() <= states.getNum()) {
-      m_initP = Drake.discreteAlgebraicRiccatiEquation(
-            discA.transpose(), C.transpose(), discQ, discR) ;
-    } else {
-      m_initP = new Matrix<>(states, states);
-    }
-
-    m_P = m_initP;
-  }
-
-  /**
-   * Returns the error covariance matrix P.
-   *
-   * @return the error covariance matrix P.
-   */
-  @Override
-  public Matrix<States, States> getP() {
-    return m_P;
-  }
-
-  /**
-   * Returns an element of the error covariance matrix P.
-   *
-   * @param row Row of P.
-   * @param col Column of P.
-   * @return the value of the error covariance matrix P at (i, j).
-   */
-  @Override
-  public double getP(int row, int col) {
-    return m_P.get(row, col);
-  }
-
-  /**
-   * Sets the entire error covariance matrix P.
-   *
-   * @param newP The new value of P to use.
-   */
-  @Override
-  public void setP(Matrix<States, States> newP) {
-    m_P = newP;
-  }
-
-  /**
-   * Returns the state estimate x-hat.
-   *
-   * @return the state estimate x-hat.
-   */
-  @Override
-  public Matrix<States, N1> getXhat() {
-    return m_xHat;
-  }
-
-  /**
-   * Returns an element of the state estimate x-hat.
-   *
-   * @param row Row of x-hat.
-   * @return the value of the state estimate x-hat at i.
-   */
-  @Override
-  public double getXhat(int row) {
-    return m_xHat.get(row, 0);
-  }
-
-  /**
-   * Set initial state estimate x-hat.
-   *
-   * @param xHat The state estimate x-hat.
-   */
-  @SuppressWarnings("ParameterName")
-  @Override
-  public void setXhat(Matrix<States, N1> xHat) {
-    m_xHat = xHat;
-  }
-
-
-  /**
-   * Set an element of the initial state estimate x-hat.
-   *
-   * @param row   Row of x-hat.
-   * @param value Value for element of x-hat.
-   */
-  @Override
-  public void setXhat(int row, double value) {
-    m_xHat.set(row, 0, value);
-  }
-
-  @Override
-  public void reset() {
-    m_xHat = new Matrix<>(m_states, Nat.N1());
-    m_P = m_initP;
-  }
-
-  /**
-   * Project the model into the future with a new control input u.
-   *
-   * @param u         New control input from controller.
-   * @param dtSeconds Timestep for prediction.
-   */
-  @SuppressWarnings("ParameterName")
-  @Override
-  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
-    predict(u, m_f, dtSeconds);
-  }
-
-  /**
-   * Project the model into the future with a new control input u.
-   *
-   * @param u         New control input from controller.
-   * @param f         The function used to linearlize the model.
-   * @param dtSeconds Timestep for prediction.
-   */
-  @SuppressWarnings("ParameterName")
-  public void predict(
-      Matrix<Inputs, N1> u, BiFunction<Matrix<States, N1>,
-        Matrix<Inputs, N1>, Matrix<States, N1>> f,
-      double dtSeconds
-  ) {
-    // Find continuous A
-    final var contA = NumericalJacobian.numericalJacobianX(m_states, m_states, f, m_xHat, u);
-
-    // Find discrete A and Q
-    final var discPair = Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds);
-    final var discA = discPair.getFirst();
-    final var discQ = discPair.getSecond();
-
-    m_xHat = RungeKutta.rungeKutta(f, m_xHat, u, dtSeconds);
-    m_P = discA.times(m_P).times(discA.transpose()).plus(discQ);
-    m_dtSeconds = dtSeconds;
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * @param u Same control input used in the predict step.
-   * @param y Measurement vector.
-   */
-  @SuppressWarnings("ParameterName")
-  @Override
-  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    correct(m_outputs, u, y, m_h, m_contR);
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * <p>This is useful for when the measurements available during a timestep's
-   * Correct() call vary. The h(x, u) passed to the constructor is used if one is
-   * not provided (the two-argument version of this function).
-   *
-   * @param <Rows>  Number of rows in the result of f(x, u).
-   * @param rows Number of rows in the result of f(x, u).
-   * @param u    Same control input used in the predict step.
-   * @param y    Measurement vector.
-   * @param h    A vector-valued function of x and u that returns the measurement
-   *             vector.
-   * @param R    Discrete measurement noise covariance matrix.
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public <Rows extends Num> void correct(
-        Nat<Rows> rows, Matrix<Inputs, N1> u,
-        Matrix<Rows, N1> y,
-        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Rows, N1>> h,
-        Matrix<Rows, Rows> R
-  ) {
-    final var C = NumericalJacobian.numericalJacobianX(rows, m_states, h, m_xHat, u);
-    final var discR = Discretization.discretizeR(R, m_dtSeconds);
-
-    final var S = C.times(m_P).times(C.transpose()).plus(discR);
-
-    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
-    // efficiently.
-    //
-    // K = PC^T S^-1
-    // KS = PC^T
-    // (KS)^T = (PC^T)^T
-    // S^T K^T = CP^T
-    //
-    // The solution of Ax = b can be found via x = A.solve(b).
-    //
-    // K^T = S^T.solve(CP^T)
-    // K = (S^T.solve(CP^T))^T
-    //
-    // Now we have the Kalman gain
-    final Matrix<States, Rows> K = S.transpose().solve(C.times(m_P.transpose())).transpose();
-
-    m_xHat = m_xHat.plus(K.times(y.minus(h.apply(m_xHat, u))));
-    m_P = Matrix.eye(m_states).minus(K.times(C)).times(m_P);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java
deleted file mode 100644
index 99fa2b7..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanFilter.java
+++ /dev/null
@@ -1,204 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import edu.wpi.first.math.Drake;
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * A Kalman filter combines predictions from a model and measurements to give an estimate of the
- * true system state. This is useful because many states cannot be measured directly as a result of
- * sensor noise, or because the state is "hidden".
- *
- * <p>Kalman filters use a K gain matrix to determine whether to trust the model or measurements
- * more. Kalman filter theory uses statistics to compute an optimal K gain which minimizes the sum
- * of squares error in the state estimate. This K gain is used to correct the state estimate by
- * some amount of the difference between the actual measurements and the measurements predicted by
- * the model.
- *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9 "Stochastic control
- * theory".
- */
-@SuppressWarnings("ClassTypeParameterName")
-public class KalmanFilter<States extends Num, Inputs extends Num,
-      Outputs extends Num> {
-  private final Nat<States> m_states;
-
-  private final LinearSystem<States, Inputs, Outputs> m_plant;
-
-  /**
-   * The steady-state Kalman gain matrix.
-   */
-  @SuppressWarnings("MemberName")
-  private final Matrix<States, Outputs> m_K;
-
-  /**
-   * The state estimate.
-   */
-  @SuppressWarnings("MemberName")
-  private Matrix<States, N1> m_xHat;
-
-  /**
-   * Constructs a state-space observer with the given plant.
-   *
-   * @param states             A Nat representing the states of the system.
-   * @param outputs            A Nat representing the outputs of the system.
-   * @param plant              The plant used for the prediction step.
-   * @param stateStdDevs       Standard deviations of model states.
-   * @param measurementStdDevs Standard deviations of measurements.
-   * @param dtSeconds          Nominal discretization timestep.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public KalmanFilter(
-      Nat<States> states, Nat<Outputs> outputs,
-      LinearSystem<States, Inputs, Outputs> plant,
-      Matrix<States, N1> stateStdDevs,
-      Matrix<Outputs, N1> measurementStdDevs,
-      double dtSeconds
-  ) {
-    this.m_states = states;
-
-    this.m_plant = plant;
-
-    var contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    var contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
-
-    var pair = Discretization.discretizeAQTaylor(plant.getA(), contQ, dtSeconds);
-    var discA = pair.getFirst();
-    var discQ = pair.getSecond();
-
-    var discR = Discretization.discretizeR(contR, dtSeconds);
-
-    var C = plant.getC();
-
-    // isStabilizable(A^T, C^T) will tell us if the system is observable.
-    var isObservable = StateSpaceUtil.isStabilizable(discA.transpose(), C.transpose());
-    if (!isObservable) {
-      MathSharedStore.reportError("The system passed to the Kalman filter is not observable!",
-          Thread.currentThread().getStackTrace());
-      throw new IllegalArgumentException(
-        "The system passed to the Kalman filter is not observable!");
-    }
-
-    var P = new Matrix<>(Drake.discreteAlgebraicRiccatiEquation(
-          discA.transpose(), C.transpose(), discQ, discR));
-
-    var S = C.times(P).times(C.transpose()).plus(discR);
-
-    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
-    // efficiently.
-    //
-    // K = PC^T S^-1
-    // KS = PC^T
-    // (KS)^T = (PC^T)^T
-    // S^T K^T = CP^T
-    //
-    // The solution of Ax = b can be found via x = A.solve(b).
-    //
-    // K^T = S^T.solve(CP^T)
-    // K = (S^T.solve(CP^T))^T
-    m_K = new Matrix<>(S.transpose().getStorage()
-          .solve((C.times(P.transpose())).getStorage()).transpose());
-
-    reset();
-  }
-
-  public void reset() {
-    m_xHat = new Matrix<>(m_states, Nat.N1());
-  }
-
-  /**
-   * Returns the steady-state Kalman gain matrix K.
-   *
-   * @return The steady-state Kalman gain matrix K.
-   */
-  public Matrix<States, Outputs> getK() {
-    return m_K;
-  }
-
-  /**
-   * Returns an element of the steady-state Kalman gain matrix K.
-   *
-   * @param row Row of K.
-   * @param col Column of K.
-   * @return the element (i, j) of the steady-state Kalman gain matrix K.
-   */
-  public double getK(int row, int col) {
-    return m_K.get(row, col);
-  }
-
-  /**
-   * Set initial state estimate x-hat.
-   *
-   * @param xhat The state estimate x-hat.
-   */
-  public void setXhat(Matrix<States, N1> xhat) {
-    this.m_xHat = xhat;
-  }
-
-  /**
-   * Set an element of the initial state estimate x-hat.
-   *
-   * @param row   Row of x-hat.
-   * @param value Value for element of x-hat.
-   */
-  public void setXhat(int row, double value) {
-    m_xHat.set(row, 0, value);
-  }
-
-  /**
-   * Returns the state estimate x-hat.
-   *
-   * @return The state estimate x-hat.
-   */
-  public Matrix<States, N1> getXhat() {
-    return m_xHat;
-  }
-
-  /**
-   * Returns an element of the state estimate x-hat.
-   *
-   * @param row Row of x-hat.
-   * @return the state estimate x-hat at i.
-   */
-  public double getXhat(int row) {
-    return m_xHat.get(row, 0);
-  }
-
-  /**
-   * Project the model into the future with a new control input u.
-   *
-   * @param u         New control input from controller.
-   * @param dtSeconds Timestep for prediction.
-   */
-  @SuppressWarnings("ParameterName")
-  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
-    this.m_xHat = m_plant.calculateX(m_xHat, u, dtSeconds);
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * @param u Same control input used in the last predict step.
-   * @param y Measurement vector.
-   */
-  @SuppressWarnings("ParameterName")
-  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    final var C = m_plant.getC();
-    final var D = m_plant.getD();
-    m_xHat = m_xHat.plus(m_K.times(y.minus(C.times(m_xHat).plus(D.times(u)))));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
deleted file mode 100644
index aa93b29..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/KalmanTypeFilter.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-@SuppressWarnings({"ParameterName", "InterfaceTypeParameterName"})
-interface KalmanTypeFilter<States extends Num, Inputs extends Num, Outputs extends Num> {
-  Matrix<States, States> getP();
-
-  double getP(int i, int j);
-
-  void setP(Matrix<States, States> newP);
-
-  Matrix<States, N1> getXhat();
-
-  double getXhat(int i);
-
-  void setXhat(Matrix<States, N1> xHat);
-
-  void setXhat(int i, double value);
-
-  void reset();
-
-  void predict(Matrix<Inputs, N1> u, double dtSeconds);
-
-  void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y);
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
deleted file mode 100644
index 56e9288..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPoints.java
+++ /dev/null
@@ -1,168 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * Generates sigma points and weights according to Van der Merwe's 2004
- * dissertation[1] for the UnscentedKalmanFilter class.
- *
- * <p>It parametrizes the sigma points using alpha, beta, kappa terms, and is the
- * version seen in most publications. Unless you know better, this should be
- * your default choice.
- *
- * <p>States is the dimensionality of the state. 2*States+1 weights will be
- * generated.
- *
- * <p>[1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
- * Inference in Dynamic State-Space Models" (Doctoral dissertation)
- */
-public class MerweScaledSigmaPoints<S extends Num> {
-
-  private final double m_alpha;
-  private final int m_kappa;
-  private final Nat<S> m_states;
-  private Matrix<?, N1> m_wm;
-  private Matrix<?, N1> m_wc;
-
-  /**
-   * Constructs a generator for Van der Merwe scaled sigma points.
-   *
-   * @param states an instance of Num that represents the number of states.
-   * @param alpha  Determines the spread of the sigma points around the mean.
-   *               Usually a small positive value (1e-3).
-   * @param beta   Incorporates prior knowledge of the distribution of the mean.
-   *               For Gaussian distributions, beta = 2 is optimal.
-   * @param kappa  Secondary scaling parameter usually set to 0 or 3 - States.
-   */
-  public MerweScaledSigmaPoints(Nat<S> states, double alpha, double beta, int kappa) {
-    this.m_states = states;
-    this.m_alpha = alpha;
-    this.m_kappa = kappa;
-
-    computeWeights(beta);
-  }
-
-  /**
-   * Constructs a generator for Van der Merwe scaled sigma points with default values for alpha,
-   * beta, and kappa.
-   *
-   * @param states an instance of Num that represents the number of states.
-   */
-  public MerweScaledSigmaPoints(Nat<S> states) {
-    this(states, 1e-3, 2, 3 - states.getNum());
-  }
-
-  /**
-   * Returns number of sigma points for each variable in the state x.
-   *
-   * @return The number of sigma points for each variable in the state x.
-   */
-  public int getNumSigmas() {
-    return 2 * m_states.getNum() + 1;
-  }
-
-  /**
-   * Computes the sigma points for an unscented Kalman filter given the mean
-   * (x) and covariance(P) of the filter.
-   *
-   * @param x An array of the means.
-   * @param P Covariance of the filter.
-   * @return Two dimensional array of sigma points. Each column contains all of
-   *         the sigmas for one dimension in the problem space. Ordered by
-   *         Xi_0, Xi_{1..n}, Xi_{n+1..2n}.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public Matrix<S, ?> sigmaPoints(
-          Matrix<S, N1> x,
-          Matrix<S, S> P) {
-    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
-
-    var intermediate = P.times(lambda + m_states.getNum());
-    var U = intermediate.lltDecompose(true); // Lower triangular
-
-    // 2 * states + 1 by states
-    Matrix<S, ?> sigmas = new Matrix<>(
-        new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
-    sigmas.setColumn(0, x);
-    for (int k = 0; k < m_states.getNum(); k++) {
-      var xPlusU = x.plus(U.extractColumnVector(k));
-      var xMinusU = x.minus(U.extractColumnVector(k));
-      sigmas.setColumn(k + 1, xPlusU);
-      sigmas.setColumn(m_states.getNum() + k + 1, xMinusU);
-    }
-
-    return new Matrix<>(sigmas);
-  }
-
-  /**
-   * Computes the weights for the scaled unscented Kalman filter.
-   *
-   * @param beta Incorporates prior knowledge of the distribution of the mean.
-   */
-  @SuppressWarnings("LocalVariableName")
-  private void computeWeights(double beta) {
-    double lambda = Math.pow(m_alpha, 2) * (m_states.getNum() + m_kappa) - m_states.getNum();
-    double c = 0.5 / (m_states.getNum() + lambda);
-
-    Matrix<?, N1> wM = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
-    Matrix<?, N1> wC = new Matrix<>(new SimpleMatrix(2 * m_states.getNum() + 1, 1));
-    wM.fill(c);
-    wC.fill(c);
-
-    wM.set(0, 0, lambda / (m_states.getNum() + lambda));
-    wC.set(0, 0, lambda / (m_states.getNum() + lambda) + (1 - Math.pow(m_alpha, 2) + beta));
-
-    this.m_wm = wM;
-    this.m_wc = wC;
-  }
-
-  /**
-   * Returns the weight for each sigma point for the mean.
-   *
-   * @return the weight for each sigma point for the mean.
-   */
-  public Matrix<?, N1> getWm() {
-    return m_wm;
-  }
-
-  /**
-   * Returns an element of the weight for each sigma point for the mean.
-   *
-   * @param element Element of vector to return.
-   * @return the element i's weight for the mean.
-   */
-  public double getWm(int element) {
-    return m_wm.get(element, 0);
-  }
-
-  /**
-   * Returns the weight for each sigma point for the covariance.
-   *
-   * @return the weight for each sigma point for the covariance.
-   */
-  public Matrix<?, N1> getWc() {
-    return m_wc;
-  }
-
-  /**
-   * Returns an element of the weight for each sigma point for the covariance.
-   *
-   * @param element Element of vector to return.
-   * @return The element I's weight for the covariance.
-   */
-  public double getWc(int element) {
-    return m_wc.get(element, 0);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
deleted file mode 100644
index ca99153..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilter.java
+++ /dev/null
@@ -1,316 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import java.util.function.BiFunction;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.Pair;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * A Kalman filter combines predictions from a model and measurements to give an estimate of the
- * true ystem state. This is useful because many states cannot be measured directly as a result of
- * sensor noise, or because the state is "hidden".
- *
- * <p>The Unscented Kalman filter is similar to the {@link KalmanFilter Kalman filter}, except that
- * it propagates carefully chosen points called sigma points through the non-linear model to obtain
- * an estimate of the true covariance (as opposed to a linearized version of it). This means that
- * the UKF works with nonlinear systems.
- */
-@SuppressWarnings({"MemberName", "ClassTypeParameterName"})
-public class UnscentedKalmanFilter<States extends Num, Inputs extends Num,
-      Outputs extends Num> implements KalmanTypeFilter<States, Inputs, Outputs> {
-
-  private final Nat<States> m_states;
-  private final Nat<Outputs> m_outputs;
-
-  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> m_f;
-  private final BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> m_h;
-
-  private Matrix<States, N1> m_xHat;
-  private Matrix<States, States> m_P;
-  private final Matrix<States, States> m_contQ;
-  private final Matrix<Outputs, Outputs> m_contR;
-  private Matrix<States, ?> m_sigmasF;
-  private double m_dtSeconds;
-
-  private final MerweScaledSigmaPoints<States> m_pts;
-
-  /**
-   * Constructs an Unscented Kalman Filter.
-   *
-   * @param states             A Nat representing the number of states.
-   * @param outputs            A Nat representing the number of outputs.
-   * @param f                  A vector-valued function of x and u that returns
-   *                           the derivative of the state vector.
-   * @param h                  A vector-valued function of x and u that returns
-   *                           the measurement vector.
-   * @param stateStdDevs       Standard deviations of model states.
-   * @param measurementStdDevs Standard deviations of measurements.
-   * @param dtSeconds          Nominal discretization timestep.
-   */
-  @SuppressWarnings("ParameterName")
-  public UnscentedKalmanFilter(Nat<States> states, Nat<Outputs> outputs,
-                               BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>,
-                                   Matrix<States, N1>> f,
-                               BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>,
-                                   Matrix<Outputs, N1>> h,
-                               Matrix<States, N1> stateStdDevs,
-                               Matrix<Outputs, N1> measurementStdDevs,
-                               double dtSeconds) {
-    this.m_states = states;
-    this.m_outputs = outputs;
-
-    m_f = f;
-    m_h = h;
-
-    m_contQ = StateSpaceUtil.makeCovarianceMatrix(states, stateStdDevs);
-    m_contR = StateSpaceUtil.makeCovarianceMatrix(outputs, measurementStdDevs);
-
-    m_dtSeconds = dtSeconds;
-
-    m_pts = new MerweScaledSigmaPoints<>(states);
-
-    reset();
-  }
-
-  @SuppressWarnings({"ParameterName", "LocalVariableName", "PMD.CyclomaticComplexity"})
-  static <S extends Num, C extends Num>
-       Pair<Matrix<C, N1>, Matrix<C, C>> unscentedTransform(
-        Nat<S> s, Nat<C> dim, Matrix<C, ?> sigmas, Matrix<?, N1> Wm, Matrix<?, N1> Wc
-  ) {
-    if (sigmas.getNumRows() != dim.getNum() || sigmas.getNumCols() != 2 * s.getNum() + 1) {
-      throw new IllegalArgumentException("Sigmas must be covDim by 2 * states + 1! Got "
-            + sigmas.getNumRows() + " by " + sigmas.getNumCols());
-    }
-
-    if (Wm.getNumRows() != 2 * s.getNum() + 1 || Wm.getNumCols() != 1 ) {
-      throw new IllegalArgumentException("Wm must be 2 * states + 1 by 1! Got "
-            + Wm.getNumRows() + " by " + Wm.getNumCols());
-    }
-
-    if (Wc.getNumRows() != 2 * s.getNum() + 1 || Wc.getNumCols() != 1) {
-      throw new IllegalArgumentException("Wc must be 2 * states + 1 by 1! Got "
-            + Wc.getNumRows() + " by " + Wc.getNumCols());
-    }
-
-    // New mean is just the sum of the sigmas * weight
-    // dot = \Sigma^n_1 (W[k]*Xi[k])
-    Matrix<C, N1> x = sigmas.times(Matrix.changeBoundsUnchecked(Wm));
-
-    // New covariance is the sum of the outer product of the residuals times the
-    // weights
-    Matrix<C, ?> y = new Matrix<>(new SimpleMatrix(dim.getNum(), 2 * s.getNum() + 1));
-    for (int i = 0; i < 2 * s.getNum() + 1; i++) {
-      y.setColumn(i, sigmas.extractColumnVector(i).minus(x));
-    }
-    Matrix<C, C> P = y.times(Matrix.changeBoundsUnchecked(Wc.diag()))
-          .times(Matrix.changeBoundsUnchecked(y.transpose()));
-
-    return new Pair<>(x, P);
-  }
-
-  /**
-   * Returns the error covariance matrix P.
-   *
-   * @return the error covariance matrix P.
-   */
-  @Override
-  public Matrix<States, States> getP() {
-    return m_P;
-  }
-
-  /**
-   * Returns an element of the error covariance matrix P.
-   *
-   * @param row Row of P.
-   * @param col Column of P.
-   * @return the value of the error covariance matrix P at (i, j).
-   */
-  @Override
-  public double getP(int row, int col) {
-    return m_P.get(row, col);
-  }
-
-  /**
-   * Sets the entire error covariance matrix P.
-   *
-   * @param newP The new value of P to use.
-   */
-  @Override
-  public void setP(Matrix<States, States> newP) {
-    m_P = newP;
-  }
-
-  /**
-   * Returns the state estimate x-hat.
-   *
-   * @return the state estimate x-hat.
-   */
-  @Override
-  public Matrix<States, N1> getXhat() {
-    return m_xHat;
-  }
-
-  /**
-   * Returns an element of the state estimate x-hat.
-   *
-   * @param row Row of x-hat.
-   * @return the value of the state estimate x-hat at i.
-   */
-  @Override
-  public double getXhat(int row) {
-    return m_xHat.get(row, 0);
-  }
-
-
-  /**
-   * Set initial state estimate x-hat.
-   *
-   * @param xHat The state estimate x-hat.
-   */
-  @SuppressWarnings("ParameterName")
-  @Override
-  public void setXhat(Matrix<States, N1> xHat) {
-    m_xHat = xHat;
-  }
-
-  /**
-   * Set an element of the initial state estimate x-hat.
-   *
-   * @param row   Row of x-hat.
-   * @param value Value for element of x-hat.
-   */
-  @Override
-  public void setXhat(int row, double value) {
-    m_xHat.set(row, 0, value);
-  }
-
-  /**
-   * Resets the observer.
-   */
-  @Override
-  public void reset() {
-    m_xHat = new Matrix<>(m_states, Nat.N1());
-    m_P = new Matrix<>(m_states, m_states);
-    m_sigmasF = new Matrix<>(new SimpleMatrix(m_states.getNum(), 2 * m_states.getNum() + 1));
-  }
-
-  /**
-   * Project the model into the future with a new control input u.
-   *
-   * @param u         New control input from controller.
-   * @param dtSeconds Timestep for prediction.
-   */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
-  @Override
-  public void predict(Matrix<Inputs, N1> u, double dtSeconds) {
-    // Discretize Q before projecting mean and covariance forward
-    Matrix<States, States> contA =
-        NumericalJacobian.numericalJacobianX(m_states, m_states, m_f, m_xHat, u);
-    var discQ =
-        Discretization.discretizeAQTaylor(contA, m_contQ, dtSeconds).getSecond();
-
-    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
-
-    for (int i = 0; i < m_pts.getNumSigmas(); ++i) {
-      Matrix<States, N1> x = sigmas.extractColumnVector(i);
-
-      m_sigmasF.setColumn(i, RungeKutta.rungeKutta(m_f, x, u, dtSeconds));
-    }
-
-    var ret = unscentedTransform(m_states, m_states,
-          m_sigmasF, m_pts.getWm(), m_pts.getWc());
-
-    m_xHat = ret.getFirst();
-    m_P = ret.getSecond().plus(discQ);
-    m_dtSeconds = dtSeconds;
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * @param u Same control input used in the predict step.
-   * @param y Measurement vector.
-   */
-  @SuppressWarnings("ParameterName")
-  @Override
-  public void correct(Matrix<Inputs, N1> u, Matrix<Outputs, N1> y) {
-    correct(m_outputs, u, y, m_h, m_contR);
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * <p>This is useful for when the measurements available during a timestep's
-   * Correct() call vary. The h(x, u) passed to the constructor is used if one
-   * is not provided (the two-argument version of this function).
-   *
-   * @param u Same control input used in the predict step.
-   * @param y Measurement vector.
-   * @param h A vector-valued function of x and u that returns
-   *          the measurement vector.
-   * @param R Measurement noise covariance matrix.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public <R extends Num> void correct(
-        Nat<R> rows, Matrix<Inputs, N1> u,
-        Matrix<R, N1> y,
-        BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<R, N1>> h,
-        Matrix<R, R> R) {
-    final var discR = Discretization.discretizeR(R, m_dtSeconds);
-
-    // Transform sigma points into measurement space
-    Matrix<R, ?> sigmasH = new Matrix<>(new SimpleMatrix(
-          rows.getNum(), 2 * m_states.getNum() + 1));
-    var sigmas = m_pts.sigmaPoints(m_xHat, m_P);
-    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
-      Matrix<R, N1> hRet = h.apply(
-            sigmas.extractColumnVector(i),
-            u
-      );
-      sigmasH.setColumn(i, hRet);
-    }
-
-    // Mean and covariance of prediction passed through unscented transform
-    var transRet = unscentedTransform(m_states, rows, sigmasH, m_pts.getWm(), m_pts.getWc());
-    var yHat = transRet.getFirst();
-    var Py = transRet.getSecond().plus(discR);
-
-    // Compute cross covariance of the state and the measurements
-    Matrix<States, R> Pxy = new Matrix<>(m_states, rows);
-    for (int i = 0; i < m_pts.getNumSigmas(); i++) {
-      var temp =
-            m_sigmasF.extractColumnVector(i).minus(m_xHat)
-                    .times(sigmasH.extractColumnVector(i).minus(yHat).transpose());
-
-      Pxy = Pxy.plus(temp.times(m_pts.getWc(i)));
-    }
-
-    // K = P_{xy} Py^-1
-    // K^T = P_y^T^-1 P_{xy}^T
-    // P_y^T K^T = P_{xy}^T
-    // K^T = P_y^T.solve(P_{xy}^T)
-    // K = (P_y^T.solve(P_{xy}^T)^T
-    Matrix<States, R> K = new Matrix<>(
-          Py.transpose().solve(Pxy.transpose()).transpose()
-    );
-
-    m_xHat = m_xHat.plus(K.times(y.minus(yHat)));
-    m_P = m_P.minus(K.times(Py).times(K.transpose()));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
deleted file mode 100644
index a0e3b9a..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
+++ /dev/null
@@ -1,252 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-import com.fasterxml.jackson.annotation.JsonAutoDetect;
-import com.fasterxml.jackson.annotation.JsonCreator;
-import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
-import com.fasterxml.jackson.annotation.JsonProperty;
-
-/**
- * Represents a 2d pose containing translational and rotational elements.
- */
-@JsonIgnoreProperties(ignoreUnknown = true)
-@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Pose2d {
-  private final Translation2d m_translation;
-  private final Rotation2d m_rotation;
-
-  /**
-   * Constructs a pose at the origin facing toward the positive X axis.
-   * (Translation2d{0, 0} and Rotation{0})
-   */
-  public Pose2d() {
-    m_translation = new Translation2d();
-    m_rotation = new Rotation2d();
-  }
-
-  /**
-   * 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.
-   */
-  @JsonCreator
-  public Pose2d(@JsonProperty(required = true, value = "translation") Translation2d translation,
-                @JsonProperty(required = true, value = "rotation") Rotation2d rotation) {
-    m_translation = translation;
-    m_rotation = 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.
-   */
-  @SuppressWarnings("ParameterName")
-  public Pose2d(double x, double y, Rotation2d rotation) {
-    m_translation = new Translation2d(x, y);
-    m_rotation = rotation;
-  }
-
-  /**
-   * Transforms the pose by the given transformation and returns the new
-   * transformed pose.
-   *
-   * <p>The matrix multiplication is as follows
-   * [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.
-   */
-  public Pose2d plus(Transform2d other) {
-    return transformBy(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.
-   */
-  public Transform2d minus(Pose2d other) {
-    final var pose = this.relativeTo(other);
-    return new Transform2d(pose.getTranslation(), pose.getRotation());
-  }
-
-  /**
-   * Returns the translation component of the transformation.
-   *
-   * @return The translational component of the pose.
-   */
-  @JsonProperty
-  public Translation2d getTranslation() {
-    return m_translation;
-  }
-
-  /**
-   * Returns the X component of the pose's translation.
-   *
-   * @return The x component of the pose's translation.
-   */
-  public double getX() {
-    return m_translation.getX();
-  }
-
-  /**
-   * Returns the Y component of the pose's translation.
-   *
-   * @return The y component of the pose's translation.
-   */
-  public double getY() {
-    return m_translation.getY();
-  }
-
-  /**
-   * Returns the rotational component of the transformation.
-   *
-   * @return The rotational component of the pose.
-   */
-  @JsonProperty
-  public Rotation2d getRotation() {
-    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.
-   */
-  public Pose2d transformBy(Transform2d other) {
-    return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
-        m_rotation.plus(other.getRotation()));
-  }
-
-  /**
-   * Returns the other pose relative to the current pose.
-   *
-   * <p>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.
-   */
-  public Pose2d relativeTo(Pose2d other) {
-    var transform = new Transform2d(other, this);
-    return new Pose2d(transform.getTranslation(), transform.getRotation());
-  }
-
-  /**
-   * Obtain a new Pose2d from a (constant curvature) velocity.
-   *
-   * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">
-   * Controls Engineering in the FIRST Robotics Competition</a> section 10.2 "Pose exponential" for
-   * a derivation.
-   *
-   * <p>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.
-   *
-   * <p>"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.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public Pose2d exp(Twist2d twist) {
-    double dx = twist.dx;
-    double dy = twist.dy;
-    double dtheta = twist.dtheta;
-
-    double sinTheta = Math.sin(dtheta);
-    double cosTheta = Math.cos(dtheta);
-
-    double s;
-    double c;
-    if (Math.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;
-    }
-    var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
-        new Rotation2d(cosTheta, sinTheta));
-
-    return this.plus(transform);
-  }
-
-  /**
-   * 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.
-   */
-  public Twist2d log(Pose2d end) {
-    final var transform = end.relativeTo(this);
-    final var dtheta = transform.getRotation().getRadians();
-    final var halfDtheta = dtheta / 2.0;
-
-    final var cosMinusOne = transform.getRotation().getCos() - 1;
-
-    double halfThetaByTanOfHalfDtheta;
-    if (Math.abs(cosMinusOne) < 1E-9) {
-      halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
-    } else {
-      halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
-    }
-
-    Translation2d translationPart = transform.getTranslation().rotateBy(
-        new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
-    ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
-
-    return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
-  }
-
-  /**
-   * Checks equality between this Pose2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Pose2d) {
-      return ((Pose2d) obj).m_translation.equals(m_translation)
-          && ((Pose2d) obj).m_rotation.equals(m_rotation);
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_translation, m_rotation);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
deleted file mode 100644
index e1c25eb..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
+++ /dev/null
@@ -1,215 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-import com.fasterxml.jackson.annotation.JsonAutoDetect;
-import com.fasterxml.jackson.annotation.JsonCreator;
-import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
-import com.fasterxml.jackson.annotation.JsonProperty;
-
-/**
- * A rotation in a 2d coordinate frame represented a point on the unit circle
- * (cosine and sine).
- */
-@JsonIgnoreProperties(ignoreUnknown = true)
-@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Rotation2d {
-  private final double m_value;
-  private final double m_cos;
-  private final double m_sin;
-
-  /**
-   * Constructs a Rotation2d with a default angle of 0 degrees.
-   */
-  public Rotation2d() {
-    m_value = 0.0;
-    m_cos = 1.0;
-    m_sin = 0.0;
-  }
-
-  /**
-   * Constructs a Rotation2d with the given radian value.
-   * The x and y don't have to be normalized.
-   *
-   * @param value The value of the angle in radians.
-   */
-  @JsonCreator
-  public Rotation2d(@JsonProperty(required = true, value = "radians") double value) {
-    m_value = value;
-    m_cos = Math.cos(value);
-    m_sin = Math.sin(value);
-  }
-
-  /**
-   * Constructs a Rotation2d with the given x and y (cosine and sine)
-   * components.
-   *
-   * @param x The x component or cosine of the rotation.
-   * @param y The y component or sine of the rotation.
-   */
-  @SuppressWarnings("ParameterName")
-  public Rotation2d(double x, double y) {
-    double magnitude = Math.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 = Math.atan2(m_sin, m_cos);
-  }
-
-  /**
-   * Constructs and returns a Rotation2d with the given degree value.
-   *
-   * @param degrees The value of the angle in degrees.
-   * @return The rotation object with the desired angle value.
-   */
-  public static Rotation2d fromDegrees(double degrees) {
-    return new Rotation2d(Math.toRadians(degrees));
-  }
-
-  /**
-   * Adds two rotations together, with the result being bounded between -pi and
-   * pi.
-   *
-   * <p>For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to add.
-   * @return The sum of the two rotations.
-   */
-  public Rotation2d plus(Rotation2d other) {
-    return rotateBy(other);
-  }
-
-  /**
-   * Subtracts the new rotation from the current rotation and returns the new
-   * rotation.
-   *
-   * <p>For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
-   * Rotation2d{-pi/2}
-   *
-   * @param other The rotation to subtract.
-   * @return The difference between the two rotations.
-   */
-  public Rotation2d minus(Rotation2d other) {
-    return rotateBy(other.unaryMinus());
-  }
-
-  /**
-   * Takes the inverse of the current rotation. This is simply the negative of
-   * the current angular value.
-   *
-   * @return The inverse of the current rotation.
-   */
-  public Rotation2d unaryMinus() {
-    return new Rotation2d(-m_value);
-  }
-
-  /**
-   * Multiplies the current rotation by a scalar.
-   *
-   * @param scalar The scalar.
-   * @return The new scaled Rotation2d.
-   */
-  public Rotation2d times(double scalar) {
-    return new Rotation2d(m_value * scalar);
-  }
-
-  /**
-   * Adds the new rotation to the current rotation using a rotation matrix.
-   *
-   * <p>The matrix multiplication is as follows:
-   * [cos_new]   [other.cos, -other.sin][cos]
-   * [sin_new] = [other.sin,  other.cos][sin]
-   * value_new = atan2(cos_new, sin_new)
-   *
-   * @param other The rotation to rotate by.
-   * @return The new rotated Rotation2d.
-   */
-  public Rotation2d rotateBy(Rotation2d other) {
-    return new Rotation2d(
-        m_cos * other.m_cos - m_sin * other.m_sin,
-        m_cos * other.m_sin + m_sin * other.m_cos
-    );
-  }
-
-  /**
-   * Returns the radian value of the rotation.
-   *
-   * @return The radian value of the rotation.
-   */
-  @JsonProperty
-  public double getRadians() {
-    return m_value;
-  }
-
-  /**
-   * Returns the degree value of the rotation.
-   *
-   * @return The degree value of the rotation.
-   */
-  public double getDegrees() {
-    return Math.toDegrees(m_value);
-  }
-
-  /**
-   * Returns the cosine of the rotation.
-   *
-   * @return The cosine of the rotation.
-   */
-  public double getCos() {
-    return m_cos;
-  }
-
-  /**
-   * Returns the sine of the rotation.
-   *
-   * @return The sine of the rotation.
-   */
-  public double getSin() {
-    return m_sin;
-  }
-
-  /**
-   * Returns the tangent of the rotation.
-   *
-   * @return The tangent of the rotation.
-   */
-  public double getTan() {
-    return m_sin / m_cos;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
-  }
-
-  /**
-   * Checks equality between this Rotation2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Rotation2d) {
-      return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_value);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
deleted file mode 100644
index 16746d5..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
+++ /dev/null
@@ -1,137 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-/**
- * Represents a transformation for a Pose2d.
- */
-public class Transform2d {
-  private final Translation2d m_translation;
-  private final Rotation2d m_rotation;
-
-  /**
-   * Constructs the transform that maps the initial pose to the final pose.
-   *
-   * @param initial The initial pose for the transformation.
-   * @param last    The final pose for the transformation.
-   */
-  public Transform2d(Pose2d initial, Pose2d last) {
-    // 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 = last.getTranslation().minus(initial.getTranslation())
-            .rotateBy(initial.getRotation().unaryMinus());
-
-    m_rotation = last.getRotation().minus(initial.getRotation());
-  }
-
-  /**
-   * Constructs a transform with the given translation and rotation components.
-   *
-   * @param translation Translational component of the transform.
-   * @param rotation    Rotational component of the transform.
-   */
-  public Transform2d(Translation2d translation, Rotation2d rotation) {
-    m_translation = translation;
-    m_rotation = rotation;
-  }
-
-  /**
-   * Constructs the identity transform -- maps an initial pose to itself.
-   */
-  public Transform2d() {
-    m_translation = new Translation2d();
-    m_rotation = new Rotation2d();
-  }
-
-  /**
-   * Scales the transform by the scalar.
-   *
-   * @param scalar The scalar.
-   * @return The scaled Transform2d.
-   */
-  public Transform2d times(double scalar) {
-    return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
-  }
-
-  /**
-   * Returns the translation component of the transformation.
-   *
-   * @return The translational component of the transform.
-   */
-  public Translation2d getTranslation() {
-    return m_translation;
-  }
-
-  /**
-   * Returns the X component of the transformation's translation.
-   *
-   * @return The x component of the transformation's translation.
-   */
-  public double getX() {
-    return m_translation.getX();
-  }
-
-  /**
-   * Returns the Y component of the transformation's translation.
-   *
-   * @return The y component of the transformation's translation.
-   */
-  public double getY() {
-    return m_translation.getY();
-  }
-
-  /**
-   * Returns the rotational component of the transformation.
-   *
-   * @return Reference to the rotational component of the transform.
-   */
-  public Rotation2d getRotation() {
-    return m_rotation;
-  }
-
-  /**
-   * Invert the transformation. This is useful for undoing a transformation.
-   *
-   * @return The inverted transformation.
-   */
-  public Transform2d inverse() {
-    // 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).
-    return new Transform2d(getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
-        getRotation().unaryMinus());
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
-  }
-
-  /**
-   * Checks equality between this Transform2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Transform2d) {
-      return ((Transform2d) obj).m_translation.equals(m_translation)
-          && ((Transform2d) obj).m_rotation.equals(m_rotation);
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_translation, m_rotation);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
deleted file mode 100644
index 5365759..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
+++ /dev/null
@@ -1,215 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-import com.fasterxml.jackson.annotation.JsonAutoDetect;
-import com.fasterxml.jackson.annotation.JsonCreator;
-import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
-import com.fasterxml.jackson.annotation.JsonProperty;
-
-/**
- * Represents a translation in 2d space.
- * This object can be used to represent a point or a vector.
- *
- * <p>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.
- */
-@SuppressWarnings({"ParameterName", "MemberName"})
-@JsonIgnoreProperties(ignoreUnknown = true)
-@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
-public class Translation2d {
-  private final double m_x;
-  private final double m_y;
-
-  /**
-   * Constructs a Translation2d with X and Y components equal to zero.
-   */
-  public Translation2d() {
-    this(0.0, 0.0);
-  }
-
-  /**
-   * 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.
-   */
-  @JsonCreator
-  public Translation2d(@JsonProperty(required = true, value = "x") double x,
-                       @JsonProperty(required = true, value = "y") double y) {
-    m_x = x;
-    m_y = y;
-  }
-
-  /**
-   * Constructs a Translation2d with the provided distance and angle. This is
-   * essentially converting from polar coordinates to Cartesian coordinates.
-   *
-   * @param distance The distance from the origin to the end of the translation.
-   * @param angle    The angle between the x-axis and the translation vector.
-   */
-  public Translation2d(double distance, Rotation2d angle) {
-    m_x = distance * angle.getCos();
-    m_y = distance * angle.getSin();
-  }
-
-  /**
-   * Calculates the distance between two translations in 2d space.
-   *
-   * <p>This function uses the pythagorean theorem to calculate the distance.
-   * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
-   *
-   * @param other The translation to compute the distance to.
-   * @return The distance between the two translations.
-   */
-  public double getDistance(Translation2d other) {
-    return Math.hypot(other.m_x - m_x, other.m_y - m_y);
-  }
-
-  /**
-   * Returns the X component of the translation.
-   *
-   * @return The x component of the translation.
-   */
-  @JsonProperty
-  public double getX() {
-    return m_x;
-  }
-
-  /**
-   * Returns the Y component of the translation.
-   *
-   * @return The y component of the translation.
-   */
-  @JsonProperty
-  public double getY() {
-    return m_y;
-  }
-
-  /**
-   * Returns the norm, or distance from the origin to the translation.
-   *
-   * @return The norm of the translation.
-   */
-  public double getNorm() {
-    return Math.hypot(m_x, m_y);
-  }
-
-  /**
-   * Applies a rotation to the translation in 2d space.
-   *
-   * <p>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]
-   *
-   * <p>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.
-   */
-  public Translation2d rotateBy(Rotation2d other) {
-    return new Translation2d(
-            m_x * other.getCos() - m_y * other.getSin(),
-            m_x * other.getSin() + m_y * other.getCos()
-    );
-  }
-
-  /**
-   * Adds two translations in 2d space and returns the sum. This is similar to
-   * vector addition.
-   *
-   * <p>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.
-   */
-  public Translation2d plus(Translation2d other) {
-    return new Translation2d(m_x + other.m_x, m_y + other.m_y);
-  }
-
-  /**
-   * Subtracts the other translation from the other translation and returns the
-   * difference.
-   *
-   * <p>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.
-   */
-  public Translation2d minus(Translation2d other) {
-    return new Translation2d(m_x - other.m_x, m_y - other.m_y);
-  }
-
-  /**
-   * 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.
-   */
-  public Translation2d unaryMinus() {
-    return new Translation2d(-m_x, -m_y);
-  }
-
-  /**
-   * Multiplies the translation by a scalar and returns the new translation.
-   *
-   * <p>For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
-   *
-   * @param scalar The scalar to multiply by.
-   * @return The scaled translation.
-   */
-  public Translation2d times(double scalar) {
-    return new Translation2d(m_x * scalar, m_y * scalar);
-  }
-
-  /**
-   * Divides the translation by a scalar and returns the new translation.
-   *
-   * <p>For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
-   *
-   * @param scalar The scalar to multiply by.
-   * @return The reference to the new mutated object.
-   */
-  public Translation2d div(double scalar) {
-    return new Translation2d(m_x / scalar, m_y / scalar);
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
-  }
-
-  /**
-   * Checks equality between this Translation2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Translation2d) {
-      return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
-          && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_x, m_y);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
deleted file mode 100644
index 1482902..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
+++ /dev/null
@@ -1,76 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import java.util.Objects;
-
-/**
- * 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.
- *
- * <p>A Twist can be used to represent a difference between two poses.
- */
-@SuppressWarnings("MemberName")
-public class Twist2d {
-  /**
-   * Linear "dx" component.
-   */
-  public double dx;
-
-  /**
-   * Linear "dy" component.
-   */
-  public double dy;
-
-  /**
-   * Angular "dtheta" component (radians).
-   */
-  public double dtheta;
-
-  public Twist2d() {
-  }
-
-  /**
-   * Constructs a Twist2d with the given values.
-   * @param dx Change in x direction relative to robot.
-   * @param dy Change in y direction relative to robot.
-   * @param dtheta Change in angle relative to robot.
-   */
-  public Twist2d(double dx, double dy, double dtheta) {
-    this.dx = dx;
-    this.dy = dy;
-    this.dtheta = dtheta;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
-  }
-
-  /**
-   * Checks equality between this Twist2d and another object.
-   *
-   * @param obj The other object.
-   * @return Whether the two objects are equal or not.
-   */
-  @Override
-  public boolean equals(Object obj) {
-    if (obj instanceof Twist2d) {
-      return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
-          && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
-          && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
-    }
-    return false;
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(dx, dy, dtheta);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
deleted file mode 100644
index a6878b3..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeeds.java
+++ /dev/null
@@ -1,91 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-/**
- * 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.
- *
- * <p>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.
- */
-@SuppressWarnings("MemberName")
-public class ChassisSpeeds {
-  /**
-   * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
-   */
-  public double vxMetersPerSecond;
-
-  /**
-   * Represents sideways velocity w.r.t the robot frame of reference. (Left is +)
-   */
-  public double vyMetersPerSecond;
-
-  /**
-   * Represents the angular velocity of the robot frame. (CCW is +)
-   */
-  public double omegaRadiansPerSecond;
-
-  /**
-   * Constructs a ChassisSpeeds with zeros for dx, dy, and theta.
-   */
-  public ChassisSpeeds() {
-  }
-
-  /**
-   * Constructs a ChassisSpeeds object.
-   *
-   * @param vxMetersPerSecond     Forward velocity.
-   * @param vyMetersPerSecond     Sideways velocity.
-   * @param omegaRadiansPerSecond Angular velocity.
-   */
-  public ChassisSpeeds(double vxMetersPerSecond, double vyMetersPerSecond,
-                       double omegaRadiansPerSecond) {
-    this.vxMetersPerSecond = vxMetersPerSecond;
-    this.vyMetersPerSecond = vyMetersPerSecond;
-    this.omegaRadiansPerSecond = omegaRadiansPerSecond;
-  }
-
-  /**
-   * Converts a user provided field-relative set of speeds into a robot-relative
-   * ChassisSpeeds object.
-   *
-   * @param vxMetersPerSecond     The component of speed in the x direction relative to the field.
-   *                              Positive x is away from your alliance wall.
-   * @param vyMetersPerSecond     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 omegaRadiansPerSecond 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.
-   */
-  public static ChassisSpeeds fromFieldRelativeSpeeds(
-      double vxMetersPerSecond, double vyMetersPerSecond,
-      double omegaRadiansPerSecond, Rotation2d robotAngle) {
-    return new ChassisSpeeds(
-        vxMetersPerSecond * robotAngle.getCos() + vyMetersPerSecond * robotAngle.getSin(),
-        -vxMetersPerSecond * robotAngle.getSin() + vyMetersPerSecond * robotAngle.getCos(),
-        omegaRadiansPerSecond
-    );
-  }
-
-  @Override
-  public String toString() {
-    return String.format("ChassisSpeeds(Vx: %.2f m/s, Vy: %.2f m/s, Omega: %.2f rad/s)",
-        vxMetersPerSecond, vyMetersPerSecond, omegaRadiansPerSecond);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
deleted file mode 100644
index 309f531..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematics.java
+++ /dev/null
@@ -1,69 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-
-/**
- * Helper class that converts a chassis velocity (dx and dtheta components) to
- * left and right wheel velocities for a differential drive.
- *
- * <p>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.
- */
-@SuppressWarnings("MemberName")
-public class DifferentialDriveKinematics {
-  public final double trackWidthMeters;
-
-  /**
-   * Constructs a differential drive kinematics object.
-   *
-   * @param trackWidthMeters 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.
-   */
-  public DifferentialDriveKinematics(double trackWidthMeters) {
-    this.trackWidthMeters = trackWidthMeters;
-    MathSharedStore.reportUsage(MathUsageId.kKinematics_DifferentialDrive, 1);
-  }
-
-  /**
-   * Returns a chassis speed from left and right component velocities using
-   * forward kinematics.
-   *
-   * @param wheelSpeeds The left and right velocities.
-   * @return The chassis speed.
-   */
-  public ChassisSpeeds toChassisSpeeds(DifferentialDriveWheelSpeeds wheelSpeeds) {
-    return new ChassisSpeeds(
-        (wheelSpeeds.leftMetersPerSecond + wheelSpeeds.rightMetersPerSecond) / 2, 0,
-        (wheelSpeeds.rightMetersPerSecond - wheelSpeeds.leftMetersPerSecond)
-            / trackWidthMeters
-    );
-  }
-
-  /**
-   * 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.
-   */
-  public DifferentialDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
-    return new DifferentialDriveWheelSpeeds(
-        chassisSpeeds.vxMetersPerSecond - trackWidthMeters / 2
-          * chassisSpeeds.omegaRadiansPerSecond,
-        chassisSpeeds.vxMetersPerSecond + trackWidthMeters / 2
-          * chassisSpeeds.omegaRadiansPerSecond
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
deleted file mode 100644
index 86470f8..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometry.java
+++ /dev/null
@@ -1,119 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-
-/**
- * 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.
- *
- * <p>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.
- *
- * <p>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.
- */
-public class DifferentialDriveOdometry {
-  private Pose2d m_poseMeters;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  private double m_prevLeftDistance;
-  private double m_prevRightDistance;
-
-  /**
-   * Constructs a DifferentialDriveOdometry object.
-   *
-   * @param gyroAngle         The angle reported by the gyroscope.
-   * @param initialPoseMeters The starting position of the robot on the field.
-   */
-  public DifferentialDriveOdometry(Rotation2d gyroAngle,
-                                   Pose2d initialPoseMeters) {
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    MathSharedStore.reportUsage(MathUsageId.kOdometry_DifferentialDrive, 1);
-  }
-
-  /**
-   * Constructs a DifferentialDriveOdometry object with the default pose at the origin.
-   *
-   * @param gyroAngle The angle reported by the gyroscope.
-   */
-  public DifferentialDriveOdometry(Rotation2d gyroAngle) {
-    this(gyroAngle, new Pose2d());
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>You NEED to reset your encoders (to zero) when calling this method.
-   *
-   * <p>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 poseMeters The position on the field that your robot is at.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-
-    m_prevLeftDistance = 0.0;
-    m_prevRightDistance = 0.0;
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-
-  /**
-   * 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 leftDistanceMeters  The distance traveled by the left encoder.
-   * @param rightDistanceMeters The distance traveled by the right encoder.
-   * @return The new pose of the robot.
-   */
-  public Pose2d update(Rotation2d gyroAngle, double leftDistanceMeters,
-                       double rightDistanceMeters) {
-    double deltaLeftDistance = leftDistanceMeters - m_prevLeftDistance;
-    double deltaRightDistance = rightDistanceMeters - m_prevRightDistance;
-
-    m_prevLeftDistance = leftDistanceMeters;
-    m_prevRightDistance = rightDistanceMeters;
-
-    double averageDeltaDistance = (deltaLeftDistance + deltaRightDistance) / 2.0;
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var newPose = m_poseMeters.exp(
-        new Twist2d(averageDeltaDistance, 0.0, angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
deleted file mode 100644
index 1716430..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveWheelSpeeds.java
+++ /dev/null
@@ -1,68 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-/**
- * Represents the wheel speeds for a differential drive drivetrain.
- */
-@SuppressWarnings("MemberName")
-public class DifferentialDriveWheelSpeeds {
-  /**
-   * Speed of the left side of the robot.
-   */
-  public double leftMetersPerSecond;
-
-  /**
-   * Speed of the right side of the robot.
-   */
-  public double rightMetersPerSecond;
-
-  /**
-   * Constructs a DifferentialDriveWheelSpeeds with zeros for left and right speeds.
-   */
-  public DifferentialDriveWheelSpeeds() {
-  }
-
-  /**
-   * Constructs a DifferentialDriveWheelSpeeds.
-   *
-   * @param leftMetersPerSecond  The left speed.
-   * @param rightMetersPerSecond The right speed.
-   */
-  public DifferentialDriveWheelSpeeds(double leftMetersPerSecond, double rightMetersPerSecond) {
-    this.leftMetersPerSecond = leftMetersPerSecond;
-    this.rightMetersPerSecond = rightMetersPerSecond;
-  }
-
-  /**
-   * 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
-   */
-  public void normalize(double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = Math.max(Math.abs(leftMetersPerSecond), Math.abs(rightMetersPerSecond));
-
-    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      leftMetersPerSecond = leftMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      rightMetersPerSecond = rightMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-    }
-  }
-
-  @Override
-  public String toString() {
-    return String.format("DifferentialDriveWheelSpeeds(Left: %.2f m/s, Right: %.2f m/s)",
-        leftMetersPerSecond, rightMetersPerSecond);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
deleted file mode 100644
index 8c1d5d3..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematics.java
+++ /dev/null
@@ -1,172 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual wheel speeds.
- *
- * <p>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 maneuvers.
- *
- * <p>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.
- *
- * <p>The inverse kinematics: [wheelSpeeds] = [wheelLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [wheelLocations] and then
- * multiply by [wheelSpeeds] to get our chassis speeds.
- *
- * <p>Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-public class MecanumDriveKinematics {
-  private SimpleMatrix m_inverseKinematics;
-  private final SimpleMatrix m_forwardKinematics;
-
-  private final Translation2d m_frontLeftWheelMeters;
-  private final Translation2d m_frontRightWheelMeters;
-  private final Translation2d m_rearLeftWheelMeters;
-  private final Translation2d m_rearRightWheelMeters;
-
-  private Translation2d m_prevCoR = new Translation2d();
-
-  /**
-   * Constructs a mecanum drive kinematics object.
-   *
-   * @param frontLeftWheelMeters  The location of the front-left wheel relative to the
-   *                              physical center of the robot.
-   * @param frontRightWheelMeters The location of the front-right wheel relative to
-   *                              the physical center of the robot.
-   * @param rearLeftWheelMeters   The location of the rear-left wheel relative to the
-   *                              physical center of the robot.
-   * @param rearRightWheelMeters  The location of the rear-right wheel relative to the
-   *                              physical center of the robot.
-   */
-  public MecanumDriveKinematics(Translation2d frontLeftWheelMeters,
-                                Translation2d frontRightWheelMeters,
-                                Translation2d rearLeftWheelMeters,
-                                Translation2d rearRightWheelMeters) {
-    m_frontLeftWheelMeters = frontLeftWheelMeters;
-    m_frontRightWheelMeters = frontRightWheelMeters;
-    m_rearLeftWheelMeters = rearLeftWheelMeters;
-    m_rearRightWheelMeters = rearRightWheelMeters;
-
-    m_inverseKinematics = new SimpleMatrix(4, 3);
-
-    setInverseKinematics(frontLeftWheelMeters, frontRightWheelMeters,
-        rearLeftWheelMeters, rearRightWheelMeters);
-    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
-
-    MathSharedStore.reportUsage(MathUsageId.kKinematics_MecanumDrive, 1);
-  }
-
-  /**
-   * 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.
-   *
-   * <p>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
-   * maneuvers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds    The desired chassis speed.
-   * @param centerOfRotationMeters 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 {@link MecanumDriveWheelSpeeds#normalize(double)} function to rectify this issue.
-   */
-  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds,
-                                               Translation2d centerOfRotationMeters) {
-    // We have a new center of rotation. We need to compute the matrix again.
-    if (!centerOfRotationMeters.equals(m_prevCoR)) {
-      var fl = m_frontLeftWheelMeters.minus(centerOfRotationMeters);
-      var fr = m_frontRightWheelMeters.minus(centerOfRotationMeters);
-      var rl = m_rearLeftWheelMeters.minus(centerOfRotationMeters);
-      var rr = m_rearRightWheelMeters.minus(centerOfRotationMeters);
-
-      setInverseKinematics(fl, fr, rl, rr);
-      m_prevCoR = centerOfRotationMeters;
-    }
-
-    var chassisSpeedsVector = new SimpleMatrix(3, 1);
-    chassisSpeedsVector.setColumn(0, 0,
-        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
-        chassisSpeeds.omegaRadiansPerSecond);
-
-    var wheelsMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-    return new MecanumDriveWheelSpeeds(
-        wheelsMatrix.get(0, 0),
-        wheelsMatrix.get(1, 0),
-        wheelsMatrix.get(2, 0),
-        wheelsMatrix.get(3, 0)
-    );
-  }
-
-  /**
-   * Performs inverse kinematics. See {@link #toWheelSpeeds(ChassisSpeeds, Translation2d)} for more
-   * information.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @return The wheel speeds.
-   */
-  public MecanumDriveWheelSpeeds toWheelSpeeds(ChassisSpeeds chassisSpeeds) {
-    return toWheelSpeeds(chassisSpeeds, new 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.
-   */
-  public ChassisSpeeds toChassisSpeeds(MecanumDriveWheelSpeeds wheelSpeeds) {
-    var wheelSpeedsMatrix = new SimpleMatrix(4, 1);
-    wheelSpeedsMatrix.setColumn(0, 0,
-        wheelSpeeds.frontLeftMetersPerSecond, wheelSpeeds.frontRightMetersPerSecond,
-        wheelSpeeds.rearLeftMetersPerSecond, wheelSpeeds.rearRightMetersPerSecond
-    );
-    var chassisSpeedsVector = m_forwardKinematics.mult(wheelSpeedsMatrix);
-
-    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
-        chassisSpeedsVector.get(2, 0));
-  }
-
-  /**
-   * 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.
-   */
-  private void setInverseKinematics(Translation2d fl, Translation2d fr,
-                                    Translation2d rl, Translation2d rr) {
-    m_inverseKinematics.setRow(0, 0, 1, -1, -(fl.getX() + fl.getY()));
-    m_inverseKinematics.setRow(1, 0, 1, 1, fr.getX() - fr.getY());
-    m_inverseKinematics.setRow(2, 0, 1, 1, rl.getX() - rl.getY());
-    m_inverseKinematics.setRow(3, 0, 1, -1, -(rr.getX() + rr.getY()));
-    m_inverseKinematics = m_inverseKinematics.scale(1.0 / Math.sqrt(2));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
deleted file mode 100644
index 756bb60..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveMotorVoltages.java
+++ /dev/null
@@ -1,65 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-/**
- * Represents the motor voltages for a mecanum drive drivetrain.
- */
-@SuppressWarnings("MemberName")
-public class MecanumDriveMotorVoltages {
-  /**
-   * Voltage of the front left motor.
-   */
-  public double frontLeftVoltage;
-
-  /**
-   * Voltage of the front right motor.
-   */
-  public double frontRightVoltage;
-
-  /**
-   * Voltage of the rear left motor.
-   */
-  public double rearLeftVoltage;
-
-  /**
-   * Voltage of the rear right motor.
-   */
-  public double rearRightVoltage;
-
-  /**
-   * Constructs a MecanumDriveMotorVoltages with zeros for all member fields.
-   */
-  public MecanumDriveMotorVoltages() {
-  }
-
-  /**
-   * Constructs a MecanumDriveMotorVoltages.
-   *
-   * @param frontLeftVoltage  Voltage of the front left motor.
-   * @param frontRightVoltage Voltage of the front right motor.
-   * @param rearLeftVoltage   Voltage of the rear left motor.
-   * @param rearRightVoltage  Voltage of the rear right motor.
-   */
-  public MecanumDriveMotorVoltages(double frontLeftVoltage,
-                                 double frontRightVoltage,
-                                 double rearLeftVoltage,
-                                 double rearRightVoltage) {
-    this.frontLeftVoltage = frontLeftVoltage;
-    this.frontRightVoltage = frontRightVoltage;
-    this.rearLeftVoltage = rearLeftVoltage;
-    this.rearRightVoltage = rearRightVoltage;
-  }
-
-  @Override
-  public String toString() {
-    return String.format("MecanumDriveMotorVoltages(Front Left: %.2f V, Front Right: %.2f V, "
-            + "Rear Left: %.2f V, Rear Right: %.2f V)", frontLeftVoltage, frontRightVoltage,
-        rearLeftVoltage, rearRightVoltage);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
deleted file mode 100644
index cd84bdf..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometry.java
+++ /dev/null
@@ -1,132 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-import edu.wpi.first.wpiutil.WPIUtilJNI;
-
-/**
- * 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.
- *
- * <p>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.
- */
-public class MecanumDriveOdometry {
-  private final MecanumDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  /**
-   * Constructs a MecanumDriveOdometry object.
-   *
-   * @param kinematics        The mecanum drive kinematics for your drivetrain.
-   * @param gyroAngle         The angle reported by the gyroscope.
-   * @param initialPoseMeters The starting position of the robot on the field.
-   */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle,
-                              Pose2d initialPoseMeters) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPoseMeters;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPoseMeters.getRotation();
-    MathSharedStore.reportUsage(MathUsageId.kOdometry_MecanumDrive, 1);
-  }
-
-  /**
-   * Constructs a MecanumDriveOdometry object with the default pose at the origin.
-   *
-   * @param kinematics The mecanum drive kinematics for your drivetrain.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public MecanumDriveOdometry(MecanumDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>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 poseMeters The position on the field that your robot is at.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public void resetPosition(Pose2d poseMeters, Rotation2d gyroAngle) {
-    m_poseMeters = poseMeters;
-    m_previousAngle = poseMeters.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * 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 currentTimeSeconds The current time in seconds.
-   * @param gyroAngle          The angle reported by the gyroscope.
-   * @param wheelSpeeds        The current wheel speeds.
-   * @return The new pose of the robot.
-   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
-                               MecanumDriveWheelSpeeds wheelSpeeds) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(wheelSpeeds);
-    var newPose = m_poseMeters.exp(
-        new Twist2d(chassisState.vxMetersPerSecond * period,
-            chassisState.vyMetersPerSecond * period,
-            angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-    return m_poseMeters;
-  }
-
-  /**
-   * 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.
-   */
-  public Pose2d update(Rotation2d gyroAngle,
-                       MecanumDriveWheelSpeeds wheelSpeeds) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle,
-        wheelSpeeds);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
deleted file mode 100644
index f00e409..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveWheelSpeeds.java
+++ /dev/null
@@ -1,91 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import java.util.stream.DoubleStream;
-
-@SuppressWarnings("MemberName")
-public class MecanumDriveWheelSpeeds {
-  /**
-   * Speed of the front left wheel.
-   */
-  public double frontLeftMetersPerSecond;
-
-  /**
-   * Speed of the front right wheel.
-   */
-  public double frontRightMetersPerSecond;
-
-  /**
-   * Speed of the rear left wheel.
-   */
-  public double rearLeftMetersPerSecond;
-
-  /**
-   * Speed of the rear right wheel.
-   */
-  public double rearRightMetersPerSecond;
-
-  /**
-   * Constructs a MecanumDriveWheelSpeeds with zeros for all member fields.
-   */
-  public MecanumDriveWheelSpeeds() {
-  }
-
-  /**
-   * Constructs a MecanumDriveWheelSpeeds.
-   *
-   * @param frontLeftMetersPerSecond  Speed of the front left wheel.
-   * @param frontRightMetersPerSecond Speed of the front right wheel.
-   * @param rearLeftMetersPerSecond   Speed of the rear left wheel.
-   * @param rearRightMetersPerSecond  Speed of the rear right wheel.
-   */
-  public MecanumDriveWheelSpeeds(double frontLeftMetersPerSecond,
-                                 double frontRightMetersPerSecond,
-                                 double rearLeftMetersPerSecond,
-                                 double rearRightMetersPerSecond) {
-    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
-    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
-    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
-    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
-  }
-
-  /**
-   * 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
-   */
-  public void normalize(double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = DoubleStream.of(frontLeftMetersPerSecond,
-        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond)
-        .max().getAsDouble();
-
-    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      frontLeftMetersPerSecond = frontLeftMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      frontRightMetersPerSecond = frontRightMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      rearLeftMetersPerSecond = rearLeftMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-      rearRightMetersPerSecond = rearRightMetersPerSecond / realMaxSpeed
-          * attainableMaxSpeedMetersPerSecond;
-    }
-  }
-
-  @Override
-  public String toString() {
-    return String.format("MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
-            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)", frontLeftMetersPerSecond,
-        frontRightMetersPerSecond, rearLeftMetersPerSecond, rearRightMetersPerSecond);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
deleted file mode 100644
index a1dba43..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematics.java
+++ /dev/null
@@ -1,199 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import java.util.Arrays;
-import java.util.Collections;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * Helper class that converts a chassis velocity (dx, dy, and dtheta components)
- * into individual module states (speed and angle).
- *
- * <p>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 maneuvers.
- *
- * <p>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.
- *
- * <p>The inverse kinematics: [moduleStates] = [moduleLocations] * [chassisSpeeds]
- * We take the Moore-Penrose pseudoinverse of [moduleLocations] and then
- * multiply by [moduleStates] to get our chassis speeds.
- *
- * <p>Forward kinematics is also used for odometry -- determining the position of
- * the robot on the field using encoders and a gyro.
- */
-public class SwerveDriveKinematics {
-  private final SimpleMatrix m_inverseKinematics;
-  private final SimpleMatrix m_forwardKinematics;
-
-  private final int m_numModules;
-  private final Translation2d[] m_modules;
-  private Translation2d m_prevCoR = new Translation2d();
-
-  /**
-   * 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 receive 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 wheelsMeters The locations of the wheels relative to the physical center
-   *                     of the robot.
-   */
-  public SwerveDriveKinematics(Translation2d... wheelsMeters) {
-    if (wheelsMeters.length < 2) {
-      throw new IllegalArgumentException("A swerve drive requires at least two modules");
-    }
-    m_numModules = wheelsMeters.length;
-    m_modules = Arrays.copyOf(wheelsMeters, m_numModules);
-    m_inverseKinematics = new SimpleMatrix(m_numModules * 2, 3);
-
-    for (int i = 0; i < m_numModules; i++) {
-      m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0, -m_modules[i].getY());
-      m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1, +m_modules[i].getX());
-    }
-    m_forwardKinematics = m_inverseKinematics.pseudoInverse();
-
-    MathSharedStore.reportUsage(MathUsageId.kKinematics_SwerveDrive, 1);
-  }
-
-  /**
-   * 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.
-   *
-   * <p>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
-   * maneuvers, vision alignment, or for any other use case, you can do so.
-   *
-   * @param chassisSpeeds    The desired chassis speed.
-   * @param centerOfRotationMeters 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
-   *          {@link #normalizeWheelSpeeds(SwerveModuleState[], double) normalizeWheelSpeeds}
-   *          function to rectify this issue.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds,
-                                                  Translation2d centerOfRotationMeters) {
-    if (!centerOfRotationMeters.equals(m_prevCoR)) {
-      for (int i = 0; i < m_numModules; i++) {
-        m_inverseKinematics.setRow(i * 2 + 0, 0, /* Start Data */ 1, 0,
-            -m_modules[i].getY() + centerOfRotationMeters.getY());
-        m_inverseKinematics.setRow(i * 2 + 1, 0, /* Start Data */ 0, 1,
-            +m_modules[i].getX() - centerOfRotationMeters.getX());
-      }
-      m_prevCoR = centerOfRotationMeters;
-    }
-
-    var chassisSpeedsVector = new SimpleMatrix(3, 1);
-    chassisSpeedsVector.setColumn(0, 0,
-        chassisSpeeds.vxMetersPerSecond, chassisSpeeds.vyMetersPerSecond,
-        chassisSpeeds.omegaRadiansPerSecond);
-
-    var moduleStatesMatrix = m_inverseKinematics.mult(chassisSpeedsVector);
-    SwerveModuleState[] moduleStates = new SwerveModuleState[m_numModules];
-
-    for (int i = 0; i < m_numModules; i++) {
-      double x = moduleStatesMatrix.get(i * 2, 0);
-      double y = moduleStatesMatrix.get(i * 2 + 1, 0);
-
-      double speed = Math.hypot(x, y);
-      Rotation2d angle = new Rotation2d(x, y);
-
-      moduleStates[i] = new SwerveModuleState(speed, angle);
-    }
-
-    return moduleStates;
-  }
-
-  /**
-   * Performs inverse kinematics. See {@link #toSwerveModuleStates(ChassisSpeeds, Translation2d)}
-   * toSwerveModuleStates for more information.
-   *
-   * @param chassisSpeeds The desired chassis speed.
-   * @return An array containing the module states.
-   */
-  public SwerveModuleState[] toSwerveModuleStates(ChassisSpeeds chassisSpeeds) {
-    return toSwerveModuleStates(chassisSpeeds, new 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.
-   */
-  public ChassisSpeeds toChassisSpeeds(SwerveModuleState... wheelStates) {
-    if (wheelStates.length != m_numModules) {
-      throw new IllegalArgumentException(
-          "Number of modules is not consistent with number of wheel locations provided in "
-              + "constructor"
-      );
-    }
-    var moduleStatesMatrix = new SimpleMatrix(m_numModules * 2, 1);
-
-    for (int i = 0; i < m_numModules; i++) {
-      var module = wheelStates[i];
-      moduleStatesMatrix.set(i * 2, 0, module.speedMetersPerSecond * module.angle.getCos());
-      moduleStatesMatrix.set(i * 2 + 1, module.speedMetersPerSecond * module.angle.getSin());
-    }
-
-    var chassisSpeedsVector = m_forwardKinematics.mult(moduleStatesMatrix);
-    return new ChassisSpeeds(chassisSpeedsVector.get(0, 0), chassisSpeedsVector.get(1, 0),
-        chassisSpeedsVector.get(2, 0));
-
-  }
-
-  /**
-   * 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 attainableMaxSpeedMetersPerSecond The absolute max speed that a module can reach.
-   */
-  public static void normalizeWheelSpeeds(SwerveModuleState[] moduleStates,
-                                          double attainableMaxSpeedMetersPerSecond) {
-    double realMaxSpeed = Collections.max(Arrays.asList(moduleStates)).speedMetersPerSecond;
-    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
-      for (SwerveModuleState moduleState : moduleStates) {
-        moduleState.speedMetersPerSecond = moduleState.speedMetersPerSecond / realMaxSpeed
-            * attainableMaxSpeedMetersPerSecond;
-      }
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
deleted file mode 100644
index 5b1f975..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometry.java
+++ /dev/null
@@ -1,135 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Twist2d;
-import edu.wpi.first.wpiutil.WPIUtilJNI;
-
-/**
- * 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.
- *
- * <p>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.
- */
-public class SwerveDriveOdometry {
-  private final SwerveDriveKinematics m_kinematics;
-  private Pose2d m_poseMeters;
-  private double m_prevTimeSeconds = -1;
-
-  private Rotation2d m_gyroOffset;
-  private Rotation2d m_previousAngle;
-
-  /**
-   * 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.
-   */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle,
-                             Pose2d initialPose) {
-    m_kinematics = kinematics;
-    m_poseMeters = initialPose;
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-    m_previousAngle = initialPose.getRotation();
-    MathSharedStore.reportUsage(MathUsageId.kOdometry_SwerveDrive, 1);
-  }
-
-  /**
-   * Constructs a SwerveDriveOdometry object with the default pose at the origin.
-   *
-   * @param kinematics The swerve drive kinematics for your drivetrain.
-   * @param gyroAngle  The angle reported by the gyroscope.
-   */
-  public SwerveDriveOdometry(SwerveDriveKinematics kinematics, Rotation2d gyroAngle) {
-    this(kinematics, gyroAngle, new Pose2d());
-  }
-
-  /**
-   * Resets the robot's position on the field.
-   *
-   * <p>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.
-   */
-  public void resetPosition(Pose2d pose, Rotation2d gyroAngle) {
-    m_poseMeters = pose;
-    m_previousAngle = pose.getRotation();
-    m_gyroOffset = m_poseMeters.getRotation().minus(gyroAngle);
-  }
-
-  /**
-   * Returns the position of the robot on the field.
-   *
-   * @return The pose of the robot (x and y are in meters).
-   */
-  public Pose2d getPoseMeters() {
-    return m_poseMeters;
-  }
-
-  /**
-   * 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 currentTimeSeconds The current time in seconds.
-   * @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.
-   */
-  public Pose2d updateWithTime(double currentTimeSeconds, Rotation2d gyroAngle,
-                               SwerveModuleState... moduleStates) {
-    double period = m_prevTimeSeconds >= 0 ? currentTimeSeconds - m_prevTimeSeconds : 0.0;
-    m_prevTimeSeconds = currentTimeSeconds;
-
-    var angle = gyroAngle.plus(m_gyroOffset);
-
-    var chassisState = m_kinematics.toChassisSpeeds(moduleStates);
-    var newPose = m_poseMeters.exp(
-        new Twist2d(chassisState.vxMetersPerSecond * period,
-            chassisState.vyMetersPerSecond * period,
-            angle.minus(m_previousAngle).getRadians()));
-
-    m_previousAngle = angle;
-    m_poseMeters = new Pose2d(newPose.getTranslation(), angle);
-
-    return m_poseMeters;
-  }
-
-  /**
-   * 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.
-   */
-  public Pose2d update(Rotation2d gyroAngle, SwerveModuleState... moduleStates) {
-    return updateWithTime(WPIUtilJNI.now() * 1.0e-6, gyroAngle, moduleStates);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
deleted file mode 100644
index f9570eb..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/kinematics/SwerveModuleState.java
+++ /dev/null
@@ -1,63 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-/**
- * Represents the state of one swerve module.
- */
-@SuppressWarnings("MemberName")
-public class SwerveModuleState implements Comparable<SwerveModuleState> {
-
-  /**
-   * Speed of the wheel of the module.
-   */
-  public double speedMetersPerSecond;
-
-  /**
-   * Angle of the module.
-   */
-  public Rotation2d angle = Rotation2d.fromDegrees(0);
-
-  /**
-   * Constructs a SwerveModuleState with zeros for speed and angle.
-   */
-  public SwerveModuleState() {
-  }
-
-  /**
-   * Constructs a SwerveModuleState.
-   *
-   * @param speedMetersPerSecond The speed of the wheel of the module.
-   * @param angle The angle of the module.
-   */
-  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
-    this.speedMetersPerSecond = speedMetersPerSecond;
-    this.angle = angle;
-  }
-
-  /**
-   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
-   * is higher than the other.
-   *
-   * @param o The other swerve module.
-   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
-   */
-  @Override
-  @SuppressWarnings("ParameterName")
-  public int compareTo(SwerveModuleState o) {
-    return Double.compare(this.speedMetersPerSecond, o.speedMetersPerSecond);
-  }
-
-  @Override
-  public String toString() {
-    return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond,
-        angle);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java
deleted file mode 100644
index ad9bf27..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/math/Discretization.java
+++ /dev/null
@@ -1,179 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.math;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.Pair;
-
-@SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-public final class Discretization {
-  private Discretization() {
-    // Utility class
-  }
-
-  /**
-   * Discretizes the given continuous A matrix.
-   *
-   * @param <States>       Num representing the number of states.
-   * @param contA     Continuous system matrix.
-   * @param dtSeconds Discretization timestep.
-   * @return the discrete matrix system.
-   */
-  public static <States extends Num> Matrix<States, States> discretizeA(
-      Matrix<States, States> contA, double dtSeconds) {
-    return contA.times(dtSeconds).exp();
-  }
-
-  /**
-   * Discretizes the given continuous A and B matrices.
-   *
-   * <p>Rather than solving a (States + Inputs) x (States + Inputs) matrix
-   * exponential like in DiscretizeAB(), we take advantage of the structure of the
-   * block matrix of A and B.
-   *
-   * <p>1) The exponential of A*t, which is only N x N, is relatively cheap.
-   * 2) The upper-right quarter of the (States + Inputs) x (States + Inputs)
-   * matrix, which we can approximate using a taylor series to several terms
-   * and still be substantially cheaper than taking the big exponential.
-   *
-   * @param states    Nat representing the states of the system.
-   * @param contA     Continuous system matrix.
-   * @param contB     Continuous input matrix.
-   * @param dtseconds Discretization timestep.
-   */
-  public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
-      Matrix<States, Inputs>>
-      discretizeABTaylor(Nat<States> states,
-                         Matrix<States, States> contA,
-                         Matrix<States, Inputs> contB,
-                         double dtseconds) {
-    Matrix<States, States> lastTerm = Matrix.eye(states);
-    double lastCoeff = dtseconds;
-
-    var phi12 = lastTerm.times(lastCoeff);
-
-    // i = 6 i.e. 5th order should be enough precision
-    for (int i = 2; i < 6; ++i) {
-      lastTerm = contA.times(lastTerm);
-      lastCoeff *= dtseconds / ((double) i);
-
-      phi12 = phi12.plus(lastTerm.times(lastCoeff));
-    }
-
-    var discB = phi12.times(contB);
-
-    var discA = discretizeA(contA, dtseconds);
-
-    return Pair.of(discA, discB);
-  }
-
-  /**
-   * Discretizes the given continuous A and Q matrices.
-   *
-   * <p>Rather than solving a 2N x 2N matrix exponential like in DiscretizeQ() (which
-   * is expensive), we take advantage of the structure of the block matrix of A
-   * and Q.
-   *
-   * <p>The exponential of A*t, which is only N x N, is relatively cheap.
-   * 2) The upper-right quarter of the 2N x 2N matrix, which we can approximate
-   * using a taylor series to several terms and still be substantially cheaper
-   * than taking the big exponential.
-   *
-   * @param <States>       Nat representing the number of states.
-   * @param contA     Continuous system matrix.
-   * @param contQ     Continuous process noise covariance matrix.
-   * @param dtSeconds Discretization timestep.
-   * @return a pair representing the discrete system matrix and process noise covariance matrix.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public static <States extends Num> Pair<Matrix<States, States>,
-            Matrix<States, States>> discretizeAQTaylor(Matrix<States, States> contA,
-                                                       Matrix<States, States> contQ,
-                                                       double dtSeconds) {
-    Matrix<States, States> Q = (contQ.plus(contQ.transpose())).div(2.0);
-
-
-    Matrix<States, States> lastTerm = Q.copy();
-    double lastCoeff = dtSeconds;
-
-    // A^T^n
-    Matrix<States, States> Atn = contA.transpose();
-    Matrix<States, States> phi12 = lastTerm.times(lastCoeff);
-
-    // i = 6 i.e. 6th order should be enough precision
-    for (int i = 2; i < 6; ++i) {
-      lastTerm = contA.times(-1).times(lastTerm).plus(Q.times(Atn));
-      lastCoeff *= dtSeconds / ((double) i);
-
-      phi12 = phi12.plus(lastTerm.times(lastCoeff));
-
-      Atn = Atn.times(contA.transpose());
-    }
-
-    var discA = discretizeA(contA, dtSeconds);
-    Q = discA.times(phi12);
-
-    // Make Q symmetric if it isn't already
-    var discQ = Q.plus(Q.transpose()).div(2.0);
-
-    return new Pair<>(discA, discQ);
-  }
-
-  /**
-   * Returns a discretized version of the provided continuous measurement noise
-   * covariance matrix. Note that dt=0.0 divides R by zero.
-   *
-   * @param <O>       Nat representing the number of outputs.
-   * @param R         Continuous measurement noise covariance matrix.
-   * @param dtSeconds Discretization timestep.
-   * @return Discretized version of the provided continuous measurement noise covariance matrix.
-   */
-  public static <O extends Num> Matrix<O, O> discretizeR(Matrix<O, O> R, double dtSeconds) {
-    return R.div(dtSeconds);
-  }
-
-  /**
-   * Discretizes the given continuous A and B matrices.
-   *
-   * @param <States>       Nat representing the states of the system.
-   * @param <Inputs>       Nat representing the inputs to the system.
-   * @param contA     Continuous system matrix.
-   * @param contB     Continuous input matrix.
-   * @param dtSeconds Discretization timestep.
-   * @return a Pair representing discA and diskB.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public static <States extends Num, Inputs extends Num> Pair<Matrix<States, States>,
-            Matrix<States, Inputs>> discretizeAB(
-                                     Matrix<States, States> contA,
-                                     Matrix<States, Inputs> contB,
-                                     double dtSeconds) {
-    var scaledA = contA.times(dtSeconds);
-    var scaledB = contB.times(dtSeconds);
-
-    var contSize = contB.getNumRows() + contB.getNumCols();
-    var Mcont = new Matrix<>(new SimpleMatrix(contSize, contSize));
-    Mcont.assignBlock(0, 0, scaledA);
-    Mcont.assignBlock(0, scaledA.getNumCols(), scaledB);
-    var Mdisc = Mcont.exp();
-
-    var discA = new Matrix<States, States>(new SimpleMatrix(contB.getNumRows(),
-        contB.getNumRows()));
-    var discB = new Matrix<States, Inputs>(new SimpleMatrix(contB.getNumRows(),
-        contB.getNumCols()));
-
-    discA.extractFrom(0, 0, Mdisc);
-    discB.extractFrom(0, contB.getNumRows(), Mdisc);
-
-    return new Pair<>(discA, discB);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java
deleted file mode 100644
index 02ca7c1..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/math/StateSpaceUtil.java
+++ /dev/null
@@ -1,180 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.math;
-
-import java.util.Random;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.math.WPIMathJNI;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpiutil.math.MathUtil;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N3;
-
-@SuppressWarnings("ParameterName")
-public final class StateSpaceUtil {
-  private StateSpaceUtil() {
-    // Utility class
-  }
-
-  /**
-   * Creates a covariance matrix from the given vector for use with Kalman
-   * filters.
-   *
-   * <p>Each element is squared and placed on the covariance matrix diagonal.
-   *
-   * @param <States>     Num representing the states of the system.
-   * @param states  A Nat representing the states of the system.
-   * @param stdDevs For a Q matrix, its elements are the standard deviations of
-   *                each state from how the model behaves. For an R matrix, its
-   *                elements are the standard deviations for each output
-   *                measurement.
-   * @return Process noise or measurement noise covariance matrix.
-   */
-  @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num> Matrix<States, States> makeCovarianceMatrix(
-      Nat<States> states, Matrix<States, N1> stdDevs
-  ) {
-    var result = new Matrix<>(states, states);
-    for (int i = 0; i < states.getNum(); i++) {
-      result.set(i, i, Math.pow(stdDevs.get(i, 0), 2));
-    }
-    return result;
-  }
-
-  /**
-   * Creates a vector of normally distributed white noise with the given noise
-   * intensities for each element.
-   *
-   * @param <N>     Num representing the dimensionality of  the noise vector to create.
-   * @param stdDevs A matrix whose elements are the standard deviations of each
-   *                element of the noise vector.
-   * @return White noise vector.
-   */
-  public static <N extends Num> Matrix<N, N1> makeWhiteNoiseVector(
-        Matrix<N, N1> stdDevs
-  ) {
-    var rand = new Random();
-
-    Matrix<N, N1> result = new Matrix<>(new SimpleMatrix(stdDevs.getNumRows(), 1));
-    for (int i = 0; i < stdDevs.getNumRows(); i++) {
-      result.set(i, 0, rand.nextGaussian() * stdDevs.get(i, 0));
-    }
-    return result;
-  }
-
-  /**
-   * Creates a cost matrix from the given vector for use with LQR.
-   *
-   * <p>The cost matrix is constructed using Bryson's rule. The inverse square of
-   * each element in the input is taken and placed on the cost matrix diagonal.
-   *
-   * @param <States>   Nat representing the states of the system.
-   * @param costs An array. For a Q matrix, its elements are the maximum allowed
-   *              excursions of the states from the reference. For an R matrix,
-   *              its elements are the maximum allowed excursions of the control
-   *              inputs from no actuation.
-   * @return State excursion or control effort cost matrix.
-   */
-  @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num> Matrix<States, States>
-      makeCostMatrix(Matrix<States, N1> costs) {
-    Matrix<States, States> result =
-        new Matrix<>(new SimpleMatrix(costs.getNumRows(), costs.getNumRows()));
-    result.fill(0.0);
-
-    for (int i = 0; i < costs.getNumRows(); i++) {
-      result.set(i, i, 1.0 / (Math.pow(costs.get(i, 0), 2)));
-    }
-
-    return result;
-  }
-
-  /**
-   * Returns true if (A, B) is a stabilizable pair.
-   *
-   * <p>(A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
-   * any, have absolute values less than one, where an eigenvalue is
-   * uncontrollable if rank(lambda * I - A, B) %3C n where n is number of states.
-   *
-   * @param <States> Num representing the size of A.
-   * @param <Inputs> Num representing the columns of B.
-   * @param A   System matrix.
-   * @param B   Input matrix.
-   * @return If the system is stabilizable.
-   */
-  @SuppressWarnings("MethodTypeParameterName")
-  public static <States extends Num, Inputs extends Num> boolean isStabilizable(
-      Matrix<States, States> A, Matrix<States, Inputs> B) {
-    return WPIMathJNI.isStabilizable(A.getNumRows(), B.getNumCols(),
-            A.getData(), B.getData());
-  }
-
-  /**
-   * Convert a {@link Pose2d} to a vector of [x, y, theta], where theta is in radians.
-   *
-   * @param pose A pose to convert to a vector.
-   * @return The given pose in vector form, with the third element, theta, in radians.
-   */
-  public static Matrix<N3, N1> poseToVector(Pose2d pose) {
-    return VecBuilder.fill(
-            pose.getX(),
-            pose.getY(),
-            pose.getRotation().getRadians()
-    );
-  }
-
-  /**
-   * Clamp the input u to the min and max.
-   *
-   * @param u    The input to clamp.
-   * @param umin The minimum input magnitude.
-   * @param umax The maximum input magnitude.
-   * @param <I>  The number of inputs.
-   * @return     The clamped input.
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public static <I extends Num> Matrix<I, N1> clampInputMaxMagnitude(Matrix<I, N1> u,
-                                                                     Matrix<I, N1> umin,
-                                                                     Matrix<I, N1> umax) {
-    var result = new Matrix<I, N1>(new SimpleMatrix(u.getNumRows(), 1));
-    for (int i = 0; i < u.getNumRows(); i++) {
-      result.set(i, 0, MathUtil.clamp(
-            u.get(i, 0),
-            umin.get(i, 0),
-            umax.get(i, 0)));
-    }
-    return result;
-  }
-
-  /**
-   * Normalize all inputs if any excedes the maximum magnitude. Useful for systems such as
-   * differential drivetrains.
-   *
-   * @param u            The input vector.
-   * @param maxMagnitude The maximum magnitude any input can have.
-   * @param <I>          The number of inputs.
-   * @return The normalizedInput
-   */
-  public static <I extends Num> Matrix<I, N1> normalizeInputVector(Matrix<I, N1> u,
-                                                                   double maxMagnitude) {
-    double maxValue = u.maxAbs();
-    boolean isCapped = maxValue > maxMagnitude;
-
-    if (isCapped) {
-      return u.times(maxMagnitude / maxValue);
-    }
-    return u;
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
deleted file mode 100644
index f387fc0..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/CubicHermiteSpline.java
+++ /dev/null
@@ -1,115 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import org.ejml.simple.SimpleMatrix;
-
-public class CubicHermiteSpline extends Spline {
-  private static SimpleMatrix hermiteBasis;
-  private final SimpleMatrix m_coefficients;
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  public CubicHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
-                            double[] yInitialControlVector, double[] yFinalControlVector) {
-    super(3);
-
-    // Populate the coefficients for the actual spline equations.
-    // Row 0 is x coefficients
-    // Row 1 is y coefficients
-    final var hermite = makeHermiteBasis();
-    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-    final var xCoeffs = (hermite.mult(x)).transpose();
-    final var yCoeffs = (hermite.mult(y)).transpose();
-
-    m_coefficients = new SimpleMatrix(6, 4);
-
-    for (int i = 0; i < 4; i++) {
-      m_coefficients.set(0, i, xCoeffs.get(0, i));
-      m_coefficients.set(1, i, yCoeffs.get(0, i));
-
-      // Populate Row 2 and Row 3 with the derivatives of the equations above.
-      // Then populate row 4 and 5 with the second derivatives.
-      // 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.set(2, i, m_coefficients.get(0, i) * (3 - i));
-      m_coefficients.set(3, i, m_coefficients.get(1, 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.set(4, i, m_coefficients.get(2, i) * (2 - i));
-      m_coefficients.set(5, i, m_coefficients.get(3, i) * (2 - i));
-    }
-
-  }
-
-  /**
-   * Returns the coefficients matrix.
-   *
-   * @return The coefficients matrix.
-   */
-  @Override
-  protected SimpleMatrix getCoefficients() {
-    return m_coefficients;
-  }
-
-  /**
-   * Returns the hermite basis matrix for cubic hermite spline interpolation.
-   *
-   * @return The hermite basis matrix for cubic hermite spline interpolation.
-   */
-  private SimpleMatrix makeHermiteBasis() {
-    if (hermiteBasis == null) {
-      hermiteBasis = new SimpleMatrix(4, 4, true, new double[]{
-          +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
-      });
-    }
-    return hermiteBasis;
-  }
-
-  /**
-   * 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.
-   */
-  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
-    if (initialVector.length != 2 || finalVector.length != 2) {
-      throw new IllegalArgumentException("Size of vectors must be 2");
-    }
-    return new SimpleMatrix(4, 1, true, new double[]{
-        initialVector[0], initialVector[1],
-        finalVector[0], finalVector[1]});
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
deleted file mode 100644
index ed8562d..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/PoseWithCurvature.java
+++ /dev/null
@@ -1,40 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * Represents a pair of a pose and a curvature.
- */
-@SuppressWarnings("MemberName")
-public class PoseWithCurvature {
-  // Represents the pose.
-  public Pose2d poseMeters;
-
-  // Represents the curvature.
-  public double curvatureRadPerMeter;
-
-  /**
-   * Constructs a PoseWithCurvature.
-   *
-   * @param poseMeters           The pose.
-   * @param curvatureRadPerMeter The curvature.
-   */
-  public PoseWithCurvature(Pose2d poseMeters, double curvatureRadPerMeter) {
-    this.poseMeters = poseMeters;
-    this.curvatureRadPerMeter = curvatureRadPerMeter;
-  }
-
-  /**
-   * Constructs a PoseWithCurvature with default values.
-   */
-  public PoseWithCurvature() {
-    poseMeters = new Pose2d();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
deleted file mode 100644
index 6073f62..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSpline.java
+++ /dev/null
@@ -1,116 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import org.ejml.simple.SimpleMatrix;
-
-public class QuinticHermiteSpline extends Spline {
-  private static SimpleMatrix hermiteBasis;
-  private final SimpleMatrix m_coefficients;
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  public QuinticHermiteSpline(double[] xInitialControlVector, double[] xFinalControlVector,
-                              double[] yInitialControlVector, double[] yFinalControlVector) {
-    super(5);
-
-    // Populate the coefficients for the actual spline equations.
-    // Row 0 is x coefficients
-    // Row 1 is y coefficients
-    final var hermite = makeHermiteBasis();
-    final var x = getControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
-    final var y = getControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
-
-    final var xCoeffs = (hermite.mult(x)).transpose();
-    final var yCoeffs = (hermite.mult(y)).transpose();
-
-    m_coefficients = new SimpleMatrix(6, 6);
-
-    for (int i = 0; i < 6; i++) {
-      m_coefficients.set(0, i, xCoeffs.get(0, i));
-      m_coefficients.set(1, i, yCoeffs.get(0, i));
-    }
-    for (int i = 0; i < 6; i++) {
-      // Populate Row 2 and Row 3 with the derivatives of the equations above.
-      // 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.set(2, i, m_coefficients.get(0, i) * (5 - i));
-      m_coefficients.set(3, i, m_coefficients.get(1, i) * (5 - i));
-    }
-    for (int i = 0; i < 5; i++) {
-      // Then populate row 4 and 5 with the second derivatives.
-      // 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.set(4, i, m_coefficients.get(2, i) * (4 - i));
-      m_coefficients.set(5, i, m_coefficients.get(3, i) * (4 - i));
-    }
-  }
-
-  /**
-   * Returns the coefficients matrix.
-   *
-   * @return The coefficients matrix.
-   */
-  @Override
-  protected SimpleMatrix getCoefficients() {
-    return m_coefficients;
-  }
-
-  /**
-   * Returns the hermite basis matrix for quintic hermite spline interpolation.
-   *
-   * @return The hermite basis matrix for quintic hermite spline interpolation.
-   */
-  private SimpleMatrix makeHermiteBasis() {
-    if (hermiteBasis == null) {
-      hermiteBasis = new SimpleMatrix(6, 6, true, new double[]{
-          -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
-      });
-    }
-    return hermiteBasis;
-  }
-
-  /**
-   * 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.
-   */
-  private SimpleMatrix getControlVectorFromArrays(double[] initialVector, double[] finalVector) {
-    if (initialVector.length != 3 || finalVector.length != 3) {
-      throw new IllegalArgumentException("Size of vectors must be 3");
-    }
-    return new SimpleMatrix(6, 1, true, new double[]{
-        initialVector[0], initialVector[1], initialVector[2],
-        finalVector[0], finalVector[1], finalVector[2]});
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
deleted file mode 100644
index 57c388f..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/Spline.java
+++ /dev/null
@@ -1,116 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.Arrays;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-/**
- * Represents a two-dimensional parametric spline that interpolates between two
- * points.
- */
-public abstract class Spline {
-  private final int m_degree;
-
-  /**
-   * Constructs a spline with the given degree.
-   *
-   * @param degree The degree of the spline.
-   */
-  Spline(int degree) {
-    m_degree = degree;
-  }
-
-  /**
-   * Returns the coefficients of the spline.
-   *
-   * @return The coefficients of the spline.
-   */
-  protected abstract SimpleMatrix getCoefficients();
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  public PoseWithCurvature getPoint(double t) {
-    SimpleMatrix polynomialBases = new SimpleMatrix(m_degree + 1, 1);
-    final var coefficients = getCoefficients();
-
-    // Populate the polynomial bases.
-    for (int i = 0; i <= m_degree; i++) {
-      polynomialBases.set(i, 0, Math.pow(t, m_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.
-    SimpleMatrix combined = coefficients.mult(polynomialBases);
-
-    // Get x and y
-    final double x = combined.get(0, 0);
-    final double y = combined.get(1, 0);
-
-    double dx;
-    double dy;
-    double ddx;
-    double ddy;
-
-    if (t == 0) {
-      dx = coefficients.get(2, m_degree - 1);
-      dy = coefficients.get(3, m_degree - 1);
-      ddx = coefficients.get(4, m_degree - 2);
-      ddy = coefficients.get(5, m_degree - 2);
-    } else {
-      // Divide out t once for first derivative.
-      dx = combined.get(2, 0) / t;
-      dy = combined.get(3, 0) / t;
-
-      // Divide out t twice for second derivative.
-      ddx = combined.get(4, 0) / t / t;
-      ddy = combined.get(5, 0) / t / t;
-    }
-
-    // Find the curvature.
-    final double curvature =
-        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * Math.hypot(dx, dy));
-
-    return new PoseWithCurvature(
-        new Pose2d(x, y, new Rotation2d(dx, dy)),
-        curvature
-    );
-  }
-
-  /**
-   * Represents a control vector for a spline.
-   *
-   * <p>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.
-   */
-  @SuppressWarnings("MemberName")
-  public static class ControlVector {
-    public double[] x;
-    public double[] y;
-
-    /**
-     * Instantiates a control vector.
-     * @param x The x dimension of the control vector.
-     * @param y The y dimension of the control vector.
-     */
-    @SuppressWarnings("ParameterName")
-    public ControlVector(double[] x, double[] y) {
-      this.x = Arrays.copyOf(x, x.length);
-      this.y = Arrays.copyOf(y, y.length);
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
deleted file mode 100644
index a2bf9dd..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineHelper.java
+++ /dev/null
@@ -1,280 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.Arrays;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-public final class SplineHelper {
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private SplineHelper() {
-  }
-
-  /**
-   * 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.
-   */
-  public static Spline.ControlVector[] getCubicControlVectorsFromWaypoints(
-      Pose2d start, Translation2d[] interiorWaypoints, Pose2d end
-  ) {
-    // Generate control vectors from poses.
-    Spline.ControlVector initialCV;
-    Spline.ControlVector endCV;
-
-    // Chooses a magnitude automatically that makes the splines look better.
-    if (interiorWaypoints.length < 1) {
-      double scalar = start.getTranslation().getDistance(end.getTranslation()) * 1.2;
-      initialCV = getCubicControlVector(scalar, start);
-      endCV = getCubicControlVector(scalar, end);
-    } else {
-      double scalar = start.getTranslation().getDistance(interiorWaypoints[0]) * 1.2;
-      initialCV = getCubicControlVector(scalar, start);
-      scalar = end.getTranslation().getDistance(interiorWaypoints[interiorWaypoints.length - 1])
-          * 1.2;
-      endCV = getCubicControlVector(scalar, end);
-    }
-    return new Spline.ControlVector[]{initialCV, endCV};
-  }
-
-  /**
-   * Returns quintic splines from a set of waypoints.
-   *
-   * @param waypoints The waypoints
-   * @return List of splines.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static QuinticHermiteSpline[] getQuinticSplinesFromWaypoints(List<Pose2d> waypoints) {
-    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[waypoints.size() - 1];
-    for (int i = 0; i < waypoints.size() - 1; ++i) {
-      var p0 = waypoints.get(i);
-      var p1 = waypoints.get(i + 1);
-
-      // This just makes the splines look better.
-      final var scalar = 1.2 * p0.getTranslation().getDistance(p1.getTranslation());
-
-      var controlVecA = getQuinticControlVector(scalar, p0);
-      var controlVecB = getQuinticControlVector(scalar, p1);
-
-      splines[i]
-          = new QuinticHermiteSpline(controlVecA.x, controlVecB.x, controlVecA.y, controlVecB.y);
-    }
-    return splines;
-  }
-
-  /**
-   * 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.
-   *
-   * @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 and control vectors.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.ExcessiveMethodLength",
-                        "PMD.AvoidInstantiatingObjectsInLoops"})
-  public static CubicHermiteSpline[] getCubicSplinesFromControlVectors(
-      Spline.ControlVector start, Translation2d[] waypoints, Spline.ControlVector end) {
-
-    CubicHermiteSpline[] splines = new CubicHermiteSpline[waypoints.length + 1];
-
-    double[] xInitial = start.x;
-    double[] yInitial = start.y;
-    double[] xFinal = end.x;
-    double[] yFinal = end.y;
-
-    if (waypoints.length > 1) {
-      Translation2d[] newWaypts = new Translation2d[waypoints.length + 2];
-
-      // Create an array of all waypoints, including the start and end.
-      newWaypts[0] = new Translation2d(xInitial[0], yInitial[0]);
-      System.arraycopy(waypoints, 0, newWaypts, 1, waypoints.length);
-      newWaypts[newWaypts.length - 1] = new Translation2d(xFinal[0], 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
-      final double[] a = new double[newWaypts.length - 2];
-
-      // Diagonal of tridiagonal matrix
-      final double[] b = new double[newWaypts.length - 2];
-      Arrays.fill(b, 4.0);
-
-      // Below-diagonal of tridiagonal matrix, zero-padded
-      final double[] c = new double[newWaypts.length - 2];
-
-      // rhs vectors
-      final double[] dx = new double[newWaypts.length - 2];
-      final double[] dy = new double[newWaypts.length - 2];
-
-      // solution vectors
-      final double[] fx = new double[newWaypts.length - 2];
-      final double[] fy = new double[newWaypts.length - 2];
-
-      // populate above-diagonal and below-diagonal vectors
-      a[0] = 0.0;
-      for (int i = 0; i < newWaypts.length - 3; i++) {
-        a[i + 1] = 1;
-        c[i] = 1;
-      }
-      c[c.length - 1] = 0.0;
-
-      // populate rhs vectors
-      dx[0] = 3 * (newWaypts[2].getX() - newWaypts[0].getX()) - xInitial[1];
-      dy[0] = 3 * (newWaypts[2].getY() - newWaypts[0].getY()) - yInitial[1];
-
-      if (newWaypts.length > 4) {
-        for (int i = 1; i <= newWaypts.length - 4; i++) {
-          // dx and dy represent the derivatives of the internal waypoints. The derivative
-          // of the second internal waypoint should involve the third and first internal waypoint,
-          // which have indices of 1 and 3 in the newWaypts list (which contains ALL waypoints).
-          dx[i] = 3 * (newWaypts[i + 2].getX() - newWaypts[i].getX());
-          dy[i] = 3 * (newWaypts[i + 2].getY() - newWaypts[i].getY());
-        }
-      }
-
-      dx[dx.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getX()
-          - newWaypts[newWaypts.length - 3].getX()) - xFinal[1];
-      dy[dy.length - 1] = 3 * (newWaypts[newWaypts.length - 1].getY()
-          - newWaypts[newWaypts.length - 3].getY()) - yFinal[1];
-
-      // Compute solution to tridiagonal system
-      thomasAlgorithm(a, b, c, dx, fx);
-      thomasAlgorithm(a, b, c, dy, fy);
-
-      double[] newFx = new double[fx.length + 2];
-      double[] newFy = new double[fy.length + 2];
-
-      newFx[0] = xInitial[1];
-      newFy[0] = yInitial[1];
-      System.arraycopy(fx, 0, newFx, 1, fx.length);
-      System.arraycopy(fy, 0, newFy, 1, fy.length);
-      newFx[newFx.length - 1] = xFinal[1];
-      newFy[newFy.length - 1] = yFinal[1];
-
-      for (int i = 0; i < newFx.length - 1; i++) {
-        splines[i] = new CubicHermiteSpline(
-            new double[]{newWaypts[i].getX(), newFx[i]},
-            new double[]{newWaypts[i + 1].getX(), newFx[i + 1]},
-            new double[]{newWaypts[i].getY(), newFy[i]},
-            new double[]{newWaypts[i + 1].getY(), newFy[i + 1]}
-        );
-      }
-    } else if (waypoints.length == 1) {
-      final var xDeriv = (3 * (xFinal[0]
-          - xInitial[0])
-          - xFinal[1] - xInitial[1])
-          / 4.0;
-      final var yDeriv = (3 * (yFinal[0]
-          - yInitial[0])
-          - yFinal[1] - yInitial[1])
-          / 4.0;
-
-      double[] midXControlVector = {waypoints[0].getX(), xDeriv};
-      double[] midYControlVector = {waypoints[0].getY(), yDeriv};
-
-      splines[0] = new CubicHermiteSpline(xInitial, midXControlVector,
-                                          yInitial, midYControlVector);
-      splines[1] = new CubicHermiteSpline(midXControlVector, xFinal,
-                                          midYControlVector, yFinal);
-    } else {
-      splines[0] = new CubicHermiteSpline(xInitial, xFinal,
-                                          yInitial, yFinal);
-    }
-    return splines;
-  }
-
-  /**
-   * Returns a set of quintic splines corresponding to the provided control vectors.
-   * The user is free to set the direction of all control vectors. 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.
-   */
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public static QuinticHermiteSpline[] getQuinticSplinesFromControlVectors(
-      Spline.ControlVector[] controlVectors) {
-    QuinticHermiteSpline[] splines = new QuinticHermiteSpline[controlVectors.length - 1];
-    for (int i = 0; i < controlVectors.length - 1; i++) {
-      var xInitial = controlVectors[i].x;
-      var xFinal = controlVectors[i + 1].x;
-      var yInitial = controlVectors[i].y;
-      var yFinal = controlVectors[i + 1].y;
-      splines[i] = new QuinticHermiteSpline(xInitial, xFinal,
-                                            yInitial, yFinal);
-    }
-    return splines;
-  }
-
-  /**
-   * 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
-   */
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  private static void thomasAlgorithm(double[] a, double[] b,
-                                      double[] c, double[] d, double[] solutionVector) {
-    int N = d.length;
-
-    double[] cStar = new double[N];
-    double[] dStar = new double[N];
-
-    // This updates the coefficients in the first row
-    // Note that we should be checking for division by zero here
-    cStar[0] = c[0] / b[0];
-    dStar[0] = d[0] / b[0];
-
-    // Create the c_star and d_star coefficients in the forward sweep
-    for (int i = 1; i < N; i++) {
-      double m = 1.0 / (b[i] - a[i] * cStar[i - 1]);
-      cStar[i] = c[i] * m;
-      dStar[i] = (d[i] - a[i] * dStar[i - 1]) * m;
-    }
-    solutionVector[N - 1] = dStar[N - 1];
-    // This is the reverse sweep, used to update the solution vector f
-    for (int i = N - 2; i >= 0; i--) {
-      solutionVector[i] = dStar[i] - cStar[i] * solutionVector[i + 1];
-    }
-  }
-
-  private static Spline.ControlVector getCubicControlVector(double scalar, Pose2d point) {
-    return new Spline.ControlVector(
-        new double[]{point.getX(), scalar * point.getRotation().getCos()},
-        new double[]{point.getY(), scalar * point.getRotation().getSin()}
-    );
-  }
-
-  private static Spline.ControlVector getQuinticControlVector(double scalar, Pose2d point) {
-    return new Spline.ControlVector(
-        new double[]{point.getX(), scalar * point.getRotation().getCos(), 0.0},
-        new double[]{point.getY(), scalar * point.getRotation().getSin(), 0.0}
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
deleted file mode 100644
index 1585214..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/spline/SplineParameterizer.java
+++ /dev/null
@@ -1,153 +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.
- */
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.ArrayDeque;
-import java.util.ArrayList;
-import java.util.List;
-
-/**
- * Class used to parameterize a spline by its arc length.
- */
-public final class SplineParameterizer {
-  private static final double kMaxDx = 0.127;
-  private static final double kMaxDy = 0.00127;
-  private static final double kMaxDtheta = 0.0872;
-
-  /**
-   * 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.
-   */
-  private static final int kMaxIterations = 5000;
-
-  @SuppressWarnings("MemberName")
-  private static class StackContents {
-    final double t1;
-    final double t0;
-
-    StackContents(double t0, double t1) {
-      this.t0 = t0;
-      this.t1 = t1;
-    }
-  }
-
-  @SuppressWarnings("serial")
-  public static class MalformedSplineException extends RuntimeException {
-    /**
-     * Create a new exception with the given message.
-     *
-     * @param message the message to pass with the exception
-     */
-    private MalformedSplineException(String message) {
-      super(message);
-    }
-  }
-
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private SplineParameterizer() {
-  }
-
-  /**
-   * 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.
-   * @return A list of poses and curvatures that represents various points on the spline.
-   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
-   *                                  with approximately opposing headings)
-   */
-  public static List<PoseWithCurvature> parameterize(Spline spline) {
-    return parameterize(spline, 0.0, 1.0);
-  }
-
-  /**
-   * 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 use 0.0.
-   * @param t1     Ending internal spline parameter. It is recommended to use 1.0.
-   * @return       A list of poses and curvatures that represents various points on the spline.
-   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
-   *                                  with approximately opposing headings)
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static List<PoseWithCurvature> parameterize(Spline spline, double t0, double t1) {
-    var splinePoints = new ArrayList<PoseWithCurvature>();
-
-    // The parameterization does not add the initial point. Let's add that.
-    splinePoints.add(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
-    var stack = new ArrayDeque<StackContents>();
-    stack.push(new StackContents(t0, t1));
-
-    StackContents current;
-    PoseWithCurvature start;
-    PoseWithCurvature end;
-    int iterations = 0;
-
-    while (!stack.isEmpty()) {
-      current = stack.removeFirst();
-      start = spline.getPoint(current.t0);
-      end = spline.getPoint(current.t1);
-
-      final var twist = start.poseMeters.log(end.poseMeters);
-      if (
-          Math.abs(twist.dy) > kMaxDy
-          || Math.abs(twist.dx) > kMaxDx
-          || Math.abs(twist.dtheta) > kMaxDtheta
-      ) {
-        stack.addFirst(new StackContents((current.t0 + current.t1) / 2, current.t1));
-        stack.addFirst(new StackContents(current.t0, (current.t0 + current.t1) / 2));
-      } else {
-        splinePoints.add(spline.getPoint(current.t1));
-      }
-
-      iterations++;
-      if (iterations >= kMaxIterations) {
-        throw new 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;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java
deleted file mode 100644
index 4a90caa..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystem.java
+++ /dev/null
@@ -1,182 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system;
-
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-@SuppressWarnings("ClassTypeParameterName")
-public class LinearSystem<States extends Num, Inputs extends Num,
-        Outputs extends Num> {
-
-  /**
-   * Continuous system matrix.
-   */
-  @SuppressWarnings("MemberName")
-  private final Matrix<States, States> m_A;
-
-  /**
-   * Continuous input matrix.
-   */
-  @SuppressWarnings("MemberName")
-  private final Matrix<States, Inputs> m_B;
-
-  /**
-   * Output matrix.
-   */
-  @SuppressWarnings("MemberName")
-  private final Matrix<Outputs, States> m_C;
-
-  /**
-   * Feedthrough matrix.
-   */
-  @SuppressWarnings("MemberName")
-  private final Matrix<Outputs, Inputs> m_D;
-
-  /**
-   * Construct a new LinearSystem from the four system matrices.
-   *
-   * @param a             The system matrix A.
-   * @param b             The input matrix B.
-   * @param c             The output matrix C.
-   * @param d             The feedthrough matrix D.
-   */
-  @SuppressWarnings("ParameterName")
-  public LinearSystem(Matrix<States, States> a, Matrix<States, Inputs> b,
-                      Matrix<Outputs, States> c, Matrix<Outputs, Inputs> d) {
-
-    this.m_A = a;
-    this.m_B = b;
-    this.m_C = c;
-    this.m_D = d;
-  }
-
-  /**
-   * Returns the system matrix A.
-   *
-   * @return the system matrix A.
-   */
-  public Matrix<States, States> getA() {
-    return m_A;
-  }
-
-  /**
-   * Returns an element of the system matrix A.
-   *
-   * @param row Row of A.
-   * @param col Column of A.
-   * @return the system matrix A at (i, j).
-   */
-  public double getA(int row, int col) {
-    return m_A.get(row, col);
-  }
-
-  /**
-   * Returns the input matrix B.
-   *
-   * @return the input matrix B.
-   */
-  public Matrix<States, Inputs> getB() {
-    return m_B;
-  }
-
-  /**
-   * Returns an element of the input matrix B.
-   *
-   * @param row Row of B.
-   * @param col Column of B.
-   * @return The value of the input matrix B at (i, j).
-   */
-  public double getB(int row, int col) {
-    return m_B.get(row, col);
-  }
-
-  /**
-   * Returns the output matrix C.
-   *
-   * @return Output matrix C.
-   */
-  public Matrix<Outputs, States> getC() {
-    return m_C;
-  }
-
-  /**
-   * Returns an element of the output matrix C.
-   *
-   * @param row Row of C.
-   * @param col Column of C.
-   * @return the double value of C at the given position.
-   */
-  public double getC(int row, int col) {
-    return m_C.get(row, col);
-  }
-
-  /**
-   * Returns the feedthrough matrix D.
-   *
-   * @return the feedthrough matrix D.
-   */
-  public Matrix<Outputs, Inputs> getD() {
-    return m_D;
-  }
-
-  /**
-   * Returns an element of the feedthrough matrix D.
-   *
-   * @param row Row of D.
-   * @param col Column of D.
-   * @return The feedthrough matrix D at (i, j).
-   */
-  public double getD(int row, int col) {
-    return m_D.get(row, col);
-  }
-
-  /**
-   * Computes the new x given the old x and the control input.
-   *
-   * <p>This is used by state observers directly to run updates based on state
-   * estimate.
-   *
-   * @param x         The current state.
-   * @param clampedU  The control input.
-   * @param dtSeconds Timestep for model update.
-   * @return the updated x.
-   */
-  @SuppressWarnings("ParameterName")
-  public Matrix<States, N1> calculateX(Matrix<States, N1> x, Matrix<Inputs, N1> clampedU,
-                                       double dtSeconds) {
-    var discABpair = Discretization.discretizeAB(m_A, m_B, dtSeconds);
-
-    return (discABpair.getFirst().times(x)).plus(discABpair.getSecond().times(clampedU));
-  }
-
-  /**
-   * Computes the new y given the control input.
-   *
-   * <p>This is used by state observers directly to run updates based on state
-   * estimate.
-   *
-   * @param x The current state.
-   * @param clampedU The control input.
-   * @return the updated output matrix Y.
-   */
-  @SuppressWarnings("ParameterName")
-  public Matrix<Outputs, N1> calculateY(
-          Matrix<States, N1> x,
-          Matrix<Inputs, N1> clampedU) {
-    return m_C.times(x).plus(m_D.times(clampedU));
-  }
-
-  @Override
-  public String toString() {
-    return String.format("Linear System: A\n%s\n\nB:\n%s\n\nC:\n%s\n\nD:\n%s\n", m_A.toString(),
-            m_B.toString(), m_C.toString(), m_D.toString());
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
deleted file mode 100644
index d44ca62..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/LinearSystemLoop.java
+++ /dev/null
@@ -1,358 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system;
-
-import java.util.function.Function;
-
-import org.ejml.MatrixDimensionException;
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpilibj.controller.LinearPlantInversionFeedforward;
-import edu.wpi.first.wpilibj.controller.LinearQuadraticRegulator;
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * Combines a plant, controller, and observer for controlling a mechanism with
- * full state feedback.
- *
- * <p>For everything in this file, "inputs" and "outputs" are defined from the
- * perspective of the plant. This means U is an input and Y is an output
- * (because you give the plant U (powers) and it gives you back a Y (sensor
- * values). This is the opposite of what they mean from the perspective of the
- * controller (U is an output because that's what goes to the motors and Y is an
- * input because that's what comes back from the sensors).
- *
- * <p>For more on the underlying math, read
- * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
- */
-@SuppressWarnings("ClassTypeParameterName")
-public class LinearSystemLoop<States extends Num, Inputs extends Num,
-        Outputs extends Num> {
-
-  private final LinearSystem<States, Inputs, Outputs> m_plant;
-  private final LinearQuadraticRegulator<States, Inputs, Outputs> m_controller;
-  private final LinearPlantInversionFeedforward<States, Inputs, Outputs> m_feedforward;
-  private final KalmanFilter<States, Inputs, Outputs> m_observer;
-  private Matrix<States, N1> m_nextR;
-  private Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> m_clampFunction;
-
-  /**
-   * Constructs a state-space loop with the given plant, controller, and
-   * observer. By default, the initial reference is all zeros. Users should
-   * call reset with the initial system state before enabling the loop. This
-   * constructor assumes that the input(s) to this system are voltage.
-   *
-   * @param plant      State-space plant.
-   * @param controller State-space controller.
-   * @param observer   State-space observer.
-   * @param maxVoltageVolts The maximum voltage that can be applied. Commonly 12.
-   * @param dtSeconds The nominal timestep.
-   */
-  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
-                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
-                          KalmanFilter<States, Inputs, Outputs> observer,
-                          double maxVoltageVolts,
-                          double dtSeconds) {
-    this(plant, controller,
-        new LinearPlantInversionFeedforward<>(plant, dtSeconds), observer,
-        u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
-  }
-
-  /**
-   * Constructs a state-space loop with the given plant, controller, and
-   * observer. By default, the initial reference is all zeros. Users should
-   * call reset with the initial system state before enabling the loop.
-   *
-   * @param plant      State-space plant.
-   * @param controller State-space controller.
-   * @param observer   State-space observer.
-   * @param clampFunction The function used to clamp the input U.
-   * @param dtSeconds The nominal timestep.
-   */
-  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
-                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
-                          KalmanFilter<States, Inputs, Outputs> observer,
-                          Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction,
-                          double dtSeconds) {
-    this(plant, controller, new LinearPlantInversionFeedforward<>(plant, dtSeconds),
-          observer, clampFunction);
-  }
-
-  /**
-   * Constructs a state-space loop with the given plant, controller, and
-   * observer. By default, the initial reference is all zeros. Users should
-   * call reset with the initial system state before enabling the loop.
-   *
-   * @param plant      State-space plant.
-   * @param controller State-space controller.
-   * @param feedforward Plant inversion feedforward.
-   * @param observer   State-space observer.
-   * @param maxVoltageVolts The maximum voltage that can be applied. Assumes that the
-   *                        inputs are voltages.
-   */
-  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
-                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
-                          LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
-                          KalmanFilter<States, Inputs, Outputs> observer,
-                          double maxVoltageVolts
-  ) {
-    this(plant, controller, feedforward,
-          observer, u -> StateSpaceUtil.normalizeInputVector(u, maxVoltageVolts));
-  }
-
-  /**
-   * Constructs a state-space loop with the given plant, controller, and
-   * observer. By default, the initial reference is all zeros. Users should
-   * call reset with the initial system state before enabling the loop.
-   *
-   * @param plant      State-space plant.
-   * @param controller State-space controller.
-   * @param feedforward Plant inversion feedforward.
-   * @param observer   State-space observer.
-   * @param clampFunction The function used to clamp the input U.
-   */
-  public LinearSystemLoop(LinearSystem<States, Inputs, Outputs> plant,
-                          LinearQuadraticRegulator<States, Inputs, Outputs> controller,
-                          LinearPlantInversionFeedforward<States, Inputs, Outputs> feedforward,
-                          KalmanFilter<States, Inputs, Outputs> observer,
-                          Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
-    this.m_plant = plant;
-    this.m_controller = controller;
-    this.m_feedforward = feedforward;
-    this.m_observer = observer;
-    this.m_clampFunction = clampFunction;
-
-    m_nextR = new Matrix<>(new SimpleMatrix(controller.getK().getNumCols(), 1));
-    reset(m_nextR);
-  }
-
-  /**
-   * Returns the observer's state estimate x-hat.
-   *
-   * @return the observer's state estimate x-hat.
-   */
-  public Matrix<States, N1> getXHat() {
-    return getObserver().getXhat();
-  }
-
-  /**
-   * Returns an element of the observer's state estimate x-hat.
-   *
-   * @param row Row of x-hat.
-   * @return the i-th element of the observer's state estimate x-hat.
-   */
-  public double getXHat(int row) {
-    return getObserver().getXhat(row);
-  }
-
-  /**
-   * Set the initial state estimate x-hat.
-   *
-   * @param xhat The initial state estimate x-hat.
-   */
-  public void setXHat(Matrix<States, N1> xhat) {
-    getObserver().setXhat(xhat);
-  }
-
-  /**
-   * Set an element of the initial state estimate x-hat.
-   *
-   * @param row   Row of x-hat.
-   * @param value Value for element of x-hat.
-   */
-  public void setXHat(int row, double value) {
-    getObserver().setXhat(row, value);
-  }
-
-  /**
-   * Returns an element of the controller's next reference r.
-   *
-   * @param row Row of r.
-   * @return the element i of the controller's next reference r.
-   */
-  public double getNextR(int row) {
-    return getNextR().get(row, 0);
-  }
-
-  /**
-   * Returns the controller's next reference r.
-   *
-   * @return the controller's next reference r.
-   */
-  public Matrix<States, N1> getNextR() {
-    return m_nextR;
-  }
-
-  /**
-   * Set the next reference r.
-   *
-   * @param nextR Next reference.
-   */
-  public void setNextR(Matrix<States, N1> nextR) {
-    m_nextR = nextR;
-  }
-
-  /**
-   * Set the next reference r.
-   *
-   * @param nextR Next reference.
-   */
-  public void setNextR(double... nextR) {
-    if (nextR.length != m_nextR.getNumRows()) {
-      throw new MatrixDimensionException(String.format("The next reference does not have the "
-                      + "correct number of entries! Expected %s, but got %s.",
-              m_nextR.getNumRows(),
-              nextR.length));
-    }
-    m_nextR = new Matrix<>(new SimpleMatrix(m_nextR.getNumRows(), 1, true, nextR));
-  }
-
-  /**
-   * Returns the controller's calculated control input u plus the calculated feedforward u_ff.
-   *
-   * @return the calculated control input u.
-   */
-  public Matrix<Inputs, N1> getU() {
-    return clampInput(m_controller.getU().plus(m_feedforward.getUff()));
-  }
-
-  /**
-   * Returns an element of the controller's calculated control input u.
-   *
-   * @param row Row of u.
-   * @return the calculated control input u at the row i.
-   */
-  public double getU(int row) {
-    return getU().get(row, 0);
-  }
-
-  /**
-   * Return the plant used internally.
-   *
-   * @return the plant used internally.
-   */
-  public LinearSystem<States, Inputs, Outputs> getPlant() {
-    return m_plant;
-  }
-
-  /**
-   * Return the controller used internally.
-   *
-   * @return the controller used internally.
-   */
-  public LinearQuadraticRegulator<States, Inputs, Outputs> getController() {
-    return m_controller;
-  }
-
-  /**
-   * Return the feedforward used internally.
-   *
-   * @return the feedforward used internally.
-   */
-  public LinearPlantInversionFeedforward<States, Inputs, Outputs> getFeedforward() {
-    return m_feedforward;
-  }
-
-  /**
-   * Return the observer used internally.
-   *
-   * @return the observer used internally.
-   */
-  public KalmanFilter<States, Inputs, Outputs> getObserver() {
-    return m_observer;
-  }
-
-  /**
-   * Zeroes reference r and controller output u. The previous reference
-   * of the PlantInversionFeedforward and the initial state estimate of
-   * the KalmanFilter are set to the initial state provided.
-   *
-   * @param initialState The initial state.
-   */
-  public void reset(Matrix<States, N1> initialState) {
-    m_nextR.fill(0.0);
-    m_controller.reset();
-    m_feedforward.reset(initialState);
-    m_observer.setXhat(initialState);
-  }
-
-  /**
-   * Returns difference between reference r and current state x-hat.
-   *
-   * @return The state error matrix.
-   */
-  public Matrix<States, N1> getError() {
-    return getController().getR().minus(m_observer.getXhat());
-  }
-
-  /**
-   * Returns difference between reference r and current state x-hat.
-   *
-   * @param index The index of the error matrix to return.
-   * @return The error at that index.
-   */
-  public double getError(int index) {
-    return (getController().getR().minus(m_observer.getXhat())).get(index, 0);
-  }
-
-  /**
-   * Get the function used to clamp the input u.
-   * @return The clamping function.
-   */
-  public Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> getClampFunction() {
-    return m_clampFunction;
-  }
-
-  /**
-   * Set the clamping function used to clamp inputs.
-   */
-  public void setClampFunction(Function<Matrix<Inputs, N1>, Matrix<Inputs, N1>> clampFunction) {
-    this.m_clampFunction = clampFunction;
-  }
-
-  /**
-   * Correct the state estimate x-hat using the measurements in y.
-   *
-   * @param y Measurement vector.
-   */
-  @SuppressWarnings("ParameterName")
-  public void correct(Matrix<Outputs, N1> y) {
-    getObserver().correct(getU(), y);
-  }
-
-  /**
-   * Sets new controller output, projects model forward, and runs observer
-   * prediction.
-   *
-   * <p>After calling this, the user should send the elements of u to the
-   * actuators.
-   *
-   * @param dtSeconds Timestep for model update.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public void predict(double dtSeconds) {
-    var u = clampInput(m_controller.calculate(getObserver().getXhat(), m_nextR)
-          .plus(m_feedforward.calculate(m_nextR)));
-    getObserver().predict(u, dtSeconds);
-  }
-
-  /**
-   * Clamp the input u to the min and max.
-   *
-   * @param unclampedU The input to clamp.
-   * @return           The clamped input.
-   */
-  public Matrix<Inputs, N1> clampInput(Matrix<Inputs, N1> unclampedU) {
-    return m_clampFunction.apply(unclampedU);
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java
deleted file mode 100644
index a808dec..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/NumericalJacobian.java
+++ /dev/null
@@ -1,111 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system;
-
-import java.util.function.BiFunction;
-import java.util.function.Function;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-public final class NumericalJacobian {
-  private NumericalJacobian() {
-    // Utility Class.
-  }
-
-  private static final double kEpsilon = 1e-5;
-
-  /**
-   * Computes the numerical Jacobian with respect to x for f(x).
-   *
-   * @param <Rows>   Number of rows in the result of f(x).
-   * @param <States> Num representing the number of rows in the output of f.
-   * @param <Cols>   Number of columns in the result of f(x).
-   * @param rows     Number of rows in the result of f(x).
-   * @param cols     Number of columns in the result of f(x).
-   * @param f        Vector-valued function from which to compute the Jacobian.
-   * @param x        Vector argument.
-   * @return The numerical Jacobian with respect to x for f(x, u, ...).
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <Rows extends Num, Cols extends Num, States extends Num> Matrix<Rows, Cols>
-      numericalJacobian(
-          Nat<Rows> rows,
-          Nat<Cols> cols,
-          Function<Matrix<Cols, N1>, Matrix<States, N1>> f,
-          Matrix<Cols, N1> x
-  ) {
-    var result = new Matrix<>(rows, cols);
-
-    for (int i = 0; i < cols.getNum(); i++) {
-      var dxPlus = x.copy();
-      var dxMinus = x.copy();
-      dxPlus.set(i, 0, dxPlus.get(i, 0) + kEpsilon);
-      dxMinus.set(i, 0, dxMinus.get(i, 0) - kEpsilon);
-      @SuppressWarnings("LocalVariableName")
-      var dF = f.apply(dxPlus).minus(f.apply(dxMinus)).div(2 * kEpsilon);
-
-      result.setColumn(i, Matrix.changeBoundsUnchecked(dF));
-    }
-
-    return result;
-  }
-
-  /**
-   * Returns numerical Jacobian with respect to x for f(x, u, ...).
-   *
-   * @param <Rows>    Number of rows in the result of f(x, u).
-   * @param <States>  Number of rows in x.
-   * @param <Inputs>  Number of rows in the second input to f.
-   * @param <Outputs> Num representing the rows in the output of f.
-   * @param rows      Number of rows in the result of f(x, u).
-   * @param states    Number of rows in x.
-   * @param f         Vector-valued function from which to compute Jacobian.
-   * @param x         State vector.
-   * @param u         Input vector.
-   * @return The numerical Jacobian with respect to x for f(x, u, ...).
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <Rows extends Num, States extends Num, Inputs extends Num, Outputs extends Num>
-      Matrix<Rows, States> numericalJacobianX(
-          Nat<Rows> rows,
-          Nat<States> states,
-          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<Outputs, N1>> f,
-          Matrix<States, N1> x,
-          Matrix<Inputs, N1> u
-  ) {
-    return numericalJacobian(rows, states, _x -> f.apply(_x, u), x);
-  }
-
-  /**
-   * Returns the numerical Jacobian with respect to u for f(x, u).
-   *
-   * @param <States> The states of the system.
-   * @param <Inputs> The inputs to the system.
-   * @param <Rows>   Number of rows in the result of f(x, u).
-   * @param rows     Number of rows in the result of f(x, u).
-   * @param inputs   Number of rows in u.
-   * @param f        Vector-valued function from which to compute the Jacobian.
-   * @param x        State vector.
-   * @param u        Input vector.
-   * @return the numerical Jacobian with respect to u for f(x, u).
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <Rows extends Num, States extends Num, Inputs extends Num> Matrix<Rows, Inputs>
-      numericalJacobianU(
-          Nat<Rows> rows,
-          Nat<Inputs> inputs,
-          BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-          Matrix<States, N1> x,
-          Matrix<Inputs, N1> u
-  ) {
-    return numericalJacobian(rows, inputs, _u -> f.apply(x, _u), u);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
deleted file mode 100644
index fef5ddf..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/RungeKutta.java
+++ /dev/null
@@ -1,113 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system;
-
-import java.util.function.BiFunction;
-import java.util.function.DoubleFunction;
-import java.util.function.Function;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Num;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-public final class RungeKutta {
-  private RungeKutta() {
-    // utility Class
-  }
-
-  /**
-   * Performs Runge Kutta integration (4th order).
-   *
-   * @param f         The function to integrate, which takes one argument x.
-   * @param x         The initial value of x.
-   * @param dtSeconds The time over which to integrate.
-   * @return the integration of dx/dt = f(x) for dt.
-   */
-  @SuppressWarnings("ParameterName")
-  public static double rungeKutta(
-          DoubleFunction<Double> f,
-          double x,
-          double dtSeconds
-  ) {
-    final var halfDt = 0.5 * dtSeconds;
-    final var k1 = f.apply(x);
-    final var k2 = f.apply(x + k1 * halfDt);
-    final var k3 = f.apply(x + k2 * halfDt);
-    final var k4 = f.apply(x + k3 * dtSeconds);
-    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-  }
-
-  /**
-   * Performs Runge Kutta integration (4th order).
-   *
-   * @param f         The function to integrate. It must take two arguments x and u.
-   * @param x         The initial value of x.
-   * @param u         The value u held constant over the integration period.
-   * @param dtSeconds The time over which to integrate.
-   * @return The result of Runge Kutta integration (4th order).
-   */
-  @SuppressWarnings("ParameterName")
-  public static double rungeKutta(
-          BiFunction<Double, Double, Double> f,
-          double x, Double u, double dtSeconds
-  ) {
-    final var halfDt = 0.5 * dtSeconds;
-    final var k1 = f.apply(x, u);
-    final var k2 = f.apply(x + k1 * halfDt, u);
-    final var k3 = f.apply(x + k2 * halfDt, u);
-    final var k4 = f.apply(x + k3 * dtSeconds, u);
-    return x + dtSeconds / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-  }
-
-  /**
-   * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
-   *
-   * @param <States>  A Num representing the states of the system to integrate.
-   * @param <Inputs>  A Num representing the inputs of the system to integrate.
-   * @param f         The function to integrate. It must take two arguments x and u.
-   * @param x         The initial value of x.
-   * @param u         The value u held constant over the integration period.
-   * @param dtSeconds The time over which to integrate.
-   * @return the integration of dx/dt = f(x, u) for dt.
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <States extends Num, Inputs extends Num> Matrix<States, N1> rungeKutta(
-      BiFunction<Matrix<States, N1>, Matrix<Inputs, N1>, Matrix<States, N1>> f,
-      Matrix<States, N1> x, Matrix<Inputs, N1> u, double dtSeconds) {
-
-    final var halfDt = 0.5 * dtSeconds;
-    Matrix<States, N1> k1 = f.apply(x, u);
-    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)), u);
-    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)), u);
-    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)), u);
-    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
-  }
-
-  /**
-   * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
-   *
-   * @param <States>  A Num prepresenting the states of the system.
-   * @param f         The function to integrate. It must take one argument x.
-   * @param x         The initial value of x.
-   * @param dtSeconds The time over which to integrate.
-   * @return 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
-   */
-  @SuppressWarnings({"ParameterName", "MethodTypeParameterName"})
-  public static <States extends Num> Matrix<States, N1> rungeKutta(
-      Function<Matrix<States, N1>, Matrix<States, N1>> f,
-      Matrix<States, N1> x, double dtSeconds) {
-
-    final var halfDt = 0.5 * dtSeconds;
-    Matrix<States, N1> k1 = f.apply(x);
-    Matrix<States, N1> k2 = f.apply(x.plus(k1.times(halfDt)));
-    Matrix<States, N1> k3 = f.apply(x.plus(k2.times(halfDt)));
-    Matrix<States, N1> k4 = f.apply(x.plus(k3.times(dtSeconds)));
-    return x.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(dtSeconds).div(6.0));
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
deleted file mode 100644
index 2e95e3f..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/DCMotor.java
+++ /dev/null
@@ -1,182 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system.plant;
-
-import edu.wpi.first.wpilibj.util.Units;
-
-/**
- * Holds the constants for a DC motor.
- */
-public class DCMotor {
-  public final double m_nominalVoltageVolts;
-  public final double m_stallTorqueNewtonMeters;
-  public final double m_stallCurrentAmps;
-  public final double m_freeCurrentAmps;
-  public final double m_freeSpeedRadPerSec;
-  @SuppressWarnings("MemberName")
-  public final double m_rOhms;
-  @SuppressWarnings("MemberName")
-  public final double m_KvRadPerSecPerVolt;
-  @SuppressWarnings("MemberName")
-  public final double m_KtNMPerAmp;
-
-
-  /**
-   * Constructs a DC motor.
-   *
-   * @param nominalVoltageVolts     Voltage at which the motor constants were measured.
-   * @param stallTorqueNewtonMeters Current draw when stalled.
-   * @param stallCurrentAmps        Current draw when stalled.
-   * @param freeCurrentAmps         Current draw under no load.
-   * @param freeSpeedRadPerSec      Angular velocity under no load.
-   */
-  public DCMotor(double nominalVoltageVolts,
-                 double stallTorqueNewtonMeters,
-                 double stallCurrentAmps,
-                 double freeCurrentAmps,
-                 double freeSpeedRadPerSec) {
-    this.m_nominalVoltageVolts = nominalVoltageVolts;
-    this.m_stallTorqueNewtonMeters = stallTorqueNewtonMeters;
-    this.m_stallCurrentAmps = stallCurrentAmps;
-    this.m_freeCurrentAmps = freeCurrentAmps;
-    this.m_freeSpeedRadPerSec = freeSpeedRadPerSec;
-
-    this.m_rOhms = nominalVoltageVolts / stallCurrentAmps;
-    this.m_KvRadPerSecPerVolt = freeSpeedRadPerSec / (nominalVoltageVolts - m_rOhms
-      * freeCurrentAmps);
-    this.m_KtNMPerAmp = stallTorqueNewtonMeters / stallCurrentAmps;
-  }
-
-  /**
-   * Estimate the current being drawn by this motor.
-   *
-   * @param speedRadiansPerSec The speed of the rotor.
-   * @param voltageInputVolts  The input voltage.
-   */
-  public double getCurrent(double speedRadiansPerSec, double voltageInputVolts) {
-    return -1.0 / m_KvRadPerSecPerVolt / m_rOhms * speedRadiansPerSec
-      + 1.0 / m_rOhms * voltageInputVolts;
-  }
-
-  /**
-   * Return a gearbox of CIM motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getCIM(int numMotors) {
-    return new DCMotor(12,
-      2.42 * numMotors, 133,
-      2.7, Units.rotationsPerMinuteToRadiansPerSecond(5310));
-  }
-
-  /**
-   * Return a gearbox of 775Pro motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getVex775Pro(int numMotors) {
-    return gearbox(new DCMotor(12,
-      0.71, 134,
-      0.7, Units.rotationsPerMinuteToRadiansPerSecond(18730)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of NEO motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getNEO(int numMotors) {
-    return gearbox(new DCMotor(12, 2.6,
-      105, 1.8, Units.rotationsPerMinuteToRadiansPerSecond(5676)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of MiniCIM motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getMiniCIM(int numMotors) {
-    return gearbox(new DCMotor(12, 1.41, 89, 3,
-      Units.rotationsPerMinuteToRadiansPerSecond(5840)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Bag motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getBag(int numMotors) {
-    return gearbox(new DCMotor(12, 0.43, 53, 1.8,
-      Units.rotationsPerMinuteToRadiansPerSecond(13180)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Andymark RS775-125 motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getAndymarkRs775_125(int numMotors) {
-    return gearbox(new DCMotor(12, 0.28, 18, 1.6,
-      Units.rotationsPerMinuteToRadiansPerSecond(5800.0)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Banebots RS775 motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getBanebotsRs775(int numMotors) {
-    return gearbox(new DCMotor(12, 0.72, 97, 2.7,
-      Units.rotationsPerMinuteToRadiansPerSecond(13050.0)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Andymark 9015 motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getAndymark9015(int numMotors) {
-    return gearbox(new DCMotor(12, 0.36, 71, 3.7,
-      Units.rotationsPerMinuteToRadiansPerSecond(14270.0)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Banebots RS 550 motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getBanebotsRs550(int numMotors) {
-    return gearbox(new DCMotor(12, 0.38, 84, 0.4,
-      Units.rotationsPerMinuteToRadiansPerSecond(19000.0)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Neo 550 motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getNeo550(int numMotors) {
-    return gearbox(new DCMotor(12, 0.97, 100, 1.4,
-      Units.rotationsPerMinuteToRadiansPerSecond(11000.0)), numMotors);
-  }
-
-  /**
-   * Return a gearbox of Falcon 500 motors.
-   *
-   * @param numMotors Number of motors in the gearbox.
-   */
-  public static DCMotor getFalcon500(int numMotors) {
-    return gearbox(new DCMotor(12, 4.69, 257, 1.5,
-      Units.rotationsPerMinuteToRadiansPerSecond(6380.0)), numMotors);
-  }
-
-  private static DCMotor gearbox(DCMotor motor, double numMotors) {
-    return new DCMotor(motor.m_nominalVoltageVolts, motor.m_stallTorqueNewtonMeters * numMotors,
-      motor.m_stallCurrentAmps, motor.m_freeCurrentAmps, motor.m_freeSpeedRadPerSec);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
deleted file mode 100644
index 25d1161..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/system/plant/LinearSystemId.java
+++ /dev/null
@@ -1,207 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system.plant;
-
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-
-public final class LinearSystemId {
-  private LinearSystemId() {
-    // Utility class
-  }
-
-  /**
-   * Create a state-space model of an elevator system.
-   *
-   * @param motor        The motor (or gearbox) attached to the arm.
-   * @param massKg       The mass of the elevator carriage, in kilograms.
-   * @param radiusMeters The radius of thd driving drum of the elevator, in meters.
-   * @param G            The reduction between motor and drum, as a ratio of output to input.
-   * @return A LinearSystem representing the given characterized constants.
-   */
-  @SuppressWarnings("ParameterName")
-  public static LinearSystem<N2, N1, N1> createElevatorSystem(DCMotor motor, double massKg,
-                                                              double radiusMeters, double G) {
-    return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1,
-                    0, -Math.pow(G, 2) * motor.m_KtNMPerAmp
-                            / (motor.m_rOhms * radiusMeters * radiusMeters * massKg
-                            * motor.m_KvRadPerSecPerVolt)),
-            VecBuilder.fill(
-                    0, G * motor.m_KtNMPerAmp / (motor.m_rOhms * radiusMeters * massKg)),
-            Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
-            new Matrix<>(Nat.N1(), Nat.N1()));
-  }
-
-  /**
-   * Create a state-space model of a flywheel system.
-   *
-   * @param motor            The motor (or gearbox) attached to the arm.
-   * @param jKgMetersSquared The moment of inertia J of the flywheel.
-   * @param G                The reduction between motor and drum, as a ratio of output to input.
-   * @return A LinearSystem representing the given characterized constants.
-   */
-  @SuppressWarnings("ParameterName")
-  public static LinearSystem<N1, N1, N1> createFlywheelSystem(DCMotor motor,
-                                                              double jKgMetersSquared,
-                                                              double G) {
-    return new LinearSystem<>(
-        VecBuilder.fill(
-                    -G * G * motor.m_KtNMPerAmp
-                            / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgMetersSquared)),
-            VecBuilder.fill(G * motor.m_KtNMPerAmp
-                    / (motor.m_rOhms * jKgMetersSquared)),
-            Matrix.eye(Nat.N1()),
-            new Matrix<>(Nat.N1(), Nat.N1()));
-  }
-
-  /**
-   * Create a state-space model of a differential drive drivetrain. In this model, the
-   * states are [v_left, v_right]^T, inputs are [V_left, V_right]^T and outputs are
-   * [v_left, v_right]^T.
-   *
-   * @param motor            the gearbox representing the motors driving the drivetrain.
-   * @param massKg           the mass of the robot.
-   * @param rMeters          the radius of the wheels in meters.
-   * @param rbMeters         the radius of the base (half the track width) in meters.
-   * @param JKgMetersSquared the moment of inertia of the robot.
-   * @param G                the gearing reduction as output over input.
-   * @return A LinearSystem representing a differential drivetrain.
-   */
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
-  public static LinearSystem<N2, N2, N2> createDrivetrainVelocitySystem(DCMotor motor,
-                                                                        double massKg,
-                                                                        double rMeters,
-                                                                        double rbMeters,
-                                                                        double JKgMetersSquared,
-                                                                        double G) {
-    var C1 =
-            -(G * G) * motor.m_KtNMPerAmp
-                    / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * rMeters * rMeters);
-    var C2 = G * motor.m_KtNMPerAmp / (motor.m_rOhms * rMeters);
-
-    final double C3 = 1 / massKg + rbMeters * rbMeters / JKgMetersSquared;
-    final double C4 = 1 / massKg - rbMeters * rbMeters / JKgMetersSquared;
-    var A = Matrix.mat(Nat.N2(), Nat.N2()).fill(
-            C3 * C1,
-            C4 * C1,
-            C4 * C1,
-            C3 * C1);
-    var B = Matrix.mat(Nat.N2(), Nat.N2()).fill(
-            C3 * C2,
-            C4 * C2,
-            C4 * C2,
-            C3 * C2);
-    var C = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0);
-    var D = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0);
-
-    return new LinearSystem<>(A, B, C, D);
-  }
-
-  /**
-   * Create a state-space model of a single jointed arm system.
-   *
-   * @param motor            The motor (or gearbox) attached to the arm.
-   * @param jKgSquaredMeters The moment of inertia J of the arm.
-   * @param G                the gearing between the motor and arm, in output over input.
-   *                         Most of the time this will be greater than 1.
-   * @return A LinearSystem representing the given characterized constants.
-   */
-  @SuppressWarnings("ParameterName")
-  public static LinearSystem<N2, N1, N1> createSingleJointedArmSystem(DCMotor motor,
-                                                                      double jKgSquaredMeters,
-                                                                      double G) {
-    return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1,
-                    0, -Math.pow(G, 2) * motor.m_KtNMPerAmp
-                            / (motor.m_KvRadPerSecPerVolt * motor.m_rOhms * jKgSquaredMeters)),
-            VecBuilder.fill(0, G * motor.m_KtNMPerAmp
-                    / (motor.m_rOhms * jKgSquaredMeters)),
-            Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0),
-            new Matrix<>(Nat.N1(), Nat.N1()));
-  }
-
-  /**
-   * Identify a velocity system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2).
-   * These constants cam be found using frc-characterization.
-   *
-   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
-   * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
-   *
-   * @param kV         The velocity gain, in volts per (units per second)
-   * @param kA         The acceleration gain, in volts per (units per second squared)
-   * @return A LinearSystem representing the given characterized constants.
-   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
-   * https://github.com/wpilibsuite/frc-characterization</a>
-   */
-  @SuppressWarnings("ParameterName")
-  public static LinearSystem<N1, N1, N1> identifyVelocitySystem(double kV, double kA) {
-    return new LinearSystem<>(
-        VecBuilder.fill(-kV / kA),
-            VecBuilder.fill(1.0 / kA),
-            VecBuilder.fill(1.0),
-            VecBuilder.fill(0.0));
-  }
-
-  /**
-   * Identify a position system from it's kV (volts/(unit/sec)) and kA (volts/(unit/sec^2).
-   * These constants cam be found using frc-characterization.
-   *
-   * <p>The distance unit you choose MUST be an SI unit (i.e. meters or radians). You can use
-   * the {@link edu.wpi.first.wpilibj.util.Units} class for converting between unit types.
-   *
-   * @param kV         The velocity gain, in volts per (units per second)
-   * @param kA         The acceleration gain, in volts per (units per second squared)
-   * @return A LinearSystem representing the given characterized constants.
-   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
-   * https://github.com/wpilibsuite/frc-characterization</a>
-   */
-  @SuppressWarnings("ParameterName")
-  public static LinearSystem<N2, N1, N1> identifyPositionSystem(double kV, double kA) {
-    return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 0.0, -kV / kA),
-            VecBuilder.fill(0.0, 1.0 / kA),
-            Matrix.mat(Nat.N1(), Nat.N2()).fill(1.0, 0.0),
-            VecBuilder.fill(0.0));
-  }
-
-  /**
-   * Identify a standard differential drive drivetrain, given the drivetrain's
-   * kV and kA in both linear (volts/(meter/sec) and volts/(meter/sec^2)) and
-   * angular (volts/(radian/sec) and volts/(radian/sec^2)) cases. This can be
-   * found using frc-characterization.
-   *
-   * @param kVLinear   The linear velocity gain, volts per (meter per second).
-   * @param kALinear   The linear acceleration gain, volts per (meter per second squared).
-   * @param kVAngular  The angular velocity gain, volts per (radians per second).
-   * @param kAAngular  The angular acceleration gain, volts per (radians per second squared).
-   * @return A LinearSystem representing the given characterized constants.
-   * @see <a href="https://github.com/wpilibsuite/frc-characterization">
-   * https://github.com/wpilibsuite/frc-characterization</a>
-   */
-  @SuppressWarnings("ParameterName")
-  public static LinearSystem<N2, N2, N2> identifyDrivetrainSystem(
-        double kVLinear, double kALinear, double kVAngular, double kAAngular) {
-
-    final double c = 0.5 / (kALinear * kAAngular);
-    final double A1 = c * (-kALinear * kVAngular - kVLinear * kAAngular);
-    final double A2 = c * (kALinear * kVAngular - kVLinear * kAAngular);
-    final double B1 = c * (kALinear + kAAngular);
-    final double B2 = c * (kAAngular - kALinear);
-
-    return new LinearSystem<>(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(A1, A2, A2, A1),
-            Matrix.mat(Nat.N2(), Nat.N2()).fill(B1, B2, B2, B1),
-            Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1),
-            Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 0, 0, 0));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
deleted file mode 100644
index 7de2d84..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/Trajectory.java
+++ /dev/null
@@ -1,349 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Objects;
-import java.util.stream.Collectors;
-
-import com.fasterxml.jackson.annotation.JsonProperty;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-
-/**
- * Represents a time-parameterized trajectory. The trajectory contains of
- * various States that represent the pose, curvature, time elapsed, velocity,
- * and acceleration at that point.
- */
-public class Trajectory {
-  private final double m_totalTimeSeconds;
-  private final List<State> m_states;
-
-  /**
-   * Constructs an empty trajectory.
-   */
-  public Trajectory() {
-    m_states = new ArrayList<>();
-    m_totalTimeSeconds = 0.0;
-  }
-
-  /**
-   * Constructs a trajectory from a vector of states.
-   *
-   * @param states A vector of states.
-   */
-  public Trajectory(final List<State> states) {
-    m_states = states;
-    m_totalTimeSeconds = m_states.get(m_states.size() - 1).timeSeconds;
-  }
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  private static double lerp(double startValue, double endValue, double t) {
-    return startValue + (endValue - startValue) * t;
-  }
-
-  /**
-   * Linearly interpolates between two poses.
-   *
-   * @param startValue The start pose.
-   * @param endValue   The end pose.
-   * @param t          The fraction for interpolation.
-   * @return The interpolated pose.
-   */
-  @SuppressWarnings("ParameterName")
-  private static Pose2d lerp(Pose2d startValue, Pose2d endValue, double t) {
-    return startValue.plus((endValue.minus(startValue)).times(t));
-  }
-
-  /**
-   * Returns the initial pose of the trajectory.
-   *
-   * @return The initial pose of the trajectory.
-   */
-  public Pose2d getInitialPose() {
-    return sample(0).poseMeters;
-  }
-
-  /**
-   * Returns the overall duration of the trajectory.
-   *
-   * @return The duration of the trajectory.
-   */
-  public double getTotalTimeSeconds() {
-    return m_totalTimeSeconds;
-  }
-
-  /**
-   * Return the states of the trajectory.
-   *
-   * @return The states of the trajectory.
-   */
-  public List<State> getStates() {
-    return m_states;
-  }
-
-  /**
-   * Sample the trajectory at a point in time.
-   *
-   * @param timeSeconds The point in time since the beginning of the trajectory to sample.
-   * @return The state at that point in time.
-   */
-  public State sample(double timeSeconds) {
-    if (timeSeconds <= m_states.get(0).timeSeconds) {
-      return m_states.get(0);
-    }
-    if (timeSeconds >= m_totalTimeSeconds) {
-      return m_states.get(m_states.size() - 1);
-    }
-
-    // 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.get(mid).timeSeconds < timeSeconds) {
-        // 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.
-    final State sample = m_states.get(low);
-    final State prevSample = m_states.get(low - 1);
-
-    // If the difference in states is negligible, then we are spot on!
-    if (Math.abs(sample.timeSeconds - prevSample.timeSeconds) < 1E-9) {
-      return sample;
-    }
-    // Interpolate between the two states for the state that we want.
-    return prevSample.interpolate(sample,
-        (timeSeconds - prevSample.timeSeconds) / (sample.timeSeconds - prevSample.timeSeconds));
-  }
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public Trajectory transformBy(Transform2d transform) {
-    var firstState = m_states.get(0);
-    var firstPose = firstState.poseMeters;
-
-    // Calculate the transformed first pose.
-    var newFirstPose = firstPose.plus(transform);
-    List<State> newStates = new ArrayList<>();
-
-    newStates.add(new State(
-        firstState.timeSeconds, firstState.velocityMetersPerSecond,
-        firstState.accelerationMetersPerSecondSq, newFirstPose, firstState.curvatureRadPerMeter
-    ));
-
-    for (int i = 1; i < m_states.size(); i++) {
-      var state = m_states.get(i);
-      // We are transforming relative to the coordinate frame of the new initial pose.
-      newStates.add(new State(
-          state.timeSeconds, state.velocityMetersPerSecond,
-          state.accelerationMetersPerSecondSq, newFirstPose.plus(state.poseMeters.minus(firstPose)),
-          state.curvatureRadPerMeter
-      ));
-    }
-
-    return new Trajectory(newStates);
-  }
-
-  /**
-   * 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.
-   */
-  public Trajectory relativeTo(Pose2d pose) {
-    return new Trajectory(m_states.stream().map(state -> new State(state.timeSeconds,
-        state.velocityMetersPerSecond, state.accelerationMetersPerSecondSq,
-        state.poseMeters.relativeTo(pose), state.curvatureRadPerMeter))
-        .collect(Collectors.toList()));
-  }
-
-  /**
-   * Represents a time-parameterized trajectory. The trajectory contains of
-   * various States that represent the pose, curvature, time elapsed, velocity,
-   * and acceleration at that point.
-   */
-  @SuppressWarnings("MemberName")
-  public static class State {
-    // The time elapsed since the beginning of the trajectory.
-    @JsonProperty("time")
-    public double timeSeconds;
-
-    // The speed at that point of the trajectory.
-    @JsonProperty("velocity")
-    public double velocityMetersPerSecond;
-
-    // The acceleration at that point of the trajectory.
-    @JsonProperty("acceleration")
-    public double accelerationMetersPerSecondSq;
-
-    // The pose at that point of the trajectory.
-    @JsonProperty("pose")
-    public Pose2d poseMeters;
-
-    // The curvature at that point of the trajectory.
-    @JsonProperty("curvature")
-    public double curvatureRadPerMeter;
-
-    public State() {
-      poseMeters = new Pose2d();
-    }
-
-    /**
-     * Constructs a State with the specified parameters.
-     *
-     * @param timeSeconds                   The time elapsed since the beginning of the trajectory.
-     * @param velocityMetersPerSecond       The speed at that point of the trajectory.
-     * @param accelerationMetersPerSecondSq The acceleration at that point of the trajectory.
-     * @param poseMeters                    The pose at that point of the trajectory.
-     * @param curvatureRadPerMeter          The curvature at that point of the trajectory.
-     */
-    public State(double timeSeconds, double velocityMetersPerSecond,
-                 double accelerationMetersPerSecondSq, Pose2d poseMeters,
-                 double curvatureRadPerMeter) {
-      this.timeSeconds = timeSeconds;
-      this.velocityMetersPerSecond = velocityMetersPerSecond;
-      this.accelerationMetersPerSecondSq = accelerationMetersPerSecondSq;
-      this.poseMeters = poseMeters;
-      this.curvatureRadPerMeter = curvatureRadPerMeter;
-    }
-
-    /**
-     * Interpolates between two States.
-     *
-     * @param endValue The end value for the interpolation.
-     * @param i        The interpolant (fraction).
-     * @return The interpolated state.
-     */
-    @SuppressWarnings("ParameterName")
-    State interpolate(State endValue, double i) {
-      // Find the new t value.
-      final double newT = lerp(timeSeconds, endValue.timeSeconds, i);
-
-      // Find the delta time between the current state and the interpolated state.
-      final double deltaT = newT - timeSeconds;
-
-      // If delta time is negative, flip the order of interpolation.
-      if (deltaT < 0) {
-        return endValue.interpolate(this, 1 - i);
-      }
-
-      // Check whether the robot is reversing at this stage.
-      final boolean reversing = velocityMetersPerSecond < 0
-          || Math.abs(velocityMetersPerSecond) < 1E-9 && accelerationMetersPerSecondSq < 0;
-
-      // Calculate the new velocity
-      // v_f = v_0 + at
-      final double newV = velocityMetersPerSecond + (accelerationMetersPerSecondSq * deltaT);
-
-      // Calculate the change in position.
-      // delta_s = v_0 t + 0.5 at^2
-      final double newS = (velocityMetersPerSecond * deltaT
-          + 0.5 * accelerationMetersPerSecondSq * Math.pow(deltaT, 2)) * (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.
-      final double interpolationFrac = newS
-          / endValue.poseMeters.getTranslation().getDistance(poseMeters.getTranslation());
-
-      return new State(
-          newT, newV, accelerationMetersPerSecondSq,
-          lerp(poseMeters, endValue.poseMeters, interpolationFrac),
-          lerp(curvatureRadPerMeter, endValue.curvatureRadPerMeter, interpolationFrac)
-      );
-    }
-
-    @Override
-    public String toString() {
-      return String.format(
-        "State(Sec: %.2f, Vel m/s: %.2f, Accel m/s/s: %.2f, Pose: %s, Curvature: %.2f)",
-        timeSeconds, velocityMetersPerSecond, accelerationMetersPerSecondSq,
-        poseMeters, curvatureRadPerMeter);
-    }
-
-    @Override
-    public boolean equals(Object obj) {
-      if (this == obj) {
-        return true;
-      }
-      if (!(obj instanceof State)) {
-        return false;
-      }
-      State state = (State) obj;
-      return Double.compare(state.timeSeconds, timeSeconds) == 0
-              && Double.compare(state.velocityMetersPerSecond, velocityMetersPerSecond) == 0
-              && Double.compare(state.accelerationMetersPerSecondSq,
-                accelerationMetersPerSecondSq) == 0
-              && Double.compare(state.curvatureRadPerMeter, curvatureRadPerMeter) == 0
-              && Objects.equals(poseMeters, state.poseMeters);
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(timeSeconds, velocityMetersPerSecond,
-              accelerationMetersPerSecondSq, poseMeters, curvatureRadPerMeter);
-    }
-  }
-
-  @Override
-  public String toString() {
-    String stateList = m_states.stream().map(State::toString).collect(Collectors.joining(", \n"));
-    return String.format("Trajectory - Seconds: %.2f, States:\n%s", m_totalTimeSeconds, stateList);
-  }
-
-  @Override
-  public int hashCode() {
-    return m_states.hashCode();
-  }
-
-  @Override
-  public boolean equals(Object obj) {
-    return obj instanceof Trajectory && m_states.equals(((Trajectory) obj).getStates());
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
deleted file mode 100644
index 6c9b56a..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryConfig.java
+++ /dev/null
@@ -1,193 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.MecanumDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.SwerveDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
-
-/**
- * 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.
- *
- * <p>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.
- */
-public class TrajectoryConfig {
-  private final double m_maxVelocity;
-  private final double m_maxAcceleration;
-  private final List<TrajectoryConstraint> m_constraints;
-  private double m_startVelocity;
-  private double m_endVelocity;
-  private boolean m_reversed;
-
-  /**
-   * Constructs the trajectory configuration class.
-   *
-   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
-   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
-   */
-  public TrajectoryConfig(double maxVelocityMetersPerSecond,
-                          double maxAccelerationMetersPerSecondSq) {
-    m_maxVelocity = maxVelocityMetersPerSecond;
-    m_maxAcceleration = maxAccelerationMetersPerSecondSq;
-    m_constraints = new ArrayList<>();
-  }
-
-  /**
-   * Adds a user-defined constraint to the trajectory.
-   *
-   * @param constraint The user-defined constraint.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig addConstraint(TrajectoryConstraint constraint) {
-    m_constraints.add(constraint);
-    return this;
-  }
-
-  /**
-   * Adds all user-defined constraints from a list to the trajectory.
-   * @param constraints List of user-defined constraints.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig addConstraints(List<? extends TrajectoryConstraint> constraints) {
-    m_constraints.addAll(constraints);
-    return this;
-  }
-
-  /**
-   * 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.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setKinematics(DifferentialDriveKinematics kinematics) {
-    addConstraint(new DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
-    return this;
-  }
-
-  /**
-  * 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.
-  * @return Instance of the current config object.
-  */
-  public TrajectoryConfig setKinematics(MecanumDriveKinematics kinematics) {
-    addConstraint(new MecanumDriveKinematicsConstraint(kinematics, m_maxVelocity));
-    return this;
-  }
-
-  /**
-  * 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.
-  * @return Instance of the current config object.
-  */
-  public TrajectoryConfig setKinematics(SwerveDriveKinematics kinematics) {
-    addConstraint(new SwerveDriveKinematicsConstraint(kinematics, m_maxVelocity));
-    return this;
-  }
-
-  /**
-  * Returns the starting velocity of the trajectory.
-  *
-  * @return The starting velocity of the trajectory.
-  */
-  public double getStartVelocity() {
-    return m_startVelocity;
-  }
-
-  /**
-   * Sets the start velocity of the trajectory.
-   *
-   * @param startVelocityMetersPerSecond The start velocity of the trajectory.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setStartVelocity(double startVelocityMetersPerSecond) {
-    m_startVelocity = startVelocityMetersPerSecond;
-    return this;
-  }
-
-  /**
-   * Returns the starting velocity of the trajectory.
-   *
-   * @return The starting velocity of the trajectory.
-   */
-  public double getEndVelocity() {
-    return m_endVelocity;
-  }
-
-  /**
-   * Sets the end velocity of the trajectory.
-   *
-   * @param endVelocityMetersPerSecond The end velocity of the trajectory.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setEndVelocity(double endVelocityMetersPerSecond) {
-    m_endVelocity = endVelocityMetersPerSecond;
-    return this;
-  }
-
-  /**
-   * Returns the maximum velocity of the trajectory.
-   *
-   * @return The maximum velocity of the trajectory.
-   */
-  public double getMaxVelocity() {
-    return m_maxVelocity;
-  }
-
-  /**
-   * Returns the maximum acceleration of the trajectory.
-   *
-   * @return The maximum acceleration of the trajectory.
-   */
-  public double getMaxAcceleration() {
-    return m_maxAcceleration;
-  }
-
-  /**
-   * Returns the user-defined constraints of the trajectory.
-   *
-   * @return The user-defined constraints of the trajectory.
-   */
-  public List<TrajectoryConstraint> getConstraints() {
-    return m_constraints;
-  }
-
-  /**
-   * Returns whether the trajectory is reversed or not.
-   *
-   * @return whether the trajectory is reversed or not.
-   */
-  public boolean isReversed() {
-    return m_reversed;
-  }
-
-  /**
-   * Sets the reversed flag of the trajectory.
-   *
-   * @param reversed Whether the trajectory should be reversed or not.
-   * @return Instance of the current config object.
-   */
-  public TrajectoryConfig setReversed(boolean reversed) {
-    m_reversed = reversed;
-    return this;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
deleted file mode 100644
index 5e55c50..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
+++ /dev/null
@@ -1,278 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.Collection;
-import java.util.List;
-import java.util.function.BiConsumer;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
-import edu.wpi.first.wpilibj.spline.Spline;
-import edu.wpi.first.wpilibj.spline.SplineHelper;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
-
-public final class TrajectoryGenerator {
-  private static final Trajectory kDoNothingTrajectory =
-      new Trajectory(Arrays.asList(new Trajectory.State()));
-  private static BiConsumer<String, StackTraceElement[]> errorFunc;
-
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private TrajectoryGenerator() {
-  }
-
-  private static void reportError(String error, StackTraceElement[] stackTrace) {
-    if (errorFunc != null) {
-      errorFunc.accept(error, stackTrace);
-    } else {
-      MathSharedStore.reportError(error, stackTrace);
-    }
-  }
-
-  /**
-   * Set error reporting function. By default, DriverStation.reportError() is used.
-   *
-   * @param func Error reporting function, arguments are error and stackTrace.
-   */
-  public static void setErrorHandler(BiConsumer<String, StackTraceElement[]> func) {
-    errorFunc = func;
-  }
-
-  /**
-   * 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.
-   */
-  public static Trajectory generateTrajectory(
-      Spline.ControlVector initial,
-      List<Translation2d> interiorWaypoints,
-      Spline.ControlVector end,
-      TrajectoryConfig config
-  ) {
-    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
-
-    // Clone the control vectors.
-    var newInitial = new Spline.ControlVector(initial.x, initial.y);
-    var newEnd = new Spline.ControlVector(end.x, end.y);
-
-    // Change the orientation if reversed.
-    if (config.isReversed()) {
-      newInitial.x[1] *= -1;
-      newInitial.y[1] *= -1;
-      newEnd.x[1] *= -1;
-      newEnd.y[1] *= -1;
-    }
-
-    // Get the spline points
-    List<PoseWithCurvature> points;
-    try {
-      points = splinePointsFromSplines(SplineHelper.getCubicSplinesFromControlVectors(newInitial,
-          interiorWaypoints.toArray(new Translation2d[0]), newEnd));
-    } catch (MalformedSplineException ex) {
-      reportError(ex.getMessage(), ex.getStackTrace());
-      return kDoNothingTrajectory;
-    }
-
-    // Change the points back to their original orientation.
-    if (config.isReversed()) {
-      for (var point : points) {
-        point.poseMeters = point.poseMeters.plus(flip);
-        point.curvatureRadPerMeter *= -1;
-      }
-    }
-
-    // Generate and return trajectory.
-    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
-        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
-        config.getMaxAcceleration(), config.isReversed());
-  }
-
-  /**
-   * 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.
-   */
-  public static Trajectory generateTrajectory(
-      Pose2d start, List<Translation2d> interiorWaypoints, Pose2d end,
-      TrajectoryConfig config
-  ) {
-    var controlVectors = SplineHelper.getCubicControlVectorsFromWaypoints(
-        start, interiorWaypoints.toArray(new Translation2d[0]), end
-    );
-
-    // Return the generated trajectory.
-    return generateTrajectory(controlVectors[0], interiorWaypoints, controlVectors[1], 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.
-   */
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public static Trajectory generateTrajectory(
-      ControlVectorList controlVectors,
-      TrajectoryConfig config
-  ) {
-    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
-    final var newControlVectors = new ArrayList<Spline.ControlVector>(controlVectors.size());
-
-    // Create a new control vector list, flipping the orientation if reversed.
-    for (final var vector : controlVectors) {
-      var newVector = new Spline.ControlVector(vector.x, vector.y);
-      if (config.isReversed()) {
-        newVector.x[1] *= -1;
-        newVector.y[1] *= -1;
-      }
-      newControlVectors.add(newVector);
-    }
-
-    // Get the spline points
-    List<PoseWithCurvature> points;
-    try {
-      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromControlVectors(
-          newControlVectors.toArray(new Spline.ControlVector[]{})
-      ));
-    } catch (MalformedSplineException ex) {
-      reportError(ex.getMessage(), ex.getStackTrace());
-      return kDoNothingTrajectory;
-    }
-
-    // Change the points back to their original orientation.
-    if (config.isReversed()) {
-      for (var point : points) {
-        point.poseMeters = point.poseMeters.plus(flip);
-        point.curvatureRadPerMeter *= -1;
-      }
-    }
-
-    // Generate and return trajectory.
-    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
-        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
-        config.getMaxAcceleration(), config.isReversed());
-
-  }
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public static Trajectory generateTrajectory(List<Pose2d> waypoints, TrajectoryConfig config) {
-    final var flip = new Transform2d(new Translation2d(), Rotation2d.fromDegrees(180.0));
-
-    List<Pose2d> newWaypoints = new ArrayList<>();
-    if (config.isReversed()) {
-      for (Pose2d originalWaypoint : waypoints) {
-        newWaypoints.add(originalWaypoint.plus(flip));
-      }
-    } else {
-      newWaypoints.addAll(waypoints);
-    }
-
-    // Get the spline points
-    List<PoseWithCurvature> points;
-    try {
-      points = splinePointsFromSplines(SplineHelper.getQuinticSplinesFromWaypoints(newWaypoints));
-    } catch (MalformedSplineException ex) {
-      reportError(ex.getMessage(), ex.getStackTrace());
-      return kDoNothingTrajectory;
-    }
-
-    // Change the points back to their original orientation.
-    if (config.isReversed()) {
-      for (var point : points) {
-        point.poseMeters = point.poseMeters.plus(flip);
-        point.curvatureRadPerMeter *= -1;
-      }
-    }
-
-    // Generate and return trajectory.
-    return TrajectoryParameterizer.timeParameterizeTrajectory(points, config.getConstraints(),
-        config.getStartVelocity(), config.getEndVelocity(), config.getMaxVelocity(),
-        config.getMaxAcceleration(), config.isReversed());
-  }
-
-  /**
-   * 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.
-   * @throws MalformedSplineException When the spline is malformed (e.g. has close adjacent points
-   *                                  with approximately opposing headings)
-   */
-  public static List<PoseWithCurvature> splinePointsFromSplines(
-      Spline[] splines) {
-    // Create the vector of spline points.
-    var splinePoints = new ArrayList<PoseWithCurvature>();
-
-    // Add the first point to the vector.
-    splinePoints.add(splines[0].getPoint(0.0));
-
-    // Iterate through the vector and parameterize each spline, adding the
-    // parameterized points to the final vector.
-    for (final var spline : splines) {
-      var 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.addAll(points.subList(1, points.size()));
-    }
-    return splinePoints;
-  }
-
-  // Work around type erasure signatures
-  @SuppressWarnings("serial")
-  public static class ControlVectorList extends ArrayList<Spline.ControlVector> {
-    public ControlVectorList(int initialCapacity) {
-      super(initialCapacity);
-    }
-
-    public ControlVectorList() {
-      super();
-    }
-
-    public ControlVectorList(Collection<? extends Spline.ControlVector> collection) {
-      super(collection);
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
deleted file mode 100644
index 3b1d2af..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryParameterizer.java
+++ /dev/null
@@ -1,318 +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.
- */
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import edu.wpi.first.wpilibj.spline.PoseWithCurvature;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
-
-/**
- * Class used to parameterize a trajectory by time.
- */
-public final class TrajectoryParameterizer {
-  /**
-   * Private constructor because this is a utility class.
-   */
-  private TrajectoryParameterizer() {
-  }
-
-  /**
-   * Parameterize the trajectory by time. This is where the velocity profile is
-   * generated.
-   *
-   * <p>The derivation of the algorithm used can be found
-   * <a href="http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf">
-   * here</a>.
-   *
-   * @param points                           Reference to the spline points.
-   * @param constraints                      A vector of various velocity and acceleration.
-   *                                         constraints.
-   * @param startVelocityMetersPerSecond     The start velocity for the trajectory.
-   * @param endVelocityMetersPerSecond       The end velocity for the trajectory.
-   * @param maxVelocityMetersPerSecond       The max velocity for the trajectory.
-   * @param maxAccelerationMetersPerSecondSq The max acceleration for the trajectory.
-   * @param reversed                         Whether the robot should move backwards.
-   *                                         Note that the robot will still move from
-   *                                         a -&gt; b -&gt; ... -&gt; z as defined in the
-   *                                         waypoints.
-   * @return The trajectory.
-   */
-  @SuppressWarnings({"PMD.ExcessiveMethodLength", "PMD.CyclomaticComplexity",
-      "PMD.NPathComplexity", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public static Trajectory timeParameterizeTrajectory(
-      List<PoseWithCurvature> points,
-      List<TrajectoryConstraint> constraints,
-      double startVelocityMetersPerSecond,
-      double endVelocityMetersPerSecond,
-      double maxVelocityMetersPerSecond,
-      double maxAccelerationMetersPerSecondSq,
-      boolean reversed
-  ) {
-    var constrainedStates = new ArrayList<ConstrainedState>(points.size());
-    var predecessor = new ConstrainedState(points.get(0), 0, startVelocityMetersPerSecond,
-        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
-
-    // Forward pass
-    for (int i = 0; i < points.size(); i++) {
-      constrainedStates.add(new ConstrainedState());
-      var constrainedState = constrainedStates.get(i);
-      constrainedState.pose = points.get(i);
-
-      // Begin constraining based on predecessor.
-      double ds = constrainedState.pose.poseMeters.getTranslation().getDistance(
-          predecessor.pose.poseMeters.getTranslation());
-      constrainedState.distanceMeters = predecessor.distanceMeters + ds;
-
-      // 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.maxVelocityMetersPerSecond = Math.min(
-            maxVelocityMetersPerSecond,
-            Math.sqrt(predecessor.maxVelocityMetersPerSecond
-                * predecessor.maxVelocityMetersPerSecond
-                + predecessor.maxAccelerationMetersPerSecondSq * ds * 2.0)
-        );
-
-        constrainedState.minAccelerationMetersPerSecondSq = -maxAccelerationMetersPerSecondSq;
-        constrainedState.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
-
-        // At this point, the constrained state is fully constructed apart from
-        // all the custom-defined user constraints.
-        for (final var constraint : constraints) {
-          constrainedState.maxVelocityMetersPerSecond = Math.min(
-              constrainedState.maxVelocityMetersPerSecond,
-              constraint.getMaxVelocityMetersPerSecond(
-                  constrainedState.pose.poseMeters, constrainedState.pose.curvatureRadPerMeter,
-                  constrainedState.maxVelocityMetersPerSecond)
-          );
-        }
-
-        // Now enforce all acceleration limits.
-        enforceAccelerationLimits(reversed, constraints, constrainedState);
-
-        if (ds < 1E-6) {
-          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.
-        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
-            * constrainedState.maxVelocityMetersPerSecond
-            - predecessor.maxVelocityMetersPerSecond * predecessor.maxVelocityMetersPerSecond)
-            / (ds * 2.0);
-
-        // If we violate the max acceleration constraint, let's modify the
-        // predecessor.
-        if (constrainedState.maxAccelerationMetersPerSecondSq < actualAcceleration - 1E-6) {
-          predecessor.maxAccelerationMetersPerSecondSq
-              = constrainedState.maxAccelerationMetersPerSecondSq;
-        } else {
-          // Constrain the predecessor's max acceleration to the current
-          // acceleration.
-          if (actualAcceleration > predecessor.minAccelerationMetersPerSecondSq) {
-            predecessor.maxAccelerationMetersPerSecondSq = actualAcceleration;
-          }
-          // If the actual acceleration is less than the predecessor's min
-          // acceleration, it will be repaired in the backward pass.
-          break;
-        }
-      }
-      predecessor = constrainedState;
-    }
-
-    var successor = new ConstrainedState(points.get(points.size() - 1),
-        constrainedStates.get(constrainedStates.size() - 1).distanceMeters,
-        endVelocityMetersPerSecond,
-        -maxAccelerationMetersPerSecondSq, maxAccelerationMetersPerSecondSq);
-
-    // Backward pass
-    for (int i = points.size() - 1; i >= 0; i--) {
-      var constrainedState = constrainedStates.get(i);
-      double ds = constrainedState.distanceMeters - successor.distanceMeters; // negative
-
-      while (true) {
-        // Enforce max velocity limit (reverse)
-        // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
-        double newMaxVelocity = Math.sqrt(
-            successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond
-                + successor.minAccelerationMetersPerSecondSq * ds * 2.0
-        );
-
-        // No more limits to impose! This state can be finalized.
-        if (newMaxVelocity >= constrainedState.maxVelocityMetersPerSecond) {
-          break;
-        }
-
-        constrainedState.maxVelocityMetersPerSecond = newMaxVelocity;
-
-        // Check all acceleration constraints with the new max velocity.
-        enforceAccelerationLimits(reversed, constraints, constrainedState);
-
-        if (ds > -1E-6) {
-          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.
-        double actualAcceleration = (constrainedState.maxVelocityMetersPerSecond
-            * constrainedState.maxVelocityMetersPerSecond
-            - successor.maxVelocityMetersPerSecond * successor.maxVelocityMetersPerSecond)
-            / (ds * 2.0);
-
-        if (constrainedState.minAccelerationMetersPerSecondSq > actualAcceleration + 1E-6) {
-          successor.minAccelerationMetersPerSecondSq
-              = constrainedState.minAccelerationMetersPerSecondSq;
-        } else {
-          successor.minAccelerationMetersPerSecondSq = actualAcceleration;
-          break;
-        }
-      }
-      successor = constrainedState;
-    }
-
-    // Now we can integrate the constrained states forward in time to obtain our
-    // trajectory states.
-    var states = new ArrayList<Trajectory.State>(points.size());
-    double timeSeconds = 0.0;
-    double distanceMeters = 0.0;
-    double velocityMetersPerSecond = 0.0;
-
-    for (int i = 0; i < constrainedStates.size(); i++) {
-      final var state = constrainedStates.get(i);
-
-      // Calculate the change in position between the current state and the previous
-      // state.
-      double ds = state.distanceMeters - distanceMeters;
-
-      // Calculate the acceleration between the current state and the previous
-      // state.
-      double accel = (state.maxVelocityMetersPerSecond * state.maxVelocityMetersPerSecond
-          - velocityMetersPerSecond * velocityMetersPerSecond) / (ds * 2);
-
-      // Calculate dt
-      double dt = 0.0;
-      if (i > 0) {
-        states.get(i - 1).accelerationMetersPerSecondSq = reversed ? -accel : accel;
-        if (Math.abs(accel) > 1E-6) {
-          // v_f = v_0 + a * t
-          dt = (state.maxVelocityMetersPerSecond - velocityMetersPerSecond) / accel;
-        } else if (Math.abs(velocityMetersPerSecond) > 1E-6) {
-          // delta_x = v * t
-          dt = ds / velocityMetersPerSecond;
-        } else {
-          throw new TrajectoryGenerationException("Something went wrong at iteration " + i
-              + " of time parameterization.");
-        }
-      }
-
-      velocityMetersPerSecond = state.maxVelocityMetersPerSecond;
-      distanceMeters = state.distanceMeters;
-
-      timeSeconds += dt;
-
-      states.add(new Trajectory.State(
-          timeSeconds,
-          reversed ? -velocityMetersPerSecond : velocityMetersPerSecond,
-          reversed ? -accel : accel,
-          state.pose.poseMeters, state.pose.curvatureRadPerMeter
-      ));
-    }
-
-    return new Trajectory(states);
-  }
-
-  private static void enforceAccelerationLimits(boolean reverse,
-                                                List<TrajectoryConstraint> constraints,
-                                                ConstrainedState state) {
-
-    for (final var constraint : constraints) {
-      double factor = reverse ? -1.0 : 1.0;
-      final var minMaxAccel = constraint.getMinMaxAccelerationMetersPerSecondSq(
-          state.pose.poseMeters, state.pose.curvatureRadPerMeter,
-          state.maxVelocityMetersPerSecond * factor);
-
-      if (minMaxAccel.minAccelerationMetersPerSecondSq
-          > minMaxAccel.maxAccelerationMetersPerSecondSq) {
-        throw new TrajectoryGenerationException("The constraint's min acceleration "
-            + "was greater than its max acceleration.\n Offending Constraint: "
-            + constraint.getClass().getName()
-            + "\n If the offending constraint was packaged with WPILib, please file a bug report.");
-      }
-
-      state.minAccelerationMetersPerSecondSq = Math.max(state.minAccelerationMetersPerSecondSq,
-          reverse ? -minMaxAccel.maxAccelerationMetersPerSecondSq
-              : minMaxAccel.minAccelerationMetersPerSecondSq);
-
-      state.maxAccelerationMetersPerSecondSq = Math.min(state.maxAccelerationMetersPerSecondSq,
-          reverse ? -minMaxAccel.minAccelerationMetersPerSecondSq
-              : minMaxAccel.maxAccelerationMetersPerSecondSq);
-    }
-
-  }
-
-  @SuppressWarnings("MemberName")
-  private static class ConstrainedState {
-    PoseWithCurvature pose;
-    double distanceMeters;
-    double maxVelocityMetersPerSecond;
-    double minAccelerationMetersPerSecondSq;
-    double maxAccelerationMetersPerSecondSq;
-
-    ConstrainedState(PoseWithCurvature pose, double distanceMeters,
-                     double maxVelocityMetersPerSecond,
-                     double minAccelerationMetersPerSecondSq,
-                     double maxAccelerationMetersPerSecondSq) {
-      this.pose = pose;
-      this.distanceMeters = distanceMeters;
-      this.maxVelocityMetersPerSecond = maxVelocityMetersPerSecond;
-      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
-      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
-    }
-
-    ConstrainedState() {
-      pose = new PoseWithCurvature();
-    }
-  }
-
-  @SuppressWarnings("serial")
-  public static class TrajectoryGenerationException extends RuntimeException {
-    public TrajectoryGenerationException(String message) {
-      super(message);
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
deleted file mode 100644
index 0cd0f49..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryUtil.java
+++ /dev/null
@@ -1,76 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.io.BufferedReader;
-import java.io.BufferedWriter;
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Path;
-import java.util.Arrays;
-
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.databind.ObjectMapper;
-import com.fasterxml.jackson.databind.ObjectReader;
-import com.fasterxml.jackson.databind.ObjectWriter;
-
-public final class TrajectoryUtil {
-  private static final ObjectReader READER = new ObjectMapper().readerFor(Trajectory.State[].class);
-  private static final ObjectWriter WRITER = new ObjectMapper().writerFor(Trajectory.State[].class);
-
-  private TrajectoryUtil() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-
-  /**
-   * 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.
-   * @throws IOException if reading from the file fails
-   */
-  public static Trajectory fromPathweaverJson(Path path) throws IOException {
-    try (BufferedReader reader = Files.newBufferedReader(path)) {
-      Trajectory.State[] state = READER.readValue(reader);
-      return new Trajectory(Arrays.asList(state));
-    }
-  }
-
-  /**
-   * 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
-   * @throws IOException if writing to the file fails
-   */
-  public static void toPathweaverJson(Trajectory trajectory, Path path) throws IOException {
-    Files.createDirectories(path.getParent());
-    try (BufferedWriter writer = Files.newBufferedWriter(path)) {
-      WRITER.writeValue(writer, trajectory.getStates().toArray(new Trajectory.State[0]));
-    }
-  }
-
-  /**
-   * Deserializes a Trajectory from PathWeaver-style JSON.
-   * @param json the string containing the serialized JSON
-   * @return the trajectory represented by the JSON
-   * @throws JsonProcessingException if deserializing the JSON fails
-   */
-  public static Trajectory deserializeTrajectory(String json) throws JsonProcessingException {
-    Trajectory.State[] state = READER.readValue(json);
-    return new Trajectory(Arrays.asList(state));
-  }
-
-  /**
-   * Serializes a Trajectory to PathWeaver-style JSON.
-   * @param trajectory the trajectory to export
-   * @return the string containing the serialized JSON
-   * @throws JsonProcessingException if serializing the Trajectory fails
-   */
-  public static String serializeTrajectory(Trajectory trajectory) throws JsonProcessingException {
-    return WRITER.writeValueAsString(trajectory.getStates().toArray(new Trajectory.State[0]));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
deleted file mode 100644
index 8289d4d..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfile.java
+++ /dev/null
@@ -1,304 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Objects;
-
-import edu.wpi.first.math.MathSharedStore;
-import edu.wpi.first.math.MathUsageId;
-
-/**
- * A trapezoid-shaped velocity profile.
- *
- * <p>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.
- *
- * <p>Initialization:
- * <pre><code>
- * TrapezoidProfile.Constraints constraints =
- *   new TrapezoidProfile.Constraints(kMaxV, kMaxA);
- * TrapezoidProfile.State previousProfiledReference =
- *   new TrapezoidProfile.State(initialReference, 0.0);
- * </code></pre>
- *
- * <p>Run on update:
- * <pre><code>
- * TrapezoidProfile profile =
- *   new TrapezoidProfile(constraints, unprofiledReference, previousProfiledReference);
- * previousProfiledReference = profile.calculate(timeSincePreviousUpdate);
- * </code></pre>
- *
- * <p>where `unprofiledReference` is free to change between calls. Note that when
- * the unprofiled reference is within the constraints, `calculate()` returns the
- * unprofiled reference unchanged.
- *
- * <p>Otherwise, a timer can be started to provide monotonic values for
- * `calculate()` and to determine when the profile has completed via
- * `isFinished()`.
- */
-public class TrapezoidProfile {
-  // The direction of the profile, either 1 for forwards or -1 for inverted
-  private int m_direction;
-
-  private Constraints m_constraints;
-  private State m_initial;
-  private State m_goal;
-
-  private double m_endAccel;
-  private double m_endFullSpeed;
-  private double m_endDeccel;
-
-  public static class Constraints {
-    @SuppressWarnings("MemberName")
-    public double maxVelocity;
-    @SuppressWarnings("MemberName")
-    public double maxAcceleration;
-
-    public Constraints() {
-      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
-    }
-
-    /**
-     * Construct constraints for a TrapezoidProfile.
-     *
-     * @param maxVelocity maximum velocity
-     * @param maxAcceleration maximum acceleration
-     */
-    public Constraints(double maxVelocity, double maxAcceleration) {
-      this.maxVelocity = maxVelocity;
-      this.maxAcceleration = maxAcceleration;
-      MathSharedStore.reportUsage(MathUsageId.kTrajectory_TrapezoidProfile, 1);
-    }
-  }
-
-  public static class State {
-    @SuppressWarnings("MemberName")
-    public double position;
-    @SuppressWarnings("MemberName")
-    public double velocity;
-
-    public State() {
-    }
-
-    public State(double position, double velocity) {
-      this.position = position;
-      this.velocity = velocity;
-    }
-
-    @Override
-    public boolean equals(Object other) {
-      if (other instanceof State) {
-        State rhs = (State) other;
-        return this.position == rhs.position && this.velocity == rhs.velocity;
-      } else {
-        return false;
-      }
-    }
-
-    @Override
-    public int hashCode() {
-      return Objects.hash(position, velocity);
-    }
-  }
-
-  /**
-   * 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).
-   */
-  public 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
-    double cutoffBegin = m_initial.velocity / m_constraints.maxAcceleration;
-    double cutoffDistBegin = cutoffBegin * cutoffBegin * m_constraints.maxAcceleration / 2.0;
-
-    double cutoffEnd = m_goal.velocity / m_constraints.maxAcceleration;
-    double 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
-
-    double fullTrapezoidDist = cutoffDistBegin + (m_goal.position - m_initial.position)
-        + cutoffDistEnd;
-    double accelerationTime = m_constraints.maxVelocity / m_constraints.maxAcceleration;
-
-    double fullSpeedDist = fullTrapezoidDist - accelerationTime * accelerationTime
-        * m_constraints.maxAcceleration;
-
-    // Handle the case where the profile never reaches full speed
-    if (fullSpeedDist < 0) {
-      accelerationTime = Math.sqrt(fullTrapezoidDist / m_constraints.maxAcceleration);
-      fullSpeedDist = 0;
-    }
-
-    m_endAccel = accelerationTime - cutoffBegin;
-    m_endFullSpeed = m_endAccel + fullSpeedDist / m_constraints.maxVelocity;
-    m_endDeccel = m_endFullSpeed + accelerationTime - cutoffEnd;
-  }
-
-  /**
-   * Construct a TrapezoidProfile.
-   *
-   * @param constraints The constraints on the profile, like maximum velocity.
-   * @param goal        The desired state when the profile is complete.
-   */
-  public TrapezoidProfile(Constraints constraints, State goal) {
-    this(constraints, goal, new State(0, 0));
-  }
-
-  /**
-   * 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.
-   */
-  @SuppressWarnings("ParameterName")
-  public State calculate(double t) {
-    State result = new State(m_initial.position, m_initial.velocity);
-
-    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;
-      double 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);
-  }
-
-  /**
-   * Returns the time left until a target distance in the profile is reached.
-   *
-   * @param target The target distance.
-   */
-  public double timeLeftUntil(double target) {
-    double position = m_initial.position * m_direction;
-    double velocity = m_initial.velocity * m_direction;
-
-    double endAccel = m_endAccel * m_direction;
-    double endFullSpeed = m_endFullSpeed * m_direction - endAccel;
-
-    if (target < position) {
-      endAccel = -endAccel;
-      endFullSpeed = -endFullSpeed;
-      velocity = -velocity;
-    }
-
-    endAccel = Math.max(endAccel, 0);
-    endFullSpeed = Math.max(endFullSpeed, 0);
-    double endDeccel = m_endDeccel - endAccel - endFullSpeed;
-    endDeccel = Math.max(endDeccel, 0);
-
-    final double acceleration = m_constraints.maxAcceleration;
-    final double decceleration = -m_constraints.maxAcceleration;
-
-    double distToTarget = Math.abs(target - position);
-    if (distToTarget < 1e-6) {
-      return 0;
-    }
-
-    double accelDist = velocity * endAccel + 0.5 * acceleration * endAccel * endAccel;
-
-    double deccelVelocity;
-    if (endAccel > 0) {
-      deccelVelocity = Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration * accelDist));
-    } else {
-      deccelVelocity = velocity;
-    }
-
-    double deccelDist = deccelVelocity * endDeccel + 0.5 * decceleration * endDeccel * endDeccel;
-
-    deccelDist = Math.max(deccelDist, 0);
-
-    double fullSpeedDist = m_constraints.maxVelocity * endFullSpeed;
-
-    if (accelDist > distToTarget) {
-      accelDist = distToTarget;
-      fullSpeedDist = 0;
-      deccelDist = 0;
-    } else if (accelDist + fullSpeedDist > distToTarget) {
-      fullSpeedDist = distToTarget - accelDist;
-      deccelDist = 0;
-    } else {
-      deccelDist = distToTarget - fullSpeedDist - accelDist;
-    }
-
-    double accelTime = (-velocity + Math.sqrt(Math.abs(velocity * velocity + 2 * acceleration
-        * accelDist))) / acceleration;
-
-    double deccelTime = (-deccelVelocity + Math.sqrt(Math.abs(deccelVelocity * deccelVelocity
-        + 2 * decceleration * deccelDist))) / decceleration;
-
-    double fullSpeedTime = fullSpeedDist / m_constraints.maxVelocity;
-
-    return accelTime + fullSpeedTime + deccelTime;
-  }
-
-  /**
-   * Returns the total time the profile takes to reach the goal.
-   */
-  public double totalTime() {
-    return m_endDeccel;
-  }
-
-  /**
-   * Returns true if the profile has reached the goal.
-   *
-   * <p>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.
-   */
-  @SuppressWarnings("ParameterName")
-  public boolean isFinished(double t) {
-    return t >= totalTime();
-  }
-
-  /**
-   * Returns true if the profile inverted.
-   *
-   * <p>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.
-   */
-  private static boolean shouldFlipAcceleration(State initial, State goal) {
-    return initial.position > goal.position;
-  }
-
-  // Flip the sign of the velocity and position if the profile is inverted
-  private State direct(State in) {
-    State result = new State(in.position, in.velocity);
-    result.position = result.position * m_direction;
-    result.velocity = result.velocity * m_direction;
-    return result;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
deleted file mode 100644
index 0b87a64..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/CentripetalAccelerationConstraint.java
+++ /dev/null
@@ -1,73 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * 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.
- *
- * <p>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.
- */
-public class CentripetalAccelerationConstraint implements TrajectoryConstraint {
-  private final double m_maxCentripetalAccelerationMetersPerSecondSq;
-
-  /**
-   * Constructs a centripetal acceleration constraint.
-   *
-   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
-   */
-  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
-    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
-  }
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // ac = v^2 / r
-    // k (curvature) = 1 / r
-
-    // therefore, ac = v^2 * k
-    // ac / k = v^2
-    // v = std::sqrt(ac / k)
-
-    return Math.sqrt(m_maxCentripetalAccelerationMetersPerSecondSq
-        / Math.abs(curvatureRadPerMeter));
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    // The acceleration of the robot has no impact on the centripetal acceleration
-    // of the robot.
-    return new MinMax();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
deleted file mode 100644
index 67cddcf..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveKinematicsConstraint.java
+++ /dev/null
@@ -1,76 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-
-/**
- * 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.
- */
-public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
-  private final double m_maxSpeedMetersPerSecond;
-  private final DifferentialDriveKinematics m_kinematics;
-
-  /**
-   * Constructs a differential drive dynamics constraint.
-   *
-   * @param kinematics A kinematics component describing the drive geometry.
-   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
-   */
-  public DifferentialDriveKinematicsConstraint(final DifferentialDriveKinematics kinematics,
-                                               double maxSpeedMetersPerSecond) {
-    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
-    m_kinematics = kinematics;
-  }
-
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // Create an object to represent the current chassis speeds.
-    var chassisSpeeds = new ChassisSpeeds(velocityMetersPerSecond,
-        0, velocityMetersPerSecond * curvatureRadPerMeter);
-
-    // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
-
-    // Return the new linear chassis speed.
-    return m_kinematics.toChassisSpeeds(wheelSpeeds).vxMetersPerSecond;
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
deleted file mode 100644
index 9e28b0c..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/DifferentialDriveVoltageConstraint.java
+++ /dev/null
@@ -1,126 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-
-import static edu.wpi.first.wpiutil.ErrorMessages.requireNonNullParam;
-
-/**
- * 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.
- */
-public class DifferentialDriveVoltageConstraint implements TrajectoryConstraint {
-  private final SimpleMotorFeedforward m_feedforward;
-  private final DifferentialDriveKinematics m_kinematics;
-  private final double m_maxVoltage;
-
-  /**
-   * 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.
-   */
-  public DifferentialDriveVoltageConstraint(SimpleMotorFeedforward feedforward,
-                                            DifferentialDriveKinematics kinematics,
-                                            double maxVoltage) {
-    m_feedforward = requireNonNullParam(feedforward, "feedforward",
-                                        "DifferentialDriveVoltageConstraint");
-    m_kinematics = requireNonNullParam(kinematics, "kinematics",
-                                       "DifferentialDriveVoltageConstraint");
-    m_maxVoltage = maxVoltage;
-  }
-
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    return Double.POSITIVE_INFINITY;
-  }
-
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(new ChassisSpeeds(velocityMetersPerSecond, 0,
-                                                                   velocityMetersPerSecond
-                                                                       * curvatureRadPerMeter));
-
-    double maxWheelSpeed = Math.max(wheelSpeeds.leftMetersPerSecond,
-                                    wheelSpeeds.rightMetersPerSecond);
-    double minWheelSpeed = Math.min(wheelSpeeds.leftMetersPerSecond,
-                                    wheelSpeeds.rightMetersPerSecond);
-
-    // Calculate maximum/minimum possible accelerations from motor dynamics
-    // and max/min wheel speeds
-    double maxWheelAcceleration =
-        m_feedforward.maxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
-    double 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
-
-    // When velocity is zero, then wheel velocities are uniformly zero (robot cannot be
-    // turning on its center) - we have to treat this as a special case, as it breaks
-    // the signum function.  Both max and min acceleration are *reduced in magnitude*
-    // in this case.
-
-    double maxChassisAcceleration;
-    double minChassisAcceleration;
-
-    if (velocityMetersPerSecond == 0) {
-      maxChassisAcceleration =
-          maxWheelAcceleration
-              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
-      minChassisAcceleration =
-          minWheelAcceleration
-              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter) / 2);
-    } else {
-      maxChassisAcceleration =
-          maxWheelAcceleration
-              / (1 + m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
-              * Math.signum(velocityMetersPerSecond) / 2);
-      minChassisAcceleration =
-          minWheelAcceleration
-              / (1 - m_kinematics.trackWidthMeters * Math.abs(curvatureRadPerMeter)
-              * Math.signum(velocityMetersPerSecond) / 2);
-    }
-
-    // When turning about a point inside of the wheelbase (i.e. radius less than half
-    // the trackwidth), the inner wheel's direction changes, but the magnitude remains
-    // the same.  The formula above changes sign for the inner wheel when this happens.
-    // We can accurately account for this by simply negating the inner wheel.
-
-    if ((m_kinematics.trackWidthMeters / 2) > (1 / Math.abs(curvatureRadPerMeter))) {
-      if (velocityMetersPerSecond > 0) {
-        minChassisAcceleration = -minChassisAcceleration;
-      } else if (velocityMetersPerSecond < 0) {
-        maxChassisAcceleration = -maxChassisAcceleration;
-      }
-    }
-
-    return new MinMax(minChassisAcceleration, maxChassisAcceleration);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
deleted file mode 100644
index eb9f7e7..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/EllipticalRegionConstraint.java
+++ /dev/null
@@ -1,80 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * Enforces a particular constraint only within an elliptical region.
- */
-public class EllipticalRegionConstraint implements TrajectoryConstraint {
-  private final Translation2d m_center;
-  private final Translation2d m_radii;
-  private final TrajectoryConstraint m_constraint;
-
-  /**
-   * Constructs a new EllipticalRegionConstraint.
-   *
-   * @param center     The center of the ellipse in which to enforce the constraint.
-   * @param xWidth     The width of the ellipse in which to enforce the constraint.
-   * @param yWidth     The height of the ellipse in which to enforce the constraint.
-   * @param rotation   The rotation to apply to all radii around the origin.
-   * @param constraint The constraint to enforce when the robot is within the region.
-   */
-  @SuppressWarnings("ParameterName")
-  public EllipticalRegionConstraint(Translation2d center, double xWidth, double yWidth,
-                                    Rotation2d rotation, TrajectoryConstraint constraint) {
-    m_center = center;
-    m_radii = new Translation2d(xWidth / 2.0, yWidth / 2.0).rotateBy(rotation);
-    m_constraint = constraint;
-  }
-
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    if (isPoseInRegion(poseMeters)) {
-      return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter,
-          velocityMetersPerSecond);
-    } else {
-      return Double.POSITIVE_INFINITY;
-    }
-  }
-
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    if (isPoseInRegion(poseMeters)) {
-      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters,
-          curvatureRadPerMeter, velocityMetersPerSecond);
-    } else {
-      return new MinMax();
-    }
-  }
-
-  /**
-   * Returns whether the specified robot pose is within the region that the constraint
-   * is enforced in.
-   *
-   * @param robotPose The robot pose.
-   * @return Whether the robot pose is within the constraint region.
-   */
-  public boolean isPoseInRegion(Pose2d robotPose) {
-    // The region (disk) bounded by the ellipse is given by the equation:
-    // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
-    // If the inequality is satisfied, then it is inside the ellipse; otherwise
-    // it is outside the ellipse.
-    // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
-    return Math.pow(robotPose.getX() - m_center.getX(), 2)
-        * Math.pow(m_radii.getY(), 2)
-        + Math.pow(robotPose.getY() - m_center.getY(), 2)
-        * Math.pow(m_radii.getX(), 2) <= Math.pow(m_radii.getX(), 2) * Math.pow(m_radii.getY(), 2);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
deleted file mode 100644
index 4d60623..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MaxVelocityConstraint.java
+++ /dev/null
@@ -1,40 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * Represents a constraint that enforces a max velocity. This can be composed with the
- * {@link EllipticalRegionConstraint} or {@link RectangularRegionConstraint} to enforce
- * a max velocity in a region.
- */
-public class MaxVelocityConstraint implements TrajectoryConstraint {
-  private final double m_maxVelocity;
-
-  /**
-   * Constructs a new MaxVelocityConstraint.
-   *
-   * @param maxVelocityMetersPerSecond The max velocity.
-   */
-  public MaxVelocityConstraint(double maxVelocityMetersPerSecond) {
-    m_maxVelocity = maxVelocityMetersPerSecond;
-  }
-
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    return m_maxVelocity;
-  }
-
-  @Override
-  public TrajectoryConstraint.MinMax getMinMaxAccelerationMetersPerSecondSq(
-      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
deleted file mode 100644
index 6758d3d..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/MecanumDriveKinematicsConstraint.java
+++ /dev/null
@@ -1,84 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.MecanumDriveKinematics;
-
-/**
- * A class that enforces constraints on the mecanum drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for all 4 wheels of the drivetrain stay below a certain
- * limit.
- */
-public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint {
-  private final double m_maxSpeedMetersPerSecond;
-  private final MecanumDriveKinematics m_kinematics;
-
-  /**
-   * Constructs a mecanum drive dynamics constraint.
-   *
-   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
-   */
-  public MecanumDriveKinematicsConstraint(final MecanumDriveKinematics kinematics,
-                                               double maxSpeedMetersPerSecond) {
-    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
-    m_kinematics = kinematics;
-  }
-
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // Represents the velocity of the chassis in the x direction
-    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
-
-    // Represents the velocity of the chassis in the y direction
-    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
-
-    // Create an object to represent the current chassis speeds.
-    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
-        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
-
-    // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-    wheelSpeeds.normalize(m_maxSpeedMetersPerSecond);
-
-    // Convert normalized wheel speeds back to chassis speeds
-    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    // Return the new linear chassis speed.
-    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
deleted file mode 100644
index c25c74c..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/RectangularRegionConstraint.java
+++ /dev/null
@@ -1,73 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-/**
- * Enforces a particular constraint only within a rectangular region.
- */
-public class RectangularRegionConstraint implements TrajectoryConstraint {
-  private final Translation2d m_bottomLeftPoint;
-  private final Translation2d m_topRightPoint;
-  private final TrajectoryConstraint m_constraint;
-
-  /**
-   * Constructs a new RectangularRegionConstraint.
-   *
-   * @param bottomLeftPoint The bottom left point of the rectangular region in which to
-   *                        enforce the constraint.
-   * @param topRightPoint   The top right point of the rectangular region in which to enforce
-   *                        the constraint.
-   * @param constraint      The constraint to enforce when the robot is within the region.
-   */
-  public RectangularRegionConstraint(Translation2d bottomLeftPoint, Translation2d topRightPoint,
-                                     TrajectoryConstraint constraint) {
-    m_bottomLeftPoint = bottomLeftPoint;
-    m_topRightPoint = topRightPoint;
-    m_constraint = constraint;
-  }
-
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    if (isPoseInRegion(poseMeters)) {
-      return m_constraint.getMaxVelocityMetersPerSecond(poseMeters, curvatureRadPerMeter,
-          velocityMetersPerSecond);
-    } else {
-      return Double.POSITIVE_INFINITY;
-    }
-  }
-
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    if (isPoseInRegion(poseMeters)) {
-      return m_constraint.getMinMaxAccelerationMetersPerSecondSq(poseMeters,
-          curvatureRadPerMeter, velocityMetersPerSecond);
-    } else {
-      return new MinMax();
-    }
-  }
-
-  /**
-   * Returns whether the specified robot pose is within the region that the constraint
-   * is enforced in.
-   *
-   * @param robotPose The robot pose.
-   * @return Whether the robot pose is within the constraint region.
-   */
-  public boolean isPoseInRegion(Pose2d robotPose) {
-    return robotPose.getX() >= m_bottomLeftPoint.getX()
-        && robotPose.getX() <= m_topRightPoint.getX()
-        && robotPose.getY() >= m_bottomLeftPoint.getY()
-        && robotPose.getY() <= m_topRightPoint.getY();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
deleted file mode 100644
index 693bfd5..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/SwerveDriveKinematicsConstraint.java
+++ /dev/null
@@ -1,84 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.SwerveDriveKinematics;
-
-/**
- * A class that enforces constraints on the swerve drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities for all 4 wheels of the drivetrain stay below a certain
- * limit.
- */
-public class SwerveDriveKinematicsConstraint implements TrajectoryConstraint {
-  private final double m_maxSpeedMetersPerSecond;
-  private final SwerveDriveKinematics m_kinematics;
-
-  /**
-   * Constructs a swerve drive dynamics constraint.
-   *
-   * @param maxSpeedMetersPerSecond The max speed that a side of the robot can travel at.
-   */
-  public SwerveDriveKinematicsConstraint(final SwerveDriveKinematics kinematics,
-                                               double maxSpeedMetersPerSecond) {
-    m_maxSpeedMetersPerSecond = maxSpeedMetersPerSecond;
-    m_kinematics = kinematics;
-  }
-
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  @Override
-  public double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                              double velocityMetersPerSecond) {
-    // Represents the velocity of the chassis in the x direction
-    var xdVelocity = velocityMetersPerSecond * poseMeters.getRotation().getCos();
-
-    // Represents the velocity of the chassis in the y direction
-    var ydVelocity = velocityMetersPerSecond * poseMeters.getRotation().getSin();
-
-    // Create an object to represent the current chassis speeds.
-    var chassisSpeeds = new ChassisSpeeds(xdVelocity,
-        ydVelocity, velocityMetersPerSecond * curvatureRadPerMeter);
-
-    // Get the wheel speeds and normalize them to within the max velocity.
-    var wheelSpeeds = m_kinematics.toSwerveModuleStates(chassisSpeeds);
-    SwerveDriveKinematics.normalizeWheelSpeeds(wheelSpeeds, m_maxSpeedMetersPerSecond);
-
-    // Convert normalized wheel speeds back to chassis speeds
-    var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    // Return the new linear chassis speed.
-    return Math.hypot(normSpeeds.vxMetersPerSecond, normSpeeds.vyMetersPerSecond);
-  }
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  @Override
-  public MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters,
-                                                       double curvatureRadPerMeter,
-                                                       double velocityMetersPerSecond) {
-    return new MinMax();
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
deleted file mode 100644
index 5962404..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/trajectory/constraint/TrajectoryConstraint.java
+++ /dev/null
@@ -1,68 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory.constraint;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-
-/**
- * An interface for defining user-defined velocity and acceleration constraints
- * while generating trajectories.
- */
-public interface TrajectoryConstraint {
-
-  /**
-   * Returns the max velocity given the current pose and curvature.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
-   *                                constraints are applied.
-   * @return The absolute maximum velocity.
-   */
-  double getMaxVelocityMetersPerSecond(Pose2d poseMeters, double curvatureRadPerMeter,
-                                       double velocityMetersPerSecond);
-
-  /**
-   * Returns the minimum and maximum allowable acceleration for the trajectory
-   * given pose, curvature, and speed.
-   *
-   * @param poseMeters              The pose at the current point in the trajectory.
-   * @param curvatureRadPerMeter    The curvature at the current point in the trajectory.
-   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
-   * @return The min and max acceleration bounds.
-   */
-  MinMax getMinMaxAccelerationMetersPerSecondSq(Pose2d poseMeters, double curvatureRadPerMeter,
-                                                double velocityMetersPerSecond);
-
-  /**
-   * Represents a minimum and maximum acceleration.
-   */
-  @SuppressWarnings("MemberName")
-  class MinMax {
-    public double minAccelerationMetersPerSecondSq = -Double.MAX_VALUE;
-    public double maxAccelerationMetersPerSecondSq = +Double.MAX_VALUE;
-
-    /**
-     * Constructs a MinMax.
-     *
-     * @param minAccelerationMetersPerSecondSq The minimum acceleration.
-     * @param maxAccelerationMetersPerSecondSq The maximum acceleration.
-     */
-    public MinMax(double minAccelerationMetersPerSecondSq,
-                  double maxAccelerationMetersPerSecondSq) {
-      this.minAccelerationMetersPerSecondSq = minAccelerationMetersPerSecondSq;
-      this.maxAccelerationMetersPerSecondSq = maxAccelerationMetersPerSecondSq;
-    }
-
-    /**
-     * Constructs a MinMax with default values.
-     */
-    public MinMax() {
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java
deleted file mode 100644
index 3f48306..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpilibj/util/Units.java
+++ /dev/null
@@ -1,104 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.util;
-
-/**
- * Utility class that converts between commonly used units in FRC.
- */
-public final class Units {
-  private static final double kInchesPerFoot = 12.0;
-  private static final double kMetersPerInch = 0.0254;
-  private static final double kSecondsPerMinute = 60;
-
-  /**
-   * Utility class, so constructor is private.
-   */
-  private Units() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-
-  /**
-   * Converts given meters to feet.
-   *
-   * @param meters The meters to convert to feet.
-   * @return Feet converted from meters.
-   */
-  public static double metersToFeet(double meters) {
-    return metersToInches(meters) / kInchesPerFoot;
-  }
-
-  /**
-   * Converts given feet to meters.
-   *
-   * @param feet The feet to convert to meters.
-   * @return Meters converted from feet.
-   */
-  public static double feetToMeters(double feet) {
-    return inchesToMeters(feet * kInchesPerFoot);
-  }
-
-  /**
-   * Converts given meters to inches.
-   *
-   * @param meters The meters to convert to inches.
-   * @return Inches converted from meters.
-   */
-  public static double metersToInches(double meters) {
-    return meters / kMetersPerInch;
-  }
-
-  /**
-   * Converts given inches to meters.
-   *
-   * @param inches The inches to convert to meters.
-   * @return Meters converted from inches.
-   */
-  public static double inchesToMeters(double inches) {
-    return inches * kMetersPerInch;
-  }
-
-  /**
-   * Converts given degrees to radians.
-   *
-   * @param degrees The degrees to convert to radians.
-   * @return Radians converted from degrees.
-   */
-  public static double degreesToRadians(double degrees) {
-    return Math.toRadians(degrees);
-  }
-
-  /**
-   * Converts given radians to degrees.
-   *
-   * @param radians The radians to convert to degrees.
-   * @return Degrees converted from radians.
-   */
-  public static double radiansToDegrees(double radians) {
-    return Math.toDegrees(radians);
-  }
-
-  /**
-   * Converts rotations per minute to radians per second.
-   *
-   * @param rpm The rotations per minute to convert to radians per second.
-   * @return Radians per second converted from rotations per minute.
-   */
-  public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
-    return rpm * Math.PI / (kSecondsPerMinute / 2);
-  }
-
-  /**
-   * Converts radians per second to rotations per minute.
-   *
-   * @param radiansPerSecond The radians per second to convert to from rotations per minute.
-   * @return Rotations per minute converted from radians per second.
-   */
-  public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
-    return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
deleted file mode 100644
index d8b20a3..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatBuilder.java
+++ /dev/null
@@ -1,50 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.Objects;
-
-import org.ejml.simple.SimpleMatrix;
-
-/**
- * A class for constructing arbitrary RxC matrices.
- *
- * @param <R> The number of rows of the desired matrix.
- * @param <C> The number of columns of the desired matrix.
- */
-public class MatBuilder<R extends Num, C extends Num> {
-  final Nat<R> m_rows;
-  final Nat<C> m_cols;
-
-  /**
-   * Fills the matrix with the given data, encoded in row major form.
-   * (The matrix is filled row by row, left to right with the given data).
-   *
-   * @param data The data to fill the matrix with.
-   * @return The constructed matrix.
-   */
-  @SuppressWarnings("LineLength")
-  public final Matrix<R, C> fill(double... data) {
-    if (Objects.requireNonNull(data).length != this.m_rows.getNum() * this.m_cols.getNum()) {
-      throw new IllegalArgumentException("Invalid matrix data provided. Wanted " + this.m_rows.getNum()
-          + " x " + this.m_cols.getNum() + " matrix, but got " + data.length + " elements");
-    } else {
-      return new Matrix<>(new SimpleMatrix(this.m_rows.getNum(), this.m_cols.getNum(), true, data));
-    }
-  }
-
-  /**
-   * Creates a new {@link MatBuilder} with the given dimensions.
-   * @param rows The number of rows of the matrix.
-   * @param cols The number of columns of the matrix.
-   */
-  public MatBuilder(Nat<R> rows, Nat<C> cols) {
-    this.m_rows = Objects.requireNonNull(rows);
-    this.m_cols = Objects.requireNonNull(cols);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
deleted file mode 100644
index bd03f6b..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MathUtil.java
+++ /dev/null
@@ -1,56 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-public final class MathUtil {
-  private MathUtil() {
-    throw new AssertionError("utility class");
-  }
-
-  /**
-   * Returns value clamped between low and high boundaries.
-   *
-   * @param value Value to clamp.
-   * @param low   The lower boundary to which to clamp value.
-   * @param high  The higher boundary to which to clamp value.
-   */
-  public static int clamp(int value, int low, int high) {
-    return Math.max(low, Math.min(value, high));
-  }
-
-  /**
-   * Returns value clamped between low and high boundaries.
-   *
-   * @param value Value to clamp.
-   * @param low   The lower boundary to which to clamp value.
-   * @param high  The higher boundary to which to clamp value.
-   */
-  public static double clamp(double value, double low, double high) {
-    return Math.max(low, Math.min(value, high));
-  }
-
-  /**
-   * Constrains theta to within the range (-pi, pi].
-   *
-   * @param theta The angle to normalize.
-   * @return The normalized angle.
-   */
-  @SuppressWarnings("LocalVariableName")
-  public static double normalizeAngle(double theta) {
-    // Constraint theta to within (-3pi, pi)
-    int nPiPos = (int) ((theta + Math.PI) / 2.0 / Math.PI);
-    theta -= nPiPos * 2.0 * Math.PI;
-
-    // Cut off the bottom half of the above range to constrain within
-    // (-pi, pi]
-    int nPiNeg = (int) ((theta - Math.PI) / 2.0 / Math.PI);
-    theta -= nPiNeg * 2.0 * Math.PI;
-
-    return theta;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
deleted file mode 100644
index a87b98a..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Matrix.java
+++ /dev/null
@@ -1,678 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.Objects;
-
-import org.ejml.MatrixDimensionException;
-import org.ejml.data.DMatrixRMaj;
-import org.ejml.dense.row.CommonOps_DDRM;
-import org.ejml.dense.row.MatrixFeatures_DDRM;
-import org.ejml.dense.row.NormOps_DDRM;
-import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
-import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.math.WPIMathJNI;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
- *
- * <p>This class is intended to be used alongside the state space library.
- *
- * @param <R> The number of rows in this matrix.
- * @param <C> The number of columns in this matrix.
- */
-@SuppressWarnings("PMD.ExcessivePublicCount")
-public class Matrix<R extends Num, C extends Num> {
-  protected final SimpleMatrix m_storage;
-
-  /**
-   * Constructs an empty zero matrix of the given dimensions.
-   *
-   * @param rows    The number of rows of the matrix.
-   * @param columns The number of columns of the matrix.
-   */
-  public Matrix(Nat<R> rows, Nat<C> columns) {
-    this.m_storage = new SimpleMatrix(
-      Objects.requireNonNull(rows).getNum(),
-      Objects.requireNonNull(columns).getNum()
-    );
-  }
-
-  /**
-   * Constructs a new {@link Matrix} with the given storage.
-   * Caller should make sure that the provided generic bounds match
-   * the shape of the provided {@link Matrix}.
-   *
-   * <p>NOTE:It is not recommend to use this constructor unless the
-   * {@link SimpleMatrix} API is absolutely necessary due to the desired
-   * function not being accessible through the {@link Matrix} wrapper.
-   *
-   * @param storage The {@link SimpleMatrix} to back this value.
-   */
-  public Matrix(SimpleMatrix storage) {
-    this.m_storage = Objects.requireNonNull(storage);
-  }
-
-  /**
-   * Constructs a new matrix with the storage of the supplied matrix.
-   *
-   * @param other The {@link Matrix} to copy the storage of.
-   */
-  public Matrix(Matrix<R, C> other) {
-    this.m_storage = Objects.requireNonNull(other).getStorage().copy();
-  }
-
-  /**
-   * Gets the underlying {@link SimpleMatrix} that this {@link Matrix} wraps.
-   *
-   * <p>NOTE:The use of this method is heavily discouraged as this removes any
-   * guarantee of type safety. This should only be called if the {@link SimpleMatrix}
-   * API is absolutely necessary due to the desired function not being accessible through
-   * the {@link Matrix} wrapper.
-   *
-   * @return The underlying {@link SimpleMatrix} storage.
-   */
-  public SimpleMatrix getStorage() {
-    return m_storage;
-  }
-
-  /**
-   * Gets the number of columns in this matrix.
-   *
-   * @return The number of columns, according to the internal storage.
-   */
-  public final int getNumCols() {
-    return this.m_storage.numCols();
-  }
-
-  /**
-   * Gets the number of rows in this matrix.
-   *
-   * @return The number of rows, according to the internal storage.
-   */
-  public final int getNumRows() {
-    return this.m_storage.numRows();
-  }
-
-  /**
-   * Get an element of this matrix.
-   *
-   * @param row The row of the element.
-   * @param col The column of the element.
-   * @return The element in this matrix at row,col.
-   */
-  public final double get(int row, int col) {
-    return this.m_storage.get(row, col);
-  }
-
-  /**
-   * Sets the value at the given indices.
-   *
-   * @param row   The row of the element.
-   * @param col   The column of the element.
-   * @param value The value to insert at the given location.
-   */
-  public final void set(int row, int col, double value) {
-    this.m_storage.set(row, col, value);
-  }
-
-  /**
-   * Sets a row to a given row vector.
-   *
-   * @param row The row to set.
-   * @param val The row vector to set the given row to.
-   */
-  public final void setRow(int row, Matrix<N1, C> val) {
-    this.m_storage.setRow(row, 0,
-        Objects.requireNonNull(val).m_storage.getDDRM().getData());
-  }
-
-  /**
-   * Sets a column to a given column vector.
-   *
-   * @param column The column to set.
-   * @param val    The column vector to set the given row to.
-   */
-  public final void setColumn(int column, Matrix<R, N1> val) {
-    this.m_storage.setColumn(column, 0,
-        Objects.requireNonNull(val).m_storage.getDDRM().getData());
-  }
-
-
-  /**
-   * Sets all the elements in "this" matrix equal to the specified value.
-   *
-   * @param value The value each element is set to.
-   */
-  public void fill(double value) {
-    this.m_storage.fill(value);
-  }
-
-  /**
-   * Returns the diagonal elements inside a vector or square matrix.
-   *
-   * <p>If "this" {@link Matrix} is a vector then a square matrix is returned. If a "this"
-   * {@link Matrix} is a matrix then a vector of diagonal elements is returned.
-   *
-   * @return The diagonal elements inside a vector or a square matrix.
-   */
-  public final Matrix<R, C> diag() {
-    return new Matrix<>(this.m_storage.diag());
-  }
-
-  /**
-   * Returns the largest element of this matrix.
-   *
-   * @return The largest element of this matrix.
-   */
-  public final double max() {
-    return CommonOps_DDRM.elementMax(this.m_storage.getDDRM());
-  }
-
-  /**
-   * Returns the absolute value of the element in this matrix with the largest absolute value.
-   *
-   * @return The absolute value of the element with the largest absolute value.
-   */
-  public final double maxAbs() {
-    return CommonOps_DDRM.elementMaxAbs(this.m_storage.getDDRM());
-  }
-
-
-  /**
-   * Returns the smallest element of this matrix.
-   *
-   * @return The smallest element of this matrix.
-   */
-  public final double minInternal() {
-    return CommonOps_DDRM.elementMin(this.m_storage.getDDRM());
-  }
-
-  /**
-   * Calculates the mean of the elements in this matrix.
-   *
-   * @return The mean value of this matrix.
-   */
-  public final double mean() {
-    return this.elementSum() / (double) this.m_storage.getNumElements();
-  }
-
-  /**
-   * Multiplies this matrix with another that has C rows.
-   *
-   * <p>As matrix multiplication is only defined if the number of columns
-   * in the first matrix matches the number of rows in the second,
-   * this operation will fail to compile under any other circumstances.
-   *
-   * @param other The other matrix to multiply by.
-   * @param <C2>  The number of columns in the second matrix.
-   * @return The result of the matrix multiplication between "this" and the given matrix.
-   */
-  public final <C2 extends Num> Matrix<R, C2> times(Matrix<C, C2> other) {
-    return new Matrix<>(this.m_storage.mult(Objects.requireNonNull(other).m_storage));
-  }
-
-  /**
-   * Multiplies all the elements of this matrix by the given scalar.
-   *
-   * @param value The scalar value to multiply by.
-   * @return A new matrix with all the elements multiplied by the given value.
-   */
-  public Matrix<R, C> times(double value) {
-    return new Matrix<>(this.m_storage.scale(value));
-  }
-
-  /**
-   * Returns a matrix which is the result of an element by element multiplication of
-   * "this" and other.
-   *
-   * <p>c<sub>i,j</sub> = a<sub>i,j</sub>*other<sub>i,j</sub>
-   *
-   *
-   * @param other The other {@link Matrix} to preform element multiplication on.
-   * @return The element by element multiplication of "this" and other.
-   */
-  public final Matrix<R, C> elementTimes(Matrix<R, C> other) {
-    return new Matrix<>(this.m_storage.elementMult(Objects.requireNonNull(other).m_storage));
-  }
-
-  /**
-   * Subtracts the given value from all the elements of this matrix.
-   *
-   * @param value The value to subtract.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> minus(double value) {
-    return new Matrix<>(this.m_storage.minus(value));
-  }
-
-
-  /**
-   * Subtracts the given matrix from this matrix.
-   *
-   * @param value The matrix to subtract.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> minus(Matrix<R, C> value) {
-    return new Matrix<>(this.m_storage.minus(Objects.requireNonNull(value).m_storage));
-  }
-
-
-  /**
-   * Adds the given value to all the elements of this matrix.
-   *
-   * @param value The value to add.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> plus(double value) {
-    return new Matrix<>(this.m_storage.plus(value));
-  }
-
-  /**
-   * Adds the given matrix to this matrix.
-   *
-   * @param value The matrix to add.
-   * @return The resultant matrix.
-   */
-  public final Matrix<R, C> plus(Matrix<R, C> value) {
-    return new Matrix<>(this.m_storage.plus(Objects.requireNonNull(value).m_storage));
-  }
-
-  /**
-   * Divides all elements of this matrix by the given value.
-   *
-   * @param value The value to divide by.
-   * @return The resultant matrix.
-   */
-  public Matrix<R, C> div(int value) {
-    return new Matrix<>(this.m_storage.divide((double) value));
-  }
-
-  /**
-   * Divides all elements of this matrix by the given value.
-   *
-   * @param value The value to divide by.
-   * @return The resultant matrix.
-   */
-  public Matrix<R, C> div(double value) {
-    return new Matrix<>(this.m_storage.divide(value));
-  }
-
-  /**
-   * Calculates the transpose, M^T of this matrix.
-   *
-   * @return The transpose matrix.
-   */
-  public final Matrix<C, R> transpose() {
-    return new Matrix<>(this.m_storage.transpose());
-  }
-
-
-  /**
-   * Returns a copy of this matrix.
-   *
-   * @return A copy of this matrix.
-   */
-  public final Matrix<R, C> copy() {
-    return new Matrix<>(this.m_storage.copy());
-  }
-
-
-  /**
-   * Returns the inverse matrix of "this" matrix.
-   *
-   * @return The inverse of "this" matrix.
-   * @throws org.ejml.data.SingularMatrixException If "this" matrix is non-invertable.
-   */
-  public final Matrix<R, C> inv() {
-    return new Matrix<>(this.m_storage.invert());
-  }
-
-  /**
-   * Returns the solution x to the equation Ax = b, where A is "this" matrix.
-   *
-   * <p>The matrix equation could also be written as x = A<sup>-1</sup>b. Where the
-   * pseudo inverse is used if A is not square.
-   *
-   * @param b The right-hand side of the equation to solve.
-   * @return The solution to the linear system.
-   */
-  @SuppressWarnings("ParameterName")
-  public final <C2 extends Num> Matrix<C, C2> solve(Matrix<R, C2> b) {
-    return new Matrix<>(this.m_storage.solve(Objects.requireNonNull(b).m_storage));
-  }
-
-  /**
-   * Computes the matrix exponential using Eigen's solver.
-   * This method only works for square matrices, and will
-   * otherwise throw an {@link MatrixDimensionException}.
-   *
-   * @return The exponential of A.
-   */
-  public final Matrix<R, C> exp() {
-    if (this.getNumRows() != this.getNumCols()) {
-      throw new MatrixDimensionException("Non-square matrices cannot be exponentiated! "
-            + "This matrix is " + this.getNumRows() + " x " + this.getNumCols());
-    }
-    Matrix<R, C> toReturn = new Matrix<>(new SimpleMatrix(this.getNumRows(), this.getNumCols()));
-    WPIMathJNI.exp(this.m_storage.getDDRM().getData(), this.getNumRows(),
-          toReturn.m_storage.getDDRM().getData());
-    return toReturn;
-  }
-
-  /**
-   * Returns the determinant of this matrix.
-   *
-   * @return The determinant of this matrix.
-   */
-  public final double det() {
-    return this.m_storage.determinant();
-  }
-
-  /**
-   * Computes the Frobenius normal of the matrix.
-   *
-   * <p>normF = Sqrt{  &sum;<sub>i=1:m</sub> &sum;<sub>j=1:n</sub> { a<sub>ij</sub><sup>2</sup>}   }
-   *
-   * @return The matrix's Frobenius normal.
-   */
-  public final double normF() {
-    return this.m_storage.normF();
-  }
-
-  /**
-   * Computes the induced p = 1 matrix norm.
-   *
-   * <p>||A||<sub>1</sub>= max(j=1 to n; sum(i=1 to m; |a<sub>ij</sub>|))
-   *
-   * @return The norm.
-   */
-  public final double normIndP1() {
-    return NormOps_DDRM.inducedP1(this.m_storage.getDDRM());
-  }
-
-  /**
-   * Computes the sum of all the elements in the matrix.
-   *
-   * @return Sum of all the elements.
-   */
-  public final double elementSum() {
-    return this.m_storage.elementSum();
-  }
-
-  /**
-   * Computes the trace of the matrix.
-   *
-   * @return The trace of the matrix.
-   */
-  public final double trace() {
-    return this.m_storage.trace();
-  }
-
-  /**
-   * Returns a matrix which is the result of an element by element power of "this" and b.
-   *
-   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
-   *
-   * @param b Scalar.
-   * @return The element by element power of "this" and b.
-   */
-  @SuppressWarnings("ParameterName")
-  public final Matrix<R, C> elementPower(double b) {
-    return new Matrix<>(this.m_storage.elementPower(b));
-  }
-
-  /**
-   * Returns a matrix which is the result of an element by element power of "this" and b.
-   *
-   * <p>c<sub>i,j</sub> = a<sub>i,j</sub> ^ b
-   *
-   * @param b Scalar.
-   * @return The element by element power of "this" and b.
-   */
-  @SuppressWarnings("ParameterName")
-  public final Matrix<R, C> elementPower(int b) {
-    return new Matrix<>(this.m_storage.elementPower((double) b));
-  }
-
-  /**
-   * Extracts a given row into a row vector with new underlying storage.
-   *
-   * @param row The row to extract a vector from.
-   * @return A row vector from the given row.
-   */
-  public final Matrix<N1, C> extractRowVector(int row) {
-    return new Matrix<>(this.m_storage.extractVector(true, row));
-  }
-
-  /**
-   * Extracts a given column into a column vector with new underlying storage.
-   *
-   * @param column The column to extract a vector from.
-   * @return A column vector from the given column.
-   */
-  public final Matrix<R, N1> extractColumnVector(int column) {
-    return new Matrix<>(this.m_storage.extractVector(false, column));
-  }
-
-  /**
-   * Extracts a matrix of a given size and start position with new underlying
-   * storage.
-   *
-   * @param height The number of rows of the extracted matrix.
-   * @param width  The number of columns of the extracted matrix.
-   * @param startingRow The starting row of the extracted matrix.
-   * @param startingCol The starting column of the extracted matrix.
-   * @return The extracted matrix.
-   */
-  public final <R2 extends Num, C2 extends Num> Matrix<R2, C2> block(
-      Nat<R2> height, Nat<C2> width, int startingRow, int startingCol) {
-    return new Matrix<>(this.m_storage.extractMatrix(
-      startingRow,
-      Objects.requireNonNull(height).getNum() + startingRow,
-      startingCol,
-      Objects.requireNonNull(width).getNum() + startingCol));
-  }
-
-  /**
-   * Assign a matrix of a given size and start position.
-   *
-   * @param startingRow The row to start at.
-   * @param startingCol  The column to start at.
-   * @param other  The matrix to assign the block to.
-   */
-  public <R2 extends Num, C2 extends Num> void assignBlock(int startingRow, int startingCol,
-                                                           Matrix<R2, C2> other) {
-    this.m_storage.insertIntoThis(
-        startingRow,
-        startingCol,
-        Objects.requireNonNull(other).m_storage);
-  }
-
-  /**
-   * Extracts a submatrix from the supplied matrix and inserts it in a submatrix in "this". The
-   * shape of "this" is used to determine the size of the matrix extracted.
-   *
-   * @param startingRow The starting row in the supplied matrix to extract the submatrix.
-   * @param startingCol The starting column in the supplied matrix to extract the submatrix.
-   * @param other       The matrix to extract the submatrix from.
-   */
-  public <R2 extends Num, C2 extends Num> void extractFrom(int startingRow, int startingCol,
-                                                           Matrix<R2, C2> other) {
-    CommonOps_DDRM.extract(other.m_storage.getDDRM(), startingRow, startingCol,
-        this.m_storage.getDDRM());
-  }
-
-  /**
-   * Decompose "this" matrix using Cholesky Decomposition. If the "this" matrix is zeros, it
-   * will return the zero matrix.
-   *
-   * @param lowerTriangular Whether or not we want to decompose to the lower triangular
-   *                        Cholesky matrix.
-   * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed(ie. is not positive
-   *                          semidefinite).
-   */
-  @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
-  public Matrix<R, C> lltDecompose(boolean lowerTriangular) {
-    SimpleMatrix temp = m_storage.copy();
-
-    CholeskyDecomposition_F64<DMatrixRMaj> chol =
-            DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
-    if (!chol.decompose(temp.getMatrix())) {
-      // check that the input is not all zeros -- if they are, we special case and return all
-      // zeros.
-      var matData = temp.getDDRM().data;
-      var isZeros = true;
-      for (double matDatum : matData) {
-        isZeros &= Math.abs(matDatum) < 1e-6;
-      }
-      if (isZeros) {
-        return new Matrix<>(new SimpleMatrix(temp.numRows(), temp.numCols()));
-      }
-
-      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n"
-          + m_storage.toString());
-    }
-
-    return new Matrix<>(SimpleMatrix.wrap(chol.getT(null)));
-  }
-
-  /**
-   * Returns the row major data of this matrix as a double array.
-   *
-   * @return The row major data of this matrix as a double array.
-   */
-  public double[] getData() {
-    return m_storage.getDDRM().getData();
-  }
-
-  /**
-   * Creates the identity matrix of the given dimension.
-   *
-   * @param dim The dimension of the desired matrix as a {@link Nat}.
-   * @param <D> The dimension of the desired matrix as a generic.
-   * @return The DxD identity matrix.
-   */
-  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
-    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
-  }
-
-  /**
-   * Creates the identity matrix of the given dimension.
-   *
-   * @param dim The dimension of the desired matrix as a {@link Num}.
-   * @param <D> The dimension of the desired matrix as a generic.
-   * @return The DxD identity matrix.
-   */
-  public static <D extends Num> Matrix<D, D> eye(D dim) {
-    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
-  }
-
-  /**
-   * Entrypoint to the {@link MatBuilder} class for creation
-   * of custom matrices with the given dimensions and contents.
-   *
-   * @param rows The number of rows of the desired matrix.
-   * @param cols The number of columns of the desired matrix.
-   * @param <R> The number of rows of the desired matrix as a generic.
-   * @param <C> The number of columns of the desired matrix as a generic.
-   * @return A builder to construct the matrix.
-   */
-  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
-    return new MatBuilder<>(Objects.requireNonNull(rows), Objects.requireNonNull(cols));
-  }
-
-  /**
-   * Reassigns dimensions of a {@link Matrix} to allow for operations with
-   * other matrices that have wildcard dimensions.
-   *
-   * @param mat The {@link Matrix} to remove the dimensions from.
-   * @return The matrix with reassigned dimensions.
-   */
-  public static <R1 extends Num, C1 extends Num> Matrix<R1, C1> changeBoundsUnchecked(
-      Matrix<?, ?> mat) {
-    return new Matrix<>(mat.m_storage);
-  }
-
-  /**
-   * Checks if another {@link Matrix} is identical to "this" one within a specified tolerance.
-   *
-   * <p>This will check if each element is in tolerance of the corresponding element
-   * from the other {@link Matrix} or if the elements have the same symbolic meaning. For two
-   * elements to have the same symbolic meaning they both must be either Double.NaN,
-   * Double.POSITIVE_INFINITY, or Double.NEGATIVE_INFINITY.
-   *
-   * <p>NOTE:It is recommend to use {@link Matrix#isEqual(Matrix, double)} over this
-   * method when checking if two matrices are equal as {@link Matrix#isEqual(Matrix, double)}
-   * will return false if an element is uncountable. This method should only be used when
-   * uncountable elements need to compared.
-   *
-   * @param other     The {@link Matrix} to check against this one.
-   * @param tolerance The tolerance to check equality with.
-   * @return true if this matrix is identical to the one supplied.
-   */
-  public boolean isIdentical(Matrix<?, ?> other, double tolerance) {
-    return MatrixFeatures_DDRM.isIdentical(this.m_storage.getDDRM(),
-        other.m_storage.getDDRM(), tolerance);
-  }
-
-  /**
-   * Checks if another {@link Matrix} is equal to "this" within a specified tolerance.
-   *
-   * <p>This will check if each element is in tolerance of the corresponding element
-   * from the other {@link Matrix}.
-   *
-   * <p>tol &ge; |a<sub>ij</sub> - b<sub>ij</sub>|
-   *
-   * @param other     The {@link Matrix} to check against this one.
-   * @param tolerance The tolerance to check equality with.
-   * @return true if this matrix is equal to the one supplied.
-   */
-  public boolean isEqual(Matrix<?, ?> other, double tolerance) {
-    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(),
-        other.m_storage.getDDRM(), tolerance);
-  }
-
-  @Override
-  public String toString() {
-    return m_storage.toString();
-  }
-
-  /**
-   * Checks if an object is equal to this {@link Matrix}.
-   *
-   * <p>a<sub>ij</sub> == b<sub>ij</sub>
-   *
-   * @param other The Object to check against this {@link Matrix}.
-   * @return true if the object supplied is a {@link Matrix} and is equal to this matrix.
-   */
-  @Override
-  public boolean equals(Object other) {
-    if (this == other) {
-      return true;
-    }
-    if (!(other instanceof Matrix)) {
-      return false;
-    }
-
-    Matrix<?, ?> matrix = (Matrix<?, ?>) other;
-    if (MatrixFeatures_DDRM.hasUncountable(matrix.m_storage.getDDRM())) {
-      return false;
-    }
-    return MatrixFeatures_DDRM.isEquals(this.m_storage.getDDRM(), matrix.m_storage.getDDRM());
-  }
-
-  @Override
-  public int hashCode() {
-    return Objects.hash(m_storage);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
deleted file mode 100644
index b3e4724..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/MatrixUtils.java
+++ /dev/null
@@ -1,84 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.Objects;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-@Deprecated
-public final class MatrixUtils {
-  private MatrixUtils() {
-    throw new AssertionError("utility class");
-  }
-
-  /**
-   * Creates a new matrix of zeros.
-   *
-   * @param rows The number of rows in the matrix.
-   * @param cols The number of columns in the matrix.
-   * @param <R> The number of rows in the matrix as a generic.
-   * @param <C> The number of columns in the matrix as a generic.
-   * @return An RxC matrix filled with zeros.
-   */
-  @SuppressWarnings("LineLength")
-  public static <R extends Num, C extends Num> Matrix<R, C> zeros(Nat<R> rows, Nat<C> cols) {
-    return new Matrix<>(
-        new SimpleMatrix(Objects.requireNonNull(rows).getNum(), Objects.requireNonNull(cols).getNum()));
-  }
-
-  /**
-   * Creates a new vector of zeros.
-   *
-   * @param nums The size of the desired vector.
-   * @param <N> The size of the desired vector as a generic.
-   * @return A vector of size N filled with zeros.
-   */
-  public static <N extends Num> Matrix<N, N1> zeros(Nat<N> nums) {
-    return new Matrix<>(new SimpleMatrix(Objects.requireNonNull(nums).getNum(), 1));
-  }
-
-  /**
-   * Creates the identity matrix of the given dimension.
-   *
-   * @param dim The dimension of the desired matrix.
-   * @param <D> The dimension of the desired matrix as a generic.
-   * @return The DxD identity matrix.
-   */
-  public static <D extends Num> Matrix<D, D> eye(Nat<D> dim) {
-    return new Matrix<>(SimpleMatrix.identity(Objects.requireNonNull(dim).getNum()));
-  }
-
-  /**
-   * Entrypoint to the MatBuilder class for creation
-   * of custom matrices with the given dimensions and contents.
-   *
-   * @param rows The number of rows of the desired matrix.
-   * @param cols The number of columns of the desired matrix.
-   * @param <R> The number of rows of the desired matrix as a generic.
-   * @param <C> The number of columns of the desired matrix as a generic.
-   * @return A builder to construct the matrix.
-   */
-  public static <R extends Num, C extends Num> MatBuilder<R, C> mat(Nat<R> rows, Nat<C> cols) {
-    return new MatBuilder<>(rows, cols);
-  }
-
-  /**
-   * Entrypoint to the VecBuilder class for creation
-   * of custom vectors with the given size and contents.
-   *
-   * @param dim The dimension of the vector.
-   * @param <D> The dimension of the vector as a generic.
-   * @return A builder to construct the vector.
-   */
-  public static <D extends Num> VecBuilder<D> vec(Nat<D> dim) {
-    return new VecBuilder<>(dim);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java
deleted file mode 100644
index 0b8a81f..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Num.java
+++ /dev/null
@@ -1,20 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-/**
- * A number expressed as a java class.
- */
-public abstract class Num {
-  /**
-   * The number this is backing.
-   *
-   * @return The number represented by this class.
-   */
-  public abstract int getNum();
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java
deleted file mode 100644
index eafbcba..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Pair.java
+++ /dev/null
@@ -1,31 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-public class Pair<A, B> {
-  private final A m_first;
-  private final B m_second;
-
-  public Pair(A first, B second) {
-    m_first = first;
-    m_second = second;
-  }
-
-  public A getFirst() {
-    return m_first;
-  }
-
-  public B getSecond() {
-    return m_second;
-  }
-
-  @SuppressWarnings("ParameterName")
-  public static <A, B> Pair<A, B> of(A a, B b) {
-    return new Pair<>(a, b);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
deleted file mode 100644
index 3f281d2..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/SimpleMatrixUtils.java
+++ /dev/null
@@ -1,228 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import java.util.function.BiFunction;
-
-import org.ejml.data.DMatrixRMaj;
-import org.ejml.dense.row.NormOps_DDRM;
-import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
-import org.ejml.interfaces.decomposition.CholeskyDecomposition_F64;
-import org.ejml.simple.SimpleBase;
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.math.WPIMathJNI;
-
-public final class SimpleMatrixUtils {
-  private SimpleMatrixUtils() {
-  }
-
-  /**
-   * Compute the matrix exponential, e^M of the given matrix.
-   *
-   * @param matrix The matrix to compute the exponential of.
-   * @return The resultant matrix.
-   */
-  @SuppressWarnings({"LocalVariableName", "LineLength"})
-  public static SimpleMatrix expm(SimpleMatrix matrix) {
-    BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider = SimpleBase::solve;
-    SimpleMatrix A = matrix;
-    double A_L1 = NormOps_DDRM.inducedP1(matrix.getDDRM());
-    int n_squarings = 0;
-
-    if (A_L1 < 1.495585217958292e-002) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade3(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else if (A_L1 < 2.539398330063230e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade5(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else if (A_L1 < 9.504178996162932e-001) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade7(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else if (A_L1 < 2.097847961257068e+000) {
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade9(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    } else {
-      double maxNorm = 5.371920351148152;
-      double log = Math.log(A_L1 / maxNorm) / Math.log(2); // logb(2, arg)
-      n_squarings = (int) Math.max(0, Math.ceil(log));
-      A = A.divide(Math.pow(2.0, n_squarings));
-      Pair<SimpleMatrix, SimpleMatrix> pair = _pade13(A);
-      return dispatchPade(pair.getFirst(), pair.getSecond(), n_squarings, solveProvider);
-    }
-  }
-
-  @SuppressWarnings({"LocalVariableName", "ParameterName", "LineLength"})
-  private static SimpleMatrix dispatchPade(SimpleMatrix U, SimpleMatrix V,
-                                           int nSquarings, BiFunction<SimpleMatrix, SimpleMatrix, SimpleMatrix> solveProvider) {
-    SimpleMatrix P = U.plus(V);
-    SimpleMatrix Q = U.negative().plus(V);
-
-    SimpleMatrix R = solveProvider.apply(Q, P);
-
-    for (int i = 0; i < nSquarings; i++) {
-      R = R.mult(R);
-    }
-
-    return R;
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade3(SimpleMatrix A) {
-    double[] b = new double[]{120, 60, 12, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix U = A.mult(A2.mult(ident.scale(b[1]).plus(b[3])));
-    SimpleMatrix V = A2.scale(b[2]).plus(ident.scale(b[0]));
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade5(SimpleMatrix A) {
-    double[] b = new double[]{30240, 15120, 3360, 420, 30, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-
-    SimpleMatrix U = A.mult(A4.scale(b[5]).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V = A4.scale(b[4]).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade7(SimpleMatrix A) {
-    double[] b = new double[]{17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-
-    SimpleMatrix U =
-            A.mult(A6.scale(b[7]).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V =
-            A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "ParameterName", "LineLength"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade9(SimpleMatrix A) {
-    double[] b = new double[]{17643225600.0, 8821612800.0, 2075673600, 302702400, 30270240,
-        2162160, 110880, 3960, 90, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-    SimpleMatrix A8 = A6.mult(A2);
-
-    SimpleMatrix U =
-            A.mult(A8.scale(b[9]).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V =
-            A8.scale(b[8]).plus(A6.scale(b[6])).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0]));
-
-    return new Pair<>(U, V);
-  }
-
-  @SuppressWarnings({"MethodName", "LocalVariableName", "LineLength", "ParameterName"})
-  private static Pair<SimpleMatrix, SimpleMatrix> _pade13(SimpleMatrix A) {
-    double[] b = new double[]{64764752532480000.0, 32382376266240000.0, 7771770303897600.0,
-        1187353796428800.0, 129060195264000.0, 10559470521600.0, 670442572800.0,
-        33522128640.0, 1323241920, 40840800, 960960, 16380, 182, 1};
-    SimpleMatrix ident = eye(A.numRows(), A.numCols());
-
-    SimpleMatrix A2 = A.mult(A);
-    SimpleMatrix A4 = A2.mult(A2);
-    SimpleMatrix A6 = A4.mult(A2);
-
-    SimpleMatrix U =
-            A.mult(A6.scale(b[13]).plus(A4.scale(b[11])).plus(A2.scale(b[9])).plus(A6.scale(b[7])).plus(A4.scale(b[5])).plus(A2.scale(b[3])).plus(ident.scale(b[1])));
-    SimpleMatrix V =
-            A6.mult(A6.scale(b[12]).plus(A4.scale(b[10])).plus(A2.scale(b[8]))).plus(A6.scale(b[6]).plus(A4.scale(b[4])).plus(A2.scale(b[2])).plus(ident.scale(b[0])));
-
-    return new Pair<>(U, V);
-  }
-
-  private static SimpleMatrix eye(int rows, int cols) {
-    return SimpleMatrix.identity(Math.min(rows, cols));
-  }
-
-  /**
-   * The identy of a square matrix.
-   *
-   * @param rows the number of rows (and columns)
-   * @return the identiy matrix, rows x rows.
-   */
-  public static SimpleMatrix eye(int rows) {
-    return SimpleMatrix.identity(rows);
-  }
-
-  /**
-   * Decompose the given matrix using Cholesky Decomposition and return a view of the upper
-   * triangular matrix (if you want lower triangular see the other overload of this method.) If the
-   * input matrix is zeros, this will return the zero matrix.
-   *
-   * @param src The matrix to decompose.
-   * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
-   *                          semidefinite).
-   */
-  public static SimpleMatrix lltDecompose(SimpleMatrix src) {
-    return lltDecompose(src, false);
-  }
-
-  /**
-   * Decompose the given matrix using Cholesky Decomposition. If the input matrix is zeros, this
-   * will return the zero matrix.
-   *
-   * @param src The matrix to decompose.
-   * @param lowerTriangular if we want to decompose to the lower triangular Cholesky matrix.
-   * @return The decomposed matrix.
-   * @throws RuntimeException if the matrix could not be decomposed (ie. is not positive
-   *                          semidefinite).
-   */
-  @SuppressWarnings("PMD.AvoidThrowingRawExceptionTypes")
-  public static SimpleMatrix lltDecompose(SimpleMatrix src, boolean lowerTriangular) {
-    SimpleMatrix temp = src.copy();
-
-    CholeskyDecomposition_F64<DMatrixRMaj> chol =
-            DecompositionFactory_DDRM.chol(temp.numRows(), lowerTriangular);
-    if (!chol.decompose(temp.getMatrix())) {
-      // check that the input is not all zeros -- if they are, we special case and return all
-      // zeros.
-      var matData = temp.getDDRM().data;
-      var isZeros = true;
-      for (double matDatum : matData) {
-        isZeros &= Math.abs(matDatum) < 1e-6;
-      }
-      if (isZeros) {
-        return new SimpleMatrix(temp.numRows(), temp.numCols());
-      }
-
-      throw new RuntimeException("Cholesky decomposition failed! Input matrix:\n" + src.toString());
-    }
-
-    return SimpleMatrix.wrap(chol.getT(null));
-  }
-
-  /**
-   * Computes the matrix exponential using Eigen's solver.
-   *
-   * @param A the matrix to exponentiate.
-   * @return the exponential of A.
-   */
-  @SuppressWarnings("ParameterName")
-  public static SimpleMatrix exp(
-          SimpleMatrix A) {
-    SimpleMatrix toReturn = new SimpleMatrix(A.numRows(), A.numRows());
-    WPIMathJNI.exp(A.getDDRM().getData(), A.numRows(), toReturn.getDDRM().getData());
-    return toReturn;
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
deleted file mode 100644
index 98200b7..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/VecBuilder.java
+++ /dev/null
@@ -1,177 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N10;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N3;
-import edu.wpi.first.wpiutil.math.numbers.N4;
-import edu.wpi.first.wpiutil.math.numbers.N5;
-import edu.wpi.first.wpiutil.math.numbers.N6;
-import edu.wpi.first.wpiutil.math.numbers.N7;
-import edu.wpi.first.wpiutil.math.numbers.N8;
-import edu.wpi.first.wpiutil.math.numbers.N9;
-
-
-
-/**
- * A specialization of {@link MatBuilder} for constructing vectors (Nx1 matrices).
- *
- * @param <N> The dimension of the vector to be constructed.
- */
-public class VecBuilder<N extends Num> extends MatBuilder<N, N1> {
-  public VecBuilder(Nat<N> rows) {
-    super(rows, Nat.N1());
-  }
-
-  private Vector<N> fillVec(double... data) {
-    return new Vector<>(fill(data));
-  }
-
-  /**
-   * Returns a 1x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   */
-  public static Vector<N1> fill(double n1) {
-    return new VecBuilder<>(Nat.N1()).fillVec(n1);
-  }
-
-  /**
-   * Returns a 2x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   */
-  public static Vector<N2> fill(double n1, double n2) {
-    return new VecBuilder<>(Nat.N2()).fillVec(n1, n2);
-  }
-
-  /**
-   * Returns a 3x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   */
-  public static Vector<N3> fill(double n1, double n2, double n3) {
-    return new VecBuilder<>(Nat.N3()).fillVec(n1, n2, n3);
-  }
-
-  /**
-   * Returns a 4x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   */
-  public static Vector<N4> fill(double n1, double n2, double n3, double n4) {
-    return new VecBuilder<>(Nat.N4()).fillVec(n1, n2, n3, n4);
-  }
-
-  /**
-   * Returns a 5x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   * @param n5 the fifth element.
-   */
-  public static Vector<N5> fill(double n1, double n2, double n3, double n4, double n5) {
-    return new VecBuilder<>(Nat.N5()).fillVec(n1, n2, n3, n4, n5);
-  }
-
-  /**
-   * Returns a 6x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   * @param n5 the fifth element.
-   * @param n6 the sixth element.
-   */
-  public static Vector<N6> fill(double n1, double n2, double n3, double n4, double n5,
-                                    double n6) {
-    return new VecBuilder<>(Nat.N6()).fillVec(n1, n2, n3, n4, n5, n6);
-  }
-
-  /**
-   * Returns a 7x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   * @param n5 the fifth element.
-   * @param n6 the sixth element.
-   * @param n7 the seventh element.
-   */
-  public static Vector<N7> fill(double n1, double n2, double n3, double n4, double n5,
-                                    double n6, double n7) {
-    return new VecBuilder<>(Nat.N7()).fillVec(n1, n2, n3, n4, n5, n6, n7);
-  }
-
-  /**
-   * Returns a 8x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   * @param n5 the fifth element.
-   * @param n6 the sixth element.
-   * @param n7 the seventh element.
-   * @param n8 the eighth element.
-   */
-  public static Vector<N8> fill(double n1, double n2, double n3, double n4, double n5,
-                                    double n6, double n7, double n8) {
-    return new VecBuilder<>(Nat.N8()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8);
-  }
-
-  /**
-   * Returns a 9x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   * @param n5 the fifth element.
-   * @param n6 the sixth element.
-   * @param n7 the seventh element.
-   * @param n8 the eighth element.
-   * @param n9 the ninth element.
-   */
-  public static Vector<N9> fill(double n1, double n2, double n3, double n4, double n5,
-                                    double n6, double n7, double n8, double n9) {
-    return new VecBuilder<>(Nat.N9()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9);
-  }
-
-  /**
-   * Returns a 10x1 vector containing the given elements.
-   *
-   * @param n1 the first element.
-   * @param n2 the second element.
-   * @param n3 the third element.
-   * @param n4 the fourth element.
-   * @param n5 the fifth element.
-   * @param n6 the sixth element.
-   * @param n7 the seventh element.
-   * @param n8 the eighth element.
-   * @param n9 the ninth element.
-   * @param n10 the tenth element.
-   */
-  @SuppressWarnings("PMD.ExcessiveParameterList")
-  public static Vector<N10> fill(double n1, double n2, double n3, double n4, double n5,
-                                    double n6, double n7, double n8, double n9, double n10) {
-    return new VecBuilder<>(Nat.N10()).fillVec(n1, n2, n3, n4, n5, n6, n7, n8, n9, n10);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java b/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java
deleted file mode 100644
index 04f46ba..0000000
--- a/third_party/allwpilib/wpimath/src/main/java/edu/wpi/first/wpiutil/math/Vector.java
+++ /dev/null
@@ -1,70 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import org.ejml.simple.SimpleMatrix;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-/**
- * A shape-safe wrapper over Efficient Java Matrix Library (EJML) matrices.
- *
- * <p>This class is intended to be used alongside the state space library.
- *
- * @param <R> The number of rows in this matrix.
- */
-public class Vector<R extends Num> extends Matrix<R, N1> {
-
-  /**
-   * Constructs an empty zero vector of the given dimensions.
-   *
-   * @param rows    The number of rows of the vector.
-   */
-  public Vector(Nat<R> rows) {
-    super(rows, Nat.N1());
-  }
-
-  /**
-   * Constructs a new {@link Vector} with the given storage.
-   * Caller should make sure that the provided generic bounds match
-   * the shape of the provided {@link Vector}.
-   *
-   * <p>NOTE:It is not recommended to use this constructor unless the
-   * {@link SimpleMatrix} API is absolutely necessary due to the desired
-   * function not being accessible through the {@link Vector} wrapper.
-   *
-   * @param storage The {@link SimpleMatrix} to back this vector.
-   */
-  public Vector(SimpleMatrix storage) {
-    super(storage);
-  }
-
-  /**
-   * Constructs a new vector with the storage of the supplied matrix.
-   *
-   * @param other The {@link Vector} to copy the storage of.
-   */
-  public Vector(Matrix<R, N1> other) {
-    super(other);
-  }
-
-  @Override
-  public Vector<R> times(double value) {
-    return new Vector<>(this.m_storage.scale(value));
-  }
-
-  @Override
-  public Vector<R> div(int value) {
-    return new Vector<>(this.m_storage.divide(value));
-  }
-
-  @Override
-  public Vector<R> div(double value) {
-    return new Vector<>(this.m_storage.divide(value));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/MathShared.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/MathShared.cpp
index 8a64f2e..5252e87 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/MathShared.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/MathShared.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpimath/MathShared.h"
 
@@ -14,7 +11,9 @@
 namespace {
 class DefaultMathShared : public MathShared {
  public:
-  void ReportError(const wpi::Twine& error) override {}
+  void ReportErrorV(fmt::string_view format, fmt::format_args args) override {}
+  void ReportWarningV(fmt::string_view format, fmt::format_args args) override {
+  }
   void ReportUsage(MathUsageId id, int count) override {}
 };
 }  // namespace
@@ -24,7 +23,9 @@
 
 MathShared& MathSharedStore::GetMathShared() {
   std::scoped_lock lock(setLock);
-  if (!mathShared) mathShared = std::make_unique<DefaultMathShared>();
+  if (!mathShared) {
+    mathShared = std::make_unique<DefaultMathShared>();
+  }
   return *mathShared;
 }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/MathUtil.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/MathUtil.cpp
new file mode 100644
index 0000000..19cd214
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/MathUtil.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/MathUtil.h"
+
+#include <cmath>
+
+namespace frc {
+
+double ApplyDeadband(double value, double deadband) {
+  if (std::abs(value) > deadband) {
+    if (value > 0.0) {
+      return (value - deadband) / (1.0 - deadband);
+    } else {
+      return (value + deadband) / (1.0 - deadband);
+    }
+  } else {
+    return 0.0;
+  }
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
index d828f30..8f1145d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/StateSpaceUtil.cpp
@@ -1,14 +1,23 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/StateSpaceUtil.h"
 
 namespace frc {
 
+Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose) {
+  return Eigen::Vector<double, 3>{pose.Translation().X().value(),
+                                  pose.Translation().Y().value(),
+                                  pose.Rotation().Radians().value()};
+}
+
+Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose) {
+  return Eigen::Vector<double, 4>{pose.Translation().X().value(),
+                                  pose.Translation().Y().value(),
+                                  pose.Rotation().Cos(), pose.Rotation().Sin()};
+}
+
 template <>
 bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
                           const Eigen::Matrix<double, 1, 1>& B) {
@@ -21,9 +30,9 @@
   return detail::IsStabilizableImpl<2, 1>(A, B);
 }
 
-Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose) {
-  return frc::MakeMatrix<3, 1>(pose.X().to<double>(), pose.Y().to<double>(),
-                               pose.Rotation().Radians().to<double>());
+Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose) {
+  return Eigen::Vector<double, 3>{pose.X().value(), pose.Y().value(),
+                                  pose.Rotation().Radians().value()};
 }
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
new file mode 100644
index 0000000..23b22cd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/HolonomicDriveController.cpp
@@ -0,0 +1,78 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/HolonomicDriveController.h"
+
+#include <utility>
+
+#include "units/angular_velocity.h"
+
+using namespace frc;
+
+HolonomicDriveController::HolonomicDriveController(
+    frc2::PIDController xController, frc2::PIDController yController,
+    ProfiledPIDController<units::radian> thetaController)
+    : m_xController(std::move(xController)),
+      m_yController(std::move(yController)),
+      m_thetaController(std::move(thetaController)) {}
+
+bool HolonomicDriveController::AtReference() const {
+  const auto& eTranslate = m_poseError.Translation();
+  const auto& eRotate = m_rotationError;
+  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) {
+  // If this is the first run, then we need to reset the theta controller to the
+  // current pose's heading.
+  if (m_firstRun) {
+    m_thetaController.Reset(currentPose.Rotation().Radians());
+    m_firstRun = false;
+  }
+
+  // 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);
+  m_rotationError = angleRef - currentPose.Rotation();
+
+  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().value(), poseRef.X().value()));
+  auto yFeedback = units::meters_per_second_t(
+      m_yController.Calculate(currentPose.Y().value(), poseRef.Y().value()));
+
+  // 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/third_party/allwpilib/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
index ae58440..4d2fbe9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/LinearQuadraticRegulator.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/controller/LinearQuadraticRegulator.h"
 
@@ -11,7 +8,7 @@
 
 LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
     const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
-    const std::array<double, 1>& Qelems, const std::array<double, 1>& Relems,
+    const wpi::array<double, 1>& Qelems, const wpi::array<double, 1>& Relems,
     units::second_t dt)
     : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
@@ -22,9 +19,15 @@
     units::second_t dt)
     : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, dt) {}
 
+LinearQuadraticRegulator<1, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B,
+    const Eigen::Matrix<double, 1, 1>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 1, 1>& N, units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<1, 1>(A, B, Q, R, N, dt) {}
+
 LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
     const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
-    const std::array<double, 2>& Qelems, const std::array<double, 1>& Relems,
+    const wpi::array<double, 2>& Qelems, const wpi::array<double, 1>& Relems,
     units::second_t dt)
     : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                MakeCostMatrix(Relems), dt) {}
@@ -35,4 +38,29 @@
     units::second_t dt)
     : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, dt) {}
 
+LinearQuadraticRegulator<2, 1>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 1, 1>& R,
+    const Eigen::Matrix<double, 2, 1>& N, units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<2, 1>(A, B, Q, R, N, dt) {}
+
+LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const wpi::array<double, 2>& Qelems, const wpi::array<double, 2>& Relems,
+    units::second_t dt)
+    : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
+                               MakeCostMatrix(Relems), dt) {}
+
+LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+    units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, dt) {}
+
+LinearQuadraticRegulator<2, 2>::LinearQuadraticRegulator(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 2>& B,
+    const Eigen::Matrix<double, 2, 2>& Q, const Eigen::Matrix<double, 2, 2>& R,
+    const Eigen::Matrix<double, 2, 2>& N, units::second_t dt)
+    : detail::LinearQuadraticRegulatorImpl<2, 2>(A, B, Q, R, N, dt) {}
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp
new file mode 100644
index 0000000..34af2aa
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/PIDController.cpp
@@ -0,0 +1,175 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/PIDController.h"
+
+#include <algorithm>
+#include <cmath>
+
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/MathUtil.h"
+#include "wpimath/MathShared.h"
+
+using namespace frc2;
+
+PIDController::PIDController(double Kp, double Ki, double Kd,
+                             units::second_t period)
+    : m_Kp(Kp), m_Ki(Ki), m_Kd(Kd), m_period(period) {
+  if (period <= 0_s) {
+    wpi::math::MathSharedStore::ReportError(
+        "Controller period must be a non-zero positive number, got {}!",
+        period.value());
+    m_period = 20_ms;
+    wpi::math::MathSharedStore::ReportWarning(
+        "{}", "Controller period defaulted to 20ms.");
+  }
+  static int instances = 0;
+  instances++;
+
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kController_PIDController2, instances);
+  wpi::SendableRegistry::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;
+}
+
+void PIDController::SetD(double Kd) {
+  m_Kd = Kd;
+}
+
+double PIDController::GetP() const {
+  return m_Kp;
+}
+
+double PIDController::GetI() const {
+  return m_Ki;
+}
+
+double PIDController::GetD() const {
+  return m_Kd;
+}
+
+units::second_t PIDController::GetPeriod() const {
+  return units::second_t(m_period);
+}
+
+void PIDController::SetSetpoint(double setpoint) {
+  m_setpoint = setpoint;
+}
+
+double PIDController::GetSetpoint() const {
+  return m_setpoint;
+}
+
+bool PIDController::AtSetpoint() const {
+  double positionError;
+  if (m_continuous) {
+    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+    positionError =
+        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+  } else {
+    positionError = m_setpoint - m_measurement;
+  }
+
+  double velocityError = (positionError - m_prevError) / m_period.value();
+
+  return std::abs(positionError) < m_positionTolerance &&
+         std::abs(velocityError) < m_velocityTolerance;
+}
+
+void PIDController::EnableContinuousInput(double minimumInput,
+                                          double maximumInput) {
+  m_continuous = true;
+  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;
+  m_maximumIntegral = maximumIntegral;
+}
+
+void PIDController::SetTolerance(double positionTolerance,
+                                 double velocityTolerance) {
+  m_positionTolerance = positionTolerance;
+  m_velocityTolerance = velocityTolerance;
+}
+
+double PIDController::GetPositionError() const {
+  return m_positionError;
+}
+
+double PIDController::GetVelocityError() const {
+  return m_velocityError;
+}
+
+double PIDController::Calculate(double measurement) {
+  m_measurement = measurement;
+  m_prevError = m_positionError;
+
+  if (m_continuous) {
+    double errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+    m_positionError =
+        frc::InputModulus(m_setpoint - m_measurement, -errorBound, errorBound);
+  } else {
+    m_positionError = m_setpoint - measurement;
+  }
+
+  m_velocityError = (m_positionError - m_prevError) / m_period.value();
+
+  if (m_Ki != 0) {
+    m_totalError =
+        std::clamp(m_totalError + m_positionError * m_period.value(),
+                   m_minimumIntegral / m_Ki, m_maximumIntegral / m_Ki);
+  }
+
+  return m_Kp * m_positionError + m_Ki * m_totalError + m_Kd * m_velocityError;
+}
+
+double PIDController::Calculate(double measurement, double setpoint) {
+  // Set setpoint to provided value
+  SetSetpoint(setpoint);
+  return Calculate(measurement);
+}
+
+void PIDController::Reset() {
+  m_prevError = 0;
+  m_totalError = 0;
+}
+
+void PIDController::InitSendable(wpi::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); });
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
new file mode 100644
index 0000000..fa58427
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/ProfiledPIDController.cpp
@@ -0,0 +1,12 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/ProfiledPIDController.h"
+
+void frc::detail::ReportProfiledPIDController() {
+  static int instances = 0;
+  ++instances;
+  wpi::math::MathSharedStore::ReportUsage(
+      wpi::math::MathUsageId::kController_ProfiledPIDController, instances);
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/controller/RamseteController.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/RamseteController.cpp
new file mode 100644
index 0000000..8aa16d8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/controller/RamseteController.cpp
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/RamseteController.h"
+
+#include <cmath>
+
+#include "units/math.h"
+
+using namespace frc;
+
+/**
+ * Returns sin(x) / x.
+ *
+ * @param x Value of which to take sinc(x).
+ */
+static double Sinc(double x) {
+  if (std::abs(x) < 1e-9) {
+    return 1.0 - 1.0 / 6.0 * x * x;
+  } else {
+    return std::sin(x) / x;
+  }
+}
+
+RamseteController::RamseteController(double b, double zeta)
+    : m_b{b}, m_zeta{zeta} {}
+
+bool RamseteController::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 RamseteController::SetTolerance(const Pose2d& poseTolerance) {
+  m_poseTolerance = poseTolerance;
+}
+
+ChassisSpeeds RamseteController::Calculate(
+    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.X().value();
+  double eY = m_poseError.Y().value();
+  double eTheta = m_poseError.Rotation().Radians().value();
+  double vRef = linearVelocityRef.value();
+  double omegaRef = angularVelocityRef.value();
+
+  double k =
+      2.0 * m_zeta * std::sqrt(std::pow(omegaRef, 2) + m_b * std::pow(vRef, 2));
+
+  units::meters_per_second_t v{vRef * m_poseError.Rotation().Cos() + k * eX};
+  units::radians_per_second_t omega{omegaRef + k * eTheta +
+                                    m_b * vRef * Sinc(eTheta) * eY};
+  return ChassisSpeeds{v, 0_mps, omega};
+}
+
+ChassisSpeeds RamseteController::Calculate(
+    const Pose2d& currentPose, const Trajectory::State& desiredState) {
+  return Calculate(currentPose, desiredState.pose, desiredState.velocity,
+                   desiredState.velocity * desiredState.curvature);
+}
+
+void RamseteController::SetEnabled(bool enabled) {
+  m_enabled = enabled;
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
index e80cadc..20ea2b7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/drake/math/discrete_algebraic_riccati_equation.cpp
@@ -1,14 +1,11 @@
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_throw.h"
-#include "drake/common/is_approx_equal_abstol.h"
-
 #include <Eigen/Eigenvalues>
 #include <Eigen/QR>
 
-// This code has https://github.com/RobotLocomotion/drake/pull/11118 applied to
-// fix an infinite loop in reorder_eigen().
+#include "drake/common/drake_assert.h"
+#include "drake/common/drake_throw.h"
+#include "drake/common/is_approx_equal_abstol.h"
 
 namespace drake {
 namespace math {
@@ -385,12 +382,11 @@
  * DiscreteAlgebraicRiccatiEquation function
  * computes the unique stabilizing solution X to the discrete-time algebraic
  * Riccati equation:
- * \f[
- * A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
- * \f]
  *
- * @throws std::runtime_error if Q is not positive semi-definite.
- * @throws std::runtime_error if R is not positive definite.
+ * AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+ *
+ * @throws std::exception if Q is not positive semi-definite.
+ * @throws std::exception if R is not positive definite.
  *
  * Based on the Schur Vector approach outlined in this paper:
  * "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
@@ -399,9 +395,9 @@
  *
  * Note: When, for example, n = 100, m = 80, and entries of A, B, Q_half,
  * R_half are sampled from standard normal distributions, where
- * Q = Q_half'*Q_half and similar for R, the absolute error of the solution
- * is 10^{-6}, while the absolute error of the solution computed by Matlab is
- * 10^{-8}.
+ * Q = Q_halfᵀ Q_half and similar for R, the absolute error of the solution
+ * is 10⁻⁶, while the absolute error of the solution computed by Matlab is
+ * 10⁻⁸.
  *
  * TODO(weiqiao.han): I may overwrite the RealQZ function to improve the
  * accuracy, together with more thorough tests.
@@ -459,5 +455,21 @@
   return X;
 }
 
+Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::MatrixXd>& B,
+    const Eigen::Ref<const Eigen::MatrixXd>& Q,
+    const Eigen::Ref<const Eigen::MatrixXd>& R,
+    const Eigen::Ref<const Eigen::MatrixXd>& N) {
+    DRAKE_DEMAND(N.rows() == B.rows() && N.cols() == B.cols());
+
+    // This is a change of variables to make the DARE that includes Q, R, and N
+    // cost matrices fit the form of the DARE that includes only Q and R cost
+    // matrices.
+    Eigen::MatrixXd A2 = A - B * R.llt().solve(N.transpose());
+    Eigen::MatrixXd Q2 = Q - N * R.llt().solve(N.transpose());
+    return DiscreteAlgebraicRiccatiEquation(A2, B, Q2, R);
+}
+
 }  // namespace math
 }  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
new file mode 100644
index 0000000..c5ed7a1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/DifferentialDrivePoseEstimator.cpp
@@ -0,0 +1,145 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/estimator/DifferentialDrivePoseEstimator.h"
+
+#include <wpi/timestamp.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+
+using namespace frc;
+
+DifferentialDrivePoseEstimator::DifferentialDrivePoseEstimator(
+    const Rotation2d& gyroAngle, const Pose2d& initialPose,
+    const wpi::array<double, 5>& stateStdDevs,
+    const wpi::array<double, 3>& localMeasurementStdDevs,
+    const wpi::array<double, 3>& visionMeasurmentStdDevs,
+    units::second_t nominalDt)
+    : m_observer(
+          &DifferentialDrivePoseEstimator::F,
+          [](const Eigen::Vector<double, 5>& x,
+             const Eigen::Vector<double, 3>& u) {
+            return Eigen::Vector<double, 3>{x(3, 0), x(4, 0), x(2, 0)};
+          },
+          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<5, 5>(2),
+          frc::AngleMean<3, 5>(2), frc::AngleResidual<5>(2),
+          frc::AngleResidual<3>(2), frc::AngleAdd<5>(2), nominalDt),
+      m_nominalDt(nominalDt) {
+  SetVisionMeasurementStdDevs(visionMeasurmentStdDevs);
+
+  // Create correction mechanism for vision measurements.
+  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+                        const Eigen::Vector<double, 3>& y) {
+    m_observer.Correct<3>(
+        u, y,
+        [](const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>&) {
+          return x.block<3, 1>(0, 0);
+        },
+        m_visionContR, frc::AngleMean<3, 5>(2), frc::AngleResidual<3>(2),
+        frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
+  };
+
+  m_gyroOffset = initialPose.Rotation() - gyroAngle;
+  m_previousAngle = initialPose.Rotation();
+  m_observer.SetXhat(FillStateVector(initialPose, 0_m, 0_m));
+}
+
+void DifferentialDrivePoseEstimator::SetVisionMeasurementStdDevs(
+    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
+  // Create R (covariances) for vision measurements.
+  m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+}
+
+void DifferentialDrivePoseEstimator::ResetPosition(
+    const Pose2d& pose, const Rotation2d& gyroAngle) {
+  // Reset state estimate and error covariance
+  m_observer.Reset();
+  m_latencyCompensator.Reset();
+
+  m_observer.SetXhat(FillStateVector(pose, 0_m, 0_m));
+
+  m_gyroOffset = GetEstimatedPosition().Rotation() - gyroAngle;
+  m_previousAngle = pose.Rotation();
+}
+
+Pose2d DifferentialDrivePoseEstimator::GetEstimatedPosition() const {
+  return Pose2d(units::meter_t(m_observer.Xhat(0)),
+                units::meter_t(m_observer.Xhat(1)),
+                Rotation2d(units::radian_t(m_observer.Xhat(2))));
+}
+
+void DifferentialDrivePoseEstimator::AddVisionMeasurement(
+    const Pose2d& visionRobotPose, units::second_t timestamp) {
+  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+      m_visionCorrect, timestamp);
+}
+
+Pose2d DifferentialDrivePoseEstimator::Update(
+    const Rotation2d& gyroAngle,
+    const DifferentialDriveWheelSpeeds& wheelSpeeds,
+    units::meter_t leftDistance, units::meter_t rightDistance) {
+  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+                        wheelSpeeds, leftDistance, rightDistance);
+}
+
+Pose2d DifferentialDrivePoseEstimator::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    const DifferentialDriveWheelSpeeds& wheelSpeeds,
+    units::meter_t leftDistance, units::meter_t rightDistance) {
+  auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+  m_prevTime = currentTime;
+
+  auto angle = gyroAngle + m_gyroOffset;
+  auto omega = (gyroAngle - m_previousAngle).Radians() / dt;
+
+  auto u = Eigen::Vector<double, 3>{
+      (wheelSpeeds.left + wheelSpeeds.right).value() / 2.0, 0.0, omega.value()};
+
+  m_previousAngle = angle;
+
+  auto localY = Eigen::Vector<double, 3>{
+      leftDistance.value(), rightDistance.value(), angle.Radians().value()};
+
+  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+  m_observer.Predict(u, dt);
+  m_observer.Correct(u, localY);
+
+  return GetEstimatedPosition();
+}
+
+Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::F(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 3>& u) {
+  // Apply a rotation matrix. Note that we do not add x because Runge-Kutta does
+  // that for us.
+  auto& theta = x(2);
+  Eigen::Matrix<double, 5, 5> toFieldRotation{
+      {std::cos(theta), -std::sin(theta), 0.0, 0.0, 0.0},
+      {std::sin(theta), std::cos(theta), 0.0, 0.0, 0.0},
+      {0.0, 0.0, 1.0, 0.0, 0.0},
+      {0.0, 0.0, 0.0, 1.0, 0.0},
+      {0.0, 0.0, 0.0, 0.0, 1.0}};
+  return toFieldRotation *
+         Eigen::Vector<double, 5>{u(0, 0), u(1, 0), u(2, 0), u(0, 0), u(1, 0)};
+}
+
+template <int Dim>
+wpi::array<double, Dim> DifferentialDrivePoseEstimator::StdDevMatrixToArray(
+    const Eigen::Vector<double, Dim>& stdDevs) {
+  wpi::array<double, Dim> array;
+  for (size_t i = 0; i < Dim; ++i) {
+    array[i] = stdDevs(i);
+  }
+  return array;
+}
+
+Eigen::Vector<double, 5> DifferentialDrivePoseEstimator::FillStateVector(
+    const Pose2d& pose, units::meter_t leftDistance,
+    units::meter_t rightDistance) {
+  return Eigen::Vector<double, 5>{pose.Translation().X().value(),
+                                  pose.Translation().Y().value(),
+                                  pose.Rotation().Radians().value(),
+                                  leftDistance.value(), rightDistance.value()};
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
index a1747ab..1209eae 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/KalmanFilter.cpp
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/estimator/KalmanFilter.h"
 
 namespace frc {
 
 KalmanFilter<1, 1, 1>::KalmanFilter(
-    LinearSystem<1, 1, 1>& plant, const std::array<double, 1>& stateStdDevs,
-    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    LinearSystem<1, 1, 1>& plant, const wpi::array<double, 1>& stateStdDevs,
+    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
     : detail::KalmanFilterImpl<1, 1, 1>{plant, stateStdDevs, measurementStdDevs,
                                         dt} {}
 
 KalmanFilter<2, 1, 1>::KalmanFilter(
-    LinearSystem<2, 1, 1>& plant, const std::array<double, 2>& stateStdDevs,
-    const std::array<double, 1>& measurementStdDevs, units::second_t dt)
+    LinearSystem<2, 1, 1>& plant, const wpi::array<double, 2>& stateStdDevs,
+    const wpi::array<double, 1>& measurementStdDevs, units::second_t dt)
     : detail::KalmanFilterImpl<2, 1, 1>{plant, stateStdDevs, measurementStdDevs,
                                         dt} {}
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
new file mode 100644
index 0000000..9d93647
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/estimator/MecanumDrivePoseEstimator.cpp
@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/estimator/MecanumDrivePoseEstimator.h"
+
+#include <wpi/timestamp.h>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+
+using namespace frc;
+
+frc::MecanumDrivePoseEstimator::MecanumDrivePoseEstimator(
+    const Rotation2d& gyroAngle, const Pose2d& initialPose,
+    MecanumDriveKinematics kinematics,
+    const wpi::array<double, 3>& stateStdDevs,
+    const wpi::array<double, 1>& localMeasurementStdDevs,
+    const wpi::array<double, 3>& visionMeasurementStdDevs,
+    units::second_t nominalDt)
+    : m_observer(
+          [](const Eigen::Vector<double, 3>& x,
+             const Eigen::Vector<double, 3>& u) { return u; },
+          [](const Eigen::Vector<double, 3>& x,
+             const Eigen::Vector<double, 3>& u) { return x.block<1, 1>(2, 0); },
+          stateStdDevs, localMeasurementStdDevs, frc::AngleMean<3, 3>(2),
+          frc::AngleMean<1, 3>(0), frc::AngleResidual<3>(2),
+          frc::AngleResidual<1>(0), frc::AngleAdd<3>(2), nominalDt),
+      m_kinematics(kinematics),
+      m_nominalDt(nominalDt) {
+  SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+  // Create vision correction mechanism.
+  m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+                        const Eigen::Vector<double, 3>& y) {
+    m_observer.Correct<3>(
+        u, y,
+        [](const Eigen::Vector<double, 3>& x, const Eigen::Vector<double, 3>&) {
+          return x;
+        },
+        m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
+        frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
+  };
+
+  // Set initial state.
+  m_observer.SetXhat(PoseTo3dVector(initialPose));
+
+  // Calculate offsets.
+  m_gyroOffset = initialPose.Rotation() - gyroAngle;
+  m_previousAngle = initialPose.Rotation();
+}
+
+void frc::MecanumDrivePoseEstimator::SetVisionMeasurementStdDevs(
+    const wpi::array<double, 3>& visionMeasurmentStdDevs) {
+  // Create R (covariances) for vision measurements.
+  m_visionContR = frc::MakeCovMatrix(visionMeasurmentStdDevs);
+}
+
+void frc::MecanumDrivePoseEstimator::ResetPosition(
+    const Pose2d& pose, const Rotation2d& gyroAngle) {
+  // Reset state estimate and error covariance
+  m_observer.Reset();
+  m_latencyCompensator.Reset();
+
+  m_observer.SetXhat(PoseTo3dVector(pose));
+
+  m_gyroOffset = pose.Rotation() - gyroAngle;
+  m_previousAngle = pose.Rotation();
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::GetEstimatedPosition() const {
+  return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
+                Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+}
+
+void frc::MecanumDrivePoseEstimator::AddVisionMeasurement(
+    const Pose2d& visionRobotPose, units::second_t timestamp) {
+  m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+      &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+      m_visionCorrect, timestamp);
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::Update(
+    const Rotation2d& gyroAngle, const MecanumDriveWheelSpeeds& wheelSpeeds) {
+  return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+                        wheelSpeeds);
+}
+
+Pose2d frc::MecanumDrivePoseEstimator::UpdateWithTime(
+    units::second_t currentTime, const Rotation2d& gyroAngle,
+    const MecanumDriveWheelSpeeds& wheelSpeeds) {
+  auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+  m_prevTime = currentTime;
+
+  auto angle = gyroAngle + m_gyroOffset;
+  auto omega = (angle - m_previousAngle).Radians() / dt;
+
+  auto chassisSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+  auto fieldRelativeVelocities =
+      Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
+          .RotateBy(angle);
+
+  Eigen::Vector<double, 3> u{fieldRelativeVelocities.X().value(),
+                             fieldRelativeVelocities.Y().value(),
+                             omega.value()};
+
+  Eigen::Vector<double, 1> localY{angle.Radians().value()};
+  m_previousAngle = angle;
+
+  m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+
+  m_observer.Predict(u, dt);
+  m_observer.Correct(u, localY);
+
+  return GetEstimatedPosition();
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
index 57dad44..b7176cd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Pose2d.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/geometry/Pose2d.h"
 
@@ -23,12 +20,6 @@
   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());
@@ -55,7 +46,7 @@
 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 dtheta = twist.dtheta.value();
 
   const auto sinTheta = std::sin(dtheta);
   const auto cosTheta = std::cos(dtheta);
@@ -77,7 +68,7 @@
 
 Twist2d Pose2d::Log(const Pose2d& end) const {
   const auto transform = end.RelativeTo(*this);
-  const auto dtheta = transform.Rotation().Radians().to<double>();
+  const auto dtheta = transform.Rotation().Radians().value();
   const auto halfDtheta = dtheta / 2.0;
 
   const auto cosMinusOne = transform.Rotation().Cos() - 1;
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
index 32a9b40..27af5ed 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Rotation2d.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/geometry/Rotation2d.h"
 
@@ -41,32 +38,20 @@
   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-() 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;
+  return std::hypot(m_cos - other.m_cos, m_sin - other.m_sin) < 1E-9;
 }
 
 bool Rotation2d::operator!=(const Rotation2d& other) const {
@@ -79,7 +64,7 @@
 }
 
 void frc::to_json(wpi::json& json, const Rotation2d& rotation) {
-  json = wpi::json{{"radians", rotation.Radians().to<double>()}};
+  json = wpi::json{{"radians", rotation.Radians().value()}};
 }
 
 void frc::from_json(const wpi::json& json, Rotation2d& rotation) {
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
index eb5e2ec..0808f35 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Transform2d.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/geometry/Transform2d.h"
 
@@ -31,6 +28,10 @@
   return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
 }
 
+Transform2d Transform2d::operator+(const Transform2d& other) const {
+  return Transform2d{Pose2d{}, Pose2d{}.TransformBy(*this).TransformBy(other)};
+}
+
 bool Transform2d::operator==(const Transform2d& other) const {
   return m_translation == other.m_translation && m_rotation == other.m_rotation;
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
index 6f4551c..5a30ec2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/geometry/Translation2d.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/geometry/Translation2d.h"
 
@@ -36,33 +33,18 @@
   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-() 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);
 }
@@ -76,14 +58,9 @@
   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>()}};
+  json =
+      wpi::json{{"x", translation.X().value()}, {"y", translation.Y().value()}};
 }
 
 void frc::from_json(const wpi::json& json, Translation2d& translation) {
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
index 26d57e2..b036833 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
+#include <exception>
+
 #include <wpi/jni_util.h>
 
 #include "Eigen/Core"
@@ -14,28 +13,38 @@
 #include "Eigen/QR"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/trajectory/TrajectoryUtil.h"
 #include "unsupported/Eigen/MatrixFunctions"
 
 using namespace wpi::java;
 
+/**
+ * Returns true if (A, B) is a stabilizable pair.
+ *
+ * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+ * any, have absolute values less than one, where an eigenvalue is
+ * uncontrollable if rank(λI - A, B) < n where n is the number of states.
+ *
+ * @param A System matrix.
+ * @param B Input matrix.
+ */
 bool check_stabilizable(const Eigen::Ref<const Eigen::MatrixXd>& A,
                         const Eigen::Ref<const Eigen::MatrixXd>& B) {
-  // This function checks if (A,B) is a stabilizable pair.
-  // (A,B) is stabilizable if and only if the uncontrollable eigenvalues of
-  // A, if any, have absolute values less than one, where an eigenvalue is
-  // uncontrollable if Rank[lambda * I - A, B] < n.
-  int n = B.rows(), m = B.cols();
-  Eigen::EigenSolver<Eigen::MatrixXd> es(A);
-  for (int i = 0; i < n; i++) {
+  int states = B.rows();
+  int inputs = B.cols();
+  Eigen::EigenSolver<Eigen::MatrixXd> es{A};
+  for (int i = 0; i < states; ++i) {
     if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
             es.eigenvalues()[i].imag() * es.eigenvalues()[i].imag() <
-        1)
+        1) {
       continue;
+    }
 
-    Eigen::MatrixXcd E(n, n + m);
-    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(n, n) - A, B;
-    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr(E);
-    if (qr.rank() != n) {
+    Eigen::MatrixXcd E{states, states + inputs};
+    E << es.eigenvalues()[i] * Eigen::MatrixXcd::Identity(states, states) - A,
+        B;
+    Eigen::ColPivHouseholderQR<Eigen::MatrixXcd> qr{E};
+    if (qr.rank() < states) {
       return false;
     }
   }
@@ -43,6 +52,46 @@
   return true;
 }
 
+std::vector<double> GetElementsFromTrajectory(
+    const frc::Trajectory& trajectory) {
+  std::vector<double> elements;
+  elements.reserve(trajectory.States().size() * 7);
+
+  for (auto&& state : trajectory.States()) {
+    elements.push_back(state.t.value());
+    elements.push_back(state.velocity.value());
+    elements.push_back(state.acceleration.value());
+    elements.push_back(state.pose.X().value());
+    elements.push_back(state.pose.Y().value());
+    elements.push_back(state.pose.Rotation().Radians().value());
+    elements.push_back(state.curvature.value());
+  }
+
+  return elements;
+}
+
+frc::Trajectory CreateTrajectoryFromElements(wpi::span<const double> elements) {
+  // Make sure that the elements have the correct length.
+  assert(elements.size() % 7 == 0);
+
+  // Create a vector of states from the elements.
+  std::vector<frc::Trajectory::State> states;
+  states.reserve(elements.size() / 7);
+
+  for (size_t i = 0; i < elements.size(); i += 7) {
+    states.emplace_back(frc::Trajectory::State{
+        units::second_t{elements[i]},
+        units::meters_per_second_t{elements[i + 1]},
+        units::meters_per_second_squared_t{elements[i + 2]},
+        frc::Pose2d{units::meter_t{elements[i + 3]},
+                    units::meter_t{elements[i + 4]},
+                    units::radian_t{elements[i + 5]}},
+        units::curvature_t{elements[i + 6]}});
+  }
+
+  return frc::Trajectory(states);
+}
+
 extern "C" {
 
 /*
@@ -73,15 +122,22 @@
       Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
       Rmat{nativeR, inputs, inputs};
 
-  Eigen::MatrixXd result =
-      drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
+  try {
+    Eigen::MatrixXd result =
+        drake::math::DiscreteAlgebraicRiccatiEquation(Amat, Bmat, Qmat, Rmat);
 
-  env->ReleaseDoubleArrayElements(A, nativeA, 0);
-  env->ReleaseDoubleArrayElements(B, nativeB, 0);
-  env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
-  env->ReleaseDoubleArrayElements(R, nativeR, 0);
+    env->ReleaseDoubleArrayElements(A, nativeA, 0);
+    env->ReleaseDoubleArrayElements(B, nativeB, 0);
+    env->ReleaseDoubleArrayElements(Q, nativeQ, 0);
+    env->ReleaseDoubleArrayElements(R, nativeR, 0);
 
-  env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+    env->SetDoubleArrayRegion(S, 0, states * states, result.data());
+  } catch (const std::runtime_error& e) {
+    jclass cls = env->FindClass("java/lang/RuntimeException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+  }
 }
 
 /*
@@ -107,6 +163,28 @@
 
 /*
  * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    pow
+ * Signature: ([DID[D)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_pow
+  (JNIEnv* env, jclass, jdoubleArray src, jint rows, jdouble exponent,
+   jdoubleArray dst)
+{
+  jdouble* arrayBody = env->GetDoubleArrayElements(src, nullptr);
+
+  Eigen::Map<
+      Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>>
+      Amat{arrayBody, rows, rows};  // NOLINT
+  Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> Apow =
+      Amat.pow(exponent);
+
+  env->ReleaseDoubleArrayElements(src, arrayBody, 0);
+  env->SetDoubleArrayRegion(dst, 0, rows * rows, Apow.data());
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
  * Method:    isStabilizable
  * Signature: (II[D[D)Z
  */
@@ -134,4 +212,99 @@
   return isStabilizable;
 }
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    fromPathweaverJson
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_fromPathweaverJson
+  (JNIEnv* env, jclass, jstring path)
+{
+  try {
+    auto trajectory =
+        frc::TrajectoryUtil::FromPathweaverJson(JStringRef{env, path}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass("java/io/IOException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    toPathweaverJson
+ * Signature: ([DLjava/lang/String;)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_toPathweaverJson
+  (JNIEnv* env, jclass, jdoubleArray elements, jstring path)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
+    frc::TrajectoryUtil::ToPathweaverJson(trajectory,
+                                          JStringRef{env, path}.c_str());
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass("java/io/IOException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    deserializeTrajectory
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeTrajectory
+  (JNIEnv* env, jclass, jstring json)
+{
+  try {
+    auto trajectory = frc::TrajectoryUtil::DeserializeTrajectory(
+        JStringRef{env, json}.c_str());
+    std::vector<double> elements = GetElementsFromTrajectory(trajectory);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass(
+        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
+        "TrajectorySerializationException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    serializeTrajectory
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
+  (JNIEnv* env, jclass, jdoubleArray elements)
+{
+  try {
+    auto trajectory =
+        CreateTrajectoryFromElements(JDoubleArrayRef{env, elements});
+    return MakeJString(env,
+                       frc::TrajectoryUtil::SerializeTrajectory(trajectory));
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass(
+        "edu/wpi/first/math/trajectory/TrajectoryUtil$"
+        "TrajectorySerializationException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
index 25b6c51..c4a4311 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveOdometry.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/DifferentialDriveOdometry.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
index 36a4952..42d3018 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/DifferentialDriveWheelSpeeds.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
index de1b2d0..c4a71fd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveKinematics.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/MecanumDriveKinematics.h"
 
@@ -24,33 +21,31 @@
     m_previousCoR = centerOfRotation;
   }
 
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+  Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
+                                      chassisSpeeds.vy.value(),
+                                      chassisSpeeds.omega.value()};
 
-  Eigen::Matrix<double, 4, 1> wheelsMatrix =
+  Eigen::Vector<double, 4> wheelsVector =
       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)};
+  wheelSpeeds.frontLeft = units::meters_per_second_t{wheelsVector(0)};
+  wheelSpeeds.frontRight = units::meters_per_second_t{wheelsVector(1)};
+  wheelSpeeds.rearLeft = units::meters_per_second_t{wheelsVector(2)};
+  wheelSpeeds.rearRight = units::meters_per_second_t{wheelsVector(3)};
   return wheelSpeeds;
 }
 
 ChassisSpeeds MecanumDriveKinematics::ToChassisSpeeds(
     const MecanumDriveWheelSpeeds& wheelSpeeds) const {
-  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::Vector<double, 4> wheelSpeedsVector{
+      wheelSpeeds.frontLeft.value(), wheelSpeeds.frontRight.value(),
+      wheelSpeeds.rearLeft.value(), wheelSpeeds.rearRight.value()};
 
   Eigen::Vector3d chassisSpeedsVector =
-      m_forwardKinematics.solve(wheelSpeedsMatrix);
+      m_forwardKinematics.solve(wheelSpeedsVector);
 
-  return {units::meters_per_second_t{chassisSpeedsVector(0)},
+  return {units::meters_per_second_t{chassisSpeedsVector(0)},  // NOLINT
           units::meters_per_second_t{chassisSpeedsVector(1)},
           units::radians_per_second_t{chassisSpeedsVector(2)}};
 }
@@ -59,11 +54,9 @@
                                                   Translation2d fr,
                                                   Translation2d rl,
                                                   Translation2d rr) const {
-  // 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);
+  m_inverseKinematics =
+      Eigen::Matrix<double, 4, 3>{{1, -1, (-(fl.X() + fl.Y())).value()},
+                                  {1, 1, (fr.X() - fr.Y()).value()},
+                                  {1, 1, (rl.X() - rl.Y()).value()},
+                                  {1, -1, (-(rr.X() + rr.Y())).value()}};
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
index 7534fc1..bbeee58 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveOdometry.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/MecanumDriveOdometry.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
index b20dddf..fc47461 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/kinematics/MecanumDriveWheelSpeeds.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/MecanumDriveWheelSpeeds.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
index c00a362..b643849 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/CubicHermiteSpline.cpp
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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) {
+    wpi::array<double, 2> xInitialControlVector,
+    wpi::array<double, 2> xFinalControlVector,
+    wpi::array<double, 2> yInitialControlVector,
+    wpi::array<double, 2> yFinalControlVector) {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
index 5b34cdb..5362b7c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/QuinticHermiteSpline.cpp
@@ -1,19 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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) {
+    wpi::array<double, 3> xInitialControlVector,
+    wpi::array<double, 3> xFinalControlVector,
+    wpi::array<double, 3> yInitialControlVector,
+    wpi::array<double, 3> yFinalControlVector) {
   const auto hermite = MakeHermiteBasis();
   const auto x =
       ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
index 58f7537..cec620c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineHelper.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/spline/SplineHelper.h"
 
@@ -16,10 +13,10 @@
     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;
+  wpi::array<double, 2> xInitial = start.x;
+  wpi::array<double, 2> yInitial = start.y;
+  wpi::array<double, 2> xFinal = end.x;
+  wpi::array<double, 2> yFinal = end.y;
 
   if (waypoints.size() > 1) {
     waypoints.emplace(waypoints.begin(),
@@ -55,29 +52,27 @@
     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]);
+    dx.emplace_back(3 * (waypoints[2].X().value() - waypoints[0].X().value()) -
+                    xInitial[1]);
+    dy.emplace_back(3 * (waypoints[2].Y().value() - waypoints[0].Y().value()) -
+                    yInitial[1]);
     if (waypoints.size() > 4) {
       for (size_t i = 1; i <= waypoints.size() - 4; ++i) {
         // dx and dy represent the derivatives of the internal waypoints. The
         // derivative of the second internal waypoint should involve the third
         // and first internal waypoint, which have indices of 1 and 3 in the
         // waypoints list (which contains ALL waypoints).
-        dx.emplace_back(3 * (waypoints[i + 2].X().to<double>() -
-                             waypoints[i].X().to<double>()));
-        dy.emplace_back(3 * (waypoints[i + 2].Y().to<double>() -
-                             waypoints[i].Y().to<double>()));
+        dx.emplace_back(
+            3 * (waypoints[i + 2].X().value() - waypoints[i].X().value()));
+        dy.emplace_back(
+            3 * (waypoints[i + 2].Y().value() - waypoints[i].Y().value()));
       }
     }
-    dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
-                         waypoints[waypoints.size() - 3].X().to<double>()) -
+    dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().value() -
+                         waypoints[waypoints.size() - 3].X().value()) -
                     xFinal[1]);
-    dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
-                         waypoints[waypoints.size() - 3].Y().to<double>()) -
+    dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().value() -
+                         waypoints[waypoints.size() - 3].Y().value()) -
                     yFinal[1]);
 
     // Compute solution to tridiagonal system
@@ -92,10 +87,10 @@
     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]}};
+          {waypoints[i].X().value(), fx[i]},
+          {waypoints[i + 1].X().value(), fx[i + 1]},
+          {waypoints[i].Y().value(), fy[i]},
+          {waypoints[i + 1].Y().value(), fy[i + 1]}};
 
       splines.push_back(spline);
     }
@@ -105,10 +100,8 @@
     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};
+    wpi::array<double, 2> midXControlVector{waypoints[0].X().value(), xDeriv};
+    wpi::array<double, 2> midYControlVector{waypoints[0].Y().value(), yDeriv};
 
     splines.emplace_back(xInitial, midXControlVector, yInitial,
                          midYControlVector);
@@ -137,22 +130,20 @@
   return splines;
 }
 
-std::array<Spline<3>::ControlVector, 2>
+wpi::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>();
+    scalar = 1.2 * start.Translation().Distance(end.Translation()).value();
   } else {
     scalar =
-        1.2 *
-        start.Translation().Distance(interiorWaypoints.front()).to<double>();
+        1.2 * start.Translation().Distance(interiorWaypoints.front()).value();
   }
   const auto initialCV = CubicControlVector(scalar, start);
   if (!interiorWaypoints.empty()) {
-    scalar =
-        1.2 * end.Translation().Distance(interiorWaypoints.back()).to<double>();
+    scalar = 1.2 * end.Translation().Distance(interiorWaypoints.back()).value();
   }
   const auto finalCV = CubicControlVector(scalar, end);
   return {initialCV, finalCV};
@@ -168,7 +159,7 @@
 
     // This just makes the splines look better.
     const auto scalar =
-        1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
+        1.2 * p0.Translation().Distance(p1.Translation()).value();
 
     auto controlVectorA = QuinticControlVector(scalar, p0);
     auto controlVectorB = QuinticControlVector(scalar, p1);
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
index b7e7f9e..73c475b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/spline/SplineParameterizer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/spline/SplineParameterizer.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
index 067b8de..db419f7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/Trajectory.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/Trajectory.h"
 
@@ -34,7 +31,9 @@
   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);
+  if (deltaT < 0_s) {
+    return endValue.Interpolate(*this, 1.0 - i);
+  }
 
   // Check whether the robot is reversing at this stage.
   const auto reversing =
@@ -68,8 +67,12 @@
 }
 
 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();
+  if (t <= m_states.front().t) {
+    return m_states.front();
+  }
+  if (t >= m_totalTime) {
+    return m_states.back();
+  }
 
   // Use binary search to get the element with a timestamp no less than the
   // requested timestamp. This starts at 1 because we use the previous state
@@ -121,12 +124,33 @@
   return Trajectory(newStates);
 }
 
+Trajectory Trajectory::operator+(const Trajectory& other) const {
+  // If this is a default constructed trajectory with no states, then we can
+  // simply return the rhs trajectory.
+  if (m_states.empty()) {
+    return other;
+  }
+
+  auto states = m_states;
+  auto otherStates = other.States();
+  for (auto& otherState : otherStates) {
+    otherState.t += m_totalTime;
+  }
+
+  // Here we omit the first state of the other trajectory because we don't want
+  // two time points with different states. Sample() will automatically
+  // interpolate between the end of this trajectory and the second state of the
+  // other trajectory.
+  states.insert(states.end(), otherStates.begin() + 1, otherStates.end());
+  return Trajectory(states);
+}
+
 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>()},
+  json = wpi::json{{"time", state.t.value()},
+                   {"velocity", state.velocity.value()},
+                   {"acceleration", state.acceleration.value()},
                    {"pose", state.pose},
-                   {"curvature", state.curvature.to<double>()}};
+                   {"curvature", state.curvature.value()}};
 }
 
 void frc::from_json(const wpi::json& json, Trajectory::State& state) {
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
index 6cb3f7a..2e2771d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp
@@ -1,15 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/TrajectoryGenerator.h"
 
 #include <utility>
 
-#include <wpi/raw_ostream.h>
+#include <fmt/format.h>
 
 #include "frc/spline/SplineHelper.h"
 #include "frc/spline/SplineParameterizer.h"
@@ -22,10 +19,11 @@
 std::function<void(const char*)> TrajectoryGenerator::s_errorFunc;
 
 void TrajectoryGenerator::ReportError(const char* error) {
-  if (s_errorFunc)
+  if (s_errorFunc) {
     s_errorFunc(error);
-  else
-    wpi::errs() << "TrajectoryGenerator error: " << error << "\n";
+  } else {
+    fmt::print(stderr, "TrajectoryGenerator error: {}\n", error);
+  }
 }
 
 Trajectory TrajectoryGenerator::GenerateTrajectory(
@@ -115,8 +113,11 @@
     const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
   auto newWaypoints = waypoints;
   const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
-  if (config.IsReversed())
-    for (auto& waypoint : newWaypoints) waypoint += flip;
+  if (config.IsReversed()) {
+    for (auto& waypoint : newWaypoints) {
+      waypoint = waypoint + flip;
+    }
+  }
 
   std::vector<SplineParameterizer::PoseWithCurvature> points;
   try {
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
index 0e78a15..d397d0c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
  * MIT License
@@ -31,7 +28,7 @@
 
 #include "frc/trajectory/TrajectoryParameterizer.h"
 
-#include <string>
+#include <fmt/format.h>
 
 #include "units/math.h"
 
@@ -88,7 +85,9 @@
       // Now enforce all acceleration limits.
       EnforceAccelerationLimits(reversed, constraints, &constrainedState);
 
-      if (ds.to<double>() < kEpsilon) break;
+      if (ds.value() < 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
@@ -133,14 +132,18 @@
                             successor.minAcceleration * ds * 2.0);
 
       // No more limits to impose! This state can be finalized.
-      if (newMaxVelocity >= constrainedState.maxVelocity) break;
+      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 (ds.value() > -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
@@ -190,9 +193,9 @@
         // delta_x = v * t
         dt = ds / v;
       } else {
-        throw std::runtime_error("Something went wrong at iteration " +
-                                 std::to_string(i) +
-                                 " of time parameterization.");
+        throw std::runtime_error(fmt::format(
+            "Something went wrong at iteration {} of time parameterization.",
+            i));
       }
     }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
index 0ae43f5..169b642 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/TrajectoryUtil.h"
 
 #include <system_error>
 
+#include <fmt/format.h>
 #include <wpi/SmallString.h>
 #include <wpi/json.h>
 #include <wpi/raw_istream.h>
@@ -17,13 +15,12 @@
 using namespace frc;
 
 void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
-                                      const wpi::Twine& path) {
+                                      std::string_view path) {
   std::error_code error_code;
 
-  wpi::SmallString<128> buf;
-  wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+  wpi::raw_fd_ostream output{path, error_code};
   if (error_code) {
-    throw std::runtime_error(("Cannot open file: " + path).str());
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
   }
 
   wpi::json json = trajectory.States();
@@ -31,13 +28,12 @@
   output.flush();
 }
 
-Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+Trajectory TrajectoryUtil::FromPathweaverJson(std::string_view path) {
   std::error_code error_code;
 
-  wpi::SmallString<128> buf;
-  wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+  wpi::raw_fd_istream input{path, error_code};
   if (error_code) {
-    throw std::runtime_error(("Cannot open file: " + path).str());
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
   }
 
   wpi::json json;
@@ -51,8 +47,7 @@
   return json.dump();
 }
 
-Trajectory TrajectoryUtil::DeserializeTrajectory(
-    const wpi::StringRef json_str) {
-  wpi::json json = wpi::json::parse(json_str);
+Trajectory TrajectoryUtil::DeserializeTrajectory(std::string_view jsonStr) {
+  wpi::json json = wpi::json::parse(jsonStr);
   return Trajectory{json.get<std::vector<Trajectory::State>>()};
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
index f04b9d6..738d243 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
index c3380e5..d1c2f6f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
index f12ce75..7c10201 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp
new file mode 100644
index 0000000..9e6e712
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MaxVelocityConstraint.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/trajectory/constraint/MaxVelocityConstraint.h"
+
+using namespace frc;
+
+MaxVelocityConstraint::MaxVelocityConstraint(
+    units::meters_per_second_t maxVelocity)
+    : m_maxVelocity(units::math::abs(maxVelocity)) {}
+
+units::meters_per_second_t MaxVelocityConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  return m_maxVelocity;
+}
+
+TrajectoryConstraint::MinMax MaxVelocityConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
index 7d95803..418a904 100644
--- a/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
+++ b/third_party/allwpilib/wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h"
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Cholesky b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Cholesky
index 1332b54..ef249de 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Cholesky
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Cholesky
@@ -33,14 +33,13 @@
 #include "src/Cholesky/LDLT.h"
 #ifdef EIGEN_USE_LAPACKE
 #ifdef EIGEN_USE_MKL
-#include "mkl_lapacke.h"
+// #include "mkl_lapacke.h"
 #else
-#include "src/misc/lapacke.h"
+// #include "src/misc/lapacke.h"
 #endif
-#include "src/Cholesky/LLT_LAPACKE.h"
+// #include "src/Cholesky/LLT_LAPACKE.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_CHOLESKY_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Core b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Core
index bd892c1..fd5e098 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Core
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Core
@@ -11,255 +11,55 @@
 #ifndef EIGEN_CORE_H
 #define EIGEN_CORE_H
 
-#if __GNUC__ >= 9
-#pragma GCC diagnostic ignored "-Wdeprecated-copy"
-#endif
-
-// first thing Eigen does: stop the compiler from committing suicide
+// first thing Eigen does: stop the compiler from reporting useless warnings.
 #include "src/Core/util/DisableStupidWarnings.h"
 
-#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
-  #define EIGEN_CUDACC __CUDACC__
+// then include this file where all our macros are defined. It's really important to do it first because
+// it's where we do all the compiler/OS/arch detections and define most defaults.
+#include "src/Core/util/Macros.h"
+
+// This detects SSE/AVX/NEON/etc. and configure alignment settings
+#include "src/Core/util/ConfigureVectorization.h"
+
+// We need cuda_runtime.h/hip_runtime.h to ensure that
+// the EIGEN_USING_STD macro works properly on the device side
+#if defined(EIGEN_CUDACC)
+  #include <cuda_runtime.h>
+#elif defined(EIGEN_HIPCC)
+  #include <hip/hip_runtime.h>
 #endif
 
-#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
-  #define EIGEN_CUDA_ARCH __CUDA_ARCH__
-#endif
-
-#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
-#define EIGEN_CUDACC_VER  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
-#elif defined(__CUDACC_VER__)
-#define EIGEN_CUDACC_VER __CUDACC_VER__
-#else
-#define EIGEN_CUDACC_VER 0
-#endif
-
-// Handle NVCC/CUDA/SYCL
-#if defined(__CUDACC__) || defined(__SYCL_DEVICE_ONLY__)
-  // Do not try asserts on CUDA and SYCL!
-  #ifndef EIGEN_NO_DEBUG
-  #define EIGEN_NO_DEBUG
-  #endif
-
-  #ifdef EIGEN_INTERNAL_DEBUGGING
-  #undef EIGEN_INTERNAL_DEBUGGING
-  #endif
-
-  #ifdef EIGEN_EXCEPTIONS
-  #undef EIGEN_EXCEPTIONS
-  #endif
-
-  // All functions callable from CUDA code must be qualified with __device__
-  #ifdef __CUDACC__
-    // Do not try to vectorize on CUDA and SYCL!
-    #ifndef EIGEN_DONT_VECTORIZE
-    #define EIGEN_DONT_VECTORIZE
-    #endif
-
-    #define EIGEN_DEVICE_FUNC __host__ __device__
-    // We need cuda_runtime.h to ensure that that EIGEN_USING_STD_MATH macro
-    // works properly on the device side
-    #include <cuda_runtime.h>
-  #else
-    #define EIGEN_DEVICE_FUNC
-  #endif
-
-#else
-  #define EIGEN_DEVICE_FUNC
-
-#endif
-
-// When compiling CUDA device code with NVCC, pull in math functions from the
-// global namespace.  In host mode, and when device doee with clang, use the
-// std versions.
-#if defined(__CUDA_ARCH__) && defined(__NVCC__)
-  #define EIGEN_USING_STD_MATH(FUNC) using ::FUNC;
-#else
-  #define EIGEN_USING_STD_MATH(FUNC) using std::FUNC;
-#endif
-
-#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(__CUDA_ARCH__) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL)
-  #define EIGEN_EXCEPTIONS
-#endif
 
 #ifdef EIGEN_EXCEPTIONS
   #include <new>
 #endif
 
-// then include this file where all our macros are defined. It's really important to do it first because
-// it's where we do all the alignment settings (platform detection and honoring the user's will if he
-// defined e.g. EIGEN_DONT_ALIGN) so it needs to be done before we do anything with vectorization.
-#include "src/Core/util/Macros.h"
-
 // Disable the ipa-cp-clone optimization flag with MinGW 6.x or newer (enabled by default with -O3)
 // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=556 for details.
-#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6)
+#if EIGEN_COMP_MINGW && EIGEN_GNUC_AT_LEAST(4,6) && EIGEN_GNUC_AT_MOST(5,5)
   #pragma GCC optimize ("-fno-ipa-cp-clone")
 #endif
 
+// Prevent ICC from specializing std::complex operators that silently fail
+// on device. This allows us to use our own device-compatible specializations
+// instead.
+#if defined(EIGEN_COMP_ICC) && defined(EIGEN_GPU_COMPILE_PHASE) \
+    && !defined(_OVERRIDE_COMPLEX_SPECIALIZATION_)
+#define _OVERRIDE_COMPLEX_SPECIALIZATION_ 1
+#endif
 #include <complex>
 
 // this include file manages BLAS and MKL related macros
 // and inclusion of their respective header files
 // #include "src/Core/util/MKL_support.h"
 
-// if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into
-// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks
-#if EIGEN_MAX_ALIGN_BYTES==0
-  #ifndef EIGEN_DONT_VECTORIZE
-    #define EIGEN_DONT_VECTORIZE
-  #endif
+
+#if defined(EIGEN_HAS_CUDA_FP16) || defined(EIGEN_HAS_HIP_FP16)
+  #define EIGEN_HAS_GPU_FP16
 #endif
 
-#if EIGEN_COMP_MSVC
-  #include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
-  #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later
-    // Remember that usage of defined() in a #define is undefined by the standard.
-    // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
-    #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
-      #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
-    #endif
-  #endif
-#else
-  // Remember that usage of defined() in a #define is undefined by the standard
-  #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) )
-    #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
-  #endif
-#endif
-
-#ifndef EIGEN_DONT_VECTORIZE
-
-  #if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
-
-    // Defines symbols for compile-time detection of which instructions are
-    // used.
-    // EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_SSE
-    #define EIGEN_VECTORIZE_SSE2
-
-    // Detect sse3/ssse3/sse4:
-    // gcc and icc defines __SSE3__, ...
-    // there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
-    // want to force the use of those instructions with msvc.
-    #ifdef __SSE3__
-      #define EIGEN_VECTORIZE_SSE3
-    #endif
-    #ifdef __SSSE3__
-      #define EIGEN_VECTORIZE_SSSE3
-    #endif
-    #ifdef __SSE4_1__
-      #define EIGEN_VECTORIZE_SSE4_1
-    #endif
-    #ifdef __SSE4_2__
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #ifdef __AVX__
-      #define EIGEN_VECTORIZE_AVX
-      #define EIGEN_VECTORIZE_SSE3
-      #define EIGEN_VECTORIZE_SSSE3
-      #define EIGEN_VECTORIZE_SSE4_1
-      #define EIGEN_VECTORIZE_SSE4_2
-    #endif
-    #ifdef __AVX2__
-      #define EIGEN_VECTORIZE_AVX2
-    #endif
-    #ifdef __FMA__
-      #define EIGEN_VECTORIZE_FMA
-    #endif
-    #if defined(__AVX512F__) && defined(EIGEN_ENABLE_AVX512)
-      #define EIGEN_VECTORIZE_AVX512
-      #define EIGEN_VECTORIZE_AVX2
-      #define EIGEN_VECTORIZE_AVX
-      #define EIGEN_VECTORIZE_FMA
-      #ifdef __AVX512DQ__
-        #define EIGEN_VECTORIZE_AVX512DQ
-      #endif
-      #ifdef __AVX512ER__
-        #define EIGEN_VECTORIZE_AVX512ER
-      #endif
-    #endif
-
-    // include files
-
-    // This extern "C" works around a MINGW-w64 compilation issue
-    // https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
-    // In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
-    // However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
-    // with conflicting linkage.  The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
-    // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
-    // notice that since these are C headers, the extern "C" is theoretically needed anyways.
-    extern "C" {
-      // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
-      // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
-      #if EIGEN_COMP_ICC >= 1110
-        #include <immintrin.h>
-      #else
-        #include <mmintrin.h>
-        #include <emmintrin.h>
-        #include <xmmintrin.h>
-        #ifdef  EIGEN_VECTORIZE_SSE3
-        #include <pmmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSSE3
-        #include <tmmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSE4_1
-        #include <smmintrin.h>
-        #endif
-        #ifdef EIGEN_VECTORIZE_SSE4_2
-        #include <nmmintrin.h>
-        #endif
-        #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
-        #include <immintrin.h>
-        #endif
-      #endif
-    } // end extern "C"
-  #elif defined __VSX__
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_VSX
-    #include <altivec.h>
-    // We need to #undef all these ugly tokens defined in <altivec.h>
-    // => use __vector instead of vector
-    #undef bool
-    #undef vector
-    #undef pixel
-  #elif defined __ALTIVEC__
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_ALTIVEC
-    #include <altivec.h>
-    // We need to #undef all these ugly tokens defined in <altivec.h>
-    // => use __vector instead of vector
-    #undef bool
-    #undef vector
-    #undef pixel
-  #elif (defined  __ARM_NEON) || (defined __ARM_NEON__)
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_NEON
-    #include <arm_neon.h>
-  #elif (defined __s390x__ && defined __VEC__)
-    #define EIGEN_VECTORIZE
-    #define EIGEN_VECTORIZE_ZVECTOR
-    #include <vecintrin.h>
-  #endif
-#endif
-
-#if defined(__F16C__) && !defined(EIGEN_COMP_CLANG)
-  // We can use the optimized fp16 to float and float to fp16 conversion routines
-  #define EIGEN_HAS_FP16_C
-#endif
-
-#if defined __CUDACC__
-  #define EIGEN_VECTORIZE_CUDA
-  #include <vector_types.h>
-  #if EIGEN_CUDACC_VER >= 70500
-    #define EIGEN_HAS_CUDA_FP16
-  #endif
-#endif
-
-#if defined EIGEN_HAS_CUDA_FP16
-  #include <host_defines.h>
-  #include <cuda_fp16.h>
+#if defined(EIGEN_HAS_CUDA_BF16) || defined(EIGEN_HAS_HIP_BF16)
+  #define EIGEN_HAS_GPU_BF16
 #endif
 
 #if (defined _OPENMP) && (!defined EIGEN_DONT_PARALLELIZE)
@@ -283,7 +83,10 @@
 #include <cmath>
 #include <cassert>
 #include <functional>
-#include <iosfwd>
+#include <sstream>
+#ifndef EIGEN_NO_IO
+  #include <iosfwd>
+#endif
 #include <cstring>
 #include <string>
 #include <limits>
@@ -291,6 +94,10 @@
 // for min/max:
 #include <algorithm>
 
+#if EIGEN_HAS_CXX11
+#include <array>
+#endif
+
 // for std::is_nothrow_move_assignable
 #ifdef EIGEN_INCLUDE_TYPE_TRAITS
 #include <type_traits>
@@ -306,38 +113,25 @@
   #include <intrin.h>
 #endif
 
-/** \brief Namespace containing all symbols from the %Eigen library. */
-namespace Eigen {
-
-inline static const char *SimdInstructionSetsInUse(void) {
-#if defined(EIGEN_VECTORIZE_AVX512)
-  return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
-#elif defined(EIGEN_VECTORIZE_AVX)
-  return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
-#elif defined(EIGEN_VECTORIZE_SSE4_2)
-  return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
-#elif defined(EIGEN_VECTORIZE_SSE4_1)
-  return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
-#elif defined(EIGEN_VECTORIZE_SSSE3)
-  return "SSE, SSE2, SSE3, SSSE3";
-#elif defined(EIGEN_VECTORIZE_SSE3)
-  return "SSE, SSE2, SSE3";
-#elif defined(EIGEN_VECTORIZE_SSE2)
-  return "SSE, SSE2";
-#elif defined(EIGEN_VECTORIZE_ALTIVEC)
-  return "AltiVec";
-#elif defined(EIGEN_VECTORIZE_VSX)
-  return "VSX";
-#elif defined(EIGEN_VECTORIZE_NEON)
-  return "ARM NEON";
-#elif defined(EIGEN_VECTORIZE_ZVECTOR)
-  return "S390X ZVECTOR";
-#else
-  return "None";
+#if defined(EIGEN_USE_SYCL)
+  #undef min
+  #undef max
+  #undef isnan
+  #undef isinf
+  #undef isfinite
+  #include <CL/sycl.hpp>
+  #include <map>
+  #include <memory>
+  #include <utility>
+  #include <thread>
+  #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM0
+  #define EIGEN_SYCL_LOCAL_THREAD_DIM0 16
+  #endif
+  #ifndef EIGEN_SYCL_LOCAL_THREAD_DIM1
+  #define EIGEN_SYCL_LOCAL_THREAD_DIM1 16
+  #endif
 #endif
-}
 
-} // end namespace Eigen
 
 #if defined EIGEN2_SUPPORT_STAGE40_FULL_EIGEN3_STRICTNESS || defined EIGEN2_SUPPORT_STAGE30_FULL_EIGEN3_API || defined EIGEN2_SUPPORT_STAGE20_RESOLVE_API_CONFLICTS || defined EIGEN2_SUPPORT_STAGE10_FULL_EIGEN2_API || defined EIGEN2_SUPPORT
 // This will generate an error message:
@@ -346,7 +140,7 @@
 
 namespace Eigen {
 
-// we use size_t frequently and we'll never remember to prepend it with std:: everytime just to
+// we use size_t frequently and we'll never remember to prepend it with std:: every time just to
 // ensure QNX/QCC support
 using std::size_t;
 // gcc 4.6.0 wants std:: for ptrdiff_t
@@ -370,58 +164,90 @@
 #include "src/Core/util/StaticAssert.h"
 #include "src/Core/util/XprHelper.h"
 #include "src/Core/util/Memory.h"
+#include "src/Core/util/IntegralConstant.h"
+#include "src/Core/util/SymbolicIndex.h"
 
 #include "src/Core/NumTraits.h"
 #include "src/Core/MathFunctions.h"
 #include "src/Core/GenericPacketMath.h"
 #include "src/Core/MathFunctionsImpl.h"
 #include "src/Core/arch/Default/ConjHelper.h"
+// Generic half float support
+#include "src/Core/arch/Default/Half.h"
+#include "src/Core/arch/Default/BFloat16.h"
+#include "src/Core/arch/Default/TypeCasting.h"
+#include "src/Core/arch/Default/GenericPacketMathFunctionsFwd.h"
 
 #if defined EIGEN_VECTORIZE_AVX512
   #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/SSE/TypeCasting.h"
+  #include "src/Core/arch/SSE/Complex.h"
   #include "src/Core/arch/AVX/PacketMath.h"
-  #include "src/Core/arch/AVX512/PacketMath.h"
-  #include "src/Core/arch/AVX512/MathFunctions.h"
+  #include "src/Core/arch/AVX/TypeCasting.h"
+  #include "src/Core/arch/AVX/Complex.h"
+  // #include "src/Core/arch/AVX512/PacketMath.h"
+  // #include "src/Core/arch/AVX512/TypeCasting.h"
+  // #include "src/Core/arch/AVX512/Complex.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/AVX/MathFunctions.h"
+  // #include "src/Core/arch/AVX512/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_AVX
   // Use AVX for floats and doubles, SSE for integers
   #include "src/Core/arch/SSE/PacketMath.h"
-  #include "src/Core/arch/SSE/Complex.h"
-  #include "src/Core/arch/SSE/MathFunctions.h"
-  #include "src/Core/arch/AVX/PacketMath.h"
-  #include "src/Core/arch/AVX/MathFunctions.h"
-  #include "src/Core/arch/AVX/Complex.h"
-  #include "src/Core/arch/AVX/TypeCasting.h"
   #include "src/Core/arch/SSE/TypeCasting.h"
+  #include "src/Core/arch/SSE/Complex.h"
+  #include "src/Core/arch/AVX/PacketMath.h"
+  #include "src/Core/arch/AVX/TypeCasting.h"
+  #include "src/Core/arch/AVX/Complex.h"
+  #include "src/Core/arch/SSE/MathFunctions.h"
+  #include "src/Core/arch/AVX/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_SSE
   #include "src/Core/arch/SSE/PacketMath.h"
+  #include "src/Core/arch/SSE/TypeCasting.h"
   #include "src/Core/arch/SSE/MathFunctions.h"
   #include "src/Core/arch/SSE/Complex.h"
-  #include "src/Core/arch/SSE/TypeCasting.h"
 #elif defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
-  #include "src/Core/arch/AltiVec/PacketMath.h"
-  #include "src/Core/arch/AltiVec/MathFunctions.h"
-  #include "src/Core/arch/AltiVec/Complex.h"
+  // #include "src/Core/arch/AltiVec/PacketMath.h"
+  // #include "src/Core/arch/AltiVec/MathFunctions.h"
+  // #include "src/Core/arch/AltiVec/Complex.h"
 #elif defined EIGEN_VECTORIZE_NEON
   #include "src/Core/arch/NEON/PacketMath.h"
+  #include "src/Core/arch/NEON/TypeCasting.h"
   #include "src/Core/arch/NEON/MathFunctions.h"
   #include "src/Core/arch/NEON/Complex.h"
+#elif defined EIGEN_VECTORIZE_SVE
+  // #include "src/Core/arch/SVE/PacketMath.h"
+  // #include "src/Core/arch/SVE/TypeCasting.h"
+  // #include "src/Core/arch/SVE/MathFunctions.h"
 #elif defined EIGEN_VECTORIZE_ZVECTOR
-  #include "src/Core/arch/ZVector/PacketMath.h"
-  #include "src/Core/arch/ZVector/MathFunctions.h"
-  #include "src/Core/arch/ZVector/Complex.h"
+  // #include "src/Core/arch/ZVector/PacketMath.h"
+  // #include "src/Core/arch/ZVector/MathFunctions.h"
+  // #include "src/Core/arch/ZVector/Complex.h"
+#elif defined EIGEN_VECTORIZE_MSA
+  // #include "src/Core/arch/MSA/PacketMath.h"
+  // #include "src/Core/arch/MSA/MathFunctions.h"
+  // #include "src/Core/arch/MSA/Complex.h"
 #endif
 
-// Half float support
-// #include "src/Core/arch/CUDA/Half.h"
-// #include "src/Core/arch/CUDA/PacketMathHalf.h"
-// #include "src/Core/arch/CUDA/TypeCasting.h"
+#if defined EIGEN_VECTORIZE_GPU
+  // #include "src/Core/arch/GPU/PacketMath.h"
+  // #include "src/Core/arch/GPU/MathFunctions.h"
+  // #include "src/Core/arch/GPU/TypeCasting.h"
+#endif
 
-#if defined EIGEN_VECTORIZE_CUDA
-  #include "src/Core/arch/CUDA/PacketMath.h"
-  #include "src/Core/arch/CUDA/MathFunctions.h"
+#if defined(EIGEN_USE_SYCL)
+  // #include "src/Core/arch/SYCL/SyclMemoryModel.h"
+  // #include "src/Core/arch/SYCL/InteropHeaders.h"
+#if !defined(EIGEN_DONT_VECTORIZE_SYCL)
+  // #include "src/Core/arch/SYCL/PacketMath.h"
+  // #include "src/Core/arch/SYCL/MathFunctions.h"
+  // #include "src/Core/arch/SYCL/TypeCasting.h"
+#endif
 #endif
 
 #include "src/Core/arch/Default/Settings.h"
+// This file provides generic implementations valid for scalar as well
+#include "src/Core/arch/Default/GenericPacketMathFunctions.h"
 
 #include "src/Core/functors/TernaryFunctors.h"
 #include "src/Core/functors/BinaryFunctors.h"
@@ -432,9 +258,16 @@
 
 // Specialized functors to enable the processing of complex numbers
 // on CUDA devices
+#ifdef EIGEN_CUDACC
 // #include "src/Core/arch/CUDA/Complex.h"
+#endif
 
-#include "src/Core/IO.h"
+#include "src/Core/util/IndexedViewHelper.h"
+#include "src/Core/util/ReshapedHelper.h"
+#include "src/Core/ArithmeticSequence.h"
+#ifndef EIGEN_NO_IO
+  #include "src/Core/IO.h"
+#endif
 #include "src/Core/DenseCoeffsBase.h"
 #include "src/Core/DenseBase.h"
 #include "src/Core/MatrixBase.h"
@@ -475,6 +308,8 @@
 #include "src/Core/Ref.h"
 #include "src/Core/Block.h"
 #include "src/Core/VectorBlock.h"
+#include "src/Core/IndexedView.h"
+#include "src/Core/Reshaped.h"
 #include "src/Core/Transpose.h"
 #include "src/Core/DiagonalMatrix.h"
 #include "src/Core/Diagonal.h"
@@ -511,27 +346,35 @@
 #include "src/Core/CoreIterators.h"
 #include "src/Core/ConditionEstimator.h"
 
+#if defined(EIGEN_VECTORIZE_ALTIVEC) || defined(EIGEN_VECTORIZE_VSX)
+  // #include "src/Core/arch/AltiVec/MatrixProduct.h"
+#elif defined EIGEN_VECTORIZE_NEON
+  #include "src/Core/arch/NEON/GeneralBlockPanelKernel.h"
+#endif
+
 #include "src/Core/BooleanRedux.h"
 #include "src/Core/Select.h"
 #include "src/Core/VectorwiseOp.h"
+#include "src/Core/PartialReduxEvaluator.h"
 #include "src/Core/Random.h"
 #include "src/Core/Replicate.h"
 #include "src/Core/Reverse.h"
 #include "src/Core/ArrayWrapper.h"
+#include "src/Core/StlIterators.h"
 
 #ifdef EIGEN_USE_BLAS
-#include "src/Core/products/GeneralMatrixMatrix_BLAS.h"
-#include "src/Core/products/GeneralMatrixVector_BLAS.h"
-#include "src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h"
-#include "src/Core/products/SelfadjointMatrixMatrix_BLAS.h"
-#include "src/Core/products/SelfadjointMatrixVector_BLAS.h"
-#include "src/Core/products/TriangularMatrixMatrix_BLAS.h"
-#include "src/Core/products/TriangularMatrixVector_BLAS.h"
-#include "src/Core/products/TriangularSolverMatrix_BLAS.h"
+// #include "src/Core/products/GeneralMatrixMatrix_BLAS.h"
+// #include "src/Core/products/GeneralMatrixVector_BLAS.h"
+// #include "src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h"
+// #include "src/Core/products/SelfadjointMatrixMatrix_BLAS.h"
+// #include "src/Core/products/SelfadjointMatrixVector_BLAS.h"
+// #include "src/Core/products/TriangularMatrixMatrix_BLAS.h"
+// #include "src/Core/products/TriangularMatrixVector_BLAS.h"
+// #include "src/Core/products/TriangularSolverMatrix_BLAS.h"
 #endif // EIGEN_USE_BLAS
 
 #ifdef EIGEN_USE_MKL_VML
-#include "src/Core/Assign_MKL.h"
+// #include "src/Core/Assign_MKL.h"
 #endif
 
 #include "src/Core/GlobalFunctions.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigen b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigen
deleted file mode 100644
index 654c8dc..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigen
+++ /dev/null
@@ -1,2 +0,0 @@
-#include "Dense"
-#include "Sparse"
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues
index 1ad6bcf..c6defe3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Eigenvalues
@@ -10,12 +10,13 @@
 
 #include "Core"
 
-#include "src/Core/util/DisableStupidWarnings.h"
-
 #include "Cholesky"
 #include "Jacobi"
 #include "Householder"
 #include "LU"
+// #include "Geometry"
+
+#include "src/Core/util/DisableStupidWarnings.h"
 
 /** \defgroup Eigenvalues_Module Eigenvalues module
   *
@@ -45,16 +46,15 @@
 #include "src/Eigenvalues/MatrixBaseEigenvalues.h"
 #ifdef EIGEN_USE_LAPACKE
 #ifdef EIGEN_USE_MKL
-#include "mkl_lapacke.h"
+// #include "mkl_lapacke.h"
 #else
-#include "src/misc/lapacke.h"
+// #include "src/misc/lapacke.h"
 #endif
-#include "src/Eigenvalues/RealSchur_LAPACKE.h"
-#include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
-#include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"
+// #include "src/Eigenvalues/RealSchur_LAPACKE.h"
+// #include "src/Eigenvalues/ComplexSchur_LAPACKE.h"
+// #include "src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_EIGENVALUES_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Householder b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Householder
index 89cd81b..f2fa799 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Householder
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Householder
@@ -27,4 +27,3 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_HOUSEHOLDER_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Jacobi b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Jacobi
index 17c1d78..43edc7a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Jacobi
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/Jacobi
@@ -29,5 +29,4 @@
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_JACOBI_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/LU b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/LU
index 6418a86..a1b5d46 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/LU
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/LU
@@ -29,22 +29,19 @@
 #include "src/LU/PartialPivLU.h"
 #ifdef EIGEN_USE_LAPACKE
 #ifdef EIGEN_USE_MKL
-#include "mkl_lapacke.h"
+// #include "mkl_lapacke.h"
 #else
-#include "src/misc/lapacke.h"
+// #include "src/misc/lapacke.h"
 #endif
-#include "src/LU/PartialPivLU_LAPACKE.h"
+// #include "src/LU/PartialPivLU_LAPACKE.h"
 #endif
 #include "src/LU/Determinant.h"
 #include "src/LU/InverseImpl.h"
 
-// Use the SSE optimized version whenever possible. At the moment the
-// SSE version doesn't compile when AVX is enabled
-#if defined EIGEN_VECTORIZE_SSE && !defined EIGEN_VECTORIZE_AVX
-  #include "src/LU/arch/Inverse_SSE.h"
+#if defined EIGEN_VECTORIZE_SSE || defined EIGEN_VECTORIZE_NEON
+  #include "src/LU/arch/InverseSize4.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_LU_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/QR b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/QR
index c7e9144..42a3fa8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/QR
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/QR
@@ -10,12 +10,12 @@
 
 #include "Core"
 
-#include "src/Core/util/DisableStupidWarnings.h"
-
 #include "Cholesky"
 #include "Jacobi"
 #include "Householder"
 
+#include "src/Core/util/DisableStupidWarnings.h"
+
 /** \defgroup QR_Module QR module
   *
   *
@@ -37,15 +37,14 @@
 #include "src/QR/CompleteOrthogonalDecomposition.h"
 #ifdef EIGEN_USE_LAPACKE
 #ifdef EIGEN_USE_MKL
-#include "mkl_lapacke.h"
+// #include "mkl_lapacke.h"
 #else
-#include "src/misc/lapacke.h"
+// #include "src/misc/lapacke.h"
 #endif
-#include "src/QR/HouseholderQR_LAPACKE.h"
-#include "src/QR/ColPivHouseholderQR_LAPACKE.h"
+// #include "src/QR/HouseholderQR_LAPACKE.h"
+// #include "src/QR/ColPivHouseholderQR_LAPACKE.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_QR_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/SVD b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/SVD
index 5d0e75f..4441a38 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/SVD
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/SVD
@@ -38,14 +38,13 @@
 #include "src/SVD/BDCSVD.h"
 #if defined(EIGEN_USE_LAPACKE) && !defined(EIGEN_USE_LAPACKE_STRICT)
 #ifdef EIGEN_USE_MKL
-#include "mkl_lapacke.h"
+// #include "mkl_lapacke.h"
 #else
-#include "src/misc/lapacke.h"
+// #include "src/misc/lapacke.h"
 #endif
-#include "src/SVD/JacobiSVD_LAPACKE.h"
+// #include "src/SVD/JacobiSVD_LAPACKE.h"
 #endif
 
 #include "src/Core/util/ReenableStupidWarnings.h"
 
 #endif // EIGEN_SVD_MODULE_H
-/* vim: set filetype=cpp et sw=2 ts=2 ai: */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
index 15ccf24..1013ca0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LDLT.h
@@ -16,6 +16,15 @@
 namespace Eigen {
 
 namespace internal {
+  template<typename _MatrixType, int _UpLo> struct traits<LDLT<_MatrixType, _UpLo> >
+   : traits<_MatrixType>
+  {
+    typedef MatrixXpr XprKind;
+    typedef SolverStorage StorageKind;
+    typedef int StorageIndex;
+    enum { Flags = 0 };
+  };
+
   template<typename MatrixType, int UpLo> struct LDLT_Traits;
 
   // PositiveSemiDef means positive semi-definite and non-zero; same for NegativeSemiDef
@@ -36,7 +45,7 @@
   * matrix \f$ A \f$ such that \f$ A =  P^TLDL^*P \f$, where P is a permutation matrix, L
   * is lower triangular with a unit diagonal and D is a diagonal matrix.
   *
-  * The decomposition uses pivoting to ensure stability, so that L will have
+  * The decomposition uses pivoting to ensure stability, so that D will have
   * zeros in the bottom right rank(A) - n submatrix. Avoiding the square root
   * on D also stabilizes the computation.
   *
@@ -44,24 +53,23 @@
   * decomposition to determine whether a system of equations has a solution.
   *
   * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  * 
+  *
   * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt(), class LLT
   */
 template<typename _MatrixType, int _UpLo> class LDLT
+        : public SolverBase<LDLT<_MatrixType, _UpLo> >
 {
   public:
     typedef _MatrixType MatrixType;
+    typedef SolverBase<LDLT> Base;
+    friend class SolverBase<LDLT>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(LDLT)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
       UpLo = _UpLo
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    typedef typename MatrixType::StorageIndex StorageIndex;
     typedef Matrix<Scalar, RowsAtCompileTime, 1, 0, MaxRowsAtCompileTime, 1> TmpMatrixType;
 
     typedef Transpositions<RowsAtCompileTime, MaxRowsAtCompileTime> TranspositionType;
@@ -180,6 +188,7 @@
       return m_sign == internal::NegativeSemiDef || m_sign == internal::ZeroSign;
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** \returns a solution x of \f$ A x = b \f$ using the current decomposition of A.
       *
       * This function also supports in-place solves using the syntax <tt>x = decompositionObject.solve(x)</tt> .
@@ -191,19 +200,14 @@
       * \f$ L^* y_4 = y_3 \f$ and \f$ P x = y_4 \f$ in succession. If the matrix \f$ A \f$ is singular, then
       * \f$ D \f$ will also be singular (all the other matrices are invertible). In that case, the
       * least-square solution of \f$ D y_3 = y_2 \f$ is computed. This does not mean that this function
-      * computes the least-square solution of \f$ A x = b \f$ is \f$ A \f$ is singular.
+      * computes the least-square solution of \f$ A x = b \f$ if \f$ A \f$ is singular.
       *
       * \sa MatrixBase::ldlt(), SelfAdjointView::ldlt()
       */
     template<typename Rhs>
     inline const Solve<LDLT, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "LDLT is not initialized.");
-      eigen_assert(m_matrix.rows()==b.rows()
-                && "LDLT::solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<LDLT, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     template<typename Derived>
     bool solveInPlace(MatrixBase<Derived> &bAndX) const;
@@ -242,12 +246,12 @@
       */
     const LDLT& adjoint() const { return *this; };
 
-    inline Index rows() const { return m_matrix.rows(); }
-    inline Index cols() const { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful,
+      * \returns \c Success if computation was successful,
       *          \c NumericalIssue if the factorization failed because of a zero pivot.
       */
     ComputationInfo info() const
@@ -258,8 +262,10 @@
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
@@ -560,14 +566,22 @@
 template<typename RhsType, typename DstType>
 void LDLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
-  eigen_assert(rhs.rows() == rows());
+  _solve_impl_transposed<true>(rhs, dst);
+}
+
+template<typename _MatrixType,int _UpLo>
+template<bool Conjugate, typename RhsType, typename DstType>
+void LDLT<_MatrixType,_UpLo>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
   // dst = P b
   dst = m_transpositions * rhs;
 
   // dst = L^-1 (P b)
-  matrixL().solveInPlace(dst);
+  // dst = L^-*T (P b)
+  matrixL().template conjugateIf<!Conjugate>().solveInPlace(dst);
 
-  // dst = D^-1 (L^-1 P b)
+  // dst = D^-* (L^-1 P b)
+  // dst = D^-1 (L^-*T P b)
   // more precisely, use pseudo-inverse of D (see bug 241)
   using std::abs;
   const typename Diagonal<const MatrixType>::RealReturnType vecD(vectorD());
@@ -579,7 +593,6 @@
   // Moreover, Lapack's xSYTRS routines use 0 for the tolerance.
   // Using numeric_limits::min() gives us more robustness to denormals.
   RealScalar tolerance = (std::numeric_limits<RealScalar>::min)();
-
   for (Index i = 0; i < vecD.size(); ++i)
   {
     if(abs(vecD(i)) > tolerance)
@@ -588,10 +601,12 @@
       dst.row(i).setZero();
   }
 
-  // dst = L^-T (D^-1 L^-1 P b)
-  matrixU().solveInPlace(dst);
+  // dst = L^-* (D^-* L^-1 P b)
+  // dst = L^-T (D^-1 L^-*T P b)
+  matrixL().transpose().template conjugateIf<Conjugate>().solveInPlace(dst);
 
-  // dst = P^-1 (L^-T D^-1 L^-1 P b) = A^-1 b
+  // dst = P^T (L^-* D^-* L^-1 P b) = A^-1 b
+  // dst = P^-T (L^-T D^-1 L^-*T P b) = A^-1 b
   dst = m_transpositions.transpose() * dst;
 }
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
index e1624d2..8c9b2b3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Cholesky/LLT.h
@@ -13,6 +13,16 @@
 namespace Eigen {
 
 namespace internal{
+
+template<typename _MatrixType, int _UpLo> struct traits<LLT<_MatrixType, _UpLo> >
+ : traits<_MatrixType>
+{
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
+  enum { Flags = 0 };
+};
+
 template<typename MatrixType, int UpLo> struct LLT_Traits;
 }
 
@@ -54,18 +64,17 @@
   * \sa MatrixBase::llt(), SelfAdjointView::llt(), class LDLT
   */
 template<typename _MatrixType, int _UpLo> class LLT
+        : public SolverBase<LLT<_MatrixType, _UpLo> >
 {
   public:
     typedef _MatrixType MatrixType;
+    typedef SolverBase<LLT> Base;
+    friend class SolverBase<LLT>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(LLT)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-    typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
-    typedef typename MatrixType::StorageIndex StorageIndex;
 
     enum {
       PacketSize = internal::packet_traits<Scalar>::size,
@@ -100,7 +109,7 @@
       compute(matrix.derived());
     }
 
-    /** \brief Constructs a LDLT factorization from a given matrix
+    /** \brief Constructs a LLT factorization from a given matrix
       *
       * This overloaded constructor is provided for \link InplaceDecomposition inplace decomposition \endlink when
       * \c MatrixType is a Eigen::Ref.
@@ -129,6 +138,7 @@
       return Traits::getL(m_matrix);
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** \returns the solution x of \f$ A x = b \f$ using the current decomposition of A.
       *
       * Since this LLT class assumes anyway that the matrix A is invertible, the solution
@@ -141,13 +151,8 @@
       */
     template<typename Rhs>
     inline const Solve<LLT, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "LLT is not initialized.");
-      eigen_assert(m_matrix.rows()==b.rows()
-                && "LLT::solve(): invalid number of rows of the right hand side matrix b");
-      return Solve<LLT, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     template<typename Derived>
     void solveInPlace(const MatrixBase<Derived> &bAndX) const;
@@ -180,7 +185,7 @@
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful,
+      * \returns \c Success if computation was successful,
       *          \c NumericalIssue if the matrix.appears not to be positive definite.
       */
     ComputationInfo info() const
@@ -194,18 +199,20 @@
       * This method is provided for compatibility with other matrix decompositions, thus enabling generic code such as:
       * \code x = decomposition.adjoint().solve(b) \endcode
       */
-    const LLT& adjoint() const { return *this; };
+    const LLT& adjoint() const EIGEN_NOEXCEPT { return *this; };
 
-    inline Index rows() const { return m_matrix.rows(); }
-    inline Index cols() const { return m_matrix.cols(); }
+    inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+    inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     template<typename VectorType>
-    LLT rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
+    LLT & rankUpdate(const VectorType& vec, const RealScalar& sigma = 1);
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
@@ -459,7 +466,7 @@
   */
 template<typename _MatrixType, int _UpLo>
 template<typename VectorType>
-LLT<_MatrixType,_UpLo> LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, const RealScalar& sigma)
+LLT<_MatrixType,_UpLo> & LLT<_MatrixType,_UpLo>::rankUpdate(const VectorType& v, const RealScalar& sigma)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorType);
   eigen_assert(v.size()==m_matrix.cols());
@@ -477,8 +484,17 @@
 template<typename RhsType, typename DstType>
 void LLT<_MatrixType,_UpLo>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
-  dst = rhs;
-  solveInPlace(dst);
+  _solve_impl_transposed<true>(rhs, dst);
+}
+
+template<typename _MatrixType,int _UpLo>
+template<bool Conjugate, typename RhsType, typename DstType>
+void LLT<_MatrixType,_UpLo>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
+    dst = rhs;
+
+    matrixL().template conjugateIf<!Conjugate>().solveInPlace(dst);
+    matrixU().template conjugateIf<!Conjugate>().solveInPlace(dst);
 }
 #endif
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h
new file mode 100644
index 0000000..b6200fa
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArithmeticSequence.h
@@ -0,0 +1,413 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARITHMETIC_SEQUENCE_H
+#define EIGEN_ARITHMETIC_SEQUENCE_H
+
+namespace Eigen {
+
+namespace internal {
+
+#if (!EIGEN_HAS_CXX11) || !((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48)
+template<typename T> struct aseq_negate {};
+
+template<> struct aseq_negate<Index> {
+  typedef Index type;
+};
+
+template<int N> struct aseq_negate<FixedInt<N> > {
+  typedef FixedInt<-N> type;
+};
+
+// Compilation error in the following case:
+template<> struct aseq_negate<FixedInt<DynamicIndex> > {};
+
+template<typename FirstType,typename SizeType,typename IncrType,
+         bool FirstIsSymbolic=symbolic::is_symbolic<FirstType>::value,
+         bool SizeIsSymbolic =symbolic::is_symbolic<SizeType>::value>
+struct aseq_reverse_first_type {
+  typedef Index type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct aseq_reverse_first_type<FirstType,SizeType,IncrType,true,true> {
+  typedef symbolic::AddExpr<FirstType,
+                            symbolic::ProductExpr<symbolic::AddExpr<SizeType,symbolic::ValueExpr<FixedInt<-1> > >,
+                                                  symbolic::ValueExpr<IncrType> >
+                           > type;
+};
+
+template<typename SizeType,typename IncrType,typename EnableIf = void>
+struct aseq_reverse_first_type_aux {
+  typedef Index type;
+};
+
+template<typename SizeType,typename IncrType>
+struct aseq_reverse_first_type_aux<SizeType,IncrType,typename internal::enable_if<bool((SizeType::value+IncrType::value)|0x1)>::type> {
+  typedef FixedInt<(SizeType::value-1)*IncrType::value> type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct aseq_reverse_first_type<FirstType,SizeType,IncrType,true,false> {
+  typedef typename aseq_reverse_first_type_aux<SizeType,IncrType>::type Aux;
+  typedef symbolic::AddExpr<FirstType,symbolic::ValueExpr<Aux> > type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct aseq_reverse_first_type<FirstType,SizeType,IncrType,false,true> {
+  typedef symbolic::AddExpr<symbolic::ProductExpr<symbolic::AddExpr<SizeType,symbolic::ValueExpr<FixedInt<-1> > >,
+                                                  symbolic::ValueExpr<IncrType> >,
+                            symbolic::ValueExpr<> > type;
+};
+#endif
+
+// Helper to cleanup the type of the increment:
+template<typename T> struct cleanup_seq_incr {
+  typedef typename cleanup_index_type<T,DynamicIndex>::type type;
+};
+
+}
+
+//--------------------------------------------------------------------------------
+// seq(first,last,incr) and seqN(first,size,incr)
+//--------------------------------------------------------------------------------
+
+template<typename FirstType=Index,typename SizeType=Index,typename IncrType=internal::FixedInt<1> >
+class ArithmeticSequence;
+
+template<typename FirstType,typename SizeType,typename IncrType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                   typename internal::cleanup_index_type<SizeType>::type,
+                   typename internal::cleanup_seq_incr<IncrType>::type >
+seqN(FirstType first, SizeType size, IncrType incr);
+
+/** \class ArithmeticSequence
+  * \ingroup Core_Module
+  *
+  * This class represents an arithmetic progression \f$ a_0, a_1, a_2, ..., a_{n-1}\f$ defined by
+  * its \em first value \f$ a_0 \f$, its \em size (aka length) \em n, and the \em increment (aka stride)
+  * that is equal to \f$ a_{i+1}-a_{i}\f$ for any \em i.
+  *
+  * It is internally used as the return type of the Eigen::seq and Eigen::seqN functions, and as the input arguments
+  * of DenseBase::operator()(const RowIndices&, const ColIndices&), and most of the time this is the
+  * only way it is used.
+  *
+  * \tparam FirstType type of the first element, usually an Index,
+  *                   but internally it can be a symbolic expression
+  * \tparam SizeType type representing the size of the sequence, usually an Index
+  *                  or a compile time integral constant. Internally, it can also be a symbolic expression
+  * \tparam IncrType type of the increment, can be a runtime Index, or a compile time integral constant (default is compile-time 1)
+  *
+  * \sa Eigen::seq, Eigen::seqN, DenseBase::operator()(const RowIndices&, const ColIndices&), class IndexedView
+  */
+template<typename FirstType,typename SizeType,typename IncrType>
+class ArithmeticSequence
+{
+public:
+  ArithmeticSequence(FirstType first, SizeType size) : m_first(first), m_size(size) {}
+  ArithmeticSequence(FirstType first, SizeType size, IncrType incr) : m_first(first), m_size(size), m_incr(incr) {}
+
+  enum {
+    SizeAtCompileTime = internal::get_fixed_value<SizeType>::value,
+    IncrAtCompileTime = internal::get_fixed_value<IncrType,DynamicIndex>::value
+  };
+
+  /** \returns the size, i.e., number of elements, of the sequence */
+  Index size()  const { return m_size; }
+
+  /** \returns the first element \f$ a_0 \f$ in the sequence */
+  Index first()  const { return m_first; }
+
+  /** \returns the value \f$ a_i \f$ at index \a i in the sequence. */
+  Index operator[](Index i) const { return m_first + i * m_incr; }
+
+  const FirstType& firstObject() const { return m_first; }
+  const SizeType&  sizeObject()  const { return m_size; }
+  const IncrType&  incrObject()  const { return m_incr; }
+
+protected:
+  FirstType m_first;
+  SizeType  m_size;
+  IncrType  m_incr;
+
+public:
+
+#if EIGEN_HAS_CXX11 && ((!EIGEN_COMP_GNUC) || EIGEN_COMP_GNUC>=48)
+  auto reverse() const -> decltype(Eigen::seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr)) {
+    return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr);
+  }
+#else
+protected:
+  typedef typename internal::aseq_negate<IncrType>::type ReverseIncrType;
+  typedef typename internal::aseq_reverse_first_type<FirstType,SizeType,IncrType>::type ReverseFirstType;
+public:
+  ArithmeticSequence<ReverseFirstType,SizeType,ReverseIncrType>
+  reverse() const {
+    return seqN(m_first+(m_size+fix<-1>())*m_incr,m_size,-m_incr);
+  }
+#endif
+};
+
+/** \returns an ArithmeticSequence starting at \a first, of length \a size, and increment \a incr
+  *
+  * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
+template<typename FirstType,typename SizeType,typename IncrType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type,typename internal::cleanup_seq_incr<IncrType>::type >
+seqN(FirstType first, SizeType size, IncrType incr)  {
+  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type,typename internal::cleanup_seq_incr<IncrType>::type>(first,size,incr);
+}
+
+/** \returns an ArithmeticSequence starting at \a first, of length \a size, and unit increment
+  *
+  * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType) */
+template<typename FirstType,typename SizeType>
+ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type >
+seqN(FirstType first, SizeType size)  {
+  return ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,typename internal::cleanup_index_type<SizeType>::type>(first,size);
+}
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+
+/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and with positive (or negative) increment \a incr
+  *
+  * It is essentially an alias to:
+  * \code
+  * seqN(f, (l-f+incr)/incr, incr);
+  * \endcode
+  *
+  * \sa seqN(FirstType,SizeType,IncrType), seq(FirstType,LastType)
+  */
+template<typename FirstType,typename LastType, typename IncrType>
+auto seq(FirstType f, LastType l, IncrType incr);
+
+/** \returns an ArithmeticSequence starting at \a f, up (or down) to \a l, and unit increment
+  *
+  * It is essentially an alias to:
+  * \code
+  * seqN(f,l-f+1);
+  * \endcode
+  *
+  * \sa seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType)
+  */
+template<typename FirstType,typename LastType>
+auto seq(FirstType f, LastType l);
+
+#else // EIGEN_PARSED_BY_DOXYGEN
+
+#if EIGEN_HAS_CXX11
+template<typename FirstType,typename LastType>
+auto seq(FirstType f, LastType l) -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+                                                   (  typename internal::cleanup_index_type<LastType>::type(l)
+                                                    - typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>())))
+{
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              (typename internal::cleanup_index_type<LastType>::type(l)
+               -typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>()));
+}
+
+template<typename FirstType,typename LastType, typename IncrType>
+auto seq(FirstType f, LastType l, IncrType incr)
+  -> decltype(seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+                   (   typename internal::cleanup_index_type<LastType>::type(l)
+                     - typename internal::cleanup_index_type<FirstType>::type(f)+typename internal::cleanup_seq_incr<IncrType>::type(incr)
+                   ) / typename internal::cleanup_seq_incr<IncrType>::type(incr),
+                   typename internal::cleanup_seq_incr<IncrType>::type(incr)))
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              ( typename internal::cleanup_index_type<LastType>::type(l)
+               -typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr)) / CleanedIncrType(incr),
+              CleanedIncrType(incr));
+}
+
+#else // EIGEN_HAS_CXX11
+
+template<typename FirstType,typename LastType>
+typename internal::enable_if<!(symbolic::is_symbolic<FirstType>::value || symbolic::is_symbolic<LastType>::value),
+                             ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,Index> >::type
+seq(FirstType f, LastType l)
+{
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              Index((typename internal::cleanup_index_type<LastType>::type(l)-typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>())));
+}
+
+template<typename FirstTypeDerived,typename LastType>
+typename internal::enable_if<!symbolic::is_symbolic<LastType>::value,
+    ArithmeticSequence<FirstTypeDerived, symbolic::AddExpr<symbolic::AddExpr<symbolic::NegateExpr<FirstTypeDerived>,symbolic::ValueExpr<> >,
+                                                            symbolic::ValueExpr<internal::FixedInt<1> > > > >::type
+seq(const symbolic::BaseExpr<FirstTypeDerived> &f, LastType l)
+{
+  return seqN(f.derived(),(typename internal::cleanup_index_type<LastType>::type(l)-f.derived()+fix<1>()));
+}
+
+template<typename FirstType,typename LastTypeDerived>
+typename internal::enable_if<!symbolic::is_symbolic<FirstType>::value,
+    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                        symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,symbolic::ValueExpr<> >,
+                                          symbolic::ValueExpr<internal::FixedInt<1> > > > >::type
+seq(FirstType f, const symbolic::BaseExpr<LastTypeDerived> &l)
+{
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),(l.derived()-typename internal::cleanup_index_type<FirstType>::type(f)+fix<1>()));
+}
+
+template<typename FirstTypeDerived,typename LastTypeDerived>
+ArithmeticSequence<FirstTypeDerived,
+                    symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,symbolic::NegateExpr<FirstTypeDerived> >,symbolic::ValueExpr<internal::FixedInt<1> > > >
+seq(const symbolic::BaseExpr<FirstTypeDerived> &f, const symbolic::BaseExpr<LastTypeDerived> &l)
+{
+  return seqN(f.derived(),(l.derived()-f.derived()+fix<1>()));
+}
+
+
+template<typename FirstType,typename LastType, typename IncrType>
+typename internal::enable_if<!(symbolic::is_symbolic<FirstType>::value || symbolic::is_symbolic<LastType>::value),
+    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,Index,typename internal::cleanup_seq_incr<IncrType>::type> >::type
+seq(FirstType f, LastType l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              Index((typename internal::cleanup_index_type<LastType>::type(l)-typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr)), incr);
+}
+
+template<typename FirstTypeDerived,typename LastType, typename IncrType>
+typename internal::enable_if<!symbolic::is_symbolic<LastType>::value,
+    ArithmeticSequence<FirstTypeDerived,
+                        symbolic::QuotientExpr<symbolic::AddExpr<symbolic::AddExpr<symbolic::NegateExpr<FirstTypeDerived>,
+                                                                                   symbolic::ValueExpr<> >,
+                                                                 symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                                              symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                        typename internal::cleanup_seq_incr<IncrType>::type> >::type
+seq(const symbolic::BaseExpr<FirstTypeDerived> &f, LastType l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(f.derived(),(typename internal::cleanup_index_type<LastType>::type(l)-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
+}
+
+template<typename FirstType,typename LastTypeDerived, typename IncrType>
+typename internal::enable_if<!symbolic::is_symbolic<FirstType>::value,
+    ArithmeticSequence<typename internal::cleanup_index_type<FirstType>::type,
+                        symbolic::QuotientExpr<symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,symbolic::ValueExpr<> >,
+                                                                 symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                                               symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                        typename internal::cleanup_seq_incr<IncrType>::type> >::type
+seq(FirstType f, const symbolic::BaseExpr<LastTypeDerived> &l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(typename internal::cleanup_index_type<FirstType>::type(f),
+              (l.derived()-typename internal::cleanup_index_type<FirstType>::type(f)+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
+}
+
+template<typename FirstTypeDerived,typename LastTypeDerived, typename IncrType>
+ArithmeticSequence<FirstTypeDerived,
+                    symbolic::QuotientExpr<symbolic::AddExpr<symbolic::AddExpr<LastTypeDerived,
+                                                                               symbolic::NegateExpr<FirstTypeDerived> >,
+                                                             symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                                          symbolic::ValueExpr<typename internal::cleanup_seq_incr<IncrType>::type> >,
+                    typename internal::cleanup_seq_incr<IncrType>::type>
+seq(const symbolic::BaseExpr<FirstTypeDerived> &f, const symbolic::BaseExpr<LastTypeDerived> &l, IncrType incr)
+{
+  typedef typename internal::cleanup_seq_incr<IncrType>::type CleanedIncrType;
+  return seqN(f.derived(),(l.derived()-f.derived()+CleanedIncrType(incr))/CleanedIncrType(incr), incr);
+}
+#endif // EIGEN_HAS_CXX11
+
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+
+#if EIGEN_HAS_CXX11 || defined(EIGEN_PARSED_BY_DOXYGEN)
+/** \cpp11
+  * \returns a symbolic ArithmeticSequence representing the last \a size elements with increment \a incr.
+  *
+  * It is a shortcut for: \code seqN(last-(size-fix<1>)*incr, size, incr) \endcode
+  * 
+  * \sa lastN(SizeType), seqN(FirstType,SizeType), seq(FirstType,LastType,IncrType) */
+template<typename SizeType,typename IncrType>
+auto lastN(SizeType size, IncrType incr)
+-> decltype(seqN(Eigen::last-(size-fix<1>())*incr, size, incr))
+{
+  return seqN(Eigen::last-(size-fix<1>())*incr, size, incr);
+}
+
+/** \cpp11
+  * \returns a symbolic ArithmeticSequence representing the last \a size elements with a unit increment.
+  *
+  *  It is a shortcut for: \code seq(last+fix<1>-size, last) \endcode
+  * 
+  * \sa lastN(SizeType,IncrType, seqN(FirstType,SizeType), seq(FirstType,LastType) */
+template<typename SizeType>
+auto lastN(SizeType size)
+-> decltype(seqN(Eigen::last+fix<1>()-size, size))
+{
+  return seqN(Eigen::last+fix<1>()-size, size);
+}
+#endif
+
+namespace internal {
+
+// Convert a symbolic span into a usable one (i.e., remove last/end "keywords")
+template<typename T>
+struct make_size_type {
+  typedef typename internal::conditional<symbolic::is_symbolic<T>::value, Index, T>::type type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType,int XprSize>
+struct IndexedViewCompatibleType<ArithmeticSequence<FirstType,SizeType,IncrType>, XprSize> {
+  typedef ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType> type;
+};
+
+template<typename FirstType,typename SizeType,typename IncrType>
+ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType>
+makeIndexedViewCompatible(const ArithmeticSequence<FirstType,SizeType,IncrType>& ids, Index size,SpecializedType) {
+  return ArithmeticSequence<Index,typename make_size_type<SizeType>::type,IncrType>(
+            eval_expr_given_size(ids.firstObject(),size),eval_expr_given_size(ids.sizeObject(),size),ids.incrObject());
+}
+
+template<typename FirstType,typename SizeType,typename IncrType>
+struct get_compile_time_incr<ArithmeticSequence<FirstType,SizeType,IncrType> > {
+  enum { value = get_fixed_value<IncrType,DynamicIndex>::value };
+};
+
+} // end namespace internal
+
+/** \namespace Eigen::indexing
+  * \ingroup Core_Module
+  * 
+  * The sole purpose of this namespace is to be able to import all functions
+  * and symbols that are expected to be used within operator() for indexing
+  * and slicing. If you already imported the whole Eigen namespace:
+  * \code using namespace Eigen; \endcode
+  * then you are already all set. Otherwise, if you don't want/cannot import
+  * the whole Eigen namespace, the following line:
+  * \code using namespace Eigen::indexing; \endcode
+  * is equivalent to:
+  * \code
+  using Eigen::all;
+  using Eigen::seq;
+  using Eigen::seqN;
+  using Eigen::lastN; // c++11 only
+  using Eigen::last;
+  using Eigen::lastp1;
+  using Eigen::fix;
+  \endcode
+  */
+namespace indexing {
+  using Eigen::all;
+  using Eigen::seq;
+  using Eigen::seqN;
+  #if EIGEN_HAS_CXX11
+  using Eigen::lastN;
+  #endif
+  using Eigen::last;
+  using Eigen::lastp1;
+  using Eigen::fix;
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_ARITHMETIC_SEQUENCE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h
index 16770fc..20c789b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Array.h
@@ -117,7 +117,7 @@
     {
       return Base::_set(other);
     }
-    
+
     /** Default constructor.
       *
       * For fixed-size matrices, does nothing.
@@ -157,11 +157,50 @@
     EIGEN_DEVICE_FUNC
     Array& operator=(Array&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
     {
-      other.swap(*this);
+      Base::operator=(std::move(other));
       return *this;
     }
 #endif
 
+    #if EIGEN_HAS_CXX11
+    /** \copydoc PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+     *
+     * Example: \include Array_variadic_ctor_cxx11.cpp
+     * Output: \verbinclude Array_variadic_ctor_cxx11.out
+     *
+     * \sa Array(const std::initializer_list<std::initializer_list<Scalar>>&)
+     * \sa Array(const Scalar&), Array(const Scalar&,const Scalar&)
+     */
+    template <typename... ArgTypes>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+      : Base(a0, a1, a2, a3, args...) {}
+
+    /** \brief Constructs an array and initializes it from the coefficients given as initializer-lists grouped by row. \cpp11
+      *
+      * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients:
+      *
+      * Example: \include Array_initializer_list_23_cxx11.cpp
+      * Output: \verbinclude Array_initializer_list_23_cxx11.out
+      *
+      * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is triggered.
+      *
+      * In the case of a compile-time column 1D array, implicit transposition from a single row is allowed.
+      * Therefore <code> Array<int,Dynamic,1>{{1,2,3,4,5}}</code> is legal and the more verbose syntax
+      * <code>Array<int,Dynamic,1>{{1},{2},{3},{4},{5}}</code> can be avoided:
+      *
+      * Example: \include Array_initializer_list_vector_cxx11.cpp
+      * Output: \verbinclude Array_initializer_list_vector_cxx11.out
+      *
+      * In the case of fixed-sized arrays, the initializer list sizes must exactly match the array sizes,
+      * and implicit transposition is allowed for compile-time 1D arrays only.
+      *
+      * \sa  Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+      */
+    EIGEN_DEVICE_FUNC
+    EIGEN_STRONG_INLINE Array(const std::initializer_list<std::initializer_list<Scalar>>& list) : Base(list) {}
+    #endif // end EIGEN_HAS_CXX11
+
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename T>
     EIGEN_DEVICE_FUNC
@@ -178,6 +217,7 @@
       Base::_check_template_params();
       this->template _init2<T0,T1>(val0, val1);
     }
+
     #else
     /** \brief Constructs a fixed-sized array initialized with coefficients starting at \a data */
     EIGEN_DEVICE_FUNC explicit Array(const Scalar *data);
@@ -189,7 +229,8 @@
       */
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE explicit Array(Index dim);
-    /** constructs an initialized 1x1 Array with the given coefficient */
+    /** constructs an initialized 1x1 Array with the given coefficient
+      * \sa const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args */
     Array(const Scalar& value);
     /** constructs an uninitialized array with \a rows rows and \a cols columns.
       *
@@ -197,11 +238,14 @@
       * it is redundant to pass these parameters, so one should use the default constructor
       * Array() instead. */
     Array(Index rows, Index cols);
-    /** constructs an initialized 2D vector with given coefficients */
+    /** constructs an initialized 2D vector with given coefficients
+      * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args) */
     Array(const Scalar& val0, const Scalar& val1);
-    #endif
+    #endif  // end EIGEN_PARSED_BY_DOXYGEN
 
-    /** constructs an initialized 3D vector with given coefficients */
+    /** constructs an initialized 3D vector with given coefficients
+      * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+      */
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2)
     {
@@ -211,7 +255,9 @@
       m_storage.data()[1] = val1;
       m_storage.data()[2] = val2;
     }
-    /** constructs an initialized 4D vector with given coefficients */
+    /** constructs an initialized 4D vector with given coefficients
+      * \sa Array(const Scalar& a0, const Scalar& a1, const Scalar& a2, const Scalar& a3, const ArgTypes&... args)
+      */
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Array(const Scalar& val0, const Scalar& val1, const Scalar& val2, const Scalar& val3)
     {
@@ -242,8 +288,10 @@
       : Base(other.derived())
     { }
 
-    EIGEN_DEVICE_FUNC inline Index innerStride() const { return 1; }
-    EIGEN_DEVICE_FUNC inline Index outerStride() const { return this->innerSize(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT{ return 1; }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); }
 
     #ifdef EIGEN_ARRAY_PLUGIN
     #include EIGEN_ARRAY_PLUGIN
@@ -258,7 +306,7 @@
 /** \defgroup arraytypedefs Global array typedefs
   * \ingroup Core_Module
   *
-  * Eigen defines several typedef shortcuts for most common 1D and 2D array types.
+  * %Eigen defines several typedef shortcuts for most common 1D and 2D array types.
   *
   * The general patterns are the following:
   *
@@ -271,6 +319,12 @@
   * There are also \c ArraySizeType which are self-explanatory. For example, \c Array4cf is
   * a fixed-size 1D array of 4 complex floats.
   *
+  * With \cpp11, template alias are also defined for common sizes.
+  * They follow the same pattern as above except that the scalar type suffix is replaced by a
+  * template parameter, i.e.:
+  *   - `ArrayRowsCols<Type>` where `Rows` and `Cols` can be \c 2,\c 3,\c 4, or \c X for fixed or dynamic size.
+  *   - `ArraySize<Type>` where `Size` can be \c 2,\c 3,\c 4 or \c X for fixed or dynamic size 1D arrays.
+  *
   * \sa class Array
   */
 
@@ -303,8 +357,42 @@
 
 #undef EIGEN_MAKE_ARRAY_TYPEDEFS_ALL_SIZES
 #undef EIGEN_MAKE_ARRAY_TYPEDEFS
+#undef EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS
 
-#undef EIGEN_MAKE_ARRAY_TYPEDEFS_LARGE
+#if EIGEN_HAS_CXX11
+
+#define EIGEN_MAKE_ARRAY_TYPEDEFS(Size, SizeSuffix)               \
+/** \ingroup arraytypedefs */                                     \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Array##SizeSuffix##SizeSuffix = Array<Type, Size, Size>;    \
+/** \ingroup arraytypedefs */                                     \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Array##SizeSuffix = Array<Type, Size, 1>;
+
+#define EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(Size)                     \
+/** \ingroup arraytypedefs */                                     \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Array##Size##X = Array<Type, Size, Dynamic>;                \
+/** \ingroup arraytypedefs */                                     \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Array##X##Size = Array<Type, Dynamic, Size>;
+
+EIGEN_MAKE_ARRAY_TYPEDEFS(2, 2)
+EIGEN_MAKE_ARRAY_TYPEDEFS(3, 3)
+EIGEN_MAKE_ARRAY_TYPEDEFS(4, 4)
+EIGEN_MAKE_ARRAY_TYPEDEFS(Dynamic, X)
+EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(2)
+EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(3)
+EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS(4)
+
+#undef EIGEN_MAKE_ARRAY_TYPEDEFS
+#undef EIGEN_MAKE_ARRAY_FIXED_TYPEDEFS
+
+#endif // EIGEN_HAS_CXX11
 
 #define EIGEN_USING_ARRAY_TYPEDEFS_FOR_TYPE_AND_SIZE(TypeSuffix, SizeSuffix) \
 using Eigen::Matrix##SizeSuffix##TypeSuffix; \
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
index 33f644e..ea3dd1c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayBase.h
@@ -69,6 +69,7 @@
     using Base::coeff;
     using Base::coeffRef;
     using Base::lazyAssign;
+    using Base::operator-;
     using Base::operator=;
     using Base::operator+=;
     using Base::operator-=;
@@ -88,7 +89,6 @@
 
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::ArrayBase
 #define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/CommonCwiseUnaryOps.h"
 #   include "../plugins/MatrixCwiseUnaryOps.h"
 #   include "../plugins/ArrayCwiseUnaryOps.h"
 #   include "../plugins/CommonCwiseBinaryOps.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
index 688aadd..2e9555b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ArrayWrapper.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_ARRAYWRAPPER_H
 #define EIGEN_ARRAYWRAPPER_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \class ArrayWrapper
   * \ingroup Core_Module
@@ -60,14 +60,14 @@
     EIGEN_DEVICE_FUNC
     explicit EIGEN_STRONG_INLINE ArrayWrapper(ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return m_expression.innerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return m_expression.outerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT { return m_expression.innerStride(); }
 
     EIGEN_DEVICE_FUNC
     inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
@@ -90,9 +90,9 @@
     EIGEN_DEVICE_FUNC
     inline void evalTo(Dest& dst) const { dst = m_expression; }
 
-    const typename internal::remove_all<NestedExpressionType>::type& 
     EIGEN_DEVICE_FUNC
-    nestedExpression() const 
+    const typename internal::remove_all<NestedExpressionType>::type&
+    nestedExpression() const
     {
       return m_expression;
     }
@@ -158,14 +158,14 @@
     EIGEN_DEVICE_FUNC
     explicit inline MatrixWrapper(ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return m_expression.innerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return m_expression.outerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT { return m_expression.innerStride(); }
 
     EIGEN_DEVICE_FUNC
     inline ScalarWithConstIfNotLvalue* data() { return m_expression.data(); }
@@ -185,8 +185,8 @@
     }
 
     EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<NestedExpressionType>::type& 
-    nestedExpression() const 
+    const typename internal::remove_all<NestedExpressionType>::type&
+    nestedExpression() const
     {
       return m_expression;
     }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
index 53806ba..655412e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign.h
@@ -16,7 +16,7 @@
 
 template<typename Derived>
 template<typename OtherDerived>
-EIGEN_STRONG_INLINE Derived& DenseBase<Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>
   ::lazyAssign(const DenseBase<OtherDerived>& other)
 {
   enum{
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
index dbe435d..7d76f0c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/AssignEvaluator.h
@@ -17,24 +17,24 @@
 // This implementation is based on Assign.h
 
 namespace internal {
-  
+
 /***************************************************************************
 * Part 1 : the logic deciding a strategy for traversal and unrolling       *
 ***************************************************************************/
 
 // copy_using_evaluator_traits is based on assign_traits
 
-template <typename DstEvaluator, typename SrcEvaluator, typename AssignFunc>
+template <typename DstEvaluator, typename SrcEvaluator, typename AssignFunc, int MaxPacketSize = -1>
 struct copy_using_evaluator_traits
 {
   typedef typename DstEvaluator::XprType Dst;
   typedef typename Dst::Scalar DstScalar;
-  
+
   enum {
     DstFlags = DstEvaluator::Flags,
     SrcFlags = SrcEvaluator::Flags
   };
-  
+
 public:
   enum {
     DstAlignment = DstEvaluator::Alignment,
@@ -51,13 +51,15 @@
     InnerMaxSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime)
               : int(DstFlags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime)
               : int(Dst::MaxRowsAtCompileTime),
+    RestrictedInnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(InnerSize,MaxPacketSize),
+    RestrictedLinearSize = EIGEN_SIZE_MIN_PREFER_FIXED(Dst::SizeAtCompileTime,MaxPacketSize),
     OuterStride = int(outer_stride_at_compile_time<Dst>::ret),
     MaxSizeAtCompileTime = Dst::SizeAtCompileTime
   };
 
   // TODO distinguish between linear traversal and inner-traversals
-  typedef typename find_best_packet<DstScalar,Dst::SizeAtCompileTime>::type LinearPacketType;
-  typedef typename find_best_packet<DstScalar,InnerSize>::type InnerPacketType;
+  typedef typename find_best_packet<DstScalar,RestrictedLinearSize>::type LinearPacketType;
+  typedef typename find_best_packet<DstScalar,RestrictedInnerSize>::type InnerPacketType;
 
   enum {
     LinearPacketSize = unpacket_traits<LinearPacketType>::size,
@@ -97,7 +99,8 @@
 
 public:
   enum {
-    Traversal = int(MayLinearVectorize) && (LinearPacketSize>InnerPacketSize) ? int(LinearVectorizedTraversal)
+    Traversal =  int(Dst::SizeAtCompileTime) == 0 ? int(AllAtOnceTraversal) // If compile-size is zero, traversing will fail at compile-time.
+              : (int(MayLinearVectorize) && (LinearPacketSize>InnerPacketSize)) ? int(LinearVectorizedTraversal)
               : int(MayInnerVectorize)   ? int(InnerVectorizedTraversal)
               : int(MayLinearVectorize)  ? int(LinearVectorizedTraversal)
               : int(MaySliceVectorize)   ? int(SliceVectorizedTraversal)
@@ -135,7 +138,7 @@
                           ? int(CompleteUnrolling)
                           : int(NoUnrolling) )
               : int(Traversal) == int(LinearTraversal)
-                ? ( bool(MayUnrollCompletely) ? int(CompleteUnrolling) 
+                ? ( bool(MayUnrollCompletely) ? int(CompleteUnrolling)
                                               : int(NoUnrolling) )
 #if EIGEN_UNALIGNED_VECTORIZE
               : int(Traversal) == int(SliceVectorizedTraversal)
@@ -172,6 +175,8 @@
     EIGEN_DEBUG_VAR(MaySliceVectorize)
     std::cerr << "Traversal" << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
     EIGEN_DEBUG_VAR(SrcEvaluator::CoeffReadCost)
+    EIGEN_DEBUG_VAR(DstEvaluator::CoeffReadCost)
+    EIGEN_DEBUG_VAR(Dst::SizeAtCompileTime)
     EIGEN_DEBUG_VAR(UnrollingLimit)
     EIGEN_DEBUG_VAR(MayUnrollCompletely)
     EIGEN_DEBUG_VAR(MayUnrollInner)
@@ -195,7 +200,7 @@
   // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
   typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
   typedef typename DstEvaluatorType::XprType DstXprType;
-  
+
   enum {
     outer = Index / DstXprType::InnerSizeAtCompileTime,
     inner = Index % DstXprType::InnerSizeAtCompileTime
@@ -261,7 +266,7 @@
   typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
   typedef typename DstEvaluatorType::XprType DstXprType;
   typedef typename Kernel::PacketType PacketType;
-  
+
   enum {
     outer = Index / DstXprType::InnerSizeAtCompileTime,
     inner = Index % DstXprType::InnerSizeAtCompileTime,
@@ -313,6 +318,22 @@
 struct dense_assignment_loop;
 
 /************************
+***** Special Cases *****
+************************/
+
+// Zero-sized assignment is a no-op.
+template<typename Kernel, int Unrolling>
+struct dense_assignment_loop<Kernel, AllAtOnceTraversal, Unrolling>
+{
+  EIGEN_DEVICE_FUNC static void EIGEN_STRONG_INLINE run(Kernel& /*kernel*/)
+  {
+    typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
+    EIGEN_STATIC_ASSERT(int(DstXprType::SizeAtCompileTime) == 0,
+      EIGEN_INTERNAL_ERROR_PLEASE_FILE_A_BUG_REPORT)
+  }
+};
+
+/************************
 *** Default traversal ***
 ************************/
 
@@ -426,10 +447,10 @@
   {
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     typedef typename Kernel::PacketType PacketType;
-    
+
     enum { size = DstXprType::SizeAtCompileTime,
            packetSize =unpacket_traits<PacketType>::size,
-           alignedSize = (size/packetSize)*packetSize };
+           alignedSize = (int(size)/packetSize)*packetSize };
 
     copy_using_evaluator_innervec_CompleteUnrolling<Kernel, 0, alignedSize>::run(kernel);
     copy_using_evaluator_DefaultTraversal_CompleteUnrolling<Kernel, alignedSize, size>::run(kernel);
@@ -530,7 +551,7 @@
     const Scalar *dst_ptr = kernel.dstDataPtr();
     if((!bool(dstIsAligned)) && (UIntPtr(dst_ptr) % sizeof(Scalar))>0)
     {
-      // the pointer is not aligend-on scalar, so alignment is not possible
+      // the pointer is not aligned-on scalar, so alignment is not possible
       return dense_assignment_loop<Kernel,DefaultTraversal,NoUnrolling>::run(kernel);
     }
     const Index packetAlignedMask = packetSize - 1;
@@ -568,14 +589,15 @@
     typedef typename Kernel::DstEvaluatorType::XprType DstXprType;
     typedef typename Kernel::PacketType PacketType;
 
-    enum { size = DstXprType::InnerSizeAtCompileTime,
+    enum { innerSize = DstXprType::InnerSizeAtCompileTime,
            packetSize =unpacket_traits<PacketType>::size,
-           vectorizableSize = (size/packetSize)*packetSize };
+           vectorizableSize = (int(innerSize) / int(packetSize)) * int(packetSize),
+           size = DstXprType::SizeAtCompileTime };
 
     for(Index outer = 0; outer < kernel.outerSize(); ++outer)
     {
       copy_using_evaluator_innervec_InnerUnrolling<Kernel, 0, vectorizableSize, 0, 0>::run(kernel, outer);
-      copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, vectorizableSize, size>::run(kernel, outer);
+      copy_using_evaluator_DefaultTraversal_InnerUnrolling<Kernel, vectorizableSize, innerSize>::run(kernel, outer);
     }
   }
 };
@@ -599,73 +621,74 @@
   typedef typename DstEvaluatorTypeT::XprType DstXprType;
   typedef typename SrcEvaluatorTypeT::XprType SrcXprType;
 public:
-  
+
   typedef DstEvaluatorTypeT DstEvaluatorType;
   typedef SrcEvaluatorTypeT SrcEvaluatorType;
   typedef typename DstEvaluatorType::Scalar Scalar;
   typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor> AssignmentTraits;
   typedef typename AssignmentTraits::PacketType PacketType;
-  
-  
-  EIGEN_DEVICE_FUNC generic_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
+
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  generic_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
     : m_dst(dst), m_src(src), m_functor(func), m_dstExpr(dstExpr)
   {
     #ifdef EIGEN_DEBUG_ASSIGN
     AssignmentTraits::debug();
     #endif
   }
-  
-  EIGEN_DEVICE_FUNC Index size() const        { return m_dstExpr.size(); }
-  EIGEN_DEVICE_FUNC Index innerSize() const   { return m_dstExpr.innerSize(); }
-  EIGEN_DEVICE_FUNC Index outerSize() const   { return m_dstExpr.outerSize(); }
-  EIGEN_DEVICE_FUNC Index rows() const        { return m_dstExpr.rows(); }
-  EIGEN_DEVICE_FUNC Index cols() const        { return m_dstExpr.cols(); }
-  EIGEN_DEVICE_FUNC Index outerStride() const { return m_dstExpr.outerStride(); }
-  
-  EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() { return m_dst; }
-  EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const { return m_src; }
-  
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_dstExpr.size(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index innerSize() const EIGEN_NOEXCEPT { return m_dstExpr.innerSize(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerSize() const EIGEN_NOEXCEPT { return m_dstExpr.outerSize(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_dstExpr.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_dstExpr.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index outerStride() const EIGEN_NOEXCEPT { return m_dstExpr.outerStride(); }
+
+  EIGEN_DEVICE_FUNC DstEvaluatorType& dstEvaluator() EIGEN_NOEXCEPT { return m_dst; }
+  EIGEN_DEVICE_FUNC const SrcEvaluatorType& srcEvaluator() const EIGEN_NOEXCEPT { return m_src; }
+
   /// Assign src(row,col) to dst(row,col) through the assignment functor.
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index row, Index col)
   {
     m_functor.assignCoeff(m_dst.coeffRef(row,col), m_src.coeff(row,col));
   }
-  
+
   /// \sa assignCoeff(Index,Index)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Index index)
   {
     m_functor.assignCoeff(m_dst.coeffRef(index), m_src.coeff(index));
   }
-  
+
   /// \sa assignCoeff(Index,Index)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeffByOuterInner(Index outer, Index inner)
   {
-    Index row = rowIndexByOuterInner(outer, inner); 
-    Index col = colIndexByOuterInner(outer, inner); 
+    Index row = rowIndexByOuterInner(outer, inner);
+    Index col = colIndexByOuterInner(outer, inner);
     assignCoeff(row, col);
   }
-  
-  
+
+
   template<int StoreMode, int LoadMode, typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index row, Index col)
   {
     m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(row,col), m_src.template packet<LoadMode,PacketType>(row,col));
   }
-  
+
   template<int StoreMode, int LoadMode, typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacket(Index index)
   {
     m_functor.template assignPacket<StoreMode>(&m_dst.coeffRef(index), m_src.template packet<LoadMode,PacketType>(index));
   }
-  
+
   template<int StoreMode, int LoadMode, typename PacketType>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner)
   {
-    Index row = rowIndexByOuterInner(outer, inner); 
+    Index row = rowIndexByOuterInner(outer, inner);
     Index col = colIndexByOuterInner(outer, inner);
     assignPacket<StoreMode,LoadMode,PacketType>(row, col);
   }
-  
+
   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Index rowIndexByOuterInner(Index outer, Index inner)
   {
     typedef typename DstEvaluatorType::ExpressionTraits Traits;
@@ -688,7 +711,7 @@
   {
     return m_dstExpr.data();
   }
-  
+
 protected:
   DstEvaluatorType& m_dst;
   const SrcEvaluatorType& m_src;
@@ -697,6 +720,27 @@
   DstXprType& m_dstExpr;
 };
 
+// Special kernel used when computing small products whose operands have dynamic dimensions.  It ensures that the
+// PacketSize used is no larger than 4, thereby increasing the chance that vectorized instructions will be used
+// when computing the product.
+
+template<typename DstEvaluatorTypeT, typename SrcEvaluatorTypeT, typename Functor>
+class restricted_packet_dense_assignment_kernel : public generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, BuiltIn>
+{
+protected:
+  typedef generic_dense_assignment_kernel<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, BuiltIn> Base;
+ public:
+    typedef typename Base::Scalar Scalar;
+    typedef typename Base::DstXprType DstXprType;
+    typedef copy_using_evaluator_traits<DstEvaluatorTypeT, SrcEvaluatorTypeT, Functor, 4> AssignmentTraits;
+    typedef typename AssignmentTraits::PacketType PacketType;
+
+    EIGEN_DEVICE_FUNC restricted_packet_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr)
+    : Base(dst, src, func, dstExpr)
+  {
+  }
+ };
+
 /***************************************************************************
 * Part 5 : Entry point for dense rectangular assignment
 ***************************************************************************/
@@ -734,13 +778,23 @@
   resize_if_allowed(dst, src, func);
 
   DstEvaluatorType dstEvaluator(dst);
-    
+
   typedef generic_dense_assignment_kernel<DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
   Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
 
   dense_assignment_loop<Kernel>::run(kernel);
 }
 
+// Specialization for filling the destination with a constant value.
+#ifndef EIGEN_GPU_COMPILE_PHASE
+template<typename DstXprType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const Eigen::CwiseNullaryOp<Eigen::internal::scalar_constant_op<typename DstXprType::Scalar>, DstXprType>& src, const internal::assign_op<typename DstXprType::Scalar,typename DstXprType::Scalar>& func)
+{
+  resize_if_allowed(dst, src, func);
+  std::fill_n(dst.data(), dst.size(), src.functor()());
+}
+#endif
+
 template<typename DstXprType, typename SrcXprType>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void call_dense_assignment_loop(DstXprType& dst, const SrcXprType& src)
 {
@@ -756,13 +810,13 @@
 // AssignmentKind must define a Kind typedef.
 template<typename DstShape, typename SrcShape> struct AssignmentKind;
 
-// Assignement kind defined in this file:
+// Assignment kind defined in this file:
 struct Dense2Dense {};
 struct EigenBase2EigenBase {};
 
 template<typename,typename> struct AssignmentKind { typedef EigenBase2EigenBase Kind; };
 template<> struct AssignmentKind<DenseShape,DenseShape> { typedef Dense2Dense Kind; };
-    
+
 // This is the main assignment class
 template< typename DstXprType, typename SrcXprType, typename Functor,
           typename Kind = typename AssignmentKind< typename evaluator_traits<DstXprType>::Shape , typename evaluator_traits<SrcXprType>::Shape >::Kind,
@@ -787,7 +841,7 @@
 {
   call_assignment(dst, src, internal::assign_op<typename Dst::Scalar,typename Src::Scalar>());
 }
-                     
+
 // Deal with "assume-aliasing"
 template<typename Dst, typename Src, typename Func>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
@@ -827,14 +881,35 @@
   typedef typename internal::conditional<NeedToTranspose, Transpose<Dst>, Dst>::type ActualDstTypeCleaned;
   typedef typename internal::conditional<NeedToTranspose, Transpose<Dst>, Dst&>::type ActualDstType;
   ActualDstType actualDst(dst);
-  
+
   // TODO check whether this is the right place to perform these checks:
   EIGEN_STATIC_ASSERT_LVALUE(Dst)
   EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(ActualDstTypeCleaned,Src)
   EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename ActualDstTypeCleaned::Scalar,typename Src::Scalar);
-  
+
   Assignment<ActualDstTypeCleaned,Src,Func>::run(actualDst, src, func);
 }
+
+template<typename Dst, typename Src, typename Func>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+void call_restricted_packet_assignment_no_alias(Dst& dst, const Src& src, const Func& func)
+{
+    typedef evaluator<Dst> DstEvaluatorType;
+    typedef evaluator<Src> SrcEvaluatorType;
+    typedef restricted_packet_dense_assignment_kernel<DstEvaluatorType,SrcEvaluatorType,Func> Kernel;
+
+    EIGEN_STATIC_ASSERT_LVALUE(Dst)
+    EIGEN_CHECK_BINARY_COMPATIBILIY(Func,typename Dst::Scalar,typename Src::Scalar);
+
+    SrcEvaluatorType srcEvaluator(src);
+    resize_if_allowed(dst, src, func);
+
+    DstEvaluatorType dstEvaluator(dst);
+    Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
+
+    dense_assignment_loop<Kernel>::run(kernel);
+}
+
 template<typename Dst, typename Src>
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
 void call_assignment_no_alias(Dst& dst, const Src& src)
@@ -875,7 +950,7 @@
 #ifndef EIGEN_NO_DEBUG
     internal::check_for_aliasing(dst, src);
 #endif
-    
+
     call_dense_assignment_loop(dst, src, func);
   }
 };
@@ -899,7 +974,7 @@
     src.evalTo(dst);
   }
 
-  // NOTE The following two functions are templated to avoid their instanciation if not needed
+  // NOTE The following two functions are templated to avoid their instantiation if not needed
   //      This is needed because some expressions supports evalTo only and/or have 'void' as scalar type.
   template<typename SrcScalarType>
   EIGEN_DEVICE_FUNC
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h
deleted file mode 100644
index 6866095..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Assign_MKL.h
+++ /dev/null
@@ -1,178 +0,0 @@
-/*
- Copyright (c) 2011, Intel Corporation. All rights reserved.
- Copyright (C) 2015 Gael Guennebaud <gael.guennebaud@inria.fr>
- 
- Redistribution and use in source and binary forms, with or without modification,
- are permitted provided that the following conditions are met:
-
- * Redistributions of source code must retain the above copyright notice, this
-   list of conditions and the following disclaimer.
- * Redistributions in binary form must reproduce the above copyright notice,
-   this list of conditions and the following disclaimer in the documentation
-   and/or other materials provided with the distribution.
- * Neither the name of Intel Corporation nor the names of its contributors may
-   be used to endorse or promote products derived from this software without
-   specific prior written permission.
-
- THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
- ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
- WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
- DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
- ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
- (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
- LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
- ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
- (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
- SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
-
- ********************************************************************************
- *   Content : Eigen bindings to Intel(R) MKL
- *   MKL VML support for coefficient-wise unary Eigen expressions like a=b.sin()
- ********************************************************************************
-*/
-
-#ifndef EIGEN_ASSIGN_VML_H
-#define EIGEN_ASSIGN_VML_H
-
-namespace Eigen { 
-
-namespace internal {
-
-template<typename Dst, typename Src>
-class vml_assign_traits
-{
-  private:
-    enum {
-      DstHasDirectAccess = Dst::Flags & DirectAccessBit,
-      SrcHasDirectAccess = Src::Flags & DirectAccessBit,
-      StorageOrdersAgree = (int(Dst::IsRowMajor) == int(Src::IsRowMajor)),
-      InnerSize = int(Dst::IsVectorAtCompileTime) ? int(Dst::SizeAtCompileTime)
-                : int(Dst::Flags)&RowMajorBit ? int(Dst::ColsAtCompileTime)
-                : int(Dst::RowsAtCompileTime),
-      InnerMaxSize  = int(Dst::IsVectorAtCompileTime) ? int(Dst::MaxSizeAtCompileTime)
-                    : int(Dst::Flags)&RowMajorBit ? int(Dst::MaxColsAtCompileTime)
-                    : int(Dst::MaxRowsAtCompileTime),
-      MaxSizeAtCompileTime = Dst::SizeAtCompileTime,
-
-      MightEnableVml = StorageOrdersAgree && DstHasDirectAccess && SrcHasDirectAccess && Src::InnerStrideAtCompileTime==1 && Dst::InnerStrideAtCompileTime==1,
-      MightLinearize = MightEnableVml && (int(Dst::Flags) & int(Src::Flags) & LinearAccessBit),
-      VmlSize = MightLinearize ? MaxSizeAtCompileTime : InnerMaxSize,
-      LargeEnough = VmlSize==Dynamic || VmlSize>=EIGEN_MKL_VML_THRESHOLD
-    };
-  public:
-    enum {
-      EnableVml = MightEnableVml && LargeEnough,
-      Traversal = MightLinearize ? LinearTraversal : DefaultTraversal
-    };
-};
-
-#define EIGEN_PP_EXPAND(ARG) ARG
-#if !defined (EIGEN_FAST_MATH) || (EIGEN_FAST_MATH != 1)
-#define EIGEN_VMLMODE_EXPAND_LA , VML_HA
-#else
-#define EIGEN_VMLMODE_EXPAND_LA , VML_LA
-#endif
-
-#define EIGEN_VMLMODE_EXPAND__ 
-
-#define EIGEN_VMLMODE_PREFIX_LA vm
-#define EIGEN_VMLMODE_PREFIX__  v
-#define EIGEN_VMLMODE_PREFIX(VMLMODE) EIGEN_CAT(EIGEN_VMLMODE_PREFIX_,VMLMODE)
-
-#define EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE)                                           \
-  template< typename DstXprType, typename SrcXprNested>                                                                         \
-  struct Assignment<DstXprType, CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested>, assign_op<EIGENTYPE,EIGENTYPE>,   \
-                   Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> {              \
-    typedef CwiseUnaryOp<scalar_##EIGENOP##_op<EIGENTYPE>, SrcXprNested> SrcXprType;                                            \
-    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &func) {                       \
-      resize_if_allowed(dst, src, func);                                                                                        \
-      eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());                                                       \
-      if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal) {                                              \
-        VMLOP(dst.size(), (const VMLTYPE*)src.nestedExpression().data(),                                                        \
-              (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) );                                           \
-      } else {                                                                                                                  \
-        const Index outerSize = dst.outerSize();                                                                                \
-        for(Index outer = 0; outer < outerSize; ++outer) {                                                                      \
-          const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.nestedExpression().coeffRef(outer,0)) :                             \
-                                                      &(src.nestedExpression().coeffRef(0, outer));                             \
-          EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer));                           \
-          VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr,                                                                      \
-                (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE));                                             \
-        }                                                                                                                       \
-      }                                                                                                                         \
-    }                                                                                                                           \
-  };                                                                                                                            \
-
-
-#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE)                                                         \
-  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),s##VMLOP), float, float, VMLMODE)           \
-  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),d##VMLOP), double, double, VMLMODE)
-
-#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE)                                                         \
-  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),c##VMLOP), scomplex, MKL_Complex8, VMLMODE) \
-  EIGEN_MKL_VML_DECLARE_UNARY_CALL(EIGENOP, EIGEN_CAT(EIGEN_VMLMODE_PREFIX(VMLMODE),z##VMLOP), dcomplex, MKL_Complex16, VMLMODE)
-  
-#define EIGEN_MKL_VML_DECLARE_UNARY_CALLS(EIGENOP, VMLOP, VMLMODE)                                                              \
-  EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(EIGENOP, VMLOP, VMLMODE)                                                               \
-  EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(EIGENOP, VMLOP, VMLMODE)
-
-  
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sin,   Sin,   LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(asin,  Asin,  LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sinh,  Sinh,  LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cos,   Cos,   LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(acos,  Acos,  LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(cosh,  Cosh,  LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tan,   Tan,   LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(atan,  Atan,  LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(tanh,  Tanh,  LA)
-// EIGEN_MKL_VML_DECLARE_UNARY_CALLS(abs,   Abs,    _)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(exp,   Exp,   LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log,   Ln,    LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(log10, Log10, LA)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS(sqrt,  Sqrt,  _)
-
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(square, Sqr,   _)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS_CPLX(arg, Arg,      _)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(round, Round,  _)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(floor, Floor,  _)
-EIGEN_MKL_VML_DECLARE_UNARY_CALLS_REAL(ceil,  Ceil,   _)
-
-#define EIGEN_MKL_VML_DECLARE_POW_CALL(EIGENOP, VMLOP, EIGENTYPE, VMLTYPE, VMLMODE)                                           \
-  template< typename DstXprType, typename SrcXprNested, typename Plain>                                                       \
-  struct Assignment<DstXprType, CwiseBinaryOp<scalar_##EIGENOP##_op<EIGENTYPE,EIGENTYPE>, SrcXprNested,                       \
-                    const CwiseNullaryOp<internal::scalar_constant_op<EIGENTYPE>,Plain> >, assign_op<EIGENTYPE,EIGENTYPE>,    \
-                   Dense2Dense, typename enable_if<vml_assign_traits<DstXprType,SrcXprNested>::EnableVml>::type> {            \
-    typedef CwiseBinaryOp<scalar_##EIGENOP##_op<EIGENTYPE,EIGENTYPE>, SrcXprNested,                                           \
-                    const CwiseNullaryOp<internal::scalar_constant_op<EIGENTYPE>,Plain> > SrcXprType;                         \
-    static void run(DstXprType &dst, const SrcXprType &src, const assign_op<EIGENTYPE,EIGENTYPE> &func) {                     \
-      resize_if_allowed(dst, src, func);                                                                                      \
-      eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());                                                     \
-      VMLTYPE exponent = reinterpret_cast<const VMLTYPE&>(src.rhs().functor().m_other);                                       \
-      if(vml_assign_traits<DstXprType,SrcXprNested>::Traversal==LinearTraversal)                                              \
-      {                                                                                                                       \
-        VMLOP( dst.size(), (const VMLTYPE*)src.lhs().data(), exponent,                                                        \
-              (VMLTYPE*)dst.data() EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE) );                                         \
-      } else {                                                                                                                \
-        const Index outerSize = dst.outerSize();                                                                              \
-        for(Index outer = 0; outer < outerSize; ++outer) {                                                                    \
-          const EIGENTYPE *src_ptr = src.IsRowMajor ? &(src.lhs().coeffRef(outer,0)) :                                        \
-                                                      &(src.lhs().coeffRef(0, outer));                                        \
-          EIGENTYPE *dst_ptr = dst.IsRowMajor ? &(dst.coeffRef(outer,0)) : &(dst.coeffRef(0, outer));                         \
-          VMLOP( dst.innerSize(), (const VMLTYPE*)src_ptr, exponent,                                                          \
-                 (VMLTYPE*)dst_ptr EIGEN_PP_EXPAND(EIGEN_VMLMODE_EXPAND_##VMLMODE));                                          \
-        }                                                                                                                     \
-      }                                                                                                                       \
-    }                                                                                                                         \
-  };
-  
-EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmsPowx, float,    float,         LA)
-EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmdPowx, double,   double,        LA)
-EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmcPowx, scomplex, MKL_Complex8,  LA)
-EIGEN_MKL_VML_DECLARE_POW_CALL(pow, vmzPowx, dcomplex, MKL_Complex16, LA)
-
-} // end namespace internal
-
-} // end namespace Eigen
-
-#endif // EIGEN_ASSIGN_VML_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
index 4978c91..878c024 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BandMatrix.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_BANDMATRIX_H
 #define EIGEN_BANDMATRIX_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 
@@ -45,7 +45,7 @@
     };
 
   public:
-    
+
     using Base::derived;
     using Base::rows;
     using Base::cols;
@@ -55,10 +55,10 @@
 
     /** \returns the number of sub diagonals */
     inline Index subs() const { return derived().subs(); }
-    
+
     /** \returns an expression of the underlying coefficient matrix */
     inline const CoefficientsType& coeffs() const { return derived().coeffs(); }
-    
+
     /** \returns an expression of the underlying coefficient matrix */
     inline CoefficientsType& coeffs() { return derived().coeffs(); }
 
@@ -67,7 +67,7 @@
       * \warning the internal storage must be column major. */
     inline Block<CoefficientsType,Dynamic,1> col(Index i)
     {
-      EIGEN_STATIC_ASSERT((Options&RowMajor)==0,THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
+      EIGEN_STATIC_ASSERT((int(Options) & int(RowMajor)) == 0, THIS_METHOD_IS_ONLY_FOR_COLUMN_MAJOR_MATRICES);
       Index start = 0;
       Index len = coeffs().rows();
       if (i<=supers())
@@ -90,7 +90,7 @@
 
     template<int Index> struct DiagonalIntReturnType {
       enum {
-        ReturnOpposite = (Options&SelfAdjoint) && (((Index)>0 && Supers==0) || ((Index)<0 && Subs==0)),
+        ReturnOpposite = (int(Options) & int(SelfAdjoint)) && (((Index) > 0 && Supers == 0) || ((Index) < 0 && Subs == 0)),
         Conjugate = ReturnOpposite && NumTraits<Scalar>::IsComplex,
         ActualIndex = ReturnOpposite ? -Index : Index,
         DiagonalSize = (RowsAtCompileTime==Dynamic || ColsAtCompileTime==Dynamic)
@@ -130,7 +130,7 @@
       eigen_assert((i<0 && -i<=subs()) || (i>=0 && i<=supers()));
       return Block<const CoefficientsType,1,Dynamic>(coeffs(), supers()-i, std::max<Index>(0,i), 1, diagonalLength(i));
     }
-    
+
     template<typename Dest> inline void evalTo(Dest& dst) const
     {
       dst.resize(rows(),cols());
@@ -192,7 +192,7 @@
     Options = _Options,
     DataRowsAtCompileTime = ((Supers!=Dynamic) && (Subs!=Dynamic)) ? 1 + Supers + Subs : Dynamic
   };
-  typedef Matrix<Scalar,DataRowsAtCompileTime,ColsAtCompileTime,Options&RowMajor?RowMajor:ColMajor> CoefficientsType;
+  typedef Matrix<Scalar, DataRowsAtCompileTime, ColsAtCompileTime, int(Options) & int(RowMajor) ? RowMajor : ColMajor> CoefficientsType;
 };
 
 template<typename _Scalar, int Rows, int Cols, int Supers, int Subs, int Options>
@@ -211,16 +211,16 @@
     }
 
     /** \returns the number of columns */
-    inline Index rows() const { return m_rows.value(); }
+    inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
 
     /** \returns the number of rows */
-    inline Index cols() const { return m_coeffs.cols(); }
+    inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); }
 
     /** \returns the number of super diagonals */
-    inline Index supers() const { return m_supers.value(); }
+    inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); }
 
     /** \returns the number of sub diagonals */
-    inline Index subs() const { return m_subs.value(); }
+    inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); }
 
     inline const CoefficientsType& coeffs() const { return m_coeffs; }
     inline CoefficientsType& coeffs() { return m_coeffs; }
@@ -275,16 +275,16 @@
     }
 
     /** \returns the number of columns */
-    inline Index rows() const { return m_rows.value(); }
+    inline EIGEN_CONSTEXPR Index rows() const { return m_rows.value(); }
 
     /** \returns the number of rows */
-    inline Index cols() const { return m_coeffs.cols(); }
+    inline EIGEN_CONSTEXPR Index cols() const { return m_coeffs.cols(); }
 
     /** \returns the number of super diagonals */
-    inline Index supers() const { return m_supers.value(); }
+    inline EIGEN_CONSTEXPR Index supers() const { return m_supers.value(); }
 
     /** \returns the number of sub diagonals */
-    inline Index subs() const { return m_subs.value(); }
+    inline EIGEN_CONSTEXPR Index subs() const { return m_subs.value(); }
 
     inline const CoefficientsType& coeffs() const { return m_coeffs; }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h
index 11de45c..3206d66 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Block.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_BLOCK_H
 #define EIGEN_BLOCK_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
@@ -52,7 +52,7 @@
     FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
     Flags = (traits<XprType>::Flags & (DirectAccessBit | (InnerPanel?CompressedAccessBit:0))) | FlagsLvalueBit | FlagsRowMajorBit,
     // FIXME DirectAccessBit should not be handled by expressions
-    // 
+    //
     // Alignment is needed by MapBase's assertions
     // We can sefely set it to false here. Internal alignment errors will be detected by an eigen_internal_assert in the respective evaluator
     Alignment = 0
@@ -61,7 +61,7 @@
 
 template<typename XprType, int BlockRows=Dynamic, int BlockCols=Dynamic, bool InnerPanel = false,
          bool HasDirectAccess = internal::has_direct_access<XprType>::ret> class BlockImpl_dense;
-         
+
 } // end namespace internal
 
 template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel, typename StorageKind> class BlockImpl;
@@ -109,13 +109,13 @@
     typedef Impl Base;
     EIGEN_GENERIC_PUBLIC_INTERFACE(Block)
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Block)
-    
+
     typedef typename internal::remove_all<XprType>::type NestedExpression;
-  
+
     /** Column or Row constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline Block(XprType& xpr, Index i) : Impl(xpr,i)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Block(XprType& xpr, Index i) : Impl(xpr,i)
     {
       eigen_assert( (i>=0) && (
           ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && i<xpr.rows())
@@ -124,8 +124,8 @@
 
     /** Fixed-size constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline Block(XprType& xpr, Index startRow, Index startCol)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Block(XprType& xpr, Index startRow, Index startCol)
       : Impl(xpr, startRow, startCol)
     {
       EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
@@ -135,8 +135,8 @@
 
     /** Dynamic-size constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline Block(XprType& xpr,
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Block(XprType& xpr,
           Index startRow, Index startCol,
           Index blockRows, Index blockCols)
       : Impl(xpr, startRow, startCol, blockRows, blockCols)
@@ -147,7 +147,7 @@
           && startCol >= 0 && blockCols >= 0 && startCol <= xpr.cols() - blockCols);
     }
 };
-         
+
 // The generic default implementation for dense block simplu forward to the internal::BlockImpl_dense
 // that must be specialized for direct and non-direct access...
 template<typename XprType, int BlockRows, int BlockCols, bool InnerPanel>
@@ -159,10 +159,10 @@
   public:
     typedef Impl Base;
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(BlockImpl)
-    EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {}
-    EIGEN_DEVICE_FUNC inline BlockImpl(XprType& xpr, Index startRow, Index startCol) : Impl(xpr, startRow, startCol) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index i) : Impl(xpr,i) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol) : Impl(xpr, startRow, startCol) {}
     EIGEN_DEVICE_FUNC
-    inline BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
+    EIGEN_STRONG_INLINE BlockImpl(XprType& xpr, Index startRow, Index startCol, Index blockRows, Index blockCols)
       : Impl(xpr, startRow, startCol, blockRows, blockCols) {}
 };
 
@@ -294,25 +294,25 @@
     EIGEN_DEVICE_FUNC inline Index outerStride() const;
     #endif
 
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
-    { 
-      return m_xpr; 
+    {
+      return m_xpr;
     }
 
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     XprType& nestedExpression() { return m_xpr; }
-      
-    EIGEN_DEVICE_FUNC
-    StorageIndex startRow() const
-    { 
-      return m_startRow.value(); 
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    StorageIndex startRow() const EIGEN_NOEXCEPT
+    {
+      return m_startRow.value();
     }
-      
-    EIGEN_DEVICE_FUNC
-    StorageIndex startCol() const
-    { 
-      return m_startCol.value(); 
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    StorageIndex startCol() const EIGEN_NOEXCEPT
+    {
+      return m_startCol.value();
     }
 
   protected:
@@ -342,9 +342,9 @@
 
     /** Column or Row constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr, Index i)
-      : Base(xpr.data() + i * (    ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor)) 
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    BlockImpl_dense(XprType& xpr, Index i)
+      : Base(xpr.data() + i * (    ((BlockRows==1) && (BlockCols==XprType::ColsAtCompileTime) && (!XprTypeIsRowMajor))
                                 || ((BlockRows==XprType::RowsAtCompileTime) && (BlockCols==1) && ( XprTypeIsRowMajor)) ? xpr.innerStride() : xpr.outerStride()),
              BlockRows==1 ? 1 : xpr.rows(),
              BlockCols==1 ? 1 : xpr.cols()),
@@ -357,8 +357,8 @@
 
     /** Fixed-size constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    BlockImpl_dense(XprType& xpr, Index startRow, Index startCol)
       : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol)),
         m_xpr(xpr), m_startRow(startRow), m_startCol(startCol)
     {
@@ -367,8 +367,8 @@
 
     /** Dynamic-size constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr,
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    BlockImpl_dense(XprType& xpr,
           Index startRow, Index startCol,
           Index blockRows, Index blockCols)
       : Base(xpr.data()+xpr.innerStride()*(XprTypeIsRowMajor?startCol:startRow) + xpr.outerStride()*(XprTypeIsRowMajor?startRow:startCol), blockRows, blockCols),
@@ -377,18 +377,18 @@
       init();
     }
 
-    EIGEN_DEVICE_FUNC
-    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
-    { 
-      return m_xpr; 
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const EIGEN_NOEXCEPT
+    {
+      return m_xpr;
     }
 
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     XprType& nestedExpression() { return m_xpr; }
-      
+
     /** \sa MapBase::innerStride() */
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index innerStride() const EIGEN_NOEXCEPT
     {
       return internal::traits<BlockType>::HasSameStorageOrderAsXprType
              ? m_xpr.innerStride()
@@ -396,23 +396,19 @@
     }
 
     /** \sa MapBase::outerStride() */
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index outerStride() const EIGEN_NOEXCEPT
     {
-      return m_outerStride;
+      return internal::traits<BlockType>::HasSameStorageOrderAsXprType
+                    ? m_xpr.outerStride()
+                    : m_xpr.innerStride();
     }
 
-    EIGEN_DEVICE_FUNC
-    StorageIndex startRow() const
-    {
-      return m_startRow.value();
-    }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    StorageIndex startRow() const EIGEN_NOEXCEPT { return m_startRow.value(); }
 
-    EIGEN_DEVICE_FUNC
-    StorageIndex startCol() const
-    {
-      return m_startCol.value();
-    }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    StorageIndex startCol() const EIGEN_NOEXCEPT { return m_startCol.value(); }
 
   #ifndef __SUNPRO_CC
   // FIXME sunstudio is not friendly with the above friend...
@@ -422,8 +418,8 @@
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     /** \internal used by allowAligned() */
-    EIGEN_DEVICE_FUNC
-    inline BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    BlockImpl_dense(XprType& xpr, const Scalar* data, Index blockRows, Index blockCols)
       : Base(data, blockRows, blockCols), m_xpr(xpr)
     {
       init();
@@ -431,7 +427,7 @@
     #endif
 
   protected:
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     void init()
     {
       m_outerStride = internal::traits<BlockType>::HasSameStorageOrderAsXprType
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
index 8409d87..852de8b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/BooleanRedux.h
@@ -14,58 +14,56 @@
 
 namespace internal {
 
-template<typename Derived, int UnrollCount>
+template<typename Derived, int UnrollCount, int Rows>
 struct all_unroller
 {
-  typedef typename Derived::ExpressionTraits Traits;
   enum {
-    col = (UnrollCount-1) / Traits::RowsAtCompileTime,
-    row = (UnrollCount-1) % Traits::RowsAtCompileTime
+    col = (UnrollCount-1) / Rows,
+    row = (UnrollCount-1) % Rows
   };
 
-  static inline bool run(const Derived &mat)
+  EIGEN_DEVICE_FUNC static inline bool run(const Derived &mat)
   {
-    return all_unroller<Derived, UnrollCount-1>::run(mat) && mat.coeff(row, col);
+    return all_unroller<Derived, UnrollCount-1, Rows>::run(mat) && mat.coeff(row, col);
   }
 };
 
-template<typename Derived>
-struct all_unroller<Derived, 0>
+template<typename Derived, int Rows>
+struct all_unroller<Derived, 0, Rows>
 {
-  static inline bool run(const Derived &/*mat*/) { return true; }
+  EIGEN_DEVICE_FUNC static inline bool run(const Derived &/*mat*/) { return true; }
 };
 
-template<typename Derived>
-struct all_unroller<Derived, Dynamic>
+template<typename Derived, int Rows>
+struct all_unroller<Derived, Dynamic, Rows>
 {
-  static inline bool run(const Derived &) { return false; }
+  EIGEN_DEVICE_FUNC static inline bool run(const Derived &) { return false; }
 };
 
-template<typename Derived, int UnrollCount>
+template<typename Derived, int UnrollCount, int Rows>
 struct any_unroller
 {
-  typedef typename Derived::ExpressionTraits Traits;
   enum {
-    col = (UnrollCount-1) / Traits::RowsAtCompileTime,
-    row = (UnrollCount-1) % Traits::RowsAtCompileTime
+    col = (UnrollCount-1) / Rows,
+    row = (UnrollCount-1) % Rows
   };
   
-  static inline bool run(const Derived &mat)
+  EIGEN_DEVICE_FUNC static inline bool run(const Derived &mat)
   {
-    return any_unroller<Derived, UnrollCount-1>::run(mat) || mat.coeff(row, col);
+    return any_unroller<Derived, UnrollCount-1, Rows>::run(mat) || mat.coeff(row, col);
   }
 };
 
-template<typename Derived>
-struct any_unroller<Derived, 0>
+template<typename Derived, int Rows>
+struct any_unroller<Derived, 0, Rows>
 {
-  static inline bool run(const Derived & /*mat*/) { return false; }
+  EIGEN_DEVICE_FUNC static inline bool run(const Derived & /*mat*/) { return false; }
 };
 
-template<typename Derived>
-struct any_unroller<Derived, Dynamic>
+template<typename Derived, int Rows>
+struct any_unroller<Derived, Dynamic, Rows>
 {
-  static inline bool run(const Derived &) { return false; }
+  EIGEN_DEVICE_FUNC static inline bool run(const Derived &) { return false; }
 };
 
 } // end namespace internal
@@ -78,16 +76,16 @@
   * \sa any(), Cwise::operator<()
   */
 template<typename Derived>
-inline bool DenseBase<Derived>::all() const
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::all() const
 {
   typedef internal::evaluator<Derived> Evaluator;
   enum {
     unroll = SizeAtCompileTime != Dynamic
-          && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits<Scalar>::AddCost) <= EIGEN_UNROLLING_LIMIT
+          && SizeAtCompileTime * (int(Evaluator::CoeffReadCost) + int(NumTraits<Scalar>::AddCost)) <= EIGEN_UNROLLING_LIMIT
   };
   Evaluator evaluator(derived());
   if(unroll)
-    return internal::all_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic>::run(evaluator);
+    return internal::all_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic, internal::traits<Derived>::RowsAtCompileTime>::run(evaluator);
   else
   {
     for(Index j = 0; j < cols(); ++j)
@@ -102,16 +100,16 @@
   * \sa all()
   */
 template<typename Derived>
-inline bool DenseBase<Derived>::any() const
+EIGEN_DEVICE_FUNC inline bool DenseBase<Derived>::any() const
 {
   typedef internal::evaluator<Derived> Evaluator;
   enum {
     unroll = SizeAtCompileTime != Dynamic
-          && SizeAtCompileTime * (Evaluator::CoeffReadCost + NumTraits<Scalar>::AddCost) <= EIGEN_UNROLLING_LIMIT
+          && SizeAtCompileTime * (int(Evaluator::CoeffReadCost) + int(NumTraits<Scalar>::AddCost)) <= EIGEN_UNROLLING_LIMIT
   };
   Evaluator evaluator(derived());
   if(unroll)
-    return internal::any_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic>::run(evaluator);
+    return internal::any_unroller<Evaluator, unroll ? int(SizeAtCompileTime) : Dynamic, internal::traits<Derived>::RowsAtCompileTime>::run(evaluator);
   else
   {
     for(Index j = 0; j < cols(); ++j)
@@ -126,7 +124,7 @@
   * \sa all(), any()
   */
 template<typename Derived>
-inline Eigen::Index DenseBase<Derived>::count() const
+EIGEN_DEVICE_FUNC inline Eigen::Index DenseBase<Derived>::count() const
 {
   return derived().template cast<bool>().template cast<Index>().sum();
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
index d218e98..c0e29c7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CommaInitializer.h
@@ -33,6 +33,8 @@
   inline CommaInitializer(XprType& xpr, const Scalar& s)
     : m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
   {
+    eigen_assert(m_xpr.rows() > 0 && m_xpr.cols() > 0
+      && "Cannot comma-initialize a 0x0 matrix (operator<<)");
     m_xpr.coeffRef(0,0) = s;
   }
 
@@ -41,6 +43,8 @@
   inline CommaInitializer(XprType& xpr, const DenseBase<OtherDerived>& other)
     : m_xpr(xpr), m_row(0), m_col(other.cols()), m_currentBlockRows(other.rows())
   {
+    eigen_assert(m_xpr.rows() >= other.rows() && m_xpr.cols() >= other.cols()
+      && "Cannot comma-initialize a 0x0 matrix (operator<<)");
     m_xpr.block(0, 0, other.rows(), other.cols()) = other;
   }
 
@@ -103,7 +107,7 @@
   EIGEN_EXCEPTION_SPEC(Eigen::eigen_assert_exception)
 #endif
   {
-      finished();
+    finished();
   }
 
   /** \returns the built matrix once all its coefficients have been set.
@@ -141,7 +145,7 @@
   * \sa CommaInitializer::finished(), class CommaInitializer
   */
 template<typename Derived>
-inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
+EIGEN_DEVICE_FUNC inline CommaInitializer<Derived> DenseBase<Derived>::operator<< (const Scalar& s)
 {
   return CommaInitializer<Derived>(*static_cast<Derived*>(this), s);
 }
@@ -149,7 +153,7 @@
 /** \sa operator<<(const Scalar&) */
 template<typename Derived>
 template<typename OtherDerived>
-inline CommaInitializer<Derived>
+EIGEN_DEVICE_FUNC inline CommaInitializer<Derived>
 DenseBase<Derived>::operator<<(const DenseBase<OtherDerived>& other)
 {
   return CommaInitializer<Derived>(*static_cast<Derived *>(this), other);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
index 910889e..0ff8c8d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreEvaluators.h
@@ -14,7 +14,7 @@
 #define EIGEN_COREEVALUATORS_H
 
 namespace Eigen {
-  
+
 namespace internal {
 
 // This class returns the evaluator kind from the expression storage kind.
@@ -63,8 +63,8 @@
 template< typename T,
           typename Kind   = typename evaluator_traits<typename T::NestedExpression>::Kind,
           typename Scalar = typename T::Scalar> struct unary_evaluator;
-          
-// evaluator_traits<T> contains traits for evaluator<T> 
+
+// evaluator_traits<T> contains traits for evaluator<T>
 
 template<typename T>
 struct evaluator_traits_base
@@ -90,7 +90,8 @@
 struct evaluator : public unary_evaluator<T>
 {
   typedef unary_evaluator<T> Base;
-  EIGEN_DEVICE_FUNC explicit evaluator(const T& xpr) : Base(xpr) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const T& xpr) : Base(xpr) {}
 };
 
 
@@ -99,21 +100,29 @@
 struct evaluator<const T>
   : evaluator<T>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   explicit evaluator(const T& xpr) : evaluator<T>(xpr) {}
 };
 
 // ---------- base class for all evaluators ----------
 
 template<typename ExpressionType>
-struct evaluator_base : public noncopyable
+struct evaluator_base
 {
   // TODO that's not very nice to have to propagate all these traits. They are currently only needed to handle outer,inner indices.
   typedef traits<ExpressionType> ExpressionTraits;
-  
+
   enum {
     Alignment = 0
   };
+  // noncopyable:
+  // Don't make this class inherit noncopyable as this kills EBO (Empty Base Optimization)
+  // and make complex evaluator much larger than then should do.
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE evaluator_base() {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ~evaluator_base() {}
+private:
+  EIGEN_DEVICE_FUNC evaluator_base(const evaluator_base&);
+  EIGEN_DEVICE_FUNC const evaluator_base& operator=(const evaluator_base&);
 };
 
 // -------------------- Matrix and Array --------------------
@@ -123,6 +132,33 @@
 // Here we directly specialize evaluator. This is not really a unary expression, and it is, by definition, dense,
 // so no need for more sophisticated dispatching.
 
+// this helper permits to completely eliminate m_outerStride if it is known at compiletime.
+template<typename Scalar,int OuterStride> class plainobjectbase_evaluator_data {
+public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) : data(ptr)
+  {
+#ifndef EIGEN_INTERNAL_DEBUGGING
+    EIGEN_UNUSED_VARIABLE(outerStride);
+#endif
+    eigen_internal_assert(outerStride==OuterStride);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+  Index outerStride() const EIGEN_NOEXCEPT { return OuterStride; }
+  const Scalar *data;
+};
+
+template<typename Scalar> class plainobjectbase_evaluator_data<Scalar,Dynamic> {
+public:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  plainobjectbase_evaluator_data(const Scalar* ptr, Index outerStride) : data(ptr), m_outerStride(outerStride) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Index outerStride() const { return m_outerStride; }
+  const Scalar *data;
+protected:
+  Index m_outerStride;
+};
+
 template<typename Derived>
 struct evaluator<PlainObjectBase<Derived> >
   : evaluator_base<Derived>
@@ -136,23 +172,28 @@
     IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime,
     RowsAtCompileTime = PlainObjectType::RowsAtCompileTime,
     ColsAtCompileTime = PlainObjectType::ColsAtCompileTime,
-    
+
     CoeffReadCost = NumTraits<Scalar>::ReadCost,
     Flags = traits<Derived>::EvaluatorFlags,
     Alignment = traits<Derived>::Alignment
   };
-  
-  EIGEN_DEVICE_FUNC evaluator()
-    : m_data(0),
-      m_outerStride(IsVectorAtCompileTime  ? 0 
-                                           : int(IsRowMajor) ? ColsAtCompileTime 
-                                           : RowsAtCompileTime)
+  enum {
+    // We do not need to know the outer stride for vectors
+    OuterStrideAtCompileTime = IsVectorAtCompileTime  ? 0
+                                                      : int(IsRowMajor) ? ColsAtCompileTime
+                                                                        : RowsAtCompileTime
+  };
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  evaluator()
+    : m_d(0,OuterStrideAtCompileTime)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
-  EIGEN_DEVICE_FUNC explicit evaluator(const PlainObjectType& m)
-    : m_data(m.data()), m_outerStride(IsVectorAtCompileTime ? 0 : m.outerStride()) 
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const PlainObjectType& m)
+    : m_d(m.data(),IsVectorAtCompileTime ? 0 : m.outerStride())
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
@@ -161,30 +202,30 @@
   CoeffReturnType coeff(Index row, Index col) const
   {
     if (IsRowMajor)
-      return m_data[row * m_outerStride.value() + col];
+      return m_d.data[row * m_d.outerStride() + col];
     else
-      return m_data[row + col * m_outerStride.value()];
+      return m_d.data[row + col * m_d.outerStride()];
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   {
-    return m_data[index];
+    return m_d.data[index];
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index row, Index col)
   {
     if (IsRowMajor)
-      return const_cast<Scalar*>(m_data)[row * m_outerStride.value() + col];
+      return const_cast<Scalar*>(m_d.data)[row * m_d.outerStride() + col];
     else
-      return const_cast<Scalar*>(m_data)[row + col * m_outerStride.value()];
+      return const_cast<Scalar*>(m_d.data)[row + col * m_d.outerStride()];
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index index)
   {
-    return const_cast<Scalar*>(m_data)[index];
+    return const_cast<Scalar*>(m_d.data)[index];
   }
 
   template<int LoadMode, typename PacketType>
@@ -192,16 +233,16 @@
   PacketType packet(Index row, Index col) const
   {
     if (IsRowMajor)
-      return ploadt<PacketType, LoadMode>(m_data + row * m_outerStride.value() + col);
+      return ploadt<PacketType, LoadMode>(m_d.data + row * m_d.outerStride() + col);
     else
-      return ploadt<PacketType, LoadMode>(m_data + row + col * m_outerStride.value());
+      return ploadt<PacketType, LoadMode>(m_d.data + row + col * m_d.outerStride());
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index index) const
   {
-    return ploadt<PacketType, LoadMode>(m_data + index);
+    return ploadt<PacketType, LoadMode>(m_d.data + index);
   }
 
   template<int StoreMode,typename PacketType>
@@ -210,26 +251,22 @@
   {
     if (IsRowMajor)
       return pstoret<Scalar, PacketType, StoreMode>
-	            (const_cast<Scalar*>(m_data) + row * m_outerStride.value() + col, x);
+	            (const_cast<Scalar*>(m_d.data) + row * m_d.outerStride() + col, x);
     else
       return pstoret<Scalar, PacketType, StoreMode>
-                    (const_cast<Scalar*>(m_data) + row + col * m_outerStride.value(), x);
+                    (const_cast<Scalar*>(m_d.data) + row + col * m_d.outerStride(), x);
   }
 
   template<int StoreMode, typename PacketType>
   EIGEN_STRONG_INLINE
   void writePacket(Index index, const PacketType& x)
   {
-    return pstoret<Scalar, PacketType, StoreMode>(const_cast<Scalar*>(m_data) + index, x);
+    return pstoret<Scalar, PacketType, StoreMode>(const_cast<Scalar*>(m_d.data) + index, x);
   }
 
 protected:
-  const Scalar *m_data;
 
-  // We do not need to know the outer stride for vectors
-  variable_if_dynamic<Index, IsVectorAtCompileTime  ? 0 
-                                                    : int(IsRowMajor) ? ColsAtCompileTime 
-                                                    : RowsAtCompileTime> m_outerStride;
+  plainobjectbase_evaluator_data<Scalar,OuterStrideAtCompileTime> m_d;
 };
 
 template<typename Scalar, int Rows, int Cols, int Options, int MaxRows, int MaxCols>
@@ -237,11 +274,13 @@
   : evaluator<PlainObjectBase<Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> > >
 {
   typedef Matrix<Scalar, Rows, Cols, Options, MaxRows, MaxCols> XprType;
-  
-  EIGEN_DEVICE_FUNC evaluator() {}
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& m)
-    : evaluator<PlainObjectBase<XprType> >(m) 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  evaluator() {}
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& m)
+    : evaluator<PlainObjectBase<XprType> >(m)
   { }
 };
 
@@ -251,10 +290,12 @@
 {
   typedef Array<Scalar, Rows, Cols, Options, MaxRows, MaxCols> XprType;
 
-  EIGEN_DEVICE_FUNC evaluator() {}
-  
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& m)
-    : evaluator<PlainObjectBase<XprType> >(m) 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  evaluator() {}
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& m)
+    : evaluator<PlainObjectBase<XprType> >(m)
   { }
 };
 
@@ -265,14 +306,15 @@
   : evaluator_base<Transpose<ArgType> >
 {
   typedef Transpose<ArgType> XprType;
-  
+
   enum {
-    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,    
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
     Flags = evaluator<ArgType>::Flags ^ RowMajorBit,
     Alignment = evaluator<ArgType>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& t) : m_argImpl(t.nestedExpression()) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit unary_evaluator(const XprType& t) : m_argImpl(t.nestedExpression()) {}
 
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
@@ -457,10 +499,10 @@
 {
   typedef CwiseNullaryOp<NullaryOp,PlainObjectType> XprType;
   typedef typename internal::remove_all<PlainObjectType>::type PlainObjectTypeCleaned;
-  
+
   enum {
     CoeffReadCost = internal::functor_traits<NullaryOp>::Cost,
-    
+
     Flags = (evaluator<PlainObjectTypeCleaned>::Flags
           &  (  HereditaryBits
               | (functor_has_linear_access<NullaryOp>::ret  ? LinearAccessBit : 0)
@@ -517,19 +559,17 @@
   : evaluator_base<CwiseUnaryOp<UnaryOp, ArgType> >
 {
   typedef CwiseUnaryOp<UnaryOp, ArgType> XprType;
-  
+
   enum {
-    CoeffReadCost = evaluator<ArgType>::CoeffReadCost + functor_traits<UnaryOp>::Cost,
-    
+    CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
+
     Flags = evaluator<ArgType>::Flags
           & (HereditaryBits | LinearAccessBit | (functor_traits<UnaryOp>::PacketAccess ? PacketAccessBit : 0)),
     Alignment = evaluator<ArgType>::Alignment
   };
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  explicit unary_evaluator(const XprType& op)
-    : m_functor(op.functor()), 
-      m_argImpl(op.nestedExpression()) 
+  explicit unary_evaluator(const XprType& op) : m_d(op)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
@@ -540,32 +580,43 @@
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
   {
-    return m_functor(m_argImpl.coeff(row, col));
+    return m_d.func()(m_d.argImpl.coeff(row, col));
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   {
-    return m_functor(m_argImpl.coeff(index));
+    return m_d.func()(m_d.argImpl.coeff(index));
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index row, Index col) const
   {
-    return m_functor.packetOp(m_argImpl.template packet<LoadMode, PacketType>(row, col));
+    return m_d.func().packetOp(m_d.argImpl.template packet<LoadMode, PacketType>(row, col));
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index index) const
   {
-    return m_functor.packetOp(m_argImpl.template packet<LoadMode, PacketType>(index));
+    return m_d.func().packetOp(m_d.argImpl.template packet<LoadMode, PacketType>(index));
   }
 
 protected:
-  const UnaryOp m_functor;
-  evaluator<ArgType> m_argImpl;
+
+  // this helper permits to completely eliminate the functor if it is empty
+  struct Data
+  {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Data(const XprType& xpr) : op(xpr.functor()), argImpl(xpr.nestedExpression()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const UnaryOp& func() const { return op; }
+    UnaryOp op;
+    evaluator<ArgType> argImpl;
+  };
+
+  Data m_d;
 };
 
 // -------------------- CwiseTernaryOp --------------------
@@ -577,7 +628,7 @@
 {
   typedef CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> XprType;
   typedef ternary_evaluator<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> > Base;
-  
+
   EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
@@ -586,10 +637,10 @@
   : evaluator_base<CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> >
 {
   typedef CwiseTernaryOp<TernaryOp, Arg1, Arg2, Arg3> XprType;
-  
+
   enum {
-    CoeffReadCost = evaluator<Arg1>::CoeffReadCost + evaluator<Arg2>::CoeffReadCost + evaluator<Arg3>::CoeffReadCost + functor_traits<TernaryOp>::Cost,
-    
+    CoeffReadCost = int(evaluator<Arg1>::CoeffReadCost) + int(evaluator<Arg2>::CoeffReadCost) + int(evaluator<Arg3>::CoeffReadCost) + int(functor_traits<TernaryOp>::Cost),
+
     Arg1Flags = evaluator<Arg1>::Flags,
     Arg2Flags = evaluator<Arg2>::Flags,
     Arg3Flags = evaluator<Arg3>::Flags,
@@ -609,11 +660,7 @@
         evaluator<Arg3>::Alignment)
   };
 
-  EIGEN_DEVICE_FUNC explicit ternary_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_arg1Impl(xpr.arg1()), 
-      m_arg2Impl(xpr.arg2()), 
-      m_arg3Impl(xpr.arg3())  
+  EIGEN_DEVICE_FUNC explicit ternary_evaluator(const XprType& xpr) : m_d(xpr)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<TernaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
@@ -624,38 +671,48 @@
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
   {
-    return m_functor(m_arg1Impl.coeff(row, col), m_arg2Impl.coeff(row, col), m_arg3Impl.coeff(row, col));
+    return m_d.func()(m_d.arg1Impl.coeff(row, col), m_d.arg2Impl.coeff(row, col), m_d.arg3Impl.coeff(row, col));
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   {
-    return m_functor(m_arg1Impl.coeff(index), m_arg2Impl.coeff(index), m_arg3Impl.coeff(index));
+    return m_d.func()(m_d.arg1Impl.coeff(index), m_d.arg2Impl.coeff(index), m_d.arg3Impl.coeff(index));
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index row, Index col) const
   {
-    return m_functor.packetOp(m_arg1Impl.template packet<LoadMode,PacketType>(row, col),
-                              m_arg2Impl.template packet<LoadMode,PacketType>(row, col),
-                              m_arg3Impl.template packet<LoadMode,PacketType>(row, col));
+    return m_d.func().packetOp(m_d.arg1Impl.template packet<LoadMode,PacketType>(row, col),
+                               m_d.arg2Impl.template packet<LoadMode,PacketType>(row, col),
+                               m_d.arg3Impl.template packet<LoadMode,PacketType>(row, col));
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index index) const
   {
-    return m_functor.packetOp(m_arg1Impl.template packet<LoadMode,PacketType>(index),
-                              m_arg2Impl.template packet<LoadMode,PacketType>(index),
-                              m_arg3Impl.template packet<LoadMode,PacketType>(index));
+    return m_d.func().packetOp(m_d.arg1Impl.template packet<LoadMode,PacketType>(index),
+                               m_d.arg2Impl.template packet<LoadMode,PacketType>(index),
+                               m_d.arg3Impl.template packet<LoadMode,PacketType>(index));
   }
 
 protected:
-  const TernaryOp m_functor;
-  evaluator<Arg1> m_arg1Impl;
-  evaluator<Arg2> m_arg2Impl;
-  evaluator<Arg3> m_arg3Impl;
+  // this helper permits to completely eliminate the functor if it is empty
+  struct Data
+  {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Data(const XprType& xpr) : op(xpr.functor()), arg1Impl(xpr.arg1()), arg2Impl(xpr.arg2()), arg3Impl(xpr.arg3()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const TernaryOp& func() const { return op; }
+    TernaryOp op;
+    evaluator<Arg1> arg1Impl;
+    evaluator<Arg2> arg2Impl;
+    evaluator<Arg3> arg3Impl;
+  };
+
+  Data m_d;
 };
 
 // -------------------- CwiseBinaryOp --------------------
@@ -667,8 +724,9 @@
 {
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
   typedef binary_evaluator<CwiseBinaryOp<BinaryOp, Lhs, Rhs> > Base;
-  
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : Base(xpr) {}
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& xpr) : Base(xpr) {}
 };
 
 template<typename BinaryOp, typename Lhs, typename Rhs>
@@ -676,10 +734,10 @@
   : evaluator_base<CwiseBinaryOp<BinaryOp, Lhs, Rhs> >
 {
   typedef CwiseBinaryOp<BinaryOp, Lhs, Rhs> XprType;
-  
+
   enum {
-    CoeffReadCost = evaluator<Lhs>::CoeffReadCost + evaluator<Rhs>::CoeffReadCost + functor_traits<BinaryOp>::Cost,
-    
+    CoeffReadCost = int(evaluator<Lhs>::CoeffReadCost) + int(evaluator<Rhs>::CoeffReadCost) + int(functor_traits<BinaryOp>::Cost),
+
     LhsFlags = evaluator<Lhs>::Flags,
     RhsFlags = evaluator<Rhs>::Flags,
     SameType = is_same<typename Lhs::Scalar,typename Rhs::Scalar>::value,
@@ -696,10 +754,8 @@
     Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<Lhs>::Alignment,evaluator<Rhs>::Alignment)
   };
 
-  EIGEN_DEVICE_FUNC explicit binary_evaluator(const XprType& xpr)
-    : m_functor(xpr.functor()),
-      m_lhsImpl(xpr.lhs()), 
-      m_rhsImpl(xpr.rhs())  
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit binary_evaluator(const XprType& xpr) : m_d(xpr)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<BinaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
@@ -710,35 +766,46 @@
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
   {
-    return m_functor(m_lhsImpl.coeff(row, col), m_rhsImpl.coeff(row, col));
+    return m_d.func()(m_d.lhsImpl.coeff(row, col), m_d.rhsImpl.coeff(row, col));
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   {
-    return m_functor(m_lhsImpl.coeff(index), m_rhsImpl.coeff(index));
+    return m_d.func()(m_d.lhsImpl.coeff(index), m_d.rhsImpl.coeff(index));
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index row, Index col) const
   {
-    return m_functor.packetOp(m_lhsImpl.template packet<LoadMode,PacketType>(row, col),
-                              m_rhsImpl.template packet<LoadMode,PacketType>(row, col));
+    return m_d.func().packetOp(m_d.lhsImpl.template packet<LoadMode,PacketType>(row, col),
+                               m_d.rhsImpl.template packet<LoadMode,PacketType>(row, col));
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index index) const
   {
-    return m_functor.packetOp(m_lhsImpl.template packet<LoadMode,PacketType>(index),
-                              m_rhsImpl.template packet<LoadMode,PacketType>(index));
+    return m_d.func().packetOp(m_d.lhsImpl.template packet<LoadMode,PacketType>(index),
+                               m_d.rhsImpl.template packet<LoadMode,PacketType>(index));
   }
 
 protected:
-  const BinaryOp m_functor;
-  evaluator<Lhs> m_lhsImpl;
-  evaluator<Rhs> m_rhsImpl;
+
+  // this helper permits to completely eliminate the functor if it is empty
+  struct Data
+  {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Data(const XprType& xpr) : op(xpr.functor()), lhsImpl(xpr.lhs()), rhsImpl(xpr.rhs()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const BinaryOp& func() const { return op; }
+    BinaryOp op;
+    evaluator<Lhs> lhsImpl;
+    evaluator<Rhs> rhsImpl;
+  };
+
+  Data m_d;
 };
 
 // -------------------- CwiseUnaryView --------------------
@@ -748,18 +815,16 @@
   : evaluator_base<CwiseUnaryView<UnaryOp, ArgType> >
 {
   typedef CwiseUnaryView<UnaryOp, ArgType> XprType;
-  
+
   enum {
-    CoeffReadCost = evaluator<ArgType>::CoeffReadCost + functor_traits<UnaryOp>::Cost,
-    
+    CoeffReadCost = int(evaluator<ArgType>::CoeffReadCost) + int(functor_traits<UnaryOp>::Cost),
+
     Flags = (evaluator<ArgType>::Flags & (HereditaryBits | LinearAccessBit | DirectAccessBit)),
-    
+
     Alignment = 0 // FIXME it is not very clear why alignment is necessarily lost...
   };
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op)
-    : m_unaryOp(op.functor()), 
-      m_argImpl(op.nestedExpression()) 
+  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& op) : m_d(op)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(functor_traits<UnaryOp>::Cost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
@@ -771,30 +836,41 @@
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
   {
-    return m_unaryOp(m_argImpl.coeff(row, col));
+    return m_d.func()(m_d.argImpl.coeff(row, col));
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   {
-    return m_unaryOp(m_argImpl.coeff(index));
+    return m_d.func()(m_d.argImpl.coeff(index));
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index row, Index col)
   {
-    return m_unaryOp(m_argImpl.coeffRef(row, col));
+    return m_d.func()(m_d.argImpl.coeffRef(row, col));
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index index)
   {
-    return m_unaryOp(m_argImpl.coeffRef(index));
+    return m_d.func()(m_d.argImpl.coeffRef(index));
   }
 
 protected:
-  const UnaryOp m_unaryOp;
-  evaluator<ArgType> m_argImpl;
+
+  // this helper permits to completely eliminate the functor if it is empty
+  struct Data
+  {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Data(const XprType& xpr) : op(xpr.functor()), argImpl(xpr.nestedExpression()) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const UnaryOp& func() const { return op; }
+    UnaryOp op;
+    evaluator<ArgType> argImpl;
+  };
+
+  Data m_d;
 };
 
 // -------------------- Map --------------------
@@ -811,14 +887,15 @@
   typedef typename XprType::PointerType PointerType;
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
-  
+
   enum {
     IsRowMajor = XprType::RowsAtCompileTime,
     ColsAtCompileTime = XprType::ColsAtCompileTime,
     CoeffReadCost = NumTraits<Scalar>::ReadCost
   };
 
-  EIGEN_DEVICE_FUNC explicit mapbase_evaluator(const XprType& map)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit mapbase_evaluator(const XprType& map)
     : m_data(const_cast<PointerType>(map.data())),
       m_innerStride(map.innerStride()),
       m_outerStride(map.outerStride())
@@ -882,17 +959,21 @@
     internal::pstoret<Scalar, PacketType, StoreMode>(m_data + index * m_innerStride.value(), x);
   }
 protected:
-  EIGEN_DEVICE_FUNC
-  inline Index rowStride() const { return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value(); }
-  EIGEN_DEVICE_FUNC
-  inline Index colStride() const { return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+  Index rowStride() const EIGEN_NOEXCEPT {
+    return XprType::IsRowMajor ? m_outerStride.value() : m_innerStride.value();
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+  Index colStride() const EIGEN_NOEXCEPT {
+     return XprType::IsRowMajor ? m_innerStride.value() : m_outerStride.value();
+  }
 
   PointerType m_data;
   const internal::variable_if_dynamic<Index, XprType::InnerStrideAtCompileTime> m_innerStride;
   const internal::variable_if_dynamic<Index, XprType::OuterStrideAtCompileTime> m_outerStride;
 };
 
-template<typename PlainObjectType, int MapOptions, typename StrideType> 
+template<typename PlainObjectType, int MapOptions, typename StrideType>
 struct evaluator<Map<PlainObjectType, MapOptions, StrideType> >
   : public mapbase_evaluator<Map<PlainObjectType, MapOptions, StrideType>, PlainObjectType>
 {
@@ -900,7 +981,7 @@
   typedef typename XprType::Scalar Scalar;
   // TODO: should check for smaller packet types once we can handle multi-sized packet types
   typedef typename packet_traits<Scalar>::type PacketScalar;
-  
+
   enum {
     InnerStrideAtCompileTime = StrideType::InnerStrideAtCompileTime == 0
                              ? int(PlainObjectType::InnerStrideAtCompileTime)
@@ -912,34 +993,35 @@
     HasNoOuterStride = StrideType::OuterStrideAtCompileTime == 0,
     HasNoStride = HasNoInnerStride && HasNoOuterStride,
     IsDynamicSize = PlainObjectType::SizeAtCompileTime==Dynamic,
-    
+
     PacketAccessMask = bool(HasNoInnerStride) ? ~int(0) : ~int(PacketAccessBit),
     LinearAccessMask = bool(HasNoStride) || bool(PlainObjectType::IsVectorAtCompileTime) ? ~int(0) : ~int(LinearAccessBit),
     Flags = int( evaluator<PlainObjectType>::Flags) & (LinearAccessMask&PacketAccessMask),
-    
+
     Alignment = int(MapOptions)&int(AlignedMask)
   };
 
   EIGEN_DEVICE_FUNC explicit evaluator(const XprType& map)
-    : mapbase_evaluator<XprType, PlainObjectType>(map) 
+    : mapbase_evaluator<XprType, PlainObjectType>(map)
   { }
 };
 
 // -------------------- Ref --------------------
 
-template<typename PlainObjectType, int RefOptions, typename StrideType> 
+template<typename PlainObjectType, int RefOptions, typename StrideType>
 struct evaluator<Ref<PlainObjectType, RefOptions, StrideType> >
   : public mapbase_evaluator<Ref<PlainObjectType, RefOptions, StrideType>, PlainObjectType>
 {
   typedef Ref<PlainObjectType, RefOptions, StrideType> XprType;
-  
+
   enum {
     Flags = evaluator<Map<PlainObjectType, RefOptions, StrideType> >::Flags,
     Alignment = evaluator<Map<PlainObjectType, RefOptions, StrideType> >::Alignment
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& ref)
-    : mapbase_evaluator<XprType, PlainObjectType>(ref) 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& ref)
+    : mapbase_evaluator<XprType, PlainObjectType>(ref)
   { }
 };
 
@@ -947,8 +1029,8 @@
 
 template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel,
          bool HasDirectAccess = internal::has_direct_access<ArgType>::ret> struct block_evaluator;
-         
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel> 
+
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
 struct evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel> >
   : block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel>
 {
@@ -956,15 +1038,15 @@
   typedef typename XprType::Scalar Scalar;
   // TODO: should check for smaller packet types once we can handle multi-sized packet types
   typedef typename packet_traits<Scalar>::type PacketScalar;
-  
+
   enum {
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-    
+
     RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
     ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
     MaxRowsAtCompileTime = traits<XprType>::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = traits<XprType>::MaxColsAtCompileTime,
-    
+
     ArgTypeIsRowMajor = (int(evaluator<ArgType>::Flags)&RowMajorBit) != 0,
     IsRowMajor = (MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1) ? 1
                : (MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1) ? 0
@@ -978,14 +1060,14 @@
                              ? int(outer_stride_at_compile_time<ArgType>::ret)
                              : int(inner_stride_at_compile_time<ArgType>::ret),
     MaskPacketAccessBit = (InnerStrideAtCompileTime == 1 || HasSameStorageOrderAsArgType) ? PacketAccessBit : 0,
-    
-    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (evaluator<ArgType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,    
+
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1 || (InnerPanel && (evaluator<ArgType>::Flags&LinearAccessBit))) ? LinearAccessBit : 0,
     FlagsRowMajorBit = XprType::Flags&RowMajorBit,
     Flags0 = evaluator<ArgType>::Flags & ( (HereditaryBits & ~RowMajorBit) |
                                            DirectAccessBit |
                                            MaskPacketAccessBit),
     Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit,
-    
+
     PacketAlignment = unpacket_traits<PacketScalar>::alignment,
     Alignment0 = (InnerPanel && (OuterStrideAtCompileTime!=Dynamic)
                              && (OuterStrideAtCompileTime!=0)
@@ -993,7 +1075,8 @@
     Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<ArgType>::Alignment, Alignment0)
   };
   typedef block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel> block_evaluator_type;
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& block) : block_evaluator_type(block)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& block) : block_evaluator_type(block)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
@@ -1006,8 +1089,9 @@
 {
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
 
-  EIGEN_DEVICE_FUNC explicit block_evaluator(const XprType& block)
-    : unary_evaluator<XprType>(block) 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit block_evaluator(const XprType& block)
+    : unary_evaluator<XprType>(block)
   {}
 };
 
@@ -1017,79 +1101,74 @@
 {
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& block)
-    : m_argImpl(block.nestedExpression()), 
-      m_startRow(block.startRow()), 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit unary_evaluator(const XprType& block)
+    : m_argImpl(block.nestedExpression()),
+      m_startRow(block.startRow()),
       m_startCol(block.startCol()),
-      m_linear_offset(InnerPanel?(XprType::IsRowMajor ? block.startRow()*block.cols() : block.startCol()*block.rows()):0)
+      m_linear_offset(ForwardLinearAccess?(ArgType::IsRowMajor ? block.startRow()*block.nestedExpression().cols() + block.startCol() : block.startCol()*block.nestedExpression().rows() + block.startRow()):0)
   { }
- 
+
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
   enum {
     RowsAtCompileTime = XprType::RowsAtCompileTime,
-    ForwardLinearAccess = InnerPanel && bool(evaluator<ArgType>::Flags&LinearAccessBit)
+    ForwardLinearAccess = (InnerPanel || int(XprType::IsRowMajor)==int(ArgType::IsRowMajor)) && bool(evaluator<ArgType>::Flags&LinearAccessBit)
   };
- 
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
-  { 
-    return m_argImpl.coeff(m_startRow.value() + row, m_startCol.value() + col); 
+  {
+    return m_argImpl.coeff(m_startRow.value() + row, m_startCol.value() + col);
   }
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
-  { 
-    if (ForwardLinearAccess)
-      return m_argImpl.coeff(m_linear_offset.value() + index); 
-    else
-      return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
+  {
+    return linear_coeff_impl(index, bool_constant<ForwardLinearAccess>());
   }
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index row, Index col)
-  { 
-    return m_argImpl.coeffRef(m_startRow.value() + row, m_startCol.value() + col); 
+  {
+    return m_argImpl.coeffRef(m_startRow.value() + row, m_startCol.value() + col);
   }
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   Scalar& coeffRef(Index index)
-  { 
-    if (ForwardLinearAccess)
-      return m_argImpl.coeffRef(m_linear_offset.value() + index); 
-    else
-      return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
-  }
- 
-  template<int LoadMode, typename PacketType>
-  EIGEN_STRONG_INLINE
-  PacketType packet(Index row, Index col) const 
-  { 
-    return m_argImpl.template packet<LoadMode,PacketType>(m_startRow.value() + row, m_startCol.value() + col); 
+  {
+    return linear_coeffRef_impl(index, bool_constant<ForwardLinearAccess>());
   }
 
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
-  PacketType packet(Index index) const 
-  { 
+  PacketType packet(Index row, Index col) const
+  {
+    return m_argImpl.template packet<LoadMode,PacketType>(m_startRow.value() + row, m_startCol.value() + col);
+  }
+
+  template<int LoadMode, typename PacketType>
+  EIGEN_STRONG_INLINE
+  PacketType packet(Index index) const
+  {
     if (ForwardLinearAccess)
       return m_argImpl.template packet<LoadMode,PacketType>(m_linear_offset.value() + index);
     else
       return packet<LoadMode,PacketType>(RowsAtCompileTime == 1 ? 0 : index,
                                          RowsAtCompileTime == 1 ? index : 0);
   }
-  
+
   template<int StoreMode, typename PacketType>
   EIGEN_STRONG_INLINE
-  void writePacket(Index row, Index col, const PacketType& x) 
+  void writePacket(Index row, Index col, const PacketType& x)
   {
-    return m_argImpl.template writePacket<StoreMode,PacketType>(m_startRow.value() + row, m_startCol.value() + col, x); 
+    return m_argImpl.template writePacket<StoreMode,PacketType>(m_startRow.value() + row, m_startCol.value() + col, x);
   }
-  
+
   template<int StoreMode, typename PacketType>
   EIGEN_STRONG_INLINE
-  void writePacket(Index index, const PacketType& x) 
+  void writePacket(Index index, const PacketType& x)
   {
     if (ForwardLinearAccess)
       return m_argImpl.template writePacket<StoreMode,PacketType>(m_linear_offset.value() + index, x);
@@ -1098,18 +1177,40 @@
                                               RowsAtCompileTime == 1 ? index : 0,
                                               x);
   }
- 
+
 protected:
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  CoeffReturnType linear_coeff_impl(Index index, internal::true_type /* ForwardLinearAccess */) const
+  {
+    return m_argImpl.coeff(m_linear_offset.value() + index);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  CoeffReturnType linear_coeff_impl(Index index, internal::false_type /* not ForwardLinearAccess */) const
+  {
+    return coeff(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Scalar& linear_coeffRef_impl(Index index, internal::true_type /* ForwardLinearAccess */)
+  {
+    return m_argImpl.coeffRef(m_linear_offset.value() + index);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Scalar& linear_coeffRef_impl(Index index, internal::false_type /* not ForwardLinearAccess */)
+  {
+    return coeffRef(RowsAtCompileTime == 1 ? 0 : index, RowsAtCompileTime == 1 ? index : 0);
+  }
+
   evaluator<ArgType> m_argImpl;
   const variable_if_dynamic<Index, (ArgType::RowsAtCompileTime == 1 && BlockRows==1) ? 0 : Dynamic> m_startRow;
   const variable_if_dynamic<Index, (ArgType::ColsAtCompileTime == 1 && BlockCols==1) ? 0 : Dynamic> m_startCol;
-  const variable_if_dynamic<Index, InnerPanel ? Dynamic : 0> m_linear_offset;
+  const variable_if_dynamic<Index, ForwardLinearAccess ? Dynamic : 0> m_linear_offset;
 };
 
-// TODO: This evaluator does not actually use the child evaluator; 
+// TODO: This evaluator does not actually use the child evaluator;
 // all action is via the data() as returned by the Block expression.
 
-template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel> 
+template<typename ArgType, int BlockRows, int BlockCols, bool InnerPanel>
 struct block_evaluator<ArgType, BlockRows, BlockCols, InnerPanel, /* HasDirectAccess */ true>
   : mapbase_evaluator<Block<ArgType, BlockRows, BlockCols, InnerPanel>,
                       typename Block<ArgType, BlockRows, BlockCols, InnerPanel>::PlainObject>
@@ -1117,8 +1218,9 @@
   typedef Block<ArgType, BlockRows, BlockCols, InnerPanel> XprType;
   typedef typename XprType::Scalar Scalar;
 
-  EIGEN_DEVICE_FUNC explicit block_evaluator(const XprType& block)
-    : mapbase_evaluator<XprType, typename XprType::PlainObject>(block) 
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit block_evaluator(const XprType& block)
+    : mapbase_evaluator<XprType, typename XprType::PlainObject>(block)
   {
     // TODO: for the 3.3 release, this should be turned to an internal assertion, but let's keep it as is for the beta lifetime
     eigen_assert(((internal::UIntPtr(block.data()) % EIGEN_PLAIN_ENUM_MAX(1,evaluator<XprType>::Alignment)) == 0) && "data is not aligned");
@@ -1141,18 +1243,19 @@
                                          evaluator<ElseMatrixType>::CoeffReadCost),
 
     Flags = (unsigned int)evaluator<ThenMatrixType>::Flags & evaluator<ElseMatrixType>::Flags & HereditaryBits,
-    
+
     Alignment = EIGEN_PLAIN_ENUM_MIN(evaluator<ThenMatrixType>::Alignment, evaluator<ElseMatrixType>::Alignment)
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& select)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& select)
     : m_conditionImpl(select.conditionMatrix()),
       m_thenImpl(select.thenMatrix()),
       m_elseImpl(select.elseMatrix())
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
- 
+
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
@@ -1172,7 +1275,7 @@
     else
       return m_elseImpl.coeff(index);
   }
- 
+
 protected:
   evaluator<ConditionMatrixType> m_conditionImpl;
   evaluator<ThenMatrixType> m_thenImpl;
@@ -1182,7 +1285,7 @@
 
 // -------------------- Replicate --------------------
 
-template<typename ArgType, int RowFactor, int ColFactor> 
+template<typename ArgType, int RowFactor, int ColFactor>
 struct unary_evaluator<Replicate<ArgType, RowFactor, ColFactor> >
   : evaluator_base<Replicate<ArgType, RowFactor, ColFactor> >
 {
@@ -1193,22 +1296,23 @@
   };
   typedef typename internal::nested_eval<ArgType,Factor>::type ArgTypeNested;
   typedef typename internal::remove_all<ArgTypeNested>::type ArgTypeNestedCleaned;
-  
+
   enum {
     CoeffReadCost = evaluator<ArgTypeNestedCleaned>::CoeffReadCost,
     LinearAccessMask = XprType::IsVectorAtCompileTime ? LinearAccessBit : 0,
     Flags = (evaluator<ArgTypeNestedCleaned>::Flags & (HereditaryBits|LinearAccessMask) & ~RowMajorBit) | (traits<XprType>::Flags & RowMajorBit),
-    
+
     Alignment = evaluator<ArgTypeNestedCleaned>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& replicate)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit unary_evaluator(const XprType& replicate)
     : m_arg(replicate.nestedExpression()),
       m_argImpl(m_arg),
       m_rows(replicate.nestedExpression().rows()),
       m_cols(replicate.nestedExpression().cols())
   {}
- 
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
   {
@@ -1219,10 +1323,10 @@
     const Index actual_col = internal::traits<XprType>::ColsAtCompileTime==1 ? 0
                            : ColFactor==1 ? col
                            : col % m_cols.value();
-    
+
     return m_argImpl.coeff(actual_row, actual_col);
   }
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index index) const
   {
@@ -1230,7 +1334,7 @@
     const Index actual_index = internal::traits<XprType>::RowsAtCompileTime==1
                                   ? (ColFactor==1 ?  index : index%m_cols.value())
                                   : (RowFactor==1 ?  index : index%m_rows.value());
-    
+
     return m_argImpl.coeff(actual_index);
   }
 
@@ -1247,7 +1351,7 @@
 
     return m_argImpl.template packet<LoadMode,PacketType>(actual_row, actual_col);
   }
-  
+
   template<int LoadMode, typename PacketType>
   EIGEN_STRONG_INLINE
   PacketType packet(Index index) const
@@ -1258,7 +1362,7 @@
 
     return m_argImpl.template packet<LoadMode,PacketType>(actual_index);
   }
- 
+
 protected:
   const ArgTypeNested m_arg;
   evaluator<ArgTypeNestedCleaned> m_argImpl;
@@ -1266,64 +1370,6 @@
   const variable_if_dynamic<Index, ArgType::ColsAtCompileTime> m_cols;
 };
 
-
-// -------------------- PartialReduxExpr --------------------
-
-template< typename ArgType, typename MemberOp, int Direction>
-struct evaluator<PartialReduxExpr<ArgType, MemberOp, Direction> >
-  : evaluator_base<PartialReduxExpr<ArgType, MemberOp, Direction> >
-{
-  typedef PartialReduxExpr<ArgType, MemberOp, Direction> XprType;
-  typedef typename internal::nested_eval<ArgType,1>::type ArgTypeNested;
-  typedef typename internal::remove_all<ArgTypeNested>::type ArgTypeNestedCleaned;
-  typedef typename ArgType::Scalar InputScalar;
-  typedef typename XprType::Scalar Scalar;
-  enum {
-    TraversalSize = Direction==int(Vertical) ? int(ArgType::RowsAtCompileTime) :  int(ArgType::ColsAtCompileTime)
-  };
-  typedef typename MemberOp::template Cost<InputScalar,int(TraversalSize)> CostOpType;
-  enum {
-    CoeffReadCost = TraversalSize==Dynamic ? HugeCost
-                  : TraversalSize * evaluator<ArgType>::CoeffReadCost + int(CostOpType::value),
-    
-    Flags = (traits<XprType>::Flags&RowMajorBit) | (evaluator<ArgType>::Flags&(HereditaryBits&(~RowMajorBit))) | LinearAccessBit,
-    
-    Alignment = 0 // FIXME this will need to be improved once PartialReduxExpr is vectorized
-  };
-
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType xpr)
-    : m_arg(xpr.nestedExpression()), m_functor(xpr.functor())
-  {
-    EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize==Dynamic ? HugeCost : int(CostOpType::value));
-    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
-  }
-
-  typedef typename XprType::CoeffReturnType CoeffReturnType;
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const Scalar coeff(Index i, Index j) const
-  {
-    if (Direction==Vertical)
-      return m_functor(m_arg.col(j));
-    else
-      return m_functor(m_arg.row(i));
-  }
-
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-  const Scalar coeff(Index index) const
-  {
-    if (Direction==Vertical)
-      return m_functor(m_arg.col(index));
-    else
-      return m_functor(m_arg.row(index));
-  }
-
-protected:
-  typename internal::add_const_on_value_type<ArgTypeNested>::type m_arg;
-  const MemberOp m_functor;
-};
-
-
 // -------------------- MatrixWrapper and ArrayWrapper --------------------
 //
 // evaluator_wrapper_base<T> is a common base class for the
@@ -1340,7 +1386,8 @@
     Alignment = evaluator<ArgType>::Alignment
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator_wrapper_base(const ArgType& arg) : m_argImpl(arg) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator_wrapper_base(const ArgType& arg) : m_argImpl(arg) {}
 
   typedef typename ArgType::Scalar Scalar;
   typedef typename ArgType::CoeffReturnType CoeffReturnType;
@@ -1407,7 +1454,8 @@
 {
   typedef MatrixWrapper<TArgType> XprType;
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& wrapper)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit unary_evaluator(const XprType& wrapper)
     : evaluator_wrapper_base<MatrixWrapper<TArgType> >(wrapper.nestedExpression())
   { }
 };
@@ -1418,7 +1466,8 @@
 {
   typedef ArrayWrapper<TArgType> XprType;
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& wrapper)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit unary_evaluator(const XprType& wrapper)
     : evaluator_wrapper_base<ArrayWrapper<TArgType> >(wrapper.nestedExpression())
   { }
 };
@@ -1445,9 +1494,9 @@
     ReversePacket = (Direction == BothDirections)
                     || ((Direction == Vertical)   && IsColMajor)
                     || ((Direction == Horizontal) && IsRowMajor),
-                    
+
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-    
+
     // let's enable LinearAccess only with vectorization because of the product overhead
     // FIXME enable DirectAccess with negative strides?
     Flags0 = evaluator<ArgType>::Flags,
@@ -1456,16 +1505,17 @@
                  ? LinearAccessBit : 0,
 
     Flags = int(Flags0) & (HereditaryBits | PacketAccessBit | LinearAccess),
-    
+
     Alignment = 0 // FIXME in some rare cases, Alignment could be preserved, like a Vector4f.
   };
 
-  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& reverse)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit unary_evaluator(const XprType& reverse)
     : m_argImpl(reverse.nestedExpression()),
       m_rows(ReverseRow ? reverse.nestedExpression().rows() : 1),
       m_cols(ReverseCol ? reverse.nestedExpression().cols() : 1)
   { }
- 
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeff(Index row, Index col) const
   {
@@ -1540,7 +1590,7 @@
     m_argImpl.template writePacket<LoadMode>
       (m_rows.value() * m_cols.value() - index - PacketSize, preverse(x));
   }
- 
+
 protected:
   evaluator<ArgType> m_argImpl;
 
@@ -1558,20 +1608,21 @@
   : evaluator_base<Diagonal<ArgType, DiagIndex> >
 {
   typedef Diagonal<ArgType, DiagIndex> XprType;
-  
+
   enum {
     CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
-    
+
     Flags = (unsigned int)(evaluator<ArgType>::Flags & (HereditaryBits | DirectAccessBit) & ~RowMajorBit) | LinearAccessBit,
-    
+
     Alignment = 0
   };
 
-  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& diagonal)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit evaluator(const XprType& diagonal)
     : m_argImpl(diagonal.nestedExpression()),
       m_index(diagonal.index())
   { }
- 
+
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
 
@@ -1604,8 +1655,10 @@
   const internal::variable_if_dynamicindex<Index, XprType::DiagIndex> m_index;
 
 private:
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rowOffset() const { return m_index.value() > 0 ? 0 : -m_index.value(); }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index colOffset() const { return m_index.value() > 0 ? m_index.value() : 0; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+  Index rowOffset() const { return m_index.value() > 0 ? 0 : -m_index.value(); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+  Index colOffset() const { return m_index.value() > 0 ? m_index.value() : 0; }
 };
 
 
@@ -1629,25 +1682,25 @@
   : public dense_xpr_base<EvalToTemp<ArgType> >::type
 {
  public:
- 
+
   typedef typename dense_xpr_base<EvalToTemp>::type Base;
   EIGEN_GENERIC_PUBLIC_INTERFACE(EvalToTemp)
- 
+
   explicit EvalToTemp(const ArgType& arg)
     : m_arg(arg)
   { }
- 
+
   const ArgType& arg() const
   {
     return m_arg;
   }
 
-  Index rows() const 
+  EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT
   {
     return m_arg.rows();
   }
 
-  Index cols() const 
+  EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT
   {
     return m_arg.cols();
   }
@@ -1655,7 +1708,7 @@
  private:
   const ArgType& m_arg;
 };
- 
+
 template<typename ArgType>
 struct evaluator<EvalToTemp<ArgType> >
   : public evaluator<typename ArgType::PlainObject>
@@ -1663,7 +1716,7 @@
   typedef EvalToTemp<ArgType>                   XprType;
   typedef typename ArgType::PlainObject         PlainObject;
   typedef evaluator<PlainObject> Base;
-  
+
   EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr)
     : m_result(xpr.arg())
   {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
index 4eb42b9..b967196 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CoreIterators.h
@@ -48,6 +48,11 @@
     * Explicit zeros are not skipped over. To skip explicit zeros, see class SparseView
     */
   EIGEN_STRONG_INLINE InnerIterator& operator++()   { m_iter.operator++(); return *this; }
+  EIGEN_STRONG_INLINE InnerIterator& operator+=(Index i) { m_iter.operator+=(i); return *this; }
+  EIGEN_STRONG_INLINE InnerIterator operator+(Index i) 
+  { InnerIterator result(*this); result+=i; return result; }
+    
+
   /// \returns the column or row index of the current coefficient.
   EIGEN_STRONG_INLINE Index index() const           { return m_iter.index(); }
   /// \returns the row index of the current coefficient.
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
index a36765e..2202b1c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseBinaryOp.h
@@ -74,7 +74,7 @@
   * \sa MatrixBase::binaryExpr(const MatrixBase<OtherDerived> &,const CustomBinaryOp &) const, class CwiseUnaryOp, class CwiseNullaryOp
   */
 template<typename BinaryOp, typename LhsType, typename RhsType>
-class CwiseBinaryOp : 
+class CwiseBinaryOp :
   public CwiseBinaryOpImpl<
           BinaryOp, LhsType, RhsType,
           typename internal::cwise_promote_storage_type<typename internal::traits<LhsType>::StorageKind,
@@ -83,7 +83,7 @@
   internal::no_assignment_operator
 {
   public:
-    
+
     typedef typename internal::remove_all<BinaryOp>::type Functor;
     typedef typename internal::remove_all<LhsType>::type Lhs;
     typedef typename internal::remove_all<RhsType>::type Rhs;
@@ -100,8 +100,14 @@
     typedef typename internal::remove_reference<LhsNested>::type _LhsNested;
     typedef typename internal::remove_reference<RhsNested>::type _RhsNested;
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, const BinaryOp& func = BinaryOp())
+#if EIGEN_COMP_MSVC && EIGEN_HAS_CXX11
+    //Required for Visual Studio or the Copy constructor will probably not get inlined!
+    EIGEN_STRONG_INLINE
+    CwiseBinaryOp(const CwiseBinaryOp<BinaryOp,LhsType,RhsType>&) = default;
+#endif
+
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    CwiseBinaryOp(const Lhs& aLhs, const Rhs& aRhs, const BinaryOp& func = BinaryOp())
       : m_lhs(aLhs), m_rhs(aRhs), m_functor(func)
     {
       EIGEN_CHECK_BINARY_COMPATIBILIY(BinaryOp,typename Lhs::Scalar,typename Rhs::Scalar);
@@ -110,31 +116,25 @@
       eigen_assert(aLhs.rows() == aRhs.rows() && aLhs.cols() == aRhs.cols());
     }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index rows() const {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT {
       // return the fixed size type if available to enable compile time optimizations
-      if (internal::traits<typename internal::remove_all<LhsNested>::type>::RowsAtCompileTime==Dynamic)
-        return m_rhs.rows();
-      else
-        return m_lhs.rows();
+      return internal::traits<typename internal::remove_all<LhsNested>::type>::RowsAtCompileTime==Dynamic ? m_rhs.rows() : m_lhs.rows();
     }
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index cols() const {
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT {
       // return the fixed size type if available to enable compile time optimizations
-      if (internal::traits<typename internal::remove_all<LhsNested>::type>::ColsAtCompileTime==Dynamic)
-        return m_rhs.cols();
-      else
-        return m_lhs.cols();
+      return internal::traits<typename internal::remove_all<LhsNested>::type>::ColsAtCompileTime==Dynamic ? m_rhs.cols() : m_lhs.cols();
     }
 
     /** \returns the left hand side nested expression */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     const _LhsNested& lhs() const { return m_lhs; }
     /** \returns the right hand side nested expression */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     const _RhsNested& rhs() const { return m_rhs; }
     /** \returns the functor representing the binary operation */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     const BinaryOp& functor() const { return m_functor; }
 
   protected:
@@ -158,7 +158,7 @@
   */
 template<typename Derived>
 template<typename OtherDerived>
-EIGEN_STRONG_INLINE Derived &
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
 MatrixBase<Derived>::operator-=(const MatrixBase<OtherDerived> &other)
 {
   call_assignment(derived(), other.derived(), internal::sub_assign_op<Scalar,typename OtherDerived::Scalar>());
@@ -171,7 +171,7 @@
   */
 template<typename Derived>
 template<typename OtherDerived>
-EIGEN_STRONG_INLINE Derived &
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived &
 MatrixBase<Derived>::operator+=(const MatrixBase<OtherDerived>& other)
 {
   call_assignment(derived(), other.derived(), internal::add_assign_op<Scalar,typename OtherDerived::Scalar>());
@@ -181,4 +181,3 @@
 } // end namespace Eigen
 
 #endif // EIGEN_CWISE_BINARY_OP_H
-
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
index ddd607e..289ec51 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseNullaryOp.h
@@ -74,10 +74,10 @@
             && (ColsAtCompileTime == Dynamic || ColsAtCompileTime == cols));
     }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index rows() const { return m_rows.value(); }
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index cols() const { return m_cols.value(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const { return m_rows.value(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const { return m_cols.value(); }
 
     /** \returns the functor representing the nullary operation */
     EIGEN_DEVICE_FUNC
@@ -105,7 +105,12 @@
   */
 template<typename Derived>
 template<typename CustomNullaryOp>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const CwiseNullaryOp<CustomNullaryOp,typename DenseBase<Derived>::PlainObject>
+#else
+const CwiseNullaryOp<CustomNullaryOp,PlainObject>
+#endif
 DenseBase<Derived>::NullaryExpr(Index rows, Index cols, const CustomNullaryOp& func)
 {
   return CwiseNullaryOp<CustomNullaryOp, PlainObject>(rows, cols, func);
@@ -126,12 +131,17 @@
   *
   * Here is an example with C++11 random generators: \include random_cpp11.cpp
   * Output: \verbinclude random_cpp11.out
-  * 
+  *
   * \sa class CwiseNullaryOp
   */
 template<typename Derived>
 template<typename CustomNullaryOp>
-EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+#else
+const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+#endif
 DenseBase<Derived>::NullaryExpr(Index size, const CustomNullaryOp& func)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
@@ -150,7 +160,12 @@
   */
 template<typename Derived>
 template<typename CustomNullaryOp>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const CwiseNullaryOp<CustomNullaryOp, typename DenseBase<Derived>::PlainObject>
+#else
+const CwiseNullaryOp<CustomNullaryOp, PlainObject>
+#endif
 DenseBase<Derived>::NullaryExpr(const CustomNullaryOp& func)
 {
   return CwiseNullaryOp<CustomNullaryOp, PlainObject>(RowsAtCompileTime, ColsAtCompileTime, func);
@@ -170,7 +185,7 @@
   * \sa class CwiseNullaryOp
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::ConstantReturnType
 DenseBase<Derived>::Constant(Index rows, Index cols, const Scalar& value)
 {
   return DenseBase<Derived>::NullaryExpr(rows, cols, internal::scalar_constant_op<Scalar>(value));
@@ -217,27 +232,32 @@
 
 /** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(Index,const Scalar&,const Scalar&)
   *
-  * \sa LinSpaced(Index,Scalar,Scalar), setLinSpaced(Index,const Scalar&,const Scalar&)
+  * \only_for_vectors
+  *
+  * Example: \include DenseBase_LinSpaced_seq_deprecated.cpp
+  * Output: \verbinclude DenseBase_LinSpaced_seq_deprecated.out
+  *
+  * \sa LinSpaced(Index,const Scalar&, const Scalar&), setLinSpaced(Index,const Scalar&,const Scalar&)
   */
 template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
+EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
 DenseBase<Derived>::LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar,PacketScalar>(low,high,size));
+  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low,high,size));
 }
 
 /** \deprecated because of accuracy loss. In Eigen 3.3, it is an alias for LinSpaced(const Scalar&,const Scalar&)
   *
-  * \sa LinSpaced(Scalar,Scalar)
+  * \sa LinSpaced(const Scalar&, const Scalar&)
   */
 template<typename Derived>
-EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
+EIGEN_DEPRECATED EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename DenseBase<Derived>::RandomAccessLinSpacedReturnType
 DenseBase<Derived>::LinSpaced(Sequential_t, const Scalar& low, const Scalar& high)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar,PacketScalar>(low,high,Derived::SizeAtCompileTime));
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar>(low,high,Derived::SizeAtCompileTime));
 }
 
 /**
@@ -268,7 +288,7 @@
 DenseBase<Derived>::LinSpaced(Index size, const Scalar& low, const Scalar& high)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar,PacketScalar>(low,high,size));
+  return DenseBase<Derived>::NullaryExpr(size, internal::linspaced_op<Scalar>(low,high,size));
 }
 
 /**
@@ -281,7 +301,7 @@
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived)
-  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar,PacketScalar>(low,high,Derived::SizeAtCompileTime));
+  return DenseBase<Derived>::NullaryExpr(Derived::SizeAtCompileTime, internal::linspaced_op<Scalar>(low,high,Derived::SizeAtCompileTime));
 }
 
 /** \returns true if all coefficients in this matrix are approximately equal to \a val, to within precision \a prec */
@@ -363,6 +383,33 @@
   return setConstant(val);
 }
 
+/** Resizes to the given size, changing only the number of columns, and sets all
+  * coefficients in this expression to the given value \a val. For the parameter
+  * of type NoChange_t, just pass the special value \c NoChange.
+  *
+  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setConstant(NoChange_t, Index cols, const Scalar& val)
+{
+  return setConstant(rows(), cols, val);
+}
+
+/** Resizes to the given size, changing only the number of rows, and sets all
+  * coefficients in this expression to the given value \a val. For the parameter
+  * of type NoChange_t, just pass the special value \c NoChange.
+  *
+  * \sa MatrixBase::setConstant(const Scalar&), setConstant(Index,const Scalar&), class CwiseNullaryOp, MatrixBase::Constant(const Scalar&)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setConstant(Index rows, NoChange_t, const Scalar& val)
+{
+  return setConstant(rows, cols(), val);
+}
+
+
 /**
   * \brief Sets a linearly spaced vector.
   *
@@ -383,7 +430,7 @@
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& DenseBase<Derived>::setLinSpaced(Index newSize, const Scalar& low, const Scalar& high)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op<Scalar,PacketScalar>(low,high,newSize));
+  return derived() = Derived::NullaryExpr(newSize, internal::linspaced_op<Scalar>(low,high,newSize));
 }
 
 /**
@@ -536,6 +583,32 @@
   return setConstant(Scalar(0));
 }
 
+/** Resizes to the given size, changing only the number of columns, and sets all
+  * coefficients in this expression to zero. For the parameter of type NoChange_t,
+  * just pass the special value \c NoChange.
+  *
+  * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(Index, NoChange_t), class CwiseNullaryOp, DenseBase::Zero()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setZero(NoChange_t, Index cols)
+{
+  return setZero(rows(), cols);
+}
+
+/** Resizes to the given size, changing only the number of rows, and sets all
+  * coefficients in this expression to zero. For the parameter of type NoChange_t,
+  * just pass the special value \c NoChange.
+  *
+  * \sa DenseBase::setZero(), setZero(Index), setZero(Index, Index), setZero(NoChange_t, Index), class CwiseNullaryOp, DenseBase::Zero()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setZero(Index rows, NoChange_t)
+{
+  return setZero(rows, cols());
+}
+
 // ones:
 
 /** \returns an expression of a matrix where all coefficients equal one.
@@ -662,6 +735,32 @@
   return setConstant(Scalar(1));
 }
 
+/** Resizes to the given size, changing only the number of rows, and sets all
+  * coefficients in this expression to one. For the parameter of type NoChange_t,
+  * just pass the special value \c NoChange.
+  *
+ * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(NoChange_t, Index), class CwiseNullaryOp, MatrixBase::Ones()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setOnes(Index rows, NoChange_t)
+{
+  return setOnes(rows, cols());
+}
+
+/** Resizes to the given size, changing only the number of columns, and sets all
+  * coefficients in this expression to one. For the parameter of type NoChange_t,
+  * just pass the special value \c NoChange.
+  *
+ * \sa MatrixBase::setOnes(), setOnes(Index), setOnes(Index, Index), setOnes(Index, NoChange_t) class CwiseNullaryOp, MatrixBase::Ones()
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setOnes(NoChange_t, Index cols)
+{
+  return setOnes(rows(), cols);
+}
+
 // Identity:
 
 /** \returns an expression of the identity matrix (not necessarily square).
@@ -861,6 +960,42 @@
 EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::BasisReturnType MatrixBase<Derived>::UnitW()
 { return Derived::Unit(3); }
 
+/** \brief Set the coefficients of \c *this to the i-th unit (basis) vector
+  *
+  * \param i index of the unique coefficient to be set to 1
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  eigen_assert(i<size());
+  derived().setZero();
+  derived().coeffRef(i) = Scalar(1);
+  return derived();
+}
+
+/** \brief Resizes to the given \a newSize, and writes the i-th unit (basis) vector into *this.
+  *
+  * \param newSize the new size of the vector
+  * \param i index of the unique coefficient to be set to 1
+  *
+  * \only_for_vectors
+  *
+  * \sa MatrixBase::setIdentity(), class CwiseNullaryOp, MatrixBase::Unit(Index,Index)
+  */
+template<typename Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived& MatrixBase<Derived>::setUnit(Index newSize, Index i)
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  eigen_assert(i<newSize);
+  derived().resize(newSize);
+  return setUnit(i);
+}
+
 } // end namespace Eigen
 
 #endif // EIGEN_CWISE_NULLARY_OP_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
index 1d2dd19..e68c4f7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryOp.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_CWISE_UNARY_OP_H
 #define EIGEN_CWISE_UNARY_OP_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 template<typename UnaryOp, typename XprType>
@@ -24,7 +24,7 @@
   typedef typename XprType::Nested XprTypeNested;
   typedef typename remove_reference<XprTypeNested>::type _XprTypeNested;
   enum {
-    Flags = _XprTypeNested::Flags & RowMajorBit 
+    Flags = _XprTypeNested::Flags & RowMajorBit
   };
 };
 }
@@ -65,10 +65,10 @@
     explicit CwiseUnaryOp(const XprType& xpr, const UnaryOp& func = UnaryOp())
       : m_xpr(xpr), m_functor(func) {}
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Index rows() const { return m_xpr.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
-    Index cols() const { return m_xpr.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
 
     /** \returns the functor representing the unary operation */
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
index 5a30fa8..a06d762 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/CwiseUnaryView.h
@@ -64,24 +64,26 @@
     typedef typename internal::ref_selector<MatrixType>::non_const_type MatrixTypeNested;
     typedef typename internal::remove_all<MatrixType>::type NestedExpression;
 
-    explicit inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
+    explicit EIGEN_DEVICE_FUNC inline CwiseUnaryView(MatrixType& mat, const ViewOp& func = ViewOp())
       : m_matrix(mat), m_functor(func) {}
 
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryView)
 
-    EIGEN_STRONG_INLINE Index rows() const { return m_matrix.rows(); }
-    EIGEN_STRONG_INLINE Index cols() const { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     /** \returns the functor representing unary operation */
-    const ViewOp& functor() const { return m_functor; }
+    EIGEN_DEVICE_FUNC const ViewOp& functor() const { return m_functor; }
 
     /** \returns the nested expression */
-    const typename internal::remove_all<MatrixTypeNested>::type&
+    EIGEN_DEVICE_FUNC const typename internal::remove_all<MatrixTypeNested>::type&
     nestedExpression() const { return m_matrix; }
 
     /** \returns the nested expression */
-    typename internal::remove_reference<MatrixTypeNested>::type&
-    nestedExpression() { return m_matrix.const_cast_derived(); }
+    EIGEN_DEVICE_FUNC typename internal::remove_reference<MatrixTypeNested>::type&
+    nestedExpression() { return m_matrix; }
 
   protected:
     MatrixTypeNested m_matrix;
@@ -108,16 +110,16 @@
 
     EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(CwiseUnaryViewImpl)
-    
+
     EIGEN_DEVICE_FUNC inline Scalar* data() { return &(this->coeffRef(0)); }
     EIGEN_DEVICE_FUNC inline const Scalar* data() const { return &(this->coeff(0)); }
 
-    EIGEN_DEVICE_FUNC inline Index innerStride() const
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const
     {
       return derived().nestedExpression().innerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
     }
 
-    EIGEN_DEVICE_FUNC inline Index outerStride() const
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const
     {
       return derived().nestedExpression().outerStride() * sizeof(typename internal::traits<MatrixType>::Scalar) / sizeof(Scalar);
     }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
index c27a8a8..9b16db6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseBase.h
@@ -14,15 +14,15 @@
 namespace Eigen {
 
 namespace internal {
-  
+
 // The index type defined by EIGEN_DEFAULT_DENSE_INDEX_TYPE must be a signed type.
 // This dummy function simply aims at checking that at compile time.
 static inline void check_DenseIndex_is_signed() {
-  EIGEN_STATIC_ASSERT(NumTraits<DenseIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE); 
+  EIGEN_STATIC_ASSERT(NumTraits<DenseIndex>::IsSigned,THE_INDEX_TYPE_MUST_BE_A_SIGNED_TYPE)
 }
 
 } // end namespace internal
-  
+
 /** \class DenseBase
   * \ingroup Core_Module
   *
@@ -40,7 +40,7 @@
   */
 template<typename Derived> class DenseBase
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-  : public DenseCoeffsBase<Derived>
+  : public DenseCoeffsBase<Derived, internal::accessors_level<Derived>::value>
 #else
   : public DenseCoeffsBase<Derived,DirectWriteAccessors>
 #endif // not EIGEN_PARSED_BY_DOXYGEN
@@ -64,14 +64,14 @@
 
     /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc. */
     typedef typename internal::traits<Derived>::Scalar Scalar;
-    
+
     /** The numeric type of the expression' coefficients, e.g. float, double, int or std::complex<float>, etc.
       *
       * It is an alias for the Scalar type */
     typedef Scalar value_type;
-    
+
     typedef typename NumTraits<Scalar>::Real RealScalar;
-    typedef DenseCoeffsBase<Derived> Base;
+    typedef DenseCoeffsBase<Derived, internal::accessors_level<Derived>::value> Base;
 
     using Base::derived;
     using Base::const_cast_derived;
@@ -150,13 +150,18 @@
           * \sa SizeAtCompileTime, MaxRowsAtCompileTime, MaxColsAtCompileTime
           */
 
-      IsVectorAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime == 1
-                           || internal::traits<Derived>::MaxColsAtCompileTime == 1,
+      IsVectorAtCompileTime = internal::traits<Derived>::RowsAtCompileTime == 1
+                           || internal::traits<Derived>::ColsAtCompileTime == 1,
         /**< This is set to true if either the number of rows or the number of
           * columns is known at compile-time to be equal to 1. Indeed, in that case,
           * we are dealing with a column-vector (if there is only one column) or with
           * a row-vector (if there is only one row). */
 
+      NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2,
+        /**< This value is equal to Tensor::NumDimensions, i.e. 0 for scalars, 1 for vectors,
+         * and 2 for matrices.
+         */
+
       Flags = internal::traits<Derived>::Flags,
         /**< This stores expression \ref flags flags which may or may not be inherited by new expressions
           * constructed from this one. See the \ref flags "list of flags".
@@ -170,11 +175,11 @@
       InnerStrideAtCompileTime = internal::inner_stride_at_compile_time<Derived>::ret,
       OuterStrideAtCompileTime = internal::outer_stride_at_compile_time<Derived>::ret
     };
-    
+
     typedef typename internal::find_best_packet<Scalar,SizeAtCompileTime>::type PacketScalar;
 
     enum { IsPlainObjectBase = 0 };
-    
+
     /** The plain matrix type corresponding to this expression.
       * \sa PlainObject */
     typedef Matrix<typename internal::traits<Derived>::Scalar,
@@ -184,7 +189,7 @@
                 internal::traits<Derived>::MaxRowsAtCompileTime,
                 internal::traits<Derived>::MaxColsAtCompileTime
           > PlainMatrix;
-    
+
     /** The plain array type corresponding to this expression.
       * \sa PlainObject */
     typedef Array<typename internal::traits<Derived>::Scalar,
@@ -206,7 +211,7 @@
 
     /** \returns the number of nonzero coefficients which is in practice the number
       * of stored coefficients. */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index nonZeros() const { return size(); }
 
     /** \returns the outer size.
@@ -214,7 +219,7 @@
       * \note For a vector, this returns just 1. For a matrix (non-vector), this is the major dimension
       * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of columns for a
       * column-major matrix, and the number of rows for a row-major matrix. */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     Index outerSize() const
     {
       return IsVectorAtCompileTime ? 1
@@ -224,9 +229,9 @@
     /** \returns the inner size.
       *
       * \note For a vector, this is just the size. For a matrix (non-vector), this is the minor dimension
-      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a 
+      * with respect to the \ref TopicStorageOrders "storage order", i.e., the number of rows for a
       * column-major matrix, and the number of columns for a row-major matrix. */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     Index innerSize() const
     {
       return IsVectorAtCompileTime ? this->size()
@@ -261,9 +266,9 @@
     /** \internal Represents a matrix with all coefficients equal to one another*/
     typedef CwiseNullaryOp<internal::scalar_constant_op<Scalar>,PlainObject> ConstantReturnType;
     /** \internal \deprecated Represents a vector with linearly spaced coefficients that allows sequential access only. */
-    typedef CwiseNullaryOp<internal::linspaced_op<Scalar,PacketScalar>,PlainObject> SequentialLinSpacedReturnType;
+    EIGEN_DEPRECATED typedef CwiseNullaryOp<internal::linspaced_op<Scalar>,PlainObject> SequentialLinSpacedReturnType;
     /** \internal Represents a vector with linearly spaced coefficients that allows random access. */
-    typedef CwiseNullaryOp<internal::linspaced_op<Scalar,PacketScalar>,PlainObject> RandomAccessLinSpacedReturnType;
+    typedef CwiseNullaryOp<internal::linspaced_op<Scalar>,PlainObject> RandomAccessLinSpacedReturnType;
     /** \internal the return type of MatrixBase::eigenvalues() */
     typedef Matrix<typename NumTraits<typename internal::traits<Derived>::Scalar>::Real, internal::traits<Derived>::ColsAtCompileTime, 1> EigenvaluesReturnType;
 
@@ -297,17 +302,17 @@
     Derived& operator=(const ReturnByValue<OtherDerived>& func);
 
     /** \internal
-      * Copies \a other into *this without evaluating other. \returns a reference to *this.
-      * \deprecated */
+      * Copies \a other into *this without evaluating other. \returns a reference to *this. */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    /** \deprecated */
+    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
     Derived& lazyAssign(const DenseBase<OtherDerived>& other);
 
     EIGEN_DEVICE_FUNC
     CommaInitializer<Derived> operator<< (const Scalar& s);
 
-    /** \deprecated it now returns \c *this */
     template<unsigned int Added,unsigned int Removed>
+    /** \deprecated it now returns \c *this */
     EIGEN_DEPRECATED
     const Derived& flagged() const
     { return derived(); }
@@ -332,12 +337,13 @@
     EIGEN_DEVICE_FUNC static const ConstantReturnType
     Constant(const Scalar& value);
 
-    EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType
+    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
     LinSpaced(Sequential_t, Index size, const Scalar& low, const Scalar& high);
+    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
+    LinSpaced(Sequential_t, const Scalar& low, const Scalar& high);
+
     EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
     LinSpaced(Index size, const Scalar& low, const Scalar& high);
-    EIGEN_DEVICE_FUNC static const SequentialLinSpacedReturnType
-    LinSpaced(Sequential_t, const Scalar& low, const Scalar& high);
     EIGEN_DEVICE_FUNC static const RandomAccessLinSpacedReturnType
     LinSpaced(const Scalar& low, const Scalar& high);
 
@@ -369,7 +375,7 @@
     template<typename OtherDerived> EIGEN_DEVICE_FUNC
     bool isApprox(const DenseBase<OtherDerived>& other,
                   const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     bool isMuchSmallerThan(const RealScalar& other,
                            const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
     template<typename OtherDerived> EIGEN_DEVICE_FUNC
@@ -380,7 +386,7 @@
     EIGEN_DEVICE_FUNC bool isConstant(const Scalar& value, const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
     EIGEN_DEVICE_FUNC bool isZero(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
     EIGEN_DEVICE_FUNC bool isOnes(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
-    
+
     inline bool hasNaN() const;
     inline bool allFinite() const;
 
@@ -394,8 +400,8 @@
       *
       * Notice that in the case of a plain matrix or vector (not an expression) this function just returns
       * a const reference, in order to avoid a useless copy.
-      * 
-      * \warning Be carefull with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page \endlink.
+      *
+      * \warning Be careful with eval() and the auto C++ keyword, as detailed in this \link TopicPitfalls_auto_keyword page \endlink.
       */
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE EvalReturnType eval() const
@@ -405,12 +411,12 @@
       // size types on MSVC.
       return typename internal::eval<Derived>::type(derived());
     }
-    
+
     /** swaps *this with the expression \a other.
       *
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     void swap(const DenseBase<OtherDerived>& other)
     {
       EIGEN_STATIC_ASSERT(!OtherDerived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
@@ -422,7 +428,7 @@
       *
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     void swap(PlainObjectBase<OtherDerived>& other)
     {
       eigen_assert(rows()==other.rows() && cols()==other.cols());
@@ -443,18 +449,58 @@
 
     EIGEN_DEVICE_FUNC Scalar prod() const;
 
+    template<int NaNPropagation>
     EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar minCoeff() const;
+    template<int NaNPropagation>
     EIGEN_DEVICE_FUNC typename internal::traits<Derived>::Scalar maxCoeff() const;
 
-    template<typename IndexType> EIGEN_DEVICE_FUNC
+
+    // By default, the fastest version with undefined NaN propagation semantics is
+    // used.
+    // TODO(rmlarsen): Replace with default template argument when we move to
+    // c++11 or beyond.
+    EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar minCoeff() const {
+      return minCoeff<PropagateFast>();
+    }
+    EIGEN_DEVICE_FUNC inline typename internal::traits<Derived>::Scalar maxCoeff() const {
+      return maxCoeff<PropagateFast>();
+    }
+
+    template<int NaNPropagation, typename IndexType>
+    EIGEN_DEVICE_FUNC
     typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const;
-    template<typename IndexType> EIGEN_DEVICE_FUNC
+    template<int NaNPropagation, typename IndexType>
+    EIGEN_DEVICE_FUNC
     typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const;
-    template<typename IndexType> EIGEN_DEVICE_FUNC
+    template<int NaNPropagation, typename IndexType>
+    EIGEN_DEVICE_FUNC
     typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const;
-    template<typename IndexType> EIGEN_DEVICE_FUNC
+    template<int NaNPropagation, typename IndexType>
+    EIGEN_DEVICE_FUNC
     typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const;
 
+    // TODO(rmlarsen): Replace these methods with a default template argument.
+    template<typename IndexType>
+    EIGEN_DEVICE_FUNC inline
+    typename internal::traits<Derived>::Scalar minCoeff(IndexType* row, IndexType* col) const {
+      return minCoeff<PropagateFast>(row, col);
+    }
+    template<typename IndexType>
+    EIGEN_DEVICE_FUNC inline
+    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* row, IndexType* col) const {
+      return maxCoeff<PropagateFast>(row, col);
+    }
+    template<typename IndexType>
+     EIGEN_DEVICE_FUNC inline
+    typename internal::traits<Derived>::Scalar minCoeff(IndexType* index) const {
+      return minCoeff<PropagateFast>(index);
+    }
+    template<typename IndexType>
+    EIGEN_DEVICE_FUNC inline
+    typename internal::traits<Derived>::Scalar maxCoeff(IndexType* index) const {
+      return maxCoeff<PropagateFast>(index);
+    }
+  
     template<typename BinaryOp>
     EIGEN_DEVICE_FUNC
     Scalar redux(const BinaryOp& func) const;
@@ -493,7 +539,7 @@
     typedef VectorwiseOp<Derived, Vertical> ColwiseReturnType;
     typedef const VectorwiseOp<const Derived, Vertical> ConstColwiseReturnType;
 
-    /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations
+    /** \returns a VectorwiseOp wrapper of *this for broadcasting and partial reductions
     *
     * Example: \include MatrixBase_rowwise.cpp
     * Output: \verbinclude MatrixBase_rowwise.out
@@ -506,7 +552,7 @@
     }
     EIGEN_DEVICE_FUNC RowwiseReturnType rowwise();
 
-    /** \returns a VectorwiseOp wrapper of *this providing additional partial reduction operations
+    /** \returns a VectorwiseOp wrapper of *this broadcasting and partial reductions
     *
     * Example: \include MatrixBase_colwise.cpp
     * Output: \verbinclude MatrixBase_colwise.out
@@ -524,16 +570,16 @@
     static const RandomReturnType Random();
 
     template<typename ThenDerived,typename ElseDerived>
-    const Select<Derived,ThenDerived,ElseDerived>
+    inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived,ElseDerived>
     select(const DenseBase<ThenDerived>& thenMatrix,
            const DenseBase<ElseDerived>& elseMatrix) const;
 
     template<typename ThenDerived>
-    inline const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
+    inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
     select(const DenseBase<ThenDerived>& thenMatrix, const typename ThenDerived::Scalar& elseScalar) const;
 
     template<typename ElseDerived>
-    inline const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
+    inline EIGEN_DEVICE_FUNC const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
     select(const typename ElseDerived::Scalar& thenScalar, const DenseBase<ElseDerived>& elseMatrix) const;
 
     template<int p> RealScalar lpNorm() const;
@@ -567,16 +613,59 @@
     }
     EIGEN_DEVICE_FUNC void reverseInPlace();
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    /** STL-like <a href="https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator">RandomAccessIterator</a>
+      * iterator type as returned by the begin() and end() methods.
+      */
+    typedef random_access_iterator_type iterator;
+    /** This is the const version of iterator (aka read-only) */
+    typedef random_access_iterator_type const_iterator;
+    #else
+    typedef typename internal::conditional< (Flags&DirectAccessBit)==DirectAccessBit,
+                                            internal::pointer_based_stl_iterator<Derived>,
+                                            internal::generic_randaccess_stl_iterator<Derived>
+                                          >::type iterator_type;
+
+    typedef typename internal::conditional< (Flags&DirectAccessBit)==DirectAccessBit,
+                                            internal::pointer_based_stl_iterator<const Derived>,
+                                            internal::generic_randaccess_stl_iterator<const Derived>
+                                          >::type const_iterator_type;
+
+    // Stl-style iterators are supported only for vectors.
+
+    typedef typename internal::conditional< IsVectorAtCompileTime,
+                                            iterator_type,
+                                            void
+                                          >::type iterator;
+
+    typedef typename internal::conditional< IsVectorAtCompileTime,
+                                            const_iterator_type,
+                                            void
+                                          >::type const_iterator;
+    #endif
+
+    inline iterator begin();
+    inline const_iterator begin() const;
+    inline const_iterator cbegin() const;
+    inline iterator end();
+    inline const_iterator end() const;
+    inline const_iterator cend() const;
+
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::DenseBase
 #define EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #define EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(COND)
+#define EIGEN_DOC_UNARY_ADDONS(X,Y)
+#   include "../plugins/CommonCwiseUnaryOps.h"
 #   include "../plugins/BlockMethods.h"
+#   include "../plugins/IndexedViewMethods.h"
+#   include "../plugins/ReshapedMethods.h"
 #   ifdef EIGEN_DENSEBASE_PLUGIN
 #     include EIGEN_DENSEBASE_PLUGIN
 #   endif
 #undef EIGEN_CURRENT_STORAGE_BASE_CLASS
 #undef EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 #undef EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF
+#undef EIGEN_DOC_UNARY_ADDONS
 
     // disable the use of evalTo for dense objects with a nice compilation error
     template<typename Dest>
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
index c4af48a..37fcdb5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseCoeffsBase.h
@@ -22,11 +22,12 @@
 /** \brief Base class providing read-only coefficient access to matrices and arrays.
   * \ingroup Core_Module
   * \tparam Derived Type of the derived class
-  * \tparam #ReadOnlyAccessors Constant indicating read-only access
+  *
+  * \note #ReadOnlyAccessors Constant indicating read-only access
   *
   * This class defines the \c operator() \c const function and friends, which can be used to read specific
   * entries of a matrix or array.
-  * 
+  *
   * \sa DenseCoeffsBase<Derived, WriteAccessors>, DenseCoeffsBase<Derived, DirectAccessors>,
   *     \ref TopicClassHierarchy
   */
@@ -288,12 +289,13 @@
 /** \brief Base class providing read/write coefficient access to matrices and arrays.
   * \ingroup Core_Module
   * \tparam Derived Type of the derived class
-  * \tparam #WriteAccessors Constant indicating read/write access
+  *
+  * \note #WriteAccessors Constant indicating read/write access
   *
   * This class defines the non-const \c operator() function and friends, which can be used to write specific
   * entries of a matrix or array. This class inherits DenseCoeffsBase<Derived, ReadOnlyAccessors> which
   * defines the const variant for reading specific entries.
-  * 
+  *
   * \sa DenseCoeffsBase<Derived, DirectAccessors>, \ref TopicClassHierarchy
   */
 template<typename Derived>
@@ -466,7 +468,8 @@
 /** \brief Base class providing direct read-only coefficient access to matrices and arrays.
   * \ingroup Core_Module
   * \tparam Derived Type of the derived class
-  * \tparam #DirectAccessors Constant indicating direct access
+  *
+  * \note #DirectAccessors Constant indicating direct access
   *
   * This class defines functions to work with strides which can be used to access entries directly. This class
   * inherits DenseCoeffsBase<Derived, ReadOnlyAccessors> which defines functions to access entries read-only using
@@ -492,7 +495,7 @@
       *
       * \sa outerStride(), rowStride(), colStride()
       */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index innerStride() const
     {
       return derived().innerStride();
@@ -503,14 +506,14 @@
       *
       * \sa innerStride(), rowStride(), colStride()
       */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index outerStride() const
     {
       return derived().outerStride();
     }
 
     // FIXME shall we remove it ?
-    inline Index stride() const
+    EIGEN_CONSTEXPR inline Index stride() const
     {
       return Derived::IsVectorAtCompileTime ? innerStride() : outerStride();
     }
@@ -519,7 +522,7 @@
       *
       * \sa innerStride(), outerStride(), colStride()
       */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index rowStride() const
     {
       return Derived::IsRowMajor ? outerStride() : innerStride();
@@ -529,7 +532,7 @@
       *
       * \sa innerStride(), outerStride(), rowStride()
       */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index colStride() const
     {
       return Derived::IsRowMajor ? innerStride() : outerStride();
@@ -539,7 +542,8 @@
 /** \brief Base class providing direct read/write coefficient access to matrices and arrays.
   * \ingroup Core_Module
   * \tparam Derived Type of the derived class
-  * \tparam #DirectWriteAccessors Constant indicating direct access
+  *
+  * \note #DirectWriteAccessors Constant indicating direct access
   *
   * This class defines functions to work with strides which can be used to access entries directly. This class
   * inherits DenseCoeffsBase<Derived, WriteAccessors> which defines functions to access entries read/write using
@@ -566,8 +570,8 @@
       *
       * \sa outerStride(), rowStride(), colStride()
       */
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT
     {
       return derived().innerStride();
     }
@@ -577,14 +581,14 @@
       *
       * \sa innerStride(), rowStride(), colStride()
       */
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT
     {
       return derived().outerStride();
     }
 
     // FIXME shall we remove it ?
-    inline Index stride() const
+    EIGEN_CONSTEXPR inline Index stride() const EIGEN_NOEXCEPT
     {
       return Derived::IsVectorAtCompileTime ? innerStride() : outerStride();
     }
@@ -593,8 +597,8 @@
       *
       * \sa innerStride(), outerStride(), colStride()
       */
-    EIGEN_DEVICE_FUNC
-    inline Index rowStride() const
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rowStride() const EIGEN_NOEXCEPT
     {
       return Derived::IsRowMajor ? outerStride() : innerStride();
     }
@@ -603,8 +607,8 @@
       *
       * \sa innerStride(), outerStride(), rowStride()
       */
-    EIGEN_DEVICE_FUNC
-    inline Index colStride() const
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index colStride() const EIGEN_NOEXCEPT
     {
       return Derived::IsRowMajor ? innerStride() : outerStride();
     }
@@ -615,7 +619,7 @@
 template<int Alignment, typename Derived, bool JustReturnZero>
 struct first_aligned_impl
 {
-  static inline Index run(const Derived&)
+  static EIGEN_CONSTEXPR inline Index run(const Derived&) EIGEN_NOEXCEPT
   { return 0; }
 };
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
index 7958fee..08ef6c5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DenseStorage.h
@@ -47,21 +47,21 @@
 
   EIGEN_DEVICE_FUNC
   plain_array()
-  { 
+  {
     check_static_allocation_size<T,Size>();
   }
 
   EIGEN_DEVICE_FUNC
   plain_array(constructor_without_unaligned_array_assert)
-  { 
+  {
     check_static_allocation_size<T,Size>();
   }
 };
 
 #if defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
   #define EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(sizemask)
-#elif EIGEN_GNUC_AT_LEAST(4,7) 
-  // GCC 4.7 is too aggressive in its optimizations and remove the alignement test based on the fact the array is declared to be aligned.
+#elif EIGEN_GNUC_AT_LEAST(4,7)
+  // GCC 4.7 is too aggressive in its optimizations and remove the alignment test based on the fact the array is declared to be aligned.
   // See this bug report: http://gcc.gnu.org/bugzilla/show_bug.cgi?id=53900
   // Hiding the origin of the array pointer behind a function argument seems to do the trick even if the function is inlined:
   template<typename PtrType>
@@ -85,15 +85,15 @@
   EIGEN_ALIGN_TO_BOUNDARY(8) T array[Size];
 
   EIGEN_DEVICE_FUNC
-  plain_array() 
+  plain_array()
   {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(7);
     check_static_allocation_size<T,Size>();
   }
 
   EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert) 
-  { 
+  plain_array(constructor_without_unaligned_array_assert)
+  {
     check_static_allocation_size<T,Size>();
   }
 };
@@ -104,15 +104,15 @@
   EIGEN_ALIGN_TO_BOUNDARY(16) T array[Size];
 
   EIGEN_DEVICE_FUNC
-  plain_array() 
-  { 
+  plain_array()
+  {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(15);
     check_static_allocation_size<T,Size>();
   }
 
   EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert) 
-  { 
+  plain_array(constructor_without_unaligned_array_assert)
+  {
     check_static_allocation_size<T,Size>();
   }
 };
@@ -123,15 +123,15 @@
   EIGEN_ALIGN_TO_BOUNDARY(32) T array[Size];
 
   EIGEN_DEVICE_FUNC
-  plain_array() 
+  plain_array()
   {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(31);
     check_static_allocation_size<T,Size>();
   }
 
   EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert) 
-  { 
+  plain_array(constructor_without_unaligned_array_assert)
+  {
     check_static_allocation_size<T,Size>();
   }
 };
@@ -142,15 +142,15 @@
   EIGEN_ALIGN_TO_BOUNDARY(64) T array[Size];
 
   EIGEN_DEVICE_FUNC
-  plain_array() 
-  { 
+  plain_array()
+  {
     EIGEN_MAKE_UNALIGNED_ARRAY_ASSERT(63);
     check_static_allocation_size<T,Size>();
   }
 
   EIGEN_DEVICE_FUNC
-  plain_array(constructor_without_unaligned_array_assert) 
-  { 
+  plain_array(constructor_without_unaligned_array_assert)
+  {
     check_static_allocation_size<T,Size>();
   }
 };
@@ -163,6 +163,30 @@
   EIGEN_DEVICE_FUNC plain_array(constructor_without_unaligned_array_assert) {}
 };
 
+struct plain_array_helper {
+  template<typename T, int Size, int MatrixOrArrayOptions, int Alignment>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  static void copy(const plain_array<T, Size, MatrixOrArrayOptions, Alignment>& src, const Eigen::Index size,
+                         plain_array<T, Size, MatrixOrArrayOptions, Alignment>& dst) {
+    smart_copy(src.array, src.array + size, dst.array);
+  }
+  
+  template<typename T, int Size, int MatrixOrArrayOptions, int Alignment>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  static void swap(plain_array<T, Size, MatrixOrArrayOptions, Alignment>& a, const Eigen::Index a_size,
+                   plain_array<T, Size, MatrixOrArrayOptions, Alignment>& b, const Eigen::Index b_size) {
+    if (a_size < b_size) {
+      std::swap_ranges(b.array, b.array + a_size, a.array);
+      smart_move(b.array + a_size, b.array + b_size, a.array + a_size);
+    } else if (a_size > b_size) {
+      std::swap_ranges(a.array, a.array + b_size, b.array);
+      smart_move(a.array + b_size, a.array + a_size, b.array + b_size);
+    } else {
+      std::swap_ranges(a.array, a.array + a_size, b.array);
+    }
+  }
+};
+
 } // end namespace internal
 
 /** \internal
@@ -190,16 +214,41 @@
     EIGEN_DEVICE_FUNC
     explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()) {}
-    EIGEN_DEVICE_FUNC 
+#if !EIGEN_HAS_CXX11 || defined(EIGEN_DENSE_STORAGE_CTOR_PLUGIN)
+    EIGEN_DEVICE_FUNC
     DenseStorage(const DenseStorage& other) : m_data(other.m_data) {
       EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN(Index size = Size)
     }
-    EIGEN_DEVICE_FUNC 
+#else
+    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage&) = default;
+#endif
+#if !EIGEN_HAS_CXX11
+    EIGEN_DEVICE_FUNC
     DenseStorage& operator=(const DenseStorage& other)
-    { 
+    {
       if (this != &other) m_data = other.m_data;
-      return *this; 
+      return *this;
     }
+#else
+    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage&) = default;
+#endif
+#if EIGEN_HAS_RVALUE_REFERENCES
+#if !EIGEN_HAS_CXX11
+    EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
+      : m_data(std::move(other.m_data))
+    {
+    }
+    EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
+    {
+      if (this != &other)
+        m_data = std::move(other.m_data);
+      return *this;
+    }
+#else
+    EIGEN_DEVICE_FUNC DenseStorage(DenseStorage&&) = default;
+    EIGEN_DEVICE_FUNC DenseStorage& operator=(DenseStorage&&) = default;
+#endif
+#endif
     EIGEN_DEVICE_FUNC DenseStorage(Index size, Index rows, Index cols) {
       EIGEN_INTERNAL_DENSE_STORAGE_CTOR_PLUGIN({})
       eigen_internal_assert(size==rows*cols && rows==_Rows && cols==_Cols);
@@ -207,9 +256,11 @@
       EIGEN_UNUSED_VARIABLE(rows);
       EIGEN_UNUSED_VARIABLE(cols);
     }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); }
-    EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;}
-    EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;}
+    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+      numext::swap(m_data, other.m_data);
+    }
+    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT {return _Rows;}
+    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) EIGEN_NOEXCEPT {return _Cols;}
     EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {}
     EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {}
     EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
@@ -226,8 +277,8 @@
     EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage&) { return *this; }
     EIGEN_DEVICE_FUNC DenseStorage(Index,Index,Index) {}
     EIGEN_DEVICE_FUNC void swap(DenseStorage& ) {}
-    EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;}
-    EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;}
+    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT {return _Rows;}
+    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) EIGEN_NOEXCEPT {return _Cols;}
     EIGEN_DEVICE_FUNC void conservativeResize(Index,Index,Index) {}
     EIGEN_DEVICE_FUNC void resize(Index,Index,Index) {}
     EIGEN_DEVICE_FUNC const T *data() const { return 0; }
@@ -254,20 +305,28 @@
     EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0), m_cols(0) {}
     EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0), m_cols(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows), m_cols(other.m_cols) {}
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) 
-    { 
+    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
+      : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows), m_cols(other.m_cols)
+    {
+      internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data);
+    }
+    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
+    {
       if (this != &other)
       {
-        m_data = other.m_data;
         m_rows = other.m_rows;
         m_cols = other.m_cols;
+        internal::plain_array_helper::copy(other.m_data, m_rows * m_cols, m_data);
       }
-      return *this; 
+      return *this;
     }
     EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index cols) : m_rows(rows), m_cols(cols) {}
     EIGEN_DEVICE_FUNC void swap(DenseStorage& other)
-    { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
+    {
+      internal::plain_array_helper::swap(m_data, m_rows * m_cols, other.m_data, other.m_rows * other.m_cols);
+      numext::swap(m_rows,other.m_rows);
+      numext::swap(m_cols,other.m_cols);
+    }
     EIGEN_DEVICE_FUNC Index rows() const {return m_rows;}
     EIGEN_DEVICE_FUNC Index cols() const {return m_cols;}
     EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index cols) { m_rows = rows; m_cols = cols; }
@@ -285,20 +344,29 @@
     EIGEN_DEVICE_FUNC DenseStorage() : m_rows(0) {}
     EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_rows(other.m_rows) {}
-    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other) 
+    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other)
+      : m_data(internal::constructor_without_unaligned_array_assert()), m_rows(other.m_rows)
+    {
+      internal::plain_array_helper::copy(other.m_data, m_rows * _Cols, m_data);
+    }
+    
+    EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
     {
       if (this != &other)
       {
-        m_data = other.m_data;
         m_rows = other.m_rows;
+        internal::plain_array_helper::copy(other.m_data, m_rows * _Cols, m_data);
       }
-      return *this; 
+      return *this;
     }
     EIGEN_DEVICE_FUNC DenseStorage(Index, Index rows, Index) : m_rows(rows) {}
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
-    EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const {return _Cols;}
+    EIGEN_DEVICE_FUNC void swap(DenseStorage& other)
+    { 
+      internal::plain_array_helper::swap(m_data, m_rows * _Cols, other.m_data, other.m_rows * _Cols);
+      numext::swap(m_rows, other.m_rows);
+    }
+    EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT {return m_rows;}
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols(void) const EIGEN_NOEXCEPT {return _Cols;}
     EIGEN_DEVICE_FUNC void conservativeResize(Index, Index rows, Index) { m_rows = rows; }
     EIGEN_DEVICE_FUNC void resize(Index, Index rows, Index) { m_rows = rows; }
     EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
@@ -314,22 +382,29 @@
     EIGEN_DEVICE_FUNC DenseStorage() : m_cols(0) {}
     EIGEN_DEVICE_FUNC explicit DenseStorage(internal::constructor_without_unaligned_array_assert)
       : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(0) {}
-    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) : m_data(other.m_data), m_cols(other.m_cols) {}
+    EIGEN_DEVICE_FUNC DenseStorage(const DenseStorage& other) 
+      : m_data(internal::constructor_without_unaligned_array_assert()), m_cols(other.m_cols)
+    {
+      internal::plain_array_helper::copy(other.m_data, _Rows * m_cols, m_data);
+    }
     EIGEN_DEVICE_FUNC DenseStorage& operator=(const DenseStorage& other)
     {
       if (this != &other)
       {
-        m_data = other.m_data;
         m_cols = other.m_cols;
+        internal::plain_array_helper::copy(other.m_data, _Rows * m_cols, m_data);
       }
       return *this;
     }
     EIGEN_DEVICE_FUNC DenseStorage(Index, Index, Index cols) : m_cols(cols) {}
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
-    EIGEN_DEVICE_FUNC Index rows(void) const {return _Rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const {return m_cols;}
-    void conservativeResize(Index, Index, Index cols) { m_cols = cols; }
-    void resize(Index, Index, Index cols) { m_cols = cols; }
+    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+      internal::plain_array_helper::swap(m_data, _Rows * m_cols, other.m_data, _Rows * other.m_cols);
+      numext::swap(m_cols, other.m_cols);
+    }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows(void) const EIGEN_NOEXCEPT {return _Rows;}
+    EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT {return m_cols;}
+    EIGEN_DEVICE_FUNC void conservativeResize(Index, Index, Index cols) { m_cols = cols; }
+    EIGEN_DEVICE_FUNC void resize(Index, Index, Index cols) { m_cols = cols; }
     EIGEN_DEVICE_FUNC const T *data() const { return m_data.array; }
     EIGEN_DEVICE_FUNC T *data() { return m_data.array; }
 };
@@ -381,18 +456,21 @@
     EIGEN_DEVICE_FUNC
     DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
     {
-      using std::swap;
-      swap(m_data, other.m_data);
-      swap(m_rows, other.m_rows);
-      swap(m_cols, other.m_cols);
+      numext::swap(m_data, other.m_data);
+      numext::swap(m_rows, other.m_rows);
+      numext::swap(m_cols, other.m_cols);
       return *this;
     }
 #endif
     EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, m_rows*m_cols); }
     EIGEN_DEVICE_FUNC void swap(DenseStorage& other)
-    { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); std::swap(m_cols,other.m_cols); }
-    EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const {return m_cols;}
+    {
+      numext::swap(m_data,other.m_data);
+      numext::swap(m_rows,other.m_rows);
+      numext::swap(m_cols,other.m_cols);
+    }
+    EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT {return m_rows;}
+    EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT {return m_cols;}
     void conservativeResize(Index size, Index rows, Index cols)
     {
       m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, m_rows*m_cols);
@@ -404,7 +482,7 @@
       if(size != m_rows*m_cols)
       {
         internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, m_rows*m_cols);
-        if (size)
+        if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative
           m_data = internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size);
         else
           m_data = 0;
@@ -446,7 +524,7 @@
         this->swap(tmp);
       }
       return *this;
-    }    
+    }
 #if EIGEN_HAS_RVALUE_REFERENCES
     EIGEN_DEVICE_FUNC
     DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
@@ -459,16 +537,18 @@
     EIGEN_DEVICE_FUNC
     DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
     {
-      using std::swap;
-      swap(m_data, other.m_data);
-      swap(m_cols, other.m_cols);
+      numext::swap(m_data, other.m_data);
+      numext::swap(m_cols, other.m_cols);
       return *this;
     }
 #endif
     EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Rows*m_cols); }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_cols,other.m_cols); }
-    EIGEN_DEVICE_FUNC static Index rows(void) {return _Rows;}
-    EIGEN_DEVICE_FUNC Index cols(void) const {return m_cols;}
+    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+      numext::swap(m_data,other.m_data);
+      numext::swap(m_cols,other.m_cols);
+    }
+    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index rows(void) EIGEN_NOEXCEPT {return _Rows;}
+    EIGEN_DEVICE_FUNC Index cols(void) const EIGEN_NOEXCEPT {return m_cols;}
     EIGEN_DEVICE_FUNC void conservativeResize(Index size, Index, Index cols)
     {
       m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, _Rows*m_cols);
@@ -479,7 +559,7 @@
       if(size != _Rows*m_cols)
       {
         internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Rows*m_cols);
-        if (size)
+        if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative
           m_data = internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size);
         else
           m_data = 0;
@@ -520,7 +600,7 @@
         this->swap(tmp);
       }
       return *this;
-    }    
+    }
 #if EIGEN_HAS_RVALUE_REFERENCES
     EIGEN_DEVICE_FUNC
     DenseStorage(DenseStorage&& other) EIGEN_NOEXCEPT
@@ -533,16 +613,18 @@
     EIGEN_DEVICE_FUNC
     DenseStorage& operator=(DenseStorage&& other) EIGEN_NOEXCEPT
     {
-      using std::swap;
-      swap(m_data, other.m_data);
-      swap(m_rows, other.m_rows);
+      numext::swap(m_data, other.m_data);
+      numext::swap(m_rows, other.m_rows);
       return *this;
     }
 #endif
     EIGEN_DEVICE_FUNC ~DenseStorage() { internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Cols*m_rows); }
-    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) { std::swap(m_data,other.m_data); std::swap(m_rows,other.m_rows); }
-    EIGEN_DEVICE_FUNC Index rows(void) const {return m_rows;}
-    EIGEN_DEVICE_FUNC static Index cols(void) {return _Cols;}
+    EIGEN_DEVICE_FUNC void swap(DenseStorage& other) {
+      numext::swap(m_data,other.m_data);
+      numext::swap(m_rows,other.m_rows);
+    }
+    EIGEN_DEVICE_FUNC Index rows(void) const EIGEN_NOEXCEPT {return m_rows;}
+    EIGEN_DEVICE_FUNC static EIGEN_CONSTEXPR Index cols(void) {return _Cols;}
     void conservativeResize(Index size, Index rows, Index)
     {
       m_data = internal::conditional_aligned_realloc_new_auto<T,(_Options&DontAlign)==0>(m_data, size, m_rows*_Cols);
@@ -553,7 +635,7 @@
       if(size != m_rows*_Cols)
       {
         internal::conditional_aligned_delete_auto<T,(_Options&DontAlign)==0>(m_data, _Cols*m_rows);
-        if (size)
+        if (size>0) // >0 and not simply !=0 to let the compiler knows that size cannot be negative
           m_data = internal::conditional_aligned_new_auto<T,(_Options&DontAlign)==0>(size);
         else
           m_data = 0;
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
index afcaf35..3112d2c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Diagonal.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_DIAGONAL_H
 #define EIGEN_DIAGONAL_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \class Diagonal
   * \ingroup Core_Module
@@ -84,20 +84,16 @@
                                : numext::mini<Index>(m_matrix.rows(),m_matrix.cols()-m_index.value());
     }
 
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return 1; }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return 1; }
 
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const
-    {
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT {
       return m_matrix.outerStride() + 1;
     }
 
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const
-    {
-      return 0;
-    }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return 0; }
 
     typedef typename internal::conditional<
                        internal::is_lvalue<MatrixType>::value,
@@ -149,8 +145,8 @@
     }
 
     EIGEN_DEVICE_FUNC
-    inline const typename internal::remove_all<typename MatrixType::Nested>::type& 
-    nestedExpression() const 
+    inline const typename internal::remove_all<typename MatrixType::Nested>::type&
+    nestedExpression() const
     {
       return m_matrix;
     }
@@ -167,12 +163,12 @@
 
   private:
     // some compilers may fail to optimize std::max etc in case of compile-time constants...
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index absDiagIndex() const { return m_index.value()>0 ? m_index.value() : -m_index.value(); }
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index rowOffset() const { return m_index.value()>0 ? 0 : -m_index.value(); }
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index colOffset() const { return m_index.value()>0 ? m_index.value() : 0; }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index absDiagIndex() const EIGEN_NOEXCEPT { return m_index.value()>0 ? m_index.value() : -m_index.value(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rowOffset() const EIGEN_NOEXCEPT { return m_index.value()>0 ? 0 : -m_index.value(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index colOffset() const EIGEN_NOEXCEPT { return m_index.value()>0 ? m_index.value() : 0; }
     // trigger a compile-time error if someone try to call packet
     template<int LoadMode> typename MatrixType::PacketReturnType packet(Index) const;
     template<int LoadMode> typename MatrixType::PacketReturnType packet(Index,Index) const;
@@ -187,7 +183,7 @@
   *
   * \sa class Diagonal */
 template<typename Derived>
-inline typename MatrixBase<Derived>::DiagonalReturnType
+EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::DiagonalReturnType
 MatrixBase<Derived>::diagonal()
 {
   return DiagonalReturnType(derived());
@@ -195,7 +191,7 @@
 
 /** This is the const version of diagonal(). */
 template<typename Derived>
-inline typename MatrixBase<Derived>::ConstDiagonalReturnType
+EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::ConstDiagonalReturnType
 MatrixBase<Derived>::diagonal() const
 {
   return ConstDiagonalReturnType(derived());
@@ -213,7 +209,7 @@
   *
   * \sa MatrixBase::diagonal(), class Diagonal */
 template<typename Derived>
-inline typename MatrixBase<Derived>::DiagonalDynamicIndexReturnType
+EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::DiagonalDynamicIndexReturnType
 MatrixBase<Derived>::diagonal(Index index)
 {
   return DiagonalDynamicIndexReturnType(derived(), index);
@@ -221,7 +217,7 @@
 
 /** This is the const version of diagonal(Index). */
 template<typename Derived>
-inline typename MatrixBase<Derived>::ConstDiagonalDynamicIndexReturnType
+EIGEN_DEVICE_FUNC inline typename MatrixBase<Derived>::ConstDiagonalDynamicIndexReturnType
 MatrixBase<Derived>::diagonal(Index index) const
 {
   return ConstDiagonalDynamicIndexReturnType(derived(), index);
@@ -240,6 +236,7 @@
   * \sa MatrixBase::diagonal(), class Diagonal */
 template<typename Derived>
 template<int Index_>
+EIGEN_DEVICE_FUNC
 inline typename MatrixBase<Derived>::template DiagonalIndexReturnType<Index_>::Type
 MatrixBase<Derived>::diagonal()
 {
@@ -249,6 +246,7 @@
 /** This is the const version of diagonal<int>(). */
 template<typename Derived>
 template<int Index_>
+EIGEN_DEVICE_FUNC
 inline typename MatrixBase<Derived>::template ConstDiagonalIndexReturnType<Index_>::Type
 MatrixBase<Derived>::diagonal() const
 {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
index ecfdce8..542685c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalMatrix.h
@@ -44,7 +44,7 @@
 
     EIGEN_DEVICE_FUNC
     DenseMatrixType toDenseMatrix() const { return derived(); }
-    
+
     EIGEN_DEVICE_FUNC
     inline const DiagonalVectorType& diagonal() const { return derived().diagonal(); }
     EIGEN_DEVICE_FUNC
@@ -83,6 +83,30 @@
     {
       return DiagonalWrapper<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,DiagonalVectorType,product) >(scalar * other.diagonal());
     }
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    inline unspecified_expression_type
+    #else
+    inline const DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(DiagonalVectorType,typename OtherDerived::DiagonalVectorType,sum) >
+    #endif
+    operator+(const DiagonalBase<OtherDerived>& other) const
+    {
+      return (diagonal() + other.diagonal()).asDiagonal();
+    }
+
+    template<typename OtherDerived>
+    EIGEN_DEVICE_FUNC
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    inline unspecified_expression_type
+    #else
+    inline const DiagonalWrapper<const EIGEN_CWISE_BINARY_RETURN_TYPE(DiagonalVectorType,typename OtherDerived::DiagonalVectorType,difference) >
+    #endif
+    operator-(const DiagonalBase<OtherDerived>& other) const
+    {
+      return (diagonal() - other.diagonal()).asDiagonal();
+    }
 };
 
 #endif
@@ -154,6 +178,30 @@
     EIGEN_DEVICE_FUNC
     inline DiagonalMatrix(const Scalar& x, const Scalar& y, const Scalar& z) : m_diagonal(x,y,z) {}
 
+    #if EIGEN_HAS_CXX11
+    /** \brief Construct a diagonal matrix with fixed size from an arbitrary number of coefficients. \cpp11
+      * 
+      * There exists C++98 anologue constructors for fixed-size diagonal matrices having 2 or 3 coefficients.
+      * 
+      * \warning To construct a diagonal matrix of fixed size, the number of values passed to this 
+      * constructor must match the fixed dimension of \c *this.
+      * 
+      * \sa DiagonalMatrix(const Scalar&, const Scalar&)
+      * \sa DiagonalMatrix(const Scalar&, const Scalar&, const Scalar&)
+      */
+    template <typename... ArgTypes>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    DiagonalMatrix(const Scalar& a0, const Scalar& a1, const Scalar& a2, const ArgTypes&... args)
+      : m_diagonal(a0, a1, a2, args...) {}
+
+    /** \brief Constructs a DiagonalMatrix and initializes it by elements given by an initializer list of initializer
+      * lists \cpp11
+      */
+    EIGEN_DEVICE_FUNC
+    explicit EIGEN_STRONG_INLINE DiagonalMatrix(const std::initializer_list<std::initializer_list<Scalar>>& list)
+      : m_diagonal(list) {}
+    #endif  // EIGEN_HAS_CXX11
+
     /** Copy constructor. */
     template<typename OtherDerived>
     EIGEN_DEVICE_FUNC
@@ -273,7 +321,7 @@
   * \sa class DiagonalWrapper, class DiagonalMatrix, diagonal(), isDiagonal()
   **/
 template<typename Derived>
-inline const DiagonalWrapper<const Derived>
+EIGEN_DEVICE_FUNC inline const DiagonalWrapper<const Derived>
 MatrixBase<Derived>::asDiagonal() const
 {
   return DiagonalWrapper<const Derived>(derived());
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
index d372b93..7911d1c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/DiagonalProduct.h
@@ -17,7 +17,7 @@
   */
 template<typename Derived>
 template<typename DiagonalDerived>
-inline const Product<Derived, DiagonalDerived, LazyProduct>
+EIGEN_DEVICE_FUNC inline const Product<Derived, DiagonalDerived, LazyProduct>
 MatrixBase<Derived>::operator*(const DiagonalBase<DiagonalDerived> &a_diagonal) const
 {
   return Product<Derived, DiagonalDerived, LazyProduct>(derived(),a_diagonal.derived());
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
index 1fe7a84..5c3441b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Dot.h
@@ -86,14 +86,14 @@
 
 //---------- implementation of L2 norm and related functions ----------
 
-/** \returns, for vectors, the squared \em l2 norm of \c *this, and for matrices the Frobenius norm.
+/** \returns, for vectors, the squared \em l2 norm of \c *this, and for matrices the squared Frobenius norm.
   * In both cases, it consists in the sum of the square of all the matrix entries.
   * For vectors, this is also equals to the dot product of \c *this with itself.
   *
   * \sa dot(), norm(), lpNorm()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::squaredNorm() const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::squaredNorm() const
 {
   return numext::real((*this).cwiseAbs2().sum());
 }
@@ -105,7 +105,7 @@
   * \sa lpNorm(), dot(), squaredNorm()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename NumTraits<typename internal::traits<Derived>::Scalar>::Real MatrixBase<Derived>::norm() const
 {
   return numext::sqrt(squaredNorm());
 }
@@ -120,7 +120,7 @@
   * \sa norm(), normalize()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
 MatrixBase<Derived>::normalized() const
 {
   typedef typename internal::nested_eval<Derived,2>::type _Nested;
@@ -142,7 +142,7 @@
   * \sa norm(), normalized()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE void MatrixBase<Derived>::normalize()
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase<Derived>::normalize()
 {
   RealScalar z = squaredNorm();
   // NOTE: after extensive benchmarking, this conditional does not impact performance, at least on recent x86 CPU
@@ -163,7 +163,7 @@
   * \sa stableNorm(), stableNormalize(), normalized()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const typename MatrixBase<Derived>::PlainObject
 MatrixBase<Derived>::stableNormalized() const
 {
   typedef typename internal::nested_eval<Derived,3>::type _Nested;
@@ -188,7 +188,7 @@
   * \sa stableNorm(), stableNormalized(), normalize()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE void MatrixBase<Derived>::stableNormalize()
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void MatrixBase<Derived>::stableNormalize()
 {
   RealScalar w = cwiseAbs().maxCoeff();
   RealScalar z = (derived()/w).squaredNorm();
@@ -207,7 +207,7 @@
   EIGEN_DEVICE_FUNC
   static inline RealScalar run(const MatrixBase<Derived>& m)
   {
-    EIGEN_USING_STD_MATH(pow)
+    EIGEN_USING_STD(pow)
     return pow(m.cwiseAbs().array().pow(p).sum(), RealScalar(1)/p);
   }
 };
@@ -260,9 +260,9 @@
 template<typename Derived>
 template<int p>
 #ifndef EIGEN_PARSED_BY_DOXYGEN
-inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
+EIGEN_DEVICE_FUNC inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
 #else
-MatrixBase<Derived>::RealScalar
+EIGEN_DEVICE_FUNC MatrixBase<Derived>::RealScalar
 #endif
 MatrixBase<Derived>::lpNorm() const
 {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
index b195506..6b3c7d3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/EigenBase.h
@@ -15,7 +15,7 @@
 
 /** \class EigenBase
   * \ingroup Core_Module
-  * 
+  *
   * Common base class for all classes T such that MatrixBase has an operator=(T) and a constructor MatrixBase(T).
   *
   * In other words, an EigenBase object is an object that can be copied into a MatrixBase.
@@ -29,11 +29,12 @@
 template<typename Derived> struct EigenBase
 {
 //   typedef typename internal::plain_matrix_type<Derived>::type PlainObject;
-  
+
   /** \brief The interface type of indices
     * \details To change this, \c \#define the preprocessor symbol \c EIGEN_DEFAULT_DENSE_INDEX_TYPE.
-    * \deprecated Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead.
     * \sa StorageIndex, \ref TopicPreprocessorDirectives.
+    * DEPRECATED: Since Eigen 3.3, its usage is deprecated. Use Eigen::Index instead.
+    * Deprecation is not marked with a doxygen comment because there are too many existing usages to add the deprecation attribute.
     */
   typedef Eigen::Index Index;
 
@@ -55,15 +56,15 @@
   { return *static_cast<const Derived*>(this); }
 
   /** \returns the number of rows. \sa cols(), RowsAtCompileTime */
-  EIGEN_DEVICE_FUNC
-  inline Index rows() const { return derived().rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); }
   /** \returns the number of columns. \sa rows(), ColsAtCompileTime*/
-  EIGEN_DEVICE_FUNC
-  inline Index cols() const { return derived().cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); }
   /** \returns the number of coefficients, which is rows()*cols().
     * \sa rows(), cols(), SizeAtCompileTime. */
-  EIGEN_DEVICE_FUNC
-  inline Index size() const { return rows() * cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  inline Index size() const EIGEN_NOEXCEPT { return rows() * cols(); }
 
   /** \internal Don't use it, but do the equivalent: \code dst = *this; \endcode */
   template<typename Dest>
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
index 7b08b45..817a43a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ForceAlignedAccess.h
@@ -41,10 +41,14 @@
 
     EIGEN_DEVICE_FUNC explicit inline ForceAlignedAccess(const ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return m_expression.outerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT { return m_expression.innerStride(); }
 
     EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const
     {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
index 3e403a0..43aa49b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Fuzzy.h
@@ -100,7 +100,7 @@
   */
 template<typename Derived>
 template<typename OtherDerived>
-bool DenseBase<Derived>::isApprox(
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isApprox(
   const DenseBase<OtherDerived>& other,
   const RealScalar& prec
 ) const
@@ -122,7 +122,7 @@
   * \sa isApprox(), isMuchSmallerThan(const DenseBase<OtherDerived>&, RealScalar) const
   */
 template<typename Derived>
-bool DenseBase<Derived>::isMuchSmallerThan(
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isMuchSmallerThan(
   const typename NumTraits<Scalar>::Real& other,
   const RealScalar& prec
 ) const
@@ -142,7 +142,7 @@
   */
 template<typename Derived>
 template<typename OtherDerived>
-bool DenseBase<Derived>::isMuchSmallerThan(
+EIGEN_DEVICE_FUNC bool DenseBase<Derived>::isMuchSmallerThan(
   const DenseBase<OtherDerived>& other,
   const RealScalar& prec
 ) const
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
index 6f0cc80..6906aa7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GeneralProduct.h
@@ -18,6 +18,16 @@
   Small = 3
 };
 
+// Define the threshold value to fallback from the generic matrix-matrix product
+// implementation (heavy) to the lightweight coeff-based product one.
+// See generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,GemmProduct>
+// in products/GeneralMatrixMatrix.h for more details.
+// TODO This threshold should also be used in the compile-time selector below.
+#ifndef EIGEN_GEMM_TO_COEFFBASED_THRESHOLD
+// This default value has been obtained on a Haswell architecture.
+#define EIGEN_GEMM_TO_COEFFBASED_THRESHOLD 20
+#endif
+
 namespace internal {
 
 template<int Rows, int Cols, int Depth> struct product_type_selector;
@@ -25,7 +35,7 @@
 template<int Size, int MaxSize> struct product_size_category
 {
   enum {
-    #ifndef EIGEN_CUDA_ARCH
+    #ifndef EIGEN_GPU_COMPILE_PHASE
     is_large = MaxSize == Dynamic ||
                Size >= EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD ||
                (Size==Dynamic && MaxSize>=EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD),
@@ -153,13 +163,13 @@
 template<typename Scalar,int Size,int MaxSize>
 struct gemv_static_vector_if<Scalar,Size,MaxSize,false>
 {
-  EIGEN_STRONG_INLINE  Scalar* data() { eigen_internal_assert(false && "should never be called"); return 0; }
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() { eigen_internal_assert(false && "should never be called"); return 0; }
 };
 
 template<typename Scalar,int Size>
 struct gemv_static_vector_if<Scalar,Size,Dynamic,true>
 {
-  EIGEN_STRONG_INLINE Scalar* data() { return 0; }
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Scalar* data() { return 0; }
 };
 
 template<typename Scalar,int Size,int MaxSize>
@@ -218,8 +228,7 @@
     ActualLhsType actualLhs = LhsBlasTraits::extract(lhs);
     ActualRhsType actualRhs = RhsBlasTraits::extract(rhs);
 
-    ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs)
-                                  * RhsBlasTraits::extractScalarFactor(rhs);
+    ResScalar actualAlpha = combine_scalar_factors(alpha, lhs, rhs);
 
     // make sure Dest is a compile-time vector type (bug 1166)
     typedef typename conditional<Dest::IsVectorAtCompileTime, Dest, typename Dest::ColXpr>::type ActualDest;
@@ -229,7 +238,7 @@
       // on, the other hand it is good for the cache to pack the vector anyways...
       EvalToDestAtCompileTime = (ActualDest::InnerStrideAtCompileTime==1),
       ComplexByReal = (NumTraits<LhsScalar>::IsComplex) && (!NumTraits<RhsScalar>::IsComplex),
-      MightCannotUseDest = (!EvalToDestAtCompileTime) || ComplexByReal
+      MightCannotUseDest = ((!EvalToDestAtCompileTime) || ComplexByReal) && (ActualDest::MaxSizeAtCompileTime!=0)
     };
 
     typedef const_blas_data_mapper<LhsScalar,Index,ColMajor> LhsMapper;
@@ -310,13 +319,12 @@
     typename add_const<ActualLhsType>::type actualLhs = LhsBlasTraits::extract(lhs);
     typename add_const<ActualRhsType>::type actualRhs = RhsBlasTraits::extract(rhs);
 
-    ResScalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(lhs)
-                                  * RhsBlasTraits::extractScalarFactor(rhs);
+    ResScalar actualAlpha = combine_scalar_factors(alpha, lhs, rhs);
 
     enum {
       // FIXME find a way to allow an inner stride on the result if packet_traits<Scalar>::size==1
       // on, the other hand it is good for the cache to pack the vector anyways...
-      DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime==1
+      DirectlyUseRhs = ActualRhsTypeCleaned::InnerStrideAtCompileTime==1 || ActualRhsTypeCleaned::MaxSizeAtCompileTime==0
     };
 
     gemv_static_vector_if<RhsScalar,ActualRhsTypeCleaned::SizeAtCompileTime,ActualRhsTypeCleaned::MaxSizeAtCompileTime,!DirectlyUseRhs> static_rhs;
@@ -386,7 +394,8 @@
   */
 template<typename Derived>
 template<typename OtherDerived>
-inline const Product<Derived, OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const Product<Derived, OtherDerived>
 MatrixBase<Derived>::operator*(const MatrixBase<OtherDerived> &other) const
 {
   // A note regarding the function declaration: In MSVC, this function will sometimes
@@ -428,6 +437,7 @@
   */
 template<typename Derived>
 template<typename OtherDerived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
 const Product<Derived,OtherDerived,LazyProduct>
 MatrixBase<Derived>::lazyProduct(const MatrixBase<OtherDerived> &other) const
 {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
index 029f8ac..cf677a1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GenericPacketMath.h
@@ -44,23 +44,29 @@
   enum {
     HasHalfPacket = 0,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasNegate = 1,
-    HasAbs    = 1,
-    HasArg    = 0,
-    HasAbs2   = 1,
-    HasMin    = 1,
-    HasMax    = 1,
-    HasConj   = 1,
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 0,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
     HasSetLinear = 1,
-    HasBlend  = 0,
+    HasBlend     = 0,
+    // This flag is used to indicate whether packet comparison is supported.
+    // pcmp_eq, pcmp_lt and pcmp_le should be defined for it to be true.
+    HasCmp       = 0,
 
     HasDiv    = 0,
     HasSqrt   = 0,
     HasRsqrt  = 0,
     HasExp    = 0,
+    HasExpm1  = 0,
     HasLog    = 0,
     HasLog1p  = 0,
     HasLog10  = 0,
@@ -81,14 +87,18 @@
     HasPolygamma = 0,
     HasErf = 0,
     HasErfc = 0,
+    HasNdtri = 0,
+    HasBessel = 0,
     HasIGamma = 0,
+    HasIGammaDerA = 0,
+    HasGammaSampleDerAlpha = 0,
     HasIGammac = 0,
     HasBetaInc = 0,
 
     HasRound  = 0,
+    HasRint   = 0,
     HasFloor  = 0,
     HasCeil   = 0,
-
     HasSign   = 0
   };
 };
@@ -119,6 +129,22 @@
 
 template<typename T> struct packet_traits<const T> : packet_traits<T> { };
 
+template<typename T> struct unpacket_traits
+{
+  typedef T type;
+  typedef T half;
+  enum
+  {
+    size = 1,
+    alignment = 1,
+    vectorizable = false,
+    masked_load_available=false,
+    masked_store_available=false
+  };
+};
+
+template<typename T> struct unpacket_traits<const T> : unpacket_traits<T> { };
+
 template <typename Src, typename Tgt> struct type_casting_traits {
   enum {
     VectorizedCast = 0,
@@ -127,6 +153,34 @@
   };
 };
 
+/** \internal Wrapper to ensure that multiple packet types can map to the same
+    same underlying vector type. */
+template<typename T, int unique_id = 0>
+struct eigen_packet_wrapper
+{
+  EIGEN_ALWAYS_INLINE operator T&() { return m_val; }
+  EIGEN_ALWAYS_INLINE operator const T&() const { return m_val; }
+  EIGEN_ALWAYS_INLINE eigen_packet_wrapper() {}
+  EIGEN_ALWAYS_INLINE eigen_packet_wrapper(const T &v) : m_val(v) {}
+  EIGEN_ALWAYS_INLINE eigen_packet_wrapper& operator=(const T &v) {
+    m_val = v;
+    return *this;
+  }
+
+  T m_val;
+};
+
+
+/** \internal A convenience utility for determining if the type is a scalar.
+ * This is used to enable some generic packet implementations.
+ */
+template<typename Packet>
+struct is_scalar {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  enum {
+    value = internal::is_same<Packet, Scalar>::value
+  };
+};
 
 /** \internal \returns static_cast<TgtType>(a) (coeff-wise) */
 template <typename SrcPacket, typename TgtPacket>
@@ -139,75 +193,406 @@
 pcast(const SrcPacket& a, const SrcPacket& /*b*/) {
   return static_cast<TgtPacket>(a);
 }
-
 template <typename SrcPacket, typename TgtPacket>
 EIGEN_DEVICE_FUNC inline TgtPacket
 pcast(const SrcPacket& a, const SrcPacket& /*b*/, const SrcPacket& /*c*/, const SrcPacket& /*d*/) {
   return static_cast<TgtPacket>(a);
 }
+template <typename SrcPacket, typename TgtPacket>
+EIGEN_DEVICE_FUNC inline TgtPacket
+pcast(const SrcPacket& a, const SrcPacket& /*b*/, const SrcPacket& /*c*/, const SrcPacket& /*d*/,
+      const SrcPacket& /*e*/, const SrcPacket& /*f*/, const SrcPacket& /*g*/, const SrcPacket& /*h*/) {
+  return static_cast<TgtPacket>(a);
+}
+
+/** \internal \returns reinterpret_cast<Target>(a) */
+template <typename Target, typename Packet>
+EIGEN_DEVICE_FUNC inline Target
+preinterpret(const Packet& a); /* { return reinterpret_cast<const Target&>(a); } */
 
 /** \internal \returns a + b (coeff-wise) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-padd(const Packet& a,
-        const Packet& b) { return a+b; }
+padd(const Packet& a, const Packet& b) { return a+b; }
+// Avoid compiler warning for boolean algebra.
+template<> EIGEN_DEVICE_FUNC inline bool
+padd(const bool& a, const bool& b) { return a || b; }
 
 /** \internal \returns a - b (coeff-wise) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-psub(const Packet& a,
-        const Packet& b) { return a-b; }
+psub(const Packet& a, const Packet& b) { return a-b; }
 
 /** \internal \returns -a (coeff-wise) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 pnegate(const Packet& a) { return -a; }
 
-/** \internal \returns conj(a) (coeff-wise) */
+template<> EIGEN_DEVICE_FUNC inline bool
+pnegate(const bool& a) { return !a; }
 
+/** \internal \returns conj(a) (coeff-wise) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 pconj(const Packet& a) { return numext::conj(a); }
 
 /** \internal \returns a * b (coeff-wise) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmul(const Packet& a,
-        const Packet& b) { return a*b; }
+pmul(const Packet& a, const Packet& b) { return a*b; }
+// Avoid compiler warning for boolean algebra.
+template<> EIGEN_DEVICE_FUNC inline bool
+pmul(const bool& a, const bool& b) { return a && b; }
 
 /** \internal \returns a / b (coeff-wise) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pdiv(const Packet& a,
-        const Packet& b) { return a/b; }
+pdiv(const Packet& a, const Packet& b) { return a/b; }
 
-/** \internal \returns the min of \a a and \a b  (coeff-wise) */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmin(const Packet& a,
-        const Packet& b) { return numext::mini(a, b); }
+// In the generic case, memset to all one bits.
+template<typename Packet, typename EnableIf = void>
+struct ptrue_impl {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/){
+    Packet b;
+    memset(static_cast<void*>(&b), 0xff, sizeof(Packet));
+    return b;
+  }
+};
 
-/** \internal \returns the max of \a a and \a b  (coeff-wise) */
+// For non-trivial scalars, set to Scalar(1) (i.e. a non-zero value).
+// Although this is technically not a valid bitmask, the scalar path for pselect
+// uses a comparison to zero, so this should still work in most cases. We don't
+// have another option, since the scalar type requires initialization.
+template<typename T>
+struct ptrue_impl<T, 
+    typename internal::enable_if<is_scalar<T>::value && NumTraits<T>::RequireInitialization>::type > {
+  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/){
+    return T(1);
+  }
+};
+
+/** \internal \returns one bits. */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pmax(const Packet& a,
-        const Packet& b) { return numext::maxi(a, b); }
+ptrue(const Packet& a) {
+  return ptrue_impl<Packet>::run(a);
+}
+
+// In the general case, memset to zero.
+template<typename Packet, typename EnableIf = void>
+struct pzero_impl {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& /*a*/) {
+    Packet b;
+    memset(static_cast<void*>(&b), 0x00, sizeof(Packet));
+    return b;
+  }
+};
+
+// For scalars, explicitly set to Scalar(0), since the underlying representation
+// for zero may not consist of all-zero bits.
+template<typename T>
+struct pzero_impl<T,
+    typename internal::enable_if<is_scalar<T>::value>::type> {
+  static EIGEN_DEVICE_FUNC inline T run(const T& /*a*/) {
+    return T(0);
+  }
+};
+
+/** \internal \returns packet of zeros */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pzero(const Packet& a) {
+  return pzero_impl<Packet>::run(a);
+}
+
+/** \internal \returns a <= b as a bit mask */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pcmp_le(const Packet& a, const Packet& b)  { return a<=b ? ptrue(a) : pzero(a); }
+
+/** \internal \returns a < b as a bit mask */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pcmp_lt(const Packet& a, const Packet& b)  { return a<b ? ptrue(a) : pzero(a); }
+
+/** \internal \returns a == b as a bit mask */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pcmp_eq(const Packet& a, const Packet& b) { return a==b ? ptrue(a) : pzero(a); }
+
+/** \internal \returns a < b or a==NaN or b==NaN as a bit mask */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pcmp_lt_or_nan(const Packet& a, const Packet& b) { return a>=b ? pzero(a) : ptrue(a); }
+
+template<typename T>
+struct bit_and {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const {
+    return a & b;
+  }
+};
+
+template<typename T>
+struct bit_or {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const {
+    return a | b;
+  }
+};
+
+template<typename T>
+struct bit_xor {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a, const T& b) const {
+    return a ^ b;
+  }
+};
+
+template<typename T>
+struct bit_not {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR EIGEN_ALWAYS_INLINE T operator()(const T& a) const {
+    return ~a;
+  }
+};
+
+// Use operators &, |, ^, ~.
+template<typename T>
+struct operator_bitwise_helper {
+  EIGEN_DEVICE_FUNC static inline T bitwise_and(const T& a, const T& b) { return bit_and<T>()(a, b); }
+  EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { return bit_or<T>()(a, b); }
+  EIGEN_DEVICE_FUNC static inline T bitwise_xor(const T& a, const T& b) { return bit_xor<T>()(a, b); }
+  EIGEN_DEVICE_FUNC static inline T bitwise_not(const T& a) { return bit_not<T>()(a); }
+};
+
+// Apply binary operations byte-by-byte
+template<typename T>
+struct bytewise_bitwise_helper {
+  EIGEN_DEVICE_FUNC static inline T bitwise_and(const T& a, const T& b) {
+    return binary(a, b, bit_and<unsigned char>());
+  }
+  EIGEN_DEVICE_FUNC static inline T bitwise_or(const T& a, const T& b) { 
+    return binary(a, b, bit_or<unsigned char>());
+   }
+  EIGEN_DEVICE_FUNC static inline T bitwise_xor(const T& a, const T& b) {
+    return binary(a, b, bit_xor<unsigned char>());
+  }
+  EIGEN_DEVICE_FUNC static inline T bitwise_not(const T& a) { 
+    return unary(a,bit_not<unsigned char>());
+   }
+  
+ private:
+  template<typename Op>
+  EIGEN_DEVICE_FUNC static inline T unary(const T& a, Op op) {
+    const unsigned char* a_ptr = reinterpret_cast<const unsigned char*>(&a);
+    T c;
+    unsigned char* c_ptr = reinterpret_cast<unsigned char*>(&c);
+    for (size_t i = 0; i < sizeof(T); ++i) {
+      *c_ptr++ = op(*a_ptr++);
+    }
+    return c;
+  }
+
+  template<typename Op>
+  EIGEN_DEVICE_FUNC static inline T binary(const T& a, const T& b, Op op) {
+    const unsigned char* a_ptr = reinterpret_cast<const unsigned char*>(&a);
+    const unsigned char* b_ptr = reinterpret_cast<const unsigned char*>(&b);
+    T c;
+    unsigned char* c_ptr = reinterpret_cast<unsigned char*>(&c);
+    for (size_t i = 0; i < sizeof(T); ++i) {
+      *c_ptr++ = op(*a_ptr++, *b_ptr++);
+    }
+    return c;
+  }
+};
+
+// In the general case, use byte-by-byte manipulation.
+template<typename T, typename EnableIf = void>
+struct bitwise_helper : public bytewise_bitwise_helper<T> {};
+
+// For integers or non-trivial scalars, use binary operators.
+template<typename T>
+struct bitwise_helper<T,
+  typename internal::enable_if<
+    is_scalar<T>::value && (NumTraits<T>::IsInteger || NumTraits<T>::RequireInitialization)>::type
+  > : public operator_bitwise_helper<T> {};
+
+/** \internal \returns the bitwise and of \a a and \a b */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pand(const Packet& a, const Packet& b) {
+  return bitwise_helper<Packet>::bitwise_and(a, b);
+}
+
+/** \internal \returns the bitwise or of \a a and \a b */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+por(const Packet& a, const Packet& b) {
+  return bitwise_helper<Packet>::bitwise_or(a, b);
+}
+
+/** \internal \returns the bitwise xor of \a a and \a b */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pxor(const Packet& a, const Packet& b) {
+  return bitwise_helper<Packet>::bitwise_xor(a, b);
+}
+
+/** \internal \returns the bitwise not of \a a */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pnot(const Packet& a) {
+  return bitwise_helper<Packet>::bitwise_not(a);
+}
+
+/** \internal \returns the bitwise and of \a a and not \a b */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pandnot(const Packet& a, const Packet& b) { return pand(a, pnot(b)); }
+
+// In the general case, use bitwise select.
+template<typename Packet, typename EnableIf = void>
+struct pselect_impl {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) {
+    return por(pand(a,mask),pandnot(b,mask));
+  }
+};
+
+// For scalars, use ternary select.
+template<typename Packet>
+struct pselect_impl<Packet, 
+    typename internal::enable_if<is_scalar<Packet>::value>::type > {
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& mask, const Packet& a, const Packet& b) {
+    return numext::equal_strict(mask, Packet(0)) ? b : a;
+  }
+};
+
+/** \internal \returns \a or \b for each field in packet according to \mask */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pselect(const Packet& mask, const Packet& a, const Packet& b) {
+  return pselect_impl<Packet>::run(mask, a, b);
+}
+
+template<> EIGEN_DEVICE_FUNC inline bool pselect<bool>(
+    const bool& cond, const bool& a, const bool& b) {
+  return cond ? a : b;
+}
+
+/** \internal \returns the min or of \a a and \a b (coeff-wise)
+    If either \a a or \a b are NaN, the result is implementation defined. */
+template<int NaNPropagation>
+struct pminmax_impl {
+  template <typename Packet, typename Op>
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
+    return op(a,b);
+  }
+};
+
+/** \internal \returns the min or max of \a a and \a b (coeff-wise)
+    If either \a a or \a b are NaN, NaN is returned. */
+template<>
+struct pminmax_impl<PropagateNaN> {
+  template <typename Packet, typename Op>
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
+  Packet not_nan_mask_a = pcmp_eq(a, a);
+  Packet not_nan_mask_b = pcmp_eq(b, b);
+  return pselect(not_nan_mask_a,
+                 pselect(not_nan_mask_b, op(a, b), b),
+                 a);
+  }
+};
+
+/** \internal \returns the min or max of \a a and \a b (coeff-wise)
+    If both \a a and \a b are NaN, NaN is returned.
+    Equivalent to std::fmin(a, b).  */
+template<>
+struct pminmax_impl<PropagateNumbers> {
+  template <typename Packet, typename Op>
+  static EIGEN_DEVICE_FUNC inline Packet run(const Packet& a, const Packet& b, Op op) {
+  Packet not_nan_mask_a = pcmp_eq(a, a);
+  Packet not_nan_mask_b = pcmp_eq(b, b);
+  return pselect(not_nan_mask_a,
+                 pselect(not_nan_mask_b, op(a, b), a),
+                 b);
+  }
+};
+
+
+#ifndef SYCL_DEVICE_ONLY
+#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) Func
+#else
+#define EIGEN_BINARY_OP_NAN_PROPAGATION(Type, Func) \
+[](const Type& a, const Type& b) { \
+        return Func(a, b);}
+#endif
+
+/** \internal \returns the min of \a a and \a b  (coeff-wise).
+    If \a a or \b b is NaN, the return value is implementation defined. */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pmin(const Packet& a, const Packet& b) { return numext::mini(a,b); }
+
+/** \internal \returns the min of \a a and \a b  (coeff-wise).
+    NaNPropagation determines the NaN propagation semantics. */
+template <int NaNPropagation, typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmin(const Packet& a, const Packet& b) {
+  return pminmax_impl<NaNPropagation>::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet, (pmin<Packet>)));
+}
+
+/** \internal \returns the max of \a a and \a b  (coeff-wise)
+    If \a a or \b b is NaN, the return value is implementation defined. */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pmax(const Packet& a, const Packet& b) { return numext::maxi(a, b); }
+
+/** \internal \returns the max of \a a and \a b  (coeff-wise).
+    NaNPropagation determines the NaN propagation semantics. */
+template <int NaNPropagation, typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pmax(const Packet& a, const Packet& b) {
+  return pminmax_impl<NaNPropagation>::run(a, b, EIGEN_BINARY_OP_NAN_PROPAGATION(Packet,(pmax<Packet>)));
+}
 
 /** \internal \returns the absolute value of \a a */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pabs(const Packet& a) { using std::abs; return abs(a); }
+pabs(const Packet& a) { return numext::abs(a); }
+template<> EIGEN_DEVICE_FUNC inline unsigned int
+pabs(const unsigned int& a) { return a; }
+template<> EIGEN_DEVICE_FUNC inline unsigned long
+pabs(const unsigned long& a) { return a; }
+template<> EIGEN_DEVICE_FUNC inline unsigned long long
+pabs(const unsigned long long& a) { return a; }
+
+/** \internal \returns the addsub value of \a a,b */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+paddsub(const Packet& a, const Packet& b) {
+  return pselect(peven_mask(a), padd(a, b), psub(a, b));
+ }
 
 /** \internal \returns the phase angle of \a a */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 parg(const Packet& a) { using numext::arg; return arg(a); }
 
-/** \internal \returns the bitwise and of \a a and \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pand(const Packet& a, const Packet& b) { return a & b; }
 
-/** \internal \returns the bitwise or of \a a and \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-por(const Packet& a, const Packet& b) { return a | b; }
+/** \internal \returns \a a logically shifted by N bits to the right */
+template<int N> EIGEN_DEVICE_FUNC inline int
+parithmetic_shift_right(const int& a) { return a >> N; }
+template<int N> EIGEN_DEVICE_FUNC inline long int
+parithmetic_shift_right(const long int& a) { return a >> N; }
 
-/** \internal \returns the bitwise xor of \a a and \a b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pxor(const Packet& a, const Packet& b) { return a ^ b; }
+/** \internal \returns \a a arithmetically shifted by N bits to the right */
+template<int N> EIGEN_DEVICE_FUNC inline int
+plogical_shift_right(const int& a) { return static_cast<int>(static_cast<unsigned int>(a) >> N); }
+template<int N> EIGEN_DEVICE_FUNC inline long int
+plogical_shift_right(const long int& a) { return static_cast<long>(static_cast<unsigned long>(a) >> N); }
 
-/** \internal \returns the bitwise andnot of \a a and \a b */
+/** \internal \returns \a a shifted by N bits to the left */
+template<int N> EIGEN_DEVICE_FUNC inline int
+plogical_shift_left(const int& a) { return a << N; }
+template<int N> EIGEN_DEVICE_FUNC inline long int
+plogical_shift_left(const long int& a) { return a << N; }
+
+/** \internal \returns the significant and exponent of the underlying floating point numbers
+  * See https://en.cppreference.com/w/cpp/numeric/math/frexp
+  */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline Packet pfrexp(const Packet& a, Packet& exponent) {
+  int exp;
+  EIGEN_USING_STD(frexp);
+  Packet result = static_cast<Packet>(frexp(a, &exp));
+  exponent = static_cast<Packet>(exp);
+  return result;
+}
+
+/** \internal \returns a * 2^((int)exponent)
+  * See https://en.cppreference.com/w/cpp/numeric/math/ldexp
+  */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pandnot(const Packet& a, const Packet& b) { return a & (!b); }
+pldexp(const Packet &a, const Packet &exponent) {
+  EIGEN_USING_STD(ldexp)
+  return static_cast<Packet>(ldexp(a, static_cast<int>(exponent)));
+}
+
+/** \internal \returns the min of \a a and \a b  (coeff-wise) */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+pabsdiff(const Packet& a, const Packet& b) { return pselect(pcmp_lt(a, b), psub(b, a), psub(a, b)); }
 
 /** \internal \returns a packet version of \a *from, from must be 16 bytes aligned */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
@@ -217,10 +602,22 @@
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 ploadu(const typename unpacket_traits<Packet>::type* from) { return *from; }
 
+/** \internal \returns a packet version of \a *from, (un-aligned masked load)
+ * There is no generic implementation. We only have implementations for specialized
+ * cases. Generic case should not be called.
+ */
+template<typename Packet> EIGEN_DEVICE_FUNC inline
+typename enable_if<unpacket_traits<Packet>::masked_load_available, Packet>::type
+ploadu(const typename unpacket_traits<Packet>::type* from, typename unpacket_traits<Packet>::mask_t umask);
+
 /** \internal \returns a packet with constant coefficients \a a, e.g.: (a,a,a,a) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 pset1(const typename unpacket_traits<Packet>::type& a) { return a; }
 
+/** \internal \returns a packet with constant coefficients set from bits */
+template<typename Packet,typename BitsType> EIGEN_DEVICE_FUNC inline Packet
+pset1frombits(BitsType a);
+
 /** \internal \returns a packet with constant coefficients \a a[0], e.g.: (a[0],a[0],a[0],a[0]) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 pload1(const typename unpacket_traits<Packet>::type  *a) { return pset1<Packet>(*a); }
@@ -237,7 +634,7 @@
   * For instance, for a packet of 8 elements, 2 scalars will be read from \a *from and
   * replicated to form: {from[0],from[0],from[0],from[0],from[1],from[1],from[1],from[1]}
   * Currently, this function is only used in matrix products.
-  * For packet-size smaller or equal to 4, this function is equivalent to pload1 
+  * For packet-size smaller or equal to 4, this function is equivalent to pload1
   */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
 ploadquad(const typename unpacket_traits<Packet>::type* from)
@@ -281,6 +678,20 @@
 template<typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet
 plset(const typename unpacket_traits<Packet>::type& a) { return a; }
 
+/** \internal \returns a packet with constant coefficients \a a, e.g.: (x, 0, x, 0),
+     where x is the value of all 1-bits. */
+template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
+peven_mask(const Packet& /*a*/) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  const size_t n = unpacket_traits<Packet>::size;
+  EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n];
+  for(size_t i = 0; i < n; ++i) {
+    memset(elements+i, ((i & 1) == 0 ? 0xff : 0), sizeof(Scalar));
+  }
+  return ploadu<Packet>(elements);
+}
+
+
 /** \internal copy the packet \a from to \a *to, \a to must be 16 bytes aligned */
 template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline void pstore(Scalar* to, const Packet& from)
 { (*to) = from; }
@@ -289,6 +700,15 @@
 template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline void pstoreu(Scalar* to, const Packet& from)
 {  (*to) = from; }
 
+/** \internal copy the packet \a from to \a *to, (un-aligned store with a mask)
+ * There is no generic implementation. We only have implementations for specialized
+ * cases. Generic case should not be called.
+ */
+template<typename Scalar, typename Packet>
+EIGEN_DEVICE_FUNC inline
+typename enable_if<unpacket_traits<Packet>::masked_store_available, void>::type
+pstoreu(Scalar* to, const Packet& from, typename unpacket_traits<Packet>::mask_t umask);
+
  template<typename Scalar, typename Packet> EIGEN_DEVICE_FUNC inline Packet pgather(const Scalar* from, Index /*stride*/)
  { return ploadu<Packet>(from); }
 
@@ -298,8 +718,10 @@
 /** \internal tries to do cache prefetching of \a addr */
 template<typename Scalar> EIGEN_DEVICE_FUNC inline void prefetch(const Scalar* addr)
 {
-#ifdef __CUDA_ARCH__
-#if defined(__LP64__)
+#if defined(EIGEN_HIP_DEVICE_COMPILE)
+  // do nothing
+#elif defined(EIGEN_CUDA_ARCH)
+#if defined(__LP64__) || EIGEN_OS_WIN64
   // 64-bit pointer operand constraint for inlined asm
   asm(" prefetch.L1 [ %1 ];" : "=l"(addr) : "l"(addr));
 #else
@@ -311,39 +733,6 @@
 #endif
 }
 
-/** \internal \returns the first element of a packet */
-template<typename Packet> EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type pfirst(const Packet& a)
-{ return a; }
-
-/** \internal \returns a packet where the element i contains the sum of the packet of \a vec[i] */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-preduxp(const Packet* vecs) { return vecs[0]; }
-
-/** \internal \returns the sum of the elements of \a a*/
-template<typename Packet> EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux(const Packet& a)
-{ return a; }
-
-/** \internal \returns the sum of the elements of \a a by block of 4 elements.
-  * For a packet {a0, a1, a2, a3, a4, a5, a6, a7}, it returns a half packet {a0+a4, a1+a5, a2+a6, a3+a7}
-  * For packet-size smaller or equal to 4, this boils down to a noop.
-  */
-template<typename Packet> EIGEN_DEVICE_FUNC inline
-typename conditional<(unpacket_traits<Packet>::size%8)==0,typename unpacket_traits<Packet>::half,Packet>::type
-predux_downto4(const Packet& a)
-{ return a; }
-
-/** \internal \returns the product of the elements of \a a*/
-template<typename Packet> EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_mul(const Packet& a)
-{ return a; }
-
-/** \internal \returns the min of the elements of \a a*/
-template<typename Packet> EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(const Packet& a)
-{ return a; }
-
-/** \internal \returns the max of the elements of \a a*/
-template<typename Packet> EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(const Packet& a)
-{ return a; }
-
 /** \internal \returns the reversed elements of \a a*/
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet preverse(const Packet& a)
 { return a; }
@@ -351,10 +740,7 @@
 /** \internal \returns \a a with real and imaginary part flipped (for complex type only) */
 template<typename Packet> EIGEN_DEVICE_FUNC inline Packet pcplxflip(const Packet& a)
 {
-  // FIXME: uncomment the following in case we drop the internal imag and real functions.
-//   using std::imag;
-//   using std::real;
-  return Packet(imag(a),real(a));
+  return Packet(numext::imag(a),numext::real(a));
 }
 
 /**************************
@@ -363,47 +749,51 @@
 
 /** \internal \returns the sine of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet psin(const Packet& a) { using std::sin; return sin(a); }
+Packet psin(const Packet& a) { EIGEN_USING_STD(sin); return sin(a); }
 
 /** \internal \returns the cosine of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pcos(const Packet& a) { using std::cos; return cos(a); }
+Packet pcos(const Packet& a) { EIGEN_USING_STD(cos); return cos(a); }
 
 /** \internal \returns the tan of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet ptan(const Packet& a) { using std::tan; return tan(a); }
+Packet ptan(const Packet& a) { EIGEN_USING_STD(tan); return tan(a); }
 
 /** \internal \returns the arc sine of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pasin(const Packet& a) { using std::asin; return asin(a); }
+Packet pasin(const Packet& a) { EIGEN_USING_STD(asin); return asin(a); }
 
 /** \internal \returns the arc cosine of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pacos(const Packet& a) { using std::acos; return acos(a); }
+Packet pacos(const Packet& a) { EIGEN_USING_STD(acos); return acos(a); }
 
 /** \internal \returns the arc tangent of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet patan(const Packet& a) { using std::atan; return atan(a); }
+Packet patan(const Packet& a) { EIGEN_USING_STD(atan); return atan(a); }
 
 /** \internal \returns the hyperbolic sine of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet psinh(const Packet& a) { using std::sinh; return sinh(a); }
+Packet psinh(const Packet& a) { EIGEN_USING_STD(sinh); return sinh(a); }
 
 /** \internal \returns the hyperbolic cosine of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pcosh(const Packet& a) { using std::cosh; return cosh(a); }
+Packet pcosh(const Packet& a) { EIGEN_USING_STD(cosh); return cosh(a); }
 
 /** \internal \returns the hyperbolic tan of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet ptanh(const Packet& a) { using std::tanh; return tanh(a); }
+Packet ptanh(const Packet& a) { EIGEN_USING_STD(tanh); return tanh(a); }
 
 /** \internal \returns the exp of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet pexp(const Packet& a) { using std::exp; return exp(a); }
+Packet pexp(const Packet& a) { EIGEN_USING_STD(exp); return exp(a); }
+
+/** \internal \returns the expm1 of \a a (coeff-wise) */
+template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+Packet pexpm1(const Packet& a) { return numext::expm1(a); }
 
 /** \internal \returns the log of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet plog(const Packet& a) { using std::log; return log(a); }
+Packet plog(const Packet& a) { EIGEN_USING_STD(log); return log(a); }
 
 /** \internal \returns the log1p of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
@@ -411,16 +801,24 @@
 
 /** \internal \returns the log10 of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet plog10(const Packet& a) { using std::log10; return log10(a); }
+Packet plog10(const Packet& a) { EIGEN_USING_STD(log10); return log10(a); }
+
+/** \internal \returns the log10 of \a a (coeff-wise) */
+template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+Packet plog2(const Packet& a) {
+  typedef typename internal::unpacket_traits<Packet>::type Scalar;
+  return pmul(pset1<Packet>(Scalar(EIGEN_LOG2E)), plog(a)); 
+}
 
 /** \internal \returns the square-root of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-Packet psqrt(const Packet& a) { using std::sqrt; return sqrt(a); }
+Packet psqrt(const Packet& a) { return numext::sqrt(a); }
 
 /** \internal \returns the reciprocal square-root of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
 Packet prsqrt(const Packet& a) {
-  return pdiv(pset1<Packet>(1), psqrt(a));
+  typedef typename internal::unpacket_traits<Packet>::type Scalar;
+  return pdiv(pset1<Packet>(Scalar(1)), psqrt(a));
 }
 
 /** \internal \returns the rounded value of \a a (coeff-wise) */
@@ -431,15 +829,121 @@
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
 Packet pfloor(const Packet& a) { using numext::floor; return floor(a); }
 
+/** \internal \returns the rounded value of \a a (coeff-wise) with current
+ * rounding mode */
+template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+Packet print(const Packet& a) { using numext::rint; return rint(a); }
+
 /** \internal \returns the ceil of \a a (coeff-wise) */
 template<typename Packet> EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
 Packet pceil(const Packet& a) { using numext::ceil; return ceil(a); }
 
+/** \internal \returns the first element of a packet */
+template<typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type
+pfirst(const Packet& a)
+{ return a; }
+
+/** \internal \returns the sum of the elements of upper and lower half of \a a if \a a is larger than 4.
+  * For a packet {a0, a1, a2, a3, a4, a5, a6, a7}, it returns a half packet {a0+a4, a1+a5, a2+a6, a3+a7}
+  * For packet-size smaller or equal to 4, this boils down to a noop.
+  */
+template<typename Packet>
+EIGEN_DEVICE_FUNC inline typename conditional<(unpacket_traits<Packet>::size%8)==0,typename unpacket_traits<Packet>::half,Packet>::type
+predux_half_dowto4(const Packet& a)
+{ return a; }
+
+// Slow generic implementation of Packet reduction.
+template <typename Packet, typename Op>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type
+predux_helper(const Packet& a, Op op) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  const size_t n = unpacket_traits<Packet>::size;
+  EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) Scalar elements[n];
+  pstoreu<Scalar>(elements, a);
+  for(size_t k = n / 2; k > 0; k /= 2)  {
+    for(size_t i = 0; i < k; ++i) {
+      elements[i] = op(elements[i], elements[i + k]);
+    }
+  }
+  return elements[0];
+}
+
+/** \internal \returns the sum of the elements of \a a*/
+template<typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type
+predux(const Packet& a)
+{
+  return a;
+}
+
+/** \internal \returns the product of the elements of \a a */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_mul(
+    const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar; 
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmul<Scalar>)));
+}
+
+/** \internal \returns the min of the elements of \a a */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(
+    const Packet &a) {
+  typedef typename unpacket_traits<Packet>::type Scalar; 
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin<PropagateFast, Scalar>)));
+}
+
+template <int NaNPropagation, typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_min(
+    const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar; 
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmin<NaNPropagation, Scalar>)));
+}
+
+/** \internal \returns the min of the elements of \a a */
+template <typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(
+    const Packet &a) {
+  typedef typename unpacket_traits<Packet>::type Scalar; 
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax<PropagateFast, Scalar>)));
+}
+
+template <int NaNPropagation, typename Packet>
+EIGEN_DEVICE_FUNC inline typename unpacket_traits<Packet>::type predux_max(
+    const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar; 
+  return predux_helper(a, EIGEN_BINARY_OP_NAN_PROPAGATION(Scalar, (pmax<NaNPropagation, Scalar>)));
+}
+
+#undef EIGEN_BINARY_OP_NAN_PROPAGATION
+
+/** \internal \returns true if all coeffs of \a a means "true"
+  * It is supposed to be called on values returned by pcmp_*.
+  */
+// not needed yet
+// template<typename Packet> EIGEN_DEVICE_FUNC inline bool predux_all(const Packet& a)
+// { return bool(a); }
+
+/** \internal \returns true if any coeffs of \a a means "true"
+  * It is supposed to be called on values returned by pcmp_*.
+  */
+template<typename Packet> EIGEN_DEVICE_FUNC inline bool predux_any(const Packet& a)
+{
+  // Dirty but generic implementation where "true" is assumed to be non 0 and all the sames.
+  // It is expected that "true" is either:
+  //  - Scalar(1)
+  //  - bits full of ones (NaN for floats),
+  //  - or first bit equals to 1 (1 for ints, smallest denormal for floats).
+  // For all these cases, taking the sum is just fine, and this boils down to a no-op for scalars.
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  return numext::not_equal_strict(predux(a), Scalar(0));
+}
+
 /***************************************************************************
 * The following functions might not have to be overwritten for vectorized types
 ***************************************************************************/
 
-/** \internal copy a packet with constant coeficient \a a (e.g., [a,a,a,a]) to \a *to. \a to must be 16 bytes aligned */
+/** \internal copy a packet with constant coefficient \a a (e.g., [a,a,a,a]) to \a *to. \a to must be 16 bytes aligned */
 // NOTE: this function must really be templated on the packet type (think about different packet types for the same scalar type)
 template<typename Packet>
 inline void pstore1(typename unpacket_traits<Packet>::type* to, const typename unpacket_traits<Packet>::type& a)
@@ -487,47 +991,18 @@
   return ploadt<Packet, LoadMode>(from);
 }
 
-/** \internal default implementation of palign() allowing partial specialization */
-template<int Offset,typename PacketType>
-struct palign_impl
-{
-  // by default data are aligned, so there is nothing to be done :)
-  static inline void run(PacketType&, const PacketType&) {}
-};
-
-/** \internal update \a first using the concatenation of the packet_size minus \a Offset last elements
-  * of \a first and \a Offset first elements of \a second.
-  * 
-  * This function is currently only used to optimize matrix-vector products on unligned matrices.
-  * It takes 2 packets that represent a contiguous memory array, and returns a packet starting
-  * at the position \a Offset. For instance, for packets of 4 elements, we have:
-  *  Input:
-  *  - first = {f0,f1,f2,f3}
-  *  - second = {s0,s1,s2,s3}
-  * Output: 
-  *   - if Offset==0 then {f0,f1,f2,f3}
-  *   - if Offset==1 then {f1,f2,f3,s0}
-  *   - if Offset==2 then {f2,f3,s0,s1}
-  *   - if Offset==3 then {f3,s0,s1,s3}
-  */
-template<int Offset,typename PacketType>
-inline void palign(PacketType& first, const PacketType& second)
-{
-  palign_impl<Offset,PacketType>::run(first,second);
-}
-
 /***************************************************************************
 * Fast complex products (GCC generates a function call which is very slow)
 ***************************************************************************/
 
 // Eigen+CUDA does not support complexes.
-#ifndef __CUDACC__
+#if !defined(EIGEN_GPUCC)
 
 template<> inline std::complex<float> pmul(const std::complex<float>& a, const std::complex<float>& b)
-{ return std::complex<float>(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); }
+{ return std::complex<float>(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); }
 
 template<> inline std::complex<double> pmul(const std::complex<double>& a, const std::complex<double>& b)
-{ return std::complex<double>(real(a)*real(b) - imag(a)*imag(b), imag(a)*real(b) + real(a)*imag(b)); }
+{ return std::complex<double>(a.real()*b.real() - a.imag()*b.imag(), a.imag()*b.real() + a.real()*b.imag()); }
 
 #endif
 
@@ -558,34 +1033,6 @@
   return ifPacket.select[0] ? thenPacket : elsePacket;
 }
 
-/** \internal \returns \a a with the first coefficient replaced by the scalar b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pinsertfirst(const Packet& a, typename unpacket_traits<Packet>::type b)
-{
-  // Default implementation based on pblend.
-  // It must be specialized for higher performance.
-  Selector<unpacket_traits<Packet>::size> mask;
-  mask.select[0] = true;
-  // This for loop should be optimized away by the compiler.
-  for(Index i=1; i<unpacket_traits<Packet>::size; ++i)
-    mask.select[i] = false;
-  return pblend(mask, pset1<Packet>(b), a);
-}
-
-/** \internal \returns \a a with the last coefficient replaced by the scalar b */
-template<typename Packet> EIGEN_DEVICE_FUNC inline Packet
-pinsertlast(const Packet& a, typename unpacket_traits<Packet>::type b)
-{
-  // Default implementation based on pblend.
-  // It must be specialized for higher performance.
-  Selector<unpacket_traits<Packet>::size> mask;
-  // This for loop should be optimized away by the compiler.
-  for(Index i=0; i<unpacket_traits<Packet>::size-1; ++i)
-    mask.select[i] = false;
-  mask.select[unpacket_traits<Packet>::size-1] = true;
-  return pblend(mask, pset1<Packet>(b), a);
-}
-
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
index 769dc25..629af94 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/GlobalFunctions.h
@@ -66,21 +66,31 @@
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sinh,scalar_sinh_op,hyperbolic sine,\sa ArrayBase::sinh)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cosh,scalar_cosh_op,hyperbolic cosine,\sa ArrayBase::cosh)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(tanh,scalar_tanh_op,hyperbolic tangent,\sa ArrayBase::tanh)
+#if EIGEN_HAS_CXX11_MATH
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(asinh,scalar_asinh_op,inverse hyperbolic sine,\sa ArrayBase::asinh)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(acosh,scalar_acosh_op,inverse hyperbolic cosine,\sa ArrayBase::acosh)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(atanh,scalar_atanh_op,inverse hyperbolic tangent,\sa ArrayBase::atanh)
+#endif
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(logistic,scalar_logistic_op,logistic function,\sa ArrayBase::logistic)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(lgamma,scalar_lgamma_op,natural logarithm of the gamma function,\sa ArrayBase::lgamma)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(digamma,scalar_digamma_op,derivative of lgamma,\sa ArrayBase::digamma)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erf,scalar_erf_op,error function,\sa ArrayBase::erf)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(erfc,scalar_erfc_op,complement error function,\sa ArrayBase::erfc)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ndtri,scalar_ndtri_op,inverse normal distribution function,\sa ArrayBase::ndtri)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(exp,scalar_exp_op,exponential,\sa ArrayBase::exp)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(expm1,scalar_expm1_op,exponential of a value minus 1,\sa ArrayBase::expm1)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log,scalar_log_op,natural logarithm,\sa Eigen::log10 DOXCOMMA ArrayBase::log)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log1p,scalar_log1p_op,natural logarithm of 1 plus the value,\sa ArrayBase::log1p)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log10,scalar_log10_op,base 10 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log10,scalar_log10_op,base 10 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log10)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(log2,scalar_log2_op,base 2 logarithm,\sa Eigen::log DOXCOMMA ArrayBase::log2)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs,scalar_abs_op,absolute value,\sa ArrayBase::abs DOXCOMMA MatrixBase::cwiseAbs)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(abs2,scalar_abs2_op,squared absolute value,\sa ArrayBase::abs2 DOXCOMMA MatrixBase::cwiseAbs2)
-  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(arg,scalar_arg_op,complex argument,\sa ArrayBase::arg)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(arg,scalar_arg_op,complex argument,\sa ArrayBase::arg DOXCOMMA MatrixBase::cwiseArg)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sqrt,scalar_sqrt_op,square root,\sa ArrayBase::sqrt DOXCOMMA MatrixBase::cwiseSqrt)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rsqrt,scalar_rsqrt_op,reciprocal square root,\sa ArrayBase::rsqrt)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(square,scalar_square_op,square (power 2),\sa Eigen::abs2 DOXCOMMA Eigen::pow DOXCOMMA ArrayBase::square)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(cube,scalar_cube_op,cube (power 3),\sa Eigen::pow DOXCOMMA ArrayBase::cube)
+  EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(rint,scalar_rint_op,nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(round,scalar_round_op,nearest integer,\sa Eigen::floor DOXCOMMA Eigen::ceil DOXCOMMA ArrayBase::round)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(floor,scalar_floor_op,nearest integer not greater than the giben value,\sa Eigen::ceil DOXCOMMA ArrayBase::floor)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(ceil,scalar_ceil_op,nearest integer not less than the giben value,\sa Eigen::floor DOXCOMMA ArrayBase::ceil)
@@ -88,7 +98,7 @@
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isinf,scalar_isinf_op,infinite value test,\sa Eigen::isnan DOXCOMMA Eigen::isfinite DOXCOMMA ArrayBase::isinf)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(isfinite,scalar_isfinite_op,finite value test,\sa Eigen::isinf DOXCOMMA Eigen::isnan DOXCOMMA ArrayBase::isfinite)
   EIGEN_ARRAY_DECLARE_GLOBAL_UNARY(sign,scalar_sign_op,sign (or 0),\sa ArrayBase::sign)
-  
+
   /** \returns an expression of the coefficient-wise power of \a x to the given constant \a exponent.
     *
     * \tparam ScalarExponent is the scalar type of \a exponent. It must be compatible with the scalar type of the given expression (\c Derived::Scalar).
@@ -102,17 +112,18 @@
   inline const CwiseBinaryOp<internal::scalar_pow_op<Derived::Scalar,ScalarExponent>,Derived,Constant<ScalarExponent> >
   pow(const Eigen::ArrayBase<Derived>& x, const ScalarExponent& exponent);
 #else
-  template<typename Derived,typename ScalarExponent>
-  inline typename internal::enable_if<   !(internal::is_same<typename Derived::Scalar,ScalarExponent>::value) && EIGEN_SCALAR_BINARY_SUPPORTED(pow,typename Derived::Scalar,ScalarExponent),
-          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,ScalarExponent,pow) >::type
-  pow(const Eigen::ArrayBase<Derived>& x, const ScalarExponent& exponent) {
-    return x.derived().pow(exponent);
-  }
-
-  template<typename Derived>
-  inline const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename Derived::Scalar,pow)
-  pow(const Eigen::ArrayBase<Derived>& x, const typename Derived::Scalar& exponent) {
-    return x.derived().pow(exponent);
+  template <typename Derived,typename ScalarExponent>
+  EIGEN_DEVICE_FUNC inline
+  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(
+    const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg<typename Derived::Scalar
+                                                 EIGEN_COMMA ScalarExponent EIGEN_COMMA
+                                                 EIGEN_SCALAR_BINARY_SUPPORTED(pow,typename Derived::Scalar,ScalarExponent)>::type,pow))
+  pow(const Eigen::ArrayBase<Derived>& x, const ScalarExponent& exponent)
+  {
+    typedef typename internal::promote_scalar_arg<typename Derived::Scalar,ScalarExponent,
+                                                  EIGEN_SCALAR_BINARY_SUPPORTED(pow,typename Derived::Scalar,ScalarExponent)>::type PromotedExponent;
+    return EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,PromotedExponent,pow)(x.derived(),
+           typename internal::plain_constant_type<Derived,PromotedExponent>::type(x.derived().rows(), x.derived().cols(), internal::scalar_constant_op<PromotedExponent>(exponent)));
   }
 #endif
 
@@ -122,21 +133,21 @@
     *
     * Example: \include Cwise_array_power_array.cpp
     * Output: \verbinclude Cwise_array_power_array.out
-    * 
+    *
     * \sa ArrayBase::pow()
     *
     * \relates ArrayBase
     */
   template<typename Derived,typename ExponentDerived>
   inline const Eigen::CwiseBinaryOp<Eigen::internal::scalar_pow_op<typename Derived::Scalar, typename ExponentDerived::Scalar>, const Derived, const ExponentDerived>
-  pow(const Eigen::ArrayBase<Derived>& x, const Eigen::ArrayBase<ExponentDerived>& exponents) 
+  pow(const Eigen::ArrayBase<Derived>& x, const Eigen::ArrayBase<ExponentDerived>& exponents)
   {
     return Eigen::CwiseBinaryOp<Eigen::internal::scalar_pow_op<typename Derived::Scalar, typename ExponentDerived::Scalar>, const Derived, const ExponentDerived>(
       x.derived(),
       exponents.derived()
     );
   }
-  
+
   /** \returns an expression of the coefficient-wise power of the scalar \a x to the given array of \a exponents.
     *
     * This function computes the coefficient-wise power between a scalar and an array of exponents.
@@ -145,7 +156,7 @@
     *
     * Example: \include Cwise_scalar_power_array.cpp
     * Output: \verbinclude Cwise_scalar_power_array.out
-    * 
+    *
     * \sa ArrayBase::pow()
     *
     * \relates ArrayBase
@@ -155,21 +166,17 @@
   inline const CwiseBinaryOp<internal::scalar_pow_op<Scalar,Derived::Scalar>,Constant<Scalar>,Derived>
   pow(const Scalar& x,const Eigen::ArrayBase<Derived>& x);
 #else
-  template<typename Scalar, typename Derived>
-  inline typename internal::enable_if<   !(internal::is_same<typename Derived::Scalar,Scalar>::value) && EIGEN_SCALAR_BINARY_SUPPORTED(pow,Scalar,typename Derived::Scalar),
-          const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,Derived,pow) >::type
-  pow(const Scalar& x, const Eigen::ArrayBase<Derived>& exponents)
-  {
-    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,Derived,pow)(
-            typename internal::plain_constant_type<Derived,Scalar>::type(exponents.rows(), exponents.cols(), x), exponents.derived() );
-  }
-
-  template<typename Derived>
-  inline const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename Derived::Scalar,Derived,pow)
-  pow(const typename Derived::Scalar& x, const Eigen::ArrayBase<Derived>& exponents)
-  {
-    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename Derived::Scalar,Derived,pow)(
-      typename internal::plain_constant_type<Derived,typename Derived::Scalar>::type(exponents.rows(), exponents.cols(), x), exponents.derived() );
+  template <typename Scalar, typename Derived>
+  EIGEN_DEVICE_FUNC inline
+  EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(
+    const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename internal::promote_scalar_arg<typename Derived::Scalar
+                                                 EIGEN_COMMA Scalar EIGEN_COMMA
+                                                 EIGEN_SCALAR_BINARY_SUPPORTED(pow,Scalar,typename Derived::Scalar)>::type,Derived,pow))
+  pow(const Scalar& x, const Eigen::ArrayBase<Derived>& exponents) {
+    typedef typename internal::promote_scalar_arg<typename Derived::Scalar,Scalar,
+                                                  EIGEN_SCALAR_BINARY_SUPPORTED(pow,Scalar,typename Derived::Scalar)>::type PromotedScalar;
+    return EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(PromotedScalar,Derived,pow)(
+           typename internal::plain_constant_type<Derived,PromotedScalar>::type(exponents.derived().rows(), exponents.derived().cols(), internal::scalar_constant_op<PromotedScalar>(x)), exponents.derived());
   }
 #endif
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h
index da7fd6c..e81c315 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IO.h
@@ -41,6 +41,7 @@
   *  - \b rowSuffix string printed at the end of each row
   *  - \b matPrefix string printed at the beginning of the matrix
   *  - \b matSuffix string printed at the end of the matrix
+  *  - \b fill character printed to fill the empty space in aligned columns
   *
   * Example: \include IOFormat.cpp
   * Output: \verbinclude IOFormat.out
@@ -53,9 +54,9 @@
   IOFormat(int _precision = StreamPrecision, int _flags = 0,
     const std::string& _coeffSeparator = " ",
     const std::string& _rowSeparator = "\n", const std::string& _rowPrefix="", const std::string& _rowSuffix="",
-    const std::string& _matPrefix="", const std::string& _matSuffix="")
+    const std::string& _matPrefix="", const std::string& _matSuffix="", const char _fill=' ')
   : matPrefix(_matPrefix), matSuffix(_matSuffix), rowPrefix(_rowPrefix), rowSuffix(_rowSuffix), rowSeparator(_rowSeparator),
-    rowSpacer(""), coeffSeparator(_coeffSeparator), precision(_precision), flags(_flags)
+    rowSpacer(""), coeffSeparator(_coeffSeparator), fill(_fill), precision(_precision), flags(_flags)
   {
     // TODO check if rowPrefix, rowSuffix or rowSeparator contains a newline
     // don't add rowSpacer if columns are not to be aligned
@@ -71,6 +72,7 @@
   std::string matPrefix, matSuffix;
   std::string rowPrefix, rowSuffix, rowSeparator, rowSpacer;
   std::string coeffSeparator;
+  char fill;
   int precision;
   int flags;
 };
@@ -128,6 +130,9 @@
 template<typename Derived>
 std::ostream & print_matrix(std::ostream & s, const Derived& _m, const IOFormat& fmt)
 {
+  using internal::is_same;
+  using internal::conditional;
+
   if(_m.size() == 0)
   {
     s << fmt.matPrefix << fmt.matSuffix;
@@ -136,6 +141,22 @@
   
   typename Derived::Nested m = _m;
   typedef typename Derived::Scalar Scalar;
+  typedef typename
+      conditional<
+          is_same<Scalar, char>::value ||
+            is_same<Scalar, unsigned char>::value ||
+            is_same<Scalar, numext::int8_t>::value ||
+            is_same<Scalar, numext::uint8_t>::value,
+          int,
+          typename conditional<
+              is_same<Scalar, std::complex<char> >::value ||
+                is_same<Scalar, std::complex<unsigned char> >::value ||
+                is_same<Scalar, std::complex<numext::int8_t> >::value ||
+                is_same<Scalar, std::complex<numext::uint8_t> >::value,
+              std::complex<int>,
+              const Scalar&
+            >::type
+        >::type PrintType;
 
   Index width = 0;
 
@@ -172,23 +193,31 @@
       {
         std::stringstream sstr;
         sstr.copyfmt(s);
-        sstr << m.coeff(i,j);
+        sstr << static_cast<PrintType>(m.coeff(i,j));
         width = std::max<Index>(width, Index(sstr.str().length()));
       }
   }
+  std::streamsize old_width = s.width();
+  char old_fill_character = s.fill();
   s << fmt.matPrefix;
   for(Index i = 0; i < m.rows(); ++i)
   {
     if (i)
       s << fmt.rowSpacer;
     s << fmt.rowPrefix;
-    if(width) s.width(width);
-    s << m.coeff(i, 0);
+    if(width) {
+      s.fill(fmt.fill);
+      s.width(width);
+    }
+    s << static_cast<PrintType>(m.coeff(i, 0));
     for(Index j = 1; j < m.cols(); ++j)
     {
       s << fmt.coeffSeparator;
-      if (width) s.width(width);
-      s << m.coeff(i, j);
+      if(width) {
+        s.fill(fmt.fill);
+        s.width(width);
+      }
+      s << static_cast<PrintType>(m.coeff(i, j));
     }
     s << fmt.rowSuffix;
     if( i < m.rows() - 1)
@@ -196,6 +225,10 @@
   }
   s << fmt.matSuffix;
   if(explicit_precision) s.precision(old_precision);
+  if(width) {
+    s.fill(old_fill_character);
+    s.width(old_width);
+  }
   return s;
 }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h
new file mode 100644
index 0000000..0847625
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/IndexedView.h
@@ -0,0 +1,237 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_INDEXED_VIEW_H
+#define EIGEN_INDEXED_VIEW_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename XprType, typename RowIndices, typename ColIndices>
+struct traits<IndexedView<XprType, RowIndices, ColIndices> >
+ : traits<XprType>
+{
+  enum {
+    RowsAtCompileTime = int(array_size<RowIndices>::value),
+    ColsAtCompileTime = int(array_size<ColIndices>::value),
+    MaxRowsAtCompileTime = RowsAtCompileTime != Dynamic ? int(RowsAtCompileTime) : Dynamic,
+    MaxColsAtCompileTime = ColsAtCompileTime != Dynamic ? int(ColsAtCompileTime) : Dynamic,
+
+    XprTypeIsRowMajor = (int(traits<XprType>::Flags)&RowMajorBit) != 0,
+    IsRowMajor = (MaxRowsAtCompileTime==1&&MaxColsAtCompileTime!=1) ? 1
+               : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
+               : XprTypeIsRowMajor,
+
+    RowIncr = int(get_compile_time_incr<RowIndices>::value),
+    ColIncr = int(get_compile_time_incr<ColIndices>::value),
+    InnerIncr = IsRowMajor ? ColIncr : RowIncr,
+    OuterIncr = IsRowMajor ? RowIncr : ColIncr,
+
+    HasSameStorageOrderAsXprType = (IsRowMajor == XprTypeIsRowMajor),
+    XprInnerStride = HasSameStorageOrderAsXprType ? int(inner_stride_at_compile_time<XprType>::ret) : int(outer_stride_at_compile_time<XprType>::ret),
+    XprOuterstride = HasSameStorageOrderAsXprType ? int(outer_stride_at_compile_time<XprType>::ret) : int(inner_stride_at_compile_time<XprType>::ret),
+
+    InnerSize = XprTypeIsRowMajor ? ColsAtCompileTime : RowsAtCompileTime,
+    IsBlockAlike = InnerIncr==1 && OuterIncr==1,
+    IsInnerPannel = HasSameStorageOrderAsXprType && is_same<AllRange<InnerSize>,typename conditional<XprTypeIsRowMajor,ColIndices,RowIndices>::type>::value,
+
+    InnerStrideAtCompileTime = InnerIncr<0 || InnerIncr==DynamicIndex || XprInnerStride==Dynamic ? Dynamic : XprInnerStride * InnerIncr,
+    OuterStrideAtCompileTime = OuterIncr<0 || OuterIncr==DynamicIndex || XprOuterstride==Dynamic ? Dynamic : XprOuterstride * OuterIncr,
+
+    ReturnAsScalar = is_same<RowIndices,SingleRange>::value && is_same<ColIndices,SingleRange>::value,
+    ReturnAsBlock = (!ReturnAsScalar) && IsBlockAlike,
+    ReturnAsIndexedView = (!ReturnAsScalar) && (!ReturnAsBlock),
+
+    // FIXME we deal with compile-time strides if and only if we have DirectAccessBit flag,
+    // but this is too strict regarding negative strides...
+    DirectAccessMask = (int(InnerIncr)!=UndefinedIncr && int(OuterIncr)!=UndefinedIncr && InnerIncr>=0 && OuterIncr>=0) ? DirectAccessBit : 0,
+    FlagsRowMajorBit = IsRowMajor ? RowMajorBit : 0,
+    FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    Flags = (traits<XprType>::Flags & (HereditaryBits | DirectAccessMask )) | FlagsLvalueBit | FlagsRowMajorBit | FlagsLinearAccessBit
+  };
+
+  typedef Block<XprType,RowsAtCompileTime,ColsAtCompileTime,IsInnerPannel> BlockType;
+};
+
+}
+
+template<typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
+class IndexedViewImpl;
+
+
+/** \class IndexedView
+  * \ingroup Core_Module
+  *
+  * \brief Expression of a non-sequential sub-matrix defined by arbitrary sequences of row and column indices
+  *
+  * \tparam XprType the type of the expression in which we are taking the intersections of sub-rows and sub-columns
+  * \tparam RowIndices the type of the object defining the sequence of row indices
+  * \tparam ColIndices the type of the object defining the sequence of column indices
+  *
+  * This class represents an expression of a sub-matrix (or sub-vector) defined as the intersection
+  * of sub-sets of rows and columns, that are themself defined by generic sequences of row indices \f$ \{r_0,r_1,..r_{m-1}\} \f$
+  * and column indices \f$ \{c_0,c_1,..c_{n-1} \}\f$. Let \f$ A \f$  be the nested matrix, then the resulting matrix \f$ B \f$ has \c m
+  * rows and \c n columns, and its entries are given by: \f$ B(i,j) = A(r_i,c_j) \f$.
+  *
+  * The \c RowIndices and \c ColIndices types must be compatible with the following API:
+  * \code
+  * <integral type> operator[](Index) const;
+  * Index size() const;
+  * \endcode
+  *
+  * Typical supported types thus include:
+  *  - std::vector<int>
+  *  - std::valarray<int>
+  *  - std::array<int>
+  *  - Plain C arrays: int[N]
+  *  - Eigen::ArrayXi
+  *  - decltype(ArrayXi::LinSpaced(...))
+  *  - Any view/expressions of the previous types
+  *  - Eigen::ArithmeticSequence
+  *  - Eigen::internal::AllRange      (helper for Eigen::all)
+  *  - Eigen::internal::SingleRange  (helper for single index)
+  *  - etc.
+  *
+  * In typical usages of %Eigen, this class should never be used directly. It is the return type of
+  * DenseBase::operator()(const RowIndices&, const ColIndices&).
+  *
+  * \sa class Block
+  */
+template<typename XprType, typename RowIndices, typename ColIndices>
+class IndexedView : public IndexedViewImpl<XprType, RowIndices, ColIndices, typename internal::traits<XprType>::StorageKind>
+{
+public:
+  typedef typename IndexedViewImpl<XprType, RowIndices, ColIndices, typename internal::traits<XprType>::StorageKind>::Base Base;
+  EIGEN_GENERIC_PUBLIC_INTERFACE(IndexedView)
+  EIGEN_INHERIT_ASSIGNMENT_OPERATORS(IndexedView)
+
+  typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
+  typedef typename internal::remove_all<XprType>::type NestedExpression;
+
+  template<typename T0, typename T1>
+  IndexedView(XprType& xpr, const T0& rowIndices, const T1& colIndices)
+    : m_xpr(xpr), m_rowIndices(rowIndices), m_colIndices(colIndices)
+  {}
+
+  /** \returns number of rows */
+  Index rows() const { return internal::size(m_rowIndices); }
+
+  /** \returns number of columns */
+  Index cols() const { return internal::size(m_colIndices); }
+
+  /** \returns the nested expression */
+  const typename internal::remove_all<XprType>::type&
+  nestedExpression() const { return m_xpr; }
+
+  /** \returns the nested expression */
+  typename internal::remove_reference<XprType>::type&
+  nestedExpression() { return m_xpr; }
+
+  /** \returns a const reference to the object storing/generating the row indices */
+  const RowIndices& rowIndices() const { return m_rowIndices; }
+
+  /** \returns a const reference to the object storing/generating the column indices */
+  const ColIndices& colIndices() const { return m_colIndices; }
+
+protected:
+  MatrixTypeNested m_xpr;
+  RowIndices m_rowIndices;
+  ColIndices m_colIndices;
+};
+
+
+// Generic API dispatcher
+template<typename XprType, typename RowIndices, typename ColIndices, typename StorageKind>
+class IndexedViewImpl
+  : public internal::generic_xpr_base<IndexedView<XprType, RowIndices, ColIndices> >::type
+{
+public:
+  typedef typename internal::generic_xpr_base<IndexedView<XprType, RowIndices, ColIndices> >::type Base;
+};
+
+namespace internal {
+
+
+template<typename ArgType, typename RowIndices, typename ColIndices>
+struct unary_evaluator<IndexedView<ArgType, RowIndices, ColIndices>, IndexBased>
+  : evaluator_base<IndexedView<ArgType, RowIndices, ColIndices> >
+{
+  typedef IndexedView<ArgType, RowIndices, ColIndices> XprType;
+
+  enum {
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost /* TODO + cost of row/col index */,
+
+    FlagsLinearAccessBit = (traits<XprType>::RowsAtCompileTime == 1 || traits<XprType>::ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+
+    FlagsRowMajorBit = traits<XprType>::FlagsRowMajorBit, 
+
+    Flags = (evaluator<ArgType>::Flags & (HereditaryBits & ~RowMajorBit /*| LinearAccessBit | DirectAccessBit*/)) | FlagsLinearAccessBit | FlagsRowMajorBit,
+
+    Alignment = 0
+  };
+
+  EIGEN_DEVICE_FUNC explicit unary_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  typedef typename XprType::Scalar Scalar;
+  typedef typename XprType::CoeffReturnType CoeffReturnType;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  CoeffReturnType coeff(Index row, Index col) const
+  {
+    return m_argImpl.coeff(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Scalar& coeffRef(Index row, Index col)
+  {
+    return m_argImpl.coeffRef(m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Scalar& coeffRef(Index index)
+  {
+    EIGEN_STATIC_ASSERT_LVALUE(XprType)
+    Index row = XprType::RowsAtCompileTime == 1 ? 0 : index;
+    Index col = XprType::RowsAtCompileTime == 1 ? index : 0;
+    return m_argImpl.coeffRef( m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  const Scalar& coeffRef(Index index) const
+  {
+    Index row = XprType::RowsAtCompileTime == 1 ? 0 : index;
+    Index col = XprType::RowsAtCompileTime == 1 ? index : 0;
+    return m_argImpl.coeffRef( m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  const CoeffReturnType coeff(Index index) const
+  {
+    Index row = XprType::RowsAtCompileTime == 1 ? 0 : index;
+    Index col = XprType::RowsAtCompileTime == 1 ? index : 0;
+    return m_argImpl.coeff( m_xpr.rowIndices()[row], m_xpr.colIndices()[col]);
+  }
+
+protected:
+
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_xpr;
+
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_INDEXED_VIEW_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
index b76f043..c514438 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Inverse.h
@@ -1,7 +1,7 @@
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
-// Copyright (C) 2014 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2014-2019 Gael Guennebaud <gael.guennebaud@inria.fr>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -10,7 +10,7 @@
 #ifndef EIGEN_INVERSE_H
 #define EIGEN_INVERSE_H
 
-namespace Eigen { 
+namespace Eigen {
 
 template<typename XprType,typename StorageKind> class InverseImpl;
 
@@ -44,19 +44,18 @@
 {
 public:
   typedef typename XprType::StorageIndex StorageIndex;
-  typedef typename XprType::PlainObject                       PlainObject;
   typedef typename XprType::Scalar                            Scalar;
   typedef typename internal::ref_selector<XprType>::type      XprTypeNested;
   typedef typename internal::remove_all<XprTypeNested>::type  XprTypeNestedCleaned;
   typedef typename internal::ref_selector<Inverse>::type Nested;
   typedef typename internal::remove_all<XprType>::type NestedExpression;
-  
+
   explicit EIGEN_DEVICE_FUNC Inverse(const XprType &xpr)
     : m_xpr(xpr)
   {}
 
-  EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); }
-  EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR  Index rows() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR  Index cols() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
 
   EIGEN_DEVICE_FUNC const XprTypeNestedCleaned& nestedExpression() const { return m_xpr; }
 
@@ -82,7 +81,7 @@
 
 /** \internal
   * \brief Default evaluator for Inverse expression.
-  * 
+  *
   * This default evaluator for Inverse expression simply evaluate the inverse into a temporary
   * by a call to internal::call_assignment_no_alias.
   * Therefore, inverse implementers only have to specialize Assignment<Dst,Inverse<...>, ...> for
@@ -97,7 +96,7 @@
   typedef Inverse<ArgType> InverseType;
   typedef typename InverseType::PlainObject PlainObject;
   typedef evaluator<PlainObject> Base;
-  
+
   enum { Flags = Base::Flags | EvalBeforeNestingBit };
 
   unary_evaluator(const InverseType& inv_xpr)
@@ -106,11 +105,11 @@
     ::new (static_cast<Base*>(this)) Base(m_result);
     internal::call_assignment_no_alias(m_result, inv_xpr);
   }
-  
+
 protected:
   PlainObject m_result;
 };
-  
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h
index 548bf9a..218cc15 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Map.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_MAP_H
 #define EIGEN_MAP_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 template<typename PlainObjectType, int MapOptions, typename StrideType>
@@ -47,7 +47,7 @@
   * \brief A matrix or vector expression mapping an existing array of data.
   *
   * \tparam PlainObjectType the equivalent matrix type of the mapped data
-  * \tparam MapOptions specifies the pointer alignment in bytes. It can be: \c #Aligned128, , \c #Aligned64, \c #Aligned32, \c #Aligned16, \c #Aligned8 or \c #Unaligned.
+  * \tparam MapOptions specifies the pointer alignment in bytes. It can be: \c #Aligned128, \c #Aligned64, \c #Aligned32, \c #Aligned16, \c #Aligned8 or \c #Unaligned.
   *                The default is \c #Unaligned.
   * \tparam StrideType optionally specifies strides. By default, Map assumes the memory layout
   *                   of an ordinary, contiguous array. This can be overridden by specifying strides.
@@ -104,19 +104,19 @@
     EIGEN_DEVICE_FUNC
     inline PointerType cast_to_pointer_type(PointerArgType ptr) { return ptr; }
 
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index innerStride() const
     {
       return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1;
     }
 
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index outerStride() const
     {
-      return int(StrideType::OuterStrideAtCompileTime) != 0 ? m_stride.outer()
-           : int(internal::traits<Map>::OuterStrideAtCompileTime) != Dynamic ? Index(internal::traits<Map>::OuterStrideAtCompileTime)
+      return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
+           : internal::traits<Map>::OuterStrideAtCompileTime != Dynamic ? Index(internal::traits<Map>::OuterStrideAtCompileTime)
            : IsVectorAtCompileTime ? (this->size() * innerStride())
-           : (int(Flags)&RowMajorBit) ? (this->cols() * innerStride())
+           : int(Flags)&RowMajorBit ? (this->cols() * innerStride())
            : (this->rows() * innerStride());
     }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
index 92c3b28..d856447 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MapBase.h
@@ -15,7 +15,7 @@
       EIGEN_STATIC_ASSERT((int(internal::evaluator<Derived>::Flags) & LinearAccessBit) || Derived::IsVectorAtCompileTime, \
                           YOU_ARE_TRYING_TO_USE_AN_INDEX_BASED_ACCESSOR_ON_AN_EXPRESSION_THAT_DOES_NOT_SUPPORT_THAT)
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \ingroup Core_Module
   *
@@ -87,9 +87,11 @@
     typedef typename Base::CoeffReturnType CoeffReturnType;
 
     /** \copydoc DenseBase::rows() */
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_rows.value(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_rows.value(); }
     /** \copydoc DenseBase::cols() */
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_cols.value(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_cols.value(); }
 
     /** Returns a pointer to the first coefficient of the matrix or vector.
       *
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
index b249ce0..61b78f4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctions.h
@@ -2,6 +2,7 @@
 // for linear algebra.
 //
 // Copyright (C) 2006-2010 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (c) 2021, NVIDIA CORPORATION. All rights reserved.
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -10,10 +11,11 @@
 #ifndef EIGEN_MATHFUNCTIONS_H
 #define EIGEN_MATHFUNCTIONS_H
 
-// source: http://www.geom.uiuc.edu/~huberty/math5337/groupe/digits.html
 // TODO this should better be moved to NumTraits
-#define EIGEN_PI 3.141592653589793238462643383279502884197169399375105820974944592307816406L
-
+// Source: WolframAlpha
+#define EIGEN_PI    3.141592653589793238462643383279502884197169399375105820974944592307816406L
+#define EIGEN_LOG2E 1.442695040888963407359924681001892137426645954152985934135449406931109219L
+#define EIGEN_LN2   0.693147180559945309417232121458176568075500134360255254120680009493393621L
 
 namespace Eigen {
 
@@ -97,7 +99,7 @@
 
 template<typename Scalar> struct real_impl : real_default_impl<Scalar> {};
 
-#ifdef __CUDA_ARCH__
+#if defined(EIGEN_GPU_COMPILE_PHASE)
 template<typename T>
 struct real_impl<std::complex<T> >
 {
@@ -145,7 +147,7 @@
 
 template<typename Scalar> struct imag_impl : imag_default_impl<Scalar> {};
 
-#ifdef __CUDA_ARCH__
+#if defined(EIGEN_GPU_COMPILE_PHASE)
 template<typename T>
 struct imag_impl<std::complex<T> >
 {
@@ -213,12 +215,12 @@
 template<typename Scalar>
 struct imag_ref_default_impl<Scalar, false>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline Scalar run(Scalar&)
   {
     return Scalar(0);
   }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline const Scalar run(const Scalar&)
   {
     return Scalar(0);
@@ -239,7 +241,7 @@
 ****************************************************************************/
 
 template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
-struct conj_impl
+struct conj_default_impl
 {
   EIGEN_DEVICE_FUNC
   static inline Scalar run(const Scalar& x)
@@ -249,7 +251,7 @@
 };
 
 template<typename Scalar>
-struct conj_impl<Scalar,true>
+struct conj_default_impl<Scalar,true>
 {
   EIGEN_DEVICE_FUNC
   static inline Scalar run(const Scalar& x)
@@ -259,6 +261,9 @@
   }
 };
 
+template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct conj_impl : conj_default_impl<Scalar, IsComplex> {};
+
 template<typename Scalar>
 struct conj_retval
 {
@@ -287,7 +292,7 @@
   EIGEN_DEVICE_FUNC
   static inline RealScalar run(const Scalar& x)
   {
-    return real(x)*real(x) + imag(x)*imag(x);
+    return x.real()*x.real() + x.imag()*x.imag();
   }
 };
 
@@ -309,18 +314,80 @@
 };
 
 /****************************************************************************
+* Implementation of sqrt/rsqrt                                             *
+****************************************************************************/
+
+template<typename Scalar>
+struct sqrt_impl
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_ALWAYS_INLINE Scalar run(const Scalar& x)
+  {
+    EIGEN_USING_STD(sqrt);
+    return sqrt(x);
+  }
+};
+
+// Complex sqrt defined in MathFunctionsImpl.h.
+template<typename T> EIGEN_DEVICE_FUNC std::complex<T> complex_sqrt(const std::complex<T>& a_x);
+
+// Custom implementation is faster than `std::sqrt`, works on
+// GPU, and correctly handles special cases (unlike MSVC).
+template<typename T>
+struct sqrt_impl<std::complex<T> >
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_ALWAYS_INLINE std::complex<T> run(const std::complex<T>& x)
+  {
+    return complex_sqrt<T>(x);
+  }
+};
+
+template<typename Scalar>
+struct sqrt_retval
+{
+  typedef Scalar type;
+};
+
+// Default implementation relies on numext::sqrt, at bottom of file.
+template<typename T>
+struct rsqrt_impl;
+
+// Complex rsqrt defined in MathFunctionsImpl.h.
+template<typename T> EIGEN_DEVICE_FUNC std::complex<T> complex_rsqrt(const std::complex<T>& a_x);
+
+template<typename T>
+struct rsqrt_impl<std::complex<T> >
+{
+  EIGEN_DEVICE_FUNC
+  static EIGEN_ALWAYS_INLINE std::complex<T> run(const std::complex<T>& x)
+  {
+    return complex_rsqrt<T>(x);
+  }
+};
+
+template<typename Scalar>
+struct rsqrt_retval
+{
+  typedef Scalar type;
+};
+
+/****************************************************************************
 * Implementation of norm1                                                *
 ****************************************************************************/
 
 template<typename Scalar, bool IsComplex>
-struct norm1_default_impl
+struct norm1_default_impl;
+
+template<typename Scalar>
+struct norm1_default_impl<Scalar,true>
 {
   typedef typename NumTraits<Scalar>::Real RealScalar;
   EIGEN_DEVICE_FUNC
   static inline RealScalar run(const Scalar& x)
   {
-    EIGEN_USING_STD_MATH(abs);
-    return abs(real(x)) + abs(imag(x));
+    EIGEN_USING_STD(abs);
+    return abs(x.real()) + abs(x.imag());
   }
 };
 
@@ -330,7 +397,7 @@
   EIGEN_DEVICE_FUNC
   static inline Scalar run(const Scalar& x)
   {
-    EIGEN_USING_STD_MATH(abs);
+    EIGEN_USING_STD(abs);
     return abs(x);
   }
 };
@@ -360,7 +427,7 @@
 * Implementation of cast                                                 *
 ****************************************************************************/
 
-template<typename OldType, typename NewType>
+template<typename OldType, typename NewType, typename EnableIf = void>
 struct cast_impl
 {
   EIGEN_DEVICE_FUNC
@@ -370,6 +437,22 @@
   }
 };
 
+// Casting from S -> Complex<T> leads to an implicit conversion from S to T,
+// generating warnings on clang.  Here we explicitly cast the real component.
+template<typename OldType, typename NewType>
+struct cast_impl<OldType, NewType,
+  typename internal::enable_if<
+    !NumTraits<OldType>::IsComplex && NumTraits<NewType>::IsComplex
+  >::type>
+{
+  EIGEN_DEVICE_FUNC
+  static inline NewType run(const OldType& x)
+  {
+    typedef typename NumTraits<NewType>::Real NewReal;
+    return static_cast<NewType>(static_cast<NewReal>(x));
+  }
+};
+
 // here, for once, we're plainly returning NewType: we don't want cast to do weird things.
 
 template<typename OldType, typename NewType>
@@ -383,29 +466,59 @@
 * Implementation of round                                                   *
 ****************************************************************************/
 
-#if EIGEN_HAS_CXX11_MATH
-  template<typename Scalar>
-  struct round_impl {
-    static inline Scalar run(const Scalar& x)
-    {
-      EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
-      using std::round;
-      return round(x);
-    }
-  };
-#else
-  template<typename Scalar>
-  struct round_impl
+template<typename Scalar>
+struct round_impl
+{
+  EIGEN_DEVICE_FUNC
+  static inline Scalar run(const Scalar& x)
   {
-    static inline Scalar run(const Scalar& x)
-    {
-      EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
-      EIGEN_USING_STD_MATH(floor);
-      EIGEN_USING_STD_MATH(ceil);
-      return (x > Scalar(0)) ? floor(x + Scalar(0.5)) : ceil(x - Scalar(0.5));
-    }
-  };
+    EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
+#if EIGEN_HAS_CXX11_MATH
+    EIGEN_USING_STD(round);
 #endif
+    return Scalar(round(x));
+  }
+};
+
+#if !EIGEN_HAS_CXX11_MATH
+#if EIGEN_HAS_C99_MATH
+// Use ::roundf for float.
+template<>
+struct round_impl<float> {
+  EIGEN_DEVICE_FUNC
+  static inline float run(const float& x)
+  {
+    return ::roundf(x);
+  }
+};
+#else
+template<typename Scalar>
+struct round_using_floor_ceil_impl
+{
+  EIGEN_DEVICE_FUNC
+  static inline Scalar run(const Scalar& x)
+  {
+    EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
+    // Without C99 round/roundf, resort to floor/ceil.
+    EIGEN_USING_STD(floor);
+    EIGEN_USING_STD(ceil);
+    // If not enough precision to resolve a decimal at all, return the input.
+    // Otherwise, adding 0.5 can trigger an increment by 1.
+    const Scalar limit = Scalar(1ull << (NumTraits<Scalar>::digits() - 1));
+    if (x >= limit || x <= -limit) {
+      return x;
+    }
+    return (x > Scalar(0)) ? Scalar(floor(x + Scalar(0.5))) : Scalar(ceil(x - Scalar(0.5)));
+  }
+};
+
+template<>
+struct round_impl<float> : round_using_floor_ceil_impl<float> {};
+
+template<>
+struct round_impl<double> : round_using_floor_ceil_impl<double> {};
+#endif // EIGEN_HAS_C99_MATH
+#endif // !EIGEN_HAS_CXX11_MATH
 
 template<typename Scalar>
 struct round_retval
@@ -414,43 +527,112 @@
 };
 
 /****************************************************************************
+* Implementation of rint                                                    *
+****************************************************************************/
+
+template<typename Scalar>
+struct rint_impl {
+  EIGEN_DEVICE_FUNC
+  static inline Scalar run(const Scalar& x)
+  {
+    EIGEN_STATIC_ASSERT((!NumTraits<Scalar>::IsComplex), NUMERIC_TYPE_MUST_BE_REAL)
+#if EIGEN_HAS_CXX11_MATH
+      EIGEN_USING_STD(rint);
+#endif
+    return rint(x);
+  }
+};
+
+#if !EIGEN_HAS_CXX11_MATH
+template<>
+struct rint_impl<double> {
+  EIGEN_DEVICE_FUNC
+  static inline double run(const double& x)
+  {
+    return ::rint(x);
+  }
+};
+template<>
+struct rint_impl<float> {
+  EIGEN_DEVICE_FUNC
+  static inline float run(const float& x)
+  {
+    return ::rintf(x);
+  }
+};
+#endif
+
+template<typename Scalar>
+struct rint_retval
+{
+  typedef Scalar type;
+};
+
+/****************************************************************************
 * Implementation of arg                                                     *
 ****************************************************************************/
 
-#if EIGEN_HAS_CXX11_MATH
-  template<typename Scalar>
-  struct arg_impl {
-    static inline Scalar run(const Scalar& x)
-    {
-      EIGEN_USING_STD_MATH(arg);
-      return arg(x);
-    }
-  };
+// Visual Studio 2017 has a bug where arg(float) returns 0 for negative inputs.
+// This seems to be fixed in VS 2019.
+#if EIGEN_HAS_CXX11_MATH && (!EIGEN_COMP_MSVC || EIGEN_COMP_MSVC >= 1920)
+// std::arg is only defined for types of std::complex, or integer types or float/double/long double
+template<typename Scalar,
+          bool HasStdImpl = NumTraits<Scalar>::IsComplex || is_integral<Scalar>::value
+                            || is_same<Scalar, float>::value || is_same<Scalar, double>::value
+                            || is_same<Scalar, long double>::value >
+struct arg_default_impl;
+
+template<typename Scalar>
+struct arg_default_impl<Scalar, true> {
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_DEVICE_FUNC
+  static inline RealScalar run(const Scalar& x)
+  {
+    #if defined(EIGEN_HIP_DEVICE_COMPILE)
+    // HIP does not seem to have a native device side implementation for the math routine "arg"
+    using std::arg;
+    #else
+    EIGEN_USING_STD(arg);
+    #endif
+    return static_cast<RealScalar>(arg(x));
+  }
+};
+
+// Must be non-complex floating-point type (e.g. half/bfloat16).
+template<typename Scalar>
+struct arg_default_impl<Scalar, false> {
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_DEVICE_FUNC
+  static inline RealScalar run(const Scalar& x)
+  {
+    return (x < Scalar(0)) ? RealScalar(EIGEN_PI) : RealScalar(0);
+  }
+};
 #else
-  template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
-  struct arg_default_impl
+template<typename Scalar, bool IsComplex = NumTraits<Scalar>::IsComplex>
+struct arg_default_impl
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_DEVICE_FUNC
+  static inline RealScalar run(const Scalar& x)
   {
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    EIGEN_DEVICE_FUNC
-    static inline RealScalar run(const Scalar& x)
-    {
-      return (x < Scalar(0)) ? Scalar(EIGEN_PI) : Scalar(0); }
-  };
+    return (x < RealScalar(0)) ? RealScalar(EIGEN_PI) : RealScalar(0);
+  }
+};
 
-  template<typename Scalar>
-  struct arg_default_impl<Scalar,true>
+template<typename Scalar>
+struct arg_default_impl<Scalar,true>
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  EIGEN_DEVICE_FUNC
+  static inline RealScalar run(const Scalar& x)
   {
-    typedef typename NumTraits<Scalar>::Real RealScalar;
-    EIGEN_DEVICE_FUNC
-    static inline RealScalar run(const Scalar& x)
-    {
-      EIGEN_USING_STD_MATH(arg);
-      return arg(x);
-    }
-  };
-
-  template<typename Scalar> struct arg_impl : arg_default_impl<Scalar> {};
+    EIGEN_USING_STD(arg);
+    return arg(x);
+  }
+};
 #endif
+template<typename Scalar> struct arg_impl : arg_default_impl<Scalar> {};
 
 template<typename Scalar>
 struct arg_retval
@@ -459,6 +641,80 @@
 };
 
 /****************************************************************************
+* Implementation of expm1                                                   *
+****************************************************************************/
+
+// This implementation is based on GSL Math's expm1.
+namespace std_fallback {
+  // fallback expm1 implementation in case there is no expm1(Scalar) function in namespace of Scalar,
+  // or that there is no suitable std::expm1 function available. Implementation
+  // attributed to Kahan. See: http://www.plunk.org/~hatch/rightway.php.
+  template<typename Scalar>
+  EIGEN_DEVICE_FUNC inline Scalar expm1(const Scalar& x) {
+    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+    typedef typename NumTraits<Scalar>::Real RealScalar;
+
+    EIGEN_USING_STD(exp);
+    Scalar u = exp(x);
+    if (numext::equal_strict(u, Scalar(1))) {
+      return x;
+    }
+    Scalar um1 = u - RealScalar(1);
+    if (numext::equal_strict(um1, Scalar(-1))) {
+      return RealScalar(-1);
+    }
+
+    EIGEN_USING_STD(log);
+    Scalar logu = log(u);
+    return numext::equal_strict(u, logu) ? u : (u - RealScalar(1)) * x / logu;
+  }
+}
+
+template<typename Scalar>
+struct expm1_impl {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x)
+  {
+    EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
+    #if EIGEN_HAS_CXX11_MATH
+    using std::expm1;
+    #else
+    using std_fallback::expm1;
+    #endif
+    return expm1(x);
+  }
+};
+
+template<typename Scalar>
+struct expm1_retval
+{
+  typedef Scalar type;
+};
+
+/****************************************************************************
+* Implementation of log                                                     *
+****************************************************************************/
+
+// Complex log defined in MathFunctionsImpl.h.
+template<typename T> EIGEN_DEVICE_FUNC std::complex<T> complex_log(const std::complex<T>& z);
+
+template<typename Scalar>
+struct log_impl {
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x)
+  {
+    EIGEN_USING_STD(log);
+    return static_cast<Scalar>(log(x));
+  }
+};
+
+template<typename Scalar>
+struct log_impl<std::complex<Scalar> > {
+  EIGEN_DEVICE_FUNC static inline std::complex<Scalar> run(const std::complex<Scalar>& z)
+  {
+    return complex_log(z);
+  }
+};
+
+/****************************************************************************
 * Implementation of log1p                                                   *
 ****************************************************************************/
 
@@ -469,25 +725,38 @@
   EIGEN_DEVICE_FUNC inline Scalar log1p(const Scalar& x) {
     EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
     typedef typename NumTraits<Scalar>::Real RealScalar;
-    EIGEN_USING_STD_MATH(log);
+    EIGEN_USING_STD(log);
     Scalar x1p = RealScalar(1) + x;
-    return numext::equal_strict(x1p, Scalar(1)) ? x : x * ( log(x1p) / (x1p - RealScalar(1)) );
+    Scalar log_1p = log_impl<Scalar>::run(x1p);
+    const bool is_small = numext::equal_strict(x1p, Scalar(1));
+    const bool is_inf = numext::equal_strict(x1p, log_1p);
+    return (is_small || is_inf) ? x : x * (log_1p / (x1p - RealScalar(1)));
   }
 }
 
 template<typename Scalar>
 struct log1p_impl {
-  static inline Scalar run(const Scalar& x)
+  EIGEN_DEVICE_FUNC static inline Scalar run(const Scalar& x)
   {
     EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar)
     #if EIGEN_HAS_CXX11_MATH
     using std::log1p;
-    #endif
+    #else
     using std_fallback::log1p;
+    #endif
     return log1p(x);
   }
 };
 
+// Specialization for complex types that are not supported by std::log1p.
+template <typename RealScalar>
+struct log1p_impl<std::complex<RealScalar> > {
+  EIGEN_DEVICE_FUNC static inline std::complex<RealScalar> run(
+      const std::complex<RealScalar>& x) {
+    EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar)
+    return std_fallback::log1p(x);
+  }
+};
 
 template<typename Scalar>
 struct log1p_retval
@@ -506,7 +775,7 @@
   typedef typename ScalarBinaryOpTraits<ScalarX,ScalarY,internal::scalar_pow_op<ScalarX,ScalarY> >::ReturnType result_type;
   static EIGEN_DEVICE_FUNC inline result_type run(const ScalarX& x, const ScalarY& y)
   {
-    EIGEN_USING_STD_MATH(pow);
+    EIGEN_USING_STD(pow);
     return pow(x, y);
   }
 };
@@ -662,8 +931,8 @@
 {
   static inline Scalar run(const Scalar& x, const Scalar& y)
   {
-    return Scalar(random(real(x), real(y)),
-                  random(imag(x), imag(y)));
+    return Scalar(random(x.real(), y.real()),
+                  random(x.imag(), y.imag()));
   }
   static inline Scalar run()
   {
@@ -684,7 +953,7 @@
   return EIGEN_MATHFUNC_IMPL(random, Scalar)::run();
 }
 
-// Implementatin of is* functions
+// Implementation of is* functions
 
 // std::is* do not work with fast-math and gcc, std::is* are available on MSVC 2013 and newer, as well as in clang.
 #if (EIGEN_HAS_CXX11_MATH && !(EIGEN_COMP_GNUC_STRICT && __FINITE_MATH_ONLY__)) || (EIGEN_COMP_MSVC>=1800) || (EIGEN_COMP_CLANG)
@@ -713,7 +982,7 @@
 typename internal::enable_if<(!internal::is_integral<T>::value)&&(!NumTraits<T>::IsComplex),bool>::type
 isfinite_impl(const T& x)
 {
-  #ifdef __CUDA_ARCH__
+  #if defined(EIGEN_GPU_COMPILE_PHASE)
     return (::isfinite)(x);
   #elif EIGEN_USE_STD_FPCLASSIFY
     using std::isfinite;
@@ -728,7 +997,7 @@
 typename internal::enable_if<(!internal::is_integral<T>::value)&&(!NumTraits<T>::IsComplex),bool>::type
 isinf_impl(const T& x)
 {
-  #ifdef __CUDA_ARCH__
+  #if defined(EIGEN_GPU_COMPILE_PHASE)
     return (::isinf)(x);
   #elif EIGEN_USE_STD_FPCLASSIFY
     using std::isinf;
@@ -743,7 +1012,7 @@
 typename internal::enable_if<(!internal::is_integral<T>::value)&&(!NumTraits<T>::IsComplex),bool>::type
 isnan_impl(const T& x)
 {
-  #ifdef __CUDA_ARCH__
+  #if defined(EIGEN_GPU_COMPILE_PHASE)
     return (::isnan)(x);
   #elif EIGEN_USE_STD_FPCLASSIFY
     using std::isnan;
@@ -800,7 +1069,6 @@
 template<typename T> EIGEN_DEVICE_FUNC bool isinf_impl(const std::complex<T>& x);
 
 template<typename T> T generic_fast_tanh_float(const T& a_x);
-
 } // end namespace internal
 
 /****************************************************************************
@@ -809,12 +1077,12 @@
 
 namespace numext {
 
-#ifndef __CUDA_ARCH__
+#if (!defined(EIGEN_GPUCC) || defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC))
 template<typename T>
 EIGEN_DEVICE_FUNC
 EIGEN_ALWAYS_INLINE T mini(const T& x, const T& y)
 {
-  EIGEN_USING_STD_MATH(min);
+  EIGEN_USING_STD(min)
   return min EIGEN_NOT_A_MACRO (x,y);
 }
 
@@ -822,7 +1090,7 @@
 EIGEN_DEVICE_FUNC
 EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y)
 {
-  EIGEN_USING_STD_MATH(max);
+  EIGEN_USING_STD(max)
   return max EIGEN_NOT_A_MACRO (x,y);
 }
 #else
@@ -838,6 +1106,24 @@
 {
   return fminf(x, y);
 }
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE double mini(const double& x, const double& y)
+{
+  return fmin(x, y);
+}
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE long double mini(const long double& x, const long double& y)
+{
+#if defined(EIGEN_HIPCC)
+  // no "fminl" on HIP yet
+  return (x < y) ? x : y;
+#else
+  return fminl(x, y);
+#endif
+}
+
 template<typename T>
 EIGEN_DEVICE_FUNC
 EIGEN_ALWAYS_INLINE T maxi(const T& x, const T& y)
@@ -850,6 +1136,92 @@
 {
   return fmaxf(x, y);
 }
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE double maxi(const double& x, const double& y)
+{
+  return fmax(x, y);
+}
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE long double maxi(const long double& x, const long double& y)
+{
+#if defined(EIGEN_HIPCC)
+  // no "fmaxl" on HIP yet
+  return (x > y) ? x : y;
+#else
+  return fmaxl(x, y);
+#endif
+}
+#endif
+
+#if defined(SYCL_DEVICE_ONLY)
+
+
+#define SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_char)   \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_short)  \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_int)    \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_long)
+#define SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_char)   \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_short)  \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_int)    \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_long)
+#define SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar)  \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort) \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_uint)   \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_ulong)
+#define SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uchar)  \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ushort) \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_uint)   \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_ulong)
+#define SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY(NAME, FUNC)
+#define SYCL_SPECIALIZE_INTEGER_TYPES_UNARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY(NAME, FUNC)
+#define SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, cl::sycl::cl_float) \
+  SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC,cl::sycl::cl_double)
+#define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(NAME, FUNC) \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, cl::sycl::cl_float) \
+  SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC,cl::sycl::cl_double)
+#define SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(NAME, FUNC, RET_TYPE) \
+  SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_float) \
+  SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, cl::sycl::cl_double)
+
+#define SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE) \
+template<>                                               \
+  EIGEN_DEVICE_FUNC                                      \
+  EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE& x) { \
+    return cl::sycl::FUNC(x);                            \
+  }
+
+#define SYCL_SPECIALIZE_UNARY_FUNC(NAME, FUNC, TYPE) \
+  SYCL_SPECIALIZE_GEN_UNARY_FUNC(NAME, FUNC, TYPE, TYPE)
+
+#define SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE1, ARG_TYPE2) \
+  template<>                                                                  \
+  EIGEN_DEVICE_FUNC                                                           \
+  EIGEN_ALWAYS_INLINE RET_TYPE NAME(const ARG_TYPE1& x, const ARG_TYPE2& y) { \
+    return cl::sycl::FUNC(x, y);                                              \
+  }
+
+#define SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE) \
+  SYCL_SPECIALIZE_GEN1_BINARY_FUNC(NAME, FUNC, RET_TYPE, ARG_TYPE, ARG_TYPE)
+
+#define SYCL_SPECIALIZE_BINARY_FUNC(NAME, FUNC, TYPE) \
+  SYCL_SPECIALIZE_GEN2_BINARY_FUNC(NAME, FUNC, TYPE, TYPE)
+
+SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(mini, min)
+SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(mini, fmin)
+SYCL_SPECIALIZE_INTEGER_TYPES_BINARY(maxi, max)
+SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(maxi, fmax)
+
 #endif
 
 
@@ -916,6 +1288,37 @@
   return EIGEN_MATHFUNC_IMPL(abs2, Scalar)::run(x);
 }
 
+EIGEN_DEVICE_FUNC
+inline bool abs2(bool x) { return x; }
+
+template<typename T>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE T absdiff(const T& x, const T& y)
+{
+  return x > y ? x - y : y - x;
+}
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE float absdiff(const float& x, const float& y)
+{
+  return fabsf(x - y);
+}
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE double absdiff(const double& x, const double& y)
+{
+  return fabs(x - y);
+}
+
+#if !defined(EIGEN_GPUCC)
+// HIP and CUDA do not support long double.
+template<>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE long double absdiff(const long double& x, const long double& y) {
+  return fabsl(x - y);
+}
+#endif
+
 template<typename Scalar>
 EIGEN_DEVICE_FUNC
 inline EIGEN_MATHFUNC_RETVAL(norm1, Scalar) norm1(const Scalar& x)
@@ -930,6 +1333,10 @@
   return EIGEN_MATHFUNC_IMPL(hypot, Scalar)::run(x, y);
 }
 
+#if defined(SYCL_DEVICE_ONLY)
+  SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(hypot, hypot)
+#endif
+
 template<typename Scalar>
 EIGEN_DEVICE_FUNC
 inline EIGEN_MATHFUNC_RETVAL(log1p, Scalar) log1p(const Scalar& x)
@@ -937,7 +1344,11 @@
   return EIGEN_MATHFUNC_IMPL(log1p, Scalar)::run(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(log1p, log1p)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float log1p(const float &x) { return ::log1pf(x); }
 
@@ -952,10 +1363,27 @@
   return internal::pow_impl<ScalarX,ScalarY>::run(x, y);
 }
 
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(pow, pow)
+#endif
+
 template<typename T> EIGEN_DEVICE_FUNC bool (isnan)   (const T &x) { return internal::isnan_impl(x); }
 template<typename T> EIGEN_DEVICE_FUNC bool (isinf)   (const T &x) { return internal::isinf_impl(x); }
 template<typename T> EIGEN_DEVICE_FUNC bool (isfinite)(const T &x) { return internal::isfinite_impl(x); }
 
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isnan, isnan, bool)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isinf, isinf, bool)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE(isfinite, isfinite, bool)
+#endif
+
+template<typename Scalar>
+EIGEN_DEVICE_FUNC
+inline EIGEN_MATHFUNC_RETVAL(rint, Scalar) rint(const Scalar& x)
+{
+  return EIGEN_MATHFUNC_IMPL(rint, Scalar)::run(x);
+}
+
 template<typename Scalar>
 EIGEN_DEVICE_FUNC
 inline EIGEN_MATHFUNC_RETVAL(round, Scalar) round(const Scalar& x)
@@ -963,15 +1391,23 @@
   return EIGEN_MATHFUNC_IMPL(round, Scalar)::run(x);
 }
 
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(round, round)
+#endif
+
 template<typename T>
 EIGEN_DEVICE_FUNC
 T (floor)(const T& x)
 {
-  EIGEN_USING_STD_MATH(floor);
+  EIGEN_USING_STD(floor)
   return floor(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(floor, floor)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float floor(const float &x) { return ::floorf(x); }
 
@@ -983,11 +1419,15 @@
 EIGEN_DEVICE_FUNC
 T (ceil)(const T& x)
 {
-  EIGEN_USING_STD_MATH(ceil);
+  EIGEN_USING_STD(ceil);
   return ceil(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(ceil, ceil)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float ceil(const float &x) { return ::ceilf(x); }
 
@@ -1020,22 +1460,42 @@
   *
   * It's usage is justified in performance critical functions, like norm/normalize.
   */
+template<typename Scalar>
+EIGEN_DEVICE_FUNC
+EIGEN_ALWAYS_INLINE EIGEN_MATHFUNC_RETVAL(sqrt, Scalar) sqrt(const Scalar& x)
+{
+  return EIGEN_MATHFUNC_IMPL(sqrt, Scalar)::run(x);
+}
+
+// Boolean specialization, avoids implicit float to bool conversion (-Wimplicit-conversion-floating-point-to-bool).
+template<>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC
+bool sqrt<bool>(const bool &x) { return x; }
+
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sqrt, sqrt)
+#endif
+
+/** \returns the reciprocal square root of \a x. **/
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
-T sqrt(const T &x)
+T rsqrt(const T& x)
 {
-  EIGEN_USING_STD_MATH(sqrt);
-  return sqrt(x);
+  return internal::rsqrt_impl<T>::run(x);
 }
 
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T log(const T &x) {
-  EIGEN_USING_STD_MATH(log);
-  return log(x);
+  return internal::log_impl<T>::run(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(log, log)
+#endif
+
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float log(const float &x) { return ::logf(x); }
 
@@ -1047,7 +1507,7 @@
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 typename internal::enable_if<NumTraits<T>::IsSigned || NumTraits<T>::IsComplex,typename NumTraits<T>::Real>::type
 abs(const T &x) {
-  EIGEN_USING_STD_MATH(abs);
+  EIGEN_USING_STD(abs);
   return abs(x);
 }
 
@@ -1058,12 +1518,12 @@
   return x;
 }
 
-#if defined(__SYCL_DEVICE_ONLY__)
-EIGEN_ALWAYS_INLINE float   abs(float x) { return cl::sycl::fabs(x); }
-EIGEN_ALWAYS_INLINE double  abs(double x) { return cl::sycl::fabs(x); }
-#endif // defined(__SYCL_DEVICE_ONLY__)
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_INTEGER_TYPES_UNARY(abs, abs)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(abs, fabs)
+#endif
 
-#ifdef __CUDACC__
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float abs(const float &x) { return ::fabsf(x); }
 
@@ -1084,26 +1544,69 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T exp(const T &x) {
-  EIGEN_USING_STD_MATH(exp);
+  EIGEN_USING_STD(exp);
   return exp(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(exp, exp)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float exp(const float &x) { return ::expf(x); }
 
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 double exp(const double &x) { return ::exp(x); }
+
+template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+std::complex<float> exp(const std::complex<float>& x) {
+  float com = ::expf(x.real());
+  float res_real = com * ::cosf(x.imag());
+  float res_imag = com * ::sinf(x.imag());
+  return std::complex<float>(res_real, res_imag);
+}
+
+template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+std::complex<double> exp(const std::complex<double>& x) {
+  double com = ::exp(x.real());
+  double res_real = com * ::cos(x.imag());
+  double res_imag = com * ::sin(x.imag());
+  return std::complex<double>(res_real, res_imag);
+}
+#endif
+
+template<typename Scalar>
+EIGEN_DEVICE_FUNC
+inline EIGEN_MATHFUNC_RETVAL(expm1, Scalar) expm1(const Scalar& x)
+{
+  return EIGEN_MATHFUNC_IMPL(expm1, Scalar)::run(x);
+}
+
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(expm1, expm1)
+#endif
+
+#if defined(EIGEN_GPUCC)
+template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+float expm1(const float &x) { return ::expm1f(x); }
+
+template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+double expm1(const double &x) { return ::expm1(x); }
 #endif
 
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T cos(const T &x) {
-  EIGEN_USING_STD_MATH(cos);
+  EIGEN_USING_STD(cos);
   return cos(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(cos,cos)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float cos(const float &x) { return ::cosf(x); }
 
@@ -1114,11 +1617,15 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T sin(const T &x) {
-  EIGEN_USING_STD_MATH(sin);
+  EIGEN_USING_STD(sin);
   return sin(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sin, sin)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float sin(const float &x) { return ::sinf(x); }
 
@@ -1129,11 +1636,15 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T tan(const T &x) {
-  EIGEN_USING_STD_MATH(tan);
+  EIGEN_USING_STD(tan);
   return tan(x);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(tan, tan)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float tan(const float &x) { return ::tanf(x); }
 
@@ -1144,11 +1655,25 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T acos(const T &x) {
-  EIGEN_USING_STD_MATH(acos);
+  EIGEN_USING_STD(acos);
   return acos(x);
 }
 
-#ifdef __CUDACC__
+#if EIGEN_HAS_CXX11_MATH
+template<typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+T acosh(const T &x) {
+  EIGEN_USING_STD(acosh);
+  return static_cast<T>(acosh(x));
+}
+#endif
+
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(acos, acos)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(acosh, acosh)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float acos(const float &x) { return ::acosf(x); }
 
@@ -1159,11 +1684,25 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T asin(const T &x) {
-  EIGEN_USING_STD_MATH(asin);
+  EIGEN_USING_STD(asin);
   return asin(x);
 }
 
-#ifdef __CUDACC__
+#if EIGEN_HAS_CXX11_MATH
+template<typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+T asinh(const T &x) {
+  EIGEN_USING_STD(asinh);
+  return static_cast<T>(asinh(x));
+}
+#endif
+
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(asin, asin)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(asinh, asinh)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float asin(const float &x) { return ::asinf(x); }
 
@@ -1174,11 +1713,25 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T atan(const T &x) {
-  EIGEN_USING_STD_MATH(atan);
-  return atan(x);
+  EIGEN_USING_STD(atan);
+  return static_cast<T>(atan(x));
 }
 
-#ifdef __CUDACC__
+#if EIGEN_HAS_CXX11_MATH
+template<typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+T atanh(const T &x) {
+  EIGEN_USING_STD(atanh);
+  return static_cast<T>(atanh(x));
+}
+#endif
+
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(atan, atan)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(atanh, atanh)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float atan(const float &x) { return ::atanf(x); }
 
@@ -1190,11 +1743,15 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T cosh(const T &x) {
-  EIGEN_USING_STD_MATH(cosh);
-  return cosh(x);
+  EIGEN_USING_STD(cosh);
+  return static_cast<T>(cosh(x));
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(cosh, cosh)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float cosh(const float &x) { return ::coshf(x); }
 
@@ -1205,11 +1762,15 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T sinh(const T &x) {
-  EIGEN_USING_STD_MATH(sinh);
-  return sinh(x);
+  EIGEN_USING_STD(sinh);
+  return static_cast<T>(sinh(x));
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(sinh, sinh)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float sinh(const float &x) { return ::sinhf(x); }
 
@@ -1220,16 +1781,20 @@
 template<typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T tanh(const T &x) {
-  EIGEN_USING_STD_MATH(tanh);
+  EIGEN_USING_STD(tanh);
   return tanh(x);
 }
 
-#if (!defined(__CUDACC__)) && EIGEN_FAST_MATH
+#if (!defined(EIGEN_GPUCC)) && EIGEN_FAST_MATH && !defined(SYCL_DEVICE_ONLY)
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float tanh(float x) { return internal::generic_fast_tanh_float(x); }
 #endif
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_UNARY(tanh, tanh)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template<> EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float tanh(const float &x) { return ::tanhf(x); }
 
@@ -1240,11 +1805,15 @@
 template <typename T>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 T fmod(const T& a, const T& b) {
-  EIGEN_USING_STD_MATH(fmod);
+  EIGEN_USING_STD(fmod);
   return fmod(a, b);
 }
 
-#ifdef __CUDACC__
+#if defined(SYCL_DEVICE_ONLY)
+SYCL_SPECIALIZE_FLOATING_TYPES_BINARY(fmod, fmod)
+#endif
+
+#if defined(EIGEN_GPUCC)
 template <>
 EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
 float fmod(const float& a, const float& b) {
@@ -1258,6 +1827,23 @@
 }
 #endif
 
+#if defined(SYCL_DEVICE_ONLY)
+#undef SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_BINARY
+#undef SYCL_SPECIALIZE_SIGNED_INTEGER_TYPES_UNARY
+#undef SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_BINARY
+#undef SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY
+#undef SYCL_SPECIALIZE_INTEGER_TYPES_BINARY
+#undef SYCL_SPECIALIZE_UNSIGNED_INTEGER_TYPES_UNARY
+#undef SYCL_SPECIALIZE_FLOATING_TYPES_BINARY
+#undef SYCL_SPECIALIZE_FLOATING_TYPES_UNARY
+#undef SYCL_SPECIALIZE_FLOATING_TYPES_UNARY_FUNC_RET_TYPE
+#undef SYCL_SPECIALIZE_GEN_UNARY_FUNC
+#undef SYCL_SPECIALIZE_UNARY_FUNC
+#undef SYCL_SPECIALIZE_GEN1_BINARY_FUNC
+#undef SYCL_SPECIALIZE_GEN2_BINARY_FUNC
+#undef SYCL_SPECIALIZE_BINARY_FUNC
+#endif
+
 } // end namespace numext
 
 namespace internal {
@@ -1381,18 +1967,23 @@
   {
     return random<int>(0,1)==0 ? false : true;
   }
+
+  static inline bool run(const bool& a, const bool& b)
+  {
+    return random<int>(a, b)==0 ? false : true;
+  }
 };
 
 template<> struct scalar_fuzzy_impl<bool>
 {
   typedef bool RealScalar;
-  
+
   template<typename OtherScalar> EIGEN_DEVICE_FUNC
   static inline bool isMuchSmallerThan(const bool& x, const bool&, const bool&)
   {
     return !x;
   }
-  
+
   EIGEN_DEVICE_FUNC
   static inline bool isApprox(bool x, bool y, bool)
   {
@@ -1404,10 +1995,61 @@
   {
     return (!x) || y;
   }
-  
+
 };
 
-  
+} // end namespace internal
+
+// Default implementations that rely on other numext implementations
+namespace internal {
+
+// Specialization for complex types that are not supported by std::expm1.
+template <typename RealScalar>
+struct expm1_impl<std::complex<RealScalar> > {
+  EIGEN_DEVICE_FUNC static inline std::complex<RealScalar> run(
+      const std::complex<RealScalar>& x) {
+    EIGEN_STATIC_ASSERT_NON_INTEGER(RealScalar)
+    RealScalar xr = x.real();
+    RealScalar xi = x.imag();
+    // expm1(z) = exp(z) - 1
+    //          = exp(x +  i * y) - 1
+    //          = exp(x) * (cos(y) + i * sin(y)) - 1
+    //          = exp(x) * cos(y) - 1 + i * exp(x) * sin(y)
+    // Imag(expm1(z)) = exp(x) * sin(y)
+    // Real(expm1(z)) = exp(x) * cos(y) - 1
+    //          = exp(x) * cos(y) - 1.
+    //          = expm1(x) + exp(x) * (cos(y) - 1)
+    //          = expm1(x) + exp(x) * (2 * sin(y / 2) ** 2)
+    RealScalar erm1 = numext::expm1<RealScalar>(xr);
+    RealScalar er = erm1 + RealScalar(1.);
+    RealScalar sin2 = numext::sin(xi / RealScalar(2.));
+    sin2 = sin2 * sin2;
+    RealScalar s = numext::sin(xi);
+    RealScalar real_part = erm1 - RealScalar(2.) * er * sin2;
+    return std::complex<RealScalar>(real_part, er * s);
+  }
+};
+
+template<typename T>
+struct rsqrt_impl {
+  EIGEN_DEVICE_FUNC
+  static EIGEN_ALWAYS_INLINE T run(const T& x) {
+    return T(1)/numext::sqrt(x);
+  }
+};
+
+#if defined(EIGEN_GPU_COMPILE_PHASE)
+template<typename T>
+struct conj_impl<std::complex<T>, true>
+{
+  EIGEN_DEVICE_FUNC
+  static inline std::complex<T> run(const std::complex<T>& x)
+  {
+    return std::complex<T>(numext::real(x), -numext::imag(x));
+  }
+};
+#endif
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
index 9c1ceb0..4eaaaa7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MathFunctionsImpl.h
@@ -17,24 +17,28 @@
 
 /** \internal \returns the hyperbolic tan of \a a (coeff-wise)
     Doesn't do anything fancy, just a 13/6-degree rational interpolant which
-    is accurate up to a couple of ulp in the range [-9, 9], outside of which
-    the tanh(x) = +/-1.
+    is accurate up to a couple of ulps in the (approximate) range [-8, 8],
+    outside of which tanh(x) = +/-1 in single precision. The input is clamped
+    to the range [-c, c]. The value c is chosen as the smallest value where
+    the approximation evaluates to exactly 1. In the reange [-0.0004, 0.0004]
+    the approxmation tanh(x) ~= x is used for better accuracy as x tends to zero.
 
     This implementation works on both scalars and packets.
 */
 template<typename T>
 T generic_fast_tanh_float(const T& a_x)
 {
-  // Clamp the inputs to the range [-9, 9] since anything outside
-  // this range is +/-1.0f in single-precision.
-  const T plus_9 = pset1<T>(9.f);
-  const T minus_9 = pset1<T>(-9.f);
-  // NOTE GCC prior to 6.3 might improperly optimize this max/min
-  //      step such that if a_x is nan, x will be either 9 or -9,
-  //      and tanh will return 1 or -1 instead of nan.
-  //      This is supposed to be fixed in gcc6.3,
-  //      see: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
-  const T x = pmax(minus_9,pmin(plus_9,a_x));
+  // Clamp the inputs to the range [-c, c]
+#ifdef EIGEN_VECTORIZE_FMA
+  const T plus_clamp = pset1<T>(7.99881172180175781f);
+  const T minus_clamp = pset1<T>(-7.99881172180175781f);
+#else
+  const T plus_clamp = pset1<T>(7.90531110763549805f);
+  const T minus_clamp = pset1<T>(-7.90531110763549805f);
+#endif
+  const T tiny = pset1<T>(0.0004f);
+  const T x = pmax(pmin(a_x, plus_clamp), minus_clamp);
+  const T tiny_mask = pcmp_lt(pabs(a_x), tiny);
   // The monomial coefficients of the numerator polynomial (odd).
   const T alpha_1 = pset1<T>(4.89352455891786e-03f);
   const T alpha_3 = pset1<T>(6.37261928875436e-04f);
@@ -62,24 +66,30 @@
   p = pmadd(x2, p, alpha_1);
   p = pmul(x, p);
 
-  // Evaluate the denominator polynomial p.
+  // Evaluate the denominator polynomial q.
   T q = pmadd(x2, beta_6, beta_4);
   q = pmadd(x2, q, beta_2);
   q = pmadd(x2, q, beta_0);
 
   // Divide the numerator by the denominator.
-  return pdiv(p, q);
+  return pselect(tiny_mask, x, pdiv(p, q));
 }
 
 template<typename RealScalar>
-EIGEN_STRONG_INLINE
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
 RealScalar positive_real_hypot(const RealScalar& x, const RealScalar& y)
 {
-  EIGEN_USING_STD_MATH(sqrt);
+  // IEEE IEC 6059 special cases.
+  if ((numext::isinf)(x) || (numext::isinf)(y))
+    return NumTraits<RealScalar>::infinity();
+  if ((numext::isnan)(x) || (numext::isnan)(y))
+    return NumTraits<RealScalar>::quiet_NaN();
+    
+  EIGEN_USING_STD(sqrt);
   RealScalar p, qp;
   p = numext::maxi(x,y);
   if(p==RealScalar(0)) return RealScalar(0);
-  qp = numext::mini(y,x) / p;    
+  qp = numext::mini(y,x) / p;
   return p * sqrt(RealScalar(1) + qp*qp);
 }
 
@@ -87,13 +97,102 @@
 struct hypot_impl
 {
   typedef typename NumTraits<Scalar>::Real RealScalar;
-  static inline RealScalar run(const Scalar& x, const Scalar& y)
+  static EIGEN_DEVICE_FUNC
+  inline RealScalar run(const Scalar& x, const Scalar& y)
   {
-    EIGEN_USING_STD_MATH(abs);
+    EIGEN_USING_STD(abs);
     return positive_real_hypot<RealScalar>(abs(x), abs(y));
   }
 };
 
+// Generic complex sqrt implementation that correctly handles corner cases
+// according to https://en.cppreference.com/w/cpp/numeric/complex/sqrt
+template<typename T>
+EIGEN_DEVICE_FUNC std::complex<T> complex_sqrt(const std::complex<T>& z) {
+  // Computes the principal sqrt of the input.
+  //
+  // For a complex square root of the number x + i*y. We want to find real
+  // numbers u and v such that
+  //    (u + i*v)^2 = x + i*y  <=>
+  //    u^2 - v^2 + i*2*u*v = x + i*v.
+  // By equating the real and imaginary parts we get:
+  //    u^2 - v^2 = x
+  //    2*u*v = y.
+  //
+  // For x >= 0, this has the numerically stable solution
+  //    u = sqrt(0.5 * (x + sqrt(x^2 + y^2)))
+  //    v = y / (2 * u)
+  // and for x < 0,
+  //    v = sign(y) * sqrt(0.5 * (-x + sqrt(x^2 + y^2)))
+  //    u = y / (2 * v)
+  //
+  // Letting w = sqrt(0.5 * (|x| + |z|)),
+  //   if x == 0: u = w, v = sign(y) * w
+  //   if x > 0:  u = w, v = y / (2 * w)
+  //   if x < 0:  u = |y| / (2 * w), v = sign(y) * w
+
+  const T x = numext::real(z);
+  const T y = numext::imag(z);
+  const T zero = T(0);
+  const T w = numext::sqrt(T(0.5) * (numext::abs(x) + numext::hypot(x, y)));
+
+  return
+    (numext::isinf)(y) ? std::complex<T>(NumTraits<T>::infinity(), y)
+      : x == zero ? std::complex<T>(w, y < zero ? -w : w)
+      : x > zero ? std::complex<T>(w, y / (2 * w))
+      : std::complex<T>(numext::abs(y) / (2 * w), y < zero ? -w : w );
+}
+
+// Generic complex rsqrt implementation.
+template<typename T>
+EIGEN_DEVICE_FUNC std::complex<T> complex_rsqrt(const std::complex<T>& z) {
+  // Computes the principal reciprocal sqrt of the input.
+  //
+  // For a complex reciprocal square root of the number z = x + i*y. We want to
+  // find real numbers u and v such that
+  //    (u + i*v)^2 = 1 / (x + i*y)  <=>
+  //    u^2 - v^2 + i*2*u*v = x/|z|^2 - i*v/|z|^2.
+  // By equating the real and imaginary parts we get:
+  //    u^2 - v^2 = x/|z|^2
+  //    2*u*v = y/|z|^2.
+  //
+  // For x >= 0, this has the numerically stable solution
+  //    u = sqrt(0.5 * (x + |z|)) / |z|
+  //    v = -y / (2 * u * |z|)
+  // and for x < 0,
+  //    v = -sign(y) * sqrt(0.5 * (-x + |z|)) / |z|
+  //    u = -y / (2 * v * |z|)
+  //
+  // Letting w = sqrt(0.5 * (|x| + |z|)),
+  //   if x == 0: u = w / |z|, v = -sign(y) * w / |z|
+  //   if x > 0:  u = w / |z|, v = -y / (2 * w * |z|)
+  //   if x < 0:  u = |y| / (2 * w * |z|), v = -sign(y) * w / |z|
+
+  const T x = numext::real(z);
+  const T y = numext::imag(z);
+  const T zero = T(0);
+
+  const T abs_z = numext::hypot(x, y);
+  const T w = numext::sqrt(T(0.5) * (numext::abs(x) + abs_z));
+  const T woz = w / abs_z;
+  // Corner cases consistent with 1/sqrt(z) on gcc/clang.
+  return
+    abs_z == zero ? std::complex<T>(NumTraits<T>::infinity(), NumTraits<T>::quiet_NaN())
+      : ((numext::isinf)(x) || (numext::isinf)(y)) ? std::complex<T>(zero, zero)
+      : x == zero ? std::complex<T>(woz, y < zero ? woz : -woz)
+      : x > zero ? std::complex<T>(woz, -y / (2 * w * abs_z))
+      : std::complex<T>(numext::abs(y) / (2 * w * abs_z), y < zero ? woz : -woz );
+}
+
+template<typename T>
+EIGEN_DEVICE_FUNC std::complex<T> complex_log(const std::complex<T>& z) {
+  // Computes complex log.
+  T a = numext::abs(z);
+  EIGEN_USING_STD(atan2);
+  T b = atan2(z.imag(), z.real());
+  return std::complex<T>(numext::log(a), b);
+}
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
index 7f4a7af..f0e59a9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Matrix.h
@@ -29,7 +29,7 @@
       required_alignment = unpacket_traits<PacketScalar>::alignment,
       packet_access_bit = (packet_traits<_Scalar>::Vectorizable && (EIGEN_UNALIGNED_VECTORIZE || (actual_alignment>=required_alignment))) ? PacketAccessBit : 0
     };
-    
+
 public:
   typedef _Scalar Scalar;
   typedef Dense StorageKind;
@@ -44,7 +44,7 @@
     Options = _Options,
     InnerStrideAtCompileTime = 1,
     OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime,
-    
+
     // FIXME, the following flag in only used to define NeedsToAlign in PlainObjectBase
     EvaluatorFlags = LinearAccessBit | DirectAccessBit | packet_access_bit | row_major_bit,
     Alignment = actual_alignment
@@ -255,53 +255,93 @@
       *
       * \sa resize(Index,Index)
       */
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix() : Base()
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Matrix() : Base()
     {
       Base::_check_template_params();
       EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
     }
 
     // FIXME is it still needed
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     explicit Matrix(internal::constructor_without_unaligned_array_assert)
       : Base(internal::constructor_without_unaligned_array_assert())
     { Base::_check_template_params(); EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED }
 
 #if EIGEN_HAS_RVALUE_REFERENCES
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     Matrix(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_constructible<Scalar>::value)
       : Base(std::move(other))
     {
       Base::_check_template_params();
     }
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     Matrix& operator=(Matrix&& other) EIGEN_NOEXCEPT_IF(std::is_nothrow_move_assignable<Scalar>::value)
     {
-      other.swap(*this);
+      Base::operator=(std::move(other));
       return *this;
     }
 #endif
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
+#if EIGEN_HAS_CXX11
+    /** \copydoc PlainObjectBase(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&... args)
+     *
+     * Example: \include Matrix_variadic_ctor_cxx11.cpp
+     * Output: \verbinclude Matrix_variadic_ctor_cxx11.out
+     *
+     * \sa Matrix(const std::initializer_list<std::initializer_list<Scalar>>&)
+     */
+    template <typename... ArgTypes>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
+      : Base(a0, a1, a2, a3, args...) {}
+
+    /** \brief Constructs a Matrix and initializes it from the coefficients given as initializer-lists grouped by row. \cpp11
+      *
+      * In the general case, the constructor takes a list of rows, each row being represented as a list of coefficients:
+      *
+      * Example: \include Matrix_initializer_list_23_cxx11.cpp
+      * Output: \verbinclude Matrix_initializer_list_23_cxx11.out
+      *
+      * Each of the inner initializer lists must contain the exact same number of elements, otherwise an assertion is triggered.
+      *
+      * In the case of a compile-time column vector, implicit transposition from a single row is allowed.
+      * Therefore <code>VectorXd{{1,2,3,4,5}}</code> is legal and the more verbose syntax
+      * <code>RowVectorXd{{1},{2},{3},{4},{5}}</code> can be avoided:
+      *
+      * Example: \include Matrix_initializer_list_vector_cxx11.cpp
+      * Output: \verbinclude Matrix_initializer_list_vector_cxx11.out
+      *
+      * In the case of fixed-sized matrices, the initializer list sizes must exactly match the matrix sizes,
+      * and implicit transposition is allowed for compile-time vectors only.
+      *
+      * \sa Matrix(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
+      */
+    EIGEN_DEVICE_FUNC
+    explicit EIGEN_STRONG_INLINE Matrix(const std::initializer_list<std::initializer_list<Scalar>>& list) : Base(list) {}
+#endif // end EIGEN_HAS_CXX11
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
 
     // This constructor is for both 1x1 matrices and dynamic vectors
     template<typename T>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE explicit Matrix(const T& x)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    explicit Matrix(const T& x)
     {
       Base::_check_template_params();
       Base::template _init1<T>(x);
     }
 
     template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Matrix(const T0& x, const T1& y)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Matrix(const T0& x, const T1& y)
     {
       Base::_check_template_params();
       Base::template _init2<T0,T1>(x, y);
     }
-    #else
+
+
+#else
     /** \brief Constructs a fixed-sized matrix initialized with coefficients starting at \a data */
     EIGEN_DEVICE_FUNC
     explicit Matrix(const Scalar *data);
@@ -311,7 +351,7 @@
       * This is useful for dynamic-size vectors. For fixed-size vectors,
       * it is redundant to pass these parameters, so one should use the default constructor
       * Matrix() instead.
-      * 
+      *
       * \warning This constructor is disabled for fixed-size \c 1x1 matrices. For instance,
       * calling Matrix<double,1,1>(1) will call the initialization constructor: Matrix(const Scalar&).
       * For fixed-size \c 1x1 matrices it is therefore recommended to use the default
@@ -319,14 +359,15 @@
       * \c EIGEN_INITIALIZE_MATRICES_BY_{ZERO,\c NAN} macros (see \ref TopicPreprocessorDirectives).
       */
     EIGEN_STRONG_INLINE explicit Matrix(Index dim);
-    /** \brief Constructs an initialized 1x1 matrix with the given coefficient */
+    /** \brief Constructs an initialized 1x1 matrix with the given coefficient
+      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...) */
     Matrix(const Scalar& x);
     /** \brief Constructs an uninitialized matrix with \a rows rows and \a cols columns.
       *
       * This is useful for dynamic-size matrices. For fixed-size matrices,
       * it is redundant to pass these parameters, so one should use the default constructor
       * Matrix() instead.
-      * 
+      *
       * \warning This constructor is disabled for fixed-size \c 1x2 and \c 2x1 vectors. For instance,
       * calling Matrix2f(2,1) will call the initialization constructor: Matrix(const Scalar& x, const Scalar& y).
       * For fixed-size \c 1x2 or \c 2x1 vectors it is therefore recommended to use the default
@@ -335,12 +376,15 @@
       */
     EIGEN_DEVICE_FUNC
     Matrix(Index rows, Index cols);
-    
-    /** \brief Constructs an initialized 2D vector with given coefficients */
-    Matrix(const Scalar& x, const Scalar& y);
-    #endif
 
-    /** \brief Constructs an initialized 3D vector with given coefficients */
+    /** \brief Constructs an initialized 2D vector with given coefficients
+      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...) */
+    Matrix(const Scalar& x, const Scalar& y);
+    #endif  // end EIGEN_PARSED_BY_DOXYGEN
+
+    /** \brief Constructs an initialized 3D vector with given coefficients
+      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...)
+      */
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z)
     {
@@ -350,7 +394,9 @@
       m_storage.data()[1] = y;
       m_storage.data()[2] = z;
     }
-    /** \brief Constructs an initialized 4D vector with given coefficients */
+    /** \brief Constructs an initialized 4D vector with given coefficients
+      * \sa Matrix(const Scalar&, const Scalar&, const Scalar&,  const Scalar&, const ArgTypes&...)
+      */
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Matrix(const Scalar& x, const Scalar& y, const Scalar& z, const Scalar& w)
     {
@@ -377,8 +423,10 @@
       : Base(other.derived())
     { }
 
-    EIGEN_DEVICE_FUNC inline Index innerStride() const { return 1; }
-    EIGEN_DEVICE_FUNC inline Index outerStride() const { return this->innerSize(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT { return 1; }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return this->innerSize(); }
 
     /////////// Geometry module ///////////
 
@@ -405,7 +453,7 @@
   *
   * \ingroup Core_Module
   *
-  * Eigen defines several typedef shortcuts for most common matrix and vector types.
+  * %Eigen defines several typedef shortcuts for most common matrix and vector types.
   *
   * The general patterns are the following:
   *
@@ -418,6 +466,15 @@
   * There are also \c VectorSizeType and \c RowVectorSizeType which are self-explanatory. For example, \c Vector4cf is
   * a fixed-size vector of 4 complex floats.
   *
+  * With \cpp11, template alias are also defined for common sizes.
+  * They follow the same pattern as above except that the scalar type suffix is replaced by a
+  * template parameter, i.e.:
+  *   - `MatrixSize<Type>` where `Size` can be \c 2,\c 3,\c 4 for fixed size square matrices or \c X for dynamic size.
+  *   - `MatrixXSize<Type>` and `MatrixSizeX<Type>` where `Size` can be \c 2,\c 3,\c 4 for hybrid dynamic/fixed matrices.
+  *   - `VectorSize<Type>` and `RowVectorSize<Type>` for column and row vectors.
+  *
+  * With \cpp11, you can also use fully generic column and row vector types: `Vector<Type,Size>` and `RowVector<Type,Size>`.
+  *
   * \sa class Matrix
   */
 
@@ -454,6 +511,55 @@
 #undef EIGEN_MAKE_TYPEDEFS
 #undef EIGEN_MAKE_FIXED_TYPEDEFS
 
+#if EIGEN_HAS_CXX11
+
+#define EIGEN_MAKE_TYPEDEFS(Size, SizeSuffix)                     \
+/** \ingroup matrixtypedefs */                                    \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Matrix##SizeSuffix = Matrix<Type, Size, Size>;              \
+/** \ingroup matrixtypedefs */                                    \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Vector##SizeSuffix = Matrix<Type, Size, 1>;                 \
+/** \ingroup matrixtypedefs */                                    \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using RowVector##SizeSuffix = Matrix<Type, 1, Size>;
+
+#define EIGEN_MAKE_FIXED_TYPEDEFS(Size)                           \
+/** \ingroup matrixtypedefs */                                    \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Matrix##Size##X = Matrix<Type, Size, Dynamic>;              \
+/** \ingroup matrixtypedefs */                                    \
+/** \brief \cpp11 */                                              \
+template <typename Type>                                          \
+using Matrix##X##Size = Matrix<Type, Dynamic, Size>;
+
+EIGEN_MAKE_TYPEDEFS(2, 2)
+EIGEN_MAKE_TYPEDEFS(3, 3)
+EIGEN_MAKE_TYPEDEFS(4, 4)
+EIGEN_MAKE_TYPEDEFS(Dynamic, X)
+EIGEN_MAKE_FIXED_TYPEDEFS(2)
+EIGEN_MAKE_FIXED_TYPEDEFS(3)
+EIGEN_MAKE_FIXED_TYPEDEFS(4)
+
+/** \ingroup matrixtypedefs
+  * \brief \cpp11 */
+template <typename Type, int Size>
+using Vector = Matrix<Type, Size, 1>;
+
+/** \ingroup matrixtypedefs
+  * \brief \cpp11 */
+template <typename Type, int Size>
+using RowVector = Matrix<Type, 1, Size>;
+
+#undef EIGEN_MAKE_TYPEDEFS
+#undef EIGEN_MAKE_FIXED_TYPEDEFS
+
+#endif // EIGEN_HAS_CXX11
+
 } // end namespace Eigen
 
 #endif // EIGEN_MATRIX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
index f8bcc8c..45c3a59 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/MatrixBase.h
@@ -76,6 +76,7 @@
     using Base::coeffRef;
     using Base::lazyAssign;
     using Base::eval;
+    using Base::operator-;
     using Base::operator+=;
     using Base::operator-=;
     using Base::operator*=;
@@ -122,7 +123,6 @@
 
 #define EIGEN_CURRENT_STORAGE_BASE_CLASS Eigen::MatrixBase
 #define EIGEN_DOC_UNARY_ADDONS(X,Y)
-#   include "../plugins/CommonCwiseUnaryOps.h"
 #   include "../plugins/CommonCwiseBinaryOps.h"
 #   include "../plugins/MatrixCwiseUnaryOps.h"
 #   include "../plugins/MatrixCwiseBinaryOps.h"
@@ -268,6 +268,8 @@
     Derived& setIdentity();
     EIGEN_DEVICE_FUNC
     Derived& setIdentity(Index rows, Index cols);
+    EIGEN_DEVICE_FUNC Derived& setUnit(Index i);
+    EIGEN_DEVICE_FUNC Derived& setUnit(Index newSize, Index i);
 
     bool isIdentity(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
     bool isDiagonal(const RealScalar& prec = NumTraits<Scalar>::dummy_precision()) const;
@@ -296,7 +298,7 @@
     EIGEN_DEVICE_FUNC inline bool operator!=(const MatrixBase<OtherDerived>& other) const
     { return cwiseNotEqual(other).any(); }
 
-    NoAlias<Derived,Eigen::MatrixBase > noalias();
+    NoAlias<Derived,Eigen::MatrixBase > EIGEN_DEVICE_FUNC noalias();
 
     // TODO forceAlignedAccess is temporarily disabled
     // Need to find a nicer workaround.
@@ -326,6 +328,7 @@
 
     inline const PartialPivLU<PlainObject> lu() const;
 
+    EIGEN_DEVICE_FUNC
     inline const Inverse<Derived> inverse() const;
 
     template<typename ResultType>
@@ -335,12 +338,15 @@
       bool& invertible,
       const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
     ) const;
+
     template<typename ResultType>
     inline void computeInverseWithCheck(
       ResultType& inverse,
       bool& invertible,
       const RealScalar& absDeterminantThreshold = NumTraits<Scalar>::dummy_precision()
     ) const;
+
+    EIGEN_DEVICE_FUNC
     Scalar determinant() const;
 
 /////////// Cholesky module ///////////
@@ -412,15 +418,19 @@
 
 ////////// Householder module ///////////
 
+    EIGEN_DEVICE_FUNC
     void makeHouseholderInPlace(Scalar& tau, RealScalar& beta);
     template<typename EssentialPart>
+    EIGEN_DEVICE_FUNC
     void makeHouseholder(EssentialPart& essential,
                          Scalar& tau, RealScalar& beta) const;
     template<typename EssentialPart>
+    EIGEN_DEVICE_FUNC
     void applyHouseholderOnTheLeft(const EssentialPart& essential,
                                    const Scalar& tau,
                                    Scalar* workspace);
     template<typename EssentialPart>
+    EIGEN_DEVICE_FUNC
     void applyHouseholderOnTheRight(const EssentialPart& essential,
                                     const Scalar& tau,
                                     Scalar* workspace);
@@ -428,8 +438,10 @@
 ///////// Jacobi module /////////
 
     template<typename OtherScalar>
+    EIGEN_DEVICE_FUNC
     void applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j);
     template<typename OtherScalar>
+    EIGEN_DEVICE_FUNC
     void applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j);
 
 ///////// SparseCore module /////////
@@ -456,6 +468,11 @@
     const MatrixFunctionReturnValue<Derived> matrixFunction(StemFunction f) const;
     EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cosh, hyperbolic cosine)
     EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sinh, hyperbolic sine)
+#if EIGEN_HAS_CXX11_MATH
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, atanh, inverse hyperbolic cosine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, acosh, inverse hyperbolic cosine)
+    EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, asinh, inverse hyperbolic sine)
+#endif
     EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, cos, cosine)
     EIGEN_MATRIX_FUNCTION(MatrixFunctionReturnValue, sin, sine)
     EIGEN_MATRIX_FUNCTION(MatrixSquareRootReturnValue, sqrt, square root)
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
index 13adf07..b427576 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NestByValue.h
@@ -16,7 +16,11 @@
 namespace internal {
 template<typename ExpressionType>
 struct traits<NestByValue<ExpressionType> > : public traits<ExpressionType>
-{};
+{
+  enum {
+    Flags = traits<ExpressionType>::Flags & ~NestByRefBit
+  };
+};
 }
 
 /** \class NestByValue
@@ -41,57 +45,13 @@
 
     EIGEN_DEVICE_FUNC explicit inline NestByValue(const ExpressionType& matrix) : m_expression(matrix) {}
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_expression.rows(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_expression.cols(); }
-    EIGEN_DEVICE_FUNC inline Index outerStride() const { return m_expression.outerStride(); }
-    EIGEN_DEVICE_FUNC inline Index innerStride() const { return m_expression.innerStride(); }
-
-    EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index row, Index col) const
-    {
-      return m_expression.coeff(row, col);
-    }
-
-    EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index row, Index col)
-    {
-      return m_expression.const_cast_derived().coeffRef(row, col);
-    }
-
-    EIGEN_DEVICE_FUNC inline const CoeffReturnType coeff(Index index) const
-    {
-      return m_expression.coeff(index);
-    }
-
-    EIGEN_DEVICE_FUNC inline Scalar& coeffRef(Index index)
-    {
-      return m_expression.const_cast_derived().coeffRef(index);
-    }
-
-    template<int LoadMode>
-    inline const PacketScalar packet(Index row, Index col) const
-    {
-      return m_expression.template packet<LoadMode>(row, col);
-    }
-
-    template<int LoadMode>
-    inline void writePacket(Index row, Index col, const PacketScalar& x)
-    {
-      m_expression.const_cast_derived().template writePacket<LoadMode>(row, col, x);
-    }
-
-    template<int LoadMode>
-    inline const PacketScalar packet(Index index) const
-    {
-      return m_expression.template packet<LoadMode>(index);
-    }
-
-    template<int LoadMode>
-    inline void writePacket(Index index, const PacketScalar& x)
-    {
-      m_expression.const_cast_derived().template writePacket<LoadMode>(index, x);
-    }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_expression.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_expression.cols(); }
 
     EIGEN_DEVICE_FUNC operator const ExpressionType&() const { return m_expression; }
 
+    EIGEN_DEVICE_FUNC const ExpressionType& nestedExpression() const { return m_expression; }
+
   protected:
     const ExpressionType m_expression;
 };
@@ -99,12 +59,27 @@
 /** \returns an expression of the temporary version of *this.
   */
 template<typename Derived>
-inline const NestByValue<Derived>
+EIGEN_DEVICE_FUNC inline const NestByValue<Derived>
 DenseBase<Derived>::nestByValue() const
 {
   return NestByValue<Derived>(derived());
 }
 
+namespace internal {
+
+// Evaluator of Solve -> eval into a temporary
+template<typename ArgType>
+struct evaluator<NestByValue<ArgType> >
+  : public evaluator<ArgType>
+{
+  typedef evaluator<ArgType> Base;
+
+  EIGEN_DEVICE_FUNC explicit evaluator(const NestByValue<ArgType>& xpr)
+    : Base(xpr.nestedExpression())
+  {}
+};
+}
+
 } // end namespace Eigen
 
 #endif // EIGEN_NESTBYVALUE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
index 3390801..570283d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NoAlias.h
@@ -33,6 +33,7 @@
   public:
     typedef typename ExpressionType::Scalar Scalar;
     
+    EIGEN_DEVICE_FUNC
     explicit NoAlias(ExpressionType& expression) : m_expression(expression) {}
     
     template<typename OtherDerived>
@@ -74,10 +75,10 @@
   *
   * More precisely, noalias() allows to bypass the EvalBeforeAssignBit flag.
   * Currently, even though several expressions may alias, only product
-  * expressions have this flag. Therefore, noalias() is only usefull when
+  * expressions have this flag. Therefore, noalias() is only useful when
   * the source expression contains a matrix product.
   *
-  * Here are some examples where noalias is usefull:
+  * Here are some examples where noalias is useful:
   * \code
   * D.noalias()  = A * B;
   * D.noalias() += A.transpose() * B;
@@ -98,7 +99,7 @@
   * \sa class NoAlias
   */
 template<typename Derived>
-NoAlias<Derived,MatrixBase> MatrixBase<Derived>::noalias()
+NoAlias<Derived,MatrixBase> EIGEN_DEVICE_FUNC MatrixBase<Derived>::noalias()
 {
   return NoAlias<Derived, Eigen::MatrixBase >(derived());
 }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
index daf4898..72eac5a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/NumTraits.h
@@ -21,12 +21,14 @@
           bool is_integer = NumTraits<T>::IsInteger>
 struct default_digits10_impl
 {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static int run() { return std::numeric_limits<T>::digits10; }
 };
 
 template<typename T>
 struct default_digits10_impl<T,false,false> // Floating point
 {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static int run() {
     using std::log10;
     using std::ceil;
@@ -38,11 +40,64 @@
 template<typename T>
 struct default_digits10_impl<T,false,true> // Integer
 {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static int run() { return 0; }
+};
+
+
+// default implementation of digits(), based on numeric_limits if specialized,
+// 0 for integer types, and log2(epsilon()) otherwise.
+template< typename T,
+          bool use_numeric_limits = std::numeric_limits<T>::is_specialized,
+          bool is_integer = NumTraits<T>::IsInteger>
+struct default_digits_impl
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static int run() { return std::numeric_limits<T>::digits; }
+};
+
+template<typename T>
+struct default_digits_impl<T,false,false> // Floating point
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static int run() {
+    using std::log;
+    using std::ceil;
+    typedef typename NumTraits<T>::Real Real;
+    return int(ceil(-log(NumTraits<Real>::epsilon())/log(static_cast<Real>(2))));
+  }
+};
+
+template<typename T>
+struct default_digits_impl<T,false,true> // Integer
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static int run() { return 0; }
 };
 
 } // end namespace internal
 
+namespace numext {
+/** \internal bit-wise cast without changing the underlying bit representation. */
+
+// TODO: Replace by std::bit_cast (available in C++20)
+template <typename Tgt, typename Src>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Tgt bit_cast(const Src& src) {
+#if EIGEN_HAS_TYPE_TRAITS
+  // The behaviour of memcpy is not specified for non-trivially copyable types
+  EIGEN_STATIC_ASSERT(std::is_trivially_copyable<Src>::value, THIS_TYPE_IS_NOT_SUPPORTED);
+  EIGEN_STATIC_ASSERT(std::is_trivially_copyable<Tgt>::value && std::is_default_constructible<Tgt>::value,
+                      THIS_TYPE_IS_NOT_SUPPORTED);
+#endif
+
+  EIGEN_STATIC_ASSERT(sizeof(Src) == sizeof(Tgt), THIS_TYPE_IS_NOT_SUPPORTED);
+  Tgt tgt;
+  EIGEN_USING_STD(memcpy)
+  memcpy(&tgt, &src, sizeof(Tgt));
+  return tgt;
+}
+}  // namespace numext
+
 /** \class NumTraits
   * \ingroup Core_Module
   *
@@ -71,7 +126,7 @@
   *     and to \c 0 otherwise.
   * \li Enum values ReadCost, AddCost and MulCost representing a rough estimate of the number of CPU cycles needed
   *     to by move / add / mul instructions respectively, assuming the data is already stored in CPU registers.
-  *     Stay vague here. No need to do architecture-specific stuff.
+  *     Stay vague here. No need to do architecture-specific stuff. If you don't know what this means, just use \c Eigen::HugeCost.
   * \li An enum value \a IsSigned. It is equal to \c 1 if \a T is a signed type and to 0 if \a T is unsigned.
   * \li An enum value \a RequireInitialization. It is equal to \c 1 if the constructor of the numeric type \a T must
   *     be called, and to 0 if it is safe not to call it. Default is 0 if \a T is an arithmetic type, and 1 otherwise.
@@ -80,9 +135,18 @@
   * \li A dummy_precision() function returning a weak epsilon value. It is mainly used as a default
   *     value by the fuzzy comparison operators.
   * \li highest() and lowest() functions returning the highest and lowest possible values respectively.
+  * \li digits() function returning the number of radix digits (non-sign digits for integers, mantissa for floating-point). This is
+  *     the analogue of <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/digits">std::numeric_limits<T>::digits</a>
+  *     which is used as the default implementation if specialized.
   * \li digits10() function returning the number of decimal digits that can be represented without change. This is
   *     the analogue of <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/digits10">std::numeric_limits<T>::digits10</a>
   *     which is used as the default implementation if specialized.
+  * \li min_exponent() and max_exponent() functions returning the highest and lowest possible values, respectively,
+  *     such that the radix raised to the power exponent-1 is a normalized floating-point number.  These are equivalent to
+  *     <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/min_exponent">std::numeric_limits<T>::min_exponent</a>/
+  *     <a href="http://en.cppreference.com/w/cpp/types/numeric_limits/max_exponent">std::numeric_limits<T>::max_exponent</a>.
+  * \li infinity() function returning a representation of positive infinity, if available.
+  * \li quiet_NaN function returning a non-signaling "not-a-number", if available.
   */
 
 template<typename T> struct GenericNumTraits
@@ -106,42 +170,60 @@
   typedef T Nested;
   typedef T Literal;
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline Real epsilon()
   {
     return numext::numeric_limits<T>::epsilon();
   }
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline int digits10()
   {
     return internal::default_digits10_impl<T>::run();
   }
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static inline int digits()
+  {
+    return internal::default_digits_impl<T>::run();
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static inline int min_exponent()
+  {
+    return numext::numeric_limits<T>::min_exponent;
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static inline int max_exponent()
+  {
+    return numext::numeric_limits<T>::max_exponent;
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline Real dummy_precision()
   {
     // make sure to override this for floating-point types
     return Real(0);
   }
 
-
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline T highest() {
     return (numext::numeric_limits<T>::max)();
   }
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline T lowest()  {
-    return IsInteger ? (numext::numeric_limits<T>::min)() : (-(numext::numeric_limits<T>::max)());
+    return IsInteger ? (numext::numeric_limits<T>::min)()
+                     : static_cast<T>(-(numext::numeric_limits<T>::max)());
   }
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline T infinity() {
     return numext::numeric_limits<T>::infinity();
   }
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline T quiet_NaN() {
     return numext::numeric_limits<T>::quiet_NaN();
   }
@@ -153,19 +235,20 @@
 template<> struct NumTraits<float>
   : GenericNumTraits<float>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline float dummy_precision() { return 1e-5f; }
 };
 
 template<> struct NumTraits<double> : GenericNumTraits<double>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline double dummy_precision() { return 1e-12; }
 };
 
 template<> struct NumTraits<long double>
   : GenericNumTraits<long double>
 {
+  EIGEN_CONSTEXPR
   static inline long double dummy_precision() { return 1e-15l; }
 };
 
@@ -182,11 +265,11 @@
     MulCost = 4 * NumTraits<Real>::MulCost + 2 * NumTraits<Real>::AddCost
   };
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline Real epsilon() { return NumTraits<Real>::epsilon(); }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline Real dummy_precision() { return NumTraits<Real>::dummy_precision(); }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline int digits10() { return NumTraits<Real>::digits10(); }
 };
 
@@ -206,16 +289,17 @@
     IsInteger = NumTraits<Scalar>::IsInteger,
     IsSigned  = NumTraits<Scalar>::IsSigned,
     RequireInitialization = 1,
-    ReadCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits<Scalar>::ReadCost,
-    AddCost  = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits<Scalar>::AddCost,
-    MulCost  = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * NumTraits<Scalar>::MulCost
+    ReadCost = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::ReadCost),
+    AddCost  = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::AddCost),
+    MulCost  = ArrayType::SizeAtCompileTime==Dynamic ? HugeCost : ArrayType::SizeAtCompileTime * int(NumTraits<Scalar>::MulCost)
   };
 
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline RealScalar epsilon() { return NumTraits<RealScalar>::epsilon(); }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static inline RealScalar dummy_precision() { return NumTraits<RealScalar>::dummy_precision(); }
 
+  EIGEN_CONSTEXPR
   static inline int digits10() { return NumTraits<Scalar>::digits10(); }
 };
 
@@ -229,6 +313,7 @@
     MulCost  = HugeCost
   };
 
+  EIGEN_CONSTEXPR
   static inline int digits10() { return 0; }
 
 private:
@@ -243,6 +328,8 @@
 // Empty specialization for void to allow template specialization based on NumTraits<T>::Real with T==void and SFINAE.
 template<> struct NumTraits<void> {};
 
+template<> struct NumTraits<bool> : GenericNumTraits<bool> {};
+
 } // end namespace Eigen
 
 #endif // EIGEN_NUMTRAITS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h
new file mode 100644
index 0000000..29abf35
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PartialReduxEvaluator.h
@@ -0,0 +1,232 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011-2018 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_PARTIALREDUX_H
+#define EIGEN_PARTIALREDUX_H
+
+namespace Eigen { 
+
+namespace internal {
+
+
+/***************************************************************************
+*
+* This file provides evaluators for partial reductions.
+* There are two modes:
+*
+*  - scalar path: simply calls the respective function on the column or row.
+*    -> nothing special here, all the tricky part is handled by the return
+*       types of VectorwiseOp's members. They embed the functor calling the
+*       respective DenseBase's member function.
+*
+*  - vectorized path: implements a packet-wise reductions followed by
+*    some (optional) processing of the outcome, e.g., division by n for mean.
+*
+* For the vectorized path let's observe that the packet-size and outer-unrolling
+* are both decided by the assignement logic. So all we have to do is to decide
+* on the inner unrolling.
+*
+* For the unrolling, we can reuse "internal::redux_vec_unroller" from Redux.h,
+* but be need to be careful to specify correct increment.
+*
+***************************************************************************/
+
+
+/* logic deciding a strategy for unrolling of vectorized paths */
+template<typename Func, typename Evaluator>
+struct packetwise_redux_traits
+{
+  enum {
+    OuterSize = int(Evaluator::IsRowMajor) ? Evaluator::RowsAtCompileTime : Evaluator::ColsAtCompileTime,
+    Cost = OuterSize == Dynamic ? HugeCost
+         : OuterSize * Evaluator::CoeffReadCost + (OuterSize-1) * functor_traits<Func>::Cost,
+    Unrolling = Cost <= EIGEN_UNROLLING_LIMIT ? CompleteUnrolling : NoUnrolling
+  };
+
+};
+
+/* Value to be returned when size==0 , by default let's return 0 */
+template<typename PacketType,typename Func>
+EIGEN_DEVICE_FUNC
+PacketType packetwise_redux_empty_value(const Func& ) { return pset1<PacketType>(0); }
+
+/* For products the default is 1 */
+template<typename PacketType,typename Scalar>
+EIGEN_DEVICE_FUNC
+PacketType packetwise_redux_empty_value(const scalar_product_op<Scalar,Scalar>& ) { return pset1<PacketType>(1); }
+
+/* Perform the actual reduction */
+template<typename Func, typename Evaluator,
+         int Unrolling = packetwise_redux_traits<Func, Evaluator>::Unrolling
+>
+struct packetwise_redux_impl;
+
+/* Perform the actual reduction with unrolling */
+template<typename Func, typename Evaluator>
+struct packetwise_redux_impl<Func, Evaluator, CompleteUnrolling>
+{
+  typedef redux_novec_unroller<Func,Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
+  typedef typename Evaluator::Scalar Scalar;
+
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
+  PacketType run(const Evaluator &eval, const Func& func, Index /*size*/)
+  {
+    return redux_vec_unroller<Func, Evaluator, 0, packetwise_redux_traits<Func, Evaluator>::OuterSize>::template run<PacketType>(eval,func);
+  }
+};
+
+/* Add a specialization of redux_vec_unroller for size==0 at compiletime.
+ * This specialization is not required for general reductions, which is
+ * why it is defined here.
+ */
+template<typename Func, typename Evaluator, int Start>
+struct redux_vec_unroller<Func, Evaluator, Start, 0>
+{
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE PacketType run(const Evaluator &, const Func& f)
+  {
+    return packetwise_redux_empty_value<PacketType>(f);
+  }
+};
+
+/* Perform the actual reduction for dynamic sizes */
+template<typename Func, typename Evaluator>
+struct packetwise_redux_impl<Func, Evaluator, NoUnrolling>
+{
+  typedef typename Evaluator::Scalar Scalar;
+  typedef typename redux_traits<Func, Evaluator>::PacketType PacketScalar;
+
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC
+  static PacketType run(const Evaluator &eval, const Func& func, Index size)
+  {
+    if(size==0)
+      return packetwise_redux_empty_value<PacketType>(func);
+    
+    const Index size4 = (size-1)&(~3);
+    PacketType p = eval.template packetByOuterInner<Unaligned,PacketType>(0,0);
+    Index i = 1;
+    // This loop is optimized for instruction pipelining:
+    // - each iteration generates two independent instructions
+    // - thanks to branch prediction and out-of-order execution we have independent instructions across loops
+    for(; i<size4; i+=4)
+      p = func.packetOp(p,
+            func.packetOp(
+              func.packetOp(eval.template packetByOuterInner<Unaligned,PacketType>(i+0,0),eval.template packetByOuterInner<Unaligned,PacketType>(i+1,0)),
+              func.packetOp(eval.template packetByOuterInner<Unaligned,PacketType>(i+2,0),eval.template packetByOuterInner<Unaligned,PacketType>(i+3,0))));
+    for(; i<size; ++i)
+      p = func.packetOp(p, eval.template packetByOuterInner<Unaligned,PacketType>(i,0));
+    return p;
+  }
+};
+
+template< typename ArgType, typename MemberOp, int Direction>
+struct evaluator<PartialReduxExpr<ArgType, MemberOp, Direction> >
+  : evaluator_base<PartialReduxExpr<ArgType, MemberOp, Direction> >
+{
+  typedef PartialReduxExpr<ArgType, MemberOp, Direction> XprType;
+  typedef typename internal::nested_eval<ArgType,1>::type ArgTypeNested;
+  typedef typename internal::add_const_on_value_type<ArgTypeNested>::type ConstArgTypeNested;
+  typedef typename internal::remove_all<ArgTypeNested>::type ArgTypeNestedCleaned;
+  typedef typename ArgType::Scalar InputScalar;
+  typedef typename XprType::Scalar Scalar;
+  enum {
+    TraversalSize = Direction==int(Vertical) ? int(ArgType::RowsAtCompileTime) :  int(ArgType::ColsAtCompileTime)
+  };
+  typedef typename MemberOp::template Cost<int(TraversalSize)> CostOpType;
+  enum {
+    CoeffReadCost = TraversalSize==Dynamic ? HugeCost
+                  : TraversalSize==0 ? 1
+                  : int(TraversalSize) * int(evaluator<ArgType>::CoeffReadCost) + int(CostOpType::value),
+    
+    _ArgFlags = evaluator<ArgType>::Flags,
+
+    _Vectorizable =  bool(int(_ArgFlags)&PacketAccessBit)
+                  && bool(MemberOp::Vectorizable)
+                  && (Direction==int(Vertical) ? bool(_ArgFlags&RowMajorBit) : (_ArgFlags&RowMajorBit)==0)
+                  && (TraversalSize!=0),
+                  
+    Flags = (traits<XprType>::Flags&RowMajorBit)
+          | (evaluator<ArgType>::Flags&(HereditaryBits&(~RowMajorBit)))
+          | (_Vectorizable ? PacketAccessBit : 0)
+          | LinearAccessBit,
+    
+    Alignment = 0 // FIXME this will need to be improved once PartialReduxExpr is vectorized
+  };
+
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType xpr)
+    : m_arg(xpr.nestedExpression()), m_functor(xpr.functor())
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(TraversalSize==Dynamic ? HugeCost : (TraversalSize==0 ? 1 : int(CostOpType::value)));
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  typedef typename XprType::CoeffReturnType CoeffReturnType;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  const Scalar coeff(Index i, Index j) const
+  {
+    return coeff(Direction==Vertical ? j : i);
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  const Scalar coeff(Index index) const
+  {
+    return m_functor(m_arg.template subVector<DirectionType(Direction)>(index));
+  }
+
+  template<int LoadMode,typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  PacketType packet(Index i, Index j) const
+  {
+    return packet<LoadMode,PacketType>(Direction==Vertical ? j : i);
+  }
+  
+  template<int LoadMode,typename PacketType>
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+  PacketType packet(Index idx) const
+  {
+    enum { PacketSize = internal::unpacket_traits<PacketType>::size };
+    typedef Block<const ArgTypeNestedCleaned,
+                  Direction==Vertical ? int(ArgType::RowsAtCompileTime) : int(PacketSize),
+                  Direction==Vertical ? int(PacketSize) : int(ArgType::ColsAtCompileTime),
+                  true /* InnerPanel */> PanelType;
+    
+    PanelType panel(m_arg,
+                    Direction==Vertical ? 0 : idx,
+                    Direction==Vertical ? idx : 0,
+                    Direction==Vertical ? m_arg.rows() : Index(PacketSize),
+                    Direction==Vertical ? Index(PacketSize) : m_arg.cols());
+
+    // FIXME
+    // See bug 1612, currently if PacketSize==1 (i.e. complex<double> with 128bits registers) then the storage-order of panel get reversed
+    // and methods like packetByOuterInner do not make sense anymore in this context.
+    // So let's just by pass "vectorization" in this case:
+    if(PacketSize==1)
+      return internal::pset1<PacketType>(coeff(idx));
+    
+    typedef typename internal::redux_evaluator<PanelType> PanelEvaluator;
+    PanelEvaluator panel_eval(panel);
+    typedef typename MemberOp::BinaryOp BinaryOp;
+    PacketType p = internal::packetwise_redux_impl<BinaryOp,PanelEvaluator>::template run<PacketType>(panel_eval,m_functor.binaryFunc(),m_arg.outerSize());
+    return p;
+  }
+
+protected:
+  ConstArgTypeNested m_arg;
+  const MemberOp m_functor;
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_PARTIALREDUX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
index b1fb455..69401bf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PermutationMatrix.h
@@ -87,25 +87,14 @@
       return derived();
     }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    Derived& operator=(const PermutationBase& other)
-    {
-      indices() = other.indices();
-      return derived();
-    }
-    #endif
-
     /** \returns the number of rows */
-    inline Index rows() const { return Index(indices().size()); }
+    inline EIGEN_DEVICE_FUNC Index rows() const { return Index(indices().size()); }
 
     /** \returns the number of columns */
-    inline Index cols() const { return Index(indices().size()); }
+    inline EIGEN_DEVICE_FUNC Index cols() const { return Index(indices().size()); }
 
     /** \returns the size of a side of the respective square matrix, i.e., the number of indices */
-    inline Index size() const { return Index(indices().size()); }
+    inline EIGEN_DEVICE_FUNC Index size() const { return Index(indices().size()); }
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename DenseDerived>
@@ -333,12 +322,6 @@
     inline PermutationMatrix(const PermutationBase<OtherDerived>& other)
       : m_indices(other.indices()) {}
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** Standard copy constructor. Defined only to prevent a default copy constructor
-      * from hiding the other templated constructor */
-    inline PermutationMatrix(const PermutationMatrix& other) : m_indices(other.indices()) {}
-    #endif
-
     /** Generic constructor from expression of the indices. The indices
       * array has the meaning that the permutations sends each integer i to indices[i].
       *
@@ -373,17 +356,6 @@
       return Base::operator=(tr.derived());
     }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    PermutationMatrix& operator=(const PermutationMatrix& other)
-    {
-      m_indices = other.m_indices;
-      return *this;
-    }
-    #endif
-
     /** const version of indices(). */
     const IndicesType& indices() const { return m_indices; }
     /** \returns a reference to the stored array representing the permutation. */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
index 1dc7e22..e2ddbd1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/PlainObjectBase.h
@@ -13,10 +13,10 @@
 
 #if defined(EIGEN_INITIALIZE_MATRICES_BY_ZERO)
 # define EIGEN_INITIALIZE_COEFFS
-# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED for(int i=0;i<base().size();++i) coeffRef(i)=Scalar(0);
+# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED for(Index i=0;i<base().size();++i) coeffRef(i)=Scalar(0);
 #elif defined(EIGEN_INITIALIZE_MATRICES_BY_NAN)
 # define EIGEN_INITIALIZE_COEFFS
-# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED for(int i=0;i<base().size();++i) coeffRef(i)=std::numeric_limits<Scalar>::quiet_NaN();
+# define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED for(Index i=0;i<base().size();++i) coeffRef(i)=std::numeric_limits<Scalar>::quiet_NaN();
 #else
 # undef EIGEN_INITIALIZE_COEFFS
 # define EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
@@ -104,7 +104,7 @@
 
     typedef typename internal::traits<Derived>::StorageKind StorageKind;
     typedef typename internal::traits<Derived>::Scalar Scalar;
-    
+
     typedef typename internal::packet_traits<Scalar>::type PacketScalar;
     typedef typename NumTraits<Scalar>::Real RealScalar;
     typedef Derived DenseType;
@@ -118,16 +118,8 @@
     using Base::IsVectorAtCompileTime;
     using Base::Flags;
 
-    template<typename PlainObjectType, int MapOptions, typename StrideType> friend class Eigen::Map;
-    friend  class Eigen::Map<Derived, Unaligned>;
     typedef Eigen::Map<Derived, Unaligned>  MapType;
-    friend  class Eigen::Map<const Derived, Unaligned>;
     typedef const Eigen::Map<const Derived, Unaligned> ConstMapType;
-#if EIGEN_MAX_ALIGN_BYTES>0
-    // for EIGEN_MAX_ALIGN_BYTES==0, AlignedMax==Unaligned, and many compilers generate warnings for friend-ing a class twice.
-    friend  class Eigen::Map<Derived, AlignedMax>;
-    friend  class Eigen::Map<const Derived, AlignedMax>;
-#endif
     typedef Eigen::Map<Derived, AlignedMax> AlignedMapType;
     typedef const Eigen::Map<const Derived, AlignedMax> ConstAlignedMapType;
     template<typename StrideType> struct StridedMapType { typedef Eigen::Map<Derived, Unaligned, StrideType> type; };
@@ -147,10 +139,10 @@
     EIGEN_DEVICE_FUNC
     const Base& base() const { return *static_cast<const Base*>(this); }
 
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index rows() const { return m_storage.rows(); }
-    EIGEN_DEVICE_FUNC
-    EIGEN_STRONG_INLINE Index cols() const { return m_storage.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_storage.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_storage.cols(); }
 
     /** This is an overloaded version of DenseCoeffsBase<Derived,ReadOnlyAccessors>::coeff(Index,Index) const
       * provided to by-pass the creation of an evaluator of the expression, thus saving compilation efforts.
@@ -358,7 +350,7 @@
       * remain row-vectors and vectors remain vectors.
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void resizeLike(const EigenBase<OtherDerived>& _other)
     {
       const OtherDerived& other = _other.derived();
@@ -383,7 +375,7 @@
       * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or
       * conservativeResize(Index, NoChange_t).
       *
-      * Matrices are resized relative to the top-left element. In case values need to be 
+      * Matrices are resized relative to the top-left element. In case values need to be
       * appended to the matrix they will be uninitialized.
       */
     EIGEN_DEVICE_FUNC
@@ -440,7 +432,7 @@
       * of rows and/or of columns, you can use conservativeResize(NoChange_t, Index) or
       * conservativeResize(Index, NoChange_t).
       *
-      * Matrices are resized relative to the top-left element. In case values need to be 
+      * Matrices are resized relative to the top-left element. In case values need to be
       * appended to the matrix they will copied from \c other.
       */
     template<typename OtherDerived>
@@ -508,8 +500,8 @@
     EIGEN_DEVICE_FUNC
     PlainObjectBase& operator=(PlainObjectBase&& other) EIGEN_NOEXCEPT
     {
-      using std::swap;
-      swap(m_storage, other.m_storage);
+      _check_template_params();
+      m_storage = std::move(other.m_storage);
       return *this;
     }
 #endif
@@ -526,6 +518,71 @@
 //       EIGEN_INITIALIZE_COEFFS_IF_THAT_OPTION_IS_ENABLED
     }
 
+    #if EIGEN_HAS_CXX11
+    /** \brief Construct a row of column vector with fixed size from an arbitrary number of coefficients. \cpp11
+      *
+      * \only_for_vectors
+      *
+      * This constructor is for 1D array or vectors with more than 4 coefficients.
+      * There exists C++98 analogue constructors for fixed-size array/vector having 1, 2, 3, or 4 coefficients.
+      *
+      * \warning To construct a column (resp. row) vector of fixed length, the number of values passed to this
+      * constructor must match the the fixed number of rows (resp. columns) of \c *this.
+      */
+    template <typename... ArgTypes>
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    PlainObjectBase(const Scalar& a0, const Scalar& a1, const Scalar& a2,  const Scalar& a3, const ArgTypes&... args)
+      : m_storage()
+    {
+      _check_template_params();
+      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, sizeof...(args) + 4);
+      m_storage.data()[0] = a0;
+      m_storage.data()[1] = a1;
+      m_storage.data()[2] = a2;
+      m_storage.data()[3] = a3;
+      Index i = 4;
+      auto x = {(m_storage.data()[i++] = args, 0)...};
+      static_cast<void>(x);
+    }
+
+    /** \brief Constructs a Matrix or Array and initializes it by elements given by an initializer list of initializer
+      * lists \cpp11
+      */
+    EIGEN_DEVICE_FUNC
+    explicit EIGEN_STRONG_INLINE PlainObjectBase(const std::initializer_list<std::initializer_list<Scalar>>& list)
+      : m_storage()
+    {
+      _check_template_params();
+
+      size_t list_size = 0;
+      if (list.begin() != list.end()) {
+        list_size = list.begin()->size();
+      }
+
+      // This is to allow syntax like VectorXi {{1, 2, 3, 4}}
+      if (ColsAtCompileTime == 1 && list.size() == 1) {
+        eigen_assert(list_size == static_cast<size_t>(RowsAtCompileTime) || RowsAtCompileTime == Dynamic);
+        resize(list_size, ColsAtCompileTime);
+        std::copy(list.begin()->begin(), list.begin()->end(), m_storage.data());
+      } else {
+        eigen_assert(list.size() == static_cast<size_t>(RowsAtCompileTime) || RowsAtCompileTime == Dynamic);
+        eigen_assert(list_size == static_cast<size_t>(ColsAtCompileTime) || ColsAtCompileTime == Dynamic);
+        resize(list.size(), list_size);
+
+        Index row_index = 0;
+        for (const std::initializer_list<Scalar>& row : list) {
+          eigen_assert(list_size == row.size());
+          Index col_index = 0;
+          for (const Scalar& e : row) {
+            coeffRef(row_index, col_index) = e;
+            ++col_index;
+          }
+          ++row_index;
+        }
+      }
+    }
+    #endif  // end EIGEN_HAS_CXX11
+
     /** \sa PlainObjectBase::operator=(const EigenBase<OtherDerived>&) */
     template<typename OtherDerived>
     EIGEN_DEVICE_FUNC
@@ -564,7 +621,7 @@
       * \copydetails DenseBase::operator=(const EigenBase<OtherDerived> &other)
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Derived& operator=(const EigenBase<OtherDerived> &other)
     {
       _resize_to_match(other);
@@ -652,18 +709,26 @@
     using Base::setConstant;
     EIGEN_DEVICE_FUNC Derived& setConstant(Index size, const Scalar& val);
     EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, Index cols, const Scalar& val);
+    EIGEN_DEVICE_FUNC Derived& setConstant(NoChange_t, Index cols, const Scalar& val);
+    EIGEN_DEVICE_FUNC Derived& setConstant(Index rows, NoChange_t, const Scalar& val);
 
     using Base::setZero;
     EIGEN_DEVICE_FUNC Derived& setZero(Index size);
     EIGEN_DEVICE_FUNC Derived& setZero(Index rows, Index cols);
+    EIGEN_DEVICE_FUNC Derived& setZero(NoChange_t, Index cols);
+    EIGEN_DEVICE_FUNC Derived& setZero(Index rows, NoChange_t);
 
     using Base::setOnes;
     EIGEN_DEVICE_FUNC Derived& setOnes(Index size);
     EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, Index cols);
+    EIGEN_DEVICE_FUNC Derived& setOnes(NoChange_t, Index cols);
+    EIGEN_DEVICE_FUNC Derived& setOnes(Index rows, NoChange_t);
 
     using Base::setRandom;
     Derived& setRandom(Index size);
     Derived& setRandom(Index rows, Index cols);
+    Derived& setRandom(NoChange_t, Index cols);
+    Derived& setRandom(Index rows, NoChange_t);
 
     #ifdef EIGEN_PLAINOBJECTBASE_PLUGIN
     #include EIGEN_PLAINOBJECTBASE_PLUGIN
@@ -678,7 +743,7 @@
       * remain row-vectors and vectors remain vectors.
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void _resize_to_match(const EigenBase<OtherDerived>& other)
     {
       #ifdef EIGEN_NO_AUTOMATIC_RESIZING
@@ -705,10 +770,10 @@
       *
       * \internal
       */
-    // aliasing is dealt once in internall::call_assignment
+    // aliasing is dealt once in internal::call_assignment
     // so at this stage we have to assume aliasing... and resising has to be done later.
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Derived& _set(const DenseBase<OtherDerived>& other)
     {
       internal::call_assignment(this->derived(), other.derived());
@@ -721,7 +786,7 @@
       * \sa operator=(const MatrixBase<OtherDerived>&), _set()
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE Derived& _set_noalias(const DenseBase<OtherDerived>& other)
     {
       // I don't think we need this resize call since the lazyAssign will anyways resize
@@ -737,23 +802,25 @@
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void _init2(Index rows, Index cols, typename internal::enable_if<Base::SizeAtCompileTime!=2,T0>::type* = 0)
     {
-      EIGEN_STATIC_ASSERT(bool(NumTraits<T0>::IsInteger) &&
-                          bool(NumTraits<T1>::IsInteger),
+      const bool t0_is_integer_alike = internal::is_valid_index_type<T0>::value;
+      const bool t1_is_integer_alike = internal::is_valid_index_type<T1>::value;
+      EIGEN_STATIC_ASSERT(t0_is_integer_alike &&
+                          t1_is_integer_alike,
                           FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED)
       resize(rows,cols);
     }
-    
+
     template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void _init2(const T0& val0, const T1& val1, typename internal::enable_if<Base::SizeAtCompileTime==2,T0>::type* = 0)
     {
       EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 2)
       m_storage.data()[0] = Scalar(val0);
       m_storage.data()[1] = Scalar(val1);
     }
-    
+
     template<typename T0, typename T1>
-    EIGEN_DEVICE_FUNC 
+    EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void _init2(const Index& val0, const Index& val1,
                                     typename internal::enable_if<    (!internal::is_same<Index,Scalar>::value)
                                                                   && (internal::is_same<T0,Index>::value)
@@ -773,14 +840,14 @@
                                                                               && ((!internal::is_same<typename internal::traits<Derived>::XprKind,ArrayXpr>::value || Base::SizeAtCompileTime==Dynamic)),T>::type* = 0)
     {
       // NOTE MSVC 2008 complains if we directly put bool(NumTraits<T>::IsInteger) as the EIGEN_STATIC_ASSERT argument.
-      const bool is_integer = NumTraits<T>::IsInteger;
-      EIGEN_UNUSED_VARIABLE(is_integer);
-      EIGEN_STATIC_ASSERT(is_integer,
+      const bool is_integer_alike = internal::is_valid_index_type<T>::value;
+      EIGEN_UNUSED_VARIABLE(is_integer_alike);
+      EIGEN_STATIC_ASSERT(is_integer_alike,
                           FLOATING_POINT_ARGUMENT_PASSED__INTEGER_WAS_EXPECTED)
       resize(size);
     }
-    
-    // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type can be implicitely converted)
+
+    // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type can be implicitly converted)
     template<typename T>
     EIGEN_DEVICE_FUNC
     EIGEN_STRONG_INLINE void _init1(const Scalar& val0, typename internal::enable_if<Base::SizeAtCompileTime==1 && internal::is_convertible<T, Scalar>::value,T>::type* = 0)
@@ -788,7 +855,7 @@
       EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(PlainObjectBase, 1)
       m_storage.data()[0] = val0;
     }
-    
+
     // We have a 1x1 matrix/array => the argument is interpreted as the value of the unique coefficient (case where scalar type match the index type)
     template<typename T>
     EIGEN_DEVICE_FUNC
@@ -844,7 +911,7 @@
     {
       this->derived() = r;
     }
-    
+
     // For fixed-size Array<Scalar,...>
     template<typename T>
     EIGEN_DEVICE_FUNC
@@ -856,7 +923,7 @@
     {
       Base::setConstant(val0);
     }
-    
+
     // For fixed-size Array<Index,...>
     template<typename T>
     EIGEN_DEVICE_FUNC
@@ -870,38 +937,38 @@
     {
       Base::setConstant(val0);
     }
-    
+
     template<typename MatrixTypeA, typename MatrixTypeB, bool SwapPointers>
     friend struct internal::matrix_swap_impl;
 
   public:
-    
+
 #ifndef EIGEN_PARSED_BY_DOXYGEN
     /** \internal
       * \brief Override DenseBase::swap() since for dynamic-sized matrices
       * of same type it is enough to swap the data pointers.
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     void swap(DenseBase<OtherDerived> & other)
     {
       enum { SwapPointers = internal::is_same<Derived, OtherDerived>::value && Base::SizeAtCompileTime==Dynamic };
       internal::matrix_swap_impl<Derived, OtherDerived, bool(SwapPointers)>::run(this->derived(), other.derived());
     }
-    
+
     /** \internal
       * \brief const version forwarded to DenseBase::swap
       */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     void swap(DenseBase<OtherDerived> const & other)
     { Base::swap(other.derived()); }
-    
-    EIGEN_DEVICE_FUNC 
+
+    EIGEN_DEVICE_FUNC
     static EIGEN_STRONG_INLINE void _check_template_params()
     {
-      EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, (Options&RowMajor)==RowMajor)
-                        && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, (Options&RowMajor)==0)
+      EIGEN_STATIC_ASSERT((EIGEN_IMPLIES(MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1, (int(Options)&RowMajor)==RowMajor)
+                        && EIGEN_IMPLIES(MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1, (int(Options)&RowMajor)==0)
                         && ((RowsAtCompileTime == Dynamic) || (RowsAtCompileTime >= 0))
                         && ((ColsAtCompileTime == Dynamic) || (ColsAtCompileTime >= 0))
                         && ((MaxRowsAtCompileTime == Dynamic) || (MaxRowsAtCompileTime >= 0))
@@ -914,6 +981,17 @@
 
     enum { IsPlainObjectBase = 1 };
 #endif
+  public:
+    // These apparently need to be down here for nvcc+icc to prevent duplicate
+    // Map symbol.
+    template<typename PlainObjectType, int MapOptions, typename StrideType> friend class Eigen::Map;
+    friend class Eigen::Map<Derived, Unaligned>;
+    friend class Eigen::Map<const Derived, Unaligned>;
+#if EIGEN_MAX_ALIGN_BYTES>0
+    // for EIGEN_MAX_ALIGN_BYTES==0, AlignedMax==Unaligned, and many compilers generate warnings for friend-ing a class twice.
+    friend class Eigen::Map<Derived, AlignedMax>;
+    friend class Eigen::Map<const Derived, AlignedMax>;
+#endif
 };
 
 namespace internal {
@@ -921,13 +999,19 @@
 template <typename Derived, typename OtherDerived, bool IsVector>
 struct conservative_resize_like_impl
 {
+  #if EIGEN_HAS_TYPE_TRAITS
+  static const bool IsRelocatable = std::is_trivially_copyable<typename Derived::Scalar>::value;
+  #else
+  static const bool IsRelocatable = !NumTraits<typename Derived::Scalar>::RequireInitialization;
+  #endif
   static void run(DenseBase<Derived>& _this, Index rows, Index cols)
   {
     if (_this.rows() == rows && _this.cols() == cols) return;
     EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived)
 
-    if ( ( Derived::IsRowMajor && _this.cols() == cols) || // row-major and we change only the number of rows
-         (!Derived::IsRowMajor && _this.rows() == rows) )  // column-major and we change only the number of columns
+    if ( IsRelocatable
+          && (( Derived::IsRowMajor && _this.cols() == cols) ||  // row-major and we change only the number of rows
+              (!Derived::IsRowMajor && _this.rows() == rows) ))  // column-major and we change only the number of columns
     {
       internal::check_rows_cols_for_overflow<Derived::MaxSizeAtCompileTime>::run(rows, cols);
       _this.derived().m_storage.conservativeResize(rows*cols,rows,cols);
@@ -935,7 +1019,7 @@
     else
     {
       // The storage order does not allow us to use reallocation.
-      typename Derived::PlainObject tmp(rows,cols);
+      Derived tmp(rows,cols);
       const Index common_rows = numext::mini(rows, _this.rows());
       const Index common_cols = numext::mini(cols, _this.cols());
       tmp.block(0,0,common_rows,common_cols) = _this.block(0,0,common_rows,common_cols);
@@ -955,8 +1039,9 @@
     EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(Derived)
     EIGEN_STATIC_ASSERT_DYNAMIC_SIZE(OtherDerived)
 
-    if ( ( Derived::IsRowMajor && _this.cols() == other.cols()) || // row-major and we change only the number of rows
-         (!Derived::IsRowMajor && _this.rows() == other.rows()) )  // column-major and we change only the number of columns
+    if ( IsRelocatable &&
+          (( Derived::IsRowMajor && _this.cols() == other.cols()) ||  // row-major and we change only the number of rows
+           (!Derived::IsRowMajor && _this.rows() == other.rows()) ))  // column-major and we change only the number of columns
     {
       const Index new_rows = other.rows() - _this.rows();
       const Index new_cols = other.cols() - _this.cols();
@@ -969,7 +1054,7 @@
     else
     {
       // The storage order does not allow us to use reallocation.
-      typename Derived::PlainObject tmp(other);
+      Derived tmp(other);
       const Index common_rows = numext::mini(tmp.rows(), _this.rows());
       const Index common_cols = numext::mini(tmp.cols(), _this.cols());
       tmp.block(0,0,common_rows,common_cols) = _this.block(0,0,common_rows,common_cols);
@@ -984,13 +1069,18 @@
 struct conservative_resize_like_impl<Derived,OtherDerived,true>
   : conservative_resize_like_impl<Derived,OtherDerived,false>
 {
-  using conservative_resize_like_impl<Derived,OtherDerived,false>::run;
-  
+  typedef conservative_resize_like_impl<Derived,OtherDerived,false> Base;
+  using Base::run;
+  using Base::IsRelocatable;
+
   static void run(DenseBase<Derived>& _this, Index size)
   {
     const Index new_rows = Derived::RowsAtCompileTime==1 ? 1 : size;
     const Index new_cols = Derived::RowsAtCompileTime==1 ? size : 1;
-    _this.derived().m_storage.conservativeResize(size,new_rows,new_cols);
+    if(IsRelocatable)
+      _this.derived().m_storage.conservativeResize(size,new_rows,new_cols);
+    else
+      Base::run(_this.derived(), new_rows, new_cols);
   }
 
   static void run(DenseBase<Derived>& _this, const DenseBase<OtherDerived>& other)
@@ -1001,7 +1091,10 @@
 
     const Index new_rows = Derived::RowsAtCompileTime==1 ? 1 : other.rows();
     const Index new_cols = Derived::RowsAtCompileTime==1 ? other.cols() : 1;
-    _this.derived().m_storage.conservativeResize(other.size(),new_rows,new_cols);
+    if(IsRelocatable)
+      _this.derived().m_storage.conservativeResize(other.size(),new_rows,new_cols);
+    else
+      Base::run(_this.derived(), new_rows, new_cols);
 
     if (num_new_elements > 0)
       _this.tail(num_new_elements) = other.tail(num_new_elements);
@@ -1012,7 +1105,7 @@
 struct matrix_swap_impl
 {
   EIGEN_DEVICE_FUNC
-  static inline void run(MatrixTypeA& a, MatrixTypeB& b)
+  static EIGEN_STRONG_INLINE void run(MatrixTypeA& a, MatrixTypeB& b)
   {
     a.base().swap(b);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h
index 676c480..70a6c10 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Product.h
@@ -23,25 +23,25 @@
   typedef typename remove_all<Rhs>::type RhsCleaned;
   typedef traits<LhsCleaned> LhsTraits;
   typedef traits<RhsCleaned> RhsTraits;
-  
+
   typedef MatrixXpr XprKind;
-  
+
   typedef typename ScalarBinaryOpTraits<typename traits<LhsCleaned>::Scalar, typename traits<RhsCleaned>::Scalar>::ReturnType Scalar;
   typedef typename product_promote_storage_type<typename LhsTraits::StorageKind,
                                                 typename RhsTraits::StorageKind,
                                                 internal::product_type<Lhs,Rhs>::ret>::ret StorageKind;
   typedef typename promote_index_type<typename LhsTraits::StorageIndex,
                                       typename RhsTraits::StorageIndex>::type StorageIndex;
-  
+
   enum {
     RowsAtCompileTime    = LhsTraits::RowsAtCompileTime,
     ColsAtCompileTime    = RhsTraits::ColsAtCompileTime,
     MaxRowsAtCompileTime = LhsTraits::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = RhsTraits::MaxColsAtCompileTime,
-    
+
     // FIXME: only needed by GeneralMatrixMatrixTriangular
     InnerSize = EIGEN_SIZE_MIN_PREFER_FIXED(LhsTraits::ColsAtCompileTime, RhsTraits::RowsAtCompileTime),
-    
+
     // The storage order is somewhat arbitrary here. The correct one will be determined through the evaluator.
     Flags = (MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1) ? RowMajorBit
           : (MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1) ? 0
@@ -74,10 +74,10 @@
                                                                                    internal::product_type<_Lhs,_Rhs>::ret>::ret>
 {
   public:
-    
+
     typedef _Lhs Lhs;
     typedef _Rhs Rhs;
-    
+
     typedef typename ProductImpl<
         Lhs, Rhs, Option,
         typename internal::product_promote_storage_type<typename internal::traits<Lhs>::StorageKind,
@@ -90,18 +90,23 @@
     typedef typename internal::remove_all<LhsNested>::type LhsNestedCleaned;
     typedef typename internal::remove_all<RhsNested>::type RhsNestedCleaned;
 
-    EIGEN_DEVICE_FUNC Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Product(const Lhs& lhs, const Rhs& rhs) : m_lhs(lhs), m_rhs(rhs)
     {
       eigen_assert(lhs.cols() == rhs.rows()
         && "invalid matrix product"
         && "if you wanted a coeff-wise or a dot product use the respective explicit functions");
     }
 
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index rows() const { return m_lhs.rows(); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Index cols() const { return m_rhs.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_lhs.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
-    EIGEN_DEVICE_FUNC const LhsNestedCleaned& lhs() const { return m_lhs; }
-    EIGEN_DEVICE_FUNC const RhsNestedCleaned& rhs() const { return m_rhs; }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const LhsNestedCleaned& lhs() const { return m_lhs; }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const RhsNestedCleaned& rhs() const { return m_rhs; }
 
   protected:
 
@@ -110,13 +115,13 @@
 };
 
 namespace internal {
-  
+
 template<typename Lhs, typename Rhs, int Option, int ProductTag = internal::product_type<Lhs,Rhs>::ret>
 class dense_product_base
  : public internal::dense_xpr_base<Product<Lhs,Rhs,Option> >::type
 {};
 
-/** Convertion to scalar for inner-products */
+/** Conversion to scalar for inner-products */
 template<typename Lhs, typename Rhs, int Option>
 class dense_product_base<Lhs, Rhs, Option, InnerProduct>
  : public internal::dense_xpr_base<Product<Lhs,Rhs,Option> >::type
@@ -126,8 +131,8 @@
 public:
   using Base::derived;
   typedef typename Base::Scalar Scalar;
-  
-  EIGEN_STRONG_INLINE operator const Scalar() const
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator const Scalar() const
   {
     return internal::evaluator<ProductXpr>(derived()).coeff(0,0);
   }
@@ -148,25 +153,25 @@
   : public internal::dense_product_base<Lhs,Rhs,Option>
 {
     typedef Product<Lhs, Rhs, Option> Derived;
-    
+
   public:
-    
+
     typedef typename internal::dense_product_base<Lhs, Rhs, Option> Base;
     EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
   protected:
     enum {
-      IsOneByOne = (RowsAtCompileTime == 1 || RowsAtCompileTime == Dynamic) && 
+      IsOneByOne = (RowsAtCompileTime == 1 || RowsAtCompileTime == Dynamic) &&
                    (ColsAtCompileTime == 1 || ColsAtCompileTime == Dynamic),
       EnableCoeff = IsOneByOne || Option==LazyProduct
     };
-    
+
   public:
-  
+
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar coeff(Index row, Index col) const
     {
       EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
       eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) );
-      
+
       return internal::evaluator<Derived>(derived()).coeff(row,col);
     }
 
@@ -174,11 +179,11 @@
     {
       EIGEN_STATIC_ASSERT(EnableCoeff, THIS_METHOD_IS_ONLY_FOR_INNER_OR_LAZY_PRODUCTS);
       eigen_assert( (Option==LazyProduct) || (this->rows() == 1 && this->cols() == 1) );
-      
+
       return internal::evaluator<Derived>(derived()).coeff(i);
     }
-    
-  
+
+
 };
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
index 9b99bd7..8cf294b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ProductEvaluators.h
@@ -14,27 +14,27 @@
 #define EIGEN_PRODUCTEVALUATORS_H
 
 namespace Eigen {
-  
+
 namespace internal {
 
 /** \internal
   * Evaluator of a product expression.
   * Since products require special treatments to handle all possible cases,
-  * we simply deffer the evaluation logic to a product_evaluator class
+  * we simply defer the evaluation logic to a product_evaluator class
   * which offers more partial specialization possibilities.
-  * 
+  *
   * \sa class product_evaluator
   */
 template<typename Lhs, typename Rhs, int Options>
-struct evaluator<Product<Lhs, Rhs, Options> > 
+struct evaluator<Product<Lhs, Rhs, Options> >
  : public product_evaluator<Product<Lhs, Rhs, Options> >
 {
   typedef Product<Lhs, Rhs, Options> XprType;
   typedef product_evaluator<XprType> Base;
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr) : Base(xpr) {}
 };
- 
+
 // Catch "scalar * ( A * B )" and transform it to "(A*scalar) * B"
 // TODO we should apply that rule only if that's really helpful
 template<typename Lhs, typename Rhs, typename Scalar1, typename Scalar2, typename Plain1>
@@ -62,12 +62,12 @@
 
 
 template<typename Lhs, typename Rhs, int DiagIndex>
-struct evaluator<Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> > 
+struct evaluator<Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> >
  : public evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex> >
 {
   typedef Diagonal<const Product<Lhs, Rhs, DefaultProduct>, DiagIndex> XprType;
   typedef evaluator<Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex> > Base;
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit evaluator(const XprType& xpr)
     : Base(Diagonal<const Product<Lhs, Rhs, LazyProduct>, DiagIndex>(
         Product<Lhs, Rhs, LazyProduct>(xpr.nestedExpression().lhs(), xpr.nestedExpression().rhs()),
@@ -108,27 +108,27 @@
     : m_result(xpr.rows(), xpr.cols())
   {
     ::new (static_cast<Base*>(this)) Base(m_result);
-    
+
 // FIXME shall we handle nested_eval here?,
 // if so, then we must take care at removing the call to nested_eval in the specializations (e.g., in permutation_matrix_product, transposition_matrix_product, etc.)
 //     typedef typename internal::nested_eval<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
 //     typedef typename internal::nested_eval<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
 //     typedef typename internal::remove_all<LhsNested>::type LhsNestedCleaned;
 //     typedef typename internal::remove_all<RhsNested>::type RhsNestedCleaned;
-//     
+//
 //     const LhsNested lhs(xpr.lhs());
 //     const RhsNested rhs(xpr.rhs());
-//   
+//
 //     generic_product_impl<LhsNestedCleaned, RhsNestedCleaned>::evalTo(m_result, lhs, rhs);
 
     generic_product_impl<Lhs, Rhs, LhsShape, RhsShape, ProductTag>::evalTo(m_result, xpr.lhs(), xpr.rhs());
   }
-  
-protected:  
+
+protected:
   PlainObject m_result;
 };
 
-// The following three shortcuts are enabled only if the scalar types match excatly.
+// The following three shortcuts are enabled only if the scalar types match exactly.
 // TODO: we could enable them for different scalar types when the product is not vectorized.
 
 // Dense = Product
@@ -137,7 +137,7 @@
   typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type>
 {
   typedef Product<Lhs,Rhs,Options> SrcXprType;
-  static EIGEN_STRONG_INLINE
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<Scalar,Scalar> &)
   {
     Index dstRows = src.rows();
@@ -155,7 +155,7 @@
   typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type>
 {
   typedef Product<Lhs,Rhs,Options> SrcXprType;
-  static EIGEN_STRONG_INLINE
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<Scalar,Scalar> &)
   {
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
@@ -170,7 +170,7 @@
   typename enable_if<(Options==DefaultProduct || Options==AliasFreeProduct)>::type>
 {
   typedef Product<Lhs,Rhs,Options> SrcXprType;
-  static EIGEN_STRONG_INLINE
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<Scalar,Scalar> &)
   {
     eigen_assert(dst.rows() == src.rows() && dst.cols() == src.cols());
@@ -190,7 +190,7 @@
   typedef CwiseBinaryOp<internal::scalar_product_op<ScalarBis,Scalar>,
                         const CwiseNullaryOp<internal::scalar_constant_op<ScalarBis>,Plain>,
                         const Product<Lhs,Rhs,DefaultProduct> > SrcXprType;
-  static EIGEN_STRONG_INLINE
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   void run(DstXprType &dst, const SrcXprType &src, const AssignFunc& func)
   {
     call_assignment_no_alias(dst, (src.lhs().functor().m_other * src.rhs().lhs())*src.rhs().rhs(), func);
@@ -217,7 +217,7 @@
 struct assignment_from_xpr_op_product
 {
   template<typename SrcXprType, typename InitialFunc>
-  static EIGEN_STRONG_INLINE
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   void run(DstXprType &dst, const SrcXprType &src, const InitialFunc& /*func*/)
   {
     call_assignment_no_alias(dst, src.lhs(), Func1());
@@ -246,19 +246,19 @@
 struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,InnerProduct>
 {
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     dst.coeffRef(0,0) = (lhs.transpose().cwiseProduct(rhs)).sum();
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     dst.coeffRef(0,0) += (lhs.transpose().cwiseProduct(rhs)).sum();
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   { dst.coeffRef(0,0) -= (lhs.transpose().cwiseProduct(rhs)).sum(); }
 };
 
@@ -269,10 +269,10 @@
 
 // Column major result
 template<typename Dst, typename Lhs, typename Rhs, typename Func>
-void outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const false_type&)
+void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const false_type&)
 {
   evaluator<Rhs> rhsEval(rhs);
-  typename nested_eval<Lhs,Rhs::SizeAtCompileTime>::type actual_lhs(lhs);
+  ei_declare_local_nested_eval(Lhs,lhs,Rhs::SizeAtCompileTime,actual_lhs);
   // FIXME if cols is large enough, then it might be useful to make sure that lhs is sequentially stored
   // FIXME not very good if rhs is real and lhs complex while alpha is real too
   const Index cols = dst.cols();
@@ -282,10 +282,10 @@
 
 // Row major result
 template<typename Dst, typename Lhs, typename Rhs, typename Func>
-void outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const true_type&)
+void EIGEN_DEVICE_FUNC outer_product_selector_run(Dst& dst, const Lhs &lhs, const Rhs &rhs, const Func& func, const true_type&)
 {
   evaluator<Lhs> lhsEval(lhs);
-  typename nested_eval<Rhs,Lhs::SizeAtCompileTime>::type actual_rhs(rhs);
+  ei_declare_local_nested_eval(Rhs,rhs,Lhs::SizeAtCompileTime,actual_rhs);
   // FIXME if rows is large enough, then it might be useful to make sure that rhs is sequentially stored
   // FIXME not very good if lhs is real and rhs complex while alpha is real too
   const Index rows = dst.rows();
@@ -298,43 +298,43 @@
 {
   template<typename T> struct is_row_major : internal::conditional<(int(T::Flags)&RowMajorBit), internal::true_type, internal::false_type>::type {};
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   // TODO it would be nice to be able to exploit our *_assign_op functors for that purpose
-  struct set  { template<typename Dst, typename Src> void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived()  = src; } };
-  struct add  { template<typename Dst, typename Src> void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } };
-  struct sub  { template<typename Dst, typename Src> void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() -= src; } };
+  struct set  { template<typename Dst, typename Src> EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived()  = src; } };
+  struct add  { template<typename Dst, typename Src> EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() += src; } };
+  struct sub  { template<typename Dst, typename Src> EIGEN_DEVICE_FUNC void operator()(const Dst& dst, const Src& src) const { dst.const_cast_derived() -= src; } };
   struct adds {
     Scalar m_scale;
     explicit adds(const Scalar& s) : m_scale(s) {}
-    template<typename Dst, typename Src> void operator()(const Dst& dst, const Src& src) const {
+    template<typename Dst, typename Src> void EIGEN_DEVICE_FUNC operator()(const Dst& dst, const Src& src) const {
       dst.const_cast_derived() += m_scale * src;
     }
   };
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, set(), is_row_major<Dst>());
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, add(), is_row_major<Dst>());
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, sub(), is_row_major<Dst>());
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
     internal::outer_product_selector_run(dst, lhs, rhs, adds(alpha), is_row_major<Dst>());
   }
-  
+
 };
 
 
@@ -343,21 +343,21 @@
 struct generic_product_impl_base
 {
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   { dst.setZero(); scaleAndAddTo(dst, lhs, rhs, Scalar(1)); }
 
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   { scaleAndAddTo(dst,lhs, rhs, Scalar(1)); }
 
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   { scaleAndAddTo(dst, lhs, rhs, Scalar(-1)); }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   { Derived::scaleAndAddTo(dst,lhs,rhs,alpha); }
 
 };
@@ -373,8 +373,13 @@
   typedef typename internal::remove_all<typename internal::conditional<int(Side)==OnTheRight,LhsNested,RhsNested>::type>::type MatrixType;
 
   template<typename Dest>
-  static EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
+    // Fallback to inner product if both the lhs and rhs is a runtime vector.
+    if (lhs.rows() == 1 && rhs.cols() == 1) {
+      dst.coeffRef(0,0) += alpha * lhs.row(0).conjugate().dot(rhs.col(0));
+      return;
+    }
     LhsNested actual_lhs(lhs);
     RhsNested actual_rhs(rhs);
     internal::gemv_dense_selector<Side,
@@ -385,35 +390,84 @@
 };
 
 template<typename Lhs, typename Rhs>
-struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,CoeffBasedProductMode> 
+struct generic_product_impl<Lhs,Rhs,DenseShape,DenseShape,CoeffBasedProductMode>
 {
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     // Same as: dst.noalias() = lhs.lazyProduct(rhs);
     // but easier on the compiler side
     call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::assign_op<typename Dst::Scalar,Scalar>());
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     // dst.noalias() += lhs.lazyProduct(rhs);
     call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::add_assign_op<typename Dst::Scalar,Scalar>());
   }
-  
+
   template<typename Dst>
-  static EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
     // dst.noalias() -= lhs.lazyProduct(rhs);
     call_assignment_no_alias(dst, lhs.lazyProduct(rhs), internal::sub_assign_op<typename Dst::Scalar,Scalar>());
   }
-  
-//   template<typename Dst>
-//   static inline void scaleAndAddTo(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
-//   { dst.noalias() += alpha * lhs.lazyProduct(rhs); }
+
+  // This is a special evaluation path called from generic_product_impl<...,GemmProduct> in file GeneralMatrixMatrix.h
+  // This variant tries to extract scalar multiples from both the LHS and RHS and factor them out. For instance:
+  //   dst {,+,-}= (s1*A)*(B*s2)
+  // will be rewritten as:
+  //   dst {,+,-}= (s1*s2) * (A.lazyProduct(B))
+  // There are at least four benefits of doing so:
+  //  1 - huge performance gain for heap-allocated matrix types as it save costly allocations.
+  //  2 - it is faster than simply by-passing the heap allocation through stack allocation.
+  //  3 - it makes this fallback consistent with the heavy GEMM routine.
+  //  4 - it fully by-passes huge stack allocation attempts when multiplying huge fixed-size matrices.
+  //      (see https://stackoverflow.com/questions/54738495)
+  // For small fixed sizes matrices, howver, the gains are less obvious, it is sometimes x2 faster, but sometimes x3 slower,
+  // and the behavior depends also a lot on the compiler... This is why this re-writting strategy is currently
+  // enabled only when falling back from the main GEMM.
+  template<typename Dst, typename Func>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  void eval_dynamic(Dst& dst, const Lhs& lhs, const Rhs& rhs, const Func &func)
+  {
+    enum {
+      HasScalarFactor = blas_traits<Lhs>::HasScalarFactor || blas_traits<Rhs>::HasScalarFactor,
+      ConjLhs = blas_traits<Lhs>::NeedToConjugate,
+      ConjRhs = blas_traits<Rhs>::NeedToConjugate
+    };
+    // FIXME: in c++11 this should be auto, and extractScalarFactor should also return auto
+    //        this is important for real*complex_mat
+    Scalar actualAlpha = combine_scalar_factors<Scalar>(lhs, rhs);
+
+    eval_dynamic_impl(dst,
+                      blas_traits<Lhs>::extract(lhs).template conjugateIf<ConjLhs>(),
+                      blas_traits<Rhs>::extract(rhs).template conjugateIf<ConjRhs>(),
+                      func,
+                      actualAlpha,
+                      typename conditional<HasScalarFactor,true_type,false_type>::type());
+  }
+
+protected:
+
+  template<typename Dst, typename LhsT, typename RhsT, typename Func, typename Scalar>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs, const Func &func, const Scalar&  s /* == 1 */, false_type)
+  {
+    EIGEN_UNUSED_VARIABLE(s);
+    eigen_internal_assert(s==Scalar(1));
+    call_restricted_packet_assignment_no_alias(dst, lhs.lazyProduct(rhs), func);
+  }
+
+  template<typename Dst, typename LhsT, typename RhsT, typename Func, typename Scalar>
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  void eval_dynamic_impl(Dst& dst, const LhsT& lhs, const RhsT& rhs, const Func &func, const Scalar& s, true_type)
+  {
+    call_restricted_packet_assignment_no_alias(dst, s * lhs.lazyProduct(rhs), func);
+  }
 };
 
 // This specialization enforces the use of a coefficient-based evaluation strategy
@@ -471,7 +525,7 @@
 
   typedef typename internal::nested_eval<Lhs,Rhs::ColsAtCompileTime>::type LhsNested;
   typedef typename internal::nested_eval<Rhs,Lhs::RowsAtCompileTime>::type RhsNested;
-  
+
   typedef typename internal::remove_all<LhsNested>::type LhsNestedCleaned;
   typedef typename internal::remove_all<RhsNested>::type RhsNestedCleaned;
 
@@ -490,19 +544,19 @@
   typedef typename find_best_packet<Scalar,ColsAtCompileTime>::type RhsVecPacketType;
 
   enum {
-      
+
     LhsCoeffReadCost = LhsEtorType::CoeffReadCost,
     RhsCoeffReadCost = RhsEtorType::CoeffReadCost,
     CoeffReadCost = InnerSize==0 ? NumTraits<Scalar>::ReadCost
                   : InnerSize == Dynamic ? HugeCost
-                  : InnerSize * (NumTraits<Scalar>::MulCost + LhsCoeffReadCost + RhsCoeffReadCost)
+                    : InnerSize * (NumTraits<Scalar>::MulCost + int(LhsCoeffReadCost) + int(RhsCoeffReadCost))
                     + (InnerSize - 1) * NumTraits<Scalar>::AddCost,
 
     Unroll = CoeffReadCost <= EIGEN_UNROLLING_LIMIT,
-    
+
     LhsFlags = LhsEtorType::Flags,
     RhsFlags = RhsEtorType::Flags,
-    
+
     LhsRowMajor = LhsFlags & RowMajorBit,
     RhsRowMajor = RhsFlags & RowMajorBit,
 
@@ -512,7 +566,7 @@
     // Here, we don't care about alignment larger than the usable packet size.
     LhsAlignment = EIGEN_PLAIN_ENUM_MIN(LhsEtorType::Alignment,LhsVecPacketSize*int(sizeof(typename LhsNestedCleaned::Scalar))),
     RhsAlignment = EIGEN_PLAIN_ENUM_MIN(RhsEtorType::Alignment,RhsVecPacketSize*int(sizeof(typename RhsNestedCleaned::Scalar))),
-      
+
     SameType = is_same<typename LhsNestedCleaned::Scalar,typename RhsNestedCleaned::Scalar>::value,
 
     CanVectorizeRhs = bool(RhsRowMajor) && (RhsFlags & PacketAccessBit) && (ColsAtCompileTime!=1),
@@ -522,12 +576,12 @@
                     : (MaxColsAtCompileTime==1&&MaxRowsAtCompileTime!=1) ? 0
                     : (bool(RhsRowMajor) && !CanVectorizeLhs),
 
-    Flags = ((unsigned int)(LhsFlags | RhsFlags) & HereditaryBits & ~RowMajorBit)
+    Flags = ((int(LhsFlags) | int(RhsFlags)) & HereditaryBits & ~RowMajorBit)
           | (EvalToRowMajor ? RowMajorBit : 0)
           // TODO enable vectorization for mixed types
           | (SameType && (CanVectorizeLhs || CanVectorizeRhs) ? PacketAccessBit : 0)
           | (XprType::IsVectorAtCompileTime ? LinearAccessBit : 0),
-          
+
     LhsOuterStrideBytes = int(LhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename LhsNestedCleaned::Scalar)),
     RhsOuterStrideBytes = int(RhsNestedCleaned::OuterStrideAtCompileTime) * int(sizeof(typename RhsNestedCleaned::Scalar)),
 
@@ -543,10 +597,10 @@
     CanVectorizeInner =    SameType
                         && LhsRowMajor
                         && (!RhsRowMajor)
-                        && (LhsFlags & RhsFlags & ActualPacketAccessBit)
-                        && (InnerSize % packet_traits<Scalar>::size == 0)
+                        && (int(LhsFlags) & int(RhsFlags) & ActualPacketAccessBit)
+                        && (int(InnerSize) % packet_traits<Scalar>::size == 0)
   };
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index row, Index col) const
   {
     return (m_lhs.row(row).transpose().cwiseProduct( m_rhs.col(col) )).sum();
@@ -556,7 +610,8 @@
    * which is why we don't set the LinearAccessBit.
    * TODO: this seems possible when the result is a vector
    */
-  EIGEN_DEVICE_FUNC const CoeffReturnType coeff(Index index) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  const CoeffReturnType coeff(Index index) const
   {
     const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? 0 : index;
     const Index col = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? index : 0;
@@ -564,6 +619,7 @@
   }
 
   template<int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   const PacketType packet(Index row, Index col) const
   {
     PacketType res;
@@ -575,6 +631,7 @@
   }
 
   template<int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   const PacketType packet(Index index) const
   {
     const Index row = (RowsAtCompileTime == 1 || MaxRowsAtCompileTime==1) ? 0 : index;
@@ -585,7 +642,7 @@
 protected:
   typename internal::add_const_on_value_type<LhsNested>::type m_lhs;
   typename internal::add_const_on_value_type<RhsNested>::type m_rhs;
-  
+
   LhsEtorType m_lhsImpl;
   RhsEtorType m_rhsImpl;
 
@@ -603,7 +660,8 @@
   enum {
     Flags = Base::Flags | EvalBeforeNestingBit
   };
-  EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit product_evaluator(const XprType& xpr)
     : Base(BaseProduct(xpr.lhs(),xpr.rhs()))
   {}
 };
@@ -615,7 +673,7 @@
 template<int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<RowMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res)
   {
     etor_product_packet_impl<RowMajor, UnrollingIndex-1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs, innerDim, res);
     res =  pmadd(pset1<Packet>(lhs.coeff(row, Index(UnrollingIndex-1))), rhs.template packet<LoadMode,Packet>(Index(UnrollingIndex-1), col), res);
@@ -625,7 +683,7 @@
 template<int UnrollingIndex, typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<ColMajor, UnrollingIndex, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet &res)
   {
     etor_product_packet_impl<ColMajor, UnrollingIndex-1, Lhs, Rhs, Packet, LoadMode>::run(row, col, lhs, rhs, innerDim, res);
     res =  pmadd(lhs.template packet<LoadMode,Packet>(row, Index(UnrollingIndex-1)), pset1<Packet>(rhs.coeff(Index(UnrollingIndex-1), col)), res);
@@ -635,7 +693,7 @@
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<RowMajor, 1, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res)
   {
     res = pmul(pset1<Packet>(lhs.coeff(row, Index(0))),rhs.template packet<LoadMode,Packet>(Index(0), col));
   }
@@ -644,7 +702,7 @@
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<ColMajor, 1, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index /*innerDim*/, Packet &res)
   {
     res = pmul(lhs.template packet<LoadMode,Packet>(row, Index(0)), pset1<Packet>(rhs.coeff(Index(0), col)));
   }
@@ -653,7 +711,7 @@
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<RowMajor, 0, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res)
   {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
   }
@@ -662,7 +720,7 @@
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<ColMajor, 0, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index /*row*/, Index /*col*/, const Lhs& /*lhs*/, const Rhs& /*rhs*/, Index /*innerDim*/, Packet &res)
   {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
   }
@@ -671,7 +729,7 @@
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<RowMajor, Dynamic, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res)
   {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
     for(Index i = 0; i < innerDim; ++i)
@@ -682,7 +740,7 @@
 template<typename Lhs, typename Rhs, typename Packet, int LoadMode>
 struct etor_product_packet_impl<ColMajor, Dynamic, Lhs, Rhs, Packet, LoadMode>
 {
-  static EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Index row, Index col, const Lhs& lhs, const Rhs& rhs, Index innerDim, Packet& res)
   {
     res = pset1<Packet>(typename unpacket_traits<Packet>::type(0));
     for(Index i = 0; i < innerDim; ++i)
@@ -704,7 +762,7 @@
   : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,TriangularShape,DenseShape,ProductTag> >
 {
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   template<typename Dest>
   static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
@@ -718,7 +776,7 @@
 : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,TriangularShape,ProductTag> >
 {
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   template<typename Dest>
   static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
@@ -739,9 +797,10 @@
   : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,SelfAdjointShape,DenseShape,ProductTag> >
 {
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   template<typename Dest>
-  static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
+  static EIGEN_DEVICE_FUNC
+  void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
     selfadjoint_product_impl<typename Lhs::MatrixType,Lhs::Mode,false,Rhs,0,Rhs::IsVectorAtCompileTime>::run(dst, lhs.nestedExpression(), rhs, alpha);
   }
@@ -752,7 +811,7 @@
 : generic_product_impl_base<Lhs,Rhs,generic_product_impl<Lhs,Rhs,DenseShape,SelfAdjointShape,ProductTag> >
 {
   typedef typename Product<Lhs,Rhs>::Scalar Scalar;
-  
+
   template<typename Dest>
   static void scaleAndAddTo(Dest& dst, const Lhs& lhs, const Rhs& rhs, const Scalar& alpha)
   {
@@ -764,7 +823,7 @@
 /***************************************************************************
 * Diagonal products
 ***************************************************************************/
-  
+
 template<typename MatrixType, typename DiagonalType, typename Derived, int ProductOrder>
 struct diagonal_product_evaluator_base
   : evaluator_base<Derived>
@@ -772,17 +831,25 @@
    typedef typename ScalarBinaryOpTraits<typename MatrixType::Scalar, typename DiagonalType::Scalar>::ReturnType Scalar;
 public:
   enum {
-    CoeffReadCost = NumTraits<Scalar>::MulCost + evaluator<MatrixType>::CoeffReadCost + evaluator<DiagonalType>::CoeffReadCost,
-    
+    CoeffReadCost = int(NumTraits<Scalar>::MulCost) + int(evaluator<MatrixType>::CoeffReadCost) + int(evaluator<DiagonalType>::CoeffReadCost),
+
     MatrixFlags = evaluator<MatrixType>::Flags,
     DiagFlags = evaluator<DiagonalType>::Flags,
-    _StorageOrder = MatrixFlags & RowMajorBit ? RowMajor : ColMajor,
+
+    _StorageOrder = (Derived::MaxRowsAtCompileTime==1 && Derived::MaxColsAtCompileTime!=1) ? RowMajor
+                  : (Derived::MaxColsAtCompileTime==1 && Derived::MaxRowsAtCompileTime!=1) ? ColMajor
+                  : MatrixFlags & RowMajorBit ? RowMajor : ColMajor,
+    _SameStorageOrder = _StorageOrder == (MatrixFlags & RowMajorBit ? RowMajor : ColMajor),
+
     _ScalarAccessOnDiag =  !((int(_StorageOrder) == ColMajor && int(ProductOrder) == OnTheLeft)
                            ||(int(_StorageOrder) == RowMajor && int(ProductOrder) == OnTheRight)),
     _SameTypes = is_same<typename MatrixType::Scalar, typename DiagonalType::Scalar>::value,
     // FIXME currently we need same types, but in the future the next rule should be the one
     //_Vectorizable = bool(int(MatrixFlags)&PacketAccessBit) && ((!_PacketOnDiag) || (_SameTypes && bool(int(DiagFlags)&PacketAccessBit))),
-    _Vectorizable = bool(int(MatrixFlags)&PacketAccessBit) && _SameTypes && (_ScalarAccessOnDiag || (bool(int(DiagFlags)&PacketAccessBit))),
+    _Vectorizable =   bool(int(MatrixFlags)&PacketAccessBit)
+                  &&  _SameTypes
+                  && (_SameStorageOrder || (MatrixFlags&LinearAccessBit)==LinearAccessBit)
+                  && (_ScalarAccessOnDiag || (bool(int(DiagFlags)&PacketAccessBit))),
     _LinearAccessMask = (MatrixType::RowsAtCompileTime==1 || MatrixType::ColsAtCompileTime==1) ? LinearAccessBit : 0,
     Flags = ((HereditaryBits|_LinearAccessMask) & (unsigned int)(MatrixFlags)) | (_Vectorizable ? PacketAccessBit : 0),
     Alignment = evaluator<MatrixType>::Alignment,
@@ -791,14 +858,14 @@
                       ||  (DiagonalType::SizeAtCompileTime==Dynamic && MatrixType::RowsAtCompileTime==1 && ProductOrder==OnTheLeft)
                       ||  (DiagonalType::SizeAtCompileTime==Dynamic && MatrixType::ColsAtCompileTime==1 && ProductOrder==OnTheRight)
   };
-  
-  diagonal_product_evaluator_base(const MatrixType &mat, const DiagonalType &diag)
+
+  EIGEN_DEVICE_FUNC diagonal_product_evaluator_base(const MatrixType &mat, const DiagonalType &diag)
     : m_diagImpl(diag), m_matImpl(mat)
   {
     EIGEN_INTERNAL_CHECK_COST_VALUE(NumTraits<Scalar>::MulCost);
     EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
   }
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index idx) const
   {
     if(AsScalarProduct)
@@ -806,7 +873,7 @@
     else
       return m_diagImpl.coeff(idx) * m_matImpl.coeff(idx);
   }
-  
+
 protected:
   template<int LoadMode,typename PacketType>
   EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::true_type) const
@@ -814,7 +881,7 @@
     return internal::pmul(m_matImpl.template packet<LoadMode,PacketType>(row, col),
                           internal::pset1<PacketType>(m_diagImpl.coeff(id)));
   }
-  
+
   template<int LoadMode,typename PacketType>
   EIGEN_STRONG_INLINE PacketType packet_impl(Index row, Index col, Index id, internal::false_type) const
   {
@@ -825,7 +892,7 @@
     return internal::pmul(m_matImpl.template packet<LoadMode,PacketType>(row, col),
                           m_diagImpl.template packet<DiagonalPacketLoadMode,PacketType>(id));
   }
-  
+
   evaluator<DiagonalType> m_diagImpl;
   evaluator<MatrixType>   m_matImpl;
 };
@@ -840,25 +907,25 @@
   using Base::m_matImpl;
   using Base::coeff;
   typedef typename Base::Scalar Scalar;
-  
+
   typedef Product<Lhs, Rhs, ProductKind> XprType;
   typedef typename XprType::PlainObject PlainObject;
-  
-  enum {
-    StorageOrder = int(Rhs::Flags) & RowMajorBit ? RowMajor : ColMajor
-  };
+  typedef typename Lhs::DiagonalVectorType DiagonalType;
+
+
+  enum { StorageOrder = Base::_StorageOrder };
 
   EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
     : Base(xpr.rhs(), xpr.lhs().diagonal())
   {
   }
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const
   {
     return m_diagImpl.coeff(row) * m_matImpl.coeff(row, col);
   }
-  
-#ifndef __CUDACC__
+
+#ifndef EIGEN_GPUCC
   template<int LoadMode,typename PacketType>
   EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const
   {
@@ -867,7 +934,7 @@
     return this->template packet_impl<LoadMode,PacketType>(row,col, row,
                                  typename internal::conditional<int(StorageOrder)==RowMajor, internal::true_type, internal::false_type>::type());
   }
-  
+
   template<int LoadMode,typename PacketType>
   EIGEN_STRONG_INLINE PacketType packet(Index idx) const
   {
@@ -886,30 +953,30 @@
   using Base::m_matImpl;
   using Base::coeff;
   typedef typename Base::Scalar Scalar;
-  
+
   typedef Product<Lhs, Rhs, ProductKind> XprType;
   typedef typename XprType::PlainObject PlainObject;
-  
-  enum { StorageOrder = int(Lhs::Flags) & RowMajorBit ? RowMajor : ColMajor };
+
+  enum { StorageOrder = Base::_StorageOrder };
 
   EIGEN_DEVICE_FUNC explicit product_evaluator(const XprType& xpr)
     : Base(xpr.lhs(), xpr.rhs().diagonal())
   {
   }
-  
+
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar coeff(Index row, Index col) const
   {
     return m_matImpl.coeff(row, col) * m_diagImpl.coeff(col);
   }
-  
-#ifndef __CUDACC__
+
+#ifndef EIGEN_GPUCC
   template<int LoadMode,typename PacketType>
   EIGEN_STRONG_INLINE PacketType packet(Index row, Index col) const
   {
     return this->template packet_impl<LoadMode,PacketType>(row,col, col,
                                  typename internal::conditional<int(StorageOrder)==ColMajor, internal::true_type, internal::false_type>::type());
   }
-  
+
   template<int LoadMode,typename PacketType>
   EIGEN_STRONG_INLINE PacketType packet(Index idx) const
   {
@@ -937,7 +1004,7 @@
     typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
 
     template<typename Dest, typename PermutationType>
-    static inline void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr)
+    static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const PermutationType& perm, const ExpressionType& xpr)
     {
       MatrixType mat(xpr);
       const Index n = Side==OnTheLeft ? mat.rows() : mat.cols();
@@ -991,7 +1058,7 @@
 struct generic_product_impl<Lhs, Rhs, PermutationShape, MatrixShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
   {
     permutation_matrix_product<Rhs, OnTheLeft, false, MatrixShape>::run(dst, lhs, rhs);
   }
@@ -1001,7 +1068,7 @@
 struct generic_product_impl<Lhs, Rhs, MatrixShape, PermutationShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
   {
     permutation_matrix_product<Lhs, OnTheRight, false, MatrixShape>::run(dst, rhs, lhs);
   }
@@ -1011,7 +1078,7 @@
 struct generic_product_impl<Inverse<Lhs>, Rhs, PermutationShape, MatrixShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Inverse<Lhs>& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Inverse<Lhs>& lhs, const Rhs& rhs)
   {
     permutation_matrix_product<Rhs, OnTheLeft, true, MatrixShape>::run(dst, lhs.nestedExpression(), rhs);
   }
@@ -1021,7 +1088,7 @@
 struct generic_product_impl<Lhs, Inverse<Rhs>, MatrixShape, PermutationShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Inverse<Rhs>& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Inverse<Rhs>& rhs)
   {
     permutation_matrix_product<Lhs, OnTheRight, true, MatrixShape>::run(dst, rhs.nestedExpression(), lhs);
   }
@@ -1043,9 +1110,9 @@
 {
   typedef typename nested_eval<ExpressionType, 1>::type MatrixType;
   typedef typename remove_all<MatrixType>::type MatrixTypeCleaned;
-  
+
   template<typename Dest, typename TranspositionType>
-  static inline void run(Dest& dst, const TranspositionType& tr, const ExpressionType& xpr)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void run(Dest& dst, const TranspositionType& tr, const ExpressionType& xpr)
   {
     MatrixType mat(xpr);
     typedef typename TranspositionType::StorageIndex StorageIndex;
@@ -1068,7 +1135,7 @@
 struct generic_product_impl<Lhs, Rhs, TranspositionsShape, MatrixShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
   {
     transposition_matrix_product<Rhs, OnTheLeft, false, MatrixShape>::run(dst, lhs, rhs);
   }
@@ -1078,7 +1145,7 @@
 struct generic_product_impl<Lhs, Rhs, MatrixShape, TranspositionsShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Rhs& rhs)
   {
     transposition_matrix_product<Lhs, OnTheRight, false, MatrixShape>::run(dst, rhs, lhs);
   }
@@ -1089,7 +1156,7 @@
 struct generic_product_impl<Transpose<Lhs>, Rhs, TranspositionsShape, MatrixShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Transpose<Lhs>& lhs, const Rhs& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Transpose<Lhs>& lhs, const Rhs& rhs)
   {
     transposition_matrix_product<Rhs, OnTheLeft, true, MatrixShape>::run(dst, lhs.nestedExpression(), rhs);
   }
@@ -1099,7 +1166,7 @@
 struct generic_product_impl<Lhs, Transpose<Rhs>, MatrixShape, TranspositionsShape, ProductTag>
 {
   template<typename Dest>
-  static void evalTo(Dest& dst, const Lhs& lhs, const Transpose<Rhs>& rhs)
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void evalTo(Dest& dst, const Lhs& lhs, const Transpose<Rhs>& rhs)
   {
     transposition_matrix_product<Lhs, OnTheRight, true, MatrixShape>::run(dst, rhs.nestedExpression(), lhs);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h
index 6faf789..dab2ac8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Random.h
@@ -128,7 +128,7 @@
   * \sa class CwiseNullaryOp, setRandom(Index), setRandom(Index,Index)
   */
 template<typename Derived>
-inline Derived& DenseBase<Derived>::setRandom()
+EIGEN_DEVICE_FUNC inline Derived& DenseBase<Derived>::setRandom()
 {
   return *this = Random(rows(), cols());
 }
@@ -177,6 +177,42 @@
   return setRandom();
 }
 
+/** Resizes to the given size, changing only the number of columns, and sets all
+  * coefficients in this expression to random values. For the parameter of type
+  * NoChange_t, just pass the special value \c NoChange.
+  *
+  * Numbers are uniformly spread through their whole definition range for integer types,
+  * and in the [-1:1] range for floating point scalar types.
+  *
+  * \not_reentrant
+  *
+  * \sa DenseBase::setRandom(), setRandom(Index), setRandom(Index, NoChange_t), class CwiseNullaryOp, DenseBase::Random()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setRandom(NoChange_t, Index cols)
+{
+  return setRandom(rows(), cols);
+}
+
+/** Resizes to the given size, changing only the number of rows, and sets all
+  * coefficients in this expression to random values. For the parameter of type
+  * NoChange_t, just pass the special value \c NoChange.
+  *
+  * Numbers are uniformly spread through their whole definition range for integer types,
+  * and in the [-1:1] range for floating point scalar types.
+  *
+  * \not_reentrant
+  *
+  * \sa DenseBase::setRandom(), setRandom(Index), setRandom(NoChange_t, Index), class CwiseNullaryOp, DenseBase::Random()
+  */
+template<typename Derived>
+EIGEN_STRONG_INLINE Derived&
+PlainObjectBase<Derived>::setRandom(Index rows, NoChange_t)
+{
+  return setRandom(rows, cols());
+}
+
 } // end namespace Eigen
 
 #endif // EIGEN_RANDOM_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
index 760e9f8..b6790d1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Redux.h
@@ -23,23 +23,29 @@
 * Part 1 : the logic deciding a strategy for vectorization and unrolling
 ***************************************************************************/
 
-template<typename Func, typename Derived>
+template<typename Func, typename Evaluator>
 struct redux_traits
 {
 public:
-    typedef typename find_best_packet<typename Derived::Scalar,Derived::SizeAtCompileTime>::type PacketType;
+    typedef typename find_best_packet<typename Evaluator::Scalar,Evaluator::SizeAtCompileTime>::type PacketType;
   enum {
     PacketSize = unpacket_traits<PacketType>::size,
-    InnerMaxSize = int(Derived::IsRowMajor)
-                 ? Derived::MaxColsAtCompileTime
-                 : Derived::MaxRowsAtCompileTime
+    InnerMaxSize = int(Evaluator::IsRowMajor)
+                 ? Evaluator::MaxColsAtCompileTime
+                 : Evaluator::MaxRowsAtCompileTime,
+    OuterMaxSize = int(Evaluator::IsRowMajor)
+                 ? Evaluator::MaxRowsAtCompileTime
+                 : Evaluator::MaxColsAtCompileTime,
+    SliceVectorizedWork = int(InnerMaxSize)==Dynamic ? Dynamic
+                        : int(OuterMaxSize)==Dynamic ? (int(InnerMaxSize)>=int(PacketSize) ? Dynamic : 0)
+                        : (int(InnerMaxSize)/int(PacketSize)) * int(OuterMaxSize)
   };
 
   enum {
-    MightVectorize = (int(Derived::Flags)&ActualPacketAccessBit)
+    MightVectorize = (int(Evaluator::Flags)&ActualPacketAccessBit)
                   && (functor_traits<Func>::PacketAccess),
-    MayLinearVectorize = bool(MightVectorize) && (int(Derived::Flags)&LinearAccessBit),
-    MaySliceVectorize  = bool(MightVectorize) && int(InnerMaxSize)>=3*PacketSize
+    MayLinearVectorize = bool(MightVectorize) && (int(Evaluator::Flags)&LinearAccessBit),
+    MaySliceVectorize  = bool(MightVectorize) && (int(SliceVectorizedWork)==Dynamic || int(SliceVectorizedWork)>=3)
   };
 
 public:
@@ -51,8 +57,8 @@
 
 public:
   enum {
-    Cost = Derived::SizeAtCompileTime == Dynamic ? HugeCost
-         : Derived::SizeAtCompileTime * Derived::CoeffReadCost + (Derived::SizeAtCompileTime-1) * functor_traits<Func>::Cost,
+    Cost = Evaluator::SizeAtCompileTime == Dynamic ? HugeCost
+         : int(Evaluator::SizeAtCompileTime) * int(Evaluator::CoeffReadCost) + (Evaluator::SizeAtCompileTime-1) * functor_traits<Func>::Cost,
     UnrollingLimit = EIGEN_UNROLLING_LIMIT * (int(Traversal) == int(DefaultTraversal) ? 1 : int(PacketSize))
   };
 
@@ -64,18 +70,20 @@
 #ifdef EIGEN_DEBUG_ASSIGN
   static void debug()
   {
-    std::cerr << "Xpr: " << typeid(typename Derived::XprType).name() << std::endl;
+    std::cerr << "Xpr: " << typeid(typename Evaluator::XprType).name() << std::endl;
     std::cerr.setf(std::ios::hex, std::ios::basefield);
-    EIGEN_DEBUG_VAR(Derived::Flags)
+    EIGEN_DEBUG_VAR(Evaluator::Flags)
     std::cerr.unsetf(std::ios::hex);
     EIGEN_DEBUG_VAR(InnerMaxSize)
+    EIGEN_DEBUG_VAR(OuterMaxSize)
+    EIGEN_DEBUG_VAR(SliceVectorizedWork)
     EIGEN_DEBUG_VAR(PacketSize)
     EIGEN_DEBUG_VAR(MightVectorize)
     EIGEN_DEBUG_VAR(MayLinearVectorize)
     EIGEN_DEBUG_VAR(MaySliceVectorize)
-    EIGEN_DEBUG_VAR(Traversal)
+    std::cerr << "Traversal" << " = " << Traversal << " (" << demangle_traversal(Traversal) << ")" << std::endl;
     EIGEN_DEBUG_VAR(UnrollingLimit)
-    EIGEN_DEBUG_VAR(Unrolling)
+    std::cerr << "Unrolling" << " = " << Unrolling << " (" << demangle_unrolling(Unrolling) << ")" << std::endl;
     std::cerr << std::endl;
   }
 #endif
@@ -87,88 +95,86 @@
 
 /*** no vectorization ***/
 
-template<typename Func, typename Derived, int Start, int Length>
+template<typename Func, typename Evaluator, int Start, int Length>
 struct redux_novec_unroller
 {
   enum {
     HalfLength = Length/2
   };
 
-  typedef typename Derived::Scalar Scalar;
+  typedef typename Evaluator::Scalar Scalar;
 
   EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func)
+  static EIGEN_STRONG_INLINE Scalar run(const Evaluator &eval, const Func& func)
   {
-    return func(redux_novec_unroller<Func, Derived, Start, HalfLength>::run(mat,func),
-                redux_novec_unroller<Func, Derived, Start+HalfLength, Length-HalfLength>::run(mat,func));
+    return func(redux_novec_unroller<Func, Evaluator, Start, HalfLength>::run(eval,func),
+                redux_novec_unroller<Func, Evaluator, Start+HalfLength, Length-HalfLength>::run(eval,func));
   }
 };
 
-template<typename Func, typename Derived, int Start>
-struct redux_novec_unroller<Func, Derived, Start, 1>
+template<typename Func, typename Evaluator, int Start>
+struct redux_novec_unroller<Func, Evaluator, Start, 1>
 {
   enum {
-    outer = Start / Derived::InnerSizeAtCompileTime,
-    inner = Start % Derived::InnerSizeAtCompileTime
+    outer = Start / Evaluator::InnerSizeAtCompileTime,
+    inner = Start % Evaluator::InnerSizeAtCompileTime
   };
 
-  typedef typename Derived::Scalar Scalar;
+  typedef typename Evaluator::Scalar Scalar;
 
   EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func&)
+  static EIGEN_STRONG_INLINE Scalar run(const Evaluator &eval, const Func&)
   {
-    return mat.coeffByOuterInner(outer, inner);
+    return eval.coeffByOuterInner(outer, inner);
   }
 };
 
 // This is actually dead code and will never be called. It is required
 // to prevent false warnings regarding failed inlining though
 // for 0 length run() will never be called at all.
-template<typename Func, typename Derived, int Start>
-struct redux_novec_unroller<Func, Derived, Start, 0>
+template<typename Func, typename Evaluator, int Start>
+struct redux_novec_unroller<Func, Evaluator, Start, 0>
 {
-  typedef typename Derived::Scalar Scalar;
+  typedef typename Evaluator::Scalar Scalar;
   EIGEN_DEVICE_FUNC 
-  static EIGEN_STRONG_INLINE Scalar run(const Derived&, const Func&) { return Scalar(); }
+  static EIGEN_STRONG_INLINE Scalar run(const Evaluator&, const Func&) { return Scalar(); }
 };
 
 /*** vectorization ***/
 
-template<typename Func, typename Derived, int Start, int Length>
+template<typename Func, typename Evaluator, int Start, int Length>
 struct redux_vec_unroller
 {
-  enum {
-    PacketSize = redux_traits<Func, Derived>::PacketSize,
-    HalfLength = Length/2
-  };
-
-  typedef typename Derived::Scalar Scalar;
-  typedef typename redux_traits<Func, Derived>::PacketType PacketScalar;
-
-  static EIGEN_STRONG_INLINE PacketScalar run(const Derived &mat, const Func& func)
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE PacketType run(const Evaluator &eval, const Func& func)
   {
+    enum {
+      PacketSize = unpacket_traits<PacketType>::size,
+      HalfLength = Length/2
+    };
+
     return func.packetOp(
-            redux_vec_unroller<Func, Derived, Start, HalfLength>::run(mat,func),
-            redux_vec_unroller<Func, Derived, Start+HalfLength, Length-HalfLength>::run(mat,func) );
+            redux_vec_unroller<Func, Evaluator, Start, HalfLength>::template run<PacketType>(eval,func),
+            redux_vec_unroller<Func, Evaluator, Start+HalfLength, Length-HalfLength>::template run<PacketType>(eval,func) );
   }
 };
 
-template<typename Func, typename Derived, int Start>
-struct redux_vec_unroller<Func, Derived, Start, 1>
+template<typename Func, typename Evaluator, int Start>
+struct redux_vec_unroller<Func, Evaluator, Start, 1>
 {
-  enum {
-    index = Start * redux_traits<Func, Derived>::PacketSize,
-    outer = index / int(Derived::InnerSizeAtCompileTime),
-    inner = index % int(Derived::InnerSizeAtCompileTime),
-    alignment = Derived::Alignment
-  };
-
-  typedef typename Derived::Scalar Scalar;
-  typedef typename redux_traits<Func, Derived>::PacketType PacketScalar;
-
-  static EIGEN_STRONG_INLINE PacketScalar run(const Derived &mat, const Func&)
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE PacketType run(const Evaluator &eval, const Func&)
   {
-    return mat.template packetByOuterInner<alignment,PacketScalar>(outer, inner);
+    enum {
+      PacketSize = unpacket_traits<PacketType>::size,
+      index = Start * PacketSize,
+      outer = index / int(Evaluator::InnerSizeAtCompileTime),
+      inner = index % int(Evaluator::InnerSizeAtCompileTime),
+      alignment = Evaluator::Alignment
+    };
+    return eval.template packetByOuterInner<alignment,PacketType>(outer, inner);
   }
 };
 
@@ -176,53 +182,65 @@
 * Part 3 : implementation of all cases
 ***************************************************************************/
 
-template<typename Func, typename Derived,
-         int Traversal = redux_traits<Func, Derived>::Traversal,
-         int Unrolling = redux_traits<Func, Derived>::Unrolling
+template<typename Func, typename Evaluator,
+         int Traversal = redux_traits<Func, Evaluator>::Traversal,
+         int Unrolling = redux_traits<Func, Evaluator>::Unrolling
 >
 struct redux_impl;
 
-template<typename Func, typename Derived>
-struct redux_impl<Func, Derived, DefaultTraversal, NoUnrolling>
+template<typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, DefaultTraversal, NoUnrolling>
 {
-  typedef typename Derived::Scalar Scalar;
-  EIGEN_DEVICE_FUNC
-  static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func)
+  typedef typename Evaluator::Scalar Scalar;
+
+  template<typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
+  Scalar run(const Evaluator &eval, const Func& func, const XprType& xpr)
   {
-    eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix");
+    eigen_assert(xpr.rows()>0 && xpr.cols()>0 && "you are using an empty matrix");
     Scalar res;
-    res = mat.coeffByOuterInner(0, 0);
-    for(Index i = 1; i < mat.innerSize(); ++i)
-      res = func(res, mat.coeffByOuterInner(0, i));
-    for(Index i = 1; i < mat.outerSize(); ++i)
-      for(Index j = 0; j < mat.innerSize(); ++j)
-        res = func(res, mat.coeffByOuterInner(i, j));
+    res = eval.coeffByOuterInner(0, 0);
+    for(Index i = 1; i < xpr.innerSize(); ++i)
+      res = func(res, eval.coeffByOuterInner(0, i));
+    for(Index i = 1; i < xpr.outerSize(); ++i)
+      for(Index j = 0; j < xpr.innerSize(); ++j)
+        res = func(res, eval.coeffByOuterInner(i, j));
     return res;
   }
 };
 
-template<typename Func, typename Derived>
-struct redux_impl<Func,Derived, DefaultTraversal, CompleteUnrolling>
-  : public redux_novec_unroller<Func,Derived, 0, Derived::SizeAtCompileTime>
-{};
-
-template<typename Func, typename Derived>
-struct redux_impl<Func, Derived, LinearVectorizedTraversal, NoUnrolling>
+template<typename Func, typename Evaluator>
+struct redux_impl<Func,Evaluator, DefaultTraversal, CompleteUnrolling>
+  : redux_novec_unroller<Func,Evaluator, 0, Evaluator::SizeAtCompileTime>
 {
-  typedef typename Derived::Scalar Scalar;
-  typedef typename redux_traits<Func, Derived>::PacketType PacketScalar;
-
-  static Scalar run(const Derived &mat, const Func& func)
+  typedef redux_novec_unroller<Func,Evaluator, 0, Evaluator::SizeAtCompileTime> Base;
+  typedef typename Evaluator::Scalar Scalar;
+  template<typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
+  Scalar run(const Evaluator &eval, const Func& func, const XprType& /*xpr*/)
   {
-    const Index size = mat.size();
+    return Base::run(eval,func);
+  }
+};
+
+template<typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, LinearVectorizedTraversal, NoUnrolling>
+{
+  typedef typename Evaluator::Scalar Scalar;
+  typedef typename redux_traits<Func, Evaluator>::PacketType PacketScalar;
+
+  template<typename XprType>
+  static Scalar run(const Evaluator &eval, const Func& func, const XprType& xpr)
+  {
+    const Index size = xpr.size();
     
-    const Index packetSize = redux_traits<Func, Derived>::PacketSize;
+    const Index packetSize = redux_traits<Func, Evaluator>::PacketSize;
     const int packetAlignment = unpacket_traits<PacketScalar>::alignment;
     enum {
-      alignment0 = (bool(Derived::Flags & DirectAccessBit) && bool(packet_traits<Scalar>::AlignedOnScalar)) ? int(packetAlignment) : int(Unaligned),
-      alignment = EIGEN_PLAIN_ENUM_MAX(alignment0, Derived::Alignment)
+      alignment0 = (bool(Evaluator::Flags & DirectAccessBit) && bool(packet_traits<Scalar>::AlignedOnScalar)) ? int(packetAlignment) : int(Unaligned),
+      alignment = EIGEN_PLAIN_ENUM_MAX(alignment0, Evaluator::Alignment)
     };
-    const Index alignedStart = internal::first_default_aligned(mat.nestedExpression());
+    const Index alignedStart = internal::first_default_aligned(xpr);
     const Index alignedSize2 = ((size-alignedStart)/(2*packetSize))*(2*packetSize);
     const Index alignedSize = ((size-alignedStart)/(packetSize))*(packetSize);
     const Index alignedEnd2 = alignedStart + alignedSize2;
@@ -230,34 +248,34 @@
     Scalar res;
     if(alignedSize)
     {
-      PacketScalar packet_res0 = mat.template packet<alignment,PacketScalar>(alignedStart);
+      PacketScalar packet_res0 = eval.template packet<alignment,PacketScalar>(alignedStart);
       if(alignedSize>packetSize) // we have at least two packets to partly unroll the loop
       {
-        PacketScalar packet_res1 = mat.template packet<alignment,PacketScalar>(alignedStart+packetSize);
+        PacketScalar packet_res1 = eval.template packet<alignment,PacketScalar>(alignedStart+packetSize);
         for(Index index = alignedStart + 2*packetSize; index < alignedEnd2; index += 2*packetSize)
         {
-          packet_res0 = func.packetOp(packet_res0, mat.template packet<alignment,PacketScalar>(index));
-          packet_res1 = func.packetOp(packet_res1, mat.template packet<alignment,PacketScalar>(index+packetSize));
+          packet_res0 = func.packetOp(packet_res0, eval.template packet<alignment,PacketScalar>(index));
+          packet_res1 = func.packetOp(packet_res1, eval.template packet<alignment,PacketScalar>(index+packetSize));
         }
 
         packet_res0 = func.packetOp(packet_res0,packet_res1);
         if(alignedEnd>alignedEnd2)
-          packet_res0 = func.packetOp(packet_res0, mat.template packet<alignment,PacketScalar>(alignedEnd2));
+          packet_res0 = func.packetOp(packet_res0, eval.template packet<alignment,PacketScalar>(alignedEnd2));
       }
       res = func.predux(packet_res0);
 
       for(Index index = 0; index < alignedStart; ++index)
-        res = func(res,mat.coeff(index));
+        res = func(res,eval.coeff(index));
 
       for(Index index = alignedEnd; index < size; ++index)
-        res = func(res,mat.coeff(index));
+        res = func(res,eval.coeff(index));
     }
     else // too small to vectorize anything.
          // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
     {
-      res = mat.coeff(0);
+      res = eval.coeff(0);
       for(Index index = 1; index < size; ++index)
-        res = func(res,mat.coeff(index));
+        res = func(res,eval.coeff(index));
     }
 
     return res;
@@ -265,130 +283,108 @@
 };
 
 // NOTE: for SliceVectorizedTraversal we simply bypass unrolling
-template<typename Func, typename Derived, int Unrolling>
-struct redux_impl<Func, Derived, SliceVectorizedTraversal, Unrolling>
+template<typename Func, typename Evaluator, int Unrolling>
+struct redux_impl<Func, Evaluator, SliceVectorizedTraversal, Unrolling>
 {
-  typedef typename Derived::Scalar Scalar;
-  typedef typename redux_traits<Func, Derived>::PacketType PacketType;
+  typedef typename Evaluator::Scalar Scalar;
+  typedef typename redux_traits<Func, Evaluator>::PacketType PacketType;
 
-  EIGEN_DEVICE_FUNC static Scalar run(const Derived &mat, const Func& func)
+  template<typename XprType>
+  EIGEN_DEVICE_FUNC static Scalar run(const Evaluator &eval, const Func& func, const XprType& xpr)
   {
-    eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix");
-    const Index innerSize = mat.innerSize();
-    const Index outerSize = mat.outerSize();
+    eigen_assert(xpr.rows()>0 && xpr.cols()>0 && "you are using an empty matrix");
+    const Index innerSize = xpr.innerSize();
+    const Index outerSize = xpr.outerSize();
     enum {
-      packetSize = redux_traits<Func, Derived>::PacketSize
+      packetSize = redux_traits<Func, Evaluator>::PacketSize
     };
     const Index packetedInnerSize = ((innerSize)/packetSize)*packetSize;
     Scalar res;
     if(packetedInnerSize)
     {
-      PacketType packet_res = mat.template packet<Unaligned,PacketType>(0,0);
+      PacketType packet_res = eval.template packet<Unaligned,PacketType>(0,0);
       for(Index j=0; j<outerSize; ++j)
         for(Index i=(j==0?packetSize:0); i<packetedInnerSize; i+=Index(packetSize))
-          packet_res = func.packetOp(packet_res, mat.template packetByOuterInner<Unaligned,PacketType>(j,i));
+          packet_res = func.packetOp(packet_res, eval.template packetByOuterInner<Unaligned,PacketType>(j,i));
 
       res = func.predux(packet_res);
       for(Index j=0; j<outerSize; ++j)
         for(Index i=packetedInnerSize; i<innerSize; ++i)
-          res = func(res, mat.coeffByOuterInner(j,i));
+          res = func(res, eval.coeffByOuterInner(j,i));
     }
     else // too small to vectorize anything.
          // since this is dynamic-size hence inefficient anyway for such small sizes, don't try to optimize.
     {
-      res = redux_impl<Func, Derived, DefaultTraversal, NoUnrolling>::run(mat, func);
+      res = redux_impl<Func, Evaluator, DefaultTraversal, NoUnrolling>::run(eval, func, xpr);
     }
 
     return res;
   }
 };
 
-template<typename Func, typename Derived>
-struct redux_impl<Func, Derived, LinearVectorizedTraversal, CompleteUnrolling>
+template<typename Func, typename Evaluator>
+struct redux_impl<Func, Evaluator, LinearVectorizedTraversal, CompleteUnrolling>
 {
-  typedef typename Derived::Scalar Scalar;
+  typedef typename Evaluator::Scalar Scalar;
 
-  typedef typename redux_traits<Func, Derived>::PacketType PacketScalar;
+  typedef typename redux_traits<Func, Evaluator>::PacketType PacketType;
   enum {
-    PacketSize = redux_traits<Func, Derived>::PacketSize,
-    Size = Derived::SizeAtCompileTime,
-    VectorizedSize = (Size / PacketSize) * PacketSize
+    PacketSize = redux_traits<Func, Evaluator>::PacketSize,
+    Size = Evaluator::SizeAtCompileTime,
+    VectorizedSize = (int(Size) / int(PacketSize)) * int(PacketSize)
   };
-  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE Scalar run(const Derived &mat, const Func& func)
+
+  template<typename XprType>
+  EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE
+  Scalar run(const Evaluator &eval, const Func& func, const XprType &xpr)
   {
-    eigen_assert(mat.rows()>0 && mat.cols()>0 && "you are using an empty matrix");
+    EIGEN_ONLY_USED_FOR_DEBUG(xpr)
+    eigen_assert(xpr.rows()>0 && xpr.cols()>0 && "you are using an empty matrix");
     if (VectorizedSize > 0) {
-      Scalar res = func.predux(redux_vec_unroller<Func, Derived, 0, Size / PacketSize>::run(mat,func));
+      Scalar res = func.predux(redux_vec_unroller<Func, Evaluator, 0, Size / PacketSize>::template run<PacketType>(eval,func));
       if (VectorizedSize != Size)
-        res = func(res,redux_novec_unroller<Func, Derived, VectorizedSize, Size-VectorizedSize>::run(mat,func));
+        res = func(res,redux_novec_unroller<Func, Evaluator, VectorizedSize, Size-VectorizedSize>::run(eval,func));
       return res;
     }
     else {
-      return redux_novec_unroller<Func, Derived, 0, Size>::run(mat,func);
+      return redux_novec_unroller<Func, Evaluator, 0, Size>::run(eval,func);
     }
   }
 };
 
 // evaluator adaptor
 template<typename _XprType>
-class redux_evaluator
+class redux_evaluator : public internal::evaluator<_XprType>
 {
+  typedef internal::evaluator<_XprType> Base;
 public:
   typedef _XprType XprType;
-  EIGEN_DEVICE_FUNC explicit redux_evaluator(const XprType &xpr) : m_evaluator(xpr), m_xpr(xpr) {}
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  explicit redux_evaluator(const XprType &xpr) : Base(xpr) {}
   
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
   typedef typename XprType::PacketScalar PacketScalar;
-  typedef typename XprType::PacketReturnType PacketReturnType;
   
   enum {
     MaxRowsAtCompileTime = XprType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = XprType::MaxColsAtCompileTime,
     // TODO we should not remove DirectAccessBit and rather find an elegant way to query the alignment offset at runtime from the evaluator
-    Flags = evaluator<XprType>::Flags & ~DirectAccessBit,
+    Flags = Base::Flags & ~DirectAccessBit,
     IsRowMajor = XprType::IsRowMajor,
     SizeAtCompileTime = XprType::SizeAtCompileTime,
-    InnerSizeAtCompileTime = XprType::InnerSizeAtCompileTime,
-    CoeffReadCost = evaluator<XprType>::CoeffReadCost,
-    Alignment = evaluator<XprType>::Alignment
+    InnerSizeAtCompileTime = XprType::InnerSizeAtCompileTime
   };
   
-  EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); }
-  EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); }
-  EIGEN_DEVICE_FUNC Index size() const { return m_xpr.size(); }
-  EIGEN_DEVICE_FUNC Index innerSize() const { return m_xpr.innerSize(); }
-  EIGEN_DEVICE_FUNC Index outerSize() const { return m_xpr.outerSize(); }
-
-  EIGEN_DEVICE_FUNC
-  CoeffReturnType coeff(Index row, Index col) const
-  { return m_evaluator.coeff(row, col); }
-
-  EIGEN_DEVICE_FUNC
-  CoeffReturnType coeff(Index index) const
-  { return m_evaluator.coeff(index); }
-
-  template<int LoadMode, typename PacketType>
-  PacketType packet(Index row, Index col) const
-  { return m_evaluator.template packet<LoadMode,PacketType>(row, col); }
-
-  template<int LoadMode, typename PacketType>
-  PacketType packet(Index index) const
-  { return m_evaluator.template packet<LoadMode,PacketType>(index); }
-  
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   CoeffReturnType coeffByOuterInner(Index outer, Index inner) const
-  { return m_evaluator.coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); }
+  { return Base::coeff(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); }
   
   template<int LoadMode, typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
   PacketType packetByOuterInner(Index outer, Index inner) const
-  { return m_evaluator.template packet<LoadMode,PacketType>(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); }
+  { return Base::template packet<LoadMode,PacketType>(IsRowMajor ? outer : inner, IsRowMajor ? inner : outer); }
   
-  const XprType & nestedExpression() const { return m_xpr; }
-  
-protected:
-  internal::evaluator<XprType> m_evaluator;
-  const XprType &m_xpr;
 };
 
 } // end namespace internal
@@ -403,39 +399,53 @@
   * The template parameter \a BinaryOp is the type of the functor \a func which must be
   * an associative operator. Both current C++98 and C++11 functor styles are handled.
   *
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
+  *
   * \sa DenseBase::sum(), DenseBase::minCoeff(), DenseBase::maxCoeff(), MatrixBase::colwise(), MatrixBase::rowwise()
   */
 template<typename Derived>
 template<typename Func>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::redux(const Func& func) const
 {
   eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
 
   typedef typename internal::redux_evaluator<Derived> ThisEvaluator;
   ThisEvaluator thisEval(derived());
-  
-  return internal::redux_impl<Func, ThisEvaluator>::run(thisEval, func);
+
+  // The initial expression is passed to the reducer as an additional argument instead of
+  // passing it as a member of redux_evaluator to help  
+  return internal::redux_impl<Func, ThisEvaluator>::run(thisEval, func, derived());
 }
 
 /** \returns the minimum of all coefficients of \c *this.
-  * \warning the result is undefined if \c *this contains NaN.
+  * In case \c *this contains NaN, NaNPropagation determines the behavior:
+  *   NaNPropagation == PropagateFast : undefined
+  *   NaNPropagation == PropagateNaN : result is NaN
+  *   NaNPropagation == PropagateNumbers : result is minimum of elements that are not NaN
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+template<int NaNPropagation>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::minCoeff() const
 {
-  return derived().redux(Eigen::internal::scalar_min_op<Scalar,Scalar>());
+  return derived().redux(Eigen::internal::scalar_min_op<Scalar,Scalar, NaNPropagation>());
 }
 
-/** \returns the maximum of all coefficients of \c *this.
-  * \warning the result is undefined if \c *this contains NaN.
+/** \returns the maximum of all coefficients of \c *this. 
+  * In case \c *this contains NaN, NaNPropagation determines the behavior:
+  *   NaNPropagation == PropagateFast : undefined
+  *   NaNPropagation == PropagateNaN : result is NaN
+  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+template<int NaNPropagation>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::maxCoeff() const
 {
-  return derived().redux(Eigen::internal::scalar_max_op<Scalar,Scalar>());
+  return derived().redux(Eigen::internal::scalar_max_op<Scalar,Scalar, NaNPropagation>());
 }
 
 /** \returns the sum of all coefficients of \c *this
@@ -445,7 +455,7 @@
   * \sa trace(), prod(), mean()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::sum() const
 {
   if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0))
@@ -458,7 +468,7 @@
 * \sa trace(), prod(), sum()
 */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::mean() const
 {
 #ifdef __INTEL_COMPILER
@@ -479,7 +489,7 @@
   * \sa sum(), mean(), trace()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::prod() const
 {
   if(SizeAtCompileTime==0 || (SizeAtCompileTime==Dynamic && size()==0))
@@ -494,7 +504,7 @@
   * \sa diagonal(), sum()
   */
 template<typename Derived>
-EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE typename internal::traits<Derived>::Scalar
 MatrixBase<Derived>::trace() const
 {
   return derived().diagonal().sum();
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
index 9c6e3c5..c2a37ea 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Ref.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_REF_H
 #define EIGEN_REF_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 
@@ -28,12 +28,13 @@
 
   template<typename Derived> struct match {
     enum {
+      IsVectorAtCompileTime = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime,
       HasDirectAccess = internal::has_direct_access<Derived>::ret,
-      StorageOrderMatch = PlainObjectType::IsVectorAtCompileTime || Derived::IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
+      StorageOrderMatch = IsVectorAtCompileTime || ((PlainObjectType::Flags&RowMajorBit)==(Derived::Flags&RowMajorBit)),
       InnerStrideMatch = int(StrideType::InnerStrideAtCompileTime)==int(Dynamic)
                       || int(StrideType::InnerStrideAtCompileTime)==int(Derived::InnerStrideAtCompileTime)
                       || (int(StrideType::InnerStrideAtCompileTime)==0 && int(Derived::InnerStrideAtCompileTime)==1),
-      OuterStrideMatch = Derived::IsVectorAtCompileTime
+      OuterStrideMatch = IsVectorAtCompileTime
                       || int(StrideType::OuterStrideAtCompileTime)==int(Dynamic) || int(StrideType::OuterStrideAtCompileTime)==int(Derived::OuterStrideAtCompileTime),
       // NOTE, this indirection of evaluator<Derived>::Alignment is needed
       // to workaround a very strange bug in MSVC related to the instantiation
@@ -47,7 +48,7 @@
     };
     typedef typename internal::conditional<MatchAtCompileTime,internal::true_type,internal::false_type>::type type;
   };
-  
+
 };
 
 template<typename Derived>
@@ -66,12 +67,12 @@
   typedef MapBase<Derived> Base;
   EIGEN_DENSE_PUBLIC_INTERFACE(RefBase)
 
-  EIGEN_DEVICE_FUNC inline Index innerStride() const
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index innerStride() const
   {
     return StrideType::InnerStrideAtCompileTime != 0 ? m_stride.inner() : 1;
   }
 
-  EIGEN_DEVICE_FUNC inline Index outerStride() const
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR inline Index outerStride() const
   {
     return StrideType::OuterStrideAtCompileTime != 0 ? m_stride.outer()
          : IsVectorAtCompileTime ? this->size()
@@ -85,36 +86,122 @@
       m_stride(StrideType::OuterStrideAtCompileTime==Dynamic?0:StrideType::OuterStrideAtCompileTime,
                StrideType::InnerStrideAtCompileTime==Dynamic?0:StrideType::InnerStrideAtCompileTime)
   {}
-  
+
   EIGEN_INHERIT_ASSIGNMENT_OPERATORS(RefBase)
 
 protected:
 
   typedef Stride<StrideType::OuterStrideAtCompileTime,StrideType::InnerStrideAtCompileTime> StrideBase;
 
-  template<typename Expression>
-  EIGEN_DEVICE_FUNC void construct(Expression& expr)
-  {
-    EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(PlainObjectType,Expression);
+  // Resolves inner stride if default 0.
+  static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveInnerStride(Index inner) {
+    return inner == 0 ? 1 : inner;
+  }
 
+  // Resolves outer stride if default 0.
+  static EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index resolveOuterStride(Index inner, Index outer, Index rows, Index cols, bool isVectorAtCompileTime, bool isRowMajor) {
+    return outer == 0 ? isVectorAtCompileTime ? inner * rows * cols : isRowMajor ? inner * cols : inner * rows : outer;
+  }
+
+  // Returns true if construction is valid, false if there is a stride mismatch,
+  // and fails if there is a size mismatch.
+  template<typename Expression>
+  EIGEN_DEVICE_FUNC bool construct(Expression& expr)
+  {
+    // Check matrix sizes.  If this is a compile-time vector, we do allow
+    // implicitly transposing.
+    EIGEN_STATIC_ASSERT(
+      EIGEN_PREDICATE_SAME_MATRIX_SIZE(PlainObjectType, Expression)
+      // If it is a vector, the transpose sizes might match.
+      || ( PlainObjectType::IsVectorAtCompileTime
+            && ((int(PlainObjectType::RowsAtCompileTime)==Eigen::Dynamic
+              || int(Expression::ColsAtCompileTime)==Eigen::Dynamic
+              || int(PlainObjectType::RowsAtCompileTime)==int(Expression::ColsAtCompileTime))
+            &&  (int(PlainObjectType::ColsAtCompileTime)==Eigen::Dynamic
+              || int(Expression::RowsAtCompileTime)==Eigen::Dynamic
+              || int(PlainObjectType::ColsAtCompileTime)==int(Expression::RowsAtCompileTime)))),
+      YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES
+    )
+
+    // Determine runtime rows and columns.
+    Index rows = expr.rows();
+    Index cols = expr.cols();
     if(PlainObjectType::RowsAtCompileTime==1)
     {
       eigen_assert(expr.rows()==1 || expr.cols()==1);
-      ::new (static_cast<Base*>(this)) Base(expr.data(), 1, expr.size());
+      rows = 1;
+      cols = expr.size();
     }
     else if(PlainObjectType::ColsAtCompileTime==1)
     {
       eigen_assert(expr.rows()==1 || expr.cols()==1);
-      ::new (static_cast<Base*>(this)) Base(expr.data(), expr.size(), 1);
+      rows = expr.size();
+      cols = 1;
     }
-    else
-      ::new (static_cast<Base*>(this)) Base(expr.data(), expr.rows(), expr.cols());
-    
-    if(Expression::IsVectorAtCompileTime && (!PlainObjectType::IsVectorAtCompileTime) && ((Expression::Flags&RowMajorBit)!=(PlainObjectType::Flags&RowMajorBit)))
-      ::new (&m_stride) StrideBase(expr.innerStride(), StrideType::InnerStrideAtCompileTime==0?0:1);
-    else
-      ::new (&m_stride) StrideBase(StrideType::OuterStrideAtCompileTime==0?0:expr.outerStride(),
-                                   StrideType::InnerStrideAtCompileTime==0?0:expr.innerStride());    
+    // Verify that the sizes are valid.
+    eigen_assert(
+      (PlainObjectType::RowsAtCompileTime == Dynamic) || (PlainObjectType::RowsAtCompileTime == rows));
+    eigen_assert(
+      (PlainObjectType::ColsAtCompileTime == Dynamic) || (PlainObjectType::ColsAtCompileTime == cols));
+
+
+    // If this is a vector, we might be transposing, which means that stride should swap.
+    const bool transpose = PlainObjectType::IsVectorAtCompileTime && (rows != expr.rows());
+    // If the storage format differs, we also need to swap the stride.
+    const bool row_major = ((PlainObjectType::Flags)&RowMajorBit) != 0;
+    const bool expr_row_major = (Expression::Flags&RowMajorBit) != 0;
+    const bool storage_differs =  (row_major != expr_row_major);
+
+    const bool swap_stride = (transpose != storage_differs);
+
+    // Determine expr's actual strides, resolving any defaults if zero.
+    const Index expr_inner_actual = resolveInnerStride(expr.innerStride());
+    const Index expr_outer_actual = resolveOuterStride(expr_inner_actual,
+                                                       expr.outerStride(),
+                                                       expr.rows(),
+                                                       expr.cols(),
+                                                       Expression::IsVectorAtCompileTime != 0,
+                                                       expr_row_major);
+
+    // If this is a column-major row vector or row-major column vector, the inner-stride
+    // is arbitrary, so set it to either the compile-time inner stride or 1.
+    const bool row_vector = (rows == 1);
+    const bool col_vector = (cols == 1);
+    const Index inner_stride =
+        ( (!row_major && row_vector) || (row_major && col_vector) ) ?
+            ( StrideType::InnerStrideAtCompileTime > 0 ? Index(StrideType::InnerStrideAtCompileTime) : 1)
+            : swap_stride ? expr_outer_actual : expr_inner_actual;
+
+    // If this is a column-major column vector or row-major row vector, the outer-stride
+    // is arbitrary, so set it to either the compile-time outer stride or vector size.
+    const Index outer_stride =
+      ( (!row_major && col_vector) || (row_major && row_vector) ) ?
+          ( StrideType::OuterStrideAtCompileTime > 0 ? Index(StrideType::OuterStrideAtCompileTime) : rows * cols * inner_stride)
+          : swap_stride ? expr_inner_actual : expr_outer_actual;
+
+    // Check if given inner/outer strides are compatible with compile-time strides.
+    const bool inner_valid = (StrideType::InnerStrideAtCompileTime == Dynamic)
+        || (resolveInnerStride(Index(StrideType::InnerStrideAtCompileTime)) == inner_stride);
+    if (!inner_valid) {
+      return false;
+    }
+
+    const bool outer_valid = (StrideType::OuterStrideAtCompileTime == Dynamic)
+        || (resolveOuterStride(
+              inner_stride,
+              Index(StrideType::OuterStrideAtCompileTime),
+              rows, cols, PlainObjectType::IsVectorAtCompileTime != 0,
+              row_major)
+            == outer_stride);
+    if (!outer_valid) {
+      return false;
+    }
+
+    ::new (static_cast<Base*>(this)) Base(expr.data(), rows, cols);
+    ::new (&m_stride) StrideBase(
+      (StrideType::OuterStrideAtCompileTime == 0) ? 0 : outer_stride,
+      (StrideType::InnerStrideAtCompileTime == 0) ? 0 : inner_stride );
+    return true;
   }
 
   StrideBase m_stride;
@@ -186,6 +273,8 @@
   * void foo(const Ref<MatrixXf,0,Stride<> >& A) { foo_impl(A); }
   * \endcode
   *
+  * See also the following stackoverflow questions for further references:
+  *  - <a href="http://stackoverflow.com/questions/21132538/correct-usage-of-the-eigenref-class">Correct usage of the Eigen::Ref<> class</a>
   *
   * \sa PlainObjectBase::Map(), \ref TopicStorageOrders
   */
@@ -209,7 +298,10 @@
                                  typename internal::enable_if<bool(Traits::template match<Derived>::MatchAtCompileTime),Derived>::type* = 0)
     {
       EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
-      Base::construct(expr.derived());
+      // Construction must pass since we will not create temprary storage in the non-const case.
+      const bool success = Base::construct(expr.derived());
+      EIGEN_UNUSED_VARIABLE(success)
+      eigen_assert(success);
     }
     template<typename Derived>
     EIGEN_DEVICE_FUNC inline Ref(const DenseBase<Derived>& expr,
@@ -223,7 +315,10 @@
       EIGEN_STATIC_ASSERT(bool(internal::is_lvalue<Derived>::value), THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
       EIGEN_STATIC_ASSERT(bool(Traits::template match<Derived>::MatchAtCompileTime), STORAGE_LAYOUT_DOES_NOT_MATCH);
       EIGEN_STATIC_ASSERT(!Derived::IsPlainObjectBase,THIS_EXPRESSION_IS_NOT_A_LVALUE__IT_IS_READ_ONLY);
-      Base::construct(expr.const_cast_derived());
+      // Construction must pass since we will not create temporary storage in the non-const case.
+      const bool success = Base::construct(expr.const_cast_derived());
+      EIGEN_UNUSED_VARIABLE(success)
+      eigen_assert(success);
     }
 
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Ref)
@@ -264,7 +359,10 @@
     template<typename Expression>
     EIGEN_DEVICE_FUNC void construct(const Expression& expr,internal::true_type)
     {
-      Base::construct(expr);
+      // Check if we can use the underlying expr's storage directly, otherwise call the copy version.
+      if (!Base::construct(expr)) {
+        construct(expr, internal::false_type());
+      }
     }
 
     template<typename Expression>
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
index 9960ef8..ab5be7e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Replicate.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_REPLICATE_H
 #define EIGEN_REPLICATE_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 template<typename MatrixType,int RowFactor,int ColFactor>
@@ -35,7 +35,7 @@
     IsRowMajor = MaxRowsAtCompileTime==1 && MaxColsAtCompileTime!=1 ? 1
                : MaxColsAtCompileTime==1 && MaxRowsAtCompileTime!=1 ? 0
                : (MatrixType::Flags & RowMajorBit) ? 1 : 0,
-    
+
     // FIXME enable DirectAccess with negative strides?
     Flags = IsRowMajor ? RowMajorBit : 0
   };
@@ -88,15 +88,15 @@
                           THE_MATRIX_OR_EXPRESSION_THAT_YOU_PASSED_DOES_NOT_HAVE_THE_EXPECTED_TYPE)
     }
 
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index rows() const { return m_matrix.rows() * m_rowFactor.value(); }
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index cols() const { return m_matrix.cols() * m_colFactor.value(); }
 
     EIGEN_DEVICE_FUNC
     const _MatrixTypeNested& nestedExpression() const
-    { 
-      return m_matrix; 
+    {
+      return m_matrix;
     }
 
   protected:
@@ -115,7 +115,7 @@
   */
 template<typename Derived>
 template<int RowFactor, int ColFactor>
-const Replicate<Derived,RowFactor,ColFactor>
+EIGEN_DEVICE_FUNC const Replicate<Derived,RowFactor,ColFactor>
 DenseBase<Derived>::replicate() const
 {
   return Replicate<Derived,RowFactor,ColFactor>(derived());
@@ -130,7 +130,7 @@
   * \sa VectorwiseOp::replicate(), DenseBase::replicate(), class Replicate
   */
 template<typename ExpressionType, int Direction>
-const typename VectorwiseOp<ExpressionType,Direction>::ReplicateReturnType
+EIGEN_DEVICE_FUNC const typename VectorwiseOp<ExpressionType,Direction>::ReplicateReturnType
 VectorwiseOp<ExpressionType,Direction>::replicate(Index factor) const
 {
   return typename VectorwiseOp<ExpressionType,Direction>::ReplicateReturnType
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h
new file mode 100644
index 0000000..52de73b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reshaped.h
@@ -0,0 +1,454 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2014 yoco <peter.xiau@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_RESHAPED_H
+#define EIGEN_RESHAPED_H
+
+namespace Eigen {
+
+/** \class Reshaped
+  * \ingroup Core_Module
+  *
+  * \brief Expression of a fixed-size or dynamic-size reshape
+  *
+  * \tparam XprType the type of the expression in which we are taking a reshape
+  * \tparam Rows the number of rows of the reshape we are taking at compile time (optional)
+  * \tparam Cols the number of columns of the reshape we are taking at compile time (optional)
+  * \tparam Order can be ColMajor or RowMajor, default is ColMajor.
+  *
+  * This class represents an expression of either a fixed-size or dynamic-size reshape.
+  * It is the return type of DenseBase::reshaped(NRowsType,NColsType) and
+  * most of the time this is the only way it is used.
+  *
+  * However, in C++98, if you want to directly maniputate reshaped expressions,
+  * for instance if you want to write a function returning such an expression, you
+  * will need to use this class. In C++11, it is advised to use the \em auto
+  * keyword for such use cases.
+  *
+  * Here is an example illustrating the dynamic case:
+  * \include class_Reshaped.cpp
+  * Output: \verbinclude class_Reshaped.out
+  *
+  * Here is an example illustrating the fixed-size case:
+  * \include class_FixedReshaped.cpp
+  * Output: \verbinclude class_FixedReshaped.out
+  *
+  * \sa DenseBase::reshaped(NRowsType,NColsType)
+  */
+
+namespace internal {
+
+template<typename XprType, int Rows, int Cols, int Order>
+struct traits<Reshaped<XprType, Rows, Cols, Order> > : traits<XprType>
+{
+  typedef typename traits<XprType>::Scalar Scalar;
+  typedef typename traits<XprType>::StorageKind StorageKind;
+  typedef typename traits<XprType>::XprKind XprKind;
+  enum{
+    MatrixRows = traits<XprType>::RowsAtCompileTime,
+    MatrixCols = traits<XprType>::ColsAtCompileTime,
+    RowsAtCompileTime = Rows,
+    ColsAtCompileTime = Cols,
+    MaxRowsAtCompileTime = Rows,
+    MaxColsAtCompileTime = Cols,
+    XpxStorageOrder = ((int(traits<XprType>::Flags) & RowMajorBit) == RowMajorBit) ? RowMajor : ColMajor,
+    ReshapedStorageOrder = (RowsAtCompileTime == 1 && ColsAtCompileTime != 1) ? RowMajor
+                         : (ColsAtCompileTime == 1 && RowsAtCompileTime != 1) ? ColMajor
+                         : XpxStorageOrder,
+    HasSameStorageOrderAsXprType = (ReshapedStorageOrder == XpxStorageOrder),
+    InnerSize = (ReshapedStorageOrder==int(RowMajor)) ? int(ColsAtCompileTime) : int(RowsAtCompileTime),
+    InnerStrideAtCompileTime = HasSameStorageOrderAsXprType
+                             ? int(inner_stride_at_compile_time<XprType>::ret)
+                             : Dynamic,
+    OuterStrideAtCompileTime = Dynamic,
+
+    HasDirectAccess = internal::has_direct_access<XprType>::ret
+                    && (Order==int(XpxStorageOrder))
+                    && ((evaluator<XprType>::Flags&LinearAccessBit)==LinearAccessBit),
+
+    MaskPacketAccessBit = (InnerSize == Dynamic || (InnerSize % packet_traits<Scalar>::size) == 0)
+                       && (InnerStrideAtCompileTime == 1)
+                        ? PacketAccessBit : 0,
+    //MaskAlignedBit = ((OuterStrideAtCompileTime!=Dynamic) && (((OuterStrideAtCompileTime * int(sizeof(Scalar))) % 16) == 0)) ? AlignedBit : 0,
+    FlagsLinearAccessBit = (RowsAtCompileTime == 1 || ColsAtCompileTime == 1) ? LinearAccessBit : 0,
+    FlagsLvalueBit = is_lvalue<XprType>::value ? LvalueBit : 0,
+    FlagsRowMajorBit = (ReshapedStorageOrder==int(RowMajor)) ? RowMajorBit : 0,
+    FlagsDirectAccessBit = HasDirectAccess ? DirectAccessBit : 0,
+    Flags0 = traits<XprType>::Flags & ( (HereditaryBits & ~RowMajorBit) | MaskPacketAccessBit),
+
+    Flags = (Flags0 | FlagsLinearAccessBit | FlagsLvalueBit | FlagsRowMajorBit | FlagsDirectAccessBit)
+  };
+};
+
+template<typename XprType, int Rows, int Cols, int Order, bool HasDirectAccess> class ReshapedImpl_dense;
+
+} // end namespace internal
+
+template<typename XprType, int Rows, int Cols, int Order, typename StorageKind> class ReshapedImpl;
+
+template<typename XprType, int Rows, int Cols, int Order> class Reshaped
+  : public ReshapedImpl<XprType, Rows, Cols, Order, typename internal::traits<XprType>::StorageKind>
+{
+    typedef ReshapedImpl<XprType, Rows, Cols, Order, typename internal::traits<XprType>::StorageKind> Impl;
+  public:
+    //typedef typename Impl::Base Base;
+    typedef Impl Base;
+    EIGEN_GENERIC_PUBLIC_INTERFACE(Reshaped)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reshaped)
+
+    /** Fixed-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline Reshaped(XprType& xpr)
+      : Impl(xpr)
+    {
+      EIGEN_STATIC_ASSERT(RowsAtCompileTime!=Dynamic && ColsAtCompileTime!=Dynamic,THIS_METHOD_IS_ONLY_FOR_FIXED_SIZE)
+      eigen_assert(Rows * Cols == xpr.rows() * xpr.cols());
+    }
+
+    /** Dynamic-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline Reshaped(XprType& xpr,
+          Index reshapeRows, Index reshapeCols)
+      : Impl(xpr, reshapeRows, reshapeCols)
+    {
+      eigen_assert((RowsAtCompileTime==Dynamic || RowsAtCompileTime==reshapeRows)
+          && (ColsAtCompileTime==Dynamic || ColsAtCompileTime==reshapeCols));
+      eigen_assert(reshapeRows * reshapeCols == xpr.rows() * xpr.cols());
+    }
+};
+
+// The generic default implementation for dense reshape simply forward to the internal::ReshapedImpl_dense
+// that must be specialized for direct and non-direct access...
+template<typename XprType, int Rows, int Cols, int Order>
+class ReshapedImpl<XprType, Rows, Cols, Order, Dense>
+  : public internal::ReshapedImpl_dense<XprType, Rows, Cols, Order,internal::traits<Reshaped<XprType,Rows,Cols,Order> >::HasDirectAccess>
+{
+    typedef internal::ReshapedImpl_dense<XprType, Rows, Cols, Order,internal::traits<Reshaped<XprType,Rows,Cols,Order> >::HasDirectAccess> Impl;
+  public:
+    typedef Impl Base;
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl)
+    EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr) : Impl(xpr) {}
+    EIGEN_DEVICE_FUNC inline ReshapedImpl(XprType& xpr, Index reshapeRows, Index reshapeCols)
+      : Impl(xpr, reshapeRows, reshapeCols) {}
+};
+
+namespace internal {
+
+/** \internal Internal implementation of dense Reshaped in the general case. */
+template<typename XprType, int Rows, int Cols, int Order>
+class ReshapedImpl_dense<XprType,Rows,Cols,Order,false>
+  : public internal::dense_xpr_base<Reshaped<XprType, Rows, Cols, Order> >::type
+{
+    typedef Reshaped<XprType, Rows, Cols, Order> ReshapedType;
+  public:
+
+    typedef typename internal::dense_xpr_base<ReshapedType>::type Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense)
+
+    typedef typename internal::ref_selector<XprType>::non_const_type MatrixTypeNested;
+    typedef typename internal::remove_all<XprType>::type NestedExpression;
+
+    class InnerIterator;
+
+    /** Fixed-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline ReshapedImpl_dense(XprType& xpr)
+      : m_xpr(xpr), m_rows(Rows), m_cols(Cols)
+    {}
+
+    /** Dynamic-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols)
+      : m_xpr(xpr), m_rows(nRows), m_cols(nCols)
+    {}
+
+    EIGEN_DEVICE_FUNC Index rows() const { return m_rows; }
+    EIGEN_DEVICE_FUNC Index cols() const { return m_cols; }
+
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    /** \sa MapBase::data() */
+    EIGEN_DEVICE_FUNC inline const Scalar* data() const;
+    EIGEN_DEVICE_FUNC inline Index innerStride() const;
+    EIGEN_DEVICE_FUNC inline Index outerStride() const;
+    #endif
+
+    /** \returns the nested expression */
+    EIGEN_DEVICE_FUNC
+    const typename internal::remove_all<XprType>::type&
+    nestedExpression() const { return m_xpr; }
+
+    /** \returns the nested expression */
+    EIGEN_DEVICE_FUNC
+    typename internal::remove_reference<XprType>::type&
+    nestedExpression() { return m_xpr; }
+
+  protected:
+
+    MatrixTypeNested m_xpr;
+    const internal::variable_if_dynamic<Index, Rows> m_rows;
+    const internal::variable_if_dynamic<Index, Cols> m_cols;
+};
+
+
+/** \internal Internal implementation of dense Reshaped in the direct access case. */
+template<typename XprType, int Rows, int Cols, int Order>
+class ReshapedImpl_dense<XprType, Rows, Cols, Order, true>
+  : public MapBase<Reshaped<XprType, Rows, Cols, Order> >
+{
+    typedef Reshaped<XprType, Rows, Cols, Order> ReshapedType;
+    typedef typename internal::ref_selector<XprType>::non_const_type XprTypeNested;
+  public:
+
+    typedef MapBase<ReshapedType> Base;
+    EIGEN_DENSE_PUBLIC_INTERFACE(ReshapedType)
+    EIGEN_INHERIT_ASSIGNMENT_OPERATORS(ReshapedImpl_dense)
+
+    /** Fixed-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline ReshapedImpl_dense(XprType& xpr)
+      : Base(xpr.data()), m_xpr(xpr)
+    {}
+
+    /** Dynamic-size constructor
+      */
+    EIGEN_DEVICE_FUNC
+    inline ReshapedImpl_dense(XprType& xpr, Index nRows, Index nCols)
+      : Base(xpr.data(), nRows, nCols),
+        m_xpr(xpr)
+    {}
+
+    EIGEN_DEVICE_FUNC
+    const typename internal::remove_all<XprTypeNested>::type& nestedExpression() const
+    {
+      return m_xpr;
+    }
+
+    EIGEN_DEVICE_FUNC
+    XprType& nestedExpression() { return m_xpr; }
+
+    /** \sa MapBase::innerStride() */
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const
+    {
+      return m_xpr.innerStride();
+    }
+
+    /** \sa MapBase::outerStride() */
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const
+    {
+      return ((Flags&RowMajorBit)==RowMajorBit) ? this->cols() : this->rows();
+    }
+
+  protected:
+
+    XprTypeNested m_xpr;
+};
+
+// Evaluators
+template<typename ArgType, int Rows, int Cols, int Order, bool HasDirectAccess> struct reshaped_evaluator;
+
+template<typename ArgType, int Rows, int Cols, int Order>
+struct evaluator<Reshaped<ArgType, Rows, Cols, Order> >
+  : reshaped_evaluator<ArgType, Rows, Cols, Order, traits<Reshaped<ArgType,Rows,Cols,Order> >::HasDirectAccess>
+{
+  typedef Reshaped<ArgType, Rows, Cols, Order> XprType;
+  typedef typename XprType::Scalar Scalar;
+  // TODO: should check for smaller packet types
+  typedef typename packet_traits<Scalar>::type PacketScalar;
+
+  enum {
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost,
+    HasDirectAccess = traits<XprType>::HasDirectAccess,
+
+//     RowsAtCompileTime = traits<XprType>::RowsAtCompileTime,
+//     ColsAtCompileTime = traits<XprType>::ColsAtCompileTime,
+//     MaxRowsAtCompileTime = traits<XprType>::MaxRowsAtCompileTime,
+//     MaxColsAtCompileTime = traits<XprType>::MaxColsAtCompileTime,
+//
+//     InnerStrideAtCompileTime = traits<XprType>::HasSameStorageOrderAsXprType
+//                              ? int(inner_stride_at_compile_time<ArgType>::ret)
+//                              : Dynamic,
+//     OuterStrideAtCompileTime = Dynamic,
+
+    FlagsLinearAccessBit = (traits<XprType>::RowsAtCompileTime == 1 || traits<XprType>::ColsAtCompileTime == 1 || HasDirectAccess) ? LinearAccessBit : 0,
+    FlagsRowMajorBit = (traits<XprType>::ReshapedStorageOrder==int(RowMajor)) ? RowMajorBit : 0,
+    FlagsDirectAccessBit =  HasDirectAccess ? DirectAccessBit : 0,
+    Flags0 = evaluator<ArgType>::Flags & (HereditaryBits & ~RowMajorBit),
+    Flags = Flags0 | FlagsLinearAccessBit | FlagsRowMajorBit | FlagsDirectAccessBit,
+
+    PacketAlignment = unpacket_traits<PacketScalar>::alignment,
+    Alignment = evaluator<ArgType>::Alignment
+  };
+  typedef reshaped_evaluator<ArgType, Rows, Cols, Order, HasDirectAccess> reshaped_evaluator_type;
+  EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr) : reshaped_evaluator_type(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+};
+
+template<typename ArgType, int Rows, int Cols, int Order>
+struct reshaped_evaluator<ArgType, Rows, Cols, Order, /* HasDirectAccess */ false>
+  : evaluator_base<Reshaped<ArgType, Rows, Cols, Order> >
+{
+  typedef Reshaped<ArgType, Rows, Cols, Order> XprType;
+
+  enum {
+    CoeffReadCost = evaluator<ArgType>::CoeffReadCost /* TODO + cost of index computations */,
+
+    Flags = (evaluator<ArgType>::Flags & (HereditaryBits /*| LinearAccessBit | DirectAccessBit*/)),
+
+    Alignment = 0
+  };
+
+  EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr) : m_argImpl(xpr.nestedExpression()), m_xpr(xpr)
+  {
+    EIGEN_INTERNAL_CHECK_COST_VALUE(CoeffReadCost);
+  }
+
+  typedef typename XprType::Scalar Scalar;
+  typedef typename XprType::CoeffReturnType CoeffReturnType;
+
+  typedef std::pair<Index, Index> RowCol;
+
+  inline RowCol index_remap(Index rowId, Index colId) const
+  {
+    if(Order==ColMajor)
+    {
+      const Index nth_elem_idx = colId * m_xpr.rows() + rowId;
+      return RowCol(nth_elem_idx % m_xpr.nestedExpression().rows(),
+                    nth_elem_idx / m_xpr.nestedExpression().rows());
+    }
+    else
+    {
+      const Index nth_elem_idx = colId + rowId * m_xpr.cols();
+      return RowCol(nth_elem_idx / m_xpr.nestedExpression().cols(),
+                    nth_elem_idx % m_xpr.nestedExpression().cols());
+    }
+  }
+
+  EIGEN_DEVICE_FUNC
+  inline Scalar& coeffRef(Index rowId, Index colId)
+  {
+    EIGEN_STATIC_ASSERT_LVALUE(XprType)
+    const RowCol row_col = index_remap(rowId, colId);
+    return m_argImpl.coeffRef(row_col.first, row_col.second);
+  }
+
+  EIGEN_DEVICE_FUNC
+  inline const Scalar& coeffRef(Index rowId, Index colId) const
+  {
+    const RowCol row_col = index_remap(rowId, colId);
+    return m_argImpl.coeffRef(row_col.first, row_col.second);
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE const CoeffReturnType coeff(Index rowId, Index colId) const
+  {
+    const RowCol row_col = index_remap(rowId, colId);
+    return m_argImpl.coeff(row_col.first, row_col.second);
+  }
+
+  EIGEN_DEVICE_FUNC
+  inline Scalar& coeffRef(Index index)
+  {
+    EIGEN_STATIC_ASSERT_LVALUE(XprType)
+    const RowCol row_col = index_remap(Rows == 1 ? 0 : index,
+                                       Rows == 1 ? index : 0);
+    return m_argImpl.coeffRef(row_col.first, row_col.second);
+
+  }
+
+  EIGEN_DEVICE_FUNC
+  inline const Scalar& coeffRef(Index index) const
+  {
+    const RowCol row_col = index_remap(Rows == 1 ? 0 : index,
+                                       Rows == 1 ? index : 0);
+    return m_argImpl.coeffRef(row_col.first, row_col.second);
+  }
+
+  EIGEN_DEVICE_FUNC
+  inline const CoeffReturnType coeff(Index index) const
+  {
+    const RowCol row_col = index_remap(Rows == 1 ? 0 : index,
+                                       Rows == 1 ? index : 0);
+    return m_argImpl.coeff(row_col.first, row_col.second);
+  }
+#if 0
+  EIGEN_DEVICE_FUNC
+  template<int LoadMode>
+  inline PacketScalar packet(Index rowId, Index colId) const
+  {
+    const RowCol row_col = index_remap(rowId, colId);
+    return m_argImpl.template packet<Unaligned>(row_col.first, row_col.second);
+
+  }
+
+  template<int LoadMode>
+  EIGEN_DEVICE_FUNC
+  inline void writePacket(Index rowId, Index colId, const PacketScalar& val)
+  {
+    const RowCol row_col = index_remap(rowId, colId);
+    m_argImpl.const_cast_derived().template writePacket<Unaligned>
+            (row_col.first, row_col.second, val);
+  }
+
+  template<int LoadMode>
+  EIGEN_DEVICE_FUNC
+  inline PacketScalar packet(Index index) const
+  {
+    const RowCol row_col = index_remap(RowsAtCompileTime == 1 ? 0 : index,
+                                        RowsAtCompileTime == 1 ? index : 0);
+    return m_argImpl.template packet<Unaligned>(row_col.first, row_col.second);
+  }
+
+  template<int LoadMode>
+  EIGEN_DEVICE_FUNC
+  inline void writePacket(Index index, const PacketScalar& val)
+  {
+    const RowCol row_col = index_remap(RowsAtCompileTime == 1 ? 0 : index,
+                                        RowsAtCompileTime == 1 ? index : 0);
+    return m_argImpl.template packet<Unaligned>(row_col.first, row_col.second, val);
+  }
+#endif
+protected:
+
+  evaluator<ArgType> m_argImpl;
+  const XprType& m_xpr;
+
+};
+
+template<typename ArgType, int Rows, int Cols, int Order>
+struct reshaped_evaluator<ArgType, Rows, Cols, Order, /* HasDirectAccess */ true>
+: mapbase_evaluator<Reshaped<ArgType, Rows, Cols, Order>,
+                      typename Reshaped<ArgType, Rows, Cols, Order>::PlainObject>
+{
+  typedef Reshaped<ArgType, Rows, Cols, Order> XprType;
+  typedef typename XprType::Scalar Scalar;
+
+  EIGEN_DEVICE_FUNC explicit reshaped_evaluator(const XprType& xpr)
+    : mapbase_evaluator<XprType, typename XprType::PlainObject>(xpr)
+  {
+    // TODO: for the 3.4 release, this should be turned to an internal assertion, but let's keep it as is for the beta lifetime
+    eigen_assert(((internal::UIntPtr(xpr.data()) % EIGEN_PLAIN_ENUM_MAX(1,evaluator<XprType>::Alignment)) == 0) && "data is not aligned");
+  }
+};
+
+} // end namespace internal
+
+} // end namespace Eigen
+
+#endif // EIGEN_RESHAPED_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
index c44b767..4dad13e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/ReturnByValue.h
@@ -60,8 +60,10 @@
     EIGEN_DEVICE_FUNC
     inline void evalTo(Dest& dst) const
     { static_cast<const Derived*>(this)->evalTo(dst); }
-    EIGEN_DEVICE_FUNC inline Index rows() const { return static_cast<const Derived*>(this)->rows(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return static_cast<const Derived*>(this)->cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return static_cast<const Derived*>(this)->rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return static_cast<const Derived*>(this)->cols(); }
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 #define Unusable YOU_ARE_TRYING_TO_ACCESS_A_SINGLE_COEFFICIENT_IN_A_SPECIAL_EXPRESSION_WHERE_THAT_IS_NOT_ALLOWED_BECAUSE_THAT_WOULD_BE_INEFFICIENT
@@ -79,7 +81,7 @@
 
 template<typename Derived>
 template<typename OtherDerived>
-Derived& DenseBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
+EIGEN_DEVICE_FUNC Derived& DenseBase<Derived>::operator=(const ReturnByValue<OtherDerived>& other)
 {
   other.evalTo(derived());
   return derived();
@@ -90,7 +92,7 @@
 // Expression is evaluated in a temporary; default implementation of Assignment is bypassed so that
 // when a ReturnByValue expression is assigned, the evaluator is not constructed.
 // TODO: Finalize port to new regime; ReturnByValue should not exist in the expression world
-  
+
 template<typename Derived>
 struct evaluator<ReturnByValue<Derived> >
   : public evaluator<typename internal::traits<Derived>::ReturnType>
@@ -98,7 +100,7 @@
   typedef ReturnByValue<Derived> XprType;
   typedef typename internal::traits<Derived>::ReturnType PlainObject;
   typedef evaluator<PlainObject> Base;
-  
+
   EIGEN_DEVICE_FUNC explicit evaluator(const XprType& xpr)
     : m_result(xpr.rows(), xpr.cols())
   {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
index 0640cda..28cdd76 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Reverse.h
@@ -12,7 +12,7 @@
 #ifndef EIGEN_REVERSE_H
 #define EIGEN_REVERSE_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 
@@ -44,7 +44,7 @@
   static inline PacketType run(const PacketType& x) { return x; }
 };
 
-} // end namespace internal 
+} // end namespace internal
 
 /** \class Reverse
   * \ingroup Core_Module
@@ -89,8 +89,10 @@
 
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Reverse)
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.rows(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     EIGEN_DEVICE_FUNC inline Index innerStride() const
     {
@@ -98,7 +100,7 @@
     }
 
     EIGEN_DEVICE_FUNC const typename internal::remove_all<typename MatrixType::Nested>::type&
-    nestedExpression() const 
+    nestedExpression() const
     {
       return m_matrix;
     }
@@ -114,7 +116,7 @@
   *
   */
 template<typename Derived>
-inline typename DenseBase<Derived>::ReverseReturnType
+EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::ReverseReturnType
 DenseBase<Derived>::reverse()
 {
   return ReverseReturnType(derived());
@@ -136,7 +138,7 @@
   *
   * \sa VectorwiseOp::reverseInPlace(), reverse() */
 template<typename Derived>
-inline void DenseBase<Derived>::reverseInPlace()
+EIGEN_DEVICE_FUNC inline void DenseBase<Derived>::reverseInPlace()
 {
   if(cols()>rows())
   {
@@ -161,7 +163,7 @@
 }
 
 namespace internal {
-  
+
 template<int Direction>
 struct vectorwise_reverse_inplace_impl;
 
@@ -171,8 +173,10 @@
   template<typename ExpressionType>
   static void run(ExpressionType &xpr)
   {
+    const int HalfAtCompileTime = ExpressionType::RowsAtCompileTime==Dynamic?Dynamic:ExpressionType::RowsAtCompileTime/2;
     Index half = xpr.rows()/2;
-    xpr.topRows(half).swap(xpr.bottomRows(half).colwise().reverse());
+    xpr.topRows(fix<HalfAtCompileTime>(half))
+       .swap(xpr.bottomRows(fix<HalfAtCompileTime>(half)).colwise().reverse());
   }
 };
 
@@ -182,8 +186,10 @@
   template<typename ExpressionType>
   static void run(ExpressionType &xpr)
   {
+    const int HalfAtCompileTime = ExpressionType::ColsAtCompileTime==Dynamic?Dynamic:ExpressionType::ColsAtCompileTime/2;
     Index half = xpr.cols()/2;
-    xpr.leftCols(half).swap(xpr.rightCols(half).rowwise().reverse());
+    xpr.leftCols(fix<HalfAtCompileTime>(half))
+       .swap(xpr.rightCols(fix<HalfAtCompileTime>(half)).rowwise().reverse());
   }
 };
 
@@ -201,9 +207,9 @@
   *
   * \sa DenseBase::reverseInPlace(), reverse() */
 template<typename ExpressionType, int Direction>
-void VectorwiseOp<ExpressionType,Direction>::reverseInPlace()
+EIGEN_DEVICE_FUNC void VectorwiseOp<ExpressionType,Direction>::reverseInPlace()
 {
-  internal::vectorwise_reverse_inplace_impl<Direction>::run(_expression().const_cast_derived());
+  internal::vectorwise_reverse_inplace_impl<Direction>::run(m_matrix);
 }
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h
index 79eec1b..7c86bf8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Select.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_SELECT_H
 #define EIGEN_SELECT_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \class Select
   * \ingroup Core_Module
@@ -67,8 +67,10 @@
       eigen_assert(m_condition.cols() == m_then.cols() && m_condition.cols() == m_else.cols());
     }
 
-    inline EIGEN_DEVICE_FUNC Index rows() const { return m_condition.rows(); }
-    inline EIGEN_DEVICE_FUNC Index cols() const { return m_condition.cols(); }
+    inline EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_condition.rows(); }
+    inline EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_condition.cols(); }
 
     inline EIGEN_DEVICE_FUNC
     const Scalar coeff(Index i, Index j) const
@@ -120,7 +122,7 @@
   */
 template<typename Derived>
 template<typename ThenDerived,typename ElseDerived>
-inline const Select<Derived,ThenDerived,ElseDerived>
+inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived,ElseDerived>
 DenseBase<Derived>::select(const DenseBase<ThenDerived>& thenMatrix,
                             const DenseBase<ElseDerived>& elseMatrix) const
 {
@@ -134,7 +136,7 @@
   */
 template<typename Derived>
 template<typename ThenDerived>
-inline const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
+inline EIGEN_DEVICE_FUNC const Select<Derived,ThenDerived, typename ThenDerived::ConstantReturnType>
 DenseBase<Derived>::select(const DenseBase<ThenDerived>& thenMatrix,
                            const typename ThenDerived::Scalar& elseScalar) const
 {
@@ -149,7 +151,7 @@
   */
 template<typename Derived>
 template<typename ElseDerived>
-inline const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
+inline EIGEN_DEVICE_FUNC const Select<Derived, typename ElseDerived::ConstantReturnType, ElseDerived >
 DenseBase<Derived>::select(const typename ElseDerived::Scalar& thenScalar,
                            const DenseBase<ElseDerived>& elseMatrix) const
 {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
index b2e51f3..8ce3b37 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SelfAdjointView.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_SELFADJOINTMATRIX_H
 #define EIGEN_SELFADJOINTMATRIX_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \class SelfAdjointView
   * \ingroup Core_Module
@@ -58,14 +58,15 @@
     typedef MatrixTypeNestedCleaned NestedExpression;
 
     /** \brief The type of coefficients in this matrix */
-    typedef typename internal::traits<SelfAdjointView>::Scalar Scalar; 
+    typedef typename internal::traits<SelfAdjointView>::Scalar Scalar;
     typedef typename MatrixType::StorageIndex StorageIndex;
     typedef typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type MatrixConjugateReturnType;
+    typedef SelfAdjointView<typename internal::add_const<MatrixType>::type, UpLo> ConstSelfAdjointView;
 
     enum {
       Mode = internal::traits<SelfAdjointView>::Mode,
       Flags = internal::traits<SelfAdjointView>::Flags,
-      TransposeMode = ((Mode & Upper) ? Lower : 0) | ((Mode & Lower) ? Upper : 0)
+      TransposeMode = ((int(Mode) & int(Upper)) ? Lower : 0) | ((int(Mode) & int(Lower)) ? Upper : 0)
     };
     typedef typename MatrixType::PlainObject PlainObject;
 
@@ -75,14 +76,14 @@
       EIGEN_STATIC_ASSERT(UpLo==Lower || UpLo==Upper,SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY);
     }
 
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return m_matrix.rows(); }
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return m_matrix.cols(); }
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return m_matrix.outerStride(); }
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return m_matrix.innerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return m_matrix.outerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT { return m_matrix.innerStride(); }
 
     /** \sa MatrixBase::coeff()
       * \warning the coordinates must fit into the referenced triangular part
@@ -131,7 +132,7 @@
     {
       return Product<OtherDerived,SelfAdjointView>(lhs.derived(),rhs);
     }
-    
+
     friend EIGEN_DEVICE_FUNC
     const SelfAdjointView<const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(Scalar,MatrixType,product),UpLo>
     operator*(const Scalar& s, const SelfAdjointView& mat)
@@ -197,6 +198,18 @@
     inline const ConjugateReturnType conjugate() const
     { return ConjugateReturnType(m_matrix.conjugate()); }
 
+    /** \returns an expression of the complex conjugate of \c *this if Cond==true,
+     *           returns \c *this otherwise.
+     */
+    template<bool Cond>
+    EIGEN_DEVICE_FUNC
+    inline typename internal::conditional<Cond,ConjugateReturnType,ConstSelfAdjointView>::type
+    conjugateIf() const
+    {
+      typedef typename internal::conditional<Cond,ConjugateReturnType,ConstSelfAdjointView>::type ReturnType;
+      return ReturnType(m_matrix.template conjugateIf<Cond>());
+    }
+
     typedef SelfAdjointView<const typename MatrixType::AdjointReturnType,TransposeMode> AdjointReturnType;
     /** \sa MatrixBase::adjoint() const */
     EIGEN_DEVICE_FUNC
@@ -287,17 +300,17 @@
   using Base::m_src;
   using Base::m_functor;
 public:
-  
+
   typedef typename Base::DstEvaluatorType DstEvaluatorType;
   typedef typename Base::SrcEvaluatorType SrcEvaluatorType;
   typedef typename Base::Scalar Scalar;
   typedef typename Base::AssignmentTraits AssignmentTraits;
-  
-  
+
+
   EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
     : Base(dst, src, func, dstExpr)
   {}
-  
+
   EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col)
   {
     eigen_internal_assert(row!=col);
@@ -305,12 +318,12 @@
     m_functor.assignCoeff(m_dst.coeffRef(row,col), tmp);
     m_functor.assignCoeff(m_dst.coeffRef(col,row), numext::conj(tmp));
   }
-  
+
   EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id)
   {
     Base::assignCoeff(id,id);
   }
-  
+
   EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index, Index)
   { eigen_internal_assert(false && "should never be called"); }
 };
@@ -324,7 +337,7 @@
 /** This is the const version of MatrixBase::selfadjointView() */
 template<typename Derived>
 template<unsigned int UpLo>
-typename MatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type
+EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::template ConstSelfAdjointViewReturnType<UpLo>::Type
 MatrixBase<Derived>::selfadjointView() const
 {
   return typename ConstSelfAdjointViewReturnType<UpLo>::Type(derived());
@@ -341,7 +354,7 @@
   */
 template<typename Derived>
 template<unsigned int UpLo>
-typename MatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type
+EIGEN_DEVICE_FUNC typename MatrixBase<Derived>::template SelfAdjointViewReturnType<UpLo>::Type
 MatrixBase<Derived>::selfadjointView()
 {
   return typename SelfAdjointViewReturnType<UpLo>::Type(derived());
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
index a8daea5..23d5cb7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Solve.h
@@ -13,13 +13,13 @@
 namespace Eigen {
 
 template<typename Decomposition, typename RhsType, typename StorageKind> class SolveImpl;
-  
+
 /** \class Solve
   * \ingroup Core_Module
   *
   * \brief Pseudo expression representing a solving operation
   *
-  * \tparam Decomposition the type of the matrix or decomposion object
+  * \tparam Decomposition the type of the matrix or decomposition object
   * \tparam Rhstype the type of the right-hand side
   *
   * This class represents an expression of A.solve(B)
@@ -64,13 +64,13 @@
 public:
   typedef typename internal::traits<Solve>::PlainObject PlainObject;
   typedef typename internal::traits<Solve>::StorageIndex StorageIndex;
-  
+
   Solve(const Decomposition &dec, const RhsType &rhs)
     : m_dec(dec), m_rhs(rhs)
   {}
-  
-  EIGEN_DEVICE_FUNC Index rows() const { return m_dec.cols(); }
-  EIGEN_DEVICE_FUNC Index cols() const { return m_rhs.cols(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_dec.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
   EIGEN_DEVICE_FUNC const Decomposition& dec() const { return m_dec; }
   EIGEN_DEVICE_FUNC const RhsType&       rhs() const { return m_rhs; }
@@ -87,14 +87,14 @@
   : public MatrixBase<Solve<Decomposition,RhsType> >
 {
   typedef Solve<Decomposition,RhsType> Derived;
-  
+
 public:
-  
+
   typedef MatrixBase<Solve<Decomposition,RhsType> > Base;
   EIGEN_DENSE_PUBLIC_INTERFACE(Derived)
 
 private:
-  
+
   Scalar coeff(Index row, Index col) const;
   Scalar coeff(Index i) const;
 };
@@ -119,15 +119,15 @@
   typedef evaluator<PlainObject> Base;
 
   enum { Flags = Base::Flags | EvalBeforeNestingBit };
-  
+
   EIGEN_DEVICE_FUNC explicit evaluator(const SolveType& solve)
     : m_result(solve.rows(), solve.cols())
   {
     ::new (static_cast<Base*>(this)) Base(m_result);
     solve.dec()._solve_impl(solve.rhs(), m_result);
   }
-  
-protected:  
+
+protected:
   PlainObject m_result;
 };
 
@@ -176,12 +176,12 @@
     Index dstCols = src.cols();
     if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
       dst.resize(dstRows, dstCols);
-    
+
     src.dec().nestedExpression().nestedExpression().template _solve_impl_transposed<true>(src.rhs(), dst);
   }
 };
 
-} // end namepsace internal
+} // end namespace internal
 
 } // end namespace Eigen
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
index 4652e2e..dfbf995 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolveTriangular.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_SOLVETRIANGULAR_H
 #define EIGEN_SOLVETRIANGULAR_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 
@@ -19,7 +19,7 @@
 template<typename LhsScalar, typename RhsScalar, typename Index, int Side, int Mode, bool Conjugate, int StorageOrder>
 struct triangular_solve_vector;
 
-template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder, int OtherStorageOrder>
+template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder, int OtherStorageOrder, int OtherInnerStride>
 struct triangular_solve_matrix;
 
 // small helper struct extracting some traits on the underlying solver operation
@@ -54,7 +54,7 @@
   typedef blas_traits<Lhs> LhsProductTraits;
   typedef typename LhsProductTraits::ExtractType ActualLhsType;
   typedef Map<Matrix<RhsScalar,Dynamic,1>, Aligned> MappedRhs;
-  static void run(const Lhs& lhs, Rhs& rhs)
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
   {
     ActualLhsType actualLhs = LhsProductTraits::extract(lhs);
 
@@ -64,7 +64,7 @@
 
     ei_declare_aligned_stack_constructed_variable(RhsScalar,actualRhs,rhs.size(),
                                                   (useRhsDirectly ? rhs.data() : 0));
-                                                  
+
     if(!useRhsDirectly)
       MappedRhs(actualRhs,rhs.size()) = rhs;
 
@@ -85,7 +85,7 @@
   typedef blas_traits<Lhs> LhsProductTraits;
   typedef typename LhsProductTraits::DirectLinearAccessType ActualLhsType;
 
-  static void run(const Lhs& lhs, Rhs& rhs)
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
   {
     typename internal::add_const_on_value_type<ActualLhsType>::type actualLhs = LhsProductTraits::extract(lhs);
 
@@ -98,8 +98,8 @@
     BlockingType blocking(rhs.rows(), rhs.cols(), size, 1, false);
 
     triangular_solve_matrix<Scalar,Index,Side,Mode,LhsProductTraits::NeedToConjugate,(int(Lhs::Flags) & RowMajorBit) ? RowMajor : ColMajor,
-                               (Rhs::Flags&RowMajorBit) ? RowMajor : ColMajor>
-      ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.outerStride(), blocking);
+                               (Rhs::Flags&RowMajorBit) ? RowMajor : ColMajor, Rhs::InnerStrideAtCompileTime>
+      ::run(size, othersize, &actualLhs.coeffRef(0,0), actualLhs.outerStride(), &rhs.coeffRef(0,0), rhs.innerStride(), rhs.outerStride(), blocking);
   }
 };
 
@@ -118,7 +118,7 @@
     DiagIndex  = IsLower ? LoopIndex : Size - LoopIndex - 1,
     StartIndex = IsLower ? 0         : DiagIndex+1
   };
-  static void run(const Lhs& lhs, Rhs& rhs)
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
   {
     if (LoopIndex>0)
       rhs.coeffRef(DiagIndex) -= lhs.row(DiagIndex).template segment<LoopIndex>(StartIndex).transpose()
@@ -133,22 +133,22 @@
 
 template<typename Lhs, typename Rhs, int Mode, int LoopIndex, int Size>
 struct triangular_solver_unroller<Lhs,Rhs,Mode,LoopIndex,Size,true> {
-  static void run(const Lhs&, Rhs&) {}
+  static EIGEN_DEVICE_FUNC void run(const Lhs&, Rhs&) {}
 };
 
 template<typename Lhs, typename Rhs, int Mode>
 struct triangular_solver_selector<Lhs,Rhs,OnTheLeft,Mode,CompleteUnrolling,1> {
-  static void run(const Lhs& lhs, Rhs& rhs)
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
   { triangular_solver_unroller<Lhs,Rhs,Mode,0,Rhs::SizeAtCompileTime>::run(lhs,rhs); }
 };
 
 template<typename Lhs, typename Rhs, int Mode>
 struct triangular_solver_selector<Lhs,Rhs,OnTheRight,Mode,CompleteUnrolling,1> {
-  static void run(const Lhs& lhs, Rhs& rhs)
+  static EIGEN_DEVICE_FUNC void run(const Lhs& lhs, Rhs& rhs)
   {
     Transpose<const Lhs> trLhs(lhs);
     Transpose<Rhs> trRhs(rhs);
-    
+
     triangular_solver_unroller<Transpose<const Lhs>,Transpose<Rhs>,
                               ((Mode&Upper)==Upper ? Lower : Upper) | (Mode&UnitDiag),
                               0,Rhs::SizeAtCompileTime>::run(trLhs,trRhs);
@@ -164,11 +164,11 @@
 #ifndef EIGEN_PARSED_BY_DOXYGEN
 template<typename MatrixType, unsigned int Mode>
 template<int Side, typename OtherDerived>
-void TriangularViewImpl<MatrixType,Mode,Dense>::solveInPlace(const MatrixBase<OtherDerived>& _other) const
+EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType,Mode,Dense>::solveInPlace(const MatrixBase<OtherDerived>& _other) const
 {
   OtherDerived& other = _other.const_cast_derived();
   eigen_assert( derived().cols() == derived().rows() && ((Side==OnTheLeft && derived().cols() == other.rows()) || (Side==OnTheRight && derived().cols() == other.cols())) );
-  eigen_assert((!(Mode & ZeroDiag)) && bool(Mode & (Upper|Lower)));
+  eigen_assert((!(int(Mode) & int(ZeroDiag))) && bool(int(Mode) & (int(Upper) | int(Lower))));
   // If solving for a 0x0 matrix, nothing to do, simply return.
   if (derived().cols() == 0)
     return;
@@ -213,8 +213,8 @@
     : m_triangularMatrix(tri), m_rhs(rhs)
   {}
 
-  inline Index rows() const { return m_rhs.rows(); }
-  inline Index cols() const { return m_rhs.cols(); }
+  inline EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_rhs.rows(); }
+  inline EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_rhs.cols(); }
 
   template<typename Dest> inline void evalTo(Dest& dst) const
   {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
index 8a4adc2..5014610 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/SolverBase.h
@@ -14,8 +14,35 @@
 
 namespace internal {
 
+template<typename Derived>
+struct solve_assertion {
+    template<bool Transpose_, typename Rhs>
+    static void run(const Derived& solver, const Rhs& b) { solver.template _check_solve_assertion<Transpose_>(b); }
+};
 
+template<typename Derived>
+struct solve_assertion<Transpose<Derived> >
+{
+    typedef Transpose<Derived> type;
 
+    template<bool Transpose_, typename Rhs>
+    static void run(const type& transpose, const Rhs& b)
+    {
+        internal::solve_assertion<typename internal::remove_all<Derived>::type>::template run<true>(transpose.nestedExpression(), b);
+    }
+};
+
+template<typename Scalar, typename Derived>
+struct solve_assertion<CwiseUnaryOp<Eigen::internal::scalar_conjugate_op<Scalar>, const Transpose<Derived> > >
+{
+    typedef CwiseUnaryOp<Eigen::internal::scalar_conjugate_op<Scalar>, const Transpose<Derived> > type;
+
+    template<bool Transpose_, typename Rhs>
+    static void run(const type& adjoint, const Rhs& b)
+    {
+        internal::solve_assertion<typename internal::remove_all<Transpose<Derived> >::type>::template run<true>(adjoint.nestedExpression(), b);
+    }
+};
 } // end namespace internal
 
 /** \class SolverBase
@@ -35,7 +62,7 @@
   *
   * \warning Currently, any other usage of transpose() and adjoint() are not supported and will produce compilation errors.
   *
-  * \sa class PartialPivLU, class FullPivLU
+  * \sa class PartialPivLU, class FullPivLU, class HouseholderQR, class ColPivHouseholderQR, class FullPivHouseholderQR, class CompleteOrthogonalDecomposition, class LLT, class LDLT, class SVDBase
   */
 template<typename Derived>
 class SolverBase : public EigenBase<Derived>
@@ -46,6 +73,9 @@
     typedef typename internal::traits<Derived>::Scalar Scalar;
     typedef Scalar CoeffReturnType;
 
+    template<typename Derived_>
+    friend struct internal::solve_assertion;
+
     enum {
       RowsAtCompileTime = internal::traits<Derived>::RowsAtCompileTime,
       ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
@@ -56,7 +86,8 @@
       MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
                                                              internal::traits<Derived>::MaxColsAtCompileTime>::ret),
       IsVectorAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime == 1
-                           || internal::traits<Derived>::MaxColsAtCompileTime == 1
+                           || internal::traits<Derived>::MaxColsAtCompileTime == 1,
+      NumDimensions = int(MaxSizeAtCompileTime) == 1 ? 0 : bool(IsVectorAtCompileTime) ? 1 : 2
     };
 
     /** Default constructor */
@@ -74,7 +105,7 @@
     inline const Solve<Derived, Rhs>
     solve(const MatrixBase<Rhs>& b) const
     {
-      eigen_assert(derived().rows()==b.rows() && "solve(): invalid number of rows of the right hand side matrix b");
+      internal::solve_assertion<typename internal::remove_all<Derived>::type>::template run<false>(derived(), b);
       return Solve<Derived, Rhs>(derived(), b.derived());
     }
 
@@ -112,6 +143,13 @@
     }
 
   protected:
+
+    template<bool Transpose_, typename Rhs>
+    void _check_solve_assertion(const Rhs& b) const {
+        EIGEN_ONLY_USED_FOR_DEBUG(b);
+        eigen_assert(derived().m_isInitialized && "Solver is not initialized.");
+        eigen_assert((Transpose_?derived().cols():derived().rows())==b.rows() && "SolverBase::solve(): invalid number of rows of the right hand side matrix b");
+    }
 };
 
 namespace internal {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
index 88c8d98..4a3f0cc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StableNorm.h
@@ -50,6 +50,71 @@
     ssq += (bl*invScale).squaredNorm();
 }
 
+template<typename VectorType, typename RealScalar>
+void stable_norm_impl_inner_step(const VectorType &vec, RealScalar& ssq, RealScalar& scale, RealScalar& invScale)
+{
+  typedef typename VectorType::Scalar Scalar;
+  const Index blockSize = 4096;
+  
+  typedef typename internal::nested_eval<VectorType,2>::type VectorTypeCopy;
+  typedef typename internal::remove_all<VectorTypeCopy>::type VectorTypeCopyClean;
+  const VectorTypeCopy copy(vec);
+  
+  enum {
+    CanAlign = (   (int(VectorTypeCopyClean::Flags)&DirectAccessBit)
+                || (int(internal::evaluator<VectorTypeCopyClean>::Alignment)>0) // FIXME Alignment)>0 might not be enough
+               ) && (blockSize*sizeof(Scalar)*2<EIGEN_STACK_ALLOCATION_LIMIT)
+                 && (EIGEN_MAX_STATIC_ALIGN_BYTES>0) // if we cannot allocate on the stack, then let's not bother about this optimization
+  };
+  typedef typename internal::conditional<CanAlign, Ref<const Matrix<Scalar,Dynamic,1,0,blockSize,1>, internal::evaluator<VectorTypeCopyClean>::Alignment>,
+                                                   typename VectorTypeCopyClean::ConstSegmentReturnType>::type SegmentWrapper;
+  Index n = vec.size();
+  
+  Index bi = internal::first_default_aligned(copy);
+  if (bi>0)
+    internal::stable_norm_kernel(copy.head(bi), ssq, scale, invScale);
+  for (; bi<n; bi+=blockSize)
+    internal::stable_norm_kernel(SegmentWrapper(copy.segment(bi,numext::mini(blockSize, n - bi))), ssq, scale, invScale);
+}
+
+template<typename VectorType>
+typename VectorType::RealScalar
+stable_norm_impl(const VectorType &vec, typename enable_if<VectorType::IsVectorAtCompileTime>::type* = 0 )
+{
+  using std::sqrt;
+  using std::abs;
+
+  Index n = vec.size();
+
+  if(n==1)
+    return abs(vec.coeff(0));
+
+  typedef typename VectorType::RealScalar RealScalar;
+  RealScalar scale(0);
+  RealScalar invScale(1);
+  RealScalar ssq(0); // sum of squares
+
+  stable_norm_impl_inner_step(vec, ssq, scale, invScale);
+  
+  return scale * sqrt(ssq);
+}
+
+template<typename MatrixType>
+typename MatrixType::RealScalar
+stable_norm_impl(const MatrixType &mat, typename enable_if<!MatrixType::IsVectorAtCompileTime>::type* = 0 )
+{
+  using std::sqrt;
+
+  typedef typename MatrixType::RealScalar RealScalar;
+  RealScalar scale(0);
+  RealScalar invScale(1);
+  RealScalar ssq(0); // sum of squares
+
+  for(Index j=0; j<mat.outerSize(); ++j)
+    stable_norm_impl_inner_step(mat.innerVector(j), ssq, scale, invScale);
+  return scale * sqrt(ssq);
+}
+
 template<typename Derived>
 inline typename NumTraits<typename traits<Derived>::Scalar>::Real
 blueNorm_impl(const EigenBase<Derived>& _vec)
@@ -58,52 +123,43 @@
   using std::pow;
   using std::sqrt;
   using std::abs;
+
+  // This program calculates the machine-dependent constants
+  // bl, b2, slm, s2m, relerr overfl
+  // from the "basic" machine-dependent numbers
+  // nbig, ibeta, it, iemin, iemax, rbig.
+  // The following define the basic machine-dependent constants.
+  // For portability, the PORT subprograms "ilmaeh" and "rlmach"
+  // are used. For any specific computer, each of the assignment
+  // statements can be replaced
+  static const int ibeta = std::numeric_limits<RealScalar>::radix;  // base for floating-point numbers
+  static const int it    = NumTraits<RealScalar>::digits();  // number of base-beta digits in mantissa
+  static const int iemin = NumTraits<RealScalar>::min_exponent();  // minimum exponent
+  static const int iemax = NumTraits<RealScalar>::max_exponent();  // maximum exponent
+  static const RealScalar rbig   = NumTraits<RealScalar>::highest();  // largest floating-point number
+  static const RealScalar b1     = RealScalar(pow(RealScalar(ibeta),RealScalar(-((1-iemin)/2))));  // lower boundary of midrange
+  static const RealScalar b2     = RealScalar(pow(RealScalar(ibeta),RealScalar((iemax + 1 - it)/2)));  // upper boundary of midrange
+  static const RealScalar s1m    = RealScalar(pow(RealScalar(ibeta),RealScalar((2-iemin)/2)));  // scaling factor for lower range
+  static const RealScalar s2m    = RealScalar(pow(RealScalar(ibeta),RealScalar(- ((iemax+it)/2))));  // scaling factor for upper range
+  static const RealScalar eps    = RealScalar(pow(double(ibeta), 1-it));
+  static const RealScalar relerr = sqrt(eps);  // tolerance for neglecting asml
+
   const Derived& vec(_vec.derived());
-  static bool initialized = false;
-  static RealScalar b1, b2, s1m, s2m, rbig, relerr;
-  if(!initialized)
-  {
-    int ibeta, it, iemin, iemax, iexp;
-    RealScalar eps;
-    // This program calculates the machine-dependent constants
-    // bl, b2, slm, s2m, relerr overfl
-    // from the "basic" machine-dependent numbers
-    // nbig, ibeta, it, iemin, iemax, rbig.
-    // The following define the basic machine-dependent constants.
-    // For portability, the PORT subprograms "ilmaeh" and "rlmach"
-    // are used. For any specific computer, each of the assignment
-    // statements can be replaced
-    ibeta = std::numeric_limits<RealScalar>::radix;                 // base for floating-point numbers
-    it    = std::numeric_limits<RealScalar>::digits;                // number of base-beta digits in mantissa
-    iemin = std::numeric_limits<RealScalar>::min_exponent;          // minimum exponent
-    iemax = std::numeric_limits<RealScalar>::max_exponent;          // maximum exponent
-    rbig  = (std::numeric_limits<RealScalar>::max)();               // largest floating-point number
-
-    iexp  = -((1-iemin)/2);
-    b1    = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp)));    // lower boundary of midrange
-    iexp  = (iemax + 1 - it)/2;
-    b2    = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp)));    // upper boundary of midrange
-
-    iexp  = (2-iemin)/2;
-    s1m   = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp)));    // scaling factor for lower range
-    iexp  = - ((iemax+it)/2);
-    s2m   = RealScalar(pow(RealScalar(ibeta),RealScalar(iexp)));    // scaling factor for upper range
-
-    eps     = RealScalar(pow(double(ibeta), 1-it));
-    relerr  = sqrt(eps);                                            // tolerance for neglecting asml
-    initialized = true;
-  }
   Index n = vec.size();
   RealScalar ab2 = b2 / RealScalar(n);
   RealScalar asml = RealScalar(0);
   RealScalar amed = RealScalar(0);
   RealScalar abig = RealScalar(0);
-  for(typename Derived::InnerIterator it(vec, 0); it; ++it)
+
+  for(Index j=0; j<vec.outerSize(); ++j)
   {
-    RealScalar ax = abs(it.value());
-    if(ax > ab2)     abig += numext::abs2(ax*s2m);
-    else if(ax < b1) asml += numext::abs2(ax*s1m);
-    else             amed += numext::abs2(ax);
+    for(typename Derived::InnerIterator iter(vec, j); iter; ++iter)
+    {
+      RealScalar ax = abs(iter.value());
+      if(ax > ab2)     abig += numext::abs2(ax*s2m);
+      else if(ax < b1) asml += numext::abs2(ax*s1m);
+      else             amed += numext::abs2(ax);
+    }
   }
   if(amed!=amed)
     return amed;  // we got a NaN
@@ -156,36 +212,7 @@
 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
 MatrixBase<Derived>::stableNorm() const
 {
-  using std::sqrt;
-  using std::abs;
-  const Index blockSize = 4096;
-  RealScalar scale(0);
-  RealScalar invScale(1);
-  RealScalar ssq(0); // sum of square
-  
-  typedef typename internal::nested_eval<Derived,2>::type DerivedCopy;
-  typedef typename internal::remove_all<DerivedCopy>::type DerivedCopyClean;
-  const DerivedCopy copy(derived());
-  
-  enum {
-    CanAlign = (   (int(DerivedCopyClean::Flags)&DirectAccessBit)
-                || (int(internal::evaluator<DerivedCopyClean>::Alignment)>0) // FIXME Alignment)>0 might not be enough
-               ) && (blockSize*sizeof(Scalar)*2<EIGEN_STACK_ALLOCATION_LIMIT)
-                 && (EIGEN_MAX_STATIC_ALIGN_BYTES>0) // if we cannot allocate on the stack, then let's not bother about this optimization
-  };
-  typedef typename internal::conditional<CanAlign, Ref<const Matrix<Scalar,Dynamic,1,0,blockSize,1>, internal::evaluator<DerivedCopyClean>::Alignment>,
-                                                   typename DerivedCopyClean::ConstSegmentReturnType>::type SegmentWrapper;
-  Index n = size();
-  
-  if(n==1)
-    return abs(this->coeff(0));
-  
-  Index bi = internal::first_default_aligned(copy);
-  if (bi>0)
-    internal::stable_norm_kernel(copy.head(bi), ssq, scale, invScale);
-  for (; bi<n; bi+=blockSize)
-    internal::stable_norm_kernel(SegmentWrapper(copy.segment(bi,numext::mini(blockSize, n - bi))), ssq, scale, invScale);
-  return scale * sqrt(ssq);
+  return internal::stable_norm_impl(derived());
 }
 
 /** \returns the \em l2 norm of \c *this using the Blue's algorithm.
@@ -213,7 +240,10 @@
 inline typename NumTraits<typename internal::traits<Derived>::Scalar>::Real
 MatrixBase<Derived>::hypotNorm() const
 {
-  return this->cwiseAbs().redux(internal::scalar_hypot_op<RealScalar>());
+  if(size()==1)
+    return numext::abs(coeff(0,0));
+  else
+    return this->cwiseAbs().redux(internal::scalar_hypot_op<RealScalar>());
 }
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h
new file mode 100644
index 0000000..09041db
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/StlIterators.h
@@ -0,0 +1,463 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2018 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_STLITERATORS_H
+#define EIGEN_STLITERATORS_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename IteratorType>
+struct indexed_based_stl_iterator_traits;
+
+template<typename  Derived>
+class indexed_based_stl_iterator_base
+{
+protected:
+  typedef indexed_based_stl_iterator_traits<Derived> traits;
+  typedef typename traits::XprType XprType;
+  typedef indexed_based_stl_iterator_base<typename traits::non_const_iterator> non_const_iterator;
+  typedef indexed_based_stl_iterator_base<typename traits::const_iterator> const_iterator;
+  typedef typename internal::conditional<internal::is_const<XprType>::value,non_const_iterator,const_iterator>::type other_iterator;
+  // NOTE: in C++03 we cannot declare friend classes through typedefs because we need to write friend class:
+  friend class indexed_based_stl_iterator_base<typename traits::const_iterator>;
+  friend class indexed_based_stl_iterator_base<typename traits::non_const_iterator>;
+public:
+  typedef Index difference_type;
+  typedef std::random_access_iterator_tag iterator_category;
+
+  indexed_based_stl_iterator_base() EIGEN_NO_THROW : mp_xpr(0), m_index(0) {}
+  indexed_based_stl_iterator_base(XprType& xpr, Index index) EIGEN_NO_THROW : mp_xpr(&xpr), m_index(index) {}
+
+  indexed_based_stl_iterator_base(const non_const_iterator& other) EIGEN_NO_THROW
+    : mp_xpr(other.mp_xpr), m_index(other.m_index)
+  {}
+
+  indexed_based_stl_iterator_base& operator=(const non_const_iterator& other)
+  {
+    mp_xpr = other.mp_xpr;
+    m_index = other.m_index;
+    return *this;
+  }
+
+  Derived& operator++() { ++m_index; return derived(); }
+  Derived& operator--() { --m_index; return derived(); }
+
+  Derived operator++(int) { Derived prev(derived()); operator++(); return prev;}
+  Derived operator--(int) { Derived prev(derived()); operator--(); return prev;}
+
+  friend Derived operator+(const indexed_based_stl_iterator_base& a, Index b) { Derived ret(a.derived()); ret += b; return ret; }
+  friend Derived operator-(const indexed_based_stl_iterator_base& a, Index b) { Derived ret(a.derived()); ret -= b; return ret; }
+  friend Derived operator+(Index a, const indexed_based_stl_iterator_base& b) { Derived ret(b.derived()); ret += a; return ret; }
+  friend Derived operator-(Index a, const indexed_based_stl_iterator_base& b) { Derived ret(b.derived()); ret -= a; return ret; }
+  
+  Derived& operator+=(Index b) { m_index += b; return derived(); }
+  Derived& operator-=(Index b) { m_index -= b; return derived(); }
+
+  difference_type operator-(const indexed_based_stl_iterator_base& other) const
+  {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index - other.m_index;
+  }
+
+  difference_type operator-(const other_iterator& other) const
+  {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return m_index - other.m_index;
+  }
+
+  bool operator==(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
+  bool operator!=(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
+  bool operator< (const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
+  bool operator<=(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
+  bool operator> (const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
+  bool operator>=(const indexed_based_stl_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
+
+  bool operator==(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
+  bool operator!=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
+  bool operator< (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
+  bool operator<=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
+  bool operator> (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
+  bool operator>=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
+
+protected:
+
+  Derived& derived() { return static_cast<Derived&>(*this); }
+  const Derived& derived() const { return static_cast<const Derived&>(*this); }
+
+  XprType *mp_xpr;
+  Index m_index;
+};
+
+template<typename  Derived>
+class indexed_based_stl_reverse_iterator_base
+{
+protected:
+  typedef indexed_based_stl_iterator_traits<Derived> traits;
+  typedef typename traits::XprType XprType;
+  typedef indexed_based_stl_reverse_iterator_base<typename traits::non_const_iterator> non_const_iterator;
+  typedef indexed_based_stl_reverse_iterator_base<typename traits::const_iterator> const_iterator;
+  typedef typename internal::conditional<internal::is_const<XprType>::value,non_const_iterator,const_iterator>::type other_iterator;
+  // NOTE: in C++03 we cannot declare friend classes through typedefs because we need to write friend class:
+  friend class indexed_based_stl_reverse_iterator_base<typename traits::const_iterator>;
+  friend class indexed_based_stl_reverse_iterator_base<typename traits::non_const_iterator>;
+public:
+  typedef Index difference_type;
+  typedef std::random_access_iterator_tag iterator_category;
+
+  indexed_based_stl_reverse_iterator_base() : mp_xpr(0), m_index(0) {}
+  indexed_based_stl_reverse_iterator_base(XprType& xpr, Index index) : mp_xpr(&xpr), m_index(index) {}
+
+  indexed_based_stl_reverse_iterator_base(const non_const_iterator& other)
+    : mp_xpr(other.mp_xpr), m_index(other.m_index)
+  {}
+
+  indexed_based_stl_reverse_iterator_base& operator=(const non_const_iterator& other)
+  {
+    mp_xpr = other.mp_xpr;
+    m_index = other.m_index;
+    return *this;
+  }
+
+  Derived& operator++() { --m_index; return derived(); }
+  Derived& operator--() { ++m_index; return derived(); }
+
+  Derived operator++(int) { Derived prev(derived()); operator++(); return prev;}
+  Derived operator--(int) { Derived prev(derived()); operator--(); return prev;}
+
+  friend Derived operator+(const indexed_based_stl_reverse_iterator_base& a, Index b) { Derived ret(a.derived()); ret += b; return ret; }
+  friend Derived operator-(const indexed_based_stl_reverse_iterator_base& a, Index b) { Derived ret(a.derived()); ret -= b; return ret; }
+  friend Derived operator+(Index a, const indexed_based_stl_reverse_iterator_base& b) { Derived ret(b.derived()); ret += a; return ret; }
+  friend Derived operator-(Index a, const indexed_based_stl_reverse_iterator_base& b) { Derived ret(b.derived()); ret -= a; return ret; }
+  
+  Derived& operator+=(Index b) { m_index -= b; return derived(); }
+  Derived& operator-=(Index b) { m_index += b; return derived(); }
+
+  difference_type operator-(const indexed_based_stl_reverse_iterator_base& other) const
+  {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return other.m_index - m_index;
+  }
+
+  difference_type operator-(const other_iterator& other) const
+  {
+    eigen_assert(mp_xpr == other.mp_xpr);
+    return other.m_index - m_index;
+  }
+
+  bool operator==(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
+  bool operator!=(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
+  bool operator< (const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
+  bool operator<=(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
+  bool operator> (const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
+  bool operator>=(const indexed_based_stl_reverse_iterator_base& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
+
+  bool operator==(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index == other.m_index; }
+  bool operator!=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index != other.m_index; }
+  bool operator< (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >  other.m_index; }
+  bool operator<=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index >= other.m_index; }
+  bool operator> (const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <  other.m_index; }
+  bool operator>=(const other_iterator& other) const { eigen_assert(mp_xpr == other.mp_xpr); return m_index <= other.m_index; }
+
+protected:
+
+  Derived& derived() { return static_cast<Derived&>(*this); }
+  const Derived& derived() const { return static_cast<const Derived&>(*this); }
+
+  XprType *mp_xpr;
+  Index m_index;
+};
+
+template<typename XprType>
+class pointer_based_stl_iterator
+{
+  enum { is_lvalue  = internal::is_lvalue<XprType>::value };
+  typedef pointer_based_stl_iterator<typename internal::remove_const<XprType>::type> non_const_iterator;
+  typedef pointer_based_stl_iterator<typename internal::add_const<XprType>::type> const_iterator;
+  typedef typename internal::conditional<internal::is_const<XprType>::value,non_const_iterator,const_iterator>::type other_iterator;
+  // NOTE: in C++03 we cannot declare friend classes through typedefs because we need to write friend class:
+  friend class pointer_based_stl_iterator<typename internal::add_const<XprType>::type>;
+  friend class pointer_based_stl_iterator<typename internal::remove_const<XprType>::type>;
+public:
+  typedef Index difference_type;
+  typedef typename XprType::Scalar value_type;
+  typedef std::random_access_iterator_tag iterator_category;
+  typedef typename internal::conditional<bool(is_lvalue), value_type*, const value_type*>::type pointer;
+  typedef typename internal::conditional<bool(is_lvalue), value_type&, const value_type&>::type reference;
+
+
+  pointer_based_stl_iterator() EIGEN_NO_THROW : m_ptr(0) {}
+  pointer_based_stl_iterator(XprType& xpr, Index index) EIGEN_NO_THROW : m_incr(xpr.innerStride())
+  {
+    m_ptr = xpr.data() + index * m_incr.value();
+  }
+
+  pointer_based_stl_iterator(const non_const_iterator& other) EIGEN_NO_THROW
+    : m_ptr(other.m_ptr), m_incr(other.m_incr)
+  {}
+
+  pointer_based_stl_iterator& operator=(const non_const_iterator& other) EIGEN_NO_THROW
+  {
+    m_ptr = other.m_ptr;
+    m_incr.setValue(other.m_incr);
+    return *this;
+  }
+
+  reference operator*()         const { return *m_ptr;   }
+  reference operator[](Index i) const { return *(m_ptr+i*m_incr.value()); }
+  pointer   operator->()        const { return m_ptr;    }
+
+  pointer_based_stl_iterator& operator++() { m_ptr += m_incr.value(); return *this; }
+  pointer_based_stl_iterator& operator--() { m_ptr -= m_incr.value(); return *this; }
+
+  pointer_based_stl_iterator operator++(int) { pointer_based_stl_iterator prev(*this); operator++(); return prev;}
+  pointer_based_stl_iterator operator--(int) { pointer_based_stl_iterator prev(*this); operator--(); return prev;}
+
+  friend pointer_based_stl_iterator operator+(const pointer_based_stl_iterator& a, Index b) { pointer_based_stl_iterator ret(a); ret += b; return ret; }
+  friend pointer_based_stl_iterator operator-(const pointer_based_stl_iterator& a, Index b) { pointer_based_stl_iterator ret(a); ret -= b; return ret; }
+  friend pointer_based_stl_iterator operator+(Index a, const pointer_based_stl_iterator& b) { pointer_based_stl_iterator ret(b); ret += a; return ret; }
+  friend pointer_based_stl_iterator operator-(Index a, const pointer_based_stl_iterator& b) { pointer_based_stl_iterator ret(b); ret -= a; return ret; }
+  
+  pointer_based_stl_iterator& operator+=(Index b) { m_ptr += b*m_incr.value(); return *this; }
+  pointer_based_stl_iterator& operator-=(Index b) { m_ptr -= b*m_incr.value(); return *this; }
+
+  difference_type operator-(const pointer_based_stl_iterator& other) const {
+    return (m_ptr - other.m_ptr)/m_incr.value();
+  }
+
+  difference_type operator-(const other_iterator& other) const {
+    return (m_ptr - other.m_ptr)/m_incr.value();
+  }
+
+  bool operator==(const pointer_based_stl_iterator& other) const { return m_ptr == other.m_ptr; }
+  bool operator!=(const pointer_based_stl_iterator& other) const { return m_ptr != other.m_ptr; }
+  bool operator< (const pointer_based_stl_iterator& other) const { return m_ptr <  other.m_ptr; }
+  bool operator<=(const pointer_based_stl_iterator& other) const { return m_ptr <= other.m_ptr; }
+  bool operator> (const pointer_based_stl_iterator& other) const { return m_ptr >  other.m_ptr; }
+  bool operator>=(const pointer_based_stl_iterator& other) const { return m_ptr >= other.m_ptr; }
+
+  bool operator==(const other_iterator& other) const { return m_ptr == other.m_ptr; }
+  bool operator!=(const other_iterator& other) const { return m_ptr != other.m_ptr; }
+  bool operator< (const other_iterator& other) const { return m_ptr <  other.m_ptr; }
+  bool operator<=(const other_iterator& other) const { return m_ptr <= other.m_ptr; }
+  bool operator> (const other_iterator& other) const { return m_ptr >  other.m_ptr; }
+  bool operator>=(const other_iterator& other) const { return m_ptr >= other.m_ptr; }
+
+protected:
+
+  pointer m_ptr;
+  internal::variable_if_dynamic<Index, XprType::InnerStrideAtCompileTime> m_incr;
+};
+
+template<typename _XprType>
+struct indexed_based_stl_iterator_traits<generic_randaccess_stl_iterator<_XprType> >
+{
+  typedef _XprType XprType;
+  typedef generic_randaccess_stl_iterator<typename internal::remove_const<XprType>::type> non_const_iterator;
+  typedef generic_randaccess_stl_iterator<typename internal::add_const<XprType>::type> const_iterator;
+};
+
+template<typename XprType>
+class generic_randaccess_stl_iterator : public indexed_based_stl_iterator_base<generic_randaccess_stl_iterator<XprType> >
+{
+public:
+  typedef typename XprType::Scalar value_type;
+
+protected:
+
+  enum {
+    has_direct_access = (internal::traits<XprType>::Flags & DirectAccessBit) ? 1 : 0,
+    is_lvalue  = internal::is_lvalue<XprType>::value
+  };
+
+  typedef indexed_based_stl_iterator_base<generic_randaccess_stl_iterator> Base;
+  using Base::m_index;
+  using Base::mp_xpr;
+
+  // TODO currently const Transpose/Reshape expressions never returns const references,
+  // so lets return by value too.
+  //typedef typename internal::conditional<bool(has_direct_access), const value_type&, const value_type>::type read_only_ref_t;
+  typedef const value_type read_only_ref_t;
+
+public:
+  
+  typedef typename internal::conditional<bool(is_lvalue), value_type *, const value_type *>::type pointer;
+  typedef typename internal::conditional<bool(is_lvalue), value_type&, read_only_ref_t>::type reference;
+  
+  generic_randaccess_stl_iterator() : Base() {}
+  generic_randaccess_stl_iterator(XprType& xpr, Index index) : Base(xpr,index) {}
+  generic_randaccess_stl_iterator(const typename Base::non_const_iterator& other) : Base(other) {}
+  using Base::operator=;
+
+  reference operator*()         const { return   (*mp_xpr)(m_index);   }
+  reference operator[](Index i) const { return   (*mp_xpr)(m_index+i); }
+  pointer   operator->()        const { return &((*mp_xpr)(m_index)); }
+};
+
+template<typename _XprType, DirectionType Direction>
+struct indexed_based_stl_iterator_traits<subvector_stl_iterator<_XprType,Direction> >
+{
+  typedef _XprType XprType;
+  typedef subvector_stl_iterator<typename internal::remove_const<XprType>::type, Direction> non_const_iterator;
+  typedef subvector_stl_iterator<typename internal::add_const<XprType>::type, Direction> const_iterator;
+};
+
+template<typename XprType, DirectionType Direction>
+class subvector_stl_iterator : public indexed_based_stl_iterator_base<subvector_stl_iterator<XprType,Direction> >
+{
+protected:
+
+  enum { is_lvalue  = internal::is_lvalue<XprType>::value };
+
+  typedef indexed_based_stl_iterator_base<subvector_stl_iterator> Base;
+  using Base::m_index;
+  using Base::mp_xpr;
+
+  typedef typename internal::conditional<Direction==Vertical,typename XprType::ColXpr,typename XprType::RowXpr>::type SubVectorType;
+  typedef typename internal::conditional<Direction==Vertical,typename XprType::ConstColXpr,typename XprType::ConstRowXpr>::type ConstSubVectorType;
+
+
+public:
+  typedef typename internal::conditional<bool(is_lvalue), SubVectorType, ConstSubVectorType>::type reference;
+  typedef typename reference::PlainObject value_type;
+
+private:
+  class subvector_stl_iterator_ptr
+  {
+  public:
+      subvector_stl_iterator_ptr(const reference &subvector) : m_subvector(subvector) {}
+      reference* operator->() { return &m_subvector; }
+  private:
+      reference m_subvector;
+  };
+public:
+
+  typedef subvector_stl_iterator_ptr pointer;
+  
+  subvector_stl_iterator() : Base() {}
+  subvector_stl_iterator(XprType& xpr, Index index) : Base(xpr,index) {}
+
+  reference operator*()         const { return (*mp_xpr).template subVector<Direction>(m_index); }
+  reference operator[](Index i) const { return (*mp_xpr).template subVector<Direction>(m_index+i); }
+  pointer   operator->()        const { return (*mp_xpr).template subVector<Direction>(m_index); }
+};
+
+template<typename _XprType, DirectionType Direction>
+struct indexed_based_stl_iterator_traits<subvector_stl_reverse_iterator<_XprType,Direction> >
+{
+  typedef _XprType XprType;
+  typedef subvector_stl_reverse_iterator<typename internal::remove_const<XprType>::type, Direction> non_const_iterator;
+  typedef subvector_stl_reverse_iterator<typename internal::add_const<XprType>::type, Direction> const_iterator;
+};
+
+template<typename XprType, DirectionType Direction>
+class subvector_stl_reverse_iterator : public indexed_based_stl_reverse_iterator_base<subvector_stl_reverse_iterator<XprType,Direction> >
+{
+protected:
+
+  enum { is_lvalue  = internal::is_lvalue<XprType>::value };
+
+  typedef indexed_based_stl_reverse_iterator_base<subvector_stl_reverse_iterator> Base;
+  using Base::m_index;
+  using Base::mp_xpr;
+
+  typedef typename internal::conditional<Direction==Vertical,typename XprType::ColXpr,typename XprType::RowXpr>::type SubVectorType;
+  typedef typename internal::conditional<Direction==Vertical,typename XprType::ConstColXpr,typename XprType::ConstRowXpr>::type ConstSubVectorType;
+
+
+public:
+  typedef typename internal::conditional<bool(is_lvalue), SubVectorType, ConstSubVectorType>::type reference;
+  typedef typename reference::PlainObject value_type;
+
+private:
+  class subvector_stl_reverse_iterator_ptr
+  {
+  public:
+      subvector_stl_reverse_iterator_ptr(const reference &subvector) : m_subvector(subvector) {}
+      reference* operator->() { return &m_subvector; }
+  private:
+      reference m_subvector;
+  };
+public:
+
+  typedef subvector_stl_reverse_iterator_ptr pointer;
+  
+  subvector_stl_reverse_iterator() : Base() {}
+  subvector_stl_reverse_iterator(XprType& xpr, Index index) : Base(xpr,index) {}
+
+  reference operator*()         const { return (*mp_xpr).template subVector<Direction>(m_index); }
+  reference operator[](Index i) const { return (*mp_xpr).template subVector<Direction>(m_index+i); }
+  pointer   operator->()        const { return (*mp_xpr).template subVector<Direction>(m_index); }
+};
+
+} // namespace internal
+
+
+/** returns an iterator to the first element of the 1D vector or array
+  * \only_for_vectors
+  * \sa end(), cbegin()
+  */
+template<typename Derived>
+inline typename DenseBase<Derived>::iterator DenseBase<Derived>::begin()
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  return iterator(derived(), 0);
+}
+
+/** const version of begin() */
+template<typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::begin() const
+{
+  return cbegin();
+}
+
+/** returns a read-only const_iterator to the first element of the 1D vector or array
+  * \only_for_vectors
+  * \sa cend(), begin()
+  */
+template<typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::cbegin() const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  return const_iterator(derived(), 0);
+}
+
+/** returns an iterator to the element following the last element of the 1D vector or array
+  * \only_for_vectors
+  * \sa begin(), cend()
+  */
+template<typename Derived>
+inline typename DenseBase<Derived>::iterator DenseBase<Derived>::end()
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  return iterator(derived(), size());
+}
+
+/** const version of end() */
+template<typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::end() const
+{
+  return cend();
+}
+
+/** returns a read-only const_iterator to the element following the last element of the 1D vector or array
+  * \only_for_vectors
+  * \sa begin(), cend()
+  */
+template<typename Derived>
+inline typename DenseBase<Derived>::const_iterator DenseBase<Derived>::cend() const
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived);
+  return const_iterator(derived(), size());
+}
+
+} // namespace Eigen
+
+#endif // EIGEN_STLITERATORS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
index 513742f..6494d51 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Stride.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_STRIDE_H
 #define EIGEN_STRIDE_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \class Stride
   * \ingroup Core_Module
@@ -38,6 +38,10 @@
   * \include Map_general_stride.cpp
   * Output: \verbinclude Map_general_stride.out
   *
+  * Both strides can be negative, however, a negative stride of -1 cannot be specified at compiletime
+  * because of the ambiguity with Dynamic which is defined to -1 (historically, negative strides were
+  * not allowed).
+  *
   * \sa class InnerStride, class OuterStride, \ref TopicStorageOrders
   */
 template<int _OuterStrideAtCompileTime, int _InnerStrideAtCompileTime>
@@ -55,6 +59,8 @@
     Stride()
       : m_outer(OuterStrideAtCompileTime), m_inner(InnerStrideAtCompileTime)
     {
+      // FIXME: for Eigen 4 we should use DynamicIndex instead of Dynamic.
+      // FIXME: for Eigen 4 we should also unify this API with fix<>
       eigen_assert(InnerStrideAtCompileTime != Dynamic && OuterStrideAtCompileTime != Dynamic);
     }
 
@@ -63,7 +69,6 @@
     Stride(Index outerStride, Index innerStride)
       : m_outer(outerStride), m_inner(innerStride)
     {
-      eigen_assert(innerStride>=0 && outerStride>=0);
     }
 
     /** Copy constructor */
@@ -73,10 +78,10 @@
     {}
 
     /** \returns the outer stride */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index outer() const { return m_outer.value(); }
     /** \returns the inner stride */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
     inline Index inner() const { return m_inner.value(); }
 
   protected:
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
index d702009..180a4e5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Swap.h
@@ -30,12 +30,13 @@
   typedef typename Base::DstXprType DstXprType;
   typedef swap_assign_op<Scalar> Functor;
   
-  EIGEN_DEVICE_FUNC generic_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  generic_dense_assignment_kernel(DstEvaluatorTypeT &dst, const SrcEvaluatorTypeT &src, const Functor &func, DstXprType& dstExpr)
     : Base(dst, src, func, dstExpr)
   {}
   
   template<int StoreMode, int LoadMode, typename PacketType>
-  void assignPacket(Index row, Index col)
+  EIGEN_STRONG_INLINE void assignPacket(Index row, Index col)
   {
     PacketType tmp = m_src.template packet<LoadMode,PacketType>(row,col);
     const_cast<SrcEvaluatorTypeT&>(m_src).template writePacket<LoadMode>(row,col, m_dst.template packet<StoreMode,PacketType>(row,col));
@@ -43,7 +44,7 @@
   }
   
   template<int StoreMode, int LoadMode, typename PacketType>
-  void assignPacket(Index index)
+  EIGEN_STRONG_INLINE void assignPacket(Index index)
   {
     PacketType tmp = m_src.template packet<LoadMode,PacketType>(index);
     const_cast<SrcEvaluatorTypeT&>(m_src).template writePacket<LoadMode>(index, m_dst.template packet<StoreMode,PacketType>(index));
@@ -52,7 +53,7 @@
   
   // TODO find a simple way not to have to copy/paste this function from generic_dense_assignment_kernel, by simple I mean no CRTP (Gael)
   template<int StoreMode, int LoadMode, typename PacketType>
-  void assignPacketByOuterInner(Index outer, Index inner)
+  EIGEN_STRONG_INLINE void assignPacketByOuterInner(Index outer, Index inner)
   {
     Index row = Base::rowIndexByOuterInner(outer, inner); 
     Index col = Base::colIndexByOuterInner(outer, inner);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
index 960dc45..2bc658f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpose.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_TRANSPOSE_H
 #define EIGEN_TRANSPOSE_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 template<typename MatrixType>
@@ -61,24 +61,27 @@
     typedef typename internal::remove_all<MatrixType>::type NestedExpression;
 
     EIGEN_DEVICE_FUNC
-    explicit inline Transpose(MatrixType& matrix) : m_matrix(matrix) {}
+    explicit EIGEN_STRONG_INLINE Transpose(MatrixType& matrix) : m_matrix(matrix) {}
 
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Transpose)
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_matrix.cols(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
 
     /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     const typename internal::remove_all<MatrixTypeNested>::type&
     nestedExpression() const { return m_matrix; }
 
     /** \returns the nested expression */
-    EIGEN_DEVICE_FUNC
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     typename internal::remove_reference<MatrixTypeNested>::type&
     nestedExpression() { return m_matrix; }
 
     /** \internal */
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
     void resize(Index nrows, Index ncols) {
       m_matrix.resize(ncols,nrows);
     }
@@ -122,8 +125,10 @@
     EIGEN_DENSE_PUBLIC_INTERFACE(Transpose<MatrixType>)
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TransposeImpl)
 
-    EIGEN_DEVICE_FUNC inline Index innerStride() const { return derived().nestedExpression().innerStride(); }
-    EIGEN_DEVICE_FUNC inline Index outerStride() const { return derived().nestedExpression().outerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Index innerStride() const { return derived().nestedExpression().innerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    Index outerStride() const { return derived().nestedExpression().outerStride(); }
 
     typedef typename internal::conditional<
                        internal::is_lvalue<MatrixType>::value,
@@ -131,18 +136,20 @@
                        const Scalar
                      >::type ScalarWithConstIfNotLvalue;
 
-    EIGEN_DEVICE_FUNC inline ScalarWithConstIfNotLvalue* data() { return derived().nestedExpression().data(); }
-    EIGEN_DEVICE_FUNC inline const Scalar* data() const { return derived().nestedExpression().data(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    ScalarWithConstIfNotLvalue* data() { return derived().nestedExpression().data(); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const Scalar* data() const { return derived().nestedExpression().data(); }
 
     // FIXME: shall we keep the const version of coeffRef?
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index rowId, Index colId) const
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const Scalar& coeffRef(Index rowId, Index colId) const
     {
       return derived().nestedExpression().coeffRef(colId, rowId);
     }
 
-    EIGEN_DEVICE_FUNC
-    inline const Scalar& coeffRef(Index index) const
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    const Scalar& coeffRef(Index index) const
     {
       return derived().nestedExpression().coeffRef(index);
     }
@@ -170,7 +177,8 @@
   *
   * \sa transposeInPlace(), adjoint() */
 template<typename Derived>
-inline Transpose<Derived>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+Transpose<Derived>
 DenseBase<Derived>::transpose()
 {
   return TransposeReturnType(derived());
@@ -182,7 +190,8 @@
   *
   * \sa transposeInPlace(), adjoint() */
 template<typename Derived>
-inline typename DenseBase<Derived>::ConstTransposeReturnType
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename DenseBase<Derived>::ConstTransposeReturnType
 DenseBase<Derived>::transpose() const
 {
   return ConstTransposeReturnType(derived());
@@ -208,7 +217,7 @@
   *
   * \sa adjointInPlace(), transpose(), conjugate(), class Transpose, class internal::scalar_conjugate_op */
 template<typename Derived>
-inline const typename MatrixBase<Derived>::AdjointReturnType
+EIGEN_DEVICE_FUNC inline const typename MatrixBase<Derived>::AdjointReturnType
 MatrixBase<Derived>::adjoint() const
 {
   return AdjointReturnType(this->transpose());
@@ -230,11 +239,10 @@
 template<typename MatrixType>
 struct inplace_transpose_selector<MatrixType,true,false> { // square matrix
   static void run(MatrixType& m) {
-    m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose());
+    m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose().template triangularView<StrictlyUpper>());
   }
 };
 
-// TODO: vectorized path is currently limited to LargestPacketSize x LargestPacketSize cases only.
 template<typename MatrixType>
 struct inplace_transpose_selector<MatrixType,true,true> { // PacketSize x PacketSize
   static void run(MatrixType& m) {
@@ -251,16 +259,66 @@
   }
 };
 
+
+template <typename MatrixType, Index Alignment>
+void BlockedInPlaceTranspose(MatrixType& m) {
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename internal::packet_traits<typename MatrixType::Scalar>::type Packet;
+  const Index PacketSize = internal::packet_traits<Scalar>::size;
+  eigen_assert(m.rows() == m.cols());
+  int row_start = 0;
+  for (; row_start + PacketSize <= m.rows(); row_start += PacketSize) {
+    for (int col_start = row_start; col_start + PacketSize <= m.cols(); col_start += PacketSize) {
+      PacketBlock<Packet> A;
+      if (row_start == col_start) {
+        for (Index i=0; i<PacketSize; ++i)
+          A.packet[i] = m.template packetByOuterInner<Alignment>(row_start + i,col_start);
+        internal::ptranspose(A);
+        for (Index i=0; i<PacketSize; ++i)
+          m.template writePacket<Alignment>(m.rowIndexByOuterInner(row_start + i, col_start), m.colIndexByOuterInner(row_start + i,col_start), A.packet[i]);
+      } else {
+        PacketBlock<Packet> B;
+        for (Index i=0; i<PacketSize; ++i) {
+          A.packet[i] = m.template packetByOuterInner<Alignment>(row_start + i,col_start);
+          B.packet[i] = m.template packetByOuterInner<Alignment>(col_start + i, row_start);
+        }
+        internal::ptranspose(A);
+        internal::ptranspose(B);
+        for (Index i=0; i<PacketSize; ++i) {
+          m.template writePacket<Alignment>(m.rowIndexByOuterInner(row_start + i, col_start), m.colIndexByOuterInner(row_start + i,col_start), B.packet[i]);
+          m.template writePacket<Alignment>(m.rowIndexByOuterInner(col_start + i, row_start), m.colIndexByOuterInner(col_start + i,row_start), A.packet[i]);
+        }
+      }
+    }
+  }
+  for (Index row = row_start; row < m.rows(); ++row) {
+    m.matrix().row(row).head(row).swap(
+        m.matrix().col(row).head(row).transpose());
+  }
+}
+
 template<typename MatrixType,bool MatchPacketSize>
-struct inplace_transpose_selector<MatrixType,false,MatchPacketSize> { // non square matrix
+struct inplace_transpose_selector<MatrixType,false,MatchPacketSize> { // non square or dynamic matrix
   static void run(MatrixType& m) {
-    if (m.rows()==m.cols())
-      m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose());
-    else
+    typedef typename MatrixType::Scalar Scalar;
+    if (m.rows() == m.cols()) {
+      const Index PacketSize = internal::packet_traits<Scalar>::size;
+      if (!NumTraits<Scalar>::IsComplex && m.rows() >= PacketSize) {
+        if ((m.rows() % PacketSize) == 0)
+          BlockedInPlaceTranspose<MatrixType,internal::evaluator<MatrixType>::Alignment>(m);
+        else
+          BlockedInPlaceTranspose<MatrixType,Unaligned>(m);
+      }
+      else {
+        m.matrix().template triangularView<StrictlyUpper>().swap(m.matrix().transpose().template triangularView<StrictlyUpper>());
+      }
+    } else {
       m = m.transpose().eval();
+    }
   }
 };
 
+
 } // end namespace internal
 
 /** This is the "in place" version of transpose(): it replaces \c *this by its own transpose.
@@ -278,12 +336,12 @@
   * Notice however that this method is only useful if you want to replace a matrix by its own transpose.
   * If you just need the transpose of a matrix, use transpose().
   *
-  * \note if the matrix is not square, then \c *this must be a resizable matrix. 
+  * \note if the matrix is not square, then \c *this must be a resizable matrix.
   * This excludes (non-square) fixed-size matrices, block-expressions and maps.
   *
   * \sa transpose(), adjoint(), adjointInPlace() */
 template<typename Derived>
-inline void DenseBase<Derived>::transposeInPlace()
+EIGEN_DEVICE_FUNC inline void DenseBase<Derived>::transposeInPlace()
 {
   eigen_assert((rows() == cols() || (RowsAtCompileTime == Dynamic && ColsAtCompileTime == Dynamic))
                && "transposeInPlace() called on a non-square non-resizable matrix");
@@ -314,7 +372,7 @@
   *
   * \sa transpose(), adjoint(), transposeInPlace() */
 template<typename Derived>
-inline void MatrixBase<Derived>::adjointInPlace()
+EIGEN_DEVICE_FUNC inline void MatrixBase<Derived>::adjointInPlace()
 {
   derived() = adjoint().eval();
 }
@@ -393,7 +451,8 @@
 template<typename Dst, typename Src>
 void check_for_aliasing(const Dst &dst, const Src &src)
 {
-  internal::checkTransposeAliasing_impl<Dst, Src>::run(dst, src);
+  if((!Dst::IsVectorAtCompileTime) && dst.rows()>1 && dst.cols()>1)
+    internal::checkTransposeAliasing_impl<Dst, Src>::run(dst, src);
 }
 
 } // end namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
index 86da5af..38a7b01 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Transpositions.h
@@ -10,20 +10,22 @@
 #ifndef EIGEN_TRANSPOSITIONS_H
 #define EIGEN_TRANSPOSITIONS_H
 
-namespace Eigen { 
+namespace Eigen {
 
 template<typename Derived>
 class TranspositionsBase
 {
     typedef internal::traits<Derived> Traits;
-    
+
   public:
 
     typedef typename Traits::IndicesType IndicesType;
     typedef typename IndicesType::Scalar StorageIndex;
     typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
 
+    EIGEN_DEVICE_FUNC
     Derived& derived() { return *static_cast<Derived*>(this); }
+    EIGEN_DEVICE_FUNC
     const Derived& derived() const { return *static_cast<const Derived*>(this); }
 
     /** Copies the \a other transpositions into \c *this */
@@ -33,26 +35,19 @@
       indices() = other.indices();
       return derived();
     }
-    
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    Derived& operator=(const TranspositionsBase& other)
-    {
-      indices() = other.indices();
-      return derived();
-    }
-    #endif
 
     /** \returns the number of transpositions */
+    EIGEN_DEVICE_FUNC
     Index size() const { return indices().size(); }
     /** \returns the number of rows of the equivalent permutation matrix */
+    EIGEN_DEVICE_FUNC
     Index rows() const { return indices().size(); }
     /** \returns the number of columns of the equivalent permutation matrix */
+    EIGEN_DEVICE_FUNC
     Index cols() const { return indices().size(); }
 
     /** Direct access to the underlying index vector */
+    EIGEN_DEVICE_FUNC
     inline const StorageIndex& coeff(Index i) const { return indices().coeff(i); }
     /** Direct access to the underlying index vector */
     inline StorageIndex& coeffRef(Index i) { return indices().coeffRef(i); }
@@ -66,8 +61,10 @@
     inline StorageIndex& operator[](Index i) { return indices()(i); }
 
     /** const version of indices(). */
+    EIGEN_DEVICE_FUNC
     const IndicesType& indices() const { return derived().indices(); }
     /** \returns a reference to the stored array representing the transpositions. */
+    EIGEN_DEVICE_FUNC
     IndicesType& indices() { return derived().indices(); }
 
     /** Resizes to given size. */
@@ -84,7 +81,7 @@
     }
 
     // FIXME: do we want such methods ?
-    // might be usefull when the target matrix expression is complex, e.g.:
+    // might be useful when the target matrix expression is complex, e.g.:
     // object.matrix().block(..,..,..,..) = trans * object.matrix().block(..,..,..,..);
     /*
     template<typename MatrixType>
@@ -171,12 +168,6 @@
     inline Transpositions(const TranspositionsBase<OtherDerived>& other)
       : m_indices(other.indices()) {}
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** Standard copy constructor. Defined only to prevent a default copy constructor
-      * from hiding the other templated constructor */
-    inline Transpositions(const Transpositions& other) : m_indices(other.indices()) {}
-    #endif
-
     /** Generic constructor from expression of the transposition indices. */
     template<typename Other>
     explicit inline Transpositions(const MatrixBase<Other>& indices) : m_indices(indices)
@@ -189,25 +180,16 @@
       return Base::operator=(other);
     }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    Transpositions& operator=(const Transpositions& other)
-    {
-      m_indices = other.m_indices;
-      return *this;
-    }
-    #endif
-
     /** Constructs an uninitialized permutation matrix of given size.
       */
     inline Transpositions(Index size) : m_indices(size)
     {}
 
     /** const version of indices(). */
+    EIGEN_DEVICE_FUNC
     const IndicesType& indices() const { return m_indices; }
     /** \returns a reference to the stored array representing the transpositions. */
+    EIGEN_DEVICE_FUNC
     IndicesType& indices() { return m_indices; }
 
   protected:
@@ -265,9 +247,11 @@
     #endif
 
     /** const version of indices(). */
+    EIGEN_DEVICE_FUNC
     const IndicesType& indices() const { return m_indices; }
-    
+
     /** \returns a reference to the stored array representing the transpositions. */
+    EIGEN_DEVICE_FUNC
     IndicesType& indices() { return m_indices; }
 
   protected:
@@ -306,21 +290,12 @@
       return Base::operator=(other);
     }
 
-    #ifndef EIGEN_PARSED_BY_DOXYGEN
-    /** This is a special case of the templated operator=. Its purpose is to
-      * prevent a default operator= from hiding the templated operator=.
-      */
-    TranspositionsWrapper& operator=(const TranspositionsWrapper& other)
-    {
-      m_indices = other.m_indices;
-      return *this;
-    }
-    #endif
-
     /** const version of indices(). */
+    EIGEN_DEVICE_FUNC
     const IndicesType& indices() const { return m_indices; }
 
     /** \returns a reference to the stored array representing the transpositions. */
+    EIGEN_DEVICE_FUNC
     IndicesType& indices() { return m_indices; }
 
   protected:
@@ -374,9 +349,12 @@
 
     explicit Transpose(const TranspositionType& t) : m_transpositions(t) {}
 
-    Index size() const { return m_transpositions.size(); }
-    Index rows() const { return m_transpositions.size(); }
-    Index cols() const { return m_transpositions.size(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index size() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return m_transpositions.size(); }
 
     /** \returns the \a matrix with the inverse transpositions applied to the columns.
       */
@@ -395,7 +373,8 @@
     {
       return Product<Transpose, OtherDerived, AliasFreeProduct>(*this, matrix.derived());
     }
-    
+
+    EIGEN_DEVICE_FUNC
     const TranspositionType& nestedExpression() const { return m_transpositions; }
 
   protected:
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
index 9abb7e3..fdb8bc1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/TriangularMatrix.h
@@ -11,12 +11,12 @@
 #ifndef EIGEN_TRIANGULARMATRIX_H
 #define EIGEN_TRIANGULARMATRIX_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
-  
+
 template<int Side, typename TriangularType, typename Rhs> struct triangular_solve_retval;
-  
+
 }
 
 /** \class TriangularBase
@@ -34,16 +34,16 @@
       ColsAtCompileTime = internal::traits<Derived>::ColsAtCompileTime,
       MaxRowsAtCompileTime = internal::traits<Derived>::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = internal::traits<Derived>::MaxColsAtCompileTime,
-      
+
       SizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::RowsAtCompileTime,
                                                    internal::traits<Derived>::ColsAtCompileTime>::ret),
       /**< This is equal to the number of coefficients, i.e. the number of
           * rows times the number of columns, or to \a Dynamic if this is not
           * known at compile-time. \sa RowsAtCompileTime, ColsAtCompileTime */
-      
+
       MaxSizeAtCompileTime = (internal::size_at_compile_time<internal::traits<Derived>::MaxRowsAtCompileTime,
                                                    internal::traits<Derived>::MaxColsAtCompileTime>::ret)
-        
+
     };
     typedef typename internal::traits<Derived>::Scalar Scalar;
     typedef typename internal::traits<Derived>::StorageKind StorageKind;
@@ -53,18 +53,19 @@
     typedef Derived const& Nested;
 
     EIGEN_DEVICE_FUNC
-    inline TriangularBase() { eigen_assert(!((Mode&UnitDiag) && (Mode&ZeroDiag))); }
+    inline TriangularBase() { eigen_assert(!((int(Mode) & int(UnitDiag)) && (int(Mode) & int(ZeroDiag)))); }
 
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return derived().rows(); }
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return derived().cols(); }
-    EIGEN_DEVICE_FUNC
-    inline Index outerStride() const { return derived().outerStride(); }
-    EIGEN_DEVICE_FUNC
-    inline Index innerStride() const { return derived().innerStride(); }
-    
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return derived().rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return derived().cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index outerStride() const EIGEN_NOEXCEPT { return derived().outerStride(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index innerStride() const EIGEN_NOEXCEPT { return derived().innerStride(); }
+
     // dummy resize function
+    EIGEN_DEVICE_FUNC
     void resize(Index rows, Index cols)
     {
       EIGEN_UNUSED_VARIABLE(rows);
@@ -155,7 +156,7 @@
   * \param MatrixType the type of the object in which we are taking the triangular part
   * \param Mode the kind of triangular matrix expression to construct. Can be #Upper,
   *             #Lower, #UnitUpper, #UnitLower, #StrictlyUpper, or #StrictlyLower.
-  *             This is in fact a bit field; it must have either #Upper or #Lower, 
+  *             This is in fact a bit field; it must have either #Upper or #Lower,
   *             and additionally it may have #UnitDiag or #ZeroDiag or neither.
   *
   * This class represents a triangular part of a matrix, not necessarily square. Strictly speaking, for rectangular
@@ -197,7 +198,8 @@
     typedef typename internal::traits<TriangularView>::MatrixTypeNestedNonRef MatrixTypeNestedNonRef;
 
     typedef typename internal::remove_all<typename MatrixType::ConjugateReturnType>::type MatrixConjugateReturnType;
-    
+    typedef TriangularView<typename internal::add_const<MatrixType>::type, _Mode> ConstTriangularView;
+
   public:
 
     typedef typename internal::traits<TriangularView>::StorageKind StorageKind;
@@ -216,15 +218,15 @@
     EIGEN_DEVICE_FUNC
     explicit inline TriangularView(MatrixType& matrix) : m_matrix(matrix)
     {}
-    
+
     EIGEN_INHERIT_ASSIGNMENT_OPERATORS(TriangularView)
 
     /** \copydoc EigenBase::rows() */
-    EIGEN_DEVICE_FUNC
-    inline Index rows() const { return m_matrix.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
     /** \copydoc EigenBase::cols() */
-    EIGEN_DEVICE_FUNC
-    inline Index cols() const { return m_matrix.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
     /** \returns a const reference to the nested expression */
     EIGEN_DEVICE_FUNC
@@ -233,13 +235,25 @@
     /** \returns a reference to the nested expression */
     EIGEN_DEVICE_FUNC
     NestedExpression& nestedExpression() { return m_matrix; }
-    
+
     typedef TriangularView<const MatrixConjugateReturnType,Mode> ConjugateReturnType;
     /** \sa MatrixBase::conjugate() const */
     EIGEN_DEVICE_FUNC
     inline const ConjugateReturnType conjugate() const
     { return ConjugateReturnType(m_matrix.conjugate()); }
 
+    /** \returns an expression of the complex conjugate of \c *this if Cond==true,
+     *           returns \c *this otherwise.
+     */
+    template<bool Cond>
+    EIGEN_DEVICE_FUNC
+    inline typename internal::conditional<Cond,ConjugateReturnType,ConstTriangularView>::type
+    conjugateIf() const
+    {
+      typedef typename internal::conditional<Cond,ConjugateReturnType,ConstTriangularView>::type ReturnType;
+      return ReturnType(m_matrix.template conjugateIf<Cond>());
+    }
+
     typedef TriangularView<const typename MatrixType::AdjointReturnType,TransposeMode> AdjointReturnType;
     /** \sa MatrixBase::adjoint() const */
     EIGEN_DEVICE_FUNC
@@ -255,7 +269,7 @@
       typename MatrixType::TransposeReturnType tmp(m_matrix);
       return TransposeReturnType(tmp);
     }
-    
+
     typedef TriangularView<const typename MatrixType::ConstTransposeReturnType,TransposeMode> ConstTransposeReturnType;
     /** \sa MatrixBase::transpose() const */
     EIGEN_DEVICE_FUNC
@@ -266,10 +280,10 @@
 
     template<typename Other>
     EIGEN_DEVICE_FUNC
-    inline const Solve<TriangularView, Other> 
+    inline const Solve<TriangularView, Other>
     solve(const MatrixBase<Other>& other) const
     { return Solve<TriangularView, Other>(*this, other.derived()); }
-    
+
   // workaround MSVC ICE
   #if EIGEN_COMP_MSVC
     template<int Side, typename Other>
@@ -313,7 +327,7 @@
       else
         return m_matrix.diagonal().prod();
     }
-      
+
   protected:
 
     MatrixTypeNested m_matrix;
@@ -375,7 +389,7 @@
       internal::call_assignment_no_alias(derived(), other.derived(), internal::sub_assign_op<Scalar,typename Other::Scalar>());
       return derived();
     }
-    
+
     /** \sa MatrixBase::operator*=() */
     EIGEN_DEVICE_FUNC
     TriangularViewType&  operator*=(const typename internal::traits<MatrixType>::Scalar& other) { return *this = derived().nestedExpression() * other; }
@@ -433,14 +447,14 @@
     TriangularViewType& operator=(const TriangularViewImpl& other)
     { return *this = other.derived().nestedExpression(); }
 
-    /** \deprecated */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    /** \deprecated */
+    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
     void lazyAssign(const TriangularBase<OtherDerived>& other);
 
-    /** \deprecated */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    /** \deprecated */
+    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
     void lazyAssign(const MatrixBase<OtherDerived>& other);
 #endif
 
@@ -468,7 +482,7 @@
       * \a Side==OnTheLeft (the default), or the right-inverse-multiply  \a other * inverse(\c *this) if
       * \a Side==OnTheRight.
       *
-      * Note that the template parameter \c Side can be ommitted, in which case \c Side==OnTheLeft
+      * Note that the template parameter \c Side can be omitted, in which case \c Side==OnTheLeft
       *
       * The matrix \c *this must be triangular and invertible (i.e., all the coefficients of the
       * diagonal must be non zero). It works as a forward (resp. backward) substitution if \c *this
@@ -486,7 +500,6 @@
       * \sa TriangularView::solveInPlace()
       */
     template<int Side, typename Other>
-    EIGEN_DEVICE_FUNC
     inline const internal::triangular_solve_retval<Side,TriangularViewType, Other>
     solve(const MatrixBase<Other>& other) const;
 
@@ -495,7 +508,7 @@
       * \warning The parameter is only marked 'const' to make the C++ compiler accept a temporary expression here.
       * This function will const_cast it, so constness isn't honored here.
       *
-      * Note that the template parameter \c Side can be ommitted, in which case \c Side==OnTheLeft
+      * Note that the template parameter \c Side can be omitted, in which case \c Side==OnTheLeft
       *
       * See TriangularView:solve() for the details.
       */
@@ -521,10 +534,10 @@
       call_assignment(derived(), other.const_cast_derived(), internal::swap_assign_op<Scalar>());
     }
 
-    /** \deprecated
-      * Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
+    /** Shortcut for \code (*this).swap(other.triangularView<(*this)::Mode>()) \endcode */
     template<typename OtherDerived>
-    EIGEN_DEVICE_FUNC
+    /** \deprecated */
+    EIGEN_DEPRECATED EIGEN_DEVICE_FUNC
     void swap(MatrixBase<OtherDerived> const & other)
     {
       EIGEN_STATIC_ASSERT_LVALUE(OtherDerived);
@@ -556,7 +569,7 @@
 // FIXME should we keep that possibility
 template<typename MatrixType, unsigned int Mode>
 template<typename OtherDerived>
-inline TriangularView<MatrixType, Mode>&
+EIGEN_DEVICE_FUNC inline TriangularView<MatrixType, Mode>&
 TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const MatrixBase<OtherDerived>& other)
 {
   internal::call_assignment_no_alias(derived(), other.derived(), internal::assign_op<Scalar,typename OtherDerived::Scalar>());
@@ -566,7 +579,7 @@
 // FIXME should we keep that possibility
 template<typename MatrixType, unsigned int Mode>
 template<typename OtherDerived>
-void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const MatrixBase<OtherDerived>& other)
+EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const MatrixBase<OtherDerived>& other)
 {
   internal::call_assignment_no_alias(derived(), other.template triangularView<Mode>());
 }
@@ -575,7 +588,7 @@
 
 template<typename MatrixType, unsigned int Mode>
 template<typename OtherDerived>
-inline TriangularView<MatrixType, Mode>&
+EIGEN_DEVICE_FUNC inline TriangularView<MatrixType, Mode>&
 TriangularViewImpl<MatrixType, Mode, Dense>::operator=(const TriangularBase<OtherDerived>& other)
 {
   eigen_assert(Mode == int(OtherDerived::Mode));
@@ -585,7 +598,7 @@
 
 template<typename MatrixType, unsigned int Mode>
 template<typename OtherDerived>
-void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const TriangularBase<OtherDerived>& other)
+EIGEN_DEVICE_FUNC void TriangularViewImpl<MatrixType, Mode, Dense>::lazyAssign(const TriangularBase<OtherDerived>& other)
 {
   eigen_assert(Mode == int(OtherDerived::Mode));
   internal::call_assignment_no_alias(derived(), other.derived());
@@ -600,7 +613,7 @@
   * If the matrix is triangular, the opposite part is set to zero. */
 template<typename Derived>
 template<typename DenseDerived>
-void TriangularBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
+EIGEN_DEVICE_FUNC void TriangularBase<Derived>::evalTo(MatrixBase<DenseDerived> &other) const
 {
   evalToLazy(other.derived());
 }
@@ -626,6 +639,7 @@
   */
 template<typename Derived>
 template<unsigned int Mode>
+EIGEN_DEVICE_FUNC
 typename MatrixBase<Derived>::template TriangularViewReturnType<Mode>::Type
 MatrixBase<Derived>::triangularView()
 {
@@ -635,6 +649,7 @@
 /** This is the const version of MatrixBase::triangularView() */
 template<typename Derived>
 template<unsigned int Mode>
+EIGEN_DEVICE_FUNC
 typename MatrixBase<Derived>::template ConstTriangularViewReturnType<Mode>::Type
 MatrixBase<Derived>::triangularView() const
 {
@@ -700,7 +715,7 @@
 
 namespace internal {
 
-  
+
 // TODO currently a triangular expression has the form TriangularView<.,.>
 //      in the future triangular-ness should be defined by the expression traits
 //      such that Transpose<TriangularView<.,.> > is valid. (currently TriangularBase::transpose() is overloaded to make it work)
@@ -717,6 +732,7 @@
 {
   typedef TriangularView<MatrixType,Mode> XprType;
   typedef evaluator<typename internal::remove_all<MatrixType>::type> Base;
+  EIGEN_DEVICE_FUNC
   unary_evaluator(const XprType &xpr) : Base(xpr.nestedExpression()) {}
 };
 
@@ -728,7 +744,7 @@
 
 template<typename Kernel, unsigned int Mode, int UnrollCount, bool ClearOpposite> struct triangular_assignment_loop;
 
- 
+
 /** \internal Specialization of the dense assignment kernel for triangular matrices.
   * The main difference is that the triangular, diagonal, and opposite parts are processed through three different functions.
   * \tparam UpLo must be either Lower or Upper
@@ -745,17 +761,17 @@
   using Base::m_src;
   using Base::m_functor;
 public:
-  
+
   typedef typename Base::DstEvaluatorType DstEvaluatorType;
   typedef typename Base::SrcEvaluatorType SrcEvaluatorType;
   typedef typename Base::Scalar Scalar;
   typedef typename Base::AssignmentTraits AssignmentTraits;
-  
-  
+
+
   EIGEN_DEVICE_FUNC triangular_dense_assignment_kernel(DstEvaluatorType &dst, const SrcEvaluatorType &src, const Functor &func, DstXprType& dstExpr)
     : Base(dst, src, func, dstExpr)
   {}
-  
+
 #ifdef EIGEN_INTERNAL_DEBUGGING
   EIGEN_DEVICE_FUNC void assignCoeff(Index row, Index col)
   {
@@ -765,16 +781,16 @@
 #else
   using Base::assignCoeff;
 #endif
-  
+
   EIGEN_DEVICE_FUNC void assignDiagonalCoeff(Index id)
   {
          if(Mode==UnitDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(1));
     else if(Mode==ZeroDiag && SetOpposite) m_functor.assignCoeff(m_dst.coeffRef(id,id), Scalar(0));
     else if(Mode==0)                       Base::assignCoeff(id,id);
   }
-  
+
   EIGEN_DEVICE_FUNC void assignOppositeCoeff(Index row, Index col)
-  { 
+  {
     eigen_internal_assert(row!=col);
     if(SetOpposite)
       m_functor.assignCoeff(m_dst.coeffRef(row,col), Scalar(0));
@@ -795,17 +811,17 @@
   if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
     dst.resize(dstRows, dstCols);
   DstEvaluatorType dstEvaluator(dst);
-    
+
   typedef triangular_dense_assignment_kernel< Mode&(Lower|Upper),Mode&(UnitDiag|ZeroDiag|SelfAdjoint),SetOpposite,
                                               DstEvaluatorType,SrcEvaluatorType,Functor> Kernel;
   Kernel kernel(dstEvaluator, srcEvaluator, func, dst.const_cast_derived());
-  
+
   enum {
       unroll = DstXprType::SizeAtCompileTime != Dynamic
             && SrcEvaluatorType::CoeffReadCost < HugeCost
-            && DstXprType::SizeAtCompileTime * (DstEvaluatorType::CoeffReadCost+SrcEvaluatorType::CoeffReadCost) / 2 <= EIGEN_UNROLLING_LIMIT
+            && DstXprType::SizeAtCompileTime * (int(DstEvaluatorType::CoeffReadCost) + int(SrcEvaluatorType::CoeffReadCost)) / 2 <= EIGEN_UNROLLING_LIMIT
     };
-  
+
   triangular_assignment_loop<Kernel, Mode, unroll ? int(DstXprType::SizeAtCompileTime) : Dynamic, SetOpposite>::run(kernel);
 }
 
@@ -827,8 +843,8 @@
   EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
   {
     eigen_assert(int(DstXprType::Mode) == int(SrcXprType::Mode));
-    
-    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);  
+
+    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);
   }
 };
 
@@ -837,7 +853,7 @@
 {
   EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
   {
-    call_triangular_assignment_loop<SrcXprType::Mode, (SrcXprType::Mode&SelfAdjoint)==0>(dst, src, func);  
+    call_triangular_assignment_loop<SrcXprType::Mode, (int(SrcXprType::Mode) & int(SelfAdjoint)) == 0>(dst, src, func);
   }
 };
 
@@ -846,7 +862,7 @@
 {
   EIGEN_DEVICE_FUNC static void run(DstXprType &dst, const SrcXprType &src, const Functor &func)
   {
-    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);  
+    call_triangular_assignment_loop<DstXprType::Mode, false>(dst, src, func);
   }
 };
 
@@ -857,19 +873,19 @@
   // FIXME: this is not very clean, perhaps this information should be provided by the kernel?
   typedef typename Kernel::DstEvaluatorType DstEvaluatorType;
   typedef typename DstEvaluatorType::XprType DstXprType;
-  
+
   enum {
     col = (UnrollCount-1) / DstXprType::RowsAtCompileTime,
     row = (UnrollCount-1) % DstXprType::RowsAtCompileTime
   };
-  
+
   typedef typename Kernel::Scalar Scalar;
 
   EIGEN_DEVICE_FUNC
   static inline void run(Kernel &kernel)
   {
     triangular_assignment_loop<Kernel, Mode, UnrollCount-1, SetOpposite>::run(kernel);
-    
+
     if(row==col)
       kernel.assignDiagonalCoeff(row);
     else if( ((Mode&Lower) && row>col) || ((Mode&Upper) && row<col) )
@@ -912,10 +928,10 @@
       }
       else
         i = maxi;
-      
+
       if(i<kernel.rows()) // then i==j
         kernel.assignDiagonalCoeff(i++);
-      
+
       if (((Mode&Upper) && SetOpposite) || (Mode&Lower))
       {
         for(; i < kernel.rows(); ++i)
@@ -932,14 +948,14 @@
   * If the matrix is triangular, the opposite part is set to zero. */
 template<typename Derived>
 template<typename DenseDerived>
-void TriangularBase<Derived>::evalToLazy(MatrixBase<DenseDerived> &other) const
+EIGEN_DEVICE_FUNC void TriangularBase<Derived>::evalToLazy(MatrixBase<DenseDerived> &other) const
 {
   other.derived().resize(this->rows(), this->cols());
-  internal::call_triangular_assignment_loop<Derived::Mode,(Derived::Mode&SelfAdjoint)==0 /* SetOpposite */>(other.derived(), derived().nestedExpression());
+  internal::call_triangular_assignment_loop<Derived::Mode, (int(Derived::Mode) & int(SelfAdjoint)) == 0 /* SetOpposite */>(other.derived(), derived().nestedExpression());
 }
 
 namespace internal {
-  
+
 // Triangular = Product
 template< typename DstXprType, typename Lhs, typename Rhs, typename Scalar>
 struct Assignment<DstXprType, Product<Lhs,Rhs,DefaultProduct>, internal::assign_op<Scalar,typename Product<Lhs,Rhs,DefaultProduct>::Scalar>, Dense2Triangular>
@@ -952,7 +968,7 @@
     if((dst.rows()!=dstRows) || (dst.cols()!=dstCols))
       dst.resize(dstRows, dstCols);
 
-    dst._assignProduct(src, 1, 0);
+    dst._assignProduct(src, Scalar(1), false);
   }
 };
 
@@ -963,7 +979,7 @@
   typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
   static void run(DstXprType &dst, const SrcXprType &src, const internal::add_assign_op<Scalar,typename SrcXprType::Scalar> &)
   {
-    dst._assignProduct(src, 1, 1);
+    dst._assignProduct(src, Scalar(1), true);
   }
 };
 
@@ -974,7 +990,7 @@
   typedef Product<Lhs,Rhs,DefaultProduct> SrcXprType;
   static void run(DstXprType &dst, const SrcXprType &src, const internal::sub_assign_op<Scalar,typename SrcXprType::Scalar> &)
   {
-    dst._assignProduct(src, -1, 1);
+    dst._assignProduct(src, Scalar(-1), true);
   }
 };
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
index d72fbf7..71c5b95 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorBlock.h
@@ -35,7 +35,7 @@
   * It is the return type of DenseBase::segment(Index,Index) and DenseBase::segment<int>(Index) and
   * most of the time this is the only way it is used.
   *
-  * However, if you want to directly maniputate sub-vector expressions,
+  * However, if you want to directly manipulate sub-vector expressions,
   * for instance if you want to write a function returning such an expression, you
   * will need to use this class.
   *
@@ -71,8 +71,8 @@
 
     /** Dynamic-size constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline VectorBlock(VectorType& vector, Index start, Index size)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    VectorBlock(VectorType& vector, Index start, Index size)
       : Base(vector,
              IsColVector ? start : 0, IsColVector ? 0 : start,
              IsColVector ? size  : 1, IsColVector ? 1 : size)
@@ -82,8 +82,8 @@
 
     /** Fixed-size constructor
       */
-    EIGEN_DEVICE_FUNC
-    inline VectorBlock(VectorType& vector, Index start)
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    VectorBlock(VectorType& vector, Index start)
       : Base(vector, IsColVector ? start : 0, IsColVector ? 0 : start)
     {
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(VectorBlock);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
index 4fe267e..870f4f1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/VectorwiseOp.h
@@ -1,7 +1,7 @@
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
-// Copyright (C) 2008-2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2008-2019 Gael Guennebaud <gael.guennebaud@inria.fr>
 // Copyright (C) 2006-2008 Benoit Jacob <jacob.benoit.1@gmail.com>
 //
 // This Source Code Form is subject to the terms of the Mozilla
@@ -65,10 +65,10 @@
     explicit PartialReduxExpr(const MatrixType& mat, const MemberOp& func = MemberOp())
       : m_matrix(mat), m_functor(func) {}
 
-    EIGEN_DEVICE_FUNC
-    Index rows() const { return (Direction==Vertical   ? 1 : m_matrix.rows()); }
-    EIGEN_DEVICE_FUNC
-    Index cols() const { return (Direction==Horizontal ? 1 : m_matrix.cols()); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return (Direction==Vertical   ? 1 : m_matrix.rows()); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return (Direction==Horizontal ? 1 : m_matrix.cols()); }
 
     EIGEN_DEVICE_FUNC
     typename MatrixType::Nested nestedExpression() const { return m_matrix; }
@@ -81,39 +81,46 @@
     const MemberOp m_functor;
 };
 
-#define EIGEN_MEMBER_FUNCTOR(MEMBER,COST)                               \
-  template <typename ResultType>                                        \
-  struct member_##MEMBER {                                              \
-    EIGEN_EMPTY_STRUCT_CTOR(member_##MEMBER)                            \
-    typedef ResultType result_type;                                     \
-    template<typename Scalar, int Size> struct Cost                     \
-    { enum { value = COST }; };                                         \
-    template<typename XprType>                                          \
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                               \
-    ResultType operator()(const XprType& mat) const                     \
-    { return mat.MEMBER(); } \
+template<typename A,typename B> struct partial_redux_dummy_func;
+
+#define EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(MEMBER,COST,VECTORIZABLE,BINARYOP)                \
+  template <typename ResultType,typename Scalar>                                                            \
+  struct member_##MEMBER {                                                                  \
+    EIGEN_EMPTY_STRUCT_CTOR(member_##MEMBER)                                                \
+    typedef ResultType result_type;                                                         \
+    typedef BINARYOP<Scalar,Scalar> BinaryOp;   \
+    template<int Size> struct Cost { enum { value = COST }; };             \
+    enum { Vectorizable = VECTORIZABLE };                                                   \
+    template<typename XprType>                                                              \
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE                                                   \
+    ResultType operator()(const XprType& mat) const                                         \
+    { return mat.MEMBER(); }                                                                \
+    BinaryOp binaryFunc() const { return BinaryOp(); }                                      \
   }
 
+#define EIGEN_MEMBER_FUNCTOR(MEMBER,COST) \
+  EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(MEMBER,COST,0,partial_redux_dummy_func)
+
 namespace internal {
 
-EIGEN_MEMBER_FUNCTOR(squaredNorm, Size * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(norm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(stableNorm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(blueNorm, (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(hypotNorm, (Size-1) * functor_traits<scalar_hypot_op<Scalar> >::Cost );
-EIGEN_MEMBER_FUNCTOR(sum, (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(mean, (Size-1)*NumTraits<Scalar>::AddCost + NumTraits<Scalar>::MulCost);
-EIGEN_MEMBER_FUNCTOR(minCoeff, (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(maxCoeff, (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(all, (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(any, (Size-1)*NumTraits<Scalar>::AddCost);
 EIGEN_MEMBER_FUNCTOR(count, (Size-1)*NumTraits<Scalar>::AddCost);
-EIGEN_MEMBER_FUNCTOR(prod, (Size-1)*NumTraits<Scalar>::MulCost);
 
-template <int p, typename ResultType>
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(sum, (Size-1)*NumTraits<Scalar>::AddCost, 1, internal::scalar_sum_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(minCoeff, (Size-1)*NumTraits<Scalar>::AddCost, 1, internal::scalar_min_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(maxCoeff, (Size-1)*NumTraits<Scalar>::AddCost, 1, internal::scalar_max_op);
+EIGEN_MAKE_PARTIAL_REDUX_FUNCTOR(prod, (Size-1)*NumTraits<Scalar>::MulCost, 1, internal::scalar_product_op);
+
+template <int p, typename ResultType,typename Scalar>
 struct member_lpnorm {
   typedef ResultType result_type;
-  template<typename Scalar, int Size> struct Cost
+  enum { Vectorizable = 0 };
+  template<int Size> struct Cost
   { enum { value = (Size+5) * NumTraits<Scalar>::MulCost + (Size-1)*NumTraits<Scalar>::AddCost }; };
   EIGEN_DEVICE_FUNC member_lpnorm() {}
   template<typename XprType>
@@ -121,17 +128,20 @@
   { return mat.template lpNorm<p>(); }
 };
 
-template <typename BinaryOp, typename Scalar>
+template <typename BinaryOpT, typename Scalar>
 struct member_redux {
+  typedef BinaryOpT BinaryOp;
   typedef typename result_of<
                      BinaryOp(const Scalar&,const Scalar&)
                    >::type  result_type;
-  template<typename _Scalar, int Size> struct Cost
-  { enum { value = (Size-1) * functor_traits<BinaryOp>::Cost }; };
+
+  enum { Vectorizable = functor_traits<BinaryOp>::PacketAccess };
+  template<int Size> struct Cost { enum { value = (Size-1) * functor_traits<BinaryOp>::Cost }; };
   EIGEN_DEVICE_FUNC explicit member_redux(const BinaryOp func) : m_functor(func) {}
   template<typename Derived>
   EIGEN_DEVICE_FUNC inline result_type operator()(const DenseBase<Derived>& mat) const
   { return mat.redux(m_functor); }
+  const BinaryOp& binaryFunc() const { return m_functor; }
   const BinaryOp m_functor;
 };
 }
@@ -139,18 +149,38 @@
 /** \class VectorwiseOp
   * \ingroup Core_Module
   *
-  * \brief Pseudo expression providing partial reduction operations
+  * \brief Pseudo expression providing broadcasting and partial reduction operations
   *
   * \tparam ExpressionType the type of the object on which to do partial reductions
-  * \tparam Direction indicates the direction of the redux (#Vertical or #Horizontal)
+  * \tparam Direction indicates whether to operate on columns (#Vertical) or rows (#Horizontal)
   *
-  * This class represents a pseudo expression with partial reduction features.
+  * This class represents a pseudo expression with broadcasting and partial reduction features.
   * It is the return type of DenseBase::colwise() and DenseBase::rowwise()
-  * and most of the time this is the only way it is used.
+  * and most of the time this is the only way it is explicitly used.
+  *
+  * To understand the logic of rowwise/colwise expression, let's consider a generic case `A.colwise().foo()`
+  * where `foo` is any method of `VectorwiseOp`. This expression is equivalent to applying `foo()` to each
+  * column of `A` and then re-assemble the outputs in a matrix expression:
+  * \code [A.col(0).foo(), A.col(1).foo(), ..., A.col(A.cols()-1).foo()] \endcode
   *
   * Example: \include MatrixBase_colwise.cpp
   * Output: \verbinclude MatrixBase_colwise.out
   *
+  * The begin() and end() methods are obviously exceptions to the previous rule as they
+  * return STL-compatible begin/end iterators to the rows or columns of the nested expression.
+  * Typical use cases include for-range-loop and calls to STL algorithms:
+  *
+  * Example: \include MatrixBase_colwise_iterator_cxx11.cpp
+  * Output: \verbinclude MatrixBase_colwise_iterator_cxx11.out
+  *
+  * For a partial reduction on an empty input, some rules apply.
+  * For the sake of clarity, let's consider a vertical reduction:
+  *   - If the number of columns is zero, then a 1x0 row-major vector expression is returned.
+  *   - Otherwise, if the number of rows is zero, then
+  *       - a row vector of zeros is returned for sum-like reductions (sum, squaredNorm, norm, etc.)
+  *       - a row vector of ones is returned for a product reduction (e.g., <code>MatrixXd(n,0).colwise().prod()</code>)
+  *       - an assert is triggered for all other reductions (minCoeff,maxCoeff,redux(bin_op))
+  *
   * \sa DenseBase::colwise(), DenseBase::rowwise(), class PartialReduxExpr
   */
 template<typename ExpressionType, int Direction> class VectorwiseOp
@@ -163,11 +193,11 @@
     typedef typename internal::ref_selector<ExpressionType>::non_const_type ExpressionTypeNested;
     typedef typename internal::remove_all<ExpressionTypeNested>::type ExpressionTypeNestedCleaned;
 
-    template<template<typename _Scalar> class Functor,
-                      typename Scalar_=Scalar> struct ReturnType
+    template<template<typename OutScalar,typename InputScalar> class Functor,
+                      typename ReturnScalar=Scalar> struct ReturnType
     {
       typedef PartialReduxExpr<ExpressionType,
-                               Functor<Scalar_>,
+                               Functor<ReturnScalar,Scalar>,
                                Direction
                               > Type;
     };
@@ -187,23 +217,6 @@
 
   protected:
 
-    typedef typename internal::conditional<isVertical,
-                               typename ExpressionType::ColXpr,
-                               typename ExpressionType::RowXpr>::type SubVector;
-    /** \internal
-      * \returns the i-th subvector according to the \c Direction */
-    EIGEN_DEVICE_FUNC
-    SubVector subVector(Index i)
-    {
-      return SubVector(m_matrix.derived(),i);
-    }
-
-    /** \internal
-      * \returns the number of subvectors in the direction \c Direction */
-    EIGEN_DEVICE_FUNC
-    Index subVectors() const
-    { return isVertical?m_matrix.cols():m_matrix.rows(); }
-
     template<typename OtherDerived> struct ExtendedType {
       typedef Replicate<OtherDerived,
                         isVertical   ? 1 : ExpressionType::RowsAtCompileTime,
@@ -258,42 +271,101 @@
     EIGEN_DEVICE_FUNC
     inline const ExpressionType& _expression() const { return m_matrix; }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
+    /** STL-like <a href="https://en.cppreference.com/w/cpp/named_req/RandomAccessIterator">RandomAccessIterator</a>
+      * iterator type over the columns or rows as returned by the begin() and end() methods.
+      */
+    random_access_iterator_type iterator;
+    /** This is the const version of iterator (aka read-only) */
+    random_access_iterator_type const_iterator;
+    #else
+    typedef internal::subvector_stl_iterator<ExpressionType,               DirectionType(Direction)> iterator;
+    typedef internal::subvector_stl_iterator<const ExpressionType,         DirectionType(Direction)> const_iterator;
+    typedef internal::subvector_stl_reverse_iterator<ExpressionType,       DirectionType(Direction)> reverse_iterator;
+    typedef internal::subvector_stl_reverse_iterator<const ExpressionType, DirectionType(Direction)> const_reverse_iterator;
+    #endif
+
+    /** returns an iterator to the first row (rowwise) or column (colwise) of the nested expression.
+      * \sa end(), cbegin()
+      */
+    iterator                 begin()       { return iterator      (m_matrix, 0); }
+    /** const version of begin() */
+    const_iterator           begin() const { return const_iterator(m_matrix, 0); }
+    /** const version of begin() */
+    const_iterator          cbegin() const { return const_iterator(m_matrix, 0); }
+
+    /** returns a reverse iterator to the last row (rowwise) or column (colwise) of the nested expression.
+      * \sa rend(), crbegin()
+      */
+    reverse_iterator        rbegin()       { return reverse_iterator       (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()-1); }
+	/** const version of rbegin() */
+    const_reverse_iterator  rbegin() const { return const_reverse_iterator (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()-1); }
+	/** const version of rbegin() */
+	const_reverse_iterator crbegin() const { return const_reverse_iterator (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()-1); }
+
+    /** returns an iterator to the row (resp. column) following the last row (resp. column) of the nested expression
+      * \sa begin(), cend()
+      */
+    iterator                 end()         { return iterator      (m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
+    /** const version of end() */
+    const_iterator           end()  const  { return const_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
+    /** const version of end() */
+    const_iterator          cend()  const  { return const_iterator(m_matrix, m_matrix.template subVectors<DirectionType(Direction)>()); }
+
+    /** returns a reverse iterator to the row (resp. column) before the first row (resp. column) of the nested expression
+      * \sa begin(), cend()
+      */
+    reverse_iterator        rend()         { return reverse_iterator       (m_matrix, -1); }
+    /** const version of rend() */
+    const_reverse_iterator  rend()  const  { return const_reverse_iterator (m_matrix, -1); }
+    /** const version of rend() */
+    const_reverse_iterator crend()  const  { return const_reverse_iterator (m_matrix, -1); }
+
     /** \returns a row or column vector expression of \c *this reduxed by \a func
       *
       * The template parameter \a BinaryOp is the type of the functor
       * of the custom redux operator. Note that func must be an associative operator.
       *
+      * \warning the size along the reduction direction must be strictly positive,
+      *          otherwise an assertion is triggered.
+      *
       * \sa class VectorwiseOp, DenseBase::colwise(), DenseBase::rowwise()
       */
     template<typename BinaryOp>
     EIGEN_DEVICE_FUNC
     const typename ReduxReturnType<BinaryOp>::Type
     redux(const BinaryOp& func = BinaryOp()) const
-    { return typename ReduxReturnType<BinaryOp>::Type(_expression(), internal::member_redux<BinaryOp,Scalar>(func)); }
+    {
+      eigen_assert(redux_length()>0 && "you are using an empty matrix");
+      return typename ReduxReturnType<BinaryOp>::Type(_expression(), internal::member_redux<BinaryOp,Scalar>(func));
+    }
 
     typedef typename ReturnType<internal::member_minCoeff>::Type MinCoeffReturnType;
     typedef typename ReturnType<internal::member_maxCoeff>::Type MaxCoeffReturnType;
-    typedef typename ReturnType<internal::member_squaredNorm,RealScalar>::Type SquaredNormReturnType;
-    typedef typename ReturnType<internal::member_norm,RealScalar>::Type NormReturnType;
+    typedef PartialReduxExpr<const CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const ExpressionTypeNestedCleaned>,internal::member_sum<RealScalar,RealScalar>,Direction> SquaredNormReturnType;
+    typedef CwiseUnaryOp<internal::scalar_sqrt_op<RealScalar>, const SquaredNormReturnType> NormReturnType;
     typedef typename ReturnType<internal::member_blueNorm,RealScalar>::Type BlueNormReturnType;
     typedef typename ReturnType<internal::member_stableNorm,RealScalar>::Type StableNormReturnType;
     typedef typename ReturnType<internal::member_hypotNorm,RealScalar>::Type HypotNormReturnType;
     typedef typename ReturnType<internal::member_sum>::Type SumReturnType;
-    typedef typename ReturnType<internal::member_mean>::Type MeanReturnType;
+    typedef EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(SumReturnType,Scalar,quotient) MeanReturnType;
     typedef typename ReturnType<internal::member_all>::Type AllReturnType;
     typedef typename ReturnType<internal::member_any>::Type AnyReturnType;
-    typedef PartialReduxExpr<ExpressionType, internal::member_count<Index>, Direction> CountReturnType;
+    typedef PartialReduxExpr<ExpressionType, internal::member_count<Index,Scalar>, Direction> CountReturnType;
     typedef typename ReturnType<internal::member_prod>::Type ProdReturnType;
     typedef Reverse<const ExpressionType, Direction> ConstReverseReturnType;
     typedef Reverse<ExpressionType, Direction> ReverseReturnType;
 
     template<int p> struct LpNormReturnType {
-      typedef PartialReduxExpr<ExpressionType, internal::member_lpnorm<p,RealScalar>,Direction> Type;
+      typedef PartialReduxExpr<ExpressionType, internal::member_lpnorm<p,RealScalar,Scalar>,Direction> Type;
     };
 
     /** \returns a row (or column) vector expression of the smallest coefficient
       * of each column (or row) of the referenced expression.
       *
+      * \warning the size along the reduction direction must be strictly positive,
+      *          otherwise an assertion is triggered.
+      *
       * \warning the result is undefined if \c *this contains NaN.
       *
       * Example: \include PartialRedux_minCoeff.cpp
@@ -302,11 +374,17 @@
       * \sa DenseBase::minCoeff() */
     EIGEN_DEVICE_FUNC
     const MinCoeffReturnType minCoeff() const
-    { return MinCoeffReturnType(_expression()); }
+    {
+      eigen_assert(redux_length()>0 && "you are using an empty matrix");
+      return MinCoeffReturnType(_expression());
+    }
 
     /** \returns a row (or column) vector expression of the largest coefficient
       * of each column (or row) of the referenced expression.
       *
+      * \warning the size along the reduction direction must be strictly positive,
+      *          otherwise an assertion is triggered.
+      *
       * \warning the result is undefined if \c *this contains NaN.
       *
       * Example: \include PartialRedux_maxCoeff.cpp
@@ -315,7 +393,10 @@
       * \sa DenseBase::maxCoeff() */
     EIGEN_DEVICE_FUNC
     const MaxCoeffReturnType maxCoeff() const
-    { return MaxCoeffReturnType(_expression()); }
+    {
+      eigen_assert(redux_length()>0 && "you are using an empty matrix");
+      return MaxCoeffReturnType(_expression());
+    }
 
     /** \returns a row (or column) vector expression of the squared norm
       * of each column (or row) of the referenced expression.
@@ -327,7 +408,7 @@
       * \sa DenseBase::squaredNorm() */
     EIGEN_DEVICE_FUNC
     const SquaredNormReturnType squaredNorm() const
-    { return SquaredNormReturnType(_expression()); }
+    { return SquaredNormReturnType(m_matrix.cwiseAbs2()); }
 
     /** \returns a row (or column) vector expression of the norm
       * of each column (or row) of the referenced expression.
@@ -339,7 +420,7 @@
       * \sa DenseBase::norm() */
     EIGEN_DEVICE_FUNC
     const NormReturnType norm() const
-    { return NormReturnType(_expression()); }
+    { return NormReturnType(squaredNorm()); }
 
     /** \returns a row (or column) vector expression of the norm
       * of each column (or row) of the referenced expression.
@@ -404,7 +485,7 @@
     * \sa DenseBase::mean() */
     EIGEN_DEVICE_FUNC
     const MeanReturnType mean() const
-    { return MeanReturnType(_expression()); }
+    { return sum() / Scalar(Direction==Vertical?m_matrix.rows():m_matrix.cols()); }
 
     /** \returns a row (or column) vector expression representing
       * whether \b all coefficients of each respective column (or row) are \c true.
@@ -500,7 +581,7 @@
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
       EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
       //eigen_assert((m_matrix.isNull()) == (other.isNull())); FIXME
-      return const_cast<ExpressionType&>(m_matrix = extendedTo(other.derived()));
+      return m_matrix = extendedTo(other.derived());
     }
 
     /** Adds the vector \a other to each subvector of \c *this */
@@ -510,7 +591,7 @@
     {
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
       EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return const_cast<ExpressionType&>(m_matrix += extendedTo(other.derived()));
+      return m_matrix += extendedTo(other.derived());
     }
 
     /** Substracts the vector \a other to each subvector of \c *this */
@@ -520,7 +601,7 @@
     {
       EIGEN_STATIC_ASSERT_VECTOR_ONLY(OtherDerived)
       EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
-      return const_cast<ExpressionType&>(m_matrix -= extendedTo(other.derived()));
+      return m_matrix -= extendedTo(other.derived());
     }
 
     /** Multiples each subvector of \c *this by the vector \a other */
@@ -532,7 +613,7 @@
       EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
       EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
       m_matrix *= extendedTo(other.derived());
-      return const_cast<ExpressionType&>(m_matrix);
+      return m_matrix;
     }
 
     /** Divides each subvector of \c *this by the vector \a other */
@@ -544,7 +625,7 @@
       EIGEN_STATIC_ASSERT_ARRAYXPR(ExpressionType)
       EIGEN_STATIC_ASSERT_SAME_XPR_KIND(ExpressionType, OtherDerived)
       m_matrix /= extendedTo(other.derived());
-      return const_cast<ExpressionType&>(m_matrix);
+      return m_matrix;
     }
 
     /** Returns the expression of the sum of the vector \a other to each subvector of \c *this */
@@ -609,7 +690,7 @@
     EIGEN_DEVICE_FUNC
     CwiseBinaryOp<internal::scalar_quotient_op<Scalar>,
                   const ExpressionTypeNestedCleaned,
-                  const typename OppositeExtendedType<typename ReturnType<internal::member_norm,RealScalar>::Type>::Type>
+                  const typename OppositeExtendedType<NormReturnType>::Type>
     normalized() const { return m_matrix.cwiseQuotient(extendedToOpposite(this->norm())); }
 
 
@@ -658,7 +739,15 @@
     EIGEN_DEVICE_FUNC
     const HNormalizedReturnType hnormalized() const;
 
+#   ifdef EIGEN_VECTORWISEOP_PLUGIN
+#     include EIGEN_VECTORWISEOP_PLUGIN
+#   endif
+
   protected:
+    Index redux_length() const
+    {
+      return Direction==Vertical ? m_matrix.rows() : m_matrix.cols();
+    }
     ExpressionTypeNested m_matrix;
 };
 
@@ -670,7 +759,7 @@
   * \sa rowwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
   */
 template<typename Derived>
-inline typename DenseBase<Derived>::ColwiseReturnType
+EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::ColwiseReturnType
 DenseBase<Derived>::colwise()
 {
   return ColwiseReturnType(derived());
@@ -684,7 +773,7 @@
   * \sa colwise(), class VectorwiseOp, \ref TutorialReductionsVisitorsBroadcasting
   */
 template<typename Derived>
-inline typename DenseBase<Derived>::RowwiseReturnType
+EIGEN_DEVICE_FUNC inline typename DenseBase<Derived>::RowwiseReturnType
 DenseBase<Derived>::rowwise()
 {
   return RowwiseReturnType(derived());
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
index 54c1883..00bcca8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/Visitor.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_VISITOR_H
 #define EIGEN_VISITOR_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
 
@@ -40,6 +40,14 @@
   }
 };
 
+// This specialization enables visitors on empty matrices at compile-time
+template<typename Visitor, typename Derived>
+struct visitor_impl<Visitor, Derived, 0> {
+  EIGEN_DEVICE_FUNC
+  static inline void run(const Derived &/*mat*/, Visitor& /*visitor*/)
+  {}
+};
+
 template<typename Visitor, typename Derived>
 struct visitor_impl<Visitor, Derived, Dynamic>
 {
@@ -62,22 +70,22 @@
 public:
   EIGEN_DEVICE_FUNC
   explicit visitor_evaluator(const XprType &xpr) : m_evaluator(xpr), m_xpr(xpr) {}
-  
+
   typedef typename XprType::Scalar Scalar;
   typedef typename XprType::CoeffReturnType CoeffReturnType;
-  
+
   enum {
     RowsAtCompileTime = XprType::RowsAtCompileTime,
     CoeffReadCost = internal::evaluator<XprType>::CoeffReadCost
   };
-  
-  EIGEN_DEVICE_FUNC Index rows() const { return m_xpr.rows(); }
-  EIGEN_DEVICE_FUNC Index cols() const { return m_xpr.cols(); }
-  EIGEN_DEVICE_FUNC Index size() const { return m_xpr.size(); }
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_xpr.rows(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_xpr.cols(); }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_xpr.size(); }
 
   EIGEN_DEVICE_FUNC CoeffReturnType coeff(Index row, Index col) const
   { return m_evaluator.coeff(row, col); }
-  
+
 protected:
   internal::evaluator<XprType> m_evaluator;
   const XprType &m_xpr;
@@ -99,6 +107,8 @@
   * \note compared to one or two \em for \em loops, visitors offer automatic
   * unrolling for small fixed size matrix.
   *
+  * \note if the matrix is empty, then the visitor is left unchanged.
+  *
   * \sa minCoeff(Index*,Index*), maxCoeff(Index*,Index*), DenseBase::redux()
   */
 template<typename Derived>
@@ -106,12 +116,15 @@
 EIGEN_DEVICE_FUNC
 void DenseBase<Derived>::visit(Visitor& visitor) const
 {
+  if(size()==0)
+    return;
+
   typedef typename internal::visitor_evaluator<Derived> ThisEvaluator;
   ThisEvaluator thisEval(derived());
-  
+
   enum {
     unroll =  SizeAtCompileTime != Dynamic
-           && SizeAtCompileTime * ThisEvaluator::CoeffReadCost + (SizeAtCompileTime-1) * internal::functor_traits<Visitor>::Cost <= EIGEN_UNROLLING_LIMIT
+           && SizeAtCompileTime * int(ThisEvaluator::CoeffReadCost) + (SizeAtCompileTime-1) * int(internal::functor_traits<Visitor>::Cost) <= EIGEN_UNROLLING_LIMIT
   };
   return internal::visitor_impl<Visitor, ThisEvaluator, unroll ? int(SizeAtCompileTime) : Dynamic>::run(thisEval, visitor);
 }
@@ -124,6 +137,9 @@
 template <typename Derived>
 struct coeff_visitor
 {
+  // default initialization to avoid countless invalid maybe-uninitialized warnings by gcc
+  EIGEN_DEVICE_FUNC
+  coeff_visitor() : row(-1), col(-1), res(0) {}
   typedef typename Derived::Scalar Scalar;
   Index row, col;
   Scalar res;
@@ -141,7 +157,7 @@
   *
   * \sa DenseBase::minCoeff(Index*, Index*)
   */
-template <typename Derived>
+template <typename Derived, int NaNPropagation>
 struct min_coeff_visitor : coeff_visitor<Derived>
 {
   typedef typename Derived::Scalar Scalar;
@@ -157,8 +173,40 @@
   }
 };
 
-template<typename Scalar>
-struct functor_traits<min_coeff_visitor<Scalar> > {
+template <typename Derived>
+struct min_coeff_visitor<Derived, PropagateNumbers> : coeff_visitor<Derived>
+{
+  typedef typename Derived::Scalar Scalar;
+  EIGEN_DEVICE_FUNC
+  void operator() (const Scalar& value, Index i, Index j)
+  {
+    if((numext::isnan)(this->res) || (!(numext::isnan)(value) && value < this->res))
+    {
+      this->res = value;
+      this->row = i;
+      this->col = j;
+    }
+  }
+};
+
+template <typename Derived>
+struct min_coeff_visitor<Derived, PropagateNaN> : coeff_visitor<Derived>
+{
+  typedef typename Derived::Scalar Scalar;
+  EIGEN_DEVICE_FUNC
+  void operator() (const Scalar& value, Index i, Index j)
+  {
+    if((numext::isnan)(value) || value < this->res)
+    {
+      this->res = value;
+      this->row = i;
+      this->col = j;
+    }
+  }
+};
+
+template<typename Scalar, int NaNPropagation>
+    struct functor_traits<min_coeff_visitor<Scalar, NaNPropagation> > {
   enum {
     Cost = NumTraits<Scalar>::AddCost
   };
@@ -169,10 +217,10 @@
   *
   * \sa DenseBase::maxCoeff(Index*, Index*)
   */
-template <typename Derived>
+template <typename Derived, int NaNPropagation>
 struct max_coeff_visitor : coeff_visitor<Derived>
 {
-  typedef typename Derived::Scalar Scalar; 
+  typedef typename Derived::Scalar Scalar;
   EIGEN_DEVICE_FUNC
   void operator() (const Scalar& value, Index i, Index j)
   {
@@ -185,8 +233,40 @@
   }
 };
 
-template<typename Scalar>
-struct functor_traits<max_coeff_visitor<Scalar> > {
+template <typename Derived>
+struct max_coeff_visitor<Derived, PropagateNumbers> : coeff_visitor<Derived>
+{
+  typedef typename Derived::Scalar Scalar;
+  EIGEN_DEVICE_FUNC
+  void operator() (const Scalar& value, Index i, Index j)
+  {
+    if((numext::isnan)(this->res) || (!(numext::isnan)(value) && value > this->res))
+    {
+      this->res = value;
+      this->row = i;
+      this->col = j;
+    }
+  }
+};
+
+template <typename Derived>
+struct max_coeff_visitor<Derived, PropagateNaN> : coeff_visitor<Derived>
+{
+  typedef typename Derived::Scalar Scalar;
+  EIGEN_DEVICE_FUNC
+  void operator() (const Scalar& value, Index i, Index j)
+  {
+    if((numext::isnan)(value) || value > this->res)
+    {
+      this->res = value;
+      this->row = i;
+      this->col = j;
+    }
+  }
+};
+
+template<typename Scalar, int NaNPropagation>
+struct functor_traits<max_coeff_visitor<Scalar, NaNPropagation> > {
   enum {
     Cost = NumTraits<Scalar>::AddCost
   };
@@ -196,17 +276,24 @@
 
 /** \fn DenseBase<Derived>::minCoeff(IndexType* rowId, IndexType* colId) const
   * \returns the minimum of all coefficients of *this and puts in *row and *col its location.
-  * \warning the result is undefined if \c *this contains NaN.
+  *
+  * In case \c *this contains NaN, NaNPropagation determines the behavior:
+  *   NaNPropagation == PropagateFast : undefined
+  *   NaNPropagation == PropagateNaN : result is NaN
+  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
   *
   * \sa DenseBase::minCoeff(Index*), DenseBase::maxCoeff(Index*,Index*), DenseBase::visit(), DenseBase::minCoeff()
   */
 template<typename Derived>
-template<typename IndexType>
+template<int NaNPropagation, typename IndexType>
 EIGEN_DEVICE_FUNC
 typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::minCoeff(IndexType* rowId, IndexType* colId) const
 {
-  internal::min_coeff_visitor<Derived> minVisitor;
+  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+
+  internal::min_coeff_visitor<Derived, NaNPropagation> minVisitor;
   this->visit(minVisitor);
   *rowId = minVisitor.row;
   if (colId) *colId = minVisitor.col;
@@ -214,18 +301,25 @@
 }
 
 /** \returns the minimum of all coefficients of *this and puts in *index its location.
-  * \warning the result is undefined if \c *this contains NaN. 
+  *
+  * In case \c *this contains NaN, NaNPropagation determines the behavior:
+  *   NaNPropagation == PropagateFast : undefined
+  *   NaNPropagation == PropagateNaN : result is NaN
+  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
   *
   * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::visit(), DenseBase::minCoeff()
   */
 template<typename Derived>
-template<typename IndexType>
+template<int NaNPropagation, typename IndexType>
 EIGEN_DEVICE_FUNC
 typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::minCoeff(IndexType* index) const
 {
+  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  internal::min_coeff_visitor<Derived> minVisitor;
+      internal::min_coeff_visitor<Derived, NaNPropagation> minVisitor;
   this->visit(minVisitor);
   *index = IndexType((RowsAtCompileTime==1) ? minVisitor.col : minVisitor.row);
   return minVisitor.res;
@@ -233,17 +327,24 @@
 
 /** \fn DenseBase<Derived>::maxCoeff(IndexType* rowId, IndexType* colId) const
   * \returns the maximum of all coefficients of *this and puts in *row and *col its location.
-  * \warning the result is undefined if \c *this contains NaN. 
+  *
+  * In case \c *this contains NaN, NaNPropagation determines the behavior:
+  *   NaNPropagation == PropagateFast : undefined
+  *   NaNPropagation == PropagateNaN : result is NaN
+  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
   *
   * \sa DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visit(), DenseBase::maxCoeff()
   */
 template<typename Derived>
-template<typename IndexType>
+template<int NaNPropagation, typename IndexType>
 EIGEN_DEVICE_FUNC
 typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::maxCoeff(IndexType* rowPtr, IndexType* colPtr) const
 {
-  internal::max_coeff_visitor<Derived> maxVisitor;
+  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+
+  internal::max_coeff_visitor<Derived, NaNPropagation> maxVisitor;
   this->visit(maxVisitor);
   *rowPtr = maxVisitor.row;
   if (colPtr) *colPtr = maxVisitor.col;
@@ -251,18 +352,25 @@
 }
 
 /** \returns the maximum of all coefficients of *this and puts in *index its location.
-  * \warning the result is undefined if \c *this contains NaN.
+  *
+  * In case \c *this contains NaN, NaNPropagation determines the behavior:
+  *   NaNPropagation == PropagateFast : undefined
+  *   NaNPropagation == PropagateNaN : result is NaN
+  *   NaNPropagation == PropagateNumbers : result is maximum of elements that are not NaN
+  * \warning the matrix must be not empty, otherwise an assertion is triggered.
   *
   * \sa DenseBase::maxCoeff(IndexType*,IndexType*), DenseBase::minCoeff(IndexType*,IndexType*), DenseBase::visitor(), DenseBase::maxCoeff()
   */
 template<typename Derived>
-template<typename IndexType>
+template<int NaNPropagation, typename IndexType>
 EIGEN_DEVICE_FUNC
 typename internal::traits<Derived>::Scalar
 DenseBase<Derived>::maxCoeff(IndexType* index) const
 {
+  eigen_assert(this->rows()>0 && this->cols()>0 && "you are using an empty matrix");
+
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  internal::max_coeff_visitor<Derived> maxVisitor;
+      internal::max_coeff_visitor<Derived, NaNPropagation> maxVisitor;
   this->visit(maxVisitor);
   *index = (RowsAtCompileTime==1) ? maxVisitor.col : maxVisitor.row;
   return maxVisitor.res;
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
index 7fa6196..ab7bd6c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/Complex.h
@@ -22,6 +22,7 @@
   __m256  v;
 };
 
+#ifndef EIGEN_VECTORIZE_AVX512
 template<> struct packet_traits<std::complex<float> >  : default_packet_traits
 {
   typedef Packet4cf type;
@@ -37,6 +38,7 @@
     HasMul    = 1,
     HasDiv    = 1,
     HasNegate = 1,
+    HasSqrt   = 1,
     HasAbs    = 0,
     HasAbs2   = 0,
     HasMin    = 0,
@@ -44,8 +46,20 @@
     HasSetLinear = 0
   };
 };
+#endif
 
-template<> struct unpacket_traits<Packet4cf> { typedef std::complex<float> type; enum {size=4, alignment=Aligned32}; typedef Packet2cf half; };
+template<> struct unpacket_traits<Packet4cf> {
+  typedef std::complex<float> type;
+  typedef Packet2cf half;
+  typedef Packet8f as_real;
+  enum {
+    size=4,
+    alignment=Aligned32,
+    vectorizable=true,
+    masked_load_available=false,
+    masked_store_available=false
+  };
+};
 
 template<> EIGEN_STRONG_INLINE Packet4cf padd<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_add_ps(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet4cf psub<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_sub_ps(a.v,b.v)); }
@@ -67,10 +81,17 @@
   return Packet4cf(result);
 }
 
+template <>
+EIGEN_STRONG_INLINE Packet4cf pcmp_eq(const Packet4cf& a, const Packet4cf& b) {
+  __m256 eq = _mm256_cmp_ps(a.v, b.v, _CMP_EQ_OQ);
+  return Packet4cf(_mm256_and_ps(eq, _mm256_permute_ps(eq, 0xb1)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4cf ptrue<Packet4cf>(const Packet4cf& a) { return Packet4cf(ptrue(Packet8f(a.v))); }
 template<> EIGEN_STRONG_INLINE Packet4cf pand   <Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_and_ps(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet4cf por    <Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_or_ps(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet4cf pxor   <Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_xor_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet4cf pandnot<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_andnot_ps(a.v,b.v)); }
+template<> EIGEN_STRONG_INLINE Packet4cf pandnot<Packet4cf>(const Packet4cf& a, const Packet4cf& b) { return Packet4cf(_mm256_andnot_ps(b.v,a.v)); }
 
 template<> EIGEN_STRONG_INLINE Packet4cf pload <Packet4cf>(const std::complex<float>* from) { EIGEN_DEBUG_ALIGNED_LOAD return Packet4cf(pload<Packet8f>(&numext::real_ref(*from))); }
 template<> EIGEN_STRONG_INLINE Packet4cf ploadu<Packet4cf>(const std::complex<float>* from) { EIGEN_DEBUG_UNALIGNED_LOAD return Packet4cf(ploadu<Packet8f>(&numext::real_ref(*from))); }
@@ -140,70 +161,12 @@
                      Packet2cf(_mm256_extractf128_ps(a.v,1))));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf preduxp<Packet4cf>(const Packet4cf* vecs)
-{
-  Packet8f t0 = _mm256_shuffle_ps(vecs[0].v, vecs[0].v, _MM_SHUFFLE(3, 1, 2 ,0));
-  Packet8f t1 = _mm256_shuffle_ps(vecs[1].v, vecs[1].v, _MM_SHUFFLE(3, 1, 2 ,0));
-  t0 = _mm256_hadd_ps(t0,t1);
-  Packet8f t2 = _mm256_shuffle_ps(vecs[2].v, vecs[2].v, _MM_SHUFFLE(3, 1, 2 ,0));
-  Packet8f t3 = _mm256_shuffle_ps(vecs[3].v, vecs[3].v, _MM_SHUFFLE(3, 1, 2 ,0));
-  t2 = _mm256_hadd_ps(t2,t3);
-  
-  t1 = _mm256_permute2f128_ps(t0,t2, 0 + (2<<4));
-  t3 = _mm256_permute2f128_ps(t0,t2, 1 + (3<<4));
-
-  return Packet4cf(_mm256_add_ps(t1,t3));
-}
-
 template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet4cf>(const Packet4cf& a)
 {
   return predux_mul(pmul(Packet2cf(_mm256_extractf128_ps(a.v, 0)),
                          Packet2cf(_mm256_extractf128_ps(a.v, 1))));
 }
 
-template<int Offset>
-struct palign_impl<Offset,Packet4cf>
-{
-  static EIGEN_STRONG_INLINE void run(Packet4cf& first, const Packet4cf& second)
-  {
-    if (Offset==0) return;
-    palign_impl<Offset*2,Packet8f>::run(first.v, second.v);
-  }
-};
-
-template<> struct conj_helper<Packet4cf, Packet4cf, false,true>
-{
-  EIGEN_STRONG_INLINE Packet4cf pmadd(const Packet4cf& x, const Packet4cf& y, const Packet4cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet4cf pmul(const Packet4cf& a, const Packet4cf& b) const
-  {
-    return internal::pmul(a, pconj(b));
-  }
-};
-
-template<> struct conj_helper<Packet4cf, Packet4cf, true,false>
-{
-  EIGEN_STRONG_INLINE Packet4cf pmadd(const Packet4cf& x, const Packet4cf& y, const Packet4cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet4cf pmul(const Packet4cf& a, const Packet4cf& b) const
-  {
-    return internal::pmul(pconj(a), b);
-  }
-};
-
-template<> struct conj_helper<Packet4cf, Packet4cf, true,true>
-{
-  EIGEN_STRONG_INLINE Packet4cf pmadd(const Packet4cf& x, const Packet4cf& y, const Packet4cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet4cf pmul(const Packet4cf& a, const Packet4cf& b) const
-  {
-    return pconj(internal::pmul(a, b));
-  }
-};
-
 EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet4cf,Packet8f)
 
 template<> EIGEN_STRONG_INLINE Packet4cf pdiv<Packet4cf>(const Packet4cf& a, const Packet4cf& b)
@@ -228,6 +191,7 @@
   __m256d  v;
 };
 
+#ifndef EIGEN_VECTORIZE_AVX512
 template<> struct packet_traits<std::complex<double> >  : default_packet_traits
 {
   typedef Packet2cd type;
@@ -243,6 +207,7 @@
     HasMul    = 1,
     HasDiv    = 1,
     HasNegate = 1,
+    HasSqrt   = 1,
     HasAbs    = 0,
     HasAbs2   = 0,
     HasMin    = 0,
@@ -250,8 +215,20 @@
     HasSetLinear = 0
   };
 };
+#endif
 
-template<> struct unpacket_traits<Packet2cd> { typedef std::complex<double> type; enum {size=2, alignment=Aligned32}; typedef Packet1cd half; };
+template<> struct unpacket_traits<Packet2cd> {
+  typedef std::complex<double> type;
+  typedef Packet1cd half;
+  typedef Packet4d as_real;
+  enum {
+    size=2,
+    alignment=Aligned32,
+    vectorizable=true,
+    masked_load_available=false,
+    masked_store_available=false
+  };
+};
 
 template<> EIGEN_STRONG_INLINE Packet2cd padd<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_add_pd(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cd psub<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_sub_pd(a.v,b.v)); }
@@ -272,10 +249,17 @@
   return Packet2cd(_mm256_addsub_pd(even, odd));
 }
 
+template <>
+EIGEN_STRONG_INLINE Packet2cd pcmp_eq(const Packet2cd& a, const Packet2cd& b) {
+  __m256d eq = _mm256_cmp_pd(a.v, b.v, _CMP_EQ_OQ);
+  return Packet2cd(pand(eq, _mm256_permute_pd(eq, 0x5)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet2cd ptrue<Packet2cd>(const Packet2cd& a) { return Packet2cd(ptrue(Packet4d(a.v))); }
 template<> EIGEN_STRONG_INLINE Packet2cd pand   <Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_and_pd(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cd por    <Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_or_pd(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cd pxor   <Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_xor_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cd pandnot<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_andnot_pd(a.v,b.v)); }
+template<> EIGEN_STRONG_INLINE Packet2cd pandnot<Packet2cd>(const Packet2cd& a, const Packet2cd& b) { return Packet2cd(_mm256_andnot_pd(b.v,a.v)); }
 
 template<> EIGEN_STRONG_INLINE Packet2cd pload <Packet2cd>(const std::complex<double>* from)
 { EIGEN_DEBUG_ALIGNED_LOAD return Packet2cd(pload<Packet4d>((const double*)from)); }
@@ -327,63 +311,12 @@
                      Packet1cd(_mm256_extractf128_pd(a.v,1))));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd preduxp<Packet2cd>(const Packet2cd* vecs)
-{
-  Packet4d t0 = _mm256_permute2f128_pd(vecs[0].v,vecs[1].v, 0 + (2<<4));
-  Packet4d t1 = _mm256_permute2f128_pd(vecs[0].v,vecs[1].v, 1 + (3<<4));
-
-  return Packet2cd(_mm256_add_pd(t0,t1));
-}
-
 template<> EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet2cd>(const Packet2cd& a)
 {
   return predux(pmul(Packet1cd(_mm256_extractf128_pd(a.v,0)),
                      Packet1cd(_mm256_extractf128_pd(a.v,1))));
 }
 
-template<int Offset>
-struct palign_impl<Offset,Packet2cd>
-{
-  static EIGEN_STRONG_INLINE void run(Packet2cd& first, const Packet2cd& second)
-  {
-    if (Offset==0) return;
-    palign_impl<Offset*2,Packet4d>::run(first.v, second.v);
-  }
-};
-
-template<> struct conj_helper<Packet2cd, Packet2cd, false,true>
-{
-  EIGEN_STRONG_INLINE Packet2cd pmadd(const Packet2cd& x, const Packet2cd& y, const Packet2cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cd pmul(const Packet2cd& a, const Packet2cd& b) const
-  {
-    return internal::pmul(a, pconj(b));
-  }
-};
-
-template<> struct conj_helper<Packet2cd, Packet2cd, true,false>
-{
-  EIGEN_STRONG_INLINE Packet2cd pmadd(const Packet2cd& x, const Packet2cd& y, const Packet2cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cd pmul(const Packet2cd& a, const Packet2cd& b) const
-  {
-    return internal::pmul(pconj(a), b);
-  }
-};
-
-template<> struct conj_helper<Packet2cd, Packet2cd, true,true>
-{
-  EIGEN_STRONG_INLINE Packet2cd pmadd(const Packet2cd& x, const Packet2cd& y, const Packet2cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cd pmul(const Packet2cd& a, const Packet2cd& b) const
-  {
-    return pconj(internal::pmul(a, b));
-  }
-};
-
 EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cd,Packet4d)
 
 template<> EIGEN_STRONG_INLINE Packet2cd pdiv<Packet2cd>(const Packet2cd& a, const Packet2cd& b)
@@ -424,24 +357,12 @@
  kernel.packet[0].v = tmp;
 }
 
-template<> EIGEN_STRONG_INLINE Packet4cf pinsertfirst(const Packet4cf& a, std::complex<float> b)
-{
-  return Packet4cf(_mm256_blend_ps(a.v,pset1<Packet4cf>(b).v,1|2));
+template<> EIGEN_STRONG_INLINE Packet2cd psqrt<Packet2cd>(const Packet2cd& a) {
+  return psqrt_complex<Packet2cd>(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cd pinsertfirst(const Packet2cd& a, std::complex<double> b)
-{
-  return Packet2cd(_mm256_blend_pd(a.v,pset1<Packet2cd>(b).v,1|2));
-}
-
-template<> EIGEN_STRONG_INLINE Packet4cf pinsertlast(const Packet4cf& a, std::complex<float> b)
-{
-  return Packet4cf(_mm256_blend_ps(a.v,pset1<Packet4cf>(b).v,(1<<7)|(1<<6)));
-}
-
-template<> EIGEN_STRONG_INLINE Packet2cd pinsertlast(const Packet2cd& a, std::complex<double> b)
-{
-  return Packet2cd(_mm256_blend_pd(a.v,pset1<Packet2cd>(b).v,(1<<3)|(1<<2)));
+template<> EIGEN_STRONG_INLINE Packet4cf psqrt<Packet4cf>(const Packet4cf& a) {
+  return psqrt_complex<Packet4cf>(a);
 }
 
 } // end namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
index 6af67ce..67041c8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/MathFunctions.h
@@ -10,7 +10,7 @@
 #ifndef EIGEN_MATH_FUNCTIONS_AVX_H
 #define EIGEN_MATH_FUNCTIONS_AVX_H
 
-/* The sin, cos, exp, and log functions of this file are loosely derived from
+/* The sin and cos functions of this file are loosely derived from
  * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
  */
 
@@ -18,187 +18,50 @@
 
 namespace internal {
 
-inline Packet8i pshiftleft(Packet8i v, int n)
-{
-#ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_slli_epi32(v, n);
-#else
-  __m128i lo = _mm_slli_epi32(_mm256_extractf128_si256(v, 0), n);
-  __m128i hi = _mm_slli_epi32(_mm256_extractf128_si256(v, 1), n);
-  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
-#endif
-}
-
-inline Packet8f pshiftright(Packet8f v, int n)
-{
-#ifdef EIGEN_VECTORIZE_AVX2
-  return _mm256_cvtepi32_ps(_mm256_srli_epi32(_mm256_castps_si256(v), n));
-#else
-  __m128i lo = _mm_srli_epi32(_mm256_extractf128_si256(_mm256_castps_si256(v), 0), n);
-  __m128i hi = _mm_srli_epi32(_mm256_extractf128_si256(_mm256_castps_si256(v), 1), n);
-  return _mm256_cvtepi32_ps(_mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1));
-#endif
-}
-
-// Sine function
-// Computes sin(x) by wrapping x to the interval [-Pi/4,3*Pi/4] and
-// evaluating interpolants in [-Pi/4,Pi/4] or [Pi/4,3*Pi/4]. The interpolants
-// are (anti-)symmetric and thus have only odd/even coefficients
 template <>
 EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
 psin<Packet8f>(const Packet8f& _x) {
-  Packet8f x = _x;
-
-  // Some useful values.
-  _EIGEN_DECLARE_CONST_Packet8i(one, 1);
-  _EIGEN_DECLARE_CONST_Packet8f(one, 1.0f);
-  _EIGEN_DECLARE_CONST_Packet8f(two, 2.0f);
-  _EIGEN_DECLARE_CONST_Packet8f(one_over_four, 0.25f);
-  _EIGEN_DECLARE_CONST_Packet8f(one_over_pi, 3.183098861837907e-01f);
-  _EIGEN_DECLARE_CONST_Packet8f(neg_pi_first, -3.140625000000000e+00f);
-  _EIGEN_DECLARE_CONST_Packet8f(neg_pi_second, -9.670257568359375e-04f);
-  _EIGEN_DECLARE_CONST_Packet8f(neg_pi_third, -6.278329571784980e-07f);
-  _EIGEN_DECLARE_CONST_Packet8f(four_over_pi, 1.273239544735163e+00f);
-
-  // Map x from [-Pi/4,3*Pi/4] to z in [-1,3] and subtract the shifted period.
-  Packet8f z = pmul(x, p8f_one_over_pi);
-  Packet8f shift = _mm256_floor_ps(padd(z, p8f_one_over_four));
-  x = pmadd(shift, p8f_neg_pi_first, x);
-  x = pmadd(shift, p8f_neg_pi_second, x);
-  x = pmadd(shift, p8f_neg_pi_third, x);
-  z = pmul(x, p8f_four_over_pi);
-
-  // Make a mask for the entries that need flipping, i.e. wherever the shift
-  // is odd.
-  Packet8i shift_ints = _mm256_cvtps_epi32(shift);
-  Packet8i shift_isodd = _mm256_castps_si256(_mm256_and_ps(_mm256_castsi256_ps(shift_ints), _mm256_castsi256_ps(p8i_one)));
-  Packet8i sign_flip_mask = pshiftleft(shift_isodd, 31);
-
-  // Create a mask for which interpolant to use, i.e. if z > 1, then the mask
-  // is set to ones for that entry.
-  Packet8f ival_mask = _mm256_cmp_ps(z, p8f_one, _CMP_GT_OQ);
-
-  // Evaluate the polynomial for the interval [1,3] in z.
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_right_0, 9.999999724233232e-01f);
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_right_2, -3.084242535619928e-01f);
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_right_4, 1.584991525700324e-02f);
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_right_6, -3.188805084631342e-04f);
-  Packet8f z_minus_two = psub(z, p8f_two);
-  Packet8f z_minus_two2 = pmul(z_minus_two, z_minus_two);
-  Packet8f right = pmadd(p8f_coeff_right_6, z_minus_two2, p8f_coeff_right_4);
-  right = pmadd(right, z_minus_two2, p8f_coeff_right_2);
-  right = pmadd(right, z_minus_two2, p8f_coeff_right_0);
-
-  // Evaluate the polynomial for the interval [-1,1] in z.
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_left_1, 7.853981525427295e-01f);
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_left_3, -8.074536727092352e-02f);
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_left_5, 2.489871967827018e-03f);
-  _EIGEN_DECLARE_CONST_Packet8f(coeff_left_7, -3.587725841214251e-05f);
-  Packet8f z2 = pmul(z, z);
-  Packet8f left = pmadd(p8f_coeff_left_7, z2, p8f_coeff_left_5);
-  left = pmadd(left, z2, p8f_coeff_left_3);
-  left = pmadd(left, z2, p8f_coeff_left_1);
-  left = pmul(left, z);
-
-  // Assemble the results, i.e. select the left and right polynomials.
-  left = _mm256_andnot_ps(ival_mask, left);
-  right = _mm256_and_ps(ival_mask, right);
-  Packet8f res = _mm256_or_ps(left, right);
-
-  // Flip the sign on the odd intervals and return the result.
-  res = _mm256_xor_ps(res, _mm256_castsi256_ps(sign_flip_mask));
-  return res;
+  return psin_float(_x);
 }
 
-// Natural logarithm
-// Computes log(x) as log(2^e * m) = C*e + log(m), where the constant C =log(2)
-// and m is in the range [sqrt(1/2),sqrt(2)). In this range, the logarithm can
-// be easily approximated by a polynomial centered on m=1 for stability.
-// TODO(gonnet): Further reduce the interval allowing for lower-degree
-//               polynomial interpolants -> ... -> profit!
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
+pcos<Packet8f>(const Packet8f& _x) {
+  return pcos_float(_x);
+}
+
 template <>
 EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
 plog<Packet8f>(const Packet8f& _x) {
-  Packet8f x = _x;
-  _EIGEN_DECLARE_CONST_Packet8f(1, 1.0f);
-  _EIGEN_DECLARE_CONST_Packet8f(half, 0.5f);
-  _EIGEN_DECLARE_CONST_Packet8f(126f, 126.0f);
+  return plog_float(_x);
+}
 
-  _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(inv_mant_mask, ~0x7f800000);
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4d
+plog<Packet4d>(const Packet4d& _x) {
+  return plog_double(_x);
+}
 
-  // The smallest non denormalized float number.
-  _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(min_norm_pos, 0x00800000);
-  _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(minus_inf, 0xff800000);
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
+plog2<Packet8f>(const Packet8f& _x) {
+  return plog2_float(_x);
+}
 
-  // Polynomial coefficients.
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_SQRTHF, 0.707106781186547524f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p0, 7.0376836292E-2f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p1, -1.1514610310E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p2, 1.1676998740E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p3, -1.2420140846E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p4, +1.4249322787E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p5, -1.6668057665E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p6, +2.0000714765E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p7, -2.4999993993E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_p8, +3.3333331174E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_q1, -2.12194440e-4f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_log_q2, 0.693359375f);
+template <>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4d
+plog2<Packet4d>(const Packet4d& _x) {
+  return plog2_double(_x);
+}
 
-  Packet8f invalid_mask = _mm256_cmp_ps(x, _mm256_setzero_ps(), _CMP_NGE_UQ); // not greater equal is true if x is NaN
-  Packet8f iszero_mask = _mm256_cmp_ps(x, _mm256_setzero_ps(), _CMP_EQ_OQ);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet8f plog1p<Packet8f>(const Packet8f& _x) {
+  return generic_plog1p(_x);
+}
 
-  // Truncate input values to the minimum positive normal.
-  x = pmax(x, p8f_min_norm_pos);
-
-  Packet8f emm0 = pshiftright(x,23);
-  Packet8f e = _mm256_sub_ps(emm0, p8f_126f);
-
-  // Set the exponents to -1, i.e. x are in the range [0.5,1).
-  x = _mm256_and_ps(x, p8f_inv_mant_mask);
-  x = _mm256_or_ps(x, p8f_half);
-
-  // part2: Shift the inputs from the range [0.5,1) to [sqrt(1/2),sqrt(2))
-  // and shift by -1. The values are then centered around 0, which improves
-  // the stability of the polynomial evaluation.
-  //   if( x < SQRTHF ) {
-  //     e -= 1;
-  //     x = x + x - 1.0;
-  //   } else { x = x - 1.0; }
-  Packet8f mask = _mm256_cmp_ps(x, p8f_cephes_SQRTHF, _CMP_LT_OQ);
-  Packet8f tmp = _mm256_and_ps(x, mask);
-  x = psub(x, p8f_1);
-  e = psub(e, _mm256_and_ps(p8f_1, mask));
-  x = padd(x, tmp);
-
-  Packet8f x2 = pmul(x, x);
-  Packet8f x3 = pmul(x2, x);
-
-  // Evaluate the polynomial approximant of degree 8 in three parts, probably
-  // to improve instruction-level parallelism.
-  Packet8f y, y1, y2;
-  y = pmadd(p8f_cephes_log_p0, x, p8f_cephes_log_p1);
-  y1 = pmadd(p8f_cephes_log_p3, x, p8f_cephes_log_p4);
-  y2 = pmadd(p8f_cephes_log_p6, x, p8f_cephes_log_p7);
-  y = pmadd(y, x, p8f_cephes_log_p2);
-  y1 = pmadd(y1, x, p8f_cephes_log_p5);
-  y2 = pmadd(y2, x, p8f_cephes_log_p8);
-  y = pmadd(y, x3, y1);
-  y = pmadd(y, x3, y2);
-  y = pmul(y, x3);
-
-  // Add the logarithm of the exponent back to the result of the interpolation.
-  y1 = pmul(e, p8f_cephes_log_q1);
-  tmp = pmul(x2, p8f_half);
-  y = padd(y, y1);
-  x = psub(x, tmp);
-  y2 = pmul(e, p8f_cephes_log_q2);
-  x = padd(x, y);
-  x = padd(x, y2);
-
-  // Filter out invalid inputs, i.e. negative arg will be NAN, 0 will be -INF.
-  return _mm256_or_ps(
-      _mm256_andnot_ps(iszero_mask, _mm256_or_ps(x, invalid_mask)),
-      _mm256_and_ps(iszero_mask, p8f_minus_inf));
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet8f pexpm1<Packet8f>(const Packet8f& _x) {
+  return generic_expm1(_x);
 }
 
 // Exponential function. Works by writing "x = m*log(2) + r" where
@@ -207,149 +70,21 @@
 template <>
 EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
 pexp<Packet8f>(const Packet8f& _x) {
-  _EIGEN_DECLARE_CONST_Packet8f(1, 1.0f);
-  _EIGEN_DECLARE_CONST_Packet8f(half, 0.5f);
-  _EIGEN_DECLARE_CONST_Packet8f(127, 127.0f);
-
-  _EIGEN_DECLARE_CONST_Packet8f(exp_hi, 88.3762626647950f);
-  _EIGEN_DECLARE_CONST_Packet8f(exp_lo, -88.3762626647949f);
-
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_LOG2EF, 1.44269504088896341f);
-
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_p0, 1.9875691500E-4f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_p1, 1.3981999507E-3f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_p2, 8.3334519073E-3f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_p3, 4.1665795894E-2f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_p4, 1.6666665459E-1f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_p5, 5.0000001201E-1f);
-
-  // Clamp x.
-  Packet8f x = pmax(pmin(_x, p8f_exp_hi), p8f_exp_lo);
-
-  // Express exp(x) as exp(m*ln(2) + r), start by extracting
-  // m = floor(x/ln(2) + 0.5).
-  Packet8f m = _mm256_floor_ps(pmadd(x, p8f_cephes_LOG2EF, p8f_half));
-
-// Get r = x - m*ln(2). If no FMA instructions are available, m*ln(2) is
-// subtracted out in two parts, m*C1+m*C2 = m*ln(2), to avoid accumulating
-// truncation errors. Note that we don't use the "pmadd" function here to
-// ensure that a precision-preserving FMA instruction is used.
-#ifdef EIGEN_VECTORIZE_FMA
-  _EIGEN_DECLARE_CONST_Packet8f(nln2, -0.6931471805599453f);
-  Packet8f r = _mm256_fmadd_ps(m, p8f_nln2, x);
-#else
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_C1, 0.693359375f);
-  _EIGEN_DECLARE_CONST_Packet8f(cephes_exp_C2, -2.12194440e-4f);
-  Packet8f r = psub(x, pmul(m, p8f_cephes_exp_C1));
-  r = psub(r, pmul(m, p8f_cephes_exp_C2));
-#endif
-
-  Packet8f r2 = pmul(r, r);
-
-  // TODO(gonnet): Split into odd/even polynomials and try to exploit
-  //               instruction-level parallelism.
-  Packet8f y = p8f_cephes_exp_p0;
-  y = pmadd(y, r, p8f_cephes_exp_p1);
-  y = pmadd(y, r, p8f_cephes_exp_p2);
-  y = pmadd(y, r, p8f_cephes_exp_p3);
-  y = pmadd(y, r, p8f_cephes_exp_p4);
-  y = pmadd(y, r, p8f_cephes_exp_p5);
-  y = pmadd(y, r2, r);
-  y = padd(y, p8f_1);
-
-  // Build emm0 = 2^m.
-  Packet8i emm0 = _mm256_cvttps_epi32(padd(m, p8f_127));
-  emm0 = pshiftleft(emm0, 23);
-
-  // Return 2^m * exp(r).
-  return pmax(pmul(y, _mm256_castsi256_ps(emm0)), _x);
+  return pexp_float(_x);
 }
 
 // Hyperbolic Tangent function.
 template <>
 EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-ptanh<Packet8f>(const Packet8f& x) {
-  return internal::generic_fast_tanh_float(x);
+ptanh<Packet8f>(const Packet8f& _x) {
+  return internal::generic_fast_tanh_float(_x);
 }
 
+// Exponential function for doubles.
 template <>
 EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4d
 pexp<Packet4d>(const Packet4d& _x) {
-  Packet4d x = _x;
-
-  _EIGEN_DECLARE_CONST_Packet4d(1, 1.0);
-  _EIGEN_DECLARE_CONST_Packet4d(2, 2.0);
-  _EIGEN_DECLARE_CONST_Packet4d(half, 0.5);
-
-  _EIGEN_DECLARE_CONST_Packet4d(exp_hi, 709.437);
-  _EIGEN_DECLARE_CONST_Packet4d(exp_lo, -709.436139303);
-
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_LOG2EF, 1.4426950408889634073599);
-
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_p0, 1.26177193074810590878e-4);
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_p1, 3.02994407707441961300e-2);
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_p2, 9.99999999999999999910e-1);
-
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_q0, 3.00198505138664455042e-6);
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_q1, 2.52448340349684104192e-3);
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_q2, 2.27265548208155028766e-1);
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_q3, 2.00000000000000000009e0);
-
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_C1, 0.693145751953125);
-  _EIGEN_DECLARE_CONST_Packet4d(cephes_exp_C2, 1.42860682030941723212e-6);
-  _EIGEN_DECLARE_CONST_Packet4i(1023, 1023);
-
-  Packet4d tmp, fx;
-
-  // clamp x
-  x = pmax(pmin(x, p4d_exp_hi), p4d_exp_lo);
-  // Express exp(x) as exp(g + n*log(2)).
-  fx = pmadd(p4d_cephes_LOG2EF, x, p4d_half);
-
-  // Get the integer modulus of log(2), i.e. the "n" described above.
-  fx = _mm256_floor_pd(fx);
-
-  // Get the remainder modulo log(2), i.e. the "g" described above. Subtract
-  // n*log(2) out in two steps, i.e. n*C1 + n*C2, C1+C2=log2 to get the last
-  // digits right.
-  tmp = pmul(fx, p4d_cephes_exp_C1);
-  Packet4d z = pmul(fx, p4d_cephes_exp_C2);
-  x = psub(x, tmp);
-  x = psub(x, z);
-
-  Packet4d x2 = pmul(x, x);
-
-  // Evaluate the numerator polynomial of the rational interpolant.
-  Packet4d px = p4d_cephes_exp_p0;
-  px = pmadd(px, x2, p4d_cephes_exp_p1);
-  px = pmadd(px, x2, p4d_cephes_exp_p2);
-  px = pmul(px, x);
-
-  // Evaluate the denominator polynomial of the rational interpolant.
-  Packet4d qx = p4d_cephes_exp_q0;
-  qx = pmadd(qx, x2, p4d_cephes_exp_q1);
-  qx = pmadd(qx, x2, p4d_cephes_exp_q2);
-  qx = pmadd(qx, x2, p4d_cephes_exp_q3);
-
-  // I don't really get this bit, copied from the SSE2 routines, so...
-  // TODO(gonnet): Figure out what is going on here, perhaps find a better
-  // rational interpolant?
-  x = _mm256_div_pd(px, psub(qx, px));
-  x = pmadd(p4d_2, x, p4d_1);
-
-  // Build e=2^n by constructing the exponents in a 128-bit vector and
-  // shifting them to where they belong in double-precision values.
-  __m128i emm0 = _mm256_cvtpd_epi32(fx);
-  emm0 = _mm_add_epi32(emm0, p4i_1023);
-  emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(3, 1, 2, 0));
-  __m128i lo = _mm_slli_epi64(emm0, 52);
-  __m128i hi = _mm_slli_epi64(_mm_srli_epi64(emm0, 32), 52);
-  __m256i e = _mm256_insertf128_si256(_mm256_setzero_si256(), lo, 0);
-  e = _mm256_insertf128_si256(e, hi, 1);
-
-  // Construct the result 2^n * exp(g) = e * x. The max is used to catch
-  // non-finite values in the input.
-  return pmax(pmul(x, _mm256_castsi256_pd(e)), _x);
+  return pexp_double(_x);
 }
 
 // Functions for sqrt.
@@ -362,37 +97,39 @@
 // For detail see here: http://www.beyond3d.com/content/articles/8/
 #if EIGEN_FAST_MATH
 template <>
-EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet8f
-psqrt<Packet8f>(const Packet8f& _x) {
-  Packet8f half = pmul(_x, pset1<Packet8f>(.5f));
-  Packet8f denormal_mask = _mm256_and_ps(
-      _mm256_cmp_ps(_x, pset1<Packet8f>((std::numeric_limits<float>::min)()),
-                    _CMP_LT_OQ),
-      _mm256_cmp_ps(_x, _mm256_setzero_ps(), _CMP_GE_OQ));
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet8f psqrt<Packet8f>(const Packet8f& _x) {
+  Packet8f minus_half_x = pmul(_x, pset1<Packet8f>(-0.5f));
+  Packet8f denormal_mask = pandnot(
+      pcmp_lt(_x, pset1<Packet8f>((std::numeric_limits<float>::min)())),
+      pcmp_lt(_x, pzero(_x)));
 
   // Compute approximate reciprocal sqrt.
   Packet8f x = _mm256_rsqrt_ps(_x);
   // Do a single step of Newton's iteration.
-  x = pmul(x, psub(pset1<Packet8f>(1.5f), pmul(half, pmul(x,x))));
+  x = pmul(x, pmadd(minus_half_x, pmul(x,x), pset1<Packet8f>(1.5f)));
   // Flush results for denormals to zero.
-  return _mm256_andnot_ps(denormal_mask, pmul(_x,x));
+  return pandnot(pmul(_x,x), denormal_mask);
 }
-#else
-template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f psqrt<Packet8f>(const Packet8f& x) {
-  return _mm256_sqrt_ps(x);
-}
-#endif
-template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4d psqrt<Packet4d>(const Packet4d& x) {
-  return _mm256_sqrt_pd(x);
-}
-#if EIGEN_FAST_MATH
 
+#else
+
+template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet8f psqrt<Packet8f>(const Packet8f& _x) {
+  return _mm256_sqrt_ps(_x);
+}
+
+#endif
+
+template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet4d psqrt<Packet4d>(const Packet4d& _x) {
+  return _mm256_sqrt_pd(_x);
+}
+
+#if EIGEN_FAST_MATH
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet8f prsqrt<Packet8f>(const Packet8f& _x) {
   _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(inf, 0x7f800000);
-  _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(nan, 0x7fc00000);
   _EIGEN_DECLARE_CONST_Packet8f(one_point_five, 1.5f);
   _EIGEN_DECLARE_CONST_Packet8f(minus_half, -0.5f);
   _EIGEN_DECLARE_CONST_Packet8f_FROM_INT(flt_min, 0x00800000);
@@ -401,36 +138,88 @@
 
   // select only the inverse sqrt of positive normal inputs (denormals are
   // flushed to zero and cause infs as well).
-  Packet8f le_zero_mask = _mm256_cmp_ps(_x, p8f_flt_min, _CMP_LT_OQ);
-  Packet8f x = _mm256_andnot_ps(le_zero_mask, _mm256_rsqrt_ps(_x));
+  Packet8f lt_min_mask = _mm256_cmp_ps(_x, p8f_flt_min, _CMP_LT_OQ);
+  Packet8f inf_mask =  _mm256_cmp_ps(_x, p8f_inf, _CMP_EQ_OQ);
+  Packet8f not_normal_finite_mask = _mm256_or_ps(lt_min_mask, inf_mask);
 
-  // Fill in NaNs and Infs for the negative/zero entries.
-  Packet8f neg_mask = _mm256_cmp_ps(_x, _mm256_setzero_ps(), _CMP_LT_OQ);
-  Packet8f zero_mask = _mm256_andnot_ps(neg_mask, le_zero_mask);
-  Packet8f infs_and_nans = _mm256_or_ps(_mm256_and_ps(neg_mask, p8f_nan),
-                                        _mm256_and_ps(zero_mask, p8f_inf));
+  // Compute an approximate result using the rsqrt intrinsic.
+  Packet8f y_approx = _mm256_rsqrt_ps(_x);
 
-  // Do a single step of Newton's iteration.
-  x = pmul(x, pmadd(neg_half, pmul(x, x), p8f_one_point_five));
+  // Do a single step of Newton-Raphson iteration to improve the approximation.
+  // This uses the formula y_{n+1} = y_n * (1.5 - y_n * (0.5 * x) * y_n).
+  // It is essential to evaluate the inner term like this because forming
+  // y_n^2 may over- or underflow.
+  Packet8f y_newton = pmul(y_approx, pmadd(y_approx, pmul(neg_half, y_approx), p8f_one_point_five));
 
-  // Insert NaNs and Infs in all the right places.
-  return _mm256_or_ps(x, infs_and_nans);
+  // Select the result of the Newton-Raphson step for positive normal arguments.
+  // For other arguments, choose the output of the intrinsic. This will
+  // return rsqrt(+inf) = 0, rsqrt(x) = NaN if x < 0, and rsqrt(x) = +inf if
+  // x is zero or a positive denormalized float (equivalent to flushing positive
+  // denormalized inputs to zero).
+  return pselect<Packet8f>(not_normal_finite_mask, y_approx, y_newton);
 }
 
 #else
 template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet8f prsqrt<Packet8f>(const Packet8f& x) {
+Packet8f prsqrt<Packet8f>(const Packet8f& _x) {
   _EIGEN_DECLARE_CONST_Packet8f(one, 1.0f);
-  return _mm256_div_ps(p8f_one, _mm256_sqrt_ps(x));
+  return _mm256_div_ps(p8f_one, _mm256_sqrt_ps(_x));
 }
 #endif
 
 template <> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4d prsqrt<Packet4d>(const Packet4d& x) {
+Packet4d prsqrt<Packet4d>(const Packet4d& _x) {
   _EIGEN_DECLARE_CONST_Packet4d(one, 1.0);
-  return _mm256_div_pd(p4d_one, _mm256_sqrt_pd(x));
+  return _mm256_div_pd(p4d_one, _mm256_sqrt_pd(_x));
 }
 
+F16_PACKET_FUNCTION(Packet8f, Packet8h, psin)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, pcos)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, plog)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, plog2)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, plog1p)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, pexpm1)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, pexp)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, ptanh)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, psqrt)
+F16_PACKET_FUNCTION(Packet8f, Packet8h, prsqrt)
+
+template <>
+EIGEN_STRONG_INLINE Packet8h pfrexp(const Packet8h& a, Packet8h& exponent) {
+  Packet8f fexponent;
+  const Packet8h out = float2half(pfrexp<Packet8f>(half2float(a), fexponent));
+  exponent = float2half(fexponent);
+  return out;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8h pldexp(const Packet8h& a, const Packet8h& exponent) {
+  return float2half(pldexp<Packet8f>(half2float(a), half2float(exponent)));
+}
+
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psin)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pcos)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog2)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, plog1p)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pexpm1)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, pexp)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, ptanh)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, psqrt)
+BF16_PACKET_FUNCTION(Packet8f, Packet8bf, prsqrt)
+
+template <>
+EIGEN_STRONG_INLINE Packet8bf pfrexp(const Packet8bf& a, Packet8bf& exponent) {
+  Packet8f fexponent;
+  const Packet8bf out = F32ToBf16(pfrexp<Packet8f>(Bf16ToF32(a), fexponent));
+  exponent = F32ToBf16(fexponent);
+  return out;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8bf pldexp(const Packet8bf& a, const Packet8bf& exponent) {
+  return F32ToBf16(pldexp<Packet8f>(Bf16ToF32(a), Bf16ToF32(exponent)));
+}
 
 }  // end namespace internal
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
index 923a124..7fc32fd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/PacketMath.h
@@ -18,11 +18,11 @@
 #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8
 #endif
 
-#ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS
-#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2*sizeof(void*))
+#if !defined(EIGEN_VECTORIZE_AVX512) && !defined(EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS)
+#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 16
 #endif
 
-#ifdef __FMA__
+#ifdef EIGEN_VECTORIZE_FMA
 #ifndef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
 #define EIGEN_HAS_SINGLE_INSTRUCTION_MADD
 #endif
@@ -31,10 +31,14 @@
 typedef __m256  Packet8f;
 typedef __m256i Packet8i;
 typedef __m256d Packet4d;
+typedef eigen_packet_wrapper<__m128i, 2> Packet8h;
+typedef eigen_packet_wrapper<__m128i, 3> Packet8bf;
 
 template<> struct is_arithmetic<__m256>  { enum { value = true }; };
 template<> struct is_arithmetic<__m256i> { enum { value = true }; };
 template<> struct is_arithmetic<__m256d> { enum { value = true }; };
+template<> struct is_arithmetic<Packet8h> { enum { value = true }; };
+template<> struct is_arithmetic<Packet8bf> { enum { value = true }; };
 
 #define _EIGEN_DECLARE_CONST_Packet8f(NAME,X) \
   const Packet8f p8f_##NAME = pset1<Packet8f>(X)
@@ -58,21 +62,28 @@
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    size=8,
+    size = 8,
     HasHalfPacket = 1,
 
-    HasDiv  = 1,
-    HasSin  = EIGEN_FAST_MATH,
-    HasCos  = 0,
-    HasLog  = 1,
-    HasExp  = 1,
+    HasCmp  = 1,
+    HasDiv = 1,
+    HasSin = EIGEN_FAST_MATH,
+    HasCos = EIGEN_FAST_MATH,
+    HasLog = 1,
+    HasLog1p = 1,
+    HasExpm1 = 1,
+    HasExp = 1,
+    HasNdtri = 1,
+    HasBessel = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
-    HasTanh  = EIGEN_FAST_MATH,
+    HasTanh = EIGEN_FAST_MATH,
+    HasErf = EIGEN_FAST_MATH,
     HasBlend = 1,
     HasRound = 1,
     HasFloor = 1,
-    HasCeil = 1
+    HasCeil = 1,
+    HasRint = 1
   };
 };
 template<> struct packet_traits<double> : default_packet_traits
@@ -85,14 +96,104 @@
     size=4,
     HasHalfPacket = 1,
 
+    HasCmp  = 1,
     HasDiv  = 1,
+    HasLog  = 1,
     HasExp  = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
     HasBlend = 1,
     HasRound = 1,
     HasFloor = 1,
-    HasCeil = 1
+    HasCeil = 1,
+    HasRint = 1
+  };
+};
+
+template <>
+struct packet_traits<Eigen::half> : default_packet_traits {
+  typedef Packet8h type;
+  // There is no half-size packet for Packet8h.
+  typedef Packet8h half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 8,
+    HasHalfPacket = 0,
+
+    HasCmp    = 1,
+    HasAdd    = 1,
+    HasSub    = 1,
+    HasMul    = 1,
+    HasDiv    = 1,
+    HasSin    = EIGEN_FAST_MATH,
+    HasCos    = EIGEN_FAST_MATH,
+    HasNegate = 1,
+    HasAbs    = 1,
+    HasAbs2   = 0,
+    HasMin    = 1,
+    HasMax    = 1,
+    HasConj   = 1,
+    HasSetLinear = 0,
+    HasLog    = 1,
+    HasLog1p  = 1,
+    HasExpm1  = 1,
+    HasExp    = 1,
+    HasSqrt   = 1,
+    HasRsqrt  = 1,
+    HasTanh   = EIGEN_FAST_MATH,
+    HasErf    = EIGEN_FAST_MATH,
+    HasBlend  = 0,
+    HasRound  = 1,
+    HasFloor  = 1,
+    HasCeil   = 1,
+    HasRint   = 1,
+    HasBessel = 1,
+    HasNdtri  = 1
+  };
+};
+
+template <>
+struct packet_traits<bfloat16> : default_packet_traits {
+  typedef Packet8bf type;
+  // There is no half-size packet for current Packet8bf.
+  // TODO: support as SSE path.
+  typedef Packet8bf half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 8,
+    HasHalfPacket = 0,
+
+    HasCmp = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasMul = 1,
+    HasDiv = 1,
+    HasSin = EIGEN_FAST_MATH,
+    HasCos = EIGEN_FAST_MATH,
+    HasNegate = 1,
+    HasAbs    = 1,
+    HasAbs2   = 0,
+    HasMin    = 1,
+    HasMax    = 1,
+    HasConj   = 1,
+    HasSetLinear = 0,
+    HasLog = 1,
+    HasLog1p  = 1,
+    HasExpm1  = 1,
+    HasExp = 1,
+    HasSqrt = 1,
+    HasRsqrt = 1,
+    HasTanh = EIGEN_FAST_MATH,
+    HasErf = EIGEN_FAST_MATH,
+    HasBlend = 0,
+    HasRound = 1,
+    HasFloor = 1,
+    HasCeil = 1,
+    HasRint = 1,
+    HasBessel = 1,
+    HasNdtri  = 1
   };
 };
 #endif
@@ -113,14 +214,45 @@
 };
 */
 
-template<> struct unpacket_traits<Packet8f> { typedef float  type; typedef Packet4f half; enum {size=8, alignment=Aligned32}; };
-template<> struct unpacket_traits<Packet4d> { typedef double type; typedef Packet2d half; enum {size=4, alignment=Aligned32}; };
-template<> struct unpacket_traits<Packet8i> { typedef int    type; typedef Packet4i half; enum {size=8, alignment=Aligned32}; };
+template<> struct unpacket_traits<Packet8f> {
+  typedef float     type;
+  typedef Packet4f  half;
+  typedef Packet8i  integer_packet;
+  typedef uint8_t   mask_t;
+  enum {size=8, alignment=Aligned32, vectorizable=true, masked_load_available=true, masked_store_available=true};
+};
+template<> struct unpacket_traits<Packet4d> {
+  typedef double type;
+  typedef Packet2d half;
+  enum {size=4, alignment=Aligned32, vectorizable=true, masked_load_available=false, masked_store_available=false};
+};
+template<> struct unpacket_traits<Packet8i> { typedef int    type; typedef Packet4i half; enum {size=8, alignment=Aligned32, vectorizable=false, masked_load_available=false, masked_store_available=false}; };
+template<> struct unpacket_traits<Packet8bf> { typedef bfloat16 type; typedef Packet8bf half; enum {size=8, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false}; };
+
+// Helper function for bit packing snippet of low precision comparison.
+// It packs the flags from 16x16 to 8x16.
+EIGEN_STRONG_INLINE __m128i Pack16To8(Packet8f rf) {
+  return _mm_packs_epi32(_mm256_extractf128_si256(_mm256_castps_si256(rf), 0),
+                         _mm256_extractf128_si256(_mm256_castps_si256(rf), 1));
+}
+
 
 template<> EIGEN_STRONG_INLINE Packet8f pset1<Packet8f>(const float&  from) { return _mm256_set1_ps(from); }
 template<> EIGEN_STRONG_INLINE Packet4d pset1<Packet4d>(const double& from) { return _mm256_set1_pd(from); }
 template<> EIGEN_STRONG_INLINE Packet8i pset1<Packet8i>(const int&    from) { return _mm256_set1_epi32(from); }
 
+template<> EIGEN_STRONG_INLINE Packet8f pset1frombits<Packet8f>(unsigned int from) { return _mm256_castsi256_ps(pset1<Packet8i>(from)); }
+template<> EIGEN_STRONG_INLINE Packet4d pset1frombits<Packet4d>(uint64_t from) { return _mm256_castsi256_pd(_mm256_set1_epi64x(from)); }
+
+template<> EIGEN_STRONG_INLINE Packet8f pzero(const Packet8f& /*a*/) { return _mm256_setzero_ps(); }
+template<> EIGEN_STRONG_INLINE Packet4d pzero(const Packet4d& /*a*/) { return _mm256_setzero_pd(); }
+template<> EIGEN_STRONG_INLINE Packet8i pzero(const Packet8i& /*a*/) { return _mm256_setzero_si256(); }
+
+
+template<> EIGEN_STRONG_INLINE Packet8f peven_mask(const Packet8f& /*a*/) { return _mm256_castsi256_ps(_mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1)); }
+template<> EIGEN_STRONG_INLINE Packet8i peven_mask(const Packet8i& /*a*/) { return _mm256_set_epi32(0, -1, 0, -1, 0, -1, 0, -1); }
+template<> EIGEN_STRONG_INLINE Packet4d peven_mask(const Packet4d& /*a*/) { return _mm256_castsi256_pd(_mm256_set_epi32(0, 0, -1, -1, 0, 0, -1, -1)); }
+
 template<> EIGEN_STRONG_INLINE Packet8f pload1<Packet8f>(const float*  from) { return _mm256_broadcast_ss(from); }
 template<> EIGEN_STRONG_INLINE Packet4d pload1<Packet4d>(const double* from) { return _mm256_broadcast_sd(from); }
 
@@ -129,9 +261,27 @@
 
 template<> EIGEN_STRONG_INLINE Packet8f padd<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_add_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d padd<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_add_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8i padd<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_add_epi32(a,b);
+#else
+  __m128i lo = _mm_add_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_add_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet8f psub<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_sub_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d psub<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_sub_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8i psub<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_sub_epi32(a,b);
+#else
+  __m128i lo = _mm_sub_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_sub_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet8f pnegate(const Packet8f& a)
 {
@@ -148,7 +298,15 @@
 
 template<> EIGEN_STRONG_INLINE Packet8f pmul<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_mul_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d pmul<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_mul_pd(a,b); }
-
+template<> EIGEN_STRONG_INLINE Packet8i pmul<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_mullo_epi32(a,b);
+#else
+  const __m128i lo = _mm_mullo_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  const __m128i hi = _mm_mullo_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet8f pdiv<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_div_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d pdiv<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_div_pd(a,b); }
@@ -157,7 +315,7 @@
   return pset1<Packet8i>(0);
 }
 
-#ifdef __FMA__
+#ifdef EIGEN_VECTORIZE_FMA
 template<> EIGEN_STRONG_INLINE Packet8f pmadd(const Packet8f& a, const Packet8f& b, const Packet8f& c) {
 #if ( (EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<80) || (EIGEN_COMP_CLANG) )
   // Clang stupidly generates a vfmadd213ps instruction plus some vmovaps on registers,
@@ -184,14 +342,112 @@
 }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet8f pmin<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_min_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pmin<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_min_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8f pcmp_le(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a,b,_CMP_LE_OQ); }
+template<> EIGEN_STRONG_INLINE Packet8f pcmp_lt(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a,b,_CMP_LT_OQ); }
+template<> EIGEN_STRONG_INLINE Packet8f pcmp_lt_or_nan(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a, b, _CMP_NGE_UQ); }
+template<> EIGEN_STRONG_INLINE Packet8f pcmp_eq(const Packet8f& a, const Packet8f& b) { return _mm256_cmp_ps(a,b,_CMP_EQ_OQ); }
 
-template<> EIGEN_STRONG_INLINE Packet8f pmax<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_max_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pmax<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_max_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4d pcmp_le(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a,b,_CMP_LE_OQ); }
+template<> EIGEN_STRONG_INLINE Packet4d pcmp_lt(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a,b,_CMP_LT_OQ); }
+template<> EIGEN_STRONG_INLINE Packet4d pcmp_lt_or_nan(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a, b, _CMP_NGE_UQ); }
+template<> EIGEN_STRONG_INLINE Packet4d pcmp_eq(const Packet4d& a, const Packet4d& b) { return _mm256_cmp_pd(a,b,_CMP_EQ_OQ); }
 
-template<> EIGEN_STRONG_INLINE Packet8f pround<Packet8f>(const Packet8f& a) { return _mm256_round_ps(a, _MM_FROUND_CUR_DIRECTION); }
-template<> EIGEN_STRONG_INLINE Packet4d pround<Packet4d>(const Packet4d& a) { return _mm256_round_pd(a, _MM_FROUND_CUR_DIRECTION); }
+
+template<> EIGEN_STRONG_INLINE Packet8i pcmp_eq(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_cmpeq_epi32(a,b);
+#else
+  __m128i lo = _mm_cmpeq_epi32(_mm256_extractf128_si256(a, 0), _mm256_extractf128_si256(b, 0));
+  __m128i hi = _mm_cmpeq_epi32(_mm256_extractf128_si256(a, 1), _mm256_extractf128_si256(b, 1));
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pmin<Packet8f>(const Packet8f& a, const Packet8f& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // There appears to be a bug in GCC, by which the optimizer may flip
+  // the argument order in calls to _mm_min_ps/_mm_max_ps, so we have to
+  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+  Packet8f res;
+  asm("vminps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  return res;
+#else
+  // Arguments are swapped to match NaN propagation behavior of std::min.
+  return _mm256_min_ps(b,a);
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet4d pmin<Packet4d>(const Packet4d& a, const Packet4d& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // See pmin above
+  Packet4d res;
+  asm("vminpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  return res;
+#else
+  // Arguments are swapped to match NaN propagation behavior of std::min.
+  return _mm256_min_pd(b,a);
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pmax<Packet8f>(const Packet8f& a, const Packet8f& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // See pmin above
+  Packet8f res;
+  asm("vmaxps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  return res;
+#else
+  // Arguments are swapped to match NaN propagation behavior of std::max.
+  return _mm256_max_ps(b,a);
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet4d pmax<Packet4d>(const Packet4d& a, const Packet4d& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // See pmin above
+  Packet4d res;
+  asm("vmaxpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  return res;
+#else
+  // Arguments are swapped to match NaN propagation behavior of std::max.
+  return _mm256_max_pd(b,a);
+#endif
+}
+
+// Add specializations for min/max with prescribed NaN progation.
+template<>
+EIGEN_STRONG_INLINE Packet8f pmin<PropagateNumbers, Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return pminmax_propagate_numbers(a, b, pmin<Packet8f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4d pmin<PropagateNumbers, Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return pminmax_propagate_numbers(a, b, pmin<Packet4d>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet8f pmax<PropagateNumbers, Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return pminmax_propagate_numbers(a, b, pmax<Packet8f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4d pmax<PropagateNumbers, Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return pminmax_propagate_numbers(a, b, pmax<Packet4d>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet8f pmin<PropagateNaN, Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return pminmax_propagate_nan(a, b, pmin<Packet8f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4d pmin<PropagateNaN, Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return pminmax_propagate_nan(a, b, pmin<Packet4d>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet8f pmax<PropagateNaN, Packet8f>(const Packet8f& a, const Packet8f& b) {
+  return pminmax_propagate_nan(a, b, pmax<Packet8f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4d pmax<PropagateNaN, Packet4d>(const Packet4d& a, const Packet4d& b) {
+  return pminmax_propagate_nan(a, b, pmax<Packet4d>);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f print<Packet8f>(const Packet8f& a) { return _mm256_round_ps(a, _MM_FROUND_CUR_DIRECTION); }
+template<> EIGEN_STRONG_INLINE Packet4d print<Packet4d>(const Packet4d& a) { return _mm256_round_pd(a, _MM_FROUND_CUR_DIRECTION); }
 
 template<> EIGEN_STRONG_INLINE Packet8f pceil<Packet8f>(const Packet8f& a) { return _mm256_ceil_ps(a); }
 template<> EIGEN_STRONG_INLINE Packet4d pceil<Packet4d>(const Packet4d& a) { return _mm256_ceil_pd(a); }
@@ -199,17 +455,124 @@
 template<> EIGEN_STRONG_INLINE Packet8f pfloor<Packet8f>(const Packet8f& a) { return _mm256_floor_ps(a); }
 template<> EIGEN_STRONG_INLINE Packet4d pfloor<Packet4d>(const Packet4d& a) { return _mm256_floor_pd(a); }
 
+
+template<> EIGEN_STRONG_INLINE Packet8i ptrue<Packet8i>(const Packet8i& a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  // vpcmpeqd has lower latency than the more general vcmpps
+  return _mm256_cmpeq_epi32(a,a);
+#else
+  const __m256 b = _mm256_castsi256_ps(a);
+  return _mm256_castps_si256(_mm256_cmp_ps(b,b,_CMP_TRUE_UQ));
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f ptrue<Packet8f>(const Packet8f& a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  // vpcmpeqd has lower latency than the more general vcmpps
+  const __m256i b = _mm256_castps_si256(a);
+  return _mm256_castsi256_ps(_mm256_cmpeq_epi32(b,b));
+#else
+  return _mm256_cmp_ps(a,a,_CMP_TRUE_UQ);
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet4d ptrue<Packet4d>(const Packet4d& a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  // vpcmpeqq has lower latency than the more general vcmppd
+  const __m256i b = _mm256_castpd_si256(a);
+  return _mm256_castsi256_pd(_mm256_cmpeq_epi64(b,b));
+#else
+  return _mm256_cmp_pd(a,a,_CMP_TRUE_UQ);
+#endif
+}
+
 template<> EIGEN_STRONG_INLINE Packet8f pand<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_and_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d pand<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_and_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8i pand<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_and_si256(a,b);
+#else
+  return _mm256_castps_si256(_mm256_and_ps(_mm256_castsi256_ps(a),_mm256_castsi256_ps(b)));
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet8f por<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_or_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d por<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_or_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8i por<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_or_si256(a,b);
+#else
+  return _mm256_castps_si256(_mm256_or_ps(_mm256_castsi256_ps(a),_mm256_castsi256_ps(b)));
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet8f pxor<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_xor_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4d pxor<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_xor_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8i pxor<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_xor_si256(a,b);
+#else
+  return _mm256_castps_si256(_mm256_xor_ps(_mm256_castsi256_ps(a),_mm256_castsi256_ps(b)));
+#endif
+}
 
-template<> EIGEN_STRONG_INLINE Packet8f pandnot<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_andnot_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4d pandnot<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_andnot_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8f pandnot<Packet8f>(const Packet8f& a, const Packet8f& b) { return _mm256_andnot_ps(b,a); }
+template<> EIGEN_STRONG_INLINE Packet4d pandnot<Packet4d>(const Packet4d& a, const Packet4d& b) { return _mm256_andnot_pd(b,a); }
+template<> EIGEN_STRONG_INLINE Packet8i pandnot<Packet8i>(const Packet8i& a, const Packet8i& b) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_andnot_si256(b,a);
+#else
+  return _mm256_castps_si256(_mm256_andnot_ps(_mm256_castsi256_ps(b),_mm256_castsi256_ps(a)));
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pround<Packet8f>(const Packet8f& a)
+{
+  const Packet8f mask = pset1frombits<Packet8f>(static_cast<numext::uint32_t>(0x80000000u));
+  const Packet8f prev0dot5 = pset1frombits<Packet8f>(static_cast<numext::uint32_t>(0x3EFFFFFFu));
+  return _mm256_round_ps(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
+}
+template<> EIGEN_STRONG_INLINE Packet4d pround<Packet4d>(const Packet4d& a)
+{
+  const Packet4d mask = pset1frombits<Packet4d>(static_cast<numext::uint64_t>(0x8000000000000000ull));
+  const Packet4d prev0dot5 = pset1frombits<Packet4d>(static_cast<numext::uint64_t>(0x3FDFFFFFFFFFFFFFull));
+  return _mm256_round_pd(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pselect<Packet8f>(const Packet8f& mask, const Packet8f& a, const Packet8f& b)
+{ return _mm256_blendv_ps(b,a,mask); }
+template<> EIGEN_STRONG_INLINE Packet4d pselect<Packet4d>(const Packet4d& mask, const Packet4d& a, const Packet4d& b)
+{ return _mm256_blendv_pd(b,a,mask); }
+
+template<int N> EIGEN_STRONG_INLINE Packet8i parithmetic_shift_right(Packet8i a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_srai_epi32(a, N);
+#else
+  __m128i lo = _mm_srai_epi32(_mm256_extractf128_si256(a, 0), N);
+  __m128i hi = _mm_srai_epi32(_mm256_extractf128_si256(a, 1), N);
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+
+template<int N> EIGEN_STRONG_INLINE Packet8i plogical_shift_right(Packet8i a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_srli_epi32(a, N);
+#else
+  __m128i lo = _mm_srli_epi32(_mm256_extractf128_si256(a, 0), N);
+  __m128i hi = _mm_srli_epi32(_mm256_extractf128_si256(a, 1), N);
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
+
+template<int N> EIGEN_STRONG_INLINE Packet8i plogical_shift_left(Packet8i a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  return _mm256_slli_epi32(a, N);
+#else
+  __m128i lo = _mm_slli_epi32(_mm256_extractf128_si256(a, 0), N);
+  __m128i hi = _mm_slli_epi32(_mm256_extractf128_si256(a, 1), N);
+  return _mm256_insertf128_si256(_mm256_castsi128_si256(lo), (hi), 1);
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet8f pload<Packet8f>(const float*   from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_ps(from); }
 template<> EIGEN_STRONG_INLINE Packet4d pload<Packet4d>(const double*  from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm256_load_pd(from); }
@@ -219,6 +582,14 @@
 template<> EIGEN_STRONG_INLINE Packet4d ploadu<Packet4d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_pd(from); }
 template<> EIGEN_STRONG_INLINE Packet8i ploadu<Packet8i>(const int* from) { EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_loadu_si256(reinterpret_cast<const __m256i*>(from)); }
 
+template<> EIGEN_STRONG_INLINE Packet8f ploadu<Packet8f>(const float* from, uint8_t umask) {
+  Packet8i mask = _mm256_set1_epi8(static_cast<char>(umask));
+  const Packet8i bit_mask = _mm256_set_epi32(0xffffff7f, 0xffffffbf, 0xffffffdf, 0xffffffef, 0xfffffff7, 0xfffffffb, 0xfffffffd, 0xfffffffe);
+  mask = por<Packet8i>(mask, bit_mask);
+  mask = pcmp_eq<Packet8i>(mask, _mm256_set1_epi32(0xffffffff));
+  EIGEN_DEBUG_UNALIGNED_LOAD return _mm256_maskload_ps(from, mask);
+}
+
 // Loads 4 floats from memory a returns the packet {a0, a0  a1, a1, a2, a2, a3, a3}
 template<> EIGEN_STRONG_INLINE Packet8f ploaddup<Packet8f>(const float* from)
 {
@@ -226,7 +597,7 @@
 //   Packet8f tmp  = _mm256_castps128_ps256(_mm_loadu_ps(from));
 //   tmp = _mm256_insertf128_ps(tmp, _mm_movehl_ps(_mm256_castps256_ps128(tmp),_mm256_castps256_ps128(tmp)), 1);
 //   return _mm256_unpacklo_ps(tmp,tmp);
-  
+
   // _mm256_insertf128_ps is very slow on Haswell, thus:
   Packet8f tmp = _mm256_broadcast_ps((const __m128*)(const void*)from);
   // mimic an "inplace" permutation of the lower 128bits using a blend
@@ -256,6 +627,14 @@
 template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet4d& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_pd(to, from); }
 template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*       to, const Packet8i& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm256_storeu_si256(reinterpret_cast<__m256i*>(to), from); }
 
+template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*   to, const Packet8f& from, uint8_t umask) {
+  Packet8i mask = _mm256_set1_epi8(static_cast<char>(umask));
+  const Packet8i bit_mask = _mm256_set_epi32(0xffffff7f, 0xffffffbf, 0xffffffdf, 0xffffffef, 0xfffffff7, 0xfffffffb, 0xfffffffd, 0xfffffffe);
+  mask = por<Packet8i>(mask, bit_mask);
+  mask = pcmp_eq<Packet8i>(mask, _mm256_set1_epi32(0xffffffff));
+  EIGEN_DEBUG_UNALIGNED_STORE return _mm256_maskstore_ps(to, mask, from);
+}
+
 // NOTE: leverage _mm256_i32gather_ps and _mm256_i32gather_pd if AVX2 instructions are available
 // NOTE: for the record the following seems to be slower: return _mm256_i32gather_ps(from, _mm256_set1_epi32(stride), 4);
 template<> EIGEN_DEVICE_FUNC inline Packet8f pgather<float, Packet8f>(const float* from, Index stride)
@@ -354,47 +733,66 @@
   return _mm256_and_pd(a,mask);
 }
 
-// preduxp should be ok
-// FIXME: why is this ok? why isn't the simply implementation working as expected?
-template<> EIGEN_STRONG_INLINE Packet8f preduxp<Packet8f>(const Packet8f* vecs)
-{
-    __m256 hsum1 = _mm256_hadd_ps(vecs[0], vecs[1]);
-    __m256 hsum2 = _mm256_hadd_ps(vecs[2], vecs[3]);
-    __m256 hsum3 = _mm256_hadd_ps(vecs[4], vecs[5]);
-    __m256 hsum4 = _mm256_hadd_ps(vecs[6], vecs[7]);
-
-    __m256 hsum5 = _mm256_hadd_ps(hsum1, hsum1);
-    __m256 hsum6 = _mm256_hadd_ps(hsum2, hsum2);
-    __m256 hsum7 = _mm256_hadd_ps(hsum3, hsum3);
-    __m256 hsum8 = _mm256_hadd_ps(hsum4, hsum4);
-
-    __m256 perm1 =  _mm256_permute2f128_ps(hsum5, hsum5, 0x23);
-    __m256 perm2 =  _mm256_permute2f128_ps(hsum6, hsum6, 0x23);
-    __m256 perm3 =  _mm256_permute2f128_ps(hsum7, hsum7, 0x23);
-    __m256 perm4 =  _mm256_permute2f128_ps(hsum8, hsum8, 0x23);
-
-    __m256 sum1 = _mm256_add_ps(perm1, hsum5);
-    __m256 sum2 = _mm256_add_ps(perm2, hsum6);
-    __m256 sum3 = _mm256_add_ps(perm3, hsum7);
-    __m256 sum4 = _mm256_add_ps(perm4, hsum8);
-
-    __m256 blend1 = _mm256_blend_ps(sum1, sum2, 0xcc);
-    __m256 blend2 = _mm256_blend_ps(sum3, sum4, 0xcc);
-
-    __m256 final = _mm256_blend_ps(blend1, blend2, 0xf0);
-    return final;
+template<> EIGEN_STRONG_INLINE Packet8f pfrexp<Packet8f>(const Packet8f& a, Packet8f& exponent) {
+  return pfrexp_generic(a,exponent);
 }
-template<> EIGEN_STRONG_INLINE Packet4d preduxp<Packet4d>(const Packet4d* vecs)
-{
- Packet4d tmp0, tmp1;
 
-  tmp0 = _mm256_hadd_pd(vecs[0], vecs[1]);
-  tmp0 = _mm256_add_pd(tmp0, _mm256_permute2f128_pd(tmp0, tmp0, 1));
+// Extract exponent without existence of Packet4l.
+template<>
+EIGEN_STRONG_INLINE  
+Packet4d pfrexp_generic_get_biased_exponent(const Packet4d& a) {
+  const Packet4d cst_exp_mask  = pset1frombits<Packet4d>(static_cast<uint64_t>(0x7ff0000000000000ull));
+  __m256i a_expo = _mm256_castpd_si256(pand(a, cst_exp_mask));
+#ifdef EIGEN_VECTORIZE_AVX2
+  a_expo = _mm256_srli_epi64(a_expo, 52);
+  __m128i lo = _mm256_extractf128_si256(a_expo, 0);
+  __m128i hi = _mm256_extractf128_si256(a_expo, 1);
+#else
+  __m128i lo = _mm256_extractf128_si256(a_expo, 0);
+  __m128i hi = _mm256_extractf128_si256(a_expo, 1);
+  lo = _mm_srli_epi64(lo, 52);
+  hi = _mm_srli_epi64(hi, 52);
+#endif
+  Packet2d exponent_lo = _mm_cvtepi32_pd(vec4i_swizzle1(lo, 0, 2, 1, 3));
+  Packet2d exponent_hi = _mm_cvtepi32_pd(vec4i_swizzle1(hi, 0, 2, 1, 3));
+  Packet4d exponent = _mm256_insertf128_pd(_mm256_setzero_pd(), exponent_lo, 0);
+  exponent = _mm256_insertf128_pd(exponent, exponent_hi, 1);
+  return exponent;
+}
 
-  tmp1 = _mm256_hadd_pd(vecs[2], vecs[3]);
-  tmp1 = _mm256_add_pd(tmp1, _mm256_permute2f128_pd(tmp1, tmp1, 1));
 
-  return _mm256_blend_pd(tmp0, tmp1, 0xC);
+template<> EIGEN_STRONG_INLINE Packet4d pfrexp<Packet4d>(const Packet4d& a, Packet4d& exponent) {
+  return pfrexp_generic(a, exponent);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pldexp<Packet8f>(const Packet8f& a, const Packet8f& exponent) {
+  return pldexp_generic(a, exponent);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4d pldexp<Packet4d>(const Packet4d& a, const Packet4d& exponent) {
+  // Clamp exponent to [-2099, 2099]
+  const Packet4d max_exponent = pset1<Packet4d>(2099.0);
+  const Packet4i e = _mm256_cvtpd_epi32(pmin(pmax(exponent, pnegate(max_exponent)), max_exponent));
+  
+  // Split 2^e into four factors and multiply.
+  const Packet4i bias = pset1<Packet4i>(1023);
+  Packet4i b = parithmetic_shift_right<2>(e);  // floor(e/4)
+  
+  // 2^b
+  Packet4i hi = vec4i_swizzle1(padd(b, bias), 0, 2, 1, 3);
+  Packet4i lo = _mm_slli_epi64(hi, 52);
+  hi = _mm_slli_epi64(_mm_srli_epi64(hi, 32), 52);
+  Packet4d c = _mm256_castsi256_pd(_mm256_insertf128_si256(_mm256_castsi128_si256(lo), hi, 1));
+  Packet4d out = pmul(pmul(pmul(a, c), c), c);  // a * 2^(3b)
+  
+  // 2^(e - 3b)
+  b = psub(psub(psub(e, b), b), b);  // e - 3b
+  hi = vec4i_swizzle1(padd(b, bias), 0, 2, 1, 3);
+  lo = _mm_slli_epi64(hi, 52);
+  hi = _mm_slli_epi64(_mm_srli_epi64(hi, 32), 52);
+  c = _mm256_castsi256_pd(_mm256_insertf128_si256(_mm256_castsi128_si256(lo), hi, 1));
+  out = pmul(out, c); // a * 2^e
+  return out;
 }
 
 template<> EIGEN_STRONG_INLINE float predux<Packet8f>(const Packet8f& a)
@@ -406,7 +804,7 @@
   return predux(Packet2d(_mm_add_pd(_mm256_castpd256_pd128(a),_mm256_extractf128_pd(a,1))));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f predux_downto4<Packet8f>(const Packet8f& a)
+template<> EIGEN_STRONG_INLINE Packet4f predux_half_dowto4<Packet8f>(const Packet8f& a)
 {
   return _mm_add_ps(_mm256_castps256_ps128(a),_mm256_extractf128_ps(a,1));
 }
@@ -450,93 +848,16 @@
   return pfirst(_mm256_max_pd(tmp, _mm256_shuffle_pd(tmp, tmp, 1)));
 }
 
+// not needed yet
+// template<> EIGEN_STRONG_INLINE bool predux_all(const Packet8f& x)
+// {
+//   return _mm256_movemask_ps(x)==0xFF;
+// }
 
-template<int Offset>
-struct palign_impl<Offset,Packet8f>
+template<> EIGEN_STRONG_INLINE bool predux_any(const Packet8f& x)
 {
-  static EIGEN_STRONG_INLINE void run(Packet8f& first, const Packet8f& second)
-  {
-    if (Offset==1)
-    {
-      first = _mm256_blend_ps(first, second, 1);
-      Packet8f tmp1 = _mm256_permute_ps (first, _MM_SHUFFLE(0,3,2,1));
-      Packet8f tmp2 = _mm256_permute2f128_ps (tmp1, tmp1, 1);
-      first = _mm256_blend_ps(tmp1, tmp2, 0x88);
-    }
-    else if (Offset==2)
-    {
-      first = _mm256_blend_ps(first, second, 3);
-      Packet8f tmp1 = _mm256_permute_ps (first, _MM_SHUFFLE(1,0,3,2));
-      Packet8f tmp2 = _mm256_permute2f128_ps (tmp1, tmp1, 1);
-      first = _mm256_blend_ps(tmp1, tmp2, 0xcc);
-    }
-    else if (Offset==3)
-    {
-      first = _mm256_blend_ps(first, second, 7);
-      Packet8f tmp1 = _mm256_permute_ps (first, _MM_SHUFFLE(2,1,0,3));
-      Packet8f tmp2 = _mm256_permute2f128_ps (tmp1, tmp1, 1);
-      first = _mm256_blend_ps(tmp1, tmp2, 0xee);
-    }
-    else if (Offset==4)
-    {
-      first = _mm256_blend_ps(first, second, 15);
-      Packet8f tmp1 = _mm256_permute_ps (first, _MM_SHUFFLE(3,2,1,0));
-      Packet8f tmp2 = _mm256_permute2f128_ps (tmp1, tmp1, 1);
-      first = _mm256_permute_ps(tmp2, _MM_SHUFFLE(3,2,1,0));
-    }
-    else if (Offset==5)
-    {
-      first = _mm256_blend_ps(first, second, 31);
-      first = _mm256_permute2f128_ps(first, first, 1);
-      Packet8f tmp = _mm256_permute_ps (first, _MM_SHUFFLE(0,3,2,1));
-      first = _mm256_permute2f128_ps(tmp, tmp, 1);
-      first = _mm256_blend_ps(tmp, first, 0x88);
-    }
-    else if (Offset==6)
-    {
-      first = _mm256_blend_ps(first, second, 63);
-      first = _mm256_permute2f128_ps(first, first, 1);
-      Packet8f tmp = _mm256_permute_ps (first, _MM_SHUFFLE(1,0,3,2));
-      first = _mm256_permute2f128_ps(tmp, tmp, 1);
-      first = _mm256_blend_ps(tmp, first, 0xcc);
-    }
-    else if (Offset==7)
-    {
-      first = _mm256_blend_ps(first, second, 127);
-      first = _mm256_permute2f128_ps(first, first, 1);
-      Packet8f tmp = _mm256_permute_ps (first, _MM_SHUFFLE(2,1,0,3));
-      first = _mm256_permute2f128_ps(tmp, tmp, 1);
-      first = _mm256_blend_ps(tmp, first, 0xee);
-    }
-  }
-};
-
-template<int Offset>
-struct palign_impl<Offset,Packet4d>
-{
-  static EIGEN_STRONG_INLINE void run(Packet4d& first, const Packet4d& second)
-  {
-    if (Offset==1)
-    {
-      first = _mm256_blend_pd(first, second, 1);
-      __m256d tmp = _mm256_permute_pd(first, 5);
-      first = _mm256_permute2f128_pd(tmp, tmp, 1);
-      first = _mm256_blend_pd(tmp, first, 0xA);
-    }
-    else if (Offset==2)
-    {
-      first = _mm256_blend_pd(first, second, 3);
-      first = _mm256_permute2f128_pd(first, first, 1);
-    }
-    else if (Offset==3)
-    {
-      first = _mm256_blend_pd(first, second, 7);
-      __m256d tmp = _mm256_permute_pd(first, 5);
-      first = _mm256_permute2f128_pd(tmp, tmp, 1);
-      first = _mm256_blend_pd(tmp, first, 5);
-    }
-  }
-};
+  return _mm256_movemask_ps(x)!=0;
+}
 
 EIGEN_DEVICE_FUNC inline void
 ptranspose(PacketBlock<Packet8f,8>& kernel) {
@@ -610,24 +931,640 @@
   return _mm256_blendv_pd(thenPacket, elsePacket, false_mask);
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pinsertfirst(const Packet8f& a, float b)
-{
-  return _mm256_blend_ps(a,pset1<Packet8f>(b),1);
+// Packet math for Eigen::half
+
+template<> struct unpacket_traits<Packet8h> { typedef Eigen::half type; enum {size=8, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false}; typedef Packet8h half; };
+
+template<> EIGEN_STRONG_INLINE Packet8h pset1<Packet8h>(const Eigen::half& from) {
+  return _mm_set1_epi16(numext::bit_cast<numext::uint16_t>(from));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4d pinsertfirst(const Packet4d& a, double b)
-{
-  return _mm256_blend_pd(a,pset1<Packet4d>(b),1);
+template<> EIGEN_STRONG_INLINE Eigen::half pfirst<Packet8h>(const Packet8h& from) {
+  return numext::bit_cast<Eigen::half>(static_cast<numext::uint16_t>(_mm_extract_epi16(from, 0)));
 }
 
-template<> EIGEN_STRONG_INLINE Packet8f pinsertlast(const Packet8f& a, float b)
-{
-  return _mm256_blend_ps(a,pset1<Packet8f>(b),(1<<7));
+template<> EIGEN_STRONG_INLINE Packet8h pload<Packet8h>(const Eigen::half* from) {
+  return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
 }
 
-template<> EIGEN_STRONG_INLINE Packet4d pinsertlast(const Packet4d& a, double b)
+template<> EIGEN_STRONG_INLINE Packet8h ploadu<Packet8h>(const Eigen::half* from) {
+  return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
+}
+
+template<> EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet8h& from) {
+  _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
+}
+
+template<> EIGEN_STRONG_INLINE void pstoreu<Eigen::half>(Eigen::half* to, const Packet8h& from) {
+  _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h
+ploaddup<Packet8h>(const Eigen::half*  from) {
+  const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
+  const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
+  const numext::uint16_t c = numext::bit_cast<numext::uint16_t>(from[2]);
+  const numext::uint16_t d = numext::bit_cast<numext::uint16_t>(from[3]);
+  return _mm_set_epi16(d, d, c, c, b, b, a, a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h
+ploadquad<Packet8h>(const Eigen::half* from) {
+  const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
+  const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
+  return _mm_set_epi16(b, b, b, b, a, a, a, a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h ptrue(const Packet8h& a) {
+ return _mm_cmpeq_epi32(a, a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8h pabs(const Packet8h& a) {
+  const __m128i sign_mask = _mm_set1_epi16(static_cast<numext::uint16_t>(0x8000));
+  return _mm_andnot_si128(sign_mask, a);
+}
+
+EIGEN_STRONG_INLINE Packet8f half2float(const Packet8h& a) {
+#ifdef EIGEN_HAS_FP16_C
+  return _mm256_cvtph_ps(a);
+#else
+  EIGEN_ALIGN32 Eigen::half aux[8];
+  pstore(aux, a);
+  float f0(aux[0]);
+  float f1(aux[1]);
+  float f2(aux[2]);
+  float f3(aux[3]);
+  float f4(aux[4]);
+  float f5(aux[5]);
+  float f6(aux[6]);
+  float f7(aux[7]);
+
+  return _mm256_set_ps(f7, f6, f5, f4, f3, f2, f1, f0);
+#endif
+}
+
+EIGEN_STRONG_INLINE Packet8h float2half(const Packet8f& a) {
+#ifdef EIGEN_HAS_FP16_C
+  return _mm256_cvtps_ph(a, _MM_FROUND_TO_NEAREST_INT|_MM_FROUND_NO_EXC);
+#else
+  EIGEN_ALIGN32 float aux[8];
+  pstore(aux, a);
+  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[0]));
+  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[1]));
+  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[2]));
+  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[3]));
+  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[4]));
+  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[5]));
+  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[6]));
+  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(Eigen::half(aux[7]));
+  return _mm_set_epi16(s7, s6, s5, s4, s3, s2, s1, s0);
+#endif
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8h pmin<Packet8h>(const Packet8h& a,
+                                            const Packet8h& b) {
+  return float2half(pmin<Packet8f>(half2float(a), half2float(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8h pmax<Packet8h>(const Packet8h& a,
+                                            const Packet8h& b) {
+  return float2half(pmax<Packet8f>(half2float(a), half2float(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8h plset<Packet8h>(const half& a) {
+  return float2half(plset<Packet8f>(static_cast<float>(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h por(const Packet8h& a,const Packet8h& b) {
+  // in some cases Packet4i is a wrapper around __m128i, so we either need to
+  // cast to Packet4i to directly call the intrinsics as below:
+  return _mm_or_si128(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8h pxor(const Packet8h& a,const Packet8h& b) {
+  return _mm_xor_si128(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8h pand(const Packet8h& a,const Packet8h& b) {
+  return _mm_and_si128(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8h pandnot(const Packet8h& a,const Packet8h& b) {
+  return _mm_andnot_si128(b,a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pselect(const Packet8h& mask, const Packet8h& a, const Packet8h& b) {
+  return _mm_blendv_epi8(b, a, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pround<Packet8h>(const Packet8h& a) {
+  return float2half(pround<Packet8f>(half2float(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h print<Packet8h>(const Packet8h& a) {
+  return float2half(print<Packet8f>(half2float(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pceil<Packet8h>(const Packet8h& a) {
+  return float2half(pceil<Packet8f>(half2float(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pfloor<Packet8h>(const Packet8h& a) {
+  return float2half(pfloor<Packet8f>(half2float(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pcmp_eq(const Packet8h& a,const Packet8h& b) {
+  return Pack16To8(pcmp_eq(half2float(a), half2float(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pcmp_le(const Packet8h& a,const Packet8h& b) {
+  return Pack16To8(pcmp_le(half2float(a), half2float(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pcmp_lt(const Packet8h& a,const Packet8h& b) {
+  return Pack16To8(pcmp_lt(half2float(a), half2float(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pcmp_lt_or_nan(const Packet8h& a,const Packet8h& b) {
+  return Pack16To8(pcmp_lt_or_nan(half2float(a), half2float(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pconj(const Packet8h& a) { return a; }
+
+template<> EIGEN_STRONG_INLINE Packet8h pnegate(const Packet8h& a) {
+  Packet8h sign_mask = _mm_set1_epi16(static_cast<numext::uint16_t>(0x8000));
+  return _mm_xor_si128(a, sign_mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h padd<Packet8h>(const Packet8h& a, const Packet8h& b) {
+  Packet8f af = half2float(a);
+  Packet8f bf = half2float(b);
+  Packet8f rf = padd(af, bf);
+  return float2half(rf);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h psub<Packet8h>(const Packet8h& a, const Packet8h& b) {
+  Packet8f af = half2float(a);
+  Packet8f bf = half2float(b);
+  Packet8f rf = psub(af, bf);
+  return float2half(rf);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pmul<Packet8h>(const Packet8h& a, const Packet8h& b) {
+  Packet8f af = half2float(a);
+  Packet8f bf = half2float(b);
+  Packet8f rf = pmul(af, bf);
+  return float2half(rf);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pdiv<Packet8h>(const Packet8h& a, const Packet8h& b) {
+  Packet8f af = half2float(a);
+  Packet8f bf = half2float(b);
+  Packet8f rf = pdiv(af, bf);
+  return float2half(rf);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pgather<Eigen::half, Packet8h>(const Eigen::half* from, Index stride)
 {
-  return _mm256_blend_pd(a,pset1<Packet4d>(b),(1<<3));
+  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(from[0*stride]);
+  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(from[1*stride]);
+  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(from[2*stride]);
+  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(from[3*stride]);
+  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(from[4*stride]);
+  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(from[5*stride]);
+  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(from[6*stride]);
+  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(from[7*stride]);
+  return _mm_set_epi16(s7, s6, s5, s4, s3, s2, s1, s0);
+}
+
+template<> EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet8h>(Eigen::half* to, const Packet8h& from, Index stride)
+{
+  EIGEN_ALIGN32 Eigen::half aux[8];
+  pstore(aux, from);
+  to[stride*0] = aux[0];
+  to[stride*1] = aux[1];
+  to[stride*2] = aux[2];
+  to[stride*3] = aux[3];
+  to[stride*4] = aux[4];
+  to[stride*5] = aux[5];
+  to[stride*6] = aux[6];
+  to[stride*7] = aux[7];
+}
+
+template<> EIGEN_STRONG_INLINE Eigen::half predux<Packet8h>(const Packet8h& a) {
+  Packet8f af = half2float(a);
+  float reduced = predux<Packet8f>(af);
+  return Eigen::half(reduced);
+}
+
+template<> EIGEN_STRONG_INLINE Eigen::half predux_max<Packet8h>(const Packet8h& a) {
+  Packet8f af = half2float(a);
+  float reduced = predux_max<Packet8f>(af);
+  return Eigen::half(reduced);
+}
+
+template<> EIGEN_STRONG_INLINE Eigen::half predux_min<Packet8h>(const Packet8h& a) {
+  Packet8f af = half2float(a);
+  float reduced = predux_min<Packet8f>(af);
+  return Eigen::half(reduced);
+}
+
+template<> EIGEN_STRONG_INLINE Eigen::half predux_mul<Packet8h>(const Packet8h& a) {
+  Packet8f af = half2float(a);
+  float reduced = predux_mul<Packet8f>(af);
+  return Eigen::half(reduced);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h preverse(const Packet8h& a)
+{
+  __m128i m = _mm_setr_epi8(14,15,12,13,10,11,8,9,6,7,4,5,2,3,0,1);
+  return _mm_shuffle_epi8(a,m);
+}
+
+EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet8h,8>& kernel) {
+  __m128i a = kernel.packet[0];
+  __m128i b = kernel.packet[1];
+  __m128i c = kernel.packet[2];
+  __m128i d = kernel.packet[3];
+  __m128i e = kernel.packet[4];
+  __m128i f = kernel.packet[5];
+  __m128i g = kernel.packet[6];
+  __m128i h = kernel.packet[7];
+
+  __m128i a03b03 = _mm_unpacklo_epi16(a, b);
+  __m128i c03d03 = _mm_unpacklo_epi16(c, d);
+  __m128i e03f03 = _mm_unpacklo_epi16(e, f);
+  __m128i g03h03 = _mm_unpacklo_epi16(g, h);
+  __m128i a47b47 = _mm_unpackhi_epi16(a, b);
+  __m128i c47d47 = _mm_unpackhi_epi16(c, d);
+  __m128i e47f47 = _mm_unpackhi_epi16(e, f);
+  __m128i g47h47 = _mm_unpackhi_epi16(g, h);
+
+  __m128i a01b01c01d01 = _mm_unpacklo_epi32(a03b03, c03d03);
+  __m128i a23b23c23d23 = _mm_unpackhi_epi32(a03b03, c03d03);
+  __m128i e01f01g01h01 = _mm_unpacklo_epi32(e03f03, g03h03);
+  __m128i e23f23g23h23 = _mm_unpackhi_epi32(e03f03, g03h03);
+  __m128i a45b45c45d45 = _mm_unpacklo_epi32(a47b47, c47d47);
+  __m128i a67b67c67d67 = _mm_unpackhi_epi32(a47b47, c47d47);
+  __m128i e45f45g45h45 = _mm_unpacklo_epi32(e47f47, g47h47);
+  __m128i e67f67g67h67 = _mm_unpackhi_epi32(e47f47, g47h47);
+
+  __m128i a0b0c0d0e0f0g0h0 = _mm_unpacklo_epi64(a01b01c01d01, e01f01g01h01);
+  __m128i a1b1c1d1e1f1g1h1 = _mm_unpackhi_epi64(a01b01c01d01, e01f01g01h01);
+  __m128i a2b2c2d2e2f2g2h2 = _mm_unpacklo_epi64(a23b23c23d23, e23f23g23h23);
+  __m128i a3b3c3d3e3f3g3h3 = _mm_unpackhi_epi64(a23b23c23d23, e23f23g23h23);
+  __m128i a4b4c4d4e4f4g4h4 = _mm_unpacklo_epi64(a45b45c45d45, e45f45g45h45);
+  __m128i a5b5c5d5e5f5g5h5 = _mm_unpackhi_epi64(a45b45c45d45, e45f45g45h45);
+  __m128i a6b6c6d6e6f6g6h6 = _mm_unpacklo_epi64(a67b67c67d67, e67f67g67h67);
+  __m128i a7b7c7d7e7f7g7h7 = _mm_unpackhi_epi64(a67b67c67d67, e67f67g67h67);
+
+  kernel.packet[0] = a0b0c0d0e0f0g0h0;
+  kernel.packet[1] = a1b1c1d1e1f1g1h1;
+  kernel.packet[2] = a2b2c2d2e2f2g2h2;
+  kernel.packet[3] = a3b3c3d3e3f3g3h3;
+  kernel.packet[4] = a4b4c4d4e4f4g4h4;
+  kernel.packet[5] = a5b5c5d5e5f5g5h5;
+  kernel.packet[6] = a6b6c6d6e6f6g6h6;
+  kernel.packet[7] = a7b7c7d7e7f7g7h7;
+}
+
+EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet8h,4>& kernel) {
+  EIGEN_ALIGN32 Eigen::half in[4][8];
+  pstore<Eigen::half>(in[0], kernel.packet[0]);
+  pstore<Eigen::half>(in[1], kernel.packet[1]);
+  pstore<Eigen::half>(in[2], kernel.packet[2]);
+  pstore<Eigen::half>(in[3], kernel.packet[3]);
+
+  EIGEN_ALIGN32 Eigen::half out[4][8];
+
+  for (int i = 0; i < 4; ++i) {
+    for (int j = 0; j < 4; ++j) {
+      out[i][j] = in[j][2*i];
+    }
+    for (int j = 0; j < 4; ++j) {
+      out[i][j+4] = in[j][2*i+1];
+    }
+  }
+
+  kernel.packet[0] = pload<Packet8h>(out[0]);
+  kernel.packet[1] = pload<Packet8h>(out[1]);
+  kernel.packet[2] = pload<Packet8h>(out[2]);
+  kernel.packet[3] = pload<Packet8h>(out[3]);
+}
+
+// BFloat16 implementation.
+
+EIGEN_STRONG_INLINE Packet8f Bf16ToF32(const Packet8bf& a) {
+#ifdef EIGEN_VECTORIZE_AVX2
+  __m256i extend = _mm256_cvtepu16_epi32(a);
+  return _mm256_castsi256_ps(_mm256_slli_epi32(extend, 16));
+#else
+  __m128i lo = _mm_cvtepu16_epi32(a);
+  __m128i hi = _mm_cvtepu16_epi32(_mm_srli_si128(a, 8));
+  __m128i lo_shift = _mm_slli_epi32(lo, 16);
+  __m128i hi_shift = _mm_slli_epi32(hi, 16);
+  return _mm256_castsi256_ps(_mm256_insertf128_si256(_mm256_castsi128_si256(lo_shift), hi_shift, 1));
+#endif
+}
+
+// Convert float to bfloat16 according to round-to-nearest-even/denormals algorithm.
+EIGEN_STRONG_INLINE Packet8bf F32ToBf16(const Packet8f& a) {
+  Packet8bf r;
+
+  __m256i input = _mm256_castps_si256(a);
+
+#ifdef EIGEN_VECTORIZE_AVX2
+  // uint32_t lsb = (input >> 16);
+  __m256i t = _mm256_srli_epi32(input, 16);
+  // uint32_t lsb = lsb & 1;
+  t = _mm256_and_si256(t, _mm256_set1_epi32(1));
+  // uint32_t rounding_bias = 0x7fff + lsb;
+  t = _mm256_add_epi32(t, _mm256_set1_epi32(0x7fff));
+  // input += rounding_bias;
+  t = _mm256_add_epi32(t, input);
+  // input = input >> 16;
+  t = _mm256_srli_epi32(t, 16);
+  // Check NaN before converting back to bf16
+  __m256 mask = _mm256_cmp_ps(a, a, _CMP_ORD_Q);
+  __m256i nan = _mm256_set1_epi32(0x7fc0);
+  t = _mm256_blendv_epi8(nan, t, _mm256_castps_si256(mask));
+  // output = numext::bit_cast<uint16_t>(input);
+  return _mm_packus_epi32(_mm256_extractf128_si256(t, 0),
+                         _mm256_extractf128_si256(t, 1));
+#else
+  // uint32_t lsb = (input >> 16);
+  __m128i lo = _mm_srli_epi32(_mm256_extractf128_si256(input, 0), 16);
+  __m128i hi = _mm_srli_epi32(_mm256_extractf128_si256(input, 1), 16);
+  // uint32_t lsb = lsb & 1;
+  lo = _mm_and_si128(lo, _mm_set1_epi32(1));
+  hi = _mm_and_si128(hi, _mm_set1_epi32(1));
+  // uint32_t rounding_bias = 0x7fff + lsb;
+  lo = _mm_add_epi32(lo, _mm_set1_epi32(0x7fff));
+  hi = _mm_add_epi32(hi, _mm_set1_epi32(0x7fff));
+  // input += rounding_bias;
+  lo = _mm_add_epi32(lo, _mm256_extractf128_si256(input, 0));
+  hi = _mm_add_epi32(hi, _mm256_extractf128_si256(input, 1));
+  // input = input >> 16;
+  lo = _mm_srli_epi32(lo, 16);
+  hi = _mm_srli_epi32(hi, 16);
+  // Check NaN before converting back to bf16
+  __m256 mask = _mm256_cmp_ps(a, a, _CMP_ORD_Q);
+  __m128i nan = _mm_set1_epi32(0x7fc0);
+  lo = _mm_blendv_epi8(nan, lo, _mm_castps_si128(_mm256_castps256_ps128(mask)));
+  hi = _mm_blendv_epi8(nan, hi, _mm_castps_si128(_mm256_extractf128_ps(mask, 1)));
+  // output = numext::bit_cast<uint16_t>(input);
+  return _mm_packus_epi32(lo, hi);
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pset1<Packet8bf>(const bfloat16& from) {
+  return _mm_set1_epi16(numext::bit_cast<numext::uint16_t>(from));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 pfirst<Packet8bf>(const Packet8bf& from) {
+  return numext::bit_cast<bfloat16>(static_cast<numext::uint16_t>(_mm_extract_epi16(from, 0)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pload<Packet8bf>(const bfloat16* from) {
+  return _mm_load_si128(reinterpret_cast<const __m128i*>(from));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf ploadu<Packet8bf>(const bfloat16* from) {
+  return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
+}
+
+template<> EIGEN_STRONG_INLINE void pstore<bfloat16>(bfloat16* to, const Packet8bf& from) {
+  _mm_store_si128(reinterpret_cast<__m128i*>(to), from);
+}
+
+template<> EIGEN_STRONG_INLINE void pstoreu<bfloat16>(bfloat16* to, const Packet8bf& from) {
+  _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf
+ploaddup<Packet8bf>(const bfloat16* from) {
+  const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
+  const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
+  const numext::uint16_t c = numext::bit_cast<numext::uint16_t>(from[2]);
+  const numext::uint16_t d = numext::bit_cast<numext::uint16_t>(from[3]);
+  return _mm_set_epi16(d, d, c, c, b, b, a, a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf
+ploadquad<Packet8bf>(const bfloat16* from) {
+  const numext::uint16_t a = numext::bit_cast<numext::uint16_t>(from[0]);
+  const numext::uint16_t b = numext::bit_cast<numext::uint16_t>(from[1]);
+  return _mm_set_epi16(b, b, b, b, a, a, a, a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf ptrue(const Packet8bf& a) {
+ return _mm_cmpeq_epi32(a, a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8bf pabs(const Packet8bf& a) {
+  const __m128i sign_mask = _mm_set1_epi16(static_cast<numext::uint16_t>(0x8000));
+  return _mm_andnot_si128(sign_mask, a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8bf pmin<Packet8bf>(const Packet8bf& a,
+                                                const Packet8bf& b) {
+  return F32ToBf16(pmin<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8bf pmax<Packet8bf>(const Packet8bf& a,
+                                                const Packet8bf& b) {
+  return F32ToBf16(pmax<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8bf plset<Packet8bf>(const bfloat16& a) {
+  return F32ToBf16(plset<Packet8f>(static_cast<float>(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf por(const Packet8bf& a,const Packet8bf& b) {
+  return _mm_or_si128(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8bf pxor(const Packet8bf& a,const Packet8bf& b) {
+  return _mm_xor_si128(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8bf pand(const Packet8bf& a,const Packet8bf& b) {
+  return _mm_and_si128(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8bf pandnot(const Packet8bf& a,const Packet8bf& b) {
+  return _mm_andnot_si128(b,a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pselect(const Packet8bf& mask, const Packet8bf& a, const Packet8bf& b) {
+  return _mm_blendv_epi8(b, a, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pround<Packet8bf>(const Packet8bf& a)
+{
+  return F32ToBf16(pround<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf print<Packet8bf>(const Packet8bf& a) {
+  return F32ToBf16(print<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pceil<Packet8bf>(const Packet8bf& a) {
+  return F32ToBf16(pceil<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pfloor<Packet8bf>(const Packet8bf& a) {
+  return F32ToBf16(pfloor<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pcmp_eq(const Packet8bf& a,const Packet8bf& b) {
+  return Pack16To8(pcmp_eq(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pcmp_le(const Packet8bf& a,const Packet8bf& b) {
+  return Pack16To8(pcmp_le(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pcmp_lt(const Packet8bf& a,const Packet8bf& b) {
+  return Pack16To8(pcmp_lt(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pcmp_lt_or_nan(const Packet8bf& a,const Packet8bf& b) {
+  return Pack16To8(pcmp_lt_or_nan(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pconj(const Packet8bf& a) { return a; }
+
+template<> EIGEN_STRONG_INLINE Packet8bf pnegate(const Packet8bf& a) {
+  Packet8bf sign_mask = _mm_set1_epi16(static_cast<numext::uint16_t>(0x8000));
+  return _mm_xor_si128(a, sign_mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf padd<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+  return F32ToBf16(padd<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf psub<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+  return F32ToBf16(psub<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pmul<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+  return F32ToBf16(pmul<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pdiv<Packet8bf>(const Packet8bf& a, const Packet8bf& b) {
+  return F32ToBf16(pdiv<Packet8f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+
+template<> EIGEN_STRONG_INLINE Packet8bf pgather<bfloat16, Packet8bf>(const bfloat16* from, Index stride)
+{
+  const numext::uint16_t s0 = numext::bit_cast<numext::uint16_t>(from[0*stride]);
+  const numext::uint16_t s1 = numext::bit_cast<numext::uint16_t>(from[1*stride]);
+  const numext::uint16_t s2 = numext::bit_cast<numext::uint16_t>(from[2*stride]);
+  const numext::uint16_t s3 = numext::bit_cast<numext::uint16_t>(from[3*stride]);
+  const numext::uint16_t s4 = numext::bit_cast<numext::uint16_t>(from[4*stride]);
+  const numext::uint16_t s5 = numext::bit_cast<numext::uint16_t>(from[5*stride]);
+  const numext::uint16_t s6 = numext::bit_cast<numext::uint16_t>(from[6*stride]);
+  const numext::uint16_t s7 = numext::bit_cast<numext::uint16_t>(from[7*stride]);
+  return _mm_set_epi16(s7, s6, s5, s4, s3, s2, s1, s0);
+}
+
+template<> EIGEN_STRONG_INLINE void pscatter<bfloat16, Packet8bf>(bfloat16* to, const Packet8bf& from, Index stride)
+{
+  EIGEN_ALIGN32 bfloat16 aux[8];
+  pstore(aux, from);
+  to[stride*0] = aux[0];
+  to[stride*1] = aux[1];
+  to[stride*2] = aux[2];
+  to[stride*3] = aux[3];
+  to[stride*4] = aux[4];
+  to[stride*5] = aux[5];
+  to[stride*6] = aux[6];
+  to[stride*7] = aux[7];
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux<Packet8bf>(const Packet8bf& a) {
+  return static_cast<bfloat16>(predux<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux_max<Packet8bf>(const Packet8bf& a) {
+  return static_cast<bfloat16>(predux_max<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux_min<Packet8bf>(const Packet8bf& a) {
+  return static_cast<bfloat16>(predux_min<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux_mul<Packet8bf>(const Packet8bf& a) {
+  return static_cast<bfloat16>(predux_mul<Packet8f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf preverse(const Packet8bf& a)
+{
+  __m128i m = _mm_setr_epi8(14,15,12,13,10,11,8,9,6,7,4,5,2,3,0,1);
+  return _mm_shuffle_epi8(a,m);
+}
+
+EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet8bf,8>& kernel) {
+  __m128i a = kernel.packet[0];
+  __m128i b = kernel.packet[1];
+  __m128i c = kernel.packet[2];
+  __m128i d = kernel.packet[3];
+  __m128i e = kernel.packet[4];
+  __m128i f = kernel.packet[5];
+  __m128i g = kernel.packet[6];
+  __m128i h = kernel.packet[7];
+
+  __m128i a03b03 = _mm_unpacklo_epi16(a, b);
+  __m128i c03d03 = _mm_unpacklo_epi16(c, d);
+  __m128i e03f03 = _mm_unpacklo_epi16(e, f);
+  __m128i g03h03 = _mm_unpacklo_epi16(g, h);
+  __m128i a47b47 = _mm_unpackhi_epi16(a, b);
+  __m128i c47d47 = _mm_unpackhi_epi16(c, d);
+  __m128i e47f47 = _mm_unpackhi_epi16(e, f);
+  __m128i g47h47 = _mm_unpackhi_epi16(g, h);
+
+  __m128i a01b01c01d01 = _mm_unpacklo_epi32(a03b03, c03d03);
+  __m128i a23b23c23d23 = _mm_unpackhi_epi32(a03b03, c03d03);
+  __m128i e01f01g01h01 = _mm_unpacklo_epi32(e03f03, g03h03);
+  __m128i e23f23g23h23 = _mm_unpackhi_epi32(e03f03, g03h03);
+  __m128i a45b45c45d45 = _mm_unpacklo_epi32(a47b47, c47d47);
+  __m128i a67b67c67d67 = _mm_unpackhi_epi32(a47b47, c47d47);
+  __m128i e45f45g45h45 = _mm_unpacklo_epi32(e47f47, g47h47);
+  __m128i e67f67g67h67 = _mm_unpackhi_epi32(e47f47, g47h47);
+
+  kernel.packet[0] = _mm_unpacklo_epi64(a01b01c01d01, e01f01g01h01);
+  kernel.packet[1] = _mm_unpackhi_epi64(a01b01c01d01, e01f01g01h01);
+  kernel.packet[2] = _mm_unpacklo_epi64(a23b23c23d23, e23f23g23h23);
+  kernel.packet[3] = _mm_unpackhi_epi64(a23b23c23d23, e23f23g23h23);
+  kernel.packet[4] = _mm_unpacklo_epi64(a45b45c45d45, e45f45g45h45);
+  kernel.packet[5] = _mm_unpackhi_epi64(a45b45c45d45, e45f45g45h45);
+  kernel.packet[6] = _mm_unpacklo_epi64(a67b67c67d67, e67f67g67h67);
+  kernel.packet[7] = _mm_unpackhi_epi64(a67b67c67d67, e67f67g67h67);
+}
+
+EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet8bf,4>& kernel) {
+  __m128i a = kernel.packet[0];
+  __m128i b = kernel.packet[1];
+  __m128i c = kernel.packet[2];
+  __m128i d = kernel.packet[3];
+
+  __m128i ab_03 = _mm_unpacklo_epi16(a, b);
+  __m128i cd_03 = _mm_unpacklo_epi16(c, d);
+  __m128i ab_47 = _mm_unpackhi_epi16(a, b);
+  __m128i cd_47 = _mm_unpackhi_epi16(c, d);
+
+  kernel.packet[0] = _mm_unpacklo_epi32(ab_03, cd_03);
+  kernel.packet[1] = _mm_unpackhi_epi32(ab_03, cd_03);
+  kernel.packet[2] = _mm_unpacklo_epi32(ab_47, cd_47);
+  kernel.packet[3] = _mm_unpackhi_epi32(ab_47, cd_47);
 }
 
 } // end namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
index 83bfdc6..d507fb6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/AVX/TypeCasting.h
@@ -35,15 +35,79 @@
 };
 
 
+#ifndef EIGEN_VECTORIZE_AVX512
+
+template <>
+struct type_casting_traits<Eigen::half, float> {
+  enum {
+    VectorizedCast = 1,
+    SrcCoeffRatio = 1,
+    TgtCoeffRatio = 1
+  };
+};
+
+
+template <>
+struct type_casting_traits<float, Eigen::half> {
+  enum {
+    VectorizedCast = 1,
+    SrcCoeffRatio = 1,
+    TgtCoeffRatio = 1
+  };
+};
+
+template <>
+struct type_casting_traits<bfloat16, float> {
+  enum {
+    VectorizedCast = 1,
+    SrcCoeffRatio = 1,
+    TgtCoeffRatio = 1
+  };
+};
+
+template <>
+struct type_casting_traits<float, bfloat16> {
+  enum {
+    VectorizedCast = 1,
+    SrcCoeffRatio = 1,
+    TgtCoeffRatio = 1
+  };
+};
+
+#endif  // EIGEN_VECTORIZE_AVX512
 
 template<> EIGEN_STRONG_INLINE Packet8i pcast<Packet8f, Packet8i>(const Packet8f& a) {
-  return _mm256_cvtps_epi32(a);
+  return _mm256_cvttps_epi32(a);
 }
 
 template<> EIGEN_STRONG_INLINE Packet8f pcast<Packet8i, Packet8f>(const Packet8i& a) {
   return _mm256_cvtepi32_ps(a);
 }
 
+template<> EIGEN_STRONG_INLINE Packet8i preinterpret<Packet8i,Packet8f>(const Packet8f& a) {
+  return _mm256_castps_si256(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f preinterpret<Packet8f,Packet8i>(const Packet8i& a) {
+  return _mm256_castsi256_ps(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pcast<Packet8h, Packet8f>(const Packet8h& a) {
+  return half2float(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8f pcast<Packet8bf, Packet8f>(const Packet8bf& a) {
+  return Bf16ToF32(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8h pcast<Packet8f, Packet8h>(const Packet8f& a) {
+  return float2half(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet8bf pcast<Packet8f, Packet8bf>(const Packet8f& a) {
+  return F32ToBf16(a);
+}
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h
new file mode 100644
index 0000000..1c28f4f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/BFloat16.h
@@ -0,0 +1,700 @@
+/* Copyright 2017 The TensorFlow Authors. All Rights Reserved.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+    http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+==============================================================================*/
+
+#ifndef EIGEN_BFLOAT16_H
+#define EIGEN_BFLOAT16_H
+
+#define BF16_PACKET_FUNCTION(PACKET_F, PACKET_BF16, METHOD)         \
+  template <>                                                       \
+  EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED  \
+  PACKET_BF16 METHOD<PACKET_BF16>(const PACKET_BF16& _x) {          \
+    return F32ToBf16(METHOD<PACKET_F>(Bf16ToF32(_x)));              \
+  }
+
+namespace Eigen {
+
+struct bfloat16;
+
+namespace bfloat16_impl {
+
+// Make our own __bfloat16_raw definition.
+struct __bfloat16_raw {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw() : value(0) {}
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw(unsigned short raw) : value(raw) {}
+  unsigned short value;
+};
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw raw_uint16_to_bfloat16(unsigned short value);
+template <bool AssumeArgumentIsNormalOrInfinityOrZero>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne(float ff);
+// Forward declarations of template specializations, to avoid Visual C++ 2019 errors, saying:
+// > error C2908: explicit specialization; 'float_to_bfloat16_rtne' has already been instantiated
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne<false>(float ff);
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne<true>(float ff);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float bfloat16_to_float(__bfloat16_raw h);
+
+struct bfloat16_base : public __bfloat16_raw {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16_base() {}
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16_base(const __bfloat16_raw& h) : __bfloat16_raw(h) {}
+};
+
+} // namespace bfloat16_impl
+
+// Class definition.
+struct bfloat16 : public bfloat16_impl::bfloat16_base {
+
+  typedef bfloat16_impl::__bfloat16_raw __bfloat16_raw;
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16() {}
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(const __bfloat16_raw& h) : bfloat16_impl::bfloat16_base(h) {}
+
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(bool b)
+      : bfloat16_impl::bfloat16_base(bfloat16_impl::raw_uint16_to_bfloat16(b ? 0x3f80 : 0)) {}
+
+  template<class T>
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(T val)
+      : bfloat16_impl::bfloat16_base(bfloat16_impl::float_to_bfloat16_rtne<internal::is_integral<T>::value>(static_cast<float>(val))) {}
+
+  explicit EIGEN_DEVICE_FUNC bfloat16(float f)
+      : bfloat16_impl::bfloat16_base(bfloat16_impl::float_to_bfloat16_rtne<false>(f)) {}
+
+  // Following the convention of numpy, converting between complex and
+  // float will lead to loss of imag value.
+  template<typename RealScalar>
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR bfloat16(const std::complex<RealScalar>& val)
+      : bfloat16_impl::bfloat16_base(bfloat16_impl::float_to_bfloat16_rtne<false>(static_cast<float>(val.real()))) {}
+
+  EIGEN_DEVICE_FUNC operator float() const {  // NOLINT: Allow implicit conversion to float, because it is lossless.
+    return bfloat16_impl::bfloat16_to_float(*this);
+  }
+};
+} // namespace Eigen
+
+namespace std {
+template<>
+struct numeric_limits<Eigen::bfloat16> {
+  static const bool is_specialized = true;
+  static const bool is_signed = true;
+  static const bool is_integer = false;
+  static const bool is_exact = false;
+  static const bool has_infinity = true;
+  static const bool has_quiet_NaN = true;
+  static const bool has_signaling_NaN = true;
+  static const float_denorm_style has_denorm = std::denorm_absent;
+  static const bool has_denorm_loss = false;
+  static const std::float_round_style round_style = numeric_limits<float>::round_style;
+  static const bool is_iec559 = false;
+  static const bool is_bounded = true;
+  static const bool is_modulo = false;
+  static const int digits = 8;
+  static const int digits10 = 2;
+  static const int max_digits10 = 4;
+  static const int radix = 2;
+  static const int min_exponent = numeric_limits<float>::min_exponent;
+  static const int min_exponent10 = numeric_limits<float>::min_exponent10;
+  static const int max_exponent = numeric_limits<float>::max_exponent;
+  static const int max_exponent10 = numeric_limits<float>::max_exponent10;
+  static const bool traps = numeric_limits<float>::traps;
+  static const bool tinyness_before = numeric_limits<float>::tinyness_before;
+
+  static Eigen::bfloat16 (min)() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x0080); }
+  static Eigen::bfloat16 lowest() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0xff7f); }
+  static Eigen::bfloat16 (max)() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f7f); }
+  static Eigen::bfloat16 epsilon() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x3c00); }
+  static Eigen::bfloat16 round_error() { return Eigen::bfloat16(0x3f00); }
+  static Eigen::bfloat16 infinity() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f80); }
+  static Eigen::bfloat16 quiet_NaN() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7fc0); }
+  static Eigen::bfloat16 signaling_NaN() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x7f81); }
+  static Eigen::bfloat16 denorm_min() { return Eigen::bfloat16_impl::raw_uint16_to_bfloat16(0x0001); }
+};
+
+// If std::numeric_limits<T> is specialized, should also specialize
+// std::numeric_limits<const T>, std::numeric_limits<volatile T>, and
+// std::numeric_limits<const volatile T>
+// https://stackoverflow.com/a/16519653/
+template<>
+struct numeric_limits<const Eigen::bfloat16> : numeric_limits<Eigen::bfloat16> {};
+template<>
+struct numeric_limits<volatile Eigen::bfloat16> : numeric_limits<Eigen::bfloat16> {};
+template<>
+struct numeric_limits<const volatile Eigen::bfloat16> : numeric_limits<Eigen::bfloat16> {};
+} // namespace std
+
+namespace Eigen {
+
+namespace bfloat16_impl {
+
+// We need to distinguish ‘clang as the CUDA compiler’ from ‘clang as the host compiler,
+// invoked by NVCC’ (e.g. on MacOS). The former needs to see both host and device implementation
+// of the functions, while the latter can only deal with one of them.
+#if !defined(EIGEN_HAS_NATIVE_BF16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC) // Emulate support for bfloat16 floats
+
+#if EIGEN_COMP_CLANG && defined(EIGEN_CUDACC)
+// We need to provide emulated *host-side* BF16 operators for clang.
+#pragma push_macro("EIGEN_DEVICE_FUNC")
+#undef EIGEN_DEVICE_FUNC
+#if defined(EIGEN_HAS_CUDA_BF16) && defined(EIGEN_HAS_NATIVE_BF16)
+#define EIGEN_DEVICE_FUNC __host__
+#else // both host and device need emulated ops.
+#define EIGEN_DEVICE_FUNC __host__ __device__
+#endif
+#endif
+
+// Definitions for CPUs, mostly working through conversion
+// to/from fp32.
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator + (const bfloat16& a, const bfloat16& b) {
+  return bfloat16(float(a) + float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator + (const bfloat16& a, const int& b) {
+  return bfloat16(float(a) + static_cast<float>(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator + (const int& a, const bfloat16& b) {
+  return bfloat16(static_cast<float>(a) + float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator * (const bfloat16& a, const bfloat16& b) {
+  return bfloat16(float(a) * float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator - (const bfloat16& a, const bfloat16& b) {
+  return bfloat16(float(a) - float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator / (const bfloat16& a, const bfloat16& b) {
+  return bfloat16(float(a) / float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator - (const bfloat16& a) {
+  bfloat16 result;
+  result.value = a.value ^ 0x8000;
+  return result;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator += (bfloat16& a, const bfloat16& b) {
+  a = bfloat16(float(a) + float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator *= (bfloat16& a, const bfloat16& b) {
+  a = bfloat16(float(a) * float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator -= (bfloat16& a, const bfloat16& b) {
+  a = bfloat16(float(a) - float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16& operator /= (bfloat16& a, const bfloat16& b) {
+  a = bfloat16(float(a) / float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator++(bfloat16& a) {
+  a += bfloat16(1);
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator--(bfloat16& a) {
+  a -= bfloat16(1);
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator++(bfloat16& a, int) {
+  bfloat16 original_value = a;
+  ++a;
+  return original_value;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator--(bfloat16& a, int) {
+  bfloat16 original_value = a;
+  --a;
+  return original_value;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const bfloat16& a, const bfloat16& b) {
+  return numext::equal_strict(float(a),float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const bfloat16& a, const bfloat16& b) {
+  return numext::not_equal_strict(float(a), float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const bfloat16& a, const bfloat16& b) {
+  return float(a) < float(b);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator <= (const bfloat16& a, const bfloat16& b) {
+  return float(a) <= float(b);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator > (const bfloat16& a, const bfloat16& b) {
+  return float(a) > float(b);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator >= (const bfloat16& a, const bfloat16& b) {
+  return float(a) >= float(b);
+}
+
+#if EIGEN_COMP_CLANG && defined(EIGEN_CUDACC)
+#pragma pop_macro("EIGEN_DEVICE_FUNC")
+#endif
+#endif  // Emulate support for bfloat16 floats
+
+// Division by an index. Do it in full float precision to avoid accuracy
+// issues in converting the denominator to bfloat16.
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 operator / (const bfloat16& a, Index b) {
+  return bfloat16(static_cast<float>(a) / static_cast<float>(b));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw truncate_to_bfloat16(const float v) {
+  __bfloat16_raw output;
+  if (Eigen::numext::isnan EIGEN_NOT_A_MACRO(v)) {
+    output.value = std::signbit(v) ? 0xFFC0: 0x7FC0;
+    return output;
+  }
+  const uint16_t* p = reinterpret_cast<const uint16_t*>(&v);
+#if defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+  output.value = p[0];
+#else
+  output.value = p[1];
+#endif
+  return output;
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __bfloat16_raw raw_uint16_to_bfloat16(numext::uint16_t value) {
+  return __bfloat16_raw(value);
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR numext::uint16_t raw_bfloat16_as_uint16(const __bfloat16_raw& bf) {
+  return bf.value;
+}
+
+// float_to_bfloat16_rtne template specialization that does not make any
+// assumption about the value of its function argument (ff).
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne<false>(float ff) {
+#if (defined(EIGEN_HAS_CUDA_BF16) && defined(EIGEN_HAS_HIP_BF16))
+  // Nothing to do here
+#else
+  __bfloat16_raw output;
+
+  if (Eigen::numext::isnan EIGEN_NOT_A_MACRO(ff)) {
+    // If the value is a NaN, squash it to a qNaN with msb of fraction set,
+    // this makes sure after truncation we don't end up with an inf.
+    //
+    // qNaN magic: All exponent bits set + most significant bit of fraction
+    // set.
+    output.value = std::signbit(ff) ? 0xFFC0: 0x7FC0;
+  } else {
+    // Fast rounding algorithm that rounds a half value to nearest even. This
+    // reduces expected error when we convert a large number of floats. Here
+    // is how it works:
+    //
+    // Definitions:
+    // To convert a float 32 to bfloat16, a float 32 can be viewed as 32 bits
+    // with the following tags:
+    //
+    // Sign |  Exp (8 bits) | Frac (23 bits)
+    //  S     EEEEEEEE         FFFFFFLRTTTTTTTTTTTTTTT
+    //
+    //  S: Sign bit.
+    //  E: Exponent bits.
+    //  F: First 6 bits of fraction.
+    //  L: Least significant bit of resulting bfloat16 if we truncate away the
+    //  rest of the float32. This is also the 7th bit of fraction
+    //  R: Rounding bit, 8th bit of fraction.
+    //  T: Sticky bits, rest of fraction, 15 bits.
+    //
+    // To round half to nearest even, there are 3 cases where we want to round
+    // down (simply truncate the result of the bits away, which consists of
+    // rounding bit and sticky bits) and two cases where we want to round up
+    // (truncate then add one to the result).
+    //
+    // The fast converting algorithm simply adds lsb (L) to 0x7fff (15 bits of
+    // 1s) as the rounding bias, adds the rounding bias to the input, then
+    // truncates the last 16 bits away.
+    //
+    // To understand how it works, we can analyze this algorithm case by case:
+    //
+    // 1. L = 0, R = 0:
+    //   Expect: round down, this is less than half value.
+    //
+    //   Algorithm:
+    //   - Rounding bias: 0x7fff + 0 = 0x7fff
+    //   - Adding rounding bias to input may create any carry, depending on
+    //   whether there is any value set to 1 in T bits.
+    //   - R may be set to 1 if there is a carry.
+    //   - L remains 0.
+    //   - Note that this case also handles Inf and -Inf, where all fraction
+    //   bits, including L, R and Ts are all 0. The output remains Inf after
+    //   this algorithm.
+    //
+    // 2. L = 1, R = 0:
+    //   Expect: round down, this is less than half value.
+    //
+    //   Algorithm:
+    //   - Rounding bias: 0x7fff + 1 = 0x8000
+    //   - Adding rounding bias to input doesn't change sticky bits but
+    //   adds 1 to rounding bit.
+    //   - L remains 1.
+    //
+    // 3. L = 0, R = 1, all of T are 0:
+    //   Expect: round down, this is exactly at half, the result is already
+    //   even (L=0).
+    //
+    //   Algorithm:
+    //   - Rounding bias: 0x7fff + 0 = 0x7fff
+    //   - Adding rounding bias to input sets all sticky bits to 1, but
+    //   doesn't create a carry.
+    //   - R remains 1.
+    //   - L remains 0.
+    //
+    // 4. L = 1, R = 1:
+    //   Expect: round up, this is exactly at half, the result needs to be
+    //   round to the next even number.
+    //
+    //   Algorithm:
+    //   - Rounding bias: 0x7fff + 1 = 0x8000
+    //   - Adding rounding bias to input doesn't change sticky bits, but
+    //   creates a carry from rounding bit.
+    //   - The carry sets L to 0, creates another carry bit and propagate
+    //   forward to F bits.
+    //   - If all the F bits are 1, a carry then propagates to the exponent
+    //   bits, which then creates the minimum value with the next exponent
+    //   value. Note that we won't have the case where exponents are all 1,
+    //   since that's either a NaN (handled in the other if condition) or inf
+    //   (handled in case 1).
+    //
+    // 5. L = 0, R = 1, any of T is 1:
+    //   Expect: round up, this is greater than half.
+    //
+    //   Algorithm:
+    //   - Rounding bias: 0x7fff + 0 = 0x7fff
+    //   - Adding rounding bias to input creates a carry from sticky bits,
+    //   sets rounding bit to 0, then create another carry.
+    //   - The second carry sets L to 1.
+    //
+    // Examples:
+    //
+    //  Exact half value that is already even:
+    //    Input:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit) | Frac (last 16 bit)
+    //     S     E E E E E E E E      F F F F F F L     RTTTTTTTTTTTTTTT
+    //     0     0 0 0 0 0 0 0 0      0 0 0 0 0 1 0     1000000000000000
+    //
+    //     This falls into case 3. We truncate the rest of 16 bits and no
+    //     carry is created into F and L:
+    //
+    //    Output:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit)
+    //     S     E E E E E E E E      F F F F F F L
+    //     0     0 0 0 0 0 0 0 0      0 0 0 0 0 1 0
+    //
+    //  Exact half value, round to next even number:
+    //    Input:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit) | Frac (last 16 bit)
+    //     S     E E E E E E E E      F F F F F F L     RTTTTTTTTTTTTTTT
+    //     0     0 0 0 0 0 0 0 0      0 0 0 0 0 0 1     1000000000000000
+    //
+    //     This falls into case 4. We create a carry from R and T,
+    //     which then propagates into L and F:
+    //
+    //    Output:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit)
+    //     S     E E E E E E E E      F F F F F F L
+    //     0     0 0 0 0 0 0 0 0      0 0 0 0 0 1 0
+    //
+    //
+    //  Max denormal value round to min normal value:
+    //    Input:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit) | Frac (last 16 bit)
+    //     S     E E E E E E E E      F F F F F F L     RTTTTTTTTTTTTTTT
+    //     0     0 0 0 0 0 0 0 0      1 1 1 1 1 1 1     1111111111111111
+    //
+    //     This falls into case 4. We create a carry from R and T,
+    //     propagate into L and F, which then propagates into exponent
+    //     bits:
+    //
+    //    Output:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit)
+    //     S     E E E E E E E E      F F F F F F L
+    //     0     0 0 0 0 0 0 0 1      0 0 0 0 0 0 0
+    //
+    //  Max normal value round to Inf:
+    //    Input:
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit) | Frac (last 16 bit)
+    //     S     E E E E E E E E      F F F F F F L     RTTTTTTTTTTTTTTT
+    //     0     1 1 1 1 1 1 1 0      1 1 1 1 1 1 1     1111111111111111
+    //
+    //     This falls into case 4. We create a carry from R and T,
+    //     propagate into L and F, which then propagates into exponent
+    //     bits:
+    //
+    //    Sign |  Exp (8 bit)     | Frac (first 7 bit)
+    //     S     E E E E E E E E      F F F F F F L
+    //     0     1 1 1 1 1 1 1 1      0 0 0 0 0 0 0
+
+    // At this point, ff must be either a normal float, or +/-infinity.
+    output = float_to_bfloat16_rtne<true>(ff);
+  }
+  return output;
+#endif
+}
+
+// float_to_bfloat16_rtne template specialization that assumes that its function
+// argument (ff) is either a normal floating point number, or +/-infinity, or
+// zero. Used to improve the runtime performance of conversion from an integer
+// type to bfloat16.
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __bfloat16_raw float_to_bfloat16_rtne<true>(float ff) {
+#if (defined(EIGEN_HAS_CUDA_BF16) && defined(EIGEN_HAS_HIP_BF16))
+    // Nothing to do here
+#else
+    numext::uint32_t input = numext::bit_cast<numext::uint32_t>(ff);
+    __bfloat16_raw output;
+
+    // Least significant bit of resulting bfloat.
+    numext::uint32_t lsb = (input >> 16) & 1;
+    numext::uint32_t rounding_bias = 0x7fff + lsb;
+    input += rounding_bias;
+    output.value = static_cast<numext::uint16_t>(input >> 16);
+    return output;
+#endif
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float bfloat16_to_float(__bfloat16_raw h) {
+    float result = 0;
+    unsigned short* q = reinterpret_cast<unsigned short*>(&result);
+#if defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+    q[0] = h.value;
+#else
+    q[1] = h.value;
+#endif
+    return result;
+}
+// --- standard functions ---
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isinf)(const bfloat16& a) {
+  EIGEN_USING_STD(isinf);
+  return (isinf)(float(a));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isnan)(const bfloat16& a) {
+  EIGEN_USING_STD(isnan);
+  return (isnan)(float(a));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isfinite)(const bfloat16& a) {
+  return !(isinf EIGEN_NOT_A_MACRO (a)) && !(isnan EIGEN_NOT_A_MACRO (a));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 abs(const bfloat16& a) {
+  bfloat16 result;
+  result.value = a.value & 0x7FFF;
+  return result;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 exp(const bfloat16& a) {
+   return bfloat16(::expf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 expm1(const bfloat16& a) {
+  return bfloat16(numext::expm1(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log(const bfloat16& a) {
+  return bfloat16(::logf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log1p(const bfloat16& a) {
+  return bfloat16(numext::log1p(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log10(const bfloat16& a) {
+  return bfloat16(::log10f(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 log2(const bfloat16& a) {
+  return bfloat16(static_cast<float>(EIGEN_LOG2E) * ::logf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sqrt(const bfloat16& a) {
+    return bfloat16(::sqrtf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 pow(const bfloat16& a, const bfloat16& b) {
+  return bfloat16(::powf(float(a), float(b)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sin(const bfloat16& a) {
+  return bfloat16(::sinf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 cos(const bfloat16& a) {
+  return bfloat16(::cosf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 tan(const bfloat16& a) {
+  return bfloat16(::tanf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 asin(const bfloat16& a) {
+  return bfloat16(::asinf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 acos(const bfloat16& a) {
+  return bfloat16(::acosf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atan(const bfloat16& a) {
+  return bfloat16(::atanf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 sinh(const bfloat16& a) {
+  return bfloat16(::sinhf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 cosh(const bfloat16& a) {
+  return bfloat16(::coshf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 tanh(const bfloat16& a) {
+  return bfloat16(::tanhf(float(a)));
+}
+#if EIGEN_HAS_CXX11_MATH
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 asinh(const bfloat16& a) {
+  return bfloat16(::asinhf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 acosh(const bfloat16& a) {
+  return bfloat16(::acoshf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 atanh(const bfloat16& a) {
+  return bfloat16(::atanhf(float(a)));
+}
+#endif
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 floor(const bfloat16& a) {
+  return bfloat16(::floorf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 ceil(const bfloat16& a) {
+  return bfloat16(::ceilf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 rint(const bfloat16& a) {
+  return bfloat16(::rintf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 round(const bfloat16& a) {
+  return bfloat16(::roundf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmod(const bfloat16& a, const bfloat16& b) {
+  return bfloat16(::fmodf(float(a), float(b)));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 (min)(const bfloat16& a, const bfloat16& b) {
+  const float f1 = static_cast<float>(a);
+  const float f2 = static_cast<float>(b);
+  return f2 < f1 ? b : a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 (max)(const bfloat16& a, const bfloat16& b) {
+  const float f1 = static_cast<float>(a);
+  const float f2 = static_cast<float>(b);
+  return f1 < f2 ? b : a;
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmin(const bfloat16& a, const bfloat16& b) {
+  const float f1 = static_cast<float>(a);
+  const float f2 = static_cast<float>(b);
+  return bfloat16(::fminf(f1, f2));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bfloat16 fmax(const bfloat16& a, const bfloat16& b) {
+  const float f1 = static_cast<float>(a);
+  const float f2 = static_cast<float>(b);
+  return bfloat16(::fmaxf(f1, f2));
+}
+
+#ifndef EIGEN_NO_IO
+EIGEN_ALWAYS_INLINE std::ostream& operator << (std::ostream& os, const bfloat16& v) {
+  os << static_cast<float>(v);
+  return os;
+}
+#endif
+
+} // namespace bfloat16_impl
+
+namespace internal {
+
+template<>
+struct random_default_impl<bfloat16, false, false>
+{
+  static inline bfloat16 run(const bfloat16& x, const bfloat16& y)
+  {
+    return x + (y-x) * bfloat16(float(std::rand()) / float(RAND_MAX));
+  }
+  static inline bfloat16 run()
+  {
+    return run(bfloat16(-1.f), bfloat16(1.f));
+  }
+};
+
+template<> struct is_arithmetic<bfloat16> { enum { value = true }; };
+
+} // namespace internal
+
+template<> struct NumTraits<Eigen::bfloat16>
+    : GenericNumTraits<Eigen::bfloat16>
+{
+  enum {
+    IsSigned = true,
+    IsInteger = false,
+    IsComplex = false,
+    RequireInitialization = false
+  };
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 epsilon() {
+    return bfloat16_impl::raw_uint16_to_bfloat16(0x3c00);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 dummy_precision() {
+    return bfloat16_impl::raw_uint16_to_bfloat16(0x3D4D);  // bfloat16(5e-2f);
+
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 highest() {
+    return bfloat16_impl::raw_uint16_to_bfloat16(0x7F7F);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 lowest() {
+    return bfloat16_impl::raw_uint16_to_bfloat16(0xFF7F);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 infinity() {
+    return bfloat16_impl::raw_uint16_to_bfloat16(0x7f80);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::bfloat16 quiet_NaN() {
+    return bfloat16_impl::raw_uint16_to_bfloat16(0x7fc0);
+  }
+};
+
+} // namespace Eigen
+
+namespace Eigen {
+namespace numext {
+
+template<>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+bool (isnan)(const Eigen::bfloat16& h) {
+  return (bfloat16_impl::isnan)(h);
+}
+
+template<>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+bool (isinf)(const Eigen::bfloat16& h) {
+  return (bfloat16_impl::isinf)(h);
+}
+
+template<>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE
+bool (isfinite)(const Eigen::bfloat16& h) {
+  return (bfloat16_impl::isfinite)(h);
+}
+
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::bfloat16 bit_cast<Eigen::bfloat16, uint16_t>(const uint16_t& src) {
+  return Eigen::bfloat16(Eigen::bfloat16_impl::raw_uint16_to_bfloat16(src));
+}
+
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC uint16_t bit_cast<uint16_t, Eigen::bfloat16>(const Eigen::bfloat16& src) {
+  return Eigen::bfloat16_impl::raw_bfloat16_as_uint16(src);
+}
+
+}  // namespace numext
+}  // namespace Eigen
+
+#if EIGEN_HAS_STD_HASH
+namespace std {
+template <>
+struct hash<Eigen::bfloat16> {
+  EIGEN_STRONG_INLINE std::size_t operator()(const Eigen::bfloat16& a) const {
+    return static_cast<std::size_t>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(a));
+  }
+};
+} // namespace std
+#endif
+
+
+#endif // EIGEN_BFLOAT16_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
index 4cfe34e..53830b5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/ConjHelper.h
@@ -11,19 +11,107 @@
 #ifndef EIGEN_ARCH_CONJ_HELPER_H
 #define EIGEN_ARCH_CONJ_HELPER_H
 
-#define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL)                                                          \
-  template<> struct conj_helper<PACKET_REAL, PACKET_CPLX, false,false> {                                          \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x, const PACKET_CPLX& y, const PACKET_CPLX& c) const \
-    { return padd(c, pmul(x,y)); }                                                                                \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x, const PACKET_CPLX& y) const                        \
-    { return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x, y.v)); }                                           \
-  };                                                                                                              \
-                                                                                                                  \
-  template<> struct conj_helper<PACKET_CPLX, PACKET_REAL, false,false> {                                          \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x, const PACKET_REAL& y, const PACKET_CPLX& c) const \
-    { return padd(c, pmul(x,y)); }                                                                                \
-    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x, const PACKET_REAL& y) const                        \
-    { return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x.v, y)); }                                           \
+#define EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(PACKET_CPLX, PACKET_REAL)      \
+  template <>                                                           \
+  struct conj_helper<PACKET_REAL, PACKET_CPLX, false, false> {          \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_REAL& x,         \
+                                          const PACKET_CPLX& y,         \
+                                          const PACKET_CPLX& c) const { \
+      return padd(c, this->pmul(x, y));                                 \
+    }                                                                   \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_REAL& x,          \
+                                         const PACKET_CPLX& y) const {  \
+      return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x, y.v));   \
+    }                                                                   \
+  };                                                                    \
+                                                                        \
+  template <>                                                           \
+  struct conj_helper<PACKET_CPLX, PACKET_REAL, false, false> {          \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmadd(const PACKET_CPLX& x,         \
+                                          const PACKET_REAL& y,         \
+                                          const PACKET_CPLX& c) const { \
+      return padd(c, this->pmul(x, y));                                 \
+    }                                                                   \
+    EIGEN_STRONG_INLINE PACKET_CPLX pmul(const PACKET_CPLX& x,          \
+                                         const PACKET_REAL& y) const {  \
+      return PACKET_CPLX(Eigen::internal::pmul<PACKET_REAL>(x.v, y));   \
+    }                                                                   \
   };
 
-#endif // EIGEN_ARCH_CONJ_HELPER_H
+namespace Eigen {
+namespace internal {
+
+template<bool Conjugate> struct conj_if;
+
+template<> struct conj_if<true> {
+  template<typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const { return numext::conj(x); }
+  template<typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T pconj(const T& x) const { return internal::pconj(x); }
+};
+
+template<> struct conj_if<false> {
+  template<typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& operator()(const T& x) const { return x; }
+  template<typename T>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const T& pconj(const T& x) const { return x; }
+};
+
+// Generic Implementation, assume scalars since the packet-version is
+// specialized below.
+template<typename LhsType, typename RhsType, bool ConjLhs, bool ConjRhs>
+struct conj_helper {
+  typedef typename ScalarBinaryOpTraits<LhsType, RhsType>::ReturnType ResultType;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
+  pmadd(const LhsType& x, const RhsType& y, const ResultType& c) const
+  { return this->pmul(x, y) + c; }
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
+  pmul(const LhsType& x, const RhsType& y) const
+  { return conj_if<ConjLhs>()(x) * conj_if<ConjRhs>()(y); }
+};
+
+template<typename LhsScalar, typename RhsScalar>
+struct conj_helper<LhsScalar, RhsScalar, true, true> {
+  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar>::ReturnType ResultType;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
+  pmadd(const LhsScalar& x, const RhsScalar& y, const ResultType& c) const
+  { return this->pmul(x, y) + c; }
+
+  // We save a conjuation by using the identity conj(a)*conj(b) = conj(a*b).
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE ResultType
+  pmul(const LhsScalar& x, const RhsScalar& y) const
+  { return numext::conj(x * y); }
+};
+
+// Implementation with equal type, use packet operations.
+template<typename Packet, bool ConjLhs, bool ConjRhs>
+struct conj_helper<Packet, Packet, ConjLhs, ConjRhs>
+{
+  typedef Packet ResultType;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmadd(const Packet& x, const Packet& y, const Packet& c) const
+  { return Eigen::internal::pmadd(conj_if<ConjLhs>().pconj(x), conj_if<ConjRhs>().pconj(y), c); }
+
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmul(const Packet& x, const Packet& y) const
+  { return Eigen::internal::pmul(conj_if<ConjLhs>().pconj(x), conj_if<ConjRhs>().pconj(y)); }
+};
+
+template<typename Packet>
+struct conj_helper<Packet, Packet, true, true>
+{
+  typedef Packet ResultType;
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmadd(const Packet& x, const Packet& y, const Packet& c) const
+  { return Eigen::internal::pmadd(pconj(x), pconj(y), c); }
+  // We save a conjuation by using the identity conj(a)*conj(b) = conj(a*b).
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet pmul(const Packet& x, const Packet& y) const
+  { return pconj(Eigen::internal::pmul(x, y)); }
+};
+
+}  // namespace internal
+}  // namespace Eigen
+
+#endif  // EIGEN_ARCH_CONJ_HELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
new file mode 100644
index 0000000..c9fbaf6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
@@ -0,0 +1,1649 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2007 Julien Pommier
+// Copyright (C) 2014 Pedro Gonnet (pedro.gonnet@gmail.com)
+// Copyright (C) 2009-2019 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+/* The exp and log functions of this file initially come from
+ * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
+ */
+
+#ifndef EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
+#define EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
+
+namespace Eigen {
+namespace internal {
+
+// Creates a Scalar integer type with same bit-width.
+template<typename T> struct make_integer;
+template<> struct make_integer<float>    { typedef numext::int32_t type; };
+template<> struct make_integer<double>   { typedef numext::int64_t type; };
+template<> struct make_integer<half>     { typedef numext::int16_t type; };
+template<> struct make_integer<bfloat16> { typedef numext::int16_t type; };
+
+template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC  
+Packet pfrexp_generic_get_biased_exponent(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  typedef typename unpacket_traits<Packet>::integer_packet PacketI;
+  enum { mantissa_bits = numext::numeric_limits<Scalar>::digits - 1};
+  return pcast<PacketI, Packet>(plogical_shift_right<mantissa_bits>(preinterpret<PacketI>(pabs(a))));
+}
+
+// Safely applies frexp, correctly handles denormals.
+// Assumes IEEE floating point format.
+template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+Packet pfrexp_generic(const Packet& a, Packet& exponent) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  typedef typename make_unsigned<typename make_integer<Scalar>::type>::type ScalarUI;
+  enum {
+    TotalBits = sizeof(Scalar) * CHAR_BIT,
+    MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
+    ExponentBits = int(TotalBits) - int(MantissaBits) - 1
+  };
+
+  EIGEN_CONSTEXPR ScalarUI scalar_sign_mantissa_mask = 
+      ~(((ScalarUI(1) << int(ExponentBits)) - ScalarUI(1)) << int(MantissaBits)); // ~0x7f800000
+  const Packet sign_mantissa_mask = pset1frombits<Packet>(static_cast<ScalarUI>(scalar_sign_mantissa_mask)); 
+  const Packet half = pset1<Packet>(Scalar(0.5));
+  const Packet zero = pzero(a);
+  const Packet normal_min = pset1<Packet>((numext::numeric_limits<Scalar>::min)()); // Minimum normal value, 2^-126
+  
+  // To handle denormals, normalize by multiplying by 2^(int(MantissaBits)+1).
+  const Packet is_denormal = pcmp_lt(pabs(a), normal_min);
+  EIGEN_CONSTEXPR ScalarUI scalar_normalization_offset = ScalarUI(int(MantissaBits) + 1); // 24
+  // The following cannot be constexpr because bfloat16(uint16_t) is not constexpr.
+  const Scalar scalar_normalization_factor = Scalar(ScalarUI(1) << int(scalar_normalization_offset)); // 2^24
+  const Packet normalization_factor = pset1<Packet>(scalar_normalization_factor);  
+  const Packet normalized_a = pselect(is_denormal, pmul(a, normalization_factor), a);
+  
+  // Determine exponent offset: -126 if normal, -126-24 if denormal
+  const Scalar scalar_exponent_offset = -Scalar((ScalarUI(1)<<(int(ExponentBits)-1)) - ScalarUI(2)); // -126
+  Packet exponent_offset = pset1<Packet>(scalar_exponent_offset);
+  const Packet normalization_offset = pset1<Packet>(-Scalar(scalar_normalization_offset)); // -24
+  exponent_offset = pselect(is_denormal, padd(exponent_offset, normalization_offset), exponent_offset);
+  
+  // Determine exponent and mantissa from normalized_a.
+  exponent = pfrexp_generic_get_biased_exponent(normalized_a);
+  // Zero, Inf and NaN return 'a' unmodified, exponent is zero
+  // (technically the exponent is unspecified for inf/NaN, but GCC/Clang set it to zero)
+  const Scalar scalar_non_finite_exponent = Scalar((ScalarUI(1) << int(ExponentBits)) - ScalarUI(1));  // 255
+  const Packet non_finite_exponent = pset1<Packet>(scalar_non_finite_exponent);
+  const Packet is_zero_or_not_finite = por(pcmp_eq(a, zero), pcmp_eq(exponent, non_finite_exponent));
+  const Packet m = pselect(is_zero_or_not_finite, a, por(pand(normalized_a, sign_mantissa_mask), half));
+  exponent = pselect(is_zero_or_not_finite, zero, padd(exponent, exponent_offset));  
+  return m;
+}
+
+// Safely applies ldexp, correctly handles overflows, underflows and denormals.
+// Assumes IEEE floating point format.
+template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+Packet pldexp_generic(const Packet& a, const Packet& exponent) {
+  // We want to return a * 2^exponent, allowing for all possible integer
+  // exponents without overflowing or underflowing in intermediate
+  // computations.
+  //
+  // Since 'a' and the output can be denormal, the maximum range of 'exponent'
+  // to consider for a float is:
+  //   -255-23 -> 255+23
+  // Below -278 any finite float 'a' will become zero, and above +278 any
+  // finite float will become inf, including when 'a' is the smallest possible 
+  // denormal.
+  //
+  // Unfortunately, 2^(278) cannot be represented using either one or two
+  // finite normal floats, so we must split the scale factor into at least
+  // three parts. It turns out to be faster to split 'exponent' into four
+  // factors, since [exponent>>2] is much faster to compute that [exponent/3].
+  //
+  // Set e = min(max(exponent, -278), 278);
+  //     b = floor(e/4);
+  //   out = ((((a * 2^(b)) * 2^(b)) * 2^(b)) * 2^(e-3*b))
+  //
+  // This will avoid any intermediate overflows and correctly handle 0, inf,
+  // NaN cases.
+  typedef typename unpacket_traits<Packet>::integer_packet PacketI;
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  typedef typename unpacket_traits<PacketI>::type ScalarI;
+  enum {
+    TotalBits = sizeof(Scalar) * CHAR_BIT,
+    MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
+    ExponentBits = int(TotalBits) - int(MantissaBits) - 1
+  };
+
+  const Packet max_exponent = pset1<Packet>(Scalar((ScalarI(1)<<int(ExponentBits)) + ScalarI(int(MantissaBits) - 1)));  // 278
+  const PacketI bias = pset1<PacketI>((ScalarI(1)<<(int(ExponentBits)-1)) - ScalarI(1));  // 127
+  const PacketI e = pcast<Packet, PacketI>(pmin(pmax(exponent, pnegate(max_exponent)), max_exponent));
+  PacketI b = parithmetic_shift_right<2>(e); // floor(e/4);
+  Packet c = preinterpret<Packet>(plogical_shift_left<int(MantissaBits)>(padd(b, bias)));  // 2^b
+  Packet out = pmul(pmul(pmul(a, c), c), c);  // a * 2^(3b)
+  b = psub(psub(psub(e, b), b), b); // e - 3b
+  c = preinterpret<Packet>(plogical_shift_left<int(MantissaBits)>(padd(b, bias)));  // 2^(e-3*b)
+  out = pmul(out, c);
+  return out;
+}
+
+// Explicitly multiplies 
+//    a * (2^e)
+// clamping e to the range
+// [NumTraits<Scalar>::min_exponent()-2, NumTraits<Scalar>::max_exponent()]
+//
+// This is approx 7x faster than pldexp_impl, but will prematurely over/underflow
+// if 2^e doesn't fit into a normal floating-point Scalar.
+//
+// Assumes IEEE floating point format
+template<typename Packet>
+struct pldexp_fast_impl {
+  typedef typename unpacket_traits<Packet>::integer_packet PacketI;
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  typedef typename unpacket_traits<PacketI>::type ScalarI;
+  enum {
+    TotalBits = sizeof(Scalar) * CHAR_BIT,
+    MantissaBits = numext::numeric_limits<Scalar>::digits - 1,
+    ExponentBits = int(TotalBits) - int(MantissaBits) - 1
+  };
+  
+  static EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+  Packet run(const Packet& a, const Packet& exponent) {
+    const Packet bias = pset1<Packet>(Scalar((ScalarI(1)<<(int(ExponentBits)-1)) - ScalarI(1)));  // 127
+    const Packet limit = pset1<Packet>(Scalar((ScalarI(1)<<int(ExponentBits)) - ScalarI(1)));     // 255
+    // restrict biased exponent between 0 and 255 for float.
+    const PacketI e = pcast<Packet, PacketI>(pmin(pmax(padd(exponent, bias), pzero(limit)), limit)); // exponent + 127
+    // return a * (2^e)
+    return pmul(a, preinterpret<Packet>(plogical_shift_left<int(MantissaBits)>(e)));
+  }
+};
+
+// Natural or base 2 logarithm.
+// Computes log(x) as log(2^e * m) = C*e + log(m), where the constant C =log(2)
+// and m is in the range [sqrt(1/2),sqrt(2)). In this range, the logarithm can
+// be easily approximated by a polynomial centered on m=1 for stability.
+// TODO(gonnet): Further reduce the interval allowing for lower-degree
+//               polynomial interpolants -> ... -> profit!
+template <typename Packet, bool base2>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog_impl_float(const Packet _x)
+{
+  Packet x = _x;
+
+  const Packet cst_1              = pset1<Packet>(1.0f);
+  const Packet cst_neg_half       = pset1<Packet>(-0.5f);
+  // The smallest non denormalized float number.
+  const Packet cst_min_norm_pos   = pset1frombits<Packet>( 0x00800000u);
+  const Packet cst_minus_inf      = pset1frombits<Packet>( 0xff800000u);
+  const Packet cst_pos_inf        = pset1frombits<Packet>( 0x7f800000u);
+
+  // Polynomial coefficients.
+  const Packet cst_cephes_SQRTHF = pset1<Packet>(0.707106781186547524f);
+  const Packet cst_cephes_log_p0 = pset1<Packet>(7.0376836292E-2f);
+  const Packet cst_cephes_log_p1 = pset1<Packet>(-1.1514610310E-1f);
+  const Packet cst_cephes_log_p2 = pset1<Packet>(1.1676998740E-1f);
+  const Packet cst_cephes_log_p3 = pset1<Packet>(-1.2420140846E-1f);
+  const Packet cst_cephes_log_p4 = pset1<Packet>(+1.4249322787E-1f);
+  const Packet cst_cephes_log_p5 = pset1<Packet>(-1.6668057665E-1f);
+  const Packet cst_cephes_log_p6 = pset1<Packet>(+2.0000714765E-1f);
+  const Packet cst_cephes_log_p7 = pset1<Packet>(-2.4999993993E-1f);
+  const Packet cst_cephes_log_p8 = pset1<Packet>(+3.3333331174E-1f);
+
+  // Truncate input values to the minimum positive normal.
+  x = pmax(x, cst_min_norm_pos);
+
+  Packet e;
+  // extract significant in the range [0.5,1) and exponent
+  x = pfrexp(x,e);
+
+  // part2: Shift the inputs from the range [0.5,1) to [sqrt(1/2),sqrt(2))
+  // and shift by -1. The values are then centered around 0, which improves
+  // the stability of the polynomial evaluation.
+  //   if( x < SQRTHF ) {
+  //     e -= 1;
+  //     x = x + x - 1.0;
+  //   } else { x = x - 1.0; }
+  Packet mask = pcmp_lt(x, cst_cephes_SQRTHF);
+  Packet tmp = pand(x, mask);
+  x = psub(x, cst_1);
+  e = psub(e, pand(cst_1, mask));
+  x = padd(x, tmp);
+
+  Packet x2 = pmul(x, x);
+  Packet x3 = pmul(x2, x);
+
+  // Evaluate the polynomial approximant of degree 8 in three parts, probably
+  // to improve instruction-level parallelism.
+  Packet y, y1, y2;
+  y  = pmadd(cst_cephes_log_p0, x, cst_cephes_log_p1);
+  y1 = pmadd(cst_cephes_log_p3, x, cst_cephes_log_p4);
+  y2 = pmadd(cst_cephes_log_p6, x, cst_cephes_log_p7);
+  y  = pmadd(y, x, cst_cephes_log_p2);
+  y1 = pmadd(y1, x, cst_cephes_log_p5);
+  y2 = pmadd(y2, x, cst_cephes_log_p8);
+  y  = pmadd(y, x3, y1);
+  y  = pmadd(y, x3, y2);
+  y  = pmul(y, x3);
+
+  y = pmadd(cst_neg_half, x2, y);
+  x = padd(x, y);
+
+  // Add the logarithm of the exponent back to the result of the interpolation.
+  if (base2) {
+    const Packet cst_log2e = pset1<Packet>(static_cast<float>(EIGEN_LOG2E));
+    x = pmadd(x, cst_log2e, e);
+  } else {
+    const Packet cst_ln2 = pset1<Packet>(static_cast<float>(EIGEN_LN2));
+    x = pmadd(e, cst_ln2, x);
+  }
+
+  Packet invalid_mask = pcmp_lt_or_nan(_x, pzero(_x));
+  Packet iszero_mask  = pcmp_eq(_x,pzero(_x));
+  Packet pos_inf_mask = pcmp_eq(_x,cst_pos_inf);
+  // Filter out invalid inputs, i.e.:
+  //  - negative arg will be NAN
+  //  - 0 will be -INF
+  //  - +INF will be +INF
+  return pselect(iszero_mask, cst_minus_inf,
+                              por(pselect(pos_inf_mask,cst_pos_inf,x), invalid_mask));
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog_float(const Packet _x)
+{
+  return plog_impl_float<Packet, /* base2 */ false>(_x);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog2_float(const Packet _x)
+{
+  return plog_impl_float<Packet, /* base2 */ true>(_x);
+}
+
+/* Returns the base e (2.718...) or base 2 logarithm of x.
+ * The argument is separated into its exponent and fractional parts.
+ * The logarithm of the fraction in the interval [sqrt(1/2), sqrt(2)],
+ * is approximated by
+ *
+ *     log(1+x) = x - 0.5 x**2 + x**3 P(x)/Q(x).
+ *
+ * for more detail see: http://www.netlib.org/cephes/
+ */
+template <typename Packet, bool base2>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog_impl_double(const Packet _x)
+{
+  Packet x = _x;
+
+  const Packet cst_1              = pset1<Packet>(1.0);
+  const Packet cst_neg_half       = pset1<Packet>(-0.5);
+  // The smallest non denormalized double.
+  const Packet cst_min_norm_pos   = pset1frombits<Packet>( static_cast<uint64_t>(0x0010000000000000ull));
+  const Packet cst_minus_inf      = pset1frombits<Packet>( static_cast<uint64_t>(0xfff0000000000000ull));
+  const Packet cst_pos_inf        = pset1frombits<Packet>( static_cast<uint64_t>(0x7ff0000000000000ull));
+
+
+ // Polynomial Coefficients for log(1+x) = x - x**2/2 + x**3 P(x)/Q(x)
+ //                             1/sqrt(2) <= x < sqrt(2)
+  const Packet cst_cephes_SQRTHF = pset1<Packet>(0.70710678118654752440E0);
+  const Packet cst_cephes_log_p0 = pset1<Packet>(1.01875663804580931796E-4);
+  const Packet cst_cephes_log_p1 = pset1<Packet>(4.97494994976747001425E-1);
+  const Packet cst_cephes_log_p2 = pset1<Packet>(4.70579119878881725854E0);
+  const Packet cst_cephes_log_p3 = pset1<Packet>(1.44989225341610930846E1);
+  const Packet cst_cephes_log_p4 = pset1<Packet>(1.79368678507819816313E1);
+  const Packet cst_cephes_log_p5 = pset1<Packet>(7.70838733755885391666E0);
+
+  const Packet cst_cephes_log_q0 = pset1<Packet>(1.0);
+  const Packet cst_cephes_log_q1 = pset1<Packet>(1.12873587189167450590E1);
+  const Packet cst_cephes_log_q2 = pset1<Packet>(4.52279145837532221105E1);
+  const Packet cst_cephes_log_q3 = pset1<Packet>(8.29875266912776603211E1);
+  const Packet cst_cephes_log_q4 = pset1<Packet>(7.11544750618563894466E1);
+  const Packet cst_cephes_log_q5 = pset1<Packet>(2.31251620126765340583E1);
+
+  // Truncate input values to the minimum positive normal.
+  x = pmax(x, cst_min_norm_pos);
+
+  Packet e;
+  // extract significant in the range [0.5,1) and exponent
+  x = pfrexp(x,e);
+  
+  // Shift the inputs from the range [0.5,1) to [sqrt(1/2),sqrt(2))
+  // and shift by -1. The values are then centered around 0, which improves
+  // the stability of the polynomial evaluation.
+  //   if( x < SQRTHF ) {
+  //     e -= 1;
+  //     x = x + x - 1.0;
+  //   } else { x = x - 1.0; }
+  Packet mask = pcmp_lt(x, cst_cephes_SQRTHF);
+  Packet tmp = pand(x, mask);
+  x = psub(x, cst_1);
+  e = psub(e, pand(cst_1, mask));
+  x = padd(x, tmp);
+
+  Packet x2 = pmul(x, x);
+  Packet x3 = pmul(x2, x);
+
+  // Evaluate the polynomial approximant , probably to improve instruction-level parallelism.
+  // y = x - 0.5*x^2 + x^3 * polevl( x, P, 5 ) / p1evl( x, Q, 5 ) );
+  Packet y, y1, y_;
+  y  = pmadd(cst_cephes_log_p0, x, cst_cephes_log_p1);
+  y1 = pmadd(cst_cephes_log_p3, x, cst_cephes_log_p4);
+  y  = pmadd(y, x, cst_cephes_log_p2);
+  y1 = pmadd(y1, x, cst_cephes_log_p5);
+  y_ = pmadd(y, x3, y1);
+
+  y  = pmadd(cst_cephes_log_q0, x, cst_cephes_log_q1);
+  y1 = pmadd(cst_cephes_log_q3, x, cst_cephes_log_q4);
+  y  = pmadd(y, x, cst_cephes_log_q2);
+  y1 = pmadd(y1, x, cst_cephes_log_q5);
+  y  = pmadd(y, x3, y1);
+
+  y_ = pmul(y_, x3);
+  y  = pdiv(y_, y);
+
+  y = pmadd(cst_neg_half, x2, y);
+  x = padd(x, y);
+
+  // Add the logarithm of the exponent back to the result of the interpolation.
+  if (base2) {
+    const Packet cst_log2e = pset1<Packet>(static_cast<double>(EIGEN_LOG2E));
+    x = pmadd(x, cst_log2e, e);
+  } else {
+    const Packet cst_ln2 = pset1<Packet>(static_cast<double>(EIGEN_LN2));
+    x = pmadd(e, cst_ln2, x);
+  }
+
+  Packet invalid_mask = pcmp_lt_or_nan(_x, pzero(_x));
+  Packet iszero_mask  = pcmp_eq(_x,pzero(_x));
+  Packet pos_inf_mask = pcmp_eq(_x,cst_pos_inf);
+  // Filter out invalid inputs, i.e.:
+  //  - negative arg will be NAN
+  //  - 0 will be -INF
+  //  - +INF will be +INF
+  return pselect(iszero_mask, cst_minus_inf,
+                              por(pselect(pos_inf_mask,cst_pos_inf,x), invalid_mask));
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog_double(const Packet _x)
+{
+  return plog_impl_double<Packet, /* base2 */ false>(_x);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog2_double(const Packet _x)
+{
+  return plog_impl_double<Packet, /* base2 */ true>(_x);
+}
+
+/** \internal \returns log(1 + x) computed using W. Kahan's formula.
+    See: http://www.plunk.org/~hatch/rightway.php
+ */
+template<typename Packet>
+Packet generic_plog1p(const Packet& x)
+{
+  typedef typename unpacket_traits<Packet>::type ScalarType;
+  const Packet one = pset1<Packet>(ScalarType(1));
+  Packet xp1 = padd(x, one);
+  Packet small_mask = pcmp_eq(xp1, one);
+  Packet log1 = plog(xp1);
+  Packet inf_mask = pcmp_eq(xp1, log1);
+  Packet log_large = pmul(x, pdiv(log1, psub(xp1, one)));
+  return pselect(por(small_mask, inf_mask), x, log_large);
+}
+
+/** \internal \returns exp(x)-1 computed using W. Kahan's formula.
+    See: http://www.plunk.org/~hatch/rightway.php
+ */
+template<typename Packet>
+Packet generic_expm1(const Packet& x)
+{
+  typedef typename unpacket_traits<Packet>::type ScalarType;
+  const Packet one = pset1<Packet>(ScalarType(1));
+  const Packet neg_one = pset1<Packet>(ScalarType(-1));
+  Packet u = pexp(x);
+  Packet one_mask = pcmp_eq(u, one);
+  Packet u_minus_one = psub(u, one);
+  Packet neg_one_mask = pcmp_eq(u_minus_one, neg_one);
+  Packet logu = plog(u);
+  // The following comparison is to catch the case where
+  // exp(x) = +inf. It is written in this way to avoid having
+  // to form the constant +inf, which depends on the packet
+  // type.
+  Packet pos_inf_mask = pcmp_eq(logu, u);
+  Packet expm1 = pmul(u_minus_one, pdiv(x, logu));
+  expm1 = pselect(pos_inf_mask, u, expm1);
+  return pselect(one_mask,
+                 x,
+                 pselect(neg_one_mask,
+                         neg_one,
+                         expm1));
+}
+
+
+// Exponential function. Works by writing "x = m*log(2) + r" where
+// "m = floor(x/log(2)+1/2)" and "r" is the remainder. The result is then
+// "exp(x) = 2^m*exp(r)" where exp(r) is in the range [-1,1).
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet pexp_float(const Packet _x)
+{
+  const Packet cst_1      = pset1<Packet>(1.0f);
+  const Packet cst_half   = pset1<Packet>(0.5f);
+  const Packet cst_exp_hi = pset1<Packet>( 88.723f);
+  const Packet cst_exp_lo = pset1<Packet>(-88.723f);
+
+  const Packet cst_cephes_LOG2EF = pset1<Packet>(1.44269504088896341f);
+  const Packet cst_cephes_exp_p0 = pset1<Packet>(1.9875691500E-4f);
+  const Packet cst_cephes_exp_p1 = pset1<Packet>(1.3981999507E-3f);
+  const Packet cst_cephes_exp_p2 = pset1<Packet>(8.3334519073E-3f);
+  const Packet cst_cephes_exp_p3 = pset1<Packet>(4.1665795894E-2f);
+  const Packet cst_cephes_exp_p4 = pset1<Packet>(1.6666665459E-1f);
+  const Packet cst_cephes_exp_p5 = pset1<Packet>(5.0000001201E-1f);
+
+  // Clamp x.
+  Packet x = pmax(pmin(_x, cst_exp_hi), cst_exp_lo);
+
+  // Express exp(x) as exp(m*ln(2) + r), start by extracting
+  // m = floor(x/ln(2) + 0.5).
+  Packet m = pfloor(pmadd(x, cst_cephes_LOG2EF, cst_half));
+
+  // Get r = x - m*ln(2). If no FMA instructions are available, m*ln(2) is
+  // subtracted out in two parts, m*C1+m*C2 = m*ln(2), to avoid accumulating
+  // truncation errors.
+  const Packet cst_cephes_exp_C1 = pset1<Packet>(-0.693359375f);
+  const Packet cst_cephes_exp_C2 = pset1<Packet>(2.12194440e-4f);
+  Packet r = pmadd(m, cst_cephes_exp_C1, x);
+  r = pmadd(m, cst_cephes_exp_C2, r);
+
+  Packet r2 = pmul(r, r);
+  Packet r3 = pmul(r2, r);
+
+  // Evaluate the polynomial approximant,improved by instruction-level parallelism.
+  Packet y, y1, y2;
+  y  = pmadd(cst_cephes_exp_p0, r, cst_cephes_exp_p1);
+  y1 = pmadd(cst_cephes_exp_p3, r, cst_cephes_exp_p4);
+  y2 = padd(r, cst_1);
+  y  = pmadd(y, r, cst_cephes_exp_p2);
+  y1 = pmadd(y1, r, cst_cephes_exp_p5);
+  y  = pmadd(y, r3, y1);
+  y  = pmadd(y, r2, y2);
+
+  // Return 2^m * exp(r).
+  // TODO: replace pldexp with faster implementation since y in [-1, 1).
+  return pmax(pldexp(y,m), _x);
+}
+
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet pexp_double(const Packet _x)
+{
+  Packet x = _x;
+
+  const Packet cst_1 = pset1<Packet>(1.0);
+  const Packet cst_2 = pset1<Packet>(2.0);
+  const Packet cst_half = pset1<Packet>(0.5);
+
+  const Packet cst_exp_hi = pset1<Packet>(709.784);
+  const Packet cst_exp_lo = pset1<Packet>(-709.784);
+
+  const Packet cst_cephes_LOG2EF = pset1<Packet>(1.4426950408889634073599);
+  const Packet cst_cephes_exp_p0 = pset1<Packet>(1.26177193074810590878e-4);
+  const Packet cst_cephes_exp_p1 = pset1<Packet>(3.02994407707441961300e-2);
+  const Packet cst_cephes_exp_p2 = pset1<Packet>(9.99999999999999999910e-1);
+  const Packet cst_cephes_exp_q0 = pset1<Packet>(3.00198505138664455042e-6);
+  const Packet cst_cephes_exp_q1 = pset1<Packet>(2.52448340349684104192e-3);
+  const Packet cst_cephes_exp_q2 = pset1<Packet>(2.27265548208155028766e-1);
+  const Packet cst_cephes_exp_q3 = pset1<Packet>(2.00000000000000000009e0);
+  const Packet cst_cephes_exp_C1 = pset1<Packet>(0.693145751953125);
+  const Packet cst_cephes_exp_C2 = pset1<Packet>(1.42860682030941723212e-6);
+
+  Packet tmp, fx;
+
+  // clamp x
+  x = pmax(pmin(x, cst_exp_hi), cst_exp_lo);
+  // Express exp(x) as exp(g + n*log(2)).
+  fx = pmadd(cst_cephes_LOG2EF, x, cst_half);
+
+  // Get the integer modulus of log(2), i.e. the "n" described above.
+  fx = pfloor(fx);
+
+  // Get the remainder modulo log(2), i.e. the "g" described above. Subtract
+  // n*log(2) out in two steps, i.e. n*C1 + n*C2, C1+C2=log2 to get the last
+  // digits right.
+  tmp = pmul(fx, cst_cephes_exp_C1);
+  Packet z = pmul(fx, cst_cephes_exp_C2);
+  x = psub(x, tmp);
+  x = psub(x, z);
+
+  Packet x2 = pmul(x, x);
+
+  // Evaluate the numerator polynomial of the rational interpolant.
+  Packet px = cst_cephes_exp_p0;
+  px = pmadd(px, x2, cst_cephes_exp_p1);
+  px = pmadd(px, x2, cst_cephes_exp_p2);
+  px = pmul(px, x);
+
+  // Evaluate the denominator polynomial of the rational interpolant.
+  Packet qx = cst_cephes_exp_q0;
+  qx = pmadd(qx, x2, cst_cephes_exp_q1);
+  qx = pmadd(qx, x2, cst_cephes_exp_q2);
+  qx = pmadd(qx, x2, cst_cephes_exp_q3);
+
+  // I don't really get this bit, copied from the SSE2 routines, so...
+  // TODO(gonnet): Figure out what is going on here, perhaps find a better
+  // rational interpolant?
+  x = pdiv(px, psub(qx, px));
+  x = pmadd(cst_2, x, cst_1);
+
+  // Construct the result 2^n * exp(g) = e * x. The max is used to catch
+  // non-finite values in the input.
+  // TODO: replace pldexp with faster implementation since x in [-1, 1).
+  return pmax(pldexp(x,fx), _x);
+}
+
+// The following code is inspired by the following stack-overflow answer:
+//   https://stackoverflow.com/questions/30463616/payne-hanek-algorithm-implementation-in-c/30465751#30465751
+// It has been largely optimized:
+//  - By-pass calls to frexp.
+//  - Aligned loads of required 96 bits of 2/pi. This is accomplished by
+//    (1) balancing the mantissa and exponent to the required bits of 2/pi are
+//    aligned on 8-bits, and (2) replicating the storage of the bits of 2/pi.
+//  - Avoid a branch in rounding and extraction of the remaining fractional part.
+// Overall, I measured a speed up higher than x2 on x86-64.
+inline float trig_reduce_huge (float xf, int *quadrant)
+{
+  using Eigen::numext::int32_t;
+  using Eigen::numext::uint32_t;
+  using Eigen::numext::int64_t;
+  using Eigen::numext::uint64_t;
+
+  const double pio2_62 = 3.4061215800865545e-19;    // pi/2 * 2^-62
+  const uint64_t zero_dot_five = uint64_t(1) << 61; // 0.5 in 2.62-bit fixed-point foramt
+
+  // 192 bits of 2/pi for Payne-Hanek reduction
+  // Bits are introduced by packet of 8 to enable aligned reads.
+  static const uint32_t two_over_pi [] = 
+  {
+    0x00000028, 0x000028be, 0x0028be60, 0x28be60db,
+    0xbe60db93, 0x60db9391, 0xdb939105, 0x9391054a,
+    0x91054a7f, 0x054a7f09, 0x4a7f09d5, 0x7f09d5f4,
+    0x09d5f47d, 0xd5f47d4d, 0xf47d4d37, 0x7d4d3770,
+    0x4d377036, 0x377036d8, 0x7036d8a5, 0x36d8a566,
+    0xd8a5664f, 0xa5664f10, 0x664f10e4, 0x4f10e410,
+    0x10e41000, 0xe4100000
+  };
+  
+  uint32_t xi = numext::bit_cast<uint32_t>(xf);
+  // Below, -118 = -126 + 8.
+  //   -126 is to get the exponent,
+  //   +8 is to enable alignment of 2/pi's bits on 8 bits.
+  // This is possible because the fractional part of x as only 24 meaningful bits.
+  uint32_t e = (xi >> 23) - 118;
+  // Extract the mantissa and shift it to align it wrt the exponent
+  xi = ((xi & 0x007fffffu)| 0x00800000u) << (e & 0x7);
+
+  uint32_t i = e >> 3;
+  uint32_t twoopi_1  = two_over_pi[i-1];
+  uint32_t twoopi_2  = two_over_pi[i+3];
+  uint32_t twoopi_3  = two_over_pi[i+7];
+
+  // Compute x * 2/pi in 2.62-bit fixed-point format.
+  uint64_t p;
+  p = uint64_t(xi) * twoopi_3;
+  p = uint64_t(xi) * twoopi_2 + (p >> 32);
+  p = (uint64_t(xi * twoopi_1) << 32) + p;
+
+  // Round to nearest: add 0.5 and extract integral part.
+  uint64_t q = (p + zero_dot_five) >> 62;
+  *quadrant = int(q);
+  // Now it remains to compute "r = x - q*pi/2" with high accuracy,
+  // since we have p=x/(pi/2) with high accuracy, we can more efficiently compute r as:
+  //   r = (p-q)*pi/2,
+  // where the product can be be carried out with sufficient accuracy using double precision.
+  p -= q<<62;
+  return float(double(int64_t(p)) * pio2_62);
+}
+
+template<bool ComputeSine,typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+#if EIGEN_GNUC_AT_LEAST(4,4) && EIGEN_COMP_GNUC_STRICT
+__attribute__((optimize("-fno-unsafe-math-optimizations")))
+#endif
+Packet psincos_float(const Packet& _x)
+{
+  typedef typename unpacket_traits<Packet>::integer_packet PacketI;
+
+  const Packet  cst_2oPI            = pset1<Packet>(0.636619746685028076171875f); // 2/PI
+  const Packet  cst_rounding_magic  = pset1<Packet>(12582912); // 2^23 for rounding
+  const PacketI csti_1              = pset1<PacketI>(1);
+  const Packet  cst_sign_mask       = pset1frombits<Packet>(0x80000000u);
+
+  Packet x = pabs(_x);
+
+  // Scale x by 2/Pi to find x's octant.
+  Packet y = pmul(x, cst_2oPI);
+
+  // Rounding trick:
+  Packet y_round = padd(y, cst_rounding_magic);
+  EIGEN_OPTIMIZATION_BARRIER(y_round)
+  PacketI y_int = preinterpret<PacketI>(y_round); // last 23 digits represent integer (if abs(x)<2^24)
+  y = psub(y_round, cst_rounding_magic); // nearest integer to x*4/pi
+
+  // Reduce x by y octants to get: -Pi/4 <= x <= +Pi/4
+  // using "Extended precision modular arithmetic"
+  #if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD)
+  // This version requires true FMA for high accuracy
+  // It provides a max error of 1ULP up to (with absolute_error < 5.9605e-08):
+  const float huge_th = ComputeSine ? 117435.992f : 71476.0625f;
+  x = pmadd(y, pset1<Packet>(-1.57079601287841796875f), x);
+  x = pmadd(y, pset1<Packet>(-3.1391647326017846353352069854736328125e-07f), x);
+  x = pmadd(y, pset1<Packet>(-5.390302529957764765544681040410068817436695098876953125e-15f), x);
+  #else
+  // Without true FMA, the previous set of coefficients maintain 1ULP accuracy
+  // up to x<15.7 (for sin), but accuracy is immediately lost for x>15.7.
+  // We thus use one more iteration to maintain 2ULPs up to reasonably large inputs.
+
+  // The following set of coefficients maintain 1ULP up to 9.43 and 14.16 for sin and cos respectively.
+  // and 2 ULP up to:
+  const float huge_th = ComputeSine ? 25966.f : 18838.f;
+  x = pmadd(y, pset1<Packet>(-1.5703125), x); // = 0xbfc90000
+  EIGEN_OPTIMIZATION_BARRIER(x)
+  x = pmadd(y, pset1<Packet>(-0.000483989715576171875), x); // = 0xb9fdc000
+  EIGEN_OPTIMIZATION_BARRIER(x)
+  x = pmadd(y, pset1<Packet>(1.62865035235881805419921875e-07), x); // = 0x342ee000
+  x = pmadd(y, pset1<Packet>(5.5644315544167710640977020375430583953857421875e-11), x); // = 0x2e74b9ee
+
+  // For the record, the following set of coefficients maintain 2ULP up
+  // to a slightly larger range:
+  // const float huge_th = ComputeSine ? 51981.f : 39086.125f;
+  // but it slightly fails to maintain 1ULP for two values of sin below pi.
+  // x = pmadd(y, pset1<Packet>(-3.140625/2.), x);
+  // x = pmadd(y, pset1<Packet>(-0.00048351287841796875), x);
+  // x = pmadd(y, pset1<Packet>(-3.13855707645416259765625e-07), x);
+  // x = pmadd(y, pset1<Packet>(-6.0771006282767103812147979624569416046142578125e-11), x);
+
+  // For the record, with only 3 iterations it is possible to maintain
+  // 1 ULP up to 3PI (maybe more) and 2ULP up to 255.
+  // The coefficients are: 0xbfc90f80, 0xb7354480, 0x2e74b9ee
+  #endif
+
+  if(predux_any(pcmp_le(pset1<Packet>(huge_th),pabs(_x))))
+  {
+    const int PacketSize = unpacket_traits<Packet>::size;
+    EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) float vals[PacketSize];
+    EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) float x_cpy[PacketSize];
+    EIGEN_ALIGN_TO_BOUNDARY(sizeof(Packet)) int y_int2[PacketSize];
+    pstoreu(vals, pabs(_x));
+    pstoreu(x_cpy, x);
+    pstoreu(y_int2, y_int);
+    for(int k=0; k<PacketSize;++k)
+    {
+      float val = vals[k];
+      if(val>=huge_th && (numext::isfinite)(val))
+        x_cpy[k] = trig_reduce_huge(val,&y_int2[k]);
+    }
+    x = ploadu<Packet>(x_cpy);
+    y_int = ploadu<PacketI>(y_int2);
+  }
+
+  // Compute the sign to apply to the polynomial.
+  // sin: sign = second_bit(y_int) xor signbit(_x)
+  // cos: sign = second_bit(y_int+1)
+  Packet sign_bit = ComputeSine ? pxor(_x, preinterpret<Packet>(plogical_shift_left<30>(y_int)))
+                                : preinterpret<Packet>(plogical_shift_left<30>(padd(y_int,csti_1)));
+  sign_bit = pand(sign_bit, cst_sign_mask); // clear all but left most bit
+
+  // Get the polynomial selection mask from the second bit of y_int
+  // We'll calculate both (sin and cos) polynomials and then select from the two.
+  Packet poly_mask = preinterpret<Packet>(pcmp_eq(pand(y_int, csti_1), pzero(y_int)));
+
+  Packet x2 = pmul(x,x);
+
+  // Evaluate the cos(x) polynomial. (-Pi/4 <= x <= Pi/4)
+  Packet y1 =        pset1<Packet>(2.4372266125283204019069671630859375e-05f);
+  y1 = pmadd(y1, x2, pset1<Packet>(-0.00138865201734006404876708984375f     ));
+  y1 = pmadd(y1, x2, pset1<Packet>(0.041666619479656219482421875f           ));
+  y1 = pmadd(y1, x2, pset1<Packet>(-0.5f));
+  y1 = pmadd(y1, x2, pset1<Packet>(1.f));
+
+  // Evaluate the sin(x) polynomial. (Pi/4 <= x <= Pi/4)
+  // octave/matlab code to compute those coefficients:
+  //    x = (0:0.0001:pi/4)';
+  //    A = [x.^3 x.^5 x.^7];
+  //    w = ((1.-(x/(pi/4)).^2).^5)*2000+1;         # weights trading relative accuracy
+  //    c = (A'*diag(w)*A)\(A'*diag(w)*(sin(x)-x)); # weighted LS, linear coeff forced to 1
+  //    printf('%.64f\n %.64f\n%.64f\n', c(3), c(2), c(1))
+  //
+  Packet y2 =        pset1<Packet>(-0.0001959234114083702898469196984621021329076029360294342041015625f);
+  y2 = pmadd(y2, x2, pset1<Packet>( 0.0083326873655616851693794799871284340042620897293090820312500000f));
+  y2 = pmadd(y2, x2, pset1<Packet>(-0.1666666203982298255503735617821803316473960876464843750000000000f));
+  y2 = pmul(y2, x2);
+  y2 = pmadd(y2, x, x);
+
+  // Select the correct result from the two polynomials.
+  y = ComputeSine ? pselect(poly_mask,y2,y1)
+                  : pselect(poly_mask,y1,y2);
+
+  // Update the sign and filter huge inputs
+  return pxor(y, sign_bit);
+}
+
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet psin_float(const Packet& x)
+{
+  return psincos_float<true>(x);
+}
+
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet pcos_float(const Packet& x)
+{
+  return psincos_float<false>(x);
+}
+
+
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet psqrt_complex(const Packet& a) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  typedef typename Scalar::value_type RealScalar;
+  typedef typename unpacket_traits<Packet>::as_real RealPacket;
+
+  // Computes the principal sqrt of the complex numbers in the input.
+  //
+  // For example, for packets containing 2 complex numbers stored in interleaved format
+  //    a = [a0, a1] = [x0, y0, x1, y1],
+  // where x0 = real(a0), y0 = imag(a0) etc., this function returns
+  //    b = [b0, b1] = [u0, v0, u1, v1],
+  // such that b0^2 = a0, b1^2 = a1.
+  //
+  // To derive the formula for the complex square roots, let's consider the equation for
+  // a single complex square root of the number x + i*y. We want to find real numbers
+  // u and v such that
+  //    (u + i*v)^2 = x + i*y  <=>
+  //    u^2 - v^2 + i*2*u*v = x + i*v.
+  // By equating the real and imaginary parts we get:
+  //    u^2 - v^2 = x
+  //    2*u*v = y.
+  //
+  // For x >= 0, this has the numerically stable solution
+  //    u = sqrt(0.5 * (x + sqrt(x^2 + y^2)))
+  //    v = 0.5 * (y / u)
+  // and for x < 0,
+  //    v = sign(y) * sqrt(0.5 * (-x + sqrt(x^2 + y^2)))
+  //    u = 0.5 * (y / v)
+  //
+  //  To avoid unnecessary over- and underflow, we compute sqrt(x^2 + y^2) as
+  //     l = max(|x|, |y|) * sqrt(1 + (min(|x|, |y|) / max(|x|, |y|))^2) ,
+
+  // In the following, without lack of generality, we have annotated the code, assuming
+  // that the input is a packet of 2 complex numbers.
+  //
+  // Step 1. Compute l = [l0, l0, l1, l1], where
+  //    l0 = sqrt(x0^2 + y0^2),  l1 = sqrt(x1^2 + y1^2)
+  // To avoid over- and underflow, we use the stable formula for each hypotenuse
+  //    l0 = (min0 == 0 ? max0 : max0 * sqrt(1 + (min0/max0)**2)),
+  // where max0 = max(|x0|, |y0|), min0 = min(|x0|, |y0|), and similarly for l1.
+
+  RealPacket a_abs = pabs(a.v);           // [|x0|, |y0|, |x1|, |y1|]
+  RealPacket a_abs_flip = pcplxflip(Packet(a_abs)).v; // [|y0|, |x0|, |y1|, |x1|]
+  RealPacket a_max = pmax(a_abs, a_abs_flip);
+  RealPacket a_min = pmin(a_abs, a_abs_flip);
+  RealPacket a_min_zero_mask = pcmp_eq(a_min, pzero(a_min));
+  RealPacket a_max_zero_mask = pcmp_eq(a_max, pzero(a_max));
+  RealPacket r = pdiv(a_min, a_max);
+  const RealPacket cst_one  = pset1<RealPacket>(RealScalar(1));
+  RealPacket l = pmul(a_max, psqrt(padd(cst_one, pmul(r, r))));  // [l0, l0, l1, l1]
+  // Set l to a_max if a_min is zero.
+  l = pselect(a_min_zero_mask, a_max, l);
+
+  // Step 2. Compute [rho0, *, rho1, *], where
+  // rho0 = sqrt(0.5 * (l0 + |x0|)), rho1 =  sqrt(0.5 * (l1 + |x1|))
+  // We don't care about the imaginary parts computed here. They will be overwritten later.
+  const RealPacket cst_half = pset1<RealPacket>(RealScalar(0.5));
+  Packet rho;
+  rho.v = psqrt(pmul(cst_half, padd(a_abs, l)));
+
+  // Step 3. Compute [rho0, eta0, rho1, eta1], where
+  // eta0 = (y0 / l0) / 2, and eta1 = (y1 / l1) / 2.
+  // set eta = 0 of input is 0 + i0.
+  RealPacket eta = pandnot(pmul(cst_half, pdiv(a.v, pcplxflip(rho).v)), a_max_zero_mask);
+  RealPacket real_mask = peven_mask(a.v);
+  Packet positive_real_result;
+  // Compute result for inputs with positive real part.
+  positive_real_result.v = pselect(real_mask, rho.v, eta);
+
+  // Step 4. Compute solution for inputs with negative real part:
+  //         [|eta0|, sign(y0)*rho0, |eta1|, sign(y1)*rho1]
+  const RealScalar neg_zero = RealScalar(numext::bit_cast<float>(0x80000000u));
+  const RealPacket cst_imag_sign_mask = pset1<Packet>(Scalar(RealScalar(0.0), neg_zero)).v;
+  RealPacket imag_signs = pand(a.v, cst_imag_sign_mask);
+  Packet negative_real_result;
+  // Notice that rho is positive, so taking it's absolute value is a noop.
+  negative_real_result.v = por(pabs(pcplxflip(positive_real_result).v), imag_signs);
+
+  // Step 5. Select solution branch based on the sign of the real parts.
+  Packet negative_real_mask;
+  negative_real_mask.v = pcmp_lt(pand(real_mask, a.v), pzero(a.v));
+  negative_real_mask.v = por(negative_real_mask.v, pcplxflip(negative_real_mask).v);
+  Packet result = pselect(negative_real_mask, negative_real_result, positive_real_result);
+
+  // Step 6. Handle special cases for infinities:
+  // * If z is (x,+∞), the result is (+∞,+∞) even if x is NaN
+  // * If z is (x,-∞), the result is (+∞,-∞) even if x is NaN
+  // * If z is (-∞,y), the result is (0*|y|,+∞) for finite or NaN y
+  // * If z is (+∞,y), the result is (+∞,0*|y|) for finite or NaN y
+  const RealPacket cst_pos_inf = pset1<RealPacket>(NumTraits<RealScalar>::infinity());
+  Packet is_inf;
+  is_inf.v = pcmp_eq(a_abs, cst_pos_inf);
+  Packet is_real_inf;
+  is_real_inf.v = pand(is_inf.v, real_mask);
+  is_real_inf = por(is_real_inf, pcplxflip(is_real_inf));
+  // prepare packet of (+∞,0*|y|) or (0*|y|,+∞), depending on the sign of the infinite real part.
+  Packet real_inf_result;
+  real_inf_result.v = pmul(a_abs, pset1<Packet>(Scalar(RealScalar(1.0), RealScalar(0.0))).v);
+  real_inf_result.v = pselect(negative_real_mask.v, pcplxflip(real_inf_result).v, real_inf_result.v);
+  // prepare packet of (+∞,+∞) or (+∞,-∞), depending on the sign of the infinite imaginary part.
+  Packet is_imag_inf;
+  is_imag_inf.v = pandnot(is_inf.v, real_mask);
+  is_imag_inf = por(is_imag_inf, pcplxflip(is_imag_inf));
+  Packet imag_inf_result;
+  imag_inf_result.v = por(pand(cst_pos_inf, real_mask), pandnot(a.v, real_mask));
+
+  return  pselect(is_imag_inf, imag_inf_result,
+                  pselect(is_real_inf, real_inf_result,result));
+}
+
+// TODO(rmlarsen): The following set of utilities for double word arithmetic
+// should perhaps be refactored as a separate file, since it would be generally
+// useful for special function implementation etc. Writing the algorithms in
+// terms if a double word type would also make the code more readable.
+
+// This function splits x into the nearest integer n and fractional part r,
+// such that x = n + r holds exactly.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void absolute_split(const Packet& x, Packet& n, Packet& r) {
+  n = pround(x);
+  r = psub(x, n);
+}
+
+// This function computes the sum {s, r}, such that x + y = s_hi + s_lo
+// holds exactly, and s_hi = fl(x+y), if |x| >= |y|.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void fast_twosum(const Packet& x, const Packet& y, Packet& s_hi, Packet& s_lo) {
+  s_hi = padd(x, y);
+  const Packet t = psub(s_hi, x);
+  s_lo = psub(y, t);
+}
+
+#ifdef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
+// This function implements the extended precision product of
+// a pair of floating point numbers. Given {x, y}, it computes the pair
+// {p_hi, p_lo} such that x * y = p_hi + p_lo holds exactly and
+// p_hi = fl(x * y).
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void twoprod(const Packet& x, const Packet& y,
+             Packet& p_hi, Packet& p_lo) {
+  p_hi = pmul(x, y);
+  p_lo = pmadd(x, y, pnegate(p_hi));
+}
+
+#else
+
+// This function implements the Veltkamp splitting. Given a floating point
+// number x it returns the pair {x_hi, x_lo} such that x_hi + x_lo = x holds
+// exactly and that half of the significant of x fits in x_hi.
+// This is Algorithm 3 from Jean-Michel Muller, "Elementary Functions",
+// 3rd edition, Birkh\"auser, 2016.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void veltkamp_splitting(const Packet& x, Packet& x_hi, Packet& x_lo) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  EIGEN_CONSTEXPR int shift = (NumTraits<Scalar>::digits() + 1) / 2;
+  const Scalar shift_scale = Scalar(uint64_t(1) << shift);  // Scalar constructor not necessarily constexpr.
+  const Packet gamma = pmul(pset1<Packet>(shift_scale + Scalar(1)), x);
+  Packet rho = psub(x, gamma);
+  x_hi = padd(rho, gamma);
+  x_lo = psub(x, x_hi);
+}
+
+// This function implements Dekker's algorithm for products x * y.
+// Given floating point numbers {x, y} computes the pair
+// {p_hi, p_lo} such that x * y = p_hi + p_lo holds exactly and
+// p_hi = fl(x * y).
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void twoprod(const Packet& x, const Packet& y,
+             Packet& p_hi, Packet& p_lo) {
+  Packet x_hi, x_lo, y_hi, y_lo;
+  veltkamp_splitting(x, x_hi, x_lo);
+  veltkamp_splitting(y, y_hi, y_lo);
+
+  p_hi = pmul(x, y);
+  p_lo = pmadd(x_hi, y_hi, pnegate(p_hi));
+  p_lo = pmadd(x_hi, y_lo, p_lo);
+  p_lo = pmadd(x_lo, y_hi, p_lo);
+  p_lo = pmadd(x_lo, y_lo, p_lo);
+}
+
+#endif  // EIGEN_HAS_SINGLE_INSTRUCTION_MADD
+
+
+// This function implements Dekker's algorithm for the addition
+// of two double word numbers represented by {x_hi, x_lo} and {y_hi, y_lo}.
+// It returns the result as a pair {s_hi, s_lo} such that
+// x_hi + x_lo + y_hi + y_lo = s_hi + s_lo holds exactly.
+// This is Algorithm 5 from Jean-Michel Muller, "Elementary Functions",
+// 3rd edition, Birkh\"auser, 2016.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+  void twosum(const Packet& x_hi, const Packet& x_lo,
+              const Packet& y_hi, const Packet& y_lo,
+              Packet& s_hi, Packet& s_lo) {
+  const Packet x_greater_mask = pcmp_lt(pabs(y_hi), pabs(x_hi));
+  Packet r_hi_1, r_lo_1;
+  fast_twosum(x_hi, y_hi,r_hi_1, r_lo_1);
+  Packet r_hi_2, r_lo_2;
+  fast_twosum(y_hi, x_hi,r_hi_2, r_lo_2);
+  const Packet r_hi = pselect(x_greater_mask, r_hi_1, r_hi_2);
+
+  const Packet s1 = padd(padd(y_lo, r_lo_1), x_lo);
+  const Packet s2 = padd(padd(x_lo, r_lo_2), y_lo);
+  const Packet s = pselect(x_greater_mask, s1, s2);
+
+  fast_twosum(r_hi, s, s_hi, s_lo);
+}
+
+// This is a version of twosum for double word numbers,
+// which assumes that |x_hi| >= |y_hi|.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+  void fast_twosum(const Packet& x_hi, const Packet& x_lo,
+              const Packet& y_hi, const Packet& y_lo,
+              Packet& s_hi, Packet& s_lo) {
+  Packet r_hi, r_lo;
+  fast_twosum(x_hi, y_hi, r_hi, r_lo);
+  const Packet s = padd(padd(y_lo, r_lo), x_lo);
+  fast_twosum(r_hi, s, s_hi, s_lo);
+}
+
+// This is a version of twosum for adding a floating point number x to
+// double word number {y_hi, y_lo} number, with the assumption
+// that |x| >= |y_hi|.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void fast_twosum(const Packet& x,
+                 const Packet& y_hi, const Packet& y_lo,
+                 Packet& s_hi, Packet& s_lo) {
+  Packet r_hi, r_lo;
+  fast_twosum(x, y_hi, r_hi, r_lo);
+  const Packet s = padd(y_lo, r_lo);
+  fast_twosum(r_hi, s, s_hi, s_lo);
+}
+
+// This function implements the multiplication of a double word
+// number represented by {x_hi, x_lo} by a floating point number y.
+// It returns the result as a pair {p_hi, p_lo} such that
+// (x_hi + x_lo) * y = p_hi + p_lo hold with a relative error
+// of less than 2*2^{-2p}, where p is the number of significand bit
+// in the floating point type.
+// This is Algorithm 7 from Jean-Michel Muller, "Elementary Functions",
+// 3rd edition, Birkh\"auser, 2016.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void twoprod(const Packet& x_hi, const Packet& x_lo, const Packet& y,
+             Packet& p_hi, Packet& p_lo) {
+  Packet c_hi, c_lo1;
+  twoprod(x_hi, y, c_hi, c_lo1);
+  const Packet c_lo2 = pmul(x_lo, y);
+  Packet t_hi, t_lo1;
+  fast_twosum(c_hi, c_lo2, t_hi, t_lo1);
+  const Packet t_lo2 = padd(t_lo1, c_lo1);
+  fast_twosum(t_hi, t_lo2, p_hi, p_lo);
+}
+
+// This function implements the multiplication of two double word
+// numbers represented by {x_hi, x_lo} and {y_hi, y_lo}.
+// It returns the result as a pair {p_hi, p_lo} such that
+// (x_hi + x_lo) * (y_hi + y_lo) = p_hi + p_lo holds with a relative error
+// of less than 2*2^{-2p}, where p is the number of significand bit
+// in the floating point type.
+template<typename Packet>
+EIGEN_STRONG_INLINE
+void twoprod(const Packet& x_hi, const Packet& x_lo,
+             const Packet& y_hi, const Packet& y_lo,
+             Packet& p_hi, Packet& p_lo) {
+  Packet p_hi_hi, p_hi_lo;
+  twoprod(x_hi, x_lo, y_hi, p_hi_hi, p_hi_lo);
+  Packet p_lo_hi, p_lo_lo;
+  twoprod(x_hi, x_lo, y_lo, p_lo_hi, p_lo_lo);
+  fast_twosum(p_hi_hi, p_hi_lo, p_lo_hi, p_lo_lo, p_hi, p_lo);
+}
+
+// This function computes the reciprocal of a floating point number
+// with extra precision and returns the result as a double word.
+template <typename Packet>
+void doubleword_reciprocal(const Packet& x, Packet& recip_hi, Packet& recip_lo) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  // 1. Approximate the reciprocal as the reciprocal of the high order element.
+  Packet approx_recip = prsqrt(x);
+  approx_recip = pmul(approx_recip, approx_recip);
+
+  // 2. Run one step of Newton-Raphson iteration in double word arithmetic
+  // to get the bottom half. The NR iteration for reciprocal of 'a' is
+  //    x_{i+1} = x_i * (2 - a * x_i)
+
+  // -a*x_i
+  Packet t1_hi, t1_lo;
+  twoprod(pnegate(x), approx_recip, t1_hi, t1_lo);
+  // 2 - a*x_i
+  Packet t2_hi, t2_lo;
+  fast_twosum(pset1<Packet>(Scalar(2)), t1_hi, t2_hi, t2_lo);
+  Packet t3_hi, t3_lo;
+  fast_twosum(t2_hi, padd(t2_lo, t1_lo), t3_hi, t3_lo);
+  // x_i * (2 - a * x_i)
+  twoprod(t3_hi, t3_lo, approx_recip, recip_hi, recip_lo);
+}
+
+
+// This function computes log2(x) and returns the result as a double word.
+template <typename Scalar>
+struct accurate_log2 {
+  template <typename Packet>
+  EIGEN_STRONG_INLINE
+  void operator()(const Packet& x, Packet& log2_x_hi, Packet& log2_x_lo) {
+    log2_x_hi = plog2(x);
+    log2_x_lo = pzero(x);
+  }
+};
+
+// This specialization uses a more accurate algorithm to compute log2(x) for
+// floats in [1/sqrt(2);sqrt(2)] with a relative accuracy of ~6.42e-10.
+// This additional accuracy is needed to counter the error-magnification
+// inherent in multiplying by a potentially large exponent in pow(x,y).
+// The minimax polynomial used was calculated using the Sollya tool.
+// See sollya.org.
+template <>
+struct accurate_log2<float> {
+  template <typename Packet>
+  EIGEN_STRONG_INLINE
+  void operator()(const Packet& z, Packet& log2_x_hi, Packet& log2_x_lo) {
+    // The function log(1+x)/x is approximated in the interval
+    // [1/sqrt(2)-1;sqrt(2)-1] by a degree 10 polynomial of the form
+    //  Q(x) = (C0 + x * (C1 + x * (C2 + x * (C3 + x * P(x))))),
+    // where the degree 6 polynomial P(x) is evaluated in single precision,
+    // while the remaining 4 terms of Q(x), as well as the final multiplication by x
+    // to reconstruct log(1+x) are evaluated in extra precision using
+    // double word arithmetic. C0 through C3 are extra precise constants
+    // stored as double words.
+    //
+    // The polynomial coefficients were calculated using Sollya commands:
+    // > n = 10;
+    // > f = log2(1+x)/x;
+    // > interval = [sqrt(0.5)-1;sqrt(2)-1];
+    // > p = fpminimax(f,n,[|double,double,double,double,single...|],interval,relative,floating);
+    
+    const Packet p6 = pset1<Packet>( 9.703654795885e-2f);
+    const Packet p5 = pset1<Packet>(-0.1690667718648f);
+    const Packet p4 = pset1<Packet>( 0.1720575392246f);
+    const Packet p3 = pset1<Packet>(-0.1789081543684f);
+    const Packet p2 = pset1<Packet>( 0.2050433009862f);
+    const Packet p1 = pset1<Packet>(-0.2404672354459f);
+    const Packet p0 = pset1<Packet>( 0.2885761857032f);
+
+    const Packet C3_hi = pset1<Packet>(-0.360674142838f);
+    const Packet C3_lo = pset1<Packet>(-6.13283912543e-09f);
+    const Packet C2_hi = pset1<Packet>(0.480897903442f);
+    const Packet C2_lo = pset1<Packet>(-1.44861207474e-08f);
+    const Packet C1_hi = pset1<Packet>(-0.721347510815f);
+    const Packet C1_lo = pset1<Packet>(-4.84483164698e-09f);
+    const Packet C0_hi = pset1<Packet>(1.44269502163f);
+    const Packet C0_lo = pset1<Packet>(2.01711713999e-08f);
+    const Packet one = pset1<Packet>(1.0f);
+
+    const Packet x = psub(z, one);
+    // Evaluate P(x) in working precision.
+    // We evaluate it in multiple parts to improve instruction level
+    // parallelism.
+    Packet x2 = pmul(x,x);
+    Packet p_even = pmadd(p6, x2, p4);
+    p_even = pmadd(p_even, x2, p2);
+    p_even = pmadd(p_even, x2, p0);
+    Packet p_odd = pmadd(p5, x2, p3);
+    p_odd = pmadd(p_odd, x2, p1);
+    Packet p = pmadd(p_odd, x, p_even);
+
+    // Now evaluate the low-order tems of Q(x) in double word precision.
+    // In the following, due to the alternating signs and the fact that
+    // |x| < sqrt(2)-1, we can assume that |C*_hi| >= q_i, and use
+    // fast_twosum instead of the slower twosum.
+    Packet q_hi, q_lo;
+    Packet t_hi, t_lo;
+    // C3 + x * p(x)
+    twoprod(p, x, t_hi, t_lo);
+    fast_twosum(C3_hi, C3_lo, t_hi, t_lo, q_hi, q_lo);
+    // C2 + x * p(x)
+    twoprod(q_hi, q_lo, x, t_hi, t_lo);
+    fast_twosum(C2_hi, C2_lo, t_hi, t_lo, q_hi, q_lo);
+    // C1 + x * p(x)
+    twoprod(q_hi, q_lo, x, t_hi, t_lo);
+    fast_twosum(C1_hi, C1_lo, t_hi, t_lo, q_hi, q_lo);
+    // C0 + x * p(x)
+    twoprod(q_hi, q_lo, x, t_hi, t_lo);
+    fast_twosum(C0_hi, C0_lo, t_hi, t_lo, q_hi, q_lo);
+
+    // log(z) ~= x * Q(x)
+    twoprod(q_hi, q_lo, x, log2_x_hi, log2_x_lo);
+  }
+};
+
+// This specialization uses a more accurate algorithm to compute log2(x) for
+// floats in [1/sqrt(2);sqrt(2)] with a relative accuracy of ~1.27e-18.
+// This additional accuracy is needed to counter the error-magnification
+// inherent in multiplying by a potentially large exponent in pow(x,y).
+// The minimax polynomial used was calculated using the Sollya tool.
+// See sollya.org.
+
+template <>
+struct accurate_log2<double> {
+  template <typename Packet>
+  EIGEN_STRONG_INLINE
+  void operator()(const Packet& x, Packet& log2_x_hi, Packet& log2_x_lo) {
+    // We use a transformation of variables:
+    //    r = c * (x-1) / (x+1),
+    // such that
+    //    log2(x) = log2((1 + r/c) / (1 - r/c)) = f(r).
+    // The function f(r) can be approximated well using an odd polynomial
+    // of the form
+    //   P(r) = ((Q(r^2) * r^2 + C) * r^2 + 1) * r,
+    // For the implementation of log2<double> here, Q is of degree 6 with
+    // coefficient represented in working precision (double), while C is a
+    // constant represented in extra precision as a double word to achieve
+    // full accuracy.
+    //
+    // The polynomial coefficients were computed by the Sollya script:
+    //
+    // c = 2 / log(2);
+    // trans = c * (x-1)/(x+1);
+    // itrans = (1+x/c)/(1-x/c);
+    // interval=[trans(sqrt(0.5)); trans(sqrt(2))];
+    // print(interval);
+    // f = log2(itrans(x));
+    // p=fpminimax(f,[|1,3,5,7,9,11,13,15,17|],[|1,DD,double...|],interval,relative,floating);
+    const Packet q12 = pset1<Packet>(2.87074255468000586e-9);
+    const Packet q10 = pset1<Packet>(2.38957980901884082e-8);
+    const Packet q8 = pset1<Packet>(2.31032094540014656e-7);
+    const Packet q6 = pset1<Packet>(2.27279857398537278e-6);
+    const Packet q4 = pset1<Packet>(2.31271023278625638e-5);
+    const Packet q2 = pset1<Packet>(2.47556738444535513e-4);
+    const Packet q0 = pset1<Packet>(2.88543873228900172e-3);
+    const Packet C_hi = pset1<Packet>(0.0400377511598501157);
+    const Packet C_lo = pset1<Packet>(-4.77726582251425391e-19);
+    const Packet one = pset1<Packet>(1.0);
+
+    const Packet cst_2_log2e_hi = pset1<Packet>(2.88539008177792677);
+    const Packet cst_2_log2e_lo = pset1<Packet>(4.07660016854549667e-17);
+    // c * (x - 1)
+    Packet num_hi, num_lo;
+    twoprod(cst_2_log2e_hi, cst_2_log2e_lo, psub(x, one), num_hi, num_lo);
+    // TODO(rmlarsen): Investigate if using the division algorithm by
+    // Muller et al. is faster/more accurate.
+    // 1 / (x + 1)
+    Packet denom_hi, denom_lo;
+    doubleword_reciprocal(padd(x, one), denom_hi, denom_lo);
+    // r =  c * (x-1) / (x+1),
+    Packet r_hi, r_lo;
+    twoprod(num_hi, num_lo, denom_hi, denom_lo, r_hi, r_lo);
+    // r2 = r * r
+    Packet r2_hi, r2_lo;
+    twoprod(r_hi, r_lo, r_hi, r_lo, r2_hi, r2_lo);
+    // r4 = r2 * r2
+    Packet r4_hi, r4_lo;
+    twoprod(r2_hi, r2_lo, r2_hi, r2_lo, r4_hi, r4_lo);
+
+    // Evaluate Q(r^2) in working precision. We evaluate it in two parts
+    // (even and odd in r^2) to improve instruction level parallelism.
+    Packet q_even = pmadd(q12, r4_hi, q8);
+    Packet q_odd = pmadd(q10, r4_hi, q6);
+    q_even = pmadd(q_even, r4_hi, q4);
+    q_odd = pmadd(q_odd, r4_hi, q2);
+    q_even = pmadd(q_even, r4_hi, q0);
+    Packet q = pmadd(q_odd, r2_hi, q_even);
+
+    // Now evaluate the low order terms of P(x) in double word precision.
+    // In the following, due to the increasing magnitude of the coefficients
+    // and r being constrained to [-0.5, 0.5] we can use fast_twosum instead
+    // of the slower twosum.
+    // Q(r^2) * r^2
+    Packet p_hi, p_lo;
+    twoprod(r2_hi, r2_lo, q, p_hi, p_lo);
+    // Q(r^2) * r^2 + C
+    Packet p1_hi, p1_lo;
+    fast_twosum(C_hi, C_lo, p_hi, p_lo, p1_hi, p1_lo);
+    // (Q(r^2) * r^2 + C) * r^2
+    Packet p2_hi, p2_lo;
+    twoprod(r2_hi, r2_lo, p1_hi, p1_lo, p2_hi, p2_lo);
+    // ((Q(r^2) * r^2 + C) * r^2 + 1)
+    Packet p3_hi, p3_lo;
+    fast_twosum(one, p2_hi, p2_lo, p3_hi, p3_lo);
+
+    // log(z) ~= ((Q(r^2) * r^2 + C) * r^2 + 1) * r
+    twoprod(p3_hi, p3_lo, r_hi, r_lo, log2_x_hi, log2_x_lo);
+  }
+};
+
+// This function computes exp2(x) (i.e. 2**x).
+template <typename Scalar>
+struct fast_accurate_exp2 {
+  template <typename Packet>
+  EIGEN_STRONG_INLINE
+  Packet operator()(const Packet& x) {
+    // TODO(rmlarsen): Add a pexp2 packetop.
+    return pexp(pmul(pset1<Packet>(Scalar(EIGEN_LN2)), x));
+  }
+};
+
+// This specialization uses a faster algorithm to compute exp2(x) for floats
+// in [-0.5;0.5] with a relative accuracy of 1 ulp.
+// The minimax polynomial used was calculated using the Sollya tool.
+// See sollya.org.
+template <>
+struct fast_accurate_exp2<float> {
+  template <typename Packet>
+  EIGEN_STRONG_INLINE
+  Packet operator()(const Packet& x) {
+    // This function approximates exp2(x) by a degree 6 polynomial of the form
+    // Q(x) = 1 + x * (C + x * P(x)), where the degree 4 polynomial P(x) is evaluated in
+    // single precision, and the remaining steps are evaluated with extra precision using
+    // double word arithmetic. C is an extra precise constant stored as a double word.
+    //
+    // The polynomial coefficients were calculated using Sollya commands:
+    // > n = 6;
+    // > f = 2^x;
+    // > interval = [-0.5;0.5];
+    // > p = fpminimax(f,n,[|1,double,single...|],interval,relative,floating);
+
+    const Packet p4 = pset1<Packet>(1.539513905e-4f);
+    const Packet p3 = pset1<Packet>(1.340007293e-3f);
+    const Packet p2 = pset1<Packet>(9.618283249e-3f);
+    const Packet p1 = pset1<Packet>(5.550328270e-2f);
+    const Packet p0 = pset1<Packet>(0.2402264923f);
+
+    const Packet C_hi = pset1<Packet>(0.6931471825f);
+    const Packet C_lo = pset1<Packet>(2.36836577e-08f);
+    const Packet one = pset1<Packet>(1.0f);
+
+    // Evaluate P(x) in working precision.
+    // We evaluate even and odd parts of the polynomial separately
+    // to gain some instruction level parallelism.
+    Packet x2 = pmul(x,x);
+    Packet p_even = pmadd(p4, x2, p2);
+    Packet p_odd = pmadd(p3, x2, p1);
+    p_even = pmadd(p_even, x2, p0);
+    Packet p = pmadd(p_odd, x, p_even);
+
+    // Evaluate the remaining terms of Q(x) with extra precision using
+    // double word arithmetic.
+    Packet p_hi, p_lo;
+    // x * p(x)
+    twoprod(p, x, p_hi, p_lo);
+    // C + x * p(x)
+    Packet q1_hi, q1_lo;
+    twosum(p_hi, p_lo, C_hi, C_lo, q1_hi, q1_lo);
+    // x * (C + x * p(x))
+    Packet q2_hi, q2_lo;
+    twoprod(q1_hi, q1_lo, x, q2_hi, q2_lo);
+    // 1 + x * (C + x * p(x))
+    Packet q3_hi, q3_lo;
+    // Since |q2_hi| <= sqrt(2)-1 < 1, we can use fast_twosum
+    // for adding it to unity here.
+    fast_twosum(one, q2_hi, q3_hi, q3_lo);
+    return padd(q3_hi, padd(q2_lo, q3_lo));
+  }
+};
+
+// in [-0.5;0.5] with a relative accuracy of 1 ulp.
+// The minimax polynomial used was calculated using the Sollya tool.
+// See sollya.org.
+template <>
+struct fast_accurate_exp2<double> {
+  template <typename Packet>
+  EIGEN_STRONG_INLINE
+  Packet operator()(const Packet& x) {
+    // This function approximates exp2(x) by a degree 10 polynomial of the form
+    // Q(x) = 1 + x * (C + x * P(x)), where the degree 8 polynomial P(x) is evaluated in
+    // single precision, and the remaining steps are evaluated with extra precision using
+    // double word arithmetic. C is an extra precise constant stored as a double word.
+    //
+    // The polynomial coefficients were calculated using Sollya commands:
+    // > n = 11;
+    // > f = 2^x;
+    // > interval = [-0.5;0.5];
+    // > p = fpminimax(f,n,[|1,DD,double...|],interval,relative,floating);
+
+    const Packet p9 = pset1<Packet>(4.431642109085495276e-10);
+    const Packet p8 = pset1<Packet>(7.073829923303358410e-9);
+    const Packet p7 = pset1<Packet>(1.017822306737031311e-7);
+    const Packet p6 = pset1<Packet>(1.321543498017646657e-6);
+    const Packet p5 = pset1<Packet>(1.525273342728892877e-5);
+    const Packet p4 = pset1<Packet>(1.540353045780084423e-4);
+    const Packet p3 = pset1<Packet>(1.333355814685869807e-3);
+    const Packet p2 = pset1<Packet>(9.618129107593478832e-3);
+    const Packet p1 = pset1<Packet>(5.550410866481961247e-2);
+    const Packet p0 = pset1<Packet>(0.240226506959101332);
+    const Packet C_hi = pset1<Packet>(0.693147180559945286); 
+    const Packet C_lo = pset1<Packet>(4.81927865669806721e-17);
+    const Packet one = pset1<Packet>(1.0);
+
+    // Evaluate P(x) in working precision.
+    // We evaluate even and odd parts of the polynomial separately
+    // to gain some instruction level parallelism.
+    Packet x2 = pmul(x,x);
+    Packet p_even = pmadd(p8, x2, p6);
+    Packet p_odd = pmadd(p9, x2, p7);
+    p_even = pmadd(p_even, x2, p4);
+    p_odd = pmadd(p_odd, x2, p5);
+    p_even = pmadd(p_even, x2, p2);
+    p_odd = pmadd(p_odd, x2, p3);
+    p_even = pmadd(p_even, x2, p0);
+    p_odd = pmadd(p_odd, x2, p1);
+    Packet p = pmadd(p_odd, x, p_even);
+
+    // Evaluate the remaining terms of Q(x) with extra precision using
+    // double word arithmetic.
+    Packet p_hi, p_lo;
+    // x * p(x)
+    twoprod(p, x, p_hi, p_lo);
+    // C + x * p(x)
+    Packet q1_hi, q1_lo;
+    twosum(p_hi, p_lo, C_hi, C_lo, q1_hi, q1_lo);
+    // x * (C + x * p(x))
+    Packet q2_hi, q2_lo;
+    twoprod(q1_hi, q1_lo, x, q2_hi, q2_lo);
+    // 1 + x * (C + x * p(x))
+    Packet q3_hi, q3_lo;
+    // Since |q2_hi| <= sqrt(2)-1 < 1, we can use fast_twosum
+    // for adding it to unity here.
+    fast_twosum(one, q2_hi, q3_hi, q3_lo);
+    return padd(q3_hi, padd(q2_lo, q3_lo));
+  }
+};
+
+// This function implements the non-trivial case of pow(x,y) where x is
+// positive and y is (possibly) non-integer.
+// Formally, pow(x,y) = exp2(y * log2(x)), where exp2(x) is shorthand for 2^x.
+// TODO(rmlarsen): We should probably add this as a packet up 'ppow', to make it
+// easier to specialize or turn off for specific types and/or backends.x
+template <typename Packet>
+EIGEN_STRONG_INLINE Packet generic_pow_impl(const Packet& x, const Packet& y) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+  // Split x into exponent e_x and mantissa m_x.
+  Packet e_x;
+  Packet m_x = pfrexp(x, e_x);
+
+  // Adjust m_x to lie in [1/sqrt(2):sqrt(2)] to minimize absolute error in log2(m_x).
+  EIGEN_CONSTEXPR Scalar sqrt_half = Scalar(0.70710678118654752440);
+  const Packet m_x_scale_mask = pcmp_lt(m_x, pset1<Packet>(sqrt_half));
+  m_x = pselect(m_x_scale_mask, pmul(pset1<Packet>(Scalar(2)), m_x), m_x);
+  e_x = pselect(m_x_scale_mask, psub(e_x, pset1<Packet>(Scalar(1))), e_x);
+
+  // Compute log2(m_x) with 6 extra bits of accuracy.
+  Packet rx_hi, rx_lo;
+  accurate_log2<Scalar>()(m_x, rx_hi, rx_lo);
+
+  // Compute the two terms {y * e_x, y * r_x} in f = y * log2(x) with doubled
+  // precision using double word arithmetic.
+  Packet f1_hi, f1_lo, f2_hi, f2_lo;
+  twoprod(e_x, y, f1_hi, f1_lo);
+  twoprod(rx_hi, rx_lo, y, f2_hi, f2_lo);
+  // Sum the two terms in f using double word arithmetic. We know
+  // that |e_x| > |log2(m_x)|, except for the case where e_x==0.
+  // This means that we can use fast_twosum(f1,f2).
+  // In the case e_x == 0, e_x * y = f1 = 0, so we don't lose any
+  // accuracy by violating the assumption of fast_twosum, because
+  // it's a no-op.
+  Packet f_hi, f_lo;
+  fast_twosum(f1_hi, f1_lo, f2_hi, f2_lo, f_hi, f_lo);
+
+  // Split f into integer and fractional parts.
+  Packet n_z, r_z;
+  absolute_split(f_hi, n_z, r_z);
+  r_z = padd(r_z, f_lo);
+  Packet n_r;
+  absolute_split(r_z, n_r, r_z);
+  n_z = padd(n_z, n_r);
+
+  // We now have an accurate split of f = n_z + r_z and can compute
+  //   x^y = 2**{n_z + r_z) = exp2(r_z) * 2**{n_z}.
+  // Since r_z is in [-0.5;0.5], we compute the first factor to high accuracy
+  // using a specialized algorithm. Multiplication by the second factor can
+  // be done exactly using pldexp(), since it is an integer power of 2.
+  const Packet e_r = fast_accurate_exp2<Scalar>()(r_z);
+  return pldexp(e_r, n_z);
+}
+
+// Generic implementation of pow(x,y).
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet generic_pow(const Packet& x, const Packet& y) {
+  typedef typename unpacket_traits<Packet>::type Scalar;
+
+  const Packet cst_pos_inf = pset1<Packet>(NumTraits<Scalar>::infinity());
+  const Packet cst_zero = pset1<Packet>(Scalar(0));
+  const Packet cst_one = pset1<Packet>(Scalar(1));
+  const Packet cst_nan = pset1<Packet>(NumTraits<Scalar>::quiet_NaN());
+
+  const Packet abs_x = pabs(x);
+  // Predicates for sign and magnitude of x.
+  const Packet x_is_zero = pcmp_eq(x, cst_zero);
+  const Packet x_is_neg = pcmp_lt(x, cst_zero);
+  const Packet abs_x_is_inf = pcmp_eq(abs_x, cst_pos_inf);
+  const Packet abs_x_is_one =  pcmp_eq(abs_x, cst_one);
+  const Packet abs_x_is_gt_one = pcmp_lt(cst_one, abs_x);
+  const Packet abs_x_is_lt_one = pcmp_lt(abs_x, cst_one);
+  const Packet x_is_one =  pandnot(abs_x_is_one, x_is_neg);
+  const Packet x_is_neg_one =  pand(abs_x_is_one, x_is_neg);
+  const Packet x_is_nan = pandnot(ptrue(x), pcmp_eq(x, x));
+
+  // Predicates for sign and magnitude of y.
+  const Packet y_is_one = pcmp_eq(y, cst_one);
+  const Packet y_is_zero = pcmp_eq(y, cst_zero);
+  const Packet y_is_neg = pcmp_lt(y, cst_zero);
+  const Packet y_is_pos = pandnot(ptrue(y), por(y_is_zero, y_is_neg));
+  const Packet y_is_nan = pandnot(ptrue(y), pcmp_eq(y, y));
+  const Packet abs_y_is_inf = pcmp_eq(pabs(y), cst_pos_inf);
+  EIGEN_CONSTEXPR Scalar huge_exponent =
+      (NumTraits<Scalar>::max_exponent() * Scalar(EIGEN_LN2)) /
+       NumTraits<Scalar>::epsilon();
+  const Packet abs_y_is_huge = pcmp_le(pset1<Packet>(huge_exponent), pabs(y));
+
+  // Predicates for whether y is integer and/or even.
+  const Packet y_is_int = pcmp_eq(pfloor(y), y);
+  const Packet y_div_2 = pmul(y, pset1<Packet>(Scalar(0.5)));
+  const Packet y_is_even = pcmp_eq(pround(y_div_2), y_div_2);
+
+  // Predicates encoding special cases for the value of pow(x,y)
+  const Packet invalid_negative_x = pandnot(pandnot(pandnot(x_is_neg, abs_x_is_inf),
+                                                    y_is_int),
+                                            abs_y_is_inf);
+  const Packet pow_is_one = por(por(x_is_one, y_is_zero),
+                                pand(x_is_neg_one,
+                                     por(abs_y_is_inf, pandnot(y_is_even, invalid_negative_x))));
+  const Packet pow_is_nan = por(invalid_negative_x, por(x_is_nan, y_is_nan));
+  const Packet pow_is_zero = por(por(por(pand(x_is_zero, y_is_pos),
+                                         pand(abs_x_is_inf, y_is_neg)),
+                                     pand(pand(abs_x_is_lt_one, abs_y_is_huge),
+                                          y_is_pos)),
+                                 pand(pand(abs_x_is_gt_one, abs_y_is_huge),
+                                      y_is_neg));
+  const Packet pow_is_inf = por(por(por(pand(x_is_zero, y_is_neg),
+                                        pand(abs_x_is_inf, y_is_pos)),
+                                    pand(pand(abs_x_is_lt_one, abs_y_is_huge),
+                                         y_is_neg)),
+                                pand(pand(abs_x_is_gt_one, abs_y_is_huge),
+                                     y_is_pos));
+
+  // General computation of pow(x,y) for positive x or negative x and integer y.
+  const Packet negate_pow_abs = pandnot(x_is_neg, y_is_even);
+  const Packet pow_abs = generic_pow_impl(abs_x, y);
+  return pselect(y_is_one, x,
+                 pselect(pow_is_one, cst_one,
+                         pselect(pow_is_nan, cst_nan,
+                                 pselect(pow_is_inf, cst_pos_inf,
+                                         pselect(pow_is_zero, cst_zero,
+                                                 pselect(negate_pow_abs, pnegate(pow_abs), pow_abs))))));
+}
+
+
+
+/* polevl (modified for Eigen)
+ *
+ *      Evaluate polynomial
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * int N;
+ * Scalar x, y, coef[N+1];
+ *
+ * y = polevl<decltype(x), N>( x, coef);
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates polynomial of degree N:
+ *
+ *                     2          N
+ * y  =  C  + C x + C x  +...+ C x
+ *        0    1     2          N
+ *
+ * Coefficients are stored in reverse order:
+ *
+ * coef[0] = C  , ..., coef[N] = C  .
+ *            N                   0
+ *
+ *  The function p1evl() assumes that coef[N] = 1.0 and is
+ * omitted from the array.  Its calling arguments are
+ * otherwise the same as polevl().
+ *
+ *
+ * The Eigen implementation is templatized.  For best speed, store
+ * coef as a const array (constexpr), e.g.
+ *
+ * const double coef[] = {1.0, 2.0, 3.0, ...};
+ *
+ */
+template <typename Packet, int N>
+struct ppolevl {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const typename unpacket_traits<Packet>::type coeff[]) {
+    EIGEN_STATIC_ASSERT((N > 0), YOU_MADE_A_PROGRAMMING_MISTAKE);
+    return pmadd(ppolevl<Packet, N-1>::run(x, coeff), x, pset1<Packet>(coeff[N]));
+  }
+};
+
+template <typename Packet>
+struct ppolevl<Packet, 0> {
+  static EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet run(const Packet& x, const typename unpacket_traits<Packet>::type coeff[]) {
+    EIGEN_UNUSED_VARIABLE(x);
+    return pset1<Packet>(coeff[0]);
+  }
+};
+
+/* chbevl (modified for Eigen)
+ *
+ *     Evaluate Chebyshev series
+ *
+ *
+ *
+ * SYNOPSIS:
+ *
+ * int N;
+ * Scalar x, y, coef[N], chebevl();
+ *
+ * y = chbevl( x, coef, N );
+ *
+ *
+ *
+ * DESCRIPTION:
+ *
+ * Evaluates the series
+ *
+ *        N-1
+ *         - '
+ *  y  =   >   coef[i] T (x/2)
+ *         -            i
+ *        i=0
+ *
+ * of Chebyshev polynomials Ti at argument x/2.
+ *
+ * Coefficients are stored in reverse order, i.e. the zero
+ * order term is last in the array.  Note N is the number of
+ * coefficients, not the order.
+ *
+ * If coefficients are for the interval a to b, x must
+ * have been transformed to x -> 2(2x - b - a)/(b-a) before
+ * entering the routine.  This maps x from (a, b) to (-1, 1),
+ * over which the Chebyshev polynomials are defined.
+ *
+ * If the coefficients are for the inverted interval, in
+ * which (a, b) is mapped to (1/b, 1/a), the transformation
+ * required is x -> 2(2ab/x - b - a)/(b-a).  If b is infinity,
+ * this becomes x -> 4a/x - 1.
+ *
+ *
+ *
+ * SPEED:
+ *
+ * Taking advantage of the recurrence properties of the
+ * Chebyshev polynomials, the routine requires one more
+ * addition per loop than evaluating a nested polynomial of
+ * the same degree.
+ *
+ */
+
+template <typename Packet, int N>
+struct pchebevl {
+  EIGEN_DEVICE_FUNC
+  static EIGEN_STRONG_INLINE Packet run(Packet x, const typename unpacket_traits<Packet>::type coef[]) {
+    typedef typename unpacket_traits<Packet>::type Scalar;
+    Packet b0 = pset1<Packet>(coef[0]);
+    Packet b1 = pset1<Packet>(static_cast<Scalar>(0.f));
+    Packet b2;
+
+    for (int i = 1; i < N; i++) {
+      b2 = b1;
+      b1 = b0;
+      b0 = psub(pmadd(x, b1, pset1<Packet>(coef[i])), b2);
+    }
+
+    return pmul(pset1<Packet>(static_cast<Scalar>(0.5f)), psub(b0, b2));
+  }
+};
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
new file mode 100644
index 0000000..177a04e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
@@ -0,0 +1,110 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2019 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
+#define EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
+
+namespace Eigen {
+namespace internal {
+
+// Forward declarations of the generic math functions
+// implemented in GenericPacketMathFunctions.h
+// This is needed to workaround a circular dependency.
+
+/***************************************************************************
+ * Some generic implementations to be used by implementors
+***************************************************************************/
+
+/** Default implementation of pfrexp.
+  * It is expected to be called by implementers of template<> pfrexp.
+  */
+template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+Packet pfrexp_generic(const Packet& a, Packet& exponent);
+
+// Extracts the biased exponent value from Packet p, and casts the results to
+// a floating-point Packet type. Used by pfrexp_generic. Override this if
+// there is no unpacket_traits<Packet>::integer_packet.
+template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+Packet pfrexp_generic_get_biased_exponent(const Packet& p);
+
+/** Default implementation of pldexp.
+  * It is expected to be called by implementers of template<> pldexp.
+  */
+template<typename Packet> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
+Packet pldexp_generic(const Packet& a, const Packet& exponent);
+
+/** \internal \returns log(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog_float(const Packet _x);
+
+/** \internal \returns log2(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog2_float(const Packet _x);
+
+/** \internal \returns log(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog_double(const Packet _x);
+
+/** \internal \returns log2(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet plog2_double(const Packet _x);
+
+/** \internal \returns log(1 + x) */
+template<typename Packet>
+Packet generic_plog1p(const Packet& x);
+
+/** \internal \returns exp(x)-1 */
+template<typename Packet>
+Packet generic_expm1(const Packet& x);
+
+/** \internal \returns exp(x) for single precision float */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet pexp_float(const Packet _x);
+
+/** \internal \returns exp(x) for double precision real numbers */
+template <typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet pexp_double(const Packet _x);
+
+/** \internal \returns sin(x) for single precision float */
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet psin_float(const Packet& x);
+
+/** \internal \returns cos(x) for single precision float */
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet pcos_float(const Packet& x);
+
+/** \internal \returns sqrt(x) for complex types */
+template<typename Packet>
+EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
+EIGEN_UNUSED
+Packet psqrt_complex(const Packet& a);
+
+template <typename Packet, int N> struct ppolevl;
+
+
+} // end namespace internal
+} // end namespace Eigen
+
+#endif // EIGEN_ARCH_GENERIC_PACKET_MATH_FUNCTIONS_FWD_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h
new file mode 100644
index 0000000..9f8e8cc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Half.h
@@ -0,0 +1,942 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+//
+// The conversion routines are Copyright (c) Fabian Giesen, 2016.
+// The original license follows:
+//
+// Copyright (c) Fabian Giesen, 2016
+// All rights reserved.
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted.
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+// Standard 16-bit float type, mostly useful for GPUs. Defines a new
+// type Eigen::half (inheriting either from CUDA's or HIP's __half struct) with
+// operator overloads such that it behaves basically as an arithmetic
+// type. It will be quite slow on CPUs (so it is recommended to stay
+// in fp32 for CPUs, except for simple parameter conversions, I/O
+// to disk and the likes), but fast on GPUs.
+
+
+#ifndef EIGEN_HALF_H
+#define EIGEN_HALF_H
+
+#include <sstream>
+
+#if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+// When compiling with GPU support, the "__half_raw" base class as well as
+// some other routines are defined in the GPU compiler header files
+// (cuda_fp16.h, hip_fp16.h), and they are not tagged constexpr
+// As a consequence, we get compile failures when compiling Eigen with
+// GPU support. Hence the need to disable EIGEN_CONSTEXPR when building
+// Eigen with GPU support
+  #pragma push_macro("EIGEN_CONSTEXPR")
+  #undef EIGEN_CONSTEXPR
+  #define EIGEN_CONSTEXPR
+#endif
+
+#define F16_PACKET_FUNCTION(PACKET_F, PACKET_F16, METHOD)           \
+  template <>                                                       \
+  EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_UNUSED                \
+  PACKET_F16 METHOD<PACKET_F16>(const PACKET_F16& _x) {             \
+    return float2half(METHOD<PACKET_F>(half2float(_x)));            \
+  }
+
+namespace Eigen {
+
+struct half;
+
+namespace half_impl {
+
+// We want to use the __half_raw struct from the HIP header file only during the device compile phase.
+// This is required because of a quirk in the way TensorFlow GPU builds are done.
+// When compiling TensorFlow source code with GPU support, files that
+//  * contain GPU kernels (i.e. *.cu.cc files) are compiled via hipcc
+//  * do not contain GPU kernels ( i.e. *.cc files) are compiled via gcc (typically)
+//
+// Tensorflow uses the Eigen::half type as its FP16 type, and there are functions that
+//  * are defined in a file that gets compiled via hipcc AND
+//  * have Eigen::half as a pass-by-value argument AND
+//  * are called in a file that gets compiled via gcc
+//
+// In the scenario described above the caller and callee will see different versions
+// of the Eigen::half base class __half_raw, and they will be compiled by different compilers
+//
+// There appears to be an ABI mismatch between gcc and clang (which is called by hipcc) that results in
+// the callee getting corrupted values for the Eigen::half argument.
+//
+// Making the host side compile phase of hipcc use the same Eigen::half impl, as the gcc compile, resolves
+// this error, and hence the following convoluted #if condition
+#if !defined(EIGEN_HAS_GPU_FP16) || !defined(EIGEN_GPU_COMPILE_PHASE)
+// Make our own __half_raw definition that is similar to CUDA's.
+struct __half_raw {
+#if (defined(EIGEN_HAS_GPU_FP16) && !defined(EIGEN_GPU_COMPILE_PHASE))
+  // Eigen::half can be used as the datatype for shared memory declarations (in Eigen and TF)
+  // The element type for shared memory cannot have non-trivial constructors
+  // and hence the following special casing (which skips the zero-initilization).
+  // Note that this check gets done even in the host compilation phase, and
+  // hence the need for this
+  EIGEN_DEVICE_FUNC __half_raw() {}
+#else
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw() : x(0) {}
+#endif
+#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(numext::bit_cast<__fp16>(raw)) {
+  }
+  __fp16 x;
+#else
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw(numext::uint16_t raw) : x(raw) {}
+  numext::uint16_t x;
+#endif
+};
+
+#elif defined(EIGEN_HAS_HIP_FP16)
+  // Nothing to do here
+  // HIP fp16 header file has a definition for __half_raw
+#elif defined(EIGEN_HAS_CUDA_FP16)
+  #if EIGEN_CUDA_SDK_VER < 90000
+    // In CUDA < 9.0, __half is the equivalent of CUDA 9's __half_raw
+    typedef __half __half_raw;
+  #endif // defined(EIGEN_HAS_CUDA_FP16)
+#elif defined(SYCL_DEVICE_ONLY)
+  typedef cl::sycl::half __half_raw;
+#endif
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff);
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h);
+
+struct half_base : public __half_raw {
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base() {}
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half_raw& h) : __half_raw(h) {}
+
+#if defined(EIGEN_HAS_GPU_FP16)
+ #if defined(EIGEN_HAS_HIP_FP16)
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half& h) { x = __half_as_ushort(h); }
+ #elif defined(EIGEN_HAS_CUDA_FP16)
+  #if EIGEN_CUDA_SDK_VER >= 90000
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half_base(const __half& h) : __half_raw(*(__half_raw*)&h) {}
+  #endif
+ #endif
+#endif
+};
+
+} // namespace half_impl
+
+// Class definition.
+struct half : public half_impl::half_base {
+
+  // Writing this out as separate #if-else blocks to make the code easier to follow
+  // The same applies to most #if-else blocks in this file
+#if !defined(EIGEN_HAS_GPU_FP16) || !defined(EIGEN_GPU_COMPILE_PHASE)
+  // Use the same base class for the following two scenarios
+  // * when compiling without GPU support enabled
+  // * during host compile phase when compiling with GPU support enabled
+  typedef half_impl::__half_raw __half_raw;
+#elif defined(EIGEN_HAS_HIP_FP16)
+  // Nothing to do here
+  // HIP fp16 header file has a definition for __half_raw
+#elif defined(EIGEN_HAS_CUDA_FP16)
+  // Note that EIGEN_CUDA_SDK_VER is set to 0 even when compiling with HIP, so
+  // (EIGEN_CUDA_SDK_VER < 90000) is true even for HIP!  So keeping this within
+  // #if defined(EIGEN_HAS_CUDA_FP16) is needed
+  #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER < 90000
+    typedef half_impl::__half_raw __half_raw;
+  #endif
+#endif
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half() {}
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half_raw& h) : half_impl::half_base(h) {}
+
+#if defined(EIGEN_HAS_GPU_FP16)
+ #if defined(EIGEN_HAS_HIP_FP16)
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {}
+ #elif defined(EIGEN_HAS_CUDA_FP16)
+  #if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(const __half& h) : half_impl::half_base(h) {}
+  #endif
+ #endif
+#endif
+
+
+  explicit EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR half(bool b)
+      : half_impl::half_base(half_impl::raw_uint16_to_half(b ? 0x3c00 : 0)) {}
+  template<class T>
+  explicit EIGEN_DEVICE_FUNC half(T val)
+      : half_impl::half_base(half_impl::float_to_half_rtne(static_cast<float>(val))) {}
+  explicit EIGEN_DEVICE_FUNC half(float f)
+      : half_impl::half_base(half_impl::float_to_half_rtne(f)) {}
+
+  // Following the convention of numpy, converting between complex and
+  // float will lead to loss of imag value.
+  template<typename RealScalar>
+  explicit EIGEN_DEVICE_FUNC half(std::complex<RealScalar> c)
+      : half_impl::half_base(half_impl::float_to_half_rtne(static_cast<float>(c.real()))) {}
+
+   EIGEN_DEVICE_FUNC operator float() const {  // NOLINT: Allow implicit conversion to float, because it is lossless.
+    return half_impl::half_to_float(*this);
+  }
+
+#if defined(EIGEN_HAS_GPU_FP16) && !defined(EIGEN_GPU_COMPILE_PHASE)
+  EIGEN_DEVICE_FUNC operator __half() const {
+    ::__half_raw hr;
+    hr.x = x;
+    return __half(hr);
+  }
+#endif
+};
+
+} // end namespace Eigen
+
+namespace std {
+template<>
+struct numeric_limits<Eigen::half> {
+  static const bool is_specialized = true;
+  static const bool is_signed = true;
+  static const bool is_integer = false;
+  static const bool is_exact = false;
+  static const bool has_infinity = true;
+  static const bool has_quiet_NaN = true;
+  static const bool has_signaling_NaN = true;
+  static const float_denorm_style has_denorm = denorm_present;
+  static const bool has_denorm_loss = false;
+  static const std::float_round_style round_style = std::round_to_nearest;
+  static const bool is_iec559 = false;
+  static const bool is_bounded = false;
+  static const bool is_modulo = false;
+  static const int digits = 11;
+  static const int digits10 = 3;      // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
+  static const int max_digits10 = 5;  // according to http://half.sourceforge.net/structstd_1_1numeric__limits_3_01half__float_1_1half_01_4.html
+  static const int radix = 2;
+  static const int min_exponent = -13;
+  static const int min_exponent10 = -4;
+  static const int max_exponent = 16;
+  static const int max_exponent10 = 4;
+  static const bool traps = true;
+  static const bool tinyness_before = false;
+
+  static Eigen::half (min)() { return Eigen::half_impl::raw_uint16_to_half(0x400); }
+  static Eigen::half lowest() { return Eigen::half_impl::raw_uint16_to_half(0xfbff); }
+  static Eigen::half (max)() { return Eigen::half_impl::raw_uint16_to_half(0x7bff); }
+  static Eigen::half epsilon() { return Eigen::half_impl::raw_uint16_to_half(0x0800); }
+  static Eigen::half round_error() { return Eigen::half(0.5); }
+  static Eigen::half infinity() { return Eigen::half_impl::raw_uint16_to_half(0x7c00); }
+  static Eigen::half quiet_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7e00); }
+  static Eigen::half signaling_NaN() { return Eigen::half_impl::raw_uint16_to_half(0x7d00); }
+  static Eigen::half denorm_min() { return Eigen::half_impl::raw_uint16_to_half(0x1); }
+};
+
+// If std::numeric_limits<T> is specialized, should also specialize
+// std::numeric_limits<const T>, std::numeric_limits<volatile T>, and
+// std::numeric_limits<const volatile T>
+// https://stackoverflow.com/a/16519653/
+template<>
+struct numeric_limits<const Eigen::half> : numeric_limits<Eigen::half> {};
+template<>
+struct numeric_limits<volatile Eigen::half> : numeric_limits<Eigen::half> {};
+template<>
+struct numeric_limits<const volatile Eigen::half> : numeric_limits<Eigen::half> {};
+} // end namespace std
+
+namespace Eigen {
+
+namespace half_impl {
+
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && \
+     EIGEN_CUDA_ARCH >= 530) ||                                  \
+    (defined(EIGEN_HAS_HIP_FP16) && defined(HIP_DEVICE_COMPILE))
+// Note: We deliberatly do *not* define this to 1 even if we have Arm's native
+// fp16 type since GPU halfs are rather different from native CPU halfs.
+// TODO: Rename to something like EIGEN_HAS_NATIVE_GPU_FP16
+#define EIGEN_HAS_NATIVE_FP16
+#endif
+
+// Intrinsics for native fp16 support. Note that on current hardware,
+// these are no faster than fp32 arithmetic (you need to use the half2
+// versions to get the ALU speed increased), but you do save the
+// conversion steps back and forth.
+
+#if defined(EIGEN_HAS_NATIVE_FP16)
+EIGEN_STRONG_INLINE __device__ half operator + (const half& a, const half& b) {
+#if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
+  return __hadd(::__half(a), ::__half(b));
+#else
+  return __hadd(a, b);
+#endif
+}
+EIGEN_STRONG_INLINE __device__ half operator * (const half& a, const half& b) {
+  return __hmul(a, b);
+}
+EIGEN_STRONG_INLINE __device__ half operator - (const half& a, const half& b) {
+  return __hsub(a, b);
+}
+EIGEN_STRONG_INLINE __device__ half operator / (const half& a, const half& b) {
+#if defined(EIGEN_CUDA_SDK_VER) && EIGEN_CUDA_SDK_VER >= 90000
+  return __hdiv(a, b);
+#else
+  float num = __half2float(a);
+  float denom = __half2float(b);
+  return __float2half(num / denom);
+#endif
+}
+EIGEN_STRONG_INLINE __device__ half operator - (const half& a) {
+  return __hneg(a);
+}
+EIGEN_STRONG_INLINE __device__ half& operator += (half& a, const half& b) {
+  a = a + b;
+  return a;
+}
+EIGEN_STRONG_INLINE __device__ half& operator *= (half& a, const half& b) {
+  a = a * b;
+  return a;
+}
+EIGEN_STRONG_INLINE __device__ half& operator -= (half& a, const half& b) {
+  a = a - b;
+  return a;
+}
+EIGEN_STRONG_INLINE __device__ half& operator /= (half& a, const half& b) {
+  a = a / b;
+  return a;
+}
+EIGEN_STRONG_INLINE __device__ bool operator == (const half& a, const half& b) {
+  return __heq(a, b);
+}
+EIGEN_STRONG_INLINE __device__ bool operator != (const half& a, const half& b) {
+  return __hne(a, b);
+}
+EIGEN_STRONG_INLINE __device__ bool operator < (const half& a, const half& b) {
+  return __hlt(a, b);
+}
+EIGEN_STRONG_INLINE __device__ bool operator <= (const half& a, const half& b) {
+  return __hle(a, b);
+}
+EIGEN_STRONG_INLINE __device__ bool operator > (const half& a, const half& b) {
+  return __hgt(a, b);
+}
+EIGEN_STRONG_INLINE __device__ bool operator >= (const half& a, const half& b) {
+  return __hge(a, b);
+}
+#endif
+
+#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator + (const half& a, const half& b) {
+  return half(vaddh_f16(a.x, b.x));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator * (const half& a, const half& b) {
+  return half(vmulh_f16(a.x, b.x));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a, const half& b) {
+  return half(vsubh_f16(a.x, b.x));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, const half& b) {
+  return half(vdivh_f16(a.x, b.x));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a) {
+  return half(vnegh_f16(a.x));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator += (half& a, const half& b) {
+  a = half(vaddh_f16(a.x, b.x));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator *= (half& a, const half& b) {
+  a = half(vmulh_f16(a.x, b.x));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator -= (half& a, const half& b) {
+  a = half(vsubh_f16(a.x, b.x));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator /= (half& a, const half& b) {
+  a = half(vdivh_f16(a.x, b.x));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const half& a, const half& b) {
+  return vceqh_f16(a.x, b.x);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const half& a, const half& b) {
+  return !vceqh_f16(a.x, b.x);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const half& a, const half& b) {
+  return vclth_f16(a.x, b.x);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator <= (const half& a, const half& b) {
+  return vcleh_f16(a.x, b.x);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator > (const half& a, const half& b) {
+  return vcgth_f16(a.x, b.x);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator >= (const half& a, const half& b) {
+  return vcgeh_f16(a.x, b.x);
+}
+// We need to distinguish ‘clang as the CUDA compiler’ from ‘clang as the host compiler,
+// invoked by NVCC’ (e.g. on MacOS). The former needs to see both host and device implementation
+// of the functions, while the latter can only deal with one of them.
+#elif !defined(EIGEN_HAS_NATIVE_FP16) || (EIGEN_COMP_CLANG && !EIGEN_COMP_NVCC) // Emulate support for half floats
+
+#if EIGEN_COMP_CLANG && defined(EIGEN_CUDACC)
+// We need to provide emulated *host-side* FP16 operators for clang.
+#pragma push_macro("EIGEN_DEVICE_FUNC")
+#undef EIGEN_DEVICE_FUNC
+#if defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_HAS_NATIVE_FP16)
+#define EIGEN_DEVICE_FUNC __host__
+#else // both host and device need emulated ops.
+#define EIGEN_DEVICE_FUNC __host__ __device__
+#endif
+#endif
+
+// Definitions for CPUs and older HIP+CUDA, mostly working through conversion
+// to/from fp32.
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator + (const half& a, const half& b) {
+  return half(float(a) + float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator * (const half& a, const half& b) {
+  return half(float(a) * float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a, const half& b) {
+  return half(float(a) - float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, const half& b) {
+  return half(float(a) / float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator - (const half& a) {
+  half result;
+  result.x = a.x ^ 0x8000;
+  return result;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator += (half& a, const half& b) {
+  a = half(float(a) + float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator *= (half& a, const half& b) {
+  a = half(float(a) * float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator -= (half& a, const half& b) {
+  a = half(float(a) - float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half& operator /= (half& a, const half& b) {
+  a = half(float(a) / float(b));
+  return a;
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator == (const half& a, const half& b) {
+  return numext::equal_strict(float(a),float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator != (const half& a, const half& b) {
+  return numext::not_equal_strict(float(a), float(b));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator < (const half& a, const half& b) {
+  return float(a) < float(b);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator <= (const half& a, const half& b) {
+  return float(a) <= float(b);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator > (const half& a, const half& b) {
+  return float(a) > float(b);
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool operator >= (const half& a, const half& b) {
+  return float(a) >= float(b);
+}
+
+#if defined(__clang__) && defined(__CUDA__)
+#pragma pop_macro("EIGEN_DEVICE_FUNC")
+#endif
+#endif  // Emulate support for half floats
+
+// Division by an index. Do it in full float precision to avoid accuracy
+// issues in converting the denominator to half.
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator / (const half& a, Index b) {
+  return half(static_cast<float>(a) / static_cast<float>(b));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator++(half& a) {
+  a += half(1);
+  return a;
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator--(half& a) {
+  a -= half(1);
+  return a;
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator++(half& a, int) {
+  half original_value = a;
+  ++a;
+  return original_value;
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half operator--(half& a, int) {
+  half original_value = a;
+  --a;
+  return original_value;
+}
+
+// Conversion routines, including fallbacks for the host or older CUDA.
+// Note that newer Intel CPUs (Haswell or newer) have vectorized versions of
+// these in hardware. If we need more performance on older/other CPUs, they are
+// also possible to vectorize directly.
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR __half_raw raw_uint16_to_half(numext::uint16_t x) {
+  // We cannot simply do a "return __half_raw(x)" here, because __half_raw is union type
+  // in the hip_fp16 header file, and that will trigger a compile error
+  // On the other hand, having anything but a return statement also triggers a compile error
+  // because this is constexpr function.
+  // Fortunately, since we need to disable EIGEN_CONSTEXPR for GPU anyway, we can get out
+  // of this catch22 by having separate bodies for GPU / non GPU
+#if defined(EIGEN_HAS_GPU_FP16)
+   __half_raw h;
+   h.x = x;
+  return h;
+#else
+  return __half_raw(x);
+#endif
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC numext::uint16_t raw_half_as_uint16(const __half_raw& h) {
+  // HIP/CUDA/Default have a member 'x' of type uint16_t.
+  // For ARM64 native half, the member 'x' is of type __fp16, so we need to bit-cast.
+  // For SYCL, cl::sycl::half is _Float16, so cast directly.
+#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  return numext::bit_cast<numext::uint16_t>(h.x);
+#elif defined(SYCL_DEVICE_ONLY)
+  return numext::bit_cast<numext::uint16_t>(h);
+#else
+  return h.x;
+#endif
+}
+
+union float32_bits {
+  unsigned int u;
+  float f;
+};
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC __half_raw float_to_half_rtne(float ff) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+  __half tmp_ff = __float2half(ff);
+  return *(__half_raw*)&tmp_ff;
+
+#elif defined(EIGEN_HAS_FP16_C)
+  __half_raw h;
+  h.x = _cvtss_sh(ff, 0);
+  return h;
+
+#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  __half_raw h;
+  h.x = static_cast<__fp16>(ff);
+  return h;
+
+#else
+  float32_bits f; f.f = ff;
+
+  const float32_bits f32infty = { 255 << 23 };
+  const float32_bits f16max = { (127 + 16) << 23 };
+  const float32_bits denorm_magic = { ((127 - 15) + (23 - 10) + 1) << 23 };
+  unsigned int sign_mask = 0x80000000u;
+  __half_raw o;
+  o.x = static_cast<numext::uint16_t>(0x0u);
+
+  unsigned int sign = f.u & sign_mask;
+  f.u ^= sign;
+
+  // NOTE all the integer compares in this function can be safely
+  // compiled into signed compares since all operands are below
+  // 0x80000000. Important if you want fast straight SSE2 code
+  // (since there's no unsigned PCMPGTD).
+
+  if (f.u >= f16max.u) {  // result is Inf or NaN (all exponent bits set)
+    o.x = (f.u > f32infty.u) ? 0x7e00 : 0x7c00; // NaN->qNaN and Inf->Inf
+  } else {  // (De)normalized number or zero
+    if (f.u < (113 << 23)) {  // resulting FP16 is subnormal or zero
+      // use a magic value to align our 10 mantissa bits at the bottom of
+      // the float. as long as FP addition is round-to-nearest-even this
+      // just works.
+      f.f += denorm_magic.f;
+
+      // and one integer subtract of the bias later, we have our final float!
+      o.x = static_cast<numext::uint16_t>(f.u - denorm_magic.u);
+    } else {
+      unsigned int mant_odd = (f.u >> 13) & 1; // resulting mantissa is odd
+
+      // update exponent, rounding bias part 1
+      // Equivalent to `f.u += ((unsigned int)(15 - 127) << 23) + 0xfff`, but
+      // without arithmetic overflow.
+      f.u += 0xc8000fffU;
+      // rounding bias part 2
+      f.u += mant_odd;
+      // take the bits!
+      o.x = static_cast<numext::uint16_t>(f.u >> 13);
+    }
+  }
+
+  o.x |= static_cast<numext::uint16_t>(sign >> 16);
+  return o;
+#endif
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC float half_to_float(__half_raw h) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+  return __half2float(h);
+#elif defined(EIGEN_HAS_FP16_C)
+  return _cvtsh_ss(h.x);
+#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  return static_cast<float>(h.x);
+#else
+  const float32_bits magic = { 113 << 23 };
+  const unsigned int shifted_exp = 0x7c00 << 13; // exponent mask after shift
+  float32_bits o;
+
+  o.u = (h.x & 0x7fff) << 13;             // exponent/mantissa bits
+  unsigned int exp = shifted_exp & o.u;   // just the exponent
+  o.u += (127 - 15) << 23;                // exponent adjust
+
+  // handle exponent special cases
+  if (exp == shifted_exp) {     // Inf/NaN?
+    o.u += (128 - 16) << 23;    // extra exp adjust
+  } else if (exp == 0) {        // Zero/Denormal?
+    o.u += 1 << 23;             // extra exp adjust
+    o.f -= magic.f;             // renormalize
+  }
+
+  o.u |= (h.x & 0x8000) << 16;    // sign bit
+  return o.f;
+#endif
+}
+
+// --- standard functions ---
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isinf)(const half& a) {
+#ifdef EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC
+  return (numext::bit_cast<numext::uint16_t>(a.x) & 0x7fff) == 0x7c00;
+#else
+  return (a.x & 0x7fff) == 0x7c00;
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isnan)(const half& a) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
+  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+  return __hisnan(a);
+#elif defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  return (numext::bit_cast<numext::uint16_t>(a.x) & 0x7fff) > 0x7c00;
+#else
+  return (a.x & 0x7fff) > 0x7c00;
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC bool (isfinite)(const half& a) {
+  return !(isinf EIGEN_NOT_A_MACRO (a)) && !(isnan EIGEN_NOT_A_MACRO (a));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half abs(const half& a) {
+#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  return half(vabsh_f16(a.x));
+#else
+  half result;
+  result.x = a.x & 0x7FFF;
+  return result;
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half exp(const half& a) {
+#if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530) || \
+  defined(EIGEN_HIP_DEVICE_COMPILE)
+  return half(hexp(a));
+#else
+   return half(::expf(float(a)));
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half expm1(const half& a) {
+  return half(numext::expm1(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log(const half& a) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && EIGEN_CUDA_SDK_VER >= 80000 && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
+  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+  return half(::hlog(a));
+#else
+  return half(::logf(float(a)));
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log1p(const half& a) {
+  return half(numext::log1p(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log10(const half& a) {
+  return half(::log10f(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half log2(const half& a) {
+  return half(static_cast<float>(EIGEN_LOG2E) * ::logf(float(a)));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half sqrt(const half& a) {
+#if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 530) || \
+  defined(EIGEN_HIP_DEVICE_COMPILE)
+  return half(hsqrt(a));
+#else
+    return half(::sqrtf(float(a)));
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half pow(const half& a, const half& b) {
+  return half(::powf(float(a), float(b)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half sin(const half& a) {
+  return half(::sinf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half cos(const half& a) {
+  return half(::cosf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tan(const half& a) {
+  return half(::tanf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half tanh(const half& a) {
+  return half(::tanhf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half asin(const half& a) {
+  return half(::asinf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half acos(const half& a) {
+  return half(::acosf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half floor(const half& a) {
+#if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 300) || \
+  defined(EIGEN_HIP_DEVICE_COMPILE)
+  return half(hfloor(a));
+#else
+  return half(::floorf(float(a)));
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half ceil(const half& a) {
+#if (EIGEN_CUDA_SDK_VER >= 80000 && defined EIGEN_CUDA_ARCH && EIGEN_CUDA_ARCH >= 300) || \
+  defined(EIGEN_HIP_DEVICE_COMPILE)
+  return half(hceil(a));
+#else
+  return half(::ceilf(float(a)));
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half rint(const half& a) {
+  return half(::rintf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half round(const half& a) {
+  return half(::roundf(float(a)));
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half fmod(const half& a, const half& b) {
+  return half(::fmodf(float(a), float(b)));
+}
+
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (min)(const half& a, const half& b) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
+  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+  return __hlt(b, a) ? b : a;
+#else
+  const float f1 = static_cast<float>(a);
+  const float f2 = static_cast<float>(b);
+  return f2 < f1 ? b : a;
+#endif
+}
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC half (max)(const half& a, const half& b) {
+#if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 530) || \
+  (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+  return __hlt(a, b) ? b : a;
+#else
+  const float f1 = static_cast<float>(a);
+  const float f2 = static_cast<float>(b);
+  return f1 < f2 ? b : a;
+#endif
+}
+
+#ifndef EIGEN_NO_IO
+EIGEN_ALWAYS_INLINE std::ostream& operator << (std::ostream& os, const half& v) {
+  os << static_cast<float>(v);
+  return os;
+}
+#endif
+
+} // end namespace half_impl
+
+// import Eigen::half_impl::half into Eigen namespace
+// using half_impl::half;
+
+namespace internal {
+
+template<>
+struct random_default_impl<half, false, false>
+{
+  static inline half run(const half& x, const half& y)
+  {
+    return x + (y-x) * half(float(std::rand()) / float(RAND_MAX));
+  }
+  static inline half run()
+  {
+    return run(half(-1.f), half(1.f));
+  }
+};
+
+template<> struct is_arithmetic<half> { enum { value = true }; };
+
+} // end namespace internal
+
+template<> struct NumTraits<Eigen::half>
+    : GenericNumTraits<Eigen::half>
+{
+  enum {
+    IsSigned = true,
+    IsInteger = false,
+    IsComplex = false,
+    RequireInitialization = false
+  };
+
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half epsilon() {
+    return half_impl::raw_uint16_to_half(0x0800);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half dummy_precision() {
+    return half_impl::raw_uint16_to_half(0x211f); //  Eigen::half(1e-2f);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half highest() {
+    return half_impl::raw_uint16_to_half(0x7bff);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half lowest() {
+    return half_impl::raw_uint16_to_half(0xfbff);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half infinity() {
+    return half_impl::raw_uint16_to_half(0x7c00);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR static EIGEN_STRONG_INLINE Eigen::half quiet_NaN() {
+    return half_impl::raw_uint16_to_half(0x7e00);
+  }
+};
+
+} // end namespace Eigen
+
+#if defined(EIGEN_HAS_GPU_FP16) || defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  #pragma pop_macro("EIGEN_CONSTEXPR")
+#endif
+
+namespace Eigen {
+namespace numext {
+
+#if defined(EIGEN_GPU_COMPILE_PHASE)
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool(isnan)(const Eigen::half& h) {
+  return (half_impl::isnan)(h);
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool(isinf)(const Eigen::half& h) {
+  return (half_impl::isinf)(h);
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE bool(isfinite)(const Eigen::half& h) {
+  return (half_impl::isfinite)(h);
+}
+
+#endif
+
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC Eigen::half bit_cast<Eigen::half, uint16_t>(const uint16_t& src) {
+  return Eigen::half(Eigen::half_impl::raw_uint16_to_half(src));
+}
+
+template <>
+EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC uint16_t bit_cast<uint16_t, Eigen::half>(const Eigen::half& src) {
+  return Eigen::half_impl::raw_half_as_uint16(src);
+}
+
+}  // namespace numext
+}  // namespace Eigen
+
+// Add the missing shfl* intrinsics.
+// The __shfl* functions are only valid on HIP or _CUDA_ARCH_ >= 300.
+//   CUDA defines them for (__CUDA_ARCH__ >= 300 || !defined(__CUDA_ARCH__))
+//
+// HIP and CUDA prior to SDK 9.0 define
+//    __shfl, __shfl_up, __shfl_down, __shfl_xor for int and float
+// CUDA since 9.0 deprecates those and instead defines
+//    __shfl_sync, __shfl_up_sync, __shfl_down_sync, __shfl_xor_sync,
+//    with native support for __half and __nv_bfloat16
+//
+// Note that the following are __device__ - only functions.
+#if (defined(EIGEN_CUDACC) && (!defined(EIGEN_CUDA_ARCH) || EIGEN_CUDA_ARCH >= 300)) \
+    || defined(EIGEN_HIPCC)
+
+#if defined(EIGEN_HAS_CUDA_FP16) && EIGEN_CUDA_SDK_VER >= 90000
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_sync(unsigned mask, Eigen::half var, int srcLane, int width=warpSize) {
+  const __half h = var;
+  return static_cast<Eigen::half>(__shfl_sync(mask, h, srcLane, width));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_up_sync(unsigned mask, Eigen::half var, unsigned int delta, int width=warpSize) {
+  const __half h = var;
+  return static_cast<Eigen::half>(__shfl_up_sync(mask, h, delta, width));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_down_sync(unsigned mask, Eigen::half var, unsigned int delta, int width=warpSize) {
+  const __half h = var;
+  return static_cast<Eigen::half>(__shfl_down_sync(mask, h, delta, width));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor_sync(unsigned mask, Eigen::half var, int laneMask, int width=warpSize) {
+  const __half h = var;
+  return static_cast<Eigen::half>(__shfl_xor_sync(mask, h, laneMask, width));
+}
+
+#else // HIP or CUDA SDK < 9.0
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl(Eigen::half var, int srcLane, int width=warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl(ivar, srcLane, width)));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_up(Eigen::half var, unsigned int delta, int width=warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl_up(ivar, delta, width)));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_down(Eigen::half var, unsigned int delta, int width=warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl_down(ivar, delta, width)));
+}
+
+__device__ EIGEN_STRONG_INLINE Eigen::half __shfl_xor(Eigen::half var, int laneMask, int width=warpSize) {
+  const int ivar = static_cast<int>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(var));
+  return Eigen::numext::bit_cast<Eigen::half>(static_cast<Eigen::numext::uint16_t>(__shfl_xor(ivar, laneMask, width)));
+}
+
+#endif // HIP vs CUDA
+#endif // __shfl*
+
+// ldg() has an overload for __half_raw, but we also need one for Eigen::half.
+#if (defined(EIGEN_CUDACC) && (!defined(EIGEN_CUDA_ARCH) || EIGEN_CUDA_ARCH >= 350)) \
+    || defined(EIGEN_HIPCC)
+EIGEN_STRONG_INLINE __device__ Eigen::half __ldg(const Eigen::half* ptr) {
+  return Eigen::half_impl::raw_uint16_to_half(__ldg(reinterpret_cast<const Eigen::numext::uint16_t*>(ptr)));
+}
+#endif // __ldg
+
+#if EIGEN_HAS_STD_HASH
+namespace std {
+template <>
+struct hash<Eigen::half> {
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE std::size_t operator()(const Eigen::half& a) const {
+    return static_cast<std::size_t>(Eigen::numext::bit_cast<Eigen::numext::uint16_t>(a));
+  }
+};
+} // end namespace std
+#endif
+
+#endif // EIGEN_HALF_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
index 097373c..a5c3ada 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/Settings.h
@@ -21,7 +21,7 @@
   * it does not correspond to the number of iterations or the number of instructions
   */
 #ifndef EIGEN_UNROLLING_LIMIT
-#define EIGEN_UNROLLING_LIMIT 100
+#define EIGEN_UNROLLING_LIMIT 110
 #endif
 
 /** Defines the threshold between a "small" and a "large" matrix.
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h
new file mode 100644
index 0000000..fb8183b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/Default/TypeCasting.h
@@ -0,0 +1,120 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2016 Benoit Steiner <benoit.steiner.goog@gmail.com>
+// Copyright (C) 2019 Rasmus Munk Larsen <rmlarsen@google.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_GENERIC_TYPE_CASTING_H
+#define EIGEN_GENERIC_TYPE_CASTING_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<>
+struct scalar_cast_op<float, Eigen::half> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef Eigen::half result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator() (const float& a) const {
+    #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+      (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+      return __float2half(a);
+    #else
+      return Eigen::half(a);
+    #endif
+  }
+};
+
+template<>
+struct functor_traits<scalar_cast_op<float, Eigen::half> >
+{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
+
+
+template<>
+struct scalar_cast_op<int, Eigen::half> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef Eigen::half result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::half operator() (const int& a) const {
+    #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+      (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+      return __float2half(static_cast<float>(a));
+    #else
+      return Eigen::half(static_cast<float>(a));
+    #endif
+  }
+};
+
+template<>
+struct functor_traits<scalar_cast_op<int, Eigen::half> >
+{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
+
+
+template<>
+struct scalar_cast_op<Eigen::half, float> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef float result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator() (const Eigen::half& a) const {
+    #if (defined(EIGEN_HAS_CUDA_FP16) && defined(EIGEN_CUDA_ARCH) && EIGEN_CUDA_ARCH >= 300) || \
+      (defined(EIGEN_HAS_HIP_FP16) && defined(EIGEN_HIP_DEVICE_COMPILE))
+      return __half2float(a);
+    #else
+      return static_cast<float>(a);
+    #endif
+  }
+};
+
+template<>
+struct functor_traits<scalar_cast_op<Eigen::half, float> >
+{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
+
+
+template<>
+struct scalar_cast_op<float, Eigen::bfloat16> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef Eigen::bfloat16 result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::bfloat16 operator() (const float& a) const {
+    return Eigen::bfloat16(a);
+  }
+};
+
+template<>
+struct functor_traits<scalar_cast_op<float, Eigen::bfloat16> >
+{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
+
+
+template<>
+struct scalar_cast_op<int, Eigen::bfloat16> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef Eigen::bfloat16 result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Eigen::bfloat16 operator() (const int& a) const {
+    return Eigen::bfloat16(static_cast<float>(a));
+  }
+};
+
+template<>
+struct functor_traits<scalar_cast_op<int, Eigen::bfloat16> >
+{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
+
+
+template<>
+struct scalar_cast_op<Eigen::bfloat16, float> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cast_op)
+  typedef float result_type;
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator() (const Eigen::bfloat16& a) const {
+    return static_cast<float>(a);
+  }
+};
+
+template<>
+struct functor_traits<scalar_cast_op<Eigen::bfloat16, float> >
+{ enum { Cost = NumTraits<float>::AddCost, PacketAccess = false }; };
+
+
+}
+}
+
+#endif  // EIGEN_GENERIC_TYPE_CASTING_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
index 306a309..f40af7f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/Complex.h
@@ -15,9 +15,10 @@
 
 namespace internal {
 
-inline uint32x4_t p4ui_CONJ_XOR() {
+inline uint32x4_t p4ui_CONJ_XOR()
+{
 // See bug 1325, clang fails to call vld1q_u64.
-#if EIGEN_COMP_CLANG
+#if EIGEN_COMP_CLANG || EIGEN_COMP_CASTXML
   uint32x4_t ret = { 0x00000000, 0x80000000, 0x00000000, 0x80000000 };
   return ret;
 #else
@@ -26,61 +27,136 @@
 #endif
 }
 
-inline uint32x2_t p2ui_CONJ_XOR() {
+inline uint32x2_t p2ui_CONJ_XOR()
+{
   static const uint32_t conj_XOR_DATA[] = { 0x00000000, 0x80000000 };
   return vld1_u32( conj_XOR_DATA );
 }
 
 //---------- float ----------
+
+struct Packet1cf
+{
+  EIGEN_STRONG_INLINE Packet1cf() {}
+  EIGEN_STRONG_INLINE explicit Packet1cf(const Packet2f& a) : v(a) {}
+  Packet2f v;
+};
 struct Packet2cf
 {
   EIGEN_STRONG_INLINE Packet2cf() {}
   EIGEN_STRONG_INLINE explicit Packet2cf(const Packet4f& a) : v(a) {}
-  Packet4f  v;
+  Packet4f v;
 };
 
-template<> struct packet_traits<std::complex<float> >  : default_packet_traits
+template<> struct packet_traits<std::complex<float> > : default_packet_traits
 {
   typedef Packet2cf type;
-  typedef Packet2cf half;
-  enum {
+  typedef Packet1cf half;
+  enum
+  {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket = 0,
+    HasHalfPacket = 1,
 
-    HasAdd    = 1,
-    HasSub    = 1,
-    HasMul    = 1,
-    HasDiv    = 1,
-    HasNegate = 1,
-    HasAbs    = 0,
-    HasAbs2   = 0,
-    HasMin    = 0,
-    HasMax    = 0,
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasMul       = 1,
+    HasDiv       = 1,
+    HasNegate    = 1,
+    HasAbs       = 0,
+    HasAbs2      = 0,
+    HasMin       = 0,
+    HasMax       = 0,
     HasSetLinear = 0
   };
 };
 
-template<> struct unpacket_traits<Packet2cf> { typedef std::complex<float> type; enum {size=2, alignment=Aligned16}; typedef Packet2cf half; };
-
-template<> EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>&  from)
+template<> struct unpacket_traits<Packet1cf>
 {
-  float32x2_t r64;
-  r64 = vld1_f32((const float *)&from);
+  typedef std::complex<float> type;
+  typedef Packet1cf half;
+  typedef Packet2f as_real;
+  enum
+  {
+    size = 1,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet2cf>
+{
+  typedef std::complex<float> type;
+  typedef Packet1cf half;
+  typedef Packet4f as_real;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
 
+template<> EIGEN_STRONG_INLINE Packet1cf pcast<float,Packet1cf>(const float& a)
+{ return Packet1cf(vset_lane_f32(a, vdup_n_f32(0.f), 0)); }
+template<> EIGEN_STRONG_INLINE Packet2cf pcast<Packet2f,Packet2cf>(const Packet2f& a)
+{ return Packet2cf(vreinterpretq_f32_u64(vmovl_u32(vreinterpret_u32_f32(a)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf pset1<Packet1cf>(const std::complex<float>& from)
+{ return Packet1cf(vld1_f32(reinterpret_cast<const float*>(&from))); }
+template<> EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>& from)
+{
+  const float32x2_t r64 = vld1_f32(reinterpret_cast<const float*>(&from));
   return Packet2cf(vcombine_f32(r64, r64));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(padd<Packet4f>(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(psub<Packet4f>(a.v,b.v)); }
+template<> EIGEN_STRONG_INLINE Packet1cf padd<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{ return Packet1cf(padd<Packet2f>(a.v, b.v)); }
+template<> EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{ return Packet2cf(padd<Packet4f>(a.v, b.v)); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf psub<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{ return Packet1cf(psub<Packet2f>(a.v, b.v)); }
+template<> EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{ return Packet2cf(psub<Packet4f>(a.v, b.v)); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf pnegate(const Packet1cf& a) { return Packet1cf(pnegate<Packet2f>(a.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cf pnegate(const Packet2cf& a) { return Packet2cf(pnegate<Packet4f>(a.v)); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf pconj(const Packet1cf& a)
+{
+  const Packet2ui b = vreinterpret_u32_f32(a.v);
+  return Packet1cf(vreinterpret_f32_u32(veor_u32(b, p2ui_CONJ_XOR())));
+}
 template<> EIGEN_STRONG_INLINE Packet2cf pconj(const Packet2cf& a)
 {
-  Packet4ui b = vreinterpretq_u32_f32(a.v);
+  const Packet4ui b = vreinterpretq_u32_f32(a.v);
   return Packet2cf(vreinterpretq_f32_u32(veorq_u32(b, p4ui_CONJ_XOR())));
 }
 
+template<> EIGEN_STRONG_INLINE Packet1cf pmul<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{
+  Packet2f v1, v2;
+
+  // Get the real values of a | a1_re | a1_re |
+  v1 = vdup_lane_f32(a.v, 0);
+  // Get the imag values of a | a1_im | a1_im |
+  v2 = vdup_lane_f32(a.v, 1);
+  // Multiply the real a with b
+  v1 = vmul_f32(v1, b.v);
+  // Multiply the imag a with b
+  v2 = vmul_f32(v2, b.v);
+  // Conjugate v2
+  v2 = vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(v2), p2ui_CONJ_XOR()));
+  // Swap real/imag elements in v2.
+  v2 = vrev64_f32(v2);
+  // Add and return the result
+  return Packet1cf(vadd_f32(v1, v2));
+}
 template<> EIGEN_STRONG_INLINE Packet2cf pmul<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
 {
   Packet4f v1, v2;
@@ -93,7 +169,7 @@
   v1 = vmulq_f32(v1, b.v);
   // Multiply the imag a with b
   v2 = vmulq_f32(v2, b.v);
-  // Conjugate v2 
+  // Conjugate v2
   v2 = vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(v2), p4ui_CONJ_XOR()));
   // Swap real/imag elements in v2.
   v2 = vrev64q_f32(v2);
@@ -101,98 +177,144 @@
   return Packet2cf(vaddq_f32(v1, v2));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf pand   <Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+template<> EIGEN_STRONG_INLINE Packet1cf pcmp_eq(const Packet1cf& a, const Packet1cf& b)
 {
-  return Packet2cf(vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a.v),vreinterpretq_u32_f32(b.v))));
+  // Compare real and imaginary parts of a and b to get the mask vector:
+  // [re(a[0])==re(b[0]), im(a[0])==im(b[0])]
+  Packet2f eq = pcmp_eq<Packet2f>(a.v, b.v);
+  // Swap real/imag elements in the mask in to get:
+  // [im(a[0])==im(b[0]), re(a[0])==re(b[0])]
+  Packet2f eq_swapped = vrev64_f32(eq);
+  // Return re(a)==re(b) && im(a)==im(b) by computing bitwise AND of eq and eq_swapped
+  return Packet1cf(pand<Packet2f>(eq, eq_swapped));
 }
-template<> EIGEN_STRONG_INLINE Packet2cf por    <Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+template<> EIGEN_STRONG_INLINE Packet2cf pcmp_eq(const Packet2cf& a, const Packet2cf& b)
 {
-  return Packet2cf(vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a.v),vreinterpretq_u32_f32(b.v))));
+  // Compare real and imaginary parts of a and b to get the mask vector:
+  // [re(a[0])==re(b[0]), im(a[0])==im(b[0]), re(a[1])==re(b[1]), im(a[1])==im(b[1])]
+  Packet4f eq = pcmp_eq<Packet4f>(a.v, b.v);
+  // Swap real/imag elements in the mask in to get:
+  // [im(a[0])==im(b[0]), re(a[0])==re(b[0]), im(a[1])==im(b[1]), re(a[1])==re(b[1])]
+  Packet4f eq_swapped = vrev64q_f32(eq);
+  // Return re(a)==re(b) && im(a)==im(b) by computing bitwise AND of eq and eq_swapped
+  return Packet2cf(pand<Packet4f>(eq, eq_swapped));
 }
-template<> EIGEN_STRONG_INLINE Packet2cf pxor   <Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{
-  return Packet2cf(vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a.v),vreinterpretq_u32_f32(b.v))));
-}
+
+template<> EIGEN_STRONG_INLINE Packet1cf pand<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{ return Packet1cf(vreinterpret_f32_u32(vand_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
+template<> EIGEN_STRONG_INLINE Packet2cf pand<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{ return Packet2cf(vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf por<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{ return Packet1cf(vreinterpret_f32_u32(vorr_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
+template<> EIGEN_STRONG_INLINE Packet2cf por<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{ return Packet2cf(vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf pxor<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{ return Packet1cf(vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
+template<> EIGEN_STRONG_INLINE Packet2cf pxor<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{ return Packet2cf(vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf pandnot<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{ return Packet1cf(vreinterpret_f32_u32(vbic_u32(vreinterpret_u32_f32(a.v), vreinterpret_u32_f32(b.v)))); }
 template<> EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{ return Packet2cf(vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a.v), vreinterpretq_u32_f32(b.v)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf pload<Packet1cf>(const std::complex<float>* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return Packet1cf(pload<Packet2f>((const float*)from)); }
+template<> EIGEN_STRONG_INLINE Packet2cf pload<Packet2cf>(const std::complex<float>* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>(reinterpret_cast<const float*>(from))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf ploadu<Packet1cf>(const std::complex<float>* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cf(ploadu<Packet2f>((const float*)from)); }
+template<> EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>(reinterpret_cast<const float*>(from))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cf ploaddup<Packet1cf>(const std::complex<float>* from)
+{ return pset1<Packet1cf>(*from); }
+template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from)
+{ return pset1<Packet2cf>(*from); }
+
+template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *to, const Packet1cf& from)
+{ EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); }
+template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *to, const Packet2cf& from)
+{ EIGEN_DEBUG_ALIGNED_STORE pstore(reinterpret_cast<float*>(to), from.v); }
+
+template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *to, const Packet1cf& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); }
+template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *to, const Packet2cf& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<float*>(to), from.v); }
+
+template<> EIGEN_DEVICE_FUNC inline Packet1cf pgather<std::complex<float>, Packet1cf>(
+    const std::complex<float>* from, Index stride)
 {
-  return Packet2cf(vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a.v),vreinterpretq_u32_f32(b.v))));
+  const Packet2f tmp = vdup_n_f32(std::real(from[0*stride]));
+  return Packet1cf(vset_lane_f32(std::imag(from[0*stride]), tmp, 1));
 }
-
-template<> EIGEN_STRONG_INLINE Packet2cf pload<Packet2cf>(const std::complex<float>* from) { EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>((const float*)from)); }
-template<> EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from) { EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>((const float*)from)); }
-
-template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from) { return pset1<Packet2cf>(*from); }
-
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<float> >(std::complex<float> *   to, const Packet2cf& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((float*)to, from.v); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<float> >(std::complex<float> *   to, const Packet2cf& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((float*)to, from.v); }
-
-template<> EIGEN_DEVICE_FUNC inline Packet2cf pgather<std::complex<float>, Packet2cf>(const std::complex<float>* from, Index stride)
+template<> EIGEN_DEVICE_FUNC inline Packet2cf pgather<std::complex<float>, Packet2cf>(
+    const std::complex<float>* from, Index stride)
 {
-  Packet4f res = pset1<Packet4f>(0.f);
-  res = vsetq_lane_f32(std::real(from[0*stride]), res, 0);
+  Packet4f res = vdupq_n_f32(std::real(from[0*stride]));
   res = vsetq_lane_f32(std::imag(from[0*stride]), res, 1);
   res = vsetq_lane_f32(std::real(from[1*stride]), res, 2);
   res = vsetq_lane_f32(std::imag(from[1*stride]), res, 3);
   return Packet2cf(res);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf>(std::complex<float>* to, const Packet2cf& from, Index stride)
+template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet1cf>(
+    std::complex<float>* to, const Packet1cf& from, Index stride)
+{ to[stride*0] = std::complex<float>(vget_lane_f32(from.v, 0), vget_lane_f32(from.v, 1)); }
+template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<float>, Packet2cf>(
+    std::complex<float>* to, const Packet2cf& from, Index stride)
 {
   to[stride*0] = std::complex<float>(vgetq_lane_f32(from.v, 0), vgetq_lane_f32(from.v, 1));
   to[stride*1] = std::complex<float>(vgetq_lane_f32(from.v, 2), vgetq_lane_f32(from.v, 3));
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *   addr) { EIGEN_ARM_PREFETCH((const float *)addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<float> >(const std::complex<float> *addr)
+{ EIGEN_ARM_PREFETCH(reinterpret_cast<const float*>(addr)); }
 
-template<> EIGEN_STRONG_INLINE std::complex<float>  pfirst<Packet2cf>(const Packet2cf& a)
+template<> EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet1cf>(const Packet1cf& a)
 {
-  std::complex<float> EIGEN_ALIGN16 x[2];
-  vst1q_f32((float *)x, a.v);
+  EIGEN_ALIGN16 std::complex<float> x;
+  vst1_f32(reinterpret_cast<float*>(&x), a.v);
+  return x;
+}
+template<> EIGEN_STRONG_INLINE std::complex<float> pfirst<Packet2cf>(const Packet2cf& a)
+{
+  EIGEN_ALIGN16 std::complex<float> x[2];
+  vst1q_f32(reinterpret_cast<float*>(x), a.v);
   return x[0];
 }
 
+template<> EIGEN_STRONG_INLINE Packet1cf preverse(const Packet1cf& a) { return a; }
 template<> EIGEN_STRONG_INLINE Packet2cf preverse(const Packet2cf& a)
-{
-  float32x2_t a_lo, a_hi;
-  Packet4f a_r128;
+{ return Packet2cf(vcombine_f32(vget_high_f32(a.v), vget_low_f32(a.v))); }
 
-  a_lo = vget_low_f32(a.v);
-  a_hi = vget_high_f32(a.v);
-  a_r128 = vcombine_f32(a_hi, a_lo);
-
-  return Packet2cf(a_r128);
-}
-
+template<> EIGEN_STRONG_INLINE Packet1cf pcplxflip<Packet1cf>(const Packet1cf& a)
+{ return Packet1cf(vrev64_f32(a.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cf pcplxflip<Packet2cf>(const Packet2cf& a)
-{
-  return Packet2cf(vrev64q_f32(a.v));
-}
+{ return Packet2cf(vrev64q_f32(a.v)); }
 
+template<> EIGEN_STRONG_INLINE std::complex<float> predux<Packet1cf>(const Packet1cf& a)
+{
+  std::complex<float> s;
+  vst1_f32((float *)&s, a.v);
+  return s;
+}
 template<> EIGEN_STRONG_INLINE std::complex<float> predux<Packet2cf>(const Packet2cf& a)
 {
-  float32x2_t a1, a2;
   std::complex<float> s;
-
-  a1 = vget_low_f32(a.v);
-  a2 = vget_high_f32(a.v);
-  a2 = vadd_f32(a1, a2);
-  vst1_f32((float *)&s, a2);
-
+  vst1_f32(reinterpret_cast<float*>(&s), vadd_f32(vget_low_f32(a.v), vget_high_f32(a.v)));
   return s;
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf preduxp<Packet2cf>(const Packet2cf* vecs)
+template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet1cf>(const Packet1cf& a)
 {
-  Packet4f sum1, sum2, sum;
-
-  // Add the first two 64-bit float32x2_t of vecs[0]
-  sum1 = vcombine_f32(vget_low_f32(vecs[0].v), vget_low_f32(vecs[1].v));
-  sum2 = vcombine_f32(vget_high_f32(vecs[0].v), vget_high_f32(vecs[1].v));
-  sum = vaddq_f32(sum1, sum2);
-
-  return Packet2cf(sum);
+  std::complex<float> s;
+  vst1_f32((float *)&s, a.v);
+  return s;
 }
-
 template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet2cf>(const Packet2cf& a)
 {
   float32x2_t a1, a2, v1, v2, prod;
@@ -208,90 +330,67 @@
   v1 = vmul_f32(v1, a2);
   // Multiply the imag a with b
   v2 = vmul_f32(v2, a2);
-  // Conjugate v2 
+  // Conjugate v2
   v2 = vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(v2), p2ui_CONJ_XOR()));
   // Swap real/imag elements in v2.
   v2 = vrev64_f32(v2);
   // Add v1, v2
   prod = vadd_f32(v1, v2);
 
-  vst1_f32((float *)&s, prod);
+  vst1_f32(reinterpret_cast<float*>(&s), prod);
 
   return s;
 }
 
-template<int Offset>
-struct palign_impl<Offset,Packet2cf>
-{
-  EIGEN_STRONG_INLINE static void run(Packet2cf& first, const Packet2cf& second)
-  {
-    if (Offset==1)
-    {
-      first.v = vextq_f32(first.v, second.v, 2);
-    }
-  }
-};
-
-template<> struct conj_helper<Packet2cf, Packet2cf, false,true>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& a, const Packet2cf& b) const
-  {
-    return internal::pmul(a, pconj(b));
-  }
-};
-
-template<> struct conj_helper<Packet2cf, Packet2cf, true,false>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& a, const Packet2cf& b) const
-  {
-    return internal::pmul(pconj(a), b);
-  }
-};
-
-template<> struct conj_helper<Packet2cf, Packet2cf, true,true>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& a, const Packet2cf& b) const
-  {
-    return pconj(internal::pmul(a, b));
-  }
-};
-
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cf,Packet2f)
 EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
 
+template<> EIGEN_STRONG_INLINE Packet1cf pdiv<Packet1cf>(const Packet1cf& a, const Packet1cf& b)
+{
+  // TODO optimize it for NEON
+  Packet1cf res = pmul(a, pconj(b));
+  Packet2f s, rev_s;
+
+  // this computes the norm
+  s = vmul_f32(b.v, b.v);
+  rev_s = vrev64_f32(s);
+
+  return Packet1cf(pdiv<Packet2f>(res.v, vadd_f32(s, rev_s)));
+}
 template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
 {
   // TODO optimize it for NEON
-  Packet2cf res = conj_helper<Packet2cf,Packet2cf,false,true>().pmul(a,b);
+  Packet2cf res = pmul(a,pconj(b));
   Packet4f s, rev_s;
 
   // this computes the norm
   s = vmulq_f32(b.v, b.v);
   rev_s = vrev64q_f32(s);
 
-  return Packet2cf(pdiv<Packet4f>(res.v, vaddq_f32(s,rev_s)));
+  return Packet2cf(pdiv<Packet4f>(res.v, vaddq_f32(s, rev_s)));
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet2cf,2>& kernel) {
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet1cf, 1>& /*kernel*/) {}
+EIGEN_DEVICE_FUNC inline void ptranspose(PacketBlock<Packet2cf, 2>& kernel)
+{
   Packet4f tmp = vcombine_f32(vget_high_f32(kernel.packet[0].v), vget_high_f32(kernel.packet[1].v));
   kernel.packet[0].v = vcombine_f32(vget_low_f32(kernel.packet[0].v), vget_low_f32(kernel.packet[1].v));
   kernel.packet[1].v = tmp;
 }
 
+template<> EIGEN_STRONG_INLINE Packet1cf psqrt<Packet1cf>(const Packet1cf& a) {
+  return psqrt_complex<Packet1cf>(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2cf psqrt<Packet2cf>(const Packet2cf& a) {
+  return psqrt_complex<Packet2cf>(a);
+}
+
 //---------- double ----------
 #if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
 
 // See bug 1325, clang fails to call vld1q_u64.
-#if EIGEN_COMP_CLANG
+#if EIGEN_COMP_CLANG || EIGEN_COMP_CASTXML
   static uint64x2_t p2ul_CONJ_XOR = {0x0, 0x8000000000000000};
 #else
   const uint64_t  p2ul_conj_XOR_DATA[] = { 0x0, 0x8000000000000000 };
@@ -309,7 +408,8 @@
 {
   typedef Packet1cd type;
   typedef Packet1cd half;
-  enum {
+  enum
+  {
     Vectorizable = 1,
     AlignedOnScalar = 0,
     size = 1,
@@ -328,24 +428,50 @@
   };
 };
 
-template<> struct unpacket_traits<Packet1cd> { typedef std::complex<double> type; enum {size=1, alignment=Aligned16}; typedef Packet1cd half; };
+template<> struct unpacket_traits<Packet1cd>
+{
+  typedef std::complex<double> type;
+  typedef Packet1cd half;
+  typedef Packet2d as_real;
+  enum
+  {
+    size=1,
+    alignment=Aligned16,
+    vectorizable=true,
+    masked_load_available=false,
+    masked_store_available=false
+  };
+};
 
-template<> EIGEN_STRONG_INLINE Packet1cd pload<Packet1cd>(const std::complex<double>* from) { EIGEN_DEBUG_ALIGNED_LOAD return Packet1cd(pload<Packet2d>((const double*)from)); }
-template<> EIGEN_STRONG_INLINE Packet1cd ploadu<Packet1cd>(const std::complex<double>* from) { EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cd(ploadu<Packet2d>((const double*)from)); }
+template<> EIGEN_STRONG_INLINE Packet1cd pload<Packet1cd>(const std::complex<double>* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return Packet1cd(pload<Packet2d>(reinterpret_cast<const double*>(from))); }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pset1<Packet1cd>(const std::complex<double>&  from)
-{ /* here we really have to use unaligned loads :( */ return ploadu<Packet1cd>(&from); }
+template<> EIGEN_STRONG_INLINE Packet1cd ploadu<Packet1cd>(const std::complex<double>* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return Packet1cd(ploadu<Packet2d>(reinterpret_cast<const double*>(from))); }
 
-template<> EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(padd<Packet2d>(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(psub<Packet2d>(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pnegate(const Packet1cd& a) { return Packet1cd(pnegate<Packet2d>(a.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pconj(const Packet1cd& a) { return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v), p2ul_CONJ_XOR))); }
+template<> EIGEN_STRONG_INLINE Packet1cd pset1<Packet1cd>(const std::complex<double>& from)
+{
+  /* here we really have to use unaligned loads :( */
+  return ploadu<Packet1cd>(&from);
+}
+
+template<> EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
+{ return Packet1cd(padd<Packet2d>(a.v, b.v)); }
+
+template<> EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
+{ return Packet1cd(psub<Packet2d>(a.v, b.v)); }
+
+template<> EIGEN_STRONG_INLINE Packet1cd pnegate(const Packet1cd& a)
+{ return Packet1cd(pnegate<Packet2d>(a.v)); }
+
+template<> EIGEN_STRONG_INLINE Packet1cd pconj(const Packet1cd& a)
+{ return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v), p2ul_CONJ_XOR))); }
 
 template<> EIGEN_STRONG_INLINE Packet1cd pmul<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
   Packet2d v1, v2;
 
-  // Get the real values of a 
+  // Get the real values of a
   v1 = vdupq_lane_f64(vget_low_f64(a.v), 0);
   // Get the imag values of a
   v2 = vdupq_lane_f64(vget_high_f64(a.v), 0);
@@ -353,7 +479,7 @@
   v1 = vmulq_f64(v1, b.v);
   // Multiply the imag a with b
   v2 = vmulq_f64(v2, b.v);
-  // Conjugate v2 
+  // Conjugate v2
   v2 = vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(v2), p2ul_CONJ_XOR));
   // Swap real/imag elements in v2.
   v2 = preverse<Packet2d>(v2);
@@ -361,31 +487,44 @@
   return Packet1cd(vaddq_f64(v1, v2));
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pand   <Packet1cd>(const Packet1cd& a, const Packet1cd& b)
+template<> EIGEN_STRONG_INLINE Packet1cd pcmp_eq(const Packet1cd& a, const Packet1cd& b)
 {
-  return Packet1cd(vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v))));
+  // Compare real and imaginary parts of a and b to get the mask vector:
+  // [re(a)==re(b), im(a)==im(b)]
+  Packet2d eq = pcmp_eq<Packet2d>(a.v, b.v);
+  // Swap real/imag elements in the mask in to get:
+  // [im(a)==im(b), re(a)==re(b)]
+  Packet2d eq_swapped = vreinterpretq_f64_u32(vrev64q_u32(vreinterpretq_u32_f64(eq)));
+  // Return re(a)==re(b) & im(a)==im(b) by computing bitwise AND of eq and eq_swapped
+  return Packet1cd(pand<Packet2d>(eq, eq_swapped));
 }
-template<> EIGEN_STRONG_INLINE Packet1cd por    <Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
-  return Packet1cd(vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v))));
-}
-template<> EIGEN_STRONG_INLINE Packet1cd pxor   <Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
-  return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v))));
-}
+
+template<> EIGEN_STRONG_INLINE Packet1cd pand<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
+{ return Packet1cd(vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cd por<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
+{ return Packet1cd(vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+
+template<> EIGEN_STRONG_INLINE Packet1cd pxor<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
+{ return Packet1cd(vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
+
 template<> EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
-{
-  return Packet1cd(vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v))));
-}
+{ return Packet1cd(vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a.v),vreinterpretq_u64_f64(b.v)))); }
 
-template<> EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<double>* from) { return pset1<Packet1cd>(*from); }
+template<> EIGEN_STRONG_INLINE Packet1cd ploaddup<Packet1cd>(const std::complex<double>* from)
+{ return pset1<Packet1cd>(*from); }
 
-template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_ALIGNED_STORE pstore((double*)to, from.v); }
-template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *   to, const Packet1cd& from) { EIGEN_DEBUG_UNALIGNED_STORE pstoreu((double*)to, from.v); }
+template<> EIGEN_STRONG_INLINE void pstore <std::complex<double> >(std::complex<double> *to, const Packet1cd& from)
+{ EIGEN_DEBUG_ALIGNED_STORE pstore(reinterpret_cast<double*>(to), from.v); }
 
-template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *   addr) { EIGEN_ARM_PREFETCH((const double *)addr); }
+template<> EIGEN_STRONG_INLINE void pstoreu<std::complex<double> >(std::complex<double> *to, const Packet1cd& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE pstoreu(reinterpret_cast<double*>(to), from.v); }
 
-template<> EIGEN_DEVICE_FUNC inline Packet1cd pgather<std::complex<double>, Packet1cd>(const std::complex<double>* from, Index stride)
+template<> EIGEN_STRONG_INLINE void prefetch<std::complex<double> >(const std::complex<double> *addr)
+{ EIGEN_ARM_PREFETCH(reinterpret_cast<const double*>(addr)); }
+
+template<> EIGEN_DEVICE_FUNC inline Packet1cd pgather<std::complex<double>, Packet1cd>(
+    const std::complex<double>* from, Index stride)
 {
   Packet2d res = pset1<Packet2d>(0.0);
   res = vsetq_lane_f64(std::real(from[0*stride]), res, 0);
@@ -393,17 +532,14 @@
   return Packet1cd(res);
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<double>, Packet1cd>(std::complex<double>* to, const Packet1cd& from, Index stride)
-{
-  to[stride*0] = std::complex<double>(vgetq_lane_f64(from.v, 0), vgetq_lane_f64(from.v, 1));
-}
+template<> EIGEN_DEVICE_FUNC inline void pscatter<std::complex<double>, Packet1cd>(
+    std::complex<double>* to, const Packet1cd& from, Index stride)
+{ to[stride*0] = std::complex<double>(vgetq_lane_f64(from.v, 0), vgetq_lane_f64(from.v, 1)); }
 
-
-template<> EIGEN_STRONG_INLINE std::complex<double>  pfirst<Packet1cd>(const Packet1cd& a)
+template<> EIGEN_STRONG_INLINE std::complex<double> pfirst<Packet1cd>(const Packet1cd& a)
 {
-  std::complex<double> EIGEN_ALIGN16 res;
+  EIGEN_ALIGN16 std::complex<double> res;
   pstore<std::complex<double> >(&res, a);
-
   return res;
 }
 
@@ -411,59 +547,14 @@
 
 template<> EIGEN_STRONG_INLINE std::complex<double> predux<Packet1cd>(const Packet1cd& a) { return pfirst(a); }
 
-template<> EIGEN_STRONG_INLINE Packet1cd preduxp<Packet1cd>(const Packet1cd* vecs) { return vecs[0]; }
-
 template<> EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet1cd>(const Packet1cd& a) { return pfirst(a); }
 
-template<int Offset>
-struct palign_impl<Offset,Packet1cd>
-{
-  static EIGEN_STRONG_INLINE void run(Packet1cd& /*first*/, const Packet1cd& /*second*/)
-  {
-    // FIXME is it sure we never have to align a Packet1cd?
-    // Even though a std::complex<double> has 16 bytes, it is not necessarily aligned on a 16 bytes boundary...
-  }
-};
-
-template<> struct conj_helper<Packet1cd, Packet1cd, false,true>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& a, const Packet1cd& b) const
-  {
-    return internal::pmul(a, pconj(b));
-  }
-};
-
-template<> struct conj_helper<Packet1cd, Packet1cd, true,false>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& a, const Packet1cd& b) const
-  {
-    return internal::pmul(pconj(a), b);
-  }
-};
-
-template<> struct conj_helper<Packet1cd, Packet1cd, true,true>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& a, const Packet1cd& b) const
-  {
-    return pconj(internal::pmul(a, b));
-  }
-};
-
 EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
 
 template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
   // TODO optimize it for NEON
-  Packet1cd res = conj_helper<Packet1cd,Packet1cd,false,true>().pmul(a,b);
+  Packet1cd res = pmul(a,pconj(b));
   Packet2d s = pmul<Packet2d>(b.v, b.v);
   Packet2d rev_s = preverse<Packet2d>(s);
 
@@ -471,9 +562,7 @@
 }
 
 EIGEN_STRONG_INLINE Packet1cd pcplxflip/*<Packet1cd>*/(const Packet1cd& x)
-{
-  return Packet1cd(preverse(Packet2d(x.v)));
-}
+{ return Packet1cd(preverse(Packet2d(x.v))); }
 
 EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet1cd,2>& kernel)
 {
@@ -481,6 +570,11 @@
   kernel.packet[0].v = vcombine_f64(vget_low_f64(kernel.packet[0].v), vget_low_f64(kernel.packet[1].v));
   kernel.packet[1].v = tmp;
 }
+
+template<> EIGEN_STRONG_INLINE Packet1cd psqrt<Packet1cd>(const Packet1cd& a) {
+  return psqrt_complex<Packet1cd>(a);
+}
+
 #endif // EIGEN_ARCH_ARM64
 
 } // end namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
new file mode 100644
index 0000000..3481f33
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/GeneralBlockPanelKernel.h
@@ -0,0 +1,183 @@
+namespace Eigen {
+namespace internal {
+  
+#if EIGEN_ARCH_ARM && EIGEN_COMP_CLANG
+
+// Clang seems to excessively spill registers in the GEBP kernel on 32-bit arm.
+// Here we specialize gebp_traits to eliminate these register spills.
+// See #2138.
+template<>
+struct gebp_traits <float,float,false,false,Architecture::NEON,GEBPPacketFull>
+ : gebp_traits<float,float,false,false,Architecture::Generic,GEBPPacketFull>
+{
+  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const
+  { 
+    // This volatile inline ASM both acts as a barrier to prevent reordering,
+    // as well as enforces strict register use.
+    asm volatile(
+      "vmla.f32 %q[r], %q[c], %q[alpha]"
+      : [r] "+w" (r)
+      : [c] "w" (c),
+        [alpha] "w" (alpha)
+      : );
+  }
+
+  template <typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const Packet4f& a, const Packet4f& b,
+                                Packet4f& c, Packet4f& tmp,
+                                const LaneIdType&) const {
+    acc(a, b, c);
+  }
+  
+  template <typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const Packet4f& a, const QuadPacket<Packet4f>& b,
+                                Packet4f& c, Packet4f& tmp,
+                                const LaneIdType& lane) const {
+    madd(a, b.get(lane), c, tmp, lane);
+  }
+};
+
+#endif // EIGEN_ARCH_ARM && EIGEN_COMP_CLANG
+
+#if EIGEN_ARCH_ARM64
+
+template<>
+struct gebp_traits <float,float,false,false,Architecture::NEON,GEBPPacketFull>
+ : gebp_traits<float,float,false,false,Architecture::Generic,GEBPPacketFull>
+{
+  typedef float RhsPacket;
+  typedef float32x4_t RhsPacketx4;
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const
+  {
+    dest = *b;
+  }
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
+  {
+    dest = vld1q_f32(b);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const
+  {
+    dest = *b;
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
+  {}
+
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
+  {
+    loadRhs(b,dest);
+  }
+
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
+  {
+    c = vfmaq_n_f32(c, a, b);
+  }
+
+  // NOTE: Template parameter inference failed when compiled with Android NDK:
+  // "candidate template ignored: could not match 'FixedInt<N>' against 'Eigen::internal::FixedInt<0>".
+
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
+  { madd_helper<0>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<1>&) const
+  { madd_helper<1>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<2>&) const
+  { madd_helper<2>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<3>&) const
+  { madd_helper<3>(a, b, c); }
+
+ private:
+  template<int LaneID>
+  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const
+  {
+    #if EIGEN_COMP_GNUC_STRICT && !(EIGEN_GNUC_AT_LEAST(9,0))
+    // workaround gcc issue https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89101
+    // vfmaq_laneq_f32 is implemented through a costly dup
+         if(LaneID==0)  asm("fmla %0.4s, %1.4s, %2.s[0]\n" : "+w" (c) : "w" (a), "w" (b) :  );
+    else if(LaneID==1)  asm("fmla %0.4s, %1.4s, %2.s[1]\n" : "+w" (c) : "w" (a), "w" (b) :  );
+    else if(LaneID==2)  asm("fmla %0.4s, %1.4s, %2.s[2]\n" : "+w" (c) : "w" (a), "w" (b) :  );
+    else if(LaneID==3)  asm("fmla %0.4s, %1.4s, %2.s[3]\n" : "+w" (c) : "w" (a), "w" (b) :  );
+    #else
+    c = vfmaq_laneq_f32(c, a, b, LaneID);
+    #endif
+  }
+};
+
+
+template<>
+struct gebp_traits <double,double,false,false,Architecture::NEON>
+ : gebp_traits<double,double,false,false,Architecture::Generic>
+{
+  typedef double RhsPacket;
+
+  struct RhsPacketx4 {
+    float64x2_t B_0, B_1;
+  };
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const
+  {
+    dest = *b;
+  }
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
+  {
+    dest.B_0 = vld1q_f64(b);
+    dest.B_1 = vld1q_f64(b+2);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacket& dest) const
+  {
+    loadRhs(b,dest);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
+  {}
+
+  EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
+  {
+    loadRhs(b,dest);
+  }
+
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
+  {
+    c = vfmaq_n_f64(c, a, b);
+  }
+
+  // NOTE: Template parameter inference failed when compiled with Android NDK:
+  // "candidate template ignored: could not match 'FixedInt<N>' against 'Eigen::internal::FixedInt<0>".
+
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<0>&) const
+  { madd_helper<0>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<1>&) const
+  { madd_helper<1>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<2>&) const
+  { madd_helper<2>(a, b, c); }
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c, RhsPacket& /*tmp*/, const FixedInt<3>&) const
+  { madd_helper<3>(a, b, c); }
+
+ private:
+  template <int LaneID>
+  EIGEN_STRONG_INLINE void madd_helper(const LhsPacket& a, const RhsPacketx4& b, AccPacket& c) const
+  {
+    #if EIGEN_COMP_GNUC_STRICT && !(EIGEN_GNUC_AT_LEAST(9,0))
+    // workaround gcc issue https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89101
+    // vfmaq_laneq_f64 is implemented through a costly dup
+         if(LaneID==0)  asm("fmla %0.2d, %1.2d, %2.d[0]\n" : "+w" (c) : "w" (a), "w" (b.B_0) :  );
+    else if(LaneID==1)  asm("fmla %0.2d, %1.2d, %2.d[1]\n" : "+w" (c) : "w" (a), "w" (b.B_0) :  );
+    else if(LaneID==2)  asm("fmla %0.2d, %1.2d, %2.d[0]\n" : "+w" (c) : "w" (a), "w" (b.B_1) :  );
+    else if(LaneID==3)  asm("fmla %0.2d, %1.2d, %2.d[1]\n" : "+w" (c) : "w" (a), "w" (b.B_1) :  );
+    #else
+         if(LaneID==0) c = vfmaq_laneq_f64(c, a, b.B_0, 0);
+    else if(LaneID==1) c = vfmaq_laneq_f64(c, a, b.B_0, 1);
+    else if(LaneID==2) c = vfmaq_laneq_f64(c, a, b.B_1, 0);
+    else if(LaneID==3) c = vfmaq_laneq_f64(c, a, b.B_1, 1);
+    #endif
+  }
+};
+
+#endif // EIGEN_ARCH_ARM64
+
+}  // namespace internal
+}  // namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
index 6bb05bb..fa6615a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/MathFunctions.h
@@ -5,10 +5,6 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* The sin, cos, exp, and log functions of this file come from
- * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
- */
-
 #ifndef EIGEN_MATH_FUNCTIONS_NEON_H
 #define EIGEN_MATH_FUNCTIONS_NEON_H
 
@@ -16,74 +12,62 @@
 
 namespace internal {
 
-template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f pexp<Packet4f>(const Packet4f& _x)
-{
-  Packet4f x = _x;
-  Packet4f tmp, fx;
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f pexp<Packet2f>(const Packet2f& x)
+{ return pexp_float(x); }
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f pexp<Packet4f>(const Packet4f& x)
+{ return pexp_float(x); }
 
-  _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
-  _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
-  _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f);
-  _EIGEN_DECLARE_CONST_Packet4f(exp_hi,  88.3762626647950f);
-  _EIGEN_DECLARE_CONST_Packet4f(exp_lo, -88.3762626647949f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_LOG2EF, 1.44269504088896341f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C1, 0.693359375f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C2, -2.12194440e-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p0, 1.9875691500E-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p1, 1.3981999507E-3f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p2, 8.3334519073E-3f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p3, 4.1665795894E-2f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f plog<Packet2f>(const Packet2f& x)
+{ return plog_float(x); }
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f plog<Packet4f>(const Packet4f& x)
+{ return plog_float(x); }
 
-  x = vminq_f32(x, p4f_exp_hi);
-  x = vmaxq_f32(x, p4f_exp_lo);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f psin<Packet2f>(const Packet2f& x)
+{ return psin_float(x); }
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f psin<Packet4f>(const Packet4f& x)
+{ return psin_float(x); }
 
-  /* express exp(x) as exp(g + n*log(2)) */
-  fx = vmlaq_f32(p4f_half, x, p4f_cephes_LOG2EF);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f pcos<Packet2f>(const Packet2f& x)
+{ return pcos_float(x); }
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f pcos<Packet4f>(const Packet4f& x)
+{ return pcos_float(x); }
 
-  /* perform a floorf */
-  tmp = vcvtq_f32_s32(vcvtq_s32_f32(fx));
+// Hyperbolic Tangent function.
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2f ptanh<Packet2f>(const Packet2f& x)
+{ return internal::generic_fast_tanh_float(x); }
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet4f ptanh<Packet4f>(const Packet4f& x)
+{ return internal::generic_fast_tanh_float(x); }
 
-  /* if greater, substract 1 */
-  Packet4ui mask = vcgtq_f32(tmp, fx);
-  mask = vandq_u32(mask, vreinterpretq_u32_f32(p4f_1));
+BF16_PACKET_FUNCTION(Packet4f, Packet4bf, psin)
+BF16_PACKET_FUNCTION(Packet4f, Packet4bf, pcos)
+BF16_PACKET_FUNCTION(Packet4f, Packet4bf, plog)
+BF16_PACKET_FUNCTION(Packet4f, Packet4bf, pexp)
+BF16_PACKET_FUNCTION(Packet4f, Packet4bf, ptanh)
 
-  fx = vsubq_f32(tmp, vreinterpretq_f32_u32(mask));
-
-  tmp = vmulq_f32(fx, p4f_cephes_exp_C1);
-  Packet4f z = vmulq_f32(fx, p4f_cephes_exp_C2);
-  x = vsubq_f32(x, tmp);
-  x = vsubq_f32(x, z);
-
-  Packet4f y = vmulq_f32(p4f_cephes_exp_p0, x);
-  z = vmulq_f32(x, x);
-  y = vaddq_f32(y, p4f_cephes_exp_p1);
-  y = vmulq_f32(y, x);
-  y = vaddq_f32(y, p4f_cephes_exp_p2);
-  y = vmulq_f32(y, x);
-  y = vaddq_f32(y, p4f_cephes_exp_p3);
-  y = vmulq_f32(y, x);
-  y = vaddq_f32(y, p4f_cephes_exp_p4);
-  y = vmulq_f32(y, x);
-  y = vaddq_f32(y, p4f_cephes_exp_p5);
-
-  y = vmulq_f32(y, z);
-  y = vaddq_f32(y, x);
-  y = vaddq_f32(y, p4f_1);
-
-  /* build 2^n */
-  int32x4_t mm;
-  mm = vcvtq_s32_f32(fx);
-  mm = vaddq_s32(mm, p4i_0x7f);
-  mm = vshlq_n_s32(mm, 23);
-  Packet4f pow2n = vreinterpretq_f32_s32(mm);
-
-  y = vmulq_f32(y, pow2n);
-  return y;
+template <>
+EIGEN_STRONG_INLINE Packet4bf pfrexp(const Packet4bf& a, Packet4bf& exponent) {
+  Packet4f fexponent;
+  const Packet4bf out = F32ToBf16(pfrexp<Packet4f>(Bf16ToF32(a), fexponent));
+  exponent = F32ToBf16(fexponent);
+  return out;
 }
 
+template <>
+EIGEN_STRONG_INLINE Packet4bf pldexp(const Packet4bf& a, const Packet4bf& exponent) {
+  return F32ToBf16(pldexp<Packet4f>(Bf16ToF32(a), Bf16ToF32(exponent)));
+}
+
+//---------- double ----------
+
+#if EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d pexp<Packet2d>(const Packet2d& x)
+{ return pexp_double(x); }
+
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED Packet2d plog<Packet2d>(const Packet2d& x)
+{ return plog_double(x); }
+
+#endif
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
index 3d5ed0d..d2aeef4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/PacketMath.h
@@ -24,54 +24,118 @@
 #define EIGEN_HAS_SINGLE_INSTRUCTION_MADD
 #endif
 
-#ifndef EIGEN_HAS_SINGLE_INSTRUCTION_CJMADD
-#define EIGEN_HAS_SINGLE_INSTRUCTION_CJMADD
-#endif
-
 #ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS
 #if EIGEN_ARCH_ARM64
 #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 32
 #else
-#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 16 
+#define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS 16
 #endif
 #endif
 
-#if EIGEN_COMP_MSVC
+#if EIGEN_COMP_MSVC_STRICT
 
 // In MSVC's arm_neon.h header file, all NEON vector types
 // are aliases to the same underlying type __n128.
 // We thus have to wrap them to make them different C++ types.
 // (See also bug 1428)
-
-template<typename T,int unique_id>
-struct eigen_packet_wrapper
-{
-  operator T&() { return m_val; }
-  operator const T&() const { return m_val; }
-  eigen_packet_wrapper() {}
-  eigen_packet_wrapper(const T &v) : m_val(v) {}
-  eigen_packet_wrapper& operator=(const T &v) {
-    m_val = v;
-    return *this;
-  }
-
-  T m_val;
-};
-typedef eigen_packet_wrapper<float32x2_t,0> Packet2f;
-typedef eigen_packet_wrapper<float32x4_t,1> Packet4f;
-typedef eigen_packet_wrapper<int32x4_t  ,2> Packet4i;
-typedef eigen_packet_wrapper<int32x2_t  ,3> Packet2i;
-typedef eigen_packet_wrapper<uint32x4_t ,4> Packet4ui;
+typedef eigen_packet_wrapper<float32x2_t,0>  Packet2f;
+typedef eigen_packet_wrapper<float32x4_t,1>  Packet4f;
+typedef eigen_packet_wrapper<int32_t    ,2>  Packet4c;
+typedef eigen_packet_wrapper<int8x8_t   ,3>  Packet8c;
+typedef eigen_packet_wrapper<int8x16_t  ,4>  Packet16c;
+typedef eigen_packet_wrapper<uint32_t   ,5>  Packet4uc;
+typedef eigen_packet_wrapper<uint8x8_t  ,6>  Packet8uc;
+typedef eigen_packet_wrapper<uint8x16_t ,7>  Packet16uc;
+typedef eigen_packet_wrapper<int16x4_t  ,8>  Packet4s;
+typedef eigen_packet_wrapper<int16x8_t  ,9>  Packet8s;
+typedef eigen_packet_wrapper<uint16x4_t ,10> Packet4us;
+typedef eigen_packet_wrapper<uint16x8_t ,11> Packet8us;
+typedef eigen_packet_wrapper<int32x2_t  ,12> Packet2i;
+typedef eigen_packet_wrapper<int32x4_t  ,13> Packet4i;
+typedef eigen_packet_wrapper<uint32x2_t ,14> Packet2ui;
+typedef eigen_packet_wrapper<uint32x4_t ,15> Packet4ui;
+typedef eigen_packet_wrapper<int64x2_t  ,16> Packet2l;
+typedef eigen_packet_wrapper<uint64x2_t ,17> Packet2ul;
 
 #else
 
-typedef float32x2_t Packet2f;
-typedef float32x4_t Packet4f;
-typedef int32x4_t   Packet4i;
-typedef int32x2_t   Packet2i;
-typedef uint32x4_t  Packet4ui;
+typedef float32x2_t                          Packet2f;
+typedef float32x4_t                          Packet4f;
+typedef eigen_packet_wrapper<int32_t    ,2>  Packet4c;
+typedef int8x8_t                             Packet8c;
+typedef int8x16_t                            Packet16c;
+typedef eigen_packet_wrapper<uint32_t   ,5>  Packet4uc;
+typedef uint8x8_t                            Packet8uc;
+typedef uint8x16_t                           Packet16uc;
+typedef int16x4_t                            Packet4s;
+typedef int16x8_t                            Packet8s;
+typedef uint16x4_t                           Packet4us;
+typedef uint16x8_t                           Packet8us;
+typedef int32x2_t                            Packet2i;
+typedef int32x4_t                            Packet4i;
+typedef uint32x2_t                           Packet2ui;
+typedef uint32x4_t                           Packet4ui;
+typedef int64x2_t                            Packet2l;
+typedef uint64x2_t                           Packet2ul;
 
-#endif // EIGEN_COMP_MSVC
+#endif // EIGEN_COMP_MSVC_STRICT
+
+EIGEN_STRONG_INLINE Packet4f shuffle1(const Packet4f& m, int mask){
+  const float* a = reinterpret_cast<const float*>(&m);
+  Packet4f res = {*(a + (mask & 3)), *(a + ((mask >> 2) & 3)), *(a + ((mask >> 4) & 3 )), *(a + ((mask >> 6) & 3))};
+  return res;
+}
+
+// fuctionally equivalent to _mm_shuffle_ps in SSE when interleave
+// == false (i.e. shuffle<false>(m, n, mask) equals _mm_shuffle_ps(m, n, mask)),
+// interleave m and n when interleave == true. Currently used in LU/arch/InverseSize4.h
+// to enable a shared implementation for fast inversion of matrices of size 4. 
+template<bool interleave> 
+EIGEN_STRONG_INLINE Packet4f shuffle2(const Packet4f &m, const Packet4f &n, int mask)
+{
+  const float* a = reinterpret_cast<const float*>(&m);
+  const float* b = reinterpret_cast<const float*>(&n);
+  Packet4f res = {*(a + (mask & 3)), *(a + ((mask >> 2) & 3)), *(b + ((mask >> 4) & 3)), *(b + ((mask >> 6) & 3))};
+  return res;
+}
+
+template<> 
+EIGEN_STRONG_INLINE Packet4f shuffle2<true>(const Packet4f &m, const Packet4f &n, int mask) 
+{
+  const float* a = reinterpret_cast<const float*>(&m);
+  const float* b = reinterpret_cast<const float*>(&n);
+  Packet4f res = {*(a + (mask & 3)), *(b + ((mask >> 2) & 3)), *(a + ((mask >> 4) & 3)), *(b + ((mask >> 6) & 3))};
+  return res;
+}
+
+EIGEN_STRONG_INLINE static int eigen_neon_shuffle_mask(int p, int q, int r, int s) {return ((s)<<6|(r)<<4|(q)<<2|(p));}
+
+EIGEN_STRONG_INLINE Packet4f vec4f_swizzle1(const Packet4f& a, int p, int q, int r, int s)
+{ 
+  return shuffle1(a, eigen_neon_shuffle_mask(p, q, r, s));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_swizzle2(const Packet4f& a, const Packet4f& b, int p, int q, int r, int s)
+{ 
+  return shuffle2<false>(a,b,eigen_neon_shuffle_mask(p, q, r, s));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_movelh(const Packet4f& a, const Packet4f& b)
+{
+  return shuffle2<false>(a,b,eigen_neon_shuffle_mask(0, 1, 0, 1));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_movehl(const Packet4f& a, const Packet4f& b)
+{
+  return shuffle2<false>(b,a,eigen_neon_shuffle_mask(2, 3, 2, 3));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_unpacklo(const Packet4f& a, const Packet4f& b)
+{
+  return shuffle2<true>(a,b,eigen_neon_shuffle_mask(0, 0, 1, 1));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_unpackhi(const Packet4f& a, const Packet4f& b)
+{
+  return shuffle2<true>(a,b,eigen_neon_shuffle_mask(2, 2, 3, 3));
+}
+#define vec4f_duplane(a, p) \
+  vdupq_lane_f32(vget_low_f32(a), p)
 
 #define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
   const Packet4f p4f_##NAME = pset1<Packet4f>(X)
@@ -98,81 +162,816 @@
   #define EIGEN_ARM_PREFETCH(ADDR)
 #endif
 
-template<> struct packet_traits<float>  : default_packet_traits
+template <>
+struct packet_traits<float> : default_packet_traits
 {
   typedef Packet4f type;
-  typedef Packet4f half; // Packet2f intrinsics not implemented yet
-  enum {
+  typedef Packet2f half;
+  enum
+  {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 4,
-    HasHalfPacket=0, // Packet2f intrinsics not implemented yet
-   
-    HasDiv  = 1,
-    // FIXME check the Has*
-    HasSin  = 0,
-    HasCos  = 0,
-    HasLog  = 0,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0,
+
+    HasDiv   = 1,
+    HasFloor = 1,
+    HasCeil = 1,
+    HasRint = 1,
+
+    HasSin  = EIGEN_FAST_MATH,
+    HasCos  = EIGEN_FAST_MATH,
+    HasLog  = 1,
     HasExp  = 1,
-    HasSqrt = 0
+    HasSqrt = 1,
+    HasRsqrt = 1,
+    HasTanh = EIGEN_FAST_MATH,
+    HasErf  = EIGEN_FAST_MATH,
+    HasBessel = 0,  // Issues with accuracy.
+    HasNdtri = 0
   };
 };
-template<> struct packet_traits<int32_t>    : default_packet_traits
+
+template <>
+struct packet_traits<int8_t> : default_packet_traits
 {
-  typedef Packet4i type;
-  typedef Packet4i half; // Packet2i intrinsics not implemented yet
-  enum {
+  typedef Packet16c type;
+  typedef Packet8c half;
+  enum
+  {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    size=4,
-    HasHalfPacket=0 // Packet2i intrinsics not implemented yet
-    // FIXME check the Has*
+    size = 16,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasAbsDiff   = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0
   };
 };
 
-#if EIGEN_GNUC_AT_MOST(4,4) && !EIGEN_COMP_LLVM
-// workaround gcc 4.2, 4.3 and 4.4 compilatin issue
+template <>
+struct packet_traits<uint8_t> : default_packet_traits
+{
+  typedef Packet16uc type;
+  typedef Packet8uc half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 16,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 0,
+    HasAbs       = 1,
+    HasAbsDiff   = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0,
+
+    HasSqrt = 1
+  };
+};
+
+template <>
+struct packet_traits<int16_t> : default_packet_traits
+{
+  typedef Packet8s type;
+  typedef Packet4s half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 8,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasAbsDiff   = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0
+  };
+};
+
+template <>
+struct packet_traits<uint16_t> : default_packet_traits
+{
+  typedef Packet8us type;
+  typedef Packet4us half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 8,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 0,
+    HasAbs       = 0,
+    HasAbsDiff   = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0,
+    HasSqrt = 1
+  };
+};
+
+template <>
+struct packet_traits<int32_t> : default_packet_traits
+{
+  typedef Packet4i type;
+  typedef Packet2i half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 4,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0
+  };
+};
+
+template <>
+struct packet_traits<uint32_t> : default_packet_traits
+{
+  typedef Packet4ui type;
+  typedef Packet2ui half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 4,
+    HasHalfPacket = 1,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 0,
+    HasAbs       = 0,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0,
+
+    HasSqrt = 1
+  };
+};
+
+template <>
+struct packet_traits<int64_t> : default_packet_traits
+{
+  typedef Packet2l type;
+  typedef Packet2l half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 2,
+    HasHalfPacket = 0,
+
+    HasCmp       = 1,
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0
+  };
+};
+
+template <>
+struct packet_traits<uint64_t> : default_packet_traits
+{
+  typedef Packet2ul type;
+  typedef Packet2ul half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 2,
+    HasHalfPacket = 0,
+
+    HasCmp       = 1,
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 0,
+    HasAbs       = 0,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0
+  };
+};
+
+#if EIGEN_GNUC_AT_MOST(4, 4) && !EIGEN_COMP_LLVM
+// workaround gcc 4.2, 4.3 and 4.4 compilation issue
 EIGEN_STRONG_INLINE float32x4_t vld1q_f32(const float* x) { return ::vld1q_f32((const float32_t*)x); }
-EIGEN_STRONG_INLINE float32x2_t vld1_f32 (const float* x) { return ::vld1_f32 ((const float32_t*)x); }
-EIGEN_STRONG_INLINE float32x2_t vld1_dup_f32 (const float* x) { return ::vld1_dup_f32 ((const float32_t*)x); }
-EIGEN_STRONG_INLINE void        vst1q_f32(float* to, float32x4_t from) { ::vst1q_f32((float32_t*)to,from); }
-EIGEN_STRONG_INLINE void        vst1_f32 (float* to, float32x2_t from) { ::vst1_f32 ((float32_t*)to,from); }
+EIGEN_STRONG_INLINE float32x2_t vld1_f32(const float* x) { return ::vld1_f32 ((const float32_t*)x); }
+EIGEN_STRONG_INLINE float32x2_t vld1_dup_f32(const float* x) { return ::vld1_dup_f32 ((const float32_t*)x); }
+EIGEN_STRONG_INLINE void vst1q_f32(float* to, float32x4_t from) { ::vst1q_f32((float32_t*)to,from); }
+EIGEN_STRONG_INLINE void vst1_f32 (float* to, float32x2_t from) { ::vst1_f32 ((float32_t*)to,from); }
 #endif
 
-template<> struct unpacket_traits<Packet4f> { typedef float   type; enum {size=4, alignment=Aligned16}; typedef Packet4f half; };
-template<> struct unpacket_traits<Packet4i> { typedef int32_t type; enum {size=4, alignment=Aligned16}; typedef Packet4i half; };
+template<> struct unpacket_traits<Packet2f>
+{
+  typedef float type;
+  typedef Packet2f half;
+  typedef Packet2i integer_packet;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet4f>
+{
+  typedef float type;
+  typedef Packet2f half;
+  typedef Packet4i integer_packet;
+  enum
+  {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet4c>
+{
+  typedef int8_t type;
+  typedef Packet4c half;
+  enum
+  {
+    size = 4,
+    alignment = Unaligned,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet8c>
+{
+  typedef int8_t type;
+  typedef Packet4c half;
+  enum
+  {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet16c>
+{
+  typedef int8_t type;
+  typedef Packet8c half;
+  enum
+  {
+    size = 16,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet4uc>
+{
+  typedef uint8_t type;
+  typedef Packet4uc half;
+  enum
+  {
+    size = 4,
+    alignment = Unaligned,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet8uc>
+{
+  typedef uint8_t type;
+  typedef Packet4uc half;
+  enum
+  {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet16uc>
+{
+  typedef uint8_t type;
+  typedef Packet8uc half;
+  enum
+  {
+    size = 16,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false};
+};
+template<> struct unpacket_traits<Packet4s>
+{
+  typedef int16_t type;
+  typedef Packet4s half;
+  enum
+  {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet8s>
+{
+  typedef int16_t type;
+  typedef Packet4s half;
+  enum
+  {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet4us>
+{
+  typedef uint16_t type;
+  typedef Packet4us half;
+  enum
+  {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet8us>
+{
+  typedef uint16_t type;
+  typedef Packet4us half;
+  enum
+  {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet2i>
+{
+  typedef int32_t type;
+  typedef Packet2i half;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet4i>
+{
+  typedef int32_t type;
+  typedef Packet2i half;
+  enum
+  {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet2ui>
+{
+  typedef uint32_t type;
+  typedef Packet2ui half;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet4ui>
+{
+  typedef uint32_t type;
+  typedef Packet2ui half;
+  enum
+  {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet2l>
+{
+  typedef int64_t type;
+  typedef Packet2l half;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+template<> struct unpacket_traits<Packet2ul>
+{
+  typedef uint64_t type;
+  typedef Packet2ul half;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
 
-template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float&  from) { return vdupq_n_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int32_t&    from)   { return vdupq_n_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet2f pset1<Packet2f>(const float& from) { return vdup_n_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4f pset1<Packet4f>(const float& from) { return vdupq_n_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4c pset1<Packet4c>(const int8_t& from)
+{ return vget_lane_s32(vreinterpret_s32_s8(vdup_n_s8(from)), 0); }
+template<> EIGEN_STRONG_INLINE Packet8c pset1<Packet8c>(const int8_t& from) { return vdup_n_s8(from); }
+template<> EIGEN_STRONG_INLINE Packet16c pset1<Packet16c>(const int8_t& from) { return vdupq_n_s8(from); }
+template<> EIGEN_STRONG_INLINE Packet4uc pset1<Packet4uc>(const uint8_t& from)
+{ return vget_lane_u32(vreinterpret_u32_u8(vdup_n_u8(from)), 0); }
+template<> EIGEN_STRONG_INLINE Packet8uc pset1<Packet8uc>(const uint8_t& from) { return vdup_n_u8(from); }
+template<> EIGEN_STRONG_INLINE Packet16uc pset1<Packet16uc>(const uint8_t& from) { return vdupq_n_u8(from); }
+template<> EIGEN_STRONG_INLINE Packet4s pset1<Packet4s>(const int16_t& from) { return vdup_n_s16(from); }
+template<> EIGEN_STRONG_INLINE Packet8s pset1<Packet8s>(const int16_t& from) { return vdupq_n_s16(from); }
+template<> EIGEN_STRONG_INLINE Packet4us pset1<Packet4us>(const uint16_t& from) { return vdup_n_u16(from); }
+template<> EIGEN_STRONG_INLINE Packet8us pset1<Packet8us>(const uint16_t& from) { return vdupq_n_u16(from); }
+template<> EIGEN_STRONG_INLINE Packet2i pset1<Packet2i>(const int32_t& from) { return vdup_n_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int32_t& from) { return vdupq_n_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet2ui pset1<Packet2ui>(const uint32_t& from) { return vdup_n_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet4ui pset1<Packet4ui>(const uint32_t& from) { return vdupq_n_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet2l pset1<Packet2l>(const int64_t& from) { return vdupq_n_s64(from); }
+template<> EIGEN_STRONG_INLINE Packet2ul pset1<Packet2ul>(const uint64_t& from) { return vdupq_n_u64(from); }
 
+template<> EIGEN_STRONG_INLINE Packet2f pset1frombits<Packet2f>(unsigned int from)
+{ return vreinterpret_f32_u32(vdup_n_u32(from)); }
+template<> EIGEN_STRONG_INLINE Packet4f pset1frombits<Packet4f>(unsigned int from)
+{ return vreinterpretq_f32_u32(vdupq_n_u32(from)); }
+
+template<> EIGEN_STRONG_INLINE Packet2f plset<Packet2f>(const float& a)
+{
+  const float c[] = {0.0f,1.0f};
+  return vadd_f32(pset1<Packet2f>(a), vld1_f32(c));
+}
 template<> EIGEN_STRONG_INLINE Packet4f plset<Packet4f>(const float& a)
 {
-  const float f[] = {0, 1, 2, 3};
-  Packet4f countdown = vld1q_f32(f);
-  return vaddq_f32(pset1<Packet4f>(a), countdown);
+  const float c[] = {0.0f,1.0f,2.0f,3.0f};
+  return vaddq_f32(pset1<Packet4f>(a), vld1q_f32(c));
+}
+template<> EIGEN_STRONG_INLINE Packet4c plset<Packet4c>(const int8_t& a)
+{ return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(vreinterpret_s8_u32(vdup_n_u32(0x03020100)), vdup_n_s8(a))), 0); }
+template<> EIGEN_STRONG_INLINE Packet8c plset<Packet8c>(const int8_t& a)
+{
+  const int8_t c[] = {0,1,2,3,4,5,6,7};
+  return vadd_s8(pset1<Packet8c>(a), vld1_s8(c));
+}
+template<> EIGEN_STRONG_INLINE Packet16c plset<Packet16c>(const int8_t& a)
+{
+  const int8_t c[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+  return vaddq_s8(pset1<Packet16c>(a), vld1q_s8(c));
+}
+template<> EIGEN_STRONG_INLINE Packet4uc plset<Packet4uc>(const uint8_t& a)
+{ return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(vreinterpret_u8_u32(vdup_n_u32(0x03020100)), vdup_n_u8(a))), 0); }
+template<> EIGEN_STRONG_INLINE Packet8uc plset<Packet8uc>(const uint8_t& a)
+{
+  const uint8_t c[] = {0,1,2,3,4,5,6,7};
+  return vadd_u8(pset1<Packet8uc>(a), vld1_u8(c));
+}
+template<> EIGEN_STRONG_INLINE Packet16uc plset<Packet16uc>(const uint8_t& a)
+{
+  const uint8_t c[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15};
+  return vaddq_u8(pset1<Packet16uc>(a), vld1q_u8(c));
+}
+template<> EIGEN_STRONG_INLINE Packet4s plset<Packet4s>(const int16_t& a)
+{
+  const int16_t c[] = {0,1,2,3};
+  return vadd_s16(pset1<Packet4s>(a), vld1_s16(c));
+}
+template<> EIGEN_STRONG_INLINE Packet4us plset<Packet4us>(const uint16_t& a)
+{
+  const uint16_t c[] = {0,1,2,3};
+  return vadd_u16(pset1<Packet4us>(a), vld1_u16(c));
+}
+template<> EIGEN_STRONG_INLINE Packet8s plset<Packet8s>(const int16_t& a)
+{
+  const int16_t c[] = {0,1,2,3,4,5,6,7};
+  return vaddq_s16(pset1<Packet8s>(a), vld1q_s16(c));
+}
+template<> EIGEN_STRONG_INLINE Packet8us plset<Packet8us>(const uint16_t& a)
+{
+  const uint16_t c[] = {0,1,2,3,4,5,6,7};
+  return vaddq_u16(pset1<Packet8us>(a), vld1q_u16(c));
+}
+template<> EIGEN_STRONG_INLINE Packet2i plset<Packet2i>(const int32_t& a)
+{
+  const int32_t c[] = {0,1};
+  return vadd_s32(pset1<Packet2i>(a), vld1_s32(c));
 }
 template<> EIGEN_STRONG_INLINE Packet4i plset<Packet4i>(const int32_t& a)
 {
-  const int32_t i[] = {0, 1, 2, 3};
-  Packet4i countdown = vld1q_s32(i);
-  return vaddq_s32(pset1<Packet4i>(a), countdown);
+  const int32_t c[] = {0,1,2,3};
+  return vaddq_s32(pset1<Packet4i>(a), vld1q_s32(c));
+}
+template<> EIGEN_STRONG_INLINE Packet2ui plset<Packet2ui>(const uint32_t& a)
+{
+  const uint32_t c[] = {0,1};
+  return vadd_u32(pset1<Packet2ui>(a), vld1_u32(c));
+}
+template<> EIGEN_STRONG_INLINE Packet4ui plset<Packet4ui>(const uint32_t& a)
+{
+  const uint32_t c[] = {0,1,2,3};
+  return vaddq_u32(pset1<Packet4ui>(a), vld1q_u32(c));
+}
+template<> EIGEN_STRONG_INLINE Packet2l plset<Packet2l>(const int64_t& a)
+{
+  const int64_t c[] = {0,1};
+  return vaddq_s64(pset1<Packet2l>(a), vld1q_s64(c));
+}
+template<> EIGEN_STRONG_INLINE Packet2ul plset<Packet2ul>(const uint64_t& a)
+{
+  const uint64_t c[] = {0,1};
+  return vaddq_u64(pset1<Packet2ul>(a), vld1q_u64(c));
 }
 
+template<> EIGEN_STRONG_INLINE Packet2f padd<Packet2f>(const Packet2f& a, const Packet2f& b) { return vadd_f32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4f padd<Packet4f>(const Packet4f& a, const Packet4f& b) { return vaddq_f32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4c padd<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c padd<Packet8c>(const Packet8c& a, const Packet8c& b) { return vadd_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c padd<Packet16c>(const Packet16c& a, const Packet16c& b) { return vaddq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc padd<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc padd<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vadd_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc padd<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vaddq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s padd<Packet4s>(const Packet4s& a, const Packet4s& b) { return vadd_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s padd<Packet8s>(const Packet8s& a, const Packet8s& b) { return vaddq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us padd<Packet4us>(const Packet4us& a, const Packet4us& b) { return vadd_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us padd<Packet8us>(const Packet8us& a, const Packet8us& b) { return vaddq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i padd<Packet2i>(const Packet2i& a, const Packet2i& b) { return vadd_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) { return vaddq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui padd<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vadd_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui padd<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vaddq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l padd<Packet2l>(const Packet2l& a, const Packet2l& b) { return vaddq_s64(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ul padd<Packet2ul>(const Packet2ul& a, const Packet2ul& b) { return vaddq_u64(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2f psub<Packet2f>(const Packet2f& a, const Packet2f& b) { return vsub_f32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) { return vsubq_f32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4c psub<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vsub_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c psub<Packet8c>(const Packet8c& a, const Packet8c& b) { return vsub_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c psub<Packet16c>(const Packet16c& a, const Packet16c& b) { return vsubq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc psub<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vsub_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc psub<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vsub_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc psub<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vsubq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s psub<Packet4s>(const Packet4s& a, const Packet4s& b) { return vsub_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s psub<Packet8s>(const Packet8s& a, const Packet8s& b) { return vsubq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us psub<Packet4us>(const Packet4us& a, const Packet4us& b) { return vsub_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us psub<Packet8us>(const Packet8us& a, const Packet8us& b) { return vsubq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i psub<Packet2i>(const Packet2i& a, const Packet2i& b) { return vsub_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) { return vsubq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui psub<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vsub_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui psub<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vsubq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l psub<Packet2l>(const Packet2l& a, const Packet2l& b) { return vsubq_s64(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ul psub<Packet2ul>(const Packet2ul& a, const Packet2ul& b) { return vsubq_u64(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2f pxor<Packet2f>(const Packet2f& a, const Packet2f& b);
+template<> EIGEN_STRONG_INLINE Packet2f paddsub<Packet2f>(const Packet2f& a, const Packet2f & b) {
+  Packet2f mask = {numext::bit_cast<float>(0x80000000u), 0.0f};
+  return padd(a, pxor(mask, b));
+}
+template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b);
+template<> EIGEN_STRONG_INLINE Packet4f paddsub<Packet4f>(const Packet4f& a, const Packet4f& b) {
+  Packet4f mask = {numext::bit_cast<float>(0x80000000u), 0.0f, numext::bit_cast<float>(0x80000000u), 0.0f};
+  return padd(a, pxor(mask, b));
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pnegate(const Packet2f& a) { return vneg_f32(a); }
 template<> EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a) { return vnegq_f32(a); }
+template<> EIGEN_STRONG_INLINE Packet4c pnegate(const Packet4c& a)
+{ return vget_lane_s32(vreinterpret_s32_s8(vneg_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0); }
+template<> EIGEN_STRONG_INLINE Packet8c pnegate(const Packet8c& a) { return vneg_s8(a); }
+template<> EIGEN_STRONG_INLINE Packet16c pnegate(const Packet16c& a) { return vnegq_s8(a); }
+template<> EIGEN_STRONG_INLINE Packet4s pnegate(const Packet4s& a) { return vneg_s16(a); }
+template<> EIGEN_STRONG_INLINE Packet8s pnegate(const Packet8s& a) { return vnegq_s16(a); }
+template<> EIGEN_STRONG_INLINE Packet2i pnegate(const Packet2i& a) { return vneg_s32(a); }
 template<> EIGEN_STRONG_INLINE Packet4i pnegate(const Packet4i& a) { return vnegq_s32(a); }
+template<> EIGEN_STRONG_INLINE Packet2l pnegate(const Packet2l& a) {
+#if EIGEN_ARCH_ARM64
+  return vnegq_s64(a);
+#else
+  return vcombine_s64(
+      vdup_n_s64(-vgetq_lane_s64(a, 0)),
+      vdup_n_s64(-vgetq_lane_s64(a, 1)));
+#endif
+}
 
+template<> EIGEN_STRONG_INLINE Packet2f pconj(const Packet2f& a) { return a; }
 template<> EIGEN_STRONG_INLINE Packet4f pconj(const Packet4f& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4c pconj(const Packet4c& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet8c pconj(const Packet8c& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet16c pconj(const Packet16c& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4uc pconj(const Packet4uc& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet8uc pconj(const Packet8uc& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet16uc pconj(const Packet16uc& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4s pconj(const Packet4s& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet8s pconj(const Packet8s& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4us pconj(const Packet4us& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet8us pconj(const Packet8us& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet2i pconj(const Packet2i& a) { return a; }
 template<> EIGEN_STRONG_INLINE Packet4i pconj(const Packet4i& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet2ui pconj(const Packet2ui& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4ui pconj(const Packet4ui& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet2l pconj(const Packet2l& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet2ul pconj(const Packet2ul& a) { return a; }
 
+template<> EIGEN_STRONG_INLINE Packet2f pmul<Packet2f>(const Packet2f& a, const Packet2f& b) { return vmul_f32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4f pmul<Packet4f>(const Packet4f& a, const Packet4f& b) { return vmulq_f32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4c pmul<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vmul_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pmul<Packet8c>(const Packet8c& a, const Packet8c& b) { return vmul_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pmul<Packet16c>(const Packet16c& a, const Packet16c& b) { return vmulq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pmul<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vmul_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pmul<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vmul_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pmul<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vmulq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pmul<Packet4s>(const Packet4s& a, const Packet4s& b) { return vmul_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pmul<Packet8s>(const Packet8s& a, const Packet8s& b) { return vmulq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pmul<Packet4us>(const Packet4us& a, const Packet4us& b) { return vmul_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pmul<Packet8us>(const Packet8us& a, const Packet8us& b) { return vmulq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pmul<Packet2i>(const Packet2i& a, const Packet2i& b) { return vmul_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i pmul<Packet4i>(const Packet4i& a, const Packet4i& b) { return vmulq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pmul<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vmul_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pmul<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vmulq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pmul<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s64(
+    vdup_n_s64(vgetq_lane_s64(a, 0)*vgetq_lane_s64(b, 0)),
+    vdup_n_s64(vgetq_lane_s64(a, 1)*vgetq_lane_s64(b, 1)));
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pmul<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u64(
+    vdup_n_u64(vgetq_lane_u64(a, 0)*vgetq_lane_u64(b, 0)),
+    vdup_n_u64(vgetq_lane_u64(a, 1)*vgetq_lane_u64(b, 1)));
+}
 
+template<> EIGEN_STRONG_INLINE Packet2f pdiv<Packet2f>(const Packet2f& a, const Packet2f& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vdiv_f32(a,b);
+#else
+  Packet2f inv, restep, div;
+
+  // NEON does not offer a divide instruction, we have to do a reciprocal approximation
+  // However NEON in contrast to other SIMD engines (AltiVec/SSE), offers
+  // a reciprocal estimate AND a reciprocal step -which saves a few instructions
+  // vrecpeq_f32() returns an estimate to 1/b, which we will finetune with
+  // Newton-Raphson and vrecpsq_f32()
+  inv = vrecpe_f32(b);
+
+  // This returns a differential, by which we will have to multiply inv to get a better
+  // approximation of 1/b.
+  restep = vrecps_f32(b, inv);
+  inv = vmul_f32(restep, inv);
+
+  // Finally, multiply a by 1/b and get the wanted result of the division.
+  div = vmul_f32(a, inv);
+
+  return div;
+#endif
+}
 template<> EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b)
 {
 #if EIGEN_ARCH_ARM64
@@ -199,357 +998,2629 @@
 #endif
 }
 
+template<> EIGEN_STRONG_INLINE Packet4c pdiv<Packet4c>(const Packet4c& /*a*/, const Packet4c& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet4c>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pdiv<Packet8c>(const Packet8c& /*a*/, const Packet8c& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet8c>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet16c pdiv<Packet16c>(const Packet16c& /*a*/, const Packet16c& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet16c>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet4uc pdiv<Packet4uc>(const Packet4uc& /*a*/, const Packet4uc& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet4uc>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pdiv<Packet8uc>(const Packet8uc& /*a*/, const Packet8uc& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet8uc>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet16uc pdiv<Packet16uc>(const Packet16uc& /*a*/, const Packet16uc& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet16uc>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet4s pdiv<Packet4s>(const Packet4s& /*a*/, const Packet4s& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet4s>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet8s pdiv<Packet8s>(const Packet8s& /*a*/, const Packet8s& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet8s>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet4us pdiv<Packet4us>(const Packet4us& /*a*/, const Packet4us& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet4us>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet8us pdiv<Packet8us>(const Packet8us& /*a*/, const Packet8us& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet8us>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet2i pdiv<Packet2i>(const Packet2i& /*a*/, const Packet2i& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet2i>(0);
+}
 template<> EIGEN_STRONG_INLINE Packet4i pdiv<Packet4i>(const Packet4i& /*a*/, const Packet4i& /*b*/)
-{ eigen_assert(false && "packet integer division are not supported by NEON");
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
   return pset1<Packet4i>(0);
 }
+template<> EIGEN_STRONG_INLINE Packet2ui pdiv<Packet2ui>(const Packet2ui& /*a*/, const Packet2ui& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet2ui>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet4ui pdiv<Packet4ui>(const Packet4ui& /*a*/, const Packet4ui& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet4ui>(0);
+}
+template<> EIGEN_STRONG_INLINE Packet2l pdiv<Packet2l>(const Packet2l& /*a*/, const Packet2l& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet2l>(0LL);
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pdiv<Packet2ul>(const Packet2ul& /*a*/, const Packet2ul& /*b*/)
+{
+  eigen_assert(false && "packet integer division are not supported by NEON");
+  return pset1<Packet2ul>(0ULL);
+}
 
-// Clang/ARM wrongly advertises __ARM_FEATURE_FMA even when it's not available,
-// then implements a slow software scalar fallback calling fmaf()!
-// Filed LLVM bug:
-//     https://llvm.org/bugs/show_bug.cgi?id=27216
-#if (defined __ARM_FEATURE_FMA) && !(EIGEN_COMP_CLANG && EIGEN_ARCH_ARM)
-// See bug 936.
-// FMA is available on VFPv4 i.e. when compiling with -mfpu=neon-vfpv4.
-// FMA is a true fused multiply-add i.e. only 1 rounding at the end, no intermediate rounding.
-// MLA is not fused i.e. does 2 roundings.
-// In addition to giving better accuracy, FMA also gives better performance here on a Krait (Nexus 4):
-// MLA: 10 GFlop/s ; FMA: 12 GFlops/s.
-template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) { return vfmaq_f32(c,a,b); }
+
+#ifdef __ARM_FEATURE_FMA
+template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c)
+{ return vfmaq_f32(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet2f pmadd(const Packet2f& a, const Packet2f& b, const Packet2f& c)
+{ return vfma_f32(c,a,b); }
 #else
-template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) {
-#if EIGEN_COMP_CLANG && EIGEN_ARCH_ARM
-  // Clang/ARM will replace VMLA by VMUL+VADD at least for some values of -mcpu,
-  // at least -mcpu=cortex-a8 and -mcpu=cortex-a7. Since the former is the default on
-  // -march=armv7-a, that is a very common case.
-  // See e.g. this thread:
-  //     http://lists.llvm.org/pipermail/llvm-dev/2013-December/068806.html
-  // Filed LLVM bug:
-  //     https://llvm.org/bugs/show_bug.cgi?id=27219
-  Packet4f r = c;
-  asm volatile(
-    "vmla.f32 %q[r], %q[a], %q[b]"
-    : [r] "+w" (r)
-    : [a] "w" (a),
-      [b] "w" (b)
-    : );
-  return r;
-#else
+template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c)
+{
   return vmlaq_f32(c,a,b);
-#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2f pmadd(const Packet2f& a, const Packet2f& b, const Packet2f& c)
+{
+  return vmla_f32(c,a,b);
 }
 #endif
 
 // No FMA instruction for int, so use MLA unconditionally.
-template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) { return vmlaq_s32(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet4c pmadd(const Packet4c& a, const Packet4c& b, const Packet4c& c)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vmla_s8(
+      vreinterpret_s8_s32(vdup_n_s32(c)),
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pmadd(const Packet8c& a, const Packet8c& b, const Packet8c& c)
+{ return vmla_s8(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pmadd(const Packet16c& a, const Packet16c& b, const Packet16c& c)
+{ return vmlaq_s8(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pmadd(const Packet4uc& a, const Packet4uc& b, const Packet4uc& c)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vmla_u8(
+      vreinterpret_u8_u32(vdup_n_u32(c)),
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pmadd(const Packet8uc& a, const Packet8uc& b, const Packet8uc& c)
+{ return vmla_u8(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pmadd(const Packet16uc& a, const Packet16uc& b, const Packet16uc& c)
+{ return vmlaq_u8(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pmadd(const Packet4s& a, const Packet4s& b, const Packet4s& c)
+{ return vmla_s16(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pmadd(const Packet8s& a, const Packet8s& b, const Packet8s& c)
+{ return vmlaq_s16(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pmadd(const Packet4us& a, const Packet4us& b, const Packet4us& c)
+{ return vmla_u16(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pmadd(const Packet8us& a, const Packet8us& b, const Packet8us& c)
+{ return vmlaq_u16(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pmadd(const Packet2i& a, const Packet2i& b, const Packet2i& c)
+{ return vmla_s32(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c)
+{ return vmlaq_s32(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pmadd(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c)
+{ return vmla_u32(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pmadd(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c)
+{ return vmlaq_u32(c,a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2f pabsdiff<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vabd_f32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4f pabsdiff<Packet4f>(const Packet4f& a, const Packet4f& b)
+{ return vabdq_f32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4c pabsdiff<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vabd_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pabsdiff<Packet8c>(const Packet8c& a, const Packet8c& b)
+{ return vabd_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pabsdiff<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return vabdq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pabsdiff<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vabd_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pabsdiff<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vabd_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pabsdiff<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vabdq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pabsdiff<Packet4s>(const Packet4s& a, const Packet4s& b)
+{ return vabd_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pabsdiff<Packet8s>(const Packet8s& a, const Packet8s& b)
+{ return vabdq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pabsdiff<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vabd_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pabsdiff<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vabdq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pabsdiff<Packet2i>(const Packet2i& a, const Packet2i& b)
+{ return vabd_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pabsdiff<Packet4i>(const Packet4i& a, const Packet4i& b)
+{ return vabdq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pabsdiff<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vabd_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pabsdiff<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vabdq_u32(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pmin<Packet2f>(const Packet2f& a, const Packet2f& b) { return vmin_f32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) { return vminq_f32(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) { return vminq_s32(a,b); }
 
+#ifdef __ARM_FEATURE_NUMERIC_MAXMIN
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
+template<> EIGEN_STRONG_INLINE Packet4f pmin<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) { return vminnmq_f32(a, b); }
+template<> EIGEN_STRONG_INLINE Packet2f pmin<PropagateNumbers, Packet2f>(const Packet2f& a, const Packet2f& b) { return vminnm_f32(a, b); }
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4f pmin<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) { return pmin<Packet4f>(a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pmin<PropagateNaN, Packet2f>(const Packet2f& a, const Packet2f& b) { return pmin<Packet2f>(a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet4c pmin<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vmin_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pmin<Packet8c>(const Packet8c& a, const Packet8c& b) { return vmin_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pmin<Packet16c>(const Packet16c& a, const Packet16c& b) { return vminq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pmin<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vmin_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pmin<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vmin_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pmin<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vminq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pmin<Packet4s>(const Packet4s& a, const Packet4s& b) { return vmin_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pmin<Packet8s>(const Packet8s& a, const Packet8s& b) { return vminq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pmin<Packet4us>(const Packet4us& a, const Packet4us& b) { return vmin_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pmin<Packet8us>(const Packet8us& a, const Packet8us& b) { return vminq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pmin<Packet2i>(const Packet2i& a, const Packet2i& b) { return vmin_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b) { return vminq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pmin<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vmin_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pmin<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vminq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pmin<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s64(
+      vdup_n_s64((std::min)(vgetq_lane_s64(a, 0), vgetq_lane_s64(b, 0))),
+      vdup_n_s64((std::min)(vgetq_lane_s64(a, 1), vgetq_lane_s64(b, 1))));
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pmin<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u64(
+      vdup_n_u64((std::min)(vgetq_lane_u64(a, 0), vgetq_lane_u64(b, 0))),
+      vdup_n_u64((std::min)(vgetq_lane_u64(a, 1), vgetq_lane_u64(b, 1))));
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pmax<Packet2f>(const Packet2f& a, const Packet2f& b) { return vmax_f32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) { return vmaxq_f32(a,b); }
+
+#ifdef __ARM_FEATURE_NUMERIC_MAXMIN
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
+template<> EIGEN_STRONG_INLINE Packet4f pmax<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) { return vmaxnmq_f32(a, b); }
+template<> EIGEN_STRONG_INLINE Packet2f pmax<PropagateNumbers, Packet2f>(const Packet2f& a, const Packet2f& b) { return vmaxnm_f32(a, b); }
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4f pmax<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) { return pmax<Packet4f>(a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pmax<PropagateNaN, Packet2f>(const Packet2f& a, const Packet2f& b) { return pmax<Packet2f>(a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet4c pmax<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vmax_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pmax<Packet8c>(const Packet8c& a, const Packet8c& b) { return vmax_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pmax<Packet16c>(const Packet16c& a, const Packet16c& b) { return vmaxq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pmax<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vmax_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pmax<Packet8uc>(const Packet8uc& a, const Packet8uc& b) { return vmax_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pmax<Packet16uc>(const Packet16uc& a, const Packet16uc& b) { return vmaxq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pmax<Packet4s>(const Packet4s& a, const Packet4s& b) { return vmax_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pmax<Packet8s>(const Packet8s& a, const Packet8s& b) { return vmaxq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pmax<Packet4us>(const Packet4us& a, const Packet4us& b) { return vmax_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pmax<Packet8us>(const Packet8us& a, const Packet8us& b) { return vmaxq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pmax<Packet2i>(const Packet2i& a, const Packet2i& b) { return vmax_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b) { return vmaxq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pmax<Packet2ui>(const Packet2ui& a, const Packet2ui& b) { return vmax_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pmax<Packet4ui>(const Packet4ui& a, const Packet4ui& b) { return vmaxq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pmax<Packet2l>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s64(
+      vdup_n_s64((std::max)(vgetq_lane_s64(a, 0), vgetq_lane_s64(b, 0))),
+      vdup_n_s64((std::max)(vgetq_lane_s64(a, 1), vgetq_lane_s64(b, 1))));
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pmax<Packet2ul>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u64(
+      vdup_n_u64((std::max)(vgetq_lane_u64(a, 0), vgetq_lane_u64(b, 0))),
+      vdup_n_u64((std::max)(vgetq_lane_u64(a, 1), vgetq_lane_u64(b, 1))));
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pcmp_le<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vcle_f32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_le<Packet4f>(const Packet4f& a, const Packet4f& b)
+{ return vreinterpretq_f32_u32(vcleq_f32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4c pcmp_le<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_u8(vcle_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pcmp_le<Packet8c>(const Packet8c& a, const Packet8c& b)
+{ return vreinterpret_s8_u8(vcle_s8(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet16c pcmp_le<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return vreinterpretq_s8_u8(vcleq_s8(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4uc pcmp_le<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vcle_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pcmp_le<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vcle_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pcmp_le<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vcleq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pcmp_le<Packet4s>(const Packet4s& a, const Packet4s& b)
+{ return vreinterpret_s16_u16(vcle_s16(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet8s pcmp_le<Packet8s>(const Packet8s& a, const Packet8s& b)
+{ return vreinterpretq_s16_u16(vcleq_s16(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4us pcmp_le<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vcle_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pcmp_le<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vcleq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pcmp_le<Packet2i>(const Packet2i& a, const Packet2i& b)
+{ return vreinterpret_s32_u32(vcle_s32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4i pcmp_le<Packet4i>(const Packet4i& a, const Packet4i& b)
+{ return vreinterpretq_s32_u32(vcleq_s32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet2ui pcmp_le<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vcle_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pcmp_le<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vcleq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pcmp_le<Packet2l>(const Packet2l& a, const Packet2l& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vreinterpretq_s64_u64(vcleq_s64(a,b));
+#else
+  return vcombine_s64(
+      vdup_n_s64(vgetq_lane_s64(a, 0) <= vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
+      vdup_n_s64(vgetq_lane_s64(a, 1) <= vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pcmp_le<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vcleq_u64(a,b);
+#else
+  return vcombine_u64(
+      vdup_n_u64(vgetq_lane_u64(a, 0) <= vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
+      vdup_n_u64(vgetq_lane_u64(a, 1) <= vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pcmp_lt<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vclt_f32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt<Packet4f>(const Packet4f& a, const Packet4f& b)
+{ return vreinterpretq_f32_u32(vcltq_f32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4c pcmp_lt<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_u8(vclt_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pcmp_lt<Packet8c>(const Packet8c& a, const Packet8c& b)
+{ return vreinterpret_s8_u8(vclt_s8(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet16c pcmp_lt<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return vreinterpretq_s8_u8(vcltq_s8(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4uc pcmp_lt<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vclt_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pcmp_lt<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vclt_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pcmp_lt<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vcltq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pcmp_lt<Packet4s>(const Packet4s& a, const Packet4s& b)
+{ return vreinterpret_s16_u16(vclt_s16(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet8s pcmp_lt<Packet8s>(const Packet8s& a, const Packet8s& b)
+{ return vreinterpretq_s16_u16(vcltq_s16(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4us pcmp_lt<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vclt_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pcmp_lt<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vcltq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pcmp_lt<Packet2i>(const Packet2i& a, const Packet2i& b)
+{ return vreinterpret_s32_u32(vclt_s32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4i pcmp_lt<Packet4i>(const Packet4i& a, const Packet4i& b)
+{ return vreinterpretq_s32_u32(vcltq_s32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet2ui pcmp_lt<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vclt_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pcmp_lt<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vcltq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pcmp_lt<Packet2l>(const Packet2l& a, const Packet2l& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vreinterpretq_s64_u64(vcltq_s64(a,b));
+#else
+  return vcombine_s64(
+      vdup_n_s64(vgetq_lane_s64(a, 0) < vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
+      vdup_n_s64(vgetq_lane_s64(a, 1) < vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pcmp_lt<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vcltq_u64(a,b);
+#else
+  return vcombine_u64(
+      vdup_n_u64(vgetq_lane_u64(a, 0) < vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
+      vdup_n_u64(vgetq_lane_u64(a, 1) < vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pcmp_eq<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vceq_f32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_eq<Packet4f>(const Packet4f& a, const Packet4f& b)
+{ return vreinterpretq_f32_u32(vceqq_f32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4c pcmp_eq<Packet4c>(const Packet4c& a, const Packet4c& b)
+{
+  return vget_lane_s32(vreinterpret_s32_u8(vceq_s8(
+      vreinterpret_s8_s32(vdup_n_s32(a)),
+      vreinterpret_s8_s32(vdup_n_s32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8c pcmp_eq<Packet8c>(const Packet8c& a, const Packet8c& b)
+{ return vreinterpret_s8_u8(vceq_s8(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet16c pcmp_eq<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return vreinterpretq_s8_u8(vceqq_s8(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4uc pcmp_eq<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vceq_u8(
+      vreinterpret_u8_u32(vdup_n_u32(a)),
+      vreinterpret_u8_u32(vdup_n_u32(b)))), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pcmp_eq<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vceq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pcmp_eq<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vceqq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pcmp_eq<Packet4s>(const Packet4s& a, const Packet4s& b)
+{ return vreinterpret_s16_u16(vceq_s16(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet8s pcmp_eq<Packet8s>(const Packet8s& a, const Packet8s& b)
+{ return vreinterpretq_s16_u16(vceqq_s16(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4us pcmp_eq<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vceq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pcmp_eq<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vceqq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pcmp_eq<Packet2i>(const Packet2i& a, const Packet2i& b)
+{ return vreinterpret_s32_u32(vceq_s32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet4i pcmp_eq<Packet4i>(const Packet4i& a, const Packet4i& b)
+{ return vreinterpretq_s32_u32(vceqq_s32(a,b)); }
+template<> EIGEN_STRONG_INLINE Packet2ui pcmp_eq<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vceq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pcmp_eq<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vceqq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pcmp_eq<Packet2l>(const Packet2l& a, const Packet2l& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vreinterpretq_s64_u64(vceqq_s64(a,b));
+#else
+  return vcombine_s64(
+      vdup_n_s64(vgetq_lane_s64(a, 0) == vgetq_lane_s64(b, 0) ? numext::int64_t(-1) : 0),
+      vdup_n_s64(vgetq_lane_s64(a, 1) == vgetq_lane_s64(b, 1) ? numext::int64_t(-1) : 0));
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pcmp_eq<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{
+#if EIGEN_ARCH_ARM64
+  return vceqq_u64(a,b);
+#else
+  return vcombine_u64(
+      vdup_n_u64(vgetq_lane_u64(a, 0) == vgetq_lane_u64(b, 0) ? numext::uint64_t(-1) : 0),
+      vdup_n_u64(vgetq_lane_u64(a, 1) == vgetq_lane_u64(b, 1) ? numext::uint64_t(-1) : 0));
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pcmp_lt_or_nan<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vmvn_u32(vcge_f32(a,b))); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt_or_nan<Packet4f>(const Packet4f& a, const Packet4f& b)
+{ return vreinterpretq_f32_u32(vmvnq_u32(vcgeq_f32(a,b))); }
 
 // Logical Operations are not supported for float, so we have to reinterpret casts using NEON intrinsics
+template<> EIGEN_STRONG_INLINE Packet2f pand<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vand_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
 template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b)
-{
-  return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
-}
+{ return vreinterpretq_f32_u32(vandq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
+template<> EIGEN_STRONG_INLINE Packet4c pand<Packet4c>(const Packet4c& a, const Packet4c& b)
+{ return a & b; }
+template<> EIGEN_STRONG_INLINE Packet8c pand<Packet8c>(const Packet8c& a, const Packet8c& b)
+{ return vand_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pand<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return vandq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pand<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{ return a & b; }
+template<> EIGEN_STRONG_INLINE Packet8uc pand<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vand_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pand<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vandq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pand<Packet4s>(const Packet4s& a, const Packet4s& b) { return vand_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pand<Packet8s>(const Packet8s& a, const Packet8s& b) { return vandq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pand<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vand_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pand<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vandq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pand<Packet2i>(const Packet2i& a, const Packet2i& b) { return vand_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return vandq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pand<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vand_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pand<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vandq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pand<Packet2l>(const Packet2l& a, const Packet2l& b) { return vandq_s64(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ul pand<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{ return vandq_u64(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2f por<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vorr_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
 template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b)
-{
-  return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
-}
+{ return vreinterpretq_f32_u32(vorrq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
+template<> EIGEN_STRONG_INLINE Packet4c por<Packet4c>(const Packet4c& a, const Packet4c& b)
+{ return a | b; }
+template<> EIGEN_STRONG_INLINE Packet8c por<Packet8c>(const Packet8c& a, const Packet8c& b) { return vorr_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c por<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return vorrq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc por<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{ return a | b; }
+template<> EIGEN_STRONG_INLINE Packet8uc por<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vorr_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc por<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vorrq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s por<Packet4s>(const Packet4s& a, const Packet4s& b)
+{ return vorr_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s por<Packet8s>(const Packet8s& a, const Packet8s& b)
+{ return vorrq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us por<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vorr_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us por<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vorrq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i por<Packet2i>(const Packet2i& a, const Packet2i& b) { return vorr_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return vorrq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui por<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vorr_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui por<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vorrq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l por<Packet2l>(const Packet2l& a, const Packet2l& b)
+{ return vorrq_s64(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ul por<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{ return vorrq_u64(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2f pxor<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(veor_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
 template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b)
-{
-  return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
-}
+{ return vreinterpretq_f32_u32(veorq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
+template<> EIGEN_STRONG_INLINE Packet4c pxor<Packet4c>(const Packet4c& a, const Packet4c& b)
+{ return a ^ b; }
+template<> EIGEN_STRONG_INLINE Packet8c pxor<Packet8c>(const Packet8c& a, const Packet8c& b)
+{ return veor_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pxor<Packet16c>(const Packet16c& a, const Packet16c& b)
+{ return veorq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pxor<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{ return a ^ b; }
+template<> EIGEN_STRONG_INLINE Packet8uc pxor<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return veor_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pxor<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return veorq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pxor<Packet4s>(const Packet4s& a, const Packet4s& b) { return veor_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pxor<Packet8s>(const Packet8s& a, const Packet8s& b) { return veorq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pxor<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return veor_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pxor<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return veorq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pxor<Packet2i>(const Packet2i& a, const Packet2i& b) { return veor_s32(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return veorq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pxor<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return veor_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pxor<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return veorq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pxor<Packet2l>(const Packet2l& a, const Packet2l& b)
+{ return veorq_s64(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ul pxor<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{ return veorq_u64(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2f pandnot<Packet2f>(const Packet2f& a, const Packet2f& b)
+{ return vreinterpret_f32_u32(vbic_u32(vreinterpret_u32_f32(a),vreinterpret_u32_f32(b))); }
 template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b)
+{ return vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b))); }
+template<> EIGEN_STRONG_INLINE Packet4c pandnot<Packet4c>(const Packet4c& a, const Packet4c& b)
+{ return a & ~b; }
+template<> EIGEN_STRONG_INLINE Packet8c pandnot<Packet8c>(const Packet8c& a, const Packet8c& b) { return vbic_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16c pandnot<Packet16c>(const Packet16c& a, const Packet16c& b) { return vbicq_s8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4uc pandnot<Packet4uc>(const Packet4uc& a, const Packet4uc& b)
+{ return a & ~b; }
+template<> EIGEN_STRONG_INLINE Packet8uc pandnot<Packet8uc>(const Packet8uc& a, const Packet8uc& b)
+{ return vbic_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16uc pandnot<Packet16uc>(const Packet16uc& a, const Packet16uc& b)
+{ return vbicq_u8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4s pandnot<Packet4s>(const Packet4s& a, const Packet4s& b)
+{ return vbic_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8s pandnot<Packet8s>(const Packet8s& a, const Packet8s& b)
+{ return vbicq_s16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4us pandnot<Packet4us>(const Packet4us& a, const Packet4us& b)
+{ return vbic_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8us pandnot<Packet8us>(const Packet8us& a, const Packet8us& b)
+{ return vbicq_u16(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2i pandnot<Packet2i>(const Packet2i& a, const Packet2i& b)
+{ return vbic_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b)
+{ return vbicq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ui pandnot<Packet2ui>(const Packet2ui& a, const Packet2ui& b)
+{ return vbic_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4ui pandnot<Packet4ui>(const Packet4ui& a, const Packet4ui& b)
+{ return vbicq_u32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2l pandnot<Packet2l>(const Packet2l& a, const Packet2l& b)
+{ return vbicq_s64(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2ul pandnot<Packet2ul>(const Packet2ul& a, const Packet2ul& b)
+{ return vbicq_u64(a,b); }
+
+
+template<int N> EIGEN_STRONG_INLINE Packet4c parithmetic_shift_right(Packet4c& a)
+{ return vget_lane_s32(vreinterpret_s32_s8(vshr_n_s8(vreinterpret_s8_s32(vdup_n_s32(a)), N)), 0); }
+template<int N> EIGEN_STRONG_INLINE Packet8c parithmetic_shift_right(Packet8c a) { return vshr_n_s8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet16c parithmetic_shift_right(Packet16c a) { return vshrq_n_s8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4uc parithmetic_shift_right(Packet4uc& a)
+{ return vget_lane_u32(vreinterpret_u32_u8(vshr_n_u8(vreinterpret_u8_u32(vdup_n_u32(a)), N)), 0); }
+template<int N> EIGEN_STRONG_INLINE Packet8uc parithmetic_shift_right(Packet8uc a) { return vshr_n_u8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet16uc parithmetic_shift_right(Packet16uc a) { return vshrq_n_u8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4s parithmetic_shift_right(Packet4s a) { return vshr_n_s16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet8s parithmetic_shift_right(Packet8s a) { return vshrq_n_s16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4us parithmetic_shift_right(Packet4us a) { return vshr_n_u16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet8us parithmetic_shift_right(Packet8us a) { return vshrq_n_u16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2i parithmetic_shift_right(Packet2i a) { return vshr_n_s32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4i parithmetic_shift_right(Packet4i a) { return vshrq_n_s32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2ui parithmetic_shift_right(Packet2ui a) { return vshr_n_u32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4ui parithmetic_shift_right(Packet4ui a) { return vshrq_n_u32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2l parithmetic_shift_right(Packet2l a) { return vshrq_n_s64(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2ul parithmetic_shift_right(Packet2ul a) { return vshrq_n_u64(a,N); }
+
+template<int N> EIGEN_STRONG_INLINE Packet4c plogical_shift_right(Packet4c& a)
+{ return vget_lane_s32(vreinterpret_s32_u8(vshr_n_u8(vreinterpret_u8_s32(vdup_n_s32(a)), N)), 0); }
+template<int N> EIGEN_STRONG_INLINE Packet8c plogical_shift_right(Packet8c a)
+{ return vreinterpret_s8_u8(vshr_n_u8(vreinterpret_u8_s8(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet16c plogical_shift_right(Packet16c a)
+{ return vreinterpretq_s8_u8(vshrq_n_u8(vreinterpretq_u8_s8(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet4uc plogical_shift_right(Packet4uc& a)
+{ return vget_lane_u32(vreinterpret_u32_s8(vshr_n_s8(vreinterpret_s8_u32(vdup_n_u32(a)), N)), 0); }
+template<int N> EIGEN_STRONG_INLINE Packet8uc plogical_shift_right(Packet8uc a) { return vshr_n_u8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet16uc plogical_shift_right(Packet16uc a) { return vshrq_n_u8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4s plogical_shift_right(Packet4s a)
+{ return vreinterpret_s16_u16(vshr_n_u16(vreinterpret_u16_s16(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet8s plogical_shift_right(Packet8s a)
+{ return vreinterpretq_s16_u16(vshrq_n_u16(vreinterpretq_u16_s16(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet4us plogical_shift_right(Packet4us a) { return vshr_n_u16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet8us plogical_shift_right(Packet8us a) { return vshrq_n_u16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2i plogical_shift_right(Packet2i a)
+{ return vreinterpret_s32_u32(vshr_n_u32(vreinterpret_u32_s32(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_right(Packet4i a)
+{ return vreinterpretq_s32_u32(vshrq_n_u32(vreinterpretq_u32_s32(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet2ui plogical_shift_right(Packet2ui a) { return vshr_n_u32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4ui plogical_shift_right(Packet4ui a) { return vshrq_n_u32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2l plogical_shift_right(Packet2l a)
+{ return vreinterpretq_s64_u64(vshrq_n_u64(vreinterpretq_u64_s64(a),N)); }
+template<int N> EIGEN_STRONG_INLINE Packet2ul plogical_shift_right(Packet2ul a) { return vshrq_n_u64(a,N); }
+
+template<int N> EIGEN_STRONG_INLINE Packet4c plogical_shift_left(Packet4c& a)
+{ return vget_lane_s32(vreinterpret_s32_s8(vshl_n_s8(vreinterpret_s8_s32(vdup_n_s32(a)), N)), 0); }
+template<int N> EIGEN_STRONG_INLINE Packet8c plogical_shift_left(Packet8c a) { return vshl_n_s8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet16c plogical_shift_left(Packet16c a) { return vshlq_n_s8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4uc plogical_shift_left(Packet4uc& a)
+{ return vget_lane_u32(vreinterpret_u32_u8(vshl_n_u8(vreinterpret_u8_u32(vdup_n_u32(a)), N)), 0); }
+template<int N> EIGEN_STRONG_INLINE Packet8uc plogical_shift_left(Packet8uc a) { return vshl_n_u8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet16uc plogical_shift_left(Packet16uc a) { return vshlq_n_u8(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4s plogical_shift_left(Packet4s a) { return vshl_n_s16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet8s plogical_shift_left(Packet8s a) { return vshlq_n_s16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4us plogical_shift_left(Packet4us a) { return vshl_n_u16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet8us plogical_shift_left(Packet8us a) { return vshlq_n_u16(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2i plogical_shift_left(Packet2i a) { return vshl_n_s32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_left(Packet4i a) { return vshlq_n_s32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2ui plogical_shift_left(Packet2ui a) { return vshl_n_u32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4ui plogical_shift_left(Packet4ui a) { return vshlq_n_u32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2l plogical_shift_left(Packet2l a) { return vshlq_n_s64(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet2ul plogical_shift_left(Packet2ul a) { return vshlq_n_u64(a,N); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pload<Packet2f>(const float* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4c pload<Packet4c>(const int8_t* from)
 {
-  return vreinterpretq_f32_u32(vbicq_u32(vreinterpretq_u32_f32(a),vreinterpretq_u32_f32(b)));
+  Packet4c res;
+  memcpy(&res, from, sizeof(Packet4c));
+  return res;
 }
-template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) { return vbicq_s32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet8c pload<Packet8c>(const int8_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_s8(from); }
+template<> EIGEN_STRONG_INLINE Packet16c pload<Packet16c>(const int8_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s8(from); }
+template<> EIGEN_STRONG_INLINE Packet4uc pload<Packet4uc>(const uint8_t* from)
+{
+  Packet4uc res;
+  memcpy(&res, from, sizeof(Packet4uc));
+  return res;
+}
+template<> EIGEN_STRONG_INLINE Packet8uc pload<Packet8uc>(const uint8_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_u8(from); }
+template<> EIGEN_STRONG_INLINE Packet16uc pload<Packet16uc>(const uint8_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u8(from); }
+template<> EIGEN_STRONG_INLINE Packet4s pload<Packet4s>(const int16_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_s16(from); }
+template<> EIGEN_STRONG_INLINE Packet8s pload<Packet8s>(const int16_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s16(from); }
+template<> EIGEN_STRONG_INLINE Packet4us pload<Packet4us>(const uint16_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_u16(from); }
+template<> EIGEN_STRONG_INLINE Packet8us pload<Packet8us>(const uint16_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u16(from); }
+template<> EIGEN_STRONG_INLINE Packet2i pload<Packet2i>(const int32_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int32_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet2ui pload<Packet2ui>(const uint32_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet4ui pload<Packet4ui>(const uint32_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet2l pload<Packet2l>(const int64_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s64(from); }
+template<> EIGEN_STRONG_INLINE Packet2ul pload<Packet2ul>(const uint64_t* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_u64(from); }
 
-template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float*    from) { EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int32_t*  from) { EIGEN_DEBUG_ALIGNED_LOAD return vld1q_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet2f ploadu<Packet2f>(const float* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4c ploadu<Packet4c>(const int8_t* from)
+{
+  Packet4c res;
+  memcpy(&res, from, sizeof(Packet4c));
+  return res;
+}
+template<> EIGEN_STRONG_INLINE Packet8c ploadu<Packet8c>(const int8_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s8(from); }
+template<> EIGEN_STRONG_INLINE Packet16c ploadu<Packet16c>(const int8_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s8(from); }
+template<> EIGEN_STRONG_INLINE Packet4uc ploadu<Packet4uc>(const uint8_t* from)
+{
+  Packet4uc res;
+  memcpy(&res, from, sizeof(Packet4uc));
+  return res;
+}
+template<> EIGEN_STRONG_INLINE Packet8uc ploadu<Packet8uc>(const uint8_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u8(from); }
+template<> EIGEN_STRONG_INLINE Packet16uc ploadu<Packet16uc>(const uint8_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u8(from); }
+template<> EIGEN_STRONG_INLINE Packet4s ploadu<Packet4s>(const int16_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s16(from); }
+template<> EIGEN_STRONG_INLINE Packet8s ploadu<Packet8s>(const int16_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s16(from); }
+template<> EIGEN_STRONG_INLINE Packet4us ploadu<Packet4us>(const uint16_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u16(from); }
+template<> EIGEN_STRONG_INLINE Packet8us ploadu<Packet8us>(const uint16_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u16(from); }
+template<> EIGEN_STRONG_INLINE Packet2i ploadu<Packet2i>(const int32_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int32_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet2ui ploadu<Packet2ui>(const uint32_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet4ui ploadu<Packet4ui>(const uint32_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet2l ploadu<Packet2l>(const int64_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s64(from); }
+template<> EIGEN_STRONG_INLINE Packet2ul ploadu<Packet2ul>(const uint64_t* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_u64(from); }
 
-template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float*   from) { EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f32(from); }
-template<> EIGEN_STRONG_INLINE Packet4i ploadu<Packet4i>(const int32_t* from) { EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_s32(from); }
-
+template<> EIGEN_STRONG_INLINE Packet2f ploaddup<Packet2f>(const float* from)
+{ return vld1_dup_f32(from); }
 template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float* from)
+{ return vcombine_f32(vld1_dup_f32(from), vld1_dup_f32(from+1)); }
+template<> EIGEN_STRONG_INLINE Packet4c ploaddup<Packet4c>(const int8_t* from)
 {
-  float32x2_t lo, hi;
-  lo = vld1_dup_f32(from);
-  hi = vld1_dup_f32(from+1);
-  return vcombine_f32(lo, hi);
+  const int8x8_t a = vreinterpret_s8_s32(vdup_n_s32(pload<Packet4c>(from)));
+  return vget_lane_s32(vreinterpret_s32_s8(vzip_s8(a,a).val[0]), 0);
 }
+template<> EIGEN_STRONG_INLINE Packet8c ploaddup<Packet8c>(const int8_t* from)
+{
+  const int8x8_t a = vld1_s8(from);
+  return vzip_s8(a,a).val[0];
+}
+template<> EIGEN_STRONG_INLINE Packet16c ploaddup<Packet16c>(const int8_t* from)
+{
+  const int8x8_t a = vld1_s8(from);
+  const int8x8x2_t b = vzip_s8(a,a);
+  return vcombine_s8(b.val[0], b.val[1]);
+}
+template<> EIGEN_STRONG_INLINE Packet4uc ploaddup<Packet4uc>(const uint8_t* from)
+{
+  const uint8x8_t a = vreinterpret_u8_u32(vdup_n_u32(pload<Packet4uc>(from)));
+  return vget_lane_u32(vreinterpret_u32_u8(vzip_u8(a,a).val[0]), 0);
+}
+template<> EIGEN_STRONG_INLINE Packet8uc ploaddup<Packet8uc>(const uint8_t* from)
+{
+  const uint8x8_t a = vld1_u8(from);
+  return vzip_u8(a,a).val[0];
+}
+template<> EIGEN_STRONG_INLINE Packet16uc ploaddup<Packet16uc>(const uint8_t* from)
+{
+  const uint8x8_t a = vld1_u8(from);
+  const uint8x8x2_t b = vzip_u8(a,a);
+  return vcombine_u8(b.val[0], b.val[1]);
+}
+template<> EIGEN_STRONG_INLINE Packet4s ploaddup<Packet4s>(const int16_t* from)
+{
+  return vreinterpret_s16_u32(vzip_u32(vreinterpret_u32_s16(vld1_dup_s16(from)),
+      vreinterpret_u32_s16(vld1_dup_s16(from+1))).val[0]);
+}
+template<> EIGEN_STRONG_INLINE Packet8s ploaddup<Packet8s>(const int16_t* from)
+{
+  const int16x4_t a = vld1_s16(from);
+  const int16x4x2_t b = vzip_s16(a,a);
+  return vcombine_s16(b.val[0], b.val[1]);
+}
+template<> EIGEN_STRONG_INLINE Packet4us ploaddup<Packet4us>(const uint16_t* from)
+{
+  return vreinterpret_u16_u32(vzip_u32(vreinterpret_u32_u16(vld1_dup_u16(from)),
+      vreinterpret_u32_u16(vld1_dup_u16(from+1))).val[0]);
+}
+template<> EIGEN_STRONG_INLINE Packet8us ploaddup<Packet8us>(const uint16_t* from)
+{
+  const uint16x4_t a = vld1_u16(from);
+  const uint16x4x2_t b = vzip_u16(a,a);
+  return vcombine_u16(b.val[0], b.val[1]);
+}
+template<> EIGEN_STRONG_INLINE Packet2i ploaddup<Packet2i>(const int32_t* from)
+{ return vld1_dup_s32(from); }
 template<> EIGEN_STRONG_INLINE Packet4i ploaddup<Packet4i>(const int32_t* from)
+{ return vcombine_s32(vld1_dup_s32(from), vld1_dup_s32(from+1)); }
+template<> EIGEN_STRONG_INLINE Packet2ui ploaddup<Packet2ui>(const uint32_t* from)
+{ return vld1_dup_u32(from); }
+template<> EIGEN_STRONG_INLINE Packet4ui ploaddup<Packet4ui>(const uint32_t* from)
+{ return vcombine_u32(vld1_dup_u32(from), vld1_dup_u32(from+1)); }
+template<> EIGEN_STRONG_INLINE Packet2l ploaddup<Packet2l>(const int64_t* from)
+{ return vld1q_dup_s64(from); }
+template<> EIGEN_STRONG_INLINE Packet2ul ploaddup<Packet2ul>(const uint64_t* from)
+{ return vld1q_dup_u64(from); }
+
+template<> EIGEN_STRONG_INLINE Packet4f ploadquad<Packet4f>(const float* from) { return vld1q_dup_f32(from); }
+template<> EIGEN_STRONG_INLINE Packet4c ploadquad<Packet4c>(const int8_t* from)
+{ return vget_lane_s32(vreinterpret_s32_s8(vld1_dup_s8(from)), 0); }
+template<> EIGEN_STRONG_INLINE Packet8c ploadquad<Packet8c>(const int8_t* from)
 {
-  int32x2_t lo, hi;
-  lo = vld1_dup_s32(from);
-  hi = vld1_dup_s32(from+1);
-  return vcombine_s32(lo, hi);
+  return vreinterpret_s8_u32(vzip_u32(
+      vreinterpret_u32_s8(vld1_dup_s8(from)),
+      vreinterpret_u32_s8(vld1_dup_s8(from+1))).val[0]);
 }
-
-template<> EIGEN_STRONG_INLINE void pstore<float>  (float*    to, const Packet4f& from) { EIGEN_DEBUG_ALIGNED_STORE vst1q_f32(to, from); }
-template<> EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t*  to, const Packet4i& from) { EIGEN_DEBUG_ALIGNED_STORE vst1q_s32(to, from); }
-
-template<> EIGEN_STRONG_INLINE void pstoreu<float>  (float*   to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to, from); }
-template<> EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to, from); }
-
-template<> EIGEN_DEVICE_FUNC inline Packet4f pgather<float, Packet4f>(const float* from, Index stride)
+template<> EIGEN_STRONG_INLINE Packet16c ploadquad<Packet16c>(const int8_t* from)
 {
-  Packet4f res = pset1<Packet4f>(0.f);
-  res = vsetq_lane_f32(from[0*stride], res, 0);
-  res = vsetq_lane_f32(from[1*stride], res, 1);
-  res = vsetq_lane_f32(from[2*stride], res, 2);
-  res = vsetq_lane_f32(from[3*stride], res, 3);
+  const int8x8_t a = vreinterpret_s8_u32(vzip_u32(
+      vreinterpret_u32_s8(vld1_dup_s8(from)),
+      vreinterpret_u32_s8(vld1_dup_s8(from+1))).val[0]);
+  const int8x8_t b = vreinterpret_s8_u32(vzip_u32(
+      vreinterpret_u32_s8(vld1_dup_s8(from+2)),
+      vreinterpret_u32_s8(vld1_dup_s8(from+3))).val[0]);
+  return vcombine_s8(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet4uc ploadquad<Packet4uc>(const uint8_t* from)
+{ return vget_lane_u32(vreinterpret_u32_u8(vld1_dup_u8(from)), 0); }
+template<> EIGEN_STRONG_INLINE Packet8uc ploadquad<Packet8uc>(const uint8_t* from)
+{
+  return vreinterpret_u8_u32(vzip_u32(
+      vreinterpret_u32_u8(vld1_dup_u8(from)),
+      vreinterpret_u32_u8(vld1_dup_u8(from+1))).val[0]);
+}
+template<> EIGEN_STRONG_INLINE Packet16uc ploadquad<Packet16uc>(const uint8_t* from)
+{
+  const uint8x8_t a = vreinterpret_u8_u32(vzip_u32(
+      vreinterpret_u32_u8(vld1_dup_u8(from)),
+      vreinterpret_u32_u8(vld1_dup_u8(from+1))).val[0]);
+  const uint8x8_t b = vreinterpret_u8_u32(vzip_u32(
+      vreinterpret_u32_u8(vld1_dup_u8(from+2)),
+      vreinterpret_u32_u8(vld1_dup_u8(from+3))).val[0]);
+  return vcombine_u8(a,b);
+}
+template<> EIGEN_STRONG_INLINE Packet8s ploadquad<Packet8s>(const int16_t* from)
+{ return vcombine_s16(vld1_dup_s16(from), vld1_dup_s16(from+1)); }
+template<> EIGEN_STRONG_INLINE Packet8us ploadquad<Packet8us>(const uint16_t* from)
+{ return vcombine_u16(vld1_dup_u16(from), vld1_dup_u16(from+1)); }
+template<> EIGEN_STRONG_INLINE Packet4i ploadquad<Packet4i>(const int32_t* from) { return vld1q_dup_s32(from); }
+template<> EIGEN_STRONG_INLINE Packet4ui ploadquad<Packet4ui>(const uint32_t* from) { return vld1q_dup_u32(from); }
+
+template<> EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet2f& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_f32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<float>(float* to, const Packet4f& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_f32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet4c& from)
+{ memcpy(to, &from, sizeof(from)); }
+template<> EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet8c& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_s8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int8_t>(int8_t* to, const Packet16c& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet4uc& from)
+{ memcpy(to, &from, sizeof(from)); }
+template<> EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet8uc& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_u8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint8_t>(uint8_t* to, const Packet16uc& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int16_t>(int16_t* to, const Packet4s& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_s16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int16_t>(int16_t* to, const Packet8s& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint16_t>(uint16_t* to, const Packet4us& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_u16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint16_t>(uint16_t* to, const Packet8us& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t* to, const Packet2i& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_s32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int32_t>(int32_t* to, const Packet4i& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet2ui& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1_u32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint32_t>(uint32_t* to, const Packet4ui& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<int64_t>(int64_t* to, const Packet2l& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_s64(to,from); }
+template<> EIGEN_STRONG_INLINE void pstore<uint64_t>(uint64_t* to, const Packet2ul& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_u64(to,from); }
+
+template<> EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet2f& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_f32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<float>(float* to, const Packet4f& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_f32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet4c& from)
+{ memcpy(to, &from, sizeof(from)); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet8c& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_s8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int8_t>(int8_t* to, const Packet16c& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet4uc& from)
+{ memcpy(to, &from, sizeof(from)); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet8uc& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_u8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint8_t>(uint8_t* to, const Packet16uc& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u8(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int16_t>(int16_t* to, const Packet4s& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_s16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int16_t>(int16_t* to, const Packet8s& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint16_t>(uint16_t* to, const Packet4us& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_u16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint16_t>(uint16_t* to, const Packet8us& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u16(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet2i& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_s32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int32_t>(int32_t* to, const Packet4i& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet2ui& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1_u32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint32_t>(uint32_t* to, const Packet4ui& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u32(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<int64_t>(int64_t* to, const Packet2l& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_s64(to,from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<uint64_t>(uint64_t* to, const Packet2ul& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_u64(to,from); }
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2f pgather<float, Packet2f>(const float* from, Index stride)
+{
+  Packet2f res = vld1_dup_f32(from);
+  res = vld1_lane_f32(from + 1*stride, res, 1);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC inline Packet4i pgather<int32_t, Packet4i>(const int32_t* from, Index stride)
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4f pgather<float, Packet4f>(const float* from, Index stride)
 {
-  Packet4i res = pset1<Packet4i>(0);
-  res = vsetq_lane_s32(from[0*stride], res, 0);
-  res = vsetq_lane_s32(from[1*stride], res, 1);
-  res = vsetq_lane_s32(from[2*stride], res, 2);
-  res = vsetq_lane_s32(from[3*stride], res, 3);
+  Packet4f res = vld1q_dup_f32(from);
+  res = vld1q_lane_f32(from + 1*stride, res, 1);
+  res = vld1q_lane_f32(from + 2*stride, res, 2);
+  res = vld1q_lane_f32(from + 3*stride, res, 3);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4c pgather<int8_t, Packet4c>(const int8_t* from, Index stride)
+{
+  Packet4c res;
+  for (int i = 0; i != 4; i++)
+    reinterpret_cast<int8_t*>(&res)[i] = *(from + i * stride);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c pgather<int8_t, Packet8c>(const int8_t* from, Index stride)
+{
+  Packet8c res = vld1_dup_s8(from);
+  res = vld1_lane_s8(from + 1*stride, res, 1);
+  res = vld1_lane_s8(from + 2*stride, res, 2);
+  res = vld1_lane_s8(from + 3*stride, res, 3);
+  res = vld1_lane_s8(from + 4*stride, res, 4);
+  res = vld1_lane_s8(from + 5*stride, res, 5);
+  res = vld1_lane_s8(from + 6*stride, res, 6);
+  res = vld1_lane_s8(from + 7*stride, res, 7);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16c pgather<int8_t, Packet16c>(const int8_t* from, Index stride)
+{
+  Packet16c res = vld1q_dup_s8(from);
+  res = vld1q_lane_s8(from + 1*stride, res, 1);
+  res = vld1q_lane_s8(from + 2*stride, res, 2);
+  res = vld1q_lane_s8(from + 3*stride, res, 3);
+  res = vld1q_lane_s8(from + 4*stride, res, 4);
+  res = vld1q_lane_s8(from + 5*stride, res, 5);
+  res = vld1q_lane_s8(from + 6*stride, res, 6);
+  res = vld1q_lane_s8(from + 7*stride, res, 7);
+  res = vld1q_lane_s8(from + 8*stride, res, 8);
+  res = vld1q_lane_s8(from + 9*stride, res, 9);
+  res = vld1q_lane_s8(from + 10*stride, res, 10);
+  res = vld1q_lane_s8(from + 11*stride, res, 11);
+  res = vld1q_lane_s8(from + 12*stride, res, 12);
+  res = vld1q_lane_s8(from + 13*stride, res, 13);
+  res = vld1q_lane_s8(from + 14*stride, res, 14);
+  res = vld1q_lane_s8(from + 15*stride, res, 15);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4uc pgather<uint8_t, Packet4uc>(const uint8_t* from, Index stride)
+{
+  Packet4uc res;
+  for (int i = 0; i != 4; i++)
+    reinterpret_cast<uint8_t*>(&res)[i] = *(from + i * stride);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc pgather<uint8_t, Packet8uc>(const uint8_t* from, Index stride)
+{
+  Packet8uc res = vld1_dup_u8(from);
+  res = vld1_lane_u8(from + 1*stride, res, 1);
+  res = vld1_lane_u8(from + 2*stride, res, 2);
+  res = vld1_lane_u8(from + 3*stride, res, 3);
+  res = vld1_lane_u8(from + 4*stride, res, 4);
+  res = vld1_lane_u8(from + 5*stride, res, 5);
+  res = vld1_lane_u8(from + 6*stride, res, 6);
+  res = vld1_lane_u8(from + 7*stride, res, 7);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16uc pgather<uint8_t, Packet16uc>(const uint8_t* from, Index stride)
+{
+  Packet16uc res = vld1q_dup_u8(from);
+  res = vld1q_lane_u8(from + 1*stride, res, 1);
+  res = vld1q_lane_u8(from + 2*stride, res, 2);
+  res = vld1q_lane_u8(from + 3*stride, res, 3);
+  res = vld1q_lane_u8(from + 4*stride, res, 4);
+  res = vld1q_lane_u8(from + 5*stride, res, 5);
+  res = vld1q_lane_u8(from + 6*stride, res, 6);
+  res = vld1q_lane_u8(from + 7*stride, res, 7);
+  res = vld1q_lane_u8(from + 8*stride, res, 8);
+  res = vld1q_lane_u8(from + 9*stride, res, 9);
+  res = vld1q_lane_u8(from + 10*stride, res, 10);
+  res = vld1q_lane_u8(from + 11*stride, res, 11);
+  res = vld1q_lane_u8(from + 12*stride, res, 12);
+  res = vld1q_lane_u8(from + 13*stride, res, 13);
+  res = vld1q_lane_u8(from + 14*stride, res, 14);
+  res = vld1q_lane_u8(from + 15*stride, res, 15);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s pgather<int16_t, Packet4s>(const int16_t* from, Index stride)
+{
+  Packet4s res = vld1_dup_s16(from);
+  res = vld1_lane_s16(from + 1*stride, res, 1);
+  res = vld1_lane_s16(from + 2*stride, res, 2);
+  res = vld1_lane_s16(from + 3*stride, res, 3);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8s pgather<int16_t, Packet8s>(const int16_t* from, Index stride)
+{
+  Packet8s res = vld1q_dup_s16(from);
+  res = vld1q_lane_s16(from + 1*stride, res, 1);
+  res = vld1q_lane_s16(from + 2*stride, res, 2);
+  res = vld1q_lane_s16(from + 3*stride, res, 3);
+  res = vld1q_lane_s16(from + 4*stride, res, 4);
+  res = vld1q_lane_s16(from + 5*stride, res, 5);
+  res = vld1q_lane_s16(from + 6*stride, res, 6);
+  res = vld1q_lane_s16(from + 7*stride, res, 7);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us pgather<uint16_t, Packet4us>(const uint16_t* from, Index stride)
+{
+  Packet4us res = vld1_dup_u16(from);
+  res = vld1_lane_u16(from + 1*stride, res, 1);
+  res = vld1_lane_u16(from + 2*stride, res, 2);
+  res = vld1_lane_u16(from + 3*stride, res, 3);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8us pgather<uint16_t, Packet8us>(const uint16_t* from, Index stride)
+{
+  Packet8us res = vld1q_dup_u16(from);
+  res = vld1q_lane_u16(from + 1*stride, res, 1);
+  res = vld1q_lane_u16(from + 2*stride, res, 2);
+  res = vld1q_lane_u16(from + 3*stride, res, 3);
+  res = vld1q_lane_u16(from + 4*stride, res, 4);
+  res = vld1q_lane_u16(from + 5*stride, res, 5);
+  res = vld1q_lane_u16(from + 6*stride, res, 6);
+  res = vld1q_lane_u16(from + 7*stride, res, 7);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2i pgather<int32_t, Packet2i>(const int32_t* from, Index stride)
+{
+  Packet2i res = vld1_dup_s32(from);
+  res = vld1_lane_s32(from + 1*stride, res, 1);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4i pgather<int32_t, Packet4i>(const int32_t* from, Index stride)
+{
+  Packet4i res = vld1q_dup_s32(from);
+  res = vld1q_lane_s32(from + 1*stride, res, 1);
+  res = vld1q_lane_s32(from + 2*stride, res, 2);
+  res = vld1q_lane_s32(from + 3*stride, res, 3);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ui pgather<uint32_t, Packet2ui>(const uint32_t* from, Index stride)
+{
+  Packet2ui res = vld1_dup_u32(from);
+  res = vld1_lane_u32(from + 1*stride, res, 1);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4ui pgather<uint32_t, Packet4ui>(const uint32_t* from, Index stride)
+{
+  Packet4ui res = vld1q_dup_u32(from);
+  res = vld1q_lane_u32(from + 1*stride, res, 1);
+  res = vld1q_lane_u32(from + 2*stride, res, 2);
+  res = vld1q_lane_u32(from + 3*stride, res, 3);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2l pgather<int64_t, Packet2l>(const int64_t* from, Index stride)
+{
+  Packet2l res = vld1q_dup_s64(from);
+  res = vld1q_lane_s64(from + 1*stride, res, 1);
+  return res;
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ul pgather<uint64_t, Packet2ul>(const uint64_t* from, Index stride)
+{
+  Packet2ul res = vld1q_dup_u64(from);
+  res = vld1q_lane_u64(from + 1*stride, res, 1);
   return res;
 }
 
-template<> EIGEN_DEVICE_FUNC inline void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride)
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<float, Packet2f>(float* to, const Packet2f& from, Index stride)
 {
-  to[stride*0] = vgetq_lane_f32(from, 0);
-  to[stride*1] = vgetq_lane_f32(from, 1);
-  to[stride*2] = vgetq_lane_f32(from, 2);
-  to[stride*3] = vgetq_lane_f32(from, 3);
+  vst1_lane_f32(to + stride*0, from, 0);
+  vst1_lane_f32(to + stride*1, from, 1);
 }
-template<> EIGEN_DEVICE_FUNC inline void pscatter<int32_t, Packet4i>(int32_t* to, const Packet4i& from, Index stride)
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride)
 {
-  to[stride*0] = vgetq_lane_s32(from, 0);
-  to[stride*1] = vgetq_lane_s32(from, 1);
-  to[stride*2] = vgetq_lane_s32(from, 2);
-  to[stride*3] = vgetq_lane_s32(from, 3);
+  vst1q_lane_f32(to + stride*0, from, 0);
+  vst1q_lane_f32(to + stride*1, from, 1);
+  vst1q_lane_f32(to + stride*2, from, 2);
+  vst1q_lane_f32(to + stride*3, from, 3);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet4c>(int8_t* to, const Packet4c& from, Index stride)
+{
+  for (int i = 0; i != 4; i++)
+    *(to + i * stride) = reinterpret_cast<const int8_t*>(&from)[i];
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet8c>(int8_t* to, const Packet8c& from, Index stride)
+{
+  vst1_lane_s8(to + stride*0, from, 0);
+  vst1_lane_s8(to + stride*1, from, 1);
+  vst1_lane_s8(to + stride*2, from, 2);
+  vst1_lane_s8(to + stride*3, from, 3);
+  vst1_lane_s8(to + stride*4, from, 4);
+  vst1_lane_s8(to + stride*5, from, 5);
+  vst1_lane_s8(to + stride*6, from, 6);
+  vst1_lane_s8(to + stride*7, from, 7);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int8_t, Packet16c>(int8_t* to, const Packet16c& from, Index stride)
+{
+  vst1q_lane_s8(to + stride*0, from, 0);
+  vst1q_lane_s8(to + stride*1, from, 1);
+  vst1q_lane_s8(to + stride*2, from, 2);
+  vst1q_lane_s8(to + stride*3, from, 3);
+  vst1q_lane_s8(to + stride*4, from, 4);
+  vst1q_lane_s8(to + stride*5, from, 5);
+  vst1q_lane_s8(to + stride*6, from, 6);
+  vst1q_lane_s8(to + stride*7, from, 7);
+  vst1q_lane_s8(to + stride*8, from, 8);
+  vst1q_lane_s8(to + stride*9, from, 9);
+  vst1q_lane_s8(to + stride*10, from, 10);
+  vst1q_lane_s8(to + stride*11, from, 11);
+  vst1q_lane_s8(to + stride*12, from, 12);
+  vst1q_lane_s8(to + stride*13, from, 13);
+  vst1q_lane_s8(to + stride*14, from, 14);
+  vst1q_lane_s8(to + stride*15, from, 15);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet4uc>(uint8_t* to, const Packet4uc& from, Index stride)
+{
+  for (int i = 0; i != 4; i++)
+    *(to + i * stride) = reinterpret_cast<const uint8_t*>(&from)[i];
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet8uc>(uint8_t* to, const Packet8uc& from, Index stride)
+{
+  vst1_lane_u8(to + stride*0, from, 0);
+  vst1_lane_u8(to + stride*1, from, 1);
+  vst1_lane_u8(to + stride*2, from, 2);
+  vst1_lane_u8(to + stride*3, from, 3);
+  vst1_lane_u8(to + stride*4, from, 4);
+  vst1_lane_u8(to + stride*5, from, 5);
+  vst1_lane_u8(to + stride*6, from, 6);
+  vst1_lane_u8(to + stride*7, from, 7);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint8_t, Packet16uc>(uint8_t* to, const Packet16uc& from, Index stride)
+{
+  vst1q_lane_u8(to + stride*0, from, 0);
+  vst1q_lane_u8(to + stride*1, from, 1);
+  vst1q_lane_u8(to + stride*2, from, 2);
+  vst1q_lane_u8(to + stride*3, from, 3);
+  vst1q_lane_u8(to + stride*4, from, 4);
+  vst1q_lane_u8(to + stride*5, from, 5);
+  vst1q_lane_u8(to + stride*6, from, 6);
+  vst1q_lane_u8(to + stride*7, from, 7);
+  vst1q_lane_u8(to + stride*8, from, 8);
+  vst1q_lane_u8(to + stride*9, from, 9);
+  vst1q_lane_u8(to + stride*10, from, 10);
+  vst1q_lane_u8(to + stride*11, from, 11);
+  vst1q_lane_u8(to + stride*12, from, 12);
+  vst1q_lane_u8(to + stride*13, from, 13);
+  vst1q_lane_u8(to + stride*14, from, 14);
+  vst1q_lane_u8(to + stride*15, from, 15);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int16_t, Packet4s>(int16_t* to, const Packet4s& from, Index stride)
+{
+  vst1_lane_s16(to + stride*0, from, 0);
+  vst1_lane_s16(to + stride*1, from, 1);
+  vst1_lane_s16(to + stride*2, from, 2);
+  vst1_lane_s16(to + stride*3, from, 3);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int16_t, Packet8s>(int16_t* to, const Packet8s& from, Index stride)
+{
+  vst1q_lane_s16(to + stride*0, from, 0);
+  vst1q_lane_s16(to + stride*1, from, 1);
+  vst1q_lane_s16(to + stride*2, from, 2);
+  vst1q_lane_s16(to + stride*3, from, 3);
+  vst1q_lane_s16(to + stride*4, from, 4);
+  vst1q_lane_s16(to + stride*5, from, 5);
+  vst1q_lane_s16(to + stride*6, from, 6);
+  vst1q_lane_s16(to + stride*7, from, 7);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint16_t, Packet4us>(uint16_t* to, const Packet4us& from, Index stride)
+{
+  vst1_lane_u16(to + stride*0, from, 0);
+  vst1_lane_u16(to + stride*1, from, 1);
+  vst1_lane_u16(to + stride*2, from, 2);
+  vst1_lane_u16(to + stride*3, from, 3);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint16_t, Packet8us>(uint16_t* to, const Packet8us& from, Index stride)
+{
+  vst1q_lane_u16(to + stride*0, from, 0);
+  vst1q_lane_u16(to + stride*1, from, 1);
+  vst1q_lane_u16(to + stride*2, from, 2);
+  vst1q_lane_u16(to + stride*3, from, 3);
+  vst1q_lane_u16(to + stride*4, from, 4);
+  vst1q_lane_u16(to + stride*5, from, 5);
+  vst1q_lane_u16(to + stride*6, from, 6);
+  vst1q_lane_u16(to + stride*7, from, 7);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int32_t, Packet2i>(int32_t* to, const Packet2i& from, Index stride)
+{
+  vst1_lane_s32(to + stride*0, from, 0);
+  vst1_lane_s32(to + stride*1, from, 1);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int32_t, Packet4i>(int32_t* to, const Packet4i& from, Index stride)
+{
+  vst1q_lane_s32(to + stride*0, from, 0);
+  vst1q_lane_s32(to + stride*1, from, 1);
+  vst1q_lane_s32(to + stride*2, from, 2);
+  vst1q_lane_s32(to + stride*3, from, 3);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint32_t, Packet2ui>(uint32_t* to, const Packet2ui& from, Index stride)
+{
+  vst1_lane_u32(to + stride*0, from, 0);
+  vst1_lane_u32(to + stride*1, from, 1);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint32_t, Packet4ui>(uint32_t* to, const Packet4ui& from, Index stride)
+{
+  vst1q_lane_u32(to + stride*0, from, 0);
+  vst1q_lane_u32(to + stride*1, from, 1);
+  vst1q_lane_u32(to + stride*2, from, 2);
+  vst1q_lane_u32(to + stride*3, from, 3);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<int64_t, Packet2l>(int64_t* to, const Packet2l& from, Index stride)
+{
+  vst1q_lane_s64(to + stride*0, from, 0);
+  vst1q_lane_s64(to + stride*1, from, 1);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<uint64_t, Packet2ul>(uint64_t* to, const Packet2ul& from, Index stride)
+{
+  vst1q_lane_u64(to + stride*0, from, 0);
+  vst1q_lane_u64(to + stride*1, from, 1);
 }
 
-template<> EIGEN_STRONG_INLINE void prefetch<float>  (const float*    addr) { EIGEN_ARM_PREFETCH(addr); }
-template<> EIGEN_STRONG_INLINE void prefetch<int32_t>(const int32_t*  addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<float>(const float* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<int8_t>(const int8_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<uint8_t>(const uint8_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<int16_t>(const int16_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<uint16_t>(const uint16_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<int32_t>(const int32_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<uint32_t>(const uint32_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<int64_t>(const int64_t* addr) { EIGEN_ARM_PREFETCH(addr); }
+template<> EIGEN_STRONG_INLINE void prefetch<uint64_t>(const uint64_t* addr) { EIGEN_ARM_PREFETCH(addr); }
 
-// FIXME only store the 2 first elements ?
-template<> EIGEN_STRONG_INLINE float   pfirst<Packet4f>(const Packet4f& a) { float   EIGEN_ALIGN16 x[4]; vst1q_f32(x, a); return x[0]; }
-template<> EIGEN_STRONG_INLINE int32_t pfirst<Packet4i>(const Packet4i& a) { int32_t EIGEN_ALIGN16 x[4]; vst1q_s32(x, a); return x[0]; }
+template<> EIGEN_STRONG_INLINE float pfirst<Packet2f>(const Packet2f& a) { return vget_lane_f32(a,0); }
+template<> EIGEN_STRONG_INLINE float pfirst<Packet4f>(const Packet4f& a) { return vgetq_lane_f32(a,0); }
+template<> EIGEN_STRONG_INLINE int8_t pfirst<Packet4c>(const Packet4c& a) { return static_cast<int8_t>(a & 0xff); }
+template<> EIGEN_STRONG_INLINE int8_t pfirst<Packet8c>(const Packet8c& a) { return vget_lane_s8(a,0); }
+template<> EIGEN_STRONG_INLINE int8_t pfirst<Packet16c>(const Packet16c& a) { return vgetq_lane_s8(a,0); }
+template<> EIGEN_STRONG_INLINE uint8_t pfirst<Packet4uc>(const Packet4uc& a) { return static_cast<uint8_t>(a & 0xff); }
+template<> EIGEN_STRONG_INLINE uint8_t pfirst<Packet8uc>(const Packet8uc& a) { return vget_lane_u8(a,0); }
+template<> EIGEN_STRONG_INLINE uint8_t pfirst<Packet16uc>(const Packet16uc& a) { return vgetq_lane_u8(a,0); }
+template<> EIGEN_STRONG_INLINE int16_t pfirst<Packet4s>(const Packet4s& a) { return vget_lane_s16(a,0); }
+template<> EIGEN_STRONG_INLINE int16_t pfirst<Packet8s>(const Packet8s& a) { return vgetq_lane_s16(a,0); }
+template<> EIGEN_STRONG_INLINE uint16_t pfirst<Packet4us>(const Packet4us& a) { return vget_lane_u16(a,0); }
+template<> EIGEN_STRONG_INLINE uint16_t pfirst<Packet8us>(const Packet8us& a) { return vgetq_lane_u16(a,0); }
+template<> EIGEN_STRONG_INLINE int32_t pfirst<Packet2i>(const Packet2i& a) { return vget_lane_s32(a,0); }
+template<> EIGEN_STRONG_INLINE int32_t pfirst<Packet4i>(const Packet4i& a) { return vgetq_lane_s32(a,0); }
+template<> EIGEN_STRONG_INLINE uint32_t pfirst<Packet2ui>(const Packet2ui& a) { return vget_lane_u32(a,0); }
+template<> EIGEN_STRONG_INLINE uint32_t pfirst<Packet4ui>(const Packet4ui& a) { return vgetq_lane_u32(a,0); }
+template<> EIGEN_STRONG_INLINE int64_t pfirst<Packet2l>(const Packet2l& a) { return vgetq_lane_s64(a,0); }
+template<> EIGEN_STRONG_INLINE uint64_t pfirst<Packet2ul>(const Packet2ul& a) { return vgetq_lane_u64(a,0); }
 
-template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a) {
-  float32x2_t a_lo, a_hi;
-  Packet4f a_r64;
-
-  a_r64 = vrev64q_f32(a);
-  a_lo = vget_low_f32(a_r64);
-  a_hi = vget_high_f32(a_r64);
-  return vcombine_f32(a_hi, a_lo);
+template<> EIGEN_STRONG_INLINE Packet2f preverse(const Packet2f& a) { return vrev64_f32(a); }
+template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a)
+{
+  const float32x4_t a_r64 = vrev64q_f32(a);
+  return vcombine_f32(vget_high_f32(a_r64), vget_low_f32(a_r64));
 }
-template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a) {
-  int32x2_t a_lo, a_hi;
-  Packet4i a_r64;
-
-  a_r64 = vrev64q_s32(a);
-  a_lo = vget_low_s32(a_r64);
-  a_hi = vget_high_s32(a_r64);
-  return vcombine_s32(a_hi, a_lo);
+template<> EIGEN_STRONG_INLINE Packet4c preverse(const Packet4c& a)
+{ return vget_lane_s32(vreinterpret_s32_s8(vrev64_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0); }
+template<> EIGEN_STRONG_INLINE Packet8c preverse(const Packet8c& a) { return vrev64_s8(a); }
+template<> EIGEN_STRONG_INLINE Packet16c preverse(const Packet16c& a)
+{
+  const int8x16_t a_r64 = vrev64q_s8(a);
+  return vcombine_s8(vget_high_s8(a_r64), vget_low_s8(a_r64));
 }
+template<> EIGEN_STRONG_INLINE Packet4uc preverse(const Packet4uc& a)
+{ return vget_lane_u32(vreinterpret_u32_u8(vrev64_u8(vreinterpret_u8_u32(vdup_n_u32(a)))), 0); }
+template<> EIGEN_STRONG_INLINE Packet8uc preverse(const Packet8uc& a) { return vrev64_u8(a); }
+template<> EIGEN_STRONG_INLINE Packet16uc preverse(const Packet16uc& a)
+{
+  const uint8x16_t a_r64 = vrev64q_u8(a);
+  return vcombine_u8(vget_high_u8(a_r64), vget_low_u8(a_r64));
+}
+template<> EIGEN_STRONG_INLINE Packet4s preverse(const Packet4s& a) { return vrev64_s16(a); }
+template<> EIGEN_STRONG_INLINE Packet8s preverse(const Packet8s& a)
+{
+  const int16x8_t a_r64 = vrev64q_s16(a);
+  return vcombine_s16(vget_high_s16(a_r64), vget_low_s16(a_r64));
+}
+template<> EIGEN_STRONG_INLINE Packet4us preverse(const Packet4us& a) { return vrev64_u16(a); }
+template<> EIGEN_STRONG_INLINE Packet8us preverse(const Packet8us& a)
+{
+  const uint16x8_t a_r64 = vrev64q_u16(a);
+  return vcombine_u16(vget_high_u16(a_r64), vget_low_u16(a_r64));
+}
+template<> EIGEN_STRONG_INLINE Packet2i preverse(const Packet2i& a) { return vrev64_s32(a); }
+template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a)
+{
+  const int32x4_t a_r64 = vrev64q_s32(a);
+  return vcombine_s32(vget_high_s32(a_r64), vget_low_s32(a_r64));
+}
+template<> EIGEN_STRONG_INLINE Packet2ui preverse(const Packet2ui& a) { return vrev64_u32(a); }
+template<> EIGEN_STRONG_INLINE Packet4ui preverse(const Packet4ui& a)
+{
+  const uint32x4_t a_r64 = vrev64q_u32(a);
+  return vcombine_u32(vget_high_u32(a_r64), vget_low_u32(a_r64));
+}
+template<> EIGEN_STRONG_INLINE Packet2l preverse(const Packet2l& a)
+{ return vcombine_s64(vget_high_s64(a), vget_low_s64(a)); }
+template<> EIGEN_STRONG_INLINE Packet2ul preverse(const Packet2ul& a)
+{ return vcombine_u64(vget_high_u64(a), vget_low_u64(a)); }
 
+template<> EIGEN_STRONG_INLINE Packet2f pabs(const Packet2f& a) { return vabs_f32(a); }
 template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a) { return vabsq_f32(a); }
+template<> EIGEN_STRONG_INLINE Packet4c pabs<Packet4c>(const Packet4c& a)
+{ return vget_lane_s32(vreinterpret_s32_s8(vabs_s8(vreinterpret_s8_s32(vdup_n_s32(a)))), 0); }
+template<> EIGEN_STRONG_INLINE Packet8c pabs(const Packet8c& a) { return vabs_s8(a); }
+template<> EIGEN_STRONG_INLINE Packet16c pabs(const Packet16c& a) { return vabsq_s8(a); }
+template<> EIGEN_STRONG_INLINE Packet4uc pabs(const Packet4uc& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet8uc pabs(const Packet8uc& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet16uc pabs(const Packet16uc& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4s pabs(const Packet4s& a) { return vabs_s16(a); }
+template<> EIGEN_STRONG_INLINE Packet8s pabs(const Packet8s& a) { return vabsq_s16(a); }
+template<> EIGEN_STRONG_INLINE Packet4us pabs(const Packet4us& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet8us pabs(const Packet8us& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet2i pabs(const Packet2i& a) { return vabs_s32(a); }
 template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a) { return vabsq_s32(a); }
+template<> EIGEN_STRONG_INLINE Packet2ui pabs(const Packet2ui& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet4ui pabs(const Packet4ui& a) { return a; }
+template<> EIGEN_STRONG_INLINE Packet2l pabs(const Packet2l& a) {
+#if EIGEN_ARCH_ARM64
+  return vabsq_s64(a);
+#else
+  return vcombine_s64(
+      vdup_n_s64((std::abs)(vgetq_lane_s64(a, 0))),
+      vdup_n_s64((std::abs)(vgetq_lane_s64(a, 1))));
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2ul pabs(const Packet2ul& a) { return a; }
 
+template<> EIGEN_STRONG_INLINE Packet2f pfrexp<Packet2f>(const Packet2f& a, Packet2f& exponent)
+{ return pfrexp_generic(a,exponent); }
+template<> EIGEN_STRONG_INLINE Packet4f pfrexp<Packet4f>(const Packet4f& a, Packet4f& exponent)
+{ return pfrexp_generic(a,exponent); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pldexp<Packet2f>(const Packet2f& a, const Packet2f& exponent)
+{ return pldexp_generic(a,exponent); }
+template<> EIGEN_STRONG_INLINE Packet4f pldexp<Packet4f>(const Packet4f& a, const Packet4f& exponent)
+{ return pldexp_generic(a,exponent); }
+
+template<> EIGEN_STRONG_INLINE float predux<Packet2f>(const Packet2f& a) { return vget_lane_f32(vpadd_f32(a,a), 0); }
 template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
 {
-  float32x2_t a_lo, a_hi, sum;
-
-  a_lo = vget_low_f32(a);
-  a_hi = vget_high_f32(a);
-  sum = vpadd_f32(a_lo, a_hi);
-  sum = vpadd_f32(sum, sum);
-  return vget_lane_f32(sum, 0);
+  const float32x2_t sum = vadd_f32(vget_low_f32(a), vget_high_f32(a));
+  return vget_lane_f32(vpadd_f32(sum, sum), 0);
 }
-
-template<> EIGEN_STRONG_INLINE Packet4f preduxp<Packet4f>(const Packet4f* vecs)
+template<> EIGEN_STRONG_INLINE int8_t predux<Packet4c>(const Packet4c& a)
 {
-  float32x4x2_t vtrn1, vtrn2, res1, res2;
-  Packet4f sum1, sum2, sum;
-
-  // NEON zip performs interleaving of the supplied vectors.
-  // We perform two interleaves in a row to acquire the transposed vector
-  vtrn1 = vzipq_f32(vecs[0], vecs[2]);
-  vtrn2 = vzipq_f32(vecs[1], vecs[3]);
-  res1 = vzipq_f32(vtrn1.val[0], vtrn2.val[0]);
-  res2 = vzipq_f32(vtrn1.val[1], vtrn2.val[1]);
-
-  // Do the addition of the resulting vectors
-  sum1 = vaddq_f32(res1.val[0], res1.val[1]);
-  sum2 = vaddq_f32(res2.val[0], res2.val[1]);
-  sum = vaddq_f32(sum1, sum2);
-
-  return sum;
+  const int8x8_t a_dup = vreinterpret_s8_s32(vdup_n_s32(a));
+  int8x8_t sum = vpadd_s8(a_dup, a_dup);
+  sum = vpadd_s8(sum, sum);
+  return vget_lane_s8(sum, 0);
 }
-
+template<> EIGEN_STRONG_INLINE int8_t predux<Packet8c>(const Packet8c& a)
+{
+  int8x8_t sum = vpadd_s8(a,a);
+  sum = vpadd_s8(sum, sum);
+  sum = vpadd_s8(sum, sum);
+  return vget_lane_s8(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE int8_t predux<Packet16c>(const Packet16c& a)
+{
+  int8x8_t sum = vadd_s8(vget_low_s8(a), vget_high_s8(a));
+  sum = vpadd_s8(sum, sum);
+  sum = vpadd_s8(sum, sum);
+  sum = vpadd_s8(sum, sum);
+  return vget_lane_s8(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux<Packet4uc>(const Packet4uc& a)
+{
+  const uint8x8_t a_dup = vreinterpret_u8_u32(vdup_n_u32(a));
+  uint8x8_t sum = vpadd_u8(a_dup, a_dup);
+  sum = vpadd_u8(sum, sum);
+  return vget_lane_u8(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux<Packet8uc>(const Packet8uc& a)
+{
+  uint8x8_t sum = vpadd_u8(a,a);
+  sum = vpadd_u8(sum, sum);
+  sum = vpadd_u8(sum, sum);
+  return vget_lane_u8(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux<Packet16uc>(const Packet16uc& a)
+{
+  uint8x8_t sum = vadd_u8(vget_low_u8(a), vget_high_u8(a));
+  sum = vpadd_u8(sum, sum);
+  sum = vpadd_u8(sum, sum);
+  sum = vpadd_u8(sum, sum);
+  return vget_lane_u8(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux<Packet4s>(const Packet4s& a)
+{
+  const int16x4_t sum = vpadd_s16(a,a);
+  return vget_lane_s16(vpadd_s16(sum, sum), 0);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux<Packet8s>(const Packet8s& a)
+{
+  int16x4_t sum = vadd_s16(vget_low_s16(a), vget_high_s16(a));
+  sum = vpadd_s16(sum, sum);
+  sum = vpadd_s16(sum, sum);
+  return vget_lane_s16(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux<Packet4us>(const Packet4us& a)
+{
+  const uint16x4_t sum = vpadd_u16(a,a);
+  return vget_lane_u16(vpadd_u16(sum, sum), 0);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux<Packet8us>(const Packet8us& a)
+{
+  uint16x4_t sum = vadd_u16(vget_low_u16(a), vget_high_u16(a));
+  sum = vpadd_u16(sum, sum);
+  sum = vpadd_u16(sum, sum);
+  return vget_lane_u16(sum, 0);
+}
+template<> EIGEN_STRONG_INLINE int32_t predux<Packet2i>(const Packet2i& a) { return vget_lane_s32(vpadd_s32(a,a), 0); }
 template<> EIGEN_STRONG_INLINE int32_t predux<Packet4i>(const Packet4i& a)
 {
-  int32x2_t a_lo, a_hi, sum;
-
-  a_lo = vget_low_s32(a);
-  a_hi = vget_high_s32(a);
-  sum = vpadd_s32(a_lo, a_hi);
-  sum = vpadd_s32(sum, sum);
-  return vget_lane_s32(sum, 0);
+  const int32x2_t sum = vadd_s32(vget_low_s32(a), vget_high_s32(a));
+  return vget_lane_s32(vpadd_s32(sum, sum), 0);
 }
-
-template<> EIGEN_STRONG_INLINE Packet4i preduxp<Packet4i>(const Packet4i* vecs)
+template<> EIGEN_STRONG_INLINE uint32_t predux<Packet2ui>(const Packet2ui& a) { return vget_lane_u32(vpadd_u32(a,a), 0); }
+template<> EIGEN_STRONG_INLINE uint32_t predux<Packet4ui>(const Packet4ui& a)
 {
-  int32x4x2_t vtrn1, vtrn2, res1, res2;
-  Packet4i sum1, sum2, sum;
-
-  // NEON zip performs interleaving of the supplied vectors.
-  // We perform two interleaves in a row to acquire the transposed vector
-  vtrn1 = vzipq_s32(vecs[0], vecs[2]);
-  vtrn2 = vzipq_s32(vecs[1], vecs[3]);
-  res1 = vzipq_s32(vtrn1.val[0], vtrn2.val[0]);
-  res2 = vzipq_s32(vtrn1.val[1], vtrn2.val[1]);
-
-  // Do the addition of the resulting vectors
-  sum1 = vaddq_s32(res1.val[0], res1.val[1]);
-  sum2 = vaddq_s32(res2.val[0], res2.val[1]);
-  sum = vaddq_s32(sum1, sum2);
-
-  return sum;
+  const uint32x2_t sum = vadd_u32(vget_low_u32(a), vget_high_u32(a));
+  return vget_lane_u32(vpadd_u32(sum, sum), 0);
 }
+template<> EIGEN_STRONG_INLINE int64_t predux<Packet2l>(const Packet2l& a)
+{ return vgetq_lane_s64(a, 0) + vgetq_lane_s64(a, 1); }
+template<> EIGEN_STRONG_INLINE uint64_t predux<Packet2ul>(const Packet2ul& a)
+{ return vgetq_lane_u64(a, 0) + vgetq_lane_u64(a, 1); }
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4c predux_half_dowto4(const Packet8c& a)
+{
+  return vget_lane_s32(vreinterpret_s32_s8(vadd_s8(a,
+      vreinterpret_s8_s32(vrev64_s32(vreinterpret_s32_s8(a))))), 0);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c predux_half_dowto4(const Packet16c& a)
+{ return vadd_s8(vget_high_s8(a), vget_low_s8(a)); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4uc predux_half_dowto4(const Packet8uc& a)
+{
+  return vget_lane_u32(vreinterpret_u32_u8(vadd_u8(a,
+      vreinterpret_u8_u32(vrev64_u32(vreinterpret_u32_u8(a))))), 0);
+}
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc predux_half_dowto4(const Packet16uc& a)
+{ return vadd_u8(vget_high_u8(a), vget_low_u8(a)); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s predux_half_dowto4(const Packet8s& a)
+{ return vadd_s16(vget_high_s16(a), vget_low_s16(a)); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us predux_half_dowto4(const Packet8us& a)
+{ return vadd_u16(vget_high_u16(a), vget_low_u16(a)); }
 
 // Other reduction functions:
 // mul
+template<> EIGEN_STRONG_INLINE float predux_mul<Packet2f>(const Packet2f& a)
+{ return vget_lane_f32(a, 0) * vget_lane_f32(a, 1); }
 template<> EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a)
+{ return predux_mul(vmul_f32(vget_low_f32(a), vget_high_f32(a))); }
+template<> EIGEN_STRONG_INLINE int8_t predux_mul<Packet4c>(const Packet4c& a)
 {
-  float32x2_t a_lo, a_hi, prod;
-
-  // Get a_lo = |a1|a2| and a_hi = |a3|a4|
-  a_lo = vget_low_f32(a);
-  a_hi = vget_high_f32(a);
-  // Get the product of a_lo * a_hi -> |a1*a3|a2*a4|
-  prod = vmul_f32(a_lo, a_hi);
-  // Multiply prod with its swapped value |a2*a4|a1*a3|
-  prod = vmul_f32(prod, vrev64_f32(prod));
-
-  return vget_lane_f32(prod, 0);
+  int8x8_t prod = vreinterpret_s8_s32(vdup_n_s32(a));
+  prod = vmul_s8(prod, vrev16_s8(prod));
+  return vget_lane_s8(prod, 0) * vget_lane_s8(prod, 2);
 }
+template<> EIGEN_STRONG_INLINE int8_t predux_mul<Packet8c>(const Packet8c& a)
+{
+  int8x8_t prod = vmul_s8(a, vrev16_s8(a));
+  prod = vmul_s8(prod, vrev32_s8(prod));
+  return vget_lane_s8(prod, 0) * vget_lane_s8(prod, 4);
+}
+template<> EIGEN_STRONG_INLINE int8_t predux_mul<Packet16c>(const Packet16c& a)
+{ return predux_mul(vmul_s8(vget_low_s8(a), vget_high_s8(a))); }
+template<> EIGEN_STRONG_INLINE uint8_t predux_mul<Packet4uc>(const Packet4uc& a)
+{
+  uint8x8_t prod = vreinterpret_u8_u32(vdup_n_u32(a));
+  prod = vmul_u8(prod, vrev16_u8(prod));
+  return vget_lane_u8(prod, 0) * vget_lane_u8(prod, 2);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_mul<Packet8uc>(const Packet8uc& a)
+{
+  uint8x8_t prod = vmul_u8(a, vrev16_u8(a));
+  prod = vmul_u8(prod, vrev32_u8(prod));
+  return vget_lane_u8(prod, 0) * vget_lane_u8(prod, 4);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_mul<Packet16uc>(const Packet16uc& a)
+{ return predux_mul(vmul_u8(vget_low_u8(a), vget_high_u8(a))); }
+template<> EIGEN_STRONG_INLINE int16_t predux_mul<Packet4s>(const Packet4s& a)
+{
+  const int16x4_t prod = vmul_s16(a, vrev32_s16(a));
+  return vget_lane_s16(prod, 0) * vget_lane_s16(prod, 2);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux_mul<Packet8s>(const Packet8s& a)
+{
+  int16x4_t prod;
+
+  // Get the product of a_lo * a_hi -> |a1*a5|a2*a6|a3*a7|a4*a8|
+  prod = vmul_s16(vget_low_s16(a), vget_high_s16(a));
+  // Swap and multiply |a1*a5*a2*a6|a3*a7*a4*a8|
+  prod = vmul_s16(prod, vrev32_s16(prod));
+  // Multiply |a1*a5*a2*a6*a3*a7*a4*a8|
+  return vget_lane_s16(prod, 0) * vget_lane_s16(prod, 2);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux_mul<Packet4us>(const Packet4us& a)
+{
+  const uint16x4_t prod = vmul_u16(a, vrev32_u16(a));
+  return vget_lane_u16(prod, 0) * vget_lane_u16(prod, 2);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux_mul<Packet8us>(const Packet8us& a)
+{
+  uint16x4_t prod;
+
+  // Get the product of a_lo * a_hi -> |a1*a5|a2*a6|a3*a7|a4*a8|
+  prod = vmul_u16(vget_low_u16(a), vget_high_u16(a));
+  // Swap and multiply |a1*a5*a2*a6|a3*a7*a4*a8|
+  prod = vmul_u16(prod, vrev32_u16(prod));
+  // Multiply |a1*a5*a2*a6*a3*a7*a4*a8|
+  return vget_lane_u16(prod, 0) * vget_lane_u16(prod, 2);
+}
+template<> EIGEN_STRONG_INLINE int32_t predux_mul<Packet2i>(const Packet2i& a)
+{ return vget_lane_s32(a, 0) * vget_lane_s32(a, 1); }
 template<> EIGEN_STRONG_INLINE int32_t predux_mul<Packet4i>(const Packet4i& a)
-{
-  int32x2_t a_lo, a_hi, prod;
-
-  // Get a_lo = |a1|a2| and a_hi = |a3|a4|
-  a_lo = vget_low_s32(a);
-  a_hi = vget_high_s32(a);
-  // Get the product of a_lo * a_hi -> |a1*a3|a2*a4|
-  prod = vmul_s32(a_lo, a_hi);
-  // Multiply prod with its swapped value |a2*a4|a1*a3|
-  prod = vmul_s32(prod, vrev64_s32(prod));
-
-  return vget_lane_s32(prod, 0);
-}
+{ return predux_mul(vmul_s32(vget_low_s32(a), vget_high_s32(a))); }
+template<> EIGEN_STRONG_INLINE uint32_t predux_mul<Packet2ui>(const Packet2ui& a)
+{ return vget_lane_u32(a, 0) * vget_lane_u32(a, 1); }
+template<> EIGEN_STRONG_INLINE uint32_t predux_mul<Packet4ui>(const Packet4ui& a)
+{ return predux_mul(vmul_u32(vget_low_u32(a), vget_high_u32(a))); }
+template<> EIGEN_STRONG_INLINE int64_t predux_mul<Packet2l>(const Packet2l& a)
+{ return vgetq_lane_s64(a, 0) * vgetq_lane_s64(a, 1); }
+template<> EIGEN_STRONG_INLINE uint64_t predux_mul<Packet2ul>(const Packet2ul& a)
+{ return vgetq_lane_u64(a, 0) * vgetq_lane_u64(a, 1); }
 
 // min
+template<> EIGEN_STRONG_INLINE float predux_min<Packet2f>(const Packet2f& a)
+{ return vget_lane_f32(vpmin_f32(a,a), 0); }
 template<> EIGEN_STRONG_INLINE float predux_min<Packet4f>(const Packet4f& a)
 {
-  float32x2_t a_lo, a_hi, min;
-
-  a_lo = vget_low_f32(a);
-  a_hi = vget_high_f32(a);
-  min = vpmin_f32(a_lo, a_hi);
-  min = vpmin_f32(min, min);
-
-  return vget_lane_f32(min, 0);
+  const float32x2_t min = vmin_f32(vget_low_f32(a), vget_high_f32(a));
+  return vget_lane_f32(vpmin_f32(min, min), 0);
 }
-
+template<> EIGEN_STRONG_INLINE int8_t predux_min<Packet4c>(const Packet4c& a)
+{
+  const int8x8_t a_dup = vreinterpret_s8_s32(vdup_n_s32(a));
+  int8x8_t min = vpmin_s8(a_dup, a_dup);
+  min = vpmin_s8(min, min);
+  return vget_lane_s8(min, 0);
+}
+template<> EIGEN_STRONG_INLINE int8_t predux_min<Packet8c>(const Packet8c& a)
+{
+  int8x8_t min = vpmin_s8(a,a);
+  min = vpmin_s8(min, min);
+  min = vpmin_s8(min, min);
+  return vget_lane_s8(min, 0);
+}
+template<> EIGEN_STRONG_INLINE int8_t predux_min<Packet16c>(const Packet16c& a)
+{
+  int8x8_t min = vmin_s8(vget_low_s8(a), vget_high_s8(a));
+  min = vpmin_s8(min, min);
+  min = vpmin_s8(min, min);
+  min = vpmin_s8(min, min);
+  return vget_lane_s8(min, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_min<Packet4uc>(const Packet4uc& a)
+{
+  const uint8x8_t a_dup = vreinterpret_u8_u32(vdup_n_u32(a));
+  uint8x8_t min = vpmin_u8(a_dup, a_dup);
+  min = vpmin_u8(min, min);
+  return vget_lane_u8(min, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_min<Packet8uc>(const Packet8uc& a)
+{
+  uint8x8_t min = vpmin_u8(a,a);
+  min = vpmin_u8(min, min);
+  min = vpmin_u8(min, min);
+  return vget_lane_u8(min, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_min<Packet16uc>(const Packet16uc& a)
+{
+  uint8x8_t min = vmin_u8(vget_low_u8(a), vget_high_u8(a));
+  min = vpmin_u8(min, min);
+  min = vpmin_u8(min, min);
+  min = vpmin_u8(min, min);
+  return vget_lane_u8(min, 0);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux_min<Packet4s>(const Packet4s& a)
+{
+  const int16x4_t min = vpmin_s16(a,a);
+  return vget_lane_s16(vpmin_s16(min, min), 0);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux_min<Packet8s>(const Packet8s& a)
+{
+  int16x4_t min = vmin_s16(vget_low_s16(a), vget_high_s16(a));
+  min = vpmin_s16(min, min);
+  min = vpmin_s16(min, min);
+  return vget_lane_s16(min, 0);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux_min<Packet4us>(const Packet4us& a)
+{
+  const uint16x4_t min = vpmin_u16(a,a);
+  return vget_lane_u16(vpmin_u16(min, min), 0);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux_min<Packet8us>(const Packet8us& a)
+{
+  uint16x4_t min = vmin_u16(vget_low_u16(a), vget_high_u16(a));
+  min = vpmin_u16(min, min);
+  min = vpmin_u16(min, min);
+  return vget_lane_u16(min, 0);
+}
+template<> EIGEN_STRONG_INLINE int32_t predux_min<Packet2i>(const Packet2i& a)
+{ return vget_lane_s32(vpmin_s32(a,a), 0); }
 template<> EIGEN_STRONG_INLINE int32_t predux_min<Packet4i>(const Packet4i& a)
 {
-  int32x2_t a_lo, a_hi, min;
-
-  a_lo = vget_low_s32(a);
-  a_hi = vget_high_s32(a);
-  min = vpmin_s32(a_lo, a_hi);
-  min = vpmin_s32(min, min);
-  
-  return vget_lane_s32(min, 0);
+  const int32x2_t min = vmin_s32(vget_low_s32(a), vget_high_s32(a));
+  return vget_lane_s32(vpmin_s32(min, min), 0);
 }
+template<> EIGEN_STRONG_INLINE uint32_t predux_min<Packet2ui>(const Packet2ui& a)
+{ return vget_lane_u32(vpmin_u32(a,a), 0); }
+template<> EIGEN_STRONG_INLINE uint32_t predux_min<Packet4ui>(const Packet4ui& a)
+{
+  const uint32x2_t min = vmin_u32(vget_low_u32(a), vget_high_u32(a));
+  return vget_lane_u32(vpmin_u32(min, min), 0);
+}
+template<> EIGEN_STRONG_INLINE int64_t predux_min<Packet2l>(const Packet2l& a)
+{ return (std::min)(vgetq_lane_s64(a, 0), vgetq_lane_s64(a, 1)); }
+template<> EIGEN_STRONG_INLINE uint64_t predux_min<Packet2ul>(const Packet2ul& a)
+{ return (std::min)(vgetq_lane_u64(a, 0), vgetq_lane_u64(a, 1)); }
 
 // max
+template<> EIGEN_STRONG_INLINE float predux_max<Packet2f>(const Packet2f& a)
+{ return vget_lane_f32(vpmax_f32(a,a), 0); }
 template<> EIGEN_STRONG_INLINE float predux_max<Packet4f>(const Packet4f& a)
 {
-  float32x2_t a_lo, a_hi, max;
-
-  a_lo = vget_low_f32(a);
-  a_hi = vget_high_f32(a);
-  max = vpmax_f32(a_lo, a_hi);
-  max = vpmax_f32(max, max);
-
-  return vget_lane_f32(max, 0);
+  const float32x2_t max = vmax_f32(vget_low_f32(a), vget_high_f32(a));
+  return vget_lane_f32(vpmax_f32(max, max), 0);
 }
-
+template<> EIGEN_STRONG_INLINE int8_t predux_max<Packet4c>(const Packet4c& a)
+{
+  const int8x8_t a_dup = vreinterpret_s8_s32(vdup_n_s32(a));
+  int8x8_t max = vpmax_s8(a_dup, a_dup);
+  max = vpmax_s8(max, max);
+  return vget_lane_s8(max, 0);
+}
+template<> EIGEN_STRONG_INLINE int8_t predux_max<Packet8c>(const Packet8c& a)
+{
+  int8x8_t max = vpmax_s8(a,a);
+  max = vpmax_s8(max, max);
+  max = vpmax_s8(max, max);
+  return vget_lane_s8(max, 0);
+}
+template<> EIGEN_STRONG_INLINE int8_t predux_max<Packet16c>(const Packet16c& a)
+{
+  int8x8_t max = vmax_s8(vget_low_s8(a), vget_high_s8(a));
+  max = vpmax_s8(max, max);
+  max = vpmax_s8(max, max);
+  max = vpmax_s8(max, max);
+  return vget_lane_s8(max, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_max<Packet4uc>(const Packet4uc& a)
+{
+  const uint8x8_t a_dup = vreinterpret_u8_u32(vdup_n_u32(a));
+  uint8x8_t max = vpmax_u8(a_dup, a_dup);
+  max = vpmax_u8(max, max);
+  return vget_lane_u8(max, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_max<Packet8uc>(const Packet8uc& a)
+{
+  uint8x8_t max = vpmax_u8(a,a);
+  max = vpmax_u8(max, max);
+  max = vpmax_u8(max, max);
+  return vget_lane_u8(max, 0);
+}
+template<> EIGEN_STRONG_INLINE uint8_t predux_max<Packet16uc>(const Packet16uc& a)
+{
+  uint8x8_t max = vmax_u8(vget_low_u8(a), vget_high_u8(a));
+  max = vpmax_u8(max, max);
+  max = vpmax_u8(max, max);
+  max = vpmax_u8(max, max);
+  return vget_lane_u8(max, 0);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux_max<Packet4s>(const Packet4s& a)
+{
+  const int16x4_t max = vpmax_s16(a,a);
+  return vget_lane_s16(vpmax_s16(max, max), 0);
+}
+template<> EIGEN_STRONG_INLINE int16_t predux_max<Packet8s>(const Packet8s& a)
+{
+  int16x4_t max = vmax_s16(vget_low_s16(a), vget_high_s16(a));
+  max = vpmax_s16(max, max);
+  max = vpmax_s16(max, max);
+  return vget_lane_s16(max, 0);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux_max<Packet4us>(const Packet4us& a)
+{
+  const uint16x4_t max = vpmax_u16(a,a);
+  return vget_lane_u16(vpmax_u16(max, max), 0);
+}
+template<> EIGEN_STRONG_INLINE uint16_t predux_max<Packet8us>(const Packet8us& a)
+{
+  uint16x4_t max = vmax_u16(vget_low_u16(a), vget_high_u16(a));
+  max = vpmax_u16(max, max);
+  max = vpmax_u16(max, max);
+  return vget_lane_u16(max, 0);
+}
+template<> EIGEN_STRONG_INLINE int32_t predux_max<Packet2i>(const Packet2i& a)
+{ return vget_lane_s32(vpmax_s32(a,a), 0); }
 template<> EIGEN_STRONG_INLINE int32_t predux_max<Packet4i>(const Packet4i& a)
 {
-  int32x2_t a_lo, a_hi, max;
+  const int32x2_t max = vmax_s32(vget_low_s32(a), vget_high_s32(a));
+  return vget_lane_s32(vpmax_s32(max, max), 0);
+}
+template<> EIGEN_STRONG_INLINE uint32_t predux_max<Packet2ui>(const Packet2ui& a)
+{ return vget_lane_u32(vpmax_u32(a,a), 0); }
+template<> EIGEN_STRONG_INLINE uint32_t predux_max<Packet4ui>(const Packet4ui& a)
+{
+  const uint32x2_t max = vmax_u32(vget_low_u32(a), vget_high_u32(a));
+  return vget_lane_u32(vpmax_u32(max, max), 0);
+}
+template<> EIGEN_STRONG_INLINE int64_t predux_max<Packet2l>(const Packet2l& a)
+{ return (std::max)(vgetq_lane_s64(a, 0), vgetq_lane_s64(a, 1)); }
+template<> EIGEN_STRONG_INLINE uint64_t predux_max<Packet2ul>(const Packet2ul& a)
+{ return (std::max)(vgetq_lane_u64(a, 0), vgetq_lane_u64(a, 1)); }
 
-  a_lo = vget_low_s32(a);
-  a_hi = vget_high_s32(a);
-  max = vpmax_s32(a_lo, a_hi);
-  max = vpmax_s32(max, max);
-
-  return vget_lane_s32(max, 0);
+template<> EIGEN_STRONG_INLINE bool predux_any(const Packet4f& x)
+{
+  uint32x2_t tmp = vorr_u32(vget_low_u32( vreinterpretq_u32_f32(x)),
+                            vget_high_u32(vreinterpretq_u32_f32(x)));
+  return vget_lane_u32(vpmax_u32(tmp, tmp), 0);
 }
 
-// this PALIGN_NEON business is to work around a bug in LLVM Clang 3.0 causing incorrect compilation errors,
-// see bug 347 and this LLVM bug: http://llvm.org/bugs/show_bug.cgi?id=11074
-#define PALIGN_NEON(Offset,Type,Command) \
-template<>\
-struct palign_impl<Offset,Type>\
-{\
-    EIGEN_STRONG_INLINE static void run(Type& first, const Type& second)\
-    {\
-        if (Offset!=0)\
-            first = Command(first, second, Offset);\
-    }\
-};\
+// Helpers for ptranspose.
+namespace detail {
+  
+template<typename Packet>
+void zip_in_place(Packet& p1, Packet& p2);
 
-PALIGN_NEON(0,Packet4f,vextq_f32)
-PALIGN_NEON(1,Packet4f,vextq_f32)
-PALIGN_NEON(2,Packet4f,vextq_f32)
-PALIGN_NEON(3,Packet4f,vextq_f32)
-PALIGN_NEON(0,Packet4i,vextq_s32)
-PALIGN_NEON(1,Packet4i,vextq_s32)
-PALIGN_NEON(2,Packet4i,vextq_s32)
-PALIGN_NEON(3,Packet4i,vextq_s32)
-
-#undef PALIGN_NEON
-
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet4f,4>& kernel) {
-  float32x4x2_t tmp1 = vzipq_f32(kernel.packet[0], kernel.packet[1]);
-  float32x4x2_t tmp2 = vzipq_f32(kernel.packet[2], kernel.packet[3]);
-
-  kernel.packet[0] = vcombine_f32(vget_low_f32(tmp1.val[0]), vget_low_f32(tmp2.val[0]));
-  kernel.packet[1] = vcombine_f32(vget_high_f32(tmp1.val[0]), vget_high_f32(tmp2.val[0]));
-  kernel.packet[2] = vcombine_f32(vget_low_f32(tmp1.val[1]), vget_low_f32(tmp2.val[1]));
-  kernel.packet[3] = vcombine_f32(vget_high_f32(tmp1.val[1]), vget_high_f32(tmp2.val[1]));
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet2f>(Packet2f& p1, Packet2f& p2) {
+  const float32x2x2_t tmp = vzip_f32(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
 }
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet4i,4>& kernel) {
-  int32x4x2_t tmp1 = vzipq_s32(kernel.packet[0], kernel.packet[1]);
-  int32x4x2_t tmp2 = vzipq_s32(kernel.packet[2], kernel.packet[3]);
-  kernel.packet[0] = vcombine_s32(vget_low_s32(tmp1.val[0]), vget_low_s32(tmp2.val[0]));
-  kernel.packet[1] = vcombine_s32(vget_high_s32(tmp1.val[0]), vget_high_s32(tmp2.val[0]));
-  kernel.packet[2] = vcombine_s32(vget_low_s32(tmp1.val[1]), vget_low_s32(tmp2.val[1]));
-  kernel.packet[3] = vcombine_s32(vget_high_s32(tmp1.val[1]), vget_high_s32(tmp2.val[1]));
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet4f>(Packet4f& p1, Packet4f& p2) {
+  const float32x4x2_t tmp = vzipq_f32(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet8c>(Packet8c& p1, Packet8c& p2) {
+  const int8x8x2_t tmp = vzip_s8(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet16c>(Packet16c& p1, Packet16c& p2) {
+  const int8x16x2_t tmp = vzipq_s8(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet8uc>(Packet8uc& p1, Packet8uc& p2) {
+  const uint8x8x2_t tmp = vzip_u8(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet16uc>(Packet16uc& p1, Packet16uc& p2) {
+  const uint8x16x2_t tmp = vzipq_u8(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet2i>(Packet2i& p1, Packet2i& p2) {
+  const int32x2x2_t tmp = vzip_s32(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet4i>(Packet4i& p1, Packet4i& p2) {
+  const int32x4x2_t tmp = vzipq_s32(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet2ui>(Packet2ui& p1, Packet2ui& p2) {
+  const uint32x2x2_t tmp = vzip_u32(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet4ui>(Packet4ui& p1, Packet4ui& p2) {
+  const uint32x4x2_t tmp = vzipq_u32(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet4s>(Packet4s& p1, Packet4s& p2) {
+  const int16x4x2_t tmp = vzip_s16(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet8s>(Packet8s& p1, Packet8s& p2) {
+  const int16x8x2_t tmp = vzipq_s16(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet4us>(Packet4us& p1, Packet4us& p2) {
+  const uint16x4x2_t tmp = vzip_u16(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet8us>(Packet8us& p1, Packet8us& p2) {
+  const uint16x8x2_t tmp = vzipq_u16(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+
+template<typename Packet>
+EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 2>& kernel) {
+  zip_in_place(kernel.packet[0], kernel.packet[1]);
+}
+
+template<typename Packet>
+EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 4>& kernel) {
+  zip_in_place(kernel.packet[0], kernel.packet[2]);
+  zip_in_place(kernel.packet[1], kernel.packet[3]);
+  zip_in_place(kernel.packet[0], kernel.packet[1]);
+  zip_in_place(kernel.packet[2], kernel.packet[3]);
+}
+
+template<typename Packet>
+EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 8>& kernel) {
+  zip_in_place(kernel.packet[0], kernel.packet[4]);
+  zip_in_place(kernel.packet[1], kernel.packet[5]);
+  zip_in_place(kernel.packet[2], kernel.packet[6]);
+  zip_in_place(kernel.packet[3], kernel.packet[7]);
+
+  zip_in_place(kernel.packet[0], kernel.packet[2]);
+  zip_in_place(kernel.packet[1], kernel.packet[3]);
+  zip_in_place(kernel.packet[4], kernel.packet[6]);
+  zip_in_place(kernel.packet[5], kernel.packet[7]);
+  
+  zip_in_place(kernel.packet[0], kernel.packet[1]);
+  zip_in_place(kernel.packet[2], kernel.packet[3]);
+  zip_in_place(kernel.packet[4], kernel.packet[5]);
+  zip_in_place(kernel.packet[6], kernel.packet[7]);
+}
+
+template<typename Packet>
+EIGEN_ALWAYS_INLINE void ptranspose_impl(PacketBlock<Packet, 16>& kernel) {
+  EIGEN_UNROLL_LOOP
+  for (int i=0; i<4; ++i) {
+    const int m = (1 << i);
+    EIGEN_UNROLL_LOOP
+    for (int j=0; j<m; ++j) {
+      const int n = (1 << (3-i));
+      EIGEN_UNROLL_LOOP
+      for (int k=0; k<n; ++k) {
+        const int idx = 2*j*n+k;
+        zip_in_place(kernel.packet[idx], kernel.packet[idx + n]);
+      }
+    }
+  }
+}
+
+} // namespace detail
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2f, 2>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4f, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4c, 4>& kernel)
+{
+  const int8x8_t a = vreinterpret_s8_s32(vset_lane_s32(kernel.packet[2], vdup_n_s32(kernel.packet[0]), 1));
+  const int8x8_t b = vreinterpret_s8_s32(vset_lane_s32(kernel.packet[3], vdup_n_s32(kernel.packet[1]), 1));
+
+  const int8x8x2_t zip8 = vzip_s8(a,b);
+  const int16x4x2_t zip16 = vzip_s16(vreinterpret_s16_s8(zip8.val[0]), vreinterpret_s16_s8(zip8.val[1]));
+
+  kernel.packet[0] = vget_lane_s32(vreinterpret_s32_s16(zip16.val[0]), 0);
+  kernel.packet[1] = vget_lane_s32(vreinterpret_s32_s16(zip16.val[0]), 1);
+  kernel.packet[2] = vget_lane_s32(vreinterpret_s32_s16(zip16.val[1]), 0);
+  kernel.packet[3] = vget_lane_s32(vreinterpret_s32_s16(zip16.val[1]), 1);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8c, 8>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8c, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet16c, 16>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet16c, 8>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet16c, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4uc, 4>& kernel)
+{
+  const uint8x8_t a = vreinterpret_u8_u32(vset_lane_u32(kernel.packet[2], vdup_n_u32(kernel.packet[0]), 1));
+  const uint8x8_t b = vreinterpret_u8_u32(vset_lane_u32(kernel.packet[3], vdup_n_u32(kernel.packet[1]), 1));
+
+  const uint8x8x2_t zip8 = vzip_u8(a,b);
+  const uint16x4x2_t zip16 = vzip_u16(vreinterpret_u16_u8(zip8.val[0]), vreinterpret_u16_u8(zip8.val[1]));
+
+  kernel.packet[0] = vget_lane_u32(vreinterpret_u32_u16(zip16.val[0]), 0);
+  kernel.packet[1] = vget_lane_u32(vreinterpret_u32_u16(zip16.val[0]), 1);
+  kernel.packet[2] = vget_lane_u32(vreinterpret_u32_u16(zip16.val[1]), 0);
+  kernel.packet[3] = vget_lane_u32(vreinterpret_u32_u16(zip16.val[1]), 1);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8uc, 8>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8uc, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet16uc, 16>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet16uc, 8>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet16uc, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4s, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8s, 8>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8s, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4us, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8us, 8>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8us, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2i, 2>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4i, 4>& kernel) {
+    detail::ptranspose_impl(kernel);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet2ui, 2>& kernel) {
+  detail::zip_in_place(kernel.packet[0], kernel.packet[1]);
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4ui, 4>& kernel) {
+  detail::ptranspose_impl(kernel);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet2l, 2>& kernel)
+{
+#if EIGEN_ARCH_ARM64
+  const int64x2_t tmp1 = vzip1q_s64(kernel.packet[0], kernel.packet[1]);
+  kernel.packet[1] = vzip2q_s64(kernel.packet[0], kernel.packet[1]);
+  kernel.packet[0] = tmp1;
+#else
+  const int64x1_t tmp[2][2] = {
+    { vget_low_s64(kernel.packet[0]), vget_high_s64(kernel.packet[0]) },
+    { vget_low_s64(kernel.packet[1]), vget_high_s64(kernel.packet[1]) }
+  };
+
+  kernel.packet[0] = vcombine_s64(tmp[0][0], tmp[1][0]);
+  kernel.packet[1] = vcombine_s64(tmp[0][1], tmp[1][1]);
+#endif
+}
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet2ul, 2>& kernel)
+{
+#if EIGEN_ARCH_ARM64
+  const uint64x2_t tmp1 = vzip1q_u64(kernel.packet[0], kernel.packet[1]);
+  kernel.packet[1] = vzip2q_u64(kernel.packet[0], kernel.packet[1]);
+  kernel.packet[0] = tmp1;
+#else
+  const uint64x1_t tmp[2][2] = {
+    { vget_low_u64(kernel.packet[0]), vget_high_u64(kernel.packet[0]) },
+    { vget_low_u64(kernel.packet[1]), vget_high_u64(kernel.packet[1]) }
+  };
+
+  kernel.packet[0] = vcombine_u64(tmp[0][0], tmp[1][0]);
+  kernel.packet[1] = vcombine_u64(tmp[0][1], tmp[1][1]);
+#endif
+}
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2f pselect( const Packet2f& mask, const Packet2f& a, const Packet2f& b)
+{ return vbsl_f32(vreinterpret_u32_f32(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4f pselect(const Packet4f& mask, const Packet4f& a, const Packet4f& b)
+{ return vbslq_f32(vreinterpretq_u32_f32(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8c pselect(const Packet8c& mask, const Packet8c& a, const Packet8c& b)
+{ return vbsl_s8(vreinterpret_u8_s8(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16c pselect(const Packet16c& mask, const Packet16c& a, const Packet16c& b)
+{ return vbslq_s8(vreinterpretq_u8_s8(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8uc pselect(const Packet8uc& mask, const Packet8uc& a, const Packet8uc& b)
+{ return vbsl_u8(mask, a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet16uc pselect(const Packet16uc& mask, const Packet16uc& a, const Packet16uc& b)
+{ return vbslq_u8(mask, a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4s pselect(const Packet4s& mask, const Packet4s& a, const Packet4s& b)
+{ return vbsl_s16(vreinterpret_u16_s16(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8s pselect(const Packet8s& mask, const Packet8s& a, const Packet8s& b)
+{ return vbslq_s16(vreinterpretq_u16_s16(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4us pselect(const Packet4us& mask, const Packet4us& a, const Packet4us& b)
+{ return vbsl_u16(mask, a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8us pselect(const Packet8us& mask, const Packet8us& a, const Packet8us& b)
+{ return vbslq_u16(mask, a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2i pselect(const Packet2i& mask, const Packet2i& a, const Packet2i& b)
+{ return vbsl_s32(vreinterpret_u32_s32(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4i pselect(const Packet4i& mask, const Packet4i& a, const Packet4i& b)
+{ return vbslq_s32(vreinterpretq_u32_s32(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ui pselect(const Packet2ui& mask, const Packet2ui& a, const Packet2ui& b)
+{ return vbsl_u32(mask, a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4ui pselect(const Packet4ui& mask, const Packet4ui& a, const Packet4ui& b)
+{ return vbslq_u32(mask, a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2l pselect(const Packet2l& mask, const Packet2l& a, const Packet2l& b)
+{ return vbslq_s64(vreinterpretq_u64_s64(mask), a, b); }
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2ul pselect(const Packet2ul& mask, const Packet2ul& a, const Packet2ul& b)
+{ return vbslq_u64(mask, a, b); }
+
+// Use armv8 rounding intinsics if available.
+#if EIGEN_ARCH_ARMV8
+template<> EIGEN_STRONG_INLINE Packet2f print<Packet2f>(const Packet2f& a)
+{ return vrndn_f32(a); }
+
+template<> EIGEN_STRONG_INLINE Packet4f print<Packet4f>(const Packet4f& a)
+{ return vrndnq_f32(a); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pfloor<Packet2f>(const Packet2f& a)
+{ return vrndm_f32(a); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a)
+{ return vrndmq_f32(a); }
+
+template<> EIGEN_STRONG_INLINE Packet2f pceil<Packet2f>(const Packet2f& a)
+{ return vrndp_f32(a); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a)
+{ return vrndpq_f32(a); }
+
+#else
+
+template<> EIGEN_STRONG_INLINE Packet4f print(const Packet4f& a) {
+  // Adds and subtracts signum(a) * 2^23 to force rounding.
+  const Packet4f limit = pset1<Packet4f>(static_cast<float>(1<<23));
+  const Packet4f abs_a = pabs(a);
+  Packet4f r = padd(abs_a, limit);
+  // Don't compile-away addition and subtraction.
+  EIGEN_OPTIMIZATION_BARRIER(r);
+  r = psub(r, limit);
+  // If greater than limit, simply return a.  Otherwise, account for sign.
+  r = pselect(pcmp_lt(abs_a, limit),
+              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  return r;
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f print(const Packet2f& a) {
+  // Adds and subtracts signum(a) * 2^23 to force rounding.
+  const Packet2f limit = pset1<Packet2f>(static_cast<float>(1<<23));
+  const Packet2f abs_a = pabs(a);
+  Packet2f r = padd(abs_a, limit);
+  // Don't compile-away addition and subtraction.
+  EIGEN_OPTIMIZATION_BARRIER(r);
+  r = psub(r, limit);
+  // If greater than limit, simply return a.  Otherwise, account for sign.
+  r = pselect(pcmp_lt(abs_a, limit),
+              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  return r;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a)
+{
+  const Packet4f cst_1 = pset1<Packet4f>(1.0f);
+  Packet4f tmp  = print<Packet4f>(a);
+  // If greater, subtract one.
+  Packet4f mask = pcmp_lt(a, tmp);
+  mask = pand(mask, cst_1);
+  return psub(tmp, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pfloor<Packet2f>(const Packet2f& a)
+{
+  const Packet2f cst_1 = pset1<Packet2f>(1.0f);
+  Packet2f tmp  = print<Packet2f>(a);
+  // If greater, subtract one.
+  Packet2f mask = pcmp_lt(a, tmp);
+  mask = pand(mask, cst_1);
+  return psub(tmp, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a)
+{
+  const Packet4f cst_1 = pset1<Packet4f>(1.0f);
+  Packet4f tmp  = print<Packet4f>(a);
+  // If smaller, add one.
+  Packet4f mask = pcmp_lt(tmp, a);
+  mask = pand(mask, cst_1);
+  return padd(tmp, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f pceil<Packet2f>(const Packet2f& a)
+{
+  const Packet2f cst_1 = pset1<Packet2f>(1.0);
+  Packet2f tmp  = print<Packet2f>(a);
+  // If smaller, add one.
+  Packet2f mask = pcmp_lt(tmp, a);
+  mask = pand(mask, cst_1);
+  return padd(tmp, mask);
+}
+
+#endif
+
+/**
+ * Computes the integer square root
+ * @remarks The calculation is performed using an algorithm which iterates through each binary digit of the result
+ *   and tests whether setting that digit to 1 would cause the square of the value to be greater than the argument
+ *   value. The algorithm is described in detail here: http://ww1.microchip.com/downloads/en/AppNotes/91040a.pdf .
+ */
+template<> EIGEN_STRONG_INLINE Packet4uc psqrt(const Packet4uc& a) {
+  uint8x8_t x = vreinterpret_u8_u32(vdup_n_u32(a));
+  uint8x8_t res = vdup_n_u8(0);
+  uint8x8_t add = vdup_n_u8(0x8);
+  for (int i = 0; i < 4; i++)
+  {
+    const uint8x8_t temp = vorr_u8(res, add);
+    res = vbsl_u8(vcge_u8(x, vmul_u8(temp, temp)), temp, res);
+    add = vshr_n_u8(add, 1);
+  }
+  return vget_lane_u32(vreinterpret_u32_u8(res), 0);
+}
+/// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
+template<> EIGEN_STRONG_INLINE Packet8uc psqrt(const Packet8uc& a) {
+  uint8x8_t res = vdup_n_u8(0);
+  uint8x8_t add = vdup_n_u8(0x8);
+  for (int i = 0; i < 4; i++)
+  {
+    const uint8x8_t temp = vorr_u8(res, add);
+    res = vbsl_u8(vcge_u8(a, vmul_u8(temp, temp)), temp, res);
+    add = vshr_n_u8(add, 1);
+  }
+  return res;
+}
+/// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
+template<> EIGEN_STRONG_INLINE Packet16uc psqrt(const Packet16uc& a) {
+  uint8x16_t res = vdupq_n_u8(0);
+  uint8x16_t add = vdupq_n_u8(0x8);
+  for (int i = 0; i < 4; i++)
+  {
+    const uint8x16_t temp = vorrq_u8(res, add);
+    res = vbslq_u8(vcgeq_u8(a, vmulq_u8(temp, temp)), temp, res);
+    add = vshrq_n_u8(add, 1);
+  }
+  return res;
+}
+/// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
+template<> EIGEN_STRONG_INLINE Packet4us psqrt(const Packet4us& a) {
+  uint16x4_t res = vdup_n_u16(0);
+  uint16x4_t add = vdup_n_u16(0x80);
+  for (int i = 0; i < 8; i++)
+  {
+    const uint16x4_t temp = vorr_u16(res, add);
+    res = vbsl_u16(vcge_u16(a, vmul_u16(temp, temp)), temp, res);
+    add = vshr_n_u16(add, 1);
+  }
+  return res;
+}
+/// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
+template<> EIGEN_STRONG_INLINE Packet8us psqrt(const Packet8us& a) {
+  uint16x8_t res = vdupq_n_u16(0);
+  uint16x8_t add = vdupq_n_u16(0x80);
+  for (int i = 0; i < 8; i++)
+  {
+    const uint16x8_t temp = vorrq_u16(res, add);
+    res = vbslq_u16(vcgeq_u16(a, vmulq_u16(temp, temp)), temp, res);
+    add = vshrq_n_u16(add, 1);
+  }
+  return res;
+}
+/// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
+template<> EIGEN_STRONG_INLINE Packet2ui psqrt(const Packet2ui& a) {
+  uint32x2_t res = vdup_n_u32(0);
+  uint32x2_t add = vdup_n_u32(0x8000);
+  for (int i = 0; i < 16; i++)
+  {
+    const uint32x2_t temp = vorr_u32(res, add);
+    res = vbsl_u32(vcge_u32(a, vmul_u32(temp, temp)), temp, res);
+    add = vshr_n_u32(add, 1);
+  }
+  return res;
+}
+/// @copydoc Eigen::internal::psqrt(const Packet4uc& a)
+template<> EIGEN_STRONG_INLINE Packet4ui psqrt(const Packet4ui& a) {
+  uint32x4_t res = vdupq_n_u32(0);
+  uint32x4_t add = vdupq_n_u32(0x8000);
+  for (int i = 0; i < 16; i++)
+  {
+    const uint32x4_t temp = vorrq_u32(res, add);
+    res = vbslq_u32(vcgeq_u32(a, vmulq_u32(temp, temp)), temp, res);
+    add = vshrq_n_u32(add, 1);
+  }
+  return res;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f prsqrt(const Packet4f& a) {
+  // Compute approximate reciprocal sqrt.
+  Packet4f x = vrsqrteq_f32(a);
+  // Do Newton iterations for 1/sqrt(x).
+  x = vmulq_f32(vrsqrtsq_f32(vmulq_f32(a, x), x), x);
+  x = vmulq_f32(vrsqrtsq_f32(vmulq_f32(a, x), x), x);
+  const Packet4f infinity = pset1<Packet4f>(NumTraits<float>::infinity());
+  return pselect(pcmp_eq(a, pzero(a)), infinity, x);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2f prsqrt(const Packet2f& a) {
+  // Compute approximate reciprocal sqrt.
+  Packet2f x = vrsqrte_f32(a);
+  // Do Newton iterations for 1/sqrt(x).
+  x = vmul_f32(vrsqrts_f32(vmul_f32(a, x), x), x);
+  x = vmul_f32(vrsqrts_f32(vmul_f32(a, x), x), x);
+  const Packet2f infinity = pset1<Packet2f>(NumTraits<float>::infinity());
+  return pselect(pcmp_eq(a, pzero(a)), infinity, x);
+}
+
+// Unfortunately vsqrt_f32 is only available for A64.
+#if EIGEN_ARCH_ARM64
+template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& _x){return vsqrtq_f32(_x);}
+template<> EIGEN_STRONG_INLINE Packet2f psqrt(const Packet2f& _x){return vsqrt_f32(_x); }
+#else
+template<> EIGEN_STRONG_INLINE Packet4f psqrt(const Packet4f& a) {
+  const Packet4f infinity = pset1<Packet4f>(NumTraits<float>::infinity());
+  const Packet4f is_zero_or_inf = por(pcmp_eq(a, pzero(a)), pcmp_eq(a, infinity));
+  return pselect(is_zero_or_inf, a, pmul(a, prsqrt(a)));
+}
+template<> EIGEN_STRONG_INLINE Packet2f psqrt(const Packet2f& a) {
+  const Packet2f infinity = pset1<Packet2f>(NumTraits<float>::infinity());
+  const Packet2f is_zero_or_inf = por(pcmp_eq(a, pzero(a)), pcmp_eq(a, infinity));
+  return pselect(is_zero_or_inf, a, pmul(a, prsqrt(a)));
+}
+#endif
+
+//---------- bfloat16 ----------
+// TODO: Add support for native armv8.6-a bfloat16_t
+
+// TODO: Guard if we have native bfloat16 support
+typedef eigen_packet_wrapper<uint16x4_t, 19> Packet4bf;
+
+template<> struct is_arithmetic<Packet4bf> { enum { value = true }; };
+
+template<> struct packet_traits<bfloat16> : default_packet_traits
+{
+  typedef Packet4bf type;
+  typedef Packet4bf half;
+  enum
+  {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 4,
+    HasHalfPacket = 0,
+
+    HasCmp       = 1,
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0,
+    HasDiv       = 1,
+    HasFloor     = 1,
+    HasCeil      = 1,
+    HasRint      = 1,
+
+    HasSin  = EIGEN_FAST_MATH,
+    HasCos  = EIGEN_FAST_MATH,
+    HasLog  = 1,
+    HasExp  = 1,
+    HasSqrt = 0,
+    HasTanh = EIGEN_FAST_MATH,
+    HasErf  = EIGEN_FAST_MATH,
+    HasBessel = 0,  // Issues with accuracy.
+    HasNdtri = 0
+  };
+};
+
+template<> struct unpacket_traits<Packet4bf>
+{
+  typedef bfloat16 type;
+  typedef Packet4bf half;
+  enum
+  {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+
+namespace detail {  
+template<>
+EIGEN_ALWAYS_INLINE void zip_in_place<Packet4bf>(Packet4bf& p1, Packet4bf& p2) {
+  const uint16x4x2_t tmp = vzip_u16(p1, p2);
+  p1 = tmp.val[0];
+  p2 = tmp.val[1];
+}
+} // namespace detail
+
+EIGEN_STRONG_INLINE Packet4bf F32ToBf16(const Packet4f& p)
+{
+  // See the scalar implemention in BFloat16.h for a comprehensible explanation
+  // of this fast rounding algorithm
+  Packet4ui input = reinterpret_cast<Packet4ui>(p);
+
+  // lsb = (input >> 16) & 1
+  Packet4ui lsb =  vandq_u32(vshrq_n_u32(input, 16), vdupq_n_u32(1));
+
+  // rounding_bias = 0x7fff + lsb
+  Packet4ui rounding_bias = vaddq_u32(lsb, vdupq_n_u32(0x7fff));
+
+  // input += rounding_bias
+  input = vaddq_u32(input, rounding_bias);
+
+  // input = input >> 16
+  input = vshrq_n_u32(input, 16);
+
+  // Replace float-nans by bfloat16-nans, that is 0x7fc0
+  const Packet4ui bf16_nan = vdupq_n_u32(0x7fc0);
+  const Packet4ui mask = vceqq_f32(p, p);
+  input = vbslq_u32(mask, input, bf16_nan);
+
+  // output = static_cast<uint16_t>(input)
+  return vmovn_u32(input);
+}
+
+EIGEN_STRONG_INLINE Packet4f Bf16ToF32(const Packet4bf& p)
+{
+  return reinterpret_cast<Packet4f>(vshlq_n_u32(vmovl_u16(p), 16));
+}
+
+EIGEN_STRONG_INLINE Packet4bf F32MaskToBf16Mask(const Packet4f& p) {
+  return vmovn_u32(vreinterpretq_u32_f32(p));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pset1<Packet4bf>(const bfloat16& from) {
+  return pset1<Packet4us>(from.value);
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 pfirst<Packet4bf>(const Packet4bf& from) {
+  return bfloat16_impl::raw_uint16_to_bfloat16(static_cast<uint16_t>(pfirst<Packet4us>(from)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pload<Packet4bf>(const bfloat16* from)
+{
+  return pload<Packet4us>(reinterpret_cast<const uint16_t*>(from));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf ploadu<Packet4bf>(const bfloat16* from)
+{
+  return ploadu<Packet4us>(reinterpret_cast<const uint16_t*>(from));
+}
+
+template<> EIGEN_STRONG_INLINE void pstore<bfloat16>(bfloat16* to, const Packet4bf& from)
+{
+  EIGEN_DEBUG_ALIGNED_STORE vst1_u16(reinterpret_cast<uint16_t*>(to), from);
+}
+
+template<> EIGEN_STRONG_INLINE void pstoreu<bfloat16>(bfloat16* to, const Packet4bf& from)
+{
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_u16(reinterpret_cast<uint16_t*>(to), from);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf ploaddup<Packet4bf>(const bfloat16* from)
+{
+  return ploaddup<Packet4us>(reinterpret_cast<const uint16_t*>(from));
+}
+
+template <> EIGEN_STRONG_INLINE Packet4bf pabs(const Packet4bf& a) {
+  return F32ToBf16(pabs<Packet4f>(Bf16ToF32(a)));
+}
+
+template <> EIGEN_STRONG_INLINE Packet4bf pmin<PropagateNumbers, Packet4bf>(const Packet4bf &a,
+                                                                            const Packet4bf &b)
+{
+  return F32ToBf16(pmin<PropagateNumbers, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+template <> EIGEN_STRONG_INLINE Packet4bf pmin<PropagateNaN, Packet4bf>(const Packet4bf &a,
+                                                                        const Packet4bf &b)
+{
+  return F32ToBf16(pmin<PropagateNaN, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template <> EIGEN_STRONG_INLINE Packet4bf pmin<Packet4bf>(const Packet4bf &a,
+                                                          const Packet4bf &b)
+{
+  return F32ToBf16(pmin<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template <> EIGEN_STRONG_INLINE Packet4bf pmax<PropagateNumbers, Packet4bf>(const Packet4bf &a,
+                                                                            const Packet4bf &b)
+{
+  return F32ToBf16(pmax<PropagateNumbers, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+template <> EIGEN_STRONG_INLINE Packet4bf pmax<PropagateNaN, Packet4bf>(const Packet4bf &a,
+                                                                        const Packet4bf &b)
+{
+  return F32ToBf16(pmax<PropagateNaN, Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template <> EIGEN_STRONG_INLINE Packet4bf pmax<Packet4bf>(const Packet4bf &a,
+                                                          const Packet4bf &b)
+{
+  return F32ToBf16(pmax<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf plset<Packet4bf>(const bfloat16& a)
+{
+  return F32ToBf16(plset<Packet4f>(static_cast<float>(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf por(const Packet4bf& a,const Packet4bf& b) {
+  return por<Packet4us>(a, b);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pxor(const Packet4bf& a,const Packet4bf& b) {
+  return pxor<Packet4us>(a, b);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pand(const Packet4bf& a,const Packet4bf& b) {
+  return pand<Packet4us>(a, b);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pandnot(const Packet4bf& a,const Packet4bf& b) {
+  return pandnot<Packet4us>(a, b);
+}
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4bf pselect(const Packet4bf& mask, const Packet4bf& a,
+                                                      const Packet4bf& b)
+{
+  return pselect<Packet4us>(mask, a, b);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf print<Packet4bf>(const Packet4bf& a)
+{
+  return F32ToBf16(print<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pfloor<Packet4bf>(const Packet4bf& a)
+{
+  return F32ToBf16(pfloor<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pceil<Packet4bf>(const Packet4bf& a)
+{
+  return F32ToBf16(pceil<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pconj(const Packet4bf& a) { return a; }
+
+template<> EIGEN_STRONG_INLINE Packet4bf padd<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+  return F32ToBf16(padd<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf psub<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+  return F32ToBf16(psub<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pmul<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+  return F32ToBf16(pmul<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pdiv<Packet4bf>(const Packet4bf& a, const Packet4bf& b) {
+  return F32ToBf16(pdiv<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<>
+EIGEN_STRONG_INLINE Packet4bf pgather<bfloat16, Packet4bf>(const bfloat16* from, Index stride)
+{
+  return pgather<uint16_t, Packet4us>(reinterpret_cast<const uint16_t*>(from), stride);
+}
+
+template<>
+EIGEN_STRONG_INLINE void pscatter<bfloat16, Packet4bf>(bfloat16* to, const Packet4bf& from, Index stride)
+{
+  pscatter<uint16_t, Packet4us>(reinterpret_cast<uint16_t*>(to), from, stride);
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux<Packet4bf>(const Packet4bf& a)
+{
+  return static_cast<bfloat16>(predux<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux_max<Packet4bf>(const Packet4bf& a)
+{
+  return static_cast<bfloat16>(predux_max<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux_min<Packet4bf>(const Packet4bf& a)
+{
+  return static_cast<bfloat16>(predux_min<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE bfloat16 predux_mul<Packet4bf>(const Packet4bf& a)
+{
+  return static_cast<bfloat16>(predux_mul<Packet4f>(Bf16ToF32(a)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf preverse<Packet4bf>(const Packet4bf& a)
+{
+  return preverse<Packet4us>(a);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4bf, 4>& kernel)
+{
+  detail::ptranspose_impl(kernel);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pabsdiff<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
+{
+  return F32ToBf16(pabsdiff<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pcmp_eq<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
+{
+  return F32MaskToBf16Mask(pcmp_eq<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pcmp_lt<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
+{
+  return F32MaskToBf16Mask(pcmp_lt<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pcmp_lt_or_nan<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
+{
+  return F32MaskToBf16Mask(pcmp_lt_or_nan<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pcmp_le<Packet4bf>(const Packet4bf& a, const Packet4bf& b)
+{
+  return F32MaskToBf16Mask(pcmp_le<Packet4f>(Bf16ToF32(a), Bf16ToF32(b)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4bf pnegate<Packet4bf>(const Packet4bf& a)
+{
+  return pxor<Packet4us>(a, pset1<Packet4us>(static_cast<uint16_t>(0x8000)));
 }
 
 //---------- double ----------
@@ -571,55 +3642,115 @@
 // Defining these functions as templates ensures that if these intrinsics are
 // already defined in arm_neon.h, then our workaround doesn't cause a conflict
 // and has lower priority in overload resolution.
-template <typename T>
-uint64x2_t vreinterpretq_u64_f64(T a)
-{
-  return (uint64x2_t) a;
-}
+template <typename T> uint64x2_t vreinterpretq_u64_f64(T a) { return (uint64x2_t) a; }
 
-template <typename T>
-float64x2_t vreinterpretq_f64_u64(T a)
-{
-  return (float64x2_t) a;
-}
+template <typename T> float64x2_t vreinterpretq_f64_u64(T a) { return (float64x2_t) a; }
 
 typedef float64x2_t Packet2d;
 typedef float64x1_t Packet1d;
 
+// fuctionally equivalent to _mm_shuffle_pd in SSE (i.e. shuffle(m, n, mask) equals _mm_shuffle_pd(m,n,mask))
+// Currently used in LU/arch/InverseSize4.h to enable a shared implementation
+// for fast inversion of matrices of size 4.
+EIGEN_STRONG_INLINE Packet2d shuffle(const Packet2d& m, const Packet2d& n, int mask)
+{
+  const double* a = reinterpret_cast<const double*>(&m);
+  const double* b = reinterpret_cast<const double*>(&n);
+  Packet2d res = {*(a + (mask & 1)), *(b + ((mask >> 1) & 1))};
+  return res;
+}
+
+EIGEN_STRONG_INLINE Packet2d vec2d_swizzle2(const Packet2d& a, const Packet2d& b, int mask)
+{
+  return shuffle(a, b, mask);
+}
+EIGEN_STRONG_INLINE Packet2d vec2d_unpacklo(const Packet2d& a,const Packet2d& b)
+{
+  return shuffle(a, b, 0);
+}
+EIGEN_STRONG_INLINE Packet2d vec2d_unpackhi(const Packet2d& a,const Packet2d& b)
+{
+  return shuffle(a, b, 3);
+}
+#define vec2d_duplane(a, p) \
+  vdupq_laneq_f64(a, p)
+
 template<> struct packet_traits<double>  : default_packet_traits
 {
   typedef Packet2d type;
   typedef Packet2d half;
-  enum {
+  enum
+  {
     Vectorizable = 1,
     AlignedOnScalar = 1,
     size = 2,
-    HasHalfPacket=0,
-   
-    HasDiv  = 1,
-    // FIXME check the Has*
+    HasHalfPacket = 0,
+
+    HasCmp       = 1,
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 1,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 1,
+    HasArg       = 0,
+    HasAbs2      = 1,
+    HasAbsDiff   = 1,
+    HasMin       = 1,
+    HasMax       = 1,
+    HasConj      = 1,
+    HasSetLinear = 0,
+    HasBlend     = 0,
+
+    HasDiv   = 1,
+    HasFloor = 1,
+    HasCeil = 1,
+    HasRint = 1,
+
     HasSin  = 0,
     HasCos  = 0,
-    HasLog  = 0,
-    HasExp  = 0,
-    HasSqrt = 0
+    HasLog  = 1,
+    HasExp  = 1,
+    HasSqrt = 1,
+    HasRsqrt = 1,
+    HasTanh = 0,
+    HasErf  = 0
   };
 };
 
-template<> struct unpacket_traits<Packet2d> { typedef double  type; enum {size=2, alignment=Aligned16}; typedef Packet2d half; };
+template<> struct unpacket_traits<Packet2d>
+{
+  typedef double type;
+  typedef Packet2d half;
+  typedef Packet2l integer_packet;
+  enum
+  {
+    size = 2,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
 
 template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double&  from) { return vdupq_n_f64(from); }
 
 template<> EIGEN_STRONG_INLINE Packet2d plset<Packet2d>(const double& a)
 {
-  const double countdown_raw[] = {0.0,1.0};
-  const Packet2d countdown = vld1q_f64(countdown_raw);
-  return vaddq_f64(pset1<Packet2d>(a), countdown);
+  const double c[] = {0.0,1.0};
+  return vaddq_f64(pset1<Packet2d>(a), vld1q_f64(c));
 }
+
 template<> EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) { return vaddq_f64(a,b); }
 
 template<> EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) { return vsubq_f64(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& , const Packet2d& );
+template<> EIGEN_STRONG_INLINE Packet2d paddsub<Packet2d>(const Packet2d& a, const Packet2d& b){
+  const Packet2d mask = {numext::bit_cast<double>(0x8000000000000000ull),0.0};
+  return padd(a, pxor(mask, b));
+}
+
 template<> EIGEN_STRONG_INLINE Packet2d pnegate(const Packet2d& a) { return vnegq_f64(a); }
 
 template<> EIGEN_STRONG_INLINE Packet2d pconj(const Packet2d& a) { return a; }
@@ -630,128 +3761,824 @@
 
 #ifdef __ARM_FEATURE_FMA
 // See bug 936. See above comment about FMA for float.
-template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) { return vfmaq_f64(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c)
+{ return vfmaq_f64(c,a,b); }
 #else
-template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) { return vmlaq_f64(c,a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c)
+{ return vmlaq_f64(c,a,b); }
 #endif
 
 template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) { return vminq_f64(a,b); }
 
+#ifdef __ARM_FEATURE_NUMERIC_MAXMIN
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
+template<> EIGEN_STRONG_INLINE Packet2d pmin<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) { return vminnmq_f64(a, b); }
+template<> EIGEN_STRONG_INLINE Packet2d pmax<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) { return vmaxnmq_f64(a, b); }
+
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet2d pmin<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) { return pmin<Packet2d>(a, b); }
+
 template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) { return vmaxq_f64(a,b); }
 
+
+template<> EIGEN_STRONG_INLINE Packet2d pmax<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) { return pmax<Packet2d>(a, b); }
+
 // Logical Operations are not supported for float, so we have to reinterpret casts using NEON intrinsics
 template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b)
-{
-  return vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b)));
-}
+{ return vreinterpretq_f64_u64(vandq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
 
 template<> EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b)
-{
-  return vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b)));
-}
+{ return vreinterpretq_f64_u64(vorrq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
 
 template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b)
-{
-  return vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b)));
-}
+{ return vreinterpretq_f64_u64(veorq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
 
 template<> EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b)
-{
-  return vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b)));
-}
+{ return vreinterpretq_f64_u64(vbicq_u64(vreinterpretq_u64_f64(a),vreinterpretq_u64_f64(b))); }
 
-template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from) { EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f64(from); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_le(const Packet2d& a, const Packet2d& b)
+{ return vreinterpretq_f64_u64(vcleq_f64(a,b)); }
 
-template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from) { EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f64(from); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt(const Packet2d& a, const Packet2d& b)
+{ return vreinterpretq_f64_u64(vcltq_f64(a,b)); }
 
-template<> EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double*   from)
-{
-  return vld1q_dup_f64(from);
-}
-template<> EIGEN_STRONG_INLINE void pstore<double>(double*   to, const Packet2d& from) { EIGEN_DEBUG_ALIGNED_STORE vst1q_f64(to, from); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt_or_nan(const Packet2d& a, const Packet2d& b)
+{ return vreinterpretq_f64_u32(vmvnq_u32(vreinterpretq_u32_u64(vcgeq_f64(a,b)))); }
 
-template<> EIGEN_STRONG_INLINE void pstoreu<double>(double*  to, const Packet2d& from) { EIGEN_DEBUG_UNALIGNED_STORE vst1q_f64(to, from); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_eq(const Packet2d& a, const Packet2d& b)
+{ return vreinterpretq_f64_u64(vceqq_f64(a,b)); }
 
-template<> EIGEN_DEVICE_FUNC inline Packet2d pgather<double, Packet2d>(const double* from, Index stride)
+template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double* from)
+{ EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f64(from); }
+
+template<> EIGEN_STRONG_INLINE Packet2d ploadu<Packet2d>(const double* from)
+{ EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f64(from); }
+
+template<> EIGEN_STRONG_INLINE Packet2d ploaddup<Packet2d>(const double* from) { return vld1q_dup_f64(from); }
+template<> EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from)
+{ EIGEN_DEBUG_ALIGNED_STORE vst1q_f64(to,from); }
+
+template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from)
+{ EIGEN_DEBUG_UNALIGNED_STORE vst1q_f64(to,from); }
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2d pgather<double, Packet2d>(const double* from, Index stride)
 {
   Packet2d res = pset1<Packet2d>(0.0);
-  res = vsetq_lane_f64(from[0*stride], res, 0);
-  res = vsetq_lane_f64(from[1*stride], res, 1);
+  res = vld1q_lane_f64(from + 0*stride, res, 0);
+  res = vld1q_lane_f64(from + 1*stride, res, 1);
   return res;
 }
-template<> EIGEN_DEVICE_FUNC inline void pscatter<double, Packet2d>(double* to, const Packet2d& from, Index stride)
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<double, Packet2d>(double* to, const Packet2d& from, Index stride)
 {
-  to[stride*0] = vgetq_lane_f64(from, 0);
-  to[stride*1] = vgetq_lane_f64(from, 1);
+  vst1q_lane_f64(to + stride*0, from, 0);
+  vst1q_lane_f64(to + stride*1, from, 1);
 }
+
 template<> EIGEN_STRONG_INLINE void prefetch<double>(const double* addr) { EIGEN_ARM_PREFETCH(addr); }
 
 // FIXME only store the 2 first elements ?
-template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return vgetq_lane_f64(a, 0); }
+template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return vgetq_lane_f64(a,0); }
 
-template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a) { return vcombine_f64(vget_high_f64(a), vget_low_f64(a)); }
+template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a)
+{ return vcombine_f64(vget_high_f64(a), vget_low_f64(a)); }
 
 template<> EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a) { return vabsq_f64(a); }
 
 #if EIGEN_COMP_CLANG && defined(__apple_build_version__)
 // workaround ICE, see bug 907
-template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a) { return (vget_low_f64(a) + vget_high_f64(a))[0]; }
+template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a)
+{ return (vget_low_f64(a) + vget_high_f64(a))[0]; }
 #else
-template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a) { return vget_lane_f64(vget_low_f64(a) + vget_high_f64(a), 0); }
+template<> EIGEN_STRONG_INLINE double predux<Packet2d>(const Packet2d& a)
+{ return vget_lane_f64(vget_low_f64(a) + vget_high_f64(a), 0); }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet2d preduxp<Packet2d>(const Packet2d* vecs)
-{
-  float64x2_t trn1, trn2;
-
-  // NEON zip performs interleaving of the supplied vectors.
-  // We perform two interleaves in a row to acquire the transposed vector
-  trn1 = vzip1q_f64(vecs[0], vecs[1]);
-  trn2 = vzip2q_f64(vecs[0], vecs[1]);
-
-  // Do the addition of the resulting vectors
-  return vaddq_f64(trn1, trn2);
-}
 // Other reduction functions:
 // mul
 #if EIGEN_COMP_CLANG && defined(__apple_build_version__)
-template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a) { return (vget_low_f64(a) * vget_high_f64(a))[0]; }
+template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a)
+{ return (vget_low_f64(a) * vget_high_f64(a))[0]; }
 #else
-template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a) { return vget_lane_f64(vget_low_f64(a) * vget_high_f64(a), 0); }
+template<> EIGEN_STRONG_INLINE double predux_mul<Packet2d>(const Packet2d& a)
+{ return vget_lane_f64(vget_low_f64(a) * vget_high_f64(a), 0); }
 #endif
 
 // min
-template<> EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a) { return vgetq_lane_f64(vpminq_f64(a, a), 0); }
+template<> EIGEN_STRONG_INLINE double predux_min<Packet2d>(const Packet2d& a)
+{ return vgetq_lane_f64(vpminq_f64(a,a), 0); }
 
 // max
-template<> EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a) { return vgetq_lane_f64(vpmaxq_f64(a, a), 0); }
+template<> EIGEN_STRONG_INLINE double predux_max<Packet2d>(const Packet2d& a)
+{ return vgetq_lane_f64(vpmaxq_f64(a,a), 0); }
 
-// this PALIGN_NEON business is to work around a bug in LLVM Clang 3.0 causing incorrect compilation errors,
-// see bug 347 and this LLVM bug: http://llvm.org/bugs/show_bug.cgi?id=11074
-#define PALIGN_NEON(Offset,Type,Command) \
-template<>\
-struct palign_impl<Offset,Type>\
-{\
-    EIGEN_STRONG_INLINE static void run(Type& first, const Type& second)\
-    {\
-        if (Offset!=0)\
-            first = Command(first, second, Offset);\
-    }\
-};\
 
-PALIGN_NEON(0,Packet2d,vextq_f64)
-PALIGN_NEON(1,Packet2d,vextq_f64)
-#undef PALIGN_NEON
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet2d, 2>& kernel)
+{
+  const float64x2_t tmp1 = vzip1q_f64(kernel.packet[0], kernel.packet[1]);
+  const float64x2_t tmp2 = vzip2q_f64(kernel.packet[0], kernel.packet[1]);
 
-EIGEN_DEVICE_FUNC inline void
-ptranspose(PacketBlock<Packet2d,2>& kernel) {
-  float64x2_t trn1 = vzip1q_f64(kernel.packet[0], kernel.packet[1]);
-  float64x2_t trn2 = vzip2q_f64(kernel.packet[0], kernel.packet[1]);
-
-  kernel.packet[0] = trn1;
-  kernel.packet[1] = trn2;
+  kernel.packet[0] = tmp1;
+  kernel.packet[1] = tmp2;
 }
-#endif // EIGEN_ARCH_ARM64 
+
+template<> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet2d pselect( const Packet2d& mask, const Packet2d& a, const Packet2d& b)
+{ return vbslq_f64(vreinterpretq_u64_f64(mask), a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet2d print<Packet2d>(const Packet2d& a)
+{ return vrndnq_f64(a); }
+
+template<> EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a)
+{ return vrndmq_f64(a); }
+
+template<> EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a)
+{ return vrndpq_f64(a); }
+
+template<> EIGEN_STRONG_INLINE Packet2d pldexp<Packet2d>(const Packet2d& a, const Packet2d& exponent)
+{ return pldexp_generic(a, exponent); }
+
+template<> EIGEN_STRONG_INLINE Packet2d pfrexp<Packet2d>(const Packet2d& a, Packet2d& exponent)
+{ return pfrexp_generic(a,exponent); }
+
+template<> EIGEN_STRONG_INLINE Packet2d pset1frombits<Packet2d>(uint64_t from)
+{ return vreinterpretq_f64_u64(vdupq_n_u64(from)); }
+
+template<> EIGEN_STRONG_INLINE Packet2d prsqrt(const Packet2d& a) {
+  // Compute approximate reciprocal sqrt.
+  Packet2d x = vrsqrteq_f64(a);
+  // Do Newton iterations for 1/sqrt(x).
+  x = vmulq_f64(vrsqrtsq_f64(vmulq_f64(a, x), x), x);
+  x = vmulq_f64(vrsqrtsq_f64(vmulq_f64(a, x), x), x);
+  x = vmulq_f64(vrsqrtsq_f64(vmulq_f64(a, x), x), x);
+  const Packet2d infinity = pset1<Packet2d>(NumTraits<double>::infinity());
+  return pselect(pcmp_eq(a, pzero(a)), infinity, x);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d psqrt(const Packet2d& _x){ return vsqrtq_f64(_x); }
+
+#endif // EIGEN_ARCH_ARM64
+
+// Do we have an fp16 types and supporting Neon intrinsics?
+#if EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+typedef float16x4_t Packet4hf;
+typedef float16x8_t Packet8hf;
+
+template <>
+struct packet_traits<Eigen::half> : default_packet_traits {
+  typedef Packet8hf type;
+  typedef Packet4hf half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 8,
+    HasHalfPacket = 1,
+
+    HasCmp = 1,
+    HasCast = 1,
+    HasAdd = 1,
+    HasSub = 1,
+    HasShift = 1,
+    HasMul = 1,
+    HasNegate = 1,
+    HasAbs = 1,
+    HasArg = 0,
+    HasAbs2 = 1,
+    HasAbsDiff = 0,
+    HasMin = 1,
+    HasMax = 1,
+    HasConj = 1,
+    HasSetLinear = 0,
+    HasBlend = 0,
+    HasInsert = 1,
+    HasReduxp = 1,
+    HasDiv = 1,
+    HasFloor = 1,
+    HasCeil = 1,
+    HasRint = 1,
+    HasSin = 0,
+    HasCos = 0,
+    HasLog = 0,
+    HasExp = 0,
+    HasSqrt = 1,
+    HasRsqrt = 1,
+    HasErf = EIGEN_FAST_MATH,
+    HasBessel = 0,  // Issues with accuracy.
+    HasNdtri = 0
+  };
+};
+
+template <>
+struct unpacket_traits<Packet4hf> {
+  typedef Eigen::half type;
+  typedef Packet4hf half;
+  enum {
+    size = 4,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+
+template <>
+struct unpacket_traits<Packet8hf> {
+  typedef Eigen::half type;
+  typedef Packet4hf half;
+  enum {
+    size = 8,
+    alignment = Aligned16,
+    vectorizable = true,
+    masked_load_available = false,
+    masked_store_available = false
+  };
+};
+
+template<>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf predux_half_dowto4<Packet8hf>(const Packet8hf& a) {
+  return vadd_f16(vget_low_f16(a), vget_high_f16(a));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pset1<Packet8hf>(const Eigen::half& from) {
+  return vdupq_n_f16(from.x);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pset1<Packet4hf>(const Eigen::half& from) {
+  return vdup_n_f16(from.x);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf plset<Packet8hf>(const Eigen::half& a) {
+  const float16_t f[] = {0, 1, 2, 3, 4, 5, 6, 7};
+  Packet8hf countdown = vld1q_f16(f);
+  return vaddq_f16(pset1<Packet8hf>(a), countdown);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf plset<Packet4hf>(const Eigen::half& a) {
+  const float16_t f[] = {0, 1, 2, 3};
+  Packet4hf countdown = vld1_f16(f);
+  return vadd_f16(pset1<Packet4hf>(a), countdown);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf padd<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vaddq_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf padd<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vadd_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf psub<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vsubq_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf psub<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vsub_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pnegate(const Packet8hf& a) {
+  return vnegq_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pnegate(const Packet4hf& a) {
+  return vneg_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pconj(const Packet8hf& a) {
+  return a;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pconj(const Packet4hf& a) {
+  return a;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmul<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vmulq_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmul<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vmul_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pdiv<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vdivq_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pdiv<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vdiv_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmadd(const Packet8hf& a, const Packet8hf& b, const Packet8hf& c) {
+  return vfmaq_f16(c, a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmadd(const Packet4hf& a, const Packet4hf& b, const Packet4hf& c) {
+  return vfma_f16(c, a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmin<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vminq_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmin<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vmin_f16(a, b);
+}
+
+#ifdef __ARM_FEATURE_NUMERIC_MAXMIN
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
+template<> EIGEN_STRONG_INLINE Packet4hf pmin<PropagateNumbers, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return vminnm_f16(a, b); }
+template<> EIGEN_STRONG_INLINE Packet8hf pmin<PropagateNumbers, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return vminnmq_f16(a, b); }
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4hf pmin<PropagateNaN, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return pmin<Packet4hf>(a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet8hf pmin<PropagateNaN, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return pmin<Packet8hf>(a, b); }
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pmax<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vmaxq_f16(a, b);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pmax<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vmax_f16(a, b);
+}
+
+#ifdef __ARM_FEATURE_NUMERIC_MAXMIN
+// numeric max and min are only available if ARM_FEATURE_NUMERIC_MAXMIN is defined (which can only be the case for Armv8 systems).
+template<> EIGEN_STRONG_INLINE Packet4hf pmax<PropagateNumbers, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return vmaxnm_f16(a, b); }
+template<> EIGEN_STRONG_INLINE Packet8hf pmax<PropagateNumbers, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return vmaxnmq_f16(a, b); }
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4hf pmax<PropagateNaN, Packet4hf>(const Packet4hf& a, const Packet4hf& b) { return pmax<Packet4hf>(a, b); }
+
+template<> EIGEN_STRONG_INLINE Packet8hf pmax<PropagateNaN, Packet8hf>(const Packet8hf& a, const Packet8hf& b) { return pmax<Packet8hf>(a, b); }
+
+#define EIGEN_MAKE_ARM_FP16_CMP_8(name)                                               \
+  template <>                                                                         \
+  EIGEN_STRONG_INLINE Packet8hf pcmp_##name(const Packet8hf& a, const Packet8hf& b) { \
+    return vreinterpretq_f16_u16(vc##name##q_f16(a, b));                              \
+  }
+
+#define EIGEN_MAKE_ARM_FP16_CMP_4(name)                                               \
+  template <>                                                                         \
+  EIGEN_STRONG_INLINE Packet4hf pcmp_##name(const Packet4hf& a, const Packet4hf& b) { \
+    return vreinterpret_f16_u16(vc##name##_f16(a, b));                                \
+  }
+
+EIGEN_MAKE_ARM_FP16_CMP_8(eq)
+EIGEN_MAKE_ARM_FP16_CMP_8(lt)
+EIGEN_MAKE_ARM_FP16_CMP_8(le)
+
+EIGEN_MAKE_ARM_FP16_CMP_4(eq)
+EIGEN_MAKE_ARM_FP16_CMP_4(lt)
+EIGEN_MAKE_ARM_FP16_CMP_4(le)
+
+#undef EIGEN_MAKE_ARM_FP16_CMP_8
+#undef EIGEN_MAKE_ARM_FP16_CMP_4
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pcmp_lt_or_nan<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vreinterpretq_f16_u16(vmvnq_u16(vcgeq_f16(a, b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pcmp_lt_or_nan<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vreinterpret_f16_u16(vmvn_u16(vcge_f16(a, b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf print<Packet8hf>(const Packet8hf& a)
+{ return vrndnq_f16(a); }
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf print<Packet4hf>(const Packet4hf& a)
+{ return vrndn_f16(a); }
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pfloor<Packet8hf>(const Packet8hf& a)
+{ return vrndmq_f16(a); }
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pfloor<Packet4hf>(const Packet4hf& a)
+{ return vrndm_f16(a); }
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pceil<Packet8hf>(const Packet8hf& a)
+{ return vrndpq_f16(a); }
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pceil<Packet4hf>(const Packet4hf& a)
+{ return vrndp_f16(a); }
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf psqrt<Packet8hf>(const Packet8hf& a) {
+  return vsqrtq_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf psqrt<Packet4hf>(const Packet4hf& a) {
+  return vsqrt_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pand<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vreinterpretq_f16_u16(vandq_u16(vreinterpretq_u16_f16(a), vreinterpretq_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pand<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vreinterpret_f16_u16(vand_u16(vreinterpret_u16_f16(a), vreinterpret_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf por<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vreinterpretq_f16_u16(vorrq_u16(vreinterpretq_u16_f16(a), vreinterpretq_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf por<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vreinterpret_f16_u16(vorr_u16(vreinterpret_u16_f16(a), vreinterpret_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pxor<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vreinterpretq_f16_u16(veorq_u16(vreinterpretq_u16_f16(a), vreinterpretq_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pxor<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vreinterpret_f16_u16(veor_u16(vreinterpret_u16_f16(a), vreinterpret_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pandnot<Packet8hf>(const Packet8hf& a, const Packet8hf& b) {
+  return vreinterpretq_f16_u16(vbicq_u16(vreinterpretq_u16_f16(a), vreinterpretq_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pandnot<Packet4hf>(const Packet4hf& a, const Packet4hf& b) {
+  return vreinterpret_f16_u16(vbic_u16(vreinterpret_u16_f16(a), vreinterpret_u16_f16(b)));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pload<Packet8hf>(const Eigen::half* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1q_f16(reinterpret_cast<const float16_t*>(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pload<Packet4hf>(const Eigen::half* from) {
+  EIGEN_DEBUG_ALIGNED_LOAD return vld1_f16(reinterpret_cast<const float16_t*>(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf ploadu<Packet8hf>(const Eigen::half* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1q_f16(reinterpret_cast<const float16_t*>(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf ploadu<Packet4hf>(const Eigen::half* from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD return vld1_f16(reinterpret_cast<const float16_t*>(from));
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf ploaddup<Packet8hf>(const Eigen::half* from) {
+  Packet8hf packet;
+  packet[0] = from[0].x;
+  packet[1] = from[0].x;
+  packet[2] = from[1].x;
+  packet[3] = from[1].x;
+  packet[4] = from[2].x;
+  packet[5] = from[2].x;
+  packet[6] = from[3].x;
+  packet[7] = from[3].x;
+  return packet;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf ploaddup<Packet4hf>(const Eigen::half* from) {
+  float16x4_t packet;
+  float16_t* tmp;
+  tmp = (float16_t*)&packet;
+  tmp[0] = from[0].x;
+  tmp[1] = from[0].x;
+  tmp[2] = from[1].x;
+  tmp[3] = from[1].x;
+  return packet;
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf ploadquad<Packet8hf>(const Eigen::half* from) {
+  Packet4hf lo, hi;
+  lo = vld1_dup_f16(reinterpret_cast<const float16_t*>(from));
+  hi = vld1_dup_f16(reinterpret_cast<const float16_t*>(from+1));
+  return vcombine_f16(lo, hi);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pinsertfirst(const Packet8hf& a, Eigen::half b) { return vsetq_lane_f16(b.x, a, 0); }
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pinsertfirst(const Packet4hf& a, Eigen::half b) { return vset_lane_f16(b.x, a, 0); }
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pselect(const Packet8hf& mask, const Packet8hf& a, const Packet8hf& b) {
+  return vbslq_f16(vreinterpretq_u16_f16(mask), a, b);
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pselect(const Packet4hf& mask, const Packet4hf& a, const Packet4hf& b) {
+  return vbsl_f16(vreinterpret_u16_f16(mask), a, b);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pinsertlast(const Packet8hf& a, Eigen::half b) { return vsetq_lane_f16(b.x, a, 7); }
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pinsertlast(const Packet4hf& a, Eigen::half b) { return vset_lane_f16(b.x, a, 3); }
+
+template <>
+EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet8hf& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1q_f16(reinterpret_cast<float16_t*>(to), from);
+}
+
+template <>
+EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet4hf& from) {
+  EIGEN_DEBUG_ALIGNED_STORE vst1_f16(reinterpret_cast<float16_t*>(to), from);
+}
+
+template <>
+EIGEN_STRONG_INLINE void pstoreu<Eigen::half>(Eigen::half* to, const Packet8hf& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1q_f16(reinterpret_cast<float16_t*>(to), from);
+}
+
+template <>
+EIGEN_STRONG_INLINE void pstoreu<Eigen::half>(Eigen::half* to, const Packet4hf& from) {
+  EIGEN_DEBUG_UNALIGNED_STORE vst1_f16(reinterpret_cast<float16_t*>(to), from);
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet8hf pgather<Eigen::half, Packet8hf>(const Eigen::half* from, Index stride) {
+  Packet8hf res = pset1<Packet8hf>(Eigen::half(0.f));
+  res = vsetq_lane_f16(from[0 * stride].x, res, 0);
+  res = vsetq_lane_f16(from[1 * stride].x, res, 1);
+  res = vsetq_lane_f16(from[2 * stride].x, res, 2);
+  res = vsetq_lane_f16(from[3 * stride].x, res, 3);
+  res = vsetq_lane_f16(from[4 * stride].x, res, 4);
+  res = vsetq_lane_f16(from[5 * stride].x, res, 5);
+  res = vsetq_lane_f16(from[6 * stride].x, res, 6);
+  res = vsetq_lane_f16(from[7 * stride].x, res, 7);
+  return res;
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet4hf pgather<Eigen::half, Packet4hf>(const Eigen::half* from, Index stride) {
+  Packet4hf res = pset1<Packet4hf>(Eigen::half(0.f));
+  res = vset_lane_f16(from[0 * stride].x, res, 0);
+  res = vset_lane_f16(from[1 * stride].x, res, 1);
+  res = vset_lane_f16(from[2 * stride].x, res, 2);
+  res = vset_lane_f16(from[3 * stride].x, res, 3);
+  return res;
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet8hf>(Eigen::half* to, const Packet8hf& from, Index stride) {
+  to[stride * 0].x = vgetq_lane_f16(from, 0);
+  to[stride * 1].x = vgetq_lane_f16(from, 1);
+  to[stride * 2].x = vgetq_lane_f16(from, 2);
+  to[stride * 3].x = vgetq_lane_f16(from, 3);
+  to[stride * 4].x = vgetq_lane_f16(from, 4);
+  to[stride * 5].x = vgetq_lane_f16(from, 5);
+  to[stride * 6].x = vgetq_lane_f16(from, 6);
+  to[stride * 7].x = vgetq_lane_f16(from, 7);
+}
+
+template <>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet4hf>(Eigen::half* to, const Packet4hf& from, Index stride) {
+  to[stride * 0].x = vget_lane_f16(from, 0);
+  to[stride * 1].x = vget_lane_f16(from, 1);
+  to[stride * 2].x = vget_lane_f16(from, 2);
+  to[stride * 3].x = vget_lane_f16(from, 3);
+}
+
+template <>
+EIGEN_STRONG_INLINE void prefetch<Eigen::half>(const Eigen::half* addr) {
+  EIGEN_ARM_PREFETCH(addr);
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half pfirst<Packet8hf>(const Packet8hf& a) {
+  float16_t x[8];
+  vst1q_f16(x, a);
+  Eigen::half h;
+  h.x = x[0];
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half pfirst<Packet4hf>(const Packet4hf& a) {
+  float16_t x[4];
+  vst1_f16(x, a);
+  Eigen::half h;
+  h.x = x[0];
+  return h;
+}
+
+template<> EIGEN_STRONG_INLINE Packet8hf preverse(const Packet8hf& a) {
+  float16x4_t a_lo, a_hi;
+  Packet8hf a_r64;
+
+  a_r64 = vrev64q_f16(a);
+  a_lo = vget_low_f16(a_r64);
+  a_hi = vget_high_f16(a_r64);
+  return vcombine_f16(a_hi, a_lo);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf preverse<Packet4hf>(const Packet4hf& a) {
+  return vrev64_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet8hf pabs<Packet8hf>(const Packet8hf& a) {
+  return vabsq_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4hf pabs<Packet4hf>(const Packet4hf& a) {
+  return vabs_f16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux<Packet8hf>(const Packet8hf& a) {
+  float16x4_t a_lo, a_hi, sum;
+
+  a_lo = vget_low_f16(a);
+  a_hi = vget_high_f16(a);
+  sum = vpadd_f16(a_lo, a_hi);
+  sum = vpadd_f16(sum, sum);
+  sum = vpadd_f16(sum, sum);
+
+  Eigen::half h;
+  h.x = vget_lane_f16(sum, 0);
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux<Packet4hf>(const Packet4hf& a) {
+  float16x4_t sum;
+
+  sum = vpadd_f16(a, a);
+  sum = vpadd_f16(sum, sum);
+  Eigen::half h;
+  h.x = vget_lane_f16(sum, 0);
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_mul<Packet8hf>(const Packet8hf& a) {
+  float16x4_t a_lo, a_hi, prod;
+
+  a_lo = vget_low_f16(a);
+  a_hi = vget_high_f16(a);
+  prod = vmul_f16(a_lo, a_hi);
+  prod = vmul_f16(prod, vrev64_f16(prod));
+
+  Eigen::half h;
+  h.x = vmulh_f16(vget_lane_f16(prod, 0), vget_lane_f16(prod, 1));
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_mul<Packet4hf>(const Packet4hf& a) {
+  float16x4_t prod;
+  prod = vmul_f16(a, vrev64_f16(a));
+  Eigen::half h;
+  h.x = vmulh_f16(vget_lane_f16(prod, 0), vget_lane_f16(prod, 1));
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_min<Packet8hf>(const Packet8hf& a) {
+  float16x4_t a_lo, a_hi, min;
+
+  a_lo = vget_low_f16(a);
+  a_hi = vget_high_f16(a);
+  min = vpmin_f16(a_lo, a_hi);
+  min = vpmin_f16(min, min);
+  min = vpmin_f16(min, min);
+
+  Eigen::half h;
+  h.x = vget_lane_f16(min, 0);
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_min<Packet4hf>(const Packet4hf& a) {
+  Packet4hf tmp;
+  tmp = vpmin_f16(a, a);
+  tmp = vpmin_f16(tmp, tmp);
+  Eigen::half h;
+  h.x = vget_lane_f16(tmp, 0);
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_max<Packet8hf>(const Packet8hf& a) {
+  float16x4_t a_lo, a_hi, max;
+
+  a_lo = vget_low_f16(a);
+  a_hi = vget_high_f16(a);
+  max = vpmax_f16(a_lo, a_hi);
+  max = vpmax_f16(max, max);
+  max = vpmax_f16(max, max);
+
+  Eigen::half h;
+  h.x = vget_lane_f16(max, 0);
+  return h;
+}
+
+template <>
+EIGEN_STRONG_INLINE Eigen::half predux_max<Packet4hf>(const Packet4hf& a) {
+  Packet4hf tmp;
+  tmp = vpmax_f16(a, a);
+  tmp = vpmax_f16(tmp, tmp);
+  Eigen::half h;
+  h.x = vget_lane_f16(tmp, 0);
+  return h;
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8hf, 4>& kernel)
+{
+  const float16x8x2_t zip16_1 = vzipq_f16(kernel.packet[0], kernel.packet[1]);
+  const float16x8x2_t zip16_2 = vzipq_f16(kernel.packet[2], kernel.packet[3]);
+
+  const float32x4x2_t zip32_1 = vzipq_f32(vreinterpretq_f32_f16(zip16_1.val[0]), vreinterpretq_f32_f16(zip16_2.val[0]));
+  const float32x4x2_t zip32_2 = vzipq_f32(vreinterpretq_f32_f16(zip16_1.val[1]), vreinterpretq_f32_f16(zip16_2.val[1]));
+
+  kernel.packet[0] = vreinterpretq_f16_f32(zip32_1.val[0]);
+  kernel.packet[1] = vreinterpretq_f16_f32(zip32_1.val[1]);
+  kernel.packet[2] = vreinterpretq_f16_f32(zip32_2.val[0]);
+  kernel.packet[3] = vreinterpretq_f16_f32(zip32_2.val[1]);
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet4hf, 4>& kernel) {
+  EIGEN_ALIGN16 float16x4x4_t tmp_x4;
+  float16_t* tmp = (float16_t*)&kernel;
+  tmp_x4 = vld4_f16(tmp);
+
+  kernel.packet[0] = tmp_x4.val[0];
+  kernel.packet[1] = tmp_x4.val[1];
+  kernel.packet[2] = tmp_x4.val[2];
+  kernel.packet[3] = tmp_x4.val[3];
+}
+
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ptranspose(PacketBlock<Packet8hf, 8>& kernel) {
+  float16x8x2_t T_1[4];
+
+  T_1[0] = vuzpq_f16(kernel.packet[0], kernel.packet[1]);
+  T_1[1] = vuzpq_f16(kernel.packet[2], kernel.packet[3]);
+  T_1[2] = vuzpq_f16(kernel.packet[4], kernel.packet[5]);
+  T_1[3] = vuzpq_f16(kernel.packet[6], kernel.packet[7]);
+
+  float16x8x2_t T_2[4];
+  T_2[0] = vuzpq_f16(T_1[0].val[0], T_1[1].val[0]);
+  T_2[1] = vuzpq_f16(T_1[0].val[1], T_1[1].val[1]);
+  T_2[2] = vuzpq_f16(T_1[2].val[0], T_1[3].val[0]);
+  T_2[3] = vuzpq_f16(T_1[2].val[1], T_1[3].val[1]);
+
+  float16x8x2_t T_3[4];
+  T_3[0] = vuzpq_f16(T_2[0].val[0], T_2[2].val[0]);
+  T_3[1] = vuzpq_f16(T_2[0].val[1], T_2[2].val[1]);
+  T_3[2] = vuzpq_f16(T_2[1].val[0], T_2[3].val[0]);
+  T_3[3] = vuzpq_f16(T_2[1].val[1], T_2[3].val[1]);
+
+  kernel.packet[0] = T_3[0].val[0];
+  kernel.packet[1] = T_3[2].val[0];
+  kernel.packet[2] = T_3[1].val[0];
+  kernel.packet[3] = T_3[3].val[0];
+  kernel.packet[4] = T_3[0].val[1];
+  kernel.packet[5] = T_3[2].val[1];
+  kernel.packet[6] = T_3[1].val[1];
+  kernel.packet[7] = T_3[3].val[1];
+}
+#endif // end EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
 
 } // end namespace internal
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h
new file mode 100644
index 0000000..54f9733
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/NEON/TypeCasting.h
@@ -0,0 +1,1419 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2018 Rasmus Munk Larsen <rmlarsen@google.com>
+// Copyright (C) 2020 Antonio Sanchez <cantonios@google.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_TYPE_CASTING_NEON_H
+#define EIGEN_TYPE_CASTING_NEON_H
+
+namespace Eigen {
+
+namespace internal {
+
+//==============================================================================
+// pcast, SrcType = float
+//==============================================================================
+template <>
+struct type_casting_traits<float, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4f, Packet4f>(const Packet4f& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet2f, Packet2f>(const Packet2f& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<float, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+struct type_casting_traits<float, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+// If float64 exists, first convert to that to keep as much precision as possible.
+#if EIGEN_ARCH_ARM64
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet4f, Packet2l>(const Packet4f& a) {
+  // Discard second half of input.
+  return vcvtq_s64_f64(vcvt_f64_f32(vget_low_f32(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet4f, Packet2ul>(const Packet4f& a) {
+  // Discard second half of input.
+  return vcvtq_u64_f64(vcvt_f64_f32(vget_low_f32(a)));
+}
+#else
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet4f, Packet2l>(const Packet4f& a) {
+  // Discard second half of input.
+  return vmovl_s32(vget_low_s32(vcvtq_s32_f32(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet4f, Packet2ul>(const Packet4f& a) {
+  // Discard second half of input.
+  return vmovl_u32(vget_low_u32(vcvtq_u32_f32(a)));
+}
+#endif  // EIGEN_ARCH_ARM64
+
+template <>
+struct type_casting_traits<float, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4f, Packet4i>(const Packet4f& a) {
+  return vcvtq_s32_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet2f, Packet2i>(const Packet2f& a) {
+  return vcvt_s32_f32(a);
+}
+
+template <>
+struct type_casting_traits<float, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4f, Packet4ui>(const Packet4f& a) {
+  return vcvtq_u32_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet2f, Packet2ui>(const Packet2f& a) {
+  return vcvt_u32_f32(a);
+}
+
+template <>
+struct type_casting_traits<float, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet4f, Packet8s>(const Packet4f& a, const Packet4f& b) {
+  return vcombine_s16(vmovn_s32(vcvtq_s32_f32(a)), vmovn_s32(vcvtq_s32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet2f, Packet4s>(const Packet2f& a, const Packet2f& b) {
+  return vmovn_s32(vcombine_s32(vcvt_s32_f32(a), vcvt_s32_f32(b)));
+}
+
+template <>
+struct type_casting_traits<float, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet4f, Packet8us>(const Packet4f& a, const Packet4f& b) {
+  return vcombine_u16(vmovn_u32(vcvtq_u32_f32(a)), vmovn_u32(vcvtq_u32_f32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet2f, Packet4us>(const Packet2f& a, const Packet2f& b) {
+  return vmovn_u32(vcombine_u32(vcvt_u32_f32(a), vcvt_u32_f32(b)));
+}
+
+template <>
+struct type_casting_traits<float, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet4f, Packet16c>(const Packet4f& a, const Packet4f& b, const Packet4f& c,
+                                                         const Packet4f& d) {
+  const int16x8_t ab_s16 = pcast<Packet4f, Packet8s>(a, b);
+  const int16x8_t cd_s16 = pcast<Packet4f, Packet8s>(c, d);
+  return vcombine_s8(vmovn_s16(ab_s16), vmovn_s16(cd_s16));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet2f, Packet8c>(const Packet2f& a, const Packet2f& b, const Packet2f& c,
+                                                       const Packet2f& d) {
+  const int16x4_t ab_s16 = pcast<Packet2f, Packet4s>(a, b);
+  const int16x4_t cd_s16 = pcast<Packet2f, Packet4s>(c, d);
+  return vmovn_s16(vcombine_s16(ab_s16, cd_s16));
+}
+
+template <>
+struct type_casting_traits<float, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet4f, Packet16uc>(const Packet4f& a, const Packet4f& b, const Packet4f& c,
+                                                           const Packet4f& d) {
+  const uint16x8_t ab_u16 = pcast<Packet4f, Packet8us>(a, b);
+  const uint16x8_t cd_u16 = pcast<Packet4f, Packet8us>(c, d);
+  return vcombine_u8(vmovn_u16(ab_u16), vmovn_u16(cd_u16));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet2f, Packet8uc>(const Packet2f& a, const Packet2f& b, const Packet2f& c,
+                                                         const Packet2f& d) {
+  const uint16x4_t ab_u16 = pcast<Packet2f, Packet4us>(a, b);
+  const uint16x4_t cd_u16 = pcast<Packet2f, Packet4us>(c, d);
+  return vmovn_u16(vcombine_u16(ab_u16, cd_u16));
+}
+
+//==============================================================================
+// pcast, SrcType = int8_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::int8_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet16c, Packet4f>(const Packet16c& a) {
+  // Discard all but first 4 bytes.
+  return vcvtq_f32_s32(vmovl_s16(vget_low_s16(vmovl_s8(vget_low_s8(a)))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet8c, Packet2f>(const Packet8c& a) {
+  // Discard all but first 2 bytes.
+  return vcvt_f32_s32(vget_low_s32(vmovl_s16(vget_low_s16(vmovl_s8(a)))));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 8 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet16c, Packet2l>(const Packet16c& a) {
+  // Discard all but first two bytes.
+  return vmovl_s32(vget_low_s32(vmovl_s16(vget_low_s16(vmovl_s8(vget_low_s8(a))))));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 8 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet16c, Packet2ul>(const Packet16c& a) {
+  return vreinterpretq_u64_s64(pcast<Packet16c, Packet2l>(a));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet16c, Packet4i>(const Packet16c& a) {
+  // Discard all but first 4 bytes.
+  return vmovl_s16(vget_low_s16(vmovl_s8(vget_low_s8(a))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet8c, Packet2i>(const Packet8c& a) {
+  // Discard all but first 2 bytes.
+  return vget_low_s32(vmovl_s16(vget_low_s16(vmovl_s8(a))));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet16c, Packet4ui>(const Packet16c& a) {
+  return vreinterpretq_u32_s32(pcast<Packet16c, Packet4i>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet8c, Packet2ui>(const Packet8c& a) {
+  return vreinterpret_u32_s32(pcast<Packet8c, Packet2i>(a));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet16c, Packet8s>(const Packet16c& a) {
+  // Discard second half of input.
+  return vmovl_s8(vget_low_s8(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet8c, Packet4s>(const Packet8c& a) {
+  // Discard second half of input.
+  return vget_low_s16(vmovl_s8(a));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet16c, Packet8us>(const Packet16c& a) {
+  return vreinterpretq_u16_s16(pcast<Packet16c, Packet8s>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet8c, Packet4us>(const Packet8c& a) {
+  return vreinterpret_u16_s16(pcast<Packet8c, Packet4s>(a));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet16c, Packet16c>(const Packet16c& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet8c, Packet8c>(const Packet8c& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4c, Packet4c>(const Packet4c& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet16c, Packet16uc>(const Packet16c& a) {
+  return vreinterpretq_u8_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet8c, Packet8uc>(const Packet8c& a) {
+  return vreinterpret_u8_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4c, Packet4uc>(const Packet4c& a) {
+  return static_cast<Packet4uc>(a);
+}
+
+//==============================================================================
+// pcast, SrcType = uint8_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::uint8_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet16uc, Packet4f>(const Packet16uc& a) {
+  // Discard all but first 4 bytes.
+  return vcvtq_f32_u32(vmovl_u16(vget_low_u16(vmovl_u8(vget_low_u8(a)))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet8uc, Packet2f>(const Packet8uc& a) {
+  // Discard all but first 2 bytes.
+  return vcvt_f32_u32(vget_low_u32(vmovl_u16(vget_low_u16(vmovl_u8(a)))));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 8 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet16uc, Packet2ul>(const Packet16uc& a) {
+  // Discard all but first two bytes.
+  return vmovl_u32(vget_low_u32(vmovl_u16(vget_low_u16(vmovl_u8(vget_low_u8(a))))));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 8 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet16uc, Packet2l>(const Packet16uc& a) {
+  return vreinterpretq_s64_u64(pcast<Packet16uc, Packet2ul>(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet16uc, Packet4ui>(const Packet16uc& a) {
+  // Discard all but first 4 bytes.
+  return vmovl_u16(vget_low_u16(vmovl_u8(vget_low_u8(a))));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet8uc, Packet2ui>(const Packet8uc& a) {
+  // Discard all but first 2 bytes.
+  return vget_low_u32(vmovl_u16(vget_low_u16(vmovl_u8(a))));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet16uc, Packet4i>(const Packet16uc& a) {
+  return vreinterpretq_s32_u32(pcast<Packet16uc, Packet4ui>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet8uc, Packet2i>(const Packet8uc& a) {
+  return vreinterpret_s32_u32(pcast<Packet8uc, Packet2ui>(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet16uc, Packet8us>(const Packet16uc& a) {
+  // Discard second half of input.
+  return vmovl_u8(vget_low_u8(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet8uc, Packet4us>(const Packet8uc& a) {
+  // Discard second half of input.
+  return vget_low_u16(vmovl_u8(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet16uc, Packet8s>(const Packet16uc& a) {
+  return vreinterpretq_s16_u16(pcast<Packet16uc, Packet8us>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet8uc, Packet4s>(const Packet8uc& a) {
+  return vreinterpret_s16_u16(pcast<Packet8uc, Packet4us>(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet16uc, Packet16uc>(const Packet16uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet8uc, Packet8uc>(const Packet8uc& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4uc pcast<Packet4uc, Packet4uc>(const Packet4uc& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet16uc, Packet16c>(const Packet16uc& a) {
+  return vreinterpretq_s8_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet8uc, Packet8c>(const Packet8uc& a) {
+  return vreinterpret_s8_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4c pcast<Packet4uc, Packet4c>(const Packet4uc& a) {
+  return static_cast<Packet4c>(a);
+}
+
+//==============================================================================
+// pcast, SrcType = int16_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::int16_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet8s, Packet4f>(const Packet8s& a) {
+  // Discard second half of input.
+  return vcvtq_f32_s32(vmovl_s16(vget_low_s16(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet4s, Packet2f>(const Packet4s& a) {
+  // Discard second half of input.
+  return vcvt_f32_s32(vget_low_s32(vmovl_s16(a)));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet8s, Packet2l>(const Packet8s& a) {
+  // Discard all but first two values.
+  return vmovl_s32(vget_low_s32(vmovl_s16(vget_low_s16(a))));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet8s, Packet2ul>(const Packet8s& a) {
+  return vreinterpretq_u64_s64(pcast<Packet8s, Packet2l>(a));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet8s, Packet4i>(const Packet8s& a) {
+  // Discard second half of input.
+  return vmovl_s16(vget_low_s16(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet4s, Packet2i>(const Packet4s& a) {
+  // Discard second half of input.
+  return vget_low_s32(vmovl_s16(a));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet8s, Packet4ui>(const Packet8s& a) {
+  return vreinterpretq_u32_s32(pcast<Packet8s, Packet4i>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet4s, Packet2ui>(const Packet4s& a) {
+  return vreinterpret_u32_s32(pcast<Packet4s, Packet2i>(a));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet8s, Packet8s>(const Packet8s& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4s, Packet4s>(const Packet4s& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet8s, Packet8us>(const Packet8s& a) {
+  return vreinterpretq_u16_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4s, Packet4us>(const Packet4s& a) {
+  return vreinterpret_u16_s16(a);
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet8s, Packet16c>(const Packet8s& a, const Packet8s& b) {
+  return vcombine_s8(vmovn_s16(a), vmovn_s16(b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet4s, Packet8c>(const Packet4s& a, const Packet4s& b) {
+  return vmovn_s16(vcombine_s16(a, b));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet8s, Packet16uc>(const Packet8s& a, const Packet8s& b) {
+  return vcombine_u8(vmovn_u16(vreinterpretq_u16_s16(a)), vmovn_u16(vreinterpretq_u16_s16(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet4s, Packet8uc>(const Packet4s& a, const Packet4s& b) {
+  return vmovn_u16(vcombine_u16(vreinterpret_u16_s16(a), vreinterpret_u16_s16(b)));
+}
+
+//==============================================================================
+// pcast, SrcType = uint16_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::uint16_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet8us, Packet4f>(const Packet8us& a) {
+  // Discard second half of input.
+  return vcvtq_f32_u32(vmovl_u16(vget_low_u16(a)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet4us, Packet2f>(const Packet4us& a) {
+  // Discard second half of input.
+  return vcvt_f32_u32(vget_low_u32(vmovl_u16(a)));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet8us, Packet2ul>(const Packet8us& a) {
+  // Discard all but first two values.
+  return vmovl_u32(vget_low_u32(vmovl_u16(vget_low_u16(a))));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet8us, Packet2l>(const Packet8us& a) {
+  return vreinterpretq_s64_u64(pcast<Packet8us, Packet2ul>(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet8us, Packet4ui>(const Packet8us& a) {
+  // Discard second half of input.
+  return vmovl_u16(vget_low_u16(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet4us, Packet2ui>(const Packet4us& a) {
+  // Discard second half of input.
+  return vget_low_u32(vmovl_u16(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet8us, Packet4i>(const Packet8us& a) {
+  return vreinterpretq_s32_u32(pcast<Packet8us, Packet4ui>(a));
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet4us, Packet2i>(const Packet4us& a) {
+  return vreinterpret_s32_u32(pcast<Packet4us, Packet2ui>(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet8us, Packet8us>(const Packet8us& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet4us, Packet4us>(const Packet4us& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet8us, Packet8s>(const Packet8us& a) {
+  return vreinterpretq_s16_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet4us, Packet4s>(const Packet4us& a) {
+  return vreinterpret_s16_u16(a);
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet8us, Packet16uc>(const Packet8us& a, const Packet8us& b) {
+  return vcombine_u8(vmovn_u16(a), vmovn_u16(b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet4us, Packet8uc>(const Packet4us& a, const Packet4us& b) {
+  return vmovn_u16(vcombine_u16(a, b));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet8us, Packet16c>(const Packet8us& a, const Packet8us& b) {
+  return vreinterpretq_s8_u8(pcast<Packet8us, Packet16uc>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet4us, Packet8c>(const Packet4us& a, const Packet4us& b) {
+  return vreinterpret_s8_u8(pcast<Packet4us, Packet8uc>(a, b));
+}
+
+//==============================================================================
+// pcast, SrcType = int32_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::int32_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4i, Packet4f>(const Packet4i& a) {
+  return vcvtq_f32_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet2i, Packet2f>(const Packet2i& a) {
+  return vcvt_f32_s32(a);
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet4i, Packet2l>(const Packet4i& a) {
+  // Discard second half of input.
+  return vmovl_s32(vget_low_s32(a));
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet4i, Packet2ul>(const Packet4i& a) {
+  return vreinterpretq_u64_s64(pcast<Packet4i, Packet2l>(a));
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4i, Packet4i>(const Packet4i& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet2i, Packet2i>(const Packet2i& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4i, Packet4ui>(const Packet4i& a) {
+  return vreinterpretq_u32_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet2i, Packet2ui>(const Packet2i& a) {
+  return vreinterpret_u32_s32(a);
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet4i, Packet8s>(const Packet4i& a, const Packet4i& b) {
+  return vcombine_s16(vmovn_s32(a), vmovn_s32(b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet2i, Packet4s>(const Packet2i& a, const Packet2i& b) {
+  return vmovn_s32(vcombine_s32(a, b));
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet4i, Packet8us>(const Packet4i& a, const Packet4i& b) {
+  return vcombine_u16(vmovn_u32(vreinterpretq_u32_s32(a)), vmovn_u32(vreinterpretq_u32_s32(b)));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet2i, Packet4us>(const Packet2i& a, const Packet2i& b) {
+  return vmovn_u32(vreinterpretq_u32_s32(vcombine_s32(a, b)));
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet4i, Packet16c>(const Packet4i& a, const Packet4i& b, const Packet4i& c,
+                                                         const Packet4i& d) {
+  const int16x8_t ab_s16 = pcast<Packet4i, Packet8s>(a, b);
+  const int16x8_t cd_s16 = pcast<Packet4i, Packet8s>(c, d);
+  return vcombine_s8(vmovn_s16(ab_s16), vmovn_s16(cd_s16));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet2i, Packet8c>(const Packet2i& a, const Packet2i& b, const Packet2i& c,
+                                                       const Packet2i& d) {
+  const int16x4_t ab_s16 = vmovn_s32(vcombine_s32(a, b));
+  const int16x4_t cd_s16 = vmovn_s32(vcombine_s32(c, d));
+  return vmovn_s16(vcombine_s16(ab_s16, cd_s16));
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet4i, Packet16uc>(const Packet4i& a, const Packet4i& b, const Packet4i& c,
+                                                           const Packet4i& d) {
+  const uint16x8_t ab_u16 = pcast<Packet4i, Packet8us>(a, b);
+  const uint16x8_t cd_u16 = pcast<Packet4i, Packet8us>(c, d);
+  return vcombine_u8(vmovn_u16(ab_u16), vmovn_u16(cd_u16));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet2i, Packet8uc>(const Packet2i& a, const Packet2i& b, const Packet2i& c,
+                                                         const Packet2i& d) {
+  const uint16x4_t ab_u16 = pcast<Packet2i, Packet4us>(a, b);
+  const uint16x4_t cd_u16 = pcast<Packet2i, Packet4us>(c, d);
+  return vmovn_u16(vcombine_u16(ab_u16, cd_u16));
+}
+
+//==============================================================================
+// pcast, SrcType = uint32_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::uint32_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet4ui, Packet4f>(const Packet4ui& a) {
+  return vcvtq_f32_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f pcast<Packet2ui, Packet2f>(const Packet2ui& a) {
+  return vcvt_f32_u32(a);
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet4ui, Packet2ul>(const Packet4ui& a) {
+  // Discard second half of input.
+  return vmovl_u32(vget_low_u32(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet4ui, Packet2l>(const Packet4ui& a) {
+  return vreinterpretq_s64_u64(pcast<Packet4ui, Packet2ul>(a));
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet4ui, Packet4ui>(const Packet4ui& a) {
+  return a;
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui pcast<Packet2ui, Packet2ui>(const Packet2ui& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet4ui, Packet4i>(const Packet4ui& a) {
+  return vreinterpretq_s32_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i pcast<Packet2ui, Packet2i>(const Packet2ui& a) {
+  return vreinterpret_s32_u32(a);
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet4ui, Packet8us>(const Packet4ui& a, const Packet4ui& b) {
+  return vcombine_u16(vmovn_u32(a), vmovn_u32(b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4us pcast<Packet2ui, Packet4us>(const Packet2ui& a, const Packet2ui& b) {
+  return vmovn_u32(vcombine_u32(a, b));
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet4ui, Packet8s>(const Packet4ui& a, const Packet4ui& b) {
+  return vreinterpretq_s16_u16(pcast<Packet4ui, Packet8us>(a, b));
+}
+template <>
+EIGEN_STRONG_INLINE Packet4s pcast<Packet2ui, Packet4s>(const Packet2ui& a, const Packet2ui& b) {
+  return vreinterpret_s16_u16(pcast<Packet2ui, Packet4us>(a, b));
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet4ui, Packet16uc>(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c,
+                                                            const Packet4ui& d) {
+  const uint16x8_t ab_u16 = vcombine_u16(vmovn_u32(a), vmovn_u32(b));
+  const uint16x8_t cd_u16 = vcombine_u16(vmovn_u32(c), vmovn_u32(d));
+  return vcombine_u8(vmovn_u16(ab_u16), vmovn_u16(cd_u16));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc pcast<Packet2ui, Packet8uc>(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c,
+                                                          const Packet2ui& d) {
+  const uint16x4_t ab_u16 = vmovn_u32(vcombine_u32(a, b));
+  const uint16x4_t cd_u16 = vmovn_u32(vcombine_u32(c, d));
+  return vmovn_u16(vcombine_u16(ab_u16, cd_u16));
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet4ui, Packet16c>(const Packet4ui& a, const Packet4ui& b, const Packet4ui& c,
+                                                          const Packet4ui& d) {
+  return vreinterpretq_s8_u8(pcast<Packet4ui, Packet16uc>(a, b, c, d));
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c pcast<Packet2ui, Packet8c>(const Packet2ui& a, const Packet2ui& b, const Packet2ui& c,
+                                                        const Packet2ui& d) {
+  return vreinterpret_s8_u8(pcast<Packet2ui, Packet8uc>(a, b, c, d));
+}
+
+//==============================================================================
+// pcast, SrcType = int64_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::int64_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet2l, Packet4f>(const Packet2l& a, const Packet2l& b) {
+  return vcvtq_f32_s32(vcombine_s32(vmovn_s64(a), vmovn_s64(b)));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2l, Packet2l>(const Packet2l& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2l, Packet2ul>(const Packet2l& a) {
+  return vreinterpretq_u64_s64(a);
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet2l, Packet4i>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_s32(vmovn_s64(a), vmovn_s64(b));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet2l, Packet4ui>(const Packet2l& a, const Packet2l& b) {
+  return vcombine_u32(vmovn_u64(vreinterpretq_u64_s64(a)), vmovn_u64(vreinterpretq_u64_s64(b)));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet2l, Packet8s>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
+                                                       const Packet2l& d) {
+  const int32x4_t ab_s32 = pcast<Packet2l, Packet4i>(a, b);
+  const int32x4_t cd_s32 = pcast<Packet2l, Packet4i>(c, d);
+  return vcombine_s16(vmovn_s32(ab_s32), vmovn_s32(cd_s32));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet2l, Packet8us>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
+                                                         const Packet2l& d) {
+  const uint32x4_t ab_u32 = pcast<Packet2l, Packet4ui>(a, b);
+  const uint32x4_t cd_u32 = pcast<Packet2l, Packet4ui>(c, d);
+  return vcombine_u16(vmovn_u32(ab_u32), vmovn_u32(cd_u32));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 8, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet2l, Packet16c>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
+                                                         const Packet2l& d, const Packet2l& e, const Packet2l& f,
+                                                         const Packet2l& g, const Packet2l& h) {
+  const int16x8_t abcd_s16 = pcast<Packet2l, Packet8s>(a, b, c, d);
+  const int16x8_t efgh_s16 = pcast<Packet2l, Packet8s>(e, f, g, h);
+  return vcombine_s8(vmovn_s16(abcd_s16), vmovn_s16(efgh_s16));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 8, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet2l, Packet16uc>(const Packet2l& a, const Packet2l& b, const Packet2l& c,
+                                                           const Packet2l& d, const Packet2l& e, const Packet2l& f,
+                                                           const Packet2l& g, const Packet2l& h) {
+  const uint16x8_t abcd_u16 = pcast<Packet2l, Packet8us>(a, b, c, d);
+  const uint16x8_t efgh_u16 = pcast<Packet2l, Packet8us>(e, f, g, h);
+  return vcombine_u8(vmovn_u16(abcd_u16), vmovn_u16(efgh_u16));
+}
+
+//==============================================================================
+// pcast, SrcType = uint64_t
+//==============================================================================
+template <>
+struct type_casting_traits<numext::uint64_t, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet2ul, Packet4f>(const Packet2ul& a, const Packet2ul& b) {
+  return vcvtq_f32_u32(vcombine_u32(vmovn_u64(a), vmovn_u64(b)));
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2ul, Packet2ul>(const Packet2ul& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2ul, Packet2l>(const Packet2ul& a) {
+  return vreinterpretq_s64_u64(a);
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet2ul, Packet4ui>(const Packet2ul& a, const Packet2ul& b) {
+  return vcombine_u32(vmovn_u64(a), vmovn_u64(b));
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet2ul, Packet4i>(const Packet2ul& a, const Packet2ul& b) {
+  return vreinterpretq_s32_u32(pcast<Packet2ul, Packet4ui>(a, b));
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet2ul, Packet8us>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
+                                                          const Packet2ul& d) {
+  const uint16x4_t ab_u16 = vmovn_u32(vcombine_u32(vmovn_u64(a), vmovn_u64(b)));
+  const uint16x4_t cd_u16 = vmovn_u32(vcombine_u32(vmovn_u64(c), vmovn_u64(d)));
+  return vcombine_u16(ab_u16, cd_u16);
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet2ul, Packet8s>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
+                                                        const Packet2ul& d) {
+  return vreinterpretq_s16_u16(pcast<Packet2ul, Packet8us>(a, b, c, d));
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 8, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet2ul, Packet16uc>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
+                                                            const Packet2ul& d, const Packet2ul& e, const Packet2ul& f,
+                                                            const Packet2ul& g, const Packet2ul& h) {
+  const uint16x8_t abcd_u16 = pcast<Packet2ul, Packet8us>(a, b, c, d);
+  const uint16x8_t efgh_u16 = pcast<Packet2ul, Packet8us>(e, f, g, h);
+  return vcombine_u8(vmovn_u16(abcd_u16), vmovn_u16(efgh_u16));
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 8, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet2ul, Packet16c>(const Packet2ul& a, const Packet2ul& b, const Packet2ul& c,
+                                                          const Packet2ul& d, const Packet2ul& e, const Packet2ul& f,
+                                                          const Packet2ul& g, const Packet2ul& h) {
+  return vreinterpretq_s8_u8(pcast<Packet2ul, Packet16uc>(a, b, c, d, e, f, g, h));
+}
+
+//==============================================================================
+// preinterpret
+//==============================================================================
+template <>
+EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet2i>(const Packet2i& a) {
+  return vreinterpret_f32_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2f preinterpret<Packet2f, Packet2ui>(const Packet2ui& a) {
+  return vreinterpret_f32_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4i>(const Packet4i& a) {
+  return vreinterpretq_f32_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f, Packet4ui>(const Packet4ui& a) {
+  return vreinterpretq_f32_u32(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4c preinterpret<Packet4c, Packet4uc>(const Packet4uc& a) {
+  return static_cast<Packet4c>(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8c preinterpret<Packet8c, Packet8uc>(const Packet8uc& a) {
+  return vreinterpret_s8_u8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16c preinterpret<Packet16c, Packet16uc>(const Packet16uc& a) {
+  return vreinterpretq_s8_u8(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4uc preinterpret<Packet4uc, Packet4c>(const Packet4c& a) {
+  return static_cast<Packet4uc>(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8uc preinterpret<Packet8uc, Packet8c>(const Packet8c& a) {
+  return vreinterpret_u8_s8(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet16uc preinterpret<Packet16uc, Packet16c>(const Packet16c& a) {
+  return vreinterpretq_u8_s8(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4s preinterpret<Packet4s, Packet4us>(const Packet4us& a) {
+  return vreinterpret_s16_u16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8s preinterpret<Packet8s, Packet8us>(const Packet8us& a) {
+  return vreinterpretq_s16_u16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet4us preinterpret<Packet4us, Packet4s>(const Packet4s& a) {
+  return vreinterpret_u16_s16(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet8us preinterpret<Packet8us, Packet8s>(const Packet8s& a) {
+  return vreinterpretq_u16_s16(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet2f>(const Packet2f& a) {
+  return vreinterpret_s32_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2i preinterpret<Packet2i, Packet2ui>(const Packet2ui& a) {
+  return vreinterpret_s32_u32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4f>(const Packet4f& a) {
+  return vreinterpretq_s32_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet4ui>(const Packet4ui& a) {
+  return vreinterpretq_s32_u32(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet2f>(const Packet2f& a) {
+  return vreinterpret_u32_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ui preinterpret<Packet2ui, Packet2i>(const Packet2i& a) {
+  return vreinterpret_u32_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4f>(const Packet4f& a) {
+  return vreinterpretq_u32_f32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4ui preinterpret<Packet4ui, Packet4i>(const Packet4i& a) {
+  return vreinterpretq_u32_s32(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2l preinterpret<Packet2l, Packet2ul>(const Packet2ul& a) {
+  return vreinterpretq_s64_u64(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul preinterpret<Packet2ul, Packet2l>(const Packet2l& a) {
+  return vreinterpretq_u64_s64(a);
+}
+
+#if EIGEN_ARCH_ARM64
+
+//==============================================================================
+// pcast/preinterpret, Double
+//==============================================================================
+
+template <>
+struct type_casting_traits<double, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet2d, Packet2d>(const Packet2d& a) {
+  return a;
+}
+
+template <>
+struct type_casting_traits<double, float> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4f pcast<Packet2d, Packet4f>(const Packet2d& a, const Packet2d& b) {
+  return vcombine_f32(vcvt_f32_f64(a), vcvt_f32_f64(b));
+}
+
+template <>
+struct type_casting_traits<double, numext::int64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2l pcast<Packet2d, Packet2l>(const Packet2d& a) {
+  return vcvtq_s64_f64(a);
+}
+
+template <>
+struct type_casting_traits<double, numext::uint64_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2ul pcast<Packet2d, Packet2ul>(const Packet2d& a) {
+  return vcvtq_u64_f64(a);
+}
+
+template <>
+struct type_casting_traits<double, numext::int32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4i pcast<Packet2d, Packet4i>(const Packet2d& a, const Packet2d& b) {
+  return vcombine_s32(vmovn_s64(vcvtq_s64_f64(a)), vmovn_s64(vcvtq_s64_f64(b)));
+}
+
+template <>
+struct type_casting_traits<double, numext::uint32_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 2, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet4ui pcast<Packet2d, Packet4ui>(const Packet2d& a, const Packet2d& b) {
+  return vcombine_u32(vmovn_u64(vcvtq_u64_f64(a)), vmovn_u64(vcvtq_u64_f64(b)));
+}
+
+template <>
+struct type_casting_traits<double, numext::int16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8s pcast<Packet2d, Packet8s>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
+                                                       const Packet2d& d) {
+  const int32x4_t ab_s32 = pcast<Packet2d, Packet4i>(a, b);
+  const int32x4_t cd_s32 = pcast<Packet2d, Packet4i>(c, d);
+  return vcombine_s16(vmovn_s32(ab_s32), vmovn_s32(cd_s32));
+}
+
+template <>
+struct type_casting_traits<double, numext::uint16_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 4, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet8us pcast<Packet2d, Packet8us>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
+                                                         const Packet2d& d) {
+  const uint32x4_t ab_u32 = pcast<Packet2d, Packet4ui>(a, b);
+  const uint32x4_t cd_u32 = pcast<Packet2d, Packet4ui>(c, d);
+  return vcombine_u16(vmovn_u32(ab_u32), vmovn_u32(cd_u32));
+}
+
+template <>
+struct type_casting_traits<double, numext::int8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 8, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16c pcast<Packet2d, Packet16c>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
+                                                         const Packet2d& d, const Packet2d& e, const Packet2d& f,
+                                                         const Packet2d& g, const Packet2d& h) {
+  const int16x8_t abcd_s16 = pcast<Packet2d, Packet8s>(a, b, c, d);
+  const int16x8_t efgh_s16 = pcast<Packet2d, Packet8s>(e, f, g, h);
+  return vcombine_s8(vmovn_s16(abcd_s16), vmovn_s16(efgh_s16));
+}
+
+template <>
+struct type_casting_traits<double, numext::uint8_t> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 8, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet16uc pcast<Packet2d, Packet16uc>(const Packet2d& a, const Packet2d& b, const Packet2d& c,
+                                                           const Packet2d& d, const Packet2d& e, const Packet2d& f,
+                                                           const Packet2d& g, const Packet2d& h) {
+  const uint16x8_t abcd_u16 = pcast<Packet2d, Packet8us>(a, b, c, d);
+  const uint16x8_t efgh_u16 = pcast<Packet2d, Packet8us>(e, f, g, h);
+  return vcombine_u8(vmovn_u16(abcd_u16), vmovn_u16(efgh_u16));
+}
+
+template <>
+struct type_casting_traits<float, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet4f, Packet2d>(const Packet4f& a) {
+  // Discard second-half of input.
+  return vcvt_f64_f32(vget_low_f32(a));
+}
+
+template <>
+struct type_casting_traits<numext::int8_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 8 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet16c, Packet2d>(const Packet16c& a) {
+  // Discard all but first two values.
+  return vcvt_f64_f32(pcast<Packet8c, Packet2f>(vget_low_s8(a)));
+}
+
+template <>
+struct type_casting_traits<numext::uint8_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 8 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet16uc, Packet2d>(const Packet16uc& a) {
+  // Discard all but first two values.
+  return vcvt_f64_f32(pcast<Packet8uc, Packet2f>(vget_low_u8(a)));
+}
+
+template <>
+struct type_casting_traits<numext::int16_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet8s, Packet2d>(const Packet8s& a) {
+  // Discard all but first two values.
+  return vcvt_f64_f32(pcast<Packet4s, Packet2f>(vget_low_s16(a)));
+}
+
+template <>
+struct type_casting_traits<numext::uint16_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 4 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet8us, Packet2d>(const Packet8us& a) {
+  // Discard all but first two values.
+  return vcvt_f64_f32(pcast<Packet4us, Packet2f>(vget_low_u16(a)));
+}
+
+template <>
+struct type_casting_traits<numext::int32_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet4i, Packet2d>(const Packet4i& a) {
+  // Discard second half of input.
+  return vcvtq_f64_s64(vmovl_s32(vget_low_s32(a)));
+}
+
+template <>
+struct type_casting_traits<numext::uint32_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 2 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet4ui, Packet2d>(const Packet4ui& a) {
+  // Discard second half of input.
+  return vcvtq_f64_u64(vmovl_u32(vget_low_u32(a)));
+}
+
+template <>
+struct type_casting_traits<numext::int64_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet2l, Packet2d>(const Packet2l& a) {
+  return vcvtq_f64_s64(a);
+}
+
+template <>
+struct type_casting_traits<numext::uint64_t, double> {
+  enum { VectorizedCast = 1, SrcCoeffRatio = 1, TgtCoeffRatio = 1 };
+};
+template <>
+EIGEN_STRONG_INLINE Packet2d pcast<Packet2ul, Packet2d>(const Packet2ul& a) {
+  return vcvtq_f64_u64(a);
+}
+
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet2l>(const Packet2l& a) {
+  return vreinterpretq_f64_s64(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet2ul>(const Packet2ul& a) {
+  return vreinterpretq_f64_u64(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2l preinterpret<Packet2l, Packet2d>(const Packet2d& a) {
+  return vreinterpretq_s64_f64(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2ul preinterpret<Packet2ul, Packet2d>(const Packet2d& a) {
+  return vreinterpretq_u64_f64(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d, Packet4i>(const Packet4i& a) {
+  return vreinterpretq_f64_s32(a);
+}
+template <>
+EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i, Packet2d>(const Packet2d& a) {
+  return vreinterpretq_s32_f64(a);
+}
+
+#endif  // EIGEN_ARCH_ARM64
+
+}  // end namespace internal
+
+}  // end namespace Eigen
+
+#endif  // EIGEN_TYPE_CASTING_NEON_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
index d075043..8fe22da 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/Complex.h
@@ -19,7 +19,7 @@
 {
   EIGEN_STRONG_INLINE Packet2cf() {}
   EIGEN_STRONG_INLINE explicit Packet2cf(const __m128& a) : v(a) {}
-  __m128  v;
+  Packet4f v;
 };
 
 // Use the packet_traits defined in AVX/PacketMath.h instead if we're going
@@ -40,20 +40,33 @@
     HasMul    = 1,
     HasDiv    = 1,
     HasNegate = 1,
+    HasSqrt   = 1,
     HasAbs    = 0,
     HasAbs2   = 0,
     HasMin    = 0,
     HasMax    = 0,
     HasSetLinear = 0,
-    HasBlend = 1
+    HasBlend  = 1
   };
 };
 #endif
 
-template<> struct unpacket_traits<Packet2cf> { typedef std::complex<float> type; enum {size=2, alignment=Aligned16}; typedef Packet2cf half; };
+template<> struct unpacket_traits<Packet2cf> {
+  typedef std::complex<float> type;
+  typedef Packet2cf half;
+  typedef Packet4f as_real;
+  enum {
+    size=2,
+    alignment=Aligned16,
+    vectorizable=true,
+    masked_load_available=false,
+    masked_store_available=false
+  };
+};
 
 template<> EIGEN_STRONG_INLINE Packet2cf padd<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_add_ps(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cf psub<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_sub_ps(a.v,b.v)); }
+
 template<> EIGEN_STRONG_INLINE Packet2cf pnegate(const Packet2cf& a)
 {
   const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x80000000,0x80000000,0x80000000));
@@ -82,10 +95,11 @@
   #endif
 }
 
+template<> EIGEN_STRONG_INLINE Packet2cf ptrue  <Packet2cf>(const Packet2cf& a) { return Packet2cf(ptrue(Packet4f(a.v))); }
 template<> EIGEN_STRONG_INLINE Packet2cf pand   <Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_and_ps(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cf por    <Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_or_ps(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet2cf pxor   <Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_xor_ps(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_andnot_ps(a.v,b.v)); }
+template<> EIGEN_STRONG_INLINE Packet2cf pandnot<Packet2cf>(const Packet2cf& a, const Packet2cf& b) { return Packet2cf(_mm_andnot_ps(b.v,a.v)); }
 
 template<> EIGEN_STRONG_INLINE Packet2cf pload <Packet2cf>(const std::complex<float>* from) { EIGEN_DEBUG_ALIGNED_LOAD return Packet2cf(pload<Packet4f>(&numext::real_ref(*from))); }
 template<> EIGEN_STRONG_INLINE Packet2cf ploadu<Packet2cf>(const std::complex<float>* from) { EIGEN_DEBUG_UNALIGNED_LOAD return Packet2cf(ploadu<Packet4f>(&numext::real_ref(*from))); }
@@ -93,19 +107,13 @@
 template<> EIGEN_STRONG_INLINE Packet2cf pset1<Packet2cf>(const std::complex<float>&  from)
 {
   Packet2cf res;
-#if EIGEN_GNUC_AT_MOST(4,2)
-  // Workaround annoying "may be used uninitialized in this function" warning with gcc 4.2
-  res.v = _mm_loadl_pi(_mm_set1_ps(0.0f), reinterpret_cast<const __m64*>(&from));
-#elif EIGEN_GNUC_AT_LEAST(4,6)
-  // Suppress annoying "may be used uninitialized in this function" warning with gcc >= 4.6
-  #pragma GCC diagnostic push
-  #pragma GCC diagnostic ignored "-Wuninitialized"
-  res.v = _mm_loadl_pi(res.v, (const __m64*)&from);
-  #pragma GCC diagnostic pop
+#ifdef EIGEN_VECTORIZE_SSE3
+  res.v = _mm_castpd_ps(_mm_loaddup_pd(reinterpret_cast<double const*>(&from)));
 #else
-  res.v = _mm_loadl_pi(res.v, (const __m64*)&from);
+  res.v = _mm_castpd_ps(_mm_load_sd(reinterpret_cast<double const*>(&from)));
+  res.v = _mm_movelh_ps(res.v, res.v);
 #endif
-  return Packet2cf(_mm_movelh_ps(res.v,res.v));
+  return res;
 }
 
 template<> EIGEN_STRONG_INLINE Packet2cf ploaddup<Packet2cf>(const std::complex<float>* from) { return pset1<Packet2cf>(*from); }
@@ -152,105 +160,34 @@
   return pfirst(Packet2cf(_mm_add_ps(a.v, _mm_movehl_ps(a.v,a.v))));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf preduxp<Packet2cf>(const Packet2cf* vecs)
-{
-  return Packet2cf(_mm_add_ps(_mm_movelh_ps(vecs[0].v,vecs[1].v), _mm_movehl_ps(vecs[1].v,vecs[0].v)));
-}
-
 template<> EIGEN_STRONG_INLINE std::complex<float> predux_mul<Packet2cf>(const Packet2cf& a)
 {
   return pfirst(pmul(a, Packet2cf(_mm_movehl_ps(a.v,a.v))));
 }
 
-template<int Offset>
-struct palign_impl<Offset,Packet2cf>
-{
-  static EIGEN_STRONG_INLINE void run(Packet2cf& first, const Packet2cf& second)
-  {
-    if (Offset==1)
-    {
-      first.v = _mm_movehl_ps(first.v, first.v);
-      first.v = _mm_movelh_ps(first.v, second.v);
-    }
-  }
-};
-
-template<> struct conj_helper<Packet2cf, Packet2cf, false,true>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& a, const Packet2cf& b) const
-  {
-    #ifdef EIGEN_VECTORIZE_SSE3
-    return internal::pmul(a, pconj(b));
-    #else
-    const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x00000000,0x80000000,0x00000000,0x80000000));
-    return Packet2cf(_mm_add_ps(_mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v), mask),
-                                _mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3),
-                                           vec4f_swizzle1(b.v, 1, 0, 3, 2))));
-    #endif
-  }
-};
-
-template<> struct conj_helper<Packet2cf, Packet2cf, true,false>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& a, const Packet2cf& b) const
-  {
-    #ifdef EIGEN_VECTORIZE_SSE3
-    return internal::pmul(pconj(a), b);
-    #else
-    const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x00000000,0x80000000,0x00000000,0x80000000));
-    return Packet2cf(_mm_add_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v),
-                                _mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3),
-                                                      vec4f_swizzle1(b.v, 1, 0, 3, 2)), mask)));
-    #endif
-  }
-};
-
-template<> struct conj_helper<Packet2cf, Packet2cf, true,true>
-{
-  EIGEN_STRONG_INLINE Packet2cf pmadd(const Packet2cf& x, const Packet2cf& y, const Packet2cf& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet2cf pmul(const Packet2cf& a, const Packet2cf& b) const
-  {
-    #ifdef EIGEN_VECTORIZE_SSE3
-    return pconj(internal::pmul(a, b));
-    #else
-    const __m128 mask = _mm_castsi128_ps(_mm_setr_epi32(0x00000000,0x80000000,0x00000000,0x80000000));
-    return Packet2cf(_mm_sub_ps(_mm_xor_ps(_mm_mul_ps(vec4f_swizzle1(a.v, 0, 0, 2, 2), b.v), mask),
-                                _mm_mul_ps(vec4f_swizzle1(a.v, 1, 1, 3, 3),
-                                           vec4f_swizzle1(b.v, 1, 0, 3, 2))));
-    #endif
-  }
-};
-
-EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
-
-template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
-{
-  // TODO optimize it for SSE3 and 4
-  Packet2cf res = conj_helper<Packet2cf,Packet2cf,false,true>().pmul(a,b);
-  __m128 s = _mm_mul_ps(b.v,b.v);
-  return Packet2cf(_mm_div_ps(res.v,_mm_add_ps(s,_mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(s), 0xb1)))));
-}
-
 EIGEN_STRONG_INLINE Packet2cf pcplxflip/* <Packet2cf> */(const Packet2cf& x)
 {
   return Packet2cf(vec4f_swizzle1(x.v, 1, 0, 3, 2));
 }
 
+EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet2cf,Packet4f)
+
+template<> EIGEN_STRONG_INLINE Packet2cf pdiv<Packet2cf>(const Packet2cf& a, const Packet2cf& b)
+{
+  // TODO optimize it for SSE3 and 4
+  Packet2cf res = pmul(a, pconj(b));
+  __m128 s = _mm_mul_ps(b.v,b.v);
+  return Packet2cf(_mm_div_ps(res.v,_mm_add_ps(s,vec4f_swizzle1(s, 1, 0, 3, 2))));
+}
+
+
 
 //---------- double ----------
 struct Packet1cd
 {
   EIGEN_STRONG_INLINE Packet1cd() {}
   EIGEN_STRONG_INLINE explicit Packet1cd(const __m128d& a) : v(a) {}
-  __m128d  v;
+  Packet2d v;
 };
 
 // Use the packet_traits defined in AVX/PacketMath.h instead if we're going
@@ -271,6 +208,7 @@
     HasMul    = 1,
     HasDiv    = 1,
     HasNegate = 1,
+    HasSqrt   = 1,
     HasAbs    = 0,
     HasAbs2   = 0,
     HasMin    = 0,
@@ -280,7 +218,18 @@
 };
 #endif
 
-template<> struct unpacket_traits<Packet1cd> { typedef std::complex<double> type; enum {size=1, alignment=Aligned16}; typedef Packet1cd half; };
+template<> struct unpacket_traits<Packet1cd> {
+  typedef std::complex<double> type;
+  typedef Packet1cd half;
+  typedef Packet2d as_real;
+  enum {
+    size=1,
+    alignment=Aligned16,
+    vectorizable=true,
+    masked_load_available=false,
+    masked_store_available=false
+  };
+};
 
 template<> EIGEN_STRONG_INLINE Packet1cd padd<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_add_pd(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet1cd psub<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_sub_pd(a.v,b.v)); }
@@ -305,10 +254,11 @@
   #endif
 }
 
+template<> EIGEN_STRONG_INLINE Packet1cd ptrue  <Packet1cd>(const Packet1cd& a) { return Packet1cd(ptrue(Packet2d(a.v))); }
 template<> EIGEN_STRONG_INLINE Packet1cd pand   <Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_and_pd(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet1cd por    <Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_or_pd(a.v,b.v)); }
 template<> EIGEN_STRONG_INLINE Packet1cd pxor   <Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_xor_pd(a.v,b.v)); }
-template<> EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_andnot_pd(a.v,b.v)); }
+template<> EIGEN_STRONG_INLINE Packet1cd pandnot<Packet1cd>(const Packet1cd& a, const Packet1cd& b) { return Packet1cd(_mm_andnot_pd(b.v,a.v)); }
 
 // FIXME force unaligned load, this is a temporary fix
 template<> EIGEN_STRONG_INLINE Packet1cd pload <Packet1cd>(const std::complex<double>* from)
@@ -340,86 +290,17 @@
   return pfirst(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd preduxp<Packet1cd>(const Packet1cd* vecs)
-{
-  return vecs[0];
-}
-
 template<> EIGEN_STRONG_INLINE std::complex<double> predux_mul<Packet1cd>(const Packet1cd& a)
 {
   return pfirst(a);
 }
 
-template<int Offset>
-struct palign_impl<Offset,Packet1cd>
-{
-  static EIGEN_STRONG_INLINE void run(Packet1cd& /*first*/, const Packet1cd& /*second*/)
-  {
-    // FIXME is it sure we never have to align a Packet1cd?
-    // Even though a std::complex<double> has 16 bytes, it is not necessarily aligned on a 16 bytes boundary...
-  }
-};
-
-template<> struct conj_helper<Packet1cd, Packet1cd, false,true>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& a, const Packet1cd& b) const
-  {
-    #ifdef EIGEN_VECTORIZE_SSE3
-    return internal::pmul(a, pconj(b));
-    #else
-    const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0));
-    return Packet1cd(_mm_add_pd(_mm_xor_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 0, 0), b.v), mask),
-                                _mm_mul_pd(vec2d_swizzle1(a.v, 1, 1),
-                                           vec2d_swizzle1(b.v, 1, 0))));
-    #endif
-  }
-};
-
-template<> struct conj_helper<Packet1cd, Packet1cd, true,false>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& a, const Packet1cd& b) const
-  {
-    #ifdef EIGEN_VECTORIZE_SSE3
-    return internal::pmul(pconj(a), b);
-    #else
-    const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0));
-    return Packet1cd(_mm_add_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 0, 0), b.v),
-                                _mm_xor_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 1, 1),
-                                                      vec2d_swizzle1(b.v, 1, 0)), mask)));
-    #endif
-  }
-};
-
-template<> struct conj_helper<Packet1cd, Packet1cd, true,true>
-{
-  EIGEN_STRONG_INLINE Packet1cd pmadd(const Packet1cd& x, const Packet1cd& y, const Packet1cd& c) const
-  { return padd(pmul(x,y),c); }
-
-  EIGEN_STRONG_INLINE Packet1cd pmul(const Packet1cd& a, const Packet1cd& b) const
-  {
-    #ifdef EIGEN_VECTORIZE_SSE3
-    return pconj(internal::pmul(a, b));
-    #else
-    const __m128d mask = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0));
-    return Packet1cd(_mm_sub_pd(_mm_xor_pd(_mm_mul_pd(vec2d_swizzle1(a.v, 0, 0), b.v), mask),
-                                _mm_mul_pd(vec2d_swizzle1(a.v, 1, 1),
-                                           vec2d_swizzle1(b.v, 1, 0))));
-    #endif
-  }
-};
-
 EIGEN_MAKE_CONJ_HELPER_CPLX_REAL(Packet1cd,Packet2d)
 
 template<> EIGEN_STRONG_INLINE Packet1cd pdiv<Packet1cd>(const Packet1cd& a, const Packet1cd& b)
 {
   // TODO optimize it for SSE3 and 4
-  Packet1cd res = conj_helper<Packet1cd,Packet1cd,false,true>().pmul(a,b);
+  Packet1cd res = pmul(a,pconj(b));
   __m128d s = _mm_mul_pd(b.v,b.v);
   return Packet1cd(_mm_div_pd(res.v, _mm_add_pd(s,_mm_shuffle_pd(s, s, 0x1))));
 }
@@ -439,33 +320,32 @@
   kernel.packet[1].v = tmp;
 }
 
+template<> EIGEN_STRONG_INLINE Packet2cf pcmp_eq(const Packet2cf& a, const Packet2cf& b)
+{
+  __m128 eq = _mm_cmpeq_ps(a.v, b.v);
+  return Packet2cf(pand<Packet4f>(eq, vec4f_swizzle1(eq, 1, 0, 3, 2)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet1cd pcmp_eq(const Packet1cd& a, const Packet1cd& b)
+{
+  __m128d eq = _mm_cmpeq_pd(a.v, b.v);
+  return Packet1cd(pand<Packet2d>(eq, vec2d_swizzle1(eq, 1, 0)));
+}
+
 template<>  EIGEN_STRONG_INLINE Packet2cf pblend(const Selector<2>& ifPacket, const Packet2cf& thenPacket, const Packet2cf& elsePacket) {
   __m128d result = pblend<Packet2d>(ifPacket, _mm_castps_pd(thenPacket.v), _mm_castps_pd(elsePacket.v));
   return Packet2cf(_mm_castpd_ps(result));
 }
 
-template<> EIGEN_STRONG_INLINE Packet2cf pinsertfirst(const Packet2cf& a, std::complex<float> b)
-{
-  return Packet2cf(_mm_loadl_pi(a.v, reinterpret_cast<const __m64*>(&b)));
+template<> EIGEN_STRONG_INLINE Packet1cd psqrt<Packet1cd>(const Packet1cd& a) {
+  return psqrt_complex<Packet1cd>(a);
 }
 
-template<> EIGEN_STRONG_INLINE Packet1cd pinsertfirst(const Packet1cd&, std::complex<double> b)
-{
-  return pset1<Packet1cd>(b);
-}
-
-template<> EIGEN_STRONG_INLINE Packet2cf pinsertlast(const Packet2cf& a, std::complex<float> b)
-{
-  return Packet2cf(_mm_loadh_pi(a.v, reinterpret_cast<const __m64*>(&b)));
-}
-
-template<> EIGEN_STRONG_INLINE Packet1cd pinsertlast(const Packet1cd&, std::complex<double> b)
-{
-  return pset1<Packet1cd>(b);
+template<> EIGEN_STRONG_INLINE Packet2cf psqrt<Packet2cf>(const Packet2cf& a) {
+  return psqrt_complex<Packet2cf>(a);
 }
 
 } // end namespace internal
-
 } // end namespace Eigen
 
 #endif // EIGEN_COMPLEX_SSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
index 7b5f948..8736d0d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/MathFunctions.h
@@ -8,7 +8,7 @@
 // Public License v. 2.0. If a copy of the MPL was not distributed
 // with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
-/* The sin, cos, exp, and log functions of this file come from
+/* The sin and cos and functions of this file come from
  * Julien Pommier's sse math library: http://gruntthepeon.free.fr/ssemath/
  */
 
@@ -20,426 +20,57 @@
 namespace internal {
 
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet4f plog<Packet4f>(const Packet4f& _x)
-{
-  Packet4f x = _x;
-  _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
-  _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
-  _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f);
+Packet4f plog<Packet4f>(const Packet4f& _x) {
+  return plog_float(_x);
+}
 
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(inv_mant_mask, ~0x7f800000);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet2d plog<Packet2d>(const Packet2d& _x) {
+  return plog_double(_x);
+}
 
-  /* the smallest non denormalized float number */
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(min_norm_pos,  0x00800000);
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(minus_inf,     0xff800000);//-1.f/0.f);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet4f plog2<Packet4f>(const Packet4f& _x) {
+  return plog2_float(_x);
+}
 
-  /* natural logarithm computed for 4 simultaneous float
-    return NaN for x <= 0
-  */
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_SQRTHF, 0.707106781186547524f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p0, 7.0376836292E-2f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p1, - 1.1514610310E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p2, 1.1676998740E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p3, - 1.2420140846E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p4, + 1.4249322787E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p5, - 1.6668057665E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p6, + 2.0000714765E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p7, - 2.4999993993E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_p8, + 3.3333331174E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_q1, -2.12194440e-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_log_q2, 0.693359375f);
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet2d plog2<Packet2d>(const Packet2d& _x) {
+  return plog2_double(_x);
+}
 
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet4f plog1p<Packet4f>(const Packet4f& _x) {
+  return generic_plog1p(_x);
+}
 
-  Packet4i emm0;
-
-  Packet4f invalid_mask = _mm_cmpnge_ps(x, _mm_setzero_ps()); // not greater equal is true if x is NaN
-  Packet4f iszero_mask = _mm_cmpeq_ps(x, _mm_setzero_ps());
-
-  x = pmax(x, p4f_min_norm_pos);  /* cut off denormalized stuff */
-  emm0 = _mm_srli_epi32(_mm_castps_si128(x), 23);
-
-  /* keep only the fractional part */
-  x = _mm_and_ps(x, p4f_inv_mant_mask);
-  x = _mm_or_ps(x, p4f_half);
-
-  emm0 = _mm_sub_epi32(emm0, p4i_0x7f);
-  Packet4f e = padd(Packet4f(_mm_cvtepi32_ps(emm0)), p4f_1);
-
-  /* part2:
-     if( x < SQRTHF ) {
-       e -= 1;
-       x = x + x - 1.0;
-     } else { x = x - 1.0; }
-  */
-  Packet4f mask = _mm_cmplt_ps(x, p4f_cephes_SQRTHF);
-  Packet4f tmp = pand(x, mask);
-  x = psub(x, p4f_1);
-  e = psub(e, pand(p4f_1, mask));
-  x = padd(x, tmp);
-
-  Packet4f x2 = pmul(x,x);
-  Packet4f x3 = pmul(x2,x);
-
-  Packet4f y, y1, y2;
-  y  = pmadd(p4f_cephes_log_p0, x, p4f_cephes_log_p1);
-  y1 = pmadd(p4f_cephes_log_p3, x, p4f_cephes_log_p4);
-  y2 = pmadd(p4f_cephes_log_p6, x, p4f_cephes_log_p7);
-  y  = pmadd(y , x, p4f_cephes_log_p2);
-  y1 = pmadd(y1, x, p4f_cephes_log_p5);
-  y2 = pmadd(y2, x, p4f_cephes_log_p8);
-  y = pmadd(y, x3, y1);
-  y = pmadd(y, x3, y2);
-  y = pmul(y, x3);
-
-  y1 = pmul(e, p4f_cephes_log_q1);
-  tmp = pmul(x2, p4f_half);
-  y = padd(y, y1);
-  x = psub(x, tmp);
-  y2 = pmul(e, p4f_cephes_log_q2);
-  x = padd(x, y);
-  x = padd(x, y2);
-  // negative arg will be NAN, 0 will be -INF
-  return _mm_or_ps(_mm_andnot_ps(iszero_mask, _mm_or_ps(x, invalid_mask)),
-                   _mm_and_ps(iszero_mask, p4f_minus_inf));
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet4f pexpm1<Packet4f>(const Packet4f& _x) {
+  return generic_expm1(_x);
 }
 
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f pexp<Packet4f>(const Packet4f& _x)
 {
-  Packet4f x = _x;
-  _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
-  _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
-  _EIGEN_DECLARE_CONST_Packet4i(0x7f, 0x7f);
-
-
-  _EIGEN_DECLARE_CONST_Packet4f(exp_hi,  88.3762626647950f);
-  _EIGEN_DECLARE_CONST_Packet4f(exp_lo, -88.3762626647949f);
-
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_LOG2EF, 1.44269504088896341f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C1, 0.693359375f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_C2, -2.12194440e-4f);
-
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p0, 1.9875691500E-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p1, 1.3981999507E-3f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p2, 8.3334519073E-3f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p3, 4.1665795894E-2f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p4, 1.6666665459E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_exp_p5, 5.0000001201E-1f);
-
-  Packet4f tmp, fx;
-  Packet4i emm0;
-
-  // clamp x
-  x = pmax(pmin(x, p4f_exp_hi), p4f_exp_lo);
-
-  /* express exp(x) as exp(g + n*log(2)) */
-  fx = pmadd(x, p4f_cephes_LOG2EF, p4f_half);
-
-#ifdef EIGEN_VECTORIZE_SSE4_1
-  fx = _mm_floor_ps(fx);
-#else
-  emm0 = _mm_cvttps_epi32(fx);
-  tmp  = _mm_cvtepi32_ps(emm0);
-  /* if greater, substract 1 */
-  Packet4f mask = _mm_cmpgt_ps(tmp, fx);
-  mask = _mm_and_ps(mask, p4f_1);
-  fx = psub(tmp, mask);
-#endif
-
-  tmp = pmul(fx, p4f_cephes_exp_C1);
-  Packet4f z = pmul(fx, p4f_cephes_exp_C2);
-  x = psub(x, tmp);
-  x = psub(x, z);
-
-  z = pmul(x,x);
-
-  Packet4f y = p4f_cephes_exp_p0;
-  y = pmadd(y, x, p4f_cephes_exp_p1);
-  y = pmadd(y, x, p4f_cephes_exp_p2);
-  y = pmadd(y, x, p4f_cephes_exp_p3);
-  y = pmadd(y, x, p4f_cephes_exp_p4);
-  y = pmadd(y, x, p4f_cephes_exp_p5);
-  y = pmadd(y, z, x);
-  y = padd(y, p4f_1);
-
-  // build 2^n
-  emm0 = _mm_cvttps_epi32(fx);
-  emm0 = _mm_add_epi32(emm0, p4i_0x7f);
-  emm0 = _mm_slli_epi32(emm0, 23);
-  return pmax(pmul(y, Packet4f(_mm_castsi128_ps(emm0))), _x);
+  return pexp_float(_x);
 }
+
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
-Packet2d pexp<Packet2d>(const Packet2d& _x)
+Packet2d pexp<Packet2d>(const Packet2d& x)
 {
-  Packet2d x = _x;
-
-  _EIGEN_DECLARE_CONST_Packet2d(1 , 1.0);
-  _EIGEN_DECLARE_CONST_Packet2d(2 , 2.0);
-  _EIGEN_DECLARE_CONST_Packet2d(half, 0.5);
-
-  _EIGEN_DECLARE_CONST_Packet2d(exp_hi,  709.437);
-  _EIGEN_DECLARE_CONST_Packet2d(exp_lo, -709.436139303);
-
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_LOG2EF, 1.4426950408889634073599);
-
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_p0, 1.26177193074810590878e-4);
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_p1, 3.02994407707441961300e-2);
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_p2, 9.99999999999999999910e-1);
-
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_q0, 3.00198505138664455042e-6);
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_q1, 2.52448340349684104192e-3);
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_q2, 2.27265548208155028766e-1);
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_q3, 2.00000000000000000009e0);
-
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_C1, 0.693145751953125);
-  _EIGEN_DECLARE_CONST_Packet2d(cephes_exp_C2, 1.42860682030941723212e-6);
-  static const __m128i p4i_1023_0 = _mm_setr_epi32(1023, 1023, 0, 0);
-
-  Packet2d tmp, fx;
-  Packet4i emm0;
-
-  // clamp x
-  x = pmax(pmin(x, p2d_exp_hi), p2d_exp_lo);
-  /* express exp(x) as exp(g + n*log(2)) */
-  fx = pmadd(p2d_cephes_LOG2EF, x, p2d_half);
-
-#ifdef EIGEN_VECTORIZE_SSE4_1
-  fx = _mm_floor_pd(fx);
-#else
-  emm0 = _mm_cvttpd_epi32(fx);
-  tmp  = _mm_cvtepi32_pd(emm0);
-  /* if greater, substract 1 */
-  Packet2d mask = _mm_cmpgt_pd(tmp, fx);
-  mask = _mm_and_pd(mask, p2d_1);
-  fx = psub(tmp, mask);
-#endif
-
-  tmp = pmul(fx, p2d_cephes_exp_C1);
-  Packet2d z = pmul(fx, p2d_cephes_exp_C2);
-  x = psub(x, tmp);
-  x = psub(x, z);
-
-  Packet2d x2 = pmul(x,x);
-
-  Packet2d px = p2d_cephes_exp_p0;
-  px = pmadd(px, x2, p2d_cephes_exp_p1);
-  px = pmadd(px, x2, p2d_cephes_exp_p2);
-  px = pmul (px, x);
-
-  Packet2d qx = p2d_cephes_exp_q0;
-  qx = pmadd(qx, x2, p2d_cephes_exp_q1);
-  qx = pmadd(qx, x2, p2d_cephes_exp_q2);
-  qx = pmadd(qx, x2, p2d_cephes_exp_q3);
-
-  x = pdiv(px,psub(qx,px));
-  x = pmadd(p2d_2,x,p2d_1);
-
-  // build 2^n
-  emm0 = _mm_cvttpd_epi32(fx);
-  emm0 = _mm_add_epi32(emm0, p4i_1023_0);
-  emm0 = _mm_slli_epi32(emm0, 20);
-  emm0 = _mm_shuffle_epi32(emm0, _MM_SHUFFLE(1,2,0,3));
-  return pmax(pmul(x, Packet2d(_mm_castsi128_pd(emm0))), _x);
+  return pexp_double(x);
 }
 
-/* evaluation of 4 sines at onces, using SSE2 intrinsics.
-
-   The code is the exact rewriting of the cephes sinf function.
-   Precision is excellent as long as x < 8192 (I did not bother to
-   take into account the special handling they have for greater values
-   -- it does not return garbage for arguments over 8192, though, but
-   the extra precision is missing).
-
-   Note that it is such that sinf((float)M_PI) = 8.74e-8, which is the
-   surprising but correct result.
-*/
-
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f psin<Packet4f>(const Packet4f& _x)
 {
-  Packet4f x = _x;
-  _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
-  _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
-
-  _EIGEN_DECLARE_CONST_Packet4i(1, 1);
-  _EIGEN_DECLARE_CONST_Packet4i(not1, ~1);
-  _EIGEN_DECLARE_CONST_Packet4i(2, 2);
-  _EIGEN_DECLARE_CONST_Packet4i(4, 4);
-
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(sign_mask, 0x80000000);
-
-  _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP1,-0.78515625f);
-  _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP2, -2.4187564849853515625e-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP3, -3.77489497744594108e-8f);
-  _EIGEN_DECLARE_CONST_Packet4f(sincof_p0, -1.9515295891E-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(sincof_p1,  8.3321608736E-3f);
-  _EIGEN_DECLARE_CONST_Packet4f(sincof_p2, -1.6666654611E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(coscof_p0,  2.443315711809948E-005f);
-  _EIGEN_DECLARE_CONST_Packet4f(coscof_p1, -1.388731625493765E-003f);
-  _EIGEN_DECLARE_CONST_Packet4f(coscof_p2,  4.166664568298827E-002f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
-
-  Packet4f xmm1, xmm2, xmm3, sign_bit, y;
-
-  Packet4i emm0, emm2;
-  sign_bit = x;
-  /* take the absolute value */
-  x = pabs(x);
-
-  /* take the modulo */
-
-  /* extract the sign bit (upper one) */
-  sign_bit = _mm_and_ps(sign_bit, p4f_sign_mask);
-
-  /* scale by 4/Pi */
-  y = pmul(x, p4f_cephes_FOPI);
-
-  /* store the integer part of y in mm0 */
-  emm2 = _mm_cvttps_epi32(y);
-  /* j=(j+1) & (~1) (see the cephes sources) */
-  emm2 = _mm_add_epi32(emm2, p4i_1);
-  emm2 = _mm_and_si128(emm2, p4i_not1);
-  y = _mm_cvtepi32_ps(emm2);
-  /* get the swap sign flag */
-  emm0 = _mm_and_si128(emm2, p4i_4);
-  emm0 = _mm_slli_epi32(emm0, 29);
-  /* get the polynom selection mask
-     there is one polynom for 0 <= x <= Pi/4
-     and another one for Pi/4<x<=Pi/2
-
-     Both branches will be computed.
-  */
-  emm2 = _mm_and_si128(emm2, p4i_2);
-  emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
-
-  Packet4f swap_sign_bit = _mm_castsi128_ps(emm0);
-  Packet4f poly_mask = _mm_castsi128_ps(emm2);
-  sign_bit = _mm_xor_ps(sign_bit, swap_sign_bit);
-
-  /* The magic pass: "Extended precision modular arithmetic"
-     x = ((x - y * DP1) - y * DP2) - y * DP3; */
-  xmm1 = pmul(y, p4f_minus_cephes_DP1);
-  xmm2 = pmul(y, p4f_minus_cephes_DP2);
-  xmm3 = pmul(y, p4f_minus_cephes_DP3);
-  x = padd(x, xmm1);
-  x = padd(x, xmm2);
-  x = padd(x, xmm3);
-
-  /* Evaluate the first polynom  (0 <= x <= Pi/4) */
-  y = p4f_coscof_p0;
-  Packet4f z = _mm_mul_ps(x,x);
-
-  y = pmadd(y, z, p4f_coscof_p1);
-  y = pmadd(y, z, p4f_coscof_p2);
-  y = pmul(y, z);
-  y = pmul(y, z);
-  Packet4f tmp = pmul(z, p4f_half);
-  y = psub(y, tmp);
-  y = padd(y, p4f_1);
-
-  /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
-
-  Packet4f y2 = p4f_sincof_p0;
-  y2 = pmadd(y2, z, p4f_sincof_p1);
-  y2 = pmadd(y2, z, p4f_sincof_p2);
-  y2 = pmul(y2, z);
-  y2 = pmul(y2, x);
-  y2 = padd(y2, x);
-
-  /* select the correct result from the two polynoms */
-  y2 = _mm_and_ps(poly_mask, y2);
-  y = _mm_andnot_ps(poly_mask, y);
-  y = _mm_or_ps(y,y2);
-  /* update the sign */
-  return _mm_xor_ps(y, sign_bit);
+  return psin_float(_x);
 }
 
-/* almost the same as psin */
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f pcos<Packet4f>(const Packet4f& _x)
 {
-  Packet4f x = _x;
-  _EIGEN_DECLARE_CONST_Packet4f(1 , 1.0f);
-  _EIGEN_DECLARE_CONST_Packet4f(half, 0.5f);
-
-  _EIGEN_DECLARE_CONST_Packet4i(1, 1);
-  _EIGEN_DECLARE_CONST_Packet4i(not1, ~1);
-  _EIGEN_DECLARE_CONST_Packet4i(2, 2);
-  _EIGEN_DECLARE_CONST_Packet4i(4, 4);
-
-  _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP1,-0.78515625f);
-  _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP2, -2.4187564849853515625e-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(minus_cephes_DP3, -3.77489497744594108e-8f);
-  _EIGEN_DECLARE_CONST_Packet4f(sincof_p0, -1.9515295891E-4f);
-  _EIGEN_DECLARE_CONST_Packet4f(sincof_p1,  8.3321608736E-3f);
-  _EIGEN_DECLARE_CONST_Packet4f(sincof_p2, -1.6666654611E-1f);
-  _EIGEN_DECLARE_CONST_Packet4f(coscof_p0,  2.443315711809948E-005f);
-  _EIGEN_DECLARE_CONST_Packet4f(coscof_p1, -1.388731625493765E-003f);
-  _EIGEN_DECLARE_CONST_Packet4f(coscof_p2,  4.166664568298827E-002f);
-  _EIGEN_DECLARE_CONST_Packet4f(cephes_FOPI, 1.27323954473516f); // 4 / M_PI
-
-  Packet4f xmm1, xmm2, xmm3, y;
-  Packet4i emm0, emm2;
-
-  x = pabs(x);
-
-  /* scale by 4/Pi */
-  y = pmul(x, p4f_cephes_FOPI);
-
-  /* get the integer part of y */
-  emm2 = _mm_cvttps_epi32(y);
-  /* j=(j+1) & (~1) (see the cephes sources) */
-  emm2 = _mm_add_epi32(emm2, p4i_1);
-  emm2 = _mm_and_si128(emm2, p4i_not1);
-  y = _mm_cvtepi32_ps(emm2);
-
-  emm2 = _mm_sub_epi32(emm2, p4i_2);
-
-  /* get the swap sign flag */
-  emm0 = _mm_andnot_si128(emm2, p4i_4);
-  emm0 = _mm_slli_epi32(emm0, 29);
-  /* get the polynom selection mask */
-  emm2 = _mm_and_si128(emm2, p4i_2);
-  emm2 = _mm_cmpeq_epi32(emm2, _mm_setzero_si128());
-
-  Packet4f sign_bit = _mm_castsi128_ps(emm0);
-  Packet4f poly_mask = _mm_castsi128_ps(emm2);
-
-  /* The magic pass: "Extended precision modular arithmetic"
-     x = ((x - y * DP1) - y * DP2) - y * DP3; */
-  xmm1 = pmul(y, p4f_minus_cephes_DP1);
-  xmm2 = pmul(y, p4f_minus_cephes_DP2);
-  xmm3 = pmul(y, p4f_minus_cephes_DP3);
-  x = padd(x, xmm1);
-  x = padd(x, xmm2);
-  x = padd(x, xmm3);
-
-  /* Evaluate the first polynom  (0 <= x <= Pi/4) */
-  y = p4f_coscof_p0;
-  Packet4f z = pmul(x,x);
-
-  y = pmadd(y,z,p4f_coscof_p1);
-  y = pmadd(y,z,p4f_coscof_p2);
-  y = pmul(y, z);
-  y = pmul(y, z);
-  Packet4f tmp = _mm_mul_ps(z, p4f_half);
-  y = psub(y, tmp);
-  y = padd(y, p4f_1);
-
-  /* Evaluate the second polynom  (Pi/4 <= x <= 0) */
-  Packet4f y2 = p4f_sincof_p0;
-  y2 = pmadd(y2, z, p4f_sincof_p1);
-  y2 = pmadd(y2, z, p4f_sincof_p2);
-  y2 = pmul(y2, z);
-  y2 = pmadd(y2, x, x);
-
-  /* select the correct result from the two polynoms */
-  y2 = _mm_and_ps(poly_mask, y2);
-  y  = _mm_andnot_ps(poly_mask, y);
-  y  = _mm_or_ps(y,y2);
-
-  /* update the sign */
-  return _mm_xor_ps(y, sign_bit);
+  return pcos_float(_x);
 }
 
 #if EIGEN_FAST_MATH
@@ -455,17 +86,17 @@
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f psqrt<Packet4f>(const Packet4f& _x)
 {
-  Packet4f half = pmul(_x, pset1<Packet4f>(.5f));
-  Packet4f denormal_mask = _mm_and_ps(
-      _mm_cmpge_ps(_x, _mm_setzero_ps()),
-      _mm_cmplt_ps(_x, pset1<Packet4f>((std::numeric_limits<float>::min)())));
+  Packet4f minus_half_x = pmul(_x, pset1<Packet4f>(-0.5f));
+  Packet4f denormal_mask = pandnot(
+      pcmp_lt(_x, pset1<Packet4f>((std::numeric_limits<float>::min)())),
+      pcmp_lt(_x, pzero(_x)));
 
   // Compute approximate reciprocal sqrt.
   Packet4f x = _mm_rsqrt_ps(_x);
   // Do a single step of Newton's iteration.
-  x = pmul(x, psub(pset1<Packet4f>(1.5f), pmul(half, pmul(x,x))));
+  x = pmul(x, pmadd(minus_half_x, pmul(x,x), pset1<Packet4f>(1.5f)));
   // Flush results for denormals to zero.
-  return _mm_andnot_ps(denormal_mask, pmul(_x,x));
+  return pandnot(pmul(_x,x), denormal_mask);
 }
 
 #else
@@ -478,41 +109,48 @@
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet2d psqrt<Packet2d>(const Packet2d& x) { return _mm_sqrt_pd(x); }
 
+template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
+Packet16b psqrt<Packet16b>(const Packet16b& x) { return x; }
+
 #if EIGEN_FAST_MATH
 
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f prsqrt<Packet4f>(const Packet4f& _x) {
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(inf, 0x7f800000);
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(nan, 0x7fc00000);
   _EIGEN_DECLARE_CONST_Packet4f(one_point_five, 1.5f);
   _EIGEN_DECLARE_CONST_Packet4f(minus_half, -0.5f);
-  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(flt_min, 0x00800000);
+  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(inf, 0x7f800000u);
+  _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(flt_min, 0x00800000u);
 
   Packet4f neg_half = pmul(_x, p4f_minus_half);
 
-  // select only the inverse sqrt of positive normal inputs (denormals are
-  // flushed to zero and cause infs as well).
-  Packet4f le_zero_mask = _mm_cmple_ps(_x, p4f_flt_min);
-  Packet4f x = _mm_andnot_ps(le_zero_mask, _mm_rsqrt_ps(_x));
+  // Identity infinite, zero, negative and denormal arguments.
+  Packet4f lt_min_mask = _mm_cmplt_ps(_x, p4f_flt_min);
+  Packet4f inf_mask = _mm_cmpeq_ps(_x, p4f_inf);
+  Packet4f not_normal_finite_mask = _mm_or_ps(lt_min_mask, inf_mask);
 
-  // Fill in NaNs and Infs for the negative/zero entries.
-  Packet4f neg_mask = _mm_cmplt_ps(_x, _mm_setzero_ps());
-  Packet4f zero_mask = _mm_andnot_ps(neg_mask, le_zero_mask);
-  Packet4f infs_and_nans = _mm_or_ps(_mm_and_ps(neg_mask, p4f_nan),
-                                     _mm_and_ps(zero_mask, p4f_inf));
+  // Compute an approximate result using the rsqrt intrinsic.
+  Packet4f y_approx = _mm_rsqrt_ps(_x);
 
-  // Do a single step of Newton's iteration.
-  x = pmul(x, pmadd(neg_half, pmul(x, x), p4f_one_point_five));
+  // Do a single step of Newton-Raphson iteration to improve the approximation.
+  // This uses the formula y_{n+1} = y_n * (1.5 - y_n * (0.5 * x) * y_n).
+  // It is essential to evaluate the inner term like this because forming
+  // y_n^2 may over- or underflow.
+  Packet4f y_newton = pmul(
+      y_approx, pmadd(y_approx, pmul(neg_half, y_approx), p4f_one_point_five));
 
-  // Insert NaNs and Infs in all the right places.
-  return _mm_or_ps(x, infs_and_nans);
+  // Select the result of the Newton-Raphson step for positive normal arguments.
+  // For other arguments, choose the output of the intrinsic. This will
+  // return rsqrt(+inf) = 0, rsqrt(x) = NaN if x < 0, and rsqrt(x) = +inf if
+  // x is zero or a positive denormalized float (equivalent to flushing positive
+  // denormalized inputs to zero).
+  return pselect<Packet4f>(not_normal_finite_mask, y_approx, y_newton);
 }
 
 #else
 
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet4f prsqrt<Packet4f>(const Packet4f& x) {
-  // Unfortunately we can't use the much faster mm_rqsrt_ps since it only provides an approximation.
+  // Unfortunately we can't use the much faster mm_rsqrt_ps since it only provides an approximation.
   return _mm_div_ps(pset1<Packet4f>(1.0f), _mm_sqrt_ps(x));
 }
 
@@ -520,7 +158,6 @@
 
 template<> EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_UNUSED
 Packet2d prsqrt<Packet2d>(const Packet2d& x) {
-  // Unfortunately we can't use the much faster mm_rqsrt_pd since it only provides an approximation.
   return _mm_div_pd(pset1<Packet2d>(1.0), _mm_sqrt_pd(x));
 }
 
@@ -548,7 +185,7 @@
 {
 #if EIGEN_COMP_GNUC_STRICT
   // This works around a GCC bug generating poor code for _mm_sqrt_pd
-  // See https://bitbucket.org/eigen/eigen/commits/14f468dba4d350d7c19c9b93072e19f7b3df563b
+  // See https://gitlab.com/libeigen/eigen/commit/8dca9f97e38970
   return internal::pfirst(internal::Packet2d(__builtin_ia32_sqrtsd(_mm_set_sd(x))));
 #else
   return internal::pfirst(internal::Packet2d(_mm_sqrt_pd(_mm_set_sd(x))));
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
index 60e2517..db102c7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/PacketMath.h
@@ -18,13 +18,15 @@
 #define EIGEN_CACHEFRIENDLY_PRODUCT_THRESHOLD 8
 #endif
 
-#ifndef EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS
+#if !defined(EIGEN_VECTORIZE_AVX) && !defined(EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS)
+// 32 bits =>  8 registers
+// 64 bits => 16 registers
 #define EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS (2*sizeof(void*))
 #endif
 
-#ifdef __FMA__
+#ifdef EIGEN_VECTORIZE_FMA
 #ifndef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
-#define EIGEN_HAS_SINGLE_INSTRUCTION_MADD 1
+#define EIGEN_HAS_SINGLE_INSTRUCTION_MADD
 #endif
 #endif
 
@@ -34,47 +36,75 @@
 // One solution is to increase ABI version using -fabi-version=4 (or greater).
 // Otherwise, we workaround this inconvenience by wrapping 128bit types into the following helper
 // structure:
-template<typename T>
-struct eigen_packet_wrapper
-{
-  EIGEN_ALWAYS_INLINE operator T&() { return m_val; }
-  EIGEN_ALWAYS_INLINE operator const T&() const { return m_val; }
-  EIGEN_ALWAYS_INLINE eigen_packet_wrapper() {}
-  EIGEN_ALWAYS_INLINE eigen_packet_wrapper(const T &v) : m_val(v) {}
-  EIGEN_ALWAYS_INLINE eigen_packet_wrapper& operator=(const T &v) {
-    m_val = v;
-    return *this;
-  }
-  
-  T m_val;
-};
 typedef eigen_packet_wrapper<__m128>  Packet4f;
-typedef eigen_packet_wrapper<__m128i> Packet4i;
 typedef eigen_packet_wrapper<__m128d> Packet2d;
 #else
 typedef __m128  Packet4f;
-typedef __m128i Packet4i;
 typedef __m128d Packet2d;
 #endif
 
+typedef eigen_packet_wrapper<__m128i, 0> Packet4i;
+typedef eigen_packet_wrapper<__m128i, 1> Packet16b;
+
 template<> struct is_arithmetic<__m128>  { enum { value = true }; };
 template<> struct is_arithmetic<__m128i> { enum { value = true }; };
 template<> struct is_arithmetic<__m128d> { enum { value = true }; };
+template<> struct is_arithmetic<Packet4i>  { enum { value = true }; };
+template<> struct is_arithmetic<Packet16b>  { enum { value = true }; };
 
+template<int p, int q, int r, int s>
+struct shuffle_mask{
+ enum { mask = (s)<<6|(r)<<4|(q)<<2|(p) };
+};
+
+// TODO: change the implementation of all swizzle* ops from macro to template,
 #define vec4f_swizzle1(v,p,q,r,s) \
-  (_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), ((s)<<6|(r)<<4|(q)<<2|(p)))))
+  Packet4f(_mm_castsi128_ps(_mm_shuffle_epi32( _mm_castps_si128(v), (shuffle_mask<p,q,r,s>::mask))))
 
 #define vec4i_swizzle1(v,p,q,r,s) \
-  (_mm_shuffle_epi32( v, ((s)<<6|(r)<<4|(q)<<2|(p))))
+  Packet4i(_mm_shuffle_epi32( v, (shuffle_mask<p,q,r,s>::mask)))
 
 #define vec2d_swizzle1(v,p,q) \
-  (_mm_castsi128_pd(_mm_shuffle_epi32( _mm_castpd_si128(v), ((q*2+1)<<6|(q*2)<<4|(p*2+1)<<2|(p*2)))))
-  
+  Packet2d(_mm_castsi128_pd(_mm_shuffle_epi32( _mm_castpd_si128(v), (shuffle_mask<2*p,2*p+1,2*q,2*q+1>::mask))))
+
 #define vec4f_swizzle2(a,b,p,q,r,s) \
-  (_mm_shuffle_ps( (a), (b), ((s)<<6|(r)<<4|(q)<<2|(p))))
+  Packet4f(_mm_shuffle_ps( (a), (b), (shuffle_mask<p,q,r,s>::mask)))
 
 #define vec4i_swizzle2(a,b,p,q,r,s) \
-  (_mm_castps_si128( (_mm_shuffle_ps( _mm_castsi128_ps(a), _mm_castsi128_ps(b), ((s)<<6|(r)<<4|(q)<<2|(p))))))
+  Packet4i(_mm_castps_si128( (_mm_shuffle_ps( _mm_castsi128_ps(a), _mm_castsi128_ps(b), (shuffle_mask<p,q,r,s>::mask)))))
+
+EIGEN_STRONG_INLINE Packet4f vec4f_movelh(const Packet4f& a, const Packet4f& b)
+{
+  return Packet4f(_mm_movelh_ps(a,b));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_movehl(const Packet4f& a, const Packet4f& b)
+{
+  return Packet4f(_mm_movehl_ps(a,b));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_unpacklo(const Packet4f& a, const Packet4f& b)
+{
+  return Packet4f(_mm_unpacklo_ps(a,b));
+}
+EIGEN_STRONG_INLINE Packet4f vec4f_unpackhi(const Packet4f& a, const Packet4f& b)
+{
+  return Packet4f(_mm_unpackhi_ps(a,b));
+}
+#define vec4f_duplane(a,p) \
+  vec4f_swizzle2(a,a,p,p,p,p)
+
+#define vec2d_swizzle2(a,b,mask) \
+  Packet2d(_mm_shuffle_pd(a,b,mask))
+
+EIGEN_STRONG_INLINE Packet2d vec2d_unpacklo(const Packet2d& a, const Packet2d& b)
+{
+  return Packet2d(_mm_unpacklo_pd(a,b));
+}
+EIGEN_STRONG_INLINE Packet2d vec2d_unpackhi(const Packet2d& a, const Packet2d& b)
+{
+  return Packet2d(_mm_unpackhi_pd(a,b));
+}
+#define vec2d_duplane(a,p) \
+  vec2d_swizzle2(a,a,(p<<1)|p)
 
 #define _EIGEN_DECLARE_CONST_Packet4f(NAME,X) \
   const Packet4f p4f_##NAME = pset1<Packet4f>(X)
@@ -83,7 +113,7 @@
   const Packet2d p2d_##NAME = pset1<Packet2d>(X)
 
 #define _EIGEN_DECLARE_CONST_Packet4f_FROM_INT(NAME,X) \
-  const Packet4f p4f_##NAME = _mm_castsi128_ps(pset1<Packet4i>(X))
+  const Packet4f p4f_##NAME = pset1frombits<Packet4f>(X)
 
 #define _EIGEN_DECLARE_CONST_Packet4i(NAME,X) \
   const Packet4i p4i_##NAME = pset1<Packet4i>(X)
@@ -92,36 +122,41 @@
 // Use the packet_traits defined in AVX/PacketMath.h instead if we're going
 // to leverage AVX instructions.
 #ifndef EIGEN_VECTORIZE_AVX
-template<> struct packet_traits<float>  : default_packet_traits
-{
+template <>
+struct packet_traits<float> : default_packet_traits {
   typedef Packet4f type;
   typedef Packet4f half;
   enum {
     Vectorizable = 1,
     AlignedOnScalar = 1,
-    size=4,
+    size = 4,
     HasHalfPacket = 0,
 
-    HasDiv  = 1,
-    HasSin  = EIGEN_FAST_MATH,
-    HasCos  = EIGEN_FAST_MATH,
-    HasLog  = 1,
-    HasExp  = 1,
+    HasCmp  = 1,
+    HasDiv = 1,
+    HasSin = EIGEN_FAST_MATH,
+    HasCos = EIGEN_FAST_MATH,
+    HasLog = 1,
+    HasLog1p = 1,
+    HasExpm1 = 1,
+    HasNdtri = 1,
+    HasExp = 1,
+    HasBessel = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
-    HasTanh  = EIGEN_FAST_MATH,
-    HasBlend = 1
-
-#ifdef EIGEN_VECTORIZE_SSE4_1
-    ,
-    HasRound = 1,
+    HasTanh = EIGEN_FAST_MATH,
+    HasErf = EIGEN_FAST_MATH,
+    HasBlend = 1,
+    HasCeil = 1,
     HasFloor = 1,
-    HasCeil = 1
+#ifdef EIGEN_VECTORIZE_SSE4_1
+    HasRound = 1,
 #endif
+    HasRint = 1
   };
 };
-template<> struct packet_traits<double> : default_packet_traits
-{
+template <>
+struct packet_traits<double> : default_packet_traits {
   typedef Packet2d type;
   typedef Packet2d half;
   enum {
@@ -130,18 +165,19 @@
     size=2,
     HasHalfPacket = 0,
 
+    HasCmp  = 1,
     HasDiv  = 1,
+    HasLog  = 1,
     HasExp  = 1,
     HasSqrt = 1,
     HasRsqrt = 1,
-    HasBlend = 1
-
-#ifdef EIGEN_VECTORIZE_SSE4_1
-    ,
-    HasRound = 1,
+    HasBlend = 1,
     HasFloor = 1,
-    HasCeil = 1
+    HasCeil = 1,
+#ifdef EIGEN_VECTORIZE_SSE4_1
+    HasRound = 1,
 #endif
+    HasRint = 1
   };
 };
 #endif
@@ -154,13 +190,56 @@
     AlignedOnScalar = 1,
     size=4,
 
+    HasShift = 1,
     HasBlend = 1
   };
 };
 
-template<> struct unpacket_traits<Packet4f> { typedef float  type; enum {size=4, alignment=Aligned16}; typedef Packet4f half; };
-template<> struct unpacket_traits<Packet2d> { typedef double type; enum {size=2, alignment=Aligned16}; typedef Packet2d half; };
-template<> struct unpacket_traits<Packet4i> { typedef int    type; enum {size=4, alignment=Aligned16}; typedef Packet4i half; };
+template<> struct packet_traits<bool> : default_packet_traits
+{
+  typedef Packet16b type;
+  typedef Packet16b half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    HasHalfPacket = 0,
+    size=16,
+
+    HasAdd       = 1,
+    HasSub       = 1,
+    HasShift     = 0,
+    HasMul       = 1,
+    HasNegate    = 1,
+    HasAbs       = 0,
+    HasAbs2      = 0,
+    HasMin       = 0,
+    HasMax       = 0,
+    HasConj      = 0,
+    HasSqrt      = 1
+  };
+};
+
+template<> struct unpacket_traits<Packet4f> {
+  typedef float     type;
+  typedef Packet4f  half;
+  typedef Packet4i  integer_packet;
+  enum {size=4, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false};
+};
+template<> struct unpacket_traits<Packet2d> {
+  typedef double    type;
+  typedef Packet2d  half;
+  enum {size=2, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false};
+};
+template<> struct unpacket_traits<Packet4i> {
+  typedef int       type;
+  typedef Packet4i  half;
+  enum {size=4, alignment=Aligned16, vectorizable=false, masked_load_available=false, masked_store_available=false};
+};
+template<> struct unpacket_traits<Packet16b> {
+  typedef bool       type;
+  typedef Packet16b  half;
+  enum {size=16, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false};
+};
 
 #ifndef EIGEN_VECTORIZE_AVX
 template<> struct scalar_div_cost<float,true> { enum { value = 7 }; };
@@ -179,6 +258,18 @@
 template<> EIGEN_STRONG_INLINE Packet2d pset1<Packet2d>(const double& from) { return _mm_set1_pd(from); }
 template<> EIGEN_STRONG_INLINE Packet4i pset1<Packet4i>(const int&    from) { return _mm_set1_epi32(from); }
 #endif
+template<> EIGEN_STRONG_INLINE Packet16b pset1<Packet16b>(const bool&    from) { return _mm_set1_epi8(static_cast<char>(from)); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pset1frombits<Packet4f>(unsigned int from) { return _mm_castsi128_ps(pset1<Packet4i>(from)); }
+template<> EIGEN_STRONG_INLINE Packet2d pset1frombits<Packet2d>(uint64_t from) { return _mm_castsi128_pd(_mm_set1_epi64x(from)); }
+
+template<> EIGEN_STRONG_INLINE Packet4f peven_mask(const Packet4f& /*a*/) { return _mm_castsi128_ps(_mm_set_epi32(0, -1, 0, -1)); }
+template<> EIGEN_STRONG_INLINE Packet4i peven_mask(const Packet4i& /*a*/) { return _mm_set_epi32(0, -1, 0, -1); }
+template<> EIGEN_STRONG_INLINE Packet2d peven_mask(const Packet2d& /*a*/) { return _mm_castsi128_pd(_mm_set_epi32(0, 0, -1, -1)); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pzero(const Packet4f& /*a*/) { return _mm_setzero_ps(); }
+template<> EIGEN_STRONG_INLINE Packet2d pzero(const Packet2d& /*a*/) { return _mm_setzero_pd(); }
+template<> EIGEN_STRONG_INLINE Packet4i pzero(const Packet4i& /*a*/) { return _mm_setzero_si128(); }
 
 // GCC generates a shufps instruction for _mm_set1_ps/_mm_load1_ps instead of the more efficient pshufd instruction.
 // However, using inrinsics for pset1 makes gcc to generate crappy code in some cases (see bug 203)
@@ -190,7 +281,7 @@
   return vec4f_swizzle1(_mm_load_ss(from),0,0,0,0);
 }
 #endif
-  
+
 template<> EIGEN_STRONG_INLINE Packet4f plset<Packet4f>(const float& a) { return _mm_add_ps(pset1<Packet4f>(a), _mm_set_ps(3,2,1,0)); }
 template<> EIGEN_STRONG_INLINE Packet2d plset<Packet2d>(const double& a) { return _mm_add_pd(pset1<Packet2d>(a),_mm_set_pd(1,0)); }
 template<> EIGEN_STRONG_INLINE Packet4i plset<Packet4i>(const int& a) { return _mm_add_epi32(pset1<Packet4i>(a),_mm_set_epi32(3,2,1,0)); }
@@ -199,9 +290,34 @@
 template<> EIGEN_STRONG_INLINE Packet2d padd<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_add_pd(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i padd<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_add_epi32(a,b); }
 
+template<> EIGEN_STRONG_INLINE Packet16b padd<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_or_si128(a,b); }
+
 template<> EIGEN_STRONG_INLINE Packet4f psub<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_sub_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet2d psub<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_sub_pd(a,b); }
 template<> EIGEN_STRONG_INLINE Packet4i psub<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_sub_epi32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16b psub<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_xor_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b);
+template<> EIGEN_STRONG_INLINE Packet4f paddsub<Packet4f>(const Packet4f& a, const Packet4f& b)
+{
+#ifdef EIGEN_VECTORIZE_SSE3
+  return _mm_addsub_ps(a,b);
+#else
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x80000000,0x0,0x80000000,0x0));
+  return padd(a, pxor(mask, b));
+#endif
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& , const Packet2d& );
+template<> EIGEN_STRONG_INLINE Packet2d paddsub<Packet2d>(const Packet2d& a, const Packet2d& b) 
+{
+#ifdef EIGEN_VECTORIZE_SSE3  
+  return _mm_addsub_pd(a,b); 
+#else
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0,0x80000000,0x0,0x0)); 
+  return padd(a, pxor(mask, b));
+#endif
+}
 
 template<> EIGEN_STRONG_INLINE Packet4f pnegate(const Packet4f& a)
 {
@@ -218,6 +334,11 @@
   return psub(Packet4i(_mm_setr_epi32(0,0,0,0)), a);
 }
 
+template<> EIGEN_STRONG_INLINE Packet16b pnegate(const Packet16b& a)
+{
+  return psub(pset1<Packet16b>(false), a);
+}
+
 template<> EIGEN_STRONG_INLINE Packet4f pconj(const Packet4f& a) { return a; }
 template<> EIGEN_STRONG_INLINE Packet2d pconj(const Packet2d& a) { return a; }
 template<> EIGEN_STRONG_INLINE Packet4i pconj(const Packet4i& a) { return a; }
@@ -240,18 +361,126 @@
 #endif
 }
 
+template<> EIGEN_STRONG_INLINE Packet16b pmul<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_and_si128(a,b); }
+
 template<> EIGEN_STRONG_INLINE Packet4f pdiv<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_div_ps(a,b); }
 template<> EIGEN_STRONG_INLINE Packet2d pdiv<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_div_pd(a,b); }
 
 // for some weird raisons, it has to be overloaded for packet of integers
 template<> EIGEN_STRONG_INLINE Packet4i pmadd(const Packet4i& a, const Packet4i& b, const Packet4i& c) { return padd(pmul(a,b), c); }
-#ifdef __FMA__
+#ifdef EIGEN_VECTORIZE_FMA
 template<> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f& a, const Packet4f& b, const Packet4f& c) { return _mm_fmadd_ps(a,b,c); }
 template<> EIGEN_STRONG_INLINE Packet2d pmadd(const Packet2d& a, const Packet2d& b, const Packet2d& c) { return _mm_fmadd_pd(a,b,c); }
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_min_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_min_pd(a,b); }
+#ifdef EIGEN_VECTORIZE_SSE4_1
+template<> EIGEN_DEVICE_FUNC inline Packet4f pselect(const Packet4f& mask, const Packet4f& a, const Packet4f& b) {
+  return _mm_blendv_ps(b,a,mask);
+}
+
+template<> EIGEN_DEVICE_FUNC inline Packet4i pselect(const Packet4i& mask, const Packet4i& a, const Packet4i& b) {
+  return _mm_castps_si128(_mm_blendv_ps(_mm_castsi128_ps(b),_mm_castsi128_ps(a),_mm_castsi128_ps(mask)));
+}
+
+template<> EIGEN_DEVICE_FUNC inline Packet2d pselect(const Packet2d& mask, const Packet2d& a, const Packet2d& b) {  return _mm_blendv_pd(b,a,mask); }
+
+template<> EIGEN_DEVICE_FUNC inline Packet16b pselect(const Packet16b& mask, const Packet16b& a, const Packet16b& b) {
+  return _mm_blendv_epi8(b,a,mask);
+}
+#else
+template<> EIGEN_DEVICE_FUNC inline Packet16b pselect(const Packet16b& mask, const Packet16b& a, const Packet16b& b) {
+  Packet16b a_part = _mm_and_si128(mask, a);
+  Packet16b b_part = _mm_andnot_si128(mask, b);
+  return _mm_or_si128(a_part, b_part);
+}
+#endif
+
+template<> EIGEN_STRONG_INLINE Packet4i ptrue<Packet4i>(const Packet4i& a) { return _mm_cmpeq_epi32(a, a); }
+template<> EIGEN_STRONG_INLINE Packet16b ptrue<Packet16b>(const Packet16b& a) { return _mm_cmpeq_epi8(a, a); }
+template<> EIGEN_STRONG_INLINE Packet4f
+ptrue<Packet4f>(const Packet4f& a) {
+  Packet4i b = _mm_castps_si128(a);
+  return _mm_castsi128_ps(_mm_cmpeq_epi32(b, b));
+}
+template<> EIGEN_STRONG_INLINE Packet2d
+ptrue<Packet2d>(const Packet2d& a) {
+  Packet4i b = _mm_castpd_si128(a);
+  return _mm_castsi128_pd(_mm_cmpeq_epi32(b, b));
+}
+
+
+template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_and_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_and_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_and_si128(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16b pand<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_and_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_or_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_or_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_or_si128(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16b por<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_or_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_xor_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_xor_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_xor_si128(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16b pxor<Packet16b>(const Packet16b& a, const Packet16b& b) { return _mm_xor_si128(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_andnot_ps(b,a); }
+template<> EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_andnot_pd(b,a); }
+template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_andnot_si128(b,a); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_le(const Packet4f& a, const Packet4f& b) { return _mm_cmple_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt(const Packet4f& a, const Packet4f& b) { return _mm_cmplt_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_lt_or_nan(const Packet4f& a, const Packet4f& b) { return _mm_cmpnge_ps(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4f pcmp_eq(const Packet4f& a, const Packet4f& b) { return _mm_cmpeq_ps(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_le(const Packet2d& a, const Packet2d& b) { return _mm_cmple_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt(const Packet2d& a, const Packet2d& b) { return _mm_cmplt_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_lt_or_nan(const Packet2d& a, const Packet2d& b) { return _mm_cmpnge_pd(a,b); }
+template<> EIGEN_STRONG_INLINE Packet2d pcmp_eq(const Packet2d& a, const Packet2d& b) { return _mm_cmpeq_pd(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4i pcmp_lt(const Packet4i& a, const Packet4i& b) { return _mm_cmplt_epi32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pcmp_eq(const Packet4i& a, const Packet4i& b) { return _mm_cmpeq_epi32(a,b); }
+template<> EIGEN_STRONG_INLINE Packet16b pcmp_eq(const Packet16b& a, const Packet16b& b) { return _mm_cmpeq_epi8(a,b); }
+template<> EIGEN_STRONG_INLINE Packet4i pcmp_le(const Packet4i& a, const Packet4i& b) { return por(pcmp_lt(a,b), pcmp_eq(a,b)); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pmin<Packet4f>(const Packet4f& a, const Packet4f& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // There appears to be a bug in GCC, by which the optimizer may
+  // flip the argument order in calls to _mm_min_ps, so we have to
+  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+  #ifdef EIGEN_VECTORIZE_AVX
+  Packet4f res;
+  asm("vminps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  #else
+  Packet4f res = b;
+  asm("minps %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
+  #endif
+  return res;
+#else
+  // Arguments are reversed to match NaN propagation behavior of std::min.
+  return _mm_min_ps(b, a);
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2d pmin<Packet2d>(const Packet2d& a, const Packet2d& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // There appears to be a bug in GCC, by which the optimizer may
+  // flip the argument order in calls to _mm_min_pd, so we have to
+  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+  #ifdef EIGEN_VECTORIZE_AVX
+  Packet2d res;
+  asm("vminpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  #else
+  Packet2d res = b;
+  asm("minpd %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
+  #endif
+  return res;
+#else
+  // Arguments are reversed to match NaN propagation behavior of std::min.
+  return _mm_min_pd(b, a);
+#endif
+}
 template<> EIGEN_STRONG_INLINE Packet4i pmin<Packet4i>(const Packet4i& a, const Packet4i& b)
 {
 #ifdef EIGEN_VECTORIZE_SSE4_1
@@ -263,8 +492,45 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_max_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_max_pd(a,b); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pmax<Packet4f>(const Packet4f& a, const Packet4f& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // There appears to be a bug in GCC, by which the optimizer may
+  // flip the argument order in calls to _mm_max_ps, so we have to
+  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+  #ifdef EIGEN_VECTORIZE_AVX
+  Packet4f res;
+  asm("vmaxps %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  #else
+  Packet4f res = b;
+  asm("maxps %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
+  #endif
+  return res;
+#else
+  // Arguments are reversed to match NaN propagation behavior of std::max.
+  return _mm_max_ps(b, a);
+#endif
+}
+template<> EIGEN_STRONG_INLINE Packet2d pmax<Packet2d>(const Packet2d& a, const Packet2d& b) {
+#if EIGEN_COMP_GNUC && EIGEN_COMP_GNUC < 63
+  // There appears to be a bug in GCC, by which the optimizer may
+  // flip the argument order in calls to _mm_max_pd, so we have to
+  // resort to inline ASM here. This is supposed to be fixed in gcc6.3,
+  // see also: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=72867
+  #ifdef EIGEN_VECTORIZE_AVX
+  Packet2d res;
+  asm("vmaxpd %[a], %[b], %[res]" : [res] "=x" (res) : [a] "x" (a), [b] "x" (b));
+  #else
+  Packet2d res = b;
+  asm("maxpd %[a], %[res]" : [res] "+x" (res) : [a] "x" (a));
+  #endif
+  return res;
+#else
+  // Arguments are reversed to match NaN propagation behavior of std::max.
+  return _mm_max_pd(b, a);
+#endif
+}
 template<> EIGEN_STRONG_INLINE Packet4i pmax<Packet4i>(const Packet4i& a, const Packet4i& b)
 {
 #ifdef EIGEN_VECTORIZE_SSE4_1
@@ -276,36 +542,180 @@
 #endif
 }
 
+template <typename Packet, typename Op>
+EIGEN_STRONG_INLINE Packet pminmax_propagate_numbers(const Packet& a, const Packet& b, Op op) {
+  // In this implementation, we take advantage of the fact that pmin/pmax for SSE
+  // always return a if either a or b is NaN.
+  Packet not_nan_mask_a = pcmp_eq(a, a);
+  Packet m = op(a, b);
+  return pselect<Packet>(not_nan_mask_a, m, b);
+}
+
+template <typename Packet, typename Op>
+EIGEN_STRONG_INLINE Packet pminmax_propagate_nan(const Packet& a, const Packet& b, Op op) {
+  // In this implementation, we take advantage of the fact that pmin/pmax for SSE
+  // always return a if either a or b is NaN.
+  Packet not_nan_mask_a = pcmp_eq(a, a);
+  Packet m = op(b, a);
+  return pselect<Packet>(not_nan_mask_a, m, a);
+}
+
+// Add specializations for min/max with prescribed NaN progation.
+template<>
+EIGEN_STRONG_INLINE Packet4f pmin<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pminmax_propagate_numbers(a, b, pmin<Packet4f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet2d pmin<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return pminmax_propagate_numbers(a, b, pmin<Packet2d>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4f pmax<PropagateNumbers, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pminmax_propagate_numbers(a, b, pmax<Packet4f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet2d pmax<PropagateNumbers, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return pminmax_propagate_numbers(a, b, pmax<Packet2d>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4f pmin<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pminmax_propagate_nan(a, b, pmin<Packet4f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet2d pmin<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return pminmax_propagate_nan(a, b, pmin<Packet2d>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet4f pmax<PropagateNaN, Packet4f>(const Packet4f& a, const Packet4f& b) {
+  return pminmax_propagate_nan(a, b, pmax<Packet4f>);
+}
+template<>
+EIGEN_STRONG_INLINE Packet2d pmax<PropagateNaN, Packet2d>(const Packet2d& a, const Packet2d& b) {
+  return pminmax_propagate_nan(a, b, pmax<Packet2d>);
+}
+
+template<int N> EIGEN_STRONG_INLINE Packet4i parithmetic_shift_right(const Packet4i& a) { return _mm_srai_epi32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_right   (const Packet4i& a) { return _mm_srli_epi32(a,N); }
+template<int N> EIGEN_STRONG_INLINE Packet4i plogical_shift_left    (const Packet4i& a) { return _mm_slli_epi32(a,N); }
+
+template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a)
+{
+  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF));
+  return _mm_and_ps(a,mask);
+}
+template<> EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a)
+{
+  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF));
+  return _mm_and_pd(a,mask);
+}
+template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a)
+{
+  #ifdef EIGEN_VECTORIZE_SSSE3
+  return _mm_abs_epi32(a);
+  #else
+  Packet4i aux = _mm_srai_epi32(a,31);
+  return _mm_sub_epi32(_mm_xor_si128(a,aux),aux);
+  #endif
+}
+
 #ifdef EIGEN_VECTORIZE_SSE4_1
-template<> EIGEN_STRONG_INLINE Packet4f pround<Packet4f>(const Packet4f& a) { return _mm_round_ps(a, 0); }
-template<> EIGEN_STRONG_INLINE Packet2d pround<Packet2d>(const Packet2d& a) { return _mm_round_pd(a, 0); }
+template<> EIGEN_STRONG_INLINE Packet4f pround<Packet4f>(const Packet4f& a)
+{
+  // Unfortunatly _mm_round_ps doesn't have a rounding mode to implement numext::round.
+  const Packet4f mask = pset1frombits<Packet4f>(0x80000000u);
+  const Packet4f prev0dot5 = pset1frombits<Packet4f>(0x3EFFFFFFu);
+  return _mm_round_ps(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d pround<Packet2d>(const Packet2d& a)
+{
+  const Packet2d mask = _mm_castsi128_pd(_mm_set_epi64x(0x8000000000000000ull, 0x8000000000000000ull));
+  const Packet2d prev0dot5 = _mm_castsi128_pd(_mm_set_epi64x(0x3FDFFFFFFFFFFFFFull, 0x3FDFFFFFFFFFFFFFull));
+  return _mm_round_pd(padd(por(pand(a, mask), prev0dot5), a), _MM_FROUND_TO_ZERO);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f print<Packet4f>(const Packet4f& a) { return _mm_round_ps(a, _MM_FROUND_CUR_DIRECTION); }
+template<> EIGEN_STRONG_INLINE Packet2d print<Packet2d>(const Packet2d& a) { return _mm_round_pd(a, _MM_FROUND_CUR_DIRECTION); }
 
 template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a) { return _mm_ceil_ps(a); }
 template<> EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a) { return _mm_ceil_pd(a); }
 
 template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a) { return _mm_floor_ps(a); }
 template<> EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a) { return _mm_floor_pd(a); }
+#else
+template<> EIGEN_STRONG_INLINE Packet4f print(const Packet4f& a) {
+  // Adds and subtracts signum(a) * 2^23 to force rounding.
+  const Packet4f limit = pset1<Packet4f>(static_cast<float>(1<<23));
+  const Packet4f abs_a = pabs(a);
+  Packet4f r = padd(abs_a, limit);
+  // Don't compile-away addition and subtraction.
+  EIGEN_OPTIMIZATION_BARRIER(r);
+  r = psub(r, limit);
+  // If greater than limit, simply return a.  Otherwise, account for sign.
+  r = pselect(pcmp_lt(abs_a, limit),
+              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  return r;
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d print(const Packet2d& a) {
+  // Adds and subtracts signum(a) * 2^52 to force rounding.
+  const Packet2d limit = pset1<Packet2d>(static_cast<double>(1ull<<52));
+  const Packet2d abs_a = pabs(a);
+  Packet2d r = padd(abs_a, limit);
+  // Don't compile-away addition and subtraction.
+  EIGEN_OPTIMIZATION_BARRIER(r);
+  r = psub(r, limit);
+  // If greater than limit, simply return a.  Otherwise, account for sign.
+  r = pselect(pcmp_lt(abs_a, limit),
+              pselect(pcmp_lt(a, pzero(a)), pnegate(r), r), a);
+  return r;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pfloor<Packet4f>(const Packet4f& a)
+{
+  const Packet4f cst_1 = pset1<Packet4f>(1.0f);
+  Packet4f tmp  = print<Packet4f>(a);
+  // If greater, subtract one.
+  Packet4f mask = _mm_cmpgt_ps(tmp, a);
+  mask = pand(mask, cst_1);
+  return psub(tmp, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d pfloor<Packet2d>(const Packet2d& a)
+{
+  const Packet2d cst_1 = pset1<Packet2d>(1.0);
+  Packet2d tmp  = print<Packet2d>(a);
+  // If greater, subtract one.
+  Packet2d mask = _mm_cmpgt_pd(tmp, a);
+  mask = pand(mask, cst_1);
+  return psub(tmp, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pceil<Packet4f>(const Packet4f& a)
+{
+  const Packet4f cst_1 = pset1<Packet4f>(1.0f);
+  Packet4f tmp  = print<Packet4f>(a);
+  // If smaller, add one.
+  Packet4f mask = _mm_cmplt_ps(tmp, a);
+  mask = pand(mask, cst_1);
+  return padd(tmp, mask);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d pceil<Packet2d>(const Packet2d& a)
+{
+  const Packet2d cst_1 = pset1<Packet2d>(1.0);
+  Packet2d tmp  = print<Packet2d>(a);
+  // If smaller, add one.
+  Packet2d mask = _mm_cmplt_pd(tmp, a);
+  mask = pand(mask, cst_1);
+  return padd(tmp, mask);
+}
 #endif
 
-template<> EIGEN_STRONG_INLINE Packet4f pand<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_and_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pand<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_and_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pand<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_and_si128(a,b); }
-
-template<> EIGEN_STRONG_INLINE Packet4f por<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_or_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d por<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_or_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i por<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_or_si128(a,b); }
-
-template<> EIGEN_STRONG_INLINE Packet4f pxor<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_xor_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pxor<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_xor_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pxor<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_xor_si128(a,b); }
-
-template<> EIGEN_STRONG_INLINE Packet4f pandnot<Packet4f>(const Packet4f& a, const Packet4f& b) { return _mm_andnot_ps(a,b); }
-template<> EIGEN_STRONG_INLINE Packet2d pandnot<Packet2d>(const Packet2d& a, const Packet2d& b) { return _mm_andnot_pd(a,b); }
-template<> EIGEN_STRONG_INLINE Packet4i pandnot<Packet4i>(const Packet4i& a, const Packet4i& b) { return _mm_andnot_si128(a,b); }
-
 template<> EIGEN_STRONG_INLINE Packet4f pload<Packet4f>(const float*   from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_ps(from); }
 template<> EIGEN_STRONG_INLINE Packet2d pload<Packet2d>(const double*  from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_pd(from); }
 template<> EIGEN_STRONG_INLINE Packet4i pload<Packet4i>(const int*     from) { EIGEN_DEBUG_ALIGNED_LOAD return _mm_load_si128(reinterpret_cast<const __m128i*>(from)); }
+template<> EIGEN_STRONG_INLINE Packet16b pload<Packet16b>(const bool*     from) { EIGEN_DEBUG_ALIGNED_LOAD return  _mm_load_si128(reinterpret_cast<const __m128i*>(from)); }
 
 #if EIGEN_COMP_MSVC
   template<> EIGEN_STRONG_INLINE Packet4f ploadu<Packet4f>(const float*  from) {
@@ -340,6 +750,10 @@
   EIGEN_DEBUG_UNALIGNED_LOAD
   return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
 }
+template<> EIGEN_STRONG_INLINE Packet16b ploadu<Packet16b>(const bool*     from) {
+  EIGEN_DEBUG_UNALIGNED_LOAD
+  return _mm_loadu_si128(reinterpret_cast<const __m128i*>(from));
+}
 
 
 template<> EIGEN_STRONG_INLINE Packet4f ploaddup<Packet4f>(const float*   from)
@@ -355,13 +769,32 @@
   return vec4i_swizzle1(tmp, 0, 0, 1, 1);
 }
 
+// Loads 8 bools from memory and returns the packet
+// {b0, b0, b1, b1, b2, b2, b3, b3, b4, b4, b5, b5, b6, b6, b7, b7}
+template<> EIGEN_STRONG_INLINE Packet16b ploaddup<Packet16b>(const bool*     from)
+{
+  __m128i tmp = _mm_castpd_si128(pload1<Packet2d>(reinterpret_cast<const double*>(from)));
+  return  _mm_unpacklo_epi8(tmp, tmp);
+}
+
+// Loads 4 bools from memory and returns the packet
+// {b0, b0  b0, b0, b1, b1, b1, b1, b2, b2, b2, b2, b3, b3, b3, b3}
+template<> EIGEN_STRONG_INLINE Packet16b
+ploadquad<Packet16b>(const bool* from) {
+  __m128i tmp = _mm_castps_si128(pload1<Packet4f>(reinterpret_cast<const float*>(from)));
+  tmp = _mm_unpacklo_epi8(tmp, tmp);
+  return  _mm_unpacklo_epi16(tmp, tmp);
+}
+
 template<> EIGEN_STRONG_INLINE void pstore<float>(float*   to, const Packet4f& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_ps(to, from); }
 template<> EIGEN_STRONG_INLINE void pstore<double>(double* to, const Packet2d& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_pd(to, from); }
 template<> EIGEN_STRONG_INLINE void pstore<int>(int*       to, const Packet4i& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from); }
+template<> EIGEN_STRONG_INLINE void pstore<bool>(bool*     to, const Packet16b& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_store_si128(reinterpret_cast<__m128i*>(to), from); }
 
 template<> EIGEN_STRONG_INLINE void pstoreu<double>(double* to, const Packet2d& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_pd(to, from); }
 template<> EIGEN_STRONG_INLINE void pstoreu<float>(float*   to, const Packet4f& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_ps(to, from); }
 template<> EIGEN_STRONG_INLINE void pstoreu<int>(int*       to, const Packet4i& from) { EIGEN_DEBUG_UNALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from); }
+template<> EIGEN_STRONG_INLINE void pstoreu<bool>(bool*     to, const Packet16b& from) { EIGEN_DEBUG_ALIGNED_STORE _mm_storeu_si128(reinterpret_cast<__m128i*>(to), from); }
 
 template<> EIGEN_DEVICE_FUNC inline Packet4f pgather<float, Packet4f>(const float* from, Index stride)
 {
@@ -374,7 +807,15 @@
 template<> EIGEN_DEVICE_FUNC inline Packet4i pgather<int, Packet4i>(const int* from, Index stride)
 {
  return _mm_set_epi32(from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
- }
+}
+
+template<> EIGEN_DEVICE_FUNC inline Packet16b pgather<bool, Packet16b>(const bool* from, Index stride)
+{
+  return _mm_set_epi8(from[15*stride], from[14*stride], from[13*stride], from[12*stride],
+                      from[11*stride], from[10*stride], from[9*stride], from[8*stride],
+                      from[7*stride], from[6*stride], from[5*stride], from[4*stride],
+                      from[3*stride], from[2*stride], from[1*stride], from[0*stride]);
+}
 
 template<> EIGEN_DEVICE_FUNC inline void pscatter<float, Packet4f>(float* to, const Packet4f& from, Index stride)
 {
@@ -395,6 +836,14 @@
   to[stride*2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2));
   to[stride*3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3));
 }
+template<> EIGEN_DEVICE_FUNC inline void pscatter<bool, Packet16b>(bool* to, const Packet16b& from, Index stride)
+{
+  to[4*stride*0] = _mm_cvtsi128_si32(from);
+  to[4*stride*1] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 1));
+  to[4*stride*2] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 2));
+  to[4*stride*3] = _mm_cvtsi128_si32(_mm_shuffle_epi32(from, 3));
+}
+
 
 // some compilers might be tempted to perform multiple moves instead of using a vector path.
 template<> EIGEN_STRONG_INLINE void pstore1<Packet4f>(float* to, const float& a)
@@ -409,7 +858,7 @@
   pstore(to, Packet2d(vec2d_swizzle1(pa,0,0)));
 }
 
-#if EIGEN_COMP_PGI
+#if EIGEN_COMP_PGI && EIGEN_COMP_PGI < 1900
 typedef const void * SsePrefetchPtrType;
 #else
 typedef const char * SsePrefetchPtrType;
@@ -437,32 +886,62 @@
 template<> EIGEN_STRONG_INLINE double pfirst<Packet2d>(const Packet2d& a) { return _mm_cvtsd_f64(a); }
 template<> EIGEN_STRONG_INLINE int    pfirst<Packet4i>(const Packet4i& a) { return _mm_cvtsi128_si32(a); }
 #endif
+template<> EIGEN_STRONG_INLINE bool   pfirst<Packet16b>(const Packet16b& a) { int x = _mm_cvtsi128_si32(a); return static_cast<bool>(x & 1); }
 
-template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a)
-{ return _mm_shuffle_ps(a,a,0x1B); }
-template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a)
-{ return _mm_shuffle_pd(a,a,0x1); }
-template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a)
-{ return _mm_shuffle_epi32(a,0x1B); }
+template<> EIGEN_STRONG_INLINE Packet4f preverse(const Packet4f& a) { return _mm_shuffle_ps(a,a,0x1B); }
+template<> EIGEN_STRONG_INLINE Packet2d preverse(const Packet2d& a) { return _mm_shuffle_pd(a,a,0x1); }
+template<> EIGEN_STRONG_INLINE Packet4i preverse(const Packet4i& a) { return _mm_shuffle_epi32(a,0x1B); }
+template<> EIGEN_STRONG_INLINE Packet16b preverse(const Packet16b& a) {
+#ifdef EIGEN_VECTORIZE_SSSE3
+  __m128i mask = _mm_set_epi8(0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15);
+  return _mm_shuffle_epi8(a, mask);
+#else
+  Packet16b tmp = _mm_shuffle_epi32(a, _MM_SHUFFLE(0, 1, 2, 3));
+  tmp = _mm_shufflehi_epi16(_mm_shufflelo_epi16(tmp, _MM_SHUFFLE(2, 3, 0, 1)), _MM_SHUFFLE(2, 3, 0, 1));
+  return _mm_or_si128(_mm_slli_epi16(tmp, 8), _mm_srli_epi16(tmp, 8));
+#endif
+}
 
-template<> EIGEN_STRONG_INLINE Packet4f pabs(const Packet4f& a)
-{
-  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF,0x7FFFFFFF));
-  return _mm_and_ps(a,mask);
+template<> EIGEN_STRONG_INLINE Packet4f pfrexp<Packet4f>(const Packet4f& a, Packet4f& exponent) {
+  return pfrexp_generic(a,exponent);
 }
-template<> EIGEN_STRONG_INLINE Packet2d pabs(const Packet2d& a)
-{
-  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0xFFFFFFFF,0x7FFFFFFF,0xFFFFFFFF,0x7FFFFFFF));
-  return _mm_and_pd(a,mask);
+
+// Extract exponent without existence of Packet2l.
+template<>
+EIGEN_STRONG_INLINE  
+Packet2d pfrexp_generic_get_biased_exponent(const Packet2d& a) {
+  const Packet2d cst_exp_mask  = pset1frombits<Packet2d>(static_cast<uint64_t>(0x7ff0000000000000ull));
+  __m128i a_expo = _mm_srli_epi64(_mm_castpd_si128(pand(a, cst_exp_mask)), 52);
+  return _mm_cvtepi32_pd(vec4i_swizzle1(a_expo, 0, 2, 1, 3));
 }
-template<> EIGEN_STRONG_INLINE Packet4i pabs(const Packet4i& a)
-{
-  #ifdef EIGEN_VECTORIZE_SSSE3
-  return _mm_abs_epi32(a);
-  #else
-  Packet4i aux = _mm_srai_epi32(a,31);
-  return _mm_sub_epi32(_mm_xor_si128(a,aux),aux);
-  #endif
+
+template<> EIGEN_STRONG_INLINE Packet2d pfrexp<Packet2d>(const Packet2d& a, Packet2d& exponent) {
+  return pfrexp_generic(a, exponent);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f pldexp<Packet4f>(const Packet4f& a, const Packet4f& exponent) {
+  return pldexp_generic(a,exponent);
+}
+
+// We specialize pldexp here, since the generic implementation uses Packet2l, which is not well
+// supported by SSE, and has more range than is needed for exponents.
+template<> EIGEN_STRONG_INLINE Packet2d pldexp<Packet2d>(const Packet2d& a, const Packet2d& exponent) {
+  // Clamp exponent to [-2099, 2099]
+  const Packet2d max_exponent = pset1<Packet2d>(2099.0);
+  const Packet2d e = pmin(pmax(exponent, pnegate(max_exponent)), max_exponent);
+  
+  // Convert e to integer and swizzle to low-order bits.
+  const Packet4i ei = vec4i_swizzle1(_mm_cvtpd_epi32(e), 0, 3, 1, 3);
+  
+  // Split 2^e into four factors and multiply:
+  const Packet4i bias = _mm_set_epi32(0, 1023, 0, 1023);
+  Packet4i b = parithmetic_shift_right<2>(ei);  // floor(e/4)
+  Packet2d c = _mm_castsi128_pd(_mm_slli_epi64(padd(b, bias), 52));  // 2^b
+  Packet2d out = pmul(pmul(pmul(a, c), c), c); // a * 2^(3b)
+  b = psub(psub(psub(ei, b), b), b);  // e - 3b
+  c = _mm_castsi128_pd(_mm_slli_epi64(padd(b, bias), 52));  // 2^(e - 3b)
+  out = pmul(out, c);  // a * 2^e
+  return out;
 }
 
 // with AVX, the default implementations based on pload1 are faster
@@ -505,38 +984,6 @@
   vecs[0] = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(vecs[0]), 0x00));
 }
 
-#ifdef EIGEN_VECTORIZE_SSE3
-template<> EIGEN_STRONG_INLINE Packet4f preduxp<Packet4f>(const Packet4f* vecs)
-{
-  return _mm_hadd_ps(_mm_hadd_ps(vecs[0], vecs[1]),_mm_hadd_ps(vecs[2], vecs[3]));
-}
-
-template<> EIGEN_STRONG_INLINE Packet2d preduxp<Packet2d>(const Packet2d* vecs)
-{
-  return _mm_hadd_pd(vecs[0], vecs[1]);
-}
-
-#else
-template<> EIGEN_STRONG_INLINE Packet4f preduxp<Packet4f>(const Packet4f* vecs)
-{
-  Packet4f tmp0, tmp1, tmp2;
-  tmp0 = _mm_unpacklo_ps(vecs[0], vecs[1]);
-  tmp1 = _mm_unpackhi_ps(vecs[0], vecs[1]);
-  tmp2 = _mm_unpackhi_ps(vecs[2], vecs[3]);
-  tmp0 = _mm_add_ps(tmp0, tmp1);
-  tmp1 = _mm_unpacklo_ps(vecs[2], vecs[3]);
-  tmp1 = _mm_add_ps(tmp1, tmp2);
-  tmp2 = _mm_movehl_ps(tmp1, tmp0);
-  tmp0 = _mm_movelh_ps(tmp0, tmp1);
-  return _mm_add_ps(tmp0, tmp2);
-}
-
-template<> EIGEN_STRONG_INLINE Packet2d preduxp<Packet2d>(const Packet2d* vecs)
-{
-  return _mm_add_pd(_mm_unpacklo_pd(vecs[0], vecs[1]), _mm_unpackhi_pd(vecs[0], vecs[1]));
-}
-#endif  // SSE3
-
 template<> EIGEN_STRONG_INLINE float predux<Packet4f>(const Packet4f& a)
 {
   // Disable SSE3 _mm_hadd_pd that is extremely slow on all existing Intel's architectures
@@ -562,38 +1009,28 @@
 }
 
 #ifdef EIGEN_VECTORIZE_SSSE3
-template<> EIGEN_STRONG_INLINE Packet4i preduxp<Packet4i>(const Packet4i* vecs)
-{
-  return _mm_hadd_epi32(_mm_hadd_epi32(vecs[0], vecs[1]),_mm_hadd_epi32(vecs[2], vecs[3]));
-}
 template<> EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a)
 {
   Packet4i tmp0 = _mm_hadd_epi32(a,a);
   return pfirst<Packet4i>(_mm_hadd_epi32(tmp0,tmp0));
 }
+
 #else
 template<> EIGEN_STRONG_INLINE int predux<Packet4i>(const Packet4i& a)
 {
   Packet4i tmp = _mm_add_epi32(a, _mm_unpackhi_epi64(a,a));
   return pfirst(tmp) + pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1));
 }
-
-template<> EIGEN_STRONG_INLINE Packet4i preduxp<Packet4i>(const Packet4i* vecs)
-{
-  Packet4i tmp0, tmp1, tmp2;
-  tmp0 = _mm_unpacklo_epi32(vecs[0], vecs[1]);
-  tmp1 = _mm_unpackhi_epi32(vecs[0], vecs[1]);
-  tmp2 = _mm_unpackhi_epi32(vecs[2], vecs[3]);
-  tmp0 = _mm_add_epi32(tmp0, tmp1);
-  tmp1 = _mm_unpacklo_epi32(vecs[2], vecs[3]);
-  tmp1 = _mm_add_epi32(tmp1, tmp2);
-  tmp2 = _mm_unpacklo_epi64(tmp0, tmp1);
-  tmp0 = _mm_unpackhi_epi64(tmp0, tmp1);
-  return _mm_add_epi32(tmp0, tmp2);
-}
 #endif
+
+template<> EIGEN_STRONG_INLINE bool predux<Packet16b>(const Packet16b& a) {
+  Packet4i tmp = _mm_or_si128(a, _mm_unpackhi_epi64(a,a));
+  return (pfirst(tmp) != 0) || (pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1)) != 0);
+}
+
 // Other reduction functions:
 
+
 // mul
 template<> EIGEN_STRONG_INLINE float predux_mul<Packet4f>(const Packet4f& a)
 {
@@ -611,7 +1048,13 @@
   // TODO try to call _mm_mul_epu32 directly
   EIGEN_ALIGN16 int aux[4];
   pstore(aux, a);
-  return  (aux[0] * aux[1]) * (aux[2] * aux[3]);;
+  return  (aux[0] * aux[1]) * (aux[2] * aux[3]);
+}
+
+template<> EIGEN_STRONG_INLINE bool predux_mul<Packet16b>(const Packet16b& a) {
+  Packet4i tmp = _mm_and_si128(a, _mm_unpackhi_epi64(a,a));
+  return ((pfirst<Packet4i>(tmp) == 0x01010101) &&
+          (pfirst<Packet4i>(_mm_shuffle_epi32(tmp, 1)) == 0x01010101));
 }
 
 // min
@@ -666,113 +1109,16 @@
 #endif // EIGEN_VECTORIZE_SSE4_1
 }
 
-#if EIGEN_COMP_GNUC
-// template <> EIGEN_STRONG_INLINE Packet4f pmadd(const Packet4f&  a, const Packet4f&  b, const Packet4f&  c)
+// not needed yet
+// template<> EIGEN_STRONG_INLINE bool predux_all(const Packet4f& x)
 // {
-//   Packet4f res = b;
-//   asm("mulps %[a], %[b] \n\taddps %[c], %[b]" : [b] "+x" (res) : [a] "x" (a), [c] "x" (c));
-//   return res;
+//   return _mm_movemask_ps(x) == 0xF;
 // }
-// EIGEN_STRONG_INLINE Packet4i _mm_alignr_epi8(const Packet4i&  a, const Packet4i&  b, const int i)
-// {
-//   Packet4i res = a;
-//   asm("palignr %[i], %[a], %[b] " : [b] "+x" (res) : [a] "x" (a), [i] "i" (i));
-//   return res;
-// }
-#endif
 
-#ifdef EIGEN_VECTORIZE_SSSE3
-// SSSE3 versions
-template<int Offset>
-struct palign_impl<Offset,Packet4f>
+template<> EIGEN_STRONG_INLINE bool predux_any(const Packet4f& x)
 {
-  static EIGEN_STRONG_INLINE void run(Packet4f& first, const Packet4f& second)
-  {
-    if (Offset!=0)
-      first = _mm_castsi128_ps(_mm_alignr_epi8(_mm_castps_si128(second), _mm_castps_si128(first), Offset*4));
-  }
-};
-
-template<int Offset>
-struct palign_impl<Offset,Packet4i>
-{
-  static EIGEN_STRONG_INLINE void run(Packet4i& first, const Packet4i& second)
-  {
-    if (Offset!=0)
-      first = _mm_alignr_epi8(second,first, Offset*4);
-  }
-};
-
-template<int Offset>
-struct palign_impl<Offset,Packet2d>
-{
-  static EIGEN_STRONG_INLINE void run(Packet2d& first, const Packet2d& second)
-  {
-    if (Offset==1)
-      first = _mm_castsi128_pd(_mm_alignr_epi8(_mm_castpd_si128(second), _mm_castpd_si128(first), 8));
-  }
-};
-#else
-// SSE2 versions
-template<int Offset>
-struct palign_impl<Offset,Packet4f>
-{
-  static EIGEN_STRONG_INLINE void run(Packet4f& first, const Packet4f& second)
-  {
-    if (Offset==1)
-    {
-      first = _mm_move_ss(first,second);
-      first = _mm_castsi128_ps(_mm_shuffle_epi32(_mm_castps_si128(first),0x39));
-    }
-    else if (Offset==2)
-    {
-      first = _mm_movehl_ps(first,first);
-      first = _mm_movelh_ps(first,second);
-    }
-    else if (Offset==3)
-    {
-      first = _mm_move_ss(first,second);
-      first = _mm_shuffle_ps(first,second,0x93);
-    }
-  }
-};
-
-template<int Offset>
-struct palign_impl<Offset,Packet4i>
-{
-  static EIGEN_STRONG_INLINE void run(Packet4i& first, const Packet4i& second)
-  {
-    if (Offset==1)
-    {
-      first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
-      first = _mm_shuffle_epi32(first,0x39);
-    }
-    else if (Offset==2)
-    {
-      first = _mm_castps_si128(_mm_movehl_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(first)));
-      first = _mm_castps_si128(_mm_movelh_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
-    }
-    else if (Offset==3)
-    {
-      first = _mm_castps_si128(_mm_move_ss(_mm_castsi128_ps(first),_mm_castsi128_ps(second)));
-      first = _mm_castps_si128(_mm_shuffle_ps(_mm_castsi128_ps(first),_mm_castsi128_ps(second),0x93));
-    }
-  }
-};
-
-template<int Offset>
-struct palign_impl<Offset,Packet2d>
-{
-  static EIGEN_STRONG_INLINE void run(Packet2d& first, const Packet2d& second)
-  {
-    if (Offset==1)
-    {
-      first = _mm_castps_pd(_mm_movehl_ps(_mm_castpd_ps(first),_mm_castpd_ps(first)));
-      first = _mm_castps_pd(_mm_movelh_ps(_mm_castpd_ps(first),_mm_castpd_ps(second)));
-    }
-  }
-};
-#endif
+  return _mm_movemask_ps(x) != 0x0;
+}
 
 EIGEN_DEVICE_FUNC inline void
 ptranspose(PacketBlock<Packet4f,4>& kernel) {
@@ -799,6 +1145,100 @@
   kernel.packet[3] = _mm_unpackhi_epi64(T2, T3);
 }
 
+EIGEN_DEVICE_FUNC inline void
+ptranspose(PacketBlock<Packet16b,4>& kernel) {
+  __m128i T0 =  _mm_unpacklo_epi8(kernel.packet[0], kernel.packet[1]);
+  __m128i T1 =  _mm_unpackhi_epi8(kernel.packet[0], kernel.packet[1]);
+  __m128i T2 =  _mm_unpacklo_epi8(kernel.packet[2], kernel.packet[3]);
+  __m128i T3 =  _mm_unpackhi_epi8(kernel.packet[2], kernel.packet[3]);
+  kernel.packet[0] = _mm_unpacklo_epi16(T0, T2);
+  kernel.packet[1] = _mm_unpackhi_epi16(T0, T2);
+  kernel.packet[2] = _mm_unpacklo_epi16(T1, T3);
+  kernel.packet[3] = _mm_unpackhi_epi16(T1, T3);
+}
+
+EIGEN_DEVICE_FUNC inline void
+ptranspose(PacketBlock<Packet16b,16>& kernel) {
+  // If we number the elements in the input thus:
+  // kernel.packet[ 0] = {00, 01, 02, 03, 04, 05, 06, 07, 08, 09, 0a, 0b, 0c, 0d, 0e, 0f}
+  // kernel.packet[ 1] = {10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 1a, 1b, 1c, 1d, 1e, 1f}
+  // ...
+  // kernel.packet[15] = {f0, f1, f2, f3, f4, f5, f6, f7, f8, f9, fa, fb, fc, fd, fe, ff},
+  //
+  // the desired output is:
+  // kernel.packet[ 0] = {00, 10, 20, 30, 40, 50, 60, 70, 80, 90, a0, b0, c0, d0, e0, f0}
+  // kernel.packet[ 1] = {01, 11, 21, 31, 41, 51, 61, 71, 81, 91, a1, b1, c1, d1, e1, f1}
+  // ...
+  // kernel.packet[15] = {0f, 1f, 2f, 3f, 4f, 5f, 6f, 7f, 8f, 9f, af, bf, cf, df, ef, ff},
+  __m128i t0 =  _mm_unpacklo_epi8(kernel.packet[0], kernel.packet[1]); // 00 10 01 11 02 12 03 13 04 14 05 15 06 16 07 17
+  __m128i t1 =  _mm_unpackhi_epi8(kernel.packet[0], kernel.packet[1]); // 08 18 09 19 0a 1a 0b 1b 0c 1c 0d 1d 0e 1e 0f 1f
+  __m128i t2 =  _mm_unpacklo_epi8(kernel.packet[2], kernel.packet[3]); // 20 30 21 31 22 32 ...                     27 37
+  __m128i t3 =  _mm_unpackhi_epi8(kernel.packet[2], kernel.packet[3]); // 28 38 29 39 2a 3a ...                     2f 3f
+  __m128i t4 =  _mm_unpacklo_epi8(kernel.packet[4], kernel.packet[5]); // 40 50 41 51 42 52                         47 57
+  __m128i t5 =  _mm_unpackhi_epi8(kernel.packet[4], kernel.packet[5]); // 48 58 49 59 4a 5a
+  __m128i t6 =  _mm_unpacklo_epi8(kernel.packet[6], kernel.packet[7]);
+  __m128i t7 =  _mm_unpackhi_epi8(kernel.packet[6], kernel.packet[7]);
+  __m128i t8 =  _mm_unpacklo_epi8(kernel.packet[8], kernel.packet[9]);
+  __m128i t9 =  _mm_unpackhi_epi8(kernel.packet[8], kernel.packet[9]);
+  __m128i ta =  _mm_unpacklo_epi8(kernel.packet[10], kernel.packet[11]);
+  __m128i tb =  _mm_unpackhi_epi8(kernel.packet[10], kernel.packet[11]);
+  __m128i tc =  _mm_unpacklo_epi8(kernel.packet[12], kernel.packet[13]);
+  __m128i td =  _mm_unpackhi_epi8(kernel.packet[12], kernel.packet[13]);
+  __m128i te =  _mm_unpacklo_epi8(kernel.packet[14], kernel.packet[15]);
+  __m128i tf =  _mm_unpackhi_epi8(kernel.packet[14], kernel.packet[15]);
+
+  __m128i s0 =  _mm_unpacklo_epi16(t0, t2); // 00 10 20 30 01 11 21 31 02 12 22 32 03 13 23 33
+  __m128i s1 =  _mm_unpackhi_epi16(t0, t2); // 04 14 24 34
+  __m128i s2 =  _mm_unpacklo_epi16(t1, t3); // 08 18 28 38 ...
+  __m128i s3 =  _mm_unpackhi_epi16(t1, t3); // 0c 1c 2c 3c ...
+  __m128i s4 =  _mm_unpacklo_epi16(t4, t6); // 40 50 60 70 41 51 61 71 42 52 62 72 43 53 63 73
+  __m128i s5 =  _mm_unpackhi_epi16(t4, t6); // 44 54 64 74 ...
+  __m128i s6 =  _mm_unpacklo_epi16(t5, t7);
+  __m128i s7 =  _mm_unpackhi_epi16(t5, t7);
+  __m128i s8 =  _mm_unpacklo_epi16(t8, ta);
+  __m128i s9 =  _mm_unpackhi_epi16(t8, ta);
+  __m128i sa =  _mm_unpacklo_epi16(t9, tb);
+  __m128i sb =  _mm_unpackhi_epi16(t9, tb);
+  __m128i sc =  _mm_unpacklo_epi16(tc, te);
+  __m128i sd =  _mm_unpackhi_epi16(tc, te);
+  __m128i se =  _mm_unpacklo_epi16(td, tf);
+  __m128i sf =  _mm_unpackhi_epi16(td, tf);
+
+  __m128i u0 =  _mm_unpacklo_epi32(s0, s4); // 00 10 20 30 40 50 60 70 01 11 21 31 41 51 61 71
+  __m128i u1 =  _mm_unpackhi_epi32(s0, s4); // 02 12 22 32 42 52 62 72 03 13 23 33 43 53 63 73
+  __m128i u2 =  _mm_unpacklo_epi32(s1, s5);
+  __m128i u3 =  _mm_unpackhi_epi32(s1, s5);
+  __m128i u4 =  _mm_unpacklo_epi32(s2, s6);
+  __m128i u5 =  _mm_unpackhi_epi32(s2, s6);
+  __m128i u6 =  _mm_unpacklo_epi32(s3, s7);
+  __m128i u7 =  _mm_unpackhi_epi32(s3, s7);
+  __m128i u8 =  _mm_unpacklo_epi32(s8, sc);
+  __m128i u9 =  _mm_unpackhi_epi32(s8, sc);
+  __m128i ua =  _mm_unpacklo_epi32(s9, sd);
+  __m128i ub =  _mm_unpackhi_epi32(s9, sd);
+  __m128i uc =  _mm_unpacklo_epi32(sa, se);
+  __m128i ud =  _mm_unpackhi_epi32(sa, se);
+  __m128i ue =  _mm_unpacklo_epi32(sb, sf);
+  __m128i uf =  _mm_unpackhi_epi32(sb, sf);
+
+  kernel.packet[0]  = _mm_unpacklo_epi64(u0, u8);
+  kernel.packet[1]  = _mm_unpackhi_epi64(u0, u8);
+  kernel.packet[2]  = _mm_unpacklo_epi64(u1, u9);
+  kernel.packet[3]  = _mm_unpackhi_epi64(u1, u9);
+  kernel.packet[4]  = _mm_unpacklo_epi64(u2, ua);
+  kernel.packet[5]  = _mm_unpackhi_epi64(u2, ua);
+  kernel.packet[6]  = _mm_unpacklo_epi64(u3, ub);
+  kernel.packet[7]  = _mm_unpackhi_epi64(u3, ub);
+  kernel.packet[8]  = _mm_unpacklo_epi64(u4, uc);
+  kernel.packet[9]  = _mm_unpackhi_epi64(u4, uc);
+  kernel.packet[10] = _mm_unpacklo_epi64(u5, ud);
+  kernel.packet[11] = _mm_unpackhi_epi64(u5, ud);
+  kernel.packet[12] = _mm_unpacklo_epi64(u6, ue);
+  kernel.packet[13] = _mm_unpackhi_epi64(u6, ue);
+  kernel.packet[14] = _mm_unpacklo_epi64(u7, uf);
+  kernel.packet[15] = _mm_unpackhi_epi64(u7, uf);
+}
+
 template<> EIGEN_STRONG_INLINE Packet4i pblend(const Selector<4>& ifPacket, const Packet4i& thenPacket, const Packet4i& elsePacket) {
   const __m128i zero = _mm_setzero_si128();
   const __m128i select = _mm_set_epi32(ifPacket.select[3], ifPacket.select[2], ifPacket.select[1], ifPacket.select[0]);
@@ -830,46 +1270,8 @@
 #endif
 }
 
-template<> EIGEN_STRONG_INLINE Packet4f pinsertfirst(const Packet4f& a, float b)
-{
-#ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_blend_ps(a,pset1<Packet4f>(b),1);
-#else
-  return _mm_move_ss(a, _mm_load_ss(&b));
-#endif
-}
-
-template<> EIGEN_STRONG_INLINE Packet2d pinsertfirst(const Packet2d& a, double b)
-{
-#ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_blend_pd(a,pset1<Packet2d>(b),1);
-#else
-  return _mm_move_sd(a, _mm_load_sd(&b));
-#endif
-}
-
-template<> EIGEN_STRONG_INLINE Packet4f pinsertlast(const Packet4f& a, float b)
-{
-#ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_blend_ps(a,pset1<Packet4f>(b),(1<<3));
-#else
-  const Packet4f mask = _mm_castsi128_ps(_mm_setr_epi32(0x0,0x0,0x0,0xFFFFFFFF));
-  return _mm_or_ps(_mm_andnot_ps(mask, a), _mm_and_ps(mask, pset1<Packet4f>(b)));
-#endif
-}
-
-template<> EIGEN_STRONG_INLINE Packet2d pinsertlast(const Packet2d& a, double b)
-{
-#ifdef EIGEN_VECTORIZE_SSE4_1
-  return _mm_blend_pd(a,pset1<Packet2d>(b),(1<<1));
-#else
-  const Packet2d mask = _mm_castsi128_pd(_mm_setr_epi32(0x0,0x0,0xFFFFFFFF,0xFFFFFFFF));
-  return _mm_or_pd(_mm_andnot_pd(mask, a), _mm_and_pd(mask, pset1<Packet2d>(b)));
-#endif
-}
-
 // Scalar path for pmadd with FMA to ensure consistency with vectorized path.
-#ifdef __FMA__
+#ifdef EIGEN_VECTORIZE_FMA
 template<> EIGEN_STRONG_INLINE float pmadd(const float& a, const float& b, const float& c) {
   return ::fmaf(a,b,c);
 }
@@ -878,11 +1280,219 @@
 }
 #endif
 
+
+// Packet math for Eigen::half
+// Disable the following code since it's broken on too many platforms / compilers.
+//#elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC)
+#if 0
+
+typedef struct {
+  __m64 x;
+} Packet4h;
+
+
+template<> struct is_arithmetic<Packet4h> { enum { value = true }; };
+
+template <>
+struct packet_traits<Eigen::half> : default_packet_traits {
+  typedef Packet4h type;
+  // There is no half-size packet for Packet4h.
+  typedef Packet4h half;
+  enum {
+    Vectorizable = 1,
+    AlignedOnScalar = 1,
+    size = 4,
+    HasHalfPacket = 0,
+    HasAdd    = 1,
+    HasSub    = 1,
+    HasMul    = 1,
+    HasDiv    = 1,
+    HasNegate = 0,
+    HasAbs    = 0,
+    HasAbs2   = 0,
+    HasMin    = 0,
+    HasMax    = 0,
+    HasConj   = 0,
+    HasSetLinear = 0,
+    HasSqrt = 0,
+    HasRsqrt = 0,
+    HasExp = 0,
+    HasLog = 0,
+    HasBlend = 0
+  };
+};
+
+
+template<> struct unpacket_traits<Packet4h> { typedef Eigen::half type; enum {size=4, alignment=Aligned16, vectorizable=true, masked_load_available=false, masked_store_available=false}; typedef Packet4h half; };
+
+template<> EIGEN_STRONG_INLINE Packet4h pset1<Packet4h>(const Eigen::half& from) {
+  Packet4h result;
+  result.x = _mm_set1_pi16(from.x);
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE Eigen::half pfirst<Packet4h>(const Packet4h& from) {
+  return half_impl::raw_uint16_to_half(static_cast<unsigned short>(_mm_cvtsi64_si32(from.x)));
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h pconj(const Packet4h& a) { return a; }
+
+template<> EIGEN_STRONG_INLINE Packet4h padd<Packet4h>(const Packet4h& a, const Packet4h& b) {
+  __int64_t a64 = _mm_cvtm64_si64(a.x);
+  __int64_t b64 = _mm_cvtm64_si64(b.x);
+
+  Eigen::half h[4];
+
+  Eigen::half ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64));
+  Eigen::half hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64));
+  h[0] = ha + hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 16));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 16));
+  h[1] = ha + hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 32));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 32));
+  h[2] = ha + hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 48));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 48));
+  h[3] = ha + hb;
+  Packet4h result;
+  result.x = _mm_set_pi16(h[3].x, h[2].x, h[1].x, h[0].x);
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h psub<Packet4h>(const Packet4h& a, const Packet4h& b) {
+  __int64_t a64 = _mm_cvtm64_si64(a.x);
+  __int64_t b64 = _mm_cvtm64_si64(b.x);
+
+  Eigen::half h[4];
+
+  Eigen::half ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64));
+  Eigen::half hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64));
+  h[0] = ha - hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 16));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 16));
+  h[1] = ha - hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 32));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 32));
+  h[2] = ha - hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 48));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 48));
+  h[3] = ha - hb;
+  Packet4h result;
+  result.x = _mm_set_pi16(h[3].x, h[2].x, h[1].x, h[0].x);
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h pmul<Packet4h>(const Packet4h& a, const Packet4h& b) {
+  __int64_t a64 = _mm_cvtm64_si64(a.x);
+  __int64_t b64 = _mm_cvtm64_si64(b.x);
+
+  Eigen::half h[4];
+
+  Eigen::half ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64));
+  Eigen::half hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64));
+  h[0] = ha * hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 16));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 16));
+  h[1] = ha * hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 32));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 32));
+  h[2] = ha * hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 48));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 48));
+  h[3] = ha * hb;
+  Packet4h result;
+  result.x = _mm_set_pi16(h[3].x, h[2].x, h[1].x, h[0].x);
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h pdiv<Packet4h>(const Packet4h& a, const Packet4h& b) {
+  __int64_t a64 = _mm_cvtm64_si64(a.x);
+  __int64_t b64 = _mm_cvtm64_si64(b.x);
+
+  Eigen::half h[4];
+
+  Eigen::half ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64));
+  Eigen::half hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64));
+  h[0] = ha / hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 16));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 16));
+  h[1] = ha / hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 32));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 32));
+  h[2] = ha / hb;
+  ha = half_impl::raw_uint16_to_half(static_cast<unsigned short>(a64 >> 48));
+  hb = half_impl::raw_uint16_to_half(static_cast<unsigned short>(b64 >> 48));
+  h[3] = ha / hb;
+  Packet4h result;
+  result.x = _mm_set_pi16(h[3].x, h[2].x, h[1].x, h[0].x);
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h pload<Packet4h>(const Eigen::half* from) {
+  Packet4h result;
+  result.x = _mm_cvtsi64_m64(*reinterpret_cast<const __int64_t*>(from));
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h ploadu<Packet4h>(const Eigen::half* from) {
+  Packet4h result;
+  result.x = _mm_cvtsi64_m64(*reinterpret_cast<const __int64_t*>(from));
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE void pstore<Eigen::half>(Eigen::half* to, const Packet4h& from) {
+  __int64_t r = _mm_cvtm64_si64(from.x);
+  *(reinterpret_cast<__int64_t*>(to)) = r;
+}
+
+template<> EIGEN_STRONG_INLINE void pstoreu<Eigen::half>(Eigen::half* to, const Packet4h& from) {
+  __int64_t r = _mm_cvtm64_si64(from.x);
+  *(reinterpret_cast<__int64_t*>(to)) = r;
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h
+ploadquad<Packet4h>(const Eigen::half* from) {
+  return pset1<Packet4h>(*from);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4h pgather<Eigen::half, Packet4h>(const Eigen::half* from, Index stride)
+{
+  Packet4h result;
+  result.x = _mm_set_pi16(from[3*stride].x, from[2*stride].x, from[1*stride].x, from[0*stride].x);
+  return result;
+}
+
+template<> EIGEN_STRONG_INLINE void pscatter<Eigen::half, Packet4h>(Eigen::half* to, const Packet4h& from, Index stride)
+{
+  __int64_t a = _mm_cvtm64_si64(from.x);
+  to[stride*0].x = static_cast<unsigned short>(a);
+  to[stride*1].x = static_cast<unsigned short>(a >> 16);
+  to[stride*2].x = static_cast<unsigned short>(a >> 32);
+  to[stride*3].x = static_cast<unsigned short>(a >> 48);
+}
+
+EIGEN_STRONG_INLINE void
+ptranspose(PacketBlock<Packet4h,4>& kernel) {
+  __m64 T0 = _mm_unpacklo_pi16(kernel.packet[0].x, kernel.packet[1].x);
+  __m64 T1 = _mm_unpacklo_pi16(kernel.packet[2].x, kernel.packet[3].x);
+  __m64 T2 = _mm_unpackhi_pi16(kernel.packet[0].x, kernel.packet[1].x);
+  __m64 T3 = _mm_unpackhi_pi16(kernel.packet[2].x, kernel.packet[3].x);
+
+  kernel.packet[0].x = _mm_unpacklo_pi32(T0, T1);
+  kernel.packet[1].x = _mm_unpackhi_pi32(T0, T1);
+  kernel.packet[2].x = _mm_unpacklo_pi32(T2, T3);
+  kernel.packet[3].x = _mm_unpackhi_pi32(T2, T3);
+}
+
+#endif
+
+
 } // end namespace internal
 
 } // end namespace Eigen
 
-#if EIGEN_COMP_PGI
+#if EIGEN_COMP_PGI && EIGEN_COMP_PGI < 1900
 // PGI++ does not define the following intrinsics in C++ mode.
 static inline __m128  _mm_castpd_ps   (__m128d x) { return reinterpret_cast<__m128&>(x);  }
 static inline __m128i _mm_castpd_si128(__m128d x) { return reinterpret_cast<__m128i&>(x); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
index c6ca8c7..d2a0037 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/arch/SSE/TypeCasting.h
@@ -69,6 +69,71 @@
   return _mm_cvtps_pd(a);
 }
 
+template<> EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i,Packet4f>(const Packet4f& a) {
+  return _mm_castps_si128(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4f preinterpret<Packet4f,Packet4i>(const Packet4i& a) {
+  return _mm_castsi128_ps(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet2d preinterpret<Packet2d,Packet4i>(const Packet4i& a) {
+  return _mm_castsi128_pd(a);
+}
+
+template<> EIGEN_STRONG_INLINE Packet4i preinterpret<Packet4i,Packet2d>(const Packet2d& a) {
+  return _mm_castpd_si128(a);
+}
+
+// Disable the following code since it's broken on too many platforms / compilers.
+//#elif defined(EIGEN_VECTORIZE_SSE) && (!EIGEN_ARCH_x86_64) && (!EIGEN_COMP_MSVC)
+#if 0
+
+template <>
+struct type_casting_traits<Eigen::half, float> {
+  enum {
+    VectorizedCast = 1,
+    SrcCoeffRatio = 1,
+    TgtCoeffRatio = 1
+  };
+};
+
+template<> EIGEN_STRONG_INLINE Packet4f pcast<Packet4h, Packet4f>(const Packet4h& a) {
+  __int64_t a64 = _mm_cvtm64_si64(a.x);
+  Eigen::half h = raw_uint16_to_half(static_cast<unsigned short>(a64));
+  float f1 = static_cast<float>(h);
+  h = raw_uint16_to_half(static_cast<unsigned short>(a64 >> 16));
+  float f2 = static_cast<float>(h);
+  h = raw_uint16_to_half(static_cast<unsigned short>(a64 >> 32));
+  float f3 = static_cast<float>(h);
+  h = raw_uint16_to_half(static_cast<unsigned short>(a64 >> 48));
+  float f4 = static_cast<float>(h);
+  return _mm_set_ps(f4, f3, f2, f1);
+}
+
+template <>
+struct type_casting_traits<float, Eigen::half> {
+  enum {
+    VectorizedCast = 1,
+    SrcCoeffRatio = 1,
+    TgtCoeffRatio = 1
+  };
+};
+
+template<> EIGEN_STRONG_INLINE Packet4h pcast<Packet4f, Packet4h>(const Packet4f& a) {
+  EIGEN_ALIGN16 float aux[4];
+  pstore(aux, a);
+  Eigen::half h0(aux[0]);
+  Eigen::half h1(aux[1]);
+  Eigen::half h2(aux[2]);
+  Eigen::half h3(aux[3]);
+
+  Packet4h result;
+  result.x = _mm_set_pi16(h3.x, h2.x, h1.x, h0.x);
+  return result;
+}
+
+#endif
 
 } // end namespace internal
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
index 4153b87..bf64ef4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/AssignmentFunctors.h
@@ -144,7 +144,7 @@
   EIGEN_EMPTY_STRUCT_CTOR(swap_assign_op)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void assignCoeff(Scalar& a, const Scalar& b) const
   {
-#ifdef __CUDACC__
+#ifdef EIGEN_GPUCC
     // FIXME is there some kind of cuda::swap?
     Scalar t=b; const_cast<Scalar&>(b)=a; a=t;
 #else
@@ -157,7 +157,16 @@
 struct functor_traits<swap_assign_op<Scalar> > {
   enum {
     Cost = 3 * NumTraits<Scalar>::ReadCost,
-    PacketAccess = packet_traits<Scalar>::Vectorizable
+    PacketAccess = 
+    #if defined(EIGEN_VECTORIZE_AVX) && EIGEN_COMP_CLANG && (EIGEN_COMP_CLANG<800 || defined(__apple_build_version__))
+    // This is a partial workaround for a bug in clang generating bad code
+    // when mixing 256/512 bits loads and 128 bits moves.
+    // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1684
+    //     https://bugs.llvm.org/show_bug.cgi?id=40815
+    0
+    #else
+    packet_traits<Scalar>::Vectorizable
+    #endif
   };
 };
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
index 3eae6b8..63f09ab 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/BinaryFunctors.h
@@ -39,32 +39,26 @@
     EIGEN_SCALAR_BINARY_OP_PLUGIN
   }
 #endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a + b; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a + b; }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
   { return internal::padd(a,b); }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
   { return internal::predux(a); }
 };
 template<typename LhsScalar,typename RhsScalar>
 struct functor_traits<scalar_sum_op<LhsScalar,RhsScalar> > {
   enum {
-    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2, // rough estimate!
+    Cost = (int(NumTraits<LhsScalar>::AddCost) + int(NumTraits<RhsScalar>::AddCost)) / 2, // rough estimate!
     PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasAdd && packet_traits<RhsScalar>::HasAdd
     // TODO vectorize mixed sum
   };
 };
 
-/** \internal
-  * \brief Template specialization to deprecate the summation of boolean expressions.
-  * This is required to solve Bug 426.
-  * \sa DenseBase::count(), DenseBase::any(), ArrayBase::cast(), MatrixBase::cast()
-  */
-template<> struct scalar_sum_op<bool,bool> : scalar_sum_op<int,int> {
-  EIGEN_DEPRECATED
-  scalar_sum_op() {}
-};
+
+template<>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool scalar_sum_op<bool,bool>::operator() (const bool& a, const bool& b) const { return a || b; }
 
 
 /** \internal
@@ -83,23 +77,27 @@
     EIGEN_SCALAR_BINARY_OP_PLUGIN
   }
 #endif
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a * b; }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return a * b; }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
   { return internal::pmul(a,b); }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
   { return internal::predux_mul(a); }
 };
 template<typename LhsScalar,typename RhsScalar>
 struct functor_traits<scalar_product_op<LhsScalar,RhsScalar> > {
   enum {
-    Cost = (NumTraits<LhsScalar>::MulCost + NumTraits<RhsScalar>::MulCost)/2, // rough estimate!
+    Cost = (int(NumTraits<LhsScalar>::MulCost) + int(NumTraits<RhsScalar>::MulCost))/2, // rough estimate!
     PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasMul && packet_traits<RhsScalar>::HasMul
     // TODO vectorize mixed product
   };
 };
 
+template<>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool scalar_product_op<bool,bool>::operator() (const bool& a, const bool& b) const { return a && b; }
+
+
 /** \internal
   * \brief Template functor to compute the conjugate product of two scalars
   *
@@ -116,11 +114,11 @@
   typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_conj_product_op>::ReturnType result_type;
   
   EIGEN_EMPTY_STRUCT_CTOR(scalar_conj_product_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const
   { return conj_helper<LhsScalar,RhsScalar,Conj,false>().pmul(a,b); }
   
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
   { return conj_helper<Packet,Packet,Conj,false>().pmul(a,b); }
 };
 template<typename LhsScalar,typename RhsScalar>
@@ -136,21 +134,28 @@
   *
   * \sa class CwiseBinaryOp, MatrixBase::cwiseMin, class VectorwiseOp, MatrixBase::minCoeff()
   */
-template<typename LhsScalar,typename RhsScalar>
+template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
 struct scalar_min_op : binary_op_base<LhsScalar,RhsScalar>
 {
   typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_min_op>::ReturnType result_type;
   EIGEN_EMPTY_STRUCT_CTOR(scalar_min_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return numext::mini(a, b); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const {
+    return internal::pmin<NaNPropagation>(a, b);
+  }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pmin(a,b); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
+  {
+    return internal::pmin<NaNPropagation>(a,b);
+  }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const
-  { return internal::predux_min(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
+  {
+    return internal::predux_min<NaNPropagation>(a);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_min_op<LhsScalar,RhsScalar> > {
+
+template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
+struct functor_traits<scalar_min_op<LhsScalar,RhsScalar, NaNPropagation> > {
   enum {
     Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
     PacketAccess = internal::is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMin
@@ -162,21 +167,28 @@
   *
   * \sa class CwiseBinaryOp, MatrixBase::cwiseMax, class VectorwiseOp, MatrixBase::maxCoeff()
   */
-template<typename LhsScalar,typename RhsScalar>
-struct scalar_max_op  : binary_op_base<LhsScalar,RhsScalar>
+template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
+struct scalar_max_op : binary_op_base<LhsScalar,RhsScalar>
 {
   typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_max_op>::ReturnType result_type;
   EIGEN_EMPTY_STRUCT_CTOR(scalar_max_op)
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const { return numext::maxi(a, b); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const LhsScalar& a, const RhsScalar& b) const {
+    return internal::pmax<NaNPropagation>(a,b);
+  }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
-  { return internal::pmax(a,b); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Packet packetOp(const Packet& a, const Packet& b) const
+  {
+    return internal::pmax<NaNPropagation>(a,b);
+  }
   template<typename Packet>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type predux(const Packet& a) const
-  { return internal::predux_max(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type predux(const Packet& a) const
+  {
+    return internal::predux_max<NaNPropagation>(a);
+  }
 };
-template<typename LhsScalar,typename RhsScalar>
-struct functor_traits<scalar_max_op<LhsScalar,RhsScalar> > {
+
+template<typename LhsScalar,typename RhsScalar, int NaNPropagation>
+struct functor_traits<scalar_max_op<LhsScalar,RhsScalar, NaNPropagation> > {
   enum {
     Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
     PacketAccess = internal::is_same<LhsScalar, RhsScalar>::value && packet_traits<LhsScalar>::HasMax
@@ -253,7 +265,6 @@
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator()(const LhsScalar& a, const RhsScalar& b) const {return a!=b;}
 };
 
-
 /** \internal
   * \brief Template functor to compute the hypot of two \b positive \b and \b real scalars
   *
@@ -287,6 +298,7 @@
 
 /** \internal
   * \brief Template functor to compute the pow of two scalars
+  * See the specification of pow in https://en.cppreference.com/w/cpp/numeric/math/pow
   */
 template<typename Scalar, typename Exponent>
 struct scalar_pow_op  : binary_op_base<Scalar,Exponent>
@@ -301,16 +313,31 @@
     EIGEN_SCALAR_BINARY_OP_PLUGIN
   }
 #endif
+
   EIGEN_DEVICE_FUNC
   inline result_type operator() (const Scalar& a, const Exponent& b) const { return numext::pow(a, b); }
+
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  {
+    return generic_pow(a,b);
+  }
 };
+
 template<typename Scalar, typename Exponent>
 struct functor_traits<scalar_pow_op<Scalar,Exponent> > {
-  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
+  enum {
+    Cost = 5 * NumTraits<Scalar>::MulCost,
+    PacketAccess = (!NumTraits<Scalar>::IsComplex && !NumTraits<Scalar>::IsInteger &&
+                    packet_traits<Scalar>::HasExp && packet_traits<Scalar>::HasLog &&
+                    packet_traits<Scalar>::HasRound && packet_traits<Scalar>::HasCmp &&
+                    // Temporarly disable packet access for half/bfloat16 until
+                    // accuracy is improved.
+                    !is_same<Scalar, half>::value && !is_same<Scalar, bfloat16>::value
+                    )
+  };
 };
 
-
-
 //---------- non associative binary functors ----------
 
 /** \internal
@@ -337,7 +364,7 @@
 template<typename LhsScalar,typename RhsScalar>
 struct functor_traits<scalar_difference_op<LhsScalar,RhsScalar> > {
   enum {
-    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
+    Cost = (int(NumTraits<LhsScalar>::AddCost) + int(NumTraits<RhsScalar>::AddCost)) / 2,
     PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasSub && packet_traits<RhsScalar>::HasSub
   };
 };
@@ -382,11 +409,14 @@
 struct scalar_boolean_and_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_and_op)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a && b; }
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pand(a,b); }
 };
 template<> struct functor_traits<scalar_boolean_and_op> {
   enum {
     Cost = NumTraits<bool>::AddCost,
-    PacketAccess = false
+    PacketAccess = true
   };
 };
 
@@ -398,11 +428,14 @@
 struct scalar_boolean_or_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_or_op)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a || b; }
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::por(a,b); }
 };
 template<> struct functor_traits<scalar_boolean_or_op> {
   enum {
     Cost = NumTraits<bool>::AddCost,
-    PacketAccess = false
+    PacketAccess = true
   };
 };
 
@@ -414,11 +447,44 @@
 struct scalar_boolean_xor_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_boolean_xor_op)
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE bool operator() (const bool& a, const bool& b) const { return a ^ b; }
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pxor(a,b); }
 };
 template<> struct functor_traits<scalar_boolean_xor_op> {
   enum {
     Cost = NumTraits<bool>::AddCost,
-    PacketAccess = false
+    PacketAccess = true
+  };
+};
+
+/** \internal
+  * \brief Template functor to compute the absolute difference of two scalars
+  *
+  * \sa class CwiseBinaryOp, MatrixBase::absolute_difference
+  */
+template<typename LhsScalar,typename RhsScalar>
+struct scalar_absolute_difference_op : binary_op_base<LhsScalar,RhsScalar>
+{
+  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar,scalar_absolute_difference_op>::ReturnType result_type;
+#ifndef EIGEN_SCALAR_BINARY_OP_PLUGIN
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_absolute_difference_op)
+#else
+  scalar_absolute_difference_op() {
+    EIGEN_SCALAR_BINARY_OP_PLUGIN
+  }
+#endif
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const LhsScalar& a, const RhsScalar& b) const
+  { return numext::absdiff(a,b); }
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a, const Packet& b) const
+  { return internal::pabsdiff(a,b); }
+};
+template<typename LhsScalar,typename RhsScalar>
+struct functor_traits<scalar_absolute_difference_op<LhsScalar,RhsScalar> > {
+  enum {
+    Cost = (NumTraits<LhsScalar>::AddCost+NumTraits<RhsScalar>::AddCost)/2,
+    PacketAccess = is_same<LhsScalar,RhsScalar>::value && packet_traits<LhsScalar>::HasAbsDiff
   };
 };
 
@@ -436,7 +502,7 @@
   typedef typename BinaryOp::second_argument_type second_argument_type;
   typedef typename BinaryOp::result_type          result_type;
 
-  bind1st_op(const first_argument_type &val) : m_value(val) {}
+  EIGEN_DEVICE_FUNC explicit bind1st_op(const first_argument_type &val) : m_value(val) {}
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const second_argument_type& b) const { return BinaryOp::operator()(m_value,b); }
 
@@ -455,7 +521,7 @@
   typedef typename BinaryOp::second_argument_type second_argument_type;
   typedef typename BinaryOp::result_type          result_type;
 
-  bind2nd_op(const second_argument_type &val) : m_value(val) {}
+  EIGEN_DEVICE_FUNC explicit bind2nd_op(const second_argument_type &val) : m_value(val) {}
 
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const first_argument_type& a) const { return BinaryOp::operator()(a,m_value); }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
index b03be02..192f225 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/NullaryFunctors.h
@@ -37,26 +37,27 @@
 struct functor_traits<scalar_identity_op<Scalar> >
 { enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = false, IsRepeatable = true }; };
 
-template <typename Scalar, typename Packet, bool IsInteger> struct linspaced_op_impl;
+template <typename Scalar, bool IsInteger> struct linspaced_op_impl;
 
-template <typename Scalar, typename Packet>
-struct linspaced_op_impl<Scalar,Packet,/*IsInteger*/false>
+template <typename Scalar>
+struct linspaced_op_impl<Scalar,/*IsInteger*/false>
 {
-  linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps) :
-    m_low(low), m_high(high), m_size1(num_steps==1 ? 1 : num_steps-1), m_step(num_steps==1 ? Scalar() : (high-low)/Scalar(num_steps-1)),
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+
+  EIGEN_DEVICE_FUNC linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps) :
+    m_low(low), m_high(high), m_size1(num_steps==1 ? 1 : num_steps-1), m_step(num_steps==1 ? Scalar() : Scalar((high-low)/RealScalar(num_steps-1))),
     m_flip(numext::abs(high)<numext::abs(low))
   {}
 
   template<typename IndexType>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (IndexType i) const {
-    typedef typename NumTraits<Scalar>::Real RealScalar;
     if(m_flip)
-      return (i==0)? m_low : (m_high - RealScalar(m_size1-i)*m_step);
+      return (i==0)? m_low : Scalar(m_high - RealScalar(m_size1-i)*m_step);
     else
-      return (i==m_size1)? m_high : (m_low + RealScalar(i)*m_step);
+      return (i==m_size1)? m_high : Scalar(m_low + RealScalar(i)*m_step);
   }
 
-  template<typename IndexType>
+  template<typename Packet, typename IndexType>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const
   {
     // Principle:
@@ -65,17 +66,17 @@
     {
       Packet pi = plset<Packet>(Scalar(i-m_size1));
       Packet res = padd(pset1<Packet>(m_high), pmul(pset1<Packet>(m_step), pi));
-      if(i==0)
-        res = pinsertfirst(res, m_low);
-      return res;
+      if (EIGEN_PREDICT_TRUE(i != 0)) return res;
+      Packet mask = pcmp_lt(pset1<Packet>(0), plset<Packet>(0));
+      return pselect<Packet>(mask, res, pset1<Packet>(m_low));
     }
     else
     {
       Packet pi = plset<Packet>(Scalar(i));
       Packet res = padd(pset1<Packet>(m_low), pmul(pset1<Packet>(m_step), pi));
-      if(i==m_size1-unpacket_traits<Packet>::size+1)
-        res = pinsertlast(res, m_high);
-      return res;
+      if(EIGEN_PREDICT_TRUE(i != m_size1-unpacket_traits<Packet>::size+1)) return res;
+      Packet mask = pcmp_lt(plset<Packet>(0), pset1<Packet>(unpacket_traits<Packet>::size-1));
+      return pselect<Packet>(mask, res, pset1<Packet>(m_high));
     }
   }
 
@@ -86,10 +87,10 @@
   const bool m_flip;
 };
 
-template <typename Scalar, typename Packet>
-struct linspaced_op_impl<Scalar,Packet,/*IsInteger*/true>
+template <typename Scalar>
+struct linspaced_op_impl<Scalar,/*IsInteger*/true>
 {
-  linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps) :
+  EIGEN_DEVICE_FUNC linspaced_op_impl(const Scalar& low, const Scalar& high, Index num_steps) :
     m_low(low),
     m_multiplier((high-low)/convert_index<Scalar>(num_steps<=1 ? 1 : num_steps-1)),
     m_divisor(convert_index<Scalar>((high>=low?num_steps:-num_steps)+(high-low))/((numext::abs(high-low)+1)==0?1:(numext::abs(high-low)+1))),
@@ -115,8 +116,8 @@
 // Forward declaration (we default to random access which does not really give
 // us a speed gain when using packet access but it allows to use the functor in
 // nested expressions).
-template <typename Scalar, typename PacketType> struct linspaced_op;
-template <typename Scalar, typename PacketType> struct functor_traits< linspaced_op<Scalar,PacketType> >
+template <typename Scalar> struct linspaced_op;
+template <typename Scalar> struct functor_traits< linspaced_op<Scalar> >
 {
   enum
   {
@@ -126,9 +127,9 @@
     IsRepeatable = true
   };
 };
-template <typename Scalar, typename PacketType> struct linspaced_op
+template <typename Scalar> struct linspaced_op
 {
-  linspaced_op(const Scalar& low, const Scalar& high, Index num_steps)
+  EIGEN_DEVICE_FUNC linspaced_op(const Scalar& low, const Scalar& high, Index num_steps)
     : impl((num_steps==1 ? high : low),high,num_steps)
   {}
 
@@ -136,11 +137,11 @@
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (IndexType i) const { return impl(i); }
 
   template<typename Packet,typename IndexType>
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const { return impl.packetOp(i); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(IndexType i) const { return impl.template packetOp<Packet>(i); }
 
   // This proxy object handles the actual required temporaries and the different
   // implementations (integer vs. floating point).
-  const linspaced_op_impl<Scalar,PacketType,NumTraits<Scalar>::IsInteger> impl;
+  const linspaced_op_impl<Scalar,NumTraits<Scalar>::IsInteger> impl;
 };
 
 // Linear access is automatically determined from the operator() prototypes available for the given functor.
@@ -166,12 +167,12 @@
 template<typename Scalar,typename IndexType>
 struct has_binary_operator<scalar_identity_op<Scalar>,IndexType> { enum { value = 1}; };
 
-template<typename Scalar, typename PacketType,typename IndexType>
-struct has_nullary_operator<linspaced_op<Scalar,PacketType>,IndexType> { enum { value = 0}; };
-template<typename Scalar, typename PacketType,typename IndexType>
-struct has_unary_operator<linspaced_op<Scalar,PacketType>,IndexType> { enum { value = 1}; };
-template<typename Scalar, typename PacketType,typename IndexType>
-struct has_binary_operator<linspaced_op<Scalar,PacketType>,IndexType> { enum { value = 0}; };
+template<typename Scalar,typename IndexType>
+struct has_nullary_operator<linspaced_op<Scalar>,IndexType> { enum { value = 0}; };
+template<typename Scalar,typename IndexType>
+struct has_unary_operator<linspaced_op<Scalar>,IndexType> { enum { value = 1}; };
+template<typename Scalar,typename IndexType>
+struct has_binary_operator<linspaced_op<Scalar>,IndexType> { enum { value = 0}; };
 
 template<typename Scalar,typename IndexType>
 struct has_nullary_operator<scalar_random_op<Scalar>,IndexType> { enum { value = 1}; };
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
index 9c1d758..4570c9b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/StlFunctors.h
@@ -12,6 +12,28 @@
 
 namespace Eigen {
 
+// Portable replacements for certain functors.
+namespace numext {
+
+template<typename T = void>
+struct equal_to {
+  typedef bool result_type;
+  EIGEN_DEVICE_FUNC bool operator()(const T& lhs, const T& rhs) const {
+    return lhs == rhs;
+  }
+};
+
+template<typename T = void>
+struct not_equal_to {
+  typedef bool result_type;
+  EIGEN_DEVICE_FUNC bool operator()(const T& lhs, const T& rhs) const {
+    return lhs != rhs;
+  }
+};
+
+}
+
+
 namespace internal {
 
 // default functor traits for STL functors:
@@ -69,10 +91,18 @@
 { enum { Cost = 1, PacketAccess = false }; };
 
 template<typename T>
+struct functor_traits<numext::equal_to<T> >
+  : functor_traits<std::equal_to<T> > {};
+
+template<typename T>
 struct functor_traits<std::not_equal_to<T> >
 { enum { Cost = 1, PacketAccess = false }; };
 
-#if (__cplusplus < 201103L) && (EIGEN_COMP_MSVC <= 1900)
+template<typename T>
+struct functor_traits<numext::not_equal_to<T> >
+  : functor_traits<std::not_equal_to<T> > {};
+
+#if (EIGEN_COMP_CXXVER < 11)
 // std::binder* are deprecated since c++11 and will be removed in c++17
 template<typename T>
 struct functor_traits<std::binder2nd<T> >
@@ -83,7 +113,7 @@
 { enum { Cost = functor_traits<T>::Cost, PacketAccess = false }; };
 #endif
 
-#if (__cplusplus < 201703L) && (EIGEN_COMP_MSVC < 1910)
+#if (EIGEN_COMP_CXXVER < 17)
 // std::unary_negate is deprecated since c++17 and will be removed in c++20
 template<typename T>
 struct functor_traits<std::unary_negate<T> >
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
index 2e6a00f..16136d1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/functors/UnaryFunctors.h
@@ -109,7 +109,7 @@
 template<typename Scalar> struct scalar_conjugate_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_conjugate_op)
   EIGEN_DEVICE_FUNC
-  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { using numext::conj; return conj(a); }
+  EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::conj(a); }
   template<typename Packet>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const { return internal::pconj(a); }
 };
@@ -117,7 +117,15 @@
 struct functor_traits<scalar_conjugate_op<Scalar> >
 {
   enum {
-    Cost = NumTraits<Scalar>::IsComplex ? NumTraits<Scalar>::AddCost : 0,
+    Cost = 0,
+    // Yes the cost is zero even for complexes because in most cases for which
+    // the cost is used, conjugation turns to be a no-op. Some examples:
+    //   cost(a*conj(b)) == cost(a*b)
+    //   cost(a+conj(b)) == cost(a+b)
+    //   <etc.
+    // If we don't set it to zero, then:
+    //   A.conjugate().lazyProduct(B.conjugate())
+    // will bake its operands. We definitely don't want that!
     PacketAccess = packet_traits<Scalar>::HasConj
   };
 };
@@ -130,7 +138,7 @@
 template<typename Scalar> struct scalar_arg_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_arg_op)
   typedef typename NumTraits<Scalar>::Real result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { using numext::arg; return arg(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const result_type operator() (const Scalar& a) const { return numext::arg(a); }
   template<typename Packet>
   EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
   { return internal::parg(a); }
@@ -159,6 +167,44 @@
 { enum { Cost = is_same<Scalar, NewType>::value ? 0 : NumTraits<NewType>::AddCost, PacketAccess = false }; };
 
 /** \internal
+  * \brief Template functor to arithmetically shift a scalar right by a number of bits
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::shift_right()
+  */
+template<typename Scalar, int N>
+struct scalar_shift_right_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_shift_right_op)
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const
+  { return a >> N; }
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::parithmetic_shift_right<N>(a); }
+};
+template<typename Scalar, int N>
+struct functor_traits<scalar_shift_right_op<Scalar,N> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasShift }; };
+
+/** \internal
+  * \brief Template functor to logically shift a scalar left by a number of bits
+  *
+  * \sa class CwiseUnaryOp, MatrixBase::shift_left()
+  */
+template<typename Scalar, int N>
+struct scalar_shift_left_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_shift_left_op)
+
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const
+  { return a << N; }
+  template<typename Packet>
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Packet packetOp(const Packet& a) const
+  { return internal::plogical_shift_left<N>(a); }
+};
+template<typename Scalar, int N>
+struct functor_traits<scalar_shift_left_op<Scalar,N> >
+{ enum { Cost = NumTraits<Scalar>::AddCost, PacketAccess = packet_traits<Scalar>::HasShift }; };
+
+/** \internal
   * \brief Template functor to extract the real part of a complex
   *
   * \sa class CwiseUnaryOp, MatrixBase::real()
@@ -264,6 +310,26 @@
 
 /** \internal
   *
+  * \brief Template functor to compute the exponential of a scalar - 1.
+  *
+  * \sa class CwiseUnaryOp, ArrayBase::expm1()
+  */
+template<typename Scalar> struct scalar_expm1_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_expm1_op)
+  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::expm1(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::pexpm1(a); }
+};
+template <typename Scalar>
+struct functor_traits<scalar_expm1_op<Scalar> > {
+  enum {
+    PacketAccess = packet_traits<Scalar>::HasExpm1,
+    Cost = functor_traits<scalar_exp_op<Scalar> >::Cost // TODO measure cost of expm1
+  };
+};
+
+/** \internal
+  *
   * \brief Template functor to compute the logarithm of a scalar
   *
   * \sa class CwiseUnaryOp, ArrayBase::log()
@@ -321,7 +387,7 @@
   */
 template<typename Scalar> struct scalar_log10_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_log10_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { EIGEN_USING_STD_MATH(log10) return log10(a); }
+  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { EIGEN_USING_STD(log10) return log10(a); }
   template <typename Packet>
   EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::plog10(a); }
 };
@@ -330,6 +396,22 @@
 { enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog10 }; };
 
 /** \internal
+  *
+  * \brief Template functor to compute the base-2 logarithm of a scalar
+  *
+  * \sa class CwiseUnaryOp, Cwise::log2()
+  */
+template<typename Scalar> struct scalar_log2_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_log2_op)
+  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return Scalar(EIGEN_LOG2E) * numext::log(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::plog2(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_log2_op<Scalar> >
+{ enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasLog }; };
+
+/** \internal
   * \brief Template functor to compute the square root of a scalar
   * \sa class CwiseUnaryOp, Cwise::sqrt()
   */
@@ -356,13 +438,25 @@
   };
 };
 
+// Boolean specialization to eliminate -Wimplicit-conversion-floating-point-to-bool warnings.
+template<> struct scalar_sqrt_op<bool> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_sqrt_op)
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator() (const bool& a) const { return a; }
+  template <typename Packet>
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return a; }
+};
+template <>
+struct functor_traits<scalar_sqrt_op<bool> > {
+  enum { Cost = 1, PacketAccess = packet_traits<bool>::Vectorizable };
+};
+
 /** \internal
   * \brief Template functor to compute the reciprocal square root of a scalar
   * \sa class CwiseUnaryOp, Cwise::rsqrt()
   */
 template<typename Scalar> struct scalar_rsqrt_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_rsqrt_op)
-  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return Scalar(1)/numext::sqrt(a); }
+  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const { return numext::rsqrt(a); }
   template <typename Packet>
   EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::prsqrt(a); }
 };
@@ -528,6 +622,23 @@
   };
 };
 
+#if EIGEN_HAS_CXX11_MATH
+/** \internal
+  * \brief Template functor to compute the atanh of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::atanh()
+  */
+template <typename Scalar>
+struct scalar_atanh_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_atanh_op)
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::atanh(a); }
+};
+
+template <typename Scalar>
+struct functor_traits<scalar_atanh_op<Scalar> > {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
+};
+#endif
+
 /** \internal
   * \brief Template functor to compute the sinh of a scalar
   * \sa class CwiseUnaryOp, ArrayBase::sinh()
@@ -547,6 +658,23 @@
   };
 };
 
+#if EIGEN_HAS_CXX11_MATH
+/** \internal
+  * \brief Template functor to compute the asinh of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::asinh()
+  */
+template <typename Scalar>
+struct scalar_asinh_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_asinh_op)
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::asinh(a); }
+};
+
+template <typename Scalar>
+struct functor_traits<scalar_asinh_op<Scalar> > {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
+};
+#endif
+
 /** \internal
   * \brief Template functor to compute the cosh of a scalar
   * \sa class CwiseUnaryOp, ArrayBase::cosh()
@@ -566,6 +694,23 @@
   };
 };
 
+#if EIGEN_HAS_CXX11_MATH
+/** \internal
+  * \brief Template functor to compute the acosh of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::acosh()
+  */
+template <typename Scalar>
+struct scalar_acosh_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_acosh_op)
+  EIGEN_DEVICE_FUNC inline const Scalar operator()(const Scalar& a) const { return numext::acosh(a); }
+};
+
+template <typename Scalar>
+struct functor_traits<scalar_acosh_op<Scalar> > {
+  enum { Cost = 5 * NumTraits<Scalar>::MulCost, PacketAccess = false };
+};
+#endif
+
 /** \internal
   * \brief Template functor to compute the inverse of a scalar
   * \sa class CwiseUnaryOp, Cwise::inverse()
@@ -578,9 +723,13 @@
   EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
   { return internal::pdiv(pset1<Packet>(Scalar(1)),a); }
 };
-template<typename Scalar>
-struct functor_traits<scalar_inverse_op<Scalar> >
-{ enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasDiv }; };
+template <typename Scalar>
+struct functor_traits<scalar_inverse_op<Scalar> > {
+  enum {
+    PacketAccess = packet_traits<Scalar>::HasDiv,
+    Cost = scalar_div_cost<Scalar, PacketAccess>::value
+  };
+};
 
 /** \internal
   * \brief Template functor to compute the square of a scalar
@@ -598,6 +747,19 @@
 struct functor_traits<scalar_square_op<Scalar> >
 { enum { Cost = NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
 
+// Boolean specialization to avoid -Wint-in-bool-context warnings on GCC.
+template<>
+struct scalar_square_op<bool> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_square_op)
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator() (const bool& a) const { return a; }
+  template<typename Packet>
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
+  { return a; }
+};
+template<>
+struct functor_traits<scalar_square_op<bool> >
+{ enum { Cost = 0, PacketAccess = packet_traits<bool>::Vectorizable }; };
+
 /** \internal
   * \brief Template functor to compute the cube of a scalar
   * \sa class CwiseUnaryOp, Cwise::cube()
@@ -614,6 +776,19 @@
 struct functor_traits<scalar_cube_op<Scalar> >
 { enum { Cost = 2*NumTraits<Scalar>::MulCost, PacketAccess = packet_traits<Scalar>::HasMul }; };
 
+// Boolean specialization to avoid -Wint-in-bool-context warnings on GCC.
+template<>
+struct scalar_cube_op<bool> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_cube_op)
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline bool operator() (const bool& a) const { return a; }
+  template<typename Packet>
+  EIGEN_DEPRECATED EIGEN_DEVICE_FUNC inline const Packet packetOp(const Packet& a) const
+  { return a; }
+};
+template<>
+struct functor_traits<scalar_cube_op<bool> >
+{ enum { Cost = 0, PacketAccess = packet_traits<bool>::Vectorizable }; };
+
 /** \internal
   * \brief Template functor to compute the rounded value of a scalar
   * \sa class CwiseUnaryOp, ArrayBase::round()
@@ -653,6 +828,25 @@
 };
 
 /** \internal
+  * \brief Template functor to compute the rounded (with current rounding mode)  value of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::rint()
+  */
+template<typename Scalar> struct scalar_rint_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_rint_op)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE const Scalar operator() (const Scalar& a) const { return numext::rint(a); }
+  template <typename Packet>
+  EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::print(a); }
+};
+template<typename Scalar>
+struct functor_traits<scalar_rint_op<Scalar> >
+{
+  enum {
+    Cost = NumTraits<Scalar>::MulCost,
+    PacketAccess = packet_traits<Scalar>::HasRint
+  };
+};
+
+/** \internal
   * \brief Template functor to compute the ceil of a scalar
   * \sa class CwiseUnaryOp, ArrayBase::ceil()
   */
@@ -678,7 +872,13 @@
 template<typename Scalar> struct scalar_isnan_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_isnan_op)
   typedef bool result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return (numext::isnan)(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const {
+#if defined(SYCL_DEVICE_ONLY)
+    return numext::isnan(a);
+#else
+    return (numext::isnan)(a);
+#endif
+  }
 };
 template<typename Scalar>
 struct functor_traits<scalar_isnan_op<Scalar> >
@@ -696,7 +896,13 @@
 template<typename Scalar> struct scalar_isinf_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_isinf_op)
   typedef bool result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return (numext::isinf)(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const {
+#if defined(SYCL_DEVICE_ONLY)
+    return numext::isinf(a);
+#else
+    return (numext::isinf)(a);
+#endif
+  }
 };
 template<typename Scalar>
 struct functor_traits<scalar_isinf_op<Scalar> >
@@ -714,7 +920,13 @@
 template<typename Scalar> struct scalar_isfinite_op {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_isfinite_op)
   typedef bool result_type;
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const { return (numext::isfinite)(a); }
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE result_type operator() (const Scalar& a) const {
+#if defined(SYCL_DEVICE_ONLY)
+    return numext::isfinite(a);
+#else
+    return (numext::isfinite)(a);
+#endif
+  }
 };
 template<typename Scalar>
 struct functor_traits<scalar_isfinite_op<Scalar> >
@@ -746,9 +958,9 @@
   * \brief Template functor to compute the signum of a scalar
   * \sa class CwiseUnaryOp, Cwise::sign()
   */
-template<typename Scalar,bool iscpx=(NumTraits<Scalar>::IsComplex!=0) > struct scalar_sign_op;
+template<typename Scalar,bool is_complex=(NumTraits<Scalar>::IsComplex!=0), bool is_integer=(NumTraits<Scalar>::IsInteger!=0) > struct scalar_sign_op;
 template<typename Scalar>
-struct scalar_sign_op<Scalar,false> {
+struct scalar_sign_op<Scalar, false, true> {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_sign_op)
   EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const
   {
@@ -758,8 +970,21 @@
   //template <typename Packet>
   //EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psign(a); }
 };
+
 template<typename Scalar>
-struct scalar_sign_op<Scalar,true> {
+struct scalar_sign_op<Scalar, false, false> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_sign_op)
+  EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const
+  {
+    return (numext::isnan)(a) ? a : Scalar( (a>Scalar(0)) - (a<Scalar(0)) );
+  }
+  //TODO
+  //template <typename Packet>
+  //EIGEN_DEVICE_FUNC inline Packet packetOp(const Packet& a) const { return internal::psign(a); }
+};
+
+template<typename Scalar, bool is_integer>
+struct scalar_sign_op<Scalar,true, is_integer> {
   EIGEN_EMPTY_STRUCT_CTOR(scalar_sign_op)
   EIGEN_DEVICE_FUNC inline const Scalar operator() (const Scalar& a) const
   {
@@ -768,7 +993,7 @@
     if (aa==real_type(0))
       return Scalar(0);
     aa = real_type(1)/aa;
-    return Scalar(real(a)*aa, imag(a)*aa );
+    return Scalar(a.real()*aa, a.imag()*aa );
   }
   //TODO
   //template <typename Packet>
@@ -777,7 +1002,7 @@
 template<typename Scalar>
 struct functor_traits<scalar_sign_op<Scalar> >
 { enum {
-    Cost = 
+    Cost =
         NumTraits<Scalar>::IsComplex
         ? ( 8*NumTraits<Scalar>::MulCost  ) // roughly
         : ( 3*NumTraits<Scalar>::AddCost),
@@ -785,6 +1010,120 @@
   };
 };
 
+/** \internal
+  * \brief Template functor to compute the logistic function of a scalar
+  * \sa class CwiseUnaryOp, ArrayBase::logistic()
+  */
+template <typename T>
+struct scalar_logistic_op {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_logistic_op)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T operator()(const T& x) const {
+    return packetOp(x);
+  }
+
+  template <typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Packet packetOp(const Packet& x) const {
+    const Packet one = pset1<Packet>(T(1));
+    return pdiv(one, padd(one, pexp(pnegate(x))));
+  }
+};
+
+#ifndef EIGEN_GPU_COMPILE_PHASE
+/** \internal
+  * \brief Template specialization of the logistic function for float.
+  *
+  *  Uses just a 9/10-degree rational interpolant which
+  *  interpolates 1/(1+exp(-x)) - 0.5 up to a couple of ulps in the range
+  *  [-9, 18]. Below -9 we use the more accurate approximation
+  *  1/(1+exp(-x)) ~= exp(x), and above 18 the logistic function is 1 withing
+  *  one ulp. The shifted logistic is interpolated because it was easier to
+  *  make the fit converge.
+  *
+  */
+template <>
+struct scalar_logistic_op<float> {
+  EIGEN_EMPTY_STRUCT_CTOR(scalar_logistic_op)
+  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE float operator()(const float& x) const {
+    return packetOp(x);
+  }
+
+  template <typename Packet> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+  Packet packetOp(const Packet& _x) const {
+    const Packet cutoff_lower = pset1<Packet>(-9.f);
+    const Packet lt_mask = pcmp_lt<Packet>(_x, cutoff_lower);
+    const bool any_small = predux_any(lt_mask);
+
+    // The upper cut-off is the smallest x for which the rational approximation evaluates to 1.
+    // Choosing this value saves us a few instructions clamping the results at the end.
+#ifdef EIGEN_VECTORIZE_FMA
+    const Packet cutoff_upper = pset1<Packet>(15.7243833541870117f);
+#else
+    const Packet cutoff_upper = pset1<Packet>(15.6437711715698242f);
+#endif
+    const Packet x = pmin(_x, cutoff_upper);
+
+    // The monomial coefficients of the numerator polynomial (odd).
+    const Packet alpha_1 = pset1<Packet>(2.48287947061529e-01f);
+    const Packet alpha_3 = pset1<Packet>(8.51377133304701e-03f);
+    const Packet alpha_5 = pset1<Packet>(6.08574864600143e-05f);
+    const Packet alpha_7 = pset1<Packet>(1.15627324459942e-07f);
+    const Packet alpha_9 = pset1<Packet>(4.37031012579801e-11f);
+
+    // The monomial coefficients of the denominator polynomial (even).
+    const Packet beta_0 = pset1<Packet>(9.93151921023180e-01f);
+    const Packet beta_2 = pset1<Packet>(1.16817656904453e-01f);
+    const Packet beta_4 = pset1<Packet>(1.70198817374094e-03f);
+    const Packet beta_6 = pset1<Packet>(6.29106785017040e-06f);
+    const Packet beta_8 = pset1<Packet>(5.76102136993427e-09f);
+    const Packet beta_10 = pset1<Packet>(6.10247389755681e-13f);
+
+    // Since the polynomials are odd/even, we need x^2.
+    const Packet x2 = pmul(x, x);
+
+    // Evaluate the numerator polynomial p.
+    Packet p = pmadd(x2, alpha_9, alpha_7);
+    p = pmadd(x2, p, alpha_5);
+    p = pmadd(x2, p, alpha_3);
+    p = pmadd(x2, p, alpha_1);
+    p = pmul(x, p);
+
+    // Evaluate the denominator polynomial q.
+    Packet q = pmadd(x2, beta_10, beta_8);
+    q = pmadd(x2, q, beta_6);
+    q = pmadd(x2, q, beta_4);
+    q = pmadd(x2, q, beta_2);
+    q = pmadd(x2, q, beta_0);
+    // Divide the numerator by the denominator and shift it up.
+    const Packet logistic = padd(pdiv(p, q), pset1<Packet>(0.5f));
+    if (EIGEN_PREDICT_FALSE(any_small)) {
+      const Packet exponential = pexp(_x);
+      return pselect(lt_mask, exponential, logistic);
+    } else {
+      return logistic;
+    }
+  }
+};
+#endif  // #ifndef EIGEN_GPU_COMPILE_PHASE
+
+template <typename T>
+struct functor_traits<scalar_logistic_op<T> > {
+  enum {
+    // The cost estimate for float here here is for the common(?) case where
+    // all arguments are greater than -9.
+    Cost = scalar_div_cost<T, packet_traits<T>::HasDiv>::value +
+           (internal::is_same<T, float>::value
+                ? NumTraits<T>::AddCost * 15 + NumTraits<T>::MulCost * 11
+                : NumTraits<T>::AddCost * 2 +
+                      functor_traits<scalar_exp_op<T> >::Cost),
+    PacketAccess =
+        packet_traits<T>::HasAdd && packet_traits<T>::HasDiv &&
+        (internal::is_same<T, float>::value
+             ? packet_traits<T>::HasMul && packet_traits<T>::HasMax &&
+                   packet_traits<T>::HasMin
+             : packet_traits<T>::HasNegate && packet_traits<T>::HasExp)
+  };
+};
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
index e3980f6..f35b760 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralBlockPanelKernel.h
@@ -15,7 +15,13 @@
 
 namespace internal {
 
-template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs=false, bool _ConjRhs=false>
+enum GEBPPacketSizeType {
+  GEBPPacketFull = 0,
+  GEBPPacketHalf,
+  GEBPPacketQuarter
+};
+
+template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs=false, bool _ConjRhs=false, int Arch=Architecture::Target, int _PacketSize=GEBPPacketFull>
 class gebp_traits;
 
 
@@ -25,16 +31,42 @@
   return a<=0 ? b : a;
 }
 
-#if EIGEN_ARCH_i386_OR_x86_64
-const std::ptrdiff_t defaultL1CacheSize = 32*1024;
-const std::ptrdiff_t defaultL2CacheSize = 256*1024;
-const std::ptrdiff_t defaultL3CacheSize = 2*1024*1024;
+#if defined(EIGEN_DEFAULT_L1_CACHE_SIZE)
+#define EIGEN_SET_DEFAULT_L1_CACHE_SIZE(val) EIGEN_DEFAULT_L1_CACHE_SIZE
 #else
-const std::ptrdiff_t defaultL1CacheSize = 16*1024;
-const std::ptrdiff_t defaultL2CacheSize = 512*1024;
-const std::ptrdiff_t defaultL3CacheSize = 512*1024;
+#define EIGEN_SET_DEFAULT_L1_CACHE_SIZE(val) val
+#endif // defined(EIGEN_DEFAULT_L1_CACHE_SIZE)
+
+#if defined(EIGEN_DEFAULT_L2_CACHE_SIZE)
+#define EIGEN_SET_DEFAULT_L2_CACHE_SIZE(val) EIGEN_DEFAULT_L2_CACHE_SIZE
+#else
+#define EIGEN_SET_DEFAULT_L2_CACHE_SIZE(val) val
+#endif // defined(EIGEN_DEFAULT_L2_CACHE_SIZE)
+
+#if defined(EIGEN_DEFAULT_L3_CACHE_SIZE)
+#define EIGEN_SET_DEFAULT_L3_CACHE_SIZE(val) EIGEN_DEFAULT_L3_CACHE_SIZE
+#else
+#define EIGEN_SET_DEFAULT_L3_CACHE_SIZE(val) val
+#endif // defined(EIGEN_DEFAULT_L3_CACHE_SIZE)
+  
+#if EIGEN_ARCH_i386_OR_x86_64
+const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(32*1024);
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(256*1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(2*1024*1024);
+#elif EIGEN_ARCH_PPC
+const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(64*1024);
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(512*1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(4*1024*1024);
+#else
+const std::ptrdiff_t defaultL1CacheSize = EIGEN_SET_DEFAULT_L1_CACHE_SIZE(16*1024);
+const std::ptrdiff_t defaultL2CacheSize = EIGEN_SET_DEFAULT_L2_CACHE_SIZE(512*1024);
+const std::ptrdiff_t defaultL3CacheSize = EIGEN_SET_DEFAULT_L3_CACHE_SIZE(512*1024);
 #endif
 
+#undef EIGEN_SET_DEFAULT_L1_CACHE_SIZE
+#undef EIGEN_SET_DEFAULT_L2_CACHE_SIZE
+#undef EIGEN_SET_DEFAULT_L3_CACHE_SIZE
+
 /** \internal */
 struct CacheSizes {
   CacheSizes(): m_l1(-1),m_l2(-1),m_l3(-1) {
@@ -50,7 +82,6 @@
   std::ptrdiff_t m_l3;
 };
 
-
 /** \internal */
 inline void manage_caching_sizes(Action action, std::ptrdiff_t* l1, std::ptrdiff_t* l2, std::ptrdiff_t* l3)
 {
@@ -101,6 +132,16 @@
   // at the register level. This small horizontal panel has to stay within L1 cache.
   std::ptrdiff_t l1, l2, l3;
   manage_caching_sizes(GetAction, &l1, &l2, &l3);
+  #ifdef EIGEN_VECTORIZE_AVX512
+  // We need to find a rationale for that, but without this adjustment,
+  // performance with AVX512 is pretty bad, like -20% slower.
+  // One reason is that with increasing packet-size, the blocking size k
+  // has to become pretty small if we want that 1 lhs panel fit within L1.
+  // For instance, with the 3pX4 kernel and double, the size of the lhs+rhs panels are:
+  //   k*(3*64 + 4*8) Bytes, with l1=32kBytes, and k%8=0, we have k=144.
+  // This is quite small for a good reuse of the accumulation registers.
+  l1 *= 4;
+  #endif
 
   if (num_threads > 1) {
     typedef typename Traits::ResScalar ResScalar;
@@ -115,7 +156,8 @@
     // registers. However once the latency is hidden there is no point in
     // increasing the value of k, so we'll cap it at 320 (value determined
     // experimentally).
-    const Index k_cache = (numext::mini<Index>)((l1-ksub)/kdiv, 320);
+    // To avoid that k vanishes, we make k_cache at least as big as kr
+    const Index k_cache = numext::maxi<Index>(kr, (numext::mini<Index>)((l1-ksub)/kdiv, 320));
     if (k_cache < k) {
       k = k_cache - (k_cache % kr);
       eigen_internal_assert(k > 0);
@@ -307,35 +349,60 @@
   computeProductBlockingSizes<LhsScalar,RhsScalar,1,Index>(k, m, n, num_threads);
 }
 
-#ifdef EIGEN_HAS_SINGLE_INSTRUCTION_CJMADD
-  #define CJMADD(CJ,A,B,C,T)  C = CJ.pmadd(A,B,C);
-#else
+template <typename RhsPacket, typename RhsPacketx4, int registers_taken>
+struct RhsPanelHelper {
+ private:
+  static const int remaining_registers = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS - registers_taken;
+ public:
+  typedef typename conditional<remaining_registers>=4, RhsPacketx4, RhsPacket>::type type;
+};
 
-  // FIXME (a bit overkill maybe ?)
+template <typename Packet>
+struct QuadPacket
+{
+  Packet B_0, B1, B2, B3;
+  const Packet& get(const FixedInt<0>&) const { return B_0; }
+  const Packet& get(const FixedInt<1>&) const { return B1; }
+  const Packet& get(const FixedInt<2>&) const { return B2; }
+  const Packet& get(const FixedInt<3>&) const { return B3; }
+};
 
-  template<typename CJ, typename A, typename B, typename C, typename T> struct gebp_madd_selector {
-    EIGEN_ALWAYS_INLINE static void run(const CJ& cj, A& a, B& b, C& c, T& /*t*/)
-    {
-      c = cj.pmadd(a,b,c);
-    }
-  };
+template <int N, typename T1, typename T2, typename T3>
+struct packet_conditional { typedef T3 type; };
 
-  template<typename CJ, typename T> struct gebp_madd_selector<CJ,T,T,T,T> {
-    EIGEN_ALWAYS_INLINE static void run(const CJ& cj, T& a, T& b, T& c, T& t)
-    {
-      t = b; t = cj.pmul(a,t); c = padd(c,t);
-    }
-  };
+template <typename T1, typename T2, typename T3>
+struct packet_conditional<GEBPPacketFull, T1, T2, T3> { typedef T1 type; };
 
-  template<typename CJ, typename A, typename B, typename C, typename T>
-  EIGEN_STRONG_INLINE void gebp_madd(const CJ& cj, A& a, B& b, C& c, T& t)
-  {
-    gebp_madd_selector<CJ,A,B,C,T>::run(cj,a,b,c,t);
-  }
+template <typename T1, typename T2, typename T3>
+struct packet_conditional<GEBPPacketHalf, T1, T2, T3> { typedef T2 type; };
 
-  #define CJMADD(CJ,A,B,C,T)  gebp_madd(CJ,A,B,C,T);
-//   #define CJMADD(CJ,A,B,C,T)  T = B; T = CJ.pmul(A,T); C = padd(C,T);
-#endif
+#define PACKET_DECL_COND_PREFIX(prefix, name, packet_size)         \
+  typedef typename packet_conditional<packet_size,                 \
+                                      typename packet_traits<name ## Scalar>::type, \
+                                      typename packet_traits<name ## Scalar>::half, \
+                                      typename unpacket_traits<typename packet_traits<name ## Scalar>::half>::half>::type \
+  prefix ## name ## Packet
+
+#define PACKET_DECL_COND(name, packet_size)                        \
+  typedef typename packet_conditional<packet_size,                 \
+                                      typename packet_traits<name ## Scalar>::type, \
+                                      typename packet_traits<name ## Scalar>::half, \
+                                      typename unpacket_traits<typename packet_traits<name ## Scalar>::half>::half>::type \
+  name ## Packet
+
+#define PACKET_DECL_COND_SCALAR_PREFIX(prefix, packet_size)        \
+  typedef typename packet_conditional<packet_size,                 \
+                                      typename packet_traits<Scalar>::type, \
+                                      typename packet_traits<Scalar>::half, \
+                                      typename unpacket_traits<typename packet_traits<Scalar>::half>::half>::type \
+  prefix ## ScalarPacket
+
+#define PACKET_DECL_COND_SCALAR(packet_size)                       \
+  typedef typename packet_conditional<packet_size,                 \
+                                      typename packet_traits<Scalar>::type, \
+                                      typename packet_traits<Scalar>::half, \
+                                      typename unpacket_traits<typename packet_traits<Scalar>::half>::half>::type \
+  ScalarPacket
 
 /* Vectorization logic
  *  real*real: unpack rhs to constant packets, ...
@@ -347,7 +414,7 @@
  *  cplx*real : unpack rhs to constant packets, ...
  *  real*cplx : load lhs as (a0,a0,a1,a1), and mul as usual
  */
-template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs, bool _ConjRhs>
+template<typename _LhsScalar, typename _RhsScalar, bool _ConjLhs, bool _ConjRhs, int Arch, int _PacketSize>
 class gebp_traits
 {
 public:
@@ -355,13 +422,17 @@
   typedef _RhsScalar RhsScalar;
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
+  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+
   enum {
     ConjLhs = _ConjLhs,
     ConjRhs = _ConjRhs,
-    Vectorizable = packet_traits<LhsScalar>::Vectorizable && packet_traits<RhsScalar>::Vectorizable,
-    LhsPacketSize = Vectorizable ? packet_traits<LhsScalar>::size : 1,
-    RhsPacketSize = Vectorizable ? packet_traits<RhsScalar>::size : 1,
-    ResPacketSize = Vectorizable ? packet_traits<ResScalar>::size : 1,
+    Vectorizable = unpacket_traits<_LhsPacket>::vectorizable && unpacket_traits<_RhsPacket>::vectorizable,
+    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
     
     NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
 
@@ -370,10 +441,12 @@
 
     // register block size along the M direction (currently, this one cannot be modified)
     default_mr = (EIGEN_PLAIN_ENUM_MIN(16,NumberOfRegisters)/2/nr)*LhsPacketSize,
-#if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD) && !defined(EIGEN_VECTORIZE_ALTIVEC) && !defined(EIGEN_VECTORIZE_VSX)
-    // we assume 16 registers
+#if defined(EIGEN_HAS_SINGLE_INSTRUCTION_MADD) && !defined(EIGEN_VECTORIZE_ALTIVEC) && !defined(EIGEN_VECTORIZE_VSX) \
+    && ((!EIGEN_COMP_MSVC) || (EIGEN_COMP_MSVC>=1914))
+    // we assume 16 registers or more
     // See bug 992, if the scalar type is not vectorizable but that EIGEN_HAS_SINGLE_INSTRUCTION_MADD is defined,
     // then using 3*LhsPacketSize triggers non-implemented paths in syrk.
+    // Bug 1515: MSVC prior to v19.14 yields to register spilling.
     mr = Vectorizable ? 3*LhsPacketSize : default_mr,
 #else
     mr = default_mr,
@@ -383,37 +456,41 @@
     RhsProgress = 1
   };
 
-  typedef typename packet_traits<LhsScalar>::type  _LhsPacket;
-  typedef typename packet_traits<RhsScalar>::type  _RhsPacket;
-  typedef typename packet_traits<ResScalar>::type  _ResPacket;
 
   typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
   typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
   typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef LhsPacket LhsPacket4Packing;
 
+  typedef QuadPacket<RhsPacket> RhsPacketx4;
   typedef ResPacket AccPacket;
   
   EIGEN_STRONG_INLINE void initAcc(AccPacket& p)
   {
     p = pset1<ResPacket>(ResScalar(0));
   }
-  
-  EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1, RhsPacket& b2, RhsPacket& b3)
-  {
-    pbroadcast4(b, b0, b1, b2, b3);
-  }
-  
-//   EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1)
-//   {
-//     pbroadcast2(b, b0, b1);
-//   }
-  
+
   template<typename RhsPacketType>
   EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const
   {
     dest = pset1<RhsPacketType>(*b);
   }
-  
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
+  {
+    pbroadcast4(b, dest.B_0, dest.B1, dest.B2, dest.B3);
+  }
+
+  template<typename RhsPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const
+  {
+    loadRhs(b, dest);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
+  {
+  }
+
   EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
   {
     dest = ploadquad<RhsPacket>(b);
@@ -431,8 +508,8 @@
     dest = ploadu<LhsPacketType>(a);
   }
 
-  template<typename LhsPacketType, typename RhsPacketType, typename AccPacketType>
-  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, AccPacketType& tmp) const
+  template<typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const LaneIdType&) const
   {
     conj_helper<LhsPacketType,RhsPacketType,ConjLhs,ConjRhs> cj;
     // It would be a lot cleaner to call pmadd all the time. Unfortunately if we
@@ -447,6 +524,12 @@
 #endif
   }
 
+  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
+  {
+    madd(a, b.get(lane), c, tmp, lane);
+  }
+
   EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const
   {
     r = pmadd(c,alpha,r);
@@ -460,21 +543,25 @@
 
 };
 
-template<typename RealScalar, bool _ConjLhs>
-class gebp_traits<std::complex<RealScalar>, RealScalar, _ConjLhs, false>
+template<typename RealScalar, bool _ConjLhs, int Arch, int _PacketSize>
+class gebp_traits<std::complex<RealScalar>, RealScalar, _ConjLhs, false, Arch, _PacketSize>
 {
 public:
   typedef std::complex<RealScalar> LhsScalar;
   typedef RealScalar RhsScalar;
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
+  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+
   enum {
     ConjLhs = _ConjLhs,
     ConjRhs = false,
-    Vectorizable = packet_traits<LhsScalar>::Vectorizable && packet_traits<RhsScalar>::Vectorizable,
-    LhsPacketSize = Vectorizable ? packet_traits<LhsScalar>::size : 1,
-    RhsPacketSize = Vectorizable ? packet_traits<RhsScalar>::size : 1,
-    ResPacketSize = Vectorizable ? packet_traits<ResScalar>::size : 1,
+    Vectorizable = unpacket_traits<_LhsPacket>::vectorizable && unpacket_traits<_RhsPacket>::vectorizable,
+    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
     
     NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
     nr = 4,
@@ -489,13 +576,12 @@
     RhsProgress = 1
   };
 
-  typedef typename packet_traits<LhsScalar>::type  _LhsPacket;
-  typedef typename packet_traits<RhsScalar>::type  _RhsPacket;
-  typedef typename packet_traits<ResScalar>::type  _ResPacket;
-
   typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
   typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
   typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef LhsPacket LhsPacket4Packing;
+
+  typedef QuadPacket<RhsPacket> RhsPacketx4;
 
   typedef ResPacket AccPacket;
 
@@ -504,13 +590,42 @@
     p = pset1<ResPacket>(ResScalar(0));
   }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const
+  template<typename RhsPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const
   {
-    dest = pset1<RhsPacket>(*b);
+    dest = pset1<RhsPacketType>(*b);
   }
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
+  {
+    pbroadcast4(b, dest.B_0, dest.B1, dest.B2, dest.B3);
+  }
+
+  template<typename RhsPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const
+  {
+    loadRhs(b, dest);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
+  {}
   
   EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
   {
+    loadRhsQuad_impl(b,dest, typename conditional<RhsPacketSize==16,true_type,false_type>::type());
+  }
+
+  EIGEN_STRONG_INLINE void loadRhsQuad_impl(const RhsScalar* b, RhsPacket& dest, const true_type&) const
+  {
+    // FIXME we can do better!
+    // what we want here is a ploadheight
+    RhsScalar tmp[4] = {b[0],b[0],b[1],b[1]};
+    dest = ploadquad<RhsPacket>(tmp);
+  }
+
+  EIGEN_STRONG_INLINE void loadRhsQuad_impl(const RhsScalar* b, RhsPacket& dest, const false_type&) const
+  {
+    eigen_internal_assert(RhsPacketSize<=8);
     dest = pset1<RhsPacket>(*b);
   }
 
@@ -519,27 +634,20 @@
     dest = pload<LhsPacket>(a);
   }
 
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacket& dest) const
+  template<typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
   {
-    dest = ploadu<LhsPacket>(a);
+    dest = ploadu<LhsPacketType>(a);
   }
 
-  EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1, RhsPacket& b2, RhsPacket& b3)
-  {
-    pbroadcast4(b, b0, b1, b2, b3);
-  }
-  
-//   EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1)
-//   {
-//     pbroadcast2(b, b0, b1);
-//   }
-
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& tmp) const
+  template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const LaneIdType&) const
   {
     madd_impl(a, b, c, tmp, typename conditional<Vectorizable,true_type,false_type>::type());
   }
 
-  EIGEN_STRONG_INLINE void madd_impl(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& tmp, const true_type&) const
+  template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType>
+  EIGEN_STRONG_INLINE void madd_impl(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const true_type&) const
   {
 #ifdef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
     EIGEN_UNUSED_VARIABLE(tmp);
@@ -554,13 +662,20 @@
     c += a * b;
   }
 
-  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const
+  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
   {
+    madd(a, b.get(lane), c, tmp, lane);
+  }
+
+  template <typename ResPacketType, typename AccPacketType>
+  EIGEN_STRONG_INLINE void acc(const AccPacketType& c, const ResPacketType& alpha, ResPacketType& r) const
+  {
+    conj_helper<ResPacketType,ResPacketType,ConjLhs,false> cj;
     r = cj.pmadd(c,alpha,r);
   }
 
 protected:
-  conj_helper<ResPacket,ResPacket,ConjLhs,false> cj;
 };
 
 template<typename Packet>
@@ -579,13 +694,57 @@
   return res;
 }
 
+// note that for DoublePacket<RealPacket> the "4" in "downto4"
+// corresponds to the number of complexes, so it means "8"
+// it terms of real coefficients.
+
 template<typename Packet>
-const DoublePacket<Packet>& predux_downto4(const DoublePacket<Packet> &a)
+const DoublePacket<Packet>&
+predux_half_dowto4(const DoublePacket<Packet> &a,
+                   typename enable_if<unpacket_traits<Packet>::size<=8>::type* = 0)
 {
   return a;
 }
 
-template<typename Packet> struct unpacket_traits<DoublePacket<Packet> > { typedef DoublePacket<Packet> half; };
+template<typename Packet>
+DoublePacket<typename unpacket_traits<Packet>::half>
+predux_half_dowto4(const DoublePacket<Packet> &a,
+                   typename enable_if<unpacket_traits<Packet>::size==16>::type* = 0)
+{
+  // yes, that's pretty hackish :(
+  DoublePacket<typename unpacket_traits<Packet>::half> res;
+  typedef std::complex<typename unpacket_traits<Packet>::type> Cplx;
+  typedef typename packet_traits<Cplx>::type CplxPacket;
+  res.first  = predux_half_dowto4(CplxPacket(a.first)).v;
+  res.second = predux_half_dowto4(CplxPacket(a.second)).v;
+  return res;
+}
+
+// same here, "quad" actually means "8" in terms of real coefficients
+template<typename Scalar, typename RealPacket>
+void loadQuadToDoublePacket(const Scalar* b, DoublePacket<RealPacket>& dest,
+                            typename enable_if<unpacket_traits<RealPacket>::size<=8>::type* = 0)
+{
+  dest.first  = pset1<RealPacket>(numext::real(*b));
+  dest.second = pset1<RealPacket>(numext::imag(*b));
+}
+
+template<typename Scalar, typename RealPacket>
+void loadQuadToDoublePacket(const Scalar* b, DoublePacket<RealPacket>& dest,
+                            typename enable_if<unpacket_traits<RealPacket>::size==16>::type* = 0)
+{
+  // yes, that's pretty hackish too :(
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  RealScalar r[4] = {numext::real(b[0]), numext::real(b[0]), numext::real(b[1]), numext::real(b[1])};
+  RealScalar i[4] = {numext::imag(b[0]), numext::imag(b[0]), numext::imag(b[1]), numext::imag(b[1])};
+  dest.first  = ploadquad<RealPacket>(r);
+  dest.second = ploadquad<RealPacket>(i);
+}
+
+
+template<typename Packet> struct unpacket_traits<DoublePacket<Packet> > {
+  typedef DoublePacket<typename unpacket_traits<Packet>::half> half;
+};
 // template<typename Packet>
 // DoublePacket<Packet> pmadd(const DoublePacket<Packet> &a, const DoublePacket<Packet> &b)
 // {
@@ -595,8 +754,8 @@
 //   return res;
 // }
 
-template<typename RealScalar, bool _ConjLhs, bool _ConjRhs>
-class gebp_traits<std::complex<RealScalar>, std::complex<RealScalar>, _ConjLhs, _ConjRhs >
+template<typename RealScalar, bool _ConjLhs, bool _ConjRhs, int Arch, int _PacketSize>
+class gebp_traits<std::complex<RealScalar>, std::complex<RealScalar>, _ConjLhs, _ConjRhs, Arch, _PacketSize >
 {
 public:
   typedef std::complex<RealScalar>  Scalar;
@@ -604,15 +763,21 @@
   typedef std::complex<RealScalar>  RhsScalar;
   typedef std::complex<RealScalar>  ResScalar;
   
+  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+  PACKET_DECL_COND(Real, _PacketSize);
+  PACKET_DECL_COND_SCALAR(_PacketSize);
+
   enum {
     ConjLhs = _ConjLhs,
     ConjRhs = _ConjRhs,
-    Vectorizable = packet_traits<RealScalar>::Vectorizable
-                && packet_traits<Scalar>::Vectorizable,
-    RealPacketSize  = Vectorizable ? packet_traits<RealScalar>::size : 1,
-    ResPacketSize   = Vectorizable ? packet_traits<ResScalar>::size : 1,
-    LhsPacketSize = Vectorizable ? packet_traits<LhsScalar>::size : 1,
-    RhsPacketSize = Vectorizable ? packet_traits<RhsScalar>::size : 1,
+    Vectorizable = unpacket_traits<RealPacket>::vectorizable
+                && unpacket_traits<ScalarPacket>::vectorizable,
+    ResPacketSize   = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
+    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<RhsScalar>::size : 1,
+    RealPacketSize  = Vectorizable ? unpacket_traits<RealPacket>::size : 1,
 
     // FIXME: should depend on NumberOfRegisters
     nr = 4,
@@ -622,14 +787,16 @@
     RhsProgress = 1
   };
   
-  typedef typename packet_traits<RealScalar>::type RealPacket;
-  typedef typename packet_traits<Scalar>::type     ScalarPacket;
-  typedef DoublePacket<RealPacket> DoublePacketType;
+  typedef DoublePacket<RealPacket>                 DoublePacketType;
 
+  typedef typename conditional<Vectorizable,ScalarPacket,Scalar>::type LhsPacket4Packing;
   typedef typename conditional<Vectorizable,RealPacket,  Scalar>::type LhsPacket;
   typedef typename conditional<Vectorizable,DoublePacketType,Scalar>::type RhsPacket;
   typedef typename conditional<Vectorizable,ScalarPacket,Scalar>::type ResPacket;
   typedef typename conditional<Vectorizable,DoublePacketType,Scalar>::type AccPacket;
+
+  // this actualy holds 8 packets!
+  typedef QuadPacket<RhsPacket> RhsPacketx4;
   
   EIGEN_STRONG_INLINE void initAcc(Scalar& p) { p = Scalar(0); }
 
@@ -640,17 +807,41 @@
   }
 
   // Scalar path
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, ResPacket& dest) const
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, ScalarPacket& dest) const
   {
-    dest = pset1<ResPacket>(*b);
+    dest = pset1<ScalarPacket>(*b);
   }
 
   // Vectorized path
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, DoublePacketType& dest) const
+  template<typename RealPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, DoublePacket<RealPacketType>& dest) const
   {
-    dest.first  = pset1<RealPacket>(real(*b));
-    dest.second = pset1<RealPacket>(imag(*b));
+    dest.first  = pset1<RealPacketType>(numext::real(*b));
+    dest.second = pset1<RealPacketType>(numext::imag(*b));
   }
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
+  {
+    loadRhs(b, dest.B_0);
+    loadRhs(b + 1, dest.B1);
+    loadRhs(b + 2, dest.B2);
+    loadRhs(b + 3, dest.B3);
+  }
+
+  // Scalar path
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, ScalarPacket& dest) const
+  {
+    loadRhs(b, dest);
+  }
+
+  // Vectorized path
+  template<typename RealPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, DoublePacket<RealPacketType>& dest) const
+  {
+    loadRhs(b, dest);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const {}
   
   EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, ResPacket& dest) const
   {
@@ -658,33 +849,7 @@
   }
   EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, DoublePacketType& dest) const
   {
-    eigen_internal_assert(unpacket_traits<ScalarPacket>::size<=4);
-    loadRhs(b,dest);
-  }
-  
-  EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1, RhsPacket& b2, RhsPacket& b3)
-  {
-    // FIXME not sure that's the best way to implement it!
-    loadRhs(b+0, b0);
-    loadRhs(b+1, b1);
-    loadRhs(b+2, b2);
-    loadRhs(b+3, b3);
-  }
-  
-  // Vectorized path
-  EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, DoublePacketType& b0, DoublePacketType& b1)
-  {
-    // FIXME not sure that's the best way to implement it!
-    loadRhs(b+0, b0);
-    loadRhs(b+1, b1);
-  }
-  
-  // Scalar path
-  EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsScalar& b0, RhsScalar& b1)
-  {
-    // FIXME not sure that's the best way to implement it!
-    loadRhs(b+0, b0);
-    loadRhs(b+1, b1);
+    loadQuadToDoublePacket(b,dest);
   }
 
   // nothing special here
@@ -693,47 +858,59 @@
     dest = pload<LhsPacket>((const typename unpacket_traits<LhsPacket>::type*)(a));
   }
 
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacket& dest) const
+  template<typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
   {
-    dest = ploadu<LhsPacket>((const typename unpacket_traits<LhsPacket>::type*)(a));
+    dest = ploadu<LhsPacketType>((const typename unpacket_traits<LhsPacketType>::type*)(a));
   }
 
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, DoublePacketType& c, RhsPacket& /*tmp*/) const
+  template<typename LhsPacketType, typename RhsPacketType, typename ResPacketType, typename TmpType, typename LaneIdType>
+  EIGEN_STRONG_INLINE
+  typename enable_if<!is_same<RhsPacketType,RhsPacketx4>::value>::type
+  madd(const LhsPacketType& a, const RhsPacketType& b, DoublePacket<ResPacketType>& c, TmpType& /*tmp*/, const LaneIdType&) const
   {
     c.first   = padd(pmul(a,b.first), c.first);
     c.second  = padd(pmul(a,b.second),c.second);
   }
 
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, ResPacket& c, RhsPacket& /*tmp*/) const
+  template<typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, ResPacket& c, RhsPacket& /*tmp*/, const LaneIdType&) const
   {
     c = cj.pmadd(a,b,c);
   }
+
+  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
+  {
+    madd(a, b.get(lane), c, tmp, lane);
+  }
   
   EIGEN_STRONG_INLINE void acc(const Scalar& c, const Scalar& alpha, Scalar& r) const { r += alpha * c; }
   
-  EIGEN_STRONG_INLINE void acc(const DoublePacketType& c, const ResPacket& alpha, ResPacket& r) const
+  template<typename RealPacketType, typename ResPacketType>
+  EIGEN_STRONG_INLINE void acc(const DoublePacket<RealPacketType>& c, const ResPacketType& alpha, ResPacketType& r) const
   {
     // assemble c
-    ResPacket tmp;
+    ResPacketType tmp;
     if((!ConjLhs)&&(!ConjRhs))
     {
-      tmp = pcplxflip(pconj(ResPacket(c.second)));
-      tmp = padd(ResPacket(c.first),tmp);
+      tmp = pcplxflip(pconj(ResPacketType(c.second)));
+      tmp = padd(ResPacketType(c.first),tmp);
     }
     else if((!ConjLhs)&&(ConjRhs))
     {
-      tmp = pconj(pcplxflip(ResPacket(c.second)));
-      tmp = padd(ResPacket(c.first),tmp);
+      tmp = pconj(pcplxflip(ResPacketType(c.second)));
+      tmp = padd(ResPacketType(c.first),tmp);
     }
     else if((ConjLhs)&&(!ConjRhs))
     {
-      tmp = pcplxflip(ResPacket(c.second));
-      tmp = padd(pconj(ResPacket(c.first)),tmp);
+      tmp = pcplxflip(ResPacketType(c.second));
+      tmp = padd(pconj(ResPacketType(c.first)),tmp);
     }
     else if((ConjLhs)&&(ConjRhs))
     {
-      tmp = pcplxflip(ResPacket(c.second));
-      tmp = psub(pconj(ResPacket(c.first)),tmp);
+      tmp = pcplxflip(ResPacketType(c.second));
+      tmp = psub(pconj(ResPacketType(c.first)),tmp);
     }
     
     r = pmadd(tmp,alpha,r);
@@ -743,8 +920,8 @@
   conj_helper<LhsScalar,RhsScalar,ConjLhs,ConjRhs> cj;
 };
 
-template<typename RealScalar, bool _ConjRhs>
-class gebp_traits<RealScalar, std::complex<RealScalar>, false, _ConjRhs >
+template<typename RealScalar, bool _ConjRhs, int Arch, int _PacketSize>
+class gebp_traits<RealScalar, std::complex<RealScalar>, false, _ConjRhs, Arch, _PacketSize >
 {
 public:
   typedef std::complex<RealScalar>  Scalar;
@@ -752,14 +929,25 @@
   typedef Scalar      RhsScalar;
   typedef Scalar      ResScalar;
 
+  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Real, _PacketSize);
+  PACKET_DECL_COND_SCALAR_PREFIX(_, _PacketSize);
+
+#undef PACKET_DECL_COND_SCALAR_PREFIX
+#undef PACKET_DECL_COND_PREFIX
+#undef PACKET_DECL_COND_SCALAR
+#undef PACKET_DECL_COND
+
   enum {
     ConjLhs = false,
     ConjRhs = _ConjRhs,
-    Vectorizable = packet_traits<RealScalar>::Vectorizable
-                && packet_traits<Scalar>::Vectorizable,
-    LhsPacketSize = Vectorizable ? packet_traits<LhsScalar>::size : 1,
-    RhsPacketSize = Vectorizable ? packet_traits<RhsScalar>::size : 1,
-    ResPacketSize = Vectorizable ? packet_traits<ResScalar>::size : 1,
+    Vectorizable = unpacket_traits<_RealPacket>::vectorizable
+                && unpacket_traits<_ScalarPacket>::vectorizable,
+    LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
+    RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
+    ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1,
     
     NumberOfRegisters = EIGEN_ARCH_DEFAULT_NUMBER_OF_REGISTERS,
     // FIXME: should depend on NumberOfRegisters
@@ -770,14 +958,11 @@
     RhsProgress = 1
   };
 
-  typedef typename packet_traits<LhsScalar>::type  _LhsPacket;
-  typedef typename packet_traits<RhsScalar>::type  _RhsPacket;
-  typedef typename packet_traits<ResScalar>::type  _ResPacket;
-
   typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
   typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
   typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
-
+  typedef LhsPacket LhsPacket4Packing;
+  typedef QuadPacket<RhsPacket> RhsPacketx4;
   typedef ResPacket AccPacket;
 
   EIGEN_STRONG_INLINE void initAcc(AccPacket& p)
@@ -785,22 +970,25 @@
     p = pset1<ResPacket>(ResScalar(0));
   }
 
-  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacket& dest) const
+  template<typename RhsPacketType>
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketType& dest) const
   {
-    dest = pset1<RhsPacket>(*b);
+    dest = pset1<RhsPacketType>(*b);
   }
-  
-  void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1, RhsPacket& b2, RhsPacket& b3)
+
+  EIGEN_STRONG_INLINE void loadRhs(const RhsScalar* b, RhsPacketx4& dest) const
   {
-    pbroadcast4(b, b0, b1, b2, b3);
+    pbroadcast4(b, dest.B_0, dest.B1, dest.B2, dest.B3);
   }
-  
-//   EIGEN_STRONG_INLINE void broadcastRhs(const RhsScalar* b, RhsPacket& b0, RhsPacket& b1)
-//   {
-//     // FIXME not sure that's the best way to implement it!
-//     b0 = pload1<RhsPacket>(b+0);
-//     b1 = pload1<RhsPacket>(b+1);
-//   }
+
+  template<typename RhsPacketType>
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar* b, RhsPacketType& dest) const
+  {
+    loadRhs(b, dest);
+  }
+
+  EIGEN_STRONG_INLINE void updateRhs(const RhsScalar*, RhsPacketx4&) const
+  {}
 
   EIGEN_STRONG_INLINE void loadLhs(const LhsScalar* a, LhsPacket& dest) const
   {
@@ -809,21 +997,23 @@
   
   EIGEN_STRONG_INLINE void loadRhsQuad(const RhsScalar* b, RhsPacket& dest) const
   {
-    eigen_internal_assert(unpacket_traits<RhsPacket>::size<=4);
-    loadRhs(b,dest);
+    dest = ploadquad<RhsPacket>(b);
   }
 
-  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacket& dest) const
+  template<typename LhsPacketType>
+  EIGEN_STRONG_INLINE void loadLhsUnaligned(const LhsScalar* a, LhsPacketType& dest) const
   {
-    dest = ploaddup<LhsPacket>(a);
+    dest = ploaddup<LhsPacketType>(a);
   }
 
-  EIGEN_STRONG_INLINE void madd(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& tmp) const
+  template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const LaneIdType&) const
   {
     madd_impl(a, b, c, tmp, typename conditional<Vectorizable,true_type,false_type>::type());
   }
 
-  EIGEN_STRONG_INLINE void madd_impl(const LhsPacket& a, const RhsPacket& b, AccPacket& c, RhsPacket& tmp, const true_type&) const
+  template <typename LhsPacketType, typename RhsPacketType, typename AccPacketType>
+  EIGEN_STRONG_INLINE void madd_impl(const LhsPacketType& a, const RhsPacketType& b, AccPacketType& c, RhsPacketType& tmp, const true_type&) const
   {
 #ifdef EIGEN_HAS_SINGLE_INSTRUCTION_MADD
     EIGEN_UNUSED_VARIABLE(tmp);
@@ -839,16 +1029,24 @@
     c += a * b;
   }
 
-  EIGEN_STRONG_INLINE void acc(const AccPacket& c, const ResPacket& alpha, ResPacket& r) const
+  template<typename LhsPacketType, typename AccPacketType, typename LaneIdType>
+  EIGEN_STRONG_INLINE void madd(const LhsPacketType& a, const RhsPacketx4& b, AccPacketType& c, RhsPacket& tmp, const LaneIdType& lane) const
   {
+    madd(a, b.get(lane), c, tmp, lane);
+  }
+
+  template <typename ResPacketType, typename AccPacketType>
+  EIGEN_STRONG_INLINE void acc(const AccPacketType& c, const ResPacketType& alpha, ResPacketType& r) const
+  {
+    conj_helper<ResPacketType,ResPacketType,false,ConjRhs> cj;
     r = cj.pmadd(alpha,c,r);
   }
 
 protected:
-  conj_helper<ResPacket,ResPacket,false,ConjRhs> cj;
+
 };
 
-/* optimized GEneral packed Block * packed Panel product kernel
+/* optimized General packed Block * packed Panel product kernel
  *
  * Mixing type logic: C += A * B
  *  |  A  |  B  | comments
@@ -858,26 +1056,47 @@
 template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs>
 struct gebp_kernel
 {
-  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs> Traits;
+  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target> Traits;
+  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target,GEBPPacketHalf> HalfTraits;
+  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target,GEBPPacketQuarter> QuarterTraits;
+  
   typedef typename Traits::ResScalar ResScalar;
   typedef typename Traits::LhsPacket LhsPacket;
   typedef typename Traits::RhsPacket RhsPacket;
   typedef typename Traits::ResPacket ResPacket;
   typedef typename Traits::AccPacket AccPacket;
+  typedef typename Traits::RhsPacketx4 RhsPacketx4;
 
-  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs> SwappedTraits;
+  typedef typename RhsPanelHelper<RhsPacket, RhsPacketx4, 15>::type RhsPanel15;
+
+  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target> SwappedTraits;
+
   typedef typename SwappedTraits::ResScalar SResScalar;
   typedef typename SwappedTraits::LhsPacket SLhsPacket;
   typedef typename SwappedTraits::RhsPacket SRhsPacket;
   typedef typename SwappedTraits::ResPacket SResPacket;
   typedef typename SwappedTraits::AccPacket SAccPacket;
 
+  typedef typename HalfTraits::LhsPacket LhsPacketHalf;
+  typedef typename HalfTraits::RhsPacket RhsPacketHalf;
+  typedef typename HalfTraits::ResPacket ResPacketHalf;
+  typedef typename HalfTraits::AccPacket AccPacketHalf;
+
+  typedef typename QuarterTraits::LhsPacket LhsPacketQuarter;
+  typedef typename QuarterTraits::RhsPacket RhsPacketQuarter;
+  typedef typename QuarterTraits::ResPacket ResPacketQuarter;
+  typedef typename QuarterTraits::AccPacket AccPacketQuarter;
+
   typedef typename DataMapper::LinearMapper LinearMapper;
 
   enum {
     Vectorizable  = Traits::Vectorizable,
     LhsProgress   = Traits::LhsProgress,
+    LhsProgressHalf      = HalfTraits::LhsProgress,
+    LhsProgressQuarter   = QuarterTraits::LhsProgress,
     RhsProgress   = Traits::RhsProgress,
+    RhsProgressHalf      = HalfTraits::RhsProgress,
+    RhsProgressQuarter   = QuarterTraits::RhsProgress,
     ResPacketSize = Traits::ResPacketSize
   };
 
@@ -887,6 +1106,299 @@
                   Index strideA=-1, Index strideB=-1, Index offsetA=0, Index offsetB=0);
 };
 
+template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs,
+int SwappedLhsProgress = gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target>::LhsProgress>
+struct last_row_process_16_packets
+{
+  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target> Traits;
+  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target> SwappedTraits;
+
+  typedef typename Traits::ResScalar ResScalar;
+  typedef typename SwappedTraits::LhsPacket SLhsPacket;
+  typedef typename SwappedTraits::RhsPacket SRhsPacket;
+  typedef typename SwappedTraits::ResPacket SResPacket;
+  typedef typename SwappedTraits::AccPacket SAccPacket;
+
+  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, SwappedTraits &straits, const LhsScalar* blA,
+                  const RhsScalar* blB, Index depth, const Index endk, Index i, Index j2,
+                  ResScalar alpha, SAccPacket &C0)
+    {
+      EIGEN_UNUSED_VARIABLE(res);
+      EIGEN_UNUSED_VARIABLE(straits);
+      EIGEN_UNUSED_VARIABLE(blA);
+      EIGEN_UNUSED_VARIABLE(blB);
+      EIGEN_UNUSED_VARIABLE(depth);
+      EIGEN_UNUSED_VARIABLE(endk);
+      EIGEN_UNUSED_VARIABLE(i);
+      EIGEN_UNUSED_VARIABLE(j2);
+      EIGEN_UNUSED_VARIABLE(alpha);
+      EIGEN_UNUSED_VARIABLE(C0);
+    }
+};
+
+
+template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs>
+struct last_row_process_16_packets<LhsScalar, RhsScalar, Index, DataMapper,  mr,  nr, ConjugateLhs,  ConjugateRhs, 16> {
+  typedef gebp_traits<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs,Architecture::Target> Traits;
+  typedef gebp_traits<RhsScalar,LhsScalar,ConjugateRhs,ConjugateLhs,Architecture::Target> SwappedTraits;
+
+  typedef typename Traits::ResScalar ResScalar;
+  typedef typename SwappedTraits::LhsPacket SLhsPacket;
+  typedef typename SwappedTraits::RhsPacket SRhsPacket;
+  typedef typename SwappedTraits::ResPacket SResPacket;
+  typedef typename SwappedTraits::AccPacket SAccPacket;
+
+  EIGEN_STRONG_INLINE void operator()(const DataMapper& res, SwappedTraits &straits, const LhsScalar* blA,
+                  const RhsScalar* blB, Index depth, const Index endk, Index i, Index j2,
+                  ResScalar alpha, SAccPacket &C0)
+  {
+    typedef typename unpacket_traits<typename unpacket_traits<SResPacket>::half>::half SResPacketQuarter;
+    typedef typename unpacket_traits<typename unpacket_traits<SLhsPacket>::half>::half SLhsPacketQuarter;
+    typedef typename unpacket_traits<typename unpacket_traits<SRhsPacket>::half>::half SRhsPacketQuarter;
+    typedef typename unpacket_traits<typename unpacket_traits<SAccPacket>::half>::half SAccPacketQuarter;
+
+    SResPacketQuarter R = res.template gatherPacket<SResPacketQuarter>(i, j2);
+    SResPacketQuarter alphav = pset1<SResPacketQuarter>(alpha);
+
+    if (depth - endk > 0)
+      {
+	// We have to handle the last row(s) of the rhs, which
+	// correspond to a half-packet
+	SAccPacketQuarter c0 = predux_half_dowto4(predux_half_dowto4(C0));
+
+	for (Index kk = endk; kk < depth; kk++)
+	  {
+	    SLhsPacketQuarter a0;
+	    SRhsPacketQuarter b0;
+	    straits.loadLhsUnaligned(blB, a0);
+	    straits.loadRhs(blA, b0);
+	    straits.madd(a0,b0,c0,b0, fix<0>);
+	    blB += SwappedTraits::LhsProgress/4;
+	    blA += 1;
+	  }
+	straits.acc(c0, alphav, R);
+      }
+    else
+      {
+	straits.acc(predux_half_dowto4(predux_half_dowto4(C0)), alphav, R);
+      }
+    res.scatterPacket(i, j2, R);
+  }
+};
+
+template<int nr, Index LhsProgress, Index RhsProgress, typename LhsScalar, typename RhsScalar, typename ResScalar, typename AccPacket, typename LhsPacket, typename RhsPacket, typename ResPacket, typename GEBPTraits, typename LinearMapper, typename DataMapper>
+struct lhs_process_one_packet
+{
+  typedef typename GEBPTraits::RhsPacketx4 RhsPacketx4;
+
+  EIGEN_STRONG_INLINE void peeled_kc_onestep(Index K, const LhsScalar* blA, const RhsScalar* blB, GEBPTraits traits, LhsPacket *A0, RhsPacketx4 *rhs_panel, RhsPacket *T0, AccPacket *C0, AccPacket *C1, AccPacket *C2, AccPacket *C3)
+  {
+    EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1X4");
+    EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");
+    traits.loadLhs(&blA[(0+1*K)*LhsProgress], *A0);
+    traits.loadRhs(&blB[(0+4*K)*RhsProgress], *rhs_panel);
+    traits.madd(*A0, *rhs_panel, *C0, *T0, fix<0>);
+    traits.madd(*A0, *rhs_panel, *C1, *T0, fix<1>);
+    traits.madd(*A0, *rhs_panel, *C2, *T0, fix<2>);
+    traits.madd(*A0, *rhs_panel, *C3, *T0, fix<3>);
+    #if EIGEN_GNUC_AT_LEAST(6,0) && defined(EIGEN_VECTORIZE_SSE)
+    __asm__  ("" : "+x,m" (*A0));
+    #endif
+    EIGEN_ASM_COMMENT("end step of gebp micro kernel 1X4");
+  }
+
+  EIGEN_STRONG_INLINE void operator()(
+    const DataMapper& res, const LhsScalar* blockA, const RhsScalar* blockB, ResScalar alpha,
+    Index peelStart, Index peelEnd, Index strideA, Index strideB, Index offsetA, Index offsetB,
+    int prefetch_res_offset, Index peeled_kc, Index pk, Index cols, Index depth, Index packet_cols4)
+  {
+    GEBPTraits traits;
+
+    // loops on each largest micro horizontal panel of lhs
+    // (LhsProgress x depth)
+    for(Index i=peelStart; i<peelEnd; i+=LhsProgress)
+    {
+      // loops on each largest micro vertical panel of rhs (depth * nr)
+      for(Index j2=0; j2<packet_cols4; j2+=nr)
+      {
+        // We select a LhsProgress x nr micro block of res
+        // which is entirely stored into 1 x nr registers.
+
+        const LhsScalar* blA = &blockA[i*strideA+offsetA*(LhsProgress)];
+        prefetch(&blA[0]);
+
+        // gets res block as register
+        AccPacket C0, C1, C2, C3;
+        traits.initAcc(C0);
+        traits.initAcc(C1);
+        traits.initAcc(C2);
+        traits.initAcc(C3);
+        // To improve instruction pipelining, let's double the accumulation registers:
+        //  even k will accumulate in C*, while odd k will accumulate in D*.
+        // This trick is crutial to get good performance with FMA, otherwise it is 
+        // actually faster to perform separated MUL+ADD because of a naturally
+        // better instruction-level parallelism.
+        AccPacket D0, D1, D2, D3;
+        traits.initAcc(D0);
+        traits.initAcc(D1);
+        traits.initAcc(D2);
+        traits.initAcc(D3);
+
+        LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
+        LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
+        LinearMapper r2 = res.getLinearMapper(i, j2 + 2);
+        LinearMapper r3 = res.getLinearMapper(i, j2 + 3);
+
+        r0.prefetch(prefetch_res_offset);
+        r1.prefetch(prefetch_res_offset);
+        r2.prefetch(prefetch_res_offset);
+        r3.prefetch(prefetch_res_offset);
+
+        // performs "inner" products
+        const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
+        prefetch(&blB[0]);
+        LhsPacket A0, A1;
+
+        for(Index k=0; k<peeled_kc; k+=pk)
+        {
+          EIGEN_ASM_COMMENT("begin gebp micro kernel 1/half/quarterX4");
+          RhsPacketx4 rhs_panel;
+          RhsPacket T0;
+
+          internal::prefetch(blB+(48+0));
+          peeled_kc_onestep(0, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
+          peeled_kc_onestep(1, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
+          peeled_kc_onestep(2, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
+          peeled_kc_onestep(3, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
+          internal::prefetch(blB+(48+16));
+          peeled_kc_onestep(4, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
+          peeled_kc_onestep(5, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
+          peeled_kc_onestep(6, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
+          peeled_kc_onestep(7, blA, blB, traits, &A1, &rhs_panel, &T0, &D0, &D1, &D2, &D3);
+
+          blB += pk*4*RhsProgress;
+          blA += pk*LhsProgress;
+
+          EIGEN_ASM_COMMENT("end gebp micro kernel 1/half/quarterX4");
+        }
+        C0 = padd(C0,D0);
+        C1 = padd(C1,D1);
+        C2 = padd(C2,D2);
+        C3 = padd(C3,D3);
+
+        // process remaining peeled loop
+        for(Index k=peeled_kc; k<depth; k++)
+        {
+          RhsPacketx4 rhs_panel;
+          RhsPacket T0;
+          peeled_kc_onestep(0, blA, blB, traits, &A0, &rhs_panel, &T0, &C0, &C1, &C2, &C3);
+          blB += 4*RhsProgress;
+          blA += LhsProgress;
+        }
+
+        ResPacket R0, R1;
+        ResPacket alphav = pset1<ResPacket>(alpha);
+
+        R0 = r0.template loadPacket<ResPacket>(0);
+        R1 = r1.template loadPacket<ResPacket>(0);
+        traits.acc(C0, alphav, R0);
+        traits.acc(C1,  alphav, R1);
+        r0.storePacket(0, R0);
+        r1.storePacket(0, R1);
+
+        R0 = r2.template loadPacket<ResPacket>(0);
+        R1 = r3.template loadPacket<ResPacket>(0);
+        traits.acc(C2,  alphav, R0);
+        traits.acc(C3,  alphav, R1);
+        r2.storePacket(0, R0);
+        r3.storePacket(0, R1);
+      }
+
+      // Deal with remaining columns of the rhs
+      for(Index j2=packet_cols4; j2<cols; j2++)
+      {
+        // One column at a time
+        const LhsScalar* blA = &blockA[i*strideA+offsetA*(LhsProgress)];
+        prefetch(&blA[0]);
+
+        // gets res block as register
+        AccPacket C0;
+        traits.initAcc(C0);
+
+        LinearMapper r0 = res.getLinearMapper(i, j2);
+
+        // performs "inner" products
+        const RhsScalar* blB = &blockB[j2*strideB+offsetB];
+        LhsPacket A0;
+
+        for(Index k= 0; k<peeled_kc; k+=pk)
+        {
+          EIGEN_ASM_COMMENT("begin gebp micro kernel 1/half/quarterX1");
+          RhsPacket B_0;
+
+#define EIGEN_GEBGP_ONESTEP(K)                                          \
+	      do {                                                      \
+		EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1/half/quarterX1"); \
+		EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
+    /* FIXME: why unaligned???? */ \
+		traits.loadLhsUnaligned(&blA[(0+1*K)*LhsProgress], A0); \
+		traits.loadRhs(&blB[(0+K)*RhsProgress], B_0);		\
+		traits.madd(A0, B_0, C0, B_0, fix<0>);				\
+		EIGEN_ASM_COMMENT("end step of gebp micro kernel 1/half/quarterX1"); \
+	      } while(false);
+
+          EIGEN_GEBGP_ONESTEP(0);
+          EIGEN_GEBGP_ONESTEP(1);
+          EIGEN_GEBGP_ONESTEP(2);
+          EIGEN_GEBGP_ONESTEP(3);
+          EIGEN_GEBGP_ONESTEP(4);
+          EIGEN_GEBGP_ONESTEP(5);
+          EIGEN_GEBGP_ONESTEP(6);
+          EIGEN_GEBGP_ONESTEP(7);
+
+          blB += pk*RhsProgress;
+          blA += pk*LhsProgress;
+
+          EIGEN_ASM_COMMENT("end gebp micro kernel 1/half/quarterX1");
+        }
+
+        // process remaining peeled loop
+        for(Index k=peeled_kc; k<depth; k++)
+        {
+          RhsPacket B_0;
+          EIGEN_GEBGP_ONESTEP(0);
+          blB += RhsProgress;
+          blA += LhsProgress;
+        }
+#undef EIGEN_GEBGP_ONESTEP
+        ResPacket R0;
+        ResPacket alphav = pset1<ResPacket>(alpha);
+        R0 = r0.template loadPacket<ResPacket>(0);
+        traits.acc(C0, alphav, R0);
+        r0.storePacket(0, R0);
+      }
+    }
+  }
+};
+
+template<int nr, Index LhsProgress, Index RhsProgress, typename LhsScalar, typename RhsScalar, typename ResScalar, typename AccPacket, typename LhsPacket, typename RhsPacket, typename ResPacket, typename GEBPTraits, typename LinearMapper, typename DataMapper>
+struct lhs_process_fraction_of_packet : lhs_process_one_packet<nr, LhsProgress, RhsProgress, LhsScalar, RhsScalar, ResScalar, AccPacket, LhsPacket, RhsPacket, ResPacket, GEBPTraits, LinearMapper, DataMapper>
+{
+
+EIGEN_STRONG_INLINE void peeled_kc_onestep(Index K, const LhsScalar* blA, const RhsScalar* blB, GEBPTraits traits, LhsPacket *A0, RhsPacket *B_0, RhsPacket *B1, RhsPacket *B2, RhsPacket *B3, AccPacket *C0, AccPacket *C1, AccPacket *C2, AccPacket *C3)
+  {
+        EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1X4");
+        EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!");
+        traits.loadLhsUnaligned(&blA[(0+1*K)*(LhsProgress)], *A0);
+        traits.broadcastRhs(&blB[(0+4*K)*RhsProgress], *B_0, *B1, *B2, *B3);
+        traits.madd(*A0, *B_0, *C0, *B_0);
+        traits.madd(*A0, *B1,  *C1, *B1);
+        traits.madd(*A0, *B2,  *C2, *B2);
+        traits.madd(*A0, *B3,  *C3, *B3);
+        EIGEN_ASM_COMMENT("end step of gebp micro kernel 1X4");
+  }
+};
+
 template<typename LhsScalar, typename RhsScalar, typename Index, typename DataMapper, int mr, int nr, bool ConjugateLhs, bool ConjugateRhs>
 EIGEN_DONT_INLINE
 void gebp_kernel<LhsScalar,RhsScalar,Index,DataMapper,mr,nr,ConjugateLhs,ConjugateRhs>
@@ -903,10 +1415,12 @@
     Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
     const Index peeled_mc3 = mr>=3*Traits::LhsProgress ? (rows/(3*LhsProgress))*(3*LhsProgress) : 0;
     const Index peeled_mc2 = mr>=2*Traits::LhsProgress ? peeled_mc3+((rows-peeled_mc3)/(2*LhsProgress))*(2*LhsProgress) : 0;
-    const Index peeled_mc1 = mr>=1*Traits::LhsProgress ? (rows/(1*LhsProgress))*(1*LhsProgress) : 0;
+    const Index peeled_mc1 = mr>=1*Traits::LhsProgress ? peeled_mc2+((rows-peeled_mc2)/(1*LhsProgress))*(1*LhsProgress) : 0;
+    const Index peeled_mc_half = mr>=LhsProgressHalf ? peeled_mc1+((rows-peeled_mc1)/(LhsProgressHalf))*(LhsProgressHalf) : 0;
+    const Index peeled_mc_quarter = mr>=LhsProgressQuarter ? peeled_mc_half+((rows-peeled_mc_half)/(LhsProgressQuarter))*(LhsProgressQuarter) : 0;
     enum { pk = 8 }; // NOTE Such a large peeling factor is important for large matrices (~ +5% when >1000 on Haswell)
     const Index peeled_kc  = depth & ~(pk-1);
-    const Index prefetch_res_offset = 32/sizeof(ResScalar);    
+    const int prefetch_res_offset = 32/sizeof(ResScalar);    
 //     const Index depth2     = depth & ~1;
 
     //---------- Process 3 * LhsProgress rows at once ----------
@@ -964,36 +1478,48 @@
           for(Index k=0; k<peeled_kc; k+=pk)
           {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 3pX4");
-            RhsPacket B_0, T0;
+            // 15 registers are taken (12 for acc, 2 for lhs).
+            RhsPanel15 rhs_panel;
+            RhsPacket T0;
             LhsPacket A2;
-
-#define EIGEN_GEBP_ONESTEP(K) \
-            do { \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX4"); \
+            #if EIGEN_COMP_GNUC_STRICT && EIGEN_ARCH_ARM64 && defined(EIGEN_VECTORIZE_NEON) && !(EIGEN_GNUC_AT_LEAST(9,0))
+            // see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=1633
+            // without this workaround A0, A1, and A2 are loaded in the same register,
+            // which is not good for pipelining
+            #define EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND __asm__  ("" : "+w,m" (A0), "+w,m" (A1), "+w,m" (A2));
+            #else
+            #define EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND
+            #endif
+#define EIGEN_GEBP_ONESTEP(K)                                                     \
+            do {                                                                  \
+              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX4");          \
               EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              internal::prefetch(blA+(3*K+16)*LhsProgress); \
-              if (EIGEN_ARCH_ARM) { internal::prefetch(blB+(4*K+16)*RhsProgress); } /* Bug 953 */ \
-              traits.loadLhs(&blA[(0+3*K)*LhsProgress], A0);  \
-              traits.loadLhs(&blA[(1+3*K)*LhsProgress], A1);  \
-              traits.loadLhs(&blA[(2+3*K)*LhsProgress], A2);  \
-              traits.loadRhs(blB + (0+4*K)*Traits::RhsProgress, B_0); \
-              traits.madd(A0, B_0, C0, T0); \
-              traits.madd(A1, B_0, C4, T0); \
-              traits.madd(A2, B_0, C8, B_0); \
-              traits.loadRhs(blB + (1+4*K)*Traits::RhsProgress, B_0); \
-              traits.madd(A0, B_0, C1, T0); \
-              traits.madd(A1, B_0, C5, T0); \
-              traits.madd(A2, B_0, C9, B_0); \
-              traits.loadRhs(blB + (2+4*K)*Traits::RhsProgress, B_0); \
-              traits.madd(A0, B_0, C2,  T0); \
-              traits.madd(A1, B_0, C6,  T0); \
-              traits.madd(A2, B_0, C10, B_0); \
-              traits.loadRhs(blB + (3+4*K)*Traits::RhsProgress, B_0); \
-              traits.madd(A0, B_0, C3 , T0); \
-              traits.madd(A1, B_0, C7,  T0); \
-              traits.madd(A2, B_0, C11, B_0); \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX4"); \
-            } while(false)
+              internal::prefetch(blA + (3 * K + 16) * LhsProgress);               \
+              if (EIGEN_ARCH_ARM || EIGEN_ARCH_MIPS) {                            \
+                internal::prefetch(blB + (4 * K + 16) * RhsProgress);             \
+              } /* Bug 953 */                                                     \
+              traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                \
+              traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                \
+              traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                \
+              EIGEN_GEBP_3PX4_REGISTER_ALLOC_WORKAROUND \
+              traits.loadRhs(blB + (0+4*K) * Traits::RhsProgress, rhs_panel);     \
+              traits.madd(A0, rhs_panel, C0, T0, fix<0>);                         \
+              traits.madd(A1, rhs_panel, C4, T0, fix<0>);                         \
+              traits.madd(A2, rhs_panel, C8, T0, fix<0>);                         \
+              traits.updateRhs(blB + (1+4*K) * Traits::RhsProgress, rhs_panel);   \
+              traits.madd(A0, rhs_panel, C1, T0, fix<1>);                         \
+              traits.madd(A1, rhs_panel, C5, T0, fix<1>);                         \
+              traits.madd(A2, rhs_panel, C9, T0, fix<1>);                         \
+              traits.updateRhs(blB + (2+4*K) * Traits::RhsProgress, rhs_panel);   \
+              traits.madd(A0, rhs_panel, C2, T0, fix<2>);                         \
+              traits.madd(A1, rhs_panel, C6, T0, fix<2>);                         \
+              traits.madd(A2, rhs_panel, C10, T0, fix<2>);                        \
+              traits.updateRhs(blB + (3+4*K) * Traits::RhsProgress, rhs_panel);   \
+              traits.madd(A0, rhs_panel, C3, T0, fix<3>);                         \
+              traits.madd(A1, rhs_panel, C7, T0, fix<3>);                         \
+              traits.madd(A2, rhs_panel, C11, T0, fix<3>);                        \
+              EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX4");            \
+            } while (false)
 
             internal::prefetch(blB);
             EIGEN_GEBP_ONESTEP(0);
@@ -1013,7 +1539,8 @@
           // process remaining peeled loop
           for(Index k=peeled_kc; k<depth; k++)
           {
-            RhsPacket B_0, T0;
+            RhsPanel15 rhs_panel;
+            RhsPacket T0;
             LhsPacket A2;
             EIGEN_GEBP_ONESTEP(0);
             blB += 4*RhsProgress;
@@ -1025,9 +1552,9 @@
           ResPacket R0, R1, R2;
           ResPacket alphav = pset1<ResPacket>(alpha);
 
-          R0 = r0.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r0.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r0.loadPacket(2 * Traits::ResPacketSize);
+          R0 = r0.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r0.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r0.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
           traits.acc(C0, alphav, R0);
           traits.acc(C4, alphav, R1);
           traits.acc(C8, alphav, R2);
@@ -1035,9 +1562,9 @@
           r0.storePacket(1 * Traits::ResPacketSize, R1);
           r0.storePacket(2 * Traits::ResPacketSize, R2);
 
-          R0 = r1.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r1.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r1.loadPacket(2 * Traits::ResPacketSize);
+          R0 = r1.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r1.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r1.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
           traits.acc(C1, alphav, R0);
           traits.acc(C5, alphav, R1);
           traits.acc(C9, alphav, R2);
@@ -1045,9 +1572,9 @@
           r1.storePacket(1 * Traits::ResPacketSize, R1);
           r1.storePacket(2 * Traits::ResPacketSize, R2);
 
-          R0 = r2.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r2.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r2.loadPacket(2 * Traits::ResPacketSize);
+          R0 = r2.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r2.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r2.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
           traits.acc(C2, alphav, R0);
           traits.acc(C6, alphav, R1);
           traits.acc(C10, alphav, R2);
@@ -1055,9 +1582,9 @@
           r2.storePacket(1 * Traits::ResPacketSize, R1);
           r2.storePacket(2 * Traits::ResPacketSize, R2);
 
-          R0 = r3.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r3.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r3.loadPacket(2 * Traits::ResPacketSize);
+          R0 = r3.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r3.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r3.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
           traits.acc(C3, alphav, R0);
           traits.acc(C7, alphav, R1);
           traits.acc(C11, alphav, R2);
@@ -1093,20 +1620,20 @@
           {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 3pX1");
             RhsPacket B_0;
-#define EIGEN_GEBGP_ONESTEP(K) \
-            do { \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX1"); \
+#define EIGEN_GEBGP_ONESTEP(K)                                                    \
+            do {                                                                  \
+              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 3pX1");          \
               EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              traits.loadLhs(&blA[(0+3*K)*LhsProgress], A0);  \
-              traits.loadLhs(&blA[(1+3*K)*LhsProgress], A1);  \
-              traits.loadLhs(&blA[(2+3*K)*LhsProgress], A2);  \
-              traits.loadRhs(&blB[(0+K)*RhsProgress], B_0);   \
-              traits.madd(A0, B_0, C0, B_0); \
-              traits.madd(A1, B_0, C4, B_0); \
-              traits.madd(A2, B_0, C8, B_0); \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX1"); \
-            } while(false)
-        
+              traits.loadLhs(&blA[(0 + 3 * K) * LhsProgress], A0);                \
+              traits.loadLhs(&blA[(1 + 3 * K) * LhsProgress], A1);                \
+              traits.loadLhs(&blA[(2 + 3 * K) * LhsProgress], A2);                \
+              traits.loadRhs(&blB[(0 + K) * RhsProgress], B_0);                   \
+              traits.madd(A0, B_0, C0, B_0, fix<0>);                              \
+              traits.madd(A1, B_0, C4, B_0, fix<0>);                              \
+              traits.madd(A2, B_0, C8, B_0, fix<0>);                              \
+              EIGEN_ASM_COMMENT("end step of gebp micro kernel 3pX1");            \
+            } while (false)
+
             EIGEN_GEBGP_ONESTEP(0);
             EIGEN_GEBGP_ONESTEP(1);
             EIGEN_GEBGP_ONESTEP(2);
@@ -1116,8 +1643,8 @@
             EIGEN_GEBGP_ONESTEP(6);
             EIGEN_GEBGP_ONESTEP(7);
 
-            blB += pk*RhsProgress;
-            blA += pk*3*Traits::LhsProgress;
+            blB += int(pk) * int(RhsProgress);
+            blA += int(pk) * 3 * int(Traits::LhsProgress);
 
             EIGEN_ASM_COMMENT("end gebp micro kernel 3pX1");
           }
@@ -1134,9 +1661,9 @@
           ResPacket R0, R1, R2;
           ResPacket alphav = pset1<ResPacket>(alpha);
 
-          R0 = r0.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r0.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r0.loadPacket(2 * Traits::ResPacketSize);
+          R0 = r0.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r0.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r0.template loadPacket<ResPacket>(2 * Traits::ResPacketSize);
           traits.acc(C0, alphav, R0);
           traits.acc(C4, alphav, R1);
           traits.acc(C8, alphav, R2);
@@ -1195,7 +1722,8 @@
           for(Index k=0; k<peeled_kc; k+=pk)
           {
             EIGEN_ASM_COMMENT("begin gebp micro kernel 2pX4");
-            RhsPacket B_0, B1, B2, B3, T0;
+            RhsPacketx4 rhs_panel;
+            RhsPacket T0;
 
           // NOTE: the begin/end asm comments below work around bug 935!
           // but they are not enough for gcc>=6 without FMA (bug 1637)
@@ -1204,24 +1732,24 @@
           #else
             #define EIGEN_GEBP_2PX4_SPILLING_WORKAROUND
           #endif
-          #define EIGEN_GEBGP_ONESTEP(K) \
-            do {                                                                \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX4");        \
-              traits.loadLhs(&blA[(0+2*K)*LhsProgress], A0);                    \
-              traits.loadLhs(&blA[(1+2*K)*LhsProgress], A1);                    \
-              traits.broadcastRhs(&blB[(0+4*K)*RhsProgress], B_0, B1, B2, B3);  \
-              traits.madd(A0, B_0, C0, T0);                                     \
-              traits.madd(A1, B_0, C4, B_0);                                    \
-              traits.madd(A0, B1,  C1, T0);                                     \
-              traits.madd(A1, B1,  C5, B1);                                     \
-              traits.madd(A0, B2,  C2, T0);                                     \
-              traits.madd(A1, B2,  C6, B2);                                     \
-              traits.madd(A0, B3,  C3, T0);                                     \
-              traits.madd(A1, B3,  C7, B3);                                     \
-              EIGEN_GEBP_2PX4_SPILLING_WORKAROUND                               \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX4");          \
-            } while(false)
-            
+#define EIGEN_GEBGP_ONESTEP(K)                                            \
+            do {                                                          \
+              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 2pX4");  \
+              traits.loadLhs(&blA[(0 + 2 * K) * LhsProgress], A0);        \
+              traits.loadLhs(&blA[(1 + 2 * K) * LhsProgress], A1);        \
+              traits.loadRhs(&blB[(0 + 4 * K) * RhsProgress], rhs_panel); \
+              traits.madd(A0, rhs_panel, C0, T0, fix<0>);                 \
+              traits.madd(A1, rhs_panel, C4, T0, fix<0>);                 \
+              traits.madd(A0, rhs_panel, C1, T0, fix<1>);                 \
+              traits.madd(A1, rhs_panel, C5, T0, fix<1>);                 \
+              traits.madd(A0, rhs_panel, C2, T0, fix<2>);                 \
+              traits.madd(A1, rhs_panel, C6, T0, fix<2>);                 \
+              traits.madd(A0, rhs_panel, C3, T0, fix<3>);                 \
+              traits.madd(A1, rhs_panel, C7, T0, fix<3>);                 \
+              EIGEN_GEBP_2PX4_SPILLING_WORKAROUND                         \
+              EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX4");    \
+            } while (false)
+
             internal::prefetch(blB+(48+0));
             EIGEN_GEBGP_ONESTEP(0);
             EIGEN_GEBGP_ONESTEP(1);
@@ -1241,7 +1769,8 @@
           // process remaining peeled loop
           for(Index k=peeled_kc; k<depth; k++)
           {
-            RhsPacket B_0, B1, B2, B3, T0;
+            RhsPacketx4 rhs_panel;
+            RhsPacket T0;
             EIGEN_GEBGP_ONESTEP(0);
             blB += 4*RhsProgress;
             blA += 2*Traits::LhsProgress;
@@ -1251,10 +1780,10 @@
           ResPacket R0, R1, R2, R3;
           ResPacket alphav = pset1<ResPacket>(alpha);
 
-          R0 = r0.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r0.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r1.loadPacket(0 * Traits::ResPacketSize);
-          R3 = r1.loadPacket(1 * Traits::ResPacketSize);
+          R0 = r0.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r0.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r1.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R3 = r1.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
           traits.acc(C0, alphav, R0);
           traits.acc(C4, alphav, R1);
           traits.acc(C1, alphav, R2);
@@ -1264,10 +1793,10 @@
           r1.storePacket(0 * Traits::ResPacketSize, R2);
           r1.storePacket(1 * Traits::ResPacketSize, R3);
 
-          R0 = r2.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r2.loadPacket(1 * Traits::ResPacketSize);
-          R2 = r3.loadPacket(0 * Traits::ResPacketSize);
-          R3 = r3.loadPacket(1 * Traits::ResPacketSize);
+          R0 = r2.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r2.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
+          R2 = r3.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R3 = r3.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
           traits.acc(C2,  alphav, R0);
           traits.acc(C6,  alphav, R1);
           traits.acc(C3,  alphav, R2);
@@ -1312,8 +1841,8 @@
               traits.loadLhs(&blA[(0+2*K)*LhsProgress], A0);                      \
               traits.loadLhs(&blA[(1+2*K)*LhsProgress], A1);                      \
               traits.loadRhs(&blB[(0+K)*RhsProgress], B_0);                       \
-              traits.madd(A0, B_0, C0, B1);                                       \
-              traits.madd(A1, B_0, C4, B_0);                                      \
+              traits.madd(A0, B_0, C0, B1, fix<0>);                               \
+              traits.madd(A1, B_0, C4, B_0, fix<0>);                              \
               EIGEN_ASM_COMMENT("end step of gebp micro kernel 2pX1");            \
             } while(false)
         
@@ -1326,8 +1855,8 @@
             EIGEN_GEBGP_ONESTEP(6);
             EIGEN_GEBGP_ONESTEP(7);
 
-            blB += pk*RhsProgress;
-            blA += pk*2*Traits::LhsProgress;
+            blB += int(pk) * int(RhsProgress);
+            blA += int(pk) * 2 * int(Traits::LhsProgress);
 
             EIGEN_ASM_COMMENT("end gebp micro kernel 2pX1");
           }
@@ -1344,8 +1873,8 @@
           ResPacket R0, R1;
           ResPacket alphav = pset1<ResPacket>(alpha);
 
-          R0 = r0.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r0.loadPacket(1 * Traits::ResPacketSize);
+          R0 = r0.template loadPacket<ResPacket>(0 * Traits::ResPacketSize);
+          R1 = r0.template loadPacket<ResPacket>(1 * Traits::ResPacketSize);
           traits.acc(C0, alphav, R0);
           traits.acc(C4, alphav, R1);
           r0.storePacket(0 * Traits::ResPacketSize, R0);
@@ -1357,186 +1886,43 @@
     //---------- Process 1 * LhsProgress rows at once ----------
     if(mr>=1*Traits::LhsProgress)
     {
-      // loops on each largest micro horizontal panel of lhs (1*LhsProgress x depth)
-      for(Index i=peeled_mc2; i<peeled_mc1; i+=1*LhsProgress)
-      {
-        // loops on each largest micro vertical panel of rhs (depth * nr)
-        for(Index j2=0; j2<packet_cols4; j2+=nr)
-        {
-          // We select a 1*Traits::LhsProgress x nr micro block of res which is entirely
-          // stored into 1 x nr registers.
-          
-          const LhsScalar* blA = &blockA[i*strideA+offsetA*(1*Traits::LhsProgress)];
-          prefetch(&blA[0]);
-
-          // gets res block as register
-          AccPacket C0, C1, C2, C3;
-          traits.initAcc(C0);
-          traits.initAcc(C1);
-          traits.initAcc(C2);
-          traits.initAcc(C3);
-
-          LinearMapper r0 = res.getLinearMapper(i, j2 + 0);
-          LinearMapper r1 = res.getLinearMapper(i, j2 + 1);
-          LinearMapper r2 = res.getLinearMapper(i, j2 + 2);
-          LinearMapper r3 = res.getLinearMapper(i, j2 + 3);
-
-          r0.prefetch(prefetch_res_offset);
-          r1.prefetch(prefetch_res_offset);
-          r2.prefetch(prefetch_res_offset);
-          r3.prefetch(prefetch_res_offset);
-
-          // performs "inner" products
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
-          prefetch(&blB[0]);
-          LhsPacket A0;
-
-          for(Index k=0; k<peeled_kc; k+=pk)
-          {
-            EIGEN_ASM_COMMENT("begin gebp micro kernel 1pX4");
-            RhsPacket B_0, B1, B2, B3;
-               
-#define EIGEN_GEBGP_ONESTEP(K) \
-            do {                                                                \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1pX4");        \
-              EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              traits.loadLhs(&blA[(0+1*K)*LhsProgress], A0);                    \
-              traits.broadcastRhs(&blB[(0+4*K)*RhsProgress], B_0, B1, B2, B3);  \
-              traits.madd(A0, B_0, C0, B_0);                                    \
-              traits.madd(A0, B1,  C1, B1);                                     \
-              traits.madd(A0, B2,  C2, B2);                                     \
-              traits.madd(A0, B3,  C3, B3);                                     \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 1pX4");          \
-            } while(false)
-            
-            internal::prefetch(blB+(48+0));
-            EIGEN_GEBGP_ONESTEP(0);
-            EIGEN_GEBGP_ONESTEP(1);
-            EIGEN_GEBGP_ONESTEP(2);
-            EIGEN_GEBGP_ONESTEP(3);
-            internal::prefetch(blB+(48+16));
-            EIGEN_GEBGP_ONESTEP(4);
-            EIGEN_GEBGP_ONESTEP(5);
-            EIGEN_GEBGP_ONESTEP(6);
-            EIGEN_GEBGP_ONESTEP(7);
-
-            blB += pk*4*RhsProgress;
-            blA += pk*1*LhsProgress;
-
-            EIGEN_ASM_COMMENT("end gebp micro kernel 1pX4");
-          }
-          // process remaining peeled loop
-          for(Index k=peeled_kc; k<depth; k++)
-          {
-            RhsPacket B_0, B1, B2, B3;
-            EIGEN_GEBGP_ONESTEP(0);
-            blB += 4*RhsProgress;
-            blA += 1*LhsProgress;
-          }
-#undef EIGEN_GEBGP_ONESTEP
-
-          ResPacket R0, R1;
-          ResPacket alphav = pset1<ResPacket>(alpha);
-
-          R0 = r0.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r1.loadPacket(0 * Traits::ResPacketSize);
-          traits.acc(C0, alphav, R0);
-          traits.acc(C1,  alphav, R1);
-          r0.storePacket(0 * Traits::ResPacketSize, R0);
-          r1.storePacket(0 * Traits::ResPacketSize, R1);
-
-          R0 = r2.loadPacket(0 * Traits::ResPacketSize);
-          R1 = r3.loadPacket(0 * Traits::ResPacketSize);
-          traits.acc(C2,  alphav, R0);
-          traits.acc(C3,  alphav, R1);
-          r2.storePacket(0 * Traits::ResPacketSize, R0);
-          r3.storePacket(0 * Traits::ResPacketSize, R1);
-        }
-
-        // Deal with remaining columns of the rhs
-        for(Index j2=packet_cols4; j2<cols; j2++)
-        {
-          // One column at a time
-          const LhsScalar* blA = &blockA[i*strideA+offsetA*(1*Traits::LhsProgress)];
-          prefetch(&blA[0]);
-
-          // gets res block as register
-          AccPacket C0;
-          traits.initAcc(C0);
-
-          LinearMapper r0 = res.getLinearMapper(i, j2);
-
-          // performs "inner" products
-          const RhsScalar* blB = &blockB[j2*strideB+offsetB];
-          LhsPacket A0;
-
-          for(Index k=0; k<peeled_kc; k+=pk)
-          {
-            EIGEN_ASM_COMMENT("begin gebp micro kernel 1pX1");
-            RhsPacket B_0;
-        
-#define EIGEN_GEBGP_ONESTEP(K) \
-            do {                                                                \
-              EIGEN_ASM_COMMENT("begin step of gebp micro kernel 1pX1");        \
-              EIGEN_ASM_COMMENT("Note: these asm comments work around bug 935!"); \
-              traits.loadLhs(&blA[(0+1*K)*LhsProgress], A0);                    \
-              traits.loadRhs(&blB[(0+K)*RhsProgress], B_0);                     \
-              traits.madd(A0, B_0, C0, B_0);                                    \
-              EIGEN_ASM_COMMENT("end step of gebp micro kernel 1pX1");          \
-            } while(false);
-
-            EIGEN_GEBGP_ONESTEP(0);
-            EIGEN_GEBGP_ONESTEP(1);
-            EIGEN_GEBGP_ONESTEP(2);
-            EIGEN_GEBGP_ONESTEP(3);
-            EIGEN_GEBGP_ONESTEP(4);
-            EIGEN_GEBGP_ONESTEP(5);
-            EIGEN_GEBGP_ONESTEP(6);
-            EIGEN_GEBGP_ONESTEP(7);
-
-            blB += pk*RhsProgress;
-            blA += pk*1*Traits::LhsProgress;
-
-            EIGEN_ASM_COMMENT("end gebp micro kernel 1pX1");
-          }
-
-          // process remaining peeled loop
-          for(Index k=peeled_kc; k<depth; k++)
-          {
-            RhsPacket B_0;
-            EIGEN_GEBGP_ONESTEP(0);
-            blB += RhsProgress;
-            blA += 1*Traits::LhsProgress;
-          }
-#undef EIGEN_GEBGP_ONESTEP
-          ResPacket R0;
-          ResPacket alphav = pset1<ResPacket>(alpha);
-          R0 = r0.loadPacket(0 * Traits::ResPacketSize);
-          traits.acc(C0, alphav, R0);
-          r0.storePacket(0 * Traits::ResPacketSize, R0);
-        }
-      }
+      lhs_process_one_packet<nr, LhsProgress, RhsProgress, LhsScalar, RhsScalar, ResScalar, AccPacket, LhsPacket, RhsPacket, ResPacket, Traits, LinearMapper, DataMapper> p;
+      p(res, blockA, blockB, alpha, peeled_mc2, peeled_mc1, strideA, strideB, offsetA, offsetB, prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
+    }
+    //---------- Process LhsProgressHalf rows at once ----------
+    if((LhsProgressHalf < LhsProgress) && mr>=LhsProgressHalf)
+    {
+      lhs_process_fraction_of_packet<nr, LhsProgressHalf, RhsProgressHalf, LhsScalar, RhsScalar, ResScalar, AccPacketHalf, LhsPacketHalf, RhsPacketHalf, ResPacketHalf, HalfTraits, LinearMapper, DataMapper> p;
+      p(res, blockA, blockB, alpha, peeled_mc1, peeled_mc_half, strideA, strideB, offsetA, offsetB, prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
+    }
+    //---------- Process LhsProgressQuarter rows at once ----------
+    if((LhsProgressQuarter < LhsProgressHalf) && mr>=LhsProgressQuarter)
+    {
+      lhs_process_fraction_of_packet<nr, LhsProgressQuarter, RhsProgressQuarter, LhsScalar, RhsScalar, ResScalar, AccPacketQuarter, LhsPacketQuarter, RhsPacketQuarter, ResPacketQuarter, QuarterTraits, LinearMapper, DataMapper> p;
+      p(res, blockA, blockB, alpha, peeled_mc_half, peeled_mc_quarter, strideA, strideB, offsetA, offsetB, prefetch_res_offset, peeled_kc, pk, cols, depth, packet_cols4);
     }
     //---------- Process remaining rows, 1 at once ----------
-    if(peeled_mc1<rows)
+    if(peeled_mc_quarter<rows)
     {
       // loop on each panel of the rhs
       for(Index j2=0; j2<packet_cols4; j2+=nr)
       {
         // loop on each row of the lhs (1*LhsProgress x depth)
-        for(Index i=peeled_mc1; i<rows; i+=1)
+        for(Index i=peeled_mc_quarter; i<rows; i+=1)
         {
           const LhsScalar* blA = &blockA[i*strideA+offsetA];
           prefetch(&blA[0]);
           const RhsScalar* blB = &blockB[j2*strideB+offsetB*nr];
 
-          // The following piece of code wont work for 512 bit registers
-          // Moreover, if LhsProgress==8 it assumes that there is a half packet of the same size
-          // as nr (which is currently 4) for the return type.
+          // If LhsProgress is 8 or 16, it assumes that there is a
+          // half or quarter packet, respectively, of the same size as
+          // nr (which is currently 4) for the return type.
           const int SResPacketHalfSize = unpacket_traits<typename unpacket_traits<SResPacket>::half>::size;
+          const int SResPacketQuarterSize = unpacket_traits<typename unpacket_traits<typename unpacket_traits<SResPacket>::half>::half>::size;
           if ((SwappedTraits::LhsProgress % 4) == 0 &&
-              (SwappedTraits::LhsProgress <= 8) &&
-              (SwappedTraits::LhsProgress!=8 || SResPacketHalfSize==nr))
+              (SwappedTraits::LhsProgress<=16) &&
+              (SwappedTraits::LhsProgress!=8  || SResPacketHalfSize==nr) &&
+              (SwappedTraits::LhsProgress!=16 || SResPacketQuarterSize==nr))
           {
             SAccPacket C0, C1, C2, C3;
             straits.initAcc(C0);
@@ -1559,15 +1945,15 @@
 
               straits.loadRhsQuad(blA+0*spk, B_0);
               straits.loadRhsQuad(blA+1*spk, B_1);
-              straits.madd(A0,B_0,C0,B_0);
-              straits.madd(A1,B_1,C1,B_1);
+              straits.madd(A0,B_0,C0,B_0, fix<0>);
+              straits.madd(A1,B_1,C1,B_1, fix<0>);
 
               straits.loadLhsUnaligned(blB+2*SwappedTraits::LhsProgress, A0);
               straits.loadLhsUnaligned(blB+3*SwappedTraits::LhsProgress, A1);
               straits.loadRhsQuad(blA+2*spk, B_0);
               straits.loadRhsQuad(blA+3*spk, B_1);
-              straits.madd(A0,B_0,C2,B_0);
-              straits.madd(A1,B_1,C3,B_1);
+              straits.madd(A0,B_0,C2,B_0, fix<0>);
+              straits.madd(A1,B_1,C3,B_1, fix<0>);
 
               blB += 4*SwappedTraits::LhsProgress;
               blA += 4*spk;
@@ -1580,7 +1966,7 @@
 
               straits.loadLhsUnaligned(blB, A0);
               straits.loadRhsQuad(blA, B_0);
-              straits.madd(A0,B_0,C0,B_0);
+              straits.madd(A0,B_0,C0,B_0, fix<0>);
 
               blB += SwappedTraits::LhsProgress;
               blA += spk;
@@ -1590,7 +1976,7 @@
               // Special case where we have to first reduce the accumulation register C0
               typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SResPacket>::half,SResPacket>::type SResPacketHalf;
               typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SLhsPacket>::half,SLhsPacket>::type SLhsPacketHalf;
-              typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SLhsPacket>::half,SRhsPacket>::type SRhsPacketHalf;
+              typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SRhsPacket>::half,SRhsPacket>::type SRhsPacketHalf;
               typedef typename conditional<SwappedTraits::LhsProgress>=8,typename unpacket_traits<SAccPacket>::half,SAccPacket>::type SAccPacketHalf;
 
               SResPacketHalf R = res.template gatherPacket<SResPacketHalf>(i, j2);
@@ -1603,16 +1989,25 @@
                 SRhsPacketHalf b0;
                 straits.loadLhsUnaligned(blB, a0);
                 straits.loadRhs(blA, b0);
-                SAccPacketHalf c0 = predux_downto4(C0);
-                straits.madd(a0,b0,c0,b0);
+                SAccPacketHalf c0 = predux_half_dowto4(C0);
+                straits.madd(a0,b0,c0,b0, fix<0>);
                 straits.acc(c0, alphav, R);
               }
               else
               {
-                straits.acc(predux_downto4(C0), alphav, R);
+                straits.acc(predux_half_dowto4(C0), alphav, R);
               }
               res.scatterPacket(i, j2, R);
             }
+            else if (SwappedTraits::LhsProgress==16)
+            {
+              // Special case where we have to first reduce the
+              // accumulation register C0. We specialize the block in
+              // template form, so that LhsProgress < 16 paths don't
+              // fail to compile
+              last_row_process_16_packets<LhsScalar, RhsScalar, Index, DataMapper, mr, nr, ConjugateLhs, ConjugateRhs> p;
+	            p(res, straits, blA, blB, depth, endk, i, j2,alpha, C0);
+            }
             else
             {
               SResPacket R = res.template gatherPacket<SResPacket>(i, j2);
@@ -1635,14 +2030,14 @@
 
               B_0 = blB[0];
               B_1 = blB[1];
-              CJMADD(cj,A0,B_0,C0,  B_0);
-              CJMADD(cj,A0,B_1,C1,  B_1);
-              
+              C0 = cj.pmadd(A0,B_0,C0);
+              C1 = cj.pmadd(A0,B_1,C1);
+
               B_0 = blB[2];
               B_1 = blB[3];
-              CJMADD(cj,A0,B_0,C2,  B_0);
-              CJMADD(cj,A0,B_1,C3,  B_1);
-              
+              C2 = cj.pmadd(A0,B_0,C2);
+              C3 = cj.pmadd(A0,B_1,C3);
+
               blB += 4;
             }
             res(i, j2 + 0) += alpha * C0;
@@ -1656,7 +2051,7 @@
       for(Index j2=packet_cols4; j2<cols; j2++)
       {
         // loop on each row of the lhs (1*LhsProgress x depth)
-        for(Index i=peeled_mc1; i<rows; i+=1)
+        for(Index i=peeled_mc_quarter; i<rows; i+=1)
         {
           const LhsScalar* blA = &blockA[i*strideA+offsetA];
           prefetch(&blA[0]);
@@ -1667,7 +2062,7 @@
           {
             LhsScalar A0 = blA[k];
             RhsScalar B_0 = blB[k];
-            CJMADD(cj, A0, B_0, C0, B_0);
+            C0 = cj.pmadd(A0, B_0, C0);
           }
           res(i, j2) += alpha * C0;
         }
@@ -1676,8 +2071,6 @@
   }
 
 
-#undef CJMADD
-
 // pack a block of the lhs
 // The traversal is as follow (mr==4):
 //   0  4  8 12 ...
@@ -1692,19 +2085,24 @@
 //
 //  32 33 34 35 ...
 //  36 36 38 39 ...
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, bool Conjugate, bool PanelMode>
-struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, ColMajor, Conjugate, PanelMode>
+template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
+struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, ColMajor, Conjugate, PanelMode>
 {
   typedef typename DataMapper::LinearMapper LinearMapper;
   EIGEN_DONT_INLINE void operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride=0, Index offset=0);
 };
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, bool Conjugate, bool PanelMode>
-EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, ColMajor, Conjugate, PanelMode>
+template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
+EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, ColMajor, Conjugate, PanelMode>
   ::operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride, Index offset)
 {
-  typedef typename packet_traits<Scalar>::type Packet;
-  enum { PacketSize = packet_traits<Scalar>::size };
+  typedef typename unpacket_traits<Packet>::half HalfPacket;
+  typedef typename unpacket_traits<typename unpacket_traits<Packet>::half>::half QuarterPacket;
+  enum { PacketSize = unpacket_traits<Packet>::size,
+         HalfPacketSize = unpacket_traits<HalfPacket>::size,
+         QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
+         HasHalf = (int)HalfPacketSize < (int)PacketSize,
+         HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize};
 
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS");
   EIGEN_UNUSED_VARIABLE(stride);
@@ -1716,9 +2114,12 @@
 
   const Index peeled_mc3 = Pack1>=3*PacketSize ? (rows/(3*PacketSize))*(3*PacketSize) : 0;
   const Index peeled_mc2 = Pack1>=2*PacketSize ? peeled_mc3+((rows-peeled_mc3)/(2*PacketSize))*(2*PacketSize) : 0;
-  const Index peeled_mc1 = Pack1>=1*PacketSize ? (rows/(1*PacketSize))*(1*PacketSize) : 0;
-  const Index peeled_mc0 = Pack2>=1*PacketSize ? peeled_mc1
-                         : Pack2>1             ? (rows/Pack2)*Pack2 : 0;
+  const Index peeled_mc1 = Pack1>=1*PacketSize ? peeled_mc2+((rows-peeled_mc2)/(1*PacketSize))*(1*PacketSize) : 0;
+  const Index peeled_mc_half = Pack1>=HalfPacketSize ? peeled_mc1+((rows-peeled_mc1)/(HalfPacketSize))*(HalfPacketSize) : 0;
+  const Index peeled_mc_quarter = Pack1>=QuarterPacketSize ? (rows/(QuarterPacketSize))*(QuarterPacketSize) : 0;
+  const Index last_lhs_progress = rows > peeled_mc_quarter ? (rows - peeled_mc_quarter) & ~1 : 0;
+  const Index peeled_mc0 = Pack2>=PacketSize ? peeled_mc_quarter
+                         : Pack2>1 && last_lhs_progress ? (rows/last_lhs_progress)*last_lhs_progress : 0;
 
   Index i=0;
 
@@ -1732,9 +2133,9 @@
       for(Index k=0; k<depth; k++)
       {
         Packet A, B, C;
-        A = lhs.loadPacket(i+0*PacketSize, k);
-        B = lhs.loadPacket(i+1*PacketSize, k);
-        C = lhs.loadPacket(i+2*PacketSize, k);
+        A = lhs.template loadPacket<Packet>(i+0*PacketSize, k);
+        B = lhs.template loadPacket<Packet>(i+1*PacketSize, k);
+        C = lhs.template loadPacket<Packet>(i+2*PacketSize, k);
         pstore(blockA+count, cj.pconj(A)); count+=PacketSize;
         pstore(blockA+count, cj.pconj(B)); count+=PacketSize;
         pstore(blockA+count, cj.pconj(C)); count+=PacketSize;
@@ -1752,8 +2153,8 @@
       for(Index k=0; k<depth; k++)
       {
         Packet A, B;
-        A = lhs.loadPacket(i+0*PacketSize, k);
-        B = lhs.loadPacket(i+1*PacketSize, k);
+        A = lhs.template loadPacket<Packet>(i+0*PacketSize, k);
+        B = lhs.template loadPacket<Packet>(i+1*PacketSize, k);
         pstore(blockA+count, cj.pconj(A)); count+=PacketSize;
         pstore(blockA+count, cj.pconj(B)); count+=PacketSize;
       }
@@ -1770,27 +2171,67 @@
       for(Index k=0; k<depth; k++)
       {
         Packet A;
-        A = lhs.loadPacket(i+0*PacketSize, k);
+        A = lhs.template loadPacket<Packet>(i+0*PacketSize, k);
         pstore(blockA+count, cj.pconj(A));
         count+=PacketSize;
       }
       if(PanelMode) count += (1*PacketSize) * (stride-offset-depth);
     }
   }
-  // Pack scalars
-  if(Pack2<PacketSize && Pack2>1)
+  // Pack half packets
+  if(HasHalf && Pack1>=HalfPacketSize)
   {
-    for(; i<peeled_mc0; i+=Pack2)
+    for(; i<peeled_mc_half; i+=HalfPacketSize)
     {
-      if(PanelMode) count += Pack2 * offset;
+      if(PanelMode) count += (HalfPacketSize) * offset;
 
       for(Index k=0; k<depth; k++)
-        for(Index w=0; w<Pack2; w++)
-          blockA[count++] = cj(lhs(i+w, k));
-
-      if(PanelMode) count += Pack2 * (stride-offset-depth);
+      {
+        HalfPacket A;
+        A = lhs.template loadPacket<HalfPacket>(i+0*(HalfPacketSize), k);
+        pstoreu(blockA+count, cj.pconj(A));
+        count+=HalfPacketSize;
+      }
+      if(PanelMode) count += (HalfPacketSize) * (stride-offset-depth);
     }
   }
+  // Pack quarter packets
+  if(HasQuarter && Pack1>=QuarterPacketSize)
+  {
+    for(; i<peeled_mc_quarter; i+=QuarterPacketSize)
+    {
+      if(PanelMode) count += (QuarterPacketSize) * offset;
+
+      for(Index k=0; k<depth; k++)
+      {
+        QuarterPacket A;
+        A = lhs.template loadPacket<QuarterPacket>(i+0*(QuarterPacketSize), k);
+        pstoreu(blockA+count, cj.pconj(A));
+        count+=QuarterPacketSize;
+      }
+      if(PanelMode) count += (QuarterPacketSize) * (stride-offset-depth);
+    }
+  }
+  // Pack2 may be *smaller* than PacketSize—that happens for
+  // products like real * complex, where we have to go half the
+  // progress on the lhs in order to duplicate those operands to
+  // address both real & imaginary parts on the rhs. This portion will
+  // pack those half ones until they match the number expected on the
+  // last peeling loop at this point (for the rhs).
+  if(Pack2<PacketSize && Pack2>1)
+  {
+    for(; i<peeled_mc0; i+=last_lhs_progress)
+    {
+      if(PanelMode) count += last_lhs_progress * offset;
+
+      for(Index k=0; k<depth; k++)
+        for(Index w=0; w<last_lhs_progress; w++)
+          blockA[count++] = cj(lhs(i+w, k));
+
+      if(PanelMode) count += last_lhs_progress * (stride-offset-depth);
+    }
+  }
+  // Pack scalars
   for(; i<rows; i++)
   {
     if(PanelMode) count += offset;
@@ -1800,19 +2241,24 @@
   }
 }
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, bool Conjugate, bool PanelMode>
-struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, RowMajor, Conjugate, PanelMode>
+template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
+struct gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, RowMajor, Conjugate, PanelMode>
 {
   typedef typename DataMapper::LinearMapper LinearMapper;
   EIGEN_DONT_INLINE void operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride=0, Index offset=0);
 };
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, bool Conjugate, bool PanelMode>
-EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, RowMajor, Conjugate, PanelMode>
+template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, bool Conjugate, bool PanelMode>
+EIGEN_DONT_INLINE void gemm_pack_lhs<Scalar, Index, DataMapper, Pack1, Pack2, Packet, RowMajor, Conjugate, PanelMode>
   ::operator()(Scalar* blockA, const DataMapper& lhs, Index depth, Index rows, Index stride, Index offset)
 {
-  typedef typename packet_traits<Scalar>::type Packet;
-  enum { PacketSize = packet_traits<Scalar>::size };
+  typedef typename unpacket_traits<Packet>::half HalfPacket;
+  typedef typename unpacket_traits<typename unpacket_traits<Packet>::half>::half QuarterPacket;
+  enum { PacketSize = unpacket_traits<Packet>::size,
+         HalfPacketSize = unpacket_traits<HalfPacket>::size,
+         QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
+         HasHalf = (int)HalfPacketSize < (int)PacketSize,
+         HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize};
 
   EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK LHS");
   EIGEN_UNUSED_VARIABLE(stride);
@@ -1820,37 +2266,51 @@
   eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
   conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
   Index count = 0;
+  bool gone_half = false, gone_quarter = false, gone_last = false;
 
-//   const Index peeled_mc3 = Pack1>=3*PacketSize ? (rows/(3*PacketSize))*(3*PacketSize) : 0;
-//   const Index peeled_mc2 = Pack1>=2*PacketSize ? peeled_mc3+((rows-peeled_mc3)/(2*PacketSize))*(2*PacketSize) : 0;
-//   const Index peeled_mc1 = Pack1>=1*PacketSize ? (rows/(1*PacketSize))*(1*PacketSize) : 0;
-
-  int pack = Pack1;
   Index i = 0;
+  int pack = Pack1;
+  int psize = PacketSize;
   while(pack>0)
   {
     Index remaining_rows = rows-i;
-    Index peeled_mc = i+(remaining_rows/pack)*pack;
+    Index peeled_mc = gone_last ? Pack2>1 ? (rows/pack)*pack : 0 : i+(remaining_rows/pack)*pack;
+    Index starting_pos = i;
     for(; i<peeled_mc; i+=pack)
     {
       if(PanelMode) count += pack * offset;
 
-      const Index peeled_k = (depth/PacketSize)*PacketSize;
       Index k=0;
-      if(pack>=PacketSize)
+      if(pack>=psize && psize >= QuarterPacketSize)
       {
-        for(; k<peeled_k; k+=PacketSize)
+        const Index peeled_k = (depth/psize)*psize;
+        for(; k<peeled_k; k+=psize)
         {
-          for (Index m = 0; m < pack; m += PacketSize)
+          for (Index m = 0; m < pack; m += psize)
           {
-            PacketBlock<Packet> kernel;
-            for (int p = 0; p < PacketSize; ++p) kernel.packet[p] = lhs.loadPacket(i+p+m, k);
-            ptranspose(kernel);
-            for (int p = 0; p < PacketSize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel.packet[p]));
+            if (psize == PacketSize) {
+              PacketBlock<Packet> kernel;
+              for (int p = 0; p < psize; ++p) kernel.packet[p] = lhs.template loadPacket<Packet>(i+p+m, k);
+              ptranspose(kernel);
+              for (int p = 0; p < psize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel.packet[p]));
+            } else if (HasHalf && psize == HalfPacketSize) {
+              gone_half = true;
+              PacketBlock<HalfPacket> kernel_half;
+              for (int p = 0; p < psize; ++p) kernel_half.packet[p] = lhs.template loadPacket<HalfPacket>(i+p+m, k);
+              ptranspose(kernel_half);
+              for (int p = 0; p < psize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel_half.packet[p]));
+            } else if (HasQuarter && psize == QuarterPacketSize) {
+              gone_quarter = true;
+              PacketBlock<QuarterPacket> kernel_quarter;
+              for (int p = 0; p < psize; ++p) kernel_quarter.packet[p] = lhs.template loadPacket<QuarterPacket>(i+p+m, k);
+              ptranspose(kernel_quarter);
+              for (int p = 0; p < psize; ++p) pstore(blockA+count+m+(pack)*p, cj.pconj(kernel_quarter.packet[p]));
+	    }
           }
-          count += PacketSize*pack;
+          count += psize*pack;
         }
       }
+
       for(; k<depth; k++)
       {
         Index w=0;
@@ -1873,9 +2333,28 @@
       if(PanelMode) count += pack * (stride-offset-depth);
     }
 
-    pack -= PacketSize;
-    if(pack<Pack2 && (pack+PacketSize)!=Pack2)
-      pack = Pack2;
+    pack -= psize;
+    Index left = rows - i;
+    if (pack <= 0) {
+      if (!gone_last &&
+          (starting_pos == i || left >= psize/2 || left >= psize/4) &&
+          ((psize/2 == HalfPacketSize && HasHalf && !gone_half) ||
+           (psize/2 == QuarterPacketSize && HasQuarter && !gone_quarter))) {
+        psize /= 2;
+        pack = psize;
+        continue;
+      }
+      // Pack2 may be *smaller* than PacketSize—that happens for
+      // products like real * complex, where we have to go half the
+      // progress on the lhs in order to duplicate those operands to
+      // address both real & imaginary parts on the rhs. This portion will
+      // pack those half ones until they match the number expected on the
+      // last peeling loop at this point (for the rhs).
+      if (Pack2 < PacketSize && !gone_last) {
+        gone_last = true;
+        psize = pack = left & ~1;
+      }
+    }
   }
 
   for(; i<rows; i++)
@@ -1931,7 +2410,7 @@
 //       const Scalar* b6 = &rhs[(j2+6)*rhsStride];
 //       const Scalar* b7 = &rhs[(j2+7)*rhsStride];
 //       Index k=0;
-//       if(PacketSize==8) // TODO enbale vectorized transposition for PacketSize==4
+//       if(PacketSize==8) // TODO enable vectorized transposition for PacketSize==4
 //       {
 //         for(; k<peeled_k; k+=PacketSize) {
 //           PacketBlock<Packet> kernel;
@@ -1978,10 +2457,10 @@
       {
         for(; k<peeled_k; k+=PacketSize) {
           PacketBlock<Packet,(PacketSize%4)==0?4:PacketSize> kernel;
-          kernel.packet[0] = dm0.loadPacket(k);
-          kernel.packet[1%PacketSize] = dm1.loadPacket(k);
-          kernel.packet[2%PacketSize] = dm2.loadPacket(k);
-          kernel.packet[3%PacketSize] = dm3.loadPacket(k);
+          kernel.packet[0           ] = dm0.template loadPacket<Packet>(k);
+          kernel.packet[1%PacketSize] = dm1.template loadPacket<Packet>(k);
+          kernel.packet[2%PacketSize] = dm2.template loadPacket<Packet>(k);
+          kernel.packet[3%PacketSize] = dm3.template loadPacket<Packet>(k);
           ptranspose(kernel);
           pstoreu(blockB+count+0*PacketSize, cj.pconj(kernel.packet[0]));
           pstoreu(blockB+count+1*PacketSize, cj.pconj(kernel.packet[1%PacketSize]));
@@ -2022,94 +2501,104 @@
 struct gemm_pack_rhs<Scalar, Index, DataMapper, nr, RowMajor, Conjugate, PanelMode>
 {
   typedef typename packet_traits<Scalar>::type Packet;
+  typedef typename unpacket_traits<Packet>::half HalfPacket;
+  typedef typename unpacket_traits<typename unpacket_traits<Packet>::half>::half QuarterPacket;
   typedef typename DataMapper::LinearMapper LinearMapper;
-  enum { PacketSize = packet_traits<Scalar>::size };
-  EIGEN_DONT_INLINE void operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride=0, Index offset=0);
-};
-
-template<typename Scalar, typename Index, typename DataMapper, int nr, bool Conjugate, bool PanelMode>
-EIGEN_DONT_INLINE void gemm_pack_rhs<Scalar, Index, DataMapper, nr, RowMajor, Conjugate, PanelMode>
-  ::operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride, Index offset)
-{
-  EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS ROWMAJOR");
-  EIGEN_UNUSED_VARIABLE(stride);
-  EIGEN_UNUSED_VARIABLE(offset);
-  eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
-  conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
-  Index packet_cols8 = nr>=8 ? (cols/8) * 8 : 0;
-  Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
-  Index count = 0;
-
-//   if(nr>=8)
-//   {
-//     for(Index j2=0; j2<packet_cols8; j2+=8)
-//     {
-//       // skip what we have before
-//       if(PanelMode) count += 8 * offset;
-//       for(Index k=0; k<depth; k++)
-//       {
-//         if (PacketSize==8) {
-//           Packet A = ploadu<Packet>(&rhs[k*rhsStride + j2]);
-//           pstoreu(blockB+count, cj.pconj(A));
-//         } else if (PacketSize==4) {
-//           Packet A = ploadu<Packet>(&rhs[k*rhsStride + j2]);
-//           Packet B = ploadu<Packet>(&rhs[k*rhsStride + j2 + PacketSize]);
-//           pstoreu(blockB+count, cj.pconj(A));
-//           pstoreu(blockB+count+PacketSize, cj.pconj(B));
-//         } else {
-//           const Scalar* b0 = &rhs[k*rhsStride + j2];
-//           blockB[count+0] = cj(b0[0]);
-//           blockB[count+1] = cj(b0[1]);
-//           blockB[count+2] = cj(b0[2]);
-//           blockB[count+3] = cj(b0[3]);
-//           blockB[count+4] = cj(b0[4]);
-//           blockB[count+5] = cj(b0[5]);
-//           blockB[count+6] = cj(b0[6]);
-//           blockB[count+7] = cj(b0[7]);
-//         }
-//         count += 8;
-//       }
-//       // skip what we have after
-//       if(PanelMode) count += 8 * (stride-offset-depth);
-//     }
-//   }
-  if(nr>=4)
+  enum { PacketSize = packet_traits<Scalar>::size,
+         HalfPacketSize = unpacket_traits<HalfPacket>::size,
+		 QuarterPacketSize = unpacket_traits<QuarterPacket>::size};
+  EIGEN_DONT_INLINE void operator()(Scalar* blockB, const DataMapper& rhs, Index depth, Index cols, Index stride=0, Index offset=0)
   {
-    for(Index j2=packet_cols8; j2<packet_cols4; j2+=4)
+    EIGEN_ASM_COMMENT("EIGEN PRODUCT PACK RHS ROWMAJOR");
+    EIGEN_UNUSED_VARIABLE(stride);
+    EIGEN_UNUSED_VARIABLE(offset);
+    eigen_assert(((!PanelMode) && stride==0 && offset==0) || (PanelMode && stride>=depth && offset<=stride));
+    const bool HasHalf = (int)HalfPacketSize < (int)PacketSize;
+    const bool HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize;
+    conj_if<NumTraits<Scalar>::IsComplex && Conjugate> cj;
+    Index packet_cols8 = nr>=8 ? (cols/8) * 8 : 0;
+    Index packet_cols4 = nr>=4 ? (cols/4) * 4 : 0;
+    Index count = 0;
+
+  //   if(nr>=8)
+  //   {
+  //     for(Index j2=0; j2<packet_cols8; j2+=8)
+  //     {
+  //       // skip what we have before
+  //       if(PanelMode) count += 8 * offset;
+  //       for(Index k=0; k<depth; k++)
+  //       {
+  //         if (PacketSize==8) {
+  //           Packet A = ploadu<Packet>(&rhs[k*rhsStride + j2]);
+  //           pstoreu(blockB+count, cj.pconj(A));
+  //         } else if (PacketSize==4) {
+  //           Packet A = ploadu<Packet>(&rhs[k*rhsStride + j2]);
+  //           Packet B = ploadu<Packet>(&rhs[k*rhsStride + j2 + PacketSize]);
+  //           pstoreu(blockB+count, cj.pconj(A));
+  //           pstoreu(blockB+count+PacketSize, cj.pconj(B));
+  //         } else {
+  //           const Scalar* b0 = &rhs[k*rhsStride + j2];
+  //           blockB[count+0] = cj(b0[0]);
+  //           blockB[count+1] = cj(b0[1]);
+  //           blockB[count+2] = cj(b0[2]);
+  //           blockB[count+3] = cj(b0[3]);
+  //           blockB[count+4] = cj(b0[4]);
+  //           blockB[count+5] = cj(b0[5]);
+  //           blockB[count+6] = cj(b0[6]);
+  //           blockB[count+7] = cj(b0[7]);
+  //         }
+  //         count += 8;
+  //       }
+  //       // skip what we have after
+  //       if(PanelMode) count += 8 * (stride-offset-depth);
+  //     }
+  //   }
+    if(nr>=4)
     {
-      // skip what we have before
-      if(PanelMode) count += 4 * offset;
+      for(Index j2=packet_cols8; j2<packet_cols4; j2+=4)
+      {
+        // skip what we have before
+        if(PanelMode) count += 4 * offset;
+        for(Index k=0; k<depth; k++)
+        {
+          if (PacketSize==4) {
+            Packet A = rhs.template loadPacket<Packet>(k, j2);
+            pstoreu(blockB+count, cj.pconj(A));
+            count += PacketSize;
+          } else if (HasHalf && HalfPacketSize==4) {
+            HalfPacket A = rhs.template loadPacket<HalfPacket>(k, j2);
+            pstoreu(blockB+count, cj.pconj(A));
+            count += HalfPacketSize;
+          } else if (HasQuarter && QuarterPacketSize==4) {
+            QuarterPacket A = rhs.template loadPacket<QuarterPacket>(k, j2);
+            pstoreu(blockB+count, cj.pconj(A));
+            count += QuarterPacketSize;
+          } else {
+            const LinearMapper dm0 = rhs.getLinearMapper(k, j2);
+            blockB[count+0] = cj(dm0(0));
+            blockB[count+1] = cj(dm0(1));
+            blockB[count+2] = cj(dm0(2));
+            blockB[count+3] = cj(dm0(3));
+            count += 4;
+          }
+        }
+        // skip what we have after
+        if(PanelMode) count += 4 * (stride-offset-depth);
+      }
+    }
+    // copy the remaining columns one at a time (nr==1)
+    for(Index j2=packet_cols4; j2<cols; ++j2)
+    {
+      if(PanelMode) count += offset;
       for(Index k=0; k<depth; k++)
       {
-        if (PacketSize==4) {
-          Packet A = rhs.loadPacket(k, j2);
-          pstoreu(blockB+count, cj.pconj(A));
-          count += PacketSize;
-        } else {
-          const LinearMapper dm0 = rhs.getLinearMapper(k, j2);
-          blockB[count+0] = cj(dm0(0));
-          blockB[count+1] = cj(dm0(1));
-          blockB[count+2] = cj(dm0(2));
-          blockB[count+3] = cj(dm0(3));
-          count += 4;
-        }
+        blockB[count] = cj(rhs(k, j2));
+        count += 1;
       }
-      // skip what we have after
-      if(PanelMode) count += 4 * (stride-offset-depth);
+      if(PanelMode) count += stride-offset-depth;
     }
   }
-  // copy the remaining columns one at a time (nr==1)
-  for(Index j2=packet_cols4; j2<cols; ++j2)
-  {
-    if(PanelMode) count += offset;
-    for(Index k=0; k<depth; k++)
-    {
-      blockB[count] = cj(rhs(k, j2));
-      count += 1;
-    }
-    if(PanelMode) count += stride-offset-depth;
-  }
-}
+};
 
 } // end namespace internal
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
index 6440e1d..caa65fc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrix.h
@@ -20,8 +20,9 @@
 template<
   typename Index,
   typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs>
-struct general_matrix_matrix_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,RowMajor>
+  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
+  int ResInnerStride>
+struct general_matrix_matrix_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride>
 {
   typedef gebp_traits<RhsScalar,LhsScalar> Traits;
 
@@ -30,7 +31,7 @@
     Index rows, Index cols, Index depth,
     const LhsScalar* lhs, Index lhsStride,
     const RhsScalar* rhs, Index rhsStride,
-    ResScalar* res, Index resStride,
+    ResScalar* res, Index resIncr, Index resStride,
     ResScalar alpha,
     level3_blocking<RhsScalar,LhsScalar>& blocking,
     GemmParallelInfo<Index>* info = 0)
@@ -39,8 +40,8 @@
     general_matrix_matrix_product<Index,
       RhsScalar, RhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateRhs,
       LhsScalar, LhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateLhs,
-      ColMajor>
-    ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resStride,alpha,blocking,info);
+      ColMajor,ResInnerStride>
+    ::run(cols,rows,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking,info);
   }
 };
 
@@ -49,8 +50,9 @@
 template<
   typename Index,
   typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs>
-struct general_matrix_matrix_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,ColMajor>
+  typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
+  int ResInnerStride>
+struct general_matrix_matrix_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride>
 {
 
 typedef gebp_traits<LhsScalar,RhsScalar> Traits;
@@ -59,23 +61,23 @@
 static void run(Index rows, Index cols, Index depth,
   const LhsScalar* _lhs, Index lhsStride,
   const RhsScalar* _rhs, Index rhsStride,
-  ResScalar* _res, Index resStride,
+  ResScalar* _res, Index resIncr, Index resStride,
   ResScalar alpha,
   level3_blocking<LhsScalar,RhsScalar>& blocking,
   GemmParallelInfo<Index>* info = 0)
 {
   typedef const_blas_data_mapper<LhsScalar, Index, LhsStorageOrder> LhsMapper;
   typedef const_blas_data_mapper<RhsScalar, Index, RhsStorageOrder> RhsMapper;
-  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor> ResMapper;
-  LhsMapper lhs(_lhs,lhsStride);
-  RhsMapper rhs(_rhs,rhsStride);
-  ResMapper res(_res, resStride);
+  typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor,Unaligned,ResInnerStride> ResMapper;
+  LhsMapper lhs(_lhs, lhsStride);
+  RhsMapper rhs(_rhs, rhsStride);
+  ResMapper res(_res, resStride, resIncr);
 
   Index kc = blocking.kc();                   // cache block size along the K direction
   Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
   Index nc = (std::min)(cols,blocking.nc());  // cache block size along the N direction
 
-  gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
+  gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
   gemm_pack_rhs<RhsScalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
   gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp;
 
@@ -108,7 +110,7 @@
       // i.e., we test that info[tid].users equals 0.
       // Then, we set info[tid].users to the number of threads to mark that all other threads are going to use it.
       while(info[tid].users!=0) {}
-      info[tid].users += threads;
+      info[tid].users = threads;
 
       pack_lhs(blockA+info[tid].lhs_start*actual_kc, lhs.getSubMapper(info[tid].lhs_start,k), actual_kc, info[tid].lhs_length);
 
@@ -146,7 +148,9 @@
       // Release all the sub blocks A'_i of A' for the current thread,
       // i.e., we simply decrement the number of users by 1
       for(Index i=0; i<threads; ++i)
+#if !EIGEN_HAS_CXX11_ATOMIC
         #pragma omp atomic
+#endif
         info[i].users -= 1;
     }
   }
@@ -226,7 +230,7 @@
     Gemm::run(rows, cols, m_lhs.cols(),
               &m_lhs.coeffRef(row,0), m_lhs.outerStride(),
               &m_rhs.coeffRef(0,col), m_rhs.outerStride(),
-              (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.outerStride(),
+              (Scalar*)&(m_dest.coeffRef(row,col)), m_dest.innerStride(), m_dest.outerStride(),
               m_actualAlpha, m_blocking, info);
   }
 
@@ -427,8 +431,14 @@
   template<typename Dst>
   static void evalTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
-    if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0)
-      lazyproduct::evalTo(dst, lhs, rhs);
+    // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=404 for a discussion and helper program
+    // to determine the following heuristic.
+    // EIGEN_GEMM_TO_COEFFBASED_THRESHOLD is typically defined to 20 in GeneralProduct.h,
+    // unless it has been specialized by the user or for a given architecture.
+    // Note that the condition rhs.rows()>0 was required because lazy product is (was?) not happy with empty inputs.
+    // I'm not sure it is still required.
+    if((rhs.rows()+dst.rows()+dst.cols())<EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows()>0)
+      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::assign_op<typename Dst::Scalar,Scalar>());
     else
     {
       dst.setZero();
@@ -439,8 +449,8 @@
   template<typename Dst>
   static void addTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
-    if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0)
-      lazyproduct::addTo(dst, lhs, rhs);
+    if((rhs.rows()+dst.rows()+dst.cols())<EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows()>0)
+      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::add_assign_op<typename Dst::Scalar,Scalar>());
     else
       scaleAndAddTo(dst,lhs, rhs, Scalar(1));
   }
@@ -448,8 +458,8 @@
   template<typename Dst>
   static void subTo(Dst& dst, const Lhs& lhs, const Rhs& rhs)
   {
-    if((rhs.rows()+dst.rows()+dst.cols())<20 && rhs.rows()>0)
-      lazyproduct::subTo(dst, lhs, rhs);
+    if((rhs.rows()+dst.rows()+dst.cols())<EIGEN_GEMM_TO_COEFFBASED_THRESHOLD && rhs.rows()>0)
+      lazyproduct::eval_dynamic(dst, lhs, rhs, internal::sub_assign_op<typename Dst::Scalar,Scalar>());
     else
       scaleAndAddTo(dst, lhs, rhs, Scalar(-1));
   }
@@ -461,11 +471,25 @@
     if(a_lhs.cols()==0 || a_lhs.rows()==0 || a_rhs.cols()==0)
       return;
 
+    if (dst.cols() == 1)
+    {
+      // Fallback to GEMV if either the lhs or rhs is a runtime vector
+      typename Dest::ColXpr dst_vec(dst.col(0));
+      return internal::generic_product_impl<Lhs,typename Rhs::ConstColXpr,DenseShape,DenseShape,GemvProduct>
+        ::scaleAndAddTo(dst_vec, a_lhs, a_rhs.col(0), alpha);
+    }
+    else if (dst.rows() == 1)
+    {
+      // Fallback to GEMV if either the lhs or rhs is a runtime vector
+      typename Dest::RowXpr dst_vec(dst.row(0));
+      return internal::generic_product_impl<typename Lhs::ConstRowXpr,Rhs,DenseShape,DenseShape,GemvProduct>
+        ::scaleAndAddTo(dst_vec, a_lhs.row(0), a_rhs, alpha);
+    }
+
     typename internal::add_const_on_value_type<ActualLhsType>::type lhs = LhsBlasTraits::extract(a_lhs);
     typename internal::add_const_on_value_type<ActualRhsType>::type rhs = RhsBlasTraits::extract(a_rhs);
 
-    Scalar actualAlpha = alpha * LhsBlasTraits::extractScalarFactor(a_lhs)
-                               * RhsBlasTraits::extractScalarFactor(a_rhs);
+    Scalar actualAlpha = combine_scalar_factors(alpha, a_lhs, a_rhs);
 
     typedef internal::gemm_blocking_space<(Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,LhsScalar,RhsScalar,
             Dest::MaxRowsAtCompileTime,Dest::MaxColsAtCompileTime,MaxDepthAtCompileTime> BlockingType;
@@ -476,7 +500,8 @@
         Index,
         LhsScalar, (ActualLhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(LhsBlasTraits::NeedToConjugate),
         RhsScalar, (ActualRhsTypeCleaned::Flags&RowMajorBit) ? RowMajor : ColMajor, bool(RhsBlasTraits::NeedToConjugate),
-        (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor>,
+        (Dest::Flags&RowMajorBit) ? RowMajor : ColMajor,
+        Dest::InnerStrideAtCompileTime>,
       ActualLhsTypeCleaned, ActualRhsTypeCleaned, Dest, BlockingType> GemmFunctor;
 
     BlockingType blocking(dst.rows(), dst.cols(), lhs.cols(), 1, true);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
index e844e37..6ba0d9b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
@@ -25,51 +25,54 @@
 **********************************************************************/
 
 // forward declarations (defined at the end of this file)
-template<typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs, int UpLo>
+template<typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs, int ResInnerStride, int UpLo>
 struct tribb_kernel;
   
 /* Optimized matrix-matrix product evaluating only one triangular half */
 template <typename Index,
           typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
           typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-                              int ResStorageOrder, int  UpLo, int Version = Specialized>
+                              int ResStorageOrder, int ResInnerStride, int  UpLo, int Version = Specialized>
 struct general_matrix_matrix_triangular_product;
 
 // as usual if the result is row major => we transpose the product
 template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-                          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, int  UpLo, int Version>
-struct general_matrix_matrix_triangular_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,RowMajor,UpLo,Version>
+                          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
+                          int ResInnerStride, int  UpLo, int Version>
+struct general_matrix_matrix_triangular_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride,UpLo,Version>
 {
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
   static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* lhs, Index lhsStride,
-                                      const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resStride,
+                                      const RhsScalar* rhs, Index rhsStride, ResScalar* res, Index resIncr, Index resStride,
                                       const ResScalar& alpha, level3_blocking<RhsScalar,LhsScalar>& blocking)
   {
     general_matrix_matrix_triangular_product<Index,
         RhsScalar, RhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateRhs,
         LhsScalar, LhsStorageOrder==RowMajor ? ColMajor : RowMajor, ConjugateLhs,
-        ColMajor, UpLo==Lower?Upper:Lower>
-      ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resStride,alpha,blocking);
+        ColMajor, ResInnerStride, UpLo==Lower?Upper:Lower>
+      ::run(size,depth,rhs,rhsStride,lhs,lhsStride,res,resIncr,resStride,alpha,blocking);
   }
 };
 
 template <typename Index, typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
-                          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs, int  UpLo, int Version>
-struct general_matrix_matrix_triangular_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,ColMajor,UpLo,Version>
+                          typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
+                          int ResInnerStride, int  UpLo, int Version>
+struct general_matrix_matrix_triangular_product<Index,LhsScalar,LhsStorageOrder,ConjugateLhs,RhsScalar,RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,UpLo,Version>
 {
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
   static EIGEN_STRONG_INLINE void run(Index size, Index depth,const LhsScalar* _lhs, Index lhsStride,
-                                      const RhsScalar* _rhs, Index rhsStride, ResScalar* _res, Index resStride,
+                                      const RhsScalar* _rhs, Index rhsStride,
+                                      ResScalar* _res, Index resIncr, Index resStride,
                                       const ResScalar& alpha, level3_blocking<LhsScalar,RhsScalar>& blocking)
   {
     typedef gebp_traits<LhsScalar,RhsScalar> Traits;
 
     typedef const_blas_data_mapper<LhsScalar, Index, LhsStorageOrder> LhsMapper;
     typedef const_blas_data_mapper<RhsScalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor> ResMapper;
+    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
     LhsMapper lhs(_lhs,lhsStride);
     RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride);
+    ResMapper res(_res, resStride, resIncr);
 
     Index kc = blocking.kc();
     Index mc = (std::min)(size,blocking.mc());
@@ -84,10 +87,10 @@
     ei_declare_aligned_stack_constructed_variable(LhsScalar, blockA, sizeA, blocking.blockA());
     ei_declare_aligned_stack_constructed_variable(RhsScalar, blockB, sizeB, blocking.blockB());
 
-    gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
+    gemm_pack_lhs<LhsScalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
     gemm_pack_rhs<RhsScalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
     gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp;
-    tribb_kernel<LhsScalar, RhsScalar, Index, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs, UpLo> sybb;
+    tribb_kernel<LhsScalar, RhsScalar, Index, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs, ResInnerStride, UpLo> sybb;
 
     for(Index k2=0; k2<depth; k2+=kc)
     {
@@ -110,8 +113,7 @@
           gebp(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc,
                (std::min)(size,i2), alpha, -1, -1, 0, 0);
 
-
-        sybb(_res+resStride*i2 + i2, resStride, blockA, blockB + actual_kc*i2, actual_mc, actual_kc, alpha);
+        sybb(_res+resStride*i2 + resIncr*i2, resIncr, resStride, blockA, blockB + actual_kc*i2, actual_mc, actual_kc, alpha);
 
         if (UpLo==Upper)
         {
@@ -133,7 +135,7 @@
 //   while the triangular block overlapping the diagonal is evaluated into a
 //   small temporary buffer which is then accumulated into the result using a
 //   triangular traversal.
-template<typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs, int UpLo>
+template<typename LhsScalar, typename RhsScalar, typename Index, int mr, int nr, bool ConjLhs, bool ConjRhs, int ResInnerStride, int UpLo>
 struct tribb_kernel
 {
   typedef gebp_traits<LhsScalar,RhsScalar,ConjLhs,ConjRhs> Traits;
@@ -142,11 +144,13 @@
   enum {
     BlockSize  = meta_least_common_multiple<EIGEN_PLAIN_ENUM_MAX(mr,nr),EIGEN_PLAIN_ENUM_MIN(mr,nr)>::ret
   };
-  void operator()(ResScalar* _res, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha)
+  void operator()(ResScalar* _res, Index resIncr, Index resStride, const LhsScalar* blockA, const RhsScalar* blockB, Index size, Index depth, const ResScalar& alpha)
   {
-    typedef blas_data_mapper<ResScalar, Index, ColMajor> ResMapper;
-    ResMapper res(_res, resStride);
-    gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, mr, nr, ConjLhs, ConjRhs> gebp_kernel;
+    typedef blas_data_mapper<ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
+    typedef blas_data_mapper<ResScalar, Index, ColMajor, Unaligned> BufferMapper;
+    ResMapper res(_res, resStride, resIncr);
+    gebp_kernel<LhsScalar, RhsScalar, Index, ResMapper, mr, nr, ConjLhs, ConjRhs> gebp_kernel1;
+    gebp_kernel<LhsScalar, RhsScalar, Index, BufferMapper, mr, nr, ConjLhs, ConjRhs> gebp_kernel2;
 
     Matrix<ResScalar,BlockSize,BlockSize,ColMajor> buffer((internal::constructor_without_unaligned_array_assert()));
 
@@ -158,31 +162,32 @@
       const RhsScalar* actual_b = blockB+j*depth;
 
       if(UpLo==Upper)
-        gebp_kernel(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha,
-                    -1, -1, 0, 0);
-
+        gebp_kernel1(res.getSubMapper(0, j), blockA, actual_b, j, depth, actualBlockSize, alpha,
+                     -1, -1, 0, 0);
+      
       // selfadjoint micro block
       {
         Index i = j;
         buffer.setZero();
         // 1 - apply the kernel on the temporary buffer
-        gebp_kernel(ResMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha,
-                    -1, -1, 0, 0);
+        gebp_kernel2(BufferMapper(buffer.data(), BlockSize), blockA+depth*i, actual_b, actualBlockSize, depth, actualBlockSize, alpha,
+                     -1, -1, 0, 0);
+
         // 2 - triangular accumulation
         for(Index j1=0; j1<actualBlockSize; ++j1)
         {
-          ResScalar* r = &res(i, j + j1);
+          typename ResMapper::LinearMapper r = res.getLinearMapper(i,j+j1);
           for(Index i1=UpLo==Lower ? j1 : 0;
               UpLo==Lower ? i1<actualBlockSize : i1<=j1; ++i1)
-            r[i1] += buffer(i1,j1);
+            r(i1) += buffer(i1,j1);
         }
       }
 
       if(UpLo==Lower)
       {
         Index i = j+actualBlockSize;
-        gebp_kernel(res.getSubMapper(i, j), blockA+depth*i, actual_b, size-i, 
-                    depth, actualBlockSize, alpha, -1, -1, 0, 0);
+        gebp_kernel1(res.getSubMapper(i, j), blockA+depth*i, actual_b, size-i, 
+                     depth, actualBlockSize, alpha, -1, -1, 0, 0);
       }
     }
   }
@@ -286,23 +291,24 @@
     internal::general_matrix_matrix_triangular_product<Index,
       typename Lhs::Scalar, LhsIsRowMajor ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate,
       typename Rhs::Scalar, RhsIsRowMajor ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate,
-      IsRowMajor ? RowMajor : ColMajor, UpLo&(Lower|Upper)>
+      IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo&(Lower|Upper)>
       ::run(size, depth,
             &actualLhs.coeffRef(SkipDiag&&(UpLo&Lower)==Lower ? 1 : 0,0), actualLhs.outerStride(),
             &actualRhs.coeffRef(0,SkipDiag&&(UpLo&Upper)==Upper ? 1 : 0), actualRhs.outerStride(),
-            mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? 1 : mat.outerStride() ) : 0), mat.outerStride(), actualAlpha, blocking);
+            mat.data() + (SkipDiag ? (bool(IsRowMajor) != ((UpLo&Lower)==Lower) ? mat.innerStride() : mat.outerStride() ) : 0),
+            mat.innerStride(), mat.outerStride(), actualAlpha, blocking);
   }
 };
 
 template<typename MatrixType, unsigned int UpLo>
 template<typename ProductType>
-TriangularView<MatrixType,UpLo>& TriangularViewImpl<MatrixType,UpLo,Dense>::_assignProduct(const ProductType& prod, const Scalar& alpha, bool beta)
+EIGEN_DEVICE_FUNC TriangularView<MatrixType,UpLo>& TriangularViewImpl<MatrixType,UpLo,Dense>::_assignProduct(const ProductType& prod, const Scalar& alpha, bool beta)
 {
   EIGEN_STATIC_ASSERT((UpLo&UnitDiag)==0, WRITING_TO_TRIANGULAR_PART_WITH_UNIT_DIAGONAL_IS_NOT_SUPPORTED);
   eigen_assert(derived().nestedExpression().rows() == prod.rows() && derived().cols() == prod.cols());
-  
+
   general_product_to_triangular_selector<MatrixType, ProductType, UpLo, internal::traits<ProductType>::InnerSize==1>::run(derived().nestedExpression().const_cast_derived(), prod, alpha, beta);
-  
+
   return derived();
 }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
index a597c1f..dfb6aeb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/GeneralMatrixVector.h
@@ -1,7 +1,7 @@
 // This file is part of Eigen, a lightweight C++ template library
 // for linear algebra.
 //
-// Copyright (C) 2008-2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2008-2016 Gael Guennebaud <gael.guennebaud@inria.fr>
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -14,11 +14,57 @@
 
 namespace internal {
 
+enum GEMVPacketSizeType {
+  GEMVPacketFull = 0,
+  GEMVPacketHalf,
+  GEMVPacketQuarter
+};
+
+template <int N, typename T1, typename T2, typename T3>
+struct gemv_packet_cond { typedef T3 type; };
+
+template <typename T1, typename T2, typename T3>
+struct gemv_packet_cond<GEMVPacketFull, T1, T2, T3> { typedef T1 type; };
+
+template <typename T1, typename T2, typename T3>
+struct gemv_packet_cond<GEMVPacketHalf, T1, T2, T3> { typedef T2 type; };
+
+template<typename LhsScalar, typename RhsScalar, int _PacketSize=GEMVPacketFull>
+class gemv_traits
+{
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
+
+#define PACKET_DECL_COND_PREFIX(prefix, name, packet_size)                        \
+  typedef typename gemv_packet_cond<packet_size,                                  \
+                                    typename packet_traits<name ## Scalar>::type, \
+                                    typename packet_traits<name ## Scalar>::half, \
+                                    typename unpacket_traits<typename packet_traits<name ## Scalar>::half>::half>::type \
+  prefix ## name ## Packet
+
+  PACKET_DECL_COND_PREFIX(_, Lhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Rhs, _PacketSize);
+  PACKET_DECL_COND_PREFIX(_, Res, _PacketSize);
+#undef PACKET_DECL_COND_PREFIX
+
+public:
+  enum {
+        Vectorizable = unpacket_traits<_LhsPacket>::vectorizable &&
+        unpacket_traits<_RhsPacket>::vectorizable &&
+        int(unpacket_traits<_LhsPacket>::size)==int(unpacket_traits<_RhsPacket>::size),
+        LhsPacketSize = Vectorizable ? unpacket_traits<_LhsPacket>::size : 1,
+        RhsPacketSize = Vectorizable ? unpacket_traits<_RhsPacket>::size : 1,
+        ResPacketSize = Vectorizable ? unpacket_traits<_ResPacket>::size : 1
+  };
+
+  typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
+  typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
+  typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+};
+
+
 /* Optimized col-major matrix * vector product:
- * This algorithm processes 4 columns at onces that allows to both reduce
- * the number of load/stores of the result by a factor 4 and to reduce
- * the instruction dependency. Moreover, we know that all bands have the
- * same alignment pattern.
+ * This algorithm processes the matrix per vertical panels,
+ * which are then processed horizontaly per chunck of 8*PacketSize x 1 vertical segments.
  *
  * Mixing type logic: C += alpha * A * B
  *  |  A  |  B  |alpha| comments
@@ -27,56 +73,30 @@
  *  |cplx |real |cplx | invalid, the caller has to do tmp: = A * B; C += alpha*tmp
  *  |cplx |real |real | optimal case, vectorization possible via real-cplx mul
  *
- * Accesses to the matrix coefficients follow the following logic:
- *
- * - if all columns have the same alignment then
- *   - if the columns have the same alignment as the result vector, then easy! (-> AllAligned case)
- *   - otherwise perform unaligned loads only (-> NoneAligned case)
- * - otherwise
- *   - if even columns have the same alignment then
- *     // odd columns are guaranteed to have the same alignment too
- *     - if even or odd columns have the same alignment as the result, then
- *       // for a register size of 2 scalars, this is guarantee to be the case (e.g., SSE with double)
- *       - perform half aligned and half unaligned loads (-> EvenAligned case)
- *     - otherwise perform unaligned loads only (-> NoneAligned case)
- *   - otherwise, if the register size is 4 scalars (e.g., SSE with float) then
- *     - one over 4 consecutive columns is guaranteed to be aligned with the result vector,
- *       perform simple aligned loads for this column and aligned loads plus re-alignment for the other. (-> FirstAligned case)
- *       // this re-alignment is done by the palign function implemented for SSE in Eigen/src/Core/arch/SSE/PacketMath.h
- *   - otherwise,
- *     // if we get here, this means the register size is greater than 4 (e.g., AVX with floats),
- *     // we currently fall back to the NoneAligned case
- *
  * The same reasoning apply for the transposed case.
- *
- * The last case (PacketSize>4) could probably be improved by generalizing the FirstAligned case, but since we do not support AVX yet...
- * One might also wonder why in the EvenAligned case we perform unaligned loads instead of using the aligned-loads plus re-alignment
- * strategy as in the FirstAligned case. The reason is that we observed that unaligned loads on a 8 byte boundary are not too slow
- * compared to unaligned loads on a 4 byte boundary.
- *
  */
 template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
 struct general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>
 {
+  typedef gemv_traits<LhsScalar,RhsScalar> Traits;
+  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketHalf> HalfTraits;
+  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketQuarter> QuarterTraits;
+
   typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
-enum {
-  Vectorizable = packet_traits<LhsScalar>::Vectorizable && packet_traits<RhsScalar>::Vectorizable
-              && int(packet_traits<LhsScalar>::size)==int(packet_traits<RhsScalar>::size),
-  LhsPacketSize = Vectorizable ? packet_traits<LhsScalar>::size : 1,
-  RhsPacketSize = Vectorizable ? packet_traits<RhsScalar>::size : 1,
-  ResPacketSize = Vectorizable ? packet_traits<ResScalar>::size : 1
-};
+  typedef typename Traits::LhsPacket LhsPacket;
+  typedef typename Traits::RhsPacket RhsPacket;
+  typedef typename Traits::ResPacket ResPacket;
 
-typedef typename packet_traits<LhsScalar>::type  _LhsPacket;
-typedef typename packet_traits<RhsScalar>::type  _RhsPacket;
-typedef typename packet_traits<ResScalar>::type  _ResPacket;
+  typedef typename HalfTraits::LhsPacket LhsPacketHalf;
+  typedef typename HalfTraits::RhsPacket RhsPacketHalf;
+  typedef typename HalfTraits::ResPacket ResPacketHalf;
 
-typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
-typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
-typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef typename QuarterTraits::LhsPacket LhsPacketQuarter;
+  typedef typename QuarterTraits::RhsPacket RhsPacketQuarter;
+  typedef typename QuarterTraits::ResPacket ResPacketQuarter;
 
-EIGEN_DONT_INLINE static void run(
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static void run(
   Index rows, Index cols,
   const LhsMapper& lhs,
   const RhsMapper& rhs,
@@ -85,244 +105,187 @@
 };
 
 template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
-EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>::run(
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>::run(
   Index rows, Index cols,
-  const LhsMapper& lhs,
+  const LhsMapper& alhs,
   const RhsMapper& rhs,
         ResScalar* res, Index resIncr,
   RhsScalar alpha)
 {
   EIGEN_UNUSED_VARIABLE(resIncr);
   eigen_internal_assert(resIncr==1);
-  #ifdef _EIGEN_ACCUMULATE_PACKETS
-  #error _EIGEN_ACCUMULATE_PACKETS has already been defined
-  #endif
-  #define _EIGEN_ACCUMULATE_PACKETS(Alignment0,Alignment13,Alignment2) \
-    pstore(&res[j], \
-      padd(pload<ResPacket>(&res[j]), \
-        padd( \
-      padd(pcj.pmul(lhs0.template load<LhsPacket, Alignment0>(j),    ptmp0), \
-      pcj.pmul(lhs1.template load<LhsPacket, Alignment13>(j),   ptmp1)),   \
-      padd(pcj.pmul(lhs2.template load<LhsPacket, Alignment2>(j),    ptmp2), \
-      pcj.pmul(lhs3.template load<LhsPacket, Alignment13>(j),   ptmp3)) )))
 
-  typedef typename LhsMapper::VectorMapper LhsScalars;
+  // The following copy tells the compiler that lhs's attributes are not modified outside this function
+  // This helps GCC to generate propoer code.
+  LhsMapper lhs(alhs);
 
   conj_helper<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs> cj;
   conj_helper<LhsPacket,RhsPacket,ConjugateLhs,ConjugateRhs> pcj;
-  if(ConjugateRhs)
-    alpha = numext::conj(alpha);
-
-  enum { AllAligned = 0, EvenAligned, FirstAligned, NoneAligned };
-  const Index columnsAtOnce = 4;
-  const Index peels = 2;
-  const Index LhsPacketAlignedMask = LhsPacketSize-1;
-  const Index ResPacketAlignedMask = ResPacketSize-1;
-//  const Index PeelAlignedMask = ResPacketSize*peels-1;
-  const Index size = rows;
+  conj_helper<LhsPacketHalf,RhsPacketHalf,ConjugateLhs,ConjugateRhs> pcj_half;
+  conj_helper<LhsPacketQuarter,RhsPacketQuarter,ConjugateLhs,ConjugateRhs> pcj_quarter;
 
   const Index lhsStride = lhs.stride();
+  // TODO: for padded aligned inputs, we could enable aligned reads
+  enum { LhsAlignment = Unaligned,
+         ResPacketSize = Traits::ResPacketSize,
+         ResPacketSizeHalf = HalfTraits::ResPacketSize,
+         ResPacketSizeQuarter = QuarterTraits::ResPacketSize,
+         LhsPacketSize = Traits::LhsPacketSize,
+         HasHalf = (int)ResPacketSizeHalf < (int)ResPacketSize,
+         HasQuarter = (int)ResPacketSizeQuarter < (int)ResPacketSizeHalf
+  };
 
-  // How many coeffs of the result do we have to skip to be aligned.
-  // Here we assume data are at least aligned on the base scalar type.
-  Index alignedStart = internal::first_default_aligned(res,size);
-  Index alignedSize = ResPacketSize>1 ? alignedStart + ((size-alignedStart) & ~ResPacketAlignedMask) : 0;
-  const Index peeledSize = alignedSize - RhsPacketSize*peels - RhsPacketSize + 1;
+  const Index n8 = rows-8*ResPacketSize+1;
+  const Index n4 = rows-4*ResPacketSize+1;
+  const Index n3 = rows-3*ResPacketSize+1;
+  const Index n2 = rows-2*ResPacketSize+1;
+  const Index n1 = rows-1*ResPacketSize+1;
+  const Index n_half = rows-1*ResPacketSizeHalf+1;
+  const Index n_quarter = rows-1*ResPacketSizeQuarter+1;
 
-  const Index alignmentStep = LhsPacketSize>1 ? (LhsPacketSize - lhsStride % LhsPacketSize) & LhsPacketAlignedMask : 0;
-  Index alignmentPattern = alignmentStep==0 ? AllAligned
-                       : alignmentStep==(LhsPacketSize/2) ? EvenAligned
-                       : FirstAligned;
+  // TODO: improve the following heuristic:
+  const Index block_cols = cols<128 ? cols : (lhsStride*sizeof(LhsScalar)<32000?16:4);
+  ResPacket palpha = pset1<ResPacket>(alpha);
+  ResPacketHalf palpha_half = pset1<ResPacketHalf>(alpha);
+  ResPacketQuarter palpha_quarter = pset1<ResPacketQuarter>(alpha);
 
-  // we cannot assume the first element is aligned because of sub-matrices
-  const Index lhsAlignmentOffset = lhs.firstAligned(size);
-
-  // find how many columns do we have to skip to be aligned with the result (if possible)
-  Index skipColumns = 0;
-  // if the data cannot be aligned (TODO add some compile time tests when possible, e.g. for floats)
-  if( (lhsAlignmentOffset < 0) || (lhsAlignmentOffset == size) || (UIntPtr(res)%sizeof(ResScalar)) )
+  for(Index j2=0; j2<cols; j2+=block_cols)
   {
-    alignedSize = 0;
-    alignedStart = 0;
-    alignmentPattern = NoneAligned;
-  }
-  else if(LhsPacketSize > 4)
-  {
-    // TODO: extend the code to support aligned loads whenever possible when LhsPacketSize > 4.
-    // Currently, it seems to be better to perform unaligned loads anyway
-    alignmentPattern = NoneAligned;
-  }
-  else if (LhsPacketSize>1)
-  {
-  //    eigen_internal_assert(size_t(firstLhs+lhsAlignmentOffset)%sizeof(LhsPacket)==0 || size<LhsPacketSize);
-
-    while (skipColumns<LhsPacketSize &&
-          alignedStart != ((lhsAlignmentOffset + alignmentStep*skipColumns)%LhsPacketSize))
-      ++skipColumns;
-    if (skipColumns==LhsPacketSize)
+    Index jend = numext::mini(j2+block_cols,cols);
+    Index i=0;
+    for(; i<n8; i+=ResPacketSize*8)
     {
-      // nothing can be aligned, no need to skip any column
-      alignmentPattern = NoneAligned;
-      skipColumns = 0;
-    }
-    else
-    {
-      skipColumns = (std::min)(skipColumns,cols);
-      // note that the skiped columns are processed later.
-    }
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+                c1 = pset1<ResPacket>(ResScalar(0)),
+                c2 = pset1<ResPacket>(ResScalar(0)),
+                c3 = pset1<ResPacket>(ResScalar(0)),
+                c4 = pset1<ResPacket>(ResScalar(0)),
+                c5 = pset1<ResPacket>(ResScalar(0)),
+                c6 = pset1<ResPacket>(ResScalar(0)),
+                c7 = pset1<ResPacket>(ResScalar(0));
 
-    /*    eigen_internal_assert(  (alignmentPattern==NoneAligned)
-                      || (skipColumns + columnsAtOnce >= cols)
-                      || LhsPacketSize > size
-                      || (size_t(firstLhs+alignedStart+lhsStride*skipColumns)%sizeof(LhsPacket))==0);*/
-  }
-  else if(Vectorizable)
-  {
-    alignedStart = 0;
-    alignedSize = size;
-    alignmentPattern = AllAligned;
-  }
-
-  const Index offset1 = (alignmentPattern==FirstAligned && alignmentStep==1)?3:1;
-  const Index offset3 = (alignmentPattern==FirstAligned && alignmentStep==1)?1:3;
-
-  Index columnBound = ((cols-skipColumns)/columnsAtOnce)*columnsAtOnce + skipColumns;
-  for (Index i=skipColumns; i<columnBound; i+=columnsAtOnce)
-  {
-    RhsPacket ptmp0 = pset1<RhsPacket>(alpha*rhs(i, 0)),
-              ptmp1 = pset1<RhsPacket>(alpha*rhs(i+offset1, 0)),
-              ptmp2 = pset1<RhsPacket>(alpha*rhs(i+2, 0)),
-              ptmp3 = pset1<RhsPacket>(alpha*rhs(i+offset3, 0));
-
-    // this helps a lot generating better binary code
-    const LhsScalars lhs0 = lhs.getVectorMapper(0, i+0),   lhs1 = lhs.getVectorMapper(0, i+offset1),
-                     lhs2 = lhs.getVectorMapper(0, i+2),   lhs3 = lhs.getVectorMapper(0, i+offset3);
-
-    if (Vectorizable)
-    {
-      /* explicit vectorization */
-      // process initial unaligned coeffs
-      for (Index j=0; j<alignedStart; ++j)
+      for(Index j=j2; j<jend; j+=1)
       {
-        res[j] = cj.pmadd(lhs0(j), pfirst(ptmp0), res[j]);
-        res[j] = cj.pmadd(lhs1(j), pfirst(ptmp1), res[j]);
-        res[j] = cj.pmadd(lhs2(j), pfirst(ptmp2), res[j]);
-        res[j] = cj.pmadd(lhs3(j), pfirst(ptmp3), res[j]);
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
+        c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*2,j),b0,c2);
+        c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*3,j),b0,c3);
+        c4 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*4,j),b0,c4);
+        c5 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*5,j),b0,c5);
+        c6 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*6,j),b0,c6);
+        c7 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*7,j),b0,c7);
       }
-
-      if (alignedSize>alignedStart)
-      {
-        switch(alignmentPattern)
-        {
-          case AllAligned:
-            for (Index j = alignedStart; j<alignedSize; j+=ResPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Aligned,Aligned,Aligned);
-            break;
-          case EvenAligned:
-            for (Index j = alignedStart; j<alignedSize; j+=ResPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Aligned,Unaligned,Aligned);
-            break;
-          case FirstAligned:
-          {
-            Index j = alignedStart;
-            if(peels>1)
-            {
-              LhsPacket A00, A01, A02, A03, A10, A11, A12, A13;
-              ResPacket T0, T1;
-
-              A01 = lhs1.template load<LhsPacket, Aligned>(alignedStart-1);
-              A02 = lhs2.template load<LhsPacket, Aligned>(alignedStart-2);
-              A03 = lhs3.template load<LhsPacket, Aligned>(alignedStart-3);
-
-              for (; j<peeledSize; j+=peels*ResPacketSize)
-              {
-                A11 = lhs1.template load<LhsPacket, Aligned>(j-1+LhsPacketSize);  palign<1>(A01,A11);
-                A12 = lhs2.template load<LhsPacket, Aligned>(j-2+LhsPacketSize);  palign<2>(A02,A12);
-                A13 = lhs3.template load<LhsPacket, Aligned>(j-3+LhsPacketSize);  palign<3>(A03,A13);
-
-                A00 = lhs0.template load<LhsPacket, Aligned>(j);
-                A10 = lhs0.template load<LhsPacket, Aligned>(j+LhsPacketSize);
-                T0  = pcj.pmadd(A00, ptmp0, pload<ResPacket>(&res[j]));
-                T1  = pcj.pmadd(A10, ptmp0, pload<ResPacket>(&res[j+ResPacketSize]));
-
-                T0  = pcj.pmadd(A01, ptmp1, T0);
-                A01 = lhs1.template load<LhsPacket, Aligned>(j-1+2*LhsPacketSize);  palign<1>(A11,A01);
-                T0  = pcj.pmadd(A02, ptmp2, T0);
-                A02 = lhs2.template load<LhsPacket, Aligned>(j-2+2*LhsPacketSize);  palign<2>(A12,A02);
-                T0  = pcj.pmadd(A03, ptmp3, T0);
-                pstore(&res[j],T0);
-                A03 = lhs3.template load<LhsPacket, Aligned>(j-3+2*LhsPacketSize);  palign<3>(A13,A03);
-                T1  = pcj.pmadd(A11, ptmp1, T1);
-                T1  = pcj.pmadd(A12, ptmp2, T1);
-                T1  = pcj.pmadd(A13, ptmp3, T1);
-                pstore(&res[j+ResPacketSize],T1);
-              }
-            }
-            for (; j<alignedSize; j+=ResPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Aligned,Unaligned,Unaligned);
-            break;
-          }
-          default:
-            for (Index j = alignedStart; j<alignedSize; j+=ResPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Unaligned,Unaligned,Unaligned);
-            break;
-        }
-      }
-    } // end explicit vectorization
-
-    /* process remaining coeffs (or all if there is no explicit vectorization) */
-    for (Index j=alignedSize; j<size; ++j)
+      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
+      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
+      pstoreu(res+i+ResPacketSize*2, pmadd(c2,palpha,ploadu<ResPacket>(res+i+ResPacketSize*2)));
+      pstoreu(res+i+ResPacketSize*3, pmadd(c3,palpha,ploadu<ResPacket>(res+i+ResPacketSize*3)));
+      pstoreu(res+i+ResPacketSize*4, pmadd(c4,palpha,ploadu<ResPacket>(res+i+ResPacketSize*4)));
+      pstoreu(res+i+ResPacketSize*5, pmadd(c5,palpha,ploadu<ResPacket>(res+i+ResPacketSize*5)));
+      pstoreu(res+i+ResPacketSize*6, pmadd(c6,palpha,ploadu<ResPacket>(res+i+ResPacketSize*6)));
+      pstoreu(res+i+ResPacketSize*7, pmadd(c7,palpha,ploadu<ResPacket>(res+i+ResPacketSize*7)));
+    }
+    if(i<n4)
     {
-      res[j] = cj.pmadd(lhs0(j), pfirst(ptmp0), res[j]);
-      res[j] = cj.pmadd(lhs1(j), pfirst(ptmp1), res[j]);
-      res[j] = cj.pmadd(lhs2(j), pfirst(ptmp2), res[j]);
-      res[j] = cj.pmadd(lhs3(j), pfirst(ptmp3), res[j]);
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+                c1 = pset1<ResPacket>(ResScalar(0)),
+                c2 = pset1<ResPacket>(ResScalar(0)),
+                c3 = pset1<ResPacket>(ResScalar(0));
+
+      for(Index j=j2; j<jend; j+=1)
+      {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
+        c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*2,j),b0,c2);
+        c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*3,j),b0,c3);
+      }
+      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
+      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
+      pstoreu(res+i+ResPacketSize*2, pmadd(c2,palpha,ploadu<ResPacket>(res+i+ResPacketSize*2)));
+      pstoreu(res+i+ResPacketSize*3, pmadd(c3,palpha,ploadu<ResPacket>(res+i+ResPacketSize*3)));
+
+      i+=ResPacketSize*4;
+    }
+    if(i<n3)
+    {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+                c1 = pset1<ResPacket>(ResScalar(0)),
+                c2 = pset1<ResPacket>(ResScalar(0));
+
+      for(Index j=j2; j<jend; j+=1)
+      {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
+        c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*2,j),b0,c2);
+      }
+      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
+      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
+      pstoreu(res+i+ResPacketSize*2, pmadd(c2,palpha,ploadu<ResPacket>(res+i+ResPacketSize*2)));
+
+      i+=ResPacketSize*3;
+    }
+    if(i<n2)
+    {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+                c1 = pset1<ResPacket>(ResScalar(0));
+
+      for(Index j=j2; j<jend; j+=1)
+      {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*0,j),b0,c0);
+        c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+LhsPacketSize*1,j),b0,c1);
+      }
+      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
+      pstoreu(res+i+ResPacketSize*1, pmadd(c1,palpha,ploadu<ResPacket>(res+i+ResPacketSize*1)));
+      i+=ResPacketSize*2;
+    }
+    if(i<n1)
+    {
+      ResPacket c0 = pset1<ResPacket>(ResScalar(0));
+      for(Index j=j2; j<jend; j+=1)
+      {
+        RhsPacket b0 = pset1<RhsPacket>(rhs(j,0));
+        c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
+      }
+      pstoreu(res+i+ResPacketSize*0, pmadd(c0,palpha,ploadu<ResPacket>(res+i+ResPacketSize*0)));
+      i+=ResPacketSize;
+    }
+    if(HasHalf && i<n_half)
+    {
+      ResPacketHalf c0 = pset1<ResPacketHalf>(ResScalar(0));
+      for(Index j=j2; j<jend; j+=1)
+      {
+        RhsPacketHalf b0 = pset1<RhsPacketHalf>(rhs(j,0));
+        c0 = pcj_half.pmadd(lhs.template load<LhsPacketHalf,LhsAlignment>(i+0,j),b0,c0);
+      }
+      pstoreu(res+i+ResPacketSizeHalf*0, pmadd(c0,palpha_half,ploadu<ResPacketHalf>(res+i+ResPacketSizeHalf*0)));
+      i+=ResPacketSizeHalf;
+    }
+    if(HasQuarter && i<n_quarter)
+    {
+      ResPacketQuarter c0 = pset1<ResPacketQuarter>(ResScalar(0));
+      for(Index j=j2; j<jend; j+=1)
+      {
+        RhsPacketQuarter b0 = pset1<RhsPacketQuarter>(rhs(j,0));
+        c0 = pcj_quarter.pmadd(lhs.template load<LhsPacketQuarter,LhsAlignment>(i+0,j),b0,c0);
+      }
+      pstoreu(res+i+ResPacketSizeQuarter*0, pmadd(c0,palpha_quarter,ploadu<ResPacketQuarter>(res+i+ResPacketSizeQuarter*0)));
+      i+=ResPacketSizeQuarter;
+    }
+    for(;i<rows;++i)
+    {
+      ResScalar c0(0);
+      for(Index j=j2; j<jend; j+=1)
+        c0 += cj.pmul(lhs(i,j), rhs(j,0));
+      res[i] += alpha*c0;
     }
   }
-
-  // process remaining first and last columns (at most columnsAtOnce-1)
-  Index end = cols;
-  Index start = columnBound;
-  do
-  {
-    for (Index k=start; k<end; ++k)
-    {
-      RhsPacket ptmp0 = pset1<RhsPacket>(alpha*rhs(k, 0));
-      const LhsScalars lhs0 = lhs.getVectorMapper(0, k);
-
-      if (Vectorizable)
-      {
-        /* explicit vectorization */
-        // process first unaligned result's coeffs
-        for (Index j=0; j<alignedStart; ++j)
-          res[j] += cj.pmul(lhs0(j), pfirst(ptmp0));
-        // process aligned result's coeffs
-        if (lhs0.template aligned<LhsPacket>(alignedStart))
-          for (Index i = alignedStart;i<alignedSize;i+=ResPacketSize)
-            pstore(&res[i], pcj.pmadd(lhs0.template load<LhsPacket, Aligned>(i), ptmp0, pload<ResPacket>(&res[i])));
-        else
-          for (Index i = alignedStart;i<alignedSize;i+=ResPacketSize)
-            pstore(&res[i], pcj.pmadd(lhs0.template load<LhsPacket, Unaligned>(i), ptmp0, pload<ResPacket>(&res[i])));
-      }
-
-      // process remaining scalars (or all if no explicit vectorization)
-      for (Index i=alignedSize; i<size; ++i)
-        res[i] += cj.pmul(lhs0(i), pfirst(ptmp0));
-    }
-    if (skipColumns)
-    {
-      start = 0;
-      end = skipColumns;
-      skipColumns = 0;
-    }
-    else
-      break;
-  } while(Vectorizable);
-  #undef _EIGEN_ACCUMULATE_PACKETS
 }
 
 /* Optimized row-major matrix * vector product:
- * This algorithm processes 4 rows at onces that allows to both reduce
+ * This algorithm processes 4 rows at once that allows to both reduce
  * the number of load/stores of the result by a factor 4 and to reduce
  * the instruction dependency. Moreover, we know that all bands have the
  * same alignment pattern.
@@ -334,25 +297,25 @@
 template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
 struct general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>
 {
-typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
+  typedef gemv_traits<LhsScalar,RhsScalar> Traits;
+  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketHalf> HalfTraits;
+  typedef gemv_traits<LhsScalar,RhsScalar,GEMVPacketQuarter> QuarterTraits;
 
-enum {
-  Vectorizable = packet_traits<LhsScalar>::Vectorizable && packet_traits<RhsScalar>::Vectorizable
-              && int(packet_traits<LhsScalar>::size)==int(packet_traits<RhsScalar>::size),
-  LhsPacketSize = Vectorizable ? packet_traits<LhsScalar>::size : 1,
-  RhsPacketSize = Vectorizable ? packet_traits<RhsScalar>::size : 1,
-  ResPacketSize = Vectorizable ? packet_traits<ResScalar>::size : 1
-};
+  typedef typename ScalarBinaryOpTraits<LhsScalar, RhsScalar>::ReturnType ResScalar;
 
-typedef typename packet_traits<LhsScalar>::type  _LhsPacket;
-typedef typename packet_traits<RhsScalar>::type  _RhsPacket;
-typedef typename packet_traits<ResScalar>::type  _ResPacket;
+  typedef typename Traits::LhsPacket LhsPacket;
+  typedef typename Traits::RhsPacket RhsPacket;
+  typedef typename Traits::ResPacket ResPacket;
 
-typedef typename conditional<Vectorizable,_LhsPacket,LhsScalar>::type LhsPacket;
-typedef typename conditional<Vectorizable,_RhsPacket,RhsScalar>::type RhsPacket;
-typedef typename conditional<Vectorizable,_ResPacket,ResScalar>::type ResPacket;
+  typedef typename HalfTraits::LhsPacket LhsPacketHalf;
+  typedef typename HalfTraits::RhsPacket RhsPacketHalf;
+  typedef typename HalfTraits::ResPacket ResPacketHalf;
 
-EIGEN_DONT_INLINE static void run(
+  typedef typename QuarterTraits::LhsPacket LhsPacketQuarter;
+  typedef typename QuarterTraits::RhsPacket RhsPacketQuarter;
+  typedef typename QuarterTraits::ResPacket ResPacketQuarter;
+
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE static void run(
   Index rows, Index cols,
   const LhsMapper& lhs,
   const RhsMapper& rhs,
@@ -361,255 +324,191 @@
 };
 
 template<typename Index, typename LhsScalar, typename LhsMapper, bool ConjugateLhs, typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version>
-EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>::run(
+EIGEN_DEVICE_FUNC EIGEN_DONT_INLINE void general_matrix_vector_product<Index,LhsScalar,LhsMapper,RowMajor,ConjugateLhs,RhsScalar,RhsMapper,ConjugateRhs,Version>::run(
   Index rows, Index cols,
-  const LhsMapper& lhs,
+  const LhsMapper& alhs,
   const RhsMapper& rhs,
   ResScalar* res, Index resIncr,
   ResScalar alpha)
 {
+  // The following copy tells the compiler that lhs's attributes are not modified outside this function
+  // This helps GCC to generate propoer code.
+  LhsMapper lhs(alhs);
+
   eigen_internal_assert(rhs.stride()==1);
-
-  #ifdef _EIGEN_ACCUMULATE_PACKETS
-  #error _EIGEN_ACCUMULATE_PACKETS has already been defined
-  #endif
-
-  #define _EIGEN_ACCUMULATE_PACKETS(Alignment0,Alignment13,Alignment2) {\
-    RhsPacket b = rhs.getVectorMapper(j, 0).template load<RhsPacket, Aligned>(0);  \
-    ptmp0 = pcj.pmadd(lhs0.template load<LhsPacket, Alignment0>(j), b, ptmp0); \
-    ptmp1 = pcj.pmadd(lhs1.template load<LhsPacket, Alignment13>(j), b, ptmp1); \
-    ptmp2 = pcj.pmadd(lhs2.template load<LhsPacket, Alignment2>(j), b, ptmp2); \
-    ptmp3 = pcj.pmadd(lhs3.template load<LhsPacket, Alignment13>(j), b, ptmp3); }
-
   conj_helper<LhsScalar,RhsScalar,ConjugateLhs,ConjugateRhs> cj;
   conj_helper<LhsPacket,RhsPacket,ConjugateLhs,ConjugateRhs> pcj;
+  conj_helper<LhsPacketHalf,RhsPacketHalf,ConjugateLhs,ConjugateRhs> pcj_half;
+  conj_helper<LhsPacketQuarter,RhsPacketQuarter,ConjugateLhs,ConjugateRhs> pcj_quarter;
 
-  typedef typename LhsMapper::VectorMapper LhsScalars;
+  // TODO: fine tune the following heuristic. The rationale is that if the matrix is very large,
+  //       processing 8 rows at once might be counter productive wrt cache.
+  const Index n8 = lhs.stride()*sizeof(LhsScalar)>32000 ? 0 : rows-7;
+  const Index n4 = rows-3;
+  const Index n2 = rows-1;
 
-  enum { AllAligned=0, EvenAligned=1, FirstAligned=2, NoneAligned=3 };
-  const Index rowsAtOnce = 4;
-  const Index peels = 2;
-  const Index RhsPacketAlignedMask = RhsPacketSize-1;
-  const Index LhsPacketAlignedMask = LhsPacketSize-1;
-  const Index depth = cols;
-  const Index lhsStride = lhs.stride();
+  // TODO: for padded aligned inputs, we could enable aligned reads
+  enum { LhsAlignment = Unaligned,
+         ResPacketSize = Traits::ResPacketSize,
+         ResPacketSizeHalf = HalfTraits::ResPacketSize,
+         ResPacketSizeQuarter = QuarterTraits::ResPacketSize,
+         LhsPacketSize = Traits::LhsPacketSize,
+         LhsPacketSizeHalf = HalfTraits::LhsPacketSize,
+         LhsPacketSizeQuarter = QuarterTraits::LhsPacketSize,
+         HasHalf = (int)ResPacketSizeHalf < (int)ResPacketSize,
+         HasQuarter = (int)ResPacketSizeQuarter < (int)ResPacketSizeHalf
+  };
 
-  // How many coeffs of the result do we have to skip to be aligned.
-  // Here we assume data are at least aligned on the base scalar type
-  // if that's not the case then vectorization is discarded, see below.
-  Index alignedStart = rhs.firstAligned(depth);
-  Index alignedSize = RhsPacketSize>1 ? alignedStart + ((depth-alignedStart) & ~RhsPacketAlignedMask) : 0;
-  const Index peeledSize = alignedSize - RhsPacketSize*peels - RhsPacketSize + 1;
-
-  const Index alignmentStep = LhsPacketSize>1 ? (LhsPacketSize - lhsStride % LhsPacketSize) & LhsPacketAlignedMask : 0;
-  Index alignmentPattern = alignmentStep==0 ? AllAligned
-                           : alignmentStep==(LhsPacketSize/2) ? EvenAligned
-                           : FirstAligned;
-
-  // we cannot assume the first element is aligned because of sub-matrices
-  const Index lhsAlignmentOffset = lhs.firstAligned(depth);
-  const Index rhsAlignmentOffset = rhs.firstAligned(rows);
-
-  // find how many rows do we have to skip to be aligned with rhs (if possible)
-  Index skipRows = 0;
-  // if the data cannot be aligned (TODO add some compile time tests when possible, e.g. for floats)
-  if( (sizeof(LhsScalar)!=sizeof(RhsScalar)) ||
-      (lhsAlignmentOffset < 0) || (lhsAlignmentOffset == depth) ||
-      (rhsAlignmentOffset < 0) || (rhsAlignmentOffset == rows) )
+  Index i=0;
+  for(; i<n8; i+=8)
   {
-    alignedSize = 0;
-    alignedStart = 0;
-    alignmentPattern = NoneAligned;
-  }
-  else if(LhsPacketSize > 4)
-  {
-    // TODO: extend the code to support aligned loads whenever possible when LhsPacketSize > 4.
-    alignmentPattern = NoneAligned;
-  }
-  else if (LhsPacketSize>1)
-  {
-  //    eigen_internal_assert(size_t(firstLhs+lhsAlignmentOffset)%sizeof(LhsPacket)==0  || depth<LhsPacketSize);
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+              c1 = pset1<ResPacket>(ResScalar(0)),
+              c2 = pset1<ResPacket>(ResScalar(0)),
+              c3 = pset1<ResPacket>(ResScalar(0)),
+              c4 = pset1<ResPacket>(ResScalar(0)),
+              c5 = pset1<ResPacket>(ResScalar(0)),
+              c6 = pset1<ResPacket>(ResScalar(0)),
+              c7 = pset1<ResPacket>(ResScalar(0));
 
-    while (skipRows<LhsPacketSize &&
-           alignedStart != ((lhsAlignmentOffset + alignmentStep*skipRows)%LhsPacketSize))
-      ++skipRows;
-    if (skipRows==LhsPacketSize)
+    Index j=0;
+    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
     {
-      // nothing can be aligned, no need to skip any column
-      alignmentPattern = NoneAligned;
-      skipRows = 0;
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j,0);
+
+      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
+      c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+1,j),b0,c1);
+      c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+2,j),b0,c2);
+      c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+3,j),b0,c3);
+      c4 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+4,j),b0,c4);
+      c5 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+5,j),b0,c5);
+      c6 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+6,j),b0,c6);
+      c7 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+7,j),b0,c7);
     }
-    else
+    ResScalar cc0 = predux(c0);
+    ResScalar cc1 = predux(c1);
+    ResScalar cc2 = predux(c2);
+    ResScalar cc3 = predux(c3);
+    ResScalar cc4 = predux(c4);
+    ResScalar cc5 = predux(c5);
+    ResScalar cc6 = predux(c6);
+    ResScalar cc7 = predux(c7);
+    for(; j<cols; ++j)
     {
-      skipRows = (std::min)(skipRows,Index(rows));
-      // note that the skiped columns are processed later.
+      RhsScalar b0 = rhs(j,0);
+
+      cc0 += cj.pmul(lhs(i+0,j), b0);
+      cc1 += cj.pmul(lhs(i+1,j), b0);
+      cc2 += cj.pmul(lhs(i+2,j), b0);
+      cc3 += cj.pmul(lhs(i+3,j), b0);
+      cc4 += cj.pmul(lhs(i+4,j), b0);
+      cc5 += cj.pmul(lhs(i+5,j), b0);
+      cc6 += cj.pmul(lhs(i+6,j), b0);
+      cc7 += cj.pmul(lhs(i+7,j), b0);
     }
-    /*    eigen_internal_assert(  alignmentPattern==NoneAligned
-                      || LhsPacketSize==1
-                      || (skipRows + rowsAtOnce >= rows)
-                      || LhsPacketSize > depth
-                      || (size_t(firstLhs+alignedStart+lhsStride*skipRows)%sizeof(LhsPacket))==0);*/
+    res[(i+0)*resIncr] += alpha*cc0;
+    res[(i+1)*resIncr] += alpha*cc1;
+    res[(i+2)*resIncr] += alpha*cc2;
+    res[(i+3)*resIncr] += alpha*cc3;
+    res[(i+4)*resIncr] += alpha*cc4;
+    res[(i+5)*resIncr] += alpha*cc5;
+    res[(i+6)*resIncr] += alpha*cc6;
+    res[(i+7)*resIncr] += alpha*cc7;
   }
-  else if(Vectorizable)
+  for(; i<n4; i+=4)
   {
-    alignedStart = 0;
-    alignedSize = depth;
-    alignmentPattern = AllAligned;
-  }
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+              c1 = pset1<ResPacket>(ResScalar(0)),
+              c2 = pset1<ResPacket>(ResScalar(0)),
+              c3 = pset1<ResPacket>(ResScalar(0));
 
-  const Index offset1 = (alignmentPattern==FirstAligned && alignmentStep==1)?3:1;
-  const Index offset3 = (alignmentPattern==FirstAligned && alignmentStep==1)?1:3;
-
-  Index rowBound = ((rows-skipRows)/rowsAtOnce)*rowsAtOnce + skipRows;
-  for (Index i=skipRows; i<rowBound; i+=rowsAtOnce)
-  {
-    // FIXME: what is the purpose of this EIGEN_ALIGN_DEFAULT ??
-    EIGEN_ALIGN_MAX ResScalar tmp0 = ResScalar(0);
-    ResScalar tmp1 = ResScalar(0), tmp2 = ResScalar(0), tmp3 = ResScalar(0);
-
-    // this helps the compiler generating good binary code
-    const LhsScalars lhs0 = lhs.getVectorMapper(i+0, 0),    lhs1 = lhs.getVectorMapper(i+offset1, 0),
-                     lhs2 = lhs.getVectorMapper(i+2, 0),    lhs3 = lhs.getVectorMapper(i+offset3, 0);
-
-    if (Vectorizable)
+    Index j=0;
+    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
     {
-      /* explicit vectorization */
-      ResPacket ptmp0 = pset1<ResPacket>(ResScalar(0)), ptmp1 = pset1<ResPacket>(ResScalar(0)),
-                ptmp2 = pset1<ResPacket>(ResScalar(0)), ptmp3 = pset1<ResPacket>(ResScalar(0));
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j,0);
 
-      // process initial unaligned coeffs
-      // FIXME this loop get vectorized by the compiler !
-      for (Index j=0; j<alignedStart; ++j)
-      {
-        RhsScalar b = rhs(j, 0);
-        tmp0 += cj.pmul(lhs0(j),b); tmp1 += cj.pmul(lhs1(j),b);
-        tmp2 += cj.pmul(lhs2(j),b); tmp3 += cj.pmul(lhs3(j),b);
-      }
+      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
+      c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+1,j),b0,c1);
+      c2 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+2,j),b0,c2);
+      c3 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+3,j),b0,c3);
+    }
+    ResScalar cc0 = predux(c0);
+    ResScalar cc1 = predux(c1);
+    ResScalar cc2 = predux(c2);
+    ResScalar cc3 = predux(c3);
+    for(; j<cols; ++j)
+    {
+      RhsScalar b0 = rhs(j,0);
 
-      if (alignedSize>alignedStart)
-      {
-        switch(alignmentPattern)
+      cc0 += cj.pmul(lhs(i+0,j), b0);
+      cc1 += cj.pmul(lhs(i+1,j), b0);
+      cc2 += cj.pmul(lhs(i+2,j), b0);
+      cc3 += cj.pmul(lhs(i+3,j), b0);
+    }
+    res[(i+0)*resIncr] += alpha*cc0;
+    res[(i+1)*resIncr] += alpha*cc1;
+    res[(i+2)*resIncr] += alpha*cc2;
+    res[(i+3)*resIncr] += alpha*cc3;
+  }
+  for(; i<n2; i+=2)
+  {
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0)),
+              c1 = pset1<ResPacket>(ResScalar(0));
+
+    Index j=0;
+    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
+    {
+      RhsPacket b0 = rhs.template load<RhsPacket, Unaligned>(j,0);
+
+      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+0,j),b0,c0);
+      c1 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i+1,j),b0,c1);
+    }
+    ResScalar cc0 = predux(c0);
+    ResScalar cc1 = predux(c1);
+    for(; j<cols; ++j)
+    {
+      RhsScalar b0 = rhs(j,0);
+
+      cc0 += cj.pmul(lhs(i+0,j), b0);
+      cc1 += cj.pmul(lhs(i+1,j), b0);
+    }
+    res[(i+0)*resIncr] += alpha*cc0;
+    res[(i+1)*resIncr] += alpha*cc1;
+  }
+  for(; i<rows; ++i)
+  {
+    ResPacket c0 = pset1<ResPacket>(ResScalar(0));
+    ResPacketHalf c0_h = pset1<ResPacketHalf>(ResScalar(0));
+    ResPacketQuarter c0_q = pset1<ResPacketQuarter>(ResScalar(0));
+    Index j=0;
+    for(; j+LhsPacketSize<=cols; j+=LhsPacketSize)
+    {
+      RhsPacket b0 = rhs.template load<RhsPacket,Unaligned>(j,0);
+      c0 = pcj.pmadd(lhs.template load<LhsPacket,LhsAlignment>(i,j),b0,c0);
+    }
+    ResScalar cc0 = predux(c0);
+    if (HasHalf) {
+      for(; j+LhsPacketSizeHalf<=cols; j+=LhsPacketSizeHalf)
         {
-          case AllAligned:
-            for (Index j = alignedStart; j<alignedSize; j+=RhsPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Aligned,Aligned,Aligned);
-            break;
-          case EvenAligned:
-            for (Index j = alignedStart; j<alignedSize; j+=RhsPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Aligned,Unaligned,Aligned);
-            break;
-          case FirstAligned:
-          {
-            Index j = alignedStart;
-            if (peels>1)
-            {
-              /* Here we proccess 4 rows with with two peeled iterations to hide
-               * the overhead of unaligned loads. Moreover unaligned loads are handled
-               * using special shift/move operations between the two aligned packets
-               * overlaping the desired unaligned packet. This is *much* more efficient
-               * than basic unaligned loads.
-               */
-              LhsPacket A01, A02, A03, A11, A12, A13;
-              A01 = lhs1.template load<LhsPacket, Aligned>(alignedStart-1);
-              A02 = lhs2.template load<LhsPacket, Aligned>(alignedStart-2);
-              A03 = lhs3.template load<LhsPacket, Aligned>(alignedStart-3);
-
-              for (; j<peeledSize; j+=peels*RhsPacketSize)
-              {
-                RhsPacket b = rhs.getVectorMapper(j, 0).template load<RhsPacket, Aligned>(0);
-                A11 = lhs1.template load<LhsPacket, Aligned>(j-1+LhsPacketSize);  palign<1>(A01,A11);
-                A12 = lhs2.template load<LhsPacket, Aligned>(j-2+LhsPacketSize);  palign<2>(A02,A12);
-                A13 = lhs3.template load<LhsPacket, Aligned>(j-3+LhsPacketSize);  palign<3>(A03,A13);
-
-                ptmp0 = pcj.pmadd(lhs0.template load<LhsPacket, Aligned>(j), b, ptmp0);
-                ptmp1 = pcj.pmadd(A01, b, ptmp1);
-                A01 = lhs1.template load<LhsPacket, Aligned>(j-1+2*LhsPacketSize);  palign<1>(A11,A01);
-                ptmp2 = pcj.pmadd(A02, b, ptmp2);
-                A02 = lhs2.template load<LhsPacket, Aligned>(j-2+2*LhsPacketSize);  palign<2>(A12,A02);
-                ptmp3 = pcj.pmadd(A03, b, ptmp3);
-                A03 = lhs3.template load<LhsPacket, Aligned>(j-3+2*LhsPacketSize);  palign<3>(A13,A03);
-
-                b = rhs.getVectorMapper(j+RhsPacketSize, 0).template load<RhsPacket, Aligned>(0);
-                ptmp0 = pcj.pmadd(lhs0.template load<LhsPacket, Aligned>(j+LhsPacketSize), b, ptmp0);
-                ptmp1 = pcj.pmadd(A11, b, ptmp1);
-                ptmp2 = pcj.pmadd(A12, b, ptmp2);
-                ptmp3 = pcj.pmadd(A13, b, ptmp3);
-              }
-            }
-            for (; j<alignedSize; j+=RhsPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Aligned,Unaligned,Unaligned);
-            break;
-          }
-          default:
-            for (Index j = alignedStart; j<alignedSize; j+=RhsPacketSize)
-              _EIGEN_ACCUMULATE_PACKETS(Unaligned,Unaligned,Unaligned);
-            break;
+          RhsPacketHalf b0 = rhs.template load<RhsPacketHalf,Unaligned>(j,0);
+          c0_h = pcj_half.pmadd(lhs.template load<LhsPacketHalf,LhsAlignment>(i,j),b0,c0_h);
         }
-        tmp0 += predux(ptmp0);
-        tmp1 += predux(ptmp1);
-        tmp2 += predux(ptmp2);
-        tmp3 += predux(ptmp3);
-      }
-    } // end explicit vectorization
-
-    // process remaining coeffs (or all if no explicit vectorization)
-    // FIXME this loop get vectorized by the compiler !
-    for (Index j=alignedSize; j<depth; ++j)
-    {
-      RhsScalar b = rhs(j, 0);
-      tmp0 += cj.pmul(lhs0(j),b); tmp1 += cj.pmul(lhs1(j),b);
-      tmp2 += cj.pmul(lhs2(j),b); tmp3 += cj.pmul(lhs3(j),b);
+      cc0 += predux(c0_h);
     }
-    res[i*resIncr]            += alpha*tmp0;
-    res[(i+offset1)*resIncr]  += alpha*tmp1;
-    res[(i+2)*resIncr]        += alpha*tmp2;
-    res[(i+offset3)*resIncr]  += alpha*tmp3;
+    if (HasQuarter) {
+      for(; j+LhsPacketSizeQuarter<=cols; j+=LhsPacketSizeQuarter)
+        {
+          RhsPacketQuarter b0 = rhs.template load<RhsPacketQuarter,Unaligned>(j,0);
+          c0_q = pcj_quarter.pmadd(lhs.template load<LhsPacketQuarter,LhsAlignment>(i,j),b0,c0_q);
+        }
+      cc0 += predux(c0_q);
+    }
+    for(; j<cols; ++j)
+    {
+      cc0 += cj.pmul(lhs(i,j), rhs(j,0));
+    }
+    res[i*resIncr] += alpha*cc0;
   }
-
-  // process remaining first and last rows (at most columnsAtOnce-1)
-  Index end = rows;
-  Index start = rowBound;
-  do
-  {
-    for (Index i=start; i<end; ++i)
-    {
-      EIGEN_ALIGN_MAX ResScalar tmp0 = ResScalar(0);
-      ResPacket ptmp0 = pset1<ResPacket>(tmp0);
-      const LhsScalars lhs0 = lhs.getVectorMapper(i, 0);
-      // process first unaligned result's coeffs
-      // FIXME this loop get vectorized by the compiler !
-      for (Index j=0; j<alignedStart; ++j)
-        tmp0 += cj.pmul(lhs0(j), rhs(j, 0));
-
-      if (alignedSize>alignedStart)
-      {
-        // process aligned rhs coeffs
-        if (lhs0.template aligned<LhsPacket>(alignedStart))
-          for (Index j = alignedStart;j<alignedSize;j+=RhsPacketSize)
-            ptmp0 = pcj.pmadd(lhs0.template load<LhsPacket, Aligned>(j), rhs.getVectorMapper(j, 0).template load<RhsPacket, Aligned>(0), ptmp0);
-        else
-          for (Index j = alignedStart;j<alignedSize;j+=RhsPacketSize)
-            ptmp0 = pcj.pmadd(lhs0.template load<LhsPacket, Unaligned>(j), rhs.getVectorMapper(j, 0).template load<RhsPacket, Aligned>(0), ptmp0);
-        tmp0 += predux(ptmp0);
-      }
-
-      // process remaining scalars
-      // FIXME this loop get vectorized by the compiler !
-      for (Index j=alignedSize; j<depth; ++j)
-        tmp0 += cj.pmul(lhs0(j), rhs(j, 0));
-      res[i*resIncr] += alpha*tmp0;
-    }
-    if (skipRows)
-    {
-      start = 0;
-      end = skipRows;
-      skipRows = 0;
-    }
-    else
-      break;
-  } while(Vectorizable);
-
-  #undef _EIGEN_ACCUMULATE_PACKETS
 }
 
 } // end namespace internal
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
index c2f084c..8f91879 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/Parallelizer.h
@@ -10,6 +10,10 @@
 #ifndef EIGEN_PARALLELIZER_H
 #define EIGEN_PARALLELIZER_H
 
+#if EIGEN_HAS_CXX11_ATOMIC
+#include <atomic>
+#endif
+
 namespace Eigen {
 
 namespace internal {
@@ -17,7 +21,8 @@
 /** \internal */
 inline void manage_multi_threading(Action action, int* v)
 {
-  static EIGEN_UNUSED int m_maxThreads = -1;
+  static int m_maxThreads = -1;
+  EIGEN_UNUSED_VARIABLE(m_maxThreads)
 
   if(action==SetAction)
   {
@@ -75,8 +80,17 @@
 {
   GemmParallelInfo() : sync(-1), users(0), lhs_start(0), lhs_length(0) {}
 
+  // volatile is not enough on all architectures (see bug 1572)
+  // to guarantee that when thread A says to thread B that it is
+  // done with packing a block, then all writes have been really
+  // carried out... C++11 memory model+atomic guarantees this.
+#if EIGEN_HAS_CXX11_ATOMIC
+  std::atomic<Index> sync;
+  std::atomic<int> users;
+#else
   Index volatile sync;
   int volatile users;
+#endif
 
   Index lhs_start;
   Index lhs_length;
@@ -87,11 +101,14 @@
 {
   // TODO when EIGEN_USE_BLAS is defined,
   // we should still enable OMP for other scalar types
-#if !(defined (EIGEN_HAS_OPENMP)) || defined (EIGEN_USE_BLAS)
+  // Without C++11, we have to disable GEMM's parallelization on
+  // non x86 architectures because there volatile is not enough for our purpose.
+  // See bug 1572.
+#if (! defined(EIGEN_HAS_OPENMP)) || defined(EIGEN_USE_BLAS) || ((!EIGEN_HAS_CXX11_ATOMIC) && !(EIGEN_ARCH_i386_OR_x86_64))
   // FIXME the transpose variable is only needed to properly split
   // the matrix product when multithreading is enabled. This is a temporary
   // fix to support row-major destination matrices. This whole
-  // parallelizer mechanism has to be redisigned anyway.
+  // parallelizer mechanism has to be redesigned anyway.
   EIGEN_UNUSED_VARIABLE(depth);
   EIGEN_UNUSED_VARIABLE(transpose);
   func(0,rows, 0,cols);
@@ -112,12 +129,12 @@
   double work = static_cast<double>(rows) * static_cast<double>(cols) *
       static_cast<double>(depth);
   double kMinTaskSize = 50000;  // FIXME improve this heuristic.
-  pb_max_threads = std::max<Index>(1, std::min<Index>(pb_max_threads, work / kMinTaskSize));
+  pb_max_threads = std::max<Index>(1, std::min<Index>(pb_max_threads, static_cast<Index>( work / kMinTaskSize ) ));
 
   // compute the number of threads we are going to use
   Index threads = std::min<Index>(nbThreads(), pb_max_threads);
 
-  // if multi-threading is explicitely disabled, not useful, or if we already are in a parallel session,
+  // if multi-threading is explicitly disabled, not useful, or if we already are in a parallel session,
   // then abort multi-threading
   // FIXME omp_get_num_threads()>1 only works for openmp, what if the user does not use openmp?
   if((!Condition) || (threads==1) || (omp_get_num_threads()>1))
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
index da6f82a..33ecf10 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
@@ -45,14 +45,23 @@
   }
   void operator()(Scalar* blockA, const Scalar* _lhs, Index lhsStride, Index cols, Index rows)
   {
-    enum { PacketSize = packet_traits<Scalar>::size };
+    typedef typename unpacket_traits<typename packet_traits<Scalar>::type>::half HalfPacket;
+    typedef typename unpacket_traits<typename unpacket_traits<typename packet_traits<Scalar>::type>::half>::half QuarterPacket;
+    enum { PacketSize = packet_traits<Scalar>::size,
+           HalfPacketSize = unpacket_traits<HalfPacket>::size,
+           QuarterPacketSize = unpacket_traits<QuarterPacket>::size,
+           HasHalf = (int)HalfPacketSize < (int)PacketSize,
+           HasQuarter = (int)QuarterPacketSize < (int)HalfPacketSize};
+
     const_blas_data_mapper<Scalar,Index,StorageOrder> lhs(_lhs,lhsStride);
     Index count = 0;
     //Index peeled_mc3 = (rows/Pack1)*Pack1;
     
     const Index peeled_mc3 = Pack1>=3*PacketSize ? (rows/(3*PacketSize))*(3*PacketSize) : 0;
     const Index peeled_mc2 = Pack1>=2*PacketSize ? peeled_mc3+((rows-peeled_mc3)/(2*PacketSize))*(2*PacketSize) : 0;
-    const Index peeled_mc1 = Pack1>=1*PacketSize ? (rows/(1*PacketSize))*(1*PacketSize) : 0;
+    const Index peeled_mc1 = Pack1>=1*PacketSize ? peeled_mc2+((rows-peeled_mc2)/(1*PacketSize))*(1*PacketSize) : 0;
+    const Index peeled_mc_half = Pack1>=HalfPacketSize ? peeled_mc1+((rows-peeled_mc1)/(HalfPacketSize))*(HalfPacketSize) : 0;
+    const Index peeled_mc_quarter = Pack1>=QuarterPacketSize ? peeled_mc_half+((rows-peeled_mc_half)/(QuarterPacketSize))*(QuarterPacketSize) : 0;
     
     if(Pack1>=3*PacketSize)
       for(Index i=0; i<peeled_mc3; i+=3*PacketSize)
@@ -66,8 +75,16 @@
       for(Index i=peeled_mc2; i<peeled_mc1; i+=1*PacketSize)
         pack<1*PacketSize>(blockA, lhs, cols, i, count);
 
+    if(HasHalf && Pack1>=HalfPacketSize)
+      for(Index i=peeled_mc1; i<peeled_mc_half; i+=HalfPacketSize)
+        pack<HalfPacketSize>(blockA, lhs, cols, i, count);
+
+    if(HasQuarter && Pack1>=QuarterPacketSize)
+      for(Index i=peeled_mc_half; i<peeled_mc_quarter; i+=QuarterPacketSize)
+        pack<QuarterPacketSize>(blockA, lhs, cols, i, count);
+
     // do the same with mr==1
-    for(Index i=peeled_mc1; i<rows; i++)
+    for(Index i=peeled_mc_quarter; i<rows; i++)
     {
       for(Index k=0; k<i; k++)
         blockA[count++] = lhs(i, k);                   // normal
@@ -277,20 +294,21 @@
 template <typename Scalar, typename Index,
           int LhsStorageOrder, bool LhsSelfAdjoint, bool ConjugateLhs,
           int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs,
-          int ResStorageOrder>
+          int ResStorageOrder, int ResInnerStride>
 struct product_selfadjoint_matrix;
 
 template <typename Scalar, typename Index,
           int LhsStorageOrder, bool LhsSelfAdjoint, bool ConjugateLhs,
-          int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs>
-struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,LhsSelfAdjoint,ConjugateLhs, RhsStorageOrder,RhsSelfAdjoint,ConjugateRhs,RowMajor>
+          int RhsStorageOrder, bool RhsSelfAdjoint, bool ConjugateRhs,
+          int ResInnerStride>
+struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,LhsSelfAdjoint,ConjugateLhs, RhsStorageOrder,RhsSelfAdjoint,ConjugateRhs,RowMajor,ResInnerStride>
 {
 
   static EIGEN_STRONG_INLINE void run(
     Index rows, Index cols,
     const Scalar* lhs, Index lhsStride,
     const Scalar* rhs, Index rhsStride,
-    Scalar* res,       Index resStride,
+    Scalar* res,       Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
   {
     product_selfadjoint_matrix<Scalar, Index,
@@ -298,33 +316,35 @@
       RhsSelfAdjoint, NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(RhsSelfAdjoint,ConjugateRhs),
       EIGEN_LOGICAL_XOR(LhsSelfAdjoint,LhsStorageOrder==RowMajor) ? ColMajor : RowMajor,
       LhsSelfAdjoint, NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(LhsSelfAdjoint,ConjugateLhs),
-      ColMajor>
-      ::run(cols, rows,  rhs, rhsStride,  lhs, lhsStride,  res, resStride,  alpha, blocking);
+      ColMajor,ResInnerStride>
+      ::run(cols, rows,  rhs, rhsStride,  lhs, lhsStride,  res, resIncr, resStride,  alpha, blocking);
   }
 };
 
 template <typename Scalar, typename Index,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs>
-struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,true,ConjugateLhs, RhsStorageOrder,false,ConjugateRhs,ColMajor>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride>
+struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,true,ConjugateLhs, RhsStorageOrder,false,ConjugateRhs,ColMajor,ResInnerStride>
 {
 
   static EIGEN_DONT_INLINE void run(
     Index rows, Index cols,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resStride,
+    Scalar* res,        Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
 };
 
 template <typename Scalar, typename Index,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs>
-EIGEN_DONT_INLINE void product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,true,ConjugateLhs, RhsStorageOrder,false,ConjugateRhs,ColMajor>::run(
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride>
+EIGEN_DONT_INLINE void product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,true,ConjugateLhs, RhsStorageOrder,false,ConjugateRhs,ColMajor,ResInnerStride>::run(
     Index rows, Index cols,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,        Index resStride,
+    Scalar* _res,       Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
   {
     Index size = rows;
@@ -334,11 +354,11 @@
     typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
     typedef const_blas_data_mapper<Scalar, Index, (LhsStorageOrder == RowMajor) ? ColMajor : RowMajor> LhsTransposeMapper;
     typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor> ResMapper;
+    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
     LhsMapper lhs(_lhs,lhsStride);
     LhsTransposeMapper lhs_transpose(_lhs,lhsStride);
     RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride);
+    ResMapper res(_res, resStride, resIncr);
 
     Index kc = blocking.kc();                   // cache block size along the K direction
     Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
@@ -352,7 +372,7 @@
     gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
     symm_pack_lhs<Scalar, Index, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
     gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder> pack_rhs;
-    gemm_pack_lhs<Scalar, Index, LhsTransposeMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder==RowMajor?ColMajor:RowMajor, true> pack_lhs_transposed;
+    gemm_pack_lhs<Scalar, Index, LhsTransposeMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder==RowMajor?ColMajor:RowMajor, true> pack_lhs_transposed;
 
     for(Index k2=0; k2<size; k2+=kc)
     {
@@ -387,7 +407,7 @@
       for(Index i2=k2+kc; i2<size; i2+=mc)
       {
         const Index actual_mc = (std::min)(i2+mc,size)-i2;
-        gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder,false>()
+        gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder,false>()
           (blockA, lhs.getSubMapper(i2, k2), actual_kc, actual_mc);
 
         gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc, actual_kc, cols, alpha);
@@ -398,26 +418,28 @@
 // matrix * selfadjoint product
 template <typename Scalar, typename Index,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs>
-struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,false,ConjugateLhs, RhsStorageOrder,true,ConjugateRhs,ColMajor>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride>
+struct product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,false,ConjugateLhs, RhsStorageOrder,true,ConjugateRhs,ColMajor,ResInnerStride>
 {
 
   static EIGEN_DONT_INLINE void run(
     Index rows, Index cols,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resStride,
+    Scalar* res,        Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
 };
 
 template <typename Scalar, typename Index,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs>
-EIGEN_DONT_INLINE void product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,false,ConjugateLhs, RhsStorageOrder,true,ConjugateRhs,ColMajor>::run(
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride>
+EIGEN_DONT_INLINE void product_selfadjoint_matrix<Scalar,Index,LhsStorageOrder,false,ConjugateLhs, RhsStorageOrder,true,ConjugateRhs,ColMajor,ResInnerStride>::run(
     Index rows, Index cols,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,        Index resStride,
+    Scalar* _res,       Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
   {
     Index size = cols;
@@ -425,9 +447,9 @@
     typedef gebp_traits<Scalar,Scalar> Traits;
 
     typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor> ResMapper;
+    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
     LhsMapper lhs(_lhs,lhsStride);
-    ResMapper res(_res,resStride);
+    ResMapper res(_res,resStride, resIncr);
 
     Index kc = blocking.kc();                   // cache block size along the K direction
     Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
@@ -437,7 +459,7 @@
     ei_declare_aligned_stack_constructed_variable(Scalar, blockB, sizeB, blocking.blockB());
 
     gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
+    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
     symm_pack_rhs<Scalar, Index, Traits::nr,RhsStorageOrder> pack_rhs;
 
     for(Index k2=0; k2<size; k2+=kc)
@@ -503,12 +525,13 @@
       NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(LhsIsUpper,bool(LhsBlasTraits::NeedToConjugate)),
       EIGEN_LOGICAL_XOR(RhsIsUpper,internal::traits<Rhs>::Flags &RowMajorBit) ? RowMajor : ColMajor, RhsIsSelfAdjoint,
       NumTraits<Scalar>::IsComplex && EIGEN_LOGICAL_XOR(RhsIsUpper,bool(RhsBlasTraits::NeedToConjugate)),
-      internal::traits<Dest>::Flags&RowMajorBit  ? RowMajor : ColMajor>
+      internal::traits<Dest>::Flags&RowMajorBit  ? RowMajor : ColMajor,
+      Dest::InnerStrideAtCompileTime>
       ::run(
         lhs.rows(), rhs.cols(),                 // sizes
         &lhs.coeffRef(0,0), lhs.outerStride(),  // lhs info
         &rhs.coeffRef(0,0), rhs.outerStride(),  // rhs info
-        &dst.coeffRef(0,0), dst.outerStride(),  // result info
+        &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(),  // result info
         actualAlpha, blocking                   // alpha
       );
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
index 3fd180e..d38fd72 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointMatrixVector.h
@@ -15,7 +15,7 @@
 namespace internal {
 
 /* Optimized selfadjoint matrix * vector product:
- * This algorithm processes 2 columns at onces that allows to both reduce
+ * This algorithm processes 2 columns at once that allows to both reduce
  * the number of load/stores of the result by a factor 2 and to reduce
  * the instruction dependency.
  */
@@ -27,7 +27,8 @@
 struct selfadjoint_matrix_vector_product
 
 {
-static EIGEN_DONT_INLINE void run(
+static EIGEN_DONT_INLINE EIGEN_DEVICE_FUNC
+void run(
   Index size,
   const Scalar*  lhs, Index lhsStride,
   const Scalar*  rhs,
@@ -36,7 +37,8 @@
 };
 
 template<typename Scalar, typename Index, int StorageOrder, int UpLo, bool ConjugateLhs, bool ConjugateRhs, int Version>
-EIGEN_DONT_INLINE void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrder,UpLo,ConjugateLhs,ConjugateRhs,Version>::run(
+EIGEN_DONT_INLINE EIGEN_DEVICE_FUNC
+void selfadjoint_matrix_vector_product<Scalar,Index,StorageOrder,UpLo,ConjugateLhs,ConjugateRhs,Version>::run(
   Index size,
   const Scalar*  lhs, Index lhsStride,
   const Scalar*  rhs,
@@ -62,8 +64,7 @@
 
   Scalar cjAlpha = ConjugateRhs ? numext::conj(alpha) : alpha;
 
-
-  Index bound = (std::max)(Index(0),size-8) & 0xfffffffe;
+  Index bound = numext::maxi(Index(0), size-8) & 0xfffffffe;
   if (FirstTriangular)
     bound = size - bound;
 
@@ -175,7 +176,8 @@
   enum { LhsUpLo = LhsMode&(Upper|Lower) };
 
   template<typename Dest>
-  static void run(Dest& dest, const Lhs &a_lhs, const Rhs &a_rhs, const Scalar& alpha)
+  static EIGEN_DEVICE_FUNC
+  void run(Dest& dest, const Lhs &a_lhs, const Rhs &a_rhs, const Scalar& alpha)
   {
     typedef typename Dest::Scalar ResScalar;
     typedef typename Rhs::Scalar RhsScalar;
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
index f038d68..a21be80 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointProduct.h
@@ -109,10 +109,10 @@
     internal::general_matrix_matrix_triangular_product<Index,
       Scalar, OtherIsRowMajor ? RowMajor : ColMajor,   OtherBlasTraits::NeedToConjugate  && NumTraits<Scalar>::IsComplex,
       Scalar, OtherIsRowMajor ? ColMajor : RowMajor, (!OtherBlasTraits::NeedToConjugate) && NumTraits<Scalar>::IsComplex,
-      IsRowMajor ? RowMajor : ColMajor, UpLo>
+      IsRowMajor ? RowMajor : ColMajor, MatrixType::InnerStrideAtCompileTime, UpLo>
       ::run(size, depth,
-            &actualOther.coeffRef(0,0), actualOther.outerStride(), &actualOther.coeffRef(0,0), actualOther.outerStride(),
-            mat.data(), mat.outerStride(), actualAlpha, blocking);
+            actualOther.data(), actualOther.outerStride(), actualOther.data(), actualOther.outerStride(),
+            mat.data(), mat.innerStride(), mat.outerStride(), actualAlpha, blocking);
   }
 };
 
@@ -120,7 +120,7 @@
 
 template<typename MatrixType, unsigned int UpLo>
 template<typename DerivedU>
-SelfAdjointView<MatrixType,UpLo>& SelfAdjointView<MatrixType,UpLo>
+EIGEN_DEVICE_FUNC SelfAdjointView<MatrixType,UpLo>& SelfAdjointView<MatrixType,UpLo>
 ::rankUpdate(const MatrixBase<DerivedU>& u, const Scalar& alpha)
 {
   selfadjoint_product_selector<MatrixType,DerivedU,UpLo>::run(_expression().const_cast_derived(), u.derived(), alpha);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
index 2ae3641..f752a0b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/SelfadjointRank2Update.h
@@ -24,7 +24,8 @@
 template<typename Scalar, typename Index, typename UType, typename VType>
 struct selfadjoint_rank2_update_selector<Scalar,Index,UType,VType,Lower>
 {
-  static void run(Scalar* mat, Index stride, const UType& u, const VType& v, const Scalar& alpha)
+  static EIGEN_DEVICE_FUNC
+  void run(Scalar* mat, Index stride, const UType& u, const VType& v, const Scalar& alpha)
   {
     const Index size = u.size();
     for (Index i=0; i<size; ++i)
@@ -57,7 +58,7 @@
 
 template<typename MatrixType, unsigned int UpLo>
 template<typename DerivedU, typename DerivedV>
-SelfAdjointView<MatrixType,UpLo>& SelfAdjointView<MatrixType,UpLo>
+EIGEN_DEVICE_FUNC SelfAdjointView<MatrixType,UpLo>& SelfAdjointView<MatrixType,UpLo>
 ::rankUpdate(const MatrixBase<DerivedU>& u, const MatrixBase<DerivedV>& v, const Scalar& alpha)
 {
   typedef internal::blas_traits<DerivedU> UBlasTraits;
@@ -79,8 +80,8 @@
   if (IsRowMajor)
     actualAlpha = numext::conj(actualAlpha);
 
-  typedef typename internal::remove_all<typename internal::conj_expr_if<IsRowMajor ^ UBlasTraits::NeedToConjugate,_ActualUType>::type>::type UType;
-  typedef typename internal::remove_all<typename internal::conj_expr_if<IsRowMajor ^ VBlasTraits::NeedToConjugate,_ActualVType>::type>::type VType;
+  typedef typename internal::remove_all<typename internal::conj_expr_if<int(IsRowMajor) ^ int(UBlasTraits::NeedToConjugate), _ActualUType>::type>::type UType;
+  typedef typename internal::remove_all<typename internal::conj_expr_if<int(IsRowMajor) ^ int(VBlasTraits::NeedToConjugate), _ActualVType>::type>::type VType;
   internal::selfadjoint_rank2_update_selector<Scalar, Index, UType, VType,
     (IsRowMajor ? int(UpLo==Upper ? Lower : Upper) : UpLo)>
     ::run(_expression().const_cast_derived().data(),_expression().outerStride(),UType(actualU),VType(actualV),actualAlpha);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
index f784507..f0c6050 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularMatrixMatrix.h
@@ -45,22 +45,24 @@
           int Mode, bool LhsIsTriangular,
           int LhsStorageOrder, bool ConjugateLhs,
           int RhsStorageOrder, bool ConjugateRhs,
-          int ResStorageOrder, int Version = Specialized>
+          int ResStorageOrder, int ResInnerStride,
+          int Version = Specialized>
 struct product_triangular_matrix_matrix;
 
 template <typename Scalar, typename Index,
           int Mode, bool LhsIsTriangular,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs, int Version>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride, int Version>
 struct product_triangular_matrix_matrix<Scalar,Index,Mode,LhsIsTriangular,
                                            LhsStorageOrder,ConjugateLhs,
-                                           RhsStorageOrder,ConjugateRhs,RowMajor,Version>
+                                           RhsStorageOrder,ConjugateRhs,RowMajor,ResInnerStride,Version>
 {
   static EIGEN_STRONG_INLINE void run(
     Index rows, Index cols, Index depth,
     const Scalar* lhs, Index lhsStride,
     const Scalar* rhs, Index rhsStride,
-    Scalar* res,       Index resStride,
+    Scalar* res,       Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
   {
     product_triangular_matrix_matrix<Scalar, Index,
@@ -70,18 +72,19 @@
       ConjugateRhs,
       LhsStorageOrder==RowMajor ? ColMajor : RowMajor,
       ConjugateLhs,
-      ColMajor>
-      ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resStride, alpha, blocking);
+      ColMajor, ResInnerStride>
+      ::run(cols, rows, depth, rhs, rhsStride, lhs, lhsStride, res, resIncr, resStride, alpha, blocking);
   }
 };
 
 // implements col-major += alpha * op(triangular) * op(general)
 template <typename Scalar, typename Index, int Mode,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs, int Version>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride, int Version>
 struct product_triangular_matrix_matrix<Scalar,Index,Mode,true,
                                            LhsStorageOrder,ConjugateLhs,
-                                           RhsStorageOrder,ConjugateRhs,ColMajor,Version>
+                                           RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>
 {
   
   typedef gebp_traits<Scalar,Scalar> Traits;
@@ -95,20 +98,21 @@
     Index _rows, Index _cols, Index _depth,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resStride,
+    Scalar* res,        Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
 };
 
 template <typename Scalar, typename Index, int Mode,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs, int Version>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride, int Version>
 EIGEN_DONT_INLINE void product_triangular_matrix_matrix<Scalar,Index,Mode,true,
                                                         LhsStorageOrder,ConjugateLhs,
-                                                        RhsStorageOrder,ConjugateRhs,ColMajor,Version>::run(
+                                                        RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>::run(
     Index _rows, Index _cols, Index _depth,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,        Index resStride,
+    Scalar* _res,       Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
   {
     // strip zeros
@@ -119,10 +123,10 @@
     
     typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
     typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor> ResMapper;
+    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
     LhsMapper lhs(_lhs,lhsStride);
     RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride);
+    ResMapper res(_res, resStride, resIncr);
 
     Index kc = blocking.kc();                   // cache block size along the K direction
     Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
@@ -151,7 +155,7 @@
       triangularBuffer.diagonal().setOnes();
 
     gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
+    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
     gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder> pack_rhs;
 
     for(Index k2=IsLower ? depth : 0;
@@ -222,7 +226,7 @@
         for(Index i2=start; i2<end; i2+=mc)
         {
           const Index actual_mc = (std::min)(i2+mc,end)-i2;
-          gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr,Traits::LhsProgress, LhsStorageOrder,false>()
+          gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr,Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder,false>()
             (blockA, lhs.getSubMapper(i2, actual_k2), actual_kc, actual_mc);
 
           gebp_kernel(res.getSubMapper(i2, 0), blockA, blockB, actual_mc,
@@ -235,10 +239,11 @@
 // implements col-major += alpha * op(general) * op(triangular)
 template <typename Scalar, typename Index, int Mode,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs, int Version>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride, int Version>
 struct product_triangular_matrix_matrix<Scalar,Index,Mode,false,
                                         LhsStorageOrder,ConjugateLhs,
-                                        RhsStorageOrder,ConjugateRhs,ColMajor,Version>
+                                        RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>
 {
   typedef gebp_traits<Scalar,Scalar> Traits;
   enum {
@@ -251,20 +256,21 @@
     Index _rows, Index _cols, Index _depth,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* res,        Index resStride,
+    Scalar* res,        Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking);
 };
 
 template <typename Scalar, typename Index, int Mode,
           int LhsStorageOrder, bool ConjugateLhs,
-          int RhsStorageOrder, bool ConjugateRhs, int Version>
+          int RhsStorageOrder, bool ConjugateRhs,
+          int ResInnerStride, int Version>
 EIGEN_DONT_INLINE void product_triangular_matrix_matrix<Scalar,Index,Mode,false,
                                                         LhsStorageOrder,ConjugateLhs,
-                                                        RhsStorageOrder,ConjugateRhs,ColMajor,Version>::run(
+                                                        RhsStorageOrder,ConjugateRhs,ColMajor,ResInnerStride,Version>::run(
     Index _rows, Index _cols, Index _depth,
     const Scalar* _lhs, Index lhsStride,
     const Scalar* _rhs, Index rhsStride,
-    Scalar* _res,        Index resStride,
+    Scalar* _res,       Index resIncr, Index resStride,
     const Scalar& alpha, level3_blocking<Scalar,Scalar>& blocking)
   {
     const Index PacketBytes = packet_traits<Scalar>::size*sizeof(Scalar);
@@ -276,10 +282,10 @@
     
     typedef const_blas_data_mapper<Scalar, Index, LhsStorageOrder> LhsMapper;
     typedef const_blas_data_mapper<Scalar, Index, RhsStorageOrder> RhsMapper;
-    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor> ResMapper;
+    typedef blas_data_mapper<typename Traits::ResScalar, Index, ColMajor, Unaligned, ResInnerStride> ResMapper;
     LhsMapper lhs(_lhs,lhsStride);
     RhsMapper rhs(_rhs,rhsStride);
-    ResMapper res(_res, resStride);
+    ResMapper res(_res, resStride, resIncr);
 
     Index kc = blocking.kc();                   // cache block size along the K direction
     Index mc = (std::min)(rows,blocking.mc());  // cache block size along the M direction
@@ -299,7 +305,7 @@
       triangularBuffer.diagonal().setOnes();
 
     gebp_kernel<Scalar, Scalar, Index, ResMapper, Traits::mr, Traits::nr, ConjugateLhs, ConjugateRhs> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, LhsStorageOrder> pack_lhs;
+    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, LhsStorageOrder> pack_lhs;
     gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder> pack_rhs;
     gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr,RhsStorageOrder,false,true> pack_rhs_panel;
 
@@ -433,12 +439,12 @@
       Mode, LhsIsTriangular,
       (internal::traits<ActualLhsTypeCleaned>::Flags&RowMajorBit) ? RowMajor : ColMajor, LhsBlasTraits::NeedToConjugate,
       (internal::traits<ActualRhsTypeCleaned>::Flags&RowMajorBit) ? RowMajor : ColMajor, RhsBlasTraits::NeedToConjugate,
-      (internal::traits<Dest          >::Flags&RowMajorBit) ? RowMajor : ColMajor>
+      (internal::traits<Dest          >::Flags&RowMajorBit) ? RowMajor : ColMajor, Dest::InnerStrideAtCompileTime>
       ::run(
         stripedRows, stripedCols, stripedDepth,   // sizes
         &lhs.coeffRef(0,0), lhs.outerStride(),    // lhs info
         &rhs.coeffRef(0,0), rhs.outerStride(),    // rhs info
-        &dst.coeffRef(0,0), dst.outerStride(),    // result info
+        &dst.coeffRef(0,0), dst.innerStride(), dst.outerStride(),    // result info
         actualAlpha, blocking
       );
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
index 223c38b..6d879ba 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverMatrix.h
@@ -15,48 +15,48 @@
 namespace internal {
 
 // if the rhs is row major, let's transpose the product
-template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder>
-struct triangular_solve_matrix<Scalar,Index,Side,Mode,Conjugate,TriStorageOrder,RowMajor>
+template <typename Scalar, typename Index, int Side, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
+struct triangular_solve_matrix<Scalar,Index,Side,Mode,Conjugate,TriStorageOrder,RowMajor,OtherInnerStride>
 {
   static void run(
     Index size, Index cols,
     const Scalar*  tri, Index triStride,
-    Scalar* _other, Index otherStride,
+    Scalar* _other, Index otherIncr, Index otherStride,
     level3_blocking<Scalar,Scalar>& blocking)
   {
     triangular_solve_matrix<
       Scalar, Index, Side==OnTheLeft?OnTheRight:OnTheLeft,
       (Mode&UnitDiag) | ((Mode&Upper) ? Lower : Upper),
       NumTraits<Scalar>::IsComplex && Conjugate,
-      TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor>
-      ::run(size, cols, tri, triStride, _other, otherStride, blocking);
+      TriStorageOrder==RowMajor ? ColMajor : RowMajor, ColMajor, OtherInnerStride>
+      ::run(size, cols, tri, triStride, _other, otherIncr, otherStride, blocking);
   }
 };
 
 /* Optimized triangular solver with multiple right hand side and the triangular matrix on the left
  */
-template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder>
-struct triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor>
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder,int OtherInnerStride>
+struct triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>
 {
   static EIGEN_DONT_INLINE void run(
     Index size, Index otherSize,
     const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherStride,
+    Scalar* _other, Index otherIncr, Index otherStride,
     level3_blocking<Scalar,Scalar>& blocking);
 };
-template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder>
-EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor>::run(
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
+EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheLeft,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>::run(
     Index size, Index otherSize,
     const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherStride,
+    Scalar* _other, Index otherIncr, Index otherStride,
     level3_blocking<Scalar,Scalar>& blocking)
   {
     Index cols = otherSize;
 
     typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> TriMapper;
-    typedef blas_data_mapper<Scalar, Index, ColMajor> OtherMapper;
+    typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> OtherMapper;
     TriMapper tri(_tri, triStride);
-    OtherMapper other(_other, otherStride);
+    OtherMapper other(_other, otherStride, otherIncr);
 
     typedef gebp_traits<Scalar,Scalar> Traits;
 
@@ -76,7 +76,7 @@
 
     conj_if<Conjugate> conj;
     gebp_kernel<Scalar, Scalar, Index, OtherMapper, Traits::mr, Traits::nr, Conjugate, false> gebp_kernel;
-    gemm_pack_lhs<Scalar, Index, TriMapper, Traits::mr, Traits::LhsProgress, TriStorageOrder> pack_lhs;
+    gemm_pack_lhs<Scalar, Index, TriMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, TriStorageOrder> pack_lhs;
     gemm_pack_rhs<Scalar, Index, OtherMapper, Traits::nr, ColMajor, false, true> pack_rhs;
 
     // the goal here is to subdivise the Rhs panels such that we keep some cache
@@ -128,19 +128,21 @@
               {
                 Scalar b(0);
                 const Scalar* l = &tri(i,s);
-                Scalar* r = &other(s,j);
+                typename OtherMapper::LinearMapper r = other.getLinearMapper(s,j);
                 for (Index i3=0; i3<k; ++i3)
-                  b += conj(l[i3]) * r[i3];
+                  b += conj(l[i3]) * r(i3);
 
                 other(i,j) = (other(i,j) - b)*a;
               }
               else
               {
-                Scalar b = (other(i,j) *= a);
-                Scalar* r = &other(s,j);
-                const Scalar* l = &tri(s,i);
+                Scalar& otherij = other(i,j);
+                otherij *= a;
+                Scalar b = otherij;
+                typename OtherMapper::LinearMapper r = other.getLinearMapper(s,j);
+                typename TriMapper::LinearMapper l = tri.getLinearMapper(s,i);
                 for (Index i3=0;i3<rs;++i3)
-                  r[i3] -= b * conj(l[i3]);
+                  r(i3) -= b * conj(l(i3));
               }
             }
           }
@@ -185,28 +187,28 @@
 
 /* Optimized triangular solver with multiple left hand sides and the triangular matrix on the right
  */
-template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder>
-struct triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor>
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
+struct triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>
 {
   static EIGEN_DONT_INLINE void run(
     Index size, Index otherSize,
     const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherStride,
+    Scalar* _other, Index otherIncr, Index otherStride,
     level3_blocking<Scalar,Scalar>& blocking);
 };
-template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder>
-EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor>::run(
+template <typename Scalar, typename Index, int Mode, bool Conjugate, int TriStorageOrder, int OtherInnerStride>
+EIGEN_DONT_INLINE void triangular_solve_matrix<Scalar,Index,OnTheRight,Mode,Conjugate,TriStorageOrder,ColMajor,OtherInnerStride>::run(
     Index size, Index otherSize,
     const Scalar* _tri, Index triStride,
-    Scalar* _other, Index otherStride,
+    Scalar* _other, Index otherIncr, Index otherStride,
     level3_blocking<Scalar,Scalar>& blocking)
   {
     Index rows = otherSize;
     typedef typename NumTraits<Scalar>::Real RealScalar;
 
-    typedef blas_data_mapper<Scalar, Index, ColMajor> LhsMapper;
+    typedef blas_data_mapper<Scalar, Index, ColMajor, Unaligned, OtherInnerStride> LhsMapper;
     typedef const_blas_data_mapper<Scalar, Index, TriStorageOrder> RhsMapper;
-    LhsMapper lhs(_other, otherStride);
+    LhsMapper lhs(_other, otherStride, otherIncr);
     RhsMapper rhs(_tri, triStride);
 
     typedef gebp_traits<Scalar,Scalar> Traits;
@@ -229,7 +231,7 @@
     gebp_kernel<Scalar, Scalar, Index, LhsMapper, Traits::mr, Traits::nr, false, Conjugate> gebp_kernel;
     gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder> pack_rhs;
     gemm_pack_rhs<Scalar, Index, RhsMapper, Traits::nr, RhsStorageOrder,false,true> pack_rhs_panel;
-    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, ColMajor, false, true> pack_lhs_panel;
+    gemm_pack_lhs<Scalar, Index, LhsMapper, Traits::mr, Traits::LhsProgress, typename Traits::LhsPacket4Packing, ColMajor, false, true> pack_lhs_panel;
 
     for(Index k2=IsLower ? size : 0;
         IsLower ? k2>0 : k2<size;
@@ -297,24 +299,24 @@
             {
               Index j = IsLower ? absolute_j2+actualPanelWidth-k-1 : absolute_j2+k;
 
-              Scalar* r = &lhs(i2,j);
+              typename LhsMapper::LinearMapper r = lhs.getLinearMapper(i2,j);
               for (Index k3=0; k3<k; ++k3)
               {
                 Scalar b = conj(rhs(IsLower ? j+1+k3 : absolute_j2+k3,j));
-                Scalar* a = &lhs(i2,IsLower ? j+1+k3 : absolute_j2+k3);
+                typename LhsMapper::LinearMapper a = lhs.getLinearMapper(i2,IsLower ? j+1+k3 : absolute_j2+k3);
                 for (Index i=0; i<actual_mc; ++i)
-                  r[i] -= a[i] * b;
+                  r(i) -= a(i) * b;
               }
               if((Mode & UnitDiag)==0)
               {
                 Scalar inv_rjj = RealScalar(1)/conj(rhs(j,j));
                 for (Index i=0; i<actual_mc; ++i)
-                  r[i] *= inv_rjj;
+                  r(i) *= inv_rjj;
               }
             }
 
             // pack the just computed part of lhs to A
-            pack_lhs_panel(blockA, LhsMapper(_other+absolute_j2*otherStride+i2, otherStride),
+            pack_lhs_panel(blockA, lhs.getSubMapper(i2,absolute_j2),
                            actualPanelWidth, actual_mc,
                            actual_kc, j2);
           }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
index b994759..6473170 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/products/TriangularSolverVector.h
@@ -58,7 +58,7 @@
       {
         // let's directly call the low level product function because:
         // 1 - it is faster to compile
-        // 2 - it is slighlty faster at runtime
+        // 2 - it is slightly faster at runtime
         Index startRow = IsLower ? pi : pi-actualPanelWidth;
         Index startCol = IsLower ? 0 : pi;
 
@@ -77,7 +77,7 @@
         if (k>0)
           rhs[i] -= (cjLhs.row(i).segment(s,k).transpose().cwiseProduct(Map<const Matrix<RhsScalar,Dynamic,1> >(rhs+s,k))).sum();
 
-        if(!(Mode & UnitDiag))
+        if((!(Mode & UnitDiag)) && numext::not_equal_strict(rhs[i],RhsScalar(0)))
           rhs[i] /= cjLhs(i,i);
       }
     }
@@ -114,20 +114,23 @@
       for(Index k=0; k<actualPanelWidth; ++k)
       {
         Index i = IsLower ? pi+k : pi-k-1;
-        if(!(Mode & UnitDiag))
-          rhs[i] /= cjLhs.coeff(i,i);
+        if(numext::not_equal_strict(rhs[i],RhsScalar(0)))
+        {
+          if(!(Mode & UnitDiag))
+            rhs[i] /= cjLhs.coeff(i,i);
 
-        Index r = actualPanelWidth - k - 1; // remaining size
-        Index s = IsLower ? i+1 : i-r;
-        if (r>0)
-          Map<Matrix<RhsScalar,Dynamic,1> >(rhs+s,r) -= rhs[i] * cjLhs.col(i).segment(s,r);
+          Index r = actualPanelWidth - k - 1; // remaining size
+          Index s = IsLower ? i+1 : i-r;
+          if (r>0)
+            Map<Matrix<RhsScalar,Dynamic,1> >(rhs+s,r) -= rhs[i] * cjLhs.col(i).segment(s,r);
+        }
       }
       Index r = IsLower ? size - endBlock : startBlock; // remaining size
       if (r > 0)
       {
         // let's directly call the low level product function because:
         // 1 - it is faster to compile
-        // 2 - it is slighlty faster at runtime
+        // 2 - it is slightly faster at runtime
         general_matrix_vector_product<Index,LhsScalar,LhsMapper,ColMajor,Conjugate,RhsScalar,RhsMapper,false>::run(
             r, actualPanelWidth,
             LhsMapper(&lhs.coeffRef(endBlock,startBlock), lhsStride),
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
index 6e6ee11..e16a564 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/BlasUtil.h
@@ -24,14 +24,14 @@
 template<typename Scalar, typename Index, typename DataMapper, int nr, int StorageOrder, bool Conjugate = false, bool PanelMode=false>
 struct gemm_pack_rhs;
 
-template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, int StorageOrder, bool Conjugate = false, bool PanelMode = false>
+template<typename Scalar, typename Index, typename DataMapper, int Pack1, int Pack2, typename Packet, int StorageOrder, bool Conjugate = false, bool PanelMode = false>
 struct gemm_pack_lhs;
 
 template<
   typename Index,
   typename LhsScalar, int LhsStorageOrder, bool ConjugateLhs,
   typename RhsScalar, int RhsStorageOrder, bool ConjugateRhs,
-  int ResStorageOrder>
+  int ResStorageOrder, int ResInnerStride>
 struct general_matrix_matrix_product;
 
 template<typename Index,
@@ -39,90 +39,6 @@
          typename RhsScalar, typename RhsMapper, bool ConjugateRhs, int Version=Specialized>
 struct general_matrix_vector_product;
 
-
-template<bool Conjugate> struct conj_if;
-
-template<> struct conj_if<true> {
-  template<typename T>
-  inline T operator()(const T& x) const { return numext::conj(x); }
-  template<typename T>
-  inline T pconj(const T& x) const { return internal::pconj(x); }
-};
-
-template<> struct conj_if<false> {
-  template<typename T>
-  inline const T& operator()(const T& x) const { return x; }
-  template<typename T>
-  inline const T& pconj(const T& x) const { return x; }
-};
-
-// Generic implementation for custom complex types.
-template<typename LhsScalar, typename RhsScalar, bool ConjLhs, bool ConjRhs>
-struct conj_helper
-{
-  typedef typename ScalarBinaryOpTraits<LhsScalar,RhsScalar>::ReturnType Scalar;
-
-  EIGEN_STRONG_INLINE Scalar pmadd(const LhsScalar& x, const RhsScalar& y, const Scalar& c) const
-  { return padd(c, pmul(x,y)); }
-
-  EIGEN_STRONG_INLINE Scalar pmul(const LhsScalar& x, const RhsScalar& y) const
-  { return conj_if<ConjLhs>()(x) *  conj_if<ConjRhs>()(y); }
-};
-
-template<typename Scalar> struct conj_helper<Scalar,Scalar,false,false>
-{
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar pmadd(const Scalar& x, const Scalar& y, const Scalar& c) const { return internal::pmadd(x,y,c); }
-  EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Scalar pmul(const Scalar& x, const Scalar& y) const { return internal::pmul(x,y); }
-};
-
-template<typename RealScalar> struct conj_helper<std::complex<RealScalar>, std::complex<RealScalar>, false,true>
-{
-  typedef std::complex<RealScalar> Scalar;
-  EIGEN_STRONG_INLINE Scalar pmadd(const Scalar& x, const Scalar& y, const Scalar& c) const
-  { return c + pmul(x,y); }
-
-  EIGEN_STRONG_INLINE Scalar pmul(const Scalar& x, const Scalar& y) const
-  { return Scalar(numext::real(x)*numext::real(y) + numext::imag(x)*numext::imag(y), numext::imag(x)*numext::real(y) - numext::real(x)*numext::imag(y)); }
-};
-
-template<typename RealScalar> struct conj_helper<std::complex<RealScalar>, std::complex<RealScalar>, true,false>
-{
-  typedef std::complex<RealScalar> Scalar;
-  EIGEN_STRONG_INLINE Scalar pmadd(const Scalar& x, const Scalar& y, const Scalar& c) const
-  { return c + pmul(x,y); }
-
-  EIGEN_STRONG_INLINE Scalar pmul(const Scalar& x, const Scalar& y) const
-  { return Scalar(numext::real(x)*numext::real(y) + numext::imag(x)*numext::imag(y), numext::real(x)*numext::imag(y) - numext::imag(x)*numext::real(y)); }
-};
-
-template<typename RealScalar> struct conj_helper<std::complex<RealScalar>, std::complex<RealScalar>, true,true>
-{
-  typedef std::complex<RealScalar> Scalar;
-  EIGEN_STRONG_INLINE Scalar pmadd(const Scalar& x, const Scalar& y, const Scalar& c) const
-  { return c + pmul(x,y); }
-
-  EIGEN_STRONG_INLINE Scalar pmul(const Scalar& x, const Scalar& y) const
-  { return Scalar(numext::real(x)*numext::real(y) - numext::imag(x)*numext::imag(y), - numext::real(x)*numext::imag(y) - numext::imag(x)*numext::real(y)); }
-};
-
-template<typename RealScalar,bool Conj> struct conj_helper<std::complex<RealScalar>, RealScalar, Conj,false>
-{
-  typedef std::complex<RealScalar> Scalar;
-  EIGEN_STRONG_INLINE Scalar pmadd(const Scalar& x, const RealScalar& y, const Scalar& c) const
-  { return padd(c, pmul(x,y)); }
-  EIGEN_STRONG_INLINE Scalar pmul(const Scalar& x, const RealScalar& y) const
-  { return conj_if<Conj>()(x)*y; }
-};
-
-template<typename RealScalar,bool Conj> struct conj_helper<RealScalar, std::complex<RealScalar>, false,Conj>
-{
-  typedef std::complex<RealScalar> Scalar;
-  EIGEN_STRONG_INLINE Scalar pmadd(const RealScalar& x, const Scalar& y, const Scalar& c) const
-  { return padd(c, pmul(x,y)); }
-  EIGEN_STRONG_INLINE Scalar pmul(const RealScalar& x, const Scalar& y) const
-  { return x*conj_if<Conj>()(y); }
-};
-
 template<typename From,typename To> struct get_factor {
   EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE To run(const From& x) { return To(x); }
 };
@@ -155,13 +71,19 @@
   Scalar* m_data;
 };
 
-template<typename Scalar, typename Index, int AlignmentType>
-class BlasLinearMapper {
-  public:
-  typedef typename packet_traits<Scalar>::type Packet;
-  typedef typename packet_traits<Scalar>::half HalfPacket;
+template<typename Scalar, typename Index, int AlignmentType, int Incr=1>
+class BlasLinearMapper;
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data) : m_data(data) {}
+template<typename Scalar, typename Index, int AlignmentType>
+class BlasLinearMapper<Scalar,Index,AlignmentType>
+{
+public:
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data, Index incr=1)
+    : m_data(data)
+  {
+    EIGEN_ONLY_USED_FOR_DEBUG(incr);
+    eigen_assert(incr==1);
+  }
 
   EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const {
     internal::prefetch(&operator()(i));
@@ -171,33 +93,86 @@
     return m_data[i];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i) const {
-    return ploadt<Packet, AlignmentType>(m_data + i);
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i) const {
+    return ploadt<PacketType, AlignmentType>(m_data + i);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE HalfPacket loadHalfPacket(Index i) const {
-    return ploadt<HalfPacket, AlignmentType>(m_data + i);
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType &p) const {
+    pstoret<Scalar, PacketType, AlignmentType>(m_data + i, p);
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const Packet &p) const {
-    pstoret<Scalar, Packet, AlignmentType>(m_data + i, p);
-  }
-
-  protected:
+protected:
   Scalar *m_data;
 };
 
 // Lightweight helper class to access matrix coefficients.
-template<typename Scalar, typename Index, int StorageOrder, int AlignmentType = Unaligned>
-class blas_data_mapper {
-  public:
-  typedef typename packet_traits<Scalar>::type Packet;
-  typedef typename packet_traits<Scalar>::half HalfPacket;
+template<typename Scalar, typename Index, int StorageOrder, int AlignmentType = Unaligned, int Incr = 1>
+class blas_data_mapper;
 
+// TMP to help PacketBlock store implementation.
+// There's currently no known use case for PacketBlock load.
+// The default implementation assumes ColMajor order.
+// It always store each packet sequentially one `stride` apart.
+template<typename Index, typename Scalar, typename Packet, int n, int idx, int StorageOrder>
+struct PacketBlockManagement
+{
+  PacketBlockManagement<Index, Scalar, Packet, n, idx - 1, StorageOrder> pbm;
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+    pbm.store(to, stride, i, j, block);
+    pstoreu<Scalar>(to + i + (j + idx)*stride, block.packet[idx]);
+  }
+};
+
+// PacketBlockManagement specialization to take care of RowMajor order without ifs.
+template<typename Index, typename Scalar, typename Packet, int n, int idx>
+struct PacketBlockManagement<Index, Scalar, Packet, n, idx, RowMajor>
+{
+  PacketBlockManagement<Index, Scalar, Packet, n, idx - 1, RowMajor> pbm;
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+    pbm.store(to, stride, i, j, block);
+    pstoreu<Scalar>(to + j + (i + idx)*stride, block.packet[idx]);
+  }
+};
+
+template<typename Index, typename Scalar, typename Packet, int n, int StorageOrder>
+struct PacketBlockManagement<Index, Scalar, Packet, n, -1, StorageOrder>
+{
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+    EIGEN_UNUSED_VARIABLE(to);
+    EIGEN_UNUSED_VARIABLE(stride);
+    EIGEN_UNUSED_VARIABLE(i);
+    EIGEN_UNUSED_VARIABLE(j);
+    EIGEN_UNUSED_VARIABLE(block);
+  }
+};
+
+template<typename Index, typename Scalar, typename Packet, int n>
+struct PacketBlockManagement<Index, Scalar, Packet, n, -1, RowMajor>
+{
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(Scalar *to, const Index stride, Index i, Index j, const PacketBlock<Packet, n> &block) const {
+    EIGEN_UNUSED_VARIABLE(to);
+    EIGEN_UNUSED_VARIABLE(stride);
+    EIGEN_UNUSED_VARIABLE(i);
+    EIGEN_UNUSED_VARIABLE(j);
+    EIGEN_UNUSED_VARIABLE(block);
+  }
+};
+
+template<typename Scalar, typename Index, int StorageOrder, int AlignmentType>
+class blas_data_mapper<Scalar,Index,StorageOrder,AlignmentType,1>
+{
+public:
   typedef BlasLinearMapper<Scalar, Index, AlignmentType> LinearMapper;
   typedef BlasVectorMapper<Scalar, Index> VectorMapper;
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride) : m_data(data), m_stride(stride) {}
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr=1)
+   : m_data(data), m_stride(stride)
+  {
+    EIGEN_ONLY_USED_FOR_DEBUG(incr);
+    eigen_assert(incr==1);
+  }
 
   EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType>
   getSubMapper(Index i, Index j) const {
@@ -218,12 +193,14 @@
     return m_data[StorageOrder==RowMajor ? j + i*m_stride : i + j*m_stride];
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Packet loadPacket(Index i, Index j) const {
-    return ploadt<Packet, AlignmentType>(&operator()(i, j));
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i, Index j) const {
+    return ploadt<PacketType, AlignmentType>(&operator()(i, j));
   }
 
-  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE HalfPacket loadHalfPacket(Index i, Index j) const {
-    return ploadt<HalfPacket, AlignmentType>(&operator()(i, j));
+  template <typename PacketT, int AlignmentT>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i, Index j) const {
+    return ploadt<PacketT, AlignmentT>(&operator()(i, j));
   }
 
   template<typename SubPacket>
@@ -246,11 +223,167 @@
     return internal::first_default_aligned(m_data, size);
   }
 
-  protected:
+  template<typename SubPacket, int n>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketBlock(Index i, Index j, const PacketBlock<SubPacket, n> &block) const {
+    PacketBlockManagement<Index, Scalar, SubPacket, n, n-1, StorageOrder> pbm;
+    pbm.store(m_data, m_stride, i, j, block);
+  }
+protected:
   Scalar* EIGEN_RESTRICT m_data;
   const Index m_stride;
 };
 
+// Implementation of non-natural increment (i.e. inner-stride != 1)
+// The exposed API is not complete yet compared to the Incr==1 case
+// because some features makes less sense in this case.
+template<typename Scalar, typename Index, int AlignmentType, int Incr>
+class BlasLinearMapper
+{
+public:
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE BlasLinearMapper(Scalar *data,Index incr) : m_data(data), m_incr(incr) {}
+
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void prefetch(int i) const {
+    internal::prefetch(&operator()(i));
+  }
+
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE Scalar& operator()(Index i) const {
+    return m_data[i*m_incr.value()];
+  }
+
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i) const {
+    return pgather<Scalar,PacketType>(m_data + i*m_incr.value(), m_incr.value());
+  }
+
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacket(Index i, const PacketType &p) const {
+    pscatter<Scalar, PacketType>(m_data + i*m_incr.value(), p, m_incr.value());
+  }
+
+protected:
+  Scalar *m_data;
+  const internal::variable_if_dynamic<Index,Incr> m_incr;
+};
+
+template<typename Scalar, typename Index, int StorageOrder, int AlignmentType,int Incr>
+class blas_data_mapper
+{
+public:
+  typedef BlasLinearMapper<Scalar, Index, AlignmentType,Incr> LinearMapper;
+
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE blas_data_mapper(Scalar* data, Index stride, Index incr) : m_data(data), m_stride(stride), m_incr(incr) {}
+
+  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE blas_data_mapper
+  getSubMapper(Index i, Index j) const {
+    return blas_data_mapper(&operator()(i, j), m_stride, m_incr.value());
+  }
+
+  EIGEN_DEVICE_FUNC  EIGEN_ALWAYS_INLINE LinearMapper getLinearMapper(Index i, Index j) const {
+    return LinearMapper(&operator()(i, j), m_incr.value());
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_ALWAYS_INLINE Scalar& operator()(Index i, Index j) const {
+    return m_data[StorageOrder==RowMajor ? j*m_incr.value() + i*m_stride : i*m_incr.value() + j*m_stride];
+  }
+
+  template<typename PacketType>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketType loadPacket(Index i, Index j) const {
+    return pgather<Scalar,PacketType>(&operator()(i, j),m_incr.value());
+  }
+
+  template <typename PacketT, int AlignmentT>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE PacketT load(Index i, Index j) const {
+    return pgather<Scalar,PacketT>(&operator()(i, j),m_incr.value());
+  }
+
+  template<typename SubPacket>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void scatterPacket(Index i, Index j, const SubPacket &p) const {
+    pscatter<Scalar, SubPacket>(&operator()(i, j), p, m_stride);
+  }
+
+  template<typename SubPacket>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE SubPacket gatherPacket(Index i, Index j) const {
+    return pgather<Scalar, SubPacket>(&operator()(i, j), m_stride);
+  }
+
+  // storePacketBlock_helper defines a way to access values inside the PacketBlock, this is essentially required by the Complex types.
+  template<typename SubPacket, typename ScalarT, int n, int idx>
+  struct storePacketBlock_helper
+  {
+    storePacketBlock_helper<SubPacket, ScalarT, n, idx-1> spbh;
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j, const PacketBlock<SubPacket, n>& block) const {
+      spbh.store(sup, i,j,block);
+      for(int l = 0; l < unpacket_traits<SubPacket>::size; l++)
+      {
+        ScalarT *v = &sup->operator()(i+l, j+idx);
+        *v = block.packet[idx][l];
+      }
+    }
+  };
+
+  template<typename SubPacket, int n, int idx>
+  struct storePacketBlock_helper<SubPacket, std::complex<float>, n, idx>
+  {
+    storePacketBlock_helper<SubPacket, std::complex<float>, n, idx-1> spbh;
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j, const PacketBlock<SubPacket, n>& block) const {
+      spbh.store(sup,i,j,block);
+      for(int l = 0; l < unpacket_traits<SubPacket>::size; l++)
+      {
+        std::complex<float> *v = &sup->operator()(i+l, j+idx);
+        v->real(block.packet[idx].v[2*l+0]);
+        v->imag(block.packet[idx].v[2*l+1]);
+      }
+    }
+  };
+
+  template<typename SubPacket, int n, int idx>
+  struct storePacketBlock_helper<SubPacket, std::complex<double>, n, idx>
+  {
+    storePacketBlock_helper<SubPacket, std::complex<double>, n, idx-1> spbh;
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>* sup, Index i, Index j, const PacketBlock<SubPacket, n>& block) const {
+      spbh.store(sup,i,j,block);
+      for(int l = 0; l < unpacket_traits<SubPacket>::size; l++)
+      {
+        std::complex<double> *v = &sup->operator()(i+l, j+idx);
+        v->real(block.packet[idx].v[2*l+0]);
+        v->imag(block.packet[idx].v[2*l+1]);
+      }
+    }
+  };
+
+  template<typename SubPacket, typename ScalarT, int n>
+  struct storePacketBlock_helper<SubPacket, ScalarT, n, -1>
+  {
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index, const PacketBlock<SubPacket, n>& ) const {
+    }
+  };
+
+  template<typename SubPacket, int n>
+  struct storePacketBlock_helper<SubPacket, std::complex<float>, n, -1>
+  {
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index, const PacketBlock<SubPacket, n>& ) const {
+    }
+  };
+
+  template<typename SubPacket, int n>
+  struct storePacketBlock_helper<SubPacket, std::complex<double>, n, -1>
+  {
+    EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void store(const blas_data_mapper<Scalar, Index, StorageOrder, AlignmentType, Incr>*, Index, Index, const PacketBlock<SubPacket, n>& ) const {
+    }
+  };
+  // This function stores a PacketBlock on m_data, this approach is really quite slow compare to Incr=1 and should be avoided when possible.
+  template<typename SubPacket, int n>
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE void storePacketBlock(Index i, Index j, const PacketBlock<SubPacket, n>&block) const {
+    storePacketBlock_helper<SubPacket, Scalar, n, n-1> spb;
+    spb.store(this, i,j,block);
+  }
+protected:
+  Scalar* EIGEN_RESTRICT m_data;
+  const Index m_stride;
+  const internal::variable_if_dynamic<Index,Incr> m_incr;
+};
+
 // lightweight helper class to access matrix coefficients (const version)
 template<typename Scalar, typename Index, int StorageOrder>
 class const_blas_data_mapper : public blas_data_mapper<const Scalar, Index, StorageOrder> {
@@ -278,14 +411,15 @@
     HasUsableDirectAccess = (    (int(XprType::Flags)&DirectAccessBit)
                               && (   bool(XprType::IsVectorAtCompileTime)
                                   || int(inner_stride_at_compile_time<XprType>::ret) == 1)
-                             ) ?  1 : 0
+                             ) ?  1 : 0,
+    HasScalarFactor = false
   };
   typedef typename conditional<bool(HasUsableDirectAccess),
     ExtractType,
     typename _ExtractType::PlainObject
     >::type DirectLinearAccessType;
-  static inline ExtractType extract(const XprType& x) { return x; }
-  static inline const Scalar extractScalarFactor(const XprType&) { return Scalar(1); }
+  static inline EIGEN_DEVICE_FUNC ExtractType extract(const XprType& x) { return x; }
+  static inline EIGEN_DEVICE_FUNC const Scalar extractScalarFactor(const XprType&) { return Scalar(1); }
 };
 
 // pop conjugate
@@ -310,17 +444,23 @@
 struct blas_traits<CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain>, NestedXpr> >
  : blas_traits<NestedXpr>
 {
+  enum {
+    HasScalarFactor = true
+  };
   typedef blas_traits<NestedXpr> Base;
   typedef CwiseBinaryOp<scalar_product_op<Scalar>, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain>, NestedXpr> XprType;
   typedef typename Base::ExtractType ExtractType;
-  static inline ExtractType extract(const XprType& x) { return Base::extract(x.rhs()); }
-  static inline Scalar extractScalarFactor(const XprType& x)
+  static inline EIGEN_DEVICE_FUNC ExtractType extract(const XprType& x) { return Base::extract(x.rhs()); }
+  static inline EIGEN_DEVICE_FUNC Scalar extractScalarFactor(const XprType& x)
   { return x.lhs().functor().m_other * Base::extractScalarFactor(x.rhs()); }
 };
 template<typename Scalar, typename NestedXpr, typename Plain>
 struct blas_traits<CwiseBinaryOp<scalar_product_op<Scalar>, NestedXpr, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain> > >
  : blas_traits<NestedXpr>
 {
+  enum {
+    HasScalarFactor = true
+  };
   typedef blas_traits<NestedXpr> Base;
   typedef CwiseBinaryOp<scalar_product_op<Scalar>, NestedXpr, const CwiseNullaryOp<scalar_constant_op<Scalar>,Plain> > XprType;
   typedef typename Base::ExtractType ExtractType;
@@ -339,6 +479,9 @@
 struct blas_traits<CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> >
  : blas_traits<NestedXpr>
 {
+  enum {
+    HasScalarFactor = true
+  };
   typedef blas_traits<NestedXpr> Base;
   typedef CwiseUnaryOp<scalar_opposite_op<Scalar>, NestedXpr> XprType;
   typedef typename Base::ExtractType ExtractType;
@@ -375,7 +518,7 @@
 
 template<typename T, bool HasUsableDirectAccess=blas_traits<T>::HasUsableDirectAccess>
 struct extract_data_selector {
-  static const typename T::Scalar* run(const T& m)
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static const typename T::Scalar* run(const T& m)
   {
     return blas_traits<T>::extract(m).data();
   }
@@ -386,11 +529,53 @@
   static typename T::Scalar* run(const T&) { return 0; }
 };
 
-template<typename T> const typename T::Scalar* extract_data(const T& m)
+template<typename T>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE const typename T::Scalar* extract_data(const T& m)
 {
   return extract_data_selector<T>::run(m);
 }
 
+/**
+ * \c combine_scalar_factors extracts and multiplies factors from GEMM and GEMV products.
+ * There is a specialization for booleans
+ */
+template<typename ResScalar, typename Lhs, typename Rhs>
+struct combine_scalar_factors_impl
+{
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static ResScalar run(const Lhs& lhs, const Rhs& rhs)
+  {
+    return blas_traits<Lhs>::extractScalarFactor(lhs) * blas_traits<Rhs>::extractScalarFactor(rhs);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static ResScalar run(const ResScalar& alpha, const Lhs& lhs, const Rhs& rhs)
+  {
+    return alpha * blas_traits<Lhs>::extractScalarFactor(lhs) * blas_traits<Rhs>::extractScalarFactor(rhs);
+  }
+};
+template<typename Lhs, typename Rhs>
+struct combine_scalar_factors_impl<bool, Lhs, Rhs>
+{
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static bool run(const Lhs& lhs, const Rhs& rhs)
+  {
+    return blas_traits<Lhs>::extractScalarFactor(lhs) && blas_traits<Rhs>::extractScalarFactor(rhs);
+  }
+  EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE static bool run(const bool& alpha, const Lhs& lhs, const Rhs& rhs)
+  {
+    return alpha && blas_traits<Lhs>::extractScalarFactor(lhs) && blas_traits<Rhs>::extractScalarFactor(rhs);
+  }
+};
+
+template<typename ResScalar, typename Lhs, typename Rhs>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE ResScalar combine_scalar_factors(const ResScalar& alpha, const Lhs& lhs, const Rhs& rhs)
+{
+  return combine_scalar_factors_impl<ResScalar,Lhs,Rhs>::run(alpha, lhs, rhs);
+}
+template<typename ResScalar, typename Lhs, typename Rhs>
+EIGEN_DEVICE_FUNC EIGEN_ALWAYS_INLINE ResScalar combine_scalar_factors(const Lhs& lhs, const Rhs& rhs)
+{
+  return combine_scalar_factors_impl<ResScalar,Lhs,Rhs>::run(lhs, rhs);
+}
+
+
 } // end namespace internal
 
 } // end namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h
new file mode 100644
index 0000000..af4e696
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ConfigureVectorization.h
@@ -0,0 +1,512 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2018 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2020, Arm Limited and Contributors
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_CONFIGURE_VECTORIZATION_H
+#define EIGEN_CONFIGURE_VECTORIZATION_H
+
+//------------------------------------------------------------------------------------------
+// Static and dynamic alignment control
+//
+// The main purpose of this section is to define EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES
+// as the maximal boundary in bytes on which dynamically and statically allocated data may be alignment respectively.
+// The values of EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES can be specified by the user. If not,
+// a default value is automatically computed based on architecture, compiler, and OS.
+//
+// This section also defines macros EIGEN_ALIGN_TO_BOUNDARY(N) and the shortcuts EIGEN_ALIGN{8,16,32,_MAX}
+// to be used to declare statically aligned buffers.
+//------------------------------------------------------------------------------------------
+
+
+/* EIGEN_ALIGN_TO_BOUNDARY(n) forces data to be n-byte aligned. This is used to satisfy SIMD requirements.
+ * However, we do that EVEN if vectorization (EIGEN_VECTORIZE) is disabled,
+ * so that vectorization doesn't affect binary compatibility.
+ *
+ * If we made alignment depend on whether or not EIGEN_VECTORIZE is defined, it would be impossible to link
+ * vectorized and non-vectorized code.
+ * 
+ * FIXME: this code can be cleaned up once we switch to proper C++11 only.
+ */
+#if (defined EIGEN_CUDACC)
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n)
+  #define EIGEN_ALIGNOF(x) __alignof(x)
+#elif EIGEN_HAS_ALIGNAS
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) alignas(n)
+  #define EIGEN_ALIGNOF(x) alignof(x)
+#elif EIGEN_COMP_GNUC || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
+  #define EIGEN_ALIGNOF(x) __alignof(x)
+#elif EIGEN_COMP_MSVC
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __declspec(align(n))
+  #define EIGEN_ALIGNOF(x) __alignof(x)
+#elif EIGEN_COMP_SUNCC
+  // FIXME not sure about this one:
+  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
+  #define EIGEN_ALIGNOF(x) __alignof(x)
+#else
+  #error Please tell me what is the equivalent of alignas(n) and alignof(x) for your compiler
+#endif
+
+// If the user explicitly disable vectorization, then we also disable alignment
+#if defined(EIGEN_DONT_VECTORIZE)
+  #if defined(EIGEN_GPUCC)
+    // GPU code is always vectorized and requires memory alignment for
+    // statically allocated buffers.
+    #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
+  #else
+    #define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
+  #endif
+#elif defined(__AVX512F__)
+  // 64 bytes static alignment is preferred only if really required
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
+#elif defined(__AVX__)
+  // 32 bytes static alignment is preferred only if really required
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
+#else
+  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
+#endif
+
+
+// EIGEN_MIN_ALIGN_BYTES defines the minimal value for which the notion of explicit alignment makes sense
+#define EIGEN_MIN_ALIGN_BYTES 16
+
+// Defined the boundary (in bytes) on which the data needs to be aligned. Note
+// that unless EIGEN_ALIGN is defined and not equal to 0, the data may not be
+// aligned at all regardless of the value of this #define.
+
+#if (defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN))  && defined(EIGEN_MAX_STATIC_ALIGN_BYTES) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
+#error EIGEN_MAX_STATIC_ALIGN_BYTES and EIGEN_DONT_ALIGN[_STATICALLY] are both defined with EIGEN_MAX_STATIC_ALIGN_BYTES!=0. Use EIGEN_MAX_STATIC_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN_STATICALLY.
+#endif
+
+// EIGEN_DONT_ALIGN_STATICALLY and EIGEN_DONT_ALIGN are deprecated
+// They imply EIGEN_MAX_STATIC_ALIGN_BYTES=0
+#if defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN)
+  #ifdef EIGEN_MAX_STATIC_ALIGN_BYTES
+    #undef EIGEN_MAX_STATIC_ALIGN_BYTES
+  #endif
+  #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
+#endif
+
+#ifndef EIGEN_MAX_STATIC_ALIGN_BYTES
+
+  // Try to automatically guess what is the best default value for EIGEN_MAX_STATIC_ALIGN_BYTES
+
+  // 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
+  // 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
+  // enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
+  // certain common platform (compiler+architecture combinations) to avoid these problems.
+  // Only static alignment is really problematic (relies on nonstandard compiler extensions),
+  // try to keep heap alignment even when we have to disable static alignment.
+  #if EIGEN_COMP_GNUC && !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64 || EIGEN_ARCH_MIPS)
+  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
+  #elif EIGEN_ARCH_ARM_OR_ARM64 && EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_MOST(4, 6)
+  // Old versions of GCC on ARM, at least 4.4, were once seen to have buggy static alignment support.
+  // Not sure which version fixed it, hopefully it doesn't affect 4.7, which is still somewhat in use.
+  // 4.8 and newer seem definitely unaffected.
+  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
+  #else
+  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0
+  #endif
+
+  // static alignment is completely disabled with GCC 3, Sun Studio, and QCC/QNX
+  #if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT \
+  && !EIGEN_GCC3_OR_OLDER \
+  && !EIGEN_COMP_SUNCC \
+  && !EIGEN_OS_QNX
+    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 1
+  #else
+    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 0
+  #endif
+
+  #if EIGEN_ARCH_WANTS_STACK_ALIGNMENT
+    #define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+  #else
+    #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
+  #endif
+
+#endif
+
+// If EIGEN_MAX_ALIGN_BYTES is defined, then it is considered as an upper bound for EIGEN_MAX_STATIC_ALIGN_BYTES
+#if defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES<EIGEN_MAX_STATIC_ALIGN_BYTES
+#undef EIGEN_MAX_STATIC_ALIGN_BYTES
+#define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
+#endif
+
+#if EIGEN_MAX_STATIC_ALIGN_BYTES==0 && !defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
+  #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
+#endif
+
+// At this stage, EIGEN_MAX_STATIC_ALIGN_BYTES>0 is the true test whether we want to align arrays on the stack or not.
+// It takes into account both the user choice to explicitly enable/disable alignment (by setting EIGEN_MAX_STATIC_ALIGN_BYTES)
+// and the architecture config (EIGEN_ARCH_WANTS_STACK_ALIGNMENT).
+// Henceforth, only EIGEN_MAX_STATIC_ALIGN_BYTES should be used.
+
+
+// Shortcuts to EIGEN_ALIGN_TO_BOUNDARY
+#define EIGEN_ALIGN8  EIGEN_ALIGN_TO_BOUNDARY(8)
+#define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16)
+#define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32)
+#define EIGEN_ALIGN64 EIGEN_ALIGN_TO_BOUNDARY(64)
+#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
+#define EIGEN_ALIGN_MAX EIGEN_ALIGN_TO_BOUNDARY(EIGEN_MAX_STATIC_ALIGN_BYTES)
+#else
+#define EIGEN_ALIGN_MAX
+#endif
+
+
+// Dynamic alignment control
+
+#if defined(EIGEN_DONT_ALIGN) && defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES>0
+#error EIGEN_MAX_ALIGN_BYTES and EIGEN_DONT_ALIGN are both defined with EIGEN_MAX_ALIGN_BYTES!=0. Use EIGEN_MAX_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN.
+#endif
+
+#ifdef EIGEN_DONT_ALIGN
+  #ifdef EIGEN_MAX_ALIGN_BYTES
+    #undef EIGEN_MAX_ALIGN_BYTES
+  #endif
+  #define EIGEN_MAX_ALIGN_BYTES 0
+#elif !defined(EIGEN_MAX_ALIGN_BYTES)
+  #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+#endif
+
+#if EIGEN_IDEAL_MAX_ALIGN_BYTES > EIGEN_MAX_ALIGN_BYTES
+#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
+#else
+#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
+#endif
+
+
+#ifndef EIGEN_UNALIGNED_VECTORIZE
+#define EIGEN_UNALIGNED_VECTORIZE 1
+#endif
+
+//----------------------------------------------------------------------
+
+// if alignment is disabled, then disable vectorization. Note: EIGEN_MAX_ALIGN_BYTES is the proper check, it takes into
+// account both the user's will (EIGEN_MAX_ALIGN_BYTES,EIGEN_DONT_ALIGN) and our own platform checks
+#if EIGEN_MAX_ALIGN_BYTES==0
+  #ifndef EIGEN_DONT_VECTORIZE
+    #define EIGEN_DONT_VECTORIZE
+  #endif
+#endif
+
+
+// The following (except #include <malloc.h> and _M_IX86_FP ??) can likely be
+// removed as gcc 4.1 and msvc 2008 are not supported anyways.
+#if EIGEN_COMP_MSVC
+  #include <malloc.h> // for _aligned_malloc -- need it regardless of whether vectorization is enabled
+  #if (EIGEN_COMP_MSVC >= 1500) // 2008 or later
+    // a user reported that in 64-bit mode, MSVC doesn't care to define _M_IX86_FP.
+    #if (defined(_M_IX86_FP) && (_M_IX86_FP >= 2)) || EIGEN_ARCH_x86_64
+      #define EIGEN_SSE2_ON_MSVC_2008_OR_LATER
+    #endif
+  #endif
+#else
+  #if (defined __SSE2__) && ( (!EIGEN_COMP_GNUC) || EIGEN_COMP_ICC || EIGEN_GNUC_AT_LEAST(4,2) )
+    #define EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC
+  #endif
+#endif
+
+#if !(defined(EIGEN_DONT_VECTORIZE) || defined(EIGEN_GPUCC))
+
+  #if defined (EIGEN_SSE2_ON_NON_MSVC_BUT_NOT_OLD_GCC) || defined(EIGEN_SSE2_ON_MSVC_2008_OR_LATER)
+
+    // Defines symbols for compile-time detection of which instructions are
+    // used.
+    // EIGEN_VECTORIZE_YY is defined if and only if the instruction set YY is used
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_SSE
+    #define EIGEN_VECTORIZE_SSE2
+
+    // Detect sse3/ssse3/sse4:
+    // gcc and icc defines __SSE3__, ...
+    // there is no way to know about this on msvc. You can define EIGEN_VECTORIZE_SSE* if you
+    // want to force the use of those instructions with msvc.
+    #ifdef __SSE3__
+      #define EIGEN_VECTORIZE_SSE3
+    #endif
+    #ifdef __SSSE3__
+      #define EIGEN_VECTORIZE_SSSE3
+    #endif
+    #ifdef __SSE4_1__
+      #define EIGEN_VECTORIZE_SSE4_1
+    #endif
+    #ifdef __SSE4_2__
+      #define EIGEN_VECTORIZE_SSE4_2
+    #endif
+    #ifdef __AVX__
+      #ifndef EIGEN_USE_SYCL 
+        #define EIGEN_VECTORIZE_AVX
+      #endif
+      #define EIGEN_VECTORIZE_SSE3
+      #define EIGEN_VECTORIZE_SSSE3
+      #define EIGEN_VECTORIZE_SSE4_1
+      #define EIGEN_VECTORIZE_SSE4_2
+    #endif
+    #ifdef __AVX2__
+      #ifndef EIGEN_USE_SYCL 
+        #define EIGEN_VECTORIZE_AVX2
+        #define EIGEN_VECTORIZE_AVX
+      #endif
+      #define EIGEN_VECTORIZE_SSE3
+      #define EIGEN_VECTORIZE_SSSE3
+      #define EIGEN_VECTORIZE_SSE4_1
+      #define EIGEN_VECTORIZE_SSE4_2
+    #endif
+    #if defined(__FMA__) || (EIGEN_COMP_MSVC && defined(__AVX2__))
+      // MSVC does not expose a switch dedicated for FMA
+      // For MSVC, AVX2 => FMA
+      #define EIGEN_VECTORIZE_FMA
+    #endif
+    #if defined(__AVX512F__)
+      #ifndef EIGEN_VECTORIZE_FMA
+      #if EIGEN_COMP_GNUC
+      #error Please add -mfma to your compiler flags: compiling with -mavx512f alone without SSE/AVX FMA is not supported (bug 1638).
+      #else
+      #error Please enable FMA in your compiler flags (e.g. -mfma): compiling with AVX512 alone without SSE/AVX FMA is not supported (bug 1638).
+      #endif
+      #endif
+      #ifndef EIGEN_USE_SYCL
+        #define EIGEN_VECTORIZE_AVX512
+        #define EIGEN_VECTORIZE_AVX2
+        #define EIGEN_VECTORIZE_AVX
+      #endif
+      #define EIGEN_VECTORIZE_FMA
+      #define EIGEN_VECTORIZE_SSE3
+      #define EIGEN_VECTORIZE_SSSE3
+      #define EIGEN_VECTORIZE_SSE4_1
+      #define EIGEN_VECTORIZE_SSE4_2
+      #ifndef EIGEN_USE_SYCL
+        #ifdef __AVX512DQ__
+          #define EIGEN_VECTORIZE_AVX512DQ
+        #endif
+        #ifdef __AVX512ER__
+          #define EIGEN_VECTORIZE_AVX512ER
+        #endif
+        #ifdef __AVX512BF16__
+          #define EIGEN_VECTORIZE_AVX512BF16
+        #endif
+      #endif
+    #endif
+
+    // Disable AVX support on broken xcode versions
+    #if defined(__apple_build_version__) && (__apple_build_version__ == 11000033 ) && ( __MAC_OS_X_VERSION_MIN_REQUIRED == 101500 )
+      // A nasty bug in the clang compiler shipped with xcode in a common compilation situation
+      // when XCode 11.0 and Mac deployment target macOS 10.15 is https://trac.macports.org/ticket/58776#no1
+      #ifdef EIGEN_VECTORIZE_AVX
+        #undef EIGEN_VECTORIZE_AVX
+        #warning "Disabling AVX support: clang compiler shipped with XCode 11.[012] generates broken assembly with -macosx-version-min=10.15 and AVX enabled. "
+        #ifdef EIGEN_VECTORIZE_AVX2
+          #undef EIGEN_VECTORIZE_AVX2
+        #endif
+        #ifdef EIGEN_VECTORIZE_FMA
+          #undef EIGEN_VECTORIZE_FMA
+        #endif
+        #ifdef EIGEN_VECTORIZE_AVX512
+          #undef EIGEN_VECTORIZE_AVX512
+        #endif
+        #ifdef EIGEN_VECTORIZE_AVX512DQ
+          #undef EIGEN_VECTORIZE_AVX512DQ
+        #endif
+        #ifdef EIGEN_VECTORIZE_AVX512ER
+          #undef EIGEN_VECTORIZE_AVX512ER
+        #endif
+      #endif
+      // NOTE: Confirmed test failures in XCode 11.0, and XCode 11.2 with  -macosx-version-min=10.15 and AVX
+      // NOTE using -macosx-version-min=10.15 with Xcode 11.0 results in runtime segmentation faults in many tests, 11.2 produce core dumps in 3 tests
+      // NOTE using -macosx-version-min=10.14 produces functioning and passing tests in all cases
+      // NOTE __clang_version__ "11.0.0 (clang-1100.0.33.8)"  XCode 11.0 <- Produces many segfault and core dumping tests
+      //                                                                    with  -macosx-version-min=10.15 and AVX
+      // NOTE __clang_version__ "11.0.0 (clang-1100.0.33.12)" XCode 11.2 <- Produces 3 core dumping tests with  
+      //                                                                    -macosx-version-min=10.15 and AVX
+    #endif
+
+    // include files
+
+    // This extern "C" works around a MINGW-w64 compilation issue
+    // https://sourceforge.net/tracker/index.php?func=detail&aid=3018394&group_id=202880&atid=983354
+    // In essence, intrin.h is included by windows.h and also declares intrinsics (just as emmintrin.h etc. below do).
+    // However, intrin.h uses an extern "C" declaration, and g++ thus complains of duplicate declarations
+    // with conflicting linkage.  The linkage for intrinsics doesn't matter, but at that stage the compiler doesn't know;
+    // so, to avoid compile errors when windows.h is included after Eigen/Core, ensure intrinsics are extern "C" here too.
+    // notice that since these are C headers, the extern "C" is theoretically needed anyways.
+    extern "C" {
+      // In theory we should only include immintrin.h and not the other *mmintrin.h header files directly.
+      // Doing so triggers some issues with ICC. However old gcc versions seems to not have this file, thus:
+      #if EIGEN_COMP_ICC >= 1110
+        #include <immintrin.h>
+      #else
+        #include <mmintrin.h>
+        #include <emmintrin.h>
+        #include <xmmintrin.h>
+        #ifdef  EIGEN_VECTORIZE_SSE3
+        #include <pmmintrin.h>
+        #endif
+        #ifdef EIGEN_VECTORIZE_SSSE3
+        #include <tmmintrin.h>
+        #endif
+        #ifdef EIGEN_VECTORIZE_SSE4_1
+        #include <smmintrin.h>
+        #endif
+        #ifdef EIGEN_VECTORIZE_SSE4_2
+        #include <nmmintrin.h>
+        #endif
+        #if defined(EIGEN_VECTORIZE_AVX) || defined(EIGEN_VECTORIZE_AVX512)
+        #include <immintrin.h>
+        #endif
+      #endif
+    } // end extern "C"
+
+  #elif defined __VSX__
+
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_VSX
+    #include <altivec.h>
+    // We need to #undef all these ugly tokens defined in <altivec.h>
+    // => use __vector instead of vector
+    #undef bool
+    #undef vector
+    #undef pixel
+
+  #elif defined __ALTIVEC__
+
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_ALTIVEC
+    #include <altivec.h>
+    // We need to #undef all these ugly tokens defined in <altivec.h>
+    // => use __vector instead of vector
+    #undef bool
+    #undef vector
+    #undef pixel
+
+  #elif ((defined  __ARM_NEON) || (defined __ARM_NEON__)) && !(defined EIGEN_ARM64_USE_SVE)
+
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_NEON
+    #include <arm_neon.h>
+
+  // We currently require SVE to be enabled explicitly via EIGEN_ARM64_USE_SVE and
+  // will not select the backend automatically
+  #elif (defined __ARM_FEATURE_SVE) && (defined EIGEN_ARM64_USE_SVE)
+
+    #define EIGEN_VECTORIZE
+    #define EIGEN_VECTORIZE_SVE
+    #include <arm_sve.h>
+
+    // Since we depend on knowing SVE vector lengths at compile-time, we need
+    // to ensure a fixed lengths is set
+    #if defined __ARM_FEATURE_SVE_BITS
+      #define EIGEN_ARM64_SVE_VL __ARM_FEATURE_SVE_BITS
+    #else
+#error "Eigen requires a fixed SVE lector length but EIGEN_ARM64_SVE_VL is not set."
+#endif
+
+#elif (defined __s390x__ && defined __VEC__)
+
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_ZVECTOR
+#include <vecintrin.h>
+
+#elif defined __mips_msa
+
+// Limit MSA optimizations to little-endian CPUs for now.
+// TODO: Perhaps, eventually support MSA optimizations on big-endian CPUs?
+#if defined(__BYTE_ORDER__) && (__BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__)
+#if defined(__LP64__)
+#define EIGEN_MIPS_64
+#else
+#define EIGEN_MIPS_32
+#endif
+#define EIGEN_VECTORIZE
+#define EIGEN_VECTORIZE_MSA
+#include <msa.h>
+#endif
+
+#endif
+#endif
+
+// Following the Arm ACLE arm_neon.h should also include arm_fp16.h but not all
+// compilers seem to follow this. We therefore include it explicitly.
+// See also: https://bugs.llvm.org/show_bug.cgi?id=47955
+#if defined(EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC)
+  #include <arm_fp16.h>
+#endif
+
+#if defined(__F16C__) && (!defined(EIGEN_GPUCC) && (!defined(EIGEN_COMP_CLANG) || EIGEN_COMP_CLANG>=380))
+  // We can use the optimized fp16 to float and float to fp16 conversion routines
+  #define EIGEN_HAS_FP16_C
+
+  #if defined(EIGEN_COMP_CLANG)
+    // Workaround for clang: The FP16C intrinsics for clang are included by
+    // immintrin.h, as opposed to emmintrin.h as suggested by Intel:
+    // https://software.intel.com/sites/landingpage/IntrinsicsGuide/#othertechs=FP16C&expand=1711
+    #include <immintrin.h>
+  #endif
+#endif
+
+#if defined EIGEN_CUDACC
+  #define EIGEN_VECTORIZE_GPU
+  #include <vector_types.h>
+  #if EIGEN_CUDA_SDK_VER >= 70500
+    #define EIGEN_HAS_CUDA_FP16
+  #endif
+#endif
+
+#if defined(EIGEN_HAS_CUDA_FP16)
+  #include <cuda_runtime_api.h>
+  #include <cuda_fp16.h>
+#endif
+
+#if defined(EIGEN_HIPCC)
+  #define EIGEN_VECTORIZE_GPU
+  #include <hip/hip_vector_types.h>
+  #define EIGEN_HAS_HIP_FP16
+  #include <hip/hip_fp16.h>
+#endif
+
+
+/** \brief Namespace containing all symbols from the %Eigen library. */
+namespace Eigen {
+
+inline static const char *SimdInstructionSetsInUse(void) {
+#if defined(EIGEN_VECTORIZE_AVX512)
+  return "AVX512, FMA, AVX2, AVX, SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+#elif defined(EIGEN_VECTORIZE_AVX)
+  return "AVX SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+#elif defined(EIGEN_VECTORIZE_SSE4_2)
+  return "SSE, SSE2, SSE3, SSSE3, SSE4.1, SSE4.2";
+#elif defined(EIGEN_VECTORIZE_SSE4_1)
+  return "SSE, SSE2, SSE3, SSSE3, SSE4.1";
+#elif defined(EIGEN_VECTORIZE_SSSE3)
+  return "SSE, SSE2, SSE3, SSSE3";
+#elif defined(EIGEN_VECTORIZE_SSE3)
+  return "SSE, SSE2, SSE3";
+#elif defined(EIGEN_VECTORIZE_SSE2)
+  return "SSE, SSE2";
+#elif defined(EIGEN_VECTORIZE_ALTIVEC)
+  return "AltiVec";
+#elif defined(EIGEN_VECTORIZE_VSX)
+  return "VSX";
+#elif defined(EIGEN_VECTORIZE_NEON)
+  return "ARM NEON";
+#elif defined(EIGEN_VECTORIZE_SVE)
+  return "ARM SVE";
+#elif defined(EIGEN_VECTORIZE_ZVECTOR)
+  return "S390X ZVECTOR";
+#elif defined(EIGEN_VECTORIZE_MSA)
+  return "MIPS MSA";
+#else
+  return "None";
+#endif
+}
+
+} // end namespace Eigen
+
+
+#endif // EIGEN_CONFIGURE_VECTORIZATION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
index 7587d68..35dcaa7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Constants.h
@@ -3,6 +3,7 @@
 //
 // Copyright (C) 2008-2015 Gael Guennebaud <gael.guennebaud@inria.fr>
 // Copyright (C) 2007-2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+// Copyright (C) 2020, Arm Limited and Contributors
 //
 // This Source Code Form is subject to the terms of the Mozilla
 // Public License v. 2.0. If a copy of the MPL was not distributed
@@ -25,6 +26,10 @@
   */
 const int DynamicIndex = 0xffffff;
 
+/** This value means that the increment to go from one value to another in a sequence is not constant for each step.
+  */
+const int UndefinedIncr = 0xfffffe;
+
 /** This value means +Infinity; it is currently used only as the p parameter to MatrixBase::lpNorm<int>().
   * The value Infinity there means the L-infinity norm.
   */
@@ -152,7 +157,7 @@
 /** \deprecated \ingroup flags
   *
   * means the first coefficient packet is guaranteed to be aligned.
-  * An expression cannot has the AlignedBit without the PacketAccessBit flag.
+  * An expression cannot have the AlignedBit without the PacketAccessBit flag.
   * In other words, this means we are allow to perform an aligned packet access to the first element regardless
   * of the expression kind:
   * \code
@@ -251,12 +256,6 @@
 };
 
 /** \ingroup enums
- * Enum used by DenseBase::corner() in Eigen2 compatibility mode. */
-// FIXME after the corner() API change, this was not needed anymore, except by AlignedBox
-// TODO: find out what to do with that. Adapt the AlignedBox API ?
-enum CornerType { TopLeft, TopRight, BottomLeft, BottomRight };
-
-/** \ingroup enums
   * Enum containing possible values for the \p Direction parameter of
   * Reverse, PartialReduxExpr and VectorwiseOp. */
 enum DirectionType { 
@@ -330,9 +329,20 @@
   * Enum for specifying whether to apply or solve on the left or right. */
 enum SideType {
   /** Apply transformation on the left. */
-  OnTheLeft = 1,  
+  OnTheLeft = 1,
   /** Apply transformation on the right. */
-  OnTheRight = 2  
+  OnTheRight = 2
+};
+
+/** \ingroup enums
+ * Enum for specifying NaN-propagation behavior, e.g. for coeff-wise min/max. */
+enum NaNPropagationOptions {
+  /**  Implementation defined behavior if NaNs are present. */
+  PropagateFast = 0,
+  /**  Always propagate NaNs. */
+  PropagateNaN,
+  /**  Always propagate not-NaNs. */
+  PropagateNumbers
 };
 
 /* the following used to be written as:
@@ -464,6 +474,8 @@
     AltiVec = 0x2,
     VSX = 0x3,
     NEON = 0x4,
+    MSA = 0x5,
+    SVE = 0x6,
 #if defined EIGEN_VECTORIZE_SSE
     Target = SSE
 #elif defined EIGEN_VECTORIZE_ALTIVEC
@@ -472,6 +484,10 @@
     Target = VSX
 #elif defined EIGEN_VECTORIZE_NEON
     Target = NEON
+#elif defined EIGEN_VECTORIZE_SVE
+    Target = SVE
+#elif defined EIGEN_VECTORIZE_MSA
+    Target = MSA
 #else
     Target = Generic
 #endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
index ce573a8..3bec072 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/DisableStupidWarnings.h
@@ -4,7 +4,6 @@
 #ifdef _MSC_VER
   // 4100 - unreferenced formal parameter (occurred e.g. in aligned_allocator::destroy(pointer p))
   // 4101 - unreferenced local variable
-  // 4127 - conditional expression is constant
   // 4181 - qualifier applied to reference type ignored
   // 4211 - nonstandard extension used : redefined extern to static
   // 4244 - 'argument' : conversion from 'type1' to 'type2', possible loss of data
@@ -20,7 +19,7 @@
   #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
     #pragma warning( push )
   #endif
-  #pragma warning( disable : 4100 4101 4127 4181 4211 4244 4273 4324 4503 4512 4522 4700 4714 4717 4800)
+  #pragma warning( disable : 4100 4101 4181 4211 4244 4273 4324 4503 4512 4522 4700 4714 4717 4800)
 
 #elif defined __INTEL_COMPILER
   // 2196 - routine is both "inline" and "noinline" ("noinline" assumed)
@@ -42,6 +41,17 @@
     #pragma clang diagnostic push
   #endif
   #pragma clang diagnostic ignored "-Wconstant-logical-operand"
+  #if __clang_major__ >= 3 && __clang_minor__ >= 5
+    #pragma clang diagnostic ignored "-Wabsolute-value"
+  #endif
+  #if __clang_major__ >= 10
+    #pragma clang diagnostic ignored "-Wimplicit-int-float-conversion"
+  #endif
+  #if ( defined(__ALTIVEC__) || defined(__VSX__) ) && __cplusplus < 201103L
+    // warning: generic selections are a C11-specific feature
+    // ignoring warnings thrown at vec_ctf in Altivec/PacketMath.h
+    #pragma clang diagnostic ignored "-Wc11-extensions"
+  #endif
 
 #elif defined __GNUC__
 
@@ -57,13 +67,18 @@
   #if __GNUC__>=6
     #pragma GCC diagnostic ignored "-Wignored-attributes"
   #endif
-  #if __GNUC__>=9
-    #pragma GCC diagnostic ignored "-Wdeprecated-copy"
+  #if __GNUC__==7
+    // See: https://gcc.gnu.org/bugzilla/show_bug.cgi?id=89325
+    #pragma GCC diagnostic ignored "-Wattributes"
   #endif
-
+  #if __GNUC__==11
+    // This warning is a false positive
+    #pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+  #endif
 #endif
 
 #if defined __NVCC__
+  #pragma diag_suppress boolean_controlling_expr_is_constant
   // Disable the "statement is unreachable" message
   #pragma diag_suppress code_is_unreachable
   // Disable the "dynamic initialization in unreachable code" message
@@ -81,6 +96,15 @@
   #pragma diag_suppress 2671
   #pragma diag_suppress 2735
   #pragma diag_suppress 2737
+  #pragma diag_suppress 2739
 #endif
 
+#else
+// warnings already disabled:
+# ifndef EIGEN_WARNINGS_DISABLED_2
+#  define EIGEN_WARNINGS_DISABLED_2
+# elif defined(EIGEN_INTERNAL_DEBUGGING)
+#  error "Do not include \"DisableStupidWarnings.h\" recursively more than twice!"
+# endif
+
 #endif // not EIGEN_WARNINGS_DISABLED
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
index ea10739..2f9cc44 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ForwardDeclarations.h
@@ -47,11 +47,7 @@
 template<typename Derived> struct EigenBase;
 template<typename Derived> class DenseBase;
 template<typename Derived> class PlainObjectBase;
-
-
-template<typename Derived,
-         int Level = internal::accessors_level<Derived>::value >
-class DenseCoeffsBase;
+template<typename Derived, int Level> class DenseCoeffsBase;
 
 template<typename _Scalar, int _Rows, int _Cols,
          int _Options = AutoAlign |
@@ -83,6 +79,8 @@
 template<typename ExpressionType> class SwapWrapper;
 
 template<typename XprType, int BlockRows=Dynamic, int BlockCols=Dynamic, bool InnerPanel = false> class Block;
+template<typename XprType, typename RowIndices, typename ColIndices> class IndexedView;
+template<typename XprType, int Rows=Dynamic, int Cols=Dynamic, int Order=0> class Reshaped;
 
 template<typename MatrixType, int Size=Dynamic> class VectorBlock;
 template<typename MatrixType> class Transpose;
@@ -112,7 +110,7 @@
 template<typename Derived,
          int Level = internal::accessors_level<Derived>::has_write_access ? WriteAccessors : ReadOnlyAccessors
 > class MapBase;
-template<int InnerStrideAtCompileTime, int OuterStrideAtCompileTime> class Stride;
+template<int OuterStrideAtCompileTime, int InnerStrideAtCompileTime> class Stride;
 template<int Value = Dynamic> class InnerStride;
 template<int Value = Dynamic> class OuterStride;
 template<typename MatrixType, int MapOptions=Unaligned, typename StrideType = Stride<0,0> > class Map;
@@ -133,6 +131,10 @@
 template<typename XprType> class InnerIterator;
 
 namespace internal {
+template<typename XprType> class generic_randaccess_stl_iterator;
+template<typename XprType> class pointer_based_stl_iterator;
+template<typename XprType, DirectionType Direction> class subvector_stl_iterator;
+template<typename XprType, DirectionType Direction> class subvector_stl_reverse_iterator;
 template<typename DecompositionType> struct kernel_retval_base;
 template<typename DecompositionType> struct kernel_retval;
 template<typename DecompositionType> struct image_retval_base;
@@ -178,14 +180,15 @@
 template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_sum_op;
 template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_difference_op;
 template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_conj_product_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_min_op;
-template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_max_op;
+template<typename LhsScalar,typename RhsScalar=LhsScalar, int NaNPropagation=PropagateFast> struct scalar_min_op;
+template<typename LhsScalar,typename RhsScalar=LhsScalar, int NaNPropagation=PropagateFast> struct scalar_max_op;
 template<typename Scalar> struct scalar_opposite_op;
 template<typename Scalar> struct scalar_conjugate_op;
 template<typename Scalar> struct scalar_real_op;
 template<typename Scalar> struct scalar_imag_op;
 template<typename Scalar> struct scalar_abs_op;
 template<typename Scalar> struct scalar_abs2_op;
+template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_absolute_difference_op;
 template<typename Scalar> struct scalar_sqrt_op;
 template<typename Scalar> struct scalar_rsqrt_op;
 template<typename Scalar> struct scalar_exp_op;
@@ -202,7 +205,7 @@
 template<typename Scalar> struct scalar_random_op;
 template<typename Scalar> struct scalar_constant_op;
 template<typename Scalar> struct scalar_identity_op;
-template<typename Scalar,bool iscpx> struct scalar_sign_op;
+template<typename Scalar,bool is_complex, bool is_integer> struct scalar_sign_op;
 template<typename Scalar,typename ScalarExponent> struct scalar_pow_op;
 template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_hypot_op;
 template<typename LhsScalar,typename RhsScalar=LhsScalar> struct scalar_product_op;
@@ -213,11 +216,27 @@
 template<typename Scalar> struct scalar_digamma_op;
 template<typename Scalar> struct scalar_erf_op;
 template<typename Scalar> struct scalar_erfc_op;
+template<typename Scalar> struct scalar_ndtri_op;
 template<typename Scalar> struct scalar_igamma_op;
 template<typename Scalar> struct scalar_igammac_op;
 template<typename Scalar> struct scalar_zeta_op;
 template<typename Scalar> struct scalar_betainc_op;
 
+// Bessel functions in SpecialFunctions module
+template<typename Scalar> struct scalar_bessel_i0_op;
+template<typename Scalar> struct scalar_bessel_i0e_op;
+template<typename Scalar> struct scalar_bessel_i1_op;
+template<typename Scalar> struct scalar_bessel_i1e_op;
+template<typename Scalar> struct scalar_bessel_j0_op;
+template<typename Scalar> struct scalar_bessel_y0_op;
+template<typename Scalar> struct scalar_bessel_j1_op;
+template<typename Scalar> struct scalar_bessel_y1_op;
+template<typename Scalar> struct scalar_bessel_k0_op;
+template<typename Scalar> struct scalar_bessel_k0e_op;
+template<typename Scalar> struct scalar_bessel_k1_op;
+template<typename Scalar> struct scalar_bessel_k1e_op;
+
+
 } // end namespace internal
 
 struct IOFormat;
@@ -255,6 +274,7 @@
 template<typename MatrixType> class ColPivHouseholderQR;
 template<typename MatrixType> class FullPivHouseholderQR;
 template<typename MatrixType> class CompleteOrthogonalDecomposition;
+template<typename MatrixType> class SVDBase;
 template<typename MatrixType, int QRPreconditioner = ColPivHouseholderQRPreconditioner> class JacobiSVD;
 template<typename MatrixType> class BDCSVD;
 template<typename MatrixType, int UpLo = Lower> class LLT;
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h
new file mode 100644
index 0000000..f85de30
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IndexedViewHelper.h
@@ -0,0 +1,186 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+
+#ifndef EIGEN_INDEXED_VIEW_HELPER_H
+#define EIGEN_INDEXED_VIEW_HELPER_H
+
+namespace Eigen {
+
+namespace internal {
+struct symbolic_last_tag {};
+}
+
+/** \var last
+  * \ingroup Core_Module
+  *
+  * Can be used as a parameter to Eigen::seq and Eigen::seqN functions to symbolically reference the last element/row/columns
+  * of the underlying vector or matrix once passed to DenseBase::operator()(const RowIndices&, const ColIndices&).
+  *
+  * This symbolic placeholder supports standard arithmetic operations.
+  *
+  * A typical usage example would be:
+  * \code
+  * using namespace Eigen;
+  * using Eigen::last;
+  * VectorXd v(n);
+  * v(seq(2,last-2)).setOnes();
+  * \endcode
+  *
+  * \sa end
+  */
+static const symbolic::SymbolExpr<internal::symbolic_last_tag> last; // PLEASE use Eigen::last   instead of Eigen::placeholders::last
+
+/** \var lastp1
+  * \ingroup Core_Module
+  *
+  * Can be used as a parameter to Eigen::seq and Eigen::seqN functions to symbolically
+  * reference the last+1 element/row/columns of the underlying vector or matrix once
+  * passed to DenseBase::operator()(const RowIndices&, const ColIndices&).
+  *
+  * This symbolic placeholder supports standard arithmetic operations.
+  * It is essentially an alias to last+fix<1>.
+  *
+  * \sa last
+  */
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+static const auto lastp1 = last+fix<1>;
+#else
+// Using a FixedExpr<1> expression is important here to make sure the compiler
+// can fully optimize the computation starting indices with zero overhead.
+static const symbolic::AddExpr<symbolic::SymbolExpr<internal::symbolic_last_tag>,symbolic::ValueExpr<Eigen::internal::FixedInt<1> > > lastp1(last+fix<1>());
+#endif
+
+namespace internal {
+
+ // Replace symbolic last/end "keywords" by their true runtime value
+inline Index eval_expr_given_size(Index x, Index /* size */)   { return x; }
+
+template<int N>
+FixedInt<N> eval_expr_given_size(FixedInt<N> x, Index /*size*/)   { return x; }
+
+template<typename Derived>
+Index eval_expr_given_size(const symbolic::BaseExpr<Derived> &x, Index size)
+{
+  return x.derived().eval(last=size-1);
+}
+
+// Extract increment/step at compile time
+template<typename T, typename EnableIf = void> struct get_compile_time_incr {
+  enum { value = UndefinedIncr };
+};
+
+// Analogue of std::get<0>(x), but tailored for our needs.
+template<typename T>
+EIGEN_CONSTEXPR Index first(const T& x) EIGEN_NOEXCEPT { return x.first(); }
+
+// IndexedViewCompatibleType/makeIndexedViewCompatible turn an arbitrary object of type T into something usable by MatrixSlice
+// The generic implementation is a no-op
+template<typename T,int XprSize,typename EnableIf=void>
+struct IndexedViewCompatibleType {
+  typedef T type;
+};
+
+template<typename T,typename Q>
+const T& makeIndexedViewCompatible(const T& x, Index /*size*/, Q) { return x; }
+
+//--------------------------------------------------------------------------------
+// Handling of a single Index
+//--------------------------------------------------------------------------------
+
+struct SingleRange {
+  enum {
+    SizeAtCompileTime = 1
+  };
+  SingleRange(Index val) : m_value(val) {}
+  Index operator[](Index) const { return m_value; }
+  static EIGEN_CONSTEXPR Index size() EIGEN_NOEXCEPT { return 1; }
+  Index first() const EIGEN_NOEXCEPT { return m_value; }
+  Index m_value;
+};
+
+template<> struct get_compile_time_incr<SingleRange> {
+  enum { value = 1 }; // 1 or 0 ??
+};
+
+// Turn a single index into something that looks like an array (i.e., that exposes a .size(), and operator[](int) methods)
+template<typename T, int XprSize>
+struct IndexedViewCompatibleType<T,XprSize,typename internal::enable_if<internal::is_integral<T>::value>::type> {
+  // Here we could simply use Array, but maybe it's less work for the compiler to use
+  // a simpler wrapper as SingleRange
+  //typedef Eigen::Array<Index,1,1> type;
+  typedef SingleRange type;
+};
+
+template<typename T, int XprSize>
+struct IndexedViewCompatibleType<T, XprSize, typename enable_if<symbolic::is_symbolic<T>::value>::type> {
+  typedef SingleRange type;
+};
+
+
+template<typename T>
+typename enable_if<symbolic::is_symbolic<T>::value,SingleRange>::type
+makeIndexedViewCompatible(const T& id, Index size, SpecializedType) {
+  return eval_expr_given_size(id,size);
+}
+
+//--------------------------------------------------------------------------------
+// Handling of all
+//--------------------------------------------------------------------------------
+
+struct all_t { all_t() {} };
+
+// Convert a symbolic 'all' into a usable range type
+template<int XprSize>
+struct AllRange {
+  enum { SizeAtCompileTime = XprSize };
+  AllRange(Index size = XprSize) : m_size(size) {}
+  EIGEN_CONSTEXPR Index operator[](Index i) const EIGEN_NOEXCEPT { return i; }
+  EIGEN_CONSTEXPR Index size() const EIGEN_NOEXCEPT { return m_size.value(); }
+  EIGEN_CONSTEXPR Index first() const EIGEN_NOEXCEPT { return 0; }
+  variable_if_dynamic<Index,XprSize> m_size;
+};
+
+template<int XprSize>
+struct IndexedViewCompatibleType<all_t,XprSize> {
+  typedef AllRange<XprSize> type;
+};
+
+template<typename XprSizeType>
+inline AllRange<get_fixed_value<XprSizeType>::value> makeIndexedViewCompatible(all_t , XprSizeType size, SpecializedType) {
+  return AllRange<get_fixed_value<XprSizeType>::value>(size);
+}
+
+template<int Size> struct get_compile_time_incr<AllRange<Size> > {
+  enum { value = 1 };
+};
+
+} // end namespace internal
+
+
+/** \var all
+  * \ingroup Core_Module
+  * Can be used as a parameter to DenseBase::operator()(const RowIndices&, const ColIndices&) to index all rows or columns
+  */
+static const Eigen::internal::all_t all; // PLEASE use Eigen::all instead of Eigen::placeholders::all
+
+
+namespace placeholders {
+  typedef symbolic::SymbolExpr<internal::symbolic_last_tag> last_t;
+  typedef symbolic::AddExpr<symbolic::SymbolExpr<internal::symbolic_last_tag>,symbolic::ValueExpr<Eigen::internal::FixedInt<1> > > end_t;
+  typedef Eigen::internal::all_t all_t;
+
+  EIGEN_DEPRECATED static const all_t  all  = Eigen::all;    // PLEASE use Eigen::all    instead of Eigen::placeholders::all
+  EIGEN_DEPRECATED static const last_t last = Eigen::last;   // PLEASE use Eigen::last   instead of Eigen::placeholders::last
+  EIGEN_DEPRECATED static const end_t  end  = Eigen::lastp1; // PLEASE use Eigen::lastp1 instead of Eigen::placeholders::end
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_INDEXED_VIEW_HELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h
new file mode 100644
index 0000000..945d426
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/IntegralConstant.h
@@ -0,0 +1,272 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+
+#ifndef EIGEN_INTEGRAL_CONSTANT_H
+#define EIGEN_INTEGRAL_CONSTANT_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<int N> class FixedInt;
+template<int N> class VariableAndFixedInt;
+
+/** \internal
+  * \class FixedInt
+  *
+  * This class embeds a compile-time integer \c N.
+  *
+  * It is similar to c++11 std::integral_constant<int,N> but with some additional features
+  * such as:
+  *  - implicit conversion to int
+  *  - arithmetic and some bitwise operators: -, +, *, /, %, &, |
+  *  - c++98/14 compatibility with fix<N> and fix<N>() syntax to define integral constants.
+  *
+  * It is strongly discouraged to directly deal with this class FixedInt. Instances are expcected to
+  * be created by the user using Eigen::fix<N> or Eigen::fix<N>(). In C++98-11, the former syntax does
+  * not create a FixedInt<N> instance but rather a point to function that needs to be \em cleaned-up
+  * using the generic helper:
+  * \code
+  * internal::cleanup_index_type<T>::type
+  * internal::cleanup_index_type<T,DynamicKey>::type
+  * \endcode
+  * where T can a FixedInt<N>, a pointer to function FixedInt<N> (*)(), or numerous other integer-like representations.
+  * \c DynamicKey is either Dynamic (default) or DynamicIndex and used to identify true compile-time values.
+  *
+  * For convenience, you can extract the compile-time value \c N in a generic way using the following helper:
+  * \code
+  * internal::get_fixed_value<T,DefaultVal>::value
+  * \endcode
+  * that will give you \c N if T equals FixedInt<N> or FixedInt<N> (*)(), and \c DefaultVal if T does not embed any compile-time value (e.g., T==int).
+  *
+  * \sa fix<N>, class VariableAndFixedInt
+  */
+template<int N> class FixedInt
+{
+public:
+  static const int value = N;
+  EIGEN_CONSTEXPR operator int() const { return value; }
+  FixedInt() {}
+  FixedInt( VariableAndFixedInt<N> other) {
+    #ifndef EIGEN_INTERNAL_DEBUGGING
+    EIGEN_UNUSED_VARIABLE(other);
+    #endif
+    eigen_internal_assert(int(other)==N);
+  }
+
+  FixedInt<-N> operator-() const { return FixedInt<-N>(); }
+  template<int M>
+  FixedInt<N+M> operator+( FixedInt<M>) const { return FixedInt<N+M>(); }
+  template<int M>
+  FixedInt<N-M> operator-( FixedInt<M>) const { return FixedInt<N-M>(); }
+  template<int M>
+  FixedInt<N*M> operator*( FixedInt<M>) const { return FixedInt<N*M>(); }
+  template<int M>
+  FixedInt<N/M> operator/( FixedInt<M>) const { return FixedInt<N/M>(); }
+  template<int M>
+  FixedInt<N%M> operator%( FixedInt<M>) const { return FixedInt<N%M>(); }
+  template<int M>
+  FixedInt<N|M> operator|( FixedInt<M>) const { return FixedInt<N|M>(); }
+  template<int M>
+  FixedInt<N&M> operator&( FixedInt<M>) const { return FixedInt<N&M>(); }
+
+#if EIGEN_HAS_CXX14_VARIABLE_TEMPLATES
+  // Needed in C++14 to allow fix<N>():
+  FixedInt operator() () const { return *this; }
+
+  VariableAndFixedInt<N> operator() (int val) const { return VariableAndFixedInt<N>(val); }
+#else
+  FixedInt ( FixedInt<N> (*)() ) {}
+#endif
+
+#if EIGEN_HAS_CXX11
+  FixedInt(std::integral_constant<int,N>) {}
+#endif
+};
+
+/** \internal
+  * \class VariableAndFixedInt
+  *
+  * This class embeds both a compile-time integer \c N and a runtime integer.
+  * Both values are supposed to be equal unless the compile-time value \c N has a special
+  * value meaning that the runtime-value should be used. Depending on the context, this special
+  * value can be either Eigen::Dynamic (for positive quantities) or Eigen::DynamicIndex (for
+  * quantities that can be negative).
+  *
+  * It is the return-type of the function Eigen::fix<N>(int), and most of the time this is the only
+  * way it is used. It is strongly discouraged to directly deal with instances of VariableAndFixedInt.
+  * Indeed, in order to write generic code, it is the responsibility of the callee to properly convert
+  * it to either a true compile-time quantity (i.e. a FixedInt<N>), or to a runtime quantity (e.g., an Index)
+  * using the following generic helper:
+  * \code
+  * internal::cleanup_index_type<T>::type
+  * internal::cleanup_index_type<T,DynamicKey>::type
+  * \endcode
+  * where T can be a template instantiation of VariableAndFixedInt or numerous other integer-like representations.
+  * \c DynamicKey is either Dynamic (default) or DynamicIndex and used to identify true compile-time values.
+  *
+  * For convenience, you can also extract the compile-time value \c N using the following helper:
+  * \code
+  * internal::get_fixed_value<T,DefaultVal>::value
+  * \endcode
+  * that will give you \c N if T equals VariableAndFixedInt<N>, and \c DefaultVal if T does not embed any compile-time value (e.g., T==int).
+  *
+  * \sa fix<N>(int), class FixedInt
+  */
+template<int N> class VariableAndFixedInt
+{
+public:
+  static const int value = N;
+  operator int() const { return m_value; }
+  VariableAndFixedInt(int val) { m_value = val; }
+protected:
+  int m_value;
+};
+
+template<typename T, int Default=Dynamic> struct get_fixed_value {
+  static const int value = Default;
+};
+
+template<int N,int Default> struct get_fixed_value<FixedInt<N>,Default> {
+  static const int value = N;
+};
+
+#if !EIGEN_HAS_CXX14
+template<int N,int Default> struct get_fixed_value<FixedInt<N> (*)(),Default> {
+  static const int value = N;
+};
+#endif
+
+template<int N,int Default> struct get_fixed_value<VariableAndFixedInt<N>,Default> {
+  static const int value = N ;
+};
+
+template<typename T, int N, int Default>
+struct get_fixed_value<variable_if_dynamic<T,N>,Default> {
+  static const int value = N;
+};
+
+template<typename T> EIGEN_DEVICE_FUNC Index get_runtime_value(const T &x) { return x; }
+#if !EIGEN_HAS_CXX14
+template<int N> EIGEN_DEVICE_FUNC Index get_runtime_value(FixedInt<N> (*)()) { return N; }
+#endif
+
+// Cleanup integer/FixedInt/VariableAndFixedInt/etc types:
+
+// By default, no cleanup:
+template<typename T, int DynamicKey=Dynamic, typename EnableIf=void> struct cleanup_index_type { typedef T type; };
+
+// Convert any integral type (e.g., short, int, unsigned int, etc.) to Eigen::Index
+template<typename T, int DynamicKey> struct cleanup_index_type<T,DynamicKey,typename internal::enable_if<internal::is_integral<T>::value>::type> { typedef Index type; };
+
+#if !EIGEN_HAS_CXX14
+// In c++98/c++11, fix<N> is a pointer to function that we better cleanup to a true FixedInt<N>:
+template<int N, int DynamicKey> struct cleanup_index_type<FixedInt<N> (*)(), DynamicKey> { typedef FixedInt<N> type; };
+#endif
+
+// If VariableAndFixedInt does not match DynamicKey, then we turn it to a pure compile-time value:
+template<int N, int DynamicKey> struct cleanup_index_type<VariableAndFixedInt<N>, DynamicKey> { typedef FixedInt<N> type; };
+// If VariableAndFixedInt matches DynamicKey, then we turn it to a pure runtime-value (aka Index):
+template<int DynamicKey> struct cleanup_index_type<VariableAndFixedInt<DynamicKey>, DynamicKey> { typedef Index type; };
+
+#if EIGEN_HAS_CXX11
+template<int N, int DynamicKey> struct cleanup_index_type<std::integral_constant<int,N>, DynamicKey> { typedef FixedInt<N> type; };
+#endif
+
+} // end namespace internal
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+#if EIGEN_HAS_CXX14_VARIABLE_TEMPLATES
+template<int N>
+static const internal::FixedInt<N> fix{};
+#else
+template<int N>
+inline internal::FixedInt<N> fix() { return internal::FixedInt<N>(); }
+
+// The generic typename T is mandatory. Otherwise, a code like fix<N> could refer to either the function above or this next overload.
+// This way a code like fix<N> can only refer to the previous function.
+template<int N,typename T>
+inline internal::VariableAndFixedInt<N> fix(T val) { return internal::VariableAndFixedInt<N>(internal::convert_index<int>(val)); }
+#endif
+
+#else // EIGEN_PARSED_BY_DOXYGEN
+
+/** \var fix<N>()
+  * \ingroup Core_Module
+  *
+  * This \em identifier permits to construct an object embedding a compile-time integer \c N.
+  *
+  * \tparam N the compile-time integer value
+  *
+  * It is typically used in conjunction with the Eigen::seq and Eigen::seqN functions to pass compile-time values to them:
+  * \code
+  * seqN(10,fix<4>,fix<-3>)   // <=> [10 7 4 1]
+  * \endcode
+  *
+  * See also the function fix(int) to pass both a compile-time and runtime value.
+  *
+  * In c++14, it is implemented as:
+  * \code
+  * template<int N> static const internal::FixedInt<N> fix{};
+  * \endcode
+  * where internal::FixedInt<N> is an internal template class similar to
+  * <a href="http://en.cppreference.com/w/cpp/types/integral_constant">\c std::integral_constant </a><tt> <int,N> </tt>
+  * Here, \c fix<N> is thus an object of type \c internal::FixedInt<N>.
+  *
+  * In c++98/11, it is implemented as a function:
+  * \code
+  * template<int N> inline internal::FixedInt<N> fix();
+  * \endcode
+  * Here internal::FixedInt<N> is thus a pointer to function.
+  *
+  * If for some reason you want a true object in c++98 then you can write: \code fix<N>() \endcode which is also valid in c++14.
+  *
+  * \sa fix<N>(int), seq, seqN
+  */
+template<int N>
+static const auto fix();
+
+/** \fn fix<N>(int)
+  * \ingroup Core_Module
+  *
+  * This function returns an object embedding both a compile-time integer \c N, and a fallback runtime value \a val.
+  *
+  * \tparam N the compile-time integer value
+  * \param  val the fallback runtime integer value
+  *
+  * This function is a more general version of the \ref fix identifier/function that can be used in template code
+  * where the compile-time value could turn out to actually mean "undefined at compile-time". For positive integers
+  * such as a size or a dimension, this case is identified by Eigen::Dynamic, whereas runtime signed integers
+  * (e.g., an increment/stride) are identified as Eigen::DynamicIndex. In such a case, the runtime value \a val
+  * will be used as a fallback.
+  *
+  * A typical use case would be:
+  * \code
+  * template<typename Derived> void foo(const MatrixBase<Derived> &mat) {
+  *   const int N = Derived::RowsAtCompileTime==Dynamic ? Dynamic : Derived::RowsAtCompileTime/2;
+  *   const int n = mat.rows()/2;
+  *   ... mat( seqN(0,fix<N>(n) ) ...;
+  * }
+  * \endcode
+  * In this example, the function Eigen::seqN knows that the second argument is expected to be a size.
+  * If the passed compile-time value N equals Eigen::Dynamic, then the proxy object returned by fix will be dissmissed, and converted to an Eigen::Index of value \c n.
+  * Otherwise, the runtime-value \c n will be dissmissed, and the returned ArithmeticSequence will be of the exact same type as <tt> seqN(0,fix<N>) </tt>.
+  *
+  * \sa fix, seqN, class ArithmeticSequence
+  */
+template<int N>
+static const auto fix(int val);
+
+#endif // EIGEN_PARSED_BY_DOXYGEN
+
+} // end namespace Eigen
+
+#endif // EIGEN_INTEGRAL_CONSTANT_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
index b2bb0c2..986c3d4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Macros.h
@@ -11,19 +11,56 @@
 #ifndef EIGEN_MACROS_H
 #define EIGEN_MACROS_H
 
+//------------------------------------------------------------------------------------------
+// Eigen version and basic defaults
+//------------------------------------------------------------------------------------------
+
 #define EIGEN_WORLD_VERSION 3
-#define EIGEN_MAJOR_VERSION 3
-#define EIGEN_MINOR_VERSION 7
+#define EIGEN_MAJOR_VERSION 4
+#define EIGEN_MINOR_VERSION 0
 
 #define EIGEN_VERSION_AT_LEAST(x,y,z) (EIGEN_WORLD_VERSION>x || (EIGEN_WORLD_VERSION>=x && \
                                       (EIGEN_MAJOR_VERSION>y || (EIGEN_MAJOR_VERSION>=y && \
                                                                  EIGEN_MINOR_VERSION>=z))))
 
+#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor
+#else
+#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::ColMajor
+#endif
+
+#ifndef EIGEN_DEFAULT_DENSE_INDEX_TYPE
+#define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
+#endif
+
+// Upperbound on the C++ version to use.
+// Expected values are 03, 11, 14, 17, etc.
+// By default, let's use an arbitrarily large C++ version.
+#ifndef EIGEN_MAX_CPP_VER
+#define EIGEN_MAX_CPP_VER 99
+#endif
+
+/** Allows to disable some optimizations which might affect the accuracy of the result.
+  * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
+  * They currently include:
+  *   - single precision ArrayBase::sin() and ArrayBase::cos() for SSE and AVX vectorization.
+  */
+#ifndef EIGEN_FAST_MATH
+#define EIGEN_FAST_MATH 1
+#endif
+
+#ifndef EIGEN_STACK_ALLOCATION_LIMIT
+// 131072 == 128 KB
+#define EIGEN_STACK_ALLOCATION_LIMIT 131072
+#endif
+
+//------------------------------------------------------------------------------------------
 // Compiler identification, EIGEN_COMP_*
+//------------------------------------------------------------------------------------------
 
 /// \internal EIGEN_COMP_GNUC set to 1 for all compilers compatible with GCC
 #ifdef __GNUC__
-  #define EIGEN_COMP_GNUC 1
+  #define EIGEN_COMP_GNUC (__GNUC__*10+__GNUC_MINOR__)
 #else
   #define EIGEN_COMP_GNUC 0
 #endif
@@ -35,6 +72,12 @@
   #define EIGEN_COMP_CLANG 0
 #endif
 
+/// \internal EIGEN_COMP_CASTXML set to 1 if being preprocessed by CastXML
+#if defined(__castxml__)
+  #define EIGEN_COMP_CASTXML 1
+#else
+  #define EIGEN_COMP_CASTXML 0
+#endif
 
 /// \internal EIGEN_COMP_LLVM set to 1 if the compiler backend is llvm
 #if defined(__llvm__)
@@ -71,14 +114,44 @@
   #define EIGEN_COMP_MSVC 0
 #endif
 
+#if defined(__NVCC__)
+#if defined(__CUDACC_VER_MAJOR__) && (__CUDACC_VER_MAJOR__ >= 9)
+  #define EIGEN_COMP_NVCC  ((__CUDACC_VER_MAJOR__ * 10000) + (__CUDACC_VER_MINOR__ * 100))
+#elif defined(__CUDACC_VER__)
+  #define EIGEN_COMP_NVCC __CUDACC_VER__
+#else
+  #error "NVCC did not define compiler version."
+#endif
+#else
+  #define EIGEN_COMP_NVCC 0
+#endif
+
 // For the record, here is a table summarizing the possible values for EIGEN_COMP_MSVC:
-//  name  ver   MSC_VER
-//  2008    9      1500
-//  2010   10      1600
-//  2012   11      1700
-//  2013   12      1800
-//  2015   14      1900
-//  "15"   15      1900
+//  name        ver   MSC_VER
+//  2008         9      1500
+//  2010        10      1600
+//  2012        11      1700
+//  2013        12      1800
+//  2015        14      1900
+//  "15"        15      1900
+//  2017-14.1   15.0    1910
+//  2017-14.11  15.3    1911
+//  2017-14.12  15.5    1912
+//  2017-14.13  15.6    1913
+//  2017-14.14  15.7    1914
+
+/// \internal EIGEN_COMP_MSVC_LANG set to _MSVC_LANG if the compiler is Microsoft Visual C++, 0 otherwise.
+#if defined(_MSVC_LANG)
+  #define EIGEN_COMP_MSVC_LANG _MSVC_LANG
+#else
+  #define EIGEN_COMP_MSVC_LANG 0
+#endif
+
+// For the record, here is a table summarizing the possible values for EIGEN_COMP_MSVC_LANG:
+// MSVC option                          Standard  MSVC_LANG
+// /std:c++14 (default as of VS 2019)   C++14     201402L
+// /std:c++17                           C++17     201703L
+// /std:c++latest                       >C++17    >201703L
 
 /// \internal EIGEN_COMP_MSVC_STRICT set to 1 if the compiler is really Microsoft Visual C++ and not ,e.g., ICC or clang-cl
 #if EIGEN_COMP_MSVC && !(EIGEN_COMP_ICC || EIGEN_COMP_LLVM || EIGEN_COMP_CLANG)
@@ -87,16 +160,21 @@
   #define EIGEN_COMP_MSVC_STRICT 0
 #endif
 
-/// \internal EIGEN_COMP_IBM set to 1 if the compiler is IBM XL C++
-#if defined(__IBMCPP__) || defined(__xlc__)
-  #define EIGEN_COMP_IBM 1
+/// \internal EIGEN_COMP_IBM set to xlc version if the compiler is IBM XL C++
+// XLC   version
+// 3.1   0x0301
+// 4.5   0x0405
+// 5.0   0x0500
+// 12.1  0x0C01
+#if defined(__IBMCPP__) || defined(__xlc__) || defined(__ibmxl__)
+  #define EIGEN_COMP_IBM __xlC__
 #else
   #define EIGEN_COMP_IBM 0
 #endif
 
-/// \internal EIGEN_COMP_PGI set to 1 if the compiler is Portland Group Compiler
+/// \internal EIGEN_COMP_PGI set to PGI version if the compiler is Portland Group Compiler
 #if defined(__PGI)
-  #define EIGEN_COMP_PGI 1
+  #define EIGEN_COMP_PGI (__PGIC__*100+__PGIC_MINOR__)
 #else
   #define EIGEN_COMP_PGI 0
 #endif
@@ -108,7 +186,7 @@
   #define EIGEN_COMP_ARM 0
 #endif
 
-/// \internal EIGEN_COMP_ARM set to 1 if the compiler is ARM Compiler
+/// \internal EIGEN_COMP_EMSCRIPTEN set to 1 if the compiler is Emscripten Compiler
 #if defined(__EMSCRIPTEN__)
   #define EIGEN_COMP_EMSCRIPTEN 1
 #else
@@ -142,9 +220,13 @@
 #endif
 
 
-// Architecture identification, EIGEN_ARCH_*
 
-#if defined(__x86_64__) || defined(_M_X64) || defined(__amd64)
+//------------------------------------------------------------------------------------------
+// Architecture identification, EIGEN_ARCH_*
+//------------------------------------------------------------------------------------------
+
+
+#if defined(__x86_64__) || (defined(_M_X64) && !defined(_M_ARM64EC)) || defined(__amd64)
   #define EIGEN_ARCH_x86_64 1
 #else
   #define EIGEN_ARCH_x86_64 0
@@ -170,18 +252,61 @@
 #endif
 
 /// \internal EIGEN_ARCH_ARM64 set to 1 if the architecture is ARM64
-#if defined(__aarch64__)
+#if defined(__aarch64__) || defined(_M_ARM64) || defined(_M_ARM64EC)
   #define EIGEN_ARCH_ARM64 1
 #else
   #define EIGEN_ARCH_ARM64 0
 #endif
 
+/// \internal EIGEN_ARCH_ARM_OR_ARM64 set to 1 if the architecture is ARM or ARM64
 #if EIGEN_ARCH_ARM || EIGEN_ARCH_ARM64
   #define EIGEN_ARCH_ARM_OR_ARM64 1
 #else
   #define EIGEN_ARCH_ARM_OR_ARM64 0
 #endif
 
+/// \internal EIGEN_ARCH_ARMV8 set to 1 if the architecture is armv8 or greater.
+#if EIGEN_ARCH_ARM_OR_ARM64 && defined(__ARM_ARCH) && __ARM_ARCH >= 8
+#define EIGEN_ARCH_ARMV8 1
+#else
+#define EIGEN_ARCH_ARMV8 0
+#endif
+
+
+/// \internal EIGEN_HAS_ARM64_FP16 set to 1 if the architecture provides an IEEE
+/// compliant Arm fp16 type
+#if EIGEN_ARCH_ARM64
+  #ifndef EIGEN_HAS_ARM64_FP16
+    #if defined(__ARM_FP16_FORMAT_IEEE)
+      #define EIGEN_HAS_ARM64_FP16 1
+    #else
+      #define EIGEN_HAS_ARM64_FP16 0
+    #endif
+  #endif
+#endif
+
+/// \internal EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC set to 1 if the architecture
+/// supports Neon vector intrinsics for fp16.
+#if EIGEN_ARCH_ARM64
+  #ifndef EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC
+    #if defined(__ARM_FEATURE_FP16_VECTOR_ARITHMETIC)
+      #define EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC 1
+    #else
+      #define EIGEN_HAS_ARM64_FP16_VECTOR_ARITHMETIC 0
+    #endif
+  #endif
+#endif
+
+/// \internal EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC set to 1 if the architecture
+/// supports Neon scalar intrinsics for fp16.
+#if EIGEN_ARCH_ARM64
+  #ifndef EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC
+    #if defined(__ARM_FEATURE_FP16_SCALAR_ARITHMETIC)
+      #define EIGEN_HAS_ARM64_FP16_SCALAR_ARITHMETIC 1
+    #endif
+  #endif
+#endif
+
 /// \internal EIGEN_ARCH_MIPS set to 1 if the architecture is MIPS
 #if defined(__mips__) || defined(__mips)
   #define EIGEN_ARCH_MIPS 1
@@ -212,7 +337,9 @@
 
 
 
+//------------------------------------------------------------------------------------------
 // Operating system identification, EIGEN_OS_*
+//------------------------------------------------------------------------------------------
 
 /// \internal EIGEN_OS_UNIX set to 1 if the OS is a unix variant
 #if defined(__unix__) || defined(__unix)
@@ -299,9 +426,17 @@
   #define EIGEN_OS_WIN_STRICT 0
 #endif
 
-/// \internal EIGEN_OS_SUN set to 1 if the OS is SUN
+/// \internal EIGEN_OS_SUN set to __SUNPRO_C if the OS is SUN
+// compiler  solaris   __SUNPRO_C
+// version   studio
+// 5.7       10        0x570
+// 5.8       11        0x580
+// 5.9       12        0x590
+// 5.10	     12.1      0x5100
+// 5.11	     12.2      0x5110
+// 5.12	     12.3      0x5120
 #if (defined(sun) || defined(__sun)) && !(defined(__SVR4) || defined(__svr4__))
-  #define EIGEN_OS_SUN 1
+  #define EIGEN_OS_SUN __SUNPRO_C
 #else
   #define EIGEN_OS_SUN 0
 #endif
@@ -314,6 +449,131 @@
 #endif
 
 
+//------------------------------------------------------------------------------------------
+// Detect GPU compilers and architectures
+//------------------------------------------------------------------------------------------
+
+// NVCC is not supported as the target platform for HIPCC
+// Note that this also makes EIGEN_CUDACC and EIGEN_HIPCC mutually exclusive
+#if defined(__NVCC__) && defined(__HIPCC__)
+  #error "NVCC as the target platform for HIPCC is currently not supported."
+#endif
+
+#if defined(__CUDACC__) && !defined(EIGEN_NO_CUDA)
+  // Means the compiler is either nvcc or clang with CUDA enabled
+  #define EIGEN_CUDACC __CUDACC__
+#endif
+
+#if defined(__CUDA_ARCH__) && !defined(EIGEN_NO_CUDA)
+  // Means we are generating code for the device
+  #define EIGEN_CUDA_ARCH __CUDA_ARCH__
+#endif
+
+#if defined(EIGEN_CUDACC)
+#include <cuda.h>
+  #define EIGEN_CUDA_SDK_VER (CUDA_VERSION * 10)
+#else
+  #define EIGEN_CUDA_SDK_VER 0
+#endif
+
+#if defined(__HIPCC__) && !defined(EIGEN_NO_HIP)
+  // Means the compiler is HIPCC (analogous to EIGEN_CUDACC, but for HIP)
+  #define EIGEN_HIPCC __HIPCC__
+
+  // We need to include hip_runtime.h here because it pulls in
+  // ++ hip_common.h which contains the define for  __HIP_DEVICE_COMPILE__
+  // ++ host_defines.h which contains the defines for the __host__ and __device__ macros
+  #include <hip/hip_runtime.h>
+
+  #if defined(__HIP_DEVICE_COMPILE__)
+    // analogous to EIGEN_CUDA_ARCH, but for HIP
+    #define EIGEN_HIP_DEVICE_COMPILE __HIP_DEVICE_COMPILE__
+  #endif
+
+  // For HIP (ROCm 3.5 and higher), we need to explicitly set the launch_bounds attribute
+  // value to 1024. The compiler assigns a default value of 256 when the attribute is not
+  // specified. This results in failures on the HIP platform, for cases when a GPU kernel
+  // without an explicit launch_bounds attribute is called with a threads_per_block value
+  // greater than 256.
+  //
+  // This is a regression in functioanlity and is expected to be fixed within the next
+  // couple of ROCm releases (compiler will go back to using 1024 value as the default)
+  //
+  // In the meantime, we will use a "only enabled for HIP" macro to set the launch_bounds
+  // attribute.
+
+  #define EIGEN_HIP_LAUNCH_BOUNDS_1024 __launch_bounds__(1024)
+
+#endif
+
+#if !defined(EIGEN_HIP_LAUNCH_BOUNDS_1024)
+#define EIGEN_HIP_LAUNCH_BOUNDS_1024
+#endif // !defined(EIGEN_HIP_LAUNCH_BOUNDS_1024)
+
+// Unify CUDA/HIPCC
+
+#if defined(EIGEN_CUDACC) || defined(EIGEN_HIPCC)
+//
+// If either EIGEN_CUDACC or EIGEN_HIPCC is defined, then define EIGEN_GPUCC
+//
+#define EIGEN_GPUCC
+//
+// EIGEN_HIPCC implies the HIP compiler and is used to tweak Eigen code for use in HIP kernels
+// EIGEN_CUDACC implies the CUDA compiler and is used to tweak Eigen code for use in CUDA kernels
+//
+// In most cases the same tweaks are required to the Eigen code to enable in both the HIP and CUDA kernels.
+// For those cases, the corresponding code should be guarded with
+//      #if defined(EIGEN_GPUCC)
+// instead of
+//      #if defined(EIGEN_CUDACC) || defined(EIGEN_HIPCC)
+//
+// For cases where the tweak is specific to HIP, the code should be guarded with
+//      #if defined(EIGEN_HIPCC)
+//
+// For cases where the tweak is specific to CUDA, the code should be guarded with
+//      #if defined(EIGEN_CUDACC)
+//
+#endif
+
+#if defined(EIGEN_CUDA_ARCH) || defined(EIGEN_HIP_DEVICE_COMPILE)
+//
+// If either EIGEN_CUDA_ARCH or EIGEN_HIP_DEVICE_COMPILE is defined, then define EIGEN_GPU_COMPILE_PHASE
+//
+#define EIGEN_GPU_COMPILE_PHASE
+//
+// GPU compilers (HIPCC, NVCC) typically do two passes over the source code,
+//   + one to compile the source for the "host" (ie CPU)
+//   + another to compile the source for the "device" (ie. GPU)
+//
+// Code that needs to enabled only during the either the "host" or "device" compilation phase
+// needs to be guarded with a macro that indicates the current compilation phase
+//
+// EIGEN_HIP_DEVICE_COMPILE implies the device compilation phase in HIP
+// EIGEN_CUDA_ARCH implies the device compilation phase in CUDA
+//
+// In most cases, the "host" / "device" specific code is the same for both HIP and CUDA
+// For those cases, the code should be guarded with
+//       #if defined(EIGEN_GPU_COMPILE_PHASE)
+// instead of
+//       #if defined(EIGEN_CUDA_ARCH) || defined(EIGEN_HIP_DEVICE_COMPILE)
+//
+// For cases where the tweak is specific to HIP, the code should be guarded with
+//      #if defined(EIGEN_HIP_DEVICE_COMPILE)
+//
+// For cases where the tweak is specific to CUDA, the code should be guarded with
+//      #if defined(EIGEN_CUDA_ARCH)
+//
+#endif
+
+#if defined(EIGEN_USE_SYCL) && defined(__SYCL_DEVICE_ONLY__)
+// EIGEN_USE_SYCL is a user-defined macro while __SYCL_DEVICE_ONLY__ is a compiler-defined macro.
+// In most cases we want to check if both macros are defined which can be done using the define below.
+#define SYCL_DEVICE_ONLY
+#endif
+
+//------------------------------------------------------------------------------------------
+// Detect Compiler/Architecture/OS specific features
+//------------------------------------------------------------------------------------------
 
 #if EIGEN_GNUC_AT_MOST(4,3) && !EIGEN_COMP_CLANG
   // see bug 89
@@ -322,20 +582,6 @@
   #define EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO 1
 #endif
 
-// This macro can be used to prevent from macro expansion, e.g.:
-//   std::max EIGEN_NOT_A_MACRO(a,b)
-#define EIGEN_NOT_A_MACRO
-
-#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
-#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::RowMajor
-#else
-#define EIGEN_DEFAULT_MATRIX_STORAGE_ORDER_OPTION Eigen::ColMajor
-#endif
-
-#ifndef EIGEN_DEFAULT_DENSE_INDEX_TYPE
-#define EIGEN_DEFAULT_DENSE_INDEX_TYPE std::ptrdiff_t
-#endif
-
 // Cross compiler wrapper around LLVM's __has_builtin
 #ifdef __has_builtin
 #  define EIGEN_HAS_BUILTIN(x) __has_builtin(x)
@@ -349,26 +595,79 @@
 # define __has_feature(x) 0
 #endif
 
-// Upperbound on the C++ version to use.
-// Expected values are 03, 11, 14, 17, etc.
-// By default, let's use an arbitrarily large C++ version.
-#ifndef EIGEN_MAX_CPP_VER
-#define EIGEN_MAX_CPP_VER 99
+// Some old compilers do not support template specializations like:
+// template<typename T,int N> void foo(const T x[N]);
+#if !(   EIGEN_COMP_CLANG && (   (EIGEN_COMP_CLANG<309)                                                       \
+                              || (defined(__apple_build_version__) && (__apple_build_version__ < 9000000)))  \
+      || EIGEN_COMP_GNUC_STRICT && EIGEN_COMP_GNUC<49)
+#define EIGEN_HAS_STATIC_ARRAY_TEMPLATE 1
+#else
+#define EIGEN_HAS_STATIC_ARRAY_TEMPLATE 0
 #endif
 
-#if EIGEN_MAX_CPP_VER>=11 && (defined(__cplusplus) && (__cplusplus >= 201103L) || EIGEN_COMP_MSVC >= 1900)
+// The macro EIGEN_CPLUSPLUS is a replacement for __cplusplus/_MSVC_LANG that
+// works for both platforms, indicating the C++ standard version number.
+//
+// With MSVC, without defining /Zc:__cplusplus, the __cplusplus macro will
+// report 199711L regardless of the language standard specified via /std.
+// We need to rely on _MSVC_LANG instead, which is only available after
+// VS2015.3.
+#if EIGEN_COMP_MSVC_LANG > 0
+#define EIGEN_CPLUSPLUS EIGEN_COMP_MSVC_LANG
+#elif EIGEN_COMP_MSVC >= 1900
+#define EIGEN_CPLUSPLUS 201103L
+#elif defined(__cplusplus)
+#define EIGEN_CPLUSPLUS __cplusplus
+#else
+#define EIGEN_CPLUSPLUS 0
+#endif
+
+// The macro EIGEN_COMP_CXXVER defines the c++ verson expected by the compiler.
+// For instance, if compiling with gcc and -std=c++17, then EIGEN_COMP_CXXVER
+// is defined to 17.
+#if EIGEN_CPLUSPLUS > 201703L
+  #define EIGEN_COMP_CXXVER 20
+#elif EIGEN_CPLUSPLUS > 201402L
+  #define EIGEN_COMP_CXXVER 17
+#elif EIGEN_CPLUSPLUS > 201103L
+  #define EIGEN_COMP_CXXVER 14
+#elif EIGEN_CPLUSPLUS >= 201103L
+  #define EIGEN_COMP_CXXVER 11
+#else
+  #define EIGEN_COMP_CXXVER 03
+#endif
+
+#ifndef EIGEN_HAS_CXX14_VARIABLE_TEMPLATES
+  #if defined(__cpp_variable_templates) && __cpp_variable_templates >= 201304 && EIGEN_MAX_CPP_VER>=14
+    #define EIGEN_HAS_CXX14_VARIABLE_TEMPLATES 1
+  #else
+    #define EIGEN_HAS_CXX14_VARIABLE_TEMPLATES 0
+  #endif
+#endif
+
+
+// The macros EIGEN_HAS_CXX?? defines a rough estimate of available c++ features
+// but in practice we should not rely on them but rather on the availabilty of
+// individual features as defined later.
+// This is why there is no EIGEN_HAS_CXX17.
+// FIXME: get rid of EIGEN_HAS_CXX14 and maybe even EIGEN_HAS_CXX11.
+#if EIGEN_MAX_CPP_VER>=11 && EIGEN_COMP_CXXVER>=11
 #define EIGEN_HAS_CXX11 1
 #else
 #define EIGEN_HAS_CXX11 0
 #endif
 
+#if EIGEN_MAX_CPP_VER>=14 && EIGEN_COMP_CXXVER>=14
+#define EIGEN_HAS_CXX14 1
+#else
+#define EIGEN_HAS_CXX14 0
+#endif
 
 // Do we support r-value references?
 #ifndef EIGEN_HAS_RVALUE_REFERENCES
 #if EIGEN_MAX_CPP_VER>=11 && \
     (__has_feature(cxx_rvalue_references) || \
-    (defined(__cplusplus) && __cplusplus >= 201103L) || \
-    (EIGEN_COMP_MSVC >= 1600))
+     (EIGEN_COMP_CXXVER >= 11) || (EIGEN_COMP_MSVC >= 1600))
   #define EIGEN_HAS_RVALUE_REFERENCES 1
 #else
   #define EIGEN_HAS_RVALUE_REFERENCES 0
@@ -376,11 +675,14 @@
 #endif
 
 // Does the compiler support C99?
+// Need to include <cmath> to make sure _GLIBCXX_USE_C99 gets defined
+#include <cmath>
 #ifndef EIGEN_HAS_C99_MATH
 #if EIGEN_MAX_CPP_VER>=11 && \
     ((defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901))       \
   || (defined(__GNUC__) && defined(_GLIBCXX_USE_C99)) \
-  || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)))
+  || (defined(_LIBCPP_VERSION) && !defined(_MSC_VER)) \
+  || (EIGEN_COMP_MSVC >= 1900) || defined(SYCL_DEVICE_ONLY))
   #define EIGEN_HAS_C99_MATH 1
 #else
   #define EIGEN_HAS_C99_MATH 0
@@ -388,21 +690,73 @@
 #endif
 
 // Does the compiler support result_of?
+// result_of was deprecated in c++17 and removed in c++ 20
 #ifndef EIGEN_HAS_STD_RESULT_OF
-#if EIGEN_MAX_CPP_VER>=11 && ((__has_feature(cxx_lambdas) || (defined(__cplusplus) && __cplusplus >= 201103L)))
+#if EIGEN_HAS_CXX11 && EIGEN_COMP_CXXVER < 17
 #define EIGEN_HAS_STD_RESULT_OF 1
 #else
 #define EIGEN_HAS_STD_RESULT_OF 0
 #endif
 #endif
 
+// Does the compiler support std::hash?
+#ifndef EIGEN_HAS_STD_HASH
+// The std::hash struct is defined in C++11 but is not labelled as a __device__
+// function and is not constexpr, so cannot be used on device.
+#if EIGEN_HAS_CXX11 && !defined(EIGEN_GPU_COMPILE_PHASE)
+#define EIGEN_HAS_STD_HASH 1
+#else
+#define EIGEN_HAS_STD_HASH 0
+#endif
+#endif  // EIGEN_HAS_STD_HASH
+
+#ifndef EIGEN_HAS_STD_INVOKE_RESULT
+#if EIGEN_MAX_CPP_VER >= 17 && EIGEN_COMP_CXXVER >= 17
+#define EIGEN_HAS_STD_INVOKE_RESULT 1
+#else
+#define EIGEN_HAS_STD_INVOKE_RESULT 0
+#endif
+#endif
+
+#ifndef EIGEN_HAS_ALIGNAS
+#if EIGEN_MAX_CPP_VER>=11 && EIGEN_HAS_CXX11 &&   \
+      (     __has_feature(cxx_alignas)            \
+        ||  EIGEN_HAS_CXX14                       \
+        || (EIGEN_COMP_MSVC >= 1800)              \
+        || (EIGEN_GNUC_AT_LEAST(4,8))             \
+        || (EIGEN_COMP_CLANG>=305)                \
+        || (EIGEN_COMP_ICC>=1500)                 \
+        || (EIGEN_COMP_PGI>=1500)                 \
+        || (EIGEN_COMP_SUNCC>=0x5130))
+#define EIGEN_HAS_ALIGNAS 1
+#else
+#define EIGEN_HAS_ALIGNAS 0
+#endif
+#endif
+
+// Does the compiler support type_traits?
+// - full support of type traits was added only to GCC 5.1.0.
+// - 20150626 corresponds to the last release of 4.x libstdc++
+#ifndef EIGEN_HAS_TYPE_TRAITS
+#if EIGEN_MAX_CPP_VER>=11 && (EIGEN_HAS_CXX11 || EIGEN_COMP_MSVC >= 1700) \
+  && ((!EIGEN_COMP_GNUC_STRICT) || EIGEN_GNUC_AT_LEAST(5, 1)) \
+  && ((!defined(__GLIBCXX__))   || __GLIBCXX__ > 20150626)
+#define EIGEN_HAS_TYPE_TRAITS 1
+#define EIGEN_INCLUDE_TYPE_TRAITS
+#else
+#define EIGEN_HAS_TYPE_TRAITS 0
+#endif
+#endif
+
 // Does the compiler support variadic templates?
 #ifndef EIGEN_HAS_VARIADIC_TEMPLATES
-#if EIGEN_MAX_CPP_VER>=11 && (__cplusplus > 199711L || EIGEN_COMP_MSVC >= 1900) \
-  && (!defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (EIGEN_CUDACC_VER >= 80000) )
+#if EIGEN_MAX_CPP_VER>=11 && (EIGEN_COMP_CXXVER >= 11) \
+  && (!defined(__NVCC__) || !EIGEN_ARCH_ARM_OR_ARM64 || (EIGEN_COMP_NVCC >= 80000) )
     // ^^ Disable the use of variadic templates when compiling with versions of nvcc older than 8.0 on ARM devices:
     //    this prevents nvcc from crashing when compiling Eigen on Tegra X1
 #define EIGEN_HAS_VARIADIC_TEMPLATES 1
+#elif  EIGEN_MAX_CPP_VER>=11 && (EIGEN_COMP_CXXVER >= 11) && defined(SYCL_DEVICE_ONLY)
+#define EIGEN_HAS_VARIADIC_TEMPLATES 1
 #else
 #define EIGEN_HAS_VARIADIC_TEMPLATES 0
 #endif
@@ -410,27 +764,33 @@
 
 // Does the compiler fully support const expressions? (as in c++14)
 #ifndef EIGEN_HAS_CONSTEXPR
+  #if defined(EIGEN_CUDACC)
+  // Const expressions are supported provided that c++11 is enabled and we're using either clang or nvcc 7.5 or above
+    #if EIGEN_MAX_CPP_VER>=14 && (EIGEN_COMP_CXXVER >= 11 && (EIGEN_COMP_CLANG || EIGEN_COMP_NVCC >= 70500))
+      #define EIGEN_HAS_CONSTEXPR 1
+    #endif
+  #elif EIGEN_MAX_CPP_VER>=14 && (__has_feature(cxx_relaxed_constexpr) || (EIGEN_COMP_CXXVER >= 14) || \
+    (EIGEN_GNUC_AT_LEAST(4,8) && (EIGEN_COMP_CXXVER >= 11)) || \
+    (EIGEN_COMP_CLANG >= 306 && (EIGEN_COMP_CXXVER >= 11)))
+    #define EIGEN_HAS_CONSTEXPR 1
+  #endif
 
-#ifdef __CUDACC__
-// Const expressions are supported provided that c++11 is enabled and we're using either clang or nvcc 7.5 or above
-#if EIGEN_MAX_CPP_VER>=14 && (__cplusplus > 199711L && (EIGEN_COMP_CLANG || EIGEN_CUDACC_VER >= 70500))
-  #define EIGEN_HAS_CONSTEXPR 1
-#endif
-#elif EIGEN_MAX_CPP_VER>=14 && (__has_feature(cxx_relaxed_constexpr) || (defined(__cplusplus) && __cplusplus >= 201402L) || \
-  (EIGEN_GNUC_AT_LEAST(4,8) && (__cplusplus > 199711L)))
-#define EIGEN_HAS_CONSTEXPR 1
-#endif
+  #ifndef EIGEN_HAS_CONSTEXPR
+    #define EIGEN_HAS_CONSTEXPR 0
+  #endif
 
-#ifndef EIGEN_HAS_CONSTEXPR
-#define EIGEN_HAS_CONSTEXPR 0
-#endif
+#endif // EIGEN_HAS_CONSTEXPR
 
+#if EIGEN_HAS_CONSTEXPR
+#define EIGEN_CONSTEXPR constexpr
+#else
+#define EIGEN_CONSTEXPR
 #endif
 
 // Does the compiler support C++11 math?
 // Let's be conservative and enable the default C++11 implementation only if we are sure it exists
 #ifndef EIGEN_HAS_CXX11_MATH
-  #if EIGEN_MAX_CPP_VER>=11 && ((__cplusplus > 201103L) || (__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC)  \
+  #if EIGEN_MAX_CPP_VER>=11 && ((EIGEN_COMP_CXXVER > 11) || (EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC)  \
       && (EIGEN_ARCH_i386_OR_x86_64) && (EIGEN_OS_GNULINUX || EIGEN_OS_WIN_STRICT || EIGEN_OS_MAC))
     #define EIGEN_HAS_CXX11_MATH 1
   #else
@@ -441,9 +801,8 @@
 // Does the compiler support proper C++11 containers?
 #ifndef EIGEN_HAS_CXX11_CONTAINERS
   #if    EIGEN_MAX_CPP_VER>=11 && \
-         ((__cplusplus > 201103L) \
-      || ((__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_ICC>=1400)) \
-      || EIGEN_COMP_MSVC >= 1900)
+         ((EIGEN_COMP_CXXVER > 11) \
+      || ((EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC>=1400)))
     #define EIGEN_HAS_CXX11_CONTAINERS 1
   #else
     #define EIGEN_HAS_CXX11_CONTAINERS 0
@@ -454,24 +813,88 @@
 #ifndef EIGEN_HAS_CXX11_NOEXCEPT
   #if    EIGEN_MAX_CPP_VER>=11 && \
          (__has_feature(cxx_noexcept) \
-      || (__cplusplus > 201103L) \
-      || ((__cplusplus >= 201103L) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_ICC>=1400)) \
-      || EIGEN_COMP_MSVC >= 1900)
+      || (EIGEN_COMP_CXXVER > 11) \
+      || ((EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_GNUC_STRICT || EIGEN_COMP_CLANG || EIGEN_COMP_MSVC || EIGEN_COMP_ICC>=1400)))
     #define EIGEN_HAS_CXX11_NOEXCEPT 1
   #else
     #define EIGEN_HAS_CXX11_NOEXCEPT 0
   #endif
 #endif
 
-/** Allows to disable some optimizations which might affect the accuracy of the result.
-  * Such optimization are enabled by default, and set EIGEN_FAST_MATH to 0 to disable them.
-  * They currently include:
-  *   - single precision ArrayBase::sin() and ArrayBase::cos() for SSE and AVX vectorization.
-  */
-#ifndef EIGEN_FAST_MATH
-#define EIGEN_FAST_MATH 1
+#ifndef EIGEN_HAS_CXX11_ATOMIC
+  #if    EIGEN_MAX_CPP_VER>=11 && \
+         (__has_feature(cxx_atomic) \
+      || (EIGEN_COMP_CXXVER > 11) \
+      || ((EIGEN_COMP_CXXVER == 11) && (EIGEN_COMP_MSVC==0 || EIGEN_COMP_MSVC >= 1700)))
+    #define EIGEN_HAS_CXX11_ATOMIC 1
+  #else
+    #define EIGEN_HAS_CXX11_ATOMIC 0
+  #endif
 #endif
 
+#ifndef EIGEN_HAS_CXX11_OVERRIDE_FINAL
+  #if    EIGEN_MAX_CPP_VER>=11 && \
+       (EIGEN_COMP_CXXVER >= 11 || EIGEN_COMP_MSVC >= 1700)
+    #define EIGEN_HAS_CXX11_OVERRIDE_FINAL 1
+  #else
+    #define EIGEN_HAS_CXX11_OVERRIDE_FINAL 0
+  #endif
+#endif
+
+// NOTE: the required Apple's clang version is very conservative
+//       and it could be that XCode 9 works just fine.
+// NOTE: the MSVC version is based on https://en.cppreference.com/w/cpp/compiler_support
+//       and not tested.
+#ifndef EIGEN_HAS_CXX17_OVERALIGN
+#if EIGEN_MAX_CPP_VER>=17 && EIGEN_COMP_CXXVER>=17 && (                                 \
+           (EIGEN_COMP_MSVC >= 1912)                                                    \
+        || (EIGEN_GNUC_AT_LEAST(7,0))                                                   \
+        || ((!defined(__apple_build_version__)) && (EIGEN_COMP_CLANG>=500))             \
+        || (( defined(__apple_build_version__)) && (__apple_build_version__>=10000000)) \
+      )
+#define EIGEN_HAS_CXX17_OVERALIGN 1
+#else
+#define EIGEN_HAS_CXX17_OVERALIGN 0
+#endif
+#endif
+
+#if defined(EIGEN_CUDACC) && EIGEN_HAS_CONSTEXPR
+  // While available already with c++11, this is useful mostly starting with c++14 and relaxed constexpr rules
+  #if defined(__NVCC__)
+    // nvcc considers constexpr functions as __host__ __device__ with the option --expt-relaxed-constexpr
+    #ifdef __CUDACC_RELAXED_CONSTEXPR__
+      #define EIGEN_CONSTEXPR_ARE_DEVICE_FUNC
+    #endif
+  #elif defined(__clang__) && defined(__CUDA__) && __has_feature(cxx_relaxed_constexpr)
+    // clang++ always considers constexpr functions as implicitly __host__ __device__
+    #define EIGEN_CONSTEXPR_ARE_DEVICE_FUNC
+  #endif
+#endif
+
+// Does the compiler support the __int128 and __uint128_t extensions for 128-bit
+// integer arithmetic?
+//
+// Clang and GCC define __SIZEOF_INT128__ when these extensions are supported,
+// but we avoid using them in certain cases:
+//
+// * Building using Clang for Windows, where the Clang runtime library has
+//   128-bit support only on LP64 architectures, but Windows is LLP64.
+#ifndef EIGEN_HAS_BUILTIN_INT128
+#if defined(__SIZEOF_INT128__) && !(EIGEN_OS_WIN && EIGEN_COMP_CLANG)
+#define EIGEN_HAS_BUILTIN_INT128 1
+#else
+#define EIGEN_HAS_BUILTIN_INT128 0
+#endif
+#endif
+
+//------------------------------------------------------------------------------------------
+// Preprocessor programming helpers
+//------------------------------------------------------------------------------------------
+
+// This macro can be used to prevent from macro expansion, e.g.:
+//   std::max EIGEN_NOT_A_MACRO(a,b)
+#define EIGEN_NOT_A_MACRO
+
 #define EIGEN_DEBUG_VAR(x) std::cerr << #x << " = " << x << std::endl;
 
 // concatenate two tokens
@@ -488,7 +911,7 @@
 // but it still doesn't use GCC's always_inline. This is useful in (common) situations where MSVC needs forceinline
 // but GCC is still doing fine with just inline.
 #ifndef EIGEN_STRONG_INLINE
-#if EIGEN_COMP_MSVC || EIGEN_COMP_ICC
+#if (EIGEN_COMP_MSVC || EIGEN_COMP_ICC) && !defined(EIGEN_GPUCC)
 #define EIGEN_STRONG_INLINE __forceinline
 #else
 #define EIGEN_STRONG_INLINE inline
@@ -503,7 +926,7 @@
 //   Eval.h:91: sorry, unimplemented: inlining failed in call to 'const Eigen::Eval<Derived> Eigen::MatrixBase<Scalar, Derived>::eval() const'
 //    : function body not available
 //   See also bug 1367
-#if EIGEN_GNUC_AT_LEAST(4,2)
+#if EIGEN_GNUC_AT_LEAST(4,2) && !defined(SYCL_DEVICE_ONLY)
 #define EIGEN_ALWAYS_INLINE __attribute__((always_inline)) inline
 #else
 #define EIGEN_ALWAYS_INLINE EIGEN_STRONG_INLINE
@@ -523,12 +946,43 @@
 #define EIGEN_PERMISSIVE_EXPR
 #endif
 
+// GPU stuff
+
+// Disable some features when compiling with GPU compilers (NVCC/clang-cuda/SYCL/HIPCC)
+#if defined(EIGEN_CUDACC) || defined(SYCL_DEVICE_ONLY) || defined(EIGEN_HIPCC)
+  // Do not try asserts on device code
+  #ifndef EIGEN_NO_DEBUG
+  #define EIGEN_NO_DEBUG
+  #endif
+
+  #ifdef EIGEN_INTERNAL_DEBUGGING
+  #undef EIGEN_INTERNAL_DEBUGGING
+  #endif
+
+  #ifdef EIGEN_EXCEPTIONS
+  #undef EIGEN_EXCEPTIONS
+  #endif
+#endif
+
+#if defined(SYCL_DEVICE_ONLY)
+  #ifndef EIGEN_DONT_VECTORIZE
+    #define EIGEN_DONT_VECTORIZE
+  #endif
+  #define EIGEN_DEVICE_FUNC __attribute__((flatten)) __attribute__((always_inline))
+// All functions callable from CUDA/HIP code must be qualified with __device__
+#elif defined(EIGEN_GPUCC)
+    #define EIGEN_DEVICE_FUNC __host__ __device__
+#else
+  #define EIGEN_DEVICE_FUNC
+#endif
+
+
 // this macro allows to get rid of linking errors about multiply defined functions.
 //  - static is not very good because it prevents definitions from different object files to be merged.
 //           So static causes the resulting linked executable to be bloated with multiple copies of the same function.
 //  - inline is not perfect either as it unwantedly hints the compiler toward inlining the function.
-#define EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS
-#define EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS inline
+#define EIGEN_DECLARE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC
+#define EIGEN_DEFINE_FUNCTION_ALLOWING_MULTIPLE_DEFINITIONS EIGEN_DEVICE_FUNC inline
 
 #ifdef NDEBUG
 # ifndef EIGEN_NO_DEBUG
@@ -538,7 +992,11 @@
 
 // eigen_plain_assert is where we implement the workaround for the assert() bug in GCC <= 4.3, see bug 89
 #ifdef EIGEN_NO_DEBUG
-  #define eigen_plain_assert(x)
+  #ifdef SYCL_DEVICE_ONLY // used to silence the warning on SYCL device
+    #define eigen_plain_assert(x) EIGEN_UNUSED_VARIABLE(x)
+  #else
+    #define eigen_plain_assert(x)
+  #endif
 #else
   #if EIGEN_SAFE_TO_USE_STANDARD_ASSERT_MACRO
     namespace Eigen {
@@ -612,7 +1070,7 @@
 // Suppresses 'unused variable' warnings.
 namespace Eigen {
   namespace internal {
-    template<typename T> EIGEN_DEVICE_FUNC void ignore_unused_variable(const T&) {}
+    template<typename T> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void ignore_unused_variable(const T&) {}
   }
 }
 #define EIGEN_UNUSED_VARIABLE(var) Eigen::internal::ignore_unused_variable(var);
@@ -626,169 +1084,75 @@
 #endif
 
 
-//------------------------------------------------------------------------------------------
-// Static and dynamic alignment control
+// Acts as a barrier preventing operations involving `X` from crossing. This
+// occurs, for example, in the fast rounding trick where a magic constant is
+// added then subtracted, which is otherwise compiled away with -ffast-math.
 //
-// The main purpose of this section is to define EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES
-// as the maximal boundary in bytes on which dynamically and statically allocated data may be alignment respectively.
-// The values of EIGEN_MAX_ALIGN_BYTES and EIGEN_MAX_STATIC_ALIGN_BYTES can be specified by the user. If not,
-// a default value is automatically computed based on architecture, compiler, and OS.
-//
-// This section also defines macros EIGEN_ALIGN_TO_BOUNDARY(N) and the shortcuts EIGEN_ALIGN{8,16,32,_MAX}
-// to be used to declare statically aligned buffers.
-//------------------------------------------------------------------------------------------
-
-
-/* EIGEN_ALIGN_TO_BOUNDARY(n) forces data to be n-byte aligned. This is used to satisfy SIMD requirements.
- * However, we do that EVEN if vectorization (EIGEN_VECTORIZE) is disabled,
- * so that vectorization doesn't affect binary compatibility.
- *
- * If we made alignment depend on whether or not EIGEN_VECTORIZE is defined, it would be impossible to link
- * vectorized and non-vectorized code.
- */
-#if (defined __CUDACC__)
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __align__(n)
-#elif EIGEN_COMP_GNUC || EIGEN_COMP_PGI || EIGEN_COMP_IBM || EIGEN_COMP_ARM
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
-#elif EIGEN_COMP_MSVC
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __declspec(align(n))
-#elif EIGEN_COMP_SUNCC
-  // FIXME not sure about this one:
-  #define EIGEN_ALIGN_TO_BOUNDARY(n) __attribute__((aligned(n)))
-#else
-  #error Please tell me what is the equivalent of __attribute__((aligned(n))) for your compiler
-#endif
-
-// If the user explicitly disable vectorization, then we also disable alignment
-#if defined(EIGEN_DONT_VECTORIZE)
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 0
-#elif defined(EIGEN_VECTORIZE_AVX512)
-  // 64 bytes static alignmeent is preferred only if really required
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 64
-#elif defined(__AVX__)
-  // 32 bytes static alignmeent is preferred only if really required
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 32
-#else
-  #define EIGEN_IDEAL_MAX_ALIGN_BYTES 16
-#endif
-
-
-// EIGEN_MIN_ALIGN_BYTES defines the minimal value for which the notion of explicit alignment makes sense
-#define EIGEN_MIN_ALIGN_BYTES 16
-
-// Defined the boundary (in bytes) on which the data needs to be aligned. Note
-// that unless EIGEN_ALIGN is defined and not equal to 0, the data may not be
-// aligned at all regardless of the value of this #define.
-
-#if (defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN))  && defined(EIGEN_MAX_STATIC_ALIGN_BYTES) && EIGEN_MAX_STATIC_ALIGN_BYTES>0
-#error EIGEN_MAX_STATIC_ALIGN_BYTES and EIGEN_DONT_ALIGN[_STATICALLY] are both defined with EIGEN_MAX_STATIC_ALIGN_BYTES!=0. Use EIGEN_MAX_STATIC_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN_STATICALLY.
-#endif
-
-// EIGEN_DONT_ALIGN_STATICALLY and EIGEN_DONT_ALIGN are deprectated
-// They imply EIGEN_MAX_STATIC_ALIGN_BYTES=0
-#if defined(EIGEN_DONT_ALIGN_STATICALLY) || defined(EIGEN_DONT_ALIGN)
-  #ifdef EIGEN_MAX_STATIC_ALIGN_BYTES
-    #undef EIGEN_MAX_STATIC_ALIGN_BYTES
-  #endif
-  #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
-#endif
-
-#ifndef EIGEN_MAX_STATIC_ALIGN_BYTES
-
-  // Try to automatically guess what is the best default value for EIGEN_MAX_STATIC_ALIGN_BYTES
-
-  // 16 byte alignment is only useful for vectorization. Since it affects the ABI, we need to enable
-  // 16 byte alignment on all platforms where vectorization might be enabled. In theory we could always
-  // enable alignment, but it can be a cause of problems on some platforms, so we just disable it in
-  // certain common platform (compiler+architecture combinations) to avoid these problems.
-  // Only static alignment is really problematic (relies on nonstandard compiler extensions),
-  // try to keep heap alignment even when we have to disable static alignment.
-  #if EIGEN_COMP_GNUC && !(EIGEN_ARCH_i386_OR_x86_64 || EIGEN_ARCH_ARM_OR_ARM64 || EIGEN_ARCH_PPC || EIGEN_ARCH_IA64)
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
-  #elif EIGEN_ARCH_ARM_OR_ARM64 && EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_MOST(4, 6)
-  // Old versions of GCC on ARM, at least 4.4, were once seen to have buggy static alignment support.
-  // Not sure which version fixed it, hopefully it doesn't affect 4.7, which is still somewhat in use.
-  // 4.8 and newer seem definitely unaffected.
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 1
+// See bug 1674
+#if !defined(EIGEN_OPTIMIZATION_BARRIER)
+  #if EIGEN_COMP_GNUC
+    // According to https://gcc.gnu.org/onlinedocs/gcc/Constraints.html:
+    //   X: Any operand whatsoever.
+    //   r: A register operand is allowed provided that it is in a general
+    //      register.
+    //   g: Any register, memory or immediate integer operand is allowed, except
+    //      for registers that are not general registers.
+    //   w: (AArch32/AArch64) Floating point register, Advanced SIMD vector
+    //      register or SVE vector register.
+    //   x: (SSE) Any SSE register.
+    //      (AArch64) Like w, but restricted to registers 0 to 15 inclusive.
+    //   v: (PowerPC) An Altivec vector register.
+    //   wa:(PowerPC) A VSX register.
+    //
+    // "X" (uppercase) should work for all cases, though this seems to fail for
+    // some versions of GCC for arm/aarch64 with
+    //   "error: inconsistent operand constraints in an 'asm'"
+    // Clang x86_64/arm/aarch64 seems to require "g" to support both scalars and
+    // vectors, otherwise
+    //   "error: non-trivial scalar-to-vector conversion, possible invalid
+    //    constraint for vector type"
+    //
+    // GCC for ppc64le generates an internal compiler error with x/X/g.
+    // GCC for AVX generates an internal compiler error with X.
+    //
+    // Tested on icc/gcc/clang for sse, avx, avx2, avx512dq
+    //           gcc for arm, aarch64,
+    //           gcc for ppc64le,
+    // both vectors and scalars.
+    //
+    // Note that this is restricted to plain types - this will not work
+    // directly for std::complex<T>, Eigen::half, Eigen::bfloat16. For these,
+    // you will need to apply to the underlying POD type.
+    #if EIGEN_ARCH_PPC && EIGEN_COMP_GNUC_STRICT
+      // This seems to be broken on clang.  Packet4f is loaded into a single
+      //   register rather than a vector, zeroing out some entries.  Integer
+      //   types also generate a compile error.
+      // General, Altivec, VSX.
+      #define EIGEN_OPTIMIZATION_BARRIER(X)  __asm__  ("" : "+r,v,wa" (X));
+    #elif EIGEN_ARCH_ARM_OR_ARM64
+      // General, NEON.
+      #define EIGEN_OPTIMIZATION_BARRIER(X)  __asm__  ("" : "+g,w" (X));
+    #elif EIGEN_ARCH_i386_OR_x86_64
+      // General, SSE.
+      #define EIGEN_OPTIMIZATION_BARRIER(X)  __asm__  ("" : "+g,x" (X));
+    #else
+      // Not implemented for other architectures.
+      #define EIGEN_OPTIMIZATION_BARRIER(X)
+    #endif
   #else
-  #define EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT 0
+    // Not implemented for other compilers.
+    #define EIGEN_OPTIMIZATION_BARRIER(X)
   #endif
-
-  // static alignment is completely disabled with GCC 3, Sun Studio, and QCC/QNX
-  #if !EIGEN_GCC_AND_ARCH_DOESNT_WANT_STACK_ALIGNMENT \
-  && !EIGEN_GCC3_OR_OLDER \
-  && !EIGEN_COMP_SUNCC \
-  && !EIGEN_OS_QNX
-    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 1
-  #else
-    #define EIGEN_ARCH_WANTS_STACK_ALIGNMENT 0
-  #endif
-
-  #if EIGEN_ARCH_WANTS_STACK_ALIGNMENT
-    #define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-  #else
-    #define EIGEN_MAX_STATIC_ALIGN_BYTES 0
-  #endif
-
 #endif
 
-// If EIGEN_MAX_ALIGN_BYTES is defined, then it is considered as an upper bound for EIGEN_MAX_ALIGN_BYTES
-#if defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES<EIGEN_MAX_STATIC_ALIGN_BYTES
-#undef EIGEN_MAX_STATIC_ALIGN_BYTES
-#define EIGEN_MAX_STATIC_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
-#endif
-
-#if EIGEN_MAX_STATIC_ALIGN_BYTES==0 && !defined(EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT)
-  #define EIGEN_DISABLE_UNALIGNED_ARRAY_ASSERT
-#endif
-
-// At this stage, EIGEN_MAX_STATIC_ALIGN_BYTES>0 is the true test whether we want to align arrays on the stack or not.
-// It takes into account both the user choice to explicitly enable/disable alignment (by settting EIGEN_MAX_STATIC_ALIGN_BYTES)
-// and the architecture config (EIGEN_ARCH_WANTS_STACK_ALIGNMENT).
-// Henceforth, only EIGEN_MAX_STATIC_ALIGN_BYTES should be used.
-
-
-// Shortcuts to EIGEN_ALIGN_TO_BOUNDARY
-#define EIGEN_ALIGN8  EIGEN_ALIGN_TO_BOUNDARY(8)
-#define EIGEN_ALIGN16 EIGEN_ALIGN_TO_BOUNDARY(16)
-#define EIGEN_ALIGN32 EIGEN_ALIGN_TO_BOUNDARY(32)
-#define EIGEN_ALIGN64 EIGEN_ALIGN_TO_BOUNDARY(64)
-#if EIGEN_MAX_STATIC_ALIGN_BYTES>0
-#define EIGEN_ALIGN_MAX EIGEN_ALIGN_TO_BOUNDARY(EIGEN_MAX_STATIC_ALIGN_BYTES)
+#if EIGEN_COMP_MSVC
+  // NOTE MSVC often gives C4127 warnings with compiletime if statements. See bug 1362.
+  // This workaround is ugly, but it does the job.
+#  define EIGEN_CONST_CONDITIONAL(cond)  (void)0, cond
 #else
-#define EIGEN_ALIGN_MAX
+#  define EIGEN_CONST_CONDITIONAL(cond)  cond
 #endif
 
-
-// Dynamic alignment control
-
-#if defined(EIGEN_DONT_ALIGN) && defined(EIGEN_MAX_ALIGN_BYTES) && EIGEN_MAX_ALIGN_BYTES>0
-#error EIGEN_MAX_ALIGN_BYTES and EIGEN_DONT_ALIGN are both defined with EIGEN_MAX_ALIGN_BYTES!=0. Use EIGEN_MAX_ALIGN_BYTES=0 as a synonym of EIGEN_DONT_ALIGN.
-#endif
-
-#ifdef EIGEN_DONT_ALIGN
-  #ifdef EIGEN_MAX_ALIGN_BYTES
-    #undef EIGEN_MAX_ALIGN_BYTES
-  #endif
-  #define EIGEN_MAX_ALIGN_BYTES 0
-#elif !defined(EIGEN_MAX_ALIGN_BYTES)
-  #define EIGEN_MAX_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-#endif
-
-#if EIGEN_IDEAL_MAX_ALIGN_BYTES > EIGEN_MAX_ALIGN_BYTES
-#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_IDEAL_MAX_ALIGN_BYTES
-#else
-#define EIGEN_DEFAULT_ALIGN_BYTES EIGEN_MAX_ALIGN_BYTES
-#endif
-
-
-#ifndef EIGEN_UNALIGNED_VECTORIZE
-#define EIGEN_UNALIGNED_VECTORIZE 1
-#endif
-
-//----------------------------------------------------------------------
-
-
 #ifdef EIGEN_DONT_USE_RESTRICT_KEYWORD
   #define EIGEN_RESTRICT
 #endif
@@ -796,10 +1160,6 @@
   #define EIGEN_RESTRICT __restrict
 #endif
 
-#ifndef EIGEN_STACK_ALLOCATION_LIMIT
-// 131072 == 128 KB
-#define EIGEN_STACK_ALLOCATION_LIMIT 131072
-#endif
 
 #ifndef EIGEN_DEFAULT_IO_FORMAT
 #ifdef EIGEN_MAKING_DOCS
@@ -814,8 +1174,23 @@
 // just an empty macro !
 #define EIGEN_EMPTY
 
-#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 || EIGEN_CUDACC_VER>0)
-  // for older MSVC versions, as well as 1900 && CUDA 8, using the base operator is sufficient (cf Bugs 1000, 1324)
+
+// When compiling CUDA/HIP device code with NVCC or HIPCC
+// pull in math functions from the global namespace.
+// In host mode, and when device code is compiled with clang,
+// use the std versions.
+#if (defined(EIGEN_CUDA_ARCH) && defined(__NVCC__)) || defined(EIGEN_HIP_DEVICE_COMPILE)
+  #define EIGEN_USING_STD(FUNC) using ::FUNC;
+#else
+  #define EIGEN_USING_STD(FUNC) using std::FUNC;
+#endif
+
+#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC < 1900 || (EIGEN_COMP_MSVC == 1900 && EIGEN_COMP_NVCC))
+  // For older MSVC versions, as well as 1900 && CUDA 8, using the base operator is necessary,
+  //   otherwise we get duplicate definition errors
+  // For later MSVC versions, we require explicit operator= definition, otherwise we get
+  //   use of implicitly deleted operator errors.
+  // (cf Bugs 920, 1000, 1324, 2291)
   #define EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Derived) \
     using Base::operator =;
 #elif EIGEN_COMP_CLANG // workaround clang bug (see http://forum.kde.org/viewtopic.php?f=74&t=102653)
@@ -841,12 +1216,13 @@
  * This is necessary, because the implicit definition is deprecated if the copy-assignment is overridden.
  */
 #if EIGEN_HAS_CXX11
-#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) EIGEN_DEVICE_FUNC CLASS(const CLASS&) = default;
+#define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS) CLASS(const CLASS&) = default;
 #else
 #define EIGEN_DEFAULT_COPY_CONSTRUCTOR(CLASS)
 #endif
 
 
+
 /** \internal
  * \brief Macro to manually inherit assignment operators.
  * This is necessary, because the implicitly defined assignment operator gets deleted when a custom operator= is defined.
@@ -865,15 +1241,18 @@
  */
 #if EIGEN_HAS_CXX11
 #define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived)  \
-    EIGEN_DEVICE_FUNC Derived() = default; \
-    EIGEN_DEVICE_FUNC ~Derived() = default;
+    Derived() = default; \
+    ~Derived() = default;
 #else
 #define EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(Derived)  \
-    EIGEN_DEVICE_FUNC Derived() {}; \
-    /* EIGEN_DEVICE_FUNC ~Derived() {}; */
+    Derived() {}; \
+    /* ~Derived() {}; */
 #endif
 
 
+
+
+
 /**
 * Just a side note. Commenting within defines works only by documenting
 * behind the object (via '!<'). Comments cannot be multi-line and thus
@@ -889,7 +1268,8 @@
   typedef typename Eigen::internal::ref_selector<Derived>::type Nested; \
   typedef typename Eigen::internal::traits<Derived>::StorageKind StorageKind; \
   typedef typename Eigen::internal::traits<Derived>::StorageIndex StorageIndex; \
-  enum { RowsAtCompileTime = Eigen::internal::traits<Derived>::RowsAtCompileTime, \
+  enum CompileTimeTraits \
+      { RowsAtCompileTime = Eigen::internal::traits<Derived>::RowsAtCompileTime, \
         ColsAtCompileTime = Eigen::internal::traits<Derived>::ColsAtCompileTime, \
         Flags = Eigen::internal::traits<Derived>::Flags, \
         SizeAtCompileTime = Base::SizeAtCompileTime, \
@@ -934,6 +1314,14 @@
 
 #define EIGEN_IMPLIES(a,b) (!(a) || (b))
 
+#if EIGEN_HAS_BUILTIN(__builtin_expect) || EIGEN_COMP_GNUC
+#define EIGEN_PREDICT_FALSE(x) (__builtin_expect(x, false))
+#define EIGEN_PREDICT_TRUE(x) (__builtin_expect(false || (x), true))
+#else
+#define EIGEN_PREDICT_FALSE(x) (x)
+#define EIGEN_PREDICT_TRUE(x) (x)
+#endif
+
 // the expression type of a standard coefficient wise binary operation
 #define EIGEN_CWISE_BINARY_RETURN_TYPE(LHS,RHS,OPNAME) \
     CwiseBinaryOp< \
@@ -965,14 +1353,14 @@
                 const typename internal::plain_constant_type<EXPR,SCALAR>::type, const EXPR>
 
 // Workaround for MSVC 2010 (see ML thread "patch with compile for for MSVC 2010")
-#if EIGEN_COMP_MSVC_STRICT<=1600
+#if EIGEN_COMP_MSVC_STRICT && (EIGEN_COMP_MSVC_STRICT<=1600)
 #define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) typename internal::enable_if<true,X>::type
 #else
 #define EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(X) X
 #endif
 
 #define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME) \
-  template <typename T> EIGEN_DEVICE_FUNC inline \
+  template <typename T> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE \
   EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(Derived,typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type,OPNAME))\
   (METHOD)(const T& scalar) const { \
     typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,Scalar,T)>::type PromotedT; \
@@ -981,7 +1369,7 @@
   }
 
 #define EIGEN_MAKE_SCALAR_BINARY_OP_ONTHELEFT(METHOD,OPNAME) \
-  template <typename T> EIGEN_DEVICE_FUNC inline friend \
+  template <typename T> EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE friend \
   EIGEN_MSVC10_WORKAROUND_BINARYOP_RETURN_TYPE(const EIGEN_SCALAR_BINARYOP_EXPR_RETURN_TYPE(typename internal::promote_scalar_arg<Scalar EIGEN_COMMA T EIGEN_COMMA EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type,Derived,OPNAME)) \
   (METHOD)(const T& scalar, const StorageBaseType& matrix) { \
     typedef typename internal::promote_scalar_arg<Scalar,T,EIGEN_SCALAR_BINARY_SUPPORTED(OPNAME,T,Scalar)>::type PromotedT; \
@@ -994,15 +1382,23 @@
   EIGEN_MAKE_SCALAR_BINARY_OP_ONTHERIGHT(METHOD,OPNAME)
 
 
+#if (defined(_CPPUNWIND) || defined(__EXCEPTIONS)) && !defined(EIGEN_CUDA_ARCH) && !defined(EIGEN_EXCEPTIONS) && !defined(EIGEN_USE_SYCL) && !defined(EIGEN_HIP_DEVICE_COMPILE)
+  #define EIGEN_EXCEPTIONS
+#endif
+
+
 #ifdef EIGEN_EXCEPTIONS
 #  define EIGEN_THROW_X(X) throw X
 #  define EIGEN_THROW throw
 #  define EIGEN_TRY try
 #  define EIGEN_CATCH(X) catch (X)
 #else
-#  ifdef __CUDA_ARCH__
+#  if defined(EIGEN_CUDA_ARCH)
 #    define EIGEN_THROW_X(X) asm("trap;")
 #    define EIGEN_THROW asm("trap;")
+#  elif defined(EIGEN_HIP_DEVICE_COMPILE)
+#    define EIGEN_THROW_X(X) asm("s_trap 0")
+#    define EIGEN_THROW asm("s_trap 0")
 #  else
 #    define EIGEN_THROW_X(X) std::abort()
 #    define EIGEN_THROW std::abort()
@@ -1022,13 +1418,47 @@
 #   define EIGEN_NOEXCEPT
 #   define EIGEN_NOEXCEPT_IF(x)
 #   define EIGEN_NO_THROW throw()
-#   if EIGEN_COMP_MSVC
+#   if EIGEN_COMP_MSVC || EIGEN_COMP_CXXVER>=17
       // MSVC does not support exception specifications (warning C4290),
-      // and they are deprecated in c++11 anyway.
+      // and they are deprecated in c++11 anyway. This is even an error in c++17.
 #     define EIGEN_EXCEPTION_SPEC(X) throw()
 #   else
 #     define EIGEN_EXCEPTION_SPEC(X) throw(X)
 #   endif
 #endif
 
+#if EIGEN_HAS_VARIADIC_TEMPLATES
+// The all function is used to enable a variadic version of eigen_assert which can take a parameter pack as its input.
+namespace Eigen {
+namespace internal {
+
+inline bool all(){ return true; }
+
+template<typename T, typename ...Ts>
+bool all(T t, Ts ... ts){ return t && all(ts...); }
+
+}
+}
+#endif
+
+#if EIGEN_HAS_CXX11_OVERRIDE_FINAL
+// provide override and final specifiers if they are available:
+#   define EIGEN_OVERRIDE override
+#   define EIGEN_FINAL final
+#else
+#   define EIGEN_OVERRIDE
+#   define EIGEN_FINAL
+#endif
+
+// Wrapping #pragma unroll in a macro since it is required for SYCL
+#if defined(SYCL_DEVICE_ONLY)
+  #if defined(_MSC_VER)
+    #define EIGEN_UNROLL_LOOP __pragma(unroll)
+  #else
+    #define EIGEN_UNROLL_LOOP _Pragma("unroll")
+  #endif
+#else
+  #define EIGEN_UNROLL_LOOP
+#endif
+
 #endif // EIGEN_MACROS_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
index 291383c..875318c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Memory.h
@@ -63,14 +63,28 @@
 
 namespace internal {
 
-EIGEN_DEVICE_FUNC 
+EIGEN_DEVICE_FUNC
 inline void throw_std_bad_alloc()
 {
   #ifdef EIGEN_EXCEPTIONS
     throw std::bad_alloc();
   #else
     std::size_t huge = static_cast<std::size_t>(-1);
-    ::operator new(huge);
+    #if defined(EIGEN_HIPCC)
+    //
+    // calls to "::operator new" are to be treated as opaque function calls (i.e no inlining),
+    // and as a consequence the code in the #else block triggers the hipcc warning :
+    // "no overloaded function has restriction specifiers that are compatible with the ambient context"
+    //
+    // "throw_std_bad_alloc" has the EIGEN_DEVICE_FUNC attribute, so it seems that hipcc expects
+    // the same on "operator new"
+    // Reverting code back to the old version in this #if block for the hipcc compiler
+    //
+    new int[huge];
+    #else
+    void* unused = ::operator new(huge);
+    EIGEN_UNUSED_VARIABLE(unused);
+    #endif
   #endif
 }
 
@@ -83,19 +97,26 @@
 /** \internal Like malloc, but the returned pointer is guaranteed to be 16-byte aligned.
   * Fast, but wastes 16 additional bytes of memory. Does not throw any exception.
   */
-inline void* handmade_aligned_malloc(std::size_t size)
+EIGEN_DEVICE_FUNC inline void* handmade_aligned_malloc(std::size_t size, std::size_t alignment = EIGEN_DEFAULT_ALIGN_BYTES)
 {
-  void *original = std::malloc(size+EIGEN_DEFAULT_ALIGN_BYTES);
+  eigen_assert(alignment >= sizeof(void*) && (alignment & (alignment-1)) == 0 && "Alignment must be at least sizeof(void*) and a power of 2");
+
+  EIGEN_USING_STD(malloc)
+  void *original = malloc(size+alignment);
+  
   if (original == 0) return 0;
-  void *aligned = reinterpret_cast<void*>((reinterpret_cast<std::size_t>(original) & ~(std::size_t(EIGEN_DEFAULT_ALIGN_BYTES-1))) + EIGEN_DEFAULT_ALIGN_BYTES);
+  void *aligned = reinterpret_cast<void*>((reinterpret_cast<std::size_t>(original) & ~(std::size_t(alignment-1))) + alignment);
   *(reinterpret_cast<void**>(aligned) - 1) = original;
   return aligned;
 }
 
 /** \internal Frees memory allocated with handmade_aligned_malloc */
-inline void handmade_aligned_free(void *ptr)
+EIGEN_DEVICE_FUNC inline void handmade_aligned_free(void *ptr)
 {
-  if (ptr) std::free(*(reinterpret_cast<void**>(ptr) - 1));
+  if (ptr) {
+    EIGEN_USING_STD(free)
+    free(*(reinterpret_cast<void**>(ptr) - 1));
+  }
 }
 
 /** \internal
@@ -114,7 +135,7 @@
   void *previous_aligned = static_cast<char *>(original)+previous_offset;
   if(aligned!=previous_aligned)
     std::memmove(aligned, previous_aligned, size);
-  
+
   *(reinterpret_cast<void**>(aligned) - 1) = original;
   return aligned;
 }
@@ -142,7 +163,7 @@
 {
   eigen_assert(is_malloc_allowed() && "heap allocation is forbidden (EIGEN_RUNTIME_NO_MALLOC is defined and g_is_malloc_allowed is false)");
 }
-#else 
+#else
 EIGEN_DEVICE_FUNC inline void check_that_malloc_is_allowed()
 {}
 #endif
@@ -156,9 +177,12 @@
 
   void *result;
   #if (EIGEN_DEFAULT_ALIGN_BYTES==0) || EIGEN_MALLOC_ALREADY_ALIGNED
-    result = std::malloc(size);
+
+    EIGEN_USING_STD(malloc)
+    result = malloc(size);
+
     #if EIGEN_DEFAULT_ALIGN_BYTES==16
-    eigen_assert((size<16 || (std::size_t(result)%16)==0) && "System's malloc returned an unaligned pointer. Compile with EIGEN_MALLOC_ALREADY_ALIGNED=0 to fallback to handmade alignd memory allocator.");
+    eigen_assert((size<16 || (std::size_t(result)%16)==0) && "System's malloc returned an unaligned pointer. Compile with EIGEN_MALLOC_ALREADY_ALIGNED=0 to fallback to handmade aligned memory allocator.");
     #endif
   #else
     result = handmade_aligned_malloc(size);
@@ -174,7 +198,10 @@
 EIGEN_DEVICE_FUNC inline void aligned_free(void *ptr)
 {
   #if (EIGEN_DEFAULT_ALIGN_BYTES==0) || EIGEN_MALLOC_ALREADY_ALIGNED
-    std::free(ptr);
+
+    EIGEN_USING_STD(free)
+    free(ptr);
+
   #else
     handmade_aligned_free(ptr);
   #endif
@@ -187,7 +214,7 @@
   */
 inline void* aligned_realloc(void *ptr, std::size_t new_size, std::size_t old_size)
 {
-  EIGEN_UNUSED_VARIABLE(old_size);
+  EIGEN_UNUSED_VARIABLE(old_size)
 
   void *result;
 #if (EIGEN_DEFAULT_ALIGN_BYTES==0) || EIGEN_MALLOC_ALREADY_ALIGNED
@@ -218,7 +245,9 @@
 {
   check_that_malloc_is_allowed();
 
-  void *result = std::malloc(size);
+  EIGEN_USING_STD(malloc)
+  void *result = malloc(size);
+
   if(!result && size)
     throw_std_bad_alloc();
   return result;
@@ -232,7 +261,8 @@
 
 template<> EIGEN_DEVICE_FUNC inline void conditional_aligned_free<false>(void *ptr)
 {
-  std::free(ptr);
+  EIGEN_USING_STD(free)
+  free(ptr);
 }
 
 template<bool Align> inline void* conditional_aligned_realloc(void* ptr, std::size_t new_size, std::size_t old_size)
@@ -331,7 +361,7 @@
 template<typename T> EIGEN_DEVICE_FUNC inline void aligned_delete(T *ptr, std::size_t size)
 {
   destruct_elements_of_array<T>(ptr, size);
-  aligned_free(ptr);
+  Eigen::internal::aligned_free(ptr);
 }
 
 /** \internal Deletes objects constructed with conditional_aligned_new
@@ -471,8 +501,8 @@
 }
 
 /** \internal Returns the smallest integer multiple of \a base and greater or equal to \a size
-  */ 
-template<typename Index> 
+  */
+template<typename Index>
 inline Index first_multiple(Index size, Index base)
 {
   return ((size+base-1)/base)*base;
@@ -493,7 +523,8 @@
     IntPtr size = IntPtr(end)-IntPtr(start);
     if(size==0) return;
     eigen_internal_assert(start!=0 && end!=0 && target!=0);
-    std::memcpy(target, start, size);
+    EIGEN_USING_STD(memcpy)
+    memcpy(target, start, size);
   }
 };
 
@@ -502,7 +533,7 @@
   { std::copy(start, end, target); }
 };
 
-// intelligent memmove. falls back to std::memmove for POD types, uses std::copy otherwise. 
+// intelligent memmove. falls back to std::memmove for POD types, uses std::copy otherwise.
 template<typename T, bool UseMemmove> struct smart_memmove_helper;
 
 template<typename T> void smart_memmove(const T* start, const T* end, T* target)
@@ -522,19 +553,30 @@
 
 template<typename T> struct smart_memmove_helper<T,false> {
   static inline void run(const T* start, const T* end, T* target)
-  { 
+  {
     if (UIntPtr(target) < UIntPtr(start))
     {
       std::copy(start, end, target);
     }
-    else                                 
+    else
     {
       std::ptrdiff_t count = (std::ptrdiff_t(end)-std::ptrdiff_t(start)) / sizeof(T);
-      std::copy_backward(start, end, target + count); 
+      std::copy_backward(start, end, target + count);
     }
   }
 };
 
+#if EIGEN_HAS_RVALUE_REFERENCES
+template<typename T> EIGEN_DEVICE_FUNC T* smart_move(T* start, T* end, T* target)
+{
+  return std::move(start, end, target);
+}
+#else
+template<typename T> EIGEN_DEVICE_FUNC T* smart_move(T* start, T* end, T* target)
+{
+  return std::copy(start, end, target);
+}
+#endif
 
 /*****************************************************************************
 *** Implementation of runtime stack allocation (falling back to malloc)    ***
@@ -542,7 +584,7 @@
 
 // you can overwrite Eigen's default behavior regarding alloca by defining EIGEN_ALLOCA
 // to the appropriate stack allocation function
-#ifndef EIGEN_ALLOCA
+#if ! defined EIGEN_ALLOCA && ! defined EIGEN_GPU_COMPILE_PHASE
   #if EIGEN_OS_LINUX || EIGEN_OS_MAC || (defined alloca)
     #define EIGEN_ALLOCA alloca
   #elif EIGEN_COMP_MSVC
@@ -550,6 +592,15 @@
   #endif
 #endif
 
+// With clang -Oz -mthumb, alloca changes the stack pointer in a way that is
+// not allowed in Thumb2. -DEIGEN_STACK_ALLOCATION_LIMIT=0 doesn't work because
+// the compiler still emits bad code because stack allocation checks use "<=".
+// TODO: Eliminate after https://bugs.llvm.org/show_bug.cgi?id=23772
+// is fixed.
+#if defined(__clang__) && defined(__thumb__)
+  #undef EIGEN_ALLOCA
+#endif
+
 // This helper class construct the allocated memory, and takes care of destructing and freeing the handled data
 // at destruction time. In practice this helper class is mainly useful to avoid memory leak in case of exceptions.
 template<typename T> class aligned_stack_memory_handler : noncopyable
@@ -561,12 +612,14 @@
      * In this case, the buffer elements will also be destructed when this handler will be destructed.
      * Finally, if \a dealloc is true, then the pointer \a ptr is freed.
      **/
+    EIGEN_DEVICE_FUNC
     aligned_stack_memory_handler(T* ptr, std::size_t size, bool dealloc)
       : m_ptr(ptr), m_size(size), m_deallocate(dealloc)
     {
       if(NumTraits<T>::RequireInitialization && m_ptr)
         Eigen::internal::construct_elements_of_array(m_ptr, size);
     }
+    EIGEN_DEVICE_FUNC
     ~aligned_stack_memory_handler()
     {
       if(NumTraits<T>::RequireInitialization && m_ptr)
@@ -580,6 +633,60 @@
     bool m_deallocate;
 };
 
+#ifdef EIGEN_ALLOCA
+
+template<typename Xpr, int NbEvaluations,
+         bool MapExternalBuffer = nested_eval<Xpr,NbEvaluations>::Evaluate && Xpr::MaxSizeAtCompileTime==Dynamic
+         >
+struct local_nested_eval_wrapper
+{
+  static const bool NeedExternalBuffer = false;
+  typedef typename Xpr::Scalar Scalar;
+  typedef typename nested_eval<Xpr,NbEvaluations>::type ObjectType;
+  ObjectType object;
+
+  EIGEN_DEVICE_FUNC
+  local_nested_eval_wrapper(const Xpr& xpr, Scalar* ptr) : object(xpr)
+  {
+    EIGEN_UNUSED_VARIABLE(ptr);
+    eigen_internal_assert(ptr==0);
+  }
+};
+
+template<typename Xpr, int NbEvaluations>
+struct local_nested_eval_wrapper<Xpr,NbEvaluations,true>
+{
+  static const bool NeedExternalBuffer = true;
+  typedef typename Xpr::Scalar Scalar;
+  typedef typename plain_object_eval<Xpr>::type PlainObject;
+  typedef Map<PlainObject,EIGEN_DEFAULT_ALIGN_BYTES> ObjectType;
+  ObjectType object;
+
+  EIGEN_DEVICE_FUNC
+  local_nested_eval_wrapper(const Xpr& xpr, Scalar* ptr)
+    : object(ptr==0 ? reinterpret_cast<Scalar*>(Eigen::internal::aligned_malloc(sizeof(Scalar)*xpr.size())) : ptr, xpr.rows(), xpr.cols()),
+      m_deallocate(ptr==0)
+  {
+    if(NumTraits<Scalar>::RequireInitialization && object.data())
+      Eigen::internal::construct_elements_of_array(object.data(), object.size());
+    object = xpr;
+  }
+
+  EIGEN_DEVICE_FUNC
+  ~local_nested_eval_wrapper()
+  {
+    if(NumTraits<Scalar>::RequireInitialization && object.data())
+      Eigen::internal::destruct_elements_of_array(object.data(), object.size());
+    if(m_deallocate)
+      Eigen::internal::aligned_free(object.data());
+  }
+
+private:
+  bool m_deallocate;
+};
+
+#endif // EIGEN_ALLOCA
+
 template<typename T> class scoped_array : noncopyable
 {
   T* m_ptr;
@@ -603,13 +710,15 @@
 {
   std::swap(a.ptr(),b.ptr());
 }
-    
+
 } // end namespace internal
 
 /** \internal
-  * Declares, allocates and construct an aligned buffer named NAME of SIZE elements of type TYPE on the stack
-  * if SIZE is smaller than EIGEN_STACK_ALLOCATION_LIMIT, and if stack allocation is supported by the platform
-  * (currently, this is Linux and Visual Studio only). Otherwise the memory is allocated on the heap.
+  *
+  * The macro ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) declares, allocates,
+  * and construct an aligned buffer named NAME of SIZE elements of type TYPE on the stack
+  * if the size in bytes is smaller than EIGEN_STACK_ALLOCATION_LIMIT, and if stack allocation is supported by the platform
+  * (currently, this is Linux, OSX and Visual Studio only). Otherwise the memory is allocated on the heap.
   * The allocated buffer is automatically deleted when exiting the scope of this declaration.
   * If BUFFER is non null, then the declared variable is simply an alias for BUFFER, and no allocation/deletion occurs.
   * Here is an example:
@@ -620,9 +729,17 @@
   * }
   * \endcode
   * The underlying stack allocation function can controlled with the EIGEN_ALLOCA preprocessor token.
+  *
+  * The macro ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) is analogue to
+  * \code
+  *   typename internal::nested_eval<XPRT_T,N>::type NAME(XPR);
+  * \endcode
+  * with the advantage of using aligned stack allocation even if the maximal size of XPR at compile time is unknown.
+  * This is accomplished through alloca if this later is supported and if the required number of bytes
+  * is below EIGEN_STACK_ALLOCATION_LIMIT.
   */
 #ifdef EIGEN_ALLOCA
-  
+
   #if EIGEN_DEFAULT_ALIGN_BYTES>0
     // We always manually re-align the result of EIGEN_ALLOCA.
     // If alloca is already aligned, the compiler should be smart enough to optimize away the re-alignment.
@@ -639,13 +756,23 @@
                     : Eigen::internal::aligned_malloc(sizeof(TYPE)*SIZE) );  \
     Eigen::internal::aligned_stack_memory_handler<TYPE> EIGEN_CAT(NAME,_stack_memory_destructor)((BUFFER)==0 ? NAME : 0,SIZE,sizeof(TYPE)*SIZE>EIGEN_STACK_ALLOCATION_LIMIT)
 
+
+  #define ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) \
+    Eigen::internal::local_nested_eval_wrapper<XPR_T,N> EIGEN_CAT(NAME,_wrapper)(XPR, reinterpret_cast<typename XPR_T::Scalar*>( \
+      ( (Eigen::internal::local_nested_eval_wrapper<XPR_T,N>::NeedExternalBuffer) && ((sizeof(typename XPR_T::Scalar)*XPR.size())<=EIGEN_STACK_ALLOCATION_LIMIT) ) \
+        ? EIGEN_ALIGNED_ALLOCA( sizeof(typename XPR_T::Scalar)*XPR.size() ) : 0 ) ) ; \
+    typename Eigen::internal::local_nested_eval_wrapper<XPR_T,N>::ObjectType NAME(EIGEN_CAT(NAME,_wrapper).object)
+
 #else
 
   #define ei_declare_aligned_stack_constructed_variable(TYPE,NAME,SIZE,BUFFER) \
     Eigen::internal::check_size_for_overflow<TYPE>(SIZE); \
     TYPE* NAME = (BUFFER)!=0 ? BUFFER : reinterpret_cast<TYPE*>(Eigen::internal::aligned_malloc(sizeof(TYPE)*SIZE));    \
     Eigen::internal::aligned_stack_memory_handler<TYPE> EIGEN_CAT(NAME,_stack_memory_destructor)((BUFFER)==0 ? NAME : 0,SIZE,true)
-    
+
+
+#define ei_declare_local_nested_eval(XPR_T,XPR,N,NAME) typename Eigen::internal::nested_eval<XPR_T,N>::type NAME(XPR)
+
 #endif
 
 
@@ -653,32 +780,56 @@
 *** Implementation of EIGEN_MAKE_ALIGNED_OPERATOR_NEW [_IF]                ***
 *****************************************************************************/
 
-#if EIGEN_MAX_ALIGN_BYTES!=0
+#if EIGEN_HAS_CXX17_OVERALIGN
+
+// C++17 -> no need to bother about alignment anymore :)
+
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign)
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar,Size)
+
+#else
+
+// HIP does not support new/delete on device.
+#if EIGEN_MAX_ALIGN_BYTES!=0 && !defined(EIGEN_HIP_DEVICE_COMPILE)
   #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
+      EIGEN_DEVICE_FUNC \
       void* operator new(std::size_t size, const std::nothrow_t&) EIGEN_NO_THROW { \
         EIGEN_TRY { return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); } \
         EIGEN_CATCH (...) { return 0; } \
       }
   #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign) \
+      EIGEN_DEVICE_FUNC \
       void *operator new(std::size_t size) { \
         return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); \
       } \
+      EIGEN_DEVICE_FUNC \
       void *operator new[](std::size_t size) { \
         return Eigen::internal::conditional_aligned_malloc<NeedsToAlign>(size); \
       } \
+      EIGEN_DEVICE_FUNC \
       void operator delete(void * ptr) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      EIGEN_DEVICE_FUNC \
       void operator delete[](void * ptr) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      EIGEN_DEVICE_FUNC \
       void operator delete(void * ptr, std::size_t /* sz */) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
+      EIGEN_DEVICE_FUNC \
       void operator delete[](void * ptr, std::size_t /* sz */) EIGEN_NO_THROW { Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); } \
       /* in-place new and delete. since (at least afaik) there is no actual   */ \
       /* memory allocated we can safely let the default implementation handle */ \
       /* this particular case. */ \
+      EIGEN_DEVICE_FUNC \
       static void *operator new(std::size_t size, void *ptr) { return ::operator new(size,ptr); } \
+      EIGEN_DEVICE_FUNC \
       static void *operator new[](std::size_t size, void* ptr) { return ::operator new[](size,ptr); } \
+      EIGEN_DEVICE_FUNC \
       void operator delete(void * memory, void *ptr) EIGEN_NO_THROW { return ::operator delete(memory,ptr); } \
+      EIGEN_DEVICE_FUNC \
       void operator delete[](void * memory, void *ptr) EIGEN_NO_THROW { return ::operator delete[](memory,ptr); } \
       /* nothrow-new (returns zero instead of std::bad_alloc) */ \
       EIGEN_MAKE_ALIGNED_OPERATOR_NEW_NOTHROW(NeedsToAlign) \
+      EIGEN_DEVICE_FUNC \
       void operator delete(void *ptr, const std::nothrow_t&) EIGEN_NO_THROW { \
         Eigen::internal::conditional_aligned_free<NeedsToAlign>(ptr); \
       } \
@@ -688,8 +839,14 @@
 #endif
 
 #define EIGEN_MAKE_ALIGNED_OPERATOR_NEW EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(true)
-#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar,Size) \
-  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(((Size)!=Eigen::Dynamic) && ((sizeof(Scalar)*(Size))%EIGEN_MAX_ALIGN_BYTES==0)))
+#define EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(Scalar,Size)                        \
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(bool(                                                             \
+        ((Size)!=Eigen::Dynamic) &&                                                                    \
+        (((EIGEN_MAX_ALIGN_BYTES>=16) && ((sizeof(Scalar)*(Size))%(EIGEN_MAX_ALIGN_BYTES  )==0)) ||    \
+         ((EIGEN_MAX_ALIGN_BYTES>=32) && ((sizeof(Scalar)*(Size))%(EIGEN_MAX_ALIGN_BYTES/2)==0)) ||    \
+         ((EIGEN_MAX_ALIGN_BYTES>=64) && ((sizeof(Scalar)*(Size))%(EIGEN_MAX_ALIGN_BYTES/4)==0))   )))
+
+#endif
 
 /****************************************************************************/
 
@@ -703,13 +860,13 @@
 *  - 32 bytes alignment if AVX is enabled.
 *  - 64 bytes alignment if AVX512 is enabled.
 *
-* This can be controled using the \c EIGEN_MAX_ALIGN_BYTES macro as documented
+* This can be controlled using the \c EIGEN_MAX_ALIGN_BYTES macro as documented
 * \link TopicPreprocessorDirectivesPerformance there \endlink.
 *
 * Example:
 * \code
 * // Matrix4f requires 16 bytes alignment:
-* std::map< int, Matrix4f, std::less<int>, 
+* std::map< int, Matrix4f, std::less<int>,
 *           aligned_allocator<std::pair<const int, Matrix4f> > > my_map_mat4;
 * // Vector3f does not require 16 bytes alignment, no need to use Eigen's allocator:
 * std::map< int, Vector3f > my_map_vec3;
@@ -744,18 +901,19 @@
 
   ~aligned_allocator() {}
 
+  #if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(7,0)
+  // In gcc std::allocator::max_size() is bugged making gcc triggers a warning:
+  // eigen/Eigen/src/Core/util/Memory.h:189:12: warning: argument 1 value '18446744073709551612' exceeds maximum object size 9223372036854775807
+  // See https://gcc.gnu.org/bugzilla/show_bug.cgi?id=87544
+  size_type max_size() const {
+    return (std::numeric_limits<std::ptrdiff_t>::max)()/sizeof(T);
+  }
+  #endif
+
   pointer allocate(size_type num, const void* /*hint*/ = 0)
   {
     internal::check_size_for_overflow<T>(num);
-    size_type size = num * sizeof(T);
-#if EIGEN_COMP_GNUC_STRICT && EIGEN_GNUC_AT_LEAST(7,0)
-    // workaround gcc bug https://gcc.gnu.org/bugzilla/show_bug.cgi?id=87544
-    // It triggered eigen/Eigen/src/Core/util/Memory.h:189:12: warning: argument 1 value '18446744073709551612' exceeds maximum object size 9223372036854775807
-    if(size>=std::size_t((std::numeric_limits<std::ptrdiff_t>::max)()))
-      return 0;
-    else
-#endif
-      return static_cast<pointer>( internal::aligned_malloc(size) );
+    return static_cast<pointer>( internal::aligned_malloc(num * sizeof(T)) );
   }
 
   void deallocate(pointer p, size_type /*num*/)
@@ -914,20 +1072,32 @@
 {
   if(max_std_funcs>=4)
     queryCacheSizes_intel_direct(l1,l2,l3);
-  else
+  else if(max_std_funcs>=2)
     queryCacheSizes_intel_codes(l1,l2,l3);
+  else
+    l1 = l2 = l3 = 0;
 }
 
 inline void queryCacheSizes_amd(int& l1, int& l2, int& l3)
 {
   int abcd[4];
   abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
-  EIGEN_CPUID(abcd,0x80000005,0);
-  l1 = (abcd[2] >> 24) * 1024; // C[31:24] = L1 size in KB
-  abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
-  EIGEN_CPUID(abcd,0x80000006,0);
-  l2 = (abcd[2] >> 16) * 1024; // C[31;16] = l2 cache size in KB
-  l3 = ((abcd[3] & 0xFFFC000) >> 18) * 512 * 1024; // D[31;18] = l3 cache size in 512KB
+  
+  // First query the max supported function.
+  EIGEN_CPUID(abcd,0x80000000,0);
+  if(static_cast<numext::uint32_t>(abcd[0]) >= static_cast<numext::uint32_t>(0x80000006))
+  {
+    EIGEN_CPUID(abcd,0x80000005,0);
+    l1 = (abcd[2] >> 24) * 1024; // C[31:24] = L1 size in KB
+    abcd[0] = abcd[1] = abcd[2] = abcd[3] = 0;
+    EIGEN_CPUID(abcd,0x80000006,0);
+    l2 = (abcd[2] >> 16) * 1024; // C[31;16] = l2 cache size in KB
+    l3 = ((abcd[3] & 0xFFFC000) >> 18) * 512 * 1024; // D[31;18] = l3 cache size in 512KB
+  }
+  else
+  {
+    l1 = l2 = l3 = 0;
+  }
 }
 #endif
 
@@ -943,7 +1113,7 @@
 
   // identify the CPU vendor
   EIGEN_CPUID(abcd,0x0,0);
-  int max_std_funcs = abcd[1];
+  int max_std_funcs = abcd[0];
   if(cpuid_is_vendor(abcd,GenuineIntel))
     queryCacheSizes_intel(l1,l2,l3,max_std_funcs);
   else if(cpuid_is_vendor(abcd,AuthenticAMD) || cpuid_is_vendor(abcd,AMDisbetter_))
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
index d31e954..7badfdc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/Meta.h
@@ -11,13 +11,54 @@
 #ifndef EIGEN_META_H
 #define EIGEN_META_H
 
-#if defined(__CUDA_ARCH__)
-#include <cfloat>
-#include <math_constants.h>
+#if defined(EIGEN_GPU_COMPILE_PHASE)
+
+ #include <cfloat>
+
+ #if defined(EIGEN_CUDA_ARCH)
+  #include <math_constants.h>
+ #endif
+
+ #if defined(EIGEN_HIP_DEVICE_COMPILE)
+  // #include "Eigen/src/Core/arch/HIP/hcc/math_constants.h"
+  #endif
+
 #endif
 
-#if EIGEN_COMP_ICC>=1600 &&  __cplusplus >= 201103L
+// Recent versions of ICC require <cstdint> for pointer types below.
+#define EIGEN_ICC_NEEDS_CSTDINT (EIGEN_COMP_ICC>=1600 && EIGEN_COMP_CXXVER >= 11)
+
+// Define portable (u)int{32,64} types
+#if EIGEN_HAS_CXX11 || EIGEN_ICC_NEEDS_CSTDINT
 #include <cstdint>
+namespace Eigen {
+namespace numext {
+typedef std::uint8_t  uint8_t;
+typedef std::int8_t   int8_t;
+typedef std::uint16_t uint16_t;
+typedef std::int16_t  int16_t;
+typedef std::uint32_t uint32_t;
+typedef std::int32_t  int32_t;
+typedef std::uint64_t uint64_t;
+typedef std::int64_t  int64_t;
+}
+}
+#else
+// Without c++11, all compilers able to compile Eigen also
+// provide the C99 stdint.h header file.
+#include <stdint.h>
+namespace Eigen {
+namespace numext {
+typedef ::uint8_t  uint8_t;
+typedef ::int8_t   int8_t;
+typedef ::uint16_t uint16_t;
+typedef ::int16_t  int16_t;
+typedef ::uint32_t uint32_t;
+typedef ::int32_t  int32_t;
+typedef ::uint64_t uint64_t;
+typedef ::int64_t  int64_t;
+}
+}
 #endif
 
 namespace Eigen {
@@ -43,26 +84,33 @@
 
 // Only recent versions of ICC complain about using ptrdiff_t to hold pointers,
 // and older versions do not provide *intptr_t types.
-#if EIGEN_COMP_ICC>=1600 &&  __cplusplus >= 201103L
+#if EIGEN_ICC_NEEDS_CSTDINT
 typedef std::intptr_t  IntPtr;
 typedef std::uintptr_t UIntPtr;
 #else
 typedef std::ptrdiff_t IntPtr;
 typedef std::size_t UIntPtr;
 #endif
+#undef EIGEN_ICC_NEEDS_CSTDINT
 
 struct true_type {  enum { value = 1 }; };
 struct false_type { enum { value = 0 }; };
 
+template<bool Condition>
+struct bool_constant;
+
+template<>
+struct bool_constant<true> : true_type {};
+
+template<>
+struct bool_constant<false> : false_type {};
+
 template<bool Condition, typename Then, typename Else>
 struct conditional { typedef Then type; };
 
 template<typename Then, typename Else>
 struct conditional <false, Then, Else> { typedef Else type; };
 
-template<typename T, typename U> struct is_same { enum { value = 0 }; };
-template<typename T> struct is_same<T,T> { enum { value = 1 }; };
-
 template<typename T> struct remove_reference { typedef T type; };
 template<typename T> struct remove_reference<T&> { typedef T type; };
 
@@ -97,17 +145,33 @@
 template<> struct is_arithmetic<signed long>   { enum { value = true }; };
 template<> struct is_arithmetic<unsigned long> { enum { value = true }; };
 
-template<typename T> struct is_integral        { enum { value = false }; };
-template<> struct is_integral<bool>            { enum { value = true }; };
-template<> struct is_integral<char>            { enum { value = true }; };
-template<> struct is_integral<signed char>     { enum { value = true }; };
-template<> struct is_integral<unsigned char>   { enum { value = true }; };
-template<> struct is_integral<signed short>    { enum { value = true }; };
-template<> struct is_integral<unsigned short>  { enum { value = true }; };
-template<> struct is_integral<signed int>      { enum { value = true }; };
-template<> struct is_integral<unsigned int>    { enum { value = true }; };
-template<> struct is_integral<signed long>     { enum { value = true }; };
-template<> struct is_integral<unsigned long>   { enum { value = true }; };
+template<typename T, typename U> struct is_same { enum { value = 0 }; };
+template<typename T> struct is_same<T,T> { enum { value = 1 }; };
+
+template< class T >
+struct is_void : is_same<void, typename remove_const<T>::type> {};
+
+#if EIGEN_HAS_CXX11
+template<> struct is_arithmetic<signed long long>   { enum { value = true }; };
+template<> struct is_arithmetic<unsigned long long> { enum { value = true }; };
+using std::is_integral;
+#else
+template<typename T> struct is_integral               { enum { value = false }; };
+template<> struct is_integral<bool>                   { enum { value = true }; };
+template<> struct is_integral<char>                   { enum { value = true }; };
+template<> struct is_integral<signed char>            { enum { value = true }; };
+template<> struct is_integral<unsigned char>          { enum { value = true }; };
+template<> struct is_integral<signed short>           { enum { value = true }; };
+template<> struct is_integral<unsigned short>         { enum { value = true }; };
+template<> struct is_integral<signed int>             { enum { value = true }; };
+template<> struct is_integral<unsigned int>           { enum { value = true }; };
+template<> struct is_integral<signed long>            { enum { value = true }; };
+template<> struct is_integral<unsigned long>          { enum { value = true }; };
+#if EIGEN_COMP_MSVC
+template<> struct is_integral<signed __int64>         { enum { value = true }; };
+template<> struct is_integral<unsigned __int64>       { enum { value = true }; };
+#endif
+#endif
 
 #if EIGEN_HAS_CXX11
 using std::make_unsigned;
@@ -129,6 +193,16 @@
 template<> struct make_unsigned<signed __int64>   { typedef unsigned __int64 type; };
 template<> struct make_unsigned<unsigned __int64> { typedef unsigned __int64 type; };
 #endif
+
+// Some platforms define int64_t as `long long` even for C++03, where
+// `long long` is not guaranteed by the standard. In this case we are missing
+// the definition for make_unsigned. If we just define it, we run into issues
+// where `long long` doesn't exist in some compilers for C++03. We therefore add
+// the specialization for these platforms only.
+#if EIGEN_OS_MAC || EIGEN_COMP_MINGW
+template<> struct make_unsigned<unsigned long long> { typedef unsigned long long type; };
+template<> struct make_unsigned<long long>          { typedef unsigned long long type; };
+#endif
 #endif
 
 template <typename T> struct add_const { typedef const T type; };
@@ -143,6 +217,11 @@
 template<typename T> struct add_const_on_value_type<T* const>  { typedef T const* const type; };
 template<typename T> struct add_const_on_value_type<T const* const>  { typedef T const* const type; };
 
+#if EIGEN_HAS_CXX11
+
+using std::is_convertible;
+
+#else
 
 template<typename From, typename To>
 struct is_convertible_impl
@@ -156,16 +235,19 @@
   struct yes {int a[1];};
   struct no  {int a[2];};
 
-  static yes test(const To&, int);
+  template<typename T>
+  static yes test(T, int);
+
+  template<typename T>
   static no  test(any_conversion, ...);
 
 public:
-  static From ms_from;
+  static typename internal::remove_reference<From>::type* ms_from;
 #ifdef __INTEL_COMPILER
   #pragma warning push
   #pragma warning ( disable : 2259 )
 #endif
-  enum { value = sizeof(test(ms_from, 0))==sizeof(yes) };
+  enum { value = sizeof(test<To>(*ms_from, 0))==sizeof(yes) };
 #ifdef __INTEL_COMPILER
   #pragma warning pop
 #endif
@@ -174,10 +256,17 @@
 template<typename From, typename To>
 struct is_convertible
 {
-  enum { value = is_convertible_impl<typename remove_all<From>::type,
-                                     typename remove_all<To  >::type>::value };
+  enum { value = is_convertible_impl<From,To>::value };
 };
 
+template<typename T>
+struct is_convertible<T,T&> { enum { value = false }; };
+
+template<typename T>
+struct is_convertible<const T,const T&> { enum { value = true }; };
+
+#endif
+
 /** \internal Allows to enable/disable an overload
   * according to a compile time condition.
   */
@@ -186,7 +275,7 @@
 template<typename T> struct enable_if<true,T>
 { typedef T type; };
 
-#if defined(__CUDA_ARCH__)
+#if defined(EIGEN_GPU_COMPILE_PHASE) && !EIGEN_HAS_CXX11
 #if !defined(__FLT_EPSILON__)
 #define __FLT_EPSILON__ FLT_EPSILON
 #define __DBL_EPSILON__ DBL_EPSILON
@@ -197,7 +286,7 @@
 template<typename T> struct numeric_limits
 {
   EIGEN_DEVICE_FUNC
-  static T epsilon() { return 0; }
+  static EIGEN_CONSTEXPR T epsilon() { return 0; }
   static T (max)() { assert(false && "Highest not supported for this type"); }
   static T (min)() { assert(false && "Lowest not supported for this type"); }
   static T infinity() { assert(false && "Infinity not supported for this type"); }
@@ -205,91 +294,130 @@
 };
 template<> struct numeric_limits<float>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static float epsilon() { return __FLT_EPSILON__; }
   EIGEN_DEVICE_FUNC
-  static float (max)() { return CUDART_MAX_NORMAL_F; }
-  EIGEN_DEVICE_FUNC
+  static float (max)() {
+  #if defined(EIGEN_CUDA_ARCH)
+    return CUDART_MAX_NORMAL_F;
+  #else
+    return HIPRT_MAX_NORMAL_F;
+  #endif
+  }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static float (min)() { return FLT_MIN; }
   EIGEN_DEVICE_FUNC
-  static float infinity() { return CUDART_INF_F; }
+  static float infinity() {
+  #if defined(EIGEN_CUDA_ARCH)
+    return CUDART_INF_F;
+  #else
+    return HIPRT_INF_F;
+  #endif
+  }
   EIGEN_DEVICE_FUNC
-  static float quiet_NaN() { return CUDART_NAN_F; }
+  static float quiet_NaN() {
+  #if defined(EIGEN_CUDA_ARCH)
+    return CUDART_NAN_F;
+  #else
+    return HIPRT_NAN_F;
+  #endif
+  }
 };
 template<> struct numeric_limits<double>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static double epsilon() { return __DBL_EPSILON__; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static double (max)() { return DBL_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static double (min)() { return DBL_MIN; }
   EIGEN_DEVICE_FUNC
-  static double infinity() { return CUDART_INF; }
+  static double infinity() {
+  #if defined(EIGEN_CUDA_ARCH)
+    return CUDART_INF;
+  #else
+    return HIPRT_INF;
+  #endif
+  }
   EIGEN_DEVICE_FUNC
-  static double quiet_NaN() { return CUDART_NAN; }
+  static double quiet_NaN() {
+  #if defined(EIGEN_CUDA_ARCH)
+    return CUDART_NAN;
+  #else
+    return HIPRT_NAN;
+  #endif
+  }
 };
 template<> struct numeric_limits<int>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static int epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static int (max)() { return INT_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static int (min)() { return INT_MIN; }
 };
 template<> struct numeric_limits<unsigned int>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned int epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned int (max)() { return UINT_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned int (min)() { return 0; }
 };
 template<> struct numeric_limits<long>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static long (max)() { return LONG_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static long (min)() { return LONG_MIN; }
 };
 template<> struct numeric_limits<unsigned long>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned long (max)() { return ULONG_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned long (min)() { return 0; }
 };
 template<> struct numeric_limits<long long>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static long long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static long long (max)() { return LLONG_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static long long (min)() { return LLONG_MIN; }
 };
 template<> struct numeric_limits<unsigned long long>
 {
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned long long epsilon() { return 0; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned long long (max)() { return ULLONG_MAX; }
-  EIGEN_DEVICE_FUNC
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
   static unsigned long long (min)() { return 0; }
 };
+template<> struct numeric_limits<bool>
+{
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static bool epsilon() { return false; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+  static bool (max)() { return true; }
+  EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR 
+  static bool (min)() { return false; }
+};
 
 }
 
-#endif
+#endif // defined(EIGEN_GPU_COMPILE_PHASE) && !EIGEN_HAS_CXX11
 
 /** \internal
-  * A base class do disable default copy ctor and copy assignement operator.
+  * A base class do disable default copy ctor and copy assignment operator.
   */
 class noncopyable
 {
@@ -301,13 +429,82 @@
 };
 
 /** \internal
-  * Convenient struct to get the result type of a unary or binary functor.
+  * Provides access to the number of elements in the object of as a compile-time constant expression.
+  * It "returns" Eigen::Dynamic if the size cannot be resolved at compile-time (default).
   *
-  * It supports both the current STL mechanism (using the result_type member) as well as
-  * upcoming next STL generation (using a templated result member).
-  * If none of these members is provided, then the type of the first argument is returned. FIXME, that behavior is a pretty bad hack.
+  * Similar to std::tuple_size, but more general.
+  *
+  * It currently supports:
+  *  - any types T defining T::SizeAtCompileTime
+  *  - plain C arrays as T[N]
+  *  - std::array (c++11)
+  *  - some internal types such as SingleRange and AllRange
+  *
+  * The second template parameter eases SFINAE-based specializations.
   */
-#if EIGEN_HAS_STD_RESULT_OF
+template<typename T, typename EnableIf = void> struct array_size {
+  enum { value = Dynamic };
+};
+
+template<typename T> struct array_size<T,typename internal::enable_if<((T::SizeAtCompileTime&0)==0)>::type> {
+  enum { value = T::SizeAtCompileTime };
+};
+
+template<typename T, int N> struct array_size<const T (&)[N]> {
+  enum { value = N };
+};
+template<typename T, int N> struct array_size<T (&)[N]> {
+  enum { value = N };
+};
+
+#if EIGEN_HAS_CXX11
+template<typename T, std::size_t N> struct array_size<const std::array<T,N> > {
+  enum { value = N };
+};
+template<typename T, std::size_t N> struct array_size<std::array<T,N> > {
+  enum { value = N };
+};
+#endif
+
+/** \internal
+  * Analogue of the std::size free function.
+  * It returns the size of the container or view \a x of type \c T
+  *
+  * It currently supports:
+  *  - any types T defining a member T::size() const
+  *  - plain C arrays as T[N]
+  *
+  */
+template<typename T>
+EIGEN_CONSTEXPR Index size(const T& x) { return x.size(); }
+
+template<typename T,std::size_t N>
+EIGEN_CONSTEXPR Index size(const T (&) [N]) { return N; }
+
+/** \internal
+  * Convenient struct to get the result type of a nullary, unary, binary, or
+  * ternary functor.
+  * 
+  * Pre C++11:
+  * Supports both a Func::result_type member and templated
+  * Func::result<Func(ArgTypes...)>::type member.
+  * 
+  * If none of these members is provided, then the type of the first
+  * argument is returned.
+  * 
+  * Post C++11:
+  * This uses std::result_of. However, note the `type` member removes
+  * const and converts references/pointers to their corresponding value type.
+  */
+#if EIGEN_HAS_STD_INVOKE_RESULT
+template<typename T> struct result_of;
+
+template<typename F, typename... ArgTypes>
+struct result_of<F(ArgTypes...)> {
+  typedef typename std::invoke_result<F, ArgTypes...>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
+#elif EIGEN_HAS_STD_RESULT_OF
 template<typename T> struct result_of {
   typedef typename std::result_of<T>::type type1;
   typedef typename remove_all<type1>::type type;
@@ -319,6 +516,28 @@
 struct has_std_result_type {int a[2];};
 struct has_tr1_result {int a[3];};
 
+template<typename Func, int SizeOf>
+struct nullary_result_of_select {};
+
+template<typename Func>
+struct nullary_result_of_select<Func, sizeof(has_std_result_type)> {typedef typename Func::result_type type;};
+
+template<typename Func>
+struct nullary_result_of_select<Func, sizeof(has_tr1_result)> {typedef typename Func::template result<Func()>::type type;};
+
+template<typename Func>
+struct result_of<Func()> {
+    template<typename T>
+    static has_std_result_type    testFunctor(T const *, typename T::result_type const * = 0);
+    template<typename T>
+    static has_tr1_result         testFunctor(T const *, typename T::template result<T()>::type const * = 0);
+    static has_none               testFunctor(...);
+
+    // note that the following indirection is needed for gcc-3.3
+    enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
+    typedef typename nullary_result_of_select<Func, FunctorType>::type type;
+};
+
 template<typename Func, typename ArgType, int SizeOf=sizeof(has_none)>
 struct unary_result_of_select {typedef typename internal::remove_all<ArgType>::type type;};
 
@@ -388,6 +607,45 @@
     enum {FunctorType = sizeof(testFunctor(static_cast<Func*>(0)))};
     typedef typename ternary_result_of_select<Func, ArgType0, ArgType1, ArgType2, FunctorType>::type type;
 };
+
+#endif
+
+#if EIGEN_HAS_STD_INVOKE_RESULT
+template<typename F, typename... ArgTypes>
+struct invoke_result {
+  typedef typename std::invoke_result<F, ArgTypes...>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
+#elif EIGEN_HAS_CXX11
+template<typename F, typename... ArgTypes>
+struct invoke_result {
+  typedef typename result_of<F(ArgTypes...)>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
+#else
+template<typename F, typename ArgType0 = void, typename ArgType1 = void, typename ArgType2 = void>
+struct invoke_result {
+  typedef typename result_of<F(ArgType0, ArgType1, ArgType2)>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
+
+template<typename F>
+struct invoke_result<F, void, void, void> {
+  typedef typename result_of<F()>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
+
+template<typename F, typename ArgType0>
+struct invoke_result<F, ArgType0, void, void> {
+  typedef typename result_of<F(ArgType0)>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
+
+template<typename F, typename ArgType0, typename ArgType1>
+struct invoke_result<F, ArgType0, ArgType1, void> {
+  typedef typename result_of<F(ArgType0, ArgType1)>::type type1;
+  typedef typename remove_all<type1>::type type;
+};
 #endif
 
 struct meta_yes { char a[1]; };
@@ -397,10 +655,10 @@
 template <typename T>
 struct has_ReturnType
 {
-  template <typename C> static meta_yes testFunctor(typename C::ReturnType const *);
-  template <typename C> static meta_no testFunctor(...);
+  template <typename C> static meta_yes testFunctor(C const *, typename C::ReturnType const * = 0);
+  template <typename C> static meta_no  testFunctor(...);
 
-  enum { value = sizeof(testFunctor<T>(0)) == sizeof(meta_yes) };
+  enum { value = sizeof(testFunctor<T>(static_cast<T*>(0))) == sizeof(meta_yes) };
 };
 
 template<typename T> const T* return_ptr();
@@ -457,20 +715,25 @@
 
 
 /** \internal Computes the least common multiple of two positive integer A and B
-  * at compile-time. It implements a naive algorithm testing all multiples of A.
-  * It thus works better if A>=B.
+  * at compile-time. 
   */
-template<int A, int B, int K=1, bool Done = ((A*K)%B)==0>
+template<int A, int B, int K=1, bool Done = ((A*K)%B)==0, bool Big=(A>=B)>
 struct meta_least_common_multiple
 {
   enum { ret = meta_least_common_multiple<A,B,K+1>::ret };
 };
+template<int A, int B, int K, bool Done>
+struct meta_least_common_multiple<A,B,K,Done,false>
+{
+  enum { ret = meta_least_common_multiple<B,A,K>::ret };
+};
 template<int A, int B, int K>
-struct meta_least_common_multiple<A,B,K,true>
+struct meta_least_common_multiple<A,B,K,true,true>
 {
   enum { ret = A*K };
 };
 
+
 /** \internal determines whether the product of two numeric types is allowed and what the return type is */
 template<typename T, typename U> struct scalar_product_traits
 {
@@ -483,17 +746,27 @@
 // typedef typename scalar_product_traits<typename remove_all<ArgType0>::type, typename remove_all<ArgType1>::type>::ReturnType type;
 // };
 
+/** \internal Obtains a POD type suitable to use as storage for an object of a size
+  * of at most Len bytes, aligned as specified by \c Align.
+  */
+template<unsigned Len, unsigned Align>
+struct aligned_storage {
+  struct type {
+    EIGEN_ALIGN_TO_BOUNDARY(Align) unsigned char data[Len];
+  };
+};
+
 } // end namespace internal
 
 namespace numext {
-  
-#if defined(__CUDA_ARCH__)
+
+#if defined(EIGEN_GPU_COMPILE_PHASE)
 template<typename T> EIGEN_DEVICE_FUNC   void swap(T &a, T &b) { T tmp = b; b = a; a = tmp; }
 #else
 template<typename T> EIGEN_STRONG_INLINE void swap(T &a, T &b) { std::swap(a,b); }
 #endif
 
-#if defined(__CUDA_ARCH__)
+#if defined(EIGEN_GPU_COMPILE_PHASE) && !EIGEN_HAS_CXX11
 using internal::device::numeric_limits;
 #else
 using std::numeric_limits;
@@ -502,6 +775,7 @@
 // Integer division with rounding up.
 // T is assumed to be an integer type with a>=0, and b>0
 template<typename T>
+EIGEN_DEVICE_FUNC
 T div_ceil(const T &a, const T &b)
 {
   return (a+b-1) / b;
@@ -509,23 +783,27 @@
 
 // The aim of the following functions is to bypass -Wfloat-equal warnings
 // when we really want a strict equality comparison on floating points.
-template<typename X, typename Y> EIGEN_STRONG_INLINE
+template<typename X, typename Y> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
 bool equal_strict(const X& x,const Y& y) { return x == y; }
 
-template<> EIGEN_STRONG_INLINE
+#if !defined(EIGEN_GPU_COMPILE_PHASE) || (!defined(EIGEN_CUDA_ARCH) && defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC))
+template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
 bool equal_strict(const float& x,const float& y) { return std::equal_to<float>()(x,y); }
 
-template<> EIGEN_STRONG_INLINE
+template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
 bool equal_strict(const double& x,const double& y) { return std::equal_to<double>()(x,y); }
+#endif
 
-template<typename X, typename Y> EIGEN_STRONG_INLINE
+template<typename X, typename Y> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
 bool not_equal_strict(const X& x,const Y& y) { return x != y; }
 
-template<> EIGEN_STRONG_INLINE
+#if !defined(EIGEN_GPU_COMPILE_PHASE) || (!defined(EIGEN_CUDA_ARCH) && defined(EIGEN_CONSTEXPR_ARE_DEVICE_FUNC))
+template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
 bool not_equal_strict(const float& x,const float& y) { return std::not_equal_to<float>()(x,y); }
 
-template<> EIGEN_STRONG_INLINE
+template<> EIGEN_STRONG_INLINE EIGEN_DEVICE_FUNC
 bool not_equal_strict(const double& x,const double& y) { return std::not_equal_to<double>()(x,y); }
+#endif
 
 } // end namespace numext
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
index ecc82b7..1ce6fd1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReenableStupidWarnings.h
@@ -1,4 +1,8 @@
-#ifdef EIGEN_WARNINGS_DISABLED
+#ifdef EIGEN_WARNINGS_DISABLED_2
+// "DisableStupidWarnings.h" was included twice recursively: Do not reenable warnings yet!
+#  undef EIGEN_WARNINGS_DISABLED_2
+
+#elif defined(EIGEN_WARNINGS_DISABLED)
 #undef EIGEN_WARNINGS_DISABLED
 
 #ifndef EIGEN_PERMANENTLY_DISABLE_STUPID_WARNINGS
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h
new file mode 100644
index 0000000..4124321
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/ReshapedHelper.h
@@ -0,0 +1,51 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+
+#ifndef EIGEN_RESHAPED_HELPER_H
+#define EIGEN_RESHAPED_HELPER_H
+
+namespace Eigen {
+
+enum AutoSize_t   { AutoSize };
+const int AutoOrder = 2;
+
+namespace internal {
+
+template<typename SizeType,typename OtherSize, int TotalSize>
+struct get_compiletime_reshape_size {
+  enum { value = get_fixed_value<SizeType>::value };
+};
+
+template<typename SizeType>
+Index get_runtime_reshape_size(SizeType size, Index /*other*/, Index /*total*/) {
+  return internal::get_runtime_value(size);
+}
+
+template<typename OtherSize, int TotalSize>
+struct get_compiletime_reshape_size<AutoSize_t,OtherSize,TotalSize> {
+  enum {
+    other_size = get_fixed_value<OtherSize>::value,
+    value = (TotalSize==Dynamic || other_size==Dynamic) ? Dynamic : TotalSize / other_size };
+};
+
+inline Index get_runtime_reshape_size(AutoSize_t /*size*/, Index other, Index total) {
+  return total/other;
+}
+
+template<int Flags, int Order>
+struct get_compiletime_reshape_order {
+  enum { value = Order == AutoOrder ? Flags & RowMajorBit : Order };
+};
+
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_RESHAPED_HELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
index 500e477..c45de59 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/StaticAssert.h
@@ -27,7 +27,7 @@
 #ifndef EIGEN_STATIC_ASSERT
 #ifndef EIGEN_NO_STATIC_ASSERT
 
-  #if EIGEN_MAX_CPP_VER>=11 && (__has_feature(cxx_static_assert) || (defined(__cplusplus) && __cplusplus >= 201103L) || (EIGEN_COMP_MSVC >= 1600))
+  #if EIGEN_MAX_CPP_VER>=11 && (__has_feature(cxx_static_assert) || (EIGEN_COMP_CXXVER >= 11) || (EIGEN_COMP_MSVC >= 1600))
 
     // if native static_assert is enabled, let's use it
     #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
@@ -103,7 +103,10 @@
         STORAGE_KIND_MUST_MATCH=1,
         STORAGE_INDEX_MUST_MATCH=1,
         CHOLMOD_SUPPORTS_DOUBLE_PRECISION_ONLY=1,
-        SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY=1
+        SELFADJOINTVIEW_ACCEPTS_UPPER_AND_LOWER_MODE_ONLY=1,
+        INVALID_TEMPLATE_PARAMETER=1,
+        GPU_TENSOR_CONTRACTION_DOES_NOT_SUPPORT_OUTPUT_KERNELS=1,
+        THE_ARRAY_SIZE_SHOULD_EQUAL_WITH_PACKET_SIZE=1
       };
     };
 
@@ -182,7 +185,7 @@
      )
 
 #define EIGEN_STATIC_ASSERT_NON_INTEGER(TYPE) \
-    EIGEN_STATIC_ASSERT(!NumTraits<TYPE>::IsInteger, THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
+    EIGEN_STATIC_ASSERT(!Eigen::NumTraits<TYPE>::IsInteger, THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
 
 
 // static assertion failing if it is guaranteed at compile-time that the two matrix expression types have different sizes
@@ -192,8 +195,8 @@
     YOU_MIXED_MATRICES_OF_DIFFERENT_SIZES)
 
 #define EIGEN_STATIC_ASSERT_SIZE_1x1(TYPE) \
-      EIGEN_STATIC_ASSERT((TYPE::RowsAtCompileTime == 1 || TYPE::RowsAtCompileTime == Dynamic) && \
-                          (TYPE::ColsAtCompileTime == 1 || TYPE::ColsAtCompileTime == Dynamic), \
+      EIGEN_STATIC_ASSERT((TYPE::RowsAtCompileTime == 1 || TYPE::RowsAtCompileTime == Eigen::Dynamic) && \
+                          (TYPE::ColsAtCompileTime == 1 || TYPE::ColsAtCompileTime == Eigen::Dynamic), \
                           THIS_METHOD_IS_ONLY_FOR_1x1_EXPRESSIONS)
 
 #define EIGEN_STATIC_ASSERT_LVALUE(Derived) \
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h
new file mode 100644
index 0000000..354dd9a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/SymbolicIndex.h
@@ -0,0 +1,293 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_SYMBOLIC_INDEX_H
+#define EIGEN_SYMBOLIC_INDEX_H
+
+namespace Eigen {
+
+/** \namespace Eigen::symbolic
+  * \ingroup Core_Module
+  *
+  * This namespace defines a set of classes and functions to build and evaluate symbolic expressions of scalar type Index.
+  * Here is a simple example:
+  *
+  * \code
+  * // First step, defines symbols:
+  * struct x_tag {};  static const symbolic::SymbolExpr<x_tag> x;
+  * struct y_tag {};  static const symbolic::SymbolExpr<y_tag> y;
+  * struct z_tag {};  static const symbolic::SymbolExpr<z_tag> z;
+  *
+  * // Defines an expression:
+  * auto expr = (x+3)/y+z;
+  *
+  * // And evaluate it: (c++14)
+  * std::cout << expr.eval(x=6,y=3,z=-13) << "\n";
+  *
+  * // In c++98/11, only one symbol per expression is supported for now:
+  * auto expr98 = (3-x)/2;
+  * std::cout << expr98.eval(x=6) << "\n";
+  * \endcode
+  *
+  * It is currently only used internally to define and manipulate the Eigen::last and Eigen::lastp1 symbols in Eigen::seq and Eigen::seqN.
+  *
+  */
+namespace symbolic {
+
+template<typename Tag> class Symbol;
+template<typename Arg0> class NegateExpr;
+template<typename Arg1,typename Arg2> class AddExpr;
+template<typename Arg1,typename Arg2> class ProductExpr;
+template<typename Arg1,typename Arg2> class QuotientExpr;
+
+// A simple wrapper around an integral value to provide the eval method.
+// We could also use a free-function symbolic_eval...
+template<typename IndexType=Index>
+class ValueExpr {
+public:
+  ValueExpr(IndexType val) : m_value(val) {}
+  template<typename T>
+  IndexType eval_impl(const T&) const { return m_value; }
+protected:
+  IndexType m_value;
+};
+
+// Specialization for compile-time value,
+// It is similar to ValueExpr(N) but this version helps the compiler to generate better code.
+template<int N>
+class ValueExpr<internal::FixedInt<N> > {
+public:
+  ValueExpr() {}
+  template<typename T>
+  EIGEN_CONSTEXPR Index eval_impl(const T&) const { return N; }
+};
+
+
+/** \class BaseExpr
+  * \ingroup Core_Module
+  * Common base class of any symbolic expressions
+  */
+template<typename Derived>
+class BaseExpr
+{
+public:
+  const Derived& derived() const { return *static_cast<const Derived*>(this); }
+
+  /** Evaluate the expression given the \a values of the symbols.
+    *
+    * \param values defines the values of the symbols, it can either be a SymbolValue or a std::tuple of SymbolValue
+    *               as constructed by SymbolExpr::operator= operator.
+    *
+    */
+  template<typename T>
+  Index eval(const T& values) const { return derived().eval_impl(values); }
+
+#if EIGEN_HAS_CXX14
+  template<typename... Types>
+  Index eval(Types&&... values) const { return derived().eval_impl(std::make_tuple(values...)); }
+#endif
+
+  NegateExpr<Derived> operator-() const { return NegateExpr<Derived>(derived()); }
+
+  AddExpr<Derived,ValueExpr<> > operator+(Index b) const
+  { return AddExpr<Derived,ValueExpr<> >(derived(),  b); }
+  AddExpr<Derived,ValueExpr<> > operator-(Index a) const
+  { return AddExpr<Derived,ValueExpr<> >(derived(), -a); }
+  ProductExpr<Derived,ValueExpr<> > operator*(Index a) const
+  { return ProductExpr<Derived,ValueExpr<> >(derived(),a); }
+  QuotientExpr<Derived,ValueExpr<> > operator/(Index a) const
+  { return QuotientExpr<Derived,ValueExpr<> >(derived(),a); }
+
+  friend AddExpr<Derived,ValueExpr<> > operator+(Index a, const BaseExpr& b)
+  { return AddExpr<Derived,ValueExpr<> >(b.derived(), a); }
+  friend AddExpr<NegateExpr<Derived>,ValueExpr<> > operator-(Index a, const BaseExpr& b)
+  { return AddExpr<NegateExpr<Derived>,ValueExpr<> >(-b.derived(), a); }
+  friend ProductExpr<ValueExpr<>,Derived> operator*(Index a, const BaseExpr& b)
+  { return ProductExpr<ValueExpr<>,Derived>(a,b.derived()); }
+  friend QuotientExpr<ValueExpr<>,Derived> operator/(Index a, const BaseExpr& b)
+  { return QuotientExpr<ValueExpr<>,Derived>(a,b.derived()); }
+
+  template<int N>
+  AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N>) const
+  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > > operator-(internal::FixedInt<N>) const
+  { return AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > >(derived(), ValueExpr<internal::FixedInt<-N> >()); }
+  template<int N>
+  ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator*(internal::FixedInt<N>) const
+  { return ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator/(internal::FixedInt<N>) const
+  { return QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
+
+  template<int N>
+  friend AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N>, const BaseExpr& b)
+  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(b.derived(), ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  friend AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > > operator-(internal::FixedInt<N>, const BaseExpr& b)
+  { return AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > >(-b.derived(), ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  friend ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator*(internal::FixedInt<N>, const BaseExpr& b)
+  { return ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
+  template<int N>
+  friend QuotientExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator/(internal::FixedInt<N>, const BaseExpr& b)
+  { return QuotientExpr<ValueExpr<internal::FixedInt<N> > ,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
+
+#if (!EIGEN_HAS_CXX14)
+  template<int N>
+  AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N> (*)()) const
+  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(), ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > > operator-(internal::FixedInt<N> (*)()) const
+  { return AddExpr<Derived,ValueExpr<internal::FixedInt<-N> > >(derived(), ValueExpr<internal::FixedInt<-N> >()); }
+  template<int N>
+  ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator*(internal::FixedInt<N> (*)()) const
+  { return ProductExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator/(internal::FixedInt<N> (*)()) const
+  { return QuotientExpr<Derived,ValueExpr<internal::FixedInt<N> > >(derived(),ValueExpr<internal::FixedInt<N> >()); }
+
+  template<int N>
+  friend AddExpr<Derived,ValueExpr<internal::FixedInt<N> > > operator+(internal::FixedInt<N> (*)(), const BaseExpr& b)
+  { return AddExpr<Derived,ValueExpr<internal::FixedInt<N> > >(b.derived(), ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  friend AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > > operator-(internal::FixedInt<N> (*)(), const BaseExpr& b)
+  { return AddExpr<NegateExpr<Derived>,ValueExpr<internal::FixedInt<N> > >(-b.derived(), ValueExpr<internal::FixedInt<N> >()); }
+  template<int N>
+  friend ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator*(internal::FixedInt<N> (*)(), const BaseExpr& b)
+  { return ProductExpr<ValueExpr<internal::FixedInt<N> >,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
+  template<int N>
+  friend QuotientExpr<ValueExpr<internal::FixedInt<N> >,Derived> operator/(internal::FixedInt<N> (*)(), const BaseExpr& b)
+  { return QuotientExpr<ValueExpr<internal::FixedInt<N> > ,Derived>(ValueExpr<internal::FixedInt<N> >(),b.derived()); }
+#endif
+
+
+  template<typename OtherDerived>
+  AddExpr<Derived,OtherDerived> operator+(const BaseExpr<OtherDerived> &b) const
+  { return AddExpr<Derived,OtherDerived>(derived(),  b.derived()); }
+
+  template<typename OtherDerived>
+  AddExpr<Derived,NegateExpr<OtherDerived> > operator-(const BaseExpr<OtherDerived> &b) const
+  { return AddExpr<Derived,NegateExpr<OtherDerived> >(derived(), -b.derived()); }
+
+  template<typename OtherDerived>
+  ProductExpr<Derived,OtherDerived> operator*(const BaseExpr<OtherDerived> &b) const
+  { return ProductExpr<Derived,OtherDerived>(derived(), b.derived()); }
+
+  template<typename OtherDerived>
+  QuotientExpr<Derived,OtherDerived> operator/(const BaseExpr<OtherDerived> &b) const
+  { return QuotientExpr<Derived,OtherDerived>(derived(), b.derived()); }
+};
+
+template<typename T>
+struct is_symbolic {
+  // BaseExpr has no conversion ctor, so we only have to check whether T can be statically cast to its base class BaseExpr<T>.
+  enum { value = internal::is_convertible<T,BaseExpr<T> >::value };
+};
+
+/** Represents the actual value of a symbol identified by its tag
+  *
+  * It is the return type of SymbolValue::operator=, and most of the time this is only way it is used.
+  */
+template<typename Tag>
+class SymbolValue
+{
+public:
+  /** Default constructor from the value \a val */
+  SymbolValue(Index val) : m_value(val) {}
+
+  /** \returns the stored value of the symbol */
+  Index value() const { return m_value; }
+protected:
+  Index m_value;
+};
+
+/** Expression of a symbol uniquely identified by the template parameter type \c tag */
+template<typename tag>
+class SymbolExpr : public BaseExpr<SymbolExpr<tag> >
+{
+public:
+  /** Alias to the template parameter \c tag */
+  typedef tag Tag;
+
+  SymbolExpr() {}
+
+  /** Associate the value \a val to the given symbol \c *this, uniquely identified by its \c Tag.
+    *
+    * The returned object should be passed to ExprBase::eval() to evaluate a given expression with this specified runtime-time value.
+    */
+  SymbolValue<Tag> operator=(Index val) const {
+    return SymbolValue<Tag>(val);
+  }
+
+  Index eval_impl(const SymbolValue<Tag> &values) const { return values.value(); }
+
+#if EIGEN_HAS_CXX14
+  // C++14 versions suitable for multiple symbols
+  template<typename... Types>
+  Index eval_impl(const std::tuple<Types...>& values) const { return std::get<SymbolValue<Tag> >(values).value(); }
+#endif
+};
+
+template<typename Arg0>
+class NegateExpr : public BaseExpr<NegateExpr<Arg0> >
+{
+public:
+  NegateExpr(const Arg0& arg0) : m_arg0(arg0) {}
+
+  template<typename T>
+  Index eval_impl(const T& values) const { return -m_arg0.eval_impl(values); }
+protected:
+  Arg0 m_arg0;
+};
+
+template<typename Arg0, typename Arg1>
+class AddExpr : public BaseExpr<AddExpr<Arg0,Arg1> >
+{
+public:
+  AddExpr(const Arg0& arg0, const Arg1& arg1) : m_arg0(arg0), m_arg1(arg1) {}
+
+  template<typename T>
+  Index eval_impl(const T& values) const { return m_arg0.eval_impl(values) + m_arg1.eval_impl(values); }
+protected:
+  Arg0 m_arg0;
+  Arg1 m_arg1;
+};
+
+template<typename Arg0, typename Arg1>
+class ProductExpr : public BaseExpr<ProductExpr<Arg0,Arg1> >
+{
+public:
+  ProductExpr(const Arg0& arg0, const Arg1& arg1) : m_arg0(arg0), m_arg1(arg1) {}
+
+  template<typename T>
+  Index eval_impl(const T& values) const { return m_arg0.eval_impl(values) * m_arg1.eval_impl(values); }
+protected:
+  Arg0 m_arg0;
+  Arg1 m_arg1;
+};
+
+template<typename Arg0, typename Arg1>
+class QuotientExpr : public BaseExpr<QuotientExpr<Arg0,Arg1> >
+{
+public:
+  QuotientExpr(const Arg0& arg0, const Arg1& arg1) : m_arg0(arg0), m_arg1(arg1) {}
+
+  template<typename T>
+  Index eval_impl(const T& values) const { return m_arg0.eval_impl(values) / m_arg1.eval_impl(values); }
+protected:
+  Arg0 m_arg0;
+  Arg1 m_arg1;
+};
+
+} // end namespace symbolic
+
+} // end namespace Eigen
+
+#endif // EIGEN_SYMBOLIC_INDEX_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
index bf424a0..71c32b8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Core/util/XprHelper.h
@@ -34,6 +34,26 @@
   return IndexDest(idx);
 }
 
+// true if T can be considered as an integral index (i.e., and integral type or enum)
+template<typename T> struct is_valid_index_type
+{
+  enum { value =
+#if EIGEN_HAS_TYPE_TRAITS
+    internal::is_integral<T>::value || std::is_enum<T>::value
+#elif EIGEN_COMP_MSVC
+    internal::is_integral<T>::value || __is_enum(T)
+#else
+    // without C++11, we use is_convertible to Index instead of is_integral in order to treat enums as Index.
+    internal::is_convertible<T,Index>::value && !internal::is_same<T,float>::value && !is_same<T,double>::value
+#endif
+  };
+};
+
+// true if both types are not valid index types
+template<typename RowIndices, typename ColIndices>
+struct valid_indexed_view_overload {
+  enum { value = !(internal::is_valid_index_type<RowIndices>::value && internal::is_valid_index_type<ColIndices>::value) };
+};
 
 // promote_scalar_arg is an helper used in operation between an expression and a scalar, like:
 //    expression * scalar
@@ -109,19 +129,23 @@
 template<typename T, int Value> class variable_if_dynamic
 {
   public:
-    EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamic)
+    EIGEN_DEFAULT_EMPTY_CONSTRUCTOR_AND_DESTRUCTOR(variable_if_dynamic)
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
-    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T value() { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
+    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    T value() { return T(Value); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    operator T() const { return T(Value); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    void setValue(T v) const { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
 };
 
 template<typename T> class variable_if_dynamic<T, Dynamic>
 {
     T m_value;
-    EIGEN_DEVICE_FUNC variable_if_dynamic() { eigen_assert(false); }
   public:
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value) : m_value(value) {}
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamic(T value = 0) EIGEN_NO_THROW : m_value(value) {}
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE T value() const { return m_value; }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE operator T() const { return m_value; }
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T value) { m_value = value; }
 };
 
@@ -132,8 +156,10 @@
   public:
     EIGEN_EMPTY_STRUCT_CTOR(variable_if_dynamicindex)
     EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE explicit variable_if_dynamicindex(T v) { EIGEN_ONLY_USED_FOR_DEBUG(v); eigen_assert(v == T(Value)); }
-    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE T value() { return T(Value); }
-    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE void setValue(T) {}
+    EIGEN_DEVICE_FUNC static EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+    T value() { return T(Value); }
+    EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+    void setValue(T) {}
 };
 
 template<typename T> class variable_if_dynamicindex<T, DynamicIndex>
@@ -158,16 +184,7 @@
 
 template<typename T> struct packet_traits;
 
-template<typename T> struct unpacket_traits
-{
-  typedef T type;
-  typedef T half;
-  enum
-  {
-    size = 1,
-    alignment = 1
-  };
-};
+template<typename T> struct unpacket_traits;
 
 template<int Size, typename PacketType,
          bool Stop = Size==Dynamic || (Size%unpacket_traits<PacketType>::size)==0 || is_same<PacketType,typename unpacket_traits<PacketType>::half>::value>
@@ -386,7 +403,7 @@
   typedef Matrix<typename traits<T>::Scalar,
                 Rows,
                 Cols,
-                (MaxCols==1&&MaxRows!=1) ? RowMajor : ColMajor,
+                (MaxCols==1&&MaxRows!=1) ? ColMajor : RowMajor,
                 MaxRows,
                 MaxCols
           > type;
@@ -403,7 +420,7 @@
     T const&,
     const T
   >::type type;
-  
+
   typedef typename conditional<
     bool(traits<T>::Flags & NestByRefBit),
     T &,
@@ -441,7 +458,7 @@
 {
   enum {
     ScalarReadCost = NumTraits<typename traits<T>::Scalar>::ReadCost,
-    CoeffReadCost = evaluator<T>::CoeffReadCost,  // NOTE What if an evaluator evaluate itself into a tempory?
+    CoeffReadCost = evaluator<T>::CoeffReadCost,  // NOTE What if an evaluator evaluate itself into a temporary?
                                                   //      Then CoeffReadCost will be small (e.g., 1) but we still have to evaluate, especially if n>1.
                                                   //      This situation is already taken care by the EvalBeforeNestingBit flag, which is turned ON
                                                   //      for all evaluator creating a temporary. This flag is then propagated by the parent evaluators.
@@ -582,14 +599,14 @@
 struct plain_row_type
 {
   typedef Matrix<Scalar, 1, ExpressionType::ColsAtCompileTime,
-                 ExpressionType::PlainObject::Options | RowMajor, 1, ExpressionType::MaxColsAtCompileTime> MatrixRowType;
+                 int(ExpressionType::PlainObject::Options) | int(RowMajor), 1, ExpressionType::MaxColsAtCompileTime> MatrixRowType;
   typedef Array<Scalar, 1, ExpressionType::ColsAtCompileTime,
-                 ExpressionType::PlainObject::Options | RowMajor, 1, ExpressionType::MaxColsAtCompileTime> ArrayRowType;
+                 int(ExpressionType::PlainObject::Options) | int(RowMajor), 1, ExpressionType::MaxColsAtCompileTime> ArrayRowType;
 
   typedef typename conditional<
     is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
     MatrixRowType,
-    ArrayRowType 
+    ArrayRowType
   >::type type;
 };
 
@@ -604,7 +621,7 @@
   typedef typename conditional<
     is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
     MatrixColType,
-    ArrayColType 
+    ArrayColType
   >::type type;
 };
 
@@ -620,7 +637,7 @@
   typedef typename conditional<
     is_same< typename traits<ExpressionType>::XprKind, MatrixXpr >::value,
     MatrixDiagType,
-    ArrayDiagType 
+    ArrayDiagType
   >::type type;
 };
 
@@ -657,24 +674,39 @@
 template<typename T, int S> struct is_diagonal<DiagonalMatrix<T,S> >
 { enum { ret = true }; };
 
+
+template<typename T> struct is_identity
+{ enum { value = false }; };
+
+template<typename T> struct is_identity<CwiseNullaryOp<internal::scalar_identity_op<typename T::Scalar>, T> >
+{ enum { value = true }; };
+
+
 template<typename S1, typename S2> struct glue_shapes;
 template<> struct glue_shapes<DenseShape,TriangularShape> { typedef TriangularShape type;  };
 
 template<typename T1, typename T2>
-bool is_same_dense(const T1 &mat1, const T2 &mat2, typename enable_if<has_direct_access<T1>::ret&&has_direct_access<T2>::ret, T1>::type * = 0)
+struct possibly_same_dense {
+  enum { value = has_direct_access<T1>::ret && has_direct_access<T2>::ret && is_same<typename T1::Scalar,typename T2::Scalar>::value };
+};
+
+template<typename T1, typename T2>
+EIGEN_DEVICE_FUNC
+bool is_same_dense(const T1 &mat1, const T2 &mat2, typename enable_if<possibly_same_dense<T1,T2>::value>::type * = 0)
 {
   return (mat1.data()==mat2.data()) && (mat1.innerStride()==mat2.innerStride()) && (mat1.outerStride()==mat2.outerStride());
 }
 
 template<typename T1, typename T2>
-bool is_same_dense(const T1 &, const T2 &, typename enable_if<!(has_direct_access<T1>::ret&&has_direct_access<T2>::ret), T1>::type * = 0)
+EIGEN_DEVICE_FUNC
+bool is_same_dense(const T1 &, const T2 &, typename enable_if<!possibly_same_dense<T1,T2>::value>::type * = 0)
 {
   return false;
 }
 
 // Internal helper defining the cost of a scalar division for the type T.
 // The default heuristic can be specialized for each scalar type and architecture.
-template<typename T,bool Vectorized=false,typename EnaleIf = void>
+template<typename T,bool Vectorized=false,typename EnableIf = void>
 struct scalar_div_cost {
   enum { value = 8*NumTraits<T>::MulCost };
 };
@@ -721,7 +753,7 @@
   if(f&DirectAccessBit)             res += " | Direct";
   if(f&NestByRefBit)                res += " | NestByRef";
   if(f&NoPreferredStorageOrderBit)  res += " | NoPreferredStorageOrderBit";
-  
+
   return res;
 }
 #endif
@@ -818,7 +850,7 @@
 #define EIGEN_CHECK_BINARY_COMPATIBILIY(BINOP,LHS,RHS) \
   EIGEN_STATIC_ASSERT((Eigen::internal::has_ReturnType<ScalarBinaryOpTraits<LHS, RHS,BINOP> >::value), \
     YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY)
-    
+
 } // end namespace Eigen
 
 #endif // EIGEN_XPRHELPER_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
index dc5fae0..081e918 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexEigenSolver.h
@@ -214,7 +214,7 @@
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful, \c NoConvergence otherwise.
+      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
       */
     ComputationInfo info() const
     {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
index 7f38919..fc71468 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/ComplexSchur.h
@@ -212,7 +212,7 @@
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful, \c NoConvergence otherwise.
+      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
       */
     ComputationInfo info() const
     {
@@ -300,10 +300,13 @@
   ComplexScalar trace = t.coeff(0,0) + t.coeff(1,1);
   ComplexScalar eival1 = (trace + disc) / RealScalar(2);
   ComplexScalar eival2 = (trace - disc) / RealScalar(2);
-
-  if(numext::norm1(eival1) > numext::norm1(eival2))
+  RealScalar eival1_norm = numext::norm1(eival1);
+  RealScalar eival2_norm = numext::norm1(eival2);
+  // A division by zero can only occur if eival1==eival2==0.
+  // In this case, det==0, and all we have to do is checking that eival2_norm!=0
+  if(eival1_norm > eival2_norm)
     eival2 = det / eival1;
-  else
+  else if(eival2_norm!=RealScalar(0))
     eival1 = det / eival2;
 
   // choose the eigenvalue closest to the bottom entry of the diagonal
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
index f205b18..572b29e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/EigenSolver.h
@@ -110,7 +110,7 @@
       *
       * \sa compute() for an example.
       */
-    EigenSolver() : m_eivec(), m_eivalues(), m_isInitialized(false), m_realSchur(), m_matT(), m_tmp() {}
+    EigenSolver() : m_eivec(), m_eivalues(), m_isInitialized(false), m_eigenvectorsOk(false), m_realSchur(), m_matT(), m_tmp() {}
 
     /** \brief Default constructor with memory preallocation
       *
@@ -277,7 +277,7 @@
     template<typename InputType>
     EigenSolver& compute(const EigenBase<InputType>& matrix, bool computeEigenvectors = true);
 
-    /** \returns NumericalIssue if the input contains INF or NaN values or overflow occured. Returns Success otherwise. */
+    /** \returns NumericalIssue if the input contains INF or NaN values or overflow occurred. Returns Success otherwise. */
     ComputationInfo info() const
     {
       eigen_assert(m_isInitialized && "EigenSolver is not initialized.");
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
index 5f6bb82..d0f9091 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
@@ -121,7 +121,7 @@
       *
       * \returns    Reference to \c *this
       *
-      * Accoring to \p options, this function computes eigenvalues and (if requested)
+      * According to \p options, this function computes eigenvalues and (if requested)
       * the eigenvectors of one of the following three generalized eigenproblems:
       * - \c Ax_lBx: \f$ Ax = \lambda B x \f$
       * - \c ABx_lx: \f$ ABx = \lambda x \f$
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
index f647f69..1f21139 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/HessenbergDecomposition.h
@@ -267,7 +267,7 @@
 
   private:
 
-    typedef Matrix<Scalar, 1, Size, Options | RowMajor, 1, MaxSize> VectorType;
+    typedef Matrix<Scalar, 1, Size, int(Options) | int(RowMajor), 1, MaxSize> VectorType;
     typedef typename NumTraits<Scalar>::Real RealScalar;
     static void _compute(MatrixType& matA, CoeffVectorType& hCoeffs, VectorType& temp);
 
@@ -315,7 +315,7 @@
 
     // A = A H'
     matA.rightCols(remainingSize)
-        .applyHouseholderOnTheRight(matA.col(i).tail(remainingSize-1).conjugate(), numext::conj(h), &temp.coeffRef(0));
+        .applyHouseholderOnTheRight(matA.col(i).tail(remainingSize-1), numext::conj(h), &temp.coeffRef(0));
   }
 }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
index e4e4260..66e5a3d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
@@ -84,7 +84,7 @@
   * \sa SelfAdjointEigenSolver::eigenvalues(), MatrixBase::eigenvalues()
   */
 template<typename MatrixType, unsigned int UpLo> 
-inline typename SelfAdjointView<MatrixType, UpLo>::EigenvaluesReturnType
+EIGEN_DEVICE_FUNC inline typename SelfAdjointView<MatrixType, UpLo>::EigenvaluesReturnType
 SelfAdjointView<MatrixType, UpLo>::eigenvalues() const
 {
   PlainObject thisAsMatrix(*this);
@@ -147,7 +147,7 @@
   * \sa eigenvalues(), MatrixBase::operatorNorm()
   */
 template<typename MatrixType, unsigned int UpLo>
-inline typename SelfAdjointView<MatrixType, UpLo>::RealScalar
+EIGEN_DEVICE_FUNC inline typename SelfAdjointView<MatrixType, UpLo>::RealScalar
 SelfAdjointView<MatrixType, UpLo>::operatorNorm() const
 {
   return eigenvalues().cwiseAbs().maxCoeff();
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
index b3a910d..5091301 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealQZ.h
@@ -90,8 +90,9 @@
         m_Z(size, size),
         m_workspace(size*2),
         m_maxIters(400),
-        m_isInitialized(false)
-        { }
+        m_isInitialized(false),
+        m_computeQZ(true)
+      {}
 
       /** \brief Constructor; computes real QZ decomposition of given matrices
        * 
@@ -108,9 +109,11 @@
         m_Z(A.rows(),A.cols()),
         m_workspace(A.rows()*2),
         m_maxIters(400),
-        m_isInitialized(false) {
-          compute(A, B, computeQZ);
-        }
+        m_isInitialized(false),
+        m_computeQZ(true)
+      {
+        compute(A, B, computeQZ);
+      }
 
       /** \brief Returns matrix Q in the QZ decomposition. 
        *
@@ -161,7 +164,7 @@
 
       /** \brief Reports whether previous computation was successful.
        *
-       * \returns \c Success if computation was succesful, \c NoConvergence otherwise.
+       * \returns \c Success if computation was successful, \c NoConvergence otherwise.
        */
       ComputationInfo info() const
       {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
index 17ea903..7304ef3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/RealSchur.h
@@ -190,7 +190,7 @@
     RealSchur& computeFromHessenberg(const HessMatrixType& matrixH, const OrthMatrixType& matrixQ,  bool computeU);
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful, \c NoConvergence otherwise.
+      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
       */
     ComputationInfo info() const
     {
@@ -236,7 +236,7 @@
     typedef Matrix<Scalar,3,1> Vector3s;
 
     Scalar computeNormOfT();
-    Index findSmallSubdiagEntry(Index iu);
+    Index findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero);
     void splitOffTwoRows(Index iu, bool computeU, const Scalar& exshift);
     void computeShift(Index iu, Index iter, Scalar& exshift, Vector3s& shiftInfo);
     void initFrancisQRStep(Index il, Index iu, const Vector3s& shiftInfo, Index& im, Vector3s& firstHouseholderVector);
@@ -270,8 +270,13 @@
   // Step 1. Reduce to Hessenberg form
   m_hess.compute(matrix.derived()/scale);
 
-  // Step 2. Reduce to real Schur form  
-  computeFromHessenberg(m_hess.matrixH(), m_hess.matrixQ(), computeU);
+  // Step 2. Reduce to real Schur form
+  // Note: we copy m_hess.matrixQ() into m_matU here and not in computeFromHessenberg
+  //       to be able to pass our working-space buffer for the Householder to Dense evaluation.
+  m_workspaceVector.resize(matrix.cols());
+  if(computeU)
+    m_hess.matrixQ().evalTo(m_matU, m_workspaceVector);
+  computeFromHessenberg(m_hess.matrixH(), m_matU, computeU);
 
   m_matT *= scale;
   
@@ -284,13 +289,13 @@
   using std::abs;
 
   m_matT = matrixH;
-  if(computeU)
+  m_workspaceVector.resize(m_matT.cols());
+  if(computeU && !internal::is_same_dense(m_matU,matrixQ))
     m_matU = matrixQ;
   
   Index maxIters = m_maxIters;
   if (maxIters == -1)
     maxIters = m_maxIterationsPerRow * matrixH.rows();
-  m_workspaceVector.resize(m_matT.cols());
   Scalar* workspace = &m_workspaceVector.coeffRef(0);
 
   // The matrix m_matT is divided in three parts. 
@@ -302,12 +307,16 @@
   Index totalIter = 0; // iteration count for whole matrix
   Scalar exshift(0);   // sum of exceptional shifts
   Scalar norm = computeNormOfT();
+  // sub-diagonal entries smaller than considerAsZero will be treated as zero.
+  // We use eps^2 to enable more precision in small eigenvalues.
+  Scalar considerAsZero = numext::maxi<Scalar>( norm * numext::abs2(NumTraits<Scalar>::epsilon()),
+                                                (std::numeric_limits<Scalar>::min)() );
 
   if(norm!=Scalar(0))
   {
     while (iu >= 0)
     {
-      Index il = findSmallSubdiagEntry(iu);
+      Index il = findSmallSubdiagEntry(iu,considerAsZero);
 
       // Check for convergence
       if (il == iu) // One root found
@@ -364,14 +373,17 @@
 
 /** \internal Look for single small sub-diagonal element and returns its index */
 template<typename MatrixType>
-inline Index RealSchur<MatrixType>::findSmallSubdiagEntry(Index iu)
+inline Index RealSchur<MatrixType>::findSmallSubdiagEntry(Index iu, const Scalar& considerAsZero)
 {
   using std::abs;
   Index res = iu;
   while (res > 0)
   {
     Scalar s = abs(m_matT.coeff(res-1,res-1)) + abs(m_matT.coeff(res,res));
-    if (abs(m_matT.coeff(res,res-1)) <= NumTraits<Scalar>::epsilon() * s)
+
+    s = numext::maxi<Scalar>(s * NumTraits<Scalar>::epsilon(), considerAsZero);
+    
+    if (abs(m_matT.coeff(res,res-1)) <= s)
       break;
     res--;
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
index 9ddd553..1469236 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
@@ -20,7 +20,9 @@
 
 namespace internal {
 template<typename SolverType,int Size,bool IsComplex> struct direct_selfadjoint_eigenvalues;
+
 template<typename MatrixType, typename DiagType, typename SubDiagType>
+EIGEN_DEVICE_FUNC
 ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec);
 }
 
@@ -42,10 +44,14 @@
   * \f$ v \f$ such that \f$ Av = \lambda v \f$.  The eigenvalues of a
   * selfadjoint matrix are always real. If \f$ D \f$ is a diagonal matrix with
   * the eigenvalues on the diagonal, and \f$ V \f$ is a matrix with the
-  * eigenvectors as its columns, then \f$ A = V D V^{-1} \f$ (for selfadjoint
-  * matrices, the matrix \f$ V \f$ is always invertible). This is called the
+  * eigenvectors as its columns, then \f$ A = V D V^{-1} \f$. This is called the
   * eigendecomposition.
   *
+  * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal
+  * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then
+  * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is
+  * equal to its transpose, \f$ V^{-1} = V^T \f$.
+  *
   * The algorithm exploits the fact that the matrix is selfadjoint, making it
   * faster and more accurate than the general purpose eigenvalue algorithms
   * implemented in EigenSolver and ComplexEigenSolver.
@@ -119,7 +125,10 @@
         : m_eivec(),
           m_eivalues(),
           m_subdiag(),
-          m_isInitialized(false)
+          m_hcoeffs(),
+          m_info(InvalidInput),
+          m_isInitialized(false),
+          m_eigenvectorsOk(false)
     { }
 
     /** \brief Constructor, pre-allocates memory for dynamic-size matrices.
@@ -139,7 +148,9 @@
         : m_eivec(size, size),
           m_eivalues(size),
           m_subdiag(size > 1 ? size - 1 : 1),
-          m_isInitialized(false)
+          m_hcoeffs(size > 1 ? size - 1 : 1),
+          m_isInitialized(false),
+          m_eigenvectorsOk(false)
     {}
 
     /** \brief Constructor; computes eigendecomposition of given matrix.
@@ -163,7 +174,9 @@
       : m_eivec(matrix.rows(), matrix.cols()),
         m_eivalues(matrix.cols()),
         m_subdiag(matrix.rows() > 1 ? matrix.rows() - 1 : 1),
-        m_isInitialized(false)
+        m_hcoeffs(matrix.cols() > 1 ? matrix.cols() - 1 : 1),
+        m_isInitialized(false),
+        m_eigenvectorsOk(false)
     {
       compute(matrix.derived(), options);
     }
@@ -250,6 +263,11 @@
       * matrix \f$ A \f$, then the matrix returned by this function is the
       * matrix \f$ V \f$ in the eigendecomposition \f$ A = V D V^{-1} \f$.
       *
+      * For a selfadjoint matrix, \f$ V \f$ is unitary, meaning its inverse is equal
+      * to its adjoint, \f$ V^{-1} = V^{\dagger} \f$. If \f$ A \f$ is real, then
+      * \f$ V \f$ is also real and therefore orthogonal, meaning its inverse is
+      * equal to its transpose, \f$ V^{-1} = V^T \f$.
+      *
       * Example: \include SelfAdjointEigenSolver_eigenvectors.cpp
       * Output: \verbinclude SelfAdjointEigenSolver_eigenvectors.out
       *
@@ -337,7 +355,7 @@
 
     /** \brief Reports whether previous computation was successful.
       *
-      * \returns \c Success if computation was succesful, \c NoConvergence otherwise.
+      * \returns \c Success if computation was successful, \c NoConvergence otherwise.
       */
     EIGEN_DEVICE_FUNC
     ComputationInfo info() const
@@ -354,7 +372,8 @@
     static const int m_maxIterations = 30;
 
   protected:
-    static void check_template_parameters()
+    static EIGEN_DEVICE_FUNC
+    void check_template_parameters()
     {
       EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
     }
@@ -362,6 +381,7 @@
     EigenvectorsType m_eivec;
     RealVectorType m_eivalues;
     typename TridiagonalizationType::SubDiagonalType m_subdiag;
+    typename TridiagonalizationType::CoeffVectorType m_hcoeffs;
     ComputationInfo m_info;
     bool m_isInitialized;
     bool m_eigenvectorsOk;
@@ -403,7 +423,7 @@
   
   const InputType &matrix(a_matrix.derived());
   
-  using std::abs;
+  EIGEN_USING_STD(abs);
   eigen_assert(matrix.cols() == matrix.rows());
   eigen_assert((options&~(EigVecMask|GenEigMask))==0
           && (options&EigVecMask)!=EigVecMask
@@ -434,7 +454,8 @@
   if(scale==RealScalar(0)) scale = RealScalar(1);
   mat.template triangularView<Lower>() /= scale;
   m_subdiag.resize(n-1);
-  internal::tridiagonalization_inplace(mat, diag, m_subdiag, computeEigenvectors);
+  m_hcoeffs.resize(n-1);
+  internal::tridiagonalization_inplace(mat, diag, m_subdiag, m_hcoeffs, computeEigenvectors);
 
   m_info = internal::computeFromTridiagonal_impl(diag, m_subdiag, m_maxIterations, computeEigenvectors, m_eivec);
   
@@ -479,10 +500,9 @@
   * \returns \c Success or \c NoConvergence
   */
 template<typename MatrixType, typename DiagType, typename SubDiagType>
+EIGEN_DEVICE_FUNC
 ComputationInfo computeFromTridiagonal_impl(DiagType& diag, SubDiagType& subdiag, const Index maxIterations, bool computeEigenvectors, MatrixType& eivec)
 {
-  using std::abs;
-
   ComputationInfo info;
   typedef typename MatrixType::Scalar Scalar;
 
@@ -493,15 +513,23 @@
   
   typedef typename DiagType::RealScalar RealScalar;
   const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
-  const RealScalar precision = RealScalar(2)*NumTraits<RealScalar>::epsilon();
-  
+  const RealScalar precision_inv = RealScalar(1)/NumTraits<RealScalar>::epsilon();
   while (end>0)
   {
-    for (Index i = start; i<end; ++i)
-      if (internal::isMuchSmallerThan(abs(subdiag[i]),(abs(diag[i])+abs(diag[i+1])),precision) || abs(subdiag[i]) <= considerAsZero)
-        subdiag[i] = 0;
+    for (Index i = start; i<end; ++i) {
+      if (numext::abs(subdiag[i]) < considerAsZero) {
+        subdiag[i] = RealScalar(0);
+      } else {
+        // abs(subdiag[i]) <= epsilon * sqrt(abs(diag[i]) + abs(diag[i+1]))
+        // Scaled to prevent underflows.
+        const RealScalar scaled_subdiag = precision_inv * subdiag[i];
+        if (scaled_subdiag * scaled_subdiag <= (numext::abs(diag[i])+numext::abs(diag[i+1]))) {
+          subdiag[i] = RealScalar(0);
+        }
+      }
+    }
 
-    // find the largest unreduced block
+    // find the largest unreduced block at the end of the matrix.
     while (end>0 && subdiag[end-1]==RealScalar(0))
     {
       end--;
@@ -535,7 +563,7 @@
       diag.segment(i,n-i).minCoeff(&k);
       if (k > 0)
       {
-        std::swap(diag[i], diag[k+i]);
+        numext::swap(diag[i], diag[k+i]);
         if(computeEigenvectors)
           eivec.col(i).swap(eivec.col(k+i));
       }
@@ -566,10 +594,10 @@
   EIGEN_DEVICE_FUNC
   static inline void computeRoots(const MatrixType& m, VectorType& roots)
   {
-    EIGEN_USING_STD_MATH(sqrt)
-    EIGEN_USING_STD_MATH(atan2)
-    EIGEN_USING_STD_MATH(cos)
-    EIGEN_USING_STD_MATH(sin)
+    EIGEN_USING_STD(sqrt)
+    EIGEN_USING_STD(atan2)
+    EIGEN_USING_STD(cos)
+    EIGEN_USING_STD(sin)
     const Scalar s_inv3 = Scalar(1)/Scalar(3);
     const Scalar s_sqrt3 = sqrt(Scalar(3));
 
@@ -605,7 +633,8 @@
   EIGEN_DEVICE_FUNC
   static inline bool extract_kernel(MatrixType& mat, Ref<VectorType> res, Ref<VectorType> representative)
   {
-    using std::abs;
+    EIGEN_USING_STD(abs);
+    EIGEN_USING_STD(sqrt);
     Index i0;
     // Find non-zero column i0 (by construction, there must exist a non zero coefficient on the diagonal):
     mat.diagonal().cwiseAbs().maxCoeff(&i0);
@@ -616,8 +645,8 @@
     VectorType c0, c1;
     n0 = (c0 = representative.cross(mat.col((i0+1)%3))).squaredNorm();
     n1 = (c1 = representative.cross(mat.col((i0+2)%3))).squaredNorm();
-    if(n0>n1) res = c0/std::sqrt(n0);
-    else      res = c1/std::sqrt(n1);
+    if(n0>n1) res = c0/sqrt(n0);
+    else      res = c1/sqrt(n1);
 
     return true;
   }
@@ -719,7 +748,7 @@
   EIGEN_DEVICE_FUNC
   static inline void computeRoots(const MatrixType& m, VectorType& roots)
   {
-    using std::sqrt;
+    EIGEN_USING_STD(sqrt);
     const Scalar t0 = Scalar(0.5) * sqrt( numext::abs2(m(0,0)-m(1,1)) + Scalar(4)*numext::abs2(m(1,0)));
     const Scalar t1 = Scalar(0.5) * (m(0,0) + m(1,1));
     roots(0) = t1 - t0;
@@ -729,8 +758,8 @@
   EIGEN_DEVICE_FUNC
   static inline void run(SolverType& solver, const MatrixType& mat, int options)
   {
-    EIGEN_USING_STD_MATH(sqrt);
-    EIGEN_USING_STD_MATH(abs);
+    EIGEN_USING_STD(sqrt);
+    EIGEN_USING_STD(abs);
     
     eigen_assert(mat.cols() == 2 && mat.cols() == mat.rows());
     eigen_assert((options&~(EigVecMask|GenEigMask))==0
@@ -803,32 +832,38 @@
 }
 
 namespace internal {
+
+// Francis implicit QR step.
 template<int StorageOrder,typename RealScalar, typename Scalar, typename Index>
 EIGEN_DEVICE_FUNC
 static void tridiagonal_qr_step(RealScalar* diag, RealScalar* subdiag, Index start, Index end, Scalar* matrixQ, Index n)
 {
-  using std::abs;
+  // Wilkinson Shift.
   RealScalar td = (diag[end-1] - diag[end])*RealScalar(0.5);
   RealScalar e = subdiag[end-1];
   // Note that thanks to scaling, e^2 or td^2 cannot overflow, however they can still
   // underflow thus leading to inf/NaN values when using the following commented code:
-//   RealScalar e2 = numext::abs2(subdiag[end-1]);
-//   RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
+  //   RealScalar e2 = numext::abs2(subdiag[end-1]);
+  //   RealScalar mu = diag[end] - e2 / (td + (td>0 ? 1 : -1) * sqrt(td*td + e2));
   // This explain the following, somewhat more complicated, version:
   RealScalar mu = diag[end];
-  if(td==RealScalar(0))
-    mu -= abs(e);
-  else
-  {
-    RealScalar e2 = numext::abs2(subdiag[end-1]);
-    RealScalar h = numext::hypot(td,e);
-    if(e2==RealScalar(0)) mu -= (e / (td + (td>RealScalar(0) ? RealScalar(1) : RealScalar(-1)))) * (e / h);
-    else                  mu -= e2 / (td + (td>RealScalar(0) ? h : -h));
+  if(td==RealScalar(0)) {
+    mu -= numext::abs(e);
+  } else if (e != RealScalar(0)) {
+    const RealScalar e2 = numext::abs2(e);
+    const RealScalar h = numext::hypot(td,e);
+    if(e2 == RealScalar(0)) {
+      mu -= e / ((td + (td>RealScalar(0) ? h : -h)) / e);
+    } else {
+      mu -= e2 / (td + (td>RealScalar(0) ? h : -h)); 
+    }
   }
-  
+
   RealScalar x = diag[start] - mu;
   RealScalar z = subdiag[start];
-  for (Index k = start; k < end; ++k)
+  // If z ever becomes zero, the Givens rotation will be the identity and
+  // z will stay zero for all future iterations.
+  for (Index k = start; k < end && z != RealScalar(0); ++k)
   {
     JacobiRotation<RealScalar> rot;
     rot.makeGivens(x, z);
@@ -841,12 +876,11 @@
     diag[k+1] = rot.s() * sdk + rot.c() * dkp1;
     subdiag[k] = rot.c() * sdk - rot.s() * dkp1;
     
-
     if (k > start)
       subdiag[k - 1] = rot.c() * subdiag[k-1] - rot.s() * z;
 
+    // "Chasing the bulge" to return to triangular form.
     x = subdiag[k];
-
     if (k < end - 1)
     {
       z = -rot.s() * subdiag[k+1];
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
index 1d102c1..674c92a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Eigenvalues/Tridiagonalization.h
@@ -11,10 +11,10 @@
 #ifndef EIGEN_TRIDIAGONALIZATION_H
 #define EIGEN_TRIDIAGONALIZATION_H
 
-namespace Eigen { 
+namespace Eigen {
 
 namespace internal {
-  
+
 template<typename MatrixType> struct TridiagonalizationMatrixTReturnType;
 template<typename MatrixType>
 struct traits<TridiagonalizationMatrixTReturnType<MatrixType> >
@@ -25,6 +25,7 @@
 };
 
 template<typename MatrixType, typename CoeffVectorType>
+EIGEN_DEVICE_FUNC
 void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs);
 }
 
@@ -344,6 +345,7 @@
   * \sa Tridiagonalization::packedMatrix()
   */
 template<typename MatrixType, typename CoeffVectorType>
+EIGEN_DEVICE_FUNC
 void tridiagonalization_inplace(MatrixType& matA, CoeffVectorType& hCoeffs)
 {
   using numext::conj;
@@ -352,7 +354,7 @@
   Index n = matA.rows();
   eigen_assert(n==matA.cols());
   eigen_assert(n==hCoeffs.size()+1 || n==1);
-  
+
   for (Index i = 0; i<n-1; ++i)
   {
     Index remainingSize = n-i-1;
@@ -423,11 +425,13 @@
   *
   * \sa class Tridiagonalization
   */
-template<typename MatrixType, typename DiagonalType, typename SubDiagonalType>
-void tridiagonalization_inplace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
+template<typename MatrixType, typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType>
+EIGEN_DEVICE_FUNC
+void tridiagonalization_inplace(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag,
+                                CoeffVectorType& hcoeffs, bool extractQ)
 {
   eigen_assert(mat.cols()==mat.rows() && diag.size()==mat.rows() && subdiag.size()==mat.rows()-1);
-  tridiagonalization_inplace_selector<MatrixType>::run(mat, diag, subdiag, extractQ);
+  tridiagonalization_inplace_selector<MatrixType>::run(mat, diag, subdiag, hcoeffs, extractQ);
 }
 
 /** \internal
@@ -439,10 +443,10 @@
   typedef typename Tridiagonalization<MatrixType>::CoeffVectorType CoeffVectorType;
   typedef typename Tridiagonalization<MatrixType>::HouseholderSequenceType HouseholderSequenceType;
   template<typename DiagonalType, typename SubDiagonalType>
-  static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
+  static EIGEN_DEVICE_FUNC
+      void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType& hCoeffs, bool extractQ)
   {
-    CoeffVectorType hCoeffs(mat.cols()-1);
-    tridiagonalization_inplace(mat,hCoeffs);
+    tridiagonalization_inplace(mat, hCoeffs);
     diag = mat.diagonal().real();
     subdiag = mat.template diagonal<-1>().real();
     if(extractQ)
@@ -462,8 +466,8 @@
   typedef typename MatrixType::Scalar Scalar;
   typedef typename MatrixType::RealScalar RealScalar;
 
-  template<typename DiagonalType, typename SubDiagonalType>
-  static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, bool extractQ)
+  template<typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType>
+  static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType& subdiag, CoeffVectorType&, bool extractQ)
   {
     using std::sqrt;
     const RealScalar tol = (std::numeric_limits<RealScalar>::min)();
@@ -507,8 +511,9 @@
 {
   typedef typename MatrixType::Scalar Scalar;
 
-  template<typename DiagonalType, typename SubDiagonalType>
-  static void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType&, bool extractQ)
+  template<typename DiagonalType, typename SubDiagonalType, typename CoeffVectorType>
+  static EIGEN_DEVICE_FUNC
+  void run(MatrixType& mat, DiagonalType& diag, SubDiagonalType&, CoeffVectorType&, bool extractQ)
   {
     diag(0,0) = numext::real(mat(0,0));
     if(extractQ)
@@ -542,8 +547,8 @@
       result.template diagonal<-1>() = m_matrix.template diagonal<-1>();
     }
 
-    Index rows() const { return m_matrix.rows(); }
-    Index cols() const { return m_matrix.cols(); }
+    EIGEN_CONSTEXPR Index rows() const EIGEN_NOEXCEPT { return m_matrix.rows(); }
+    EIGEN_CONSTEXPR Index cols() const EIGEN_NOEXCEPT { return m_matrix.cols(); }
 
   protected:
     typename MatrixType::Nested m_matrix;
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
index 01a7ed1..39ce1c2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/BlockHouseholder.h
@@ -63,8 +63,15 @@
       triFactor.row(i).tail(rt).noalias() = -hCoeffs(i) * vectors.col(i).tail(rs).adjoint()
                                                         * vectors.bottomRightCorner(rs, rt).template triangularView<UnitLower>();
             
-      // FIXME add .noalias() once the triangular product can work inplace
-      triFactor.row(i).tail(rt) = triFactor.row(i).tail(rt) * triFactor.bottomRightCorner(rt,rt).template triangularView<Upper>();
+      // FIXME use the following line with .noalias() once the triangular product can work inplace
+      // triFactor.row(i).tail(rt) = triFactor.row(i).tail(rt) * triFactor.bottomRightCorner(rt,rt).template triangularView<Upper>();
+      for(Index j=nbVecs-1; j>i; --j)
+      {
+        typename TriangularFactorType::Scalar z = triFactor(i,j);
+        triFactor(i,j) = z * triFactor(j,j);
+        if(nbVecs-j-1>0)
+          triFactor.row(i).tail(nbVecs-j-1) += z * triFactor.row(j).tail(nbVecs-j-1);
+      }
       
     }
     triFactor(i,i) = hCoeffs(i);
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
index 80de2c3..5bc037f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/Householder.h
@@ -39,6 +39,7 @@
   *     MatrixBase::applyHouseholderOnTheRight()
   */
 template<typename Derived>
+EIGEN_DEVICE_FUNC
 void MatrixBase<Derived>::makeHouseholderInPlace(Scalar& tau, RealScalar& beta)
 {
   VectorBlock<Derived, internal::decrement_size<Base::SizeAtCompileTime>::ret> essentialPart(derived(), 1, size()-1);
@@ -62,6 +63,7 @@
   */
 template<typename Derived>
 template<typename EssentialPart>
+EIGEN_DEVICE_FUNC
 void MatrixBase<Derived>::makeHouseholder(
   EssentialPart& essential,
   Scalar& tau,
@@ -103,13 +105,14 @@
   * \param essential the essential part of the vector \c v
   * \param tau the scaling factor of the Householder transformation
   * \param workspace a pointer to working space with at least
-  *                  this->cols() * essential.size() entries
+  *                  this->cols() entries
   *
   * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(), 
   *     MatrixBase::applyHouseholderOnTheRight()
   */
 template<typename Derived>
 template<typename EssentialPart>
+EIGEN_DEVICE_FUNC
 void MatrixBase<Derived>::applyHouseholderOnTheLeft(
   const EssentialPart& essential,
   const Scalar& tau,
@@ -140,13 +143,14 @@
   * \param essential the essential part of the vector \c v
   * \param tau the scaling factor of the Householder transformation
   * \param workspace a pointer to working space with at least
-  *                  this->cols() * essential.size() entries
+  *                  this->rows() entries
   *
   * \sa MatrixBase::makeHouseholder(), MatrixBase::makeHouseholderInPlace(), 
   *     MatrixBase::applyHouseholderOnTheLeft()
   */
 template<typename Derived>
 template<typename EssentialPart>
+EIGEN_DEVICE_FUNC
 void MatrixBase<Derived>::applyHouseholderOnTheRight(
   const EssentialPart& essential,
   const Scalar& tau,
@@ -160,10 +164,10 @@
   {
     Map<typename internal::plain_col_type<PlainObject>::type> tmp(workspace,rows());
     Block<Derived, Derived::RowsAtCompileTime, EssentialPart::SizeAtCompileTime> right(derived(), 0, 1, rows(), cols()-1);
-    tmp.noalias() = right * essential.conjugate();
+    tmp.noalias() = right * essential;
     tmp += this->col(0);
     this->col(0) -= tau * tmp;
-    right.noalias() -= tau * tmp * essential.transpose();
+    right.noalias() -= tau * tmp * essential.adjoint();
   }
 }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
index 3ce0a69..022f6c3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Householder/HouseholderSequence.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_HOUSEHOLDER_SEQUENCE_H
 #define EIGEN_HOUSEHOLDER_SEQUENCE_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \ingroup Householder_Module
   * \householder_module
@@ -34,8 +34,8 @@
   * form \f$ H = \prod_{i=0}^{n-1} H_i \f$ where the i-th Householder reflection is \f$ H_i = I - h_i v_i
   * v_i^* \f$. The i-th Householder coefficient \f$ h_i \f$ is a scalar and the i-th Householder vector \f$
   * v_i \f$ is a vector of the form
-  * \f[ 
-  * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ]. 
+  * \f[
+  * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ].
   * \f]
   * The last \f$ n-i \f$ entries of \f$ v_i \f$ are called the essential part of the Householder vector.
   *
@@ -87,7 +87,7 @@
 {
   typedef Block<const VectorsType, Dynamic, 1> EssentialVectorType;
   typedef HouseholderSequence<VectorsType, CoeffsType, OnTheLeft> HouseholderSequenceType;
-  static inline const EssentialVectorType essentialVector(const HouseholderSequenceType& h, Index k)
+  static EIGEN_DEVICE_FUNC inline const EssentialVectorType essentialVector(const HouseholderSequenceType& h, Index k)
   {
     Index start = k+1+h.m_shift;
     return Block<const VectorsType,Dynamic,1>(h.m_vectors, start, k, h.rows()-start, 1);
@@ -120,7 +120,7 @@
   : public EigenBase<HouseholderSequence<VectorsType,CoeffsType,Side> >
 {
     typedef typename internal::hseq_side_dependent_impl<VectorsType,CoeffsType,Side>::EssentialVectorType EssentialVectorType;
-  
+
   public:
     enum {
       RowsAtCompileTime = internal::traits<HouseholderSequence>::RowsAtCompileTime,
@@ -140,6 +140,28 @@
       Side
     > ConjugateReturnType;
 
+    typedef HouseholderSequence<
+      VectorsType,
+      typename internal::conditional<NumTraits<Scalar>::IsComplex,
+        typename internal::remove_all<typename CoeffsType::ConjugateReturnType>::type,
+        CoeffsType>::type,
+      Side
+    > AdjointReturnType;
+
+    typedef HouseholderSequence<
+      typename internal::conditional<NumTraits<Scalar>::IsComplex,
+        typename internal::remove_all<typename VectorsType::ConjugateReturnType>::type,
+        VectorsType>::type,
+      CoeffsType,
+      Side
+    > TransposeReturnType;
+
+    typedef HouseholderSequence<
+      typename internal::add_const<VectorsType>::type,
+      typename internal::add_const<CoeffsType>::type,
+      Side
+    > ConstHouseholderSequence;
+
     /** \brief Constructor.
       * \param[in]  v      %Matrix containing the essential parts of the Householder vectors
       * \param[in]  h      Vector containing the Householder coefficients
@@ -157,33 +179,37 @@
       *
       * \sa setLength(), setShift()
       */
+    EIGEN_DEVICE_FUNC
     HouseholderSequence(const VectorsType& v, const CoeffsType& h)
-      : m_vectors(v), m_coeffs(h), m_trans(false), m_length(v.diagonalSize()),
+      : m_vectors(v), m_coeffs(h), m_reverse(false), m_length(v.diagonalSize()),
         m_shift(0)
     {
     }
 
     /** \brief Copy constructor. */
+    EIGEN_DEVICE_FUNC
     HouseholderSequence(const HouseholderSequence& other)
       : m_vectors(other.m_vectors),
         m_coeffs(other.m_coeffs),
-        m_trans(other.m_trans),
+        m_reverse(other.m_reverse),
         m_length(other.m_length),
         m_shift(other.m_shift)
     {
     }
 
     /** \brief Number of rows of transformation viewed as a matrix.
-      * \returns Number of rows 
+      * \returns Number of rows
       * \details This equals the dimension of the space that the transformation acts on.
       */
-    Index rows() const { return Side==OnTheLeft ? m_vectors.rows() : m_vectors.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index rows() const EIGEN_NOEXCEPT { return Side==OnTheLeft ? m_vectors.rows() : m_vectors.cols(); }
 
     /** \brief Number of columns of transformation viewed as a matrix.
       * \returns Number of columns
       * \details This equals the dimension of the space that the transformation acts on.
       */
-    Index cols() const { return rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    Index cols() const EIGEN_NOEXCEPT { return rows(); }
 
     /** \brief Essential part of a Householder vector.
       * \param[in]  k  Index of Householder reflection
@@ -191,14 +217,15 @@
       *
       * This function returns the essential part of the Householder vector \f$ v_i \f$. This is a vector of
       * length \f$ n-i \f$ containing the last \f$ n-i \f$ entries of the vector
-      * \f[ 
-      * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ]. 
+      * \f[
+      * v_i = [\underbrace{0, \ldots, 0}_{i-1\mbox{ zeros}}, 1, \underbrace{*, \ldots,*}_{n-i\mbox{ arbitrary entries}} ].
       * \f]
       * The index \f$ i \f$ equals \p k + shift(), corresponding to the k-th column of the matrix \p v
       * passed to the constructor.
       *
       * \sa setShift(), shift()
       */
+    EIGEN_DEVICE_FUNC
     const EssentialVectorType essentialVector(Index k) const
     {
       eigen_assert(k >= 0 && k < m_length);
@@ -206,31 +233,51 @@
     }
 
     /** \brief %Transpose of the Householder sequence. */
-    HouseholderSequence transpose() const
+    TransposeReturnType transpose() const
     {
-      return HouseholderSequence(*this).setTrans(!m_trans);
+      return TransposeReturnType(m_vectors.conjugate(), m_coeffs)
+              .setReverseFlag(!m_reverse)
+              .setLength(m_length)
+              .setShift(m_shift);
     }
 
     /** \brief Complex conjugate of the Householder sequence. */
     ConjugateReturnType conjugate() const
     {
       return ConjugateReturnType(m_vectors.conjugate(), m_coeffs.conjugate())
-             .setTrans(m_trans)
+             .setReverseFlag(m_reverse)
              .setLength(m_length)
              .setShift(m_shift);
     }
 
-    /** \brief Adjoint (conjugate transpose) of the Householder sequence. */
-    ConjugateReturnType adjoint() const
+    /** \returns an expression of the complex conjugate of \c *this if Cond==true,
+     *           returns \c *this otherwise.
+     */
+    template<bool Cond>
+    EIGEN_DEVICE_FUNC
+    inline typename internal::conditional<Cond,ConjugateReturnType,ConstHouseholderSequence>::type
+    conjugateIf() const
     {
-      return conjugate().setTrans(!m_trans);
+      typedef typename internal::conditional<Cond,ConjugateReturnType,ConstHouseholderSequence>::type ReturnType;
+      return ReturnType(m_vectors.template conjugateIf<Cond>(), m_coeffs.template conjugateIf<Cond>());
+    }
+
+    /** \brief Adjoint (conjugate transpose) of the Householder sequence. */
+    AdjointReturnType adjoint() const
+    {
+      return AdjointReturnType(m_vectors, m_coeffs.conjugate())
+              .setReverseFlag(!m_reverse)
+              .setLength(m_length)
+              .setShift(m_shift);
     }
 
     /** \brief Inverse of the Householder sequence (equals the adjoint). */
-    ConjugateReturnType inverse() const { return adjoint(); }
+    AdjointReturnType inverse() const { return adjoint(); }
 
     /** \internal */
-    template<typename DestType> inline void evalTo(DestType& dst) const
+    template<typename DestType>
+    inline EIGEN_DEVICE_FUNC
+    void evalTo(DestType& dst) const
     {
       Matrix<Scalar, DestType::RowsAtCompileTime, 1,
              AutoAlign|ColMajor, DestType::MaxRowsAtCompileTime, 1> workspace(rows());
@@ -239,6 +286,7 @@
 
     /** \internal */
     template<typename Dest, typename Workspace>
+    EIGEN_DEVICE_FUNC
     void evalTo(Dest& dst, Workspace& workspace) const
     {
       workspace.resize(rows());
@@ -251,7 +299,7 @@
         for(Index k = vecs-1; k >= 0; --k)
         {
           Index cornerSize = rows() - k - m_shift;
-          if(m_trans)
+          if(m_reverse)
             dst.bottomRightCorner(cornerSize, cornerSize)
                .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), workspace.data());
           else
@@ -265,18 +313,26 @@
         for(Index k = 0; k<cols()-vecs ; ++k)
           dst.col(k).tail(rows()-k-1).setZero();
       }
+      else if(m_length>BlockSize)
+      {
+        dst.setIdentity(rows(), rows());
+        if(m_reverse)
+          applyThisOnTheLeft(dst,workspace,true);
+        else
+          applyThisOnTheLeft(dst,workspace,true);
+      }
       else
       {
         dst.setIdentity(rows(), rows());
         for(Index k = vecs-1; k >= 0; --k)
         {
           Index cornerSize = rows() - k - m_shift;
-          if(m_trans)
+          if(m_reverse)
             dst.bottomRightCorner(cornerSize, cornerSize)
-               .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), &workspace.coeffRef(0));
+               .applyHouseholderOnTheRight(essentialVector(k), m_coeffs.coeff(k), workspace.data());
           else
             dst.bottomRightCorner(cornerSize, cornerSize)
-               .applyHouseholderOnTheLeft(essentialVector(k), m_coeffs.coeff(k), &workspace.coeffRef(0));
+               .applyHouseholderOnTheLeft(essentialVector(k), m_coeffs.coeff(k), workspace.data());
         }
       }
     }
@@ -295,42 +351,52 @@
       workspace.resize(dst.rows());
       for(Index k = 0; k < m_length; ++k)
       {
-        Index actual_k = m_trans ? m_length-k-1 : k;
+        Index actual_k = m_reverse ? m_length-k-1 : k;
         dst.rightCols(rows()-m_shift-actual_k)
            .applyHouseholderOnTheRight(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
       }
     }
 
     /** \internal */
-    template<typename Dest> inline void applyThisOnTheLeft(Dest& dst) const
+    template<typename Dest> inline void applyThisOnTheLeft(Dest& dst, bool inputIsIdentity = false) const
     {
       Matrix<Scalar,1,Dest::ColsAtCompileTime,RowMajor,1,Dest::MaxColsAtCompileTime> workspace;
-      applyThisOnTheLeft(dst, workspace);
+      applyThisOnTheLeft(dst, workspace, inputIsIdentity);
     }
 
     /** \internal */
     template<typename Dest, typename Workspace>
-    inline void applyThisOnTheLeft(Dest& dst, Workspace& workspace) const
+    inline void applyThisOnTheLeft(Dest& dst, Workspace& workspace, bool inputIsIdentity = false) const
     {
-      const Index BlockSize = 48;
+      if(inputIsIdentity && m_reverse)
+        inputIsIdentity = false;
       // if the entries are large enough, then apply the reflectors by block
       if(m_length>=BlockSize && dst.cols()>1)
       {
-        for(Index i = 0; i < m_length; i+=BlockSize)
+        // Make sure we have at least 2 useful blocks, otherwise it is point-less:
+        Index blockSize = m_length<Index(2*BlockSize) ? (m_length+1)/2 : Index(BlockSize);
+        for(Index i = 0; i < m_length; i+=blockSize)
         {
-          Index end = m_trans ? (std::min)(m_length,i+BlockSize) : m_length-i;
-          Index k = m_trans ? i : (std::max)(Index(0),end-BlockSize);
+          Index end = m_reverse ? (std::min)(m_length,i+blockSize) : m_length-i;
+          Index k = m_reverse ? i : (std::max)(Index(0),end-blockSize);
           Index bs = end-k;
           Index start = k + m_shift;
-          
+
           typedef Block<typename internal::remove_all<VectorsType>::type,Dynamic,Dynamic> SubVectorsType;
           SubVectorsType sub_vecs1(m_vectors.const_cast_derived(), Side==OnTheRight ? k : start,
                                                                    Side==OnTheRight ? start : k,
                                                                    Side==OnTheRight ? bs : m_vectors.rows()-start,
                                                                    Side==OnTheRight ? m_vectors.cols()-start : bs);
           typename internal::conditional<Side==OnTheRight, Transpose<SubVectorsType>, SubVectorsType&>::type sub_vecs(sub_vecs1);
-          Block<Dest,Dynamic,Dynamic> sub_dst(dst,dst.rows()-rows()+m_shift+k,0, rows()-m_shift-k,dst.cols());
-          apply_block_householder_on_the_left(sub_dst, sub_vecs, m_coeffs.segment(k, bs), !m_trans);
+
+          Index dstStart = dst.rows()-rows()+m_shift+k;
+          Index dstRows  = rows()-m_shift-k;
+          Block<Dest,Dynamic,Dynamic> sub_dst(dst,
+                                              dstStart,
+                                              inputIsIdentity ? dstStart : 0,
+                                              dstRows,
+                                              inputIsIdentity ? dstRows : dst.cols());
+          apply_block_householder_on_the_left(sub_dst, sub_vecs, m_coeffs.segment(k, bs), !m_reverse);
         }
       }
       else
@@ -338,8 +404,9 @@
         workspace.resize(dst.cols());
         for(Index k = 0; k < m_length; ++k)
         {
-          Index actual_k = m_trans ? k : m_length-k-1;
-          dst.bottomRows(rows()-m_shift-actual_k)
+          Index actual_k = m_reverse ? k : m_length-k-1;
+          Index dstStart = rows()-m_shift-actual_k;
+          dst.bottomRightCorner(dstStart, inputIsIdentity ? dstStart : dst.cols())
             .applyHouseholderOnTheLeft(essentialVector(actual_k), m_coeffs.coeff(actual_k), workspace.data());
         }
       }
@@ -357,7 +424,7 @@
     {
       typename internal::matrix_type_times_scalar_type<Scalar, OtherDerived>::Type
         res(other.template cast<typename internal::matrix_type_times_scalar_type<Scalar,OtherDerived>::ResultScalar>());
-      applyThisOnTheLeft(res);
+      applyThisOnTheLeft(res, internal::is_identity<OtherDerived>::value && res.rows()==res.cols());
       return res;
     }
 
@@ -372,6 +439,7 @@
       *
       * \sa length()
       */
+    EIGEN_DEVICE_FUNC
     HouseholderSequence& setLength(Index length)
     {
       m_length = length;
@@ -389,13 +457,17 @@
       *
       * \sa shift()
       */
+    EIGEN_DEVICE_FUNC
     HouseholderSequence& setShift(Index shift)
     {
       m_shift = shift;
       return *this;
     }
 
+    EIGEN_DEVICE_FUNC
     Index length() const { return m_length; }  /**< \brief Returns the length of the Householder sequence. */
+
+    EIGEN_DEVICE_FUNC
     Index shift() const { return m_shift; }    /**< \brief Returns the shift of the Householder sequence. */
 
     /* Necessary for .adjoint() and .conjugate() */
@@ -403,27 +475,30 @@
 
   protected:
 
-    /** \brief Sets the transpose flag.
-      * \param [in]  trans  New value of the transpose flag.
+    /** \internal
+      * \brief Sets the reverse flag.
+      * \param [in]  reverse  New value of the reverse flag.
       *
-      * By default, the transpose flag is not set. If the transpose flag is set, then this object represents 
-      * \f$ H^T = H_{n-1}^T \ldots H_1^T H_0^T \f$ instead of \f$ H = H_0 H_1 \ldots H_{n-1} \f$.
+      * By default, the reverse flag is not set. If the reverse flag is set, then this object represents
+      * \f$ H^r = H_{n-1} \ldots H_1 H_0 \f$ instead of \f$ H = H_0 H_1 \ldots H_{n-1} \f$.
+      * \note For real valued HouseholderSequence this is equivalent to transposing \f$ H \f$.
       *
-      * \sa trans()
+      * \sa reverseFlag(), transpose(), adjoint()
       */
-    HouseholderSequence& setTrans(bool trans)
+    HouseholderSequence& setReverseFlag(bool reverse)
     {
-      m_trans = trans;
+      m_reverse = reverse;
       return *this;
     }
 
-    bool trans() const { return m_trans; }     /**< \brief Returns the transpose flag. */
+    bool reverseFlag() const { return m_reverse; }     /**< \internal \brief Returns the reverse flag. */
 
     typename VectorsType::Nested m_vectors;
     typename CoeffsType::Nested m_coeffs;
-    bool m_trans;
+    bool m_reverse;
     Index m_length;
     Index m_shift;
+    enum { BlockSize = 48 };
 };
 
 /** \brief Computes the product of a matrix with a Householder sequence.
@@ -444,7 +519,7 @@
 }
 
 /** \ingroup Householder_Module \householder_module
-  * \brief Convenience function for constructing a Householder sequence. 
+  * \brief Convenience function for constructing a Householder sequence.
   * \returns A HouseholderSequence constructed from the specified arguments.
   */
 template<typename VectorsType, typename CoeffsType>
@@ -454,7 +529,7 @@
 }
 
 /** \ingroup Householder_Module \householder_module
-  * \brief Convenience function for constructing a Householder sequence. 
+  * \brief Convenience function for constructing a Householder sequence.
   * \returns A HouseholderSequence constructed from the specified arguments.
   * \details This function differs from householderSequence() in that the template argument \p OnTheSide of
   * the constructed HouseholderSequence is set to OnTheRight, instead of the default OnTheLeft.
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
index 1998c63..76668a5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/Jacobi/Jacobi.h
@@ -11,7 +11,7 @@
 #ifndef EIGEN_JACOBI_H
 #define EIGEN_JACOBI_H
 
-namespace Eigen { 
+namespace Eigen {
 
 /** \ingroup Jacobi_Module
   * \jacobi_module
@@ -37,17 +37,20 @@
     typedef typename NumTraits<Scalar>::Real RealScalar;
 
     /** Default constructor without any initialization. */
+    EIGEN_DEVICE_FUNC
     JacobiRotation() {}
 
     /** Construct a planar rotation from a cosine-sine pair (\a c, \c s). */
+    EIGEN_DEVICE_FUNC
     JacobiRotation(const Scalar& c, const Scalar& s) : m_c(c), m_s(s) {}
 
-    Scalar& c() { return m_c; }
-    Scalar c() const { return m_c; }
-    Scalar& s() { return m_s; }
-    Scalar s() const { return m_s; }
+    EIGEN_DEVICE_FUNC Scalar& c() { return m_c; }
+    EIGEN_DEVICE_FUNC Scalar c() const { return m_c; }
+    EIGEN_DEVICE_FUNC Scalar& s() { return m_s; }
+    EIGEN_DEVICE_FUNC Scalar s() const { return m_s; }
 
     /** Concatenates two planar rotation */
+    EIGEN_DEVICE_FUNC
     JacobiRotation operator*(const JacobiRotation& other)
     {
       using numext::conj;
@@ -56,19 +59,26 @@
     }
 
     /** Returns the transposed transformation */
+    EIGEN_DEVICE_FUNC
     JacobiRotation transpose() const { using numext::conj; return JacobiRotation(m_c, -conj(m_s)); }
 
     /** Returns the adjoint transformation */
+    EIGEN_DEVICE_FUNC
     JacobiRotation adjoint() const { using numext::conj; return JacobiRotation(conj(m_c), -m_s); }
 
     template<typename Derived>
+    EIGEN_DEVICE_FUNC
     bool makeJacobi(const MatrixBase<Derived>&, Index p, Index q);
+    EIGEN_DEVICE_FUNC
     bool makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z);
 
+    EIGEN_DEVICE_FUNC
     void makeGivens(const Scalar& p, const Scalar& q, Scalar* r=0);
 
   protected:
+    EIGEN_DEVICE_FUNC
     void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type);
+    EIGEN_DEVICE_FUNC
     void makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type);
 
     Scalar m_c, m_s;
@@ -80,10 +90,12 @@
   * \sa MatrixBase::makeJacobi(const MatrixBase<Derived>&, Index, Index), MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
   */
 template<typename Scalar>
+EIGEN_DEVICE_FUNC
 bool JacobiRotation<Scalar>::makeJacobi(const RealScalar& x, const Scalar& y, const RealScalar& z)
 {
   using std::sqrt;
   using std::abs;
+
   RealScalar deno = RealScalar(2)*abs(y);
   if(deno < (std::numeric_limits<RealScalar>::min)())
   {
@@ -123,6 +135,7 @@
   */
 template<typename Scalar>
 template<typename Derived>
+EIGEN_DEVICE_FUNC
 inline bool JacobiRotation<Scalar>::makeJacobi(const MatrixBase<Derived>& m, Index p, Index q)
 {
   return makeJacobi(numext::real(m.coeff(p,p)), m.coeff(p,q), numext::real(m.coeff(q,q)));
@@ -145,6 +158,7 @@
   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
   */
 template<typename Scalar>
+EIGEN_DEVICE_FUNC
 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r)
 {
   makeGivens(p, q, r, typename internal::conditional<NumTraits<Scalar>::IsComplex, internal::true_type, internal::false_type>::type());
@@ -153,12 +167,13 @@
 
 // specialization for complexes
 template<typename Scalar>
+EIGEN_DEVICE_FUNC
 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::true_type)
 {
   using std::sqrt;
   using std::abs;
   using numext::conj;
-  
+
   if(q==Scalar(0))
   {
     m_c = numext::real(p)<0 ? Scalar(-1) : Scalar(1);
@@ -212,6 +227,7 @@
 
 // specialization for reals
 template<typename Scalar>
+EIGEN_DEVICE_FUNC
 void JacobiRotation<Scalar>::makeGivens(const Scalar& p, const Scalar& q, Scalar* r, internal::false_type)
 {
   using std::sqrt;
@@ -257,12 +273,13 @@
 
 namespace internal {
 /** \jacobi_module
-  * Applies the clock wise 2D rotation \a j to the set of 2D vectors of cordinates \a x and \a y:
+  * Applies the clock wise 2D rotation \a j to the set of 2D vectors of coordinates \a x and \a y:
   * \f$ \left ( \begin{array}{cc} x \\ y \end{array} \right )  =  J \left ( \begin{array}{cc} x \\ y \end{array} \right ) \f$
   *
   * \sa MatrixBase::applyOnTheLeft(), MatrixBase::applyOnTheRight()
   */
 template<typename VectorX, typename VectorY, typename OtherScalar>
+EIGEN_DEVICE_FUNC
 void apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j);
 }
 
@@ -274,6 +291,7 @@
   */
 template<typename Derived>
 template<typename OtherScalar>
+EIGEN_DEVICE_FUNC
 inline void MatrixBase<Derived>::applyOnTheLeft(Index p, Index q, const JacobiRotation<OtherScalar>& j)
 {
   RowXpr x(this->row(p));
@@ -289,6 +307,7 @@
   */
 template<typename Derived>
 template<typename OtherScalar>
+EIGEN_DEVICE_FUNC
 inline void MatrixBase<Derived>::applyOnTheRight(Index p, Index q, const JacobiRotation<OtherScalar>& j)
 {
   ColXpr x(this->col(p));
@@ -302,7 +321,8 @@
          int SizeAtCompileTime, int MinAlignment, bool Vectorizable>
 struct apply_rotation_in_the_plane_selector
 {
-  static inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
+  static EIGEN_DEVICE_FUNC
+  inline void run(Scalar *x, Index incrx, Scalar *y, Index incry, Index size, OtherScalar c, OtherScalar s)
   {
     for(Index i=0; i<size; ++i)
     {
@@ -429,10 +449,11 @@
 };
 
 template<typename VectorX, typename VectorY, typename OtherScalar>
+EIGEN_DEVICE_FUNC
 void /*EIGEN_DONT_INLINE*/ apply_rotation_in_the_plane(DenseBase<VectorX>& xpr_x, DenseBase<VectorY>& xpr_y, const JacobiRotation<OtherScalar>& j)
 {
   typedef typename VectorX::Scalar Scalar;
-  const bool Vectorizable =    (VectorX::Flags & VectorY::Flags & PacketAccessBit)
+  const bool Vectorizable =    (int(VectorX::Flags) & int(VectorY::Flags) & PacketAccessBit)
                             && (int(packet_traits<Scalar>::size) == int(packet_traits<OtherScalar>::size));
 
   eigen_assert(xpr_x.size() == xpr_y.size());
@@ -442,7 +463,7 @@
 
   Scalar* EIGEN_RESTRICT x = &xpr_x.derived().coeffRef(0);
   Scalar* EIGEN_RESTRICT y = &xpr_y.derived().coeffRef(0);
-  
+
   OtherScalar c = j.c();
   OtherScalar s = j.s();
   if (c==OtherScalar(1) && s==OtherScalar(0))
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
index d6a3c1e..3a41e6f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/Determinant.h
@@ -15,6 +15,7 @@
 namespace internal {
 
 template<typename Derived>
+EIGEN_DEVICE_FUNC
 inline const typename Derived::Scalar bruteforce_det3_helper
 (const MatrixBase<Derived>& matrix, int a, int b, int c)
 {
@@ -22,14 +23,6 @@
          * (matrix.coeff(1,b) * matrix.coeff(2,c) - matrix.coeff(1,c) * matrix.coeff(2,b));
 }
 
-template<typename Derived>
-const typename Derived::Scalar bruteforce_det4_helper
-(const MatrixBase<Derived>& matrix, int j, int k, int m, int n)
-{
-  return (matrix.coeff(j,0) * matrix.coeff(k,1) - matrix.coeff(k,0) * matrix.coeff(j,1))
-       * (matrix.coeff(m,2) * matrix.coeff(n,3) - matrix.coeff(n,2) * matrix.coeff(m,3));
-}
-
 template<typename Derived,
          int DeterminantType = Derived::RowsAtCompileTime
 > struct determinant_impl
@@ -44,7 +37,8 @@
 
 template<typename Derived> struct determinant_impl<Derived, 1>
 {
-  static inline typename traits<Derived>::Scalar run(const Derived& m)
+  static inline EIGEN_DEVICE_FUNC
+  typename traits<Derived>::Scalar run(const Derived& m)
   {
     return m.coeff(0,0);
   }
@@ -52,7 +46,8 @@
 
 template<typename Derived> struct determinant_impl<Derived, 2>
 {
-  static inline typename traits<Derived>::Scalar run(const Derived& m)
+  static inline EIGEN_DEVICE_FUNC
+  typename traits<Derived>::Scalar run(const Derived& m)
   {
     return m.coeff(0,0) * m.coeff(1,1) - m.coeff(1,0) * m.coeff(0,1);
   }
@@ -60,7 +55,8 @@
 
 template<typename Derived> struct determinant_impl<Derived, 3>
 {
-  static inline typename traits<Derived>::Scalar run(const Derived& m)
+  static inline EIGEN_DEVICE_FUNC
+  typename traits<Derived>::Scalar run(const Derived& m)
   {
     return bruteforce_det3_helper(m,0,1,2)
           - bruteforce_det3_helper(m,1,0,2)
@@ -70,15 +66,34 @@
 
 template<typename Derived> struct determinant_impl<Derived, 4>
 {
-  static typename traits<Derived>::Scalar run(const Derived& m)
+  typedef typename traits<Derived>::Scalar Scalar;
+  static EIGEN_DEVICE_FUNC
+  Scalar run(const Derived& m)
   {
-    // trick by Martin Costabel to compute 4x4 det with only 30 muls
-    return bruteforce_det4_helper(m,0,1,2,3)
-          - bruteforce_det4_helper(m,0,2,1,3)
-          + bruteforce_det4_helper(m,0,3,1,2)
-          + bruteforce_det4_helper(m,1,2,0,3)
-          - bruteforce_det4_helper(m,1,3,0,2)
-          + bruteforce_det4_helper(m,2,3,0,1);
+    Scalar d2_01 = det2(m, 0, 1);
+    Scalar d2_02 = det2(m, 0, 2);
+    Scalar d2_03 = det2(m, 0, 3);
+    Scalar d2_12 = det2(m, 1, 2);
+    Scalar d2_13 = det2(m, 1, 3);
+    Scalar d2_23 = det2(m, 2, 3);
+    Scalar d3_0 = det3(m, 1,d2_23, 2,d2_13, 3,d2_12);
+    Scalar d3_1 = det3(m, 0,d2_23, 2,d2_03, 3,d2_02);
+    Scalar d3_2 = det3(m, 0,d2_13, 1,d2_03, 3,d2_01);
+    Scalar d3_3 = det3(m, 0,d2_12, 1,d2_02, 2,d2_01);
+    return internal::pmadd(-m(0,3),d3_0, m(1,3)*d3_1) +
+           internal::pmadd(-m(2,3),d3_2, m(3,3)*d3_3);
+  }
+protected:
+  static EIGEN_DEVICE_FUNC
+  Scalar det2(const Derived& m, Index i0, Index i1)
+  {
+    return m(i0,0) * m(i1,1) - m(i1,0) * m(i0,1);
+  }
+
+  static EIGEN_DEVICE_FUNC
+  Scalar det3(const Derived& m, Index i0, const Scalar& d0, Index i1, const Scalar& d1, Index i2, const Scalar& d2)
+  {
+    return internal::pmadd(m(i0,2), d0, internal::pmadd(-m(i1,2), d1, m(i2,2)*d2));
   }
 };
 
@@ -89,6 +104,7 @@
   * \returns the determinant of this matrix
   */
 template<typename Derived>
+EIGEN_DEVICE_FUNC
 inline typename internal::traits<Derived>::Scalar MatrixBase<Derived>::determinant() const
 {
   eigen_assert(rows() == cols());
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
index 03b6af7..ba1749f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/FullPivLU.h
@@ -18,6 +18,7 @@
 {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
   enum { Flags = 0 };
 };
 
@@ -48,12 +49,12 @@
   * The data of the LU decomposition can be directly accessed through the methods matrixLU(),
   * permutationP(), permutationQ().
   *
-  * As an exemple, here is how the original matrix can be retrieved:
+  * As an example, here is how the original matrix can be retrieved:
   * \include class_FullPivLU.cpp
   * Output: \verbinclude class_FullPivLU.out
   *
   * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  * 
+  *
   * \sa MatrixBase::fullPivLu(), MatrixBase::determinant(), MatrixBase::inverse()
   */
 template<typename _MatrixType> class FullPivLU
@@ -62,9 +63,9 @@
   public:
     typedef _MatrixType MatrixType;
     typedef SolverBase<FullPivLU> Base;
+    friend class SolverBase<FullPivLU>;
 
     EIGEN_GENERIC_PUBLIC_INTERFACE(FullPivLU)
-    // FIXME StorageIndex defined in EIGEN_GENERIC_PUBLIC_INTERFACE should be int
     enum {
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
@@ -218,6 +219,7 @@
       return internal::image_retval<FullPivLU>(*this, originalMatrix);
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** \return a solution x to the equation Ax=b, where A is the matrix of which
       * *this is the LU decomposition.
       *
@@ -237,14 +239,10 @@
       *
       * \sa TriangularView::solve(), kernel(), inverse()
       */
-    // FIXME this is a copy-paste of the base-class member to add the isInitialized assertion.
     template<typename Rhs>
     inline const Solve<FullPivLU, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "LU is not initialized.");
-      return Solve<FullPivLU, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     /** \returns an estimate of the reciprocal condition number of the matrix of which \c *this is
         the LU decomposition.
@@ -320,7 +318,7 @@
       return m_usePrescribedThreshold ? m_prescribedThreshold
       // this formula comes from experimenting (see "LU precision tuning" thread on the list)
       // and turns out to be identical to Higham's formula used already in LDLt.
-                                      : NumTraits<Scalar>::epsilon() * m_lu.diagonalSize();
+          : NumTraits<Scalar>::epsilon() * RealScalar(m_lu.diagonalSize());
     }
 
     /** \returns the rank of the matrix of which *this is the LU decomposition.
@@ -406,16 +404,16 @@
 
     MatrixType reconstructedMatrix() const;
 
-    EIGEN_DEVICE_FUNC inline Index rows() const { return m_lu.rows(); }
-    EIGEN_DEVICE_FUNC inline Index cols() const { return m_lu.cols(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+    EIGEN_DEVICE_FUNC EIGEN_CONSTEXPR
+    inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
 
     template<bool Conjugate, typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
@@ -531,8 +529,8 @@
       m_nonzero_pivots = k;
       for(Index i = k; i < size; ++i)
       {
-        m_rowsTranspositions.coeffRef(i) = i;
-        m_colsTranspositions.coeffRef(i) = i;
+        m_rowsTranspositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
+        m_colsTranspositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
       }
       break;
     }
@@ -543,8 +541,8 @@
     // Now that we've found the pivot, we need to apply the row/col swaps to
     // bring it to the location (k,k).
 
-    m_rowsTranspositions.coeffRef(k) = row_of_biggest_in_corner;
-    m_colsTranspositions.coeffRef(k) = col_of_biggest_in_corner;
+    m_rowsTranspositions.coeffRef(k) = internal::convert_index<StorageIndex>(row_of_biggest_in_corner);
+    m_colsTranspositions.coeffRef(k) = internal::convert_index<StorageIndex>(col_of_biggest_in_corner);
     if(k != row_of_biggest_in_corner) {
       m_lu.row(k).swap(m_lu.row(row_of_biggest_in_corner));
       ++number_of_transpositions;
@@ -757,7 +755,6 @@
   const Index rows = this->rows(),
               cols = this->cols(),
               nonzero_pivots = this->rank();
-  eigen_assert(rhs.rows() == rows);
   const Index smalldim = (std::min)(rows, cols);
 
   if(nonzero_pivots == 0)
@@ -807,7 +804,6 @@
 
   const Index rows = this->rows(), cols = this->cols(),
     nonzero_pivots = this->rank();
-   eigen_assert(rhs.rows() == cols);
   const Index smalldim = (std::min)(rows, cols);
 
   if(nonzero_pivots == 0)
@@ -821,29 +817,19 @@
   // Step 1
   c = permutationQ().inverse() * rhs;
 
-  if (Conjugate) {
-    // Step 2
-    m_lu.topLeftCorner(nonzero_pivots, nonzero_pivots)
-        .template triangularView<Upper>()
-        .adjoint()
-        .solveInPlace(c.topRows(nonzero_pivots));
-    // Step 3
-    m_lu.topLeftCorner(smalldim, smalldim)
-        .template triangularView<UnitLower>()
-        .adjoint()
-        .solveInPlace(c.topRows(smalldim));
-  } else {
-    // Step 2
-    m_lu.topLeftCorner(nonzero_pivots, nonzero_pivots)
-        .template triangularView<Upper>()
-        .transpose()
-        .solveInPlace(c.topRows(nonzero_pivots));
-    // Step 3
-    m_lu.topLeftCorner(smalldim, smalldim)
-        .template triangularView<UnitLower>()
-        .transpose()
-        .solveInPlace(c.topRows(smalldim));
-  }
+  // Step 2
+  m_lu.topLeftCorner(nonzero_pivots, nonzero_pivots)
+      .template triangularView<Upper>()
+      .transpose()
+      .template conjugateIf<Conjugate>()
+      .solveInPlace(c.topRows(nonzero_pivots));
+
+  // Step 3
+  m_lu.topLeftCorner(smalldim, smalldim)
+      .template triangularView<UnitLower>()
+      .transpose()
+      .template conjugateIf<Conjugate>()
+      .solveInPlace(c.topRows(smalldim));
 
   // Step 4
   PermutationPType invp = permutationP().inverse().eval();
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
index f49f233..a40cefa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/InverseImpl.h
@@ -77,10 +77,11 @@
     const MatrixType& matrix, const typename ResultType::Scalar& invdet,
     ResultType& result)
 {
+  typename ResultType::Scalar temp = matrix.coeff(0,0);
   result.coeffRef(0,0) =  matrix.coeff(1,1) * invdet;
   result.coeffRef(1,0) = -matrix.coeff(1,0) * invdet;
   result.coeffRef(0,1) = -matrix.coeff(0,1) * invdet;
-  result.coeffRef(1,1) =  matrix.coeff(0,0) * invdet;
+  result.coeffRef(1,1) =  temp * invdet;
 }
 
 template<typename MatrixType, typename ResultType>
@@ -143,13 +144,18 @@
     const Matrix<typename ResultType::Scalar,3,1>& cofactors_col0,
     ResultType& result)
 {
-  result.row(0) = cofactors_col0 * invdet;
-  result.coeffRef(1,0) =  cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
-  result.coeffRef(1,1) =  cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
+  // Compute cofactors in a way that avoids aliasing issues.
+  typedef typename ResultType::Scalar Scalar;
+  const Scalar c01 = cofactor_3x3<MatrixType,0,1>(matrix) * invdet;
+  const Scalar c11 = cofactor_3x3<MatrixType,1,1>(matrix) * invdet;
+  const Scalar c02 = cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
   result.coeffRef(1,2) =  cofactor_3x3<MatrixType,2,1>(matrix) * invdet;
-  result.coeffRef(2,0) =  cofactor_3x3<MatrixType,0,2>(matrix) * invdet;
   result.coeffRef(2,1) =  cofactor_3x3<MatrixType,1,2>(matrix) * invdet;
   result.coeffRef(2,2) =  cofactor_3x3<MatrixType,2,2>(matrix) * invdet;
+  result.coeffRef(1,0) =  c01;
+  result.coeffRef(1,1) =  c11;
+  result.coeffRef(2,0) =  c02;  
+  result.row(0) = cofactors_col0 * invdet;
 }
 
 template<typename MatrixType, typename ResultType>
@@ -181,14 +187,13 @@
     bool& invertible
   )
   {
-    using std::abs;
     typedef typename ResultType::Scalar Scalar;
     Matrix<Scalar,3,1> cofactors_col0;
     cofactors_col0.coeffRef(0) =  cofactor_3x3<MatrixType,0,0>(matrix);
     cofactors_col0.coeffRef(1) =  cofactor_3x3<MatrixType,1,0>(matrix);
     cofactors_col0.coeffRef(2) =  cofactor_3x3<MatrixType,2,0>(matrix);
     determinant = (cofactors_col0.cwiseProduct(matrix.col(0))).sum();
-    invertible = abs(determinant) > absDeterminantThreshold;
+    invertible = Eigen::numext::abs(determinant) > absDeterminantThreshold;
     if(!invertible) return;
     const Scalar invdet = Scalar(1) / determinant;
     compute_inverse_size3_helper(matrix, invdet, cofactors_col0, inverse);
@@ -273,7 +278,13 @@
     using std::abs;
     determinant = matrix.determinant();
     invertible = abs(determinant) > absDeterminantThreshold;
-    if(invertible) compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
+    if(invertible && extract_data(matrix) != extract_data(inverse)) {
+      compute_inverse<MatrixType, ResultType>::run(matrix, inverse);
+    }
+    else if(invertible) {
+      MatrixType matrix_t = matrix;
+      compute_inverse<MatrixType, ResultType>::run(matrix_t, inverse);
+    }
   }
 };
 
@@ -290,6 +301,7 @@
 struct Assignment<DstXprType, Inverse<XprType>, internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar>, Dense2Dense>
 {
   typedef Inverse<XprType> SrcXprType;
+  EIGEN_DEVICE_FUNC
   static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename XprType::Scalar> &)
   {
     Index dstRows = src.rows();
@@ -332,6 +344,7 @@
   * \sa computeInverseAndDetWithCheck()
   */
 template<typename Derived>
+EIGEN_DEVICE_FUNC
 inline const Inverse<Derived> MatrixBase<Derived>::inverse() const
 {
   EIGEN_STATIC_ASSERT(!NumTraits<Scalar>::IsInteger,THIS_FUNCTION_IS_NOT_FOR_INTEGER_NUMERIC_TYPES)
@@ -345,6 +358,8 @@
   *
   * This is only for fixed-size square matrices of size up to 4x4.
   *
+  * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
+  *
   * \param inverse Reference to the matrix in which to store the inverse.
   * \param determinant Reference to the variable in which to store the determinant.
   * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
@@ -385,6 +400,8 @@
   *
   * This is only for fixed-size square matrices of size up to 4x4.
   *
+  * Notice that it will trigger a copy of input matrix when trying to do the inverse in place.
+  *
   * \param inverse Reference to the matrix in which to store the inverse.
   * \param invertible Reference to the bool variable in which to store whether the matrix is invertible.
   * \param absDeterminantThreshold Optional parameter controlling the invertibility check.
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
index d439618..34aed72 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/PartialPivLU.h
@@ -19,6 +19,7 @@
 {
   typedef MatrixXpr XprKind;
   typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
   typedef traits<_MatrixType> BaseTraits;
   enum {
     Flags = BaseTraits::Flags & RowMajorBit,
@@ -69,7 +70,7 @@
   * The data of the LU decomposition can be directly accessed through the methods matrixLU(), permutationP().
   *
   * This class supports the \link InplaceDecomposition inplace decomposition \endlink mechanism.
-  * 
+  *
   * \sa MatrixBase::partialPivLu(), MatrixBase::determinant(), MatrixBase::inverse(), MatrixBase::computeInverse(), class FullPivLU
   */
 template<typename _MatrixType> class PartialPivLU
@@ -79,8 +80,9 @@
 
     typedef _MatrixType MatrixType;
     typedef SolverBase<PartialPivLU> Base;
+    friend class SolverBase<PartialPivLU>;
+
     EIGEN_GENERIC_PUBLIC_INTERFACE(PartialPivLU)
-    // FIXME StorageIndex defined in EIGEN_GENERIC_PUBLIC_INTERFACE should be int
     enum {
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
@@ -152,6 +154,7 @@
       return m_p;
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** This method returns the solution x to the equation Ax=b, where A is the matrix of which
       * *this is the LU decomposition.
       *
@@ -169,14 +172,10 @@
       *
       * \sa TriangularView::solve(), inverse(), computeInverse()
       */
-    // FIXME this is a copy-paste of the base-class member to add the isInitialized assertion.
     template<typename Rhs>
     inline const Solve<PartialPivLU, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "PartialPivLU is not initialized.");
-      return Solve<PartialPivLU, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     /** \returns an estimate of the reciprocal condition number of the matrix of which \c *this is
         the LU decomposition.
@@ -217,8 +216,8 @@
 
     MatrixType reconstructedMatrix() const;
 
-    inline Index rows() const { return m_lu.rows(); }
-    inline Index cols() const { return m_lu.cols(); }
+    EIGEN_CONSTEXPR inline Index rows() const EIGEN_NOEXCEPT { return m_lu.rows(); }
+    EIGEN_CONSTEXPR inline Index cols() const EIGEN_NOEXCEPT { return m_lu.cols(); }
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
@@ -231,8 +230,6 @@
       * Step 3: replace c by the solution x to Ux = c.
       */
 
-      eigen_assert(rhs.rows() == m_lu.rows());
-
       // Step 1
       dst = permutationP() * rhs;
 
@@ -246,26 +243,21 @@
     template<bool Conjugate, typename RhsType, typename DstType>
     EIGEN_DEVICE_FUNC
     void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const {
-     /* The decomposition PA = LU can be rewritten as A = P^{-1} L U.
+     /* The decomposition PA = LU can be rewritten as A^T = U^T L^T P.
       * So we proceed as follows:
-      * Step 1: compute c = Pb.
-      * Step 2: replace c by the solution x to Lx = c.
-      * Step 3: replace c by the solution x to Ux = c.
+      * Step 1: compute c as the solution to L^T c = b
+      * Step 2: replace c by the solution x to U^T x = c.
+      * Step 3: update  c = P^-1 c.
       */
 
       eigen_assert(rhs.rows() == m_lu.cols());
 
-      if (Conjugate) {
-        // Step 1
-        dst = m_lu.template triangularView<Upper>().adjoint().solve(rhs);
-        // Step 2
-        m_lu.template triangularView<UnitLower>().adjoint().solveInPlace(dst);
-      } else {
-        // Step 1
-        dst = m_lu.template triangularView<Upper>().transpose().solve(rhs);
-        // Step 2
-        m_lu.template triangularView<UnitLower>().transpose().solveInPlace(dst);
-      }
+      // Step 1
+      dst = m_lu.template triangularView<Upper>().transpose()
+                .template conjugateIf<Conjugate>().solve(rhs);
+      // Step 2
+      m_lu.template triangularView<UnitLower>().transpose()
+          .template conjugateIf<Conjugate>().solveInPlace(dst);
       // Step 3
       dst = permutationP().transpose() * dst;
     }
@@ -339,17 +331,18 @@
 namespace internal {
 
 /** \internal This is the blocked version of fullpivlu_unblocked() */
-template<typename Scalar, int StorageOrder, typename PivIndex>
+template<typename Scalar, int StorageOrder, typename PivIndex, int SizeAtCompileTime=Dynamic>
 struct partial_lu_impl
 {
-  // FIXME add a stride to Map, so that the following mapping becomes easier,
-  // another option would be to create an expression being able to automatically
-  // warp any Map, Matrix, and Block expressions as a unique type, but since that's exactly
-  // a Map + stride, why not adding a stride to Map, and convenient ctors from a Matrix,
-  // and Block.
-  typedef Map<Matrix<Scalar, Dynamic, Dynamic, StorageOrder> > MapLU;
-  typedef Block<MapLU, Dynamic, Dynamic> MatrixType;
-  typedef Block<MatrixType,Dynamic,Dynamic> BlockType;
+  static const int UnBlockedBound = 16;
+  static const bool UnBlockedAtCompileTime = SizeAtCompileTime!=Dynamic && SizeAtCompileTime<=UnBlockedBound;
+  static const int ActualSizeAtCompileTime = UnBlockedAtCompileTime ? SizeAtCompileTime : Dynamic;
+  // Remaining rows and columns at compile-time:
+  static const int RRows = SizeAtCompileTime==2 ? 1 : Dynamic;
+  static const int RCols = SizeAtCompileTime==2 ? 1 : Dynamic;
+  typedef Matrix<Scalar, ActualSizeAtCompileTime, ActualSizeAtCompileTime, StorageOrder> MatrixType;
+  typedef Ref<MatrixType> MatrixTypeRef;
+  typedef Ref<Matrix<Scalar, Dynamic, Dynamic, StorageOrder> > BlockType;
   typedef typename MatrixType::RealScalar RealScalar;
 
   /** \internal performs the LU decomposition in-place of the matrix \a lu
@@ -362,19 +355,22 @@
     *
     * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise.
     */
-  static Index unblocked_lu(MatrixType& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions)
+  static Index unblocked_lu(MatrixTypeRef& lu, PivIndex* row_transpositions, PivIndex& nb_transpositions)
   {
     typedef scalar_score_coeff_op<Scalar> Scoring;
     typedef typename Scoring::result_type Score;
     const Index rows = lu.rows();
     const Index cols = lu.cols();
     const Index size = (std::min)(rows,cols);
+    // For small compile-time matrices it is worth processing the last row separately:
+    //  speedup: +100% for 2x2, +10% for others.
+    const Index endk = UnBlockedAtCompileTime ? size-1 : size;
     nb_transpositions = 0;
     Index first_zero_pivot = -1;
-    for(Index k = 0; k < size; ++k)
+    for(Index k = 0; k < endk; ++k)
     {
-      Index rrows = rows-k-1;
-      Index rcols = cols-k-1;
+      int rrows = internal::convert_index<int>(rows-k-1);
+      int rcols = internal::convert_index<int>(cols-k-1);
 
       Index row_of_biggest_in_col;
       Score biggest_in_corner
@@ -391,9 +387,7 @@
           ++nb_transpositions;
         }
 
-        // FIXME shall we introduce a safe quotient expression in cas 1/lu.coeff(k,k)
-        // overflow but not the actual quotient?
-        lu.col(k).tail(rrows) /= lu.coeff(k,k);
+        lu.col(k).tail(fix<RRows>(rrows)) /= lu.coeff(k,k);
       }
       else if(first_zero_pivot==-1)
       {
@@ -403,8 +397,18 @@
       }
 
       if(k<rows-1)
-        lu.bottomRightCorner(rrows,rcols).noalias() -= lu.col(k).tail(rrows) * lu.row(k).tail(rcols);
+        lu.bottomRightCorner(fix<RRows>(rrows),fix<RCols>(rcols)).noalias() -= lu.col(k).tail(fix<RRows>(rrows)) * lu.row(k).tail(fix<RCols>(rcols));
     }
+
+    // special handling of the last entry
+    if(UnBlockedAtCompileTime)
+    {
+      Index k = endk;
+      row_transpositions[k] = PivIndex(k);
+      if (Scoring()(lu(k, k)) == Score(0) && first_zero_pivot == -1)
+        first_zero_pivot = k;
+    }
+
     return first_zero_pivot;
   }
 
@@ -420,18 +424,17 @@
     * \returns The index of the first pivot which is exactly zero if any, or a negative number otherwise.
     *
     * \note This very low level interface using pointers, etc. is to:
-    *   1 - reduce the number of instanciations to the strict minimum
-    *   2 - avoid infinite recursion of the instanciations with Block<Block<Block<...> > >
+    *   1 - reduce the number of instantiations to the strict minimum
+    *   2 - avoid infinite recursion of the instantiations with Block<Block<Block<...> > >
     */
   static Index blocked_lu(Index rows, Index cols, Scalar* lu_data, Index luStride, PivIndex* row_transpositions, PivIndex& nb_transpositions, Index maxBlockSize=256)
   {
-    MapLU lu1(lu_data,StorageOrder==RowMajor?rows:luStride,StorageOrder==RowMajor?luStride:cols);
-    MatrixType lu(lu1,0,0,rows,cols);
+    MatrixTypeRef lu = MatrixType::Map(lu_data,rows, cols, OuterStride<>(luStride));
 
     const Index size = (std::min)(rows,cols);
 
     // if the matrix is too small, no blocking:
-    if(size<=16)
+    if(UnBlockedAtCompileTime || size<=UnBlockedBound)
     {
       return unblocked_lu(lu, row_transpositions, nb_transpositions);
     }
@@ -457,12 +460,12 @@
       //                          A00 | A01 | A02
       // lu  = A_0 | A_1 | A_2 =  A10 | A11 | A12
       //                          A20 | A21 | A22
-      BlockType A_0(lu,0,0,rows,k);
-      BlockType A_2(lu,0,k+bs,rows,tsize);
-      BlockType A11(lu,k,k,bs,bs);
-      BlockType A12(lu,k,k+bs,bs,tsize);
-      BlockType A21(lu,k+bs,k,trows,bs);
-      BlockType A22(lu,k+bs,k+bs,trows,tsize);
+      BlockType A_0 = lu.block(0,0,rows,k);
+      BlockType A_2 = lu.block(0,k+bs,rows,tsize);
+      BlockType A11 = lu.block(k,k,bs,bs);
+      BlockType A12 = lu.block(k,k+bs,bs,tsize);
+      BlockType A21 = lu.block(k+bs,k,trows,bs);
+      BlockType A22 = lu.block(k+bs,k+bs,trows,tsize);
 
       PivIndex nb_transpositions_in_panel;
       // recursively call the blocked LU algorithm on [A11^T A21^T]^T
@@ -501,11 +504,18 @@
 template<typename MatrixType, typename TranspositionType>
 void partial_lu_inplace(MatrixType& lu, TranspositionType& row_transpositions, typename TranspositionType::StorageIndex& nb_transpositions)
 {
+  // Special-case of zero matrix.
+  if (lu.rows() == 0 || lu.cols() == 0) {
+    nb_transpositions = 0;
+    return;
+  }
   eigen_assert(lu.cols() == row_transpositions.size());
-  eigen_assert((&row_transpositions.coeffRef(1)-&row_transpositions.coeffRef(0)) == 1);
+  eigen_assert(row_transpositions.size() < 2 || (&row_transpositions.coeffRef(1)-&row_transpositions.coeffRef(0)) == 1);
 
   partial_lu_impl
-    <typename MatrixType::Scalar, MatrixType::Flags&RowMajorBit?RowMajor:ColMajor, typename TranspositionType::StorageIndex>
+    < typename MatrixType::Scalar, MatrixType::Flags&RowMajorBit?RowMajor:ColMajor,
+      typename TranspositionType::StorageIndex,
+      EIGEN_SIZE_MIN_PREFER_FIXED(MatrixType::RowsAtCompileTime,MatrixType::ColsAtCompileTime)>
     ::blocked_lu(lu.rows(), lu.cols(), &lu.coeffRef(0,0), lu.outerStride(), &row_transpositions.coeffRef(0), nb_transpositions);
 }
 
@@ -519,7 +529,10 @@
   // the row permutation is stored as int indices, so just to be sure:
   eigen_assert(m_lu.rows()<NumTraits<int>::highest());
 
-  m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff();
+  if(m_lu.cols()>0)
+    m_l1_norm = m_lu.cwiseAbs().colwise().sum().maxCoeff();
+  else
+    m_l1_norm = RealScalar(0);
 
   eigen_assert(m_lu.rows() == m_lu.cols() && "PartialPivLU is only for square (and moreover invertible) matrices");
   const Index size = m_lu.rows();
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h
new file mode 100644
index 0000000..a232ffc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/InverseSize4.h
@@ -0,0 +1,351 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2001 Intel Corporation
+// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
+// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+//
+// The algorithm below is a reimplementation of former \src\LU\Inverse_SSE.h using PacketMath.
+// inv(M) = M#/|M|, where inv(M), M# and |M| denote the inverse of M,
+// adjugate of M and determinant of M respectively. M# is computed block-wise
+// using specific formulae. For proof, see:
+// https://lxjk.github.io/2017/09/03/Fast-4x4-Matrix-Inverse-with-SSE-SIMD-Explained.html
+// Variable names are adopted from \src\LU\Inverse_SSE.h.
+//
+// The SSE code for the 4x4 float and double matrix inverse in former (deprecated) \src\LU\Inverse_SSE.h
+// comes from the following Intel's library:
+// http://software.intel.com/en-us/articles/optimized-matrix-library-for-use-with-the-intel-pentiumr-4-processors-sse2-instructions/
+//
+// Here is the respective copyright and license statement:
+//
+//   Copyright (c) 2001 Intel Corporation.
+//
+// Permition is granted to use, copy, distribute and prepare derivative works
+// of this library for any purpose and without fee, provided, that the above
+// copyright notice and this statement appear in all copies.
+// Intel makes no representations about the suitability of this software for
+// any purpose, and specifically disclaims all warranties.
+// See LEGAL.TXT for all the legal information.
+//
+// TODO: Unify implementations of different data types (i.e. float and double).
+#ifndef EIGEN_INVERSE_SIZE_4_H
+#define EIGEN_INVERSE_SIZE_4_H
+
+namespace Eigen
+{
+namespace internal
+{
+template <typename MatrixType, typename ResultType>
+struct compute_inverse_size4<Architecture::Target, float, MatrixType, ResultType>
+{
+  enum
+  {
+    MatrixAlignment = traits<MatrixType>::Alignment,
+    ResultAlignment = traits<ResultType>::Alignment,
+    StorageOrdersMatch = (MatrixType::Flags & RowMajorBit) == (ResultType::Flags & RowMajorBit)
+  };
+  typedef typename conditional<(MatrixType::Flags & LinearAccessBit), MatrixType const &, typename MatrixType::PlainObject>::type ActualMatrixType;
+
+  static void run(const MatrixType &mat, ResultType &result)
+  {
+    ActualMatrixType matrix(mat);
+
+    const float* data = matrix.data();
+    const Index stride = matrix.innerStride();
+    Packet4f _L1 = ploadt<Packet4f,MatrixAlignment>(data);
+    Packet4f _L2 = ploadt<Packet4f,MatrixAlignment>(data + stride*4);
+    Packet4f _L3 = ploadt<Packet4f,MatrixAlignment>(data + stride*8);
+    Packet4f _L4 = ploadt<Packet4f,MatrixAlignment>(data + stride*12);
+
+    // Four 2x2 sub-matrices of the input matrix
+    // input = [[A, B],
+    //          [C, D]]
+    Packet4f A, B, C, D;
+
+    if (!StorageOrdersMatch)
+    {
+      A = vec4f_unpacklo(_L1, _L2);
+      B = vec4f_unpacklo(_L3, _L4);
+      C = vec4f_unpackhi(_L1, _L2);
+      D = vec4f_unpackhi(_L3, _L4);
+    }
+    else
+    {
+      A = vec4f_movelh(_L1, _L2);
+      B = vec4f_movehl(_L2, _L1);
+      C = vec4f_movelh(_L3, _L4);
+      D = vec4f_movehl(_L4, _L3);
+    }
+
+    Packet4f AB, DC;
+
+    // AB = A# * B, where A# denotes the adjugate of A, and * denotes matrix product.
+    AB = pmul(vec4f_swizzle2(A, A, 3, 3, 0, 0), B);
+    AB = psub(AB, pmul(vec4f_swizzle2(A, A, 1, 1, 2, 2), vec4f_swizzle2(B, B, 2, 3, 0, 1)));
+
+    // DC = D#*C
+    DC = pmul(vec4f_swizzle2(D, D, 3, 3, 0, 0), C);
+    DC = psub(DC, pmul(vec4f_swizzle2(D, D, 1, 1, 2, 2), vec4f_swizzle2(C, C, 2, 3, 0, 1)));
+
+    // determinants of the sub-matrices
+    Packet4f dA, dB, dC, dD;
+
+    dA = pmul(vec4f_swizzle2(A, A, 3, 3, 1, 1), A);
+    dA = psub(dA, vec4f_movehl(dA, dA));
+
+    dB = pmul(vec4f_swizzle2(B, B, 3, 3, 1, 1), B);
+    dB = psub(dB, vec4f_movehl(dB, dB));
+
+    dC = pmul(vec4f_swizzle2(C, C, 3, 3, 1, 1), C);
+    dC = psub(dC, vec4f_movehl(dC, dC));
+
+    dD = pmul(vec4f_swizzle2(D, D, 3, 3, 1, 1), D);
+    dD = psub(dD, vec4f_movehl(dD, dD));
+
+    Packet4f d, d1, d2;
+
+    d = pmul(vec4f_swizzle2(DC, DC, 0, 2, 1, 3), AB);
+    d = padd(d, vec4f_movehl(d, d));
+    d = padd(d, vec4f_swizzle2(d, d, 1, 0, 0, 0));
+    d1 = pmul(dA, dD);
+    d2 = pmul(dB, dC);
+
+    // determinant of the input matrix, det = |A||D| + |B||C| - trace(A#*B*D#*C)
+    Packet4f det = vec4f_duplane(psub(padd(d1, d2), d), 0);
+
+    // reciprocal of the determinant of the input matrix, rd = 1/det
+    Packet4f rd = pdiv(pset1<Packet4f>(1.0f), det);
+
+    // Four sub-matrices of the inverse
+    Packet4f iA, iB, iC, iD;
+
+    // iD = D*|A| - C*A#*B
+    iD = pmul(vec4f_swizzle2(C, C, 0, 0, 2, 2), vec4f_movelh(AB, AB));
+    iD = padd(iD, pmul(vec4f_swizzle2(C, C, 1, 1, 3, 3), vec4f_movehl(AB, AB)));
+    iD = psub(pmul(D, vec4f_duplane(dA, 0)), iD);
+
+    // iA = A*|D| - B*D#*C
+    iA = pmul(vec4f_swizzle2(B, B, 0, 0, 2, 2), vec4f_movelh(DC, DC));
+    iA = padd(iA, pmul(vec4f_swizzle2(B, B, 1, 1, 3, 3), vec4f_movehl(DC, DC)));
+    iA = psub(pmul(A, vec4f_duplane(dD, 0)), iA);
+
+    // iB = C*|B| - D * (A#B)# = C*|B| - D*B#*A
+    iB = pmul(D, vec4f_swizzle2(AB, AB, 3, 0, 3, 0));
+    iB = psub(iB, pmul(vec4f_swizzle2(D, D, 1, 0, 3, 2), vec4f_swizzle2(AB, AB, 2, 1, 2, 1)));
+    iB = psub(pmul(C, vec4f_duplane(dB, 0)), iB);
+
+    // iC = B*|C| - A * (D#C)# = B*|C| - A*C#*D
+    iC = pmul(A, vec4f_swizzle2(DC, DC, 3, 0, 3, 0));
+    iC = psub(iC, pmul(vec4f_swizzle2(A, A, 1, 0, 3, 2), vec4f_swizzle2(DC, DC, 2, 1, 2, 1)));
+    iC = psub(pmul(B, vec4f_duplane(dC, 0)), iC);
+
+    const float sign_mask[4] = {0.0f, numext::bit_cast<float>(0x80000000u), numext::bit_cast<float>(0x80000000u), 0.0f};
+    const Packet4f p4f_sign_PNNP = ploadu<Packet4f>(sign_mask);
+    rd = pxor(rd, p4f_sign_PNNP);
+    iA = pmul(iA, rd);
+    iB = pmul(iB, rd);
+    iC = pmul(iC, rd);
+    iD = pmul(iD, rd);
+
+    Index res_stride = result.outerStride();
+    float *res = result.data();
+
+    pstoret<float, Packet4f, ResultAlignment>(res + 0, vec4f_swizzle2(iA, iB, 3, 1, 3, 1));
+    pstoret<float, Packet4f, ResultAlignment>(res + res_stride, vec4f_swizzle2(iA, iB, 2, 0, 2, 0));
+    pstoret<float, Packet4f, ResultAlignment>(res + 2 * res_stride, vec4f_swizzle2(iC, iD, 3, 1, 3, 1));
+    pstoret<float, Packet4f, ResultAlignment>(res + 3 * res_stride, vec4f_swizzle2(iC, iD, 2, 0, 2, 0));
+  }
+};
+
+#if !(defined EIGEN_VECTORIZE_NEON && !(EIGEN_ARCH_ARM64 && !EIGEN_APPLE_DOUBLE_NEON_BUG))
+// same algorithm as above, except that each operand is split into
+// halves for two registers to hold.
+template <typename MatrixType, typename ResultType>
+struct compute_inverse_size4<Architecture::Target, double, MatrixType, ResultType>
+{
+  enum
+  {
+    MatrixAlignment = traits<MatrixType>::Alignment,
+    ResultAlignment = traits<ResultType>::Alignment,
+    StorageOrdersMatch = (MatrixType::Flags & RowMajorBit) == (ResultType::Flags & RowMajorBit)
+  };
+  typedef typename conditional<(MatrixType::Flags & LinearAccessBit),
+                               MatrixType const &,
+                               typename MatrixType::PlainObject>::type
+      ActualMatrixType;
+
+  static void run(const MatrixType &mat, ResultType &result)
+  {
+    ActualMatrixType matrix(mat);
+
+    // Four 2x2 sub-matrices of the input matrix, each is further divided into upper and lower
+    // row e.g. A1, upper row of A, A2, lower row of A
+    // input = [[A, B],  =  [[[A1, [B1,
+    //          [C, D]]        A2], B2]],
+    //                       [[C1, [D1,
+    //                         C2], D2]]]
+
+    Packet2d A1, A2, B1, B2, C1, C2, D1, D2;
+
+    const double* data = matrix.data();
+    const Index stride = matrix.innerStride();
+    if (StorageOrdersMatch)
+    {
+      A1 = ploadt<Packet2d,MatrixAlignment>(data + stride*0);
+      B1 = ploadt<Packet2d,MatrixAlignment>(data + stride*2);
+      A2 = ploadt<Packet2d,MatrixAlignment>(data + stride*4);
+      B2 = ploadt<Packet2d,MatrixAlignment>(data + stride*6);
+      C1 = ploadt<Packet2d,MatrixAlignment>(data + stride*8);
+      D1 = ploadt<Packet2d,MatrixAlignment>(data + stride*10);
+      C2 = ploadt<Packet2d,MatrixAlignment>(data + stride*12);
+      D2 = ploadt<Packet2d,MatrixAlignment>(data + stride*14);
+    }
+    else
+    {
+      Packet2d temp;
+      A1 = ploadt<Packet2d,MatrixAlignment>(data + stride*0);
+      C1 = ploadt<Packet2d,MatrixAlignment>(data + stride*2);
+      A2 = ploadt<Packet2d,MatrixAlignment>(data + stride*4);
+      C2 = ploadt<Packet2d,MatrixAlignment>(data + stride*6);
+      temp = A1;
+      A1 = vec2d_unpacklo(A1, A2);
+      A2 = vec2d_unpackhi(temp, A2);
+
+      temp = C1;
+      C1 = vec2d_unpacklo(C1, C2);
+      C2 = vec2d_unpackhi(temp, C2);
+
+      B1 = ploadt<Packet2d,MatrixAlignment>(data + stride*8);
+      D1 = ploadt<Packet2d,MatrixAlignment>(data + stride*10);
+      B2 = ploadt<Packet2d,MatrixAlignment>(data + stride*12);
+      D2 = ploadt<Packet2d,MatrixAlignment>(data + stride*14);
+
+      temp = B1;
+      B1 = vec2d_unpacklo(B1, B2);
+      B2 = vec2d_unpackhi(temp, B2);
+
+      temp = D1;
+      D1 = vec2d_unpacklo(D1, D2);
+      D2 = vec2d_unpackhi(temp, D2);
+    }
+
+    // determinants of the sub-matrices
+    Packet2d dA, dB, dC, dD;
+
+    dA = vec2d_swizzle2(A2, A2, 1);
+    dA = pmul(A1, dA);
+    dA = psub(dA, vec2d_duplane(dA, 1));
+
+    dB = vec2d_swizzle2(B2, B2, 1);
+    dB = pmul(B1, dB);
+    dB = psub(dB, vec2d_duplane(dB, 1));
+
+    dC = vec2d_swizzle2(C2, C2, 1);
+    dC = pmul(C1, dC);
+    dC = psub(dC, vec2d_duplane(dC, 1));
+
+    dD = vec2d_swizzle2(D2, D2, 1);
+    dD = pmul(D1, dD);
+    dD = psub(dD, vec2d_duplane(dD, 1));
+
+    Packet2d DC1, DC2, AB1, AB2;
+
+    // AB = A# * B, where A# denotes the adjugate of A, and * denotes matrix product.
+    AB1 = pmul(B1, vec2d_duplane(A2, 1));
+    AB2 = pmul(B2, vec2d_duplane(A1, 0));
+    AB1 = psub(AB1, pmul(B2, vec2d_duplane(A1, 1)));
+    AB2 = psub(AB2, pmul(B1, vec2d_duplane(A2, 0)));
+
+    // DC = D#*C
+    DC1 = pmul(C1, vec2d_duplane(D2, 1));
+    DC2 = pmul(C2, vec2d_duplane(D1, 0));
+    DC1 = psub(DC1, pmul(C2, vec2d_duplane(D1, 1)));
+    DC2 = psub(DC2, pmul(C1, vec2d_duplane(D2, 0)));
+
+    Packet2d d1, d2;
+
+    // determinant of the input matrix, det = |A||D| + |B||C| - trace(A#*B*D#*C)
+    Packet2d det;
+
+    // reciprocal of the determinant of the input matrix, rd = 1/det
+    Packet2d rd;
+
+    d1 = pmul(AB1, vec2d_swizzle2(DC1, DC2, 0));
+    d2 = pmul(AB2, vec2d_swizzle2(DC1, DC2, 3));
+    rd = padd(d1, d2);
+    rd = padd(rd, vec2d_duplane(rd, 1));
+
+    d1 = pmul(dA, dD);
+    d2 = pmul(dB, dC);
+
+    det = padd(d1, d2);
+    det = psub(det, rd);
+    det = vec2d_duplane(det, 0);
+    rd = pdiv(pset1<Packet2d>(1.0), det);
+
+    // rows of four sub-matrices of the inverse
+    Packet2d iA1, iA2, iB1, iB2, iC1, iC2, iD1, iD2;
+
+    // iD = D*|A| - C*A#*B
+    iD1 = pmul(AB1, vec2d_duplane(C1, 0));
+    iD2 = pmul(AB1, vec2d_duplane(C2, 0));
+    iD1 = padd(iD1, pmul(AB2, vec2d_duplane(C1, 1)));
+    iD2 = padd(iD2, pmul(AB2, vec2d_duplane(C2, 1)));
+    dA = vec2d_duplane(dA, 0);
+    iD1 = psub(pmul(D1, dA), iD1);
+    iD2 = psub(pmul(D2, dA), iD2);
+
+    // iA = A*|D| - B*D#*C
+    iA1 = pmul(DC1, vec2d_duplane(B1, 0));
+    iA2 = pmul(DC1, vec2d_duplane(B2, 0));
+    iA1 = padd(iA1, pmul(DC2, vec2d_duplane(B1, 1)));
+    iA2 = padd(iA2, pmul(DC2, vec2d_duplane(B2, 1)));
+    dD = vec2d_duplane(dD, 0);
+    iA1 = psub(pmul(A1, dD), iA1);
+    iA2 = psub(pmul(A2, dD), iA2);
+
+    // iB = C*|B| - D * (A#B)# = C*|B| - D*B#*A
+    iB1 = pmul(D1, vec2d_swizzle2(AB2, AB1, 1));
+    iB2 = pmul(D2, vec2d_swizzle2(AB2, AB1, 1));
+    iB1 = psub(iB1, pmul(vec2d_swizzle2(D1, D1, 1), vec2d_swizzle2(AB2, AB1, 2)));
+    iB2 = psub(iB2, pmul(vec2d_swizzle2(D2, D2, 1), vec2d_swizzle2(AB2, AB1, 2)));
+    dB = vec2d_duplane(dB, 0);
+    iB1 = psub(pmul(C1, dB), iB1);
+    iB2 = psub(pmul(C2, dB), iB2);
+
+    // iC = B*|C| - A * (D#C)# = B*|C| - A*C#*D
+    iC1 = pmul(A1, vec2d_swizzle2(DC2, DC1, 1));
+    iC2 = pmul(A2, vec2d_swizzle2(DC2, DC1, 1));
+    iC1 = psub(iC1, pmul(vec2d_swizzle2(A1, A1, 1), vec2d_swizzle2(DC2, DC1, 2)));
+    iC2 = psub(iC2, pmul(vec2d_swizzle2(A2, A2, 1), vec2d_swizzle2(DC2, DC1, 2)));
+    dC = vec2d_duplane(dC, 0);
+    iC1 = psub(pmul(B1, dC), iC1);
+    iC2 = psub(pmul(B2, dC), iC2);
+
+    const double sign_mask1[2] = {0.0, numext::bit_cast<double>(0x8000000000000000ull)};
+    const double sign_mask2[2] = {numext::bit_cast<double>(0x8000000000000000ull), 0.0};
+    const Packet2d sign_PN = ploadu<Packet2d>(sign_mask1);
+    const Packet2d sign_NP = ploadu<Packet2d>(sign_mask2);
+    d1 = pxor(rd, sign_PN);
+    d2 = pxor(rd, sign_NP);
+
+    Index res_stride = result.outerStride();
+    double *res = result.data();
+    pstoret<double, Packet2d, ResultAlignment>(res + 0, pmul(vec2d_swizzle2(iA2, iA1, 3), d1));
+    pstoret<double, Packet2d, ResultAlignment>(res + res_stride, pmul(vec2d_swizzle2(iA2, iA1, 0), d2));
+    pstoret<double, Packet2d, ResultAlignment>(res + 2, pmul(vec2d_swizzle2(iB2, iB1, 3), d1));
+    pstoret<double, Packet2d, ResultAlignment>(res + res_stride + 2, pmul(vec2d_swizzle2(iB2, iB1, 0), d2));
+    pstoret<double, Packet2d, ResultAlignment>(res + 2 * res_stride, pmul(vec2d_swizzle2(iC2, iC1, 3), d1));
+    pstoret<double, Packet2d, ResultAlignment>(res + 3 * res_stride, pmul(vec2d_swizzle2(iC2, iC1, 0), d2));
+    pstoret<double, Packet2d, ResultAlignment>(res + 2 * res_stride + 2, pmul(vec2d_swizzle2(iD2, iD1, 3), d1));
+    pstoret<double, Packet2d, ResultAlignment>(res + 3 * res_stride + 2, pmul(vec2d_swizzle2(iD2, iD1, 0), d2));
+  }
+};
+#endif
+} // namespace internal
+} // namespace Eigen
+#endif
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h
deleted file mode 100644
index ebb64a6..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/LU/arch/Inverse_SSE.h
+++ /dev/null
@@ -1,338 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2001 Intel Corporation
-// Copyright (C) 2010 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-// The SSE code for the 4x4 float and double matrix inverse in this file
-// comes from the following Intel's library:
-// http://software.intel.com/en-us/articles/optimized-matrix-library-for-use-with-the-intel-pentiumr-4-processors-sse2-instructions/
-//
-// Here is the respective copyright and license statement:
-//
-//   Copyright (c) 2001 Intel Corporation.
-//
-// Permition is granted to use, copy, distribute and prepare derivative works
-// of this library for any purpose and without fee, provided, that the above
-// copyright notice and this statement appear in all copies.
-// Intel makes no representations about the suitability of this software for
-// any purpose, and specifically disclaims all warranties.
-// See LEGAL.TXT for all the legal information.
-
-#ifndef EIGEN_INVERSE_SSE_H
-#define EIGEN_INVERSE_SSE_H
-
-namespace Eigen { 
-
-namespace internal {
-
-template<typename MatrixType, typename ResultType>
-struct compute_inverse_size4<Architecture::SSE, float, MatrixType, ResultType>
-{
-  enum {
-    MatrixAlignment     = traits<MatrixType>::Alignment,
-    ResultAlignment     = traits<ResultType>::Alignment,
-    StorageOrdersMatch  = (MatrixType::Flags&RowMajorBit) == (ResultType::Flags&RowMajorBit)
-  };
-  typedef typename conditional<(MatrixType::Flags&LinearAccessBit),MatrixType const &,typename MatrixType::PlainObject>::type ActualMatrixType;
-  
-  static void run(const MatrixType& mat, ResultType& result)
-  {
-    ActualMatrixType matrix(mat);
-    EIGEN_ALIGN16 const unsigned int _Sign_PNNP[4] = { 0x00000000, 0x80000000, 0x80000000, 0x00000000 };
-
-    // Load the full matrix into registers
-    __m128 _L1 = matrix.template packet<MatrixAlignment>( 0);
-    __m128 _L2 = matrix.template packet<MatrixAlignment>( 4);
-    __m128 _L3 = matrix.template packet<MatrixAlignment>( 8);
-    __m128 _L4 = matrix.template packet<MatrixAlignment>(12);
-
-    // The inverse is calculated using "Divide and Conquer" technique. The
-    // original matrix is divide into four 2x2 sub-matrices. Since each
-    // register holds four matrix element, the smaller matrices are
-    // represented as a registers. Hence we get a better locality of the
-    // calculations.
-
-    __m128 A, B, C, D; // the four sub-matrices
-    if(!StorageOrdersMatch)
-    {
-      A = _mm_unpacklo_ps(_L1, _L2);
-      B = _mm_unpacklo_ps(_L3, _L4);
-      C = _mm_unpackhi_ps(_L1, _L2);
-      D = _mm_unpackhi_ps(_L3, _L4);
-    }
-    else
-    {
-      A = _mm_movelh_ps(_L1, _L2);
-      B = _mm_movehl_ps(_L2, _L1);
-      C = _mm_movelh_ps(_L3, _L4);
-      D = _mm_movehl_ps(_L4, _L3);
-    }
-
-    __m128 iA, iB, iC, iD,                 // partial inverse of the sub-matrices
-            DC, AB;
-    __m128 dA, dB, dC, dD;                 // determinant of the sub-matrices
-    __m128 det, d, d1, d2;
-    __m128 rd;                             // reciprocal of the determinant
-
-    //  AB = A# * B
-    AB = _mm_mul_ps(_mm_shuffle_ps(A,A,0x0F), B);
-    AB = _mm_sub_ps(AB,_mm_mul_ps(_mm_shuffle_ps(A,A,0xA5), _mm_shuffle_ps(B,B,0x4E)));
-    //  DC = D# * C
-    DC = _mm_mul_ps(_mm_shuffle_ps(D,D,0x0F), C);
-    DC = _mm_sub_ps(DC,_mm_mul_ps(_mm_shuffle_ps(D,D,0xA5), _mm_shuffle_ps(C,C,0x4E)));
-
-    //  dA = |A|
-    dA = _mm_mul_ps(_mm_shuffle_ps(A, A, 0x5F),A);
-    dA = _mm_sub_ss(dA, _mm_movehl_ps(dA,dA));
-    //  dB = |B|
-    dB = _mm_mul_ps(_mm_shuffle_ps(B, B, 0x5F),B);
-    dB = _mm_sub_ss(dB, _mm_movehl_ps(dB,dB));
-
-    //  dC = |C|
-    dC = _mm_mul_ps(_mm_shuffle_ps(C, C, 0x5F),C);
-    dC = _mm_sub_ss(dC, _mm_movehl_ps(dC,dC));
-    //  dD = |D|
-    dD = _mm_mul_ps(_mm_shuffle_ps(D, D, 0x5F),D);
-    dD = _mm_sub_ss(dD, _mm_movehl_ps(dD,dD));
-
-    //  d = trace(AB*DC) = trace(A#*B*D#*C)
-    d = _mm_mul_ps(_mm_shuffle_ps(DC,DC,0xD8),AB);
-
-    //  iD = C*A#*B
-    iD = _mm_mul_ps(_mm_shuffle_ps(C,C,0xA0), _mm_movelh_ps(AB,AB));
-    iD = _mm_add_ps(iD,_mm_mul_ps(_mm_shuffle_ps(C,C,0xF5), _mm_movehl_ps(AB,AB)));
-    //  iA = B*D#*C
-    iA = _mm_mul_ps(_mm_shuffle_ps(B,B,0xA0), _mm_movelh_ps(DC,DC));
-    iA = _mm_add_ps(iA,_mm_mul_ps(_mm_shuffle_ps(B,B,0xF5), _mm_movehl_ps(DC,DC)));
-
-    //  d = trace(AB*DC) = trace(A#*B*D#*C) [continue]
-    d  = _mm_add_ps(d, _mm_movehl_ps(d, d));
-    d  = _mm_add_ss(d, _mm_shuffle_ps(d, d, 1));
-    d1 = _mm_mul_ss(dA,dD);
-    d2 = _mm_mul_ss(dB,dC);
-
-    //  iD = D*|A| - C*A#*B
-    iD = _mm_sub_ps(_mm_mul_ps(D,_mm_shuffle_ps(dA,dA,0)), iD);
-
-    //  iA = A*|D| - B*D#*C;
-    iA = _mm_sub_ps(_mm_mul_ps(A,_mm_shuffle_ps(dD,dD,0)), iA);
-
-    //  det = |A|*|D| + |B|*|C| - trace(A#*B*D#*C)
-    det = _mm_sub_ss(_mm_add_ss(d1,d2),d);
-    rd  = _mm_div_ss(_mm_set_ss(1.0f), det);
-
-//     #ifdef ZERO_SINGULAR
-//         rd = _mm_and_ps(_mm_cmpneq_ss(det,_mm_setzero_ps()), rd);
-//     #endif
-
-    //  iB = D * (A#B)# = D*B#*A
-    iB = _mm_mul_ps(D, _mm_shuffle_ps(AB,AB,0x33));
-    iB = _mm_sub_ps(iB, _mm_mul_ps(_mm_shuffle_ps(D,D,0xB1), _mm_shuffle_ps(AB,AB,0x66)));
-    //  iC = A * (D#C)# = A*C#*D
-    iC = _mm_mul_ps(A, _mm_shuffle_ps(DC,DC,0x33));
-    iC = _mm_sub_ps(iC, _mm_mul_ps(_mm_shuffle_ps(A,A,0xB1), _mm_shuffle_ps(DC,DC,0x66)));
-
-    rd = _mm_shuffle_ps(rd,rd,0);
-    rd = _mm_xor_ps(rd, _mm_load_ps((float*)_Sign_PNNP));
-
-    //  iB = C*|B| - D*B#*A
-    iB = _mm_sub_ps(_mm_mul_ps(C,_mm_shuffle_ps(dB,dB,0)), iB);
-
-    //  iC = B*|C| - A*C#*D;
-    iC = _mm_sub_ps(_mm_mul_ps(B,_mm_shuffle_ps(dC,dC,0)), iC);
-
-    //  iX = iX / det
-    iA = _mm_mul_ps(rd,iA);
-    iB = _mm_mul_ps(rd,iB);
-    iC = _mm_mul_ps(rd,iC);
-    iD = _mm_mul_ps(rd,iD);
-
-    Index res_stride = result.outerStride();
-    float* res = result.data();
-    pstoret<float, Packet4f, ResultAlignment>(res+0,            _mm_shuffle_ps(iA,iB,0x77));
-    pstoret<float, Packet4f, ResultAlignment>(res+res_stride,   _mm_shuffle_ps(iA,iB,0x22));
-    pstoret<float, Packet4f, ResultAlignment>(res+2*res_stride, _mm_shuffle_ps(iC,iD,0x77));
-    pstoret<float, Packet4f, ResultAlignment>(res+3*res_stride, _mm_shuffle_ps(iC,iD,0x22));
-  }
-
-};
-
-template<typename MatrixType, typename ResultType>
-struct compute_inverse_size4<Architecture::SSE, double, MatrixType, ResultType>
-{
-  enum {
-    MatrixAlignment     = traits<MatrixType>::Alignment,
-    ResultAlignment     = traits<ResultType>::Alignment,
-    StorageOrdersMatch  = (MatrixType::Flags&RowMajorBit) == (ResultType::Flags&RowMajorBit)
-  };
-  typedef typename conditional<(MatrixType::Flags&LinearAccessBit),MatrixType const &,typename MatrixType::PlainObject>::type ActualMatrixType;
-  
-  static void run(const MatrixType& mat, ResultType& result)
-  {
-    ActualMatrixType matrix(mat);
-    const __m128d _Sign_NP = _mm_castsi128_pd(_mm_set_epi32(0x0,0x0,0x80000000,0x0));
-    const __m128d _Sign_PN = _mm_castsi128_pd(_mm_set_epi32(0x80000000,0x0,0x0,0x0));
-
-    // The inverse is calculated using "Divide and Conquer" technique. The
-    // original matrix is divide into four 2x2 sub-matrices. Since each
-    // register of the matrix holds two elements, the smaller matrices are
-    // consisted of two registers. Hence we get a better locality of the
-    // calculations.
-
-    // the four sub-matrices
-    __m128d A1, A2, B1, B2, C1, C2, D1, D2;
-    
-    if(StorageOrdersMatch)
-    {
-      A1 = matrix.template packet<MatrixAlignment>( 0); B1 = matrix.template packet<MatrixAlignment>( 2);
-      A2 = matrix.template packet<MatrixAlignment>( 4); B2 = matrix.template packet<MatrixAlignment>( 6);
-      C1 = matrix.template packet<MatrixAlignment>( 8); D1 = matrix.template packet<MatrixAlignment>(10);
-      C2 = matrix.template packet<MatrixAlignment>(12); D2 = matrix.template packet<MatrixAlignment>(14);
-    }
-    else
-    {
-      __m128d tmp;
-      A1 = matrix.template packet<MatrixAlignment>( 0); C1 = matrix.template packet<MatrixAlignment>( 2);
-      A2 = matrix.template packet<MatrixAlignment>( 4); C2 = matrix.template packet<MatrixAlignment>( 6);
-      tmp = A1;
-      A1 = _mm_unpacklo_pd(A1,A2);
-      A2 = _mm_unpackhi_pd(tmp,A2);
-      tmp = C1;
-      C1 = _mm_unpacklo_pd(C1,C2);
-      C2 = _mm_unpackhi_pd(tmp,C2);
-      
-      B1 = matrix.template packet<MatrixAlignment>( 8); D1 = matrix.template packet<MatrixAlignment>(10);
-      B2 = matrix.template packet<MatrixAlignment>(12); D2 = matrix.template packet<MatrixAlignment>(14);
-      tmp = B1;
-      B1 = _mm_unpacklo_pd(B1,B2);
-      B2 = _mm_unpackhi_pd(tmp,B2);
-      tmp = D1;
-      D1 = _mm_unpacklo_pd(D1,D2);
-      D2 = _mm_unpackhi_pd(tmp,D2);
-    }
-    
-    __m128d iA1, iA2, iB1, iB2, iC1, iC2, iD1, iD2,     // partial invese of the sub-matrices
-            DC1, DC2, AB1, AB2;
-    __m128d dA, dB, dC, dD;     // determinant of the sub-matrices
-    __m128d det, d1, d2, rd;
-
-    //  dA = |A|
-    dA = _mm_shuffle_pd(A2, A2, 1);
-    dA = _mm_mul_pd(A1, dA);
-    dA = _mm_sub_sd(dA, _mm_shuffle_pd(dA,dA,3));
-    //  dB = |B|
-    dB = _mm_shuffle_pd(B2, B2, 1);
-    dB = _mm_mul_pd(B1, dB);
-    dB = _mm_sub_sd(dB, _mm_shuffle_pd(dB,dB,3));
-
-    //  AB = A# * B
-    AB1 = _mm_mul_pd(B1, _mm_shuffle_pd(A2,A2,3));
-    AB2 = _mm_mul_pd(B2, _mm_shuffle_pd(A1,A1,0));
-    AB1 = _mm_sub_pd(AB1, _mm_mul_pd(B2, _mm_shuffle_pd(A1,A1,3)));
-    AB2 = _mm_sub_pd(AB2, _mm_mul_pd(B1, _mm_shuffle_pd(A2,A2,0)));
-
-    //  dC = |C|
-    dC = _mm_shuffle_pd(C2, C2, 1);
-    dC = _mm_mul_pd(C1, dC);
-    dC = _mm_sub_sd(dC, _mm_shuffle_pd(dC,dC,3));
-    //  dD = |D|
-    dD = _mm_shuffle_pd(D2, D2, 1);
-    dD = _mm_mul_pd(D1, dD);
-    dD = _mm_sub_sd(dD, _mm_shuffle_pd(dD,dD,3));
-
-    //  DC = D# * C
-    DC1 = _mm_mul_pd(C1, _mm_shuffle_pd(D2,D2,3));
-    DC2 = _mm_mul_pd(C2, _mm_shuffle_pd(D1,D1,0));
-    DC1 = _mm_sub_pd(DC1, _mm_mul_pd(C2, _mm_shuffle_pd(D1,D1,3)));
-    DC2 = _mm_sub_pd(DC2, _mm_mul_pd(C1, _mm_shuffle_pd(D2,D2,0)));
-
-    //  rd = trace(AB*DC) = trace(A#*B*D#*C)
-    d1 = _mm_mul_pd(AB1, _mm_shuffle_pd(DC1, DC2, 0));
-    d2 = _mm_mul_pd(AB2, _mm_shuffle_pd(DC1, DC2, 3));
-    rd = _mm_add_pd(d1, d2);
-    rd = _mm_add_sd(rd, _mm_shuffle_pd(rd, rd,3));
-
-    //  iD = C*A#*B
-    iD1 = _mm_mul_pd(AB1, _mm_shuffle_pd(C1,C1,0));
-    iD2 = _mm_mul_pd(AB1, _mm_shuffle_pd(C2,C2,0));
-    iD1 = _mm_add_pd(iD1, _mm_mul_pd(AB2, _mm_shuffle_pd(C1,C1,3)));
-    iD2 = _mm_add_pd(iD2, _mm_mul_pd(AB2, _mm_shuffle_pd(C2,C2,3)));
-
-    //  iA = B*D#*C
-    iA1 = _mm_mul_pd(DC1, _mm_shuffle_pd(B1,B1,0));
-    iA2 = _mm_mul_pd(DC1, _mm_shuffle_pd(B2,B2,0));
-    iA1 = _mm_add_pd(iA1, _mm_mul_pd(DC2, _mm_shuffle_pd(B1,B1,3)));
-    iA2 = _mm_add_pd(iA2, _mm_mul_pd(DC2, _mm_shuffle_pd(B2,B2,3)));
-
-    //  iD = D*|A| - C*A#*B
-    dA = _mm_shuffle_pd(dA,dA,0);
-    iD1 = _mm_sub_pd(_mm_mul_pd(D1, dA), iD1);
-    iD2 = _mm_sub_pd(_mm_mul_pd(D2, dA), iD2);
-
-    //  iA = A*|D| - B*D#*C;
-    dD = _mm_shuffle_pd(dD,dD,0);
-    iA1 = _mm_sub_pd(_mm_mul_pd(A1, dD), iA1);
-    iA2 = _mm_sub_pd(_mm_mul_pd(A2, dD), iA2);
-
-    d1 = _mm_mul_sd(dA, dD);
-    d2 = _mm_mul_sd(dB, dC);
-
-    //  iB = D * (A#B)# = D*B#*A
-    iB1 = _mm_mul_pd(D1, _mm_shuffle_pd(AB2,AB1,1));
-    iB2 = _mm_mul_pd(D2, _mm_shuffle_pd(AB2,AB1,1));
-    iB1 = _mm_sub_pd(iB1, _mm_mul_pd(_mm_shuffle_pd(D1,D1,1), _mm_shuffle_pd(AB2,AB1,2)));
-    iB2 = _mm_sub_pd(iB2, _mm_mul_pd(_mm_shuffle_pd(D2,D2,1), _mm_shuffle_pd(AB2,AB1,2)));
-
-    //  det = |A|*|D| + |B|*|C| - trace(A#*B*D#*C)
-    det = _mm_add_sd(d1, d2);
-    det = _mm_sub_sd(det, rd);
-
-    //  iC = A * (D#C)# = A*C#*D
-    iC1 = _mm_mul_pd(A1, _mm_shuffle_pd(DC2,DC1,1));
-    iC2 = _mm_mul_pd(A2, _mm_shuffle_pd(DC2,DC1,1));
-    iC1 = _mm_sub_pd(iC1, _mm_mul_pd(_mm_shuffle_pd(A1,A1,1), _mm_shuffle_pd(DC2,DC1,2)));
-    iC2 = _mm_sub_pd(iC2, _mm_mul_pd(_mm_shuffle_pd(A2,A2,1), _mm_shuffle_pd(DC2,DC1,2)));
-
-    rd = _mm_div_sd(_mm_set_sd(1.0), det);
-//     #ifdef ZERO_SINGULAR
-//         rd = _mm_and_pd(_mm_cmpneq_sd(det,_mm_setzero_pd()), rd);
-//     #endif
-    rd = _mm_shuffle_pd(rd,rd,0);
-
-    //  iB = C*|B| - D*B#*A
-    dB = _mm_shuffle_pd(dB,dB,0);
-    iB1 = _mm_sub_pd(_mm_mul_pd(C1, dB), iB1);
-    iB2 = _mm_sub_pd(_mm_mul_pd(C2, dB), iB2);
-
-    d1 = _mm_xor_pd(rd, _Sign_PN);
-    d2 = _mm_xor_pd(rd, _Sign_NP);
-
-    //  iC = B*|C| - A*C#*D;
-    dC = _mm_shuffle_pd(dC,dC,0);
-    iC1 = _mm_sub_pd(_mm_mul_pd(B1, dC), iC1);
-    iC2 = _mm_sub_pd(_mm_mul_pd(B2, dC), iC2);
-
-    Index res_stride = result.outerStride();
-    double* res = result.data();
-    pstoret<double, Packet2d, ResultAlignment>(res+0,             _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 3), d1));
-    pstoret<double, Packet2d, ResultAlignment>(res+res_stride,    _mm_mul_pd(_mm_shuffle_pd(iA2, iA1, 0), d2));
-    pstoret<double, Packet2d, ResultAlignment>(res+2,             _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 3), d1));
-    pstoret<double, Packet2d, ResultAlignment>(res+res_stride+2,  _mm_mul_pd(_mm_shuffle_pd(iB2, iB1, 0), d2));
-    pstoret<double, Packet2d, ResultAlignment>(res+2*res_stride,  _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 3), d1));
-    pstoret<double, Packet2d, ResultAlignment>(res+3*res_stride,  _mm_mul_pd(_mm_shuffle_pd(iC2, iC1, 0), d2));
-    pstoret<double, Packet2d, ResultAlignment>(res+2*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 3), d1));
-    pstoret<double, Packet2d, ResultAlignment>(res+3*res_stride+2,_mm_mul_pd(_mm_shuffle_pd(iD2, iD1, 0), d2));
-  }
-};
-
-} // end namespace internal
-
-} // end namespace Eigen
-
-#endif // EIGEN_INVERSE_SSE_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
index a7b47d5..9b677e9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/ColPivHouseholderQR.h
@@ -17,6 +17,9 @@
 template<typename _MatrixType> struct traits<ColPivHouseholderQR<_MatrixType> >
  : traits<_MatrixType>
 {
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
   enum { Flags = 0 };
 };
 
@@ -46,20 +49,19 @@
   * \sa MatrixBase::colPivHouseholderQr()
   */
 template<typename _MatrixType> class ColPivHouseholderQR
+        : public SolverBase<ColPivHouseholderQR<_MatrixType> >
 {
   public:
 
     typedef _MatrixType MatrixType;
+    typedef SolverBase<ColPivHouseholderQR> Base;
+    friend class SolverBase<ColPivHouseholderQR>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(ColPivHouseholderQR)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    // FIXME should be int
-    typedef typename MatrixType::StorageIndex StorageIndex;
     typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
     typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime> PermutationType;
     typedef typename internal::plain_row_type<MatrixType, Index>::type IntRowVectorType;
@@ -156,6 +158,7 @@
       computeInPlace();
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
       * *this is the QR decomposition, if any exists.
       *
@@ -172,11 +175,8 @@
       */
     template<typename Rhs>
     inline const Solve<ColPivHouseholderQR, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "ColPivHouseholderQR is not initialized.");
-      return Solve<ColPivHouseholderQR, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     HouseholderSequenceType householderQ() const;
     HouseholderSequenceType matrixQ() const
@@ -402,7 +402,7 @@
       */
     RealScalar maxPivot() const { return m_maxpivot; }
 
-    /** \brief Reports whether the QR factorization was succesful.
+    /** \brief Reports whether the QR factorization was successful.
       *
       * \note This function always returns \c Success. It is provided for compatibility
       * with other factorization routines.
@@ -416,8 +416,10 @@
 
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
@@ -584,8 +586,6 @@
 template<typename RhsType, typename DstType>
 void ColPivHouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
-  eigen_assert(rhs.rows() == rows());
-
   const Index nonzero_pivots = nonzeroPivots();
 
   if(nonzero_pivots == 0)
@@ -596,11 +596,7 @@
 
   typename RhsType::PlainObject c(rhs);
 
-  // Note that the matrix Q = H_0^* H_1^*... so its inverse is Q^* = (H_0 H_1 ...)^T
-  c.applyOnTheLeft(householderSequence(m_qr, m_hCoeffs)
-                    .setLength(nonzero_pivots)
-                    .transpose()
-    );
+  c.applyOnTheLeft(householderQ().setLength(nonzero_pivots).adjoint() );
 
   m_qr.topLeftCorner(nonzero_pivots, nonzero_pivots)
       .template triangularView<Upper>()
@@ -609,6 +605,31 @@
   for(Index i = 0; i < nonzero_pivots; ++i) dst.row(m_colsPermutation.indices().coeff(i)) = c.row(i);
   for(Index i = nonzero_pivots; i < cols(); ++i) dst.row(m_colsPermutation.indices().coeff(i)).setZero();
 }
+
+template<typename _MatrixType>
+template<bool Conjugate, typename RhsType, typename DstType>
+void ColPivHouseholderQR<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
+  const Index nonzero_pivots = nonzeroPivots();
+
+  if(nonzero_pivots == 0)
+  {
+    dst.setZero();
+    return;
+  }
+
+  typename RhsType::PlainObject c(m_colsPermutation.transpose()*rhs);
+
+  m_qr.topLeftCorner(nonzero_pivots, nonzero_pivots)
+        .template triangularView<Upper>()
+        .transpose().template conjugateIf<Conjugate>()
+        .solveInPlace(c.topRows(nonzero_pivots));
+
+  dst.topRows(nonzero_pivots) = c.topRows(nonzero_pivots);
+  dst.bottomRows(rows()-nonzero_pivots).setZero();
+
+  dst.applyOnTheLeft(householderQ().setLength(nonzero_pivots).template conjugateIf<!Conjugate>() );
+}
 #endif
 
 namespace internal {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
index 34c637b..486d337 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/CompleteOrthogonalDecomposition.h
@@ -16,6 +16,9 @@
 template <typename _MatrixType>
 struct traits<CompleteOrthogonalDecomposition<_MatrixType> >
     : traits<_MatrixType> {
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
   enum { Flags = 0 };
 };
 
@@ -44,19 +47,21 @@
   * 
   * \sa MatrixBase::completeOrthogonalDecomposition()
   */
-template <typename _MatrixType>
-class CompleteOrthogonalDecomposition {
+template <typename _MatrixType> class CompleteOrthogonalDecomposition
+          : public SolverBase<CompleteOrthogonalDecomposition<_MatrixType> >
+{
  public:
   typedef _MatrixType MatrixType;
+  typedef SolverBase<CompleteOrthogonalDecomposition> Base;
+
+  template<typename Derived>
+  friend struct internal::solve_assertion;
+
+  EIGEN_GENERIC_PUBLIC_INTERFACE(CompleteOrthogonalDecomposition)
   enum {
-    RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-    ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
   };
-  typedef typename MatrixType::Scalar Scalar;
-  typedef typename MatrixType::RealScalar RealScalar;
-  typedef typename MatrixType::StorageIndex StorageIndex;
   typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
   typedef PermutationMatrix<ColsAtCompileTime, MaxColsAtCompileTime>
       PermutationType;
@@ -131,9 +136,9 @@
       m_temp(matrix.cols())
   {
     computeInPlace();
-  }
+  } 
 
-
+  #ifdef EIGEN_PARSED_BY_DOXYGEN
   /** This method computes the minimum-norm solution X to a least squares
    * problem \f[\mathrm{minimize} \|A X - B\|, \f] where \b A is the matrix of
    * which \c *this is the complete orthogonal decomposition.
@@ -145,11 +150,8 @@
    */
   template <typename Rhs>
   inline const Solve<CompleteOrthogonalDecomposition, Rhs> solve(
-      const MatrixBase<Rhs>& b) const {
-    eigen_assert(m_cpqr.m_isInitialized &&
-                 "CompleteOrthogonalDecomposition is not initialized.");
-    return Solve<CompleteOrthogonalDecomposition, Rhs>(*this, b.derived());
-  }
+      const MatrixBase<Rhs>& b) const;
+  #endif
 
   HouseholderSequenceType householderQ(void) const;
   HouseholderSequenceType matrixQ(void) const { return m_cpqr.householderQ(); }
@@ -158,8 +160,8 @@
    */
   MatrixType matrixZ() const {
     MatrixType Z = MatrixType::Identity(m_cpqr.cols(), m_cpqr.cols());
-    applyZAdjointOnTheLeftInPlace(Z);
-    return Z.adjoint();
+    applyZOnTheLeftInPlace<false>(Z);
+    return Z;
   }
 
   /** \returns a reference to the matrix where the complete orthogonal
@@ -275,6 +277,7 @@
    */
   inline const Inverse<CompleteOrthogonalDecomposition> pseudoInverse() const
   {
+    eigen_assert(m_cpqr.m_isInitialized && "CompleteOrthogonalDecomposition is not initialized.");
     return Inverse<CompleteOrthogonalDecomposition>(*this);
   }
 
@@ -353,7 +356,7 @@
   inline RealScalar maxPivot() const { return m_cpqr.maxPivot(); }
 
   /** \brief Reports whether the complete orthogonal decomposition was
-   * succesful.
+   * successful.
    *
    * \note This function always returns \c Success. It is provided for
    * compatibility
@@ -367,7 +370,10 @@
 
 #ifndef EIGEN_PARSED_BY_DOXYGEN
   template <typename RhsType, typename DstType>
-  EIGEN_DEVICE_FUNC void _solve_impl(const RhsType& rhs, DstType& dst) const;
+  void _solve_impl(const RhsType& rhs, DstType& dst) const;
+
+  template<bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
 #endif
 
  protected:
@@ -375,8 +381,22 @@
     EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
   }
 
+  template<bool Transpose_, typename Rhs>
+  void _check_solve_assertion(const Rhs& b) const {
+      EIGEN_ONLY_USED_FOR_DEBUG(b);
+      eigen_assert(m_cpqr.m_isInitialized && "CompleteOrthogonalDecomposition is not initialized.");
+      eigen_assert((Transpose_?derived().cols():derived().rows())==b.rows() && "CompleteOrthogonalDecomposition::solve(): invalid number of rows of the right hand side matrix b");
+  }
+
   void computeInPlace();
 
+  /** Overwrites \b rhs with \f$ \mathbf{Z} * \mathbf{rhs} \f$ or
+   *  \f$ \mathbf{\overline Z} * \mathbf{rhs} \f$ if \c Conjugate 
+   *  is set to \c true.
+   */
+  template <bool Conjugate, typename Rhs>
+  void applyZOnTheLeftInPlace(Rhs& rhs) const;
+
   /** Overwrites \b rhs with \f$ \mathbf{Z}^* * \mathbf{rhs} \f$.
    */
   template <typename Rhs>
@@ -452,7 +472,7 @@
         // Apply Z(k) to the first k rows of X_k
         m_cpqr.m_qr.topRightCorner(k, cols - rank + 1)
             .applyHouseholderOnTheRight(
-                m_cpqr.m_qr.row(k).tail(cols - rank).transpose(), m_zCoeffs(k),
+                m_cpqr.m_qr.row(k).tail(cols - rank).adjoint(), m_zCoeffs(k),
                 &m_temp(0));
       }
       if (k != rank - 1) {
@@ -465,13 +485,35 @@
 }
 
 template <typename MatrixType>
+template <bool Conjugate, typename Rhs>
+void CompleteOrthogonalDecomposition<MatrixType>::applyZOnTheLeftInPlace(
+    Rhs& rhs) const {
+  const Index cols = this->cols();
+  const Index nrhs = rhs.cols();
+  const Index rank = this->rank();
+  Matrix<typename Rhs::Scalar, Dynamic, 1> temp((std::max)(cols, nrhs));
+  for (Index k = rank-1; k >= 0; --k) {
+    if (k != rank - 1) {
+      rhs.row(k).swap(rhs.row(rank - 1));
+    }
+    rhs.middleRows(rank - 1, cols - rank + 1)
+        .applyHouseholderOnTheLeft(
+            matrixQTZ().row(k).tail(cols - rank).transpose().template conjugateIf<!Conjugate>(), zCoeffs().template conjugateIf<Conjugate>()(k),
+            &temp(0));
+    if (k != rank - 1) {
+      rhs.row(k).swap(rhs.row(rank - 1));
+    }
+  }
+}
+
+template <typename MatrixType>
 template <typename Rhs>
 void CompleteOrthogonalDecomposition<MatrixType>::applyZAdjointOnTheLeftInPlace(
     Rhs& rhs) const {
   const Index cols = this->cols();
   const Index nrhs = rhs.cols();
   const Index rank = this->rank();
-  Matrix<typename MatrixType::Scalar, Dynamic, 1> temp((std::max)(cols, nrhs));
+  Matrix<typename Rhs::Scalar, Dynamic, 1> temp((std::max)(cols, nrhs));
   for (Index k = 0; k < rank; ++k) {
     if (k != rank - 1) {
       rhs.row(k).swap(rhs.row(rank - 1));
@@ -491,8 +533,6 @@
 template <typename RhsType, typename DstType>
 void CompleteOrthogonalDecomposition<_MatrixType>::_solve_impl(
     const RhsType& rhs, DstType& dst) const {
-  eigen_assert(rhs.rows() == this->rows());
-
   const Index rank = this->rank();
   if (rank == 0) {
     dst.setZero();
@@ -500,11 +540,8 @@
   }
 
   // Compute c = Q^* * rhs
-  // Note that the matrix Q = H_0^* H_1^*... so its inverse is
-  // Q^* = (H_0 H_1 ...)^T
   typename RhsType::PlainObject c(rhs);
-  c.applyOnTheLeft(
-      householderSequence(matrixQTZ(), hCoeffs()).setLength(rank).transpose());
+  c.applyOnTheLeft(matrixQ().setLength(rank).adjoint());
 
   // Solve T z = c(1:rank, :)
   dst.topRows(rank) = matrixT()
@@ -523,10 +560,45 @@
   // Undo permutation to get x = P^{-1} * y.
   dst = colsPermutation() * dst;
 }
+
+template<typename _MatrixType>
+template<bool Conjugate, typename RhsType, typename DstType>
+void CompleteOrthogonalDecomposition<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
+  const Index rank = this->rank();
+
+  if (rank == 0) {
+    dst.setZero();
+    return;
+  }
+
+  typename RhsType::PlainObject c(colsPermutation().transpose()*rhs);
+
+  if (rank < cols()) {
+    applyZOnTheLeftInPlace<!Conjugate>(c);
+  }
+
+  matrixT().topLeftCorner(rank, rank)
+           .template triangularView<Upper>()
+           .transpose().template conjugateIf<Conjugate>()
+           .solveInPlace(c.topRows(rank));
+
+  dst.topRows(rank) = c.topRows(rank);
+  dst.bottomRows(rows()-rank).setZero();
+
+  dst.applyOnTheLeft(householderQ().setLength(rank).template conjugateIf<!Conjugate>() );
+}
 #endif
 
 namespace internal {
 
+template<typename MatrixType>
+struct traits<Inverse<CompleteOrthogonalDecomposition<MatrixType> > >
+  : traits<typename Transpose<typename MatrixType::PlainObject>::PlainObject>
+{
+  enum { Flags = 0 };
+};
+
 template<typename DstXprType, typename MatrixType>
 struct Assignment<DstXprType, Inverse<CompleteOrthogonalDecomposition<MatrixType> >, internal::assign_op<typename DstXprType::Scalar,typename CompleteOrthogonalDecomposition<MatrixType>::Scalar>, Dense2Dense>
 {
@@ -534,7 +606,8 @@
   typedef Inverse<CodType> SrcXprType;
   static void run(DstXprType &dst, const SrcXprType &src, const internal::assign_op<typename DstXprType::Scalar,typename CodType::Scalar> &)
   {
-    dst = src.nestedExpression().solve(MatrixType::Identity(src.rows(), src.rows()));
+    typedef Matrix<typename CodType::Scalar, CodType::RowsAtCompileTime, CodType::RowsAtCompileTime, 0, CodType::MaxRowsAtCompileTime, CodType::MaxRowsAtCompileTime> IdentityMatrixType;
+    dst = src.nestedExpression().solve(IdentityMatrixType::Identity(src.cols(), src.cols()));
   }
 };
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
index e489bdd..d0664a1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/FullPivHouseholderQR.h
@@ -18,6 +18,9 @@
 template<typename _MatrixType> struct traits<FullPivHouseholderQR<_MatrixType> >
  : traits<_MatrixType>
 {
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
   enum { Flags = 0 };
 };
 
@@ -55,20 +58,19 @@
   * \sa MatrixBase::fullPivHouseholderQr()
   */
 template<typename _MatrixType> class FullPivHouseholderQR
+        : public SolverBase<FullPivHouseholderQR<_MatrixType> >
 {
   public:
 
     typedef _MatrixType MatrixType;
+    typedef SolverBase<FullPivHouseholderQR> Base;
+    friend class SolverBase<FullPivHouseholderQR>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(FullPivHouseholderQR)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    // FIXME should be int
-    typedef typename MatrixType::StorageIndex StorageIndex;
     typedef internal::FullPivHouseholderQRMatrixQReturnType<MatrixType> MatrixQReturnType;
     typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
     typedef Matrix<StorageIndex, 1,
@@ -156,6 +158,7 @@
       computeInPlace();
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
       * \c *this is the QR decomposition.
       *
@@ -173,11 +176,8 @@
       */
     template<typename Rhs>
     inline const Solve<FullPivHouseholderQR, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "FullPivHouseholderQR is not initialized.");
-      return Solve<FullPivHouseholderQR, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     /** \returns Expression object representing the matrix Q
       */
@@ -392,22 +392,24 @@
       *          diagonal coefficient of U.
       */
     RealScalar maxPivot() const { return m_maxpivot; }
-    
+
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
-    
+
     static void check_template_parameters()
     {
       EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
     }
-    
+
     void computeInPlace();
-    
+
     MatrixType m_qr;
     HCoeffsType m_hCoeffs;
     IntDiagSizeVectorType m_rows_transpositions;
@@ -499,15 +501,15 @@
       m_nonzero_pivots = k;
       for(Index i = k; i < size; i++)
       {
-        m_rows_transpositions.coeffRef(i) = i;
-        m_cols_transpositions.coeffRef(i) = i;
+        m_rows_transpositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
+        m_cols_transpositions.coeffRef(i) = internal::convert_index<StorageIndex>(i);
         m_hCoeffs.coeffRef(i) = Scalar(0);
       }
       break;
     }
 
-    m_rows_transpositions.coeffRef(k) = row_of_biggest_in_corner;
-    m_cols_transpositions.coeffRef(k) = col_of_biggest_in_corner;
+    m_rows_transpositions.coeffRef(k) = internal::convert_index<StorageIndex>(row_of_biggest_in_corner);
+    m_cols_transpositions.coeffRef(k) = internal::convert_index<StorageIndex>(col_of_biggest_in_corner);
     if(k != row_of_biggest_in_corner) {
       m_qr.row(k).tail(cols-k).swap(m_qr.row(row_of_biggest_in_corner).tail(cols-k));
       ++number_of_transpositions;
@@ -541,7 +543,6 @@
 template<typename RhsType, typename DstType>
 void FullPivHouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
-  eigen_assert(rhs.rows() == rows());
   const Index l_rank = rank();
 
   // FIXME introduce nonzeroPivots() and use it here. and more generally,
@@ -554,7 +555,7 @@
 
   typename RhsType::PlainObject c(rhs);
 
-  Matrix<Scalar,1,RhsType::ColsAtCompileTime> temp(rhs.cols());
+  Matrix<typename RhsType::Scalar,1,RhsType::ColsAtCompileTime> temp(rhs.cols());
   for (Index k = 0; k < l_rank; ++k)
   {
     Index remainingSize = rows()-k;
@@ -571,6 +572,42 @@
   for(Index i = 0; i < l_rank; ++i) dst.row(m_cols_permutation.indices().coeff(i)) = c.row(i);
   for(Index i = l_rank; i < cols(); ++i) dst.row(m_cols_permutation.indices().coeff(i)).setZero();
 }
+
+template<typename _MatrixType>
+template<bool Conjugate, typename RhsType, typename DstType>
+void FullPivHouseholderQR<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
+  const Index l_rank = rank();
+
+  if(l_rank == 0)
+  {
+    dst.setZero();
+    return;
+  }
+
+  typename RhsType::PlainObject c(m_cols_permutation.transpose()*rhs);
+
+  m_qr.topLeftCorner(l_rank, l_rank)
+         .template triangularView<Upper>()
+         .transpose().template conjugateIf<Conjugate>()
+         .solveInPlace(c.topRows(l_rank));
+
+  dst.topRows(l_rank) = c.topRows(l_rank);
+  dst.bottomRows(rows()-l_rank).setZero();
+
+  Matrix<Scalar, 1, DstType::ColsAtCompileTime> temp(dst.cols());
+  const Index size = (std::min)(rows(), cols());
+  for (Index k = size-1; k >= 0; --k)
+  {
+    Index remainingSize = rows()-k;
+
+    dst.bottomRightCorner(remainingSize, dst.cols())
+       .applyHouseholderOnTheLeft(m_qr.col(k).tail(remainingSize-1).template conjugateIf<!Conjugate>(),
+                                  m_hCoeffs.template conjugateIf<Conjugate>().coeff(k), &temp.coeffRef(0));
+
+    dst.row(k).swap(dst.row(m_rows_transpositions.coeff(k)));
+  }
+}
 #endif
 
 namespace internal {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
index 3513d99..801739f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/QR/HouseholderQR.h
@@ -14,6 +14,18 @@
 
 namespace Eigen { 
 
+namespace internal {
+template<typename _MatrixType> struct traits<HouseholderQR<_MatrixType> >
+ : traits<_MatrixType>
+{
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
+  enum { Flags = 0 };
+};
+
+} // end namespace internal
+
 /** \ingroup QR_Module
   *
   *
@@ -42,20 +54,19 @@
   * \sa MatrixBase::householderQr()
   */
 template<typename _MatrixType> class HouseholderQR
+        : public SolverBase<HouseholderQR<_MatrixType> >
 {
   public:
 
     typedef _MatrixType MatrixType;
+    typedef SolverBase<HouseholderQR> Base;
+    friend class SolverBase<HouseholderQR>;
+
+    EIGEN_GENERIC_PUBLIC_INTERFACE(HouseholderQR)
     enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      ColsAtCompileTime = MatrixType::ColsAtCompileTime,
       MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
       MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime
     };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    // FIXME should be int
-    typedef typename MatrixType::StorageIndex StorageIndex;
     typedef Matrix<Scalar, RowsAtCompileTime, RowsAtCompileTime, (MatrixType::Flags&RowMajorBit) ? RowMajor : ColMajor, MaxRowsAtCompileTime, MaxRowsAtCompileTime> MatrixQType;
     typedef typename internal::plain_diag_type<MatrixType>::type HCoeffsType;
     typedef typename internal::plain_row_type<MatrixType>::type RowVectorType;
@@ -121,6 +132,7 @@
       computeInPlace();
     }
 
+    #ifdef EIGEN_PARSED_BY_DOXYGEN
     /** This method finds a solution x to the equation Ax=b, where A is the matrix of which
       * *this is the QR decomposition, if any exists.
       *
@@ -137,11 +149,8 @@
       */
     template<typename Rhs>
     inline const Solve<HouseholderQR, Rhs>
-    solve(const MatrixBase<Rhs>& b) const
-    {
-      eigen_assert(m_isInitialized && "HouseholderQR is not initialized.");
-      return Solve<HouseholderQR, Rhs>(*this, b.derived());
-    }
+    solve(const MatrixBase<Rhs>& b) const;
+    #endif
 
     /** This method returns an expression of the unitary matrix Q as a sequence of Householder transformations.
       *
@@ -204,28 +213,30 @@
 
     inline Index rows() const { return m_qr.rows(); }
     inline Index cols() const { return m_qr.cols(); }
-    
+
     /** \returns a const reference to the vector of Householder coefficients used to represent the factor \c Q.
       * 
       * For advanced uses only.
       */
     const HCoeffsType& hCoeffs() const { return m_hCoeffs; }
-    
+
     #ifndef EIGEN_PARSED_BY_DOXYGEN
     template<typename RhsType, typename DstType>
-    EIGEN_DEVICE_FUNC
     void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+    template<bool Conjugate, typename RhsType, typename DstType>
+    void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
     #endif
 
   protected:
-    
+
     static void check_template_parameters()
     {
       EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
     }
 
     void computeInPlace();
-    
+
     MatrixType m_qr;
     HCoeffsType m_hCoeffs;
     RowVectorType m_temp;
@@ -292,7 +303,7 @@
   bool InnerStrideIsOne = (MatrixQR::InnerStrideAtCompileTime == 1 && HCoeffs::InnerStrideAtCompileTime == 1)>
 struct householder_qr_inplace_blocked
 {
-  // This is specialized for MKL-supported Scalar types in HouseholderQR_MKL.h
+  // This is specialized for LAPACK-supported Scalar types in HouseholderQR_LAPACKE.h
   static void run(MatrixQR& mat, HCoeffs& hCoeffs, Index maxBlockSize=32,
       typename MatrixQR::Scalar* tempData = 0)
   {
@@ -350,15 +361,10 @@
 void HouseholderQR<_MatrixType>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
   const Index rank = (std::min)(rows(), cols());
-  eigen_assert(rhs.rows() == rows());
 
   typename RhsType::PlainObject c(rhs);
 
-  // Note that the matrix Q = H_0^* H_1^*... so its inverse is Q^* = (H_0 H_1 ...)^T
-  c.applyOnTheLeft(householderSequence(
-    m_qr.leftCols(rank),
-    m_hCoeffs.head(rank)).transpose()
-  );
+  c.applyOnTheLeft(householderQ().setLength(rank).adjoint() );
 
   m_qr.topLeftCorner(rank, rank)
       .template triangularView<Upper>()
@@ -367,6 +373,25 @@
   dst.topRows(rank) = c.topRows(rank);
   dst.bottomRows(cols()-rank).setZero();
 }
+
+template<typename _MatrixType>
+template<bool Conjugate, typename RhsType, typename DstType>
+void HouseholderQR<_MatrixType>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
+  const Index rank = (std::min)(rows(), cols());
+
+  typename RhsType::PlainObject c(rhs);
+
+  m_qr.topLeftCorner(rank, rank)
+      .template triangularView<Upper>()
+      .transpose().template conjugateIf<Conjugate>()
+      .solveInPlace(c.topRows(rank));
+
+  dst.topRows(rank) = c.topRows(rank);
+  dst.bottomRows(rows()-rank).setZero();
+
+  dst.applyOnTheLeft(householderQ().setLength(rank).template conjugateIf<!Conjugate>() );
+}
 #endif
 
 /** Performs the QR factorization of the given matrix \a matrix. The result of
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
index 1134d66..17f8e44 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/BDCSVD.h
@@ -22,6 +22,11 @@
 // #define EIGEN_BDCSVD_DEBUG_VERBOSE
 // #define EIGEN_BDCSVD_SANITY_CHECKS
 
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+#undef eigen_internal_assert
+#define eigen_internal_assert(X) assert(X);
+#endif
+
 namespace Eigen {
 
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
@@ -34,6 +39,7 @@
 
 template<typename _MatrixType> 
 struct traits<BDCSVD<_MatrixType> >
+        : traits<_MatrixType>
 {
   typedef _MatrixType MatrixType;
 };  
@@ -57,7 +63,7 @@
  * recommended and can several order of magnitude faster.
  *
  * \warning this algorithm is unlikely to provide accurate result when compiled with unsafe math optimizations.
- * For instance, this concerns Intel's compiler (ICC), which perfroms such optimization by default unless
+ * For instance, this concerns Intel's compiler (ICC), which performs such optimization by default unless
  * you compile with the \c -fp-model \c precise option. Likewise, the \c -ffast-math option of GCC or clang will
  * significantly degrade the accuracy.
  *
@@ -105,7 +111,7 @@
    * The default constructor is useful in cases in which the user intends to
    * perform decompositions via BDCSVD::compute(const MatrixType&).
    */
-  BDCSVD() : m_algoswap(16), m_numIters(0)
+  BDCSVD() : m_algoswap(16), m_isTranspose(false), m_compU(false), m_compV(false), m_numIters(0)
   {}
 
 
@@ -202,6 +208,7 @@
   using Base::m_computeThinV;
   using Base::m_matrixU;
   using Base::m_matrixV;
+  using Base::m_info;
   using Base::m_isInitialized;
   using Base::m_nonzeroSingularValues;
 
@@ -212,7 +219,7 @@
 
 // Method to allocate and initialize matrix and attributes
 template<typename MatrixType>
-void BDCSVD<MatrixType>::allocate(Index rows, Index cols, unsigned int computationOptions)
+void BDCSVD<MatrixType>::allocate(Eigen::Index rows, Eigen::Index cols, unsigned int computationOptions)
 {
   m_isTranspose = (cols > rows);
 
@@ -250,16 +257,25 @@
   {
     // FIXME this line involves temporaries
     JacobiSVD<MatrixType> jsvd(matrix,computationOptions);
-    if(computeU()) m_matrixU = jsvd.matrixU();
-    if(computeV()) m_matrixV = jsvd.matrixV();
-    m_singularValues = jsvd.singularValues();
-    m_nonzeroSingularValues = jsvd.nonzeroSingularValues();
     m_isInitialized = true;
+    m_info = jsvd.info();
+    if (m_info == Success || m_info == NoConvergence) {
+      if(computeU()) m_matrixU = jsvd.matrixU();
+      if(computeV()) m_matrixV = jsvd.matrixV();
+      m_singularValues = jsvd.singularValues();
+      m_nonzeroSingularValues = jsvd.nonzeroSingularValues();
+    }
     return *this;
   }
   
   //**** step 0 - Copy the input matrix and apply scaling to reduce over/under-flows
-  RealScalar scale = matrix.cwiseAbs().maxCoeff();
+  RealScalar scale = matrix.cwiseAbs().template maxCoeff<PropagateNaN>();
+  if (!(numext::isfinite)(scale)) {
+    m_isInitialized = true;
+    m_info = InvalidInput;
+    return *this;
+  }
+
   if(scale==Literal(0)) scale = Literal(1);
   MatrixX copy;
   if (m_isTranspose) copy = matrix.adjoint()/scale;
@@ -276,7 +292,11 @@
   m_computed.topRows(m_diagSize) = bid.bidiagonal().toDenseMatrix().transpose();
   m_computed.template bottomRows<1>().setZero();
   divide(0, m_diagSize - 1, 0, 0, 0);
-
+  if (m_info != Success && m_info != NoConvergence) {
+    m_isInitialized = true;
+    return *this;
+  }
+    
   //**** step 3 - Copy singular values and vectors
   for (int i=0; i<m_diagSize; i++)
   {
@@ -388,7 +408,7 @@
 //@param shift : Each time one takes the left submatrix, one must add 1 to the shift. Why? Because! We actually want the last column of the U submatrix 
 // to become the first column (*coeff) and to shift all the other columns to the right. There are more details on the reference paper.
 template<typename MatrixType>
-void BDCSVD<MatrixType>::divide (Index firstCol, Index lastCol, Index firstRowW, Index firstColW, Index shift)
+void BDCSVD<MatrixType>::divide(Eigen::Index firstCol, Eigen::Index lastCol, Eigen::Index firstRowW, Eigen::Index firstColW, Eigen::Index shift)
 {
   // requires rows = cols + 1;
   using std::pow;
@@ -408,6 +428,8 @@
   {
     // FIXME this line involves temporaries
     JacobiSVD<MatrixXr> b(m_computed.block(firstCol, firstCol, n + 1, n), ComputeFullU | (m_compV ? ComputeFullV : 0));
+    m_info = b.info();
+    if (m_info != Success && m_info != NoConvergence) return;
     if (m_compU)
       m_naiveU.block(firstCol, firstCol, n + 1, n + 1).real() = b.matrixU();
     else 
@@ -427,7 +449,9 @@
   // and the divide of the right submatrice reads one column of the left submatrice. That's why we need to treat the 
   // right submatrix before the left one. 
   divide(k + 1 + firstCol, lastCol, k + 1 + firstRowW, k + 1 + firstColW, shift);
+  if (m_info != Success && m_info != NoConvergence) return;
   divide(firstCol, k - 1 + firstCol, firstRowW, firstColW + 1, shift + 1);
+  if (m_info != Success && m_info != NoConvergence) return;
 
   if (m_compU)
   {
@@ -568,7 +592,7 @@
 // handling of round-off errors, be consistent in ordering
 // For instance, to solve the secular equation using FMM, see http://www.stat.uchicago.edu/~lekheng/courses/302/classics/greengard-rokhlin.pdf
 template <typename MatrixType>
-void BDCSVD<MatrixType>::computeSVDofM(Index firstCol, Index n, MatrixXr& U, VectorType& singVals, MatrixXr& V)
+void BDCSVD<MatrixType>::computeSVDofM(Eigen::Index firstCol, Eigen::Index n, MatrixXr& U, VectorType& singVals, MatrixXr& V)
 {
   const RealScalar considerZero = (std::numeric_limits<RealScalar>::min)();
   using std::abs;
@@ -591,7 +615,7 @@
   // but others are interleaved and we must ignore them at this stage.
   // To this end, let's compute a permutation skipping them:
   Index actual_n = n;
-  while(actual_n>1 && diag(actual_n-1)==Literal(0)) --actual_n;
+  while(actual_n>1 && diag(actual_n-1)==Literal(0)) {--actual_n; eigen_internal_assert(col0(actual_n)==Literal(0)); }
   Index m = 0; // size of the deflated problem
   for(Index k=0;k<actual_n;++k)
     if(abs(col0(k))>considerZero)
@@ -618,13 +642,11 @@
   std::cout << "  shift:    " << shifts.transpose() << "\n";
   
   {
-    Index actual_n = n;
-    while(actual_n>1 && abs(col0(actual_n-1))<considerZero) --actual_n;
     std::cout << "\n\n    mus:    " << mus.head(actual_n).transpose() << "\n\n";
     std::cout << "    check1 (expect0) : " << ((singVals.array()-(shifts+mus)) / singVals.array()).head(actual_n).transpose() << "\n\n";
+    assert((((singVals.array()-(shifts+mus)) / singVals.array()).head(actual_n) >= 0).all());
     std::cout << "    check2 (>0)      : " << ((singVals.array()-diag) / singVals.array()).head(actual_n).transpose() << "\n\n";
-    std::cout << "    check3 (>0)      : " << ((diag.segment(1,actual_n-1)-singVals.head(actual_n-1).array()) / singVals.head(actual_n-1).array()).transpose() << "\n\n\n";
-    std::cout << "    check4 (>0)      : " << ((singVals.segment(1,actual_n-1)-singVals.head(actual_n-1))).transpose() << "\n\n\n";
+    assert((((singVals.array()-diag) / singVals.array()).head(actual_n) >= 0).all());
   }
 #endif
   
@@ -652,13 +674,13 @@
 #endif
   
 #ifdef EIGEN_BDCSVD_SANITY_CHECKS
-  assert(U.allFinite());
-  assert(V.allFinite());
-  assert((U.transpose() * U - MatrixXr(MatrixXr::Identity(U.cols(),U.cols()))).norm() < 1e-14 * n);
-  assert((V.transpose() * V - MatrixXr(MatrixXr::Identity(V.cols(),V.cols()))).norm() < 1e-14 * n);
   assert(m_naiveU.allFinite());
   assert(m_naiveV.allFinite());
   assert(m_computed.allFinite());
+  assert(U.allFinite());
+  assert(V.allFinite());
+//   assert((U.transpose() * U - MatrixXr(MatrixXr::Identity(U.cols(),U.cols()))).norm() < 100*NumTraits<RealScalar>::epsilon() * n);
+//   assert((V.transpose() * V - MatrixXr(MatrixXr::Identity(V.cols(),V.cols()))).norm() < 100*NumTraits<RealScalar>::epsilon() * n);
 #endif
   
   // Because of deflation, the singular values might not be completely sorted.
@@ -673,6 +695,15 @@
       if(m_compV) V.col(i).swap(V.col(i+1));
     }
   }
+
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+  {
+    bool singular_values_sorted = (((singVals.segment(1,actual_n-1)-singVals.head(actual_n-1))).array() >= 0).all();
+    if(!singular_values_sorted)
+      std::cout << "Singular values are not sorted: " << singVals.segment(1,actual_n).transpose() << "\n";
+    assert(singular_values_sorted);
+  }
+#endif
   
   // Reverse order so that singular values in increased order
   // Because of deflation, the zeros singular-values are already at the end
@@ -749,25 +780,43 @@
     RealScalar mid = left + (right-left) / Literal(2);
     RealScalar fMid = secularEq(mid, col0, diag, perm, diag, Literal(0));
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-    std::cout << right-left << "\n";
-    std::cout << "fMid = " << fMid << " " << secularEq(mid-left, col0, diag, perm, diag-left, left) << " " << secularEq(mid-right, col0, diag, perm, diag-right, right)   << "\n";
-    std::cout << "     = " << secularEq(0.1*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.2*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.3*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.4*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.49*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.5*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.51*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.6*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.7*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.8*(left+right), col0, diag, perm, diag, 0)
-              << " "       << secularEq(0.9*(left+right), col0, diag, perm, diag, 0) << "\n";
+    std::cout << "right-left = " << right-left << "\n";
+//     std::cout << "fMid = " << fMid << " " << secularEq(mid-left, col0, diag, perm, ArrayXr(diag-left), left)
+//                            << " " << secularEq(mid-right, col0, diag, perm, ArrayXr(diag-right), right)   << "\n";
+    std::cout << "     = " << secularEq(left+RealScalar(0.000001)*(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.1)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.2)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.3)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.4)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.49)    *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.5)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.51)    *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.6)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.7)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.8)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.9)     *(right-left), col0, diag, perm, diag, 0)
+              << " "       << secularEq(left+RealScalar(0.999999)*(right-left), col0, diag, perm, diag, 0) << "\n";
 #endif
     RealScalar shift = (k == actual_n-1 || fMid > Literal(0)) ? left : right;
     
     // measure everything relative to shift
     Map<ArrayXr> diagShifted(m_workspace.data()+4*n, n);
     diagShifted = diag - shift;
+
+    if(k!=actual_n-1)
+    {
+      // check that after the shift, f(mid) is still negative:
+      RealScalar midShifted = (right - left) / RealScalar(2);
+      if(shift==right)
+        midShifted = -midShifted;
+      RealScalar fMidShifted = secularEq(midShifted, col0, diag, perm, diagShifted, shift);
+      if(fMidShifted>0)
+      {
+        // fMid was erroneous, fix it:
+        shift =  fMidShifted > Literal(0) ? left : right;
+        diagShifted = diag - shift;
+      }
+    }
     
     // initial guess
     RealScalar muPrev, muCur;
@@ -804,13 +853,16 @@
       // And find mu such that f(mu)==0:
       RealScalar muZero = -a/b;
       RealScalar fZero = secularEq(muZero, col0, diag, perm, diagShifted, shift);
+
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+      assert((numext::isfinite)(fZero));
+#endif
       
       muPrev = muCur;
       fPrev = fCur;
       muCur = muZero;
       fCur = fZero;
       
-      
       if (shift == left  && (muCur < Literal(0) || muCur > right - left)) useBisection = true;
       if (shift == right && (muCur < -(right - left) || muCur > Literal(0))) useBisection = true;
       if (abs(fCur)>abs(fPrev)) useBisection = true;
@@ -843,44 +895,82 @@
         else
           rightShifted = -(std::numeric_limits<RealScalar>::min)();
       }
-      
-      RealScalar fLeft = secularEq(leftShifted, col0, diag, perm, diagShifted, shift);
 
-#if defined EIGEN_INTERNAL_DEBUGGING || defined EIGEN_BDCSVD_DEBUG_VERBOSE
+      RealScalar fLeft = secularEq(leftShifted, col0, diag, perm, diagShifted, shift);
+      eigen_internal_assert(fLeft<Literal(0));
+
+#if defined EIGEN_INTERNAL_DEBUGGING || defined EIGEN_BDCSVD_SANITY_CHECKS
       RealScalar fRight = secularEq(rightShifted, col0, diag, perm, diagShifted, shift);
 #endif
 
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+      if(!(numext::isfinite)(fLeft))
+        std::cout << "f(" << leftShifted << ") =" << fLeft << " ; " << left << " " << shift << " " << right << "\n";
+      assert((numext::isfinite)(fLeft));
+
+      if(!(numext::isfinite)(fRight))
+        std::cout << "f(" << rightShifted << ") =" << fRight << " ; " << left << " " << shift << " " << right << "\n";
+      // assert((numext::isfinite)(fRight));
+#endif
+    
 #ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
       if(!(fLeft * fRight<0))
       {
-        std::cout << "fLeft: " << leftShifted << " - " << diagShifted.head(10).transpose()  << "\n ; " << bool(left==shift) << " " << (left-shift) << "\n";
-        std::cout << k << " : " <<  fLeft << " * " << fRight << " == " << fLeft * fRight << "  ;  " << left << " - " << right << " -> " <<  leftShifted << " " << rightShifted << "   shift=" << shift << "\n";
+        std::cout << "f(leftShifted) using  leftShifted=" << leftShifted << " ;  diagShifted(1:10):" << diagShifted.head(10).transpose()  << "\n ; "
+                  << "left==shift=" << bool(left==shift) << " ; left-shift = " << (left-shift) << "\n";
+        std::cout << "k=" << k << ", " <<  fLeft << " * " << fRight << " == " << fLeft * fRight << "  ;  "
+                  << "[" << left << " .. " << right << "] -> [" << leftShifted << " " << rightShifted << "], shift=" << shift
+                  << " ,  f(right)=" << secularEq(0,     col0, diag, perm, diagShifted, shift)
+                           << " == " << secularEq(right, col0, diag, perm, diag, 0) << " == " << fRight << "\n";
       }
 #endif
       eigen_internal_assert(fLeft * fRight < Literal(0));
-      
-      while (rightShifted - leftShifted > Literal(2) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(abs(leftShifted), abs(rightShifted)))
-      {
-        RealScalar midShifted = (leftShifted + rightShifted) / Literal(2);
-        fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift);
-        if (fLeft * fMid < Literal(0))
-        {
-          rightShifted = midShifted;
-        }
-        else
-        {
-          leftShifted = midShifted;
-          fLeft = fMid;
-        }
-      }
 
-      muCur = (leftShifted + rightShifted) / Literal(2);
+      if(fLeft<Literal(0))
+      {
+        while (rightShifted - leftShifted > Literal(2) * NumTraits<RealScalar>::epsilon() * numext::maxi<RealScalar>(abs(leftShifted), abs(rightShifted)))
+        {
+          RealScalar midShifted = (leftShifted + rightShifted) / Literal(2);
+          fMid = secularEq(midShifted, col0, diag, perm, diagShifted, shift);
+          eigen_internal_assert((numext::isfinite)(fMid));
+
+          if (fLeft * fMid < Literal(0))
+          {
+            rightShifted = midShifted;
+          }
+          else
+          {
+            leftShifted = midShifted;
+            fLeft = fMid;
+          }
+        }
+        muCur = (leftShifted + rightShifted) / Literal(2);
+      }
+      else 
+      {
+        // We have a problem as shifting on the left or right give either a positive or negative value
+        // at the middle of [left,right]...
+        // Instead fo abbording or entering an infinite loop,
+        // let's just use the middle as the estimated zero-crossing:
+        muCur = (right - left) * RealScalar(0.5);
+        if(shift == right)
+          muCur = -muCur;
+      }
     }
       
     singVals[k] = shift + muCur;
     shifts[k] = shift;
     mus[k] = muCur;
 
+#ifdef  EIGEN_BDCSVD_DEBUG_VERBOSE
+    if(k+1<n)
+      std::cout << "found " << singVals[k] << " == " << shift << " + " << muCur << " from " << diag(k) << " .. "  << diag(k+1) << "\n";
+#endif
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+    assert(k==0 || singVals[k]>=singVals[k-1]);
+    assert(singVals[k]>=diag(k));
+#endif
+
     // perturb singular value slightly if it equals diagonal entry to avoid division by zero later
     // (deflation is supposed to avoid this from happening)
     // - this does no seem to be necessary anymore -
@@ -904,7 +994,7 @@
     zhat.setZero();
     return;
   }
-  Index last = perm(m-1);
+  Index lastIdx = perm(m-1);
   // The offset permits to skip deflated entries while computing zhat
   for (Index k = 0; k < n; ++k)
   {
@@ -914,27 +1004,58 @@
     {
       // see equation (3.6)
       RealScalar dk = diag(k);
-      RealScalar prod = (singVals(last) + dk) * (mus(last) + (shifts(last) - dk));
+      RealScalar prod = (singVals(lastIdx) + dk) * (mus(lastIdx) + (shifts(lastIdx) - dk));
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+      if(prod<0) {
+        std::cout << "k = " << k << " ;  z(k)=" << col0(k) << ", diag(k)=" << dk << "\n";
+        std::cout << "prod = " << "(" << singVals(lastIdx) << " + " << dk << ") * (" << mus(lastIdx) << " + (" << shifts(lastIdx) << " - " << dk << "))" << "\n";
+        std::cout << "     = " << singVals(lastIdx) + dk << " * " << mus(lastIdx) + (shifts(lastIdx) - dk) <<  "\n";
+      }
+      assert(prod>=0);
+#endif
 
       for(Index l = 0; l<m; ++l)
       {
         Index i = perm(l);
         if(i!=k)
         {
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+          if(i>=k && (l==0 || l-1>=m))
+          {
+            std::cout << "Error in perturbCol0\n";
+            std::cout << "  " << k << "/" << n << " "  << l << "/" << m << " " << i << "/" << n << " ; " << col0(k) << " " << diag(k) << " "  <<  "\n";
+            std::cout << "  " <<diag(i) << "\n";
+            Index j = (i<k /*|| l==0*/) ? i : perm(l-1);
+            std::cout << "  " << "j=" << j << "\n";
+          }
+#endif
           Index j = i<k ? i : perm(l-1);
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+          if(!(dk!=Literal(0) || diag(i)!=Literal(0)))
+          {
+            std::cout << "k=" << k << ", i=" << i << ", l=" << l << ", perm.size()=" << perm.size() << "\n";
+          }
+          assert(dk!=Literal(0) || diag(i)!=Literal(0));
+#endif
           prod *= ((singVals(j)+dk) / ((diag(i)+dk))) * ((mus(j)+(shifts(j)-dk)) / ((diag(i)-dk)));
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+          assert(prod>=0);
+#endif
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-          if(i!=k && std::abs(((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) - 1) > 0.9 )
+          if(i!=k && numext::abs(((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) - 1) > 0.9 )
             std::cout << "     " << ((singVals(j)+dk)*(mus(j)+(shifts(j)-dk)))/((diag(i)+dk)*(diag(i)-dk)) << " == (" << (singVals(j)+dk) << " * " << (mus(j)+(shifts(j)-dk))
                        << ") / (" << (diag(i)+dk) << " * " << (diag(i)-dk) << ")\n";
 #endif
         }
       }
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-      std::cout << "zhat(" << k << ") =  sqrt( " << prod << ")  ;  " << (singVals(last) + dk) << " * " << mus(last) + shifts(last) << " - " << dk << "\n";
+      std::cout << "zhat(" << k << ") =  sqrt( " << prod << ")  ;  " << (singVals(lastIdx) + dk) << " * " << mus(lastIdx) + shifts(lastIdx) << " - " << dk << "\n";
 #endif
       RealScalar tmp = sqrt(prod);
-      zhat(k) = col0(k) > Literal(0) ? tmp : -tmp;
+#ifdef EIGEN_BDCSVD_SANITY_CHECKS
+      assert((numext::isfinite)(tmp));
+#endif
+      zhat(k) = col0(k) > Literal(0) ? RealScalar(tmp) : RealScalar(-tmp);
     }
   }
 }
@@ -987,7 +1108,7 @@
 // i >= 1, di almost null and zi non null.
 // We use a rotation to zero out zi applied to the left of M
 template <typename MatrixType>
-void BDCSVD<MatrixType>::deflation43(Index firstCol, Index shift, Index i, Index size)
+void BDCSVD<MatrixType>::deflation43(Eigen::Index firstCol, Eigen::Index shift, Eigen::Index i, Eigen::Index size)
 {
   using std::abs;
   using std::sqrt;
@@ -1016,7 +1137,7 @@
 // We apply two rotations to have zj = 0;
 // TODO deflation44 is still broken and not properly tested
 template <typename MatrixType>
-void BDCSVD<MatrixType>::deflation44(Index firstColu , Index firstColm, Index firstRowW, Index firstColW, Index i, Index j, Index size)
+void BDCSVD<MatrixType>::deflation44(Eigen::Index firstColu , Eigen::Index firstColm, Eigen::Index firstRowW, Eigen::Index firstColW, Eigen::Index i, Eigen::Index j, Eigen::Index size)
 {
   using std::abs;
   using std::sqrt;
@@ -1043,7 +1164,7 @@
   }
   c/=r;
   s/=r;
-  m_computed(firstColm + i, firstColm) = r;  
+  m_computed(firstColm + i, firstColm) = r;
   m_computed(firstColm + j, firstColm + j) = m_computed(firstColm + i, firstColm + i);
   m_computed(firstColm + j, firstColm) = Literal(0);
 
@@ -1056,7 +1177,7 @@
 
 // acts on block from (firstCol+shift, firstCol+shift) to (lastCol+shift, lastCol+shift) [inclusive]
 template <typename MatrixType>
-void BDCSVD<MatrixType>::deflation(Index firstCol, Index lastCol, Index k, Index firstRowW, Index firstColW, Index shift)
+void BDCSVD<MatrixType>::deflation(Eigen::Index firstCol, Eigen::Index lastCol, Eigen::Index k, Eigen::Index firstRowW, Eigen::Index firstColW, Eigen::Index shift)
 {
   using std::sqrt;
   using std::abs;
@@ -1117,6 +1238,7 @@
 #endif
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
   std::cout << "to be sorted: " << diag.transpose() << "\n\n";
+  std::cout << "            : " << col0.transpose() << "\n\n";
 #endif
   {
     // Check for total deflation
@@ -1207,7 +1329,7 @@
        if( (diag(i) - diag(i-1)) < NumTraits<RealScalar>::epsilon()*maxDiag )
       {
 #ifdef EIGEN_BDCSVD_DEBUG_VERBOSE
-        std::cout << "deflation 4.4 with i = " << i << " because " << (diag(i) - diag(i-1)) << " < " << NumTraits<RealScalar>::epsilon()*diag(i) << "\n";
+        std::cout << "deflation 4.4 with i = " << i << " because " << diag(i) << " - " << diag(i-1) << " == " << (diag(i) - diag(i-1)) << " < " << NumTraits<RealScalar>::epsilon()*/*diag(i)*/maxDiag << "\n";
 #endif
         eigen_internal_assert(abs(diag(i) - diag(i-1))<epsilon_coarse && " diagonal entries are not properly sorted");
         deflation44(firstCol, firstCol + shift, firstRowW, firstColW, i-1, i, length);
@@ -1226,7 +1348,6 @@
 #endif
 }//end deflation
 
-#ifndef __CUDACC__
 /** \svd_module
   *
   * \return the singular value decomposition of \c *this computed by Divide & Conquer algorithm
@@ -1239,7 +1360,6 @@
 {
   return BDCSVD<PlainObject>(*this, computationOptions);
 }
-#endif
 
 } // end namespace Eigen
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
index 43488b1..9d95acd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/JacobiSVD.h
@@ -112,12 +112,12 @@
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    TrOptions = RowsAtCompileTime==1 ? (MatrixType::Options & ~(RowMajor))
-              : ColsAtCompileTime==1 ? (MatrixType::Options |   RowMajor)
-              : MatrixType::Options
+    Options = MatrixType::Options
   };
-  typedef Matrix<Scalar, ColsAtCompileTime, RowsAtCompileTime, TrOptions, MaxColsAtCompileTime, MaxRowsAtCompileTime>
-          TransposeTypeWithSameStorageOrder;
+
+  typedef typename internal::make_proper_matrix_type<
+    Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime
+  >::type TransposeTypeWithSameStorageOrder;
 
   void allocate(const JacobiSVD<MatrixType, FullPivHouseholderQRPreconditioner>& svd)
   {
@@ -202,13 +202,12 @@
     ColsAtCompileTime = MatrixType::ColsAtCompileTime,
     MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime,
     MaxColsAtCompileTime = MatrixType::MaxColsAtCompileTime,
-    TrOptions = RowsAtCompileTime==1 ? (MatrixType::Options & ~(RowMajor))
-              : ColsAtCompileTime==1 ? (MatrixType::Options |   RowMajor)
-              : MatrixType::Options
+    Options = MatrixType::Options
   };
 
-  typedef Matrix<Scalar, ColsAtCompileTime, RowsAtCompileTime, TrOptions, MaxColsAtCompileTime, MaxRowsAtCompileTime>
-          TransposeTypeWithSameStorageOrder;
+  typedef typename internal::make_proper_matrix_type<
+    Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime
+  >::type TransposeTypeWithSameStorageOrder;
 
   void allocate(const JacobiSVD<MatrixType, ColPivHouseholderQRPreconditioner>& svd)
   {
@@ -303,8 +302,9 @@
     Options = MatrixType::Options
   };
 
-  typedef Matrix<Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime>
-          TransposeTypeWithSameStorageOrder;
+  typedef typename internal::make_proper_matrix_type<
+    Scalar, ColsAtCompileTime, RowsAtCompileTime, Options, MaxColsAtCompileTime, MaxRowsAtCompileTime
+  >::type TransposeTypeWithSameStorageOrder;
 
   void allocate(const JacobiSVD<MatrixType, HouseholderQRPreconditioner>& svd)
   {
@@ -425,6 +425,7 @@
 
 template<typename _MatrixType, int QRPreconditioner> 
 struct traits<JacobiSVD<_MatrixType,QRPreconditioner> >
+        : traits<_MatrixType>
 {
   typedef _MatrixType MatrixType;
 };
@@ -584,6 +585,7 @@
     using Base::m_matrixU;
     using Base::m_matrixV;
     using Base::m_singularValues;
+    using Base::m_info;
     using Base::m_isInitialized;
     using Base::m_isAllocated;
     using Base::m_usePrescribedThreshold;
@@ -610,7 +612,7 @@
 };
 
 template<typename MatrixType, int QRPreconditioner>
-void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Index rows, Index cols, unsigned int computationOptions)
+void JacobiSVD<MatrixType, QRPreconditioner>::allocate(Eigen::Index rows, Eigen::Index cols, unsigned int computationOptions)
 {
   eigen_assert(rows >= 0 && cols >= 0);
 
@@ -624,6 +626,7 @@
 
   m_rows = rows;
   m_cols = cols;
+  m_info = Success;
   m_isInitialized = false;
   m_isAllocated = true;
   m_computationOptions = computationOptions;
@@ -673,7 +676,12 @@
   const RealScalar considerAsZero = (std::numeric_limits<RealScalar>::min)();
 
   // Scaling factor to reduce over/under-flows
-  RealScalar scale = matrix.cwiseAbs().maxCoeff();
+  RealScalar scale = matrix.cwiseAbs().template maxCoeff<PropagateNaN>();
+  if (!(numext::isfinite)(scale)) {
+    m_isInitialized = true;
+    m_info = InvalidInput;
+    return *this;
+  }
   if(scale==RealScalar(0)) scale = RealScalar(1);
   
   /*** step 1. The R-SVD step: we use a QR decomposition to reduce to the case of a square matrix */
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
index 3d1ef37..bc7ab88 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/SVDBase.h
@@ -17,6 +17,18 @@
 #define EIGEN_SVDBASE_H
 
 namespace Eigen {
+
+namespace internal {
+template<typename Derived> struct traits<SVDBase<Derived> >
+ : traits<Derived>
+{
+  typedef MatrixXpr XprKind;
+  typedef SolverStorage StorageKind;
+  typedef int StorageIndex;
+  enum { Flags = 0 };
+};
+}
+
 /** \ingroup SVD_Module
  *
  *
@@ -39,20 +51,26 @@
  * smaller value among \a n and \a p, there are only \a m singular vectors; the remaining columns of \a U and \a V do not correspond to actual
  * singular vectors. Asking for \em thin \a U or \a V means asking for only their \a m first columns to be formed. So \a U is then a n-by-m matrix,
  * and \a V is then a p-by-m matrix. Notice that thin \a U and \a V are all you need for (least squares) solving.
+ * 
+ * The status of the computation can be retrived using the \a info() method. Unless \a info() returns \a Success, the results should be not
+ * considered well defined.
  *  
- * If the input matrix has inf or nan coefficients, the result of the computation is undefined, but the computation is guaranteed to
+ * If the input matrix has inf or nan coefficients, the result of the computation is undefined, and \a info() will return \a InvalidInput, but the computation is guaranteed to
  * terminate in finite (and reasonable) time.
  * \sa class BDCSVD, class JacobiSVD
  */
-template<typename Derived>
-class SVDBase
+template<typename Derived> class SVDBase
+ : public SolverBase<SVDBase<Derived> >
 {
+public: 
+   
+  template<typename Derived_>
+  friend struct internal::solve_assertion;
 
-public:
   typedef typename internal::traits<Derived>::MatrixType MatrixType;
   typedef typename MatrixType::Scalar Scalar;
   typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-  typedef typename MatrixType::StorageIndex StorageIndex;
+  typedef typename Eigen::internal::traits<SVDBase>::StorageIndex StorageIndex;
   typedef Eigen::Index Index; ///< \deprecated since Eigen 3.3
   enum {
     RowsAtCompileTime = MatrixType::RowsAtCompileTime,
@@ -82,7 +100,7 @@
    */
   const MatrixUType& matrixU() const
   {
-    eigen_assert(m_isInitialized && "SVD is not initialized.");
+    _check_compute_assertions();
     eigen_assert(computeU() && "This SVD decomposition didn't compute U. Did you ask for it?");
     return m_matrixU;
   }
@@ -98,7 +116,7 @@
    */
   const MatrixVType& matrixV() const
   {
-    eigen_assert(m_isInitialized && "SVD is not initialized.");
+    _check_compute_assertions();
     eigen_assert(computeV() && "This SVD decomposition didn't compute V. Did you ask for it?");
     return m_matrixV;
   }
@@ -110,14 +128,14 @@
    */
   const SingularValuesType& singularValues() const
   {
-    eigen_assert(m_isInitialized && "SVD is not initialized.");
+    _check_compute_assertions();
     return m_singularValues;
   }
 
   /** \returns the number of singular values that are not exactly 0 */
   Index nonzeroSingularValues() const
   {
-    eigen_assert(m_isInitialized && "SVD is not initialized.");
+    _check_compute_assertions();
     return m_nonzeroSingularValues;
   }
   
@@ -130,7 +148,7 @@
   inline Index rank() const
   {
     using std::abs;
-    eigen_assert(m_isInitialized && "JacobiSVD is not initialized.");
+    _check_compute_assertions();
     if(m_singularValues.size()==0) return 0;
     RealScalar premultiplied_threshold = numext::maxi<RealScalar>(m_singularValues.coeff(0) * threshold(), (std::numeric_limits<RealScalar>::min)());
     Index i = m_nonzeroSingularValues-1;
@@ -183,7 +201,7 @@
     // this temporary is needed to workaround a MSVC issue
     Index diagSize = (std::max<Index>)(1,m_diagSize);
     return m_usePrescribedThreshold ? m_prescribedThreshold
-                                    : diagSize*NumTraits<Scalar>::epsilon();
+                                    : RealScalar(diagSize)*NumTraits<Scalar>::epsilon();
   }
 
   /** \returns true if \a U (full or thin) is asked for in this SVD decomposition */
@@ -194,6 +212,7 @@
   inline Index rows() const { return m_rows; }
   inline Index cols() const { return m_cols; }
   
+  #ifdef EIGEN_PARSED_BY_DOXYGEN
   /** \returns a (least squares) solution of \f$ A x = b \f$ using the current SVD decomposition of A.
     *
     * \param b the right-hand-side of the equation to solve.
@@ -205,32 +224,55 @@
     */
   template<typename Rhs>
   inline const Solve<Derived, Rhs>
-  solve(const MatrixBase<Rhs>& b) const
+  solve(const MatrixBase<Rhs>& b) const;
+  #endif
+
+
+  /** \brief Reports whether previous computation was successful.
+   *
+   * \returns \c Success if computation was successful.
+   */
+  EIGEN_DEVICE_FUNC
+  ComputationInfo info() const
   {
     eigen_assert(m_isInitialized && "SVD is not initialized.");
-    eigen_assert(computeU() && computeV() && "SVD::solve() requires both unitaries U and V to be computed (thin unitaries suffice).");
-    return Solve<Derived, Rhs>(derived(), b.derived());
+    return m_info;
   }
-  
+
   #ifndef EIGEN_PARSED_BY_DOXYGEN
   template<typename RhsType, typename DstType>
-  EIGEN_DEVICE_FUNC
   void _solve_impl(const RhsType &rhs, DstType &dst) const;
+
+  template<bool Conjugate, typename RhsType, typename DstType>
+  void _solve_impl_transposed(const RhsType &rhs, DstType &dst) const;
   #endif
 
 protected:
-  
+
   static void check_template_parameters()
   {
     EIGEN_STATIC_ASSERT_NON_INTEGER(Scalar);
   }
-  
+
+  void _check_compute_assertions() const {
+    eigen_assert(m_isInitialized && "SVD is not initialized.");
+  }
+
+  template<bool Transpose_, typename Rhs>
+  void _check_solve_assertion(const Rhs& b) const {
+      EIGEN_ONLY_USED_FOR_DEBUG(b);
+      _check_compute_assertions();
+      eigen_assert(computeU() && computeV() && "SVDBase::solve(): Both unitaries U and V are required to be computed (thin unitaries suffice).");
+      eigen_assert((Transpose_?cols():rows())==b.rows() && "SVDBase::solve(): invalid number of rows of the right hand side matrix b");
+  }
+
   // return true if already allocated
   bool allocate(Index rows, Index cols, unsigned int computationOptions) ;
 
   MatrixUType m_matrixU;
   MatrixVType m_matrixV;
   SingularValuesType m_singularValues;
+  ComputationInfo m_info;
   bool m_isInitialized, m_isAllocated, m_usePrescribedThreshold;
   bool m_computeFullU, m_computeThinU;
   bool m_computeFullV, m_computeThinV;
@@ -243,9 +285,14 @@
    * Default constructor of SVDBase
    */
   SVDBase()
-    : m_isInitialized(false),
+    : m_info(Success),
+      m_isInitialized(false),
       m_isAllocated(false),
       m_usePrescribedThreshold(false),
+      m_computeFullU(false),
+      m_computeThinU(false),
+      m_computeFullV(false),
+      m_computeThinV(false),
       m_computationOptions(0),
       m_rows(-1), m_cols(-1), m_diagSize(0)
   {
@@ -260,17 +307,30 @@
 template<typename RhsType, typename DstType>
 void SVDBase<Derived>::_solve_impl(const RhsType &rhs, DstType &dst) const
 {
-  eigen_assert(rhs.rows() == rows());
-
   // A = U S V^*
   // So A^{-1} = V S^{-1} U^*
 
-  Matrix<Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime, RhsType::MaxColsAtCompileTime> tmp;
+  Matrix<typename RhsType::Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime, RhsType::MaxColsAtCompileTime> tmp;
   Index l_rank = rank();
   tmp.noalias() =  m_matrixU.leftCols(l_rank).adjoint() * rhs;
   tmp = m_singularValues.head(l_rank).asDiagonal().inverse() * tmp;
   dst = m_matrixV.leftCols(l_rank) * tmp;
 }
+
+template<typename Derived>
+template<bool Conjugate, typename RhsType, typename DstType>
+void SVDBase<Derived>::_solve_impl_transposed(const RhsType &rhs, DstType &dst) const
+{
+  // A = U S V^*
+  // So  A^{-*} = U S^{-1} V^*
+  // And A^{-T} = U_conj S^{-1} V^T
+  Matrix<typename RhsType::Scalar, Dynamic, RhsType::ColsAtCompileTime, 0, MatrixType::MaxRowsAtCompileTime, RhsType::MaxColsAtCompileTime> tmp;
+  Index l_rank = rank();
+
+  tmp.noalias() =  m_matrixV.leftCols(l_rank).transpose().template conjugateIf<Conjugate>() * rhs;
+  tmp = m_singularValues.head(l_rank).asDiagonal().inverse() * tmp;
+  dst = m_matrixU.template conjugateIf<!Conjugate>().leftCols(l_rank) * tmp;
+}
 #endif
 
 template<typename MatrixType>
@@ -288,6 +348,7 @@
 
   m_rows = rows;
   m_cols = cols;
+  m_info = Success;
   m_isInitialized = false;
   m_isAllocated = true;
   m_computationOptions = computationOptions;
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
index 11ac847..997defc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/SVD/UpperBidiagonalization.h
@@ -127,7 +127,7 @@
        .makeHouseholderInPlace(mat.coeffRef(k,k+1), upper_diagonal[k]);
     // apply householder transform to remaining part of mat on the left
     mat.bottomRightCorner(remainingRows-1, remainingCols)
-       .applyHouseholderOnTheRight(mat.row(k).tail(remainingCols-1).transpose(), mat.coeff(k,k+1), tempData);
+       .applyHouseholderOnTheRight(mat.row(k).tail(remainingCols-1).adjoint(), mat.coeff(k,k+1), tempData);
   }
 }
 
@@ -202,7 +202,7 @@
       {
         SubColumnType y_k( Y.col(k).tail(remainingCols) );
         
-        // let's use the begining of column k of Y as a temporary vector
+        // let's use the beginning of column k of Y as a temporary vector
         SubColumnType tmp( Y.col(k).head(k) );
         y_k.noalias()  = A.block(k,k+1, remainingRows,remainingCols).adjoint() * v_k; // bottleneck
         tmp.noalias()  = V_k1.adjoint()  * v_k;
@@ -231,7 +231,7 @@
       {
         SubColumnType x_k ( X.col(k).tail(remainingRows-1) );
         
-        // let's use the begining of column k of X as a temporary vectors
+        // let's use the beginning of column k of X as a temporary vectors
         // note that tmp0 and tmp1 overlaps
         SubColumnType tmp0 ( X.col(k).head(k) ),
                       tmp1 ( X.col(k).head(k+1) );
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
index cf1fedf..6d47e75 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdDeque.h
@@ -36,7 +36,7 @@
     deque(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : deque_base(first, last, a) {} \
     deque(const deque& c) : deque_base(c) {}  \
     explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
-    deque(iterator start, iterator end) : deque_base(start, end) {}  \
+    deque(iterator start_, iterator end_) : deque_base(start_, end_) {}  \
     deque& operator=(const deque& x) {  \
       deque_base::operator=(x);  \
       return *this;  \
@@ -62,7 +62,7 @@
     : deque_base(first, last, a) {} \
     deque(const deque& c) : deque_base(c) {}  \
     explicit deque(size_type num, const value_type& val = value_type()) : deque_base(num, val) {} \
-    deque(iterator start, iterator end) : deque_base(start, end) {}  \
+    deque(iterator start_, iterator end_) : deque_base(start_, end_) {}  \
     deque& operator=(const deque& x) {  \
       deque_base::operator=(x);  \
       return *this;  \
@@ -98,17 +98,7 @@
   { return deque_base::insert(position,x); }
   void insert(const_iterator position, size_type new_size, const value_type& x)
   { deque_base::insert(position, new_size, x); }
-#elif defined(_GLIBCXX_DEQUE) && EIGEN_GNUC_AT_LEAST(4,2)
-  // workaround GCC std::deque implementation
-  void resize(size_type new_size, const value_type& x)
-  {
-    if (new_size < deque_base::size())
-      deque_base::_M_erase_at_end(this->_M_impl._M_start + new_size);
-    else
-      deque_base::insert(deque_base::end(), new_size - deque_base::size(), x);
-  }
 #else
-  // either GCC 4.1 or non-GCC
   // default implementation which should always work.
   void resize(size_type new_size, const value_type& x)
   {
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
index e1eba49..8ba3fad 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdList.h
@@ -35,7 +35,7 @@
     list(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : list_base(first, last, a) {} \
     list(const list& c) : list_base(c) {}  \
     explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
-    list(iterator start, iterator end) : list_base(start, end) {}  \
+    list(iterator start_, iterator end_) : list_base(start_, end_) {}  \
     list& operator=(const list& x) {  \
       list_base::operator=(x);  \
       return *this;  \
@@ -62,7 +62,7 @@
     : list_base(first, last, a) {} \
     list(const list& c) : list_base(c) {}  \
     explicit list(size_type num, const value_type& val = value_type()) : list_base(num, val) {} \
-    list(iterator start, iterator end) : list_base(start, end) {}  \
+    list(iterator start_, iterator end_) : list_base(start_, end_) {}  \
     list& operator=(const list& x) {  \
     list_base::operator=(x);  \
     return *this; \
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
index ec22821..9fcf19b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/StlSupport/StdVector.h
@@ -36,7 +36,7 @@
     vector(InputIterator first, InputIterator last, const allocator_type& a = allocator_type()) : vector_base(first, last, a) {} \
     vector(const vector& c) : vector_base(c) {}  \
     explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
-    vector(iterator start, iterator end) : vector_base(start, end) {}  \
+    vector(iterator start_, iterator end_) : vector_base(start_, end_) {}  \
     vector& operator=(const vector& x) {  \
       vector_base::operator=(x);  \
       return *this;  \
@@ -62,7 +62,7 @@
     : vector_base(first, last, a) {} \
     vector(const vector& c) : vector_base(c) {}  \
     explicit vector(size_type num, const value_type& val = value_type()) : vector_base(num, val) {} \
-    vector(iterator start, iterator end) : vector_base(start, end) {}  \
+    vector(iterator start_, iterator end_) : vector_base(start_, end_) {}  \
     vector& operator=(const vector& x) {  \
       vector_base::operator=(x);  \
       return *this;  \
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
index 1f8a531..0e5d544 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseBinaryOps.h
@@ -75,6 +75,32 @@
   return (max)(Derived::PlainObject::Constant(rows(), cols(), other));
 }
 
+/** \returns an expression of the coefficient-wise absdiff of \c *this and \a other
+  *
+  * Example: \include Cwise_absolute_difference.cpp
+  * Output: \verbinclude Cwise_absolute_difference.out
+  *
+  * \sa absolute_difference()
+  */
+EIGEN_MAKE_CWISE_BINARY_OP(absolute_difference,absolute_difference)
+
+/** \returns an expression of the coefficient-wise absolute_difference of \c *this and scalar \a other
+  *
+  * \sa absolute_difference()
+  */
+EIGEN_DEVICE_FUNC
+EIGEN_STRONG_INLINE const CwiseBinaryOp<internal::scalar_absolute_difference_op<Scalar,Scalar>, const Derived,
+                                        const CwiseNullaryOp<internal::scalar_constant_op<Scalar>, PlainObject> >
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+absolute_difference
+#else
+(absolute_difference)
+#endif
+(const Scalar &other) const
+{
+  return (absolute_difference)(Derived::PlainObject::Constant(rows(), cols(), other));
+}
+
 /** \returns an expression of the coefficient-wise power of \c *this to the given array of \a exponents.
   *
   * This function computes the coefficient-wise power.
@@ -119,7 +145,7 @@
   return this->OP(Derived::PlainObject::Constant(rows(), cols(), s)); \
 } \
 EIGEN_DEVICE_FUNC friend EIGEN_STRONG_INLINE const RCmp ## COMPARATOR ## ReturnType \
-OP(const Scalar& s, const Derived& d) { \
+OP(const Scalar& s, const EIGEN_CURRENT_STORAGE_BASE_CLASS<Derived>& d) { \
   return Derived::PlainObject::Constant(d.rows(), d.cols(), s).OP(d); \
 }
 
@@ -314,9 +340,9 @@
   *
   * It returns the Riemann zeta function of two arguments \c *this and \a q:
   *
-  * \param *this is the exposent, it must be > 1
   * \param q is the shift, it must be > 0
   *
+  * \note *this is the exponent, it must be > 1.
   * \note This function supports only float and double scalar types. To support other scalar types, the user has
   * to provide implementations of zeta(T,T) for any scalar type T to be supported.
   *
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
index ebaa3f1..13c55f4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ArrayCwiseUnaryOps.h
@@ -10,9 +10,11 @@
 typedef CwiseUnaryOp<internal::scalar_boolean_not_op<Scalar>, const Derived> BooleanNotReturnType;
 
 typedef CwiseUnaryOp<internal::scalar_exp_op<Scalar>, const Derived> ExpReturnType;
+typedef CwiseUnaryOp<internal::scalar_expm1_op<Scalar>, const Derived> Expm1ReturnType;
 typedef CwiseUnaryOp<internal::scalar_log_op<Scalar>, const Derived> LogReturnType;
 typedef CwiseUnaryOp<internal::scalar_log1p_op<Scalar>, const Derived> Log1pReturnType;
 typedef CwiseUnaryOp<internal::scalar_log10_op<Scalar>, const Derived> Log10ReturnType;
+typedef CwiseUnaryOp<internal::scalar_log2_op<Scalar>, const Derived> Log2ReturnType;
 typedef CwiseUnaryOp<internal::scalar_cos_op<Scalar>, const Derived> CosReturnType;
 typedef CwiseUnaryOp<internal::scalar_sin_op<Scalar>, const Derived> SinReturnType;
 typedef CwiseUnaryOp<internal::scalar_tan_op<Scalar>, const Derived> TanReturnType;
@@ -20,11 +22,18 @@
 typedef CwiseUnaryOp<internal::scalar_asin_op<Scalar>, const Derived> AsinReturnType;
 typedef CwiseUnaryOp<internal::scalar_atan_op<Scalar>, const Derived> AtanReturnType;
 typedef CwiseUnaryOp<internal::scalar_tanh_op<Scalar>, const Derived> TanhReturnType;
+typedef CwiseUnaryOp<internal::scalar_logistic_op<Scalar>, const Derived> LogisticReturnType;
 typedef CwiseUnaryOp<internal::scalar_sinh_op<Scalar>, const Derived> SinhReturnType;
+#if EIGEN_HAS_CXX11_MATH
+typedef CwiseUnaryOp<internal::scalar_atanh_op<Scalar>, const Derived> AtanhReturnType;
+typedef CwiseUnaryOp<internal::scalar_asinh_op<Scalar>, const Derived> AsinhReturnType;
+typedef CwiseUnaryOp<internal::scalar_acosh_op<Scalar>, const Derived> AcoshReturnType;
+#endif
 typedef CwiseUnaryOp<internal::scalar_cosh_op<Scalar>, const Derived> CoshReturnType;
 typedef CwiseUnaryOp<internal::scalar_square_op<Scalar>, const Derived> SquareReturnType;
 typedef CwiseUnaryOp<internal::scalar_cube_op<Scalar>, const Derived> CubeReturnType;
 typedef CwiseUnaryOp<internal::scalar_round_op<Scalar>, const Derived> RoundReturnType;
+typedef CwiseUnaryOp<internal::scalar_rint_op<Scalar>, const Derived> RintReturnType;
 typedef CwiseUnaryOp<internal::scalar_floor_op<Scalar>, const Derived> FloorReturnType;
 typedef CwiseUnaryOp<internal::scalar_ceil_op<Scalar>, const Derived> CeilReturnType;
 typedef CwiseUnaryOp<internal::scalar_isnan_op<Scalar>, const Derived> IsNaNReturnType;
@@ -90,6 +99,20 @@
   return ExpReturnType(derived());
 }
 
+/** \returns an expression of the coefficient-wise exponential of *this minus 1.
+  *
+  * In exact arithmetic, \c x.expm1() is equivalent to \c x.exp() - 1,
+  * however, with finite precision, this function is much more accurate when \c x is close to zero.
+  *
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_expm1">Math functions</a>, exp()
+  */
+EIGEN_DEVICE_FUNC
+inline const Expm1ReturnType
+expm1() const
+{
+  return Expm1ReturnType(derived());
+}
+
 /** \returns an expression of the coefficient-wise logarithm of *this.
   *
   * This function computes the coefficient-wise logarithm. The function MatrixBase::log() in the
@@ -98,7 +121,7 @@
   * Example: \include Cwise_log.cpp
   * Output: \verbinclude Cwise_log.out
   *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log">Math functions</a>, exp()
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_log">Math functions</a>, log()
   */
 EIGEN_DEVICE_FUNC
 inline const LogReturnType
@@ -137,6 +160,18 @@
   return Log10ReturnType(derived());
 }
 
+/** \returns an expression of the coefficient-wise base-2 logarithm of *this.
+  *
+  * This function computes the coefficient-wise base-2 logarithm.
+  *
+  */
+EIGEN_DEVICE_FUNC
+inline const Log2ReturnType
+log2() const
+{
+  return Log2ReturnType(derived());
+}
+
 /** \returns an expression of the coefficient-wise square root of *this.
   *
   * This function computes the coefficient-wise square root. The function MatrixBase::sqrt() in the
@@ -311,7 +346,7 @@
   * Example: \include Cwise_cosh.cpp
   * Output: \verbinclude Cwise_cosh.out
   *
-  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cosh">Math functions</a>, tan(), sinh(), cosh()
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_cosh">Math functions</a>, tanh(), sinh(), cosh()
   */
 EIGEN_DEVICE_FUNC
 inline const CoshReturnType
@@ -320,6 +355,50 @@
   return CoshReturnType(derived());
 }
 
+#if EIGEN_HAS_CXX11_MATH
+/** \returns an expression of the coefficient-wise inverse hyperbolic tan of *this.
+  *
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_atanh">Math functions</a>, atanh(), asinh(), acosh()
+  */
+EIGEN_DEVICE_FUNC
+inline const AtanhReturnType
+atanh() const
+{
+  return AtanhReturnType(derived());
+}
+
+/** \returns an expression of the coefficient-wise inverse hyperbolic sin of *this.
+  *
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_asinh">Math functions</a>, atanh(), asinh(), acosh()
+  */
+EIGEN_DEVICE_FUNC
+inline const AsinhReturnType
+asinh() const
+{
+  return AsinhReturnType(derived());
+}
+
+/** \returns an expression of the coefficient-wise inverse hyperbolic cos of *this.
+  *
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_acosh">Math functions</a>, atanh(), asinh(), acosh()
+  */
+EIGEN_DEVICE_FUNC
+inline const AcoshReturnType
+acosh() const
+{
+  return AcoshReturnType(derived());
+}
+#endif
+
+/** \returns an expression of the coefficient-wise logistic of *this.
+  */
+EIGEN_DEVICE_FUNC
+inline const LogisticReturnType
+logistic() const
+{
+  return LogisticReturnType(derived());
+}
+
 /** \returns an expression of the coefficient-wise inverse of *this.
   *
   * Example: \include Cwise_inverse.cpp
@@ -362,6 +441,20 @@
   return CubeReturnType(derived());
 }
 
+/** \returns an expression of the coefficient-wise rint of *this.
+  *
+  * Example: \include Cwise_rint.cpp
+  * Output: \verbinclude Cwise_rint.out
+  *
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_rint">Math functions</a>, ceil(), floor()
+  */
+EIGEN_DEVICE_FUNC
+inline const RintReturnType
+rint() const
+{
+  return RintReturnType(derived());
+}
+
 /** \returns an expression of the coefficient-wise round of *this.
   *
   * Example: \include Cwise_round.cpp
@@ -404,6 +497,45 @@
   return CeilReturnType(derived());
 }
 
+template<int N> struct ShiftRightXpr {
+  typedef CwiseUnaryOp<internal::scalar_shift_right_op<Scalar, N>, const Derived> Type;
+};
+
+/** \returns an expression of \c *this with the \a Scalar type arithmetically
+  * shifted right by \a N bit positions.
+  *
+  * The template parameter \a N specifies the number of bit positions to shift.
+  * 
+  * \sa shiftLeft()
+  */
+template<int N>
+EIGEN_DEVICE_FUNC
+typename ShiftRightXpr<N>::Type
+shiftRight() const
+{
+  return typename ShiftRightXpr<N>::Type(derived());
+}
+
+
+template<int N> struct ShiftLeftXpr {
+  typedef CwiseUnaryOp<internal::scalar_shift_left_op<Scalar, N>, const Derived> Type;
+};
+
+/** \returns an expression of \c *this with the \a Scalar type logically
+  * shifted left by \a N bit positions.
+  *
+  * The template parameter \a N specifies the number of bit positions to shift.
+  *
+  * \sa shiftRight()
+  */
+template<int N>
+EIGEN_DEVICE_FUNC
+typename ShiftLeftXpr<N>::Type
+shiftLeft() const
+{
+  return typename ShiftLeftXpr<N>::Type(derived());
+}
+
 /** \returns an expression of the coefficient-wise isnan of *this.
   *
   * Example: \include Cwise_isNaN.cpp
@@ -471,14 +603,12 @@
 typedef CwiseUnaryOp<internal::scalar_digamma_op<Scalar>, const Derived> DigammaReturnType;
 typedef CwiseUnaryOp<internal::scalar_erf_op<Scalar>, const Derived> ErfReturnType;
 typedef CwiseUnaryOp<internal::scalar_erfc_op<Scalar>, const Derived> ErfcReturnType;
+typedef CwiseUnaryOp<internal::scalar_ndtri_op<Scalar>, const Derived> NdtriReturnType;
 
 /** \cpp11 \returns an expression of the coefficient-wise ln(|gamma(*this)|).
   *
   * \specialfunctions_module
   *
-  * Example: \include Cwise_lgamma.cpp
-  * Output: \verbinclude Cwise_lgamma.out
-  *
   * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
   * or float/double in non c++11 mode, the user has to provide implementations of lgamma(T) for any scalar
   * type T to be supported.
@@ -514,9 +644,6 @@
   *
   * \specialfunctions_module
   *
-  * Example: \include Cwise_erf.cpp
-  * Output: \verbinclude Cwise_erf.out
-  *
   * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
   * or float/double in non c++11 mode, the user has to provide implementations of erf(T) for any scalar
   * type T to be supported.
@@ -535,9 +662,6 @@
   *
   * \specialfunctions_module
   *
-  * Example: \include Cwise_erfc.cpp
-  * Output: \verbinclude Cwise_erfc.out
-  *
   * \note This function supports only float and double scalar types in c++11 mode. To support other scalar types,
   * or float/double in non c++11 mode, the user has to provide implementations of erfc(T) for any scalar
   * type T to be supported.
@@ -550,3 +674,23 @@
 {
   return ErfcReturnType(derived());
 }
+
+/** \returns an expression of the coefficient-wise inverse of the CDF of the Normal distribution function
+  * function of *this.
+  *
+  * \specialfunctions_module
+  * 
+  * In other words, considering `x = ndtri(y)`, it returns the argument, x, for which the area under the
+  * Gaussian probability density function (integrated from minus infinity to x) is equal to y.
+  *
+  * \note This function supports only float and double scalar types. To support other scalar types,
+  * the user has to provide implementations of ndtri(T) for any scalar type T to be supported.
+  *
+  * \sa <a href="group__CoeffwiseMathFunctions.html#cwisetable_ndtri">Math functions</a>
+  */
+EIGEN_DEVICE_FUNC
+inline const NdtriReturnType
+ndtri() const
+{
+  return NdtriReturnType(derived());
+}
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
index ac35a00..63a52a6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/BlockMethods.h
@@ -40,68 +40,126 @@
 template<int Size> struct FixedSegmentReturnType { typedef VectorBlock<Derived, Size> Type; };
 template<int Size> struct ConstFixedSegmentReturnType { typedef const VectorBlock<const Derived, Size> Type; };
 
+/// \internal inner-vector
+typedef Block<Derived,IsRowMajor?1:Dynamic,IsRowMajor?Dynamic:1,true>       InnerVectorReturnType;
+typedef Block<const Derived,IsRowMajor?1:Dynamic,IsRowMajor?Dynamic:1,true> ConstInnerVectorReturnType;
+
+/// \internal set of inner-vectors
+typedef Block<Derived,Dynamic,Dynamic,true> InnerVectorsReturnType;
+typedef Block<const Derived,Dynamic,Dynamic,true> ConstInnerVectorsReturnType;
+
 #endif // not EIGEN_PARSED_BY_DOXYGEN
 
-/// \returns a dynamic-size expression of a block in *this.
+/// \returns an expression of a block in \c *this with either dynamic or fixed sizes.
 ///
-/// \param startRow the first row in the block
-/// \param startCol the first column in the block
-/// \param blockRows the number of rows in the block
-/// \param blockCols the number of columns in the block
+/// \param  startRow  the first row in the block
+/// \param  startCol  the first column in the block
+/// \param  blockRows number of rows in the block, specified at either run-time or compile-time
+/// \param  blockCols number of columns in the block, specified at either run-time or compile-time
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
-/// Example: \include MatrixBase_block_int_int_int_int.cpp
+/// Example using runtime (aka dynamic) sizes: \include MatrixBase_block_int_int_int_int.cpp
 /// Output: \verbinclude MatrixBase_block_int_int_int_int.out
 ///
-/// \note Even though the returned expression has dynamic size, in the case
+/// \newin{3.4}:
+///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. In the later case, \c n plays the role of a runtime fallback value in case \c N equals Eigen::Dynamic.
+/// Here is an example with a fixed number of rows \c NRows and dynamic number of columns \c cols:
+/// \code
+/// mat.block(i,j,fix<NRows>,cols)
+/// \endcode
+///
+/// This function thus fully covers the features offered by the following overloads block<NRows,NCols>(Index, Index),
+/// and block<NRows,NCols>(Index, Index, Index, Index) that are thus obsolete. Indeed, this generic version avoids
+/// redundancy, it preserves the argument order, and prevents the need to rely on the template keyword in templated code.
+///
+/// but with less redundancy and more consistency as it does not modify the argument order
+/// and seamlessly enable hybrid fixed/dynamic sizes.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
 /// when it is applied to a fixed-size matrix, it inherits a fixed maximal size,
 /// which means that evaluating it does not cause a dynamic memory allocation.
 ///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index)
+/// \sa class Block, fix, fix<N>(int)
 ///
-EIGEN_DEVICE_FUNC
-inline BlockXpr block(Index startRow, Index startCol, Index blockRows, Index blockCols)
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename FixedBlockXpr<...,...>::Type
+#endif
+block(Index startRow, Index startCol, NRowsType blockRows, NColsType blockCols)
 {
-  return BlockXpr(derived(), startRow, startCol, blockRows, blockCols);
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type(
+            derived(), startRow, startCol, internal::get_runtime_value(blockRows), internal::get_runtime_value(blockCols));
 }
 
-/// This is the const version of block(Index,Index,Index,Index). */
-EIGEN_DEVICE_FUNC
-inline const ConstBlockXpr block(Index startRow, Index startCol, Index blockRows, Index blockCols) const
+/// This is the const version of block(Index,Index,NRowsType,NColsType)
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstFixedBlockXpr<...,...>::Type
+#endif
+block(Index startRow, Index startCol, NRowsType blockRows, NColsType blockCols) const
 {
-  return ConstBlockXpr(derived(), startRow, startCol, blockRows, blockCols);
+  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type(
+            derived(), startRow, startCol, internal::get_runtime_value(blockRows), internal::get_runtime_value(blockCols));
 }
 
 
 
-
-/// \returns a dynamic-size expression of a top-right corner of *this.
+/// \returns a expression of a top-right corner of \c *this with either dynamic or fixed sizes.
 ///
 /// \param cRows the number of rows in the corner
 /// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
-/// Example: \include MatrixBase_topRightCorner_int_int.cpp
+/// Example with dynamic sizes: \include MatrixBase_topRightCorner_int_int.cpp
 /// Output: \verbinclude MatrixBase_topRightCorner_int_int.out
 ///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline BlockXpr topRightCorner(Index cRows, Index cCols)
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename FixedBlockXpr<...,...>::Type
+#endif
+topRightCorner(NRowsType cRows, NColsType cCols)
 {
-  return BlockXpr(derived(), 0, cols() - cCols, cRows, cCols);
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, cols() - internal::get_runtime_value(cCols), internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// This is the const version of topRightCorner(Index, Index).
-EIGEN_DEVICE_FUNC
-inline const ConstBlockXpr topRightCorner(Index cRows, Index cCols) const
+/// This is the const version of topRightCorner(NRowsType, NColsType).
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstFixedBlockXpr<...,...>::Type
+#endif
+topRightCorner(NRowsType cRows, NColsType cCols) const
 {
-  return ConstBlockXpr(derived(), 0, cols() - cCols, cRows, cCols);
+  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, cols() - internal::get_runtime_value(cCols), internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// \returns an expression of a fixed-size top-right corner of *this.
+/// \returns an expression of a fixed-size top-right corner of \c *this.
 ///
 /// \tparam CRows the number of rows in the corner
 /// \tparam CCols the number of columns in the corner
@@ -114,21 +172,21 @@
 /// \sa class Block, block<int,int>(Index,Index)
 ///
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline typename FixedBlockXpr<CRows,CCols>::Type topRightCorner()
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type topRightCorner()
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - CCols);
 }
 
 /// This is the const version of topRightCorner<int, int>().
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type topRightCorner() const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type topRightCorner() const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - CCols);
 }
 
-/// \returns an expression of a top-right corner of *this.
+/// \returns an expression of a top-right corner of \c *this.
 ///
 /// \tparam CRows number of rows in corner as specified at compile-time
 /// \tparam CCols number of columns in corner as specified at compile-time
@@ -148,46 +206,67 @@
 /// \sa class Block
 ///
 template<int CRows, int CCols>
-inline typename FixedBlockXpr<CRows,CCols>::Type topRightCorner(Index cRows, Index cCols)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type topRightCorner(Index cRows, Index cCols)
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - cCols, cRows, cCols);
 }
 
 /// This is the const version of topRightCorner<int, int>(Index, Index).
 template<int CRows, int CCols>
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type topRightCorner(Index cRows, Index cCols) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type topRightCorner(Index cRows, Index cCols) const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, cols() - cCols, cRows, cCols);
 }
 
 
 
-/// \returns a dynamic-size expression of a top-left corner of *this.
+/// \returns an expression of a top-left corner of \c *this  with either dynamic or fixed sizes.
 ///
 /// \param cRows the number of rows in the corner
 /// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
 /// Example: \include MatrixBase_topLeftCorner_int_int.cpp
 /// Output: \verbinclude MatrixBase_topLeftCorner_int_int.out
 ///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline BlockXpr topLeftCorner(Index cRows, Index cCols)
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename FixedBlockXpr<...,...>::Type
+#endif
+topLeftCorner(NRowsType cRows, NColsType cCols)
 {
-  return BlockXpr(derived(), 0, 0, cRows, cCols);
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, 0, internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
 /// This is the const version of topLeftCorner(Index, Index).
-EIGEN_DEVICE_FUNC
-inline const ConstBlockXpr topLeftCorner(Index cRows, Index cCols) const
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstFixedBlockXpr<...,...>::Type
+#endif
+topLeftCorner(NRowsType cRows, NColsType cCols) const
 {
-  return ConstBlockXpr(derived(), 0, 0, cRows, cCols);
+  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, 0, internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// \returns an expression of a fixed-size top-left corner of *this.
+/// \returns an expression of a fixed-size top-left corner of \c *this.
 ///
 /// The template parameters CRows and CCols are the number of rows and columns in the corner.
 ///
@@ -196,24 +275,24 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline typename FixedBlockXpr<CRows,CCols>::Type topLeftCorner()
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type topLeftCorner()
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0);
 }
 
 /// This is the const version of topLeftCorner<int, int>().
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type topLeftCorner() const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type topLeftCorner() const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0);
 }
 
-/// \returns an expression of a top-left corner of *this.
+/// \returns an expression of a top-left corner of \c *this.
 ///
 /// \tparam CRows number of rows in corner as specified at compile-time
 /// \tparam CCols number of columns in corner as specified at compile-time
@@ -233,46 +312,69 @@
 /// \sa class Block
 ///
 template<int CRows, int CCols>
-inline typename FixedBlockXpr<CRows,CCols>::Type topLeftCorner(Index cRows, Index cCols)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type topLeftCorner(Index cRows, Index cCols)
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0, cRows, cCols);
 }
 
 /// This is the const version of topLeftCorner<int, int>(Index, Index).
 template<int CRows, int CCols>
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type topLeftCorner(Index cRows, Index cCols) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type topLeftCorner(Index cRows, Index cCols) const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), 0, 0, cRows, cCols);
 }
 
 
 
-/// \returns a dynamic-size expression of a bottom-right corner of *this.
+/// \returns an expression of a bottom-right corner of \c *this  with either dynamic or fixed sizes.
 ///
 /// \param cRows the number of rows in the corner
 /// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
 /// Example: \include MatrixBase_bottomRightCorner_int_int.cpp
 /// Output: \verbinclude MatrixBase_bottomRightCorner_int_int.out
 ///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline BlockXpr bottomRightCorner(Index cRows, Index cCols)
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename FixedBlockXpr<...,...>::Type
+#endif
+bottomRightCorner(NRowsType cRows, NColsType cCols)
 {
-  return BlockXpr(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), rows() - internal::get_runtime_value(cRows), cols() - internal::get_runtime_value(cCols),
+                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// This is the const version of bottomRightCorner(Index, Index).
-EIGEN_DEVICE_FUNC
-inline const ConstBlockXpr bottomRightCorner(Index cRows, Index cCols) const
+/// This is the const version of bottomRightCorner(NRowsType, NColsType).
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstFixedBlockXpr<...,...>::Type
+#endif
+bottomRightCorner(NRowsType cRows, NColsType cCols) const
 {
-  return ConstBlockXpr(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
+  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), rows() - internal::get_runtime_value(cRows), cols() - internal::get_runtime_value(cCols),
+                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// \returns an expression of a fixed-size bottom-right corner of *this.
+/// \returns an expression of a fixed-size bottom-right corner of \c *this.
 ///
 /// The template parameters CRows and CCols are the number of rows and columns in the corner.
 ///
@@ -281,24 +383,24 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline typename FixedBlockXpr<CRows,CCols>::Type bottomRightCorner()
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type bottomRightCorner()
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, cols() - CCols);
 }
 
 /// This is the const version of bottomRightCorner<int, int>().
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomRightCorner() const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomRightCorner() const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, cols() - CCols);
 }
 
-/// \returns an expression of a bottom-right corner of *this.
+/// \returns an expression of a bottom-right corner of \c *this.
 ///
 /// \tparam CRows number of rows in corner as specified at compile-time
 /// \tparam CCols number of columns in corner as specified at compile-time
@@ -318,46 +420,69 @@
 /// \sa class Block
 ///
 template<int CRows, int CCols>
-inline typename FixedBlockXpr<CRows,CCols>::Type bottomRightCorner(Index cRows, Index cCols)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type bottomRightCorner(Index cRows, Index cCols)
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
 }
 
 /// This is the const version of bottomRightCorner<int, int>(Index, Index).
 template<int CRows, int CCols>
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomRightCorner(Index cRows, Index cCols) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomRightCorner(Index cRows, Index cCols) const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, cols() - cCols, cRows, cCols);
 }
 
 
 
-/// \returns a dynamic-size expression of a bottom-left corner of *this.
+/// \returns an expression of a bottom-left corner of \c *this  with either dynamic or fixed sizes.
 ///
 /// \param cRows the number of rows in the corner
 /// \param cCols the number of columns in the corner
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
 /// Example: \include MatrixBase_bottomLeftCorner_int_int.cpp
 /// Output: \verbinclude MatrixBase_bottomLeftCorner_int_int.out
 ///
+/// The number of rows \a blockRows and columns \a blockCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline BlockXpr bottomLeftCorner(Index cRows, Index cCols)
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename FixedBlockXpr<...,...>::Type
+#endif
+bottomLeftCorner(NRowsType cRows, NColsType cCols)
 {
-  return BlockXpr(derived(), rows() - cRows, 0, cRows, cCols);
+  return typename FixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), rows() - internal::get_runtime_value(cRows), 0,
+                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// This is the const version of bottomLeftCorner(Index, Index).
-EIGEN_DEVICE_FUNC
-inline const ConstBlockXpr bottomLeftCorner(Index cRows, Index cCols) const
+/// This is the const version of bottomLeftCorner(NRowsType, NColsType).
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename ConstFixedBlockXpr<...,...>::Type
+#endif
+bottomLeftCorner(NRowsType cRows, NColsType cCols) const
 {
-  return ConstBlockXpr(derived(), rows() - cRows, 0, cRows, cCols);
+  return typename ConstFixedBlockXpr<internal::get_fixed_value<NRowsType>::value,internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), rows() - internal::get_runtime_value(cRows), 0,
+                        internal::get_runtime_value(cRows), internal::get_runtime_value(cCols));
 }
 
-/// \returns an expression of a fixed-size bottom-left corner of *this.
+/// \returns an expression of a fixed-size bottom-left corner of \c *this.
 ///
 /// The template parameters CRows and CCols are the number of rows and columns in the corner.
 ///
@@ -366,24 +491,24 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline typename FixedBlockXpr<CRows,CCols>::Type bottomLeftCorner()
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type bottomLeftCorner()
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, 0);
 }
 
 /// This is the const version of bottomLeftCorner<int, int>().
 template<int CRows, int CCols>
-EIGEN_DEVICE_FUNC
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomLeftCorner() const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomLeftCorner() const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - CRows, 0);
 }
 
-/// \returns an expression of a bottom-left corner of *this.
+/// \returns an expression of a bottom-left corner of \c *this.
 ///
 /// \tparam CRows number of rows in corner as specified at compile-time
 /// \tparam CCols number of columns in corner as specified at compile-time
@@ -403,45 +528,66 @@
 /// \sa class Block
 ///
 template<int CRows, int CCols>
-inline typename FixedBlockXpr<CRows,CCols>::Type bottomLeftCorner(Index cRows, Index cCols)
+EIGEN_STRONG_INLINE
+typename FixedBlockXpr<CRows,CCols>::Type bottomLeftCorner(Index cRows, Index cCols)
 {
   return typename FixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, 0, cRows, cCols);
 }
 
 /// This is the const version of bottomLeftCorner<int, int>(Index, Index).
 template<int CRows, int CCols>
-inline const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomLeftCorner(Index cRows, Index cCols) const
+EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<CRows,CCols>::Type bottomLeftCorner(Index cRows, Index cCols) const
 {
   return typename ConstFixedBlockXpr<CRows,CCols>::Type(derived(), rows() - cRows, 0, cRows, cCols);
 }
 
 
 
-/// \returns a block consisting of the top rows of *this.
+/// \returns a block consisting of the top rows of \c *this.
 ///
 /// \param n the number of rows in the block
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
 ///
 /// Example: \include MatrixBase_topRows_int.cpp
 /// Output: \verbinclude MatrixBase_topRows_int.out
 ///
+/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline RowsBlockXpr topRows(Index n)
+template<typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+typename NRowsBlockXpr<...>::Type
+#endif
+topRows(NRowsType n)
 {
-  return RowsBlockXpr(derived(), 0, 0, n, cols());
+  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+            (derived(), 0, 0, internal::get_runtime_value(n), cols());
 }
 
-/// This is the const version of topRows(Index).
-EIGEN_DEVICE_FUNC
-inline ConstRowsBlockXpr topRows(Index n) const
+/// This is the const version of topRows(NRowsType).
+template<typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+const typename ConstNRowsBlockXpr<...>::Type
+#endif
+topRows(NRowsType n) const
 {
-  return ConstRowsBlockXpr(derived(), 0, 0, n, cols());
+  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+            (derived(), 0, 0, internal::get_runtime_value(n), cols());
 }
 
-/// \returns a block consisting of the top rows of *this.
+/// \returns a block consisting of the top rows of \c *this.
 ///
 /// \tparam N the number of rows in the block as specified at compile-time
 /// \param n the number of rows in the block as specified at run-time
@@ -454,50 +600,69 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename NRowsBlockXpr<N>::Type topRows(Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename NRowsBlockXpr<N>::Type topRows(Index n = N)
 {
   return typename NRowsBlockXpr<N>::Type(derived(), 0, 0, n, cols());
 }
 
 /// This is the const version of topRows<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstNRowsBlockXpr<N>::Type topRows(Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstNRowsBlockXpr<N>::Type topRows(Index n = N) const
 {
   return typename ConstNRowsBlockXpr<N>::Type(derived(), 0, 0, n, cols());
 }
 
 
 
-/// \returns a block consisting of the bottom rows of *this.
+/// \returns a block consisting of the bottom rows of \c *this.
 ///
 /// \param n the number of rows in the block
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
 ///
 /// Example: \include MatrixBase_bottomRows_int.cpp
 /// Output: \verbinclude MatrixBase_bottomRows_int.out
 ///
+/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline RowsBlockXpr bottomRows(Index n)
+template<typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+typename NRowsBlockXpr<...>::Type
+#endif
+bottomRows(NRowsType n)
 {
-  return RowsBlockXpr(derived(), rows() - n, 0, n, cols());
+  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+            (derived(), rows() - internal::get_runtime_value(n), 0, internal::get_runtime_value(n), cols());
 }
 
-/// This is the const version of bottomRows(Index).
-EIGEN_DEVICE_FUNC
-inline ConstRowsBlockXpr bottomRows(Index n) const
+/// This is the const version of bottomRows(NRowsType).
+template<typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+const typename ConstNRowsBlockXpr<...>::Type
+#endif
+bottomRows(NRowsType n) const
 {
-  return ConstRowsBlockXpr(derived(), rows() - n, 0, n, cols());
+  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+            (derived(), rows() - internal::get_runtime_value(n), 0, internal::get_runtime_value(n), cols());
 }
 
-/// \returns a block consisting of the bottom rows of *this.
+/// \returns a block consisting of the bottom rows of \c *this.
 ///
 /// \tparam N the number of rows in the block as specified at compile-time
 /// \param n the number of rows in the block as specified at run-time
@@ -510,51 +675,70 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename NRowsBlockXpr<N>::Type bottomRows(Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename NRowsBlockXpr<N>::Type bottomRows(Index n = N)
 {
   return typename NRowsBlockXpr<N>::Type(derived(), rows() - n, 0, n, cols());
 }
 
 /// This is the const version of bottomRows<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstNRowsBlockXpr<N>::Type bottomRows(Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstNRowsBlockXpr<N>::Type bottomRows(Index n = N) const
 {
   return typename ConstNRowsBlockXpr<N>::Type(derived(), rows() - n, 0, n, cols());
 }
 
 
 
-/// \returns a block consisting of a range of rows of *this.
+/// \returns a block consisting of a range of rows of \c *this.
 ///
 /// \param startRow the index of the first row in the block
 /// \param n the number of rows in the block
+/// \tparam NRowsType the type of the value handling the number of rows in the block, typically Index.
 ///
 /// Example: \include DenseBase_middleRows_int.cpp
 /// Output: \verbinclude DenseBase_middleRows_int.out
 ///
+/// The number of rows \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline RowsBlockXpr middleRows(Index startRow, Index n)
+template<typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+typename NRowsBlockXpr<...>::Type
+#endif
+middleRows(Index startRow, NRowsType n)
 {
-  return RowsBlockXpr(derived(), startRow, 0, n, cols());
+  return typename NRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+            (derived(), startRow, 0, internal::get_runtime_value(n), cols());
 }
 
-/// This is the const version of middleRows(Index,Index).
-EIGEN_DEVICE_FUNC
-inline ConstRowsBlockXpr middleRows(Index startRow, Index n) const
+/// This is the const version of middleRows(Index,NRowsType).
+template<typename NRowsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+#else
+const typename ConstNRowsBlockXpr<...>::Type
+#endif
+middleRows(Index startRow, NRowsType n) const
 {
-  return ConstRowsBlockXpr(derived(), startRow, 0, n, cols());
+  return typename ConstNRowsBlockXpr<internal::get_fixed_value<NRowsType>::value>::Type
+            (derived(), startRow, 0, internal::get_runtime_value(n), cols());
 }
 
-/// \returns a block consisting of a range of rows of *this.
+/// \returns a block consisting of a range of rows of \c *this.
 ///
 /// \tparam N the number of rows in the block as specified at compile-time
 /// \param startRow the index of the first row in the block
@@ -568,50 +752,69 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename NRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename NRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N)
 {
   return typename NRowsBlockXpr<N>::Type(derived(), startRow, 0, n, cols());
 }
 
 /// This is the const version of middleRows<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstNRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstNRowsBlockXpr<N>::Type middleRows(Index startRow, Index n = N) const
 {
   return typename ConstNRowsBlockXpr<N>::Type(derived(), startRow, 0, n, cols());
 }
 
 
 
-/// \returns a block consisting of the left columns of *this.
+/// \returns a block consisting of the left columns of \c *this.
 ///
 /// \param n the number of columns in the block
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
 /// Example: \include MatrixBase_leftCols_int.cpp
 /// Output: \verbinclude MatrixBase_leftCols_int.out
 ///
+/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline ColsBlockXpr leftCols(Index n)
+template<typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename NColsBlockXpr<...>::Type
+#endif
+leftCols(NColsType n)
 {
-  return ColsBlockXpr(derived(), 0, 0, rows(), n);
+  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, 0, rows(), internal::get_runtime_value(n));
 }
 
-/// This is the const version of leftCols(Index).
-EIGEN_DEVICE_FUNC
-inline ConstColsBlockXpr leftCols(Index n) const
+/// This is the const version of leftCols(NColsType).
+template<typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstNColsBlockXpr<...>::Type
+#endif
+leftCols(NColsType n) const
 {
-  return ConstColsBlockXpr(derived(), 0, 0, rows(), n);
+  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, 0, rows(), internal::get_runtime_value(n));
 }
 
-/// \returns a block consisting of the left columns of *this.
+/// \returns a block consisting of the left columns of \c *this.
 ///
 /// \tparam N the number of columns in the block as specified at compile-time
 /// \param n the number of columns in the block as specified at run-time
@@ -624,50 +827,69 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename NColsBlockXpr<N>::Type leftCols(Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename NColsBlockXpr<N>::Type leftCols(Index n = N)
 {
   return typename NColsBlockXpr<N>::Type(derived(), 0, 0, rows(), n);
 }
 
 /// This is the const version of leftCols<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstNColsBlockXpr<N>::Type leftCols(Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstNColsBlockXpr<N>::Type leftCols(Index n = N) const
 {
   return typename ConstNColsBlockXpr<N>::Type(derived(), 0, 0, rows(), n);
 }
 
 
 
-/// \returns a block consisting of the right columns of *this.
+/// \returns a block consisting of the right columns of \c *this.
 ///
 /// \param n the number of columns in the block
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
 /// Example: \include MatrixBase_rightCols_int.cpp
 /// Output: \verbinclude MatrixBase_rightCols_int.out
 ///
+/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline ColsBlockXpr rightCols(Index n)
+template<typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename NColsBlockXpr<...>::Type
+#endif
+rightCols(NColsType n)
 {
-  return ColsBlockXpr(derived(), 0, cols() - n, rows(), n);
+  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, cols() - internal::get_runtime_value(n), rows(), internal::get_runtime_value(n));
 }
 
-/// This is the const version of rightCols(Index).
-EIGEN_DEVICE_FUNC
-inline ConstColsBlockXpr rightCols(Index n) const
+/// This is the const version of rightCols(NColsType).
+template<typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstNColsBlockXpr<...>::Type
+#endif
+rightCols(NColsType n) const
 {
-  return ConstColsBlockXpr(derived(), 0, cols() - n, rows(), n);
+  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, cols() - internal::get_runtime_value(n), rows(), internal::get_runtime_value(n));
 }
 
-/// \returns a block consisting of the right columns of *this.
+/// \returns a block consisting of the right columns of \c *this.
 ///
 /// \tparam N the number of columns in the block as specified at compile-time
 /// \param n the number of columns in the block as specified at run-time
@@ -680,51 +902,70 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename NColsBlockXpr<N>::Type rightCols(Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename NColsBlockXpr<N>::Type rightCols(Index n = N)
 {
   return typename NColsBlockXpr<N>::Type(derived(), 0, cols() - n, rows(), n);
 }
 
 /// This is the const version of rightCols<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstNColsBlockXpr<N>::Type rightCols(Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstNColsBlockXpr<N>::Type rightCols(Index n = N) const
 {
   return typename ConstNColsBlockXpr<N>::Type(derived(), 0, cols() - n, rows(), n);
 }
 
 
 
-/// \returns a block consisting of a range of columns of *this.
+/// \returns a block consisting of a range of columns of \c *this.
 ///
 /// \param startCol the index of the first column in the block
 /// \param numCols the number of columns in the block
+/// \tparam NColsType the type of the value handling the number of columns in the block, typically Index.
 ///
 /// Example: \include DenseBase_middleCols_int.cpp
 /// Output: \verbinclude DenseBase_middleCols_int.out
 ///
+/// The number of columns \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline ColsBlockXpr middleCols(Index startCol, Index numCols)
+template<typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+typename NColsBlockXpr<...>::Type
+#endif
+middleCols(Index startCol, NColsType numCols)
 {
-  return ColsBlockXpr(derived(), 0, startCol, rows(), numCols);
+  return typename NColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, startCol, rows(), internal::get_runtime_value(numCols));
 }
 
-/// This is the const version of middleCols(Index,Index).
-EIGEN_DEVICE_FUNC
-inline ConstColsBlockXpr middleCols(Index startCol, Index numCols) const
+/// This is the const version of middleCols(Index,NColsType).
+template<typename NColsType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+#else
+const typename ConstNColsBlockXpr<...>::Type
+#endif
+middleCols(Index startCol, NColsType numCols) const
 {
-  return ConstColsBlockXpr(derived(), 0, startCol, rows(), numCols);
+  return typename ConstNColsBlockXpr<internal::get_fixed_value<NColsType>::value>::Type
+            (derived(), 0, startCol, rows(), internal::get_runtime_value(numCols));
 }
 
-/// \returns a block consisting of a range of columns of *this.
+/// \returns a block consisting of a range of columns of \c *this.
 ///
 /// \tparam N the number of columns in the block as specified at compile-time
 /// \param startCol the index of the first column in the block
@@ -738,26 +979,26 @@
 ///
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename NColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename NColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N)
 {
   return typename NColsBlockXpr<N>::Type(derived(), 0, startCol, rows(), n);
 }
 
 /// This is the const version of middleCols<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstNColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstNColsBlockXpr<N>::Type middleCols(Index startCol, Index n = N) const
 {
   return typename ConstNColsBlockXpr<N>::Type(derived(), 0, startCol, rows(), n);
 }
 
 
 
-/// \returns a fixed-size expression of a block in *this.
+/// \returns a fixed-size expression of a block of \c *this.
 ///
 /// The template parameters \a NRows and \a NCols are the number of
 /// rows and columns in the block.
@@ -768,29 +1009,35 @@
 /// Example: \include MatrixBase_block_int_int.cpp
 /// Output: \verbinclude MatrixBase_block_int_int.out
 ///
+/// \note The usage of of this overload is discouraged from %Eigen 3.4, better used the generic
+/// block(Index,Index,NRowsType,NColsType), here is the one-to-one equivalence:
+/// \code
+/// mat.template block<NRows,NCols>(i,j)  <-->  mat.block(i,j,fix<NRows>,fix<NCols>)
+/// \endcode
+///
 /// \note since block is a templated member, the keyword template has to be used
 /// if the matrix type is also a template parameter: \code m.template block<3,3>(1,1); \endcode
 ///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int NRows, int NCols>
-EIGEN_DEVICE_FUNC
-inline typename FixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol)
 {
   return typename FixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol);
 }
 
 /// This is the const version of block<>(Index, Index). */
 template<int NRows, int NCols>
-EIGEN_DEVICE_FUNC
-inline const typename ConstFixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol) const
 {
   return typename ConstFixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol);
 }
 
-/// \returns an expression of a block in *this.
+/// \returns an expression of a block of \c *this.
 ///
 /// \tparam NRows number of rows in block as specified at compile-time
 /// \tparam NCols number of columns in block as specified at compile-time
@@ -805,14 +1052,25 @@
 /// \a NRows is \a Dynamic, and the same for the number of columns.
 ///
 /// Example: \include MatrixBase_template_int_int_block_int_int_int_int.cpp
-/// Output: \verbinclude MatrixBase_template_int_int_block_int_int_int_int.cpp
+/// Output: \verbinclude MatrixBase_template_int_int_block_int_int_int_int.out
+///
+/// \note The usage of of this overload is discouraged from %Eigen 3.4, better used the generic
+/// block(Index,Index,NRowsType,NColsType), here is the one-to-one complete equivalence:
+/// \code
+/// mat.template block<NRows,NCols>(i,j,rows,cols)     <-->  mat.block(i,j,fix<NRows>(rows),fix<NCols>(cols))
+/// \endcode
+/// If we known that, e.g., NRows==Dynamic and NCols!=Dynamic, then the equivalence becomes:
+/// \code
+/// mat.template block<Dynamic,NCols>(i,j,rows,NCols)  <-->  mat.block(i,j,rows,fix<NCols>)
+/// \endcode
 ///
 EIGEN_DOC_BLOCK_ADDONS_NOT_INNER_PANEL
 ///
-/// \sa class Block, block(Index,Index,Index,Index)
+/// \sa block(Index,Index,NRowsType,NColsType), class Block
 ///
 template<int NRows, int NCols>
-inline typename FixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol,
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol,
                                                   Index blockRows, Index blockCols)
 {
   return typename FixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol, blockRows, blockCols);
@@ -820,13 +1078,14 @@
 
 /// This is the const version of block<>(Index, Index, Index, Index).
 template<int NRows, int NCols>
-inline const typename ConstFixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol,
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const typename ConstFixedBlockXpr<NRows,NCols>::Type block(Index startRow, Index startCol,
                                                               Index blockRows, Index blockCols) const
 {
   return typename ConstFixedBlockXpr<NRows,NCols>::Type(derived(), startRow, startCol, blockRows, blockCols);
 }
 
-/// \returns an expression of the \a i-th column of *this. Note that the numbering starts at 0.
+/// \returns an expression of the \a i-th column of \c *this. Note that the numbering starts at 0.
 ///
 /// Example: \include MatrixBase_col.cpp
 /// Output: \verbinclude MatrixBase_col.out
@@ -834,20 +1093,20 @@
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(column-major)
 /**
   * \sa row(), class Block */
-EIGEN_DEVICE_FUNC
-inline ColXpr col(Index i)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+ColXpr col(Index i)
 {
   return ColXpr(derived(), i);
 }
 
 /// This is the const version of col().
-EIGEN_DEVICE_FUNC
-inline ConstColXpr col(Index i) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+ConstColXpr col(Index i) const
 {
   return ConstColXpr(derived(), i);
 }
 
-/// \returns an expression of the \a i-th row of *this. Note that the numbering starts at 0.
+/// \returns an expression of the \a i-th row of \c *this. Note that the numbering starts at 0.
 ///
 /// Example: \include MatrixBase_row.cpp
 /// Output: \verbinclude MatrixBase_row.out
@@ -855,109 +1114,166 @@
 EIGEN_DOC_BLOCK_ADDONS_INNER_PANEL_IF(row-major)
 /**
   * \sa col(), class Block */
-EIGEN_DEVICE_FUNC
-inline RowXpr row(Index i)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+RowXpr row(Index i)
 {
   return RowXpr(derived(), i);
 }
 
 /// This is the const version of row(). */
-EIGEN_DEVICE_FUNC
-inline ConstRowXpr row(Index i) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+ConstRowXpr row(Index i) const
 {
   return ConstRowXpr(derived(), i);
 }
 
-/// \returns a dynamic-size expression of a segment (i.e. a vector block) in *this.
+/// \returns an expression of a segment (i.e. a vector block) in \c *this with either dynamic or fixed sizes.
 ///
 /// \only_for_vectors
 ///
 /// \param start the first coefficient in the segment
 /// \param n the number of coefficients in the segment
+/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
 ///
 /// Example: \include MatrixBase_segment_int_int.cpp
 /// Output: \verbinclude MatrixBase_segment_int_int.out
 ///
-/// \note Even though the returned expression has dynamic size, in the case
+/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
 /// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
 /// which means that evaluating it does not cause a dynamic memory allocation.
 ///
-/// \sa class Block, segment(Index)
+/// \sa block(Index,Index,NRowsType,NColsType), fix<N>, fix<N>(int), class Block
 ///
-EIGEN_DEVICE_FUNC
-inline SegmentReturnType segment(Index start, Index n)
+template<typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+typename FixedSegmentReturnType<...>::Type
+#endif
+segment(Index start, NType n)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return SegmentReturnType(derived(), start, n);
+  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+            (derived(), start, internal::get_runtime_value(n));
 }
 
 
-/// This is the const version of segment(Index,Index).
-EIGEN_DEVICE_FUNC
-inline ConstSegmentReturnType segment(Index start, Index n) const
+/// This is the const version of segment(Index,NType).
+template<typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+const typename ConstFixedSegmentReturnType<...>::Type
+#endif
+segment(Index start, NType n) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return ConstSegmentReturnType(derived(), start, n);
+  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+            (derived(), start, internal::get_runtime_value(n));
 }
 
-/// \returns a dynamic-size expression of the first coefficients of *this.
+/// \returns an expression of the first coefficients of \c *this with either dynamic or fixed sizes.
 ///
 /// \only_for_vectors
 ///
 /// \param n the number of coefficients in the segment
+/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
 ///
 /// Example: \include MatrixBase_start_int.cpp
 /// Output: \verbinclude MatrixBase_start_int.out
 ///
-/// \note Even though the returned expression has dynamic size, in the case
+/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
 /// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
 /// which means that evaluating it does not cause a dynamic memory allocation.
 ///
 /// \sa class Block, block(Index,Index)
 ///
-EIGEN_DEVICE_FUNC
-inline SegmentReturnType head(Index n)
+template<typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+typename FixedSegmentReturnType<...>::Type
+#endif
+head(NType n)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return SegmentReturnType(derived(), 0, n);
+  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+              (derived(), 0, internal::get_runtime_value(n));
 }
 
-/// This is the const version of head(Index).
-EIGEN_DEVICE_FUNC
-inline ConstSegmentReturnType head(Index n) const
+/// This is the const version of head(NType).
+template<typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+const typename ConstFixedSegmentReturnType<...>::Type
+#endif
+head(NType n) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return ConstSegmentReturnType(derived(), 0, n);
+  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+            (derived(), 0, internal::get_runtime_value(n));
 }
 
-/// \returns a dynamic-size expression of the last coefficients of *this.
+/// \returns an expression of a last coefficients of \c *this with either dynamic or fixed sizes.
 ///
 /// \only_for_vectors
 ///
 /// \param n the number of coefficients in the segment
+/// \tparam NType the type of the value handling the number of coefficients in the segment, typically Index.
 ///
 /// Example: \include MatrixBase_end_int.cpp
 /// Output: \verbinclude MatrixBase_end_int.out
 ///
-/// \note Even though the returned expression has dynamic size, in the case
+/// The number of coefficients \a n can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments.
+/// See \link block(Index,Index,NRowsType,NColsType) block() \endlink for the details.
+///
+/// \note Even in the case that the returned expression has dynamic size, in the case
 /// when it is applied to a fixed-size vector, it inherits a fixed maximal size,
 /// which means that evaluating it does not cause a dynamic memory allocation.
 ///
 /// \sa class Block, block(Index,Index)
 ///
-EIGEN_DEVICE_FUNC
-inline SegmentReturnType tail(Index n)
+template<typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+typename FixedSegmentReturnType<...>::Type
+#endif
+tail(NType n)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return SegmentReturnType(derived(), this->size() - n, n);
+  return typename FixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+            (derived(), this->size() - internal::get_runtime_value(n), internal::get_runtime_value(n));
 }
 
 /// This is the const version of tail(Index).
-EIGEN_DEVICE_FUNC
-inline ConstSegmentReturnType tail(Index n) const
+template<typename NType>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+const typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+#else
+const typename ConstFixedSegmentReturnType<...>::Type
+#endif
+tail(NType n) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
-  return ConstSegmentReturnType(derived(), this->size() - n, n);
+  return typename ConstFixedSegmentReturnType<internal::get_fixed_value<NType>::value>::Type
+            (derived(), this->size() - internal::get_runtime_value(n), internal::get_runtime_value(n));
 }
 
 /// \returns a fixed-size expression of a segment (i.e. a vector block) in \c *this
@@ -974,11 +1290,11 @@
 /// Example: \include MatrixBase_template_int_segment.cpp
 /// Output: \verbinclude MatrixBase_template_int_segment.out
 ///
-/// \sa class Block
+/// \sa segment(Index,NType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename FixedSegmentReturnType<N>::Type segment(Index start, Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedSegmentReturnType<N>::Type segment(Index start, Index n = N)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return typename FixedSegmentReturnType<N>::Type(derived(), start, n);
@@ -986,14 +1302,14 @@
 
 /// This is the const version of segment<int>(Index).
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstFixedSegmentReturnType<N>::Type segment(Index start, Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstFixedSegmentReturnType<N>::Type segment(Index start, Index n = N) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return typename ConstFixedSegmentReturnType<N>::Type(derived(), start, n);
 }
 
-/// \returns a fixed-size expression of the first coefficients of *this.
+/// \returns a fixed-size expression of the first coefficients of \c *this.
 ///
 /// \only_for_vectors
 ///
@@ -1006,11 +1322,11 @@
 /// Example: \include MatrixBase_template_int_start.cpp
 /// Output: \verbinclude MatrixBase_template_int_start.out
 ///
-/// \sa class Block
+/// \sa head(NType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename FixedSegmentReturnType<N>::Type head(Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedSegmentReturnType<N>::Type head(Index n = N)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return typename FixedSegmentReturnType<N>::Type(derived(), 0, n);
@@ -1018,14 +1334,14 @@
 
 /// This is the const version of head<int>().
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstFixedSegmentReturnType<N>::Type head(Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstFixedSegmentReturnType<N>::Type head(Index n = N) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return typename ConstFixedSegmentReturnType<N>::Type(derived(), 0, n);
 }
 
-/// \returns a fixed-size expression of the last coefficients of *this.
+/// \returns a fixed-size expression of the last coefficients of \c *this.
 ///
 /// \only_for_vectors
 ///
@@ -1038,11 +1354,11 @@
 /// Example: \include MatrixBase_template_int_end.cpp
 /// Output: \verbinclude MatrixBase_template_int_end.out
 ///
-/// \sa class Block
+/// \sa tail(NType), class Block
 ///
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename FixedSegmentReturnType<N>::Type tail(Index n = N)
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename FixedSegmentReturnType<N>::Type tail(Index n = N)
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return typename FixedSegmentReturnType<N>::Type(derived(), size() - n);
@@ -1050,9 +1366,77 @@
 
 /// This is the const version of tail<int>.
 template<int N>
-EIGEN_DEVICE_FUNC
-inline typename ConstFixedSegmentReturnType<N>::Type tail(Index n = N) const
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename ConstFixedSegmentReturnType<N>::Type tail(Index n = N) const
 {
   EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
   return typename ConstFixedSegmentReturnType<N>::Type(derived(), size() - n);
 }
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major).
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+InnerVectorReturnType innerVector(Index outer)
+{ return InnerVectorReturnType(derived(), outer); }
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major). Read-only.
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const ConstInnerVectorReturnType innerVector(Index outer) const
+{ return ConstInnerVectorReturnType(derived(), outer); }
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major).
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+InnerVectorsReturnType
+innerVectors(Index outerStart, Index outerSize)
+{
+  return Block<Derived,Dynamic,Dynamic,true>(derived(),
+                                             IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
+                                             IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize);
+
+}
+
+/// \returns the \a outer -th column (resp. row) of the matrix \c *this if \c *this
+/// is col-major (resp. row-major). Read-only.
+///
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+const ConstInnerVectorsReturnType
+innerVectors(Index outerStart, Index outerSize) const
+{
+  return Block<const Derived,Dynamic,Dynamic,true>(derived(),
+                                                  IsRowMajor ? outerStart : 0, IsRowMajor ? 0 : outerStart,
+                                                  IsRowMajor ? outerSize : rows(), IsRowMajor ? cols() : outerSize);
+
+}
+
+/** \returns the i-th subvector (column or vector) according to the \c Direction
+  * \sa subVectors()
+  */
+template<DirectionType Direction>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename internal::conditional<Direction==Vertical,ColXpr,RowXpr>::type
+subVector(Index i)
+{
+  return typename internal::conditional<Direction==Vertical,ColXpr,RowXpr>::type(derived(),i);
+}
+
+/** This is the const version of subVector(Index) */
+template<DirectionType Direction>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE
+typename internal::conditional<Direction==Vertical,ConstColXpr,ConstRowXpr>::type
+subVector(Index i) const
+{
+  return typename internal::conditional<Direction==Vertical,ConstColXpr,ConstRowXpr>::type(derived(),i);
+}
+
+/** \returns the number of subvectors (rows or columns) in the direction \c Direction
+  * \sa subVector(Index)
+  */
+template<DirectionType Direction>
+EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE EIGEN_CONSTEXPR
+Index subVectors() const
+{ return (Direction==Vertical)?cols():rows(); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
index 89f4faa..5418dc4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/CommonCwiseUnaryOps.h
@@ -76,6 +76,20 @@
   return ConjugateReturnType(derived());
 }
 
+/// \returns an expression of the complex conjugate of \c *this if Cond==true, returns derived() otherwise.
+///
+EIGEN_DOC_UNARY_ADDONS(conjugate,complex conjugate)
+///
+/// \sa conjugate()
+template<bool Cond>
+EIGEN_DEVICE_FUNC
+inline typename internal::conditional<Cond,ConjugateReturnType,const Derived&>::type
+conjugateIf() const
+{
+  typedef typename internal::conditional<Cond,ConjugateReturnType,const Derived&>::type ReturnType;
+  return ReturnType(derived());
+}
+
 /// \returns a read-only expression of the real part of \c *this.
 ///
 EIGEN_DOC_UNARY_ADDONS(real,real part function)
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h
new file mode 100644
index 0000000..5bfb19a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/IndexedViewMethods.h
@@ -0,0 +1,262 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2017 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#if !defined(EIGEN_PARSED_BY_DOXYGEN)
+
+// This file is automatically included twice to generate const and non-const versions
+
+#ifndef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
+#define EIGEN_INDEXED_VIEW_METHOD_CONST const
+#define EIGEN_INDEXED_VIEW_METHOD_TYPE  ConstIndexedViewType
+#else
+#define EIGEN_INDEXED_VIEW_METHOD_CONST
+#define EIGEN_INDEXED_VIEW_METHOD_TYPE IndexedViewType
+#endif
+
+#ifndef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
+protected:
+
+// define some aliases to ease readability
+
+template<typename Indices>
+struct IvcRowType : public internal::IndexedViewCompatibleType<Indices,RowsAtCompileTime> {};
+
+template<typename Indices>
+struct IvcColType : public internal::IndexedViewCompatibleType<Indices,ColsAtCompileTime> {};
+
+template<typename Indices>
+struct IvcType : public internal::IndexedViewCompatibleType<Indices,SizeAtCompileTime> {};
+
+typedef typename internal::IndexedViewCompatibleType<Index,1>::type IvcIndex;
+
+template<typename Indices>
+typename IvcRowType<Indices>::type
+ivcRow(const Indices& indices) const {
+  return internal::makeIndexedViewCompatible(indices, internal::variable_if_dynamic<Index,RowsAtCompileTime>(derived().rows()),Specialized);
+}
+
+template<typename Indices>
+typename IvcColType<Indices>::type
+ivcCol(const Indices& indices) const {
+  return internal::makeIndexedViewCompatible(indices, internal::variable_if_dynamic<Index,ColsAtCompileTime>(derived().cols()),Specialized);
+}
+
+template<typename Indices>
+typename IvcColType<Indices>::type
+ivcSize(const Indices& indices) const {
+  return internal::makeIndexedViewCompatible(indices, internal::variable_if_dynamic<Index,SizeAtCompileTime>(derived().size()),Specialized);
+}
+
+public:
+
+#endif
+
+template<typename RowIndices, typename ColIndices>
+struct EIGEN_INDEXED_VIEW_METHOD_TYPE {
+  typedef IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,
+                      typename IvcRowType<RowIndices>::type,
+                      typename IvcColType<ColIndices>::type> type;
+};
+
+// This is the generic version
+
+template<typename RowIndices, typename ColIndices>
+typename internal::enable_if<internal::valid_indexed_view_overload<RowIndices,ColIndices>::value
+  && internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::ReturnAsIndexedView,
+  typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type >::type
+operator()(const RowIndices& rowIndices, const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  return typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type
+            (derived(), ivcRow(rowIndices), ivcCol(colIndices));
+}
+
+// The following overload returns a Block<> object
+
+template<typename RowIndices, typename ColIndices>
+typename internal::enable_if<internal::valid_indexed_view_overload<RowIndices,ColIndices>::value
+  && internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::ReturnAsBlock,
+  typename internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::BlockType>::type
+operator()(const RowIndices& rowIndices, const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  typedef typename internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::BlockType BlockType;
+  typename IvcRowType<RowIndices>::type actualRowIndices = ivcRow(rowIndices);
+  typename IvcColType<ColIndices>::type actualColIndices = ivcCol(colIndices);
+  return BlockType(derived(),
+                   internal::first(actualRowIndices),
+                   internal::first(actualColIndices),
+                   internal::size(actualRowIndices),
+                   internal::size(actualColIndices));
+}
+
+// The following overload returns a Scalar
+
+template<typename RowIndices, typename ColIndices>
+typename internal::enable_if<internal::valid_indexed_view_overload<RowIndices,ColIndices>::value
+  && internal::traits<typename EIGEN_INDEXED_VIEW_METHOD_TYPE<RowIndices,ColIndices>::type>::ReturnAsScalar,
+  CoeffReturnType >::type
+operator()(const RowIndices& rowIndices, const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  return Base::operator()(internal::eval_expr_given_size(rowIndices,rows()),internal::eval_expr_given_size(colIndices,cols()));
+}
+
+#if EIGEN_HAS_STATIC_ARRAY_TEMPLATE
+
+// The following three overloads are needed to handle raw Index[N] arrays.
+
+template<typename RowIndicesT, std::size_t RowIndicesN, typename ColIndices>
+IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN],typename IvcColType<ColIndices>::type>
+operator()(const RowIndicesT (&rowIndices)[RowIndicesN], const ColIndices& colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN],typename IvcColType<ColIndices>::type>
+                    (derived(), rowIndices, ivcCol(colIndices));
+}
+
+template<typename RowIndices, typename ColIndicesT, std::size_t ColIndicesN>
+IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcRowType<RowIndices>::type, const ColIndicesT (&)[ColIndicesN]>
+operator()(const RowIndices& rowIndices, const ColIndicesT (&colIndices)[ColIndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcRowType<RowIndices>::type,const ColIndicesT (&)[ColIndicesN]>
+                    (derived(), ivcRow(rowIndices), colIndices);
+}
+
+template<typename RowIndicesT, std::size_t RowIndicesN, typename ColIndicesT, std::size_t ColIndicesN>
+IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN], const ColIndicesT (&)[ColIndicesN]>
+operator()(const RowIndicesT (&rowIndices)[RowIndicesN], const ColIndicesT (&colIndices)[ColIndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const RowIndicesT (&)[RowIndicesN],const ColIndicesT (&)[ColIndicesN]>
+                    (derived(), rowIndices, colIndices);
+}
+
+#endif // EIGEN_HAS_STATIC_ARRAY_TEMPLATE
+
+// Overloads for 1D vectors/arrays
+
+template<typename Indices>
+typename internal::enable_if<
+  IsRowMajor && (!(internal::get_compile_time_incr<typename IvcType<Indices>::type>::value==1 || internal::is_valid_index_type<Indices>::value)),
+  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,typename IvcType<Indices>::type> >::type
+operator()(const Indices& indices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,typename IvcType<Indices>::type>
+            (derived(), IvcIndex(0), ivcCol(indices));
+}
+
+template<typename Indices>
+typename internal::enable_if<
+  (!IsRowMajor) && (!(internal::get_compile_time_incr<typename IvcType<Indices>::type>::value==1 || internal::is_valid_index_type<Indices>::value)),
+  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcType<Indices>::type,IvcIndex> >::type
+operator()(const Indices& indices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,typename IvcType<Indices>::type,IvcIndex>
+            (derived(), ivcRow(indices), IvcIndex(0));
+}
+
+template<typename Indices>
+typename internal::enable_if<
+  (internal::get_compile_time_incr<typename IvcType<Indices>::type>::value==1) && (!internal::is_valid_index_type<Indices>::value) && (!symbolic::is_symbolic<Indices>::value),
+  VectorBlock<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,internal::array_size<Indices>::value> >::type
+operator()(const Indices& indices) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  typename IvcType<Indices>::type actualIndices = ivcSize(indices);
+  return VectorBlock<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,internal::array_size<Indices>::value>
+            (derived(), internal::first(actualIndices), internal::size(actualIndices));
+}
+
+template<typename IndexType>
+typename internal::enable_if<symbolic::is_symbolic<IndexType>::value, CoeffReturnType >::type
+operator()(const IndexType& id) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  return Base::operator()(internal::eval_expr_given_size(id,size()));
+}
+
+#if EIGEN_HAS_STATIC_ARRAY_TEMPLATE
+
+template<typename IndicesT, std::size_t IndicesN>
+typename internal::enable_if<IsRowMajor,
+  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,const IndicesT (&)[IndicesN]> >::type
+operator()(const IndicesT (&indices)[IndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,IvcIndex,const IndicesT (&)[IndicesN]>
+            (derived(), IvcIndex(0), indices);
+}
+
+template<typename IndicesT, std::size_t IndicesN>
+typename internal::enable_if<!IsRowMajor,
+  IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const IndicesT (&)[IndicesN],IvcIndex> >::type
+operator()(const IndicesT (&indices)[IndicesN]) EIGEN_INDEXED_VIEW_METHOD_CONST
+{
+  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Derived)
+  return IndexedView<EIGEN_INDEXED_VIEW_METHOD_CONST Derived,const IndicesT (&)[IndicesN],IvcIndex>
+            (derived(), indices, IvcIndex(0));
+}
+
+#endif // EIGEN_HAS_STATIC_ARRAY_TEMPLATE
+
+#undef EIGEN_INDEXED_VIEW_METHOD_CONST
+#undef EIGEN_INDEXED_VIEW_METHOD_TYPE
+
+#ifndef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
+#define EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
+#include "IndexedViewMethods.h"
+#undef EIGEN_INDEXED_VIEW_METHOD_2ND_PASS
+#endif
+
+#else // EIGEN_PARSED_BY_DOXYGEN
+
+/**
+  * \returns a generic submatrix view defined by the rows and columns indexed \a rowIndices and \a colIndices respectively.
+  *
+  * Each parameter must either be:
+  *  - An integer indexing a single row or column
+  *  - Eigen::all indexing the full set of respective rows or columns in increasing order
+  *  - An ArithmeticSequence as returned by the Eigen::seq and Eigen::seqN functions
+  *  - Any %Eigen's vector/array of integers or expressions
+  *  - Plain C arrays: \c int[N]
+  *  - And more generally any type exposing the following two member functions:
+  * \code
+  * <integral type> operator[](<integral type>) const;
+  * <integral type> size() const;
+  * \endcode
+  * where \c <integral \c type>  stands for any integer type compatible with Eigen::Index (i.e. \c std::ptrdiff_t).
+  *
+  * The last statement implies compatibility with \c std::vector, \c std::valarray, \c std::array, many of the Range-v3's ranges, etc.
+  *
+  * If the submatrix can be represented using a starting position \c (i,j) and positive sizes \c (rows,columns), then this
+  * method will returns a Block object after extraction of the relevant information from the passed arguments. This is the case
+  * when all arguments are either:
+  *  - An integer
+  *  - Eigen::all
+  *  - An ArithmeticSequence with compile-time increment strictly equal to 1, as returned by Eigen::seq(a,b), and Eigen::seqN(a,N).
+  *
+  * Otherwise a more general IndexedView<Derived,RowIndices',ColIndices'> object will be returned, after conversion of the inputs
+  * to more suitable types \c RowIndices' and \c ColIndices'.
+  *
+  * For 1D vectors and arrays, you better use the operator()(const Indices&) overload, which behave the same way but taking a single parameter.
+  *
+  * See also this <a href="https://stackoverflow.com/questions/46110917/eigen-replicate-items-along-one-dimension-without-useless-allocations">question</a> and its answer for an example of how to duplicate coefficients.
+  *
+  * \sa operator()(const Indices&), class Block, class IndexedView, DenseBase::block(Index,Index,Index,Index)
+  */
+template<typename RowIndices, typename ColIndices>
+IndexedView_or_Block
+operator()(const RowIndices& rowIndices, const ColIndices& colIndices);
+
+/** This is an overload of operator()(const RowIndices&, const ColIndices&) for 1D vectors or arrays
+  *
+  * \only_for_vectors
+  */
+template<typename Indices>
+IndexedView_or_VectorBlock
+operator()(const Indices& indices);
+
+#endif  // EIGEN_PARSED_BY_DOXYGEN
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
index f1084ab..a0feef8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseBinaryOps.h
@@ -39,10 +39,10 @@
   */
 template<typename OtherDerived>
 EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<std::equal_to<Scalar>, const Derived, const OtherDerived>
+inline const CwiseBinaryOp<numext::equal_to<Scalar>, const Derived, const OtherDerived>
 cwiseEqual(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
 {
-  return CwiseBinaryOp<std::equal_to<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
+  return CwiseBinaryOp<numext::equal_to<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
 }
 
 /** \returns an expression of the coefficient-wise != operator of *this and \a other
@@ -59,10 +59,10 @@
   */
 template<typename OtherDerived>
 EIGEN_DEVICE_FUNC
-inline const CwiseBinaryOp<std::not_equal_to<Scalar>, const Derived, const OtherDerived>
+inline const CwiseBinaryOp<numext::not_equal_to<Scalar>, const Derived, const OtherDerived>
 cwiseNotEqual(const EIGEN_CURRENT_STORAGE_BASE_CLASS<OtherDerived> &other) const
 {
-  return CwiseBinaryOp<std::not_equal_to<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
+  return CwiseBinaryOp<numext::not_equal_to<Scalar>, const Derived, const OtherDerived>(derived(), other.derived());
 }
 
 /** \returns an expression of the coefficient-wise min of *this and \a other
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
index b1be3d5..0514d8f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/MatrixCwiseUnaryOps.h
@@ -14,6 +14,7 @@
 
 typedef CwiseUnaryOp<internal::scalar_abs_op<Scalar>, const Derived> CwiseAbsReturnType;
 typedef CwiseUnaryOp<internal::scalar_abs2_op<Scalar>, const Derived> CwiseAbs2ReturnType;
+typedef CwiseUnaryOp<internal::scalar_arg_op<Scalar>, const Derived> CwiseArgReturnType;
 typedef CwiseUnaryOp<internal::scalar_sqrt_op<Scalar>, const Derived> CwiseSqrtReturnType;
 typedef CwiseUnaryOp<internal::scalar_sign_op<Scalar>, const Derived> CwiseSignReturnType;
 typedef CwiseUnaryOp<internal::scalar_inverse_op<Scalar>, const Derived> CwiseInverseReturnType;
@@ -82,4 +83,13 @@
 inline const CwiseInverseReturnType
 cwiseInverse() const { return CwiseInverseReturnType(derived()); }
 
+/// \returns an expression of the coefficient-wise phase angle of \c *this
+///
+/// Example: \include MatrixBase_cwiseArg.cpp
+/// Output: \verbinclude MatrixBase_cwiseArg.out
+///
+EIGEN_DOC_UNARY_ADDONS(cwiseArg,arg)
 
+EIGEN_DEVICE_FUNC
+inline const CwiseArgReturnType
+cwiseArg() const { return CwiseArgReturnType(derived()); }
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h
new file mode 100644
index 0000000..482a6b0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/Eigen/src/plugins/ReshapedMethods.h
@@ -0,0 +1,149 @@
+
+#ifdef EIGEN_PARSED_BY_DOXYGEN
+
+/// \returns an expression of \c *this with reshaped sizes.
+///
+/// \param nRows the number of rows in the reshaped expression, specified at either run-time or compile-time, or AutoSize
+/// \param nCols the number of columns in the reshaped expression, specified at either run-time or compile-time, or AutoSize
+/// \tparam Order specifies whether the coefficients should be processed in column-major-order (ColMajor), in row-major-order (RowMajor),
+///               or follows the \em natural order of the nested expression (AutoOrder). The default is ColMajor.
+/// \tparam NRowsType the type of the value handling the number of rows, typically Index.
+/// \tparam NColsType the type of the value handling the number of columns, typically Index.
+///
+/// Dynamic size example: \include MatrixBase_reshaped_int_int.cpp
+/// Output: \verbinclude MatrixBase_reshaped_int_int.out
+///
+/// The number of rows \a nRows and columns \a nCols can also be specified at compile-time by passing Eigen::fix<N>,
+/// or Eigen::fix<N>(n) as arguments. In the later case, \c n plays the role of a runtime fallback value in case \c N equals Eigen::Dynamic.
+/// Here is an example with a fixed number of rows and columns:
+/// \include MatrixBase_reshaped_fixed.cpp
+/// Output: \verbinclude MatrixBase_reshaped_fixed.out
+///
+/// Finally, one of the sizes parameter can be automatically deduced from the other one by passing AutoSize as in the following example:
+/// \include MatrixBase_reshaped_auto.cpp
+/// Output: \verbinclude MatrixBase_reshaped_auto.out
+/// AutoSize does preserve compile-time sizes when possible, i.e., when the sizes of the input are known at compile time \b and
+/// that the other size is passed at compile-time using Eigen::fix<N> as above.
+///
+/// \sa class Reshaped, fix, fix<N>(int)
+///
+template<int Order = ColMajor, typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC
+inline Reshaped<Derived,...>
+reshaped(NRowsType nRows, NColsType nCols);
+
+/// This is the const version of reshaped(NRowsType,NColsType).
+template<int Order = ColMajor, typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC
+inline const Reshaped<const Derived,...>
+reshaped(NRowsType nRows, NColsType nCols) const;
+
+/// \returns an expression of \c *this with columns (or rows) stacked to a linear column vector
+///
+/// \tparam Order specifies whether the coefficients should be processed in column-major-order (ColMajor), in row-major-order (RowMajor),
+///               or follows the \em natural order of the nested expression (AutoOrder). The default is ColMajor.
+///
+/// This overloads is essentially a shortcut for `A.reshaped<Order>(AutoSize,fix<1>)`.
+///
+/// - If `Order==ColMajor` (the default), then it returns a column-vector from the stacked columns of \c *this.
+/// - If `Order==RowMajor`, then it returns a column-vector from the stacked rows of \c *this.
+/// - If `Order==AutoOrder`, then it returns a column-vector with elements stacked following the storage order of \c *this.
+///   This mode is the recommended one when the particular ordering of the element is not relevant.
+///
+/// Example:
+/// \include MatrixBase_reshaped_to_vector.cpp
+/// Output: \verbinclude MatrixBase_reshaped_to_vector.out
+///
+/// If you want more control, you can still fall back to reshaped(NRowsType,NColsType).
+///
+/// \sa reshaped(NRowsType,NColsType), class Reshaped
+///
+template<int Order = ColMajor>
+EIGEN_DEVICE_FUNC
+inline Reshaped<Derived,...>
+reshaped();
+
+/// This is the const version of reshaped().
+template<int Order = ColMajor>
+EIGEN_DEVICE_FUNC
+inline const Reshaped<const Derived,...>
+reshaped() const;
+
+#else
+
+// This file is automatically included twice to generate const and non-const versions
+
+#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
+#define EIGEN_RESHAPED_METHOD_CONST const
+#else
+#define EIGEN_RESHAPED_METHOD_CONST
+#endif
+
+#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
+
+// This part is included once
+
+#endif
+
+template<typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC
+inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
+                internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
+                internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value>
+reshaped(NRowsType nRows, NColsType nCols) EIGEN_RESHAPED_METHOD_CONST
+{
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
+                  internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
+                  internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value>
+                (derived(),
+                 internal::get_runtime_reshape_size(nRows,internal::get_runtime_value(nCols),size()),
+                 internal::get_runtime_reshape_size(nCols,internal::get_runtime_value(nRows),size()));
+}
+
+template<int Order, typename NRowsType, typename NColsType>
+EIGEN_DEVICE_FUNC
+inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
+                internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
+                internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value,
+                internal::get_compiletime_reshape_order<Flags,Order>::value>
+reshaped(NRowsType nRows, NColsType nCols) EIGEN_RESHAPED_METHOD_CONST
+{
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,
+                  internal::get_compiletime_reshape_size<NRowsType,NColsType,SizeAtCompileTime>::value,
+                  internal::get_compiletime_reshape_size<NColsType,NRowsType,SizeAtCompileTime>::value,
+                  internal::get_compiletime_reshape_order<Flags,Order>::value>
+                (derived(),
+                 internal::get_runtime_reshape_size(nRows,internal::get_runtime_value(nCols),size()),
+                 internal::get_runtime_reshape_size(nCols,internal::get_runtime_value(nRows),size()));
+}
+
+// Views as linear vectors
+
+EIGEN_DEVICE_FUNC
+inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,SizeAtCompileTime,1>
+reshaped() EIGEN_RESHAPED_METHOD_CONST
+{
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived,SizeAtCompileTime,1>(derived(),size(),1);
+}
+
+template<int Order>
+EIGEN_DEVICE_FUNC
+inline Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1,
+                internal::get_compiletime_reshape_order<Flags,Order>::value>
+reshaped() EIGEN_RESHAPED_METHOD_CONST
+{
+  EIGEN_STATIC_ASSERT(Order==RowMajor || Order==ColMajor || Order==AutoOrder, INVALID_TEMPLATE_PARAMETER);
+  return Reshaped<EIGEN_RESHAPED_METHOD_CONST Derived, SizeAtCompileTime, 1,
+                  internal::get_compiletime_reshape_order<Flags,Order>::value>
+                (derived(), size(), 1);
+}
+
+#undef EIGEN_RESHAPED_METHOD_CONST
+
+#ifndef EIGEN_RESHAPED_METHOD_2ND_PASS
+#define EIGEN_RESHAPED_METHOD_2ND_PASS
+#include "ReshapedMethods.h"
+#undef EIGEN_RESHAPED_METHOD_2ND_PASS
+#endif
+
+#endif // EIGEN_PARSED_BY_DOXYGEN
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff
new file mode 100644
index 0000000..7a4ff46
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/AutoDiff
@@ -0,0 +1,46 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_AUTODIFF_MODULE
+#define EIGEN_AUTODIFF_MODULE
+
+namespace Eigen {
+
+/**
+  * \defgroup AutoDiff_Module Auto Diff module
+  *
+  * This module features forward automatic differentation via a simple
+  * templated scalar type wrapper AutoDiffScalar.
+  *
+  * Warning : this should NOT be confused with numerical differentiation, which
+  * is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
+  *
+  * \code
+  * #include <unsupported/Eigen/AutoDiff>
+  * \endcode
+  */
+//@{
+
+}
+#include "../../Eigen/src/Core/util/DisableStupidWarnings.h"
+
+
+#include "src/AutoDiff/AutoDiffScalar.h"
+// #include "src/AutoDiff/AutoDiffVector.h"
+#include "src/AutoDiff/AutoDiffJacobian.h"
+
+#include "../../Eigen/src/Core/util/ReenableStupidWarnings.h"
+
+
+
+namespace Eigen {
+//@}
+}
+
+#endif // EIGEN_AUTODIFF_MODULE
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions
new file mode 100644
index 0000000..20c23d1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/MatrixFunctions
@@ -0,0 +1,504 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Jitse Niesen <jitse@maths.leeds.ac.uk>
+// Copyright (C) 2012 Chen-Pang He <jdh8@ms63.hinet.net>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIX_FUNCTIONS
+#define EIGEN_MATRIX_FUNCTIONS
+
+#include <cfloat>
+#include <list>
+
+#include "../../Eigen/Core"
+#include "../../Eigen/LU"
+#include "../../Eigen/Eigenvalues"
+
+/**
+  * \defgroup MatrixFunctions_Module Matrix functions module
+  * \brief This module aims to provide various methods for the computation of
+  * matrix functions. 
+  *
+  * To use this module, add 
+  * \code
+  * #include <unsupported/Eigen/MatrixFunctions>
+  * \endcode
+  * at the start of your source file.
+  *
+  * This module defines the following MatrixBase methods.
+  *  - \ref matrixbase_cos "MatrixBase::cos()", for computing the matrix cosine
+  *  - \ref matrixbase_cosh "MatrixBase::cosh()", for computing the matrix hyperbolic cosine
+  *  - \ref matrixbase_exp "MatrixBase::exp()", for computing the matrix exponential
+  *  - \ref matrixbase_log "MatrixBase::log()", for computing the matrix logarithm
+  *  - \ref matrixbase_pow "MatrixBase::pow()", for computing the matrix power
+  *  - \ref matrixbase_matrixfunction "MatrixBase::matrixFunction()", for computing general matrix functions
+  *  - \ref matrixbase_sin "MatrixBase::sin()", for computing the matrix sine
+  *  - \ref matrixbase_sinh "MatrixBase::sinh()", for computing the matrix hyperbolic sine
+  *  - \ref matrixbase_sqrt "MatrixBase::sqrt()", for computing the matrix square root
+  *
+  * These methods are the main entry points to this module. 
+  *
+  * %Matrix functions are defined as follows.  Suppose that \f$ f \f$
+  * is an entire function (that is, a function on the complex plane
+  * that is everywhere complex differentiable).  Then its Taylor
+  * series
+  * \f[ f(0) + f'(0) x + \frac{f''(0)}{2} x^2 + \frac{f'''(0)}{3!} x^3 + \cdots \f]
+  * converges to \f$ f(x) \f$. In this case, we can define the matrix
+  * function by the same series:
+  * \f[ f(M) = f(0) + f'(0) M + \frac{f''(0)}{2} M^2 + \frac{f'''(0)}{3!} M^3 + \cdots \f]
+  *
+  */
+
+#include "../../Eigen/src/Core/util/DisableStupidWarnings.h"
+
+#include "src/MatrixFunctions/MatrixExponential.h"
+#include "src/MatrixFunctions/MatrixFunction.h"
+#include "src/MatrixFunctions/MatrixSquareRoot.h"
+#include "src/MatrixFunctions/MatrixLogarithm.h"
+#include "src/MatrixFunctions/MatrixPower.h"
+
+#include "../../Eigen/src/Core/util/ReenableStupidWarnings.h"
+
+
+/** 
+\page matrixbaseextra_page
+\ingroup MatrixFunctions_Module
+
+\section matrixbaseextra MatrixBase methods defined in the MatrixFunctions module
+
+The remainder of the page documents the following MatrixBase methods
+which are defined in the MatrixFunctions module.
+
+
+
+\subsection matrixbase_cos MatrixBase::cos()
+
+Compute the matrix cosine.
+
+\code
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
+\endcode
+
+\param[in]  M  a square matrix.
+\returns  expression representing \f$ \cos(M) \f$.
+
+This function computes the matrix cosine. Use ArrayBase::cos() for computing the entry-wise cosine.
+
+The implementation calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cos().
+
+\sa \ref matrixbase_sin "sin()" for an example.
+
+
+
+\subsection matrixbase_cosh MatrixBase::cosh()
+
+Compute the matrix hyberbolic cosine.
+
+\code
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
+\endcode
+
+\param[in]  M  a square matrix.
+\returns  expression representing \f$ \cosh(M) \f$
+
+This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cosh().
+
+\sa \ref matrixbase_sinh "sinh()" for an example.
+
+
+
+\subsection matrixbase_exp MatrixBase::exp()
+
+Compute the matrix exponential.
+
+\code
+const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
+\endcode
+
+\param[in]  M  matrix whose exponential is to be computed.
+\returns    expression representing the matrix exponential of \p M.
+
+The matrix exponential of \f$ M \f$ is defined by
+\f[ \exp(M) = \sum_{k=0}^\infty \frac{M^k}{k!}. \f]
+The matrix exponential can be used to solve linear ordinary
+differential equations: the solution of \f$ y' = My \f$ with the
+initial condition \f$ y(0) = y_0 \f$ is given by
+\f$ y(t) = \exp(M) y_0 \f$.
+
+The matrix exponential is different from applying the exp function to all the entries in the matrix.
+Use ArrayBase::exp() if you want to do the latter.
+
+The cost of the computation is approximately \f$ 20 n^3 \f$ for
+matrices of size \f$ n \f$. The number 20 depends weakly on the
+norm of the matrix.
+
+The matrix exponential is computed using the scaling-and-squaring
+method combined with Pad&eacute; approximation. The matrix is first
+rescaled, then the exponential of the reduced matrix is computed
+approximant, and then the rescaling is undone by repeated
+squaring. The degree of the Pad&eacute; approximant is chosen such
+that the approximation error is less than the round-off
+error. However, errors may accumulate during the squaring phase.
+
+Details of the algorithm can be found in: Nicholas J. Higham, "The
+scaling and squaring method for the matrix exponential revisited,"
+<em>SIAM J. %Matrix Anal. Applic.</em>, <b>26</b>:1179&ndash;1193,
+2005.
+
+Example: The following program checks that
+\f[ \exp \left[ \begin{array}{ccc}
+      0 & \frac14\pi & 0 \\
+      -\frac14\pi & 0 & 0 \\
+      0 & 0 & 0
+    \end{array} \right] = \left[ \begin{array}{ccc}
+      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
+      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
+      0 & 0 & 1
+    \end{array} \right]. \f]
+This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
+the z-axis.
+
+\include MatrixExponential.cpp
+Output: \verbinclude MatrixExponential.out
+
+\note \p M has to be a matrix of \c float, \c double, `long double`
+\c complex<float>, \c complex<double>, or `complex<long double>` .
+
+
+\subsection matrixbase_log MatrixBase::log()
+
+Compute the matrix logarithm.
+
+\code
+const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
+\endcode
+
+\param[in]  M  invertible matrix whose logarithm is to be computed.
+\returns    expression representing the matrix logarithm root of \p M.
+
+The matrix logarithm of \f$ M \f$ is a matrix \f$ X \f$ such that 
+\f$ \exp(X) = M \f$ where exp denotes the matrix exponential. As for
+the scalar logarithm, the equation \f$ \exp(X) = M \f$ may have
+multiple solutions; this function returns a matrix whose eigenvalues
+have imaginary part in the interval \f$ (-\pi,\pi] \f$.
+
+The matrix logarithm is different from applying the log function to all the entries in the matrix.
+Use ArrayBase::log() if you want to do the latter.
+
+In the real case, the matrix \f$ M \f$ should be invertible and
+it should have no eigenvalues which are real and negative (pairs of
+complex conjugate eigenvalues are allowed). In the complex case, it
+only needs to be invertible.
+
+This function computes the matrix logarithm using the Schur-Parlett
+algorithm as implemented by MatrixBase::matrixFunction(). The
+logarithm of an atomic block is computed by MatrixLogarithmAtomic,
+which uses direct computation for 1-by-1 and 2-by-2 blocks and an
+inverse scaling-and-squaring algorithm for bigger blocks, with the
+square roots computed by MatrixBase::sqrt().
+
+Details of the algorithm can be found in Section 11.6.2 of:
+Nicholas J. Higham,
+<em>Functions of Matrices: Theory and Computation</em>,
+SIAM 2008. ISBN 978-0-898716-46-7.
+
+Example: The following program checks that
+\f[ \log \left[ \begin{array}{ccc} 
+      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
+      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
+      0 & 0 & 1
+    \end{array} \right] = \left[ \begin{array}{ccc}
+      0 & \frac14\pi & 0 \\ 
+      -\frac14\pi & 0 & 0 \\
+      0 & 0 & 0 
+    \end{array} \right]. \f]
+This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
+the z-axis. This is the inverse of the example used in the
+documentation of \ref matrixbase_exp "exp()".
+
+\include MatrixLogarithm.cpp
+Output: \verbinclude MatrixLogarithm.out
+
+\note \p M has to be a matrix of \c float, \c double, `long
+double`, \c complex<float>, \c complex<double>, or `complex<long double>`.
+
+\sa MatrixBase::exp(), MatrixBase::matrixFunction(), 
+    class MatrixLogarithmAtomic, MatrixBase::sqrt().
+
+
+\subsection matrixbase_pow MatrixBase::pow()
+
+Compute the matrix raised to arbitrary real power.
+
+\code
+const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(RealScalar p) const
+\endcode
+
+\param[in]  M  base of the matrix power, should be a square matrix.
+\param[in]  p  exponent of the matrix power.
+
+The matrix power \f$ M^p \f$ is defined as \f$ \exp(p \log(M)) \f$,
+where exp denotes the matrix exponential, and log denotes the matrix
+logarithm. This is different from raising all the entries in the matrix
+to the p-th power. Use ArrayBase::pow() if you want to do the latter.
+
+If \p p is complex, the scalar type of \p M should be the type of \p
+p . \f$ M^p \f$ simply evaluates into \f$ \exp(p \log(M)) \f$.
+Therefore, the matrix \f$ M \f$ should meet the conditions to be an
+argument of matrix logarithm.
+
+If \p p is real, it is casted into the real scalar type of \p M. Then
+this function computes the matrix power using the Schur-Pad&eacute;
+algorithm as implemented by class MatrixPower. The exponent is split
+into integral part and fractional part, where the fractional part is
+in the interval \f$ (-1, 1) \f$. The main diagonal and the first
+super-diagonal is directly computed.
+
+If \p M is singular with a semisimple zero eigenvalue and \p p is
+positive, the Schur factor \f$ T \f$ is reordered with Givens
+rotations, i.e.
+
+\f[ T = \left[ \begin{array}{cc}
+      T_1 & T_2 \\
+      0   & 0
+    \end{array} \right] \f]
+
+where \f$ T_1 \f$ is invertible. Then \f$ T^p \f$ is given by
+
+\f[ T^p = \left[ \begin{array}{cc}
+      T_1^p & T_1^{-1} T_1^p T_2 \\
+      0     & 0
+    \end{array}. \right] \f]
+
+\warning Fractional power of a matrix with a non-semisimple zero
+eigenvalue is not well-defined. We introduce an assertion failure
+against inaccurate result, e.g. \code
+#include <unsupported/Eigen/MatrixFunctions>
+#include <iostream>
+
+int main()
+{
+  Eigen::Matrix4d A;
+  A << 0, 0, 2, 3,
+       0, 0, 4, 5,
+       0, 0, 6, 7,
+       0, 0, 8, 9;
+  std::cout << A.pow(0.37) << std::endl;
+  
+  // The 1 makes eigenvalue 0 non-semisimple.
+  A.coeffRef(0, 1) = 1;
+
+  // This fails if EIGEN_NO_DEBUG is undefined.
+  std::cout << A.pow(0.37) << std::endl;
+
+  return 0;
+}
+\endcode
+
+Details of the algorithm can be found in: Nicholas J. Higham and
+Lijing Lin, "A Schur-Pad&eacute; algorithm for fractional powers of a
+matrix," <em>SIAM J. %Matrix Anal. Applic.</em>,
+<b>32(3)</b>:1056&ndash;1078, 2011.
+
+Example: The following program checks that
+\f[ \left[ \begin{array}{ccc}
+      \cos1 & -\sin1 & 0 \\
+      \sin1 & \cos1 & 0 \\
+      0 & 0 & 1
+    \end{array} \right]^{\frac14\pi} = \left[ \begin{array}{ccc}
+      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
+      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
+      0 & 0 & 1
+    \end{array} \right]. \f]
+This corresponds to \f$ \frac14\pi \f$ rotations of 1 radian around
+the z-axis.
+
+\include MatrixPower.cpp
+Output: \verbinclude MatrixPower.out
+
+MatrixBase::pow() is user-friendly. However, there are some
+circumstances under which you should use class MatrixPower directly.
+MatrixPower can save the result of Schur decomposition, so it's
+better for computing various powers for the same matrix.
+
+Example:
+\include MatrixPower_optimal.cpp
+Output: \verbinclude MatrixPower_optimal.out
+
+\note \p M has to be a matrix of \c float, \c double, `long
+double`, \c complex<float>, \c complex<double>, or
+\c complex<long double> .
+
+\sa MatrixBase::exp(), MatrixBase::log(), class MatrixPower.
+
+
+\subsection matrixbase_matrixfunction MatrixBase::matrixFunction()
+
+Compute a matrix function.
+
+\code
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
+\endcode
+
+\param[in]  M  argument of matrix function, should be a square matrix.
+\param[in]  f  an entire function; \c f(x,n) should compute the n-th
+derivative of f at x.
+\returns  expression representing \p f applied to \p M.
+
+Suppose that \p M is a matrix whose entries have type \c Scalar. 
+Then, the second argument, \p f, should be a function with prototype
+\code 
+ComplexScalar f(ComplexScalar, int) 
+\endcode
+where \c ComplexScalar = \c std::complex<Scalar> if \c Scalar is
+real (e.g., \c float or \c double) and \c ComplexScalar =
+\c Scalar if \c Scalar is complex. The return value of \c f(x,n)
+should be \f$ f^{(n)}(x) \f$, the n-th derivative of f at x.
+
+This routine uses the algorithm described in:
+Philip Davies and Nicholas J. Higham, 
+"A Schur-Parlett algorithm for computing matrix functions", 
+<em>SIAM J. %Matrix Anal. Applic.</em>, <b>25</b>:464&ndash;485, 2003.
+
+The actual work is done by the MatrixFunction class.
+
+Example: The following program checks that
+\f[ \exp \left[ \begin{array}{ccc} 
+      0 & \frac14\pi & 0 \\ 
+      -\frac14\pi & 0 & 0 \\
+      0 & 0 & 0 
+    \end{array} \right] = \left[ \begin{array}{ccc}
+      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
+      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
+      0 & 0 & 1
+    \end{array} \right]. \f]
+This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
+the z-axis. This is the same example as used in the documentation
+of \ref matrixbase_exp "exp()".
+
+\include MatrixFunction.cpp
+Output: \verbinclude MatrixFunction.out
+
+Note that the function \c expfn is defined for complex numbers 
+\c x, even though the matrix \c A is over the reals. Instead of
+\c expfn, we could also have used StdStemFunctions::exp:
+\code
+A.matrixFunction(StdStemFunctions<std::complex<double> >::exp, &B);
+\endcode
+
+
+
+\subsection matrixbase_sin MatrixBase::sin()
+
+Compute the matrix sine.
+
+\code
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
+\endcode
+
+\param[in]  M  a square matrix.
+\returns  expression representing \f$ \sin(M) \f$.
+
+This function computes the matrix sine. Use ArrayBase::sin() for computing the entry-wise sine.
+
+The implementation calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sin().
+
+Example: \include MatrixSine.cpp
+Output: \verbinclude MatrixSine.out
+
+
+
+\subsection matrixbase_sinh MatrixBase::sinh()
+
+Compute the matrix hyperbolic sine.
+
+\code
+MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
+\endcode
+
+\param[in]  M  a square matrix.
+\returns  expression representing \f$ \sinh(M) \f$
+
+This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sinh().
+
+Example: \include MatrixSinh.cpp
+Output: \verbinclude MatrixSinh.out
+
+
+\subsection matrixbase_sqrt MatrixBase::sqrt()
+
+Compute the matrix square root.
+
+\code
+const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
+\endcode
+
+\param[in]  M  invertible matrix whose square root is to be computed.
+\returns    expression representing the matrix square root of \p M.
+
+The matrix square root of \f$ M \f$ is the matrix \f$ M^{1/2} \f$
+whose square is the original matrix; so if \f$ S = M^{1/2} \f$ then
+\f$ S^2 = M \f$. This is different from taking the square root of all
+the entries in the matrix; use ArrayBase::sqrt() if you want to do the
+latter.
+
+In the <b>real case</b>, the matrix \f$ M \f$ should be invertible and
+it should have no eigenvalues which are real and negative (pairs of
+complex conjugate eigenvalues are allowed). In that case, the matrix
+has a square root which is also real, and this is the square root
+computed by this function. 
+
+The matrix square root is computed by first reducing the matrix to
+quasi-triangular form with the real Schur decomposition. The square
+root of the quasi-triangular matrix can then be computed directly. The
+cost is approximately \f$ 25 n^3 \f$ real flops for the real Schur
+decomposition and \f$ 3\frac13 n^3 \f$ real flops for the remainder
+(though the computation time in practice is likely more than this
+indicates).
+
+Details of the algorithm can be found in: Nicholas J. Highan,
+"Computing real square roots of a real matrix", <em>Linear Algebra
+Appl.</em>, 88/89:405&ndash;430, 1987.
+
+If the matrix is <b>positive-definite symmetric</b>, then the square
+root is also positive-definite symmetric. In this case, it is best to
+use SelfAdjointEigenSolver::operatorSqrt() to compute it.
+
+In the <b>complex case</b>, the matrix \f$ M \f$ should be invertible;
+this is a restriction of the algorithm. The square root computed by
+this algorithm is the one whose eigenvalues have an argument in the
+interval \f$ (-\frac12\pi, \frac12\pi] \f$. This is the usual branch
+cut.
+
+The computation is the same as in the real case, except that the
+complex Schur decomposition is used to reduce the matrix to a
+triangular matrix. The theoretical cost is the same. Details are in:
+&Aring;ke Bj&ouml;rck and Sven Hammarling, "A Schur method for the
+square root of a matrix", <em>Linear Algebra Appl.</em>,
+52/53:127&ndash;140, 1983.
+
+Example: The following program checks that the square root of
+\f[ \left[ \begin{array}{cc} 
+              \cos(\frac13\pi) & -\sin(\frac13\pi) \\
+              \sin(\frac13\pi) & \cos(\frac13\pi)
+    \end{array} \right], \f]
+corresponding to a rotation over 60 degrees, is a rotation over 30 degrees:
+\f[ \left[ \begin{array}{cc} 
+              \cos(\frac16\pi) & -\sin(\frac16\pi) \\
+              \sin(\frac16\pi) & \cos(\frac16\pi)
+    \end{array} \right]. \f]
+
+\include MatrixSquareRoot.cpp
+Output: \verbinclude MatrixSquareRoot.out
+
+\sa class RealSchur, class ComplexSchur, class MatrixSquareRoot,
+    SelfAdjointEigenSolver::operatorSqrt().
+
+*/
+
+#endif // EIGEN_MATRIX_FUNCTIONS
+
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
similarity index 100%
rename from third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
rename to third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
new file mode 100644
index 0000000..0f166e3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
@@ -0,0 +1,730 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_AUTODIFF_SCALAR_H
+#define EIGEN_AUTODIFF_SCALAR_H
+
+namespace Eigen {
+
+namespace internal {
+
+template<typename A, typename B>
+struct make_coherent_impl {
+  static void run(A&, B&) {}
+};
+
+// resize a to match b is a.size()==0, and conversely.
+template<typename A, typename B>
+void make_coherent(const A& a, const B&b)
+{
+  make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
+}
+
+template<typename DerivativeType, bool Enable> struct auto_diff_special_op;
+
+} // end namespace internal
+
+template<typename DerivativeType> class AutoDiffScalar;
+
+template<typename NewDerType>
+inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
+  return AutoDiffScalar<NewDerType>(value,der);
+}
+
+/** \class AutoDiffScalar
+  * \brief A scalar type replacement with automatic differentiation capability
+  *
+  * \param DerivativeType the vector type used to store/represent the derivatives. The base scalar type
+  *                 as well as the number of derivatives to compute are determined from this type.
+  *                 Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
+  *                 if the number of derivatives is not known at compile time, and/or, the number
+  *                 of derivatives is large.
+  *                 Note that DerivativeType can also be a reference (e.g., \c VectorXf&) to wrap a
+  *                 existing vector into an AutoDiffScalar.
+  *                 Finally, DerivativeType can also be any Eigen compatible expression.
+  *
+  * This class represents a scalar value while tracking its respective derivatives using Eigen's expression
+  * template mechanism.
+  *
+  * It supports the following list of global math function:
+  *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
+  *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
+  *  - internal::conj, internal::real, internal::imag, numext::abs2.
+  *
+  * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
+  * in that case, the expression template mechanism only occurs at the top Matrix level,
+  * while derivatives are computed right away.
+  *
+  */
+
+template<typename DerivativeType>
+class AutoDiffScalar
+  : public internal::auto_diff_special_op
+            <DerivativeType, !internal::is_same<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar,
+                                          typename NumTraits<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar>::Real>::value>
+{
+  public:
+    typedef internal::auto_diff_special_op
+            <DerivativeType, !internal::is_same<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar,
+                       typename NumTraits<typename internal::traits<typename internal::remove_all<DerivativeType>::type>::Scalar>::Real>::value> Base;
+    typedef typename internal::remove_all<DerivativeType>::type DerType;
+    typedef typename internal::traits<DerType>::Scalar Scalar;
+    typedef typename NumTraits<Scalar>::Real Real;
+
+    using Base::operator+;
+    using Base::operator*;
+
+    /** Default constructor without any initialization. */
+    AutoDiffScalar() {}
+
+    /** Constructs an active scalar from its \a value,
+        and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
+    AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
+      : m_value(value), m_derivatives(DerType::Zero(nbDer))
+    {
+      m_derivatives.coeffRef(derNumber) = Scalar(1);
+    }
+
+    /** Conversion from a scalar constant to an active scalar.
+      * The derivatives are set to zero. */
+    /*explicit*/ AutoDiffScalar(const Real& value)
+      : m_value(value)
+    {
+      if(m_derivatives.size()>0)
+        m_derivatives.setZero();
+    }
+
+    /** Constructs an active scalar from its \a value and derivatives \a der */
+    AutoDiffScalar(const Scalar& value, const DerType& der)
+      : m_value(value), m_derivatives(der)
+    {}
+
+    template<typename OtherDerType>
+    AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+    , typename internal::enable_if<
+            internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
+        &&  internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
+#endif
+    )
+      : m_value(other.value()), m_derivatives(other.derivatives())
+    {}
+
+    friend  std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
+    {
+      return s << a.value();
+    }
+
+    AutoDiffScalar(const AutoDiffScalar& other)
+      : m_value(other.value()), m_derivatives(other.derivatives())
+    {}
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      m_value = other.value();
+      m_derivatives = other.derivatives();
+      return *this;
+    }
+
+    inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
+    {
+      m_value = other.value();
+      m_derivatives = other.derivatives();
+      return *this;
+    }
+
+    inline AutoDiffScalar& operator=(const Scalar& other)
+    {
+      m_value = other;
+      if(m_derivatives.size()>0)
+        m_derivatives.setZero();
+      return *this;
+    }
+
+//     inline operator const Scalar& () const { return m_value; }
+//     inline operator Scalar& () { return m_value; }
+
+    inline const Scalar& value() const { return m_value; }
+    inline Scalar& value() { return m_value; }
+
+    inline const DerType& derivatives() const { return m_derivatives; }
+    inline DerType& derivatives() { return m_derivatives; }
+
+    inline bool operator< (const Scalar& other) const  { return m_value <  other; }
+    inline bool operator<=(const Scalar& other) const  { return m_value <= other; }
+    inline bool operator> (const Scalar& other) const  { return m_value >  other; }
+    inline bool operator>=(const Scalar& other) const  { return m_value >= other; }
+    inline bool operator==(const Scalar& other) const  { return m_value == other; }
+    inline bool operator!=(const Scalar& other) const  { return m_value != other; }
+
+    friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a <  b.value(); }
+    friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
+    friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a >  b.value(); }
+    friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
+    friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
+    friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
+
+    template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const  { return m_value <  b.value(); }
+    template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value <= b.value(); }
+    template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const  { return m_value >  b.value(); }
+    template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value >= b.value(); }
+    template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const  { return m_value == b.value(); }
+    template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value != b.value(); }
+
+    inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
+    {
+      return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
+    }
+
+    friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
+    {
+      return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
+    }
+
+//     inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
+//     {
+//       return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
+//     }
+
+//     friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
+//     {
+//       return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
+//     }
+
+    inline AutoDiffScalar& operator+=(const Scalar& other)
+    {
+      value() += other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
+    operator+(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
+        m_value + other.value(),
+        m_derivatives + other.derivatives());
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar&
+    operator+=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      (*this) = (*this) + other;
+      return *this;
+    }
+
+    inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
+    {
+      return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
+    }
+
+    friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
+    operator-(const Scalar& a, const AutoDiffScalar& b)
+    {
+      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
+            (a - b.value(), -b.derivatives());
+    }
+
+    inline AutoDiffScalar& operator-=(const Scalar& other)
+    {
+      value() -= other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
+    operator-(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
+        m_value - other.value(),
+        m_derivatives - other.derivatives());
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar&
+    operator-=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      *this = *this - other;
+      return *this;
+    }
+
+    inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
+    operator-() const
+    {
+      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
+        -m_value,
+        -m_derivatives);
+    }
+
+    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator*(const Scalar& other) const
+    {
+      return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
+    }
+
+    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator*(const Scalar& other, const AutoDiffScalar& a)
+    {
+      return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
+    }
+
+//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator*(const Real& other) const
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         m_value * other,
+//         (m_derivatives * other));
+//     }
+//
+//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator*(const Real& other, const AutoDiffScalar& a)
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         a.value() * other,
+//         a.derivatives() * other);
+//     }
+
+    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator/(const Scalar& other) const
+    {
+      return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
+    }
+
+    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
+    operator/(const Scalar& other, const AutoDiffScalar& a)
+    {
+      return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
+    }
+
+//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator/(const Real& other) const
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         m_value / other,
+//         (m_derivatives * (Real(1)/other)));
+//     }
+//
+//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
+//     operator/(const Real& other, const AutoDiffScalar& a)
+//     {
+//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
+//         other / a.value(),
+//         a.derivatives() * (-Real(1)/other));
+//     }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
+        CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
+          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
+          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
+    operator/(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return MakeAutoDiffScalar(
+        m_value / other.value(),
+          ((m_derivatives * other.value()) - (other.derivatives() * m_value))
+        * (Scalar(1)/(other.value()*other.value())));
+    }
+
+    template<typename OtherDerType>
+    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
+        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
+        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
+    operator*(const AutoDiffScalar<OtherDerType>& other) const
+    {
+      internal::make_coherent(m_derivatives, other.derivatives());
+      return MakeAutoDiffScalar(
+        m_value * other.value(),
+        (m_derivatives * other.value()) + (other.derivatives() * m_value));
+    }
+
+    inline AutoDiffScalar& operator*=(const Scalar& other)
+    {
+      *this = *this * other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      *this = *this * other;
+      return *this;
+    }
+
+    inline AutoDiffScalar& operator/=(const Scalar& other)
+    {
+      *this = *this / other;
+      return *this;
+    }
+
+    template<typename OtherDerType>
+    inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
+    {
+      *this = *this / other;
+      return *this;
+    }
+
+  protected:
+    Scalar m_value;
+    DerType m_derivatives;
+
+};
+
+namespace internal {
+
+template<typename DerivativeType>
+struct auto_diff_special_op<DerivativeType, true>
+//   : auto_diff_scalar_op<DerivativeType, typename NumTraits<Scalar>::Real,
+//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
+{
+  typedef typename remove_all<DerivativeType>::type DerType;
+  typedef typename traits<DerType>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real Real;
+
+//   typedef auto_diff_scalar_op<DerivativeType, typename NumTraits<Scalar>::Real,
+//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
+
+//   using Base::operator+;
+//   using Base::operator+=;
+//   using Base::operator-;
+//   using Base::operator-=;
+//   using Base::operator*;
+//   using Base::operator*=;
+
+  const AutoDiffScalar<DerivativeType>& derived() const { return *static_cast<const AutoDiffScalar<DerivativeType>*>(this); }
+  AutoDiffScalar<DerivativeType>& derived() { return *static_cast<AutoDiffScalar<DerivativeType>*>(this); }
+
+
+  inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
+  {
+    return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
+  }
+
+  friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<DerivativeType>& b)
+  {
+    return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
+  }
+
+  inline AutoDiffScalar<DerivativeType>& operator+=(const Real& other)
+  {
+    derived().value() += other;
+    return derived();
+  }
+
+
+  inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
+  operator*(const Real& other) const
+  {
+    return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
+      derived().value() * other,
+      derived().derivatives() * other);
+  }
+
+  friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
+  operator*(const Real& other, const AutoDiffScalar<DerivativeType>& a)
+  {
+    return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
+      a.value() * other,
+      a.derivatives() * other);
+  }
+
+  inline AutoDiffScalar<DerivativeType>& operator*=(const Scalar& other)
+  {
+    *this = *this * other;
+    return derived();
+  }
+};
+
+template<typename DerivativeType>
+struct auto_diff_special_op<DerivativeType, false>
+{
+  void operator*() const;
+  void operator-() const;
+  void operator+() const;
+};
+
+template<typename BinOp, typename A, typename B, typename RefType>
+void make_coherent_expression(CwiseBinaryOp<BinOp,A,B> xpr, const RefType &ref)
+{
+  make_coherent(xpr.const_cast_derived().lhs(), ref);
+  make_coherent(xpr.const_cast_derived().rhs(), ref);
+}
+
+template<typename UnaryOp, typename A, typename RefType>
+void make_coherent_expression(const CwiseUnaryOp<UnaryOp,A> &xpr, const RefType &ref)
+{
+  make_coherent(xpr.nestedExpression().const_cast_derived(), ref);
+}
+
+// needed for compilation only
+template<typename UnaryOp, typename A, typename RefType>
+void make_coherent_expression(const CwiseNullaryOp<UnaryOp,A> &, const RefType &)
+{}
+
+template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
+struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
+  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
+  static void run(A& a, B& b) {
+    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
+    {
+      a.resize(b.size());
+      a.setZero();
+    }
+    else if (B::SizeAtCompileTime==Dynamic && a.size()!=0 && b.size()==0)
+    {
+      make_coherent_expression(b,a);
+    }
+  }
+};
+
+template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
+struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
+  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
+  static void run(A& a, B& b) {
+    if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
+    {
+      b.resize(a.size());
+      b.setZero();
+    }
+    else if (A::SizeAtCompileTime==Dynamic && b.size()!=0 && a.size()==0)
+    {
+      make_coherent_expression(a,b);
+    }
+  }
+};
+
+template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
+         typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
+struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
+                          Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
+  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
+  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
+  static void run(A& a, B& b) {
+    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
+    {
+      a.resize(b.size());
+      a.setZero();
+    }
+    else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
+    {
+      b.resize(a.size());
+      b.setZero();
+    }
+  }
+};
+
+} // end namespace internal
+
+template<typename DerType, typename BinOp>
+struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
+{
+  typedef AutoDiffScalar<DerType> ReturnType;
+};
+
+template<typename DerType, typename BinOp>
+struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
+{
+  typedef AutoDiffScalar<DerType> ReturnType;
+};
+
+
+// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
+
+// template<typename DerType, typename BinOp>
+// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
+// {
+//   enum { Defined = 1 };
+//   typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
+// };
+//
+// template<typename DerType1,typename DerType2, typename BinOp>
+// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
+// {
+//   enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
+//   typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
+// };
+
+#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
+  template<typename DerType> \
+  inline const Eigen::AutoDiffScalar< \
+  EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
+  FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
+    using namespace Eigen; \
+    typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
+    EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
+    CODE; \
+  }
+
+template<typename DerType>
+struct CleanedUpDerType {
+  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> type;
+};
+
+template<typename DerType>
+inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x)  { return x; }
+template<typename DerType>
+inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x)  { return x; }
+template<typename DerType>
+inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&)    { return 0.; }
+template<typename DerType, typename T>
+inline typename CleanedUpDerType<DerType>::type (min)(const AutoDiffScalar<DerType>& x, const T& y) {
+  typedef typename CleanedUpDerType<DerType>::type ADS;
+  return (x <= y ? ADS(x) : ADS(y));
+}
+template<typename DerType, typename T>
+inline typename CleanedUpDerType<DerType>::type (max)(const AutoDiffScalar<DerType>& x, const T& y) {
+  typedef typename CleanedUpDerType<DerType>::type ADS;
+  return (x >= y ? ADS(x) : ADS(y));
+}
+template<typename DerType, typename T>
+inline typename CleanedUpDerType<DerType>::type (min)(const T& x, const AutoDiffScalar<DerType>& y) {
+  typedef typename CleanedUpDerType<DerType>::type ADS;
+  return (x < y ? ADS(x) : ADS(y));
+}
+template<typename DerType, typename T>
+inline typename CleanedUpDerType<DerType>::type (max)(const T& x, const AutoDiffScalar<DerType>& y) {
+  typedef typename CleanedUpDerType<DerType>::type ADS;
+  return (x > y ? ADS(x) : ADS(y));
+}
+template<typename DerType>
+inline typename CleanedUpDerType<DerType>::type (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
+  return (x.value() < y.value() ? x : y);
+}
+template<typename DerType>
+inline typename CleanedUpDerType<DerType>::type (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
+  return (x.value() >= y.value() ? x : y);
+}
+
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
+  using std::abs;
+  return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
+  using numext::abs2;
+  return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
+  using std::sqrt;
+  Scalar sqrtx = sqrt(x.value());
+  return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
+  using std::cos;
+  using std::sin;
+  return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
+  using std::sin;
+  using std::cos;
+  return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
+  using std::exp;
+  Scalar expx = exp(x.value());
+  return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
+  using std::log;
+  return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
+
+template<typename DerType>
+inline const Eigen::AutoDiffScalar<
+EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
+pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
+{
+  using namespace Eigen;
+  using std::pow;
+  return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
+}
+
+
+template<typename DerTypeA,typename DerTypeB>
+inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
+atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
+{
+  using std::atan2;
+  typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
+  typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
+  PlainADS ret;
+  ret.value() = atan2(a.value(), b.value());
+  
+  Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
+  
+  // if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
+  ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
+
+  return ret;
+}
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
+  using std::tan;
+  using std::cos;
+  return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
+  using std::sqrt;
+  using std::asin;
+  return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
+  
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
+  using std::sqrt;
+  using std::acos;
+  return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
+  using std::cosh;
+  using std::tanh;
+  return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
+  using std::sinh;
+  using std::cosh;
+  return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
+
+EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
+  using std::sinh;
+  using std::cosh;
+  return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
+
+#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
+
+template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
+  : NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
+{
+  typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
+  typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
+                                0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
+  typedef AutoDiffScalar<DerType> NonInteger;
+  typedef AutoDiffScalar<DerType> Nested;
+  typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
+  enum{
+    RequireInitialization = 1
+  };
+};
+
+}
+
+namespace std {
+
+template <typename T>
+class numeric_limits<Eigen::AutoDiffScalar<T> >
+  : public numeric_limits<typename T::Scalar> {};
+
+template <typename T>
+class numeric_limits<Eigen::AutoDiffScalar<T&> >
+  : public numeric_limits<typename T::Scalar> {};
+
+}  // namespace std
+
+#endif // EIGEN_AUTODIFF_SCALAR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
similarity index 100%
rename from third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
rename to third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
new file mode 100644
index 0000000..02284b0
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
@@ -0,0 +1,441 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009, 2010, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
+// Copyright (C) 2011, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIX_EXPONENTIAL
+#define EIGEN_MATRIX_EXPONENTIAL
+
+#include "StemFunction.h"
+
+namespace Eigen {
+namespace internal {
+
+/** \brief Scaling operator.
+ *
+ * This struct is used by CwiseUnaryOp to scale a matrix by \f$ 2^{-s} \f$.
+ */
+template <typename RealScalar>
+struct MatrixExponentialScalingOp
+{
+  /** \brief Constructor.
+   *
+   * \param[in] squarings  The integer \f$ s \f$ in this document.
+   */
+  MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) { }
+
+
+  /** \brief Scale a matrix coefficient.
+   *
+   * \param[in,out] x  The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
+   */
+  inline const RealScalar operator() (const RealScalar& x) const
+  {
+    using std::ldexp;
+    return ldexp(x, -m_squarings);
+  }
+
+  typedef std::complex<RealScalar> ComplexScalar;
+
+  /** \brief Scale a matrix coefficient.
+   *
+   * \param[in,out] x  The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
+   */
+  inline const ComplexScalar operator() (const ComplexScalar& x) const
+  {
+    using std::ldexp;
+    return ComplexScalar(ldexp(x.real(), -m_squarings), ldexp(x.imag(), -m_squarings));
+  }
+
+  private:
+    int m_squarings;
+};
+
+/** \brief Compute the (3,3)-Pad&eacute; approximant to the exponential.
+ *
+ *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
+ *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
+ */
+template <typename MatA, typename MatU, typename MatV>
+void matrix_exp_pade3(const MatA& A, MatU& U, MatV& V)
+{
+  typedef typename MatA::PlainObject MatrixType;
+  typedef typename NumTraits<typename traits<MatA>::Scalar>::Real RealScalar;
+  const RealScalar b[] = {120.L, 60.L, 12.L, 1.L};
+  const MatrixType A2 = A * A;
+  const MatrixType tmp = b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  U.noalias() = A * tmp;
+  V = b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
+}
+
+/** \brief Compute the (5,5)-Pad&eacute; approximant to the exponential.
+ *
+ *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
+ *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
+ */
+template <typename MatA, typename MatU, typename MatV>
+void matrix_exp_pade5(const MatA& A, MatU& U, MatV& V)
+{
+  typedef typename MatA::PlainObject MatrixType;
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
+  const RealScalar b[] = {30240.L, 15120.L, 3360.L, 420.L, 30.L, 1.L};
+  const MatrixType A2 = A * A;
+  const MatrixType A4 = A2 * A2;
+  const MatrixType tmp = b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  U.noalias() = A * tmp;
+  V = b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
+}
+
+/** \brief Compute the (7,7)-Pad&eacute; approximant to the exponential.
+ *
+ *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
+ *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
+ */
+template <typename MatA, typename MatU, typename MatV>
+void matrix_exp_pade7(const MatA& A, MatU& U, MatV& V)
+{
+  typedef typename MatA::PlainObject MatrixType;
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
+  const RealScalar b[] = {17297280.L, 8648640.L, 1995840.L, 277200.L, 25200.L, 1512.L, 56.L, 1.L};
+  const MatrixType A2 = A * A;
+  const MatrixType A4 = A2 * A2;
+  const MatrixType A6 = A4 * A2;
+  const MatrixType tmp = b[7] * A6 + b[5] * A4 + b[3] * A2 
+    + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  U.noalias() = A * tmp;
+  V = b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
+
+}
+
+/** \brief Compute the (9,9)-Pad&eacute; approximant to the exponential.
+ *
+ *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
+ *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
+ */
+template <typename MatA, typename MatU, typename MatV>
+void matrix_exp_pade9(const MatA& A, MatU& U, MatV& V)
+{
+  typedef typename MatA::PlainObject MatrixType;
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
+  const RealScalar b[] = {17643225600.L, 8821612800.L, 2075673600.L, 302702400.L, 30270240.L,
+                          2162160.L, 110880.L, 3960.L, 90.L, 1.L};
+  const MatrixType A2 = A * A;
+  const MatrixType A4 = A2 * A2;
+  const MatrixType A6 = A4 * A2;
+  const MatrixType A8 = A6 * A2;
+  const MatrixType tmp = b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 
+    + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  U.noalias() = A * tmp;
+  V = b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
+}
+
+/** \brief Compute the (13,13)-Pad&eacute; approximant to the exponential.
+ *
+ *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
+ *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
+ */
+template <typename MatA, typename MatU, typename MatV>
+void matrix_exp_pade13(const MatA& A, MatU& U, MatV& V)
+{
+  typedef typename MatA::PlainObject MatrixType;
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
+  const RealScalar b[] = {64764752532480000.L, 32382376266240000.L, 7771770303897600.L,
+                          1187353796428800.L, 129060195264000.L, 10559470521600.L, 670442572800.L,
+                          33522128640.L, 1323241920.L, 40840800.L, 960960.L, 16380.L, 182.L, 1.L};
+  const MatrixType A2 = A * A;
+  const MatrixType A4 = A2 * A2;
+  const MatrixType A6 = A4 * A2;
+  V = b[13] * A6 + b[11] * A4 + b[9] * A2; // used for temporary storage
+  MatrixType tmp = A6 * V;
+  tmp += b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  U.noalias() = A * tmp;
+  tmp = b[12] * A6 + b[10] * A4 + b[8] * A2;
+  V.noalias() = A6 * tmp;
+  V += b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
+}
+
+/** \brief Compute the (17,17)-Pad&eacute; approximant to the exponential.
+ *
+ *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
+ *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
+ *
+ *  This function activates only if your long double is double-double or quadruple.
+ */
+#if LDBL_MANT_DIG > 64
+template <typename MatA, typename MatU, typename MatV>
+void matrix_exp_pade17(const MatA& A, MatU& U, MatV& V)
+{
+  typedef typename MatA::PlainObject MatrixType;
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
+  const RealScalar b[] = {830034394580628357120000.L, 415017197290314178560000.L,
+                          100610229646136770560000.L, 15720348382208870400000.L,
+                          1774878043152614400000.L, 153822763739893248000.L, 10608466464820224000.L,
+                          595373117923584000.L, 27563570274240000.L, 1060137318240000.L,
+                          33924394183680.L, 899510451840.L, 19554575040.L, 341863200.L, 4651200.L,
+                          46512.L, 306.L, 1.L};
+  const MatrixType A2 = A * A;
+  const MatrixType A4 = A2 * A2;
+  const MatrixType A6 = A4 * A2;
+  const MatrixType A8 = A4 * A4;
+  V = b[17] * A8 + b[15] * A6 + b[13] * A4 + b[11] * A2; // used for temporary storage
+  MatrixType tmp = A8 * V;
+  tmp += b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 
+    + b[1] * MatrixType::Identity(A.rows(), A.cols());
+  U.noalias() = A * tmp;
+  tmp = b[16] * A8 + b[14] * A6 + b[12] * A4 + b[10] * A2;
+  V.noalias() = tmp * A8;
+  V += b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 
+    + b[0] * MatrixType::Identity(A.rows(), A.cols());
+}
+#endif
+
+template <typename MatrixType, typename RealScalar = typename NumTraits<typename traits<MatrixType>::Scalar>::Real>
+struct matrix_exp_computeUV
+{
+  /** \brief Compute Pad&eacute; approximant to the exponential.
+    *
+    * Computes \c U, \c V and \c squarings such that \f$ (V+U)(V-U)^{-1} \f$ is a Pad&eacute;
+    * approximant of \f$ \exp(2^{-\mbox{squarings}}M) \f$ around \f$ M = 0 \f$, where \f$ M \f$
+    * denotes the matrix \c arg. The degree of the Pad&eacute; approximant and the value of squarings
+    * are chosen such that the approximation error is no more than the round-off error.
+    */
+  static void run(const MatrixType& arg, MatrixType& U, MatrixType& V, int& squarings);
+};
+
+template <typename MatrixType>
+struct matrix_exp_computeUV<MatrixType, float>
+{
+  template <typename ArgType>
+  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
+  {
+    using std::frexp;
+    using std::pow;
+    const float l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
+    squarings = 0;
+    if (l1norm < 4.258730016922831e-001f) {
+      matrix_exp_pade3(arg, U, V);
+    } else if (l1norm < 1.880152677804762e+000f) {
+      matrix_exp_pade5(arg, U, V);
+    } else {
+      const float maxnorm = 3.925724783138660f;
+      frexp(l1norm / maxnorm, &squarings);
+      if (squarings < 0) squarings = 0;
+      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<float>(squarings));
+      matrix_exp_pade7(A, U, V);
+    }
+  }
+};
+
+template <typename MatrixType>
+struct matrix_exp_computeUV<MatrixType, double>
+{
+  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
+  template <typename ArgType>
+  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
+  {
+    using std::frexp;
+    using std::pow;
+    const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
+    squarings = 0;
+    if (l1norm < 1.495585217958292e-002) {
+      matrix_exp_pade3(arg, U, V);
+    } else if (l1norm < 2.539398330063230e-001) {
+      matrix_exp_pade5(arg, U, V);
+    } else if (l1norm < 9.504178996162932e-001) {
+      matrix_exp_pade7(arg, U, V);
+    } else if (l1norm < 2.097847961257068e+000) {
+      matrix_exp_pade9(arg, U, V);
+    } else {
+      const RealScalar maxnorm = 5.371920351148152;
+      frexp(l1norm / maxnorm, &squarings);
+      if (squarings < 0) squarings = 0;
+      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<RealScalar>(squarings));
+      matrix_exp_pade13(A, U, V);
+    }
+  }
+};
+  
+template <typename MatrixType>
+struct matrix_exp_computeUV<MatrixType, long double>
+{
+  template <typename ArgType>
+  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
+  {
+#if   LDBL_MANT_DIG == 53   // double precision
+    matrix_exp_computeUV<MatrixType, double>::run(arg, U, V, squarings);
+  
+#else
+  
+    using std::frexp;
+    using std::pow;
+    const long double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
+    squarings = 0;
+  
+#if LDBL_MANT_DIG <= 64   // extended precision
+  
+    if (l1norm < 4.1968497232266989671e-003L) {
+      matrix_exp_pade3(arg, U, V);
+    } else if (l1norm < 1.1848116734693823091e-001L) {
+      matrix_exp_pade5(arg, U, V);
+    } else if (l1norm < 5.5170388480686700274e-001L) {
+      matrix_exp_pade7(arg, U, V);
+    } else if (l1norm < 1.3759868875587845383e+000L) {
+      matrix_exp_pade9(arg, U, V);
+    } else {
+      const long double maxnorm = 4.0246098906697353063L;
+      frexp(l1norm / maxnorm, &squarings);
+      if (squarings < 0) squarings = 0;
+      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
+      matrix_exp_pade13(A, U, V);
+    }
+  
+#elif LDBL_MANT_DIG <= 106  // double-double
+  
+    if (l1norm < 3.2787892205607026992947488108213e-005L) {
+      matrix_exp_pade3(arg, U, V);
+    } else if (l1norm < 6.4467025060072760084130906076332e-003L) {
+      matrix_exp_pade5(arg, U, V);
+    } else if (l1norm < 6.8988028496595374751374122881143e-002L) {
+      matrix_exp_pade7(arg, U, V);
+    } else if (l1norm < 2.7339737518502231741495857201670e-001L) {
+      matrix_exp_pade9(arg, U, V);
+    } else if (l1norm < 1.3203382096514474905666448850278e+000L) {
+      matrix_exp_pade13(arg, U, V);
+    } else {
+      const long double maxnorm = 3.2579440895405400856599663723517L;
+      frexp(l1norm / maxnorm, &squarings);
+      if (squarings < 0) squarings = 0;
+      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
+      matrix_exp_pade17(A, U, V);
+    }
+  
+#elif LDBL_MANT_DIG <= 113  // quadruple precision
+  
+    if (l1norm < 1.639394610288918690547467954466970e-005L) {
+      matrix_exp_pade3(arg, U, V);
+    } else if (l1norm < 4.253237712165275566025884344433009e-003L) {
+      matrix_exp_pade5(arg, U, V);
+    } else if (l1norm < 5.125804063165764409885122032933142e-002L) {
+      matrix_exp_pade7(arg, U, V);
+    } else if (l1norm < 2.170000765161155195453205651889853e-001L) {
+      matrix_exp_pade9(arg, U, V);
+    } else if (l1norm < 1.125358383453143065081397882891878e+000L) {
+      matrix_exp_pade13(arg, U, V);
+    } else {
+      const long double maxnorm = 2.884233277829519311757165057717815L;
+      frexp(l1norm / maxnorm, &squarings);
+      if (squarings < 0) squarings = 0;
+      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
+      matrix_exp_pade17(A, U, V);
+    }
+  
+#else
+  
+    // this case should be handled in compute()
+    eigen_assert(false && "Bug in MatrixExponential"); 
+  
+#endif
+#endif  // LDBL_MANT_DIG
+  }
+};
+
+template<typename T> struct is_exp_known_type : false_type {};
+template<> struct is_exp_known_type<float> : true_type {};
+template<> struct is_exp_known_type<double> : true_type {};
+#if LDBL_MANT_DIG <= 113
+template<> struct is_exp_known_type<long double> : true_type {};
+#endif
+
+template <typename ArgType, typename ResultType>
+void matrix_exp_compute(const ArgType& arg, ResultType &result, true_type) // natively supported scalar type
+{
+  typedef typename ArgType::PlainObject MatrixType;
+  MatrixType U, V;
+  int squarings;
+  matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
+  MatrixType numer = U + V;
+  MatrixType denom = -U + V;
+  result = denom.partialPivLu().solve(numer);
+  for (int i=0; i<squarings; i++)
+    result *= result;   // undo scaling by repeated squaring
+}
+
+
+/* Computes the matrix exponential
+ *
+ * \param arg    argument of matrix exponential (should be plain object)
+ * \param result variable in which result will be stored
+ */
+template <typename ArgType, typename ResultType>
+void matrix_exp_compute(const ArgType& arg, ResultType &result, false_type) // default
+{
+  typedef typename ArgType::PlainObject MatrixType;
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  typedef typename std::complex<RealScalar> ComplexScalar;
+  result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
+}
+
+} // end namespace Eigen::internal
+
+/** \ingroup MatrixFunctions_Module
+  *
+  * \brief Proxy for the matrix exponential of some matrix (expression).
+  *
+  * \tparam Derived  Type of the argument to the matrix exponential.
+  *
+  * This class holds the argument to the matrix exponential until it is assigned or evaluated for
+  * some other reason (so the argument should not be changed in the meantime). It is the return type
+  * of MatrixBase::exp() and most of the time this is the only way it is used.
+  */
+template<typename Derived> struct MatrixExponentialReturnValue
+: public ReturnByValue<MatrixExponentialReturnValue<Derived> >
+{
+  public:
+    /** \brief Constructor.
+      *
+      * \param src %Matrix (expression) forming the argument of the matrix exponential.
+      */
+    MatrixExponentialReturnValue(const Derived& src) : m_src(src) { }
+
+    /** \brief Compute the matrix exponential.
+      *
+      * \param result the matrix exponential of \p src in the constructor.
+      */
+    template <typename ResultType>
+    inline void evalTo(ResultType& result) const
+    {
+      const typename internal::nested_eval<Derived, 10>::type tmp(m_src);
+      internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type<typename Derived::RealScalar>());
+    }
+
+    Index rows() const { return m_src.rows(); }
+    Index cols() const { return m_src.cols(); }
+
+  protected:
+    const typename internal::ref_selector<Derived>::type m_src;
+};
+
+namespace internal {
+template<typename Derived>
+struct traits<MatrixExponentialReturnValue<Derived> >
+{
+  typedef typename Derived::PlainObject ReturnType;
+};
+}
+
+template <typename Derived>
+const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
+{
+  eigen_assert(rows() == cols());
+  return MatrixExponentialReturnValue<Derived>(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MATRIX_EXPONENTIAL
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
new file mode 100644
index 0000000..cc12ab6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
@@ -0,0 +1,569 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2009-2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIX_FUNCTION_H
+#define EIGEN_MATRIX_FUNCTION_H
+
+#include "StemFunction.h"
+
+
+namespace Eigen { 
+
+namespace internal {
+
+/** \brief Maximum distance allowed between eigenvalues to be considered "close". */
+static const float matrix_function_separation = 0.1f;
+
+/** \ingroup MatrixFunctions_Module
+  * \class MatrixFunctionAtomic
+  * \brief Helper class for computing matrix functions of atomic matrices.
+  *
+  * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
+  */
+template <typename MatrixType>
+class MatrixFunctionAtomic 
+{
+  public:
+
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename stem_function<Scalar>::type StemFunction;
+
+    /** \brief Constructor
+      * \param[in]  f  matrix function to compute.
+      */
+    MatrixFunctionAtomic(StemFunction f) : m_f(f) { }
+
+    /** \brief Compute matrix function of atomic matrix
+      * \param[in]  A  argument of matrix function, should be upper triangular and atomic
+      * \returns  f(A), the matrix function evaluated at the given matrix
+      */
+    MatrixType compute(const MatrixType& A);
+
+  private:
+    StemFunction* m_f;
+};
+
+template <typename MatrixType>
+typename NumTraits<typename MatrixType::Scalar>::Real matrix_function_compute_mu(const MatrixType& A)
+{
+  typedef typename plain_col_type<MatrixType>::type VectorType;
+  Index rows = A.rows();
+  const MatrixType N = MatrixType::Identity(rows, rows) - A;
+  VectorType e = VectorType::Ones(rows);
+  N.template triangularView<Upper>().solveInPlace(e);
+  return e.cwiseAbs().maxCoeff();
+}
+
+template <typename MatrixType>
+MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A)
+{
+  // TODO: Use that A is upper triangular
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  Index rows = A.rows();
+  Scalar avgEival = A.trace() / Scalar(RealScalar(rows));
+  MatrixType Ashifted = A - avgEival * MatrixType::Identity(rows, rows);
+  RealScalar mu = matrix_function_compute_mu(Ashifted);
+  MatrixType F = m_f(avgEival, 0) * MatrixType::Identity(rows, rows);
+  MatrixType P = Ashifted;
+  MatrixType Fincr;
+  for (Index s = 1; double(s) < 1.1 * double(rows) + 10.0; s++) { // upper limit is fairly arbitrary
+    Fincr = m_f(avgEival, static_cast<int>(s)) * P;
+    F += Fincr;
+    P = Scalar(RealScalar(1)/RealScalar(s + 1)) * P * Ashifted;
+
+    // test whether Taylor series converged
+    const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff();
+    const RealScalar Fincr_norm = Fincr.cwiseAbs().rowwise().sum().maxCoeff();
+    if (Fincr_norm < NumTraits<Scalar>::epsilon() * F_norm) {
+      RealScalar delta = 0;
+      RealScalar rfactorial = 1;
+      for (Index r = 0; r < rows; r++) {
+        RealScalar mx = 0;
+        for (Index i = 0; i < rows; i++)
+          mx = (std::max)(mx, std::abs(m_f(Ashifted(i, i) + avgEival, static_cast<int>(s+r))));
+        if (r != 0)
+          rfactorial *= RealScalar(r);
+        delta = (std::max)(delta, mx / rfactorial);
+      }
+      const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff();
+      if (mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm) // series converged
+        break;
+    }
+  }
+  return F;
+}
+
+/** \brief Find cluster in \p clusters containing some value 
+  * \param[in] key Value to find
+  * \returns Iterator to cluster containing \p key, or \c clusters.end() if no cluster in \p m_clusters
+  * contains \p key.
+  */
+template <typename Index, typename ListOfClusters>
+typename ListOfClusters::iterator matrix_function_find_cluster(Index key, ListOfClusters& clusters)
+{
+  typename std::list<Index>::iterator j;
+  for (typename ListOfClusters::iterator i = clusters.begin(); i != clusters.end(); ++i) {
+    j = std::find(i->begin(), i->end(), key);
+    if (j != i->end())
+      return i;
+  }
+  return clusters.end();
+}
+
+/** \brief Partition eigenvalues in clusters of ei'vals close to each other
+  * 
+  * \param[in]  eivals    Eigenvalues
+  * \param[out] clusters  Resulting partition of eigenvalues
+  *
+  * The partition satisfies the following two properties:
+  * # Any eigenvalue in a certain cluster is at most matrix_function_separation() away from another eigenvalue
+  *   in the same cluster.
+  * # The distance between two eigenvalues in different clusters is more than matrix_function_separation().  
+  * The implementation follows Algorithm 4.1 in the paper of Davies and Higham.
+  */
+template <typename EivalsType, typename Cluster>
+void matrix_function_partition_eigenvalues(const EivalsType& eivals, std::list<Cluster>& clusters)
+{
+  typedef typename EivalsType::RealScalar RealScalar;
+  for (Index i=0; i<eivals.rows(); ++i) {
+    // Find cluster containing i-th ei'val, adding a new cluster if necessary
+    typename std::list<Cluster>::iterator qi = matrix_function_find_cluster(i, clusters);
+    if (qi == clusters.end()) {
+      Cluster l;
+      l.push_back(i);
+      clusters.push_back(l);
+      qi = clusters.end();
+      --qi;
+    }
+
+    // Look for other element to add to the set
+    for (Index j=i+1; j<eivals.rows(); ++j) {
+      if (abs(eivals(j) - eivals(i)) <= RealScalar(matrix_function_separation)
+          && std::find(qi->begin(), qi->end(), j) == qi->end()) {
+        typename std::list<Cluster>::iterator qj = matrix_function_find_cluster(j, clusters);
+        if (qj == clusters.end()) {
+          qi->push_back(j);
+        } else {
+          qi->insert(qi->end(), qj->begin(), qj->end());
+          clusters.erase(qj);
+        }
+      }
+    }
+  }
+}
+
+/** \brief Compute size of each cluster given a partitioning */
+template <typename ListOfClusters, typename Index>
+void matrix_function_compute_cluster_size(const ListOfClusters& clusters, Matrix<Index, Dynamic, 1>& clusterSize)
+{
+  const Index numClusters = static_cast<Index>(clusters.size());
+  clusterSize.setZero(numClusters);
+  Index clusterIndex = 0;
+  for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
+    clusterSize[clusterIndex] = cluster->size();
+    ++clusterIndex;
+  }
+}
+
+/** \brief Compute start of each block using clusterSize */
+template <typename VectorType>
+void matrix_function_compute_block_start(const VectorType& clusterSize, VectorType& blockStart)
+{
+  blockStart.resize(clusterSize.rows());
+  blockStart(0) = 0;
+  for (Index i = 1; i < clusterSize.rows(); i++) {
+    blockStart(i) = blockStart(i-1) + clusterSize(i-1);
+  }
+}
+
+/** \brief Compute mapping of eigenvalue indices to cluster indices */
+template <typename EivalsType, typename ListOfClusters, typename VectorType>
+void matrix_function_compute_map(const EivalsType& eivals, const ListOfClusters& clusters, VectorType& eivalToCluster)
+{
+  eivalToCluster.resize(eivals.rows());
+  Index clusterIndex = 0;
+  for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
+    for (Index i = 0; i < eivals.rows(); ++i) {
+      if (std::find(cluster->begin(), cluster->end(), i) != cluster->end()) {
+        eivalToCluster[i] = clusterIndex;
+      }
+    }
+    ++clusterIndex;
+  }
+}
+
+/** \brief Compute permutation which groups ei'vals in same cluster together */
+template <typename DynVectorType, typename VectorType>
+void matrix_function_compute_permutation(const DynVectorType& blockStart, const DynVectorType& eivalToCluster, VectorType& permutation)
+{
+  DynVectorType indexNextEntry = blockStart;
+  permutation.resize(eivalToCluster.rows());
+  for (Index i = 0; i < eivalToCluster.rows(); i++) {
+    Index cluster = eivalToCluster[i];
+    permutation[i] = indexNextEntry[cluster];
+    ++indexNextEntry[cluster];
+  }
+}  
+
+/** \brief Permute Schur decomposition in U and T according to permutation */
+template <typename VectorType, typename MatrixType>
+void matrix_function_permute_schur(VectorType& permutation, MatrixType& U, MatrixType& T)
+{
+  for (Index i = 0; i < permutation.rows() - 1; i++) {
+    Index j;
+    for (j = i; j < permutation.rows(); j++) {
+      if (permutation(j) == i) break;
+    }
+    eigen_assert(permutation(j) == i);
+    for (Index k = j-1; k >= i; k--) {
+      JacobiRotation<typename MatrixType::Scalar> rotation;
+      rotation.makeGivens(T(k, k+1), T(k+1, k+1) - T(k, k));
+      T.applyOnTheLeft(k, k+1, rotation.adjoint());
+      T.applyOnTheRight(k, k+1, rotation);
+      U.applyOnTheRight(k, k+1, rotation);
+      std::swap(permutation.coeffRef(k), permutation.coeffRef(k+1));
+    }
+  }
+}
+
+/** \brief Compute block diagonal part of matrix function.
+  *
+  * This routine computes the matrix function applied to the block diagonal part of \p T (which should be
+  * upper triangular), with the blocking given by \p blockStart and \p clusterSize. The matrix function of
+  * each diagonal block is computed by \p atomic. The off-diagonal parts of \p fT are set to zero.
+  */
+template <typename MatrixType, typename AtomicType, typename VectorType>
+void matrix_function_compute_block_atomic(const MatrixType& T, AtomicType& atomic, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
+{ 
+  fT.setZero(T.rows(), T.cols());
+  for (Index i = 0; i < clusterSize.rows(); ++i) {
+    fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
+      = atomic.compute(T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)));
+  }
+}
+
+/** \brief Solve a triangular Sylvester equation AX + XB = C 
+  *
+  * \param[in]  A  the matrix A; should be square and upper triangular
+  * \param[in]  B  the matrix B; should be square and upper triangular
+  * \param[in]  C  the matrix C; should have correct size.
+  *
+  * \returns the solution X.
+  *
+  * If A is m-by-m and B is n-by-n, then both C and X are m-by-n.  The (i,j)-th component of the Sylvester
+  * equation is
+  * \f[ 
+  *     \sum_{k=i}^m A_{ik} X_{kj} + \sum_{k=1}^j X_{ik} B_{kj} = C_{ij}. 
+  * \f]
+  * This can be re-arranged to yield:
+  * \f[ 
+  *     X_{ij} = \frac{1}{A_{ii} + B_{jj}} \Bigl( C_{ij}
+  *     - \sum_{k=i+1}^m A_{ik} X_{kj} - \sum_{k=1}^{j-1} X_{ik} B_{kj} \Bigr).
+  * \f]
+  * It is assumed that A and B are such that the numerator is never zero (otherwise the Sylvester equation
+  * does not have a unique solution). In that case, these equations can be evaluated in the order 
+  * \f$ i=m,\ldots,1 \f$ and \f$ j=1,\ldots,n \f$.
+  */
+template <typename MatrixType>
+MatrixType matrix_function_solve_triangular_sylvester(const MatrixType& A, const MatrixType& B, const MatrixType& C)
+{
+  eigen_assert(A.rows() == A.cols());
+  eigen_assert(A.isUpperTriangular());
+  eigen_assert(B.rows() == B.cols());
+  eigen_assert(B.isUpperTriangular());
+  eigen_assert(C.rows() == A.rows());
+  eigen_assert(C.cols() == B.rows());
+
+  typedef typename MatrixType::Scalar Scalar;
+
+  Index m = A.rows();
+  Index n = B.rows();
+  MatrixType X(m, n);
+
+  for (Index i = m - 1; i >= 0; --i) {
+    for (Index j = 0; j < n; ++j) {
+
+      // Compute AX = \sum_{k=i+1}^m A_{ik} X_{kj}
+      Scalar AX;
+      if (i == m - 1) {
+	AX = 0; 
+      } else {
+	Matrix<Scalar,1,1> AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i);
+	AX = AXmatrix(0,0);
+      }
+
+      // Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj}
+      Scalar XB;
+      if (j == 0) {
+	XB = 0; 
+      } else {
+	Matrix<Scalar,1,1> XBmatrix = X.row(i).head(j) * B.col(j).head(j);
+	XB = XBmatrix(0,0);
+      }
+
+      X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j));
+    }
+  }
+  return X;
+}
+
+/** \brief Compute part of matrix function above block diagonal.
+  *
+  * This routine completes the computation of \p fT, denoting a matrix function applied to the triangular
+  * matrix \p T. It assumes that the block diagonal part of \p fT has already been computed. The part below
+  * the diagonal is zero, because \p T is upper triangular.
+  */
+template <typename MatrixType, typename VectorType>
+void matrix_function_compute_above_diagonal(const MatrixType& T, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
+{ 
+  typedef internal::traits<MatrixType> Traits;
+  typedef typename MatrixType::Scalar Scalar;
+  static const int Options = MatrixType::Options;
+  typedef Matrix<Scalar, Dynamic, Dynamic, Options, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime> DynMatrixType;
+
+  for (Index k = 1; k < clusterSize.rows(); k++) {
+    for (Index i = 0; i < clusterSize.rows() - k; i++) {
+      // compute (i, i+k) block
+      DynMatrixType A = T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i));
+      DynMatrixType B = -T.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
+      DynMatrixType C = fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
+        * T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k));
+      C -= T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
+        * fT.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
+      for (Index m = i + 1; m < i + k; m++) {
+        C += fT.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
+          * T.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
+        C -= T.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
+          * fT.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
+      }
+      fT.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
+        = matrix_function_solve_triangular_sylvester(A, B, C);
+    }
+  }
+}
+
+/** \ingroup MatrixFunctions_Module
+  * \brief Class for computing matrix functions.
+  * \tparam  MatrixType  type of the argument of the matrix function,
+  *                      expected to be an instantiation of the Matrix class template.
+  * \tparam  AtomicType  type for computing matrix function of atomic blocks.
+  * \tparam  IsComplex   used internally to select correct specialization.
+  *
+  * This class implements the Schur-Parlett algorithm for computing matrix functions. The spectrum of the
+  * matrix is divided in clustered of eigenvalues that lies close together. This class delegates the
+  * computation of the matrix function on every block corresponding to these clusters to an object of type
+  * \p AtomicType and uses these results to compute the matrix function of the whole matrix. The class
+  * \p AtomicType should have a \p compute() member function for computing the matrix function of a block.
+  *
+  * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic
+  */
+template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
+struct matrix_function_compute
+{  
+    /** \brief Compute the matrix function.
+      *
+      * \param[in]  A       argument of matrix function, should be a square matrix.
+      * \param[in]  atomic  class for computing matrix function of atomic blocks.
+      * \param[out] result  the function \p f applied to \p A, as
+      * specified in the constructor.
+      *
+      * See MatrixBase::matrixFunction() for details on how this computation
+      * is implemented.
+      */
+    template <typename AtomicType, typename ResultType> 
+    static void run(const MatrixType& A, AtomicType& atomic, ResultType &result);    
+};
+
+/** \internal \ingroup MatrixFunctions_Module 
+  * \brief Partial specialization of MatrixFunction for real matrices
+  *
+  * This converts the real matrix to a complex matrix, compute the matrix function of that matrix, and then
+  * converts the result back to a real matrix.
+  */
+template <typename MatrixType>
+struct matrix_function_compute<MatrixType, 0>
+{  
+  template <typename MatA, typename AtomicType, typename ResultType>
+  static void run(const MatA& A, AtomicType& atomic, ResultType &result)
+  {
+    typedef internal::traits<MatrixType> Traits;
+    typedef typename Traits::Scalar Scalar;
+    static const int Rows = Traits::RowsAtCompileTime, Cols = Traits::ColsAtCompileTime;
+    static const int MaxRows = Traits::MaxRowsAtCompileTime, MaxCols = Traits::MaxColsAtCompileTime;
+
+    typedef std::complex<Scalar> ComplexScalar;
+    typedef Matrix<ComplexScalar, Rows, Cols, 0, MaxRows, MaxCols> ComplexMatrix;
+
+    ComplexMatrix CA = A.template cast<ComplexScalar>();
+    ComplexMatrix Cresult;
+    matrix_function_compute<ComplexMatrix>::run(CA, atomic, Cresult);
+    result = Cresult.real();
+  }
+};
+
+/** \internal \ingroup MatrixFunctions_Module 
+  * \brief Partial specialization of MatrixFunction for complex matrices
+  */
+template <typename MatrixType>
+struct matrix_function_compute<MatrixType, 1>
+{
+  template <typename MatA, typename AtomicType, typename ResultType>
+  static void run(const MatA& A, AtomicType& atomic, ResultType &result)
+  {
+    typedef internal::traits<MatrixType> Traits;
+    
+    // compute Schur decomposition of A
+    const ComplexSchur<MatrixType> schurOfA(A);
+    eigen_assert(schurOfA.info()==Success);
+    MatrixType T = schurOfA.matrixT();
+    MatrixType U = schurOfA.matrixU();
+
+    // partition eigenvalues into clusters of ei'vals "close" to each other
+    std::list<std::list<Index> > clusters; 
+    matrix_function_partition_eigenvalues(T.diagonal(), clusters);
+
+    // compute size of each cluster
+    Matrix<Index, Dynamic, 1> clusterSize;
+    matrix_function_compute_cluster_size(clusters, clusterSize);
+
+    // blockStart[i] is row index at which block corresponding to i-th cluster starts 
+    Matrix<Index, Dynamic, 1> blockStart; 
+    matrix_function_compute_block_start(clusterSize, blockStart);
+
+    // compute map so that eivalToCluster[i] = j means that i-th ei'val is in j-th cluster 
+    Matrix<Index, Dynamic, 1> eivalToCluster;
+    matrix_function_compute_map(T.diagonal(), clusters, eivalToCluster);
+
+    // compute permutation which groups ei'vals in same cluster together 
+    Matrix<Index, Traits::RowsAtCompileTime, 1> permutation;
+    matrix_function_compute_permutation(blockStart, eivalToCluster, permutation);
+
+    // permute Schur decomposition
+    matrix_function_permute_schur(permutation, U, T);
+
+    // compute result
+    MatrixType fT; // matrix function applied to T
+    matrix_function_compute_block_atomic(T, atomic, blockStart, clusterSize, fT);
+    matrix_function_compute_above_diagonal(T, blockStart, clusterSize, fT);
+    result = U * (fT.template triangularView<Upper>() * U.adjoint());
+  }
+};
+
+} // end of namespace internal
+
+/** \ingroup MatrixFunctions_Module
+  *
+  * \brief Proxy for the matrix function of some matrix (expression).
+  *
+  * \tparam Derived  Type of the argument to the matrix function.
+  *
+  * This class holds the argument to the matrix function until it is assigned or evaluated for some other
+  * reason (so the argument should not be changed in the meantime). It is the return type of
+  * matrixBase::matrixFunction() and related functions and most of the time this is the only way it is used.
+  */
+template<typename Derived> class MatrixFunctionReturnValue
+: public ReturnByValue<MatrixFunctionReturnValue<Derived> >
+{
+  public:
+    typedef typename Derived::Scalar Scalar;
+    typedef typename internal::stem_function<Scalar>::type StemFunction;
+
+  protected:
+    typedef typename internal::ref_selector<Derived>::type DerivedNested;
+
+  public:
+
+    /** \brief Constructor.
+      *
+      * \param[in] A  %Matrix (expression) forming the argument of the matrix function.
+      * \param[in] f  Stem function for matrix function under consideration.
+      */
+    MatrixFunctionReturnValue(const Derived& A, StemFunction f) : m_A(A), m_f(f) { }
+
+    /** \brief Compute the matrix function.
+      *
+      * \param[out] result \p f applied to \p A, where \p f and \p A are as in the constructor.
+      */
+    template <typename ResultType>
+    inline void evalTo(ResultType& result) const
+    {
+      typedef typename internal::nested_eval<Derived, 10>::type NestedEvalType;
+      typedef typename internal::remove_all<NestedEvalType>::type NestedEvalTypeClean;
+      typedef internal::traits<NestedEvalTypeClean> Traits;
+      typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
+      typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime> DynMatrixType;
+
+      typedef internal::MatrixFunctionAtomic<DynMatrixType> AtomicType;
+      AtomicType atomic(m_f);
+
+      internal::matrix_function_compute<typename NestedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
+    }
+
+    Index rows() const { return m_A.rows(); }
+    Index cols() const { return m_A.cols(); }
+
+  private:
+    const DerivedNested m_A;
+    StemFunction *m_f;
+};
+
+namespace internal {
+template<typename Derived>
+struct traits<MatrixFunctionReturnValue<Derived> >
+{
+  typedef typename Derived::PlainObject ReturnType;
+};
+}
+
+
+/********** MatrixBase methods **********/
+
+
+template <typename Derived>
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
+{
+  eigen_assert(rows() == cols());
+  return MatrixFunctionReturnValue<Derived>(derived(), f);
+}
+
+template <typename Derived>
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
+{
+  eigen_assert(rows() == cols());
+  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
+  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sin<ComplexScalar>);
+}
+
+template <typename Derived>
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
+{
+  eigen_assert(rows() == cols());
+  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
+  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cos<ComplexScalar>);
+}
+
+template <typename Derived>
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
+{
+  eigen_assert(rows() == cols());
+  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
+  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sinh<ComplexScalar>);
+}
+
+template <typename Derived>
+const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
+{
+  eigen_assert(rows() == cols());
+  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
+  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cosh<ComplexScalar>);
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MATRIX_FUNCTION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
new file mode 100644
index 0000000..e917013
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
@@ -0,0 +1,373 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
+// Copyright (C) 2011 Chen-Pang He <jdh8@ms63.hinet.net>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIX_LOGARITHM
+#define EIGEN_MATRIX_LOGARITHM
+
+namespace Eigen { 
+
+namespace internal { 
+
+template <typename Scalar>
+struct matrix_log_min_pade_degree 
+{
+  static const int value = 3;
+};
+
+template <typename Scalar>
+struct matrix_log_max_pade_degree 
+{
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  static const int value = std::numeric_limits<RealScalar>::digits<= 24?  5:  // single precision
+                           std::numeric_limits<RealScalar>::digits<= 53?  7:  // double precision
+                           std::numeric_limits<RealScalar>::digits<= 64?  8:  // extended precision
+                           std::numeric_limits<RealScalar>::digits<=106? 10:  // double-double
+                                                                         11;  // quadruple precision
+};
+
+/** \brief Compute logarithm of 2x2 triangular matrix. */
+template <typename MatrixType>
+void matrix_log_compute_2x2(const MatrixType& A, MatrixType& result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename MatrixType::RealScalar RealScalar;
+  using std::abs;
+  using std::ceil;
+  using std::imag;
+  using std::log;
+
+  Scalar logA00 = log(A(0,0));
+  Scalar logA11 = log(A(1,1));
+
+  result(0,0) = logA00;
+  result(1,0) = Scalar(0);
+  result(1,1) = logA11;
+
+  Scalar y = A(1,1) - A(0,0);
+  if (y==Scalar(0))
+  {
+    result(0,1) = A(0,1) / A(0,0);
+  }
+  else if ((abs(A(0,0)) < RealScalar(0.5)*abs(A(1,1))) || (abs(A(0,0)) > 2*abs(A(1,1))))
+  {
+    result(0,1) = A(0,1) * (logA11 - logA00) / y;
+  }
+  else
+  {
+    // computation in previous branch is inaccurate if A(1,1) \approx A(0,0)
+    RealScalar unwindingNumber = ceil((imag(logA11 - logA00) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI));
+    result(0,1) = A(0,1) * (numext::log1p(y/A(0,0)) + Scalar(0,RealScalar(2*EIGEN_PI)*unwindingNumber)) / y;
+  }
+}
+
+/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = float) */
+inline int matrix_log_get_pade_degree(float normTminusI)
+{
+  const float maxNormForPade[] = { 2.5111573934555054e-1 /* degree = 3 */ , 4.0535837411880493e-1,
+            5.3149729967117310e-1 };
+  const int minPadeDegree = matrix_log_min_pade_degree<float>::value;
+  const int maxPadeDegree = matrix_log_max_pade_degree<float>::value;
+  int degree = minPadeDegree;
+  for (; degree <= maxPadeDegree; ++degree) 
+    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
+      break;
+  return degree;
+}
+
+/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = double) */
+inline int matrix_log_get_pade_degree(double normTminusI)
+{
+  const double maxNormForPade[] = { 1.6206284795015624e-2 /* degree = 3 */ , 5.3873532631381171e-2,
+            1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
+  const int minPadeDegree = matrix_log_min_pade_degree<double>::value;
+  const int maxPadeDegree = matrix_log_max_pade_degree<double>::value;
+  int degree = minPadeDegree;
+  for (; degree <= maxPadeDegree; ++degree)
+    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
+      break;
+  return degree;
+}
+
+/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = long double) */
+inline int matrix_log_get_pade_degree(long double normTminusI)
+{
+#if   LDBL_MANT_DIG == 53         // double precision
+  const long double maxNormForPade[] = { 1.6206284795015624e-2L /* degree = 3 */ , 5.3873532631381171e-2L,
+            1.1352802267628681e-1L, 1.8662860613541288e-1L, 2.642960831111435e-1L };
+#elif LDBL_MANT_DIG <= 64         // extended precision
+  const long double maxNormForPade[] = { 5.48256690357782863103e-3L /* degree = 3 */, 2.34559162387971167321e-2L,
+            5.84603923897347449857e-2L, 1.08486423756725170223e-1L, 1.68385767881294446649e-1L,
+            2.32777776523703892094e-1L };
+#elif LDBL_MANT_DIG <= 106        // double-double
+  const long double maxNormForPade[] = { 8.58970550342939562202529664318890e-5L /* degree = 3 */,
+            9.34074328446359654039446552677759e-4L, 4.26117194647672175773064114582860e-3L,
+            1.21546224740281848743149666560464e-2L, 2.61100544998339436713088248557444e-2L,
+            4.66170074627052749243018566390567e-2L, 7.32585144444135027565872014932387e-2L,
+            1.05026503471351080481093652651105e-1L };
+#else                             // quadruple precision
+  const long double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5L /* degree = 3 */,
+            5.8853168473544560470387769480192666e-4L, 2.9216120366601315391789493628113520e-3L,
+            8.8415758124319434347116734705174308e-3L, 1.9850836029449446668518049562565291e-2L,
+            3.6688019729653446926585242192447447e-2L, 5.9290962294020186998954055264528393e-2L,
+            8.6998436081634343903250580992127677e-2L, 1.1880960220216759245467951592883642e-1L };
+#endif
+  const int minPadeDegree = matrix_log_min_pade_degree<long double>::value;
+  const int maxPadeDegree = matrix_log_max_pade_degree<long double>::value;
+  int degree = minPadeDegree;
+  for (; degree <= maxPadeDegree; ++degree)
+    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
+      break;
+  return degree;
+}
+
+/* \brief Compute Pade approximation to matrix logarithm */
+template <typename MatrixType>
+void matrix_log_compute_pade(MatrixType& result, const MatrixType& T, int degree)
+{
+  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
+  const int minPadeDegree = 3;
+  const int maxPadeDegree = 11;
+  assert(degree >= minPadeDegree && degree <= maxPadeDegree);
+  // FIXME this creates float-conversion-warnings if these are enabled.
+  // Either manually convert each value, or disable the warning locally
+  const RealScalar nodes[][maxPadeDegree] = { 
+    { 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L,  // degree 3
+      0.8872983346207416885179265399782400L }, 
+    { 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L,  // degree 4
+      0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L },
+    { 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L,  // degree 5
+      0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
+      0.9530899229693319963988134391496965L },
+    { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L,  // degree 6
+      0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
+      0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L },
+    { 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L,  // degree 7
+      0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
+      0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
+      0.9745539561713792622630948420239256L },
+    { 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L,  // degree 8
+      0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
+      0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
+      0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L },
+    { 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L,  // degree 9
+      0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
+      0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
+      0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
+      0.9840801197538130449177881014518364L },
+    { 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L,  // degree 10
+      0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
+      0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
+      0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
+      0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L },
+    { 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L,  // degree 11
+      0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
+      0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
+      0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
+      0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
+      0.9891143290730284964019690005614287L } };
+
+  const RealScalar weights[][maxPadeDegree] = { 
+    { 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L,  // degree 3
+      0.2777777777777777777777777777777778L },
+    { 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L,  // degree 4
+      0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L },
+    { 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L,  // degree 5
+      0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
+      0.1184634425280945437571320203599587L },
+    { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L,  // degree 6
+      0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
+      0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L },
+    { 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L,  // degree 7
+      0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
+      0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
+      0.0647424830844348466353057163395410L },
+    { 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L,  // degree 8
+      0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
+      0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
+      0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L },
+    { 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L,  // degree 9
+      0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
+      0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
+      0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
+      0.0406371941807872059859460790552618L },
+    { 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L,  // degree 10
+      0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
+      0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
+      0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
+      0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L },
+    { 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L,  // degree 11
+      0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
+      0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
+      0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
+      0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
+      0.0278342835580868332413768602212743L } };
+
+  MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
+  result.setZero(T.rows(), T.rows());
+  for (int k = 0; k < degree; ++k) {
+    RealScalar weight = weights[degree-minPadeDegree][k];
+    RealScalar node = nodes[degree-minPadeDegree][k];
+    result += weight * (MatrixType::Identity(T.rows(), T.rows()) + node * TminusI)
+                       .template triangularView<Upper>().solve(TminusI);
+  }
+} 
+
+/** \brief Compute logarithm of triangular matrices with size > 2. 
+  * \details This uses a inverse scale-and-square algorithm. */
+template <typename MatrixType>
+void matrix_log_compute_big(const MatrixType& A, MatrixType& result)
+{
+  typedef typename MatrixType::Scalar Scalar;
+  typedef typename NumTraits<Scalar>::Real RealScalar;
+  using std::pow;
+
+  int numberOfSquareRoots = 0;
+  int numberOfExtraSquareRoots = 0;
+  int degree;
+  MatrixType T = A, sqrtT;
+
+  const int maxPadeDegree = matrix_log_max_pade_degree<Scalar>::value;
+  const RealScalar maxNormForPade = RealScalar(
+                                    maxPadeDegree<= 5? 5.3149729967117310e-1L:                    // single precision
+                                    maxPadeDegree<= 7? 2.6429608311114350e-1L:                    // double precision
+                                    maxPadeDegree<= 8? 2.32777776523703892094e-1L:                // extended precision
+                                    maxPadeDegree<=10? 1.05026503471351080481093652651105e-1L:    // double-double
+                                                       1.1880960220216759245467951592883642e-1L); // quadruple precision
+
+  while (true) {
+    RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
+    if (normTminusI < maxNormForPade) {
+      degree = matrix_log_get_pade_degree(normTminusI);
+      int degree2 = matrix_log_get_pade_degree(normTminusI / RealScalar(2));
+      if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) 
+        break;
+      ++numberOfExtraSquareRoots;
+    }
+    matrix_sqrt_triangular(T, sqrtT);
+    T = sqrtT.template triangularView<Upper>();
+    ++numberOfSquareRoots;
+  }
+
+  matrix_log_compute_pade(result, T, degree);
+  result *= pow(RealScalar(2), RealScalar(numberOfSquareRoots)); // TODO replace by bitshift if possible
+}
+
+/** \ingroup MatrixFunctions_Module
+  * \class MatrixLogarithmAtomic
+  * \brief Helper class for computing matrix logarithm of atomic matrices.
+  *
+  * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
+  *
+  * \sa class MatrixFunctionAtomic, MatrixBase::log()
+  */
+template <typename MatrixType>
+class MatrixLogarithmAtomic
+{
+public:
+  /** \brief Compute matrix logarithm of atomic matrix
+    * \param[in]  A  argument of matrix logarithm, should be upper triangular and atomic
+    * \returns  The logarithm of \p A.
+    */
+  MatrixType compute(const MatrixType& A);
+};
+
+template <typename MatrixType>
+MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A)
+{
+  using std::log;
+  MatrixType result(A.rows(), A.rows());
+  if (A.rows() == 1)
+    result(0,0) = log(A(0,0));
+  else if (A.rows() == 2)
+    matrix_log_compute_2x2(A, result);
+  else
+    matrix_log_compute_big(A, result);
+  return result;
+}
+
+} // end of namespace internal
+
+/** \ingroup MatrixFunctions_Module
+  *
+  * \brief Proxy for the matrix logarithm of some matrix (expression).
+  *
+  * \tparam Derived  Type of the argument to the matrix function.
+  *
+  * This class holds the argument to the matrix function until it is
+  * assigned or evaluated for some other reason (so the argument
+  * should not be changed in the meantime). It is the return type of
+  * MatrixBase::log() and most of the time this is the only way it
+  * is used.
+  */
+template<typename Derived> class MatrixLogarithmReturnValue
+: public ReturnByValue<MatrixLogarithmReturnValue<Derived> >
+{
+public:
+  typedef typename Derived::Scalar Scalar;
+  typedef typename Derived::Index Index;
+
+protected:
+  typedef typename internal::ref_selector<Derived>::type DerivedNested;
+
+public:
+
+  /** \brief Constructor.
+    *
+    * \param[in]  A  %Matrix (expression) forming the argument of the matrix logarithm.
+    */
+  explicit MatrixLogarithmReturnValue(const Derived& A) : m_A(A) { }
+  
+  /** \brief Compute the matrix logarithm.
+    *
+    * \param[out]  result  Logarithm of \c A, where \c A is as specified in the constructor.
+    */
+  template <typename ResultType>
+  inline void evalTo(ResultType& result) const
+  {
+    typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
+    typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
+    typedef internal::traits<DerivedEvalTypeClean> Traits;
+    typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
+    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, Traits::RowsAtCompileTime, Traits::ColsAtCompileTime> DynMatrixType;
+    typedef internal::MatrixLogarithmAtomic<DynMatrixType> AtomicType;
+    AtomicType atomic;
+    
+    internal::matrix_function_compute<typename DerivedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
+  }
+
+  Index rows() const { return m_A.rows(); }
+  Index cols() const { return m_A.cols(); }
+  
+private:
+  const DerivedNested m_A;
+};
+
+namespace internal {
+  template<typename Derived>
+  struct traits<MatrixLogarithmReturnValue<Derived> >
+  {
+    typedef typename Derived::PlainObject ReturnType;
+  };
+}
+
+
+/********** MatrixBase method **********/
+
+
+template <typename Derived>
+const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
+{
+  eigen_assert(rows() == cols());
+  return MatrixLogarithmReturnValue<Derived>(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MATRIX_LOGARITHM
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
new file mode 100644
index 0000000..d7672d7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
@@ -0,0 +1,705 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2012, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIX_POWER
+#define EIGEN_MATRIX_POWER
+
+namespace Eigen {
+
+template<typename MatrixType> class MatrixPower;
+
+/**
+ * \ingroup MatrixFunctions_Module
+ *
+ * \brief Proxy for the matrix power of some matrix.
+ *
+ * \tparam MatrixType  type of the base, a matrix.
+ *
+ * This class holds the arguments to the matrix power until it is
+ * assigned or evaluated for some other reason (so the argument
+ * should not be changed in the meantime). It is the return type of
+ * MatrixPower::operator() and related functions and most of the
+ * time this is the only way it is used.
+ */
+/* TODO This class is only used by MatrixPower, so it should be nested
+ * into MatrixPower, like MatrixPower::ReturnValue. However, my
+ * compiler complained about unused template parameter in the
+ * following declaration in namespace internal.
+ *
+ * template<typename MatrixType>
+ * struct traits<MatrixPower<MatrixType>::ReturnValue>;
+ */
+template<typename MatrixType>
+class MatrixPowerParenthesesReturnValue : public ReturnByValue< MatrixPowerParenthesesReturnValue<MatrixType> >
+{
+  public:
+    typedef typename MatrixType::RealScalar RealScalar;
+
+    /**
+     * \brief Constructor.
+     *
+     * \param[in] pow  %MatrixPower storing the base.
+     * \param[in] p    scalar, the exponent of the matrix power.
+     */
+    MatrixPowerParenthesesReturnValue(MatrixPower<MatrixType>& pow, RealScalar p) : m_pow(pow), m_p(p)
+    { }
+
+    /**
+     * \brief Compute the matrix power.
+     *
+     * \param[out] result
+     */
+    template<typename ResultType>
+    inline void evalTo(ResultType& result) const
+    { m_pow.compute(result, m_p); }
+
+    Index rows() const { return m_pow.rows(); }
+    Index cols() const { return m_pow.cols(); }
+
+  private:
+    MatrixPower<MatrixType>& m_pow;
+    const RealScalar m_p;
+};
+
+/**
+ * \ingroup MatrixFunctions_Module
+ *
+ * \brief Class for computing matrix powers.
+ *
+ * \tparam MatrixType  type of the base, expected to be an instantiation
+ * of the Matrix class template.
+ *
+ * This class is capable of computing triangular real/complex matrices
+ * raised to a power in the interval \f$ (-1, 1) \f$.
+ *
+ * \note Currently this class is only used by MatrixPower. One may
+ * insist that this be nested into MatrixPower. This class is here to
+ * facilitate future development of triangular matrix functions.
+ */
+template<typename MatrixType>
+class MatrixPowerAtomic : internal::noncopyable
+{
+  private:
+    enum {
+      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
+      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime
+    };
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+    typedef std::complex<RealScalar> ComplexScalar;
+    typedef Block<MatrixType,Dynamic,Dynamic> ResultType;
+
+    const MatrixType& m_A;
+    RealScalar m_p;
+
+    void computePade(int degree, const MatrixType& IminusT, ResultType& res) const;
+    void compute2x2(ResultType& res, RealScalar p) const;
+    void computeBig(ResultType& res) const;
+    static int getPadeDegree(float normIminusT);
+    static int getPadeDegree(double normIminusT);
+    static int getPadeDegree(long double normIminusT);
+    static ComplexScalar computeSuperDiag(const ComplexScalar&, const ComplexScalar&, RealScalar p);
+    static RealScalar computeSuperDiag(RealScalar, RealScalar, RealScalar p);
+
+  public:
+    /**
+     * \brief Constructor.
+     *
+     * \param[in] T  the base of the matrix power.
+     * \param[in] p  the exponent of the matrix power, should be in
+     * \f$ (-1, 1) \f$.
+     *
+     * The class stores a reference to T, so it should not be changed
+     * (or destroyed) before evaluation. Only the upper triangular
+     * part of T is read.
+     */
+    MatrixPowerAtomic(const MatrixType& T, RealScalar p);
+    
+    /**
+     * \brief Compute the matrix power.
+     *
+     * \param[out] res  \f$ A^p \f$ where A and p are specified in the
+     * constructor.
+     */
+    void compute(ResultType& res) const;
+};
+
+template<typename MatrixType>
+MatrixPowerAtomic<MatrixType>::MatrixPowerAtomic(const MatrixType& T, RealScalar p) :
+  m_A(T), m_p(p)
+{
+  eigen_assert(T.rows() == T.cols());
+  eigen_assert(p > -1 && p < 1);
+}
+
+template<typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::compute(ResultType& res) const
+{
+  using std::pow;
+  switch (m_A.rows()) {
+    case 0:
+      break;
+    case 1:
+      res(0,0) = pow(m_A(0,0), m_p);
+      break;
+    case 2:
+      compute2x2(res, m_p);
+      break;
+    default:
+      computeBig(res);
+  }
+}
+
+template<typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::computePade(int degree, const MatrixType& IminusT, ResultType& res) const
+{
+  int i = 2*degree;
+  res = (m_p-RealScalar(degree)) / RealScalar(2*i-2) * IminusT;
+
+  for (--i; i; --i) {
+    res = (MatrixType::Identity(IminusT.rows(), IminusT.cols()) + res).template triangularView<Upper>()
+	.solve((i==1 ? -m_p : i&1 ? (-m_p-RealScalar(i/2))/RealScalar(2*i) : (m_p-RealScalar(i/2))/RealScalar(2*i-2)) * IminusT).eval();
+  }
+  res += MatrixType::Identity(IminusT.rows(), IminusT.cols());
+}
+
+// This function assumes that res has the correct size (see bug 614)
+template<typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::compute2x2(ResultType& res, RealScalar p) const
+{
+  using std::abs;
+  using std::pow;
+  res.coeffRef(0,0) = pow(m_A.coeff(0,0), p);
+
+  for (Index i=1; i < m_A.cols(); ++i) {
+    res.coeffRef(i,i) = pow(m_A.coeff(i,i), p);
+    if (m_A.coeff(i-1,i-1) == m_A.coeff(i,i))
+      res.coeffRef(i-1,i) = p * pow(m_A.coeff(i,i), p-1);
+    else if (2*abs(m_A.coeff(i-1,i-1)) < abs(m_A.coeff(i,i)) || 2*abs(m_A.coeff(i,i)) < abs(m_A.coeff(i-1,i-1)))
+      res.coeffRef(i-1,i) = (res.coeff(i,i)-res.coeff(i-1,i-1)) / (m_A.coeff(i,i)-m_A.coeff(i-1,i-1));
+    else
+      res.coeffRef(i-1,i) = computeSuperDiag(m_A.coeff(i,i), m_A.coeff(i-1,i-1), p);
+    res.coeffRef(i-1,i) *= m_A.coeff(i-1,i);
+  }
+}
+
+template<typename MatrixType>
+void MatrixPowerAtomic<MatrixType>::computeBig(ResultType& res) const
+{
+  using std::ldexp;
+  const int digits = std::numeric_limits<RealScalar>::digits;
+  const RealScalar maxNormForPade = RealScalar(
+                                    digits <=  24? 4.3386528e-1L                            // single precision
+                                  : digits <=  53? 2.789358995219730e-1L                    // double precision
+                                  : digits <=  64? 2.4471944416607995472e-1L                // extended precision
+                                  : digits <= 106? 1.1016843812851143391275867258512e-1L    // double-double
+                                  :                9.134603732914548552537150753385375e-2L); // quadruple precision
+  MatrixType IminusT, sqrtT, T = m_A.template triangularView<Upper>();
+  RealScalar normIminusT;
+  int degree, degree2, numberOfSquareRoots = 0;
+  bool hasExtraSquareRoot = false;
+
+  for (Index i=0; i < m_A.cols(); ++i)
+    eigen_assert(m_A(i,i) != RealScalar(0));
+
+  while (true) {
+    IminusT = MatrixType::Identity(m_A.rows(), m_A.cols()) - T;
+    normIminusT = IminusT.cwiseAbs().colwise().sum().maxCoeff();
+    if (normIminusT < maxNormForPade) {
+      degree = getPadeDegree(normIminusT);
+      degree2 = getPadeDegree(normIminusT/2);
+      if (degree - degree2 <= 1 || hasExtraSquareRoot)
+	break;
+      hasExtraSquareRoot = true;
+    }
+    matrix_sqrt_triangular(T, sqrtT);
+    T = sqrtT.template triangularView<Upper>();
+    ++numberOfSquareRoots;
+  }
+  computePade(degree, IminusT, res);
+
+  for (; numberOfSquareRoots; --numberOfSquareRoots) {
+    compute2x2(res, ldexp(m_p, -numberOfSquareRoots));
+    res = res.template triangularView<Upper>() * res;
+  }
+  compute2x2(res, m_p);
+}
+  
+template<typename MatrixType>
+inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(float normIminusT)
+{
+  const float maxNormForPade[] = { 2.8064004e-1f /* degree = 3 */ , 4.3386528e-1f };
+  int degree = 3;
+  for (; degree <= 4; ++degree)
+    if (normIminusT <= maxNormForPade[degree - 3])
+      break;
+  return degree;
+}
+
+template<typename MatrixType>
+inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(double normIminusT)
+{
+  const double maxNormForPade[] = { 1.884160592658218e-2 /* degree = 3 */ , 6.038881904059573e-2, 1.239917516308172e-1,
+      1.999045567181744e-1, 2.789358995219730e-1 };
+  int degree = 3;
+  for (; degree <= 7; ++degree)
+    if (normIminusT <= maxNormForPade[degree - 3])
+      break;
+  return degree;
+}
+
+template<typename MatrixType>
+inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(long double normIminusT)
+{
+#if   LDBL_MANT_DIG == 53
+  const int maxPadeDegree = 7;
+  const double maxNormForPade[] = { 1.884160592658218e-2L /* degree = 3 */ , 6.038881904059573e-2L, 1.239917516308172e-1L,
+      1.999045567181744e-1L, 2.789358995219730e-1L };
+#elif LDBL_MANT_DIG <= 64
+  const int maxPadeDegree = 8;
+  const long double maxNormForPade[] = { 6.3854693117491799460e-3L /* degree = 3 */ , 2.6394893435456973676e-2L,
+      6.4216043030404063729e-2L, 1.1701165502926694307e-1L, 1.7904284231268670284e-1L, 2.4471944416607995472e-1L };
+#elif LDBL_MANT_DIG <= 106
+  const int maxPadeDegree = 10;
+  const double maxNormForPade[] = { 1.0007161601787493236741409687186e-4L /* degree = 3 */ ,
+      1.0007161601787493236741409687186e-3L, 4.7069769360887572939882574746264e-3L, 1.3220386624169159689406653101695e-2L,
+      2.8063482381631737920612944054906e-2L, 4.9625993951953473052385361085058e-2L, 7.7367040706027886224557538328171e-2L,
+      1.1016843812851143391275867258512e-1L };
+#else
+  const int maxPadeDegree = 10;
+  const double maxNormForPade[] = { 5.524506147036624377378713555116378e-5L /* degree = 3 */ ,
+      6.640600568157479679823602193345995e-4L, 3.227716520106894279249709728084626e-3L,
+      9.619593944683432960546978734646284e-3L, 2.134595382433742403911124458161147e-2L,
+      3.908166513900489428442993794761185e-2L, 6.266780814639442865832535460550138e-2L,
+      9.134603732914548552537150753385375e-2L };
+#endif
+  int degree = 3;
+  for (; degree <= maxPadeDegree; ++degree)
+    if (normIminusT <= maxNormForPade[degree - 3])
+      break;
+  return degree;
+}
+
+template<typename MatrixType>
+inline typename MatrixPowerAtomic<MatrixType>::ComplexScalar
+MatrixPowerAtomic<MatrixType>::computeSuperDiag(const ComplexScalar& curr, const ComplexScalar& prev, RealScalar p)
+{
+  using std::ceil;
+  using std::exp;
+  using std::log;
+  using std::sinh;
+
+  ComplexScalar logCurr = log(curr);
+  ComplexScalar logPrev = log(prev);
+  RealScalar unwindingNumber = ceil((numext::imag(logCurr - logPrev) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI));
+  ComplexScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2) + ComplexScalar(0, RealScalar(EIGEN_PI)*unwindingNumber);
+  return RealScalar(2) * exp(RealScalar(0.5) * p * (logCurr + logPrev)) * sinh(p * w) / (curr - prev);
+}
+
+template<typename MatrixType>
+inline typename MatrixPowerAtomic<MatrixType>::RealScalar
+MatrixPowerAtomic<MatrixType>::computeSuperDiag(RealScalar curr, RealScalar prev, RealScalar p)
+{
+  using std::exp;
+  using std::log;
+  using std::sinh;
+
+  RealScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2);
+  return 2 * exp(p * (log(curr) + log(prev)) / 2) * sinh(p * w) / (curr - prev);
+}
+
+/**
+ * \ingroup MatrixFunctions_Module
+ *
+ * \brief Class for computing matrix powers.
+ *
+ * \tparam MatrixType  type of the base, expected to be an instantiation
+ * of the Matrix class template.
+ *
+ * This class is capable of computing real/complex matrices raised to
+ * an arbitrary real power. Meanwhile, it saves the result of Schur
+ * decomposition if an non-integral power has even been calculated.
+ * Therefore, if you want to compute multiple (>= 2) matrix powers
+ * for the same matrix, using the class directly is more efficient than
+ * calling MatrixBase::pow().
+ *
+ * Example:
+ * \include MatrixPower_optimal.cpp
+ * Output: \verbinclude MatrixPower_optimal.out
+ */
+template<typename MatrixType>
+class MatrixPower : internal::noncopyable
+{
+  private:
+    typedef typename MatrixType::Scalar Scalar;
+    typedef typename MatrixType::RealScalar RealScalar;
+
+  public:
+    /**
+     * \brief Constructor.
+     *
+     * \param[in] A  the base of the matrix power.
+     *
+     * The class stores a reference to A, so it should not be changed
+     * (or destroyed) before evaluation.
+     */
+    explicit MatrixPower(const MatrixType& A) :
+      m_A(A),
+      m_conditionNumber(0),
+      m_rank(A.cols()),
+      m_nulls(0)
+    { eigen_assert(A.rows() == A.cols()); }
+
+    /**
+     * \brief Returns the matrix power.
+     *
+     * \param[in] p  exponent, a real scalar.
+     * \return The expression \f$ A^p \f$, where A is specified in the
+     * constructor.
+     */
+    const MatrixPowerParenthesesReturnValue<MatrixType> operator()(RealScalar p)
+    { return MatrixPowerParenthesesReturnValue<MatrixType>(*this, p); }
+
+    /**
+     * \brief Compute the matrix power.
+     *
+     * \param[in]  p    exponent, a real scalar.
+     * \param[out] res  \f$ A^p \f$ where A is specified in the
+     * constructor.
+     */
+    template<typename ResultType>
+    void compute(ResultType& res, RealScalar p);
+    
+    Index rows() const { return m_A.rows(); }
+    Index cols() const { return m_A.cols(); }
+
+  private:
+    typedef std::complex<RealScalar> ComplexScalar;
+    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0,
+              MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> ComplexMatrix;
+
+    /** \brief Reference to the base of matrix power. */
+    typename MatrixType::Nested m_A;
+
+    /** \brief Temporary storage. */
+    MatrixType m_tmp;
+
+    /** \brief Store the result of Schur decomposition. */
+    ComplexMatrix m_T, m_U;
+    
+    /** \brief Store fractional power of m_T. */
+    ComplexMatrix m_fT;
+
+    /**
+     * \brief Condition number of m_A.
+     *
+     * It is initialized as 0 to avoid performing unnecessary Schur
+     * decomposition, which is the bottleneck.
+     */
+    RealScalar m_conditionNumber;
+
+    /** \brief Rank of m_A. */
+    Index m_rank;
+    
+    /** \brief Rank deficiency of m_A. */
+    Index m_nulls;
+
+    /**
+     * \brief Split p into integral part and fractional part.
+     *
+     * \param[in]  p        The exponent.
+     * \param[out] p        The fractional part ranging in \f$ (-1, 1) \f$.
+     * \param[out] intpart  The integral part.
+     *
+     * Only if the fractional part is nonzero, it calls initialize().
+     */
+    void split(RealScalar& p, RealScalar& intpart);
+
+    /** \brief Perform Schur decomposition for fractional power. */
+    void initialize();
+
+    template<typename ResultType>
+    void computeIntPower(ResultType& res, RealScalar p);
+
+    template<typename ResultType>
+    void computeFracPower(ResultType& res, RealScalar p);
+
+    template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+    static void revertSchur(
+        Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
+        const ComplexMatrix& T,
+        const ComplexMatrix& U);
+
+    template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+    static void revertSchur(
+        Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
+        const ComplexMatrix& T,
+        const ComplexMatrix& U);
+};
+
+template<typename MatrixType>
+template<typename ResultType>
+void MatrixPower<MatrixType>::compute(ResultType& res, RealScalar p)
+{
+  using std::pow;
+  switch (cols()) {
+    case 0:
+      break;
+    case 1:
+      res(0,0) = pow(m_A.coeff(0,0), p);
+      break;
+    default:
+      RealScalar intpart;
+      split(p, intpart);
+
+      res = MatrixType::Identity(rows(), cols());
+      computeIntPower(res, intpart);
+      if (p) computeFracPower(res, p);
+  }
+}
+
+template<typename MatrixType>
+void MatrixPower<MatrixType>::split(RealScalar& p, RealScalar& intpart)
+{
+  using std::floor;
+  using std::pow;
+
+  intpart = floor(p);
+  p -= intpart;
+
+  // Perform Schur decomposition if it is not yet performed and the power is
+  // not an integer.
+  if (!m_conditionNumber && p)
+    initialize();
+
+  // Choose the more stable of intpart = floor(p) and intpart = ceil(p).
+  if (p > RealScalar(0.5) && p > (1-p) * pow(m_conditionNumber, p)) {
+    --p;
+    ++intpart;
+  }
+}
+
+template<typename MatrixType>
+void MatrixPower<MatrixType>::initialize()
+{
+  const ComplexSchur<MatrixType> schurOfA(m_A);
+  JacobiRotation<ComplexScalar> rot;
+  ComplexScalar eigenvalue;
+
+  m_fT.resizeLike(m_A);
+  m_T = schurOfA.matrixT();
+  m_U = schurOfA.matrixU();
+  m_conditionNumber = m_T.diagonal().array().abs().maxCoeff() / m_T.diagonal().array().abs().minCoeff();
+
+  // Move zero eigenvalues to the bottom right corner.
+  for (Index i = cols()-1; i>=0; --i) {
+    if (m_rank <= 2)
+      return;
+    if (m_T.coeff(i,i) == RealScalar(0)) {
+      for (Index j=i+1; j < m_rank; ++j) {
+        eigenvalue = m_T.coeff(j,j);
+        rot.makeGivens(m_T.coeff(j-1,j), eigenvalue);
+        m_T.applyOnTheRight(j-1, j, rot);
+        m_T.applyOnTheLeft(j-1, j, rot.adjoint());
+        m_T.coeffRef(j-1,j-1) = eigenvalue;
+        m_T.coeffRef(j,j) = RealScalar(0);
+        m_U.applyOnTheRight(j-1, j, rot);
+      }
+      --m_rank;
+    }
+  }
+
+  m_nulls = rows() - m_rank;
+  if (m_nulls) {
+    eigen_assert(m_T.bottomRightCorner(m_nulls, m_nulls).isZero()
+        && "Base of matrix power should be invertible or with a semisimple zero eigenvalue.");
+    m_fT.bottomRows(m_nulls).fill(RealScalar(0));
+  }
+}
+
+template<typename MatrixType>
+template<typename ResultType>
+void MatrixPower<MatrixType>::computeIntPower(ResultType& res, RealScalar p)
+{
+  using std::abs;
+  using std::fmod;
+  RealScalar pp = abs(p);
+
+  if (p<0) 
+    m_tmp = m_A.inverse();
+  else     
+    m_tmp = m_A;
+
+  while (true) {
+    if (fmod(pp, 2) >= 1)
+      res = m_tmp * res;
+    pp /= 2;
+    if (pp < 1)
+      break;
+    m_tmp *= m_tmp;
+  }
+}
+
+template<typename MatrixType>
+template<typename ResultType>
+void MatrixPower<MatrixType>::computeFracPower(ResultType& res, RealScalar p)
+{
+  Block<ComplexMatrix,Dynamic,Dynamic> blockTp(m_fT, 0, 0, m_rank, m_rank);
+  eigen_assert(m_conditionNumber);
+  eigen_assert(m_rank + m_nulls == rows());
+
+  MatrixPowerAtomic<ComplexMatrix>(m_T.topLeftCorner(m_rank, m_rank), p).compute(blockTp);
+  if (m_nulls) {
+    m_fT.topRightCorner(m_rank, m_nulls) = m_T.topLeftCorner(m_rank, m_rank).template triangularView<Upper>()
+        .solve(blockTp * m_T.topRightCorner(m_rank, m_nulls));
+  }
+  revertSchur(m_tmp, m_fT, m_U);
+  res = m_tmp * res;
+}
+
+template<typename MatrixType>
+template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+inline void MatrixPower<MatrixType>::revertSchur(
+    Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
+    const ComplexMatrix& T,
+    const ComplexMatrix& U)
+{ res.noalias() = U * (T.template triangularView<Upper>() * U.adjoint()); }
+
+template<typename MatrixType>
+template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
+inline void MatrixPower<MatrixType>::revertSchur(
+    Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
+    const ComplexMatrix& T,
+    const ComplexMatrix& U)
+{ res.noalias() = (U * (T.template triangularView<Upper>() * U.adjoint())).real(); }
+
+/**
+ * \ingroup MatrixFunctions_Module
+ *
+ * \brief Proxy for the matrix power of some matrix (expression).
+ *
+ * \tparam Derived  type of the base, a matrix (expression).
+ *
+ * This class holds the arguments to the matrix power until it is
+ * assigned or evaluated for some other reason (so the argument
+ * should not be changed in the meantime). It is the return type of
+ * MatrixBase::pow() and related functions and most of the
+ * time this is the only way it is used.
+ */
+template<typename Derived>
+class MatrixPowerReturnValue : public ReturnByValue< MatrixPowerReturnValue<Derived> >
+{
+  public:
+    typedef typename Derived::PlainObject PlainObject;
+    typedef typename Derived::RealScalar RealScalar;
+
+    /**
+     * \brief Constructor.
+     *
+     * \param[in] A  %Matrix (expression), the base of the matrix power.
+     * \param[in] p  real scalar, the exponent of the matrix power.
+     */
+    MatrixPowerReturnValue(const Derived& A, RealScalar p) : m_A(A), m_p(p)
+    { }
+
+    /**
+     * \brief Compute the matrix power.
+     *
+     * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
+     * constructor.
+     */
+    template<typename ResultType>
+    inline void evalTo(ResultType& result) const
+    { MatrixPower<PlainObject>(m_A.eval()).compute(result, m_p); }
+
+    Index rows() const { return m_A.rows(); }
+    Index cols() const { return m_A.cols(); }
+
+  private:
+    const Derived& m_A;
+    const RealScalar m_p;
+};
+
+/**
+ * \ingroup MatrixFunctions_Module
+ *
+ * \brief Proxy for the matrix power of some matrix (expression).
+ *
+ * \tparam Derived  type of the base, a matrix (expression).
+ *
+ * This class holds the arguments to the matrix power until it is
+ * assigned or evaluated for some other reason (so the argument
+ * should not be changed in the meantime). It is the return type of
+ * MatrixBase::pow() and related functions and most of the
+ * time this is the only way it is used.
+ */
+template<typename Derived>
+class MatrixComplexPowerReturnValue : public ReturnByValue< MatrixComplexPowerReturnValue<Derived> >
+{
+  public:
+    typedef typename Derived::PlainObject PlainObject;
+    typedef typename std::complex<typename Derived::RealScalar> ComplexScalar;
+
+    /**
+     * \brief Constructor.
+     *
+     * \param[in] A  %Matrix (expression), the base of the matrix power.
+     * \param[in] p  complex scalar, the exponent of the matrix power.
+     */
+    MatrixComplexPowerReturnValue(const Derived& A, const ComplexScalar& p) : m_A(A), m_p(p)
+    { }
+
+    /**
+     * \brief Compute the matrix power.
+     *
+     * Because \p p is complex, \f$ A^p \f$ is simply evaluated as \f$
+     * \exp(p \log(A)) \f$.
+     *
+     * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
+     * constructor.
+     */
+    template<typename ResultType>
+    inline void evalTo(ResultType& result) const
+    { result = (m_p * m_A.log()).exp(); }
+
+    Index rows() const { return m_A.rows(); }
+    Index cols() const { return m_A.cols(); }
+
+  private:
+    const Derived& m_A;
+    const ComplexScalar m_p;
+};
+
+namespace internal {
+
+template<typename MatrixPowerType>
+struct traits< MatrixPowerParenthesesReturnValue<MatrixPowerType> >
+{ typedef typename MatrixPowerType::PlainObject ReturnType; };
+
+template<typename Derived>
+struct traits< MatrixPowerReturnValue<Derived> >
+{ typedef typename Derived::PlainObject ReturnType; };
+
+template<typename Derived>
+struct traits< MatrixComplexPowerReturnValue<Derived> >
+{ typedef typename Derived::PlainObject ReturnType; };
+
+}
+
+template<typename Derived>
+const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(const RealScalar& p) const
+{ return MatrixPowerReturnValue<Derived>(derived(), p); }
+
+template<typename Derived>
+const MatrixComplexPowerReturnValue<Derived> MatrixBase<Derived>::pow(const std::complex<RealScalar>& p) const
+{ return MatrixComplexPowerReturnValue<Derived>(derived(), p); }
+
+} // namespace Eigen
+
+#endif // EIGEN_MATRIX_POWER
diff --git a/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
new file mode 100644
index 0000000..e363e77
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
@@ -0,0 +1,368 @@
+// This file is part of Eigen, a lightweight C++ template library
+// for linear algebra.
+//
+// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
+//
+// This Source Code Form is subject to the terms of the Mozilla
+// Public License v. 2.0. If a copy of the MPL was not distributed
+// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef EIGEN_MATRIX_SQUARE_ROOT
+#define EIGEN_MATRIX_SQUARE_ROOT
+
+namespace Eigen { 
+
+namespace internal {
+
+// pre:  T.block(i,i,2,2) has complex conjugate eigenvalues
+// post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, Index i, ResultType& sqrtT)
+{
+  // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
+  //       in EigenSolver. If we expose it, we could call it directly from here.
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  Matrix<Scalar,2,2> block = T.template block<2,2>(i,i);
+  EigenSolver<Matrix<Scalar,2,2> > es(block);
+  sqrtT.template block<2,2>(i,i)
+    = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
+}
+
+// pre:  block structure of T is such that (i,j) is a 1x1 block,
+//       all blocks of sqrtT to left of and below (i,j) are correct
+// post: sqrtT(i,j) has the correct value
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
+{
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value();
+  sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j));
+}
+
+// similar to compute1x1offDiagonalBlock()
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
+{
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
+  if (j-i > 1)
+    rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2);
+  Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity();
+  A += sqrtT.template block<2,2>(j,j).transpose();
+  sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose());
+}
+
+// similar to compute1x1offDiagonalBlock()
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
+{
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
+  if (j-i > 2)
+    rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1);
+  Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity();
+  A += sqrtT.template block<2,2>(i,i);
+  sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs);
+}
+
+// solves the equation A X + X B = C where all matrices are 2-by-2
+template <typename MatrixType>
+void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B, const MatrixType& C)
+{
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero();
+  coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
+  coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1);
+  coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0);
+  coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1);
+  coeffMatrix.coeffRef(0,1) = B.coeff(1,0);
+  coeffMatrix.coeffRef(0,2) = A.coeff(0,1);
+  coeffMatrix.coeffRef(1,0) = B.coeff(0,1);
+  coeffMatrix.coeffRef(1,3) = A.coeff(0,1);
+  coeffMatrix.coeffRef(2,0) = A.coeff(1,0);
+  coeffMatrix.coeffRef(2,3) = B.coeff(1,0);
+  coeffMatrix.coeffRef(3,1) = A.coeff(1,0);
+  coeffMatrix.coeffRef(3,2) = B.coeff(0,1);
+
+  Matrix<Scalar,4,1> rhs;
+  rhs.coeffRef(0) = C.coeff(0,0);
+  rhs.coeffRef(1) = C.coeff(0,1);
+  rhs.coeffRef(2) = C.coeff(1,0);
+  rhs.coeffRef(3) = C.coeff(1,1);
+
+  Matrix<Scalar,4,1> result;
+  result = coeffMatrix.fullPivLu().solve(rhs);
+
+  X.coeffRef(0,0) = result.coeff(0);
+  X.coeffRef(0,1) = result.coeff(1);
+  X.coeffRef(1,0) = result.coeff(2);
+  X.coeffRef(1,1) = result.coeff(3);
+}
+
+// similar to compute1x1offDiagonalBlock()
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, Index i, Index j, ResultType& sqrtT)
+{
+  typedef typename traits<MatrixType>::Scalar Scalar;
+  Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
+  Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
+  Matrix<Scalar,2,2> C = T.template block<2,2>(i,j);
+  if (j-i > 2)
+    C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2);
+  Matrix<Scalar,2,2> X;
+  matrix_sqrt_quasi_triangular_solve_auxiliary_equation(X, A, B, C);
+  sqrtT.template block<2,2>(i,j) = X;
+}
+
+// pre:  T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
+// post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT)
+{
+  using std::sqrt;
+  const Index size = T.rows();
+  for (Index i = 0; i < size; i++) {
+    if (i == size - 1 || T.coeff(i+1, i) == 0) {
+      eigen_assert(T(i,i) >= 0);
+      sqrtT.coeffRef(i,i) = sqrt(T.coeff(i,i));
+    }
+    else {
+      matrix_sqrt_quasi_triangular_2x2_diagonal_block(T, i, sqrtT);
+      ++i;
+    }
+  }
+}
+
+// pre:  T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
+// post: sqrtT is the square root of T.
+template <typename MatrixType, typename ResultType>
+void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT)
+{
+  const Index size = T.rows();
+  for (Index j = 1; j < size; j++) {
+      if (T.coeff(j, j-1) != 0)  // if T(j-1:j, j-1:j) is a 2-by-2 block
+	continue;
+    for (Index i = j-1; i >= 0; i--) {
+      if (i > 0 && T.coeff(i, i-1) != 0)  // if T(i-1:i, i-1:i) is a 2-by-2 block
+	continue;
+      bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0);
+      bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0);
+      if (iBlockIs2x2 && jBlockIs2x2) 
+        matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(T, i, j, sqrtT);
+      else if (iBlockIs2x2 && !jBlockIs2x2) 
+        matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(T, i, j, sqrtT);
+      else if (!iBlockIs2x2 && jBlockIs2x2) 
+        matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(T, i, j, sqrtT);
+      else if (!iBlockIs2x2 && !jBlockIs2x2) 
+        matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(T, i, j, sqrtT);
+    }
+  }
+}
+
+} // end of namespace internal
+
+/** \ingroup MatrixFunctions_Module
+  * \brief Compute matrix square root of quasi-triangular matrix.
+  *
+  * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
+  *                      expected to be an instantiation of the Matrix class template.
+  * \tparam  ResultType  type of \p result, where result is to be stored.
+  * \param[in]  arg      argument of matrix square root.
+  * \param[out] result   matrix square root of upper Hessenberg part of \p arg.
+  *
+  * This function computes the square root of the upper quasi-triangular matrix stored in the upper
+  * Hessenberg part of \p arg.  Only the upper Hessenberg part of \p result is updated, the rest is
+  * not touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
+  *
+  * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
+  */
+template <typename MatrixType, typename ResultType> 
+void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result)
+{
+  eigen_assert(arg.rows() == arg.cols());
+  result.resize(arg.rows(), arg.cols());
+  internal::matrix_sqrt_quasi_triangular_diagonal(arg, result);
+  internal::matrix_sqrt_quasi_triangular_off_diagonal(arg, result);
+}
+
+
+/** \ingroup MatrixFunctions_Module
+  * \brief Compute matrix square root of triangular matrix.
+  *
+  * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
+  *                      expected to be an instantiation of the Matrix class template.
+  * \tparam  ResultType  type of \p result, where result is to be stored.
+  * \param[in]  arg      argument of matrix square root.
+  * \param[out] result   matrix square root of upper triangular part of \p arg.
+  *
+  * Only the upper triangular part (including the diagonal) of \p result is updated, the rest is not
+  * touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
+  *
+  * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
+  */
+template <typename MatrixType, typename ResultType> 
+void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
+{
+  using std::sqrt;
+  typedef typename MatrixType::Scalar Scalar;
+
+  eigen_assert(arg.rows() == arg.cols());
+
+  // Compute square root of arg and store it in upper triangular part of result
+  // This uses that the square root of triangular matrices can be computed directly.
+  result.resize(arg.rows(), arg.cols());
+  for (Index i = 0; i < arg.rows(); i++) {
+    result.coeffRef(i,i) = sqrt(arg.coeff(i,i));
+  }
+  for (Index j = 1; j < arg.cols(); j++) {
+    for (Index i = j-1; i >= 0; i--) {
+      // if i = j-1, then segment has length 0 so tmp = 0
+      Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value();
+      // denominator may be zero if original matrix is singular
+      result.coeffRef(i,j) = (arg.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j));
+    }
+  }
+}
+
+
+namespace internal {
+
+/** \ingroup MatrixFunctions_Module
+  * \brief Helper struct for computing matrix square roots of general matrices.
+  * \tparam  MatrixType  type of the argument of the matrix square root,
+  *                      expected to be an instantiation of the Matrix class template.
+  *
+  * \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt()
+  */
+template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
+struct matrix_sqrt_compute
+{
+  /** \brief Compute the matrix square root
+    *
+    * \param[in]  arg     matrix whose square root is to be computed.
+    * \param[out] result  square root of \p arg.
+    *
+    * See MatrixBase::sqrt() for details on how this computation is implemented.
+    */
+  template <typename ResultType> static void run(const MatrixType &arg, ResultType &result);    
+};
+
+
+// ********** Partial specialization for real matrices **********
+
+template <typename MatrixType>
+struct matrix_sqrt_compute<MatrixType, 0>
+{
+  typedef typename MatrixType::PlainObject PlainType;
+  template <typename ResultType>
+  static void run(const MatrixType &arg, ResultType &result)
+  {
+    eigen_assert(arg.rows() == arg.cols());
+
+    // Compute Schur decomposition of arg
+    const RealSchur<PlainType> schurOfA(arg);
+    const PlainType& T = schurOfA.matrixT();
+    const PlainType& U = schurOfA.matrixU();
+    
+    // Compute square root of T
+    PlainType sqrtT = PlainType::Zero(arg.rows(), arg.cols());
+    matrix_sqrt_quasi_triangular(T, sqrtT);
+    
+    // Compute square root of arg
+    result = U * sqrtT * U.adjoint();
+  }
+};
+
+
+// ********** Partial specialization for complex matrices **********
+
+template <typename MatrixType>
+struct matrix_sqrt_compute<MatrixType, 1>
+{
+  typedef typename MatrixType::PlainObject PlainType;
+  template <typename ResultType>
+  static void run(const MatrixType &arg, ResultType &result)
+  {
+    eigen_assert(arg.rows() == arg.cols());
+
+    // Compute Schur decomposition of arg
+    const ComplexSchur<PlainType> schurOfA(arg);
+    const PlainType& T = schurOfA.matrixT();
+    const PlainType& U = schurOfA.matrixU();
+    
+    // Compute square root of T
+    PlainType sqrtT;
+    matrix_sqrt_triangular(T, sqrtT);
+    
+    // Compute square root of arg
+    result = U * (sqrtT.template triangularView<Upper>() * U.adjoint());
+  }
+};
+
+} // end namespace internal
+
+/** \ingroup MatrixFunctions_Module
+  *
+  * \brief Proxy for the matrix square root of some matrix (expression).
+  *
+  * \tparam Derived  Type of the argument to the matrix square root.
+  *
+  * This class holds the argument to the matrix square root until it
+  * is assigned or evaluated for some other reason (so the argument
+  * should not be changed in the meantime). It is the return type of
+  * MatrixBase::sqrt() and most of the time this is the only way it is
+  * used.
+  */
+template<typename Derived> class MatrixSquareRootReturnValue
+: public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
+{
+  protected:
+    typedef typename internal::ref_selector<Derived>::type DerivedNested;
+
+  public:
+    /** \brief Constructor.
+      *
+      * \param[in]  src  %Matrix (expression) forming the argument of the
+      * matrix square root.
+      */
+    explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { }
+
+    /** \brief Compute the matrix square root.
+      *
+      * \param[out]  result  the matrix square root of \p src in the
+      * constructor.
+      */
+    template <typename ResultType>
+    inline void evalTo(ResultType& result) const
+    {
+      typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
+      typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
+      DerivedEvalType tmp(m_src);
+      internal::matrix_sqrt_compute<DerivedEvalTypeClean>::run(tmp, result);
+    }
+
+    Index rows() const { return m_src.rows(); }
+    Index cols() const { return m_src.cols(); }
+
+  protected:
+    const DerivedNested m_src;
+};
+
+namespace internal {
+template<typename Derived>
+struct traits<MatrixSquareRootReturnValue<Derived> >
+{
+  typedef typename Derived::PlainObject ReturnType;
+};
+}
+
+template <typename Derived>
+const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
+{
+  eigen_assert(rows() == cols());
+  return MatrixSquareRootReturnValue<Derived>(derived());
+}
+
+} // end namespace Eigen
+
+#endif // EIGEN_MATRIX_FUNCTION
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h b/third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
similarity index 100%
rename from third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
rename to third_party/allwpilib/wpimath/src/main/native/eigeninclude/unsupported/Eigen/src/MatrixFunctions/StemFunction.h
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assert.h b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assert.h
index 21e7bd1..88587fa 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assert.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assert.h
@@ -83,12 +83,19 @@
 namespace drake {
 namespace internal {
 // Abort the program with an error message.
-[[noreturn]]
-void Abort(const char* condition, const char* func, const char* file, int line);
+[[noreturn]] void Abort(const char* condition, const char* func,
+                        const char* file, int line);
 // Report an assertion failure; will either Abort(...) or throw.
-[[noreturn]]
-void AssertionFailed(
-    const char* condition, const char* func, const char* file, int line);
+[[noreturn]] void AssertionFailed(const char* condition, const char* func,
+                                  const char* file, int line);
+template <bool>
+constexpr void DrakeAssertWasUsedWithRawPointer() {}
+template<>
+[[deprecated("\nDRAKE DEPRECATED: When using DRAKE_ASSERT or DRAKE_DEMAND on"
+" a raw pointer, always write out DRAKE_ASSERT(foo != nullptr), do not write"
+" DRAKE_ASSERT(foo) and rely on implicit pointer-to-bool conversion."
+"\nThe deprecated code will be removed from Drake on or after 2021-12-01.")]]
+constexpr void DrakeAssertWasUsedWithRawPointer<true>() {}
 }  // namespace internal
 namespace assert {
 // Allows for specialization of how to bool-convert Conditions used in
@@ -98,7 +105,7 @@
 // require special handling.
 template <typename Condition>
 struct ConditionTraits {
-  static constexpr bool is_valid = std::is_convertible<Condition, bool>::value;
+  static constexpr bool is_valid = std::is_convertible_v<Condition, bool>;
   static bool Evaluate(const Condition& value) {
     return value;
   }
@@ -113,8 +120,10 @@
 #define DRAKE_DEMAND(condition)                                              \
   do {                                                                       \
     typedef ::drake::assert::ConditionTraits<                                \
-        typename std::remove_cv<decltype(condition)>::type> Trait;           \
+        typename std::remove_cv_t<decltype(condition)>> Trait;               \
     static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+    ::drake::internal::DrakeAssertWasUsedWithRawPointer<                     \
+        std::is_pointer_v<decltype(condition)>>();                           \
     if (!Trait::Evaluate(condition)) {                                       \
       ::drake::internal::AssertionFailed(                                    \
            #condition, __func__, __FILE__, __LINE__);                        \
@@ -130,7 +139,7 @@
 # define DRAKE_ASSERT(condition) DRAKE_DEMAND(condition)
 # define DRAKE_ASSERT_VOID(expression) do {                     \
     static_assert(                                              \
-        std::is_convertible<decltype(expression), void>::value, \
+        std::is_convertible_v<decltype(expression), void>,      \
         "Expression should be void.");                          \
     expression;                                                 \
   } while (0)
@@ -140,12 +149,16 @@
 constexpr bool kDrakeAssertIsArmed = false;
 constexpr bool kDrakeAssertIsDisarmed = true;
 }  // namespace drake
-# define DRAKE_ASSERT(condition) static_assert(                        \
-    ::drake::assert::ConditionTraits<                                  \
-        typename std::remove_cv<decltype(condition)>::type>::is_valid, \
-    "Condition should be bool-convertible.");
+# define DRAKE_ASSERT(condition) do {                                  \
+    static_assert(                                                     \
+        ::drake::assert::ConditionTraits<                              \
+            typename std::remove_cv_t<decltype(condition)>>::is_valid, \
+        "Condition should be bool-convertible.");                      \
+    ::drake::internal::DrakeAssertWasUsedWithRawPointer<               \
+        std::is_pointer_v<decltype(condition)>>();                     \
+  } while (0)
 # define DRAKE_ASSERT_VOID(expression) static_assert(           \
-    std::is_convertible<decltype(expression), void>::value,     \
+    std::is_convertible_v<decltype(expression), void>,          \
     "Expression should be void.")
 #endif
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assertion_error.h b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assertion_error.h
index 541b118..b428474 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assertion_error.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_assertion_error.h
@@ -6,8 +6,8 @@
 namespace drake {
 namespace internal {
 
-/// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
-/// configured to throw.
+// This is what DRAKE_ASSERT and DRAKE_DEMAND throw when our assertions are
+// configured to throw.
 class assertion_error : public std::runtime_error {
  public:
   explicit assertion_error(const std::string& what_arg)
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_copyable.h b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_copyable.h
index 086f1f7..a96a6fb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_copyable.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_copyable.h
@@ -19,8 +19,8 @@
 /** DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN deletes the special member functions for
 copy-construction, copy-assignment, move-construction, and move-assignment.
 Drake's Doxygen is customized to render the deletions in detail, with
-appropriate comments.  Invoke this this macro in the public section of the
-class declaration, e.g.:
+appropriate comments.  Invoke this macro in the public section of the class
+declaration, e.g.:
 <pre>
 class Foo {
  public:
@@ -43,8 +43,8 @@
 functions could conceivably still be ill-formed, in which case they will
 effectively not be declared or used -- but because the copy constructor exists
 the type will still be MoveConstructible.  Drake's Doxygen is customized to
-render the functions in detail, with appropriate comments.  Invoke this this
-macro in the public section of the class declaration, e.g.:
+render the functions in detail, with appropriate comments.  Typically, you
+should invoke this macro in the public section of the class declaration, e.g.:
 <pre>
 class Foo {
  public:
@@ -53,60 +53,38 @@
   // ...
 };
 </pre>
+
+However, if Foo has a virtual destructor (i.e., is subclassable), then
+typically you should use either DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN in the
+public section or else DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN in the
+protected section, to prevent
+<a href="https://en.wikipedia.org/wiki/Object_slicing">object slicing</a>.
+
+The macro contains a built-in self-check that copy-assignment is well-formed.
+This self-check proves that the Classname is CopyConstructible, CopyAssignable,
+MoveConstructible, and MoveAssignable (in all but the most arcane cases where a
+member of the Classname is somehow CopyAssignable but not CopyConstructible).
+Therefore, classes that use this macro typically will not need to have any unit
+tests that check for the presence nor correctness of these functions.
+
+However, the self-check does not provide any checks of the runtime efficiency
+of the functions.  If it is important for performance that the move functions
+actually move (instead of making a copy), then you should consider capturing
+that in a unit test.
 */
 #define DRAKE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
   Classname(const Classname&) = default;                        \
   Classname& operator=(const Classname&) = default;             \
   Classname(Classname&&) = default;                             \
   Classname& operator=(Classname&&) = default;                  \
-  /* Fails at compile-time if default-copy doesn't work. */     \
-  static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() {        \
-    (void) static_cast<Classname& (Classname::*)(               \
-        const Classname&)>(&Classname::operator=);              \
-  }
-
-/** DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN declares (but does not define) the
-special member functions for copy-construction, copy-assignment,
-move-construction, and move-assignment.  Drake's Doxygen is customized to
-render the declarations with appropriate comments.
-
-This is useful when paired with DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T
-to work around https://gcc.gnu.org/bugzilla/show_bug.cgi?id=57728 whereby the
-declaration and definition must be split.  Once Drake no longer supports GCC
-versions prior to 6.3, this macro could be removed.
-
-Invoke this this macro in the public section of the class declaration, e.g.:
-<pre>
-template <typename T>
-class Foo {
- public:
-  DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Foo)
-
-  // ...
-};
-DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Foo)
-</pre>
-*/
-#define DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN(Classname)       \
-  Classname(const Classname&);                                          \
-  Classname& operator=(const Classname&);                               \
-  Classname(Classname&&);                                               \
-  Classname& operator=(Classname&&);                                    \
-  /* Fails at compile-time if default-copy doesn't work. */             \
-  static void DRAKE_COPYABLE_DEMAND_COPY_CAN_COMPILE() {                \
-    (void) static_cast<Classname& (Classname::*)(                       \
-        const Classname&)>(&Classname::operator=);                      \
-  }
-
-/** Helper for DRAKE_DECLARE_COPY_AND_MOVE_AND_ASSIGN.  Provides defaulted
-definitions for the four special member functions of a templated class.
-*/
-#define DRAKE_DEFINE_DEFAULT_COPY_AND_MOVE_AND_ASSIGN_T(Classname)      \
-  template <typename T>                                                 \
-  Classname<T>::Classname(const Classname<T>&) = default;               \
-  template <typename T>                                                 \
-  Classname<T>& Classname<T>::operator=(const Classname<T>&) = default; \
-  template <typename T>                                                 \
-  Classname<T>::Classname(Classname<T>&&) = default;                    \
-  template <typename T>                                                 \
-  Classname<T>& Classname<T>::operator=(Classname<T>&&) = default;
+  /* Fails at compile-time if copy-assign doesn't compile. */   \
+  /* Note that we do not test the copy-ctor here, because  */   \
+  /* it will not exist when Classname is abstract.         */   \
+  static void DrakeDefaultCopyAndMoveAndAssign_DoAssign(        \
+      Classname* a, const Classname& b) { *a = b; }             \
+  static_assert(                                                \
+      &DrakeDefaultCopyAndMoveAndAssign_DoAssign ==             \
+      &DrakeDefaultCopyAndMoveAndAssign_DoAssign,               \
+      "This assertion is never false; its only purpose is to "  \
+      "generate 'use of deleted function: operator=' errors "   \
+      "when Classname is a template.");
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_throw.h b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_throw.h
index ff42bb7..bb4bae8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_throw.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/drake_throw.h
@@ -12,19 +12,42 @@
 namespace drake {
 namespace internal {
 // Throw an error message.
-[[noreturn]]
-void Throw(const char* condition, const char* func, const char* file, int line);
+[[noreturn]] void Throw(const char* condition, const char* func,
+                        const char* file, int line);
+
+template <bool>
+constexpr void DrakeThrowUnlessWasUsedWithRawPointer() {}
+template<>
+[[deprecated("\nDRAKE DEPRECATED: When using DRAKE_THROW_UNLESS on a raw"
+" pointer, always write out DRAKE_THROW_UNLESS(foo != nullptr), do not write"
+" DRAKE_THROW_UNLESS(foo) and rely on implicit pointer-to-bool conversion."
+"\nThe deprecated code will be removed from Drake on or after 2021-12-01.")]]
+constexpr void DrakeThrowUnlessWasUsedWithRawPointer<true>() {}
+
 }  // namespace internal
 }  // namespace drake
 
 /// Evaluates @p condition and iff the value is false will throw an exception
 /// with a message showing at least the condition text, function name, file,
 /// and line.
+///
+/// The condition must not be a pointer, where we'd implicitly rely on its
+/// nullness. Instead, always write out "!= nullptr" to be precise.
+///
+/// Correct: `DRAKE_THROW_UNLESS(foo != nullptr);`
+/// Incorrect: `DRAKE_THROW_UNLESS(foo);`
+///
+/// Because this macro is intended to provide a useful exception message to
+/// users, we should err on the side of extra detail about the failure. The
+/// meaning of "foo" isolated within error message text does not make it
+/// clear that a null pointer is the proximate cause of the problem.
 #define DRAKE_THROW_UNLESS(condition)                                        \
   do {                                                                       \
     typedef ::drake::assert::ConditionTraits<                                \
-        typename std::remove_cv<decltype(condition)>::type> Trait;           \
+        typename std::remove_cv_t<decltype(condition)>> Trait;               \
     static_assert(Trait::is_valid, "Condition should be bool-convertible."); \
+    ::drake::internal::DrakeThrowUnlessWasUsedWithRawPointer<                \
+        std::is_pointer_v<decltype(condition)>>();                           \
     if (!Trait::Evaluate(condition)) {                                       \
       ::drake::internal::Throw(#condition, __func__, __FILE__, __LINE__);    \
     }                                                                        \
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/never_destroyed.h b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/never_destroyed.h
index 2033fd0..024b355 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/drake/common/never_destroyed.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/drake/common/never_destroyed.h
@@ -56,6 +56,25 @@
 ///   return string_to_enum.access().at(foo_string);
 /// }
 /// @endcode
+///
+/// In cases where computing the static data is more complicated than an
+/// initializer_list, you can use a temporary lambda to populate the value:
+/// @code
+/// const std::vector<double>& GetConstantMagicNumbers() {
+///   static const drake::never_destroyed<std::vector<double>> result{[]() {
+///     std::vector<double> prototype;
+///     std::mt19937 random_generator;
+///     for (int i = 0; i < 10; ++i) {
+///       double new_value = random_generator();
+///       prototype.push_back(new_value);
+///     }
+///     return prototype;
+///   }()};
+///   return result.access();
+/// }
+/// @endcode
+///
+/// Note in particular the `()` after the lambda. That causes it to be invoked.
 //
 // The above examples are repeated in the unit test; keep them in sync.
 template <typename T>
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h b/third_party/allwpilib/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
index e45cdc8..55b8442 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/drake/math/discrete_algebraic_riccati_equation.h
@@ -4,29 +4,82 @@
 #include <cstdlib>
 
 #include <Eigen/Core>
+#include <wpi/SymbolExports.h>
 
 namespace drake {
 namespace math {
 
-/// Computes the unique stabilizing solution X to the discrete-time algebraic
-/// Riccati equation:
-///
-/// \f[
-/// A'XA - X - A'XB(B'XB+R)^{-1}B'XA + Q = 0
-/// \f]
-///
-/// @throws std::runtime_error if Q is not positive semi-definite.
-/// @throws std::runtime_error if R is not positive definite.
-///
-/// Based on the Schur Vector approach outlined in this paper:
-/// "On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
-/// by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
-///
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+AᵀXA − X − AᵀXB(BᵀXB + R)⁻¹BᵀXA + Q = 0
+
+@throws std::exception if Q is not positive semi-definite.
+@throws std::exception if R is not positive definite.
+
+Based on the Schur Vector approach outlined in this paper:
+"On the Numerical Solution of the Discrete-Time Algebraic Riccati Equation"
+by Thrasyvoulos Pappas, Alan J. Laub, and Nils R. Sandell
+*/
+WPILIB_DLLEXPORT
 Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
     const Eigen::Ref<const Eigen::MatrixXd>& A,
     const Eigen::Ref<const Eigen::MatrixXd>& B,
     const Eigen::Ref<const Eigen::MatrixXd>& Q,
     const Eigen::Ref<const Eigen::MatrixXd>& R);
+
+/**
+Computes the unique stabilizing solution X to the discrete-time algebraic
+Riccati equation:
+
+AᵀXA − X − (AᵀXB + N)(BᵀXB + R)⁻¹(BᵀXA + Nᵀ) + Q = 0
+
+This is equivalent to solving the original DARE:
+
+A₂ᵀXA₂ − X − A₂ᵀXB(BᵀXB + R)⁻¹BᵀXA₂ + Q₂ = 0
+
+where A₂ and Q₂ are a change of variables:
+
+A₂ = A − BR⁻¹Nᵀ and Q₂ = Q − NR⁻¹Nᵀ
+
+This overload of the DARE is useful for finding the control law uₖ that
+minimizes the following cost function subject to xₖ₊₁ = Axₖ + Buₖ.
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q  N][xₖ]
+J = Σ [uₖ] [Nᵀ R][uₖ] ΔT
+   k=0
+@endverbatim
+
+This is a more general form of the following. The linear-quadratic regulator
+is the feedback control law uₖ that minimizes the following cost function
+subject to xₖ₊₁ = Axₖ + Buₖ:
+
+@verbatim
+    ∞
+J = Σ (xₖᵀQxₖ + uₖᵀRuₖ) ΔT
+   k=0
+@endverbatim
+
+This can be refactored as:
+
+@verbatim
+    ∞ [xₖ]ᵀ[Q 0][xₖ]
+J = Σ [uₖ] [0 R][uₖ] ΔT
+   k=0
+@endverbatim
+
+@throws std::runtime_error if Q − NR⁻¹Nᵀ is not positive semi-definite.
+@throws std::runtime_error if R is not positive definite.
+*/
+WPILIB_DLLEXPORT
+Eigen::MatrixXd DiscreteAlgebraicRiccatiEquation(
+    const Eigen::Ref<const Eigen::MatrixXd>& A,
+    const Eigen::Ref<const Eigen::MatrixXd>& B,
+    const Eigen::Ref<const Eigen::MatrixXd>& Q,
+    const Eigen::Ref<const Eigen::MatrixXd>& R,
+    const Eigen::Ref<const Eigen::MatrixXd>& N);
 }  // namespace math
 }  // namespace drake
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/LinearFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/LinearFilter.h
deleted file mode 100644
index 1fe0edc..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/LinearFilter.h
+++ /dev/null
@@ -1,196 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#pragma once
-
-#include <cassert>
-#include <cmath>
-#include <initializer_list>
-#include <vector>
-
-#include <wpi/ArrayRef.h>
-#include <wpi/circular_buffer.h>
-
-#include "units/time.h"
-#include "wpimath/MathShared.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++;
-    wpi::math::MathSharedStore::ReportUsage(
-        wpi::math::MathUsageId::kFilter_Linear, 1);
-  }
-
-  /**
-   * 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/third_party/allwpilib/wpimath/src/main/native/include/frc/MathUtil.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/MathUtil.h
new file mode 100644
index 0000000..54a77af
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/MathUtil.h
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/numbers>
+
+#include "units/angle.h"
+
+namespace frc {
+
+/**
+ * Returns 0.0 if the given value is within the specified range around zero.
+ * The remaining range between the deadband and 1.0 is scaled from 0.0 to 1.0.
+ *
+ * @param value    Value to clip.
+ * @param deadband Range around zero.
+ */
+WPILIB_DLLEXPORT
+double ApplyDeadband(double value, double deadband);
+
+/**
+ * Returns modulus of input.
+ *
+ * @param input        Input value to wrap.
+ * @param minimumInput The minimum value expected from the input.
+ * @param maximumInput The maximum value expected from the input.
+ */
+template <typename T>
+constexpr T InputModulus(T input, T minimumInput, T maximumInput) {
+  T modulus = maximumInput - minimumInput;
+
+  // Wrap input if it's above the maximum input
+  int numMax = (input - minimumInput) / modulus;
+  input -= numMax * modulus;
+
+  // Wrap input if it's below the minimum input
+  int numMin = (input - maximumInput) / modulus;
+  input -= numMin * modulus;
+
+  return input;
+}
+
+/**
+ * Wraps an angle to the range -pi to pi radians (-180 to 180 degrees).
+ *
+ * @param angle Angle to wrap.
+ */
+WPILIB_DLLEXPORT
+constexpr units::radian_t AngleModulus(units::radian_t angle) {
+  return InputModulus<units::radian_t>(angle,
+                                       units::radian_t{-wpi::numbers::pi},
+                                       units::radian_t{wpi::numbers::pi});
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/MedianFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/MedianFilter.h
deleted file mode 100644
index 3ccccbf..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/MedianFilter.h
+++ /dev/null
@@ -1,80 +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 <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/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index b461005..730c4b9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,9 +9,12 @@
 #include <random>
 #include <type_traits>
 
+#include <wpi/SymbolExports.h>
+#include <wpi/deprecated.h>
+
 #include "Eigen/Core"
+#include "Eigen/Eigenvalues"
 #include "Eigen/QR"
-#include "Eigen/src/Eigenvalues/EigenSolver.h"
 #include "frc/geometry/Pose2d.h"
 
 namespace frc {
@@ -61,7 +61,7 @@
 template <int States, int Inputs>
 bool IsStabilizableImpl(const Eigen::Matrix<double, States, States>& A,
                         const Eigen::Matrix<double, States, Inputs>& B) {
-  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es(A);
+  Eigen::EigenSolver<Eigen::Matrix<double, States, States>> es{A};
 
   for (int i = 0; i < States; ++i) {
     if (es.eigenvalues()[i].real() * es.eigenvalues()[i].real() +
@@ -78,7 +78,7 @@
 
     Eigen::ColPivHouseholderQR<
         Eigen::Matrix<std::complex<double>, States, States + Inputs>>
-        qr(E);
+        qr{E};
     if (qr.rank() < States) {
       return false;
     }
@@ -95,10 +95,13 @@
  *
  * @param elems An array of elements in the matrix.
  * @return A matrix containing the given elements.
+ * @deprecated Use Eigen::Matrix or Eigen::Vector initializer list constructor.
  */
 template <int Rows, int Cols, typename... Ts,
           typename =
               std::enable_if_t<std::conjunction_v<std::is_same<double, Ts>...>>>
+WPI_DEPRECATED(
+    "Use Eigen::Matrix or Eigen::Vector initializer list constructor")
 Eigen::Matrix<double, Rows, Cols> MakeMatrix(Ts... elems) {
   static_assert(
       sizeof...(elems) == Rows * Cols,
@@ -213,12 +216,12 @@
  * @return White noise vector.
  */
 template <int N>
-Eigen::Matrix<double, N, 1> MakeWhiteNoiseVector(
+Eigen::Vector<double, N> MakeWhiteNoiseVector(
     const std::array<double, N>& stdDevs) {
   std::random_device rd;
   std::mt19937 gen{rd()};
 
-  Eigen::Matrix<double, N, 1> result;
+  Eigen::Vector<double, N> result;
   for (int i = 0; i < N; ++i) {
     // Passing a standard deviation of 0.0 to std::normal_distribution is
     // undefined behavior
@@ -233,12 +236,34 @@
 }
 
 /**
+ * Converts a Pose2d into a vector of [x, y, theta].
+ *
+ * @param pose The pose that is being represented.
+ *
+ * @return The vector.
+ */
+WPILIB_DLLEXPORT
+Eigen::Vector<double, 3> PoseTo3dVector(const Pose2d& pose);
+
+/**
+ * Converts a Pose2d into a vector of [x, y, std::cos(theta), std::sin(theta)].
+ *
+ * @param pose The pose that is being represented.
+ *
+ * @return The vector.
+ */
+WPILIB_DLLEXPORT
+Eigen::Vector<double, 4> PoseTo4dVector(const Pose2d& pose);
+
+/**
  * Returns true if (A, B) is a stabilizable pair.
  *
- * (A,B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
+ * (A, B) is stabilizable if and only if the uncontrollable eigenvalues of A, if
  * any, have absolute values less than one, where an eigenvalue is
- * uncontrollable if rank(lambda * I - A, B) < n where n is number of states.
+ * uncontrollable if rank(λI - A, B) < n where n is the number of states.
  *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
  * @param A System matrix.
  * @param B Input matrix.
  */
@@ -248,17 +273,36 @@
   return detail::IsStabilizableImpl<States, Inputs>(A, B);
 }
 
-// Template specializations are used here to make common state-input pairs
-// compile faster.
-template <>
-bool IsStabilizable<1, 1>(const Eigen::Matrix<double, 1, 1>& A,
-                          const Eigen::Matrix<double, 1, 1>& B);
+/**
+ * Returns true if (A, C) is a detectable pair.
+ *
+ * (A, C) is detectable if and only if the unobservable eigenvalues of A, if
+ * any, have absolute values less than one, where an eigenvalue is unobservable
+ * if rank(λI - A; C) < n where n is the number of states.
+ *
+ * @tparam States The number of states.
+ * @tparam Outputs The number of outputs.
+ * @param A System matrix.
+ * @param C Output matrix.
+ */
+template <int States, int Outputs>
+bool IsDetectable(const Eigen::Matrix<double, States, States>& A,
+                  const Eigen::Matrix<double, Outputs, States>& C) {
+  return detail::IsStabilizableImpl<States, Outputs>(A.transpose(),
+                                                     C.transpose());
+}
 
 // Template specializations are used here to make common state-input pairs
 // compile faster.
 template <>
-bool IsStabilizable<2, 1>(const Eigen::Matrix<double, 2, 2>& A,
-                          const Eigen::Matrix<double, 2, 1>& B);
+WPILIB_DLLEXPORT bool IsStabilizable<1, 1>(
+    const Eigen::Matrix<double, 1, 1>& A, const Eigen::Matrix<double, 1, 1>& B);
+
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+WPILIB_DLLEXPORT bool IsStabilizable<2, 1>(
+    const Eigen::Matrix<double, 2, 2>& A, const Eigen::Matrix<double, 2, 1>& B);
 
 /**
  * Converts a Pose2d into a vector of [x, y, theta].
@@ -267,20 +311,24 @@
  *
  * @return The vector.
  */
-Eigen::Matrix<double, 3, 1> PoseToVector(const Pose2d& pose);
+WPILIB_DLLEXPORT
+Eigen::Vector<double, 3> PoseToVector(const Pose2d& pose);
 
 /**
  * Clamps input vector between system's minimum and maximum allowable input.
  *
+ * @tparam Inputs The number of inputs.
  * @param u Input vector to clamp.
+ * @param umin The minimum input magnitude.
+ * @param umax The maximum input magnitude.
  * @return Clamped input vector.
  */
 template <int Inputs>
-Eigen::Matrix<double, Inputs, 1> ClampInputMaxMagnitude(
-    const Eigen::Matrix<double, Inputs, 1>& u,
-    const Eigen::Matrix<double, Inputs, 1>& umin,
-    const Eigen::Matrix<double, Inputs, 1>& umax) {
-  Eigen::Matrix<double, Inputs, 1> result;
+Eigen::Vector<double, Inputs> ClampInputMaxMagnitude(
+    const Eigen::Vector<double, Inputs>& u,
+    const Eigen::Vector<double, Inputs>& umin,
+    const Eigen::Vector<double, Inputs>& umax) {
+  Eigen::Vector<double, Inputs> result;
   for (int i = 0; i < Inputs; ++i) {
     result(i) = std::clamp(u(i), umin(i), umax(i));
   }
@@ -291,14 +339,14 @@
  * Normalize all inputs if any excedes the maximum magnitude. Useful for systems
  * such as differential drivetrains.
  *
+ * @tparam Inputs      The number of inputs.
  * @param u            The input vector.
  * @param maxMagnitude The maximum magnitude any input can have.
- * @param <I>          The number of inputs.
  * @return The normalizedInput
  */
 template <int Inputs>
-Eigen::Matrix<double, Inputs, 1> NormalizeInputVector(
-    const Eigen::Matrix<double, Inputs, 1>& u, double maxMagnitude) {
+Eigen::Vector<double, Inputs> NormalizeInputVector(
+    const Eigen::Vector<double, Inputs>& u, double maxMagnitude) {
   double maxValue = u.template lpNorm<Eigen::Infinity>();
 
   if (maxValue > maxMagnitude) {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
index 14df187..eb7cb76 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <wpi/MathExtras.h>
+#include <wpi/SymbolExports.h>
 
 #include "units/angle.h"
 #include "units/angular_velocity.h"
@@ -19,7 +17,7 @@
  * 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 {
+class WPILIB_DLLEXPORT ArmFeedforward {
  public:
   using Angle = units::radians;
   using Velocity = units::radians_per_second;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
index 134ed97..656f767 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ControlAffinePlantInversionFeedforward.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -35,6 +32,9 @@
  *
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs the number of inputs.
  */
 template <int States, int Inputs>
 class ControlAffinePlantInversionFeedforward {
@@ -50,18 +50,17 @@
    * @param dt The timestep between calls of calculate().
    */
   ControlAffinePlantInversionFeedforward(
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&,
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, Inputs>&)>
           f,
       units::second_t dt)
       : m_dt(dt), m_f(f) {
     m_B = NumericalJacobianU<States, States, Inputs>(
-        f, Eigen::Matrix<double, States, 1>::Zero(),
-        Eigen::Matrix<double, Inputs, 1>::Zero());
+        f, Eigen::Vector<double, States>::Zero(),
+        Eigen::Vector<double, Inputs>::Zero());
 
-    m_r.setZero();
-    Reset(m_r);
+    Reset();
   }
 
   /**
@@ -74,17 +73,16 @@
    * @param dt The timestep between calls of calculate().
    */
   ControlAffinePlantInversionFeedforward(
-      std::function<Eigen::Matrix<double, States, 1>(
-          const Eigen::Matrix<double, States, 1>&)>
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&)>
           f,
       const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
       : m_B(B), m_dt(dt) {
-    m_f = [=](const Eigen::Matrix<double, States, 1>& x,
-              const Eigen::Matrix<double, Inputs, 1>& u)
-        -> Eigen::Matrix<double, States, 1> { return f(x); };
+    m_f = [=](const Eigen::Vector<double, States>& x,
+              const Eigen::Vector<double, Inputs>& u)
+        -> Eigen::Vector<double, States> { return f(x); };
 
-    m_r.setZero();
-    Reset(m_r);
+    Reset();
   }
 
   ControlAffinePlantInversionFeedforward(
@@ -97,12 +95,12 @@
    *
    * @return The calculated feedforward.
    */
-  const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
+  const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
 
   /**
    * Returns an element of the previously calculated feedforward.
    *
-   * @param row Row of uff.
+   * @param i Row of uff.
    *
    * @return The row of the calculated feedforward.
    */
@@ -113,7 +111,7 @@
    *
    * @return The current reference vector.
    */
-  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+  const Eigen::Vector<double, States>& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -129,7 +127,7 @@
    *
    * @param initialState The initial state vector.
    */
-  void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
     m_r = initialState;
     m_uff.setZero();
   }
@@ -147,16 +145,16 @@
    * future reference. This uses the internally stored "current"
    * reference.
    *
-   * If this method is used the initial state of the system is the one
-   * set using Reset(const Eigen::Matrix<double, States, 1>&).
-   * If the initial state is not set it defaults to a zero vector.
+   * If this method is used the initial state of the system is the one set using
+   * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+   * set it defaults to a zero vector.
    *
    * @param nextR The reference state of the future timestep (k + dt).
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& nextR) {
     return Calculate(m_r, nextR);
   }
 
@@ -168,13 +166,13 @@
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& r,
-      const Eigen::Matrix<double, States, 1>& nextR) {
-    Eigen::Matrix<double, States, 1> rDot = (nextR - r) / m_dt.to<double>();
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& r,
+      const Eigen::Vector<double, States>& nextR) {
+    Eigen::Vector<double, States> rDot = (nextR - r) / m_dt.value();
 
     m_uff = m_B.householderQr().solve(
-        rDot - m_f(r, Eigen::Matrix<double, Inputs, 1>::Zero()));
+        rDot - m_f(r, Eigen::Vector<double, Inputs>::Zero()));
 
     m_r = nextR;
     return m_uff;
@@ -188,16 +186,16 @@
   /**
    * The model dynamics.
    */
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_f;
 
   // Current reference
-  Eigen::Matrix<double, States, 1> m_r;
+  Eigen::Vector<double, States> m_r;
 
   // Computed feedforward
-  Eigen::Matrix<double, Inputs, 1> m_uff;
+  Eigen::Vector<double, Inputs> m_uff;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
index b82d960..269a7e6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <wpi/MathExtras.h>
 
+#include "units/time.h"
 #include "units/voltage.h"
 
 namespace frc {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
new file mode 100644
index 0000000..398a872
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/HolonomicDriveController.h
@@ -0,0 +1,112 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.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"
+#include "units/angle.h"
+#include "units/velocity.h"
+
+namespace frc {
+/**
+ * This holonomic drive controller can be used to follow trajectories using a
+ * holonomic drivetrain (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 WPILIB_DLLEXPORT 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(
+      frc2::PIDController xController, frc2::PIDController yController,
+      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 tolerance 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;
+  Rotation2d m_rotationError;
+  Pose2d m_poseTolerance;
+  bool m_enabled = true;
+
+  frc2::PIDController m_xController;
+  frc2::PIDController m_yController;
+  ProfiledPIDController<units::radian> m_thetaController;
+
+  bool m_firstRun = true;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
index ea86d90..519368d 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearPlantInversionFeedforward.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -27,6 +24,9 @@
  *
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
  */
 template <int States, int Inputs>
 class LinearPlantInversionFeedforward {
@@ -34,8 +34,9 @@
   /**
    * Constructs a feedforward with the given plant.
    *
-   * @param plant     The plant being controlled.
-   * @param dtSeconds Discretization timestep.
+   * @tparam Outputs The number of outputs.
+   * @param plant The plant being controlled.
+   * @param dt    Discretization timestep.
    */
   template <int Outputs>
   LinearPlantInversionFeedforward(
@@ -45,18 +46,16 @@
   /**
    * Constructs a feedforward with the given coefficients.
    *
-   * @param A         Continuous system matrix of the plant being controlled.
-   * @param B         Continuous input matrix of the plant being controlled.
-   * @param dtSeconds Discretization timestep.
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param dt Discretization timestep.
    */
   LinearPlantInversionFeedforward(
       const Eigen::Matrix<double, States, States>& A,
       const Eigen::Matrix<double, States, Inputs>& B, units::second_t dt)
       : m_dt(dt) {
     DiscretizeAB<States, Inputs>(A, B, dt, &m_A, &m_B);
-
-    m_r.setZero();
-    Reset(m_r);
+    Reset();
   }
 
   /**
@@ -64,12 +63,12 @@
    *
    * @return The calculated feedforward.
    */
-  const Eigen::Matrix<double, Inputs, 1>& Uff() const { return m_uff; }
+  const Eigen::Vector<double, Inputs>& Uff() const { return m_uff; }
 
   /**
    * Returns an element of the previously calculated feedforward.
    *
-   * @param row Row of uff.
+   * @param i Row of uff.
    *
    * @return The row of the calculated feedforward.
    */
@@ -80,7 +79,7 @@
    *
    * @return The current reference vector.
    */
-  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+  const Eigen::Vector<double, States>& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -96,7 +95,7 @@
    *
    * @param initialState The initial state vector.
    */
-  void Reset(const Eigen::Matrix<double, States, 1>& initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
     m_r = initialState;
     m_uff.setZero();
   }
@@ -114,17 +113,17 @@
    * future reference. This uses the internally stored "current"
    * reference.
    *
-   * If this method is used the initial state of the system is the one
-   * set using Reset(const Eigen::Matrix<double, States, 1>&).
-   * If the initial state is not set it defaults to a zero vector.
+   * If this method is used the initial state of the system is the one set using
+   * Reset(const Eigen::Vector<double, States>&). If the initial state is not
+   * set it defaults to a zero vector.
    *
    * @param nextR The reference state of the future timestep (k + dt).
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& nextR) {
-    return Calculate(m_r, nextR);
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& nextR) {
+    return Calculate(m_r, nextR);  // NOLINT
   }
 
   /**
@@ -135,9 +134,9 @@
    *
    * @return The calculated feedforward.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& r,
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& r,
+      const Eigen::Vector<double, States>& nextR) {
     m_uff = m_B.householderQr().solve(nextR - (m_A * r));
     m_r = nextR;
     return m_uff;
@@ -150,10 +149,10 @@
   units::second_t m_dt;
 
   // Current reference
-  Eigen::Matrix<double, States, 1> m_r;
+  Eigen::Vector<double, States> m_r;
 
   // Computed feedforward
-  Eigen::Matrix<double, Inputs, 1> m_uff;
+  Eigen::Vector<double, Inputs> m_uff;
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
index f448957..c934e0a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/LinearQuadraticRegulator.h
@@ -1,21 +1,26 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
+#include <frc/fmt/Eigen.h>
 
+#include <string>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
-#include "Eigen/src/Cholesky/LLT.h"
+#include "Eigen/Eigenvalues"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
 #include "frc/system/LinearSystem.h"
 #include "units/time.h"
+#include "unsupported/Eigen/MatrixFunctions"
+#include "wpimath/MathShared.h"
 
 namespace frc {
 namespace detail {
@@ -27,6 +32,9 @@
  *
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
  */
 template <int States, int Inputs>
 class LinearQuadraticRegulatorImpl {
@@ -42,8 +50,8 @@
   template <int Outputs>
   LinearQuadraticRegulatorImpl(
       const LinearSystem<States, Inputs, Outputs>& plant,
-      const std::array<double, States>& Qelems,
-      const std::array<double, Inputs>& Relems, units::second_t dt)
+      const wpi::array<double, States>& Qelems,
+      const wpi::array<double, Inputs>& Relems, units::second_t dt)
       : LinearQuadraticRegulatorImpl(plant.A(), plant.B(), Qelems, Relems, dt) {
   }
 
@@ -58,8 +66,8 @@
    */
   LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
                                const Eigen::Matrix<double, States, Inputs>& B,
-                               const std::array<double, States>& Qelems,
-                               const std::array<double, Inputs>& Relems,
+                               const wpi::array<double, States>& Qelems,
+                               const wpi::array<double, Inputs>& Relems,
                                units::second_t dt)
       : LinearQuadraticRegulatorImpl(A, B, MakeCostMatrix(Qelems),
                                      MakeCostMatrix(Relems), dt) {}
@@ -67,11 +75,11 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
-   * @param A      Continuous system matrix of the plant being controlled.
-   * @param B      Continuous input matrix of the plant being controlled.
-   * @param Q      The state cost matrix.
-   * @param R      The input cost matrix.
-   * @param dt     Discretization timestep.
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param Q  The state cost matrix.
+   * @param R  The input cost matrix.
+   * @param dt Discretization timestep.
    */
   LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
                                const Eigen::Matrix<double, States, Inputs>& B,
@@ -82,11 +90,54 @@
     Eigen::Matrix<double, States, Inputs> discB;
     DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
 
+    if (!IsStabilizable<States, Inputs>(discA, discB)) {
+      std::string msg = fmt::format(
+          "The system passed to the LQR is uncontrollable!\n\nA =\n{}\nB "
+          "=\n{}\n",
+          discA, discB);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
+    }
+
     Eigen::Matrix<double, States, States> S =
         drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R);
-    Eigen::Matrix<double, Inputs, Inputs> tmp =
-        discB.transpose() * S * discB + R;
-    m_K = tmp.llt().solve(discB.transpose() * S * discA);
+
+    // K = (BᵀSB + R)⁻¹BᵀSA
+    m_K = (discB.transpose() * S * discB + R)
+              .llt()
+              .solve(discB.transpose() * S * discA);
+
+    Reset();
+  }
+
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param Q  The state cost matrix.
+   * @param R  The input cost matrix.
+   * @param N  The state-input cross-term cost matrix.
+   * @param dt Discretization timestep.
+   */
+  LinearQuadraticRegulatorImpl(const Eigen::Matrix<double, States, States>& A,
+                               const Eigen::Matrix<double, States, Inputs>& B,
+                               const Eigen::Matrix<double, States, States>& Q,
+                               const Eigen::Matrix<double, Inputs, Inputs>& R,
+                               const Eigen::Matrix<double, States, Inputs>& N,
+                               units::second_t dt) {
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(A, B, dt, &discA, &discB);
+
+    Eigen::Matrix<double, States, States> S =
+        drake::math::DiscreteAlgebraicRiccatiEquation(discA, discB, Q, R, N);
+
+    // K = (BᵀSB + R)⁻¹(BᵀSA + Nᵀ)
+    m_K = (B.transpose() * S * B + R)
+              .llt()
+              .solve(discB.transpose() * S * discA + N.transpose());
 
     Reset();
   }
@@ -113,7 +164,7 @@
    *
    * @return The reference vector.
    */
-  const Eigen::Matrix<double, States, 1>& R() const { return m_r; }
+  const Eigen::Vector<double, States>& R() const { return m_r; }
 
   /**
    * Returns an element of the reference vector r.
@@ -129,7 +180,7 @@
    *
    * @return The control input.
    */
-  const Eigen::Matrix<double, Inputs, 1>& U() const { return m_u; }
+  const Eigen::Vector<double, Inputs>& U() const { return m_u; }
 
   /**
    * Returns an element of the control input vector u.
@@ -153,8 +204,8 @@
    *
    * @param x The current state x.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& x) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& x) {
     m_u = m_K * (m_r - x);
     return m_u;
   }
@@ -165,19 +216,45 @@
    * @param x     The current state x.
    * @param nextR The next reference vector r.
    */
-  Eigen::Matrix<double, Inputs, 1> Calculate(
-      const Eigen::Matrix<double, States, 1>& x,
-      const Eigen::Matrix<double, States, 1>& nextR) {
+  Eigen::Vector<double, Inputs> Calculate(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, States>& nextR) {
     m_r = nextR;
     return Calculate(x);
   }
 
+  /**
+   * Adjusts LQR controller gain to compensate for a pure time delay in the
+   * input.
+   *
+   * Linear-Quadratic regulator controller gains tend to be aggressive. If
+   * sensor measurements are time-delayed too long, the LQR may be unstable.
+   * However, if we know the amount of delay, we can compute the control based
+   * on where the system will be after the time delay.
+   *
+   * See https://file.tavsys.net/control/controls-engineering-in-frc.pdf
+   * appendix C.4 for a derivation.
+   *
+   * @param plant      The plant being controlled.
+   * @param dt         Discretization timestep.
+   * @param inputDelay Input time delay.
+   */
+  template <int Outputs>
+  void LatencyCompensate(const LinearSystem<States, Inputs, Outputs>& plant,
+                         units::second_t dt, units::second_t inputDelay) {
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, Inputs> discB;
+    DiscretizeAB<States, Inputs>(plant.A(), plant.B(), dt, &discA, &discB);
+
+    m_K = m_K * (discA - discB * m_K).pow(inputDelay / dt);
+  }
+
  private:
   // Current reference
-  Eigen::Matrix<double, States, 1> m_r;
+  Eigen::Vector<double, States> m_r;
 
   // Computed controller output
-  Eigen::Matrix<double, Inputs, 1> m_u;
+  Eigen::Vector<double, Inputs> m_u;
 
   // Controller gain
   Eigen::Matrix<double, Inputs, States> m_K;
@@ -192,15 +269,16 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
-   * @param system The plant being controlled.
+   * @tparam Outputs The number of outputs.
+   * @param plant  The plant being controlled.
    * @param Qelems The maximum desired error tolerance for each state.
    * @param Relems The maximum desired control effort for each input.
    * @param dt     Discretization timestep.
    */
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<States, Inputs, Outputs>& plant,
-                           const std::array<double, States>& Qelems,
-                           const std::array<double, Inputs>& Relems,
+                           const wpi::array<double, States>& Qelems,
+                           const wpi::array<double, Inputs>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
@@ -215,8 +293,8 @@
    */
   LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
                            const Eigen::Matrix<double, States, Inputs>& B,
-                           const std::array<double, States>& Qelems,
-                           const std::array<double, Inputs>& Relems,
+                           const wpi::array<double, States>& Qelems,
+                           const wpi::array<double, Inputs>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(A, B, MakeCostMatrix(Qelems),
                                  MakeCostMatrix(Relems), dt) {}
@@ -224,11 +302,11 @@
   /**
    * Constructs a controller with the given coefficients and plant.
    *
-   * @param A      Continuous system matrix of the plant being controlled.
-   * @param B      Continuous input matrix of the plant being controlled.
-   * @param Q      The state cost matrix.
-   * @param R      The input cost matrix.
-   * @param dt     Discretization timestep.
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param Q  The state cost matrix.
+   * @param R  The input cost matrix.
+   * @param dt Discretization timestep.
    */
   LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
                            const Eigen::Matrix<double, States, Inputs>& B,
@@ -237,6 +315,25 @@
                            units::second_t dt)
       : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q, R, dt} {}
 
+  /**
+   * Constructs a controller with the given coefficients and plant.
+   *
+   * @param A  Continuous system matrix of the plant being controlled.
+   * @param B  Continuous input matrix of the plant being controlled.
+   * @param Q  The state cost matrix.
+   * @param R  The input cost matrix.
+   * @param N  The state-input cross-term cost matrix.
+   * @param dt Discretization timestep.
+   */
+  LinearQuadraticRegulator(const Eigen::Matrix<double, States, States>& A,
+                           const Eigen::Matrix<double, States, Inputs>& B,
+                           const Eigen::Matrix<double, States, States>& Q,
+                           const Eigen::Matrix<double, Inputs, Inputs>& R,
+                           const Eigen::Matrix<double, States, Inputs>& N,
+                           units::second_t dt)
+      : detail::LinearQuadraticRegulatorImpl<States, Inputs>{A, B, Q,
+                                                             R, N, dt} {}
+
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
   LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
 };
@@ -244,20 +341,20 @@
 // Template specializations are used here to make common state-input pairs
 // compile faster.
 template <>
-class LinearQuadraticRegulator<1, 1>
+class WPILIB_DLLEXPORT LinearQuadraticRegulator<1, 1>
     : public detail::LinearQuadraticRegulatorImpl<1, 1> {
  public:
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<1, 1, Outputs>& plant,
-                           const std::array<double, 1>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 1>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
                            const Eigen::Matrix<double, 1, 1>& B,
-                           const std::array<double, 1>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 1>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt);
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
@@ -266,6 +363,13 @@
                            const Eigen::Matrix<double, 1, 1>& R,
                            units::second_t dt);
 
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 1, 1>& A,
+                           const Eigen::Matrix<double, 1, 1>& B,
+                           const Eigen::Matrix<double, 1, 1>& Q,
+                           const Eigen::Matrix<double, 1, 1>& R,
+                           const Eigen::Matrix<double, 1, 1>& N,
+                           units::second_t dt);
+
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
   LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
 };
@@ -273,20 +377,20 @@
 // Template specializations are used here to make common state-input pairs
 // compile faster.
 template <>
-class LinearQuadraticRegulator<2, 1>
+class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 1>
     : public detail::LinearQuadraticRegulatorImpl<2, 1> {
  public:
   template <int Outputs>
   LinearQuadraticRegulator(const LinearSystem<2, 1, Outputs>& plant,
-                           const std::array<double, 2>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 2>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt)
       : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
                            const Eigen::Matrix<double, 2, 1>& B,
-                           const std::array<double, 2>& Qelems,
-                           const std::array<double, 1>& Relems,
+                           const wpi::array<double, 2>& Qelems,
+                           const wpi::array<double, 1>& Relems,
                            units::second_t dt);
 
   LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
@@ -295,6 +399,49 @@
                            const Eigen::Matrix<double, 1, 1>& R,
                            units::second_t dt);
 
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
+                           const Eigen::Matrix<double, 2, 1>& B,
+                           const Eigen::Matrix<double, 2, 2>& Q,
+                           const Eigen::Matrix<double, 1, 1>& R,
+                           const Eigen::Matrix<double, 2, 1>& N,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
+  LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
+};
+
+// Template specializations are used here to make common state-input pairs
+// compile faster.
+template <>
+class WPILIB_DLLEXPORT LinearQuadraticRegulator<2, 2>
+    : public detail::LinearQuadraticRegulatorImpl<2, 2> {
+ public:
+  template <int Outputs>
+  LinearQuadraticRegulator(const LinearSystem<2, 2, Outputs>& plant,
+                           const wpi::array<double, 2>& Qelems,
+                           const wpi::array<double, 2>& Relems,
+                           units::second_t dt)
+      : LinearQuadraticRegulator(plant.A(), plant.B(), Qelems, Relems, dt) {}
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
+                           const Eigen::Matrix<double, 2, 2>& B,
+                           const wpi::array<double, 2>& Qelems,
+                           const wpi::array<double, 2>& Relems,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
+                           const Eigen::Matrix<double, 2, 2>& B,
+                           const Eigen::Matrix<double, 2, 2>& Q,
+                           const Eigen::Matrix<double, 2, 2>& R,
+                           units::second_t dt);
+
+  LinearQuadraticRegulator(const Eigen::Matrix<double, 2, 2>& A,
+                           const Eigen::Matrix<double, 2, 2>& B,
+                           const Eigen::Matrix<double, 2, 2>& Q,
+                           const Eigen::Matrix<double, 2, 2>& R,
+                           const Eigen::Matrix<double, 2, 2>& N,
+                           units::second_t dt);
+
   LinearQuadraticRegulator(LinearQuadraticRegulator&&) = default;
   LinearQuadraticRegulator& operator=(LinearQuadraticRegulator&&) = default;
 };
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h
new file mode 100644
index 0000000..98625f8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/PIDController.h
@@ -0,0 +1,249 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <limits>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "units/time.h"
+
+namespace frc2 {
+
+/**
+ * Implements a PID control loop.
+ */
+class WPILIB_DLLEXPORT PIDController
+    : public wpi::Sendable,
+      public wpi::SendableHelper<PIDController> {
+ public:
+  /**
+   * Allocates a PIDController with the given constants for Kp, Ki, and Kd.
+   *
+   * @param Kp     The proportional coefficient.
+   * @param Ki     The integral coefficient.
+   * @param Kd     The derivative coefficient.
+   * @param period The period between controller updates in seconds. The
+   *               default is 20 milliseconds. Must be non-zero and positive.
+   */
+  PIDController(double Kp, double Ki, double Kd,
+                units::second_t period = 20_ms);
+
+  ~PIDController() override = default;
+
+  PIDController(const PIDController&) = default;
+  PIDController& operator=(const PIDController&) = default;
+  PIDController(PIDController&&) = default;
+  PIDController& operator=(PIDController&&) = default;
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  void SetPID(double Kp, double Ki, double Kd);
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  void SetP(double Kp);
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  void SetI(double Ki);
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  void SetD(double Kd);
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const;
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const;
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const;
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  units::second_t GetPeriod() const;
+
+  /**
+   * Sets the setpoint for the PIDController.
+   *
+   * @param setpoint The desired setpoint.
+   */
+  void SetSetpoint(double setpoint);
+
+  /**
+   * Returns the current setpoint of the PIDController.
+   *
+   * @return The current setpoint.
+   */
+  double GetSetpoint() const;
+
+  /**
+   * 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.
+   */
+  bool AtSetpoint() const;
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinuousInput(double minimumInput, double maximumInput);
+
+  /**
+   * Disables continuous input.
+   */
+  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
+   * output rather than the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  void SetIntegratorRange(double minimumIntegral, double maximumIntegral);
+
+  /**
+   * Sets the error which is considered tolerable for use with AtSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  void SetTolerance(
+      double positionTolerance,
+      double velocityTolerance = std::numeric_limits<double>::infinity());
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   */
+  double GetPositionError() const;
+
+  /**
+   * Returns the velocity error.
+   */
+  double GetVelocityError() const;
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  double Calculate(double measurement);
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param setpoint The new setpoint of the controller.
+   */
+  double Calculate(double measurement, double setpoint);
+
+  /**
+   * Reset the previous error, the integral term, and disable the controller.
+   */
+  void Reset();
+
+  void InitSendable(wpi::SendableBuilder& builder) override;
+
+ private:
+  // Factor for "proportional" control
+  double m_Kp;
+
+  // Factor for "integral" control
+  double m_Ki;
+
+  // Factor for "derivative" control
+  double m_Kd;
+
+  // The period (in seconds) of the control loop running this controller
+  units::second_t m_period;
+
+  double m_maximumIntegral = 1.0;
+
+  double m_minimumIntegral = -1.0;
+
+  double m_maximumInput = 0;
+
+  double m_minimumInput = 0;
+
+  // Do the endpoints wrap around? eg. Absolute encoder
+  bool m_continuous = false;
+
+  // The error at the time of the most recent call to Calculate()
+  double m_positionError = 0;
+  double m_velocityError = 0;
+
+  // The error at the time of the second-most-recent call to Calculate() (used
+  // to compute velocity)
+  double m_prevError = 0;
+
+  // The sum of the errors for use in the integral calc
+  double m_totalError = 0;
+
+  // The error that is considered at setpoint.
+  double m_positionTolerance = 0.05;
+  double m_velocityTolerance = std::numeric_limits<double>::infinity();
+
+  double m_setpoint = 0;
+  double m_measurement = 0;
+};
+
+}  // namespace frc2
+
+namespace frc {
+
+using frc2::PIDController;
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
new file mode 100644
index 0000000..e0b10c7
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/ProfiledPIDController.h
@@ -0,0 +1,367 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <limits>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/sendable/Sendable.h>
+#include <wpi/sendable/SendableBuilder.h>
+#include <wpi/sendable/SendableHelper.h>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/PIDController.h"
+#include "frc/trajectory/TrapezoidProfile.h"
+#include "units/time.h"
+
+namespace frc {
+namespace detail {
+WPILIB_DLLEXPORT
+void ReportProfiledPIDController();
+}  // namespace detail
+
+/**
+ * Implements a PID control loop whose setpoint is constrained by a trapezoid
+ * profile.
+ */
+template <class Distance>
+class ProfiledPIDController
+    : public wpi::Sendable,
+      public wpi::SendableHelper<ProfiledPIDController<Distance>> {
+ public:
+  using Distance_t = units::unit_t<Distance>;
+  using Velocity =
+      units::compound_unit<Distance, units::inverse<units::seconds>>;
+  using Velocity_t = units::unit_t<Velocity>;
+  using Acceleration =
+      units::compound_unit<Velocity, units::inverse<units::seconds>>;
+  using Acceleration_t = units::unit_t<Acceleration>;
+  using State = typename TrapezoidProfile<Distance>::State;
+  using Constraints = typename TrapezoidProfile<Distance>::Constraints;
+
+  /**
+   * Allocates a ProfiledPIDController with the given constants for Kp, Ki, and
+   * Kd. Users should call reset() when they first start running the controller
+   * to avoid unwanted behavior.
+   *
+   * @param Kp          The proportional coefficient.
+   * @param Ki          The integral coefficient.
+   * @param Kd          The derivative coefficient.
+   * @param constraints Velocity and acceleration constraints for goal.
+   * @param period      The period between controller updates in seconds. The
+   *                    default is 20 milliseconds.
+   */
+  ProfiledPIDController(double Kp, double Ki, double Kd,
+                        Constraints constraints, units::second_t period = 20_ms)
+      : m_controller(Kp, Ki, Kd, period), m_constraints(constraints) {
+    detail::ReportProfiledPIDController();
+  }
+
+  ~ProfiledPIDController() override = default;
+
+  ProfiledPIDController(const ProfiledPIDController&) = default;
+  ProfiledPIDController& operator=(const ProfiledPIDController&) = default;
+  ProfiledPIDController(ProfiledPIDController&&) = default;
+  ProfiledPIDController& operator=(ProfiledPIDController&&) = default;
+
+  /**
+   * Sets the PID Controller gain parameters.
+   *
+   * Sets the proportional, integral, and differential coefficients.
+   *
+   * @param Kp Proportional coefficient
+   * @param Ki Integral coefficient
+   * @param Kd Differential coefficient
+   */
+  void SetPID(double Kp, double Ki, double Kd) {
+    m_controller.SetPID(Kp, Ki, Kd);
+  }
+
+  /**
+   * Sets the proportional coefficient of the PID controller gain.
+   *
+   * @param Kp proportional coefficient
+   */
+  void SetP(double Kp) { m_controller.SetP(Kp); }
+
+  /**
+   * Sets the integral coefficient of the PID controller gain.
+   *
+   * @param Ki integral coefficient
+   */
+  void SetI(double Ki) { m_controller.SetI(Ki); }
+
+  /**
+   * Sets the differential coefficient of the PID controller gain.
+   *
+   * @param Kd differential coefficient
+   */
+  void SetD(double Kd) { m_controller.SetD(Kd); }
+
+  /**
+   * Gets the proportional coefficient.
+   *
+   * @return proportional coefficient
+   */
+  double GetP() const { return m_controller.GetP(); }
+
+  /**
+   * Gets the integral coefficient.
+   *
+   * @return integral coefficient
+   */
+  double GetI() const { return m_controller.GetI(); }
+
+  /**
+   * Gets the differential coefficient.
+   *
+   * @return differential coefficient
+   */
+  double GetD() const { return m_controller.GetD(); }
+
+  /**
+   * Gets the period of this controller.
+   *
+   * @return The period of the controller.
+   */
+  units::second_t GetPeriod() const { return m_controller.GetPeriod(); }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(State goal) { m_goal = goal; }
+
+  /**
+   * Sets the goal for the ProfiledPIDController.
+   *
+   * @param goal The desired unprofiled setpoint.
+   */
+  void SetGoal(Distance_t goal) { m_goal = {goal, Velocity_t(0)}; }
+
+  /**
+   * Gets the goal for the ProfiledPIDController.
+   */
+  State GetGoal() const { return m_goal; }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtGoal() const { return AtSetpoint() && m_goal == m_setpoint; }
+
+  /**
+   * Set velocity and acceleration constraints for goal.
+   *
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  void SetConstraints(Constraints constraints) { m_constraints = constraints; }
+
+  /**
+   * Returns the current setpoint of the ProfiledPIDController.
+   *
+   * @return The current setpoint.
+   */
+  State GetSetpoint() const { return m_setpoint; }
+
+  /**
+   * Returns true if the error is within the tolerance of the error.
+   *
+   * Currently this just reports on target as the actual value passes through
+   * the setpoint. Ideally it should be based on being within the tolerance for
+   * some period of time.
+   *
+   * This will return false until at least one input value has been computed.
+   */
+  bool AtSetpoint() const { return m_controller.AtSetpoint(); }
+
+  /**
+   * Enables continuous input.
+   *
+   * Rather then using the max and min input range as constraints, it considers
+   * them to be the same point and automatically calculates the shortest route
+   * to the setpoint.
+   *
+   * @param minimumInput The minimum value expected from the input.
+   * @param maximumInput The maximum value expected from the input.
+   */
+  void EnableContinuousInput(Distance_t minimumInput, Distance_t maximumInput) {
+    m_controller.EnableContinuousInput(minimumInput.value(),
+                                       maximumInput.value());
+    m_minimumInput = minimumInput;
+    m_maximumInput = maximumInput;
+  }
+
+  /**
+   * Disables continuous input.
+   */
+  void DisableContinuousInput() { m_controller.DisableContinuousInput(); }
+
+  /**
+   * Sets the minimum and maximum values for the integrator.
+   *
+   * When the cap is reached, the integrator value is added to the controller
+   * output rather than the integrator value times the integral gain.
+   *
+   * @param minimumIntegral The minimum value of the integrator.
+   * @param maximumIntegral The maximum value of the integrator.
+   */
+  void SetIntegratorRange(double minimumIntegral, double maximumIntegral) {
+    m_controller.SetIntegratorRange(minimumIntegral, maximumIntegral);
+  }
+
+  /**
+   * Sets the error which is considered tolerable for use with
+   * AtSetpoint().
+   *
+   * @param positionTolerance Position error which is tolerable.
+   * @param velocityTolerance Velocity error which is tolerable.
+   */
+  void SetTolerance(
+      Distance_t positionTolerance,
+      Velocity_t velocityTolerance = std::numeric_limits<double>::infinity()) {
+    m_controller.SetTolerance(positionTolerance.value(),
+                              velocityTolerance.value());
+  }
+
+  /**
+   * Returns the difference between the setpoint and the measurement.
+   *
+   * @return The error.
+   */
+  Distance_t GetPositionError() const {
+    return Distance_t(m_controller.GetPositionError());
+  }
+
+  /**
+   * Returns the change in error per second.
+   */
+  Velocity_t GetVelocityError() const {
+    return Velocity_t(m_controller.GetVelocityError());
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   */
+  double Calculate(Distance_t measurement) {
+    if (m_controller.IsContinuousInputEnabled()) {
+      // Get error which is smallest distance between goal and measurement
+      auto errorBound = (m_maximumInput - m_minimumInput) / 2.0;
+      auto goalMinDistance = frc::InputModulus<Distance_t>(
+          m_goal.position - measurement, -errorBound, errorBound);
+      auto setpointMinDistance = frc::InputModulus<Distance_t>(
+          m_setpoint.position - measurement, -errorBound, errorBound);
+
+      // 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.value(),
+                                  m_setpoint.position.value());
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  double Calculate(Distance_t measurement, State goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal The new goal of the controller.
+   */
+  double Calculate(Distance_t measurement, Distance_t goal) {
+    SetGoal(goal);
+    return Calculate(measurement);
+  }
+
+  /**
+   * Returns the next output of the PID controller.
+   *
+   * @param measurement The current measurement of the process variable.
+   * @param goal        The new goal of the controller.
+   * @param constraints Velocity and acceleration constraints for goal.
+   */
+  double Calculate(
+      Distance_t measurement, Distance_t goal,
+      typename frc::TrapezoidProfile<Distance>::Constraints constraints) {
+    SetConstraints(constraints);
+    return Calculate(measurement, goal);
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measurement The current measured State of the system.
+   */
+  void Reset(const State& measurement) {
+    m_controller.Reset();
+    m_setpoint = measurement;
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system.
+   * @param measuredVelocity The current measured velocity of the system.
+   */
+  void Reset(Distance_t measuredPosition, Velocity_t measuredVelocity) {
+    Reset(State{measuredPosition, measuredVelocity});
+  }
+
+  /**
+   * Reset the previous error and the integral term.
+   *
+   * @param measuredPosition The current measured position of the system. The
+   * velocity is assumed to be zero.
+   */
+  void Reset(Distance_t measuredPosition) {
+    Reset(measuredPosition, Velocity_t(0));
+  }
+
+  void InitSendable(wpi::SendableBuilder& builder) override {
+    builder.SetSmartDashboardType("ProfiledPIDController");
+    builder.AddDoubleProperty(
+        "p", [this] { return GetP(); }, [this](double value) { SetP(value); });
+    builder.AddDoubleProperty(
+        "i", [this] { return GetI(); }, [this](double value) { SetI(value); });
+    builder.AddDoubleProperty(
+        "d", [this] { return GetD(); }, [this](double value) { SetD(value); });
+    builder.AddDoubleProperty(
+        "goal", [this] { return GetGoal().position.value(); },
+        [this](double value) { SetGoal(Distance_t{value}); });
+  }
+
+ 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;
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/RamseteController.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/RamseteController.h
new file mode 100644
index 0000000..022fff9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/RamseteController.h
@@ -0,0 +1,120 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/ChassisSpeeds.h"
+#include "frc/trajectory/Trajectory.h"
+#include "units/angular_velocity.h"
+#include "units/velocity.h"
+
+namespace frc {
+
+/**
+ * Ramsete is a nonlinear time-varying feedback controller for unicycle models
+ * that drives the model to a desired pose along a two-dimensional trajectory.
+ * Why would we need a nonlinear control law in addition to the linear ones we
+ * have used so far like PID? If we use the original approach with PID
+ * controllers for left and right position and velocity states, the controllers
+ * only deal with the local pose. If the robot deviates from the path, there is
+ * no way for the controllers to correct and the robot may not reach the desired
+ * global pose. This is due to multiple endpoints existing for the robot which
+ * have the same encoder path arc lengths.
+ *
+ * Instead of using wheel path arc lengths (which are in the robot's local
+ * coordinate frame), nonlinear controllers like pure pursuit and Ramsete use
+ * global pose. The controller uses this extra information to guide a linear
+ * reference tracker like the PID controllers back in by adjusting the
+ * references of the PID controllers.
+ *
+ * The paper "Control of Wheeled Mobile Robots: An Experimental Overview"
+ * describes a nonlinear controller for a wheeled vehicle with unicycle-like
+ * kinematics; a global pose consisting of x, y, and theta; and a desired pose
+ * consisting of x_d, y_d, and theta_d. We call it Ramsete because that's the
+ * acronym for the title of the book it came from in Italian ("Robotica
+ * Articolata e Mobile per i SErvizi e le TEcnologie").
+ *
+ * See <https://file.tavsys.net/control/controls-engineering-in-frc.pdf> section
+ * on Ramsete unicycle controller for a derivation and analysis.
+ */
+class WPILIB_DLLEXPORT RamseteController {
+ public:
+  /**
+   * Construct a Ramsete unicycle controller.
+   *
+   * @param b    Tuning parameter (b > 0) for which larger values make
+   *             convergence more aggressive like a proportional term.
+   * @param zeta Tuning parameter (0 < zeta < 1) for which larger values provide
+   *             more damping in response.
+   */
+  RamseteController(double b, double zeta);
+
+  /**
+   * Construct a Ramsete unicycle controller. The default arguments for
+   * b and zeta of 2.0 and 0.7 have been well-tested to produce desirable
+   * results.
+   */
+  RamseteController() : RamseteController(2.0, 0.7) {}
+
+  /**
+   * 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& poseTolerance);
+
+  /**
+   * Returns the next output of the Ramsete 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 angularVelocityRef The desired angular velocity.
+   */
+  ChassisSpeeds Calculate(const Pose2d& currentPose, const Pose2d& poseRef,
+                          units::meters_per_second_t linearVelocityRef,
+                          units::radians_per_second_t angularVelocityRef);
+
+  /**
+   * Returns the next output of the Ramsete 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.
+   */
+  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/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
index 1afab40..df1a52c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h
@@ -1,14 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <wpi/MathExtras.h>
 
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/system/plant/LinearSystemId.h"
 #include "units/time.h"
 #include "units/voltage.h"
 
@@ -55,6 +55,28 @@
     return kS * wpi::sgn(velocity) + kV * velocity + kA * acceleration;
   }
 
+  /**
+   * Calculates the feedforward from the gains and setpoints.
+   *
+   * @param currentVelocity The current velocity setpoint, in distance per
+   *                        second.
+   * @param nextVelocity    The next velocity setpoint, in distance per second.
+   * @param dt              Time between velocity setpoints in seconds.
+   * @return The computed feedforward, in volts.
+   */
+  units::volt_t Calculate(units::unit_t<Velocity> currentVelocity,
+                          units::unit_t<Velocity> nextVelocity,
+                          units::second_t dt) const {
+    auto plant = LinearSystemId::IdentifyVelocitySystem<Distance>(kV, kA);
+    LinearPlantInversionFeedforward<1, 1> feedforward{plant, dt};
+
+    Eigen::Vector<double, 1> r{currentVelocity.value()};
+    Eigen::Vector<double, 1> nextR{nextVelocity.value()};
+
+    return kS * wpi::sgn(currentVelocity.value()) +
+           units::volt_t{feedforward.Calculate(r, nextR)(0)};
+  }
+
   // Rearranging the main equation from the calculate() method yields the
   // formulas for the methods below:
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
new file mode 100644
index 0000000..3ddabc1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/AngleStatistics.h
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/numbers>
+
+#include "Eigen/Core"
+#include "frc/MathUtil.h"
+
+namespace frc {
+
+/**
+ * Subtracts a and b while normalizing the resulting value in the selected row
+ * as if it were an angle.
+ *
+ * @tparam States The number of states.
+ * @param a A vector to subtract from.
+ * @param b A vector to subtract with.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+Eigen::Vector<double, States> AngleResidual(
+    const Eigen::Vector<double, States>& a,
+    const Eigen::Vector<double, States>& b, int angleStateIdx) {
+  Eigen::Vector<double, States> ret = a - b;
+  ret[angleStateIdx] =
+      AngleModulus(units::radian_t{ret[angleStateIdx]}).value();
+  return ret;
+}
+
+/**
+ * Returns a function that subtracts two vectors while normalizing the resulting
+ * value in the selected row as if it were an angle.
+ *
+ * @tparam States The number of states.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+std::function<Eigen::Vector<double, States>(
+    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+AngleResidual(int angleStateIdx) {
+  return [=](auto a, auto b) {
+    return AngleResidual<States>(a, b, angleStateIdx);
+  };
+}
+
+/**
+ * Adds a and b while normalizing the resulting value in the selected row as an
+ * angle.
+ *
+ * @tparam States The number of states.
+ * @param a A vector to add with.
+ * @param b A vector to add with.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+Eigen::Vector<double, States> AngleAdd(const Eigen::Vector<double, States>& a,
+                                       const Eigen::Vector<double, States>& b,
+                                       int angleStateIdx) {
+  Eigen::Vector<double, States> ret = a + b;
+  ret[angleStateIdx] =
+      InputModulus(ret[angleStateIdx], -wpi::numbers::pi, wpi::numbers::pi);
+  return ret;
+}
+
+/**
+ * Returns a function that adds two vectors while normalizing the resulting
+ * value in the selected row as an angle.
+ *
+ * @tparam States The number of states.
+ * @param angleStateIdx The row containing angles to be normalized.
+ */
+template <int States>
+std::function<Eigen::Vector<double, States>(
+    const Eigen::Vector<double, States>&, const Eigen::Vector<double, States>&)>
+AngleAdd(int angleStateIdx) {
+  return [=](auto a, auto b) { return AngleAdd<States>(a, b, angleStateIdx); };
+}
+
+/**
+ * Computes the mean of sigmas with the weights Wm while computing a special
+ * angle mean for a select row.
+ *
+ * @tparam CovDim Dimension of covariance of sigma points after passing through
+ *                the transform.
+ * @tparam States The number of states.
+ * @param sigmas Sigma points.
+ * @param Wm Weights for the mean.
+ * @param angleStatesIdx The row containing the angles.
+ */
+template <int CovDim, int States>
+Eigen::Vector<double, CovDim> AngleMean(
+    const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
+    const Eigen::Vector<double, 2 * States + 1>& Wm, int angleStatesIdx) {
+  double sumSin = sigmas.row(angleStatesIdx)
+                      .unaryExpr([](auto it) { return std::sin(it); })
+                      .sum();
+  double sumCos = sigmas.row(angleStatesIdx)
+                      .unaryExpr([](auto it) { return std::cos(it); })
+                      .sum();
+
+  Eigen::Vector<double, CovDim> ret = sigmas * Wm;
+  ret[angleStatesIdx] = std::atan2(sumSin, sumCos);
+  return ret;
+}
+
+/**
+ * Returns a function that computes the mean of sigmas with the weights Wm while
+ * computing a special angle mean for a select row.
+ *
+ * @tparam CovDim Dimension of covariance of sigma points after passing through
+ *                the transform.
+ * @tparam States The number of states.
+ * @param angleStateIdx The row containing the angles.
+ */
+template <int CovDim, int States>
+std::function<Eigen::Vector<double, CovDim>(
+    const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
+    const Eigen::Vector<double, 2 * States + 1>&)>
+AngleMean(int angleStateIdx) {
+  return [=](auto sigmas, auto Wm) {
+    return AngleMean<CovDim, States>(sigmas, Wm, angleStateIdx);
+  };
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
new file mode 100644
index 0000000..b957c1e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/DifferentialDrivePoseEstimator.h
@@ -0,0 +1,242 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "Eigen/Core"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * vision measurements with differential drive encoder measurements. It will
+ * correct for noisy vision measurements and encoder drift. It is intended to be
+ * an easy drop-in for DifferentialDriveOdometry. In fact, if you never call
+ * AddVisionMeasurement(), and only call Update(), this will behave exactly the
+ * same as DifferentialDriveOdometry.
+ *
+ * Update() should be called every robot loop (if your robot loops are faster or
+ * slower than the default, then you should change the nominal delta time via
+ * the constructor).
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave like regular encoder odometry.
+ *
+ * The state-space system used internally has the following states (x), inputs
+ * (u), and outputs (y):
+ *
+ * <strong> x = [x, y, theta, dist_l, dist_r]ᵀ </strong> in the field coordinate
+ * system containing x position, y position, heading, left encoder distance,
+ * and right encoder distance.
+ *
+ * <strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity,
+ * right wheel velocity, and change in gyro heading.
+ *
+ * NB: Using velocities make things considerably easier, because it means that
+ * teams don't have to worry about getting an accurate model. Basically, we
+ * suspect that it's easier for teams to get good encoder data than it is for
+ * them to perform system identification well enough to get a good model.
+ *
+ * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
+ * position, and heading; or <strong>y = [dist_l, dist_r, theta] </strong>
+ * containing left encoder position, right encoder position, and gyro heading.
+ */
+class WPILIB_DLLEXPORT DifferentialDrivePoseEstimator {
+ public:
+  /**
+   * Constructs a DifferentialDrivePoseEstimator.
+   *
+   * @param gyroAngle                The gyro angle of the robot.
+   * @param initialPose              The estimated initial pose.
+   * @param stateStdDevs             Standard deviations of model states.
+   *                                 Increase these numbers to trust your
+   *                                 model's state estimates less. This matrix
+   *                                 is in the form
+   *                                 [x, y, theta, dist_l, dist_r]ᵀ,
+   *                                 with units in meters and radians.
+   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro
+   *                                 measurements. Increase these numbers to
+   *                                 trust sensor readings from
+   *                                 encoders and gyros less.
+   *                                 This matrix is in the form
+   *                                 [dist_l, dist_r, theta]ᵀ, with units in
+   *                                 meters and radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from
+   *                                 vision less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   * @param nominalDt                The period of the loop calling Update().
+   */
+  DifferentialDrivePoseEstimator(
+      const Rotation2d& gyroAngle, const Pose2d& initialPose,
+      const wpi::array<double, 5>& stateStdDevs,
+      const wpi::array<double, 3>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
+      units::second_t nominalDt = 0.02_s);
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * 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 estimated pose of the robot on the field.
+   * @param gyroAngle The current gyro angle.
+   */
+  void ResetPosition(const Pose2d& pose, const Rotation2d& gyroAngle);
+
+  /**
+   * Returns the pose of the robot at the current time as estimated by the
+   * Unscented Kalman Filter.
+   *
+   * @return The estimated robot pose.
+   */
+  Pose2d GetEstimatedPosition() const;
+
+  /**
+   * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *                        camera.
+   * @param timestamp       The timestamp of the vision measurement in seconds.
+   *                        Note that if you don't use your own time source by
+   *                        calling UpdateWithTime(), then you must use a
+   *                        timestamp with an epoch since FPGA startup (i.e. the
+   *                        epoch of this timestamp is the same epoch as
+   *                        frc::Timer::GetFPGATimestamp(). This means that
+   *                        you should use frc::Timer::GetFPGATimestamp() as
+   *                        your time source in this case.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp);
+
+  /**
+   * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose          The pose of the robot as measured by the
+   *                                 vision camera.
+   * @param timestamp                The timestamp of the vision measurement in
+   *                                 seconds. Note that if you don't use your
+   *                                 own time source by calling
+   *                                 UpdateWithTime(), then you must use a
+   *                                 timestamp with an epoch since FPGA startup
+   *                                 (i.e. the epoch of this timestamp is the
+   *                                 same epoch as
+   *                                 frc::Timer::GetFPGATimestamp(). This means
+   *                                 that you should use
+   *                                 frc::Timer::GetFPGATimestamp() as your
+   *                                 time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   */
+  void AddVisionMeasurement(
+      const Pose2d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the Unscented Kalman Filter using only wheel encoder information.
+   * Note that this should be called every loop iteration.
+   *
+   * @param gyroAngle     The current gyro angle.
+   * @param wheelSpeeds   The velocities of the wheels in meters per second.
+   * @param leftDistance  The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   *
+   * @return The estimated pose of the robot.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle,
+                const DifferentialDriveWheelSpeeds& wheelSpeeds,
+                units::meter_t leftDistance, units::meter_t rightDistance);
+
+  /**
+   * Updates the Unscented Kalman Filter using only wheel encoder information.
+   * Note that this should be called every loop iteration.
+   *
+   * @param currentTime   The time at which this method was called.
+   * @param gyroAngle     The current gyro angle.
+   * @param wheelSpeeds   The velocities of the wheels in meters per second.
+   * @param leftDistance  The distance traveled by the left encoder.
+   * @param rightDistance The distance traveled by the right encoder.
+   *
+   * @return The estimated pose of the robot.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        const DifferentialDriveWheelSpeeds& wheelSpeeds,
+                        units::meter_t leftDistance,
+                        units::meter_t rightDistance);
+
+ private:
+  UnscentedKalmanFilter<5, 3, 3> m_observer;
+  KalmanFilterLatencyCompensator<5, 3, 3, UnscentedKalmanFilter<5, 3, 3>>
+      m_latencyCompensator;
+  std::function<void(const Eigen::Vector<double, 3>& u,
+                     const Eigen::Vector<double, 3>& y)>
+      m_visionCorrect;
+
+  Eigen::Matrix<double, 3, 3> m_visionContR;
+
+  units::second_t m_nominalDt;
+  units::second_t m_prevTime = -1_s;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  template <int Dim>
+  static wpi::array<double, Dim> StdDevMatrixToArray(
+      const Eigen::Vector<double, Dim>& stdDevs);
+
+  static Eigen::Vector<double, 5> F(const Eigen::Vector<double, 5>& x,
+                                    const Eigen::Vector<double, 3>& u);
+  static Eigen::Vector<double, 5> FillStateVector(const Pose2d& pose,
+                                                  units::meter_t leftDistance,
+                                                  units::meter_t rightDistance);
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
index 6f0fc85..3e5edb8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/ExtendedKalmanFilter.h
@@ -1,26 +1,48 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
 #include <functional>
 
+#include <wpi/array.h>
+
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
-#include "Eigen/src/Cholesky/LDLT.h"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
 #include "units/time.h"
 
 namespace frc {
 
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * An extended Kalman filter supports nonlinear state and measurement models. It
+ * propagates the error covariance by linearizing the models around the state
+ * estimate, then applying the linear Kalman filter equations.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
+ * @tparam Outputs The number of outputs.
+ */
 template <int States, int Inputs, int Outputs>
 class ExtendedKalmanFilter {
  public:
@@ -35,30 +57,36 @@
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  ExtendedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, Inputs, 1>&)>
+  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
                            f,
-                       std::function<Eigen::Matrix<double, Outputs, 1>(
-                           const Eigen::Matrix<double, States, 1>&,
-                           const Eigen::Matrix<double, Inputs, 1>&)>
+                       std::function<Eigen::Vector<double, Outputs>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
                            h,
-                       const std::array<double, States>& stateStdDevs,
-                       const std::array<double, Outputs>& measurementStdDevs,
+                       const wpi::array<double, States>& stateStdDevs,
+                       const wpi::array<double, Outputs>& measurementStdDevs,
                        units::second_t dt)
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);
     m_contR = MakeCovMatrix(measurementStdDevs);
+    m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
+      return a - b;
+    };
+    m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
+      return a + b;
+    };
     m_dt = dt;
 
     Reset();
 
     Eigen::Matrix<double, States, States> contA =
         NumericalJacobianX<States, States, Inputs>(
-            m_f, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+            m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
     Eigen::Matrix<double, Outputs, States> C =
         NumericalJacobianX<Outputs, States, Inputs>(
-            m_h, m_xHat, Eigen::Matrix<double, Inputs, 1>::Zero());
+            m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
 
     Eigen::Matrix<double, States, States> discA;
     Eigen::Matrix<double, States, States> discQ;
@@ -67,10 +95,70 @@
     Eigen::Matrix<double, Outputs, Outputs> discR =
         DiscretizeR<Outputs>(m_contR, dt);
 
-    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
-    bool isObservable =
-        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
-    if (isObservable && Outputs <= States) {
+    if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
+      m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
+          discA.transpose(), C.transpose(), discQ, discR);
+    } else {
+      m_initP = Eigen::Matrix<double, States, States>::Zero();
+    }
+    m_P = m_initP;
+  }
+
+  /**
+   * Constructs an extended Kalman filter.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param residualFuncY      A function that computes the residual of two
+   *                           measurement vectors (i.e. it subtracts them.)
+   * @param addFuncX           A function that adds two state vectors.
+   * @param dt                 Nominal discretization timestep.
+   */
+  ExtendedKalmanFilter(std::function<Eigen::Vector<double, States>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
+                           f,
+                       std::function<Eigen::Vector<double, Outputs>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, Inputs>&)>
+                           h,
+                       const wpi::array<double, States>& stateStdDevs,
+                       const wpi::array<double, Outputs>& measurementStdDevs,
+                       std::function<Eigen::Vector<double, Outputs>(
+                           const Eigen::Vector<double, Outputs>&,
+                           const Eigen::Vector<double, Outputs>&)>
+                           residualFuncY,
+                       std::function<Eigen::Vector<double, States>(
+                           const Eigen::Vector<double, States>&,
+                           const Eigen::Vector<double, States>&)>
+                           addFuncX,
+                       units::second_t dt)
+      : m_f(f), m_h(h), m_residualFuncY(residualFuncY), m_addFuncX(addFuncX) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
+    m_dt = dt;
+
+    Reset();
+
+    Eigen::Matrix<double, States, States> contA =
+        NumericalJacobianX<States, States, Inputs>(
+            m_f, m_xHat, Eigen::Vector<double, Inputs>::Zero());
+    Eigen::Matrix<double, Outputs, States> C =
+        NumericalJacobianX<Outputs, States, Inputs>(
+            m_h, m_xHat, Eigen::Vector<double, Inputs>::Zero());
+
+    Eigen::Matrix<double, States, States> discA;
+    Eigen::Matrix<double, States, States> discQ;
+    DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
+
+    Eigen::Matrix<double, Outputs, Outputs> discR =
+        DiscretizeR<Outputs>(m_contR, dt);
+
+    if (IsDetectable<States, Outputs>(discA, C) && Outputs <= States) {
       m_initP = drake::math::DiscreteAlgebraicRiccatiEquation(
           discA.transpose(), C.transpose(), discQ, discR);
     } else {
@@ -102,7 +190,7 @@
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -116,7 +204,7 @@
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -140,9 +228,7 @@
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
-    m_dt = dt;
-
+  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     // Find continuous A
     Eigen::Matrix<double, States, States> contA =
         NumericalJacobianX<States, States, Inputs>(m_f, m_xHat, u);
@@ -152,8 +238,12 @@
     Eigen::Matrix<double, States, States> discQ;
     DiscretizeAQTaylor<States>(contA, m_contQ, dt, &discA, &discQ);
 
-    m_xHat = RungeKutta(m_f, m_xHat, u, dt);
+    m_xHat = RK4(m_f, m_xHat, u, dt);
+
+    // Pₖ₊₁⁻ = APₖ⁻Aᵀ + Q
     m_P = discA * m_P * discA.transpose() + discQ;
+
+    m_dt = dt;
   }
 
   /**
@@ -162,9 +252,26 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Outputs, 1>& y) {
-    Correct<Outputs>(u, y, m_h, m_contR);
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Outputs>& y) {
+    Correct<Outputs>(u, y, m_h, m_contR, m_residualFuncY, m_addFuncX);
+  }
+
+  template <int Rows>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
+                   h,
+               const Eigen::Matrix<double, Rows, Rows>& R) {
+    auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
+      return a - b;
+    };
+    auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
+      return a + b;
+    };
+    Correct<Rows>(u, y, h, R, residualFuncY, addFuncX);
   }
 
   /**
@@ -174,55 +281,77 @@
    * Correct() call vary. The h(x, u) passed to the constructor is used if one
    * is not provided (the two-argument version of this function).
    *
-   * @param u Same control input used in the predict step.
-   * @param y Measurement vector.
-   * @param h A vector-valued function of x and u that returns
-   *          the measurement vector.
-   * @param R Discrete measurement noise covariance matrix.
+   * @param u             Same control input used in the predict step.
+   * @param y             Measurement vector.
+   * @param h             A vector-valued function of x and u that returns
+   *                      the measurement vector.
+   * @param R             Discrete measurement noise covariance matrix.
+   * @param residualFuncY A function that computes the residual of two
+   *                      measurement vectors (i.e. it subtracts them.)
+   * @param addFuncX      A function that adds two state vectors.
    */
   template <int Rows>
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Rows, 1>& y,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, Inputs, 1>&)>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
                    h,
-               const Eigen::Matrix<double, Rows, Rows>& R) {
+               const Eigen::Matrix<double, Rows, Rows>& R,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, Rows>&,
+                   const Eigen::Vector<double, Rows>&)>
+                   residualFuncY,
+               std::function<Eigen::Vector<double, States>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, States>)>
+                   addFuncX) {
     const Eigen::Matrix<double, Rows, States> C =
         NumericalJacobianX<Rows, States, Inputs>(h, m_xHat, u);
     const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
 
     Eigen::Matrix<double, Rows, Rows> S = C * m_P * C.transpose() + discR;
 
-    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
     // efficiently.
     //
-    // K = PC^T S^-1
-    // KS = PC^T
-    // (KS)^T = (PC^T)^T
-    // S^T K^T = CP^T
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
     //
     // The solution of Ax = b can be found via x = A.solve(b).
     //
-    // K^T = S^T.solve(CP^T)
-    // K = (S^T.solve(CP^T))^T
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
     Eigen::Matrix<double, States, Rows> K =
         S.transpose().ldlt().solve(C * m_P.transpose()).transpose();
 
-    m_xHat += K * (y - h(m_xHat, u));
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − h(x̂ₖ₊₁⁻, uₖ₊₁))
+    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, h(m_xHat, u)));
+
+    // Pₖ₊₁⁺ = (I − KC)Pₖ₊₁⁻
     m_P = (Eigen::Matrix<double, States, States>::Identity() - K * C) * m_P;
   }
 
  private:
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_f;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_h;
-  Eigen::Matrix<double, States, 1> m_xHat;
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, Outputs>&,
+      const Eigen::Vector<double, Outputs>)>
+      m_residualFuncY;
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, States>)>
+      m_addFuncX;
+  Eigen::Vector<double, States> m_xHat;
   Eigen::Matrix<double, States, States> m_P;
   Eigen::Matrix<double, States, States> m_contQ;
   Eigen::Matrix<double, Outputs, Outputs> m_contR;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
index c395080..3aa4dbd 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilter.h
@@ -1,17 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
-#include <cmath>
+#include <frc/fmt/Eigen.h>
 
+#include <cmath>
+#include <string>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
-#include "Eigen/src/Cholesky/LDLT.h"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/system/Discretization.h"
@@ -37,6 +39,10 @@
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
  * "Stochastic control theory".
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
+ * @tparam Outputs The number of outputs.
  */
 template <int States, int Inputs, int Outputs>
 class KalmanFilterImpl {
@@ -50,8 +56,8 @@
    * @param dt                 Nominal discretization timestep.
    */
   KalmanFilterImpl(LinearSystem<States, Inputs, Outputs>& plant,
-                   const std::array<double, States>& stateStdDevs,
-                   const std::array<double, Outputs>& measurementStdDevs,
+                   const wpi::array<double, States>& stateStdDevs,
+                   const wpi::array<double, Outputs>& measurementStdDevs,
                    units::second_t dt) {
     m_plant = &plant;
 
@@ -66,34 +72,35 @@
 
     const auto& C = plant.C();
 
-    // IsStabilizable(A^T, C^T) will tell us if the system is observable.
-    bool isObservable =
-        IsStabilizable<States, Outputs>(discA.transpose(), C.transpose());
-    if (!isObservable) {
-      wpi::math::MathSharedStore::ReportError(
-          "The system passed to the Kalman filter is not observable!");
-      throw std::invalid_argument(
-          "The system passed to the Kalman filter is not observable!");
+    if (!IsDetectable<States, Outputs>(discA, C)) {
+      std::string msg = fmt::format(
+          "The system passed to the Kalman filter is "
+          "unobservable!\n\nA =\n{}\nC =\n{}\n",
+          discA, C);
+
+      wpi::math::MathSharedStore::ReportError(msg);
+      throw std::invalid_argument(msg);
     }
 
     Eigen::Matrix<double, States, States> P =
         drake::math::DiscreteAlgebraicRiccatiEquation(
             discA.transpose(), C.transpose(), discQ, discR);
 
+    // S = CPCᵀ + R
     Eigen::Matrix<double, Outputs, Outputs> S = C * P * C.transpose() + discR;
 
-    // We want to put K = PC^T S^-1 into Ax = b form so we can solve it more
+    // We want to put K = PCᵀS⁻¹ into Ax = b form so we can solve it more
     // efficiently.
     //
-    // K = PC^T S^-1
-    // KS = PC^T
-    // (KS)^T = (PC^T)^T
-    // S^T K^T = CP^T
+    // K = PCᵀS⁻¹
+    // KS = PCᵀ
+    // (KS)ᵀ = (PCᵀ)ᵀ
+    // SᵀKᵀ = CPᵀ
     //
     // The solution of Ax = b can be found via x = A.solve(b).
     //
-    // K^T = S^T.solve(CP^T)
-    // K = (S^T.solve(CP^T))^T
+    // Kᵀ = Sᵀ.solve(CPᵀ)
+    // K = (Sᵀ.solve(CPᵀ))ᵀ
     m_K = S.transpose().ldlt().solve(C * P.transpose()).transpose();
 
     Reset();
@@ -118,7 +125,7 @@
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -132,7 +139,7 @@
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -153,7 +160,7 @@
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     m_xHat = m_plant->CalculateX(m_xHat, u, dt);
   }
 
@@ -163,8 +170,9 @@
    * @param u Same control input used in the last predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Outputs, 1>& y) {
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Outputs>& y) {
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − (Cx̂ₖ₊₁⁻ + Duₖ₊₁))
     m_xHat += m_K * (y - (m_plant->C() * m_xHat + m_plant->D() * u));
   }
 
@@ -179,7 +187,7 @@
   /**
    * The state estimate.
    */
-  Eigen::Matrix<double, States, 1> m_xHat;
+  Eigen::Vector<double, States> m_xHat;
 };
 
 }  // namespace detail
@@ -196,8 +204,8 @@
    * @param dt                 Nominal discretization timestep.
    */
   KalmanFilter(LinearSystem<States, Inputs, Outputs>& plant,
-               const std::array<double, States>& stateStdDevs,
-               const std::array<double, Outputs>& measurementStdDevs,
+               const wpi::array<double, States>& stateStdDevs,
+               const wpi::array<double, Outputs>& measurementStdDevs,
                units::second_t dt)
       : detail::KalmanFilterImpl<States, Inputs, Outputs>{
             plant, stateStdDevs, measurementStdDevs, dt} {}
@@ -209,11 +217,12 @@
 // Template specializations are used here to make common state-input-output
 // triplets compile faster.
 template <>
-class KalmanFilter<1, 1, 1> : public detail::KalmanFilterImpl<1, 1, 1> {
+class WPILIB_DLLEXPORT KalmanFilter<1, 1, 1>
+    : public detail::KalmanFilterImpl<1, 1, 1> {
  public:
   KalmanFilter(LinearSystem<1, 1, 1>& plant,
-               const std::array<double, 1>& stateStdDevs,
-               const std::array<double, 1>& measurementStdDevs,
+               const wpi::array<double, 1>& stateStdDevs,
+               const wpi::array<double, 1>& measurementStdDevs,
                units::second_t dt);
 
   KalmanFilter(KalmanFilter&&) = default;
@@ -223,11 +232,12 @@
 // Template specializations are used here to make common state-input-output
 // triplets compile faster.
 template <>
-class KalmanFilter<2, 1, 1> : public detail::KalmanFilterImpl<2, 1, 1> {
+class WPILIB_DLLEXPORT KalmanFilter<2, 1, 1>
+    : public detail::KalmanFilterImpl<2, 1, 1> {
  public:
   KalmanFilter(LinearSystem<2, 1, 1>& plant,
-               const std::array<double, 2>& stateStdDevs,
-               const std::array<double, 1>& measurementStdDevs,
+               const wpi::array<double, 2>& stateStdDevs,
+               const wpi::array<double, 1>& measurementStdDevs,
                units::second_t dt);
 
   KalmanFilter(KalmanFilter&&) = default;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
new file mode 100644
index 0000000..aabb8ec
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/KalmanFilterLatencyCompensator.h
@@ -0,0 +1,150 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <array>
+#include <functional>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Core"
+#include "units/math.h"
+#include "units/time.h"
+
+namespace frc {
+
+template <int States, int Inputs, int Outputs, typename KalmanFilterType>
+class KalmanFilterLatencyCompensator {
+ public:
+  struct ObserverSnapshot {
+    Eigen::Vector<double, States> xHat;
+    Eigen::Matrix<double, States, States> errorCovariances;
+    Eigen::Vector<double, Inputs> inputs;
+    Eigen::Vector<double, Outputs> localMeasurements;
+
+    ObserverSnapshot(const KalmanFilterType& observer,
+                     const Eigen::Vector<double, Inputs>& u,
+                     const Eigen::Vector<double, Outputs>& localY)
+        : xHat(observer.Xhat()),
+          errorCovariances(observer.P()),
+          inputs(u),
+          localMeasurements(localY) {}
+  };
+
+  /**
+   * Clears the observer snapshot buffer.
+   */
+  void Reset() { m_pastObserverSnapshots.clear(); }
+
+  /**
+   * Add past observer states to the observer snapshots list.
+   *
+   * @param observer  The observer.
+   * @param u         The input at the timestamp.
+   * @param localY    The local output at the timestamp
+   * @param timestamp The timesnap of the state.
+   */
+  void AddObserverState(const KalmanFilterType& observer,
+                        Eigen::Vector<double, Inputs> u,
+                        Eigen::Vector<double, Outputs> localY,
+                        units::second_t timestamp) {
+    // Add the new state into the vector.
+    m_pastObserverSnapshots.emplace_back(timestamp,
+                                         ObserverSnapshot{observer, u, localY});
+
+    // Remove the oldest snapshot if the vector exceeds our maximum size.
+    if (m_pastObserverSnapshots.size() > kMaxPastObserverStates) {
+      m_pastObserverSnapshots.erase(m_pastObserverSnapshots.begin());
+    }
+  }
+
+  /**
+   * Add past global measurements (such as from vision)to the estimator.
+   *
+   * @param observer                 The observer to apply the past global
+   *                                 measurement.
+   * @param nominalDt                The nominal timestep.
+   * @param y                        The measurement.
+   * @param globalMeasurementCorrect The function take calls correct() on the
+   *                                 observer.
+   * @param timestamp                The timestamp of the measurement.
+   */
+  template <int Rows>
+  void ApplyPastGlobalMeasurement(
+      KalmanFilterType* observer, units::second_t nominalDt,
+      Eigen::Vector<double, Rows> y,
+      std::function<void(const Eigen::Vector<double, Inputs>& u,
+                         const Eigen::Vector<double, Rows>& y)>
+          globalMeasurementCorrect,
+      units::second_t timestamp) {
+    if (m_pastObserverSnapshots.size() == 0) {
+      // State map was empty, which means that we got a measurement right at
+      // startup. The only thing we can do is ignore the measurement.
+      return;
+    }
+
+    // We will perform a binary search to find the index of the element in the
+    // vector that has a timestamp that is equal to or greater than the vision
+    // measurement timestamp.
+    auto lowerBoundIter = std::lower_bound(
+        m_pastObserverSnapshots.cbegin(), m_pastObserverSnapshots.cend(),
+        timestamp,
+        [](const auto& entry, const auto& ts) { return entry.first < ts; });
+    int index = std::distance(m_pastObserverSnapshots.cbegin(), lowerBoundIter);
+
+    // High and Low should be the same. The sampled timestamp is greater than or
+    // equal to the vision pose timestamp. We will now find the entry which is
+    // closest in time to the requested timestamp.
+
+    size_t indexOfClosestEntry =
+        units::math::abs(
+            timestamp - m_pastObserverSnapshots[std::max(index - 1, 0)].first) <
+                units::math::abs(timestamp -
+                                 m_pastObserverSnapshots[index].first)
+            ? index - 1
+            : index;
+
+    units::second_t lastTimestamp =
+        m_pastObserverSnapshots[indexOfClosestEntry].first - nominalDt;
+
+    // We will now go back in time to the state of the system at the time when
+    // the measurement was captured. We will reset the observer to that state,
+    // and apply correction based on the measurement. Then, we will go back
+    // through all observer states until the present and apply past inputs to
+    // get the present estimated state.
+    for (size_t i = indexOfClosestEntry; i < m_pastObserverSnapshots.size();
+         ++i) {
+      auto& [key, snapshot] = m_pastObserverSnapshots[i];
+
+      if (i == indexOfClosestEntry) {
+        observer->SetP(snapshot.errorCovariances);
+        observer->SetXhat(snapshot.xHat);
+      }
+
+      observer->Predict(snapshot.inputs, key - lastTimestamp);
+      observer->Correct(snapshot.inputs, snapshot.localMeasurements);
+
+      if (i == indexOfClosestEntry) {
+        // Note that the measurement is at a timestep close but probably not
+        // exactly equal to the timestep for which we called predict. This makes
+        // the assumption that the dt is small enough that the difference
+        // between the measurement time and the time that the inputs were
+        // captured at is very small.
+        globalMeasurementCorrect(snapshot.inputs, y);
+      }
+
+      lastTimestamp = key;
+      snapshot = ObserverSnapshot{*observer, snapshot.inputs,
+                                  snapshot.localMeasurements};
+    }
+  }
+
+ private:
+  static constexpr size_t kMaxPastObserverStates = 300;
+  std::vector<std::pair<units::second_t, ObserverSnapshot>>
+      m_pastObserverSnapshots;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
new file mode 100644
index 0000000..93c9f1e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MecanumDrivePoseEstimator.h
@@ -0,0 +1,233 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
+#include "Eigen/Core"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * vision measurements with mecanum drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an
+ * easy but more accurate drop-in for MecanumDriveOdometry.
+ *
+ * Update() should be called every robot loop. If your loops are faster or
+ * slower than the default of 0.02s, then you should change the nominal delta
+ * time by specifying it in the constructor.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave mostly like regular encoder
+ * odometry.
+ *
+ * The state-space system used internally has the following states (x), inputs
+ * (u), and outputs (y):
+ *
+ * <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
+ * containing x position, y position, and heading.
+ *
+ * <strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity,
+ * right wheel velocity, and change in gyro heading.
+ *
+ * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
+ * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
+ * heading.
+ */
+class WPILIB_DLLEXPORT MecanumDrivePoseEstimator {
+ public:
+  /**
+   * Constructs a MecanumDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPose              The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object
+   *                                 for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states.
+   *                                 Increase these numbers to trust your
+   *                                 model's state estimates less. This matrix
+   *                                 is in the form [x, y, theta]ᵀ, with units
+   *                                 in meters and radians.
+   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro
+   *                                 measurements. Increase these numbers to
+   *                                 trust sensor readings from encoders
+   *                                 and gyros less. This matrix is in the form
+   *                                 [theta], with units in radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   * @param nominalDt                The time in seconds between each robot
+   *                                 loop.
+   */
+  MecanumDrivePoseEstimator(
+      const Rotation2d& gyroAngle, const Pose2d& initialPose,
+      MecanumDriveKinematics kinematics,
+      const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 1>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
+      units::second_t nominalDt = 0.02_s);
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 3>& visionMeasurementStdDevs);
+
+  /**
+   * Resets the robot's position on the field.
+   *
+   * <p>You NEED to reset your encoders (to zero) when calling this method.
+   *
+   * <p>The gyroscope angle does not need to be reset in 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);
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Extended
+   * Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const;
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *                        camera.
+   * @param timestamp       The timestamp of the vision measurement in seconds.
+   *                        Note that if you don't use your own time source by
+   *                        calling UpdateWithTime() then you must use a
+   *                        timestamp with an epoch since FPGA startup
+   *                        (i.e. the epoch of this timestamp is the same
+   *                        epoch as Timer#GetFPGATimestamp.) This means
+   *                        that you should use Timer#GetFPGATimestamp as your
+   *                        time source or sync the epochs.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp);
+
+  /**
+   * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose          The pose of the robot as measured by the
+   *                                 vision camera.
+   * @param timestamp                The timestamp of the vision measurement in
+   *                                 seconds. Note that if you don't use your
+   *                                 own time source by calling
+   *                                 UpdateWithTime(), then you must use a
+   *                                 timestamp with an epoch since FPGA startup
+   *                                 (i.e. the epoch of this timestamp is the
+   *                                 same epoch as
+   *                                 frc::Timer::GetFPGATimestamp(). This means
+   *                                 that you should use
+   *                                 frc::Timer::GetFPGATimestamp() as your
+   *                                 time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   */
+  void AddVisionMeasurement(
+      const Pose2d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param gyroAngle   The current gyro angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d Update(const Rotation2d& gyroAngle,
+                const MecanumDriveWheelSpeeds& wheelSpeeds);
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param currentTime Time at which this method was called, in seconds.
+   * @param gyroAngle   The current gyroscope angle.
+   * @param wheelSpeeds The current speeds of the mecanum drive wheels.
+   * @return The estimated pose of the robot in meters.
+   */
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        const MecanumDriveWheelSpeeds& wheelSpeeds);
+
+ private:
+  UnscentedKalmanFilter<3, 3, 1> m_observer;
+  MecanumDriveKinematics m_kinematics;
+  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
+      m_latencyCompensator;
+  std::function<void(const Eigen::Vector<double, 3>& u,
+                     const Eigen::Vector<double, 3>& y)>
+      m_visionCorrect;
+
+  Eigen::Matrix3d m_visionContR;
+
+  units::second_t m_nominalDt;
+  units::second_t m_prevTime = -1_s;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  template <int Dim>
+  static wpi::array<double, Dim> StdDevMatrixToArray(
+      const Eigen::Vector<double, Dim>& vector) {
+    wpi::array<double, Dim> array;
+    for (size_t i = 0; i < Dim; ++i) {
+      array[i] = vector(i);
+    }
+    return array;
+  }
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
index 72abb80..42f5593 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/MerweScaledSigmaPoints.h
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <cmath>
 
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
-#include "Eigen/src/Cholesky/LLT.h"
 
 namespace frc {
 
@@ -22,11 +19,11 @@
  * version seen in most publications. Unless you know better, this should be
  * your default choice.
  *
- * @tparam States The dimensionality of the state. 2*States+1 weights will be
- *                generated.
- *
  * [1] R. Van der Merwe "Sigma-Point Kalman Filters for Probabilitic
  *     Inference in Dynamic State-Space Models" (Doctoral dissertation)
+ *
+ * @tparam States The dimensionality of the state. 2*States+1 weights will be
+ *                generated.
  */
 template <int States>
 class MerweScaledSigmaPoints {
@@ -40,8 +37,8 @@
    *             For Gaussian distributions, beta = 2 is optimal.
    * @param kappa Secondary scaling parameter usually set to 0 or 3 - States.
    */
-  MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2,
-                         int kappa = 3 - States) {
+  explicit MerweScaledSigmaPoints(double alpha = 1e-3, double beta = 2,
+                                  int kappa = 3 - States) {
     m_alpha = alpha;
     m_kappa = kappa;
 
@@ -66,7 +63,7 @@
    *
    */
   Eigen::Matrix<double, States, 2 * States + 1> SigmaPoints(
-      const Eigen::Matrix<double, States, 1>& x,
+      const Eigen::Vector<double, States>& x,
       const Eigen::Matrix<double, States, States>& P) {
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
     Eigen::Matrix<double, States, States> U =
@@ -87,7 +84,7 @@
   /**
    * Returns the weight for each sigma point for the mean.
    */
-  const Eigen::Matrix<double, 2 * States + 1, 1>& Wm() const { return m_Wm; }
+  const Eigen::Vector<double, 2 * States + 1>& Wm() const { return m_Wm; }
 
   /**
    * Returns an element of the weight for each sigma point for the mean.
@@ -99,7 +96,7 @@
   /**
    * Returns the weight for each sigma point for the covariance.
    */
-  const Eigen::Matrix<double, 2 * States + 1, 1>& Wc() const { return m_Wc; }
+  const Eigen::Vector<double, 2 * States + 1>& Wc() const { return m_Wc; }
 
   /**
    * Returns an element of the weight for each sigma point for the covariance.
@@ -109,8 +106,8 @@
   double Wc(int i) const { return m_Wc(i, 0); }
 
  private:
-  Eigen::Matrix<double, 2 * States + 1, 1> m_Wm;
-  Eigen::Matrix<double, 2 * States + 1, 1> m_Wc;
+  Eigen::Vector<double, 2 * States + 1> m_Wm;
+  Eigen::Vector<double, 2 * States + 1> m_Wc;
   double m_alpha;
   int m_kappa;
 
@@ -123,8 +120,8 @@
     double lambda = std::pow(m_alpha, 2) * (States + m_kappa) - States;
 
     double c = 0.5 / (States + lambda);
-    m_Wm = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
-    m_Wc = Eigen::Matrix<double, 2 * States + 1, 1>::Constant(c);
+    m_Wm = Eigen::Vector<double, 2 * States + 1>::Constant(c);
+    m_Wc = Eigen::Vector<double, 2 * States + 1>::Constant(c);
 
     m_Wm(0) = lambda / (States + lambda);
     m_Wc(0) = lambda / (States + lambda) + (1 - std::pow(m_alpha, 2) + beta);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
new file mode 100644
index 0000000..91e0e50
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/SwerveDrivePoseEstimator.h
@@ -0,0 +1,317 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <limits>
+
+#include <wpi/array.h>
+#include <wpi/timestamp.h>
+
+#include "Eigen/Core"
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
+#include "frc/estimator/KalmanFilterLatencyCompensator.h"
+#include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "units/time.h"
+
+namespace frc {
+/**
+ * This class wraps an Unscented Kalman Filter to fuse latency-compensated
+ * vision measurements with swerve drive encoder velocity measurements. It will
+ * correct for noisy measurements and encoder drift. It is intended to be an
+ * easy but more accurate drop-in for SwerveDriveOdometry.
+ *
+ * Update() should be called every robot loop. If your loops are faster or
+ * slower than the default of 0.02s, then you should change the nominal delta
+ * time by specifying it in the constructor.
+ *
+ * AddVisionMeasurement() can be called as infrequently as you want; if you
+ * never call it, then this class will behave mostly like regular encoder
+ * odometry.
+ *
+ * The state-space system used internally has the following states (x), inputs
+ * (u), and outputs (y):
+ *
+ * <strong> x = [x, y, theta]ᵀ </strong> in the field coordinate system
+ * containing x position, y position, and heading.
+ *
+ * <strong> u = [v_l, v_r, dtheta]ᵀ </strong> containing left wheel velocity,
+ * right wheel velocity, and change in gyro heading.
+ *
+ * <strong> y = [x, y, theta]ᵀ </strong> from vision containing x position, y
+ * position, and heading; or <strong> y = [theta]ᵀ </strong> containing gyro
+ * heading.
+ */
+template <size_t NumModules>
+class SwerveDrivePoseEstimator {
+ public:
+  /**
+   * Constructs a SwerveDrivePoseEstimator.
+   *
+   * @param gyroAngle                The current gyro angle.
+   * @param initialPose              The starting pose estimate.
+   * @param kinematics               A correctly-configured kinematics object
+   *                                 for your drivetrain.
+   * @param stateStdDevs             Standard deviations of model states.
+   *                                 Increase these numbers to trust your
+   *                                 model's state estimates less. This matrix
+   *                                 is in the form [x, y, theta]ᵀ, with units
+   *                                 in meters and radians.
+   * @param localMeasurementStdDevs  Standard deviations of the encoder and gyro
+   *                                 measurements. Increase these numbers to
+   *                                 trust sensor readings from encoders
+   *                                 and gyros less. This matrix is in the form
+   *                                 [theta], with units in radians.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   * @param nominalDt                The time in seconds between each robot
+   *                                 loop.
+   */
+  SwerveDrivePoseEstimator(
+      const Rotation2d& gyroAngle, const Pose2d& initialPose,
+      SwerveDriveKinematics<NumModules>& kinematics,
+      const wpi::array<double, 3>& stateStdDevs,
+      const wpi::array<double, 1>& localMeasurementStdDevs,
+      const wpi::array<double, 3>& visionMeasurementStdDevs,
+      units::second_t nominalDt = 0.02_s)
+      : m_observer([](const Eigen::Vector<double, 3>& x,
+                      const Eigen::Vector<double, 3>& u) { return u; },
+                   [](const Eigen::Vector<double, 3>& x,
+                      const Eigen::Vector<double, 3>& u) {
+                     return x.block<1, 1>(2, 0);
+                   },
+                   stateStdDevs, localMeasurementStdDevs,
+                   frc::AngleMean<3, 3>(2), frc::AngleMean<1, 3>(0),
+                   frc::AngleResidual<3>(2), frc::AngleResidual<1>(0),
+                   frc::AngleAdd<3>(2), nominalDt),
+        m_kinematics(kinematics),
+        m_nominalDt(nominalDt) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+
+    // Create correction mechanism for vision measurements.
+    m_visionCorrect = [&](const Eigen::Vector<double, 3>& u,
+                          const Eigen::Vector<double, 3>& y) {
+      m_observer.Correct<3>(
+          u, y,
+          [](const Eigen::Vector<double, 3>& x,
+             const Eigen::Vector<double, 3>& u) { return x; },
+          m_visionContR, frc::AngleMean<3, 3>(2), frc::AngleResidual<3>(2),
+          frc::AngleResidual<3>(2), frc::AngleAdd<3>(2));
+    };
+
+    // Set initial state.
+    m_observer.SetXhat(PoseTo3dVector(initialPose));
+
+    // Calculate offsets.
+    m_gyroOffset = initialPose.Rotation() - gyroAngle;
+    m_previousAngle = initialPose.Rotation();
+  }
+
+  /**
+   * 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 in 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) {
+    // Reset state estimate and error covariance
+    m_observer.Reset();
+    m_latencyCompensator.Reset();
+
+    m_observer.SetXhat(PoseTo3dVector(pose));
+
+    m_gyroOffset = pose.Rotation() - gyroAngle;
+    m_previousAngle = pose.Rotation();
+  }
+
+  /**
+   * Gets the pose of the robot at the current time as estimated by the Extended
+   * Kalman Filter.
+   *
+   * @return The estimated robot pose in meters.
+   */
+  Pose2d GetEstimatedPosition() const {
+    return Pose2d(m_observer.Xhat(0) * 1_m, m_observer.Xhat(1) * 1_m,
+                  Rotation2d(units::radian_t{m_observer.Xhat(2)}));
+  }
+
+  /**
+   * Sets the pose estimator's trust of global measurements. This might be used
+   * to change trust in vision measurements after the autonomous period, or to
+   * change trust as distance to a vision target increases.
+   *
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   */
+  void SetVisionMeasurementStdDevs(
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    // Create R (covariances) for vision measurements.
+    m_visionContR = frc::MakeCovMatrix(visionMeasurementStdDevs);
+  }
+
+  /**
+   * Add a vision measurement to the Unscented Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * @param visionRobotPose The pose of the robot as measured by the vision
+   *                        camera.
+   * @param timestamp       The timestamp of the vision measurement in seconds.
+   *                        Note that if you don't use your own time source by
+   *                        calling UpdateWithTime() then you must use a
+   *                        timestamp with an epoch since FPGA startup
+   *                        (i.e. the epoch of this timestamp is the same
+   *                        epoch as Timer#GetFPGATimestamp.) This means
+   *                        that you should use Timer#GetFPGATimestamp as your
+   *                        time source or sync the epochs.
+   */
+  void AddVisionMeasurement(const Pose2d& visionRobotPose,
+                            units::second_t timestamp) {
+    m_latencyCompensator.ApplyPastGlobalMeasurement<3>(
+        &m_observer, m_nominalDt, PoseTo3dVector(visionRobotPose),
+        m_visionCorrect, timestamp);
+  }
+
+  /**
+   * Adds a vision measurement to the Unscented Kalman Filter. This will correct
+   * the odometry pose estimate while still accounting for measurement noise.
+   *
+   * This method can be called as infrequently as you want, as long as you are
+   * calling Update() every loop.
+   *
+   * Note that the vision measurement standard deviations passed into this
+   * method will continue to apply to future measurements until a subsequent
+   * call to SetVisionMeasurementStdDevs() or this method.
+   *
+   * @param visionRobotPose          The pose of the robot as measured by the
+   *                                 vision camera.
+   * @param timestamp                The timestamp of the vision measurement in
+   *                                 seconds. Note that if you don't use your
+   *                                 own time source by calling
+   *                                 UpdateWithTime(), then you must use a
+   *                                 timestamp with an epoch since FPGA startup
+   *                                 (i.e. the epoch of this timestamp is the
+   *                                 same epoch as
+   *                                 frc::Timer::GetFPGATimestamp(). This means
+   *                                 that you should use
+   *                                 frc::Timer::GetFPGATimestamp() as your
+   *                                 time source in this case.
+   * @param visionMeasurementStdDevs Standard deviations of the vision
+   *                                 measurements. Increase these numbers to
+   *                                 trust global measurements from vision
+   *                                 less. This matrix is in the form
+   *                                 [x, y, theta]ᵀ, with units in meters and
+   *                                 radians.
+   */
+  void AddVisionMeasurement(
+      const Pose2d& visionRobotPose, units::second_t timestamp,
+      const wpi::array<double, 3>& visionMeasurementStdDevs) {
+    SetVisionMeasurementStdDevs(visionMeasurementStdDevs);
+    AddVisionMeasurement(visionRobotPose, timestamp);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param gyroAngle    The current gyro angle.
+   * @param moduleStates The current velocities and rotations of the swerve
+   *                     modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  template <typename... ModuleState>
+  Pose2d Update(const Rotation2d& gyroAngle, ModuleState&&... moduleStates) {
+    return UpdateWithTime(units::microsecond_t(wpi::Now()), gyroAngle,
+                          moduleStates...);
+  }
+
+  /**
+   * Updates the the Unscented Kalman Filter using only wheel encoder
+   * information. This should be called every loop, and the correct loop period
+   * must be passed into the constructor of this class.
+   *
+   * @param currentTime  Time at which this method was called, in seconds.
+   * @param gyroAngle    The current gyroscope angle.
+   * @param moduleStates The current velocities and rotations of the swerve
+   *                     modules.
+   * @return The estimated pose of the robot in meters.
+   */
+  template <typename... ModuleState>
+  Pose2d UpdateWithTime(units::second_t currentTime,
+                        const Rotation2d& gyroAngle,
+                        ModuleState&&... moduleStates) {
+    auto dt = m_prevTime >= 0_s ? currentTime - m_prevTime : m_nominalDt;
+    m_prevTime = currentTime;
+
+    auto angle = gyroAngle + m_gyroOffset;
+    auto omega = (angle - m_previousAngle).Radians() / dt;
+
+    auto chassisSpeeds = m_kinematics.ToChassisSpeeds(moduleStates...);
+    auto fieldRelativeSpeeds =
+        Translation2d(chassisSpeeds.vx * 1_s, chassisSpeeds.vy * 1_s)
+            .RotateBy(angle);
+
+    Eigen::Vector<double, 3> u{fieldRelativeSpeeds.X().value(),
+                               fieldRelativeSpeeds.Y().value(), omega.value()};
+
+    Eigen::Vector<double, 1> localY{angle.Radians().value()};
+    m_previousAngle = angle;
+
+    m_latencyCompensator.AddObserverState(m_observer, u, localY, currentTime);
+
+    m_observer.Predict(u, dt);
+    m_observer.Correct(u, localY);
+
+    return GetEstimatedPosition();
+  }
+
+ private:
+  UnscentedKalmanFilter<3, 3, 1> m_observer;
+  SwerveDriveKinematics<NumModules>& m_kinematics;
+  KalmanFilterLatencyCompensator<3, 3, 1, UnscentedKalmanFilter<3, 3, 1>>
+      m_latencyCompensator;
+  std::function<void(const Eigen::Vector<double, 3>& u,
+                     const Eigen::Vector<double, 3>& y)>
+      m_visionCorrect;
+
+  Eigen::Matrix3d m_visionContR;
+
+  units::second_t m_nominalDt;
+  units::second_t m_prevTime = -1_s;
+
+  Rotation2d m_gyroOffset;
+  Rotation2d m_previousAngle;
+
+  template <int Dim>
+  static wpi::array<double, Dim> StdDevMatrixToArray(
+      const Eigen::Vector<double, Dim>& vector) {
+    wpi::array<double, Dim> array;
+    for (size_t i = 0; i < Dim; ++i) {
+      array[i] = vector(i);
+    }
+    return array;
+  }
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
index 8c2c31f..3aa3e59 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedKalmanFilter.h
@@ -1,27 +1,49 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
 #include <functional>
 
+#include <wpi/array.h>
+
+#include "Eigen/Cholesky"
 #include "Eigen/Core"
-#include "Eigen/src/Cholesky/LDLT.h"
 #include "frc/StateSpaceUtil.h"
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 #include "frc/estimator/UnscentedTransform.h"
 #include "frc/system/Discretization.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
 #include "units/time.h"
 
 namespace frc {
 
+/**
+ * A Kalman filter combines predictions from a model and measurements to give an
+ * estimate of the true system state. This is useful because many states cannot
+ * be measured directly as a result of sensor noise, or because the state is
+ * "hidden".
+ *
+ * Kalman filters use a K gain matrix to determine whether to trust the model or
+ * measurements more. Kalman filter theory uses statistics to compute an optimal
+ * K gain which minimizes the sum of squares error in the state estimate. This K
+ * gain is used to correct the state estimate by some amount of the difference
+ * between the actual measurements and the measurements predicted by the model.
+ *
+ * An unscented Kalman filter uses nonlinear state and measurement models. It
+ * propagates the error covariance using sigma points chosen to approximate the
+ * true probability distribution.
+ *
+ * For more on the underlying math, read
+ * https://file.tavsys.net/control/controls-engineering-in-frc.pdf chapter 9
+ * "Stochastic control theory".
+ *
+ * @tparam States The number of states.
+ * @tparam Inputs The number of inputs.
+ * @tparam Outputs The number of outputs.
+ */
 template <int States, int Inputs, int Outputs>
 class UnscentedKalmanFilter {
  public:
@@ -36,20 +58,105 @@
    * @param measurementStdDevs Standard deviations of measurements.
    * @param dt                 Nominal discretization timestep.
    */
-  UnscentedKalmanFilter(std::function<Eigen::Matrix<double, States, 1>(
-                            const Eigen::Matrix<double, States, 1>&,
-                            const Eigen::Matrix<double, Inputs, 1>&)>
+  UnscentedKalmanFilter(std::function<Eigen::Vector<double, States>(
+                            const Eigen::Vector<double, States>&,
+                            const Eigen::Vector<double, Inputs>&)>
                             f,
-                        std::function<Eigen::Matrix<double, Outputs, 1>(
-                            const Eigen::Matrix<double, States, 1>&,
-                            const Eigen::Matrix<double, Inputs, 1>&)>
+                        std::function<Eigen::Vector<double, Outputs>(
+                            const Eigen::Vector<double, States>&,
+                            const Eigen::Vector<double, Inputs>&)>
                             h,
-                        const std::array<double, States>& stateStdDevs,
-                        const std::array<double, Outputs>& measurementStdDevs,
+                        const wpi::array<double, States>& stateStdDevs,
+                        const wpi::array<double, Outputs>& measurementStdDevs,
                         units::second_t dt)
       : m_f(f), m_h(h) {
     m_contQ = MakeCovMatrix(stateStdDevs);
     m_contR = MakeCovMatrix(measurementStdDevs);
+    m_meanFuncX = [](auto sigmas, auto Wm) -> Eigen::Vector<double, States> {
+      return sigmas * Wm;
+    };
+    m_meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Outputs> {
+      return sigmas * Wc;
+    };
+    m_residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
+      return a - b;
+    };
+    m_residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Outputs> {
+      return a - b;
+    };
+    m_addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
+      return a + b;
+    };
+    m_dt = dt;
+
+    Reset();
+  }
+
+  /**
+   * Constructs an unscented Kalman filter with custom mean, residual, and
+   * addition functions. Using custom functions for arithmetic can be useful if
+   * you have angles in the state or measurements, because they allow you to
+   * correctly account for the modular nature of angle arithmetic.
+   *
+   * @param f                  A vector-valued function of x and u that returns
+   *                           the derivative of the state vector.
+   * @param h                  A vector-valued function of x and u that returns
+   *                           the measurement vector.
+   * @param stateStdDevs       Standard deviations of model states.
+   * @param measurementStdDevs Standard deviations of measurements.
+   * @param meanFuncX          A function that computes the mean of 2 * States +
+   *                           1 state vectors using a given set of weights.
+   * @param meanFuncY          A function that computes the mean of 2 * States +
+   *                           1 measurement vectors using a given set of
+   *                           weights.
+   * @param residualFuncX      A function that computes the residual of two
+   *                           state vectors (i.e. it subtracts them.)
+   * @param residualFuncY      A function that computes the residual of two
+   *                           measurement vectors (i.e. it subtracts them.)
+   * @param addFuncX           A function that adds two state vectors.
+   * @param dt                 Nominal discretization timestep.
+   */
+  UnscentedKalmanFilter(
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, Inputs>&)>
+          f,
+      std::function<
+          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, States>&,
+                                         const Eigen::Vector<double, Inputs>&)>
+          h,
+      const wpi::array<double, States>& stateStdDevs,
+      const wpi::array<double, Outputs>& measurementStdDevs,
+      std::function<Eigen::Vector<double, States>(
+          const Eigen::Matrix<double, States, 2 * States + 1>&,
+          const Eigen::Vector<double, 2 * States + 1>&)>
+          meanFuncX,
+      std::function<Eigen::Vector<double, Outputs>(
+          const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
+          const Eigen::Vector<double, 2 * States + 1>&)>
+          meanFuncY,
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, States>&)>
+          residualFuncX,
+      std::function<
+          Eigen::Vector<double, Outputs>(const Eigen::Vector<double, Outputs>&,
+                                         const Eigen::Vector<double, Outputs>&)>
+          residualFuncY,
+      std::function<
+          Eigen::Vector<double, States>(const Eigen::Vector<double, States>&,
+                                        const Eigen::Vector<double, States>&)>
+          addFuncX,
+      units::second_t dt)
+      : m_f(f),
+        m_h(h),
+        m_meanFuncX(meanFuncX),
+        m_meanFuncY(meanFuncY),
+        m_residualFuncX(residualFuncX),
+        m_residualFuncY(residualFuncY),
+        m_addFuncX(addFuncX) {
+    m_contQ = MakeCovMatrix(stateStdDevs);
+    m_contR = MakeCovMatrix(measurementStdDevs);
     m_dt = dt;
 
     Reset();
@@ -78,7 +185,7 @@
   /**
    * Returns the state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const { return m_xHat; }
+  const Eigen::Vector<double, States>& Xhat() const { return m_xHat; }
 
   /**
    * Returns an element of the state estimate x-hat.
@@ -92,7 +199,7 @@
    *
    * @param xHat The state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) { m_xHat = xHat; }
+  void SetXhat(const Eigen::Vector<double, States>& xHat) { m_xHat = xHat; }
 
   /**
    * Set an element of the initial state estimate x-hat.
@@ -117,7 +224,7 @@
    * @param u  New control input from controller.
    * @param dt Timestep for prediction.
    */
-  void Predict(const Eigen::Matrix<double, Inputs, 1>& u, units::second_t dt) {
+  void Predict(const Eigen::Vector<double, Inputs>& u, units::second_t dt) {
     m_dt = dt;
 
     // Discretize Q before projecting mean and covariance forward
@@ -131,13 +238,12 @@
         m_pts.SigmaPoints(m_xHat, m_P);
 
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      Eigen::Matrix<double, States, 1> x =
-          sigmas.template block<States, 1>(0, i);
-      m_sigmasF.template block<States, 1>(0, i) = RungeKutta(m_f, x, u, dt);
+      Eigen::Vector<double, States> x = sigmas.template block<States, 1>(0, i);
+      m_sigmasF.template block<States, 1>(0, i) = RK4(m_f, x, u, dt);
     }
 
-    auto ret =
-        UnscentedTransform<States, States>(m_sigmasF, m_pts.Wm(), m_pts.Wc());
+    auto ret = UnscentedTransform<States, States>(
+        m_sigmasF, m_pts.Wm(), m_pts.Wc(), m_meanFuncX, m_residualFuncX);
     m_xHat = std::get<0>(ret);
     m_P = std::get<1>(ret);
 
@@ -150,9 +256,10 @@
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Outputs, 1>& y) {
-    Correct<Outputs>(u, y, m_h, m_contR);
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Outputs>& y) {
+    Correct<Outputs>(u, y, m_h, m_contR, m_meanFuncY, m_residualFuncY,
+                     m_residualFuncX, m_addFuncX);
   }
 
   /**
@@ -164,18 +271,78 @@
    *
    * @param u Same control input used in the predict step.
    * @param y Measurement vector.
-   * @param h A vector-valued function of x and u that returns
-   *          the measurement vector.
-   * @param R Measurement noise covariance matrix.
+   * @param h A vector-valued function of x and u that returns the measurement
+   *          vector.
+   * @param R Measurement noise covariance matrix (continuous-time).
    */
   template <int Rows>
-  void Correct(const Eigen::Matrix<double, Inputs, 1>& u,
-               const Eigen::Matrix<double, Rows, 1>& y,
-               std::function<Eigen::Matrix<double, Rows, 1>(
-                   const Eigen::Matrix<double, States, 1>&,
-                   const Eigen::Matrix<double, Inputs, 1>&)>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
                    h,
                const Eigen::Matrix<double, Rows, Rows>& R) {
+    auto meanFuncY = [](auto sigmas, auto Wc) -> Eigen::Vector<double, Rows> {
+      return sigmas * Wc;
+    };
+    auto residualFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
+      return a - b;
+    };
+    auto residualFuncY = [](auto a, auto b) -> Eigen::Vector<double, Rows> {
+      return a - b;
+    };
+    auto addFuncX = [](auto a, auto b) -> Eigen::Vector<double, States> {
+      return a + b;
+    };
+    Correct<Rows>(u, y, h, R, meanFuncY, residualFuncY, residualFuncX,
+                  addFuncX);
+  }
+
+  /**
+   * Correct the state estimate x-hat using the measurements in y.
+   *
+   * This is useful for when the measurements available during a timestep's
+   * Correct() call vary. The h(x, u) passed to the constructor is used if one
+   * is not provided (the two-argument version of this function).
+   *
+   * @param u             Same control input used in the predict step.
+   * @param y             Measurement vector.
+   * @param h             A vector-valued function of x and u that returns the
+   *                      measurement vector.
+   * @param R             Measurement noise covariance matrix (continuous-time).
+   * @param meanFuncY     A function that computes the mean of 2 * States + 1
+   *                      measurement vectors using a given set of weights.
+   * @param residualFuncY A function that computes the residual of two
+   *                      measurement vectors (i.e. it subtracts them.)
+   * @param residualFuncX A function that computes the residual of two state
+   *                      vectors (i.e. it subtracts them.)
+   * @param addFuncX      A function that adds two state vectors.
+   */
+  template <int Rows>
+  void Correct(const Eigen::Vector<double, Inputs>& u,
+               const Eigen::Vector<double, Rows>& y,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, Inputs>&)>
+                   h,
+               const Eigen::Matrix<double, Rows, Rows>& R,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Matrix<double, Rows, 2 * States + 1>&,
+                   const Eigen::Vector<double, 2 * States + 1>&)>
+                   meanFuncY,
+               std::function<Eigen::Vector<double, Rows>(
+                   const Eigen::Vector<double, Rows>&,
+                   const Eigen::Vector<double, Rows>&)>
+                   residualFuncY,
+               std::function<Eigen::Vector<double, States>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, States>&)>
+                   residualFuncX,
+               std::function<Eigen::Vector<double, States>(
+                   const Eigen::Vector<double, States>&,
+                   const Eigen::Vector<double, States>)>
+                   addFuncX) {
     const Eigen::Matrix<double, Rows, Rows> discR = DiscretizeR<Rows>(R, m_dt);
 
     // Transform sigma points into measurement space
@@ -188,41 +355,67 @@
     }
 
     // Mean and covariance of prediction passed through UT
-    auto [yHat, Py] =
-        UnscentedTransform<States, Rows>(sigmasH, m_pts.Wm(), m_pts.Wc());
+    auto [yHat, Py] = UnscentedTransform<Rows, States>(
+        sigmasH, m_pts.Wm(), m_pts.Wc(), meanFuncY, residualFuncY);
     Py += discR;
 
     // Compute cross covariance of the state and the measurements
     Eigen::Matrix<double, States, Rows> Pxy;
     Pxy.setZero();
     for (int i = 0; i < m_pts.NumSigmas(); ++i) {
-      Pxy += m_pts.Wc(i) *
-             (m_sigmasF.template block<States, 1>(0, i) - m_xHat) *
-             (sigmasH.template block<Rows, 1>(0, i) - yHat).transpose();
+      // Pxy += (sigmas_f[:, i] - x̂)(sigmas_h[:, i] - ŷ)ᵀ W_c[i]
+      Pxy +=
+          m_pts.Wc(i) *
+          (residualFuncX(m_sigmasF.template block<States, 1>(0, i), m_xHat)) *
+          (residualFuncY(sigmasH.template block<Rows, 1>(0, i), yHat))
+              .transpose();
     }
 
-    // K = P_{xy} Py^-1
-    // K^T = P_y^T^-1 P_{xy}^T
-    // P_y^T K^T = P_{xy}^T
-    // K^T = P_y^T.solve(P_{xy}^T)
-    // K = (P_y^T.solve(P_{xy}^T)^T
+    // K = P_{xy} P_y⁻¹
+    // Kᵀ = P_yᵀ⁻¹ P_{xy}ᵀ
+    // P_yᵀKᵀ = P_{xy}ᵀ
+    // Kᵀ = P_yᵀ.solve(P_{xy}ᵀ)
+    // K = (P_yᵀ.solve(P_{xy}ᵀ)ᵀ
     Eigen::Matrix<double, States, Rows> K =
         Py.transpose().ldlt().solve(Pxy.transpose()).transpose();
 
-    m_xHat += K * (y - yHat);
+    // x̂ₖ₊₁⁺ = x̂ₖ₊₁⁻ + K(y − ŷ)
+    m_xHat = addFuncX(m_xHat, K * residualFuncY(y, yHat));
+
+    // Pₖ₊₁⁺ = Pₖ₊₁⁻ − KP_yKᵀ
     m_P -= K * Py * K.transpose();
   }
 
  private:
-  std::function<Eigen::Matrix<double, States, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_f;
-  std::function<Eigen::Matrix<double, Outputs, 1>(
-      const Eigen::Matrix<double, States, 1>&,
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, Inputs>&)>
       m_h;
-  Eigen::Matrix<double, States, 1> m_xHat;
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Matrix<double, States, 2 * States + 1>&,
+      const Eigen::Vector<double, 2 * States + 1>&)>
+      m_meanFuncX;
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Matrix<double, Outputs, 2 * States + 1>&,
+      const Eigen::Vector<double, 2 * States + 1>&)>
+      m_meanFuncY;
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, States>&)>
+      m_residualFuncX;
+  std::function<Eigen::Vector<double, Outputs>(
+      const Eigen::Vector<double, Outputs>&,
+      const Eigen::Vector<double, Outputs>)>
+      m_residualFuncY;
+  std::function<Eigen::Vector<double, States>(
+      const Eigen::Vector<double, States>&,
+      const Eigen::Vector<double, States>)>
+      m_addFuncX;
+  Eigen::Vector<double, States> m_xHat;
   Eigen::Matrix<double, States, States> m_P;
   Eigen::Matrix<double, States, States> m_contQ;
   Eigen::Matrix<double, Outputs, Outputs> m_contR;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
index 22b32ce..2df0a47 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/estimator/UnscentedTransform.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -14,37 +11,51 @@
 namespace frc {
 
 /**
- * Computes unscented transform of a set of sigma points and weights. CovDimurns
- * the mean and covariance in a tuple.
+ * Computes unscented transform of a set of sigma points and weights. CovDim
+ * returns the mean and covariance in a tuple.
  *
  * This works in conjunction with the UnscentedKalmanFilter class.
  *
- * @tparam States  Number of states.
- * @tparam CovDim  Dimension of covariance of sigma points after passing through
- *                 the transform.
- * @param sigmas   List of sigma points.
- * @param Wm       Weights for the mean.
- * @param Wc       Weights for the covariance.
+ * @tparam CovDim      Dimension of covariance of sigma points after passing
+ *                     through the transform.
+ * @tparam States      Number of states.
+ * @param sigmas       List of sigma points.
+ * @param Wm           Weights for the mean.
+ * @param Wc           Weights for the covariance.
+ * @param meanFunc     A function that computes the mean of 2 * States + 1 state
+ *                     vectors using a given set of weights.
+ * @param residualFunc A function that computes the residual of two state
+ *                     vectors (i.e. it subtracts them.)
  *
  * @return Tuple of x, mean of sigma points; P, covariance of sigma points after
  *         passing through the transform.
  */
-template <int States, int CovDim>
-std::tuple<Eigen::Matrix<double, CovDim, 1>,
-           Eigen::Matrix<double, CovDim, CovDim>>
+template <int CovDim, int States>
+std::tuple<Eigen::Vector<double, CovDim>, Eigen::Matrix<double, CovDim, CovDim>>
 UnscentedTransform(const Eigen::Matrix<double, CovDim, 2 * States + 1>& sigmas,
-                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wm,
-                   const Eigen::Matrix<double, 2 * States + 1, 1>& Wc) {
-  // New mean is just the sum of the sigmas * weight
-  // dot = \Sigma^n_1 (W[k]*Xi[k])
-  Eigen::Matrix<double, CovDim, 1> x = sigmas * Wm;
+                   const Eigen::Vector<double, 2 * States + 1>& Wm,
+                   const Eigen::Vector<double, 2 * States + 1>& Wc,
+                   std::function<Eigen::Vector<double, CovDim>(
+                       const Eigen::Matrix<double, CovDim, 2 * States + 1>&,
+                       const Eigen::Vector<double, 2 * States + 1>&)>
+                       meanFunc,
+                   std::function<Eigen::Vector<double, CovDim>(
+                       const Eigen::Vector<double, CovDim>&,
+                       const Eigen::Vector<double, CovDim>&)>
+                       residualFunc) {
+  // New mean is usually just the sum of the sigmas * weight:
+  //       n
+  // dot = Σ W[k] Xᵢ[k]
+  //      k=1
+  Eigen::Vector<double, CovDim> x = meanFunc(sigmas, Wm);
 
   // New covariance is the sum of the outer product of the residuals times the
   // weights
   Eigen::Matrix<double, CovDim, 2 * States + 1> y;
   for (int i = 0; i < 2 * States + 1; ++i) {
+    // y[:, i] = sigmas[:, i] - x
     y.template block<CovDim, 1>(0, i) =
-        sigmas.template block<CovDim, 1>(0, i) - x;
+        residualFunc(sigmas.template block<CovDim, 1>(0, i), x);
   }
   Eigen::Matrix<double, CovDim, CovDim> P =
       y * Eigen::DiagonalMatrix<double, 2 * States + 1>(Wc) * y.transpose();
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h
new file mode 100644
index 0000000..92d8bdc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/LinearFilter.h
@@ -0,0 +1,298 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <initializer_list>
+#include <stdexcept>
+#include <vector>
+
+#include <wpi/circular_buffer.h>
+#include <wpi/span.h>
+
+#include "Eigen/Core"
+#include "Eigen/QR"
+#include "units/time.h"
+#include "wpimath/MathShared.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 "feedforward" or FIR gains.
+   * @param fbGains The "feedback" or IIR gains.
+   */
+  LinearFilter(wpi::span<const double> ffGains, wpi::span<const double> fbGains)
+      : m_inputs(ffGains.size()),
+        m_outputs(fbGains.size()),
+        m_inputGains(ffGains.begin(), ffGains.end()),
+        m_outputGains(fbGains.begin(), fbGains.end()) {
+    for (size_t i = 0; i < ffGains.size(); ++i) {
+      m_inputs.emplace_front(0.0);
+    }
+    for (size_t i = 0; i < fbGains.size(); ++i) {
+      m_outputs.emplace_front(0.0);
+    }
+
+    static int instances = 0;
+    instances++;
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kFilter_Linear, 1);
+  }
+
+  /**
+   * Create a linear FIR or IIR filter.
+   *
+   * @param ffGains The "feedforward" or FIR gains.
+   * @param fbGains The "feedback" or IIR gains.
+   */
+  LinearFilter(std::initializer_list<double> ffGains,
+               std::initializer_list<double> fbGains)
+      : LinearFilter({ffGains.begin(), ffGains.end()},
+                     {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
+   *
+   * Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
+   * above which the input starts to attenuate.
+   *
+   * 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.value() / 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
+   *
+   * Note: T = 1 / (2 pi f) where f is the cutoff frequency in Hz, the frequency
+   * below which the input starts to attenuate.
+   *
+   * 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.value() / 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) {
+    if (taps <= 0) {
+      throw std::runtime_error("Number of taps must be greater than zero.");
+    }
+
+    std::vector<double> gains(taps, 1.0 / taps);
+    return LinearFilter(gains, {});
+  }
+
+  /**
+   * Creates a backward finite difference filter that computes the nth
+   * derivative of the input given the specified number of samples.
+   *
+   * For example, a first derivative filter that uses two samples and a sample
+   * period of 20 ms would be
+   *
+   * <pre><code>
+   * LinearFilter<double>::BackwardFiniteDifference<1, 2>(20_ms);
+   * </code></pre>
+   *
+   * @tparam Derivative The order of the derivative to compute.
+   * @tparam Samples    The number of samples to use to compute the given
+   *                    derivative. This must be one more than the order of
+   *                    derivative or higher.
+   * @param period      The period in seconds between samples taken by the user.
+   */
+  template <int Derivative, int Samples>
+  static auto BackwardFiniteDifference(units::second_t period) {
+    // See
+    // https://en.wikipedia.org/wiki/Finite_difference_coefficient#Arbitrary_stencil_points
+    //
+    // For a given list of stencil points s of length n and the order of
+    // derivative d < n, the finite difference coefficients can be obtained by
+    // solving the following linear system for the vector a.
+    //
+    // @verbatim
+    // [s₁⁰   ⋯  sₙ⁰ ][a₁]      [ δ₀,d ]
+    // [ ⋮    ⋱  ⋮   ][⋮ ] = d! [  ⋮   ]
+    // [s₁ⁿ⁻¹ ⋯ sₙⁿ⁻¹][aₙ]      [δₙ₋₁,d]
+    // @endverbatim
+    //
+    // where δᵢ,ⱼ are the Kronecker delta. For backward finite difference, the
+    // stencil points are the range [-n + 1, 0]. The FIR gains are the elements
+    // of the vector a in reverse order divided by hᵈ.
+    //
+    // The order of accuracy of the approximation is of the form O(hⁿ⁻ᵈ).
+
+    static_assert(Derivative >= 1,
+                  "Order of derivative must be greater than or equal to one.");
+    static_assert(Samples > 0, "Number of samples must be greater than zero.");
+    static_assert(Derivative < Samples,
+                  "Order of derivative must be less than number of samples.");
+
+    Eigen::Matrix<double, Samples, Samples> S;
+    for (int row = 0; row < Samples; ++row) {
+      for (int col = 0; col < Samples; ++col) {
+        double s = 1 - Samples + col;
+        S(row, col) = std::pow(s, row);
+      }
+    }
+
+    // Fill in Kronecker deltas: https://en.wikipedia.org/wiki/Kronecker_delta
+    Eigen::Vector<double, Samples> d;
+    for (int i = 0; i < Samples; ++i) {
+      d(i) = (i == Derivative) ? Factorial(Derivative) : 0.0;
+    }
+
+    Eigen::Vector<double, Samples> a =
+        S.householderQr().solve(d) / std::pow(period.value(), Derivative);
+
+    // Reverse gains list
+    std::vector<double> gains;
+    for (int i = Samples - 1; i >= 0; --i) {
+      gains.push_back(a(i));
+    }
+
+    return LinearFilter(gains, {});
+  }
+
+  /**
+   * Reset the filter state.
+   */
+  void Reset() {
+    std::fill(m_inputs.begin(), m_inputs.end(), T{0.0});
+    std::fill(m_outputs.begin(), m_outputs.end(), T{0.0});
+  }
+
+  /**
+   * 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{0.0};
+
+    // Rotate the inputs
+    if (m_inputGains.size() > 0) {
+      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
+    if (m_outputGains.size() > 0) {
+      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;
+
+  /**
+   * Factorial of n.
+   *
+   * @param n Argument of which to take factorial.
+   */
+  static constexpr int Factorial(int n) {
+    if (n < 2) {
+      return 1;
+    } else {
+      return n * Factorial(n - 1);
+    }
+  }
+};
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/MedianFilter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/MedianFilter.h
new file mode 100644
index 0000000..40422a6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/MedianFilter.h
@@ -0,0 +1,77 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this 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;
+    }
+
+    // Add next value to circular buffer
+    m_valueBuffer.push_front(next);
+
+    if (curSize % 2 != 0) {
+      // 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/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
new file mode 100644
index 0000000..f99c1af
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/filter/SlewRateLimiter.h
@@ -0,0 +1,73 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <algorithm>
+
+#include <wpi/timestamp.h>
+
+#include "units/time.h"
+
+namespace frc {
+/**
+ * A class that limits the rate of change of an input value.  Useful for
+ * implementing voltage, setpoint, and/or output ramps.  A slew-rate limit
+ * is most appropriate when the quantity being controlled is a velocity or
+ * a voltage; when controlling a position, consider using a TrapezoidProfile
+ * instead.
+ *
+ * @see TrapezoidProfile
+ */
+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>;
+
+  /**
+   * Creates a new SlewRateLimiter with the given rate limit and initial value.
+   *
+   * @param rateLimit The rate-of-change limit.
+   * @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_prevTime{units::microsecond_t(wpi::Now())} {}
+
+  /**
+   * Filters the input to limit its slew rate.
+   *
+   * @param input The input value whose slew rate is to be limited.
+   * @return The filtered value, which will not change faster than the slew
+   * rate.
+   */
+  Unit_t Calculate(Unit_t input) {
+    units::second_t currentTime = units::microsecond_t(wpi::Now());
+    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;
+  }
+
+  /**
+   * Resets the slew rate limiter to the specified value; ignores the rate limit
+   * when doing so.
+   *
+   * @param value The value to reset to.
+   */
+  void Reset(Unit_t value) {
+    m_prevVal = value;
+    m_prevTime = units::microsecond_t(wpi::Now());
+  }
+
+ private:
+  Rate_t m_rateLimit;
+  Unit_t m_prevVal;
+  units::second_t m_prevTime;
+};
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/fmt/Eigen.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/fmt/Eigen.h
new file mode 100644
index 0000000..f6cc7b6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/fmt/Eigen.h
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fmt/format.h>
+
+#include "Eigen/Core"
+
+/**
+ * Formatter for Eigen::Matrix<>.
+ *
+ * @tparam Rows Number of rows.
+ * @tparam Cols Number of columns.
+ * @tparam Args Defaulted template arguments to Eigen::Matrix<>.
+ */
+template <int Rows, int Cols, int... Args>
+struct fmt::formatter<Eigen::Matrix<double, Rows, Cols, Args...>> {
+  /**
+   * Storage for format specifier.
+   */
+  char presentation = 'f';
+
+  /**
+   * Format string parser.
+   *
+   * @param ctx Format string context.
+   */
+  constexpr auto parse(fmt::format_parse_context& ctx) {
+    auto it = ctx.begin(), end = ctx.end();
+    if (it != end && (*it == 'f' || *it == 'e')) {
+      presentation = *it++;
+    }
+
+    if (it != end && *it != '}') {
+      throw fmt::format_error("invalid format");
+    }
+
+    return it;
+  }
+
+  /**
+   * Writes out a formatted matrix.
+   *
+   * @tparam FormatContext Format string context type.
+   * @param mat Matrix to format.
+   * @param ctx Format string context.
+   */
+  template <typename FormatContext>
+  auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
+              FormatContext& ctx) {
+    auto out = ctx.out();
+    for (int i = 0; i < Rows; ++i) {
+      for (int j = 0; j < Cols; ++j) {
+        out = fmt::format_to(out, "  {:f}", mat(i, j));
+      }
+
+      if (i < Rows - 1) {
+        out = fmt::format_to(out, "\n");
+      }
+    }
+
+    return out;
+  }
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/fmt/Units.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/fmt/Units.h
new file mode 100644
index 0000000..1ec61ca
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/fmt/Units.h
@@ -0,0 +1,218 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fmt/format.h>
+
+#include "units/base.h"
+
+/**
+ * Formatter for unit types.
+ *
+ * @tparam Units Unit tag for which type of units the `unit_t` represents (e.g.
+ *               meters).
+ * @tparam T Underlying type of the storage. Defaults to double.
+ * @tparam NonLinearScale Optional scale class for the units. Defaults to linear
+ *                        (i.e. does not scale the unit value). Examples of
+ *                        non-linear scales could be logarithmic, decibel, or
+ *                        richter scales. Non-linear scales must adhere to the
+ *                        non-linear-scale concept.
+ */
+template <class Units, typename T, template <typename> class NonLinearScale>
+struct fmt::formatter<units::unit_t<Units, T, NonLinearScale>>
+    : fmt::formatter<double> {
+  /**
+   * Writes out a formatted unit.
+   *
+   * @tparam FormatContext Format string context type.
+   * @param obj Unit instance.
+   * @param ctx Format string context.
+   */
+  template <typename FormatContext>
+  auto format(const units::unit_t<Units, T, NonLinearScale>& obj,
+              FormatContext& ctx) {
+    using BaseUnits =
+        units::unit<std::ratio<1>,
+                    typename units::traits::unit_traits<Units>::base_unit_type>;
+
+    auto out = ctx.out();
+
+    out = fmt::formatter<double>::format(
+        units::convert<Units, BaseUnits>(obj()), ctx);
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::num != 0) {
+      out = fmt::format_to(out, " m");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::meter_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::meter_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::num != 0) {
+      out = fmt::format_to(out, " kg");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::num != 1) {
+      out = fmt::format_to(out, "^{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::kilogram_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::den != 1) {
+      out = fmt::format_to(out, "/{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::kilogram_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::num != 0) {
+      out = fmt::format_to(out, " s");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::second_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::second_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::num != 0) {
+      out = fmt::format_to(out, " A");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::ampere_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::ampere_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::num != 0) {
+      out = fmt::format_to(out, " K");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::kelvin_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::num != 0) {
+      out = fmt::format_to(out, " mol");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::mole_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::mole_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::num != 0) {
+      out = fmt::format_to(out, " cd");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::num != 1) {
+      out = fmt::format_to(out, "^{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::candela_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::den != 1) {
+      out = fmt::format_to(out, "/{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::candela_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::num != 0) {
+      out = fmt::format_to(out, " rad");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::radian_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::radian_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::num != 0) {
+      out = fmt::format_to(out, " b");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::byte_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::byte_ratio::den);
+    }
+
+    return out;
+  }
+};
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index 43f4756..ebaa7c1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "Transform2d.h"
 #include "Translation2d.h"
 #include "Twist2d.h"
@@ -20,7 +19,7 @@
 /**
  * Represents a 2d pose containing translational and rotational elements.
  */
-class Pose2d {
+class WPILIB_DLLEXPORT Pose2d {
  public:
   /**
    * Constructs a pose at the origin facing toward the positive X axis.
@@ -61,18 +60,6 @@
   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.
@@ -186,8 +173,10 @@
   Rotation2d m_rotation;
 };
 
+WPILIB_DLLEXPORT
 void to_json(wpi::json& json, const Pose2d& pose);
 
+WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Pose2d& pose);
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index 914eba4..94a17fc 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "units/angle.h"
 
 namespace wpi {
@@ -19,7 +18,7 @@
  * A rotation in a 2d coordinate frame represented a point on the unit circle
  * (cosine and sine).
  */
-class Rotation2d {
+class WPILIB_DLLEXPORT Rotation2d {
  public:
   /**
    * Constructs a Rotation2d with a default angle of 0 degrees.
@@ -31,14 +30,14 @@
    *
    * @param value The value of the angle in radians.
    */
-  Rotation2d(units::radian_t value);  // NOLINT(runtime/explicit)
+  Rotation2d(units::radian_t value);  // NOLINT
 
   /**
    * Constructs a Rotation2d with the given degree value.
    *
    * @param value The value of the angle in degrees.
    */
-  Rotation2d(units::degree_t value);  // NOLINT(runtime/explicit)
+  Rotation2d(units::degree_t value);  // NOLINT
 
   /**
    * Constructs a Rotation2d with the given x and y (cosine and sine)
@@ -63,18 +62,6 @@
   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.
    *
@@ -88,18 +75,6 @@
   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.
    *
@@ -134,10 +109,11 @@
   /**
    * Adds the new rotation to the current rotation using a rotation matrix.
    *
+   * <pre>
    * [cos_new]   [other.cos, -other.sin][cos]
    * [sin_new] = [other.sin,  other.cos][sin]
-   *
-   * value_new = std::atan2(cos_new, sin_new)
+   * value_new = std::atan2(sin_new, cos_new)
+   * </pre>
    *
    * @param other The rotation to rotate by.
    *
@@ -186,8 +162,10 @@
   double m_sin = 0;
 };
 
+WPILIB_DLLEXPORT
 void to_json(wpi::json& json, const Rotation2d& rotation);
 
+WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Rotation2d& rotation);
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index 8f05413..3d5e1d6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -1,22 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "Translation2d.h"
 
 namespace frc {
 
-class Pose2d;
+class WPILIB_DLLEXPORT Pose2d;
 
 /**
  * Represents a transformation for a Pose2d.
  */
-class Transform2d {
+class WPILIB_DLLEXPORT Transform2d {
  public:
   /**
    * Constructs the transform that maps the initial pose to the final pose.
@@ -85,6 +84,14 @@
   }
 
   /**
+   * Composes two transformations.
+   *
+   * @param other The transform to compose with this one.
+   * @return The composition of the two transformations.
+   */
+  Transform2d operator+(const Transform2d& other) const;
+
+  /**
    * Checks equality between this Transform2d and another object.
    *
    * @param other The other object.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index 0c3ee3d..204da30 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "Rotation2d.h"
 #include "units/length.h"
 
@@ -24,7 +23,7 @@
  * 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 {
+class WPILIB_DLLEXPORT Translation2d {
  public:
   /**
    * Constructs a Translation2d with X and Y components equal to zero.
@@ -114,18 +113,6 @@
   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.
    *
@@ -139,18 +126,6 @@
   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.
@@ -171,17 +146,6 @@
   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}
@@ -208,24 +172,15 @@
    */
   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;
 };
 
+WPILIB_DLLEXPORT
 void to_json(wpi::json& json, const Translation2d& state);
 
+WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Translation2d& state);
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h
index b71ee56..9d7a856 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/geometry/Twist2d.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "units/angle.h"
 #include "units/length.h"
 #include "units/math.h"
@@ -19,7 +18,7 @@
  *
  * A Twist can be used to represent a difference between two poses.
  */
-struct Twist2d {
+struct WPILIB_DLLEXPORT Twist2d {
   /**
    * Linear "dx" component
    */
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
index 1716ca7..7414dec 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/ChassisSpeeds.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/geometry/Rotation2d.h"
 #include "units/angular_velocity.h"
 #include "units/velocity.h"
@@ -23,7 +22,7 @@
  * 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 {
+struct WPILIB_DLLEXPORT ChassisSpeeds {
   /**
    * Represents forward velocity w.r.t the robot frame of reference. (Fwd is +)
    */
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
index 9e48b5e..4bf27ff 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveWheelSpeeds.h"
 #include "units/angle.h"
@@ -22,7 +21,7 @@
  * velocity components whereas forward kinematics converts left and right
  * component velocities into a linear and angular chassis speed.
  */
-class DifferentialDriveKinematics {
+class WPILIB_DLLEXPORT DifferentialDriveKinematics {
  public:
   /**
    * Constructs a differential drive kinematics object.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
index a65b52a..70179de 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveOdometry.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "DifferentialDriveKinematics.h"
 #include "frc/geometry/Pose2d.h"
 #include "units/length.h"
@@ -24,7 +23,7 @@
  * 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 {
+class WPILIB_DLLEXPORT DifferentialDriveOdometry {
  public:
   /**
    * Constructs a DifferentialDriveOdometry object.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
index 20085bb..2bf9fb9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveWheelSpeeds.h
@@ -1,19 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "units/velocity.h"
 
 namespace frc {
 /**
  * Represents the wheel speeds for a differential drive drivetrain.
  */
-struct DifferentialDriveWheelSpeeds {
+struct WPILIB_DLLEXPORT DifferentialDriveWheelSpeeds {
   /**
    * Speed of the left side of the robot.
    */
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
index a0ccb52..9a7cef9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "Eigen/Core"
 #include "Eigen/QR"
 #include "frc/geometry/Translation2d.h"
@@ -38,7 +37,7 @@
  * Forward kinematics is also used for odometry -- determining the position of
  * the robot on the field using encoders and a gyro.
  */
-class MecanumDriveKinematics {
+class WPILIB_DLLEXPORT MecanumDriveKinematics {
  public:
   /**
    * Constructs a mecanum drive kinematics object.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
index 546a498..bdd1239 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveOdometry.h
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
 #include <wpi/timestamp.h>
 
 #include "frc/geometry/Pose2d.h"
@@ -25,7 +23,7 @@
  * path following. Furthermore, odometry can be used for latency compensation
  * when using computer-vision systems.
  */
-class MecanumDriveOdometry {
+class WPILIB_DLLEXPORT MecanumDriveOdometry {
  public:
   /**
    * Constructs a MecanumDriveOdometry object.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
index aa82b99..c24b134 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/MecanumDriveWheelSpeeds.h
@@ -1,19 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "units/velocity.h"
 
 namespace frc {
 /**
  * Represents the wheel speeds for a mecanum drive drivetrain.
  */
-struct MecanumDriveWheelSpeeds {
+struct WPILIB_DLLEXPORT MecanumDriveWheelSpeeds {
   /**
    * Speed of the front-left wheel.
    */
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
index 0ed50b4..84057ac 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
 #include <cstddef>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "Eigen/QR"
 #include "frc/geometry/Rotation2d.h"
@@ -53,8 +51,10 @@
    * 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.
+   * @param wheel  The location of the first wheel relative to the physical
+   *               center of the robot.
+   * @param wheels The locations of the other wheels relative to the physical
+   *               center of the robot.
    */
   template <typename... Wheels>
   explicit SwerveDriveKinematics(Translation2d wheel, Wheels&&... wheels)
@@ -65,8 +65,25 @@
     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>();
+        1, 0, (-m_modules[i].Y()).value(),
+        0, 1, (+m_modules[i].X()).value();
+      // clang-format on
+    }
+
+    m_forwardKinematics = m_inverseKinematics.householderQr();
+
+    wpi::math::MathSharedStore::ReportUsage(
+        wpi::math::MathUsageId::kKinematics_SwerveDrive, 1);
+  }
+
+  explicit SwerveDriveKinematics(
+      const wpi::array<Translation2d, NumModules>& wheels)
+      : m_modules{wheels} {
+    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()).value(),
+        0, 1, (+m_modules[i].X()).value();
       // clang-format on
     }
 
@@ -97,15 +114,16 @@
    * @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
+   * NormalizeWheelSpeeds(wpi::array<SwerveModuleState, NumModules>*,
+   * units::meters_per_second_t) 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(
+  wpi::array<SwerveModuleState, NumModules> ToSwerveModuleStates(
       const ChassisSpeeds& chassisSpeeds,
       const Translation2d& centerOfRotation = Translation2d()) const;
 
@@ -130,7 +148,7 @@
    * 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
+   * @param moduleStates The state of the modules as an wpi::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.
@@ -138,7 +156,7 @@
    * @return The resulting chassis speed.
    */
   ChassisSpeeds ToChassisSpeeds(
-      std::array<SwerveModuleState, NumModules> moduleStates) const;
+      wpi::array<SwerveModuleState, NumModules> moduleStates) const;
 
   /**
    * Normalizes the wheel speeds using some max attainable speed. Sometimes,
@@ -153,14 +171,14 @@
    * @param attainableMaxSpeed The absolute max speed that a module can reach.
    */
   static void NormalizeWheelSpeeds(
-      std::array<SwerveModuleState, NumModules>* moduleStates,
+      wpi::array<SwerveModuleState, NumModules>* moduleStates,
       units::meters_per_second_t attainableMaxSpeed);
 
  private:
   mutable Eigen::Matrix<double, NumModules * 2, 3> m_inverseKinematics;
   Eigen::HouseholderQR<Eigen::Matrix<double, NumModules * 2, 3>>
       m_forwardKinematics;
-  std::array<Translation2d, NumModules> m_modules;
+  wpi::array<Translation2d, NumModules> m_modules;
 
   mutable Translation2d m_previousCoR;
 };
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
index 08eba50..1747453 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveKinematics.inc
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <algorithm>
 
+#include "frc/kinematics/SwerveDriveKinematics.h"
 #include "units/math.h"
 
 namespace frc {
@@ -18,7 +16,7 @@
     -> SwerveDriveKinematics<1 + sizeof...(Wheels)>;
 
 template <size_t NumModules>
-std::array<SwerveModuleState, NumModules>
+wpi::array<SwerveModuleState, NumModules>
 SwerveDriveKinematics<NumModules>::ToSwerveModuleStates(
     const ChassisSpeeds& chassisSpeeds,
     const Translation2d& centerOfRotation) const {
@@ -26,30 +24,29 @@
   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>();
+      m_inverseKinematics.template block<2, 3>(i * 2, 0) =
+        Eigen::Matrix<double, 2, 3>{
+          {1, 0, (-m_modules[i].Y() + centerOfRotation.Y()).value()},
+          {0, 1, (+m_modules[i].X() - centerOfRotation.X()).value()}};
       // clang-format on
     }
     m_previousCoR = centerOfRotation;
   }
 
-  Eigen::Vector3d chassisSpeedsVector;
-  chassisSpeedsVector << chassisSpeeds.vx.to<double>(),
-      chassisSpeeds.vy.to<double>(), chassisSpeeds.omega.to<double>();
+  Eigen::Vector3d chassisSpeedsVector{chassisSpeeds.vx.value(),
+                                      chassisSpeeds.vy.value(),
+                                      chassisSpeeds.omega.value()};
 
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix =
       m_inverseKinematics * chassisSpeedsVector;
-  std::array<SwerveModuleState, NumModules> moduleStates;
+  wpi::array<SwerveModuleState, NumModules> moduleStates{wpi::empty_array};
 
   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)};
+    units::meters_per_second_t x{moduleStatesMatrix(i * 2, 0)};
+    units::meters_per_second_t y{moduleStatesMatrix(i * 2 + 1, 0)};
 
     auto speed = units::math::hypot(x, y);
-    Rotation2d rotation{x.to<double>(), y.to<double>()};
+    Rotation2d rotation{x.value(), y.value()};
 
     moduleStates[i] = {speed, rotation};
   }
@@ -65,22 +62,21 @@
                 "Number of modules is not consistent with number of wheel "
                 "locations provided in constructor.");
 
-  std::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
+  wpi::array<SwerveModuleState, NumModules> moduleStates{wheelStates...};
 
   return this->ToChassisSpeeds(moduleStates);
 }
 
 template <size_t NumModules>
 ChassisSpeeds SwerveDriveKinematics<NumModules>::ToChassisSpeeds(
-    std::array<SwerveModuleState, NumModules> moduleStates) const {
+    wpi::array<SwerveModuleState, NumModules> moduleStates) const {
   Eigen::Matrix<double, NumModules * 2, 1> moduleStatesMatrix;
 
-  for (size_t i = 0; i < NumModules; i++) {
+  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();
+    moduleStatesMatrix(i * 2, 0) = module.speed.value() * module.angle.Cos();
+    moduleStatesMatrix(i * 2 + 1, 0) =
+        module.speed.value() * module.angle.Sin();
   }
 
   Eigen::Vector3d chassisSpeedsVector =
@@ -93,7 +89,7 @@
 
 template <size_t NumModules>
 void SwerveDriveKinematics<NumModules>::NormalizeWheelSpeeds(
-    std::array<SwerveModuleState, NumModules>* moduleStates,
+    wpi::array<SwerveModuleState, NumModules>* moduleStates,
     units::meters_per_second_t attainableMaxSpeed) {
   auto& states = *moduleStates;
   auto realMaxSpeed = std::max_element(states.begin(), states.end(),
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
index 03591da..d1a4958 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
index e7bb093..96db930 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveDriveOdometry.inc
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include "frc/kinematics/SwerveDriveOdometry.h"
 #include "wpimath/MathShared.h"
 
 namespace frc {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
index b5ae7f1..cae2d53 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/kinematics/SwerveModuleState.h
@@ -1,20 +1,21 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/geometry/Rotation2d.h"
+#include "units/angle.h"
+#include "units/math.h"
 #include "units/velocity.h"
 
 namespace frc {
 /**
  * Represents the state of one swerve module.
  */
-struct SwerveModuleState {
+struct WPILIB_DLLEXPORT SwerveModuleState {
   /**
    * Speed of the wheel of the module.
    */
@@ -24,5 +25,24 @@
    * Angle of the module.
    */
   Rotation2d angle;
+
+  /**
+   * Minimize the change in heading the desired swerve module state would
+   * require by potentially reversing the direction the wheel spins. If this is
+   * used with the PIDController class's continuous input functionality, the
+   * furthest a wheel will ever rotate is 90 degrees.
+   *
+   * @param desiredState The desired state.
+   * @param currentAngle The current module angle.
+   */
+  static SwerveModuleState Optimize(const SwerveModuleState& desiredState,
+                                    const Rotation2d& currentAngle) {
+    auto delta = desiredState.angle - currentAngle;
+    if (units::math::abs(delta.Degrees()) > 90_deg) {
+      return {-desiredState.speed, desiredState.angle + Rotation2d{180_deg}};
+    } else {
+      return {desiredState.speed, desiredState.angle};
+    }
+  }
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
index c9cf2d0..8126349 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/CubicHermiteSpline.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
 
 #include "Eigen/Core"
 #include "frc/spline/Spline.h"
@@ -16,7 +14,7 @@
 /**
  * Represents a hermite spline of degree 3.
  */
-class CubicHermiteSpline : public Spline<3> {
+class WPILIB_DLLEXPORT CubicHermiteSpline : public Spline<3> {
  public:
   /**
    * Constructs a cubic hermite spline with the specified control vectors. Each
@@ -32,10 +30,10 @@
    * @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);
+  CubicHermiteSpline(wpi::array<double, 2> xInitialControlVector,
+                     wpi::array<double, 2> xFinalControlVector,
+                     wpi::array<double, 2> yInitialControlVector,
+                     wpi::array<double, 2> yFinalControlVector);
 
  protected:
   /**
@@ -55,13 +53,31 @@
    * @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
+    // Given P(i), P'(i), P(i+1), P'(i+1), the control vectors, we want to find
+    // the coefficients of the spline P(t) = a3 * t^3 + a2 * t^2 + a1 * t + a0.
+    //
+    // P(i)    = P(0)  = a0
+    // P'(i)   = P'(0) = a1
+    // P(i+1)  = P(1)  = a3 + a2 + a1 + a0
+    // P'(i+1) = P'(1) = 3 * a3 + 2 * a2 + a1
+    //
+    // [ P(i)    ] = [ 0 0 0 1 ][ a3 ]
+    // [ P'(i)   ] = [ 0 0 1 0 ][ a2 ]
+    // [ P(i+1)  ] = [ 1 1 1 1 ][ a1 ]
+    // [ P'(i+1) ] = [ 3 2 1 0 ][ a0 ]
+    //
+    // To solve for the coefficients, we can invert the 4x4 matrix and move it
+    // to the other side of the equation.
+    //
+    // [ a3 ] = [  2  1 -2  1 ][ P(i)    ]
+    // [ a2 ] = [ -3 -2  3 -1 ][ P'(i)   ]
+    // [ a1 ] = [  0  1  0  0 ][ P(i+1)  ]
+    // [ a0 ] = [  1  0  0  0 ][ P'(i+1) ]
+
+    static const Eigen::Matrix<double, 4, 4> basis{{+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}};
     return basis;
   }
 
@@ -75,10 +91,9 @@
    * @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();
+      wpi::array<double, 2> initialVector, wpi::array<double, 2> finalVector) {
+    return Eigen::Vector4d{initialVector[0], initialVector[1], finalVector[0],
+                           finalVector[1]};
   }
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
index 201c402..5ba3e2a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/QuinticHermiteSpline.h
@@ -1,13 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
 
 #include "Eigen/Core"
 #include "frc/spline/Spline.h"
@@ -16,7 +14,7 @@
 /**
  * Represents a hermite spline of degree 5.
  */
-class QuinticHermiteSpline : public Spline<5> {
+class WPILIB_DLLEXPORT QuinticHermiteSpline : public Spline<5> {
  public:
   /**
    * Constructs a quintic hermite spline with the specified control vectors.
@@ -32,10 +30,10 @@
    * @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);
+  QuinticHermiteSpline(wpi::array<double, 3> xInitialControlVector,
+                       wpi::array<double, 3> xFinalControlVector,
+                       wpi::array<double, 3> yInitialControlVector,
+                       wpi::array<double, 3> yFinalControlVector);
 
  protected:
   /**
@@ -55,15 +53,41 @@
    * @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
+    // Given P(i), P'(i), P''(i), P(i+1), P'(i+1), P''(i+1), the control
+    // vectors, we want to find the coefficients of the spline
+    // P(t) = a5 * t^5 + a4 * t^4 + a3 * t^3 + a2 * t^2 + a1 * t + a0.
+    //
+    // P(i)     = P(0)   = a0
+    // P'(i)    = P'(0)  = a1
+    // P''(i)   = P''(0) = 2 * a2
+    // P(i+1)   = P(1)   = a5 + a4 + a3 + a2 + a1 + a0
+    // P'(i+1)  = P'(1)  = 5 * a5 + 4 * a4 + 3 * a3 + 2 * a2 + a1
+    // P''(i+1) = P''(1) = 20 * a5 + 12 * a4 + 6 * a3 + 2 * a2
+    //
+    // [ P(i)     ] = [  0  0  0  0  0  1 ][ a5 ]
+    // [ P'(i)    ] = [  0  0  0  0  1  0 ][ a4 ]
+    // [ P''(i)   ] = [  0  0  0  2  0  0 ][ a3 ]
+    // [ P(i+1)   ] = [  1  1  1  1  1  1 ][ a2 ]
+    // [ P'(i+1)  ] = [  5  4  3  2  1  0 ][ a1 ]
+    // [ P''(i+1) ] = [ 20 12  6  2  0  0 ][ a0 ]
+    //
+    // To solve for the coefficients, we can invert the 6x6 matrix and move it
+    // to the other side of the equation.
+    //
+    // [ a5 ] = [  -6.0  -3.0  -0.5   6.0  -3.0   0.5 ][ P(i)     ]
+    // [ a4 ] = [  15.0   8.0   1.5 -15.0   7.0  -1.0 ][ P'(i)    ]
+    // [ a3 ] = [ -10.0  -6.0  -1.5  10.0  -4.0   0.5 ][ P''(i)   ]
+    // [ a2 ] = [   0.0   0.0   0.5   0.0   0.0   0.0 ][ P(i+1)   ]
+    // [ a1 ] = [   0.0   1.0   0.0   0.0   0.0   0.0 ][ P'(i+1)  ]
+    // [ a0 ] = [   1.0   0.0   0.0   0.0   0.0   0.0 ][ P''(i+1) ]
+
+    static const Eigen::Matrix<double, 6, 6> basis{
+        {-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}};
     return basis;
   }
 
@@ -76,11 +100,11 @@
    *
    * @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();
+  static Eigen::Vector<double, 6> ControlVectorFromArrays(
+      wpi::array<double, 3> initialVector, wpi::array<double, 3> finalVector) {
+    return Eigen::Vector<double, 6>{initialVector[0], initialVector[1],
+                                    initialVector[2], finalVector[0],
+                                    finalVector[1],   finalVector[2]};
   }
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h
index 2964476..2dd248a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/Spline.h
@@ -1,16 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
 #include <utility>
 #include <vector>
 
+#include <wpi/array.h>
+
 #include "Eigen/Core"
 #include "frc/geometry/Pose2d.h"
 #include "units/curvature.h"
@@ -46,8 +44,8 @@
    * dimension.
    */
   struct ControlVector {
-    std::array<double, (Degree + 1) / 2> x;
-    std::array<double, (Degree + 1) / 2> y;
+    wpi::array<double, (Degree + 1) / 2> x;
+    wpi::array<double, (Degree + 1) / 2> y;
   };
 
   /**
@@ -57,7 +55,7 @@
    * @return The pose and curvature at that point.
    */
   PoseWithCurvature GetPoint(double t) const {
-    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
+    Eigen::Vector<double, Degree + 1> polynomialBases;
 
     // Populate the polynomial bases
     for (int i = 0; i <= Degree; i++) {
@@ -66,7 +64,7 @@
 
     // 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;
+    Eigen::Vector<double, 6> combined = Coefficients() * polynomialBases;
 
     double dx, dy, ddx, ddy;
 
@@ -111,9 +109,7 @@
    * @return The vector.
    */
   static Eigen::Vector2d ToVector(const Translation2d& translation) {
-    return (Eigen::Vector2d() << translation.X().to<double>(),
-            translation.Y().to<double>())
-        .finished();
+    return Eigen::Vector2d{translation.X().value(), translation.Y().value()};
   }
 
   /**
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h
index e04fa45..90b6107 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineHelper.h
@@ -1,16 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <array>
 #include <utility>
 #include <vector>
 
+#include <wpi/SymbolExports.h>
+#include <wpi/array.h>
+
 #include "frc/spline/CubicHermiteSpline.h"
 #include "frc/spline/QuinticHermiteSpline.h"
 
@@ -19,7 +18,7 @@
  * Helper class that is used to generate cubic and quintic splines from user
  * provided waypoints.
  */
-class SplineHelper {
+class WPILIB_DLLEXPORT SplineHelper {
  public:
   /**
    * Returns 2 cubic control vectors from a set of exterior waypoints and
@@ -30,7 +29,7 @@
    * @param end               The ending pose.
    * @return 2 cubic control vectors.
    */
-  static std::array<Spline<3>::ControlVector, 2>
+  static wpi::array<Spline<3>::ControlVector, 2>
   CubicControlVectorsFromWaypoints(
       const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
       const Pose2d& end);
@@ -81,14 +80,14 @@
  private:
   static Spline<3>::ControlVector CubicControlVector(double scalar,
                                                      const Pose2d& point) {
-    return {{point.X().to<double>(), scalar * point.Rotation().Cos()},
-            {point.Y().to<double>(), scalar * point.Rotation().Sin()}};
+    return {{point.X().value(), scalar * point.Rotation().Cos()},
+            {point.Y().value(), scalar * point.Rotation().Sin()}};
   }
 
   static Spline<5>::ControlVector QuinticControlVector(double scalar,
                                                        const Pose2d& point) {
-    return {{point.X().to<double>(), scalar * point.Rotation().Cos(), 0.0},
-            {point.Y().to<double>(), scalar * point.Rotation().Sin(), 0.0}};
+    return {{point.X().value(), scalar * point.Rotation().Cos(), 0.0},
+            {point.Y().value(), scalar * point.Rotation().Sin(), 0.0}};
   }
 
   /**
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
index 8e7079c..0720cd1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/spline/SplineParameterizer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
  * MIT License
@@ -36,7 +33,7 @@
 #include <utility>
 #include <vector>
 
-#include <wpi/Twine.h>
+#include <wpi/SymbolExports.h>
 
 #include "frc/spline/Spline.h"
 #include "units/angle.h"
@@ -49,7 +46,7 @@
 /**
  * Class used to parameterize a spline by its arc length.
  */
-class SplineParameterizer {
+class WPILIB_DLLEXPORT SplineParameterizer {
  public:
   using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/Discretization.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/Discretization.h
index 72d0226..722bb5f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/Discretization.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/Discretization.h
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include "Eigen/Core"
-#include "Eigen/src/LU/PartialPivLU.h"
 #include "units/time.h"
-#include "unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h"
+#include "unsupported/Eigen/MatrixFunctions"
 
 namespace frc {
 
 /**
  * Discretizes the given continuous A matrix.
  *
+ * @tparam States Number of states.
  * @param contA Continuous system matrix.
  * @param dt    Discretization timestep.
  * @param discA Storage for discrete system matrix.
@@ -25,12 +22,14 @@
 void DiscretizeA(const Eigen::Matrix<double, States, States>& contA,
                  units::second_t dt,
                  Eigen::Matrix<double, States, States>* discA) {
-  *discA = (contA * dt.to<double>()).exp();
+  *discA = (contA * dt.value()).exp();
 }
 
 /**
  * Discretizes the given continuous A and B matrices.
  *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
  * @param contA Continuous system matrix.
  * @param contB Continuous input matrix.
  * @param dt    Discretization timestep.
@@ -46,8 +45,8 @@
   // Matrices are blocked here to minimize matrix exponentiation calculations
   Eigen::Matrix<double, States + Inputs, States + Inputs> Mcont;
   Mcont.setZero();
-  Mcont.template block<States, States>(0, 0) = contA * dt.to<double>();
-  Mcont.template block<States, Inputs>(0, States) = contB * dt.to<double>();
+  Mcont.template block<States, States>(0, 0) = contA * dt.value();
+  Mcont.template block<States, Inputs>(0, States) = contB * dt.value();
 
   // Discretize A and B with the given timestep
   Eigen::Matrix<double, States + Inputs, States + Inputs> Mdisc = Mcont.exp();
@@ -58,6 +57,7 @@
 /**
  * Discretizes the given continuous A and Q matrices.
  *
+ * @tparam States Number of states.
  * @param contA Continuous system matrix.
  * @param contQ Continuous process noise covariance matrix.
  * @param dt    Discretization timestep.
@@ -80,8 +80,7 @@
   M.template block<States, States>(States, 0).setZero();
   M.template block<States, States>(States, States) = contA.transpose();
 
-  Eigen::Matrix<double, 2 * States, 2 * States> phi =
-      (M * dt.to<double>()).exp();
+  Eigen::Matrix<double, 2 * States, 2 * States> phi = (M * dt.value()).exp();
 
   // Phi12 = phi[0:States,        States:2*States]
   // Phi22 = phi[States:2*States, States:2*States]
@@ -110,6 +109,7 @@
  *    using a taylor series to several terms and still be substantially cheaper
  *    than taking the big exponential.
  *
+ * @tparam States Number of states.
  * @param contA Continuous system matrix.
  * @param contQ Continuous process noise covariance matrix.
  * @param dt    Discretization timestep.
@@ -126,9 +126,9 @@
   Eigen::Matrix<double, States, States> Q = (contQ + contQ.transpose()) / 2.0;
 
   Eigen::Matrix<double, States, States> lastTerm = Q;
-  double lastCoeff = dt.to<double>();
+  double lastCoeff = dt.value();
 
-  // A^T^n
+  // Aᵀⁿ
   Eigen::Matrix<double, States, States> Atn = contA.transpose();
 
   Eigen::Matrix<double, States, States> phi12 = lastTerm * lastCoeff;
@@ -136,7 +136,7 @@
   // i = 6 i.e. 5th order should be enough precision
   for (int i = 2; i < 6; ++i) {
     lastTerm = -contA * lastTerm + Q * Atn;
-    lastCoeff *= dt.to<double>() / static_cast<double>(i);
+    lastCoeff *= dt.value() / static_cast<double>(i);
 
     phi12 += lastTerm * lastCoeff;
 
@@ -154,13 +154,14 @@
  * Returns a discretized version of the provided continuous measurement noise
  * covariance matrix.
  *
+ * @tparam Outputs Number of outputs.
  * @param R  Continuous measurement noise covariance matrix.
  * @param dt Discretization timestep.
  */
 template <int Outputs>
 Eigen::Matrix<double, Outputs, Outputs> DiscretizeR(
     const Eigen::Matrix<double, Outputs, Outputs>& R, units::second_t dt) {
-  return R / dt.to<double>();
+  return R / dt.value();
 }
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystem.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystem.h
index 975fa0e..bd3ff40 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystem.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystem.h
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <algorithm>
 #include <functional>
+#include <stdexcept>
 
 #include "Eigen/Core"
 #include "frc/StateSpaceUtil.h"
@@ -24,6 +22,10 @@
  *
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
  */
 template <int States, int Inputs, int Outputs>
 class LinearSystem {
@@ -35,11 +37,33 @@
    * @param B    Input matrix.
    * @param C    Output matrix.
    * @param D    Feedthrough matrix.
+   * @throws std::domain_error if any matrix element isn't finite.
    */
   LinearSystem(const Eigen::Matrix<double, States, States>& A,
                const Eigen::Matrix<double, States, Inputs>& B,
                const Eigen::Matrix<double, Outputs, States>& C,
                const Eigen::Matrix<double, Outputs, Inputs>& D) {
+    if (!A.allFinite()) {
+      throw std::domain_error(
+          "Elements of A aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+    if (!B.allFinite()) {
+      throw std::domain_error(
+          "Elements of B aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+    if (!C.allFinite()) {
+      throw std::domain_error(
+          "Elements of C aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+    if (!D.allFinite()) {
+      throw std::domain_error(
+          "Elements of D aren't finite. This is usually due to model "
+          "implementation errors.");
+    }
+
     m_A = A;
     m_B = B;
     m_C = C;
@@ -109,14 +133,13 @@
    * This is used by state observers directly to run updates based on state
    * estimate.
    *
-   * @param x  The current state.
-   * @param u  The control input.
-   * @param dt Timestep for model update.
+   * @param x        The current state.
+   * @param clampedU The control input.
+   * @param dt       Timestep for model update.
    */
-  Eigen::Matrix<double, States, 1> CalculateX(
-      const Eigen::Matrix<double, States, 1>& x,
-      const Eigen::Matrix<double, Inputs, 1>& clampedU,
-      units::second_t dt) const {
+  Eigen::Vector<double, States> CalculateX(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, Inputs>& clampedU, units::second_t dt) const {
     Eigen::Matrix<double, States, States> discA;
     Eigen::Matrix<double, States, Inputs> discB;
     DiscretizeAB<States, Inputs>(m_A, m_B, dt, &discA, &discB);
@@ -133,9 +156,9 @@
    * @param x The current state.
    * @param clampedU The control input.
    */
-  Eigen::Matrix<double, Outputs, 1> CalculateY(
-      const Eigen::Matrix<double, States, 1>& x,
-      const Eigen::Matrix<double, Inputs, 1>& clampedU) const {
+  Eigen::Vector<double, Outputs> CalculateY(
+      const Eigen::Vector<double, States>& x,
+      const Eigen::Vector<double, Inputs>& clampedU) const {
     return m_C * x + m_D * clampedU;
   }
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
index d5f25fb..9ee2ea2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/LinearSystemLoop.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -18,8 +15,8 @@
 namespace frc {
 
 /**
- * Combines a plant, controller, and observer for controlling a mechanism with
- * full state feedback.
+ * Combines a controller, feedforward, and observer for controlling a mechanism
+ * with full state feedback.
  *
  * For everything in this file, "inputs" and "outputs" are defined from the
  * perspective of the plant. This means U is an input and Y is an output
@@ -30,6 +27,10 @@
  *
  * For more on the underlying math, read
  * https://file.tavsys.net/control/controls-engineering-in-frc.pdf.
+ *
+ * @tparam States Number of states.
+ * @tparam Inputs Number of inputs.
+ * @tparam Outputs Number of outputs.
  */
 template <int States, int Inputs, int Outputs>
 class LinearSystemLoop {
@@ -52,9 +53,8 @@
                    units::volt_t maxVoltage, units::second_t dt)
       : LinearSystemLoop(
             plant, controller, observer,
-            [=](Eigen::Matrix<double, Inputs, 1> u) {
-              return frc::NormalizeInputVector<Inputs>(
-                  u, maxVoltage.template to<double>());
+            [=](const Eigen::Vector<double, Inputs>& u) {
+              return frc::NormalizeInputVector<Inputs>(u, maxVoltage.value());
             },
             dt) {}
 
@@ -73,21 +73,20 @@
   LinearSystemLoop(LinearSystem<States, Inputs, Outputs>& plant,
                    LinearQuadraticRegulator<States, Inputs>& controller,
                    KalmanFilter<States, Inputs, Outputs>& observer,
-                   std::function<Eigen::Matrix<double, Inputs, 1>(
-                       const Eigen::Matrix<double, Inputs, 1>&)>
+                   std::function<Eigen::Vector<double, Inputs>(
+                       const Eigen::Vector<double, Inputs>&)>
                        clampFunction,
                    units::second_t dt)
       : LinearSystemLoop(
-            plant, controller,
+            controller,
             LinearPlantInversionFeedforward<States, Inputs>{plant, dt},
             observer, clampFunction) {}
 
   /**
-   * Constructs a state-space loop with the given plant, controller, and
+   * Constructs a state-space loop with the given controller, feedforward and
    * observer. By default, the initial reference is all zeros. Users should
-   * call reset with the initial system state before enabling the loop.
+   * call reset with the initial system state.
    *
-   * @param plant       State-space plant.
    * @param controller  State-space controller.
    * @param feedforward Plant inversion feedforward.
    * @param observer    State-space observer.
@@ -95,48 +94,48 @@
    * that the inputs are voltages.
    */
   LinearSystemLoop(
-      LinearSystem<States, Inputs, Outputs>& plant,
       LinearQuadraticRegulator<States, Inputs>& controller,
       const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
       KalmanFilter<States, Inputs, Outputs>& observer, units::volt_t maxVoltage)
-      : LinearSystemLoop(plant, controller, feedforward, observer,
-                         [=](Eigen::Matrix<double, Inputs, 1> u) {
+      : LinearSystemLoop(controller, feedforward, observer,
+                         [=](const Eigen::Vector<double, Inputs>& u) {
                            return frc::NormalizeInputVector<Inputs>(
-                               u, maxVoltage.template to<double>());
+                               u, maxVoltage.value());
                          }) {}
 
   /**
-   * Constructs a state-space loop with the given plant, controller, and
-   * observer.
+   * Constructs a state-space loop with the given controller, feedforward,
+   * observer and clamp function. By default, the initial reference is all
+   * zeros. Users should call reset with the initial system state.
    *
-   * @param plant         State-space plant.
    * @param controller    State-space controller.
    * @param feedforward   Plant-inversion feedforward.
    * @param observer      State-space observer.
    * @param clampFunction The function used to clamp the input vector.
    */
   LinearSystemLoop(
-      LinearSystem<States, Inputs, Outputs>& plant,
       LinearQuadraticRegulator<States, Inputs>& controller,
       const LinearPlantInversionFeedforward<States, Inputs>& feedforward,
       KalmanFilter<States, Inputs, Outputs>& observer,
-      std::function<Eigen::Matrix<double, Inputs, 1>(
-          const Eigen::Matrix<double, Inputs, 1>&)>
+      std::function<
+          Eigen::Vector<double, Inputs>(const Eigen::Vector<double, Inputs>&)>
           clampFunction)
-      : m_plant(plant),
-        m_controller(controller),
+      : m_controller(&controller),
         m_feedforward(feedforward),
-        m_observer(observer),
+        m_observer(&observer),
         m_clampFunc(clampFunction) {
     m_nextR.setZero();
     Reset(m_nextR);
   }
 
+  LinearSystemLoop(LinearSystemLoop&&) = default;
+  LinearSystemLoop& operator=(LinearSystemLoop&&) = default;
+
   /**
    * Returns the observer's state estimate x-hat.
    */
-  const Eigen::Matrix<double, States, 1>& Xhat() const {
-    return m_observer.Xhat();
+  const Eigen::Vector<double, States>& Xhat() const {
+    return m_observer->Xhat();
   }
 
   /**
@@ -144,12 +143,12 @@
    *
    * @param i Row of x-hat.
    */
-  double Xhat(int i) const { return m_observer.Xhat(i); }
+  double Xhat(int i) const { return m_observer->Xhat(i); }
 
   /**
    * Returns the controller's next reference r.
    */
-  const Eigen::Matrix<double, States, 1>& NextR() const { return m_nextR; }
+  const Eigen::Vector<double, States>& NextR() const { return m_nextR; }
 
   /**
    * Returns an element of the controller's next reference r.
@@ -161,8 +160,8 @@
   /**
    * Returns the controller's calculated control input u.
    */
-  Eigen::Matrix<double, Inputs, 1> U() const {
-    return ClampInput(m_controller.U() + m_feedforward.Uff());
+  Eigen::Vector<double, Inputs> U() const {
+    return ClampInput(m_controller->U() + m_feedforward.Uff());
   }
 
   /**
@@ -177,8 +176,8 @@
    *
    * @param xHat The initial state estimate x-hat.
    */
-  void SetXhat(const Eigen::Matrix<double, States, 1>& xHat) {
-    m_observer.SetXhat(xHat);
+  void SetXhat(const Eigen::Vector<double, States>& xHat) {
+    m_observer->SetXhat(xHat);
   }
 
   /**
@@ -187,27 +186,20 @@
    * @param i     Row of x-hat.
    * @param value Value for element of x-hat.
    */
-  void SetXhat(int i, double value) { m_observer.SetXhat(i, value); }
+  void SetXhat(int i, double value) { m_observer->SetXhat(i, value); }
 
   /**
    * Set the next reference r.
    *
    * @param nextR Next reference.
    */
-  void SetNextR(const Eigen::Matrix<double, States, 1>& nextR) {
-    m_nextR = nextR;
-  }
-
-  /**
-   * Return the plant used internally.
-   */
-  const LinearSystem<States, Inputs, Outputs>& Plant() const { return m_plant; }
+  void SetNextR(const Eigen::Vector<double, States>& nextR) { m_nextR = nextR; }
 
   /**
    * Return the controller used internally.
    */
   const LinearQuadraticRegulator<States, Inputs>& Controller() const {
-    return m_controller;
+    return *m_controller;
   }
 
   /**
@@ -233,18 +225,18 @@
    *
    * @param initialState The initial state.
    */
-  void Reset(Eigen::Matrix<double, States, 1> initialState) {
+  void Reset(const Eigen::Vector<double, States>& initialState) {
     m_nextR.setZero();
-    m_controller.Reset();
+    m_controller->Reset();
     m_feedforward.Reset(initialState);
-    m_observer.SetXhat(initialState);
+    m_observer->SetXhat(initialState);
   }
 
   /**
    * Returns difference between reference r and current state x-hat.
    */
-  const Eigen::Matrix<double, States, 1> Error() const {
-    return m_controller.R() - m_observer.Xhat();
+  Eigen::Vector<double, States> Error() const {
+    return m_controller->R() - m_observer->Xhat();
   }
 
   /**
@@ -252,8 +244,8 @@
    *
    * @param y Measurement vector.
    */
-  void Correct(const Eigen::Matrix<double, Outputs, 1>& y) {
-    m_observer.Correct(U(), y);
+  void Correct(const Eigen::Vector<double, Outputs>& y) {
+    m_observer->Correct(U(), y);
   }
 
   /**
@@ -266,10 +258,10 @@
    * @param dt Timestep for model update.
    */
   void Predict(units::second_t dt) {
-    Eigen::Matrix<double, Inputs, 1> u =
-        ClampInput(m_controller.Calculate(m_observer.Xhat(), m_nextR) +
+    Eigen::Vector<double, Inputs> u =
+        ClampInput(m_controller->Calculate(m_observer->Xhat(), m_nextR) +
                    m_feedforward.Calculate(m_nextR));
-    m_observer.Predict(u, dt);
+    m_observer->Predict(u, dt);
   }
 
   /**
@@ -278,26 +270,25 @@
    * @param u Input vector to clamp.
    * @return Clamped input vector.
    */
-  Eigen::Matrix<double, Inputs, 1> ClampInput(
-      const Eigen::Matrix<double, Inputs, 1>& u) const {
+  Eigen::Vector<double, Inputs> ClampInput(
+      const Eigen::Vector<double, Inputs>& u) const {
     return m_clampFunc(u);
   }
 
  protected:
-  LinearSystem<States, Inputs, Outputs>& m_plant;
-  LinearQuadraticRegulator<States, Inputs>& m_controller;
+  LinearQuadraticRegulator<States, Inputs>* m_controller;
   LinearPlantInversionFeedforward<States, Inputs> m_feedforward;
-  KalmanFilter<States, Inputs, Outputs>& m_observer;
+  KalmanFilter<States, Inputs, Outputs>* m_observer;
 
   /**
    * Clamping function.
    */
-  std::function<Eigen::Matrix<double, Inputs, 1>(
-      const Eigen::Matrix<double, Inputs, 1>&)>
+  std::function<Eigen::Vector<double, Inputs>(
+      const Eigen::Vector<double, Inputs>&)>
       m_clampFunc;
 
   // Reference to go to in the next cycle (used by feedforward controller).
-  Eigen::Matrix<double, States, 1> m_nextR;
+  Eigen::Vector<double, States> m_nextR;
 
   // These are accessible from non-templated subclasses.
   static constexpr int kStates = States;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalIntegration.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
new file mode 100644
index 0000000..68d047f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalIntegration.h
@@ -0,0 +1,209 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <frc/StateSpaceUtil.h>
+
+#include <algorithm>
+#include <array>
+
+#include "Eigen/Core"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
+ *
+ * @param f  The function to integrate. It must take one argument x.
+ * @param x  The initial value of x.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RK4(F&& f, T x, units::second_t dt) {
+  const auto h = dt.value();
+
+  T k1 = f(x);
+  T k2 = f(x + h * 0.5 * k1);
+  T k3 = f(x + h * 0.5 * k2);
+  T k4 = f(x + h * k3);
+
+  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+/**
+ * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments x and u.
+ * @param x  The initial value of x.
+ * @param u  The value u held constant over the integration period.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T, typename U>
+T RK4(F&& f, T x, U u, units::second_t dt) {
+  const auto h = dt.value();
+
+  T k1 = f(x, u);
+  T k2 = f(x + h * 0.5 * k1, u);
+  T k3 = f(x + h * 0.5 * k2, u);
+  T k4 = f(x + h * k3, u);
+
+  return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+/**
+ * Performs adaptive RKF45 integration of dx/dt = f(x, u) for dt, as described
+ * in https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+ *
+ * @param f        The function to integrate. It must take two arguments x and
+ *                 u.
+ * @param x        The initial value of x.
+ * @param u        The value u held constant over the integration period.
+ * @param dt       The time over which to integrate.
+ * @param maxError The maximum acceptable truncation error. Usually a small
+ *                 number like 1e-6.
+ */
+template <typename F, typename T, typename U>
+T RKF45(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
+  // See
+  // https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta%E2%80%93Fehlberg_method
+  // for the Butcher tableau the following arrays came from.
+  constexpr int kDim = 6;
+
+  // clang-format off
+  constexpr double A[kDim - 1][kDim - 1]{
+      {     1.0 / 4.0},
+      {     3.0 / 32.0,       9.0 / 32.0},
+      {1932.0 / 2197.0, -7200.0 / 2197.0,  7296.0 / 2197.0},
+      {  439.0 / 216.0,             -8.0,   3680.0 / 513.0, -845.0 / 4104.0},
+      {    -8.0 / 27.0,              2.0, -3544.0 / 2565.0, 1859.0 / 4104.0, -11.0 / 40.0}};
+  // clang-format on
+
+  constexpr std::array<double, kDim> b1{16.0 / 135.0,     0.0,
+                                        6656.0 / 12825.0, 28561.0 / 56430.0,
+                                        -9.0 / 50.0,      2.0 / 55.0};
+  constexpr std::array<double, kDim> b2{
+      25.0 / 216.0, 0.0, 1408.0 / 2565.0, 2197.0 / 4104.0, -1.0 / 5.0, 0.0};
+
+  T newX;
+  double truncationError;
+
+  double dtElapsed = 0.0;
+  double h = dt.value();
+
+  // Loop until we've gotten to our desired dt
+  while (dtElapsed < dt.value()) {
+    do {
+      // Only allow us to advance up to the dt remaining
+      h = std::min(h, dt.value() - dtElapsed);
+
+      // Notice how the derivative in the Wikipedia notation is dy/dx.
+      // That means their y is our x and their x is our t
+      // clang-format off
+      T k1 = f(x, u);
+      T k2 = f(x + h * (A[0][0] * k1), u);
+      T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
+      T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
+      T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
+      T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
+      // clang-format on
+
+      newX = x + h * (b1[0] * k1 + b1[1] * k2 + b1[2] * k3 + b1[3] * k4 +
+                      b1[4] * k5 + b1[5] * k6);
+      truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
+                              (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
+                              (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6))
+                            .norm();
+
+      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+    } while (truncationError > maxError);
+
+    dtElapsed += h;
+    x = newX;
+  }
+
+  return x;
+}
+
+/**
+ * Performs adaptive Dormand-Prince integration of dx/dt = f(x, u) for dt.
+ *
+ * @param f        The function to integrate. It must take two arguments x and
+ *                 u.
+ * @param x        The initial value of x.
+ * @param u        The value u held constant over the integration period.
+ * @param dt       The time over which to integrate.
+ * @param maxError The maximum acceptable truncation error. Usually a small
+ *                 number like 1e-6.
+ */
+template <typename F, typename T, typename U>
+T RKDP(F&& f, T x, U u, units::second_t dt, double maxError = 1e-6) {
+  // See https://en.wikipedia.org/wiki/Dormand%E2%80%93Prince_method for the
+  // Butcher tableau the following arrays came from.
+
+  constexpr int kDim = 7;
+
+  // clang-format off
+  constexpr double A[kDim - 1][kDim - 1]{
+      {      1.0 / 5.0},
+      {      3.0 / 40.0,        9.0 / 40.0},
+      {     44.0 / 45.0,      -56.0 / 15.0,       32.0 / 9.0},
+      {19372.0 / 6561.0, -25360.0 / 2187.0, 64448.0 / 6561.0, -212.0 / 729.0},
+      { 9017.0 / 3168.0,     -355.0 / 33.0, 46732.0 / 5247.0,   49.0 / 176.0, -5103.0 / 18656.0},
+      {    35.0 / 384.0,               0.0,   500.0 / 1113.0,  125.0 / 192.0,  -2187.0 / 6784.0, 11.0 / 84.0}};
+  // clang-format on
+
+  constexpr std::array<double, kDim> b1{
+      35.0 / 384.0, 0.0, 500.0 / 1113.0, 125.0 / 192.0, -2187.0 / 6784.0,
+      11.0 / 84.0,  0.0};
+  constexpr std::array<double, kDim> b2{5179.0 / 57600.0,    0.0,
+                                        7571.0 / 16695.0,    393.0 / 640.0,
+                                        -92097.0 / 339200.0, 187.0 / 2100.0,
+                                        1.0 / 40.0};
+
+  T newX;
+  double truncationError;
+
+  double dtElapsed = 0.0;
+  double h = dt.value();
+
+  // Loop until we've gotten to our desired dt
+  while (dtElapsed < dt.value()) {
+    do {
+      // Only allow us to advance up to the dt remaining
+      h = std::min(h, dt.value() - dtElapsed);
+
+      // clang-format off
+      T k1 = f(x, u);
+      T k2 = f(x + h * (A[0][0] * k1), u);
+      T k3 = f(x + h * (A[1][0] * k1 + A[1][1] * k2), u);
+      T k4 = f(x + h * (A[2][0] * k1 + A[2][1] * k2 + A[2][2] * k3), u);
+      T k5 = f(x + h * (A[3][0] * k1 + A[3][1] * k2 + A[3][2] * k3 + A[3][3] * k4), u);
+      T k6 = f(x + h * (A[4][0] * k1 + A[4][1] * k2 + A[4][2] * k3 + A[4][3] * k4 + A[4][4] * k5), u);
+      // clang-format on
+
+      // Since the final row of A and the array b1 have the same coefficients
+      // and k7 has no effect on newX, we can reuse the calculation.
+      newX = x + h * (A[5][0] * k1 + A[5][1] * k2 + A[5][2] * k3 +
+                      A[5][3] * k4 + A[5][4] * k5 + A[5][5] * k6);
+      T k7 = f(newX, u);
+
+      truncationError = (h * ((b1[0] - b2[0]) * k1 + (b1[1] - b2[1]) * k2 +
+                              (b1[2] - b2[2]) * k3 + (b1[3] - b2[3]) * k4 +
+                              (b1[4] - b2[4]) * k5 + (b1[5] - b2[5]) * k6 +
+                              (b1[6] - b2[6]) * k7))
+                            .norm();
+
+      h *= 0.9 * std::pow(maxError / truncationError, 1.0 / 5.0);
+    } while (truncationError > maxError);
+
+    dtElapsed += h;
+    x = newX;
+  }
+
+  return x;
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalJacobian.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
index cbd6943..5f1bc78 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/NumericalJacobian.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -20,16 +17,16 @@
  * @param x     Vector argument.
  */
 template <int Rows, int Cols, typename F>
-auto NumericalJacobian(F&& f, const Eigen::Matrix<double, Cols, 1>& x) {
+auto NumericalJacobian(F&& f, const Eigen::Vector<double, Cols>& x) {
   constexpr double kEpsilon = 1e-5;
   Eigen::Matrix<double, Rows, Cols> result;
   result.setZero();
 
   // It's more expensive, but +- epsilon will be more accurate
   for (int i = 0; i < Cols; ++i) {
-    Eigen::Matrix<double, Cols, 1> dX_plus = x;
+    Eigen::Vector<double, Cols> dX_plus = x;
     dX_plus(i) += kEpsilon;
-    Eigen::Matrix<double, Cols, 1> dX_minus = x;
+    Eigen::Vector<double, Cols> dX_minus = x;
     dX_minus(i) -= kEpsilon;
     result.col(i) = (f(dX_plus) - f(dX_minus)) / (kEpsilon * 2.0);
   }
@@ -44,17 +41,19 @@
  * @tparam States  Number of rows in x.
  * @tparam Inputs  Number of rows in u.
  * @tparam F       Function object type.
- * @tparam Args... Remaining arguments to f(x, u, ...).
+ * @tparam Args... Types of remaining arguments to f(x, u, ...).
  * @param f        Vector-valued function from which to compute Jacobian.
  * @param x        State vector.
  * @param u        Input vector.
+ * @param args     Remaining arguments to f(x, u, ...).
  */
 template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianX(F&& f, const Eigen::Matrix<double, States, 1>& x,
-                        const Eigen::Matrix<double, Inputs, 1>& u,
+auto NumericalJacobianX(F&& f, const Eigen::Vector<double, States>& x,
+                        const Eigen::Vector<double, Inputs>& u,
                         Args&&... args) {
   return NumericalJacobian<Rows, States>(
-      [&](Eigen::Matrix<double, States, 1> x) { return f(x, u, args...); }, x);
+      [&](const Eigen::Vector<double, States>& x) { return f(x, u, args...); },
+      x);
 }
 
 /**
@@ -64,17 +63,19 @@
  * @tparam States  Number of rows in x.
  * @tparam Inputs  Number of rows in u.
  * @tparam F       Function object type.
- * @tparam Args... Remaining arguments to f(x, u, ...).
+ * @tparam Args... Types of remaining arguments to f(x, u, ...).
  * @param f        Vector-valued function from which to compute Jacobian.
  * @param x        State vector.
  * @param u        Input vector.
+ * @param args     Remaining arguments to f(x, u, ...).
  */
 template <int Rows, int States, int Inputs, typename F, typename... Args>
-auto NumericalJacobianU(F&& f, const Eigen::Matrix<double, States, 1>& x,
-                        const Eigen::Matrix<double, Inputs, 1>& u,
+auto NumericalJacobianU(F&& f, const Eigen::Vector<double, States>& x,
+                        const Eigen::Vector<double, Inputs>& u,
                         Args&&... args) {
   return NumericalJacobian<Rows, Inputs>(
-      [&](Eigen::Matrix<double, Inputs, 1> u) { return f(x, u, args...); }, u);
+      [&](const Eigen::Vector<double, Inputs>& u) { return f(x, u, args...); },
+      u);
 }
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/RungeKutta.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/RungeKutta.h
deleted file mode 100644
index a83cafc..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/RungeKutta.h
+++ /dev/null
@@ -1,69 +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 "Eigen/Core"
-#include "units/time.h"
-
-namespace frc {
-
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(x) for dt.
- *
- * @param f  The function to integrate. It must take one argument x.
- * @param x  The initial value of x.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T>
-T RungeKutta(F&& f, T x, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
-  T k1 = f(x);
-  T k2 = f(x + k1 * halfDt.to<double>());
-  T k3 = f(x + k2 * halfDt.to<double>());
-  T k4 = f(x + k3 * dt.to<double>());
-  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
- *
- * @param f  The function to integrate. It must take two arguments x and u.
- * @param x  The initial value of x.
- * @param u  The value u held constant over the integration period.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T, typename U>
-T RungeKutta(F&& f, T x, U u, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
-  T k1 = f(x, u);
-  T k2 = f(x + k1 * halfDt.to<double>(), u);
-  T k3 = f(x + k2 * halfDt.to<double>(), u);
-  T k4 = f(x + k3 * dt.to<double>(), u);
-  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-/**
- * Performs 4th order Runge-Kutta integration of dx/dt = f(t, x) for dt.
- *
- * @param f  The function to integrate. It must take two arguments x and t.
- * @param x  The initial value of x.
- * @param t  The initial value of t.
- * @param dt The time over which to integrate.
- */
-template <typename F, typename T>
-T RungeKuttaTimeVarying(F&& f, T x, units::second_t t, units::second_t dt) {
-  const auto halfDt = 0.5 * dt;
-  T k1 = f(t, x);
-  T k2 = f(t + halfDt, x + k1 / 2.0);
-  T k3 = f(t + halfDt, x + k2 / 2.0);
-  T k4 = f(t + dt, x + k3);
-
-  return x + dt.to<double>() / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
-}
-
-}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
index d2a2ba8..a519b0e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/DCMotor.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "units/angular_velocity.h"
 #include "units/current.h"
 #include "units/impedance.h"
@@ -18,7 +17,7 @@
 /**
  * Holds the constants for a DC motor.
  */
-class DCMotor {
+class WPILIB_DLLEXPORT DCMotor {
  public:
   using radians_per_second_per_volt_t =
       units::unit_t<units::compound_unit<units::radians_per_second,
@@ -46,7 +45,7 @@
    * Constructs a DC motor.
    *
    * @param nominalVoltage Voltage at which the motor constants were measured.
-   * @param stallTorque    Current draw when stalled.
+   * @param stallTorque    Torque when stalled.
    * @param stallCurrent   Current draw when stalled.
    * @param freeCurrent    Current draw under no load.
    * @param freeSpeed      Angular velocity under no load.
@@ -58,12 +57,12 @@
                     units::radians_per_second_t freeSpeed, int numMotors = 1)
       : nominalVoltage(nominalVoltage),
         stallTorque(stallTorque * numMotors),
-        stallCurrent(stallCurrent),
-        freeCurrent(freeCurrent),
+        stallCurrent(stallCurrent * numMotors),
+        freeCurrent(freeCurrent * numMotors),
         freeSpeed(freeSpeed),
-        R(nominalVoltage / stallCurrent),
-        Kv(freeSpeed / (nominalVoltage - R * freeCurrent)),
-        Kt(stallTorque * numMotors / stallCurrent) {}
+        R(nominalVoltage / this->stallCurrent),
+        Kv(freeSpeed / (nominalVoltage - R * this->freeCurrent)),
+        Kt(this->stallTorque / this->stallCurrent) {}
 
   /**
    * Returns current drawn by motor with given speed and input voltage.
@@ -152,6 +151,14 @@
   static constexpr DCMotor Falcon500(int numMotors = 1) {
     return DCMotor(12_V, 4.69_Nm, 257_A, 1.5_A, 6380_rpm, numMotors);
   }
+
+  /**
+   * Return a gearbox of Romi/TI_RSLK MAX motors.
+   */
+  static constexpr DCMotor RomiBuiltIn(int numMotors = 1) {
+    // From https://www.pololu.com/product/1520/specs
+    return DCMotor(4.5_V, 0.1765_Nm, 1.25_A, 0.13_A, 150_rpm, numMotors);
+  }
 };
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
index b712460..c0f4506 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/system/plant/LinearSystemId.h
@@ -1,24 +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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include "frc/StateSpaceUtil.h"
+#include <stdexcept>
+
+#include <wpi/SymbolExports.h>
+
 #include "frc/system/LinearSystem.h"
 #include "frc/system/plant/DCMotor.h"
 #include "units/acceleration.h"
 #include "units/angular_acceleration.h"
 #include "units/angular_velocity.h"
+#include "units/length.h"
 #include "units/moment_of_inertia.h"
 #include "units/velocity.h"
 #include "units/voltage.h"
 
 namespace frc {
-class LinearSystemId {
+class WPILIB_DLLEXPORT LinearSystemId {
  public:
   template <typename Distance>
   using Velocity_t = units::unit_t<
@@ -40,19 +41,30 @@
    * @param m Carriage mass.
    * @param r Pulley radius.
    * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if m <= 0, r <= 0, or G <= 0.
    */
   static LinearSystem<2, 1, 1> ElevatorSystem(DCMotor motor,
                                               units::kilogram_t m,
                                               units::meter_t r, double G) {
-    auto A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0,
-        (-std::pow(G, 2) * motor.Kt /
-         (motor.R * units::math::pow<2>(r) * m * motor.Kv))
-            .to<double>());
-    auto B = frc::MakeMatrix<2, 1>(
-        0.0, (G * motor.Kt / (motor.R * r * m)).to<double>());
-    auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    auto D = frc::MakeMatrix<1, 1>(0.0);
+    if (m <= 0_kg) {
+      throw std::domain_error("m must be greater than zero.");
+    }
+    if (r <= 0_m) {
+      throw std::domain_error("r must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0, (-std::pow(G, 2) * motor.Kt /
+               (motor.R * units::math::pow<2>(r) * m * motor.Kv))
+                  .value()}};
+    Eigen::Matrix<double, 2, 1> B{0.0,
+                                  (G * motor.Kt / (motor.R * r * m)).value()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
@@ -67,16 +79,23 @@
    * @param motor Instance of DCMotor.
    * @param J Moment of inertia.
    * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<2, 1, 1> SingleJointedArmSystem(
       DCMotor motor, units::kilogram_square_meter_t J, double G) {
-    auto A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0,
-        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
-    auto B =
-        frc::MakeMatrix<2, 1>(0.0, (G * motor.Kt / (motor.R * J)).to<double>());
-    auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    auto D = frc::MakeMatrix<1, 1>(0.0);
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
+    Eigen::Matrix<double, 2, 2> A{
+        {0.0, 1.0},
+        {0.0, (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()}};
+    Eigen::Matrix<double, 2, 1> B{0.0, (G * motor.Kt / (motor.R * J)).value()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
@@ -100,6 +119,7 @@
    *
    * @param kV The velocity gain, in volt seconds per distance.
    * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
   template <typename Distance, typename = std::enable_if_t<
                                    std::is_same_v<units::meter, Distance> ||
@@ -107,11 +127,17 @@
   static LinearSystem<1, 1, 1> IdentifyVelocitySystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    auto A = frc::MakeMatrix<1, 1>(-kV.template to<double>() /
-                                   kA.template to<double>());
-    auto B = frc::MakeMatrix<1, 1>(1.0 / kA.template to<double>());
-    auto C = frc::MakeMatrix<1, 1>(1.0);
-    auto D = frc::MakeMatrix<1, 1>(0.0);
+    if (kV <= decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than zero.");
+    }
+    if (kA <= decltype(kA){0}) {
+      throw std::domain_error("Ka must be greater than zero.");
+    }
+
+    Eigen::Matrix<double, 1, 1> A{-kV.value() / kA.value()};
+    Eigen::Matrix<double, 1, 1> B{1.0 / kA.value()};
+    Eigen::Matrix<double, 1, 1> C{1.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<1, 1, 1>(A, B, C, D);
   }
@@ -135,6 +161,7 @@
    *
    * @param kV The velocity gain, in volt seconds per distance.
    * @param kA The acceleration gain, in volt seconds^2 per distance.
+   * @throws std::domain_error if kV <= 0 or kA <= 0.
    */
   template <typename Distance, typename = std::enable_if_t<
                                    std::is_same_v<units::meter, Distance> ||
@@ -142,11 +169,17 @@
   static LinearSystem<2, 1, 1> IdentifyPositionSystem(
       decltype(1_V / Velocity_t<Distance>(1)) kV,
       decltype(1_V / Acceleration_t<Distance>(1)) kA) {
-    auto A = frc::MakeMatrix<2, 2>(
-        0.0, 1.0, 0.0, -kV.template to<double>() / kA.template to<double>());
-    auto B = frc::MakeMatrix<2, 1>(0.0, 1.0 / kA.template to<double>());
-    auto C = frc::MakeMatrix<1, 2>(1.0, 0.0);
-    auto D = frc::MakeMatrix<1, 1>(0.0);
+    if (kV <= decltype(kV){0}) {
+      throw std::domain_error("Kv must be greater than zero.");
+    }
+    if (kA <= decltype(kA){0}) {
+      throw std::domain_error("Ka must be greater than zero.");
+    }
+
+    Eigen::Matrix<double, 2, 2> A{{0.0, 1.0}, {0.0, -kV.value() / kA.value()}};
+    Eigen::Matrix<double, 2, 1> B{0.0, 1.0 / kA.value()};
+    Eigen::Matrix<double, 1, 2> C{1.0, 0.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<2, 1, 1>(A, B, C, D);
   }
@@ -159,31 +192,101 @@
    * Inputs: [[left voltage], [right voltage]]
    * Outputs: [[left velocity], [right velocity]]
    *
-   * @param kVlinear The linear velocity gain, in volt seconds per distance.
-   * @param kAlinear The linear acceleration gain, in volt seconds^2 per
-   * distance.
-   * @param kVangular The angular velocity gain, in volt seconds per angle.
-   * @param kAangular The angular acceleration gain, in volt seconds^2 per
-   * angle.
+   * @param kVlinear  The linear velocity gain in volts per (meter per second).
+   * @param kAlinear  The linear acceleration gain in volts per (meter per
+   *                  second squared).
+   * @param kVangular The angular velocity gain in volts per (meter per second).
+   * @param kAangular The angular acceleration gain in volts per (meter per
+   *                  second squared).
+   * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
+   *         or kAangular <= 0.
+   */
+  static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
+      decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
+      decltype(1_V / 1_mps) kVangular, decltype(1_V / 1_mps_sq) kAangular) {
+    if (kVlinear <= decltype(kVlinear){0}) {
+      throw std::domain_error("Kv,linear must be greater than zero.");
+    }
+    if (kAlinear <= decltype(kAlinear){0}) {
+      throw std::domain_error("Ka,linear must be greater than zero.");
+    }
+    if (kVangular <= decltype(kVangular){0}) {
+      throw std::domain_error("Kv,angular must be greater than zero.");
+    }
+    if (kAangular <= decltype(kAangular){0}) {
+      throw std::domain_error("Ka,angular must be greater than zero.");
+    }
+
+    double A1 = -(kVlinear.value() / kAlinear.value() +
+                  kVangular.value() / kAangular.value());
+    double A2 = -(kVlinear.value() / kAlinear.value() -
+                  kVangular.value() / kAangular.value());
+    double B1 = 1.0 / kAlinear.value() + 1.0 / kAangular.value();
+    double B2 = 1.0 / kAlinear.value() - 1.0 / kAangular.value();
+
+    Eigen::Matrix<double, 2, 2> A =
+        0.5 * Eigen::Matrix<double, 2, 2>{{A1, A2}, {A2, A1}};
+    Eigen::Matrix<double, 2, 2> B =
+        0.5 * Eigen::Matrix<double, 2, 2>{{B1, B2}, {B2, B1}};
+    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
+
+    return LinearSystem<2, 2, 2>(A, B, C, D);
+  }
+
+  /**
+   * Constructs the state-space model for a 2 DOF drivetrain velocity system
+   * from system identification data.
+   *
+   * States: [[left velocity], [right velocity]]
+   * Inputs: [[left voltage], [right voltage]]
+   * Outputs: [[left velocity], [right velocity]]
+   *
+   * @param kVlinear   The linear velocity gain in volts per (meter per second).
+   * @param kAlinear   The linear acceleration gain in volts per (meter per
+   *                   second squared).
+   * @param kVangular  The angular velocity gain in volts per (radian per
+   *                   second).
+   * @param kAangular  The angular acceleration gain in volts per (radian per
+   *                   second squared).
+   * @param trackwidth The width of the drivetrain.
+   * @throws domain_error if kVlinear <= 0, kAlinear <= 0, kVangular <= 0,
+   *         kAangular <= 0, or trackwidth <= 0.
    */
   static LinearSystem<2, 2, 2> IdentifyDrivetrainSystem(
       decltype(1_V / 1_mps) kVlinear, decltype(1_V / 1_mps_sq) kAlinear,
       decltype(1_V / 1_rad_per_s) kVangular,
-      decltype(1_V / 1_rad_per_s_sq) kAangular) {
-    double c = 0.5 / (kAlinear.to<double>() * kAangular.to<double>());
-    double A1 = c * (-kAlinear.to<double>() * kVangular.to<double>() -
-                     kVlinear.to<double>() * kAangular.to<double>());
-    double A2 = c * (kAlinear.to<double>() * kVangular.to<double>() -
-                     kVlinear.to<double>() * kAangular.to<double>());
-    double B1 = c * (kAlinear.to<double>() + kAangular.to<double>());
-    double B2 = c * (kAangular.to<double>() - kAlinear.to<double>());
+      decltype(1_V / 1_rad_per_s_sq) kAangular, units::meter_t trackwidth) {
+    if (kVlinear <= decltype(kVlinear){0}) {
+      throw std::domain_error("Kv,linear must be greater than zero.");
+    }
+    if (kAlinear <= decltype(kAlinear){0}) {
+      throw std::domain_error("Ka,linear must be greater than zero.");
+    }
+    if (kVangular <= decltype(kVangular){0}) {
+      throw std::domain_error("Kv,angular must be greater than zero.");
+    }
+    if (kAangular <= decltype(kAangular){0}) {
+      throw std::domain_error("Ka,angular must be greater than zero.");
+    }
+    if (trackwidth <= 0_m) {
+      throw std::domain_error("r must be greater than zero.");
+    }
 
-    auto A = frc::MakeMatrix<2, 2>(A1, A2, A2, A1);
-    auto B = frc::MakeMatrix<2, 2>(B1, B2, B2, B1);
-    auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
-    auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
-
-    return LinearSystem<2, 2, 2>(A, B, C, D);
+    // We want to find a factor to include in Kv,angular that will convert
+    // `u = Kv,angular omega` to `u = Kv,angular v`.
+    //
+    // v = omega r
+    // omega = v/r
+    // omega = 1/r v
+    // omega = 1/(trackwidth/2) v
+    // omega = 2/trackwidth v
+    //
+    // So multiplying by 2/trackwidth converts the angular gains from V/(rad/s)
+    // to V/m/s).
+    return IdentifyDrivetrainSystem(kVlinear, kAlinear,
+                                    kVangular * 2.0 / trackwidth * 1_rad,
+                                    kAangular * 2.0 / trackwidth * 1_rad);
   }
 
   /**
@@ -196,15 +299,23 @@
    * @param motor Instance of DCMotor.
    * @param J Moment of inertia.
    * @param G Gear ratio from motor to carriage.
+   * @throws std::domain_error if J <= 0 or G <= 0.
    */
   static LinearSystem<1, 1, 1> FlywheelSystem(DCMotor motor,
                                               units::kilogram_square_meter_t J,
                                               double G) {
-    auto A = frc::MakeMatrix<1, 1>(
-        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).to<double>());
-    auto B = frc::MakeMatrix<1, 1>((G * motor.Kt / (motor.R * J)).to<double>());
-    auto C = frc::MakeMatrix<1, 1>(1.0);
-    auto D = frc::MakeMatrix<1, 1>(0.0);
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
+    Eigen::Matrix<double, 1, 1> A{
+        (-std::pow(G, 2) * motor.Kt / (motor.Kv * motor.R * J)).value()};
+    Eigen::Matrix<double, 1, 1> B{(G * motor.Kt / (motor.R * J)).value()};
+    Eigen::Matrix<double, 1, 1> C{1.0};
+    Eigen::Matrix<double, 1, 1> D{0.0};
 
     return LinearSystem<1, 1, 1>(A, B, C, D);
   }
@@ -220,28 +331,46 @@
    * @param m Drivetrain mass.
    * @param r Wheel radius.
    * @param rb Robot radius.
-   * @param G Gear ratio from motor to wheel.
    * @param J Moment of inertia.
+   * @param G Gear ratio from motor to wheel.
+   * @throws std::domain_error if m <= 0, r <= 0, rb <= 0, J <= 0, or
+   *         G <= 0.
    */
   static LinearSystem<2, 2, 2> DrivetrainVelocitySystem(
       const DCMotor& motor, units::kilogram_t m, units::meter_t r,
       units::meter_t rb, units::kilogram_square_meter_t J, double G) {
+    if (m <= 0_kg) {
+      throw std::domain_error("m must be greater than zero.");
+    }
+    if (r <= 0_m) {
+      throw std::domain_error("r must be greater than zero.");
+    }
+    if (rb <= 0_m) {
+      throw std::domain_error("rb must be greater than zero.");
+    }
+    if (J <= 0_kg_sq_m) {
+      throw std::domain_error("J must be greater than zero.");
+    }
+    if (G <= 0.0) {
+      throw std::domain_error("G must be greater than zero.");
+    }
+
     auto C1 = -std::pow(G, 2) * motor.Kt /
               (motor.Kv * motor.R * units::math::pow<2>(r));
     auto C2 = G * motor.Kt / (motor.R * r);
 
-    auto A = frc::MakeMatrix<2, 2>(
-        ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C1).to<double>(),
-        ((1 / m + units::math::pow<2>(rb) / J) * C1).to<double>());
-    auto B = frc::MakeMatrix<2, 2>(
-        ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
-        ((1 / m - units::math::pow<2>(rb) / J) * C2).to<double>(),
-        ((1 / m + units::math::pow<2>(rb) / J) * C2).to<double>());
-    auto C = frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0);
-    auto D = frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0);
+    Eigen::Matrix<double, 2, 2> A{
+        {((1 / m + units::math::pow<2>(rb) / J) * C1).value(),
+         ((1 / m - units::math::pow<2>(rb) / J) * C1).value()},
+        {((1 / m - units::math::pow<2>(rb) / J) * C1).value(),
+         ((1 / m + units::math::pow<2>(rb) / J) * C1).value()}};
+    Eigen::Matrix<double, 2, 2> B{
+        {((1 / m + units::math::pow<2>(rb) / J) * C2).value(),
+         ((1 / m - units::math::pow<2>(rb) / J) * C2).value()},
+        {((1 / m - units::math::pow<2>(rb) / J) * C2).value(),
+         ((1 / m + units::math::pow<2>(rb) / J) * C2).value()}};
+    Eigen::Matrix<double, 2, 2> C{{1.0, 0.0}, {0.0, 1.0}};
+    Eigen::Matrix<double, 2, 2> D{{0.0, 0.0}, {0.0, 0.0}};
 
     return LinearSystem<2, 2, 2>(A, B, C, D);
   }
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
index 023b2c2..2fad345 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/Trajectory.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <vector>
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/geometry/Transform2d.h"
 #include "units/acceleration.h"
@@ -26,12 +25,12 @@
  * various States that represent the pose, curvature, time elapsed, velocity,
  * and acceleration at that point.
  */
-class Trajectory {
+class WPILIB_DLLEXPORT Trajectory {
  public:
   /**
    * Represents one point on the trajectory.
    */
-  struct State {
+  struct WPILIB_DLLEXPORT State {
     // The time elapsed since the beginning of the trajectory.
     units::second_t t = 0_s;
 
@@ -123,6 +122,16 @@
   Trajectory RelativeTo(const Pose2d& pose);
 
   /**
+   * Concatenates another trajectory to the current trajectory. The user is
+   * responsible for making sure that the end pose of this trajectory and the
+   * start pose of the other trajectory match (if that is the desired behavior).
+   *
+   * @param other The trajectory to concatenate.
+   * @return The concatenated trajectory.
+   */
+  Trajectory operator+(const Trajectory& other) const;
+
+  /**
    * Returns the initial pose of the trajectory.
    *
    * @return The initial pose of the trajectory.
@@ -164,8 +173,10 @@
   }
 };
 
+WPILIB_DLLEXPORT
 void to_json(wpi::json& json, const Trajectory::State& state);
 
+WPILIB_DLLEXPORT
 void from_json(const wpi::json& json, Trajectory::State& state);
 
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
index 5bd7977..b1a0b52 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryConfig.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -11,6 +8,8 @@
 #include <utility>
 #include <vector>
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
@@ -32,7 +31,7 @@
  * have been defaulted to reasonable values (0, 0, {}, false). These values can
  * be changed via the SetXXX methods.
  */
-class TrajectoryConfig {
+class WPILIB_DLLEXPORT TrajectoryConfig {
  public:
   /**
    * Constructs a config object.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
index f1747fd..84ec0e0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,6 +9,8 @@
 #include <utility>
 #include <vector>
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/spline/SplineParameterizer.h"
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/TrajectoryConfig.h"
@@ -22,7 +21,7 @@
 /**
  * Helper class used to generate trajectories with various constraints.
  */
-class TrajectoryGenerator {
+class WPILIB_DLLEXPORT TrajectoryGenerator {
  public:
   using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
index 378f007..eea1c07 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
  * MIT License
@@ -35,6 +32,8 @@
 #include <utility>
 #include <vector>
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/trajectory/Trajectory.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 
@@ -42,7 +41,7 @@
 /**
  * Class used to parameterize a trajectory by time.
  */
-class TrajectoryParameterizer {
+class WPILIB_DLLEXPORT TrajectoryParameterizer {
  public:
   using PoseWithCurvature = std::pair<Pose2d, units::curvature_t>;
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
index f05cd14..6e52a09 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrajectoryUtil.h
@@ -1,21 +1,18 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <string>
+#include <string_view>
 
-#include <wpi/StringRef.h>
-#include <wpi/Twine.h>
+#include <wpi/SymbolExports.h>
 
 #include "frc/trajectory/Trajectory.h"
 
 namespace frc {
-class TrajectoryUtil {
+class WPILIB_DLLEXPORT TrajectoryUtil {
  public:
   TrajectoryUtil() = delete;
 
@@ -24,11 +21,9 @@
    *
    * @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);
+                               std::string_view path);
   /**
    * Imports a Trajectory from a PathWeaver-style JSON file.
    *
@@ -36,24 +31,24 @@
    *
    * @return The trajectory represented by the file.
    */
-  static Trajectory FromPathweaverJson(const wpi::Twine& path);
+  static Trajectory FromPathweaverJson(std::string_view path);
 
   /**
-     * Deserializes a Trajectory from PathWeaver-style JSON.
-
-     * @param json the string containing the serialized JSON
-
-     * @return the trajectory represented by the JSON
-     */
+   * Deserializes a Trajectory from PathWeaver-style JSON.
+   *
+   * @param trajectory the trajectory to export
+   *
+   * @return the string containing the serialized 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
+   *
+   * @param jsonStr the string containing the serialized JSON
+   *
+   * @return the trajectory represented by the JSON
    */
-  static Trajectory DeserializeTrajectory(wpi::StringRef json_str);
+  static Trajectory DeserializeTrajectory(std::string_view jsonStr);
 };
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
index 5f9f1b8..0e623eb 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
index 8718cc0..47a598e 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/TrapezoidProfile.inc
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <algorithm>
 
+#include "frc/trajectory/TrapezoidProfile.h"
 #include "units/math.h"
 
 namespace frc {
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
index 4f897ba..5ef15a4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 #include "units/acceleration.h"
 #include "units/curvature.h"
@@ -23,7 +22,8 @@
  * robot to slow down around tight turns, making it easier to track trajectories
  * with sharp turns.
  */
-class CentripetalAccelerationConstraint : public TrajectoryConstraint {
+class WPILIB_DLLEXPORT CentripetalAccelerationConstraint
+    : public TrajectoryConstraint {
  public:
   explicit CentripetalAccelerationConstraint(
       units::meters_per_second_squared_t maxCentripetalAcceleration);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
index f23c1e2..ad643bf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 #include "units/velocity.h"
@@ -18,7 +17,8 @@
  * commanded velocities for both sides of the drivetrain stay below a certain
  * limit.
  */
-class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
+class WPILIB_DLLEXPORT DifferentialDriveKinematicsConstraint
+    : public TrajectoryConstraint {
  public:
   DifferentialDriveKinematicsConstraint(
       const DifferentialDriveKinematics& kinematics,
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
index 23c690d..06d0e50 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/DifferentialDriveVoltageConstraint.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/controller/SimpleMotorFeedforward.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -20,7 +19,8 @@
  * 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 {
+class WPILIB_DLLEXPORT DifferentialDriveVoltageConstraint
+    : public TrajectoryConstraint {
  public:
   /**
    * Creates a new DifferentialDriveVoltageConstraint.
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
index 78bc569..e2ef37b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
index 7a30881..b7375d5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MaxVelocityConstraint.h
@@ -1,12 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 #include "units/math.h"
 #include "units/velocity.h"
@@ -17,26 +16,21 @@
  * with the EllipticalRegionConstraint or RectangularRegionConstraint to enforce
  * a max velocity within a region.
  */
-class MaxVelocityConstraint : public TrajectoryConstraint {
+class WPILIB_DLLEXPORT MaxVelocityConstraint : public TrajectoryConstraint {
  public:
   /**
    * Constructs a new MaxVelocityConstraint.
    *
    * @param maxVelocity The max velocity.
    */
-  explicit MaxVelocityConstraint(units::meters_per_second_t maxVelocity)
-      : m_maxVelocity(units::math::abs(maxVelocity)) {}
+  explicit MaxVelocityConstraint(units::meters_per_second_t maxVelocity);
 
   units::meters_per_second_t MaxVelocity(
       const Pose2d& pose, units::curvature_t curvature,
-      units::meters_per_second_t velocity) const override {
-    return m_maxVelocity;
-  }
+      units::meters_per_second_t velocity) const override;
 
   MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
-                            units::meters_per_second_t speed) const override {
-    return {};
-  }
+                            units::meters_per_second_t speed) const override;
 
  private:
   units::meters_per_second_t m_maxVelocity;
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
index 0166f56..816d8ef 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/MecanumDriveKinematicsConstraint.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <cmath>
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/kinematics/MecanumDriveKinematics.h"
 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
 #include "units/velocity.h"
@@ -20,7 +19,8 @@
  * commanded velocities for wheels of the drivetrain stay below a certain
  * limit.
  */
-class MecanumDriveKinematicsConstraint : public TrajectoryConstraint {
+class WPILIB_DLLEXPORT MecanumDriveKinematicsConstraint
+    : public TrajectoryConstraint {
  public:
   MecanumDriveKinematicsConstraint(const MecanumDriveKinematics& kinematics,
                                    units::meters_per_second_t maxSpeed);
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
index 203b237..c5bc559 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
index 0f43e29..67e9fc9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -15,13 +12,12 @@
 
 namespace frc {
 
-template <size_t NumModules>
-
 /**
  * A class that enforces constraints on the swerve drive kinematics.
  * This can be used to ensure that the trajectory is constructed so that the
  * commanded velocities of the wheels stay below a certain limit.
  */
+template <size_t NumModules>
 class SwerveDriveKinematicsConstraint : public TrajectoryConstraint {
  public:
   SwerveDriveKinematicsConstraint(
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
index 1af8511..1a1e4b8 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/SwerveDriveKinematicsConstraint.inc
@@ -1,23 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include "frc/trajectory/constraint/SwerveDriveKinematicsConstraint.h"
 #include "units/math.h"
 
 namespace frc {
 
 template <size_t NumModules>
-
-/**
- * A class that enforces constraints on the swerve drive kinematics.
- * This can be used to ensure that the trajectory is constructed so that the
- * commanded velocities of the wheels stay below a certain limit.
- */
 SwerveDriveKinematicsConstraint<NumModules>::SwerveDriveKinematicsConstraint(
     const frc::SwerveDriveKinematics<NumModules>& kinematics,
     units::meters_per_second_t maxSpeed)
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
index b5548c5..47ca820 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h
@@ -1,14 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <limits>
 
+#include <wpi/SymbolExports.h>
+
 #include "frc/geometry/Pose2d.h"
 #include "frc/spline/Spline.h"
 #include "units/acceleration.h"
@@ -20,7 +19,7 @@
  * An interface for defining user-defined velocity and acceleration constraints
  * while generating trajectories.
  */
-class TrajectoryConstraint {
+class WPILIB_DLLEXPORT TrajectoryConstraint {
  public:
   TrajectoryConstraint() = default;
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/acceleration.h b/third_party/allwpilib/wpimath/src/main/native/include/units/acceleration.h
index 5427160..a0d12b0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/acceleration.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/acceleration.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/angle.h b/third_party/allwpilib/wpimath/src/main/native/include/units/angle.h
index a0f802f..876bd60 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/angle.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/angle.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/angular_acceleration.h b/third_party/allwpilib/wpimath/src/main/native/include/units/angular_acceleration.h
index 4b1af0f..6a411c4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/angular_acceleration.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/angular_acceleration.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/angular_velocity.h b/third_party/allwpilib/wpimath/src/main/native/include/units/angular_velocity.h
index 580d021..16f39e1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/angular_velocity.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/angular_velocity.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/area.h b/third_party/allwpilib/wpimath/src/main/native/include/units/area.h
index 1bdd3e3..e4d82d9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/area.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/area.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/base.h b/third_party/allwpilib/wpimath/src/main/native/include/units/base.h
index 579ec88..f2d45cf 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/base.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/base.h
@@ -74,36 +74,42 @@
 #include <cmath>
 #include <limits>
 
-#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
 	#include <iostream>
-	#include <string>
 	#include <locale>
-
-	//------------------------------
-	//	STRING FORMATTER
-	//------------------------------
-
-	namespace units
-	{
-		namespace detail
-		{
-			template <typename T> std::string to_string(const T& t)
-			{
-				std::string str{ std::to_string(t) };
-				int offset{ 1 };
-
-				// remove trailing decimal points for integer value units. Locale aware!
-				struct lconv * lc;
-				lc = localeconv();
-				char decimalPoint = *lc->decimal_point;
-				if (str.find_last_not_of('0') == str.find(decimalPoint)) { offset = 0; }
-				str.erase(str.find_last_not_of('0') + offset, std::string::npos);
-				return str;
-			}
-		}
-	}
+	#include <string>
+#else
+	#include <locale>
+	#include <string>
+	#include <fmt/format.h>
 #endif
 
+#include <wpi/SymbolExports.h>
+
+//------------------------------
+//	STRING FORMATTER
+//------------------------------
+
+namespace units
+{
+  namespace detail
+  {
+    template <typename T> std::string to_string(const T& t)
+    {
+      std::string str{ std::to_string(t) };
+      int offset{ 1 };
+
+      // remove trailing decimal points for integer value units. Locale aware!
+      struct lconv * lc;
+      lc = localeconv();
+      char decimalPoint = *lc->decimal_point;
+      if (str.find_last_not_of('0') == str.find(decimalPoint)) { offset = 0; }
+      str.erase(str.find_last_not_of('0') + offset, std::string::npos);
+      return str;
+    }
+  }
+}
+
 namespace units
 {
 	template<typename T> inline constexpr const char* name(const T&);
@@ -172,10 +178,33 @@
  * @param		namespaceName namespace in which the new units will be encapsulated.
  * @param		nameSingular singular version of the unit name, e.g. 'meter'
  * @param		abbrev - abbreviated unit name, e.g. 'm'
- * @note		When UNIT_LIB_DISABLE_IOSTREAM is defined, the macro does not generate any code
+ * @note		When UNIT_LIB_ENABLE_IOSTREAM isn't defined, the macro does not generate any code
  */
-#if defined(UNIT_LIB_DISABLE_IOSTREAM)
-	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)
+#if !defined(UNIT_LIB_ENABLE_IOSTREAM)
+	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
+	}\
+	template <>\
+	struct fmt::formatter<units::namespaceName::nameSingular ## _t> \
+		: fmt::formatter<double> \
+	{\
+		template <typename FormatContext>\
+		auto format(const units::namespaceName::nameSingular ## _t& obj,\
+								FormatContext& ctx) -> decltype(ctx.out()) \
+		{\
+			auto out = ctx.out();\
+			out = fmt::formatter<double>::format(obj(), ctx);\
+			return fmt::format_to(out, " " #abbrev);\
+		}\
+	};\
+	namespace units\
+	{\
+	namespace namespaceName\
+	{\
+		inline std::string to_string(const nameSingular ## _t& obj)\
+		{\
+			return units::detail::to_string(obj()) + std::string(" "#abbrev);\
+		}\
+	}
 #else
 	#define UNIT_ADD_IO(namespaceName, nameSingular, abbrev)\
 	namespace namespaceName\
@@ -2180,7 +2209,7 @@
 		return UnitType(value);
 	}
 
-#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
 	template<class Units, typename T, template<typename> class NonLinearScale>
 	inline std::ostream& operator<<(std::ostream& os, const unit_t<Units, T, NonLinearScale>& obj) noexcept
 	{
@@ -2815,11 +2844,31 @@
 	namespace dimensionless
 	{
 		typedef unit_t<scalar, UNIT_LIB_DEFAULT_TYPE, decibel_scale> dB_t;
-#if !defined(UNIT_LIB_DISABLE_IOSTREAM)
+#if defined(UNIT_LIB_ENABLE_IOSTREAM)
 		inline std::ostream& operator<<(std::ostream& os, const dB_t& obj) { os << obj() << " dB"; return os; }
-#endif
 		typedef dB_t dBi_t;
 	}
+#else
+}
+}
+template <>
+struct fmt::formatter<units::dimensionless::dB_t> : fmt::formatter<double>
+{
+	template <typename FormatContext>
+	auto format(const units::dimensionless::dB_t& obj,
+							FormatContext& ctx) -> decltype(ctx.out())
+	{
+		auto out = ctx.out();
+		out = fmt::formatter<double>::format(obj(), ctx);
+		return fmt::format_to(out, " dB");
+	}
+};
+
+namespace units {
+namespace dimensionless {
+		typedef dB_t dBi_t;
+	}
+#endif
 
 	//------------------------------
 	//	DECIBEL ARITHMETIC
@@ -3365,3 +3414,5 @@
 namespace units::literals {}
 using namespace units::literals;
 #endif  // UNIT_HAS_LITERAL_SUPPORT
+
+#include "frc/fmt/Units.h"
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/capacitance.h b/third_party/allwpilib/wpimath/src/main/native/include/units/capacitance.h
index feaf88c..e9e22f6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/capacitance.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/capacitance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/charge.h b/third_party/allwpilib/wpimath/src/main/native/include/units/charge.h
index 42064b0..841f3a4 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/charge.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/charge.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/concentration.h b/third_party/allwpilib/wpimath/src/main/native/include/units/concentration.h
index b276f82..3128ff6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/concentration.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/concentration.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/conductance.h b/third_party/allwpilib/wpimath/src/main/native/include/units/conductance.h
index d0508c4..d2abff1 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/conductance.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/conductance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/constants.h b/third_party/allwpilib/wpimath/src/main/native/include/units/constants.h
index 7d5c49f..efadaf7 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/constants.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/constants.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
@@ -46,13 +43,7 @@
 #include "units/time.h"
 #include "units/velocity.h"
 
-namespace units {
-/**
- * @brief namespace for physical constants like PI and Avogadro's Number.
- * @sa See unit_t for more information on unit type containers.
- */
-#if !defined(DISABLE_PREDEFINED_UNITS)
-namespace constants {
+namespace units::constants {
 /**
  * @name Unit Containers
  * @anchor constantContainers
@@ -108,6 +99,4 @@
           (15 * math::cpow<3>(h) * math::cpow<2>(c) *
            math::cpow<4>(N_A)));  ///< Stefan-Boltzmann constant.
 /** @} */
-}  // namespace constants
-#endif
-}  // namespace units
+}  // namespace units::constants
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/current.h b/third_party/allwpilib/wpimath/src/main/native/include/units/current.h
index 54a408c..b187bb3 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/current.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/current.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/curvature.h b/third_party/allwpilib/wpimath/src/main/native/include/units/curvature.h
index 233ad61..062b09a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/curvature.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/curvature.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/data.h b/third_party/allwpilib/wpimath/src/main/native/include/units/data.h
index 386c0c2..90691a2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/data.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/data.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/data_transfer_rate.h b/third_party/allwpilib/wpimath/src/main/native/include/units/data_transfer_rate.h
index 67de063..29fb028 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/data_transfer_rate.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/data_transfer_rate.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/density.h b/third_party/allwpilib/wpimath/src/main/native/include/units/density.h
index 2509f49..8616517 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/density.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/density.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/dimensionless.h b/third_party/allwpilib/wpimath/src/main/native/include/units/dimensionless.h
index 64f75ba..26c118b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/dimensionless.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/dimensionless.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/energy.h b/third_party/allwpilib/wpimath/src/main/native/include/units/energy.h
index c206e5d..36996e2 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/energy.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/energy.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/force.h b/third_party/allwpilib/wpimath/src/main/native/include/units/force.h
index 9813958..2c2769f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/force.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/force.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/frequency.h b/third_party/allwpilib/wpimath/src/main/native/include/units/frequency.h
index f030329..f1795d5 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/frequency.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/frequency.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/illuminance.h b/third_party/allwpilib/wpimath/src/main/native/include/units/illuminance.h
index 976f6b5..f653ec6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/illuminance.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/illuminance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/impedance.h b/third_party/allwpilib/wpimath/src/main/native/include/units/impedance.h
index b4b92ad..abe0375 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/impedance.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/impedance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/inductance.h b/third_party/allwpilib/wpimath/src/main/native/include/units/inductance.h
index 6a5be7f..1e6d9f6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/inductance.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/inductance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/length.h b/third_party/allwpilib/wpimath/src/main/native/include/units/length.h
index 637797d..8b75c7c 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/length.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/length.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_flux.h b/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_flux.h
index ca7a079..31ca391 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_flux.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_flux.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_intensity.h b/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_intensity.h
index f907d2e..7d48dfe 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_intensity.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/luminous_intensity.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_field_strength.h b/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_field_strength.h
index e7a7086..5e953e9 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_field_strength.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_field_strength.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_flux.h b/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_flux.h
index 739b9e7..6516172 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_flux.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/magnetic_flux.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/mass.h b/third_party/allwpilib/wpimath/src/main/native/include/units/mass.h
index 21fa3b5..f81e68a 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/mass.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/mass.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/math.h b/third_party/allwpilib/wpimath/src/main/native/include/units/math.h
index ccb3a62..995335b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/math.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/math.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
@@ -31,13 +28,10 @@
 
 #include <cmath>
 
-#include <wpi/math>
-
 #include "units/angle.h"
 #include "units/base.h"
 #include "units/dimensionless.h"
 
-namespace units {
 //----------------------------------
 // UNIT-ENABLED CMATH FUNCTIONS
 //----------------------------------
@@ -48,7 +42,7 @@
  *          rounding functions, etc.
  * @sa See `unit_t` for more information on unit type containers.
  */
-namespace math {
+namespace units::math {
 //----------------------------------
 // TRIGONOMETRIC FUNCTIONS
 //----------------------------------
@@ -755,25 +749,4 @@
       "Unit types are not compatible.");
   return resultType(std::fma(x(), y(), resultType(z)()));
 }
-
-/**
- * Constrains theta to within the range (-pi, pi].
- *
- * @param theta Angle to normalize.
- */
-constexpr units::radian_t NormalizeAngle(units::radian_t theta) {
-  units::radian_t pi(wpi::math::pi);
-
-  // Constrain theta to within (-3pi, pi)
-  const int n_pi_pos = (theta + pi) / 2.0 / pi;
-  theta = theta - units::radian_t{n_pi_pos * 2.0 * wpi::math::pi};
-
-  // Cut off the bottom half of the above range to constrain within
-  // (-pi, pi]
-  const int n_pi_neg = (theta - pi) / 2.0 / pi;
-  theta = theta - units::radian_t{n_pi_neg * 2.0 * wpi::math::pi};
-
-  return theta;
-}
-}  // namespace math
-}  // namespace units
+}  // namespace units::math
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/moment_of_inertia.h b/third_party/allwpilib/wpimath/src/main/native/include/units/moment_of_inertia.h
index 938a635..9d30852 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/moment_of_inertia.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/moment_of_inertia.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/power.h b/third_party/allwpilib/wpimath/src/main/native/include/units/power.h
index d1a9504..b4c5f13 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/power.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/power.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/pressure.h b/third_party/allwpilib/wpimath/src/main/native/include/units/pressure.h
index c14bae1..63c0e37 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/pressure.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/pressure.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/radiation.h b/third_party/allwpilib/wpimath/src/main/native/include/units/radiation.h
index b631336..84f8eed 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/radiation.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/radiation.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
@@ -38,6 +35,7 @@
 #include "units/base.h"
 #include "units/energy.h"
 #include "units/frequency.h"
+#include "units/mass.h"
 
 namespace units {
 /**
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/solid_angle.h b/third_party/allwpilib/wpimath/src/main/native/include/units/solid_angle.h
index 0e38f55..2e0182b 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/solid_angle.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/solid_angle.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/substance.h b/third_party/allwpilib/wpimath/src/main/native/include/units/substance.h
index c774497..8691818 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/substance.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/substance.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/temperature.h b/third_party/allwpilib/wpimath/src/main/native/include/units/temperature.h
index 25a9b98..24f22a0 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/temperature.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/temperature.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/time.h b/third_party/allwpilib/wpimath/src/main/native/include/units/time.h
index 13e66c4..6366123 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/time.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/time.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/torque.h b/third_party/allwpilib/wpimath/src/main/native/include/units/torque.h
index 58f4ca3..42ab326 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/torque.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/torque.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/velocity.h b/third_party/allwpilib/wpimath/src/main/native/include/units/velocity.h
index 5a0ebcb..d63d1e6 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/velocity.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/velocity.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/voltage.h b/third_party/allwpilib/wpimath/src/main/native/include/units/voltage.h
index 917c52a..605baed 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/voltage.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/voltage.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/units/volume.h b/third_party/allwpilib/wpimath/src/main/native/include/units/volume.h
index f13fdef..c361a8f 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/units/volume.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/units/volume.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 // Copyright (c) 2016 Nic Holthaus
 //
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff
deleted file mode 100644
index abf5b7d..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/AutoDiff
+++ /dev/null
@@ -1,40 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2008-2009 Gael Guennebaud <g.gael@free.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_AUTODIFF_MODULE
-#define EIGEN_AUTODIFF_MODULE
-
-namespace Eigen {
-
-/**
-  * \defgroup AutoDiff_Module Auto Diff module
-  *
-  * This module features forward automatic differentation via a simple
-  * templated scalar type wrapper AutoDiffScalar.
-  *
-  * Warning : this should NOT be confused with numerical differentiation, which
-  * is a different method and has its own module in Eigen : \ref NumericalDiff_Module.
-  *
-  * \code
-  * #include <unsupported/Eigen/AutoDiff>
-  * \endcode
-  */
-//@{
-
-}
-
-#include "src/AutoDiff/AutoDiffScalar.h"
-// #include "src/AutoDiff/AutoDiffVector.h"
-#include "src/AutoDiff/AutoDiffJacobian.h"
-
-namespace Eigen {
-//@}
-}
-
-#endif // EIGEN_AUTODIFF_MODULE
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions
deleted file mode 100644
index 60dc0a6..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/MatrixFunctions
+++ /dev/null
@@ -1,500 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Jitse Niesen <jitse@maths.leeds.ac.uk>
-// Copyright (C) 2012 Chen-Pang He <jdh8@ms63.hinet.net>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIX_FUNCTIONS
-#define EIGEN_MATRIX_FUNCTIONS
-
-#include <cfloat>
-#include <list>
-
-#include <Eigen/Core>
-#include <Eigen/LU>
-#include <Eigen/Eigenvalues>
-
-/**
-  * \defgroup MatrixFunctions_Module Matrix functions module
-  * \brief This module aims to provide various methods for the computation of
-  * matrix functions. 
-  *
-  * To use this module, add 
-  * \code
-  * #include <unsupported/Eigen/MatrixFunctions>
-  * \endcode
-  * at the start of your source file.
-  *
-  * This module defines the following MatrixBase methods.
-  *  - \ref matrixbase_cos "MatrixBase::cos()", for computing the matrix cosine
-  *  - \ref matrixbase_cosh "MatrixBase::cosh()", for computing the matrix hyperbolic cosine
-  *  - \ref matrixbase_exp "MatrixBase::exp()", for computing the matrix exponential
-  *  - \ref matrixbase_log "MatrixBase::log()", for computing the matrix logarithm
-  *  - \ref matrixbase_pow "MatrixBase::pow()", for computing the matrix power
-  *  - \ref matrixbase_matrixfunction "MatrixBase::matrixFunction()", for computing general matrix functions
-  *  - \ref matrixbase_sin "MatrixBase::sin()", for computing the matrix sine
-  *  - \ref matrixbase_sinh "MatrixBase::sinh()", for computing the matrix hyperbolic sine
-  *  - \ref matrixbase_sqrt "MatrixBase::sqrt()", for computing the matrix square root
-  *
-  * These methods are the main entry points to this module. 
-  *
-  * %Matrix functions are defined as follows.  Suppose that \f$ f \f$
-  * is an entire function (that is, a function on the complex plane
-  * that is everywhere complex differentiable).  Then its Taylor
-  * series
-  * \f[ f(0) + f'(0) x + \frac{f''(0)}{2} x^2 + \frac{f'''(0)}{3!} x^3 + \cdots \f]
-  * converges to \f$ f(x) \f$. In this case, we can define the matrix
-  * function by the same series:
-  * \f[ f(M) = f(0) + f'(0) M + \frac{f''(0)}{2} M^2 + \frac{f'''(0)}{3!} M^3 + \cdots \f]
-  *
-  */
-
-#include "src/MatrixFunctions/MatrixExponential.h"
-#include "src/MatrixFunctions/MatrixFunction.h"
-#include "src/MatrixFunctions/MatrixSquareRoot.h"
-#include "src/MatrixFunctions/MatrixLogarithm.h"
-#include "src/MatrixFunctions/MatrixPower.h"
-
-
-/** 
-\page matrixbaseextra_page
-\ingroup MatrixFunctions_Module
-
-\section matrixbaseextra MatrixBase methods defined in the MatrixFunctions module
-
-The remainder of the page documents the following MatrixBase methods
-which are defined in the MatrixFunctions module.
-
-
-
-\subsection matrixbase_cos MatrixBase::cos()
-
-Compute the matrix cosine.
-
-\code
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
-\endcode
-
-\param[in]  M  a square matrix.
-\returns  expression representing \f$ \cos(M) \f$.
-
-This function computes the matrix cosine. Use ArrayBase::cos() for computing the entry-wise cosine.
-
-The implementation calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cos().
-
-\sa \ref matrixbase_sin "sin()" for an example.
-
-
-
-\subsection matrixbase_cosh MatrixBase::cosh()
-
-Compute the matrix hyberbolic cosine.
-
-\code
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
-\endcode
-
-\param[in]  M  a square matrix.
-\returns  expression representing \f$ \cosh(M) \f$
-
-This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::cosh().
-
-\sa \ref matrixbase_sinh "sinh()" for an example.
-
-
-
-\subsection matrixbase_exp MatrixBase::exp()
-
-Compute the matrix exponential.
-
-\code
-const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
-\endcode
-
-\param[in]  M  matrix whose exponential is to be computed.
-\returns    expression representing the matrix exponential of \p M.
-
-The matrix exponential of \f$ M \f$ is defined by
-\f[ \exp(M) = \sum_{k=0}^\infty \frac{M^k}{k!}. \f]
-The matrix exponential can be used to solve linear ordinary
-differential equations: the solution of \f$ y' = My \f$ with the
-initial condition \f$ y(0) = y_0 \f$ is given by
-\f$ y(t) = \exp(M) y_0 \f$.
-
-The matrix exponential is different from applying the exp function to all the entries in the matrix.
-Use ArrayBase::exp() if you want to do the latter.
-
-The cost of the computation is approximately \f$ 20 n^3 \f$ for
-matrices of size \f$ n \f$. The number 20 depends weakly on the
-norm of the matrix.
-
-The matrix exponential is computed using the scaling-and-squaring
-method combined with Pad&eacute; approximation. The matrix is first
-rescaled, then the exponential of the reduced matrix is computed
-approximant, and then the rescaling is undone by repeated
-squaring. The degree of the Pad&eacute; approximant is chosen such
-that the approximation error is less than the round-off
-error. However, errors may accumulate during the squaring phase.
-
-Details of the algorithm can be found in: Nicholas J. Higham, "The
-scaling and squaring method for the matrix exponential revisited,"
-<em>SIAM J. %Matrix Anal. Applic.</em>, <b>26</b>:1179&ndash;1193,
-2005.
-
-Example: The following program checks that
-\f[ \exp \left[ \begin{array}{ccc}
-      0 & \frac14\pi & 0 \\
-      -\frac14\pi & 0 & 0 \\
-      0 & 0 & 0
-    \end{array} \right] = \left[ \begin{array}{ccc}
-      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
-      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
-      0 & 0 & 1
-    \end{array} \right]. \f]
-This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
-the z-axis.
-
-\include MatrixExponential.cpp
-Output: \verbinclude MatrixExponential.out
-
-\note \p M has to be a matrix of \c float, \c double, `long double`
-\c complex<float>, \c complex<double>, or `complex<long double>` .
-
-
-\subsection matrixbase_log MatrixBase::log()
-
-Compute the matrix logarithm.
-
-\code
-const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
-\endcode
-
-\param[in]  M  invertible matrix whose logarithm is to be computed.
-\returns    expression representing the matrix logarithm root of \p M.
-
-The matrix logarithm of \f$ M \f$ is a matrix \f$ X \f$ such that 
-\f$ \exp(X) = M \f$ where exp denotes the matrix exponential. As for
-the scalar logarithm, the equation \f$ \exp(X) = M \f$ may have
-multiple solutions; this function returns a matrix whose eigenvalues
-have imaginary part in the interval \f$ (-\pi,\pi] \f$.
-
-The matrix logarithm is different from applying the log function to all the entries in the matrix.
-Use ArrayBase::log() if you want to do the latter.
-
-In the real case, the matrix \f$ M \f$ should be invertible and
-it should have no eigenvalues which are real and negative (pairs of
-complex conjugate eigenvalues are allowed). In the complex case, it
-only needs to be invertible.
-
-This function computes the matrix logarithm using the Schur-Parlett
-algorithm as implemented by MatrixBase::matrixFunction(). The
-logarithm of an atomic block is computed by MatrixLogarithmAtomic,
-which uses direct computation for 1-by-1 and 2-by-2 blocks and an
-inverse scaling-and-squaring algorithm for bigger blocks, with the
-square roots computed by MatrixBase::sqrt().
-
-Details of the algorithm can be found in Section 11.6.2 of:
-Nicholas J. Higham,
-<em>Functions of Matrices: Theory and Computation</em>,
-SIAM 2008. ISBN 978-0-898716-46-7.
-
-Example: The following program checks that
-\f[ \log \left[ \begin{array}{ccc} 
-      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
-      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
-      0 & 0 & 1
-    \end{array} \right] = \left[ \begin{array}{ccc}
-      0 & \frac14\pi & 0 \\ 
-      -\frac14\pi & 0 & 0 \\
-      0 & 0 & 0 
-    \end{array} \right]. \f]
-This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
-the z-axis. This is the inverse of the example used in the
-documentation of \ref matrixbase_exp "exp()".
-
-\include MatrixLogarithm.cpp
-Output: \verbinclude MatrixLogarithm.out
-
-\note \p M has to be a matrix of \c float, \c double, `long
-double`, \c complex<float>, \c complex<double>, or `complex<long double>`.
-
-\sa MatrixBase::exp(), MatrixBase::matrixFunction(), 
-    class MatrixLogarithmAtomic, MatrixBase::sqrt().
-
-
-\subsection matrixbase_pow MatrixBase::pow()
-
-Compute the matrix raised to arbitrary real power.
-
-\code
-const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(RealScalar p) const
-\endcode
-
-\param[in]  M  base of the matrix power, should be a square matrix.
-\param[in]  p  exponent of the matrix power.
-
-The matrix power \f$ M^p \f$ is defined as \f$ \exp(p \log(M)) \f$,
-where exp denotes the matrix exponential, and log denotes the matrix
-logarithm. This is different from raising all the entries in the matrix
-to the p-th power. Use ArrayBase::pow() if you want to do the latter.
-
-If \p p is complex, the scalar type of \p M should be the type of \p
-p . \f$ M^p \f$ simply evaluates into \f$ \exp(p \log(M)) \f$.
-Therefore, the matrix \f$ M \f$ should meet the conditions to be an
-argument of matrix logarithm.
-
-If \p p is real, it is casted into the real scalar type of \p M. Then
-this function computes the matrix power using the Schur-Pad&eacute;
-algorithm as implemented by class MatrixPower. The exponent is split
-into integral part and fractional part, where the fractional part is
-in the interval \f$ (-1, 1) \f$. The main diagonal and the first
-super-diagonal is directly computed.
-
-If \p M is singular with a semisimple zero eigenvalue and \p p is
-positive, the Schur factor \f$ T \f$ is reordered with Givens
-rotations, i.e.
-
-\f[ T = \left[ \begin{array}{cc}
-      T_1 & T_2 \\
-      0   & 0
-    \end{array} \right] \f]
-
-where \f$ T_1 \f$ is invertible. Then \f$ T^p \f$ is given by
-
-\f[ T^p = \left[ \begin{array}{cc}
-      T_1^p & T_1^{-1} T_1^p T_2 \\
-      0     & 0
-    \end{array}. \right] \f]
-
-\warning Fractional power of a matrix with a non-semisimple zero
-eigenvalue is not well-defined. We introduce an assertion failure
-against inaccurate result, e.g. \code
-#include <unsupported/Eigen/MatrixFunctions>
-#include <iostream>
-
-int main()
-{
-  Eigen::Matrix4d A;
-  A << 0, 0, 2, 3,
-       0, 0, 4, 5,
-       0, 0, 6, 7,
-       0, 0, 8, 9;
-  std::cout << A.pow(0.37) << std::endl;
-  
-  // The 1 makes eigenvalue 0 non-semisimple.
-  A.coeffRef(0, 1) = 1;
-
-  // This fails if EIGEN_NO_DEBUG is undefined.
-  std::cout << A.pow(0.37) << std::endl;
-
-  return 0;
-}
-\endcode
-
-Details of the algorithm can be found in: Nicholas J. Higham and
-Lijing Lin, "A Schur-Pad&eacute; algorithm for fractional powers of a
-matrix," <em>SIAM J. %Matrix Anal. Applic.</em>,
-<b>32(3)</b>:1056&ndash;1078, 2011.
-
-Example: The following program checks that
-\f[ \left[ \begin{array}{ccc}
-      \cos1 & -\sin1 & 0 \\
-      \sin1 & \cos1 & 0 \\
-      0 & 0 & 1
-    \end{array} \right]^{\frac14\pi} = \left[ \begin{array}{ccc}
-      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
-      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
-      0 & 0 & 1
-    \end{array} \right]. \f]
-This corresponds to \f$ \frac14\pi \f$ rotations of 1 radian around
-the z-axis.
-
-\include MatrixPower.cpp
-Output: \verbinclude MatrixPower.out
-
-MatrixBase::pow() is user-friendly. However, there are some
-circumstances under which you should use class MatrixPower directly.
-MatrixPower can save the result of Schur decomposition, so it's
-better for computing various powers for the same matrix.
-
-Example:
-\include MatrixPower_optimal.cpp
-Output: \verbinclude MatrixPower_optimal.out
-
-\note \p M has to be a matrix of \c float, \c double, `long
-double`, \c complex<float>, \c complex<double>, or
-\c complex<long double> .
-
-\sa MatrixBase::exp(), MatrixBase::log(), class MatrixPower.
-
-
-\subsection matrixbase_matrixfunction MatrixBase::matrixFunction()
-
-Compute a matrix function.
-
-\code
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
-\endcode
-
-\param[in]  M  argument of matrix function, should be a square matrix.
-\param[in]  f  an entire function; \c f(x,n) should compute the n-th
-derivative of f at x.
-\returns  expression representing \p f applied to \p M.
-
-Suppose that \p M is a matrix whose entries have type \c Scalar. 
-Then, the second argument, \p f, should be a function with prototype
-\code 
-ComplexScalar f(ComplexScalar, int) 
-\endcode
-where \c ComplexScalar = \c std::complex<Scalar> if \c Scalar is
-real (e.g., \c float or \c double) and \c ComplexScalar =
-\c Scalar if \c Scalar is complex. The return value of \c f(x,n)
-should be \f$ f^{(n)}(x) \f$, the n-th derivative of f at x.
-
-This routine uses the algorithm described in:
-Philip Davies and Nicholas J. Higham, 
-"A Schur-Parlett algorithm for computing matrix functions", 
-<em>SIAM J. %Matrix Anal. Applic.</em>, <b>25</b>:464&ndash;485, 2003.
-
-The actual work is done by the MatrixFunction class.
-
-Example: The following program checks that
-\f[ \exp \left[ \begin{array}{ccc} 
-      0 & \frac14\pi & 0 \\ 
-      -\frac14\pi & 0 & 0 \\
-      0 & 0 & 0 
-    \end{array} \right] = \left[ \begin{array}{ccc}
-      \frac12\sqrt2 & -\frac12\sqrt2 & 0 \\
-      \frac12\sqrt2 & \frac12\sqrt2 & 0 \\
-      0 & 0 & 1
-    \end{array} \right]. \f]
-This corresponds to a rotation of \f$ \frac14\pi \f$ radians around
-the z-axis. This is the same example as used in the documentation
-of \ref matrixbase_exp "exp()".
-
-\include MatrixFunction.cpp
-Output: \verbinclude MatrixFunction.out
-
-Note that the function \c expfn is defined for complex numbers 
-\c x, even though the matrix \c A is over the reals. Instead of
-\c expfn, we could also have used StdStemFunctions::exp:
-\code
-A.matrixFunction(StdStemFunctions<std::complex<double> >::exp, &B);
-\endcode
-
-
-
-\subsection matrixbase_sin MatrixBase::sin()
-
-Compute the matrix sine.
-
-\code
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
-\endcode
-
-\param[in]  M  a square matrix.
-\returns  expression representing \f$ \sin(M) \f$.
-
-This function computes the matrix sine. Use ArrayBase::sin() for computing the entry-wise sine.
-
-The implementation calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sin().
-
-Example: \include MatrixSine.cpp
-Output: \verbinclude MatrixSine.out
-
-
-
-\subsection matrixbase_sinh MatrixBase::sinh()
-
-Compute the matrix hyperbolic sine.
-
-\code
-MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
-\endcode
-
-\param[in]  M  a square matrix.
-\returns  expression representing \f$ \sinh(M) \f$
-
-This function calls \ref matrixbase_matrixfunction "matrixFunction()" with StdStemFunctions::sinh().
-
-Example: \include MatrixSinh.cpp
-Output: \verbinclude MatrixSinh.out
-
-
-\subsection matrixbase_sqrt MatrixBase::sqrt()
-
-Compute the matrix square root.
-
-\code
-const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
-\endcode
-
-\param[in]  M  invertible matrix whose square root is to be computed.
-\returns    expression representing the matrix square root of \p M.
-
-The matrix square root of \f$ M \f$ is the matrix \f$ M^{1/2} \f$
-whose square is the original matrix; so if \f$ S = M^{1/2} \f$ then
-\f$ S^2 = M \f$. This is different from taking the square root of all
-the entries in the matrix; use ArrayBase::sqrt() if you want to do the
-latter.
-
-In the <b>real case</b>, the matrix \f$ M \f$ should be invertible and
-it should have no eigenvalues which are real and negative (pairs of
-complex conjugate eigenvalues are allowed). In that case, the matrix
-has a square root which is also real, and this is the square root
-computed by this function. 
-
-The matrix square root is computed by first reducing the matrix to
-quasi-triangular form with the real Schur decomposition. The square
-root of the quasi-triangular matrix can then be computed directly. The
-cost is approximately \f$ 25 n^3 \f$ real flops for the real Schur
-decomposition and \f$ 3\frac13 n^3 \f$ real flops for the remainder
-(though the computation time in practice is likely more than this
-indicates).
-
-Details of the algorithm can be found in: Nicholas J. Highan,
-"Computing real square roots of a real matrix", <em>Linear Algebra
-Appl.</em>, 88/89:405&ndash;430, 1987.
-
-If the matrix is <b>positive-definite symmetric</b>, then the square
-root is also positive-definite symmetric. In this case, it is best to
-use SelfAdjointEigenSolver::operatorSqrt() to compute it.
-
-In the <b>complex case</b>, the matrix \f$ M \f$ should be invertible;
-this is a restriction of the algorithm. The square root computed by
-this algorithm is the one whose eigenvalues have an argument in the
-interval \f$ (-\frac12\pi, \frac12\pi] \f$. This is the usual branch
-cut.
-
-The computation is the same as in the real case, except that the
-complex Schur decomposition is used to reduce the matrix to a
-triangular matrix. The theoretical cost is the same. Details are in:
-&Aring;ke Bj&ouml;rck and Sven Hammarling, "A Schur method for the
-square root of a matrix", <em>Linear Algebra Appl.</em>,
-52/53:127&ndash;140, 1983.
-
-Example: The following program checks that the square root of
-\f[ \left[ \begin{array}{cc} 
-              \cos(\frac13\pi) & -\sin(\frac13\pi) \\
-              \sin(\frac13\pi) & \cos(\frac13\pi)
-    \end{array} \right], \f]
-corresponding to a rotation over 60 degrees, is a rotation over 30 degrees:
-\f[ \left[ \begin{array}{cc} 
-              \cos(\frac16\pi) & -\sin(\frac16\pi) \\
-              \sin(\frac16\pi) & \cos(\frac16\pi)
-    \end{array} \right]. \f]
-
-\include MatrixSquareRoot.cpp
-Output: \verbinclude MatrixSquareRoot.out
-
-\sa class RealSchur, class ComplexSchur, class MatrixSquareRoot,
-    SelfAdjointEigenSolver::operatorSqrt().
-
-*/
-
-#endif // EIGEN_MATRIX_FUNCTIONS
-
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
deleted file mode 100644
index 2f50e99..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
+++ /dev/null
@@ -1,694 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_AUTODIFF_SCALAR_H
-#define EIGEN_AUTODIFF_SCALAR_H
-
-namespace Eigen {
-
-namespace internal {
-
-template<typename A, typename B>
-struct make_coherent_impl {
-  static void run(A&, B&) {}
-};
-
-// resize a to match b is a.size()==0, and conversely.
-template<typename A, typename B>
-void make_coherent(const A& a, const B&b)
-{
-  make_coherent_impl<A,B>::run(a.const_cast_derived(), b.const_cast_derived());
-}
-
-template<typename _DerType, bool Enable> struct auto_diff_special_op;
-
-} // end namespace internal
-
-template<typename _DerType> class AutoDiffScalar;
-
-template<typename NewDerType>
-inline AutoDiffScalar<NewDerType> MakeAutoDiffScalar(const typename NewDerType::Scalar& value, const NewDerType &der) {
-  return AutoDiffScalar<NewDerType>(value,der);
-}
-
-/** \class AutoDiffScalar
-  * \brief A scalar type replacement with automatic differentation capability
-  *
-  * \param _DerType the vector type used to store/represent the derivatives. The base scalar type
-  *                 as well as the number of derivatives to compute are determined from this type.
-  *                 Typical choices include, e.g., \c Vector4f for 4 derivatives, or \c VectorXf
-  *                 if the number of derivatives is not known at compile time, and/or, the number
-  *                 of derivatives is large.
-  *                 Note that _DerType can also be a reference (e.g., \c VectorXf&) to wrap a
-  *                 existing vector into an AutoDiffScalar.
-  *                 Finally, _DerType can also be any Eigen compatible expression.
-  *
-  * This class represents a scalar value while tracking its respective derivatives using Eigen's expression
-  * template mechanism.
-  *
-  * It supports the following list of global math function:
-  *  - std::abs, std::sqrt, std::pow, std::exp, std::log, std::sin, std::cos,
-  *  - internal::abs, internal::sqrt, numext::pow, internal::exp, internal::log, internal::sin, internal::cos,
-  *  - internal::conj, internal::real, internal::imag, numext::abs2.
-  *
-  * AutoDiffScalar can be used as the scalar type of an Eigen::Matrix object. However,
-  * in that case, the expression template mechanism only occurs at the top Matrix level,
-  * while derivatives are computed right away.
-  *
-  */
-
-template<typename _DerType>
-class AutoDiffScalar
-  : public internal::auto_diff_special_op
-            <_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
-                                          typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value>
-{
-  public:
-    typedef internal::auto_diff_special_op
-            <_DerType, !internal::is_same<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar,
-                       typename NumTraits<typename internal::traits<typename internal::remove_all<_DerType>::type>::Scalar>::Real>::value> Base;
-    typedef typename internal::remove_all<_DerType>::type DerType;
-    typedef typename internal::traits<DerType>::Scalar Scalar;
-    typedef typename NumTraits<Scalar>::Real Real;
-
-    using Base::operator+;
-    using Base::operator*;
-
-    /** Default constructor without any initialization. */
-    AutoDiffScalar() {}
-
-    /** Constructs an active scalar from its \a value,
-        and initializes the \a nbDer derivatives such that it corresponds to the \a derNumber -th variable */
-    AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
-      : m_value(value), m_derivatives(DerType::Zero(nbDer))
-    {
-      m_derivatives.coeffRef(derNumber) = Scalar(1);
-    }
-
-    /** Conversion from a scalar constant to an active scalar.
-      * The derivatives are set to zero. */
-    /*explicit*/ AutoDiffScalar(const Real& value)
-      : m_value(value)
-    {
-      if(m_derivatives.size()>0)
-        m_derivatives.setZero();
-    }
-
-    /** Constructs an active scalar from its \a value and derivatives \a der */
-    AutoDiffScalar(const Scalar& value, const DerType& der)
-      : m_value(value), m_derivatives(der)
-    {}
-
-    template<typename OtherDerType>
-    AutoDiffScalar(const AutoDiffScalar<OtherDerType>& other
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-    , typename internal::enable_if<
-            internal::is_same<Scalar, typename internal::traits<typename internal::remove_all<OtherDerType>::type>::Scalar>::value
-        &&  internal::is_convertible<OtherDerType,DerType>::value , void*>::type = 0
-#endif
-    )
-      : m_value(other.value()), m_derivatives(other.derivatives())
-    {}
-
-    friend  std::ostream & operator << (std::ostream & s, const AutoDiffScalar& a)
-    {
-      return s << a.value();
-    }
-
-    AutoDiffScalar(const AutoDiffScalar& other)
-      : m_value(other.value()), m_derivatives(other.derivatives())
-    {}
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      m_value = other.value();
-      m_derivatives = other.derivatives();
-      return *this;
-    }
-
-    inline AutoDiffScalar& operator=(const AutoDiffScalar& other)
-    {
-      m_value = other.value();
-      m_derivatives = other.derivatives();
-      return *this;
-    }
-
-    inline AutoDiffScalar& operator=(const Scalar& other)
-    {
-      m_value = other;
-      if(m_derivatives.size()>0)
-        m_derivatives.setZero();
-      return *this;
-    }
-
-//     inline operator const Scalar& () const { return m_value; }
-//     inline operator Scalar& () { return m_value; }
-
-    inline const Scalar& value() const { return m_value; }
-    inline Scalar& value() { return m_value; }
-
-    inline const DerType& derivatives() const { return m_derivatives; }
-    inline DerType& derivatives() { return m_derivatives; }
-
-    inline bool operator< (const Scalar& other) const  { return m_value <  other; }
-    inline bool operator<=(const Scalar& other) const  { return m_value <= other; }
-    inline bool operator> (const Scalar& other) const  { return m_value >  other; }
-    inline bool operator>=(const Scalar& other) const  { return m_value >= other; }
-    inline bool operator==(const Scalar& other) const  { return m_value == other; }
-    inline bool operator!=(const Scalar& other) const  { return m_value != other; }
-
-    friend inline bool operator< (const Scalar& a, const AutoDiffScalar& b) { return a <  b.value(); }
-    friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) { return a <= b.value(); }
-    friend inline bool operator> (const Scalar& a, const AutoDiffScalar& b) { return a >  b.value(); }
-    friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) { return a >= b.value(); }
-    friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) { return a == b.value(); }
-    friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) { return a != b.value(); }
-
-    template<typename OtherDerType> inline bool operator< (const AutoDiffScalar<OtherDerType>& b) const  { return m_value <  b.value(); }
-    template<typename OtherDerType> inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value <= b.value(); }
-    template<typename OtherDerType> inline bool operator> (const AutoDiffScalar<OtherDerType>& b) const  { return m_value >  b.value(); }
-    template<typename OtherDerType> inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value >= b.value(); }
-    template<typename OtherDerType> inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const  { return m_value == b.value(); }
-    template<typename OtherDerType> inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const  { return m_value != b.value(); }
-
-    inline const AutoDiffScalar<DerType&> operator+(const Scalar& other) const
-    {
-      return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
-    }
-
-    friend inline const AutoDiffScalar<DerType&> operator+(const Scalar& a, const AutoDiffScalar& b)
-    {
-      return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-    }
-
-//     inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
-//     {
-//       return AutoDiffScalar<DerType&>(m_value + other, m_derivatives);
-//     }
-
-//     friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar& b)
-//     {
-//       return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-//     }
-
-    inline AutoDiffScalar& operator+=(const Scalar& other)
-    {
-      value() += other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >
-    operator+(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,const DerType,const typename internal::remove_all<OtherDerType>::type> >(
-        m_value + other.value(),
-        m_derivatives + other.derivatives());
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar&
-    operator+=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      (*this) = (*this) + other;
-      return *this;
-    }
-
-    inline const AutoDiffScalar<DerType&> operator-(const Scalar& b) const
-    {
-      return AutoDiffScalar<DerType&>(m_value - b, m_derivatives);
-    }
-
-    friend inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
-    operator-(const Scalar& a, const AutoDiffScalar& b)
-    {
-      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
-            (a - b.value(), -b.derivatives());
-    }
-
-    inline AutoDiffScalar& operator-=(const Scalar& other)
-    {
-      value() -= other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >
-    operator-(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return AutoDiffScalar<CwiseBinaryOp<internal::scalar_difference_op<Scalar>, const DerType,const typename internal::remove_all<OtherDerType>::type> >(
-        m_value - other.value(),
-        m_derivatives - other.derivatives());
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar&
-    operator-=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      *this = *this - other;
-      return *this;
-    }
-
-    inline const AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >
-    operator-() const
-    {
-      return AutoDiffScalar<CwiseUnaryOp<internal::scalar_opposite_op<Scalar>, const DerType> >(
-        -m_value,
-        -m_derivatives);
-    }
-
-    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator*(const Scalar& other) const
-    {
-      return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
-    }
-
-    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator*(const Scalar& other, const AutoDiffScalar& a)
-    {
-      return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
-    }
-
-//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator*(const Real& other) const
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         m_value * other,
-//         (m_derivatives * other));
-//     }
-//
-//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator*(const Real& other, const AutoDiffScalar& a)
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         a.value() * other,
-//         a.derivatives() * other);
-//     }
-
-    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator/(const Scalar& other) const
-    {
-      return MakeAutoDiffScalar(m_value / other, (m_derivatives * (Scalar(1)/other)));
-    }
-
-    friend inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) >
-    operator/(const Scalar& other, const AutoDiffScalar& a)
-    {
-      return MakeAutoDiffScalar(other / a.value(), a.derivatives() * (Scalar(-other) / (a.value()*a.value())));
-    }
-
-//     inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator/(const Real& other) const
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         m_value / other,
-//         (m_derivatives * (Real(1)/other)));
-//     }
-//
-//     friend inline const AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >
-//     operator/(const Real& other, const AutoDiffScalar& a)
-//     {
-//       return AutoDiffScalar<typename CwiseUnaryOp<internal::scalar_multiple_op<Real>, DerType>::Type >(
-//         other / a.value(),
-//         a.derivatives() * (-Real(1)/other));
-//     }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(
-        CwiseBinaryOp<internal::scalar_difference_op<Scalar> EIGEN_COMMA
-          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product) EIGEN_COMMA
-          const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) >,Scalar,product) >
-    operator/(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return MakeAutoDiffScalar(
-        m_value / other.value(),
-          ((m_derivatives * other.value()) - (other.derivatives() * m_value))
-        * (Scalar(1)/(other.value()*other.value())));
-    }
-
-    template<typename OtherDerType>
-    inline const AutoDiffScalar<CwiseBinaryOp<internal::scalar_sum_op<Scalar>,
-        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(DerType,Scalar,product),
-        const EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<OtherDerType>::type,Scalar,product) > >
-    operator*(const AutoDiffScalar<OtherDerType>& other) const
-    {
-      internal::make_coherent(m_derivatives, other.derivatives());
-      return MakeAutoDiffScalar(
-        m_value * other.value(),
-        (m_derivatives * other.value()) + (other.derivatives() * m_value));
-    }
-
-    inline AutoDiffScalar& operator*=(const Scalar& other)
-    {
-      *this = *this * other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      *this = *this * other;
-      return *this;
-    }
-
-    inline AutoDiffScalar& operator/=(const Scalar& other)
-    {
-      *this = *this / other;
-      return *this;
-    }
-
-    template<typename OtherDerType>
-    inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other)
-    {
-      *this = *this / other;
-      return *this;
-    }
-
-  protected:
-    Scalar m_value;
-    DerType m_derivatives;
-
-};
-
-namespace internal {
-
-template<typename _DerType>
-struct auto_diff_special_op<_DerType, true>
-//   : auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
-//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value>
-{
-  typedef typename remove_all<_DerType>::type DerType;
-  typedef typename traits<DerType>::Scalar Scalar;
-  typedef typename NumTraits<Scalar>::Real Real;
-
-//   typedef auto_diff_scalar_op<_DerType, typename NumTraits<Scalar>::Real,
-//                            is_same<Scalar,typename NumTraits<Scalar>::Real>::value> Base;
-
-//   using Base::operator+;
-//   using Base::operator+=;
-//   using Base::operator-;
-//   using Base::operator-=;
-//   using Base::operator*;
-//   using Base::operator*=;
-
-  const AutoDiffScalar<_DerType>& derived() const { return *static_cast<const AutoDiffScalar<_DerType>*>(this); }
-  AutoDiffScalar<_DerType>& derived() { return *static_cast<AutoDiffScalar<_DerType>*>(this); }
-
-
-  inline const AutoDiffScalar<DerType&> operator+(const Real& other) const
-  {
-    return AutoDiffScalar<DerType&>(derived().value() + other, derived().derivatives());
-  }
-
-  friend inline const AutoDiffScalar<DerType&> operator+(const Real& a, const AutoDiffScalar<_DerType>& b)
-  {
-    return AutoDiffScalar<DerType&>(a + b.value(), b.derivatives());
-  }
-
-  inline AutoDiffScalar<_DerType>& operator+=(const Real& other)
-  {
-    derived().value() += other;
-    return derived();
-  }
-
-
-  inline const AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >
-  operator*(const Real& other) const
-  {
-    return AutoDiffScalar<typename CwiseUnaryOp<bind2nd_op<scalar_product_op<Scalar,Real> >, DerType>::Type >(
-      derived().value() * other,
-      derived().derivatives() * other);
-  }
-
-  friend inline const AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >
-  operator*(const Real& other, const AutoDiffScalar<_DerType>& a)
-  {
-    return AutoDiffScalar<typename CwiseUnaryOp<bind1st_op<scalar_product_op<Real,Scalar> >, DerType>::Type >(
-      a.value() * other,
-      a.derivatives() * other);
-  }
-
-  inline AutoDiffScalar<_DerType>& operator*=(const Scalar& other)
-  {
-    *this = *this * other;
-    return derived();
-  }
-};
-
-template<typename _DerType>
-struct auto_diff_special_op<_DerType, false>
-{
-  void operator*() const;
-  void operator-() const;
-  void operator+() const;
-};
-
-template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols, typename B>
-struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>, B> {
-  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
-  static void run(A& a, B& b) {
-    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
-    {
-      a.resize(b.size());
-      a.setZero();
-    }
-  }
-};
-
-template<typename A, typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
-struct make_coherent_impl<A, Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
-  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
-  static void run(A& a, B& b) {
-    if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
-    {
-      b.resize(a.size());
-      b.setZero();
-    }
-  }
-};
-
-template<typename A_Scalar, int A_Rows, int A_Cols, int A_Options, int A_MaxRows, int A_MaxCols,
-         typename B_Scalar, int B_Rows, int B_Cols, int B_Options, int B_MaxRows, int B_MaxCols>
-struct make_coherent_impl<Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols>,
-                             Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> > {
-  typedef Matrix<A_Scalar, A_Rows, A_Cols, A_Options, A_MaxRows, A_MaxCols> A;
-  typedef Matrix<B_Scalar, B_Rows, B_Cols, B_Options, B_MaxRows, B_MaxCols> B;
-  static void run(A& a, B& b) {
-    if((A_Rows==Dynamic || A_Cols==Dynamic) && (a.size()==0))
-    {
-      a.resize(b.size());
-      a.setZero();
-    }
-    else if((B_Rows==Dynamic || B_Cols==Dynamic) && (b.size()==0))
-    {
-      b.resize(a.size());
-      b.setZero();
-    }
-  }
-};
-
-} // end namespace internal
-
-template<typename DerType, typename BinOp>
-struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,typename DerType::Scalar,BinOp>
-{
-  typedef AutoDiffScalar<DerType> ReturnType;
-};
-
-template<typename DerType, typename BinOp>
-struct ScalarBinaryOpTraits<typename DerType::Scalar,AutoDiffScalar<DerType>, BinOp>
-{
-  typedef AutoDiffScalar<DerType> ReturnType;
-};
-
-
-// The following is an attempt to let Eigen's known about expression template, but that's more tricky!
-
-// template<typename DerType, typename BinOp>
-// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType>,AutoDiffScalar<DerType>, BinOp>
-// {
-//   enum { Defined = 1 };
-//   typedef AutoDiffScalar<typename DerType::PlainObject> ReturnType;
-// };
-//
-// template<typename DerType1,typename DerType2, typename BinOp>
-// struct ScalarBinaryOpTraits<AutoDiffScalar<DerType1>,AutoDiffScalar<DerType2>, BinOp>
-// {
-//   enum { Defined = 1 };//internal::is_same<typename DerType1::Scalar,typename DerType2::Scalar>::value };
-//   typedef AutoDiffScalar<typename DerType1::PlainObject> ReturnType;
-// };
-
-#define EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(FUNC,CODE) \
-  template<typename DerType> \
-  inline const Eigen::AutoDiffScalar< \
-  EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename Eigen::internal::remove_all<DerType>::type, typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar, product) > \
-  FUNC(const Eigen::AutoDiffScalar<DerType>& x) { \
-    using namespace Eigen; \
-    typedef typename Eigen::internal::traits<typename Eigen::internal::remove_all<DerType>::type>::Scalar Scalar; \
-    EIGEN_UNUSED_VARIABLE(sizeof(Scalar)); \
-    CODE; \
-  }
-
-template<typename DerType>
-inline const AutoDiffScalar<DerType>& conj(const AutoDiffScalar<DerType>& x)  { return x; }
-template<typename DerType>
-inline const AutoDiffScalar<DerType>& real(const AutoDiffScalar<DerType>& x)  { return x; }
-template<typename DerType>
-inline typename DerType::Scalar imag(const AutoDiffScalar<DerType>&)    { return 0.; }
-template<typename DerType, typename T>
-inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const T& y) {
-  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
-  return (x <= y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const T& y) {
-  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
-  return (x >= y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const T& x, const AutoDiffScalar<DerType>& y) {
-  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
-  return (x < y ? ADS(x) : ADS(y));
-}
-template<typename DerType, typename T>
-inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const T& x, const AutoDiffScalar<DerType>& y) {
-  typedef AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> ADS;
-  return (x > y ? ADS(x) : ADS(y));
-}
-template<typename DerType>
-inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (min)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
-  return (x.value() < y.value() ? x : y);
-}
-template<typename DerType>
-inline AutoDiffScalar<typename Eigen::internal::remove_all<DerType>::type::PlainObject> (max)(const AutoDiffScalar<DerType>& x, const AutoDiffScalar<DerType>& y) {
-  return (x.value() >= y.value() ? x : y);
-}
-
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs,
-  using std::abs;
-  return Eigen::MakeAutoDiffScalar(abs(x.value()), x.derivatives() * (x.value()<0 ? -1 : 1) );)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(abs2,
-  using numext::abs2;
-  return Eigen::MakeAutoDiffScalar(abs2(x.value()), x.derivatives() * (Scalar(2)*x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sqrt,
-  using std::sqrt;
-  Scalar sqrtx = sqrt(x.value());
-  return Eigen::MakeAutoDiffScalar(sqrtx,x.derivatives() * (Scalar(0.5) / sqrtx));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cos,
-  using std::cos;
-  using std::sin;
-  return Eigen::MakeAutoDiffScalar(cos(x.value()), x.derivatives() * (-sin(x.value())));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sin,
-  using std::sin;
-  using std::cos;
-  return Eigen::MakeAutoDiffScalar(sin(x.value()),x.derivatives() * cos(x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(exp,
-  using std::exp;
-  Scalar expx = exp(x.value());
-  return Eigen::MakeAutoDiffScalar(expx,x.derivatives() * expx);)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(log,
-  using std::log;
-  return Eigen::MakeAutoDiffScalar(log(x.value()),x.derivatives() * (Scalar(1)/x.value()));)
-
-template<typename DerType>
-inline const Eigen::AutoDiffScalar<
-EIGEN_EXPR_BINARYOP_SCALAR_RETURN_TYPE(typename internal::remove_all<DerType>::type,typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar,product) >
-pow(const Eigen::AutoDiffScalar<DerType> &x, const typename internal::traits<typename internal::remove_all<DerType>::type>::Scalar &y)
-{
-  using namespace Eigen;
-  using std::pow;
-  return Eigen::MakeAutoDiffScalar(pow(x.value(),y), x.derivatives() * (y * pow(x.value(),y-1)));
-}
-
-
-template<typename DerTypeA,typename DerTypeB>
-inline const AutoDiffScalar<Matrix<typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar,Dynamic,1> >
-atan2(const AutoDiffScalar<DerTypeA>& a, const AutoDiffScalar<DerTypeB>& b)
-{
-  using std::atan2;
-  typedef typename internal::traits<typename internal::remove_all<DerTypeA>::type>::Scalar Scalar;
-  typedef AutoDiffScalar<Matrix<Scalar,Dynamic,1> > PlainADS;
-  PlainADS ret;
-  ret.value() = atan2(a.value(), b.value());
-  
-  Scalar squared_hypot = a.value() * a.value() + b.value() * b.value();
-  
-  // if (squared_hypot==0) the derivation is undefined and the following results in a NaN:
-  ret.derivatives() = (a.derivatives() * b.value() - a.value() * b.derivatives()) / squared_hypot;
-
-  return ret;
-}
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tan,
-  using std::tan;
-  using std::cos;
-  return Eigen::MakeAutoDiffScalar(tan(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cos(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(asin,
-  using std::sqrt;
-  using std::asin;
-  return Eigen::MakeAutoDiffScalar(asin(x.value()),x.derivatives() * (Scalar(1)/sqrt(1-numext::abs2(x.value()))));)
-  
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(acos,
-  using std::sqrt;
-  using std::acos;
-  return Eigen::MakeAutoDiffScalar(acos(x.value()),x.derivatives() * (Scalar(-1)/sqrt(1-numext::abs2(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(tanh,
-  using std::cosh;
-  using std::tanh;
-  return Eigen::MakeAutoDiffScalar(tanh(x.value()),x.derivatives() * (Scalar(1)/numext::abs2(cosh(x.value()))));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(sinh,
-  using std::sinh;
-  using std::cosh;
-  return Eigen::MakeAutoDiffScalar(sinh(x.value()),x.derivatives() * cosh(x.value()));)
-
-EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY(cosh,
-  using std::sinh;
-  using std::cosh;
-  return Eigen::MakeAutoDiffScalar(cosh(x.value()),x.derivatives() * sinh(x.value()));)
-
-#undef EIGEN_AUTODIFF_DECLARE_GLOBAL_UNARY
-
-template<typename DerType> struct NumTraits<AutoDiffScalar<DerType> >
-  : NumTraits< typename NumTraits<typename internal::remove_all<DerType>::type::Scalar>::Real >
-{
-  typedef typename internal::remove_all<DerType>::type DerTypeCleaned;
-  typedef AutoDiffScalar<Matrix<typename NumTraits<typename DerTypeCleaned::Scalar>::Real,DerTypeCleaned::RowsAtCompileTime,DerTypeCleaned::ColsAtCompileTime,
-                                0, DerTypeCleaned::MaxRowsAtCompileTime, DerTypeCleaned::MaxColsAtCompileTime> > Real;
-  typedef AutoDiffScalar<DerType> NonInteger;
-  typedef AutoDiffScalar<DerType> Nested;
-  typedef typename NumTraits<typename DerTypeCleaned::Scalar>::Literal Literal;
-  enum{
-    RequireInitialization = 1
-  };
-};
-
-}
-
-namespace std {
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T> >
-  : public numeric_limits<typename T::Scalar> {};
-
-}  // namespace std
-
-#endif // EIGEN_AUTODIFF_SCALAR_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
deleted file mode 100644
index e5ebbcf..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h
+++ /dev/null
@@ -1,442 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009, 2010, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
-// Copyright (C) 2011, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIX_EXPONENTIAL
-#define EIGEN_MATRIX_EXPONENTIAL
-
-#include "StemFunction.h"
-
-namespace Eigen {
-namespace internal {
-
-/** \brief Scaling operator.
- *
- * This struct is used by CwiseUnaryOp to scale a matrix by \f$ 2^{-s} \f$.
- */
-template <typename RealScalar>
-struct MatrixExponentialScalingOp
-{
-  /** \brief Constructor.
-   *
-   * \param[in] squarings  The integer \f$ s \f$ in this document.
-   */
-  MatrixExponentialScalingOp(int squarings) : m_squarings(squarings) { }
-
-
-  /** \brief Scale a matrix coefficient.
-   *
-   * \param[in,out] x  The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
-   */
-  inline const RealScalar operator() (const RealScalar& x) const
-  {
-    using std::ldexp;
-    return ldexp(x, -m_squarings);
-  }
-
-  typedef std::complex<RealScalar> ComplexScalar;
-
-  /** \brief Scale a matrix coefficient.
-   *
-   * \param[in,out] x  The scalar to be scaled, becoming \f$ 2^{-s} x \f$.
-   */
-  inline const ComplexScalar operator() (const ComplexScalar& x) const
-  {
-    using std::ldexp;
-    return ComplexScalar(ldexp(x.real(), -m_squarings), ldexp(x.imag(), -m_squarings));
-  }
-
-  private:
-    int m_squarings;
-};
-
-/** \brief Compute the (3,3)-Pad&eacute; approximant to the exponential.
- *
- *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
- *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
- */
-template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade3(const MatA& A, MatU& U, MatV& V)
-{
-  typedef typename MatA::PlainObject MatrixType;
-  typedef typename NumTraits<typename traits<MatA>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {120.L, 60.L, 12.L, 1.L};
-  const MatrixType A2 = A * A;
-  const MatrixType tmp = b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
-  U.noalias() = A * tmp;
-  V = b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
-}
-
-/** \brief Compute the (5,5)-Pad&eacute; approximant to the exponential.
- *
- *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
- *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
- */
-template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade5(const MatA& A, MatU& U, MatV& V)
-{
-  typedef typename MatA::PlainObject MatrixType;
-  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {30240.L, 15120.L, 3360.L, 420.L, 30.L, 1.L};
-  const MatrixType A2 = A * A;
-  const MatrixType A4 = A2 * A2;
-  const MatrixType tmp = b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
-  U.noalias() = A * tmp;
-  V = b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
-}
-
-/** \brief Compute the (7,7)-Pad&eacute; approximant to the exponential.
- *
- *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
- *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
- */
-template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade7(const MatA& A, MatU& U, MatV& V)
-{
-  typedef typename MatA::PlainObject MatrixType;
-  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {17297280.L, 8648640.L, 1995840.L, 277200.L, 25200.L, 1512.L, 56.L, 1.L};
-  const MatrixType A2 = A * A;
-  const MatrixType A4 = A2 * A2;
-  const MatrixType A6 = A4 * A2;
-  const MatrixType tmp = b[7] * A6 + b[5] * A4 + b[3] * A2 
-    + b[1] * MatrixType::Identity(A.rows(), A.cols());
-  U.noalias() = A * tmp;
-  V = b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
-
-}
-
-/** \brief Compute the (9,9)-Pad&eacute; approximant to the exponential.
- *
- *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
- *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
- */
-template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade9(const MatA& A, MatU& U, MatV& V)
-{
-  typedef typename MatA::PlainObject MatrixType;
-  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {17643225600.L, 8821612800.L, 2075673600.L, 302702400.L, 30270240.L,
-                          2162160.L, 110880.L, 3960.L, 90.L, 1.L};
-  const MatrixType A2 = A * A;
-  const MatrixType A4 = A2 * A2;
-  const MatrixType A6 = A4 * A2;
-  const MatrixType A8 = A6 * A2;
-  const MatrixType tmp = b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 
-    + b[1] * MatrixType::Identity(A.rows(), A.cols());
-  U.noalias() = A * tmp;
-  V = b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
-}
-
-/** \brief Compute the (13,13)-Pad&eacute; approximant to the exponential.
- *
- *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
- *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
- */
-template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade13(const MatA& A, MatU& U, MatV& V)
-{
-  typedef typename MatA::PlainObject MatrixType;
-  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {64764752532480000.L, 32382376266240000.L, 7771770303897600.L,
-                          1187353796428800.L, 129060195264000.L, 10559470521600.L, 670442572800.L,
-                          33522128640.L, 1323241920.L, 40840800.L, 960960.L, 16380.L, 182.L, 1.L};
-  const MatrixType A2 = A * A;
-  const MatrixType A4 = A2 * A2;
-  const MatrixType A6 = A4 * A2;
-  V = b[13] * A6 + b[11] * A4 + b[9] * A2; // used for temporary storage
-  MatrixType tmp = A6 * V;
-  tmp += b[7] * A6 + b[5] * A4 + b[3] * A2 + b[1] * MatrixType::Identity(A.rows(), A.cols());
-  U.noalias() = A * tmp;
-  tmp = b[12] * A6 + b[10] * A4 + b[8] * A2;
-  V.noalias() = A6 * tmp;
-  V += b[6] * A6 + b[4] * A4 + b[2] * A2 + b[0] * MatrixType::Identity(A.rows(), A.cols());
-}
-
-/** \brief Compute the (17,17)-Pad&eacute; approximant to the exponential.
- *
- *  After exit, \f$ (V+U)(V-U)^{-1} \f$ is the Pad&eacute;
- *  approximant of \f$ \exp(A) \f$ around \f$ A = 0 \f$.
- *
- *  This function activates only if your long double is double-double or quadruple.
- */
-#if LDBL_MANT_DIG > 64
-template <typename MatA, typename MatU, typename MatV>
-void matrix_exp_pade17(const MatA& A, MatU& U, MatV& V)
-{
-  typedef typename MatA::PlainObject MatrixType;
-  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  const RealScalar b[] = {830034394580628357120000.L, 415017197290314178560000.L,
-                          100610229646136770560000.L, 15720348382208870400000.L,
-                          1774878043152614400000.L, 153822763739893248000.L, 10608466464820224000.L,
-                          595373117923584000.L, 27563570274240000.L, 1060137318240000.L,
-                          33924394183680.L, 899510451840.L, 19554575040.L, 341863200.L, 4651200.L,
-                          46512.L, 306.L, 1.L};
-  const MatrixType A2 = A * A;
-  const MatrixType A4 = A2 * A2;
-  const MatrixType A6 = A4 * A2;
-  const MatrixType A8 = A4 * A4;
-  V = b[17] * A8 + b[15] * A6 + b[13] * A4 + b[11] * A2; // used for temporary storage
-  MatrixType tmp = A8 * V;
-  tmp += b[9] * A8 + b[7] * A6 + b[5] * A4 + b[3] * A2 
-    + b[1] * MatrixType::Identity(A.rows(), A.cols());
-  U.noalias() = A * tmp;
-  tmp = b[16] * A8 + b[14] * A6 + b[12] * A4 + b[10] * A2;
-  V.noalias() = tmp * A8;
-  V += b[8] * A8 + b[6] * A6 + b[4] * A4 + b[2] * A2 
-    + b[0] * MatrixType::Identity(A.rows(), A.cols());
-}
-#endif
-
-template <typename MatrixType, typename RealScalar = typename NumTraits<typename traits<MatrixType>::Scalar>::Real>
-struct matrix_exp_computeUV
-{
-  /** \brief Compute Pad&eacute; approximant to the exponential.
-    *
-    * Computes \c U, \c V and \c squarings such that \f$ (V+U)(V-U)^{-1} \f$ is a Pad&eacute;
-    * approximant of \f$ \exp(2^{-\mbox{squarings}}M) \f$ around \f$ M = 0 \f$, where \f$ M \f$
-    * denotes the matrix \c arg. The degree of the Pad&eacute; approximant and the value of squarings
-    * are chosen such that the approximation error is no more than the round-off error.
-    */
-  static void run(const MatrixType& arg, MatrixType& U, MatrixType& V, int& squarings);
-};
-
-template <typename MatrixType>
-struct matrix_exp_computeUV<MatrixType, float>
-{
-  template <typename ArgType>
-  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
-  {
-    using std::frexp;
-    using std::pow;
-    const float l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
-    squarings = 0;
-    if (l1norm < 4.258730016922831e-001f) {
-      matrix_exp_pade3(arg, U, V);
-    } else if (l1norm < 1.880152677804762e+000f) {
-      matrix_exp_pade5(arg, U, V);
-    } else {
-      const float maxnorm = 3.925724783138660f;
-      frexp(l1norm / maxnorm, &squarings);
-      if (squarings < 0) squarings = 0;
-      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<float>(squarings));
-      matrix_exp_pade7(A, U, V);
-    }
-  }
-};
-
-template <typename MatrixType>
-struct matrix_exp_computeUV<MatrixType, double>
-{
-  typedef typename NumTraits<typename traits<MatrixType>::Scalar>::Real RealScalar;
-  template <typename ArgType>
-  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
-  {
-    using std::frexp;
-    using std::pow;
-    const RealScalar l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
-    squarings = 0;
-    if (l1norm < 1.495585217958292e-002) {
-      matrix_exp_pade3(arg, U, V);
-    } else if (l1norm < 2.539398330063230e-001) {
-      matrix_exp_pade5(arg, U, V);
-    } else if (l1norm < 9.504178996162932e-001) {
-      matrix_exp_pade7(arg, U, V);
-    } else if (l1norm < 2.097847961257068e+000) {
-      matrix_exp_pade9(arg, U, V);
-    } else {
-      const RealScalar maxnorm = 5.371920351148152;
-      frexp(l1norm / maxnorm, &squarings);
-      if (squarings < 0) squarings = 0;
-      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<RealScalar>(squarings));
-      matrix_exp_pade13(A, U, V);
-    }
-  }
-};
-  
-template <typename MatrixType>
-struct matrix_exp_computeUV<MatrixType, long double>
-{
-  template <typename ArgType>
-  static void run(const ArgType& arg, MatrixType& U, MatrixType& V, int& squarings)
-  {
-#if   LDBL_MANT_DIG == 53   // double precision
-    matrix_exp_computeUV<MatrixType, double>::run(arg, U, V, squarings);
-  
-#else
-  
-    using std::frexp;
-    using std::pow;
-    const long double l1norm = arg.cwiseAbs().colwise().sum().maxCoeff();
-    squarings = 0;
-  
-#if LDBL_MANT_DIG <= 64   // extended precision
-  
-    if (l1norm < 4.1968497232266989671e-003L) {
-      matrix_exp_pade3(arg, U, V);
-    } else if (l1norm < 1.1848116734693823091e-001L) {
-      matrix_exp_pade5(arg, U, V);
-    } else if (l1norm < 5.5170388480686700274e-001L) {
-      matrix_exp_pade7(arg, U, V);
-    } else if (l1norm < 1.3759868875587845383e+000L) {
-      matrix_exp_pade9(arg, U, V);
-    } else {
-      const long double maxnorm = 4.0246098906697353063L;
-      frexp(l1norm / maxnorm, &squarings);
-      if (squarings < 0) squarings = 0;
-      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
-      matrix_exp_pade13(A, U, V);
-    }
-  
-#elif LDBL_MANT_DIG <= 106  // double-double
-  
-    if (l1norm < 3.2787892205607026992947488108213e-005L) {
-      matrix_exp_pade3(arg, U, V);
-    } else if (l1norm < 6.4467025060072760084130906076332e-003L) {
-      matrix_exp_pade5(arg, U, V);
-    } else if (l1norm < 6.8988028496595374751374122881143e-002L) {
-      matrix_exp_pade7(arg, U, V);
-    } else if (l1norm < 2.7339737518502231741495857201670e-001L) {
-      matrix_exp_pade9(arg, U, V);
-    } else if (l1norm < 1.3203382096514474905666448850278e+000L) {
-      matrix_exp_pade13(arg, U, V);
-    } else {
-      const long double maxnorm = 3.2579440895405400856599663723517L;
-      frexp(l1norm / maxnorm, &squarings);
-      if (squarings < 0) squarings = 0;
-      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
-      matrix_exp_pade17(A, U, V);
-    }
-  
-#elif LDBL_MANT_DIG <= 112  // quadruple precison
-  
-    if (l1norm < 1.639394610288918690547467954466970e-005L) {
-      matrix_exp_pade3(arg, U, V);
-    } else if (l1norm < 4.253237712165275566025884344433009e-003L) {
-      matrix_exp_pade5(arg, U, V);
-    } else if (l1norm < 5.125804063165764409885122032933142e-002L) {
-      matrix_exp_pade7(arg, U, V);
-    } else if (l1norm < 2.170000765161155195453205651889853e-001L) {
-      matrix_exp_pade9(arg, U, V);
-    } else if (l1norm < 1.125358383453143065081397882891878e+000L) {
-      matrix_exp_pade13(arg, U, V);
-    } else {
-      const long double maxnorm = 2.884233277829519311757165057717815L;
-      frexp(l1norm / maxnorm, &squarings);
-      if (squarings < 0) squarings = 0;
-      MatrixType A = arg.unaryExpr(MatrixExponentialScalingOp<long double>(squarings));
-      matrix_exp_pade17(A, U, V);
-    }
-  
-#else
-  
-    // this case should be handled in compute()
-    eigen_assert(false && "Bug in MatrixExponential"); 
-  
-#endif
-#endif  // LDBL_MANT_DIG
-  }
-};
-
-template<typename T> struct is_exp_known_type : false_type {};
-template<> struct is_exp_known_type<float> : true_type {};
-template<> struct is_exp_known_type<double> : true_type {};
-#if LDBL_MANT_DIG <= 112
-template<> struct is_exp_known_type<long double> : true_type {};
-#endif
-
-template <typename ArgType, typename ResultType>
-void matrix_exp_compute(const ArgType& arg, ResultType &result, true_type) // natively supported scalar type
-{
-  typedef typename ArgType::PlainObject MatrixType;
-  MatrixType U, V;
-  int squarings;
-  matrix_exp_computeUV<MatrixType>::run(arg, U, V, squarings); // Pade approximant is (U+V) / (-U+V)
-  MatrixType numer = U + V;
-  MatrixType denom = -U + V;
-  result = denom.partialPivLu().solve(numer);
-  for (int i=0; i<squarings; i++)
-    result *= result;   // undo scaling by repeated squaring
-}
-
-
-/* Computes the matrix exponential
- *
- * \param arg    argument of matrix exponential (should be plain object)
- * \param result variable in which result will be stored
- */
-template <typename ArgType, typename ResultType>
-void matrix_exp_compute(const ArgType& arg, ResultType &result, false_type) // default
-{
-  typedef typename ArgType::PlainObject MatrixType;
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  typedef typename std::complex<RealScalar> ComplexScalar;
-  result = arg.matrixFunction(internal::stem_function_exp<ComplexScalar>);
-}
-
-} // end namespace Eigen::internal
-
-/** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix exponential of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix exponential.
-  *
-  * This class holds the argument to the matrix exponential until it is assigned or evaluated for
-  * some other reason (so the argument should not be changed in the meantime). It is the return type
-  * of MatrixBase::exp() and most of the time this is the only way it is used.
-  */
-template<typename Derived> struct MatrixExponentialReturnValue
-: public ReturnByValue<MatrixExponentialReturnValue<Derived> >
-{
-    typedef typename Derived::Index Index;
-  public:
-    /** \brief Constructor.
-      *
-      * \param src %Matrix (expression) forming the argument of the matrix exponential.
-      */
-    MatrixExponentialReturnValue(const Derived& src) : m_src(src) { }
-
-    /** \brief Compute the matrix exponential.
-      *
-      * \param result the matrix exponential of \p src in the constructor.
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      const typename internal::nested_eval<Derived, 10>::type tmp(m_src);
-      internal::matrix_exp_compute(tmp, result, internal::is_exp_known_type<typename Derived::Scalar>());
-    }
-
-    Index rows() const { return m_src.rows(); }
-    Index cols() const { return m_src.cols(); }
-
-  protected:
-    const typename internal::ref_selector<Derived>::type m_src;
-};
-
-namespace internal {
-template<typename Derived>
-struct traits<MatrixExponentialReturnValue<Derived> >
-{
-  typedef typename Derived::PlainObject ReturnType;
-};
-}
-
-template <typename Derived>
-const MatrixExponentialReturnValue<Derived> MatrixBase<Derived>::exp() const
-{
-  eigen_assert(rows() == cols());
-  return MatrixExponentialReturnValue<Derived>(derived());
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_MATRIX_EXPONENTIAL
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
deleted file mode 100644
index 3df8239..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h
+++ /dev/null
@@ -1,580 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2009-2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIX_FUNCTION_H
-#define EIGEN_MATRIX_FUNCTION_H
-
-#include "StemFunction.h"
-
-
-namespace Eigen { 
-
-namespace internal {
-
-/** \brief Maximum distance allowed between eigenvalues to be considered "close". */
-static const float matrix_function_separation = 0.1f;
-
-/** \ingroup MatrixFunctions_Module
-  * \class MatrixFunctionAtomic
-  * \brief Helper class for computing matrix functions of atomic matrices.
-  *
-  * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
-  */
-template <typename MatrixType>
-class MatrixFunctionAtomic 
-{
-  public:
-
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename stem_function<Scalar>::type StemFunction;
-
-    /** \brief Constructor
-      * \param[in]  f  matrix function to compute.
-      */
-    MatrixFunctionAtomic(StemFunction f) : m_f(f) { }
-
-    /** \brief Compute matrix function of atomic matrix
-      * \param[in]  A  argument of matrix function, should be upper triangular and atomic
-      * \returns  f(A), the matrix function evaluated at the given matrix
-      */
-    MatrixType compute(const MatrixType& A);
-
-  private:
-    StemFunction* m_f;
-};
-
-template <typename MatrixType>
-typename NumTraits<typename MatrixType::Scalar>::Real matrix_function_compute_mu(const MatrixType& A)
-{
-  typedef typename plain_col_type<MatrixType>::type VectorType;
-  typename MatrixType::Index rows = A.rows();
-  const MatrixType N = MatrixType::Identity(rows, rows) - A;
-  VectorType e = VectorType::Ones(rows);
-  N.template triangularView<Upper>().solveInPlace(e);
-  return e.cwiseAbs().maxCoeff();
-}
-
-template <typename MatrixType>
-MatrixType MatrixFunctionAtomic<MatrixType>::compute(const MatrixType& A)
-{
-  // TODO: Use that A is upper triangular
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  typedef typename MatrixType::Index Index;
-  Index rows = A.rows();
-  Scalar avgEival = A.trace() / Scalar(RealScalar(rows));
-  MatrixType Ashifted = A - avgEival * MatrixType::Identity(rows, rows);
-  RealScalar mu = matrix_function_compute_mu(Ashifted);
-  MatrixType F = m_f(avgEival, 0) * MatrixType::Identity(rows, rows);
-  MatrixType P = Ashifted;
-  MatrixType Fincr;
-  for (Index s = 1; s < 1.1 * rows + 10; s++) { // upper limit is fairly arbitrary
-    Fincr = m_f(avgEival, static_cast<int>(s)) * P;
-    F += Fincr;
-    P = Scalar(RealScalar(1.0/(s + 1))) * P * Ashifted;
-
-    // test whether Taylor series converged
-    const RealScalar F_norm = F.cwiseAbs().rowwise().sum().maxCoeff();
-    const RealScalar Fincr_norm = Fincr.cwiseAbs().rowwise().sum().maxCoeff();
-    if (Fincr_norm < NumTraits<Scalar>::epsilon() * F_norm) {
-      RealScalar delta = 0;
-      RealScalar rfactorial = 1;
-      for (Index r = 0; r < rows; r++) {
-        RealScalar mx = 0;
-        for (Index i = 0; i < rows; i++)
-          mx = (std::max)(mx, std::abs(m_f(Ashifted(i, i) + avgEival, static_cast<int>(s+r))));
-        if (r != 0)
-          rfactorial *= RealScalar(r);
-        delta = (std::max)(delta, mx / rfactorial);
-      }
-      const RealScalar P_norm = P.cwiseAbs().rowwise().sum().maxCoeff();
-      if (mu * delta * P_norm < NumTraits<Scalar>::epsilon() * F_norm) // series converged
-        break;
-    }
-  }
-  return F;
-}
-
-/** \brief Find cluster in \p clusters containing some value 
-  * \param[in] key Value to find
-  * \returns Iterator to cluster containing \p key, or \c clusters.end() if no cluster in \p m_clusters
-  * contains \p key.
-  */
-template <typename Index, typename ListOfClusters>
-typename ListOfClusters::iterator matrix_function_find_cluster(Index key, ListOfClusters& clusters)
-{
-  typename std::list<Index>::iterator j;
-  for (typename ListOfClusters::iterator i = clusters.begin(); i != clusters.end(); ++i) {
-    j = std::find(i->begin(), i->end(), key);
-    if (j != i->end())
-      return i;
-  }
-  return clusters.end();
-}
-
-/** \brief Partition eigenvalues in clusters of ei'vals close to each other
-  * 
-  * \param[in]  eivals    Eigenvalues
-  * \param[out] clusters  Resulting partition of eigenvalues
-  *
-  * The partition satisfies the following two properties:
-  * # Any eigenvalue in a certain cluster is at most matrix_function_separation() away from another eigenvalue
-  *   in the same cluster.
-  * # The distance between two eigenvalues in different clusters is more than matrix_function_separation().  
-  * The implementation follows Algorithm 4.1 in the paper of Davies and Higham.
-  */
-template <typename EivalsType, typename Cluster>
-void matrix_function_partition_eigenvalues(const EivalsType& eivals, std::list<Cluster>& clusters)
-{
-  typedef typename EivalsType::Index Index;
-  typedef typename EivalsType::RealScalar RealScalar;
-  for (Index i=0; i<eivals.rows(); ++i) {
-    // Find cluster containing i-th ei'val, adding a new cluster if necessary
-    typename std::list<Cluster>::iterator qi = matrix_function_find_cluster(i, clusters);
-    if (qi == clusters.end()) {
-      Cluster l;
-      l.push_back(i);
-      clusters.push_back(l);
-      qi = clusters.end();
-      --qi;
-    }
-
-    // Look for other element to add to the set
-    for (Index j=i+1; j<eivals.rows(); ++j) {
-      if (abs(eivals(j) - eivals(i)) <= RealScalar(matrix_function_separation)
-          && std::find(qi->begin(), qi->end(), j) == qi->end()) {
-        typename std::list<Cluster>::iterator qj = matrix_function_find_cluster(j, clusters);
-        if (qj == clusters.end()) {
-          qi->push_back(j);
-        } else {
-          qi->insert(qi->end(), qj->begin(), qj->end());
-          clusters.erase(qj);
-        }
-      }
-    }
-  }
-}
-
-/** \brief Compute size of each cluster given a partitioning */
-template <typename ListOfClusters, typename Index>
-void matrix_function_compute_cluster_size(const ListOfClusters& clusters, Matrix<Index, Dynamic, 1>& clusterSize)
-{
-  const Index numClusters = static_cast<Index>(clusters.size());
-  clusterSize.setZero(numClusters);
-  Index clusterIndex = 0;
-  for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
-    clusterSize[clusterIndex] = cluster->size();
-    ++clusterIndex;
-  }
-}
-
-/** \brief Compute start of each block using clusterSize */
-template <typename VectorType>
-void matrix_function_compute_block_start(const VectorType& clusterSize, VectorType& blockStart)
-{
-  blockStart.resize(clusterSize.rows());
-  blockStart(0) = 0;
-  for (typename VectorType::Index i = 1; i < clusterSize.rows(); i++) {
-    blockStart(i) = blockStart(i-1) + clusterSize(i-1);
-  }
-}
-
-/** \brief Compute mapping of eigenvalue indices to cluster indices */
-template <typename EivalsType, typename ListOfClusters, typename VectorType>
-void matrix_function_compute_map(const EivalsType& eivals, const ListOfClusters& clusters, VectorType& eivalToCluster)
-{
-  typedef typename EivalsType::Index Index;
-  eivalToCluster.resize(eivals.rows());
-  Index clusterIndex = 0;
-  for (typename ListOfClusters::const_iterator cluster = clusters.begin(); cluster != clusters.end(); ++cluster) {
-    for (Index i = 0; i < eivals.rows(); ++i) {
-      if (std::find(cluster->begin(), cluster->end(), i) != cluster->end()) {
-        eivalToCluster[i] = clusterIndex;
-      }
-    }
-    ++clusterIndex;
-  }
-}
-
-/** \brief Compute permutation which groups ei'vals in same cluster together */
-template <typename DynVectorType, typename VectorType>
-void matrix_function_compute_permutation(const DynVectorType& blockStart, const DynVectorType& eivalToCluster, VectorType& permutation)
-{
-  typedef typename VectorType::Index Index;
-  DynVectorType indexNextEntry = blockStart;
-  permutation.resize(eivalToCluster.rows());
-  for (Index i = 0; i < eivalToCluster.rows(); i++) {
-    Index cluster = eivalToCluster[i];
-    permutation[i] = indexNextEntry[cluster];
-    ++indexNextEntry[cluster];
-  }
-}  
-
-/** \brief Permute Schur decomposition in U and T according to permutation */
-template <typename VectorType, typename MatrixType>
-void matrix_function_permute_schur(VectorType& permutation, MatrixType& U, MatrixType& T)
-{
-  typedef typename VectorType::Index Index;
-  for (Index i = 0; i < permutation.rows() - 1; i++) {
-    Index j;
-    for (j = i; j < permutation.rows(); j++) {
-      if (permutation(j) == i) break;
-    }
-    eigen_assert(permutation(j) == i);
-    for (Index k = j-1; k >= i; k--) {
-      JacobiRotation<typename MatrixType::Scalar> rotation;
-      rotation.makeGivens(T(k, k+1), T(k+1, k+1) - T(k, k));
-      T.applyOnTheLeft(k, k+1, rotation.adjoint());
-      T.applyOnTheRight(k, k+1, rotation);
-      U.applyOnTheRight(k, k+1, rotation);
-      std::swap(permutation.coeffRef(k), permutation.coeffRef(k+1));
-    }
-  }
-}
-
-/** \brief Compute block diagonal part of matrix function.
-  *
-  * This routine computes the matrix function applied to the block diagonal part of \p T (which should be
-  * upper triangular), with the blocking given by \p blockStart and \p clusterSize. The matrix function of
-  * each diagonal block is computed by \p atomic. The off-diagonal parts of \p fT are set to zero.
-  */
-template <typename MatrixType, typename AtomicType, typename VectorType>
-void matrix_function_compute_block_atomic(const MatrixType& T, AtomicType& atomic, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
-{ 
-  fT.setZero(T.rows(), T.cols());
-  for (typename VectorType::Index i = 0; i < clusterSize.rows(); ++i) {
-    fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
-      = atomic.compute(T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i)));
-  }
-}
-
-/** \brief Solve a triangular Sylvester equation AX + XB = C 
-  *
-  * \param[in]  A  the matrix A; should be square and upper triangular
-  * \param[in]  B  the matrix B; should be square and upper triangular
-  * \param[in]  C  the matrix C; should have correct size.
-  *
-  * \returns the solution X.
-  *
-  * If A is m-by-m and B is n-by-n, then both C and X are m-by-n.  The (i,j)-th component of the Sylvester
-  * equation is
-  * \f[ 
-  *     \sum_{k=i}^m A_{ik} X_{kj} + \sum_{k=1}^j X_{ik} B_{kj} = C_{ij}. 
-  * \f]
-  * This can be re-arranged to yield:
-  * \f[ 
-  *     X_{ij} = \frac{1}{A_{ii} + B_{jj}} \Bigl( C_{ij}
-  *     - \sum_{k=i+1}^m A_{ik} X_{kj} - \sum_{k=1}^{j-1} X_{ik} B_{kj} \Bigr).
-  * \f]
-  * It is assumed that A and B are such that the numerator is never zero (otherwise the Sylvester equation
-  * does not have a unique solution). In that case, these equations can be evaluated in the order 
-  * \f$ i=m,\ldots,1 \f$ and \f$ j=1,\ldots,n \f$.
-  */
-template <typename MatrixType>
-MatrixType matrix_function_solve_triangular_sylvester(const MatrixType& A, const MatrixType& B, const MatrixType& C)
-{
-  eigen_assert(A.rows() == A.cols());
-  eigen_assert(A.isUpperTriangular());
-  eigen_assert(B.rows() == B.cols());
-  eigen_assert(B.isUpperTriangular());
-  eigen_assert(C.rows() == A.rows());
-  eigen_assert(C.cols() == B.rows());
-
-  typedef typename MatrixType::Index Index;
-  typedef typename MatrixType::Scalar Scalar;
-
-  Index m = A.rows();
-  Index n = B.rows();
-  MatrixType X(m, n);
-
-  for (Index i = m - 1; i >= 0; --i) {
-    for (Index j = 0; j < n; ++j) {
-
-      // Compute AX = \sum_{k=i+1}^m A_{ik} X_{kj}
-      Scalar AX;
-      if (i == m - 1) {
-	AX = 0; 
-      } else {
-	Matrix<Scalar,1,1> AXmatrix = A.row(i).tail(m-1-i) * X.col(j).tail(m-1-i);
-	AX = AXmatrix(0,0);
-      }
-
-      // Compute XB = \sum_{k=1}^{j-1} X_{ik} B_{kj}
-      Scalar XB;
-      if (j == 0) {
-	XB = 0; 
-      } else {
-	Matrix<Scalar,1,1> XBmatrix = X.row(i).head(j) * B.col(j).head(j);
-	XB = XBmatrix(0,0);
-      }
-
-      X(i,j) = (C(i,j) - AX - XB) / (A(i,i) + B(j,j));
-    }
-  }
-  return X;
-}
-
-/** \brief Compute part of matrix function above block diagonal.
-  *
-  * This routine completes the computation of \p fT, denoting a matrix function applied to the triangular
-  * matrix \p T. It assumes that the block diagonal part of \p fT has already been computed. The part below
-  * the diagonal is zero, because \p T is upper triangular.
-  */
-template <typename MatrixType, typename VectorType>
-void matrix_function_compute_above_diagonal(const MatrixType& T, const VectorType& blockStart, const VectorType& clusterSize, MatrixType& fT)
-{ 
-  typedef internal::traits<MatrixType> Traits;
-  typedef typename MatrixType::Scalar Scalar;
-  typedef typename MatrixType::Index Index;
-  static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
-  static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
-  static const int Options = MatrixType::Options;
-  typedef Matrix<Scalar, Dynamic, Dynamic, Options, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
-
-  for (Index k = 1; k < clusterSize.rows(); k++) {
-    for (Index i = 0; i < clusterSize.rows() - k; i++) {
-      // compute (i, i+k) block
-      DynMatrixType A = T.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i));
-      DynMatrixType B = -T.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
-      DynMatrixType C = fT.block(blockStart(i), blockStart(i), clusterSize(i), clusterSize(i))
-        * T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k));
-      C -= T.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
-        * fT.block(blockStart(i+k), blockStart(i+k), clusterSize(i+k), clusterSize(i+k));
-      for (Index m = i + 1; m < i + k; m++) {
-        C += fT.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
-          * T.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
-        C -= T.block(blockStart(i), blockStart(m), clusterSize(i), clusterSize(m))
-          * fT.block(blockStart(m), blockStart(i+k), clusterSize(m), clusterSize(i+k));
-      }
-      fT.block(blockStart(i), blockStart(i+k), clusterSize(i), clusterSize(i+k))
-        = matrix_function_solve_triangular_sylvester(A, B, C);
-    }
-  }
-}
-
-/** \ingroup MatrixFunctions_Module
-  * \brief Class for computing matrix functions.
-  * \tparam  MatrixType  type of the argument of the matrix function,
-  *                      expected to be an instantiation of the Matrix class template.
-  * \tparam  AtomicType  type for computing matrix function of atomic blocks.
-  * \tparam  IsComplex   used internally to select correct specialization.
-  *
-  * This class implements the Schur-Parlett algorithm for computing matrix functions. The spectrum of the
-  * matrix is divided in clustered of eigenvalues that lies close together. This class delegates the
-  * computation of the matrix function on every block corresponding to these clusters to an object of type
-  * \p AtomicType and uses these results to compute the matrix function of the whole matrix. The class
-  * \p AtomicType should have a \p compute() member function for computing the matrix function of a block.
-  *
-  * \sa class MatrixFunctionAtomic, class MatrixLogarithmAtomic
-  */
-template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
-struct matrix_function_compute
-{  
-    /** \brief Compute the matrix function.
-      *
-      * \param[in]  A       argument of matrix function, should be a square matrix.
-      * \param[in]  atomic  class for computing matrix function of atomic blocks.
-      * \param[out] result  the function \p f applied to \p A, as
-      * specified in the constructor.
-      *
-      * See MatrixBase::matrixFunction() for details on how this computation
-      * is implemented.
-      */
-    template <typename AtomicType, typename ResultType> 
-    static void run(const MatrixType& A, AtomicType& atomic, ResultType &result);    
-};
-
-/** \internal \ingroup MatrixFunctions_Module 
-  * \brief Partial specialization of MatrixFunction for real matrices
-  *
-  * This converts the real matrix to a complex matrix, compute the matrix function of that matrix, and then
-  * converts the result back to a real matrix.
-  */
-template <typename MatrixType>
-struct matrix_function_compute<MatrixType, 0>
-{  
-  template <typename MatA, typename AtomicType, typename ResultType>
-  static void run(const MatA& A, AtomicType& atomic, ResultType &result)
-  {
-    typedef internal::traits<MatrixType> Traits;
-    typedef typename Traits::Scalar Scalar;
-    static const int Rows = Traits::RowsAtCompileTime, Cols = Traits::ColsAtCompileTime;
-    static const int MaxRows = Traits::MaxRowsAtCompileTime, MaxCols = Traits::MaxColsAtCompileTime;
-
-    typedef std::complex<Scalar> ComplexScalar;
-    typedef Matrix<ComplexScalar, Rows, Cols, 0, MaxRows, MaxCols> ComplexMatrix;
-
-    ComplexMatrix CA = A.template cast<ComplexScalar>();
-    ComplexMatrix Cresult;
-    matrix_function_compute<ComplexMatrix>::run(CA, atomic, Cresult);
-    result = Cresult.real();
-  }
-};
-
-/** \internal \ingroup MatrixFunctions_Module 
-  * \brief Partial specialization of MatrixFunction for complex matrices
-  */
-template <typename MatrixType>
-struct matrix_function_compute<MatrixType, 1>
-{
-  template <typename MatA, typename AtomicType, typename ResultType>
-  static void run(const MatA& A, AtomicType& atomic, ResultType &result)
-  {
-    typedef internal::traits<MatrixType> Traits;
-    
-    // compute Schur decomposition of A
-    const ComplexSchur<MatrixType> schurOfA(A);  
-    MatrixType T = schurOfA.matrixT();
-    MatrixType U = schurOfA.matrixU();
-
-    // partition eigenvalues into clusters of ei'vals "close" to each other
-    std::list<std::list<Index> > clusters; 
-    matrix_function_partition_eigenvalues(T.diagonal(), clusters);
-
-    // compute size of each cluster
-    Matrix<Index, Dynamic, 1> clusterSize;
-    matrix_function_compute_cluster_size(clusters, clusterSize);
-
-    // blockStart[i] is row index at which block corresponding to i-th cluster starts 
-    Matrix<Index, Dynamic, 1> blockStart; 
-    matrix_function_compute_block_start(clusterSize, blockStart);
-
-    // compute map so that eivalToCluster[i] = j means that i-th ei'val is in j-th cluster 
-    Matrix<Index, Dynamic, 1> eivalToCluster;
-    matrix_function_compute_map(T.diagonal(), clusters, eivalToCluster);
-
-    // compute permutation which groups ei'vals in same cluster together 
-    Matrix<Index, Traits::RowsAtCompileTime, 1> permutation;
-    matrix_function_compute_permutation(blockStart, eivalToCluster, permutation);
-
-    // permute Schur decomposition
-    matrix_function_permute_schur(permutation, U, T);
-
-    // compute result
-    MatrixType fT; // matrix function applied to T
-    matrix_function_compute_block_atomic(T, atomic, blockStart, clusterSize, fT);
-    matrix_function_compute_above_diagonal(T, blockStart, clusterSize, fT);
-    result = U * (fT.template triangularView<Upper>() * U.adjoint());
-  }
-};
-
-} // end of namespace internal
-
-/** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix function of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix function.
-  *
-  * This class holds the argument to the matrix function until it is assigned or evaluated for some other
-  * reason (so the argument should not be changed in the meantime). It is the return type of
-  * matrixBase::matrixFunction() and related functions and most of the time this is the only way it is used.
-  */
-template<typename Derived> class MatrixFunctionReturnValue
-: public ReturnByValue<MatrixFunctionReturnValue<Derived> >
-{
-  public:
-    typedef typename Derived::Scalar Scalar;
-    typedef typename Derived::Index Index;
-    typedef typename internal::stem_function<Scalar>::type StemFunction;
-
-  protected:
-    typedef typename internal::ref_selector<Derived>::type DerivedNested;
-
-  public:
-
-    /** \brief Constructor.
-      *
-      * \param[in] A  %Matrix (expression) forming the argument of the matrix function.
-      * \param[in] f  Stem function for matrix function under consideration.
-      */
-    MatrixFunctionReturnValue(const Derived& A, StemFunction f) : m_A(A), m_f(f) { }
-
-    /** \brief Compute the matrix function.
-      *
-      * \param[out] result \p f applied to \p A, where \p f and \p A are as in the constructor.
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      typedef typename internal::nested_eval<Derived, 10>::type NestedEvalType;
-      typedef typename internal::remove_all<NestedEvalType>::type NestedEvalTypeClean;
-      typedef internal::traits<NestedEvalTypeClean> Traits;
-      static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
-      static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
-      typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
-      typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
-
-      typedef internal::MatrixFunctionAtomic<DynMatrixType> AtomicType;
-      AtomicType atomic(m_f);
-
-      internal::matrix_function_compute<typename NestedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
-    }
-
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
-
-  private:
-    const DerivedNested m_A;
-    StemFunction *m_f;
-};
-
-namespace internal {
-template<typename Derived>
-struct traits<MatrixFunctionReturnValue<Derived> >
-{
-  typedef typename Derived::PlainObject ReturnType;
-};
-}
-
-
-/********** MatrixBase methods **********/
-
-
-template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::matrixFunction(typename internal::stem_function<typename internal::traits<Derived>::Scalar>::type f) const
-{
-  eigen_assert(rows() == cols());
-  return MatrixFunctionReturnValue<Derived>(derived(), f);
-}
-
-template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sin() const
-{
-  eigen_assert(rows() == cols());
-  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
-  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sin<ComplexScalar>);
-}
-
-template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cos() const
-{
-  eigen_assert(rows() == cols());
-  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
-  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cos<ComplexScalar>);
-}
-
-template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::sinh() const
-{
-  eigen_assert(rows() == cols());
-  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
-  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_sinh<ComplexScalar>);
-}
-
-template <typename Derived>
-const MatrixFunctionReturnValue<Derived> MatrixBase<Derived>::cosh() const
-{
-  eigen_assert(rows() == cols());
-  typedef typename internal::stem_function<Scalar>::ComplexScalar ComplexScalar;
-  return MatrixFunctionReturnValue<Derived>(derived(), internal::stem_function_cosh<ComplexScalar>);
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_MATRIX_FUNCTION_H
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
deleted file mode 100644
index cf5fffa..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h
+++ /dev/null
@@ -1,373 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
-// Copyright (C) 2011 Chen-Pang He <jdh8@ms63.hinet.net>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIX_LOGARITHM
-#define EIGEN_MATRIX_LOGARITHM
-
-namespace Eigen { 
-
-namespace internal { 
-
-template <typename Scalar>
-struct matrix_log_min_pade_degree 
-{
-  static const int value = 3;
-};
-
-template <typename Scalar>
-struct matrix_log_max_pade_degree 
-{
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  static const int value = std::numeric_limits<RealScalar>::digits<= 24?  5:  // single precision
-                           std::numeric_limits<RealScalar>::digits<= 53?  7:  // double precision
-                           std::numeric_limits<RealScalar>::digits<= 64?  8:  // extended precision
-                           std::numeric_limits<RealScalar>::digits<=106? 10:  // double-double
-                                                                         11;  // quadruple precision
-};
-
-/** \brief Compute logarithm of 2x2 triangular matrix. */
-template <typename MatrixType>
-void matrix_log_compute_2x2(const MatrixType& A, MatrixType& result)
-{
-  typedef typename MatrixType::Scalar Scalar;
-  typedef typename MatrixType::RealScalar RealScalar;
-  using std::abs;
-  using std::ceil;
-  using std::imag;
-  using std::log;
-
-  Scalar logA00 = log(A(0,0));
-  Scalar logA11 = log(A(1,1));
-
-  result(0,0) = logA00;
-  result(1,0) = Scalar(0);
-  result(1,1) = logA11;
-
-  Scalar y = A(1,1) - A(0,0);
-  if (y==Scalar(0))
-  {
-    result(0,1) = A(0,1) / A(0,0);
-  }
-  else if ((abs(A(0,0)) < RealScalar(0.5)*abs(A(1,1))) || (abs(A(0,0)) > 2*abs(A(1,1))))
-  {
-    result(0,1) = A(0,1) * (logA11 - logA00) / y;
-  }
-  else
-  {
-    // computation in previous branch is inaccurate if A(1,1) \approx A(0,0)
-    int unwindingNumber = static_cast<int>(ceil((imag(logA11 - logA00) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI)));
-    result(0,1) = A(0,1) * (numext::log1p(y/A(0,0)) + Scalar(0,2*EIGEN_PI*unwindingNumber)) / y;
-  }
-}
-
-/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = float) */
-inline int matrix_log_get_pade_degree(float normTminusI)
-{
-  const float maxNormForPade[] = { 2.5111573934555054e-1 /* degree = 3 */ , 4.0535837411880493e-1,
-            5.3149729967117310e-1 };
-  const int minPadeDegree = matrix_log_min_pade_degree<float>::value;
-  const int maxPadeDegree = matrix_log_max_pade_degree<float>::value;
-  int degree = minPadeDegree;
-  for (; degree <= maxPadeDegree; ++degree) 
-    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
-      break;
-  return degree;
-}
-
-/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = double) */
-inline int matrix_log_get_pade_degree(double normTminusI)
-{
-  const double maxNormForPade[] = { 1.6206284795015624e-2 /* degree = 3 */ , 5.3873532631381171e-2,
-            1.1352802267628681e-1, 1.8662860613541288e-1, 2.642960831111435e-1 };
-  const int minPadeDegree = matrix_log_min_pade_degree<double>::value;
-  const int maxPadeDegree = matrix_log_max_pade_degree<double>::value;
-  int degree = minPadeDegree;
-  for (; degree <= maxPadeDegree; ++degree)
-    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
-      break;
-  return degree;
-}
-
-/* \brief Get suitable degree for Pade approximation. (specialized for RealScalar = long double) */
-inline int matrix_log_get_pade_degree(long double normTminusI)
-{
-#if   LDBL_MANT_DIG == 53         // double precision
-  const long double maxNormForPade[] = { 1.6206284795015624e-2L /* degree = 3 */ , 5.3873532631381171e-2L,
-            1.1352802267628681e-1L, 1.8662860613541288e-1L, 2.642960831111435e-1L };
-#elif LDBL_MANT_DIG <= 64         // extended precision
-  const long double maxNormForPade[] = { 5.48256690357782863103e-3L /* degree = 3 */, 2.34559162387971167321e-2L,
-            5.84603923897347449857e-2L, 1.08486423756725170223e-1L, 1.68385767881294446649e-1L,
-            2.32777776523703892094e-1L };
-#elif LDBL_MANT_DIG <= 106        // double-double
-  const long double maxNormForPade[] = { 8.58970550342939562202529664318890e-5L /* degree = 3 */,
-            9.34074328446359654039446552677759e-4L, 4.26117194647672175773064114582860e-3L,
-            1.21546224740281848743149666560464e-2L, 2.61100544998339436713088248557444e-2L,
-            4.66170074627052749243018566390567e-2L, 7.32585144444135027565872014932387e-2L,
-            1.05026503471351080481093652651105e-1L };
-#else                             // quadruple precision
-  const long double maxNormForPade[] = { 4.7419931187193005048501568167858103e-5L /* degree = 3 */,
-            5.8853168473544560470387769480192666e-4L, 2.9216120366601315391789493628113520e-3L,
-            8.8415758124319434347116734705174308e-3L, 1.9850836029449446668518049562565291e-2L,
-            3.6688019729653446926585242192447447e-2L, 5.9290962294020186998954055264528393e-2L,
-            8.6998436081634343903250580992127677e-2L, 1.1880960220216759245467951592883642e-1L };
-#endif
-  const int minPadeDegree = matrix_log_min_pade_degree<long double>::value;
-  const int maxPadeDegree = matrix_log_max_pade_degree<long double>::value;
-  int degree = minPadeDegree;
-  for (; degree <= maxPadeDegree; ++degree)
-    if (normTminusI <= maxNormForPade[degree - minPadeDegree])
-      break;
-  return degree;
-}
-
-/* \brief Compute Pade approximation to matrix logarithm */
-template <typename MatrixType>
-void matrix_log_compute_pade(MatrixType& result, const MatrixType& T, int degree)
-{
-  typedef typename NumTraits<typename MatrixType::Scalar>::Real RealScalar;
-  const int minPadeDegree = 3;
-  const int maxPadeDegree = 11;
-  assert(degree >= minPadeDegree && degree <= maxPadeDegree);
-
-  const RealScalar nodes[][maxPadeDegree] = { 
-    { 0.1127016653792583114820734600217600L, 0.5000000000000000000000000000000000L,  // degree 3
-      0.8872983346207416885179265399782400L }, 
-    { 0.0694318442029737123880267555535953L, 0.3300094782075718675986671204483777L,  // degree 4
-      0.6699905217924281324013328795516223L, 0.9305681557970262876119732444464048L },
-    { 0.0469100770306680036011865608503035L, 0.2307653449471584544818427896498956L,  // degree 5
-      0.5000000000000000000000000000000000L, 0.7692346550528415455181572103501044L,
-      0.9530899229693319963988134391496965L },
-    { 0.0337652428984239860938492227530027L, 0.1693953067668677431693002024900473L,  // degree 6
-      0.3806904069584015456847491391596440L, 0.6193095930415984543152508608403560L,
-      0.8306046932331322568306997975099527L, 0.9662347571015760139061507772469973L },
-    { 0.0254460438286207377369051579760744L, 0.1292344072003027800680676133596058L,  // degree 7
-      0.2970774243113014165466967939615193L, 0.5000000000000000000000000000000000L,
-      0.7029225756886985834533032060384807L, 0.8707655927996972199319323866403942L,
-      0.9745539561713792622630948420239256L },
-    { 0.0198550717512318841582195657152635L, 0.1016667612931866302042230317620848L,  // degree 8
-      0.2372337950418355070911304754053768L, 0.4082826787521750975302619288199080L,
-      0.5917173212478249024697380711800920L, 0.7627662049581644929088695245946232L,
-      0.8983332387068133697957769682379152L, 0.9801449282487681158417804342847365L },
-    { 0.0159198802461869550822118985481636L, 0.0819844463366821028502851059651326L,  // degree 9
-      0.1933142836497048013456489803292629L, 0.3378732882980955354807309926783317L,
-      0.5000000000000000000000000000000000L, 0.6621267117019044645192690073216683L,
-      0.8066857163502951986543510196707371L, 0.9180155536633178971497148940348674L,
-      0.9840801197538130449177881014518364L },
-    { 0.0130467357414141399610179939577740L, 0.0674683166555077446339516557882535L,  // degree 10
-      0.1602952158504877968828363174425632L, 0.2833023029353764046003670284171079L,
-      0.4255628305091843945575869994351400L, 0.5744371694908156054424130005648600L,
-      0.7166976970646235953996329715828921L, 0.8397047841495122031171636825574368L,
-      0.9325316833444922553660483442117465L, 0.9869532642585858600389820060422260L },
-    { 0.0108856709269715035980309994385713L, 0.0564687001159523504624211153480364L,  // degree 11
-      0.1349239972129753379532918739844233L, 0.2404519353965940920371371652706952L,
-      0.3652284220238275138342340072995692L, 0.5000000000000000000000000000000000L,
-      0.6347715779761724861657659927004308L, 0.7595480646034059079628628347293048L,
-      0.8650760027870246620467081260155767L, 0.9435312998840476495375788846519636L,
-      0.9891143290730284964019690005614287L } };
-
-  const RealScalar weights[][maxPadeDegree] = { 
-    { 0.2777777777777777777777777777777778L, 0.4444444444444444444444444444444444L,  // degree 3
-      0.2777777777777777777777777777777778L },
-    { 0.1739274225687269286865319746109997L, 0.3260725774312730713134680253890003L,  // degree 4
-      0.3260725774312730713134680253890003L, 0.1739274225687269286865319746109997L },
-    { 0.1184634425280945437571320203599587L, 0.2393143352496832340206457574178191L,  // degree 5
-      0.2844444444444444444444444444444444L, 0.2393143352496832340206457574178191L,
-      0.1184634425280945437571320203599587L },
-    { 0.0856622461895851725201480710863665L, 0.1803807865240693037849167569188581L,  // degree 6
-      0.2339569672863455236949351719947755L, 0.2339569672863455236949351719947755L,
-      0.1803807865240693037849167569188581L, 0.0856622461895851725201480710863665L },
-    { 0.0647424830844348466353057163395410L, 0.1398526957446383339507338857118898L,  // degree 7
-      0.1909150252525594724751848877444876L, 0.2089795918367346938775510204081633L,
-      0.1909150252525594724751848877444876L, 0.1398526957446383339507338857118898L,
-      0.0647424830844348466353057163395410L },
-    { 0.0506142681451881295762656771549811L, 0.1111905172266872352721779972131204L,  // degree 8
-      0.1568533229389436436689811009933007L, 0.1813418916891809914825752246385978L,
-      0.1813418916891809914825752246385978L, 0.1568533229389436436689811009933007L,
-      0.1111905172266872352721779972131204L, 0.0506142681451881295762656771549811L },
-    { 0.0406371941807872059859460790552618L, 0.0903240803474287020292360156214564L,  // degree 9
-      0.1303053482014677311593714347093164L, 0.1561735385200014200343152032922218L,
-      0.1651196775006298815822625346434870L, 0.1561735385200014200343152032922218L,
-      0.1303053482014677311593714347093164L, 0.0903240803474287020292360156214564L,
-      0.0406371941807872059859460790552618L },
-    { 0.0333356721543440687967844049466659L, 0.0747256745752902965728881698288487L,  // degree 10
-      0.1095431812579910219977674671140816L, 0.1346333596549981775456134607847347L,
-      0.1477621123573764350869464973256692L, 0.1477621123573764350869464973256692L,
-      0.1346333596549981775456134607847347L, 0.1095431812579910219977674671140816L,
-      0.0747256745752902965728881698288487L, 0.0333356721543440687967844049466659L },
-    { 0.0278342835580868332413768602212743L, 0.0627901847324523123173471496119701L,  // degree 11
-      0.0931451054638671257130488207158280L, 0.1165968822959952399592618524215876L,
-      0.1314022722551233310903444349452546L, 0.1364625433889503153572417641681711L,
-      0.1314022722551233310903444349452546L, 0.1165968822959952399592618524215876L,
-      0.0931451054638671257130488207158280L, 0.0627901847324523123173471496119701L,
-      0.0278342835580868332413768602212743L } };
-
-  MatrixType TminusI = T - MatrixType::Identity(T.rows(), T.rows());
-  result.setZero(T.rows(), T.rows());
-  for (int k = 0; k < degree; ++k) {
-    RealScalar weight = weights[degree-minPadeDegree][k];
-    RealScalar node = nodes[degree-minPadeDegree][k];
-    result += weight * (MatrixType::Identity(T.rows(), T.rows()) + node * TminusI)
-                       .template triangularView<Upper>().solve(TminusI);
-  }
-} 
-
-/** \brief Compute logarithm of triangular matrices with size > 2. 
-  * \details This uses a inverse scale-and-square algorithm. */
-template <typename MatrixType>
-void matrix_log_compute_big(const MatrixType& A, MatrixType& result)
-{
-  typedef typename MatrixType::Scalar Scalar;
-  typedef typename NumTraits<Scalar>::Real RealScalar;
-  using std::pow;
-
-  int numberOfSquareRoots = 0;
-  int numberOfExtraSquareRoots = 0;
-  int degree;
-  MatrixType T = A, sqrtT;
-
-  int maxPadeDegree = matrix_log_max_pade_degree<Scalar>::value;
-  const RealScalar maxNormForPade = maxPadeDegree<= 5? 5.3149729967117310e-1L:                    // single precision
-                                    maxPadeDegree<= 7? 2.6429608311114350e-1L:                    // double precision
-                                    maxPadeDegree<= 8? 2.32777776523703892094e-1L:                // extended precision
-                                    maxPadeDegree<=10? 1.05026503471351080481093652651105e-1L:    // double-double
-                                                       1.1880960220216759245467951592883642e-1L;  // quadruple precision
-
-  while (true) {
-    RealScalar normTminusI = (T - MatrixType::Identity(T.rows(), T.rows())).cwiseAbs().colwise().sum().maxCoeff();
-    if (normTminusI < maxNormForPade) {
-      degree = matrix_log_get_pade_degree(normTminusI);
-      int degree2 = matrix_log_get_pade_degree(normTminusI / RealScalar(2));
-      if ((degree - degree2 <= 1) || (numberOfExtraSquareRoots == 1)) 
-        break;
-      ++numberOfExtraSquareRoots;
-    }
-    matrix_sqrt_triangular(T, sqrtT);
-    T = sqrtT.template triangularView<Upper>();
-    ++numberOfSquareRoots;
-  }
-
-  matrix_log_compute_pade(result, T, degree);
-  result *= pow(RealScalar(2), numberOfSquareRoots);
-}
-
-/** \ingroup MatrixFunctions_Module
-  * \class MatrixLogarithmAtomic
-  * \brief Helper class for computing matrix logarithm of atomic matrices.
-  *
-  * Here, an atomic matrix is a triangular matrix whose diagonal entries are close to each other.
-  *
-  * \sa class MatrixFunctionAtomic, MatrixBase::log()
-  */
-template <typename MatrixType>
-class MatrixLogarithmAtomic
-{
-public:
-  /** \brief Compute matrix logarithm of atomic matrix
-    * \param[in]  A  argument of matrix logarithm, should be upper triangular and atomic
-    * \returns  The logarithm of \p A.
-    */
-  MatrixType compute(const MatrixType& A);
-};
-
-template <typename MatrixType>
-MatrixType MatrixLogarithmAtomic<MatrixType>::compute(const MatrixType& A)
-{
-  using std::log;
-  MatrixType result(A.rows(), A.rows());
-  if (A.rows() == 1)
-    result(0,0) = log(A(0,0));
-  else if (A.rows() == 2)
-    matrix_log_compute_2x2(A, result);
-  else
-    matrix_log_compute_big(A, result);
-  return result;
-}
-
-} // end of namespace internal
-
-/** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix logarithm of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix function.
-  *
-  * This class holds the argument to the matrix function until it is
-  * assigned or evaluated for some other reason (so the argument
-  * should not be changed in the meantime). It is the return type of
-  * MatrixBase::log() and most of the time this is the only way it
-  * is used.
-  */
-template<typename Derived> class MatrixLogarithmReturnValue
-: public ReturnByValue<MatrixLogarithmReturnValue<Derived> >
-{
-public:
-  typedef typename Derived::Scalar Scalar;
-  typedef typename Derived::Index Index;
-
-protected:
-  typedef typename internal::ref_selector<Derived>::type DerivedNested;
-
-public:
-
-  /** \brief Constructor.
-    *
-    * \param[in]  A  %Matrix (expression) forming the argument of the matrix logarithm.
-    */
-  explicit MatrixLogarithmReturnValue(const Derived& A) : m_A(A) { }
-  
-  /** \brief Compute the matrix logarithm.
-    *
-    * \param[out]  result  Logarithm of \c A, where \c A is as specified in the constructor.
-    */
-  template <typename ResultType>
-  inline void evalTo(ResultType& result) const
-  {
-    typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
-    typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
-    typedef internal::traits<DerivedEvalTypeClean> Traits;
-    static const int RowsAtCompileTime = Traits::RowsAtCompileTime;
-    static const int ColsAtCompileTime = Traits::ColsAtCompileTime;
-    typedef std::complex<typename NumTraits<Scalar>::Real> ComplexScalar;
-    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0, RowsAtCompileTime, ColsAtCompileTime> DynMatrixType;
-    typedef internal::MatrixLogarithmAtomic<DynMatrixType> AtomicType;
-    AtomicType atomic;
-    
-    internal::matrix_function_compute<typename DerivedEvalTypeClean::PlainObject>::run(m_A, atomic, result);
-  }
-
-  Index rows() const { return m_A.rows(); }
-  Index cols() const { return m_A.cols(); }
-  
-private:
-  const DerivedNested m_A;
-};
-
-namespace internal {
-  template<typename Derived>
-  struct traits<MatrixLogarithmReturnValue<Derived> >
-  {
-    typedef typename Derived::PlainObject ReturnType;
-  };
-}
-
-
-/********** MatrixBase method **********/
-
-
-template <typename Derived>
-const MatrixLogarithmReturnValue<Derived> MatrixBase<Derived>::log() const
-{
-  eigen_assert(rows() == cols());
-  return MatrixLogarithmReturnValue<Derived>(derived());
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_MATRIX_LOGARITHM
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
deleted file mode 100644
index a3273da..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h
+++ /dev/null
@@ -1,709 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2012, 2013 Chen-Pang He <jdh8@ms63.hinet.net>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIX_POWER
-#define EIGEN_MATRIX_POWER
-
-namespace Eigen {
-
-template<typename MatrixType> class MatrixPower;
-
-/**
- * \ingroup MatrixFunctions_Module
- *
- * \brief Proxy for the matrix power of some matrix.
- *
- * \tparam MatrixType  type of the base, a matrix.
- *
- * This class holds the arguments to the matrix power until it is
- * assigned or evaluated for some other reason (so the argument
- * should not be changed in the meantime). It is the return type of
- * MatrixPower::operator() and related functions and most of the
- * time this is the only way it is used.
- */
-/* TODO This class is only used by MatrixPower, so it should be nested
- * into MatrixPower, like MatrixPower::ReturnValue. However, my
- * compiler complained about unused template parameter in the
- * following declaration in namespace internal.
- *
- * template<typename MatrixType>
- * struct traits<MatrixPower<MatrixType>::ReturnValue>;
- */
-template<typename MatrixType>
-class MatrixPowerParenthesesReturnValue : public ReturnByValue< MatrixPowerParenthesesReturnValue<MatrixType> >
-{
-  public:
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::Index Index;
-
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] pow  %MatrixPower storing the base.
-     * \param[in] p    scalar, the exponent of the matrix power.
-     */
-    MatrixPowerParenthesesReturnValue(MatrixPower<MatrixType>& pow, RealScalar p) : m_pow(pow), m_p(p)
-    { }
-
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[out] result
-     */
-    template<typename ResultType>
-    inline void evalTo(ResultType& result) const
-    { m_pow.compute(result, m_p); }
-
-    Index rows() const { return m_pow.rows(); }
-    Index cols() const { return m_pow.cols(); }
-
-  private:
-    MatrixPower<MatrixType>& m_pow;
-    const RealScalar m_p;
-};
-
-/**
- * \ingroup MatrixFunctions_Module
- *
- * \brief Class for computing matrix powers.
- *
- * \tparam MatrixType  type of the base, expected to be an instantiation
- * of the Matrix class template.
- *
- * This class is capable of computing triangular real/complex matrices
- * raised to a power in the interval \f$ (-1, 1) \f$.
- *
- * \note Currently this class is only used by MatrixPower. One may
- * insist that this be nested into MatrixPower. This class is here to
- * faciliate future development of triangular matrix functions.
- */
-template<typename MatrixType>
-class MatrixPowerAtomic : internal::noncopyable
-{
-  private:
-    enum {
-      RowsAtCompileTime = MatrixType::RowsAtCompileTime,
-      MaxRowsAtCompileTime = MatrixType::MaxRowsAtCompileTime
-    };
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef std::complex<RealScalar> ComplexScalar;
-    typedef typename MatrixType::Index Index;
-    typedef Block<MatrixType,Dynamic,Dynamic> ResultType;
-
-    const MatrixType& m_A;
-    RealScalar m_p;
-
-    void computePade(int degree, const MatrixType& IminusT, ResultType& res) const;
-    void compute2x2(ResultType& res, RealScalar p) const;
-    void computeBig(ResultType& res) const;
-    static int getPadeDegree(float normIminusT);
-    static int getPadeDegree(double normIminusT);
-    static int getPadeDegree(long double normIminusT);
-    static ComplexScalar computeSuperDiag(const ComplexScalar&, const ComplexScalar&, RealScalar p);
-    static RealScalar computeSuperDiag(RealScalar, RealScalar, RealScalar p);
-
-  public:
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] T  the base of the matrix power.
-     * \param[in] p  the exponent of the matrix power, should be in
-     * \f$ (-1, 1) \f$.
-     *
-     * The class stores a reference to T, so it should not be changed
-     * (or destroyed) before evaluation. Only the upper triangular
-     * part of T is read.
-     */
-    MatrixPowerAtomic(const MatrixType& T, RealScalar p);
-    
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[out] res  \f$ A^p \f$ where A and p are specified in the
-     * constructor.
-     */
-    void compute(ResultType& res) const;
-};
-
-template<typename MatrixType>
-MatrixPowerAtomic<MatrixType>::MatrixPowerAtomic(const MatrixType& T, RealScalar p) :
-  m_A(T), m_p(p)
-{
-  eigen_assert(T.rows() == T.cols());
-  eigen_assert(p > -1 && p < 1);
-}
-
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::compute(ResultType& res) const
-{
-  using std::pow;
-  switch (m_A.rows()) {
-    case 0:
-      break;
-    case 1:
-      res(0,0) = pow(m_A(0,0), m_p);
-      break;
-    case 2:
-      compute2x2(res, m_p);
-      break;
-    default:
-      computeBig(res);
-  }
-}
-
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::computePade(int degree, const MatrixType& IminusT, ResultType& res) const
-{
-  int i = 2*degree;
-  res = (m_p-degree) / (2*i-2) * IminusT;
-
-  for (--i; i; --i) {
-    res = (MatrixType::Identity(IminusT.rows(), IminusT.cols()) + res).template triangularView<Upper>()
-	.solve((i==1 ? -m_p : i&1 ? (-m_p-i/2)/(2*i) : (m_p-i/2)/(2*i-2)) * IminusT).eval();
-  }
-  res += MatrixType::Identity(IminusT.rows(), IminusT.cols());
-}
-
-// This function assumes that res has the correct size (see bug 614)
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::compute2x2(ResultType& res, RealScalar p) const
-{
-  using std::abs;
-  using std::pow;
-  res.coeffRef(0,0) = pow(m_A.coeff(0,0), p);
-
-  for (Index i=1; i < m_A.cols(); ++i) {
-    res.coeffRef(i,i) = pow(m_A.coeff(i,i), p);
-    if (m_A.coeff(i-1,i-1) == m_A.coeff(i,i))
-      res.coeffRef(i-1,i) = p * pow(m_A.coeff(i,i), p-1);
-    else if (2*abs(m_A.coeff(i-1,i-1)) < abs(m_A.coeff(i,i)) || 2*abs(m_A.coeff(i,i)) < abs(m_A.coeff(i-1,i-1)))
-      res.coeffRef(i-1,i) = (res.coeff(i,i)-res.coeff(i-1,i-1)) / (m_A.coeff(i,i)-m_A.coeff(i-1,i-1));
-    else
-      res.coeffRef(i-1,i) = computeSuperDiag(m_A.coeff(i,i), m_A.coeff(i-1,i-1), p);
-    res.coeffRef(i-1,i) *= m_A.coeff(i-1,i);
-  }
-}
-
-template<typename MatrixType>
-void MatrixPowerAtomic<MatrixType>::computeBig(ResultType& res) const
-{
-  using std::ldexp;
-  const int digits = std::numeric_limits<RealScalar>::digits;
-  const RealScalar maxNormForPade = digits <=  24? 4.3386528e-1L                            // single precision
-                                  : digits <=  53? 2.789358995219730e-1L                    // double precision
-                                  : digits <=  64? 2.4471944416607995472e-1L                // extended precision
-                                  : digits <= 106? 1.1016843812851143391275867258512e-1L    // double-double
-                                  :                9.134603732914548552537150753385375e-2L; // quadruple precision
-  MatrixType IminusT, sqrtT, T = m_A.template triangularView<Upper>();
-  RealScalar normIminusT;
-  int degree, degree2, numberOfSquareRoots = 0;
-  bool hasExtraSquareRoot = false;
-
-  for (Index i=0; i < m_A.cols(); ++i)
-    eigen_assert(m_A(i,i) != RealScalar(0));
-
-  while (true) {
-    IminusT = MatrixType::Identity(m_A.rows(), m_A.cols()) - T;
-    normIminusT = IminusT.cwiseAbs().colwise().sum().maxCoeff();
-    if (normIminusT < maxNormForPade) {
-      degree = getPadeDegree(normIminusT);
-      degree2 = getPadeDegree(normIminusT/2);
-      if (degree - degree2 <= 1 || hasExtraSquareRoot)
-	break;
-      hasExtraSquareRoot = true;
-    }
-    matrix_sqrt_triangular(T, sqrtT);
-    T = sqrtT.template triangularView<Upper>();
-    ++numberOfSquareRoots;
-  }
-  computePade(degree, IminusT, res);
-
-  for (; numberOfSquareRoots; --numberOfSquareRoots) {
-    compute2x2(res, ldexp(m_p, -numberOfSquareRoots));
-    res = res.template triangularView<Upper>() * res;
-  }
-  compute2x2(res, m_p);
-}
-  
-template<typename MatrixType>
-inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(float normIminusT)
-{
-  const float maxNormForPade[] = { 2.8064004e-1f /* degree = 3 */ , 4.3386528e-1f };
-  int degree = 3;
-  for (; degree <= 4; ++degree)
-    if (normIminusT <= maxNormForPade[degree - 3])
-      break;
-  return degree;
-}
-
-template<typename MatrixType>
-inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(double normIminusT)
-{
-  const double maxNormForPade[] = { 1.884160592658218e-2 /* degree = 3 */ , 6.038881904059573e-2, 1.239917516308172e-1,
-      1.999045567181744e-1, 2.789358995219730e-1 };
-  int degree = 3;
-  for (; degree <= 7; ++degree)
-    if (normIminusT <= maxNormForPade[degree - 3])
-      break;
-  return degree;
-}
-
-template<typename MatrixType>
-inline int MatrixPowerAtomic<MatrixType>::getPadeDegree(long double normIminusT)
-{
-#if   LDBL_MANT_DIG == 53
-  const int maxPadeDegree = 7;
-  const double maxNormForPade[] = { 1.884160592658218e-2L /* degree = 3 */ , 6.038881904059573e-2L, 1.239917516308172e-1L,
-      1.999045567181744e-1L, 2.789358995219730e-1L };
-#elif LDBL_MANT_DIG <= 64
-  const int maxPadeDegree = 8;
-  const long double maxNormForPade[] = { 6.3854693117491799460e-3L /* degree = 3 */ , 2.6394893435456973676e-2L,
-      6.4216043030404063729e-2L, 1.1701165502926694307e-1L, 1.7904284231268670284e-1L, 2.4471944416607995472e-1L };
-#elif LDBL_MANT_DIG <= 106
-  const int maxPadeDegree = 10;
-  const double maxNormForPade[] = { 1.0007161601787493236741409687186e-4L /* degree = 3 */ ,
-      1.0007161601787493236741409687186e-3L, 4.7069769360887572939882574746264e-3L, 1.3220386624169159689406653101695e-2L,
-      2.8063482381631737920612944054906e-2L, 4.9625993951953473052385361085058e-2L, 7.7367040706027886224557538328171e-2L,
-      1.1016843812851143391275867258512e-1L };
-#else
-  const int maxPadeDegree = 10;
-  const double maxNormForPade[] = { 5.524506147036624377378713555116378e-5L /* degree = 3 */ ,
-      6.640600568157479679823602193345995e-4L, 3.227716520106894279249709728084626e-3L,
-      9.619593944683432960546978734646284e-3L, 2.134595382433742403911124458161147e-2L,
-      3.908166513900489428442993794761185e-2L, 6.266780814639442865832535460550138e-2L,
-      9.134603732914548552537150753385375e-2L };
-#endif
-  int degree = 3;
-  for (; degree <= maxPadeDegree; ++degree)
-    if (normIminusT <= maxNormForPade[degree - 3])
-      break;
-  return degree;
-}
-
-template<typename MatrixType>
-inline typename MatrixPowerAtomic<MatrixType>::ComplexScalar
-MatrixPowerAtomic<MatrixType>::computeSuperDiag(const ComplexScalar& curr, const ComplexScalar& prev, RealScalar p)
-{
-  using std::ceil;
-  using std::exp;
-  using std::log;
-  using std::sinh;
-
-  ComplexScalar logCurr = log(curr);
-  ComplexScalar logPrev = log(prev);
-  int unwindingNumber = ceil((numext::imag(logCurr - logPrev) - RealScalar(EIGEN_PI)) / RealScalar(2*EIGEN_PI));
-  ComplexScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2) + ComplexScalar(0, EIGEN_PI*unwindingNumber);
-  return RealScalar(2) * exp(RealScalar(0.5) * p * (logCurr + logPrev)) * sinh(p * w) / (curr - prev);
-}
-
-template<typename MatrixType>
-inline typename MatrixPowerAtomic<MatrixType>::RealScalar
-MatrixPowerAtomic<MatrixType>::computeSuperDiag(RealScalar curr, RealScalar prev, RealScalar p)
-{
-  using std::exp;
-  using std::log;
-  using std::sinh;
-
-  RealScalar w = numext::log1p((curr-prev)/prev)/RealScalar(2);
-  return 2 * exp(p * (log(curr) + log(prev)) / 2) * sinh(p * w) / (curr - prev);
-}
-
-/**
- * \ingroup MatrixFunctions_Module
- *
- * \brief Class for computing matrix powers.
- *
- * \tparam MatrixType  type of the base, expected to be an instantiation
- * of the Matrix class template.
- *
- * This class is capable of computing real/complex matrices raised to
- * an arbitrary real power. Meanwhile, it saves the result of Schur
- * decomposition if an non-integral power has even been calculated.
- * Therefore, if you want to compute multiple (>= 2) matrix powers
- * for the same matrix, using the class directly is more efficient than
- * calling MatrixBase::pow().
- *
- * Example:
- * \include MatrixPower_optimal.cpp
- * Output: \verbinclude MatrixPower_optimal.out
- */
-template<typename MatrixType>
-class MatrixPower : internal::noncopyable
-{
-  private:
-    typedef typename MatrixType::Scalar Scalar;
-    typedef typename MatrixType::RealScalar RealScalar;
-    typedef typename MatrixType::Index Index;
-
-  public:
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] A  the base of the matrix power.
-     *
-     * The class stores a reference to A, so it should not be changed
-     * (or destroyed) before evaluation.
-     */
-    explicit MatrixPower(const MatrixType& A) :
-      m_A(A),
-      m_conditionNumber(0),
-      m_rank(A.cols()),
-      m_nulls(0)
-    { eigen_assert(A.rows() == A.cols()); }
-
-    /**
-     * \brief Returns the matrix power.
-     *
-     * \param[in] p  exponent, a real scalar.
-     * \return The expression \f$ A^p \f$, where A is specified in the
-     * constructor.
-     */
-    const MatrixPowerParenthesesReturnValue<MatrixType> operator()(RealScalar p)
-    { return MatrixPowerParenthesesReturnValue<MatrixType>(*this, p); }
-
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[in]  p    exponent, a real scalar.
-     * \param[out] res  \f$ A^p \f$ where A is specified in the
-     * constructor.
-     */
-    template<typename ResultType>
-    void compute(ResultType& res, RealScalar p);
-    
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
-
-  private:
-    typedef std::complex<RealScalar> ComplexScalar;
-    typedef Matrix<ComplexScalar, Dynamic, Dynamic, 0,
-              MatrixType::RowsAtCompileTime, MatrixType::ColsAtCompileTime> ComplexMatrix;
-
-    /** \brief Reference to the base of matrix power. */
-    typename MatrixType::Nested m_A;
-
-    /** \brief Temporary storage. */
-    MatrixType m_tmp;
-
-    /** \brief Store the result of Schur decomposition. */
-    ComplexMatrix m_T, m_U;
-    
-    /** \brief Store fractional power of m_T. */
-    ComplexMatrix m_fT;
-
-    /**
-     * \brief Condition number of m_A.
-     *
-     * It is initialized as 0 to avoid performing unnecessary Schur
-     * decomposition, which is the bottleneck.
-     */
-    RealScalar m_conditionNumber;
-
-    /** \brief Rank of m_A. */
-    Index m_rank;
-    
-    /** \brief Rank deficiency of m_A. */
-    Index m_nulls;
-
-    /**
-     * \brief Split p into integral part and fractional part.
-     *
-     * \param[in]  p        The exponent.
-     * \param[out] p        The fractional part ranging in \f$ (-1, 1) \f$.
-     * \param[out] intpart  The integral part.
-     *
-     * Only if the fractional part is nonzero, it calls initialize().
-     */
-    void split(RealScalar& p, RealScalar& intpart);
-
-    /** \brief Perform Schur decomposition for fractional power. */
-    void initialize();
-
-    template<typename ResultType>
-    void computeIntPower(ResultType& res, RealScalar p);
-
-    template<typename ResultType>
-    void computeFracPower(ResultType& res, RealScalar p);
-
-    template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-    static void revertSchur(
-        Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-        const ComplexMatrix& T,
-        const ComplexMatrix& U);
-
-    template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-    static void revertSchur(
-        Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-        const ComplexMatrix& T,
-        const ComplexMatrix& U);
-};
-
-template<typename MatrixType>
-template<typename ResultType>
-void MatrixPower<MatrixType>::compute(ResultType& res, RealScalar p)
-{
-  using std::pow;
-  switch (cols()) {
-    case 0:
-      break;
-    case 1:
-      res(0,0) = pow(m_A.coeff(0,0), p);
-      break;
-    default:
-      RealScalar intpart;
-      split(p, intpart);
-
-      res = MatrixType::Identity(rows(), cols());
-      computeIntPower(res, intpart);
-      if (p) computeFracPower(res, p);
-  }
-}
-
-template<typename MatrixType>
-void MatrixPower<MatrixType>::split(RealScalar& p, RealScalar& intpart)
-{
-  using std::floor;
-  using std::pow;
-
-  intpart = floor(p);
-  p -= intpart;
-
-  // Perform Schur decomposition if it is not yet performed and the power is
-  // not an integer.
-  if (!m_conditionNumber && p)
-    initialize();
-
-  // Choose the more stable of intpart = floor(p) and intpart = ceil(p).
-  if (p > RealScalar(0.5) && p > (1-p) * pow(m_conditionNumber, p)) {
-    --p;
-    ++intpart;
-  }
-}
-
-template<typename MatrixType>
-void MatrixPower<MatrixType>::initialize()
-{
-  const ComplexSchur<MatrixType> schurOfA(m_A);
-  JacobiRotation<ComplexScalar> rot;
-  ComplexScalar eigenvalue;
-
-  m_fT.resizeLike(m_A);
-  m_T = schurOfA.matrixT();
-  m_U = schurOfA.matrixU();
-  m_conditionNumber = m_T.diagonal().array().abs().maxCoeff() / m_T.diagonal().array().abs().minCoeff();
-
-  // Move zero eigenvalues to the bottom right corner.
-  for (Index i = cols()-1; i>=0; --i) {
-    if (m_rank <= 2)
-      return;
-    if (m_T.coeff(i,i) == RealScalar(0)) {
-      for (Index j=i+1; j < m_rank; ++j) {
-        eigenvalue = m_T.coeff(j,j);
-        rot.makeGivens(m_T.coeff(j-1,j), eigenvalue);
-        m_T.applyOnTheRight(j-1, j, rot);
-        m_T.applyOnTheLeft(j-1, j, rot.adjoint());
-        m_T.coeffRef(j-1,j-1) = eigenvalue;
-        m_T.coeffRef(j,j) = RealScalar(0);
-        m_U.applyOnTheRight(j-1, j, rot);
-      }
-      --m_rank;
-    }
-  }
-
-  m_nulls = rows() - m_rank;
-  if (m_nulls) {
-    eigen_assert(m_T.bottomRightCorner(m_nulls, m_nulls).isZero()
-        && "Base of matrix power should be invertible or with a semisimple zero eigenvalue.");
-    m_fT.bottomRows(m_nulls).fill(RealScalar(0));
-  }
-}
-
-template<typename MatrixType>
-template<typename ResultType>
-void MatrixPower<MatrixType>::computeIntPower(ResultType& res, RealScalar p)
-{
-  using std::abs;
-  using std::fmod;
-  RealScalar pp = abs(p);
-
-  if (p<0) 
-    m_tmp = m_A.inverse();
-  else     
-    m_tmp = m_A;
-
-  while (true) {
-    if (fmod(pp, 2) >= 1)
-      res = m_tmp * res;
-    pp /= 2;
-    if (pp < 1)
-      break;
-    m_tmp *= m_tmp;
-  }
-}
-
-template<typename MatrixType>
-template<typename ResultType>
-void MatrixPower<MatrixType>::computeFracPower(ResultType& res, RealScalar p)
-{
-  Block<ComplexMatrix,Dynamic,Dynamic> blockTp(m_fT, 0, 0, m_rank, m_rank);
-  eigen_assert(m_conditionNumber);
-  eigen_assert(m_rank + m_nulls == rows());
-
-  MatrixPowerAtomic<ComplexMatrix>(m_T.topLeftCorner(m_rank, m_rank), p).compute(blockTp);
-  if (m_nulls) {
-    m_fT.topRightCorner(m_rank, m_nulls) = m_T.topLeftCorner(m_rank, m_rank).template triangularView<Upper>()
-        .solve(blockTp * m_T.topRightCorner(m_rank, m_nulls));
-  }
-  revertSchur(m_tmp, m_fT, m_U);
-  res = m_tmp * res;
-}
-
-template<typename MatrixType>
-template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-inline void MatrixPower<MatrixType>::revertSchur(
-    Matrix<ComplexScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-    const ComplexMatrix& T,
-    const ComplexMatrix& U)
-{ res.noalias() = U * (T.template triangularView<Upper>() * U.adjoint()); }
-
-template<typename MatrixType>
-template<int Rows, int Cols, int Options, int MaxRows, int MaxCols>
-inline void MatrixPower<MatrixType>::revertSchur(
-    Matrix<RealScalar, Rows, Cols, Options, MaxRows, MaxCols>& res,
-    const ComplexMatrix& T,
-    const ComplexMatrix& U)
-{ res.noalias() = (U * (T.template triangularView<Upper>() * U.adjoint())).real(); }
-
-/**
- * \ingroup MatrixFunctions_Module
- *
- * \brief Proxy for the matrix power of some matrix (expression).
- *
- * \tparam Derived  type of the base, a matrix (expression).
- *
- * This class holds the arguments to the matrix power until it is
- * assigned or evaluated for some other reason (so the argument
- * should not be changed in the meantime). It is the return type of
- * MatrixBase::pow() and related functions and most of the
- * time this is the only way it is used.
- */
-template<typename Derived>
-class MatrixPowerReturnValue : public ReturnByValue< MatrixPowerReturnValue<Derived> >
-{
-  public:
-    typedef typename Derived::PlainObject PlainObject;
-    typedef typename Derived::RealScalar RealScalar;
-    typedef typename Derived::Index Index;
-
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] A  %Matrix (expression), the base of the matrix power.
-     * \param[in] p  real scalar, the exponent of the matrix power.
-     */
-    MatrixPowerReturnValue(const Derived& A, RealScalar p) : m_A(A), m_p(p)
-    { }
-
-    /**
-     * \brief Compute the matrix power.
-     *
-     * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
-     * constructor.
-     */
-    template<typename ResultType>
-    inline void evalTo(ResultType& result) const
-    { MatrixPower<PlainObject>(m_A.eval()).compute(result, m_p); }
-
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
-
-  private:
-    const Derived& m_A;
-    const RealScalar m_p;
-};
-
-/**
- * \ingroup MatrixFunctions_Module
- *
- * \brief Proxy for the matrix power of some matrix (expression).
- *
- * \tparam Derived  type of the base, a matrix (expression).
- *
- * This class holds the arguments to the matrix power until it is
- * assigned or evaluated for some other reason (so the argument
- * should not be changed in the meantime). It is the return type of
- * MatrixBase::pow() and related functions and most of the
- * time this is the only way it is used.
- */
-template<typename Derived>
-class MatrixComplexPowerReturnValue : public ReturnByValue< MatrixComplexPowerReturnValue<Derived> >
-{
-  public:
-    typedef typename Derived::PlainObject PlainObject;
-    typedef typename std::complex<typename Derived::RealScalar> ComplexScalar;
-    typedef typename Derived::Index Index;
-
-    /**
-     * \brief Constructor.
-     *
-     * \param[in] A  %Matrix (expression), the base of the matrix power.
-     * \param[in] p  complex scalar, the exponent of the matrix power.
-     */
-    MatrixComplexPowerReturnValue(const Derived& A, const ComplexScalar& p) : m_A(A), m_p(p)
-    { }
-
-    /**
-     * \brief Compute the matrix power.
-     *
-     * Because \p p is complex, \f$ A^p \f$ is simply evaluated as \f$
-     * \exp(p \log(A)) \f$.
-     *
-     * \param[out] result  \f$ A^p \f$ where \p A and \p p are as in the
-     * constructor.
-     */
-    template<typename ResultType>
-    inline void evalTo(ResultType& result) const
-    { result = (m_p * m_A.log()).exp(); }
-
-    Index rows() const { return m_A.rows(); }
-    Index cols() const { return m_A.cols(); }
-
-  private:
-    const Derived& m_A;
-    const ComplexScalar m_p;
-};
-
-namespace internal {
-
-template<typename MatrixPowerType>
-struct traits< MatrixPowerParenthesesReturnValue<MatrixPowerType> >
-{ typedef typename MatrixPowerType::PlainObject ReturnType; };
-
-template<typename Derived>
-struct traits< MatrixPowerReturnValue<Derived> >
-{ typedef typename Derived::PlainObject ReturnType; };
-
-template<typename Derived>
-struct traits< MatrixComplexPowerReturnValue<Derived> >
-{ typedef typename Derived::PlainObject ReturnType; };
-
-}
-
-template<typename Derived>
-const MatrixPowerReturnValue<Derived> MatrixBase<Derived>::pow(const RealScalar& p) const
-{ return MatrixPowerReturnValue<Derived>(derived(), p); }
-
-template<typename Derived>
-const MatrixComplexPowerReturnValue<Derived> MatrixBase<Derived>::pow(const std::complex<RealScalar>& p) const
-{ return MatrixComplexPowerReturnValue<Derived>(derived(), p); }
-
-} // namespace Eigen
-
-#endif // EIGEN_MATRIX_POWER
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h b/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
deleted file mode 100644
index 2e5abda..0000000
--- a/third_party/allwpilib/wpimath/src/main/native/include/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h
+++ /dev/null
@@ -1,366 +0,0 @@
-// This file is part of Eigen, a lightweight C++ template library
-// for linear algebra.
-//
-// Copyright (C) 2011, 2013 Jitse Niesen <jitse@maths.leeds.ac.uk>
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef EIGEN_MATRIX_SQUARE_ROOT
-#define EIGEN_MATRIX_SQUARE_ROOT
-
-namespace Eigen { 
-
-namespace internal {
-
-// pre:  T.block(i,i,2,2) has complex conjugate eigenvalues
-// post: sqrtT.block(i,i,2,2) is square root of T.block(i,i,2,2)
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_2x2_diagonal_block(const MatrixType& T, typename MatrixType::Index i, ResultType& sqrtT)
-{
-  // TODO: This case (2-by-2 blocks with complex conjugate eigenvalues) is probably hidden somewhere
-  //       in EigenSolver. If we expose it, we could call it directly from here.
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,2,2> block = T.template block<2,2>(i,i);
-  EigenSolver<Matrix<Scalar,2,2> > es(block);
-  sqrtT.template block<2,2>(i,i)
-    = (es.eigenvectors() * es.eigenvalues().cwiseSqrt().asDiagonal() * es.eigenvectors().inverse()).real();
-}
-
-// pre:  block structure of T is such that (i,j) is a 1x1 block,
-//       all blocks of sqrtT to left of and below (i,j) are correct
-// post: sqrtT(i,j) has the correct value
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
-{
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  Scalar tmp = (sqrtT.row(i).segment(i+1,j-i-1) * sqrtT.col(j).segment(i+1,j-i-1)).value();
-  sqrtT.coeffRef(i,j) = (T.coeff(i,j) - tmp) / (sqrtT.coeff(i,i) + sqrtT.coeff(j,j));
-}
-
-// similar to compute1x1offDiagonalBlock()
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
-{
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,1,2> rhs = T.template block<1,2>(i,j);
-  if (j-i > 1)
-    rhs -= sqrtT.block(i, i+1, 1, j-i-1) * sqrtT.block(i+1, j, j-i-1, 2);
-  Matrix<Scalar,2,2> A = sqrtT.coeff(i,i) * Matrix<Scalar,2,2>::Identity();
-  A += sqrtT.template block<2,2>(j,j).transpose();
-  sqrtT.template block<1,2>(i,j).transpose() = A.fullPivLu().solve(rhs.transpose());
-}
-
-// similar to compute1x1offDiagonalBlock()
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
-{
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,2,1> rhs = T.template block<2,1>(i,j);
-  if (j-i > 2)
-    rhs -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 1);
-  Matrix<Scalar,2,2> A = sqrtT.coeff(j,j) * Matrix<Scalar,2,2>::Identity();
-  A += sqrtT.template block<2,2>(i,i);
-  sqrtT.template block<2,1>(i,j) = A.fullPivLu().solve(rhs);
-}
-
-// solves the equation A X + X B = C where all matrices are 2-by-2
-template <typename MatrixType>
-void matrix_sqrt_quasi_triangular_solve_auxiliary_equation(MatrixType& X, const MatrixType& A, const MatrixType& B, const MatrixType& C)
-{
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,4,4> coeffMatrix = Matrix<Scalar,4,4>::Zero();
-  coeffMatrix.coeffRef(0,0) = A.coeff(0,0) + B.coeff(0,0);
-  coeffMatrix.coeffRef(1,1) = A.coeff(0,0) + B.coeff(1,1);
-  coeffMatrix.coeffRef(2,2) = A.coeff(1,1) + B.coeff(0,0);
-  coeffMatrix.coeffRef(3,3) = A.coeff(1,1) + B.coeff(1,1);
-  coeffMatrix.coeffRef(0,1) = B.coeff(1,0);
-  coeffMatrix.coeffRef(0,2) = A.coeff(0,1);
-  coeffMatrix.coeffRef(1,0) = B.coeff(0,1);
-  coeffMatrix.coeffRef(1,3) = A.coeff(0,1);
-  coeffMatrix.coeffRef(2,0) = A.coeff(1,0);
-  coeffMatrix.coeffRef(2,3) = B.coeff(1,0);
-  coeffMatrix.coeffRef(3,1) = A.coeff(1,0);
-  coeffMatrix.coeffRef(3,2) = B.coeff(0,1);
-
-  Matrix<Scalar,4,1> rhs;
-  rhs.coeffRef(0) = C.coeff(0,0);
-  rhs.coeffRef(1) = C.coeff(0,1);
-  rhs.coeffRef(2) = C.coeff(1,0);
-  rhs.coeffRef(3) = C.coeff(1,1);
-
-  Matrix<Scalar,4,1> result;
-  result = coeffMatrix.fullPivLu().solve(rhs);
-
-  X.coeffRef(0,0) = result.coeff(0);
-  X.coeffRef(0,1) = result.coeff(1);
-  X.coeffRef(1,0) = result.coeff(2);
-  X.coeffRef(1,1) = result.coeff(3);
-}
-
-// similar to compute1x1offDiagonalBlock()
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(const MatrixType& T, typename MatrixType::Index i, typename MatrixType::Index j, ResultType& sqrtT)
-{
-  typedef typename traits<MatrixType>::Scalar Scalar;
-  Matrix<Scalar,2,2> A = sqrtT.template block<2,2>(i,i);
-  Matrix<Scalar,2,2> B = sqrtT.template block<2,2>(j,j);
-  Matrix<Scalar,2,2> C = T.template block<2,2>(i,j);
-  if (j-i > 2)
-    C -= sqrtT.block(i, i+2, 2, j-i-2) * sqrtT.block(i+2, j, j-i-2, 2);
-  Matrix<Scalar,2,2> X;
-  matrix_sqrt_quasi_triangular_solve_auxiliary_equation(X, A, B, C);
-  sqrtT.template block<2,2>(i,j) = X;
-}
-
-// pre:  T is quasi-upper-triangular and sqrtT is a zero matrix of the same size
-// post: the diagonal blocks of sqrtT are the square roots of the diagonal blocks of T
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_diagonal(const MatrixType& T, ResultType& sqrtT)
-{
-  using std::sqrt;
-  const Index size = T.rows();
-  for (Index i = 0; i < size; i++) {
-    if (i == size - 1 || T.coeff(i+1, i) == 0) {
-      eigen_assert(T(i,i) >= 0);
-      sqrtT.coeffRef(i,i) = sqrt(T.coeff(i,i));
-    }
-    else {
-      matrix_sqrt_quasi_triangular_2x2_diagonal_block(T, i, sqrtT);
-      ++i;
-    }
-  }
-}
-
-// pre:  T is quasi-upper-triangular and diagonal blocks of sqrtT are square root of diagonal blocks of T.
-// post: sqrtT is the square root of T.
-template <typename MatrixType, typename ResultType>
-void matrix_sqrt_quasi_triangular_off_diagonal(const MatrixType& T, ResultType& sqrtT)
-{
-  const Index size = T.rows();
-  for (Index j = 1; j < size; j++) {
-      if (T.coeff(j, j-1) != 0)  // if T(j-1:j, j-1:j) is a 2-by-2 block
-	continue;
-    for (Index i = j-1; i >= 0; i--) {
-      if (i > 0 && T.coeff(i, i-1) != 0)  // if T(i-1:i, i-1:i) is a 2-by-2 block
-	continue;
-      bool iBlockIs2x2 = (i < size - 1) && (T.coeff(i+1, i) != 0);
-      bool jBlockIs2x2 = (j < size - 1) && (T.coeff(j+1, j) != 0);
-      if (iBlockIs2x2 && jBlockIs2x2) 
-        matrix_sqrt_quasi_triangular_2x2_off_diagonal_block(T, i, j, sqrtT);
-      else if (iBlockIs2x2 && !jBlockIs2x2) 
-        matrix_sqrt_quasi_triangular_2x1_off_diagonal_block(T, i, j, sqrtT);
-      else if (!iBlockIs2x2 && jBlockIs2x2) 
-        matrix_sqrt_quasi_triangular_1x2_off_diagonal_block(T, i, j, sqrtT);
-      else if (!iBlockIs2x2 && !jBlockIs2x2) 
-        matrix_sqrt_quasi_triangular_1x1_off_diagonal_block(T, i, j, sqrtT);
-    }
-  }
-}
-
-} // end of namespace internal
-
-/** \ingroup MatrixFunctions_Module
-  * \brief Compute matrix square root of quasi-triangular matrix.
-  *
-  * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
-  *                      expected to be an instantiation of the Matrix class template.
-  * \tparam  ResultType  type of \p result, where result is to be stored.
-  * \param[in]  arg      argument of matrix square root.
-  * \param[out] result   matrix square root of upper Hessenberg part of \p arg.
-  *
-  * This function computes the square root of the upper quasi-triangular matrix stored in the upper
-  * Hessenberg part of \p arg.  Only the upper Hessenberg part of \p result is updated, the rest is
-  * not touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
-  *
-  * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
-  */
-template <typename MatrixType, typename ResultType> 
-void matrix_sqrt_quasi_triangular(const MatrixType &arg, ResultType &result)
-{
-  eigen_assert(arg.rows() == arg.cols());
-  result.resize(arg.rows(), arg.cols());
-  internal::matrix_sqrt_quasi_triangular_diagonal(arg, result);
-  internal::matrix_sqrt_quasi_triangular_off_diagonal(arg, result);
-}
-
-
-/** \ingroup MatrixFunctions_Module
-  * \brief Compute matrix square root of triangular matrix.
-  *
-  * \tparam  MatrixType  type of \p arg, the argument of matrix square root,
-  *                      expected to be an instantiation of the Matrix class template.
-  * \tparam  ResultType  type of \p result, where result is to be stored.
-  * \param[in]  arg      argument of matrix square root.
-  * \param[out] result   matrix square root of upper triangular part of \p arg.
-  *
-  * Only the upper triangular part (including the diagonal) of \p result is updated, the rest is not
-  * touched.  See MatrixBase::sqrt() for details on how this computation is implemented.
-  *
-  * \sa MatrixSquareRoot, MatrixSquareRootQuasiTriangular
-  */
-template <typename MatrixType, typename ResultType> 
-void matrix_sqrt_triangular(const MatrixType &arg, ResultType &result)
-{
-  using std::sqrt;
-      typedef typename MatrixType::Scalar Scalar;
-
-  eigen_assert(arg.rows() == arg.cols());
-
-  // Compute square root of arg and store it in upper triangular part of result
-  // This uses that the square root of triangular matrices can be computed directly.
-  result.resize(arg.rows(), arg.cols());
-  for (Index i = 0; i < arg.rows(); i++) {
-    result.coeffRef(i,i) = sqrt(arg.coeff(i,i));
-  }
-  for (Index j = 1; j < arg.cols(); j++) {
-    for (Index i = j-1; i >= 0; i--) {
-      // if i = j-1, then segment has length 0 so tmp = 0
-      Scalar tmp = (result.row(i).segment(i+1,j-i-1) * result.col(j).segment(i+1,j-i-1)).value();
-      // denominator may be zero if original matrix is singular
-      result.coeffRef(i,j) = (arg.coeff(i,j) - tmp) / (result.coeff(i,i) + result.coeff(j,j));
-    }
-  }
-}
-
-
-namespace internal {
-
-/** \ingroup MatrixFunctions_Module
-  * \brief Helper struct for computing matrix square roots of general matrices.
-  * \tparam  MatrixType  type of the argument of the matrix square root,
-  *                      expected to be an instantiation of the Matrix class template.
-  *
-  * \sa MatrixSquareRootTriangular, MatrixSquareRootQuasiTriangular, MatrixBase::sqrt()
-  */
-template <typename MatrixType, int IsComplex = NumTraits<typename internal::traits<MatrixType>::Scalar>::IsComplex>
-struct matrix_sqrt_compute
-{
-  /** \brief Compute the matrix square root
-    *
-    * \param[in]  arg     matrix whose square root is to be computed.
-    * \param[out] result  square root of \p arg.
-    *
-    * See MatrixBase::sqrt() for details on how this computation is implemented.
-    */
-  template <typename ResultType> static void run(const MatrixType &arg, ResultType &result);    
-};
-
-
-// ********** Partial specialization for real matrices **********
-
-template <typename MatrixType>
-struct matrix_sqrt_compute<MatrixType, 0>
-{
-  template <typename ResultType>
-  static void run(const MatrixType &arg, ResultType &result)
-  {
-    eigen_assert(arg.rows() == arg.cols());
-
-    // Compute Schur decomposition of arg
-    const RealSchur<MatrixType> schurOfA(arg);  
-    const MatrixType& T = schurOfA.matrixT();
-    const MatrixType& U = schurOfA.matrixU();
-    
-    // Compute square root of T
-    MatrixType sqrtT = MatrixType::Zero(arg.rows(), arg.cols());
-    matrix_sqrt_quasi_triangular(T, sqrtT);
-    
-    // Compute square root of arg
-    result = U * sqrtT * U.adjoint();
-  }
-};
-
-
-// ********** Partial specialization for complex matrices **********
-
-template <typename MatrixType>
-struct matrix_sqrt_compute<MatrixType, 1>
-{
-  template <typename ResultType>
-  static void run(const MatrixType &arg, ResultType &result)
-  {
-    eigen_assert(arg.rows() == arg.cols());
-
-    // Compute Schur decomposition of arg
-    const ComplexSchur<MatrixType> schurOfA(arg);  
-    const MatrixType& T = schurOfA.matrixT();
-    const MatrixType& U = schurOfA.matrixU();
-    
-    // Compute square root of T
-    MatrixType sqrtT;
-    matrix_sqrt_triangular(T, sqrtT);
-    
-    // Compute square root of arg
-    result = U * (sqrtT.template triangularView<Upper>() * U.adjoint());
-  }
-};
-
-} // end namespace internal
-
-/** \ingroup MatrixFunctions_Module
-  *
-  * \brief Proxy for the matrix square root of some matrix (expression).
-  *
-  * \tparam Derived  Type of the argument to the matrix square root.
-  *
-  * This class holds the argument to the matrix square root until it
-  * is assigned or evaluated for some other reason (so the argument
-  * should not be changed in the meantime). It is the return type of
-  * MatrixBase::sqrt() and most of the time this is the only way it is
-  * used.
-  */
-template<typename Derived> class MatrixSquareRootReturnValue
-: public ReturnByValue<MatrixSquareRootReturnValue<Derived> >
-{
-  protected:
-    typedef typename internal::ref_selector<Derived>::type DerivedNested;
-
-  public:
-    /** \brief Constructor.
-      *
-      * \param[in]  src  %Matrix (expression) forming the argument of the
-      * matrix square root.
-      */
-    explicit MatrixSquareRootReturnValue(const Derived& src) : m_src(src) { }
-
-    /** \brief Compute the matrix square root.
-      *
-      * \param[out]  result  the matrix square root of \p src in the
-      * constructor.
-      */
-    template <typename ResultType>
-    inline void evalTo(ResultType& result) const
-    {
-      typedef typename internal::nested_eval<Derived, 10>::type DerivedEvalType;
-      typedef typename internal::remove_all<DerivedEvalType>::type DerivedEvalTypeClean;
-      DerivedEvalType tmp(m_src);
-      internal::matrix_sqrt_compute<DerivedEvalTypeClean>::run(tmp, result);
-    }
-
-    Index rows() const { return m_src.rows(); }
-    Index cols() const { return m_src.cols(); }
-
-  protected:
-    const DerivedNested m_src;
-};
-
-namespace internal {
-template<typename Derived>
-struct traits<MatrixSquareRootReturnValue<Derived> >
-{
-  typedef typename Derived::PlainObject ReturnType;
-};
-}
-
-template <typename Derived>
-const MatrixSquareRootReturnValue<Derived> MatrixBase<Derived>::sqrt() const
-{
-  eigen_assert(rows() == cols());
-  return MatrixSquareRootReturnValue<Derived>(derived());
-}
-
-} // end namespace Eigen
-
-#endif // EIGEN_MATRIX_FUNCTION
diff --git a/third_party/allwpilib/wpimath/src/main/native/include/wpimath/MathShared.h b/third_party/allwpilib/wpimath/src/main/native/include/wpimath/MathShared.h
index ea23a88..f4a1795 100644
--- a/third_party/allwpilib/wpimath/src/main/native/include/wpimath/MathShared.h
+++ b/third_party/allwpilib/wpimath/src/main/native/include/wpimath/MathShared.h
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
 #include <memory>
 
-#include <wpi/Twine.h>
+#include <fmt/format.h>
+#include <wpi/SymbolExports.h>
 
 namespace wpi::math {
 
@@ -21,24 +19,52 @@
   kFilter_Linear,
   kOdometry_DifferentialDrive,
   kOdometry_SwerveDrive,
-  kOdometry_MecanumDrive
+  kOdometry_MecanumDrive,
+  kController_PIDController2,
+  kController_ProfiledPIDController,
 };
 
-class MathShared {
+class WPILIB_DLLEXPORT MathShared {
  public:
   virtual ~MathShared() = default;
-  virtual void ReportError(const wpi::Twine& error) = 0;
+  virtual void ReportErrorV(fmt::string_view format, fmt::format_args args) = 0;
+  virtual void ReportWarningV(fmt::string_view format,
+                              fmt::format_args args) = 0;
   virtual void ReportUsage(MathUsageId id, int count) = 0;
+
+  template <typename S, typename... Args>
+  inline void ReportError(const S& format, Args&&... args) {
+    ReportErrorV(format, fmt::make_args_checked<Args...>(format, args...));
+  }
+
+  template <typename S, typename... Args>
+  inline void ReportWarning(const S& format, Args&&... args) {
+    ReportWarningV(format, fmt::make_args_checked<Args...>(format, args...));
+  }
 };
 
-class MathSharedStore {
+class WPILIB_DLLEXPORT MathSharedStore {
  public:
   static MathShared& GetMathShared();
 
   static void SetMathShared(std::unique_ptr<MathShared> shared);
 
-  static void ReportError(const wpi::Twine& error) {
-    GetMathShared().ReportError(error);
+  static void ReportErrorV(fmt::string_view format, fmt::format_args args) {
+    GetMathShared().ReportErrorV(format, args);
+  }
+
+  template <typename S, typename... Args>
+  static inline void ReportError(const S& format, Args&&... args) {
+    ReportErrorV(format, fmt::make_args_checked<Args...>(format, args...));
+  }
+
+  static void ReportWarningV(fmt::string_view format, fmt::format_args args) {
+    GetMathShared().ReportWarningV(format, args);
+  }
+
+  template <typename S, typename... Args>
+  static inline void ReportWarning(const S& format, Args&&... args) {
+    ReportWarningV(format, fmt::make_args_checked<Args...>(format, args...));
   }
 
   static void ReportUsage(MathUsageId id, int count) {
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
index 2697c6c..4140fae 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/DrakeTest.java
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
 @SuppressWarnings({"ParameterName", "LocalVariableName"})
 public class DrakeTest {
   public static void assertMatrixEqual(SimpleMatrix A, SimpleMatrix B) {
@@ -24,8 +20,8 @@
     }
   }
 
-  private boolean solveDAREandVerify(SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q,
-                                     SimpleMatrix R) {
+  private boolean solveDAREandVerify(
+      SimpleMatrix A, SimpleMatrix B, SimpleMatrix Q, SimpleMatrix R) {
     var X = Drake.discreteAlgebraicRiccatiEquation(A, B, Q, R);
 
     // expect that x is the same as it's transpose
@@ -33,11 +29,19 @@
     assertMatrixEqual(X, X.transpose());
 
     // Verify that this is a solution to the DARE.
-    SimpleMatrix Y = A.transpose().mult(X).mult(A)
+    SimpleMatrix Y =
+        A.transpose()
+            .mult(X)
+            .mult(A)
             .minus(X)
-            .minus(A.transpose().mult(X).mult(B)
-                    .mult(((B.transpose().mult(X).mult(B)).plus(R))
-                            .invert()).mult(B.transpose()).mult(X).mult(A))
+            .minus(
+                A.transpose()
+                    .mult(X)
+                    .mult(B)
+                    .mult(((B.transpose().mult(X).mult(B)).plus(R)).invert())
+                    .mult(B.transpose())
+                    .mult(X)
+                    .mult(A))
             .plus(Q);
     assertMatrixEqual(Y, new SimpleMatrix(Y.numRows(), Y.numCols()));
 
@@ -50,20 +54,21 @@
     int m1 = 1;
 
     // we know from Scipy that this should be [[0.05048525 0.10097051 0.20194102 0.40388203]]
-    SimpleMatrix A1 = new SimpleMatrix(n1, n1, true, new double[]{0.5, 1, 0, 0, 0, 0, 1,
-        0, 0, 0, 0, 1, 0, 0, 0, 0}).transpose();
-    SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[]{0, 0, 0, 1});
-    SimpleMatrix Q1 = new SimpleMatrix(n1, n1, true, new double[]{1, 0,
-                                                                  0, 0, 0, 0, 0, 0, 0, 0,
-                                                                  0, 0, 0, 0, 0, 0});
-    SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[]{0.25});
+    SimpleMatrix A1 =
+        new SimpleMatrix(
+                n1, n1, true, new double[] {0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0})
+            .transpose();
+    SimpleMatrix B1 = new SimpleMatrix(n1, m1, true, new double[] {0, 0, 0, 1});
+    SimpleMatrix Q1 =
+        new SimpleMatrix(
+            n1, n1, true, new double[] {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0});
+    SimpleMatrix R1 = new SimpleMatrix(m1, m1, true, new double[] {0.25});
     assertTrue(solveDAREandVerify(A1, B1, Q1, R1));
 
-    SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[]{1, 1, 0, 1});
-    SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[]{0, 1});
-    SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[]{1, 0, 0, 0});
-    SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[]{0.3});
+    SimpleMatrix A2 = new SimpleMatrix(2, 2, true, new double[] {1, 1, 0, 1});
+    SimpleMatrix B2 = new SimpleMatrix(2, 1, true, new double[] {0, 1});
+    SimpleMatrix Q2 = new SimpleMatrix(2, 2, true, new double[] {1, 0, 0, 0});
+    SimpleMatrix R2 = new SimpleMatrix(1, 1, true, new double[] {0.3});
     assertTrue(solveDAREandVerify(A2, B2, Q2, R2));
-
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
new file mode 100644
index 0000000..bb116ce
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MathUtilTest.java
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class MathUtilTest {
+  @Test
+  void testApplyDeadband() {
+    // < 0
+    assertEquals(-1.0, MathUtil.applyDeadband(-1.0, 0.02));
+    assertEquals((-0.03 + 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(-0.03, 0.02));
+    assertEquals(0.0, MathUtil.applyDeadband(-0.02, 0.02));
+    assertEquals(0.0, MathUtil.applyDeadband(-0.01, 0.02));
+
+    // == 0
+    assertEquals(0.0, MathUtil.applyDeadband(0.0, 0.02));
+
+    // > 0
+    assertEquals(0.0, MathUtil.applyDeadband(0.01, 0.02));
+    assertEquals(0.0, MathUtil.applyDeadband(0.02, 0.02));
+    assertEquals((0.03 - 0.02) / (1.0 - 0.02), MathUtil.applyDeadband(0.03, 0.02));
+    assertEquals(1.0, MathUtil.applyDeadband(1.0, 0.02));
+  }
+
+  @Test
+  void testInputModulus() {
+    // These tests check error wrapping. That is, the result of wrapping the
+    // result of an angle reference minus the measurement.
+
+    // Test symmetric range
+    assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0), -180.0, 180.0));
+    assertEquals(-20.0, MathUtil.inputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
+    assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
+    assertEquals(20.0, MathUtil.inputModulus(-170.0 - 170.0, -180.0, 180.0));
+    assertEquals(20.0, MathUtil.inputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
+    assertEquals(20.0, MathUtil.inputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
+
+    // Test range start at zero
+    assertEquals(340.0, MathUtil.inputModulus(170.0 - 190.0, 0.0, 360.0));
+    assertEquals(340.0, MathUtil.inputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
+    assertEquals(340.0, MathUtil.inputModulus(170.0 - (190.0 + 360), 0.0, 360.0));
+
+    // Test asymmetric range that doesn't start at zero
+    assertEquals(-20.0, MathUtil.inputModulus(170.0 - (-170.0), -170.0, 190.0));
+
+    // Test range with both positive endpoints
+    assertEquals(2.0, MathUtil.inputModulus(0.0, 1.0, 3.0));
+    assertEquals(3.0, MathUtil.inputModulus(1.0, 1.0, 3.0));
+    assertEquals(2.0, MathUtil.inputModulus(2.0, 1.0, 3.0));
+    assertEquals(3.0, MathUtil.inputModulus(3.0, 1.0, 3.0));
+    assertEquals(2.0, MathUtil.inputModulus(4.0, 1.0, 3.0));
+  }
+
+  @Test
+  void testAngleModulus() {
+    assertEquals(MathUtil.angleModulus(Math.toRadians(-2000)), Math.toRadians(160), 1e-6);
+    assertEquals(MathUtil.angleModulus(Math.toRadians(358)), Math.toRadians(-2), 1e-6);
+    assertEquals(MathUtil.angleModulus(Math.toRadians(360)), 0, 1e-6);
+
+    assertEquals(MathUtil.angleModulus(5 * Math.PI), Math.PI);
+    assertEquals(MathUtil.angleModulus(-5 * Math.PI), Math.PI);
+    assertEquals(MathUtil.angleModulus(Math.PI / 2), Math.PI / 2);
+    assertEquals(MathUtil.angleModulus(-Math.PI / 2), -Math.PI / 2);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
new file mode 100644
index 0000000..45df8fe
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/MatrixTest.java
@@ -0,0 +1,134 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N4;
+import org.ejml.data.SingularMatrixException;
+import org.junit.jupiter.api.Test;
+
+public class MatrixTest {
+  @Test
+  void testMatrixMultiplication() {
+    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 0.0, 1.0);
+    var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 0.0, 0.0, 2.5);
+
+    Matrix<N2, N2> result = mat1.times(mat2);
+
+    assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5));
+
+    var mat3 = Matrix.mat(Nat.N2(), Nat.N3()).fill(1.0, 3.0, 0.5, 2.0, 4.3, 1.2);
+    var mat4 =
+        Matrix.mat(Nat.N3(), Nat.N4())
+            .fill(3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0);
+
+    Matrix<N2, N4> result2 = mat3.times(mat4);
+
+    assertTrue(
+        Matrix.mat(Nat.N2(), Nat.N4())
+            .fill(12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53)
+            .isEqual(result2, 1E-9));
+  }
+
+  @Test
+  void testMatrixVectorMultiplication() {
+    var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 1.0, 0.0, 1.0);
+
+    var vec = VecBuilder.fill(3.0, 2.0);
+
+    Matrix<N2, N1> result = mat.times(vec);
+    assertEquals(VecBuilder.fill(5.0, 2.0), result);
+  }
+
+  @Test
+  void testTranspose() {
+    Matrix<N3, N1> vec = VecBuilder.fill(1.0, 2.0, 3.0);
+
+    Matrix<N1, N3> transpose = vec.transpose();
+
+    assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
+  }
+
+  @Test
+  void testSolve() {
+    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+    var vec1 = VecBuilder.fill(1.0, 2.0);
+
+    var solve1 = mat1.solve(vec1);
+
+    assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
+
+    var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
+    var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
+
+    var solve2 = mat2.solve(vec2);
+
+    assertEquals(VecBuilder.fill(0.0, 0.5), solve2);
+  }
+
+  @Test
+  void testInverse() {
+    var mat = Matrix.mat(Nat.N3(), Nat.N3()).fill(1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5);
+
+    var inv = mat.inv();
+
+    assertTrue(Matrix.eye(Nat.N3()).isEqual(mat.times(inv), 1E-9));
+
+    assertTrue(Matrix.eye(Nat.N3()).isEqual(inv.times(mat), 1E-9));
+  }
+
+  @Test
+  void testUninvertableMatrix() {
+    var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 1.0, 2.0, 1.0);
+
+    assertThrows(SingularMatrixException.class, singularMatrix::inv);
+  }
+
+  @Test
+  void testMatrixScalarArithmetic() {
+    var mat = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0), mat.times(2.0));
+
+    assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(mat.div(2.0), 1E-3));
+  }
+
+  @Test
+  void testMatrixMatrixArithmetic() {
+    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
+
+    var mat2 = Matrix.mat(Nat.N2(), Nat.N2()).fill(5.0, 6.0, 7.0, 8.0);
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0), mat1.minus(mat2));
+
+    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0), mat1.plus(mat2));
+  }
+
+  @Test
+  void testMatrixExponential() {
+    var matrix = Matrix.eye(Nat.N2());
+    var result = matrix.exp();
+
+    assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
+
+    matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
+    result = matrix.times(0.01).exp();
+
+    assertTrue(
+        result.isEqual(
+            Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
+            1E-8));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
new file mode 100644
index 0000000..456506c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -0,0 +1,185 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import java.util.ArrayList;
+import java.util.List;
+import org.ejml.dense.row.MatrixFeatures_DDRM;
+import org.ejml.simple.SimpleMatrix;
+import org.junit.jupiter.api.Test;
+
+public class StateSpaceUtilTest {
+  @Test
+  public void testCostArray() {
+    var mat = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(1.0, 2.0, 3.0));
+
+    assertEquals(1.0, mat.get(0, 0), 1e-3);
+    assertEquals(0.0, mat.get(0, 1), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 0), 1e-3);
+    assertEquals(1.0 / 4.0, mat.get(1, 1), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(1.0 / 9.0, mat.get(2, 2), 1e-3);
+  }
+
+  @Test
+  public void testCovArray() {
+    var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(), VecBuilder.fill(1.0, 2.0, 3.0));
+
+    assertEquals(1.0, mat.get(0, 0), 1e-3);
+    assertEquals(0.0, mat.get(0, 1), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 0), 1e-3);
+    assertEquals(4.0, mat.get(1, 1), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(0.0, mat.get(0, 2), 1e-3);
+    assertEquals(0.0, mat.get(1, 2), 1e-3);
+    assertEquals(9.0, mat.get(2, 2), 1e-3);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testIsStabilizable() {
+    Matrix<N2, N2> A;
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    // First eigenvalue is uncontrollable and unstable.
+    // Second eigenvalue is controllable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and marginally stable.
+    // Second eigenvalue is controllable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and stable.
+    // Second eigenvalue is controllable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
+    assertTrue(StateSpaceUtil.isStabilizable(A, B));
+
+    // First eigenvalue is uncontrollable and stable.
+    // Second eigenvalue is controllable and unstable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
+    assertTrue(StateSpaceUtil.isStabilizable(A, B));
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testIsDetectable() {
+    Matrix<N2, N2> A;
+    Matrix<N1, N2> C = Matrix.mat(Nat.N1(), Nat.N2()).fill(0, 1);
+
+    // First eigenvalue is unobservable and unstable.
+    // Second eigenvalue is observable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and marginally stable.
+    // Second eigenvalue is observable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
+    assertFalse(StateSpaceUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and stable.
+    // Second eigenvalue is observable and stable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
+    assertTrue(StateSpaceUtil.isDetectable(A, C));
+
+    // First eigenvalue is unobservable and stable.
+    // Second eigenvalue is observable and unstable.
+    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
+    assertTrue(StateSpaceUtil.isDetectable(A, C));
+  }
+
+  @Test
+  public void testMakeWhiteNoiseVector() {
+    var firstData = new ArrayList<Double>();
+    var secondData = new ArrayList<Double>();
+    for (int i = 0; i < 1000; i++) {
+      var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
+      firstData.add(noiseVec.get(0, 0));
+      secondData.add(noiseVec.get(1, 0));
+    }
+    assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
+    assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
+  }
+
+  private double calculateStandardDeviation(List<Double> numArray) {
+    double sum = 0.0;
+    double standardDeviation = 0.0;
+    int length = numArray.size();
+
+    for (double num : numArray) {
+      sum += num;
+    }
+
+    double mean = sum / length;
+
+    for (double num : numArray) {
+      standardDeviation += Math.pow(num - mean, 2);
+    }
+
+    return Math.sqrt(standardDeviation / length);
+  }
+
+  @Test
+  public void testMatrixExp() {
+    Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
+    var wrappedResult = wrappedMatrix.exp();
+
+    assertTrue(
+        wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
+
+    var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
+    wrappedResult = matrix.times(0.01).exp();
+
+    assertTrue(
+        wrappedResult.isEqual(
+            Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912, 0.03076368, 1.04111993),
+            1E-8));
+  }
+
+  @Test
+  public void testSimpleMatrixExp() {
+    SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
+    var result = SimpleMatrixUtils.exp(matrix);
+
+    assertTrue(
+        MatrixFeatures_DDRM.isIdentical(
+            result.getDDRM(),
+            new SimpleMatrix(2, 2, true, new double[] {Math.E, 0, 0, Math.E}).getDDRM(),
+            1E-9));
+
+    matrix = new SimpleMatrix(2, 2, true, new double[] {1, 2, 3, 4});
+    result = SimpleMatrixUtils.exp(matrix.scale(0.01));
+
+    assertTrue(
+        MatrixFeatures_DDRM.isIdentical(
+            result.getDDRM(),
+            new SimpleMatrix(
+                    2, 2, true, new double[] {1.01035625, 0.02050912, 0.03076368, 1.04111993})
+                .getDDRM(),
+            1E-8));
+  }
+
+  @Test
+  public void testPoseToVector() {
+    Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
+    var vector = StateSpaceUtil.poseToVector(pose);
+    assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
+    assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
+    assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
index 10ec44c..6a1bf2e 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/WPIMathJNITest.java
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.math;
 
-import org.junit.jupiter.api.Test;
-
 import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
 
+import org.junit.jupiter.api.Test;
+
 public class WPIMathJNITest {
   @Test
   public void testLink() {
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
new file mode 100644
index 0000000..f64608e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ControlAffinePlantInversionFeedforwardTest.java
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+class ControlAffinePlantInversionFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<N2, N1>(
+            Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+
+    assertEquals(
+        48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculateState() {
+    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
+        new ControlAffinePlantInversionFeedforward<N2, N1>(
+            Nat.N2(), Nat.N1(), this::getDynamics, 0.02);
+
+    assertEquals(
+        48.0, feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0), 1e-6);
+  }
+
+  @SuppressWarnings("ParameterName")
+  protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
+    return Matrix.mat(Nat.N2(), Nat.N2())
+        .fill(1.000, 0, 0, 1.000)
+        .times(x)
+        .plus(VecBuilder.fill(0, 1).times(u));
+  }
+
+  @SuppressWarnings("ParameterName")
+  protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
+    return Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java
new file mode 100644
index 0000000..db3f6dc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/HolonomicDriveControllerTest.java
@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import java.util.ArrayList;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class HolonomicDriveControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  void testReachesReference() {
+    HolonomicDriveController controller =
+        new HolonomicDriveController(
+            new PIDController(1.0, 0.0, 0.0),
+            new PIDController(1.0, 0.0, 0.0),
+            new ProfiledPIDController(
+                1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2.0 * Math.PI, Math.PI)));
+    Pose2d robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    List<Pose2d> waypoints = new ArrayList<>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.8)));
+
+    TrajectoryConfig config = new TrajectoryConfig(8.0, 4.0);
+    Trajectory trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final double kDt = 0.02;
+    final double kTotalTime = trajectory.getTotalTimeSeconds();
+
+    for (int i = 0; i < (kTotalTime / kDt); i++) {
+      Trajectory.State state = trajectory.sample(kDt * i);
+      ChassisSpeeds output = controller.calculate(robotPose, state, new Rotation2d());
+
+      robotPose =
+          robotPose.exp(
+              new Twist2d(
+                  output.vxMetersPerSecond * kDt,
+                  output.vyMetersPerSecond * kDt,
+                  output.omegaRadiansPerSecond * kDt));
+    }
+
+    final List<Trajectory.State> states = trajectory.getStates();
+    final Pose2d endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final Pose2d finalRobotPose = robotPose;
+
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+
+  @Test
+  void testDoesNotRotateUnnecessarily() {
+    var controller =
+        new HolonomicDriveController(
+            new PIDController(1, 0, 0),
+            new PIDController(1, 0, 0),
+            new ProfiledPIDController(1, 0, 0, new TrapezoidProfile.Constraints(4, 2)));
+
+    ChassisSpeeds speeds =
+        controller.calculate(
+            new Pose2d(0, 0, new Rotation2d(1.57)), new Pose2d(), 0, new Rotation2d(1.57));
+
+    assertEquals(0.0, speeds.omegaRadiansPerSecond);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
new file mode 100644
index 0000000..98b0e6c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearPlantInversionFeedforwardTest.java
@@ -0,0 +1,31 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+class LinearPlantInversionFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
+
+    LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
+        new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
+
+    assertEquals(
+        47.502599,
+        feedforward.calculate(VecBuilder.fill(2, 2), VecBuilder.fill(3, 3)).get(0, 0),
+        0.002);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
new file mode 100644
index 0000000..5d9c2b8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearQuadraticRegulatorTest.java
@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+public class LinearQuadraticRegulatorTest {
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testLQROnElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+
+    var plant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+
+    var qElms = VecBuilder.fill(0.02, 0.4);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
+
+    var k = controller.getK();
+
+    assertEquals(522.153, k.get(0, 0), 0.1);
+    assertEquals(38.2, k.get(0, 1), 0.1);
+  }
+
+  @Test
+  public void testFourMotorElevator() {
+    var dt = 0.020;
+
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
+
+    var regulator =
+        new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
+
+    assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
+    assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testLQROnArm() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 4.0;
+    var r = 0.4;
+    var G = 100.0;
+
+    var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
+
+    var qElms = VecBuilder.fill(0.01745, 0.08726);
+    var rElms = VecBuilder.fill(12.0);
+    var dt = 0.00505;
+
+    var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, dt);
+
+    var k = controller.getK();
+
+    assertEquals(19.16, k.get(0, 0), 0.1);
+    assertEquals(3.32, k.get(0, 1), 0.1);
+  }
+
+  @Test
+  public void testLatencyCompensate() {
+    var dt = 0.02;
+
+    var plant =
+        LinearSystemId.createElevatorSystem(
+            DCMotor.getVex775Pro(4), 8.0, 0.75 * 25.4 / 1000.0, 14.67);
+
+    var regulator =
+        new LinearQuadraticRegulator<>(plant, VecBuilder.fill(0.1, 0.2), VecBuilder.fill(12.0), dt);
+
+    regulator.latencyCompensate(plant, dt, 0.01);
+
+    assertEquals(8.97115941, regulator.getK().get(0, 0), 1e-3);
+    assertEquals(0.07904881, regulator.getK().get(0, 1), 1e-3);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
new file mode 100644
index 0000000..597ae7f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/LinearSystemLoopTest.java
@@ -0,0 +1,128 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.estimator.KalmanFilter;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.LinearSystemLoop;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+public class LinearSystemLoopTest {
+  public static final double kDt = 0.00505;
+  private static final double kPositionStddev = 0.0001;
+  private static final Random random = new Random();
+
+  LinearSystem<N2, N1, N1> m_plant =
+      LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5, 0.0181864, 1.0);
+
+  KalmanFilter<N2, N1, N1> m_observer =
+      new KalmanFilter<>(
+          Nat.N2(), Nat.N1(), m_plant, VecBuilder.fill(0.05, 1.0), VecBuilder.fill(0.0001), kDt);
+
+  LinearQuadraticRegulator<N2, N1, N1> m_controller =
+      new LinearQuadraticRegulator<>(
+          m_plant, VecBuilder.fill(0.02, 0.4), VecBuilder.fill(12.0), 0.00505);
+
+  private final LinearSystemLoop<N2, N1, N1> m_loop =
+      new LinearSystemLoop<>(m_plant, m_controller, m_observer, 12, 0.00505);
+
+  @SuppressWarnings("LocalVariableName")
+  private static void updateTwoState(
+      LinearSystem<N2, N1, N1> plant, LinearSystemLoop<N2, N1, N1> loop, double noise) {
+    Matrix<N1, N1> y = plant.calculateY(loop.getXHat(), loop.getU()).plus(VecBuilder.fill(noise));
+
+    loop.correct(y);
+    loop.predict(kDt);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testStateSpaceEnabled() {
+    m_loop.reset(VecBuilder.fill(0, 0));
+    Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
+    var constraints = new TrapezoidProfile.Constraints(4, 3);
+    m_loop.setNextR(references);
+
+    TrapezoidProfile profile;
+    TrapezoidProfile.State state;
+    for (int i = 0; i < 1000; i++) {
+      profile =
+          new TrapezoidProfile(
+              constraints,
+              new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
+              new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0)));
+      state = profile.calculate(kDt);
+      m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
+
+      updateTwoState(m_plant, m_loop, (random.nextGaussian()) * kPositionStddev);
+      var u = m_loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+    }
+
+    assertEquals(2.0, m_loop.getXHat(0), 0.05);
+    assertEquals(0.0, m_loop.getXHat(1), 0.5);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testFlywheelEnabled() {
+    LinearSystem<N1, N1, N1> plant =
+        LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00289, 1.0);
+    KalmanFilter<N1, N1, N1> observer =
+        new KalmanFilter<>(
+            Nat.N1(), Nat.N1(), plant, VecBuilder.fill(1.0), VecBuilder.fill(kPositionStddev), kDt);
+
+    var qElms = VecBuilder.fill(9.0);
+    var rElms = VecBuilder.fill(12.0);
+
+    var controller = new LinearQuadraticRegulator<>(plant, qElms, rElms, kDt);
+
+    var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
+
+    var loop = new LinearSystemLoop<>(controller, feedforward, observer, 12);
+
+    loop.reset(VecBuilder.fill(0.0));
+    var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
+
+    loop.setNextR(references);
+
+    var time = 0.0;
+    while (time < 10) {
+      if (time > 10) {
+        break;
+      }
+
+      loop.setNextR(references);
+
+      Matrix<N1, N1> y =
+          plant
+              .calculateY(loop.getXHat(), loop.getU())
+              .plus(VecBuilder.fill(random.nextGaussian() * kPositionStddev));
+
+      loop.correct(y);
+      loop.predict(kDt);
+      var u = loop.getU(0);
+
+      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
+
+      time += kDt;
+    }
+
+    assertEquals(0.0, loop.getError(0), 0.1);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
new file mode 100644
index 0000000..1fe4cb1
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/PIDInputOutputTest.java
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class PIDInputOutputTest {
+  private PIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new PIDController(0, 0, 0);
+  }
+
+  @Test
+  void continuousInputTest() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+    assertEquals(m_controller.calculate(-179, 179), -2, 1e-5);
+
+    m_controller.enableContinuousInput(0, 360);
+    assertEquals(m_controller.calculate(1, 359), -2, 1e-5);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
new file mode 100644
index 0000000..b525f49
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/PIDToleranceTest.java
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class PIDToleranceTest {
+  private static final double kSetpoint = 50.0;
+  private static final double kTolerance = 10.0;
+  private static final double kRange = 200;
+
+  @Test
+  void initialToleranceTest() {
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+      assertTrue(controller.atSetpoint());
+    }
+  }
+
+  @Test
+  void absoluteToleranceTest() {
+    try (var controller = new PIDController(0.05, 0.0, 0.0)) {
+      controller.enableContinuousInput(-kRange / 2, kRange / 2);
+
+      assertTrue(
+          controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+              + controller.getPositionError());
+
+      controller.setTolerance(kTolerance);
+      controller.setSetpoint(kSetpoint);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getPositionError());
+
+      controller.calculate(0.0);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getPositionError());
+
+      controller.calculate(kSetpoint + kTolerance / 2);
+
+      assertTrue(
+          controller.atSetpoint(),
+          "Error was not in tolerance when it should have been. Error was "
+              + controller.getPositionError());
+
+      controller.calculate(kSetpoint + 10 * kTolerance);
+
+      assertFalse(
+          controller.atSetpoint(),
+          "Error was in tolerance when it should not have been. Error was "
+              + controller.getPositionError());
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDControllerTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDControllerTest.java
new file mode 100644
index 0000000..d87943a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDControllerTest.java
@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import org.junit.jupiter.api.Test;
+
+class ProfiledPIDControllerTest {
+  @Test
+  void testStartFromNonZeroPosition() {
+    ProfiledPIDController controller =
+        new ProfiledPIDController(1.0, 0.0, 0.0, new TrapezoidProfile.Constraints(1.0, 1.0));
+
+    controller.reset(20);
+
+    assertEquals(0.0, controller.calculate(20), 0.05);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDInputOutputTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDInputOutputTest.java
new file mode 100644
index 0000000..e0a4945
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/ProfiledPIDInputOutputTest.java
@@ -0,0 +1,114 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import org.junit.jupiter.api.BeforeEach;
+import org.junit.jupiter.api.Test;
+
+class ProfiledPIDInputOutputTest {
+  private ProfiledPIDController m_controller;
+
+  @BeforeEach
+  void setUp() {
+    m_controller = new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(360, 180));
+  }
+
+  @Test
+  void continuousInputTest1() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-180, 180);
+
+    final double kSetpoint = -179.0;
+    final double kMeasurement = -179.0;
+    final double kGoal = 179.0;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < 180.0);
+  }
+
+  @Test
+  void continuousInputTest2() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.4826633343199735;
+    final double kMeasurement = -3.1352207333939606;
+    final double kGoal = -3.534162788601621;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest3() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(-Math.PI, Math.PI);
+
+    final double kSetpoint = -3.5176604690006377;
+    final double kMeasurement = 3.1191729343822456;
+    final double kGoal = 2.709680418117445;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI);
+  }
+
+  @Test
+  void continuousInputTest4() {
+    m_controller.setP(1);
+    m_controller.enableContinuousInput(0, 2.0 * Math.PI);
+
+    final double kSetpoint = 2.78;
+    final double kMeasurement = 3.12;
+    final double kGoal = 2.71;
+
+    m_controller.reset(kSetpoint);
+    assertTrue(m_controller.calculate(kMeasurement, kGoal) < 0.0);
+
+    // Error must be less than half the input range at all times
+    assertTrue(Math.abs(m_controller.getSetpoint().position - kMeasurement) < Math.PI / 2.0);
+  }
+
+  @Test
+  void proportionalGainOutputTest() {
+    m_controller.setP(4);
+
+    assertEquals(-0.1, m_controller.calculate(0.025, 0), 1e-5);
+  }
+
+  @Test
+  void integralGainOutputTest() {
+    m_controller.setI(4);
+
+    double out = 0;
+
+    for (int i = 0; i < 5; i++) {
+      out = m_controller.calculate(0.025, 0);
+    }
+
+    assertEquals(-0.5 * m_controller.getPeriod(), out, 1e-5);
+  }
+
+  @Test
+  void derivativeGainOutputTest() {
+    m_controller.setD(4);
+
+    m_controller.calculate(0, 0);
+
+    assertEquals(-0.01 / m_controller.getPeriod(), m_controller.calculate(0.0025, 0), 1e-5);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java
new file mode 100644
index 0000000..813bf42
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/RamseteControllerTest.java
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Twist2d;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.ArrayList;
+import org.junit.jupiter.api.Test;
+
+class RamseteControllerTest {
+  private static final double kTolerance = 1 / 12.0;
+  private static final double kAngularTolerance = Math.toRadians(2);
+
+  @Test
+  void testReachesReference() {
+    final var controller = new RamseteController(2.0, 0.7);
+    var robotPose = new Pose2d(2.7, 23.0, Rotation2d.fromDegrees(0.0));
+
+    final var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(new Pose2d(2.75, 22.521, new Rotation2d(0)));
+    waypoints.add(new Pose2d(24.73, 19.68, new Rotation2d(5.846)));
+    var config = new TrajectoryConfig(8.8, 0.1);
+    final var trajectory = TrajectoryGenerator.generateTrajectory(waypoints, config);
+
+    final double kDt = 0.02;
+    final var totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / kDt); ++i) {
+      var state = trajectory.sample(kDt * i);
+
+      var output = controller.calculate(robotPose, state);
+      robotPose =
+          robotPose.exp(
+              new Twist2d(output.vxMetersPerSecond * kDt, 0, output.omegaRadiansPerSecond * kDt));
+    }
+
+    final var states = trajectory.getStates();
+    final var endPose = states.get(states.size() - 1).poseMeters;
+
+    // Java lambdas require local variables referenced from a lambda expression
+    // must be final or effectively final.
+    final var finalRobotPose = robotPose;
+    assertAll(
+        () -> assertEquals(endPose.getX(), finalRobotPose.getX(), kTolerance),
+        () -> assertEquals(endPose.getY(), finalRobotPose.getY(), kTolerance),
+        () ->
+            assertEquals(
+                0.0,
+                MathUtil.angleModulus(
+                    endPose.getRotation().getRadians() - finalRobotPose.getRotation().getRadians()),
+                kAngularTolerance));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
new file mode 100644
index 0000000..83fced2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/controller/SimpleMotorFeedforwardTest.java
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.controller;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+class SimpleMotorFeedforwardTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCalculate() {
+    double Ks = 0.5;
+    double Kv = 3.0;
+    double Ka = 0.6;
+    double dt = 0.02;
+
+    var A = Matrix.mat(Nat.N1(), Nat.N1()).fill(-Kv / Ka);
+    var B = Matrix.mat(Nat.N1(), Nat.N1()).fill(1.0 / Ka);
+
+    var plantInversion = new LinearPlantInversionFeedforward<N1, N1, N1>(A, B, dt);
+    var simpleMotor = new SimpleMotorFeedforward(Ks, Kv, Ka);
+
+    var r = VecBuilder.fill(2.0);
+    var nextR = VecBuilder.fill(3.0);
+
+    assertEquals(37.524995834325161 + 0.5, simpleMotor.calculate(2.0, 3.0, dt), 0.002);
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + Ks,
+        simpleMotor.calculate(2.0, 3.0, dt),
+        0.002);
+
+    // These won't match exactly. It's just an approximation to make sure they're
+    // in the same ballpark.
+    assertEquals(
+        plantInversion.calculate(r, nextR).get(0, 0) + Ks,
+        simpleMotor.calculate(2.0, 1.0 / dt),
+        2.0);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
new file mode 100644
index 0000000..9fcf5e3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/AngleStatisticsTest.java
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import org.junit.jupiter.api.Test;
+
+public class AngleStatisticsTest {
+  @Test
+  public void testMean() {
+    // 3 states, 2 sigmas
+    var sigmas =
+        Matrix.mat(Nat.N3(), Nat.N2()).fill(1, 1.2, Math.toRadians(359), Math.toRadians(3), 1, 2);
+    // Weights need to produce the mean of the sigmas
+    var weights = new Matrix<>(Nat.N2(), Nat.N1());
+    weights.fill(1.0 / sigmas.getNumCols());
+
+    assertTrue(
+        AngleStatistics.angleMean(sigmas, weights, 1)
+            .isEqual(VecBuilder.fill(1.1, Math.toRadians(1), 1.5), 1e-6));
+  }
+
+  @Test
+  public void testResidual() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(
+        AngleStatistics.angleResidual(first, second, 1)
+            .isEqual(VecBuilder.fill(0, Math.toRadians(2), 1), 1e-6));
+  }
+
+  @Test
+  public void testAdd() {
+    var first = VecBuilder.fill(1, Math.toRadians(1), 2);
+    var second = VecBuilder.fill(1, Math.toRadians(359), 1);
+    assertTrue(AngleStatistics.angleAdd(first, second, 1).isEqual(VecBuilder.fill(2, 0, 3), 1e-6));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
new file mode 100644
index 0000000..6e4b261
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/DifferentialDrivePoseEstimatorTest.java
@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+public class DifferentialDrivePoseEstimatorTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  public void testAccuracy() {
+    var estimator =
+        new DifferentialDrivePoseEstimator(
+            new Rotation2d(),
+            new Pose2d(),
+            new MatBuilder<>(Nat.N5(), Nat.N1()).fill(0.02, 0.02, 0.01, 0.02, 0.02),
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.01, 0.01, 0.001),
+            new MatBuilder<>(Nat.N3(), Nat.N1()).fill(0.1, 0.1, 0.01));
+
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(10, 5));
+
+    var kinematics = new DifferentialDriveKinematics(1);
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double distanceLeft = 0.0;
+    double distanceRight = 0.0;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    Trajectory.State groundtruthState;
+    DifferentialDriveWheelSpeeds input;
+    while (t <= traj.getTotalTimeSeconds()) {
+      groundtruthState = traj.sample(t);
+      input =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundtruthState.velocityMetersPerSecond,
+                  0.0,
+                  // ds/dt * dtheta/ds = dtheta/dt
+                  groundtruthState.velocityMetersPerSecond
+                      * groundtruthState.curvatureRadPerMeter));
+
+      if (lastVisionUpdateTime + visionUpdateRate + rand.nextGaussian() * 0.4 < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+        var groundPose = groundtruthState.poseMeters;
+        lastVisionPose =
+            new Pose2d(
+                new Translation2d(
+                    groundPose.getTranslation().getX() + rand.nextGaussian() * 0.1,
+                    groundPose.getTranslation().getY() + rand.nextGaussian() * 0.1),
+                new Rotation2d(rand.nextGaussian() * 0.01).plus(groundPose.getRotation()));
+        lastVisionUpdateTime = t;
+      }
+
+      input.leftMetersPerSecond += rand.nextGaussian() * 0.01;
+      input.rightMetersPerSecond += rand.nextGaussian() * 0.01;
+
+      distanceLeft += input.leftMetersPerSecond * dt;
+      distanceRight += input.rightMetersPerSecond * dt;
+
+      var rotNoise = new Rotation2d(rand.nextGaussian() * 0.001);
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              groundtruthState.poseMeters.getRotation().plus(rotNoise),
+              input,
+              distanceLeft,
+              distanceRight);
+
+      double error =
+          groundtruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(0.0, errorSum / (traj.getTotalTimeSeconds() / dt), 0.035, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.055, "Incorrect max error");
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
new file mode 100644
index 0000000..18b095b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/ExtendedKalmanFilterTest.java
@@ -0,0 +1,189 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N5;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.Arrays;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+@SuppressWarnings("ParameterName")
+public class ExtendedKalmanFilterTest {
+  public static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
+    final var motors = DCMotor.getCIM(2);
+
+    final var gr = 7.08; // Gear ratio
+    final var rb = 0.8382 / 2.0; // Wheelbase radius (track width)
+    final var r = 0.0746125; // Wheel radius
+    final var m = 63.503; // Robot mass
+    final var J = 5.6; // Robot moment of inertia
+
+    final var C1 =
+        -Math.pow(gr, 2) * motors.KtNMPerAmp / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
+    final var C2 = gr * motors.KtNMPerAmp / (motors.rOhms * r);
+    final var k1 = 1.0 / m + rb * rb / J;
+    final var k2 = 1.0 / m - rb * rb / J;
+
+    final var vl = x.get(3, 0);
+    final var vr = x.get(4, 0);
+    final var Vl = u.get(0, 0);
+    final var Vr = u.get(1, 0);
+
+    final Matrix<N5, N1> result = new Matrix<>(Nat.N5(), Nat.N1());
+    final var v = 0.5 * (vl + vr);
+    result.set(0, 0, v * Math.cos(x.get(2, 0)));
+    result.set(1, 0, v * Math.sin(x.get(2, 0)));
+    result.set(2, 0, (vr - vl) / (2.0 * rb));
+    result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)));
+    result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+    return result;
+  }
+
+  public static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
+  }
+
+  public static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(0, 0), x.get(1, 0), x.get(2, 0), x.get(3, 0), x.get(4, 0));
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  public void testInit() {
+    double dtSeconds = 0.00505;
+
+    assertDoesNotThrow(
+        () -> {
+          ExtendedKalmanFilter<N5, N2, N3> observer =
+              new ExtendedKalmanFilter<>(
+                  Nat.N5(),
+                  Nat.N2(),
+                  Nat.N3(),
+                  ExtendedKalmanFilterTest::getDynamics,
+                  ExtendedKalmanFilterTest::getLocalMeasurementModel,
+                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+                  VecBuilder.fill(0.0001, 0.01, 0.01),
+                  dtSeconds);
+
+          Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
+          observer.predict(u, dtSeconds);
+
+          var localY = getLocalMeasurementModel(observer.getXhat(), u);
+          observer.correct(u, localY);
+
+          var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+          var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+          observer.correct(
+              Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
+        });
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  public void testConvergence() {
+    double dtSeconds = 0.00505;
+    double rbMeters = 0.8382 / 2.0; // Robot radius
+
+    ExtendedKalmanFilter<N5, N2, N3> observer =
+        new ExtendedKalmanFilter<>(
+            Nat.N5(),
+            Nat.N2(),
+            Nat.N3(),
+            ExtendedKalmanFilterTest::getDynamics,
+            ExtendedKalmanFilterTest::getLocalMeasurementModel,
+            VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
+            VecBuilder.fill(0.001, 0.01, 0.01),
+            dtSeconds);
+
+    List<Pose2d> waypoints =
+        Arrays.asList(
+            new Pose2d(2.75, 22.521, new Rotation2d()),
+            new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
+
+    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
+
+    Matrix<N5, N1> nextR = new Matrix<>(Nat.N5(), Nat.N1());
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    var B =
+        NumericalJacobian.numericalJacobianU(
+            Nat.N5(),
+            Nat.N2(),
+            ExtendedKalmanFilterTest::getDynamics,
+            new Matrix<>(Nat.N5(), Nat.N1()),
+            u);
+
+    observer.setXhat(
+        VecBuilder.fill(
+            trajectory.getInitialPose().getTranslation().getX(),
+            trajectory.getInitialPose().getTranslation().getY(),
+            trajectory.getInitialPose().getRotation().getRadians(),
+            0.0,
+            0.0));
+
+    var groundTruthX = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / dtSeconds); i++) {
+      var ref = trajectory.sample(dtSeconds * i);
+      double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
+      double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
+
+      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
+      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
+      nextR.set(2, 0, ref.poseMeters.getRotation().getRadians());
+      nextR.set(3, 0, vl);
+      nextR.set(4, 0, vr);
+
+      var localY = getLocalMeasurementModel(groundTruthX, u);
+      var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
+      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
+
+      Matrix<N5, N1> rdot = nextR.minus(r).div(dtSeconds);
+      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      observer.predict(u, dtSeconds);
+
+      groundTruthX =
+          NumericalIntegration.rk4(
+              ExtendedKalmanFilterTest::getDynamics, groundTruthX, u, dtSeconds);
+
+      r = nextR;
+    }
+
+    var localY = getLocalMeasurementModel(observer.getXhat(), u);
+    observer.correct(u, localY);
+
+    var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
+    var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
+    observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
+
+    var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
+    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0);
+    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0);
+    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
+    assertEquals(0.0, observer.getXhat(3), 1.0);
+    assertEquals(0.0, observer.getXhat(4), 1.0);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
new file mode 100644
index 0000000..0a18497
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/KalmanFilterTest.java
@@ -0,0 +1,191 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.system.LinearSystem;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+public class KalmanFilterTest {
+  private static LinearSystem<N2, N1, N1> elevatorPlant;
+
+  private static final double kDt = 0.00505;
+
+  static {
+    createElevator();
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  public static void createElevator() {
+    var motors = DCMotor.getVex775Pro(2);
+
+    var m = 5.0;
+    var r = 0.0181864;
+    var G = 1.0;
+    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
+  }
+
+  // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]ᵀ,
+  // Y is [x, y, theta]ᵀ and u is [ax, ay, alpha}ᵀ
+  LinearSystem<N6, N3, N3> m_swerveObserverSystem =
+      new LinearSystem<>(
+          Matrix.mat(Nat.N6(), Nat.N6())
+              .fill( // A
+                  0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                  0, 0, 0, 0, 0, 0, 0, 0, 0),
+          Matrix.mat(Nat.N6(), Nat.N3())
+              .fill( // B
+                  0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1),
+          Matrix.mat(Nat.N3(), Nat.N6())
+              .fill( // C
+                  1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0),
+          new Matrix<>(Nat.N3(), Nat.N3())); // D
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testElevatorKalmanFilter() {
+    var Q = VecBuilder.fill(0.05, 1.0);
+    var R = VecBuilder.fill(0.0001);
+
+    assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt));
+  }
+
+  @Test
+  public void testSwerveKFStationary() {
+    var random = new Random();
+
+    var filter =
+        new KalmanFilter<>(
+            Nat.N6(),
+            Nat.N3(),
+            m_swerveObserverSystem,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+            // weights
+            VecBuilder.fill(2, 2, 2), // measurement weights
+            0.020);
+
+    Matrix<N3, N1> measurement;
+    for (int i = 0; i < 100; i++) {
+      // the robot is at [0, 0, 0] so we just park here
+      measurement =
+          VecBuilder.fill(random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
+      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+
+      // we continue to not accelerate
+      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+    }
+
+    assertEquals(0.0, filter.getXhat(0), 0.3);
+    assertEquals(0.0, filter.getXhat(0), 0.3);
+  }
+
+  @Test
+  public void testSwerveKFMovingWithoutAccelerating() {
+    var random = new Random();
+
+    var filter =
+        new KalmanFilter<>(
+            Nat.N6(),
+            Nat.N3(),
+            m_swerveObserverSystem,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+            // weights
+            VecBuilder.fill(4, 4, 4), // measurement weights
+            0.020);
+
+    // we set the velocity of the robot so that it's moving forward slowly
+    filter.setXhat(0, 0.5);
+    filter.setXhat(1, 0.5);
+
+    for (int i = 0; i < 300; i++) {
+      // the robot is at [0, 0, 0] so we just park here
+      var measurement =
+          VecBuilder.fill(
+              random.nextGaussian() / 10d,
+              random.nextGaussian() / 10d,
+              random.nextGaussian() / 4d // std dev of [1, 1, 1]
+              );
+
+      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
+
+      // we continue to not accelerate
+      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
+    }
+
+    assertEquals(0.0, filter.getXhat(0), 0.2);
+    assertEquals(0.0, filter.getXhat(1), 0.2);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testSwerveKFMovingOverTrajectory() {
+    var random = new Random();
+
+    var filter =
+        new KalmanFilter<>(
+            Nat.N6(),
+            Nat.N3(),
+            m_swerveObserverSystem,
+            VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
+            // weights
+            VecBuilder.fill(4, 4, 4), // measurement weights
+            0.020);
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(new Pose2d(0, 0, new Rotation2d()), new Pose2d(5, 5, new Rotation2d())),
+            new TrajectoryConfig(2, 2));
+    var time = 0.0;
+    var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
+
+    while (time <= trajectory.getTotalTimeSeconds()) {
+      var sample = trajectory.sample(time);
+      var measurement =
+          VecBuilder.fill(
+              sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d,
+              sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d,
+              sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d);
+
+      var velocity =
+          VecBuilder.fill(
+              sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(),
+              sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(),
+              sample.curvatureRadPerMeter * sample.velocityMetersPerSecond);
+      var u = (velocity.minus(lastVelocity)).div(0.020);
+      lastVelocity = velocity;
+
+      filter.correct(u, measurement);
+      filter.predict(u, 0.020);
+
+      time += 0.020;
+    }
+
+    assertEquals(
+        trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getX(),
+        filter.getXhat(0),
+        0.2);
+    assertEquals(
+        trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters.getTranslation().getY(),
+        filter.getXhat(1),
+        0.2);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
new file mode 100644
index 0000000..38b0d20
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MecanumDrivePoseEstimatorTest.java
@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+public class MecanumDrivePoseEstimatorTest {
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testAccuracy() {
+    var kinematics =
+        new MecanumDriveKinematics(
+            new Translation2d(1, 1), new Translation2d(1, -1),
+            new Translation2d(-1, -1), new Translation2d(-1, 1));
+
+    var estimator =
+        new MecanumDrivePoseEstimator(
+            new Rotation2d(),
+            new Pose2d(),
+            kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.05),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(0)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(180)),
+                new Pose2d(30, 30, Rotation2d.fromDegrees(0)),
+                new Pose2d(20, 20, Rotation2d.fromDegrees(180)),
+                new Pose2d(10, 10, Rotation2d.fromDegrees(0))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(5190);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose =
+            new Pose2d(
+                new Translation2d(
+                    groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
+                    groundTruthState.poseMeters.getTranslation().getY()
+                        + rand.nextGaussian() * 0.1),
+                new Rotation2d(rand.nextGaussian() * 0.1)
+                    .plus(groundTruthState.poseMeters.getRotation()));
+
+        lastVisionUpdateTime = t;
+      }
+
+      var wheelSpeeds =
+          kinematics.toWheelSpeeds(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond,
+                  0,
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.curvatureRadPerMeter));
+
+      wheelSpeeds.frontLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.frontRightMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearLeftMetersPerSecond += rand.nextGaussian() * 0.1;
+      wheelSpeeds.rearRightMetersPerSecond += rand.nextGaussian() * 0.1;
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              wheelSpeeds);
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
new file mode 100644
index 0000000..b6e32fc
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/MerweScaledSigmaPointsTest.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import org.junit.jupiter.api.Test;
+
+public class MerweScaledSigmaPointsTest {
+  @Test
+  public void testZeroMeanPoints() {
+    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
+    var points =
+        merweScaledSigmaPoints.sigmaPoints(
+            VecBuilder.fill(0, 0), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
+
+    assertTrue(
+        points.isEqual(
+            Matrix.mat(Nat.N2(), Nat.N5())
+                .fill(
+                    0.0, 0.00173205, 0.0, -0.00173205, 0.0, 0.0, 0.0, 0.00173205, 0.0, -0.00173205),
+            1E-6));
+  }
+
+  @Test
+  public void testNonzeroMeanPoints() {
+    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
+    var points =
+        merweScaledSigmaPoints.sigmaPoints(
+            VecBuilder.fill(1, 2), Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
+
+    assertTrue(
+        points.isEqual(
+            Matrix.mat(Nat.N2(), Nat.N5())
+                .fill(1.0, 1.00173205, 1.0, 0.99826795, 1.0, 2.0, 2.0, 2.00547723, 2.0, 1.99452277),
+            1E-6));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
new file mode 100644
index 0000000..607e8de
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/SwerveDrivePoseEstimatorTest.java
@@ -0,0 +1,116 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.List;
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+public class SwerveDrivePoseEstimatorTest {
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testAccuracy() {
+    var kinematics =
+        new SwerveDriveKinematics(
+            new Translation2d(1, 1),
+            new Translation2d(1, -1),
+            new Translation2d(-1, -1),
+            new Translation2d(-1, 1));
+    var estimator =
+        new SwerveDrivePoseEstimator(
+            new Rotation2d(),
+            new Pose2d(),
+            kinematics,
+            VecBuilder.fill(0.1, 0.1, 0.1),
+            VecBuilder.fill(0.005),
+            VecBuilder.fill(0.1, 0.1, 0.1));
+
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            List.of(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45)),
+                new Pose2d(3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(135)),
+                new Pose2d(-3, 0, Rotation2d.fromDegrees(-90)),
+                new Pose2d(0, 0, Rotation2d.fromDegrees(45))),
+            new TrajectoryConfig(0.5, 2));
+
+    var rand = new Random(4915);
+
+    final double dt = 0.02;
+    double t = 0.0;
+
+    final double visionUpdateRate = 0.1;
+    Pose2d lastVisionPose = null;
+    double lastVisionUpdateTime = Double.NEGATIVE_INFINITY;
+
+    double maxError = Double.NEGATIVE_INFINITY;
+    double errorSum = 0;
+    while (t <= trajectory.getTotalTimeSeconds()) {
+      var groundTruthState = trajectory.sample(t);
+
+      if (lastVisionUpdateTime + visionUpdateRate < t) {
+        if (lastVisionPose != null) {
+          estimator.addVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+        }
+
+        lastVisionPose =
+            new Pose2d(
+                new Translation2d(
+                    groundTruthState.poseMeters.getTranslation().getX() + rand.nextGaussian() * 0.1,
+                    groundTruthState.poseMeters.getTranslation().getY()
+                        + rand.nextGaussian() * 0.1),
+                new Rotation2d(rand.nextGaussian() * 0.1)
+                    .plus(groundTruthState.poseMeters.getRotation()));
+
+        lastVisionUpdateTime = t;
+      }
+
+      var moduleStates =
+          kinematics.toSwerveModuleStates(
+              new ChassisSpeeds(
+                  groundTruthState.velocityMetersPerSecond,
+                  0.0,
+                  groundTruthState.velocityMetersPerSecond
+                      * groundTruthState.curvatureRadPerMeter));
+      for (var moduleState : moduleStates) {
+        moduleState.angle = moduleState.angle.plus(new Rotation2d(rand.nextGaussian() * 0.005));
+        moduleState.speedMetersPerSecond += rand.nextGaussian() * 0.1;
+      }
+
+      var xHat =
+          estimator.updateWithTime(
+              t,
+              groundTruthState
+                  .poseMeters
+                  .getRotation()
+                  .plus(new Rotation2d(rand.nextGaussian() * 0.05)),
+              moduleStates);
+
+      double error =
+          groundTruthState.poseMeters.getTranslation().getDistance(xHat.getTranslation());
+      if (error > maxError) {
+        maxError = error;
+      }
+      errorSum += error;
+
+      t += dt;
+    }
+
+    assertEquals(
+        0.0, errorSum / (trajectory.getTotalTimeSeconds() / dt), 0.25, "Incorrect mean error");
+    assertEquals(0.0, maxError, 0.42, "Incorrect max error");
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
new file mode 100644
index 0000000..1264591
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/estimator/UnscentedKalmanFilterTest.java
@@ -0,0 +1,329 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.estimator;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.StateSpaceUtil;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N2;
+import edu.wpi.first.math.numbers.N4;
+import edu.wpi.first.math.numbers.N6;
+import edu.wpi.first.math.system.Discretization;
+import edu.wpi.first.math.system.NumericalIntegration;
+import edu.wpi.first.math.system.NumericalJacobian;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
+import java.util.Arrays;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+public class UnscentedKalmanFilterTest {
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+    var motors = DCMotor.getCIM(2);
+
+    var gHigh = 7.08;
+    var rb = 0.8382 / 2.0;
+    var r = 0.0746125;
+    var m = 63.503;
+    var J = 5.6;
+
+    var C1 =
+        -Math.pow(gHigh, 2)
+            * motors.KtNMPerAmp
+            / (motors.KvRadPerSecPerVolt * motors.rOhms * r * r);
+    var C2 = gHigh * motors.KtNMPerAmp / (motors.rOhms * r);
+
+    var c = x.get(2, 0);
+    var s = x.get(3, 0);
+    var vl = x.get(4, 0);
+    var vr = x.get(5, 0);
+
+    var Vl = u.get(0, 0);
+    var Vr = u.get(1, 0);
+
+    var k1 = 1.0 / m + rb * rb / J;
+    var k2 = 1.0 / m - rb * rb / J;
+
+    var xvel = (vl + vr) / 2;
+    var w = (vr - vl) / (2.0 * rb);
+
+    return VecBuilder.fill(
+        xvel * c,
+        xvel * s,
+        -s * w,
+        c * w,
+        k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
+        k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
+  }
+
+  @SuppressWarnings("ParameterName")
+  public static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
+  }
+
+  @SuppressWarnings("ParameterName")
+  public static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
+    return x.copy();
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  public void testInit() {
+    assertDoesNotThrow(
+        () -> {
+          UnscentedKalmanFilter<N6, N2, N4> observer =
+              new UnscentedKalmanFilter<>(
+                  Nat.N6(),
+                  Nat.N4(),
+                  UnscentedKalmanFilterTest::getDynamics,
+                  UnscentedKalmanFilterTest::getLocalMeasurementModel,
+                  VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
+                  VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+                  0.00505);
+
+          var u = VecBuilder.fill(12.0, 12.0);
+          observer.predict(u, 0.00505);
+
+          var localY = getLocalMeasurementModel(observer.getXhat(), u);
+          observer.correct(u, localY);
+        });
+  }
+
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  public void testConvergence() {
+    double dtSeconds = 0.00505;
+    double rbMeters = 0.8382 / 2.0; // Robot radius
+
+    UnscentedKalmanFilter<N6, N2, N4> observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N6(),
+            Nat.N4(),
+            UnscentedKalmanFilterTest::getDynamics,
+            UnscentedKalmanFilterTest::getLocalMeasurementModel,
+            VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
+            VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
+            dtSeconds);
+
+    List<Pose2d> waypoints =
+        Arrays.asList(
+            new Pose2d(2.75, 22.521, new Rotation2d()),
+            new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(waypoints, new TrajectoryConfig(8.8, 0.1));
+
+    Matrix<N6, N1> nextR;
+    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
+
+    var B =
+        NumericalJacobian.numericalJacobianU(
+            Nat.N6(),
+            Nat.N2(),
+            UnscentedKalmanFilterTest::getDynamics,
+            new Matrix<>(Nat.N6(), Nat.N1()),
+            u);
+
+    observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
+
+    var ref = trajectory.sample(0.0);
+
+    Matrix<N6, N1> r =
+        VecBuilder.fill(
+            ref.poseMeters.getTranslation().getX(),
+            ref.poseMeters.getTranslation().getY(),
+            ref.poseMeters.getRotation().getCos(),
+            ref.poseMeters.getRotation().getSin(),
+            ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
+            ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters)));
+    nextR = r.copy();
+
+    var trueXhat = observer.getXhat();
+
+    double totalTime = trajectory.getTotalTimeSeconds();
+    for (int i = 0; i < (totalTime / dtSeconds); i++) {
+      ref = trajectory.sample(dtSeconds * i);
+      double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
+      double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
+
+      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
+      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
+      nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
+      nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
+      nextR.set(4, 0, vl);
+      nextR.set(5, 0, vr);
+
+      Matrix<N4, N1> localY = getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
+      var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
+
+      observer.correct(u, localY.plus(StateSpaceUtil.makeWhiteNoiseVector(noiseStdDev)));
+
+      var rdot = nextR.minus(r).div(dtSeconds);
+      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
+
+      r = nextR;
+      observer.predict(u, dtSeconds);
+      trueXhat =
+          NumericalIntegration.rk4(UnscentedKalmanFilterTest::getDynamics, trueXhat, u, dtSeconds);
+    }
+
+    var localY = getLocalMeasurementModel(trueXhat, u);
+    observer.correct(u, localY);
+
+    var globalY = getGlobalMeasurementModel(trueXhat, u);
+    var R = StateSpaceUtil.makeCostMatrix(VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
+    observer.correct(
+        Nat.N6(),
+        u,
+        globalY,
+        UnscentedKalmanFilterTest::getGlobalMeasurementModel,
+        R,
+        (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
+        Matrix::minus,
+        Matrix::minus,
+        Matrix::plus);
+
+    final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
+
+    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
+    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
+    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
+    assertEquals(0.0, observer.getXhat(3), 1.0);
+    assertEquals(0.0, observer.getXhat(4), 1.0);
+  }
+
+  @Test
+  @SuppressWarnings({"LocalVariableName", "ParameterName"})
+  public void testLinearUKF() {
+    var dt = 0.020;
+    var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
+    var observer =
+        new UnscentedKalmanFilter<>(
+            Nat.N1(),
+            Nat.N1(),
+            (x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
+            plant::calculateY,
+            VecBuilder.fill(0.05),
+            VecBuilder.fill(1.0),
+            dt);
+
+    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    Matrix<N1, N1> ref = VecBuilder.fill(100);
+    Matrix<N1, N1> u = VecBuilder.fill(0);
+
+    for (int i = 0; i < (2.0 / dt); i++) {
+      observer.predict(u, dt);
+
+      u = discB.solve(ref.minus(discA.times(ref)));
+    }
+
+    assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
+  }
+
+  @Test
+  public void testUnscentedTransform() {
+    // From FilterPy
+    var ret =
+        UnscentedKalmanFilter.unscentedTransform(
+            Nat.N4(),
+            Nat.N4(),
+            Matrix.mat(Nat.N4(), Nat.N9())
+                .fill(
+                    -0.9,
+                    -0.822540333075852,
+                    -0.8922540333075852,
+                    -0.9,
+                    -0.9,
+                    -0.9774596669241481,
+                    -0.9077459666924148,
+                    -0.9,
+                    -0.9,
+                    1.0,
+                    1.0,
+                    1.077459666924148,
+                    1.0,
+                    1.0,
+                    1.0,
+                    0.9225403330758519,
+                    1.0,
+                    1.0,
+                    -0.9,
+                    -0.9,
+                    -0.9,
+                    -0.822540333075852,
+                    -0.8922540333075852,
+                    -0.9,
+                    -0.9,
+                    -0.9774596669241481,
+                    -0.9077459666924148,
+                    1.0,
+                    1.0,
+                    1.0,
+                    1.0,
+                    1.077459666924148,
+                    1.0,
+                    1.0,
+                    1.0,
+                    0.9225403330758519),
+            VecBuilder.fill(
+                -132.33333333,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667),
+            VecBuilder.fill(
+                -129.34333333,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667,
+                16.66666667),
+            (sigmas, weights) -> sigmas.times(Matrix.changeBoundsUnchecked(weights)),
+            Matrix::minus);
+
+    assertTrue(VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(ret.getFirst(), 1E-5));
+
+    assertTrue(
+        Matrix.mat(Nat.N4(), Nat.N4())
+            .fill(
+                2.02000002e-01,
+                2.00000500e-02,
+                -2.69044710e-29,
+                -4.59511477e-29,
+                2.00000500e-02,
+                2.00001000e-01,
+                -2.98781068e-29,
+                -5.12759588e-29,
+                -2.73372625e-29,
+                -3.09882635e-29,
+                2.02000002e-01,
+                2.00000500e-02,
+                -4.67065917e-29,
+                -5.10705197e-29,
+                2.00000500e-02,
+                2.00001000e-01)
+            .isEqual(ret.getSecond(), 1E-5));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
new file mode 100644
index 0000000..805129f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/LinearFilterTest.java
@@ -0,0 +1,220 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.params.provider.Arguments.arguments;
+
+import java.util.Random;
+import java.util.function.DoubleFunction;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.params.ParameterizedTest;
+import org.junit.jupiter.params.provider.Arguments;
+import org.junit.jupiter.params.provider.MethodSource;
+
+class LinearFilterTest {
+  private static final double kFilterStep = 0.005;
+  private static final double kFilterTime = 2.0;
+  private static final double kSinglePoleIIRTimeConstant = 0.015915;
+  private static final double kHighPassTimeConstant = 0.006631;
+  private static final int kMovAvgTaps = 6;
+
+  private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
+  private static final double kHighPassExpectedOutput = 10.074717;
+  private static final double kMovAvgExpectedOutput = -10.191644;
+
+  private static double getData(double t) {
+    return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
+  }
+
+  private static double getPulseData(double t) {
+    if (Math.abs(t - 1.0) < 0.001) {
+      return 1.0;
+    } else {
+      return 0.0;
+    }
+  }
+
+  @Test
+  void illegalTapNumberTest() {
+    assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
+  }
+
+  /** Test if the filter reduces the noise produced by a signal generator. */
+  @ParameterizedTest
+  @MethodSource("noiseFilterProvider")
+  void noiseReduceTest(final LinearFilter filter) {
+    double noiseGenError = 0.0;
+    double filterError = 0.0;
+
+    final Random gen = new Random();
+    final double kStdDev = 10.0;
+
+    for (double t = 0; t < kFilterTime; t += kFilterStep) {
+      final double theory = getData(t);
+      final double noise = gen.nextGaussian() * kStdDev;
+      filterError += Math.abs(filter.calculate(theory + noise) - theory);
+      noiseGenError += Math.abs(noise - theory);
+    }
+
+    assertTrue(
+        noiseGenError > filterError,
+        "Filter should have reduced noise accumulation from "
+            + noiseGenError
+            + " but failed. The filter error was "
+            + filterError);
+  }
+
+  static Stream<LinearFilter> noiseFilterProvider() {
+    return Stream.of(
+        LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+        LinearFilter.movingAverage(kMovAvgTaps));
+  }
+
+  /** Test if the linear filters produce consistent output for a given data set. */
+  @ParameterizedTest
+  @MethodSource("outputFilterProvider")
+  void outputTest(
+      final LinearFilter filter, final DoubleFunction<Double> data, final double expectedOutput) {
+    double filterOutput = 0.0;
+    for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
+      filterOutput = filter.calculate(data.apply(t));
+    }
+
+    assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
+  }
+
+  static Stream<Arguments> outputFilterProvider() {
+    return Stream.of(
+        arguments(
+            LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kSinglePoleIIRExpectedOutput),
+        arguments(
+            LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kHighPassExpectedOutput),
+        arguments(
+            LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getData,
+            kMovAvgExpectedOutput),
+        arguments(
+            LinearFilter.movingAverage(kMovAvgTaps),
+            (DoubleFunction<Double>) LinearFilterTest::getPulseData,
+            0.0));
+  }
+
+  /** Test backward finite difference. */
+  @Test
+  void backwardFiniteDifferenceTest() {
+    double h = 0.005;
+
+    assertResults(
+        1,
+        2,
+        // f(x) = x²
+        (double x) -> x * x,
+        // df/dx = 2x
+        (double x) -> 2.0 * x,
+        h,
+        -20.0,
+        20.0);
+
+    assertResults(
+        1,
+        2,
+        // f(x) = sin(x)
+        (double x) -> Math.sin(x),
+        // df/dx = cos(x)
+        (double x) -> Math.cos(x),
+        h,
+        -20.0,
+        20.0);
+
+    assertResults(
+        1,
+        2,
+        // f(x) = ln(x)
+        (double x) -> Math.log(x),
+        // df/dx = 1 / x
+        (double x) -> 1.0 / x,
+        h,
+        1.0,
+        20.0);
+
+    assertResults(
+        2,
+        4,
+        // f(x) = x²
+        (double x) -> x * x,
+        // d²f/dx² = 2
+        (double x) -> 2.0,
+        h,
+        -20.0,
+        20.0);
+
+    assertResults(
+        2,
+        4,
+        // f(x) = sin(x)
+        (double x) -> Math.sin(x),
+        // d²f/dx² = -sin(x)
+        (double x) -> -Math.sin(x),
+        h,
+        -20.0,
+        20.0);
+
+    assertResults(
+        2,
+        4,
+        // f(x) = ln(x)
+        (double x) -> Math.log(x),
+        // d²f/dx² = -1 / x²
+        (double x) -> -1.0 / (x * x),
+        h,
+        1.0,
+        20.0);
+  }
+
+  /**
+   * Helper for checking results of backward finite difference.
+   *
+   * @param derivative The order of the derivative.
+   * @param samples The number of sample points.
+   * @param f Function of which to take derivative.
+   * @param dfdx Derivative of f.
+   * @param h Sample period in seconds.
+   * @param min Minimum of f's domain to test.
+   * @param max Maximum of f's domain to test.
+   */
+  void assertResults(
+      int derivative,
+      int samples,
+      DoubleFunction<Double> f,
+      DoubleFunction<Double> dfdx,
+      double h,
+      double min,
+      double max) {
+    var filter = LinearFilter.backwardFiniteDifference(derivative, samples, h);
+
+    for (int i = (int) (min / h); i < (int) (max / h); ++i) {
+      // Let filter initialize
+      if (i < (int) (min / h) + samples) {
+        filter.calculate(f.apply(i * h));
+        continue;
+      }
+
+      // The order of accuracy is O(h^(N - d)) where N is number of stencil
+      // points and d is order of derivative
+      assertEquals(
+          dfdx.apply(i * h),
+          filter.calculate(f.apply(i * h)),
+          10.0 * Math.pow(h, samples - derivative));
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java
new file mode 100644
index 0000000..06b3d01
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/MedianFilterTest.java
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+public class MedianFilterTest {
+  @Test
+  void medianFilterNotFullTestEven() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+
+    assertEquals(3.5, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterNotFullTestOdd() {
+    MedianFilter filter = new MedianFilter(10);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(4);
+    filter.calculate(7);
+
+    assertEquals(4, filter.calculate(1000));
+  }
+
+  @Test
+  void medianFilterFullTestEven() {
+    MedianFilter filter = new MedianFilter(6);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(4.5, filter.calculate(99));
+  }
+
+  @Test
+  void medianFilterFullTestOdd() {
+    MedianFilter filter = new MedianFilter(5);
+
+    filter.calculate(3);
+    filter.calculate(0);
+    filter.calculate(5);
+    filter.calculate(4);
+    filter.calculate(1000);
+
+    assertEquals(5, filter.calculate(99));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
new file mode 100644
index 0000000..9c1f2f3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/filter/SlewRateLimiterTest.java
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.util.WPIUtilJNI;
+import org.junit.jupiter.api.Test;
+
+public class SlewRateLimiterTest {
+  @Test
+  void slewRateLimitTest() {
+    WPIUtilJNI.enableMockTime();
+
+    var limiter = new SlewRateLimiter(1);
+    WPIUtilJNI.setMockTime(1000000L);
+    assertTrue(limiter.calculate(2) < 2);
+
+    WPIUtilJNI.setMockTime(0L);
+  }
+
+  @Test
+  void slewRateNoLimitTest() {
+    WPIUtilJNI.enableMockTime();
+
+    var limiter = new SlewRateLimiter(1);
+    WPIUtilJNI.setMockTime(1000000L);
+    assertEquals(limiter.calculate(0.5), 0.5);
+
+    WPIUtilJNI.setMockTime(0L);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
new file mode 100644
index 0000000..b6e66af
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -0,0 +1,67 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import org.junit.jupiter.api.Test;
+
+class Pose2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testTransformBy() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+
+    assertAll(
+        () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon));
+  }
+
+  @Test
+  void testRelativeTo() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    var finalRelativeToInitial = last.relativeTo(initial);
+
+    assertAll(
+        () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
+        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
+    var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
+    assertNotEquals(one, two);
+  }
+
+  void testMinus() {
+    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
+    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
+
+    final var transform = last.minus(initial);
+
+    assertAll(
+        () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
+        () -> assertEquals(transform.getY(), 0.0, kEpsilon),
+        () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
new file mode 100644
index 0000000..cb3f0f3
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import org.junit.jupiter.api.Test;
+
+class Rotation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testRadiansToDegrees() {
+    var rot1 = new Rotation2d(Math.PI / 3);
+    var rot2 = new Rotation2d(Math.PI / 4);
+
+    assertAll(
+        () -> assertEquals(rot1.getDegrees(), 60.0, kEpsilon),
+        () -> assertEquals(rot2.getDegrees(), 45.0, kEpsilon));
+  }
+
+  @Test
+  void testRadiansAndDegrees() {
+    var rot1 = Rotation2d.fromDegrees(45.0);
+    var rot2 = Rotation2d.fromDegrees(30.0);
+
+    assertAll(
+        () -> assertEquals(rot1.getRadians(), Math.PI / 4, kEpsilon),
+        () -> assertEquals(rot2.getRadians(), Math.PI / 6, kEpsilon));
+  }
+
+  @Test
+  void testRotateByFromZero() {
+    var zero = new Rotation2d();
+    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
+        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon));
+  }
+
+  @Test
+  void testRotateByNonZero() {
+    var rot = Rotation2d.fromDegrees(90.0);
+    rot = rot.plus(Rotation2d.fromDegrees(30.0));
+
+    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
+  }
+
+  @Test
+  void testMinus() {
+    var rot1 = Rotation2d.fromDegrees(70.0);
+    var rot2 = Rotation2d.fromDegrees(30.0);
+
+    assertEquals(rot1.minus(rot2).getDegrees(), 40.0, kEpsilon);
+  }
+
+  @Test
+  void testEquality() {
+    var rot1 = Rotation2d.fromDegrees(43.0);
+    var rot2 = Rotation2d.fromDegrees(43.0);
+    assertEquals(rot1, rot2);
+
+    var rot3 = Rotation2d.fromDegrees(-180.0);
+    var rot4 = Rotation2d.fromDegrees(180.0);
+    assertEquals(rot3, rot4);
+  }
+
+  @Test
+  void testInequality() {
+    var rot1 = Rotation2d.fromDegrees(43.0);
+    var rot2 = Rotation2d.fromDegrees(43.5);
+    assertNotEquals(rot1, rot2);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
new file mode 100644
index 0000000..7265e25
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class Transform2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testInverse() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transform = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transform);
+    var untransformed = transformed.plus(transform.inverse());
+
+    assertAll(
+        () -> assertEquals(initial.getX(), untransformed.getX(), kEpsilon),
+        () -> assertEquals(initial.getY(), untransformed.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                initial.getRotation().getDegrees(),
+                untransformed.getRotation().getDegrees(),
+                kEpsilon));
+  }
+
+  @Test
+  void testComposition() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transform1 = new Transform2d(new Translation2d(5.0, 0.0), Rotation2d.fromDegrees(5.0));
+    var transform2 = new Transform2d(new Translation2d(0.0, 2.0), Rotation2d.fromDegrees(5.0));
+
+    var transformedSeparate = initial.plus(transform1).plus(transform2);
+    var transformedCombined = initial.plus(transform1.plus(transform2));
+
+    assertAll(
+        () -> assertEquals(transformedSeparate.getX(), transformedCombined.getX(), kEpsilon),
+        () -> assertEquals(transformedSeparate.getY(), transformedCombined.getY(), kEpsilon),
+        () ->
+            assertEquals(
+                transformedSeparate.getRotation().getDegrees(),
+                transformedCombined.getRotation().getDegrees(),
+                kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
new file mode 100644
index 0000000..2d8eeaa
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Translation2dTest.java
@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import org.junit.jupiter.api.Test;
+
+class Translation2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testSum() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var sum = one.plus(two);
+
+    assertAll(
+        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
+        () -> assertEquals(sum.getY(), 8.0, kEpsilon));
+  }
+
+  @Test
+  void testDifference() {
+    var one = new Translation2d(1.0, 3.0);
+    var two = new Translation2d(2.0, 5.0);
+
+    var difference = one.minus(two);
+
+    assertAll(
+        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
+        () -> assertEquals(difference.getY(), -2.0, kEpsilon));
+  }
+
+  @Test
+  void testRotateBy() {
+    var another = new Translation2d(3.0, 0.0);
+    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
+
+    assertAll(
+        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
+        () -> assertEquals(rotated.getY(), 3.0, kEpsilon));
+  }
+
+  @Test
+  void testMultiplication() {
+    var original = new Translation2d(3.0, 5.0);
+    var mult = original.times(3);
+
+    assertAll(
+        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
+        () -> assertEquals(mult.getY(), 15.0, kEpsilon));
+  }
+
+  @Test
+  void testDivision() {
+    var original = new Translation2d(3.0, 5.0);
+    var div = original.div(2);
+
+    assertAll(
+        () -> assertEquals(div.getX(), 1.5, kEpsilon),
+        () -> assertEquals(div.getY(), 2.5, kEpsilon));
+  }
+
+  @Test
+  void testNorm() {
+    var one = new Translation2d(3.0, 5.0);
+    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
+  }
+
+  @Test
+  void testDistance() {
+    var one = new Translation2d(1, 1);
+    var two = new Translation2d(6, 6);
+    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
+  }
+
+  @Test
+  void testUnaryMinus() {
+    var original = new Translation2d(-4.5, 7);
+    var inverted = original.unaryMinus();
+
+    assertAll(
+        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
+        () -> assertEquals(inverted.getY(), -7, kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.5);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Translation2d(9, 5.5);
+    var two = new Translation2d(9, 5.7);
+    assertNotEquals(one, two);
+  }
+
+  @Test
+  void testPolarConstructor() {
+    var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
+    var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
+    assertAll(
+        () -> assertEquals(one.getX(), 1.0, kEpsilon),
+        () -> assertEquals(one.getY(), 1.0, kEpsilon),
+        () -> assertEquals(two.getX(), 1.0, kEpsilon),
+        () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
new file mode 100644
index 0000000..c13bb09
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/geometry/Twist2dTest.java
@@ -0,0 +1,74 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+
+import org.junit.jupiter.api.Test;
+
+class Twist2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testStraightLineTwist() {
+    var straight = new Twist2d(5.0, 0.0, 0.0);
+    var straightPose = new Pose2d().exp(straight);
+
+    assertAll(
+        () -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
+        () -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
+        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon));
+  }
+
+  @Test
+  void testQuarterCirleTwist() {
+    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
+    var quarterCirclePose = new Pose2d().exp(quarterCircle);
+
+    assertAll(
+        () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
+        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon));
+  }
+
+  @Test
+  void testDiagonalNoDtheta() {
+    var diagonal = new Twist2d(2.0, 2.0, 0.0);
+    var diagonalPose = new Pose2d().exp(diagonal);
+
+    assertAll(
+        () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
+        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon));
+  }
+
+  @Test
+  void testEquality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1, 3);
+    assertEquals(one, two);
+  }
+
+  @Test
+  void testInequality() {
+    var one = new Twist2d(5, 1, 3);
+    var two = new Twist2d(5, 1.2, 3);
+    assertNotEquals(one, two);
+  }
+
+  void testPose2dLog() {
+    final var start = new Pose2d();
+    final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
+
+    final var twist = start.log(end);
+
+    assertAll(
+        () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
+        () -> assertEquals(twist.dy, 0.0, kEpsilon),
+        () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
new file mode 100644
index 0000000..b9c3785
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/ChassisSpeedsTest.java
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+class ChassisSpeedsTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testFieldRelativeConstruction() {
+    final var chassisSpeeds =
+        ChassisSpeeds.fromFieldRelativeSpeeds(1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0));
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java
new file mode 100644
index 0000000..adee41f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematicsTest.java
@@ -0,0 +1,79 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveKinematics m_kinematics =
+      new DifferentialDriveKinematics(0.381 * 2);
+
+  @Test
+  void testInverseKinematicsForZeros() {
+    var chassisSpeeds = new ChassisSpeeds();
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testForwardKinematicsForZeros() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds();
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testInverseKinematicsForStraightLine() {
+    var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testForwardKinematicsForStraightLine() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testInverseKinematicsForRotateInPlace() {
+    var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
+    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
+
+    assertAll(
+        () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
+        () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testForwardKinematicsForRotateInPlace() {
+    var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
new file mode 100644
index 0000000..f85e8fb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/DifferentialDriveOdometryTest.java
@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveOdometryTest {
+  private static final double kEpsilon = 1E-9;
+  private final DifferentialDriveOdometry m_odometry =
+      new DifferentialDriveOdometry(new Rotation2d());
+
+  @Test
+  void testOdometryWithEncoderDistances() {
+    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
+    var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
+
+    assertAll(
+        () -> assertEquals(pose.getX(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getY(), 5.0, kEpsilon),
+        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
new file mode 100644
index 0000000..d334679
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveKinematicsTest.java
@@ -0,0 +1,175 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  @Test
+  void testStraightLineInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(5.0, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(5.0, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(5.0, moduleStates.rearRightMetersPerSecond, 0.1));
+  }
+
+  @Test
+  void testStraightLineForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(3.536, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testStrafeInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    assertAll(
+        () -> assertEquals(-4.0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(4.0, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(4.0, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(-4.0, moduleStates.rearRightMetersPerSecond, 0.1));
+  }
+
+  @Test
+  void testStrafeForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(2.8284, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    assertAll(
+        () -> assertEquals(-150.79645, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(150.79645, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-150.79645, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(150.79645, moduleStates.rearRightMetersPerSecond, 0.1));
+  }
+
+  @Test
+  void testRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-150.79645, 150.79645, -150.79645, 150.79645);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testMixedTranslationRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds);
+
+    assertAll(
+        () -> assertEquals(-25.0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(29.0, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-19.0, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(23.0, moduleStates.rearRightMetersPerSecond, 0.1));
+  }
+
+  @Test
+  void testMixedTranslationRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(1.413, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(2.122, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testOffCenterRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+    assertAll(
+        () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(24.0, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-24.0, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(48.0, moduleStates.rearRightMetersPerSecond, 0.1));
+  }
+
+  @Test
+  void testOffCenterRotationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(8.48525, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-8.48525, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testOffCenterTranslationRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
+    var moduleStates = m_kinematics.toWheelSpeeds(speeds, m_fl);
+
+    assertAll(
+        () -> assertEquals(3.0, moduleStates.frontLeftMetersPerSecond, 0.1),
+        () -> assertEquals(31.0, moduleStates.frontRightMetersPerSecond, 0.1),
+        () -> assertEquals(-17.0, moduleStates.rearLeftMetersPerSecond, 0.1),
+        () -> assertEquals(51.0, moduleStates.rearRightMetersPerSecond, 0.1));
+  }
+
+  @Test
+  void testOffCenterRotationTranslationForwardKinematicsKinematics() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
+    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(12.02, moduleStates.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-7.07, moduleStates.vyMetersPerSecond, 0.1),
+        () -> assertEquals(0.707, moduleStates.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testNormalize() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
+    wheelSpeeds.normalize(5.5);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
new file mode 100644
index 0000000..3f43109
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/MecanumDriveOdometryTest.java
@@ -0,0 +1,83 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import org.junit.jupiter.api.Test;
+
+class MecanumDriveOdometryTest {
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final MecanumDriveKinematics m_kinematics =
+      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  private final MecanumDriveOdometry m_odometry =
+      new MecanumDriveOdometry(m_kinematics, new Rotation2d());
+
+  @Test
+  void testMultipleConsecutiveUpdates() {
+    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(secondPose.getX(), 0.0, 0.01),
+        () -> assertEquals(secondPose.getY(), 0.0, 0.01),
+        () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01));
+  }
+
+  @Test
+  void testTwoIterations() {
+    // 5 units/sec  in the x axis (forward)
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(0.3536, pose.getX(), 0.01),
+        () -> assertEquals(0.0, pose.getY(), 0.01),
+        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01));
+  }
+
+  @Test
+  void test90degreeTurn() {
+    // This is a 90 degree turn about the point between front left and rear left wheels
+    // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
+    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
+    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(8.4855, pose.getX(), 0.01),
+        () -> assertEquals(8.4855, pose.getY(), 0.01),
+        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01));
+  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
+    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
+    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
+
+    assertAll(
+        () -> assertEquals(3.536, pose.getX(), 0.1),
+        () -> assertEquals(0.0, pose.getY(), 0.1),
+        () -> assertEquals(0.0, pose.getRotation().getRadians(), 0.1));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
new file mode 100644
index 0000000..eec6aa2
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveKinematicsTest.java
@@ -0,0 +1,249 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import org.junit.jupiter.api.Test;
+
+class SwerveDriveKinematicsTest {
+  private static final double kEpsilon = 1E-9;
+
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  @Test
+  void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
+
+    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon));
+  }
+
+  @Test
+  void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testStraightStrafeInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    assertAll(
+        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testStraightStrafeForwardKinematics() {
+    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon));
+  }
+
+  @Test
+  void testTurnInPlaceInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
+
+    /*
+    The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
+    the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
+    trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
+    our wheels must trace out 1 rotation (or 106.63 inches) per second.
+      */
+
+    assertAll(
+        () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
+        () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
+        () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testTurnInPlaceForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
+    SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
+    SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
+    SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
+        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testOffCenterCORRotationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
+
+    /*
+    This one is a bit trickier. Because we are rotating about the front-left wheel,
+    it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
+    an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
+    radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
+    should be pointing straight forward, the back-left wheel should be pointing straight right,
+    and the back-right wheel should be at a -45 degree angle
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
+        () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
+        () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
+        () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
+        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
+        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testOffCenterCORRotationForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
+    SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
+    SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
+    SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    /*
+    We already know that our omega should be 2pi from the previous test. Next, we need to determine
+    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
+    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
+    a full revolution about the center of revolution once every second. Therefore, the center of
+    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
+    1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
+    */
+
+    assertAll(
+        () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
+        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1));
+  }
+
+  private void assertModuleState(
+      SwerveModuleState expected, SwerveModuleState actual, SwerveModuleState tolerance) {
+    assertAll(
+        () ->
+            assertEquals(
+                expected.speedMetersPerSecond,
+                actual.speedMetersPerSecond,
+                tolerance.speedMetersPerSecond),
+        () ->
+            assertEquals(
+                expected.angle.getDegrees(),
+                actual.angle.getDegrees(),
+                tolerance.angle.getDegrees()));
+  }
+
+  /**
+   * Test the rotation of the robot about a non-central point with both linear and angular
+   * velocities.
+   */
+  @Test
+  void testOffCenterCORRotationAndTranslationInverseKinematics() {
+    ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
+    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
+
+    // By equation (13.14) from state-space guide, our wheels/angles will be as follows,
+    // (+-1 degree or speed):
+    SwerveModuleState[] expectedStates =
+        new SwerveModuleState[] {
+          new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
+          new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
+          new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
+          new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
+        };
+    var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
+
+    for (int i = 0; i < expectedStates.length; i++) {
+      assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
+    }
+  }
+
+  @Test
+  void testOffCenterCORRotationAndTranslationForwardKinematics() {
+    SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
+    SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
+    SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
+    SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
+
+    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
+
+    /*
+    From equation (13.17), we know that chassis motion is th dot product of the
+    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
+    (0,0) -- we don't want the motion of the center of rotation, we want it of
+    the center of the robot). These above SwerveModuleStates are known to be from
+    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
+    calculated using Numpy's linalg.pinv function.
+    */
+
+    assertAll(
+        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
+        () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
+        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1));
+  }
+
+  @Test
+  void testNormalize() {
+    SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
+    SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
+    SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
+    SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
+
+    SwerveModuleState[] arr = {fl, fr, bl, br};
+    SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
+
+    double factor = 5.5 / 7.0;
+
+    assertAll(
+        () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
new file mode 100644
index 0000000..cb6dfdf
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveDriveOdometryTest.java
@@ -0,0 +1,92 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import org.junit.jupiter.api.Test;
+
+class SwerveDriveOdometryTest {
+  private final Translation2d m_fl = new Translation2d(12, 12);
+  private final Translation2d m_fr = new Translation2d(12, -12);
+  private final Translation2d m_bl = new Translation2d(-12, 12);
+  private final Translation2d m_br = new Translation2d(-12, -12);
+
+  private final SwerveDriveKinematics m_kinematics =
+      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
+
+  private final SwerveDriveOdometry m_odometry =
+      new SwerveDriveOdometry(m_kinematics, new Rotation2d());
+
+  @Test
+  void testTwoIterations() {
+    // 5 units/sec  in the x axis (forward)
+    final SwerveModuleState[] wheelSpeeds = {
+      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+      new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
+      new SwerveModuleState(5, Rotation2d.fromDegrees(0))
+    };
+
+    m_odometry.updateWithTime(
+        0.0,
+        new Rotation2d(),
+        new SwerveModuleState(),
+        new SwerveModuleState(),
+        new SwerveModuleState(),
+        new SwerveModuleState());
+    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
+        () -> assertEquals(0, pose.getY(), 0.01),
+        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01));
+  }
+
+  @Test
+  void test90degreeTurn() {
+    // This is a 90 degree turn about the point between front left and rear left wheels
+    //        Module 0: speed 18.84955592153876 angle 90.0
+    //        Module 1: speed 42.14888838624436 angle 26.565051177077986
+    //        Module 2: speed 18.84955592153876 angle -90.0
+    //        Module 3: speed 42.14888838624436 angle -26.565051177077986
+
+    final SwerveModuleState[] wheelSpeeds = {
+      new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
+      new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
+      new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
+      new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
+    };
+    final var zero = new SwerveModuleState();
+
+    m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
+    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
+
+    assertAll(
+        () -> assertEquals(12.0, pose.getX(), 0.01),
+        () -> assertEquals(12.0, pose.getY(), 0.01),
+        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01));
+  }
+
+  @Test
+  void testGyroAngleReset() {
+    var gyro = Rotation2d.fromDegrees(90.0);
+    var fieldAngle = Rotation2d.fromDegrees(0.0);
+    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
+    var state = new SwerveModuleState();
+    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
+    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
+    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
+
+    assertAll(
+        () -> assertEquals(1.0, pose.getX(), 0.1),
+        () -> assertEquals(0.00, pose.getY(), 0.1),
+        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
new file mode 100644
index 0000000..01815be
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/kinematics/SwerveModuleStateTest.java
@@ -0,0 +1,53 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.kinematics;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import org.junit.jupiter.api.Test;
+
+class SwerveModuleStateTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testOptimize() {
+    var angleA = Rotation2d.fromDegrees(45);
+    var refA = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(180));
+    var optimizedA = SwerveModuleState.optimize(refA, angleA);
+
+    assertAll(
+        () -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(0.0, optimizedA.angle.getDegrees(), kEpsilon));
+
+    var angleB = Rotation2d.fromDegrees(-50);
+    var refB = new SwerveModuleState(4.7, Rotation2d.fromDegrees(41));
+    var optimizedB = SwerveModuleState.optimize(refB, angleB);
+
+    assertAll(
+        () -> assertEquals(-4.7, optimizedB.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-139.0, optimizedB.angle.getDegrees(), kEpsilon));
+  }
+
+  @Test
+  void testNoOptimize() {
+    var angleA = Rotation2d.fromDegrees(0);
+    var refA = new SwerveModuleState(2.0, Rotation2d.fromDegrees(89));
+    var optimizedA = SwerveModuleState.optimize(refA, angleA);
+
+    assertAll(
+        () -> assertEquals(2.0, optimizedA.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(89.0, optimizedA.angle.getDegrees(), kEpsilon));
+
+    var angleB = Rotation2d.fromDegrees(0);
+    var refB = new SwerveModuleState(-2.0, Rotation2d.fromDegrees(-2));
+    var optimizedB = SwerveModuleState.optimize(refB, angleB);
+
+    assertAll(
+        () -> assertEquals(-2.0, optimizedB.speedMetersPerSecond, kEpsilon),
+        () -> assertEquals(-2.0, optimizedB.angle.getDegrees(), kEpsilon));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
new file mode 100644
index 0000000..dd08e45
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/CubicHermiteSplineTest.java
@@ -0,0 +1,162 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class CubicHermiteSplineTest {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  @SuppressWarnings("ParameterName")
+  private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
+    // Start the timer.
+    // var start = System.nanoTime();
+
+    // Generate and parameterize the spline.
+    var controlVectors =
+        SplineHelper.getCubicControlVectorsFromWaypoints(
+            a, waypoints.toArray(new Translation2d[0]), b);
+    var splines =
+        SplineHelper.getCubicSplinesFromControlVectors(
+            controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
+
+    var poses = new ArrayList<PoseWithCurvature>();
+
+    poses.add(splines[0].getPoint(0.0));
+
+    for (var spline : splines) {
+      poses.addAll(SplineParameterizer.parameterize(spline));
+    }
+
+    // End the timer.
+    // var end = System.nanoTime();
+
+    // Calculate the duration (used when benchmarking)
+    // var durationMicroseconds = (end - start) / 1000.0;
+
+    for (int i = 0; i < poses.size() - 1; i++) {
+      var p0 = poses.get(i);
+      var p1 = poses.get(i + 1);
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      var twist = p0.poseMeters.log(p1.poseMeters);
+      assertAll(
+          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
+    }
+
+    // Check first point
+    assertAll(
+        () -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
+        () -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
+        () ->
+            assertEquals(
+                a.getRotation().getRadians(),
+                poses.get(0).poseMeters.getRotation().getRadians(),
+                1E-9));
+
+    // Check interior waypoints
+    boolean interiorsGood = true;
+    for (var waypoint : waypoints) {
+      boolean found = false;
+      for (var state : poses) {
+        if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
+          found = true;
+        }
+      }
+      interiorsGood &= found;
+    }
+
+    assertTrue(interiorsGood);
+
+    // Check last point
+    assertAll(
+        () -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
+        () -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
+        () ->
+            assertEquals(
+                b.getRotation().getRadians(),
+                poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(),
+                1E-9));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testStraightLine() {
+    run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSCurve() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(1, 1));
+    waypoints.add(new Translation2d(2, -1));
+    var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
+
+    run(start, waypoints, end);
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testOneInterior() {
+    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(2.0, 0.0));
+    var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testWindyPath() {
+    final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
+    final ArrayList<Translation2d> waypoints = new ArrayList<>();
+    waypoints.add(new Translation2d(0.5, 0.5));
+    waypoints.add(new Translation2d(0.5, 0.5));
+    waypoints.add(new Translation2d(1.0, 0.0));
+    waypoints.add(new Translation2d(1.5, 0.5));
+    waypoints.add(new Translation2d(2.0, 0.0));
+    waypoints.add(new Translation2d(2.5, 0.5));
+    final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
+
+    run(start, waypoints, end);
+  }
+
+  @Test
+  void testMalformed() {
+    assertThrows(
+        MalformedSplineException.class,
+        () ->
+            run(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+                new ArrayList<>(),
+                new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(
+        MalformedSplineException.class,
+        () ->
+            run(
+                new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+                Arrays.asList(new Translation2d(10, 10.5)),
+                new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
new file mode 100644
index 0000000..8367070
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/spline/QuinticHermiteSplineTest.java
@@ -0,0 +1,106 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.spline;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.spline.SplineParameterizer.MalformedSplineException;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class QuinticHermiteSplineTest {
+  private static final double kMaxDx = 0.127;
+  private static final double kMaxDy = 0.00127;
+  private static final double kMaxDtheta = 0.0872;
+
+  @SuppressWarnings("ParameterName")
+  private void run(Pose2d a, Pose2d b) {
+    // Start the timer.
+    // var start = System.nanoTime();
+
+    // Generate and parameterize the spline.
+    var spline = SplineHelper.getQuinticSplinesFromWaypoints(List.of(a, b))[0];
+    var poses = SplineParameterizer.parameterize(spline);
+
+    // End the timer.
+    // var end = System.nanoTime();
+
+    // Calculate the duration (used when benchmarking)
+    // var durationMicroseconds = (end - start) / 1000.0;
+
+    for (int i = 0; i < poses.size() - 1; i++) {
+      var p0 = poses.get(i);
+      var p1 = poses.get(i + 1);
+
+      // Make sure the twist is under the tolerance defined by the Spline class.
+      var twist = p0.poseMeters.log(p1.poseMeters);
+      assertAll(
+          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
+          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
+          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
+    }
+
+    // Check first point
+    assertAll(
+        () -> assertEquals(a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
+        () -> assertEquals(a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
+        () ->
+            assertEquals(
+                a.getRotation().getRadians(),
+                poses.get(0).poseMeters.getRotation().getRadians(),
+                1E-9));
+
+    // Check last point
+    assertAll(
+        () -> assertEquals(b.getX(), poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
+        () -> assertEquals(b.getY(), poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
+        () ->
+            assertEquals(
+                b.getRotation().getRadians(),
+                poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(),
+                1E-9));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testStraightLine() {
+    run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSimpleSCurve() {
+    run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
+  }
+
+  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
+  @Test
+  void testSquiggly() {
+    run(
+        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
+        new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
+  }
+
+  @Test
+  void testMalformed() {
+    assertThrows(
+        MalformedSplineException.class,
+        () ->
+            run(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+                new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
+    assertThrows(
+        MalformedSplineException.class,
+        () ->
+            run(
+                new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
+                new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
new file mode 100644
index 0000000..add1afb
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/DiscretizationTest.java
@@ -0,0 +1,217 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N2;
+import org.junit.jupiter.api.Test;
+
+public class DiscretizationTest {
+  // Check that for a simple second-order system that we can easily analyze
+  // analytically,
+  @Test
+  public void testDiscretizeA() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var x0 = VecBuilder.fill(1, 1);
+
+    final var discA = Discretization.discretizeA(contA, 1.0);
+    final var x1Discrete = discA.times(x0);
+
+    // We now have pos = vel = 1 and accel = 0, which should give us:
+    final var x1Truth =
+        VecBuilder.fill(
+            1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0), 0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0));
+
+    assertEquals(x1Truth, x1Discrete);
+  }
+
+  // Check that for a simple second-order system that we can easily analyze
+  // analytically,
+  @Test
+  public void testDiscretizeAB() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contB = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(0, 1);
+
+    final var x0 = VecBuilder.fill(1, 1);
+    final var u = VecBuilder.fill(1);
+
+    var discABPair = Discretization.discretizeAB(contA, contB, 1.0);
+    var discA = discABPair.getFirst();
+    var discB = discABPair.getSecond();
+
+    var x1Discrete = discA.times(x0).plus(discB.times(u));
+
+    // We now have pos = vel = accel = 1, which should give us:
+    final var x1Truth =
+        VecBuilder.fill(
+            1.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 0.5 * u.get(0, 0),
+            0.0 * x0.get(0, 0) + 1.0 * x0.get(1, 0) + 1.0 * u.get(0, 0));
+
+    assertEquals(x1Truth, x1Discrete);
+  }
+
+  //                                             dt
+  // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+  //                                             0
+  @Test
+  public void testDiscretizeSlowModelAQ() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+    final double dt = 1.0;
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discQ = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQ).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscQ:\n"
+            + discQ
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+  }
+
+  //                                             dt
+  // Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+  //                                             0
+  @Test
+  public void testDiscretizeFastModelAQ() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1406.29);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+
+    final var dt = 0.005;
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discQ = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQ).normF() < 1e-3,
+        "Expected these to be nearly equal:\ndiscQ:\n"
+            + discQ
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+  }
+
+  // Test that the Taylor series discretization produces nearly identical results.
+  @Test
+  public void testDiscretizeSlowModelAQTaylor() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
+
+    final var dt = 1.0;
+
+    // Continuous Q should be positive semidefinite
+    final var esCont = contQ.getStorage().eig();
+    for (int i = 0; i < contQ.getNumRows(); ++i) {
+      assertTrue(esCont.getEigenvalue(i).real >= 0);
+    }
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discA = Discretization.discretizeA(contA, dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discATaylor = discAQPair.getFirst();
+    var discQTaylor = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQTaylor).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscQTaylor:\n"
+            + discQTaylor
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
+
+    // Discrete Q should be positive semidefinite
+    final var esDisc = discQTaylor.getStorage().eig();
+    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
+      assertTrue(esDisc.getEigenvalue(i).real >= 0);
+    }
+  }
+
+  // Test that the Taylor series discretization produces nearly identical results.
+  @Test
+  public void testDiscretizeFastModelAQTaylor() {
+    final var contA = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0, 1, 0, -1500);
+    final var contQ = new MatBuilder<>(Nat.N2(), Nat.N2()).fill(0.0025, 0, 0, 1);
+
+    final var dt = 0.005;
+
+    // Continuous Q should be positive semidefinite
+    final var esCont = contQ.getStorage().eig();
+    for (int i = 0; i < contQ.getNumRows(); ++i) {
+      assertTrue(esCont.getEigenvalue(i).real >= 0);
+    }
+
+    final var discQIntegrated =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N2, N2> x) ->
+                contA.times(t).exp().times(contQ).times(contA.transpose().times(t).exp()),
+            0.0,
+            new Matrix<>(Nat.N2(), Nat.N2()),
+            dt);
+
+    var discA = Discretization.discretizeA(contA, dt);
+
+    var discAQPair = Discretization.discretizeAQ(contA, contQ, dt);
+    var discATaylor = discAQPair.getFirst();
+    var discQTaylor = discAQPair.getSecond();
+
+    assertTrue(
+        discQIntegrated.minus(discQTaylor).normF() < 1e-3,
+        "Expected these to be nearly equal:\ndiscQTaylor:\n"
+            + discQTaylor
+            + "\ndiscQIntegrated:\n"
+            + discQIntegrated);
+    assertTrue(discA.minus(discATaylor).normF() < 1e-10);
+
+    // Discrete Q should be positive semidefinite
+    final var esDisc = discQTaylor.getStorage().eig();
+    for (int i = 0; i < discQTaylor.getNumRows(); ++i) {
+      assertTrue(esDisc.getEigenvalue(i).real >= 0);
+    }
+  }
+
+  // Test that DiscretizeR() works
+  @Test
+  public void testDiscretizeR() {
+    var contR = Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 0.0, 0.0, 1.0);
+    var discRTruth = Matrix.mat(Nat.N2(), Nat.N2()).fill(4.0, 0.0, 0.0, 2.0);
+
+    var discR = Discretization.discretizeR(contR, 0.5);
+
+    assertTrue(
+        discRTruth.minus(discR).normF() < 1e-10,
+        "Expected these to be nearly equal:\ndiscR:\n" + discR + "\ndiscRTruth:\n" + discRTruth);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
new file mode 100644
index 0000000..37cb1ec
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/LinearSystemIDTest.java
@@ -0,0 +1,91 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import org.junit.jupiter.api.Test;
+
+class LinearSystemIDTest {
+  @Test
+  public void testDrivetrainVelocitySystem() {
+    var model =
+        LinearSystemId.createDrivetrainVelocitySystem(DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6);
+    assertTrue(
+        model
+            .getA()
+            .isEqual(
+                Matrix.mat(Nat.N2(), Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132),
+                0.001));
+
+    assertTrue(
+        model
+            .getB()
+            .isEqual(
+                Matrix.mat(Nat.N2(), Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
+
+    assertTrue(
+        model.getC().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001));
+
+    assertTrue(
+        model.getD().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
+  }
+
+  @Test
+  public void testElevatorSystem() {
+    var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
+    assertTrue(
+        model.getA().isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -99.05473), 0.001));
+
+    assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
+
+    assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(), Nat.N2()).fill(1, 0), 0.001));
+
+    assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
+  }
+
+  @Test
+  public void testFlywheelSystem() {
+    var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
+    assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
+
+    assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
+
+    assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
+
+    assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
+  }
+
+  @Test
+  public void testIdentifyPositionSystem() {
+    // By controls engineering in frc,
+    // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
+    var kv = 1.0;
+    var ka = 0.5;
+    var model = LinearSystemId.identifyPositionSystem(kv, ka);
+
+    assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
+    assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
+  }
+
+  @Test
+  public void testIdentifyVelocitySystem() {
+    // By controls engineering in frc,
+    // V = kv * velocity + ka * acceleration
+    // x-dot =  -kv/ka * v + 1/ka \cdot V
+    var kv = 1.0;
+    var ka = 0.5;
+    var model = LinearSystemId.identifyVelocitySystem(kv, ka);
+
+    assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
+    assertEquals(model.getB(), VecBuilder.fill(1 / ka));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
new file mode 100644
index 0000000..b3fe7e6
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/NumericalIntegrationTest.java
@@ -0,0 +1,68 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+public class NumericalIntegrationTest {
+  @Test
+  public void testExponential() {
+    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
+
+    var y1 =
+        NumericalIntegration.rk4(
+            (Matrix<N1, N1> x) -> {
+              var y = new Matrix<>(Nat.N1(), Nat.N1());
+              y.set(0, 0, Math.exp(x.get(0, 0)));
+              return y;
+            },
+            y0,
+            0.1);
+
+    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
+  }
+
+  @Test
+  public void testExponentialRKF45() {
+    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
+
+    var y1 =
+        NumericalIntegration.rkf45(
+            (x, u) -> {
+              var y = new Matrix<>(Nat.N1(), Nat.N1());
+              y.set(0, 0, Math.exp(x.get(0, 0)));
+              return y;
+            },
+            y0,
+            VecBuilder.fill(0),
+            0.1);
+
+    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
+  }
+
+  @Test
+  public void testExponentialRKDP() {
+    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
+
+    var y1 =
+        NumericalIntegration.rkdp(
+            (x, u) -> {
+              var y = new Matrix<>(Nat.N1(), Nat.N1());
+              y.set(0, 0, Math.exp(x.get(0, 0)));
+              return y;
+            },
+            y0,
+            VecBuilder.fill(0),
+            0.1);
+
+    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
new file mode 100644
index 0000000..7b5e844
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVarying.java
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Num;
+import java.util.function.BiFunction;
+
+public final class RungeKuttaTimeVarying {
+  private RungeKuttaTimeVarying() {
+    // Utility class
+  }
+
+  /**
+   * Performs 4th order Runge-Kutta integration of dx/dt = f(t, y) for dt.
+   *
+   * @param <Rows> Rows in y.
+   * @param <Cols> Columns in y.
+   * @param f The function to integrate. It must take two arguments t and y.
+   * @param t The initial value of t.
+   * @param y The initial value of y.
+   * @param dtSeconds The time over which to integrate.
+   */
+  @SuppressWarnings("MethodTypeParameterName")
+  public static <Rows extends Num, Cols extends Num> Matrix<Rows, Cols> rungeKuttaTimeVarying(
+      BiFunction<Double, Matrix<Rows, Cols>, Matrix<Rows, Cols>> f,
+      double t,
+      Matrix<Rows, Cols> y,
+      double dtSeconds) {
+    final var h = dtSeconds;
+
+    Matrix<Rows, Cols> k1 = f.apply(t, y);
+    Matrix<Rows, Cols> k2 = f.apply(t + dtSeconds * 0.5, y.plus(k1.times(h * 0.5)));
+    Matrix<Rows, Cols> k3 = f.apply(t + dtSeconds * 0.5, y.plus(k2.times(h * 0.5)));
+    Matrix<Rows, Cols> k4 = f.apply(t + dtSeconds, y.plus(k3.times(h)));
+
+    return y.plus((k1.plus(k2.times(2.0)).plus(k3.times(2.0)).plus(k4)).times(h / 6.0));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
new file mode 100644
index 0000000..ee843ab
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/system/RungeKuttaTimeVaryingTest.java
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.system;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+import edu.wpi.first.math.numbers.N1;
+import org.junit.jupiter.api.Test;
+
+public class RungeKuttaTimeVaryingTest {
+  private static Matrix<N1, N1> rungeKuttaTimeVaryingSolution(double t) {
+    return new MatBuilder<>(Nat.N1(), Nat.N1())
+        .fill(12.0 * Math.exp(t) / (Math.pow(Math.exp(t) + 1.0, 2.0)));
+  }
+
+  // Tests RK4 with a time varying solution. From
+  // http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+  //   x' = x (2 / (e^t + 1) - 1)
+  //
+  // The true (analytical) solution is:
+  //
+  // x(t) = 12 * e^t / ((e^t + 1)^2)
+  @Test
+  public void rungeKuttaTimeVaryingTest() {
+    final var y0 = rungeKuttaTimeVaryingSolution(5.0);
+
+    final var y1 =
+        RungeKuttaTimeVarying.rungeKuttaTimeVarying(
+            (Double t, Matrix<N1, N1> x) -> {
+              return new MatBuilder<>(Nat.N1(), Nat.N1())
+                  .fill(x.get(0, 0) * (2.0 / (Math.exp(t) + 1.0) - 1.0));
+            },
+            5.0,
+            y0,
+            1.0);
+    assertEquals(rungeKuttaTimeVaryingSolution(6.0).get(0, 0), y1.get(0, 0), 1e-3);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
new file mode 100644
index 0000000..1805589
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/CentripetalAccelerationConstraintTest.java
@@ -0,0 +1,37 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint;
+import edu.wpi.first.math.util.Units;
+import java.util.Collections;
+import org.junit.jupiter.api.Test;
+
+class CentripetalAccelerationConstraintTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testCentripetalAccelerationConstraint() {
+    double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
+    var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
+
+    Trajectory trajectory =
+        TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var centripetalAcceleration =
+          Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
+
+      t += dt;
+      assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
new file mode 100644
index 0000000..4c8a8ba
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveKinematicsConstraintTest.java
@@ -0,0 +1,48 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import edu.wpi.first.math.util.Units;
+import java.util.Collections;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveKinematicsConstraintTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testDifferentialDriveKinematicsConstraint() {
+    double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
+    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
+    var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
+
+    Trajectory trajectory =
+        TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var chassisSpeeds =
+          new ChassisSpeeds(
+              point.velocityMetersPerSecond,
+              0,
+              point.velocityMetersPerSecond * point.curvatureRadPerMeter);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      t += dt;
+      assertAll(
+          () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
+          () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05));
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
new file mode 100644
index 0000000..87c1bd9
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/DifferentialDriveVoltageConstraintTest.java
@@ -0,0 +1,105 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
+import java.util.ArrayList;
+import java.util.Collections;
+import org.junit.jupiter.api.Test;
+
+class DifferentialDriveVoltageConstraintTest {
+  @SuppressWarnings("LocalVariableName")
+  @Test
+  void testDifferentialDriveVoltageConstraint() {
+    // Pick an unreasonably large kA to ensure the constraint has to do some work
+    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
+    var kinematics = new DifferentialDriveKinematics(0.5);
+    double maxVoltage = 10;
+    var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);
+
+    Trajectory trajectory =
+        TrajectoryGeneratorTest.getTrajectory(Collections.singletonList(constraint));
+
+    var duration = trajectory.getTotalTimeSeconds();
+    var t = 0.0;
+    var dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      var chassisSpeeds =
+          new ChassisSpeeds(
+              point.velocityMetersPerSecond,
+              0,
+              point.velocityMetersPerSecond * point.curvatureRadPerMeter);
+
+      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
+
+      t += dt;
+
+      // 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
+      assertAll(
+          () ->
+              assertTrue(
+                  feedforward.calculate(
+                          wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
+                      <= maxVoltage + 0.05),
+          () ->
+              assertTrue(
+                  feedforward.calculate(
+                          wheelSpeeds.leftMetersPerSecond, point.accelerationMetersPerSecondSq)
+                      >= -maxVoltage - 0.05),
+          () ->
+              assertTrue(
+                  feedforward.calculate(
+                          wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
+                      <= maxVoltage + 0.05),
+          () ->
+              assertTrue(
+                  feedforward.calculate(
+                          wheelSpeeds.rightMetersPerSecond, point.accelerationMetersPerSecondSq)
+                      >= -maxVoltage - 0.05));
+    }
+  }
+
+  @Test
+  void testEndpointHighCurvature() {
+    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
+
+    // Large trackwidth - need to test with radius of curvature less than half of trackwidth
+    var kinematics = new DifferentialDriveKinematics(3);
+    double maxVoltage = 10;
+    var constraint = new DifferentialDriveVoltageConstraint(feedforward, kinematics, maxVoltage);
+
+    var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
+
+    // Radius of curvature should be ~1 meter.
+    assertDoesNotThrow(
+        () ->
+            TrajectoryGenerator.generateTrajectory(
+                new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
+                new ArrayList<Translation2d>(),
+                new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
+                config));
+
+    assertDoesNotThrow(
+        () ->
+            TrajectoryGenerator.generateTrajectory(
+                new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
+                new ArrayList<Translation2d>(),
+                new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
+                config.setReversed(true)));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
new file mode 100644
index 0000000..f9e3c18
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/EllipticalRegionConstraintTest.java
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.constraint.EllipticalRegionConstraint;
+import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+public class EllipticalRegionConstraintTest {
+  @Test
+  void testConstraint() {
+    // Create constraints
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+    var regionConstraint =
+        new EllipticalRegionConstraint(
+            new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
+            Units.feetToMeters(10.0),
+            Units.feetToMeters(5.0),
+            Rotation2d.fromDegrees(180.0),
+            maxVelocityConstraint);
+
+    // Get trajectory
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
+
+    // Iterate through trajectory and check constraints
+    boolean exceededConstraintOutsideRegion = false;
+    for (var point : trajectory.getStates()) {
+      var translation = point.poseMeters.getTranslation();
+
+      if (translation.getX() < Units.feetToMeters(10)
+          && translation.getY() < Units.feetToMeters(5)) {
+        assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
+      } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
+        exceededConstraintOutsideRegion = true;
+      }
+    }
+    assertTrue(exceededConstraintOutsideRegion);
+  }
+
+  @Test
+  void testIsPoseWithinRegion() {
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+
+    var regionConstraintNoRotation =
+        new EllipticalRegionConstraint(
+            new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+            Units.feetToMeters(2.0),
+            Units.feetToMeters(4.0),
+            new Rotation2d(),
+            maxVelocityConstraint);
+
+    assertFalse(
+        regionConstraintNoRotation.isPoseInRegion(
+            new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
+
+    var regionConstraintWithRotation =
+        new EllipticalRegionConstraint(
+            new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+            Units.feetToMeters(2.0),
+            Units.feetToMeters(4.0),
+            Rotation2d.fromDegrees(90.0),
+            maxVelocityConstraint);
+
+    assertTrue(
+        regionConstraintWithRotation.isPoseInRegion(
+            new Pose2d(Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d())));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
new file mode 100644
index 0000000..1ab826e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/RectangularRegionConstraintTest.java
@@ -0,0 +1,61 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertFalse;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.constraint.MaxVelocityConstraint;
+import edu.wpi.first.math.trajectory.constraint.RectangularRegionConstraint;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+public class RectangularRegionConstraintTest {
+  @Test
+  void testConstraint() {
+    // Create constraints
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+    var regionConstraint =
+        new RectangularRegionConstraint(
+            new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+            new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
+            maxVelocityConstraint);
+
+    // Get trajectory
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
+
+    // Iterate through trajectory and check constraints
+    boolean exceededConstraintOutsideRegion = false;
+    for (var point : trajectory.getStates()) {
+      if (regionConstraint.isPoseInRegion(point.poseMeters)) {
+        assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
+      } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
+        exceededConstraintOutsideRegion = true;
+      }
+    }
+    assertTrue(exceededConstraintOutsideRegion);
+  }
+
+  @Test
+  void testIsPoseWithinRegion() {
+    double maxVelocity = Units.feetToMeters(3.0);
+    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
+    var regionConstraint =
+        new RectangularRegionConstraint(
+            new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
+            new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
+            maxVelocityConstraint);
+
+    assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
+    assertTrue(
+        regionConstraint.isPoseInRegion(
+            new Pose2d(Units.feetToMeters(3.0), Units.feetToMeters(14.5), new Rotation2d())));
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java
new file mode 100644
index 0000000..2e80d7b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryConcatenateTest.java
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryConcatenateTest {
+  @Test
+  void testStates() {
+    var t1 =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(),
+            List.of(),
+            new Pose2d(1, 1, new Rotation2d()),
+            new TrajectoryConfig(2, 2));
+
+    var t2 =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(1, 1, new Rotation2d()),
+            List.of(),
+            new Pose2d(2, 2, Rotation2d.fromDegrees(45)),
+            new TrajectoryConfig(2, 2));
+
+    var t = t1.concatenate(t2);
+
+    double time = -1.0;
+    for (int i = 0; i < t.getStates().size(); ++i) {
+      var state = t.getStates().get(i);
+
+      // Make sure that the timestamps are strictly increasing.
+      assertTrue(state.timeSeconds > time);
+      time = state.timeSeconds;
+
+      // Ensure that the states in t are the same as those in t1 and t2.
+      if (i < t1.getStates().size()) {
+        assertEquals(state, t1.getStates().get(i));
+      } else {
+        var st = t2.getStates().get(i - t1.getStates().size() + 1);
+        st.timeSeconds += t1.getTotalTimeSeconds();
+        assertEquals(state, st);
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
new file mode 100644
index 0000000..97c1858
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryGeneratorTest.java
@@ -0,0 +1,84 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static edu.wpi.first.math.util.Units.feetToMeters;
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryGeneratorTest {
+  static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
+    final double maxVelocity = feetToMeters(12.0);
+    final double maxAccel = feetToMeters(12);
+
+    // 2018 cross scale auto waypoints.
+    var sideStart =
+        new Pose2d(feetToMeters(1.54), feetToMeters(23.23), Rotation2d.fromDegrees(-180));
+    var crossScale =
+        new Pose2d(feetToMeters(23.7), feetToMeters(6.8), Rotation2d.fromDegrees(-160));
+
+    var waypoints = new ArrayList<Pose2d>();
+    waypoints.add(sideStart);
+    waypoints.add(
+        sideStart.plus(
+            new Transform2d(
+                new Translation2d(feetToMeters(-13), feetToMeters(0)), new Rotation2d())));
+    waypoints.add(
+        sideStart.plus(
+            new Transform2d(
+                new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
+                Rotation2d.fromDegrees(-90))));
+    waypoints.add(crossScale);
+
+    TrajectoryConfig config =
+        new TrajectoryConfig(maxVelocity, maxAccel).setReversed(true).addConstraints(constraints);
+
+    return TrajectoryGenerator.generateTrajectory(waypoints, config);
+  }
+
+  @Test
+  @SuppressWarnings("LocalVariableName")
+  void testGenerationAndConstraints() {
+    Trajectory trajectory = getTrajectory(new ArrayList<>());
+
+    double duration = trajectory.getTotalTimeSeconds();
+    double t = 0.0;
+    double dt = 0.02;
+
+    while (t < duration) {
+      var point = trajectory.sample(t);
+      t += dt;
+      assertAll(
+          () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
+          () ->
+              assertTrue(
+                  Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0) + 0.05));
+    }
+  }
+
+  @Test
+  void testMalformedTrajectory() {
+    var traj =
+        TrajectoryGenerator.generateTrajectory(
+            Arrays.asList(
+                new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
+                new Pose2d(1, 0, Rotation2d.fromDegrees(180))),
+            new TrajectoryConfig(feetToMeters(12), feetToMeters(12)));
+
+    assertEquals(traj.getStates().size(), 1);
+    assertEquals(traj.getTotalTimeSeconds(), 0);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java
new file mode 100644
index 0000000..bb72601
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryJsonTest.java
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveKinematicsConstraint;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+public class TrajectoryJsonTest {
+  @Test
+  void deserializeMatches() {
+    var config =
+        List.of(new DifferentialDriveKinematicsConstraint(new DifferentialDriveKinematics(20), 3));
+    var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
+
+    var deserialized =
+        assertDoesNotThrow(
+            () ->
+                TrajectoryUtil.deserializeTrajectory(
+                    TrajectoryUtil.serializeTrajectory(trajectory)));
+
+    assertEquals(trajectory.getStates(), deserialized.getStates());
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java
new file mode 100644
index 0000000..6268768
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrajectoryTransformTest.java
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class TrajectoryTransformTest {
+  @Test
+  void testTransformBy() {
+    var config = new TrajectoryConfig(3, 3);
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)), config);
+
+    var transformedTrajectory =
+        trajectory.transformBy(
+            new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
+
+    // Test initial pose.
+    assertEquals(
+        new Pose2d(1, 2, Rotation2d.fromDegrees(30)), transformedTrajectory.sample(0).poseMeters);
+
+    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+  }
+
+  @Test
+  void testRelativeTo() {
+    var config = new TrajectoryConfig(3, 3);
+    var trajectory =
+        TrajectoryGenerator.generateTrajectory(
+            new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
+            List.of(),
+            new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
+            config);
+
+    var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
+
+    // Test initial pose.
+    assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
+
+    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
+  }
+
+  void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
+    for (int i = 0; i < statesA.size() - 1; i++) {
+      var a1 = statesA.get(i).poseMeters;
+      var a2 = statesA.get(i + 1).poseMeters;
+
+      var b1 = statesB.get(i).poseMeters;
+      var b2 = statesB.get(i + 1).poseMeters;
+
+      assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
new file mode 100644
index 0000000..062563f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/trajectory/TrapezoidProfileTest.java
@@ -0,0 +1,253 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.trajectory;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class TrapezoidProfileTest {
+  private static final double kDt = 0.01;
+
+  /**
+   * Asserts "val1" is less than or equal to "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   */
+  private static void assertLessThanOrEquals(double val1, double val2) {
+    assertTrue(val1 <= val2, val1 + " is greater than " + val2);
+  }
+
+  /**
+   * Asserts "val1" is within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertNear(double val1, double val2, double eps) {
+    assertTrue(
+        Math.abs(val1 - val2) <= eps,
+        "Difference between " + val1 + " and " + val2 + " is greater than " + eps);
+  }
+
+  /**
+   * Asserts "val1" is less than or within "eps" of "val2".
+   *
+   * @param val1 First operand in comparison.
+   * @param val2 Second operand in comparison.
+   * @param eps Tolerance for whether values are near to each other.
+   */
+  private static void assertLessThanOrNear(double val1, double val2, double eps) {
+    if (val1 <= val2) {
+      assertLessThanOrEquals(val1, val2);
+    } else {
+      assertNear(val1, val2, eps);
+    }
+  }
+
+  @Test
+  void reachesGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 450; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(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
+  void posContinousUnderVelChange() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double lastPos = state.position;
+    for (int i = 0; i < 1600; ++i) {
+      if (i == 400) {
+        constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+      }
+
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      double 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.
+        assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
+
+        assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
+      }
+
+      lastPos = state.position;
+    }
+    assertEquals(state, goal);
+  }
+
+  // There is some somewhat tricky code for dealing with going backwards
+  @Test
+  void backwards() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void switchGoalInMiddle() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNotEquals(state, goal);
+
+    goal = new TrapezoidProfile.State(0.0, 0.0);
+    for (int i = 0; i < 550; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  // Checks to make sure that it hits top speed
+  @Test
+  void topSpeed() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 200; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertNear(constraints.maxVelocity, state.velocity, 10e-5);
+
+    for (int i = 0; i < 2000; ++i) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+    }
+    assertEquals(state, goal);
+  }
+
+  @Test
+  void timingToCurrent() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+    TrapezoidProfile.State state = new TrapezoidProfile.State();
+
+    for (int i = 0; i < 400; i++) {
+      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
+    }
+  }
+
+  @Test
+  void timingToGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(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
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(1);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingToNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && state.equals(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
+        assertNear(predictedTimeLeft, i / 100.0, 0.25);
+        reachedGoal = true;
+      }
+    }
+  }
+
+  @Test
+  void timingBeforeNegativeGoal() {
+    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
+    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
+
+    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
+    TrapezoidProfile.State state = profile.calculate(kDt);
+
+    double predictedTimeLeft = profile.timeLeftUntil(-1);
+    boolean reachedGoal = false;
+    for (int i = 0; i < 400; i++) {
+      profile = new TrapezoidProfile(constraints, goal, state);
+      state = profile.calculate(kDt);
+      if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
+        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
+        reachedGoal = true;
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java
new file mode 100644
index 0000000..99a8b5a
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/math/util/UnitsTest.java
@@ -0,0 +1,75 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.math.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.wpilibj.UtilityClassTest;
+import org.junit.jupiter.api.Test;
+
+class UnitsTest extends UtilityClassTest<Units> {
+  UnitsTest() {
+    super(Units.class);
+  }
+
+  @Test
+  void metersToFeetTest() {
+    assertEquals(3.28, Units.metersToFeet(1), 1e-2);
+  }
+
+  @Test
+  void feetToMetersTest() {
+    assertEquals(0.30, Units.feetToMeters(1), 1e-2);
+  }
+
+  @Test
+  void metersToInchesTest() {
+    assertEquals(39.37, Units.metersToInches(1), 1e-2);
+  }
+
+  @Test
+  void inchesToMetersTest() {
+    assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
+  }
+
+  @Test
+  void degreesToRadiansTest() {
+    assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
+  }
+
+  @Test
+  void radiansToDegreesTest() {
+    assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
+  }
+
+  @Test
+  void rotationsPerMinuteToRadiansPerSecondTest() {
+    assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
+  }
+
+  @Test
+  void radiansPerSecondToRotationsPerMinute() {
+    assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
+  }
+
+  @Test
+  void millisecondsToSeconds() {
+    assertEquals(0.5, Units.millisecondsToSeconds(500), 1e-2);
+  }
+
+  @Test
+  void secondsToMilliseconds() {
+    assertEquals(1500, Units.secondsToMilliseconds(1.5), 1e-2);
+  }
+
+  void kilogramsToLbsTest() {
+    assertEquals(2.20462, Units.kilogramsToLbs(1), 1e-2);
+  }
+
+  @Test
+  void lbsToKilogramsTest() {
+    assertEquals(0.453592, Units.lbsToKilograms(1), 1e-2);
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
deleted file mode 100644
index da58b29..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/LinearFilterTest.java
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import java.util.Random;
-import java.util.function.DoubleFunction;
-import java.util.stream.Stream;
-
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.params.ParameterizedTest;
-import org.junit.jupiter.params.provider.Arguments;
-import org.junit.jupiter.params.provider.MethodSource;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-import static org.junit.jupiter.params.provider.Arguments.arguments;
-
-class LinearFilterTest {
-  private static final double kFilterStep = 0.005;
-  private static final double kFilterTime = 2.0;
-  private static final double kSinglePoleIIRTimeConstant = 0.015915;
-  private static final double kHighPassTimeConstant = 0.006631;
-  private static final int kMovAvgTaps = 6;
-
-  private static final double kSinglePoleIIRExpectedOutput = -3.2172003;
-  private static final double kHighPassExpectedOutput = 10.074717;
-  private static final double kMovAvgExpectedOutput = -10.191644;
-
-  @SuppressWarnings("ParameterName")
-  private static double getData(double t) {
-    return 100.0 * Math.sin(2.0 * Math.PI * t) + 20.0 * Math.cos(50.0 * Math.PI * t);
-  }
-
-  @SuppressWarnings("ParameterName")
-  private static double getPulseData(double t) {
-    if (Math.abs(t - 1.0) < 0.001) {
-      return 1.0;
-    } else {
-      return 0.0;
-    }
-  }
-
-  @Test
-  void illegalTapNumberTest() {
-    assertThrows(IllegalArgumentException.class, () -> LinearFilter.movingAverage(0));
-  }
-
-  /**
-   * Test if the filter reduces the noise produced by a signal generator.
-   */
-  @ParameterizedTest
-  @MethodSource("noiseFilterProvider")
-  void noiseReduceTest(final LinearFilter filter) {
-    double noiseGenError = 0.0;
-    double filterError = 0.0;
-
-    final Random gen = new Random();
-    final double kStdDev = 10.0;
-
-    for (double t = 0; t < kFilterTime; t += kFilterStep) {
-      final double theory = getData(t);
-      final double noise = gen.nextGaussian() * kStdDev;
-      filterError += Math.abs(filter.calculate(theory + noise) - theory);
-      noiseGenError += Math.abs(noise - theory);
-    }
-
-    assertTrue(noiseGenError > filterError,
-        "Filter should have reduced noise accumulation from " + noiseGenError
-            + " but failed. The filter error was " + filterError);
-  }
-
-  static Stream<LinearFilter> noiseFilterProvider() {
-    return Stream.of(
-        LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
-        LinearFilter.movingAverage(kMovAvgTaps)
-    );
-  }
-
-  /**
-   * Test if the linear filters produce consistent output for a given data set.
-   */
-  @ParameterizedTest
-  @MethodSource("outputFilterProvider")
-  void outputTest(final LinearFilter filter, final DoubleFunction<Double> data,
-                  final double expectedOutput) {
-    double filterOutput = 0.0;
-    for (double t = 0.0; t < kFilterTime; t += kFilterStep) {
-      filterOutput = filter.calculate(data.apply(t));
-    }
-
-    assertEquals(expectedOutput, filterOutput, 5e-5, "Filter output was incorrect.");
-  }
-
-  static Stream<Arguments> outputFilterProvider() {
-    return Stream.of(
-        arguments(LinearFilter.singlePoleIIR(kSinglePoleIIRTimeConstant, kFilterStep),
-            (DoubleFunction<Double>) LinearFilterTest::getData,
-            kSinglePoleIIRExpectedOutput),
-        arguments(LinearFilter.highPass(kHighPassTimeConstant, kFilterStep),
-            (DoubleFunction<Double>) LinearFilterTest::getData,
-            kHighPassExpectedOutput),
-        arguments(LinearFilter.movingAverage(kMovAvgTaps),
-            (DoubleFunction<Double>) LinearFilterTest::getData,
-            kMovAvgExpectedOutput),
-        arguments(LinearFilter.movingAverage(kMovAvgTaps),
-            (DoubleFunction<Double>) LinearFilterTest::getPulseData,
-            0.0)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
deleted file mode 100644
index b2b596c..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/MedianFilterTest.java
+++ /dev/null
@@ -1,64 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class MedianFilterTest {
-  @Test
-  void medianFilterNotFullTestEven() {
-    MedianFilter filter = new MedianFilter(10);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(4);
-
-    assertEquals(3.5, filter.calculate(1000));
-  }
-
-  @Test
-  void medianFilterNotFullTestOdd() {
-    MedianFilter filter = new MedianFilter(10);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(4);
-    filter.calculate(7);
-
-    assertEquals(4, filter.calculate(1000));
-  }
-
-  @Test
-  void medianFilterFullTestEven() {
-    MedianFilter filter = new MedianFilter(6);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(0);
-    filter.calculate(5);
-    filter.calculate(4);
-    filter.calculate(1000);
-
-    assertEquals(4.5, filter.calculate(99));
-  }
-
-  @Test
-  void medianFilterFullTestOdd() {
-    MedianFilter filter = new MedianFilter(5);
-
-    filter.calculate(3);
-    filter.calculate(0);
-    filter.calculate(5);
-    filter.calculate(4);
-    filter.calculate(1000);
-
-    assertEquals(5, filter.calculate(99));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
index 05c5786..8eed93f 100644
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
+++ b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/UtilityClassTest.java
@@ -1,28 +1,30 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 package edu.wpi.first.wpilibj;
 
-import java.lang.reflect.Constructor;
-import java.lang.reflect.InvocationTargetException;
-import java.lang.reflect.Modifier;
-import java.util.Arrays;
-import java.util.stream.Stream;
-
-import org.junit.jupiter.api.DynamicTest;
-import org.junit.jupiter.api.Test;
-import org.junit.jupiter.api.TestFactory;
-
 import static org.junit.jupiter.api.Assertions.assertEquals;
 import static org.junit.jupiter.api.Assertions.assertFalse;
 import static org.junit.jupiter.api.Assertions.assertThrows;
 import static org.junit.jupiter.api.Assertions.assertTrue;
 import static org.junit.jupiter.api.DynamicTest.dynamicTest;
 
+import java.lang.reflect.Constructor;
+import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Modifier;
+import java.util.Arrays;
+import java.util.stream.Stream;
+import org.junit.jupiter.api.DynamicTest;
+import org.junit.jupiter.api.Test;
+import org.junit.jupiter.api.TestFactory;
+
+// Declaring this class abstract prevents UtilityClassTest from running on itself and throwing the
+// following exception:
+//
+// org.junit.jupiter.api.extension.ParameterResolutionException: No ParameterResolver registered
+// for parameter [java.lang.Class<T> arg0] in constructor [protected
+// edu.wpi.first.wpilibj.UtilityClassTest(java.lang.Class<T>)].
 @SuppressWarnings("PMD.AbstractClassWithoutAbstractMethod")
 public abstract class UtilityClassTest<T> {
   private final Class<T> m_clazz;
@@ -33,8 +35,7 @@
 
   @Test
   public void singleConstructorTest() {
-    assertEquals(1, m_clazz.getDeclaredConstructors().length,
-        "More than one constructor defined");
+    assertEquals(1, m_clazz.getDeclaredConstructors().length, "More than one constructor defined");
   }
 
   @Test
@@ -55,7 +56,9 @@
   Stream<DynamicTest> publicMethodsStaticTestFactory() {
     return Arrays.stream(m_clazz.getDeclaredMethods())
         .filter(method -> Modifier.isPublic(method.getModifiers()))
-        .map(method -> dynamicTest(method.getName(),
-            () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
+        .map(
+            method ->
+                dynamicTest(
+                    method.getName(), () -> assertTrue(Modifier.isStatic(method.getModifiers()))));
   }
 }
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java
deleted file mode 100644
index 75a29e3..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/ControlAffinePlantInversionFeedforwardTest.java
+++ /dev/null
@@ -1,71 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class ControlAffinePlantInversionFeedforwardTest {
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  void testCalculate() {
-    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
-            new ControlAffinePlantInversionFeedforward<N2, N1>(
-                    Nat.N2(),
-                    Nat.N1(),
-                    this::getDynamics,
-                    0.02);
-
-    assertEquals(48.0, feedforward.calculate(
-         VecBuilder.fill(2, 2),
-         VecBuilder.fill(3, 3)).get(0, 0),
-         1e-6);
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  void testCalculateState() {
-    ControlAffinePlantInversionFeedforward<N2, N1> feedforward =
-        new ControlAffinePlantInversionFeedforward<N2, N1>(
-            Nat.N2(),
-            Nat.N1(),
-            this::getDynamics,
-            0.02);
-
-    assertEquals(48.0, feedforward.calculate(
-            VecBuilder.fill(2, 2),
-            VecBuilder.fill(3, 3)).get(0, 0),
-            1e-6);
-  }
-
-  @SuppressWarnings("ParameterName")
-  protected Matrix<N2, N1> getDynamics(Matrix<N2, N1> x, Matrix<N1, N1> u) {
-    var result = new Matrix<>(Nat.N2(), Nat.N1());
-
-    result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x)
-            .plus(VecBuilder.fill(0, 1).times(u));
-
-    return result;
-  }
-
-  @SuppressWarnings("ParameterName")
-  protected Matrix<N2, N1> getStateDynamics(Matrix<N2, N1> x) {
-    var result = new Matrix<>(Nat.N2(), Nat.N1());
-
-    result = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.000, 0, 0, 1.000).times(x);
-
-    return result;
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java
deleted file mode 100644
index 8a09383..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearPlantInversionFeedforwardTest.java
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class LinearPlantInversionFeedforwardTest {
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  void testCalculate() {
-    Matrix<N2, N2> A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1);
-    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
-
-    LinearPlantInversionFeedforward<N2, N1, N1> feedforward =
-            new LinearPlantInversionFeedforward<N2, N1, N1>(A, B, 0.02);
-
-    assertEquals(47.502599, feedforward.calculate(
-            VecBuilder.fill(2, 2),
-            VecBuilder.fill(3, 3)).get(0, 0),
-            0.002);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java
deleted file mode 100644
index e047198..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearQuadraticRegulatorTest.java
+++ /dev/null
@@ -1,116 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class LinearQuadraticRegulatorTest {
-  public static LinearSystem<N2, N1, N1> elevatorPlant;
-  static LinearSystem<N2, N1, N1> armPlant;
-
-  static {
-    createArm();
-    createElevator();
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  public static void createArm() {
-    var motors = DCMotor.getVex775Pro(2);
-
-    var m = 4.0;
-    var r = 0.4;
-    var J = 1d / 3d * m * r * r;
-    var G = 100.0;
-
-    armPlant = LinearSystemId.createSingleJointedArmSystem(motors, J, G);
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  public static void createElevator() {
-    var motors = DCMotor.getVex775Pro(2);
-
-    var m = 5.0;
-    var r = 0.0181864;
-    var G = 1.0;
-    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
-  }
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  public void testLQROnElevator() {
-
-    var qElms = VecBuilder.fill(0.02, 0.4);
-    var rElms = VecBuilder.fill(12.0);
-    var dt = 0.00505;
-
-    var controller = new LinearQuadraticRegulator<>(
-          elevatorPlant, qElms, rElms, dt);
-
-    var k = controller.getK();
-
-    assertEquals(522.153, k.get(0, 0), 0.1);
-    assertEquals(38.2, k.get(0, 1), 0.1);
-  }
-
-  @Test
-  public void testFourMotorElevator() {
-
-    var dt = 0.020;
-
-    var plant = LinearSystemId.createElevatorSystem(
-          DCMotor.getVex775Pro(4),
-          8.0,
-          0.75 * 25.4 / 1000.0,
-          14.67);
-
-    var regulator = new LinearQuadraticRegulator<>(
-          plant,
-          VecBuilder.fill(0.1, 0.2),
-          VecBuilder.fill(12.0),
-          dt);
-
-    assertEquals(10.381, regulator.getK().get(0, 0), 1e-2);
-    assertEquals(0.6929, regulator.getK().get(0, 1), 1e-2);
-
-  }
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  public void testLQROnArm() {
-
-    var motors = DCMotor.getVex775Pro(2);
-
-    var m = 4.0;
-    var r = 0.4;
-    var G = 100.0;
-
-    var plant = LinearSystemId.createSingleJointedArmSystem(motors, 1d / 3d * m * r * r, G);
-
-    var qElms = VecBuilder.fill(0.01745, 0.08726);
-    var rElms = VecBuilder.fill(12.0);
-    var dt = 0.00505;
-
-    var controller = new LinearQuadraticRegulator<>(
-          plant, qElms, rElms, dt);
-
-    var k = controller.getK();
-
-    assertEquals(19.16, k.get(0, 0), 0.1);
-    assertEquals(3.32, k.get(0, 1), 0.1);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
deleted file mode 100644
index 14a0a9d..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/controller/LinearSystemLoopTest.java
+++ /dev/null
@@ -1,169 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.controller;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Random;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.estimator.KalmanFilter;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.LinearSystemLoop;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class LinearSystemLoopTest {
-  public static final double kDt = 0.00505;
-  private static final double kPositionStddev = 0.0001;
-  private static final Random random = new Random();
-  private final LinearSystemLoop<N2, N1, N1> m_loop;
-
-  @SuppressWarnings("LocalVariableName")
-  public LinearSystemLoopTest() {
-    LinearSystem<N2, N1, N1> plant = LinearSystemId.createElevatorSystem(DCMotor.getVex775Pro(2), 5,
-          0.0181864, 1.0);
-    KalmanFilter<N2, N1, N1> observer = new KalmanFilter<>(Nat.N2(), Nat.N1(), plant,
-          VecBuilder.fill(0.05, 1.0),
-          VecBuilder.fill(0.0001), kDt);
-
-    var qElms = VecBuilder.fill(0.02, 0.4);
-    var rElms = VecBuilder.fill(12.0);
-    var dt = 0.00505;
-
-    var controller = new LinearQuadraticRegulator<>(
-          plant, qElms, rElms, dt);
-
-    m_loop = new LinearSystemLoop<>(plant, controller, observer, 12, dt);
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  private static void updateTwoState(LinearSystemLoop<N2, N1, N1> loop, double noise) {
-    Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
-          VecBuilder.fill(noise)
-    );
-
-    loop.correct(y);
-    loop.predict(kDt);
-  }
-
-  @Test
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public void testStateSpaceEnabled() {
-
-    m_loop.reset(VecBuilder.fill(0, 0));
-    Matrix<N2, N1> references = VecBuilder.fill(2.0, 0.0);
-    var constraints = new TrapezoidProfile.Constraints(4, 3);
-    m_loop.setNextR(references);
-
-    TrapezoidProfile profile;
-    TrapezoidProfile.State state;
-    for (int i = 0; i < 1000; i++) {
-      profile = new TrapezoidProfile(
-            constraints, new TrapezoidProfile.State(m_loop.getXHat(0), m_loop.getXHat(1)),
-            new TrapezoidProfile.State(references.get(0, 0), references.get(1, 0))
-      );
-      state = profile.calculate(kDt);
-      m_loop.setNextR(VecBuilder.fill(state.position, state.velocity));
-
-      updateTwoState(m_loop, (random.nextGaussian()) * kPositionStddev);
-      var u = m_loop.getU(0);
-
-      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
-    }
-
-    assertEquals(2.0, m_loop.getXHat(0), 0.05);
-    assertEquals(0.0, m_loop.getXHat(1), 0.5);
-
-  }
-
-  @Test
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public void testFlywheelEnabled() {
-
-    LinearSystem<N1, N1, N1> plant = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2),
-          0.00289, 1.0);
-    KalmanFilter<N1, N1, N1> observer = new KalmanFilter<>(Nat.N1(), Nat.N1(), plant,
-          VecBuilder.fill(1.0),
-          VecBuilder.fill(kPositionStddev), kDt);
-
-    var qElms = VecBuilder.fill(9.0);
-    var rElms = VecBuilder.fill(12.0);
-
-    var controller = new LinearQuadraticRegulator<>(
-          plant, qElms, rElms, kDt);
-
-    var feedforward = new LinearPlantInversionFeedforward<>(plant, kDt);
-
-    var loop = new LinearSystemLoop<>(plant, controller, feedforward, observer, 12);
-
-    loop.reset(VecBuilder.fill(0.0));
-    var references = VecBuilder.fill(3000 / 60d * 2 * Math.PI);
-
-    loop.setNextR(references);
-
-    List<Double> timeData = new ArrayList<>();
-    List<Double> xHat = new ArrayList<>();
-    List<Double> volt = new ArrayList<>();
-    List<Double> reference = new ArrayList<>();
-    List<Double> error = new ArrayList<>();
-
-    var time = 0.0;
-    while (time < 10) {
-
-      if (time > 10) {
-        break;
-      }
-
-      loop.setNextR(references);
-
-      Matrix<N1, N1> y = loop.getPlant().calculateY(loop.getXHat(), loop.getU()).plus(
-            VecBuilder.fill(random.nextGaussian() * kPositionStddev)
-      );
-
-      loop.correct(y);
-      loop.predict(kDt);
-      var u = loop.getU(0);
-
-      assertTrue(u >= -12.1 && u <= 12.1, "U out of bounds! Got " + u);
-
-      xHat.add(loop.getXHat(0) / 2d / Math.PI * 60);
-      volt.add(u);
-      timeData.add(time);
-      reference.add(references.get(0, 0) / 2d / Math.PI * 60);
-      error.add(loop.getError(0) / 2d / Math.PI * 60);
-      time += kDt;
-    }
-
-    //    XYChart bigChart = new XYChartBuilder().build();
-    //    bigChart.addSeries("Xhat, RPM", timeData, xHat);
-    //    bigChart.addSeries("Reference, RPM", timeData, reference);
-    //    bigChart.addSeries("Error, RPM", timeData, error);
-
-    //    XYChart smolChart = new XYChartBuilder().build();
-    //    smolChart.addSeries("Volts, V", timeData, volt);
-
-    //  try {
-    //      new SwingWrapper<>(List.of(bigChart, smolChart)).displayChartMatrix();
-    //      Thread.sleep(10000000);
-    //    } catch (InterruptedException e) { e.printStackTrace(); }
-
-    assertEquals(0.0, loop.getError(0), 0.1);
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
deleted file mode 100644
index 6c7cc92..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/ExtendedKalmanFilterTest.java
+++ /dev/null
@@ -1,219 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N3;
-import edu.wpi.first.wpiutil.math.numbers.N5;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-@SuppressWarnings("ParameterName")
-public class ExtendedKalmanFilterTest {
-  public static Matrix<N5, N1> getDynamics(final Matrix<N5, N1> x, final Matrix<N2, N1> u) {
-    final var motors = DCMotor.getCIM(2);
-
-    final var gr = 7.08; // Gear ratio
-    final var rb = 0.8382 / 2.0; // Wheelbase radius (track width)
-    final var r = 0.0746125; // Wheel radius
-    final var m = 63.503; // Robot mass
-    final var J = 5.6; // Robot moment of inertia
-
-    final var C1 =
-          -Math.pow(gr, 2) * motors.m_KtNMPerAmp / (
-                motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
-    final var C2 = gr * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
-    final var k1 = 1.0 / m + rb * rb / J;
-    final var k2 = 1.0 / m - rb * rb / J;
-
-    final var vl = x.get(3, 0);
-    final var vr = x.get(4, 0);
-    final var Vl = u.get(0, 0);
-    final var Vr = u.get(1, 0);
-
-    final Matrix<N5, N1> result = new Matrix<>(Nat.N5(), Nat.N1());
-    final var v = 0.5 * (vl + vr);
-    result.set(0, 0, v * Math.cos(x.get(2, 0)));
-    result.set(1, 0, v * Math.sin(x.get(2, 0)));
-    result.set(2, 0, (vr - vl) / (2.0 * rb));
-    result.set(3, 0, k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)));
-    result.set(4, 0, k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr)));
-    return result;
-  }
-
-  public static Matrix<N3, N1> getLocalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
-    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0));
-  }
-
-  public static Matrix<N5, N1> getGlobalMeasurementModel(Matrix<N5, N1> x, Matrix<N2, N1> u) {
-    return VecBuilder.fill(
-          x.get(0, 0),
-          x.get(1, 0),
-          x.get(2, 0),
-          x.get(3, 0),
-          x.get(4, 0)
-    );
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  public void testInit() {
-    double dtSeconds = 0.00505;
-
-    assertDoesNotThrow(() -> {
-      ExtendedKalmanFilter<N5, N2, N3> observer =
-            new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
-                  ExtendedKalmanFilterTest::getDynamics,
-                  ExtendedKalmanFilterTest::getLocalMeasurementModel,
-                  VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
-                  VecBuilder.fill(0.0001, 0.01, 0.01), dtSeconds);
-
-      Matrix<N2, N1> u = VecBuilder.fill(12.0, 12.0);
-      observer.predict(u, dtSeconds);
-
-      var localY = getLocalMeasurementModel(observer.getXhat(), u);
-      observer.correct(u, localY);
-
-      var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
-      var R = StateSpaceUtil.makeCostMatrix(
-            VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
-      observer.correct(Nat.N5(),
-            u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
-    });
-
-  }
-
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
-      "PMD.ExcessiveMethodLength"})
-  @Test
-  public void testConvergence() {
-    double dtSeconds = 0.00505;
-    double rbMeters = 0.8382 / 2.0; // Robot radius
-
-    ExtendedKalmanFilter<N5, N2, N3> observer =
-          new ExtendedKalmanFilter<>(Nat.N5(), Nat.N2(), Nat.N3(),
-                ExtendedKalmanFilterTest::getDynamics,
-                ExtendedKalmanFilterTest::getLocalMeasurementModel,
-                VecBuilder.fill(0.5, 0.5, 10.0, 1.0, 1.0),
-                VecBuilder.fill(0.001, 0.01, 0.01), dtSeconds);
-
-    List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
-          new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-          waypoints,
-          new TrajectoryConfig(8.8, 0.1)
-    );
-
-    Matrix<N5, N1> r = new Matrix<>(Nat.N5(), Nat.N1());
-
-    Matrix<N5, N1> nextR = new Matrix<>(Nat.N5(), Nat.N1());
-    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
-
-    List<Double> trajXs = new ArrayList<>();
-    List<Double> trajYs = new ArrayList<>();
-
-    List<Double> observerXs = new ArrayList<>();
-    List<Double> observerYs = new ArrayList<>();
-
-    var B = NumericalJacobian.numericalJacobianU(Nat.N5(), Nat.N2(),
-          ExtendedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N5(), Nat.N1()), u);
-
-    observer.setXhat(VecBuilder.fill(
-          trajectory.getInitialPose().getTranslation().getX(),
-          trajectory.getInitialPose().getTranslation().getY(),
-          trajectory.getInitialPose().getRotation().getRadians(),
-          0.0, 0.0));
-
-    var groundTruthX = observer.getXhat();
-
-    double totalTime = trajectory.getTotalTimeSeconds();
-    for (int i = 0; i < (totalTime / dtSeconds); i++) {
-      var ref = trajectory.sample(dtSeconds * i);
-      double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
-      double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
-
-      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
-      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
-      nextR.set(2, 0, ref.poseMeters.getRotation().getRadians());
-      nextR.set(3, 0, vl);
-      nextR.set(4, 0, vr);
-
-      var localY =
-            getLocalMeasurementModel(groundTruthX, u);
-      var whiteNoiseStdDevs = VecBuilder.fill(0.0001, 0.5, 0.5);
-      observer.correct(u,
-            localY.plus(StateSpaceUtil.makeWhiteNoiseVector(whiteNoiseStdDevs)));
-
-      Matrix<N5, N1> rdot = nextR.minus(r).div(dtSeconds);
-      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
-
-      observer.predict(u, dtSeconds);
-
-      groundTruthX = RungeKutta.rungeKutta(ExtendedKalmanFilterTest::getDynamics,
-            groundTruthX, u, dtSeconds);
-
-      r = nextR;
-
-      trajXs.add(ref.poseMeters.getTranslation().getX());
-      trajYs.add(ref.poseMeters.getTranslation().getY());
-
-      observerXs.add(observer.getXhat().get(0, 0));
-      observerYs.add(observer.getXhat().get(1, 0));
-    }
-
-    var localY = getLocalMeasurementModel(observer.getXhat(), u);
-    observer.correct(u, localY);
-
-    var globalY = getGlobalMeasurementModel(observer.getXhat(), u);
-    var R = StateSpaceUtil.makeCostMatrix(
-          VecBuilder.fill(0.01, 0.01, 0.0001, 0.5, 0.5));
-    observer.correct(Nat.N5(), u, globalY, ExtendedKalmanFilterTest::getGlobalMeasurementModel, R);
-
-    //    var chartBuilder = new XYChartBuilder();
-    //    chartBuilder.title = "The Magic of Sensor Fusion, now with a "
-    //          + observer.getClass().getSimpleName();
-    //    var xyPosChart = chartBuilder.build();
-    //
-    //    xyPosChart.setXAxisTitle("X pos, meters");
-    //    xyPosChart.setYAxisTitle("Y pos, meters");
-    //    xyPosChart.addSeries("Trajectory", trajXs, trajYs);
-    //    xyPosChart.addSeries("xHat", observerXs, observerYs);
-    //    new SwingWrapper<>(xyPosChart).displayChart();
-    //    try {
-    //      Thread.sleep(1000000000);
-    //    } catch (InterruptedException ex) {
-    //      ex.printStackTrace();
-    //    }
-
-    var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
-    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 1.0);
-    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 1.0);
-    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
-    assertEquals(0.0, observer.getXhat(3), 1.0);
-    assertEquals(0.0, observer.getXhat(4), 1.0);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java
deleted file mode 100644
index 2a434fb..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/KalmanFilterTest.java
+++ /dev/null
@@ -1,258 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import java.util.ArrayList;
-import java.util.List;
-import java.util.Random;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.system.LinearSystem;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N3;
-import edu.wpi.first.wpiutil.math.numbers.N6;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class KalmanFilterTest {
-  private static LinearSystem<N2, N1, N1> elevatorPlant;
-
-  private static final double kDt = 0.00505;
-
-  static {
-    createElevator();
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  public static void createElevator() {
-    var motors = DCMotor.getVex775Pro(2);
-
-    var m = 5.0;
-    var r = 0.0181864;
-    var G = 1.0;
-    elevatorPlant = LinearSystemId.createElevatorSystem(motors, m, r, G);
-  }
-
-  // A swerve drive system where the states are [x, y, theta, vx, vy, vTheta]^T,
-  // Y is [x, y, theta]^T and u is [ax, ay, alpha}^T
-  LinearSystem<N6, N3, N3> m_swerveObserverSystem = new LinearSystem<>(
-      Matrix.mat(Nat.N6(), Nat.N6()).fill( // A
-              0, 0, 0, 1, 0, 0,
-              0, 0, 0, 0, 1, 0,
-              0, 0, 0, 0, 0, 1,
-              0, 0, 0, 0, 0, 0,
-              0, 0, 0, 0, 0, 0,
-              0, 0, 0, 0, 0, 0),
-        Matrix.mat(Nat.N6(), Nat.N3()).fill( // B
-              0, 0, 0,
-              0, 0, 0,
-              0, 0, 0,
-              1, 0, 0,
-              0, 1, 0,
-              0, 0, 1
-        ),
-        Matrix.mat(Nat.N3(), Nat.N6()).fill( // C
-              1, 0, 0, 0, 0, 0,
-              0, 1, 0, 0, 0, 0,
-              0, 0, 1, 0, 0, 0
-        ),
-        new Matrix<>(Nat.N3(), Nat.N3())); // D
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  public void testElevatorKalmanFilter() {
-
-    var Q = VecBuilder.fill(0.05, 1.0);
-    var R = VecBuilder.fill(0.0001);
-
-    assertDoesNotThrow(() -> new KalmanFilter<>(Nat.N2(), Nat.N1(), elevatorPlant, Q, R, kDt));
-  }
-
-  @Test
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public void testSwerveKFStationary() {
-
-    var random = new Random();
-
-    var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
-          m_swerveObserverSystem,
-          VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
-          // weights
-          VecBuilder.fill(2, 2, 2), // measurement weights
-          0.020
-    );
-
-    List<Double> xhatsX = new ArrayList<>();
-    List<Double> measurementsX = new ArrayList<>();
-    List<Double> xhatsY = new ArrayList<>();
-    List<Double> measurementsY = new ArrayList<>();
-
-    Matrix<N3, N1> measurement;
-    for (int i = 0; i < 100; i++) {
-      // the robot is at [0, 0, 0] so we just park here
-      measurement = VecBuilder.fill(
-            random.nextGaussian(), random.nextGaussian(), random.nextGaussian());
-      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
-
-      // we continue to not accelerate
-      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
-
-      measurementsX.add(measurement.get(0, 0));
-      measurementsY.add(measurement.get(1, 0));
-      xhatsX.add(filter.getXhat(0));
-      xhatsY.add(filter.getXhat(1));
-    }
-
-    //var chart = new XYChartBuilder().build();
-    //chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
-    //chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
-    //try {
-    //  new SwingWrapper<>(chart).displayChart();
-    //  Thread.sleep(10000000);
-    //} catch (Exception ign) {
-    //}
-    assertEquals(0.0, filter.getXhat(0), 0.3);
-    assertEquals(0.0, filter.getXhat(0), 0.3);
-  }
-
-  @Test
-  @SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-  public void testSwerveKFMovingWithoutAccelerating() {
-
-    var random = new Random();
-
-    var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
-          m_swerveObserverSystem,
-          VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
-          // weights
-          VecBuilder.fill(4, 4, 4), // measurement weights
-          0.020
-    );
-
-    List<Double> xhatsX = new ArrayList<>();
-    List<Double> measurementsX = new ArrayList<>();
-    List<Double> xhatsY = new ArrayList<>();
-    List<Double> measurementsY = new ArrayList<>();
-
-    // we set the velocity of the robot so that it's moving forward slowly
-    filter.setXhat(0, 0.5);
-    filter.setXhat(1, 0.5);
-
-    for (int i = 0; i < 300; i++) {
-      // the robot is at [0, 0, 0] so we just park here
-      var measurement = VecBuilder.fill(
-            random.nextGaussian() / 10d,
-            random.nextGaussian() / 10d,
-            random.nextGaussian() / 4d // std dev of [1, 1, 1]
-      );
-
-      filter.correct(VecBuilder.fill(0.0, 0.0, 0.0), measurement);
-
-      // we continue to not accelerate
-      filter.predict(VecBuilder.fill(0.0, 0.0, 0.0), 0.020);
-
-      measurementsX.add(measurement.get(0, 0));
-      measurementsY.add(measurement.get(1, 0));
-      xhatsX.add(filter.getXhat(0));
-      xhatsY.add(filter.getXhat(1));
-    }
-
-    //var chart = new XYChartBuilder().build();
-    //chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
-    //chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
-    //try {
-    //  new SwingWrapper<>(chart).displayChart();
-    //  Thread.sleep(10000000);
-    //} catch (Exception ign) {}
-
-    assertEquals(0.0, filter.getXhat(0), 0.2);
-    assertEquals(0.0, filter.getXhat(1), 0.2);
-  }
-
-  @Test
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public void testSwerveKFMovingOverTrajectory() {
-
-    var random = new Random();
-
-    var filter = new KalmanFilter<>(Nat.N6(), Nat.N3(),
-          m_swerveObserverSystem,
-          VecBuilder.fill(0.1, 0.1, 0.1, 0.1, 0.1, 0.1), // state
-          // weights
-          VecBuilder.fill(4, 4, 4), // measurement weights
-          0.020
-    );
-
-    List<Double> xhatsX = new ArrayList<>();
-    List<Double> measurementsX = new ArrayList<>();
-    List<Double> xhatsY = new ArrayList<>();
-    List<Double> measurementsY = new ArrayList<>();
-
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-          List.of(
-                new Pose2d(0, 0, new Rotation2d()),
-                new Pose2d(5, 5, new Rotation2d())
-          ), new TrajectoryConfig(2, 2)
-    );
-    var time = 0.0;
-    var lastVelocity = VecBuilder.fill(0.0, 0.0, 0.0);
-
-    while (time <= trajectory.getTotalTimeSeconds()) {
-      var sample = trajectory.sample(time);
-      var measurement = VecBuilder.fill(
-            sample.poseMeters.getTranslation().getX() + random.nextGaussian() / 5d,
-            sample.poseMeters.getTranslation().getY() + random.nextGaussian() / 5d,
-            sample.poseMeters.getRotation().getRadians() + random.nextGaussian() / 3d
-      );
-
-      var velocity = VecBuilder.fill(
-            sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getCos(),
-            sample.velocityMetersPerSecond * sample.poseMeters.getRotation().getSin(),
-            sample.curvatureRadPerMeter * sample.velocityMetersPerSecond
-      );
-      var u = (velocity.minus(lastVelocity)).div(0.020);
-      lastVelocity = velocity;
-
-      filter.correct(u, measurement);
-      filter.predict(u, 0.020);
-
-      measurementsX.add(measurement.get(0, 0));
-      measurementsY.add(measurement.get(1, 0));
-      xhatsX.add(filter.getXhat(0));
-      xhatsY.add(filter.getXhat(1));
-
-      time += 0.020;
-    }
-
-    //var chart = new XYChartBuilder().build();
-    //chart.addSeries("Xhat, x/y", xhatsX, xhatsY);
-    //chart.addSeries("Measured position, x/y", measurementsX, measurementsY);
-    //try {
-    //        new SwingWrapper<>(chart).displayChart();
-    //        Thread.sleep(10000000);
-    //} catch (Exception ign) {
-    //}
-
-    assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
-          .getTranslation().getX(), filter.getXhat(0), 0.2);
-    assertEquals(trajectory.sample(trajectory.getTotalTimeSeconds()).poseMeters
-          .getTranslation().getY(), filter.getXhat(1), 0.2);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java
deleted file mode 100644
index 529a3c5..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/MerweScaledSigmaPointsTest.java
+++ /dev/null
@@ -1,42 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class MerweScaledSigmaPointsTest {
-  @Test
-  public void testZeroMeanPoints() {
-    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
-    var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(0, 0),
-          Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 1));
-
-    assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill(
-            0.0, 0.00173205, 0.0, -0.00173205, 0.0,
-            0.0, 0.0, 0.00173205, 0.0, -0.00173205
-    ), 1E-6));
-  }
-
-  @Test
-  public void testNonzeroMeanPoints() {
-    var merweScaledSigmaPoints = new MerweScaledSigmaPoints<>(Nat.N2());
-    var points = merweScaledSigmaPoints.sigmaPoints(VecBuilder.fill(1, 2),
-          Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 10));
-
-    assertTrue(points.isEqual(Matrix.mat(Nat.N2(), Nat.N5()).fill(
-            1.0, 1.00173205, 1.0, 0.99826795, 1.0,
-            2.0, 2.0, 2.00547723, 2.0, 1.99452277
-    ), 1E-6));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
deleted file mode 100644
index c4340ae..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/estimator/UnscentedKalmanFilterTest.java
+++ /dev/null
@@ -1,396 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.estimator;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.math.Discretization;
-import edu.wpi.first.wpilibj.math.StateSpaceUtil;
-import edu.wpi.first.wpilibj.system.NumericalJacobian;
-import edu.wpi.first.wpilibj.system.RungeKutta;
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
-import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N4;
-import edu.wpi.first.wpiutil.math.numbers.N6;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-
-public class UnscentedKalmanFilterTest {
-  @SuppressWarnings({"LocalVariableName", "ParameterName"})
-  public static Matrix<N6, N1> getDynamics(Matrix<N6, N1> x, Matrix<N2, N1> u) {
-    var motors = DCMotor.getCIM(2);
-
-    var gHigh = 7.08;
-    var rb = 0.8382 / 2.0;
-    var r = 0.0746125;
-    var m = 63.503;
-    var J = 5.6;
-
-    var C1 = -Math.pow(gHigh, 2) * motors.m_KtNMPerAmp
-          / (motors.m_KvRadPerSecPerVolt * motors.m_rOhms * r * r);
-    var C2 = gHigh * motors.m_KtNMPerAmp / (motors.m_rOhms * r);
-
-    var c = x.get(2, 0);
-    var s = x.get(3, 0);
-    var vl = x.get(4, 0);
-    var vr = x.get(5, 0);
-
-    var Vl = u.get(0, 0);
-    var Vr = u.get(1, 0);
-
-    var k1 = 1.0 / m + rb * rb / J;
-    var k2 = 1.0 / m - rb * rb / J;
-
-    var xvel = (vl + vr) / 2;
-    var w = (vr - vl) / (2.0 * rb);
-
-    return VecBuilder.fill(
-          xvel * c,
-          xvel * s,
-          -s * w,
-          c * w,
-          k1 * ((C1 * vl) + (C2 * Vl)) + k2 * ((C1 * vr) + (C2 * Vr)),
-          k2 * ((C1 * vl) + (C2 * Vl)) + k1 * ((C1 * vr) + (C2 * Vr))
-    );
-  }
-
-  @SuppressWarnings("ParameterName")
-  public static Matrix<N4, N1> getLocalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
-    return VecBuilder.fill(x.get(2, 0), x.get(3, 0), x.get(4, 0), x.get(5, 0));
-  }
-
-  @SuppressWarnings("ParameterName")
-  public static Matrix<N6, N1> getGlobalMeasurementModel(Matrix<N6, N1> x, Matrix<N2, N1> u) {
-    return x.copy();
-  }
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  public void testInit() {
-    assertDoesNotThrow(() -> {
-      UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
-            Nat.N6(), Nat.N4(),
-            UnscentedKalmanFilterTest::getDynamics,
-            UnscentedKalmanFilterTest::getLocalMeasurementModel,
-            VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
-            VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
-            0.00505);
-
-      var u = VecBuilder.fill(12.0, 12.0);
-      observer.predict(u, 0.00505);
-
-      var localY = getLocalMeasurementModel(observer.getXhat(), u);
-      observer.correct(u, localY);
-    });
-  }
-
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops",
-        "PMD.ExcessiveMethodLength"})
-  @Test
-  public void testConvergence() {
-    double dtSeconds = 0.00505;
-    double rbMeters = 0.8382 / 2.0; // Robot radius
-
-    List<Double> trajXs = new ArrayList<>();
-    List<Double> trajYs = new ArrayList<>();
-
-    List<Double> observerXs = new ArrayList<>();
-    List<Double> observerYs = new ArrayList<>();
-    List<Double> observerC = new ArrayList<>();
-    List<Double> observerS = new ArrayList<>();
-    List<Double> observervl = new ArrayList<>();
-    List<Double> observervr = new ArrayList<>();
-
-    List<Double> inputVl = new ArrayList<>();
-    List<Double> inputVr = new ArrayList<>();
-
-    List<Double> timeData = new ArrayList<>();
-    List<Matrix<?, ?>> rdots = new ArrayList<>();
-
-    UnscentedKalmanFilter<N6, N2, N4> observer = new UnscentedKalmanFilter<>(
-          Nat.N6(), Nat.N4(),
-          UnscentedKalmanFilterTest::getDynamics,
-          UnscentedKalmanFilterTest::getLocalMeasurementModel,
-          VecBuilder.fill(0.5, 0.5, 0.7, 0.7, 1.0, 1.0),
-          VecBuilder.fill(0.001, 0.001, 0.5, 0.5),
-          dtSeconds);
-
-    List<Pose2d> waypoints = Arrays.asList(new Pose2d(2.75, 22.521, new Rotation2d()),
-          new Pose2d(24.73, 19.68, Rotation2d.fromDegrees(5.846)));
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-          waypoints,
-          new TrajectoryConfig(8.8, 0.1)
-    );
-
-    Matrix<N6, N1> nextR;
-    Matrix<N2, N1> u = new Matrix<>(Nat.N2(), Nat.N1());
-
-    var B = NumericalJacobian.numericalJacobianU(Nat.N6(), Nat.N2(),
-          UnscentedKalmanFilterTest::getDynamics, new Matrix<>(Nat.N6(), Nat.N1()), u);
-
-    observer.setXhat(VecBuilder.fill(2.75, 22.521, 1.0, 0.0, 0.0, 0.0)); // TODO not hard code this
-
-    var ref = trajectory.sample(0.0);
-
-    Matrix<N6, N1> r = VecBuilder.fill(
-          ref.poseMeters.getTranslation().getX(),
-          ref.poseMeters.getTranslation().getY(),
-          ref.poseMeters.getRotation().getCos(),
-          ref.poseMeters.getRotation().getSin(),
-          ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters)),
-          ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters))
-    );
-    nextR = r.copy();
-
-    var trueXhat = observer.getXhat();
-
-    double totalTime = trajectory.getTotalTimeSeconds();
-    for (int i = 0; i < (totalTime / dtSeconds); i++) {
-
-      ref = trajectory.sample(dtSeconds * i);
-      double vl = ref.velocityMetersPerSecond * (1 - (ref.curvatureRadPerMeter * rbMeters));
-      double vr = ref.velocityMetersPerSecond * (1 + (ref.curvatureRadPerMeter * rbMeters));
-
-      nextR.set(0, 0, ref.poseMeters.getTranslation().getX());
-      nextR.set(1, 0, ref.poseMeters.getTranslation().getY());
-      nextR.set(2, 0, ref.poseMeters.getRotation().getCos());
-      nextR.set(3, 0, ref.poseMeters.getRotation().getSin());
-      nextR.set(4, 0, vl);
-      nextR.set(5, 0, vr);
-
-      Matrix<N4, N1> localY =
-            getLocalMeasurementModel(trueXhat, new Matrix<>(Nat.N2(), Nat.N1()));
-      var noiseStdDev = VecBuilder.fill(0.001, 0.001, 0.5, 0.5);
-
-      observer.correct(u,
-            localY.plus(StateSpaceUtil.makeWhiteNoiseVector(
-                  noiseStdDev)));
-
-      var rdot = nextR.minus(r).div(dtSeconds);
-      u = new Matrix<>(B.solve(rdot.minus(getDynamics(r, new Matrix<>(Nat.N2(), Nat.N1())))));
-
-      rdots.add(rdot);
-
-      trajXs.add(ref.poseMeters.getTranslation().getX());
-      trajYs.add(ref.poseMeters.getTranslation().getY());
-
-      observerXs.add(observer.getXhat().get(0, 0));
-      observerYs.add(observer.getXhat().get(1, 0));
-
-      observerC.add(observer.getXhat(2));
-      observerS.add(observer.getXhat(3));
-
-      observervl.add(observer.getXhat(4));
-      observervr.add(observer.getXhat(5));
-
-      inputVl.add(u.get(0, 0));
-      inputVr.add(u.get(1, 0));
-
-      timeData.add(i * dtSeconds);
-
-      r = nextR;
-      observer.predict(u, dtSeconds);
-      trueXhat = RungeKutta.rungeKutta(UnscentedKalmanFilterTest::getDynamics,
-            trueXhat, u, dtSeconds);
-    }
-
-    var localY = getLocalMeasurementModel(trueXhat, u);
-    observer.correct(u, localY);
-
-    var globalY = getGlobalMeasurementModel(trueXhat, u);
-    var R = StateSpaceUtil.makeCostMatrix(
-          VecBuilder.fill(0.01, 0.01, 0.0001, 0.0001, 0.5, 0.5));
-    observer.correct(Nat.N6(), u, globalY,
-          UnscentedKalmanFilterTest::getGlobalMeasurementModel, R);
-
-    final var finalPosition = trajectory.sample(trajectory.getTotalTimeSeconds());
-
-    //     var chartBuilder = new XYChartBuilder();
-    //     chartBuilder.title = "The Magic of Sensor Fusion, now with a "
-    //           + observer.getClass().getSimpleName();
-    //     var xyPosChart = chartBuilder.build();
-
-    //     xyPosChart.setXAxisTitle("X pos, meters");
-    //     xyPosChart.setYAxisTitle("Y pos, meters");
-    //     xyPosChart.addSeries("Trajectory", trajXs, trajYs);
-    //     xyPosChart.addSeries("xHat", observerXs, observerYs);
-
-    //     var stateChart = new XYChartBuilder()
-    //           .title("States (x-hat)").build();
-    //     stateChart.addSeries("Cos", timeData, observerC);
-    //     stateChart.addSeries("Sin", timeData, observerS);
-    //     stateChart.addSeries("vl, m/s", timeData, observervl);
-    //     stateChart.addSeries("vr, m/s", timeData, observervr);
-
-    //     var inputChart = new XYChartBuilder().title("Inputs").build();
-    //     inputChart.addSeries("Left voltage", timeData, inputVl);
-    //     inputChart.addSeries("Right voltage", timeData, inputVr);
-
-    //     var rdotChart = new XYChartBuilder().title("Rdot").build();
-    //     rdotChart.addSeries("xdot, or vx", timeData, rdots.stream().map(it -> it.get(0, 0))
-    //           .collect(Collectors.toList()));
-    //     rdotChart.addSeries("ydot, or vy", timeData, rdots.stream().map(it -> it.get(1, 0))
-    //           .collect(Collectors.toList()));
-    //     rdotChart.addSeries("cos dot", timeData, rdots.stream().map(it -> it.get(2, 0))
-    //           .collect(Collectors.toList()));
-    //     rdotChart.addSeries("sin dot", timeData, rdots.stream().map(it -> it.get(3, 0))
-    //           .collect(Collectors.toList()));
-    //     rdotChart.addSeries("vl dot, or al", timeData, rdots.stream().map(it -> it.get(4, 0))
-    //           .collect(Collectors.toList()));
-    //     rdotChart.addSeries("vr dot, or ar", timeData, rdots.stream().map(it -> it.get(5, 0))
-    //           .collect(Collectors.toList()));
-
-    //     List<XYChart> charts = new ArrayList<>();
-    //     charts.add(xyPosChart);
-    //     charts.add(stateChart);
-    //     charts.add(inputChart);
-    //     charts.add(rdotChart);
-    //     new SwingWrapper<>(charts).displayChartMatrix();
-    //     try {
-    //       Thread.sleep(1000000000);
-    //     } catch (InterruptedException ex) {
-    //       ex.printStackTrace();
-    //     }
-
-    assertEquals(finalPosition.poseMeters.getTranslation().getX(), observer.getXhat(0), 0.25);
-    assertEquals(finalPosition.poseMeters.getTranslation().getY(), observer.getXhat(1), 0.25);
-    assertEquals(finalPosition.poseMeters.getRotation().getRadians(), observer.getXhat(2), 1.0);
-    assertEquals(0.0, observer.getXhat(3), 1.0);
-    assertEquals(0.0, observer.getXhat(4), 1.0);
-  }
-
-  @Test
-  @SuppressWarnings({"LocalVariableName", "ParameterName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  public void testLinearUKF() {
-    var dt = 0.020;
-    var plant = LinearSystemId.identifyVelocitySystem(0.02, 0.006);
-    var observer = new UnscentedKalmanFilter<>(Nat.N1(), Nat.N1(),
-        (x, u) -> plant.getA().times(x).plus(plant.getB().times(u)),
-          plant::calculateY,
-          VecBuilder.fill(0.05),
-          VecBuilder.fill(1.0),
-          dt);
-
-    var time = new ArrayList<Double>();
-    var refData = new ArrayList<Double>();
-    var xhat = new ArrayList<Double>();
-    var udata = new ArrayList<Double>();
-    var xdotData = new ArrayList<Double>();
-
-    var discABPair = Discretization.discretizeAB(plant.getA(), plant.getB(), dt);
-    var discA = discABPair.getFirst();
-    var discB = discABPair.getSecond();
-
-    Matrix<N1, N1> ref = VecBuilder.fill(100);
-    Matrix<N1, N1> u = VecBuilder.fill(0);
-
-    Matrix<N1, N1> xdot;
-    for (int i = 0; i < (2.0 / dt); i++) {
-      observer.predict(u, dt);
-
-      u = discB.solve(ref.minus(discA.times(ref)));
-
-      xdot = plant.getA().times(observer.getXhat()).plus(plant.getB().times(u));
-
-      time.add(i * dt);
-      refData.add(ref.get(0, 0));
-      xhat.add(observer.getXhat(0));
-      udata.add(u.get(0, 0));
-      xdotData.add(xdot.get(0, 0));
-    }
-
-    //    var chartBuilder = new XYChartBuilder();
-    //    chartBuilder.title = "The Magic of Sensor Fusion";
-    //    var chart = chartBuilder.build();
-
-    //    chart.addSeries("Ref", time, refData);
-    //    chart.addSeries("xHat", time, xhat);
-    //    chart.addSeries("input", time, udata);
-    ////    chart.addSeries("xdot", time, xdotData);
-
-    //    new SwingWrapper<>(chart).displayChart();
-    //    try {
-    //      Thread.sleep(1000000000);
-    //    } catch (InterruptedException e) {
-    //    }
-
-    assertEquals(ref.get(0, 0), observer.getXhat(0), 5);
-  }
-
-  @Test
-  public void testUnscentedTransform() {
-    // From FilterPy
-    var ret = UnscentedKalmanFilter.unscentedTransform(Nat.N4(), Nat.N4(),
-          Matrix.mat(Nat.N4(), Nat.N9()).fill(
-              -0.9, -0.822540333075852, -0.8922540333075852, -0.9,
-                  -0.9, -0.9774596669241481, -0.9077459666924148, -0.9, -0.9,
-              1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519, 1.0, 1.0,
-              -0.9, -0.9, -0.9, -0.822540333075852, -0.8922540333075852, -0.9,
-                  -0.9, -0.9774596669241481, -0.9077459666924148,
-              1.0, 1.0, 1.0, 1.0, 1.077459666924148, 1.0, 1.0, 1.0, 0.9225403330758519
-          ),
-          VecBuilder.fill(
-              -132.33333333,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667
-          ),
-          VecBuilder.fill(
-              -129.34333333,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667,
-              16.66666667
-          )
-    );
-
-    assertTrue(
-          VecBuilder.fill(-0.9, 1, -0.9, 1).isEqual(
-          ret.getFirst(), 1E-5
-    ));
-
-    assertTrue(
-            Matrix.mat(Nat.N4(), Nat.N4()).fill(
-                  2.02000002e-01, 2.00000500e-02, -2.69044710e-29,
-                  -4.59511477e-29,
-                  2.00000500e-02, 2.00001000e-01, -2.98781068e-29,
-                  -5.12759588e-29,
-                  -2.73372625e-29, -3.09882635e-29, 2.02000002e-01,
-                  2.00000500e-02,
-                  -4.67065917e-29, -5.10705197e-29, 2.00000500e-02,
-                  2.00001000e-01
-            ).isEqual(
-            ret.getSecond(), 1E-5
-    ));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
deleted file mode 100644
index 14bad1f..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Pose2dTest.java
+++ /dev/null
@@ -1,75 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Pose2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testTransformBy() {
-    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
-    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
-        Rotation2d.fromDegrees(5.0));
-
-    var transformed = initial.plus(transformation);
-
-    assertAll(
-        () -> assertEquals(transformed.getX(), 1 + 5.0 / Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transformed.getY(), 2 + 5.0 / Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transformed.getRotation().getDegrees(), 50.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRelativeTo() {
-    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
-    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
-
-    var finalRelativeToInitial = last.relativeTo(initial);
-
-    assertAll(
-        () -> assertEquals(finalRelativeToInitial.getX(), 5.0 * Math.sqrt(2.0),
-            kEpsilon),
-        () -> assertEquals(finalRelativeToInitial.getY(), 0.0, kEpsilon),
-        () -> assertEquals(finalRelativeToInitial.getRotation().getDegrees(), 0.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testEquality() {
-    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
-    var two = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = new Pose2d(0.0, 5.0, Rotation2d.fromDegrees(43.0));
-    var two = new Pose2d(0.0, 1.524, Rotation2d.fromDegrees(43.0));
-    assertNotEquals(one, two);
-  }
-
-  void testMinus() {
-    var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
-    var last = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(45.0));
-
-    final var transform = last.minus(initial);
-
-    assertAll(
-        () -> assertEquals(transform.getX(), 5.0 * Math.sqrt(2.0), kEpsilon),
-        () -> assertEquals(transform.getY(), 0.0, kEpsilon),
-        () -> assertEquals(transform.getRotation().getDegrees(), 0.0, kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
deleted file mode 100644
index 8a08944..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Rotation2dTest.java
+++ /dev/null
@@ -1,81 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Rotation2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testRadiansToDegrees() {
-    var one = new Rotation2d(Math.PI / 3);
-    var two = new Rotation2d(Math.PI / 4);
-
-    assertAll(
-        () -> assertEquals(one.getDegrees(), 60.0, kEpsilon),
-        () -> assertEquals(two.getDegrees(), 45.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRadiansAndDegrees() {
-    var one = Rotation2d.fromDegrees(45.0);
-    var two = Rotation2d.fromDegrees(30.0);
-
-    assertAll(
-        () -> assertEquals(one.getRadians(), Math.PI / 4, kEpsilon),
-        () -> assertEquals(two.getRadians(), Math.PI / 6, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRotateByFromZero() {
-    var zero = new Rotation2d();
-    var rotated = zero.rotateBy(Rotation2d.fromDegrees(90.0));
-
-    assertAll(
-        () -> assertEquals(rotated.getRadians(), Math.PI / 2.0, kEpsilon),
-        () -> assertEquals(rotated.getDegrees(), 90.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRotateByNonZero() {
-    var rot = Rotation2d.fromDegrees(90.0);
-    rot = rot.plus(Rotation2d.fromDegrees(30.0));
-
-    assertEquals(rot.getDegrees(), 120.0, kEpsilon);
-  }
-
-  @Test
-  void testMinus() {
-    var one = Rotation2d.fromDegrees(70.0);
-    var two = Rotation2d.fromDegrees(30.0);
-
-    assertEquals(one.minus(two).getDegrees(), 40.0, kEpsilon);
-  }
-
-  @Test
-  void testEquality() {
-    var one = Rotation2d.fromDegrees(43.0);
-    var two = Rotation2d.fromDegrees(43.0);
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = Rotation2d.fromDegrees(43.0);
-    var two = Rotation2d.fromDegrees(43.5);
-    assertNotEquals(one, two);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java
deleted file mode 100644
index c375d22..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class Transform2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testInverse() {
-    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
-    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
-        Rotation2d.fromDegrees(5.0));
-
-    var transformed = initial.plus(transformation);
-    var untransformed = transformed.plus(transformation.inverse());
-
-    assertAll(
-        () -> assertEquals(initial.getX(), untransformed.getX(),
-                           kEpsilon),
-        () -> assertEquals(initial.getY(), untransformed.getY(),
-                           kEpsilon),
-        () -> assertEquals(initial.getRotation().getDegrees(),
-                           untransformed.getRotation().getDegrees(), kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
deleted file mode 100644
index d6844a5..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Translation2dTest.java
+++ /dev/null
@@ -1,127 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Translation2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testSum() {
-    var one = new Translation2d(1.0, 3.0);
-    var two = new Translation2d(2.0, 5.0);
-
-    var sum = one.plus(two);
-
-    assertAll(
-        () -> assertEquals(sum.getX(), 3.0, kEpsilon),
-        () -> assertEquals(sum.getY(), 8.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testDifference() {
-    var one = new Translation2d(1.0, 3.0);
-    var two = new Translation2d(2.0, 5.0);
-
-    var difference = one.minus(two);
-
-    assertAll(
-        () -> assertEquals(difference.getX(), -1.0, kEpsilon),
-        () -> assertEquals(difference.getY(), -2.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testRotateBy() {
-    var another = new Translation2d(3.0, 0.0);
-    var rotated = another.rotateBy(Rotation2d.fromDegrees(90.0));
-
-    assertAll(
-        () -> assertEquals(rotated.getX(), 0.0, kEpsilon),
-        () -> assertEquals(rotated.getY(), 3.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testMultiplication() {
-    var original = new Translation2d(3.0, 5.0);
-    var mult = original.times(3);
-
-    assertAll(
-        () -> assertEquals(mult.getX(), 9.0, kEpsilon),
-        () -> assertEquals(mult.getY(), 15.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testDivision() {
-    var original = new Translation2d(3.0, 5.0);
-    var div = original.div(2);
-
-    assertAll(
-        () -> assertEquals(div.getX(), 1.5, kEpsilon),
-        () -> assertEquals(div.getY(), 2.5, kEpsilon)
-    );
-  }
-
-  @Test
-  void testNorm() {
-    var one = new Translation2d(3.0, 5.0);
-    assertEquals(one.getNorm(), Math.hypot(3.0, 5.0), kEpsilon);
-  }
-
-  @Test
-  void testDistance() {
-    var one = new Translation2d(1, 1);
-    var two = new Translation2d(6, 6);
-    assertEquals(one.getDistance(two), 5 * Math.sqrt(2), kEpsilon);
-  }
-
-  @Test
-  void testUnaryMinus() {
-    var original = new Translation2d(-4.5, 7);
-    var inverted = original.unaryMinus();
-
-    assertAll(
-        () -> assertEquals(inverted.getX(), 4.5, kEpsilon),
-        () -> assertEquals(inverted.getY(), -7, kEpsilon)
-    );
-  }
-
-  @Test
-  void testEquality() {
-    var one = new Translation2d(9, 5.5);
-    var two = new Translation2d(9, 5.5);
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = new Translation2d(9, 5.5);
-    var two = new Translation2d(9, 5.7);
-    assertNotEquals(one, two);
-  }
-
-  @Test
-  void testPolarConstructor() {
-    var one = new Translation2d(Math.sqrt(2), Rotation2d.fromDegrees(45.0));
-    var two = new Translation2d(2, Rotation2d.fromDegrees(60.0));
-    assertAll(
-        () -> assertEquals(one.getX(), 1.0, kEpsilon),
-        () -> assertEquals(one.getY(), 1.0, kEpsilon),
-        () -> assertEquals(two.getX(), 1.0, kEpsilon),
-        () -> assertEquals(two.getY(), Math.sqrt(3), kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
deleted file mode 100644
index 18ea6d9..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/geometry/Twist2dTest.java
+++ /dev/null
@@ -1,81 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.geometry;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-
-class Twist2dTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testStraightLineTwist() {
-    var straight = new Twist2d(5.0, 0.0, 0.0);
-    var straightPose = new Pose2d().exp(straight);
-
-    assertAll(
-        () -> assertEquals(straightPose.getX(), 5.0, kEpsilon),
-        () -> assertEquals(straightPose.getY(), 0.0, kEpsilon),
-        () -> assertEquals(straightPose.getRotation().getRadians(), 0.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testQuarterCirleTwist() {
-    var quarterCircle = new Twist2d(5.0 / 2.0 * Math.PI, 0, Math.PI / 2.0);
-    var quarterCirclePose = new Pose2d().exp(quarterCircle);
-
-    assertAll(
-        () -> assertEquals(quarterCirclePose.getX(), 5.0, kEpsilon),
-        () -> assertEquals(quarterCirclePose.getY(), 5.0, kEpsilon),
-        () -> assertEquals(quarterCirclePose.getRotation().getDegrees(), 90.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testDiagonalNoDtheta() {
-    var diagonal = new Twist2d(2.0, 2.0, 0.0);
-    var diagonalPose = new Pose2d().exp(diagonal);
-
-    assertAll(
-        () -> assertEquals(diagonalPose.getX(), 2.0, kEpsilon),
-        () -> assertEquals(diagonalPose.getY(), 2.0, kEpsilon),
-        () -> assertEquals(diagonalPose.getRotation().getDegrees(), 0.0, kEpsilon)
-    );
-  }
-
-  @Test
-  void testEquality() {
-    var one = new Twist2d(5, 1, 3);
-    var two = new Twist2d(5, 1, 3);
-    assertEquals(one, two);
-  }
-
-  @Test
-  void testInequality() {
-    var one = new Twist2d(5, 1, 3);
-    var two = new Twist2d(5, 1.2, 3);
-    assertNotEquals(one, two);
-  }
-
-  void testPose2dLog() {
-    final var start = new Pose2d();
-    final var end = new Pose2d(5.0, 5.0, Rotation2d.fromDegrees(90.0));
-
-    final var twist = start.log(end);
-
-    assertAll(
-        () -> assertEquals(twist.dx, 5.0 / 2.0 * Math.PI, kEpsilon),
-        () -> assertEquals(twist.dy, 0.0, kEpsilon),
-        () -> assertEquals(twist.dtheta, Math.PI / 2.0, kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
deleted file mode 100644
index b06abe6..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/ChassisSpeedsTest.java
+++ /dev/null
@@ -1,32 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class ChassisSpeedsTest {
-  private static final double kEpsilon = 1E-9;
-
-  @Test
-  void testFieldRelativeConstruction() {
-    final var chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
-        1.0, 0.0, 0.5, Rotation2d.fromDegrees(-90.0)
-    );
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(1.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.5, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
deleted file mode 100644
index 9d2ad4e..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveKinematicsTest.java
+++ /dev/null
@@ -1,88 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class DifferentialDriveKinematicsTest {
-  private static final double kEpsilon = 1E-9;
-  private final DifferentialDriveKinematics m_kinematics
-      = new DifferentialDriveKinematics(0.381 * 2);
-
-  @Test
-  void testInverseKinematicsForZeros() {
-    var chassisSpeeds = new ChassisSpeeds();
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-
-    assertAll(
-        () -> assertEquals(0.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testForwardKinematicsForZeros() {
-    var wheelSpeeds = new DifferentialDriveWheelSpeeds();
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testInverseKinematicsForStraightLine() {
-    var chassisSpeeds = new ChassisSpeeds(3, 0, 0);
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-
-    assertAll(
-        () -> assertEquals(3.0, wheelSpeeds.leftMetersPerSecond, kEpsilon),
-        () -> assertEquals(3.0, wheelSpeeds.rightMetersPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testForwardKinematicsForStraightLine() {
-    var wheelSpeeds = new DifferentialDriveWheelSpeeds(3, 3);
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(3.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testInverseKinematicsForRotateInPlace() {
-    var chassisSpeeds = new ChassisSpeeds(0, 0, Math.PI);
-    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
-
-    assertAll(
-        () -> assertEquals(-0.381 * Math.PI, wheelSpeeds.leftMetersPerSecond, kEpsilon),
-        () -> assertEquals(+0.381 * Math.PI, wheelSpeeds.rightMetersPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testForwardKinematicsForRotateInPlace() {
-    var wheelSpeeds = new DifferentialDriveWheelSpeeds(+0.381 * Math.PI, -0.381 * Math.PI);
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(-Math.PI, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
deleted file mode 100644
index e6022de..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/DifferentialDriveOdometryTest.java
+++ /dev/null
@@ -1,34 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class DifferentialDriveOdometryTest {
-  private static final double kEpsilon = 1E-9;
-  private final DifferentialDriveOdometry m_odometry = new DifferentialDriveOdometry(
-      new Rotation2d());
-
-  @Test
-  void testOdometryWithEncoderDistances() {
-    m_odometry.resetPosition(new Pose2d(), Rotation2d.fromDegrees(45));
-    var pose = m_odometry.update(Rotation2d.fromDegrees(135.0), 0.0, 5 * Math.PI);
-
-    assertAll(
-        () -> assertEquals(pose.getX(), 5.0, kEpsilon),
-        () -> assertEquals(pose.getY(), 5.0, kEpsilon),
-        () -> assertEquals(pose.getRotation().getDegrees(), 90.0, kEpsilon)
-    );
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
deleted file mode 100644
index 93c8d6a..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveKinematicsTest.java
+++ /dev/null
@@ -1,262 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class MecanumDriveKinematicsTest {
-  private static final double kEpsilon = 1E-9;
-
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-  private final MecanumDriveKinematics m_kinematics =
-      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  @Test
-  void testStraightLineInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
-    var moduleStates = m_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
-      */
-
-    assertAll(
-        () -> assertEquals(3.536, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(3.536, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(3.536, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(3.536, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testStraightLineForwardKinematicsKinematics() {
-
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-    var moduleStates = m_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]]
-      */
-
-    assertAll(
-        () -> assertEquals(5, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testStrafeInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 4, 0);
-    var moduleStates = m_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
-      */
-
-    assertAll(
-        () -> assertEquals(-2.828427, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(2.828427, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(2.828427, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(-2.828427, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testStrafeForwardKinematicsKinematics() {
-
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(-2.828427, 2.828427, 2.828427, -2.828427);
-    var moduleStates = m_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]]
-      */
-
-    assertAll(
-        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(4, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
-    var moduleStates = m_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
-      */
-
-    assertAll(
-        () -> assertEquals(-106.629191, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(106.629191, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-106.629191, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(106.629191, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testRotationForwardKinematicsKinematics() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(-106.629191, 106.629191, -106.629191, 106.629191);
-    var moduleStates = m_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]]
-      */
-
-    assertAll(
-        () -> assertEquals(0, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(0, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(2 * Math.PI, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testMixedTranslationRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(2, 3, 1);
-    var moduleStates = m_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
-      */
-
-    assertAll(
-        () -> assertEquals(-17.677670, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(20.506097, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-13.435, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(16.26, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testMixedTranslationRotationForwardKinematicsKinematics() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(-17.677670, 20.51, -13.44, 16.26);
-    var moduleStates = m_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]]
-      */
-
-    assertAll(
-        () -> assertEquals(2, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(3, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 1);
-    var moduleStates = m_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
-      */
-
-    assertAll(
-        () -> assertEquals(0, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(16.971, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-16.971, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(33.941, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterRotationForwardKinematicsKinematics() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(0, 16.971, -16.971, 33.941);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
-    velocities should be [[12][-12][1]]
-      */
-
-    assertAll(
-        () -> assertEquals(12, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-12, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterTranslationRotationInverseKinematics() {
-    ChassisSpeeds speeds = new ChassisSpeeds(5, 2, 1);
-    var moduleStates = m_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
-      */
-
-    assertAll(
-        () -> assertEquals(2.12, moduleStates.frontLeftMetersPerSecond, 0.1),
-        () -> assertEquals(21.92, moduleStates.frontRightMetersPerSecond, 0.1),
-        () -> assertEquals(-12.02, moduleStates.rearLeftMetersPerSecond, 0.1),
-        () -> assertEquals(36.06, moduleStates.rearRightMetersPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterRotationTranslationForwardKinematicsKinematics() {
-
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(2.12, 21.92, -12.02, 36.06);
-    var moduleStates = m_kinematics.toChassisSpeeds(wheelSpeeds);
-
-    /*
-    By equation (13.13) of the state-space-guide, the chassis motion from the wheel
-    velocities should be [[17][-10][1]]
-      */
-
-    assertAll(
-        () -> assertEquals(17, moduleStates.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-10, moduleStates.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1, moduleStates.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testNormalize() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(5, 6, 4, 7);
-    wheelSpeeds.normalize(5.5);
-
-    double factor = 5.5 / 7.0;
-
-    assertAll(
-        () -> assertEquals(5.0 * factor, wheelSpeeds.frontLeftMetersPerSecond, kEpsilon),
-        () -> assertEquals(6.0 * factor, wheelSpeeds.frontRightMetersPerSecond, kEpsilon),
-        () -> assertEquals(4.0 * factor, wheelSpeeds.rearLeftMetersPerSecond, kEpsilon),
-        () -> assertEquals(7.0 * factor, wheelSpeeds.rearRightMetersPerSecond, kEpsilon)
-    );
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
deleted file mode 100644
index 5ece28b..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/MecanumDriveOdometryTest.java
+++ /dev/null
@@ -1,94 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class MecanumDriveOdometryTest {
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-
-  private final MecanumDriveKinematics m_kinematics =
-      new MecanumDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  private final MecanumDriveOdometry m_odometry = new MecanumDriveOdometry(m_kinematics,
-      new Rotation2d());
-
-  @Test
-  void testMultipleConsecutiveUpdates() {
-    var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
-    var secondPose = m_odometry.updateWithTime(0.0, new Rotation2d(), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(secondPose.getX(), 0.0, 0.01),
-        () -> assertEquals(secondPose.getY(), 0.0, 0.01),
-        () -> assertEquals(secondPose.getRotation().getDegrees(), 0.0, 0.01)
-    );
-  }
-
-  @Test
-  void testTwoIterations() {
-    // 5 units/sec  in the x axis (forward)
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(3.536, 3.536, 3.536, 3.536);
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
-        () -> assertEquals(0, pose.getY(), 0.01),
-        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void test90degreeTurn() {
-    // This is a 90 degree turn about the point between front left and rear left wheels
-    // fl -13.328649 fr 39.985946 rl -13.328649 rr 39.985946
-    final var wheelSpeeds = new MecanumDriveWheelSpeeds(-13.328, 39.986, -13.329, 39.986);
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), new MecanumDriveWheelSpeeds());
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(12.0, pose.getX(), 0.01),
-        () -> assertEquals(12.0, pose.getY(), 0.01),
-        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void testGyroAngleReset() {
-    var gyro = Rotation2d.fromDegrees(90.0);
-    var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var speeds = new MecanumDriveWheelSpeeds(3.536, 3.536,
-        3.536, 3.536);
-    m_odometry.updateWithTime(0.0, gyro, new MecanumDriveWheelSpeeds());
-    var pose = m_odometry.updateWithTime(1.0, gyro, speeds);
-
-    assertAll(
-        () -> assertEquals(5.0, pose.getX(), 0.1),
-        () -> assertEquals(0.00, pose.getY(), 0.1),
-        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
-    );
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
deleted file mode 100644
index e9fbcd1..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveKinematicsTest.java
+++ /dev/null
@@ -1,262 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class SwerveDriveKinematicsTest {
-  private static final double kEpsilon = 1E-9;
-
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-  private final SwerveDriveKinematics m_kinematics =
-      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  @Test
-  void testStraightLineInverseKinematics() { // test inverse kinematics going in a straight line
-
-    ChassisSpeeds speeds = new ChassisSpeeds(5, 0, 0);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    assertAll(
-        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, moduleStates[0].angle.getRadians(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[1].angle.getRadians(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[2].angle.getRadians(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[3].angle.getRadians(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testStraightLineForwardKinematics() { // test forward kinematics going in a straight line
-    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testStraightStrafeInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 5, 0);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    assertAll(
-        () -> assertEquals(5.0, moduleStates[0].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[1].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[2].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, moduleStates[3].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(90.0, moduleStates[0].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(90.0, moduleStates[1].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(90.0, moduleStates[3].angle.getDegrees(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testStraightStrafeForwardKinematics() {
-    SwerveModuleState state = new SwerveModuleState(5.0, Rotation2d.fromDegrees(90.0));
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(state, state, state, state);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(5.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.omegaRadiansPerSecond, kEpsilon)
-    );
-  }
-
-  @Test
-  void testTurnInPlaceInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds);
-
-    /*
-    The circumference of the wheels about the COR is pi * diameter, or 2 * pi * radius
-    the radius is the sqrt(12^2in + 12^2in), or 16.9706in, so the circumference the wheels
-    trace out is 106.629190516in. since we want our robot to rotate at 1 rotation per second,
-    our wheels must trace out 1 rotation (or 106.63 inches) per second.
-      */
-
-    assertAll(
-        () -> assertEquals(106.63, moduleStates[0].speedMetersPerSecond, 0.1),
-        () -> assertEquals(106.63, moduleStates[1].speedMetersPerSecond, 0.1),
-        () -> assertEquals(106.63, moduleStates[2].speedMetersPerSecond, 0.1),
-        () -> assertEquals(106.63, moduleStates[3].speedMetersPerSecond, 0.1),
-        () -> assertEquals(135.0, moduleStates[0].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(45.0, moduleStates[1].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-135.0, moduleStates[2].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testTurnInPlaceForwardKinematics() {
-    SwerveModuleState flState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(135));
-    SwerveModuleState frState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(45));
-    SwerveModuleState blState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-135));
-    SwerveModuleState brState = new SwerveModuleState(106.629, Rotation2d.fromDegrees(-45));
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, kEpsilon),
-        () -> assertEquals(0.0, chassisSpeeds.vyMetersPerSecond, kEpsilon),
-        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testOffCenterCORRotationInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0, 0, 2 * Math.PI);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, m_fl);
-
-    /*
-    This one is a bit trickier. Because we are rotating about the front-left wheel,
-    it should be parked at 0 degrees and 0 speed. The front-right and back-left wheels both travel
-    an arc with radius 24 (and circumference 150.796), and the back-right wheel travels an arc with
-    radius sqrt(24^2 + 24^2) and circumference 213.2584. As for angles, the front-right wheel
-    should be pointing straight forward, the back-left wheel should be pointing straight right,
-    and the back-right wheel should be at a -45 degree angle
-    */
-
-    assertAll(
-        () -> assertEquals(0.0, moduleStates[0].speedMetersPerSecond, 0.1),
-        () -> assertEquals(150.796, moduleStates[1].speedMetersPerSecond, 0.1),
-        () -> assertEquals(150.796, moduleStates[2].speedMetersPerSecond, 0.1),
-        () -> assertEquals(213.258, moduleStates[3].speedMetersPerSecond, 0.1),
-        () -> assertEquals(0.0, moduleStates[0].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(0.0, moduleStates[1].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-90.0, moduleStates[2].angle.getDegrees(), kEpsilon),
-        () -> assertEquals(-45.0, moduleStates[3].angle.getDegrees(), kEpsilon)
-    );
-  }
-
-  @Test
-  void testOffCenterCORRotationForwardKinematics() {
-    SwerveModuleState flState = new SwerveModuleState(0.0, Rotation2d.fromDegrees(0.0));
-    SwerveModuleState frState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(0.0));
-    SwerveModuleState blState = new SwerveModuleState(150.796, Rotation2d.fromDegrees(-90));
-    SwerveModuleState brState = new SwerveModuleState(213.258, Rotation2d.fromDegrees(-45));
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
-
-    /*
-    We already know that our omega should be 2pi from the previous test. Next, we need to determine
-    the vx and vy of our chassis center. Because our COR is at a 45 degree angle from the center,
-    we know that vx and vy must be the same. Furthermore, we know that the center of mass makes
-    a full revolution about the center of revolution once every second. Therefore, the center of
-    mass must be moving at 106.629in/sec. Recalling that the ratios of a 45/45/90 triagle are
-    1:sqrt(2)/2:sqrt(2)/2, we find that the COM vx is -75.398, and vy is 75.398.
-    */
-
-    assertAll(
-        () -> assertEquals(75.398, chassisSpeeds.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-75.398, chassisSpeeds.vyMetersPerSecond, 0.1),
-        () -> assertEquals(2 * Math.PI, chassisSpeeds.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  private void assertModuleState(SwerveModuleState expected, SwerveModuleState actual,
-                                 SwerveModuleState tolerance) {
-    assertAll(
-        () -> assertEquals(expected.speedMetersPerSecond, actual.speedMetersPerSecond,
-            tolerance.speedMetersPerSecond),
-        () -> assertEquals(expected.angle.getDegrees(), actual.angle.getDegrees(),
-            tolerance.angle.getDegrees())
-    );
-  }
-
-  /**
-   * Test the rotation of the robot about a non-central point with
-   * both linear and angular velocities.
-   */
-  @Test
-  void testOffCenterCORRotationAndTranslationInverseKinematics() {
-
-    ChassisSpeeds speeds = new ChassisSpeeds(0.0, 3.0, 1.5);
-    var moduleStates = m_kinematics.toSwerveModuleStates(speeds, new Translation2d(24, 0));
-
-    // By equation (13.14) from state-space guide, our wheels/angles will be as follows,
-    // (+-1 degree or speed):
-    SwerveModuleState[] expectedStates = new SwerveModuleState[]{
-        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19)),
-        new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81)),
-        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44)),
-        new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56))
-    };
-    var stateTolerance = new SwerveModuleState(0.1, Rotation2d.fromDegrees(0.1));
-
-    for (int i = 0; i < expectedStates.length; i++) {
-      assertModuleState(expectedStates[i], moduleStates[i], stateTolerance);
-    }
-  }
-
-  @Test
-  void testOffCenterCORRotationAndTranslationForwardKinematics() {
-    SwerveModuleState flState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-140.19));
-    SwerveModuleState frState = new SwerveModuleState(23.43, Rotation2d.fromDegrees(-39.81));
-    SwerveModuleState blState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-109.44));
-    SwerveModuleState brState = new SwerveModuleState(54.08, Rotation2d.fromDegrees(-70.56));
-
-    var chassisSpeeds = m_kinematics.toChassisSpeeds(flState, frState, blState, brState);
-
-    /*
-    From equation (13.17), we know that chassis motion is th dot product of the
-    pseudoinverse of the inverseKinematics matrix (with the center of rotation at
-    (0,0) -- we don't want the motion of the center of rotation, we want it of
-    the center of the robot). These above SwerveModuleStates are known to be from
-    a velocity of [[0][3][1.5]] about (0, 24), and the expected numbers have been
-    calculated using Numpy's linalg.pinv function.
-    */
-
-    assertAll(
-        () -> assertEquals(0.0, chassisSpeeds.vxMetersPerSecond, 0.1),
-        () -> assertEquals(-33.0, chassisSpeeds.vyMetersPerSecond, 0.1),
-        () -> assertEquals(1.5, chassisSpeeds.omegaRadiansPerSecond, 0.1)
-    );
-  }
-
-  @Test
-  void testNormalize() {
-    SwerveModuleState fl = new SwerveModuleState(5, new Rotation2d());
-    SwerveModuleState fr = new SwerveModuleState(6, new Rotation2d());
-    SwerveModuleState bl = new SwerveModuleState(4, new Rotation2d());
-    SwerveModuleState br = new SwerveModuleState(7, new Rotation2d());
-
-    SwerveModuleState[] arr = {fl, fr, bl, br};
-    SwerveDriveKinematics.normalizeWheelSpeeds(arr, 5.5);
-
-    double factor = 5.5 / 7.0;
-
-    assertAll(
-        () -> assertEquals(5.0 * factor, arr[0].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(6.0 * factor, arr[1].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(4.0 * factor, arr[2].speedMetersPerSecond, kEpsilon),
-        () -> assertEquals(7.0 * factor, arr[3].speedMetersPerSecond, kEpsilon)
-    );
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
deleted file mode 100644
index f1ee907..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/kinematics/SwerveDriveOdometryTest.java
+++ /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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.kinematics;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class SwerveDriveOdometryTest {
-  private final Translation2d m_fl = new Translation2d(12, 12);
-  private final Translation2d m_fr = new Translation2d(12, -12);
-  private final Translation2d m_bl = new Translation2d(-12, 12);
-  private final Translation2d m_br = new Translation2d(-12, -12);
-
-  private final SwerveDriveKinematics m_kinematics =
-      new SwerveDriveKinematics(m_fl, m_fr, m_bl, m_br);
-
-  private final SwerveDriveOdometry m_odometry = new SwerveDriveOdometry(m_kinematics,
-      new Rotation2d());
-
-  @Test
-  void testTwoIterations() {
-    // 5 units/sec  in the x axis (forward)
-    final SwerveModuleState[] wheelSpeeds = {
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0)),
-        new SwerveModuleState(5, Rotation2d.fromDegrees(0))
-    };
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(),
-        new SwerveModuleState(), new SwerveModuleState(),
-        new SwerveModuleState(), new SwerveModuleState());
-    var pose = m_odometry.updateWithTime(0.10, new Rotation2d(), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(5.0 / 10.0, pose.getX(), 0.01),
-        () -> assertEquals(0, pose.getY(), 0.01),
-        () -> assertEquals(0.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void test90degreeTurn() {
-    // This is a 90 degree turn about the point between front left and rear left wheels
-    //        Module 0: speed 18.84955592153876 angle 90.0
-    //        Module 1: speed 42.14888838624436 angle 26.565051177077986
-    //        Module 2: speed 18.84955592153876 angle -90.0
-    //        Module 3: speed 42.14888838624436 angle -26.565051177077986
-
-    final SwerveModuleState[] wheelSpeeds = {
-        new SwerveModuleState(18.85, Rotation2d.fromDegrees(90.0)),
-        new SwerveModuleState(42.15, Rotation2d.fromDegrees(26.565)),
-        new SwerveModuleState(18.85, Rotation2d.fromDegrees(-90)),
-        new SwerveModuleState(42.15, Rotation2d.fromDegrees(-26.565))
-    };
-    final var zero = new SwerveModuleState();
-
-    m_odometry.updateWithTime(0.0, new Rotation2d(), zero, zero, zero, zero);
-    final var pose = m_odometry.updateWithTime(1.0, Rotation2d.fromDegrees(90.0), wheelSpeeds);
-
-    assertAll(
-        () -> assertEquals(12.0, pose.getX(), 0.01),
-        () -> assertEquals(12.0, pose.getY(), 0.01),
-        () -> assertEquals(90.0, pose.getRotation().getDegrees(), 0.01)
-    );
-  }
-
-  @Test
-  void testGyroAngleReset() {
-    var gyro = Rotation2d.fromDegrees(90.0);
-    var fieldAngle = Rotation2d.fromDegrees(0.0);
-    m_odometry.resetPosition(new Pose2d(new Translation2d(), fieldAngle), gyro);
-    var state = new SwerveModuleState();
-    m_odometry.updateWithTime(0.0, gyro, state, state, state, state);
-    state = new SwerveModuleState(1.0, Rotation2d.fromDegrees(0));
-    var pose = m_odometry.updateWithTime(1.0, gyro, state, state, state, state);
-
-    assertAll(
-        () -> assertEquals(1.0, pose.getX(), 0.1),
-        () -> assertEquals(0.00, pose.getY(), 0.1),
-        () -> assertEquals(0.00, pose.getRotation().getRadians(), 0.1)
-    );
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java
deleted file mode 100644
index 244ca53..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/math/StateSpaceUtilTest.java
+++ /dev/null
@@ -1,199 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.math;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import org.ejml.dense.row.MatrixFeatures_DDRM;
-import org.ejml.simple.SimpleMatrix;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.SimpleMatrixUtils;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class StateSpaceUtilTest {
-  @Test
-  public void testCostArray() {
-    var mat = StateSpaceUtil.makeCostMatrix(
-          VecBuilder.fill(1.0, 2.0, 3.0));
-
-    assertEquals(1.0, mat.get(0, 0), 1e-3);
-    assertEquals(0.0, mat.get(0, 1), 1e-3);
-    assertEquals(0.0, mat.get(0, 2), 1e-3);
-    assertEquals(0.0, mat.get(1, 0), 1e-3);
-    assertEquals(1.0 / 4.0, mat.get(1, 1), 1e-3);
-    assertEquals(0.0, mat.get(1, 2), 1e-3);
-    assertEquals(0.0, mat.get(0, 2), 1e-3);
-    assertEquals(0.0, mat.get(1, 2), 1e-3);
-    assertEquals(1.0 / 9.0, mat.get(2, 2), 1e-3);
-  }
-
-  @Test
-  public void testCovArray() {
-    var mat = StateSpaceUtil.makeCovarianceMatrix(Nat.N3(),
-          VecBuilder.fill(1.0, 2.0, 3.0));
-
-    assertEquals(1.0, mat.get(0, 0), 1e-3);
-    assertEquals(0.0, mat.get(0, 1), 1e-3);
-    assertEquals(0.0, mat.get(0, 2), 1e-3);
-    assertEquals(0.0, mat.get(1, 0), 1e-3);
-    assertEquals(4.0, mat.get(1, 1), 1e-3);
-    assertEquals(0.0, mat.get(1, 2), 1e-3);
-    assertEquals(0.0, mat.get(0, 2), 1e-3);
-    assertEquals(0.0, mat.get(1, 2), 1e-3);
-    assertEquals(9.0, mat.get(2, 2), 1e-3);
-  }
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  public void testIsStabilizable() {
-    Matrix<N2, N2> A;
-    Matrix<N2, N1> B = VecBuilder.fill(0, 1);
-
-    // First eigenvalue is uncontrollable and unstable.
-    // Second eigenvalue is controllable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.2, 0, 0, 0.5);
-    assertFalse(StateSpaceUtil.isStabilizable(A, B));
-
-    // First eigenvalue is uncontrollable and marginally stable.
-    // Second eigenvalue is controllable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 0, 0, 0.5);
-    assertFalse(StateSpaceUtil.isStabilizable(A, B));
-
-    // First eigenvalue is uncontrollable and stable.
-    // Second eigenvalue is controllable and stable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 0.5);
-    assertTrue(StateSpaceUtil.isStabilizable(A, B));
-
-    // First eigenvalue is uncontrollable and stable.
-    // Second eigenvalue is controllable and unstable.
-    A = Matrix.mat(Nat.N2(), Nat.N2()).fill(0.2, 0, 0, 1.2);
-    assertTrue(StateSpaceUtil.isStabilizable(A, B));
-  }
-
-  @Test
-  public void testMakeWhiteNoiseVector() {
-    var firstData = new ArrayList<Double>();
-    var secondData = new ArrayList<Double>();
-    for (int i = 0; i < 1000; i++) {
-      var noiseVec = StateSpaceUtil.makeWhiteNoiseVector(VecBuilder.fill(1.0, 2.0));
-      firstData.add(noiseVec.get(0, 0));
-      secondData.add(noiseVec.get(1, 0));
-    }
-    assertEquals(1.0, calculateStandardDeviation(firstData), 0.2);
-    assertEquals(2.0, calculateStandardDeviation(secondData), 0.2);
-  }
-
-  private double calculateStandardDeviation(List<Double> numArray) {
-    double sum = 0.0;
-    double standardDeviation = 0.0;
-    int length = numArray.size();
-
-    for (double num : numArray) {
-      sum += num;
-    }
-
-    double mean = sum / length;
-
-    for (double num : numArray) {
-      standardDeviation += Math.pow(num - mean, 2);
-    }
-
-    return Math.sqrt(standardDeviation / length);
-  }
-
-  @Test
-  public void testDiscretizeA() {
-    var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
-    var x0 = VecBuilder.fill(1, 1);
-    var discA = Discretization.discretizeA(contA, 1.0);
-    var x1Discrete = discA.times(x0);
-
-    // We now have pos = vel = 1 and accel = 0, which should give us:
-    var x1Truth = VecBuilder.fill(x0.get(0, 0) + 1.0 * x0.get(1, 0),
-          x0.get(1, 0));
-    assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
-  }
-
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  public void testDiscretizeAB() {
-    var contA = Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, 0);
-    var contB = VecBuilder.fill(0, 1);
-    var x0 = VecBuilder.fill(1, 1);
-    var u = VecBuilder.fill(1);
-
-    var abPair = Discretization.discretizeAB(contA, contB, 1.0);
-
-    var x1Discrete = abPair.getFirst().times(x0).plus(abPair.getSecond().times(u));
-
-    // We now have pos = vel = accel = 1, which should give us:
-    var x1Truth = VecBuilder.fill(x0.get(0, 0) + x0.get(1, 0) + 0.5 * u.get(0, 0), x0.get(0, 0)
-          + u.get(0, 0));
-
-    assertTrue(x1Truth.isEqual(x1Discrete, 1E-4));
-  }
-
-  @Test
-  public void testMatrixExp() {
-    Matrix<N2, N2> wrappedMatrix = Matrix.eye(Nat.N2());
-    var wrappedResult = wrappedMatrix.exp();
-
-    assertTrue(wrappedResult.isEqual(
-        Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
-
-    var matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
-    wrappedResult = matrix.times(0.01).exp();
-
-    assertTrue(wrappedResult.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912,
-              0.03076368, 1.04111993), 1E-8));
-  }
-
-  @Test
-  public void testSimpleMatrixExp() {
-    SimpleMatrix matrix = SimpleMatrixUtils.eye(2);
-    var result = SimpleMatrixUtils.exp(matrix);
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-          result.getDDRM(),
-          new SimpleMatrix(2, 2, true, new double[]{Math.E, 0, 0, Math.E}).getDDRM(),
-          1E-9
-    ));
-
-    matrix = new SimpleMatrix(2, 2, true, new double[]{1, 2, 3, 4});
-    result = SimpleMatrixUtils.exp(matrix.scale(0.01));
-
-    assertTrue(MatrixFeatures_DDRM.isIdentical(
-          result.getDDRM(),
-          new SimpleMatrix(2, 2, true, new double[]{1.01035625, 0.02050912,
-              0.03076368, 1.04111993}).getDDRM(),
-          1E-8
-    ));
-  }
-
-  @Test
-  public void testPoseToVector() {
-    Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
-    var vector = StateSpaceUtil.poseToVector(pose);
-    assertEquals(pose.getTranslation().getX(), vector.get(0, 0), 1e-6);
-    assertEquals(pose.getTranslation().getY(), vector.get(1, 0), 1e-6);
-    assertEquals(pose.getRotation().getRadians(), vector.get(2, 0), 1e-6);
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
deleted file mode 100644
index 710bab5..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/CubicHermiteSplineTest.java
+++ /dev/null
@@ -1,162 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-
-class CubicHermiteSplineTest {
-  private static final double kMaxDx = 0.127;
-  private static final double kMaxDy = 0.00127;
-  private static final double kMaxDtheta = 0.0872;
-
-  @SuppressWarnings({"ParameterName", "PMD.UnusedLocalVariable"})
-  private void run(Pose2d a, List<Translation2d> waypoints, Pose2d b) {
-    // Start the timer.
-    //var start = System.nanoTime();
-
-    // Generate and parameterize the spline.
-    var controlVectors =
-        SplineHelper.getCubicControlVectorsFromWaypoints(a,
-            waypoints.toArray(new Translation2d[0]), b);
-    var splines
-        = SplineHelper.getCubicSplinesFromControlVectors(
-        controlVectors[0], waypoints.toArray(new Translation2d[0]), controlVectors[1]);
-
-    var poses = new ArrayList<PoseWithCurvature>();
-
-    poses.add(splines[0].getPoint(0.0));
-
-    for (var spline : splines) {
-      poses.addAll(SplineParameterizer.parameterize(spline));
-    }
-
-    // End the timer.
-    //var end = System.nanoTime();
-
-    // Calculate the duration (used when benchmarking)
-    //var durationMicroseconds = (end - start) / 1000.0;
-
-    for (int i = 0; i < poses.size() - 1; i++) {
-      var p0 = poses.get(i);
-      var p1 = poses.get(i + 1);
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      var twist = p0.poseMeters.log(p1.poseMeters);
-      assertAll(
-          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
-          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
-          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta)
-      );
-    }
-
-    // Check first point
-    assertAll(
-        () -> assertEquals(a.getX(),
-            poses.get(0).poseMeters.getX(), 1E-9),
-        () -> assertEquals(a.getY(),
-            poses.get(0).poseMeters.getY(), 1E-9),
-        () -> assertEquals(a.getRotation().getRadians(),
-            poses.get(0).poseMeters.getRotation().getRadians(), 1E-9)
-    );
-
-    // Check interior waypoints
-    boolean interiorsGood = true;
-    for (var waypoint : waypoints) {
-      boolean found = false;
-      for (var state : poses) {
-        if (waypoint.getDistance(state.poseMeters.getTranslation()) == 0) {
-          found = true;
-        }
-      }
-      interiorsGood &= found;
-    }
-
-    assertTrue(interiorsGood);
-
-    // Check last point
-    assertAll(
-        () -> assertEquals(b.getX(),
-            poses.get(poses.size() - 1).poseMeters.getX(), 1E-9),
-        () -> assertEquals(b.getY(),
-            poses.get(poses.size() - 1).poseMeters.getY(), 1E-9),
-        () -> assertEquals(b.getRotation().getRadians(),
-            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9)
-    );
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testStraightLine() {
-    run(new Pose2d(), new ArrayList<>(), new Pose2d(3, 0, new Rotation2d()));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testSCurve() {
-    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(90.0));
-    ArrayList<Translation2d> waypoints = new ArrayList<>();
-    waypoints.add(new Translation2d(1, 1));
-    waypoints.add(new Translation2d(2, -1));
-    var end = new Pose2d(3, 0, Rotation2d.fromDegrees(90.0));
-
-    run(start, waypoints, end);
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testOneInterior() {
-    var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
-    ArrayList<Translation2d> waypoints = new ArrayList<>();
-    waypoints.add(new Translation2d(2.0, 0.0));
-    var end = new Pose2d(4, 0, Rotation2d.fromDegrees(0.0));
-
-    run(start, waypoints, end);
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testWindyPath() {
-    final var start = new Pose2d(0, 0, Rotation2d.fromDegrees(0.0));
-    final ArrayList<Translation2d> waypoints = new ArrayList<>();
-    waypoints.add(new Translation2d(0.5, 0.5));
-    waypoints.add(new Translation2d(0.5, 0.5));
-    waypoints.add(new Translation2d(1.0, 0.0));
-    waypoints.add(new Translation2d(1.5, 0.5));
-    waypoints.add(new Translation2d(2.0, 0.0));
-    waypoints.add(new Translation2d(2.5, 0.5));
-    final var end = new Pose2d(3.0, 0.0, Rotation2d.fromDegrees(0.0));
-
-    run(start, waypoints, end);
-  }
-
-  @Test
-  void testMalformed() {
-    assertThrows(MalformedSplineException.class, () -> run(
-        new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-        new ArrayList<>(), new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
-    assertThrows(MalformedSplineException.class, () -> run(
-        new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
-        Arrays.asList(new Translation2d(10, 10.5)),
-        new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
deleted file mode 100644
index 3b35db0..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/spline/QuinticHermiteSplineTest.java
+++ /dev/null
@@ -1,106 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.spline;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.spline.SplineParameterizer.MalformedSplineException;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class QuinticHermiteSplineTest {
-  private static final double kMaxDx = 0.127;
-  private static final double kMaxDy = 0.00127;
-  private static final double kMaxDtheta = 0.0872;
-
-  @SuppressWarnings({ "ParameterName", "PMD.UnusedLocalVariable" })
-  private void run(Pose2d a, Pose2d b) {
-    // Start the timer.
-    //var start = System.nanoTime();
-
-    // Generate and parameterize the spline.
-    var spline = SplineHelper.getQuinticSplinesFromWaypoints(List.of(a, b))[0];
-    var poses = SplineParameterizer.parameterize(spline);
-
-    // End the timer.
-    //var end = System.nanoTime();
-
-    // Calculate the duration (used when benchmarking)
-    //var durationMicroseconds = (end - start) / 1000.0;
-
-    for (int i = 0; i < poses.size() - 1; i++) {
-      var p0 = poses.get(i);
-      var p1 = poses.get(i + 1);
-
-      // Make sure the twist is under the tolerance defined by the Spline class.
-      var twist = p0.poseMeters.log(p1.poseMeters);
-      assertAll(
-          () -> assertTrue(Math.abs(twist.dx) < kMaxDx),
-          () -> assertTrue(Math.abs(twist.dy) < kMaxDy),
-          () -> assertTrue(Math.abs(twist.dtheta) < kMaxDtheta));
-    }
-
-    // Check first point
-    assertAll(
-        () -> assertEquals(
-            a.getX(), poses.get(0).poseMeters.getX(), 1E-9),
-        () -> assertEquals(
-            a.getY(), poses.get(0).poseMeters.getY(), 1E-9),
-        () -> assertEquals(
-            a.getRotation().getRadians(), poses.get(0).poseMeters.getRotation().getRadians(),
-            1E-9));
-
-    // Check last point
-    assertAll(
-        () -> assertEquals(b.getX(), poses.get(poses.size() - 1)
-            .poseMeters.getX(), 1E-9),
-        () -> assertEquals(b.getY(), poses.get(poses.size() - 1)
-            .poseMeters.getY(), 1E-9),
-        () -> assertEquals(b.getRotation().getRadians(),
-            poses.get(poses.size() - 1).poseMeters.getRotation().getRadians(), 1E-9));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testStraightLine() {
-    run(new Pose2d(), new Pose2d(3, 0, new Rotation2d()));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testSimpleSCurve() {
-    run(new Pose2d(), new Pose2d(1, 1, new Rotation2d()));
-  }
-
-  @SuppressWarnings("PMD.JUnitTestsShouldIncludeAssert")
-  @Test
-  void testSquiggly() {
-    run(
-        new Pose2d(0, 0, Rotation2d.fromDegrees(90)),
-        new Pose2d(-1, 0, Rotation2d.fromDegrees(90)));
-  }
-
-  @Test
-  void testMalformed() {
-    assertThrows(MalformedSplineException.class,
-        () -> run(
-          new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-          new Pose2d(1, 0, Rotation2d.fromDegrees(180))));
-    assertThrows(MalformedSplineException.class,
-        () -> run(
-          new Pose2d(10, 10, Rotation2d.fromDegrees(90)),
-          new Pose2d(10, 11, Rotation2d.fromDegrees(-90))));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java
deleted file mode 100644
index 8af7ef0..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/system/LinearSystemIDTest.java
+++ /dev/null
@@ -1,91 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.system.plant.DCMotor;
-import edu.wpi.first.wpilibj.system.plant.LinearSystemId;
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class LinearSystemIDTest {
-  @Test
-  public void testDrivetrainVelocitySystem() {
-    var model = LinearSystemId.createDrivetrainVelocitySystem(
-            DCMotor.getNEO(4), 70, 0.05, 0.4, 6.0, 6
-    );
-    assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(),
-            Nat.N2()).fill(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
-
-    assertTrue(model.getB().isEqual(Matrix.mat(Nat.N2(),
-            Nat.N2()).fill(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
-
-    assertTrue(model.getC().isEqual(Matrix.mat(Nat.N2(),
-            Nat.N2()).fill(1.0, 0.0, 0.0, 1.0), 0.001));
-
-    assertTrue(model.getD().isEqual(Matrix.mat(Nat.N2(),
-            Nat.N2()).fill(0.0, 0.0, 0.0, 0.0), 0.001));
-  }
-
-  @Test
-  public void testElevatorSystem() {
-
-    var model = LinearSystemId.createElevatorSystem(DCMotor.getNEO(2), 5, 0.05, 12);
-    assertTrue(model.getA().isEqual(Matrix.mat(Nat.N2(),
-            Nat.N2()).fill(0, 1, 0, -99.05473), 0.001));
-
-    assertTrue(model.getB().isEqual(VecBuilder.fill(0, 20.8), 0.001));
-
-    assertTrue(model.getC().isEqual(Matrix.mat(Nat.N1(),
-            Nat.N2()).fill(1, 0), 0.001));
-
-    assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
-  }
-
-  @Test
-  public void testFlywheelSystem() {
-    var model = LinearSystemId.createFlywheelSystem(DCMotor.getNEO(2), 0.00032, 1.0);
-    assertTrue(model.getA().isEqual(VecBuilder.fill(-26.87032), 0.001));
-
-    assertTrue(model.getB().isEqual(VecBuilder.fill(1354.166667), 0.001));
-
-    assertTrue(model.getC().isEqual(VecBuilder.fill(1), 0.001));
-
-    assertTrue(model.getD().isEqual(VecBuilder.fill(0), 0.001));
-  }
-
-  @Test
-  public void testIdentifyPositionSystem() {
-    // By controls engineering in frc,
-    // x-dot = [0 1 | 0 -kv/ka] x = [0 | 1/ka] u
-    var kv = 1.0;
-    var ka = 0.5;
-    var model = LinearSystemId.identifyPositionSystem(kv, ka);
-
-    assertEquals(model.getA(), Matrix.mat(Nat.N2(), Nat.N2()).fill(0, 1, 0, -kv / ka));
-    assertEquals(model.getB(), VecBuilder.fill(0, 1 / ka));
-  }
-
-  @Test
-  public void testIdentifyVelocitySystem() {
-    // By controls engineering in frc,
-    // V = kv * velocity + ka * acceleration
-    // x-dot =  -kv/ka * v + 1/ka \cdot V
-    var kv = 1.0;
-    var ka = 0.5;
-    var model = LinearSystemId.identifyVelocitySystem(kv, ka);
-
-    assertEquals(model.getA(), VecBuilder.fill(-kv / ka));
-    assertEquals(model.getB(), VecBuilder.fill(1 / ka));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
deleted file mode 100644
index de39295..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/system/RungeKuttaTest.java
+++ /dev/null
@@ -1,36 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.system;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.Matrix;
-import edu.wpi.first.wpiutil.math.Nat;
-import edu.wpi.first.wpiutil.math.VecBuilder;
-import edu.wpi.first.wpiutil.math.numbers.N1;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class RungeKuttaTest {
-  @Test
-  @SuppressWarnings({"ParameterName", "LocalVariableName"})
-  public void testExponential() {
-
-    Matrix<N1, N1> y0 = VecBuilder.fill(0.0);
-
-    //noinspection SuspiciousNameCombination
-    var y1 = RungeKutta.rungeKutta((Matrix<N1, N1> x) -> {
-      var y = new Matrix<>(Nat.N1(), Nat.N1());
-      y.set(0, 0, Math.exp(x.get(0, 0)));
-      return y; },
-            y0, 0.1
-    );
-
-    assertEquals(Math.exp(0.1) - Math.exp(0.0), y1.get(0, 0), 1e-3);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
deleted file mode 100644
index 0c2f4f1..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/CentripetalAccelerationConstraintTest.java
+++ /dev/null
@@ -1,43 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Collections;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.trajectory.constraint.CentripetalAccelerationConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class CentripetalAccelerationConstraintTest {
-  @SuppressWarnings("LocalVariableName")
-  @Test
-  void testCentripetalAccelerationConstraint() {
-    double maxCentripetalAcceleration = Units.feetToMeters(7.0); // 7 feet per second squared
-    var constraint = new CentripetalAccelerationConstraint(maxCentripetalAcceleration);
-
-    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
-        Collections.singletonList(constraint));
-
-    var duration = trajectory.getTotalTimeSeconds();
-    var t = 0.0;
-    var dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      var centripetalAcceleration
-          = Math.pow(point.velocityMetersPerSecond, 2) * point.curvatureRadPerMeter;
-
-      t += dt;
-      assertTrue(centripetalAcceleration <= maxCentripetalAcceleration + 0.05);
-    }
-  }
-
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
deleted file mode 100644
index 6ed9966..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveKinematicsConstraintTest.java
+++ /dev/null
@@ -1,53 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.Collections;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class DifferentialDriveKinematicsConstraintTest {
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  @Test
-  void testDifferentialDriveKinematicsConstraint() {
-    double maxVelocity = Units.feetToMeters(12.0); // 12 feet per second
-    var kinematics = new DifferentialDriveKinematics(Units.inchesToMeters(27));
-    var constraint = new DifferentialDriveKinematicsConstraint(kinematics, maxVelocity);
-
-    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
-        Collections.singletonList(constraint));
-
-    var duration = trajectory.getTotalTimeSeconds();
-    var t = 0.0;
-    var dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      var chassisSpeeds = new ChassisSpeeds(
-          point.velocityMetersPerSecond, 0,
-          point.velocityMetersPerSecond * point.curvatureRadPerMeter
-      );
-
-      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
-
-      t += dt;
-      assertAll(
-          () -> assertTrue(wheelSpeeds.leftMetersPerSecond <= maxVelocity + 0.05),
-          () -> assertTrue(wheelSpeeds.rightMetersPerSecond <= maxVelocity + 0.05)
-      );
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
deleted file mode 100644
index fff4c61..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/DifferentialDriveVoltageConstraintTest.java
+++ /dev/null
@@ -1,103 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.Collections;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
-
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class DifferentialDriveVoltageConstraintTest {
-  @SuppressWarnings({"LocalVariableName", "PMD.AvoidInstantiatingObjectsInLoops"})
-  @Test
-  void testDifferentialDriveVoltageConstraint() {
-    // Pick an unreasonably large kA to ensure the constraint has to do some work
-    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
-    var kinematics = new DifferentialDriveKinematics(0.5);
-    double maxVoltage = 10;
-    var constraint = new DifferentialDriveVoltageConstraint(feedforward,
-                                                            kinematics,
-                                                            maxVoltage);
-
-    Trajectory trajectory = TrajectoryGeneratorTest.getTrajectory(
-        Collections.singletonList(constraint));
-
-    var duration = trajectory.getTotalTimeSeconds();
-    var t = 0.0;
-    var dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      var chassisSpeeds = new ChassisSpeeds(
-          point.velocityMetersPerSecond, 0,
-          point.velocityMetersPerSecond * point.curvatureRadPerMeter
-      );
-
-      var wheelSpeeds = kinematics.toWheelSpeeds(chassisSpeeds);
-
-      t += dt;
-
-      // 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
-      assertAll(
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               <= maxVoltage + 0.05),
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.leftMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               >= -maxVoltage - 0.05),
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               <= maxVoltage + 0.05),
-          () -> assertTrue(feedforward.calculate(wheelSpeeds.rightMetersPerSecond,
-                                                 point.accelerationMetersPerSecondSq)
-                               >= -maxVoltage - 0.05)
-      );
-    }
-  }
-
-  @Test
-  void testEndpointHighCurvature() {
-    var feedforward = new SimpleMotorFeedforward(1, 1, 3);
-
-    // Large trackwidth - need to test with radius of curvature less than half of trackwidth
-    var kinematics = new DifferentialDriveKinematics(3);
-    double maxVoltage = 10;
-    var constraint = new DifferentialDriveVoltageConstraint(feedforward,
-                                                            kinematics,
-                                                            maxVoltage);
-
-    var config = new TrajectoryConfig(12, 12).addConstraint(constraint);
-
-    // Radius of curvature should be ~1 meter.
-    assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
-        new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
-        new ArrayList<Translation2d>(),
-        new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
-        config));
-
-    assertDoesNotThrow(() -> TrajectoryGenerator.generateTrajectory(
-        new Pose2d(0, 1, Rotation2d.fromDegrees(180)),
-        new ArrayList<Translation2d>(),
-        new Pose2d(1, 0, Rotation2d.fromDegrees(90)),
-        config.setReversed(true)));
-
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java
deleted file mode 100644
index 513db06..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/EllipticalRegionConstraintTest.java
+++ /dev/null
@@ -1,77 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.constraint.EllipticalRegionConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class EllipticalRegionConstraintTest {
-  @Test
-  void testConstraint() {
-    // Create constraints
-    double maxVelocity = Units.feetToMeters(3.0);
-    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
-    var regionConstraint = new EllipticalRegionConstraint(
-        new Translation2d(Units.feetToMeters(5.0), Units.feetToMeters(5.0)),
-        Units.feetToMeters(10.0), Units.feetToMeters(5.0), Rotation2d.fromDegrees(180.0),
-        maxVelocityConstraint
-    );
-
-    // Get trajectory
-    var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
-
-    // Iterate through trajectory and check constraints
-    boolean exceededConstraintOutsideRegion = false;
-    for (var point : trajectory.getStates()) {
-      var translation = point.poseMeters.getTranslation();
-
-      if (translation.getX() < Units.feetToMeters(10)
-          && translation.getY() < Units.feetToMeters(5)) {
-        assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
-      } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
-        exceededConstraintOutsideRegion = true;
-      }
-    }
-    assertTrue(exceededConstraintOutsideRegion);
-  }
-
-  @Test
-  void testIsPoseWithinRegion() {
-    double maxVelocity = Units.feetToMeters(3.0);
-    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
-
-    var regionConstraintNoRotation = new EllipticalRegionConstraint(
-        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
-        Units.feetToMeters(2.0), Units.feetToMeters(4.0), new Rotation2d(),
-        maxVelocityConstraint);
-
-    assertFalse(regionConstraintNoRotation.isPoseInRegion(new Pose2d(
-        Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
-    )));
-
-    var regionConstraintWithRotation = new EllipticalRegionConstraint(
-        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
-        Units.feetToMeters(2.0), Units.feetToMeters(4.0), Rotation2d.fromDegrees(90.0),
-        maxVelocityConstraint);
-
-    assertTrue(regionConstraintWithRotation.isPoseInRegion(new Pose2d(
-        Units.feetToMeters(2.1), Units.feetToMeters(1.0), new Rotation2d()
-    )));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java
deleted file mode 100644
index 94eeb35..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/RectangularRegionConstraintTest.java
+++ /dev/null
@@ -1,65 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.constraint.MaxVelocityConstraint;
-import edu.wpi.first.wpilibj.trajectory.constraint.RectangularRegionConstraint;
-import edu.wpi.first.wpilibj.util.Units;
-
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class RectangularRegionConstraintTest {
-  @Test
-  void testConstraint() {
-    // Create constraints
-    double maxVelocity = Units.feetToMeters(3.0);
-    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
-    var regionConstraint = new RectangularRegionConstraint(
-        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
-        new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
-        maxVelocityConstraint
-    );
-
-    // Get trajectory
-    var trajectory = TrajectoryGeneratorTest.getTrajectory(List.of(regionConstraint));
-
-    // Iterate through trajectory and check constraints
-    boolean exceededConstraintOutsideRegion = false;
-    for (var point : trajectory.getStates()) {
-      if (regionConstraint.isPoseInRegion(point.poseMeters)) {
-        assertTrue(Math.abs(point.velocityMetersPerSecond) < maxVelocity + 0.05);
-      } else if (Math.abs(point.velocityMetersPerSecond) >= maxVelocity + 0.05) {
-        exceededConstraintOutsideRegion = true;
-      }
-    }
-    assertTrue(exceededConstraintOutsideRegion);
-  }
-
-  @Test
-  void testIsPoseWithinRegion() {
-    double maxVelocity = Units.feetToMeters(3.0);
-    var maxVelocityConstraint = new MaxVelocityConstraint(maxVelocity);
-    var regionConstraint = new RectangularRegionConstraint(
-        new Translation2d(Units.feetToMeters(1.0), Units.feetToMeters(1.0)),
-        new Translation2d(Units.feetToMeters(7.0), Units.feetToMeters(27.0)),
-        maxVelocityConstraint
-    );
-
-    assertFalse(regionConstraint.isPoseInRegion(new Pose2d()));
-    assertTrue(regionConstraint.isPoseInRegion(new Pose2d(Units.feetToMeters(3.0),
-        Units.feetToMeters(14.5), new Rotation2d())));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
deleted file mode 100644
index 2bfa972..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGeneratorTest.java
+++ /dev/null
@@ -1,89 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint;
-
-import static edu.wpi.first.wpilibj.util.Units.feetToMeters;
-import static org.junit.jupiter.api.Assertions.assertAll;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-class TrajectoryGeneratorTest {
-  static Trajectory getTrajectory(List<? extends TrajectoryConstraint> constraints) {
-    final double maxVelocity = feetToMeters(12.0);
-    final double maxAccel = feetToMeters(12);
-
-    // 2018 cross scale auto waypoints.
-    var sideStart = new Pose2d(feetToMeters(1.54), feetToMeters(23.23),
-        Rotation2d.fromDegrees(-180));
-    var crossScale = new Pose2d(feetToMeters(23.7), feetToMeters(6.8),
-        Rotation2d.fromDegrees(-160));
-
-    var waypoints = new ArrayList<Pose2d>();
-    waypoints.add(sideStart);
-    waypoints.add(sideStart.plus(
-        new Transform2d(new Translation2d(feetToMeters(-13), feetToMeters(0)),
-            new Rotation2d())));
-    waypoints.add(sideStart.plus(
-        new Transform2d(new Translation2d(feetToMeters(-19.5), feetToMeters(5)),
-            Rotation2d.fromDegrees(-90))));
-    waypoints.add(crossScale);
-
-    TrajectoryConfig config = new TrajectoryConfig(maxVelocity, maxAccel)
-        .setReversed(true)
-        .addConstraints(constraints);
-
-    return TrajectoryGenerator.generateTrajectory(waypoints, config);
-  }
-
-  @Test
-  @SuppressWarnings("LocalVariableName")
-  void testGenerationAndConstraints() {
-    Trajectory trajectory = getTrajectory(new ArrayList<>());
-
-    double duration = trajectory.getTotalTimeSeconds();
-    double t = 0.0;
-    double dt = 0.02;
-
-    while (t < duration) {
-      var point = trajectory.sample(t);
-      t += dt;
-      assertAll(
-          () -> assertTrue(Math.abs(point.velocityMetersPerSecond) < feetToMeters(12.0) + 0.05),
-          () -> assertTrue(Math.abs(point.accelerationMetersPerSecondSq) < feetToMeters(12.0)
-              + 0.05)
-      );
-    }
-  }
-
-  @Test
-  void testMalformedTrajectory() {
-    var traj =
-        TrajectoryGenerator.generateTrajectory(
-          Arrays.asList(
-            new Pose2d(0, 0, Rotation2d.fromDegrees(0)),
-            new Pose2d(1, 0, Rotation2d.fromDegrees(180))
-          ),
-          new TrajectoryConfig(feetToMeters(12), feetToMeters(12))
-        );
-
-    assertEquals(traj.getStates().size(), 1);
-    assertEquals(traj.getTotalTimeSeconds(), 0);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
deleted file mode 100644
index d8d59b1..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryJsonTest.java
+++ /dev/null
@@ -1,33 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
-import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-public class TrajectoryJsonTest {
-  @Test
-  void deserializeMatches() {
-    var config = List.of(new DifferentialDriveKinematicsConstraint(
-        new DifferentialDriveKinematics(20), 3));
-    var trajectory = TrajectoryGeneratorTest.getTrajectory(config);
-
-    var deserialized =
-        assertDoesNotThrow(() ->
-            TrajectoryUtil.deserializeTrajectory(TrajectoryUtil.serializeTrajectory(trajectory)));
-
-    assertEquals(trajectory.getStates(), deserialized.getStates());
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
deleted file mode 100644
index b17046b..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrajectoryTransformTest.java
+++ /dev/null
@@ -1,68 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import java.util.List;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.geometry.Pose2d;
-import edu.wpi.first.wpilibj.geometry.Rotation2d;
-import edu.wpi.first.wpilibj.geometry.Transform2d;
-import edu.wpi.first.wpilibj.geometry.Translation2d;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class TrajectoryTransformTest {
-  @Test
-  void testTransformBy() {
-    var config = new TrajectoryConfig(3, 3);
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(), List.of(), new Pose2d(1, 1, Rotation2d.fromDegrees(90)),
-        config
-    );
-
-    var transformedTrajectory = trajectory.transformBy(
-        new Transform2d(new Translation2d(1, 2), Rotation2d.fromDegrees(30)));
-
-    // Test initial pose.
-    assertEquals(new Pose2d(1, 2, Rotation2d.fromDegrees(30)),
-        transformedTrajectory.sample(0).poseMeters);
-
-    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
-  }
-
-  @Test
-  void testRelativeTo() {
-    var config = new TrajectoryConfig(3, 3);
-    var trajectory = TrajectoryGenerator.generateTrajectory(
-        new Pose2d(1, 2, Rotation2d.fromDegrees(30.0)),
-        List.of(), new Pose2d(5, 7, Rotation2d.fromDegrees(90)),
-        config
-    );
-
-    var transformedTrajectory = trajectory.relativeTo(new Pose2d(1, 2, Rotation2d.fromDegrees(30)));
-
-    // Test initial pose.
-    assertEquals(new Pose2d(), transformedTrajectory.sample(0).poseMeters);
-
-    testSameShapedTrajectory(trajectory.getStates(), transformedTrajectory.getStates());
-  }
-
-  void testSameShapedTrajectory(List<Trajectory.State> statesA, List<Trajectory.State> statesB) {
-    for (int i = 0; i < statesA.size() - 1; i++) {
-      var a1 = statesA.get(i).poseMeters;
-      var a2 = statesA.get(i + 1).poseMeters;
-
-      var b1 = statesB.get(i).poseMeters;
-      var b2 = statesB.get(i + 1).poseMeters;
-
-      assertEquals(a2.relativeTo(a1), b2.relativeTo(b1));
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
deleted file mode 100644
index e155188..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/trajectory/TrapezoidProfileTest.java
+++ /dev/null
@@ -1,257 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.trajectory;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertNotEquals;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-@SuppressWarnings("PMD.AvoidInstantiatingObjectsInLoops")
-class TrapezoidProfileTest {
-  private static final double kDt = 0.01;
-
-  /**
-   * Asserts "val1" is less than or equal to "val2".
-   *
-   * @param val1 First operand in comparison.
-   * @param val2 Second operand in comparison.
-   */
-  private static void assertLessThanOrEquals(double val1, double val2) {
-    assertTrue(val1 <= val2, val1 + " is greater than " + val2);
-  }
-
-  /**
-   * Asserts "val1" is within "eps" of "val2".
-   *
-   * @param val1 First operand in comparison.
-   * @param val2 Second operand in comparison.
-   * @param eps Tolerance for whether values are near to each other.
-   */
-  private static void assertNear(double val1, double val2, double eps) {
-    assertTrue(Math.abs(val1 - val2) <= eps, "Difference between " + val1 + " and " + val2
-        + " is greater than " + eps);
-  }
-
-  /**
-   * Asserts "val1" is less than or within "eps" of "val2".
-   *
-   * @param val1 First operand in comparison.
-   * @param val2 Second operand in comparison.
-   * @param eps Tolerance for whether values are near to each other.
-   */
-  private static void assertLessThanOrNear(double val1, double val2, double eps) {
-    if (val1 <= val2) {
-      assertLessThanOrEquals(val1, val2);
-    } else {
-      assertNear(val1, val2, eps);
-    }
-  }
-
-  @Test
-  void reachesGoal() {
-    TrapezoidProfile.Constraints constraints =
-        new TrapezoidProfile.Constraints(1.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(3, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 450; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(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
-  void posContinousUnderVelChange() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(1.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(12, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double lastPos = state.position;
-    for (int i = 0; i < 1600; ++i) {
-      if (i == 400) {
-        constraints.maxVelocity = 0.75;
-      }
-
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      double 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.
-        assertLessThanOrNear(estimatedVel, constraints.maxVelocity, 1e-4);
-
-        assertLessThanOrEquals(state.velocity, constraints.maxVelocity);
-      }
-
-      lastPos = state.position;
-    }
-    assertEquals(state, goal);
-  }
-
-  // There is some somewhat tricky code for dealing with going backwards
-  @Test
-  void backwards() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 400; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  @Test
-  void switchGoalInMiddle() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertNotEquals(state, goal);
-
-    goal = new TrapezoidProfile.State(0.0, 0.0);
-    for (int i = 0; i < 550; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  // Checks to make sure that it hits top speed
-  @Test
-  void topSpeed() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(4, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 200; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertNear(constraints.maxVelocity, state.velocity, 10e-5);
-
-    for (int i = 0; i < 2000; ++i) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-    }
-    assertEquals(state, goal);
-  }
-
-  @Test
-  void timingToCurrent() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
-    TrapezoidProfile.State state = new TrapezoidProfile.State();
-
-    for (int i = 0; i < 400; i++) {
-      TrapezoidProfile profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      assertNear(profile.timeLeftUntil(state.position), 0, 2e-2);
-    }
-  }
-
-  @Test
-  void timingToGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
-    boolean reachedGoal = false;
-    for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      if (!reachedGoal && state.equals(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
-        assertNear(predictedTimeLeft, i / 100.0, 0.25);
-        reachedGoal = true;
-      }
-    }
-  }
-
-  @Test
-  void timingBeforeGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(1);
-    boolean reachedGoal = false;
-    for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      if (!reachedGoal && (Math.abs(state.velocity - 1) < 10e-5)) {
-        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
-        reachedGoal = true;
-      }
-    }
-  }
-
-  @Test
-  void timingToNegativeGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(goal.position);
-    boolean reachedGoal = false;
-    for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      if (!reachedGoal && state.equals(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
-        assertNear(predictedTimeLeft, i / 100.0, 0.25);
-        reachedGoal = true;
-      }
-    }
-  }
-
-  @Test
-  void timingBeforeNegativeGoal() {
-    TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(0.75, 0.75);
-    TrapezoidProfile.State goal = new TrapezoidProfile.State(-2, 0);
-
-    TrapezoidProfile profile = new TrapezoidProfile(constraints, goal);
-    TrapezoidProfile.State state = profile.calculate(kDt);
-
-    double predictedTimeLeft = profile.timeLeftUntil(-1);
-    boolean reachedGoal = false;
-    for (int i = 0; i < 400; i++) {
-      profile = new TrapezoidProfile(constraints, goal, state);
-      state = profile.calculate(kDt);
-      if (!reachedGoal && (Math.abs(state.velocity + 1) < 10e-5)) {
-        assertNear(predictedTimeLeft, i / 100.0, 2e-2);
-        reachedGoal = true;
-      }
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
deleted file mode 100644
index 9938660..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpilibj/util/UnitsTest.java
+++ /dev/null
@@ -1,60 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpilibj.util;
-
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpilibj.UtilityClassTest;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class UnitsTest extends UtilityClassTest<Units> {
-  UnitsTest() {
-    super(Units.class);
-  }
-
-  @Test
-  void metersToFeetTest() {
-    assertEquals(3.28, Units.metersToFeet(1), 1e-2);
-  }
-
-  @Test
-  void feetToMetersTest() {
-    assertEquals(0.30, Units.feetToMeters(1), 1e-2);
-  }
-
-  @Test
-  void metersToInchesTest() {
-    assertEquals(39.37, Units.metersToInches(1), 1e-2);
-  }
-
-  @Test
-  void inchesToMetersTest() {
-    assertEquals(0.0254, Units.inchesToMeters(1), 1e-3);
-  }
-
-  @Test
-  void degreesToRadiansTest() {
-    assertEquals(0.017, Units.degreesToRadians(1), 1e-3);
-  }
-
-  @Test
-  void radiansToDegreesTest() {
-    assertEquals(114.59, Units.radiansToDegrees(2), 1e-2);
-  }
-
-  @Test
-  void rotationsPerMinuteToRadiansPerSecondTest() {
-    assertEquals(6.28, Units.rotationsPerMinuteToRadiansPerSecond(60), 1e-2);
-  }
-
-  @Test
-  void radiansPerSecondToRotationsPerMinute() {
-    assertEquals(76.39, Units.radiansPerSecondToRotationsPerMinute(8), 1e-2);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java
deleted file mode 100644
index 14a0f7c..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MathUtilTest.java
+++ /dev/null
@@ -1,22 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class MathUtilTest {
-  @Test
-  void testAngleNormalize() {
-    assertEquals(MathUtil.normalizeAngle(5 * Math.PI), Math.PI);
-    assertEquals(MathUtil.normalizeAngle(-5 * Math.PI), Math.PI);
-    assertEquals(MathUtil.normalizeAngle(Math.PI / 2), Math.PI / 2);
-    assertEquals(MathUtil.normalizeAngle(-Math.PI / 2), -Math.PI / 2);
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java b/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
deleted file mode 100644
index 1af0625..0000000
--- a/third_party/allwpilib/wpimath/src/test/java/edu/wpi/first/wpiutil/math/MatrixTest.java
+++ /dev/null
@@ -1,174 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.math;
-
-import org.ejml.data.SingularMatrixException;
-import org.junit.jupiter.api.Test;
-
-import edu.wpi.first.wpiutil.math.numbers.N1;
-import edu.wpi.first.wpiutil.math.numbers.N2;
-import edu.wpi.first.wpiutil.math.numbers.N3;
-import edu.wpi.first.wpiutil.math.numbers.N4;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-import static org.junit.jupiter.api.Assertions.assertThrows;
-import static org.junit.jupiter.api.Assertions.assertTrue;
-
-public class MatrixTest {
-  @Test
-  void testMatrixMultiplication() {
-    var mat1 = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(2.0, 1.0,
-            0.0, 1.0);
-    var mat2 = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(3.0, 0.0,
-            0.0, 2.5);
-
-    Matrix<N2, N2> result = mat1.times(mat2);
-
-    assertEquals(result, Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 2.5, 0.0, 2.5));
-
-    var mat3 = Matrix.mat(Nat.N2(), Nat.N3())
-        .fill(1.0, 3.0, 0.5,
-            2.0, 4.3, 1.2);
-    var mat4 = Matrix.mat(Nat.N3(), Nat.N4())
-        .fill(3.0, 1.5, 2.0, 4.5,
-            2.3, 1.0, 1.6, 3.1,
-            5.2, 2.1, 2.0, 1.0);
-
-    Matrix<N2, N4> result2 = mat3.times(mat4);
-
-    assertTrue(Matrix.mat(Nat.N2(), Nat.N4())
-        .fill(12.5, 5.55, 7.8, 14.3,
-            22.13, 9.82, 13.28, 23.53).isEqual(
-            result2,
-              1E-9
-    ));
-  }
-
-  @Test
-  void testMatrixVectorMultiplication() {
-    var mat = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(1.0, 1.0,
-            0.0, 1.0);
-
-    var vec = VecBuilder.fill(3.0, 2.0);
-
-    Matrix<N2, N1> result = mat.times(vec);
-    assertEquals(VecBuilder.fill(5.0, 2.0), result);
-  }
-
-  @Test
-  void testTranspose() {
-    Matrix<N3, N1> vec = VecBuilder
-        .fill(1.0,
-            2.0,
-            3.0);
-
-    Matrix<N1, N3> transpose = vec.transpose();
-
-    assertEquals(Matrix.mat(Nat.N1(), Nat.N3()).fill(1.0, 2.0, 3.0), transpose);
-  }
-
-  @Test
-  void testSolve() {
-    var mat1 = Matrix.mat(Nat.N2(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0);
-    var vec1 = VecBuilder.fill(1.0, 2.0);
-
-    var solve1 = mat1.solve(vec1);
-
-    assertEquals(VecBuilder.fill(0.0, 0.5), solve1);
-
-    var mat2 = Matrix.mat(Nat.N3(), Nat.N2()).fill(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
-    var vec2 = VecBuilder.fill(1.0, 2.0, 3.0);
-
-    var solve2 = mat2.solve(vec2);
-
-    assertEquals(VecBuilder.fill(0.0, 0.5), solve2);
-  }
-
-  @Test
-  void testInverse() {
-    var mat = Matrix.mat(Nat.N3(), Nat.N3())
-        .fill(1.0, 3.0, 2.0,
-            5.0, 2.0, 1.5,
-            0.0, 1.3, 2.5);
-
-    var inv = mat.inv();
-
-    assertTrue(Matrix.eye(Nat.N3()).isEqual(
-        mat.times(inv),
-        1E-9
-    ));
-
-    assertTrue(Matrix.eye(Nat.N3()).isEqual(
-        inv.times(mat),
-        1E-9
-    ));
-  }
-
-  @Test
-  void testUninvertableMatrix() {
-    var singularMatrix = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(2.0, 1.0,
-            2.0, 1.0);
-
-    assertThrows(SingularMatrixException.class, singularMatrix::inv);
-  }
-
-  @Test
-  void testMatrixScalarArithmetic() {
-    var mat = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(1.0, 2.0,
-            3.0, 4.0);
-
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(3.0, 4.0, 5.0, 6.0), mat.plus(2.0));
-
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.0, 1.0, 2.0, 3.0), mat.minus(1.0));
-
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(2.0, 4.0, 6.0, 8.0),  mat.times(2.0));
-
-    assertTrue(Matrix.mat(Nat.N2(), Nat.N2()).fill(0.5, 1.0, 1.5, 2.0).isEqual(
-        mat.div(2.0),
-        1E-3
-    ));
-  }
-
-  @Test
-  void testMatrixMatrixArithmetic() {
-    var mat1 = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(1.0, 2.0,
-            3.0, 4.0);
-
-    var mat2 = Matrix.mat(Nat.N2(), Nat.N2())
-        .fill(5.0, 6.0,
-            7.0, 8.0);
-
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(-4.0, -4.0, -4.0, -4.0),
-        mat1.minus(mat2)
-    );
-
-    assertEquals(Matrix.mat(Nat.N2(), Nat.N2()).fill(6.0, 8.0, 10.0, 12.0),
-        mat1.plus(mat2)
-    );
-  }
-
-  @Test
-  void testMatrixExponential() {
-    var matrix = Matrix.eye(Nat.N2());
-    var result = matrix.exp();
-
-    assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(Math.E, 0, 0, Math.E), 1E-9));
-
-    matrix = Matrix.mat(Nat.N2(), Nat.N2()).fill(1, 2, 3, 4);
-    result = matrix.times(0.01).exp();
-
-    assertTrue(result.isEqual(Matrix.mat(Nat.N2(), Nat.N2()).fill(1.01035625, 0.02050912,
-        0.03076368, 1.04111993), 1E-8));
-  }
-}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/EigenTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/EigenTest.cpp
index 2065e26..c1786c3 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/EigenTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/EigenTest.cpp
@@ -1,57 +1,46 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "Eigen/Core"
 #include "Eigen/LU"
 #include "gtest/gtest.h"
 
-TEST(EigenTest, MultiplicationTest) {
-  Eigen::Matrix<double, 2, 2> m1;
-  m1 << 2, 1, 0, 1;
-
-  Eigen::Matrix<double, 2, 2> m2;
-  m2 << 3, 0, 0, 2.5;
+TEST(EigenTest, Multiplication) {
+  Eigen::Matrix<double, 2, 2> m1{{2, 1}, {0, 1}};
+  Eigen::Matrix<double, 2, 2> m2{{3, 0}, {0, 2.5}};
 
   const auto result = m1 * m2;
 
-  Eigen::Matrix<double, 2, 2> expectedResult;
-  expectedResult << 6.0, 2.5, 0.0, 2.5;
+  Eigen::Matrix<double, 2, 2> expectedResult{{6.0, 2.5}, {0.0, 2.5}};
 
   EXPECT_TRUE(expectedResult.isApprox(result));
 
-  Eigen::Matrix<double, 2, 3> m3;
-  m3 << 1.0, 3.0, 0.5, 2.0, 4.3, 1.2;
-
-  Eigen::Matrix<double, 3, 4> m4;
-  m4 << 3.0, 1.5, 2.0, 4.5, 2.3, 1.0, 1.6, 3.1, 5.2, 2.1, 2.0, 1.0;
+  Eigen::Matrix<double, 2, 3> m3{{1.0, 3.0, 0.5}, {2.0, 4.3, 1.2}};
+  Eigen::Matrix<double, 3, 4> m4{
+      {3.0, 1.5, 2.0, 4.5}, {2.3, 1.0, 1.6, 3.1}, {5.2, 2.1, 2.0, 1.0}};
 
   const auto result2 = m3 * m4;
 
-  Eigen::Matrix<double, 2, 4> expectedResult2;
-  expectedResult2 << 12.5, 5.55, 7.8, 14.3, 22.13, 9.82, 13.28, 23.53;
+  Eigen::Matrix<double, 2, 4> expectedResult2{{12.5, 5.55, 7.8, 14.3},
+                                              {22.13, 9.82, 13.28, 23.53}};
 
   EXPECT_TRUE(expectedResult2.isApprox(result2));
 }
 
-TEST(EigenTest, TransposeTest) {
-  Eigen::Matrix<double, 3, 1> vec;
-  vec << 1, 2, 3;
+TEST(EigenTest, Transpose) {
+  Eigen::Vector<double, 3> vec{1, 2, 3};
 
   const auto transpose = vec.transpose();
 
-  Eigen::Matrix<double, 1, 3> expectedTranspose;
-  expectedTranspose << 1, 2, 3;
+  Eigen::RowVector<double, 3> expectedTranspose{1, 2, 3};
 
   EXPECT_TRUE(expectedTranspose.isApprox(transpose));
 }
 
-TEST(EigenTest, InverseTest) {
-  Eigen::Matrix<double, 3, 3> mat;
-  mat << 1.0, 3.0, 2.0, 5.0, 2.0, 1.5, 0.0, 1.3, 2.5;
+TEST(EigenTest, Inverse) {
+  Eigen::Matrix<double, 3, 3> mat{
+      {1.0, 3.0, 2.0}, {5.0, 2.0, 1.5}, {0.0, 1.3, 2.5}};
 
   const auto inverse = mat.inverse();
   const auto identity = Eigen::MatrixXd::Identity(3, 3);
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/FormatterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/FormatterTest.cpp
new file mode 100644
index 0000000..cd7ef5c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <fmt/format.h>
+
+#include "frc/fmt/Eigen.h"
+#include "frc/fmt/Units.h"
+#include "gtest/gtest.h"
+#include "units/velocity.h"
+
+TEST(FormatterTest, Eigen) {
+  Eigen::Matrix<double, 3, 2> A{{1.0, 2.0}, {3.0, 4.0}, {5.0, 6.0}};
+  EXPECT_EQ(
+      "  1.000000  2.000000\n"
+      "  3.000000  4.000000\n"
+      "  5.000000  6.000000",
+      fmt::format("{}", A));
+}
+
+TEST(FormatterTest, Units) {
+  EXPECT_EQ("4 mps", fmt::format("{}", 4_mps));
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp
deleted file mode 100644
index 934e14b..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/LinearFilterNoiseTest.cpp
+++ /dev/null
@@ -1,94 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"  // NOLINT(build/include_order)
-
-#include <cmath>
-#include <memory>
-#include <random>
-
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-#include "units/time.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/third_party/allwpilib/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp
deleted file mode 100644
index d321518..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/LinearFilterOutputTest.cpp
+++ /dev/null
@@ -1,136 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#include "frc/LinearFilter.h"  // NOLINT(build/include_order)
-
-#include <cmath>
-#include <functional>
-#include <memory>
-#include <random>
-
-#include <wpi/math>
-
-#include "gtest/gtest.h"
-#include "units/time.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/third_party/allwpilib/wpimath/src/test/native/cpp/MathUtilTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/MathUtilTest.cpp
new file mode 100644
index 0000000..6b5af2b
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/MathUtilTest.cpp
@@ -0,0 +1,90 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/MathUtil.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+
+#define EXPECT_UNITS_EQ(a, b) EXPECT_DOUBLE_EQ((a).value(), (b).value())
+
+#define EXPECT_UNITS_NEAR(a, b, c) EXPECT_NEAR((a).value(), (b).value(), c)
+
+TEST(MathUtilTest, ApplyDeadband) {
+  // < 0
+  EXPECT_DOUBLE_EQ(-1.0, frc::ApplyDeadband(-1.0, 0.02));
+  EXPECT_DOUBLE_EQ((-0.03 + 0.02) / (1.0 - 0.02),
+                   frc::ApplyDeadband(-0.03, 0.02));
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.02, 0.02));
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(-0.01, 0.02));
+
+  // == 0
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.0, 0.02));
+
+  // > 0
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.01, 0.02));
+  EXPECT_DOUBLE_EQ(0.0, frc::ApplyDeadband(0.02, 0.02));
+  EXPECT_DOUBLE_EQ((0.03 - 0.02) / (1.0 - 0.02),
+                   frc::ApplyDeadband(0.03, 0.02));
+  EXPECT_DOUBLE_EQ(1.0, frc::ApplyDeadband(1.0, 0.02));
+}
+
+TEST(MathUtilTest, InputModulus) {
+  // These tests check error wrapping. That is, the result of wrapping the
+  // result of an angle reference minus the measurement.
+
+  // Test symmetric range
+  EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(-20.0,
+                   frc::InputModulus(170.0 + 360.0 - (-170.0), -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(-20.0,
+                   frc::InputModulus(170.0 - (-170.0 + 360.0), -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(20.0, frc::InputModulus(-170.0 - 170.0, -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(20.0,
+                   frc::InputModulus(-170.0 + 360.0 - 170.0, -180.0, 180.0));
+  EXPECT_DOUBLE_EQ(20.0,
+                   frc::InputModulus(-170.0 - (170.0 + 360.0), -180.0, 180.0));
+
+  // Test range starting at zero
+  EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 - 190.0, 0.0, 360.0));
+  EXPECT_DOUBLE_EQ(340.0, frc::InputModulus(170.0 + 360.0 - 190.0, 0.0, 360.0));
+  EXPECT_DOUBLE_EQ(340.0,
+                   frc::InputModulus(170.0 - (190.0 + 360.0), 0.0, 360.0));
+
+  // Test asymmetric range that doesn't start at zero
+  EXPECT_DOUBLE_EQ(-20.0, frc::InputModulus(170.0 - (-170.0), -170.0, 190.0));
+
+  // Test range with both positive endpoints
+  EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(0.0, 1.0, 3.0));
+  EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(1.0, 1.0, 3.0));
+  EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(2.0, 1.0, 3.0));
+  EXPECT_DOUBLE_EQ(3.0, frc::InputModulus(3.0, 1.0, 3.0));
+  EXPECT_DOUBLE_EQ(2.0, frc::InputModulus(4.0, 1.0, 3.0));
+
+  // Test all supported types
+  EXPECT_DOUBLE_EQ(-20.0,
+                   frc::InputModulus<double>(170.0 - (-170.0), -170.0, 190.0));
+  EXPECT_EQ(-20, frc::InputModulus<int>(170 - (-170), -170, 190));
+  EXPECT_EQ(-20_deg, frc::InputModulus<units::degree_t>(170_deg - (-170_deg),
+                                                        -170_deg, 190_deg));
+}
+
+TEST(MathUtilTest, AngleModulus) {
+  EXPECT_UNITS_NEAR(
+      frc::AngleModulus(units::radian_t{-2000 * wpi::numbers::pi / 180}),
+      units::radian_t{160 * wpi::numbers::pi / 180}, 1e-10);
+  EXPECT_UNITS_NEAR(
+      frc::AngleModulus(units::radian_t{358 * wpi::numbers::pi / 180}),
+      units::radian_t{-2 * wpi::numbers::pi / 180}, 1e-10);
+  EXPECT_UNITS_NEAR(frc::AngleModulus(units::radian_t{2.0 * wpi::numbers::pi}),
+                    0_rad, 1e-10);
+
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(5 * wpi::numbers::pi)),
+                  units::radian_t(wpi::numbers::pi));
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-5 * wpi::numbers::pi)),
+                  units::radian_t(wpi::numbers::pi));
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(wpi::numbers::pi / 2)),
+                  units::radian_t(wpi::numbers::pi / 2));
+  EXPECT_UNITS_EQ(frc::AngleModulus(units::radian_t(-wpi::numbers::pi / 2)),
+                  units::radian_t(-wpi::numbers::pi / 2));
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/MedianFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/MedianFilterTest.cpp
deleted file mode 100644
index 2a02e1c..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/MedianFilterTest.cpp
+++ /dev/null
@@ -1,55 +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/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/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceTest.cpp
index a1600fc..0dc3978 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -25,7 +22,7 @@
 constexpr double kPositionStddev = 0.0001;
 constexpr auto kDt = 0.00505_s;
 
-class StateSpace : public testing::Test {
+class StateSpaceTest : public testing::Test {
  public:
   LinearSystem<2, 1, 1> plant = [] {
     auto motors = DCMotor::Vex775Pro(2);
@@ -46,24 +43,23 @@
   LinearSystemLoop<2, 1, 1> loop{plant, controller, observer, 12_V, kDt};
 };
 
-void Update(LinearSystemLoop<2, 1, 1>& loop, double noise) {
-  Eigen::Matrix<double, 1, 1> y =
-      loop.Plant().CalculateY(loop.Xhat(), loop.U()) +
-      Eigen::Matrix<double, 1, 1>(noise);
+void Update(const LinearSystem<2, 1, 1>& plant, LinearSystemLoop<2, 1, 1>& loop,
+            double noise) {
+  Eigen::Vector<double, 1> y =
+      plant.CalculateY(loop.Xhat(), loop.U()) + Eigen::Vector<double, 1>{noise};
   loop.Correct(y);
   loop.Predict(kDt);
 }
 
-TEST_F(StateSpace, CorrectPredictLoop) {
+TEST_F(StateSpaceTest, CorrectPredictLoop) {
   std::default_random_engine generator;
   std::normal_distribution<double> dist{0.0, kPositionStddev};
 
-  Eigen::Matrix<double, 2, 1> references;
-  references << 2.0, 0.0;
+  Eigen::Vector<double, 2> references{2.0, 0.0};
   loop.SetNextR(references);
 
   for (int i = 0; i < 1000; i++) {
-    Update(loop, dist(generator));
+    Update(plant, loop, dist(generator));
     EXPECT_PRED_FORMAT2(testing::DoubleLE, -12.0, loop.U(0));
     EXPECT_PRED_FORMAT2(testing::DoubleLE, loop.U(0), 12.0);
   }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
index 24e9cf2..57b93bb 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/StateSpaceUtilTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -11,16 +8,16 @@
 
 #include "Eigen/Core"
 #include "frc/StateSpaceUtil.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
 
 TEST(StateSpaceUtilTest, MakeMatrix) {
   // Column vector
-  Eigen::Matrix<double, 2, 1> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
+  Eigen::Vector<double, 2> mat1 = frc::MakeMatrix<2, 1>(1.0, 2.0);
   EXPECT_NEAR(mat1(0), 1.0, 1e-3);
   EXPECT_NEAR(mat1(1), 2.0, 1e-3);
 
   // Row vector
-  Eigen::Matrix<double, 1, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
+  Eigen::RowVector<double, 2> mat2 = frc::MakeMatrix<1, 2>(1.0, 2.0);
   EXPECT_NEAR(mat2(0), 1.0, 1e-3);
   EXPECT_NEAR(mat2(1), 2.0, 1e-3);
 
@@ -105,44 +102,59 @@
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorParameterPack) {
-  Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
+  Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector(2.0, 3.0);
   static_cast<void>(vec);
 }
 
 TEST(StateSpaceUtilTest, WhiteNoiseVectorArray) {
-  Eigen::Matrix<double, 2, 1> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
+  Eigen::Vector<double, 2> vec = frc::MakeWhiteNoiseVector<2>({2.0, 3.0});
   static_cast<void>(vec);
 }
 
 TEST(StateSpaceUtilTest, IsStabilizable) {
-  Eigen::Matrix<double, 2, 2> A;
-  Eigen::Matrix<double, 2, 1> B;
-  B << 0, 1;
-
-  // We separate the result of IsStabilizable from the assertion because
-  // templates break gtest.
+  Eigen::Matrix<double, 2, 1> B{0, 1};
 
   // First eigenvalue is uncontrollable and unstable.
   // Second eigenvalue is controllable and stable.
-  A << 1.2, 0, 0, 0.5;
-  bool ret = frc::IsStabilizable<2, 1>(A, B);
-  EXPECT_FALSE(ret);
+  EXPECT_FALSE((frc::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and marginally stable.
   // Second eigenvalue is controllable and stable.
-  A << 1, 0, 0, 0.5;
-  ret = frc::IsStabilizable<2, 1>(A, B);
-  EXPECT_FALSE(ret);
+  EXPECT_FALSE((frc::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and stable.
-  A << 0.2, 0, 0, 0.5;
-  ret = frc::IsStabilizable<2, 1>(A, B);
-  EXPECT_TRUE(ret);
+  EXPECT_TRUE((frc::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, B)));
 
   // First eigenvalue is uncontrollable and stable.
   // Second eigenvalue is controllable and unstable.
-  A << 0.2, 0, 0, 1.2;
-  ret = frc::IsStabilizable<2, 1>(A, B);
-  EXPECT_TRUE(ret);
+  EXPECT_TRUE((frc::IsStabilizable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, B)));
+}
+
+TEST(StateSpaceUtilTest, IsDetectable) {
+  Eigen::Matrix<double, 1, 2> C{0, 1};
+
+  // First eigenvalue is unobservable and unstable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_FALSE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1.2, 0}, {0, 0.5}}, C)));
+
+  // First eigenvalue is unobservable and marginally stable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_FALSE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{1, 0}, {0, 0.5}}, C)));
+
+  // First eigenvalue is unobservable and stable.
+  // Second eigenvalue is observable and stable.
+  EXPECT_TRUE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 0.5}}, C)));
+
+  // First eigenvalue is unobservable and stable.
+  // Second eigenvalue is observable and unstable.
+  EXPECT_TRUE((frc::IsDetectable<2, 1>(
+      Eigen::Matrix<double, 2, 2>{{0.2, 0}, {0, 1.2}}, C)));
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/UnitsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/UnitsTest.cpp
index 54e8195..ec38a47 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/UnitsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/UnitsTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <array>
 #include <chrono>
@@ -96,66 +93,66 @@
 
 class TypeTraits : public ::testing::Test {
  protected:
-  TypeTraits() {}
-  virtual ~TypeTraits() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  TypeTraits() = default;
+  ~TypeTraits() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class UnitManipulators : public ::testing::Test {
  protected:
-  UnitManipulators() {}
-  virtual ~UnitManipulators() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  UnitManipulators() = default;
+  ~UnitManipulators() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class UnitContainer : public ::testing::Test {
  protected:
-  UnitContainer() {}
-  virtual ~UnitContainer() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  UnitContainer() = default;
+  ~UnitContainer() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class UnitConversion : public ::testing::Test {
  protected:
-  UnitConversion() {}
-  virtual ~UnitConversion() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  UnitConversion() = default;
+  ~UnitConversion() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class UnitMath : public ::testing::Test {
  protected:
-  UnitMath() {}
-  virtual ~UnitMath() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  UnitMath() = default;
+  ~UnitMath() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class CompileTimeArithmetic : public ::testing::Test {
  protected:
-  CompileTimeArithmetic() {}
-  virtual ~CompileTimeArithmetic() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  CompileTimeArithmetic() = default;
+  ~CompileTimeArithmetic() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class Constexpr : public ::testing::Test {
  protected:
-  Constexpr() {}
-  virtual ~Constexpr() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  Constexpr() = default;
+  ~Constexpr() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 };
 
 class CaseStudies : public ::testing::Test {
  protected:
-  CaseStudies() {}
-  virtual ~CaseStudies() {}
-  virtual void SetUp() {}
-  virtual void TearDown() {}
+  CaseStudies() = default;
+  ~CaseStudies() override = default;
+  void SetUp() override {}
+  void TearDown() override {}
 
   struct RightTriangle {
     using a = unit_value_t<meters, 3>;
@@ -1327,7 +1324,7 @@
 }
 
 TEST_F(UnitContainer, valueMethod) {
-  double test = meter_t(3.0).to<double>();
+  double test = meter_t(3.0).value();
   EXPECT_DOUBLE_EQ(3.0, test);
 
   auto test2 = meter_t(4.0).value();
@@ -1336,11 +1333,11 @@
 }
 
 TEST_F(UnitContainer, convertMethod) {
-  double test = meter_t(3.0).convert<feet>().to<double>();
+  double test = meter_t(3.0).convert<feet>().value();
   EXPECT_NEAR(9.84252, test, 5.0e-6);
 }
 
-#ifndef UNIT_LIB_DISABLE_IOSTREAM
+#ifdef UNIT_LIB_ENABLE_IOSTREAM
 TEST_F(UnitContainer, cout) {
   testing::internal::CaptureStdout();
   std::cout << degree_t(349.87);
@@ -1421,6 +1418,88 @@
   EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
 #endif
 }
+#endif
+
+TEST_F(UnitContainer, fmtlib) {
+  testing::internal::CaptureStdout();
+  fmt::print("{}", degree_t(349.87));
+  std::string output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("349.87 deg", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", meter_t(1.0));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("1 m", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", dB_t(31.0));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("31 dB", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", volt_t(21.79));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("21.79 V", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", dBW_t(12.0));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("12 dBW", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", dBm_t(120.0));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("120 dBm", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", miles_per_hour_t(72.1));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("72.1 mph", output.c_str());
+
+  // undefined unit
+  testing::internal::CaptureStdout();
+  fmt::print("{}", units::math::cpow<4>(meter_t(2)));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("16 m^4", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{}", units::math::cpow<3>(foot_t(2)));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("8 cu_ft", output.c_str());
+
+  testing::internal::CaptureStdout();
+  fmt::print("{:.9}", units::math::cpow<4>(foot_t(2)));
+  output = testing::internal::GetCapturedStdout();
+  EXPECT_STREQ("0.138095597 m^4", output.c_str());
+
+  // constants
+  testing::internal::CaptureStdout();
+  fmt::print("{:.8}", constants::k_B);
+  output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+  EXPECT_STREQ("1.3806485e-023 m^2 kg s^-2 K^-1", output.c_str());
+#else
+  EXPECT_STREQ("1.3806485e-23 m^2 kg s^-2 K^-1", output.c_str());
+#endif
+
+  testing::internal::CaptureStdout();
+  fmt::print("{:.9}", constants::mu_B);
+  output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+  EXPECT_STREQ("9.27400999e-024 m^2 A", output.c_str());
+#else
+  EXPECT_STREQ("9.27400999e-24 m^2 A", output.c_str());
+#endif
+
+  testing::internal::CaptureStdout();
+  fmt::print("{:.7}", constants::sigma);
+  output = testing::internal::GetCapturedStdout();
+#if defined(_MSC_VER) && (_MSC_VER <= 1800)
+  EXPECT_STREQ("5.670367e-008 kg s^-3 K^-4", output.c_str());
+#else
+  EXPECT_STREQ("5.670367e-08 kg s^-3 K^-4", output.c_str());
+#endif
+}
 
 TEST_F(UnitContainer, to_string) {
   foot_t a(3.5);
@@ -1479,18 +1558,17 @@
   EXPECT_STREQ("m", b.abbreviation());
   EXPECT_STREQ("meter", b.name());
 }
-#endif
 
 TEST_F(UnitContainer, negative) {
   meter_t a(5.3);
   meter_t b(-5.3);
-  EXPECT_NEAR(a.to<double>(), -b.to<double>(), 5.0e-320);
-  EXPECT_NEAR(b.to<double>(), -a.to<double>(), 5.0e-320);
+  EXPECT_NEAR(a.value(), -b.value(), 5.0e-320);
+  EXPECT_NEAR(b.value(), -a.value(), 5.0e-320);
 
   dB_t c(2.87);
   dB_t d(-2.87);
-  EXPECT_NEAR(c.to<double>(), -d.to<double>(), 5.0e-320);
-  EXPECT_NEAR(d.to<double>(), -c.to<double>(), 5.0e-320);
+  EXPECT_NEAR(c.value(), -d.value(), 5.0e-320);
+  EXPECT_NEAR(d.value(), -c.value(), 5.0e-320);
 
   ppm_t e = -1 * ppm_t(10);
   EXPECT_EQ(e, -ppm_t(10));
@@ -1501,7 +1579,7 @@
   ppb_t a(ppm_t(1));
   EXPECT_EQ(ppb_t(1000), a);
   EXPECT_EQ(0.000001, a);
-  EXPECT_EQ(0.000001, a.to<double>());
+  EXPECT_EQ(0.000001, a.value());
 
   scalar_t b(ppm_t(1));
   EXPECT_EQ(0.000001, b);
@@ -1711,7 +1789,7 @@
 
   year_t twoYears(2.0);
   week_t twoYearsInWeeks = twoYears;
-  EXPECT_NEAR(week_t(104.286).to<double>(), twoYearsInWeeks.to<double>(),
+  EXPECT_NEAR(week_t(104.286).value(), twoYearsInWeeks.value(),
               5.0e-4);
 
   double test;
@@ -1748,8 +1826,8 @@
 TEST_F(UnitConversion, angle) {
   angle::degree_t quarterCircleDeg(90.0);
   angle::radian_t quarterCircleRad = quarterCircleDeg;
-  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).to<double>(),
-              quarterCircleRad.to<double>(), 5.0e-12);
+  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 2.0).value(),
+              quarterCircleRad.value(), 5.0e-12);
 
   double test;
 
@@ -2553,7 +2631,7 @@
   EXPECT_TRUE(constants::pi < 4.0);
 
   // explicit conversion
-  EXPECT_NEAR(3.14159, constants::pi.to<double>(), 5.0e-6);
+  EXPECT_NEAR(3.14159, constants::pi.value(), 5.0e-6);
 
   // auto multiplication
   EXPECT_TRUE(
@@ -2562,16 +2640,16 @@
       (std::is_same<meter_t, decltype(meter_t(1) * constants::pi)>::value));
 
   EXPECT_NEAR(constants::detail::PI_VAL,
-              (constants::pi * meter_t(1)).to<double>(), 5.0e-10);
+              (constants::pi * meter_t(1)).value(), 5.0e-10);
   EXPECT_NEAR(constants::detail::PI_VAL,
-              (meter_t(1) * constants::pi).to<double>(), 5.0e-10);
+              (meter_t(1) * constants::pi).value(), 5.0e-10);
 
   // explicit multiplication
   meter_t a = constants::pi * meter_t(1);
   meter_t b = meter_t(1) * constants::pi;
 
-  EXPECT_NEAR(constants::detail::PI_VAL, a.to<double>(), 5.0e-10);
-  EXPECT_NEAR(constants::detail::PI_VAL, b.to<double>(), 5.0e-10);
+  EXPECT_NEAR(constants::detail::PI_VAL, a.value(), 5.0e-10);
+  EXPECT_NEAR(constants::detail::PI_VAL, b.value(), 5.0e-10);
 
   // auto division
   EXPECT_TRUE(
@@ -2580,16 +2658,16 @@
       (std::is_same<second_t, decltype(second_t(1) / constants::pi)>::value));
 
   EXPECT_NEAR(constants::detail::PI_VAL,
-              (constants::pi / second_t(1)).to<double>(), 5.0e-10);
+              (constants::pi / second_t(1)).value(), 5.0e-10);
   EXPECT_NEAR(1.0 / constants::detail::PI_VAL,
-              (second_t(1) / constants::pi).to<double>(), 5.0e-10);
+              (second_t(1) / constants::pi).value(), 5.0e-10);
 
   // explicit
   hertz_t c = constants::pi / second_t(1);
   second_t d = second_t(1) / constants::pi;
 
-  EXPECT_NEAR(constants::detail::PI_VAL, c.to<double>(), 5.0e-10);
-  EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.to<double>(), 5.0e-10);
+  EXPECT_NEAR(constants::detail::PI_VAL, c.value(), 5.0e-10);
+  EXPECT_NEAR(1.0 / constants::detail::PI_VAL, d.value(), 5.0e-10);
 }
 
 TEST_F(UnitConversion, constants) {
@@ -2696,12 +2774,12 @@
       (std::is_same<
           typename std::decay<angle::radian_t>::type,
           typename std::decay<decltype(acos(scalar_t(0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(2).to<double>(),
-              acos(scalar_t(-0.41614683654)).to<double>(), 5.0e-11);
+  EXPECT_NEAR(angle::radian_t(2).value(),
+              acos(scalar_t(-0.41614683654)).value(), 5.0e-11);
   EXPECT_NEAR(
-      angle::degree_t(135).to<double>(),
+      angle::degree_t(135).value(),
       angle::degree_t(acos(scalar_t(-0.70710678118654752440084436210485)))
-          .to<double>(),
+          .value(),
       5.0e-12);
 }
 
@@ -2710,12 +2788,12 @@
       (std::is_same<
           typename std::decay<angle::radian_t>::type,
           typename std::decay<decltype(asin(scalar_t(0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(1.14159265).to<double>(),
-              asin(scalar_t(0.90929742682)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(angle::radian_t(1.14159265).value(),
+              asin(scalar_t(0.90929742682)).value(), 5.0e-9);
   EXPECT_NEAR(
-      angle::degree_t(45).to<double>(),
+      angle::degree_t(45).value(),
       angle::degree_t(asin(scalar_t(0.70710678118654752440084436210485)))
-          .to<double>(),
+          .value(),
       5.0e-12);
 }
 
@@ -2724,32 +2802,32 @@
       (std::is_same<
           typename std::decay<angle::radian_t>::type,
           typename std::decay<decltype(atan(scalar_t(0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(-1.14159265).to<double>(),
-              atan(scalar_t(-2.18503986326)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(angle::degree_t(-45).to<double>(),
-              angle::degree_t(atan(scalar_t(-1.0))).to<double>(), 5.0e-12);
+  EXPECT_NEAR(angle::radian_t(-1.14159265).value(),
+              atan(scalar_t(-2.18503986326)).value(), 5.0e-9);
+  EXPECT_NEAR(angle::degree_t(-45).value(),
+              angle::degree_t(atan(scalar_t(-1.0))).value(), 5.0e-12);
 }
 
 TEST_F(UnitMath, atan2) {
   EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
                             typename std::decay<decltype(atan2(
                                 scalar_t(1), scalar_t(1)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).to<double>(),
-              atan2(scalar_t(2), scalar_t(2)).to<double>(), 5.0e-12);
+  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 4).value(),
+              atan2(scalar_t(2), scalar_t(2)).value(), 5.0e-12);
   EXPECT_NEAR(
-      angle::degree_t(45).to<double>(),
-      angle::degree_t(atan2(scalar_t(2), scalar_t(2))).to<double>(),
+      angle::degree_t(45).value(),
+      angle::degree_t(atan2(scalar_t(2), scalar_t(2))).value(),
       5.0e-12);
 
   EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
                             typename std::decay<decltype(atan2(
                                 scalar_t(1), scalar_t(1)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).to<double>(),
-              atan2(scalar_t(1), scalar_t(std::sqrt(3))).to<double>(),
+  EXPECT_NEAR(angle::radian_t(constants::detail::PI_VAL / 6).value(),
+              atan2(scalar_t(1), scalar_t(std::sqrt(3))).value(),
               5.0e-12);
-  EXPECT_NEAR(angle::degree_t(30).to<double>(),
+  EXPECT_NEAR(angle::degree_t(30).value(),
               angle::degree_t(atan2(scalar_t(1), scalar_t(std::sqrt(3))))
-                  .to<double>(),
+                  .value(),
               5.0e-12);
 }
 
@@ -2781,30 +2859,30 @@
   EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
                             typename std::decay<decltype(
                                 acosh(scalar_t(0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(1.316957896924817).to<double>(),
-              acosh(scalar_t(2.0)).to<double>(), 5.0e-11);
-  EXPECT_NEAR(angle::degree_t(75.456129290216893).to<double>(),
-              angle::degree_t(acosh(scalar_t(2.0))).to<double>(), 5.0e-12);
+  EXPECT_NEAR(angle::radian_t(1.316957896924817).value(),
+              acosh(scalar_t(2.0)).value(), 5.0e-11);
+  EXPECT_NEAR(angle::degree_t(75.456129290216893).value(),
+              angle::degree_t(acosh(scalar_t(2.0))).value(), 5.0e-12);
 }
 
 TEST_F(UnitMath, asinh) {
   EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
                             typename std::decay<decltype(
                                 asinh(scalar_t(0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(1.443635475178810).to<double>(),
-              asinh(scalar_t(2)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(angle::degree_t(82.714219883108939).to<double>(),
-              angle::degree_t(asinh(scalar_t(2))).to<double>(), 5.0e-12);
+  EXPECT_NEAR(angle::radian_t(1.443635475178810).value(),
+              asinh(scalar_t(2)).value(), 5.0e-9);
+  EXPECT_NEAR(angle::degree_t(82.714219883108939).value(),
+              angle::degree_t(asinh(scalar_t(2))).value(), 5.0e-12);
 }
 
 TEST_F(UnitMath, atanh) {
   EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
                             typename std::decay<decltype(
                                 atanh(scalar_t(0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(0.549306144334055).to<double>(),
-              atanh(scalar_t(0.5)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(angle::degree_t(31.472923730945389).to<double>(),
-              angle::degree_t(atanh(scalar_t(0.5))).to<double>(), 5.0e-12);
+  EXPECT_NEAR(angle::radian_t(0.549306144334055).value(),
+              atanh(scalar_t(0.5)).value(), 5.0e-9);
+  EXPECT_NEAR(angle::degree_t(31.472923730945389).value(),
+              angle::degree_t(atanh(scalar_t(0.5))).value(), 5.0e-12);
 }
 
 TEST_F(UnitMath, exp) {
@@ -2876,14 +2954,14 @@
   EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
                             typename std::decay<decltype(sqrt(
                                 square_meter_t(4.0)))>::type>::value));
-  EXPECT_NEAR(meter_t(2.0).to<double>(),
-              sqrt(square_meter_t(4.0)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(meter_t(2.0).value(),
+              sqrt(square_meter_t(4.0)).value(), 5.0e-9);
 
   EXPECT_TRUE((std::is_same<typename std::decay<angle::radian_t>::type,
                             typename std::decay<decltype(
                                 sqrt(steradian_t(16.0)))>::type>::value));
-  EXPECT_NEAR(angle::radian_t(4.0).to<double>(),
-              sqrt(steradian_t(16.0)).to<double>(), 5.0e-9);
+  EXPECT_NEAR(angle::radian_t(4.0).value(),
+              sqrt(steradian_t(16.0)).value(), 5.0e-9);
 
   EXPECT_TRUE((std::is_convertible<typename std::decay<foot_t>::type,
                                    typename std::decay<decltype(sqrt(
@@ -2892,9 +2970,9 @@
   // for rational conversion (i.e. no integral root) let's check a bunch of
   // different ways this could go wrong
   foot_t resultFt = sqrt(square_foot_t(10.0));
-  EXPECT_NEAR(foot_t(3.16227766017).to<double>(),
-              sqrt(square_foot_t(10.0)).to<double>(), 5.0e-9);
-  EXPECT_NEAR(foot_t(3.16227766017).to<double>(), resultFt.to<double>(),
+  EXPECT_NEAR(foot_t(3.16227766017).value(),
+              sqrt(square_foot_t(10.0)).value(), 5.0e-9);
+  EXPECT_NEAR(foot_t(3.16227766017).value(), resultFt.value(),
               5.0e-9);
   EXPECT_EQ(resultFt, sqrt(square_foot_t(10.0)));
 }
@@ -2903,19 +2981,19 @@
   EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
                             typename std::decay<decltype(hypot(
                                 meter_t(3.0), meter_t(4.0)))>::type>::value));
-  EXPECT_NEAR(meter_t(5.0).to<double>(),
-              (hypot(meter_t(3.0), meter_t(4.0))).to<double>(), 5.0e-9);
+  EXPECT_NEAR(meter_t(5.0).value(),
+              (hypot(meter_t(3.0), meter_t(4.0))).value(), 5.0e-9);
 
   EXPECT_TRUE((std::is_same<typename std::decay<foot_t>::type,
                             typename std::decay<decltype(hypot(
                                 foot_t(3.0), meter_t(1.2192)))>::type>::value));
-  EXPECT_NEAR(foot_t(5.0).to<double>(),
-              (hypot(foot_t(3.0), meter_t(1.2192))).to<double>(), 5.0e-9);
+  EXPECT_NEAR(foot_t(5.0).value(),
+              (hypot(foot_t(3.0), meter_t(1.2192))).value(), 5.0e-9);
 }
 
 TEST_F(UnitMath, ceil) {
   double val = 101.1;
-  EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).to<double>());
+  EXPECT_EQ(std::ceil(val), ceil(meter_t(val)).value());
   EXPECT_TRUE((std::is_same<typename std::decay<meter_t>::type,
                             typename std::decay<decltype(
                                 ceil(meter_t(val)))>::type>::value));
@@ -2928,7 +3006,7 @@
 
 TEST_F(UnitMath, fmod) {
   EXPECT_EQ(std::fmod(100.0, 101.2),
-            fmod(meter_t(100.0), meter_t(101.2)).to<double>());
+            fmod(meter_t(100.0), meter_t(101.2)).value());
 }
 
 TEST_F(UnitMath, trunc) {
@@ -2951,8 +3029,8 @@
 TEST_F(UnitMath, fdim) {
   EXPECT_EQ(meter_t(0.0), fdim(meter_t(8.0), meter_t(10.0)));
   EXPECT_EQ(meter_t(2.0), fdim(meter_t(10.0), meter_t(8.0)));
-  EXPECT_NEAR(meter_t(9.3904).to<double>(),
-              fdim(meter_t(10.0), foot_t(2.0)).to<double>(),
+  EXPECT_NEAR(meter_t(9.3904).value(),
+              fdim(meter_t(10.0), foot_t(2.0)).value(),
               5.0e-320);  // not sure why they aren't comparing exactly equal,
                           // but clearly they are.
 }
@@ -2979,17 +3057,6 @@
   EXPECT_EQ(meter_t(10.0), abs(meter_t(10.0)));
 }
 
-TEST_F(UnitMath, normalize) {
-  EXPECT_EQ(NormalizeAngle(radian_t(5 * wpi::math::pi)),
-      radian_t(wpi::math::pi));
-  EXPECT_EQ(NormalizeAngle(radian_t(-5 * wpi::math::pi)),
-      radian_t(wpi::math::pi));
-  EXPECT_EQ(NormalizeAngle(radian_t(wpi::math::pi / 2)),
-      radian_t(wpi::math::pi / 2));
-  EXPECT_EQ(NormalizeAngle(radian_t(-wpi::math::pi / 2)),
-      radian_t(-wpi::math::pi / 2));
-}
-
 // Constexpr
 #if !defined(_MSC_VER) || _MSC_VER > 1800
 TEST_F(Constexpr, construction) {
@@ -3096,7 +3163,7 @@
 }
 
 TEST_F(CompileTimeArithmetic, is_unit_value_t) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
+  using mRatio = unit_value_t<meters, 3, 2>;
 
   EXPECT_TRUE((traits::is_unit_value_t<mRatio>::value));
   EXPECT_FALSE((traits::is_unit_value_t<meter_t>::value));
@@ -3108,7 +3175,7 @@
 }
 
 TEST_F(CompileTimeArithmetic, is_unit_value_t_category) {
-  typedef unit_value_t<feet, 3, 2> mRatio;
+  using mRatio = unit_value_t<feet, 3, 2>;
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, mRatio>::value));
   EXPECT_FALSE(
@@ -3120,90 +3187,90 @@
 }
 
 TEST_F(CompileTimeArithmetic, unit_value_add) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
+  using mRatio = unit_value_t<meters, 3, 2>;
 
   using sum = unit_value_add<mRatio, mRatio>;
   EXPECT_EQ(meter_t(3.0), sum::value());
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, sum>::value));
 
-  typedef unit_value_t<feet, 1> ftRatio;
+  using ftRatio = unit_value_t<feet, 1>;
 
   using sumf = unit_value_add<ftRatio, mRatio>;
   EXPECT_TRUE((
       std::is_same<typename std::decay<foot_t>::type,
                    typename std::decay<decltype(sumf::value())>::type>::value));
-  EXPECT_NEAR(5.92125984, sumf::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(5.92125984, sumf::value().value(), 5.0e-8);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, sumf>::value));
 
-  typedef unit_value_t<celsius, 1> cRatio;
-  typedef unit_value_t<fahrenheit, 2> fRatio;
+  using cRatio = unit_value_t<celsius, 1>;
+  using fRatio = unit_value_t<fahrenheit, 2>;
 
   using sumc = unit_value_add<cRatio, fRatio>;
   EXPECT_TRUE((
       std::is_same<typename std::decay<celsius_t>::type,
                    typename std::decay<decltype(sumc::value())>::type>::value));
-  EXPECT_NEAR(2.11111111111, sumc::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(2.11111111111, sumc::value().value(), 5.0e-8);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
                                                 sumc>::value));
 
-  typedef unit_value_t<angle::radian, 1> rRatio;
-  typedef unit_value_t<angle::degree, 3> dRatio;
+  using rRatio = unit_value_t<angle::radian, 1>;
+  using dRatio = unit_value_t<angle::degree, 3>;
 
   using sumr = unit_value_add<rRatio, dRatio>;
   EXPECT_TRUE((
       std::is_same<typename std::decay<angle::radian_t>::type,
                    typename std::decay<decltype(sumr::value())>::type>::value));
-  EXPECT_NEAR(1.05235988, sumr::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(1.05235988, sumr::value().value(), 5.0e-8);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::angle_unit, sumr>::value));
 }
 
 TEST_F(CompileTimeArithmetic, unit_value_subtract) {
-  typedef unit_value_t<meters, 3, 2> mRatio;
+  using mRatio = unit_value_t<meters, 3, 2>;
 
   using diff = unit_value_subtract<mRatio, mRatio>;
   EXPECT_EQ(meter_t(0), diff::value());
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, diff>::value));
 
-  typedef unit_value_t<feet, 1> ftRatio;
+  using ftRatio = unit_value_t<feet, 1>;
 
   using difff = unit_value_subtract<ftRatio, mRatio>;
   EXPECT_TRUE((std::is_same<
                typename std::decay<foot_t>::type,
                typename std::decay<decltype(difff::value())>::type>::value));
-  EXPECT_NEAR(-3.92125984, difff::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(-3.92125984, difff::value().value(), 5.0e-8);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, difff>::value));
 
-  typedef unit_value_t<celsius, 1> cRatio;
-  typedef unit_value_t<fahrenheit, 2> fRatio;
+  using cRatio = unit_value_t<celsius, 1>;
+  using fRatio = unit_value_t<fahrenheit, 2>;
 
   using diffc = unit_value_subtract<cRatio, fRatio>;
   EXPECT_TRUE((std::is_same<
                typename std::decay<celsius_t>::type,
                typename std::decay<decltype(diffc::value())>::type>::value));
-  EXPECT_NEAR(-0.11111111111, diffc::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(-0.11111111111, diffc::value().value(), 5.0e-8);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::temperature_unit,
                                                 diffc>::value));
 
-  typedef unit_value_t<angle::radian, 1> rRatio;
-  typedef unit_value_t<angle::degree, 3> dRatio;
+  using rRatio = unit_value_t<angle::radian, 1>;
+  using dRatio = unit_value_t<angle::degree, 3>;
 
   using diffr = unit_value_subtract<rRatio, dRatio>;
   EXPECT_TRUE((std::is_same<
                typename std::decay<angle::radian_t>::type,
                typename std::decay<decltype(diffr::value())>::type>::value));
-  EXPECT_NEAR(0.947640122, diffr::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(0.947640122, diffr::value().value(), 5.0e-8);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::angle_unit, diffr>::value));
 }
 
 TEST_F(CompileTimeArithmetic, unit_value_multiply) {
-  typedef unit_value_t<meters, 2> mRatio;
-  typedef unit_value_t<feet, 656168, 100000> ftRatio;  // 2 meter
+  using mRatio = unit_value_t<meters, 2>;
+  using ftRatio = unit_value_t<feet, 656168, 100000>;  // 2 meter
 
   using product = unit_value_multiply<mRatio, mRatio>;
   EXPECT_EQ(square_meter_t(4), product::value());
@@ -3215,7 +3282,7 @@
   EXPECT_TRUE((std::is_same<
                typename std::decay<square_meter_t>::type,
                typename std::decay<decltype(productM::value())>::type>::value));
-  EXPECT_NEAR(4.0, productM::value().to<double>(), 5.0e-7);
+  EXPECT_NEAR(4.0, productM::value().value(), 5.0e-7);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::area_unit, productM>::value));
 
@@ -3223,7 +3290,7 @@
   EXPECT_TRUE((std::is_same<
                typename std::decay<square_foot_t>::type,
                typename std::decay<decltype(productF::value())>::type>::value));
-  EXPECT_NEAR(43.0556444224, productF::value().to<double>(), 5.0e-6);
+  EXPECT_NEAR(43.0556444224, productF::value().value(), 5.0e-6);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::area_unit, productF>::value));
 
@@ -3232,11 +3299,11 @@
       (std::is_same<
           typename std::decay<square_foot_t>::type,
           typename std::decay<decltype(productF2::value())>::type>::value));
-  EXPECT_NEAR(43.0556444224, productF2::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(43.0556444224, productF2::value().value(), 5.0e-8);
   EXPECT_TRUE((
       traits::is_unit_value_t_category<category::area_unit, productF2>::value));
 
-  typedef unit_value_t<units::force::newton, 5> nRatio;
+  using nRatio = unit_value_t<units::force::newton, 5>;
 
   using productN = unit_value_multiply<nRatio, ftRatio>;
   EXPECT_FALSE(
@@ -3246,30 +3313,30 @@
   EXPECT_TRUE((std::is_convertible<
                typename std::decay<torque::newton_meter_t>::type,
                typename std::decay<decltype(productN::value())>::type>::value));
-  EXPECT_NEAR(32.8084, productN::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().to<double>()),
+  EXPECT_NEAR(32.8084, productN::value().value(), 5.0e-8);
+  EXPECT_NEAR(10.0, (productN::value().convert<newton_meter>().value()),
               5.0e-7);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::torque_unit,
                                                 productN>::value));
 
-  typedef unit_value_t<angle::radian, 11, 10> r1Ratio;
-  typedef unit_value_t<angle::radian, 22, 10> r2Ratio;
+  using r1Ratio = unit_value_t<angle::radian, 11, 10>;
+  using r2Ratio = unit_value_t<angle::radian, 22, 10>;
 
   using productR = unit_value_multiply<r1Ratio, r2Ratio>;
   EXPECT_TRUE((std::is_same<
                typename std::decay<steradian_t>::type,
                typename std::decay<decltype(productR::value())>::type>::value));
-  EXPECT_NEAR(2.42, productR::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(2.42, productR::value().value(), 5.0e-8);
   EXPECT_NEAR(7944.39137,
-              (productR::value().convert<degrees_squared>().to<double>()),
+              (productR::value().convert<degrees_squared>().value()),
               5.0e-6);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
                                                 productR>::value));
 }
 
 TEST_F(CompileTimeArithmetic, unit_value_divide) {
-  typedef unit_value_t<meters, 2> mRatio;
-  typedef unit_value_t<feet, 656168, 100000> ftRatio;  // 2 meter
+  using mRatio = unit_value_t<meters, 2>;
+  using ftRatio = unit_value_t<feet, 656168, 100000>;  // 2 meter
 
   using product = unit_value_divide<mRatio, mRatio>;
   EXPECT_EQ(scalar_t(1), product::value());
@@ -3281,7 +3348,7 @@
   EXPECT_TRUE((std::is_same<
                typename std::decay<scalar_t>::type,
                typename std::decay<decltype(productM::value())>::type>::value));
-  EXPECT_NEAR(1, productM::value().to<double>(), 5.0e-7);
+  EXPECT_NEAR(1, productM::value().value(), 5.0e-7);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
                                                 productM>::value));
 
@@ -3289,7 +3356,7 @@
   EXPECT_TRUE((std::is_same<
                typename std::decay<scalar_t>::type,
                typename std::decay<decltype(productF::value())>::type>::value));
-  EXPECT_NEAR(1.0, productF::value().to<double>(), 5.0e-6);
+  EXPECT_NEAR(1.0, productF::value().value(), 5.0e-6);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
                                                 productF>::value));
 
@@ -3298,90 +3365,90 @@
       (std::is_same<
           typename std::decay<scalar_t>::type,
           typename std::decay<decltype(productF2::value())>::type>::value));
-  EXPECT_NEAR(1.0, productF2::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(1.0, productF2::value().value(), 5.0e-8);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::scalar_unit,
                                                 productF2>::value));
 
-  typedef unit_value_t<seconds, 10> sRatio;
+  using sRatio = unit_value_t<seconds, 10>;
 
   using productMS = unit_value_divide<mRatio, sRatio>;
   EXPECT_TRUE(
       (std::is_same<
           typename std::decay<meters_per_second_t>::type,
           typename std::decay<decltype(productMS::value())>::type>::value));
-  EXPECT_NEAR(0.2, productMS::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(0.2, productMS::value().value(), 5.0e-8);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::velocity_unit,
                                                 productMS>::value));
 
-  typedef unit_value_t<angle::radian, 20> rRatio;
+  using rRatio = unit_value_t<angle::radian, 20>;
 
   using productRS = unit_value_divide<rRatio, sRatio>;
   EXPECT_TRUE(
       (std::is_same<
           typename std::decay<radians_per_second_t>::type,
           typename std::decay<decltype(productRS::value())>::type>::value));
-  EXPECT_NEAR(2, productRS::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(2, productRS::value().value(), 5.0e-8);
   EXPECT_NEAR(114.592,
-              (productRS::value().convert<degrees_per_second>().to<double>()),
+              (productRS::value().convert<degrees_per_second>().value()),
               5.0e-4);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::angular_velocity_unit,
                                                 productRS>::value));
 }
 
 TEST_F(CompileTimeArithmetic, unit_value_power) {
-  typedef unit_value_t<meters, 2> mRatio;
+  using mRatio = unit_value_t<meters, 2>;
 
   using sq = unit_value_power<mRatio, 2>;
   EXPECT_TRUE((std::is_convertible<
                typename std::decay<square_meter_t>::type,
                typename std::decay<decltype(sq::value())>::type>::value));
-  EXPECT_NEAR(4, sq::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(4, sq::value().value(), 5.0e-8);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::area_unit, sq>::value));
 
-  typedef unit_value_t<angle::radian, 18, 10> rRatio;
+  using rRatio = unit_value_t<angle::radian, 18, 10>;
 
   using sqr = unit_value_power<rRatio, 2>;
   EXPECT_TRUE((std::is_convertible<
                typename std::decay<steradian_t>::type,
                typename std::decay<decltype(sqr::value())>::type>::value));
-  EXPECT_NEAR(3.24, sqr::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(3.24, sqr::value().value(), 5.0e-8);
   EXPECT_NEAR(10636.292574038049895092690529904,
-              (sqr::value().convert<degrees_squared>().to<double>()), 5.0e-10);
+              (sqr::value().convert<degrees_squared>().value()), 5.0e-10);
   EXPECT_TRUE((traits::is_unit_value_t_category<category::solid_angle_unit,
                                                 sqr>::value));
 }
 
 TEST_F(CompileTimeArithmetic, unit_value_sqrt) {
-  typedef unit_value_t<square_meters, 10> mRatio;
+  using mRatio = unit_value_t<square_meters, 10>;
 
   using root = unit_value_sqrt<mRatio>;
   EXPECT_TRUE((std::is_convertible<
                typename std::decay<meter_t>::type,
                typename std::decay<decltype(root::value())>::type>::value));
-  EXPECT_NEAR(3.16227766017, root::value().to<double>(), 5.0e-9);
+  EXPECT_NEAR(3.16227766017, root::value().value(), 5.0e-9);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, root>::value));
 
-  typedef unit_value_t<hectare, 51, 7> hRatio;
+  using hRatio = unit_value_t<hectare, 51, 7>;
 
   using rooth = unit_value_sqrt<hRatio, 100000000>;
   EXPECT_TRUE((std::is_convertible<
                typename std::decay<mile_t>::type,
                typename std::decay<decltype(rooth::value())>::type>::value));
-  EXPECT_NEAR(2.69920623253, rooth::value().to<double>(), 5.0e-8);
-  EXPECT_NEAR(269.920623, rooth::value().convert<meters>().to<double>(),
+  EXPECT_NEAR(2.69920623253, rooth::value().value(), 5.0e-8);
+  EXPECT_NEAR(269.920623, rooth::value().convert<meters>().value(),
               5.0e-6);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::length_unit, rooth>::value));
 
-  typedef unit_value_t<steradian, 18, 10> rRatio;
+  using rRatio = unit_value_t<steradian, 18, 10>;
 
   using rootr = unit_value_sqrt<rRatio>;
   EXPECT_TRUE((traits::is_angle_unit<decltype(rootr::value())>::value));
-  EXPECT_NEAR(1.3416407865, rootr::value().to<double>(), 5.0e-8);
+  EXPECT_NEAR(1.3416407865, rootr::value().value(), 5.0e-8);
   EXPECT_NEAR(76.870352574,
-              rootr::value().convert<angle::degrees>().to<double>(), 5.0e-6);
+              rootr::value().convert<angle::degrees>().value(), 5.0e-6);
   EXPECT_TRUE(
       (traits::is_unit_value_t_category<category::angle_unit, rootr>::value));
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
index 79f1a6d..354ed18 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ControlAffinePlantInversionFeedforwardTest.cpp
@@ -1,70 +1,52 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
 #include <cmath>
 
 #include "Eigen/Core"
-#include "frc/StateSpaceUtil.h"
 #include "frc/controller/ControlAffinePlantInversionFeedforward.h"
 #include "units/time.h"
 
 namespace frc {
 
-Eigen::Matrix<double, 2, 1> Dynamics(const Eigen::Matrix<double, 2, 1>& x,
-                                     const Eigen::Matrix<double, 1, 1>& u) {
-  Eigen::Matrix<double, 2, 1> result;
-
-  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x) +
-           (frc::MakeMatrix<2, 1>(0.0, 1.0) * u);
-
-  return result;
+Eigen::Vector<double, 2> Dynamics(const Eigen::Vector<double, 2>& x,
+                                  const Eigen::Vector<double, 1>& u) {
+  return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x +
+         Eigen::Matrix<double, 2, 1>{0.0, 1.0} * u;
 }
 
-Eigen::Matrix<double, 2, 1> StateDynamics(
-    const Eigen::Matrix<double, 2, 1>& x) {
-  Eigen::Matrix<double, 2, 1> result;
-
-  result = (frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0) * x);
-
-  return result;
+Eigen::Vector<double, 2> StateDynamics(const Eigen::Vector<double, 2>& x) {
+  return Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}} * x;
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, Calculate) {
-  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&,
-                                            const Eigen::Matrix<double, 1, 1>&)>
+  std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&,
+                                         const Eigen::Vector<double, 1>&)>
       modelDynamics = [](auto& x, auto& u) { return Dynamics(x, u); };
 
   frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
-      modelDynamics, units::second_t(0.02)};
+      modelDynamics, units::second_t{0.02}};
 
-  Eigen::Matrix<double, 2, 1> r;
-  r << 2, 2;
-  Eigen::Matrix<double, 2, 1> nextR;
-  nextR << 3, 3;
+  Eigen::Vector<double, 2> r{2, 2};
+  Eigen::Vector<double, 2> nextR{3, 3};
 
   EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
 }
 
 TEST(ControlAffinePlantInversionFeedforwardTest, CalculateState) {
-  std::function<Eigen::Matrix<double, 2, 1>(const Eigen::Matrix<double, 2, 1>&)>
+  std::function<Eigen::Vector<double, 2>(const Eigen::Vector<double, 2>&)>
       modelDynamics = [](auto& x) { return StateDynamics(x); };
 
-  Eigen::Matrix<double, 2, 1> B;
-  B << 0, 1;
+  Eigen::Matrix<double, 2, 1> B{0, 1};
 
   frc::ControlAffinePlantInversionFeedforward<2, 1> feedforward{
       modelDynamics, B, units::second_t(0.02)};
 
-  Eigen::Matrix<double, 2, 1> r;
-  r << 2, 2;
-  Eigen::Matrix<double, 2, 1> nextR;
-  nextR << 3, 3;
+  Eigen::Vector<double, 2> r{2, 2};
+  Eigen::Vector<double, 2> nextR{3, 3};
 
   EXPECT_NEAR(48, feedforward.Calculate(r, nextR)(0, 0), 1e-6);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
new file mode 100644
index 0000000..c6b669c
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/HolonomicDriveControllerTest.cpp
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/numbers>
+
+#include "frc/MathUtil.h"
+#include "frc/controller/HolonomicDriveController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/angular_acceleration.h"
+#include "units/math.h"
+#include "units/time.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::numbers::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{
+              units::radians_per_second_t{2.0 * wpi::numbers::pi},
+              units::radians_per_second_squared_t{wpi::numbers::pi}}}};
+
+  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).value(); ++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(frc::AngleModulus(robotPose.Rotation().Radians()), 0_rad,
+                    kAngularTolerance);
+}
+
+TEST(HolonomicDriveControllerTest, DoesNotRotateUnnecessarily) {
+  frc::HolonomicDriveController controller{
+      frc2::PIDController{1, 0, 0}, frc2::PIDController{1, 0, 0},
+      frc::ProfiledPIDController<units::radian>{
+          1, 0, 0,
+          frc::TrapezoidProfile<units::radian>::Constraints{
+              4_rad_per_s, 2_rad_per_s / 1_s}}};
+
+  frc::ChassisSpeeds speeds = controller.Calculate(
+      frc::Pose2d(0_m, 0_m, 1.57_rad), frc::Pose2d(), 0_mps, 1.57_rad);
+
+  EXPECT_EQ(0, speeds.omega.value());
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
index abc22d9..6e61706 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearPlantInversionFeedforwardTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -16,19 +13,14 @@
 namespace frc {
 
 TEST(LinearPlantInversionFeedforwardTest, Calculate) {
-  Eigen::Matrix<double, 2, 2> A;
-  A << 1, 0, 0, 1;
-
-  Eigen::Matrix<double, 2, 1> B;
-  B << 0, 1;
+  Eigen::Matrix<double, 2, 2> A{{1, 0}, {0, 1}};
+  Eigen::Matrix<double, 2, 1> B{0, 1};
 
   frc::LinearPlantInversionFeedforward<2, 1> feedforward{A, B,
                                                          units::second_t(0.02)};
 
-  Eigen::Matrix<double, 2, 1> r;
-  r << 2, 2;
-  Eigen::Matrix<double, 2, 1> nextR;
-  nextR << 3, 3;
+  Eigen::Vector<double, 2> r{2, 2};
+  Eigen::Vector<double, 2> nextR{3, 3};
 
   EXPECT_NEAR(47.502599, feedforward.Calculate(r, nextR)(0, 0), 0.002);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
index eecaf34..8c52cd0 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/LinearQuadraticRegulatorTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -85,4 +82,27 @@
   EXPECT_NEAR(0.69, controller.K(0, 1), 1e-1);
 }
 
+TEST(LinearQuadraticRegulatorTest, LatencyCompensate) {
+  LinearSystem<2, 1, 1> plant = [] {
+    auto motors = DCMotor::Vex775Pro(4);
+
+    // Carriage mass
+    constexpr auto m = 8_kg;
+
+    // Radius of pulley
+    constexpr auto r = 0.75_in;
+
+    // Gear ratio
+    constexpr double G = 14.67;
+
+    return frc::LinearSystemId::ElevatorSystem(motors, m, r, G);
+  }();
+  LinearQuadraticRegulator<2, 1> controller{plant, {0.1, 0.2}, {12.0}, 0.02_s};
+
+  controller.LatencyCompensate(plant, 0.02_s, 0.01_s);
+
+  EXPECT_NEAR(8.97115941, controller.K(0, 0), 1e-3);
+  EXPECT_NEAR(0.07904881, controller.K(0, 1), 1e-3);
+}
+
 }  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
new file mode 100644
index 0000000..379db9e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/PIDInputOutputTest.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+class PIDInputOutputTest : public testing::Test {
+ protected:
+  frc2::PIDController* controller;
+
+  void SetUp() override { controller = new frc2::PIDController(0, 0, 0); }
+
+  void TearDown() override { delete controller; }
+};
+
+TEST_F(PIDInputOutputTest, ContinuousInput) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-180, 180);
+  EXPECT_DOUBLE_EQ(controller->Calculate(-179, 179), -2);
+
+  controller->EnableContinuousInput(0, 360);
+  EXPECT_DOUBLE_EQ(controller->Calculate(1, 359), -2);
+}
+
+TEST_F(PIDInputOutputTest, ProportionalGainOutput) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025, 0));
+}
+
+TEST_F(PIDInputOutputTest, IntegralGainOutput) {
+  controller->SetI(4);
+
+  double out = 0;
+
+  for (int i = 0; i < 5; i++) {
+    out = controller->Calculate(0.025, 0);
+  }
+
+  EXPECT_DOUBLE_EQ(-0.5 * controller->GetPeriod().value(), out);
+}
+
+TEST_F(PIDInputOutputTest, DerivativeGainOutput) {
+  controller->SetD(4);
+
+  controller->Calculate(0, 0);
+
+  EXPECT_DOUBLE_EQ(-10_ms / controller->GetPeriod(),
+                   controller->Calculate(0.0025, 0));
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
new file mode 100644
index 0000000..0aec438
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/PIDToleranceTest.cpp
@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/controller/PIDController.h"
+#include "gtest/gtest.h"
+
+static constexpr double kSetpoint = 50.0;
+static constexpr double kRange = 200;
+static constexpr double kTolerance = 10.0;
+
+TEST(PIDToleranceTest, InitialTolerance) {
+  frc2::PIDController controller{0.5, 0.0, 0.0};
+  controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+  EXPECT_TRUE(controller.AtSetpoint());
+}
+
+TEST(PIDToleranceTest, AbsoluteTolerance) {
+  frc2::PIDController controller{0.5, 0.0, 0.0};
+  controller.EnableContinuousInput(-kRange / 2, kRange / 2);
+
+  EXPECT_TRUE(controller.AtSetpoint())
+      << "Error was not in tolerance when it should have been. Error was "
+      << controller.GetPositionError();
+
+  controller.SetTolerance(kTolerance);
+  controller.SetSetpoint(kSetpoint);
+
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+
+  controller.Calculate(0.0);
+
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+
+  controller.Calculate(kSetpoint + kTolerance / 2);
+
+  EXPECT_TRUE(controller.AtSetpoint())
+      << "Error was not in tolerance when it should have been. Error was "
+      << controller.GetPositionError();
+
+  controller.Calculate(kSetpoint + 10 * kTolerance);
+
+  EXPECT_FALSE(controller.AtSetpoint())
+      << "Error was in tolerance when it should not have been. Error was "
+      << controller.GetPositionError();
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
new file mode 100644
index 0000000..da402ae
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/ProfiledPIDInputOutputTest.cpp
@@ -0,0 +1,117 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/numbers>
+
+#include "frc/controller/ProfiledPIDController.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/angular_acceleration.h"
+#include "units/angular_velocity.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, ContinuousInput1) {
+  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, ContinuousInput2) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
+                                    units::radian_t{wpi::numbers::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::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInput3) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(-units::radian_t{wpi::numbers::pi},
+                                    units::radian_t{wpi::numbers::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::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ContinuousInput4) {
+  controller->SetP(1);
+  controller->EnableContinuousInput(0_rad,
+                                    units::radian_t{2.0 * wpi::numbers::pi});
+
+  static constexpr units::radian_t kSetpoint{2.78};
+  static constexpr units::radian_t kMeasurement{3.12};
+  static constexpr units::radian_t kGoal{2.71};
+
+  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::numbers::pi});
+}
+
+TEST_F(ProfiledPIDInputOutputTest, ProportionalGainOutput) {
+  controller->SetP(4);
+
+  EXPECT_DOUBLE_EQ(-0.1, controller->Calculate(0.025_deg, 0_deg));
+}
+
+TEST_F(ProfiledPIDInputOutputTest, IntegralGainOutput) {
+  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().value(), out);
+}
+
+TEST_F(ProfiledPIDInputOutputTest, DerivativeGainOutput) {
+  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/third_party/allwpilib/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
new file mode 100644
index 0000000..5e297f4
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/RamseteControllerTest.cpp
@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/MathUtil.h"
+#include "frc/controller/RamseteController.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/math.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::numbers::pi /
+                                                   180.0};
+
+TEST(RamseteControllerTest, ReachesReference) {
+  frc::RamseteController controller{2.0, 0.7};
+  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.8_mps, 0.1_mps_sq});
+
+  constexpr auto kDt = 0.02_s;
+  auto totalTime = trajectory.TotalTime();
+  for (size_t i = 0; i < (totalTime / kDt).value(); ++i) {
+    auto state = trajectory.Sample(kDt * i);
+    auto [vx, vy, omega] = controller.Calculate(robotPose, state);
+    static_cast<void>(vy);
+
+    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, 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(frc::AngleModulus(endPose.Rotation().Radians() -
+                                      robotPose.Rotation().Radians()),
+                    0_rad, kAngularTolerance);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
new file mode 100644
index 0000000..3cf944e
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/controller/SimpleMotorFeedforwardTest.cpp
@@ -0,0 +1,46 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "Eigen/Core"
+#include "frc/controller/LinearPlantInversionFeedforward.h"
+#include "frc/controller/SimpleMotorFeedforward.h"
+#include "units/acceleration.h"
+#include "units/length.h"
+#include "units/time.h"
+
+namespace frc {
+
+TEST(SimpleMotorFeedforwardTest, Calculate) {
+  double Ks = 0.5;
+  double Kv = 3.0;
+  double Ka = 0.6;
+  auto dt = 0.02_s;
+
+  Eigen::Matrix<double, 1, 1> A{-Kv / Ka};
+  Eigen::Matrix<double, 1, 1> B{1.0 / Ka};
+
+  frc::LinearPlantInversionFeedforward<1, 1> plantInversion{A, B, dt};
+  frc::SimpleMotorFeedforward<units::meter> simpleMotor{
+      units::volt_t{Ks}, units::volt_t{Kv} / 1_mps,
+      units::volt_t{Ka} / 1_mps_sq};
+
+  Eigen::Vector<double, 1> r{2.0};
+  Eigen::Vector<double, 1> nextR{3.0};
+
+  EXPECT_NEAR(37.524995834325161 + Ks,
+              simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
+              simpleMotor.Calculate(2_mps, 3_mps, dt).value(), 0.002);
+
+  // These won't match exactly. It's just an approximation to make sure they're
+  // in the same ballpark.
+  EXPECT_NEAR(plantInversion.Calculate(r, nextR)(0) + Ks,
+              simpleMotor.Calculate(2_mps, 1_mps / dt).value(), 2.0);
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
new file mode 100644
index 0000000..8631d6f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/drake/discrete_algebraic_riccati_equation_test.cpp
@@ -0,0 +1,124 @@
+#include "drake/math/discrete_algebraic_riccati_equation.h"
+
+#include <Eigen/Eigenvalues>
+#include <gtest/gtest.h>
+
+#include "drake/common/test_utilities/eigen_matrix_compare.h"
+// #include "drake/math/autodiff.h"
+
+using Eigen::MatrixXd;
+
+namespace drake {
+namespace math {
+namespace {
+void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
+                        const Eigen::Ref<const MatrixXd>& B,
+                        const Eigen::Ref<const MatrixXd>& Q,
+                        const Eigen::Ref<const MatrixXd>& R) {
+  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
+  // Check that X is positive semi-definite.
+  EXPECT_TRUE(
+      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
+  int n = X.rows();
+  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
+  for (int i = 0; i < n; i++) {
+    EXPECT_GE(es.eigenvalues()[i], 0);
+  }
+  // Check that X is the solution to the discrete time ARE.
+  // clang-format off
+  MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - (A.transpose() * X * B * (B.transpose() * X * B + R).inverse()
+        * B.transpose() * X * A)
+      + Q;
+  // clang-format on
+  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
+                              MatrixCompareType::absolute));
+}
+
+void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
+                        const Eigen::Ref<const MatrixXd>& B,
+                        const Eigen::Ref<const MatrixXd>& Q,
+                        const Eigen::Ref<const MatrixXd>& R,
+                        const Eigen::Ref<const MatrixXd>& N) {
+  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R, N);
+  // Check that X is positive semi-definite.
+  EXPECT_TRUE(
+      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
+  int n = X.rows();
+  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
+  for (int i = 0; i < n; i++) {
+    EXPECT_GE(es.eigenvalues()[i], 0);
+  }
+  // Check that X is the solution to the discrete time ARE.
+  // clang-format off
+  MatrixXd Y =
+      A.transpose() * X * A
+      - X
+      - ((A.transpose() * X * B + N) * (B.transpose() * X * B + R).inverse()
+        * (B.transpose() * X * A + N.transpose()))
+      + Q;
+  // clang-format on
+  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
+                              MatrixCompareType::absolute));
+}
+
+GTEST_TEST(DARE, SolveDAREandVerify) {
+  // Test 1: non-invertible A
+  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
+  // Riccati Equation"
+  int n1 = 4, m1 = 1;
+  MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
+  A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
+  B1 << 0, 0, 0, 1;
+  Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
+  R1 << 0.25;
+  SolveDAREandVerify(A1, B1, Q1, R1);
+
+  MatrixXd Aref1(n1, n1);
+  Aref1 << 0.25, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
+  SolveDAREandVerify(A1, B1, (A1 - Aref1).transpose() * Q1 * (A1 - Aref1),
+      B1.transpose() * Q1 * B1 + R1, (A1 - Aref1).transpose() * Q1 * B1);
+
+  // Test 2: invertible A
+  int n2 = 2, m2 = 1;
+  MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
+  A2 << 1, 1, 0, 1;
+  B2 << 0, 1;
+  Q2 << 1, 0, 0, 0;
+  R2 << 0.3;
+  SolveDAREandVerify(A2, B2, Q2, R2);
+
+  MatrixXd Aref2(n2, n2);
+  Aref2 << 0.5, 1, 0, 1;
+  SolveDAREandVerify(A2, B2, (A2 - Aref2).transpose() * Q2 * (A2 - Aref2),
+      B2.transpose() * Q2 * B2 + R2, (A2 - Aref2).transpose() * Q2 * B2);
+
+  // Test 3: the first generalized eigenvalue of (S,T) is stable
+  int n3 = 2, m3 = 1;
+  MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
+  A3 << 0, 1, 0, 0;
+  B3 << 0, 1;
+  Q3 << 1, 0, 0, 1;
+  R3 << 1;
+  SolveDAREandVerify(A3, B3, Q3, R3);
+
+  MatrixXd Aref3(n3, n3);
+  Aref3 << 0, 0.5, 0, 0;
+  SolveDAREandVerify(A3, B3, (A3 - Aref3).transpose() * Q3 * (A3 - Aref3),
+      B3.transpose() * Q3 * B3 + R3, (A3 - Aref3).transpose() * Q3 * B3);
+
+  // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
+  const Eigen::MatrixXd A4{Eigen::Matrix2d::Identity()};
+  const Eigen::MatrixXd B4{Eigen::Matrix2d::Identity()};
+  const Eigen::MatrixXd Q4{Eigen::Matrix2d::Identity()};
+  const Eigen::MatrixXd R4{Eigen::Matrix2d::Identity()};
+  SolveDAREandVerify(A4, B4, Q4, R4);
+
+  const Eigen::MatrixXd N4{Eigen::Matrix2d::Identity()};
+  SolveDAREandVerify(A4, B4, Q4, R4, N4);
+}
+}  // namespace
+}  // namespace math
+}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp
deleted file mode 100644
index edcb772..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/drake/math/discrete_algebraic_riccati_equation_test.cpp
+++ /dev/null
@@ -1,76 +0,0 @@
-#include "drake/math/discrete_algebraic_riccati_equation.h"
-
-#include <Eigen/Core>
-#include <Eigen/Eigenvalues>
-
-#include <gtest/gtest.h>
-
-#include "drake/common/test_utilities/eigen_matrix_compare.h"
-#include "drake/math/autodiff.h"
-
-using Eigen::MatrixXd;
-
-namespace drake {
-namespace math {
-namespace {
-void SolveDAREandVerify(const Eigen::Ref<const MatrixXd>& A,
-                        const Eigen::Ref<const MatrixXd>& B,
-                        const Eigen::Ref<const MatrixXd>& Q,
-                        const Eigen::Ref<const MatrixXd>& R) {
-  MatrixXd X = DiscreteAlgebraicRiccatiEquation(A, B, Q, R);
-  // Check that X is positive semi-definite.
-  EXPECT_TRUE(
-      CompareMatrices(X, X.transpose(), 1E-10, MatrixCompareType::absolute));
-  int n = X.rows();
-  Eigen::SelfAdjointEigenSolver<MatrixXd> es(X);
-  for (int i = 0; i < n; i++) {
-    EXPECT_GE(es.eigenvalues()[i], 0);
-  }
-  // Check that X is the solution to the discrete time ARE.
-  MatrixXd Y = A.transpose() * X * A - X -
-               A.transpose() * X * B * (B.transpose() * X * B + R).inverse() *
-                   B.transpose() * X * A +
-               Q;
-  EXPECT_TRUE(CompareMatrices(Y, MatrixXd::Zero(n, n), 1E-10,
-                              MatrixCompareType::absolute));
-}
-
-GTEST_TEST(DARE, SolveDAREandVerify) {
-  // Test 1: non-invertible A
-  // Example 2 of "On the Numerical Solution of the Discrete-Time Algebraic
-  // Riccati Equation"
-  int n1 = 4, m1 = 1;
-  MatrixXd A1(n1, n1), B1(n1, m1), Q1(n1, n1), R1(m1, m1);
-  A1 << 0.5, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0;
-  B1 << 0, 0, 0, 1;
-  Q1 << 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
-  R1 << 0.25;
-  SolveDAREandVerify(A1, B1, Q1, R1);
-  // Test 2: invertible A
-  int n2 = 2, m2 = 1;
-  MatrixXd A2(n2, n2), B2(n2, m2), Q2(n2, n2), R2(m2, m2);
-  A2 << 1, 1, 0, 1;
-  B2 << 0, 1;
-  Q2 << 1, 0, 0, 0;
-  R2 << 0.3;
-  SolveDAREandVerify(A2, B2, Q2, R2);
-  // Test 3: the first generalized eigenvalue of (S,T) is stable
-  int n3 = 2, m3 = 1;
-  MatrixXd A3(n3, n3), B3(n3, m3), Q3(n3, n3), R3(m3, m3);
-  A3 << 0, 1, 0, 0;
-  B3 << 0, 1;
-  Q3 << 1, 0, 0, 1;
-  R3 << 1;
-  SolveDAREandVerify(A3, B3, Q3, R3);
-  // Test 4: A = B = Q = R = I_2 (2-by-2 identity matrix)
-  int n4 = 2, m4 = 2;
-  MatrixXd A4(n4, n4), B4(n4, m4), Q4(n4, n4), R4(m4, m4);
-  A4 << 1, 0, 0, 1;
-  B4 << 1, 0, 0, 1;
-  Q4 << 1, 0, 0, 1;
-  R4 << 1, 0, 0, 1;
-  SolveDAREandVerify(A4, B4, Q4, R4);
-}
-}  // namespace
-}  // namespace math
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
new file mode 100644
index 0000000..ee1da7f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include <wpi/numbers>
+
+#include "Eigen/Core"
+#include "frc/estimator/AngleStatistics.h"
+
+TEST(AngleStatisticsTest, Mean) {
+  Eigen::Matrix<double, 3, 3> sigmas{
+      {1, 1.2, 0},
+      {359 * wpi::numbers::pi / 180, 3 * wpi::numbers::pi / 180, 0},
+      {1, 2, 0}};
+  // Weights need to produce the mean of the sigmas
+  Eigen::Vector3d weights;
+  weights.fill(1.0 / sigmas.cols());
+
+  EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
+                  .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
+}
+
+TEST(AngleStatisticsTest, Residual) {
+  Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
+  Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+
+  EXPECT_TRUE(frc::AngleResidual<3>(a, b, 1).isApprox(
+      Eigen::Vector3d{0, 2 * wpi::numbers::pi / 180, 1}));
+}
+
+TEST(AngleStatisticsTest, Add) {
+  Eigen::Vector3d a{1, 1 * wpi::numbers::pi / 180, 2};
+  Eigen::Vector3d b{1, 359 * wpi::numbers::pi / 180, 1};
+
+  EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
new file mode 100644
index 0000000..4a854fd
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/DifferentialDrivePoseEstimatorTest.cpp
@@ -0,0 +1,98 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <limits>
+#include <random>
+
+#include "frc/StateSpaceUtil.h"
+#include "frc/estimator/DifferentialDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/kinematics/DifferentialDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+#include "units/angle.h"
+#include "units/length.h"
+#include "units/time.h"
+
+TEST(DifferentialDrivePoseEstimatorTest, Accuracy) {
+  frc::DifferentialDrivePoseEstimator estimator{frc::Rotation2d(),
+                                                frc::Pose2d(),
+                                                {0.02, 0.02, 0.01, 0.02, 0.02},
+                                                {0.01, 0.01, 0.001},
+                                                {0.1, 0.1, 0.01}};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+      frc::TrajectoryConfig(10_mps, 5.0_mps_sq));
+
+  frc::DifferentialDriveKinematics kinematics{1.0_m};
+  frc::DifferentialDriveOdometry odometry{frc::Rotation2d()};
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0.0_s;
+
+  units::meter_t leftDistance = 0_m;
+  units::meter_t rightDistance = 0_m;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t <= trajectory.TotalTime()) {
+    auto groundTruthState = trajectory.Sample(t);
+    auto input = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d()) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d(
+              frc::Translation2d(distribution(generator) * 0.1 * 1_m,
+                                 distribution(generator) * 0.1 * 1_m),
+              frc::Rotation2d(distribution(generator) * 0.01 * 1_rad));
+
+      lastVisionUpdateTime = t;
+    }
+
+    leftDistance += input.left * distribution(generator) * 0.01 * dt;
+    rightDistance += input.right * distribution(generator) * 0.01 * dt;
+
+    auto xhat = estimator.UpdateWithTime(
+        t,
+        groundTruthState.pose.Rotation() +
+            frc::Rotation2d(units::radian_t(distribution(generator) * 0.001)),
+        input, leftDistance, rightDistance);
+
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_NEAR(0.0, errorSum / (trajectory.TotalTime().value() / dt.value()),
+              0.2);
+  EXPECT_NEAR(0.0, maxError, 0.4);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
index 387593b..6d51185 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/ExtendedKalmanFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -21,8 +18,8 @@
 
 namespace {
 
-Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
-                                     const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
+                                  const Eigen::Vector<double, 2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;       // Low gear ratio
@@ -43,36 +40,26 @@
   units::volt_t Vl{u(0)};
   units::volt_t Vr{u(1)};
 
-  Eigen::Matrix<double, 5, 1> result;
   auto v = 0.5 * (vl + vr);
-  result(0) = v.to<double>() * std::cos(x(2));
-  result(1) = v.to<double>() * std::sin(x(2));
-  result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
-  result(3) =
-      k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  result(4) =
-      k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  return result;
+  return Eigen::Vector<double, 5>{
+      v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
+      ((vr - vl) / (2.0 * rb)).value(),
+      k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+          k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
+      k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+          k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> LocalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 3, 1> y;
-  y << x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
 }
 
-Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> GlobalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 5, 1> y;
-  y << x(0), x(1), x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
 
@@ -84,8 +71,7 @@
                                               {0.5, 0.5, 10.0, 1.0, 1.0},
                                               {0.0001, 0.01, 0.01},
                                               dt};
-  Eigen::Matrix<double, 2, 1> u;
-  u << 12.0, 12.0;
+  Eigen::Vector<double, 2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
   auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -112,41 +98,37 @@
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
-  Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+  Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
+  Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
 
-  Eigen::Matrix<double, 5, 1> nextR;
-  Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
+                                            Eigen::Vector<double, 5>::Zero(),
+                                            Eigen::Vector<double, 2>::Zero());
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(
-      Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
-
-  observer.SetXhat(frc::MakeMatrix<5, 1>(
-      trajectory.InitialPose().Translation().X().to<double>(),
-      trajectory.InitialPose().Translation().Y().to<double>(),
-      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+  observer.SetXhat(Eigen::Vector<double, 5>{
+      trajectory.InitialPose().Translation().X().value(),
+      trajectory.InitialPose().Translation().Y().value(),
+      trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
 
   auto totalTime = trajectory.TotalTime();
-  for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
+  for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
     auto ref = trajectory.Sample(dt * i);
     units::meters_per_second_t vl =
-        ref.velocity * (1 - (ref.curvature * rb).to<double>());
+        ref.velocity * (1 - (ref.curvature * rb).value());
     units::meters_per_second_t vr =
-        ref.velocity * (1 + (ref.curvature * rb).to<double>());
+        ref.velocity * (1 + (ref.curvature * rb).value());
 
-    nextR(0) = ref.pose.Translation().X().to<double>();
-    nextR(1) = ref.pose.Translation().Y().to<double>();
-    nextR(2) = ref.pose.Rotation().Radians().to<double>();
-    nextR(3) = vl.to<double>();
-    nextR(4) = vr.to<double>();
+    Eigen::Vector<double, 5> nextR{
+        ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
+        ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
     auto localY =
-        LocalMeasurementModel(nextR, Eigen::Matrix<double, 2, 1>::Zero());
+        LocalMeasurementModel(nextR, Eigen::Vector<double, 2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
-    Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
-    u = B.householderQr().solve(
-        rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+    Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
+    u = B.householderQr().solve(rdot -
+                                Dynamics(r, Eigen::Vector<double, 2>::Zero()));
 
     observer.Predict(u, dt);
 
@@ -161,12 +143,12 @@
   observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
 
   auto finalPosition = trajectory.Sample(trajectory.TotalTime());
-  ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
-              observer.Xhat(0), 1.0);
-  ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
-              observer.Xhat(1), 1.0);
-  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
-              observer.Xhat(2), 1.0);
+  ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+              1.0);
+  ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+              1.0);
+  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+              1.0);
   ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
   ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
index 53c54ef..fc373ca 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/KalmanFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
new file mode 100644
index 0000000..881d4e8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MecanumDrivePoseEstimatorTest.cpp
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <limits>
+#include <random>
+
+#include "frc/estimator/MecanumDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/MecanumDriveKinematics.h"
+#include "frc/kinematics/MecanumDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(MecanumDrivePoseEstimatorTest, Accuracy) {
+  frc::MecanumDriveKinematics kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::MecanumDrivePoseEstimator estimator{
+      frc::Rotation2d(), frc::Pose2d(), kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+
+  frc::MecanumDriveOdometry odometry{kinematics, frc::Rotation2d()};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  std::vector<frc::Pose2d> visionPoses;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d()) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d(
+              frc::Translation2d(distribution(generator) * 0.1_m,
+                                 distribution(generator) * 0.1_m),
+              frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
+      visionPoses.push_back(lastVisionPose);
+      lastVisionUpdateTime = t;
+    }
+
+    auto wheelSpeeds = kinematics.ToWheelSpeeds(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    auto xhat = estimator.UpdateWithTime(
+        t,
+        groundTruthState.pose.Rotation() +
+            frc::Rotation2d(distribution(generator) * 0.05_rad),
+        wheelSpeeds);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
+  EXPECT_LT(maxError, 0.4);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
index ace5e79..c012435 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/MerweScaledSigmaPointsTest.cpp
@@ -1,41 +1,37 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
-#include "frc/StateSpaceUtil.h"
 #include "frc/estimator/MerweScaledSigmaPoints.h"
 
-namespace drake {
-namespace math {
+namespace drake::math {
 namespace {
-TEST(MerweScaledSigmaPointsTest, TestZeroMean) {
+TEST(MerweScaledSigmaPointsTest, ZeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points =
-      sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(0.0, 0.0),
-                              frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0));
+  auto points = sigmaPoints.SigmaPoints(
+      Eigen::Vector<double, 2>{0.0, 0.0},
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}});
 
   EXPECT_TRUE(
-      (points - frc::MakeMatrix<2, 5>(0.0, 0.00173205, 0.0, -0.00173205, 0.0,
-                                      0.0, 0.0, 0.00173205, 0.0, -0.00173205))
+      (points -
+       Eigen::Matrix<double, 2, 5>{{0.0, 0.00173205, 0.0, -0.00173205, 0.0},
+                                   {0.0, 0.0, 0.00173205, 0.0, -0.00173205}})
           .norm() < 1e-3);
 }
 
-TEST(MerweScaledSigmaPointsTest, TestNonzeroMean) {
+TEST(MerweScaledSigmaPointsTest, NonzeroMean) {
   frc::MerweScaledSigmaPoints<2> sigmaPoints;
-  auto points =
-      sigmaPoints.SigmaPoints(frc::MakeMatrix<2, 1>(1.0, 2.0),
-                              frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 10.0));
+  auto points = sigmaPoints.SigmaPoints(
+      Eigen::Vector<double, 2>{1.0, 2.0},
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 10.0}});
 
   EXPECT_TRUE(
-      (points - frc::MakeMatrix<2, 5>(1.0, 1.00173205, 1.0, 0.998268, 1.0, 2.0,
-                                      2.0, 2.00548, 2.0, 1.99452))
+      (points -
+       Eigen::Matrix<double, 2, 5>{{1.0, 1.00173205, 1.0, 0.998268, 1.0},
+                                   {2.0, 2.0, 2.00548, 2.0, 1.99452}})
           .norm() < 1e-3);
 }
 }  // namespace
-}  // namespace math
-}  // namespace drake
+}  // namespace drake::math
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
new file mode 100644
index 0000000..ee01f6f
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/SwerveDrivePoseEstimatorTest.cpp
@@ -0,0 +1,89 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <limits>
+#include <random>
+
+#include "frc/estimator/SwerveDrivePoseEstimator.h"
+#include "frc/geometry/Pose2d.h"
+#include "frc/kinematics/SwerveDriveKinematics.h"
+#include "frc/kinematics/SwerveDriveOdometry.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(SwerveDrivePoseEstimatorTest, Accuracy) {
+  frc::SwerveDriveKinematics<4> kinematics{
+      frc::Translation2d{1_m, 1_m}, frc::Translation2d{1_m, -1_m},
+      frc::Translation2d{-1_m, -1_m}, frc::Translation2d{-1_m, 1_m}};
+
+  frc::SwerveDrivePoseEstimator<4> estimator{
+      frc::Rotation2d(), frc::Pose2d(), kinematics,
+      {0.1, 0.1, 0.1},   {0.05},        {0.1, 0.1, 0.1}};
+
+  frc::SwerveDriveOdometry<4> odometry{kinematics, frc::Rotation2d()};
+
+  frc::Trajectory trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
+      std::vector{frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg)),
+                  frc::Pose2d(3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(135_deg)),
+                  frc::Pose2d(-3_m, 0_m, frc::Rotation2d(-90_deg)),
+                  frc::Pose2d(0_m, 0_m, frc::Rotation2d(45_deg))},
+      frc::TrajectoryConfig(5.0_mps, 2.0_mps_sq));
+
+  std::default_random_engine generator;
+  std::normal_distribution<double> distribution(0.0, 1.0);
+
+  units::second_t dt = 0.02_s;
+  units::second_t t = 0_s;
+
+  units::second_t kVisionUpdateRate = 0.1_s;
+  frc::Pose2d lastVisionPose;
+  units::second_t lastVisionUpdateTime{-std::numeric_limits<double>::max()};
+
+  std::vector<frc::Pose2d> visionPoses;
+
+  double maxError = -std::numeric_limits<double>::max();
+  double errorSum = 0;
+
+  while (t < trajectory.TotalTime()) {
+    frc::Trajectory::State groundTruthState = trajectory.Sample(t);
+
+    if (lastVisionUpdateTime + kVisionUpdateRate < t) {
+      if (lastVisionPose != frc::Pose2d()) {
+        estimator.AddVisionMeasurement(lastVisionPose, lastVisionUpdateTime);
+      }
+      lastVisionPose =
+          groundTruthState.pose +
+          frc::Transform2d(
+              frc::Translation2d(distribution(generator) * 0.1_m,
+                                 distribution(generator) * 0.1_m),
+              frc::Rotation2d(distribution(generator) * 0.1 * 1_rad));
+      visionPoses.push_back(lastVisionPose);
+      lastVisionUpdateTime = t;
+    }
+
+    auto moduleStates = kinematics.ToSwerveModuleStates(
+        {groundTruthState.velocity, 0_mps,
+         groundTruthState.velocity * groundTruthState.curvature});
+
+    auto xhat = estimator.UpdateWithTime(
+        t,
+        groundTruthState.pose.Rotation() +
+            frc::Rotation2d(distribution(generator) * 0.05_rad),
+        moduleStates[0], moduleStates[1], moduleStates[2], moduleStates[3]);
+    double error = groundTruthState.pose.Translation()
+                       .Distance(xhat.Translation())
+                       .value();
+
+    if (error > maxError) {
+      maxError = error;
+    }
+    errorSum += error;
+
+    t += dt;
+  }
+
+  EXPECT_LT(errorSum / (trajectory.TotalTime().value() / dt.value()), 0.2);
+  EXPECT_LT(maxError, 0.4);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
index 0017b42..9665442 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/estimator/UnscentedKalmanFilterTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -13,17 +10,18 @@
 #include "Eigen/Core"
 #include "Eigen/QR"
 #include "frc/StateSpaceUtil.h"
+#include "frc/estimator/AngleStatistics.h"
 #include "frc/estimator/UnscentedKalmanFilter.h"
+#include "frc/system/NumericalIntegration.h"
 #include "frc/system/NumericalJacobian.h"
-#include "frc/system/RungeKutta.h"
 #include "frc/system/plant/DCMotor.h"
 #include "frc/trajectory/TrajectoryGenerator.h"
 #include "units/moment_of_inertia.h"
 
 namespace {
 
-Eigen::Matrix<double, 5, 1> Dynamics(const Eigen::Matrix<double, 5, 1>& x,
-                                     const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> Dynamics(const Eigen::Vector<double, 5>& x,
+                                  const Eigen::Vector<double, 2>& u) {
   auto motors = frc::DCMotor::CIM(2);
 
   // constexpr double Glow = 15.32;       // Low gear ratio
@@ -44,49 +42,38 @@
   units::volt_t Vl{u(0)};
   units::volt_t Vr{u(1)};
 
-  Eigen::Matrix<double, 5, 1> result;
   auto v = 0.5 * (vl + vr);
-  result(0) = v.to<double>() * std::cos(x(2));
-  result(1) = v.to<double>() * std::sin(x(2));
-  result(2) = ((vr - vl) / (2.0 * rb)).to<double>();
-  result(3) =
-      k1.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k2.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  result(4) =
-      k2.to<double>() * ((C1 * vl).to<double>() + (C2 * Vl).to<double>()) +
-      k1.to<double>() * ((C1 * vr).to<double>() + (C2 * Vr).to<double>());
-  return result;
+  return Eigen::Vector<double, 5>{
+      v.value() * std::cos(x(2)), v.value() * std::sin(x(2)),
+      ((vr - vl) / (2.0 * rb)).value(),
+      k1.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+          k2.value() * ((C1 * vr).value() + (C2 * Vr).value()),
+      k2.value() * ((C1 * vl).value() + (C2 * Vl).value()) +
+          k1.value() * ((C1 * vr).value() + (C2 * Vr).value())};
 }
 
-Eigen::Matrix<double, 3, 1> LocalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> LocalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 3, 1> y;
-  y << x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 3>{x(2), x(3), x(4)};
 }
 
-Eigen::Matrix<double, 5, 1> GlobalMeasurementModel(
-    const Eigen::Matrix<double, 5, 1>& x,
-    const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 5> GlobalMeasurementModel(
+    const Eigen::Vector<double, 5>& x, const Eigen::Vector<double, 2>& u) {
   static_cast<void>(u);
-  Eigen::Matrix<double, 5, 1> y;
-  y << x(0), x(1), x(2), x(3), x(4);
-  return y;
+  return Eigen::Vector<double, 5>{x(0), x(1), x(2), x(3), x(4)};
 }
 }  // namespace
 
 TEST(UnscentedKalmanFilterTest, Init) {
-  constexpr auto dt = 0.00505_s;
+  constexpr auto dt = 5_ms;
 
   frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
                                                LocalMeasurementModel,
                                                {0.5, 0.5, 10.0, 1.0, 1.0},
                                                {0.0001, 0.01, 0.01},
                                                dt};
-  Eigen::Matrix<double, 2, 1> u;
-  u << 12.0, 12.0;
+  Eigen::Vector<double, 2> u{12.0, 12.0};
   observer.Predict(u, dt);
 
   auto localY = LocalMeasurementModel(observer.Xhat(), u);
@@ -94,11 +81,13 @@
 
   auto globalY = GlobalMeasurementModel(observer.Xhat(), u);
   auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.01, 0.01);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+                      frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
+                      frc::AngleResidual<5>(2), frc::AngleAdd<5>(2));
 }
 
 TEST(UnscentedKalmanFilterTest, Convergence) {
-  constexpr auto dt = 0.00505_s;
+  constexpr auto dt = 5_ms;
   constexpr auto rb = 0.8382_m / 2.0;  // Robot radius
 
   frc::UnscentedKalmanFilter<5, 2, 3> observer{Dynamics,
@@ -113,48 +102,44 @@
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       waypoints, {8.8_mps, 0.1_mps_sq});
 
-  Eigen::Matrix<double, 5, 1> r = Eigen::Matrix<double, 5, 1>::Zero();
+  Eigen::Vector<double, 5> r = Eigen::Vector<double, 5>::Zero();
+  Eigen::Vector<double, 2> u = Eigen::Vector<double, 2>::Zero();
 
-  Eigen::Matrix<double, 5, 1> nextR;
-  Eigen::Matrix<double, 2, 1> u = Eigen::Matrix<double, 2, 1>::Zero();
+  auto B = frc::NumericalJacobianU<5, 5, 2>(Dynamics,
+                                            Eigen::Vector<double, 5>::Zero(),
+                                            Eigen::Vector<double, 2>::Zero());
 
-  auto B = frc::NumericalJacobianU<5, 5, 2>(
-      Dynamics, Eigen::Matrix<double, 5, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
-
-  observer.SetXhat(frc::MakeMatrix<5, 1>(
-      trajectory.InitialPose().Translation().X().to<double>(),
-      trajectory.InitialPose().Translation().Y().to<double>(),
-      trajectory.InitialPose().Rotation().Radians().to<double>(), 0.0, 0.0));
+  observer.SetXhat(Eigen::Vector<double, 5>{
+      trajectory.InitialPose().Translation().X().value(),
+      trajectory.InitialPose().Translation().Y().value(),
+      trajectory.InitialPose().Rotation().Radians().value(), 0.0, 0.0});
 
   auto trueXhat = observer.Xhat();
 
   auto totalTime = trajectory.TotalTime();
-  for (size_t i = 0; i < (totalTime / dt).to<double>(); ++i) {
+  for (size_t i = 0; i < (totalTime / dt).value(); ++i) {
     auto ref = trajectory.Sample(dt * i);
     units::meters_per_second_t vl =
-        ref.velocity * (1 - (ref.curvature * rb).to<double>());
+        ref.velocity * (1 - (ref.curvature * rb).value());
     units::meters_per_second_t vr =
-        ref.velocity * (1 + (ref.curvature * rb).to<double>());
+        ref.velocity * (1 + (ref.curvature * rb).value());
 
-    nextR(0) = ref.pose.Translation().X().to<double>();
-    nextR(1) = ref.pose.Translation().Y().to<double>();
-    nextR(2) = ref.pose.Rotation().Radians().to<double>();
-    nextR(3) = vl.to<double>();
-    nextR(4) = vr.to<double>();
+    Eigen::Vector<double, 5> nextR{
+        ref.pose.Translation().X().value(), ref.pose.Translation().Y().value(),
+        ref.pose.Rotation().Radians().value(), vl.value(), vr.value()};
 
     auto localY =
-        LocalMeasurementModel(trueXhat, Eigen::Matrix<double, 2, 1>::Zero());
+        LocalMeasurementModel(trueXhat, Eigen::Vector<double, 2>::Zero());
     observer.Correct(u, localY + frc::MakeWhiteNoiseVector(0.0001, 0.5, 0.5));
 
-    Eigen::Matrix<double, 5, 1> rdot = (nextR - r) / dt.to<double>();
-    u = B.householderQr().solve(
-        rdot - Dynamics(r, Eigen::Matrix<double, 2, 1>::Zero()));
+    Eigen::Vector<double, 5> rdot = (nextR - r) / dt.value();
+    u = B.householderQr().solve(rdot -
+                                Dynamics(r, Eigen::Vector<double, 2>::Zero()));
 
     observer.Predict(u, dt);
 
     r = nextR;
-    trueXhat = frc::RungeKutta(Dynamics, trueXhat, u, dt);
+    trueXhat = frc::RK4(Dynamics, trueXhat, u, dt);
   }
 
   auto localY = LocalMeasurementModel(trueXhat, u);
@@ -162,15 +147,19 @@
 
   auto globalY = GlobalMeasurementModel(trueXhat, u);
   auto R = frc::MakeCovMatrix(0.01, 0.01, 0.0001, 0.5, 0.5);
-  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R);
+  observer.Correct<5>(u, globalY, GlobalMeasurementModel, R,
+                      frc::AngleMean<5, 5>(2), frc::AngleResidual<5>(2),
+                      frc::AngleResidual<5>(2), frc::AngleAdd<5>(2)
+
+  );
 
   auto finalPosition = trajectory.Sample(trajectory.TotalTime());
-  ASSERT_NEAR(finalPosition.pose.Translation().X().template to<double>(),
-              observer.Xhat(0), 1.0);
-  ASSERT_NEAR(finalPosition.pose.Translation().Y().template to<double>(),
-              observer.Xhat(1), 1.0);
-  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().template to<double>(),
-              observer.Xhat(2), 1.0);
+  ASSERT_NEAR(finalPosition.pose.Translation().X().value(), observer.Xhat(0),
+              1.0);
+  ASSERT_NEAR(finalPosition.pose.Translation().Y().value(), observer.Xhat(1),
+              1.0);
+  ASSERT_NEAR(finalPosition.pose.Rotation().Radians().value(), observer.Xhat(2),
+              1.0);
   ASSERT_NEAR(0.0, observer.Xhat(3), 1.0);
   ASSERT_NEAR(0.0, observer.Xhat(4), 1.0);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
new file mode 100644
index 0000000..5ccd829
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/LinearFilterNoiseTest.cpp
@@ -0,0 +1,69 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/filter/LinearFilter.h"  // NOLINT(build/include_order)
+
+#include <cmath>
+#include <random>
+
+#include <wpi/numbers>
+
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+// Filter constants
+static constexpr auto kFilterStep = 5_ms;
+static constexpr auto kFilterTime = 2_s;
+static constexpr double kSinglePoleIIRTimeConstant = 0.015915;
+static constexpr int32_t kMovAvgTaps = 6;
+
+enum LinearFilterNoiseTestType { kTestSinglePoleIIR, kTestMovAvg };
+
+static double GetData(double t) {
+  return 100.0 * std::sin(2.0 * wpi::numbers::pi * t);
+}
+
+class LinearFilterNoiseTest
+    : public testing::TestWithParam<LinearFilterNoiseTestType> {
+ protected:
+  frc::LinearFilter<double> m_filter = [=] {
+    switch (GetParam()) {
+      case kTestSinglePoleIIR:
+        return frc::LinearFilter<double>::SinglePoleIIR(
+            kSinglePoleIIRTimeConstant, kFilterStep);
+        break;
+      default:
+        return 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.value());
+    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(Tests, LinearFilterNoiseTest,
+                         testing::Values(kTestSinglePoleIIR, kTestMovAvg));
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
new file mode 100644
index 0000000..bca3f9d
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/LinearFilterOutputTest.cpp
@@ -0,0 +1,214 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/filter/LinearFilter.h"  // NOLINT(build/include_order)
+
+#include <cmath>
+#include <functional>
+#include <memory>
+#include <random>
+
+#include <wpi/numbers>
+
+#include "gtest/gtest.h"
+#include "units/time.h"
+
+// Filter constants
+static constexpr auto kFilterStep = 5_ms;
+static constexpr auto kFilterTime = 2_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 {
+  kTestSinglePoleIIR,
+  kTestHighPass,
+  kTestMovAvg,
+  kTestPulse
+};
+
+static double GetData(double t) {
+  return 100.0 * std::sin(2.0 * wpi::numbers::pi * t) +
+         20.0 * std::cos(50.0 * wpi::numbers::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:
+  frc::LinearFilter<double> m_filter = [=] {
+    switch (GetParam()) {
+      case kTestSinglePoleIIR:
+        return frc::LinearFilter<double>::SinglePoleIIR(
+            kSinglePoleIIRTimeConstant, kFilterStep);
+        break;
+      case kTestHighPass:
+        return frc::LinearFilter<double>::HighPass(kHighPassTimeConstant,
+                                                   kFilterStep);
+        break;
+      case kTestMovAvg:
+        return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+        break;
+      default:
+        return frc::LinearFilter<double>::MovingAverage(kMovAvgTaps);
+        break;
+    }
+  }();
+  std::function<double(double)> m_data;
+  double m_expectedOutput = 0.0;
+
+  LinearFilterOutputTest() {
+    switch (GetParam()) {
+      case kTestSinglePoleIIR: {
+        m_data = GetData;
+        m_expectedOutput = kSinglePoleIIRExpectedOutput;
+        break;
+      }
+
+      case kTestHighPass: {
+        m_data = GetData;
+        m_expectedOutput = kHighPassExpectedOutput;
+        break;
+      }
+
+      case kTestMovAvg: {
+        m_data = GetData;
+        m_expectedOutput = kMovAvgExpectedOutput;
+        break;
+      }
+
+      case kTestPulse: {
+        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.value()));
+  }
+
+  RecordProperty("LinearFilterOutput", filterOutput);
+
+  EXPECT_FLOAT_EQ(m_expectedOutput, filterOutput)
+      << "Filter output didn't match expected value";
+}
+
+INSTANTIATE_TEST_SUITE_P(Tests, LinearFilterOutputTest,
+                         testing::Values(kTestSinglePoleIIR, kTestHighPass,
+                                         kTestMovAvg, kTestPulse));
+
+template <int Derivative, int Samples, typename F, typename DfDx>
+void AssertResults(F&& f, DfDx&& dfdx, units::second_t h, double min,
+                   double max) {
+  auto filter =
+      frc::LinearFilter<double>::BackwardFiniteDifference<Derivative, Samples>(
+          h);
+
+  for (int i = min / h.value(); i < max / h.value(); ++i) {
+    // Let filter initialize
+    if (i < static_cast<int>(min / h.value()) + Samples) {
+      filter.Calculate(f(i * h.value()));
+      continue;
+    }
+
+    // The order of accuracy is O(h^(N - d)) where N is number of stencil
+    // points and d is order of derivative
+    EXPECT_NEAR(dfdx(i * h.value()), filter.Calculate(f(i * h.value())),
+                10.0 * std::pow(h.value(), Samples - Derivative));
+  }
+}
+
+/**
+ * Test backward finite difference.
+ */
+TEST(LinearFilterOutputTest, BackwardFiniteDifference) {
+  constexpr auto h = 5_ms;
+
+  AssertResults<1, 2>(
+      [](double x) {
+        // f(x) = x²
+        return x * x;
+      },
+      [](double x) {
+        // df/dx = 2x
+        return 2.0 * x;
+      },
+      h, -20.0, 20.0);
+
+  AssertResults<1, 2>(
+      [](double x) {
+        // f(x) = std::sin(x)
+        return std::sin(x);
+      },
+      [](double x) {
+        // df/dx = std::cos(x)
+        return std::cos(x);
+      },
+      h, -20.0, 20.0);
+
+  AssertResults<1, 2>(
+      [](double x) {
+        // f(x) = ln(x)
+        return std::log(x);
+      },
+      [](double x) {
+        // df/dx = 1 / x
+        return 1.0 / x;
+      },
+      h, 1.0, 20.0);
+
+  AssertResults<2, 4>(
+      [](double x) {
+        // f(x) = x^2
+        return x * x;
+      },
+      [](double x) {
+        // d²f/dx² = 2
+        return 2.0;
+      },
+      h, -20.0, 20.0);
+
+  AssertResults<2, 4>(
+      [](double x) {
+        // f(x) = std::sin(x)
+        return std::sin(x);
+      },
+      [](double x) {
+        // d²f/dx² = -std::sin(x)
+        return -std::sin(x);
+      },
+      h, -20.0, 20.0);
+
+  AssertResults<2, 4>(
+      [](double x) {
+        // f(x) = ln(x)
+        return std::log(x);
+      },
+      [](double x) {
+        // d²f/dx² = -1 / x²
+        return -1.0 / (x * x);
+      },
+      h, 1.0, 20.0);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
new file mode 100644
index 0000000..8151a45
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/MedianFilterTest.cpp
@@ -0,0 +1,52 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/filter/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/third_party/allwpilib/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
new file mode 100644
index 0000000..d2c0bae
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/filter/SlewRateLimiterTest.cpp
@@ -0,0 +1,33 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <wpi/timestamp.h>
+
+#include "frc/filter/SlewRateLimiter.h"
+#include "gtest/gtest.h"
+#include "units/length.h"
+#include "units/time.h"
+#include "units/velocity.h"
+
+static units::second_t now = 0_s;
+
+TEST(SlewRateLimiterTest, SlewRateLimit) {
+  WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+
+  frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+  now += 1_s;
+
+  EXPECT_LT(limiter.Calculate(2_m), 2_m);
+}
+
+TEST(SlewRateLimiterTest, SlewRateNoLimit) {
+  WPI_SetNowImpl([] { return units::microsecond_t{now}.to<uint64_t>(); });
+
+  frc::SlewRateLimiter<units::meters> limiter(1_mps);
+
+  now += 1_s;
+
+  EXPECT_EQ(limiter.Calculate(0.5_m), 0.5_m);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index 620c9d3..cd5b127 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
@@ -20,9 +17,9 @@
 
   const auto transformed = initial + transform;
 
-  EXPECT_NEAR(transformed.X().to<double>(), 1 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Y().to<double>(), 2 + 5 / std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transformed.Rotation().Degrees().to<double>(), 50.0, kEpsilon);
+  EXPECT_NEAR(transformed.X().value(), 1 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Y().value(), 2 + 5 / std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transformed.Rotation().Degrees().value(), 50.0, kEpsilon);
 }
 
 TEST(Pose2dTest, RelativeTo) {
@@ -31,10 +28,10 @@
 
   const auto finalRelativeToInitial = final.RelativeTo(initial);
 
-  EXPECT_NEAR(finalRelativeToInitial.X().to<double>(), 5.0 * std::sqrt(2.0),
+  EXPECT_NEAR(finalRelativeToInitial.X().value(), 5.0 * std::sqrt(2.0),
               kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().to<double>(), 0.0,
+  EXPECT_NEAR(finalRelativeToInitial.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(finalRelativeToInitial.Rotation().Degrees().value(), 0.0,
               kEpsilon);
 }
 
@@ -56,7 +53,7 @@
 
   const auto transform = final - initial;
 
-  EXPECT_NEAR(transform.X().to<double>(), 5.0 * std::sqrt(2.0), kEpsilon);
-  EXPECT_NEAR(transform.Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(transform.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(transform.X().value(), 5.0 * std::sqrt(2.0), kEpsilon);
+  EXPECT_NEAR(transform.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(transform.Rotation().Degrees().value(), 0.0, kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index a29371f..ed3b6b5 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/geometry/Rotation2d.h"
 #include "gtest/gtest.h"
@@ -17,51 +14,55 @@
 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)};
+  const Rotation2d rot1{units::radian_t(wpi::numbers::pi / 3)};
+  const Rotation2d rot2{units::radian_t(wpi::numbers::pi / 4)};
 
-  EXPECT_NEAR(one.Degrees().to<double>(), 60.0, kEpsilon);
-  EXPECT_NEAR(two.Degrees().to<double>(), 45.0, kEpsilon);
+  EXPECT_NEAR(rot1.Degrees().value(), 60.0, kEpsilon);
+  EXPECT_NEAR(rot2.Degrees().value(), 45.0, kEpsilon);
 }
 
 TEST(Rotation2dTest, DegreesToRadians) {
-  const auto one = Rotation2d(45.0_deg);
-  const auto two = Rotation2d(30.0_deg);
+  const auto rot1 = Rotation2d(45.0_deg);
+  const auto rot2 = 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);
+  EXPECT_NEAR(rot1.Radians().value(), wpi::numbers::pi / 4.0, kEpsilon);
+  EXPECT_NEAR(rot2.Radians().value(), wpi::numbers::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);
+  EXPECT_NEAR(sum.Radians().value(), wpi::numbers::pi / 2.0, kEpsilon);
+  EXPECT_NEAR(sum.Degrees().value(), 90.0, kEpsilon);
 }
 
 TEST(Rotation2dTest, RotateByNonZero) {
   auto rot = Rotation2d(90.0_deg);
-  rot += Rotation2d(30.0_deg);
+  rot = rot + Rotation2d(30.0_deg);
 
-  EXPECT_NEAR(rot.Degrees().to<double>(), 120.0, kEpsilon);
+  EXPECT_NEAR(rot.Degrees().value(), 120.0, kEpsilon);
 }
 
 TEST(Rotation2dTest, Minus) {
-  const auto one = Rotation2d(70.0_deg);
-  const auto two = Rotation2d(30.0_deg);
+  const auto rot1 = Rotation2d(70.0_deg);
+  const auto rot2 = Rotation2d(30.0_deg);
 
-  EXPECT_NEAR((one - two).Degrees().to<double>(), 40.0, kEpsilon);
+  EXPECT_NEAR((rot1 - rot2).Degrees().value(), 40.0, kEpsilon);
 }
 
 TEST(Rotation2dTest, Equality) {
-  const auto one = Rotation2d(43_deg);
-  const auto two = Rotation2d(43_deg);
-  EXPECT_TRUE(one == two);
+  const auto rot1 = Rotation2d(43_deg);
+  const auto rot2 = Rotation2d(43_deg);
+  EXPECT_EQ(rot1, rot2);
+
+  const auto rot3 = Rotation2d(-180_deg);
+  const auto rot4 = Rotation2d(180_deg);
+  EXPECT_EQ(rot3, rot4);
 }
 
 TEST(Rotation2dTest, Inequality) {
-  const auto one = Rotation2d(43_deg);
-  const auto two = Rotation2d(43.5_deg);
-  EXPECT_TRUE(one != two);
+  const auto rot1 = Rotation2d(43_deg);
+  const auto rot2 = Rotation2d(43.5_deg);
+  EXPECT_NE(rot1, rot2);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index b302fad..968ab29 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
@@ -18,16 +15,30 @@
 static constexpr double kEpsilon = 1E-9;
 
 TEST(Transform2dTest, Inverse) {
-  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 Pose2d initial{1_m, 2_m, 45_deg};
+  const Transform2d transform{{5_m, 0_m}, 5_deg};
 
   auto transformed = initial + transform;
   auto untransformed = transformed + transform.Inverse();
 
-  EXPECT_NEAR(initial.X().to<double>(), untransformed.X().to<double>(),
+  EXPECT_NEAR(initial.X().value(), untransformed.X().value(), kEpsilon);
+  EXPECT_NEAR(initial.Y().value(), untransformed.Y().value(), kEpsilon);
+  EXPECT_NEAR(initial.Rotation().Degrees().value(),
+              untransformed.Rotation().Degrees().value(), kEpsilon);
+}
+
+TEST(Transform2dTest, Composition) {
+  const Pose2d initial{1_m, 2_m, 45_deg};
+  const Transform2d transform1{{5_m, 0_m}, 5_deg};
+  const Transform2d transform2{{0_m, 2_m}, 5_deg};
+
+  auto transformedSeparate = initial + transform1 + transform2;
+  auto transformedCombined = initial + (transform1 + transform2);
+
+  EXPECT_NEAR(transformedSeparate.X().value(), transformedCombined.X().value(),
               kEpsilon);
-  EXPECT_NEAR(initial.Y().to<double>(), untransformed.Y().to<double>(),
+  EXPECT_NEAR(transformedSeparate.Y().value(), transformedCombined.Y().value(),
               kEpsilon);
-  EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
-              untransformed.Rotation().Degrees().to<double>(), kEpsilon);
+  EXPECT_NEAR(transformedSeparate.Rotation().Degrees().value(),
+              transformedCombined.Rotation().Degrees().value(), kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
index 8e487f3..efdcace 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Translation2dTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
@@ -20,8 +17,8 @@
 
   const auto sum = one + two;
 
-  EXPECT_NEAR(sum.X().to<double>(), 3.0, kEpsilon);
-  EXPECT_NEAR(sum.Y().to<double>(), 8.0, kEpsilon);
+  EXPECT_NEAR(sum.X().value(), 3.0, kEpsilon);
+  EXPECT_NEAR(sum.Y().value(), 8.0, kEpsilon);
 }
 
 TEST(Translation2dTest, Difference) {
@@ -30,51 +27,51 @@
 
   const auto difference = one - two;
 
-  EXPECT_NEAR(difference.X().to<double>(), -1.0, kEpsilon);
-  EXPECT_NEAR(difference.Y().to<double>(), -2.0, kEpsilon);
+  EXPECT_NEAR(difference.X().value(), -1.0, kEpsilon);
+  EXPECT_NEAR(difference.Y().value(), -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);
+  EXPECT_NEAR(rotated.X().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(rotated.Y().value(), 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);
+  EXPECT_NEAR(mult.X().value(), 9.0, kEpsilon);
+  EXPECT_NEAR(mult.Y().value(), 15.0, kEpsilon);
 }
 
-TEST(Translation2d, Division) {
+TEST(Translation2dTest, 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);
+  EXPECT_NEAR(div.X().value(), 1.5, kEpsilon);
+  EXPECT_NEAR(div.Y().value(), 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);
+  EXPECT_NEAR(one.Norm().value(), 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);
+  EXPECT_NEAR(one.Distance(two).value(), 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);
+  EXPECT_NEAR(inverted.X().value(), 4.5, kEpsilon);
+  EXPECT_NEAR(inverted.Y().value(), -7, kEpsilon);
 }
 
 TEST(Translation2dTest, Equality) {
@@ -91,10 +88,10 @@
 
 TEST(Translation2dTest, PolarConstructor) {
   Translation2d one{std::sqrt(2) * 1_m, Rotation2d(45_deg)};
-  EXPECT_NEAR(one.X().to<double>(), 1.0, kEpsilon);
-  EXPECT_NEAR(one.Y().to<double>(), 1.0, kEpsilon);
+  EXPECT_NEAR(one.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(one.Y().value(), 1.0, kEpsilon);
 
   Translation2d two{2_m, Rotation2d(60_deg)};
-  EXPECT_NEAR(two.X().to<double>(), 1.0, kEpsilon);
-  EXPECT_NEAR(two.Y().to<double>(), std::sqrt(3.0), kEpsilon);
+  EXPECT_NEAR(two.X().value(), 1.0, kEpsilon);
+  EXPECT_NEAR(two.Y().value(), std::sqrt(3.0), kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
index 4766bd4..fa9eecc 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/geometry/Twist2dTest.cpp
@@ -1,13 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cmath>
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/geometry/Pose2d.h"
 #include "gtest/gtest.h"
@@ -20,29 +17,28 @@
   const Twist2d straight{5.0_m, 0.0_m, 0.0_rad};
   const auto straightPose = Pose2d().Exp(straight);
 
-  EXPECT_NEAR(straightPose.X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Y().to<double>(), 0.0, kEpsilon);
-  EXPECT_NEAR(straightPose.Rotation().Radians().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(straightPose.X().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Y().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(straightPose.Rotation().Radians().value(), 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 Twist2d quarterCircle{5.0_m / 2.0 * wpi::numbers::pi, 0_m,
+                              units::radian_t(wpi::numbers::pi / 2.0)};
   const auto quarterCirclePose = Pose2d().Exp(quarterCircle);
 
-  EXPECT_NEAR(quarterCirclePose.X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Y().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().to<double>(), 90.0,
-              kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.X().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Y().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(quarterCirclePose.Rotation().Degrees().value(), 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.X().to<double>(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Y().to<double>(), 2.0, kEpsilon);
-  EXPECT_NEAR(diagonalPose.Rotation().Degrees().to<double>(), 0.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.X().value(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Y().value(), 2.0, kEpsilon);
+  EXPECT_NEAR(diagonalPose.Rotation().Degrees().value(), 0.0, kEpsilon);
 }
 
 TEST(Twist2dTest, Equality) {
@@ -63,7 +59,7 @@
 
   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);
+  EXPECT_NEAR(twist.dx.value(), 5 / 2.0 * wpi::numbers::pi, kEpsilon);
+  EXPECT_NEAR(twist.dy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(twist.dtheta.value(), wpi::numbers::pi / 2.0, kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
index 864860a..7665a97 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/ChassisSpeedsTest.cpp
@@ -1,20 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "gtest/gtest.h"
 
 static constexpr double kEpsilon = 1E-9;
 
-TEST(ChassisSpeeds, FieldRelativeConstruction) {
+TEST(ChassisSpeedsTest, 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);
+  EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), kEpsilon);
+  EXPECT_NEAR(1.0, chassisSpeeds.vy.value(), kEpsilon);
+  EXPECT_NEAR(0.5, chassisSpeeds.omega.value(), kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
index 7c1a28d..224e231 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveKinematicsTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/kinematics/ChassisSpeeds.h"
 #include "frc/kinematics/DifferentialDriveKinematics.h"
@@ -18,62 +15,62 @@
 
 static constexpr double kEpsilon = 1E-9;
 
-TEST(DifferentialDriveKinematics, InverseKinematicsFromZero) {
+TEST(DifferentialDriveKinematicsTest, 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);
+  EXPECT_NEAR(wheelSpeeds.left.value(), 0, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.value(), 0, kEpsilon);
 }
 
-TEST(DifferentialDriveKinematics, ForwardKinematicsFromZero) {
+TEST(DifferentialDriveKinematicsTest, 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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
 }
 
-TEST(DifferentialDriveKinematics, InverseKinematicsForStraightLine) {
+TEST(DifferentialDriveKinematicsTest, 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);
+  EXPECT_NEAR(wheelSpeeds.left.value(), 3, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.value(), 3, kEpsilon);
 }
 
-TEST(DifferentialDriveKinematics, ForwardKinematicsForStraightLine) {
+TEST(DifferentialDriveKinematicsTest, 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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 3, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 0, kEpsilon);
 }
 
-TEST(DifferentialDriveKinematics, InverseKinematicsForRotateInPlace) {
+TEST(DifferentialDriveKinematicsTest, 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 ChassisSpeeds chassisSpeeds{
+      0.0_mps, 0.0_mps, units::radians_per_second_t{wpi::numbers::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);
+  EXPECT_NEAR(wheelSpeeds.left.value(), -0.381 * wpi::numbers::pi, kEpsilon);
+  EXPECT_NEAR(wheelSpeeds.right.value(), +0.381 * wpi::numbers::pi, kEpsilon);
 }
 
-TEST(DifferentialDriveKinematics, ForwardKinematicsForRotateInPlace) {
+TEST(DifferentialDriveKinematicsTest, 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)};
+      units::meters_per_second_t(+0.381 * wpi::numbers::pi),
+      units::meters_per_second_t(-0.381 * wpi::numbers::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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), 0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), -wpi::numbers::pi, kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
index 89d65ea..da16b28 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/DifferentialDriveOdometryTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/kinematics/DifferentialDriveKinematics.h"
 #include "frc/kinematics/DifferentialDriveOdometry.h"
@@ -15,13 +12,13 @@
 
 using namespace frc;
 
-TEST(DifferentialDriveOdometry, EncoderDistances) {
+TEST(DifferentialDriveOdometryTest, EncoderDistances) {
   DifferentialDriveOdometry odometry{Rotation2d(45_deg)};
 
   const auto& pose = odometry.Update(Rotation2d(135_deg), 0_m,
-                                     units::meter_t(5 * wpi::math::pi));
+                                     units::meter_t(5 * wpi::numbers::pi));
 
-  EXPECT_NEAR(pose.X().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(pose.Y().to<double>(), 5.0, kEpsilon);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, kEpsilon);
+  EXPECT_NEAR(pose.X().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(pose.Y().value(), 5.0, kEpsilon);
+  EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
index cb03d97..18b72de 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveKinematicsTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/MecanumDriveKinematics.h"
@@ -28,113 +25,69 @@
   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);
+  EXPECT_NEAR(5.0, moduleStates.frontLeft.value(), 0.1);
+  EXPECT_NEAR(5.0, moduleStates.frontRight.value(), 0.1);
+  EXPECT_NEAR(5.0, moduleStates.rearLeft.value(), 0.1);
+  EXPECT_NEAR(5.0, moduleStates.rearRight.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, StraightLineForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{3.536_mps, 3.536_mps, 3.536_mps,
-                                      3.536_mps};
+  MecanumDriveWheelSpeeds wheelSpeeds{5_mps, 5_mps, 5_mps, 5_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);
+  EXPECT_NEAR(5.0, chassisSpeeds.vx.value(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 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);
+  EXPECT_NEAR(-4.0, moduleStates.frontLeft.value(), 0.1);
+  EXPECT_NEAR(4.0, moduleStates.frontRight.value(), 0.1);
+  EXPECT_NEAR(4.0, moduleStates.rearLeft.value(), 0.1);
+  EXPECT_NEAR(-4.0, moduleStates.rearRight.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, StrafeForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-2.828427_mps, 2.828427_mps, 2.828427_mps,
-                                      -2.828427_mps};
+  MecanumDriveWheelSpeeds wheelSpeeds{-5_mps, 5_mps, 5_mps, -5_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);
+  EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
+  EXPECT_NEAR(5.0, chassisSpeeds.vy.value(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.omega.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, RotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
+                       units::radians_per_second_t(2 * wpi::numbers::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);
+  EXPECT_NEAR(-150.79644737, moduleStates.frontLeft.value(), 0.1);
+  EXPECT_NEAR(150.79644737, moduleStates.frontRight.value(), 0.1);
+  EXPECT_NEAR(-150.79644737, moduleStates.rearLeft.value(), 0.1);
+  EXPECT_NEAR(150.79644737, moduleStates.rearRight.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, RotationForwardKinematics) {
-  MecanumDriveWheelSpeeds wheelSpeeds{-106.62919_mps, 106.62919_mps,
-                                      -106.62919_mps, 106.62919_mps};
+  MecanumDriveWheelSpeeds wheelSpeeds{-150.79644737_mps, 150.79644737_mps,
+                                      -150.79644737_mps, 150.79644737_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);
+  EXPECT_NEAR(0.0, chassisSpeeds.vx.value(), 0.1);
+  EXPECT_NEAR(0.0, chassisSpeeds.vy.value(), 0.1);
+  EXPECT_NEAR(2 * wpi::numbers::pi, chassisSpeeds.omega.value(), 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);
+  EXPECT_NEAR(-25.0, moduleStates.frontLeft.value(), 0.1);
+  EXPECT_NEAR(29.0, moduleStates.frontRight.value(), 0.1);
+  EXPECT_NEAR(-19.0, moduleStates.rearLeft.value(), 0.1);
+  EXPECT_NEAR(23.0, moduleStates.rearRight.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, MixedRotationTranslationForwardKinematics) {
@@ -143,31 +96,19 @@
 
   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);
+  EXPECT_NEAR(1.41335, chassisSpeeds.vx.value(), 0.1);
+  EXPECT_NEAR(2.1221, chassisSpeeds.vy.value(), 0.1);
+  EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 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);
+  EXPECT_NEAR(0, moduleStates.frontLeft.value(), 0.1);
+  EXPECT_NEAR(24.0, moduleStates.frontRight.value(), 0.1);
+  EXPECT_NEAR(-24.0, moduleStates.rearLeft.value(), 0.1);
+  EXPECT_NEAR(48.0, moduleStates.rearRight.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest, OffCenterRotationForwardKinematics) {
@@ -175,14 +116,9 @@
                                       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);
+  EXPECT_NEAR(8.48525, chassisSpeeds.vx.value(), 0.1);
+  EXPECT_NEAR(-8.48525, chassisSpeeds.vy.value(), 0.1);
+  EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest,
@@ -190,15 +126,10 @@
   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);
+  EXPECT_NEAR(3.0, moduleStates.frontLeft.value(), 0.1);
+  EXPECT_NEAR(31.0, moduleStates.frontRight.value(), 0.1);
+  EXPECT_NEAR(-17.0, moduleStates.rearLeft.value(), 0.1);
+  EXPECT_NEAR(51.0, moduleStates.rearRight.value(), 0.1);
 }
 
 TEST_F(MecanumDriveKinematicsTest,
@@ -207,24 +138,19 @@
                                       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);
+  EXPECT_NEAR(12.02, chassisSpeeds.vx.value(), 0.1);
+  EXPECT_NEAR(-7.07, chassisSpeeds.vy.value(), 0.1);
+  EXPECT_NEAR(0.707, chassisSpeeds.omega.value(), 0.1);
 }
 
-TEST_F(MecanumDriveKinematicsTest, NormalizeTest) {
+TEST_F(MecanumDriveKinematicsTest, Normalize) {
   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);
+  EXPECT_NEAR(wheelSpeeds.frontLeft.value(), 5.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.frontRight.value(), 6.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearLeft.value(), 4.0 * kFactor, 1E-9);
+  EXPECT_NEAR(wheelSpeeds.rearRight.value(), 7.0 * kFactor, 1E-9);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
index cb85ec7..152506d 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/MecanumDriveOdometryTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/MecanumDriveOdometry.h"
 #include "gtest/gtest.h"
@@ -29,9 +26,9 @@
   odometry.UpdateWithTime(0_s, Rotation2d(), wheelSpeeds);
   auto secondPose = odometry.UpdateWithTime(0.0_s, Rotation2d(), wheelSpeeds);
 
-  EXPECT_NEAR(secondPose.X().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(secondPose.Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(secondPose.Rotation().Radians().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(secondPose.X().value(), 0.0, 0.01);
+  EXPECT_NEAR(secondPose.Y().value(), 0.0, 0.01);
+  EXPECT_NEAR(secondPose.Rotation().Radians().value(), 0.0, 0.01);
 }
 
 TEST_F(MecanumDriveOdometryTest, TwoIterations) {
@@ -41,21 +38,21 @@
   odometry.UpdateWithTime(0_s, Rotation2d(), MecanumDriveWheelSpeeds{});
   auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(), speeds);
 
-  EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
-  EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
+  EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
 }
 
-TEST_F(MecanumDriveOdometryTest, Test90DegreeTurn) {
+TEST_F(MecanumDriveOdometryTest, 90DegreeTurn) {
   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.X().to<double>(), 12, 0.01);
-  EXPECT_NEAR(pose.Y().to<double>(), 12, 0.01);
-  EXPECT_NEAR(pose.Rotation().Degrees().to<double>(), 90.0, 0.01);
+  EXPECT_NEAR(pose.X().value(), 8.4855, 0.01);
+  EXPECT_NEAR(pose.Y().value(), 8.4855, 0.01);
+  EXPECT_NEAR(pose.Rotation().Degrees().value(), 90.0, 0.01);
 }
 
 TEST_F(MecanumDriveOdometryTest, GyroAngleReset) {
@@ -66,7 +63,7 @@
   odometry.UpdateWithTime(0_s, Rotation2d(90_deg), MecanumDriveWheelSpeeds{});
   auto pose = odometry.UpdateWithTime(0.10_s, Rotation2d(90_deg), speeds);
 
-  EXPECT_NEAR(pose.X().to<double>(), 0.5, 0.01);
-  EXPECT_NEAR(pose.Y().to<double>(), 0.0, 0.01);
-  EXPECT_NEAR(pose.Rotation().Radians().to<double>(), 0.0, 0.01);
+  EXPECT_NEAR(pose.X().value(), 0.3536, 0.01);
+  EXPECT_NEAR(pose.Y().value(), 0.0, 0.01);
+  EXPECT_NEAR(pose.Rotation().Radians().value(), 0.0, 0.01);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
index 368dbaf..9384d89 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveKinematicsTest.cpp
@@ -1,11 +1,8 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <wpi/math>
+#include <wpi/numbers>
 
 #include "frc/geometry/Translation2d.h"
 #include "frc/kinematics/SwerveDriveKinematics.h"
@@ -31,15 +28,15 @@
 
   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.speed.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 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);
+  EXPECT_NEAR(fl.angle.Radians().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Radians().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Radians().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Radians().value(), 0.0, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest, StraightLineForwardKinematics) {
@@ -47,49 +44,49 @@
 
   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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 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.speed.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 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);
+  EXPECT_NEAR(fl.angle.Degrees().value(), 90.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().value(), 90.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().value(), 90.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().value(), 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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), 5.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 0.0, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
+                       units::radians_per_second_t(2 * wpi::numbers::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.speed.value(), 106.63, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 106.63, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 106.63, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 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);
+  EXPECT_NEAR(fl.angle.Degrees().value(), 135.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().value(), 45.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().value(), -135.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest, TurnInPlaceForwardKinematics) {
@@ -100,25 +97,25 @@
 
   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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationInverseKinematics) {
   ChassisSpeeds speeds{0_mps, 0_mps,
-                       units::radians_per_second_t(2 * wpi::math::pi)};
+                       units::radians_per_second_t(2 * wpi::numbers::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.speed.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 150.796, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 150.796, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 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);
+  EXPECT_NEAR(fl.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().value(), 0.0, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().value(), -90.0, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().value(), -45.0, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest, OffCenterCORRotationForwardKinematics) {
@@ -129,9 +126,9 @@
 
   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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 75.398, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), -75.398, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 2 * wpi::numbers::pi, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest,
@@ -140,15 +137,15 @@
   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.speed.value(), 23.43, kEpsilon);
+  EXPECT_NEAR(fr.speed.value(), 23.43, kEpsilon);
+  EXPECT_NEAR(bl.speed.value(), 54.08, kEpsilon);
+  EXPECT_NEAR(br.speed.value(), 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);
+  EXPECT_NEAR(fl.angle.Degrees().value(), -140.19, kEpsilon);
+  EXPECT_NEAR(fr.angle.Degrees().value(), -39.81, kEpsilon);
+  EXPECT_NEAR(bl.angle.Degrees().value(), -109.44, kEpsilon);
+  EXPECT_NEAR(br.angle.Degrees().value(), -70.56, kEpsilon);
 }
 
 TEST_F(SwerveDriveKinematicsTest,
@@ -160,24 +157,24 @@
 
   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);
+  EXPECT_NEAR(chassisSpeeds.vx.value(), 0.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.vy.value(), -33.0, kEpsilon);
+  EXPECT_NEAR(chassisSpeeds.omega.value(), 1.5, kEpsilon);
 }
 
-TEST_F(SwerveDriveKinematicsTest, NormalizeTest) {
+TEST_F(SwerveDriveKinematicsTest, Normalize) {
   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};
+  wpi::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);
+  EXPECT_NEAR(arr[0].speed.value(), 5.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[1].speed.value(), 6.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[2].speed.value(), 4.0 * kFactor, kEpsilon);
+  EXPECT_NEAR(arr[3].speed.value(), 7.0 * kFactor, kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
index 40207a1..27d2a6e 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveDriveOdometryTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/kinematics/SwerveDriveKinematics.h"
 #include "frc/kinematics/SwerveDriveOdometry.h"
@@ -34,9 +31,9 @@
   auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(), state, state,
                                         state, state);
 
-  EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
 }
 
 TEST_F(SwerveDriveOdometryTest, 90DegreeTurn) {
@@ -52,9 +49,9 @@
   auto pose =
       m_odometry.UpdateWithTime(1_s, Rotation2d(90_deg), fl, fr, bl, br);
 
-  EXPECT_NEAR(12.0, pose.X().to<double>(), kEpsilon);
-  EXPECT_NEAR(12.0, pose.Y().to<double>(), kEpsilon);
-  EXPECT_NEAR(90.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+  EXPECT_NEAR(12.0, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(12.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(90.0, pose.Rotation().Degrees().value(), kEpsilon);
 }
 
 TEST_F(SwerveDriveOdometryTest, GyroAngleReset) {
@@ -68,7 +65,7 @@
   auto pose = m_odometry.UpdateWithTime(0.1_s, Rotation2d(90_deg), state, state,
                                         state, state);
 
-  EXPECT_NEAR(0.5, pose.X().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Y().to<double>(), kEpsilon);
-  EXPECT_NEAR(0.0, pose.Rotation().Degrees().to<double>(), kEpsilon);
+  EXPECT_NEAR(0.5, pose.X().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Y().value(), kEpsilon);
+  EXPECT_NEAR(0.0, pose.Rotation().Degrees().value(), kEpsilon);
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
new file mode 100644
index 0000000..4880bef
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/kinematics/SwerveModuleStateTest.cpp
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/geometry/Rotation2d.h"
+#include "frc/kinematics/SwerveModuleState.h"
+#include "gtest/gtest.h"
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(SwerveModuleStateTest, Optimize) {
+  frc::Rotation2d angleA{45_deg};
+  frc::SwerveModuleState refA{-2_mps, 180_deg};
+  auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
+
+  EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 0.0, kEpsilon);
+
+  frc::Rotation2d angleB{-50_deg};
+  frc::SwerveModuleState refB{4.7_mps, 41_deg};
+  auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
+
+  EXPECT_NEAR(optimizedB.speed.value(), -4.7, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -139.0, kEpsilon);
+}
+
+TEST(SwerveModuleStateTest, NoOptimize) {
+  frc::Rotation2d angleA{0_deg};
+  frc::SwerveModuleState refA{2_mps, 89_deg};
+  auto optimizedA = frc::SwerveModuleState::Optimize(refA, angleA);
+
+  EXPECT_NEAR(optimizedA.speed.value(), 2.0, kEpsilon);
+  EXPECT_NEAR(optimizedA.angle.Degrees().value(), 89.0, kEpsilon);
+
+  frc::Rotation2d angleB{0_deg};
+  frc::SwerveModuleState refB{-2_mps, -2_deg};
+  auto optimizedB = frc::SwerveModuleState::Optimize(refB, angleB);
+
+  EXPECT_NEAR(optimizedB.speed.value(), -2.0, kEpsilon);
+  EXPECT_NEAR(optimizedB.angle.Degrees().value(), -2.0, kEpsilon);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/main.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/main.cpp
index e2126f2..09072ee 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gtest/gtest.h"
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
index fe084e0..69e202f 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/CubicHermiteSplineTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <iostream>
@@ -56,27 +53,26 @@
 
       // 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>());
+      EXPECT_LT(std::abs(twist.dx.value()),
+                SplineParameterizer::kMaxDx.value());
+      EXPECT_LT(std::abs(twist.dy.value()),
+                SplineParameterizer::kMaxDy.value());
+      EXPECT_LT(std::abs(twist.dtheta.value()),
+                SplineParameterizer::kMaxDtheta.value());
     }
 
     // Check first point.
-    EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
-                a.Rotation().Radians().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Rotation().Radians().value(),
+                a.Rotation().Radians().value(), 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>()) <
+        if (std::abs(waypoint.Distance(state.first.Translation()).value()) <
             1E-9) {
           found = true;
         }
@@ -87,10 +83,10 @@
     EXPECT_TRUE(interiorsGood);
 
     // Check last point.
-    EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
+                b.Rotation().Radians().value(), 1E-9);
 
     static_cast<void>(duration);
   }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
index 30b5b31..25449fb 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/spline/QuinticHermiteSplineTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <chrono>
 #include <iostream>
@@ -43,25 +40,25 @@
 
       // 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>());
+      EXPECT_LT(std::abs(twist.dx.value()),
+                SplineParameterizer::kMaxDx.value());
+      EXPECT_LT(std::abs(twist.dy.value()),
+                SplineParameterizer::kMaxDy.value());
+      EXPECT_LT(std::abs(twist.dtheta.value()),
+                SplineParameterizer::kMaxDtheta.value());
     }
 
     // Check first point.
-    EXPECT_NEAR(poses.front().first.X().to<double>(), a.X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Y().to<double>(), a.Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.front().first.Rotation().Radians().to<double>(),
-                a.Rotation().Radians().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.front().first.X().value(), a.X().value(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Y().value(), a.Y().value(), 1E-9);
+    EXPECT_NEAR(poses.front().first.Rotation().Radians().value(),
+                a.Rotation().Radians().value(), 1E-9);
 
     // Check last point.
-    EXPECT_NEAR(poses.back().first.X().to<double>(), b.X().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Y().to<double>(), b.Y().to<double>(), 1E-9);
-    EXPECT_NEAR(poses.back().first.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
+    EXPECT_NEAR(poses.back().first.X().value(), b.X().value(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Y().value(), b.Y().value(), 1E-9);
+    EXPECT_NEAR(poses.back().first.Rotation().Radians().value(),
+                b.Rotation().Radians().value(), 1E-9);
 
     static_cast<void>(duration);
   }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
index dbeb518..b5a0fdc 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/DiscretizationTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
@@ -12,25 +9,23 @@
 #include "Eigen/Core"
 #include "Eigen/Eigenvalues"
 #include "frc/system/Discretization.h"
-#include "frc/system/RungeKutta.h"
+#include "frc/system/NumericalIntegration.h"
+#include "frc/system/RungeKuttaTimeVarying.h"
 
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
 TEST(DiscretizationTest, DiscretizeA) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
 
-  Eigen::Matrix<double, 2, 1> x0;
-  x0 << 1, 1;
+  Eigen::Vector<double, 2> x0{1, 1};
   Eigen::Matrix<double, 2, 2> discA;
 
   frc::DiscretizeA<2>(contA, 1_s, &discA);
-  Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0;
+  Eigen::Vector<double, 2> x1Discrete = discA * x0;
 
   // We now have pos = vel = 1 and accel = 0, which should give us:
-  Eigen::Matrix<double, 2, 1> x1Truth;
-  x1Truth(1) = x0(1);
-  x1Truth(0) = x0(0) + 1.0 * x0(1);
+  Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1),
+                                   0.0 * x0(0) + 1.0 * x0(1)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
@@ -38,38 +33,30 @@
 // Check that for a simple second-order system that we can easily analyze
 // analytically,
 TEST(DiscretizationTest, DiscretizeAB) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  Eigen::Matrix<double, 2, 1> contB{0, 1};
 
-  Eigen::Matrix<double, 2, 1> contB;
-  contB << 0, 1;
-
-  Eigen::Matrix<double, 2, 1> x0;
-  x0 << 1, 1;
-  Eigen::Matrix<double, 1, 1> u;
-  u << 1;
+  Eigen::Vector<double, 2> x0{1, 1};
+  Eigen::Vector<double, 1> u{1};
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 1> discB;
 
   frc::DiscretizeAB<2, 1>(contA, contB, 1_s, &discA, &discB);
-  Eigen::Matrix<double, 2, 1> x1Discrete = discA * x0 + discB * u;
+  Eigen::Vector<double, 2> x1Discrete = discA * x0 + discB * u;
 
   // We now have pos = vel = accel = 1, which should give us:
-  Eigen::Matrix<double, 2, 1> x1Truth;
-  x1Truth(1) = x0(1) + 1.0 * u(0);
-  x1Truth(0) = x0(0) + 1.0 * x0(1) + 0.5 * u(0);
+  Eigen::Vector<double, 2> x1Truth{1.0 * x0(0) + 1.0 * x0(1) + 0.5 * u(0),
+                                   0.0 * x0(0) + 1.0 * x0(1) + 1.0 * u(0)};
 
   EXPECT_EQ(x1Truth, x1Discrete);
 }
 
-// Test that the discrete approximation of Q is roughly equal to
-// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+//                                             dt
+// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+//                                             0
 TEST(DiscretizationTest, DiscretizeSlowModelAQ) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 1, 0, 0, 1;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
 
   constexpr auto dt = 1_s;
 
@@ -79,10 +66,10 @@
       Eigen::Matrix<double, 2, 2>>(
       [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
         return Eigen::Matrix<double, 2, 2>(
-            (contA * t.to<double>()).exp() * contQ *
-            (contA.transpose() * t.to<double>()).exp());
+            (contA * t.value()).exp() * contQ *
+            (contA.transpose() * t.value()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discQ;
@@ -94,16 +81,14 @@
       << discQIntegrated;
 }
 
-// Test that the discrete approximation of Q is roughly equal to
-// integral from 0 to dt of e^(A tau) Q e^(A.T tau) dtau
+//                                             dt
+// Test that the discrete approximation of Q ≈ ∫ e^(Aτ) Q e^(Aᵀτ) dτ
+//                                             0
 TEST(DiscretizationTest, DiscretizeFastModelAQ) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, -1406.29;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1406.29}};
+  Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
 
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 0.0025, 0, 0, 1;
-
-  constexpr auto dt = 5.05_ms;
+  constexpr auto dt = 5_ms;
 
   Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
       std::function<Eigen::Matrix<double, 2, 2>(
@@ -111,10 +96,10 @@
       Eigen::Matrix<double, 2, 2>>(
       [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
         return Eigen::Matrix<double, 2, 2>(
-            (contA * t.to<double>()).exp() * contQ *
-            (contA.transpose() * t.to<double>()).exp());
+            (contA * t.value()).exp() * contQ *
+            (contA.transpose() * t.value()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discQ;
@@ -128,26 +113,19 @@
 
 // Test that the Taylor series discretization produces nearly identical results.
 TEST(DiscretizationTest, DiscretizeSlowModelAQTaylor) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, 0;
-
-  Eigen::Matrix<double, 2, 1> contB;
-  contB << 0, 1;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 1, 0, 0, 1;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, 0}};
+  Eigen::Matrix<double, 2, 2> contQ{{1, 0}, {0, 1}};
 
   constexpr auto dt = 1_s;
 
   Eigen::Matrix<double, 2, 2> discQTaylor;
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discATaylor;
-  Eigen::Matrix<double, 2, 1> discB;
 
   // Continuous Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
-  for (int i = 0; i < contQ.rows(); i++) {
-    EXPECT_GT(esCont.eigenvalues()[i], 0);
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont{contQ};
+  for (int i = 0; i < contQ.rows(); ++i) {
+    EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
 
   Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -156,12 +134,12 @@
       Eigen::Matrix<double, 2, 2>>(
       [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
         return Eigen::Matrix<double, 2, 2>(
-            (contA * t.to<double>()).exp() * contQ *
-            (contA.transpose() * t.to<double>()).exp());
+            (contA * t.value()).exp() * contQ *
+            (contA.transpose() * t.value()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
-  frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+  frc::DiscretizeA<2>(contA, dt, &discA);
   frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
 
   EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-10)
@@ -171,34 +149,27 @@
   EXPECT_LT((discA - discATaylor).norm(), 1e-10);
 
   // Discrete Q should be positive semidefinite
-  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
-  for (int i = 0; i < discQTaylor.rows(); i++) {
-    EXPECT_GT(esDisc.eigenvalues()[i], 0);
+  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc{discQTaylor};
+  for (int i = 0; i < discQTaylor.rows(); ++i) {
+    EXPECT_GE(esDisc.eigenvalues()[i], 0);
   }
 }
 
 // Test that the Taylor series discretization produces nearly identical results.
 TEST(DiscretizationTest, DiscretizeFastModelAQTaylor) {
-  Eigen::Matrix<double, 2, 2> contA;
-  contA << 0, 1, 0, -1500;
+  Eigen::Matrix<double, 2, 2> contA{{0, 1}, {0, -1500}};
+  Eigen::Matrix<double, 2, 2> contQ{{0.0025, 0}, {0, 1}};
 
-  Eigen::Matrix<double, 2, 1> contB;
-  contB << 0, 1;
-
-  Eigen::Matrix<double, 2, 2> contQ;
-  contQ << 0.0025, 0, 0, 1;
-
-  constexpr auto dt = 5.05_ms;
+  constexpr auto dt = 5_ms;
 
   Eigen::Matrix<double, 2, 2> discQTaylor;
   Eigen::Matrix<double, 2, 2> discA;
   Eigen::Matrix<double, 2, 2> discATaylor;
-  Eigen::Matrix<double, 2, 1> discB;
 
   // Continuous Q should be positive semidefinite
   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esCont(contQ);
-  for (int i = 0; i < contQ.rows(); i++) {
-    EXPECT_GT(esCont.eigenvalues()[i], 0);
+  for (int i = 0; i < contQ.rows(); ++i) {
+    EXPECT_GE(esCont.eigenvalues()[i], 0);
   }
 
   Eigen::Matrix<double, 2, 2> discQIntegrated = frc::RungeKuttaTimeVarying<
@@ -207,12 +178,12 @@
       Eigen::Matrix<double, 2, 2>>(
       [&](units::second_t t, const Eigen::Matrix<double, 2, 2>&) {
         return Eigen::Matrix<double, 2, 2>(
-            (contA * t.to<double>()).exp() * contQ *
-            (contA.transpose() * t.to<double>()).exp());
+            (contA * t.value()).exp() * contQ *
+            (contA.transpose() * t.value()).exp());
       },
-      Eigen::Matrix<double, 2, 2>::Zero(), 0_s, dt);
+      0_s, Eigen::Matrix<double, 2, 2>::Zero(), dt);
 
-  frc::DiscretizeAB<2, 1>(contA, contB, dt, &discA, &discB);
+  frc::DiscretizeA<2>(contA, dt, &discA);
   frc::DiscretizeAQTaylor<2>(contA, contQ, dt, &discATaylor, &discQTaylor);
 
   EXPECT_LT((discQIntegrated - discQTaylor).norm(), 1e-3)
@@ -223,18 +194,15 @@
 
   // Discrete Q should be positive semidefinite
   Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> esDisc(discQTaylor);
-  for (int i = 0; i < discQTaylor.rows(); i++) {
-    EXPECT_GT(esDisc.eigenvalues()[i], 0);
+  for (int i = 0; i < discQTaylor.rows(); ++i) {
+    EXPECT_GE(esDisc.eigenvalues()[i], 0);
   }
 }
 
 // Test that DiscretizeR() works
 TEST(DiscretizationTest, DiscretizeR) {
-  Eigen::Matrix<double, 2, 2> contR;
-  contR << 2.0, 0.0, 0.0, 1.0;
-
-  Eigen::Matrix<double, 2, 2> discRTruth;
-  discRTruth << 4.0, 0.0, 0.0, 2.0;
+  Eigen::Matrix<double, 2, 2> contR{{2.0, 0.0}, {0.0, 1.0}};
+  Eigen::Matrix<double, 2, 2> discRTruth{{4.0, 0.0}, {0.0, 2.0}};
 
   Eigen::Matrix<double, 2, 2> discR = frc::DiscretizeR<2>(contR, 500_ms);
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
index eedf8c0..1fa12c2 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/LinearSystemIDTest.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <frc/system/LinearSystem.h>
 #include <frc/system/plant/DCMotor.h>
 #include <frc/system/plant/LinearSystemId.h>
 #include <gtest/gtest.h>
 
-#include "frc/StateSpaceUtil.h"
 #include "frc/system/plant/LinearSystemId.h"
 #include "units/length.h"
 #include "units/mass.h"
@@ -20,32 +16,37 @@
       frc::DCMotor::NEO(4), 70_kg, 0.05_m, 0.4_m, 6.0_kg_sq_m, 6.0);
 
   ASSERT_TRUE(model.A().isApprox(
-      frc::MakeMatrix<2, 2>(-10.14132, 3.06598, 3.06598, -10.14132), 0.001));
+      Eigen::Matrix<double, 2, 2>{{-10.14132, 3.06598}, {3.06598, -10.14132}},
+      0.001));
   ASSERT_TRUE(model.B().isApprox(
-      frc::MakeMatrix<2, 2>(4.2590, -1.28762, -1.2876, 4.2590), 0.001));
-  ASSERT_TRUE(
-      model.C().isApprox(frc::MakeMatrix<2, 2>(1.0, 0.0, 0.0, 1.0), 0.001));
-  ASSERT_TRUE(
-      model.D().isApprox(frc::MakeMatrix<2, 2>(0.0, 0.0, 0.0, 0.0), 0.001));
+      Eigen::Matrix<double, 2, 2>{{4.2590, -1.28762}, {-1.2876, 4.2590}},
+      0.001));
+  ASSERT_TRUE(model.C().isApprox(
+      Eigen::Matrix<double, 2, 2>{{1.0, 0.0}, {0.0, 1.0}}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(
+      Eigen::Matrix<double, 2, 2>{{0.0, 0.0}, {0.0, 0.0}}, 0.001));
 }
 
 TEST(LinearSystemIDTest, ElevatorSystem) {
   auto model = frc::LinearSystemId::ElevatorSystem(frc::DCMotor::NEO(2), 5_kg,
                                                    0.05_m, 12);
   ASSERT_TRUE(model.A().isApprox(
-      frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -99.05473), 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 20.8), 0.001));
-  ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 2>(1.0, 0.0), 0.001));
-  ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+      Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -99.05473}}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 20.8}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 2>{1.0, 0.0}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, FlywheelSystem) {
   auto model = frc::LinearSystemId::FlywheelSystem(frc::DCMotor::NEO(2),
                                                    0.00032_kg_sq_m, 1.0);
-  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-26.87032), 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1354.166667), 0.001));
-  ASSERT_TRUE(model.C().isApprox(frc::MakeMatrix<1, 1>(1.0), 0.001));
-  ASSERT_TRUE(model.D().isApprox(frc::MakeMatrix<1, 1>(0.0), 0.001));
+  ASSERT_TRUE(
+      model.A().isApprox(Eigen::Matrix<double, 1, 1>{-26.87032}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 1, 1>{1354.166667}, 0.001));
+  ASSERT_TRUE(model.C().isApprox(Eigen::Matrix<double, 1, 1>{1.0}, 0.001));
+  ASSERT_TRUE(model.D().isApprox(Eigen::Matrix<double, 1, 1>{0.0}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyPositionSystem) {
@@ -56,9 +57,10 @@
   auto model = frc::LinearSystemId::IdentifyPositionSystem<units::meter>(
       kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
-  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<2, 2>(0.0, 1.0, 0.0, -kv / ka),
-                                 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<2, 1>(0.0, 1.0 / ka), 0.001));
+  ASSERT_TRUE(model.A().isApprox(
+      Eigen::Matrix<double, 2, 2>{{0.0, 1.0}, {0.0, -kv / ka}}, 0.001));
+  ASSERT_TRUE(
+      model.B().isApprox(Eigen::Matrix<double, 2, 1>{0.0, 1.0 / ka}, 0.001));
 }
 
 TEST(LinearSystemIDTest, IdentifyVelocitySystem) {
@@ -70,6 +72,6 @@
   auto model = frc::LinearSystemId::IdentifyVelocitySystem<units::meter>(
       kv * 1_V / 1_mps, ka * 1_V / 1_mps_sq);
 
-  ASSERT_TRUE(model.A().isApprox(frc::MakeMatrix<1, 1>(-kv / ka), 0.001));
-  ASSERT_TRUE(model.B().isApprox(frc::MakeMatrix<1, 1>(1.0 / ka), 0.001));
+  ASSERT_TRUE(model.A().isApprox(Eigen::Matrix<double, 1, 1>{-kv / ka}, 0.001));
+  ASSERT_TRUE(model.B().isApprox(Eigen::Matrix<double, 1, 1>{1.0 / ka}, 0.001));
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
new file mode 100644
index 0000000..fd9c039
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalIntegrationTest.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/system/NumericalIntegration.h"
+
+// Tests that integrating dx/dt = e^x works.
+TEST(NumericalIntegrationTest, Exponential) {
+  Eigen::Vector<double, 1> y0{0.0};
+
+  Eigen::Vector<double, 1> y1 = frc::RK4(
+      [](const Eigen::Vector<double, 1>& x) {
+        return Eigen::Vector<double, 1>{std::exp(x(0))};
+      },
+      y0, 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works when we provide a U.
+TEST(NumericalIntegrationTest, ExponentialWithU) {
+  Eigen::Vector<double, 1> y0{0.0};
+
+  Eigen::Vector<double, 1> y1 = frc::RK4(
+      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+        return Eigen::Vector<double, 1>{std::exp(u(0) * x(0))};
+      },
+      y0, Eigen::Vector<double, 1>{1.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works with RKF45.
+TEST(NumericalIntegrationTest, ExponentialRKF45) {
+  Eigen::Vector<double, 1> y0{0.0};
+
+  Eigen::Vector<double, 1> y1 = frc::RKF45(
+      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+        return Eigen::Vector<double, 1>{std::exp(x(0))};
+      },
+      y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
+
+// Tests that integrating dx/dt = e^x works with RKDP
+TEST(NumericalIntegrationTest, ExponentialRKDP) {
+  Eigen::Vector<double, 1> y0{0.0};
+
+  Eigen::Vector<double, 1> y1 = frc::RKDP(
+      [](const Eigen::Vector<double, 1>& x, const Eigen::Vector<double, 1>& u) {
+        return Eigen::Vector<double, 1>{std::exp(x(0))};
+      },
+      y0, Eigen::Vector<double, 1>{0.0}, 0.1_s);
+  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
index ddc3f68..4e64825 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/NumericalJacobianTest.cpp
@@ -1,68 +1,58 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <gtest/gtest.h>
 
 #include "frc/system/NumericalJacobian.h"
 
-Eigen::Matrix<double, 4, 4> A = (Eigen::Matrix<double, 4, 4>() << 1, 2, 4, 1, 5,
-                                 2, 3, 4, 5, 1, 3, 2, 1, 1, 3, 7)
-                                    .finished();
-
-Eigen::Matrix<double, 4, 2> B =
-    (Eigen::Matrix<double, 4, 2>() << 1, 1, 2, 1, 3, 2, 3, 7).finished();
+Eigen::Matrix<double, 4, 4> A{
+    {1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}, {1, 1, 3, 7}};
+Eigen::Matrix<double, 4, 2> B{{1, 1}, {2, 1}, {3, 2}, {3, 7}};
 
 // Function from which to recover A and B
-Eigen::Matrix<double, 4, 1> AxBuFn(const Eigen::Matrix<double, 4, 1>& x,
-                                   const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 4> AxBuFn(const Eigen::Vector<double, 4>& x,
+                                const Eigen::Vector<double, 2>& u) {
   return A * x + B * u;
 }
 
 // Test that we can recover A from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Ax) {
-  Eigen::Matrix<double, 4, 4> newA = frc::NumericalJacobianX<4, 4, 2>(
-      AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 4, 4> newA =
+      frc::NumericalJacobianX<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newA.isApprox(A));
 }
 
 // Test that we can recover B from AxBuFn() pretty accurately
 TEST(NumericalJacobianTest, Bu) {
-  Eigen::Matrix<double, 4, 2> newB = frc::NumericalJacobianU<4, 4, 2>(
-      AxBuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 4, 2> newB =
+      frc::NumericalJacobianU<4, 4, 2>(AxBuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newB.isApprox(B));
 }
 
-Eigen::Matrix<double, 3, 4> C =
-    (Eigen::Matrix<double, 3, 4>() << 1, 2, 4, 1, 5, 2, 3, 4, 5, 1, 3, 2)
-        .finished();
-
-Eigen::Matrix<double, 3, 2> D =
-    (Eigen::Matrix<double, 3, 2>() << 1, 1, 2, 1, 3, 2).finished();
+Eigen::Matrix<double, 3, 4> C{{1, 2, 4, 1}, {5, 2, 3, 4}, {5, 1, 3, 2}};
+Eigen::Matrix<double, 3, 2> D{{1, 1}, {2, 1}, {3, 2}};
 
 // Function from which to recover C and D
-Eigen::Matrix<double, 3, 1> CxDuFn(const Eigen::Matrix<double, 4, 1>& x,
-                                   const Eigen::Matrix<double, 2, 1>& u) {
+Eigen::Vector<double, 3> CxDuFn(const Eigen::Vector<double, 4>& x,
+                                const Eigen::Vector<double, 2>& u) {
   return C * x + D * u;
 }
 
 // Test that we can recover C from CxDuFn() pretty accurately
 TEST(NumericalJacobianTest, Cx) {
-  Eigen::Matrix<double, 3, 4> newC = frc::NumericalJacobianX<3, 4, 2>(
-      CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 3, 4> newC =
+      frc::NumericalJacobianX<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newC.isApprox(C));
 }
 
 // Test that we can recover D from CxDuFn() pretty accurately
 TEST(NumericalJacobianTest, Du) {
-  Eigen::Matrix<double, 3, 2> newD = frc::NumericalJacobianU<3, 4, 2>(
-      CxDuFn, Eigen::Matrix<double, 4, 1>::Zero(),
-      Eigen::Matrix<double, 2, 1>::Zero());
+  Eigen::Matrix<double, 3, 2> newD =
+      frc::NumericalJacobianU<3, 4, 2>(CxDuFn, Eigen::Vector<double, 4>::Zero(),
+                                       Eigen::Vector<double, 2>::Zero());
   EXPECT_TRUE(newD.isApprox(D));
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
deleted file mode 100644
index a12c1b7..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/system/RungeKuttaTest.cpp
+++ /dev/null
@@ -1,71 +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 <gtest/gtest.h>
-
-#include <cmath>
-
-#include "frc/system/RungeKutta.h"
-
-// Tests that integrating dx/dt = e^x works.
-TEST(RungeKuttaTest, Exponential) {
-  Eigen::Matrix<double, 1, 1> y0;
-  y0(0) = 0.0;
-
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
-      [](Eigen::Matrix<double, 1, 1> x) {
-        Eigen::Matrix<double, 1, 1> y;
-        y(0) = std::exp(x(0));
-        return y;
-      },
-      y0, 0.1_s);
-  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
-}
-
-// Tests that integrating dx/dt = e^x works when we provide a U.
-TEST(RungeKuttaTest, ExponentialWithU) {
-  Eigen::Matrix<double, 1, 1> y0;
-  y0(0) = 0.0;
-
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKutta(
-      [](Eigen::Matrix<double, 1, 1> x, Eigen::Matrix<double, 1, 1> u) {
-        Eigen::Matrix<double, 1, 1> y;
-        y(0) = std::exp(u(0) * x(0));
-        return y;
-      },
-      y0, (Eigen::Matrix<double, 1, 1>() << 1.0).finished(), 0.1_s);
-  EXPECT_NEAR(y1(0), std::exp(0.1) - std::exp(0), 1e-3);
-}
-
-namespace {
-Eigen::Matrix<double, 1, 1> RungeKuttaTimeVaryingSolution(double t) {
-  return (Eigen::Matrix<double, 1, 1>()
-          << 12.0 * std::exp(t) / (std::pow(std::exp(t) + 1.0, 2.0)))
-      .finished();
-}
-}  // namespace
-
-// Tests RungeKutta with a time varying solution.
-// Now, lets test RK4 with a time varying solution.  From
-// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
-//   x' = x (2 / (e^t + 1) - 1)
-//
-// The true (analytical) solution is:
-//
-// x(t) = 12 * e^t / ((e^t + 1)^2)
-TEST(RungeKuttaTest, RungeKuttaTimeVarying) {
-  Eigen::Matrix<double, 1, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
-
-  Eigen::Matrix<double, 1, 1> y1 = frc::RungeKuttaTimeVarying(
-      [](units::second_t t, Eigen::Matrix<double, 1, 1> x) {
-        return (Eigen::Matrix<double, 1, 1>()
-                << x(0) * (2.0 / (std::exp(t.to<double>()) + 1.0) - 1.0))
-            .finished();
-      },
-      y0, 5_s, 1_s);
-  EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
-}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
new file mode 100644
index 0000000..f1be861
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/system/RungeKuttaTimeVaryingTest.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+#include "frc/system/RungeKuttaTimeVarying.h"
+
+namespace {
+Eigen::Vector<double, 1> RungeKuttaTimeVaryingSolution(double t) {
+  return Eigen::Vector<double, 1>{12.0 * std::exp(t) /
+                                  (std::pow(std::exp(t) + 1.0, 2.0))};
+}
+}  // namespace
+
+// Tests RK4 with a time varying solution. From
+// http://www2.hawaii.edu/~jmcfatri/math407/RungeKuttaTest.html:
+//   x' = x (2 / (e^t + 1) - 1)
+//
+// The true (analytical) solution is:
+//
+// x(t) = 12 * e^t / ((e^t + 1)^2)
+TEST(RungeKuttaTimeVaryingTest, RungeKuttaTimeVarying) {
+  Eigen::Vector<double, 1> y0 = RungeKuttaTimeVaryingSolution(5.0);
+
+  Eigen::Vector<double, 1> y1 = frc::RungeKuttaTimeVarying(
+      [](units::second_t t, const Eigen::Vector<double, 1>& x) {
+        return Eigen::Vector<double, 1>{
+            x(0) * (2.0 / (std::exp(t.value()) + 1.0) - 1.0)};
+      },
+      5_s, y0, 1_s);
+  EXPECT_NEAR(y1(0), RungeKuttaTimeVaryingSolution(6.0)(0), 1e-3);
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
index 42e9fe2..e2f7112 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/CentripetalAccelerationConstraintTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <memory>
 #include <vector>
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
index 636b002..e3723b5 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveKinematicsTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <memory>
 #include <vector>
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
index 6cd8075..b21ef7b 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/DifferentialDriveVoltageTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <iostream>
 #include <memory>
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
index 44c4222..88dc2b8 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/EllipticalRegionConstraintTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <vector>
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
index 33f753f..77310ae 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/RectangularRegionConstraintTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <vector>
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
new file mode 100644
index 0000000..2b733a8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryConcatenateTest.cpp
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/trajectory/TrajectoryConfig.h"
+#include "frc/trajectory/TrajectoryGenerator.h"
+#include "gtest/gtest.h"
+
+TEST(TrajectoryConcatenateTest, States) {
+  auto t1 = frc::TrajectoryGenerator::GenerateTrajectory(
+      {}, {}, {1_m, 1_m, 0_deg}, {2_mps, 2_mps_sq});
+  auto t2 = frc::TrajectoryGenerator::GenerateTrajectory(
+      {1_m, 1_m, 0_deg}, {}, {2_m, 2_m, 45_deg}, {2_mps, 2_mps_sq});
+
+  auto t = t1 + t2;
+
+  double time = -1.0;
+  for (size_t i = 0; i < t.States().size(); ++i) {
+    const auto& state = t.States()[i];
+
+    // Make sure that the timestamps are strictly increasing.
+    EXPECT_GT(state.t.value(), time);
+    time = state.t.value();
+
+    // Ensure that the states in t are the same as those in t1 and t2.
+    if (i < t1.States().size()) {
+      EXPECT_EQ(state, t1.States()[i]);
+    } else {
+      auto st = t2.States()[i - t1.States().size() + 1];
+      st.t += t1.TotalTime();
+      EXPECT_EQ(state, st);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
index 378aff7..175becf 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryGeneratorTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <vector>
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
index c18a3e9..90c6dc0 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryJsonTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/TrajectoryConfig.h"
 #include "frc/trajectory/TrajectoryUtil.h"
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
index 349ff5c..0c6e07a 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrajectoryTransformTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <vector>
 
@@ -24,14 +21,14 @@
     auto a = a2.RelativeTo(a1);
     auto b = b2.RelativeTo(b1);
 
-    EXPECT_NEAR(a.X().to<double>(), b.X().to<double>(), 1E-9);
-    EXPECT_NEAR(a.Y().to<double>(), b.Y().to<double>(), 1E-9);
-    EXPECT_NEAR(a.Rotation().Radians().to<double>(),
-                b.Rotation().Radians().to<double>(), 1E-9);
+    EXPECT_NEAR(a.X().value(), b.X().value(), 1E-9);
+    EXPECT_NEAR(a.Y().value(), b.Y().value(), 1E-9);
+    EXPECT_NEAR(a.Rotation().Radians().value(), b.Rotation().Radians().value(),
+                1E-9);
   }
 }
 
-TEST(TrajectoryTransforms, TransformBy) {
+TEST(TrajectoryTransformsTest, 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)},
@@ -42,14 +39,14 @@
 
   auto firstPose = transformedTrajectory.Sample(0_s).pose;
 
-  EXPECT_NEAR(firstPose.X().to<double>(), 1.0, 1E-9);
-  EXPECT_NEAR(firstPose.Y().to<double>(), 2.0, 1E-9);
-  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 30.0, 1E-9);
+  EXPECT_NEAR(firstPose.X().value(), 1.0, 1E-9);
+  EXPECT_NEAR(firstPose.Y().value(), 2.0, 1E-9);
+  EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 30.0, 1E-9);
 
   TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
 }
 
-TEST(TrajectoryTransforms, RelativeTo) {
+TEST(TrajectoryTransformsTest, RelativeTo) {
   frc::TrajectoryConfig config{3_mps, 3_mps_sq};
   auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
       frc::Pose2d{1_m, 2_m, frc::Rotation2d(30_deg)}, {},
@@ -60,9 +57,9 @@
 
   auto firstPose = transformedTrajectory.Sample(0_s).pose;
 
-  EXPECT_NEAR(firstPose.X().to<double>(), 0, 1E-9);
-  EXPECT_NEAR(firstPose.Y().to<double>(), 0, 1E-9);
-  EXPECT_NEAR(firstPose.Rotation().Degrees().to<double>(), 0, 1E-9);
+  EXPECT_NEAR(firstPose.X().value(), 0, 1E-9);
+  EXPECT_NEAR(firstPose.Y().value(), 0, 1E-9);
+  EXPECT_NEAR(firstPose.Rotation().Degrees().value(), 0, 1E-9);
 
   TestSameShapedTrajectory(trajectory.States(), transformedTrajectory.States());
 }
diff --git a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
index 63ea916..6a35261 100644
--- a/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
+++ b/third_party/allwpilib/wpimath/src/test/native/cpp/trajectory/TrapezoidProfileTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "frc/trajectory/TrapezoidProfile.h"  // NOLINT(build/include_order)
 
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiff.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiff.h
deleted file mode 100644
index 66fd88a..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiff.h
+++ /dev/null
@@ -1,34 +0,0 @@
-#pragma once
-/// @file This header provides a single inclusion point for autodiff-related
-/// header files in the `drake/common` directory. Users should include this
-/// file. Including other individual headers such as `drake/common/autodiffxd.h`
-/// will generate a compile-time warning.
-
-// In each header included below, it asserts that this macro
-// `DRAKE_COMMON_AUTODIFF_HEADER` is defined. If the macro is not defined, it
-// generates diagnostic warning messages.
-#define DRAKE_COMMON_AUTODIFF_HEADER
-
-#include <Eigen/Core>
-#include <unsupported/Eigen/AutoDiff>
-
-static_assert(EIGEN_VERSION_AT_LEAST(3, 3, 3),
-              "Drake requires Eigen >= v3.3.3.");
-
-// Do not alpha-sort the following block of hard-coded #includes, which is
-// protected by `clang-format on/off`.
-//
-// Rationale: We want to maximize the use of this header, `autodiff.h`, even
-// inside of the autodiff-related files to avoid any mistakes which might not be
-// detected. By centralizing the list here, we make sure that everyone will see
-// the correct order which respects the inter-dependencies of the autodiff
-// headers. This shields us from triggering undefined behaviors due to
-// order-of-specialization-includes-changed mistakes.
-//
-// clang-format off
-#include "drake/common/eigen_autodiff_limits.h"
-#include "drake/common/eigen_autodiff_types.h"
-#include "drake/common/autodiffxd.h"
-#include "drake/common/autodiff_overloads.h"
-// clang-format on
-#undef DRAKE_COMMON_AUTODIFF_HEADER
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiff_overloads.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiff_overloads.h
deleted file mode 100644
index 7eaeb3f..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiff_overloads.h
+++ /dev/null
@@ -1,203 +0,0 @@
-/// @file
-/// Overloads for STL mathematical operations on AutoDiffScalar.
-///
-/// Used via argument-dependent lookup (ADL). These functions appear
-/// in the Eigen namespace so that ADL can automatically choose between
-/// the STL version and the overloaded version to match the type of the
-/// arguments. The proper use would be e.g.
-///
-/// \code{.cc}
-///    void mymethod() {
-///       using std::isinf;
-///       isinf(myval);
-///    }
-/// \endcode{}
-///
-/// @note The if_then_else and cond functions for AutoDiffScalar are in
-/// namespace drake because cond is defined in namespace drake in
-/// "drake/common/cond.h" file.
-
-#pragma once
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <cmath>
-#include <limits>
-
-#include "drake/common/cond.h"
-#include "drake/common/drake_assert.h"
-#include "drake/common/dummy_value.h"
-
-namespace Eigen {
-
-/// Overloads nexttoward to mimic std::nexttoward from <cmath>.
-template <typename DerType>
-double nexttoward(const Eigen::AutoDiffScalar<DerType>& from, long double to) {
-  using std::nexttoward;
-  return nexttoward(from.value(), to);
-}
-
-/// Overloads round to mimic std::round from <cmath>.
-template <typename DerType>
-double round(const Eigen::AutoDiffScalar<DerType>& x) {
-  using std::round;
-  return round(x.value());
-}
-
-/// Overloads isinf to mimic std::isinf from <cmath>.
-template <typename DerType>
-bool isinf(const Eigen::AutoDiffScalar<DerType>& x) {
-  using std::isinf;
-  return isinf(x.value());
-}
-
-/// Overloads isfinite to mimic std::isfinite from <cmath>.
-template <typename DerType>
-bool isfinite(const Eigen::AutoDiffScalar<DerType>& x) {
-  using std::isfinite;
-  return isfinite(x.value());
-}
-
-/// Overloads isnan to mimic std::isnan from <cmath>.
-template <typename DerType>
-bool isnan(const Eigen::AutoDiffScalar<DerType>& x) {
-  using std::isnan;
-  return isnan(x.value());
-}
-
-/// Overloads floor to mimic std::floor from <cmath>.
-template <typename DerType>
-double floor(const Eigen::AutoDiffScalar<DerType>& x) {
-  using std::floor;
-  return floor(x.value());
-}
-
-/// Overloads ceil to mimic std::ceil from <cmath>.
-template <typename DerType>
-double ceil(const Eigen::AutoDiffScalar<DerType>& x) {
-  using std::ceil;
-  return ceil(x.value());
-}
-
-/// Overloads copysign from <cmath>.
-template <typename DerType, typename T>
-Eigen::AutoDiffScalar<DerType> copysign(const Eigen::AutoDiffScalar<DerType>& x,
-                                        const T& y) {
-  using std::isnan;
-  if (isnan(x)) return (y >= 0) ? NAN : -NAN;
-  if ((x < 0 && y >= 0) || (x >= 0 && y < 0))
-    return -x;
-  else
-    return x;
-}
-
-/// Overloads copysign from <cmath>.
-template <typename DerType>
-double copysign(double x, const Eigen::AutoDiffScalar<DerType>& y) {
-  using std::isnan;
-  if (isnan(x)) return (y >= 0) ? NAN : -NAN;
-  if ((x < 0 && y >= 0) || (x >= 0 && y < 0))
-    return -x;
-  else
-    return x;
-}
-
-/// Overloads pow for an AutoDiffScalar base and exponent, implementing the
-/// chain rule.
-template <typename DerTypeA, typename DerTypeB>
-Eigen::AutoDiffScalar<
-    typename internal::remove_all<DerTypeA>::type::PlainObject>
-pow(const Eigen::AutoDiffScalar<DerTypeA>& base,
-    const Eigen::AutoDiffScalar<DerTypeB>& exponent) {
-  // The two AutoDiffScalars being exponentiated must have the same matrix
-  // type. This includes, but is not limited to, the same scalar type and
-  // the same dimension.
-  static_assert(
-      std::is_same<
-          typename internal::remove_all<DerTypeA>::type::PlainObject,
-          typename internal::remove_all<DerTypeB>::type::PlainObject>::value,
-      "The derivative types must match.");
-
-  internal::make_coherent(base.derivatives(), exponent.derivatives());
-
-  const auto& x = base.value();
-  const auto& xgrad = base.derivatives();
-  const auto& y = exponent.value();
-  const auto& ygrad = exponent.derivatives();
-
-  using std::pow;
-  using std::log;
-  const auto x_to_the_y = pow(x, y);
-  if (ygrad.isZero(std::numeric_limits<double>::epsilon()) ||
-      ygrad.size() == 0) {
-    // The derivative only depends on ∂(x^y)/∂x -- this prevents undefined
-    // behavior in the corner case where ∂(x^y)/∂y is infinite when x = 0,
-    // despite ∂y/∂v being 0.
-    return Eigen::MakeAutoDiffScalar(x_to_the_y, y * pow(x, y - 1) * xgrad);
-  }
-  return Eigen::MakeAutoDiffScalar(
-      // The value is x ^ y.
-      x_to_the_y,
-      // The multivariable chain rule states:
-      // df/dv_i = (∂f/∂x * dx/dv_i) + (∂f/∂y * dy/dv_i)
-      // ∂f/∂x is y*x^(y-1)
-      y * pow(x, y - 1) * xgrad +
-      // ∂f/∂y is (x^y)*ln(x)
-      x_to_the_y * log(x) * ygrad);
-}
-
-}  // namespace Eigen
-
-namespace drake {
-
-/// Returns the autodiff scalar's value() as a double.  Never throws.
-/// Overloads ExtractDoubleOrThrow from common/extract_double.h.
-template <typename DerType>
-double ExtractDoubleOrThrow(const Eigen::AutoDiffScalar<DerType>& scalar) {
-  return static_cast<double>(scalar.value());
-}
-
-/// Specializes common/dummy_value.h.
-template <typename DerType>
-struct dummy_value<Eigen::AutoDiffScalar<DerType>> {
-  static constexpr Eigen::AutoDiffScalar<DerType> get() {
-    constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
-    DerType derivatives;
-    derivatives.fill(kNaN);
-    return Eigen::AutoDiffScalar<DerType>(kNaN, derivatives);
-  }
-};
-
-/// Provides if-then-else expression for Eigen::AutoDiffScalar type. To support
-/// Eigen's generic expressions, we use casting to the plain object after
-/// applying Eigen::internal::remove_all. It is based on the Eigen's
-/// implementation of min/max function for AutoDiffScalar type
-/// (https://bitbucket.org/eigen/eigen/src/10a1de58614569c9250df88bdfc6402024687bc6/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h?at=default&fileviewer=file-view-default#AutoDiffScalar.h-546).
-template <typename DerType1, typename DerType2>
-inline Eigen::AutoDiffScalar<
-    typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
-if_then_else(bool f_cond, const Eigen::AutoDiffScalar<DerType1>& x,
-             const Eigen::AutoDiffScalar<DerType2>& y) {
-  typedef Eigen::AutoDiffScalar<
-      typename Eigen::internal::remove_all<DerType1>::type::PlainObject>
-      ADS1;
-  typedef Eigen::AutoDiffScalar<
-      typename Eigen::internal::remove_all<DerType2>::type::PlainObject>
-      ADS2;
-  static_assert(std::is_same<ADS1, ADS2>::value,
-                "The derivative types must match.");
-  return f_cond ? ADS1(x) : ADS2(y);
-}
-
-/// Provides special case of cond expression for Eigen::AutoDiffScalar type.
-template <typename DerType, typename... Rest>
-Eigen::AutoDiffScalar<
-    typename Eigen::internal::remove_all<DerType>::type::PlainObject>
-cond(bool f_cond, const Eigen::AutoDiffScalar<DerType>& e_then, Rest... rest) {
-  return if_then_else(f_cond, e_then, cond(rest...));
-}
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiffxd.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiffxd.h
deleted file mode 100644
index a99b2f0..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/autodiffxd.h
+++ /dev/null
@@ -1,423 +0,0 @@
-#pragma once
-
-// This file is a modification of Eigen-3.3.3's AutoDiffScalar.h file which is
-// available at
-// https://bitbucket.org/eigen/eigen/raw/67e894c6cd8f5f1f604b27d37ed47fdf012674ff/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h
-//
-// Copyright (C) 2009 Gael Guennebaud <gael.guennebaud@inria.fr>
-// Copyright (C) 2017 Drake Authors
-//
-// This Source Code Form is subject to the terms of the Mozilla
-// Public License v. 2.0. If a copy of the MPL was not distributed
-// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <cmath>
-#include <ostream>
-
-#include <Eigen/Core>
-
-namespace Eigen {
-
-#if !defined(DRAKE_DOXYGEN_CXX)
-// Explicit template specializations of Eigen::AutoDiffScalar for VectorXd.
-//
-// AutoDiffScalar tries to call internal::make_coherent to promote empty
-// derivatives. However, it fails to do the promotion when an operand is an
-// expression tree (i.e. CwiseBinaryOp). Our solution is to provide special
-// overloading for VectorXd and change the return types of its operators. With
-// this change, the operators evaluate terms immediately and return an
-// AutoDiffScalar<VectorXd> instead of expression trees (such as CwiseBinaryOp).
-// Eigen's implementation of internal::make_coherent makes use of const_cast in
-// order to promote zero sized derivatives. This however interferes badly with
-// our caching system and produces unexpected behaviors. See #10971 for details.
-// Therefore our implementation stops using internal::make_coherent and treats
-// scalars with zero sized derivatives as constants, as it should.
-//
-// We also provide overloading of math functions for AutoDiffScalar<VectorXd>
-// which return AutoDiffScalar<VectorXd> instead of an expression tree.
-//
-// See https://github.com/RobotLocomotion/drake/issues/6944 for more
-// information. See also drake/common/autodiff_overloads.h.
-//
-// TODO(soonho-tri): Next time when we upgrade Eigen, please check if we still
-// need these specializations.
-template <>
-class AutoDiffScalar<VectorXd>
-    : public internal::auto_diff_special_op<VectorXd, false> {
- public:
-  typedef internal::auto_diff_special_op<VectorXd, false> Base;
-  typedef typename internal::remove_all<VectorXd>::type DerType;
-  typedef typename internal::traits<DerType>::Scalar Scalar;
-  typedef typename NumTraits<Scalar>::Real Real;
-
-  using Base::operator+;
-  using Base::operator*;
-
-  AutoDiffScalar() {}
-
-  AutoDiffScalar(const Scalar& value, int nbDer, int derNumber)
-      : m_value(value), m_derivatives(DerType::Zero(nbDer)) {
-    m_derivatives.coeffRef(derNumber) = Scalar(1);
-  }
-
-  // NOLINTNEXTLINE(runtime/explicit): Code from Eigen.
-  AutoDiffScalar(const Real& value) : m_value(value) {
-    if (m_derivatives.size() > 0) m_derivatives.setZero();
-  }
-
-  AutoDiffScalar(const Scalar& value, const DerType& der)
-      : m_value(value), m_derivatives(der) {}
-
-  template <typename OtherDerType>
-  AutoDiffScalar(
-      const AutoDiffScalar<OtherDerType>& other
-#ifndef EIGEN_PARSED_BY_DOXYGEN
-      ,
-      typename internal::enable_if<
-          internal::is_same<
-              Scalar, typename internal::traits<typename internal::remove_all<
-                          OtherDerType>::type>::Scalar>::value,
-          void*>::type = 0
-#endif
-      )
-      : m_value(other.value()), m_derivatives(other.derivatives()) {
-  }
-
-  friend std::ostream& operator<<(std::ostream& s, const AutoDiffScalar& a) {
-    return s << a.value();
-  }
-
-  AutoDiffScalar(const AutoDiffScalar& other)
-      : m_value(other.value()), m_derivatives(other.derivatives()) {}
-
-  template <typename OtherDerType>
-  inline AutoDiffScalar& operator=(const AutoDiffScalar<OtherDerType>& other) {
-    m_value = other.value();
-    m_derivatives = other.derivatives();
-    return *this;
-  }
-
-  inline AutoDiffScalar& operator=(const AutoDiffScalar& other) {
-    m_value = other.value();
-    m_derivatives = other.derivatives();
-    return *this;
-  }
-
-  inline AutoDiffScalar& operator=(const Scalar& other) {
-    m_value = other;
-    if (m_derivatives.size() > 0) m_derivatives.setZero();
-    return *this;
-  }
-
-  inline const Scalar& value() const { return m_value; }
-  inline Scalar& value() { return m_value; }
-
-  inline const DerType& derivatives() const { return m_derivatives; }
-  inline DerType& derivatives() { return m_derivatives; }
-
-  inline bool operator<(const Scalar& other) const { return m_value < other; }
-  inline bool operator<=(const Scalar& other) const { return m_value <= other; }
-  inline bool operator>(const Scalar& other) const { return m_value > other; }
-  inline bool operator>=(const Scalar& other) const { return m_value >= other; }
-  inline bool operator==(const Scalar& other) const { return m_value == other; }
-  inline bool operator!=(const Scalar& other) const { return m_value != other; }
-
-  friend inline bool operator<(const Scalar& a, const AutoDiffScalar& b) {
-    return a < b.value();
-  }
-  friend inline bool operator<=(const Scalar& a, const AutoDiffScalar& b) {
-    return a <= b.value();
-  }
-  friend inline bool operator>(const Scalar& a, const AutoDiffScalar& b) {
-    return a > b.value();
-  }
-  friend inline bool operator>=(const Scalar& a, const AutoDiffScalar& b) {
-    return a >= b.value();
-  }
-  friend inline bool operator==(const Scalar& a, const AutoDiffScalar& b) {
-    return a == b.value();
-  }
-  friend inline bool operator!=(const Scalar& a, const AutoDiffScalar& b) {
-    return a != b.value();
-  }
-
-  template <typename OtherDerType>
-  inline bool operator<(const AutoDiffScalar<OtherDerType>& b) const {
-    return m_value < b.value();
-  }
-  template <typename OtherDerType>
-  inline bool operator<=(const AutoDiffScalar<OtherDerType>& b) const {
-    return m_value <= b.value();
-  }
-  template <typename OtherDerType>
-  inline bool operator>(const AutoDiffScalar<OtherDerType>& b) const {
-    return m_value > b.value();
-  }
-  template <typename OtherDerType>
-  inline bool operator>=(const AutoDiffScalar<OtherDerType>& b) const {
-    return m_value >= b.value();
-  }
-  template <typename OtherDerType>
-  inline bool operator==(const AutoDiffScalar<OtherDerType>& b) const {
-    return m_value == b.value();
-  }
-  template <typename OtherDerType>
-  inline bool operator!=(const AutoDiffScalar<OtherDerType>& b) const {
-    return m_value != b.value();
-  }
-
-  inline const AutoDiffScalar<DerType> operator+(const Scalar& other) const {
-    return AutoDiffScalar<DerType>(m_value + other, m_derivatives);
-  }
-
-  friend inline const AutoDiffScalar<DerType> operator+(
-      const Scalar& a, const AutoDiffScalar& b) {
-    return AutoDiffScalar<DerType>(a + b.value(), b.derivatives());
-  }
-
-  inline AutoDiffScalar& operator+=(const Scalar& other) {
-    value() += other;
-    return *this;
-  }
-
-  template <typename OtherDerType>
-  inline const AutoDiffScalar<DerType> operator+(
-      const AutoDiffScalar<OtherDerType>& other) const {
-    const bool has_this_der = m_derivatives.size() > 0;
-    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
-    return MakeAutoDiffScalar(
-        m_value + other.value(),
-        has_both_der
-            ? VectorXd(m_derivatives + other.derivatives())
-            : has_this_der ? m_derivatives : VectorXd(other.derivatives()));
-  }
-
-  template <typename OtherDerType>
-  inline AutoDiffScalar& operator+=(const AutoDiffScalar<OtherDerType>& other) {
-    (*this) = (*this) + other;
-    return *this;
-  }
-
-  inline const AutoDiffScalar<DerType> operator-(const Scalar& b) const {
-    return AutoDiffScalar<DerType>(m_value - b, m_derivatives);
-  }
-
-  friend inline const AutoDiffScalar<DerType> operator-(
-      const Scalar& a, const AutoDiffScalar& b) {
-    return AutoDiffScalar<DerType>(a - b.value(), -b.derivatives());
-  }
-
-  inline AutoDiffScalar& operator-=(const Scalar& other) {
-    value() -= other;
-    return *this;
-  }
-
-  template <typename OtherDerType>
-  inline const AutoDiffScalar<DerType> operator-(
-      const AutoDiffScalar<OtherDerType>& other) const {
-    const bool has_this_der = m_derivatives.size() > 0;
-    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
-    return MakeAutoDiffScalar(
-        m_value - other.value(),
-        has_both_der
-            ? VectorXd(m_derivatives - other.derivatives())
-            : has_this_der ? m_derivatives : VectorXd(-other.derivatives()));
-  }
-
-  template <typename OtherDerType>
-  inline AutoDiffScalar& operator-=(const AutoDiffScalar<OtherDerType>& other) {
-    *this = *this - other;
-    return *this;
-  }
-
-  inline const AutoDiffScalar<DerType> operator-() const {
-    return AutoDiffScalar<DerType>(-m_value, -m_derivatives);
-  }
-
-  inline const AutoDiffScalar<DerType> operator*(const Scalar& other) const {
-    return MakeAutoDiffScalar(m_value * other, m_derivatives * other);
-  }
-
-  friend inline const AutoDiffScalar<DerType> operator*(
-      const Scalar& other, const AutoDiffScalar& a) {
-    return MakeAutoDiffScalar(a.value() * other, a.derivatives() * other);
-  }
-
-  inline const AutoDiffScalar<DerType> operator/(const Scalar& other) const {
-    return MakeAutoDiffScalar(m_value / other,
-                              (m_derivatives * (Scalar(1) / other)));
-  }
-
-  friend inline const AutoDiffScalar<DerType> operator/(
-      const Scalar& other, const AutoDiffScalar& a) {
-    return MakeAutoDiffScalar(
-        other / a.value(),
-        a.derivatives() * (Scalar(-other) / (a.value() * a.value())));
-  }
-
-  template <typename OtherDerType>
-  inline const AutoDiffScalar<DerType> operator/(
-      const AutoDiffScalar<OtherDerType>& other) const {
-    const auto& this_der = m_derivatives;
-    const auto& other_der = other.derivatives();
-    const bool has_this_der = m_derivatives.size() > 0;
-    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
-    const double scale = 1. / (other.value() * other.value());
-    return MakeAutoDiffScalar(
-        m_value / other.value(),
-        has_both_der ?
-            VectorXd(this_der * other.value() - other_der * m_value) * scale :
-        has_this_der ?
-            VectorXd(this_der * other.value()) * scale :
-        // has_other_der || has_neither
-            VectorXd(other_der * -m_value) * scale);
-  }
-
-  template <typename OtherDerType>
-  inline const AutoDiffScalar<DerType> operator*(
-      const AutoDiffScalar<OtherDerType>& other) const {
-    const bool has_this_der = m_derivatives.size() > 0;
-    const bool has_both_der = has_this_der && (other.derivatives().size() > 0);
-    return MakeAutoDiffScalar(
-        m_value * other.value(),
-        has_both_der ? VectorXd(m_derivatives * other.value() +
-                                other.derivatives() * m_value)
-                     : has_this_der ? VectorXd(m_derivatives * other.value())
-                                    : VectorXd(other.derivatives() * m_value));
-  }
-
-  inline AutoDiffScalar& operator*=(const Scalar& other) {
-    *this = *this * other;
-    return *this;
-  }
-
-  template <typename OtherDerType>
-  inline AutoDiffScalar& operator*=(const AutoDiffScalar<OtherDerType>& other) {
-    *this = *this * other;
-    return *this;
-  }
-
-  inline AutoDiffScalar& operator/=(const Scalar& other) {
-    *this = *this / other;
-    return *this;
-  }
-
-  template <typename OtherDerType>
-  inline AutoDiffScalar& operator/=(const AutoDiffScalar<OtherDerType>& other) {
-    *this = *this / other;
-    return *this;
-  }
-
- protected:
-  Scalar m_value;
-  DerType m_derivatives;
-};
-
-#define DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(FUNC, CODE) \
-  inline const AutoDiffScalar<VectorXd> FUNC(                   \
-      const AutoDiffScalar<VectorXd>& x) {                      \
-    EIGEN_UNUSED typedef double Scalar;                         \
-    CODE;                                                       \
-  }
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    abs, using std::abs; return Eigen::MakeAutoDiffScalar(
-        abs(x.value()), x.derivatives() * (x.value() < 0 ? -1 : 1));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    abs2, using numext::abs2; return Eigen::MakeAutoDiffScalar(
-        abs2(x.value()), x.derivatives() * (Scalar(2) * x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    sqrt, using std::sqrt; Scalar sqrtx = sqrt(x.value());
-    return Eigen::MakeAutoDiffScalar(sqrtx,
-                                     x.derivatives() * (Scalar(0.5) / sqrtx));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    cos, using std::cos; using std::sin;
-    return Eigen::MakeAutoDiffScalar(cos(x.value()),
-                                     x.derivatives() * (-sin(x.value())));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    sin, using std::sin; using std::cos;
-    return Eigen::MakeAutoDiffScalar(sin(x.value()),
-                                     x.derivatives() * cos(x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    exp, using std::exp; Scalar expx = exp(x.value());
-    return Eigen::MakeAutoDiffScalar(expx, x.derivatives() * expx);)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    log, using std::log; return Eigen::MakeAutoDiffScalar(
-        log(x.value()), x.derivatives() * (Scalar(1) / x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    tan, using std::tan; using std::cos; return Eigen::MakeAutoDiffScalar(
-        tan(x.value()),
-        x.derivatives() * (Scalar(1) / numext::abs2(cos(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    asin, using std::sqrt; using std::asin; return Eigen::MakeAutoDiffScalar(
-        asin(x.value()),
-        x.derivatives() * (Scalar(1) / sqrt(1 - numext::abs2(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    acos, using std::sqrt; using std::acos; return Eigen::MakeAutoDiffScalar(
-        acos(x.value()),
-        x.derivatives() * (Scalar(-1) / sqrt(1 - numext::abs2(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    atan, using std::atan; return Eigen::MakeAutoDiffScalar(
-        atan(x.value()),
-        x.derivatives() * (Scalar(1) / (1 + x.value() * x.value())));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    tanh, using std::cosh; using std::tanh; return Eigen::MakeAutoDiffScalar(
-        tanh(x.value()),
-        x.derivatives() * (Scalar(1) / numext::abs2(cosh(x.value()))));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    sinh, using std::sinh; using std::cosh;
-    return Eigen::MakeAutoDiffScalar(sinh(x.value()),
-                                     x.derivatives() * cosh(x.value()));)
-
-DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY(
-    cosh, using std::sinh; using std::cosh;
-    return Eigen::MakeAutoDiffScalar(cosh(x.value()),
-                                     x.derivatives() * sinh(x.value()));)
-
-#undef DRAKE_EIGEN_AUTODIFFXD_DECLARE_GLOBAL_UNARY
-
-// We have this specialization here because the Eigen-3.3.3's atan2
-// implementation for AutoDiffScalar does not make a return with properly sized
-// derivatives.
-inline const AutoDiffScalar<VectorXd> atan2(const AutoDiffScalar<VectorXd>& a,
-                                            const AutoDiffScalar<VectorXd>& b) {
-  const bool has_a_der = a.derivatives().size() > 0;
-  const bool has_both_der = has_a_der && (b.derivatives().size() > 0);
-  const double squared_hypot = a.value() * a.value() + b.value() * b.value();
-  return MakeAutoDiffScalar(
-      std::atan2(a.value(), b.value()),
-      VectorXd((has_both_der
-                    ? VectorXd(a.derivatives() * b.value() -
-                               a.value() * b.derivatives())
-                    : has_a_der ? VectorXd(a.derivatives() * b.value())
-                                : VectorXd(-a.value() * b.derivatives())) /
-               squared_hypot));
-}
-
-inline const AutoDiffScalar<VectorXd> pow(const AutoDiffScalar<VectorXd>& a,
-                                          double b) {
-  using std::pow;
-  return MakeAutoDiffScalar(pow(a.value(), b),
-                            a.derivatives() * (b * pow(a.value(), b - 1)));
-}
-
-#endif
-
-}  // namespace Eigen
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/cond.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/cond.h
deleted file mode 100644
index 16dd21e..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/cond.h
+++ /dev/null
@@ -1,44 +0,0 @@
-#pragma once
-
-#include <functional>
-#include <type_traits>
-
-#include "drake/common/double_overloads.h"
-
-namespace drake {
-/** @name cond
-  Constructs conditional expression (similar to Lisp's cond).
-
-  @verbatim
-    cond(cond_1, expr_1,
-         cond_2, expr_2,
-            ...,   ...,
-         cond_n, expr_n,
-         expr_{n+1})
-  @endverbatim
-
-  The value returned by the above cond expression is @c expr_1 if @c cond_1 is
-  true; else if @c cond_2 is true then @c expr_2; ... ; else if @c cond_n is
-  true then @c expr_n. If none of the conditions are true, it returns @c
-  expr_{n+1}.
-
-  @note This functions assumes that @p ScalarType provides @c operator< and the
-  type of @c f_cond is the type of the return type of <tt>operator<(ScalarType,
-  ScalarType)</tt>. For example, @c symbolic::Expression can be used as a @p
-  ScalarType because it provides <tt>symbolic::Formula
-  operator<(symbolic::Expression, symbolic::Expression)</tt>.
-
-
-  @{
- */
-template <typename ScalarType>
-ScalarType cond(const ScalarType& e) {
-  return e;
-}
-template <typename ScalarType, typename... Rest>
-ScalarType cond(const decltype(ScalarType() < ScalarType()) & f_cond,
-                const ScalarType& e_then, Rest... rest) {
-  return if_then_else(f_cond, e_then, cond(rest...));
-}
-///@}
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/constants.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/constants.h
deleted file mode 100644
index 0ccddca..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/constants.h
+++ /dev/null
@@ -1,19 +0,0 @@
-#pragma once
-
-namespace drake {
-
-constexpr int kQuaternionSize = 4;
-
-constexpr int kSpaceDimension = 3;
-
-constexpr int kRpySize = 3;
-
-/// https://en.wikipedia.org/wiki/Screw_theory#Twist
-constexpr int kTwistSize = 6;
-
-/// http://www.euclideanspace.com/maths/geometry/affine/matrix4x4/
-constexpr int kHomogeneousTransformSize = 16;
-
-const int kRotmatSize = kSpaceDimension * kSpaceDimension;
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/double_overloads.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/double_overloads.h
deleted file mode 100644
index 125f113..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/double_overloads.h
+++ /dev/null
@@ -1,21 +0,0 @@
-/// @file
-/// Provides necessary operations on double to have it as a ScalarType in drake.
-
-#pragma once
-
-namespace drake {
-/// Provides if-then-else expression for double. The value returned by the
-/// if-then-else expression is @p v_then if @p f_cond is @c true. Otherwise, it
-/// returns @p v_else.
-
-/// The semantics is similar but not exactly the same as C++'s conditional
-/// expression constructed by its ternary operator, @c ?:. In
-/// <tt>if_then_else(f_cond, v_then, v_else)</tt>, both of @p v_then and @p
-/// v_else are evaluated regardless of the evaluation of @p f_cond. In contrast,
-/// only one of @p v_then or @p v_else is evaluated in C++'s conditional
-/// expression <tt>f_cond ? v_then : v_else</tt>.
-inline double if_then_else(bool f_cond, double v_then, double v_else) {
-  return f_cond ? v_then : v_else;
-}
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/drake_deprecated.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/drake_deprecated.h
deleted file mode 100644
index 5ce6328..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/drake_deprecated.h
+++ /dev/null
@@ -1,65 +0,0 @@
-#pragma once
-
-/** @file
-Provides a portable macro for use in generating compile-time warnings for
-use of code that is permitted but discouraged. */
-
-#ifdef DRAKE_DOXYGEN_CXX
-/** Use `DRAKE_DEPRECATED("removal_date", "message")` to discourage use of
-certain APIs.  It can be used on classes, typedefs, variables, non-static data
-members, functions, arguments, enumerations, and template specializations. When
-code refers to the deprecated item, a compile time warning will be issued
-displaying the given message, preceded by "DRAKE DEPRECATED: ". The Doxygen API
-reference will show that the API is deprecated, along with the message.
-
-This is typically used for constructs that have been replaced by something
-better and it is good practice to suggest the appropriate replacement in the
-deprecation message. Deprecation warnings are conventionally used to convey to
-users that a feature they are depending on will be removed in a future release.
-
-Every deprecation notice must include a date (as YYYY-MM-DD string) where the
-deprecated item is planned for removal.  Future commits may change the date
-(e.g., delaying the removal) but should generally always reflect the best
-current expectation for removal.
-
-Absent any other particular need, we prefer to use a deprecation period of
-three months by default, often rounded up to the next first of the month.  So
-for code announced as deprecated on 2018-01-22 the removal_date would nominally
-be set to 2018-05-01.
-
-Try to keep the date string immediately after the DRAKE_DEPRECATED macro name,
-even if the message itself must be wrapped to a new line:
-@code
-  DRAKE_DEPRECATED("2038-01-19",
-      "foo is being replaced with a safer, const-method named bar().")
-  int foo();
-@endcode
-
-Sample uses: @code
-  // Attribute comes *before* declaration of a deprecated function or variable;
-  // no semicolon is allowed.
-  DRAKE_DEPRECATED("2038-01-19", "f() is slow; use g() instead.")
-  int f(int arg);
-
-  // Attribute comes *after* struct, class, enum keyword.
-  class DRAKE_DEPRECATED("2038-01-19", "Use MyNewClass instead.")
-  MyClass {
-  };
-
-  // Type alias goes before the '='.
-  using NewType
-      DRAKE_DEPRECATED("2038-01-19", "Use NewType instead.")
-      = OldType;
-@endcode
-*/
-#define DRAKE_DEPRECATED(removal_date, message)
-
-#else  // DRAKE_DOXYGEN_CXX
-
-#define DRAKE_DEPRECATED(removal_date, message)         \
-  [[deprecated(                                         \
-  "\nDRAKE DEPRECATED: " message                        \
-  "\nThe deprecated code will be removed from Drake"    \
-  " on or after " removal_date ".")]]
-
-#endif  // DRAKE_DOXYGEN_CXX
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/drake_nodiscard.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/drake_nodiscard.h
deleted file mode 100644
index 29f078d..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/drake_nodiscard.h
+++ /dev/null
@@ -1,13 +0,0 @@
-#pragma once
-
-// TODO(jwnimmer-tri) Once we are in --std=c++17 mode as our minimum version,
-// we can remove this file and just say [[nodiscard]] directly everywhere.
-
-#if defined(DRAKE_DOXYGEN_CXX) || __has_cpp_attribute(nodiscard)
-/** Synonym for [[nodiscard]], iff the current compiler supports it;
-see https://en.cppreference.com/w/cpp/language/attributes/nodiscard. */
-// NOLINTNEXTLINE(whitespace/braces)
-#define DRAKE_NODISCARD [[nodiscard]]
-#else
-#define DRAKE_NODISCARD
-#endif
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/dummy_value.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/dummy_value.h
deleted file mode 100644
index b9c616a..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/dummy_value.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#pragma once
-
-#include <limits>
-
-namespace drake {
-
-/// Provides a "dummy" value for a ScalarType -- a value that is unlikely to be
-/// mistaken for a purposefully-computed value, useful for initializing a value
-/// before the true result is available.
-///
-/// Defaults to using std::numeric_limits::quiet_NaN when available; it is a
-/// compile-time error to call the unspecialized dummy_value::get() when
-/// quiet_NaN is unavailable.
-///
-/// See autodiff_overloads.h to use this with Eigen's AutoDiffScalar.
-template <typename T>
-struct dummy_value {
-  static constexpr T get() {
-    static_assert(std::numeric_limits<T>::has_quiet_NaN,
-                  "Custom scalar types should specialize this struct");
-    return std::numeric_limits<T>::quiet_NaN();
-  }
-};
-
-template <>
-struct dummy_value<int> {
-  static constexpr int get() {
-    // D is for "Dummy".  We assume as least 32 bits (per cppguide) -- if `int`
-    // is larger than 32 bits, this will leave some fraction of the bytes zero
-    // instead of 0xDD, but that's okay.
-    return 0xDDDDDDDD;
-  }
-};
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h
deleted file mode 100644
index 49175ce..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_autodiff_limits.h
+++ /dev/null
@@ -1,20 +0,0 @@
-#pragma once
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <limits>
-
-// Eigen provides `numeric_limits<AutoDiffScalar<T>>` starting with v3.3.4.
-#if !EIGEN_VERSION_AT_LEAST(3, 3, 4)  // Eigen Version < v3.3.4
-
-namespace std {
-template <typename T>
-class numeric_limits<Eigen::AutoDiffScalar<T>>
-    : public numeric_limits<typename T::Scalar> {};
-
-}  // namespace std
-
-#endif  // Eigen Version < v3.3.4
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h
deleted file mode 100644
index 10ffec6..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_autodiff_types.h
+++ /dev/null
@@ -1,38 +0,0 @@
-#pragma once
-
-/// @file
-/// This file contains abbreviated definitions for certain uses of
-/// AutoDiffScalar that are commonly used in Drake.
-/// @see also eigen_types.h
-
-#ifndef DRAKE_COMMON_AUTODIFF_HEADER
-// TODO(soonho-tri): Change to #error.
-#warning Do not directly include this file. Include "drake/common/autodiff.h".
-#endif
-
-#include <type_traits>
-
-#include <Eigen/Core>
-
-#include "drake/common/eigen_types.h"
-
-namespace drake {
-
-/// An autodiff variable with a dynamic number of partials.
-using AutoDiffXd = Eigen::AutoDiffScalar<Eigen::VectorXd>;
-
-// TODO(hongkai-dai): Recursive template to get arbitrary gradient order.
-
-/// An autodiff variable with `num_vars` partials.
-template <int num_vars>
-using AutoDiffd = Eigen::AutoDiffScalar<Eigen::Matrix<double, num_vars, 1> >;
-
-/// A vector of `rows` autodiff variables, each with `num_vars` partials.
-template <int num_vars, int rows>
-using AutoDiffVecd = Eigen::Matrix<AutoDiffd<num_vars>, rows, 1>;
-
-/// A dynamic-sized vector of autodiff variables, each with a dynamic-sized
-/// vector of partials.
-typedef AutoDiffVecd<Eigen::Dynamic, Eigen::Dynamic> AutoDiffVecXd;
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_types.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_types.h
deleted file mode 100644
index abe3e0b..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/eigen_types.h
+++ /dev/null
@@ -1,461 +0,0 @@
-#pragma once
-
-/// @file
-/// This file contains abbreviated definitions for certain specializations of
-/// Eigen::Matrix that are commonly used in Drake.
-/// These convenient definitions are templated on the scalar type of the Eigen
-/// object. While Drake uses `<T>` for scalar types across the entire code base
-/// we decided in this file to use `<Scalar>` to be more consistent with the
-/// usage of `<Scalar>` in Eigen's code base.
-/// @see also eigen_autodiff_types.h
-
-#include <utility>
-
-#include <Eigen/Core>
-
-#include "drake/common/constants.h"
-#include "drake/common/drake_assert.h"
-#include "drake/common/drake_copyable.h"
-#include "drake/common/drake_deprecated.h"
-
-namespace drake {
-
-/// The empty column vector (zero rows, one column), templated on scalar type.
-template <typename Scalar>
-using Vector0 = Eigen::Matrix<Scalar, 0, 1>;
-
-/// A column vector of size 1 (that is, a scalar), templated on scalar type.
-template <typename Scalar>
-using Vector1 = Eigen::Matrix<Scalar, 1, 1>;
-
-/// A column vector of size 1 of doubles.
-using Vector1d = Eigen::Matrix<double, 1, 1>;
-
-/// A column vector of size 2, templated on scalar type.
-template <typename Scalar>
-using Vector2 = Eigen::Matrix<Scalar, 2, 1>;
-
-/// A column vector of size 3, templated on scalar type.
-template <typename Scalar>
-using Vector3 = Eigen::Matrix<Scalar, 3, 1>;
-
-/// A column vector of size 4, templated on scalar type.
-template <typename Scalar>
-using Vector4 = Eigen::Matrix<Scalar, 4, 1>;
-
-/// A column vector of size 6.
-template <typename Scalar>
-using Vector6 = Eigen::Matrix<Scalar, 6, 1>;
-
-/// A column vector templated on the number of rows.
-template <typename Scalar, int Rows>
-using Vector = Eigen::Matrix<Scalar, Rows, 1>;
-
-/// A column vector of any size, templated on scalar type.
-template <typename Scalar>
-using VectorX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;
-
-/// A vector of dynamic size templated on scalar type, up to a maximum of 6
-/// elements.
-template <typename Scalar>
-using VectorUpTo6 = Eigen::Matrix<Scalar, Eigen::Dynamic, 1, 0, 6, 1>;
-
-/// A row vector of size 2, templated on scalar type.
-template <typename Scalar>
-using RowVector2 = Eigen::Matrix<Scalar, 1, 2>;
-
-/// A row vector of size 3, templated on scalar type.
-template <typename Scalar>
-using RowVector3 = Eigen::Matrix<Scalar, 1, 3>;
-
-/// A row vector of size 4, templated on scalar type.
-template <typename Scalar>
-using RowVector4 = Eigen::Matrix<Scalar, 1, 4>;
-
-/// A row vector of size 6.
-template <typename Scalar>
-using RowVector6 = Eigen::Matrix<Scalar, 1, 6>;
-
-/// A row vector templated on the number of columns.
-template <typename Scalar, int Cols>
-using RowVector = Eigen::Matrix<Scalar, 1, Cols>;
-
-/// A row vector of any size, templated on scalar type.
-template <typename Scalar>
-using RowVectorX = Eigen::Matrix<Scalar, 1, Eigen::Dynamic>;
-
-
-/// A matrix of 2 rows and 2 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix2 = Eigen::Matrix<Scalar, 2, 2>;
-
-/// A matrix of 3 rows and 3 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix3 = Eigen::Matrix<Scalar, 3, 3>;
-
-/// A matrix of 4 rows and 4 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix4 = Eigen::Matrix<Scalar, 4, 4>;
-
-/// A matrix of 6 rows and 6 columns, templated on scalar type.
-template <typename Scalar>
-using Matrix6 = Eigen::Matrix<Scalar, 6, 6>;
-
-/// A matrix of 2 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix2X = Eigen::Matrix<Scalar, 2, Eigen::Dynamic>;
-
-/// A matrix of 3 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
-
-/// A matrix of 4 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix4X = Eigen::Matrix<Scalar, 4, Eigen::Dynamic>;
-
-/// A matrix of 6 rows, dynamic columns, templated on scalar type.
-template <typename Scalar>
-using Matrix6X = Eigen::Matrix<Scalar, 6, Eigen::Dynamic>;
-
-/// A matrix of dynamic size, templated on scalar type.
-template <typename Scalar>
-using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
-
-/// A matrix of dynamic size templated on scalar type, up to a maximum of 6 rows
-/// and 6 columns. Rectangular matrices, with different number of rows and
-/// columns, are allowed.
-template <typename Scalar>
-using MatrixUpTo6 =
-Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, 0, 6, 6>;
-
-/// A quaternion templated on scalar type.
-template <typename Scalar>
-using Quaternion = Eigen::Quaternion<Scalar>;
-
-/// An AngleAxis templated on scalar type.
-template <typename Scalar>
-using AngleAxis = Eigen::AngleAxis<Scalar>;
-
-/// An Isometry templated on scalar type.
-template <typename Scalar>
-using Isometry3 = Eigen::Transform<Scalar, 3, Eigen::Isometry>;
-
-/// A translation in 3D templated on scalar type.
-template <typename Scalar>
-using Translation3 = Eigen::Translation<Scalar, 3>;
-
-/// A column vector consisting of one twist.
-template <typename Scalar>
-using TwistVector = Eigen::Matrix<Scalar, kTwistSize, 1>;
-
-/// A matrix with one twist per column, and dynamically many columns.
-template <typename Scalar>
-using TwistMatrix = Eigen::Matrix<Scalar, kTwistSize, Eigen::Dynamic>;
-
-/// A six-by-six matrix.
-template <typename Scalar>
-using SquareTwistMatrix = Eigen::Matrix<Scalar, kTwistSize, kTwistSize>;
-
-/// A column vector consisting of one wrench (spatial force) = `[r X f; f]`,
-/// where f is a force (translational force) applied at a point `P` and `r` is
-/// the position vector from a point `O` (called the "moment center") to point
-/// `P`.
-template <typename Scalar>
-using WrenchVector = Eigen::Matrix<Scalar, 6, 1>;
-
-/// A column vector consisting of a concatenated rotational and translational
-/// force.  The wrench is a special case of a SpatialForce.  For a general
-/// SpatialForce the rotational force can be a pure torque or the accumulation
-/// of moments and need not necessarily be a function of the force term.
-template <typename Scalar>
-using SpatialForce
-DRAKE_DEPRECATED("2019-10-15", "Please use Vector6<> instead.")
-    = Eigen::Matrix<Scalar, 6, 1>;
-
-/// EigenSizeMinPreferDynamic<a, b>::value gives the min between compile-time
-/// sizes @p a and @p b. 0 has absolute priority, followed by 1, followed by
-/// Dynamic, followed by other finite values.
-///
-/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_DYNAMIC
-/// macro in "Eigen/Core/util/Macros.h".
-template <int a, int b>
-struct EigenSizeMinPreferDynamic {
-  // clang-format off
-  static constexpr int value = (a == 0 || b == 0) ? 0 :
-                               (a == 1 || b == 1) ? 1 :
-     (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic :
-                                           a <= b ? a : b;
-  // clang-format on
-};
-
-/// EigenSizeMinPreferFixed is a variant of EigenSizeMinPreferDynamic. The
-/// difference is that finite values now have priority over Dynamic, so that
-/// EigenSizeMinPreferFixed<3, Dynamic>::value gives 3.
-///
-/// Note that this is a type-trait version of EIGEN_SIZE_MIN_PREFER_FIXED macro
-/// in "Eigen/Core/util/Macros.h".
-template <int a, int b>
-struct EigenSizeMinPreferFixed {
-  // clang-format off
-  static constexpr int value = (a == 0 || b == 0) ? 0 :
-                               (a == 1 || b == 1) ? 1 :
-     (a == Eigen::Dynamic && b == Eigen::Dynamic) ? Eigen::Dynamic :
-                            (a == Eigen::Dynamic) ? b :
-                            (b == Eigen::Dynamic) ? a :
-                                           a <= b ? a : b;
-  // clang-format on
-};
-
-/// MultiplyEigenSizes<a, b> gives a * b if both of a and b are fixed
-/// sizes. Otherwise it gives Eigen::Dynamic.
-template <int a, int b>
-struct MultiplyEigenSizes {
-  static constexpr int value =
-      (a == Eigen::Dynamic || b == Eigen::Dynamic) ? Eigen::Dynamic : a * b;
-};
-
-/*
- * Determines if a type is derived from EigenBase<> (e.g. ArrayBase<>,
- * MatrixBase<>).
- */
-template <typename Derived>
-struct is_eigen_type : std::is_base_of<Eigen::EigenBase<Derived>, Derived> {};
-
-/*
- * Determines if an EigenBase<> has a specific scalar type.
- */
-template <typename Derived, typename Scalar>
-struct is_eigen_scalar_same
-    : std::integral_constant<
-          bool, is_eigen_type<Derived>::value &&
-                    std::is_same<typename Derived::Scalar, Scalar>::value> {};
-
-/*
- * Determines if an EigenBase<> type is a compile-time (column) vector.
- * This will not check for run-time size.
- */
-template <typename Derived>
-struct is_eigen_vector
-    : std::integral_constant<bool, is_eigen_type<Derived>::value &&
-                                       Derived::ColsAtCompileTime == 1> {};
-
-/*
- * Determines if an EigenBase<> type is a compile-time (column) vector of a
- * scalar type. This will not check for run-time size.
- */
-template <typename Derived, typename Scalar>
-struct is_eigen_vector_of
-    : std::integral_constant<
-          bool, is_eigen_scalar_same<Derived, Scalar>::value &&
-                    is_eigen_vector<Derived>::value> {};
-
-// TODO(eric.cousineau): A 1x1 matrix will be disqualified in this case, and
-// this logic will qualify it as a vector. Address the downstream logic if this
-// becomes an issue.
-/*
- * Determines if a EigenBase<> type is a compile-time non-column-vector matrix
- * of a scalar type. This will not check for run-time size.
- * @note For an EigenBase<> of the correct Scalar type, this logic is
- * exclusive to is_eigen_vector_of<> such that distinct specializations are not
- * ambiguous.
- */
-template <typename Derived, typename Scalar>
-struct is_eigen_nonvector_of
-    : std::integral_constant<
-          bool, is_eigen_scalar_same<Derived, Scalar>::value &&
-                    !is_eigen_vector<Derived>::value> {};
-
-// TODO(eric.cousineau): Add alias is_eigen_matrix_of = is_eigen_scalar_same if
-// appropriate.
-
-/// This wrapper class provides a way to write non-template functions taking raw
-/// pointers to Eigen objects as parameters while limiting the number of copies,
-/// similar to `Eigen::Ref`. Internally, it keeps an instance of `Eigen::Ref<T>`
-/// and provides access to it via `operator*` and `operator->`.
-///
-/// The motivation of this class is to follow <a
-/// href="https://google.github.io/styleguide/cppguide.html#Reference_Arguments">GSG's
-/// "output arguments should be pointers" rule</a> while taking advantage of
-/// using `Eigen::Ref`. Here is an example.
-///
-/// @code
-/// // This function is taking an Eigen::Ref of a matrix and modifies it in
-/// // the body. This violates GSG's rule on output parameters.
-/// void foo(Eigen::Ref<Eigen::MatrixXd> M) {
-///    M(0, 0) = 0;
-/// }
-/// // At Call-site, we have:
-/// foo(M);
-/// foo(M.block(0, 0, 2, 2));
-///
-/// // We can rewrite the above function into the following using EigenPtr.
-/// void foo(EigenPtr<Eigen::MatrixXd> M) {
-///    (*M)(0, 0) = 0;
-/// }
-/// // Note that, call sites should be changed to:
-/// foo(&M);
-///
-/// // We need tmp to avoid taking the address of a temporary object such as the
-/// // return value of .block().
-/// auto tmp = M.block(0, 0, 2, 2);
-/// foo(&tmp);
-/// @endcode
-///
-/// Notice that methods taking an EigenPtr can mutate the entries of a matrix as
-/// in method `foo()` in the example code above, but cannot change its size.
-/// This is because `operator*` and `operator->` return an `Eigen::Ref<T>`
-/// object and only plain matrices/arrays can be resized and not expressions.
-/// This **is** the desired behavior, since resizing the block of a matrix or
-/// even a more general expression should not be allowed. If you do want to be
-/// able to resize a mutable matrix argument, then you must pass it as a
-/// `Matrix<T>*`, like so:
-/// @code
-/// void bar(Eigen::MatrixXd* M) {
-///   DRAKE_THROW_UNLESS(M != nullptr);
-///   // In this case this method only works with 4x3 matrices.
-///   if (M->rows() != 4 && M->cols() != 3) {
-///     M->resize(4, 3);
-///   }
-///   (*M)(0, 0) = 0;
-/// }
-/// @endcode
-///
-/// @note This class provides a way to avoid the `const_cast` hack introduced in
-/// <a
-/// href="https://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html#TopicPlainFunctionsFailing">Eigen's
-/// documentation</a>.
-template <typename PlainObjectType>
-class EigenPtr {
- public:
-  typedef Eigen::Ref<PlainObjectType> RefType;
-
-  EigenPtr() : EigenPtr(nullptr) {}
-
-  /// Overload for `nullptr`.
-  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
-  EigenPtr(std::nullptr_t) {}
-
-  /// Constructs with a reference to the given matrix type.
-  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
-  EigenPtr(const EigenPtr& other) { assign(other); }
-
-  /// Constructs with a reference to another matrix type.
-  /// May be `nullptr`.
-  template <typename PlainObjectTypeIn>
-  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
-  EigenPtr(PlainObjectTypeIn* m) {
-    if (m) {
-      m_.set_value(m);
-    }
-  }
-
-  /// Constructs from another EigenPtr.
-  template <typename PlainObjectTypeIn>
-  // NOLINTNEXTLINE(runtime/explicit) This conversion is desirable.
-  EigenPtr(const EigenPtr<PlainObjectTypeIn>& other) {
-    // Cannot directly construct `m_` from `other.m_`.
-    assign(other);
-  }
-
-  EigenPtr& operator=(const EigenPtr& other) {
-    // We must explicitly override this version of operator=.
-    // The template below will not take precedence over this one.
-    return assign(other);
-  }
-
-  template <typename PlainObjectTypeIn>
-  EigenPtr& operator=(const EigenPtr<PlainObjectTypeIn>& other) {
-    return assign(other);
-  }
-
-  /// @throws std::runtime_error if this is a null dereference.
-  RefType& operator*() const { return get_reference(); }
-
-  /// @throws std::runtime_error if this is a null dereference.
-  RefType* operator->() const { return &get_reference(); }
-
-  /// Returns whether or not this contains a valid reference.
-  operator bool() const { return is_valid(); }
-
-  bool operator==(std::nullptr_t) const { return !is_valid(); }
-
-  bool operator!=(std::nullptr_t) const { return is_valid(); }
-
- private:
-  // Simple reassignable container without requirement of heap allocation.
-  // This is used because `drake::optional<>` does not work with `Eigen::Ref<>`
-  // because `Ref` deletes the necessary `operator=` overload for
-  // `std::is_copy_assignable`.
-  class ReassignableRef {
-   public:
-    DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(ReassignableRef)
-    ReassignableRef() {}
-    ~ReassignableRef() {
-      reset();
-    }
-
-    // Reset value to null.
-    void reset() {
-      if (has_value_) {
-        raw_value().~RefType();
-        has_value_ = false;
-      }
-    }
-
-    // Set value.
-    template <typename PlainObjectTypeIn>
-    void set_value(PlainObjectTypeIn* value_in) {
-      if (has_value_) {
-        raw_value().~RefType();
-      }
-      new (&raw_value()) RefType(*value_in);
-      has_value_ = true;
-    }
-
-    // Access to value.
-    RefType& value() {
-      DRAKE_ASSERT(has_value());
-      return raw_value();
-    }
-
-    // Indicates if it has a value.
-    bool has_value() const { return has_value_; }
-
-   private:
-    // Unsafe access to value.
-    RefType& raw_value() { return reinterpret_cast<RefType&>(storage_); }
-
-    bool has_value_{};
-    typename std::aligned_storage<sizeof(RefType), alignof(RefType)>::type
-        storage_;
-  };
-
-  // Use mutable, reassignable ref to permit pointer-like semantics (with
-  // ownership) on the stack.
-  mutable ReassignableRef m_;
-
-  // Consolidate assignment here, so that both the copy constructor and the
-  // construction from another type may be used.
-  template <typename PlainObjectTypeIn>
-  EigenPtr& assign(const EigenPtr<PlainObjectTypeIn>& other) {
-    if (other) {
-      m_.set_value(&(*other));
-    } else {
-      m_.reset();
-    }
-    return *this;
-  }
-
-  // Consolidate getting a reference here.
-  RefType& get_reference() const {
-    if (!m_.has_value())
-      throw std::runtime_error("EigenPtr: nullptr dereference");
-    return m_.value();
-  }
-
-  bool is_valid() const {
-    return m_.has_value();
-  }
-};
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
index 8847d87..d6bcbb8 100644
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
+++ b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/test_utilities/eigen_matrix_compare.h
@@ -7,7 +7,7 @@
 #include <Eigen/Core>
 #include <gtest/gtest.h>
 
-#include "drake/common/drake_nodiscard.h"
+// #include "drake/common/text_logging.h"
 
 namespace drake {
 
@@ -28,7 +28,7 @@
  * @return true if the two matrices are equal based on the specified tolerance.
  */
 template <typename DerivedA, typename DerivedB>
-DRAKE_NODISCARD ::testing::AssertionResult CompareMatrices(
+[[nodiscard]] ::testing::AssertionResult CompareMatrices(
     const Eigen::MatrixBase<DerivedA>& m1,
     const Eigen::MatrixBase<DerivedB>& m2, double tolerance = 0.0,
     MatrixCompareType compare_type = MatrixCompareType::absolute) {
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/unused.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/common/unused.h
deleted file mode 100644
index 5a28b01..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/common/unused.h
+++ /dev/null
@@ -1,53 +0,0 @@
-#pragma once
-
-namespace drake {
-
-/// Documents the argument(s) as unused, placating GCC's -Wunused-parameter
-/// warning.  This can be called within function bodies to mark that certain
-/// parameters are unused.
-///
-/// When possible, removing the unused parameter is better than placating the
-/// warning.  However, in some cases the parameter is part of a virtual API or
-/// template concept that is used elsewhere, so we can't remove it.  In those
-/// cases, this function might be an appropriate work-around.
-///
-/// Here's rough advice on how to fix Wunused-parameter warnings:
-///
-/// (1) If the parameter can be removed entirely, prefer that as the first
-///     choice.  (This may not be possible if, e.g., a method must match some
-///     virtual API or template concept.)
-///
-/// (2) Unless the parameter name has acute value, prefer to omit the name of
-///     the parameter, leaving only the type, e.g.
-/// @code
-/// void Print(const State& state) override { /* No state to print. */ }
-/// @endcode
-///     changes to
-/// @code
-/// void Print(const State&) override { /* No state to print. */}
-/// @endcode
-///     This no longer triggers the warning and further makes it clear that a
-///     parameter required by the API is definitively unused in the function.
-///
-///     This is an especially good solution in the context of method
-///     definitions (vs declarations); the parameter name used in a definition
-///     is entirely irrelevant to Doxygen and most readers.
-///
-/// (3) When leaving the parameter name intact has acute value, it is
-///     acceptable to keep the name and mark it `unused`.  For example, when
-///     the name appears as part of a virtual method's base class declaration,
-///     the name is used by Doxygen to document the method, e.g.,
-/// @code
-/// /** Sets the default State of a System.  This default implementation is to
-///     set all zeros.  Subclasses may override to use non-zero defaults.  The
-///     custom defaults may be based on the given @p context, when relevant.  */
-/// virtual void SetDefault(const Context<T>& context, State<T>* state) const {
-///   unused(context);
-///   state->SetZero();
-/// }
-/// @endcode
-///
-template <typename ... Args>
-void unused(const Args& ...) {}
-
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/drake/math/autodiff.h b/third_party/allwpilib/wpimath/src/test/native/include/drake/math/autodiff.h
deleted file mode 100644
index 52edb11..0000000
--- a/third_party/allwpilib/wpimath/src/test/native/include/drake/math/autodiff.h
+++ /dev/null
@@ -1,332 +0,0 @@
-/// @file
-/// Utilities for arithmetic on AutoDiffScalar.
-
-// TODO(russt): rename methods to be GSG compliant.
-
-#pragma once
-
-#include <cmath>
-#include <tuple>
-
-#include <Eigen/Core>
-
-#include "drake/common/autodiff.h"
-#include "drake/common/unused.h"
-
-namespace drake {
-namespace math {
-
-template <typename Derived>
-struct AutoDiffToValueMatrix {
-  typedef typename Eigen::Matrix<typename Derived::Scalar::Scalar,
-                                 Derived::RowsAtCompileTime,
-                                 Derived::ColsAtCompileTime> type;
-};
-
-template <typename Derived>
-typename AutoDiffToValueMatrix<Derived>::type autoDiffToValueMatrix(
-    const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
-  typename AutoDiffToValueMatrix<Derived>::type ret(auto_diff_matrix.rows(),
-                                                    auto_diff_matrix.cols());
-  for (int i = 0; i < auto_diff_matrix.rows(); i++) {
-    for (int j = 0; j < auto_diff_matrix.cols(); ++j) {
-      ret(i, j) = auto_diff_matrix(i, j).value();
-    }
-  }
-  return ret;
-}
-
-/** `B = DiscardGradient(A)` enables casting from a matrix of AutoDiffScalars
- * to AutoDiffScalar::Scalar type, explicitly throwing away any gradient
- * information. For a matrix of type, e.g. `MatrixX<AutoDiffXd> A`, the
- * comparable operation
- *   `B = A.cast<double>()`
- * should (and does) fail to compile.  Use `DiscardGradient(A)` if you want to
- * force the cast (and explicitly declare that information is lost).
- *
- * This method is overloaded to permit the user to call it for double types and
- * AutoDiffScalar types (to avoid the calling function having to handle the
- * two cases differently).
- *
- * @see DiscardZeroGradient
- */
-template <typename Derived>
-typename std::enable_if<
-    !std::is_same<typename Derived::Scalar, double>::value,
-    Eigen::Matrix<typename Derived::Scalar::Scalar, Derived::RowsAtCompileTime,
-                  Derived::ColsAtCompileTime, 0, Derived::MaxRowsAtCompileTime,
-                  Derived::MaxColsAtCompileTime>>::type
-DiscardGradient(const Eigen::MatrixBase<Derived>& auto_diff_matrix) {
-  return autoDiffToValueMatrix(auto_diff_matrix);
-}
-
-/// @see DiscardGradient().
-template <typename Derived>
-typename std::enable_if<
-    std::is_same<typename Derived::Scalar, double>::value,
-    const Eigen::MatrixBase<Derived>&>::type
-DiscardGradient(const Eigen::MatrixBase<Derived>& matrix) {
-  return matrix;
-}
-
-/// @see DiscardGradient().
-template <typename _Scalar, int _Dim, int _Mode, int _Options>
-typename std::enable_if<
-    !std::is_same<_Scalar, double>::value,
-    Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>>::type
-DiscardGradient(const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>&
-                    auto_diff_transform) {
-  return Eigen::Transform<typename _Scalar::Scalar, _Dim, _Mode, _Options>(
-      autoDiffToValueMatrix(auto_diff_transform.matrix()));
-}
-
-/// @see DiscardGradient().
-template <typename _Scalar, int _Dim, int _Mode, int _Options>
-typename std::enable_if<std::is_same<_Scalar, double>::value,
-                        const Eigen::Transform<_Scalar, _Dim, _Mode,
-    _Options>&>::type
-DiscardGradient(
-    const Eigen::Transform<_Scalar, _Dim, _Mode, _Options>& transform) {
-  return transform;
-}
-
-
-/** \brief Initialize a single autodiff matrix given the corresponding value
- *matrix.
- *
- * Set the values of \p auto_diff_matrix to be equal to \p val, and for each
- *element i of \p auto_diff_matrix,
- * resize the derivatives vector to \p num_derivatives, and set derivative
- *number \p deriv_num_start + i to one (all other elements of the derivative
- *vector set to zero).
- *
- * \param[in] mat 'regular' matrix of values
- * \param[out] ret AutoDiff matrix
- * \param[in] num_derivatives the size of the derivatives vector @default the
- *size of mat
- * \param[in] deriv_num_start starting index into derivative vector (i.e.
- *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
- *@default 0
- */
-template <typename Derived, typename DerivedAutoDiff>
-void initializeAutoDiff(const Eigen::MatrixBase<Derived>& val,
-                        // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
-                        Eigen::MatrixBase<DerivedAutoDiff>& auto_diff_matrix,
-                        Eigen::DenseIndex num_derivatives = Eigen::Dynamic,
-                        Eigen::DenseIndex deriv_num_start = 0) {
-  using ADScalar = typename DerivedAutoDiff::Scalar;
-  static_assert(static_cast<int>(Derived::RowsAtCompileTime) ==
-                    static_cast<int>(DerivedAutoDiff::RowsAtCompileTime),
-                "auto diff matrix has wrong number of rows at compile time");
-  static_assert(static_cast<int>(Derived::ColsAtCompileTime) ==
-                    static_cast<int>(DerivedAutoDiff::ColsAtCompileTime),
-                "auto diff matrix has wrong number of columns at compile time");
-
-  if (num_derivatives == Eigen::Dynamic) num_derivatives = val.size();
-
-  auto_diff_matrix.resize(val.rows(), val.cols());
-  Eigen::DenseIndex deriv_num = deriv_num_start;
-  for (Eigen::DenseIndex i = 0; i < val.size(); i++) {
-    auto_diff_matrix(i) = ADScalar(val(i), num_derivatives, deriv_num++);
-  }
-}
-
-/** \brief The appropriate AutoDiffScalar gradient type given the value type and
- * the number of derivatives at compile time
- */
-template <typename Derived, int Nq>
-using AutoDiffMatrixType = Eigen::Matrix<
-    Eigen::AutoDiffScalar<Eigen::Matrix<typename Derived::Scalar, Nq, 1>>,
-    Derived::RowsAtCompileTime, Derived::ColsAtCompileTime, 0,
-    Derived::MaxRowsAtCompileTime, Derived::MaxColsAtCompileTime>;
-
-/** \brief Initialize a single autodiff matrix given the corresponding value
- *matrix.
- *
- * Create autodiff matrix that matches \p mat in size with derivatives of
- *compile time size \p Nq and runtime size \p num_derivatives.
- * Set its values to be equal to \p val, and for each element i of \p
- *auto_diff_matrix, set derivative number \p deriv_num_start + i to one (all
- *other derivatives set to zero).
- *
- * \param[in] mat 'regular' matrix of values
- * \param[in] num_derivatives the size of the derivatives vector @default the
- *size of mat
- * \param[in] deriv_num_start starting index into derivative vector (i.e.
- *element deriv_num_start in derivative vector corresponds to mat(0, 0)).
- *@default 0
- * \return AutoDiff matrix
- */
-template <int Nq = Eigen::Dynamic, typename Derived>
-AutoDiffMatrixType<Derived, Nq> initializeAutoDiff(
-    const Eigen::MatrixBase<Derived>& mat,
-    Eigen::DenseIndex num_derivatives = -1,
-    Eigen::DenseIndex deriv_num_start = 0) {
-  if (num_derivatives == -1) num_derivatives = mat.size();
-
-  AutoDiffMatrixType<Derived, Nq> ret(mat.rows(), mat.cols());
-  initializeAutoDiff(mat, ret, num_derivatives, deriv_num_start);
-  return ret;
-}
-
-namespace internal {
-template <typename Derived, typename Scalar>
-struct ResizeDerivativesToMatchScalarImpl {
-  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
-  static void run(Eigen::MatrixBase<Derived>&, const Scalar&) {}
-};
-
-template <typename Derived, typename DerivType>
-struct ResizeDerivativesToMatchScalarImpl<Derived,
-                                          Eigen::AutoDiffScalar<DerivType>> {
-  using Scalar = Eigen::AutoDiffScalar<DerivType>;
-  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
-  static void run(Eigen::MatrixBase<Derived>& mat, const Scalar& scalar) {
-    for (int i = 0; i < mat.size(); i++) {
-      auto& derivs = mat(i).derivatives();
-      if (derivs.size() == 0) {
-        derivs.resize(scalar.derivatives().size());
-        derivs.setZero();
-      }
-    }
-  }
-};
-}  // namespace internal
-
-/** Resize derivatives vector of each element of a matrix to to match the size
- * of the derivatives vector of a given scalar.
- * \brief If the mat and scalar inputs are AutoDiffScalars, resize the
- * derivatives vector of each element of the matrix mat to match
- * the number of derivatives of the scalar. This is useful in functions that
- * return matrices that do not depend on an AutoDiffScalar
- * argument (e.g. a function with a constant output), while it is desired that
- * information about the number of derivatives is preserved.
- * \param mat matrix, for which the derivative vectors of the elements will be
- * resized
- * \param scalar scalar to match the derivative size vector against.
- */
-template <typename Derived>
-// TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
-void resizeDerivativesToMatchScalar(Eigen::MatrixBase<Derived>& mat,
-                                    const typename Derived::Scalar& scalar) {
-  internal::ResizeDerivativesToMatchScalarImpl<
-      Derived, typename Derived::Scalar>::run(mat, scalar);
-}
-
-namespace internal {
-/** \brief Helper for totalSizeAtCompileTime function (recursive)
- */
-template <typename Head, typename... Tail>
-struct TotalSizeAtCompileTime {
-  static constexpr int eval() {
-    return Head::SizeAtCompileTime == Eigen::Dynamic ||
-                   TotalSizeAtCompileTime<Tail...>::eval() == Eigen::Dynamic
-               ? Eigen::Dynamic
-               : Head::SizeAtCompileTime +
-                     TotalSizeAtCompileTime<Tail...>::eval();
-  }
-};
-
-/** \brief Helper for totalSizeAtCompileTime function (base case)
- */
-template <typename Head>
-struct TotalSizeAtCompileTime<Head> {
-  static constexpr int eval() { return Head::SizeAtCompileTime; }
-};
-
-/** \brief Determine the total size at compile time of a number of arguments
- * based on their SizeAtCompileTime static members
- */
-template <typename... Args>
-constexpr int totalSizeAtCompileTime() {
-  return TotalSizeAtCompileTime<Args...>::eval();
-}
-
-/** \brief Determine the total size at runtime of a number of arguments using
- * their size() methods (base case).
- */
-constexpr Eigen::DenseIndex totalSizeAtRunTime() { return 0; }
-
-/** \brief Determine the total size at runtime of a number of arguments using
- * their size() methods (recursive)
- */
-template <typename Head, typename... Tail>
-Eigen::DenseIndex totalSizeAtRunTime(const Eigen::MatrixBase<Head>& head,
-                                     const Tail&... tail) {
-  return head.size() + totalSizeAtRunTime(tail...);
-}
-
-/** \brief Helper for initializeAutoDiffTuple function (recursive)
- */
-template <size_t Index>
-struct InitializeAutoDiffTupleHelper {
-  template <typename... ValueTypes, typename... AutoDiffTypes>
-  static void run(const std::tuple<ValueTypes...>& values,
-                  // TODO(#2274) Fix NOLINTNEXTLINE(runtime/references).
-                  std::tuple<AutoDiffTypes...>& auto_diffs,
-                  Eigen::DenseIndex num_derivatives,
-                  Eigen::DenseIndex deriv_num_start) {
-    constexpr size_t tuple_index = sizeof...(AutoDiffTypes)-Index;
-    const auto& value = std::get<tuple_index>(values);
-    auto& auto_diff = std::get<tuple_index>(auto_diffs);
-    auto_diff.resize(value.rows(), value.cols());
-    initializeAutoDiff(value, auto_diff, num_derivatives, deriv_num_start);
-    InitializeAutoDiffTupleHelper<Index - 1>::run(
-        values, auto_diffs, num_derivatives, deriv_num_start + value.size());
-  }
-};
-
-/** \brief Helper for initializeAutoDiffTuple function (base case)
- */
-template <>
-struct InitializeAutoDiffTupleHelper<0> {
-  template <typename... ValueTypes, typename... AutoDiffTypes>
-  static void run(const std::tuple<ValueTypes...>& values,
-                  const std::tuple<AutoDiffTypes...>& auto_diffs,
-                  Eigen::DenseIndex num_derivatives,
-                  Eigen::DenseIndex deriv_num_start) {
-    unused(values, auto_diffs, num_derivatives, deriv_num_start);
-  }
-};
-}  // namespace internal
-
-/** \brief Given a series of Eigen matrices, create a tuple of corresponding
- *AutoDiff matrices with values equal to the input matrices and properly
- *initialized derivative vectors.
- *
- * The size of the derivative vector of each element of the matrices in the
- *output tuple will be the same, and will equal the sum of the number of
- *elements of the matrices in \p args.
- * If all of the matrices in \p args have fixed size, then the derivative
- *vectors will also have fixed size (being the sum of the sizes at compile time
- *of all of the input arguments),
- * otherwise the derivative vectors will have dynamic size.
- * The 0th element of the derivative vectors will correspond to the derivative
- *with respect to the 0th element of the first argument.
- * Subsequent derivative vector elements correspond first to subsequent elements
- *of the first input argument (traversed first by row, then by column), and so
- *on for subsequent arguments.
- *
- * \param args a series of Eigen matrices
- * \return a tuple of properly initialized AutoDiff matrices corresponding to \p
- *args
- *
- */
-template <typename... Deriveds>
-std::tuple<AutoDiffMatrixType<
-    Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
-initializeAutoDiffTuple(const Eigen::MatrixBase<Deriveds>&... args) {
-  Eigen::DenseIndex dynamic_num_derivs = internal::totalSizeAtRunTime(args...);
-  std::tuple<AutoDiffMatrixType<
-      Deriveds, internal::totalSizeAtCompileTime<Deriveds...>()>...>
-      ret(AutoDiffMatrixType<Deriveds,
-                             internal::totalSizeAtCompileTime<Deriveds...>()>(
-          args.rows(), args.cols())...);
-  auto values = std::forward_as_tuple(args...);
-  internal::InitializeAutoDiffTupleHelper<sizeof...(args)>::run(
-      values, ret, dynamic_num_derivs, 0);
-  return ret;
-}
-
-}  // namespace math
-}  // namespace drake
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h b/third_party/allwpilib/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
new file mode 100644
index 0000000..23a20e8
--- /dev/null
+++ b/third_party/allwpilib/wpimath/src/test/native/include/frc/system/RungeKuttaTimeVarying.h
@@ -0,0 +1,34 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+
+#include "Eigen/Core"
+#include "units/time.h"
+
+namespace frc {
+
+/**
+ * Performs 4th order Runge-Kutta integration of dy/dt = f(t, y) for dt.
+ *
+ * @param f  The function to integrate. It must take two arguments t and y.
+ * @param t  The initial value of t.
+ * @param y  The initial value of y.
+ * @param dt The time over which to integrate.
+ */
+template <typename F, typename T>
+T RungeKuttaTimeVarying(F&& f, units::second_t t, T y, units::second_t dt) {
+  const auto h = dt.value();
+
+  T k1 = f(t, y);
+  T k2 = f(t + dt * 0.5, y + h * k1 * 0.5);
+  T k3 = f(t + dt * 0.5, y + h * k2 * 0.5);
+  T k4 = f(t + dt, y + h * k3);
+
+  return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+}
+
+}  // namespace frc
diff --git a/third_party/allwpilib/wpimath/src/test/native/include/trajectory/TestTrajectory.h b/third_party/allwpilib/wpimath/src/test/native/include/trajectory/TestTrajectory.h
index 1cac87f..de7b8b8 100644
--- a/third_party/allwpilib/wpimath/src/test/native/include/trajectory/TestTrajectory.h
+++ b/third_party/allwpilib/wpimath/src/test/native/include/trajectory/TestTrajectory.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpimath/wpimath-config.cmake.in b/third_party/allwpilib/wpimath/wpimath-config.cmake.in
index 2f661d9..4769e43 100644
--- a/third_party/allwpilib/wpimath/wpimath-config.cmake.in
+++ b/third_party/allwpilib/wpimath/wpimath-config.cmake.in
@@ -2,4 +2,5 @@
 @FILENAME_DEP_REPLACE@
 @WPIUTIL_DEP_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpimath.cmake)
diff --git a/third_party/allwpilib/wpiutil/.styleguide b/third_party/allwpilib/wpiutil/.styleguide
index 2edcfaa..e8c1f84 100644
--- a/third_party/allwpilib/wpiutil/.styleguide
+++ b/third_party/allwpilib/wpiutil/.styleguide
@@ -2,6 +2,8 @@
   \.h$
   \.inc$
   \.inl$
+  math$
+  numbers$
 }
 
 cppSrcFileInclude {
@@ -46,7 +48,6 @@
   src/main/native/include/wpi/SmallSet\.h$
   src/main/native/include/wpi/SmallString\.h$
   src/main/native/include/wpi/SmallVector\.h$
-  src/main/native/include/wpi/StringExtras\.h$
   src/main/native/include/wpi/StringMap\.h$
   src/main/native/include/wpi/StringRef\.h$
   src/main/native/include/wpi/SwapByteOrder\.h$
@@ -58,6 +59,13 @@
   src/main/native/include/wpi/iterator_range\.h$
   src/main/native/include/wpi/raw_os_ostream\.h$
   src/main/native/include/wpi/raw_ostream\.h$
+  src/main/native/include/wpi/span\.h$
+  src/main/native/include/wpi/fs\.h$
+  src/main/native/include/wpi/mpack\.h$
+  src/main/native/cpp/fs\.cpp$
+  src/main/native/cpp/mpack\.cpp$
+  src/main/native/include/wpi/ghc/
+  src/test/native/cpp/span/
   src/main/native/include/wpi/type_traits\.h$
   src/main/native/cpp/json
   src/main/native/include/wpi/json
@@ -65,6 +73,7 @@
   src/main/native/include/uv\.h$
   src/main/native/include/uv/
   src/main/native/libuv/
+  src/main/native/fmtlib/
   src/main/native/resources/
   src/test/native/cpp/llvm/
   src/main/native/windows/StackWalker
diff --git a/third_party/allwpilib/wpiutil/BUILD b/third_party/allwpilib/wpiutil/BUILD
index 8c1b8db..58b9918 100644
--- a/third_party/allwpilib/wpiutil/BUILD
+++ b/third_party/allwpilib/wpiutil/BUILD
@@ -5,15 +5,16 @@
     srcs = glob(
         [
             "src/main/native/cpp/llvm/*.cpp",
+            "src/main/native/fmtlib/src/*.cpp",
         ],
     ) + [
-        "src/main/native/cpp/llvm/Unix/Path.inc",
         "src/main/native/cpp/timestamp.cpp",
         "src/main/native/cpp/SafeThread.cpp",
     ],
     hdrs = glob(
         [
             "src/main/native/include/**",
+            "src/main/native/fmtlib/include/**",
         ],
         exclude = ["STLExtras.h"],
     ),
@@ -22,6 +23,7 @@
     ],
     includes = [
         "src/main/native/include",
+        "src/main/native/fmtlib/include",
     ],
     target_compatible_with = ["//tools/platforms/hardware:roborio"],
     visibility = ["//visibility:public"],
diff --git a/third_party/allwpilib/wpiutil/CMakeLists.txt b/third_party/allwpilib/wpiutil/CMakeLists.txt
index f500c89..a152018 100644
--- a/third_party/allwpilib/wpiutil/CMakeLists.txt
+++ b/third_party/allwpilib/wpiutil/CMakeLists.txt
@@ -12,11 +12,11 @@
   find_package(Java REQUIRED)
   find_package(JNI REQUIRED)
   include(UseJava)
-  set(CMAKE_JAVA_COMPILE_FLAGS "-Xlint:unchecked")
+  set(CMAKE_JAVA_COMPILE_FLAGS "-encoding" "UTF8" "-Xlint:unchecked")
 
-  if(NOT EXISTS "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar")
+  if(NOT EXISTS "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/jackson-core-2.10.0.jar")
         set(BASE_URL "https://search.maven.org/remotecontent?filepath=")
-        set(JAR_ROOT "${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson")
+        set(JAR_ROOT "${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson")
 
         message(STATUS "Downloading Jackson jarfiles...")
 
@@ -31,7 +31,7 @@
     endif()
 
   file(GLOB JACKSON_JARS
-        ${CMAKE_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar)
+        ${WPILIB_BINARY_DIR}/wpiutil/thirdparty/jackson/*.jar)
 
   set(CMAKE_JAVA_INCLUDE_PATH wpiutil.jar ${JACKSON_JARS})
 
@@ -90,6 +90,8 @@
 file(GLOB_RECURSE wpiutil_unix_src src/main/native/unix/*.cpp)
 file(GLOB_RECURSE wpiutil_windows_src src/main/native/windows/*.cpp)
 
+file(GLOB fmtlib_native_src src/main/native/fmtlib/src/*.cpp)
+
 file(GLOB uv_native_src src/main/native/libuv/src/*.cpp)
 
 file(GLOB uv_windows_src src/main/native/libuv/src/win/*.cpp)
@@ -132,7 +134,7 @@
     src/main/native/libuv/src/unix/sysinfo-loadavg.cpp
 )
 
-add_library(wpiutil ${wpiutil_native_src} ${wpiutil_resources_src})
+add_library(wpiutil ${wpiutil_native_src} ${fmtlib_native_src} ${wpiutil_resources_src})
 set_target_properties(wpiutil PROPERTIES DEBUG_POSTFIX "d")
 
 set_property(TARGET wpiutil PROPERTY FOLDER "libraries")
@@ -145,6 +147,17 @@
 wpilib_target_warnings(wpiutil)
 target_link_libraries(wpiutil Threads::Threads ${CMAKE_DL_LIBS} ${ATOMIC})
 
+if (NOT USE_VCPKG_FMTLIB)
+    target_sources(wpiutil PRIVATE ${fmtlib_native_src})
+    install(DIRECTORY src/main/native/fmtlib/include/ DESTINATION "${include_dest}/wpiutil")
+    target_include_directories(wpiutil PUBLIC
+                            $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/src/main/native/fmtlib/include>
+                            $<INSTALL_INTERFACE:${include_dest}/wpiutil>)
+else()
+    find_package(fmt CONFIG REQUIRED)
+    target_link_libraries(wpiutil fmt::fmt)
+endif()
+
 if (NOT USE_VCPKG_LIBUV)
     target_sources(wpiutil PRIVATE ${uv_native_src})
     install(DIRECTORY src/main/native/libuv/include/ DESTINATION "${include_dest}/wpiutil")
@@ -195,8 +208,8 @@
     set (wpiutil_config_dir share/wpiutil)
 endif()
 
-configure_file(wpiutil-config.cmake.in ${CMAKE_BINARY_DIR}/wpiutil-config.cmake )
-install(FILES ${CMAKE_BINARY_DIR}/wpiutil-config.cmake DESTINATION ${wpiutil_config_dir})
+configure_file(wpiutil-config.cmake.in ${WPILIB_BINARY_DIR}/wpiutil-config.cmake )
+install(FILES ${WPILIB_BINARY_DIR}/wpiutil-config.cmake DESTINATION ${wpiutil_config_dir})
 install(EXPORT wpiutil DESTINATION ${wpiutil_config_dir})
 
 SUBDIR_LIST(wpiutil_examples "${CMAKE_CURRENT_SOURCE_DIR}/examples")
diff --git a/third_party/allwpilib/wpiutil/build.gradle b/third_party/allwpilib/wpiutil/build.gradle
index f2efe28..752e709 100644
--- a/third_party/allwpilib/wpiutil/build.gradle
+++ b/third_party/allwpilib/wpiutil/build.gradle
@@ -24,6 +24,15 @@
                     srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src'
                 }
             }
+            fmtlibCpp(CppSourceSet) {
+                source {
+                    srcDirs 'src/main/native/fmtlib/src'
+                    include '*.cpp'
+                }
+                exportedHeaders {
+                    srcDirs 'src/main/native/fmtlib/include'
+                }
+            }
             resourcesCpp(CppSourceSet) {
                 source {
                     srcDirs "$buildDir/generated/main/cpp", "$rootDir/shared/singlelib"
@@ -146,26 +155,44 @@
 
 def examplesMap = [:];
 file("$projectDir/examples").list(new FilenameFilter() {
-    @Override
-    public boolean accept(File current, String name) {
-        return new File(current, name).isDirectory();
-    }
-}).each {
-    examplesMap.put(it, [])
-}
+            @Override
+            public boolean accept(File current, String name) {
+                return new File(current, name).isDirectory();
+            }
+        }).each {
+            examplesMap.put(it, [])
+        }
 
 apply from: "${rootDir}/shared/jni/setupBuild.gradle"
 
 nativeUtils.exportsConfigs {
     wpiutil {
-        x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
-        x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
-                            '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
-                            '_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
-                            '_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
+        x86ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
+        x64ExcludeSymbols = [
+            '_CT??_R0?AV_System_error',
+            '_CT??_R0?AVexception',
+            '_CT??_R0?AVfailure',
+            '_CT??_R0?AVruntime_error',
+            '_CT??_R0?AVsystem_error',
+            '_CTA5?AVfailure',
+            '_TI5?AVfailure',
+            '_CT??_R0?AVout_of_range',
+            '_CTA3?AVout_of_range',
+            '_TI3?AVout_of_range',
+            '_CT??_R0?AVbad_cast'
+        ]
     }
 }
 
@@ -173,6 +200,9 @@
     from('src/main/native/libuv/include') {
         into '/'
     }
+    from('src/main/native/fmtlib/include') {
+        into '/'
+    }
 }
 
 model {
@@ -180,7 +210,7 @@
         all {
             it.sources.each {
                 it.exportedHeaders {
-                    srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src'
+                    srcDirs 'src/main/native/include', 'src/main/native/libuv/include', 'src/main/native/libuv/src', 'src/main/native/fmtlib/include'
                 }
             }
         }
@@ -210,7 +240,9 @@
             sources {
                 cpp {
                     source {
-                        srcDirs = ['src/netconsoleServer/native/cpp']
+                        srcDirs = [
+                            'src/netconsoleServer/native/cpp'
+                        ]
                         includes = ['**/*.cpp']
                     }
                 }
@@ -227,7 +259,9 @@
             sources {
                 cpp {
                     source {
-                        srcDirs = ['src/netconsoleTee/native/cpp']
+                        srcDirs = [
+                            'src/netconsoleTee/native/cpp'
+                        ]
                         includes = ['**/*.cpp']
                     }
                 }
diff --git a/third_party/allwpilib/wpiutil/examples/dsclient/dsclient.cpp b/third_party/allwpilib/wpiutil/examples/dsclient/dsclient.cpp
new file mode 100644
index 0000000..69c1061
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/examples/dsclient/dsclient.cpp
@@ -0,0 +1,35 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdio>
+
+#include "fmt/format.h"
+#include "wpi/DsClient.h"
+#include "wpi/EventLoopRunner.h"
+#include "wpi/Logger.h"
+#include "wpi/uv/Error.h"
+
+namespace uv = wpi::uv;
+
+static void logfunc(unsigned int level, const char* file, unsigned int line,
+                    const char* msg) {
+  std::fprintf(stderr, "(%d) %s:%d: %s\n", level, file, line, msg);
+}
+
+int main() {
+  wpi::Logger logger{logfunc, 0};
+
+  // Kick off the event loop on a separate thread
+  wpi::EventLoopRunner loop;
+  std::shared_ptr<wpi::DsClient> client;
+  loop.ExecAsync([&](uv::Loop& loop) {
+    client = wpi::DsClient::Create(loop, logger);
+    client->setIp.connect(
+        [](std::string_view ip) { fmt::print("got IP: {}\n", ip); });
+    client->clearIp.connect([] { std::fputs("cleared IP\n", stdout); });
+  });
+
+  // wait for a keypress to terminate
+  std::getchar();
+}
diff --git a/third_party/allwpilib/wpiutil/examples/parallelconnect/parallelconnect.cpp b/third_party/allwpilib/wpiutil/examples/parallelconnect/parallelconnect.cpp
new file mode 100644
index 0000000..929c847
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/examples/parallelconnect/parallelconnect.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cstdio>
+
+#include "wpi/EventLoopRunner.h"
+#include "wpi/Logger.h"
+#include "wpi/ParallelTcpConnector.h"
+#include "wpi/uv/Error.h"
+#include "wpi/uv/Tcp.h"
+
+namespace uv = wpi::uv;
+
+static void logfunc(unsigned int level, const char* file, unsigned int line,
+                    const char* msg) {
+  std::fprintf(stderr, "(%d) %s:%d: %s\n", level, file, line, msg);
+}
+
+int main() {
+  wpi::Logger logger{logfunc, 0};
+
+  // Kick off the event loop on a separate thread
+  wpi::EventLoopRunner loop;
+  std::shared_ptr<wpi::ParallelTcpConnector> connect;
+  loop.ExecAsync([&](uv::Loop& loop) {
+    connect = wpi::ParallelTcpConnector::Create(
+        loop, uv::Timer::Time{2000}, logger, [&](uv::Tcp& tcp) {
+          std::fputs("Got connection, accepting!\n", stdout);
+          tcp.StartRead();
+          connect->Succeeded(tcp);
+          tcp.end.connect([&] {
+            std::fputs("TCP connection ended, disconnecting!\n", stdout);
+            tcp.Close();
+            connect->Disconnected();
+          });
+          tcp.error.connect([&](uv::Error) {
+            std::fputs("TCP error, disconnecting!\n", stdout);
+            connect->Disconnected();
+          });
+        });
+    connect->SetServers({{{"roborio-294-frc.local", 8080},
+                          {"roborio-294-frc.frc-field.local", 8080},
+                          {"10.2.94.2", 8080},
+                          {"127.0.0.1", 8080}}});
+  });
+
+  // wait for a keypress to terminate
+  std::getchar();
+}
diff --git a/third_party/allwpilib/wpiutil/examples/webserver/webserver.cpp b/third_party/allwpilib/wpiutil/examples/webserver/webserver.cpp
index e112d04..503405f 100644
--- a/third_party/allwpilib/wpiutil/examples/webserver/webserver.cpp
+++ b/third_party/allwpilib/wpiutil/examples/webserver/webserver.cpp
@@ -1,16 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <cstdio>
 
+#include "fmt/format.h"
 #include "wpi/EventLoopRunner.h"
 #include "wpi/HttpServerConnection.h"
 #include "wpi/UrlParser.h"
-#include "wpi/raw_ostream.h"
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/Tcp.h"
 
@@ -26,7 +23,7 @@
 };
 
 void MyHttpServerConnection::ProcessRequest() {
-  wpi::errs() << "HTTP request: '" << m_request.GetUrl() << "'\n";
+  fmt::print(stderr, "HTTP request: '{}'\n", m_request.GetUrl());
   wpi::UrlParser url{m_request.GetUrl(),
                      m_request.GetMethod() == wpi::HTTP_CONNECT};
   if (!url.IsValid()) {
@@ -35,23 +32,25 @@
     return;
   }
 
-  wpi::StringRef path;
-  if (url.HasPath()) path = url.GetPath();
-  wpi::errs() << "path: \"" << path << "\"\n";
+  std::string_view path;
+  if (url.HasPath()) {
+    path = url.GetPath();
+  }
+  fmt::print(stderr, "path: \"{}\"\n", path);
 
-  wpi::StringRef query;
-  if (url.HasQuery()) query = url.GetQuery();
-  wpi::errs() << "query: \"" << query << "\"\n";
+  std::string_view query;
+  if (url.HasQuery()) {
+    query = url.GetQuery();
+  }
+  fmt::print(stderr, "query: \"{}\"\n", query);
 
   const bool isGET = m_request.GetMethod() == wpi::HTTP_GET;
-  if (isGET && path.equals("/")) {
+  if (isGET && path == "/") {
     // build HTML root page
-    wpi::SmallString<256> buf;
-    wpi::raw_svector_ostream os{buf};
-    os << "<html><head><title>WebServer Example</title></head>";
-    os << "<body><p>This is an example root page from the webserver.";
-    os << "</body></html>";
-    SendResponse(200, "OK", "text/html", os.str());
+    SendResponse(200, "OK", "text/html",
+                 "<html><head><title>WebServer Example</title></head>"
+                 "<body><p>This is an example root page from the webserver."
+                 "</body></html>");
   } else {
     SendError(404, "Resource not found");
   }
@@ -69,8 +68,10 @@
     // when we get a connection, accept it and start reading
     tcp->connection.connect([srv = tcp.get()] {
       auto tcp = srv->Accept();
-      if (!tcp) return;
-      wpi::errs() << "Got a connection\n";
+      if (!tcp) {
+        return;
+      }
+      std::fputs("Got a connection\n", stderr);
       auto conn = std::make_shared<MyHttpServerConnection>(tcp);
       tcp->SetData(conn);
     });
@@ -78,7 +79,7 @@
     // start listening for incoming connections
     tcp->Listen();
 
-    wpi::errs() << "Listening on port 8080\n";
+    std::fputs("Listening on port 8080\n", stderr);
   });
 
   // wait for a keypress to terminate
diff --git a/third_party/allwpilib/wpiutil/src/dev/java/edu/wpi/first/util/DevMain.java b/third_party/allwpilib/wpiutil/src/dev/java/edu/wpi/first/util/DevMain.java
new file mode 100644
index 0000000..3d5b134
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/dev/java/edu/wpi/first/util/DevMain.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+public final class DevMain {
+  /** Main entry point. */
+  public static void main(String[] args) {
+    System.out.println("Hello World!");
+    System.out.println(RuntimeDetector.getPlatformPath());
+  }
+
+  private DevMain() {}
+}
diff --git a/third_party/allwpilib/wpiutil/src/dev/java/edu/wpi/first/wpiutil/DevMain.java b/third_party/allwpilib/wpiutil/src/dev/java/edu/wpi/first/wpiutil/DevMain.java
deleted file mode 100644
index c5f1754..0000000
--- a/third_party/allwpilib/wpiutil/src/dev/java/edu/wpi/first/wpiutil/DevMain.java
+++ /dev/null
@@ -1,21 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-public final class DevMain {
-  /**
-   * Main entry point.
-   */
-  public static void main(String[] args) {
-    System.out.println("Hello World!");
-    System.out.println(RuntimeDetector.getPlatformPath());
-  }
-
-  private DevMain() {
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/dev/native/cpp/main.cpp b/third_party/allwpilib/wpiutil/src/dev/native/cpp/main.cpp
index 9ac3bab..14ddda2 100644
--- a/third_party/allwpilib/wpiutil/src/dev/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpiutil/src/dev/native/cpp/main.cpp
@@ -1,17 +1,11 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
-#include <iostream>
-
-#include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
-#include "wpi/hostname.h"
+#include "fmt/core.h"
+#include "wpi/SmallString.h"
 
 int main() {
-  wpi::StringRef v1("Hello");
-  std::cout << v1.lower() << std::endl;
+  wpi::SmallString<128> v1("Hello");
+  fmt::print("{}\n", v1.str());
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java
new file mode 100644
index 0000000..f9129c7
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CircularBuffer.java
@@ -0,0 +1,191 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+/** This is a simple circular buffer so we don't need to "bucket brigade" copy old values. */
+public class CircularBuffer {
+  private double[] m_data;
+
+  // Index of element at front of buffer
+  private int m_front;
+
+  // Number of elements used in buffer
+  private int m_length;
+
+  /**
+   * Create a CircularBuffer with the provided size.
+   *
+   * @param size The size of the circular buffer.
+   */
+  public CircularBuffer(int size) {
+    m_data = new double[size];
+    for (int i = 0; i < m_data.length; i++) {
+      m_data[i] = 0.0;
+    }
+  }
+
+  /**
+   * Returns number of elements in buffer.
+   *
+   * @return number of elements in buffer
+   */
+  double size() {
+    return m_length;
+  }
+
+  /**
+   * Get value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  double getFirst() {
+    return m_data[m_front];
+  }
+
+  /**
+   * Get value at back of buffer.
+   *
+   * @return value at back of buffer
+   */
+  double getLast() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    return m_data[(m_front + m_length - 1) % m_data.length];
+  }
+
+  /**
+   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
+   * full.
+   *
+   * @param value The value to push.
+   */
+  public void addFirst(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_front = moduloDec(m_front);
+
+    m_data[m_front] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
+   * full.
+   *
+   * @param value The value to push.
+   */
+  public void addLast(double value) {
+    if (m_data.length == 0) {
+      return;
+    }
+
+    m_data[(m_front + m_length) % m_data.length] = value;
+
+    if (m_length < m_data.length) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = moduloInc(m_front);
+    }
+  }
+
+  /**
+   * Pop value at front of buffer.
+   *
+   * @return value at front of buffer
+   */
+  public double removeFirst() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    double temp = m_data[m_front];
+    m_front = moduloInc(m_front);
+    m_length--;
+    return temp;
+  }
+
+  /**
+   * Pop value at back of buffer.
+   *
+   * @return value at back of buffer
+   */
+  public double removeLast() {
+    // If there are no elements in the buffer, do nothing
+    if (m_length == 0) {
+      return 0.0;
+    }
+
+    m_length--;
+    return m_data[(m_front + m_length) % m_data.length];
+  }
+
+  /**
+   * Resizes internal buffer to given size.
+   *
+   * <p>A new buffer is allocated because arrays are not resizable.
+   *
+   * @param size New buffer size.
+   */
+  void resize(int size) {
+    double[] newBuffer = new double[size];
+    m_length = Math.min(m_length, size);
+    for (int i = 0; i < m_length; i++) {
+      newBuffer[i] = m_data[(m_front + i) % m_data.length];
+    }
+    m_data = newBuffer;
+    m_front = 0;
+  }
+
+  /** Sets internal buffer contents to zero. */
+  public void clear() {
+    for (int i = 0; i < m_data.length; i++) {
+      m_data[i] = 0.0;
+    }
+    m_front = 0;
+    m_length = 0;
+  }
+
+  /**
+   * Get the element at the provided index relative to the start of the buffer.
+   *
+   * @param index Index into the buffer.
+   * @return Element at index starting from front of buffer.
+   */
+  public double get(int index) {
+    return m_data[(m_front + index) % m_data.length];
+  }
+
+  /**
+   * Increment an index modulo the length of the m_data buffer.
+   *
+   * @param index Index into the buffer.
+   */
+  private int moduloInc(int index) {
+    return (index + 1) % m_data.length;
+  }
+
+  /**
+   * Decrement an index modulo the length of the m_data buffer.
+   *
+   * @param index Index into the buffer.
+   */
+  private int moduloDec(int index) {
+    if (index == 0) {
+      return m_data.length - 1;
+    } else {
+      return index - 1;
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java
new file mode 100644
index 0000000..9b23144
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/CombinedRuntimeLoader.java
@@ -0,0 +1,186 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import com.fasterxml.jackson.core.type.TypeReference;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import java.util.Objects;
+
+public final class CombinedRuntimeLoader {
+  private CombinedRuntimeLoader() {}
+
+  private static String extractionDirectory;
+
+  public static synchronized String getExtractionDirectory() {
+    return extractionDirectory;
+  }
+
+  private static synchronized void setExtractionDirectory(String directory) {
+    extractionDirectory = directory;
+  }
+
+  public static native String setDllDirectory(String directory);
+
+  private static String getLoadErrorMessage(String libraryName, UnsatisfiedLinkError ule) {
+    StringBuilder msg = new StringBuilder(512);
+    msg.append(libraryName)
+        .append(" could not be loaded from path\n" + "\tattempted to load for platform ")
+        .append(RuntimeDetector.getPlatformPath())
+        .append("\nLast Load Error: \n")
+        .append(ule.getMessage())
+        .append('\n');
+    if (RuntimeDetector.isWindows()) {
+      msg.append(
+          "A common cause of this error is missing the C++ runtime.\n"
+              + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n");
+    }
+    return msg.toString();
+  }
+
+  /**
+   * Extract a list of native libraries.
+   *
+   * @param <T> The class where the resources would be located
+   * @param clazz The actual class object
+   * @param resourceName The resource name on the classpath to use for file lookup
+   * @return List of all libraries that were extracted
+   * @throws IOException Thrown if resource not found or file could not be extracted
+   */
+  @SuppressWarnings({"PMD.UnnecessaryCastRule", "unchecked"})
+  public static <T> List<String> extractLibraries(Class<T> clazz, String resourceName)
+      throws IOException {
+    TypeReference<HashMap<String, Object>> typeRef =
+        new TypeReference<HashMap<String, Object>>() {};
+    ObjectMapper mapper = new ObjectMapper();
+    Map<String, Object> map;
+    try (var stream = clazz.getResourceAsStream(resourceName)) {
+      map = mapper.readValue(stream, typeRef);
+    }
+
+    var platformPath = Paths.get(RuntimeDetector.getPlatformPath());
+    var platform = platformPath.getName(0).toString();
+    var arch = platformPath.getName(1).toString();
+
+    var platformMap = (Map<String, List<String>>) map.get(platform);
+
+    var fileList = platformMap.get(arch);
+
+    var extractionPathString = getExtractionDirectory();
+
+    if (extractionPathString == null) {
+      String hash = (String) map.get("hash");
+
+      var defaultExtractionRoot = RuntimeLoader.getDefaultExtractionRoot();
+      var extractionPath = Paths.get(defaultExtractionRoot, platform, arch, hash);
+      extractionPathString = extractionPath.toString();
+
+      setExtractionDirectory(extractionPathString);
+    }
+
+    List<String> extractedFiles = new ArrayList<>();
+
+    byte[] buffer = new byte[0x10000]; // 64K copy buffer
+
+    for (var file : fileList) {
+      try (var stream = clazz.getResourceAsStream(file)) {
+        Objects.requireNonNull(stream);
+
+        var outputFile = Paths.get(extractionPathString, new File(file).getName());
+        extractedFiles.add(outputFile.toString());
+        if (outputFile.toFile().exists()) {
+          continue;
+        }
+        var parent = outputFile.getParent();
+        if (parent == null) {
+          throw new IOException("Output file has no parent");
+        }
+        parent.toFile().mkdirs();
+
+        try (var os = Files.newOutputStream(outputFile)) {
+          int readBytes;
+          while ((readBytes = stream.read(buffer)) != -1) { // NOPMD
+            os.write(buffer, 0, readBytes);
+          }
+        }
+      }
+    }
+
+    return extractedFiles;
+  }
+
+  /**
+   * Load a single library from a list of extracted files.
+   *
+   * @param libraryName The library name to load
+   * @param extractedFiles The extracted files to search
+   * @throws IOException If library was not found
+   */
+  public static void loadLibrary(String libraryName, List<String> extractedFiles)
+      throws IOException {
+    String currentPath = null;
+    String oldDllDirectory = null;
+    try {
+      if (RuntimeDetector.isWindows()) {
+        var extractionPathString = getExtractionDirectory();
+        oldDllDirectory = setDllDirectory(extractionPathString);
+      }
+      for (var extractedFile : extractedFiles) {
+        if (extractedFile.contains(libraryName)) {
+          // Load it
+          currentPath = extractedFile;
+          System.load(extractedFile);
+          return;
+        }
+      }
+      throw new IOException("Could not find library " + libraryName);
+    } catch (UnsatisfiedLinkError ule) {
+      throw new IOException(getLoadErrorMessage(currentPath, ule));
+    } finally {
+      if (oldDllDirectory != null) {
+        setDllDirectory(oldDllDirectory);
+      }
+    }
+  }
+
+  /**
+   * Load a list of native libraries out of a single directory.
+   *
+   * @param <T> The class where the resources would be located
+   * @param clazz The actual class object
+   * @param librariesToLoad List of libraries to load
+   * @throws IOException Throws an IOException if not found
+   */
+  public static <T> void loadLibraries(Class<T> clazz, String... librariesToLoad)
+      throws IOException {
+    // Extract everything
+
+    var extractedFiles = extractLibraries(clazz, "/ResourceInformation.json");
+
+    String currentPath = "";
+
+    try {
+      if (RuntimeDetector.isWindows()) {
+        var extractionPathString = getExtractionDirectory();
+        // Load windows, set dll directory
+        currentPath = Paths.get(extractionPathString, "WindowsLoaderHelper.dll").toString();
+        System.load(currentPath);
+      }
+    } catch (UnsatisfiedLinkError ule) {
+      throw new IOException(getLoadErrorMessage(currentPath, ule));
+    }
+
+    for (var library : librariesToLoad) {
+      loadLibrary(library, extractedFiles);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/ErrorMessages.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/ErrorMessages.java
new file mode 100644
index 0000000..9292ed9
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/ErrorMessages.java
@@ -0,0 +1,41 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import static java.util.Objects.requireNonNull;
+
+/** Utility class for common WPILib error messages. */
+public final class ErrorMessages {
+  /** Utility class, so constructor is private. */
+  private ErrorMessages() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Requires that a parameter of a method not be null; prints an error message with helpful
+   * debugging instructions if the parameter is null.
+   *
+   * @param <T> Type of object.
+   * @param obj The parameter that must not be null.
+   * @param paramName The name of the parameter.
+   * @param methodName The name of the method.
+   * @return The object parameter confirmed not to be null.
+   */
+  public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
+    return requireNonNull(
+        obj,
+        "Parameter "
+            + paramName
+            + " in method "
+            + methodName
+            + " was null when it"
+            + " should not have been!  Check the stacktrace to find the responsible line of code - "
+            + "usually, it is the first line of user-written code indicated in the stacktrace.  "
+            + "Make sure all objects passed to the method in question were properly initialized -"
+            + " note that this may not be obvious if it is being called under "
+            + "dynamically-changing conditions!  Please do not seek additional technical assistance"
+            + " without doing this first!");
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java
new file mode 100644
index 0000000..320bc5b
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeDetector.java
@@ -0,0 +1,179 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import java.io.BufferedReader;
+import java.io.File;
+import java.io.IOException;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+
+public final class RuntimeDetector {
+  private static String filePrefix;
+  private static String fileExtension;
+  private static String filePath;
+
+  private static synchronized void computePlatform() {
+    if (fileExtension != null && filePath != null && filePrefix != null) {
+      return;
+    }
+
+    boolean intel32 = is32BitIntel();
+    boolean intel64 = is64BitIntel();
+
+    if (isWindows()) {
+      filePrefix = "";
+      fileExtension = ".dll";
+      if (intel32) {
+        filePath = "/windows/x86/";
+      } else {
+        filePath = "/windows/x86-64/";
+      }
+    } else if (isMac()) {
+      filePrefix = "lib";
+      fileExtension = ".dylib";
+      if (intel32) {
+        filePath = "/osx/x86";
+      } else {
+        filePath = "/osx/x86-64/";
+      }
+    } else if (isLinux()) {
+      filePrefix = "lib";
+      fileExtension = ".so";
+      if (intel32) {
+        filePath = "/linux/x86/";
+      } else if (intel64) {
+        filePath = "/linux/x86-64/";
+      } else if (isAthena()) {
+        filePath = "/linux/athena/";
+      } else if (isRaspbian()) {
+        filePath = "/linux/raspbian/";
+      } else if (isAarch64()) {
+        filePath = "/linux/aarch64bionic/";
+      } else {
+        filePath = "/linux/nativearm/";
+      }
+    } else {
+      throw new IllegalStateException("Failed to determine OS");
+    }
+  }
+
+  /**
+   * Get the file prefix for the current system.
+   *
+   * @return The file prefix.
+   */
+  public static synchronized String getFilePrefix() {
+    computePlatform();
+
+    return filePrefix;
+  }
+
+  /**
+   * Get the file extension for the current system.
+   *
+   * @return The file extension.
+   */
+  public static synchronized String getFileExtension() {
+    computePlatform();
+
+    return fileExtension;
+  }
+
+  /**
+   * Get the platform path for the current system.
+   *
+   * @return The platform path.
+   */
+  public static synchronized String getPlatformPath() {
+    computePlatform();
+
+    return filePath;
+  }
+
+  /**
+   * Get the path to the requested resource.
+   *
+   * @param libName Library name.
+   * @return The path to the requested resource.
+   */
+  public static synchronized String getLibraryResource(String libName) {
+    computePlatform();
+
+    return filePath + filePrefix + libName + fileExtension;
+  }
+
+  /**
+   * Get the path to the hash to the requested resource.
+   *
+   * @param libName Library name.
+   * @return The path to the hash to the requested resource.
+   */
+  public static synchronized String getHashLibraryResource(String libName) {
+    computePlatform();
+
+    return filePath + libName + ".hash";
+  }
+
+  /**
+   * Check if hardware platform is Athena.
+   *
+   * @return True if hardware platform is Athena.
+   */
+  public static boolean isAthena() {
+    File runRobotFile = new File("/usr/local/frc/bin/frcRunRobot.sh");
+    return runRobotFile.exists();
+  }
+
+  /**
+   * Check if OS is Raspbian.
+   *
+   * @return True if OS is Raspbian.
+   */
+  public static boolean isRaspbian() {
+    try (BufferedReader reader = Files.newBufferedReader(Paths.get("/etc/os-release"))) {
+      String value = reader.readLine();
+      if (value == null) {
+        return false;
+      }
+      return value.contains("Raspbian");
+    } catch (IOException ex) {
+      return false;
+    }
+  }
+
+  /**
+   * check if architecture is aarch64.
+   *
+   * @return if architecture is aarch64
+   */
+  public static boolean isAarch64() {
+    return System.getProperty("os.arch").equals("aarch64");
+  }
+
+  public static boolean isLinux() {
+    return System.getProperty("os.name").startsWith("Linux");
+  }
+
+  public static boolean isWindows() {
+    return System.getProperty("os.name").startsWith("Windows");
+  }
+
+  public static boolean isMac() {
+    return System.getProperty("os.name").startsWith("Mac");
+  }
+
+  public static boolean is32BitIntel() {
+    String arch = System.getProperty("os.arch");
+    return "x86".equals(arch) || "i386".equals(arch);
+  }
+
+  public static boolean is64BitIntel() {
+    String arch = System.getProperty("os.arch");
+    return "amd64".equals(arch) || "x86_64".equals(arch);
+  }
+
+  private RuntimeDetector() {}
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java
new file mode 100644
index 0000000..8a2c11a
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/RuntimeLoader.java
@@ -0,0 +1,199 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import java.io.File;
+import java.io.IOException;
+import java.io.InputStream;
+import java.io.OutputStream;
+import java.nio.charset.StandardCharsets;
+import java.nio.file.Files;
+import java.nio.file.Paths;
+import java.security.DigestInputStream;
+import java.security.MessageDigest;
+import java.security.NoSuchAlgorithmException;
+import java.util.Locale;
+import java.util.Scanner;
+
+public final class RuntimeLoader<T> {
+  private static String defaultExtractionRoot;
+
+  /**
+   * Gets the default extration root location (~/.wpilib/nativecache).
+   *
+   * @return The default extraction root location.
+   */
+  public static synchronized String getDefaultExtractionRoot() {
+    if (defaultExtractionRoot != null) {
+      return defaultExtractionRoot;
+    }
+    String home = System.getProperty("user.home");
+    defaultExtractionRoot = Paths.get(home, ".wpilib", "nativecache").toString();
+    return defaultExtractionRoot;
+  }
+
+  private final String m_libraryName;
+  private final Class<T> m_loadClass;
+  private final String m_extractionRoot;
+
+  /**
+   * Creates a new library loader.
+   *
+   * @param libraryName Name of library to load.
+   * @param extractionRoot Location from which to load the library.
+   * @param cls Class whose classpath the given library belongs.
+   */
+  public RuntimeLoader(String libraryName, String extractionRoot, Class<T> cls) {
+    m_libraryName = libraryName;
+    m_loadClass = cls;
+    m_extractionRoot = extractionRoot;
+  }
+
+  /**
+   * Returns a load error message given the information in the provided UnsatisfiedLinkError.
+   *
+   * @param ule UnsatisfiedLinkError object.
+   * @return A load error message.
+   */
+  private String getLoadErrorMessage(UnsatisfiedLinkError ule) {
+    StringBuilder msg = new StringBuilder(512);
+    msg.append(m_libraryName)
+        .append(
+            " could not be loaded from path or an embedded resource.\n"
+                + "\tattempted to load for platform ")
+        .append(RuntimeDetector.getPlatformPath())
+        .append("\nLast Load Error: \n")
+        .append(ule.getMessage())
+        .append('\n');
+    if (RuntimeDetector.isWindows()) {
+      msg.append(
+          "A common cause of this error is missing the C++ runtime.\n"
+              + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n");
+    }
+    return msg.toString();
+  }
+
+  /**
+   * Loads a native library.
+   *
+   * @throws IOException if the library fails to load
+   */
+  @SuppressWarnings("PMD.PreserveStackTrace")
+  public void loadLibrary() throws IOException {
+    try {
+      // First, try loading path
+      System.loadLibrary(m_libraryName);
+    } catch (UnsatisfiedLinkError ule) {
+      // Then load the hash from the resources
+      String hashName = RuntimeDetector.getHashLibraryResource(m_libraryName);
+      String resname = RuntimeDetector.getLibraryResource(m_libraryName);
+      try (InputStream hashIs = m_loadClass.getResourceAsStream(hashName)) {
+        if (hashIs == null) {
+          throw new IOException(getLoadErrorMessage(ule));
+        }
+        try (Scanner scanner = new Scanner(hashIs, StandardCharsets.UTF_8.name())) {
+          String hash = scanner.nextLine();
+          File jniLibrary = new File(m_extractionRoot, resname + "." + hash);
+          try {
+            // Try to load from an already extracted hash
+            System.load(jniLibrary.getAbsolutePath());
+          } catch (UnsatisfiedLinkError ule2) {
+            // If extraction failed, extract
+            try (InputStream resIs = m_loadClass.getResourceAsStream(resname)) {
+              if (resIs == null) {
+                throw new IOException(getLoadErrorMessage(ule));
+              }
+
+              var parentFile = jniLibrary.getParentFile();
+              if (parentFile == null) {
+                throw new IOException("JNI library has no parent file");
+              }
+              parentFile.mkdirs();
+
+              try (OutputStream os = Files.newOutputStream(jniLibrary.toPath())) {
+                byte[] buffer = new byte[0xFFFF]; // 64K copy buffer
+                int readBytes;
+                while ((readBytes = resIs.read(buffer)) != -1) { // NOPMD
+                  os.write(buffer, 0, readBytes);
+                }
+              }
+              System.load(jniLibrary.getAbsolutePath());
+            }
+          }
+        }
+      }
+    }
+  }
+
+  /**
+   * Load a native library by directly hashing the file.
+   *
+   * @throws IOException if the library failed to load
+   */
+  @SuppressWarnings({"PMD.PreserveStackTrace", "PMD.EmptyWhileStmt"})
+  public void loadLibraryHashed() throws IOException {
+    try {
+      // First, try loading path
+      System.loadLibrary(m_libraryName);
+    } catch (UnsatisfiedLinkError ule) {
+      // Then load the hash from the input file
+      String resname = RuntimeDetector.getLibraryResource(m_libraryName);
+      String hash = null;
+      try (InputStream is = m_loadClass.getResourceAsStream(resname)) {
+        if (is == null) {
+          throw new IOException(getLoadErrorMessage(ule));
+        }
+        MessageDigest md = null;
+        try {
+          md = MessageDigest.getInstance("MD5");
+        } catch (NoSuchAlgorithmException nsae) {
+          throw new RuntimeException("Weird Hash Algorithm?");
+        }
+        try (DigestInputStream dis = new DigestInputStream(is, md)) {
+          // Read the entire buffer once to hash
+          byte[] buffer = new byte[0xFFFF];
+          while (dis.read(buffer) > -1) {}
+          MessageDigest digest = dis.getMessageDigest();
+          byte[] digestOutput = digest.digest();
+          StringBuilder builder = new StringBuilder();
+          for (byte b : digestOutput) {
+            builder.append(String.format("%02X", b));
+          }
+          hash = builder.toString().toLowerCase(Locale.ENGLISH);
+        }
+      }
+      if (hash == null) {
+        throw new IOException("Weird Hash?");
+      }
+      File jniLibrary = new File(m_extractionRoot, resname + "." + hash);
+      try {
+        // Try to load from an already extracted hash
+        System.load(jniLibrary.getAbsolutePath());
+      } catch (UnsatisfiedLinkError ule2) {
+        // If extraction failed, extract
+        try (InputStream resIs = m_loadClass.getResourceAsStream(resname)) {
+          if (resIs == null) {
+            throw new IOException(getLoadErrorMessage(ule));
+          }
+
+          var parentFile = jniLibrary.getParentFile();
+          if (parentFile == null) {
+            throw new IOException("JNI library has no parent file");
+          }
+          parentFile.mkdirs();
+
+          try (OutputStream os = Files.newOutputStream(jniLibrary.toPath())) {
+            byte[] buffer = new byte[0xFFFF]; // 64K copy buffer
+            int readBytes;
+            while ((readBytes = resIs.read(buffer)) != -1) { // NOPMD
+              os.write(buffer, 0, readBytes);
+            }
+          }
+          System.load(jniLibrary.getAbsolutePath());
+        }
+      }
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
new file mode 100644
index 0000000..b7b2f59
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/WPIUtilJNI.java
@@ -0,0 +1,125 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import java.io.IOException;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+public final class WPIUtilJNI {
+  static boolean libraryLoaded = false;
+  static RuntimeLoader<WPIUtilJNI> loader = null;
+
+  public static class Helper {
+    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
+
+    public static boolean getExtractOnStaticLoad() {
+      return extractOnStaticLoad.get();
+    }
+
+    public static void setExtractOnStaticLoad(boolean load) {
+      extractOnStaticLoad.set(load);
+    }
+  }
+
+  static {
+    if (Helper.getExtractOnStaticLoad()) {
+      try {
+        loader =
+            new RuntimeLoader<>(
+                "wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIUtilJNI.class);
+        loader.loadLibrary();
+      } catch (IOException ex) {
+        ex.printStackTrace();
+        System.exit(1);
+      }
+      libraryLoaded = true;
+    }
+  }
+
+  /**
+   * Force load the library.
+   *
+   * @throws IOException if the library failed to load
+   */
+  public static synchronized void forceLoad() throws IOException {
+    if (libraryLoaded) {
+      return;
+    }
+    loader =
+        new RuntimeLoader<>(
+            "wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIUtilJNI.class);
+    loader.loadLibrary();
+    libraryLoaded = true;
+  }
+
+  public static native void enableMockTime();
+
+  public static native void setMockTime(long time);
+
+  public static native long now();
+
+  public static native void addPortForwarder(int port, String remoteHost, int remotePort);
+
+  public static native void removePortForwarder(int port);
+
+  public static native int createEvent(boolean manualReset, boolean initialState);
+
+  public static native void destroyEvent(int eventHandle);
+
+  public static native void setEvent(int eventHandle);
+
+  public static native void resetEvent(int eventHandle);
+
+  public static native int createSemaphore(int initialCount, int maximumCount);
+
+  public static native void destroySemaphore(int semHandle);
+
+  public static native boolean releaseSemaphore(int semHandle, int releaseCount);
+
+  /**
+   * Waits for an handle to be signaled.
+   *
+   * @param handle handle to wait on
+   * @throws InterruptedException on failure (e.g. object was destroyed)
+   */
+  public static native void waitForObject(int handle) throws InterruptedException;
+
+  /**
+   * Waits for an handle to be signaled, with timeout.
+   *
+   * @param handle handle to wait on
+   * @param timeout timeout in seconds
+   * @return True if timeout reached without handle being signaled
+   * @throws InterruptedException on failure (e.g. object was destroyed)
+   */
+  public static native boolean waitForObjectTimeout(int handle, double timeout)
+      throws InterruptedException;
+
+  /**
+   * Waits for one or more handles to be signaled.
+   *
+   * <p>Invalid handles are treated as signaled; the returned array will have the handle error bit
+   * set for any invalid handles.
+   *
+   * @param handles array of handles to wait on
+   * @return array of signaled handles
+   * @throws InterruptedException on failure (e.g. no objects were signaled)
+   */
+  public static native int[] waitForObjects(int[] handles) throws InterruptedException;
+
+  /**
+   * Waits for one or more handles to be signaled, with timeout.
+   *
+   * <p>Invalid handles are treated as signaled; the returned array will have the handle error bit
+   * set for any invalid handles.
+   *
+   * @param handles array of handles to wait on
+   * @param timeout timeout in seconds
+   * @return array of signaled handles; empty if timeout reached without any handle being signaled
+   * @throws InterruptedException on failure (e.g. no objects were signaled and no timeout)
+   */
+  public static native int[] waitForObjectsTimeout(int[] handles, double timeout)
+      throws InterruptedException;
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/concurrent/Event.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/concurrent/Event.java
new file mode 100644
index 0000000..bda2e88
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/concurrent/Event.java
@@ -0,0 +1,70 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.concurrent;
+
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * An atomic signaling event for synchronization.
+ *
+ * <p>Events have binary state (signaled or not signaled) and may be either automatically reset or
+ * manually reset. Automatic-reset events go to non-signaled state when a waitForObject is woken up
+ * by the event; manual-reset events require reset() to be called to set the event to non-signaled
+ * state; if reset() is not called, any waiter on that event will immediately wake when called.
+ */
+public final class Event implements AutoCloseable {
+  /**
+   * Constructor.
+   *
+   * @param manualReset true for manual reset, false for automatic reset
+   * @param initialState true to make the event initially in signaled state
+   */
+  public Event(boolean manualReset, boolean initialState) {
+    m_handle = WPIUtilJNI.createEvent(manualReset, initialState);
+  }
+
+  /**
+   * Constructor. Initial state is false.
+   *
+   * @param manualReset true for manual reset, false for automatic reset
+   */
+  public Event(boolean manualReset) {
+    this(manualReset, false);
+  }
+
+  /** Constructor. Automatic reset, initial state is false. */
+  public Event() {
+    this(false, false);
+  }
+
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      WPIUtilJNI.destroyEvent(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  /**
+   * Gets the event handle (e.g. for waitForObject).
+   *
+   * @return handle
+   */
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /** Sets the event to signaled state. */
+  public void set() {
+    WPIUtilJNI.setEvent(m_handle);
+  }
+
+  /** Sets the event to non-signaled state. */
+  public void reset() {
+    WPIUtilJNI.resetEvent(m_handle);
+  }
+
+  private int m_handle;
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/concurrent/Semaphore.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/concurrent/Semaphore.java
new file mode 100644
index 0000000..4e9e85f
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/concurrent/Semaphore.java
@@ -0,0 +1,80 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.concurrent;
+
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * A semaphore for synchronization.
+ *
+ * <p>Semaphores keep an internal counter. Releasing the semaphore increases the count. A semaphore
+ * with a non-zero count is considered signaled. When a waiter wakes up it atomically decrements the
+ * count by 1. This is generally useful in a single-supplier, multiple-consumer scenario.
+ */
+public final class Semaphore implements AutoCloseable {
+  /**
+   * Constructor.
+   *
+   * @param initialCount initial value for the semaphore's internal counter
+   * @param maximumCount maximum value for the samephore's internal counter
+   */
+  public Semaphore(int initialCount, int maximumCount) {
+    m_handle = WPIUtilJNI.createSemaphore(initialCount, maximumCount);
+  }
+
+  /**
+   * Constructor. Maximum count is Integer.MAX_VALUE.
+   *
+   * @param initialCount initial value for the semaphore's internal counter
+   */
+  public Semaphore(int initialCount) {
+    this(initialCount, Integer.MAX_VALUE);
+  }
+
+  /** Constructor. Initial count is 0, maximum count is Integer.MAX_VALUE. */
+  public Semaphore() {
+    this(0, Integer.MAX_VALUE);
+  }
+
+  @Override
+  public void close() {
+    if (m_handle != 0) {
+      WPIUtilJNI.destroySemaphore(m_handle);
+      m_handle = 0;
+    }
+  }
+
+  /**
+   * Gets the semaphore handle (e.g. for waitForObject).
+   *
+   * @return handle
+   */
+  public int getHandle() {
+    return m_handle;
+  }
+
+  /**
+   * Releases N counts of the semaphore.
+   *
+   * @param releaseCount amount to add to semaphore's internal counter; must be positive
+   * @return True on successful release, false on failure (e.g. release count would exceed maximum
+   *     value, or handle invalid)
+   */
+  public boolean release(int releaseCount) {
+    return WPIUtilJNI.releaseSemaphore(m_handle, releaseCount);
+  }
+
+  /**
+   * Releases 1 count of the semaphore.
+   *
+   * @return True on successful release, false on failure (e.g. release count would exceed maximum
+   *     value, or handle invalid)
+   */
+  public boolean release() {
+    return release(1);
+  }
+
+  private int m_handle;
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/BooleanConsumer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/BooleanConsumer.java
new file mode 100644
index 0000000..98fd6a2
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/BooleanConsumer.java
@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.function;
+
+/**
+ * Represents an operation that accepts a single boolean-valued argument and returns no result. This
+ * is the primitive type specialization of {@link java.util.function.Consumer} for boolean. Unlike
+ * most other functional interfaces, BooleanConsumer is expected to operate via side-effects.
+ *
+ * <p>This is a functional interface whose functional method is {@link #accept(boolean)}.
+ */
+@FunctionalInterface
+public interface BooleanConsumer {
+  /**
+   * Performs this operation on the given argument.
+   *
+   * @param value the input argument
+   */
+  void accept(boolean value);
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/FloatConsumer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/FloatConsumer.java
new file mode 100644
index 0000000..0fc7ff7
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/FloatConsumer.java
@@ -0,0 +1,22 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.function;
+
+/**
+ * Represents an operation that accepts a single float-valued argument and returns no result. This
+ * is the primitive type specialization of {@link java.util.function.Consumer} for float. Unlike
+ * most other functional interfaces, BooleanConsumer is expected to operate via side-effects.
+ *
+ * <p>This is a functional interface whose functional method is {@link #accept(float)}.
+ */
+@FunctionalInterface
+public interface FloatConsumer {
+  /**
+   * Performs this operation on the given argument.
+   *
+   * @param value the input argument
+   */
+  void accept(float value);
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/FloatSupplier.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/FloatSupplier.java
new file mode 100644
index 0000000..eb94b2b
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/function/FloatSupplier.java
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.function;
+
+/**
+ * Represents a supplier of float-valued results.
+ *
+ * <p>This is the float-producing primitive specialization of {@link java.util.function.Supplier}.
+ *
+ * <p>There is no requirement that a distinct result be returned each time the supplier is invoked.
+ *
+ * <p>This is a functional interface whose functional method is {@link #getAsFloat()}.
+ */
+@FunctionalInterface
+public interface FloatSupplier {
+  /**
+   * Gets a result.
+   *
+   * @return a result
+   */
+  float getAsFloat();
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/net/PortForwarder.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/net/PortForwarder.java
new file mode 100644
index 0000000..5c4c20a
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/net/PortForwarder.java
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.net;
+
+import edu.wpi.first.util.WPIUtilJNI;
+
+/**
+ * Forward ports to another host. This is primarily useful for accessing Ethernet-connected devices
+ * from a computer tethered to the RoboRIO USB port.
+ */
+public final class PortForwarder {
+  private PortForwarder() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Forward a local TCP port to a remote host and port. Note that local ports less than 1024 won't
+   * work as a normal user.
+   *
+   * @param port local port number
+   * @param remoteHost remote IP address / DNS name
+   * @param remotePort remote port number
+   */
+  public static void add(int port, String remoteHost, int remotePort) {
+    WPIUtilJNI.addPortForwarder(port, remoteHost, remotePort);
+  }
+
+  /**
+   * Stop TCP forwarding on a port.
+   *
+   * @param port local port number
+   */
+  public static void remove(int port) {
+    WPIUtilJNI.removePortForwarder(port);
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/Sendable.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/Sendable.java
new file mode 100644
index 0000000..ad1fe28
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/Sendable.java
@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.sendable;
+
+/** The base interface for objects that can be sent over the network. */
+public interface Sendable {
+  /**
+   * Initializes this {@link Sendable} object.
+   *
+   * @param builder sendable builder
+   */
+  void initSendable(SendableBuilder builder);
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java
new file mode 100644
index 0000000..35d98fa
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableBuilder.java
@@ -0,0 +1,127 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.sendable;
+
+import edu.wpi.first.util.function.BooleanConsumer;
+import java.util.function.BooleanSupplier;
+import java.util.function.Consumer;
+import java.util.function.DoubleConsumer;
+import java.util.function.DoubleSupplier;
+import java.util.function.Supplier;
+
+public interface SendableBuilder {
+  /** The backend kinds used for the sendable builder. */
+  enum BackendKind {
+    kUnknown,
+    kNetworkTables
+  }
+
+  /**
+   * Set the string representation of the named data type that will be used by the smart dashboard
+   * for this sendable.
+   *
+   * @param type data type
+   */
+  void setSmartDashboardType(String type);
+
+  /**
+   * Set a flag indicating if this sendable should be treated as an actuator. By default this flag
+   * is false.
+   *
+   * @param value true if actuator, false if not
+   */
+  void setActuator(boolean value);
+
+  /**
+   * Set the function that should be called to set the Sendable into a safe state. This is called
+   * when entering and exiting Live Window mode.
+   *
+   * @param func function
+   */
+  void setSafeState(Runnable func);
+
+  /**
+   * Add a boolean property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addBooleanProperty(String key, BooleanSupplier getter, BooleanConsumer setter);
+
+  /**
+   * Add a double property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addDoubleProperty(String key, DoubleSupplier getter, DoubleConsumer setter);
+
+  /**
+   * Add a string property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addStringProperty(String key, Supplier<String> getter, Consumer<String> setter);
+
+  /**
+   * Add a boolean array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addBooleanArrayProperty(String key, Supplier<boolean[]> getter, Consumer<boolean[]> setter);
+
+  /**
+   * Add a double array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addDoubleArrayProperty(String key, Supplier<double[]> getter, Consumer<double[]> setter);
+
+  /**
+   * Add a string array property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addStringArrayProperty(String key, Supplier<String[]> getter, Consumer<String[]> setter);
+
+  /**
+   * Add a raw property.
+   *
+   * @param key property name
+   * @param getter getter function (returns current value)
+   * @param setter setter function (sets new value)
+   */
+  void addRawProperty(String key, Supplier<byte[]> getter, Consumer<byte[]> setter);
+
+  /**
+   * Gets the kind of backend being used.
+   *
+   * @return Backend kind
+   */
+  BackendKind getBackendKind();
+
+  /**
+   * Return whether this sendable has been published.
+   *
+   * @return True if it has been published, false if not.
+   */
+  boolean isPublished();
+
+  /** Update the published values by calling the getters for all properties. */
+  void update();
+
+  /** Clear properties. */
+  void clearProperties();
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java
new file mode 100644
index 0000000..9d919af
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/util/sendable/SendableRegistry.java
@@ -0,0 +1,513 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util.sendable;
+
+import java.lang.ref.WeakReference;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+import java.util.Map;
+import java.util.WeakHashMap;
+import java.util.function.Consumer;
+import java.util.function.Supplier;
+
+/**
+ * The SendableRegistry class is the public interface for registering sensors and actuators for use
+ * on dashboards and LiveWindow.
+ */
+public class SendableRegistry {
+  private static class Component {
+    Component() {}
+
+    Component(Sendable sendable) {
+      m_sendable = new WeakReference<>(sendable);
+    }
+
+    WeakReference<Sendable> m_sendable;
+    SendableBuilder m_builder;
+    String m_name;
+    String m_subsystem = "Ungrouped";
+    WeakReference<Sendable> m_parent;
+    boolean m_liveWindow;
+    Object[] m_data;
+
+    void setName(String moduleType, int channel) {
+      m_name = moduleType + "[" + channel + "]";
+    }
+
+    void setName(String moduleType, int moduleNumber, int channel) {
+      m_name = moduleType + "[" + moduleNumber + "," + channel + "]";
+    }
+  }
+
+  private static Supplier<SendableBuilder> liveWindowFactory;
+  private static final Map<Object, Component> components = new WeakHashMap<>();
+  private static int nextDataHandle;
+
+  private static Component getOrAdd(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      comp = new Component(sendable);
+      components.put(sendable, comp);
+    } else {
+      if (comp.m_sendable == null) {
+        comp.m_sendable = new WeakReference<>(sendable);
+      }
+    }
+    return comp;
+  }
+
+  private SendableRegistry() {
+    throw new UnsupportedOperationException("This is a utility class!");
+  }
+
+  /**
+   * Sets the factory for LiveWindow builders.
+   *
+   * @param factory factory function
+   */
+  public static synchronized void setLiveWindowBuilderFactory(Supplier<SendableBuilder> factory) {
+    liveWindowFactory = factory;
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param name component name
+   */
+  public static synchronized void add(Sendable sendable, String name) {
+    Component comp = getOrAdd(sendable);
+    comp.m_name = name;
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @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
+   */
+  public static synchronized void add(Sendable sendable, String moduleType, int channel) {
+    Component comp = getOrAdd(sendable);
+    comp.setName(moduleType, channel);
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param moduleType A string that defines the module name in the label for the value
+   * @param moduleNumber The number of the particular module type
+   * @param channel The channel number the device is plugged into
+   */
+  public static synchronized void add(
+      Sendable sendable, String moduleType, int moduleNumber, int channel) {
+    Component comp = getOrAdd(sendable);
+    comp.setName(moduleType, moduleNumber, channel);
+  }
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param subsystem subsystem name
+   * @param name component name
+   */
+  public static synchronized void add(Sendable sendable, String subsystem, String name) {
+    Component comp = getOrAdd(sendable);
+    comp.m_name = name;
+    comp.m_subsystem = subsystem;
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param name component name
+   */
+  public static synchronized void addLW(Sendable sendable, String name) {
+    Component comp = getOrAdd(sendable);
+    if (liveWindowFactory != null) {
+      comp.m_builder = liveWindowFactory.get();
+    }
+    comp.m_liveWindow = true;
+    comp.m_name = name;
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @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
+   */
+  public static synchronized void addLW(Sendable sendable, String moduleType, int channel) {
+    Component comp = getOrAdd(sendable);
+    if (liveWindowFactory != null) {
+      comp.m_builder = liveWindowFactory.get();
+    }
+    comp.m_liveWindow = true;
+    comp.setName(moduleType, channel);
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param moduleType A string that defines the module name in the label for the value
+   * @param moduleNumber The number of the particular module type
+   * @param channel The channel number the device is plugged into
+   */
+  public static synchronized void addLW(
+      Sendable sendable, String moduleType, int moduleNumber, int channel) {
+    Component comp = getOrAdd(sendable);
+    if (liveWindowFactory != null) {
+      comp.m_builder = liveWindowFactory.get();
+    }
+    comp.m_liveWindow = true;
+    comp.setName(moduleType, moduleNumber, channel);
+  }
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param subsystem subsystem name
+   * @param name component name
+   */
+  public static synchronized void addLW(Sendable sendable, String subsystem, String name) {
+    Component comp = getOrAdd(sendable);
+    if (liveWindowFactory != null) {
+      comp.m_builder = liveWindowFactory.get();
+    }
+    comp.m_liveWindow = true;
+    comp.m_name = name;
+    comp.m_subsystem = subsystem;
+  }
+
+  /**
+   * Adds a child object to an object. Adds the child object to the registry if it's not already
+   * present.
+   *
+   * @param parent parent object
+   * @param child child object
+   */
+  public static synchronized void addChild(Sendable parent, Object child) {
+    Component comp = components.get(child);
+    if (comp == null) {
+      comp = new Component();
+      components.put(child, comp);
+    }
+    comp.m_parent = new WeakReference<>(parent);
+  }
+
+  /**
+   * Removes an object from the registry.
+   *
+   * @param sendable object to remove
+   * @return true if the object was removed; false if it was not present
+   */
+  public static synchronized boolean remove(Sendable sendable) {
+    return components.remove(sendable) != null;
+  }
+
+  /**
+   * Determines if an object is in the registry.
+   *
+   * @param sendable object to check
+   * @return True if in registry, false if not.
+   */
+  public static synchronized boolean contains(Sendable sendable) {
+    return components.containsKey(sendable);
+  }
+
+  /**
+   * Gets the name of an object.
+   *
+   * @param sendable object
+   * @return Name (empty if object is not in registry)
+   */
+  public static synchronized String getName(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      return "";
+    }
+    return comp.m_name;
+  }
+
+  /**
+   * Sets the name of an object.
+   *
+   * @param sendable object
+   * @param name name
+   */
+  public static synchronized void setName(Sendable sendable, String name) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_name = name;
+    }
+  }
+
+  /**
+   * Sets the name of an object with a channel number.
+   *
+   * @param sendable object
+   * @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
+   */
+  public static synchronized void setName(Sendable sendable, String moduleType, int channel) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.setName(moduleType, channel);
+    }
+  }
+
+  /**
+   * Sets the name of an object with a module and channel number.
+   *
+   * @param sendable object
+   * @param moduleType A string that defines the module name in the label for the value
+   * @param moduleNumber The number of the particular module type
+   * @param channel The channel number the device is plugged into
+   */
+  public static synchronized void setName(
+      Sendable sendable, String moduleType, int moduleNumber, int channel) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.setName(moduleType, moduleNumber, channel);
+    }
+  }
+
+  /**
+   * Sets both the subsystem name and device name of an object.
+   *
+   * @param sendable object
+   * @param subsystem subsystem name
+   * @param name device name
+   */
+  public static synchronized void setName(Sendable sendable, String subsystem, String name) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_name = name;
+      comp.m_subsystem = subsystem;
+    }
+  }
+
+  /**
+   * Gets the subsystem name of an object.
+   *
+   * @param sendable object
+   * @return Subsystem name (empty if object is not in registry)
+   */
+  public static synchronized String getSubsystem(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      return "";
+    }
+    return comp.m_subsystem;
+  }
+
+  /**
+   * Sets the subsystem name of an object.
+   *
+   * @param sendable object
+   * @param subsystem subsystem name
+   */
+  public static synchronized void setSubsystem(Sendable sendable, String subsystem) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_subsystem = subsystem;
+    }
+  }
+
+  /**
+   * Gets a unique handle for setting/getting data with setData() and getData().
+   *
+   * @return Handle
+   */
+  public static synchronized int getDataHandle() {
+    return nextDataHandle++;
+  }
+
+  /**
+   * Associates arbitrary data with an object in the registry.
+   *
+   * @param sendable object
+   * @param handle data handle returned by getDataHandle()
+   * @param data data to set
+   * @return Previous data (may be null)
+   */
+  public static synchronized Object setData(Sendable sendable, int handle, Object data) {
+    Component comp = components.get(sendable);
+    if (comp == null) {
+      return null;
+    }
+    Object rv = null;
+    if (comp.m_data == null) {
+      comp.m_data = new Object[handle + 1];
+    } else if (handle < comp.m_data.length) {
+      rv = comp.m_data[handle];
+    } else {
+      comp.m_data = Arrays.copyOf(comp.m_data, handle + 1);
+    }
+    comp.m_data[handle] = data;
+    return rv;
+  }
+
+  /**
+   * Gets arbitrary data associated with an object in the registry.
+   *
+   * @param sendable object
+   * @param handle data handle returned by getDataHandle()
+   * @return data (may be null if none associated)
+   */
+  public static synchronized Object getData(Sendable sendable, int handle) {
+    Component comp = components.get(sendable);
+    if (comp == null || comp.m_data == null || handle >= comp.m_data.length) {
+      return null;
+    }
+    return comp.m_data[handle];
+  }
+
+  /**
+   * Enables LiveWindow for an object.
+   *
+   * @param sendable object
+   */
+  public static synchronized void enableLiveWindow(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_liveWindow = true;
+    }
+  }
+
+  /**
+   * Disables LiveWindow for an object.
+   *
+   * @param sendable object
+   */
+  public static synchronized void disableLiveWindow(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp != null) {
+      comp.m_liveWindow = false;
+    }
+  }
+
+  /**
+   * Publishes an object in the registry to a builder.
+   *
+   * @param sendable object
+   * @param builder sendable builder
+   */
+  public static synchronized void publish(Sendable sendable, SendableBuilder builder) {
+    Component comp = getOrAdd(sendable);
+    if (comp.m_builder != null) {
+      comp.m_builder.clearProperties();
+    }
+    comp.m_builder = builder; // clear any current builder
+    sendable.initSendable(comp.m_builder);
+    comp.m_builder.update();
+  }
+
+  /**
+   * Updates network table information from an object.
+   *
+   * @param sendable object
+   */
+  public static synchronized void update(Sendable sendable) {
+    Component comp = components.get(sendable);
+    if (comp != null && comp.m_builder != null) {
+      comp.m_builder.update();
+    }
+  }
+
+  /** Data passed to foreachLiveWindow() callback function. */
+  public static class CallbackData {
+    /** Sendable object. */
+    @SuppressWarnings("MemberName")
+    public Sendable sendable;
+
+    /** Name. */
+    @SuppressWarnings("MemberName")
+    public String name;
+
+    /** Subsystem. */
+    @SuppressWarnings("MemberName")
+    public String subsystem;
+
+    /** Parent sendable object. */
+    @SuppressWarnings("MemberName")
+    public Sendable parent;
+
+    /** Data stored in object with setData(). Update this to change the data. */
+    @SuppressWarnings("MemberName")
+    public Object data;
+
+    /** Sendable builder for the sendable. */
+    @SuppressWarnings("MemberName")
+    public SendableBuilder builder;
+  }
+
+  // As foreachLiveWindow is single threaded, cache the components it
+  // iterates over to avoid risk of ConcurrentModificationException
+  private static List<Component> foreachComponents = new ArrayList<>();
+
+  /**
+   * Iterates over LiveWindow-enabled objects in the registry. It is *not* safe to call other
+   * SendableRegistry functions from the callback.
+   *
+   * @param dataHandle data handle to get data object passed to callback
+   * @param callback function to call for each object
+   */
+  @SuppressWarnings("PMD.AvoidCatchingThrowable")
+  public static synchronized void foreachLiveWindow(
+      int dataHandle, Consumer<CallbackData> callback) {
+    CallbackData cbdata = new CallbackData();
+    foreachComponents.clear();
+    foreachComponents.addAll(components.values());
+    for (Component comp : foreachComponents) {
+      if (comp.m_builder == null || comp.m_sendable == null) {
+        continue;
+      }
+      cbdata.sendable = comp.m_sendable.get();
+      if (cbdata.sendable != null && comp.m_liveWindow) {
+        cbdata.name = comp.m_name;
+        cbdata.subsystem = comp.m_subsystem;
+        if (comp.m_parent != null) {
+          cbdata.parent = comp.m_parent.get();
+        } else {
+          cbdata.parent = null;
+        }
+        if (comp.m_data != null && dataHandle < comp.m_data.length) {
+          cbdata.data = comp.m_data[dataHandle];
+        } else {
+          cbdata.data = null;
+        }
+        cbdata.builder = comp.m_builder;
+        try {
+          callback.accept(cbdata);
+        } catch (Throwable throwable) {
+          Throwable cause = throwable.getCause();
+          if (cause != null) {
+            throwable = cause;
+          }
+          System.err.println("Unhandled exception calling LiveWindow for " + comp.m_name + ": ");
+          throwable.printStackTrace();
+          comp.m_liveWindow = false;
+        }
+        if (cbdata.data != null) {
+          if (comp.m_data == null) {
+            comp.m_data = new Object[dataHandle + 1];
+          } else if (dataHandle >= comp.m_data.length) {
+            comp.m_data = Arrays.copyOf(comp.m_data, dataHandle + 1);
+          }
+          comp.m_data[dataHandle] = cbdata.data;
+        }
+      }
+    }
+    foreachComponents.clear();
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/CircularBuffer.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/CircularBuffer.java
deleted file mode 100644
index 5325b26..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/CircularBuffer.java
+++ /dev/null
@@ -1,186 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-/**
- * This is a simple circular buffer so we don't need to "bucket brigade" copy old values.
- */
-public class CircularBuffer {
-  private double[] m_data;
-
-  // Index of element at front of buffer
-  private int m_front;
-
-  // Number of elements used in buffer
-  private int m_length;
-
-  /**
-   * Create a CircularBuffer with the provided size.
-   *
-   * @param size The size of the circular buffer.
-   */
-  public CircularBuffer(int size) {
-    m_data = new double[size];
-    for (int i = 0; i < m_data.length; i++) {
-      m_data[i] = 0.0;
-    }
-  }
-
-  /**
-   * Returns number of elements in buffer.
-   *
-   * @return number of elements in buffer
-   */
-  double size() {
-    return m_length;
-  }
-
-  /**
-   * Get value at front of buffer.
-   *
-   * @return value at front of buffer
-   */
-  double getFirst() {
-    return m_data[m_front];
-  }
-
-  /**
-   * Get value at back of buffer.
-   *
-   * @return value at back of buffer
-   */
-  double getLast() {
-    // If there are no elements in the buffer, do nothing
-    if (m_length == 0) {
-      return 0.0;
-    }
-
-    return m_data[(m_front + m_length - 1) % m_data.length];
-  }
-
-  /**
-   * Push new value onto front of the buffer. The value at the back is overwritten if the buffer is
-   * full.
-   */
-  public void addFirst(double value) {
-    if (m_data.length == 0) {
-      return;
-    }
-
-    m_front = moduloDec(m_front);
-
-    m_data[m_front] = value;
-
-    if (m_length < m_data.length) {
-      m_length++;
-    }
-  }
-
-  /**
-   * Push new value onto back of the buffer. The value at the front is overwritten if the buffer is
-   * full.
-   */
-  public void addLast(double value) {
-    if (m_data.length == 0) {
-      return;
-    }
-
-    m_data[(m_front + m_length) % m_data.length] = value;
-
-    if (m_length < m_data.length) {
-      m_length++;
-    } else {
-      // Increment front if buffer is full to maintain size
-      m_front = moduloInc(m_front);
-    }
-  }
-
-  /**
-   * Pop value at front of buffer.
-   *
-   * @return value at front of buffer
-   */
-  public double removeFirst() {
-    // If there are no elements in the buffer, do nothing
-    if (m_length == 0) {
-      return 0.0;
-    }
-
-    double temp = m_data[m_front];
-    m_front = moduloInc(m_front);
-    m_length--;
-    return temp;
-  }
-
-
-  /**
-   * Pop value at back of buffer.
-   */
-  public double removeLast() {
-    // If there are no elements in the buffer, do nothing
-    if (m_length == 0) {
-      return 0.0;
-    }
-
-    m_length--;
-    return m_data[(m_front + m_length) % m_data.length];
-  }
-
-  /**
-   * Resizes internal buffer to given size.
-   *
-   * <p>A new buffer is allocated because arrays are not resizable.
-   */
-  void resize(int size) {
-    double[] newBuffer = new double[size];
-    m_length = Math.min(m_length, size);
-    for (int i = 0; i < m_length; i++) {
-      newBuffer[i] = m_data[(m_front + i) % m_data.length];
-    }
-    m_data = newBuffer;
-    m_front = 0;
-  }
-
-  /**
-   * Sets internal buffer contents to zero.
-   */
-  public void clear() {
-    for (int i = 0; i < m_data.length; i++) {
-      m_data[i] = 0.0;
-    }
-    m_front = 0;
-    m_length = 0;
-  }
-
-  /**
-   * Get the element at the provided index relative to the start of the buffer.
-   *
-   * @return Element at index starting from front of buffer.
-   */
-  public double get(int index) {
-    return m_data[(m_front + index) % m_data.length];
-  }
-
-  /**
-   * Increment an index modulo the length of the m_data buffer.
-   */
-  private int moduloInc(int index) {
-    return (index + 1) % m_data.length;
-  }
-
-  /**
-   * Decrement an index modulo the length of the m_data buffer.
-   */
-  private int moduloDec(int index) {
-    if (index == 0) {
-      return m_data.length - 1;
-    } else {
-      return index - 1;
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java
deleted file mode 100644
index 8f0d53e..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/CombinedRuntimeLoader.java
+++ /dev/null
@@ -1,184 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import java.io.File;
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Paths;
-import java.util.ArrayList;
-import java.util.HashMap;
-import java.util.List;
-import java.util.Map;
-import java.util.Objects;
-
-import com.fasterxml.jackson.core.type.TypeReference;
-import com.fasterxml.jackson.databind.ObjectMapper;
-
-public final class CombinedRuntimeLoader {
-  private CombinedRuntimeLoader() {
-  }
-
-  private static String extractionDirectory;
-
-  public static synchronized String getExtractionDirectory() {
-    return extractionDirectory;
-  }
-
-  private static synchronized void setExtractionDirectory(String directory) {
-    extractionDirectory = directory;
-  }
-
-  public static native String setDllDirectory(String directory);
-
-  private static String getLoadErrorMessage(String libraryName, UnsatisfiedLinkError ule) {
-    StringBuilder msg = new StringBuilder(512);
-    msg.append(libraryName).append(" could not be loaded from path\n"
-        + "\tattempted to load for platform ")
-        .append(RuntimeDetector.getPlatformPath())
-        .append("\nLast Load Error: \n")
-        .append(ule.getMessage())
-        .append('\n');
-    if (RuntimeDetector.isWindows()) {
-      msg.append("A common cause of this error is missing the C++ runtime.\n"
-          + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n");
-    }
-    return msg.toString();
-  }
-
-  /**
-   * Extract a list of native libraries.
-   * @param <T>             The class where the resources would be located
-   * @param clazz           The actual class object
-   * @param resourceName    The resource name on the classpath to use for file lookup
-   * @return                List of all libraries that were extracted
-   * @throws IOException    Thrown if resource not found or file could not be extracted
-   */
-  @SuppressWarnings({"PMD.AvoidInstantiatingObjectsInLoops", "PMD.UnnecessaryCastRule"})
-  public static <T> List<String> extractLibraries(Class<T> clazz, String resourceName)
-      throws IOException {
-    TypeReference<HashMap<String, Object>> typeRef = new TypeReference<HashMap<String, Object>>() {
-    };
-    ObjectMapper mapper = new ObjectMapper();
-    Map<String, Object> map;
-    try (var stream = clazz.getResourceAsStream(resourceName)) {
-      map = mapper.readValue(stream, typeRef);
-    }
-
-    var platformPath = Paths.get(RuntimeDetector.getPlatformPath());
-    var platform = platformPath.getName(0).toString();
-    var arch = platformPath.getName(1).toString();
-
-    var platformMap = (Map<String, List<String>>) map.get(platform);
-
-    var fileList = platformMap.get(arch);
-
-    var extractionPathString = getExtractionDirectory();
-
-    if (extractionPathString == null) {
-      String hash = (String) map.get("hash");
-
-      var defaultExtractionRoot = RuntimeLoader.getDefaultExtractionRoot();
-      var extractionPath = Paths.get(defaultExtractionRoot, platform, arch, hash);
-      extractionPathString = extractionPath.toString();
-
-      setExtractionDirectory(extractionPathString);
-    }
-
-    List<String> extractedFiles = new ArrayList<>();
-
-    byte[] buffer = new byte[0x10000]; // 64K copy buffer
-
-    for (var file : fileList) {
-      try (var stream = clazz.getResourceAsStream(file)) {
-        Objects.requireNonNull(stream);
-
-        var outputFile = Paths.get(extractionPathString, new File(file).getName());
-        extractedFiles.add(outputFile.toString());
-        if (outputFile.toFile().exists()) {
-          continue;
-        }
-        outputFile.getParent().toFile().mkdirs();
-
-        try (var os = Files.newOutputStream(outputFile)) {
-          int readBytes;
-          while ((readBytes = stream.read(buffer)) != -1) { // NOPMD
-            os.write(buffer, 0, readBytes);
-          }
-        }
-      }
-    }
-
-    return extractedFiles;
-  }
-
-  /**
-   * Load a single library from a list of extracted files.
-   * @param libraryName The library name to load
-   * @param extractedFiles The extracted files to search
-   * @throws IOException If library was not found
-   */
-  public static void loadLibrary(String libraryName, List<String> extractedFiles)
-      throws IOException {
-    String currentPath = null;
-    String oldDllDirectory = null;
-    try {
-      if (RuntimeDetector.isWindows()) {
-        var extractionPathString = getExtractionDirectory();
-        oldDllDirectory = setDllDirectory(extractionPathString);
-      }
-      for (var extractedFile : extractedFiles) {
-        if (extractedFile.contains(libraryName)) {
-          // Load it
-          currentPath = extractedFile;
-          System.load(extractedFile);
-          return;
-        }
-      }
-      throw new IOException("Could not find library " + libraryName);
-    } catch (UnsatisfiedLinkError ule) {
-      throw new IOException(getLoadErrorMessage(currentPath, ule));
-    } finally {
-      if (oldDllDirectory != null) {
-        setDllDirectory(oldDllDirectory);
-      }
-    }
-  }
-
-  /**
-   * Load a list of native libraries out of a single directory.
-   *
-   * @param <T>             The class where the resources would be located
-   * @param clazz           The actual class object
-   * @param librariesToLoad List of libraries to load
-   * @throws IOException Throws an IOException if not found
-   */
-  public static <T> void loadLibraries(Class<T> clazz, String... librariesToLoad)
-      throws IOException {
-    // Extract everything
-
-    var extractedFiles = extractLibraries(clazz, "/ResourceInformation.json");
-
-    String currentPath = "";
-
-    try {
-      if (RuntimeDetector.isWindows()) {
-        var extractionPathString = getExtractionDirectory();
-        // Load windows, set dll directory
-        currentPath = Paths.get(extractionPathString, "WindowsLoaderHelper.dll").toString();
-        System.load(currentPath);
-      }
-    } catch (UnsatisfiedLinkError ule) {
-      throw new IOException(getLoadErrorMessage(currentPath, ule));
-    }
-
-    for (var library : librariesToLoad) {
-      loadLibrary(library, extractedFiles);
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java
deleted file mode 100644
index 70387eb..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/ErrorMessages.java
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import static java.util.Objects.requireNonNull;
-
-/**
- * Utility class for common WPILib error messages.
- */
-public final class ErrorMessages {
-  /**
-   * Utility class, so constructor is private.
-   */
-  private ErrorMessages() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-
-  /**
-   * Requires that a parameter of a method not be null; prints an error message with
-   * helpful debugging instructions if the parameter is null.
-   *
-   * @param obj The parameter that must not be null.
-   * @param paramName The name of the parameter.
-   * @param methodName The name of the method.
-   */
-  public static <T> T requireNonNullParam(T obj, String paramName, String methodName) {
-    return requireNonNull(obj,
-        "Parameter " + paramName + " in method " + methodName + " was null when it"
-            + " should not have been!  Check the stacktrace to find the responsible line of code - "
-            + "usually, it is the first line of user-written code indicated in the stacktrace.  "
-            + "Make sure all objects passed to the method in question were properly initialized -"
-            + " note that this may not be obvious if it is being called under "
-            + "dynamically-changing conditions!  Please do not seek additional technical assistance"
-            + " without doing this first!");
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java
deleted file mode 100644
index b4c07b7..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeDetector.java
+++ /dev/null
@@ -1,164 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import java.io.BufferedReader;
-import java.io.File;
-import java.io.IOException;
-import java.nio.file.Files;
-import java.nio.file.Paths;
-
-public final class RuntimeDetector {
-  private static String filePrefix;
-  private static String fileExtension;
-  private static String filePath;
-
-  @SuppressWarnings("PMD.CyclomaticComplexity")
-  private static synchronized void computePlatform() {
-    if (fileExtension != null && filePath != null && filePrefix != null) {
-      return;
-    }
-
-
-    boolean intel32 = is32BitIntel();
-    boolean intel64 = is64BitIntel();
-
-    if (isWindows()) {
-      filePrefix = "";
-      fileExtension = ".dll";
-      if (intel32) {
-        filePath = "/windows/x86/";
-      } else {
-        filePath = "/windows/x86-64/";
-      }
-    } else if (isMac()) {
-      filePrefix = "lib";
-      fileExtension = ".dylib";
-      if (intel32) {
-        filePath = "/osx/x86";
-      } else {
-        filePath = "/osx/x86-64/";
-      }
-    } else if (isLinux()) {
-      filePrefix = "lib";
-      fileExtension = ".so";
-      if (intel32) {
-        filePath = "/linux/x86/";
-      } else if (intel64) {
-        filePath = "/linux/x86-64/";
-      } else if (isAthena()) {
-        filePath = "/linux/athena/";
-      } else if (isRaspbian()) {
-        filePath = "/linux/raspbian/";
-      } else if (isAarch64()) {
-        filePath = "/linux/aarch64bionic/";
-      } else {
-        filePath = "/linux/nativearm/";
-      }
-    } else {
-      throw new IllegalStateException("Failed to determine OS");
-    }
-  }
-
-  /**
-   * Get the file prefix for the current system.
-   */
-  public static synchronized String getFilePrefix() {
-    computePlatform();
-
-    return filePrefix;
-  }
-
-  /**
-   * Get the file extension for the current system.
-   */
-  public static synchronized String getFileExtension() {
-    computePlatform();
-
-    return fileExtension;
-  }
-
-  /**
-   * Get the platform path for the current system.
-   */
-  public static synchronized String getPlatformPath() {
-    computePlatform();
-
-    return filePath;
-  }
-
-  /**
-   * Get the path to the requested resource.
-   */
-  public static synchronized String getLibraryResource(String libName) {
-    computePlatform();
-
-    return filePath + filePrefix + libName + fileExtension;
-  }
-
-  /**
-   * Get the path to the hash to the requested resource.
-   */
-  public static synchronized String getHashLibraryResource(String libName) {
-    computePlatform();
-
-    return filePath + libName + ".hash";
-  }
-
-  public static boolean isAthena() {
-    File runRobotFile = new File("/usr/local/frc/bin/frcRunRobot.sh");
-    return runRobotFile.exists();
-  }
-
-  /** check if os is raspbian.
-   *
-   * @return if os is raspbian
-   */
-  public static boolean isRaspbian() {
-    try (BufferedReader reader = Files.newBufferedReader(Paths.get("/etc/os-release"))) {
-      String value = reader.readLine();
-      return value.contains("Raspbian");
-    } catch (IOException ex) {
-      return false;
-    }
-  }
-
-  /** check if architecture is aarch64.
-   *
-   * @return if architecture is aarch64
-   */
-  public static boolean isAarch64() {
-    return System.getProperty("os.arch").equals("aarch64");
-  }
-
-  public static boolean isLinux() {
-    return System.getProperty("os.name").startsWith("Linux");
-  }
-
-  public static boolean isWindows() {
-    return System.getProperty("os.name").startsWith("Windows");
-  }
-
-  public static boolean isMac() {
-    return System.getProperty("os.name").startsWith("Mac");
-  }
-
-  public static boolean is32BitIntel() {
-    String arch = System.getProperty("os.arch");
-    return "x86".equals(arch) || "i386".equals(arch);
-  }
-
-  public static boolean is64BitIntel() {
-    String arch = System.getProperty("os.arch");
-    return "amd64".equals(arch) || "x86_64".equals(arch);
-  }
-
-  private RuntimeDetector() {
-
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java
deleted file mode 100644
index 21193a2..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/RuntimeLoader.java
+++ /dev/null
@@ -1,176 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import java.io.File;
-import java.io.IOException;
-import java.io.InputStream;
-import java.io.OutputStream;
-import java.nio.charset.StandardCharsets;
-import java.nio.file.Files;
-import java.nio.file.Paths;
-import java.security.DigestInputStream;
-import java.security.MessageDigest;
-import java.security.NoSuchAlgorithmException;
-import java.util.Locale;
-import java.util.Scanner;
-
-public final class RuntimeLoader<T> {
-  private static String defaultExtractionRoot;
-
-  /**
-   * Gets the default extration root location (~/.wpilib/nativecache).
-   */
-  public static synchronized String getDefaultExtractionRoot() {
-    if (defaultExtractionRoot != null) {
-      return defaultExtractionRoot;
-    }
-    String home = System.getProperty("user.home");
-    defaultExtractionRoot = Paths.get(home, ".wpilib", "nativecache").toString();
-    return defaultExtractionRoot;
-  }
-
-  private final String m_libraryName;
-  private final Class<T> m_loadClass;
-  private final String m_extractionRoot;
-
-  /**
-   * Creates a new library loader.
-   *
-   * <p>Resources loaded on disk from extractionRoot, and from classpath from the
-   * passed in class. Library name is the passed in name.
-   */
-  public RuntimeLoader(String libraryName, String extractionRoot, Class<T> cls) {
-    m_libraryName = libraryName;
-    m_loadClass = cls;
-    m_extractionRoot = extractionRoot;
-  }
-
-  private String getLoadErrorMessage(UnsatisfiedLinkError ule) {
-    StringBuilder msg = new StringBuilder(512);
-    msg.append(m_libraryName)
-       .append(" could not be loaded from path or an embedded resource.\n"
-               + "\tattempted to load for platform ")
-       .append(RuntimeDetector.getPlatformPath())
-       .append("\nLast Load Error: \n")
-       .append(ule.getMessage())
-       .append('\n');
-    if (RuntimeDetector.isWindows()) {
-      msg.append("A common cause of this error is missing the C++ runtime.\n"
-                 + "Download the latest at https://support.microsoft.com/en-us/help/2977003/the-latest-supported-visual-c-downloads\n");
-    }
-    return msg.toString();
-  }
-
-  /**
-   * Loads a native library.
-   */
-  @SuppressWarnings("PMD.PreserveStackTrace")
-  public void loadLibrary() throws IOException {
-    try {
-      // First, try loading path
-      System.loadLibrary(m_libraryName);
-    } catch (UnsatisfiedLinkError ule) {
-      // Then load the hash from the resources
-      String hashName = RuntimeDetector.getHashLibraryResource(m_libraryName);
-      String resname = RuntimeDetector.getLibraryResource(m_libraryName);
-      try (InputStream hashIs = m_loadClass.getResourceAsStream(hashName)) {
-        if (hashIs == null) {
-          throw new IOException(getLoadErrorMessage(ule));
-        }
-        try (Scanner scanner = new Scanner(hashIs, StandardCharsets.UTF_8.name())) {
-          String hash = scanner.nextLine();
-          File jniLibrary = new File(m_extractionRoot, resname + "." + hash);
-          try {
-            // Try to load from an already extracted hash
-            System.load(jniLibrary.getAbsolutePath());
-          } catch (UnsatisfiedLinkError ule2) {
-            // If extraction failed, extract
-            try (InputStream resIs = m_loadClass.getResourceAsStream(resname)) {
-              if (resIs == null) {
-                throw new IOException(getLoadErrorMessage(ule));
-              }
-              jniLibrary.getParentFile().mkdirs();
-              try (OutputStream os = Files.newOutputStream(jniLibrary.toPath())) {
-                byte[] buffer = new byte[0xFFFF]; // 64K copy buffer
-                int readBytes;
-                while ((readBytes = resIs.read(buffer)) != -1) { // NOPMD
-                  os.write(buffer, 0, readBytes);
-                }
-              }
-              System.load(jniLibrary.getAbsolutePath());
-            }
-          }
-        }
-      }
-    }
-  }
-
-  /**
-   * Load a native library by directly hashing the file.
-   */
-  @SuppressWarnings({"PMD.NPathComplexity", "PMD.PreserveStackTrace", "PMD.EmptyWhileStmt",
-                     "PMD.AvoidThrowingRawExceptionTypes", "PMD.CyclomaticComplexity"})
-  public void loadLibraryHashed() throws IOException {
-    try {
-      // First, try loading path
-      System.loadLibrary(m_libraryName);
-    } catch (UnsatisfiedLinkError ule) {
-      // Then load the hash from the input file
-      String resname = RuntimeDetector.getLibraryResource(m_libraryName);
-      String hash = null;
-      try (InputStream is = m_loadClass.getResourceAsStream(resname)) {
-        if (is == null) {
-          throw new IOException(getLoadErrorMessage(ule));
-        }
-        MessageDigest md = null;
-        try {
-          md = MessageDigest.getInstance("MD5");
-        } catch (NoSuchAlgorithmException nsae) {
-          throw new RuntimeException("Weird Hash Algorithm?");
-        }
-        try (DigestInputStream dis = new DigestInputStream(is, md)) {
-          // Read the entire buffer once to hash
-          byte[] buffer = new byte[0xFFFF];
-          while (dis.read(buffer) > -1) {}
-          MessageDigest digest = dis.getMessageDigest();
-          byte[] digestOutput = digest.digest();
-          StringBuilder builder = new StringBuilder();
-          for (byte b : digestOutput) {
-            builder.append(String.format("%02X", b));
-          }
-          hash = builder.toString().toLowerCase(Locale.ENGLISH);
-        }
-      }
-      if (hash == null) {
-        throw new IOException("Weird Hash?");
-      }
-      File jniLibrary = new File(m_extractionRoot, resname + "." + hash);
-      try {
-        // Try to load from an already extracted hash
-        System.load(jniLibrary.getAbsolutePath());
-      } catch (UnsatisfiedLinkError ule2) {
-        // If extraction failed, extract
-        try (InputStream resIs = m_loadClass.getResourceAsStream(resname)) {
-          if (resIs == null) {
-            throw new IOException(getLoadErrorMessage(ule));
-          }
-          jniLibrary.getParentFile().mkdirs();
-          try (OutputStream os = Files.newOutputStream(jniLibrary.toPath())) {
-            byte[] buffer = new byte[0xFFFF]; // 64K copy buffer
-            int readBytes;
-            while ((readBytes = resIs.read(buffer)) != -1) { // NOPMD
-              os.write(buffer, 0, readBytes);
-            }
-          }
-          System.load(jniLibrary.getAbsolutePath());
-        }
-      }
-    }
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java
deleted file mode 100644
index 3a22c7b..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/WPIUtilJNI.java
+++ /dev/null
@@ -1,58 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import java.io.IOException;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-public final class WPIUtilJNI {
-  static boolean libraryLoaded = false;
-  static RuntimeLoader<WPIUtilJNI> loader = null;
-
-  public static class Helper {
-    private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
-
-    public static boolean getExtractOnStaticLoad() {
-      return extractOnStaticLoad.get();
-    }
-
-    public static void setExtractOnStaticLoad(boolean load) {
-      extractOnStaticLoad.set(load);
-    }
-  }
-
-  static {
-    if (Helper.getExtractOnStaticLoad()) {
-      try {
-        loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIUtilJNI.class);
-        loader.loadLibrary();
-      } catch (IOException ex) {
-        ex.printStackTrace();
-        System.exit(1);
-      }
-      libraryLoaded = true;
-    }
-  }
-
-  /**
-   * Force load the library.
-   */
-  public static synchronized void forceLoad() throws IOException {
-    if (libraryLoaded) {
-      return;
-    }
-    loader = new RuntimeLoader<>("wpiutiljni", RuntimeLoader.getDefaultExtractionRoot(), WPIUtilJNI.class);
-    loader.loadLibrary();
-    libraryLoaded = true;
-  }
-
-  public static native long now();
-
-  public static native void addPortForwarder(int port, String remoteHost, int remotePort);
-  public static native void removePortForwarder(int port);
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/net/PortForwarder.java b/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/net/PortForwarder.java
deleted file mode 100644
index 4fbdbf5..0000000
--- a/third_party/allwpilib/wpiutil/src/main/java/edu/wpi/first/wpiutil/net/PortForwarder.java
+++ /dev/null
@@ -1,41 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil.net;
-
-import edu.wpi.first.wpiutil.WPIUtilJNI;
-
-/**
- * Forward ports to another host. This is primarily useful for accessing
- * Ethernet-connected devices from a computer tethered to the RoboRIO USB port.
- */
-public final class PortForwarder {
-  private PortForwarder() {
-    throw new UnsupportedOperationException("This is a utility class!");
-  }
-
-  /**
-   * Forward a local TCP port to a remote host and port.
-   * Note that local ports less than 1024 won't work as a normal user.
-   *
-   * @param port       local port number
-   * @param remoteHost remote IP address / DNS name
-   * @param remotePort remote port number
-   */
-  public static void add(int port, String remoteHost, int remotePort) {
-    WPIUtilJNI.addPortForwarder(port, remoteHost, remotePort);
-  }
-
-  /**
-   * Stop TCP forwarding on a port.
-   *
-   * @param port local port number
-   */
-  public static void remove(int port) {
-    WPIUtilJNI.removePortForwarder(port);
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/Base64.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/Base64.cpp
index 35ac76c..8f1f810 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/Base64.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/Base64.cpp
@@ -80,13 +80,19 @@
     64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64, 64,
     64, 64, 64, 64, 64, 64, 64, 64, 64};
 
-size_t Base64Decode(raw_ostream& os, StringRef encoded) {
-  const unsigned char* end = encoded.bytes_begin();
-  while (pr2six[*end] <= 63 && end != encoded.bytes_end()) ++end;
-  size_t nprbytes = end - encoded.bytes_begin();
-  if (nprbytes == 0) return 0;
+size_t Base64Decode(raw_ostream& os, std::string_view encoded) {
+  auto bytes_begin = reinterpret_cast<const unsigned char*>(encoded.data());
+  auto bytes_end = bytes_begin + encoded.size();
+  const unsigned char* end = bytes_begin;
+  while (pr2six[*end] <= 63 && end != bytes_end) {
+    ++end;
+  }
+  size_t nprbytes = end - bytes_begin;
+  if (nprbytes == 0) {
+    return 0;
+  }
 
-  const unsigned char* cur = encoded.bytes_begin();
+  const unsigned char* cur = bytes_begin;
 
   while (nprbytes > 4) {
     os << static_cast<unsigned char>(pr2six[cur[0]] << 2 | pr2six[cur[1]] >> 4);
@@ -97,17 +103,20 @@
   }
 
   // Note: (nprbytes == 1) would be an error, so just ignore that case
-  if (nprbytes > 1)
+  if (nprbytes > 1) {
     os << static_cast<unsigned char>(pr2six[cur[0]] << 2 | pr2six[cur[1]] >> 4);
-  if (nprbytes > 2)
+  }
+  if (nprbytes > 2) {
     os << static_cast<unsigned char>(pr2six[cur[1]] << 4 | pr2six[cur[2]] >> 2);
-  if (nprbytes > 3)
+  }
+  if (nprbytes > 3) {
     os << static_cast<unsigned char>(pr2six[cur[2]] << 6 | pr2six[cur[3]]);
+  }
 
-  return (end - encoded.bytes_begin()) + ((4 - nprbytes) & 3);
+  return (end - bytes_begin) + ((4 - nprbytes) & 3);
 }
 
-size_t Base64Decode(StringRef encoded, std::string* plain) {
+size_t Base64Decode(std::string_view encoded, std::string* plain) {
   plain->resize(0);
   raw_string_ostream os(*plain);
   size_t rv = Base64Decode(os, encoded);
@@ -115,19 +124,35 @@
   return rv;
 }
 
-StringRef Base64Decode(StringRef encoded, size_t* num_read,
-                       SmallVectorImpl<char>& buf) {
+std::string_view Base64Decode(std::string_view encoded, size_t* num_read,
+                              SmallVectorImpl<char>& buf) {
   buf.clear();
   raw_svector_ostream os(buf);
   *num_read = Base64Decode(os, encoded);
   return os.str();
 }
 
+size_t Base64Decode(std::string_view encoded, std::vector<uint8_t>* plain) {
+  plain->resize(0);
+  raw_uvector_ostream os(*plain);
+  return Base64Decode(os, encoded);
+}
+
+span<uint8_t> Base64Decode(std::string_view encoded, size_t* num_read,
+                           SmallVectorImpl<uint8_t>& buf) {
+  buf.clear();
+  raw_usvector_ostream os(buf);
+  *num_read = Base64Decode(os, encoded);
+  return os.array();
+}
+
 static const char basis_64[] =
     "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
 
-void Base64Encode(raw_ostream& os, StringRef plain) {
-  if (plain.empty()) return;
+void Base64Encode(raw_ostream& os, std::string_view plain) {
+  if (plain.empty()) {
+    return;
+  }
   size_t len = plain.size();
 
   size_t i;
@@ -153,14 +178,35 @@
   }
 }
 
-void Base64Encode(StringRef plain, std::string* encoded) {
+void Base64Encode(std::string_view plain, std::string* encoded) {
   encoded->resize(0);
   raw_string_ostream os(*encoded);
   Base64Encode(os, plain);
   os.flush();
 }
 
-StringRef Base64Encode(StringRef plain, SmallVectorImpl<char>& buf) {
+std::string_view Base64Encode(std::string_view plain,
+                              SmallVectorImpl<char>& buf) {
+  buf.clear();
+  raw_svector_ostream os(buf);
+  Base64Encode(os, plain);
+  return os.str();
+}
+
+void Base64Encode(raw_ostream& os, span<const uint8_t> plain) {
+  Base64Encode(os, std::string_view{reinterpret_cast<const char*>(plain.data()),
+                                    plain.size()});
+}
+
+void Base64Encode(span<const uint8_t> plain, std::string* encoded) {
+  encoded->resize(0);
+  raw_string_ostream os(*encoded);
+  Base64Encode(os, plain);
+  os.flush();
+}
+
+std::string_view Base64Encode(span<const uint8_t> plain,
+                              SmallVectorImpl<char>& buf) {
   buf.clear();
   raw_svector_ostream os(buf);
   Base64Encode(os, plain);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/DsClient.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/DsClient.cpp
new file mode 100644
index 0000000..455f10c
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/DsClient.cpp
@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/DsClient.h"
+
+#include <fmt/format.h>
+#include <wpi/StringExtras.h>
+#include <wpi/json.h>
+#include <wpi/uv/Tcp.h>
+#include <wpi/uv/Timer.h>
+
+#include "wpi/Logger.h"
+
+using namespace wpi;
+
+static constexpr uv::Timer::Time kReconnectTime{500};
+
+DsClient::DsClient(wpi::uv::Loop& loop, wpi::Logger& logger,
+                   const private_init&)
+    : m_logger{logger},
+      m_tcp{uv::Tcp::Create(loop)},
+      m_timer{uv::Timer::Create(loop)} {
+  m_tcp->end.connect([this] {
+    WPI_DEBUG4(m_logger, "{}", "DS connection closed");
+    clearIp();
+    // try to connect again
+    m_tcp->Reuse([this] { m_timer->Start(kReconnectTime); });
+  });
+  m_tcp->data.connect([this](wpi::uv::Buffer buf, size_t len) {
+    HandleIncoming({buf.base, len});
+  });
+  m_timer->timeout.connect([this] { Connect(); });
+  Connect();
+}
+
+DsClient::~DsClient() = default;
+
+void DsClient::Close() {
+  m_tcp->Close();
+  m_timer->Close();
+  clearIp();
+}
+
+void DsClient::Connect() {
+  auto connreq = std::make_shared<uv::TcpConnectReq>();
+  connreq->connected.connect([this] {
+    m_json.clear();
+    m_tcp->StopRead();
+    m_tcp->StartRead();
+  });
+
+  connreq->error = [this](uv::Error err) {
+    WPI_DEBUG4(m_logger, "DS connect failure: {}", err.str());
+    // try to connect again
+    m_tcp->Reuse([this] { m_timer->Start(kReconnectTime); });
+  };
+
+  WPI_DEBUG4(m_logger, "{}", "Starting DS connection attempt");
+  m_tcp->Connect("127.0.0.1", 1742, connreq);
+}
+
+void DsClient::HandleIncoming(std::string_view in) {
+  // this is very bare-bones, as there are never nested {} in these messages
+  while (!in.empty()) {
+    // if json is empty, look for the first { (and discard)
+    if (m_json.empty()) {
+      auto start = in.find('{');
+      in = wpi::slice(in, start, std::string_view::npos);
+    }
+
+    // look for the terminating } (and save)
+    auto end = in.find('}');
+    if (end == std::string_view::npos) {
+      m_json.append(in);
+      return;  // nothing left to read
+    }
+
+    // have complete json message
+    ++end;
+    m_json.append(wpi::slice(in, 0, end));
+    in = wpi::slice(in, end, std::string_view::npos);
+    ParseJson();
+    m_json.clear();
+  }
+}
+
+void DsClient::ParseJson() {
+  WPI_DEBUG4(m_logger, "DsClient JSON: {}", m_json);
+  unsigned int ip = 0;
+  try {
+    ip = wpi::json::parse(m_json).at("robotIP").get<unsigned int>();
+  } catch (wpi::json::exception& e) {
+    WPI_INFO(m_logger, "DsClient JSON error: {}", e.what());
+    return;
+  }
+
+  if (ip == 0) {
+    clearIp();
+  } else {
+    // Convert number into dotted quad
+    auto newip = fmt::format("{}.{}.{}.{}", (ip >> 24) & 0xff,
+                             (ip >> 16) & 0xff, (ip >> 8) & 0xff, ip & 0xff);
+    WPI_INFO(m_logger, "DS received server IP: {}", newip);
+    setIp(newip);
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/EventLoopRunner.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/EventLoopRunner.cpp
index 1c54bdf..c86176e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/EventLoopRunner.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/EventLoopRunner.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/EventLoopRunner.h"
 
@@ -21,7 +18,9 @@
 
   Thread() : m_loop(uv::Loop::Create()) {
     // set up async handles
-    if (!m_loop) return;
+    if (!m_loop) {
+      return;
+    }
 
     // run function
     m_doExec = UvExecFunc::Create(
@@ -31,8 +30,10 @@
         });
   }
 
-  void Main() {
-    if (m_loop) m_loop->Run();
+  void Main() override {
+    if (m_loop) {
+      m_loop->Run();
+    }
   }
 
   // the loop
@@ -42,9 +43,13 @@
   std::weak_ptr<UvExecFunc> m_doExec;
 };
 
-EventLoopRunner::EventLoopRunner() { m_owner.Start(); }
+EventLoopRunner::EventLoopRunner() {
+  m_owner.Start();
+}
 
-EventLoopRunner::~EventLoopRunner() { Stop(); }
+EventLoopRunner::~EventLoopRunner() {
+  Stop();
+}
 
 void EventLoopRunner::Stop() {
   ExecAsync([](uv::Loop& loop) {
@@ -60,7 +65,7 @@
 void EventLoopRunner::ExecAsync(LoopFunc func) {
   if (auto thr = m_owner.GetThread()) {
     if (auto doExec = thr->m_doExec.lock()) {
-      doExec->Call(func);
+      doExec->Call(std::move(func));
     }
   }
 }
@@ -69,13 +74,17 @@
   wpi::future<void> f;
   if (auto thr = m_owner.GetThread()) {
     if (auto doExec = thr->m_doExec.lock()) {
-      f = doExec->Call(func);
+      f = doExec->Call(std::move(func));
     }
   }
-  if (f.valid()) f.wait();
+  if (f.valid()) {
+    f.wait();
+  }
 }
 
 std::shared_ptr<uv::Loop> EventLoopRunner::GetLoop() {
-  if (auto thr = m_owner.GetThread()) return thr->m_loop;
+  if (auto thr = m_owner.GetThread()) {
+    return thr->m_loop;
+  }
   return nullptr;
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpParser.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpParser.cpp
index 4560b38..3c18e0f 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpParser.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpParser.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/HttpParser.h"
 
@@ -37,8 +34,10 @@
   m_settings.on_url = [](http_parser* p, const char* at, size_t length) -> int {
     auto& self = *static_cast<HttpParser*>(p->data);
     // append to buffer
-    if ((self.m_urlBuf.size() + length) > self.m_maxLength) return 1;
-    self.m_urlBuf += StringRef{at, length};
+    if ((self.m_urlBuf.size() + length) > self.m_maxLength) {
+      return 1;
+    }
+    self.m_urlBuf += std::string_view{at, length};
     self.m_state = kUrl;
     return 0;
   };
@@ -48,8 +47,10 @@
                             size_t length) -> int {
     auto& self = *static_cast<HttpParser*>(p->data);
     // use valueBuf for the status
-    if ((self.m_valueBuf.size() + length) > self.m_maxLength) return 1;
-    self.m_valueBuf += StringRef{at, length};
+    if ((self.m_valueBuf.size() + length) > self.m_maxLength) {
+      return 1;
+    }
+    self.m_valueBuf += std::string_view{at, length};
     self.m_state = kStatus;
     return 0;
   };
@@ -62,19 +63,25 @@
     // once we're in header, we know the URL is complete
     if (self.m_state == kUrl) {
       self.url(self.m_urlBuf);
-      if (self.m_aborted) return 1;
+      if (self.m_aborted) {
+        return 1;
+      }
     }
 
     // once we're in header, we know the status is complete
     if (self.m_state == kStatus) {
       self.status(self.m_valueBuf);
-      if (self.m_aborted) return 1;
+      if (self.m_aborted) {
+        return 1;
+      }
     }
 
     // if we previously were in value state, that means we finished a header
     if (self.m_state == kValue) {
       self.header(self.m_fieldBuf, self.m_valueBuf);
-      if (self.m_aborted) return 1;
+      if (self.m_aborted) {
+        return 1;
+      }
     }
 
     // clear field and value when we enter this state
@@ -85,8 +92,10 @@
     }
 
     // append data to field buffer
-    if ((self.m_fieldBuf.size() + length) > self.m_maxLength) return 1;
-    self.m_fieldBuf += StringRef{at, length};
+    if ((self.m_fieldBuf.size() + length) > self.m_maxLength) {
+      return 1;
+    }
+    self.m_fieldBuf += std::string_view{at, length};
     return 0;
   };
 
@@ -102,8 +111,10 @@
     }
 
     // append data to value buffer
-    if ((self.m_valueBuf.size() + length) > self.m_maxLength) return 1;
-    self.m_valueBuf += StringRef{at, length};
+    if ((self.m_valueBuf.size() + length) > self.m_maxLength) {
+      return 1;
+    }
+    self.m_valueBuf += std::string_view{at, length};
     return 0;
   };
 
@@ -114,19 +125,25 @@
     // if we previously were in url state, that means we finished the url
     if (self.m_state == kUrl) {
       self.url(self.m_urlBuf);
-      if (self.m_aborted) return 1;
+      if (self.m_aborted) {
+        return 1;
+      }
     }
 
     // if we previously were in status state, that means we finished the status
     if (self.m_state == kStatus) {
       self.status(self.m_valueBuf);
-      if (self.m_aborted) return 1;
+      if (self.m_aborted) {
+        return 1;
+      }
     }
 
     // if we previously were in value state, that means we finished a header
     if (self.m_state == kValue) {
       self.header(self.m_fieldBuf, self.m_valueBuf);
-      if (self.m_aborted) return 1;
+      if (self.m_aborted) {
+        return 1;
+      }
     }
 
     self.headersComplete(self.ShouldKeepAlive());
@@ -137,7 +154,7 @@
   m_settings.on_body = [](http_parser* p, const char* at,
                           size_t length) -> int {
     auto& self = *static_cast<HttpParser*>(p->data);
-    self.body(StringRef{at, length}, self.IsBodyFinal());
+    self.body(std::string_view{at, length}, self.IsBodyFinal());
     return self.m_aborted;
   };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpServerConnection.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpServerConnection.cpp
index 0308e8c..716d2af 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpServerConnection.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpServerConnection.cpp
@@ -1,14 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/HttpServerConnection.h"
 
+#include "fmt/format.h"
 #include "wpi/SmallString.h"
 #include "wpi/SmallVector.h"
+#include "wpi/SpanExtras.h"
+#include "wpi/StringExtras.h"
+#include "wpi/fmt/raw_ostream.h"
 #include "wpi/raw_uv_ostream.h"
 
 using namespace wpi;
@@ -24,16 +25,18 @@
 
   // look for Accept-Encoding headers to determine if gzip is acceptable
   m_request.messageBegin.connect([this] { m_acceptGzip = false; });
-  m_request.header.connect([this](StringRef name, StringRef value) {
-    if (name.equals_lower("accept-encoding") && value.contains("gzip")) {
-      m_acceptGzip = true;
-    }
-  });
+  m_request.header.connect(
+      [this](std::string_view name, std::string_view value) {
+        if (wpi::equals_lower(name, "accept-encoding") &&
+            wpi::contains(value, "gzip")) {
+          m_acceptGzip = true;
+        }
+      });
 
   // pass incoming data to HTTP parser
   m_dataConn =
       stream->data.connect_connection([this](uv::Buffer& buf, size_t size) {
-        m_request.Execute(StringRef{buf.base, size});
+        m_request.Execute({buf.base, size});
         if (m_request.HasError()) {
           // could not parse; just close the connection
           m_stream.Close();
@@ -57,37 +60,46 @@
 }
 
 void HttpServerConnection::BuildHeader(raw_ostream& os, int code,
-                                       const Twine& codeText,
-                                       const Twine& contentType,
+                                       std::string_view codeText,
+                                       std::string_view contentType,
                                        uint64_t contentLength,
-                                       const Twine& extra) {
-  os << "HTTP/" << m_request.GetMajor() << '.' << m_request.GetMinor() << ' '
-     << code << ' ' << codeText << "\r\n";
-  if (contentLength == 0) m_keepAlive = false;
-  if (!m_keepAlive) os << "Connection: close\r\n";
+                                       std::string_view extra) {
+  fmt::print(os, "HTTP/{}.{} {} {}\r\n", m_request.GetMajor(),
+             m_request.GetMinor(), code, codeText);
+  if (contentLength == 0) {
+    m_keepAlive = false;
+  }
+  if (!m_keepAlive) {
+    os << "Connection: close\r\n";
+  }
   BuildCommonHeaders(os);
   os << "Content-Type: " << contentType << "\r\n";
-  if (contentLength != 0) os << "Content-Length: " << contentLength << "\r\n";
+  if (contentLength != 0) {
+    fmt::print(os, "Content-Length: {}\r\n", contentLength);
+  }
   os << "Access-Control-Allow-Origin: *\r\nAccess-Control-Allow-Methods: *\r\n";
-  SmallString<128> extraBuf;
-  StringRef extraStr = extra.toStringRef(extraBuf);
-  if (!extraStr.empty()) os << extraStr;
+  if (!extra.empty()) {
+    os << extra;
+  }
   os << "\r\n";  // header ends with a blank line
 }
 
-void HttpServerConnection::SendData(ArrayRef<uv::Buffer> bufs,
+void HttpServerConnection::SendData(span<const uv::Buffer> bufs,
                                     bool closeAfter) {
-  m_stream.Write(bufs, [closeAfter, stream = &m_stream](
-                           MutableArrayRef<uv::Buffer> bufs, uv::Error) {
-    for (auto&& buf : bufs) buf.Deallocate();
-    if (closeAfter) stream->Close();
+  m_stream.Write(bufs, [closeAfter, stream = &m_stream](auto bufs, uv::Error) {
+    for (auto&& buf : bufs) {
+      buf.Deallocate();
+    }
+    if (closeAfter) {
+      stream->Close();
+    }
   });
 }
 
-void HttpServerConnection::SendResponse(int code, const Twine& codeText,
-                                        const Twine& contentType,
-                                        StringRef content,
-                                        const Twine& extraHeader) {
+void HttpServerConnection::SendResponse(int code, std::string_view codeText,
+                                        std::string_view contentType,
+                                        std::string_view content,
+                                        std::string_view extraHeader) {
   SmallVector<uv::Buffer, 4> toSend;
   raw_uv_ostream os{toSend, 4096};
   BuildHeader(os, code, codeText, contentType, content.size(), extraHeader);
@@ -96,33 +108,37 @@
   SendData(os.bufs(), !m_keepAlive);
 }
 
-void HttpServerConnection::SendStaticResponse(int code, const Twine& codeText,
-                                              const Twine& contentType,
-                                              StringRef content, bool gzipped,
-                                              const Twine& extraHeader) {
+void HttpServerConnection::SendStaticResponse(
+    int code, std::string_view codeText, std::string_view contentType,
+    std::string_view content, bool gzipped, std::string_view extraHeader) {
   // TODO: handle remote side not accepting gzip (very rare)
 
-  StringRef contentEncodingHeader;
-  if (gzipped /* && m_acceptGzip*/)
+  std::string_view contentEncodingHeader;
+  if (gzipped /* && m_acceptGzip*/) {
     contentEncodingHeader = "Content-Encoding: gzip\r\n";
+  }
 
   SmallVector<uv::Buffer, 4> bufs;
   raw_uv_ostream os{bufs, 4096};
   BuildHeader(os, code, codeText, contentType, content.size(),
-              extraHeader + contentEncodingHeader);
+              fmt::format("{}{}", extraHeader, contentEncodingHeader));
   // can send content without copying
   bufs.emplace_back(content);
 
   m_stream.Write(bufs, [closeAfter = !m_keepAlive, stream = &m_stream](
-                           MutableArrayRef<uv::Buffer> bufs, uv::Error) {
+                           auto bufs, uv::Error) {
     // don't deallocate the static content
-    for (auto&& buf : bufs.drop_back()) buf.Deallocate();
-    if (closeAfter) stream->Close();
+    for (auto&& buf : wpi::drop_back(bufs)) {
+      buf.Deallocate();
+    }
+    if (closeAfter) {
+      stream->Close();
+    }
   });
 }
 
-void HttpServerConnection::SendError(int code, const Twine& message) {
-  StringRef codeText, extra, baseMessage;
+void HttpServerConnection::SendError(int code, std::string_view message) {
+  std::string_view codeText, extra, baseMessage;
   switch (code) {
     case 401:
       codeText = "Unauthorized";
@@ -155,8 +171,8 @@
       baseMessage = "501: Not Implemented!";
       break;
   }
-  SmallString<256> content = baseMessage;
+  SmallString<256> content{baseMessage};
   content += "\r\n";
-  message.toVector(content);
-  SendResponse(code, codeText, "text/plain", content, extra);
+  content += message;
+  SendResponse(code, codeText, "text/plain", content.str(), extra);
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpUtil.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpUtil.cpp
index 866ee10..afce3a6 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpUtil.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/HttpUtil.cpp
@@ -1,70 +1,64 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/HttpUtil.h"
 
 #include <cctype>
 
+#include "fmt/format.h"
 #include "wpi/Base64.h"
-#include "wpi/STLExtras.h"
 #include "wpi/StringExtras.h"
 #include "wpi/TCPConnector.h"
 #include "wpi/raw_ostream.h"
 
 namespace wpi {
 
-StringRef UnescapeURI(const Twine& str, SmallVectorImpl<char>& buf,
-                      bool* error) {
-  SmallString<128> strBuf;
-  StringRef strStr = str.toStringRef(strBuf);
+std::string_view UnescapeURI(std::string_view str, SmallVectorImpl<char>& buf,
+                             bool* error) {
   buf.clear();
-  for (auto i = strStr.begin(), end = strStr.end(); i != end; ++i) {
+  for (auto i = str.begin(), end = str.end(); i != end; ++i) {
     // pass non-escaped characters to output
     if (*i != '%') {
       // decode + to space
-      if (*i == '+')
+      if (*i == '+') {
         buf.push_back(' ');
-      else
+      } else {
         buf.push_back(*i);
+      }
       continue;
     }
 
     // are there enough characters left?
     if (i + 2 >= end) {
       *error = true;
-      return StringRef{};
+      return {};
     }
 
     // replace %xx with the corresponding character
     unsigned val1 = hexDigitValue(*++i);
     if (val1 == -1U) {
       *error = true;
-      return StringRef{};
+      return {};
     }
     unsigned val2 = hexDigitValue(*++i);
     if (val2 == -1U) {
       *error = true;
-      return StringRef{};
+      return {};
     }
     buf.push_back((val1 << 4) | val2);
   }
 
   *error = false;
-  return StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
-StringRef EscapeURI(const Twine& str, SmallVectorImpl<char>& buf,
-                    bool spacePlus) {
+std::string_view EscapeURI(std::string_view str, SmallVectorImpl<char>& buf,
+                           bool spacePlus) {
   static const char* const hexLut = "0123456789ABCDEF";
 
-  SmallString<128> strBuf;
-  StringRef strStr = str.toStringRef(strBuf);
   buf.clear();
-  for (auto i = strStr.begin(), end = strStr.end(); i != end; ++i) {
+  for (auto i = str.begin(), end = str.end(); i != end; ++i) {
     // pass unreserved characters to output
     if (std::isalnum(*i) || *i == '-' || *i == '_' || *i == '.' || *i == '~') {
       buf.push_back(*i);
@@ -83,42 +77,48 @@
     buf.push_back(hexLut[(*i) & 0x0f]);
   }
 
-  return StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
-HttpQueryMap::HttpQueryMap(wpi::StringRef query) {
-  wpi::SmallVector<wpi::StringRef, 16> queryElems;
-  query.split(queryElems, '&', 100, false);
+HttpQueryMap::HttpQueryMap(std::string_view query) {
+  SmallVector<std::string_view, 16> queryElems;
+  split(query, queryElems, '&', 100, false);
   for (auto elem : queryElems) {
-    auto [nameEsc, valueEsc] = elem.split('=');
-    wpi::SmallString<64> nameBuf;
+    auto [nameEsc, valueEsc] = split(elem, '=');
+    SmallString<64> nameBuf;
     bool err = false;
     auto name = wpi::UnescapeURI(nameEsc, nameBuf, &err);
     // note: ignores duplicates
-    if (!err) m_elems.try_emplace(name, valueEsc);
+    if (!err) {
+      m_elems.try_emplace(name, valueEsc);
+    }
   }
 }
 
-std::optional<wpi::StringRef> HttpQueryMap::Get(
-    wpi::StringRef name, wpi::SmallVectorImpl<char>& buf) const {
+std::optional<std::string_view> HttpQueryMap::Get(
+    std::string_view name, wpi::SmallVectorImpl<char>& buf) const {
   auto it = m_elems.find(name);
-  if (it == m_elems.end()) return {};
+  if (it == m_elems.end()) {
+    return {};
+  }
   bool err = false;
   auto val = wpi::UnescapeURI(it->second, buf, &err);
-  if (err) return {};
+  if (err) {
+    return {};
+  }
   return val;
 }
 
-HttpPath::HttpPath(wpi::StringRef path) {
+HttpPath::HttpPath(std::string_view path) {
   // special-case root path to be a single empty element
   if (path == "/") {
     m_pathEnds.emplace_back(0);
     return;
   }
-  wpi::SmallVector<wpi::StringRef, 16> pathElems;
-  path.split(pathElems, '/', 100, false);
+  wpi::SmallVector<std::string_view, 16> pathElems;
+  split(path, pathElems, '/', 100, false);
   for (auto elem : pathElems) {
-    wpi::SmallString<64> buf;
+    SmallString<64> buf;
     bool err = false;
     auto val = wpi::UnescapeURI(elem, buf, &err);
     if (err) {
@@ -130,59 +130,78 @@
   }
 }
 
-bool HttpPath::startswith(size_t start, ArrayRef<StringRef> match) const {
-  if (m_pathEnds.size() < (start + match.size())) return false;
+bool HttpPath::startswith(size_t start,
+                          span<const std::string_view> match) const {
+  if (m_pathEnds.size() < (start + match.size())) {
+    return false;
+  }
   bool first = start == 0;
   auto p = m_pathEnds.begin() + start;
   for (auto m : match) {
-    auto val = m_pathBuf.slice(first ? 0 : *(p - 1), *p);
-    if (val != m) return false;
+    auto val = slice(m_pathBuf, first ? 0 : *(p - 1), *p);
+    if (val != m) {
+      return false;
+    }
     first = false;
     ++p;
   }
   return true;
 }
 
+std::string_view HttpPath::operator[](size_t n) const {
+  return slice(m_pathBuf, n == 0 ? 0 : m_pathEnds[n - 1], m_pathEnds[n]);
+}
+
 bool ParseHttpHeaders(raw_istream& is, SmallVectorImpl<char>* contentType,
                       SmallVectorImpl<char>* contentLength) {
-  if (contentType) contentType->clear();
-  if (contentLength) contentLength->clear();
+  if (contentType) {
+    contentType->clear();
+  }
+  if (contentLength) {
+    contentLength->clear();
+  }
 
   bool inContentType = false;
   bool inContentLength = false;
   SmallString<64> lineBuf;
   for (;;) {
-    StringRef line = is.getline(lineBuf, 1024).rtrim();
-    if (is.has_error()) return false;
-    if (line.empty()) return true;  // empty line signals end of headers
+    std::string_view line = rtrim(is.getline(lineBuf, 1024));
+    if (is.has_error()) {
+      return false;
+    }
+    if (line.empty()) {
+      return true;  // empty line signals end of headers
+    }
 
     // header fields start at the beginning of the line
     if (!std::isspace(line[0])) {
       inContentType = false;
       inContentLength = false;
-      StringRef field;
-      std::tie(field, line) = line.split(':');
-      field = field.rtrim();
-      if (field.equals_lower("content-type"))
+      std::string_view field;
+      std::tie(field, line) = split(line, ':');
+      field = rtrim(field);
+      if (equals_lower(field, "content-type")) {
         inContentType = true;
-      else if (field.equals_lower("content-length"))
+      } else if (equals_lower(field, "content-length")) {
         inContentLength = true;
-      else
+      } else {
         continue;  // ignore other fields
+      }
     }
 
     // collapse whitespace
-    line = line.ltrim();
+    line = ltrim(line);
 
     // save field data
-    if (inContentType && contentType)
+    if (inContentType && contentType) {
       contentType->append(line.begin(), line.end());
-    else if (inContentLength && contentLength)
+    } else if (inContentLength && contentLength) {
       contentLength->append(line.begin(), line.end());
+    }
   }
 }
 
-bool FindMultipartBoundary(raw_istream& is, StringRef boundary,
+bool FindMultipartBoundary(raw_istream& is, std::string_view boundary,
                            std::string* saveBuf) {
   SmallString<64> searchBuf;
   searchBuf.resize(boundary.size() + 2);
@@ -194,7 +213,9 @@
   if (!saveBuf) {
     do {
       is.read(searchBuf.data(), 1);
-      if (is.has_error()) return false;
+      if (is.has_error()) {
+        return false;
+      }
     } while (searchBuf[0] == '\r' || searchBuf[0] == '\n');
     searchPos = 1;
   }
@@ -205,19 +226,26 @@
   // there's a bunch of continuous -'s in the output, but that's unlikely.
   for (;;) {
     is.read(searchBuf.data() + searchPos, searchBuf.size() - searchPos);
-    if (is.has_error()) return false;
+    if (is.has_error()) {
+      return false;
+    }
 
     // Did we find the boundary?
     if (searchBuf[0] == '-' && searchBuf[1] == '-' &&
-        searchBuf.substr(2) == boundary)
+        searchBuf.substr(2) == boundary) {
       return true;
+    }
 
     // Fast-scan for '-'
     size_t pos = searchBuf.find('-', searchBuf[0] == '-' ? 1 : 0);
-    if (pos == StringRef::npos) {
-      if (saveBuf) saveBuf->append(searchBuf.data(), searchBuf.size());
+    if (pos == std::string_view::npos) {
+      if (saveBuf) {
+        saveBuf->append(searchBuf.data(), searchBuf.size());
+      }
     } else {
-      if (saveBuf) saveBuf->append(searchBuf.data(), pos);
+      if (saveBuf) {
+        saveBuf->append(searchBuf.data(), pos);
+      }
 
       // move '-' and following to start of buffer (next read will fill)
       std::memmove(searchBuf.data(), searchBuf.data() + pos,
@@ -227,63 +255,58 @@
   }
 }
 
-HttpLocation::HttpLocation(const Twine& url_, bool* error,
+HttpLocation::HttpLocation(std::string_view url_, bool* error,
                            std::string* errorMsg)
-    : url{url_.str()} {
+    : url{url_} {
   // Split apart into components
-  StringRef query{url};
+  std::string_view query{url};
 
   // scheme:
-  StringRef scheme;
-  std::tie(scheme, query) = query.split(':');
-  if (!scheme.equals_lower("http")) {
+  std::string_view scheme;
+  std::tie(scheme, query) = split(query, ':');
+  if (!equals_lower(scheme, "http")) {
     *errorMsg = "only supports http URLs";
     *error = true;
     return;
   }
 
   // "//"
-  if (!query.startswith("//")) {
+  if (!starts_with(query, "//")) {
     *errorMsg = "expected http://...";
     *error = true;
     return;
   }
-  query = query.drop_front(2);
+  query.remove_prefix(2);
 
   // user:password@host:port/
-  StringRef authority;
-  std::tie(authority, query) = query.split('/');
+  std::string_view authority;
+  std::tie(authority, query) = split(query, '/');
 
-  StringRef userpass, hostport;
-  std::tie(userpass, hostport) = authority.split('@');
+  auto [userpass, hostport] = split(authority, '@');
   // split leaves the RHS empty if the split char isn't present...
   if (hostport.empty()) {
     hostport = userpass;
-    userpass = StringRef{};
+    userpass = {};
   }
 
   if (!userpass.empty()) {
-    StringRef rawUser, rawPassword;
-    std::tie(rawUser, rawPassword) = userpass.split(':');
+    auto [rawUser, rawPassword] = split(userpass, ':');
     SmallString<64> userBuf, passBuf;
     user = UnescapeURI(rawUser, userBuf, error);
     if (*error) {
-      raw_string_ostream oss(*errorMsg);
-      oss << "could not unescape user \"" << rawUser << "\"";
-      oss.flush();
+      *errorMsg = fmt::format("could not unescape user \"{}\"", rawUser);
       return;
     }
     password = UnescapeURI(rawPassword, passBuf, error);
     if (*error) {
-      raw_string_ostream oss(*errorMsg);
-      oss << "could not unescape password \"" << rawPassword << "\"";
-      oss.flush();
+      *errorMsg =
+          fmt::format("could not unescape password \"{}\"", rawPassword);
       return;
     }
   }
 
-  StringRef portStr;
-  std::tie(host, portStr) = hostport.rsplit(':');
+  std::string_view portStr;
+  std::tie(host, portStr) = rsplit(hostport, ':');
   if (host.empty()) {
     *errorMsg = "host is empty";
     *error = true;
@@ -291,44 +314,42 @@
   }
   if (portStr.empty()) {
     port = 80;
-  } else if (portStr.getAsInteger(10, port)) {
-    raw_string_ostream oss(*errorMsg);
-    oss << "port \"" << portStr << "\" is not an integer";
-    oss.flush();
+  } else if (auto p = parse_integer<int>(portStr, 10)) {
+    port = p.value();
+  } else {
+    *errorMsg = fmt::format("port \"{}\" is not an integer", portStr);
     *error = true;
     return;
   }
 
   // path?query#fragment
-  std::tie(query, fragment) = query.split('#');
-  std::tie(path, query) = query.split('?');
+  std::tie(query, fragment) = split(query, '#');
+  std::tie(path, query) = split(query, '?');
 
   // Split query string into parameters
   while (!query.empty()) {
     // split out next param and value
-    StringRef rawParam, rawValue;
-    std::tie(rawParam, query) = query.split('&');
-    if (rawParam.empty()) continue;  // ignore "&&"
-    std::tie(rawParam, rawValue) = rawParam.split('=');
+    std::string_view rawParam, rawValue;
+    std::tie(rawParam, query) = split(query, '&');
+    if (rawParam.empty()) {
+      continue;  // ignore "&&"
+    }
+    std::tie(rawParam, rawValue) = split(rawParam, '=');
 
     // unescape param
     *error = false;
     SmallString<64> paramBuf;
-    StringRef param = UnescapeURI(rawParam, paramBuf, error);
+    std::string_view param = UnescapeURI(rawParam, paramBuf, error);
     if (*error) {
-      raw_string_ostream oss(*errorMsg);
-      oss << "could not unescape parameter \"" << rawParam << "\"";
-      oss.flush();
+      *errorMsg = fmt::format("could not unescape parameter \"{}\"", rawParam);
       return;
     }
 
     // unescape value
     SmallString<64> valueBuf;
-    StringRef value = UnescapeURI(rawValue, valueBuf, error);
+    std::string_view value = UnescapeURI(rawValue, valueBuf, error);
     if (*error) {
-      raw_string_ostream oss(*errorMsg);
-      oss << "could not unescape value \"" << rawValue << "\"";
-      oss.flush();
+      *errorMsg = fmt::format("could not unescape value \"{}\"", rawValue);
       return;
     }
 
@@ -344,7 +365,7 @@
     userpass += loc.user;
     userpass += ':';
     userpass += loc.password;
-    Base64Encode(userpass, &auth);
+    Base64Encode(userpass.str(), &auth);
   }
 }
 
@@ -353,31 +374,30 @@
   // send GET request
   os << "GET /" << request.path << " HTTP/1.1\r\n";
   os << "Host: " << request.host << "\r\n";
-  if (!request.auth.empty())
+  if (!request.auth.empty()) {
     os << "Authorization: Basic " << request.auth << "\r\n";
+  }
   os << "\r\n";
   os.flush();
 
   // read first line of response
   SmallString<64> lineBuf;
-  StringRef line = is.getline(lineBuf, 1024).rtrim();
+  std::string_view line = rtrim(is.getline(lineBuf, 1024));
   if (is.has_error()) {
     *warnMsg = "disconnected before response";
     return false;
   }
 
   // see if we got a HTTP 200 response
-  StringRef httpver, code, codeText;
-  std::tie(httpver, line) = line.split(' ');
-  std::tie(code, codeText) = line.split(' ');
-  if (!httpver.startswith("HTTP")) {
+  std::string_view httpver, code, codeText;
+  std::tie(httpver, line) = split(line, ' ');
+  std::tie(code, codeText) = split(line, ' ');
+  if (!starts_with(httpver, "HTTP")) {
     *warnMsg = "did not receive HTTP response";
     return false;
   }
   if (code != "200") {
-    raw_string_ostream oss(*warnMsg);
-    oss << "received " << code << " " << codeText << " response";
-    oss.flush();
+    *warnMsg = fmt::format("received {} {} response", code, codeText);
     return false;
   }
 
@@ -390,7 +410,7 @@
   return true;
 }
 
-void HttpMultipartScanner::SetBoundary(StringRef boundary) {
+void HttpMultipartScanner::SetBoundary(std::string_view boundary) {
   m_boundaryWith = "\n--";
   m_boundaryWith += boundary;
   m_boundaryWithout = "\n";
@@ -406,9 +426,13 @@
   m_buf.resize(0);
 }
 
-StringRef HttpMultipartScanner::Execute(StringRef in) {
-  if (m_state == kDone) Reset(m_saveSkipped);
-  if (m_saveSkipped) m_buf += in;
+std::string_view HttpMultipartScanner::Execute(std::string_view in) {
+  if (m_state == kDone) {
+    Reset(m_saveSkipped);
+  }
+  if (m_saveSkipped) {
+    m_buf += in;
+  }
 
   size_t pos = 0;
   if (m_state == kBoundary) {
@@ -449,19 +473,21 @@
   }
 
   if (m_state == kPadding) {
-    for (char ch : in.drop_front(pos)) {
+    for (char ch : drop_front(in, pos)) {
       ++pos;
       if (ch == '\n') {
         // Found the LF; return remaining input buffer (following it)
         m_state = kDone;
-        if (m_saveSkipped) m_buf.resize(m_buf.size() - in.size() + pos);
-        return in.drop_front(pos);
+        if (m_saveSkipped) {
+          m_buf.resize(m_buf.size() - in.size() + pos);
+        }
+        return drop_front(in, pos);
       }
     }
   }
 
   // We consumed the entire input
-  return StringRef{};
+  return {};
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/Logger.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/Logger.cpp
new file mode 100644
index 0000000..2b8ec6d
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/Logger.cpp
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/Logger.h"
+
+using namespace wpi;
+
+void Logger::DoLog(unsigned int level, const char* file, unsigned int line,
+                   const char* msg) {
+  if (!m_func || level < m_min_level) {
+    return;
+  }
+  m_func(level, file, line, msg);
+}
+
+void Logger::LogV(unsigned int level, const char* file, unsigned int line,
+                  fmt::string_view format, fmt::format_args args) {
+  if (!m_func || level < m_min_level) {
+    return;
+  }
+  fmt::memory_buffer out;
+  fmt::vformat_to(fmt::appender{out}, format, args);
+  out.push_back('\0');
+  m_func(level, file, line, out.data());
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/MimeTypes.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/MimeTypes.cpp
index 5bf9d29..082dab1 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/MimeTypes.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/MimeTypes.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/MimeTypes.h"
 
@@ -13,7 +10,7 @@
 
 // derived partially from
 // https://github.com/DEGoodmanWilson/libmime/blob/stable/0.1.2/mime/mime.cpp
-StringRef MimeTypeFromPath(StringRef path) {
+std::string_view MimeTypeFromPath(std::string_view path) {
   // https://developer.mozilla.org/en-US/docs/Web/HTTP/Basics_of_HTTP/MIME_types
   static StringMap<const char*> mimeTypes{
       // text
@@ -55,11 +52,11 @@
   static const char* defaultType = "application/octet-stream";
 
   auto pos = path.find_last_of("/");
-  if (pos != StringRef::npos) {
+  if (pos != std::string_view::npos) {
     path = path.substr(pos + 1);
   }
   auto dot_pos = path.find_last_of(".");
-  if (dot_pos > 0 && dot_pos != StringRef::npos) {
+  if (dot_pos > 0 && dot_pos != std::string_view::npos) {
     auto type = mimeTypes.find(path.substr(dot_pos + 1));
     if (type != mimeTypes.end()) {
       return type->getValue();
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/ParallelTcpConnector.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/ParallelTcpConnector.cpp
new file mode 100644
index 0000000..5a8394a
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/ParallelTcpConnector.cpp
@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/ParallelTcpConnector.h"
+
+#include <fmt/format.h>
+
+#include "wpi/Logger.h"
+#include "wpi/uv/GetAddrInfo.h"
+#include "wpi/uv/Loop.h"
+#include "wpi/uv/Tcp.h"
+#include "wpi/uv/Timer.h"
+#include "wpi/uv/util.h"
+
+using namespace wpi;
+
+ParallelTcpConnector::ParallelTcpConnector(
+    wpi::uv::Loop& loop, wpi::uv::Timer::Time reconnectRate,
+    wpi::Logger& logger, std::function<void(wpi::uv::Tcp& tcp)> connected,
+    const private_init&)
+    : m_loop{loop},
+      m_logger{logger},
+      m_reconnectRate{reconnectRate},
+      m_connected{std::move(connected)},
+      m_reconnectTimer{uv::Timer::Create(loop)} {
+  m_reconnectTimer->timeout.connect([this] {
+    if (!IsConnected()) {
+      WPI_DEBUG1(m_logger, "{}", "timed out, reconnecting");
+      Connect();
+    }
+  });
+}
+
+ParallelTcpConnector::~ParallelTcpConnector() = default;
+
+void ParallelTcpConnector::Close() {
+  CancelAll();
+  m_reconnectTimer->Close();
+}
+
+void ParallelTcpConnector::SetServers(
+    wpi::span<const std::pair<std::string, unsigned int>> servers) {
+  m_servers.assign(servers.begin(), servers.end());
+  if (!IsConnected()) {
+    Connect();
+  }
+}
+
+void ParallelTcpConnector::Disconnected() {
+  if (m_isConnected) {
+    m_isConnected = false;
+    Connect();
+  }
+}
+
+void ParallelTcpConnector::Succeeded(uv::Tcp& tcp) {
+  if (!m_isConnected) {
+    m_isConnected = true;
+    m_reconnectTimer->Stop();
+    CancelAll(&tcp);
+  }
+}
+
+void ParallelTcpConnector::Connect() {
+  if (IsConnected()) {
+    return;
+  }
+
+  CancelAll();
+  m_reconnectTimer->Start(m_reconnectRate);
+
+  WPI_DEBUG3(m_logger, "{}", "starting new connection attempts");
+
+  // kick off parallel lookups
+  for (auto&& server : m_servers) {
+    auto req = std::make_shared<uv::GetAddrInfoReq>();
+    m_resolvers.emplace_back(req);
+
+    req->resolved.connect(
+        [this, req = req.get()](const addrinfo& addrinfo) {
+          if (IsConnected()) {
+            return;
+          }
+
+          // kick off parallel connection attempts
+          for (auto ai = &addrinfo; ai; ai = ai->ai_next) {
+            auto tcp = uv::Tcp::Create(m_loop);
+            m_attempts.emplace_back(tcp);
+
+            auto connreq = std::make_shared<uv::TcpConnectReq>();
+            connreq->connected.connect(
+                [this, tcp = tcp.get()] {
+                  if (m_logger.min_level() <= wpi::WPI_LOG_DEBUG4) {
+                    std::string ip;
+                    unsigned int port = 0;
+                    uv::AddrToName(tcp->GetPeer(), &ip, &port);
+                    WPI_DEBUG4(m_logger,
+                               "successful connection ({}) to {} port {}",
+                               static_cast<void*>(tcp), ip, port);
+                  }
+                  if (IsConnected()) {
+                    tcp->Shutdown([tcp] { tcp->Close(); });
+                    return;
+                  }
+                  if (m_connected) {
+                    m_connected(*tcp);
+                  }
+                },
+                shared_from_this());
+
+            connreq->error = [selfWeak = weak_from_this(),
+                              tcp = tcp.get()](uv::Error err) {
+              if (auto self = selfWeak.lock()) {
+                WPI_DEBUG1(self->m_logger, "connect failure ({}): {}",
+                           static_cast<void*>(tcp), err.str());
+              }
+            };
+
+            if (m_logger.min_level() <= wpi::WPI_LOG_DEBUG4) {
+              std::string ip;
+              unsigned int port = 0;
+              uv::AddrToName(*reinterpret_cast<sockaddr_storage*>(ai->ai_addr),
+                             &ip, &port);
+              WPI_DEBUG4(
+                  m_logger,
+                  "Info({}) starting connection attempt ({}) to {} port {}",
+                  static_cast<void*>(req), static_cast<void*>(tcp.get()), ip,
+                  port);
+            }
+            tcp->Connect(*ai->ai_addr, connreq);
+          }
+        },
+        shared_from_this());
+
+    req->error = [req = req.get(), selfWeak = weak_from_this()](uv::Error err) {
+      if (auto self = selfWeak.lock()) {
+        WPI_DEBUG1(self->m_logger, "GetAddrInfo({}) failure: {}",
+                   static_cast<void*>(req), err.str());
+      }
+    };
+
+    WPI_DEBUG4(m_logger, "starting GetAddrInfo({}) for {} port {}",
+               static_cast<void*>(req.get()), server.first, server.second);
+    addrinfo hints;
+    std::memset(&hints, 0, sizeof(hints));
+    hints.ai_family = AF_UNSPEC;
+    hints.ai_socktype = SOCK_STREAM;
+    hints.ai_protocol = IPPROTO_TCP;
+    hints.ai_flags = AI_NUMERICSERV | AI_ADDRCONFIG;
+    uv::GetAddrInfo(m_loop, req, server.first, fmt::format("{}", server.second),
+                    &hints);
+  }
+}
+
+void ParallelTcpConnector::CancelAll(wpi::uv::Tcp* except) {
+  WPI_DEBUG4(m_logger, "{}", "canceling previous attempts");
+  for (auto&& resolverWeak : m_resolvers) {
+    if (auto resolver = resolverWeak.lock()) {
+      WPI_DEBUG4(m_logger, "canceling GetAddrInfo({})",
+                 static_cast<void*>(resolver.get()));
+      resolver->Cancel();
+    }
+  }
+  m_resolvers.clear();
+
+  for (auto&& tcpWeak : m_attempts) {
+    if (auto tcp = tcpWeak.lock()) {
+      if (tcp.get() != except) {
+        WPI_DEBUG4(m_logger, "canceling connection attempt ({})",
+                   static_cast<void*>(tcp.get()));
+        tcp->Close();
+      }
+    }
+  }
+  m_attempts.clear();
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/PortForwarder.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/PortForwarder.cpp
index fe54b63..a423d48 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/PortForwarder.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/PortForwarder.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/PortForwarder.h"
 
+#include "fmt/format.h"
 #include "wpi/DenseMap.h"
 #include "wpi/EventLoopRunner.h"
-#include "wpi/SmallString.h"
-#include "wpi/raw_ostream.h"
 #include "wpi/uv/GetAddrInfo.h"
 #include "wpi/uv/Tcp.h"
 #include "wpi/uv/Timer.h"
@@ -36,16 +32,19 @@
     buf2.len = len;
     auto out = outWeak.lock();
     if (!out) {
+      buf2.Deallocate();
       in.Close();
       return;
     }
-    out->Write(buf2, [](auto bufs, uv::Error) {
-      for (auto buf : bufs) buf.Deallocate();
+    out->Write({buf2}, [](auto bufs, uv::Error) {
+      for (auto buf : bufs) {
+        buf.Deallocate();
+      }
     });
   });
 }
 
-void PortForwarder::Add(unsigned int port, const Twine& remoteHost,
+void PortForwarder::Add(unsigned int port, std::string_view remoteHost,
                         unsigned int remotePort) {
   m_impl->runner.ExecSync([&](uv::Loop& loop) {
     auto server = uv::Tcp::Create(loop);
@@ -55,10 +54,12 @@
 
     // when we get a connection, accept it
     server->connection.connect([serverPtr = server.get(),
-                                host = remoteHost.str(), remotePort] {
+                                host = std::string{remoteHost}, remotePort] {
       auto& loop = serverPtr->GetLoopRef();
       auto client = serverPtr->Accept();
-      if (!client) return;
+      if (!client) {
+        return;
+      }
 
       // close on error
       client->error.connect(
@@ -73,20 +74,20 @@
           [remotePtr = remote.get(),
            clientWeak = std::weak_ptr<uv::Tcp>(client)](uv::Error err) {
             remotePtr->Close();
-            if (auto client = clientWeak.lock()) client->Close();
+            if (auto client = clientWeak.lock()) {
+              client->Close();
+            }
           });
 
-      // convert port to string
-      SmallString<16> remotePortStr;
-      raw_svector_ostream(remotePortStr) << remotePort;
-
       // resolve address
       uv::GetAddrInfo(
           loop,
           [clientWeak = std::weak_ptr<uv::Tcp>(client),
            remoteWeak = std::weak_ptr<uv::Tcp>(remote)](const addrinfo& addr) {
             auto remote = remoteWeak.lock();
-            if (!remote) return;
+            if (!remote) {
+              return;
+            }
 
             // connect to remote address/port
             remote->Connect(*addr.ai_addr, [remotePtr = remote.get(),
@@ -101,11 +102,15 @@
               // close both when either side closes
               client->end.connect([clientPtr = client.get(), remoteWeak] {
                 clientPtr->Close();
-                if (auto remote = remoteWeak.lock()) remote->Close();
+                if (auto remote = remoteWeak.lock()) {
+                  remote->Close();
+                }
               });
               remotePtr->end.connect([remotePtr, clientWeak] {
                 remotePtr->Close();
-                if (auto client = clientWeak.lock()) client->Close();
+                if (auto client = clientWeak.lock()) {
+                  client->Close();
+                }
               });
 
               // copy bidirectionally
@@ -115,7 +120,7 @@
               CopyStream(*remotePtr, clientWeak);
             });
           },
-          host, remotePortStr);
+          host, fmt::to_string(remotePort));
 
       // time out for connection
       uv::Timer::SingleShot(loop, uv::Timer::Time{500},
@@ -124,10 +129,12 @@
                              remoteWeak = std::weak_ptr<uv::Tcp>(remote)] {
                               if (auto connected = connectedWeak.lock()) {
                                 if (!*connected) {
-                                  if (auto client = clientWeak.lock())
+                                  if (auto client = clientWeak.lock()) {
                                     client->Close();
-                                  if (auto remote = remoteWeak.lock())
+                                  }
+                                  if (auto remote = remoteWeak.lock()) {
                                     remote->Close();
+                                  }
                                 }
                               }
                             });
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/SafeThread.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/SafeThread.cpp
index 3a906bd..bbecc5c 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/SafeThread.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/SafeThread.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/SafeThread.h"
 
@@ -12,7 +9,9 @@
 detail::SafeThreadProxyBase::SafeThreadProxyBase(
     std::shared_ptr<SafeThread> thr)
     : m_thread(std::move(thr)) {
-  if (!m_thread) return;
+  if (!m_thread) {
+    return;
+  }
   m_lock = std::unique_lock<wpi::mutex>(m_thread->m_mutex);
   if (!m_thread->m_active) {
     m_lock.unlock();
@@ -22,15 +21,18 @@
 }
 
 detail::SafeThreadOwnerBase::~SafeThreadOwnerBase() {
-  if (m_joinAtExit)
+  if (m_joinAtExit) {
     Join();
-  else
+  } else {
     Stop();
+  }
 }
 
 void detail::SafeThreadOwnerBase::Start(std::shared_ptr<SafeThread> thr) {
   std::scoped_lock lock(m_mutex);
-  if (auto thr = m_thread.lock()) return;
+  if (auto thr = m_thread.lock()) {
+    return;
+  }
   m_stdThread = std::thread([=] { thr->Main(); });
   thr->m_threadId = m_stdThread.get_id();
   m_thread = thr;
@@ -43,7 +45,9 @@
     thr->m_cond.notify_all();
     m_thread.reset();
   }
-  if (m_stdThread.joinable()) m_stdThread.detach();
+  if (m_stdThread.joinable()) {
+    m_stdThread.detach();
+  }
 }
 
 void detail::SafeThreadOwnerBase::Join() {
@@ -62,7 +66,9 @@
 
 void detail::swap(SafeThreadOwnerBase& lhs, SafeThreadOwnerBase& rhs) noexcept {
   using std::swap;
-  if (&lhs == &rhs) return;
+  if (&lhs == &rhs) {
+    return;
+  }
   std::scoped_lock lock(lhs.m_mutex, rhs.m_mutex);
   std::swap(lhs.m_stdThread, rhs.m_stdThread);
   std::swap(lhs.m_thread, rhs.m_thread);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/SocketError.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/SocketError.cpp
index 1695846..3f08d1e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/SocketError.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/SocketError.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/SocketError.h"
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/StringExtras.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/StringExtras.cpp
new file mode 100644
index 0000000..45d23ea
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/StringExtras.cpp
@@ -0,0 +1,360 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//===-- StringExtras.cpp - Implement the StringExtras header --------------===//
+//
+//                     The LLVM Compiler Infrastructure
+//
+// This file is distributed under the University of Illinois Open Source
+// License. See LICENSE.TXT for details.
+//
+//===----------------------------------------------------------------------===//
+//
+// This file implements the StringExtras.h header
+//
+//===----------------------------------------------------------------------===//
+
+#include "wpi/StringExtras.h"
+
+#include <algorithm>
+#include <cstdlib>
+#include <string_view>
+
+#include "wpi/SmallString.h"
+#include "wpi/SmallVector.h"
+
+// strncasecmp() is not available on non-POSIX systems, so define an
+// alternative function here.
+static int ascii_strncasecmp(const char* lhs, const char* rhs,
+                             size_t length) noexcept {
+  for (size_t i = 0; i < length; ++i) {
+    unsigned char lhc = wpi::toLower(lhs[i]);
+    unsigned char rhc = wpi::toLower(rhs[i]);
+    if (lhc != rhc) {
+      return lhc < rhc ? -1 : 1;
+    }
+  }
+  return 0;
+}
+
+int wpi::compare_lower(std::string_view lhs, std::string_view rhs) noexcept {
+  if (int Res = ascii_strncasecmp(lhs.data(), rhs.data(),
+                                  (std::min)(lhs.size(), rhs.size()))) {
+    return Res;
+  }
+  if (lhs.size() == rhs.size()) {
+    return 0;
+  }
+  return lhs.size() < rhs.size() ? -1 : 1;
+}
+
+std::string_view::size_type wpi::find_lower(
+    std::string_view str, char ch, std::string_view::size_type from) noexcept {
+  char lch = toLower(ch);
+  auto s = drop_front(str, from);
+  while (!s.empty()) {
+    if (toLower(s.front()) == lch) {
+      return str.size() - s.size();
+    }
+    s.remove_prefix(1);
+  }
+  return std::string_view::npos;
+}
+
+std::string_view::size_type wpi::find_lower(
+    std::string_view str, std::string_view other,
+    std::string_view::size_type from) noexcept {
+  auto s = str.substr(from);
+  while (s.size() >= other.size()) {
+    if (starts_with_lower(s, other)) {
+      return from;
+    }
+    s.remove_prefix(1);
+    ++from;
+  }
+  return std::string_view::npos;
+}
+
+std::string_view::size_type wpi::rfind_lower(
+    std::string_view str, char ch, std::string_view::size_type from) noexcept {
+  from = (std::min)(from, str.size());
+  auto data = str.data();
+  std::string_view::size_type i = from;
+  while (i != 0) {
+    --i;
+    if (toLower(data[i]) == toLower(ch)) {
+      return i;
+    }
+  }
+  return std::string_view::npos;
+}
+
+std::string_view::size_type wpi::rfind_lower(std::string_view str,
+                                             std::string_view other) noexcept {
+  std::string_view::size_type n = other.size();
+  if (n > str.size()) {
+    return std::string_view::npos;
+  }
+  for (size_t i = str.size() - n + 1, e = 0; i != e;) {
+    --i;
+    if (equals_lower(str.substr(i, n), other)) {
+      return i;
+    }
+  }
+  return std::string_view::npos;
+}
+
+bool wpi::starts_with_lower(std::string_view str,
+                            std::string_view prefix) noexcept {
+  return str.size() >= prefix.size() &&
+         ascii_strncasecmp(str.data(), prefix.data(), prefix.size()) == 0;
+}
+
+bool wpi::ends_with_lower(std::string_view str,
+                          std::string_view suffix) noexcept {
+  return str.size() >= suffix.size() &&
+         ascii_strncasecmp(str.data() + str.size() - suffix.size(),
+                           suffix.data(), suffix.size()) == 0;
+}
+
+void wpi::split(std::string_view str, SmallVectorImpl<std::string_view>& arr,
+                std::string_view separator, int maxSplit,
+                bool keepEmpty) noexcept {
+  std::string_view s = str;
+
+  // Count down from maxSplit. When maxSplit is -1, this will just split
+  // "forever". This doesn't support splitting more than 2^31 times
+  // intentionally; if we ever want that we can make maxSplit a 64-bit integer
+  // but that seems unlikely to be useful.
+  while (maxSplit-- != 0) {
+    auto idx = s.find(separator);
+    if (idx == std::string_view::npos) {
+      break;
+    }
+
+    // Push this split.
+    if (keepEmpty || idx > 0) {
+      arr.push_back(slice(s, 0, idx));
+    }
+
+    // Jump forward.
+    s = slice(s, idx + separator.size(), std::string_view::npos);
+  }
+
+  // Push the tail.
+  if (keepEmpty || !s.empty()) {
+    arr.push_back(s);
+  }
+}
+
+void wpi::split(std::string_view str, SmallVectorImpl<std::string_view>& arr,
+                char separator, int maxSplit, bool keepEmpty) noexcept {
+  std::string_view s = str;
+
+  // Count down from maxSplit. When maxSplit is -1, this will just split
+  // "forever". This doesn't support splitting more than 2^31 times
+  // intentionally; if we ever want that we can make maxSplit a 64-bit integer
+  // but that seems unlikely to be useful.
+  while (maxSplit-- != 0) {
+    size_t idx = s.find(separator);
+    if (idx == std::string_view::npos) {
+      break;
+    }
+
+    // Push this split.
+    if (keepEmpty || idx > 0) {
+      arr.push_back(slice(s, 0, idx));
+    }
+
+    // Jump forward.
+    s = slice(s, idx + 1, std::string_view::npos);
+  }
+
+  // Push the tail.
+  if (keepEmpty || !s.empty()) {
+    arr.push_back(s);
+  }
+}
+
+static unsigned GetAutoSenseRadix(std::string_view& str) noexcept {
+  if (str.empty()) {
+    return 10;
+  }
+
+  if (wpi::starts_with(str, "0x") || wpi::starts_with(str, "0X")) {
+    str.remove_prefix(2);
+    return 16;
+  }
+
+  if (wpi::starts_with(str, "0b") || wpi::starts_with(str, "0B")) {
+    str.remove_prefix(2);
+    return 2;
+  }
+
+  if (wpi::starts_with(str, "0o")) {
+    str.remove_prefix(2);
+    return 8;
+  }
+
+  if (str[0] == '0' && str.size() > 1 && wpi::isDigit(str[1])) {
+    str.remove_prefix(1);
+    return 8;
+  }
+
+  return 10;
+}
+
+bool wpi::detail::ConsumeUnsignedInteger(
+    std::string_view& str, unsigned radix,
+    unsigned long long& result) noexcept {  // NOLINT(runtime/int)
+  // Autosense radix if not specified.
+  if (radix == 0) {
+    radix = GetAutoSenseRadix(str);
+  }
+
+  // Empty strings (after the radix autosense) are invalid.
+  if (str.empty()) {
+    return true;
+  }
+
+  // Parse all the bytes of the string given this radix.  Watch for overflow.
+  std::string_view str2 = str;
+  result = 0;
+  while (!str2.empty()) {
+    unsigned charVal;
+    if (str2[0] >= '0' && str2[0] <= '9') {
+      charVal = str2[0] - '0';
+    } else if (str2[0] >= 'a' && str2[0] <= 'z') {
+      charVal = str2[0] - 'a' + 10;
+    } else if (str2[0] >= 'A' && str2[0] <= 'Z') {
+      charVal = str2[0] - 'A' + 10;
+    } else {
+      break;
+    }
+
+    // If the parsed value is larger than the integer radix, we cannot
+    // consume any more characters.
+    if (charVal >= radix) {
+      break;
+    }
+
+    // Add in this character.
+    unsigned long long prevResult = result;  // NOLINT(runtime/int)
+    result = result * radix + charVal;
+
+    // Check for overflow by shifting back and seeing if bits were lost.
+    if (result / radix < prevResult) {
+      return true;
+    }
+
+    str2.remove_prefix(1);
+  }
+
+  // We consider the operation a failure if no characters were consumed
+  // successfully.
+  if (str.size() == str2.size()) {
+    return true;
+  }
+
+  str = str2;
+  return false;
+}
+
+bool wpi::detail::ConsumeSignedInteger(
+    std::string_view& str, unsigned radix,
+    long long& result) noexcept {  // NOLINT(runtime/int)
+  unsigned long long ullVal;       // NOLINT(runtime/int)
+
+  // Handle positive strings first.
+  if (str.empty() || str.front() != '-') {
+    if (wpi::detail::ConsumeUnsignedInteger(str, radix, ullVal) ||
+        // Check for value so large it overflows a signed value.
+        static_cast<long long>(ullVal) < 0) {  // NOLINT(runtime/int)
+      return true;
+    }
+    result = ullVal;
+    return false;
+  }
+
+  // Get the positive part of the value.
+  std::string_view str2 = wpi::drop_front(str, 1);
+  if (wpi::detail::ConsumeUnsignedInteger(str2, radix, ullVal) ||
+      // Reject values so large they'd overflow as negative signed, but allow
+      // "-0".  This negates the unsigned so that the negative isn't undefined
+      // on signed overflow.
+      static_cast<long long>(-ullVal) > 0) {  // NOLINT(runtime/int)
+    return true;
+  }
+
+  str = str2;
+  result = -ullVal;
+  return false;
+}
+
+bool wpi::detail::GetAsUnsignedInteger(
+    std::string_view str, unsigned radix,
+    unsigned long long& result) noexcept {  // NOLINT(runtime/int)
+  if (wpi::detail::ConsumeUnsignedInteger(str, radix, result)) {
+    return true;
+  }
+
+  // For getAsUnsignedInteger, we require the whole string to be consumed or
+  // else we consider it a failure.
+  return !str.empty();
+}
+
+bool wpi::detail::GetAsSignedInteger(
+    std::string_view str, unsigned radix,
+    long long& result) noexcept {  // NOLINT(runtime/int)
+  if (wpi::detail::ConsumeSignedInteger(str, radix, result)) {
+    return true;
+  }
+
+  // For getAsSignedInteger, we require the whole string to be consumed or else
+  // we consider it a failure.
+  return !str.empty();
+}
+
+template <>
+std::optional<float> wpi::parse_float<float>(std::string_view str) noexcept {
+  if (str.empty()) {
+    return std::nullopt;
+  }
+  wpi::SmallString<32> storage{str};
+  char* end;
+  float val = std::strtof(storage.c_str(), &end);
+  if (*end != '\0') {
+    return std::nullopt;
+  }
+  return val;
+}
+
+template <>
+std::optional<double> wpi::parse_float<double>(std::string_view str) noexcept {
+  if (str.empty()) {
+    return std::nullopt;
+  }
+  wpi::SmallString<32> storage{str};
+  char* end;
+  double val = std::strtod(storage.c_str(), &end);
+  if (*end != '\0') {
+    return std::nullopt;
+  }
+  return val;
+}
+
+template <>
+std::optional<long double> wpi::parse_float<long double>(
+    std::string_view str) noexcept {
+  if (str.empty()) {
+    return std::nullopt;
+  }
+  wpi::SmallString<32> storage{str};
+  char* end;
+  long double val = std::strtold(storage.c_str(), &end);
+  if (*end != '\0') {
+    return std::nullopt;
+  }
+  return val;
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/Synchronization.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/Synchronization.cpp
new file mode 100644
index 0000000..da97897
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/Synchronization.cpp
@@ -0,0 +1,368 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/Synchronization.h"
+
+#include <algorithm>
+#include <cstring>
+#include <mutex>
+
+#include "wpi/DenseMap.h"
+#include "wpi/SmallVector.h"
+#include "wpi/UidVector.h"
+#include "wpi/condition_variable.h"
+#include "wpi/mutex.h"
+
+using namespace wpi;
+
+namespace {
+
+struct State {
+  int signaled{0};
+  bool autoReset{false};
+  wpi::SmallVector<wpi::condition_variable*, 2> waiters;
+};
+
+struct HandleManager {
+  wpi::mutex mutex;
+  wpi::UidVector<int, 8> eventIds;
+  wpi::UidVector<int, 8> semaphoreIds;
+  wpi::DenseMap<WPI_Handle, State> states;
+};
+
+}  // namespace
+
+static HandleManager& GetManager() {
+  static HandleManager manager;
+  return manager;
+}
+
+WPI_EventHandle wpi::CreateEvent(bool manualReset, bool initialState) {
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+
+  auto index = manager.eventIds.emplace_back(0);
+  WPI_EventHandle handle = (kHandleTypeEvent << 24) | (index & 0xffffff);
+
+  // configure state data
+  auto& state = manager.states[handle];
+  state.signaled = initialState ? 1 : 0;
+  state.autoReset = !manualReset;
+
+  return handle;
+}
+
+void wpi::DestroyEvent(WPI_EventHandle handle) {
+  if ((handle >> 24) != kHandleTypeEvent) {
+    return;
+  }
+
+  DestroySignalObject(handle);
+
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+  manager.eventIds.erase(handle & 0xffffff);
+}
+
+void wpi::SetEvent(WPI_EventHandle handle) {
+  if ((handle >> 24) != kHandleTypeEvent) {
+    return;
+  }
+
+  SetSignalObject(handle);
+}
+
+void wpi::ResetEvent(WPI_EventHandle handle) {
+  if ((handle >> 24) != kHandleTypeEvent) {
+    return;
+  }
+
+  ResetSignalObject(handle);
+}
+
+WPI_SemaphoreHandle wpi::CreateSemaphore(int initialCount, int maximumCount) {
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+
+  auto index = manager.semaphoreIds.emplace_back(maximumCount);
+  WPI_EventHandle handle = (kHandleTypeSemaphore << 24) | (index & 0xffffff);
+
+  // configure state data
+  auto& state = manager.states[handle];
+  state.signaled = initialCount;
+  state.autoReset = true;
+
+  return handle;
+}
+
+void wpi::DestroySemaphore(WPI_SemaphoreHandle handle) {
+  if ((handle >> 24) != kHandleTypeSemaphore) {
+    return;
+  }
+
+  DestroySignalObject(handle);
+
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+  manager.eventIds.erase(handle & 0xffffff);
+}
+
+bool wpi::ReleaseSemaphore(WPI_SemaphoreHandle handle, int releaseCount,
+                           int* prevCount) {
+  if ((handle >> 24) != kHandleTypeSemaphore) {
+    return false;
+  }
+  if (releaseCount <= 0) {
+    return false;
+  }
+  int index = handle & 0xffffff;
+
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+  auto it = manager.states.find(handle);
+  if (it == manager.states.end()) {
+    return false;
+  }
+  auto& state = it->second;
+  int maxCount = manager.eventIds[index];
+  if (prevCount) {
+    *prevCount = state.signaled;
+  }
+  if ((maxCount - state.signaled) < releaseCount) {
+    return false;
+  }
+  state.signaled += releaseCount;
+  for (auto& waiter : state.waiters) {
+    waiter->notify_all();
+  }
+  return true;
+}
+
+bool wpi::WaitForObject(WPI_Handle handle) {
+  return WaitForObject(handle, -1, nullptr);
+}
+
+bool wpi::WaitForObject(WPI_Handle handle, double timeout, bool* timedOut) {
+  WPI_Handle signaledValue;
+  auto signaled = WaitForObjects(
+      wpi::span(&handle, 1), wpi::span(&signaledValue, 1), timeout, timedOut);
+  if (signaled.empty()) {
+    return false;
+  }
+  return (signaled[0] & 0x80000000ul) == 0;
+}
+
+wpi::span<WPI_Handle> wpi::WaitForObjects(wpi::span<const WPI_Handle> handles,
+                                          wpi::span<WPI_Handle> signaled) {
+  return WaitForObjects(handles, signaled, -1, nullptr);
+}
+
+wpi::span<WPI_Handle> wpi::WaitForObjects(wpi::span<const WPI_Handle> handles,
+                                          wpi::span<WPI_Handle> signaled,
+                                          double timeout, bool* timedOut) {
+  auto& manager = GetManager();
+  std::unique_lock lock{manager.mutex};
+  wpi::condition_variable cv;
+  bool addedWaiters = false;
+  bool timedOutVal = false;
+  size_t count = 0;
+
+  for (;;) {
+    for (auto handle : handles) {
+      auto it = manager.states.find(handle);
+      if (it == manager.states.end()) {
+        if (count < signaled.size()) {
+          // treat a non-existent handle as signaled, but set the error bit
+          signaled[count++] = handle | 0x80000000ul;
+        }
+      } else {
+        auto& state = it->second;
+        if (state.signaled > 0) {
+          if (count < signaled.size()) {
+            signaled[count++] = handle;
+          }
+          if (state.autoReset) {
+            --state.signaled;
+            if (state.signaled < 0) {
+              state.signaled = 0;
+            }
+          }
+        }
+      }
+    }
+
+    if (timedOutVal || count != 0) {
+      break;
+    }
+
+    if (timeout == 0) {
+      timedOutVal = true;
+      break;
+    }
+
+    if (!addedWaiters) {
+      addedWaiters = true;
+      for (auto handle : handles) {
+        auto& state = manager.states[handle];
+        state.waiters.emplace_back(&cv);
+      }
+    }
+
+    if (timeout < 0) {
+      cv.wait(lock);
+    } else {
+      auto timeoutTime = std::chrono::steady_clock::now() +
+                         std::chrono::duration<double>(timeout);
+      if (cv.wait_until(lock, timeoutTime) == std::cv_status::timeout) {
+        timedOutVal = true;
+      }
+    }
+  }
+
+  if (addedWaiters) {
+    for (auto handle : handles) {
+      auto& state = manager.states[handle];
+      auto it = std::find(state.waiters.begin(), state.waiters.end(), &cv);
+      if (it != state.waiters.end()) {
+        state.waiters.erase(it);
+      }
+    }
+  }
+
+  if (timedOut) {
+    *timedOut = timedOutVal;
+  }
+
+  return signaled.subspan(0, count);
+}
+
+void wpi::CreateSignalObject(WPI_Handle handle, bool manualReset,
+                             bool initialState) {
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+  auto& state = manager.states[handle];
+  state.signaled = initialState ? 1 : 0;
+  state.autoReset = !manualReset;
+}
+
+void wpi::SetSignalObject(WPI_Handle handle) {
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+  auto it = manager.states.find(handle);
+  if (it == manager.states.end()) {
+    return;
+  }
+  auto& state = it->second;
+  state.signaled = 1;
+  for (auto& waiter : state.waiters) {
+    waiter->notify_all();
+    if (state.autoReset) {
+      // expect the first waiter to reset it
+      break;
+    }
+  }
+}
+
+void wpi::ResetSignalObject(WPI_Handle handle) {
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+  auto it = manager.states.find(handle);
+  if (it != manager.states.end()) {
+    it->second.signaled = 0;
+  }
+}
+
+void wpi::DestroySignalObject(WPI_Handle handle) {
+  auto& manager = GetManager();
+  std::scoped_lock lock{manager.mutex};
+
+  auto it = manager.states.find(handle);
+  if (it != manager.states.end()) {
+    // wake up any waiters
+    for (auto& waiter : it->second.waiters) {
+      waiter->notify_all();
+    }
+    manager.states.erase(it);
+  }
+}
+
+extern "C" {
+
+WPI_EventHandle WPI_CreateEvent(int manual_reset, int initial_state) {
+  return wpi::CreateEvent(manual_reset != 0, initial_state != 0);
+}
+
+void WPI_DestroyEvent(WPI_EventHandle handle) {
+  wpi::DestroyEvent(handle);
+}
+
+void WPI_SetEvent(WPI_EventHandle handle) {
+  wpi::SetEvent(handle);
+}
+
+void WPI_ResetEvent(WPI_EventHandle handle) {
+  wpi::ResetEvent(handle);
+}
+
+WPI_SemaphoreHandle WPI_CreateSemaphore(int initial_count, int maximum_count) {
+  return wpi::CreateSemaphore(initial_count, maximum_count);
+}
+
+void WPI_DestroySemaphore(WPI_SemaphoreHandle handle) {
+  wpi::DestroySemaphore(handle);
+}
+
+int WPI_ReleaseSemaphore(WPI_SemaphoreHandle handle, int release_count,
+                         int* prev_count) {
+  return wpi::ReleaseSemaphore(handle, release_count, prev_count);
+}
+
+int WPI_WaitForObject(WPI_Handle handle) {
+  return wpi::WaitForObject(handle);
+}
+
+int WPI_WaitForObjectTimeout(WPI_Handle handle, double timeout,
+                             int* timed_out) {
+  bool timedOutBool;
+  int rv = wpi::WaitForObject(handle, timeout, &timedOutBool);
+  *timed_out = timedOutBool ? 1 : 0;
+  return rv;
+}
+
+int WPI_WaitForObjects(const WPI_Handle* handles, int handles_count,
+                       WPI_Handle* signaled) {
+  return wpi::WaitForObjects(wpi::span(handles, handles_count),
+                             wpi::span(signaled, handles_count))
+      .size();
+}
+
+int WPI_WaitForObjectsTimeout(const WPI_Handle* handles, int handles_count,
+                              WPI_Handle* signaled, double timeout,
+                              int* timed_out) {
+  bool timedOutBool;
+  auto signaledResult = wpi::WaitForObjects(wpi::span(handles, handles_count),
+                                            wpi::span(signaled, handles_count),
+                                            timeout, &timedOutBool);
+  *timed_out = timedOutBool ? 1 : 0;
+  return signaledResult.size();
+}
+
+void WPI_CreateSignalObject(WPI_Handle handle, int manual_reset,
+                            int initial_state) {
+  wpi::CreateSignalObject(handle, manual_reset, initial_state);
+}
+
+void WPI_SetSignalObject(WPI_Handle handle) {
+  wpi::SetSignalObject(handle);
+}
+
+void WPI_ResetSignalObject(WPI_Handle handle) {
+  wpi::ResetSignalObject(handle);
+}
+
+void WPI_DestroySignalObject(WPI_Handle handle) {
+  wpi::DestroySignalObject(handle);
+}
+
+}  // extern "C"
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPAcceptor.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPAcceptor.cpp
index 90d9496..8d12ac3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPAcceptor.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPAcceptor.cpp
@@ -44,7 +44,7 @@
 
 using namespace wpi;
 
-TCPAcceptor::TCPAcceptor(int port, const char* address, Logger& logger)
+TCPAcceptor::TCPAcceptor(int port, std::string_view address, Logger& logger)
     : m_lsd(0),
       m_port(port),
       m_address(address),
@@ -73,11 +73,13 @@
 }
 
 int TCPAcceptor::start() {
-  if (m_listening) return 0;
+  if (m_listening) {
+    return 0;
+  }
 
   m_lsd = socket(PF_INET, SOCK_STREAM, 0);
   if (m_lsd < 0) {
-    WPI_ERROR(m_logger, "could not create socket");
+    WPI_ERROR(m_logger, "{}", "could not create socket");
     return -1;
   }
   struct sockaddr_in address;
@@ -93,7 +95,7 @@
     int res = inet_pton(PF_INET, m_address.c_str(), &(address.sin_addr));
 #endif
     if (res != 1) {
-      WPI_ERROR(m_logger, "could not resolve " << m_address << " address");
+      WPI_ERROR(m_logger, "could not resolve {} address", m_address);
       return -1;
     }
   } else {
@@ -114,15 +116,15 @@
   int result = bind(m_lsd, reinterpret_cast<struct sockaddr*>(&address),
                     sizeof(address));
   if (result != 0) {
-    WPI_ERROR(m_logger,
-              "bind() to port " << m_port << " failed: " << SocketStrerror());
+    WPI_ERROR(m_logger, "bind() to port {} failed: {}", m_port,
+              SocketStrerror());
     return result;
   }
 
   result = listen(m_lsd, 5);
   if (result != 0) {
-    WPI_ERROR(m_logger,
-              "listen() on port " << m_port << " failed: " << SocketStrerror());
+    WPI_ERROR(m_logger, "listen() on port {} failed: {}", m_port,
+              SocketStrerror());
     return result;
   }
   m_listening = true;
@@ -153,7 +155,8 @@
   address.sin_port = htons(m_port);
 
   int result = -1, sd = socket(AF_INET, SOCK_STREAM, 0);
-  if (sd < 0) return;
+  if (sd < 0)
+    return;
 
   // Set socket to non-blocking
   u_long mode = 1;
@@ -176,7 +179,9 @@
 }
 
 std::unique_ptr<NetworkStream> TCPAcceptor::accept() {
-  if (!m_listening || m_shutdown) return nullptr;
+  if (!m_listening || m_shutdown) {
+    return nullptr;
+  }
 
   struct sockaddr_in address;
 #ifdef _WIN32
@@ -185,11 +190,12 @@
   socklen_t len = sizeof(address);
 #endif
   std::memset(&address, 0, sizeof(address));
-  int sd = ::accept(m_lsd, (struct sockaddr*)&address, &len);
+  int sd = ::accept(m_lsd, reinterpret_cast<struct sockaddr*>(&address), &len);
   if (sd < 0) {
-    if (!m_shutdown)
-      WPI_ERROR(m_logger, "accept() on port "
-                              << m_port << " failed: " << SocketStrerror());
+    if (!m_shutdown) {
+      WPI_ERROR(m_logger, "accept() on port {} failed: {}", m_port,
+                SocketStrerror());
+    }
     return nullptr;
   }
   if (m_shutdown) {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector.cpp
index 6110133..ed97962 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector.cpp
@@ -96,7 +96,7 @@
     int res = inet_pton(PF_INET, server, &(address.sin_addr));
 #endif
     if (res != 1) {
-      WPI_ERROR(logger, "could not resolve " << server << " address");
+      WPI_ERROR(logger, "could not resolve {} address", server);
       return nullptr;
     }
   }
@@ -105,12 +105,13 @@
   if (timeout == 0) {
     int sd = socket(AF_INET, SOCK_STREAM, 0);
     if (sd < 0) {
-      WPI_ERROR(logger, "could not create socket");
+      WPI_ERROR(logger, "{}", "could not create socket");
       return nullptr;
     }
-    if (::connect(sd, (struct sockaddr*)&address, sizeof(address)) != 0) {
-      WPI_ERROR(logger, "connect() to " << server << " port " << port
-                                        << " failed: " << SocketStrerror());
+    if (::connect(sd, reinterpret_cast<struct sockaddr*>(&address),
+                  sizeof(address)) != 0) {
+      WPI_ERROR(logger, "connect() to {} port {} failed: {}", server, port,
+                SocketStrerror());
 #ifdef _WIN32
       closesocket(sd);
 #else
@@ -126,7 +127,7 @@
   socklen_t len;
   int result = -1, valopt, sd = socket(AF_INET, SOCK_STREAM, 0);
   if (sd < 0) {
-    WPI_ERROR(logger, "could not create socket");
+    WPI_ERROR(logger, "{}", "could not create socket");
     return nullptr;
   }
 
@@ -134,25 +135,26 @@
 #ifdef _WIN32
   u_long mode = 1;
   if (ioctlsocket(sd, FIONBIO, &mode) == SOCKET_ERROR)
-    WPI_WARNING(logger,
-                "could not set socket to non-blocking: " << SocketStrerror());
+    WPI_WARNING(logger, "could not set socket to non-blocking: {}",
+                SocketStrerror());
 #else
   int arg;
   arg = fcntl(sd, F_GETFL, nullptr);
   if (arg < 0) {
-    WPI_WARNING(logger,
-                "could not set socket to non-blocking: " << SocketStrerror());
+    WPI_WARNING(logger, "could not set socket to non-blocking: {}",
+                SocketStrerror());
   } else {
     arg |= O_NONBLOCK;
-    if (fcntl(sd, F_SETFL, arg) < 0)
-      WPI_WARNING(logger,
-                  "could not set socket to non-blocking: " << SocketStrerror());
+    if (fcntl(sd, F_SETFL, arg) < 0) {
+      WPI_WARNING(logger, "could not set socket to non-blocking: {}",
+                  SocketStrerror());
+    }
   }
 #endif
 
   // Connect with time limit
-  if ((result = ::connect(sd, (struct sockaddr*)&address, sizeof(address))) <
-      0) {
+  if ((result = ::connect(sd, reinterpret_cast<struct sockaddr*>(&address),
+                          sizeof(address))) < 0) {
     int my_errno = SocketErrno();
 #ifdef _WIN32
     if (my_errno == WSAEWOULDBLOCK || my_errno == WSAEINPROGRESS) {
@@ -168,21 +170,18 @@
         getsockopt(sd, SOL_SOCKET, SO_ERROR, reinterpret_cast<char*>(&valopt),
                    &len);
         if (valopt) {
-          WPI_ERROR(logger, "select() to " << server << " port " << port
-                                           << " error " << valopt << " - "
-                                           << SocketStrerror(valopt));
-        }
-        // connection established
-        else
+          WPI_ERROR(logger, "select() to {} port {} error {} - {}", server,
+                    port, valopt, SocketStrerror(valopt));
+        } else {
+          // connection established
           result = 0;
+        }
       } else {
-        WPI_INFO(logger,
-                 "connect() to " << server << " port " << port << " timed out");
+        WPI_INFO(logger, "connect() to {} port {} timed out", server, port);
       }
     } else {
-      WPI_ERROR(logger, "connect() to " << server << " port " << port
-                                        << " error " << SocketErrno() << " - "
-                                        << SocketStrerror());
+      WPI_ERROR(logger, "connect() to {} port {} error {} - {}", server, port,
+                SocketErrno(), SocketStrerror());
     }
   }
 
@@ -190,18 +189,19 @@
 #ifdef _WIN32
   mode = 0;
   if (ioctlsocket(sd, FIONBIO, &mode) == SOCKET_ERROR)
-    WPI_WARNING(logger,
-                "could not set socket to blocking: " << SocketStrerror());
+    WPI_WARNING(logger, "could not set socket to blocking: {}",
+                SocketStrerror());
 #else
   arg = fcntl(sd, F_GETFL, nullptr);
   if (arg < 0) {
-    WPI_WARNING(logger,
-                "could not set socket to blocking: " << SocketStrerror());
+    WPI_WARNING(logger, "could not set socket to blocking: {}",
+                SocketStrerror());
   } else {
     arg &= (~O_NONBLOCK);
-    if (fcntl(sd, F_SETFL, arg) < 0)
-      WPI_WARNING(logger,
-                  "could not set socket to blocking: " << SocketStrerror());
+    if (fcntl(sd, F_SETFL, arg) < 0) {
+      WPI_WARNING(logger, "could not set socket to blocking: {}",
+                  SocketStrerror());
+    }
   }
 #endif
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp
index ee5803f..26258cf 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPConnector_parallel.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/TCPConnector.h"  // NOLINT(build/include_order)
 
@@ -27,9 +24,11 @@
 #endif
 
 std::unique_ptr<NetworkStream> TCPConnector::connect_parallel(
-    ArrayRef<std::pair<const char*, int>> servers, Logger& logger,
+    span<const std::pair<const char*, int>> servers, Logger& logger,
     int timeout) {
-  if (servers.empty()) return nullptr;
+  if (servers.empty()) {
+    return nullptr;
+  }
 
   // structure to make sure we don't start duplicate workers
   struct GlobalState {
@@ -75,13 +74,15 @@
     // attempt to the same server
     {
       std::scoped_lock lock(local->mtx);
-      if (local->active.count(active_tracker) > 0) continue;  // already in set
+      if (local->active.count(active_tracker) > 0) {
+        continue;  // already in set
+      }
     }
 
     ++num_workers;
 
     // start the worker
-    std::thread([=]() {
+    std::thread([=] {
       if (!result->done) {
         // add to global state
         {
@@ -102,7 +103,9 @@
         // successful connection
         if (stream) {
           std::scoped_lock lock(result->mtx);
-          if (!result->done.exchange(true)) result->stream = std::move(stream);
+          if (!result->done.exchange(true)) {
+            result->stream = std::move(stream);
+          }
         }
       }
       ++result->count;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPStream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPStream.cpp
index 4af3e6f..4567161 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPStream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/TCPStream.cpp
@@ -57,7 +57,9 @@
   m_peerPort = ntohs(address->sin_port);
 }
 
-TCPStream::~TCPStream() { close(); }
+TCPStream::~TCPStream() {
+  close();
+}
 
 size_t TCPStream::send(const char* buffer, size_t len, Error* err) {
   if (m_sd < 0) {
@@ -101,10 +103,11 @@
   ssize_t rv = ::send(m_sd, buffer, len, 0);
 #endif
   if (rv < 0) {
-    if (!m_blocking && (errno == EAGAIN || errno == EWOULDBLOCK))
+    if (!m_blocking && (errno == EAGAIN || errno == EWOULDBLOCK)) {
       *err = kWouldBlock;
-    else
+    } else {
       *err = kConnectionReset;
+    }
     return 0;
   }
 #endif
@@ -139,13 +142,14 @@
   }
   if (rv < 0) {
 #ifdef _WIN32
-    if (!m_blocking && WSAGetLastError() == WSAEWOULDBLOCK)
+    if (!m_blocking && WSAGetLastError() == WSAEWOULDBLOCK) {
 #else
-    if (!m_blocking && (errno == EAGAIN || errno == EWOULDBLOCK))
+    if (!m_blocking && (errno == EAGAIN || errno == EWOULDBLOCK)) {
 #endif
       *err = kWouldBlock;
-    else
+    } else {
       *err = kConnectionReset;
+    }
     return 0;
   }
   return static_cast<size_t>(rv);
@@ -164,35 +168,52 @@
   m_sd = -1;
 }
 
-StringRef TCPStream::getPeerIP() const { return m_peerIP; }
+std::string_view TCPStream::getPeerIP() const {
+  return m_peerIP;
+}
 
-int TCPStream::getPeerPort() const { return m_peerPort; }
+int TCPStream::getPeerPort() const {
+  return m_peerPort;
+}
 
 void TCPStream::setNoDelay() {
-  if (m_sd < 0) return;
+  if (m_sd < 0) {
+    return;
+  }
   int optval = 1;
   setsockopt(m_sd, IPPROTO_TCP, TCP_NODELAY, reinterpret_cast<char*>(&optval),
              sizeof optval);
 }
 
 bool TCPStream::setBlocking(bool enabled) {
-  if (m_sd < 0) return true;  // silently accept
+  if (m_sd < 0) {
+    return true;  // silently accept
+  }
 #ifdef _WIN32
   u_long mode = enabled ? 0 : 1;
-  if (ioctlsocket(m_sd, FIONBIO, &mode) == SOCKET_ERROR) return false;
+  if (ioctlsocket(m_sd, FIONBIO, &mode) == SOCKET_ERROR) {
+    return false;
+  }
 #else
   int flags = fcntl(m_sd, F_GETFL, nullptr);
-  if (flags < 0) return false;
-  if (enabled)
+  if (flags < 0) {
+    return false;
+  }
+  if (enabled) {
     flags &= ~O_NONBLOCK;
-  else
+  } else {
     flags |= O_NONBLOCK;
-  if (fcntl(m_sd, F_SETFL, flags) < 0) return false;
+  }
+  if (fcntl(m_sd, F_SETFL, flags) < 0) {
+    return false;
+  }
 #endif
   return true;
 }
 
-int TCPStream::getNativeHandle() const { return m_sd; }
+int TCPStream::getNativeHandle() const {
+  return m_sd;
+}
 
 bool TCPStream::WaitForReadEvent(int timeout) {
   fd_set sdset;
@@ -202,7 +223,7 @@
   tv.tv_usec = 0;
   FD_ZERO(&sdset);
   FD_SET(m_sd, &sdset);
-  if (select(m_sd + 1, &sdset, NULL, NULL, &tv) > 0) {
+  if (select(m_sd + 1, &sdset, nullptr, nullptr, &tv) > 0) {
     return true;
   }
   return false;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/UDPClient.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/UDPClient.cpp
index aafdf47..108ef54 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/UDPClient.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/UDPClient.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/UDPClient.h"
 
@@ -19,14 +16,15 @@
 #endif
 
 #include "wpi/Logger.h"
+#include "wpi/SmallString.h"
 #include "wpi/SocketError.h"
 
 using namespace wpi;
 
 UDPClient::UDPClient(Logger& logger) : UDPClient("", logger) {}
 
-UDPClient::UDPClient(const Twine& address, Logger& logger)
-    : m_lsd(0), m_port(0), m_address(address.str()), m_logger(logger) {}
+UDPClient::UDPClient(std::string_view address, Logger& logger)
+    : m_lsd(0), m_port(0), m_address(address), m_logger(logger) {}
 
 UDPClient::UDPClient(UDPClient&& other)
     : m_lsd(other.m_lsd),
@@ -44,7 +42,9 @@
 }
 
 UDPClient& UDPClient::operator=(UDPClient&& other) {
-  if (this == &other) return *this;
+  if (this == &other) {
+    return *this;
+  }
   shutdown();
   m_logger = other.m_logger;
   m_lsd = other.m_lsd;
@@ -55,10 +55,14 @@
   return *this;
 }
 
-int UDPClient::start() { return start(0); }
+int UDPClient::start() {
+  return start(0);
+}
 
 int UDPClient::start(int port) {
-  if (m_lsd > 0) return 0;
+  if (m_lsd > 0) {
+    return 0;
+  }
 
 #ifdef _WIN32
   WSAData wsaData;
@@ -69,7 +73,7 @@
   m_lsd = socket(AF_INET, SOCK_DGRAM, 0);
 
   if (m_lsd < 0) {
-    WPI_ERROR(m_logger, "could not create socket");
+    WPI_ERROR(m_logger, "{}", "could not create socket");
     return -1;
   }
 
@@ -85,7 +89,7 @@
     int res = inet_pton(PF_INET, m_address.c_str(), &(addr.sin_addr));
 #endif
     if (res != 1) {
-      WPI_ERROR(m_logger, "could not resolve " << m_address << " address");
+      WPI_ERROR(m_logger, "could not resolve {} address", m_address);
       return -1;
     }
   } else {
@@ -107,7 +111,7 @@
 
   int result = bind(m_lsd, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
   if (result != 0) {
-    WPI_ERROR(m_logger, "bind() failed: " << SocketStrerror());
+    WPI_ERROR(m_logger, "bind() failed: {}", SocketStrerror());
     return result;
   }
   m_port = port;
@@ -129,25 +133,25 @@
   }
 }
 
-int UDPClient::send(ArrayRef<uint8_t> data, const Twine& server, int port) {
+int UDPClient::send(span<const uint8_t> data, std::string_view server,
+                    int port) {
   // server must be a resolvable IP address
   struct sockaddr_in addr;
   std::memset(&addr, 0, sizeof(addr));
   addr.sin_family = AF_INET;
-  SmallVector<char, 128> addr_store;
-  StringRef remoteAddr = server.toNullTerminatedStringRef(addr_store);
+  SmallString<128> remoteAddr{server};
   if (remoteAddr.empty()) {
-    WPI_ERROR(m_logger, "server must be passed");
+    WPI_ERROR(m_logger, "{}", "server must be passed");
     return -1;
   }
 
 #ifdef _WIN32
-  int res = InetPton(AF_INET, remoteAddr.data(), &(addr.sin_addr));
+  int res = InetPton(AF_INET, remoteAddr.c_str(), &(addr.sin_addr));
 #else
-  int res = inet_pton(AF_INET, remoteAddr.data(), &(addr.sin_addr));
+  int res = inet_pton(AF_INET, remoteAddr.c_str(), &(addr.sin_addr));
 #endif
   if (res != 1) {
-    WPI_ERROR(m_logger, "could not resolve " << server << " address");
+    WPI_ERROR(m_logger, "could not resolve {} address", server);
     return -1;
   }
   addr.sin_port = htons(port);
@@ -159,25 +163,24 @@
   return result;
 }
 
-int UDPClient::send(StringRef data, const Twine& server, int port) {
+int UDPClient::send(std::string_view data, std::string_view server, int port) {
   // server must be a resolvable IP address
   struct sockaddr_in addr;
   std::memset(&addr, 0, sizeof(addr));
   addr.sin_family = AF_INET;
-  SmallVector<char, 128> addr_store;
-  StringRef remoteAddr = server.toNullTerminatedStringRef(addr_store);
+  SmallString<128> remoteAddr{server};
   if (remoteAddr.empty()) {
-    WPI_ERROR(m_logger, "server must be passed");
+    WPI_ERROR(m_logger, "{}", "server must be passed");
     return -1;
   }
 
 #ifdef _WIN32
-  int res = InetPton(AF_INET, remoteAddr.data(), &(addr.sin_addr));
+  int res = InetPton(AF_INET, remoteAddr.c_str(), &(addr.sin_addr));
 #else
-  int res = inet_pton(AF_INET, remoteAddr.data(), &(addr.sin_addr));
+  int res = inet_pton(AF_INET, remoteAddr.c_str(), &(addr.sin_addr));
 #endif
   if (res != 1) {
-    WPI_ERROR(m_logger, "could not resolve " << server << " address");
+    WPI_ERROR(m_logger, "could not resolve {} address", server);
     return -1;
   }
   addr.sin_port = htons(port);
@@ -189,14 +192,18 @@
 }
 
 int UDPClient::receive(uint8_t* data_received, int receive_len) {
-  if (m_port == 0) return -1;  // return if not receiving
+  if (m_port == 0) {
+    return -1;  // return if not receiving
+  }
   return recv(m_lsd, reinterpret_cast<char*>(data_received), receive_len, 0);
 }
 
 int UDPClient::receive(uint8_t* data_received, int receive_len,
                        SmallVectorImpl<char>* addr_received,
                        int* port_received) {
-  if (m_port == 0) return -1;  // return if not receiving
+  if (m_port == 0) {
+    return -1;  // return if not receiving
+  }
 
   struct sockaddr_in remote;
   socklen_t remote_len = sizeof(remote);
@@ -225,13 +232,17 @@
 }
 
 int UDPClient::set_timeout(double timeout) {
-  if (timeout < 0) return -1;
+  if (timeout < 0) {
+    return -1;
+  }
   struct timeval tv;
   tv.tv_sec = timeout;             // truncating will give seconds
   timeout -= tv.tv_sec;            // remove seconds portion
   tv.tv_usec = timeout * 1000000;  // fractions of a second to us
   int ret = setsockopt(m_lsd, SOL_SOCKET, SO_RCVTIMEO,
                        reinterpret_cast<char*>(&tv), sizeof(tv));
-  if (ret < 0) WPI_ERROR(m_logger, "set timeout failed");
+  if (ret < 0) {
+    WPI_ERROR(m_logger, "{}", "set timeout failed");
+  }
   return ret;
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocket.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocket.cpp
index 82c83bb..4bb49d3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocket.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocket.cpp
@@ -1,18 +1,17 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WebSocket.h"
 
 #include <random>
 
+#include "fmt/format.h"
 #include "wpi/Base64.h"
 #include "wpi/HttpParser.h"
 #include "wpi/SmallString.h"
 #include "wpi/SmallVector.h"
+#include "wpi/StringExtras.h"
 #include "wpi/raw_uv_ostream.h"
 #include "wpi/sha1.h"
 #include "wpi/uv/Stream.h"
@@ -23,14 +22,18 @@
 class WebSocketWriteReq : public uv::WriteReq {
  public:
   explicit WebSocketWriteReq(
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    finish.connect([=](uv::Error err) {
-      MutableArrayRef<uv::Buffer> bufs{m_bufs};
-      for (auto&& buf : bufs.slice(0, m_startUser)) buf.Deallocate();
-      callback(bufs.slice(m_startUser), err);
+      std::function<void(span<uv::Buffer>, uv::Error)> callback)
+      : m_callback{std::move(callback)} {
+    finish.connect([this](uv::Error err) {
+      span<uv::Buffer> bufs{m_bufs};
+      for (auto&& buf : bufs.subspan(0, m_startUser)) {
+        buf.Deallocate();
+      }
+      m_callback(bufs.subspan(m_startUser), err);
     });
   }
 
+  std::function<void(span<uv::Buffer>, uv::Error)> m_callback;
   SmallVector<uv::Buffer, 4> m_bufs;
   size_t m_startUser;
 };
@@ -44,9 +47,11 @@
     static std::default_random_engine gen{rd()};
     std::uniform_int_distribution<unsigned int> dist(0, 255);
     char nonce[16];  // the nonce sent to the server
-    for (char& v : nonce) v = static_cast<char>(dist(gen));
+    for (char& v : nonce) {
+      v = static_cast<char>(dist(gen));
+    }
     raw_svector_ostream os(key);
-    Base64Encode(os, StringRef{nonce, 16});
+    Base64Encode(os, {nonce, 16});
   }
   ~ClientHandshakeData() {
     if (auto t = timer.lock()) {
@@ -66,7 +71,8 @@
   std::weak_ptr<uv::Timer> timer;
 };
 
-static StringRef AcceptHash(StringRef key, SmallVectorImpl<char>& buf) {
+static std::string_view AcceptHash(std::string_view key,
+                                   SmallVectorImpl<char>& buf) {
   SHA1 hash;
   hash.Update(key);
   hash.Update("258EAFA5-E914-47DA-95CA-C5AB0DC85B11");
@@ -79,7 +85,7 @@
   // Connect closed and error signals to ourselves
   m_stream.closed.connect([this]() { SetClosed(1006, "handle closed"); });
   m_stream.error.connect([this](uv::Error err) {
-    Terminate(1006, "stream error: " + Twine(err.name()));
+    Terminate(1006, fmt::format("stream error: {}", err.name()));
   });
 
   // Start reading
@@ -91,11 +97,11 @@
       [this]() { Terminate(1006, "remote end closed connection"); });
 }
 
-WebSocket::~WebSocket() {}
+WebSocket::~WebSocket() = default;
 
 std::shared_ptr<WebSocket> WebSocket::CreateClient(
-    uv::Stream& stream, const Twine& uri, const Twine& host,
-    ArrayRef<StringRef> protocols, const ClientOptions& options) {
+    uv::Stream& stream, std::string_view uri, std::string_view host,
+    span<const std::string_view> protocols, const ClientOptions& options) {
   auto ws = std::make_shared<WebSocket>(stream, false, private_init{});
   stream.SetData(ws);
   ws->StartClient(uri, host, protocols, options);
@@ -103,35 +109,41 @@
 }
 
 std::shared_ptr<WebSocket> WebSocket::CreateServer(uv::Stream& stream,
-                                                   StringRef key,
-                                                   StringRef version,
-                                                   StringRef protocol) {
+                                                   std::string_view key,
+                                                   std::string_view version,
+                                                   std::string_view protocol) {
   auto ws = std::make_shared<WebSocket>(stream, true, private_init{});
   stream.SetData(ws);
   ws->StartServer(key, version, protocol);
   return ws;
 }
 
-void WebSocket::Close(uint16_t code, const Twine& reason) {
+void WebSocket::Close(uint16_t code, std::string_view reason) {
   SendClose(code, reason);
-  if (m_state != FAILED && m_state != CLOSED) m_state = CLOSING;
+  if (m_state != FAILED && m_state != CLOSED) {
+    m_state = CLOSING;
+  }
 }
 
-void WebSocket::Fail(uint16_t code, const Twine& reason) {
-  if (m_state == FAILED || m_state == CLOSED) return;
+void WebSocket::Fail(uint16_t code, std::string_view reason) {
+  if (m_state == FAILED || m_state == CLOSED) {
+    return;
+  }
   SendClose(code, reason);
   SetClosed(code, reason, true);
   Shutdown();
 }
 
-void WebSocket::Terminate(uint16_t code, const Twine& reason) {
-  if (m_state == FAILED || m_state == CLOSED) return;
+void WebSocket::Terminate(uint16_t code, std::string_view reason) {
+  if (m_state == FAILED || m_state == CLOSED) {
+    return;
+  }
   SetClosed(code, reason);
   Shutdown();
 }
 
-void WebSocket::StartClient(const Twine& uri, const Twine& host,
-                            ArrayRef<StringRef> protocols,
+void WebSocket::StartClient(std::string_view uri, std::string_view host,
+                            span<const std::string_view> protocols,
                             const ClientOptions& options) {
   // Create client handshake data
   m_clientHandshake = std::make_unique<ClientHandshakeData>();
@@ -152,10 +164,11 @@
     os << "Sec-WebSocket-Protocol: ";
     bool first = true;
     for (auto protocol : protocols) {
-      if (!first)
+      if (!first) {
         os << ", ";
-      else
+      } else {
         first = false;
+      }
       os << protocol;
       // also save for later checking against server response
       m_clientHandshake->protocols.emplace_back(protocol);
@@ -164,52 +177,64 @@
   }
 
   // other headers
-  for (auto&& header : options.extraHeaders)
+  for (auto&& header : options.extraHeaders) {
     os << header.first << ": " << header.second << "\r\n";
+  }
 
   // finish headers
   os << "\r\n";
 
   // Send client request
   m_stream.Write(bufs, [](auto bufs, uv::Error) {
-    for (auto& buf : bufs) buf.Deallocate();
+    for (auto& buf : bufs) {
+      buf.Deallocate();
+    }
   });
 
   // Set up client response handling
-  m_clientHandshake->parser.status.connect([this](StringRef status) {
+  m_clientHandshake->parser.status.connect([this](std::string_view status) {
     unsigned int code = m_clientHandshake->parser.GetStatusCode();
-    if (code != 101) Terminate(code, status);
+    if (code != 101) {
+      Terminate(code, status);
+    }
   });
   m_clientHandshake->parser.header.connect(
-      [this](StringRef name, StringRef value) {
-        value = value.trim();
-        if (name.equals_lower("upgrade")) {
-          if (!value.equals_lower("websocket"))
+      [this](std::string_view name, std::string_view value) {
+        value = trim(value);
+        if (equals_lower(name, "upgrade")) {
+          if (!equals_lower(value, "websocket")) {
             return Terminate(1002, "invalid upgrade response value");
+          }
           m_clientHandshake->hasUpgrade = true;
-        } else if (name.equals_lower("connection")) {
-          if (!value.equals_lower("upgrade"))
+        } else if (equals_lower(name, "connection")) {
+          if (!equals_lower(value, "upgrade")) {
             return Terminate(1002, "invalid connection response value");
+          }
           m_clientHandshake->hasConnection = true;
-        } else if (name.equals_lower("sec-websocket-accept")) {
+        } else if (equals_lower(name, "sec-websocket-accept")) {
           // Check against expected response
           SmallString<64> acceptBuf;
-          if (!value.equals(AcceptHash(m_clientHandshake->key, acceptBuf)))
+          if (!equals(value, AcceptHash(m_clientHandshake->key, acceptBuf))) {
             return Terminate(1002, "invalid accept key");
+          }
           m_clientHandshake->hasAccept = true;
-        } else if (name.equals_lower("sec-websocket-extensions")) {
+        } else if (equals_lower(name, "sec-websocket-extensions")) {
           // No extensions are supported
-          if (!value.empty()) return Terminate(1010, "unsupported extension");
-        } else if (name.equals_lower("sec-websocket-protocol")) {
+          if (!value.empty()) {
+            return Terminate(1010, "unsupported extension");
+          }
+        } else if (equals_lower(name, "sec-websocket-protocol")) {
           // Make sure it was one of the provided protocols
           bool match = false;
           for (auto&& protocol : m_clientHandshake->protocols) {
-            if (value.equals_lower(protocol)) {
+            if (equals_lower(value, protocol)) {
               match = true;
               break;
             }
           }
-          if (!match) return Terminate(1003, "unsupported protocol");
+          if (!match) {
+            return Terminate(1003, "unsupported protocol");
+          }
           m_clientHandshake->hasProtocol = true;
           m_protocol = value;
         }
@@ -237,8 +262,8 @@
   }
 }
 
-void WebSocket::StartServer(StringRef key, StringRef version,
-                            StringRef protocol) {
+void WebSocket::StartServer(std::string_view key, std::string_view version,
+                            std::string_view protocol) {
   m_protocol = protocol;
 
   // Build server response
@@ -251,7 +276,9 @@
     os << "Upgrade: WebSocket\r\n";
     os << "Sec-WebSocket-Version: 13\r\n\r\n";
     m_stream.Write(bufs, [this](auto bufs, uv::Error) {
-      for (auto& buf : bufs) buf.Deallocate();
+      for (auto& buf : bufs) {
+        buf.Deallocate();
+      }
       // XXX: Should we support sending a new handshake on the same connection?
       // XXX: "this->" is required by GCC 5.5 (bug)
       this->Terminate(1003, "unsupported protocol version");
@@ -267,14 +294,18 @@
   SmallString<64> acceptBuf;
   os << "Sec-WebSocket-Accept: " << AcceptHash(key, acceptBuf) << "\r\n";
 
-  if (!protocol.empty()) os << "Sec-WebSocket-Protocol: " << protocol << "\r\n";
+  if (!protocol.empty()) {
+    os << "Sec-WebSocket-Protocol: " << protocol << "\r\n";
+  }
 
   // end headers
   os << "\r\n";
 
   // Send server response
   m_stream.Write(bufs, [this](auto bufs, uv::Error) {
-    for (auto& buf : bufs) buf.Deallocate();
+    for (auto& buf : bufs) {
+      buf.Deallocate();
+    }
     if (m_state == CONNECTING) {
       m_state = OPEN;
       open(m_protocol);
@@ -282,25 +313,28 @@
   });
 }
 
-void WebSocket::SendClose(uint16_t code, const Twine& reason) {
+void WebSocket::SendClose(uint16_t code, std::string_view reason) {
   SmallVector<uv::Buffer, 4> bufs;
   if (code != 1005) {
     raw_uv_ostream os{bufs, 4096};
     const uint8_t codeMsb[] = {static_cast<uint8_t>((code >> 8) & 0xff),
                                static_cast<uint8_t>(code & 0xff)};
-    os << ArrayRef<uint8_t>(codeMsb);
-    reason.print(os);
+    os << span{codeMsb};
+    os << reason;
   }
   Send(kFlagFin | kOpClose, bufs, [](auto bufs, uv::Error) {
-    for (auto&& buf : bufs) buf.Deallocate();
+    for (auto&& buf : bufs) {
+      buf.Deallocate();
+    }
   });
 }
 
-void WebSocket::SetClosed(uint16_t code, const Twine& reason, bool failed) {
-  if (m_state == FAILED || m_state == CLOSED) return;
+void WebSocket::SetClosed(uint16_t code, std::string_view reason, bool failed) {
+  if (m_state == FAILED || m_state == CLOSED) {
+    return;
+  }
   m_state = failed ? FAILED : CLOSED;
-  SmallString<64> reasonBuf;
-  closed(code, reason.toStringRef(reasonBuf));
+  closed(code, reason);
 }
 
 void WebSocket::Shutdown() {
@@ -309,18 +343,23 @@
 
 void WebSocket::HandleIncoming(uv::Buffer& buf, size_t size) {
   // ignore incoming data if we're failed or closed
-  if (m_state == FAILED || m_state == CLOSED) return;
+  if (m_state == FAILED || m_state == CLOSED) {
+    return;
+  }
 
-  StringRef data{buf.base, size};
+  std::string_view data{buf.base, size};
 
   // Handle connecting state (mainly on client)
   if (m_state == CONNECTING) {
     if (m_clientHandshake) {
       data = m_clientHandshake->parser.Execute(data);
       // check for parser failure
-      if (m_clientHandshake->parser.HasError())
+      if (m_clientHandshake->parser.HasError()) {
         return Terminate(1003, "invalid response");
-      if (m_state != OPEN) return;  // not done with handshake yet
+      }
+      if (m_state != OPEN) {
+        return;  // not done with handshake yet
+      }
 
       // we're done with the handshake, so release its memory
       m_clientHandshake.reset();
@@ -337,45 +376,58 @@
       // Need at least two bytes to determine header length
       if (m_header.size() < 2u) {
         size_t toCopy = (std::min)(2u - m_header.size(), data.size());
-        m_header.append(data.bytes_begin(), data.bytes_begin() + toCopy);
-        data = data.drop_front(toCopy);
-        if (m_header.size() < 2u) return;  // need more data
+        m_header.append(data.data(), data.data() + toCopy);
+        data.remove_prefix(toCopy);
+        if (m_header.size() < 2u) {
+          return;  // need more data
+        }
 
         // Validate RSV bits are zero
-        if ((m_header[0] & 0x70) != 0) return Fail(1002, "nonzero RSV");
+        if ((m_header[0] & 0x70) != 0) {
+          return Fail(1002, "nonzero RSV");
+        }
       }
 
       // Once we have first two bytes, we can calculate the header size
       if (m_headerSize == 0) {
         m_headerSize = 2;
         uint8_t len = m_header[1] & kLenMask;
-        if (len == 126)
+        if (len == 126) {
           m_headerSize += 2;
-        else if (len == 127)
+        } else if (len == 127) {
           m_headerSize += 8;
+        }
         bool masking = (m_header[1] & kFlagMasking) != 0;
-        if (masking) m_headerSize += 4;  // masking key
+        if (masking) {
+          m_headerSize += 4;  // masking key
+        }
         // On server side, incoming messages MUST be masked
         // On client side, incoming messages MUST NOT be masked
-        if (m_server && !masking) return Fail(1002, "client data not masked");
-        if (!m_server && masking) return Fail(1002, "server data masked");
+        if (m_server && !masking) {
+          return Fail(1002, "client data not masked");
+        }
+        if (!m_server && masking) {
+          return Fail(1002, "server data masked");
+        }
       }
 
       // Need to complete header to calculate message size
       if (m_header.size() < m_headerSize) {
         size_t toCopy = (std::min)(m_headerSize - m_header.size(), data.size());
-        m_header.append(data.bytes_begin(), data.bytes_begin() + toCopy);
-        data = data.drop_front(toCopy);
-        if (m_header.size() < m_headerSize) return;  // need more data
+        m_header.append(data.data(), data.data() + toCopy);
+        data.remove_prefix(toCopy);
+        if (m_header.size() < m_headerSize) {
+          return;  // need more data
+        }
       }
 
       if (m_header.size() >= m_headerSize) {
         // get payload length
         uint8_t len = m_header[1] & kLenMask;
-        if (len == 126)
+        if (len == 126) {
           m_frameSize = (static_cast<uint16_t>(m_header[2]) << 8) |
                         static_cast<uint16_t>(m_header[3]);
-        else if (len == 127)
+        } else if (len == 127) {
           m_frameSize = (static_cast<uint64_t>(m_header[2]) << 56) |
                         (static_cast<uint64_t>(m_header[3]) << 48) |
                         (static_cast<uint64_t>(m_header[4]) << 40) |
@@ -384,20 +436,22 @@
                         (static_cast<uint64_t>(m_header[7]) << 16) |
                         (static_cast<uint64_t>(m_header[8]) << 8) |
                         static_cast<uint64_t>(m_header[9]);
-        else
+        } else {
           m_frameSize = len;
+        }
 
         // limit maximum size
-        if ((m_payload.size() + m_frameSize) > m_maxMessageSize)
+        if ((m_payload.size() + m_frameSize) > m_maxMessageSize) {
           return Fail(1009, "message too large");
+        }
       }
     }
 
     if (m_frameSize != UINT64_MAX) {
       size_t need = m_frameStart + m_frameSize - m_payload.size();
       size_t toCopy = (std::min)(need, data.size());
-      m_payload.append(data.bytes_begin(), data.bytes_begin() + toCopy);
-      data = data.drop_front(toCopy);
+      m_payload.append(data.data(), data.data() + toCopy);
+      data.remove_prefix(toCopy);
       need -= toCopy;
       if (need == 0) {
         // We have a complete frame
@@ -407,10 +461,11 @@
               m_header[m_headerSize - 4], m_header[m_headerSize - 3],
               m_header[m_headerSize - 2], m_header[m_headerSize - 1]};
           int n = 0;
-          for (uint8_t& ch :
-               MutableArrayRef<uint8_t>{m_payload}.slice(m_frameStart)) {
+          for (uint8_t& ch : span{m_payload}.subspan(m_frameStart)) {
             ch ^= key[n++];
-            if (n >= 4) n = 0;
+            if (n >= 4) {
+              n = 0;
+            }
           }
         }
 
@@ -421,36 +476,53 @@
           case kOpCont:
             switch (m_fragmentOpcode) {
               case kOpText:
-                if (!m_combineFragments || fin)
-                  text(StringRef{reinterpret_cast<char*>(m_payload.data()),
-                                 m_payload.size()},
+                if (!m_combineFragments || fin) {
+                  text(std::string_view{reinterpret_cast<char*>(
+                                            m_payload.data()),
+                                        m_payload.size()},
                        fin);
+                }
                 break;
               case kOpBinary:
-                if (!m_combineFragments || fin) binary(m_payload, fin);
+                if (!m_combineFragments || fin) {
+                  binary(m_payload, fin);
+                }
                 break;
               default:
                 // no preceding message?
                 return Fail(1002, "invalid continuation message");
             }
-            if (fin) m_fragmentOpcode = 0;
+            if (fin) {
+              m_fragmentOpcode = 0;
+            }
             break;
           case kOpText:
-            if (m_fragmentOpcode != 0) return Fail(1002, "incomplete fragment");
-            if (!m_combineFragments || fin)
-              text(StringRef{reinterpret_cast<char*>(m_payload.data()),
-                             m_payload.size()},
+            if (m_fragmentOpcode != 0) {
+              return Fail(1002, "incomplete fragment");
+            }
+            if (!m_combineFragments || fin) {
+              text(std::string_view{reinterpret_cast<char*>(m_payload.data()),
+                                    m_payload.size()},
                    fin);
-            if (!fin) m_fragmentOpcode = opcode;
+            }
+            if (!fin) {
+              m_fragmentOpcode = opcode;
+            }
             break;
           case kOpBinary:
-            if (m_fragmentOpcode != 0) return Fail(1002, "incomplete fragment");
-            if (!m_combineFragments || fin) binary(m_payload, fin);
-            if (!fin) m_fragmentOpcode = opcode;
+            if (m_fragmentOpcode != 0) {
+              return Fail(1002, "incomplete fragment");
+            }
+            if (!m_combineFragments || fin) {
+              binary(m_payload, fin);
+            }
+            if (!fin) {
+              m_fragmentOpcode = opcode;
+            }
             break;
           case kOpClose: {
             uint16_t code;
-            StringRef reason;
+            std::string_view reason;
             if (!fin) {
               code = 1002;
               reason = "cannot fragment control frames";
@@ -459,23 +531,31 @@
             } else {
               code = (static_cast<uint16_t>(m_payload[0]) << 8) |
                      static_cast<uint16_t>(m_payload[1]);
-              reason = StringRef{reinterpret_cast<char*>(m_payload.data()),
-                                 m_payload.size()}
-                           .drop_front(2);
+              reason = drop_front(
+                  {reinterpret_cast<char*>(m_payload.data()), m_payload.size()},
+                  2);
             }
             // Echo the close if we didn't previously send it
-            if (m_state != CLOSING) SendClose(code, reason);
+            if (m_state != CLOSING) {
+              SendClose(code, reason);
+            }
             SetClosed(code, reason);
             // If we're the server, shutdown the connection.
-            if (m_server) Shutdown();
+            if (m_server) {
+              Shutdown();
+            }
             break;
           }
           case kOpPing:
-            if (!fin) return Fail(1002, "cannot fragment control frames");
+            if (!fin) {
+              return Fail(1002, "cannot fragment control frames");
+            }
             ping(m_payload);
             break;
           case kOpPong:
-            if (!fin) return Fail(1002, "cannot fragment control frames");
+            if (!fin) {
+              return Fail(1002, "cannot fragment control frames");
+            }
             pong(m_payload);
             break;
           default:
@@ -485,7 +565,9 @@
         // Prepare for next message
         m_header.clear();
         m_headerSize = 0;
-        if (!m_combineFragments || fin) m_payload.clear();
+        if (!m_combineFragments || fin) {
+          m_payload.clear();
+        }
         m_frameStart = m_payload.size();
         m_frameSize = UINT64_MAX;
       }
@@ -494,21 +576,22 @@
 }
 
 void WebSocket::Send(
-    uint8_t opcode, ArrayRef<uv::Buffer> data,
-    std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
+    uint8_t opcode, span<const uv::Buffer> data,
+    std::function<void(span<uv::Buffer>, uv::Error)> callback) {
   // If we're not open, emit an error and don't send the data
   if (m_state != OPEN) {
     int err;
-    if (m_state == CONNECTING)
+    if (m_state == CONNECTING) {
       err = UV_EAGAIN;
-    else
+    } else {
       err = UV_ESHUTDOWN;
+    }
     SmallVector<uv::Buffer, 4> bufs{data.begin(), data.end()};
     callback(bufs, uv::Error{err});
     return;
   }
 
-  auto req = std::make_shared<WebSocketWriteReq>(callback);
+  auto req = std::make_shared<WebSocketWriteReq>(std::move(callback));
   raw_uv_ostream os{req->m_bufs, 4096};
 
   // opcode (includes FIN bit)
@@ -516,14 +599,16 @@
 
   // payload length
   uint64_t size = 0;
-  for (auto&& buf : data) size += buf.len;
+  for (auto&& buf : data) {
+    size += buf.len;
+  }
   if (size < 126) {
     os << static_cast<unsigned char>((m_server ? 0x00 : kFlagMasking) | size);
   } else if (size <= 0xffff) {
     os << static_cast<unsigned char>((m_server ? 0x00 : kFlagMasking) | 126);
     const uint8_t sizeMsb[] = {static_cast<uint8_t>((size >> 8) & 0xff),
                                static_cast<uint8_t>(size & 0xff)};
-    os << ArrayRef<uint8_t>(sizeMsb);
+    os << span{sizeMsb};
   } else {
     os << static_cast<unsigned char>((m_server ? 0x00 : kFlagMasking) | 127);
     const uint8_t sizeMsb[] = {static_cast<uint8_t>((size >> 56) & 0xff),
@@ -534,7 +619,7 @@
                                static_cast<uint8_t>((size >> 16) & 0xff),
                                static_cast<uint8_t>((size >> 8) & 0xff),
                                static_cast<uint8_t>(size & 0xff)};
-    os << ArrayRef<uint8_t>(sizeMsb);
+    os << span{sizeMsb};
   }
 
   // clients need to mask the input data
@@ -544,21 +629,24 @@
     static std::default_random_engine gen{rd()};
     std::uniform_int_distribution<unsigned int> dist(0, 255);
     uint8_t key[4];
-    for (uint8_t& v : key) v = dist(gen);
-    os << ArrayRef<uint8_t>{key, 4};
+    for (uint8_t& v : key) {
+      v = dist(gen);
+    }
+    os << span<const uint8_t>{key, 4};
     // copy and mask data
     int n = 0;
     for (auto&& buf : data) {
       for (auto&& ch : buf.data()) {
         os << static_cast<unsigned char>(static_cast<uint8_t>(ch) ^ key[n++]);
-        if (n >= 4) n = 0;
+        if (n >= 4) {
+          n = 0;
+        }
       }
     }
     req->m_startUser = req->m_bufs.size();
     req->m_bufs.append(data.begin(), data.end());
     // don't send the user bufs as we copied their data
-    m_stream.Write(ArrayRef<uv::Buffer>{req->m_bufs}.slice(0, req->m_startUser),
-                   req);
+    m_stream.Write(span{req->m_bufs}.subspan(0, req->m_startUser), req);
   } else {
     // servers can just send the buffers directly without masking
     req->m_startUser = req->m_bufs.size();
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocketServer.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocketServer.cpp
index 056cb62..1562f3b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocketServer.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/WebSocketServer.cpp
@@ -1,12 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WebSocketServer.h"
 
+#include <utility>
+
+#include "wpi/StringExtras.h"
+#include "wpi/fmt/raw_ostream.h"
 #include "wpi/raw_uv_ostream.h"
 #include "wpi/uv/Buffer.h"
 #include "wpi/uv/Stream.h"
@@ -14,76 +15,92 @@
 using namespace wpi;
 
 WebSocketServerHelper::WebSocketServerHelper(HttpParser& req) {
-  req.header.connect([this](StringRef name, StringRef value) {
-    if (name.equals_lower("host")) {
+  req.header.connect([this](std::string_view name, std::string_view value) {
+    if (equals_lower(name, "host")) {
       m_gotHost = true;
-    } else if (name.equals_lower("upgrade")) {
-      if (value.equals_lower("websocket")) m_websocket = true;
-    } else if (name.equals_lower("sec-websocket-key")) {
+    } else if (equals_lower(name, "upgrade")) {
+      if (equals_lower(value, "websocket")) {
+        m_websocket = true;
+      }
+    } else if (equals_lower(name, "sec-websocket-key")) {
       m_key = value;
-    } else if (name.equals_lower("sec-websocket-version")) {
+    } else if (equals_lower(name, "sec-websocket-version")) {
       m_version = value;
-    } else if (name.equals_lower("sec-websocket-protocol")) {
+    } else if (equals_lower(name, "sec-websocket-protocol")) {
       // Protocols are comma delimited, repeated headers add to list
-      SmallVector<StringRef, 2> protocols;
-      value.split(protocols, ",", -1, false);
+      SmallVector<std::string_view, 2> protocols;
+      split(value, protocols, ",", -1, false);
       for (auto protocol : protocols) {
-        protocol = protocol.trim();
-        if (!protocol.empty()) m_protocols.emplace_back(protocol);
+        protocol = trim(protocol);
+        if (!protocol.empty()) {
+          m_protocols.emplace_back(protocol);
+        }
       }
     }
   });
   req.headersComplete.connect([&req, this](bool) {
-    if (req.IsUpgrade() && IsUpgrade()) upgrade();
+    if (req.IsUpgrade() && IsUpgrade()) {
+      upgrade();
+    }
   });
 }
 
-std::pair<bool, StringRef> WebSocketServerHelper::MatchProtocol(
-    ArrayRef<StringRef> protocols) {
-  if (protocols.empty() && m_protocols.empty())
-    return std::make_pair(true, StringRef{});
+std::pair<bool, std::string_view> WebSocketServerHelper::MatchProtocol(
+    span<const std::string_view> protocols) {
+  if (protocols.empty() && m_protocols.empty()) {
+    return {true, {}};
+  }
   for (auto protocol : protocols) {
     for (auto&& clientProto : m_protocols) {
-      if (protocol == clientProto) return std::make_pair(true, protocol);
+      if (protocol == clientProto) {
+        return {true, protocol};
+      }
     }
   }
-  return std::make_pair(false, StringRef{});
+  return {false, {}};
 }
 
 WebSocketServer::WebSocketServer(uv::Stream& stream,
-                                 ArrayRef<StringRef> protocols,
-                                 const ServerOptions& options,
-                                 const private_init&)
+                                 span<const std::string_view> protocols,
+                                 ServerOptions options, const private_init&)
     : m_stream{stream},
       m_helper{m_req},
       m_protocols{protocols.begin(), protocols.end()},
-      m_options{options} {
+      m_options{std::move(options)} {
   // Header handling
-  m_req.header.connect([this](StringRef name, StringRef value) {
-    if (name.equals_lower("host")) {
+  m_req.header.connect([this](std::string_view name, std::string_view value) {
+    if (equals_lower(name, "host")) {
       if (m_options.checkHost) {
-        if (!m_options.checkHost(value)) Abort(401, "Unrecognized Host");
+        if (!m_options.checkHost(value)) {
+          Abort(401, "Unrecognized Host");
+        }
       }
     }
   });
-  m_req.url.connect([this](StringRef name) {
+  m_req.url.connect([this](std::string_view name) {
     if (m_options.checkUrl) {
-      if (!m_options.checkUrl(name)) Abort(404, "Not Found");
+      if (!m_options.checkUrl(name)) {
+        Abort(404, "Not Found");
+      }
     }
   });
   m_req.headersComplete.connect([this](bool) {
     // We only accept websocket connections
-    if (!m_helper.IsUpgrade() || !m_req.IsUpgrade())
+    if (!m_helper.IsUpgrade() || !m_req.IsUpgrade()) {
       Abort(426, "Upgrade Required");
+    }
   });
 
   // Handle upgrade event
   m_helper.upgrade.connect([this] {
-    if (m_aborted) return;
+    if (m_aborted) {
+      return;
+    }
 
     // Negotiate sub-protocol
-    SmallVector<StringRef, 2> protocols{m_protocols.begin(), m_protocols.end()};
-    StringRef protocol = m_helper.MatchProtocol(protocols).second;
+    SmallVector<std::string_view, 2> protocols{m_protocols.begin(),
+                                               m_protocols.end()};
+    std::string_view protocol = m_helper.MatchProtocol(protocols).second;
 
     // Disconnect our header reader
     m_dataConn.disconnect();
@@ -96,19 +113,24 @@
     auto ws = m_helper.Accept(m_stream, protocol);
 
     // Connect the websocket open event to our connected event.
-    ws->open.connect_extended([self, s = ws.get()](auto conn, StringRef) {
-      self->connected(self->m_req.GetUrl(), *s);
-      conn.disconnect();  // one-shot
-    });
+    ws->open.connect_extended(
+        [self, s = ws.get()](auto conn, std::string_view) {
+          self->connected(self->m_req.GetUrl(), *s);
+          conn.disconnect();  // one-shot
+        });
   });
 
   // Set up stream
   stream.StartRead();
   m_dataConn =
       stream.data.connect_connection([this](uv::Buffer& buf, size_t size) {
-        if (m_aborted) return;
-        m_req.Execute(StringRef{buf.base, size});
-        if (m_req.HasError()) Abort(400, "Bad Request");
+        if (m_aborted) {
+          return;
+        }
+        m_req.Execute(std::string_view{buf.base, size});
+        if (m_req.HasError()) {
+          Abort(400, "Bad Request");
+        }
       });
   m_errorConn =
       stream.error.connect_connection([this](uv::Error) { m_stream.Close(); });
@@ -116,7 +138,7 @@
 }
 
 std::shared_ptr<WebSocketServer> WebSocketServer::Create(
-    uv::Stream& stream, ArrayRef<StringRef> protocols,
+    uv::Stream& stream, span<const std::string_view> protocols,
     const ServerOptions& options) {
   auto server = std::make_shared<WebSocketServer>(stream, protocols, options,
                                                   private_init{});
@@ -124,8 +146,10 @@
   return server;
 }
 
-void WebSocketServer::Abort(uint16_t code, StringRef reason) {
-  if (m_aborted) return;
+void WebSocketServer::Abort(uint16_t code, std::string_view reason) {
+  if (m_aborted) {
+    return;
+  }
   m_aborted = true;
 
   // Build response
@@ -133,11 +157,15 @@
   raw_uv_ostream os{bufs, 1024};
 
   // Handle unsupported version
-  os << "HTTP/1.1 " << code << ' ' << reason << "\r\n";
-  if (code == 426) os << "Upgrade: WebSocket\r\n";
+  fmt::print(os, "HTTP/1.1 {} {}\r\n", code, reason);
+  if (code == 426) {
+    os << "Upgrade: WebSocket\r\n";
+  }
   os << "\r\n";
   m_stream.Write(bufs, [this](auto bufs, uv::Error) {
-    for (auto& buf : bufs) buf.Deallocate();
+    for (auto& buf : bufs) {
+      buf.Deallocate();
+    }
     m_stream.Shutdown([this] { m_stream.Close(); });
   });
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/fs.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/fs.cpp
new file mode 100644
index 0000000..34d6cca
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/fs.cpp
@@ -0,0 +1,326 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//===----------------------------------------------------------------------===//
+//
+//                     The LLVM Compiler Infrastructure
+//
+// This file is distributed under the University of Illinois Open Source
+// License. See LICENSE.TXT for details.
+//
+//===----------------------------------------------------------------------===//
+
+#include <cassert>
+
+#ifdef _WIN32
+#include <fcntl.h>
+#include <io.h>
+#include <sys/types.h>
+// Require at least Windows 7 API.
+#define _WIN32_WINNT 0x0601
+#define _WIN32_IE 0x0800  // MinGW at it again. FIXME: verify if still needed.
+#define WIN32_LEAN_AND_MEAN
+#ifndef NOMINMAX
+#define NOMINMAX
+#endif
+
+#define WIN32_NO_STATUS
+#include <windows.h>
+#undef WIN32_NO_STATUS
+#include <winternl.h>
+#include <ntstatus.h>
+
+#include <shellapi.h>
+#include <shlobj.h>
+
+#include "wpi/WindowsError.h"
+
+#else  // _WIN32
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#endif  // _WIN32
+
+#if defined(__APPLE__)
+#include <Availability.h>
+#endif
+#if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) \
+     || (defined(__cplusplus) && __cplusplus >= 201703L)) \
+    && defined(__has_include)
+#if __has_include(<filesystem>) \
+    && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) \
+        || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101500) \
+    && (defined(__clang__) || !defined(__GNUC__) || __GNUC__ >= 10 \
+        || (__GNUC__ >= 9 && __GNUC_MINOR__ >= 1))
+#define GHC_USE_STD_FS
+#endif
+#endif
+#ifndef GHC_USE_STD_FS
+// #define GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE
+#define GHC_FILESYSTEM_IMPLEMENTATION
+#include "wpi/ghc/filesystem.hpp"
+#endif
+
+#include "wpi/Errno.h"
+#include "wpi/ErrorHandling.h"
+#include "wpi/WindowsError.h"
+#include "wpi/fs.h"
+
+namespace fs {
+
+#ifdef _WIN32
+
+#ifdef _MSC_VER
+#pragma comment(lib, "shell32.lib")
+#pragma comment(lib, "ole32.lib")
+#pragma warning(push)
+#pragma warning(disable : 4244 4267 4146)
+#endif
+
+const file_t kInvalidFile = INVALID_HANDLE_VALUE;
+
+static DWORD nativeDisposition(CreationDisposition Disp, OpenFlags Flags) {
+  // This is a compatibility hack.  Really we should respect the creation
+  // disposition, but a lot of old code relied on the implicit assumption that
+  // OF_Append implied it would open an existing file.  Since the disposition is
+  // now explicit and defaults to CD_CreateAlways, this assumption would cause
+  // any usage of OF_Append to append to a new file, even if the file already
+  // existed.  A better solution might have two new creation dispositions:
+  // CD_AppendAlways and CD_AppendNew.  This would also address the problem of
+  // OF_Append being used on a read-only descriptor, which doesn't make sense.
+  if (Flags & OF_Append)
+    return OPEN_ALWAYS;
+
+  switch (Disp) {
+    case CD_CreateAlways:
+      return CREATE_ALWAYS;
+    case CD_CreateNew:
+      return CREATE_NEW;
+    case CD_OpenAlways:
+      return OPEN_ALWAYS;
+    case CD_OpenExisting:
+      return OPEN_EXISTING;
+  }
+  wpi_unreachable("unreachable!");
+}
+
+static DWORD nativeAccess(FileAccess Access, OpenFlags Flags) {
+  DWORD Result = 0;
+  if (Access & FA_Read)
+    Result |= GENERIC_READ;
+  if (Access & FA_Write)
+    Result |= GENERIC_WRITE;
+  if (Flags & OF_Delete)
+    Result |= DELETE;
+  if (Flags & OF_UpdateAtime)
+    Result |= FILE_WRITE_ATTRIBUTES;
+  return Result;
+}
+
+static file_t openFileInternal(const path& Path, std::error_code& EC,
+                               DWORD Disp, DWORD Access, DWORD Flags,
+                               bool Inherit = false) {
+  SECURITY_ATTRIBUTES SA;
+  SA.nLength = sizeof(SA);
+  SA.lpSecurityDescriptor = nullptr;
+  SA.bInheritHandle = Inherit;
+
+  HANDLE H =
+      ::CreateFileW(Path.c_str(), Access,
+                    FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, &SA,
+                    Disp, Flags, NULL);
+  if (H == INVALID_HANDLE_VALUE) {
+    DWORD LastError = ::GetLastError();
+    EC = wpi::mapWindowsError(LastError);
+    // Provide a better error message when trying to open directories.
+    // This only runs if we failed to open the file, so there is probably
+    // no performances issues.
+    if (LastError != ERROR_ACCESS_DENIED) {
+      return kInvalidFile;
+    }
+    if (is_directory(Path)) {
+      EC = std::make_error_code(std::errc::is_a_directory);
+    }
+    return kInvalidFile;
+  }
+  EC = std::error_code();
+  return H;
+}
+
+static std::error_code setDeleteDisposition(HANDLE Handle, bool Delete) {
+  FILE_DISPOSITION_INFO Disposition;
+  Disposition.DeleteFile = Delete;
+  if (!::SetFileInformationByHandle(Handle, FileDispositionInfo, &Disposition,
+                                    sizeof(Disposition)))
+    return wpi::mapWindowsError(::GetLastError());
+  return std::error_code();
+}
+
+file_t OpenFile(const path& Path, std::error_code& EC, CreationDisposition Disp,
+                FileAccess Access, OpenFlags Flags, unsigned Mode) {
+  // Verify that we don't have both "append" and "excl".
+  assert((!(Disp == CD_CreateNew) || !(Flags & OF_Append)) &&
+         "Cannot specify both 'CreateNew' and 'Append' file creation flags!");
+
+  DWORD NativeDisp = nativeDisposition(Disp, Flags);
+  DWORD NativeAccess = nativeAccess(Access, Flags);
+
+  bool Inherit = false;
+  if (Flags & OF_ChildInherit) {
+    Inherit = true;
+  }
+
+  file_t Result = openFileInternal(Path, EC, NativeDisp, NativeAccess,
+                                   FILE_ATTRIBUTE_NORMAL, Inherit);
+  if (EC) {
+    return Result;
+  }
+
+  if (Flags & OF_UpdateAtime) {
+    FILETIME FileTime;
+    SYSTEMTIME SystemTime;
+    ::GetSystemTime(&SystemTime);
+    if (::SystemTimeToFileTime(&SystemTime, &FileTime) == 0 ||
+        ::SetFileTime(Result, NULL, &FileTime, NULL) == 0) {
+      DWORD LastError = ::GetLastError();
+      ::CloseHandle(Result);
+      EC = wpi::mapWindowsError(LastError);
+      return kInvalidFile;
+    }
+  }
+
+  if (Flags & OF_Delete) {
+    if ((EC = setDeleteDisposition(Result, true))) {
+      ::CloseHandle(Result);
+      return kInvalidFile;
+    }
+  }
+  return Result;
+}
+
+file_t OpenFileForRead(const path& Path, std::error_code& EC, OpenFlags Flags) {
+  return OpenFile(Path, EC, CD_OpenExisting, FA_Read, Flags);
+}
+
+int FileToFd(file_t& F, std::error_code& EC, OpenFlags Flags) {
+  if (F == kInvalidFile) {
+    EC = wpi::mapWindowsError(ERROR_INVALID_HANDLE);
+    return -1;
+  }
+
+  int CrtOpenFlags = 0;
+  if (Flags & OF_Append) {
+    CrtOpenFlags |= _O_APPEND;
+  }
+
+  if (Flags & OF_Text) {
+    CrtOpenFlags |= _O_TEXT;
+  }
+
+  int ResultFD = ::_open_osfhandle(intptr_t(F), CrtOpenFlags);
+  if (ResultFD == -1) {
+    ::CloseHandle(F);
+    EC = wpi::mapWindowsError(ERROR_INVALID_HANDLE);
+    return -1;
+  }
+
+  EC = std::error_code();
+  F = kInvalidFile;
+  return ResultFD;
+}
+
+void CloseFile(file_t& F) {
+  ::CloseHandle(F);
+  F = kInvalidFile;
+}
+
+#else  // _WIN32
+
+const file_t kInvalidFile = -1;
+
+static int nativeOpenFlags(CreationDisposition Disp, OpenFlags Flags,
+                           FileAccess Access) {
+  int Result = 0;
+  if (Access == FA_Read) {
+    Result |= O_RDONLY;
+  } else if (Access == FA_Write) {
+    Result |= O_WRONLY;
+  } else if (Access == (FA_Read | FA_Write)) {
+    Result |= O_RDWR;
+  }
+
+  // This is for compatibility with old code that assumed F_Append implied
+  // would open an existing file.  See Windows/Path.inc for a longer comment.
+  if (Flags & F_Append) {
+    Disp = CD_OpenAlways;
+  }
+
+  if (Disp == CD_CreateNew) {
+    Result |= O_CREAT;  // Create if it doesn't exist.
+    Result |= O_EXCL;   // Fail if it does.
+  } else if (Disp == CD_CreateAlways) {
+    Result |= O_CREAT;  // Create if it doesn't exist.
+    Result |= O_TRUNC;  // Truncate if it does.
+  } else if (Disp == CD_OpenAlways) {
+    Result |= O_CREAT;  // Create if it doesn't exist.
+  } else if (Disp == CD_OpenExisting) {
+    // Nothing special, just don't add O_CREAT and we get these semantics.
+  }
+
+  if (Flags & F_Append) {
+    Result |= O_APPEND;
+  }
+
+#ifdef O_CLOEXEC
+  if (!(Flags & OF_ChildInherit)) {
+    Result |= O_CLOEXEC;
+  }
+#endif
+
+  return Result;
+}
+
+file_t OpenFile(const path& Path, std::error_code& EC, CreationDisposition Disp,
+                FileAccess Access, OpenFlags Flags, unsigned Mode) {
+  int OpenFlags = nativeOpenFlags(Disp, Flags, Access);
+  file_t ResultFD = kInvalidFile;
+
+  // Call ::open in a lambda to avoid overload resolution in RetryAfterSignal
+  // when open is overloaded, such as in Bionic.
+  auto Open = [&]() { return ::open(Path.c_str(), OpenFlags, Mode); };
+  if ((ResultFD = wpi::sys::RetryAfterSignal(-1, Open)) < 0) {
+    EC = std::error_code(errno, std::generic_category());
+    return kInvalidFile;
+  }
+#ifndef O_CLOEXEC
+  if (!(Flags & OF_ChildInherit)) {
+    int r = fcntl(ResultFD, F_SETFD, FD_CLOEXEC);
+    (void)r;
+    assert(r == 0 && "fcntl(F_SETFD, FD_CLOEXEC) failed");
+  }
+#endif
+  EC = std::error_code();
+  return ResultFD;
+}
+
+file_t OpenFileForRead(const path& Path, std::error_code& EC, OpenFlags Flags) {
+  return OpenFile(Path, EC, CD_OpenExisting, FA_Read, Flags, 0666);
+}
+
+int FileToFd(file_t& F, std::error_code& EC, OpenFlags Flags) {
+  int fd = F;
+  F = kInvalidFile;
+  EC = std::error_code();
+  return fd;
+}
+
+void CloseFile(file_t& F) {
+  ::close(F);
+  F = kInvalidFile;
+}
+
+#endif  // _WIN32
+
+}  // namespace fs
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/future.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/future.cpp
index 7ce0875..109eeb9 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/future.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/future.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/future.h"
 
@@ -28,10 +25,14 @@
 }
 
 bool PromiseFactoryBase::EraseRequest(uint64_t request) {
-  if (request == 0) return false;
+  if (request == 0) {
+    return false;
+  }
   auto it = std::find_if(m_requests.begin(), m_requests.end(),
                          [=](auto r) { return r == request; });
-  if (it == m_requests.end()) return false;  // no waiters
+  if (it == m_requests.end()) {
+    return false;  // no waiters
+  }
   m_requests.erase(it);
   return true;
 }
@@ -47,7 +48,9 @@
 
 void PromiseFactory<void>::SetValue(uint64_t request) {
   std::unique_lock lock(GetResultMutex());
-  if (!EraseRequest(request)) return;
+  if (!EraseRequest(request)) {
+    return;
+  }
   auto it = std::find_if(m_thens.begin(), m_thens.end(),
                          [=](const auto& x) { return x.request == request; });
   if (it != m_thens.end()) {
@@ -105,7 +108,9 @@
     // Did we get a response to *our* request?
     auto it = std::find_if(m_results.begin(), m_results.end(),
                            [=](const auto& r) { return r == request; });
-    if (it != m_results.end()) return;
+    if (it != m_results.end()) {
+      return;
+    }
     // No, keep waiting for a response
     Wait(lock);
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/hostname.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/hostname.cpp
index ffc5cad..d907023 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/hostname.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/hostname.cpp
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/hostname.h"
 
 #include <cstdlib>
 #include <string>
+#include <string_view>
 
 #include "uv.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
 
 namespace wpi {
 
@@ -27,14 +24,16 @@
   } else if (err == UV_ENOBUFS) {
     char* name2 = static_cast<char*>(std::malloc(size));
     err = uv_os_gethostname(name2, &size);
-    if (err == 0) rv.assign(name2, size);
+    if (err == 0) {
+      rv.assign(name2, size);
+    }
     std::free(name2);
   }
 
   return rv;
 }
 
-StringRef GetHostname(SmallVectorImpl<char>& name) {
+std::string_view GetHostname(SmallVectorImpl<char>& name) {
   // Use a tmp array to not require the SmallVector to be too large.
   char tmpName[256];
   size_t size = sizeof(tmpName);
@@ -47,10 +46,12 @@
   } else if (err == UV_ENOBUFS) {
     name.resize(size);
     err = uv_os_gethostname(name.data(), &size);
-    if (err != 0) size = 0;
+    if (err != 0) {
+      size = 0;
+    }
   }
 
-  return StringRef{name.data(), size};
+  return {name.data(), size};
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
index 90a2291..4fc5f45 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/jni/WPIUtilJNI.cpp
@@ -1,50 +1,94 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <jni.h>
 
-#include "edu_wpi_first_wpiutil_WPIUtilJNI.h"
+#include "edu_wpi_first_util_WPIUtilJNI.h"
 #include "wpi/PortForwarder.h"
+#include "wpi/Synchronization.h"
 #include "wpi/jni_util.h"
 #include "wpi/timestamp.h"
 
 using namespace wpi::java;
 
+static bool mockTimeEnabled = false;
+static uint64_t mockNow = 0;
+
+static JException interruptedEx;
+
 extern "C" {
 
 JNIEXPORT jint JNICALL JNI_OnLoad(JavaVM* vm, void* reserved) {
   JNIEnv* env;
-  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK)
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
     return JNI_ERR;
+  }
+
+  interruptedEx = JException(env, "java/lang/InterruptedException");
+  if (!interruptedEx) {
+    return JNI_ERR;
+  }
 
   return JNI_VERSION_1_6;
 }
 
-JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {}
+JNIEXPORT void JNICALL JNI_OnUnload(JavaVM* vm, void* reserved) {
+  JNIEnv* env;
+  if (vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6) != JNI_OK) {
+    return;
+  }
+  interruptedEx.free(env);
+}
 
 /*
- * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    enableMockTime
+ * Signature: ()V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_enableMockTime
+  (JNIEnv*, jclass)
+{
+  mockTimeEnabled = true;
+  wpi::SetNowImpl([] { return mockNow; });
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    setMockTime
+ * Signature: (J)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_setMockTime
+  (JNIEnv*, jclass, jlong time)
+{
+  mockNow = time;
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
  * Method:    now
  * Signature: ()J
  */
 JNIEXPORT jlong JNICALL
-Java_edu_wpi_first_wpiutil_WPIUtilJNI_now
+Java_edu_wpi_first_util_WPIUtilJNI_now
   (JNIEnv*, jclass)
 {
-  return wpi::Now();
+  if (mockTimeEnabled) {
+    return mockNow;
+  } else {
+    return wpi::Now();
+  }
 }
 
 /*
- * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
+ * Class:     edu_wpi_first_util_WPIUtilJNI
  * Method:    addPortForwarder
  * Signature: (ILjava/lang/String;I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_wpiutil_WPIUtilJNI_addPortForwarder
+Java_edu_wpi_first_util_WPIUtilJNI_addPortForwarder
   (JNIEnv* env, jclass, jint port, jstring remoteHost, jint remotePort)
 {
   wpi::PortForwarder::GetInstance().Add(static_cast<unsigned int>(port),
@@ -53,15 +97,180 @@
 }
 
 /*
- * Class:     edu_wpi_first_wpiutil_WPIUtilJNI
+ * Class:     edu_wpi_first_util_WPIUtilJNI
  * Method:    removePortForwarder
  * Signature: (I)V
  */
 JNIEXPORT void JNICALL
-Java_edu_wpi_first_wpiutil_WPIUtilJNI_removePortForwarder
+Java_edu_wpi_first_util_WPIUtilJNI_removePortForwarder
   (JNIEnv* env, jclass, jint port)
 {
   wpi::PortForwarder::GetInstance().Remove(port);
 }
 
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    createEvent
+ * Signature: (ZZ)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_createEvent
+  (JNIEnv*, jclass, jboolean manualReset, jboolean initialState)
+{
+  return wpi::CreateEvent(manualReset, initialState);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    destroyEvent
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_destroyEvent
+  (JNIEnv*, jclass, jint eventHandle)
+{
+  wpi::DestroyEvent(eventHandle);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    setEvent
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_setEvent
+  (JNIEnv*, jclass, jint eventHandle)
+{
+  wpi::SetEvent(eventHandle);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    resetEvent
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_resetEvent
+  (JNIEnv*, jclass, jint eventHandle)
+{
+  wpi::ResetEvent(eventHandle);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    createSemaphore
+ * Signature: (II)I
+ */
+JNIEXPORT jint JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_createSemaphore
+  (JNIEnv*, jclass, jint initialCount, jint maximumCount)
+{
+  return wpi::CreateSemaphore(initialCount, maximumCount);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    destroySemaphore
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_destroySemaphore
+  (JNIEnv*, jclass, jint semHandle)
+{
+  wpi::DestroySemaphore(semHandle);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    releaseSemaphore
+ * Signature: (II)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_releaseSemaphore
+  (JNIEnv*, jclass, jint semHandle, jint releaseCount)
+{
+  return wpi::ReleaseSemaphore(semHandle, releaseCount);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    waitForObject
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_waitForObject
+  (JNIEnv* env, jclass, jint handle)
+{
+  if (!wpi::WaitForObject(handle)) {
+    interruptedEx.Throw(env, "WaitForObject interrupted");
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    waitForObjectTimeout
+ * Signature: (ID)Z
+ */
+JNIEXPORT jboolean JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_waitForObjectTimeout
+  (JNIEnv* env, jclass, jint handle, jdouble timeout)
+{
+  bool timedOut;
+  if (!wpi::WaitForObject(handle, timeout, &timedOut) && !timedOut) {
+    interruptedEx.Throw(env, "WaitForObject interrupted");
+    return false;
+  }
+  return timedOut;
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    waitForObjects
+ * Signature: ([I)[I
+ */
+JNIEXPORT jintArray JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_waitForObjects
+  (JNIEnv* env, jclass, jintArray handles)
+{
+  JIntArrayRef handlesArr{env, handles};
+  wpi::SmallVector<WPI_Handle, 8> signaledBuf;
+  signaledBuf.resize(handlesArr.size());
+  wpi::span<const WPI_Handle> handlesArr2{
+      reinterpret_cast<const WPI_Handle*>(handlesArr.array().data()),
+      handlesArr.size()};
+
+  auto signaled = wpi::WaitForObjects(handlesArr2, signaledBuf);
+  if (signaled.empty()) {
+    interruptedEx.Throw(env, "WaitForObjects interrupted");
+    return nullptr;
+  }
+  return MakeJIntArray(env, signaled);
+}
+
+/*
+ * Class:     edu_wpi_first_util_WPIUtilJNI
+ * Method:    waitForObjectsTimeout
+ * Signature: ([ID)[I
+ */
+JNIEXPORT jintArray JNICALL
+Java_edu_wpi_first_util_WPIUtilJNI_waitForObjectsTimeout
+  (JNIEnv* env, jclass, jintArray handles, jdouble timeout)
+{
+  JIntArrayRef handlesArr{env, handles};
+  wpi::SmallVector<WPI_Handle, 8> signaledBuf;
+  signaledBuf.resize(handlesArr.size());
+  wpi::span<const WPI_Handle> handlesArr2{
+      reinterpret_cast<const WPI_Handle*>(handlesArr.array().data()),
+      handlesArr.size()};
+
+  bool timedOut;
+  auto signaled =
+      wpi::WaitForObjects(handlesArr2, signaledBuf, timeout, &timedOut);
+  if (signaled.empty() && !timedOut) {
+    interruptedEx.Throw(env, "WaitForObjects interrupted");
+    return nullptr;
+  }
+  return MakeJIntArray(env, signaled);
+}
+
 }  // extern "C"
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/json.cpp
index ba9cd8e..f1cfb14 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/json.cpp
@@ -34,39 +34,51 @@
 #define WPI_JSON_IMPLEMENTATION
 #include "wpi/json.h"
 
+#include "fmt/format.h"
 #include "wpi/raw_ostream.h"
 
 namespace wpi {
 namespace detail {
 
-exception::exception(int id_, const Twine& what_arg)
-    : id(id_), m(what_arg.str()) {}
+exception::exception(int id_, std::string_view what_arg)
+    : id(id_), m(std::string{what_arg}) {}
 
-parse_error parse_error::create(int id_, std::size_t byte_, const Twine& what_arg)
+parse_error parse_error::create(int id_, std::size_t byte_, std::string_view what_arg)
 {
-    return parse_error(id_, byte_, "[json.exception.parse_error." + Twine(id_) + "] parse error" +
-                    (byte_ != 0 ? (" at " + Twine(byte_)) : Twine("")) +
-                    ": " + what_arg);
+    if (byte_ != 0)
+        return parse_error(id_, byte_, fmt::format("[json.exception.parse_error.{}] parse error at {}: {}", id_, byte_, what_arg));
+    else
+        return parse_error(id_, byte_, fmt::format("[json.exception.parse_error.{}] parse error: {}", id_, what_arg));
 }
 
-invalid_iterator invalid_iterator::create(int id_, const Twine& what_arg)
+invalid_iterator invalid_iterator::create(int id_, std::string_view what_arg)
 {
-    return invalid_iterator(id_, "[json.exception.invalid_iterator." + Twine(id_) + "] " + what_arg);
+    return invalid_iterator(id_, fmt::format("[json.exception.invalid_iterator.{}] {}", id_, what_arg));
 }
 
-type_error type_error::create(int id_, const Twine& what_arg)
+invalid_iterator invalid_iterator::create(int id_, std::string_view what_arg, std::string_view type_info)
 {
-    return type_error(id_, "[json.exception.type_error." + Twine(id_) + "] " + what_arg);
+    return invalid_iterator(id_, fmt::format("[json.exception.invalid_iterator.{}] {} {}", id_, what_arg, type_info));
 }
 
-out_of_range out_of_range::create(int id_, const Twine& what_arg)
+type_error type_error::create(int id_, std::string_view what_arg)
 {
-    return out_of_range(id_, "[json.exception.out_of_range." + Twine(id_) + "] " + what_arg);
+    return type_error(id_, fmt::format("[json.exception.type_error.{}] {}", id_, what_arg));
 }
 
-other_error other_error::create(int id_, const Twine& what_arg)
+type_error type_error::create(int id_, std::string_view what_arg, std::string_view type_info)
 {
-    return other_error(id_, "[json.exception.other_error." + Twine(id_) + "] " + what_arg);
+    return type_error(id_, fmt::format("[json.exception.type_error.{}] {} {}", id_, what_arg, type_info));
+}
+
+out_of_range out_of_range::create(int id_, std::string_view what_arg)
+{
+    return out_of_range(id_, fmt::format("[json.exception.out_of_range.{}] {}", id_, what_arg));
+}
+
+other_error other_error::create(int id_, std::string_view what_arg)
+{
+    return other_error(id_, fmt::format("[json.exception.other_error.{}] {}", id_, what_arg));
 }
 
 }  // namespace detail
@@ -353,12 +365,12 @@
         JSON_CATCH (std::out_of_range&)
         {
             // create better exception explanation
-            JSON_THROW(out_of_range::create(401, "array index " + Twine(idx) + " is out of range"));
+            JSON_THROW(out_of_range::create(401, fmt::format("array index {} is out of range", idx)));
         }
     }
     else
     {
-        JSON_THROW(type_error::create(304, "cannot use at() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(304, "cannot use at() with", type_name()));
     }
 }
 
@@ -374,16 +386,16 @@
         JSON_CATCH (std::out_of_range&)
         {
             // create better exception explanation
-            JSON_THROW(out_of_range::create(401, "array index " + Twine(idx) + " is out of range"));
+            JSON_THROW(out_of_range::create(401, fmt::format("array index {} is out of range", idx)));
         }
     }
     else
     {
-        JSON_THROW(type_error::create(304, "cannot use at() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(304, "cannot use at() with", type_name()));
     }
 }
 
-json::reference json::at(StringRef key)
+json::reference json::at(std::string_view key)
 {
     // at only works for objects
     if (JSON_LIKELY(is_object()))
@@ -392,17 +404,17 @@
         if (it == m_value.object->end())
         {
             // create better exception explanation
-            JSON_THROW(out_of_range::create(403, "key '" + Twine(key) + "' not found"));
+            JSON_THROW(out_of_range::create(403, fmt::format("key '{}' not found", key)));
         }
         return it->second;
     }
     else
     {
-        JSON_THROW(type_error::create(304, "cannot use at() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(304, "cannot use at() with", type_name()));
     }
 }
 
-json::const_reference json::at(StringRef key) const
+json::const_reference json::at(std::string_view key) const
 {
     // at only works for objects
     if (JSON_LIKELY(is_object()))
@@ -411,13 +423,13 @@
         if (it == m_value.object->end())
         {
             // create better exception explanation
-            JSON_THROW(out_of_range::create(403, "key '" + Twine(key) + "' not found"));
+            JSON_THROW(out_of_range::create(403, fmt::format("key '{}' not found", key)));
         }
         return it->second;
     }
     else
     {
-        JSON_THROW(type_error::create(304, "cannot use at() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(304, "cannot use at() with", type_name()));
     }
 }
 
@@ -445,7 +457,7 @@
         return m_value.array->operator[](idx);
     }
 
-    JSON_THROW(type_error::create(305, "cannot use operator[] with " + Twine(type_name())));
+    JSON_THROW(type_error::create(305, "cannot use operator[] with", type_name()));
 }
 
 json::const_reference json::operator[](size_type idx) const
@@ -456,10 +468,10 @@
         return m_value.array->operator[](idx);
     }
 
-    JSON_THROW(type_error::create(305, "cannot use operator[] with " + Twine(type_name())));
+    JSON_THROW(type_error::create(305, "cannot use operator[] with", type_name()));
 }
 
-json::reference json::operator[](StringRef key)
+json::reference json::operator[](std::string_view key)
 {
     // implicitly convert null value to an empty object
     if (is_null())
@@ -475,10 +487,10 @@
         return m_value.object->operator[](key);
     }
 
-    JSON_THROW(type_error::create(305, "cannot use operator[] with " + Twine(type_name())));
+    JSON_THROW(type_error::create(305, "cannot use operator[] with", type_name()));
 }
 
-json::const_reference json::operator[](StringRef key) const
+json::const_reference json::operator[](std::string_view key) const
 {
     // const operator[] only works for objects
     if (JSON_LIKELY(is_object()))
@@ -487,10 +499,10 @@
         return m_value.object->find(key)->second;
     }
 
-    JSON_THROW(type_error::create(305, "cannot use operator[] with " + Twine(type_name())));
+    JSON_THROW(type_error::create(305, "cannot use operator[] with", type_name()));
 }
 
-json::size_type json::erase(StringRef key)
+json::size_type json::erase(std::string_view key)
 {
     // this erase only works for objects
     if (JSON_LIKELY(is_object()))
@@ -498,7 +510,7 @@
         return m_value.object->erase(key);
     }
 
-    JSON_THROW(type_error::create(307, "cannot use erase() with " + Twine(type_name())));
+    JSON_THROW(type_error::create(307, "cannot use erase() with", type_name()));
 }
 
 void json::erase(const size_type idx)
@@ -508,18 +520,18 @@
     {
         if (JSON_UNLIKELY(idx >= size()))
         {
-            JSON_THROW(out_of_range::create(401, "array index " + Twine(idx) + " is out of range"));
+            JSON_THROW(out_of_range::create(401, fmt::format("array index {} is out of range", idx)));
         }
 
         m_value.array->erase(m_value.array->begin() + static_cast<difference_type>(idx));
     }
     else
     {
-        JSON_THROW(type_error::create(307, "cannot use erase() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(307, "cannot use erase() with", type_name()));
     }
 }
 
-json::iterator json::find(StringRef key)
+json::iterator json::find(std::string_view key)
 {
     auto result = end();
 
@@ -531,7 +543,7 @@
     return result;
 }
 
-json::const_iterator json::find(StringRef key) const
+json::const_iterator json::find(std::string_view key) const
 {
     auto result = cend();
 
@@ -543,7 +555,7 @@
     return result;
 }
 
-json::size_type json::count(StringRef key) const
+json::size_type json::count(std::string_view key) const
 {
     // return 0 for all nonobject types
     return is_object() ? m_value.object->count(key) : 0;
@@ -689,7 +701,7 @@
     // push_back only works for null objects or arrays
     if (JSON_UNLIKELY(not(is_null() or is_array())))
     {
-        JSON_THROW(type_error::create(308, "cannot use push_back() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(308, "cannot use push_back() with", type_name()));
     }
 
     // transform null object into an array
@@ -711,7 +723,7 @@
     // push_back only works for null objects or arrays
     if (JSON_UNLIKELY(not(is_null() or is_array())))
     {
-        JSON_THROW(type_error::create(308, "cannot use push_back() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(308, "cannot use push_back() with", type_name()));
     }
 
     // transform null object into an array
@@ -731,7 +743,7 @@
     if (is_object() and init.size() == 2 and (*init.begin())->is_string())
     {
         std::string key = init.begin()->moved_or_copied();
-        push_back(std::pair<StringRef, json>(key, (init.begin() + 1)->moved_or_copied()));
+        push_back(std::pair<std::string_view, json>(key, (init.begin() + 1)->moved_or_copied()));
     }
     else
     {
@@ -756,7 +768,7 @@
         return result;
     }
 
-    JSON_THROW(type_error::create(309, "cannot use insert() with " + Twine(type_name())));
+    JSON_THROW(type_error::create(309, "cannot use insert() with", type_name()));
 }
 
 json::iterator json::insert(const_iterator pos, size_type cnt, const json& val)
@@ -776,7 +788,7 @@
         return result;
     }
 
-    JSON_THROW(type_error::create(309, "cannot use insert() with " + Twine(type_name())));
+    JSON_THROW(type_error::create(309, "cannot use insert() with", type_name()));
 }
 
 json::iterator json::insert(const_iterator pos, const_iterator first, const_iterator last)
@@ -784,7 +796,7 @@
     // insert only works for arrays
     if (JSON_UNLIKELY(not is_array()))
     {
-        JSON_THROW(type_error::create(309, "cannot use insert() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(309, "cannot use insert() with", type_name()));
     }
 
     // check if iterator pos fits to this JSON value
@@ -818,7 +830,7 @@
     // insert only works for arrays
     if (JSON_UNLIKELY(not is_array()))
     {
-        JSON_THROW(type_error::create(309, "cannot use insert() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(309, "cannot use insert() with", type_name()));
     }
 
     // check if iterator pos fits to this JSON value
@@ -838,7 +850,7 @@
     // insert only works for objects
     if (JSON_UNLIKELY(not is_object()))
     {
-        JSON_THROW(type_error::create(309, "cannot use insert() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(309, "cannot use insert() with", type_name()));
     }
 
     // check if range iterators belong to the same JSON object
@@ -871,11 +883,11 @@
 
     if (JSON_UNLIKELY(not is_object()))
     {
-        JSON_THROW(type_error::create(312, "cannot use update() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(312, "cannot use update() with", type_name()));
     }
     if (JSON_UNLIKELY(not j.is_object()))
     {
-        JSON_THROW(type_error::create(312, "cannot use update() with " + Twine(j.type_name())));
+        JSON_THROW(type_error::create(312, "cannot use update() with", j.type_name()));
     }
 
     for (auto it = j.cbegin(); it != j.cend(); ++it)
@@ -896,7 +908,7 @@
 
     if (JSON_UNLIKELY(not is_object()))
     {
-        JSON_THROW(type_error::create(312, "cannot use update() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(312, "cannot use update() with", type_name()));
     }
 
     // check if range iterators belong to the same JSON object
@@ -1156,7 +1168,7 @@
                         if (JSON_UNLIKELY(static_cast<size_type>(idx) > parent.size()))
                         {
                             // avoid undefined behavior
-                            JSON_THROW(out_of_range::create(401, "array index " + Twine(idx) + " is out of range"));
+                            JSON_THROW(out_of_range::create(401, fmt::format("array index {} is out of range", idx)));
                         }
                         else
                         {
@@ -1194,7 +1206,7 @@
             }
             else
             {
-                JSON_THROW(out_of_range::create(403, "key '" + Twine(last_path) + "' not found"));
+                JSON_THROW(out_of_range::create(403, fmt::format("key '{}' not found", last_path)));
             }
         }
         else if (parent.is_array())
@@ -1227,13 +1239,13 @@
             // check if desired value is present
             if (JSON_UNLIKELY(it == val.m_value.object->end()))
             {
-                JSON_THROW(parse_error::create(105, 0, Twine(error_msg) + " must have member '" + Twine(member) + "'"));
+                JSON_THROW(parse_error::create(105, 0, fmt::format("{} must have member '{}'", error_msg, member)));
             }
 
             // check if result is of type string
             if (JSON_UNLIKELY(string_type and not it->second.is_string()))
             {
-                JSON_THROW(parse_error::create(105, 0, Twine(error_msg) + " must have string member '" + Twine(member) + "'"));
+                JSON_THROW(parse_error::create(105, 0, fmt::format("{} must have string member '{}'", error_msg, member)));
             }
 
             // no error: return value
@@ -1321,7 +1333,7 @@
                 // throw an exception if test fails
                 if (JSON_UNLIKELY(not success))
                 {
-                    JSON_THROW(other_error::create(501, "unsuccessful: " + Twine(val.dump())));
+                    JSON_THROW(other_error::create(501, fmt::format("unsuccessful: {}", val.dump())));
                 }
 
                 break;
@@ -1331,7 +1343,7 @@
             {
                 // op must be "add", "remove", "replace", "move", "copy", or
                 // "test"
-                JSON_THROW(parse_error::create(105, 0, "operation value '" + Twine(op) + "' is invalid"));
+                JSON_THROW(parse_error::create(105, 0, fmt::format("operation value '{}' is invalid", op)));
             }
         }
     }
@@ -1413,7 +1425,7 @@
                 for (auto it = source.cbegin(); it != source.cend(); ++it)
                 {
                     // escape the key name to be used in a JSON patch
-                    const auto key = json_pointer::escape(it.key());
+                    const auto key = json_pointer::escape(std::string{it.key()});
 
                     if (target.find(it.key()) != target.end())
                     {
@@ -1437,7 +1449,7 @@
                     if (source.find(it.key()) == source.end())
                     {
                         // found a key that is not in this -> add it
-                        const auto key = json_pointer::escape(it.key());
+                        const auto key = json_pointer::escape(std::string{it.key()});
                         result.push_back(
                         {
                             {"op", "add"}, {"path", path + "/" + key},
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_reader.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_reader.cpp
index 9298e92..cee5e39 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_reader.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_reader.cpp
@@ -36,6 +36,7 @@
 
 #include <cmath>  // ldexp
 
+#include "fmt/format.h"
 #include "wpi/raw_istream.h"
 
 namespace wpi {
@@ -711,7 +712,7 @@
 
         default: // anything else (0xFF is handled inside the other types)
         {
-            JSON_THROW(parse_error::create(112, chars_read, "error reading CBOR; last byte: 0x" + Twine::utohexstr(current)));
+            JSON_THROW(parse_error::create(112, chars_read, fmt::format("error reading CBOR; last byte: {:#02x}", current)));
         }
     }
 }
@@ -1034,7 +1035,7 @@
         default: // anything else
         {
             JSON_THROW(parse_error::create(112, chars_read,
-                                           "error reading MessagePack; last byte: 0x" + Twine::utohexstr(current)));
+                fmt::format("error reading MessagePack; last byte: {:#02x}", current)));
         }
     }
 }
@@ -1106,7 +1107,8 @@
 
         default:
         {
-            JSON_THROW(parse_error::create(113, chars_read, "expected a CBOR string; last byte: 0x" + Twine::utohexstr(current)));
+            JSON_THROW(parse_error::create(113, chars_read,
+                fmt::format("expected a CBOR string; last byte: {:#02x}", current)));
         }
     }
 }
@@ -1172,7 +1174,7 @@
         default:
         {
             JSON_THROW(parse_error::create(113, chars_read,
-                                           "expected a MessagePack string; last byte: 0x" + Twine::utohexstr(current)));
+                fmt::format("expected a MessagePack string; last byte: {:#02x}", current)));
         }
     }
 }
@@ -1200,7 +1202,7 @@
             return get_string(get_number<int64_t>());
         default:
             JSON_THROW(parse_error::create(113, chars_read,
-                                           "expected a UBJSON string; last byte: 0x" + Twine::utohexstr(current)));
+                fmt::format("expected a UBJSON string; last byte: {:#02x}", current)));
     }
 }
 
@@ -1220,7 +1222,7 @@
         if (current != '#')
         {
             JSON_THROW(parse_error::create(112, chars_read,
-                                           "expected '#' after UBJSON type information; last byte: 0x" + Twine::utohexstr(current)));
+                fmt::format("expected '#' after UBJSON type information; last byte: {:#02x}", current)));
         }
         sz = parse_ubjson_internal();
     }
@@ -1269,7 +1271,7 @@
             if (JSON_UNLIKELY(current > 127))
             {
                 JSON_THROW(parse_error::create(113, chars_read,
-                                               "byte after 'C' must be in range 0x00..0x7F; last byte: 0x" + Twine::utohexstr(current)));
+                    fmt::format("byte after 'C' must be in range 0x00..0x7F; last byte: {:#02x}", current)));
             }
             return std::string(1, static_cast<char>(current));
         }
@@ -1285,7 +1287,7 @@
 
         default: // anything else
             JSON_THROW(parse_error::create(112, chars_read,
-                                           "error reading UBJSON; last byte: 0x" + Twine::utohexstr(current)));
+                fmt::format("error reading UBJSON; last byte: {:#02x}", current)));
     }
 }
 
@@ -1299,7 +1301,7 @@
         if (JSON_UNLIKELY(size_and_type.first > result.max_size()))
         {
             JSON_THROW(out_of_range::create(408,
-                                            "excessive array size: " + Twine(size_and_type.first)));
+                fmt::format("excessive array size: {}", size_and_type.first)));
         }
 
         if (size_and_type.second != 0)
@@ -1344,7 +1346,7 @@
         if (JSON_UNLIKELY(size_and_type.first > result.max_size()))
         {
             JSON_THROW(out_of_range::create(408,
-                                            "excessive object size: " + Twine(size_and_type.first)));
+                fmt::format("excessive object size: {}", size_and_type.first)));
         }
 
         if (size_and_type.second != 0)
@@ -1382,7 +1384,7 @@
     return binary_reader(is).parse_cbor(strict);
 }
 
-json json::from_cbor(ArrayRef<uint8_t> arr, const bool strict)
+json json::from_cbor(span<const uint8_t> arr, const bool strict)
 {
     raw_mem_istream is(arr);
     return from_cbor(is, strict);
@@ -1393,7 +1395,7 @@
     return binary_reader(is).parse_msgpack(strict);
 }
 
-json json::from_msgpack(ArrayRef<uint8_t> arr, const bool strict)
+json json::from_msgpack(span<const uint8_t> arr, const bool strict)
 {
     raw_mem_istream is(arr);
     return from_msgpack(is, strict);
@@ -1404,7 +1406,7 @@
     return binary_reader(is).parse_ubjson(strict);
 }
 
-json json::from_ubjson(ArrayRef<uint8_t> arr, const bool strict)
+json json::from_ubjson(span<const uint8_t> arr, const bool strict)
 {
     raw_mem_istream is(arr);
     return from_ubjson(is, strict);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_writer.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_writer.cpp
index 2c0bbee..db23f8d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_writer.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_binary_writer.cpp
@@ -34,6 +34,7 @@
 #define WPI_JSON_IMPLEMENTATION
 #include "wpi/json.h"
 
+#include "fmt/format.h"
 #include "wpi/raw_ostream.h"
 
 namespace wpi {
@@ -75,9 +76,9 @@
                       const bool use_type, const bool add_prefix = true);
 
   private:
-    void write_cbor_string(StringRef str);
+    void write_cbor_string(std::string_view str);
 
-    void write_msgpack_string(StringRef str);
+    void write_msgpack_string(std::string_view str);
 
     /*
     @brief write a number to output input
@@ -630,7 +631,7 @@
                 o << static_cast<CharType>('S');
             }
             write_number_with_ubjson_prefix(j.m_value.string->size(), true);
-            o << j.m_value.string;
+            o << *j.m_value.string;
             break;
         }
 
@@ -731,7 +732,7 @@
     }
 }
 
-void json::binary_writer::write_cbor_string(StringRef str)
+void json::binary_writer::write_cbor_string(std::string_view str)
 {
     // step 1: write control byte and the string length
     const auto N = str.size();
@@ -766,7 +767,7 @@
     o << str;
 }
 
-void json::binary_writer::write_msgpack_string(StringRef str)
+void json::binary_writer::write_msgpack_string(std::string_view str)
 {
     // step 1: write control byte and the string length
     const auto N = str.size();
@@ -812,7 +813,7 @@
         std::reverse(vec.begin(), vec.end());
     }
 
-    o << ArrayRef<uint8_t>(vec.data(), sizeof(NumberType));
+    o << span{vec.data(), sizeof(NumberType)};
 }
 
 template<typename NumberType, typename std::enable_if<
@@ -862,7 +863,7 @@
     }
     else
     {
-        JSON_THROW(out_of_range::create(407, "number overflow serializing " + Twine(n)));
+        JSON_THROW(out_of_range::create(407, fmt::format("number overflow serializing {}", n)));
     }
 }
 
@@ -915,7 +916,7 @@
     // LCOV_EXCL_START
     else
     {
-        JSON_THROW(out_of_range::create(407, "number overflow serializing " + Twine(n)));
+        JSON_THROW(out_of_range::create(407, fmt::format("number overflow serializing {}", n)));
     }
     // LCOV_EXCL_STOP
 }
@@ -1003,7 +1004,7 @@
     return result;
 }
 
-ArrayRef<uint8_t> json::to_cbor(const json& j, std::vector<uint8_t>& buf)
+span<uint8_t> json::to_cbor(const json& j, std::vector<uint8_t>& buf)
 {
     buf.clear();
     raw_uvector_ostream os(buf);
@@ -1011,7 +1012,7 @@
     return os.array();
 }
 
-ArrayRef<uint8_t> json::to_cbor(const json& j, SmallVectorImpl<uint8_t>& buf)
+span<uint8_t> json::to_cbor(const json& j, SmallVectorImpl<uint8_t>& buf)
 {
     buf.clear();
     raw_usvector_ostream os(buf);
@@ -1032,7 +1033,7 @@
     return result;
 }
 
-ArrayRef<uint8_t> json::to_msgpack(const json& j, std::vector<uint8_t>& buf)
+span<uint8_t> json::to_msgpack(const json& j, std::vector<uint8_t>& buf)
 {
     buf.clear();
     raw_uvector_ostream os(buf);
@@ -1040,7 +1041,7 @@
     return os.array();
 }
 
-ArrayRef<uint8_t> json::to_msgpack(const json& j, SmallVectorImpl<uint8_t>& buf)
+span<uint8_t> json::to_msgpack(const json& j, SmallVectorImpl<uint8_t>& buf)
 {
     buf.clear();
     raw_usvector_ostream os(buf);
@@ -1063,8 +1064,8 @@
     return result;
 }
 
-ArrayRef<uint8_t> json::to_ubjson(const json& j, std::vector<uint8_t>& buf,
-                                  const bool use_size, const bool use_type)
+span<uint8_t> json::to_ubjson(const json& j, std::vector<uint8_t>& buf,
+                              const bool use_size, const bool use_type)
 {
     buf.clear();
     raw_uvector_ostream os(buf);
@@ -1072,8 +1073,8 @@
     return os.array();
 }
 
-ArrayRef<uint8_t> json::to_ubjson(const json& j, SmallVectorImpl<uint8_t>& buf,
-                                  const bool use_size, const bool use_type)
+span<uint8_t> json::to_ubjson(const json& j, SmallVectorImpl<uint8_t>& buf,
+                              const bool use_size, const bool use_type)
 {
     buf.clear();
     raw_usvector_ostream os(buf);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_parser.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_parser.cpp
index e0d7db3..db1ed11 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_parser.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_parser.cpp
@@ -38,7 +38,7 @@
 #include <cmath>
 #include <cstdlib>
 
-#include "wpi/Format.h"
+#include "fmt/format.h"
 #include "wpi/SmallString.h"
 #include "wpi/raw_istream.h"
 #include "wpi/raw_ostream.h"
@@ -336,7 +336,7 @@
     }
 
     /// return current string value
-    StringRef get_string()
+    std::string_view get_string()
     {
         return token_buffer;
     }
@@ -1407,7 +1407,7 @@
         if (c <= '\x1F')
         {
             // escape control characters
-            ss << "<U+" << format_hex_no_prefix(c, 4, true) << '>';
+            ss << fmt::format("<U+{:04X}>", c);
         }
         else
         {
@@ -1605,7 +1605,7 @@
 
                 if (keep and keep_tag and not value.is_discarded())
                 {
-                    result.m_value.object->try_emplace(StringRef(key.data(), key.size()), std::move(value));
+                    result.m_value.object->try_emplace(std::string_view(key.data(), key.size()), std::move(value));
                 }
 
                 // comma -> next value
@@ -1757,8 +1757,8 @@
             {
                 if (allow_exceptions)
                 {
-                    JSON_THROW(out_of_range::create(406, "number overflow parsing '" +
-                                                    Twine(m_lexer.get_token_string()) + "'"));
+                    JSON_THROW(out_of_range::create(406,
+                        fmt::format("number overflow parsing '{}'", m_lexer.get_token_string())));
                 }
                 expect(token_type::uninitialized);
             }
@@ -1917,15 +1917,15 @@
     JSON_THROW(parse_error::create(101, m_lexer.get_position(), error_msg));
 }
 
-json json::parse(StringRef s,
+json json::parse(std::string_view s,
                         const parser_callback_t cb,
                         const bool allow_exceptions)
 {
-    raw_mem_istream is(makeArrayRef(s.data(), s.size()));
+    raw_mem_istream is(span<const char>(s.data(), s.size()));
     return parse(is, cb, allow_exceptions);
 }
 
-json json::parse(ArrayRef<uint8_t> arr,
+json json::parse(span<const uint8_t> arr,
                         const parser_callback_t cb,
                         const bool allow_exceptions)
 {
@@ -1942,13 +1942,13 @@
     return result;
 }
 
-bool json::accept(StringRef s)
+bool json::accept(std::string_view s)
 {
-    raw_mem_istream is(makeArrayRef(s.data(), s.size()));
+    raw_mem_istream is(span<const char>(s.data(), s.size()));
     return parser(is).accept(true);
 }
 
-bool json::accept(ArrayRef<uint8_t> arr)
+bool json::accept(span<const uint8_t> arr)
 {
     raw_mem_istream is(arr);
     return parser(is).accept(true);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_pointer.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_pointer.cpp
index 9cb1ec4..51548b3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_pointer.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_pointer.cpp
@@ -36,7 +36,9 @@
 
 #include <numeric> // accumulate
 
+#include "fmt/format.h"
 #include "wpi/SmallString.h"
+#include "wpi/StringExtras.h"
 
 namespace wpi {
 
@@ -50,17 +52,16 @@
     });
 }
 
-int json_pointer::array_index(const Twine& s)
+int json_pointer::array_index(std::string_view s)
 {
-    SmallString<128> buf;
-    StringRef str = s.toNullTerminatedStringRef(buf);
+    SmallString<128> str{s};
     std::size_t processed_chars = 0;
-    const int res = std::stoi(str, &processed_chars);
+    const int res = std::stoi(str.c_str(), &processed_chars);
 
     // check if the string was completely read
     if (JSON_UNLIKELY(processed_chars != str.size()))
     {
-        JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + Twine(s) + "'"));
+        JSON_THROW(detail::out_of_range::create(404, fmt::format("unresolved reference token '{}'", s)));
     }
 
     return res;
@@ -108,7 +109,7 @@
                 }
                 JSON_CATCH(std::invalid_argument&)
                 {
-                    JSON_THROW(detail::parse_error::create(109, 0, "array index '" + Twine(reference_token) + "' is not a number"));
+                    JSON_THROW(detail::parse_error::create(109, 0, fmt::format("array index '{}' is not a number", reference_token)));
                 }
                 break;
             }
@@ -164,8 +165,7 @@
                 if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0'))
                 {
                     JSON_THROW(detail::parse_error::create(106, 0,
-                                                           "array index '" + Twine(reference_token) +
-                                                           "' must not begin with '0'"));
+                        fmt::format("array index '{}' must not begin with '0'", reference_token)));
                 }
 
                 if (reference_token == "-")
@@ -183,14 +183,16 @@
                     }
                     JSON_CATCH(std::invalid_argument&)
                     {
-                        JSON_THROW(detail::parse_error::create(109, 0, "array index '" + Twine(reference_token) + "' is not a number"));
+                        JSON_THROW(detail::parse_error::create(109, 0,
+                            fmt::format("array index '{}' is not a number", reference_token)));
                     }
                 }
                 break;
             }
 
             default:
-                JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + Twine(reference_token) + "'"));
+                JSON_THROW(detail::out_of_range::create(404,
+                    fmt::format("unresolved reference token '{}'", reference_token)));
         }
     }
 
@@ -217,16 +219,14 @@
                 {
                     // "-" always fails the range check
                     JSON_THROW(detail::out_of_range::create(402,
-                                                            "array index '-' (" + Twine(ptr->m_value.array->size()) +
-                                                            ") is out of range"));
+                        fmt::format("array index '-' ({}) is out of range", ptr->m_value.array->size())));
                 }
 
                 // error condition (cf. RFC 6901, Sect. 4)
                 if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0'))
                 {
                     JSON_THROW(detail::parse_error::create(106, 0,
-                                                           "array index '" + Twine(reference_token) +
-                                                           "' must not begin with '0'"));
+                        fmt::format("array index '{}' must not begin with '0'", reference_token)));
                 }
 
                 // note: at performs range check
@@ -236,13 +236,15 @@
                 }
                 JSON_CATCH(std::invalid_argument&)
                 {
-                    JSON_THROW(detail::parse_error::create(109, 0, "array index '" + Twine(reference_token) + "' is not a number"));
+                    JSON_THROW(detail::parse_error::create(109, 0,
+                        fmt::format("array index '{}' is not a number", reference_token)));
                 }
                 break;
             }
 
             default:
-                JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + Twine(reference_token) + "'"));
+                JSON_THROW(detail::out_of_range::create(404,
+                    fmt::format("unresolved reference token '{}'", reference_token)));
         }
     }
 
@@ -269,16 +271,14 @@
                 {
                     // "-" cannot be used for const access
                     JSON_THROW(detail::out_of_range::create(402,
-                                                            "array index '-' (" + Twine(ptr->m_value.array->size()) +
-                                                            ") is out of range"));
+                        fmt::format("array index '-' ({}) is out of range", ptr->m_value.array->size())));
                 }
 
                 // error condition (cf. RFC 6901, Sect. 4)
                 if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0'))
                 {
                     JSON_THROW(detail::parse_error::create(106, 0,
-                                                           "array index '" + Twine(reference_token) +
-                                                           "' must not begin with '0'"));
+                        fmt::format("array index '{}' must not begin with '0'", reference_token)));
                 }
 
                 // use unchecked array access
@@ -289,13 +289,15 @@
                 }
                 JSON_CATCH(std::invalid_argument&)
                 {
-                    JSON_THROW(detail::parse_error::create(109, 0, "array index '" + Twine(reference_token) + "' is not a number"));
+                    JSON_THROW(detail::parse_error::create(109, 0,
+                        fmt::format("array index '{}' is not a number", reference_token)));
                 }
                 break;
             }
 
             default:
-                JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + Twine(reference_token) + "'"));
+                JSON_THROW(detail::out_of_range::create(404,
+                    fmt::format("unresolved reference token '{}'", reference_token)));
         }
     }
 
@@ -322,16 +324,14 @@
                 {
                     // "-" always fails the range check
                     JSON_THROW(detail::out_of_range::create(402,
-                                                            "array index '-' (" + Twine(ptr->m_value.array->size()) +
-                                                            ") is out of range"));
+                        fmt::format("array index '-' ({}) is out of range", ptr->m_value.array->size())));
                 }
 
                 // error condition (cf. RFC 6901, Sect. 4)
                 if (JSON_UNLIKELY(reference_token.size() > 1 and reference_token[0] == '0'))
                 {
                     JSON_THROW(detail::parse_error::create(106, 0,
-                                                           "array index '" + Twine(reference_token) +
-                                                           "' must not begin with '0'"));
+                        fmt::format("array index '{}' must not begin with '0'", reference_token)));
                 }
 
                 // note: at performs range check
@@ -341,23 +341,23 @@
                 }
                 JSON_CATCH(std::invalid_argument&)
                 {
-                    JSON_THROW(detail::parse_error::create(109, 0, "array index '" + Twine(reference_token) + "' is not a number"));
+                    JSON_THROW(detail::parse_error::create(109, 0,
+                        fmt::format("array index '{}' is not a number", reference_token)));
                 }
                 break;
             }
 
             default:
-                JSON_THROW(detail::out_of_range::create(404, "unresolved reference token '" + Twine(reference_token) + "'"));
+                JSON_THROW(detail::out_of_range::create(404,
+                    fmt::format("unresolved reference token '{}'", reference_token)));
         }
     }
 
     return *ptr;
 }
 
-std::vector<std::string> json_pointer::split(const Twine& reference_string)
+std::vector<std::string> json_pointer::split(std::string_view ref_str)
 {
-    SmallString<128> ref_str_buf;
-    StringRef ref_str = reference_string.toStringRef(ref_str_buf);
     std::vector<std::string> result;
 
     // special case: empty reference string -> no reference tokens
@@ -370,8 +370,7 @@
     if (JSON_UNLIKELY(ref_str[0] != '/'))
     {
         JSON_THROW(detail::parse_error::create(107, 1,
-                                               "JSON pointer must be empty or begin with '/' - was: '" +
-                                               Twine(ref_str) + "'"));
+            fmt::format("JSON pointer must be empty or begin with '/' - was: '{}'", ref_str)));
     }
 
     // extract the reference tokens:
@@ -392,11 +391,11 @@
     {
         // use the text between the beginning of the reference token
         // (start) and the last slash (slash).
-        auto reference_token = ref_str.slice(start, slash);
+        auto reference_token = slice(ref_str, start, slash);
 
         // check reference tokens are properly escaped
         for (std::size_t pos = reference_token.find_first_of('~');
-                pos != StringRef::npos;
+                pos != std::string_view::npos;
                 pos = reference_token.find_first_of('~', pos + 1))
         {
             assert(reference_token[pos] == '~');
@@ -411,7 +410,7 @@
         }
 
         // finally, store the reference token
-        std::string ref_tok = reference_token;
+        std::string ref_tok{reference_token};
         unescape(ref_tok);
         result.emplace_back(std::move(ref_tok));
     }
@@ -444,7 +443,7 @@
     replace_substring(s, "~0", "~");
 }
 
-void json_pointer::flatten(const Twine& reference_string,
+void json_pointer::flatten(std::string_view reference_string,
                     const json& value,
                     json& result)
 {
@@ -455,15 +454,14 @@
             if (value.m_value.array->empty())
             {
                 // flatten empty array as null
-                SmallString<64> buf;
-                result[reference_string.toStringRef(buf)] = nullptr;
+                result[reference_string] = nullptr;
             }
             else
             {
                 // iterate array and use index as reference string
                 for (std::size_t i = 0; i < value.m_value.array->size(); ++i)
                 {
-                    flatten(reference_string + "/" + Twine(i),
+                    flatten(fmt::format("{}/{}", reference_string, i),
                             value.m_value.array->operator[](i), result);
                 }
             }
@@ -475,15 +473,14 @@
             if (value.m_value.object->empty())
             {
                 // flatten empty object as null
-                SmallString<64> buf;
-                result[reference_string.toStringRef(buf)] = nullptr;
+                result[reference_string] = nullptr;
             }
             else
             {
                 // iterate object and use keys as reference string
                 for (const auto& element : *value.m_value.object)
                 {
-                    flatten(reference_string + "/" + Twine(escape(element.first())), element.second, result);
+                    flatten(fmt::format("{}/{}", reference_string, escape(std::string{element.first()})), element.second, result);
                 }
             }
             break;
@@ -492,8 +489,7 @@
         default:
         {
             // add primitive value with its reference string
-            SmallString<64> buf;
-            result[reference_string.toStringRef(buf)] = value;
+            result[reference_string] = value;
             break;
         }
     }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.cpp
index 8ab92b1..1101f66 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.cpp
@@ -34,12 +34,11 @@
 #define WPI_JSON_IMPLEMENTATION
 #include "wpi/json.h"
 
-#include "wpi/Format.h"
+#include "fmt/format.h"
 #include "wpi/SmallString.h"
-#include "wpi/StringExtras.h"
 #include "wpi/raw_os_ostream.h"
 
-#include "json_serializer.h"
+#include "wpi/json_serializer.h"
 
 namespace wpi {
 
@@ -1332,7 +1331,7 @@
     }
 }
 
-void json::serializer::dump_escaped(StringRef s, const bool ensure_ascii)
+void json::serializer::dump_escaped(std::string_view s, const bool ensure_ascii)
 {
     uint32_t codepoint;
     uint8_t state = UTF8_ACCEPT;
@@ -1397,12 +1396,12 @@
                         {
                             if (codepoint <= 0xFFFF)
                             {
-                                o << '\\' << 'u' << format_hex_no_prefix(codepoint, 4);
+                                o << fmt::format("\\u{:04x}", codepoint);
                             }
                             else
                             {
-                                o << '\\' << 'u' << format_hex_no_prefix(0xD7C0 + (codepoint >> 10), 4);
-                                o << '\\' << 'u' << format_hex_no_prefix(0xDC00 + (codepoint & 0x3FF), 4);
+                                o << fmt::format("\\u{:04x}", 0xD7C0 + (codepoint >> 10));
+                                o << fmt::format("\\u{:04x}", 0xDC00 + (codepoint & 0x3FF));
                             }
                         }
                         else
@@ -1419,7 +1418,7 @@
 
             case UTF8_REJECT:  // decode found invalid UTF-8 byte
             {
-                JSON_THROW(type_error::create(316, "invalid UTF-8 byte at index " + Twine(i) + ": 0x" + Twine::utohexstr(byte)));
+                JSON_THROW(type_error::create(316, fmt::format("invalid UTF-8 byte at index {}: {:#02x}", i, byte)));
             }
 
             default:  // decode found yet incomplete multi-byte code point
@@ -1437,7 +1436,7 @@
     if (JSON_UNLIKELY(state != UTF8_ACCEPT))
     {
         // we finish reading, but do not accept: string was incomplete
-        JSON_THROW(type_error::create(316, "incomplete UTF-8 string; last byte: 0x" + Twine::utohexstr(static_cast<uint8_t>(s.back()))));
+        JSON_THROW(type_error::create(316, fmt::format("incomplete UTF-8 string; last byte: {:#02x}", s.back())));
     }
 }
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.h b/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.h
deleted file mode 100644
index 6c5ffbe..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/json_serializer.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Modifications Copyright (c) 2017-2018 FIRST. All Rights Reserved.          */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-/*
-    __ _____ _____ _____
- __|  |   __|     |   | |  JSON for Modern C++
-|  |  |__   |  |  | | | |  version 3.1.2
-|_____|_____|_____|_|___|  https://github.com/nlohmann/json
-
-Licensed under the MIT License <http://opensource.org/licenses/MIT>.
-Copyright (c) 2013-2018 Niels Lohmann <http://nlohmann.me>.
-
-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 "wpi/json.h"
-
-#include <clocale> // lconv, localeconv
-#include <cmath>  // labs, isfinite, isnan, signbit, ldexp
-#include <locale> // locale
-#include <type_traits>
-
-#include "wpi/raw_ostream.h"
-
-namespace wpi {
-
-class json::serializer
-{
-    static constexpr uint8_t UTF8_ACCEPT = 0;
-    static constexpr uint8_t UTF8_REJECT = 1;
-
-  public:
-    /*!
-    @param[in] s  output stream to serialize to
-    @param[in] ichar  indentation character to use
-    */
-    serializer(raw_ostream& s, const char ichar)
-        : o(s), loc(std::localeconv()), indent_char(ichar),
-          indent_string(512, indent_char)
-    {}
-
-    // delete because of pointer members
-    serializer(const serializer&) = delete;
-    serializer& operator=(const serializer&) = delete;
-
-    /*!
-    @brief internal implementation of the serialization function
-
-    This function is called by the public member function dump and organizes
-    the serialization internally. The indentation level is propagated as
-    additional parameter. In case of arrays and objects, the function is
-    called recursively.
-
-    - strings and object keys are escaped using `escape_string()`
-    - integer numbers are converted implicitly via `operator<<`
-    - floating-point numbers are converted to a string using `"%g"` format
-
-    @param[in] val             value to serialize
-    @param[in] pretty_print    whether the output shall be pretty-printed
-    @param[in] indent_step     the indent level
-    @param[in] current_indent  the current indent level (only used internally)
-    */
-    void dump(const json& val, const bool pretty_print,
-              const bool ensure_ascii,
-              const unsigned int indent_step,
-              const unsigned int current_indent = 0);
-
-  private:
-    /*!
-    @brief dump escaped string
-
-    Escape a string by replacing certain special characters by a sequence of an
-    escape character (backslash) and another character and other control
-    characters by a sequence of "\u" followed by a four-digit hex
-    representation. The escaped string is written to output stream @a o.
-
-    @param[in] s  the string to escape
-    @param[in] ensure_ascii  whether to escape non-ASCII characters with
-                             \uXXXX sequences
-
-    @complexity Linear in the length of string @a s.
-    */
-    void dump_escaped(StringRef s, const bool ensure_ascii);
-
-    template <typename NumberType,
-              detail::enable_if_t<std::is_same_v<NumberType, uint64_t>, int> = 0>
-    bool is_negative_integer(NumberType x) {
-      return false;
-    }
-
-    template <typename NumberType,
-              detail::enable_if_t<std::is_same_v<NumberType, int64_t>, int> = 0>
-    bool is_negative_integer(NumberType x) {
-      return x < 0;
-    }
-
-    /*!
-    @brief dump an integer
-
-    Dump a given integer to output stream @a o. Works internally with
-    @a number_buffer.
-
-    @param[in] x  integer number (signed or unsigned) to dump
-    @tparam NumberType either @a int64_t or @a uint64_t
-    */
-    template<typename NumberType, detail::enable_if_t<
-                 std::is_same<NumberType, uint64_t>::value or
-                 std::is_same<NumberType, int64_t>::value,
-                 int> = 0>
-    void dump_integer(NumberType x)
-    {
-        // special case for "0"
-        if (x == 0)
-        {
-            o << '0';
-            return;
-        }
-
-        const bool is_negative = is_negative_integer(x);  // see issue #755
-        std::size_t i = 0;
-
-        while (x != 0)
-        {
-            // spare 1 byte for '\0'
-            assert(i < number_buffer.size() - 1);
-
-            const auto digit = std::labs(static_cast<long>(x % 10));
-            number_buffer[i++] = static_cast<char>('0' + digit);
-            x /= 10;
-        }
-
-        if (is_negative)
-        {
-            // make sure there is capacity for the '-'
-            assert(i < number_buffer.size() - 2);
-            number_buffer[i++] = '-';
-        }
-
-        std::reverse(number_buffer.begin(), number_buffer.begin() + i);
-        o.write(number_buffer.data(), i);
-    }
-
-    /*!
-    @brief dump a floating-point number
-
-    Dump a given floating-point number to output stream @a o. Works internally
-    with @a number_buffer.
-
-    @param[in] x  floating-point number to dump
-    */
-    void dump_float(double x);
-
-    /*!
-    @brief check whether a string is UTF-8 encoded
-
-    The function checks each byte of a string whether it is UTF-8 encoded. The
-    result of the check is stored in the @a state parameter. The function must
-    be called initially with state 0 (accept). State 1 means the string must
-    be rejected, because the current byte is not allowed. If the string is
-    completely processed, but the state is non-zero, the string ended
-    prematurely; that is, the last byte indicated more bytes should have
-    followed.
-
-    @param[in,out] state  the state of the decoding
-    @param[in,out] codep  codepoint (valid only if resulting state is UTF8_ACCEPT)
-    @param[in] byte       next byte to decode
-    @return               new state
-
-    @note The function has been edited: a std::array is used.
-
-    @copyright Copyright (c) 2008-2009 Bjoern Hoehrmann <bjoern@hoehrmann.de>
-    @sa http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
-    */
-    static uint8_t decode(uint8_t& state, uint32_t& codep, const uint8_t byte) noexcept;
-
-  private:
-    /// the output of the serializer
-    raw_ostream& o;
-
-    /// a (hopefully) large enough character buffer
-    std::array<char, 64> number_buffer{{}};
-
-    /// the locale
-    const std::lconv* loc = nullptr;
-
-    /// the indentation character
-    const char indent_char;
-    /// the indentation string
-    std::string indent_string;
-};
-
-}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/leb128.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/leb128.cpp
index 202ee9a..9657883 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/leb128.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/leb128.cpp
@@ -1,13 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/leb128.h"
 
+#include "wpi/SpanExtras.h"
 #include "wpi/raw_istream.h"
+#include "wpi/raw_ostream.h"
+#include "wpi/span.h"
 
 namespace wpi {
 
@@ -24,11 +24,12 @@
   size_t count = 0;
 
   do {
-    unsigned char byte = val & 0x7f;
+    uint8_t byte = val & 0x7f;
     val >>= 7;
 
-    if (val != 0)
+    if (val != 0) {
       byte |= 0x80;  // mark this byte to show that more bytes will follow
+    }
 
     dest.push_back(byte);
     count++;
@@ -37,6 +38,19 @@
   return count;
 }
 
+void WriteUleb128(raw_ostream& os, uint64_t val) {
+  do {
+    uint8_t byte = val & 0x7f;
+    val >>= 7;
+
+    if (val != 0) {
+      byte |= 0x80;  // mark this byte to show that more bytes will follow
+    }
+
+    os << byte;
+  } while (val != 0);
+}
+
 uint64_t ReadUleb128(const char* addr, uint64_t* ret) {
   uint64_t result = 0;
   int shift = 0;
@@ -47,10 +61,12 @@
     addr++;
     count++;
 
-    result |= (byte & 0x7f) << shift;
+    result |= (byte & 0x7fULL) << shift;
     shift += 7;
 
-    if (!(byte & 0x80)) break;
+    if (!(byte & 0x80)) {
+      break;
+    }
   }
 
   *ret = result;
@@ -65,12 +81,16 @@
   while (1) {
     unsigned char byte;
     is.read(reinterpret_cast<char*>(&byte), 1);
-    if (is.has_error()) return false;
+    if (is.has_error()) {
+      return false;
+    }
 
-    result |= (byte & 0x7f) << shift;
+    result |= (byte & 0x7fULL) << shift;
     shift += 7;
 
-    if (!(byte & 0x80)) break;
+    if (!(byte & 0x80)) {
+      break;
+    }
   }
 
   *ret = result;
@@ -78,4 +98,22 @@
   return true;
 }
 
+std::optional<uint64_t> Uleb128Reader::ReadOne(span<const uint8_t>* in) {
+  while (!in->empty()) {
+    uint8_t byte = in->front();
+    *in = wpi::drop_front(*in);
+
+    m_result |= (byte & 0x7fULL) << m_shift;
+    m_shift += 7;
+
+    if (!(byte & 0x80)) {
+      uint64_t result = m_result;
+      m_result = 0;
+      m_shift = 0;
+      return result;
+    }
+  }
+  return std::nullopt;
+}
+
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTF.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTF.cpp
index abe3744..3050e63 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTF.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTF.cpp
@@ -52,6 +52,11 @@
 #endif
 #include <assert.h>
 
+#ifdef _WIN32
+#include "wpi/WindowsError.h"
+#include "Windows/WindowsSupport.h"
+#endif
+
 /*
  * This code extensively uses fall-through switches.
  * Keep the compiler from warning about that.
@@ -734,6 +739,96 @@
 
    --------------------------------------------------------------------- */
 
+#ifdef _WIN32
+
+namespace sys {
+namespace windows {
+std::error_code CodePageToUTF16(unsigned codepage,
+                                std::string_view original,
+                                wpi::SmallVectorImpl<wchar_t> &utf16) {
+  if (!original.empty()) {
+    int len = ::MultiByteToWideChar(codepage, MB_ERR_INVALID_CHARS, original.data(),
+                                    original.size(), utf16.begin(), 0);
+
+    if (len == 0) {
+      return mapWindowsError(::GetLastError());
+    }
+
+    utf16.reserve(len + 1);
+    utf16.set_size(len);
+
+    len = ::MultiByteToWideChar(codepage, MB_ERR_INVALID_CHARS, original.data(),
+                                original.size(), utf16.begin(), utf16.size());
+
+    if (len == 0) {
+      return mapWindowsError(::GetLastError());
+    }
+  }
+
+  // Make utf16 null terminated.
+  utf16.push_back(0);
+  utf16.pop_back();
+
+  return std::error_code();
+}
+
+std::error_code UTF8ToUTF16(std::string_view utf8,
+                            wpi::SmallVectorImpl<wchar_t> &utf16) {
+  return CodePageToUTF16(CP_UTF8, utf8, utf16);
+}
+
+std::error_code CurCPToUTF16(std::string_view curcp,
+                            wpi::SmallVectorImpl<wchar_t> &utf16) {
+  return CodePageToUTF16(CP_ACP, curcp, utf16);
+}
+
+static
+std::error_code UTF16ToCodePage(unsigned codepage, const wchar_t *utf16,
+                                size_t utf16_len,
+                                wpi::SmallVectorImpl<char> &converted) {
+  if (utf16_len) {
+    // Get length.
+    int len = ::WideCharToMultiByte(codepage, 0, utf16, utf16_len, converted.begin(),
+                                    0, NULL, NULL);
+
+    if (len == 0) {
+      return mapWindowsError(::GetLastError());
+    }
+
+    converted.reserve(len);
+    converted.set_size(len);
+
+    // Now do the actual conversion.
+    len = ::WideCharToMultiByte(codepage, 0, utf16, utf16_len, converted.data(),
+                                converted.size(), NULL, NULL);
+
+    if (len == 0) {
+      return mapWindowsError(::GetLastError());
+    }
+  }
+
+  // Make the new string null terminated.
+  converted.push_back(0);
+  converted.pop_back();
+
+  return std::error_code();
+}
+
+std::error_code UTF16ToUTF8(const wchar_t *utf16, size_t utf16_len,
+                            wpi::SmallVectorImpl<char> &utf8) {
+  return UTF16ToCodePage(CP_UTF8, utf16, utf16_len, utf8);
+}
+
+std::error_code UTF16ToCurCP(const wchar_t *utf16, size_t utf16_len,
+                             wpi::SmallVectorImpl<char> &curcp) {
+  return UTF16ToCodePage(CP_ACP, utf16, utf16_len, curcp);
+}
+
+} // end namespace windows
+} // end namespace sys
+
+#endif  // _WIN32
+
 } // namespace llvm
 
 ConvertUTF_RESTORE_WARNINGS
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTFWrapper.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTFWrapper.cpp
index 3402988..db190d0 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTFWrapper.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ConvertUTFWrapper.cpp
@@ -8,6 +8,7 @@
 //===----------------------------------------------------------------------===//
 
 #include "wpi/ConvertUTF.h"
+#include "wpi/SmallVector.h"
 #include <string>
 #include <vector>
 
@@ -28,13 +29,13 @@
   return true;
 }
 
-bool hasUTF16ByteOrderMark(ArrayRef<char> S) {
+bool hasUTF16ByteOrderMark(span<const char> S) {
   return (S.size() >= 2 &&
           ((S[0] == '\xff' && S[1] == '\xfe') ||
            (S[0] == '\xfe' && S[1] == '\xff')));
 }
 
-bool convertUTF16ToUTF8String(ArrayRef<UTF16> SrcUTF16,
+bool convertUTF16ToUTF8String(span<const UTF16> SrcUTF16,
                               SmallVectorImpl<char> &DstUTF8) {
   assert(DstUTF8.empty());
 
@@ -80,7 +81,7 @@
   return true;
 }
 
-bool convertUTF8ToUTF16String(StringRef SrcUTF8,
+bool convertUTF8ToUTF16String(std::string_view SrcUTF8,
                               SmallVectorImpl<UTF16> &DstUTF16) {
   assert(DstUTF16.empty());
 
@@ -91,8 +92,8 @@
     return true;
   }
 
-  const UTF8 *Src = reinterpret_cast<const UTF8 *>(SrcUTF8.begin());
-  const UTF8 *SrcEnd = reinterpret_cast<const UTF8 *>(SrcUTF8.end());
+  const UTF8 *Src = reinterpret_cast<const UTF8 *>(SrcUTF8.data());
+  const UTF8 *SrcEnd = reinterpret_cast<const UTF8 *>(SrcUTF8.data() + SrcUTF8.size());
 
   // Allocate the same number of UTF-16 code units as UTF-8 code units. Encoding
   // as UTF-16 should always require the same amount or less code units than the
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Error.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Error.cpp
deleted file mode 100644
index 7bf9c5f..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Error.cpp
+++ /dev/null
@@ -1,138 +0,0 @@
-//===----- lib/Support/Error.cpp - Error and associated utilities ---------===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#include "wpi/Error.h"
-#include "wpi/Twine.h"
-#include "wpi/ErrorHandling.h"
-#include "wpi/ManagedStatic.h"
-#include <system_error>
-
-using namespace wpi;
-
-namespace {
-
-  enum class ErrorErrorCode : int {
-    MultipleErrors = 1,
-    FileError,
-    InconvertibleError
-  };
-
-  // FIXME: This class is only here to support the transition to wpi::Error. It
-  // will be removed once this transition is complete. Clients should prefer to
-  // deal with the Error value directly, rather than converting to error_code.
-  class ErrorErrorCategory : public std::error_category {
-  public:
-    const char *name() const noexcept override { return "Error"; }
-
-    std::string message(int condition) const override {
-      switch (static_cast<ErrorErrorCode>(condition)) {
-      case ErrorErrorCode::MultipleErrors:
-        return "Multiple errors";
-      case ErrorErrorCode::InconvertibleError:
-        return "Inconvertible error value. An error has occurred that could "
-               "not be converted to a known std::error_code. Please file a "
-               "bug.";
-      case ErrorErrorCode::FileError:
-          return "A file error occurred.";
-      }
-      wpi_unreachable("Unhandled error code");
-    }
-  };
-
-}
-
-static ManagedStatic<ErrorErrorCategory> ErrorErrorCat;
-
-namespace wpi {
-
-void ErrorInfoBase::anchor() {}
-char ErrorInfoBase::ID = 0;
-char ErrorList::ID = 0;
-void ECError::anchor() {}
-char ECError::ID = 0;
-char StringError::ID = 0;
-char FileError::ID = 0;
-
-void logAllUnhandledErrors(Error E, raw_ostream &OS, Twine ErrorBanner) {
-  if (!E)
-    return;
-  OS << ErrorBanner;
-  handleAllErrors(std::move(E), [&](const ErrorInfoBase &EI) {
-    EI.log(OS);
-    OS << "\n";
-  });
-}
-
-
-std::error_code ErrorList::convertToErrorCode() const {
-  return std::error_code(static_cast<int>(ErrorErrorCode::MultipleErrors),
-                         *ErrorErrorCat);
-}
-
-std::error_code inconvertibleErrorCode() {
-  return std::error_code(static_cast<int>(ErrorErrorCode::InconvertibleError),
-                         *ErrorErrorCat);
-}
-
-std::error_code FileError::convertToErrorCode() const {
-  return std::error_code(static_cast<int>(ErrorErrorCode::FileError),
-                         *ErrorErrorCat);
-}
-
-Error errorCodeToError(std::error_code EC) {
-  if (!EC)
-    return Error::success();
-  return Error(std::make_unique<ECError>(ECError(EC)));
-}
-
-std::error_code errorToErrorCode(Error Err) {
-  std::error_code EC;
-  handleAllErrors(std::move(Err), [&](const ErrorInfoBase &EI) {
-    EC = EI.convertToErrorCode();
-  });
-  if (EC == inconvertibleErrorCode())
-    report_fatal_error(EC.message());
-  return EC;
-}
-
-StringError::StringError(std::error_code EC, const Twine &S)
-    : Msg(S.str()), EC(EC) {}
-
-StringError::StringError(const Twine &S, std::error_code EC)
-    : Msg(S.str()), EC(EC), PrintMsgOnly(true) {}
-
-void StringError::log(raw_ostream &OS) const {
-  if (PrintMsgOnly) {
-    OS << Msg;
-  } else {
-    OS << EC.message();
-    if (!Msg.empty())
-      OS << (" " + Msg);
-  }
-}
-
-std::error_code StringError::convertToErrorCode() const {
-  return EC;
-}
-
-Error createStringError(std::error_code EC, char const *Msg) {
-  return make_error<StringError>(Msg, EC);
-}
-
-void report_fatal_error(Error Err, bool GenCrashDiag) {
-  assert(Err && "report_fatal_error called with success value");
-  std::string ErrMsg;
-  {
-    raw_string_ostream ErrStream(ErrMsg);
-    logAllUnhandledErrors(std::move(Err), ErrStream);
-  }
-  report_fatal_error(ErrMsg);
-}
-
-} // end namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ErrorHandling.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ErrorHandling.cpp
index ef79456..22ae636 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ErrorHandling.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/ErrorHandling.cpp
@@ -14,11 +14,10 @@
 
 #include "wpi/ErrorHandling.h"
 #include "wpi/SmallVector.h"
-#include "wpi/Twine.h"
-#include "wpi/Error.h"
 #include "wpi/WindowsError.h"
-#include "wpi/raw_ostream.h"
+#include "fmt/format.h"
 #include <cassert>
+#include <cstdio>
 #include <cstdlib>
 #include <mutex>
 #include <new>
@@ -68,18 +67,14 @@
 }
 
 void wpi::report_fatal_error(const char *Reason, bool GenCrashDiag) {
-  report_fatal_error(Twine(Reason), GenCrashDiag);
+  report_fatal_error(std::string_view(Reason), GenCrashDiag);
 }
 
 void wpi::report_fatal_error(const std::string &Reason, bool GenCrashDiag) {
-  report_fatal_error(Twine(Reason), GenCrashDiag);
+  report_fatal_error(std::string_view(Reason), GenCrashDiag);
 }
 
-void wpi::report_fatal_error(StringRef Reason, bool GenCrashDiag) {
-  report_fatal_error(Twine(Reason), GenCrashDiag);
-}
-
-void wpi::report_fatal_error(const Twine &Reason, bool GenCrashDiag) {
+void wpi::report_fatal_error(std::string_view Reason, bool GenCrashDiag) {
   wpi::fatal_error_handler_t handler = nullptr;
   void* handlerData = nullptr;
   {
@@ -91,21 +86,9 @@
   }
 
   if (handler) {
-    handler(handlerData, Reason.str(), GenCrashDiag);
+    handler(handlerData, std::string{Reason}, GenCrashDiag);
   } else {
-    // Blast the result out to stderr.  We don't try hard to make sure this
-    // succeeds (e.g. handling EINTR) and we can't use errs() here because
-    // raw ostreams can call report_fatal_error.
-    SmallVector<char, 64> Buffer;
-    raw_svector_ostream OS(Buffer);
-    OS << "LLVM ERROR: " << Reason << "\n";
-    StringRef MessageStr = OS.str();
-#ifdef _WIN32
-    int written = ::_write(2, MessageStr.data(), MessageStr.size());
-#else
-    ssize_t written = ::write(2, MessageStr.data(), MessageStr.size());
-#endif
-    (void)written; // If something went wrong, we deliberately just give up.
+    fmt::print(stderr, "LLVM ERROR: {}\n", Reason);
   }
 
   exit(1);
@@ -185,11 +168,11 @@
   // wpi_unreachable is intended to be used to indicate "impossible"
   // situations, and not legitimate runtime errors.
   if (msg)
-    errs() << msg << "\n";
-  errs() << "UNREACHABLE executed";
+    fmt::print(stderr, "{}\n", msg);
+  std::fputs("UNREACHABLE executed", stderr);
   if (file)
-    errs() << " at " << file << ":" << line;
-  errs() << "!\n";
+    fmt::print(stderr, " at {}:{}", file, line);
+  fmt::print(stderr, "{}", "!\n");
   abort();
 #ifdef LLVM_BUILTIN_UNREACHABLE
   // Windows systems and possibly others don't declare abort() to be noreturn,
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/NativeFormatting.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/NativeFormatting.cpp
deleted file mode 100644
index 985e269..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/NativeFormatting.cpp
+++ /dev/null
@@ -1,264 +0,0 @@
-//===- NativeFormatting.cpp - Low level formatting helpers -------*- C++-*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#include "wpi/NativeFormatting.h"
-
-#include "wpi/ArrayRef.h"
-#include "wpi/SmallString.h"
-#include "wpi/StringExtras.h"
-#include "wpi/Format.h"
-
-#include <float.h>
-
-using namespace wpi;
-
-template<typename T, std::size_t N>
-static int format_to_buffer(T Value, char (&Buffer)[N]) {
-  char *EndPtr = std::end(Buffer);
-  char *CurPtr = EndPtr;
-
-  do {
-    *--CurPtr = '0' + char(Value % 10);
-    Value /= 10;
-  } while (Value);
-  return EndPtr - CurPtr;
-}
-
-static void writeWithCommas(raw_ostream &S, ArrayRef<char> Buffer) {
-  assert(!Buffer.empty());
-
-  ArrayRef<char> ThisGroup;
-  int InitialDigits = ((Buffer.size() - 1) % 3) + 1;
-  ThisGroup = Buffer.take_front(InitialDigits);
-  S.write(ThisGroup.data(), ThisGroup.size());
-
-  Buffer = Buffer.drop_front(InitialDigits);
-  assert(Buffer.size() % 3 == 0);
-  while (!Buffer.empty()) {
-    S << ',';
-    ThisGroup = Buffer.take_front(3);
-    S.write(ThisGroup.data(), 3);
-    Buffer = Buffer.drop_front(3);
-  }
-}
-
-template <typename T>
-static void write_unsigned_impl(raw_ostream &S, T N, size_t MinDigits,
-                                IntegerStyle Style, bool IsNegative) {
-  static_assert(std::is_unsigned<T>::value, "Value is not unsigned!");
-
-  char NumberBuffer[128];
-  std::memset(NumberBuffer, '0', sizeof(NumberBuffer));
-
-  size_t Len = 0;
-  Len = format_to_buffer(N, NumberBuffer);
-
-  if (IsNegative)
-    S << '-';
-
-  if (Len < MinDigits && Style != IntegerStyle::Number) {
-    for (size_t I = Len; I < MinDigits; ++I)
-      S << '0';
-  }
-
-  if (Style == IntegerStyle::Number) {
-    writeWithCommas(S, ArrayRef<char>(std::end(NumberBuffer) - Len, Len));
-  } else {
-    S.write(std::end(NumberBuffer) - Len, Len);
-  }
-}
-
-template <typename T>
-static void write_unsigned(raw_ostream &S, T N, size_t MinDigits,
-                           IntegerStyle Style, bool IsNegative = false) {
-  // Output using 32-bit div/mod if possible.
-  if (N == static_cast<uint32_t>(N))
-    write_unsigned_impl(S, static_cast<uint32_t>(N), MinDigits, Style,
-                        IsNegative);
-  else
-    write_unsigned_impl(S, N, MinDigits, Style, IsNegative);
-}
-
-template <typename T>
-static void write_signed(raw_ostream &S, T N, size_t MinDigits,
-                         IntegerStyle Style) {
-  static_assert(std::is_signed<T>::value, "Value is not signed!");
-
-  using UnsignedT = typename std::make_unsigned<T>::type;
-
-  if (N >= 0) {
-    write_unsigned(S, static_cast<UnsignedT>(N), MinDigits, Style);
-    return;
-  }
-
-  UnsignedT UN = -(UnsignedT)N;
-  write_unsigned(S, UN, MinDigits, Style, true);
-}
-
-void wpi::write_integer(raw_ostream &S, unsigned int N, size_t MinDigits,
-                         IntegerStyle Style) {
-  write_unsigned(S, N, MinDigits, Style);
-}
-
-void wpi::write_integer(raw_ostream &S, int N, size_t MinDigits,
-                         IntegerStyle Style) {
-  write_signed(S, N, MinDigits, Style);
-}
-
-void wpi::write_integer(raw_ostream &S, unsigned long N, size_t MinDigits,
-                         IntegerStyle Style) {
-  write_unsigned(S, N, MinDigits, Style);
-}
-
-void wpi::write_integer(raw_ostream &S, long N, size_t MinDigits,
-                         IntegerStyle Style) {
-  write_signed(S, N, MinDigits, Style);
-}
-
-void wpi::write_integer(raw_ostream &S, unsigned long long N, size_t MinDigits,
-                         IntegerStyle Style) {
-  write_unsigned(S, N, MinDigits, Style);
-}
-
-void wpi::write_integer(raw_ostream &S, long long N, size_t MinDigits,
-                         IntegerStyle Style) {
-  write_signed(S, N, MinDigits, Style);
-}
-
-void wpi::write_hex(raw_ostream &S, uint64_t N, HexPrintStyle Style,
-                     std::optional<size_t> Width) {
-  const size_t kMaxWidth = 128u;
-
-  size_t W = std::min(kMaxWidth, Width.value_or(0u));
-
-  unsigned Nibbles = (64 - countLeadingZeros(N) + 3) / 4;
-  bool Prefix = (Style == HexPrintStyle::PrefixLower ||
-                 Style == HexPrintStyle::PrefixUpper);
-  bool Upper =
-      (Style == HexPrintStyle::Upper || Style == HexPrintStyle::PrefixUpper);
-  unsigned PrefixChars = Prefix ? 2 : 0;
-  unsigned NumChars =
-      std::max(static_cast<unsigned>(W), std::max(1u, Nibbles) + PrefixChars);
-
-  char NumberBuffer[kMaxWidth];
-  ::memset(NumberBuffer, '0', wpi::array_lengthof(NumberBuffer));
-  if (Prefix)
-    NumberBuffer[1] = 'x';
-  char *EndPtr = NumberBuffer + NumChars;
-  char *CurPtr = EndPtr;
-  while (N) {
-    unsigned char x = static_cast<unsigned char>(N) % 16;
-    *--CurPtr = hexdigit(x, !Upper);
-    N /= 16;
-  }
-
-  S.write(NumberBuffer, NumChars);
-}
-
-void wpi::write_double(raw_ostream &S, double N, FloatStyle Style,
-                        std::optional<size_t> Precision) {
-  size_t Prec = Precision.value_or(getDefaultPrecision(Style));
-
-  if (std::isnan(N)) {
-    S << "nan";
-    return;
-  } else if (std::isinf(N)) {
-    S << "INF";
-    return;
-  }
-
-  char Letter;
-  if (Style == FloatStyle::Exponent)
-    Letter = 'e';
-  else if (Style == FloatStyle::ExponentUpper)
-    Letter = 'E';
-  else
-    Letter = 'f';
-
-  SmallString<8> Spec;
-  wpi::raw_svector_ostream Out(Spec);
-  Out << "%." << Prec << Letter;
-
-  if (Style == FloatStyle::Exponent || Style == FloatStyle::ExponentUpper) {
-#ifdef _WIN32
-// On MSVCRT and compatible, output of %e is incompatible to Posix
-// by default. Number of exponent digits should be at least 2. "%+03d"
-// FIXME: Implement our formatter to here or Support/Format.h!
-#if defined(__MINGW32__)
-    // FIXME: It should be generic to C++11.
-    if (N == 0.0 && std::signbit(N)) {
-      char NegativeZero[] = "-0.000000e+00";
-      if (Style == FloatStyle::ExponentUpper)
-        NegativeZero[strlen(NegativeZero) - 4] = 'E';
-      S << NegativeZero;
-      return;
-    }
-#else
-    int fpcl = _fpclass(N);
-
-    // negative zero
-    if (fpcl == _FPCLASS_NZ) {
-      char NegativeZero[] = "-0.000000e+00";
-      if (Style == FloatStyle::ExponentUpper)
-        NegativeZero[strlen(NegativeZero) - 4] = 'E';
-      S << NegativeZero;
-      return;
-    }
-#endif
-
-    char buf[32];
-    unsigned len;
-    len = format(Spec.c_str(), N).snprint(buf, sizeof(buf));
-    if (len <= sizeof(buf) - 2) {
-      if (len >= 5 && (buf[len - 5] == 'e' || buf[len - 5] == 'E') &&
-          buf[len - 3] == '0') {
-        int cs = buf[len - 4];
-        if (cs == '+' || cs == '-') {
-          int c1 = buf[len - 2];
-          int c0 = buf[len - 1];
-          if (isdigit(static_cast<unsigned char>(c1)) &&
-              isdigit(static_cast<unsigned char>(c0))) {
-            // Trim leading '0': "...e+012" -> "...e+12\0"
-            buf[len - 3] = c1;
-            buf[len - 2] = c0;
-            buf[--len] = 0;
-          }
-        }
-      }
-      S << buf;
-      return;
-    }
-#endif
-  }
-
-  if (Style == FloatStyle::Percent)
-    N *= 100.0;
-
-  char Buf[32];
-  format(Spec.c_str(), N).snprint(Buf, sizeof(Buf));
-  S << Buf;
-  if (Style == FloatStyle::Percent)
-    S << '%';
-}
-
-bool wpi::isPrefixedHexStyle(HexPrintStyle S) {
-  return (S == HexPrintStyle::PrefixLower || S == HexPrintStyle::PrefixUpper);
-}
-
-size_t wpi::getDefaultPrecision(FloatStyle Style) {
-  switch (Style) {
-  case FloatStyle::Exponent:
-  case FloatStyle::ExponentUpper:
-    return 6; // Number of decimal places.
-  case FloatStyle::Fixed:
-  case FloatStyle::Percent:
-    return 2; // Number of decimal places.
-  }
-  LLVM_BUILTIN_UNREACHABLE;
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Path.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Path.cpp
deleted file mode 100644
index 49f92d3..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Path.cpp
+++ /dev/null
@@ -1,833 +0,0 @@
-//===-- Path.cpp - Implement OS Path Concept ------------------------------===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-//  This file implements the operating system Path API.
-//
-//===----------------------------------------------------------------------===//
-
-#include "wpi/Path.h"
-#include "wpi/ArrayRef.h"
-#include "wpi/Endian.h"
-#include "wpi/Errc.h"
-#include "wpi/ErrorHandling.h"
-#include "wpi/FileSystem.h"
-#include "wpi/SmallString.h"
-#include <cctype>
-#include <cstring>
-
-#if !defined(_MSC_VER) && !defined(__MINGW32__)
-#include <unistd.h>
-#else
-#include <io.h>
-#endif
-
-using namespace wpi;
-using namespace wpi::support::endian;
-
-namespace {
-  using wpi::StringRef;
-  using wpi::sys::path::is_separator;
-  using wpi::sys::path::Style;
-
-  inline Style real_style(Style style) {
-#ifdef _WIN32
-    return (style == Style::posix) ? Style::posix : Style::windows;
-#else
-    return (style == Style::windows) ? Style::windows : Style::posix;
-#endif
-  }
-
-  inline const char *separators(Style style) {
-    if (real_style(style) == Style::windows)
-      return "\\/";
-    return "/";
-  }
-
-  inline char preferred_separator(Style style) {
-    if (real_style(style) == Style::windows)
-      return '\\';
-    return '/';
-  }
-
-  StringRef find_first_component(StringRef path, Style style) {
-    // Look for this first component in the following order.
-    // * empty (in this case we return an empty string)
-    // * either C: or {//,\\}net.
-    // * {/,\}
-    // * {file,directory}name
-
-    if (path.empty())
-      return path;
-
-    if (real_style(style) == Style::windows) {
-      // C:
-      if (path.size() >= 2 &&
-          std::isalpha(static_cast<unsigned char>(path[0])) && path[1] == ':')
-        return path.substr(0, 2);
-    }
-
-    // //net
-    if ((path.size() > 2) && is_separator(path[0], style) &&
-        path[0] == path[1] && !is_separator(path[2], style)) {
-      // Find the next directory separator.
-      size_t end = path.find_first_of(separators(style), 2);
-      return path.substr(0, end);
-    }
-
-    // {/,\}
-    if (is_separator(path[0], style))
-      return path.substr(0, 1);
-
-    // * {file,directory}name
-    size_t end = path.find_first_of(separators(style));
-    return path.substr(0, end);
-  }
-
-  // Returns the first character of the filename in str. For paths ending in
-  // '/', it returns the position of the '/'.
-  size_t filename_pos(StringRef str, Style style) {
-    if (str.size() > 0 && is_separator(str[str.size() - 1], style))
-      return str.size() - 1;
-
-    size_t pos = str.find_last_of(separators(style), str.size() - 1);
-
-    if (real_style(style) == Style::windows) {
-      if (pos == StringRef::npos)
-        pos = str.find_last_of(':', str.size() - 2);
-    }
-
-    if (pos == StringRef::npos || (pos == 1 && is_separator(str[0], style)))
-      return 0;
-
-    return pos + 1;
-  }
-
-  // Returns the position of the root directory in str. If there is no root
-  // directory in str, it returns StringRef::npos.
-  size_t root_dir_start(StringRef str, Style style) {
-    // case "c:/"
-    if (real_style(style) == Style::windows) {
-      if (str.size() > 2 && str[1] == ':' && is_separator(str[2], style))
-        return 2;
-    }
-
-    // case "//net"
-    if (str.size() > 3 && is_separator(str[0], style) && str[0] == str[1] &&
-        !is_separator(str[2], style)) {
-      return str.find_first_of(separators(style), 2);
-    }
-
-    // case "/"
-    if (str.size() > 0 && is_separator(str[0], style))
-      return 0;
-
-    return StringRef::npos;
-  }
-
-  // Returns the position past the end of the "parent path" of path. The parent
-  // path will not end in '/', unless the parent is the root directory. If the
-  // path has no parent, 0 is returned.
-  size_t parent_path_end(StringRef path, Style style) {
-    size_t end_pos = filename_pos(path, style);
-
-    bool filename_was_sep =
-        path.size() > 0 && is_separator(path[end_pos], style);
-
-    // Skip separators until we reach root dir (or the start of the string).
-    size_t root_dir_pos = root_dir_start(path, style);
-    while (end_pos > 0 &&
-           (root_dir_pos == StringRef::npos || end_pos > root_dir_pos) &&
-           is_separator(path[end_pos - 1], style))
-      --end_pos;
-
-    if (end_pos == root_dir_pos && !filename_was_sep) {
-      // We've reached the root dir and the input path was *not* ending in a
-      // sequence of slashes. Include the root dir in the parent path.
-      return root_dir_pos + 1;
-    }
-
-    // Otherwise, just include before the last slash.
-    return end_pos;
-  }
-} // end unnamed namespace
-
-namespace wpi {
-namespace sys  {
-namespace path {
-
-const_iterator begin(StringRef path, Style style) {
-  const_iterator i;
-  i.Path      = path;
-  i.Component = find_first_component(path, style);
-  i.Position  = 0;
-  i.S = style;
-  return i;
-}
-
-const_iterator end(StringRef path) {
-  const_iterator i;
-  i.Path      = path;
-  i.Position  = path.size();
-  return i;
-}
-
-const_iterator &const_iterator::operator++() {
-  assert(Position < Path.size() && "Tried to increment past end!");
-
-  // Increment Position to past the current component
-  Position += Component.size();
-
-  // Check for end.
-  if (Position == Path.size()) {
-    Component = StringRef();
-    return *this;
-  }
-
-  // Both POSIX and Windows treat paths that begin with exactly two separators
-  // specially.
-  bool was_net = Component.size() > 2 && is_separator(Component[0], S) &&
-                 Component[1] == Component[0] && !is_separator(Component[2], S);
-
-  // Handle separators.
-  if (is_separator(Path[Position], S)) {
-    // Root dir.
-    if (was_net ||
-        // c:/
-        (real_style(S) == Style::windows && Component.endswith(":"))) {
-      Component = Path.substr(Position, 1);
-      return *this;
-    }
-
-    // Skip extra separators.
-    while (Position != Path.size() && is_separator(Path[Position], S)) {
-      ++Position;
-    }
-
-    // Treat trailing '/' as a '.', unless it is the root dir.
-    if (Position == Path.size() && Component != "/") {
-      --Position;
-      Component = ".";
-      return *this;
-    }
-  }
-
-  // Find next component.
-  size_t end_pos = Path.find_first_of(separators(S), Position);
-  Component = Path.slice(Position, end_pos);
-
-  return *this;
-}
-
-bool const_iterator::operator==(const const_iterator &RHS) const {
-  return Path.begin() == RHS.Path.begin() && Position == RHS.Position;
-}
-
-ptrdiff_t const_iterator::operator-(const const_iterator &RHS) const {
-  return Position - RHS.Position;
-}
-
-reverse_iterator rbegin(StringRef Path, Style style) {
-  reverse_iterator I;
-  I.Path = Path;
-  I.Position = Path.size();
-  I.S = style;
-  return ++I;
-}
-
-reverse_iterator rend(StringRef Path) {
-  reverse_iterator I;
-  I.Path = Path;
-  I.Component = Path.substr(0, 0);
-  I.Position = 0;
-  return I;
-}
-
-reverse_iterator &reverse_iterator::operator++() {
-  size_t root_dir_pos = root_dir_start(Path, S);
-
-  // Skip separators unless it's the root directory.
-  size_t end_pos = Position;
-  while (end_pos > 0 && (end_pos - 1) != root_dir_pos &&
-         is_separator(Path[end_pos - 1], S))
-    --end_pos;
-
-  // Treat trailing '/' as a '.', unless it is the root dir.
-  if (Position == Path.size() && !Path.empty() &&
-      is_separator(Path.back(), S) &&
-      (root_dir_pos == StringRef::npos || end_pos - 1 > root_dir_pos)) {
-    --Position;
-    Component = ".";
-    return *this;
-  }
-
-  // Find next separator.
-  size_t start_pos = filename_pos(Path.substr(0, end_pos), S);
-  Component = Path.slice(start_pos, end_pos);
-  Position = start_pos;
-  return *this;
-}
-
-bool reverse_iterator::operator==(const reverse_iterator &RHS) const {
-  return Path.begin() == RHS.Path.begin() && Component == RHS.Component &&
-         Position == RHS.Position;
-}
-
-ptrdiff_t reverse_iterator::operator-(const reverse_iterator &RHS) const {
-  return Position - RHS.Position;
-}
-
-StringRef root_path(StringRef path, Style style) {
-  const_iterator b = begin(path, style), pos = b, e = end(path);
-  if (b != e) {
-    bool has_net =
-        b->size() > 2 && is_separator((*b)[0], style) && (*b)[1] == (*b)[0];
-    bool has_drive = (real_style(style) == Style::windows) && b->endswith(":");
-
-    if (has_net || has_drive) {
-      if ((++pos != e) && is_separator((*pos)[0], style)) {
-        // {C:/,//net/}, so get the first two components.
-        return path.substr(0, b->size() + pos->size());
-      } else {
-        // just {C:,//net}, return the first component.
-        return *b;
-      }
-    }
-
-    // POSIX style root directory.
-    if (is_separator((*b)[0], style)) {
-      return *b;
-    }
-  }
-
-  return StringRef();
-}
-
-StringRef root_name(StringRef path, Style style) {
-  const_iterator b = begin(path, style), e = end(path);
-  if (b != e) {
-    bool has_net =
-        b->size() > 2 && is_separator((*b)[0], style) && (*b)[1] == (*b)[0];
-    bool has_drive = (real_style(style) == Style::windows) && b->endswith(":");
-
-    if (has_net || has_drive) {
-      // just {C:,//net}, return the first component.
-      return *b;
-    }
-  }
-
-  // No path or no name.
-  return StringRef();
-}
-
-StringRef root_directory(StringRef path, Style style) {
-  const_iterator b = begin(path, style), pos = b, e = end(path);
-  if (b != e) {
-    bool has_net =
-        b->size() > 2 && is_separator((*b)[0], style) && (*b)[1] == (*b)[0];
-    bool has_drive = (real_style(style) == Style::windows) && b->endswith(":");
-
-    if ((has_net || has_drive) &&
-        // {C:,//net}, skip to the next component.
-        (++pos != e) && is_separator((*pos)[0], style)) {
-      return *pos;
-    }
-
-    // POSIX style root directory.
-    if (!has_net && is_separator((*b)[0], style)) {
-      return *b;
-    }
-  }
-
-  // No path or no root.
-  return StringRef();
-}
-
-StringRef relative_path(StringRef path, Style style) {
-  StringRef root = root_path(path, style);
-  return path.substr(root.size());
-}
-
-void append(SmallVectorImpl<char> &path, Style style, const Twine &a,
-            const Twine &b, const Twine &c, const Twine &d) {
-  SmallString<32> a_storage;
-  SmallString<32> b_storage;
-  SmallString<32> c_storage;
-  SmallString<32> d_storage;
-
-  SmallVector<StringRef, 4> components;
-  if (!a.isTriviallyEmpty()) components.push_back(a.toStringRef(a_storage));
-  if (!b.isTriviallyEmpty()) components.push_back(b.toStringRef(b_storage));
-  if (!c.isTriviallyEmpty()) components.push_back(c.toStringRef(c_storage));
-  if (!d.isTriviallyEmpty()) components.push_back(d.toStringRef(d_storage));
-
-  for (auto &component : components) {
-    bool path_has_sep =
-        !path.empty() && is_separator(path[path.size() - 1], style);
-    if (path_has_sep) {
-      // Strip separators from beginning of component.
-      size_t loc = component.find_first_not_of(separators(style));
-      StringRef c = component.substr(loc);
-
-      // Append it.
-      path.append(c.begin(), c.end());
-      continue;
-    }
-
-    bool component_has_sep =
-        !component.empty() && is_separator(component[0], style);
-    if (!component_has_sep &&
-        !(path.empty() || has_root_name(component, style))) {
-      // Add a separator.
-      path.push_back(preferred_separator(style));
-    }
-
-    path.append(component.begin(), component.end());
-  }
-}
-
-void append(SmallVectorImpl<char> &path, const Twine &a, const Twine &b,
-            const Twine &c, const Twine &d) {
-  append(path, Style::native, a, b, c, d);
-}
-
-void append(SmallVectorImpl<char> &path, const_iterator begin,
-            const_iterator end, Style style) {
-  for (; begin != end; ++begin)
-    path::append(path, style, *begin);
-}
-
-StringRef parent_path(StringRef path, Style style) {
-  size_t end_pos = parent_path_end(path, style);
-  if (end_pos == StringRef::npos)
-    return StringRef();
-  else
-    return path.substr(0, end_pos);
-}
-
-void remove_filename(SmallVectorImpl<char> &path, Style style) {
-  size_t end_pos = parent_path_end(StringRef(path.begin(), path.size()), style);
-  if (end_pos != StringRef::npos)
-    path.set_size(end_pos);
-}
-
-void replace_extension(SmallVectorImpl<char> &path, const Twine &extension,
-                       Style style) {
-  StringRef p(path.begin(), path.size());
-  SmallString<32> ext_storage;
-  StringRef ext = extension.toStringRef(ext_storage);
-
-  // Erase existing extension.
-  size_t pos = p.find_last_of('.');
-  if (pos != StringRef::npos && pos >= filename_pos(p, style))
-    path.set_size(pos);
-
-  // Append '.' if needed.
-  if (ext.size() > 0 && ext[0] != '.')
-    path.push_back('.');
-
-  // Append extension.
-  path.append(ext.begin(), ext.end());
-}
-
-void replace_path_prefix(SmallVectorImpl<char> &Path,
-                         const StringRef &OldPrefix, const StringRef &NewPrefix,
-                         Style style) {
-  if (OldPrefix.empty() && NewPrefix.empty())
-    return;
-
-  StringRef OrigPath(Path.begin(), Path.size());
-  if (!OrigPath.startswith(OldPrefix))
-    return;
-
-  // If prefixes have the same size we can simply copy the new one over.
-  if (OldPrefix.size() == NewPrefix.size()) {
-    wpi::copy(NewPrefix, Path.begin());
-    return;
-  }
-
-  StringRef RelPath = OrigPath.substr(OldPrefix.size());
-  SmallString<256> NewPath;
-  path::append(NewPath, style, NewPrefix);
-  path::append(NewPath, style, RelPath);
-  Path.swap(NewPath);
-}
-
-void native(const Twine &path, SmallVectorImpl<char> &result, Style style) {
-  assert((!path.isSingleStringRef() ||
-          path.getSingleStringRef().data() != result.data()) &&
-         "path and result are not allowed to overlap!");
-  // Clear result.
-  result.clear();
-  path.toVector(result);
-  native(result, style);
-}
-
-void native(SmallVectorImpl<char> &Path, Style style) {
-  if (Path.empty())
-    return;
-  if (real_style(style) == Style::windows) {
-    std::replace(Path.begin(), Path.end(), '/', '\\');
-    if (Path[0] == '~' && (Path.size() == 1 || is_separator(Path[1], style))) {
-      SmallString<128> PathHome;
-      home_directory(PathHome);
-      PathHome.append(Path.begin() + 1, Path.end());
-      Path = PathHome;
-    }
-  } else {
-    for (auto PI = Path.begin(), PE = Path.end(); PI < PE; ++PI) {
-      if (*PI == '\\') {
-        auto PN = PI + 1;
-        if (PN < PE && *PN == '\\')
-          ++PI; // increment once, the for loop will move over the escaped slash
-        else
-          *PI = '/';
-      }
-    }
-  }
-}
-
-std::string convert_to_slash(StringRef path, Style style) {
-  if (real_style(style) != Style::windows)
-    return path;
-
-  std::string s = path.str();
-  std::replace(s.begin(), s.end(), '\\', '/');
-  return s;
-}
-
-StringRef filename(StringRef path, Style style) { return *rbegin(path, style); }
-
-StringRef stem(StringRef path, Style style) {
-  StringRef fname = filename(path, style);
-  size_t pos = fname.find_last_of('.');
-  if (pos == StringRef::npos)
-    return fname;
-  else
-    if ((fname.size() == 1 && fname == ".") ||
-        (fname.size() == 2 && fname == ".."))
-      return fname;
-    else
-      return fname.substr(0, pos);
-}
-
-StringRef extension(StringRef path, Style style) {
-  StringRef fname = filename(path, style);
-  size_t pos = fname.find_last_of('.');
-  if (pos == StringRef::npos)
-    return StringRef();
-  else
-    if ((fname.size() == 1 && fname == ".") ||
-        (fname.size() == 2 && fname == ".."))
-      return StringRef();
-    else
-      return fname.substr(pos);
-}
-
-bool is_separator(char value, Style style) {
-  if (value == '/')
-    return true;
-  if (real_style(style) == Style::windows)
-    return value == '\\';
-  return false;
-}
-
-StringRef get_separator(Style style) {
-  if (real_style(style) == Style::windows)
-    return "\\";
-  return "/";
-}
-
-bool has_root_name(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !root_name(p, style).empty();
-}
-
-bool has_root_directory(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !root_directory(p, style).empty();
-}
-
-bool has_root_path(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !root_path(p, style).empty();
-}
-
-bool has_relative_path(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !relative_path(p, style).empty();
-}
-
-bool has_filename(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !filename(p, style).empty();
-}
-
-bool has_parent_path(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !parent_path(p, style).empty();
-}
-
-bool has_stem(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !stem(p, style).empty();
-}
-
-bool has_extension(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  return !extension(p, style).empty();
-}
-
-bool is_absolute(const Twine &path, Style style) {
-  SmallString<128> path_storage;
-  StringRef p = path.toStringRef(path_storage);
-
-  bool rootDir = has_root_directory(p, style);
-  bool rootName =
-      (real_style(style) != Style::windows) || has_root_name(p, style);
-
-  return rootDir && rootName;
-}
-
-bool is_relative(const Twine &path, Style style) {
-  return !is_absolute(path, style);
-}
-
-StringRef remove_leading_dotslash(StringRef Path, Style style) {
-  // Remove leading "./" (or ".//" or "././" etc.)
-  while (Path.size() > 2 && Path[0] == '.' && is_separator(Path[1], style)) {
-    Path = Path.substr(2);
-    while (Path.size() > 0 && is_separator(Path[0], style))
-      Path = Path.substr(1);
-  }
-  return Path;
-}
-
-static SmallString<256> remove_dots(StringRef path, bool remove_dot_dot,
-                                    Style style) {
-  SmallVector<StringRef, 16> components;
-
-  // Skip the root path, then look for traversal in the components.
-  StringRef rel = path::relative_path(path, style);
-  for (StringRef C :
-       wpi::make_range(path::begin(rel, style), path::end(rel))) {
-    if (C == ".")
-      continue;
-    // Leading ".." will remain in the path unless it's at the root.
-    if (remove_dot_dot && C == "..") {
-      if (!components.empty() && components.back() != "..") {
-        components.pop_back();
-        continue;
-      }
-      if (path::is_absolute(path, style))
-        continue;
-    }
-    components.push_back(C);
-  }
-
-  SmallString<256> buffer = path::root_path(path, style);
-  for (StringRef C : components)
-    path::append(buffer, style, C);
-  return buffer;
-}
-
-bool remove_dots(SmallVectorImpl<char> &path, bool remove_dot_dot,
-                 Style style) {
-  StringRef p(path.data(), path.size());
-
-  SmallString<256> result = remove_dots(p, remove_dot_dot, style);
-  if (result == path)
-    return false;
-
-  path.swap(result);
-  return true;
-}
-
-} // end namespace path
-
-namespace fs {
-
-std::error_code getUniqueID(const Twine Path, UniqueID &Result) {
-  file_status Status;
-  std::error_code EC = status(Path, Status);
-  if (EC)
-    return EC;
-  Result = Status.getUniqueID();
-  return std::error_code();
-}
-
-void make_absolute(const Twine &current_directory,
-                   SmallVectorImpl<char> &path) {
-  StringRef p(path.data(), path.size());
-
-  bool rootDirectory = path::has_root_directory(p);
-  bool rootName =
-      (real_style(Style::native) != Style::windows) || path::has_root_name(p);
-
-  // Already absolute.
-  if (rootName && rootDirectory)
-    return;
-
-  // All of the following conditions will need the current directory.
-  SmallString<128> current_dir;
-  current_directory.toVector(current_dir);
-
-  // Relative path. Prepend the current directory.
-  if (!rootName && !rootDirectory) {
-    // Append path to the current directory.
-    path::append(current_dir, p);
-    // Set path to the result.
-    path.swap(current_dir);
-    return;
-  }
-
-  if (!rootName && rootDirectory) {
-    StringRef cdrn = path::root_name(current_dir);
-    SmallString<128> curDirRootName(cdrn.begin(), cdrn.end());
-    path::append(curDirRootName, p);
-    // Set path to the result.
-    path.swap(curDirRootName);
-    return;
-  }
-
-  if (rootName && !rootDirectory) {
-    StringRef pRootName      = path::root_name(p);
-    StringRef bRootDirectory = path::root_directory(current_dir);
-    StringRef bRelativePath  = path::relative_path(current_dir);
-    StringRef pRelativePath  = path::relative_path(p);
-
-    SmallString<128> res;
-    path::append(res, pRootName, bRootDirectory, bRelativePath, pRelativePath);
-    path.swap(res);
-    return;
-  }
-
-  wpi_unreachable("All rootName and rootDirectory combinations should have "
-                   "occurred above!");
-}
-
-std::error_code make_absolute(SmallVectorImpl<char> &path) {
-  if (path::is_absolute(path))
-    return {};
-
-  SmallString<128> current_dir;
-  if (std::error_code ec = current_path(current_dir))
-    return ec;
-
-  make_absolute(current_dir, path);
-  return {};
-}
-
-bool exists(const basic_file_status &status) {
-  return status_known(status) && status.type() != file_type::file_not_found;
-}
-
-bool status_known(const basic_file_status &s) {
-  return s.type() != file_type::status_error;
-}
-
-file_type get_file_type(const Twine &Path, bool Follow) {
-  file_status st;
-  if (status(Path, st, Follow))
-    return file_type::status_error;
-  return st.type();
-}
-
-bool is_directory(const basic_file_status &status) {
-  return status.type() == file_type::directory_file;
-}
-
-std::error_code is_directory(const Twine &path, bool &result) {
-  file_status st;
-  if (std::error_code ec = status(path, st))
-    return ec;
-  result = is_directory(st);
-  return std::error_code();
-}
-
-bool is_regular_file(const basic_file_status &status) {
-  return status.type() == file_type::regular_file;
-}
-
-std::error_code is_regular_file(const Twine &path, bool &result) {
-  file_status st;
-  if (std::error_code ec = status(path, st))
-    return ec;
-  result = is_regular_file(st);
-  return std::error_code();
-}
-
-bool is_symlink_file(const basic_file_status &status) {
-  return status.type() == file_type::symlink_file;
-}
-
-std::error_code is_symlink_file(const Twine &path, bool &result) {
-  file_status st;
-  if (std::error_code ec = status(path, st, false))
-    return ec;
-  result = is_symlink_file(st);
-  return std::error_code();
-}
-
-bool is_other(const basic_file_status &status) {
-  return exists(status) &&
-         !is_regular_file(status) &&
-         !is_directory(status);
-}
-
-std::error_code is_other(const Twine &Path, bool &Result) {
-  file_status FileStatus;
-  if (std::error_code EC = status(Path, FileStatus))
-    return EC;
-  Result = is_other(FileStatus);
-  return std::error_code();
-}
-
-void directory_entry::replace_filename(const Twine &Filename, file_type Type,
-                                       basic_file_status Status) {
-  SmallString<128> PathStr = path::parent_path(Path);
-  path::append(PathStr, Filename);
-  this->Path = PathStr.str();
-  this->Type = Type;
-  this->Status = Status;
-}
-
-ErrorOr<perms> getPermissions(const Twine &Path) {
-  file_status Status;
-  if (std::error_code EC = status(Path, Status))
-    return EC;
-
-  return Status.permissions();
-}
-
-} // end namespace fs
-} // end namespace sys
-} // end namespace wpi
-
-// Include the truly platform-specific parts.
-#ifdef _WIN32
-#include "Windows/Path.inc"
-#else
-#include "Unix/Path.inc"
-#endif
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/SmallPtrSet.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/SmallPtrSet.cpp
index 1dab1fc..1b33f4c 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/SmallPtrSet.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/SmallPtrSet.cpp
@@ -15,6 +15,7 @@
 #include "wpi/SmallPtrSet.h"
 #include "wpi/DenseMapInfo.h"
 #include "wpi/MathExtras.h"
+#include "wpi/MemAlloc.h"
 #include "wpi/ErrorHandling.h"
 #include <algorithm>
 #include <cassert>
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringExtras.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringExtras.cpp
deleted file mode 100644
index e4bfe8a..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringExtras.cpp
+++ /dev/null
@@ -1,91 +0,0 @@
-//===-- StringExtras.cpp - Implement the StringExtras header --------------===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file implements the StringExtras.h header
-//
-//===----------------------------------------------------------------------===//
-
-#include "wpi/StringExtras.h"
-#include "wpi/SmallVector.h"
-#include "wpi/raw_ostream.h"
-using namespace wpi;
-
-/// StrInStrNoCase - Portable version of strcasestr.  Locates the first
-/// occurrence of string 's1' in string 's2', ignoring case.  Returns
-/// the offset of s2 in s1 or npos if s2 cannot be found.
-StringRef::size_type wpi::StrInStrNoCase(StringRef s1, StringRef s2) {
-  size_t N = s2.size(), M = s1.size();
-  if (N > M)
-    return StringRef::npos;
-  for (size_t i = 0, e = M - N + 1; i != e; ++i)
-    if (s1.substr(i, N).equals_lower(s2))
-      return i;
-  return StringRef::npos;
-}
-
-/// getToken - This function extracts one token from source, ignoring any
-/// leading characters that appear in the Delimiters string, and ending the
-/// token at any of the characters that appear in the Delimiters string.  If
-/// there are no tokens in the source string, an empty string is returned.
-/// The function returns a pair containing the extracted token and the
-/// remaining tail string.
-std::pair<StringRef, StringRef> wpi::getToken(StringRef Source,
-                                               StringRef Delimiters) {
-  // Figure out where the token starts.
-  StringRef::size_type Start = Source.find_first_not_of(Delimiters);
-
-  // Find the next occurrence of the delimiter.
-  StringRef::size_type End = Source.find_first_of(Delimiters, Start);
-
-  return std::make_pair(Source.slice(Start, End), Source.substr(End));
-}
-
-/// SplitString - Split up the specified string according to the specified
-/// delimiters, appending the result fragments to the output list.
-void wpi::SplitString(StringRef Source,
-                       SmallVectorImpl<StringRef> &OutFragments,
-                       StringRef Delimiters) {
-  std::pair<StringRef, StringRef> S = getToken(Source, Delimiters);
-  while (!S.first.empty()) {
-    OutFragments.push_back(S.first);
-    S = getToken(S.second, Delimiters);
-  }
-}
-
-void wpi::printEscapedString(StringRef Name, raw_ostream &Out) {
-  for (unsigned i = 0, e = Name.size(); i != e; ++i) {
-    unsigned char C = Name[i];
-    if (isPrint(C) && C != '\\' && C != '"')
-      Out << C;
-    else
-      Out << '\\' << hexdigit(C >> 4) << hexdigit(C & 0x0F);
-  }
-}
-
-void wpi::printHTMLEscaped(StringRef String, raw_ostream &Out) {
-  for (char C : String) {
-    if (C == '&')
-      Out << "&amp;";
-    else if (C == '<')
-      Out << "&lt;";
-    else if (C == '>')
-      Out << "&gt;";
-    else if (C == '\"')
-      Out << "&quot;";
-    else if (C == '\'')
-      Out << "&apos;";
-    else
-      Out << C;
-  }
-}
-
-void wpi::printLowerCase(StringRef String, raw_ostream &Out) {
-  for (const char C : String)
-    Out << toLower(C);
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringMap.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringMap.cpp
index 5c625c7..768801d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringMap.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringMap.cpp
@@ -19,6 +19,21 @@
 
 using namespace wpi;
 
+/// HashString - Hash function for strings.
+///
+/// This is the Bernstein hash function.
+//
+// FIXME: Investigate whether a modified bernstein hash function performs
+// better: http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx
+//   X*33+c -> X*33^c
+static inline unsigned HashString(std::string_view str,
+                                  unsigned result = 0) noexcept {
+  for (std::string_view::size_type i = 0, e = str.size(); i != e; ++i) {
+    result = result * 33 + static_cast<unsigned char>(str[i]);
+  }
+  return result;
+}
+
 /// Returns the number of buckets to allocate to ensure that the DenseMap can
 /// accommodate \p NumEntries without need to grow().
 static unsigned getMinBucketToReserveForEntries(unsigned NumEntries) {
@@ -74,7 +89,7 @@
 /// specified bucket will be non-null.  Otherwise, it will be null.  In either
 /// case, the FullHashValue field of the bucket will be set to the hash value
 /// of the string.
-unsigned StringMapImpl::LookupBucketFor(StringRef Name) {
+unsigned StringMapImpl::LookupBucketFor(std::string_view Name) {
   unsigned HTSize = NumBuckets;
   if (HTSize == 0) {  // Hash table unallocated so far?
     init(16);
@@ -113,7 +128,7 @@
       // Do the comparison like this because Name isn't necessarily
       // null-terminated!
       char *ItemStr = (char*)BucketItem+ItemSize;
-      if (Name == StringRef(ItemStr, BucketItem->getKeyLength())) {
+      if (Name == std::string_view(ItemStr, BucketItem->getKeyLength())) {
         // We found a match!
         return BucketNo;
       }
@@ -131,7 +146,7 @@
 /// FindKey - Look up the bucket that contains the specified key. If it exists
 /// in the map, return the bucket number of the key.  Otherwise return -1.
 /// This does not modify the map.
-int StringMapImpl::FindKey(StringRef Key) const {
+int StringMapImpl::FindKey(std::string_view Key) const {
   unsigned HTSize = NumBuckets;
   if (HTSize == 0) return -1;  // Really empty table?
   unsigned FullHashValue = HashString(Key);
@@ -156,7 +171,7 @@
       // Do the comparison like this because NameStart isn't necessarily
       // null-terminated!
       char *ItemStr = (char*)BucketItem+ItemSize;
-      if (Key == StringRef(ItemStr, BucketItem->getKeyLength())) {
+      if (Key == std::string_view(ItemStr, BucketItem->getKeyLength())) {
         // We found a match!
         return BucketNo;
       }
@@ -175,14 +190,14 @@
 /// delete it.  This aborts if the value isn't in the table.
 void StringMapImpl::RemoveKey(StringMapEntryBase *V) {
   const char *VStr = (char*)V + ItemSize;
-  StringMapEntryBase *V2 = RemoveKey(StringRef(VStr, V->getKeyLength()));
+  StringMapEntryBase *V2 = RemoveKey(std::string_view(VStr, V->getKeyLength()));
   (void)V2;
   assert(V == V2 && "Didn't find key?");
 }
 
 /// RemoveKey - Remove the StringMapEntry for the specified key from the
 /// table, returning it.  If the key is not in the table, this returns null.
-StringMapEntryBase *StringMapImpl::RemoveKey(StringRef Key) {
+StringMapEntryBase *StringMapImpl::RemoveKey(std::string_view Key) {
   int Bucket = FindKey(Key);
   if (Bucket == -1) return nullptr;
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringRef.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringRef.cpp
deleted file mode 100644
index ea44bea..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/StringRef.cpp
+++ /dev/null
@@ -1,507 +0,0 @@
-//===-- StringRef.cpp - Lightweight String References ---------------------===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#include "wpi/StringRef.h"
-#include "wpi/Hashing.h"
-#include "wpi/StringExtras.h"
-#include "wpi/SmallVector.h"
-#include <bitset>
-#include <climits>
-#include <ostream>
-
-using namespace wpi;
-
-// MSVC emits references to this into the translation units which reference it.
-#ifndef _MSC_VER
-const size_t StringRef::npos;
-#endif
-
-// strncasecmp() is not available on non-POSIX systems, so define an
-// alternative function here.
-static int ascii_strncasecmp(const char *LHS, const char *RHS, size_t Length) noexcept {
-  for (size_t I = 0; I < Length; ++I) {
-    unsigned char LHC = toLower(LHS[I]);
-    unsigned char RHC = toLower(RHS[I]);
-    if (LHC != RHC)
-      return LHC < RHC ? -1 : 1;
-  }
-  return 0;
-}
-
-/// compare_lower - Compare strings, ignoring case.
-int StringRef::compare_lower(StringRef RHS) const noexcept {
-  if (int Res = ascii_strncasecmp(Data, RHS.Data, std::min(Length, RHS.Length)))
-    return Res;
-  if (Length == RHS.Length)
-    return 0;
-  return Length < RHS.Length ? -1 : 1;
-}
-
-/// Check if this string starts with the given \p Prefix, ignoring case.
-bool StringRef::startswith_lower(StringRef Prefix) const noexcept {
-  return Length >= Prefix.Length &&
-      ascii_strncasecmp(Data, Prefix.Data, Prefix.Length) == 0;
-}
-
-/// Check if this string ends with the given \p Suffix, ignoring case.
-bool StringRef::endswith_lower(StringRef Suffix) const noexcept {
-  return Length >= Suffix.Length &&
-      ascii_strncasecmp(end() - Suffix.Length, Suffix.Data, Suffix.Length) == 0;
-}
-
-size_t StringRef::find_lower(char C, size_t From) const noexcept {
-  char L = toLower(C);
-  return find_if([L](char D) { return toLower(D) == L; }, From);
-}
-
-/// compare_numeric - Compare strings, handle embedded numbers.
-int StringRef::compare_numeric(StringRef RHS) const noexcept {
-  for (size_t I = 0, E = std::min(Length, RHS.Length); I != E; ++I) {
-    // Check for sequences of digits.
-    if (isDigit(Data[I]) && isDigit(RHS.Data[I])) {
-      // The longer sequence of numbers is considered larger.
-      // This doesn't really handle prefixed zeros well.
-      size_t J;
-      for (J = I + 1; J != E + 1; ++J) {
-        bool ld = J < Length && isDigit(Data[J]);
-        bool rd = J < RHS.Length && isDigit(RHS.Data[J]);
-        if (ld != rd)
-          return rd ? -1 : 1;
-        if (!rd)
-          break;
-      }
-      // The two number sequences have the same length (J-I), just memcmp them.
-      if (int Res = compareMemory(Data + I, RHS.Data + I, J - I))
-        return Res < 0 ? -1 : 1;
-      // Identical number sequences, continue search after the numbers.
-      I = J - 1;
-      continue;
-    }
-    if (Data[I] != RHS.Data[I])
-      return (unsigned char)Data[I] < (unsigned char)RHS.Data[I] ? -1 : 1;
-  }
-  if (Length == RHS.Length)
-    return 0;
-  return Length < RHS.Length ? -1 : 1;
-}
-
-//===----------------------------------------------------------------------===//
-// String Operations
-//===----------------------------------------------------------------------===//
-
-std::string StringRef::lower() const {
-  std::string Result(size(), char());
-  for (size_type i = 0, e = size(); i != e; ++i) {
-    Result[i] = toLower(Data[i]);
-  }
-  return Result;
-}
-
-std::string StringRef::upper() const {
-  std::string Result(size(), char());
-  for (size_type i = 0, e = size(); i != e; ++i) {
-    Result[i] = toUpper(Data[i]);
-  }
-  return Result;
-}
-
-//===----------------------------------------------------------------------===//
-// String Searching
-//===----------------------------------------------------------------------===//
-
-
-/// find - Search for the first string \arg Str in the string.
-///
-/// \return - The index of the first occurrence of \arg Str, or npos if not
-/// found.
-size_t StringRef::find(StringRef Str, size_t From) const noexcept {
-  if (From > Length)
-    return npos;
-
-  const char *Start = Data + From;
-  size_t Size = Length - From;
-
-  const char *Needle = Str.data();
-  size_t N = Str.size();
-  if (N == 0)
-    return From;
-  if (Size < N)
-    return npos;
-  if (N == 1) {
-    const char *Ptr = (const char *)::memchr(Start, Needle[0], Size);
-    return Ptr == nullptr ? npos : Ptr - Data;
-  }
-
-  const char *Stop = Start + (Size - N + 1);
-
-  // For short haystacks or unsupported needles fall back to the naive algorithm
-  if (Size < 16 || N > 255) {
-    do {
-      if (std::memcmp(Start, Needle, N) == 0)
-        return Start - Data;
-      ++Start;
-    } while (Start < Stop);
-    return npos;
-  }
-
-  // Build the bad char heuristic table, with uint8_t to reduce cache thrashing.
-  uint8_t BadCharSkip[256];
-  std::memset(BadCharSkip, N, 256);
-  for (unsigned i = 0; i != N-1; ++i)
-    BadCharSkip[(uint8_t)Str[i]] = N-1-i;
-
-  do {
-    uint8_t Last = Start[N - 1];
-    if (LLVM_UNLIKELY(Last == (uint8_t)Needle[N - 1]))
-      if (std::memcmp(Start, Needle, N - 1) == 0)
-        return Start - Data;
-
-    // Otherwise skip the appropriate number of bytes.
-    Start += BadCharSkip[Last];
-  } while (Start < Stop);
-
-  return npos;
-}
-
-size_t StringRef::find_lower(StringRef Str, size_t From) const noexcept {
-  StringRef This = substr(From);
-  while (This.size() >= Str.size()) {
-    if (This.startswith_lower(Str))
-      return From;
-    This = This.drop_front();
-    ++From;
-  }
-  return npos;
-}
-
-size_t StringRef::rfind_lower(char C, size_t From) const noexcept {
-  From = std::min(From, Length);
-  size_t i = From;
-  while (i != 0) {
-    --i;
-    if (toLower(Data[i]) == toLower(C))
-      return i;
-  }
-  return npos;
-}
-
-/// rfind - Search for the last string \arg Str in the string.
-///
-/// \return - The index of the last occurrence of \arg Str, or npos if not
-/// found.
-size_t StringRef::rfind(StringRef Str) const noexcept {
-  size_t N = Str.size();
-  if (N > Length)
-    return npos;
-  for (size_t i = Length - N + 1, e = 0; i != e;) {
-    --i;
-    if (substr(i, N).equals(Str))
-      return i;
-  }
-  return npos;
-}
-
-size_t StringRef::rfind_lower(StringRef Str) const noexcept {
-  size_t N = Str.size();
-  if (N > Length)
-    return npos;
-  for (size_t i = Length - N + 1, e = 0; i != e;) {
-    --i;
-    if (substr(i, N).equals_lower(Str))
-      return i;
-  }
-  return npos;
-}
-
-/// find_first_of - Find the first character in the string that is in \arg
-/// Chars, or npos if not found.
-///
-/// Note: O(size() + Chars.size())
-StringRef::size_type StringRef::find_first_of(StringRef Chars,
-                                              size_t From) const noexcept {
-  std::bitset<1 << CHAR_BIT> CharBits;
-  for (size_type i = 0; i != Chars.size(); ++i)
-    CharBits.set((unsigned char)Chars[i]);
-
-  for (size_type i = std::min(From, Length), e = Length; i != e; ++i)
-    if (CharBits.test((unsigned char)Data[i]))
-      return i;
-  return npos;
-}
-
-/// find_first_not_of - Find the first character in the string that is not
-/// \arg C or npos if not found.
-StringRef::size_type StringRef::find_first_not_of(char C, size_t From) const noexcept {
-  for (size_type i = std::min(From, Length), e = Length; i != e; ++i)
-    if (Data[i] != C)
-      return i;
-  return npos;
-}
-
-/// find_first_not_of - Find the first character in the string that is not
-/// in the string \arg Chars, or npos if not found.
-///
-/// Note: O(size() + Chars.size())
-StringRef::size_type StringRef::find_first_not_of(StringRef Chars,
-                                                  size_t From) const noexcept {
-  std::bitset<1 << CHAR_BIT> CharBits;
-  for (size_type i = 0; i != Chars.size(); ++i)
-    CharBits.set((unsigned char)Chars[i]);
-
-  for (size_type i = std::min(From, Length), e = Length; i != e; ++i)
-    if (!CharBits.test((unsigned char)Data[i]))
-      return i;
-  return npos;
-}
-
-/// find_last_of - Find the last character in the string that is in \arg C,
-/// or npos if not found.
-///
-/// Note: O(size() + Chars.size())
-StringRef::size_type StringRef::find_last_of(StringRef Chars,
-                                             size_t From) const noexcept {
-  std::bitset<1 << CHAR_BIT> CharBits;
-  for (size_type i = 0; i != Chars.size(); ++i)
-    CharBits.set((unsigned char)Chars[i]);
-
-  for (size_type i = std::min(From, Length) - 1, e = -1; i != e; --i)
-    if (CharBits.test((unsigned char)Data[i]))
-      return i;
-  return npos;
-}
-
-/// find_last_not_of - Find the last character in the string that is not
-/// \arg C, or npos if not found.
-StringRef::size_type StringRef::find_last_not_of(char C, size_t From) const noexcept {
-  for (size_type i = std::min(From, Length) - 1, e = -1; i != e; --i)
-    if (Data[i] != C)
-      return i;
-  return npos;
-}
-
-/// find_last_not_of - Find the last character in the string that is not in
-/// \arg Chars, or npos if not found.
-///
-/// Note: O(size() + Chars.size())
-StringRef::size_type StringRef::find_last_not_of(StringRef Chars,
-                                                 size_t From) const noexcept {
-  std::bitset<1 << CHAR_BIT> CharBits;
-  for (size_type i = 0, e = Chars.size(); i != e; ++i)
-    CharBits.set((unsigned char)Chars[i]);
-
-  for (size_type i = std::min(From, Length) - 1, e = -1; i != e; --i)
-    if (!CharBits.test((unsigned char)Data[i]))
-      return i;
-  return npos;
-}
-
-void StringRef::split(SmallVectorImpl<StringRef> &A,
-                      StringRef Separator, int MaxSplit,
-                      bool KeepEmpty) const {
-  StringRef S = *this;
-
-  // Count down from MaxSplit. When MaxSplit is -1, this will just split
-  // "forever". This doesn't support splitting more than 2^31 times
-  // intentionally; if we ever want that we can make MaxSplit a 64-bit integer
-  // but that seems unlikely to be useful.
-  while (MaxSplit-- != 0) {
-    size_t Idx = S.find(Separator);
-    if (Idx == npos)
-      break;
-
-    // Push this split.
-    if (KeepEmpty || Idx > 0)
-      A.push_back(S.slice(0, Idx));
-
-    // Jump forward.
-    S = S.slice(Idx + Separator.size(), npos);
-  }
-
-  // Push the tail.
-  if (KeepEmpty || !S.empty())
-    A.push_back(S);
-}
-
-void StringRef::split(SmallVectorImpl<StringRef> &A, char Separator,
-                      int MaxSplit, bool KeepEmpty) const {
-  StringRef S = *this;
-
-  // Count down from MaxSplit. When MaxSplit is -1, this will just split
-  // "forever". This doesn't support splitting more than 2^31 times
-  // intentionally; if we ever want that we can make MaxSplit a 64-bit integer
-  // but that seems unlikely to be useful.
-  while (MaxSplit-- != 0) {
-    size_t Idx = S.find(Separator);
-    if (Idx == npos)
-      break;
-
-    // Push this split.
-    if (KeepEmpty || Idx > 0)
-      A.push_back(S.slice(0, Idx));
-
-    // Jump forward.
-    S = S.slice(Idx + 1, npos);
-  }
-
-  // Push the tail.
-  if (KeepEmpty || !S.empty())
-    A.push_back(S);
-}
-
-//===----------------------------------------------------------------------===//
-// Helpful Algorithms
-//===----------------------------------------------------------------------===//
-
-/// count - Return the number of non-overlapped occurrences of \arg Str in
-/// the string.
-size_t StringRef::count(StringRef Str) const noexcept {
-  size_t Count = 0;
-  size_t N = Str.size();
-  if (N > Length)
-    return 0;
-  for (size_t i = 0, e = Length - N + 1; i != e; ++i)
-    if (substr(i, N).equals(Str))
-      ++Count;
-  return Count;
-}
-
-static unsigned GetAutoSenseRadix(StringRef &Str) noexcept {
-  if (Str.empty())
-    return 10;
-
-  if (Str.startswith("0x") || Str.startswith("0X")) {
-    Str = Str.substr(2);
-    return 16;
-  }
-
-  if (Str.startswith("0b") || Str.startswith("0B")) {
-    Str = Str.substr(2);
-    return 2;
-  }
-
-  if (Str.startswith("0o")) {
-    Str = Str.substr(2);
-    return 8;
-  }
-
-  if (Str[0] == '0' && Str.size() > 1 && isDigit(Str[1])) {
-    Str = Str.substr(1);
-    return 8;
-  }
-
-  return 10;
-}
-
-bool wpi::consumeUnsignedInteger(StringRef &Str, unsigned Radix,
-                                  unsigned long long &Result) noexcept {
-  // Autosense radix if not specified.
-  if (Radix == 0)
-    Radix = GetAutoSenseRadix(Str);
-
-  // Empty strings (after the radix autosense) are invalid.
-  if (Str.empty()) return true;
-
-  // Parse all the bytes of the string given this radix.  Watch for overflow.
-  StringRef Str2 = Str;
-  Result = 0;
-  while (!Str2.empty()) {
-    unsigned CharVal;
-    if (Str2[0] >= '0' && Str2[0] <= '9')
-      CharVal = Str2[0] - '0';
-    else if (Str2[0] >= 'a' && Str2[0] <= 'z')
-      CharVal = Str2[0] - 'a' + 10;
-    else if (Str2[0] >= 'A' && Str2[0] <= 'Z')
-      CharVal = Str2[0] - 'A' + 10;
-    else
-      break;
-
-    // If the parsed value is larger than the integer radix, we cannot
-    // consume any more characters.
-    if (CharVal >= Radix)
-      break;
-
-    // Add in this character.
-    unsigned long long PrevResult = Result;
-    Result = Result * Radix + CharVal;
-
-    // Check for overflow by shifting back and seeing if bits were lost.
-    if (Result / Radix < PrevResult)
-      return true;
-
-    Str2 = Str2.substr(1);
-  }
-
-  // We consider the operation a failure if no characters were consumed
-  // successfully.
-  if (Str.size() == Str2.size())
-    return true;
-
-  Str = Str2;
-  return false;
-}
-
-bool wpi::consumeSignedInteger(StringRef &Str, unsigned Radix,
-                                long long &Result) noexcept {
-  unsigned long long ULLVal;
-
-  // Handle positive strings first.
-  if (Str.empty() || Str.front() != '-') {
-    if (consumeUnsignedInteger(Str, Radix, ULLVal) ||
-        // Check for value so large it overflows a signed value.
-        (long long)ULLVal < 0)
-      return true;
-    Result = ULLVal;
-    return false;
-  }
-
-  // Get the positive part of the value.
-  StringRef Str2 = Str.drop_front(1);
-  if (consumeUnsignedInteger(Str2, Radix, ULLVal) ||
-      // Reject values so large they'd overflow as negative signed, but allow
-      // "-0".  This negates the unsigned so that the negative isn't undefined
-      // on signed overflow.
-      (long long)-ULLVal > 0)
-    return true;
-
-  Str = Str2;
-  Result = -ULLVal;
-  return false;
-}
-
-/// GetAsUnsignedInteger - Workhorse method that converts a integer character
-/// sequence of radix up to 36 to an unsigned long long value.
-bool wpi::getAsUnsignedInteger(StringRef Str, unsigned Radix,
-                                unsigned long long &Result) noexcept {
-  if (consumeUnsignedInteger(Str, Radix, Result))
-    return true;
-
-  // For getAsUnsignedInteger, we require the whole string to be consumed or
-  // else we consider it a failure.
-  return !Str.empty();
-}
-
-bool wpi::getAsSignedInteger(StringRef Str, unsigned Radix,
-                              long long &Result) noexcept {
-  if (consumeSignedInteger(Str, Radix, Result))
-    return true;
-
-  // For getAsSignedInteger, we require the whole string to be consumed or else
-  // we consider it a failure.
-  return !Str.empty();
-}
-
-std::ostream &wpi::operator<<(std::ostream &os, StringRef string) {
-  os.write(string.data(), string.size());
-  return os;
-}
-
-// Implementation of StringRef hashing.
-hash_code wpi::hash_value(StringRef S) {
-  return hash_combine_range(S.begin(), S.end());
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Twine.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Twine.cpp
deleted file mode 100644
index ac705ff..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Twine.cpp
+++ /dev/null
@@ -1,169 +0,0 @@
-//===-- Twine.cpp - Fast Temporary String Concatenation -------------------===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#include "wpi/Twine.h"
-#include "wpi/SmallString.h"
-#include "wpi/raw_ostream.h"
-using namespace wpi;
-
-std::string Twine::str() const {
-  // If we're storing only a std::string, just return it.
-  if (LHSKind == StdStringKind && RHSKind == EmptyKind)
-    return *LHS.stdString;
-
-  // Otherwise, flatten and copy the contents first.
-  SmallString<256> Vec;
-  return toStringRef(Vec).str();
-}
-
-void Twine::toVector(SmallVectorImpl<char> &Out) const {
-  raw_svector_ostream OS(Out);
-  print(OS);
-}
-
-StringRef Twine::toNullTerminatedStringRef(SmallVectorImpl<char> &Out) const {
-  if (isUnary()) {
-    switch (getLHSKind()) {
-    case CStringKind:
-      // Already null terminated, yay!
-      return StringRef(LHS.cString);
-    case StdStringKind: {
-      const std::string *str = LHS.stdString;
-      return StringRef(str->c_str(), str->size());
-    }
-    default:
-      break;
-    }
-  }
-  toVector(Out);
-  Out.push_back(0);
-  Out.pop_back();
-  return StringRef(Out.data(), Out.size());
-}
-
-void Twine::printOneChild(raw_ostream &OS, Child Ptr,
-                          NodeKind Kind) const {
-  switch (Kind) {
-  case Twine::NullKind: break;
-  case Twine::EmptyKind: break;
-  case Twine::TwineKind:
-    Ptr.twine->print(OS);
-    break;
-  case Twine::CStringKind:
-    OS << Ptr.cString;
-    break;
-  case Twine::StdStringKind:
-    OS << *Ptr.stdString;
-    break;
-  case Twine::StringRefKind:
-    OS << *Ptr.stringRef;
-    break;
-  case Twine::SmallStringKind:
-    OS << *Ptr.smallString;
-    break;
-  case Twine::CharKind:
-    OS << Ptr.character;
-    break;
-  case Twine::DecUIKind:
-    OS << Ptr.decUI;
-    break;
-  case Twine::DecIKind:
-    OS << Ptr.decI;
-    break;
-  case Twine::DecULKind:
-    OS << *Ptr.decUL;
-    break;
-  case Twine::DecLKind:
-    OS << *Ptr.decL;
-    break;
-  case Twine::DecULLKind:
-    OS << *Ptr.decULL;
-    break;
-  case Twine::DecLLKind:
-    OS << *Ptr.decLL;
-    break;
-  case Twine::UHexKind:
-    OS.write_hex(*Ptr.uHex);
-    break;
-  }
-}
-
-void Twine::printOneChildRepr(raw_ostream &OS, Child Ptr,
-                              NodeKind Kind) const {
-  switch (Kind) {
-  case Twine::NullKind:
-    OS << "null"; break;
-  case Twine::EmptyKind:
-    OS << "empty"; break;
-  case Twine::TwineKind:
-    OS << "rope:";
-    Ptr.twine->printRepr(OS);
-    break;
-  case Twine::CStringKind:
-    OS << "cstring:\""
-       << Ptr.cString << "\"";
-    break;
-  case Twine::StdStringKind:
-    OS << "std::string:\""
-       << Ptr.stdString << "\"";
-    break;
-  case Twine::StringRefKind:
-    OS << "stringref:\""
-       << Ptr.stringRef << "\"";
-    break;
-  case Twine::SmallStringKind:
-    OS << "smallstring:\"" << *Ptr.smallString << "\"";
-    break;
-  case Twine::CharKind:
-    OS << "char:\"" << Ptr.character << "\"";
-    break;
-  case Twine::DecUIKind:
-    OS << "decUI:\"" << Ptr.decUI << "\"";
-    break;
-  case Twine::DecIKind:
-    OS << "decI:\"" << Ptr.decI << "\"";
-    break;
-  case Twine::DecULKind:
-    OS << "decUL:\"" << *Ptr.decUL << "\"";
-    break;
-  case Twine::DecLKind:
-    OS << "decL:\"" << *Ptr.decL << "\"";
-    break;
-  case Twine::DecULLKind:
-    OS << "decULL:\"" << *Ptr.decULL << "\"";
-    break;
-  case Twine::DecLLKind:
-    OS << "decLL:\"" << *Ptr.decLL << "\"";
-    break;
-  case Twine::UHexKind:
-    OS << "uhex:\"" << Ptr.uHex << "\"";
-    break;
-  }
-}
-
-void Twine::print(raw_ostream &OS) const {
-  printOneChild(OS, LHS, getLHSKind());
-  printOneChild(OS, RHS, getRHSKind());
-}
-
-void Twine::printRepr(raw_ostream &OS) const {
-  OS << "(Twine ";
-  printOneChildRepr(OS, LHS, getLHSKind());
-  OS << " ";
-  printOneChildRepr(OS, RHS, getRHSKind());
-  OS << ")";
-}
-
-LLVM_DUMP_METHOD void Twine::dump() const {
-  print(errs());
-}
-
-LLVM_DUMP_METHOD void Twine::dumpRepr() const {
-  printRepr(errs());
-}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc
deleted file mode 100644
index 4ec9e86..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Unix/Path.inc
+++ /dev/null
@@ -1,539 +0,0 @@
-//===- llvm/Support/Unix/Path.inc - Unix Path Implementation ----*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file implements the Unix specific implementation of the Path API.
-//
-//===----------------------------------------------------------------------===//
-
-//===----------------------------------------------------------------------===//
-//=== WARNING: Implementation here must contain only generic UNIX code that
-//===          is guaranteed to work on *all* UNIX variants.
-//===----------------------------------------------------------------------===//
-
-#include "wpi/Errno.h"
-#include <fcntl.h>
-#include <limits.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <sys/mman.h>
-#include <dirent.h>
-#include <pwd.h>
-#include <sys/param.h>
-#include <sys/types.h>
-#include <unistd.h>
-
-using namespace wpi;
-
-namespace wpi {
-namespace sys  {
-namespace fs {
-
-const file_t kInvalidFile = -1;
-
-TimePoint<> basic_file_status::getLastAccessedTime() const {
-  return toTimePoint(fs_st_atime, fs_st_atime_nsec);
-}
-
-TimePoint<> basic_file_status::getLastModificationTime() const {
-  return toTimePoint(fs_st_mtime, fs_st_mtime_nsec);
-}
-
-UniqueID file_status::getUniqueID() const {
-  return UniqueID(fs_st_dev, fs_st_ino);
-}
-
-uint32_t file_status::getLinkCount() const {
-  return fs_st_nlinks;
-}
-
-std::error_code current_path(SmallVectorImpl<char> &result) {
-  result.clear();
-
-  const char *pwd = ::getenv("PWD");
-  wpi::sys::fs::file_status PWDStatus, DotStatus;
-  if (pwd && wpi::sys::path::is_absolute(pwd) &&
-      !wpi::sys::fs::status(pwd, PWDStatus) &&
-      !wpi::sys::fs::status(".", DotStatus) &&
-      PWDStatus.getUniqueID() == DotStatus.getUniqueID()) {
-    result.append(pwd, pwd + strlen(pwd));
-    return std::error_code();
-  }
-
-#ifdef MAXPATHLEN
-  result.reserve(MAXPATHLEN);
-#else
-  result.reserve(1024);
-#endif
-
-  while (true) {
-    if (::getcwd(result.data(), result.capacity()) == nullptr) {
-      // See if there was a real error.
-      if (errno != ENOMEM)
-        return std::error_code(errno, std::generic_category());
-      // Otherwise there just wasn't enough space.
-      result.reserve(result.capacity() * 2);
-    } else
-      break;
-  }
-
-  result.set_size(strlen(result.data()));
-  return std::error_code();
-}
-
-static int convertAccessMode(AccessMode Mode) {
-  switch (Mode) {
-  case AccessMode::Exist:
-    return F_OK;
-  case AccessMode::Write:
-    return W_OK;
-  case AccessMode::Execute:
-    return R_OK | X_OK; // scripts also need R_OK.
-  default:
-    return F_OK;
-  }
-}
-
-std::error_code access(const Twine &Path, AccessMode Mode) {
-  SmallString<128> PathStorage;
-  StringRef P = Path.toNullTerminatedStringRef(PathStorage);
-
-  if (::access(P.begin(), convertAccessMode(Mode)) == -1)
-    return std::error_code(errno, std::generic_category());
-
-  if (Mode == AccessMode::Execute) {
-    // Don't say that directories are executable.
-    struct stat buf;
-    if (0 != stat(P.begin(), &buf))
-      return errc::permission_denied;
-    if (!S_ISREG(buf.st_mode))
-      return errc::permission_denied;
-  }
-
-  return std::error_code();
-}
-
-bool equivalent(file_status A, file_status B) {
-  assert(status_known(A) && status_known(B));
-  return A.fs_st_dev == B.fs_st_dev &&
-         A.fs_st_ino == B.fs_st_ino;
-}
-
-std::error_code equivalent(const Twine &A, const Twine &B, bool &result) {
-  file_status fsA, fsB;
-  if (std::error_code ec = status(A, fsA))
-    return ec;
-  if (std::error_code ec = status(B, fsB))
-    return ec;
-  result = equivalent(fsA, fsB);
-  return std::error_code();
-}
-
-static file_type typeForMode(mode_t Mode) {
-  if (S_ISDIR(Mode))
-    return file_type::directory_file;
-  else if (S_ISREG(Mode))
-    return file_type::regular_file;
-  else if (S_ISBLK(Mode))
-    return file_type::block_file;
-  else if (S_ISCHR(Mode))
-    return file_type::character_file;
-  else if (S_ISFIFO(Mode))
-    return file_type::fifo_file;
-  else if (S_ISSOCK(Mode))
-    return file_type::socket_file;
-  else if (S_ISLNK(Mode))
-    return file_type::symlink_file;
-  return file_type::type_unknown;
-}
-
-static std::error_code fillStatus(int StatRet, const struct stat &Status,
-                                  file_status &Result) {
-  if (StatRet != 0) {
-    std::error_code EC(errno, std::generic_category());
-    if (EC == errc::no_such_file_or_directory)
-      Result = file_status(file_type::file_not_found);
-    else
-      Result = file_status(file_type::status_error);
-    return EC;
-  }
-
-  uint32_t atime_nsec, mtime_nsec;
-#if defined(__APPLE__)
-  atime_nsec = Status.st_atimespec.tv_nsec;
-  mtime_nsec = Status.st_mtimespec.tv_nsec;
-#else
-  atime_nsec = Status.st_atim.tv_nsec;
-  mtime_nsec = Status.st_mtim.tv_nsec;
-#endif
-
-  perms Perms = static_cast<perms>(Status.st_mode) & all_perms;
-  Result = file_status(typeForMode(Status.st_mode), Perms, Status.st_dev,
-                       Status.st_nlink, Status.st_ino,
-                       Status.st_atime, atime_nsec, Status.st_mtime, mtime_nsec,
-                       Status.st_uid, Status.st_gid, Status.st_size);
-
-  return std::error_code();
-}
-
-std::error_code status(const Twine &Path, file_status &Result, bool Follow) {
-  SmallString<128> PathStorage;
-  StringRef P = Path.toNullTerminatedStringRef(PathStorage);
-
-  struct stat Status;
-  int StatRet = (Follow ? ::stat : ::lstat)(P.begin(), &Status);
-  return fillStatus(StatRet, Status, Result);
-}
-
-std::error_code status(int FD, file_status &Result) {
-  struct stat Status;
-  int StatRet = ::fstat(FD, &Status);
-  return fillStatus(StatRet, Status, Result);
-}
-
-std::error_code mapped_file_region::init(int FD, uint64_t Offset,
-                                         mapmode Mode) {
-  assert(Size != 0);
-
-  int flags = (Mode == readwrite) ? MAP_SHARED : MAP_PRIVATE;
-  int prot = (Mode == readonly) ? PROT_READ : (PROT_READ | PROT_WRITE);
-#if defined(__APPLE__)
-  //----------------------------------------------------------------------
-  // Newer versions of MacOSX have a flag that will allow us to read from
-  // binaries whose code signature is invalid without crashing by using
-  // the MAP_RESILIENT_CODESIGN flag. Also if a file from removable media
-  // is mapped we can avoid crashing and return zeroes to any pages we try
-  // to read if the media becomes unavailable by using the
-  // MAP_RESILIENT_MEDIA flag.  These flags are only usable when mapping
-  // with PROT_READ, so take care not to specify them otherwise.
-  //----------------------------------------------------------------------
-  if (Mode == readonly) {
-#if defined(MAP_RESILIENT_CODESIGN)
-    flags |= MAP_RESILIENT_CODESIGN;
-#endif
-#if defined(MAP_RESILIENT_MEDIA)
-    flags |= MAP_RESILIENT_MEDIA;
-#endif
-  }
-#endif // #if defined (__APPLE__)
-
-  Mapping = ::mmap(nullptr, Size, prot, flags, FD, Offset);
-  if (Mapping == MAP_FAILED)
-    return std::error_code(errno, std::generic_category());
-  return std::error_code();
-}
-
-mapped_file_region::mapped_file_region(int fd, mapmode mode, size_t length,
-                                       uint64_t offset, std::error_code &ec)
-    : Size(length), Mapping(), Mode(mode) {
-  (void)Mode;
-  ec = init(fd, offset, mode);
-  if (ec)
-    Mapping = nullptr;
-}
-
-mapped_file_region::~mapped_file_region() {
-  if (Mapping)
-    ::munmap(Mapping, Size);
-}
-
-size_t mapped_file_region::size() const {
-  assert(Mapping && "Mapping failed but used anyway!");
-  return Size;
-}
-
-char *mapped_file_region::data() const {
-  assert(Mapping && "Mapping failed but used anyway!");
-  return reinterpret_cast<char*>(Mapping);
-}
-
-const char *mapped_file_region::const_data() const {
-  assert(Mapping && "Mapping failed but used anyway!");
-  return reinterpret_cast<const char*>(Mapping);
-}
-
-int mapped_file_region::alignment() {
-  return ::sysconf(_SC_PAGE_SIZE);
-}
-
-std::error_code detail::directory_iterator_construct(detail::DirIterState &it,
-                                                     StringRef path,
-                                                     bool follow_symlinks) {
-  SmallString<128> path_null(path);
-  DIR *directory = ::opendir(path_null.c_str());
-  if (!directory)
-    return std::error_code(errno, std::generic_category());
-
-  it.IterationHandle = reinterpret_cast<intptr_t>(directory);
-  // Add something for replace_filename to replace.
-  path::append(path_null, ".");
-  it.CurrentEntry = directory_entry(path_null.str(), follow_symlinks);
-  return directory_iterator_increment(it);
-}
-
-std::error_code detail::directory_iterator_destruct(detail::DirIterState &it) {
-  if (it.IterationHandle)
-    ::closedir(reinterpret_cast<DIR *>(it.IterationHandle));
-  it.IterationHandle = 0;
-  it.CurrentEntry = directory_entry();
-  return std::error_code();
-}
-
-static file_type direntType(dirent* Entry) {
-  // Most platforms provide the file type in the dirent: Linux/BSD/Mac.
-  // The DTTOIF macro lets us reuse our status -> type conversion.
-  return typeForMode(DTTOIF(Entry->d_type));
-}
-
-std::error_code detail::directory_iterator_increment(detail::DirIterState &It) {
-  errno = 0;
-  dirent *CurDir = ::readdir(reinterpret_cast<DIR *>(It.IterationHandle));
-  if (CurDir == nullptr && errno != 0) {
-    return std::error_code(errno, std::generic_category());
-  } else if (CurDir != nullptr) {
-    StringRef Name(CurDir->d_name);
-    if ((Name.size() == 1 && Name[0] == '.') ||
-        (Name.size() == 2 && Name[0] == '.' && Name[1] == '.'))
-      return directory_iterator_increment(It);
-    It.CurrentEntry.replace_filename(Name, direntType(CurDir));
-  } else
-    return directory_iterator_destruct(It);
-
-  return std::error_code();
-}
-
-ErrorOr<basic_file_status> directory_entry::status() const {
-  file_status s;
-  if (auto EC = fs::status(Path, s, FollowSymlinks))
-    return EC;
-  return s;
-}
-
-#if !defined(F_GETPATH)
-static bool hasProcSelfFD() {
-  // If we have a /proc filesystem mounted, we can quickly establish the
-  // real name of the file with readlink
-  static const bool Result = (::access("/proc/self/fd", R_OK) == 0);
-  return Result;
-}
-#endif
-
-static int nativeOpenFlags(CreationDisposition Disp, OpenFlags Flags,
-                           FileAccess Access) {
-  int Result = 0;
-  if (Access == FA_Read)
-    Result |= O_RDONLY;
-  else if (Access == FA_Write)
-    Result |= O_WRONLY;
-  else if (Access == (FA_Read | FA_Write))
-    Result |= O_RDWR;
-
-  // This is for compatibility with old code that assumed F_Append implied
-  // would open an existing file.  See Windows/Path.inc for a longer comment.
-  if (Flags & F_Append)
-    Disp = CD_OpenAlways;
-
-  if (Disp == CD_CreateNew) {
-    Result |= O_CREAT; // Create if it doesn't exist.
-    Result |= O_EXCL;  // Fail if it does.
-  } else if (Disp == CD_CreateAlways) {
-    Result |= O_CREAT; // Create if it doesn't exist.
-    Result |= O_TRUNC; // Truncate if it does.
-  } else if (Disp == CD_OpenAlways) {
-    Result |= O_CREAT; // Create if it doesn't exist.
-  } else if (Disp == CD_OpenExisting) {
-    // Nothing special, just don't add O_CREAT and we get these semantics.
-  }
-
-  if (Flags & F_Append)
-    Result |= O_APPEND;
-
-#ifdef O_CLOEXEC
-  if (!(Flags & OF_ChildInherit))
-    Result |= O_CLOEXEC;
-#endif
-
-  return Result;
-}
-
-std::error_code openFile(const Twine &Name, int &ResultFD,
-                         CreationDisposition Disp, FileAccess Access,
-                         OpenFlags Flags, unsigned Mode) {
-  int OpenFlags = nativeOpenFlags(Disp, Flags, Access);
-
-  SmallString<128> Storage;
-  StringRef P = Name.toNullTerminatedStringRef(Storage);
-  // Call ::open in a lambda to avoid overload resolution in RetryAfterSignal
-  // when open is overloaded, such as in Bionic.
-  auto Open = [&]() { return ::open(P.begin(), OpenFlags, Mode); };
-  if ((ResultFD = sys::RetryAfterSignal(-1, Open)) < 0)
-    return std::error_code(errno, std::generic_category());
-#ifndef O_CLOEXEC
-  if (!(Flags & OF_ChildInherit)) {
-    int r = fcntl(ResultFD, F_SETFD, FD_CLOEXEC);
-    (void)r;
-    assert(r == 0 && "fcntl(F_SETFD, FD_CLOEXEC) failed");
-  }
-#endif
-  return std::error_code();
-}
-
-Expected<int> openNativeFile(const Twine &Name, CreationDisposition Disp,
-                             FileAccess Access, OpenFlags Flags,
-                             unsigned Mode) {
-
-  int FD;
-  std::error_code EC = openFile(Name, FD, Disp, Access, Flags, Mode);
-  if (EC)
-    return errorCodeToError(EC);
-  return FD;
-}
-
-std::error_code openFileForRead(const Twine &Name, int &ResultFD,
-                                OpenFlags Flags,
-                                SmallVectorImpl<char> *RealPath) {
-  std::error_code EC =
-      openFile(Name, ResultFD, CD_OpenExisting, FA_Read, Flags, 0666);
-  if (EC)
-    return EC;
-
-  // Attempt to get the real name of the file, if the user asked
-  if(!RealPath)
-    return std::error_code();
-  RealPath->clear();
-#if defined(F_GETPATH)
-  // When F_GETPATH is available, it is the quickest way to get
-  // the real path name.
-  char Buffer[MAXPATHLEN];
-  if (::fcntl(ResultFD, F_GETPATH, Buffer) != -1)
-    RealPath->append(Buffer, Buffer + strlen(Buffer));
-#else
-  char Buffer[PATH_MAX];
-  if (hasProcSelfFD()) {
-    char ProcPath[64];
-    snprintf(ProcPath, sizeof(ProcPath), "/proc/self/fd/%d", ResultFD);
-    ssize_t CharCount = ::readlink(ProcPath, Buffer, sizeof(Buffer));
-    if (CharCount > 0)
-      RealPath->append(Buffer, Buffer + CharCount);
-  } else {
-    SmallString<128> Storage;
-    StringRef P = Name.toNullTerminatedStringRef(Storage);
-
-    // Use ::realpath to get the real path name
-    if (::realpath(P.begin(), Buffer) != nullptr)
-      RealPath->append(Buffer, Buffer + strlen(Buffer));
-  }
-#endif
-  return std::error_code();
-}
-
-Expected<file_t> openNativeFileForRead(const Twine &Name, OpenFlags Flags,
-                                       SmallVectorImpl<char> *RealPath) {
-  file_t ResultFD;
-  std::error_code EC = openFileForRead(Name, ResultFD, Flags, RealPath);
-  if (EC)
-    return errorCodeToError(EC);
-  return ResultFD;
-}
-
-void closeFile(file_t &F) {
-  ::close(F);
-  F = kInvalidFile;
-}
-
-} // end namespace fs
-
-namespace path {
-
-bool home_directory(SmallVectorImpl<char> &result) {
-  char *RequestedDir = getenv("HOME");
-  if (!RequestedDir) {
-    struct passwd *pw = getpwuid(getuid());
-    if (pw && pw->pw_dir)
-      RequestedDir = pw->pw_dir;
-  }
-  if (!RequestedDir)
-    return false;
-
-  result.clear();
-  result.append(RequestedDir, RequestedDir + strlen(RequestedDir));
-  return true;
-}
-
-static bool getDarwinConfDir(bool TempDir, SmallVectorImpl<char> &Result) {
-  #if defined(_CS_DARWIN_USER_TEMP_DIR) && defined(_CS_DARWIN_USER_CACHE_DIR)
-  // On Darwin, use DARWIN_USER_TEMP_DIR or DARWIN_USER_CACHE_DIR.
-  // macros defined in <unistd.h> on darwin >= 9
-  int ConfName = TempDir ? _CS_DARWIN_USER_TEMP_DIR
-                         : _CS_DARWIN_USER_CACHE_DIR;
-  size_t ConfLen = confstr(ConfName, nullptr, 0);
-  if (ConfLen > 0) {
-    do {
-      Result.resize(ConfLen);
-      ConfLen = confstr(ConfName, Result.data(), Result.size());
-    } while (ConfLen > 0 && ConfLen != Result.size());
-
-    if (ConfLen > 0) {
-      assert(Result.back() == 0);
-      Result.pop_back();
-      return true;
-    }
-
-    Result.clear();
-  }
-  #endif
-  return false;
-}
-
-static const char *getEnvTempDir() {
-  // Check whether the temporary directory is specified by an environment
-  // variable.
-  const char *EnvironmentVariables[] = {"TMPDIR", "TMP", "TEMP", "TEMPDIR"};
-  for (const char *Env : EnvironmentVariables) {
-    if (const char *Dir = std::getenv(Env))
-      return Dir;
-  }
-
-  return nullptr;
-}
-
-static const char *getDefaultTempDir(bool ErasedOnReboot) {
-#ifdef P_tmpdir
-  if ((bool)P_tmpdir)
-    return P_tmpdir;
-#endif
-
-  if (ErasedOnReboot)
-    return "/tmp";
-  return "/var/tmp";
-}
-
-void system_temp_directory(bool ErasedOnReboot, SmallVectorImpl<char> &Result) {
-  Result.clear();
-
-  if (ErasedOnReboot) {
-    // There is no env variable for the cache directory.
-    if (const char *RequestedDir = getEnvTempDir()) {
-      Result.append(RequestedDir, RequestedDir + strlen(RequestedDir));
-      return;
-    }
-  }
-
-  if (getDarwinConfDir(ErasedOnReboot, Result))
-    return;
-
-  const char *RequestedDir = getDefaultTempDir(ErasedOnReboot);
-  Result.append(RequestedDir, RequestedDir + strlen(RequestedDir));
-}
-
-} // end namespace path
-} // end namespace sys
-} // end namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/Path.inc b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/Path.inc
deleted file mode 100644
index e9b5280..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/Path.inc
+++ /dev/null
@@ -1,971 +0,0 @@
-//===- llvm/Support/Windows/Path.inc - Windows Path Impl --------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file implements the Windows specific implementation of the Path API.
-//
-//===----------------------------------------------------------------------===//
-
-//===----------------------------------------------------------------------===//
-//=== WARNING: Implementation here must contain only generic Windows code that
-//===          is guaranteed to work on *all* Windows variants.
-//===----------------------------------------------------------------------===//
-
-#include "wpi/STLExtras.h"
-#include "wpi/ConvertUTF.h"
-#include "wpi/WindowsError.h"
-#include <fcntl.h>
-#include <io.h>
-#include <sys/stat.h>
-#include <sys/types.h>
-
-// These two headers must be included last, and make sure shlobj is required
-// after Windows.h to make sure it picks up our definition of _WIN32_WINNT
-#include "WindowsSupport.h"
-#include <shellapi.h>
-#include <shlobj.h>
-
-#undef max
-
-// MinGW doesn't define this.
-#ifndef _ERRNO_T_DEFINED
-#define _ERRNO_T_DEFINED
-typedef int errno_t;
-#endif
-
-#ifdef _MSC_VER
-# pragma comment(lib, "shell32.lib")
-# pragma comment(lib, "ole32.lib")
-#pragma warning(push)
-#pragma warning(disable: 4244 4267 4146)
-#endif
-
-using namespace wpi;
-
-using wpi::sys::windows::UTF8ToUTF16;
-using wpi::sys::windows::CurCPToUTF16;
-using wpi::sys::windows::UTF16ToUTF8;
-using wpi::sys::path::widenPath;
-
-static bool is_separator(const wchar_t value) {
-  switch (value) {
-  case L'\\':
-  case L'/':
-    return true;
-  default:
-    return false;
-  }
-}
-
-namespace wpi {
-namespace sys  {
-namespace path {
-
-// Convert a UTF-8 path to UTF-16.  Also, if the absolute equivalent of the
-// path is longer than CreateDirectory can tolerate, make it absolute and
-// prefixed by '\\?\'.
-std::error_code widenPath(const Twine &Path8,
-                          SmallVectorImpl<wchar_t> &Path16) {
-  const size_t MaxDirLen = MAX_PATH - 12; // Must leave room for 8.3 filename.
-
-  // Several operations would convert Path8 to SmallString; more efficient to
-  // do it once up front.
-  SmallString<128> Path8Str;
-  Path8.toVector(Path8Str);
-
-  // If we made this path absolute, how much longer would it get?
-  size_t CurPathLen;
-  if (wpi::sys::path::is_absolute(Twine(Path8Str)))
-    CurPathLen = 0; // No contribution from current_path needed.
-  else {
-    CurPathLen = ::GetCurrentDirectoryW(0, NULL);
-    if (CurPathLen == 0)
-      return mapWindowsError(::GetLastError());
-  }
-
-  // Would the absolute path be longer than our limit?
-  if ((Path8Str.size() + CurPathLen) >= MaxDirLen &&
-      !Path8Str.startswith("\\\\?\\")) {
-    SmallString<2*MAX_PATH> FullPath("\\\\?\\");
-    if (CurPathLen) {
-      SmallString<80> CurPath;
-      if (std::error_code EC = wpi::sys::fs::current_path(CurPath))
-        return EC;
-      FullPath.append(CurPath);
-    }
-    // Traverse the requested path, canonicalizing . and .. (because the \\?\
-    // prefix is documented to treat them as real components).  Ignore
-    // separators, which can be returned from the iterator if the path has a
-    // drive name.  We don't need to call native() on the result since append()
-    // always attaches preferred_separator.
-    for (wpi::sys::path::const_iterator I = wpi::sys::path::begin(Path8Str),
-                                         E = wpi::sys::path::end(Path8Str);
-                                         I != E; ++I) {
-      if (I->size() == 1 && is_separator((*I)[0]))
-        continue;
-      if (I->size() == 1 && *I == ".")
-        continue;
-      if (I->size() == 2 && *I == "..")
-        wpi::sys::path::remove_filename(FullPath);
-      else
-        wpi::sys::path::append(FullPath, *I);
-    }
-    return UTF8ToUTF16(FullPath, Path16);
-  }
-
-  // Just use the caller's original path.
-  return UTF8ToUTF16(Path8Str, Path16);
-}
-} // end namespace path
-
-namespace fs {
-
-const file_t kInvalidFile = INVALID_HANDLE_VALUE;
-
-UniqueID file_status::getUniqueID() const {
-  // The file is uniquely identified by the volume serial number along
-  // with the 64-bit file identifier.
-  uint64_t FileID = (static_cast<uint64_t>(FileIndexHigh) << 32ULL) |
-                    static_cast<uint64_t>(FileIndexLow);
-
-  return UniqueID(VolumeSerialNumber, FileID);
-}
-
-TimePoint<> basic_file_status::getLastAccessedTime() const {
-  FILETIME Time;
-  Time.dwLowDateTime = LastAccessedTimeLow;
-  Time.dwHighDateTime = LastAccessedTimeHigh;
-  return toTimePoint(Time);
-}
-
-TimePoint<> basic_file_status::getLastModificationTime() const {
-  FILETIME Time;
-  Time.dwLowDateTime = LastWriteTimeLow;
-  Time.dwHighDateTime = LastWriteTimeHigh;
-  return toTimePoint(Time);
-}
-
-uint32_t file_status::getLinkCount() const {
-  return NumLinks;
-}
-
-std::error_code current_path(SmallVectorImpl<char> &result) {
-  SmallVector<wchar_t, MAX_PATH> cur_path;
-  DWORD len = MAX_PATH;
-
-  do {
-    cur_path.reserve(len);
-    len = ::GetCurrentDirectoryW(cur_path.capacity(), cur_path.data());
-
-    // A zero return value indicates a failure other than insufficient space.
-    if (len == 0)
-      return mapWindowsError(::GetLastError());
-
-    // If there's insufficient space, the len returned is larger than the len
-    // given.
-  } while (len > cur_path.capacity());
-
-  // On success, GetCurrentDirectoryW returns the number of characters not
-  // including the null-terminator.
-  cur_path.set_size(len);
-  return UTF16ToUTF8(cur_path.begin(), cur_path.size(), result);
-}
-
-static std::error_code realPathFromHandle(HANDLE H,
-                                          SmallVectorImpl<wchar_t> &Buffer) {
-  DWORD CountChars = ::GetFinalPathNameByHandleW(
-      H, Buffer.begin(), Buffer.capacity() - 1, FILE_NAME_NORMALIZED);
-  if (CountChars > Buffer.capacity()) {
-    // The buffer wasn't big enough, try again.  In this case the return value
-    // *does* indicate the size of the null terminator.
-    Buffer.reserve(CountChars);
-    CountChars = ::GetFinalPathNameByHandleW(
-        H, Buffer.data(), Buffer.capacity() - 1, FILE_NAME_NORMALIZED);
-  }
-  if (CountChars == 0)
-    return mapWindowsError(GetLastError());
-  Buffer.set_size(CountChars);
-  return std::error_code();
-}
-
-static std::error_code realPathFromHandle(HANDLE H,
-                                          SmallVectorImpl<char> &RealPath) {
-  RealPath.clear();
-  SmallVector<wchar_t, MAX_PATH> Buffer;
-  if (std::error_code EC = realPathFromHandle(H, Buffer))
-    return EC;
-
-  const wchar_t *Data = Buffer.data();
-  DWORD CountChars = Buffer.size();
-  if (CountChars >= 4) {
-    if (0 == ::memcmp(Data, L"\\\\?\\", 8)) {
-      CountChars -= 4;
-      Data += 4;
-    }
-  }
-
-  // Convert the result from UTF-16 to UTF-8.
-  return UTF16ToUTF8(Data, CountChars, RealPath);
-}
-
-static std::error_code setDeleteDisposition(HANDLE Handle, bool Delete) {
-  FILE_DISPOSITION_INFO Disposition;
-  Disposition.DeleteFile = Delete;
-  if (!SetFileInformationByHandle(Handle, FileDispositionInfo, &Disposition,
-                                  sizeof(Disposition)))
-    return mapWindowsError(::GetLastError());
-  return std::error_code();
-}
-
-std::error_code access(const Twine &Path, AccessMode Mode) {
-  SmallVector<wchar_t, 128> PathUtf16;
-
-  if (std::error_code EC = widenPath(Path, PathUtf16))
-    return EC;
-
-  DWORD Attributes = ::GetFileAttributesW(PathUtf16.begin());
-
-  if (Attributes == INVALID_FILE_ATTRIBUTES) {
-    // See if the file didn't actually exist.
-    DWORD LastError = ::GetLastError();
-    if (LastError != ERROR_FILE_NOT_FOUND &&
-        LastError != ERROR_PATH_NOT_FOUND)
-      return mapWindowsError(LastError);
-    return errc::no_such_file_or_directory;
-  }
-
-  if (Mode == AccessMode::Write && (Attributes & FILE_ATTRIBUTE_READONLY))
-    return errc::permission_denied;
-
-  return std::error_code();
-}
-
-bool equivalent(file_status A, file_status B) {
-  assert(status_known(A) && status_known(B));
-  return A.FileIndexHigh         == B.FileIndexHigh &&
-         A.FileIndexLow          == B.FileIndexLow &&
-         A.FileSizeHigh          == B.FileSizeHigh &&
-         A.FileSizeLow           == B.FileSizeLow &&
-         A.LastAccessedTimeHigh  == B.LastAccessedTimeHigh &&
-         A.LastAccessedTimeLow   == B.LastAccessedTimeLow &&
-         A.LastWriteTimeHigh     == B.LastWriteTimeHigh &&
-         A.LastWriteTimeLow      == B.LastWriteTimeLow &&
-         A.VolumeSerialNumber    == B.VolumeSerialNumber;
-}
-
-std::error_code equivalent(const Twine &A, const Twine &B, bool &result) {
-  file_status fsA, fsB;
-  if (std::error_code ec = status(A, fsA))
-    return ec;
-  if (std::error_code ec = status(B, fsB))
-    return ec;
-  result = equivalent(fsA, fsB);
-  return std::error_code();
-}
-
-static bool isReservedName(StringRef path) {
-  // This list of reserved names comes from MSDN, at:
-  // http://msdn.microsoft.com/en-us/library/aa365247%28v=vs.85%29.aspx
-  static const char *const sReservedNames[] = { "nul", "con", "prn", "aux",
-                                                "com1", "com2", "com3", "com4",
-                                                "com5", "com6", "com7", "com8",
-                                                "com9", "lpt1", "lpt2", "lpt3",
-                                                "lpt4", "lpt5", "lpt6", "lpt7",
-                                                "lpt8", "lpt9" };
-
-  // First, check to see if this is a device namespace, which always
-  // starts with \\.\, since device namespaces are not legal file paths.
-  if (path.startswith("\\\\.\\"))
-    return true;
-
-  // Then compare against the list of ancient reserved names.
-  for (size_t i = 0; i < array_lengthof(sReservedNames); ++i) {
-    if (path.equals_lower(sReservedNames[i]))
-      return true;
-  }
-
-  // The path isn't what we consider reserved.
-  return false;
-}
-
-static file_type file_type_from_attrs(DWORD Attrs) {
-  return (Attrs & FILE_ATTRIBUTE_DIRECTORY) ? file_type::directory_file
-                                            : file_type::regular_file;
-}
-
-static perms perms_from_attrs(DWORD Attrs) {
-  return (Attrs & FILE_ATTRIBUTE_READONLY) ? (all_read | all_exe) : all_all;
-}
-
-static std::error_code getStatus(HANDLE FileHandle, file_status &Result) {
-  if (FileHandle == INVALID_HANDLE_VALUE)
-    goto handle_status_error;
-
-  switch (::GetFileType(FileHandle)) {
-  default:
-    Result = file_status(file_type::type_unknown);
-    return std::error_code();
-  case FILE_TYPE_UNKNOWN: {
-    DWORD Err = ::GetLastError();
-    if (Err != NO_ERROR)
-      return mapWindowsError(Err);
-    Result = file_status(file_type::type_unknown);
-    return std::error_code();
-  }
-  case FILE_TYPE_DISK:
-    break;
-  case FILE_TYPE_CHAR:
-    Result = file_status(file_type::character_file);
-    return std::error_code();
-  case FILE_TYPE_PIPE:
-    Result = file_status(file_type::fifo_file);
-    return std::error_code();
-  }
-
-  BY_HANDLE_FILE_INFORMATION Info;
-  if (!::GetFileInformationByHandle(FileHandle, &Info))
-    goto handle_status_error;
-
-  Result = file_status(
-      file_type_from_attrs(Info.dwFileAttributes),
-      perms_from_attrs(Info.dwFileAttributes), Info.nNumberOfLinks,
-      Info.ftLastAccessTime.dwHighDateTime, Info.ftLastAccessTime.dwLowDateTime,
-      Info.ftLastWriteTime.dwHighDateTime, Info.ftLastWriteTime.dwLowDateTime,
-      Info.dwVolumeSerialNumber, Info.nFileSizeHigh, Info.nFileSizeLow,
-      Info.nFileIndexHigh, Info.nFileIndexLow);
-  return std::error_code();
-
-handle_status_error:
-  DWORD LastError = ::GetLastError();
-  if (LastError == ERROR_FILE_NOT_FOUND ||
-      LastError == ERROR_PATH_NOT_FOUND)
-    Result = file_status(file_type::file_not_found);
-  else if (LastError == ERROR_SHARING_VIOLATION)
-    Result = file_status(file_type::type_unknown);
-  else
-    Result = file_status(file_type::status_error);
-  return mapWindowsError(LastError);
-}
-
-std::error_code status(const Twine &path, file_status &result, bool Follow) {
-  SmallString<128> path_storage;
-  SmallVector<wchar_t, 128> path_utf16;
-
-  StringRef path8 = path.toStringRef(path_storage);
-  if (isReservedName(path8)) {
-    result = file_status(file_type::character_file);
-    return std::error_code();
-  }
-
-  if (std::error_code ec = widenPath(path8, path_utf16))
-    return ec;
-
-  DWORD attr = ::GetFileAttributesW(path_utf16.begin());
-  if (attr == INVALID_FILE_ATTRIBUTES)
-    return getStatus(INVALID_HANDLE_VALUE, result);
-
-  DWORD Flags = FILE_FLAG_BACKUP_SEMANTICS;
-  // Handle reparse points.
-  if (!Follow && (attr & FILE_ATTRIBUTE_REPARSE_POINT))
-    Flags |= FILE_FLAG_OPEN_REPARSE_POINT;
-
-  ScopedFileHandle h(
-      ::CreateFileW(path_utf16.begin(), 0, // Attributes only.
-                    FILE_SHARE_DELETE | FILE_SHARE_READ | FILE_SHARE_WRITE,
-                    NULL, OPEN_EXISTING, Flags, 0));
-  if (!h)
-    return getStatus(INVALID_HANDLE_VALUE, result);
-
-  return getStatus(h, result);
-}
-
-std::error_code status(int FD, file_status &Result) {
-  HANDLE FileHandle = reinterpret_cast<HANDLE>(_get_osfhandle(FD));
-  return getStatus(FileHandle, Result);
-}
-
-std::error_code mapped_file_region::init(int FD, uint64_t Offset,
-                                         mapmode Mode) {
-  this->Mode = Mode;
-  HANDLE OrigFileHandle = reinterpret_cast<HANDLE>(_get_osfhandle(FD));
-  if (OrigFileHandle == INVALID_HANDLE_VALUE)
-    return make_error_code(errc::bad_file_descriptor);
-
-  DWORD flprotect;
-  switch (Mode) {
-  case readonly:  flprotect = PAGE_READONLY; break;
-  case readwrite: flprotect = PAGE_READWRITE; break;
-  case priv:      flprotect = PAGE_WRITECOPY; break;
-  }
-
-  HANDLE FileMappingHandle =
-      ::CreateFileMappingW(OrigFileHandle, 0, flprotect,
-                           Hi_32(Size),
-                           Lo_32(Size),
-                           0);
-  if (FileMappingHandle == NULL) {
-    std::error_code ec = mapWindowsError(GetLastError());
-    return ec;
-  }
-
-  DWORD dwDesiredAccess;
-  switch (Mode) {
-  case readonly:  dwDesiredAccess = FILE_MAP_READ; break;
-  case readwrite: dwDesiredAccess = FILE_MAP_WRITE; break;
-  case priv:      dwDesiredAccess = FILE_MAP_COPY; break;
-  }
-  Mapping = ::MapViewOfFile(FileMappingHandle,
-                            dwDesiredAccess,
-                            Offset >> 32,
-                            Offset & 0xffffffff,
-                            Size);
-  if (Mapping == NULL) {
-    std::error_code ec = mapWindowsError(GetLastError());
-    ::CloseHandle(FileMappingHandle);
-    return ec;
-  }
-
-  if (Size == 0) {
-    MEMORY_BASIC_INFORMATION mbi;
-    SIZE_T Result = VirtualQuery(Mapping, &mbi, sizeof(mbi));
-    if (Result == 0) {
-      std::error_code ec = mapWindowsError(GetLastError());
-      ::UnmapViewOfFile(Mapping);
-      ::CloseHandle(FileMappingHandle);
-      return ec;
-    }
-    Size = mbi.RegionSize;
-  }
-
-  // Close the file mapping handle, as it's kept alive by the file mapping. But
-  // neither the file mapping nor the file mapping handle keep the file handle
-  // alive, so we need to keep a reference to the file in case all other handles
-  // are closed and the file is deleted, which may cause invalid data to be read
-  // from the file.
-  ::CloseHandle(FileMappingHandle);
-  if (!::DuplicateHandle(::GetCurrentProcess(), OrigFileHandle,
-                         ::GetCurrentProcess(), &FileHandle, 0, 0,
-                         DUPLICATE_SAME_ACCESS)) {
-    std::error_code ec = mapWindowsError(GetLastError());
-    ::UnmapViewOfFile(Mapping);
-    return ec;
-  }
-
-  return std::error_code();
-}
-
-mapped_file_region::mapped_file_region(int fd, mapmode mode, size_t length,
-                                       uint64_t offset, std::error_code &ec)
-    : Size(length), Mapping() {
-  ec = init(fd, offset, mode);
-  if (ec)
-    Mapping = 0;
-}
-
-static bool hasFlushBufferKernelBug() {
-  static bool Ret{GetWindowsOSVersion() < wpi::VersionTuple(10, 0, 0, 17763)};
-  return Ret;
-}
-
-static bool isEXE(StringRef Magic) {
-  static const char PEMagic[] = {'P', 'E', '\0', '\0'};
-  if (Magic.startswith(StringRef("MZ")) && Magic.size() >= 0x3c + 4) {
-    uint32_t off = read32le(Magic.data() + 0x3c);
-    // PE/COFF file, either EXE or DLL.
-    if (Magic.substr(off).startswith(StringRef(PEMagic, sizeof(PEMagic))))
-      return true;
-  }
-  return false;
-}
-
-mapped_file_region::~mapped_file_region() {
-  if (Mapping) {
-
-    bool Exe = isEXE(StringRef((char *)Mapping, Size));
-
-    ::UnmapViewOfFile(Mapping);
-
-    if (Mode == mapmode::readwrite && Exe && hasFlushBufferKernelBug()) {
-      // There is a Windows kernel bug, the exact trigger conditions of which
-      // are not well understood.  When triggered, dirty pages are not properly
-      // flushed and subsequent process's attempts to read a file can return
-      // invalid data.  Calling FlushFileBuffers on the write handle is
-      // sufficient to ensure that this bug is not triggered.
-      // The bug only occurs when writing an executable and executing it right
-      // after, under high I/O pressure.
-      ::FlushFileBuffers(FileHandle);
-    }
-
-    ::CloseHandle(FileHandle);
-  }
-}
-
-size_t mapped_file_region::size() const {
-  assert(Mapping && "Mapping failed but used anyway!");
-  return Size;
-}
-
-char *mapped_file_region::data() const {
-  assert(Mapping && "Mapping failed but used anyway!");
-  return reinterpret_cast<char*>(Mapping);
-}
-
-const char *mapped_file_region::const_data() const {
-  assert(Mapping && "Mapping failed but used anyway!");
-  return reinterpret_cast<const char*>(Mapping);
-}
-
-int mapped_file_region::alignment() {
-  SYSTEM_INFO SysInfo;
-  ::GetSystemInfo(&SysInfo);
-  return SysInfo.dwAllocationGranularity;
-}
-
-static basic_file_status status_from_find_data(WIN32_FIND_DATAW *FindData) {
-  return basic_file_status(file_type_from_attrs(FindData->dwFileAttributes),
-                           perms_from_attrs(FindData->dwFileAttributes),
-                           FindData->ftLastAccessTime.dwHighDateTime,
-                           FindData->ftLastAccessTime.dwLowDateTime,
-                           FindData->ftLastWriteTime.dwHighDateTime,
-                           FindData->ftLastWriteTime.dwLowDateTime,
-                           FindData->nFileSizeHigh, FindData->nFileSizeLow);
-}
-
-std::error_code detail::directory_iterator_construct(detail::DirIterState &IT,
-                                                     StringRef Path,
-                                                     bool FollowSymlinks) {
-  SmallVector<wchar_t, 128> PathUTF16;
-
-  if (std::error_code EC = widenPath(Path, PathUTF16))
-    return EC;
-
-  // Convert path to the format that Windows is happy with.
-  if (PathUTF16.size() > 0 &&
-      !is_separator(PathUTF16[Path.size() - 1]) &&
-      PathUTF16[Path.size() - 1] != L':') {
-    PathUTF16.push_back(L'\\');
-    PathUTF16.push_back(L'*');
-  } else {
-    PathUTF16.push_back(L'*');
-  }
-
-  //  Get the first directory entry.
-  WIN32_FIND_DATAW FirstFind;
-  ScopedFindHandle FindHandle(::FindFirstFileExW(
-      c_str(PathUTF16), FindExInfoBasic, &FirstFind, FindExSearchNameMatch,
-      NULL, FIND_FIRST_EX_LARGE_FETCH));
-  if (!FindHandle)
-    return mapWindowsError(::GetLastError());
-
-  size_t FilenameLen = ::wcslen(FirstFind.cFileName);
-  while ((FilenameLen == 1 && FirstFind.cFileName[0] == L'.') ||
-         (FilenameLen == 2 && FirstFind.cFileName[0] == L'.' &&
-                              FirstFind.cFileName[1] == L'.'))
-    if (!::FindNextFileW(FindHandle, &FirstFind)) {
-      DWORD LastError = ::GetLastError();
-      // Check for end.
-      if (LastError == ERROR_NO_MORE_FILES)
-        return detail::directory_iterator_destruct(IT);
-      return mapWindowsError(LastError);
-    } else
-      FilenameLen = ::wcslen(FirstFind.cFileName);
-
-  // Construct the current directory entry.
-  SmallString<128> DirectoryEntryNameUTF8;
-  if (std::error_code EC =
-          UTF16ToUTF8(FirstFind.cFileName, ::wcslen(FirstFind.cFileName),
-                      DirectoryEntryNameUTF8))
-    return EC;
-
-  IT.IterationHandle = intptr_t(FindHandle.take());
-  SmallString<128> DirectoryEntryPath(Path);
-  path::append(DirectoryEntryPath, DirectoryEntryNameUTF8);
-  IT.CurrentEntry =
-      directory_entry(DirectoryEntryPath, FollowSymlinks,
-                      file_type_from_attrs(FirstFind.dwFileAttributes),
-                      status_from_find_data(&FirstFind));
-
-  return std::error_code();
-}
-
-std::error_code detail::directory_iterator_destruct(detail::DirIterState &IT) {
-  if (IT.IterationHandle != 0)
-    // Closes the handle if it's valid.
-    ScopedFindHandle close(HANDLE(IT.IterationHandle));
-  IT.IterationHandle = 0;
-  IT.CurrentEntry = directory_entry();
-  return std::error_code();
-}
-
-std::error_code detail::directory_iterator_increment(detail::DirIterState &IT) {
-  WIN32_FIND_DATAW FindData;
-  if (!::FindNextFileW(HANDLE(IT.IterationHandle), &FindData)) {
-    DWORD LastError = ::GetLastError();
-    // Check for end.
-    if (LastError == ERROR_NO_MORE_FILES)
-      return detail::directory_iterator_destruct(IT);
-    return mapWindowsError(LastError);
-  }
-
-  size_t FilenameLen = ::wcslen(FindData.cFileName);
-  if ((FilenameLen == 1 && FindData.cFileName[0] == L'.') ||
-      (FilenameLen == 2 && FindData.cFileName[0] == L'.' &&
-                           FindData.cFileName[1] == L'.'))
-    return directory_iterator_increment(IT);
-
-  SmallString<128> DirectoryEntryPathUTF8;
-  if (std::error_code EC =
-          UTF16ToUTF8(FindData.cFileName, ::wcslen(FindData.cFileName),
-                      DirectoryEntryPathUTF8))
-    return EC;
-
-  IT.CurrentEntry.replace_filename(
-      Twine(DirectoryEntryPathUTF8),
-      file_type_from_attrs(FindData.dwFileAttributes),
-      status_from_find_data(&FindData));
-  return std::error_code();
-}
-
-ErrorOr<basic_file_status> directory_entry::status() const {
-  return Status;
-}
-
-static std::error_code nativeFileToFd(Expected<HANDLE> H, int &ResultFD,
-                                      OpenFlags Flags) {
-  int CrtOpenFlags = 0;
-  if (Flags & OF_Append)
-    CrtOpenFlags |= _O_APPEND;
-
-  if (Flags & OF_Text)
-    CrtOpenFlags |= _O_TEXT;
-
-  ResultFD = -1;
-  if (!H)
-    return errorToErrorCode(H.takeError());
-
-  ResultFD = ::_open_osfhandle(intptr_t(*H), CrtOpenFlags);
-  if (ResultFD == -1) {
-    ::CloseHandle(*H);
-    return mapWindowsError(ERROR_INVALID_HANDLE);
-  }
-  return std::error_code();
-}
-
-static DWORD nativeDisposition(CreationDisposition Disp, OpenFlags Flags) {
-  // This is a compatibility hack.  Really we should respect the creation
-  // disposition, but a lot of old code relied on the implicit assumption that
-  // OF_Append implied it would open an existing file.  Since the disposition is
-  // now explicit and defaults to CD_CreateAlways, this assumption would cause
-  // any usage of OF_Append to append to a new file, even if the file already
-  // existed.  A better solution might have two new creation dispositions:
-  // CD_AppendAlways and CD_AppendNew.  This would also address the problem of
-  // OF_Append being used on a read-only descriptor, which doesn't make sense.
-  if (Flags & OF_Append)
-    return OPEN_ALWAYS;
-
-  switch (Disp) {
-  case CD_CreateAlways:
-    return CREATE_ALWAYS;
-  case CD_CreateNew:
-    return CREATE_NEW;
-  case CD_OpenAlways:
-    return OPEN_ALWAYS;
-  case CD_OpenExisting:
-    return OPEN_EXISTING;
-  }
-  wpi_unreachable("unreachable!");
-}
-
-static DWORD nativeAccess(FileAccess Access, OpenFlags Flags) {
-  DWORD Result = 0;
-  if (Access & FA_Read)
-    Result |= GENERIC_READ;
-  if (Access & FA_Write)
-    Result |= GENERIC_WRITE;
-  if (Flags & OF_Delete)
-    Result |= DELETE;
-  if (Flags & OF_UpdateAtime)
-    Result |= FILE_WRITE_ATTRIBUTES;
-  return Result;
-}
-
-static std::error_code openNativeFileInternal(const Twine &Name,
-                                              file_t &ResultFile, DWORD Disp,
-                                              DWORD Access, DWORD Flags,
-                                              bool Inherit = false) {
-  SmallVector<wchar_t, 128> PathUTF16;
-  if (std::error_code EC = widenPath(Name, PathUTF16))
-    return EC;
-
-  SECURITY_ATTRIBUTES SA;
-  SA.nLength = sizeof(SA);
-  SA.lpSecurityDescriptor = nullptr;
-  SA.bInheritHandle = Inherit;
-
-  HANDLE H =
-      ::CreateFileW(PathUTF16.begin(), Access,
-                    FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, &SA,
-                    Disp, Flags, NULL);
-  if (H == INVALID_HANDLE_VALUE) {
-    DWORD LastError = ::GetLastError();
-    std::error_code EC = mapWindowsError(LastError);
-    // Provide a better error message when trying to open directories.
-    // This only runs if we failed to open the file, so there is probably
-    // no performances issues.
-    if (LastError != ERROR_ACCESS_DENIED)
-      return EC;
-    if (is_directory(Name))
-      return make_error_code(errc::is_a_directory);
-    return EC;
-  }
-  ResultFile = H;
-  return std::error_code();
-}
-
-Expected<file_t> openNativeFile(const Twine &Name, CreationDisposition Disp,
-                                FileAccess Access, OpenFlags Flags,
-                                unsigned Mode) {
-  // Verify that we don't have both "append" and "excl".
-  assert((!(Disp == CD_CreateNew) || !(Flags & OF_Append)) &&
-         "Cannot specify both 'CreateNew' and 'Append' file creation flags!");
-
-  DWORD NativeDisp = nativeDisposition(Disp, Flags);
-  DWORD NativeAccess = nativeAccess(Access, Flags);
-
-  bool Inherit = false;
-  if (Flags & OF_ChildInherit)
-    Inherit = true;
-
-  file_t Result;
-  std::error_code EC = openNativeFileInternal(
-      Name, Result, NativeDisp, NativeAccess, FILE_ATTRIBUTE_NORMAL, Inherit);
-  if (EC)
-    return errorCodeToError(EC);
-
-  if (Flags & OF_UpdateAtime) {
-    FILETIME FileTime;
-    SYSTEMTIME SystemTime;
-    GetSystemTime(&SystemTime);
-    if (SystemTimeToFileTime(&SystemTime, &FileTime) == 0 ||
-        SetFileTime(Result, NULL, &FileTime, NULL) == 0) {
-      DWORD LastError = ::GetLastError();
-      ::CloseHandle(Result);
-      return errorCodeToError(mapWindowsError(LastError));
-    }
-  }
-
-  if (Flags & OF_Delete) {
-    if ((EC = setDeleteDisposition(Result, true))) {
-      ::CloseHandle(Result);
-      return errorCodeToError(EC);
-    }
-  }
-  return Result;
-}
-
-std::error_code openFile(const Twine &Name, int &ResultFD,
-                         CreationDisposition Disp, FileAccess Access,
-                         OpenFlags Flags, unsigned int Mode) {
-  Expected<file_t> Result = openNativeFile(Name, Disp, Access, Flags);
-  if (!Result)
-    return errorToErrorCode(Result.takeError());
-
-  return nativeFileToFd(*Result, ResultFD, Flags);
-}
-
-static std::error_code directoryRealPath(const Twine &Name,
-                                         SmallVectorImpl<char> &RealPath) {
-  file_t File;
-  std::error_code EC = openNativeFileInternal(
-      Name, File, OPEN_EXISTING, GENERIC_READ, FILE_FLAG_BACKUP_SEMANTICS);
-  if (EC)
-    return EC;
-
-  EC = realPathFromHandle(File, RealPath);
-  ::CloseHandle(File);
-  return EC;
-}
-
-std::error_code openFileForRead(const Twine &Name, int &ResultFD,
-                                OpenFlags Flags,
-                                SmallVectorImpl<char> *RealPath) {
-  Expected<HANDLE> NativeFile = openNativeFileForRead(Name, Flags, RealPath);
-  return nativeFileToFd(std::move(NativeFile), ResultFD, OF_None);
-}
-
-Expected<file_t> openNativeFileForRead(const Twine &Name, OpenFlags Flags,
-                                       SmallVectorImpl<char> *RealPath) {
-  Expected<file_t> Result =
-      openNativeFile(Name, CD_OpenExisting, FA_Read, Flags);
-
-  // Fetch the real name of the file, if the user asked
-  if (Result && RealPath)
-    realPathFromHandle(*Result, *RealPath);
-
-  return Result;
-}
-
-void closeFile(file_t &F) {
-  ::CloseHandle(F);
-  F = kInvalidFile;
-}
-
-
-} // end namespace fs
-
-namespace path {
-static bool getKnownFolderPath(KNOWNFOLDERID folderId,
-                               SmallVectorImpl<char> &result) {
-  wchar_t *path = nullptr;
-  if (::SHGetKnownFolderPath(folderId, KF_FLAG_CREATE, nullptr, &path) != S_OK)
-    return false;
-
-  bool ok = !UTF16ToUTF8(path, ::wcslen(path), result);
-  ::CoTaskMemFree(path);
-  return ok;
-}
-
-bool home_directory(SmallVectorImpl<char> &result) {
-  return getKnownFolderPath(FOLDERID_Profile, result);
-}
-
-static bool getTempDirEnvVar(const wchar_t *Var, SmallVectorImpl<char> &Res) {
-  SmallVector<wchar_t, 1024> Buf;
-  size_t Size = 1024;
-  do {
-    Buf.reserve(Size);
-    Size = GetEnvironmentVariableW(Var, Buf.data(), Buf.capacity());
-    if (Size == 0)
-      return false;
-
-    // Try again with larger buffer.
-  } while (Size > Buf.capacity());
-  Buf.set_size(Size);
-
-  return !windows::UTF16ToUTF8(Buf.data(), Size, Res);
-}
-
-static bool getTempDirEnvVar(SmallVectorImpl<char> &Res) {
-  const wchar_t *EnvironmentVariables[] = {L"TMP", L"TEMP", L"USERPROFILE"};
-  for (auto *Env : EnvironmentVariables) {
-    if (getTempDirEnvVar(Env, Res))
-      return true;
-  }
-  return false;
-}
-
-void system_temp_directory(bool ErasedOnReboot, SmallVectorImpl<char> &Result) {
-  (void)ErasedOnReboot;
-  Result.clear();
-
-  // Check whether the temporary directory is specified by an environment var.
-  // This matches GetTempPath logic to some degree. GetTempPath is not used
-  // directly as it cannot handle evn var longer than 130 chars on Windows 7
-  // (fixed on Windows 8).
-  if (getTempDirEnvVar(Result)) {
-    assert(!Result.empty() && "Unexpected empty path");
-    native(Result); // Some Unix-like shells use Unix path separator in $TMP.
-    fs::make_absolute(Result); // Make it absolute if not already.
-    return;
-  }
-
-  // Fall back to a system default.
-  const char *DefaultResult = "C:\\Temp";
-  Result.append(DefaultResult, DefaultResult + strlen(DefaultResult));
-}
-} // end namespace path
-
-namespace windows {
-std::error_code CodePageToUTF16(unsigned codepage,
-                                wpi::StringRef original,
-                                wpi::SmallVectorImpl<wchar_t> &utf16) {
-  if (!original.empty()) {
-    int len = ::MultiByteToWideChar(codepage, MB_ERR_INVALID_CHARS, original.begin(),
-                                    original.size(), utf16.begin(), 0);
-
-    if (len == 0) {
-      return mapWindowsError(::GetLastError());
-    }
-
-    utf16.reserve(len + 1);
-    utf16.set_size(len);
-
-    len = ::MultiByteToWideChar(codepage, MB_ERR_INVALID_CHARS, original.begin(),
-                                original.size(), utf16.begin(), utf16.size());
-
-    if (len == 0) {
-      return mapWindowsError(::GetLastError());
-    }
-  }
-
-  // Make utf16 null terminated.
-  utf16.push_back(0);
-  utf16.pop_back();
-
-  return std::error_code();
-}
-
-std::error_code UTF8ToUTF16(wpi::StringRef utf8,
-                            wpi::SmallVectorImpl<wchar_t> &utf16) {
-  return CodePageToUTF16(CP_UTF8, utf8, utf16);
-}
-
-std::error_code CurCPToUTF16(wpi::StringRef curcp,
-                            wpi::SmallVectorImpl<wchar_t> &utf16) {
-  return CodePageToUTF16(CP_ACP, curcp, utf16);
-}
-
-static
-std::error_code UTF16ToCodePage(unsigned codepage, const wchar_t *utf16,
-                                size_t utf16_len,
-                                wpi::SmallVectorImpl<char> &converted) {
-  if (utf16_len) {
-    // Get length.
-    int len = ::WideCharToMultiByte(codepage, 0, utf16, utf16_len, converted.begin(),
-                                    0, NULL, NULL);
-
-    if (len == 0) {
-      return mapWindowsError(::GetLastError());
-    }
-
-    converted.reserve(len);
-    converted.set_size(len);
-
-    // Now do the actual conversion.
-    len = ::WideCharToMultiByte(codepage, 0, utf16, utf16_len, converted.data(),
-                                converted.size(), NULL, NULL);
-
-    if (len == 0) {
-      return mapWindowsError(::GetLastError());
-    }
-  }
-
-  // Make the new string null terminated.
-  converted.push_back(0);
-  converted.pop_back();
-
-  return std::error_code();
-}
-
-std::error_code UTF16ToUTF8(const wchar_t *utf16, size_t utf16_len,
-                            wpi::SmallVectorImpl<char> &utf8) {
-  return UTF16ToCodePage(CP_UTF8, utf16, utf16_len, utf8);
-}
-
-std::error_code UTF16ToCurCP(const wchar_t *utf16, size_t utf16_len,
-                             wpi::SmallVectorImpl<char> &curcp) {
-  return UTF16ToCodePage(CP_ACP, utf16, utf16_len, curcp);
-}
-
-} // end namespace windows
-} // end namespace sys
-} // end namespace wpi
-
-#ifdef _MSC_VER
-#pragma warning(pop)
-#endif
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/WindowsSupport.h b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/WindowsSupport.h
index d830e33..7307337 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/WindowsSupport.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/Windows/WindowsSupport.h
@@ -36,13 +36,12 @@
 
 #include "wpi/SmallVector.h"
 #include "wpi/StringExtras.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
 #include "wpi/Chrono.h"
 #include "wpi/Compiler.h"
 #include "wpi/VersionTuple.h"
 #include <cassert>
 #include <string>
+#include <string_view>
 #include <system_error>
 #define WIN32_NO_STATUS
 #include <windows.h>
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/raw_ostream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/raw_ostream.cpp
index 9f2942c..622e0bf 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/raw_ostream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/llvm/raw_ostream.cpp
@@ -16,17 +16,14 @@
 #endif
 
 #include "wpi/raw_ostream.h"
-#include "wpi/STLExtras.h"
 #include "wpi/SmallString.h"
 #include "wpi/SmallVector.h"
 #include "wpi/StringExtras.h"
 #include "wpi/Compiler.h"
 #include "wpi/ErrorHandling.h"
-#include "wpi/FileSystem.h"
-#include "wpi/Format.h"
 #include "wpi/MathExtras.h"
-#include "wpi/NativeFormatting.h"
 #include "wpi/WindowsError.h"
+#include "wpi/fs.h"
 #include <algorithm>
 #include <cctype>
 #include <cerrno>
@@ -67,6 +64,14 @@
 
 using namespace wpi;
 
+namespace {
+// Find the length of an array.
+template <class T, std::size_t N>
+constexpr inline size_t array_lengthof(T (&)[N]) {
+  return N;
+}
+}  // namespace
+
 raw_ostream::~raw_ostream() {
   // raw_ostream's subclasses should take care to flush the buffer
   // in their destructors.
@@ -113,32 +118,7 @@
   assert(OutBufStart <= OutBufEnd && "Invalid size!");
 }
 
-raw_ostream &raw_ostream::operator<<(unsigned long N) {
-  write_integer(*this, static_cast<uint64_t>(N), 0, IntegerStyle::Integer);
-  return *this;
-}
-
-raw_ostream &raw_ostream::operator<<(long N) {
-  write_integer(*this, static_cast<int64_t>(N), 0, IntegerStyle::Integer);
-  return *this;
-}
-
-raw_ostream &raw_ostream::operator<<(unsigned long long N) {
-  write_integer(*this, static_cast<uint64_t>(N), 0, IntegerStyle::Integer);
-  return *this;
-}
-
-raw_ostream &raw_ostream::operator<<(long long N) {
-  write_integer(*this, static_cast<int64_t>(N), 0, IntegerStyle::Integer);
-  return *this;
-}
-
-raw_ostream &raw_ostream::write_hex(unsigned long long N) {
-  wpi::write_hex(*this, N, HexPrintStyle::Lower);
-  return *this;
-}
-
-raw_ostream &raw_ostream::write_escaped(StringRef Str,
+raw_ostream &raw_ostream::write_escaped(std::string_view Str,
                                         bool UseHexEscapes) {
   for (unsigned char c : Str) {
     switch (c) {
@@ -178,16 +158,6 @@
   return *this;
 }
 
-raw_ostream &raw_ostream::operator<<(const void *P) {
-  wpi::write_hex(*this, (uintptr_t)P, HexPrintStyle::PrefixLower);
-  return *this;
-}
-
-raw_ostream &raw_ostream::operator<<(double N) {
-  wpi::write_double(*this, N, FloatStyle::Exponent);
-  return *this;
-}
-
 void raw_ostream::flush_nonempty() {
   assert(OutBufCur > OutBufStart && "Invalid call to flush_nonempty.");
   size_t Length = OutBufCur - OutBufStart;
@@ -277,170 +247,6 @@
   OutBufCur += Size;
 }
 
-// Formatted output.
-raw_ostream &raw_ostream::operator<<(const format_object_base &Fmt) {
-  // If we have more than a few bytes left in our output buffer, try
-  // formatting directly onto its end.
-  size_t NextBufferSize = 127;
-  size_t BufferBytesLeft = OutBufEnd - OutBufCur;
-  if (BufferBytesLeft > 3) {
-    size_t BytesUsed = Fmt.print(OutBufCur, BufferBytesLeft);
-
-    // Common case is that we have plenty of space.
-    if (BytesUsed <= BufferBytesLeft) {
-      OutBufCur += BytesUsed;
-      return *this;
-    }
-
-    // Otherwise, we overflowed and the return value tells us the size to try
-    // again with.
-    NextBufferSize = BytesUsed;
-  }
-
-  // If we got here, we didn't have enough space in the output buffer for the
-  // string.  Try printing into a SmallVector that is resized to have enough
-  // space.  Iterate until we win.
-  SmallVector<char, 128> V;
-
-  while (true) {
-    V.resize(NextBufferSize);
-
-    // Try formatting into the SmallVector.
-    size_t BytesUsed = Fmt.print(V.data(), NextBufferSize);
-
-    // If BytesUsed fit into the vector, we win.
-    if (BytesUsed <= NextBufferSize)
-      return write(V.data(), BytesUsed);
-
-    // Otherwise, try again with a new size.
-    assert(BytesUsed > NextBufferSize && "Didn't grow buffer!?");
-    NextBufferSize = BytesUsed;
-  }
-}
-
-raw_ostream &raw_ostream::operator<<(const FormattedString &FS) {
-  if (FS.Str.size() >= FS.Width || FS.Justify == FormattedString::JustifyNone) {
-    this->operator<<(FS.Str);
-    return *this;
-  }
-  const size_t Difference = FS.Width - FS.Str.size();
-  switch (FS.Justify) {
-  case FormattedString::JustifyLeft:
-    this->operator<<(FS.Str);
-    this->indent(Difference);
-    break;
-  case FormattedString::JustifyRight:
-    this->indent(Difference);
-    this->operator<<(FS.Str);
-    break;
-  case FormattedString::JustifyCenter: {
-    int PadAmount = Difference / 2;
-    this->indent(PadAmount);
-    this->operator<<(FS.Str);
-    this->indent(Difference - PadAmount);
-    break;
-  }
-  default:
-    wpi_unreachable("Bad Justification");
-  }
-  return *this;
-}
-
-raw_ostream &raw_ostream::operator<<(const FormattedNumber &FN) {
-  if (FN.Hex) {
-    HexPrintStyle Style;
-    if (FN.Upper && FN.HexPrefix)
-      Style = HexPrintStyle::PrefixUpper;
-    else if (FN.Upper && !FN.HexPrefix)
-      Style = HexPrintStyle::Upper;
-    else if (!FN.Upper && FN.HexPrefix)
-      Style = HexPrintStyle::PrefixLower;
-    else
-      Style = HexPrintStyle::Lower;
-    wpi::write_hex(*this, FN.HexValue, Style, FN.Width);
-  } else {
-    wpi::SmallString<16> Buffer;
-    wpi::raw_svector_ostream Stream(Buffer);
-    wpi::write_integer(Stream, FN.DecValue, 0, IntegerStyle::Integer);
-    if (Buffer.size() < FN.Width)
-      indent(FN.Width - Buffer.size());
-    (*this) << Buffer;
-  }
-  return *this;
-}
-
-raw_ostream &raw_ostream::operator<<(const FormattedBytes &FB) {
-  if (FB.Bytes.empty())
-    return *this;
-
-  size_t LineIndex = 0;
-  auto Bytes = FB.Bytes;
-  const size_t Size = Bytes.size();
-  HexPrintStyle HPS = FB.Upper ? HexPrintStyle::Upper : HexPrintStyle::Lower;
-  uint64_t OffsetWidth = 0;
-  if (FB.FirstByteOffset.has_value()) {
-    // Figure out how many nibbles are needed to print the largest offset
-    // represented by this data set, so that we can align the offset field
-    // to the right width.
-    size_t Lines = Size / FB.NumPerLine;
-    uint64_t MaxOffset = *FB.FirstByteOffset + Lines * FB.NumPerLine;
-    unsigned Power = 0;
-    if (MaxOffset > 0)
-      Power = wpi::Log2_64_Ceil(MaxOffset);
-    OffsetWidth = std::max<uint64_t>(4, wpi::alignTo(Power, 4) / 4);
-  }
-
-  // The width of a block of data including all spaces for group separators.
-  unsigned NumByteGroups =
-      alignTo(FB.NumPerLine, FB.ByteGroupSize) / FB.ByteGroupSize;
-  unsigned BlockCharWidth = FB.NumPerLine * 2 + NumByteGroups - 1;
-
-  while (!Bytes.empty()) {
-    indent(FB.IndentLevel);
-
-    if (FB.FirstByteOffset.has_value()) {
-      uint64_t Offset = FB.FirstByteOffset.value();
-      wpi::write_hex(*this, Offset + LineIndex, HPS, OffsetWidth);
-      *this << ": ";
-    }
-
-    auto Line = Bytes.take_front(FB.NumPerLine);
-
-    size_t CharsPrinted = 0;
-    // Print the hex bytes for this line in groups
-    for (size_t I = 0; I < Line.size(); ++I, CharsPrinted += 2) {
-      if (I && (I % FB.ByteGroupSize) == 0) {
-        ++CharsPrinted;
-        *this << " ";
-      }
-      wpi::write_hex(*this, Line[I], HPS, 2);
-    }
-
-    if (FB.ASCII) {
-      // Print any spaces needed for any bytes that we didn't print on this
-      // line so that the ASCII bytes are correctly aligned.
-      assert(BlockCharWidth >= CharsPrinted);
-      indent(BlockCharWidth - CharsPrinted + 2);
-      *this << "|";
-
-      // Print the ASCII char values for each byte on this line
-      for (uint8_t Byte : Line) {
-        if (isPrint(Byte))
-          *this << static_cast<char>(Byte);
-        else
-          *this << '.';
-      }
-      *this << '|';
-    }
-
-    Bytes = Bytes.drop_front(Line.size());
-    LineIndex += Line.size();
-    if (LineIndex < Size)
-      *this << '\n';
-  }
-  return *this;
-}
-
 template <char C>
 static raw_ostream &write_padding(raw_ostream &OS, unsigned NumChars) {
   static const char Chars[] = {C, C, C, C, C, C, C, C, C, C, C, C, C, C, C, C,
@@ -475,21 +281,13 @@
 void raw_ostream::anchor() {}
 
 //===----------------------------------------------------------------------===//
-//  Formatted Output
-//===----------------------------------------------------------------------===//
-
-// Out of line virtual method.
-void format_object_base::home() {
-}
-
-//===----------------------------------------------------------------------===//
 //  raw_fd_ostream
 //===----------------------------------------------------------------------===//
 
-static int getFD(StringRef Filename, std::error_code &EC,
-                 sys::fs::CreationDisposition Disp, sys::fs::FileAccess Access,
-                 sys::fs::OpenFlags Flags) {
-  assert((Access & sys::fs::FA_Write) &&
+static int getFD(std::string_view Filename, std::error_code &EC,
+                 fs::CreationDisposition Disp, fs::FileAccess Access,
+                 fs::OpenFlags Flags) {
+  assert((Access & fs::FA_Write) &&
          "Cannot make a raw_ostream from a read-only descriptor!");
 
   // Handle "-" as stdout. Note that when we do this, we consider ourself
@@ -498,7 +296,7 @@
     EC = std::error_code();
     // If user requested binary then put stdout into binary mode if
     // possible.
-    if (!(Flags & sys::fs::OF_Text)) {
+    if (!(Flags & fs::OF_Text)) {
 #if defined(_WIN32)
       _setmode(_fileno(stdout), _O_BINARY);
 #endif
@@ -506,39 +304,43 @@
     return STDOUT_FILENO;
   }
 
-  int FD;
-  if (Access & sys::fs::FA_Read)
-    EC = sys::fs::openFileForReadWrite(Filename, FD, Disp, Flags);
-  else
-    EC = sys::fs::openFileForWrite(Filename, FD, Disp, Flags);
+  fs::file_t F;
+  if (Access & fs::FA_Read) {
+    F = fs::OpenFileForReadWrite(fs::path{std::string_view{Filename.data(), Filename.size()}}, EC, Disp, Flags);
+  } else {
+    F = fs::OpenFileForWrite(fs::path{std::string_view{Filename.data(), Filename.size()}}, EC, Disp, Flags);
+  }
+  if (EC)
+    return -1;
+  int FD = fs::FileToFd(F, EC, Flags);
   if (EC)
     return -1;
 
   return FD;
 }
 
-raw_fd_ostream::raw_fd_ostream(StringRef Filename, std::error_code &EC)
-    : raw_fd_ostream(Filename, EC, sys::fs::CD_CreateAlways, sys::fs::FA_Write,
-                     sys::fs::OF_None) {}
+raw_fd_ostream::raw_fd_ostream(std::string_view Filename, std::error_code &EC)
+    : raw_fd_ostream(Filename, EC, fs::CD_CreateAlways, fs::FA_Write,
+                     fs::OF_None) {}
 
-raw_fd_ostream::raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                               sys::fs::CreationDisposition Disp)
-    : raw_fd_ostream(Filename, EC, Disp, sys::fs::FA_Write, sys::fs::OF_None) {}
+raw_fd_ostream::raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                               fs::CreationDisposition Disp)
+    : raw_fd_ostream(Filename, EC, Disp, fs::FA_Write, fs::OF_None) {}
 
-raw_fd_ostream::raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                               sys::fs::FileAccess Access)
-    : raw_fd_ostream(Filename, EC, sys::fs::CD_CreateAlways, Access,
-                     sys::fs::OF_None) {}
+raw_fd_ostream::raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                               fs::FileAccess Access)
+    : raw_fd_ostream(Filename, EC, fs::CD_CreateAlways, Access,
+                     fs::OF_None) {}
 
-raw_fd_ostream::raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                               sys::fs::OpenFlags Flags)
-    : raw_fd_ostream(Filename, EC, sys::fs::CD_CreateAlways, sys::fs::FA_Write,
+raw_fd_ostream::raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                               fs::OpenFlags Flags)
+    : raw_fd_ostream(Filename, EC, fs::CD_CreateAlways, fs::FA_Write,
                      Flags) {}
 
-raw_fd_ostream::raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                               sys::fs::CreationDisposition Disp,
-                               sys::fs::FileAccess Access,
-                               sys::fs::OpenFlags Flags)
+raw_fd_ostream::raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                               fs::CreationDisposition Disp,
+                               fs::FileAccess Access,
+                               fs::OpenFlags Flags)
     : raw_fd_ostream(getFD(Filename, EC, Disp, Access, Flags), true) {}
 
 /// FD is the file descriptor that this writes to.  If ShouldClose is true, this
@@ -618,7 +420,7 @@
 // the input is UTF-8 or transcode from the local codepage to UTF-8 before
 // quoting it. If they don't, this may mess up the encoding, but this is still
 // probably the best compromise we can make.
-static bool write_console_impl(int FD, StringRef Data) {
+static bool write_console_impl(int FD, std::string_view Data) {
   SmallVector<wchar_t, 256> WideText;
 
   // Fall back to ::write if it wasn't valid UTF-8.
@@ -661,7 +463,7 @@
   // If this is a Windows console device, try re-encoding from UTF-8 to UTF-16
   // and using WriteConsoleW. If that fails, fall back to plain write().
   if (IsWindowsConsole)
-    if (write_console_impl(FD, StringRef(Ptr, Size)))
+    if (write_console_impl(FD, std::string_view(Ptr, Size)))
       return;
 #endif
 
@@ -783,7 +585,7 @@
 raw_ostream &wpi::outs() {
   // Set buffer settings to model stdout behavior.
   std::error_code EC;
-  static raw_fd_ostream* S = new raw_fd_ostream("-", EC, sys::fs::F_None);
+  static raw_fd_ostream* S = new raw_fd_ostream("-", EC, fs::F_None);
   assert(!EC);
   return *S;
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/mpack.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/mpack.cpp
new file mode 100644
index 0000000..fbcb6a4
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/mpack.cpp
@@ -0,0 +1,7251 @@
+/**
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2015-2021 Nicholas Fraser and the MPack authors
+ * 
+ * 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.
+ * 
+ */
+
+/*
+ * This is the MPack 1.1 amalgamation package.
+ *
+ * http://github.com/ludocode/mpack
+ */
+
+#define MPACK_INTERNAL 1
+#define MPACK_EMIT_INLINE_DEFS 0
+
+#include "wpi/mpack.h"
+
+
+/* mpack/mpack-platform.c.c */
+
+
+// We define MPACK_EMIT_INLINE_DEFS and include mpack.h to emit
+// standalone definitions of all (non-static) inline functions in MPack.
+
+#define MPACK_INTERNAL 1
+#define MPACK_EMIT_INLINE_DEFS 0
+
+/* #include "mpack-platform.h" */
+/* #include "mpack.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+namespace mpack {
+
+#if MPACK_DEBUG
+
+#if MPACK_STDIO
+void mpack_assert_fail_format(const char* format, ...) {
+    char buffer[512];
+    va_list args;
+    va_start(args, format);
+    vsnprintf(buffer, sizeof(buffer), format, args);
+    va_end(args);
+    buffer[sizeof(buffer) - 1] = 0;
+    mpack_assert_fail_wrapper(buffer);
+}
+
+void mpack_break_hit_format(const char* format, ...) {
+    char buffer[512];
+    va_list args;
+    va_start(args, format);
+    vsnprintf(buffer, sizeof(buffer), format, args);
+    va_end(args);
+    buffer[sizeof(buffer) - 1] = 0;
+    mpack_break_hit(buffer);
+}
+#endif
+
+#if !MPACK_CUSTOM_ASSERT
+void mpack_assert_fail(const char* message) {
+    MPACK_UNUSED(message);
+
+    #if MPACK_STDIO
+    fprintf(stderr, "%s\n", message);
+    #endif
+}
+#endif
+
+// We split the assert failure from the wrapper so that a
+// custom assert function can return.
+void mpack_assert_fail_wrapper(const char* message) {
+
+    #ifdef MPACK_GCOV
+    // gcov marks even __builtin_unreachable() as an uncovered line. this
+    // silences it.
+    (mpack_assert_fail(message), __builtin_unreachable());
+
+    #else
+    mpack_assert_fail(message);
+
+    // mpack_assert_fail() is not supposed to return. in case it does, we
+    // abort.
+
+    #if !MPACK_NO_BUILTINS
+    #if defined(__GNUC__) || defined(__clang__)
+    __builtin_trap();
+    #elif defined(WIN32)
+    __debugbreak();
+    #endif
+    #endif
+
+    #if (defined(__GNUC__) || defined(__clang__)) && !MPACK_NO_BUILTINS
+    __builtin_abort();
+    #elif MPACK_STDLIB
+    abort();
+    #endif
+
+    MPACK_UNREACHABLE;
+    #endif
+}
+
+#if !MPACK_CUSTOM_BREAK
+
+// If we have a custom assert handler, break wraps it by default.
+// This allows users of MPack to only implement mpack_assert_fail() without
+// having to worry about the difference between assert and break.
+//
+// MPACK_CUSTOM_BREAK is available to define a separate break handler
+// (which is needed by the unit test suite), but this is not offered in
+// mpack-config.h for simplicity.
+
+#if MPACK_CUSTOM_ASSERT
+void mpack_break_hit(const char* message) {
+    mpack_assert_fail_wrapper(message);
+}
+#else
+void mpack_break_hit(const char* message) {
+    MPACK_UNUSED(message);
+
+    #if MPACK_STDIO
+    fprintf(stderr, "%s\n", message);
+    #endif
+
+    #if defined(__GNUC__) || defined(__clang__) && !MPACK_NO_BUILTINS
+    __builtin_trap();
+    #elif defined(WIN32) && !MPACK_NO_BUILTINS
+    __debugbreak();
+    #elif MPACK_STDLIB
+    abort();
+    #endif
+}
+#endif
+
+#endif
+
+#endif
+
+
+
+// The below are adapted from the C wikibook:
+//     https://en.wikibooks.org/wiki/C_Programming/Strings
+
+#ifndef mpack_memcmp
+int mpack_memcmp(const void* s1, const void* s2, size_t n) {
+     const unsigned char *us1 = (const unsigned char *) s1;
+     const unsigned char *us2 = (const unsigned char *) s2;
+     while (n-- != 0) {
+         if (*us1 != *us2)
+             return (*us1 < *us2) ? -1 : +1;
+         us1++;
+         us2++;
+     }
+     return 0;
+}
+#endif
+
+#ifndef mpack_memcpy
+void* mpack_memcpy(void* MPACK_RESTRICT s1, const void* MPACK_RESTRICT s2, size_t n) {
+    char* MPACK_RESTRICT dst = (char *)s1;
+    const char* MPACK_RESTRICT src = (const char *)s2;
+    while (n-- != 0)
+        *dst++ = *src++;
+    return s1;
+}
+#endif
+
+#ifndef mpack_memmove
+void* mpack_memmove(void* s1, const void* s2, size_t n) {
+    char *p1 = (char *)s1;
+    const char *p2 = (const char *)s2;
+    if (p2 < p1 && p1 < p2 + n) {
+        p2 += n;
+        p1 += n;
+        while (n-- != 0)
+            *--p1 = *--p2;
+    } else
+        while (n-- != 0)
+            *p1++ = *p2++;
+    return s1;
+}
+#endif
+
+#ifndef mpack_memset
+void* mpack_memset(void* s, int c, size_t n) {
+    unsigned char *us = (unsigned char *)s;
+    unsigned char uc = (unsigned char)c;
+    while (n-- != 0)
+        *us++ = uc;
+    return s;
+}
+#endif
+
+#ifndef mpack_strlen
+size_t mpack_strlen(const char* s) {
+    const char* p = s;
+    while (*p != '\0')
+        p++;
+    return (size_t)(p - s);
+}
+#endif
+
+
+
+#if defined(MPACK_MALLOC) && !defined(MPACK_REALLOC)
+void* mpack_realloc(void* old_ptr, size_t used_size, size_t new_size) {
+    if (new_size == 0) {
+        if (old_ptr)
+            MPACK_FREE(old_ptr);
+        return NULL;
+    }
+
+    void* new_ptr = MPACK_MALLOC(new_size);
+    if (new_ptr == NULL)
+        return NULL;
+
+    mpack_memcpy(new_ptr, old_ptr, used_size);
+    MPACK_FREE(old_ptr);
+    return new_ptr;
+}
+#endif
+
+MPACK_SILENCE_WARNINGS_END
+
+/* mpack/mpack-common.c.c */
+
+#define MPACK_INTERNAL 1
+
+/* #include "mpack-common.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+
+const char* mpack_error_to_string(mpack_error_t error) {
+    #if MPACK_STRINGS
+    switch (error) {
+        #define MPACK_ERROR_STRING_CASE(e) case e: return #e
+        MPACK_ERROR_STRING_CASE(mpack_ok);
+        MPACK_ERROR_STRING_CASE(mpack_error_io);
+        MPACK_ERROR_STRING_CASE(mpack_error_invalid);
+        MPACK_ERROR_STRING_CASE(mpack_error_unsupported);
+        MPACK_ERROR_STRING_CASE(mpack_error_type);
+        MPACK_ERROR_STRING_CASE(mpack_error_too_big);
+        MPACK_ERROR_STRING_CASE(mpack_error_memory);
+        MPACK_ERROR_STRING_CASE(mpack_error_bug);
+        MPACK_ERROR_STRING_CASE(mpack_error_data);
+        MPACK_ERROR_STRING_CASE(mpack_error_eof);
+        #undef MPACK_ERROR_STRING_CASE
+    }
+    mpack_assert(0, "unrecognized error %i", (int)error);
+    return "(unknown mpack_error_t)";
+    #else
+    MPACK_UNUSED(error);
+    return "";
+    #endif
+}
+
+const char* mpack_type_to_string(mpack_type_t type) {
+    #if MPACK_STRINGS
+    switch (type) {
+        #define MPACK_TYPE_STRING_CASE(e) case e: return #e
+        MPACK_TYPE_STRING_CASE(mpack_type_missing);
+        MPACK_TYPE_STRING_CASE(mpack_type_nil);
+        MPACK_TYPE_STRING_CASE(mpack_type_bool);
+        MPACK_TYPE_STRING_CASE(mpack_type_float);
+        MPACK_TYPE_STRING_CASE(mpack_type_double);
+        MPACK_TYPE_STRING_CASE(mpack_type_int);
+        MPACK_TYPE_STRING_CASE(mpack_type_uint);
+        MPACK_TYPE_STRING_CASE(mpack_type_str);
+        MPACK_TYPE_STRING_CASE(mpack_type_bin);
+        MPACK_TYPE_STRING_CASE(mpack_type_array);
+        MPACK_TYPE_STRING_CASE(mpack_type_map);
+        #if MPACK_EXTENSIONS
+        MPACK_TYPE_STRING_CASE(mpack_type_ext);
+        #endif
+        #undef MPACK_TYPE_STRING_CASE
+    }
+    mpack_assert(0, "unrecognized type %i", (int)type);
+    return "(unknown mpack_type_t)";
+    #else
+    MPACK_UNUSED(type);
+    return "";
+    #endif
+}
+
+int mpack_tag_cmp(mpack_tag_t left, mpack_tag_t right) {
+
+    // positive numbers may be stored as int; convert to uint
+    if (left.type == mpack_type_int && left.v.i >= 0) {
+        left.type = mpack_type_uint;
+        left.v.u = (uint64_t)left.v.i;
+    }
+    if (right.type == mpack_type_int && right.v.i >= 0) {
+        right.type = mpack_type_uint;
+        right.v.u = (uint64_t)right.v.i;
+    }
+
+    if (left.type != right.type)
+        return ((int)left.type < (int)right.type) ? -1 : 1;
+
+    switch (left.type) {
+        case mpack_type_missing: // fallthrough
+        case mpack_type_nil:
+            return 0;
+
+        case mpack_type_bool:
+            return (int)left.v.b - (int)right.v.b;
+
+        case mpack_type_int:
+            if (left.v.i == right.v.i)
+                return 0;
+            return (left.v.i < right.v.i) ? -1 : 1;
+
+        case mpack_type_uint:
+            if (left.v.u == right.v.u)
+                return 0;
+            return (left.v.u < right.v.u) ? -1 : 1;
+
+        case mpack_type_array:
+        case mpack_type_map:
+            if (left.v.n == right.v.n)
+                return 0;
+            return (left.v.n < right.v.n) ? -1 : 1;
+
+        case mpack_type_str:
+        case mpack_type_bin:
+            if (left.v.l == right.v.l)
+                return 0;
+            return (left.v.l < right.v.l) ? -1 : 1;
+
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            if (left.exttype == right.exttype) {
+                if (left.v.l == right.v.l)
+                    return 0;
+                return (left.v.l < right.v.l) ? -1 : 1;
+            }
+            return (int)left.exttype - (int)right.exttype;
+        #endif
+
+        // floats should not normally be compared for equality. we compare
+        // with memcmp() to silence compiler warnings, but this will return
+        // equal if both are NaNs with the same representation (though we may
+        // want this, for instance if you are for some bizarre reason using
+        // floats as map keys.) i'm not sure what the right thing to
+        // do is here. check for NaN first? always return false if the type
+        // is float? use operator== and pragmas to silence compiler warning?
+        // please send me your suggestions.
+        // note also that we don't convert floats to doubles, so when this is
+        // used for ordering purposes, all floats are ordered before all
+        // doubles.
+        case mpack_type_float:
+            return mpack_memcmp(&left.v.f, &right.v.f, sizeof(left.v.f));
+        case mpack_type_double:
+            return mpack_memcmp(&left.v.d, &right.v.d, sizeof(left.v.d));
+    }
+
+    mpack_assert(0, "unrecognized type %i", (int)left.type);
+    return false;
+}
+
+#if MPACK_DEBUG && MPACK_STDIO
+static char mpack_hex_char(uint8_t hex_value) {
+    // Older compilers (e.g. GCC 4.4.7) promote the result of this ternary to
+    // int and warn under -Wconversion, so we have to cast it back to char.
+    return (char)((hex_value < 10) ? (char)('0' + hex_value) : (char)('a' + (hex_value - 10)));
+}
+
+static void mpack_tag_debug_complete_bin_ext(mpack_tag_t tag, size_t string_length, char* buffer, size_t buffer_size,
+        const char* prefix, size_t prefix_size)
+{
+    // If at any point in this function we run out of space in the buffer, we
+    // bail out. The outer tag print wrapper will make sure we have a
+    // null-terminator.
+
+    if (string_length == 0 || string_length >= buffer_size)
+        return;
+    buffer += string_length;
+    buffer_size -= string_length;
+
+    size_t total = mpack_tag_bytes(&tag);
+    if (total == 0) {
+        strncpy(buffer, ">", buffer_size);
+        return;
+    }
+
+    strncpy(buffer, ": ", buffer_size);
+    if (buffer_size < 2)
+        return;
+    buffer += 2;
+    buffer_size -= 2;
+
+    size_t hex_bytes = 0;
+    size_t i;
+    for (i = 0; i < MPACK_PRINT_BYTE_COUNT && i < prefix_size && buffer_size > 2; ++i) {
+        uint8_t byte = (uint8_t)prefix[i];
+        buffer[0] = mpack_hex_char((uint8_t)(byte >> 4));
+        buffer[1] = mpack_hex_char((uint8_t)(byte & 0xfu));
+        buffer += 2;
+        buffer_size -= 2;
+        ++hex_bytes;
+    }
+
+    if (buffer_size != 0)
+        mpack_snprintf(buffer, buffer_size, "%s>", (total > hex_bytes) ? "..." : "");
+}
+
+static void mpack_tag_debug_pseudo_json_bin(mpack_tag_t tag, char* buffer, size_t buffer_size,
+        const char* prefix, size_t prefix_size)
+{
+    mpack_assert(mpack_tag_type(&tag) == mpack_type_bin);
+    size_t length = (size_t)mpack_snprintf(buffer, buffer_size, "<binary data of length %u", tag.v.l);
+    mpack_tag_debug_complete_bin_ext(tag, length, buffer, buffer_size, prefix, prefix_size);
+}
+
+#if MPACK_EXTENSIONS
+static void mpack_tag_debug_pseudo_json_ext(mpack_tag_t tag, char* buffer, size_t buffer_size,
+        const char* prefix, size_t prefix_size)
+{
+    mpack_assert(mpack_tag_type(&tag) == mpack_type_ext);
+    size_t length = (size_t)mpack_snprintf(buffer, buffer_size, "<ext data of type %i and length %u",
+            mpack_tag_ext_exttype(&tag), mpack_tag_ext_length(&tag));
+    mpack_tag_debug_complete_bin_ext(tag, length, buffer, buffer_size, prefix, prefix_size);
+}
+#endif
+
+static void mpack_tag_debug_pseudo_json_impl(mpack_tag_t tag, char* buffer, size_t buffer_size,
+        const char* prefix, size_t prefix_size)
+{
+    switch (tag.type) {
+        case mpack_type_missing:
+            mpack_snprintf(buffer, buffer_size, "<missing!>");
+            return;
+        case mpack_type_nil:
+            mpack_snprintf(buffer, buffer_size, "null");
+            return;
+        case mpack_type_bool:
+            mpack_snprintf(buffer, buffer_size, tag.v.b ? "true" : "false");
+            return;
+        case mpack_type_int:
+            mpack_snprintf(buffer, buffer_size, "%" PRIi64, tag.v.i);
+            return;
+        case mpack_type_uint:
+            mpack_snprintf(buffer, buffer_size, "%" PRIu64, tag.v.u);
+            return;
+        case mpack_type_float:
+            #if MPACK_FLOAT
+            mpack_snprintf(buffer, buffer_size, "%f", tag.v.f);
+            #else
+            mpack_snprintf(buffer, buffer_size, "<float>");
+            #endif
+            return;
+        case mpack_type_double:
+            #if MPACK_DOUBLE
+            mpack_snprintf(buffer, buffer_size, "%f", tag.v.d);
+            #else
+            mpack_snprintf(buffer, buffer_size, "<double>");
+            #endif
+            return;
+
+        case mpack_type_str:
+            mpack_snprintf(buffer, buffer_size, "<string of %u bytes>", tag.v.l);
+            return;
+        case mpack_type_bin:
+            mpack_tag_debug_pseudo_json_bin(tag, buffer, buffer_size, prefix, prefix_size);
+            return;
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            mpack_tag_debug_pseudo_json_ext(tag, buffer, buffer_size, prefix, prefix_size);
+            return;
+        #endif
+
+        case mpack_type_array:
+            mpack_snprintf(buffer, buffer_size, "<array of %u elements>", tag.v.n);
+            return;
+        case mpack_type_map:
+            mpack_snprintf(buffer, buffer_size, "<map of %u key-value pairs>", tag.v.n);
+            return;
+    }
+
+    mpack_snprintf(buffer, buffer_size, "<unknown!>");
+}
+
+void mpack_tag_debug_pseudo_json(mpack_tag_t tag, char* buffer, size_t buffer_size,
+        const char* prefix, size_t prefix_size)
+{
+    mpack_assert(buffer_size > 0, "buffer size cannot be zero!");
+    buffer[0] = 0;
+
+    mpack_tag_debug_pseudo_json_impl(tag, buffer, buffer_size, prefix, prefix_size);
+
+    // We always null-terminate the buffer manually just in case the snprintf()
+    // function doesn't null-terminate when the string doesn't fit.
+    buffer[buffer_size - 1] = 0;
+}
+
+static void mpack_tag_debug_describe_impl(mpack_tag_t tag, char* buffer, size_t buffer_size) {
+    switch (tag.type) {
+        case mpack_type_missing:
+            mpack_snprintf(buffer, buffer_size, "missing");
+            return;
+        case mpack_type_nil:
+            mpack_snprintf(buffer, buffer_size, "nil");
+            return;
+        case mpack_type_bool:
+            mpack_snprintf(buffer, buffer_size, tag.v.b ? "true" : "false");
+            return;
+        case mpack_type_int:
+            mpack_snprintf(buffer, buffer_size, "int %" PRIi64, tag.v.i);
+            return;
+        case mpack_type_uint:
+            mpack_snprintf(buffer, buffer_size, "uint %" PRIu64, tag.v.u);
+            return;
+        case mpack_type_float:
+            #if MPACK_FLOAT
+            mpack_snprintf(buffer, buffer_size, "float %f", tag.v.f);
+            #else
+            mpack_snprintf(buffer, buffer_size, "float");
+            #endif
+            return;
+        case mpack_type_double:
+            #if MPACK_DOUBLE
+            mpack_snprintf(buffer, buffer_size, "double %f", tag.v.d);
+            #else
+            mpack_snprintf(buffer, buffer_size, "double");
+            #endif
+            return;
+        case mpack_type_str:
+            mpack_snprintf(buffer, buffer_size, "str of %u bytes", tag.v.l);
+            return;
+        case mpack_type_bin:
+            mpack_snprintf(buffer, buffer_size, "bin of %u bytes", tag.v.l);
+            return;
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            mpack_snprintf(buffer, buffer_size, "ext of type %i, %u bytes",
+                    mpack_tag_ext_exttype(&tag), mpack_tag_ext_length(&tag));
+            return;
+        #endif
+        case mpack_type_array:
+            mpack_snprintf(buffer, buffer_size, "array of %u elements", tag.v.n);
+            return;
+        case mpack_type_map:
+            mpack_snprintf(buffer, buffer_size, "map of %u key-value pairs", tag.v.n);
+            return;
+    }
+
+    mpack_snprintf(buffer, buffer_size, "unknown!");
+}
+
+void mpack_tag_debug_describe(mpack_tag_t tag, char* buffer, size_t buffer_size) {
+    mpack_assert(buffer_size > 0, "buffer size cannot be zero!");
+    buffer[0] = 0;
+
+    mpack_tag_debug_describe_impl(tag, buffer, buffer_size);
+
+    // We always null-terminate the buffer manually just in case the snprintf()
+    // function doesn't null-terminate when the string doesn't fit.
+    buffer[buffer_size - 1] = 0;
+}
+#endif
+
+
+
+#if MPACK_READ_TRACKING || MPACK_WRITE_TRACKING
+
+#ifndef MPACK_TRACKING_INITIAL_CAPACITY
+// seems like a reasonable number. we grow by doubling, and it only
+// needs to be as long as the maximum depth of the message.
+#define MPACK_TRACKING_INITIAL_CAPACITY 8
+#endif
+
+mpack_error_t mpack_track_init(mpack_track_t* track) {
+    track->count = 0;
+    track->capacity = MPACK_TRACKING_INITIAL_CAPACITY;
+    track->elements = (mpack_track_element_t*)MPACK_MALLOC(sizeof(mpack_track_element_t) * track->capacity);
+    if (track->elements == NULL)
+        return mpack_error_memory;
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_grow(mpack_track_t* track) {
+    mpack_assert(track->elements, "null track elements!");
+    mpack_assert(track->count == track->capacity, "incorrect growing?");
+
+    size_t new_capacity = track->capacity * 2;
+
+    mpack_track_element_t* new_elements = (mpack_track_element_t*)mpack_realloc(track->elements,
+            sizeof(mpack_track_element_t) * track->count, sizeof(mpack_track_element_t) * new_capacity);
+    if (new_elements == NULL)
+        return mpack_error_memory;
+
+    track->elements = new_elements;
+    track->capacity = new_capacity;
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_push(mpack_track_t* track, mpack_type_t type, uint32_t count) {
+    mpack_assert(track->elements, "null track elements!");
+    mpack_log("track pushing %s count %i\n", mpack_type_to_string(type), (int)count);
+
+    // grow if needed
+    if (track->count == track->capacity) {
+        mpack_error_t error = mpack_track_grow(track);
+        if (error != mpack_ok)
+            return error;
+    }
+
+    // insert new track
+    track->elements[track->count].type = type;
+    track->elements[track->count].left = count;
+    track->elements[track->count].builder = false;
+    track->elements[track->count].key_needs_value = false;
+    ++track->count;
+    return mpack_ok;
+}
+
+// TODO dedupe this
+mpack_error_t mpack_track_push_builder(mpack_track_t* track, mpack_type_t type) {
+    mpack_assert(track->elements, "null track elements!");
+    mpack_log("track pushing %s builder\n", mpack_type_to_string(type));
+
+    // grow if needed
+    if (track->count == track->capacity) {
+        mpack_error_t error = mpack_track_grow(track);
+        if (error != mpack_ok)
+            return error;
+    }
+
+    // insert new track
+    track->elements[track->count].type = type;
+    track->elements[track->count].left = 0;
+    track->elements[track->count].builder = true;
+    track->elements[track->count].key_needs_value = false;
+    ++track->count;
+    return mpack_ok;
+}
+
+static mpack_error_t mpack_track_pop_impl(mpack_track_t* track, mpack_type_t type, bool builder) {
+    mpack_assert(track->elements, "null track elements!");
+    mpack_log("track popping %s\n", mpack_type_to_string(type));
+
+    if (track->count == 0) {
+        mpack_break("attempting to close a %s but nothing was opened!", mpack_type_to_string(type));
+        return mpack_error_bug;
+    }
+
+    mpack_track_element_t* element = &track->elements[track->count - 1];
+
+    if (element->type != type) {
+        mpack_break("attempting to close a %s but the open element is a %s!",
+                mpack_type_to_string(type), mpack_type_to_string(element->type));
+        return mpack_error_bug;
+    }
+
+    if (element->key_needs_value) {
+        mpack_assert(type == mpack_type_map, "key_needs_value can only be true for maps!");
+        mpack_break("attempting to close a %s but an odd number of elements were written",
+                mpack_type_to_string(type));
+        return mpack_error_bug;
+    }
+
+    if (element->left != 0) {
+        mpack_break("attempting to close a %s but there are %i %s left",
+                mpack_type_to_string(type), element->left,
+                (type == mpack_type_map || type == mpack_type_array) ? "elements" : "bytes");
+        return mpack_error_bug;
+    }
+
+    if (element->builder != builder) {
+        mpack_break("attempting to pop a %sbuilder but the open element is %sa builder",
+                builder ? "" : "non-",
+                element->builder ? "" : "not ");
+        return mpack_error_bug;
+    }
+
+    --track->count;
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_pop(mpack_track_t* track, mpack_type_t type) {
+    return mpack_track_pop_impl(track, type, false);
+}
+
+mpack_error_t mpack_track_pop_builder(mpack_track_t* track, mpack_type_t type) {
+    return mpack_track_pop_impl(track, type, true);
+}
+
+mpack_error_t mpack_track_peek_element(mpack_track_t* track, bool read) {
+    MPACK_UNUSED(read);
+    mpack_assert(track->elements, "null track elements!");
+
+    // if there are no open elements, that's fine, we can read/write elements at will
+    if (track->count == 0)
+        return mpack_ok;
+
+    mpack_track_element_t* element = &track->elements[track->count - 1];
+
+    if (element->type != mpack_type_map && element->type != mpack_type_array) {
+        mpack_break("elements cannot be %s within an %s", read ? "read" : "written",
+                mpack_type_to_string(element->type));
+        return mpack_error_bug;
+    }
+
+    if (!element->builder && element->left == 0 && !element->key_needs_value) {
+        mpack_break("too many elements %s for %s", read ? "read" : "written",
+                mpack_type_to_string(element->type));
+        return mpack_error_bug;
+    }
+
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_element(mpack_track_t* track, bool read) {
+    mpack_error_t error = mpack_track_peek_element(track, read);
+    if (track->count == 0 || error != mpack_ok)
+        return error;
+
+    mpack_track_element_t* element = &track->elements[track->count - 1];
+
+    if (element->type == mpack_type_map) {
+        if (!element->key_needs_value) {
+            element->key_needs_value = true;
+            return mpack_ok; // don't decrement
+        }
+        element->key_needs_value = false;
+    }
+
+    if (!element->builder)
+        --element->left;
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_bytes(mpack_track_t* track, bool read, size_t count) {
+    MPACK_UNUSED(read);
+    mpack_assert(track->elements, "null track elements!");
+
+    if (count > MPACK_UINT32_MAX) {
+        mpack_break("%s more bytes than could possibly fit in a str/bin/ext!",
+                read ? "reading" : "writing");
+        return mpack_error_bug;
+    }
+
+    if (track->count == 0) {
+        mpack_break("bytes cannot be %s with no open bin, str or ext", read ? "read" : "written");
+        return mpack_error_bug;
+    }
+
+    mpack_track_element_t* element = &track->elements[track->count - 1];
+
+    if (element->type == mpack_type_map || element->type == mpack_type_array) {
+        mpack_break("bytes cannot be %s within an %s", read ? "read" : "written",
+                mpack_type_to_string(element->type));
+        return mpack_error_bug;
+    }
+
+    if (element->left < count) {
+        mpack_break("too many bytes %s for %s", read ? "read" : "written",
+                mpack_type_to_string(element->type));
+        return mpack_error_bug;
+    }
+
+    element->left -= (uint32_t)count;
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_str_bytes_all(mpack_track_t* track, bool read, size_t count) {
+    mpack_error_t error = mpack_track_bytes(track, read, count);
+    if (error != mpack_ok)
+        return error;
+
+    mpack_track_element_t* element = &track->elements[track->count - 1];
+
+    if (element->type != mpack_type_str) {
+        mpack_break("the open type must be a string, not a %s", mpack_type_to_string(element->type));
+        return mpack_error_bug;
+    }
+
+    if (element->left != 0) {
+        mpack_break("not all bytes were read; the wrong byte count was requested for a string read.");
+        return mpack_error_bug;
+    }
+
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_check_empty(mpack_track_t* track) {
+    if (track->count != 0) {
+        mpack_break("unclosed %s", mpack_type_to_string(track->elements[0].type));
+        return mpack_error_bug;
+    }
+    return mpack_ok;
+}
+
+mpack_error_t mpack_track_destroy(mpack_track_t* track, bool cancel) {
+    mpack_error_t error = cancel ? mpack_ok : mpack_track_check_empty(track);
+    if (track->elements) {
+        MPACK_FREE(track->elements);
+        track->elements = NULL;
+    }
+    return error;
+}
+#endif
+
+
+
+static bool mpack_utf8_check_impl(const uint8_t* str, size_t count, bool allow_null) {
+    while (count > 0) {
+        uint8_t lead = str[0];
+
+        // NUL
+        if (!allow_null && lead == '\0') // we don't allow NUL bytes in MPack C-strings
+            return false;
+
+        // ASCII
+        if (lead <= 0x7F) {
+            ++str;
+            --count;
+
+        // 2-byte sequence
+        } else if ((lead & 0xE0) == 0xC0) {
+            if (count < 2) // truncated sequence
+                return false;
+
+            uint8_t cont = str[1];
+            if ((cont & 0xC0) != 0x80) // not a continuation byte
+                return false;
+
+            str += 2;
+            count -= 2;
+
+            uint32_t z = ((uint32_t)(lead & ~0xE0) << 6) |
+                          (uint32_t)(cont & ~0xC0);
+
+            if (z < 0x80) // overlong sequence
+                return false;
+
+        // 3-byte sequence
+        } else if ((lead & 0xF0) == 0xE0) {
+            if (count < 3) // truncated sequence
+                return false;
+
+            uint8_t cont1 = str[1];
+            if ((cont1 & 0xC0) != 0x80) // not a continuation byte
+                return false;
+            uint8_t cont2 = str[2];
+            if ((cont2 & 0xC0) != 0x80) // not a continuation byte
+                return false;
+
+            str += 3;
+            count -= 3;
+
+            uint32_t z = ((uint32_t)(lead  & ~0xF0) << 12) |
+                         ((uint32_t)(cont1 & ~0xC0) <<  6) |
+                          (uint32_t)(cont2 & ~0xC0);
+
+            if (z < 0x800) // overlong sequence
+                return false;
+            if (z >= 0xD800 && z <= 0xDFFF) // surrogate
+                return false;
+
+        // 4-byte sequence
+        } else if ((lead & 0xF8) == 0xF0) {
+            if (count < 4) // truncated sequence
+                return false;
+
+            uint8_t cont1 = str[1];
+            if ((cont1 & 0xC0) != 0x80) // not a continuation byte
+                return false;
+            uint8_t cont2 = str[2];
+            if ((cont2 & 0xC0) != 0x80) // not a continuation byte
+                return false;
+            uint8_t cont3 = str[3];
+            if ((cont3 & 0xC0) != 0x80) // not a continuation byte
+                return false;
+
+            str += 4;
+            count -= 4;
+
+            uint32_t z = ((uint32_t)(lead  & ~0xF8) << 18) |
+                         ((uint32_t)(cont1 & ~0xC0) << 12) |
+                         ((uint32_t)(cont2 & ~0xC0) <<  6) |
+                          (uint32_t)(cont3 & ~0xC0);
+
+            if (z < 0x10000) // overlong sequence
+                return false;
+            if (z > 0x10FFFF) // codepoint limit
+                return false;
+
+        } else {
+            return false; // continuation byte without a lead, or lead for a 5-byte sequence or longer
+        }
+    }
+    return true;
+}
+
+bool mpack_utf8_check(const char* str, size_t bytes) {
+    return mpack_utf8_check_impl((const uint8_t*)str, bytes, true);
+}
+
+bool mpack_utf8_check_no_null(const char* str, size_t bytes) {
+    return mpack_utf8_check_impl((const uint8_t*)str, bytes, false);
+}
+
+bool mpack_str_check_no_null(const char* str, size_t bytes) {
+    size_t i;
+    for (i = 0; i < bytes; ++i)
+        if (str[i] == '\0')
+            return false;
+    return true;
+}
+
+#if MPACK_DEBUG && MPACK_STDIO
+void mpack_print_append(mpack_print_t* print, const char* data, size_t count) {
+
+    // copy whatever fits into the buffer
+    size_t copy = print->size - print->count;
+    if (copy > count)
+        copy = count;
+    mpack_memcpy(print->buffer + print->count, data, copy);
+    print->count += copy;
+    data += copy;
+    count -= copy;
+
+    // if we don't need to flush or can't flush there's nothing else to do
+    if (count == 0 || print->callback == NULL)
+        return;
+
+    // flush the buffer
+    print->callback(print->context, print->buffer, print->count);
+
+    if (count > print->size / 2) {
+        // flush the rest of the data
+        print->count = 0;
+        print->callback(print->context, data, count);
+    } else {
+        // copy the rest of the data into the buffer
+        mpack_memcpy(print->buffer, data, count);
+        print->count = count;
+    }
+
+}
+
+void mpack_print_flush(mpack_print_t* print) {
+    if (print->count > 0 && print->callback != NULL) {
+        print->callback(print->context, print->buffer, print->count);
+        print->count = 0;
+    }
+}
+
+void mpack_print_file_callback(void* context, const char* data, size_t count) {
+    FILE* file = (FILE*)context;
+    fwrite(data, 1, count, file);
+}
+#endif
+
+MPACK_SILENCE_WARNINGS_END
+
+/* mpack/mpack-writer.c.c */
+
+#define MPACK_INTERNAL 1
+
+/* #include "mpack-writer.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+
+#if MPACK_WRITER
+
+#if MPACK_BUILDER
+static void mpack_builder_flush(mpack_writer_t* writer);
+#endif
+
+#if MPACK_WRITE_TRACKING
+static void mpack_writer_flag_if_error(mpack_writer_t* writer, mpack_error_t error) {
+    if (error != mpack_ok)
+        mpack_writer_flag_error(writer, error);
+}
+
+void mpack_writer_track_push(mpack_writer_t* writer, mpack_type_t type, uint32_t count) {
+    if (writer->error == mpack_ok)
+        mpack_writer_flag_if_error(writer, mpack_track_push(&writer->track, type, count));
+}
+
+void mpack_writer_track_push_builder(mpack_writer_t* writer, mpack_type_t type) {
+    if (writer->error == mpack_ok)
+        mpack_writer_flag_if_error(writer, mpack_track_push_builder(&writer->track, type));
+}
+
+void mpack_writer_track_pop(mpack_writer_t* writer, mpack_type_t type) {
+    if (writer->error == mpack_ok)
+        mpack_writer_flag_if_error(writer, mpack_track_pop(&writer->track, type));
+}
+
+void mpack_writer_track_pop_builder(mpack_writer_t* writer, mpack_type_t type) {
+    if (writer->error == mpack_ok)
+        mpack_writer_flag_if_error(writer, mpack_track_pop_builder(&writer->track, type));
+}
+
+void mpack_writer_track_bytes(mpack_writer_t* writer, size_t count) {
+    if (writer->error == mpack_ok)
+        mpack_writer_flag_if_error(writer, mpack_track_bytes(&writer->track, false, count));
+}
+#endif
+
+// This should probably be renamed. It's not solely used for tracking.
+static inline void mpack_writer_track_element(mpack_writer_t* writer) {
+    (void)writer;
+
+    #if MPACK_WRITE_TRACKING
+    if (writer->error == mpack_ok)
+        mpack_writer_flag_if_error(writer, mpack_track_element(&writer->track, false));
+    #endif
+
+    #if MPACK_BUILDER
+    if (writer->builder.current_build != NULL) {
+        mpack_build_t* build = writer->builder.current_build;
+        // We only track this write if it's not nested within another non-build
+        // map or array.
+        if (build->nested_compound_elements == 0) {
+            if (build->type != mpack_type_map) {
+                ++build->count;
+                mpack_log("adding element to build %p, now %u elements\n", (void*)build, build->count);
+            } else if (build->key_needs_value) {
+                build->key_needs_value = false;
+                ++build->count;
+            } else {
+                build->key_needs_value = true;
+            }
+        }
+    }
+    #endif
+}
+
+static void mpack_writer_clear(mpack_writer_t* writer) {
+    #if MPACK_COMPATIBILITY
+    writer->version = mpack_version_current;
+    #endif
+    writer->flush = NULL;
+    writer->error_fn = NULL;
+    writer->teardown = NULL;
+    writer->context = NULL;
+
+    writer->buffer = NULL;
+    writer->position = NULL;
+    writer->end = NULL;
+    writer->error = mpack_ok;
+
+    #if MPACK_WRITE_TRACKING
+    mpack_memset(&writer->track, 0, sizeof(writer->track));
+    #endif
+
+    #if MPACK_BUILDER
+    writer->builder.current_build = NULL;
+    writer->builder.latest_build = NULL;
+    writer->builder.current_page = NULL;
+    writer->builder.pages = NULL;
+    writer->builder.stash_buffer = NULL;
+    writer->builder.stash_position = NULL;
+    writer->builder.stash_end = NULL;
+    #endif
+}
+
+void mpack_writer_init(mpack_writer_t* writer, char* buffer, size_t size) {
+    mpack_assert(buffer != NULL, "cannot initialize writer with empty buffer");
+    mpack_writer_clear(writer);
+    writer->buffer = buffer;
+    writer->position = buffer;
+    writer->end = writer->buffer + size;
+
+    #if MPACK_WRITE_TRACKING
+    mpack_writer_flag_if_error(writer, mpack_track_init(&writer->track));
+    #endif
+
+    mpack_log("===========================\n");
+    mpack_log("initializing writer with buffer size %i\n", (int)size);
+}
+
+void mpack_writer_init_error(mpack_writer_t* writer, mpack_error_t error) {
+    mpack_writer_clear(writer);
+    writer->error = error;
+
+    mpack_log("===========================\n");
+    mpack_log("initializing writer in error state %i\n", (int)error);
+}
+
+void mpack_writer_set_flush(mpack_writer_t* writer, mpack_writer_flush_t flush) {
+    MPACK_STATIC_ASSERT(MPACK_WRITER_MINIMUM_BUFFER_SIZE >= MPACK_MAXIMUM_TAG_SIZE,
+            "minimum buffer size must fit any tag!");
+    MPACK_STATIC_ASSERT(31 + MPACK_TAG_SIZE_FIXSTR >= MPACK_WRITER_MINIMUM_BUFFER_SIZE,
+            "minimum buffer size must fit the largest possible fixstr!");
+
+    if (mpack_writer_buffer_size(writer) < MPACK_WRITER_MINIMUM_BUFFER_SIZE) {
+        mpack_break("buffer size is %i, but minimum buffer size for flush is %i",
+                (int)mpack_writer_buffer_size(writer), MPACK_WRITER_MINIMUM_BUFFER_SIZE);
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+
+    writer->flush = flush;
+}
+
+#ifdef MPACK_MALLOC
+typedef struct mpack_growable_writer_t {
+    char** target_data;
+    size_t* target_size;
+} mpack_growable_writer_t;
+
+static char* mpack_writer_get_reserved(mpack_writer_t* writer) {
+    // This is in a separate function in order to avoid false strict aliasing
+    // warnings. We aren't actually violating strict aliasing (the reserved
+    // space is only ever dereferenced as an mpack_growable_writer_t.)
+    return (char*)writer->reserved;
+}
+
+static void mpack_growable_writer_flush(mpack_writer_t* writer, const char* data, size_t count) {
+
+    // This is an intrusive flush function which modifies the writer's buffer
+    // in response to a flush instead of emptying it in order to add more
+    // capacity for data. This removes the need to copy data from a fixed buffer
+    // into a growable one, improving performance.
+    //
+    // There are three ways flush can be called:
+    //   - flushing the buffer during writing (used is zero, count is all data, data is buffer)
+    //   - flushing extra data during writing (used is all flushed data, count is extra data, data is not buffer)
+    //   - flushing during teardown (used and count are both all flushed data, data is buffer)
+    //
+    // In the first two cases, we grow the buffer by at least double, enough
+    // to ensure that new data will fit. We ignore the teardown flush.
+
+    if (data == writer->buffer) {
+
+        // teardown, do nothing
+        if (mpack_writer_buffer_used(writer) == count)
+            return;
+
+        // otherwise leave the data in the buffer and just grow
+        writer->position = writer->buffer + count;
+        count = 0;
+    }
+
+    size_t used = mpack_writer_buffer_used(writer);
+    size_t size = mpack_writer_buffer_size(writer);
+
+    mpack_log("flush size %i used %i data %p buffer %p\n",
+            (int)count, (int)used, data, writer->buffer);
+
+    mpack_assert(data == writer->buffer || used + count > size,
+            "extra flush for %i but there is %i space left in the buffer! (%i/%i)",
+            (int)count, (int)mpack_writer_buffer_left(writer), (int)used, (int)size);
+
+    // grow to fit the data
+    // TODO: this really needs to correctly test for overflow
+    size_t new_size = size * 2;
+    while (new_size < used + count)
+        new_size *= 2;
+
+    mpack_log("flush growing buffer size from %i to %i\n", (int)size, (int)new_size);
+
+    // grow the buffer
+    char* new_buffer = (char*)mpack_realloc(writer->buffer, used, new_size);
+    if (new_buffer == NULL) {
+        mpack_writer_flag_error(writer, mpack_error_memory);
+        return;
+    }
+    writer->position = new_buffer + used;
+    writer->buffer = new_buffer;
+    writer->end = writer->buffer + new_size;
+
+    // append the extra data
+    if (count > 0) {
+        mpack_memcpy(writer->position, data, count);
+        writer->position += count;
+    }
+
+    mpack_log("new buffer %p, used %i\n", new_buffer, (int)mpack_writer_buffer_used(writer));
+}
+
+static void mpack_growable_writer_teardown(mpack_writer_t* writer) {
+    mpack_growable_writer_t* growable_writer = (mpack_growable_writer_t*)mpack_writer_get_reserved(writer);
+
+    if (mpack_writer_error(writer) == mpack_ok) {
+
+        // shrink the buffer to an appropriate size if the data is
+        // much smaller than the buffer
+        if (mpack_writer_buffer_used(writer) < mpack_writer_buffer_size(writer) / 2) {
+            size_t used = mpack_writer_buffer_used(writer);
+
+            // We always return a non-null pointer that must be freed, even if
+            // nothing was written. malloc() and realloc() do not necessarily
+            // do this so we enforce it ourselves.
+            size_t size = (used != 0) ? used : 1;
+
+            char* buffer = (char*)mpack_realloc(writer->buffer, used, size);
+            if (!buffer) {
+                MPACK_FREE(writer->buffer);
+                mpack_writer_flag_error(writer, mpack_error_memory);
+                return;
+            }
+            writer->buffer = buffer;
+            writer->end = (writer->position = writer->buffer + used);
+        }
+
+        *growable_writer->target_data = writer->buffer;
+        *growable_writer->target_size = mpack_writer_buffer_used(writer);
+        writer->buffer = NULL;
+
+    } else if (writer->buffer) {
+        MPACK_FREE(writer->buffer);
+        writer->buffer = NULL;
+    }
+
+    writer->context = NULL;
+}
+
+void mpack_writer_init_growable(mpack_writer_t* writer, char** target_data, size_t* target_size) {
+    mpack_assert(target_data != NULL, "cannot initialize writer without a destination for the data");
+    mpack_assert(target_size != NULL, "cannot initialize writer without a destination for the size");
+
+    *target_data = NULL;
+    *target_size = 0;
+
+    MPACK_STATIC_ASSERT(sizeof(mpack_growable_writer_t) <= sizeof(writer->reserved),
+            "not enough reserved space for growable writer!");
+    mpack_growable_writer_t* growable_writer = (mpack_growable_writer_t*)mpack_writer_get_reserved(writer);
+
+    growable_writer->target_data = target_data;
+    growable_writer->target_size = target_size;
+
+    size_t capacity = MPACK_BUFFER_SIZE;
+    char* buffer = (char*)MPACK_MALLOC(capacity);
+    if (buffer == NULL) {
+        mpack_writer_init_error(writer, mpack_error_memory);
+        return;
+    }
+
+    mpack_writer_init(writer, buffer, capacity);
+    mpack_writer_set_flush(writer, mpack_growable_writer_flush);
+    mpack_writer_set_teardown(writer, mpack_growable_writer_teardown);
+}
+#endif
+
+#if MPACK_STDIO
+static void mpack_file_writer_flush(mpack_writer_t* writer, const char* buffer, size_t count) {
+    FILE* file = (FILE*)writer->context;
+    size_t written = fwrite((const void*)buffer, 1, count, file);
+    if (written != count)
+        mpack_writer_flag_error(writer, mpack_error_io);
+}
+
+static void mpack_file_writer_teardown(mpack_writer_t* writer) {
+    MPACK_FREE(writer->buffer);
+    writer->buffer = NULL;
+    writer->context = NULL;
+}
+
+static void mpack_file_writer_teardown_close(mpack_writer_t* writer) {
+    FILE* file = (FILE*)writer->context;
+
+    if (file) {
+        int ret = fclose(file);
+        if (ret != 0)
+            mpack_writer_flag_error(writer, mpack_error_io);
+    }
+
+    mpack_file_writer_teardown(writer);
+}
+
+void mpack_writer_init_stdfile(mpack_writer_t* writer, FILE* file, bool close_when_done) {
+    mpack_assert(file != NULL, "file is NULL");
+
+    size_t capacity = MPACK_BUFFER_SIZE;
+    char* buffer = (char*)MPACK_MALLOC(capacity);
+    if (buffer == NULL) {
+        mpack_writer_init_error(writer, mpack_error_memory);
+        if (close_when_done) {
+            fclose(file);
+        }
+        return;
+    }
+
+    mpack_writer_init(writer, buffer, capacity);
+    mpack_writer_set_context(writer, file);
+    mpack_writer_set_flush(writer, mpack_file_writer_flush);
+    mpack_writer_set_teardown(writer, close_when_done ?
+            mpack_file_writer_teardown_close :
+            mpack_file_writer_teardown);
+}
+
+void mpack_writer_init_filename(mpack_writer_t* writer, const char* filename) {
+    mpack_assert(filename != NULL, "filename is NULL");
+
+    FILE* file = fopen(filename, "wb");
+    if (file == NULL) {
+        mpack_writer_init_error(writer, mpack_error_io);
+        return;
+    }
+
+    mpack_writer_init_stdfile(writer, file, true);
+}
+#endif
+
+void mpack_writer_flag_error(mpack_writer_t* writer, mpack_error_t error) {
+    mpack_log("writer %p setting error %i: %s\n", (void*)writer, (int)error, mpack_error_to_string(error));
+
+    if (writer->error == mpack_ok) {
+        writer->error = error;
+        if (writer->error_fn)
+            writer->error_fn(writer, writer->error);
+    }
+}
+
+MPACK_STATIC_INLINE void mpack_writer_flush_unchecked(mpack_writer_t* writer) {
+    // This is a bit ugly; we reset used before calling flush so that
+    // a flush function can distinguish between flushing the buffer
+    // versus flushing external data. see mpack_growable_writer_flush()
+    size_t used = mpack_writer_buffer_used(writer);
+    writer->position = writer->buffer;
+    writer->flush(writer, writer->buffer, used);
+}
+
+void mpack_writer_flush_message(mpack_writer_t* writer) {
+    if (writer->error != mpack_ok)
+        return;
+
+    #if MPACK_WRITE_TRACKING
+    // You cannot flush while there are elements open.
+    mpack_writer_flag_if_error(writer, mpack_track_check_empty(&writer->track));
+    if (writer->error != mpack_ok)
+        return;
+    #endif
+
+    #if MPACK_BUILDER
+    if (writer->builder.current_build != NULL) {
+        mpack_break("cannot call mpack_writer_flush_message() while there are elements open!");
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+    #endif
+
+    if (writer->flush == NULL) {
+        mpack_break("cannot call mpack_writer_flush_message() without a flush function!");
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+
+    if (mpack_writer_buffer_used(writer) > 0)
+        mpack_writer_flush_unchecked(writer);
+}
+
+// Ensures there are at least count bytes free in the buffer. This
+// will flag an error if the flush function fails to make enough
+// room in the buffer.
+MPACK_NOINLINE static bool mpack_writer_ensure(mpack_writer_t* writer, size_t count) {
+    mpack_assert(count != 0, "cannot ensure zero bytes!");
+    mpack_assert(count <= MPACK_WRITER_MINIMUM_BUFFER_SIZE,
+            "cannot ensure %i bytes, this is more than the minimum buffer size %i!",
+            (int)count, (int)MPACK_WRITER_MINIMUM_BUFFER_SIZE);
+    mpack_assert(count > mpack_writer_buffer_left(writer),
+            "request to ensure %i bytes but there are already %i left in the buffer!",
+            (int)count, (int)mpack_writer_buffer_left(writer));
+
+    mpack_log("ensuring %i bytes, %i left\n", (int)count, (int)mpack_writer_buffer_left(writer));
+
+    if (mpack_writer_error(writer) != mpack_ok)
+        return false;
+
+    #if MPACK_BUILDER
+    // if we have a build in progress, we just ask the builder for a page.
+    // either it will have space for a tag, or it will flag a memory error.
+    if (writer->builder.current_build != NULL) {
+        mpack_builder_flush(writer);
+        return mpack_writer_error(writer) == mpack_ok;
+    }
+    #endif
+
+    if (writer->flush == NULL) {
+        mpack_writer_flag_error(writer, mpack_error_too_big);
+        return false;
+    }
+
+    mpack_writer_flush_unchecked(writer);
+    if (mpack_writer_error(writer) != mpack_ok)
+        return false;
+
+    if (mpack_writer_buffer_left(writer) >= count)
+        return true;
+
+    mpack_writer_flag_error(writer, mpack_error_io);
+    return false;
+}
+
+// Writes encoded bytes to the buffer when we already know the data
+// does not fit in the buffer (i.e. it straddles the edge of the
+// buffer.) If there is a flush function, it is guaranteed to be
+// called; otherwise mpack_error_too_big is raised.
+MPACK_NOINLINE static void mpack_write_native_straddle(mpack_writer_t* writer, const char* p, size_t count) {
+    mpack_assert(count == 0 || p != NULL, "data pointer for %i bytes is NULL", (int)count);
+
+    if (mpack_writer_error(writer) != mpack_ok)
+        return;
+    mpack_log("big write for %i bytes from %p, %i space left in buffer\n",
+            (int)count, p, (int)mpack_writer_buffer_left(writer));
+    mpack_assert(count > mpack_writer_buffer_left(writer),
+            "big write requested for %i bytes, but there is %i available "
+            "space in buffer. should have called mpack_write_native() instead",
+            (int)count, (int)(mpack_writer_buffer_left(writer)));
+
+    #if MPACK_BUILDER
+    // if we have a build in progress, we can't flush. we need to copy all
+    // bytes into as many build buffer pages as it takes.
+    if (writer->builder.current_build != NULL) {
+        while (true) {
+            size_t step = (size_t)(writer->end - writer->position);
+            if (step > count)
+                step = count;
+            mpack_memcpy(writer->position, p, step);
+            writer->position += step;
+            p += step;
+            count -= step;
+
+            if (count == 0)
+                return;
+
+            mpack_builder_flush(writer);
+            if (mpack_writer_error(writer) != mpack_ok)
+                return;
+            mpack_assert(writer->position != writer->end);
+        }
+    }
+    #endif
+
+    // we'll need a flush function
+    if (!writer->flush) {
+        mpack_writer_flag_error(writer, mpack_error_too_big);
+        return;
+    }
+
+    // flush the buffer
+    mpack_writer_flush_unchecked(writer);
+    if (mpack_writer_error(writer) != mpack_ok)
+        return;
+
+    // note that an intrusive flush function (such as mpack_growable_writer_flush())
+    // may have changed size and/or reset used to a non-zero value. we treat both as
+    // though they may have changed, and there may still be data in the buffer.
+
+    // flush the extra data directly if it doesn't fit in the buffer
+    if (count > mpack_writer_buffer_left(writer)) {
+        writer->flush(writer, p, count);
+        if (mpack_writer_error(writer) != mpack_ok)
+            return;
+    } else {
+        mpack_memcpy(writer->position, p, count);
+        writer->position += count;
+    }
+}
+
+// Writes encoded bytes to the buffer, flushing if necessary.
+MPACK_STATIC_INLINE void mpack_write_native(mpack_writer_t* writer, const char* p, size_t count) {
+    mpack_assert(count == 0 || p != NULL, "data pointer for %i bytes is NULL", (int)count);
+
+    if (mpack_writer_buffer_left(writer) < count) {
+        mpack_write_native_straddle(writer, p, count);
+    } else {
+        mpack_memcpy(writer->position, p, count);
+        writer->position += count;
+    }
+}
+
+mpack_error_t mpack_writer_destroy(mpack_writer_t* writer) {
+
+    // clean up tracking, asserting if we're not already in an error state
+    #if MPACK_WRITE_TRACKING
+    mpack_track_destroy(&writer->track, writer->error != mpack_ok);
+    #endif
+
+    // flush any outstanding data
+    if (mpack_writer_error(writer) == mpack_ok && mpack_writer_buffer_used(writer) != 0 && writer->flush != NULL) {
+        writer->flush(writer, writer->buffer, mpack_writer_buffer_used(writer));
+        writer->flush = NULL;
+    }
+
+    if (writer->teardown) {
+        writer->teardown(writer);
+        writer->teardown = NULL;
+    }
+
+    return writer->error;
+}
+
+void mpack_write_tag(mpack_writer_t* writer, mpack_tag_t value) {
+    switch (value.type) {
+        case mpack_type_missing:
+            mpack_break("cannot write a missing value!");
+            mpack_writer_flag_error(writer, mpack_error_bug);
+            return;
+
+        case mpack_type_nil:    mpack_write_nil   (writer);            return;
+        case mpack_type_bool:   mpack_write_bool  (writer, value.v.b); return;
+        case mpack_type_int:    mpack_write_int   (writer, value.v.i); return;
+        case mpack_type_uint:   mpack_write_uint  (writer, value.v.u); return;
+
+        case mpack_type_float:
+            #if MPACK_FLOAT
+            mpack_write_float
+            #else
+            mpack_write_raw_float
+            #endif
+                (writer, value.v.f);
+            return;
+        case mpack_type_double:
+            #if MPACK_DOUBLE
+            mpack_write_double
+            #else
+            mpack_write_raw_double
+            #endif
+                (writer, value.v.d);
+            return;
+
+        case mpack_type_str: mpack_start_str(writer, value.v.l); return;
+        case mpack_type_bin: mpack_start_bin(writer, value.v.l); return;
+
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            mpack_start_ext(writer, mpack_tag_ext_exttype(&value), mpack_tag_ext_length(&value));
+            return;
+        #endif
+
+        case mpack_type_array: mpack_start_array(writer, value.v.n); return;
+        case mpack_type_map:   mpack_start_map(writer, value.v.n);   return;
+    }
+
+    mpack_break("unrecognized type %i", (int)value.type);
+    mpack_writer_flag_error(writer, mpack_error_bug);
+}
+
+MPACK_STATIC_INLINE void mpack_write_byte_element(mpack_writer_t* writer, char value) {
+    mpack_writer_track_element(writer);
+    if (MPACK_LIKELY(mpack_writer_buffer_left(writer) >= 1) || mpack_writer_ensure(writer, 1))
+        *(writer->position++) = value;
+}
+
+void mpack_write_nil(mpack_writer_t* writer) {
+    mpack_write_byte_element(writer, (char)0xc0);
+}
+
+void mpack_write_bool(mpack_writer_t* writer, bool value) {
+    mpack_write_byte_element(writer, (char)(0xc2 | (value ? 1 : 0)));
+}
+
+void mpack_write_true(mpack_writer_t* writer) {
+    mpack_write_byte_element(writer, (char)0xc3);
+}
+
+void mpack_write_false(mpack_writer_t* writer) {
+    mpack_write_byte_element(writer, (char)0xc2);
+}
+
+void mpack_write_object_bytes(mpack_writer_t* writer, const char* data, size_t bytes) {
+    mpack_writer_track_element(writer);
+    mpack_write_native(writer, data, bytes);
+}
+
+/*
+ * Encode functions
+ */
+
+MPACK_STATIC_INLINE void mpack_encode_fixuint(char* p, uint8_t value) {
+    mpack_assert(value <= 127);
+    mpack_store_u8(p, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_u8(char* p, uint8_t value) {
+    mpack_assert(value > 127);
+    mpack_store_u8(p, 0xcc);
+    mpack_store_u8(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_u16(char* p, uint16_t value) {
+    mpack_assert(value > MPACK_UINT8_MAX);
+    mpack_store_u8(p, 0xcd);
+    mpack_store_u16(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_u32(char* p, uint32_t value) {
+    mpack_assert(value > MPACK_UINT16_MAX);
+    mpack_store_u8(p, 0xce);
+    mpack_store_u32(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_u64(char* p, uint64_t value) {
+    mpack_assert(value > MPACK_UINT32_MAX);
+    mpack_store_u8(p, 0xcf);
+    mpack_store_u64(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixint(char* p, int8_t value) {
+    // this can encode positive or negative fixints
+    mpack_assert(value >= -32);
+    mpack_store_i8(p, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_i8(char* p, int8_t value) {
+    mpack_assert(value < -32);
+    mpack_store_u8(p, 0xd0);
+    mpack_store_i8(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_i16(char* p, int16_t value) {
+    mpack_assert(value < MPACK_INT8_MIN);
+    mpack_store_u8(p, 0xd1);
+    mpack_store_i16(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_i32(char* p, int32_t value) {
+    mpack_assert(value < MPACK_INT16_MIN);
+    mpack_store_u8(p, 0xd2);
+    mpack_store_i32(p + 1, value);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_i64(char* p, int64_t value) {
+    mpack_assert(value < MPACK_INT32_MIN);
+    mpack_store_u8(p, 0xd3);
+    mpack_store_i64(p + 1, value);
+}
+
+#if MPACK_FLOAT
+MPACK_STATIC_INLINE void mpack_encode_float(char* p, float value) {
+    mpack_store_u8(p, 0xca);
+    mpack_store_float(p + 1, value);
+}
+#else
+MPACK_STATIC_INLINE void mpack_encode_raw_float(char* p, uint32_t value) {
+    mpack_store_u8(p, 0xca);
+    mpack_store_u32(p + 1, value);
+}
+#endif
+
+#if MPACK_DOUBLE
+MPACK_STATIC_INLINE void mpack_encode_double(char* p, double value) {
+    mpack_store_u8(p, 0xcb);
+    mpack_store_double(p + 1, value);
+}
+#else
+MPACK_STATIC_INLINE void mpack_encode_raw_double(char* p, uint64_t value) {
+    mpack_store_u8(p, 0xcb);
+    mpack_store_u64(p + 1, value);
+}
+#endif
+
+MPACK_STATIC_INLINE void mpack_encode_fixarray(char* p, uint8_t count) {
+    mpack_assert(count <= 15);
+    mpack_store_u8(p, (uint8_t)(0x90 | count));
+}
+
+MPACK_STATIC_INLINE void mpack_encode_array16(char* p, uint16_t count) {
+    mpack_assert(count > 15);
+    mpack_store_u8(p, 0xdc);
+    mpack_store_u16(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_array32(char* p, uint32_t count) {
+    mpack_assert(count > MPACK_UINT16_MAX);
+    mpack_store_u8(p, 0xdd);
+    mpack_store_u32(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixmap(char* p, uint8_t count) {
+    mpack_assert(count <= 15);
+    mpack_store_u8(p, (uint8_t)(0x80 | count));
+}
+
+MPACK_STATIC_INLINE void mpack_encode_map16(char* p, uint16_t count) {
+    mpack_assert(count > 15);
+    mpack_store_u8(p, 0xde);
+    mpack_store_u16(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_map32(char* p, uint32_t count) {
+    mpack_assert(count > MPACK_UINT16_MAX);
+    mpack_store_u8(p, 0xdf);
+    mpack_store_u32(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixstr(char* p, uint8_t count) {
+    mpack_assert(count <= 31);
+    mpack_store_u8(p, (uint8_t)(0xa0 | count));
+}
+
+MPACK_STATIC_INLINE void mpack_encode_str8(char* p, uint8_t count) {
+    mpack_assert(count > 31);
+    mpack_store_u8(p, 0xd9);
+    mpack_store_u8(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_str16(char* p, uint16_t count) {
+    // we might be encoding a raw in compatibility mode, so we
+    // allow count to be in the range [32, MPACK_UINT8_MAX].
+    mpack_assert(count > 31);
+    mpack_store_u8(p, 0xda);
+    mpack_store_u16(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_str32(char* p, uint32_t count) {
+    mpack_assert(count > MPACK_UINT16_MAX);
+    mpack_store_u8(p, 0xdb);
+    mpack_store_u32(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_bin8(char* p, uint8_t count) {
+    mpack_store_u8(p, 0xc4);
+    mpack_store_u8(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_bin16(char* p, uint16_t count) {
+    mpack_assert(count > MPACK_UINT8_MAX);
+    mpack_store_u8(p, 0xc5);
+    mpack_store_u16(p + 1, count);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_bin32(char* p, uint32_t count) {
+    mpack_assert(count > MPACK_UINT16_MAX);
+    mpack_store_u8(p, 0xc6);
+    mpack_store_u32(p + 1, count);
+}
+
+#if MPACK_EXTENSIONS
+MPACK_STATIC_INLINE void mpack_encode_fixext1(char* p, int8_t exttype) {
+    mpack_store_u8(p, 0xd4);
+    mpack_store_i8(p + 1, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixext2(char* p, int8_t exttype) {
+    mpack_store_u8(p, 0xd5);
+    mpack_store_i8(p + 1, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixext4(char* p, int8_t exttype) {
+    mpack_store_u8(p, 0xd6);
+    mpack_store_i8(p + 1, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixext8(char* p, int8_t exttype) {
+    mpack_store_u8(p, 0xd7);
+    mpack_store_i8(p + 1, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_fixext16(char* p, int8_t exttype) {
+    mpack_store_u8(p, 0xd8);
+    mpack_store_i8(p + 1, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_ext8(char* p, int8_t exttype, uint8_t count) {
+    mpack_assert(count != 1 && count != 2 && count != 4 && count != 8 && count != 16);
+    mpack_store_u8(p, 0xc7);
+    mpack_store_u8(p + 1, count);
+    mpack_store_i8(p + 2, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_ext16(char* p, int8_t exttype, uint16_t count) {
+    mpack_assert(count > MPACK_UINT8_MAX);
+    mpack_store_u8(p, 0xc8);
+    mpack_store_u16(p + 1, count);
+    mpack_store_i8(p + 3, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_ext32(char* p, int8_t exttype, uint32_t count) {
+    mpack_assert(count > MPACK_UINT16_MAX);
+    mpack_store_u8(p, 0xc9);
+    mpack_store_u32(p + 1, count);
+    mpack_store_i8(p + 5, exttype);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_timestamp_4(char* p, uint32_t seconds) {
+    mpack_encode_fixext4(p, MPACK_EXTTYPE_TIMESTAMP);
+    mpack_store_u32(p + MPACK_TAG_SIZE_FIXEXT4, seconds);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_timestamp_8(char* p, int64_t seconds, uint32_t nanoseconds) {
+    mpack_assert(nanoseconds <= MPACK_TIMESTAMP_NANOSECONDS_MAX);
+    mpack_encode_fixext8(p, MPACK_EXTTYPE_TIMESTAMP);
+    uint64_t encoded = ((uint64_t)nanoseconds << 34) | (uint64_t)seconds;
+    mpack_store_u64(p + MPACK_TAG_SIZE_FIXEXT8, encoded);
+}
+
+MPACK_STATIC_INLINE void mpack_encode_timestamp_12(char* p, int64_t seconds, uint32_t nanoseconds) {
+    mpack_assert(nanoseconds <= MPACK_TIMESTAMP_NANOSECONDS_MAX);
+    mpack_encode_ext8(p, MPACK_EXTTYPE_TIMESTAMP, 12);
+    mpack_store_u32(p + MPACK_TAG_SIZE_EXT8, nanoseconds);
+    mpack_store_i64(p + MPACK_TAG_SIZE_EXT8 + 4, seconds);
+}
+#endif
+
+
+
+/*
+ * Write functions
+ */
+
+// This is a macro wrapper to the encode functions to encode
+// directly into the buffer. If mpack_writer_ensure() fails
+// it will flag an error so we don't have to do anything.
+#define MPACK_WRITE_ENCODED(encode_fn, size, ...) do {                                                 \
+    if (MPACK_LIKELY(mpack_writer_buffer_left(writer) >= size) || mpack_writer_ensure(writer, size)) { \
+        MPACK_EXPAND(encode_fn(writer->position, __VA_ARGS__));                                        \
+        writer->position += size;                                                                      \
+    }                                                                                                  \
+} while (0)
+
+void mpack_write_u8(mpack_writer_t* writer, uint8_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_write_u64(writer, value);
+    #else
+    mpack_writer_track_element(writer);
+    if (value <= 127) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixuint, MPACK_TAG_SIZE_FIXUINT, value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, value);
+    }
+    #endif
+}
+
+void mpack_write_u16(mpack_writer_t* writer, uint16_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_write_u64(writer, value);
+    #else
+    mpack_writer_track_element(writer);
+    if (value <= 127) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixuint, MPACK_TAG_SIZE_FIXUINT, (uint8_t)value);
+    } else if (value <= MPACK_UINT8_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, (uint8_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_u16, MPACK_TAG_SIZE_U16, value);
+    }
+    #endif
+}
+
+void mpack_write_u32(mpack_writer_t* writer, uint32_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_write_u64(writer, value);
+    #else
+    mpack_writer_track_element(writer);
+    if (value <= 127) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixuint, MPACK_TAG_SIZE_FIXUINT, (uint8_t)value);
+    } else if (value <= MPACK_UINT8_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, (uint8_t)value);
+    } else if (value <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_u16, MPACK_TAG_SIZE_U16, (uint16_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_u32, MPACK_TAG_SIZE_U32, value);
+    }
+    #endif
+}
+
+void mpack_write_u64(mpack_writer_t* writer, uint64_t value) {
+    mpack_writer_track_element(writer);
+
+    if (value <= 127) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixuint, MPACK_TAG_SIZE_FIXUINT, (uint8_t)value);
+    } else if (value <= MPACK_UINT8_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, (uint8_t)value);
+    } else if (value <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_u16, MPACK_TAG_SIZE_U16, (uint16_t)value);
+    } else if (value <= MPACK_UINT32_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_u32, MPACK_TAG_SIZE_U32, (uint32_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_u64, MPACK_TAG_SIZE_U64, value);
+    }
+}
+
+void mpack_write_i8(mpack_writer_t* writer, int8_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_write_i64(writer, value);
+    #else
+    mpack_writer_track_element(writer);
+    if (value >= -32) {
+        // we encode positive and negative fixints together
+        MPACK_WRITE_ENCODED(mpack_encode_fixint, MPACK_TAG_SIZE_FIXINT, (int8_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_i8, MPACK_TAG_SIZE_I8, (int8_t)value);
+    }
+    #endif
+}
+
+void mpack_write_i16(mpack_writer_t* writer, int16_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_write_i64(writer, value);
+    #else
+    mpack_writer_track_element(writer);
+    if (value >= -32) {
+        if (value <= 127) {
+            // we encode positive and negative fixints together
+            MPACK_WRITE_ENCODED(mpack_encode_fixint, MPACK_TAG_SIZE_FIXINT, (int8_t)value);
+        } else if (value <= MPACK_UINT8_MAX) {
+            MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, (uint8_t)value);
+        } else {
+            MPACK_WRITE_ENCODED(mpack_encode_u16, MPACK_TAG_SIZE_U16, (uint16_t)value);
+        }
+    } else if (value >= MPACK_INT8_MIN) {
+        MPACK_WRITE_ENCODED(mpack_encode_i8, MPACK_TAG_SIZE_I8, (int8_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_i16, MPACK_TAG_SIZE_I16, (int16_t)value);
+    }
+    #endif
+}
+
+void mpack_write_i32(mpack_writer_t* writer, int32_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_write_i64(writer, value);
+    #else
+    mpack_writer_track_element(writer);
+    if (value >= -32) {
+        if (value <= 127) {
+            // we encode positive and negative fixints together
+            MPACK_WRITE_ENCODED(mpack_encode_fixint, MPACK_TAG_SIZE_FIXINT, (int8_t)value);
+        } else if (value <= MPACK_UINT8_MAX) {
+            MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, (uint8_t)value);
+        } else if (value <= MPACK_UINT16_MAX) {
+            MPACK_WRITE_ENCODED(mpack_encode_u16, MPACK_TAG_SIZE_U16, (uint16_t)value);
+        } else {
+            MPACK_WRITE_ENCODED(mpack_encode_u32, MPACK_TAG_SIZE_U32, (uint32_t)value);
+        }
+    } else if (value >= MPACK_INT8_MIN) {
+        MPACK_WRITE_ENCODED(mpack_encode_i8, MPACK_TAG_SIZE_I8, (int8_t)value);
+    } else if (value >= MPACK_INT16_MIN) {
+        MPACK_WRITE_ENCODED(mpack_encode_i16, MPACK_TAG_SIZE_I16, (int16_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_i32, MPACK_TAG_SIZE_I32, value);
+    }
+    #endif
+}
+
+void mpack_write_i64(mpack_writer_t* writer, int64_t value) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    if (value > 127) {
+        // for non-fix positive ints we call the u64 writer to save space
+        mpack_write_u64(writer, (uint64_t)value);
+        return;
+    }
+    #endif
+
+    mpack_writer_track_element(writer);
+    if (value >= -32) {
+        #if MPACK_OPTIMIZE_FOR_SIZE
+        MPACK_WRITE_ENCODED(mpack_encode_fixint, MPACK_TAG_SIZE_FIXINT, (int8_t)value);
+        #else
+        if (value <= 127) {
+            MPACK_WRITE_ENCODED(mpack_encode_fixint, MPACK_TAG_SIZE_FIXINT, (int8_t)value);
+        } else if (value <= MPACK_UINT8_MAX) {
+            MPACK_WRITE_ENCODED(mpack_encode_u8, MPACK_TAG_SIZE_U8, (uint8_t)value);
+        } else if (value <= MPACK_UINT16_MAX) {
+            MPACK_WRITE_ENCODED(mpack_encode_u16, MPACK_TAG_SIZE_U16, (uint16_t)value);
+        } else if (value <= MPACK_UINT32_MAX) {
+            MPACK_WRITE_ENCODED(mpack_encode_u32, MPACK_TAG_SIZE_U32, (uint32_t)value);
+        } else {
+            MPACK_WRITE_ENCODED(mpack_encode_u64, MPACK_TAG_SIZE_U64, (uint64_t)value);
+        }
+        #endif
+    } else if (value >= MPACK_INT8_MIN) {
+        MPACK_WRITE_ENCODED(mpack_encode_i8, MPACK_TAG_SIZE_I8, (int8_t)value);
+    } else if (value >= MPACK_INT16_MIN) {
+        MPACK_WRITE_ENCODED(mpack_encode_i16, MPACK_TAG_SIZE_I16, (int16_t)value);
+    } else if (value >= MPACK_INT32_MIN) {
+        MPACK_WRITE_ENCODED(mpack_encode_i32, MPACK_TAG_SIZE_I32, (int32_t)value);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_i64, MPACK_TAG_SIZE_I64, value);
+    }
+}
+
+#if MPACK_FLOAT
+void mpack_write_float(mpack_writer_t* writer, float value) {
+    mpack_writer_track_element(writer);
+    MPACK_WRITE_ENCODED(mpack_encode_float, MPACK_TAG_SIZE_FLOAT, value);
+}
+#else
+void mpack_write_raw_float(mpack_writer_t* writer, uint32_t value) {
+    mpack_writer_track_element(writer);
+    MPACK_WRITE_ENCODED(mpack_encode_raw_float, MPACK_TAG_SIZE_FLOAT, value);
+}
+#endif
+
+#if MPACK_DOUBLE
+void mpack_write_double(mpack_writer_t* writer, double value) {
+    mpack_writer_track_element(writer);
+    MPACK_WRITE_ENCODED(mpack_encode_double, MPACK_TAG_SIZE_DOUBLE, value);
+}
+#else
+void mpack_write_raw_double(mpack_writer_t* writer, uint64_t value) {
+    mpack_writer_track_element(writer);
+    MPACK_WRITE_ENCODED(mpack_encode_raw_double, MPACK_TAG_SIZE_DOUBLE, value);
+}
+#endif
+
+#if MPACK_EXTENSIONS
+void mpack_write_timestamp(mpack_writer_t* writer, int64_t seconds, uint32_t nanoseconds) {
+    #if MPACK_COMPATIBILITY
+    if (writer->version <= mpack_version_v4) {
+        mpack_break("Timestamps require spec version v5 or later. This writer is in v%i mode.", (int)writer->version);
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+    #endif
+
+    if (nanoseconds > MPACK_TIMESTAMP_NANOSECONDS_MAX) {
+        mpack_break("timestamp nanoseconds out of bounds: %u", nanoseconds);
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+
+    mpack_writer_track_element(writer);
+
+    if (seconds < 0 || seconds >= (MPACK_INT64_C(1) << 34)) {
+        MPACK_WRITE_ENCODED(mpack_encode_timestamp_12, MPACK_EXT_SIZE_TIMESTAMP12, seconds, nanoseconds);
+    } else if (seconds > MPACK_UINT32_MAX || nanoseconds > 0) {
+        MPACK_WRITE_ENCODED(mpack_encode_timestamp_8, MPACK_EXT_SIZE_TIMESTAMP8, seconds, nanoseconds);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_timestamp_4, MPACK_EXT_SIZE_TIMESTAMP4, (uint32_t)seconds);
+    }
+}
+#endif
+
+static void mpack_write_array_notrack(mpack_writer_t* writer, uint32_t count) {
+    if (count <= 15) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixarray, MPACK_TAG_SIZE_FIXARRAY, (uint8_t)count);
+    } else if (count <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_array16, MPACK_TAG_SIZE_ARRAY16, (uint16_t)count);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_array32, MPACK_TAG_SIZE_ARRAY32, (uint32_t)count);
+    }
+}
+
+static void mpack_write_map_notrack(mpack_writer_t* writer, uint32_t count) {
+    if (count <= 15) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixmap, MPACK_TAG_SIZE_FIXMAP, (uint8_t)count);
+    } else if (count <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_map16, MPACK_TAG_SIZE_MAP16, (uint16_t)count);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_map32, MPACK_TAG_SIZE_MAP32, (uint32_t)count);
+    }
+}
+
+void mpack_start_array(mpack_writer_t* writer, uint32_t count) {
+    mpack_writer_track_element(writer);
+    mpack_write_array_notrack(writer, count);
+    mpack_writer_track_push(writer, mpack_type_array, count);
+    mpack_builder_compound_push(writer);
+}
+
+void mpack_start_map(mpack_writer_t* writer, uint32_t count) {
+    mpack_writer_track_element(writer);
+    mpack_write_map_notrack(writer, count);
+    mpack_writer_track_push(writer, mpack_type_map, count);
+    mpack_builder_compound_push(writer);
+}
+
+static void mpack_start_str_notrack(mpack_writer_t* writer, uint32_t count) {
+    if (count <= 31) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixstr, MPACK_TAG_SIZE_FIXSTR, (uint8_t)count);
+
+    // str8 is only supported in v5 or later.
+    } else if (count <= MPACK_UINT8_MAX
+            #if MPACK_COMPATIBILITY
+            && writer->version >= mpack_version_v5
+            #endif
+            ) {
+        MPACK_WRITE_ENCODED(mpack_encode_str8, MPACK_TAG_SIZE_STR8, (uint8_t)count);
+
+    } else if (count <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_str16, MPACK_TAG_SIZE_STR16, (uint16_t)count);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_str32, MPACK_TAG_SIZE_STR32, (uint32_t)count);
+    }
+}
+
+static void mpack_start_bin_notrack(mpack_writer_t* writer, uint32_t count) {
+    #if MPACK_COMPATIBILITY
+    // In the v4 spec, there was only the raw type for any kind of
+    // variable-length data. In v4 mode, we support the bin functions,
+    // but we produce an old-style raw.
+    if (writer->version <= mpack_version_v4) {
+        mpack_start_str_notrack(writer, count);
+        return;
+    }
+    #endif
+
+    if (count <= MPACK_UINT8_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_bin8, MPACK_TAG_SIZE_BIN8, (uint8_t)count);
+    } else if (count <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_bin16, MPACK_TAG_SIZE_BIN16, (uint16_t)count);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_bin32, MPACK_TAG_SIZE_BIN32, (uint32_t)count);
+    }
+}
+
+void mpack_start_str(mpack_writer_t* writer, uint32_t count) {
+    mpack_writer_track_element(writer);
+    mpack_start_str_notrack(writer, count);
+    mpack_writer_track_push(writer, mpack_type_str, count);
+}
+
+void mpack_start_bin(mpack_writer_t* writer, uint32_t count) {
+    mpack_writer_track_element(writer);
+    mpack_start_bin_notrack(writer, count);
+    mpack_writer_track_push(writer, mpack_type_bin, count);
+}
+
+#if MPACK_EXTENSIONS
+void mpack_start_ext(mpack_writer_t* writer, int8_t exttype, uint32_t count) {
+    #if MPACK_COMPATIBILITY
+    if (writer->version <= mpack_version_v4) {
+        mpack_break("Ext types require spec version v5 or later. This writer is in v%i mode.", (int)writer->version);
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+    #endif
+
+    mpack_writer_track_element(writer);
+
+    if (count == 1) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixext1, MPACK_TAG_SIZE_FIXEXT1, exttype);
+    } else if (count == 2) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixext2, MPACK_TAG_SIZE_FIXEXT2, exttype);
+    } else if (count == 4) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixext4, MPACK_TAG_SIZE_FIXEXT4, exttype);
+    } else if (count == 8) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixext8, MPACK_TAG_SIZE_FIXEXT8, exttype);
+    } else if (count == 16) {
+        MPACK_WRITE_ENCODED(mpack_encode_fixext16, MPACK_TAG_SIZE_FIXEXT16, exttype);
+    } else if (count <= MPACK_UINT8_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_ext8, MPACK_TAG_SIZE_EXT8, exttype, (uint8_t)count);
+    } else if (count <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_ext16, MPACK_TAG_SIZE_EXT16, exttype, (uint16_t)count);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_ext32, MPACK_TAG_SIZE_EXT32, exttype, (uint32_t)count);
+    }
+
+    mpack_writer_track_push(writer, mpack_type_ext, count);
+}
+#endif
+
+
+
+/*
+ * Compound helpers and other functions
+ */
+
+void mpack_write_str(mpack_writer_t* writer, const char* data, uint32_t count) {
+    mpack_assert(data != NULL, "data for string of length %i is NULL", (int)count);
+
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_writer_track_element(writer);
+    mpack_start_str_notrack(writer, count);
+    mpack_write_native(writer, data, count);
+    #else
+
+    mpack_writer_track_element(writer);
+
+    if (count <= 31) {
+        // The minimum buffer size when using a flush function is guaranteed to
+        // fit the largest possible fixstr.
+        size_t size = count + MPACK_TAG_SIZE_FIXSTR;
+        if (MPACK_LIKELY(mpack_writer_buffer_left(writer) >= size) || mpack_writer_ensure(writer, size)) {
+            char* MPACK_RESTRICT p = writer->position;
+            mpack_encode_fixstr(p, (uint8_t)count);
+            mpack_memcpy(p + MPACK_TAG_SIZE_FIXSTR, data, count);
+            writer->position += count + MPACK_TAG_SIZE_FIXSTR;
+        }
+        return;
+    }
+
+    if (count <= MPACK_UINT8_MAX
+            #if MPACK_COMPATIBILITY
+            && writer->version >= mpack_version_v5
+            #endif
+            ) {
+        if (count + MPACK_TAG_SIZE_STR8 <= mpack_writer_buffer_left(writer)) {
+            char* MPACK_RESTRICT p = writer->position;
+            mpack_encode_str8(p, (uint8_t)count);
+            mpack_memcpy(p + MPACK_TAG_SIZE_STR8, data, count);
+            writer->position += count + MPACK_TAG_SIZE_STR8;
+        } else {
+            MPACK_WRITE_ENCODED(mpack_encode_str8, MPACK_TAG_SIZE_STR8, (uint8_t)count);
+            mpack_write_native(writer, data, count);
+        }
+        return;
+    }
+
+    // str16 and str32 are likely to be a significant fraction of the buffer
+    // size, so we don't bother with a combined space check in order to
+    // minimize code size.
+    if (count <= MPACK_UINT16_MAX) {
+        MPACK_WRITE_ENCODED(mpack_encode_str16, MPACK_TAG_SIZE_STR16, (uint16_t)count);
+        mpack_write_native(writer, data, count);
+    } else {
+        MPACK_WRITE_ENCODED(mpack_encode_str32, MPACK_TAG_SIZE_STR32, (uint32_t)count);
+        mpack_write_native(writer, data, count);
+    }
+
+    #endif
+}
+
+void mpack_write_bin(mpack_writer_t* writer, const char* data, uint32_t count) {
+    mpack_assert(data != NULL, "data pointer for bin of %i bytes is NULL", (int)count);
+    mpack_start_bin(writer, count);
+    mpack_write_bytes(writer, data, count);
+    mpack_finish_bin(writer);
+}
+
+#if MPACK_EXTENSIONS
+void mpack_write_ext(mpack_writer_t* writer, int8_t exttype, const char* data, uint32_t count) {
+    mpack_assert(data != NULL, "data pointer for ext of type %i and %i bytes is NULL", exttype, (int)count);
+    mpack_start_ext(writer, exttype, count);
+    mpack_write_bytes(writer, data, count);
+    mpack_finish_ext(writer);
+}
+#endif
+
+void mpack_write_bytes(mpack_writer_t* writer, const char* data, size_t count) {
+    mpack_assert(data != NULL, "data pointer for %i bytes is NULL", (int)count);
+    mpack_writer_track_bytes(writer, count);
+    mpack_write_native(writer, data, count);
+}
+
+void mpack_write_cstr(mpack_writer_t* writer, const char* cstr) {
+    mpack_assert(cstr != NULL, "cstr pointer is NULL");
+    size_t length = mpack_strlen(cstr);
+    if (length > MPACK_UINT32_MAX)
+        mpack_writer_flag_error(writer, mpack_error_invalid);
+    mpack_write_str(writer, cstr, (uint32_t)length);
+}
+
+void mpack_write_cstr_or_nil(mpack_writer_t* writer, const char* cstr) {
+    if (cstr)
+        mpack_write_cstr(writer, cstr);
+    else
+        mpack_write_nil(writer);
+}
+
+void mpack_write_utf8(mpack_writer_t* writer, const char* str, uint32_t length) {
+    mpack_assert(str != NULL, "data for string of length %i is NULL", (int)length);
+    if (!mpack_utf8_check(str, length)) {
+        mpack_writer_flag_error(writer, mpack_error_invalid);
+        return;
+    }
+    mpack_write_str(writer, str, length);
+}
+
+void mpack_write_utf8_cstr(mpack_writer_t* writer, const char* cstr) {
+    mpack_assert(cstr != NULL, "cstr pointer is NULL");
+    size_t length = mpack_strlen(cstr);
+    if (length > MPACK_UINT32_MAX) {
+        mpack_writer_flag_error(writer, mpack_error_invalid);
+        return;
+    }
+    mpack_write_utf8(writer, cstr, (uint32_t)length);
+}
+
+void mpack_write_utf8_cstr_or_nil(mpack_writer_t* writer, const char* cstr) {
+    if (cstr)
+        mpack_write_utf8_cstr(writer, cstr);
+    else
+        mpack_write_nil(writer);
+}
+
+/*
+ * Builder implementation
+ *
+ * When a writer is in build mode, it diverts writes to an internal growable
+ * buffer. All elements other than builder start tags are encoded as normal
+ * into the builder buffer (even nested maps and arrays of known size, e.g.
+ * `mpack_start_array()`.) But for compound elements of unknown size, an
+ * mpack_build_t is written to the buffer instead.
+ *
+ * The mpack_build_t tracks everything needed to re-constitute the final
+ * message once all sizes are known. When the last build element is completed,
+ * the builder resolves the build by walking through the builds, outputting the
+ * final encoded tag, and copying everything in between to the writer's true
+ * buffer.
+ *
+ * To make things extra complicated, the builder buffer is not contiguous. It's
+ * allocated in pages, where the first page may be an internal page in the
+ * writer. But, each mpack_build_t must itself be contiguous and aligned
+ * properly within the buffer. This means bytes can be skipped (and wasted)
+ * before the builds or at the end of pages.
+ *
+ * To keep track of this, builds store both their element count and the number
+ * of encoded bytes that follow, and pages store the number of bytes used. As
+ * elements are written, each element adds to the count in the current open
+ * build, and the number of bytes written adds to the current page and the byte
+ * count in the last started build (whether or not it is completed.)
+ */
+
+#if MPACK_BUILDER
+
+#ifdef MPACK_ALIGNOF
+    #define MPACK_BUILD_ALIGNMENT MPACK_ALIGNOF(mpack_build_t)
+#else
+    // without alignof, we just align to the greater of size_t, void* and uint64_t.
+    // (we do this even though we don't have uint64_t in it in case we add it later.)
+    #define MPACK_BUILD_ALIGNMENT_MAX(x, y) ((x) > (y) ? (x) : (y))
+    #define MPACK_BUILD_ALIGNMENT (MPACK_BUILD_ALIGNMENT_MAX(sizeof(void*), \
+                MPACK_BUILD_ALIGNMENT_MAX(sizeof(size_t), sizeof(uint64_t))))
+#endif
+
+static inline void mpack_builder_check_sizes(mpack_writer_t* writer) {
+
+    // We check internal and page sizes here so that we don't have to check
+    // them again. A new page with a build in it will have a page header,
+    // build, and minimum space for a tag. This will perform horribly and waste
+    // tons of memory if the page size is small, so you're best off just
+    // sticking with the defaults.
+    //
+    // These are all known at compile time, so if they are large
+    // enough this function should trivially optimize to a no-op.
+
+    #if MPACK_BUILDER_INTERNAL_STORAGE
+    // make sure the internal storage is big enough to be useful
+    MPACK_STATIC_ASSERT(MPACK_BUILDER_INTERNAL_STORAGE_SIZE >= (sizeof(mpack_builder_page_t) +
+            sizeof(mpack_build_t) + MPACK_WRITER_MINIMUM_BUFFER_SIZE),
+            "MPACK_BUILDER_INTERNAL_STORAGE_SIZE is too small to be useful!");
+    if (MPACK_BUILDER_INTERNAL_STORAGE_SIZE < (sizeof(mpack_builder_page_t) +
+            sizeof(mpack_build_t) + MPACK_WRITER_MINIMUM_BUFFER_SIZE))
+    {
+        mpack_break("MPACK_BUILDER_INTERNAL_STORAGE_SIZE is too small to be useful!");
+        mpack_writer_flag_error(writer, mpack_error_bug);
+    }
+    #endif
+
+    // make sure the builder page size is big enough to be useful
+    MPACK_STATIC_ASSERT(MPACK_BUILDER_PAGE_SIZE >= (sizeof(mpack_builder_page_t) +
+            sizeof(mpack_build_t) + MPACK_WRITER_MINIMUM_BUFFER_SIZE),
+            "MPACK_BUILDER_PAGE_SIZE is too small to be useful!");
+    if (MPACK_BUILDER_PAGE_SIZE < (sizeof(mpack_builder_page_t) +
+            sizeof(mpack_build_t) + MPACK_WRITER_MINIMUM_BUFFER_SIZE))
+    {
+        mpack_break("MPACK_BUILDER_PAGE_SIZE is too small to be useful!");
+        mpack_writer_flag_error(writer, mpack_error_bug);
+    }
+}
+
+static inline size_t mpack_builder_page_size(mpack_writer_t* writer, mpack_builder_page_t* page) {
+    #if MPACK_BUILDER_INTERNAL_STORAGE
+    if ((char*)page == writer->builder.internal)
+        return sizeof(writer->builder.internal);
+    #else
+    (void)writer;
+    (void)page;
+    #endif
+    return MPACK_BUILDER_PAGE_SIZE;
+}
+
+static inline size_t mpack_builder_align_build(size_t bytes_used) {
+    size_t offset = bytes_used;
+    offset += MPACK_BUILD_ALIGNMENT - 1;
+    offset -= offset % MPACK_BUILD_ALIGNMENT;
+    mpack_log("aligned %zi to %zi\n", bytes_used, offset);
+    return offset;
+}
+
+static inline void mpack_builder_free_page(mpack_writer_t* writer, mpack_builder_page_t* page) {
+    mpack_log("freeing page %p\n", (void*)page);
+    #if MPACK_BUILDER_INTERNAL_STORAGE
+    if ((char*)page == writer->builder.internal)
+        return;
+    #else
+    (void)writer;
+    #endif
+    MPACK_FREE(page);
+}
+
+static inline size_t mpack_builder_page_remaining(mpack_writer_t* writer, mpack_builder_page_t* page) {
+    return mpack_builder_page_size(writer, page) - page->bytes_used;
+}
+
+static void mpack_builder_configure_buffer(mpack_writer_t* writer) {
+    if (mpack_writer_error(writer) != mpack_ok)
+        return;
+    mpack_builder_t* builder = &writer->builder;
+
+    mpack_builder_page_t* page = builder->current_page;
+    mpack_assert(page != NULL, "page is null??");
+
+    // This diverts the writer into the remainder of the current page of our
+    // build buffer.
+    writer->buffer = (char*)page + page->bytes_used;
+    writer->position = (char*)page + page->bytes_used;
+    writer->end = (char*)page + mpack_builder_page_size(writer, page);
+    mpack_log("configuring buffer from %p to %p\n", (void*)writer->position, (void*)writer->end);
+}
+
+static void mpack_builder_add_page(mpack_writer_t* writer) {
+    mpack_builder_t* builder = &writer->builder;
+    mpack_assert(writer->error == mpack_ok);
+
+    mpack_log("adding a page.\n");
+    mpack_builder_page_t* page = (mpack_builder_page_t*)MPACK_MALLOC(MPACK_BUILDER_PAGE_SIZE);
+    if (page == NULL) {
+        mpack_writer_flag_error(writer, mpack_error_memory);
+        return;
+    }
+
+    page->next = NULL;
+    page->bytes_used = sizeof(mpack_builder_page_t);
+    builder->current_page->next = page;
+    builder->current_page = page;
+}
+
+// Checks how many bytes the writer wrote to the page, adding it to the page's
+// bytes_used. This must be followed up with mpack_builder_configure_buffer()
+// (after adding a new page, build, etc) to reset the writer's buffer pointers.
+static void mpack_builder_apply_writes(mpack_writer_t* writer) {
+    mpack_assert(writer->error == mpack_ok);
+    mpack_builder_t* builder = &writer->builder;
+    mpack_log("latest build is %p\n", (void*)builder->latest_build);
+
+    // The difference between buffer and current is the number of bytes that
+    // were written to the page.
+    size_t bytes_written = (size_t)(writer->position - writer->buffer);
+    mpack_log("applying write of %zi bytes to build %p\n", bytes_written, (void*)builder->latest_build);
+
+    mpack_assert(builder->current_page != NULL);
+    mpack_assert(builder->latest_build != NULL);
+    builder->current_page->bytes_used += bytes_written;
+    builder->latest_build->bytes += bytes_written;
+    mpack_log("latest build %p now has %zi bytes\n", (void*)builder->latest_build, builder->latest_build->bytes);
+}
+
+static void mpack_builder_flush(mpack_writer_t* writer) {
+    mpack_assert(writer->error == mpack_ok);
+    mpack_builder_apply_writes(writer);
+    mpack_builder_add_page(writer);
+    mpack_builder_configure_buffer(writer);
+}
+
+MPACK_NOINLINE static void mpack_builder_begin(mpack_writer_t* writer) {
+    mpack_builder_t* builder = &writer->builder;
+    mpack_assert(writer->error == mpack_ok);
+    mpack_assert(builder->current_build == NULL);
+    mpack_assert(builder->latest_build == NULL);
+    mpack_assert(builder->pages == NULL);
+
+    // If this is the first build, we need to stash the real buffer backing our
+    // writer. We'll be diverting the writer to our build buffer.
+    builder->stash_buffer = writer->buffer;
+    builder->stash_position = writer->position;
+    builder->stash_end = writer->end;
+
+    mpack_builder_page_t* page;
+
+    // we've checked that both these sizes are large enough above.
+    #if MPACK_BUILDER_INTERNAL_STORAGE
+    page = (mpack_builder_page_t*)builder->internal;
+    mpack_log("beginning builder with internal storage %p\n", (void*)page);
+    #else
+    page = (mpack_builder_page_t*)MPACK_MALLOC(MPACK_BUILDER_PAGE_SIZE);
+    if (page == NULL) {
+        mpack_writer_flag_error(writer, mpack_error_memory);
+        return;
+    }
+    mpack_log("beginning builder with allocated page %p\n", (void*)page);
+    #endif
+
+    page->next = NULL;
+    page->bytes_used = sizeof(mpack_builder_page_t);
+    builder->pages = page;
+    builder->current_page = page;
+}
+
+static void mpack_builder_build(mpack_writer_t* writer, mpack_type_t type) {
+    mpack_builder_check_sizes(writer);
+    if (mpack_writer_error(writer) != mpack_ok)
+        return;
+
+    mpack_writer_track_element(writer);
+    mpack_writer_track_push_builder(writer, type);
+
+    mpack_builder_t* builder = &writer->builder;
+
+    if (builder->current_build == NULL) {
+        mpack_builder_begin(writer);
+    } else {
+        mpack_builder_apply_writes(writer);
+    }
+    if (mpack_writer_error(writer) != mpack_ok)
+        return;
+
+    // find aligned space for a new build. if there isn't enough space in the
+    // current page, we discard the remaining space in it and allocate a new
+    // page.
+    size_t offset = mpack_builder_align_build(builder->current_page->bytes_used);
+    if (offset + sizeof(mpack_build_t) > mpack_builder_page_size(writer, builder->current_page)) {
+        mpack_log("not enough space for a build. %zi bytes used of %zi in this page\n",
+                builder->current_page->bytes_used, mpack_builder_page_size(writer, builder->current_page));
+        mpack_builder_add_page(writer);
+        // there is always enough space in a fresh page.
+        offset = mpack_builder_align_build(builder->current_page->bytes_used);
+    }
+
+    // allocate the build within the page. note that we don't keep track of the
+    // space wasted due to the offset. instead the previous build has stored
+    // how many bytes follow it, and we'll redo this offset calculation to find
+    // this build after it.
+    mpack_builder_page_t* page = builder->current_page;
+    page->bytes_used = offset + sizeof(mpack_build_t);
+    mpack_assert(page->bytes_used <= mpack_builder_page_size(writer, page));
+    mpack_build_t* build = (mpack_build_t*)((char*)page + offset);
+    mpack_log("created new build %p within page %p, which now has %zi bytes used\n",
+            (void*)build, (void*)page, page->bytes_used);
+
+    // configure the new build
+    build->parent = builder->current_build;
+    build->bytes = 0;
+    build->count = 0;
+    build->type = type;
+    build->key_needs_value = false;
+    build->nested_compound_elements = 0;
+
+    mpack_log("setting current and latest build to new build %p\n", (void*)build);
+    builder->current_build = build;
+    builder->latest_build = build;
+
+    // we always need to provide a buffer that meets the minimum buffer size.
+    // if there isn't enough space, we discard the remaining space in the
+    // current page and allocate a new one.
+    if (mpack_builder_page_remaining(writer, page) < MPACK_WRITER_MINIMUM_BUFFER_SIZE) {
+        mpack_log("less than minimum buffer size in current page. %zi bytes used of %zi in this page\n",
+                builder->current_page->bytes_used, mpack_builder_page_size(writer, builder->current_page));
+        mpack_builder_add_page(writer);
+        if (mpack_writer_error(writer) != mpack_ok)
+            return;
+    }
+    mpack_assert(mpack_builder_page_remaining(writer, builder->current_page) >= MPACK_WRITER_MINIMUM_BUFFER_SIZE);
+    mpack_builder_configure_buffer(writer);
+}
+
+MPACK_NOINLINE
+static void mpack_builder_resolve(mpack_writer_t* writer) {
+    mpack_builder_t* builder = &writer->builder;
+
+    // The starting page is the internal storage (if we have it), otherwise
+    // it's the first page in the array
+    mpack_builder_page_t* page =
+        #if MPACK_BUILDER_INTERNAL_STORAGE
+        (mpack_builder_page_t*)builder->internal
+        #else
+        builder->pages
+        #endif
+        ;
+
+    // We start by restoring the writer's original buffer so we can write the
+    // data for real.
+    writer->buffer = builder->stash_buffer;
+    writer->position = builder->stash_position;
+    writer->end = builder->stash_end;
+
+    // We can also close out the build now.
+    builder->current_build = NULL;
+    builder->latest_build = NULL;
+    builder->current_page = NULL;
+    builder->pages = NULL;
+
+    // the starting page always starts with the first build
+    size_t offset = mpack_builder_align_build(sizeof(mpack_builder_page_t));
+    mpack_build_t* build = (mpack_build_t*)((char*)page + offset);
+    mpack_log("starting resolve with build %p in page %p\n", (void*)build, (void*)page);
+
+    // encoded data immediately follows the build
+    offset += sizeof(mpack_build_t);
+
+    // Walk the list of builds, writing everything out in the buffer. Note that
+    // we don't check for errors anywhere. The lower-level write functions will
+    // all check for errors. We need to walk all pages anyway to free them, so
+    // there's not much point in optimizing an error path at the expense of the
+    // normal path.
+    while (true) {
+
+        // write out the container tag
+        mpack_log("writing out an %s with count %u followed by %zi bytes\n",
+                mpack_type_to_string(build->type), build->count, build->bytes);
+        switch (build->type) {
+            case mpack_type_map:
+                mpack_write_map_notrack(writer, build->count);
+                break;
+            case mpack_type_array:
+                mpack_write_array_notrack(writer, build->count);
+                break;
+            default:
+                mpack_break("invalid type in builder?");
+                mpack_writer_flag_error(writer, mpack_error_bug);
+                return;
+        }
+
+        // figure out how many bytes follow this container. we're going to be
+        // freeing pages as we write, so we need to be done with this build.
+        size_t left = build->bytes;
+        build = NULL;
+
+        // write out all bytes following this container
+        while (left > 0) {
+            size_t bytes_used = page->bytes_used;
+            if (offset < bytes_used) {
+                size_t step = bytes_used - offset;
+                if (step > left)
+                    step = left;
+                mpack_log("writing out %zi bytes starting at %p in page %p\n",
+                        step, (void*)((char*)page + offset), (void*)page);
+                mpack_write_native(writer, (char*)page + offset, step);
+                offset += step;
+                left -= step;
+            }
+
+            if (left == 0) {
+                mpack_log("done writing bytes for this build\n");
+                break;
+            }
+
+            // still need to write more bytes. free this page and jump to the
+            // next one.
+            mpack_builder_page_t* next_page = page->next;
+            mpack_builder_free_page(writer, page);
+            page = next_page;
+            // bytes on the next page immediately follow the header.
+            offset = sizeof(mpack_builder_page_t);
+        }
+
+        // now see if we can find another build.
+        offset = mpack_builder_align_build(offset);
+        if (offset + sizeof(mpack_build_t) >= mpack_builder_page_size(writer, page)) {
+            mpack_log("not enough room in this page for another build\n");
+            mpack_builder_page_t* next_page = page->next;
+            mpack_builder_free_page(writer, page);
+            page = next_page;
+            if (page == NULL) {
+                mpack_log("no more pages\n");
+                // there are no more pages. we're done.
+                break;
+            }
+            offset = mpack_builder_align_build(sizeof(mpack_builder_page_t));
+        }
+        if (offset + sizeof(mpack_build_t) > page->bytes_used) {
+            // there is no more data. we're done.
+            mpack_log("no more data\n");
+            mpack_builder_free_page(writer, page);
+            break;
+        }
+
+        // we've found another build. loop around!
+        build = (mpack_build_t*)((char*)page + offset);
+        offset += sizeof(mpack_build_t);
+        mpack_log("found build %p\n", (void*)build);
+    }
+
+    mpack_log("done resolve.\n");
+}
+
+static void mpack_builder_complete(mpack_writer_t* writer, mpack_type_t type) {
+    if (mpack_writer_error(writer) != mpack_ok)
+        return;
+
+    mpack_writer_track_pop_builder(writer, type);
+    mpack_builder_t* builder = &writer->builder;
+    mpack_assert(builder->current_build != NULL, "no build in progress!");
+    mpack_assert(builder->latest_build != NULL, "missing latest build!");
+    mpack_assert(builder->current_build->type == type, "completing wrong type!");
+    mpack_log("completing build %p\n", (void*)builder->current_build);
+
+    if (builder->current_build->key_needs_value) {
+        mpack_break("an odd number of elements were written in a map!");
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+
+    if (builder->current_build->nested_compound_elements != 0) {
+        mpack_break("there is a nested unfinished non-build map or array in this build.");
+        mpack_writer_flag_error(writer, mpack_error_bug);
+        return;
+    }
+
+    // We need to apply whatever writes have been made to the current build
+    // before popping it.
+    mpack_builder_apply_writes(writer);
+
+    // For a nested build, we just switch the current build back to its parent.
+    if (builder->current_build->parent != NULL) {
+        mpack_log("setting current build to parent build %p. latest is still %p.\n",
+                (void*)builder->current_build->parent, (void*)builder->latest_build);
+        builder->current_build = builder->current_build->parent;
+        mpack_builder_configure_buffer(writer);
+    } else {
+        // We're completing the final build.
+        mpack_builder_resolve(writer);
+    }
+}
+
+void mpack_build_map(mpack_writer_t* writer) {
+    mpack_builder_build(writer, mpack_type_map);
+}
+
+void mpack_build_array(mpack_writer_t* writer) {
+    mpack_builder_build(writer, mpack_type_array);
+}
+
+void mpack_complete_map(mpack_writer_t* writer) {
+    mpack_builder_complete(writer, mpack_type_map);
+}
+
+void mpack_complete_array(mpack_writer_t* writer) {
+    mpack_builder_complete(writer, mpack_type_array);
+}
+
+#endif // MPACK_BUILDER
+#endif // MPACK_WRITER
+
+MPACK_SILENCE_WARNINGS_END
+
+/* mpack/mpack-reader.c.c */
+
+#define MPACK_INTERNAL 1
+
+/* #include "mpack-reader.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+
+#if MPACK_READER
+
+static void mpack_reader_skip_using_fill(mpack_reader_t* reader, size_t count);
+
+void mpack_reader_init(mpack_reader_t* reader, char* buffer, size_t size, size_t count) {
+    mpack_assert(buffer != NULL, "buffer is NULL");
+
+    mpack_memset(reader, 0, sizeof(*reader));
+    reader->buffer = buffer;
+    reader->size = size;
+    reader->data = buffer;
+    reader->end = buffer + count;
+
+    #if MPACK_READ_TRACKING
+    mpack_reader_flag_if_error(reader, mpack_track_init(&reader->track));
+    #endif
+
+    mpack_log("===========================\n");
+    mpack_log("initializing reader with buffer size %i\n", (int)size);
+}
+
+void mpack_reader_init_error(mpack_reader_t* reader, mpack_error_t error) {
+    mpack_memset(reader, 0, sizeof(*reader));
+    reader->error = error;
+
+    mpack_log("===========================\n");
+    mpack_log("initializing reader error state %i\n", (int)error);
+}
+
+void mpack_reader_init_data(mpack_reader_t* reader, const char* data, size_t count) {
+    mpack_assert(data != NULL, "data is NULL");
+
+    mpack_memset(reader, 0, sizeof(*reader));
+    reader->data = data;
+    reader->end = data + count;
+
+    #if MPACK_READ_TRACKING
+    mpack_reader_flag_if_error(reader, mpack_track_init(&reader->track));
+    #endif
+
+    mpack_log("===========================\n");
+    mpack_log("initializing reader with data size %i\n", (int)count);
+}
+
+void mpack_reader_set_fill(mpack_reader_t* reader, mpack_reader_fill_t fill) {
+    MPACK_STATIC_ASSERT(MPACK_READER_MINIMUM_BUFFER_SIZE >= MPACK_MAXIMUM_TAG_SIZE,
+            "minimum buffer size must fit any tag!");
+
+    if (reader->size == 0) {
+        mpack_break("cannot use fill function without a writeable buffer!");
+        mpack_reader_flag_error(reader, mpack_error_bug);
+        return;
+    }
+
+    if (reader->size < MPACK_READER_MINIMUM_BUFFER_SIZE) {
+        mpack_break("buffer size is %i, but minimum buffer size for fill is %i",
+                (int)reader->size, MPACK_READER_MINIMUM_BUFFER_SIZE);
+        mpack_reader_flag_error(reader, mpack_error_bug);
+        return;
+    }
+
+    reader->fill = fill;
+}
+
+void mpack_reader_set_skip(mpack_reader_t* reader, mpack_reader_skip_t skip) {
+    mpack_assert(reader->size != 0, "cannot use skip function without a writeable buffer!");
+    reader->skip = skip;
+}
+
+#if MPACK_STDIO
+static size_t mpack_file_reader_fill(mpack_reader_t* reader, char* buffer, size_t count) {
+    if (feof((FILE *)reader->context)) {
+       mpack_reader_flag_error(reader, mpack_error_eof);
+       return 0;
+    }
+    return fread((void*)buffer, 1, count, (FILE*)reader->context);
+}
+
+static void mpack_file_reader_skip(mpack_reader_t* reader, size_t count) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return;
+    FILE* file = (FILE*)reader->context;
+
+    // We call ftell() to test whether the stream is seekable
+    // without causing a file error.
+    if (ftell(file) >= 0) {
+        mpack_log("seeking forward %i bytes\n", (int)count);
+        if (fseek(file, (long int)count, SEEK_CUR) == 0)
+            return;
+        mpack_log("fseek() didn't return zero!\n");
+        if (ferror(file)) {
+            mpack_reader_flag_error(reader, mpack_error_io);
+            return;
+        }
+    }
+
+    // If the stream is not seekable, fall back to the fill function.
+    mpack_reader_skip_using_fill(reader, count);
+}
+
+static void mpack_file_reader_teardown(mpack_reader_t* reader) {
+    MPACK_FREE(reader->buffer);
+    reader->buffer = NULL;
+    reader->context = NULL;
+    reader->size = 0;
+    reader->fill = NULL;
+    reader->skip = NULL;
+    reader->teardown = NULL;
+}
+
+static void mpack_file_reader_teardown_close(mpack_reader_t* reader) {
+    FILE* file = (FILE*)reader->context;
+
+    if (file) {
+        int ret = fclose(file);
+        if (ret != 0)
+            mpack_reader_flag_error(reader, mpack_error_io);
+    }
+
+    mpack_file_reader_teardown(reader);
+}
+
+void mpack_reader_init_stdfile(mpack_reader_t* reader, FILE* file, bool close_when_done) {
+    mpack_assert(file != NULL, "file is NULL");
+
+    size_t capacity = MPACK_BUFFER_SIZE;
+    char* buffer = (char*)MPACK_MALLOC(capacity);
+    if (buffer == NULL) {
+        mpack_reader_init_error(reader, mpack_error_memory);
+        if (close_when_done) {
+            fclose(file);
+        }
+        return;
+    }
+
+    mpack_reader_init(reader, buffer, capacity, 0);
+    mpack_reader_set_context(reader, file);
+    mpack_reader_set_fill(reader, mpack_file_reader_fill);
+    mpack_reader_set_skip(reader, mpack_file_reader_skip);
+    mpack_reader_set_teardown(reader, close_when_done ?
+            mpack_file_reader_teardown_close :
+            mpack_file_reader_teardown);
+}
+
+void mpack_reader_init_filename(mpack_reader_t* reader, const char* filename) {
+    mpack_assert(filename != NULL, "filename is NULL");
+
+    FILE* file = fopen(filename, "rb");
+    if (file == NULL) {
+        mpack_reader_init_error(reader, mpack_error_io);
+        return;
+    }
+
+    mpack_reader_init_stdfile(reader, file, true);
+}
+#endif
+
+mpack_error_t mpack_reader_destroy(mpack_reader_t* reader) {
+
+    // clean up tracking, asserting if we're not already in an error state
+    #if MPACK_READ_TRACKING
+    mpack_reader_flag_if_error(reader, mpack_track_destroy(&reader->track, mpack_reader_error(reader) != mpack_ok));
+    #endif
+
+    if (reader->teardown)
+        reader->teardown(reader);
+    reader->teardown = NULL;
+
+    return reader->error;
+}
+
+size_t mpack_reader_remaining(mpack_reader_t* reader, const char** data) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return 0;
+
+    #if MPACK_READ_TRACKING
+    if (mpack_reader_flag_if_error(reader, mpack_track_check_empty(&reader->track)) != mpack_ok)
+        return 0;
+    #endif
+
+    if (data)
+        *data = reader->data;
+    return (size_t)(reader->end - reader->data);
+}
+
+void mpack_reader_flag_error(mpack_reader_t* reader, mpack_error_t error) {
+    mpack_log("reader %p setting error %i: %s\n", (void*)reader, (int)error, mpack_error_to_string(error));
+
+    if (reader->error == mpack_ok) {
+        reader->error = error;
+        reader->end = reader->data;
+        if (reader->error_fn)
+            reader->error_fn(reader, error);
+    }
+}
+
+// Loops on the fill function, reading between the minimum and
+// maximum number of bytes and flagging an error if it fails.
+MPACK_NOINLINE static size_t mpack_fill_range(mpack_reader_t* reader, char* p, size_t min_bytes, size_t max_bytes) {
+    mpack_assert(reader->fill != NULL, "mpack_fill_range() called with no fill function?");
+    mpack_assert(min_bytes > 0, "cannot fill zero bytes!");
+    mpack_assert(max_bytes >= min_bytes, "min_bytes %i cannot be larger than max_bytes %i!",
+            (int)min_bytes, (int)max_bytes);
+
+    size_t count = 0;
+    while (count < min_bytes) {
+        size_t read = reader->fill(reader, p + count, max_bytes - count);
+
+        // Reader fill functions can flag an error or return 0 on failure. We
+        // also guard against functions that return -1 just in case.
+        if (mpack_reader_error(reader) != mpack_ok)
+            return 0;
+        if (read == 0 || read == ((size_t)(-1))) {
+            mpack_reader_flag_error(reader, mpack_error_io);
+            return 0;
+        }
+
+        count += read;
+    }
+    return count;
+}
+
+MPACK_NOINLINE bool mpack_reader_ensure_straddle(mpack_reader_t* reader, size_t count) {
+    mpack_assert(count != 0, "cannot ensure zero bytes!");
+    mpack_assert(reader->error == mpack_ok, "reader cannot be in an error state!");
+
+    mpack_assert(count > (size_t)(reader->end - reader->data),
+            "straddling ensure requested for %i bytes, but there are %i bytes "
+            "left in buffer. call mpack_reader_ensure() instead",
+            (int)count, (int)(reader->end - reader->data));
+
+    // we'll need a fill function to get more data. if there's no
+    // fill function, the buffer should contain an entire MessagePack
+    // object, so we raise mpack_error_invalid instead of mpack_error_io
+    // on truncated data.
+    if (reader->fill == NULL) {
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        return false;
+    }
+
+    // we need enough space in the buffer. if the buffer is not
+    // big enough, we return mpack_error_too_big (since this is
+    // for an in-place read larger than the buffer size.)
+    if (count > reader->size) {
+        mpack_reader_flag_error(reader, mpack_error_too_big);
+        return false;
+    }
+
+    // move the existing data to the start of the buffer
+    size_t left = (size_t)(reader->end - reader->data);
+    mpack_memmove(reader->buffer, reader->data, left);
+    reader->end -= reader->data - reader->buffer;
+    reader->data = reader->buffer;
+
+    // read at least the necessary number of bytes, accepting up to the
+    // buffer size
+    size_t read = mpack_fill_range(reader, reader->buffer + left,
+            count - left, reader->size - left);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return false;
+    reader->end += read;
+    return true;
+}
+
+// Reads count bytes into p. Used when there are not enough bytes
+// left in the buffer to satisfy a read.
+MPACK_NOINLINE void mpack_read_native_straddle(mpack_reader_t* reader, char* p, size_t count) {
+    mpack_assert(count == 0 || p != NULL, "data pointer for %i bytes is NULL", (int)count);
+
+    if (mpack_reader_error(reader) != mpack_ok) {
+        mpack_memset(p, 0, count);
+        return;
+    }
+
+    size_t left = (size_t)(reader->end - reader->data);
+    mpack_log("big read for %i bytes into %p, %i left in buffer, buffer size %i\n",
+            (int)count, p, (int)left, (int)reader->size);
+
+    if (count <= left) {
+        mpack_assert(0,
+                "big read requested for %i bytes, but there are %i bytes "
+                "left in buffer. call mpack_read_native() instead",
+                (int)count, (int)left);
+        mpack_reader_flag_error(reader, mpack_error_bug);
+        mpack_memset(p, 0, count);
+        return;
+    }
+
+    // we'll need a fill function to get more data. if there's no
+    // fill function, the buffer should contain an entire MessagePack
+    // object, so we raise mpack_error_invalid instead of mpack_error_io
+    // on truncated data.
+    if (reader->fill == NULL) {
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        mpack_memset(p, 0, count);
+        return;
+    }
+
+    if (reader->size == 0) {
+        // somewhat debatable what error should be returned here. when
+        // initializing a reader with an in-memory buffer it's not
+        // necessarily a bug if the data is blank; it might just have
+        // been truncated to zero. for this reason we return the same
+        // error as if the data was truncated.
+        mpack_reader_flag_error(reader, mpack_error_io);
+        mpack_memset(p, 0, count);
+        return;
+    }
+
+    // flush what's left of the buffer
+    if (left > 0) {
+        mpack_log("flushing %i bytes remaining in buffer\n", (int)left);
+        mpack_memcpy(p, reader->data, left);
+        count -= left;
+        p += left;
+        reader->data += left;
+    }
+
+    // if the remaining data needed is some small fraction of the
+    // buffer size, we'll try to fill the buffer as much as possible
+    // and copy the needed data out.
+    if (count <= reader->size / MPACK_READER_SMALL_FRACTION_DENOMINATOR) {
+        size_t read = mpack_fill_range(reader, reader->buffer, count, reader->size);
+        if (mpack_reader_error(reader) != mpack_ok)
+            return;
+        mpack_memcpy(p, reader->buffer, count);
+        reader->data = reader->buffer + count;
+        reader->end = reader->buffer + read;
+
+    // otherwise we read the remaining data directly into the target.
+    } else {
+        mpack_log("reading %i additional bytes\n", (int)count);
+        mpack_fill_range(reader, p, count, count);
+    }
+}
+
+MPACK_NOINLINE static void mpack_skip_bytes_straddle(mpack_reader_t* reader, size_t count) {
+
+    // we'll need at least a fill function to skip more data. if there's
+    // no fill function, the buffer should contain an entire MessagePack
+    // object, so we raise mpack_error_invalid instead of mpack_error_io
+    // on truncated data. (see mpack_read_native_straddle())
+    if (reader->fill == NULL) {
+        mpack_log("reader has no fill function!\n");
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        return;
+    }
+
+    // discard whatever's left in the buffer
+    size_t left = (size_t)(reader->end - reader->data);
+    mpack_log("discarding %i bytes still in buffer\n", (int)left);
+    count -= left;
+    reader->data = reader->end;
+
+    // use the skip function if we've got one, and if we're trying
+    // to skip a lot of data. if we only need to skip some tiny
+    // fraction of the buffer size, it's probably better to just
+    // fill the buffer and skip from it instead of trying to seek.
+    if (reader->skip && count > reader->size / 16) {
+        mpack_log("calling skip function for %i bytes\n", (int)count);
+        reader->skip(reader, count);
+        return;
+    }
+
+    mpack_reader_skip_using_fill(reader, count);
+}
+
+void mpack_skip_bytes(mpack_reader_t* reader, size_t count) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return;
+    mpack_log("skip requested for %i bytes\n", (int)count);
+
+    mpack_reader_track_bytes(reader, count);
+
+    // check if we have enough in the buffer already
+    size_t left = (size_t)(reader->end - reader->data);
+    if (left >= count) {
+        mpack_log("skipping %u bytes still in buffer\n", (uint32_t)count);
+        reader->data += count;
+        return;
+    }
+
+    mpack_skip_bytes_straddle(reader, count);
+}
+
+MPACK_NOINLINE static void mpack_reader_skip_using_fill(mpack_reader_t* reader, size_t count) {
+    mpack_assert(reader->fill != NULL, "missing fill function!");
+    mpack_assert(reader->data == reader->end, "there are bytes left in the buffer!");
+    mpack_assert(reader->error == mpack_ok, "should not have called this in an error state (%i)", reader->error);
+    mpack_log("skip using fill for %i bytes\n", (int)count);
+
+    // fill and discard multiples of the buffer size
+    while (count > reader->size) {
+        mpack_log("filling and discarding buffer of %i bytes\n", (int)reader->size);
+        if (mpack_fill_range(reader, reader->buffer, reader->size, reader->size) < reader->size) {
+            mpack_reader_flag_error(reader, mpack_error_io);
+            return;
+        }
+        count -= reader->size;
+    }
+
+    // fill the buffer as much as possible
+    reader->data = reader->buffer;
+    size_t read = mpack_fill_range(reader, reader->buffer, count, reader->size);
+    if (read < count) {
+        mpack_reader_flag_error(reader, mpack_error_io);
+        return;
+    }
+    reader->end = reader->data + read;
+    mpack_log("filled %i bytes into buffer; discarding %i bytes\n", (int)read, (int)count);
+    reader->data += count;
+}
+
+void mpack_read_bytes(mpack_reader_t* reader, char* p, size_t count) {
+    mpack_assert(p != NULL, "destination for read of %i bytes is NULL", (int)count);
+    mpack_reader_track_bytes(reader, count);
+    mpack_read_native(reader, p, count);
+}
+
+void mpack_read_utf8(mpack_reader_t* reader, char* p, size_t byte_count) {
+    mpack_assert(p != NULL, "destination for read of %i bytes is NULL", (int)byte_count);
+    mpack_reader_track_str_bytes_all(reader, byte_count);
+    mpack_read_native(reader, p, byte_count);
+
+    if (mpack_reader_error(reader) == mpack_ok && !mpack_utf8_check(p, byte_count))
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+static void mpack_read_cstr_unchecked(mpack_reader_t* reader, char* buf, size_t buffer_size, size_t byte_count) {
+    mpack_assert(buf != NULL, "destination for read of %i bytes is NULL", (int)byte_count);
+    mpack_assert(buffer_size >= 1, "buffer size is zero; you must have room for at least a null-terminator");
+
+    if (mpack_reader_error(reader)) {
+        buf[0] = 0;
+        return;
+    }
+
+    if (byte_count > buffer_size - 1) {
+        mpack_reader_flag_error(reader, mpack_error_too_big);
+        buf[0] = 0;
+        return;
+    }
+
+    mpack_reader_track_str_bytes_all(reader, byte_count);
+    mpack_read_native(reader, buf, byte_count);
+    buf[byte_count] = 0;
+}
+
+void mpack_read_cstr(mpack_reader_t* reader, char* buf, size_t buffer_size, size_t byte_count) {
+    mpack_read_cstr_unchecked(reader, buf, buffer_size, byte_count);
+
+    // check for null bytes
+    if (mpack_reader_error(reader) == mpack_ok && !mpack_str_check_no_null(buf, byte_count)) {
+        buf[0] = 0;
+        mpack_reader_flag_error(reader, mpack_error_type);
+    }
+}
+
+void mpack_read_utf8_cstr(mpack_reader_t* reader, char* buf, size_t buffer_size, size_t byte_count) {
+    mpack_read_cstr_unchecked(reader, buf, buffer_size, byte_count);
+
+    // check encoding
+    if (mpack_reader_error(reader) == mpack_ok && !mpack_utf8_check_no_null(buf, byte_count)) {
+        buf[0] = 0;
+        mpack_reader_flag_error(reader, mpack_error_type);
+    }
+}
+
+#ifdef MPACK_MALLOC
+// Reads native bytes with error callback disabled. This allows MPack reader functions
+// to hold an allocated buffer and read native data into it without leaking it in
+// case of a non-local jump (longjmp, throw) out of an error handler.
+static void mpack_read_native_noerrorfn(mpack_reader_t* reader, char* p, size_t count) {
+    mpack_assert(reader->error == mpack_ok, "cannot call if an error is already flagged!");
+    mpack_reader_error_t error_fn = reader->error_fn;
+    reader->error_fn = NULL;
+    mpack_read_native(reader, p, count);
+    reader->error_fn = error_fn;
+}
+
+char* mpack_read_bytes_alloc_impl(mpack_reader_t* reader, size_t count, bool null_terminated) {
+
+    // track the bytes first in case it jumps
+    mpack_reader_track_bytes(reader, count);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return NULL;
+
+    // cannot allocate zero bytes. this is not an error.
+    if (count == 0 && null_terminated == false)
+        return NULL;
+
+    // allocate data
+    char* data = (char*)MPACK_MALLOC(count + (null_terminated ? 1 : 0)); // TODO: can this overflow?
+    if (data == NULL) {
+        mpack_reader_flag_error(reader, mpack_error_memory);
+        return NULL;
+    }
+
+    // read with error callback disabled so we don't leak our buffer
+    mpack_read_native_noerrorfn(reader, data, count);
+
+    // report flagged errors
+    if (mpack_reader_error(reader) != mpack_ok) {
+        MPACK_FREE(data);
+        if (reader->error_fn)
+            reader->error_fn(reader, mpack_reader_error(reader));
+        return NULL;
+    }
+
+    if (null_terminated)
+        data[count] = '\0';
+    return data;
+}
+#endif
+
+// read inplace without tracking (since there are different
+// tracking modes for different inplace readers)
+static const char* mpack_read_bytes_inplace_notrack(mpack_reader_t* reader, size_t count) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return NULL;
+
+    // if we have enough bytes already in the buffer, we can return it directly.
+    if ((size_t)(reader->end - reader->data) >= count) {
+        const char* bytes = reader->data;
+        reader->data += count;
+        return bytes;
+    }
+
+    if (!mpack_reader_ensure(reader, count))
+        return NULL;
+
+    const char* bytes = reader->data;
+    reader->data += count;
+    return bytes;
+}
+
+const char* mpack_read_bytes_inplace(mpack_reader_t* reader, size_t count) {
+    mpack_reader_track_bytes(reader, count);
+    return mpack_read_bytes_inplace_notrack(reader, count);
+}
+
+const char* mpack_read_utf8_inplace(mpack_reader_t* reader, size_t count) {
+    mpack_reader_track_str_bytes_all(reader, count);
+    const char* str = mpack_read_bytes_inplace_notrack(reader, count);
+
+    if (mpack_reader_error(reader) == mpack_ok && !mpack_utf8_check(str, count)) {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return NULL;
+    }
+
+    return str;
+}
+
+static size_t mpack_parse_tag(mpack_reader_t* reader, mpack_tag_t* tag) {
+    mpack_assert(reader->error == mpack_ok, "reader cannot be in an error state!");
+
+    if (!mpack_reader_ensure(reader, 1))
+        return 0;
+    uint8_t type = mpack_load_u8(reader->data);
+
+    // unfortunately, by far the fastest way to parse a tag is to switch
+    // on the first byte, and to explicitly list every possible byte. so for
+    // infix types, the list of cases is quite large.
+    //
+    // in size-optimized builds, we switch on the top four bits first to
+    // handle most infix types with a smaller jump table to save space.
+
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    switch (type >> 4) {
+
+        // positive fixnum
+        case 0x0: case 0x1: case 0x2: case 0x3:
+        case 0x4: case 0x5: case 0x6: case 0x7:
+            *tag = mpack_tag_make_uint(type);
+            return 1;
+
+        // negative fixnum
+        case 0xe: case 0xf:
+            *tag = mpack_tag_make_int((int8_t)type);
+            return 1;
+
+        // fixmap
+        case 0x8:
+            *tag = mpack_tag_make_map(type & ~0xf0u);
+            return 1;
+
+        // fixarray
+        case 0x9:
+            *tag = mpack_tag_make_array(type & ~0xf0u);
+            return 1;
+
+        // fixstr
+        case 0xa: case 0xb:
+            *tag = mpack_tag_make_str(type & ~0xe0u);
+            return 1;
+
+        // not one of the common infix types
+        default:
+            break;
+
+    }
+    #endif
+
+    // handle individual type tags
+    switch (type) {
+
+        #if !MPACK_OPTIMIZE_FOR_SIZE
+        // positive fixnum
+        case 0x00: case 0x01: case 0x02: case 0x03: case 0x04: case 0x05: case 0x06: case 0x07:
+        case 0x08: case 0x09: case 0x0a: case 0x0b: case 0x0c: case 0x0d: case 0x0e: case 0x0f:
+        case 0x10: case 0x11: case 0x12: case 0x13: case 0x14: case 0x15: case 0x16: case 0x17:
+        case 0x18: case 0x19: case 0x1a: case 0x1b: case 0x1c: case 0x1d: case 0x1e: case 0x1f:
+        case 0x20: case 0x21: case 0x22: case 0x23: case 0x24: case 0x25: case 0x26: case 0x27:
+        case 0x28: case 0x29: case 0x2a: case 0x2b: case 0x2c: case 0x2d: case 0x2e: case 0x2f:
+        case 0x30: case 0x31: case 0x32: case 0x33: case 0x34: case 0x35: case 0x36: case 0x37:
+        case 0x38: case 0x39: case 0x3a: case 0x3b: case 0x3c: case 0x3d: case 0x3e: case 0x3f:
+        case 0x40: case 0x41: case 0x42: case 0x43: case 0x44: case 0x45: case 0x46: case 0x47:
+        case 0x48: case 0x49: case 0x4a: case 0x4b: case 0x4c: case 0x4d: case 0x4e: case 0x4f:
+        case 0x50: case 0x51: case 0x52: case 0x53: case 0x54: case 0x55: case 0x56: case 0x57:
+        case 0x58: case 0x59: case 0x5a: case 0x5b: case 0x5c: case 0x5d: case 0x5e: case 0x5f:
+        case 0x60: case 0x61: case 0x62: case 0x63: case 0x64: case 0x65: case 0x66: case 0x67:
+        case 0x68: case 0x69: case 0x6a: case 0x6b: case 0x6c: case 0x6d: case 0x6e: case 0x6f:
+        case 0x70: case 0x71: case 0x72: case 0x73: case 0x74: case 0x75: case 0x76: case 0x77:
+        case 0x78: case 0x79: case 0x7a: case 0x7b: case 0x7c: case 0x7d: case 0x7e: case 0x7f:
+            *tag = mpack_tag_make_uint(type);
+            return 1;
+
+        // negative fixnum
+        case 0xe0: case 0xe1: case 0xe2: case 0xe3: case 0xe4: case 0xe5: case 0xe6: case 0xe7:
+        case 0xe8: case 0xe9: case 0xea: case 0xeb: case 0xec: case 0xed: case 0xee: case 0xef:
+        case 0xf0: case 0xf1: case 0xf2: case 0xf3: case 0xf4: case 0xf5: case 0xf6: case 0xf7:
+        case 0xf8: case 0xf9: case 0xfa: case 0xfb: case 0xfc: case 0xfd: case 0xfe: case 0xff:
+            *tag = mpack_tag_make_int((int8_t)type);
+            return 1;
+
+        // fixmap
+        case 0x80: case 0x81: case 0x82: case 0x83: case 0x84: case 0x85: case 0x86: case 0x87:
+        case 0x88: case 0x89: case 0x8a: case 0x8b: case 0x8c: case 0x8d: case 0x8e: case 0x8f:
+            *tag = mpack_tag_make_map(type & ~0xf0u);
+            return 1;
+
+        // fixarray
+        case 0x90: case 0x91: case 0x92: case 0x93: case 0x94: case 0x95: case 0x96: case 0x97:
+        case 0x98: case 0x99: case 0x9a: case 0x9b: case 0x9c: case 0x9d: case 0x9e: case 0x9f:
+            *tag = mpack_tag_make_array(type & ~0xf0u);
+            return 1;
+
+        // fixstr
+        case 0xa0: case 0xa1: case 0xa2: case 0xa3: case 0xa4: case 0xa5: case 0xa6: case 0xa7:
+        case 0xa8: case 0xa9: case 0xaa: case 0xab: case 0xac: case 0xad: case 0xae: case 0xaf:
+        case 0xb0: case 0xb1: case 0xb2: case 0xb3: case 0xb4: case 0xb5: case 0xb6: case 0xb7:
+        case 0xb8: case 0xb9: case 0xba: case 0xbb: case 0xbc: case 0xbd: case 0xbe: case 0xbf:
+            *tag = mpack_tag_make_str(type & ~0xe0u);
+            return 1;
+        #endif
+
+        // nil
+        case 0xc0:
+            *tag = mpack_tag_make_nil();
+            return 1;
+
+        // bool
+        case 0xc2: case 0xc3:
+            *tag = mpack_tag_make_bool((bool)(type & 1));
+            return 1;
+
+        // bin8
+        case 0xc4:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_BIN8))
+                return 0;
+            *tag = mpack_tag_make_bin(mpack_load_u8(reader->data + 1));
+            return MPACK_TAG_SIZE_BIN8;
+
+        // bin16
+        case 0xc5:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_BIN16))
+                return 0;
+            *tag = mpack_tag_make_bin(mpack_load_u16(reader->data + 1));
+            return MPACK_TAG_SIZE_BIN16;
+
+        // bin32
+        case 0xc6:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_BIN32))
+                return 0;
+            *tag = mpack_tag_make_bin(mpack_load_u32(reader->data + 1));
+            return MPACK_TAG_SIZE_BIN32;
+
+        #if MPACK_EXTENSIONS
+        // ext8
+        case 0xc7:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_EXT8))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 2), mpack_load_u8(reader->data + 1));
+            return MPACK_TAG_SIZE_EXT8;
+
+        // ext16
+        case 0xc8:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_EXT16))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 3), mpack_load_u16(reader->data + 1));
+            return MPACK_TAG_SIZE_EXT16;
+
+        // ext32
+        case 0xc9:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_EXT32))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 5), mpack_load_u32(reader->data + 1));
+            return MPACK_TAG_SIZE_EXT32;
+        #endif
+
+        // float
+        case 0xca:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_FLOAT))
+                return 0;
+            #if MPACK_FLOAT
+            *tag = mpack_tag_make_float(mpack_load_float(reader->data + 1));
+            #else
+            *tag = mpack_tag_make_raw_float(mpack_load_u32(reader->data + 1));
+            #endif
+            return MPACK_TAG_SIZE_FLOAT;
+
+        // double
+        case 0xcb:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_DOUBLE))
+                return 0;
+            #if MPACK_DOUBLE
+            *tag = mpack_tag_make_double(mpack_load_double(reader->data + 1));
+            #else
+            *tag = mpack_tag_make_raw_double(mpack_load_u64(reader->data + 1));
+            #endif
+            return MPACK_TAG_SIZE_DOUBLE;
+
+        // uint8
+        case 0xcc:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_U8))
+                return 0;
+            *tag = mpack_tag_make_uint(mpack_load_u8(reader->data + 1));
+            return MPACK_TAG_SIZE_U8;
+
+        // uint16
+        case 0xcd:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_U16))
+                return 0;
+            *tag = mpack_tag_make_uint(mpack_load_u16(reader->data + 1));
+            return MPACK_TAG_SIZE_U16;
+
+        // uint32
+        case 0xce:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_U32))
+                return 0;
+            *tag = mpack_tag_make_uint(mpack_load_u32(reader->data + 1));
+            return MPACK_TAG_SIZE_U32;
+
+        // uint64
+        case 0xcf:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_U64))
+                return 0;
+            *tag = mpack_tag_make_uint(mpack_load_u64(reader->data + 1));
+            return MPACK_TAG_SIZE_U64;
+
+        // int8
+        case 0xd0:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_I8))
+                return 0;
+            *tag = mpack_tag_make_int(mpack_load_i8(reader->data + 1));
+            return MPACK_TAG_SIZE_I8;
+
+        // int16
+        case 0xd1:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_I16))
+                return 0;
+            *tag = mpack_tag_make_int(mpack_load_i16(reader->data + 1));
+            return MPACK_TAG_SIZE_I16;
+
+        // int32
+        case 0xd2:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_I32))
+                return 0;
+            *tag = mpack_tag_make_int(mpack_load_i32(reader->data + 1));
+            return MPACK_TAG_SIZE_I32;
+
+        // int64
+        case 0xd3:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_I64))
+                return 0;
+            *tag = mpack_tag_make_int(mpack_load_i64(reader->data + 1));
+            return MPACK_TAG_SIZE_I64;
+
+        #if MPACK_EXTENSIONS
+        // fixext1
+        case 0xd4:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_FIXEXT1))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 1), 1);
+            return MPACK_TAG_SIZE_FIXEXT1;
+
+        // fixext2
+        case 0xd5:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_FIXEXT2))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 1), 2);
+            return MPACK_TAG_SIZE_FIXEXT2;
+
+        // fixext4
+        case 0xd6:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_FIXEXT4))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 1), 4);
+            return 2;
+
+        // fixext8
+        case 0xd7:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_FIXEXT8))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 1), 8);
+            return MPACK_TAG_SIZE_FIXEXT8;
+
+        // fixext16
+        case 0xd8:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_FIXEXT16))
+                return 0;
+            *tag = mpack_tag_make_ext(mpack_load_i8(reader->data + 1), 16);
+            return MPACK_TAG_SIZE_FIXEXT16;
+        #endif
+
+        // str8
+        case 0xd9:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_STR8))
+                return 0;
+            *tag = mpack_tag_make_str(mpack_load_u8(reader->data + 1));
+            return MPACK_TAG_SIZE_STR8;
+
+        // str16
+        case 0xda:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_STR16))
+                return 0;
+            *tag = mpack_tag_make_str(mpack_load_u16(reader->data + 1));
+            return MPACK_TAG_SIZE_STR16;
+
+        // str32
+        case 0xdb:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_STR32))
+                return 0;
+            *tag = mpack_tag_make_str(mpack_load_u32(reader->data + 1));
+            return MPACK_TAG_SIZE_STR32;
+
+        // array16
+        case 0xdc:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_ARRAY16))
+                return 0;
+            *tag = mpack_tag_make_array(mpack_load_u16(reader->data + 1));
+            return MPACK_TAG_SIZE_ARRAY16;
+
+        // array32
+        case 0xdd:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_ARRAY32))
+                return 0;
+            *tag = mpack_tag_make_array(mpack_load_u32(reader->data + 1));
+            return MPACK_TAG_SIZE_ARRAY32;
+
+        // map16
+        case 0xde:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_MAP16))
+                return 0;
+            *tag = mpack_tag_make_map(mpack_load_u16(reader->data + 1));
+            return MPACK_TAG_SIZE_MAP16;
+
+        // map32
+        case 0xdf:
+            if (!mpack_reader_ensure(reader, MPACK_TAG_SIZE_MAP32))
+                return 0;
+            *tag = mpack_tag_make_map(mpack_load_u32(reader->data + 1));
+            return MPACK_TAG_SIZE_MAP32;
+
+        // reserved
+        case 0xc1:
+            mpack_reader_flag_error(reader, mpack_error_invalid);
+            return 0;
+
+        #if !MPACK_EXTENSIONS
+        // ext
+        case 0xc7: // fallthrough
+        case 0xc8: // fallthrough
+        case 0xc9: // fallthrough
+        // fixext
+        case 0xd4: // fallthrough
+        case 0xd5: // fallthrough
+        case 0xd6: // fallthrough
+        case 0xd7: // fallthrough
+        case 0xd8:
+            mpack_reader_flag_error(reader, mpack_error_unsupported);
+            return 0;
+        #endif
+
+        #if MPACK_OPTIMIZE_FOR_SIZE
+        // any other bytes should have been handled by the infix switch
+        default:
+            break;
+        #endif
+    }
+
+    mpack_assert(0, "unreachable");
+    return 0;
+}
+
+mpack_tag_t mpack_read_tag(mpack_reader_t* reader) {
+    mpack_log("reading tag\n");
+
+    // make sure we can read a tag
+    if (mpack_reader_error(reader) != mpack_ok)
+        return mpack_tag_nil();
+    if (mpack_reader_track_element(reader) != mpack_ok)
+        return mpack_tag_nil();
+
+    mpack_tag_t tag = MPACK_TAG_ZERO;
+    size_t count = mpack_parse_tag(reader, &tag);
+    if (count == 0)
+        return mpack_tag_nil();
+
+    #if MPACK_READ_TRACKING
+    mpack_error_t track_error = mpack_ok;
+
+    switch (tag.type) {
+        case mpack_type_map:
+        case mpack_type_array:
+            track_error = mpack_track_push(&reader->track, tag.type, tag.v.n);
+            break;
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+        #endif
+        case mpack_type_str:
+        case mpack_type_bin:
+            track_error = mpack_track_push(&reader->track, tag.type, tag.v.l);
+            break;
+        default:
+            break;
+    }
+
+    if (track_error != mpack_ok) {
+        mpack_reader_flag_error(reader, track_error);
+        return mpack_tag_nil();
+    }
+    #endif
+
+    reader->data += count;
+    return tag;
+}
+
+mpack_tag_t mpack_peek_tag(mpack_reader_t* reader) {
+    mpack_log("peeking tag\n");
+
+    // make sure we can peek a tag
+    if (mpack_reader_error(reader) != mpack_ok)
+        return mpack_tag_nil();
+    if (mpack_reader_track_peek_element(reader) != mpack_ok)
+        return mpack_tag_nil();
+
+    mpack_tag_t tag = MPACK_TAG_ZERO;
+    if (mpack_parse_tag(reader, &tag) == 0)
+        return mpack_tag_nil();
+    return tag;
+}
+
+void mpack_discard(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (mpack_reader_error(reader))
+        return;
+    switch (var.type) {
+        case mpack_type_str:
+            mpack_skip_bytes(reader, var.v.l);
+            mpack_done_str(reader);
+            break;
+        case mpack_type_bin:
+            mpack_skip_bytes(reader, var.v.l);
+            mpack_done_bin(reader);
+            break;
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            mpack_skip_bytes(reader, var.v.l);
+            mpack_done_ext(reader);
+            break;
+        #endif
+        case mpack_type_array: {
+            for (; var.v.n > 0; --var.v.n) {
+                mpack_discard(reader);
+                if (mpack_reader_error(reader))
+                    break;
+            }
+            mpack_done_array(reader);
+            break;
+        }
+        case mpack_type_map: {
+            for (; var.v.n > 0; --var.v.n) {
+                mpack_discard(reader);
+                mpack_discard(reader);
+                if (mpack_reader_error(reader))
+                    break;
+            }
+            mpack_done_map(reader);
+            break;
+        }
+        default:
+            break;
+    }
+}
+
+#if MPACK_EXTENSIONS
+mpack_timestamp_t mpack_read_timestamp(mpack_reader_t* reader, size_t size) {
+    mpack_timestamp_t timestamp = {0, 0};
+
+    if (size != 4 && size != 8 && size != 12) {
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        return timestamp;
+    }
+
+    char buf[12];
+    mpack_read_bytes(reader, buf, size);
+    mpack_done_ext(reader);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return timestamp;
+
+    switch (size) {
+        case 4:
+            timestamp.seconds = (int64_t)(uint64_t)mpack_load_u32(buf);
+            break;
+
+        case 8: {
+            uint64_t packed = mpack_load_u64(buf);
+            timestamp.seconds = (int64_t)(packed & ((MPACK_UINT64_C(1) << 34) - 1));
+            timestamp.nanoseconds = (uint32_t)(packed >> 34);
+            break;
+        }
+
+        case 12:
+            timestamp.nanoseconds = mpack_load_u32(buf);
+            timestamp.seconds = mpack_load_i64(buf + 4);
+            break;
+
+        default:
+            mpack_assert(false, "unreachable");
+            break;
+    }
+
+    if (timestamp.nanoseconds > MPACK_TIMESTAMP_NANOSECONDS_MAX) {
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        mpack_timestamp_t zero = {0, 0};
+        return zero;
+    }
+
+    return timestamp;
+}
+#endif
+
+#if MPACK_READ_TRACKING
+void mpack_done_type(mpack_reader_t* reader, mpack_type_t type) {
+    if (mpack_reader_error(reader) == mpack_ok)
+        mpack_reader_flag_if_error(reader, mpack_track_pop(&reader->track, type));
+}
+#endif
+
+#if MPACK_DEBUG && MPACK_STDIO
+static size_t mpack_print_read_prefix(mpack_reader_t* reader, size_t length, char* buffer, size_t buffer_size) {
+    if (length == 0)
+        return 0;
+
+    size_t read = (length < buffer_size) ? length : buffer_size;
+    mpack_read_bytes(reader, buffer, read);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return 0;
+
+    mpack_skip_bytes(reader, length - read);
+    return read;
+}
+
+static void mpack_print_element(mpack_reader_t* reader, mpack_print_t* print, size_t depth) {
+    mpack_tag_t val = mpack_read_tag(reader);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return;
+
+    // We read some bytes from bin and ext so we can print its prefix in hex.
+    char buffer[MPACK_PRINT_BYTE_COUNT];
+    size_t count = 0;
+    size_t i, j;
+
+    switch (val.type) {
+        case mpack_type_str:
+            mpack_print_append_cstr(print, "\"");
+            for (i = 0; i < val.v.l; ++i) {
+                char c;
+                mpack_read_bytes(reader, &c, 1);
+                if (mpack_reader_error(reader) != mpack_ok)
+                    return;
+                switch (c) {
+                    case '\n': mpack_print_append_cstr(print, "\\n"); break;
+                    case '\\': mpack_print_append_cstr(print, "\\\\"); break;
+                    case '"': mpack_print_append_cstr(print, "\\\""); break;
+                    default: mpack_print_append(print, &c, 1); break;
+                }
+            }
+            mpack_print_append_cstr(print, "\"");
+            mpack_done_str(reader);
+            return;
+
+        case mpack_type_array:
+            mpack_print_append_cstr(print, "[\n");
+            for (i = 0; i < val.v.n; ++i) {
+                for (j = 0; j < depth + 1; ++j)
+                    mpack_print_append_cstr(print, "    ");
+                mpack_print_element(reader, print, depth + 1);
+                if (mpack_reader_error(reader) != mpack_ok)
+                    return;
+                if (i != val.v.n - 1)
+                    mpack_print_append_cstr(print, ",");
+                mpack_print_append_cstr(print, "\n");
+            }
+            for (i = 0; i < depth; ++i)
+                mpack_print_append_cstr(print, "    ");
+            mpack_print_append_cstr(print, "]");
+            mpack_done_array(reader);
+            return;
+
+        case mpack_type_map:
+            mpack_print_append_cstr(print, "{\n");
+            for (i = 0; i < val.v.n; ++i) {
+                for (j = 0; j < depth + 1; ++j)
+                    mpack_print_append_cstr(print, "    ");
+                mpack_print_element(reader, print, depth + 1);
+                if (mpack_reader_error(reader) != mpack_ok)
+                    return;
+                mpack_print_append_cstr(print, ": ");
+                mpack_print_element(reader, print, depth + 1);
+                if (mpack_reader_error(reader) != mpack_ok)
+                    return;
+                if (i != val.v.n - 1)
+                    mpack_print_append_cstr(print, ",");
+                mpack_print_append_cstr(print, "\n");
+            }
+            for (i = 0; i < depth; ++i)
+                mpack_print_append_cstr(print, "    ");
+            mpack_print_append_cstr(print, "}");
+            mpack_done_map(reader);
+            return;
+
+        // The above cases return so as not to print a pseudo-json value. The
+        // below cases break and print pseudo-json.
+
+        case mpack_type_bin:
+            count = mpack_print_read_prefix(reader, mpack_tag_bin_length(&val), buffer, sizeof(buffer));
+            mpack_done_bin(reader);
+            break;
+
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            count = mpack_print_read_prefix(reader, mpack_tag_ext_length(&val), buffer, sizeof(buffer));
+            mpack_done_ext(reader);
+            break;
+        #endif
+
+        default:
+            break;
+    }
+
+    char buf[256];
+    mpack_tag_debug_pseudo_json(val, buf, sizeof(buf), buffer, count);
+    mpack_print_append_cstr(print, buf);
+}
+
+static void mpack_print_and_destroy(mpack_reader_t* reader, mpack_print_t* print, size_t depth) {
+    size_t i;
+    for (i = 0; i < depth; ++i)
+        mpack_print_append_cstr(print, "    ");
+    mpack_print_element(reader, print, depth);
+
+    size_t remaining = mpack_reader_remaining(reader, NULL);
+
+    char buf[256];
+    if (mpack_reader_destroy(reader) != mpack_ok) {
+        mpack_snprintf(buf, sizeof(buf), "\n<mpack parsing error %s>", mpack_error_to_string(mpack_reader_error(reader)));
+        buf[sizeof(buf) - 1] = '\0';
+        mpack_print_append_cstr(print, buf);
+    } else if (remaining > 0) {
+        mpack_snprintf(buf, sizeof(buf), "\n<%i extra bytes at end of message>", (int)remaining);
+        buf[sizeof(buf) - 1] = '\0';
+        mpack_print_append_cstr(print, buf);
+    }
+}
+
+static void mpack_print_data(const char* data, size_t len, mpack_print_t* print, size_t depth) {
+    mpack_reader_t reader;
+    mpack_reader_init_data(&reader, data, len);
+    mpack_print_and_destroy(&reader, print, depth);
+}
+
+void mpack_print_data_to_buffer(const char* data, size_t data_size, char* buffer, size_t buffer_size) {
+    if (buffer_size == 0) {
+        mpack_assert(false, "buffer size is zero!");
+        return;
+    }
+
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = buffer_size;
+    mpack_print_data(data, data_size, &print, 0);
+    mpack_print_append(&print, "",  1); // null-terminator
+    mpack_print_flush(&print);
+
+    // we always make sure there's a null-terminator at the end of the buffer
+    // in case we ran out of space.
+    print.buffer[print.size - 1] = '\0';
+}
+
+void mpack_print_data_to_callback(const char* data, size_t size, mpack_print_callback_t callback, void* context) {
+    char buffer[1024];
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = sizeof(buffer);
+    print.callback = callback;
+    print.context = context;
+    mpack_print_data(data, size, &print, 0);
+    mpack_print_flush(&print);
+}
+
+void mpack_print_data_to_file(const char* data, size_t len, FILE* file) {
+    mpack_assert(data != NULL, "data is NULL");
+    mpack_assert(file != NULL, "file is NULL");
+
+    char buffer[1024];
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = sizeof(buffer);
+    print.callback = &mpack_print_file_callback;
+    print.context = file;
+
+    mpack_print_data(data, len, &print, 2);
+    mpack_print_append_cstr(&print, "\n");
+    mpack_print_flush(&print);
+}
+
+void mpack_print_stdfile_to_callback(FILE* file, mpack_print_callback_t callback, void* context) {
+    char buffer[1024];
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = sizeof(buffer);
+    print.callback = callback;
+    print.context = context;
+
+    mpack_reader_t reader;
+    mpack_reader_init_stdfile(&reader, file, false);
+    mpack_print_and_destroy(&reader, &print, 0);
+    mpack_print_flush(&print);
+}
+#endif
+
+#endif
+
+MPACK_SILENCE_WARNINGS_END
+
+/* mpack/mpack-expect.c.c */
+
+#define MPACK_INTERNAL 1
+
+/* #include "mpack-expect.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+
+#if MPACK_EXPECT
+
+
+// Helpers
+
+MPACK_STATIC_INLINE uint8_t mpack_expect_native_u8(mpack_reader_t* reader) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return 0;
+    uint8_t type;
+    if (!mpack_reader_ensure(reader, sizeof(type)))
+        return 0;
+    type = mpack_load_u8(reader->data);
+    reader->data += sizeof(type);
+    return type;
+}
+
+#if !MPACK_OPTIMIZE_FOR_SIZE
+MPACK_STATIC_INLINE uint16_t mpack_expect_native_u16(mpack_reader_t* reader) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return 0;
+    uint16_t type;
+    if (!mpack_reader_ensure(reader, sizeof(type)))
+        return 0;
+    type = mpack_load_u16(reader->data);
+    reader->data += sizeof(type);
+    return type;
+}
+
+MPACK_STATIC_INLINE uint32_t mpack_expect_native_u32(mpack_reader_t* reader) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return 0;
+    uint32_t type;
+    if (!mpack_reader_ensure(reader, sizeof(type)))
+        return 0;
+    type = mpack_load_u32(reader->data);
+    reader->data += sizeof(type);
+    return type;
+}
+#endif
+
+MPACK_STATIC_INLINE uint8_t mpack_expect_type_byte(mpack_reader_t* reader) {
+    mpack_reader_track_element(reader);
+    return mpack_expect_native_u8(reader);
+}
+
+
+// Basic Number Functions
+
+uint8_t mpack_expect_u8(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_UINT8_MAX)
+            return (uint8_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= 0 && var.v.i <= MPACK_UINT8_MAX)
+            return (uint8_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+uint16_t mpack_expect_u16(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_UINT16_MAX)
+            return (uint16_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= 0 && var.v.i <= MPACK_UINT16_MAX)
+            return (uint16_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+uint32_t mpack_expect_u32(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_UINT32_MAX)
+            return (uint32_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= 0 && var.v.i <= MPACK_UINT32_MAX)
+            return (uint32_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+uint64_t mpack_expect_u64(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        return var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= 0)
+            return (uint64_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+int8_t mpack_expect_i8(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_INT8_MAX)
+            return (int8_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= MPACK_INT8_MIN && var.v.i <= MPACK_INT8_MAX)
+            return (int8_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+int16_t mpack_expect_i16(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_INT16_MAX)
+            return (int16_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= MPACK_INT16_MIN && var.v.i <= MPACK_INT16_MAX)
+            return (int16_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+int32_t mpack_expect_i32(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_INT32_MAX)
+            return (int32_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        if (var.v.i >= MPACK_INT32_MIN && var.v.i <= MPACK_INT32_MAX)
+            return (int32_t)var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+int64_t mpack_expect_i64(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint) {
+        if (var.v.u <= MPACK_INT64_MAX)
+            return (int64_t)var.v.u;
+    } else if (var.type == mpack_type_int) {
+        return var.v.i;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+#if MPACK_FLOAT
+float mpack_expect_float(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint)
+        return (float)var.v.u;
+    if (var.type == mpack_type_int)
+        return (float)var.v.i;
+    if (var.type == mpack_type_float)
+        return var.v.f;
+
+    if (var.type == mpack_type_double) {
+        #if MPACK_DOUBLE
+        return (float)var.v.d;
+        #else
+        return mpack_shorten_raw_double_to_float(var.v.d);
+        #endif
+    }
+
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0.0f;
+}
+#endif
+
+#if MPACK_DOUBLE
+double mpack_expect_double(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_uint)
+        return (double)var.v.u;
+    else if (var.type == mpack_type_int)
+        return (double)var.v.i;
+    else if (var.type == mpack_type_float)
+        return (double)var.v.f;
+    else if (var.type == mpack_type_double)
+        return var.v.d;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0.0;
+}
+#endif
+
+#if MPACK_FLOAT
+float mpack_expect_float_strict(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_float)
+        return var.v.f;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0.0f;
+}
+#endif
+
+#if MPACK_DOUBLE
+double mpack_expect_double_strict(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_float)
+        return (double)var.v.f;
+    else if (var.type == mpack_type_double)
+        return var.v.d;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0.0;
+}
+#endif
+
+#if !MPACK_FLOAT
+uint32_t mpack_expect_raw_float(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_float)
+        return var.v.f;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+#endif
+
+#if !MPACK_DOUBLE
+uint64_t mpack_expect_raw_double(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_double)
+        return var.v.d;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+#endif
+
+
+// Ranged Number Functions
+//
+// All ranged functions are identical other than the type, so we
+// define their content with a macro. The prototypes are still written
+// out in full to support ctags/IDE tools.
+
+#define MPACK_EXPECT_RANGE_IMPL(name, type_t)                           \
+                                                                        \
+    /* make sure the range is sensible */                               \
+    mpack_assert(min_value <= max_value,                                \
+            "min_value %i must be less than or equal to max_value %i",  \
+            min_value, max_value);                                      \
+                                                                        \
+    /* read the value */                                                \
+    type_t val = mpack_expect_##name(reader);                           \
+    if (mpack_reader_error(reader) != mpack_ok)                         \
+        return min_value;                                               \
+                                                                        \
+    /* make sure it fits */                                             \
+    if (val < min_value || val > max_value) {                           \
+        mpack_reader_flag_error(reader, mpack_error_type);              \
+        return min_value;                                               \
+    }                                                                   \
+                                                                        \
+    return val;
+
+uint8_t mpack_expect_u8_range(mpack_reader_t* reader, uint8_t min_value, uint8_t max_value) {MPACK_EXPECT_RANGE_IMPL(u8, uint8_t)}
+uint16_t mpack_expect_u16_range(mpack_reader_t* reader, uint16_t min_value, uint16_t max_value) {MPACK_EXPECT_RANGE_IMPL(u16, uint16_t)}
+uint32_t mpack_expect_u32_range(mpack_reader_t* reader, uint32_t min_value, uint32_t max_value) {MPACK_EXPECT_RANGE_IMPL(u32, uint32_t)}
+uint64_t mpack_expect_u64_range(mpack_reader_t* reader, uint64_t min_value, uint64_t max_value) {MPACK_EXPECT_RANGE_IMPL(u64, uint64_t)}
+
+int8_t mpack_expect_i8_range(mpack_reader_t* reader, int8_t min_value, int8_t max_value) {MPACK_EXPECT_RANGE_IMPL(i8, int8_t)}
+int16_t mpack_expect_i16_range(mpack_reader_t* reader, int16_t min_value, int16_t max_value) {MPACK_EXPECT_RANGE_IMPL(i16, int16_t)}
+int32_t mpack_expect_i32_range(mpack_reader_t* reader, int32_t min_value, int32_t max_value) {MPACK_EXPECT_RANGE_IMPL(i32, int32_t)}
+int64_t mpack_expect_i64_range(mpack_reader_t* reader, int64_t min_value, int64_t max_value) {MPACK_EXPECT_RANGE_IMPL(i64, int64_t)}
+
+#if MPACK_FLOAT
+float mpack_expect_float_range(mpack_reader_t* reader, float min_value, float max_value) {MPACK_EXPECT_RANGE_IMPL(float, float)}
+#endif
+#if MPACK_DOUBLE
+double mpack_expect_double_range(mpack_reader_t* reader, double min_value, double max_value) {MPACK_EXPECT_RANGE_IMPL(double, double)}
+#endif
+
+uint32_t mpack_expect_map_range(mpack_reader_t* reader, uint32_t min_value, uint32_t max_value) {MPACK_EXPECT_RANGE_IMPL(map, uint32_t)}
+uint32_t mpack_expect_array_range(mpack_reader_t* reader, uint32_t min_value, uint32_t max_value) {MPACK_EXPECT_RANGE_IMPL(array, uint32_t)}
+
+
+// Matching Number Functions
+
+void mpack_expect_uint_match(mpack_reader_t* reader, uint64_t value) {
+    if (mpack_expect_u64(reader) != value)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+void mpack_expect_int_match(mpack_reader_t* reader, int64_t value) {
+    if (mpack_expect_i64(reader) != value)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+
+// Other Basic Types
+
+void mpack_expect_nil(mpack_reader_t* reader) {
+    if (mpack_expect_type_byte(reader) != 0xc0)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+bool mpack_expect_bool(mpack_reader_t* reader) {
+    uint8_t type = mpack_expect_type_byte(reader);
+    if ((type & ~1) != 0xc2)
+        mpack_reader_flag_error(reader, mpack_error_type);
+    return (bool)(type & 1);
+}
+
+void mpack_expect_true(mpack_reader_t* reader) {
+    if (mpack_expect_bool(reader) != true)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+void mpack_expect_false(mpack_reader_t* reader) {
+    if (mpack_expect_bool(reader) != false)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+#if MPACK_EXTENSIONS
+mpack_timestamp_t mpack_expect_timestamp(mpack_reader_t* reader) {
+    mpack_timestamp_t zero = {0, 0};
+
+    mpack_tag_t tag = mpack_read_tag(reader);
+    if (tag.type != mpack_type_ext) {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return zero;
+    }
+    if (mpack_tag_ext_exttype(&tag) != MPACK_EXTTYPE_TIMESTAMP) {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return zero;
+    }
+
+    return mpack_read_timestamp(reader, mpack_tag_ext_length(&tag));
+}
+
+int64_t mpack_expect_timestamp_truncate(mpack_reader_t* reader) {
+    return mpack_expect_timestamp(reader).seconds;
+}
+#endif
+
+
+// Compound Types
+
+uint32_t mpack_expect_map(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_map)
+        return var.v.n;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+void mpack_expect_map_match(mpack_reader_t* reader, uint32_t count) {
+    if (mpack_expect_map(reader) != count)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+bool mpack_expect_map_or_nil(mpack_reader_t* reader, uint32_t* count) {
+    mpack_assert(count != NULL, "count cannot be NULL");
+
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_nil) {
+        *count = 0;
+        return false;
+    }
+    if (var.type == mpack_type_map) {
+        *count = var.v.n;
+        return true;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    *count = 0;
+    return false;
+}
+
+bool mpack_expect_map_max_or_nil(mpack_reader_t* reader, uint32_t max_count, uint32_t* count) {
+    mpack_assert(count != NULL, "count cannot be NULL");
+
+    bool has_map = mpack_expect_map_or_nil(reader, count);
+    if (has_map && *count > max_count) {
+        *count = 0;
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return false;
+    }
+    return has_map;
+}
+
+uint32_t mpack_expect_array(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_array)
+        return var.v.n;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+void mpack_expect_array_match(mpack_reader_t* reader, uint32_t count) {
+    if (mpack_expect_array(reader) != count)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+bool mpack_expect_array_or_nil(mpack_reader_t* reader, uint32_t* count) {
+    mpack_assert(count != NULL, "count cannot be NULL");
+
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_nil) {
+        *count = 0;
+        return false;
+    }
+    if (var.type == mpack_type_array) {
+        *count = var.v.n;
+        return true;
+    }
+    mpack_reader_flag_error(reader, mpack_error_type);
+    *count = 0;
+    return false;
+}
+
+bool mpack_expect_array_max_or_nil(mpack_reader_t* reader, uint32_t max_count, uint32_t* count) {
+    mpack_assert(count != NULL, "count cannot be NULL");
+
+    bool has_array = mpack_expect_array_or_nil(reader, count);
+    if (has_array && *count > max_count) {
+        *count = 0;
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return false;
+    }
+    return has_array;
+}
+
+#ifdef MPACK_MALLOC
+void* mpack_expect_array_alloc_impl(mpack_reader_t* reader, size_t element_size, uint32_t max_count, uint32_t* out_count, bool allow_nil) {
+    mpack_assert(out_count != NULL, "out_count cannot be NULL");
+    *out_count = 0;
+
+    uint32_t count;
+    bool has_array = true;
+    if (allow_nil)
+        has_array = mpack_expect_array_max_or_nil(reader, max_count, &count);
+    else
+        count = mpack_expect_array_max(reader, max_count);
+    if (mpack_reader_error(reader))
+        return NULL;
+
+    // size 0 is not an error; we return NULL for no elements.
+    if (count == 0) {
+        // we call mpack_done_array() automatically ONLY if we are using
+        // the _or_nil variant. this is the only way to allow nil and empty
+        // to work the same way.
+        if (allow_nil && has_array)
+            mpack_done_array(reader);
+        return NULL;
+    }
+
+    void* p = MPACK_MALLOC(element_size * count);
+    if (p == NULL) {
+        mpack_reader_flag_error(reader, mpack_error_memory);
+        return NULL;
+    }
+
+    *out_count = count;
+    return p;
+}
+#endif
+
+
+// Str, Bin and Ext Functions
+
+uint32_t mpack_expect_str(mpack_reader_t* reader) {
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_str)
+        return var.v.l;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+    #else
+    uint8_t type = mpack_expect_type_byte(reader);
+    uint32_t count;
+
+    if ((type >> 5) == 5) {
+        count = type & (uint8_t)~0xe0;
+    } else if (type == 0xd9) {
+        count = mpack_expect_native_u8(reader);
+    } else if (type == 0xda) {
+        count = mpack_expect_native_u16(reader);
+    } else if (type == 0xdb) {
+        count = mpack_expect_native_u32(reader);
+    } else {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return 0;
+    }
+
+    #if MPACK_READ_TRACKING
+    mpack_reader_flag_if_error(reader, mpack_track_push(&reader->track, mpack_type_str, count));
+    #endif
+    return count;
+    #endif
+}
+
+size_t mpack_expect_str_buf(mpack_reader_t* reader, char* buf, size_t bufsize) {
+    mpack_assert(buf != NULL, "buf cannot be NULL");
+
+    size_t length = mpack_expect_str(reader);
+    if (mpack_reader_error(reader))
+        return 0;
+
+    if (length > bufsize) {
+        mpack_reader_flag_error(reader, mpack_error_too_big);
+        return 0;
+    }
+
+    mpack_read_bytes(reader, buf, length);
+    if (mpack_reader_error(reader))
+        return 0;
+
+    mpack_done_str(reader);
+    return length;
+}
+
+size_t mpack_expect_utf8(mpack_reader_t* reader, char* buf, size_t size) {
+    mpack_assert(buf != NULL, "buf cannot be NULL");
+
+    size_t length = mpack_expect_str_buf(reader, buf, size);
+
+    if (!mpack_utf8_check(buf, length)) {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return 0;
+    }
+
+    return length;
+}
+
+uint32_t mpack_expect_bin(mpack_reader_t* reader) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_bin)
+        return var.v.l;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+size_t mpack_expect_bin_buf(mpack_reader_t* reader, char* buf, size_t bufsize) {
+    mpack_assert(buf != NULL, "buf cannot be NULL");
+
+    size_t binsize = mpack_expect_bin(reader);
+    if (mpack_reader_error(reader))
+        return 0;
+    if (binsize > bufsize) {
+        mpack_reader_flag_error(reader, mpack_error_too_big);
+        return 0;
+    }
+    mpack_read_bytes(reader, buf, binsize);
+    if (mpack_reader_error(reader))
+        return 0;
+    mpack_done_bin(reader);
+    return binsize;
+}
+
+void mpack_expect_bin_size_buf(mpack_reader_t* reader, char* buf, uint32_t size) {
+    mpack_assert(buf != NULL, "buf cannot be NULL");
+    mpack_expect_bin_size(reader, size);
+    mpack_read_bytes(reader, buf, size);
+    mpack_done_bin(reader);
+}
+
+#if MPACK_EXTENSIONS
+uint32_t mpack_expect_ext(mpack_reader_t* reader, int8_t* type) {
+    mpack_tag_t var = mpack_read_tag(reader);
+    if (var.type == mpack_type_ext) {
+        *type = mpack_tag_ext_exttype(&var);
+        return mpack_tag_ext_length(&var);
+    }
+    *type = 0;
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return 0;
+}
+
+size_t mpack_expect_ext_buf(mpack_reader_t* reader, int8_t* type, char* buf, size_t bufsize) {
+    mpack_assert(buf != NULL, "buf cannot be NULL");
+
+    size_t extsize = mpack_expect_ext(reader, type);
+    if (mpack_reader_error(reader))
+        return 0;
+    if (extsize > bufsize) {
+        *type = 0;
+        mpack_reader_flag_error(reader, mpack_error_too_big);
+        return 0;
+    }
+    mpack_read_bytes(reader, buf, extsize);
+    if (mpack_reader_error(reader)) {
+        *type = 0;
+        return 0;
+    }
+    mpack_done_ext(reader);
+    return extsize;
+}
+#endif
+
+void mpack_expect_cstr(mpack_reader_t* reader, char* buf, size_t bufsize) {
+    uint32_t length = mpack_expect_str(reader);
+    mpack_read_cstr(reader, buf, bufsize, length);
+    mpack_done_str(reader);
+}
+
+void mpack_expect_utf8_cstr(mpack_reader_t* reader, char* buf, size_t bufsize) {
+    uint32_t length = mpack_expect_str(reader);
+    mpack_read_utf8_cstr(reader, buf, bufsize, length);
+    mpack_done_str(reader);
+}
+
+#ifdef MPACK_MALLOC
+static char* mpack_expect_cstr_alloc_unchecked(mpack_reader_t* reader, size_t maxsize, size_t* out_length) {
+    mpack_assert(out_length != NULL, "out_length cannot be NULL");
+    *out_length = 0;
+
+    // make sure argument makes sense
+    if (maxsize < 1) {
+        mpack_break("maxsize is zero; you must have room for at least a null-terminator");
+        mpack_reader_flag_error(reader, mpack_error_bug);
+        return NULL;
+    }
+
+    if (SIZE_MAX < MPACK_UINT32_MAX) {
+        if (maxsize > SIZE_MAX)
+            maxsize = SIZE_MAX;
+    } else {
+        if (maxsize > (size_t)MPACK_UINT32_MAX)
+            maxsize = (size_t)MPACK_UINT32_MAX;
+    }
+
+    size_t length = mpack_expect_str_max(reader, (uint32_t)maxsize - 1);
+    char* str = mpack_read_bytes_alloc_impl(reader, length, true);
+    mpack_done_str(reader);
+
+    if (str)
+        *out_length = length;
+    return str;
+}
+
+char* mpack_expect_cstr_alloc(mpack_reader_t* reader, size_t maxsize) {
+    size_t length;
+    char* str = mpack_expect_cstr_alloc_unchecked(reader, maxsize, &length);
+
+    if (str && !mpack_str_check_no_null(str, length)) {
+        MPACK_FREE(str);
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return NULL;
+    }
+
+    return str;
+}
+
+char* mpack_expect_utf8_cstr_alloc(mpack_reader_t* reader, size_t maxsize) {
+    size_t length;
+    char* str = mpack_expect_cstr_alloc_unchecked(reader, maxsize, &length);
+
+    if (str && !mpack_utf8_check_no_null(str, length)) {
+        MPACK_FREE(str);
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return NULL;
+    }
+
+    return str;
+}
+#endif
+
+void mpack_expect_str_match(mpack_reader_t* reader, const char* str, size_t len) {
+    mpack_assert(str != NULL, "str cannot be NULL");
+
+    // expect a str the correct length
+    if (len > MPACK_UINT32_MAX)
+        mpack_reader_flag_error(reader, mpack_error_type);
+    mpack_expect_str_length(reader, (uint32_t)len);
+    if (mpack_reader_error(reader))
+        return;
+    mpack_reader_track_bytes(reader, (uint32_t)len);
+
+    // check each byte one by one (matched strings are likely to be very small)
+    for (; len > 0; --len) {
+        if (mpack_expect_native_u8(reader) != *str++) {
+            mpack_reader_flag_error(reader, mpack_error_type);
+            return;
+        }
+    }
+
+    mpack_done_str(reader);
+}
+
+void mpack_expect_tag(mpack_reader_t* reader, mpack_tag_t expected) {
+    mpack_tag_t actual = mpack_read_tag(reader);
+    if (!mpack_tag_equal(actual, expected))
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+#ifdef MPACK_MALLOC
+char* mpack_expect_bin_alloc(mpack_reader_t* reader, size_t maxsize, size_t* size) {
+    mpack_assert(size != NULL, "size cannot be NULL");
+    *size = 0;
+
+    if (SIZE_MAX < MPACK_UINT32_MAX) {
+        if (maxsize > SIZE_MAX)
+            maxsize = SIZE_MAX;
+    } else {
+        if (maxsize > (size_t)MPACK_UINT32_MAX)
+            maxsize = (size_t)MPACK_UINT32_MAX;
+    }
+
+    size_t length = mpack_expect_bin_max(reader, (uint32_t)maxsize);
+    if (mpack_reader_error(reader))
+        return NULL;
+
+    char* data = mpack_read_bytes_alloc(reader, length);
+    mpack_done_bin(reader);
+
+    if (data)
+        *size = length;
+    return data;
+}
+#endif
+
+#if MPACK_EXTENSIONS && defined(MPACK_MALLOC)
+char* mpack_expect_ext_alloc(mpack_reader_t* reader, int8_t* type, size_t maxsize, size_t* size) {
+    mpack_assert(size != NULL, "size cannot be NULL");
+    *size = 0;
+
+    if (SIZE_MAX < MPACK_UINT32_MAX) {
+        if (maxsize > SIZE_MAX)
+            maxsize = SIZE_MAX;
+    } else {
+        if (maxsize > (size_t)MPACK_UINT32_MAX)
+            maxsize = (size_t)MPACK_UINT32_MAX;
+    }
+
+    size_t length = mpack_expect_ext_max(reader, type, (uint32_t)maxsize);
+    if (mpack_reader_error(reader))
+        return NULL;
+
+    char* data = mpack_read_bytes_alloc(reader, length);
+    mpack_done_ext(reader);
+
+    if (data) {
+        *size = length;
+    } else {
+        *type = 0;
+    }
+    return data;
+}
+#endif
+
+size_t mpack_expect_enum(mpack_reader_t* reader, const char* strings[], size_t count) {
+
+    // read the string in-place
+    size_t keylen = mpack_expect_str(reader);
+    const char* key = mpack_read_bytes_inplace(reader, keylen);
+    mpack_done_str(reader);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return count;
+
+    // find what key it matches
+    size_t i;
+    for (i = 0; i < count; ++i) {
+        const char* other = strings[i];
+        size_t otherlen = mpack_strlen(other);
+        if (keylen == otherlen && mpack_memcmp(key, other, keylen) == 0)
+            return i;
+    }
+
+    // no matches
+    mpack_reader_flag_error(reader, mpack_error_type);
+    return count;
+}
+
+size_t mpack_expect_enum_optional(mpack_reader_t* reader, const char* strings[], size_t count) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return count;
+
+    mpack_assert(count != 0, "count cannot be zero; no strings are valid!");
+    mpack_assert(strings != NULL, "strings cannot be NULL");
+
+    // the key is only recognized if it is a string
+    if (mpack_peek_tag(reader).type != mpack_type_str) {
+        mpack_discard(reader);
+        return count;
+    }
+
+    // read the string in-place
+    size_t keylen = mpack_expect_str(reader);
+    const char* key = mpack_read_bytes_inplace(reader, keylen);
+    mpack_done_str(reader);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return count;
+
+    // find what key it matches
+    size_t i;
+    for (i = 0; i < count; ++i) {
+        const char* other = strings[i];
+        size_t otherlen = mpack_strlen(other);
+        if (keylen == otherlen && mpack_memcmp(key, other, keylen) == 0)
+            return i;
+    }
+
+    // no matches
+    return count;
+}
+
+size_t mpack_expect_key_uint(mpack_reader_t* reader, bool found[], size_t count) {
+    if (mpack_reader_error(reader) != mpack_ok)
+        return count;
+
+    if (count == 0) {
+        mpack_break("count cannot be zero; no keys are valid!");
+        mpack_reader_flag_error(reader, mpack_error_bug);
+        return count;
+    }
+    mpack_assert(found != NULL, "found cannot be NULL");
+
+    // the key is only recognized if it is an unsigned int
+    if (mpack_peek_tag(reader).type != mpack_type_uint) {
+        mpack_discard(reader);
+        return count;
+    }
+
+    // read the key
+    uint64_t value = mpack_expect_u64(reader);
+    if (mpack_reader_error(reader) != mpack_ok)
+        return count;
+
+    // unrecognized keys are fine, we just return count
+    if (value >= count)
+        return count;
+
+    // check if this key is a duplicate
+    if (found[value]) {
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        return count;
+    }
+
+    found[value] = true;
+    return (size_t)value;
+}
+
+size_t mpack_expect_key_cstr(mpack_reader_t* reader, const char* keys[], bool found[], size_t count) {
+    size_t i = mpack_expect_enum_optional(reader, keys, count);
+
+    // unrecognized keys are fine, we just return count
+    if (i == count)
+        return count;
+
+    // check if this key is a duplicate
+    mpack_assert(found != NULL, "found cannot be NULL");
+    if (found[i]) {
+        mpack_reader_flag_error(reader, mpack_error_invalid);
+        return count;
+    }
+
+    found[i] = true;
+    return i;
+}
+
+#endif
+
+MPACK_SILENCE_WARNINGS_END
+
+/* mpack/mpack-node.c.c */
+
+#define MPACK_INTERNAL 1
+
+/* #include "mpack-node.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+
+#if MPACK_NODE
+
+MPACK_STATIC_INLINE const char* mpack_node_data_unchecked(mpack_node_t node) {
+    mpack_assert(mpack_node_error(node) == mpack_ok, "tree is in an error state!");
+
+    mpack_type_t type = node.data->type;
+    MPACK_UNUSED(type);
+    #if MPACK_EXTENSIONS
+    mpack_assert(type == mpack_type_str || type == mpack_type_bin || type == mpack_type_ext,
+            "node of type %i (%s) is not a data type!", type, mpack_type_to_string(type));
+    #else
+    mpack_assert(type == mpack_type_str || type == mpack_type_bin,
+            "node of type %i (%s) is not a data type!", type, mpack_type_to_string(type));
+    #endif
+
+    return node.tree->data + node.data->value.offset;
+}
+
+#if MPACK_EXTENSIONS
+MPACK_STATIC_INLINE int8_t mpack_node_exttype_unchecked(mpack_node_t node) {
+    mpack_assert(mpack_node_error(node) == mpack_ok, "tree is in an error state!");
+
+    mpack_type_t type = node.data->type;
+    MPACK_UNUSED(type);
+    mpack_assert(type == mpack_type_ext, "node of type %i (%s) is not an ext type!",
+            type, mpack_type_to_string(type));
+
+    // the exttype of an ext node is stored in the byte preceding the data
+    return mpack_load_i8(mpack_node_data_unchecked(node) - 1);
+}
+#endif
+
+
+
+/*
+ * Tree Parsing
+ */
+
+#ifdef MPACK_MALLOC
+
+// fix up the alloc size to make sure it exactly fits the
+// maximum number of nodes it can contain (the allocator will
+// waste it back anyway, but we round it down just in case)
+
+#define MPACK_NODES_PER_PAGE \
+    ((MPACK_NODE_PAGE_SIZE - sizeof(mpack_tree_page_t)) / sizeof(mpack_node_data_t) + 1)
+
+#define MPACK_PAGE_ALLOC_SIZE \
+    (sizeof(mpack_tree_page_t) + sizeof(mpack_node_data_t) * (MPACK_NODES_PER_PAGE - 1))
+
+#endif
+
+#ifdef MPACK_MALLOC
+/*
+ * Fills the tree until we have at least enough bytes for the current node.
+ */
+static bool mpack_tree_reserve_fill(mpack_tree_t* tree) {
+    mpack_assert(tree->parser.state == mpack_tree_parse_state_in_progress);
+
+    size_t bytes = tree->parser.current_node_reserved;
+    mpack_assert(bytes > tree->parser.possible_nodes_left,
+            "there are already enough bytes! call mpack_tree_ensure() instead.");
+    mpack_log("filling to reserve %i bytes\n", (int)bytes);
+
+    // if the necessary bytes would put us over the maximum tree
+    // size, fail right away.
+    // TODO: check for overflow?
+    if (tree->data_length + bytes > tree->max_size) {
+        mpack_tree_flag_error(tree, mpack_error_too_big);
+        return false;
+    }
+
+    // we'll need a read function to fetch more data. if there's
+    // no read function, the data should contain an entire message
+    // (or messages), so we flag it as invalid.
+    if (tree->read_fn == NULL) {
+        mpack_log("tree has no read function!\n");
+        mpack_tree_flag_error(tree, mpack_error_invalid);
+        return false;
+    }
+
+    // expand the buffer if needed
+    if (tree->data_length + bytes > tree->buffer_capacity) {
+
+        // TODO: check for overflow?
+        size_t new_capacity = (tree->buffer_capacity == 0) ? MPACK_BUFFER_SIZE : tree->buffer_capacity;
+        while (new_capacity < tree->data_length + bytes)
+            new_capacity *= 2;
+        if (new_capacity > tree->max_size)
+            new_capacity = tree->max_size;
+
+        mpack_log("expanding buffer from %i to %i\n", (int)tree->buffer_capacity, (int)new_capacity);
+
+        char* new_buffer;
+        if (tree->buffer == NULL)
+            new_buffer = (char*)MPACK_MALLOC(new_capacity);
+        else
+            new_buffer = (char*)mpack_realloc(tree->buffer, tree->data_length, new_capacity);
+
+        if (new_buffer == NULL) {
+            mpack_tree_flag_error(tree, mpack_error_memory);
+            return false;
+        }
+
+        tree->data = new_buffer;
+        tree->buffer = new_buffer;
+        tree->buffer_capacity = new_capacity;
+    }
+
+    // request as much data as possible, looping until we have
+    // all the data we need
+    do {
+        size_t read = tree->read_fn(tree, tree->buffer + tree->data_length, tree->buffer_capacity - tree->data_length);
+
+        // If the fill function encounters an error, it should flag an error on
+        // the tree.
+        if (mpack_tree_error(tree) != mpack_ok)
+            return false;
+
+        // We guard against fill functions that return -1 just in case.
+        if (read == (size_t)(-1)) {
+            mpack_tree_flag_error(tree, mpack_error_io);
+            return false;
+        }
+
+        // If the fill function returns 0, the data is not available yet. We
+        // return false to stop parsing the current node.
+        if (read == 0) {
+            mpack_log("not enough data.\n");
+            return false;
+        }
+
+        mpack_log("read %u more bytes\n", (uint32_t)read);
+        tree->data_length += read;
+        tree->parser.possible_nodes_left += read;
+    } while (tree->parser.possible_nodes_left < bytes);
+
+    return true;
+}
+#endif
+
+/*
+ * Ensures there are enough additional bytes in the tree for the current node
+ * (including reserved bytes for the children of this node, and in addition to
+ * the reserved bytes for children of previous compound nodes), reading more
+ * data if needed.
+ *
+ * extra_bytes is the number of additional bytes to reserve for the current
+ * node beyond the type byte (since one byte is already reserved for each node
+ * by its parent array or map.)
+ *
+ * This may reallocate the tree, which means the tree->data pointer may change!
+ *
+ * Returns false if not enough bytes could be read.
+ */
+MPACK_STATIC_INLINE bool mpack_tree_reserve_bytes(mpack_tree_t* tree, size_t extra_bytes) {
+    mpack_assert(tree->parser.state == mpack_tree_parse_state_in_progress);
+
+    // We guard against overflow here. A compound type could declare more than
+    // MPACK_UINT32_MAX contents which overflows SIZE_MAX on 32-bit platforms. We
+    // flag mpack_error_invalid instead of mpack_error_too_big since it's far
+    // more likely that the message is corrupt than that the data is valid but
+    // not parseable on this architecture (see test_read_node_possible() in
+    // test-node.c .)
+    if ((uint64_t)tree->parser.current_node_reserved + (uint64_t)extra_bytes > SIZE_MAX) {
+        mpack_tree_flag_error(tree, mpack_error_invalid);
+        return false;
+    }
+
+    tree->parser.current_node_reserved += extra_bytes;
+
+    // Note that possible_nodes_left already accounts for reserved bytes for
+    // children of previous compound nodes. So even if there are hundreds of
+    // bytes left in the buffer, we might need to read anyway.
+    if (tree->parser.current_node_reserved <= tree->parser.possible_nodes_left)
+        return true;
+
+    #ifdef MPACK_MALLOC
+    return mpack_tree_reserve_fill(tree);
+    #else
+    return false;
+    #endif
+}
+
+MPACK_STATIC_INLINE size_t mpack_tree_parser_stack_capacity(mpack_tree_t* tree) {
+    #ifdef MPACK_MALLOC
+    return tree->parser.stack_capacity;
+    #else
+    return sizeof(tree->parser.stack) / sizeof(tree->parser.stack[0]);
+    #endif
+}
+
+static bool mpack_tree_push_stack(mpack_tree_t* tree, mpack_node_data_t* first_child, size_t total) {
+    mpack_tree_parser_t* parser = &tree->parser;
+    mpack_assert(parser->state == mpack_tree_parse_state_in_progress);
+
+    // No need to push empty containers
+    if (total == 0)
+        return true;
+
+    // Make sure we have enough room in the stack
+    if (parser->level + 1 == mpack_tree_parser_stack_capacity(tree)) {
+        #ifdef MPACK_MALLOC
+        size_t new_capacity = parser->stack_capacity * 2;
+        mpack_log("growing parse stack to capacity %i\n", (int)new_capacity);
+
+        // Replace the stack-allocated parsing stack
+        if (!parser->stack_owned) {
+            mpack_level_t* new_stack = (mpack_level_t*)MPACK_MALLOC(sizeof(mpack_level_t) * new_capacity);
+            if (!new_stack) {
+                mpack_tree_flag_error(tree, mpack_error_memory);
+                return false;
+            }
+            mpack_memcpy(new_stack, parser->stack, sizeof(mpack_level_t) * parser->stack_capacity);
+            parser->stack = new_stack;
+            parser->stack_owned = true;
+
+        // Realloc the allocated parsing stack
+        } else {
+            mpack_level_t* new_stack = (mpack_level_t*)mpack_realloc(parser->stack,
+                    sizeof(mpack_level_t) * parser->stack_capacity, sizeof(mpack_level_t) * new_capacity);
+            if (!new_stack) {
+                mpack_tree_flag_error(tree, mpack_error_memory);
+                return false;
+            }
+            parser->stack = new_stack;
+        }
+        parser->stack_capacity = new_capacity;
+        #else
+        mpack_tree_flag_error(tree, mpack_error_too_big);
+        return false;
+        #endif
+    }
+
+    // Push the contents of this node onto the parsing stack
+    ++parser->level;
+    parser->stack[parser->level].child = first_child;
+    parser->stack[parser->level].left = total;
+    return true;
+}
+
+static bool mpack_tree_parse_children(mpack_tree_t* tree, mpack_node_data_t* node) {
+    mpack_tree_parser_t* parser = &tree->parser;
+    mpack_assert(parser->state == mpack_tree_parse_state_in_progress);
+
+    mpack_type_t type = node->type;
+    size_t total = node->len;
+
+    // Calculate total elements to read
+    if (type == mpack_type_map) {
+        if ((uint64_t)total * 2 > SIZE_MAX) {
+            mpack_tree_flag_error(tree, mpack_error_too_big);
+            return false;
+        }
+        total *= 2;
+    }
+
+    // Make sure we are under our total node limit (TODO can this overflow?)
+    tree->node_count += total;
+    if (tree->node_count > tree->max_nodes) {
+        mpack_tree_flag_error(tree, mpack_error_too_big);
+        return false;
+    }
+
+    // Each node is at least one byte. Count these bytes now to make
+    // sure there is enough data left.
+    if (!mpack_tree_reserve_bytes(tree, total))
+        return false;
+
+    // If there are enough nodes left in the current page, no need to grow
+    if (total <= parser->nodes_left) {
+        node->value.children = parser->nodes;
+        parser->nodes += total;
+        parser->nodes_left -= total;
+
+    } else {
+
+        #ifdef MPACK_MALLOC
+
+        // We can't grow if we're using a fixed pool (i.e. we didn't start with a page)
+        if (!tree->next) {
+            mpack_tree_flag_error(tree, mpack_error_too_big);
+            return false;
+        }
+
+        // Otherwise we need to grow, and the node's children need to be contiguous.
+        // This is a heuristic to decide whether we should waste the remaining space
+        // in the current page and start a new one, or give the children their
+        // own page. With a fraction of 1/8, this causes at most 12% additional
+        // waste. Note that reducing this too much causes less cache coherence and
+        // more malloc() overhead due to smaller allocations, so there's a tradeoff
+        // here. This heuristic could use some improvement, especially with custom
+        // page sizes.
+
+        mpack_tree_page_t* page;
+
+        if (total > MPACK_NODES_PER_PAGE || parser->nodes_left > MPACK_NODES_PER_PAGE / 8) {
+            // TODO: this should check for overflow
+            page = (mpack_tree_page_t*)MPACK_MALLOC(
+                    sizeof(mpack_tree_page_t) + sizeof(mpack_node_data_t) * (total - 1));
+            if (page == NULL) {
+                mpack_tree_flag_error(tree, mpack_error_memory);
+                return false;
+            }
+            mpack_log("allocated seperate page %p for %i children, %i left in page of %i total\n",
+                    (void*)page, (int)total, (int)parser->nodes_left, (int)MPACK_NODES_PER_PAGE);
+
+            node->value.children = page->nodes;
+
+        } else {
+            page = (mpack_tree_page_t*)MPACK_MALLOC(MPACK_PAGE_ALLOC_SIZE);
+            if (page == NULL) {
+                mpack_tree_flag_error(tree, mpack_error_memory);
+                return false;
+            }
+            mpack_log("allocated new page %p for %i children, wasting %i in page of %i total\n",
+                    (void*)page, (int)total, (int)parser->nodes_left, (int)MPACK_NODES_PER_PAGE);
+
+            node->value.children = page->nodes;
+            parser->nodes = page->nodes + total;
+            parser->nodes_left = MPACK_NODES_PER_PAGE - total;
+        }
+
+        page->next = tree->next;
+        tree->next = page;
+
+        #else
+        // We can't grow if we don't have an allocator
+        mpack_tree_flag_error(tree, mpack_error_too_big);
+        return false;
+        #endif
+    }
+
+    return mpack_tree_push_stack(tree, node->value.children, total);
+}
+
+static bool mpack_tree_parse_bytes(mpack_tree_t* tree, mpack_node_data_t* node) {
+    node->value.offset = tree->size + tree->parser.current_node_reserved + 1;
+    return mpack_tree_reserve_bytes(tree, node->len);
+}
+
+#if MPACK_EXTENSIONS
+static bool mpack_tree_parse_ext(mpack_tree_t* tree, mpack_node_data_t* node) {
+    // reserve space for exttype
+    tree->parser.current_node_reserved += sizeof(int8_t);
+    node->type = mpack_type_ext;
+    return mpack_tree_parse_bytes(tree, node);
+}
+#endif
+
+static bool mpack_tree_parse_node_contents(mpack_tree_t* tree, mpack_node_data_t* node) {
+    mpack_assert(tree->parser.state == mpack_tree_parse_state_in_progress);
+    mpack_assert(node != NULL, "null node?");
+
+    // read the type. we've already accounted for this byte in
+    // possible_nodes_left, so we already know it is in bounds, and we don't
+    // need to reserve it for this node.
+    mpack_assert(tree->data_length > tree->size);
+    uint8_t type = mpack_load_u8(tree->data + tree->size);
+    mpack_log("node type %x\n", type);
+    tree->parser.current_node_reserved = 0;
+
+    // as with mpack_read_tag(), the fastest way to parse a node is to switch
+    // on the first byte, and to explicitly list every possible byte. we switch
+    // on the first four bits in size-optimized builds.
+
+    #if MPACK_OPTIMIZE_FOR_SIZE
+    switch (type >> 4) {
+
+        // positive fixnum
+        case 0x0: case 0x1: case 0x2: case 0x3:
+        case 0x4: case 0x5: case 0x6: case 0x7:
+            node->type = mpack_type_uint;
+            node->value.u = type;
+            return true;
+
+        // negative fixnum
+        case 0xe: case 0xf:
+            node->type = mpack_type_int;
+            node->value.i = (int8_t)type;
+            return true;
+
+        // fixmap
+        case 0x8:
+            node->type = mpack_type_map;
+            node->len = (uint32_t)(type & ~0xf0);
+            return mpack_tree_parse_children(tree, node);
+
+        // fixarray
+        case 0x9:
+            node->type = mpack_type_array;
+            node->len = (uint32_t)(type & ~0xf0);
+            return mpack_tree_parse_children(tree, node);
+
+        // fixstr
+        case 0xa: case 0xb:
+            node->type = mpack_type_str;
+            node->len = (uint32_t)(type & ~0xe0);
+            return mpack_tree_parse_bytes(tree, node);
+
+        // not one of the common infix types
+        default:
+            break;
+    }
+    #endif
+
+    switch (type) {
+
+        #if !MPACK_OPTIMIZE_FOR_SIZE
+        // positive fixnum
+        case 0x00: case 0x01: case 0x02: case 0x03: case 0x04: case 0x05: case 0x06: case 0x07:
+        case 0x08: case 0x09: case 0x0a: case 0x0b: case 0x0c: case 0x0d: case 0x0e: case 0x0f:
+        case 0x10: case 0x11: case 0x12: case 0x13: case 0x14: case 0x15: case 0x16: case 0x17:
+        case 0x18: case 0x19: case 0x1a: case 0x1b: case 0x1c: case 0x1d: case 0x1e: case 0x1f:
+        case 0x20: case 0x21: case 0x22: case 0x23: case 0x24: case 0x25: case 0x26: case 0x27:
+        case 0x28: case 0x29: case 0x2a: case 0x2b: case 0x2c: case 0x2d: case 0x2e: case 0x2f:
+        case 0x30: case 0x31: case 0x32: case 0x33: case 0x34: case 0x35: case 0x36: case 0x37:
+        case 0x38: case 0x39: case 0x3a: case 0x3b: case 0x3c: case 0x3d: case 0x3e: case 0x3f:
+        case 0x40: case 0x41: case 0x42: case 0x43: case 0x44: case 0x45: case 0x46: case 0x47:
+        case 0x48: case 0x49: case 0x4a: case 0x4b: case 0x4c: case 0x4d: case 0x4e: case 0x4f:
+        case 0x50: case 0x51: case 0x52: case 0x53: case 0x54: case 0x55: case 0x56: case 0x57:
+        case 0x58: case 0x59: case 0x5a: case 0x5b: case 0x5c: case 0x5d: case 0x5e: case 0x5f:
+        case 0x60: case 0x61: case 0x62: case 0x63: case 0x64: case 0x65: case 0x66: case 0x67:
+        case 0x68: case 0x69: case 0x6a: case 0x6b: case 0x6c: case 0x6d: case 0x6e: case 0x6f:
+        case 0x70: case 0x71: case 0x72: case 0x73: case 0x74: case 0x75: case 0x76: case 0x77:
+        case 0x78: case 0x79: case 0x7a: case 0x7b: case 0x7c: case 0x7d: case 0x7e: case 0x7f:
+            node->type = mpack_type_uint;
+            node->value.u = type;
+            return true;
+
+        // negative fixnum
+        case 0xe0: case 0xe1: case 0xe2: case 0xe3: case 0xe4: case 0xe5: case 0xe6: case 0xe7:
+        case 0xe8: case 0xe9: case 0xea: case 0xeb: case 0xec: case 0xed: case 0xee: case 0xef:
+        case 0xf0: case 0xf1: case 0xf2: case 0xf3: case 0xf4: case 0xf5: case 0xf6: case 0xf7:
+        case 0xf8: case 0xf9: case 0xfa: case 0xfb: case 0xfc: case 0xfd: case 0xfe: case 0xff:
+            node->type = mpack_type_int;
+            node->value.i = (int8_t)type;
+            return true;
+
+        // fixmap
+        case 0x80: case 0x81: case 0x82: case 0x83: case 0x84: case 0x85: case 0x86: case 0x87:
+        case 0x88: case 0x89: case 0x8a: case 0x8b: case 0x8c: case 0x8d: case 0x8e: case 0x8f:
+            node->type = mpack_type_map;
+            node->len = (uint32_t)(type & ~0xf0);
+            return mpack_tree_parse_children(tree, node);
+
+        // fixarray
+        case 0x90: case 0x91: case 0x92: case 0x93: case 0x94: case 0x95: case 0x96: case 0x97:
+        case 0x98: case 0x99: case 0x9a: case 0x9b: case 0x9c: case 0x9d: case 0x9e: case 0x9f:
+            node->type = mpack_type_array;
+            node->len = (uint32_t)(type & ~0xf0);
+            return mpack_tree_parse_children(tree, node);
+
+        // fixstr
+        case 0xa0: case 0xa1: case 0xa2: case 0xa3: case 0xa4: case 0xa5: case 0xa6: case 0xa7:
+        case 0xa8: case 0xa9: case 0xaa: case 0xab: case 0xac: case 0xad: case 0xae: case 0xaf:
+        case 0xb0: case 0xb1: case 0xb2: case 0xb3: case 0xb4: case 0xb5: case 0xb6: case 0xb7:
+        case 0xb8: case 0xb9: case 0xba: case 0xbb: case 0xbc: case 0xbd: case 0xbe: case 0xbf:
+            node->type = mpack_type_str;
+            node->len = (uint32_t)(type & ~0xe0);
+            return mpack_tree_parse_bytes(tree, node);
+        #endif
+
+        // nil
+        case 0xc0:
+            node->type = mpack_type_nil;
+            return true;
+
+        // bool
+        case 0xc2: case 0xc3:
+            node->type = mpack_type_bool;
+            node->value.b = type & 1;
+            return true;
+
+        // bin8
+        case 0xc4:
+            node->type = mpack_type_bin;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint8_t)))
+                return false;
+            node->len = mpack_load_u8(tree->data + tree->size + 1);
+            return mpack_tree_parse_bytes(tree, node);
+
+        // bin16
+        case 0xc5:
+            node->type = mpack_type_bin;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint16_t)))
+                return false;
+            node->len = mpack_load_u16(tree->data + tree->size + 1);
+            return mpack_tree_parse_bytes(tree, node);
+
+        // bin32
+        case 0xc6:
+            node->type = mpack_type_bin;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->len = mpack_load_u32(tree->data + tree->size + 1);
+            return mpack_tree_parse_bytes(tree, node);
+
+        #if MPACK_EXTENSIONS
+        // ext8
+        case 0xc7:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint8_t)))
+                return false;
+            node->len = mpack_load_u8(tree->data + tree->size + 1);
+            return mpack_tree_parse_ext(tree, node);
+
+        // ext16
+        case 0xc8:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint16_t)))
+                return false;
+            node->len = mpack_load_u16(tree->data + tree->size + 1);
+            return mpack_tree_parse_ext(tree, node);
+
+        // ext32
+        case 0xc9:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->len = mpack_load_u32(tree->data + tree->size + 1);
+            return mpack_tree_parse_ext(tree, node);
+        #endif
+
+        // float
+        case 0xca:
+            #if MPACK_FLOAT
+            if (!mpack_tree_reserve_bytes(tree, sizeof(float)))
+                return false;
+            node->value.f = mpack_load_float(tree->data + tree->size + 1);
+            #else
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->value.f = mpack_load_u32(tree->data + tree->size + 1);
+            #endif
+            node->type = mpack_type_float;
+            return true;
+
+        // double
+        case 0xcb:
+            #if MPACK_DOUBLE
+            if (!mpack_tree_reserve_bytes(tree, sizeof(double)))
+                return false;
+            node->value.d = mpack_load_double(tree->data + tree->size + 1);
+            #else
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint64_t)))
+                return false;
+            node->value.d = mpack_load_u64(tree->data + tree->size + 1);
+            #endif
+            node->type = mpack_type_double;
+            return true;
+
+        // uint8
+        case 0xcc:
+            node->type = mpack_type_uint;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint8_t)))
+                return false;
+            node->value.u = mpack_load_u8(tree->data + tree->size + 1);
+            return true;
+
+        // uint16
+        case 0xcd:
+            node->type = mpack_type_uint;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint16_t)))
+                return false;
+            node->value.u = mpack_load_u16(tree->data + tree->size + 1);
+            return true;
+
+        // uint32
+        case 0xce:
+            node->type = mpack_type_uint;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->value.u = mpack_load_u32(tree->data + tree->size + 1);
+            return true;
+
+        // uint64
+        case 0xcf:
+            node->type = mpack_type_uint;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint64_t)))
+                return false;
+            node->value.u = mpack_load_u64(tree->data + tree->size + 1);
+            return true;
+
+        // int8
+        case 0xd0:
+            node->type = mpack_type_int;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(int8_t)))
+                return false;
+            node->value.i = mpack_load_i8(tree->data + tree->size + 1);
+            return true;
+
+        // int16
+        case 0xd1:
+            node->type = mpack_type_int;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(int16_t)))
+                return false;
+            node->value.i = mpack_load_i16(tree->data + tree->size + 1);
+            return true;
+
+        // int32
+        case 0xd2:
+            node->type = mpack_type_int;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(int32_t)))
+                return false;
+            node->value.i = mpack_load_i32(tree->data + tree->size + 1);
+            return true;
+
+        // int64
+        case 0xd3:
+            node->type = mpack_type_int;
+            if (!mpack_tree_reserve_bytes(tree, sizeof(int64_t)))
+                return false;
+            node->value.i = mpack_load_i64(tree->data + tree->size + 1);
+            return true;
+
+        #if MPACK_EXTENSIONS
+        // fixext1
+        case 0xd4:
+            node->len = 1;
+            return mpack_tree_parse_ext(tree, node);
+
+        // fixext2
+        case 0xd5:
+            node->len = 2;
+            return mpack_tree_parse_ext(tree, node);
+
+        // fixext4
+        case 0xd6:
+            node->len = 4;
+            return mpack_tree_parse_ext(tree, node);
+
+        // fixext8
+        case 0xd7:
+            node->len = 8;
+            return mpack_tree_parse_ext(tree, node);
+
+        // fixext16
+        case 0xd8:
+            node->len = 16;
+            return mpack_tree_parse_ext(tree, node);
+        #endif
+
+        // str8
+        case 0xd9:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint8_t)))
+                return false;
+            node->len = mpack_load_u8(tree->data + tree->size + 1);
+            node->type = mpack_type_str;
+            return mpack_tree_parse_bytes(tree, node);
+
+        // str16
+        case 0xda:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint16_t)))
+                return false;
+            node->len = mpack_load_u16(tree->data + tree->size + 1);
+            node->type = mpack_type_str;
+            return mpack_tree_parse_bytes(tree, node);
+
+        // str32
+        case 0xdb:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->len = mpack_load_u32(tree->data + tree->size + 1);
+            node->type = mpack_type_str;
+            return mpack_tree_parse_bytes(tree, node);
+
+        // array16
+        case 0xdc:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint16_t)))
+                return false;
+            node->len = mpack_load_u16(tree->data + tree->size + 1);
+            node->type = mpack_type_array;
+            return mpack_tree_parse_children(tree, node);
+
+        // array32
+        case 0xdd:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->len = mpack_load_u32(tree->data + tree->size + 1);
+            node->type = mpack_type_array;
+            return mpack_tree_parse_children(tree, node);
+
+        // map16
+        case 0xde:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint16_t)))
+                return false;
+            node->len = mpack_load_u16(tree->data + tree->size + 1);
+            node->type = mpack_type_map;
+            return mpack_tree_parse_children(tree, node);
+
+        // map32
+        case 0xdf:
+            if (!mpack_tree_reserve_bytes(tree, sizeof(uint32_t)))
+                return false;
+            node->len = mpack_load_u32(tree->data + tree->size + 1);
+            node->type = mpack_type_map;
+            return mpack_tree_parse_children(tree, node);
+
+        // reserved
+        case 0xc1:
+            mpack_tree_flag_error(tree, mpack_error_invalid);
+            return false;
+
+        #if !MPACK_EXTENSIONS
+        // ext
+        case 0xc7: // fallthrough
+        case 0xc8: // fallthrough
+        case 0xc9: // fallthrough
+        // fixext
+        case 0xd4: // fallthrough
+        case 0xd5: // fallthrough
+        case 0xd6: // fallthrough
+        case 0xd7: // fallthrough
+        case 0xd8:
+            mpack_tree_flag_error(tree, mpack_error_unsupported);
+            return false;
+        #endif
+
+        #if MPACK_OPTIMIZE_FOR_SIZE
+        // any other bytes should have been handled by the infix switch
+        default:
+            break;
+        #endif
+    }
+
+    mpack_assert(0, "unreachable");
+    return false;
+}
+
+static bool mpack_tree_parse_node(mpack_tree_t* tree, mpack_node_data_t* node) {
+    mpack_log("parsing a node at position %i in level %i\n",
+            (int)tree->size, (int)tree->parser.level);
+
+    if (!mpack_tree_parse_node_contents(tree, node)) {
+        mpack_log("node parsing returned false\n");
+        return false;
+    }
+
+    tree->parser.possible_nodes_left -= tree->parser.current_node_reserved;
+
+    // The reserve for the current node does not include the initial byte
+    // previously reserved as part of its parent.
+    size_t node_size = tree->parser.current_node_reserved + 1;
+
+    // If the parsed type is a map or array, the reserve includes one byte for
+    // each child. We want to subtract these out of possible_nodes_left, but
+    // not out of the current size of the tree.
+    if (node->type == mpack_type_array)
+        node_size -= node->len;
+    else if (node->type == mpack_type_map)
+        node_size -= node->len * 2;
+    tree->size += node_size;
+
+    mpack_log("parsed a node of type %s of %i bytes and "
+            "%i additional bytes reserved for children.\n",
+            mpack_type_to_string(node->type), (int)node_size,
+            (int)tree->parser.current_node_reserved + 1 - (int)node_size);
+
+    return true;
+}
+
+/*
+ * We read nodes in a loop instead of recursively for maximum performance. The
+ * stack holds the amount of children left to read in each level of the tree.
+ * Parsing can pause and resume when more data becomes available.
+ */
+static bool mpack_tree_continue_parsing(mpack_tree_t* tree) {
+    if (mpack_tree_error(tree) != mpack_ok)
+        return false;
+
+    mpack_tree_parser_t* parser = &tree->parser;
+    mpack_assert(parser->state == mpack_tree_parse_state_in_progress);
+    mpack_log("parsing tree elements, %i bytes in buffer\n", (int)tree->data_length);
+
+    // we loop parsing nodes until the parse stack is empty. we break
+    // by returning out of the function.
+    while (true) {
+        mpack_node_data_t* node = parser->stack[parser->level].child;
+        size_t level = parser->level;
+        if (!mpack_tree_parse_node(tree, node))
+            return false;
+        --parser->stack[level].left;
+        ++parser->stack[level].child;
+
+        mpack_assert(mpack_tree_error(tree) == mpack_ok,
+                "mpack_tree_parse_node() should have returned false due to error!");
+
+        // pop empty stack levels, exiting the outer loop when the stack is empty.
+        // (we could tail-optimize containers by pre-emptively popping empty
+        // stack levels before reading the new element, this way we wouldn't
+        // have to loop. but we eventually want to use the parse stack to give
+        // better error messages that contain the location of the error, so
+        // it needs to be complete.)
+        while (parser->stack[parser->level].left == 0) {
+            if (parser->level == 0)
+                return true;
+            --parser->level;
+        }
+    }
+}
+
+static void mpack_tree_cleanup(mpack_tree_t* tree) {
+    MPACK_UNUSED(tree);
+
+    #ifdef MPACK_MALLOC
+    if (tree->parser.stack_owned) {
+        MPACK_FREE(tree->parser.stack);
+        tree->parser.stack = NULL;
+        tree->parser.stack_owned = false;
+    }
+
+    mpack_tree_page_t* page = tree->next;
+    while (page != NULL) {
+        mpack_tree_page_t* next = page->next;
+        mpack_log("freeing page %p\n", (void*)page);
+        MPACK_FREE(page);
+        page = next;
+    }
+    tree->next = NULL;
+    #endif
+}
+
+static bool mpack_tree_parse_start(mpack_tree_t* tree) {
+    if (mpack_tree_error(tree) != mpack_ok)
+        return false;
+
+    mpack_tree_parser_t* parser = &tree->parser;
+    mpack_assert(parser->state != mpack_tree_parse_state_in_progress,
+            "previous parsing was not finished!");
+
+    if (parser->state == mpack_tree_parse_state_parsed)
+        mpack_tree_cleanup(tree);
+
+    mpack_log("starting parse\n");
+    tree->parser.state = mpack_tree_parse_state_in_progress;
+    tree->parser.current_node_reserved = 0;
+
+    // check if we previously parsed a tree
+    if (tree->size > 0) {
+        #ifdef MPACK_MALLOC
+        // if we're buffered, move the remaining data back to the
+        // start of the buffer
+        // TODO: This is not ideal performance-wise. We should only move data
+        // when we need to call the fill function.
+        // TODO: We could consider shrinking the buffer here, especially if we
+        // determine that the fill function is providing less than a quarter of
+        // the buffer size or if messages take up less than a quarter of the
+        // buffer size. Maybe this should be configurable.
+        if (tree->buffer != NULL) {
+            mpack_memmove(tree->buffer, tree->buffer + tree->size, tree->data_length - tree->size);
+        }
+        else
+        #endif
+        // otherwise advance past the parsed data
+        {
+            tree->data += tree->size;
+        }
+        tree->data_length -= tree->size;
+        tree->size = 0;
+        tree->node_count = 0;
+    }
+
+    // make sure we have at least one byte available before allocating anything
+    parser->possible_nodes_left = tree->data_length;
+    if (!mpack_tree_reserve_bytes(tree, sizeof(uint8_t))) {
+        tree->parser.state = mpack_tree_parse_state_not_started;
+        return false;
+    }
+    mpack_log("parsing tree at %p starting with byte %x\n", tree->data, (uint8_t)tree->data[0]);
+    parser->possible_nodes_left -= 1;
+    tree->node_count = 1;
+
+    #ifdef MPACK_MALLOC
+    parser->stack = parser->stack_local;
+    parser->stack_owned = false;
+    parser->stack_capacity = sizeof(parser->stack_local) / sizeof(*parser->stack_local);
+
+    if (tree->pool == NULL) {
+
+        // allocate first page
+        mpack_tree_page_t* page = (mpack_tree_page_t*)MPACK_MALLOC(MPACK_PAGE_ALLOC_SIZE);
+        mpack_log("allocated initial page %p of size %i count %i\n",
+                (void*)page, (int)MPACK_PAGE_ALLOC_SIZE, (int)MPACK_NODES_PER_PAGE);
+        if (page == NULL) {
+            tree->error = mpack_error_memory;
+            return false;
+        }
+        page->next = NULL;
+        tree->next = page;
+
+        parser->nodes = page->nodes;
+        parser->nodes_left = MPACK_NODES_PER_PAGE;
+    }
+    else
+    #endif
+    {
+        // otherwise use the provided pool
+        mpack_assert(tree->pool != NULL, "no pool provided?");
+        parser->nodes = tree->pool;
+        parser->nodes_left = tree->pool_count;
+    }
+
+    tree->root = parser->nodes;
+    ++parser->nodes;
+    --parser->nodes_left;
+
+    parser->level = 0;
+    parser->stack[0].child = tree->root;
+    parser->stack[0].left = 1;
+
+    return true;
+}
+
+void mpack_tree_parse(mpack_tree_t* tree) {
+    if (mpack_tree_error(tree) != mpack_ok)
+        return;
+
+    if (tree->parser.state != mpack_tree_parse_state_in_progress) {
+        if (!mpack_tree_parse_start(tree)) {
+            mpack_tree_flag_error(tree, (tree->read_fn == NULL) ?
+                    mpack_error_invalid : mpack_error_io);
+            return;
+        }
+    }
+
+    if (!mpack_tree_continue_parsing(tree)) {
+        if (mpack_tree_error(tree) != mpack_ok)
+            return;
+
+        // We're parsing synchronously on a blocking fill function. If we
+        // didn't completely finish parsing the tree, it's an error.
+        mpack_log("tree parsing incomplete. flagging error.\n");
+        mpack_tree_flag_error(tree, (tree->read_fn == NULL) ?
+                mpack_error_invalid : mpack_error_io);
+        return;
+    }
+
+    mpack_assert(mpack_tree_error(tree) == mpack_ok);
+    mpack_assert(tree->parser.level == 0);
+    tree->parser.state = mpack_tree_parse_state_parsed;
+    mpack_log("parsed tree of %i bytes, %i bytes left\n", (int)tree->size, (int)tree->parser.possible_nodes_left);
+    mpack_log("%i nodes in final page\n", (int)tree->parser.nodes_left);
+}
+
+bool mpack_tree_try_parse(mpack_tree_t* tree) {
+    if (mpack_tree_error(tree) != mpack_ok)
+        return false;
+
+    if (tree->parser.state != mpack_tree_parse_state_in_progress)
+        if (!mpack_tree_parse_start(tree))
+            return false;
+
+    if (!mpack_tree_continue_parsing(tree))
+        return false;
+
+    mpack_assert(mpack_tree_error(tree) == mpack_ok);
+    mpack_assert(tree->parser.level == 0);
+    tree->parser.state = mpack_tree_parse_state_parsed;
+    return true;
+}
+
+
+
+/*
+ * Tree functions
+ */
+
+mpack_node_t mpack_tree_root(mpack_tree_t* tree) {
+    if (mpack_tree_error(tree) != mpack_ok)
+        return mpack_tree_nil_node(tree);
+
+    // We check that a tree was parsed successfully and assert if not. You must
+    // call mpack_tree_parse() (or mpack_tree_try_parse() with a success
+    // result) in order to access the root node.
+    if (tree->parser.state != mpack_tree_parse_state_parsed) {
+        mpack_break("Tree has not been parsed! "
+                "Did you call mpack_tree_parse() or mpack_tree_try_parse()?");
+        mpack_tree_flag_error(tree, mpack_error_bug);
+        return mpack_tree_nil_node(tree);
+    }
+
+    return mpack_node(tree, tree->root);
+}
+
+static void mpack_tree_init_clear(mpack_tree_t* tree) {
+    mpack_memset(tree, 0, sizeof(*tree));
+    tree->nil_node.type = mpack_type_nil;
+    tree->missing_node.type = mpack_type_missing;
+    tree->max_size = SIZE_MAX;
+    tree->max_nodes = SIZE_MAX;
+}
+
+#ifdef MPACK_MALLOC
+void mpack_tree_init_data(mpack_tree_t* tree, const char* data, size_t length) {
+    mpack_tree_init_clear(tree);
+
+    MPACK_STATIC_ASSERT(MPACK_NODE_PAGE_SIZE >= sizeof(mpack_tree_page_t),
+            "MPACK_NODE_PAGE_SIZE is too small");
+
+    MPACK_STATIC_ASSERT(MPACK_PAGE_ALLOC_SIZE <= MPACK_NODE_PAGE_SIZE,
+            "incorrect page rounding?");
+
+    tree->data = data;
+    tree->data_length = length;
+    tree->pool = NULL;
+    tree->pool_count = 0;
+    tree->next = NULL;
+
+    mpack_log("===========================\n");
+    mpack_log("initializing tree with data of size %i\n", (int)length);
+}
+#endif
+
+void mpack_tree_init_pool(mpack_tree_t* tree, const char* data, size_t length,
+        mpack_node_data_t* node_pool, size_t node_pool_count)
+{
+    mpack_tree_init_clear(tree);
+    #ifdef MPACK_MALLOC
+    tree->next = NULL;
+    #endif
+
+    if (node_pool_count == 0) {
+        mpack_break("initial page has no nodes!");
+        mpack_tree_flag_error(tree, mpack_error_bug);
+        return;
+    }
+
+    tree->data = data;
+    tree->data_length = length;
+    tree->pool = node_pool;
+    tree->pool_count = node_pool_count;
+
+    mpack_log("===========================\n");
+    mpack_log("initializing tree with data of size %i and pool of count %i\n",
+            (int)length, (int)node_pool_count);
+}
+
+void mpack_tree_init_error(mpack_tree_t* tree, mpack_error_t error) {
+    mpack_tree_init_clear(tree);
+    tree->error = error;
+
+    mpack_log("===========================\n");
+    mpack_log("initializing tree error state %i\n", (int)error);
+}
+
+#ifdef MPACK_MALLOC
+void mpack_tree_init_stream(mpack_tree_t* tree, mpack_tree_read_t read_fn, void* context,
+        size_t max_message_size, size_t max_message_nodes) {
+    mpack_tree_init_clear(tree);
+
+    tree->read_fn = read_fn;
+    tree->context = context;
+
+    mpack_tree_set_limits(tree, max_message_size, max_message_nodes);
+    tree->max_size = max_message_size;
+    tree->max_nodes = max_message_nodes;
+
+    mpack_log("===========================\n");
+    mpack_log("initializing tree with stream, max size %i max nodes %i\n",
+            (int)max_message_size, (int)max_message_nodes);
+}
+#endif
+
+void mpack_tree_set_limits(mpack_tree_t* tree, size_t max_message_size, size_t max_message_nodes) {
+    mpack_assert(max_message_size > 0);
+    mpack_assert(max_message_nodes > 0);
+    tree->max_size = max_message_size;
+    tree->max_nodes = max_message_nodes;
+}
+
+#if MPACK_STDIO
+typedef struct mpack_file_tree_t {
+    char* data;
+    size_t size;
+    char buffer[MPACK_BUFFER_SIZE];
+} mpack_file_tree_t;
+
+static void mpack_file_tree_teardown(mpack_tree_t* tree) {
+    mpack_file_tree_t* file_tree = (mpack_file_tree_t*)tree->context;
+    MPACK_FREE(file_tree->data);
+    MPACK_FREE(file_tree);
+}
+
+static bool mpack_file_tree_read(mpack_tree_t* tree, mpack_file_tree_t* file_tree, FILE* file, size_t max_bytes) {
+
+    // get the file size
+    errno = 0;
+    int error = 0;
+    fseek(file, 0, SEEK_END);
+    error |= errno;
+    long size = ftell(file);
+    error |= errno;
+    fseek(file, 0, SEEK_SET);
+    error |= errno;
+
+    // check for errors
+    if (error != 0 || size < 0) {
+        mpack_tree_init_error(tree, mpack_error_io);
+        return false;
+    }
+    if (size == 0) {
+        mpack_tree_init_error(tree, mpack_error_invalid);
+        return false;
+    }
+
+    // make sure the size is less than max_bytes
+    // (this mess exists to safely convert between long and size_t regardless of their widths)
+    if (max_bytes != 0 && (((uint64_t)LONG_MAX > (uint64_t)SIZE_MAX && size > (long)SIZE_MAX) || (size_t)size > max_bytes)) {
+        mpack_tree_init_error(tree, mpack_error_too_big);
+        return false;
+    }
+
+    // allocate data
+    file_tree->data = (char*)MPACK_MALLOC((size_t)size);
+    if (file_tree->data == NULL) {
+        mpack_tree_init_error(tree, mpack_error_memory);
+        return false;
+    }
+
+    // read the file
+    long total = 0;
+    while (total < size) {
+        size_t read = fread(file_tree->data + total, 1, (size_t)(size - total), file);
+        if (read <= 0) {
+            mpack_tree_init_error(tree, mpack_error_io);
+            MPACK_FREE(file_tree->data);
+            return false;
+        }
+        total += (long)read;
+    }
+
+    file_tree->size = (size_t)size;
+    return true;
+}
+
+static bool mpack_tree_file_check_max_bytes(mpack_tree_t* tree, size_t max_bytes) {
+
+    // the C STDIO family of file functions use long (e.g. ftell)
+    if (max_bytes > LONG_MAX) {
+        mpack_break("max_bytes of %" PRIu64 " is invalid, maximum is LONG_MAX", (uint64_t)max_bytes);
+        mpack_tree_init_error(tree, mpack_error_bug);
+        return false;
+    }
+
+    return true;
+}
+
+static void mpack_tree_init_stdfile_noclose(mpack_tree_t* tree, FILE* stdfile, size_t max_bytes) {
+
+    // allocate file tree
+    mpack_file_tree_t* file_tree = (mpack_file_tree_t*) MPACK_MALLOC(sizeof(mpack_file_tree_t));
+    if (file_tree == NULL) {
+        mpack_tree_init_error(tree, mpack_error_memory);
+        return;
+    }
+
+    // read all data
+    if (!mpack_file_tree_read(tree, file_tree, stdfile, max_bytes)) {
+        MPACK_FREE(file_tree);
+        return;
+    }
+
+    mpack_tree_init_data(tree, file_tree->data, file_tree->size);
+    mpack_tree_set_context(tree, file_tree);
+    mpack_tree_set_teardown(tree, mpack_file_tree_teardown);
+}
+
+void mpack_tree_init_stdfile(mpack_tree_t* tree, FILE* stdfile, size_t max_bytes, bool close_when_done) {
+    if (!mpack_tree_file_check_max_bytes(tree, max_bytes))
+        return;
+
+    mpack_tree_init_stdfile_noclose(tree, stdfile, max_bytes);
+
+    if (close_when_done)
+        fclose(stdfile);
+}
+
+void mpack_tree_init_filename(mpack_tree_t* tree, const char* filename, size_t max_bytes) {
+    if (!mpack_tree_file_check_max_bytes(tree, max_bytes))
+        return;
+
+    // open the file
+    FILE* file = fopen(filename, "rb");
+    if (!file) {
+        mpack_tree_init_error(tree, mpack_error_io);
+        return;
+    }
+
+    mpack_tree_init_stdfile(tree, file, max_bytes, true);
+}
+#endif
+
+mpack_error_t mpack_tree_destroy(mpack_tree_t* tree) {
+    mpack_tree_cleanup(tree);
+
+    #ifdef MPACK_MALLOC
+    if (tree->buffer)
+        MPACK_FREE(tree->buffer);
+    #endif
+
+    if (tree->teardown)
+        tree->teardown(tree);
+    tree->teardown = NULL;
+
+    return tree->error;
+}
+
+void mpack_tree_flag_error(mpack_tree_t* tree, mpack_error_t error) {
+    if (tree->error == mpack_ok) {
+        mpack_log("tree %p setting error %i: %s\n", (void*)tree, (int)error, mpack_error_to_string(error));
+        tree->error = error;
+        if (tree->error_fn)
+            tree->error_fn(tree, error);
+    }
+
+}
+
+
+
+/*
+ * Node misc functions
+ */
+
+void mpack_node_flag_error(mpack_node_t node, mpack_error_t error) {
+    mpack_tree_flag_error(node.tree, error);
+}
+
+mpack_tag_t mpack_node_tag(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return mpack_tag_nil();
+
+    mpack_tag_t tag = MPACK_TAG_ZERO;
+
+    tag.type = node.data->type;
+    switch (node.data->type) {
+        case mpack_type_missing:
+            // If a node is missing, I don't know if it makes sense to ask for
+            // a tag for it. We'll return a missing tag to match the missing
+            // node I guess, but attempting to use the tag for anything (like
+            // writing it for example) will flag mpack_error_bug.
+            break;
+        case mpack_type_nil:                                            break;
+        case mpack_type_bool:    tag.v.b = node.data->value.b;          break;
+        case mpack_type_float:   tag.v.f = node.data->value.f;          break;
+        case mpack_type_double:  tag.v.d = node.data->value.d;          break;
+        case mpack_type_int:     tag.v.i = node.data->value.i;          break;
+        case mpack_type_uint:    tag.v.u = node.data->value.u;          break;
+
+        case mpack_type_str:     tag.v.l = node.data->len;     break;
+        case mpack_type_bin:     tag.v.l = node.data->len;     break;
+
+        #if MPACK_EXTENSIONS
+        case mpack_type_ext:
+            tag.v.l = node.data->len;
+            tag.exttype = mpack_node_exttype_unchecked(node);
+            break;
+        #endif
+
+        case mpack_type_array:   tag.v.n = node.data->len;  break;
+        case mpack_type_map:     tag.v.n = node.data->len;  break;
+
+        default:
+            mpack_assert(0, "unrecognized type %i", (int)node.data->type);
+            break;
+    }
+    return tag;
+}
+
+#if MPACK_DEBUG && MPACK_STDIO
+static void mpack_node_print_element(mpack_node_t node, mpack_print_t* print, size_t depth) {
+    mpack_node_data_t* data = node.data;
+    size_t i,j;
+    switch (data->type) {
+        case mpack_type_str:
+            {
+                mpack_print_append_cstr(print, "\"");
+                const char* bytes = mpack_node_data_unchecked(node);
+                for (i = 0; i < data->len; ++i) {
+                    char c = bytes[i];
+                    switch (c) {
+                        case '\n': mpack_print_append_cstr(print, "\\n"); break;
+                        case '\\': mpack_print_append_cstr(print, "\\\\"); break;
+                        case '"': mpack_print_append_cstr(print, "\\\""); break;
+                        default: mpack_print_append(print, &c, 1); break;
+                    }
+                }
+                mpack_print_append_cstr(print, "\"");
+            }
+            break;
+
+        case mpack_type_array:
+            mpack_print_append_cstr(print, "[\n");
+            for (i = 0; i < data->len; ++i) {
+                for (j = 0; j < depth + 1; ++j)
+                    mpack_print_append_cstr(print, "    ");
+                mpack_node_print_element(mpack_node_array_at(node, i), print, depth + 1);
+                if (i != data->len - 1)
+                    mpack_print_append_cstr(print, ",");
+                mpack_print_append_cstr(print, "\n");
+            }
+            for (i = 0; i < depth; ++i)
+                mpack_print_append_cstr(print, "    ");
+            mpack_print_append_cstr(print, "]");
+            break;
+
+        case mpack_type_map:
+            mpack_print_append_cstr(print, "{\n");
+            for (i = 0; i < data->len; ++i) {
+                for (j = 0; j < depth + 1; ++j)
+                    mpack_print_append_cstr(print, "    ");
+                mpack_node_print_element(mpack_node_map_key_at(node, i), print, depth + 1);
+                mpack_print_append_cstr(print, ": ");
+                mpack_node_print_element(mpack_node_map_value_at(node, i), print, depth + 1);
+                if (i != data->len - 1)
+                    mpack_print_append_cstr(print, ",");
+                mpack_print_append_cstr(print, "\n");
+            }
+            for (i = 0; i < depth; ++i)
+                mpack_print_append_cstr(print, "    ");
+            mpack_print_append_cstr(print, "}");
+            break;
+
+        default:
+            {
+                const char* prefix = NULL;
+                size_t prefix_length = 0;
+                if (mpack_node_type(node) == mpack_type_bin
+                        #if MPACK_EXTENSIONS
+                        || mpack_node_type(node) == mpack_type_ext
+                        #endif
+                ) {
+                    prefix = mpack_node_data(node);
+                    prefix_length = mpack_node_data_len(node);
+                }
+
+                char buf[256];
+                mpack_tag_t tag = mpack_node_tag(node);
+                mpack_tag_debug_pseudo_json(tag, buf, sizeof(buf), prefix, prefix_length);
+                mpack_print_append_cstr(print, buf);
+            }
+            break;
+    }
+}
+
+void mpack_node_print_to_buffer(mpack_node_t node, char* buffer, size_t buffer_size) {
+    if (buffer_size == 0) {
+        mpack_assert(false, "buffer size is zero!");
+        return;
+    }
+
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = buffer_size;
+    mpack_node_print_element(node, &print, 0);
+    mpack_print_append(&print, "",  1); // null-terminator
+    mpack_print_flush(&print);
+
+    // we always make sure there's a null-terminator at the end of the buffer
+    // in case we ran out of space.
+    print.buffer[print.size - 1] = '\0';
+}
+
+void mpack_node_print_to_callback(mpack_node_t node, mpack_print_callback_t callback, void* context) {
+    char buffer[1024];
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = sizeof(buffer);
+    print.callback = callback;
+    print.context = context;
+    mpack_node_print_element(node, &print, 0);
+    mpack_print_flush(&print);
+}
+
+void mpack_node_print_to_file(mpack_node_t node, FILE* file) {
+    mpack_assert(file != NULL, "file is NULL");
+
+    char buffer[1024];
+    mpack_print_t print;
+    mpack_memset(&print, 0, sizeof(print));
+    print.buffer = buffer;
+    print.size = sizeof(buffer);
+    print.callback = &mpack_print_file_callback;
+    print.context = file;
+
+    size_t depth = 2;
+    size_t i;
+    for (i = 0; i < depth; ++i)
+        mpack_print_append_cstr(&print, "    ");
+    mpack_node_print_element(node, &print, depth);
+    mpack_print_append_cstr(&print, "\n");
+    mpack_print_flush(&print);
+}
+#endif
+
+
+
+/*
+ * Node Value Functions
+ */
+
+#if MPACK_EXTENSIONS
+mpack_timestamp_t mpack_node_timestamp(mpack_node_t node) {
+    mpack_timestamp_t timestamp = {0, 0};
+
+    // we'll let mpack_node_exttype() do most checks
+    if (mpack_node_exttype(node) != MPACK_EXTTYPE_TIMESTAMP) {
+        mpack_log("exttype %i\n", mpack_node_exttype(node));
+        mpack_node_flag_error(node, mpack_error_type);
+        return timestamp;
+    }
+
+    const char* p = mpack_node_data_unchecked(node);
+
+    switch (node.data->len) {
+        case 4:
+            timestamp.nanoseconds = 0;
+            timestamp.seconds = mpack_load_u32(p);
+            break;
+
+        case 8: {
+            uint64_t value = mpack_load_u64(p);
+            timestamp.nanoseconds = (uint32_t)(value >> 34);
+            timestamp.seconds = value & ((MPACK_UINT64_C(1) << 34) - 1);
+            break;
+        }
+
+        case 12:
+            timestamp.nanoseconds = mpack_load_u32(p);
+            timestamp.seconds = mpack_load_i64(p + 4);
+            break;
+
+        default:
+            mpack_tree_flag_error(node.tree, mpack_error_invalid);
+            return timestamp;
+    }
+
+    if (timestamp.nanoseconds > MPACK_TIMESTAMP_NANOSECONDS_MAX) {
+        mpack_tree_flag_error(node.tree, mpack_error_invalid);
+        mpack_timestamp_t zero = {0, 0};
+        return zero;
+    }
+
+    return timestamp;
+}
+
+int64_t mpack_node_timestamp_seconds(mpack_node_t node) {
+    return mpack_node_timestamp(node).seconds;
+}
+
+uint32_t mpack_node_timestamp_nanoseconds(mpack_node_t node) {
+    return mpack_node_timestamp(node).nanoseconds;
+}
+#endif
+
+
+
+/*
+ * Node Data Functions
+ */
+
+void mpack_node_check_utf8(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return;
+    mpack_node_data_t* data = node.data;
+    if (data->type != mpack_type_str || !mpack_utf8_check(mpack_node_data_unchecked(node), data->len))
+        mpack_node_flag_error(node, mpack_error_type);
+}
+
+void mpack_node_check_utf8_cstr(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return;
+    mpack_node_data_t* data = node.data;
+    if (data->type != mpack_type_str || !mpack_utf8_check_no_null(mpack_node_data_unchecked(node), data->len))
+        mpack_node_flag_error(node, mpack_error_type);
+}
+
+size_t mpack_node_copy_data(mpack_node_t node, char* buffer, size_t bufsize) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    mpack_assert(bufsize == 0 || buffer != NULL, "buffer is NULL for maximum of %i bytes", (int)bufsize);
+
+    mpack_type_t type = node.data->type;
+    if (type != mpack_type_str && type != mpack_type_bin
+            #if MPACK_EXTENSIONS
+            && type != mpack_type_ext
+            #endif
+    ) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return 0;
+    }
+
+    if (node.data->len > bufsize) {
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return 0;
+    }
+
+    mpack_memcpy(buffer, mpack_node_data_unchecked(node), node.data->len);
+    return (size_t)node.data->len;
+}
+
+size_t mpack_node_copy_utf8(mpack_node_t node, char* buffer, size_t bufsize) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    mpack_assert(bufsize == 0 || buffer != NULL, "buffer is NULL for maximum of %i bytes", (int)bufsize);
+
+    mpack_type_t type = node.data->type;
+    if (type != mpack_type_str) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return 0;
+    }
+
+    if (node.data->len > bufsize) {
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return 0;
+    }
+
+    if (!mpack_utf8_check(mpack_node_data_unchecked(node), node.data->len)) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return 0;
+    }
+
+    mpack_memcpy(buffer, mpack_node_data_unchecked(node), node.data->len);
+    return (size_t)node.data->len;
+}
+
+void mpack_node_copy_cstr(mpack_node_t node, char* buffer, size_t bufsize) {
+
+    // we can't break here because the error isn't recoverable; we
+    // have to add a null-terminator.
+    mpack_assert(buffer != NULL, "buffer is NULL");
+    mpack_assert(bufsize >= 1, "buffer size is zero; you must have room for at least a null-terminator");
+
+    if (mpack_node_error(node) != mpack_ok) {
+        buffer[0] = '\0';
+        return;
+    }
+
+    if (node.data->type != mpack_type_str) {
+        buffer[0] = '\0';
+        mpack_node_flag_error(node, mpack_error_type);
+        return;
+    }
+
+    if (node.data->len > bufsize - 1) {
+        buffer[0] = '\0';
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return;
+    }
+
+    if (!mpack_str_check_no_null(mpack_node_data_unchecked(node), node.data->len)) {
+        buffer[0] = '\0';
+        mpack_node_flag_error(node, mpack_error_type);
+        return;
+    }
+
+    mpack_memcpy(buffer, mpack_node_data_unchecked(node), node.data->len);
+    buffer[node.data->len] = '\0';
+}
+
+void mpack_node_copy_utf8_cstr(mpack_node_t node, char* buffer, size_t bufsize) {
+
+    // we can't break here because the error isn't recoverable; we
+    // have to add a null-terminator.
+    mpack_assert(buffer != NULL, "buffer is NULL");
+    mpack_assert(bufsize >= 1, "buffer size is zero; you must have room for at least a null-terminator");
+
+    if (mpack_node_error(node) != mpack_ok) {
+        buffer[0] = '\0';
+        return;
+    }
+
+    if (node.data->type != mpack_type_str) {
+        buffer[0] = '\0';
+        mpack_node_flag_error(node, mpack_error_type);
+        return;
+    }
+
+    if (node.data->len > bufsize - 1) {
+        buffer[0] = '\0';
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return;
+    }
+
+    if (!mpack_utf8_check_no_null(mpack_node_data_unchecked(node), node.data->len)) {
+        buffer[0] = '\0';
+        mpack_node_flag_error(node, mpack_error_type);
+        return;
+    }
+
+    mpack_memcpy(buffer, mpack_node_data_unchecked(node), node.data->len);
+    buffer[node.data->len] = '\0';
+}
+
+#ifdef MPACK_MALLOC
+char* mpack_node_data_alloc(mpack_node_t node, size_t maxlen) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    // make sure this is a valid data type
+    mpack_type_t type = node.data->type;
+    if (type != mpack_type_str && type != mpack_type_bin
+            #if MPACK_EXTENSIONS
+            && type != mpack_type_ext
+            #endif
+    ) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    if (node.data->len > maxlen) {
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return NULL;
+    }
+
+    char* ret = (char*) MPACK_MALLOC((size_t)node.data->len);
+    if (ret == NULL) {
+        mpack_node_flag_error(node, mpack_error_memory);
+        return NULL;
+    }
+
+    mpack_memcpy(ret, mpack_node_data_unchecked(node), node.data->len);
+    return ret;
+}
+
+char* mpack_node_cstr_alloc(mpack_node_t node, size_t maxlen) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    // make sure maxlen makes sense
+    if (maxlen < 1) {
+        mpack_break("maxlen is zero; you must have room for at least a null-terminator");
+        mpack_node_flag_error(node, mpack_error_bug);
+        return NULL;
+    }
+
+    if (node.data->type != mpack_type_str) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    if (node.data->len > maxlen - 1) {
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return NULL;
+    }
+
+    if (!mpack_str_check_no_null(mpack_node_data_unchecked(node), node.data->len)) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    char* ret = (char*) MPACK_MALLOC((size_t)(node.data->len + 1));
+    if (ret == NULL) {
+        mpack_node_flag_error(node, mpack_error_memory);
+        return NULL;
+    }
+
+    mpack_memcpy(ret, mpack_node_data_unchecked(node), node.data->len);
+    ret[node.data->len] = '\0';
+    return ret;
+}
+
+char* mpack_node_utf8_cstr_alloc(mpack_node_t node, size_t maxlen) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    // make sure maxlen makes sense
+    if (maxlen < 1) {
+        mpack_break("maxlen is zero; you must have room for at least a null-terminator");
+        mpack_node_flag_error(node, mpack_error_bug);
+        return NULL;
+    }
+
+    if (node.data->type != mpack_type_str) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    if (node.data->len > maxlen - 1) {
+        mpack_node_flag_error(node, mpack_error_too_big);
+        return NULL;
+    }
+
+    if (!mpack_utf8_check_no_null(mpack_node_data_unchecked(node), node.data->len)) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    char* ret = (char*) MPACK_MALLOC((size_t)(node.data->len + 1));
+    if (ret == NULL) {
+        mpack_node_flag_error(node, mpack_error_memory);
+        return NULL;
+    }
+
+    mpack_memcpy(ret, mpack_node_data_unchecked(node), node.data->len);
+    ret[node.data->len] = '\0';
+    return ret;
+}
+#endif
+
+
+/*
+ * Compound Node Functions
+ */
+
+static mpack_node_data_t* mpack_node_map_int_impl(mpack_node_t node, int64_t num) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    if (node.data->type != mpack_type_map) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    mpack_node_data_t* found = NULL;
+
+    size_t i;
+    for (i = 0; i < node.data->len; ++i) {
+        mpack_node_data_t* key = mpack_node_child(node, i * 2);
+
+        if ((key->type == mpack_type_int && key->value.i == num) ||
+            (key->type == mpack_type_uint && num >= 0 && key->value.u == (uint64_t)num))
+        {
+            if (found) {
+                mpack_node_flag_error(node, mpack_error_data);
+                return NULL;
+            }
+            found = mpack_node_child(node, i * 2 + 1);
+        }
+    }
+
+    if (found)
+        return found;
+
+    return NULL;
+}
+
+static mpack_node_data_t* mpack_node_map_uint_impl(mpack_node_t node, uint64_t num) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    if (node.data->type != mpack_type_map) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    mpack_node_data_t* found = NULL;
+
+    size_t i;
+    for (i = 0; i < node.data->len; ++i) {
+        mpack_node_data_t* key = mpack_node_child(node, i * 2);
+
+        if ((key->type == mpack_type_uint && key->value.u == num) ||
+            (key->type == mpack_type_int && key->value.i >= 0 && (uint64_t)key->value.i == num))
+        {
+            if (found) {
+                mpack_node_flag_error(node, mpack_error_data);
+                return NULL;
+            }
+            found = mpack_node_child(node, i * 2 + 1);
+        }
+    }
+
+    if (found)
+        return found;
+
+    return NULL;
+}
+
+static mpack_node_data_t* mpack_node_map_str_impl(mpack_node_t node, const char* str, size_t length) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    mpack_assert(length == 0 || str != NULL, "str of length %i is NULL", (int)length);
+
+    if (node.data->type != mpack_type_map) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return NULL;
+    }
+
+    mpack_tree_t* tree = node.tree;
+    mpack_node_data_t* found = NULL;
+
+    size_t i;
+    for (i = 0; i < node.data->len; ++i) {
+        mpack_node_data_t* key = mpack_node_child(node, i * 2);
+
+        if (key->type == mpack_type_str && key->len == length &&
+                mpack_memcmp(str, mpack_node_data_unchecked(mpack_node(tree, key)), length) == 0) {
+            if (found) {
+                mpack_node_flag_error(node, mpack_error_data);
+                return NULL;
+            }
+            found = mpack_node_child(node, i * 2 + 1);
+        }
+    }
+
+    if (found)
+        return found;
+
+    return NULL;
+}
+
+static mpack_node_t mpack_node_wrap_lookup(mpack_tree_t* tree, mpack_node_data_t* data) {
+    if (!data) {
+        if (tree->error == mpack_ok)
+            mpack_tree_flag_error(tree, mpack_error_data);
+        return mpack_tree_nil_node(tree);
+    }
+    return mpack_node(tree, data);
+}
+
+static mpack_node_t mpack_node_wrap_lookup_optional(mpack_tree_t* tree, mpack_node_data_t* data) {
+    if (!data) {
+        if (tree->error == mpack_ok)
+            return mpack_tree_missing_node(tree);
+        return mpack_tree_nil_node(tree);
+    }
+    return mpack_node(tree, data);
+}
+
+mpack_node_t mpack_node_map_int(mpack_node_t node, int64_t num) {
+    return mpack_node_wrap_lookup(node.tree, mpack_node_map_int_impl(node, num));
+}
+
+mpack_node_t mpack_node_map_int_optional(mpack_node_t node, int64_t num) {
+    return mpack_node_wrap_lookup_optional(node.tree, mpack_node_map_int_impl(node, num));
+}
+
+mpack_node_t mpack_node_map_uint(mpack_node_t node, uint64_t num) {
+    return mpack_node_wrap_lookup(node.tree, mpack_node_map_uint_impl(node, num));
+}
+
+mpack_node_t mpack_node_map_uint_optional(mpack_node_t node, uint64_t num) {
+    return mpack_node_wrap_lookup_optional(node.tree, mpack_node_map_uint_impl(node, num));
+}
+
+mpack_node_t mpack_node_map_str(mpack_node_t node, const char* str, size_t length) {
+    return mpack_node_wrap_lookup(node.tree, mpack_node_map_str_impl(node, str, length));
+}
+
+mpack_node_t mpack_node_map_str_optional(mpack_node_t node, const char* str, size_t length) {
+    return mpack_node_wrap_lookup_optional(node.tree, mpack_node_map_str_impl(node, str, length));
+}
+
+mpack_node_t mpack_node_map_cstr(mpack_node_t node, const char* cstr) {
+    mpack_assert(cstr != NULL, "cstr is NULL");
+    return mpack_node_map_str(node, cstr, mpack_strlen(cstr));
+}
+
+mpack_node_t mpack_node_map_cstr_optional(mpack_node_t node, const char* cstr) {
+    mpack_assert(cstr != NULL, "cstr is NULL");
+    return mpack_node_map_str_optional(node, cstr, mpack_strlen(cstr));
+}
+
+bool mpack_node_map_contains_int(mpack_node_t node, int64_t num) {
+    return mpack_node_map_int_impl(node, num) != NULL;
+}
+
+bool mpack_node_map_contains_uint(mpack_node_t node, uint64_t num) {
+    return mpack_node_map_uint_impl(node, num) != NULL;
+}
+
+bool mpack_node_map_contains_str(mpack_node_t node, const char* str, size_t length) {
+    return mpack_node_map_str_impl(node, str, length) != NULL;
+}
+
+bool mpack_node_map_contains_cstr(mpack_node_t node, const char* cstr) {
+    mpack_assert(cstr != NULL, "cstr is NULL");
+    return mpack_node_map_contains_str(node, cstr, mpack_strlen(cstr));
+}
+
+size_t mpack_node_enum_optional(mpack_node_t node, const char* strings[], size_t count) {
+    if (mpack_node_error(node) != mpack_ok)
+        return count;
+
+    // the value is only recognized if it is a string
+    if (mpack_node_type(node) != mpack_type_str)
+        return count;
+
+    // fetch the string
+    const char* key = mpack_node_str(node);
+    size_t keylen = mpack_node_strlen(node);
+    mpack_assert(mpack_node_error(node) == mpack_ok, "these should not fail");
+
+    // find what key it matches
+    size_t i;
+    for (i = 0; i < count; ++i) {
+        const char* other = strings[i];
+        size_t otherlen = mpack_strlen(other);
+        if (keylen == otherlen && mpack_memcmp(key, other, keylen) == 0)
+            return i;
+    }
+
+    // no matches
+    return count;
+}
+
+size_t mpack_node_enum(mpack_node_t node, const char* strings[], size_t count) {
+    size_t value = mpack_node_enum_optional(node, strings, count);
+    if (value == count)
+        mpack_node_flag_error(node, mpack_error_type);
+    return value;
+}
+
+mpack_type_t mpack_node_type(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return mpack_type_nil;
+    return node.data->type;
+}
+
+bool mpack_node_is_nil(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok) {
+        // All nodes are treated as nil nodes when we are in error.
+        return true;
+    }
+    return node.data->type == mpack_type_nil;
+}
+
+bool mpack_node_is_missing(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok) {
+        // errors still return nil nodes, not missing nodes.
+        return false;
+    }
+    return node.data->type == mpack_type_missing;
+}
+
+void mpack_node_nil(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return;
+    if (node.data->type != mpack_type_nil)
+        mpack_node_flag_error(node, mpack_error_type);
+}
+
+void mpack_node_missing(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return;
+    if (node.data->type != mpack_type_missing)
+        mpack_node_flag_error(node, mpack_error_type);
+}
+
+bool mpack_node_bool(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return false;
+
+    if (node.data->type == mpack_type_bool)
+        return node.data->value.b;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return false;
+}
+
+void mpack_node_true(mpack_node_t node) {
+    if (mpack_node_bool(node) != true)
+        mpack_node_flag_error(node, mpack_error_type);
+}
+
+void mpack_node_false(mpack_node_t node) {
+    if (mpack_node_bool(node) != false)
+        mpack_node_flag_error(node, mpack_error_type);
+}
+
+uint8_t mpack_node_u8(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= MPACK_UINT8_MAX)
+            return (uint8_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= 0 && node.data->value.i <= MPACK_UINT8_MAX)
+            return (uint8_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+int8_t mpack_node_i8(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= MPACK_INT8_MAX)
+            return (int8_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= MPACK_INT8_MIN && node.data->value.i <= MPACK_INT8_MAX)
+            return (int8_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+uint16_t mpack_node_u16(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= MPACK_UINT16_MAX)
+            return (uint16_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= 0 && node.data->value.i <= MPACK_UINT16_MAX)
+            return (uint16_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+int16_t mpack_node_i16(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= MPACK_INT16_MAX)
+            return (int16_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= MPACK_INT16_MIN && node.data->value.i <= MPACK_INT16_MAX)
+            return (int16_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+uint32_t mpack_node_u32(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= MPACK_UINT32_MAX)
+            return (uint32_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= 0 && node.data->value.i <= MPACK_UINT32_MAX)
+            return (uint32_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+int32_t mpack_node_i32(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= MPACK_INT32_MAX)
+            return (int32_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= MPACK_INT32_MIN && node.data->value.i <= MPACK_INT32_MAX)
+            return (int32_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+uint64_t mpack_node_u64(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        return node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        if (node.data->value.i >= 0)
+            return (uint64_t)node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+int64_t mpack_node_i64(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_uint) {
+        if (node.data->value.u <= (uint64_t)MPACK_INT64_MAX)
+            return (int64_t)node.data->value.u;
+    } else if (node.data->type == mpack_type_int) {
+        return node.data->value.i;
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+unsigned int mpack_node_uint(mpack_node_t node) {
+
+    // This should be true at compile-time, so this just wraps the 32-bit function.
+    if (sizeof(unsigned int) == 4)
+        return (unsigned int)mpack_node_u32(node);
+
+    // Otherwise we use u64 and check the range.
+    uint64_t val = mpack_node_u64(node);
+    if (val <= MPACK_UINT_MAX)
+        return (unsigned int)val;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+int mpack_node_int(mpack_node_t node) {
+
+    // This should be true at compile-time, so this just wraps the 32-bit function.
+    if (sizeof(int) == 4)
+        return (int)mpack_node_i32(node);
+
+    // Otherwise we use i64 and check the range.
+    int64_t val = mpack_node_i64(node);
+    if (val >= MPACK_INT_MIN && val <= MPACK_INT_MAX)
+        return (int)val;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+#if MPACK_FLOAT
+float mpack_node_float(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0.0f;
+
+    if (node.data->type == mpack_type_uint)
+        return (float)node.data->value.u;
+    if (node.data->type == mpack_type_int)
+        return (float)node.data->value.i;
+    if (node.data->type == mpack_type_float)
+        return node.data->value.f;
+
+    if (node.data->type == mpack_type_double) {
+        #if MPACK_DOUBLE
+        return (float)node.data->value.d;
+        #else
+        return mpack_shorten_raw_double_to_float(node.data->value.d);
+        #endif
+    }
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0.0f;
+}
+#endif
+
+#if MPACK_DOUBLE
+double mpack_node_double(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0.0;
+
+    if (node.data->type == mpack_type_uint)
+        return (double)node.data->value.u;
+    else if (node.data->type == mpack_type_int)
+        return (double)node.data->value.i;
+    else if (node.data->type == mpack_type_float)
+        return (double)node.data->value.f;
+    else if (node.data->type == mpack_type_double)
+        return node.data->value.d;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0.0;
+}
+#endif
+
+#if MPACK_FLOAT
+float mpack_node_float_strict(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0.0f;
+
+    if (node.data->type == mpack_type_float)
+        return node.data->value.f;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0.0f;
+}
+#endif
+
+#if MPACK_DOUBLE
+double mpack_node_double_strict(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0.0;
+
+    if (node.data->type == mpack_type_float)
+        return (double)node.data->value.f;
+    else if (node.data->type == mpack_type_double)
+        return node.data->value.d;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0.0;
+}
+#endif
+
+#if !MPACK_FLOAT
+uint32_t mpack_node_raw_float(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_float)
+        return node.data->value.f;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+#endif
+
+#if !MPACK_DOUBLE
+uint64_t mpack_node_raw_double(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_double)
+        return node.data->value.d;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+#endif
+
+#if MPACK_EXTENSIONS
+int8_t mpack_node_exttype(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_ext)
+        return mpack_node_exttype_unchecked(node);
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+#endif
+
+uint32_t mpack_node_data_len(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    mpack_type_t type = node.data->type;
+    if (type == mpack_type_str || type == mpack_type_bin
+            #if MPACK_EXTENSIONS
+            || type == mpack_type_ext
+            #endif
+            )
+        return (uint32_t)node.data->len;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+size_t mpack_node_strlen(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_str)
+        return (size_t)node.data->len;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+const char* mpack_node_str(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    mpack_type_t type = node.data->type;
+    if (type == mpack_type_str)
+        return mpack_node_data_unchecked(node);
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return NULL;
+}
+
+const char* mpack_node_data(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    mpack_type_t type = node.data->type;
+    if (type == mpack_type_str || type == mpack_type_bin
+            #if MPACK_EXTENSIONS
+            || type == mpack_type_ext
+            #endif
+            )
+        return mpack_node_data_unchecked(node);
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return NULL;
+}
+
+const char* mpack_node_bin_data(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return NULL;
+
+    if (node.data->type == mpack_type_bin)
+        return mpack_node_data_unchecked(node);
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return NULL;
+}
+
+size_t mpack_node_bin_size(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type == mpack_type_bin)
+        return (size_t)node.data->len;
+
+    mpack_node_flag_error(node, mpack_error_type);
+    return 0;
+}
+
+size_t mpack_node_array_length(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type != mpack_type_array) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return 0;
+    }
+
+    return (size_t)node.data->len;
+}
+
+mpack_node_t mpack_node_array_at(mpack_node_t node, size_t index) {
+    if (mpack_node_error(node) != mpack_ok)
+        return mpack_tree_nil_node(node.tree);
+
+    if (node.data->type != mpack_type_array) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return mpack_tree_nil_node(node.tree);
+    }
+
+    if (index >= node.data->len) {
+        mpack_node_flag_error(node, mpack_error_data);
+        return mpack_tree_nil_node(node.tree);
+    }
+
+    return mpack_node(node.tree, mpack_node_child(node, index));
+}
+
+size_t mpack_node_map_count(mpack_node_t node) {
+    if (mpack_node_error(node) != mpack_ok)
+        return 0;
+
+    if (node.data->type != mpack_type_map) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return 0;
+    }
+
+    return node.data->len;
+}
+
+// internal node map lookup
+static mpack_node_t mpack_node_map_at(mpack_node_t node, size_t index, size_t offset) {
+    if (mpack_node_error(node) != mpack_ok)
+        return mpack_tree_nil_node(node.tree);
+
+    if (node.data->type != mpack_type_map) {
+        mpack_node_flag_error(node, mpack_error_type);
+        return mpack_tree_nil_node(node.tree);
+    }
+
+    if (index >= node.data->len) {
+        mpack_node_flag_error(node, mpack_error_data);
+        return mpack_tree_nil_node(node.tree);
+    }
+
+    return mpack_node(node.tree, mpack_node_child(node, index * 2 + offset));
+}
+
+mpack_node_t mpack_node_map_key_at(mpack_node_t node, size_t index) {
+    return mpack_node_map_at(node, index, 0);
+}
+
+mpack_node_t mpack_node_map_value_at(mpack_node_t node, size_t index) {
+    return mpack_node_map_at(node, index, 1);
+}
+
+#endif
+
+}  // namespace wpi
+MPACK_SILENCE_WARNINGS_END
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_istream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_istream.cpp
index f63c217..a179b98 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_istream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_istream.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #define _CRT_NONSTDC_NO_WARNINGS
 
@@ -17,10 +14,10 @@
 
 #include <cstdlib>
 #include <cstring>
+#include <string_view>
 
-#include "wpi/FileSystem.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
+#include "wpi/fs.h"
 
 #if defined(_MSC_VER)
 #ifndef STDIN_FILENO
@@ -36,22 +33,30 @@
 
 using namespace wpi;
 
-StringRef raw_istream::getline(SmallVectorImpl<char>& buf, int maxLen) {
+std::string_view raw_istream::getline(SmallVectorImpl<char>& buf, int maxLen) {
   buf.clear();
   for (int i = 0; i < maxLen; ++i) {
     char c;
     read(c);
-    if (has_error()) return StringRef{buf.data(), buf.size()};
-    if (c == '\r') continue;
+    if (has_error()) {
+      return {buf.data(), buf.size()};
+    }
+    if (c == '\r') {
+      continue;
+    }
     buf.push_back(c);
-    if (c == '\n') break;
+    if (c == '\n') {
+      break;
+    }
   }
-  return StringRef{buf.data(), buf.size()};
+  return {buf.data(), buf.size()};
 }
 
 void raw_mem_istream::close() {}
 
-size_t raw_mem_istream::in_avail() const { return m_left; }
+size_t raw_mem_istream::in_avail() const {
+  return m_left;
+}
 
 void raw_mem_istream::read_impl(void* data, size_t len) {
   if (len > m_left) {
@@ -64,25 +69,27 @@
   set_read_count(len);
 }
 
-static int getFD(const Twine& Filename, std::error_code& EC) {
+static int getFD(std::string_view Filename, std::error_code& EC) {
   // Handle "-" as stdin. Note that when we do this, we consider ourself
   // the owner of stdin. This means that we can do things like close the
   // file descriptor when we're done and set the "binary" flag globally.
-  if (Filename.isSingleStringRef() && Filename.getSingleStringRef() == "-") {
+  if (Filename == "-") {
     EC = std::error_code();
     return STDIN_FILENO;
   }
 
-  int FD;
-
-  EC = sys::fs::openFileForRead(Filename, FD);
-  if (EC) return -1;
-
-  EC = std::error_code();
+  fs::file_t F = fs::OpenFileForRead(Filename, EC);
+  if (EC) {
+    return -1;
+  }
+  int FD = fs::FileToFd(F, EC, fs::OF_None);
+  if (EC) {
+    return -1;
+  }
   return FD;
 }
 
-raw_fd_istream::raw_fd_istream(const Twine& filename, std::error_code& ec,
+raw_fd_istream::raw_fd_istream(std::string_view filename, std::error_code& ec,
                                size_t bufSize)
     : raw_fd_istream(getFD(filename, ec), true, bufSize) {}
 
@@ -92,7 +99,9 @@
 }
 
 raw_fd_istream::~raw_fd_istream() {
-  if (m_shouldClose) close();
+  if (m_shouldClose) {
+    close();
+  }
   std::free(m_buf);
 }
 
@@ -103,7 +112,9 @@
   }
 }
 
-size_t raw_fd_istream::in_avail() const { return m_end - m_cur; }
+size_t raw_fd_istream::in_avail() const {
+  return m_end - m_cur;
+}
 
 void raw_fd_istream::read_impl(void* data, size_t len) {
   char* cdata = static_cast<char*>(data);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_istream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_istream.cpp
index 68c4dd6..c6b759e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_istream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_istream.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/raw_socket_istream.h"
 
@@ -27,6 +24,10 @@
   set_read_count(pos);
 }
 
-void raw_socket_istream::close() { m_stream.close(); }
+void raw_socket_istream::close() {
+  m_stream.close();
+}
 
-size_t raw_socket_istream::in_avail() const { return 0; }
+size_t raw_socket_istream::in_avail() const {
+  return 0;
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_ostream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_ostream.cpp
index fb8d94d..af01c84 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_ostream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_socket_ostream.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/raw_socket_ostream.h"
 
@@ -13,7 +10,9 @@
 
 raw_socket_ostream::~raw_socket_ostream() {
   flush();
-  if (m_shouldClose) close();
+  if (m_shouldClose) {
+    close();
+  }
 }
 
 void raw_socket_ostream::write_impl(const char* data, size_t len) {
@@ -30,10 +29,14 @@
   }
 }
 
-uint64_t raw_socket_ostream::current_pos() const { return 0; }
+uint64_t raw_socket_ostream::current_pos() const {
+  return 0;
+}
 
 void raw_socket_ostream::close() {
-  if (!m_shouldClose) return;
+  if (!m_shouldClose) {
+    return;
+  }
   flush();
   m_stream.close();
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_uv_ostream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_uv_ostream.cpp
index 8fc103d..f055a2a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_uv_ostream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/raw_uv_ostream.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/raw_uv_ostream.h"
 
@@ -35,6 +32,8 @@
 
 uint64_t raw_uv_ostream::current_pos() const {
   uint64_t size = 0;
-  for (auto&& buf : m_bufs) size += buf.len;
+  for (auto&& buf : m_bufs) {
+    size += buf.len;
+  }
   return size;
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/sendable/SendableRegistry.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/sendable/SendableRegistry.cpp
new file mode 100644
index 0000000..3ad39e5
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/sendable/SendableRegistry.cpp
@@ -0,0 +1,434 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/sendable/SendableRegistry.h"
+
+#include <memory>
+
+#include "fmt/format.h"
+#include "wpi/DenseMap.h"
+#include "wpi/SmallVector.h"
+#include "wpi/UidVector.h"
+#include "wpi/mutex.h"
+#include "wpi/sendable/Sendable.h"
+#include "wpi/sendable/SendableBuilder.h"
+
+using namespace wpi;
+
+namespace {
+struct Component {
+  Sendable* sendable = nullptr;
+  std::unique_ptr<SendableBuilder> builder;
+  std::string name;
+  std::string subsystem = "Ungrouped";
+  Sendable* parent = nullptr;
+  bool liveWindow = false;
+  wpi::SmallVector<std::shared_ptr<void>, 2> data;
+
+  void SetName(std::string_view moduleType, int channel) {
+    name = fmt::format("{}[{}]", moduleType, channel);
+  }
+
+  void SetName(std::string_view moduleType, int moduleNumber, int channel) {
+    name = fmt::format("{}[{},{}]", moduleType, moduleNumber, channel);
+  }
+};
+
+struct SendableRegistryInst {
+  wpi::recursive_mutex mutex;
+
+  std::function<std::unique_ptr<SendableBuilder>()> liveWindowFactory;
+  wpi::UidVector<std::unique_ptr<Component>, 32> components;
+  wpi::DenseMap<void*, SendableRegistry::UID> componentMap;
+  int nextDataHandle = 0;
+
+  Component& GetOrAdd(void* sendable, SendableRegistry::UID* uid = nullptr);
+};
+}  // namespace
+
+Component& SendableRegistryInst::GetOrAdd(void* sendable,
+                                          SendableRegistry::UID* uid) {
+  SendableRegistry::UID& compUid = componentMap[sendable];
+  if (compUid == 0) {
+    compUid = components.emplace_back(std::make_unique<Component>()) + 1;
+  }
+  if (uid) {
+    *uid = compUid;
+  }
+
+  return *components[compUid - 1];
+}
+
+static SendableRegistryInst& GetInstance() {
+  static SendableRegistryInst instance;
+  return instance;
+}
+
+void SendableRegistry::SetLiveWindowBuilderFactory(
+    std::function<std::unique_ptr<SendableBuilder>()> factory) {
+  GetInstance().liveWindowFactory = std::move(factory);
+}
+
+void SendableRegistry::Add(Sendable* sendable, std::string_view name) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  comp.name = name;
+}
+
+void SendableRegistry::Add(Sendable* sendable, std::string_view moduleType,
+                           int channel) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  comp.SetName(moduleType, channel);
+}
+
+void SendableRegistry::Add(Sendable* sendable, std::string_view moduleType,
+                           int moduleNumber, int channel) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  comp.SetName(moduleType, moduleNumber, channel);
+}
+
+void SendableRegistry::Add(Sendable* sendable, std::string_view subsystem,
+                           std::string_view name) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  comp.name = name;
+  comp.subsystem = subsystem;
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, std::string_view name) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  if (inst.liveWindowFactory) {
+    comp.builder = inst.liveWindowFactory();
+  }
+  comp.liveWindow = true;
+  comp.name = name;
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, std::string_view moduleType,
+                             int channel) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  if (inst.liveWindowFactory) {
+    comp.builder = inst.liveWindowFactory();
+  }
+  comp.liveWindow = true;
+  comp.SetName(moduleType, channel);
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, std::string_view moduleType,
+                             int moduleNumber, int channel) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  if (inst.liveWindowFactory) {
+    comp.builder = inst.liveWindowFactory();
+  }
+  comp.liveWindow = true;
+  comp.SetName(moduleType, moduleNumber, channel);
+}
+
+void SendableRegistry::AddLW(Sendable* sendable, std::string_view subsystem,
+                             std::string_view name) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(sendable);
+  comp.sendable = sendable;
+  if (inst.liveWindowFactory) {
+    comp.builder = inst.liveWindowFactory();
+  }
+  comp.liveWindow = true;
+  comp.name = name;
+  comp.subsystem = subsystem;
+}
+
+void SendableRegistry::AddChild(Sendable* parent, Sendable* child) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(child);
+  comp.parent = parent;
+}
+
+void SendableRegistry::AddChild(Sendable* parent, void* child) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto& comp = inst.GetOrAdd(child);
+  comp.parent = parent;
+}
+
+bool SendableRegistry::Remove(Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end()) {
+    return false;
+  }
+  UID compUid = it->getSecond();
+  inst.components.erase(compUid - 1);
+  inst.componentMap.erase(it);
+  // update any parent pointers
+  for (auto&& comp : inst.components) {
+    if (comp->parent == sendable) {
+      comp->parent = nullptr;
+    }
+  }
+  return true;
+}
+
+void SendableRegistry::Move(Sendable* to, Sendable* from) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(from);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  UID compUid = it->getSecond();
+  inst.componentMap.erase(it);
+  inst.componentMap[to] = compUid;
+  auto& comp = *inst.components[compUid - 1];
+  comp.sendable = to;
+  if (comp.builder && comp.builder->IsPublished()) {
+    // rebuild builder, as lambda captures can point to "from"
+    comp.builder->ClearProperties();
+    to->InitSendable(*comp.builder);
+  }
+  // update any parent pointers
+  for (auto&& comp : inst.components) {
+    if (comp->parent == from) {
+      comp->parent = to;
+    }
+  }
+}
+
+bool SendableRegistry::Contains(const Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  return inst.componentMap.count(sendable) != 0;
+}
+
+std::string SendableRegistry::GetName(const Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return {};
+  }
+  return inst.components[it->getSecond() - 1]->name;
+}
+
+void SendableRegistry::SetName(Sendable* sendable, std::string_view name) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  inst.components[it->getSecond() - 1]->name = name;
+}
+
+void SendableRegistry::SetName(Sendable* sendable, std::string_view moduleType,
+                               int channel) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  inst.components[it->getSecond() - 1]->SetName(moduleType, channel);
+}
+
+void SendableRegistry::SetName(Sendable* sendable, std::string_view moduleType,
+                               int moduleNumber, int channel) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  inst.components[it->getSecond() - 1]->SetName(moduleType, moduleNumber,
+                                                channel);
+}
+
+void SendableRegistry::SetName(Sendable* sendable, std::string_view subsystem,
+                               std::string_view name) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  auto& comp = *inst.components[it->getSecond() - 1];
+  comp.name = name;
+  comp.subsystem = subsystem;
+}
+
+std::string SendableRegistry::GetSubsystem(const Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return {};
+  }
+  return inst.components[it->getSecond() - 1]->subsystem;
+}
+
+void SendableRegistry::SetSubsystem(Sendable* sendable,
+                                    std::string_view subsystem) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  inst.components[it->getSecond() - 1]->subsystem = subsystem;
+}
+
+int SendableRegistry::GetDataHandle() {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  return inst.nextDataHandle++;
+}
+
+std::shared_ptr<void> SendableRegistry::SetData(Sendable* sendable, int handle,
+                                                std::shared_ptr<void> data) {
+  auto& inst = GetInstance();
+  assert(handle >= 0);
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return nullptr;
+  }
+  auto& comp = *inst.components[it->getSecond() - 1];
+  std::shared_ptr<void> rv;
+  if (static_cast<size_t>(handle) < comp.data.size()) {
+    rv = std::move(comp.data[handle]);
+  } else {
+    comp.data.resize(handle + 1);
+  }
+  comp.data[handle] = std::move(data);
+  return rv;
+}
+
+std::shared_ptr<void> SendableRegistry::GetData(Sendable* sendable,
+                                                int handle) {
+  auto& inst = GetInstance();
+  assert(handle >= 0);
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return nullptr;
+  }
+  auto& comp = *inst.components[it->getSecond() - 1];
+  if (static_cast<size_t>(handle) >= comp.data.size()) {
+    return nullptr;
+  }
+  return comp.data[handle];
+}
+
+void SendableRegistry::EnableLiveWindow(Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  inst.components[it->getSecond() - 1]->liveWindow = true;
+}
+
+void SendableRegistry::DisableLiveWindow(Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  auto it = inst.componentMap.find(sendable);
+  if (it == inst.componentMap.end() || !inst.components[it->getSecond() - 1]) {
+    return;
+  }
+  inst.components[it->getSecond() - 1]->liveWindow = false;
+}
+
+SendableRegistry::UID SendableRegistry::GetUniqueId(Sendable* sendable) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  UID uid;
+  auto& comp = inst.GetOrAdd(sendable, &uid);
+  comp.sendable = sendable;
+  return uid;
+}
+
+Sendable* SendableRegistry::GetSendable(UID uid) {
+  auto& inst = GetInstance();
+  if (uid == 0) {
+    return nullptr;
+  }
+  std::scoped_lock lock(inst.mutex);
+  if ((uid - 1) >= inst.components.size() || !inst.components[uid - 1]) {
+    return nullptr;
+  }
+  return inst.components[uid - 1]->sendable;
+}
+
+void SendableRegistry::Publish(UID sendableUid,
+                               std::unique_ptr<SendableBuilder> builder) {
+  auto& inst = GetInstance();
+  std::scoped_lock lock(inst.mutex);
+  if (sendableUid == 0 || (sendableUid - 1) >= inst.components.size() ||
+      !inst.components[sendableUid - 1]) {
+    return;
+  }
+  auto& comp = *inst.components[sendableUid - 1];
+  comp.builder = std::move(builder);  // clear any current builder
+  comp.sendable->InitSendable(*comp.builder);
+  comp.builder->Update();
+}
+
+void SendableRegistry::Update(UID sendableUid) {
+  auto& inst = GetInstance();
+  if (sendableUid == 0) {
+    return;
+  }
+  std::scoped_lock lock(inst.mutex);
+  if ((sendableUid - 1) >= inst.components.size() ||
+      !inst.components[sendableUid - 1]) {
+    return;
+  }
+  if (inst.components[sendableUid - 1]->builder) {
+    inst.components[sendableUid - 1]->builder->Update();
+  }
+}
+
+void SendableRegistry::ForeachLiveWindow(
+    int dataHandle, wpi::function_ref<void(CallbackData& data)> callback) {
+  auto& inst = GetInstance();
+  assert(dataHandle >= 0);
+  std::scoped_lock lock(inst.mutex);
+  wpi::SmallVector<Component*, 128> components;
+  for (auto&& comp : inst.components) {
+    components.emplace_back(comp.get());
+  }
+  for (auto comp : components) {
+    if (comp && comp->builder && comp->sendable && comp->liveWindow) {
+      if (static_cast<size_t>(dataHandle) >= comp->data.size()) {
+        comp->data.resize(dataHandle + 1);
+      }
+      CallbackData cbdata{comp->sendable,         comp->name,
+                          comp->subsystem,        comp->parent,
+                          comp->data[dataHandle], *comp->builder};
+      callback(cbdata);
+    }
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/sha1.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/sha1.cpp
index e1346f4..8ec7cb1 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/sha1.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/sha1.cpp
@@ -212,10 +212,12 @@
   }
 }
 
-SHA1::SHA1() { reset(digest, buf_size, transforms); }
+SHA1::SHA1() {
+  reset(digest, buf_size, transforms);
+}
 
-void SHA1::Update(StringRef s) {
-  raw_mem_istream is(makeArrayRef(s.data(), s.size()));
+void SHA1::Update(std::string_view s) {
+  raw_mem_istream is(span<const char>(s.data(), s.size()));
   Update(is);
 }
 
@@ -291,7 +293,7 @@
   return os.str();
 }
 
-StringRef SHA1::Final(SmallVectorImpl<char>& buf) {
+std::string_view SHA1::Final(SmallVectorImpl<char>& buf) {
   raw_svector_ostream os(buf);
 
   finalize(digest, buffer, buf_size, transforms, os, true);
@@ -299,7 +301,7 @@
   return os.str();
 }
 
-StringRef SHA1::RawFinal(SmallVectorImpl<char>& buf) {
+std::string_view SHA1::RawFinal(SmallVectorImpl<char>& buf) {
   raw_svector_ostream os(buf);
 
   finalize(digest, buffer, buf_size, transforms, os, false);
@@ -307,7 +309,7 @@
   return os.str();
 }
 
-std::string SHA1::FromFile(StringRef filename) {
+std::string SHA1::FromFile(std::string_view filename) {
   std::error_code ec;
   raw_fd_istream stream(filename, ec);
   SHA1 checksum;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp
index 7f3b0cf..9c32bf3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/timestamp.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/timestamp.h"
 
@@ -19,7 +16,7 @@
 #endif
 
 // offset in microseconds
-static uint64_t zerotime() {
+static uint64_t zerotime() noexcept {
 #ifdef _WIN32
   FILETIME ft;
   uint64_t tmpres = 0;
@@ -43,7 +40,7 @@
 #endif
 }
 
-static uint64_t timestamp() {
+static uint64_t timestamp() noexcept {
 #ifdef _WIN32
   LARGE_INTEGER li;
   QueryPerformanceCounter(&li);
@@ -95,14 +92,22 @@
   now_impl = func ? func : NowDefault;
 }
 
-uint64_t wpi::Now() { return (now_impl.load())(); }
+uint64_t wpi::Now() {
+  return (now_impl.load())();
+}
 
 extern "C" {
 
-uint64_t WPI_NowDefault(void) { return wpi::NowDefault(); }
+uint64_t WPI_NowDefault(void) {
+  return wpi::NowDefault();
+}
 
-void WPI_SetNowImpl(uint64_t (*func)(void)) { wpi::SetNowImpl(func); }
+void WPI_SetNowImpl(uint64_t (*func)(void)) {
+  wpi::SetNowImpl(func);
+}
 
-uint64_t WPI_Now(void) { return wpi::Now(); }
+uint64_t WPI_Now(void) {
+  return wpi::Now();
+}
 
 }  // extern "C"
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Async.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Async.cpp
index 5479f49..c9d698e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Async.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Async.cpp
@@ -1,22 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Async.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 Async<>::~Async() noexcept {
-  if (auto loop = m_loop.lock())
+  if (auto loop = m_loop.lock()) {
     Close();
-  else
+  } else {
     ForceClosed();
+  }
 }
 
 std::shared_ptr<Async<>> Async<>::Create(const std::shared_ptr<Loop>& loop) {
@@ -33,5 +30,4 @@
   return h;
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Check.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Check.cpp
index 0f4cccf..97265d4 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Check.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Check.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Check.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Check> Check::Create(Loop& loop) {
   auto h = std::make_shared<Check>(private_init{});
@@ -30,5 +26,4 @@
   });
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/FsEvent.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/FsEvent.cpp
index 54ba31f..3c83d1d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/FsEvent.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/FsEvent.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/FsEvent.h"
 
@@ -12,8 +9,7 @@
 #include "wpi/SmallString.h"
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<FsEvent> FsEvent::Create(Loop& loop) {
   auto h = std::make_shared<FsEvent>(private_init{});
@@ -26,18 +22,19 @@
   return h;
 }
 
-void FsEvent::Start(const Twine& path, unsigned int flags) {
-  SmallString<128> pathBuf;
+void FsEvent::Start(std::string_view path, unsigned int flags) {
+  SmallString<128> pathBuf{path};
   Invoke(
       &uv_fs_event_start, GetRaw(),
       [](uv_fs_event_t* handle, const char* filename, int events, int status) {
         FsEvent& h = *static_cast<FsEvent*>(handle->data);
-        if (status < 0)
+        if (status < 0) {
           h.ReportError(status);
-        else
+        } else {
           h.fsEvent(filename, events);
+        }
       },
-      path.toNullTerminatedStringRef(pathBuf).data(), flags);
+      pathBuf.c_str(), flags);
 }
 
 std::string FsEvent::GetPath() {
@@ -63,5 +60,4 @@
   return std::string{};
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetAddrInfo.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetAddrInfo.cpp
index 21f6404..2e6e38f 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetAddrInfo.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetAddrInfo.cpp
@@ -1,55 +1,51 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/GetAddrInfo.h"
 
+#include "wpi/SmallString.h"
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/util.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 GetAddrInfoReq::GetAddrInfoReq() {
   error = [this](Error err) { GetLoop().error(err); };
 }
 
 void GetAddrInfo(Loop& loop, const std::shared_ptr<GetAddrInfoReq>& req,
-                 const Twine& node, const Twine& service,
+                 std::string_view node, std::string_view service,
                  const addrinfo* hints) {
-  SmallVector<char, 128> nodeStr;
-  SmallVector<char, 128> serviceStr;
+  SmallString<128> nodeStr{node};
+  SmallString<128> serviceStr{service};
   int err = uv_getaddrinfo(
       loop.GetRaw(), req->GetRaw(),
       [](uv_getaddrinfo_t* req, int status, addrinfo* res) {
         auto& h = *static_cast<GetAddrInfoReq*>(req->data);
-        if (status < 0)
+        if (status < 0) {
           h.ReportError(status);
-        else
+        } else {
           h.resolved(*res);
+        }
         uv_freeaddrinfo(res);
         h.Release();  // this is always a one-shot
       },
-      node.isNull() ? nullptr : node.toNullTerminatedStringRef(nodeStr).data(),
-      service.isNull() ? nullptr
-                       : service.toNullTerminatedStringRef(serviceStr).data(),
-      hints);
-  if (err < 0)
+      node.empty() ? nullptr : nodeStr.c_str(),
+      service.empty() ? nullptr : serviceStr.c_str(), hints);
+  if (err < 0) {
     loop.ReportError(err);
-  else
+  } else {
     req->Keep();
+  }
 }
 
 void GetAddrInfo(Loop& loop, std::function<void(const addrinfo&)> callback,
-                 const Twine& node, const Twine& service,
+                 std::string_view node, std::string_view service,
                  const addrinfo* hints) {
   auto req = std::make_shared<GetAddrInfoReq>();
-  req->resolved.connect(callback);
+  req->resolved.connect(std::move(callback));
   GetAddrInfo(loop, req, node, service, hints);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp
index 6a4446f..4e662f3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/GetNameInfo.cpp
@@ -1,17 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/GetNameInfo.h"
 
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/util.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 GetNameInfoReq::GetNameInfoReq() {
   error = [this](Error err) { GetLoop().error(err); };
@@ -24,68 +20,75 @@
       [](uv_getnameinfo_t* req, int status, const char* hostname,
          const char* service) {
         auto& h = *static_cast<GetNameInfoReq*>(req->data);
-        if (status < 0)
+        if (status < 0) {
           h.ReportError(status);
-        else
+        } else {
           h.resolved(hostname, service);
+        }
         h.Release();  // this is always a one-shot
       },
       &addr, flags);
-  if (err < 0)
+  if (err < 0) {
     loop.ReportError(err);
-  else
+  } else {
     req->Keep();
+  }
 }
 
 void GetNameInfo(Loop& loop,
                  std::function<void(const char*, const char*)> callback,
                  const sockaddr& addr, int flags) {
   auto req = std::make_shared<GetNameInfoReq>();
-  req->resolved.connect(callback);
+  req->resolved.connect(std::move(callback));
   GetNameInfo(loop, req, addr, flags);
 }
 
 void GetNameInfo4(Loop& loop, const std::shared_ptr<GetNameInfoReq>& req,
-                  const Twine& ip, unsigned int port, int flags) {
+                  std::string_view ip, unsigned int port, int flags) {
   sockaddr_in addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     loop.ReportError(err);
-  else
+  } else {
     GetNameInfo(loop, req, reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
 void GetNameInfo4(Loop& loop,
                   std::function<void(const char*, const char*)> callback,
-                  const Twine& ip, unsigned int port, int flags) {
+                  std::string_view ip, unsigned int port, int flags) {
   sockaddr_in addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     loop.ReportError(err);
-  else
-    GetNameInfo(loop, callback, reinterpret_cast<const sockaddr&>(addr), flags);
+  } else {
+    GetNameInfo(loop, std::move(callback),
+                reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
 void GetNameInfo6(Loop& loop, const std::shared_ptr<GetNameInfoReq>& req,
-                  const Twine& ip, unsigned int port, int flags) {
+                  std::string_view ip, unsigned int port, int flags) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     loop.ReportError(err);
-  else
+  } else {
     GetNameInfo(loop, req, reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
 void GetNameInfo6(Loop& loop,
                   std::function<void(const char*, const char*)> callback,
-                  const Twine& ip, unsigned int port, int flags) {
+                  std::string_view ip, unsigned int port, int flags) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     loop.ReportError(err);
-  else
-    GetNameInfo(loop, callback, reinterpret_cast<const sockaddr&>(addr), flags);
+  } else {
+    GetNameInfo(loop, std::move(callback),
+                reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Handle.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Handle.cpp
index 93ef423..74c4c60 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Handle.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Handle.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Handle.h"
 
@@ -11,9 +8,9 @@
 
 Handle::~Handle() noexcept {
   if (!m_closed && m_uv_handle->type != UV_UNKNOWN_HANDLE) {
-    uv_close(m_uv_handle, [](uv_handle_t* uv_handle) { delete uv_handle; });
+    uv_close(m_uv_handle, [](uv_handle_t* uv_handle) { std::free(uv_handle); });
   } else {
-    delete m_uv_handle;
+    std::free(m_uv_handle);
   }
 }
 
@@ -33,4 +30,6 @@
   *buf = h.m_allocBuf(size);
 }
 
-void Handle::DefaultFreeBuf(Buffer& buf) { buf.Deallocate(); }
+void Handle::DefaultFreeBuf(Buffer& buf) {
+  buf.Deallocate();
+}
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Idle.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Idle.cpp
index 9eae218..6bf8602 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Idle.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Idle.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Idle.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Idle> Idle::Create(Loop& loop) {
   auto h = std::make_shared<Idle>(private_init{});
@@ -30,5 +26,4 @@
   });
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Loop.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Loop.cpp
index a31ad09..c5b7163 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Loop.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Loop.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Loop.h"
 
@@ -29,7 +26,9 @@
 
 std::shared_ptr<Loop> Loop::Create() {
   auto loop = std::make_shared<Loop>(private_init{});
-  if (uv_loop_init(&loop->m_loopStruct) < 0) return nullptr;
+  if (uv_loop_init(&loop->m_loopStruct) < 0) {
+    return nullptr;
+  }
   loop->m_loop = &loop->m_loopStruct;
   loop->m_loop->data = loop.get();
   return loop;
@@ -38,22 +37,26 @@
 std::shared_ptr<Loop> Loop::GetDefault() {
   static std::shared_ptr<Loop> loop = std::make_shared<Loop>(private_init{});
   loop->m_loop = uv_default_loop();
-  if (!loop->m_loop) return nullptr;
+  if (!loop->m_loop) {
+    return nullptr;
+  }
   loop->m_loop->data = loop.get();
   return loop;
 }
 
 void Loop::Close() {
   int err = uv_loop_close(m_loop);
-  if (err < 0) ReportError(err);
+  if (err < 0) {
+    ReportError(err);
+  }
 }
 
-void Loop::Walk(std::function<void(Handle&)> callback) {
+void Loop::Walk(function_ref<void(Handle&)> callback) {
   uv_walk(
       m_loop,
       [](uv_handle_t* handle, void* func) {
         auto& h = *static_cast<Handle*>(handle->data);
-        auto& f = *static_cast<std::function<void(Handle&)>*>(func);
+        auto& f = *static_cast<function_ref<void(Handle&)>*>(func);
         f(h);
       },
       &callback);
@@ -61,5 +64,7 @@
 
 void Loop::Fork() {
   int err = uv_loop_fork(m_loop);
-  if (err < 0) ReportError(err);
+  if (err < 0) {
+    ReportError(err);
+  }
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NameToAddr.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NameToAddr.cpp
index b407c2b..23ec6da 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NameToAddr.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NameToAddr.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/util.h"  // NOLINT(build/include_order)
 
@@ -11,58 +8,52 @@
 
 #include "wpi/SmallString.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-int NameToAddr(const Twine& ip, unsigned int port, sockaddr_in* addr) {
-  SmallString<128> tmp;
-  StringRef ipStr = ip.toNullTerminatedStringRef(tmp);
-  if (ipStr.empty()) {
+int NameToAddr(std::string_view ip, unsigned int port, sockaddr_in* addr) {
+  if (ip.empty()) {
     std::memset(addr, 0, sizeof(sockaddr_in));
     addr->sin_family = PF_INET;
     addr->sin_addr.s_addr = INADDR_ANY;
     addr->sin_port = htons(port);
     return 0;
   } else {
-    return uv_ip4_addr(ipStr.data(), port, addr);
+    SmallString<128> ipBuf{ip};
+    return uv_ip4_addr(ipBuf.c_str(), port, addr);
   }
 }
 
-int NameToAddr(const Twine& ip, unsigned int port, sockaddr_in6* addr) {
-  SmallString<128> tmp;
-  StringRef ipStr = ip.toNullTerminatedStringRef(tmp);
-  if (ipStr.empty()) {
+int NameToAddr(std::string_view ip, unsigned int port, sockaddr_in6* addr) {
+  if (ip.empty()) {
     std::memset(addr, 0, sizeof(sockaddr_in6));
     addr->sin6_family = PF_INET6;
     addr->sin6_addr = in6addr_any;
     addr->sin6_port = htons(port);
     return 0;
   } else {
-    return uv_ip6_addr(ipStr.data(), port, addr);
+    SmallString<128> ipBuf{ip};
+    return uv_ip6_addr(ipBuf.c_str(), port, addr);
   }
 }
 
-int NameToAddr(const Twine& ip, in_addr* addr) {
-  SmallString<128> tmp;
-  StringRef ipStr = ip.toNullTerminatedStringRef(tmp);
-  if (ipStr.empty()) {
+int NameToAddr(std::string_view ip, in_addr* addr) {
+  if (ip.empty()) {
     addr->s_addr = INADDR_ANY;
     return 0;
   } else {
-    return uv_inet_pton(AF_INET, ipStr.data(), addr);
+    SmallString<128> ipBuf{ip};
+    return uv_inet_pton(AF_INET, ipBuf.c_str(), addr);
   }
 }
 
-int NameToAddr(const Twine& ip, in6_addr* addr) {
-  SmallString<128> tmp;
-  StringRef ipStr = ip.toNullTerminatedStringRef(tmp);
-  if (ipStr.empty()) {
+int NameToAddr(std::string_view ip, in6_addr* addr) {
+  if (ip.empty()) {
     *addr = in6addr_any;
     return 0;
   } else {
-    return uv_inet_pton(AF_INET6, ipStr.data(), addr);
+    SmallString<128> ipBuf{ip};
+    return uv_inet_pton(AF_INET6, ipBuf.c_str(), addr);
   }
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NetworkStream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NetworkStream.cpp
index 6e327a7..0bc3337 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NetworkStream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/NetworkStream.cpp
@@ -1,14 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/NetworkStream.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 ConnectReq::ConnectReq() {
   error = [this](Error err) { GetStream().error(err); };
@@ -18,17 +14,17 @@
   Invoke(&uv_listen, GetRawStream(), backlog,
          [](uv_stream_t* handle, int status) {
            auto& h = *static_cast<NetworkStream*>(handle->data);
-           if (status < 0)
+           if (status < 0) {
              h.ReportError(status);
-           else
+           } else {
              h.connection();
+           }
          });
 }
 
 void NetworkStream::Listen(std::function<void()> callback, int backlog) {
-  connection.connect(callback);
+  connection.connect(std::move(callback));
   Listen(backlog);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Pipe.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Pipe.cpp
index 9db879a..b5ca673 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Pipe.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Pipe.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Pipe.h"
 
@@ -11,8 +8,7 @@
 
 #include "wpi/SmallString.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Pipe> Pipe::Create(Loop& loop, bool ipc) {
   auto h = std::make_shared<Pipe>(private_init{});
@@ -26,13 +22,19 @@
 }
 
 void Pipe::Reuse(std::function<void()> callback, bool ipc) {
-  if (IsClosing()) return;
-  if (!m_reuseData) m_reuseData = std::make_unique<ReuseData>();
-  m_reuseData->callback = callback;
+  if (IsClosing()) {
+    return;
+  }
+  if (!m_reuseData) {
+    m_reuseData = std::make_unique<ReuseData>();
+  }
+  m_reuseData->callback = std::move(callback);
   m_reuseData->ipc = ipc;
   uv_close(GetRawHandle(), [](uv_handle_t* handle) {
     Pipe& h = *static_cast<Pipe*>(handle->data);
-    if (!h.m_reuseData) return;
+    if (!h.m_reuseData) {
+      return;
+    }
     auto data = std::move(h.m_reuseData);
     auto err =
         uv_pipe_init(h.GetLoopRef().GetRaw(), h.GetRaw(), data->ipc ? 1 : 0);
@@ -46,7 +48,9 @@
 
 std::shared_ptr<Pipe> Pipe::Accept() {
   auto client = Create(GetLoopRef(), GetRaw()->ipc);
-  if (!client) return nullptr;
+  if (!client) {
+    return nullptr;
+  }
   if (!Accept(client)) {
     client->Release();
     return nullptr;
@@ -54,33 +58,34 @@
   return client;
 }
 
-Pipe* Pipe::DoAccept() { return Accept().get(); }
-
-void Pipe::Bind(const Twine& name) {
-  SmallString<128> nameBuf;
-  Invoke(&uv_pipe_bind, GetRaw(),
-         name.toNullTerminatedStringRef(nameBuf).data());
+Pipe* Pipe::DoAccept() {
+  return Accept().get();
 }
 
-void Pipe::Connect(const Twine& name,
+void Pipe::Bind(std::string_view name) {
+  SmallString<128> nameBuf{name};
+  Invoke(&uv_pipe_bind, GetRaw(), nameBuf.c_str());
+}
+
+void Pipe::Connect(std::string_view name,
                    const std::shared_ptr<PipeConnectReq>& req) {
-  SmallString<128> nameBuf;
-  uv_pipe_connect(req->GetRaw(), GetRaw(),
-                  name.toNullTerminatedStringRef(nameBuf).data(),
+  SmallString<128> nameBuf{name};
+  uv_pipe_connect(req->GetRaw(), GetRaw(), nameBuf.c_str(),
                   [](uv_connect_t* req, int status) {
                     auto& h = *static_cast<PipeConnectReq*>(req->data);
-                    if (status < 0)
+                    if (status < 0) {
                       h.ReportError(status);
-                    else
+                    } else {
                       h.connected();
+                    }
                     h.Release();  // this is always a one-shot
                   });
   req->Keep();
 }
 
-void Pipe::Connect(const Twine& name, std::function<void()> callback) {
+void Pipe::Connect(std::string_view name, std::function<void()> callback) {
   auto req = std::make_shared<PipeConnectReq>();
-  req->connected.connect(callback);
+  req->connected.connect(std::move(callback));
   Connect(name, req);
 }
 
@@ -130,5 +135,4 @@
   return std::string{};
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Poll.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Poll.cpp
index 8b608cb..090a40b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Poll.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Poll.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Poll.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Poll> Poll::Create(Loop& loop, int fd) {
   auto h = std::make_shared<Poll>(private_init{});
@@ -35,14 +31,20 @@
 }
 
 void Poll::Reuse(int fd, std::function<void()> callback) {
-  if (IsClosing()) return;
-  if (!m_reuseData) m_reuseData = std::make_unique<ReuseData>();
-  m_reuseData->callback = callback;
+  if (IsClosing()) {
+    return;
+  }
+  if (!m_reuseData) {
+    m_reuseData = std::make_unique<ReuseData>();
+  }
+  m_reuseData->callback = std::move(callback);
   m_reuseData->isSocket = false;
   m_reuseData->fd = fd;
   uv_close(GetRawHandle(), [](uv_handle_t* handle) {
     Poll& h = *static_cast<Poll*>(handle->data);
-    if (!h.m_reuseData || h.m_reuseData->isSocket) return;  // just in case
+    if (!h.m_reuseData || h.m_reuseData->isSocket) {
+      return;  // just in case
+    }
     auto data = std::move(h.m_reuseData);
     int err = uv_poll_init(h.GetLoopRef().GetRaw(), h.GetRaw(), data->fd);
     if (err < 0) {
@@ -54,14 +56,20 @@
 }
 
 void Poll::ReuseSocket(uv_os_sock_t sock, std::function<void()> callback) {
-  if (IsClosing()) return;
-  if (!m_reuseData) m_reuseData = std::make_unique<ReuseData>();
-  m_reuseData->callback = callback;
+  if (IsClosing()) {
+    return;
+  }
+  if (!m_reuseData) {
+    m_reuseData = std::make_unique<ReuseData>();
+  }
+  m_reuseData->callback = std::move(callback);
   m_reuseData->isSocket = true;
   m_reuseData->sock = sock;
   uv_close(GetRawHandle(), [](uv_handle_t* handle) {
     Poll& h = *static_cast<Poll*>(handle->data);
-    if (!h.m_reuseData || !h.m_reuseData->isSocket) return;  // just in case
+    if (!h.m_reuseData || !h.m_reuseData->isSocket) {
+      return;  // just in case
+    }
     auto data = std::move(h.m_reuseData);
     int err = uv_poll_init(h.GetLoopRef().GetRaw(), h.GetRaw(), data->sock);
     if (err < 0) {
@@ -76,12 +84,12 @@
   Invoke(&uv_poll_start, GetRaw(), events,
          [](uv_poll_t* handle, int status, int events) {
            Poll& h = *static_cast<Poll*>(handle->data);
-           if (status < 0)
+           if (status < 0) {
              h.ReportError(status);
-           else
+           } else {
              h.pollEvent(events);
+           }
          });
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Prepare.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Prepare.cpp
index f27f477..048fd08 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Prepare.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Prepare.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Prepare.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Prepare> Prepare::Create(Loop& loop) {
   auto h = std::make_shared<Prepare>(private_init{});
@@ -30,5 +26,4 @@
   });
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Process.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Process.cpp
index 5778965..c8d5229 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Process.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Process.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Process.h"
 
@@ -11,11 +8,10 @@
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/Pipe.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-std::shared_ptr<Process> Process::SpawnArray(Loop& loop, const Twine& file,
-                                             ArrayRef<Option> options) {
+std::shared_ptr<Process> Process::SpawnArray(Loop& loop, std::string_view file,
+                                             span<const Option> options) {
   // convert Option array to libuv structure
   uv_process_options_t coptions;
 
@@ -24,8 +20,8 @@
     h.exited(status, signal);
   };
 
-  SmallString<128> fileBuf;
-  coptions.file = file.toNullTerminatedStringRef(fileBuf).data();
+  SmallString<128> fileBuf{file};
+  coptions.file = fileBuf.c_str();
   coptions.cwd = nullptr;
   coptions.flags = 0;
   coptions.uid = 0;
@@ -68,28 +64,36 @@
         break;
       case Option::kStdioIgnore: {
         size_t index = o.m_data.stdio.index;
-        if (index >= stdioBuf.size()) stdioBuf.resize(index + 1);
+        if (index >= stdioBuf.size()) {
+          stdioBuf.resize(index + 1);
+        }
         stdioBuf[index].flags = UV_IGNORE;
         stdioBuf[index].data.fd = 0;
         break;
       }
       case Option::kStdioInheritFd: {
         size_t index = o.m_data.stdio.index;
-        if (index >= stdioBuf.size()) stdioBuf.resize(index + 1);
+        if (index >= stdioBuf.size()) {
+          stdioBuf.resize(index + 1);
+        }
         stdioBuf[index].flags = UV_INHERIT_FD;
         stdioBuf[index].data.fd = o.m_data.stdio.fd;
         break;
       }
       case Option::kStdioInheritPipe: {
         size_t index = o.m_data.stdio.index;
-        if (index >= stdioBuf.size()) stdioBuf.resize(index + 1);
+        if (index >= stdioBuf.size()) {
+          stdioBuf.resize(index + 1);
+        }
         stdioBuf[index].flags = UV_INHERIT_STREAM;
         stdioBuf[index].data.stream = o.m_data.stdio.pipe->GetRawStream();
         break;
       }
       case Option::kStdioCreatePipe: {
         size_t index = o.m_data.stdio.index;
-        if (index >= stdioBuf.size()) stdioBuf.resize(index + 1);
+        if (index >= stdioBuf.size()) {
+          stdioBuf.resize(index + 1);
+        }
         stdioBuf[index].flags =
             static_cast<uv_stdio_flags>(UV_CREATE_PIPE | o.m_data.stdio.flags);
         stdioBuf[index].data.stream = o.m_data.stdio.pipe->GetRawStream();
@@ -100,7 +104,9 @@
     }
   }
 
-  if (argsBuf.empty()) argsBuf.push_back(const_cast<char*>(coptions.file));
+  if (argsBuf.empty()) {
+    argsBuf.push_back(const_cast<char*>(coptions.file));
+  }
   argsBuf.push_back(nullptr);
   coptions.args = argsBuf.data();
 
@@ -124,5 +130,4 @@
   return h;
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Signal.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Signal.cpp
index 4569ac2..81d7c3e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Signal.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Signal.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Signal.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Signal> Signal::Create(Loop& loop) {
   auto h = std::make_shared<Signal>(private_init{});
@@ -33,5 +29,4 @@
       signum);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Stream.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Stream.cpp
index b1fd294..a37750b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Stream.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Stream.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Stream.h"
 
@@ -15,10 +12,11 @@
 namespace {
 class CallbackWriteReq : public WriteReq {
  public:
-  CallbackWriteReq(ArrayRef<Buffer> bufs,
-                   std::function<void(MutableArrayRef<Buffer>, Error)> callback)
+  CallbackWriteReq(span<const Buffer> bufs,
+                   std::function<void(span<Buffer>, Error)> callback)
       : m_bufs{bufs.begin(), bufs.end()} {
-    finish.connect([=](Error err) { callback(m_bufs, err); });
+    finish.connect(
+        [this, f = std::move(callback)](Error err) { f(m_bufs, err); });
   }
 
  private:
@@ -26,8 +24,7 @@
 };
 }  // namespace
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 ShutdownReq::ShutdownReq() {
   error = [this](Error err) { GetStream().error(err); };
@@ -41,18 +38,22 @@
   if (Invoke(&uv_shutdown, req->GetRaw(), GetRawStream(),
              [](uv_shutdown_t* req, int status) {
                auto& h = *static_cast<ShutdownReq*>(req->data);
-               if (status < 0)
+               if (status < 0) {
                  h.ReportError(status);
-               else
+               } else {
                  h.complete();
+               }
                h.Release();  // this is always a one-shot
-             }))
+             })) {
     req->Keep();
+  }
 }
 
 void Stream::Shutdown(std::function<void()> callback) {
   auto req = std::make_shared<ShutdownReq>();
-  if (callback) req->complete.connect(callback);
+  if (callback) {
+    req->complete.connect(std::move(callback));
+  }
   Shutdown(req);
 }
 
@@ -63,37 +64,40 @@
            Buffer data = *buf;
 
            // nread=0 is simply ignored
-           if (nread == UV_EOF)
+           if (nread == UV_EOF) {
              h.end();
-           else if (nread > 0)
+           } else if (nread > 0) {
              h.data(data, static_cast<size_t>(nread));
-           else if (nread < 0)
+           } else if (nread < 0) {
              h.ReportError(nread);
+           }
 
            // free the buffer
            h.FreeBuf(data);
          });
 }
 
-void Stream::Write(ArrayRef<Buffer> bufs,
+void Stream::Write(span<const Buffer> bufs,
                    const std::shared_ptr<WriteReq>& req) {
   if (Invoke(&uv_write, req->GetRaw(), GetRawStream(), bufs.data(), bufs.size(),
              [](uv_write_t* r, int status) {
                auto& h = *static_cast<WriteReq*>(r->data);
-               if (status < 0) h.ReportError(status);
+               if (status < 0) {
+                 h.ReportError(status);
+               }
                h.finish(Error(status));
                h.Release();  // this is always a one-shot
-             }))
+             })) {
     req->Keep();
+  }
 }
 
-void Stream::Write(
-    ArrayRef<Buffer> bufs,
-    std::function<void(MutableArrayRef<Buffer>, Error)> callback) {
-  Write(bufs, std::make_shared<CallbackWriteReq>(bufs, callback));
+void Stream::Write(span<const Buffer> bufs,
+                   std::function<void(span<Buffer>, Error)> callback) {
+  Write(bufs, std::make_shared<CallbackWriteReq>(bufs, std::move(callback)));
 }
 
-int Stream::TryWrite(ArrayRef<Buffer> bufs) {
+int Stream::TryWrite(span<const Buffer> bufs) {
   int val = uv_try_write(GetRawStream(), bufs.data(), bufs.size());
   if (val < 0) {
     this->ReportError(val);
@@ -102,5 +106,4 @@
   return val;
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tcp.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tcp.cpp
index f71e055..6f92557 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tcp.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tcp.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Tcp.h"
 
@@ -11,8 +8,7 @@
 
 #include "wpi/uv/util.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Tcp> Tcp::Create(Loop& loop, unsigned int flags) {
   auto h = std::make_shared<Tcp>(private_init{});
@@ -26,13 +22,19 @@
 }
 
 void Tcp::Reuse(std::function<void()> callback, unsigned int flags) {
-  if (IsClosing()) return;
-  if (!m_reuseData) m_reuseData = std::make_unique<ReuseData>();
-  m_reuseData->callback = callback;
+  if (IsClosing()) {
+    return;
+  }
+  if (!m_reuseData) {
+    m_reuseData = std::make_unique<ReuseData>();
+  }
+  m_reuseData->callback = std::move(callback);
   m_reuseData->flags = flags;
   uv_close(GetRawHandle(), [](uv_handle_t* handle) {
     Tcp& h = *static_cast<Tcp*>(handle->data);
-    if (!h.m_reuseData) return;  // just in case
+    if (!h.m_reuseData) {
+      return;  // just in case
+    }
     auto data = std::move(h.m_reuseData);
     int err = uv_tcp_init_ex(h.GetLoopRef().GetRaw(), h.GetRaw(), data->flags);
     if (err < 0) {
@@ -45,7 +47,9 @@
 
 std::shared_ptr<Tcp> Tcp::Accept() {
   auto client = Create(GetLoopRef());
-  if (!client) return nullptr;
+  if (!client) {
+    return nullptr;
+  }
   if (!Accept(client)) {
     client->Release();
     return nullptr;
@@ -53,32 +57,37 @@
   return client;
 }
 
-Tcp* Tcp::DoAccept() { return Accept().get(); }
-
-void Tcp::Bind(const Twine& ip, unsigned int port, unsigned int flags) {
-  sockaddr_in addr;
-  int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
-    ReportError(err);
-  else
-    Bind(reinterpret_cast<const sockaddr&>(addr), flags);
+Tcp* Tcp::DoAccept() {
+  return Accept().get();
 }
 
-void Tcp::Bind6(const Twine& ip, unsigned int port, unsigned int flags) {
+void Tcp::Bind(std::string_view ip, unsigned int port, unsigned int flags) {
+  sockaddr_in addr;
+  int err = NameToAddr(ip, port, &addr);
+  if (err < 0) {
+    ReportError(err);
+  } else {
+    Bind(reinterpret_cast<const sockaddr&>(addr), flags);
+  }
+}
+
+void Tcp::Bind6(std::string_view ip, unsigned int port, unsigned int flags) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Bind(reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
 sockaddr_storage Tcp::GetSock() {
   sockaddr_storage name;
   int len = sizeof(name);
   if (!Invoke(&uv_tcp_getsockname, GetRaw(), reinterpret_cast<sockaddr*>(&name),
-              &len))
+              &len)) {
     std::memset(&name, 0, sizeof(name));
+  }
   return name;
 }
 
@@ -86,8 +95,9 @@
   sockaddr_storage name;
   int len = sizeof(name);
   if (!Invoke(&uv_tcp_getpeername, GetRaw(), reinterpret_cast<sockaddr*>(&name),
-              &len))
+              &len)) {
     std::memset(&name, 0, sizeof(name));
+  }
   return name;
 }
 
@@ -96,60 +106,65 @@
   if (Invoke(&uv_tcp_connect, req->GetRaw(), GetRaw(), &addr,
              [](uv_connect_t* req, int status) {
                auto& h = *static_cast<TcpConnectReq*>(req->data);
-               if (status < 0)
+               if (status < 0) {
                  h.ReportError(status);
-               else
+               } else {
                  h.connected();
+               }
                h.Release();  // this is always a one-shot
-             }))
+             })) {
     req->Keep();
+  }
 }
 
 void Tcp::Connect(const sockaddr& addr, std::function<void()> callback) {
   auto req = std::make_shared<TcpConnectReq>();
-  req->connected.connect(callback);
+  req->connected.connect(std::move(callback));
   Connect(addr, req);
 }
 
-void Tcp::Connect(const Twine& ip, unsigned int port,
+void Tcp::Connect(std::string_view ip, unsigned int port,
                   const std::shared_ptr<TcpConnectReq>& req) {
   sockaddr_in addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Connect(reinterpret_cast<const sockaddr&>(addr), req);
+  }
 }
 
-void Tcp::Connect(const Twine& ip, unsigned int port,
+void Tcp::Connect(std::string_view ip, unsigned int port,
                   std::function<void()> callback) {
   sockaddr_in addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
-    Connect(reinterpret_cast<const sockaddr&>(addr), callback);
+  } else {
+    Connect(reinterpret_cast<const sockaddr&>(addr), std::move(callback));
+  }
 }
 
-void Tcp::Connect6(const Twine& ip, unsigned int port,
+void Tcp::Connect6(std::string_view ip, unsigned int port,
                    const std::shared_ptr<TcpConnectReq>& req) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Connect(reinterpret_cast<const sockaddr&>(addr), req);
+  }
 }
 
-void Tcp::Connect6(const Twine& ip, unsigned int port,
+void Tcp::Connect6(std::string_view ip, unsigned int port,
                    std::function<void()> callback) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
-    Connect(reinterpret_cast<const sockaddr&>(addr), callback);
+  } else {
+    Connect(reinterpret_cast<const sockaddr&>(addr), std::move(callback));
+  }
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Timer.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Timer.cpp
index 0fdd3be..33fd851 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Timer.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Timer.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Timer.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Timer> Timer::Create(Loop& loop) {
   auto h = std::make_shared<Timer>(private_init{});
@@ -25,9 +21,11 @@
 
 void Timer::SingleShot(Loop& loop, Time timeout, std::function<void()> func) {
   auto h = Create(loop);
-  if (!h) return;
-  h->timeout.connect([theTimer = h.get(), func]() {
-    func();
+  if (!h) {
+    return;
+  }
+  h->timeout.connect([theTimer = h.get(), f = std::move(func)]() {
+    f();
     theTimer->Close();
   });
   h->Start(timeout);
@@ -43,5 +41,4 @@
       timeout.count(), repeat.count());
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tty.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tty.cpp
index cdd6fd5..4531ded 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tty.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Tty.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Tty.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 std::shared_ptr<Tty> Tty::Create(Loop& loop, uv_file fd, bool readable) {
   auto h = std::make_shared<Tty>(private_init{});
@@ -23,5 +19,4 @@
   return h;
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Udp.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Udp.cpp
index 2c0d29f..bea2b57 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Udp.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Udp.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Udp.h"
 
@@ -20,11 +17,11 @@
 
 class CallbackUdpSendReq : public UdpSendReq {
  public:
-  CallbackUdpSendReq(
-      ArrayRef<Buffer> bufs,
-      std::function<void(MutableArrayRef<Buffer>, Error)> callback)
+  CallbackUdpSendReq(span<const Buffer> bufs,
+                     std::function<void(span<Buffer>, Error)> callback)
       : m_bufs{bufs.begin(), bufs.end()} {
-    complete.connect([=](Error err) { callback(m_bufs, err); });
+    complete.connect(
+        [this, f = std::move(callback)](Error err) { f(m_bufs, err); });
   }
 
  private:
@@ -33,8 +30,7 @@
 
 }  // namespace
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 UdpSendReq::UdpSendReq() {
   error = [this](Error err) { GetUdp().error(err); };
@@ -51,48 +47,53 @@
   return h;
 }
 
-void Udp::Bind(const Twine& ip, unsigned int port, unsigned int flags) {
+void Udp::Bind(std::string_view ip, unsigned int port, unsigned int flags) {
   sockaddr_in addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Bind(reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
-void Udp::Bind6(const Twine& ip, unsigned int port, unsigned int flags) {
+void Udp::Bind6(std::string_view ip, unsigned int port, unsigned int flags) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Bind(reinterpret_cast<const sockaddr&>(addr), flags);
+  }
 }
 
-void Udp::Connect(const Twine& ip, unsigned int port) {
+void Udp::Connect(std::string_view ip, unsigned int port) {
   sockaddr_in addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Connect(reinterpret_cast<const sockaddr&>(addr));
+  }
 }
 
-void Udp::Connect6(const Twine& ip, unsigned int port) {
+void Udp::Connect6(std::string_view ip, unsigned int port) {
   sockaddr_in6 addr;
   int err = NameToAddr(ip, port, &addr);
-  if (err < 0)
+  if (err < 0) {
     ReportError(err);
-  else
+  } else {
     Connect(reinterpret_cast<const sockaddr&>(addr));
+  }
 }
 
 sockaddr_storage Udp::GetPeer() {
   sockaddr_storage name;
   int len = sizeof(name);
   if (!Invoke(&uv_udp_getpeername, GetRaw(), reinterpret_cast<sockaddr*>(&name),
-              &len))
+              &len)) {
     std::memset(&name, 0, sizeof(name));
+  }
   return name;
 }
 
@@ -100,58 +101,65 @@
   sockaddr_storage name;
   int len = sizeof(name);
   if (!Invoke(&uv_udp_getsockname, GetRaw(), reinterpret_cast<sockaddr*>(&name),
-              &len))
+              &len)) {
     std::memset(&name, 0, sizeof(name));
+  }
   return name;
 }
 
-void Udp::SetMembership(const Twine& multicastAddr, const Twine& interfaceAddr,
+void Udp::SetMembership(std::string_view multicastAddr,
+                        std::string_view interfaceAddr,
                         uv_membership membership) {
-  SmallString<128> multicastAddrBuf;
-  SmallString<128> interfaceAddrBuf;
-  Invoke(&uv_udp_set_membership, GetRaw(),
-         multicastAddr.toNullTerminatedStringRef(multicastAddrBuf).data(),
-         interfaceAddr.toNullTerminatedStringRef(interfaceAddrBuf).data(),
-         membership);
+  SmallString<128> multicastAddrBuf{multicastAddr};
+  SmallString<128> interfaceAddrBuf{interfaceAddr};
+  Invoke(&uv_udp_set_membership, GetRaw(), multicastAddrBuf.c_str(),
+         interfaceAddrBuf.c_str(), membership);
 }
 
-void Udp::SetMulticastInterface(const Twine& interfaceAddr) {
-  SmallString<128> interfaceAddrBuf;
-  Invoke(&uv_udp_set_multicast_interface, GetRaw(),
-         interfaceAddr.toNullTerminatedStringRef(interfaceAddrBuf).data());
+void Udp::SetMulticastInterface(std::string_view interfaceAddr) {
+  SmallString<128> interfaceAddrBuf{interfaceAddr};
+  Invoke(&uv_udp_set_multicast_interface, GetRaw(), interfaceAddrBuf.c_str());
 }
 
-void Udp::Send(const sockaddr& addr, ArrayRef<Buffer> bufs,
+void Udp::Send(const sockaddr& addr, span<const Buffer> bufs,
                const std::shared_ptr<UdpSendReq>& req) {
   if (Invoke(&uv_udp_send, req->GetRaw(), GetRaw(), bufs.data(), bufs.size(),
              &addr, [](uv_udp_send_t* r, int status) {
                auto& h = *static_cast<UdpSendReq*>(r->data);
-               if (status < 0) h.ReportError(status);
+               if (status < 0) {
+                 h.ReportError(status);
+               }
                h.complete(Error(status));
                h.Release();  // this is always a one-shot
-             }))
+             })) {
     req->Keep();
+  }
 }
 
-void Udp::Send(const sockaddr& addr, ArrayRef<Buffer> bufs,
-               std::function<void(MutableArrayRef<Buffer>, Error)> callback) {
-  Send(addr, bufs, std::make_shared<CallbackUdpSendReq>(bufs, callback));
+void Udp::Send(const sockaddr& addr, span<const Buffer> bufs,
+               std::function<void(span<Buffer>, Error)> callback) {
+  Send(addr, bufs,
+       std::make_shared<CallbackUdpSendReq>(bufs, std::move(callback)));
 }
 
-void Udp::Send(ArrayRef<Buffer> bufs, const std::shared_ptr<UdpSendReq>& req) {
+void Udp::Send(span<const Buffer> bufs,
+               const std::shared_ptr<UdpSendReq>& req) {
   if (Invoke(&uv_udp_send, req->GetRaw(), GetRaw(), bufs.data(), bufs.size(),
              nullptr, [](uv_udp_send_t* r, int status) {
                auto& h = *static_cast<UdpSendReq*>(r->data);
-               if (status < 0) h.ReportError(status);
+               if (status < 0) {
+                 h.ReportError(status);
+               }
                h.complete(Error(status));
                h.Release();  // this is always a one-shot
-             }))
+             })) {
     req->Keep();
+  }
 }
 
-void Udp::Send(ArrayRef<Buffer> bufs,
-               std::function<void(MutableArrayRef<Buffer>, Error)> callback) {
-  Send(bufs, std::make_shared<CallbackUdpSendReq>(bufs, callback));
+void Udp::Send(span<const Buffer> bufs,
+               std::function<void(span<Buffer>, Error)> callback) {
+  Send(bufs, std::make_shared<CallbackUdpSendReq>(bufs, std::move(callback)));
 }
 
 void Udp::StartRecv() {
@@ -162,15 +170,15 @@
            Buffer data = *buf;
 
            // nread=0 is simply ignored
-           if (nread > 0)
+           if (nread > 0) {
              h.received(data, static_cast<size_t>(nread), *addr, flags);
-           else if (nread < 0)
+           } else if (nread < 0) {
              h.ReportError(nread);
+           }
 
            // free the buffer
            h.FreeBuf(data);
          });
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Work.cpp b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Work.cpp
index e5d7bb6..0fc254e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Work.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/cpp/uv/Work.cpp
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Work.h"
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 WorkReq::WorkReq() {
   error = [this](Error err) { GetLoop().error(err); };
@@ -25,25 +21,30 @@
       },
       [](uv_work_t* req, int status) {
         auto& h = *static_cast<WorkReq*>(req->data);
-        if (status < 0)
+        if (status < 0) {
           h.ReportError(status);
-        else
+        } else {
           h.afterWork();
+        }
         h.Release();  // this is always a one-shot
       });
-  if (err < 0)
+  if (err < 0) {
     loop.ReportError(err);
-  else
+  } else {
     req->Keep();
+  }
 }
 
 void QueueWork(Loop& loop, std::function<void()> work,
                std::function<void()> afterWork) {
   auto req = std::make_shared<WorkReq>();
-  if (work) req->work.connect(work);
-  if (afterWork) req->afterWork.connect(afterWork);
+  if (work) {
+    req->work.connect(std::move(work));
+  }
+  if (afterWork) {
+    req->afterWork.connect(std::move(afterWork));
+  }
   QueueWork(loop, req);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/args.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/args.h
new file mode 100644
index 0000000..562e8ab
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/args.h
@@ -0,0 +1,232 @@
+// Formatting library for C++ - dynamic format arguments
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_ARGS_H_
+#define FMT_ARGS_H_
+
+#include <functional>  // std::reference_wrapper
+#include <memory>      // std::unique_ptr
+#include <vector>
+
+#include "core.h"
+
+FMT_BEGIN_NAMESPACE
+
+namespace detail {
+
+template <typename T> struct is_reference_wrapper : std::false_type {};
+template <typename T>
+struct is_reference_wrapper<std::reference_wrapper<T>> : std::true_type {};
+
+template <typename T> const T& unwrap(const T& v) { return v; }
+template <typename T> const T& unwrap(const std::reference_wrapper<T>& v) {
+  return static_cast<const T&>(v);
+}
+
+class dynamic_arg_list {
+  // Workaround for clang's -Wweak-vtables. Unlike for regular classes, for
+  // templates it doesn't complain about inability to deduce single translation
+  // unit for placing vtable. So storage_node_base is made a fake template.
+  template <typename = void> struct node {
+    virtual ~node() = default;
+    std::unique_ptr<node<>> next;
+  };
+
+  template <typename T> struct typed_node : node<> {
+    T value;
+
+    template <typename Arg>
+    FMT_CONSTEXPR typed_node(const Arg& arg) : value(arg) {}
+
+    template <typename Char>
+    FMT_CONSTEXPR typed_node(const basic_string_view<Char>& arg)
+        : value(arg.data(), arg.size()) {}
+  };
+
+  std::unique_ptr<node<>> head_;
+
+ public:
+  template <typename T, typename Arg> const T& push(const Arg& arg) {
+    auto new_node = std::unique_ptr<typed_node<T>>(new typed_node<T>(arg));
+    auto& value = new_node->value;
+    new_node->next = std::move(head_);
+    head_ = std::move(new_node);
+    return value;
+  }
+};
+}  // namespace detail
+
+/**
+  \rst
+  A dynamic version of `fmt::format_arg_store`.
+  It's equipped with a storage to potentially temporary objects which lifetimes
+  could be shorter than the format arguments object.
+
+  It can be implicitly converted into `~fmt::basic_format_args` for passing
+  into type-erased formatting functions such as `~fmt::vformat`.
+  \endrst
+ */
+template <typename Context>
+class dynamic_format_arg_store
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 409
+    // Workaround a GCC template argument substitution bug.
+    : public basic_format_args<Context>
+#endif
+{
+ private:
+  using char_type = typename Context::char_type;
+
+  template <typename T> struct need_copy {
+    static constexpr detail::type mapped_type =
+        detail::mapped_type_constant<T, Context>::value;
+
+    enum {
+      value = !(detail::is_reference_wrapper<T>::value ||
+                std::is_same<T, basic_string_view<char_type>>::value ||
+                std::is_same<T, detail::std_string_view<char_type>>::value ||
+                (mapped_type != detail::type::cstring_type &&
+                 mapped_type != detail::type::string_type &&
+                 mapped_type != detail::type::custom_type))
+    };
+  };
+
+  template <typename T>
+  using stored_type = conditional_t<detail::is_string<T>::value &&
+                                        !has_formatter<T, Context>::value &&
+                                        !detail::is_reference_wrapper<T>::value,
+                                    std::basic_string<char_type>, T>;
+
+  // Storage of basic_format_arg must be contiguous.
+  std::vector<basic_format_arg<Context>> data_;
+  std::vector<detail::named_arg_info<char_type>> named_info_;
+
+  // Storage of arguments not fitting into basic_format_arg must grow
+  // without relocation because items in data_ refer to it.
+  detail::dynamic_arg_list dynamic_args_;
+
+  friend class basic_format_args<Context>;
+
+  unsigned long long get_types() const {
+    return detail::is_unpacked_bit | data_.size() |
+           (named_info_.empty()
+                ? 0ULL
+                : static_cast<unsigned long long>(detail::has_named_args_bit));
+  }
+
+  const basic_format_arg<Context>* data() const {
+    return named_info_.empty() ? data_.data() : data_.data() + 1;
+  }
+
+  template <typename T> void emplace_arg(const T& arg) {
+    data_.emplace_back(detail::make_arg<Context>(arg));
+  }
+
+  template <typename T>
+  void emplace_arg(const detail::named_arg<char_type, T>& arg) {
+    if (named_info_.empty()) {
+      constexpr const detail::named_arg_info<char_type>* zero_ptr{nullptr};
+      data_.insert(data_.begin(), {zero_ptr, 0});
+    }
+    data_.emplace_back(detail::make_arg<Context>(detail::unwrap(arg.value)));
+    auto pop_one = [](std::vector<basic_format_arg<Context>>* data) {
+      data->pop_back();
+    };
+    std::unique_ptr<std::vector<basic_format_arg<Context>>, decltype(pop_one)>
+        guard{&data_, pop_one};
+    named_info_.push_back({arg.name, static_cast<int>(data_.size() - 2u)});
+    data_[0].value_.named_args = {named_info_.data(), named_info_.size()};
+    guard.release();
+  }
+
+ public:
+  /**
+    \rst
+    Adds an argument into the dynamic store for later passing to a formatting
+    function.
+
+    Note that custom types and string types (but not string views) are copied
+    into the store dynamically allocating memory if necessary.
+
+    **Example**::
+
+      fmt::dynamic_format_arg_store<fmt::format_context> store;
+      store.push_back(42);
+      store.push_back("abc");
+      store.push_back(1.5f);
+      std::string result = fmt::vformat("{} and {} and {}", store);
+    \endrst
+  */
+  template <typename T> void push_back(const T& arg) {
+    if (detail::const_check(need_copy<T>::value))
+      emplace_arg(dynamic_args_.push<stored_type<T>>(arg));
+    else
+      emplace_arg(detail::unwrap(arg));
+  }
+
+  /**
+    \rst
+    Adds a reference to the argument into the dynamic store for later passing to
+    a formatting function.
+
+    **Example**::
+
+      fmt::dynamic_format_arg_store<fmt::format_context> store;
+      char band[] = "Rolling Stones";
+      store.push_back(std::cref(band));
+      band[9] = 'c'; // Changing str affects the output.
+      std::string result = fmt::vformat("{}", store);
+      // result == "Rolling Scones"
+    \endrst
+  */
+  template <typename T> void push_back(std::reference_wrapper<T> arg) {
+    static_assert(
+        need_copy<T>::value,
+        "objects of built-in types and string views are always copied");
+    emplace_arg(arg.get());
+  }
+
+  /**
+    Adds named argument into the dynamic store for later passing to a formatting
+    function. ``std::reference_wrapper`` is supported to avoid copying of the
+    argument. The name is always copied into the store.
+  */
+  template <typename T>
+  void push_back(const detail::named_arg<char_type, T>& arg) {
+    const char_type* arg_name =
+        dynamic_args_.push<std::basic_string<char_type>>(arg.name).c_str();
+    if (detail::const_check(need_copy<T>::value)) {
+      emplace_arg(
+          fmt::arg(arg_name, dynamic_args_.push<stored_type<T>>(arg.value)));
+    } else {
+      emplace_arg(fmt::arg(arg_name, arg.value));
+    }
+  }
+
+  /** Erase all elements from the store */
+  void clear() {
+    data_.clear();
+    named_info_.clear();
+    dynamic_args_ = detail::dynamic_arg_list();
+  }
+
+  /**
+    \rst
+    Reserves space to store at least *new_cap* arguments including
+    *new_cap_named* named arguments.
+    \endrst
+  */
+  void reserve(size_t new_cap, size_t new_cap_named) {
+    FMT_ASSERT(new_cap >= new_cap_named,
+               "Set of arguments includes set of named arguments");
+    data_.reserve(new_cap);
+    named_info_.reserve(new_cap_named);
+  }
+};
+
+FMT_END_NAMESPACE
+
+#endif  // FMT_ARGS_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/chrono.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/chrono.h
new file mode 100644
index 0000000..c024fd7
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/chrono.h
@@ -0,0 +1,1308 @@
+// Formatting library for C++ - chrono support
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_CHRONO_H_
+#define FMT_CHRONO_H_
+
+#include <algorithm>
+#include <chrono>
+#include <ctime>
+#include <locale>
+#include <sstream>
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+
+// Enable safe chrono durations, unless explicitly disabled.
+#ifndef FMT_SAFE_DURATION_CAST
+#  define FMT_SAFE_DURATION_CAST 1
+#endif
+#if FMT_SAFE_DURATION_CAST
+
+// For conversion between std::chrono::durations without undefined
+// behaviour or erroneous results.
+// This is a stripped down version of duration_cast, for inclusion in fmt.
+// See https://github.com/pauldreik/safe_duration_cast
+//
+// Copyright Paul Dreik 2019
+namespace safe_duration_cast {
+
+template <typename To, typename From,
+          FMT_ENABLE_IF(!std::is_same<From, To>::value &&
+                        std::numeric_limits<From>::is_signed ==
+                            std::numeric_limits<To>::is_signed)>
+FMT_CONSTEXPR To lossless_integral_conversion(const From from, int& ec) {
+  ec = 0;
+  using F = std::numeric_limits<From>;
+  using T = std::numeric_limits<To>;
+  static_assert(F::is_integer, "From must be integral");
+  static_assert(T::is_integer, "To must be integral");
+
+  // A and B are both signed, or both unsigned.
+  if (F::digits <= T::digits) {
+    // From fits in To without any problem.
+  } else {
+    // From does not always fit in To, resort to a dynamic check.
+    if (from < (T::min)() || from > (T::max)()) {
+      // outside range.
+      ec = 1;
+      return {};
+    }
+  }
+  return static_cast<To>(from);
+}
+
+/**
+ * converts From to To, without loss. If the dynamic value of from
+ * can't be converted to To without loss, ec is set.
+ */
+template <typename To, typename From,
+          FMT_ENABLE_IF(!std::is_same<From, To>::value &&
+                        std::numeric_limits<From>::is_signed !=
+                            std::numeric_limits<To>::is_signed)>
+FMT_CONSTEXPR To lossless_integral_conversion(const From from, int& ec) {
+  ec = 0;
+  using F = std::numeric_limits<From>;
+  using T = std::numeric_limits<To>;
+  static_assert(F::is_integer, "From must be integral");
+  static_assert(T::is_integer, "To must be integral");
+
+  if (detail::const_check(F::is_signed && !T::is_signed)) {
+    // From may be negative, not allowed!
+    if (fmt::detail::is_negative(from)) {
+      ec = 1;
+      return {};
+    }
+    // From is positive. Can it always fit in To?
+    if (F::digits > T::digits &&
+        from > static_cast<From>(detail::max_value<To>())) {
+      ec = 1;
+      return {};
+    }
+  }
+
+  if (!F::is_signed && T::is_signed && F::digits >= T::digits &&
+      from > static_cast<From>(detail::max_value<To>())) {
+    ec = 1;
+    return {};
+  }
+  return static_cast<To>(from);  // Lossless conversion.
+}
+
+template <typename To, typename From,
+          FMT_ENABLE_IF(std::is_same<From, To>::value)>
+FMT_CONSTEXPR To lossless_integral_conversion(const From from, int& ec) {
+  ec = 0;
+  return from;
+}  // function
+
+// clang-format off
+/**
+ * converts From to To if possible, otherwise ec is set.
+ *
+ * input                            |    output
+ * ---------------------------------|---------------
+ * NaN                              | NaN
+ * Inf                              | Inf
+ * normal, fits in output           | converted (possibly lossy)
+ * normal, does not fit in output   | ec is set
+ * subnormal                        | best effort
+ * -Inf                             | -Inf
+ */
+// clang-format on
+template <typename To, typename From,
+          FMT_ENABLE_IF(!std::is_same<From, To>::value)>
+FMT_CONSTEXPR To safe_float_conversion(const From from, int& ec) {
+  ec = 0;
+  using T = std::numeric_limits<To>;
+  static_assert(std::is_floating_point<From>::value, "From must be floating");
+  static_assert(std::is_floating_point<To>::value, "To must be floating");
+
+  // catch the only happy case
+  if (std::isfinite(from)) {
+    if (from >= T::lowest() && from <= (T::max)()) {
+      return static_cast<To>(from);
+    }
+    // not within range.
+    ec = 1;
+    return {};
+  }
+
+  // nan and inf will be preserved
+  return static_cast<To>(from);
+}  // function
+
+template <typename To, typename From,
+          FMT_ENABLE_IF(std::is_same<From, To>::value)>
+FMT_CONSTEXPR To safe_float_conversion(const From from, int& ec) {
+  ec = 0;
+  static_assert(std::is_floating_point<From>::value, "From must be floating");
+  return from;
+}
+
+/**
+ * safe duration cast between integral durations
+ */
+template <typename To, typename FromRep, typename FromPeriod,
+          FMT_ENABLE_IF(std::is_integral<FromRep>::value),
+          FMT_ENABLE_IF(std::is_integral<typename To::rep>::value)>
+To safe_duration_cast(std::chrono::duration<FromRep, FromPeriod> from,
+                      int& ec) {
+  using From = std::chrono::duration<FromRep, FromPeriod>;
+  ec = 0;
+  // the basic idea is that we need to convert from count() in the from type
+  // to count() in the To type, by multiplying it with this:
+  struct Factor
+      : std::ratio_divide<typename From::period, typename To::period> {};
+
+  static_assert(Factor::num > 0, "num must be positive");
+  static_assert(Factor::den > 0, "den must be positive");
+
+  // the conversion is like this: multiply from.count() with Factor::num
+  // /Factor::den and convert it to To::rep, all this without
+  // overflow/underflow. let's start by finding a suitable type that can hold
+  // both To, From and Factor::num
+  using IntermediateRep =
+      typename std::common_type<typename From::rep, typename To::rep,
+                                decltype(Factor::num)>::type;
+
+  // safe conversion to IntermediateRep
+  IntermediateRep count =
+      lossless_integral_conversion<IntermediateRep>(from.count(), ec);
+  if (ec) return {};
+  // multiply with Factor::num without overflow or underflow
+  if (detail::const_check(Factor::num != 1)) {
+    const auto max1 = detail::max_value<IntermediateRep>() / Factor::num;
+    if (count > max1) {
+      ec = 1;
+      return {};
+    }
+    const auto min1 =
+        (std::numeric_limits<IntermediateRep>::min)() / Factor::num;
+    if (count < min1) {
+      ec = 1;
+      return {};
+    }
+    count *= Factor::num;
+  }
+
+  if (detail::const_check(Factor::den != 1)) count /= Factor::den;
+  auto tocount = lossless_integral_conversion<typename To::rep>(count, ec);
+  return ec ? To() : To(tocount);
+}
+
+/**
+ * safe duration_cast between floating point durations
+ */
+template <typename To, typename FromRep, typename FromPeriod,
+          FMT_ENABLE_IF(std::is_floating_point<FromRep>::value),
+          FMT_ENABLE_IF(std::is_floating_point<typename To::rep>::value)>
+To safe_duration_cast(std::chrono::duration<FromRep, FromPeriod> from,
+                      int& ec) {
+  using From = std::chrono::duration<FromRep, FromPeriod>;
+  ec = 0;
+  if (std::isnan(from.count())) {
+    // nan in, gives nan out. easy.
+    return To{std::numeric_limits<typename To::rep>::quiet_NaN()};
+  }
+  // maybe we should also check if from is denormal, and decide what to do about
+  // it.
+
+  // +-inf should be preserved.
+  if (std::isinf(from.count())) {
+    return To{from.count()};
+  }
+
+  // the basic idea is that we need to convert from count() in the from type
+  // to count() in the To type, by multiplying it with this:
+  struct Factor
+      : std::ratio_divide<typename From::period, typename To::period> {};
+
+  static_assert(Factor::num > 0, "num must be positive");
+  static_assert(Factor::den > 0, "den must be positive");
+
+  // the conversion is like this: multiply from.count() with Factor::num
+  // /Factor::den and convert it to To::rep, all this without
+  // overflow/underflow. let's start by finding a suitable type that can hold
+  // both To, From and Factor::num
+  using IntermediateRep =
+      typename std::common_type<typename From::rep, typename To::rep,
+                                decltype(Factor::num)>::type;
+
+  // force conversion of From::rep -> IntermediateRep to be safe,
+  // even if it will never happen be narrowing in this context.
+  IntermediateRep count =
+      safe_float_conversion<IntermediateRep>(from.count(), ec);
+  if (ec) {
+    return {};
+  }
+
+  // multiply with Factor::num without overflow or underflow
+  if (Factor::num != 1) {
+    constexpr auto max1 = detail::max_value<IntermediateRep>() /
+                          static_cast<IntermediateRep>(Factor::num);
+    if (count > max1) {
+      ec = 1;
+      return {};
+    }
+    constexpr auto min1 = std::numeric_limits<IntermediateRep>::lowest() /
+                          static_cast<IntermediateRep>(Factor::num);
+    if (count < min1) {
+      ec = 1;
+      return {};
+    }
+    count *= static_cast<IntermediateRep>(Factor::num);
+  }
+
+  // this can't go wrong, right? den>0 is checked earlier.
+  if (Factor::den != 1) {
+    using common_t = typename std::common_type<IntermediateRep, intmax_t>::type;
+    count /= static_cast<common_t>(Factor::den);
+  }
+
+  // convert to the to type, safely
+  using ToRep = typename To::rep;
+
+  const ToRep tocount = safe_float_conversion<ToRep>(count, ec);
+  if (ec) {
+    return {};
+  }
+  return To{tocount};
+}
+}  // namespace safe_duration_cast
+#endif
+
+// Prevents expansion of a preceding token as a function-style macro.
+// Usage: f FMT_NOMACRO()
+#define FMT_NOMACRO
+
+namespace detail {
+template <typename T = void> struct null {};
+inline null<> localtime_r FMT_NOMACRO(...) { return null<>(); }
+inline null<> localtime_s(...) { return null<>(); }
+inline null<> gmtime_r(...) { return null<>(); }
+inline null<> gmtime_s(...) { return null<>(); }
+
+inline auto do_write(const std::tm& time, const std::locale& loc, char format,
+                     char modifier) -> std::string {
+  auto&& os = std::ostringstream();
+  os.imbue(loc);
+  using iterator = std::ostreambuf_iterator<char>;
+  const auto& facet = std::use_facet<std::time_put<char, iterator>>(loc);
+  auto end = facet.put(os, os, ' ', &time, format, modifier);
+  if (end.failed()) FMT_THROW(format_error("failed to format time"));
+  auto str = os.str();
+  if (!detail::is_utf8() || loc == std::locale::classic()) return str;
+    // char16_t and char32_t codecvts are broken in MSVC (linkage errors) and
+    // gcc-4.
+#if FMT_MSC_VER != 0 || \
+    (defined(__GLIBCXX__) && !defined(_GLIBCXX_USE_DUAL_ABI))
+  // The _GLIBCXX_USE_DUAL_ABI macro is always defined in libstdc++ from gcc-5
+  // and newer.
+  using code_unit = wchar_t;
+#else
+  using code_unit = char32_t;
+#endif
+  auto& f = std::use_facet<std::codecvt<code_unit, char, std::mbstate_t>>(loc);
+  auto mb = std::mbstate_t();
+  const char* from_next = nullptr;
+  code_unit* to_next = nullptr;
+  constexpr size_t buf_size = 32;
+  code_unit buf[buf_size] = {};
+  auto result = f.in(mb, str.data(), str.data() + str.size(), from_next, buf,
+                     buf + buf_size, to_next);
+  if (result != std::codecvt_base::ok)
+    FMT_THROW(format_error("failed to format time"));
+  str.clear();
+  for (code_unit* p = buf; p != to_next; ++p) {
+    uint32_t c = static_cast<uint32_t>(*p);
+    if (sizeof(code_unit) == 2 && c >= 0xd800 && c <= 0xdfff) {
+      // surrogate pair
+      ++p;
+      if (p == to_next || (c & 0xfc00) != 0xd800 || (*p & 0xfc00) != 0xdc00) {
+        FMT_THROW(format_error("failed to format time"));
+      }
+      c = (c << 10) + static_cast<uint32_t>(*p) - 0x35fdc00;
+    }
+    if (c < 0x80) {
+      str.push_back(static_cast<char>(c));
+    } else if (c < 0x800) {
+      str.push_back(static_cast<char>(0xc0 | (c >> 6)));
+      str.push_back(static_cast<char>(0x80 | (c & 0x3f)));
+    } else if ((c >= 0x800 && c <= 0xd7ff) || (c >= 0xe000 && c <= 0xffff)) {
+      str.push_back(static_cast<char>(0xe0 | (c >> 12)));
+      str.push_back(static_cast<char>(0x80 | ((c & 0xfff) >> 6)));
+      str.push_back(static_cast<char>(0x80 | (c & 0x3f)));
+    } else if (c >= 0x10000 && c <= 0x10ffff) {
+      str.push_back(static_cast<char>(0xf0 | (c >> 18)));
+      str.push_back(static_cast<char>(0x80 | ((c & 0x3ffff) >> 12)));
+      str.push_back(static_cast<char>(0x80 | ((c & 0xfff) >> 6)));
+      str.push_back(static_cast<char>(0x80 | (c & 0x3f)));
+    } else {
+      FMT_THROW(format_error("failed to format time"));
+    }
+  }
+  return str;
+}
+
+template <typename OutputIt>
+auto write(OutputIt out, const std::tm& time, const std::locale& loc,
+           char format, char modifier = 0) -> OutputIt {
+  auto str = do_write(time, loc, format, modifier);
+  return std::copy(str.begin(), str.end(), out);
+}
+}  // namespace detail
+
+FMT_MODULE_EXPORT_BEGIN
+
+/**
+  Converts given time since epoch as ``std::time_t`` value into calendar time,
+  expressed in local time. Unlike ``std::localtime``, this function is
+  thread-safe on most platforms.
+ */
+inline std::tm localtime(std::time_t time) {
+  struct dispatcher {
+    std::time_t time_;
+    std::tm tm_;
+
+    dispatcher(std::time_t t) : time_(t) {}
+
+    bool run() {
+      using namespace fmt::detail;
+      return handle(localtime_r(&time_, &tm_));
+    }
+
+    bool handle(std::tm* tm) { return tm != nullptr; }
+
+    bool handle(detail::null<>) {
+      using namespace fmt::detail;
+      return fallback(localtime_s(&tm_, &time_));
+    }
+
+    bool fallback(int res) { return res == 0; }
+
+#if !FMT_MSC_VER
+    bool fallback(detail::null<>) {
+      using namespace fmt::detail;
+      std::tm* tm = std::localtime(&time_);
+      if (tm) tm_ = *tm;
+      return tm != nullptr;
+    }
+#endif
+  };
+  dispatcher lt(time);
+  // Too big time values may be unsupported.
+  if (!lt.run()) FMT_THROW(format_error("time_t value out of range"));
+  return lt.tm_;
+}
+
+inline std::tm localtime(
+    std::chrono::time_point<std::chrono::system_clock> time_point) {
+  return localtime(std::chrono::system_clock::to_time_t(time_point));
+}
+
+/**
+  Converts given time since epoch as ``std::time_t`` value into calendar time,
+  expressed in Coordinated Universal Time (UTC). Unlike ``std::gmtime``, this
+  function is thread-safe on most platforms.
+ */
+inline std::tm gmtime(std::time_t time) {
+  struct dispatcher {
+    std::time_t time_;
+    std::tm tm_;
+
+    dispatcher(std::time_t t) : time_(t) {}
+
+    bool run() {
+      using namespace fmt::detail;
+      return handle(gmtime_r(&time_, &tm_));
+    }
+
+    bool handle(std::tm* tm) { return tm != nullptr; }
+
+    bool handle(detail::null<>) {
+      using namespace fmt::detail;
+      return fallback(gmtime_s(&tm_, &time_));
+    }
+
+    bool fallback(int res) { return res == 0; }
+
+#if !FMT_MSC_VER
+    bool fallback(detail::null<>) {
+      std::tm* tm = std::gmtime(&time_);
+      if (tm) tm_ = *tm;
+      return tm != nullptr;
+    }
+#endif
+  };
+  dispatcher gt(time);
+  // Too big time values may be unsupported.
+  if (!gt.run()) FMT_THROW(format_error("time_t value out of range"));
+  return gt.tm_;
+}
+
+inline std::tm gmtime(
+    std::chrono::time_point<std::chrono::system_clock> time_point) {
+  return gmtime(std::chrono::system_clock::to_time_t(time_point));
+}
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+inline size_t strftime(char* str, size_t count, const char* format,
+                       const std::tm* time) {
+  // Assign to a pointer to suppress GCCs -Wformat-nonliteral
+  // First assign the nullptr to suppress -Wsuggest-attribute=format
+  std::size_t (*strftime)(char*, std::size_t, const char*, const std::tm*) =
+      nullptr;
+  strftime = std::strftime;
+  return strftime(str, count, format, time);
+}
+
+inline size_t strftime(wchar_t* str, size_t count, const wchar_t* format,
+                       const std::tm* time) {
+  // See above
+  std::size_t (*wcsftime)(wchar_t*, std::size_t, const wchar_t*,
+                          const std::tm*) = nullptr;
+  wcsftime = std::wcsftime;
+  return wcsftime(str, count, format, time);
+}
+
+FMT_END_DETAIL_NAMESPACE
+
+template <typename Char, typename Duration>
+struct formatter<std::chrono::time_point<std::chrono::system_clock, Duration>,
+                 Char> : formatter<std::tm, Char> {
+  FMT_CONSTEXPR formatter() {
+    this->specs = {default_specs, sizeof(default_specs) / sizeof(Char)};
+  }
+
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    auto it = ctx.begin();
+    if (it != ctx.end() && *it == ':') ++it;
+    auto end = it;
+    while (end != ctx.end() && *end != '}') ++end;
+    if (end != it) this->specs = {it, detail::to_unsigned(end - it)};
+    return end;
+  }
+
+  template <typename FormatContext>
+  auto format(std::chrono::time_point<std::chrono::system_clock> val,
+              FormatContext& ctx) -> decltype(ctx.out()) {
+    std::tm time = localtime(val);
+    return formatter<std::tm, Char>::format(time, ctx);
+  }
+
+  static constexpr Char default_specs[] = {'%', 'Y', '-', '%', 'm', '-',
+                                           '%', 'd', ' ', '%', 'H', ':',
+                                           '%', 'M', ':', '%', 'S'};
+};
+
+template <typename Char, typename Duration>
+constexpr Char
+    formatter<std::chrono::time_point<std::chrono::system_clock, Duration>,
+              Char>::default_specs[];
+
+template <typename Char> struct formatter<std::tm, Char> {
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    auto it = ctx.begin();
+    if (it != ctx.end() && *it == ':') ++it;
+    auto end = it;
+    while (end != ctx.end() && *end != '}') ++end;
+    specs = {it, detail::to_unsigned(end - it)};
+    return end;
+  }
+
+  template <typename FormatContext>
+  auto format(const std::tm& tm, FormatContext& ctx) const
+      -> decltype(ctx.out()) {
+    basic_memory_buffer<Char> tm_format;
+    tm_format.append(specs.begin(), specs.end());
+    // By appending an extra space we can distinguish an empty result that
+    // indicates insufficient buffer size from a guaranteed non-empty result
+    // https://github.com/fmtlib/fmt/issues/2238
+    tm_format.push_back(' ');
+    tm_format.push_back('\0');
+    basic_memory_buffer<Char> buf;
+    size_t start = buf.size();
+    for (;;) {
+      size_t size = buf.capacity() - start;
+      size_t count = detail::strftime(&buf[start], size, &tm_format[0], &tm);
+      if (count != 0) {
+        buf.resize(start + count);
+        break;
+      }
+      const size_t MIN_GROWTH = 10;
+      buf.reserve(buf.capacity() + (size > MIN_GROWTH ? size : MIN_GROWTH));
+    }
+    // Remove the extra space.
+    return std::copy(buf.begin(), buf.end() - 1, ctx.out());
+  }
+
+  basic_string_view<Char> specs;
+};
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+template <typename Period> FMT_CONSTEXPR inline const char* get_units() {
+  if (std::is_same<Period, std::atto>::value) return "as";
+  if (std::is_same<Period, std::femto>::value) return "fs";
+  if (std::is_same<Period, std::pico>::value) return "ps";
+  if (std::is_same<Period, std::nano>::value) return "ns";
+  if (std::is_same<Period, std::micro>::value) return "µs";
+  if (std::is_same<Period, std::milli>::value) return "ms";
+  if (std::is_same<Period, std::centi>::value) return "cs";
+  if (std::is_same<Period, std::deci>::value) return "ds";
+  if (std::is_same<Period, std::ratio<1>>::value) return "s";
+  if (std::is_same<Period, std::deca>::value) return "das";
+  if (std::is_same<Period, std::hecto>::value) return "hs";
+  if (std::is_same<Period, std::kilo>::value) return "ks";
+  if (std::is_same<Period, std::mega>::value) return "Ms";
+  if (std::is_same<Period, std::giga>::value) return "Gs";
+  if (std::is_same<Period, std::tera>::value) return "Ts";
+  if (std::is_same<Period, std::peta>::value) return "Ps";
+  if (std::is_same<Period, std::exa>::value) return "Es";
+  if (std::is_same<Period, std::ratio<60>>::value) return "m";
+  if (std::is_same<Period, std::ratio<3600>>::value) return "h";
+  return nullptr;
+}
+
+enum class numeric_system {
+  standard,
+  // Alternative numeric system, e.g. 十二 instead of 12 in ja_JP locale.
+  alternative
+};
+
+// Parses a put_time-like format string and invokes handler actions.
+template <typename Char, typename Handler>
+FMT_CONSTEXPR const Char* parse_chrono_format(const Char* begin,
+                                              const Char* end,
+                                              Handler&& handler) {
+  auto ptr = begin;
+  while (ptr != end) {
+    auto c = *ptr;
+    if (c == '}') break;
+    if (c != '%') {
+      ++ptr;
+      continue;
+    }
+    if (begin != ptr) handler.on_text(begin, ptr);
+    ++ptr;  // consume '%'
+    if (ptr == end) FMT_THROW(format_error("invalid format"));
+    c = *ptr++;
+    switch (c) {
+    case '%':
+      handler.on_text(ptr - 1, ptr);
+      break;
+    case 'n': {
+      const Char newline[] = {'\n'};
+      handler.on_text(newline, newline + 1);
+      break;
+    }
+    case 't': {
+      const Char tab[] = {'\t'};
+      handler.on_text(tab, tab + 1);
+      break;
+    }
+    // Day of the week:
+    case 'a':
+      handler.on_abbr_weekday();
+      break;
+    case 'A':
+      handler.on_full_weekday();
+      break;
+    case 'w':
+      handler.on_dec0_weekday(numeric_system::standard);
+      break;
+    case 'u':
+      handler.on_dec1_weekday(numeric_system::standard);
+      break;
+    // Month:
+    case 'b':
+      handler.on_abbr_month();
+      break;
+    case 'B':
+      handler.on_full_month();
+      break;
+    // Hour, minute, second:
+    case 'H':
+      handler.on_24_hour(numeric_system::standard);
+      break;
+    case 'I':
+      handler.on_12_hour(numeric_system::standard);
+      break;
+    case 'M':
+      handler.on_minute(numeric_system::standard);
+      break;
+    case 'S':
+      handler.on_second(numeric_system::standard);
+      break;
+    // Other:
+    case 'c':
+      handler.on_datetime(numeric_system::standard);
+      break;
+    case 'x':
+      handler.on_loc_date(numeric_system::standard);
+      break;
+    case 'X':
+      handler.on_loc_time(numeric_system::standard);
+      break;
+    case 'D':
+      handler.on_us_date();
+      break;
+    case 'F':
+      handler.on_iso_date();
+      break;
+    case 'r':
+      handler.on_12_hour_time();
+      break;
+    case 'R':
+      handler.on_24_hour_time();
+      break;
+    case 'T':
+      handler.on_iso_time();
+      break;
+    case 'p':
+      handler.on_am_pm();
+      break;
+    case 'Q':
+      handler.on_duration_value();
+      break;
+    case 'q':
+      handler.on_duration_unit();
+      break;
+    case 'z':
+      handler.on_utc_offset();
+      break;
+    case 'Z':
+      handler.on_tz_name();
+      break;
+    // Alternative representation:
+    case 'E': {
+      if (ptr == end) FMT_THROW(format_error("invalid format"));
+      c = *ptr++;
+      switch (c) {
+      case 'c':
+        handler.on_datetime(numeric_system::alternative);
+        break;
+      case 'x':
+        handler.on_loc_date(numeric_system::alternative);
+        break;
+      case 'X':
+        handler.on_loc_time(numeric_system::alternative);
+        break;
+      default:
+        FMT_THROW(format_error("invalid format"));
+      }
+      break;
+    }
+    case 'O':
+      if (ptr == end) FMT_THROW(format_error("invalid format"));
+      c = *ptr++;
+      switch (c) {
+      case 'w':
+        handler.on_dec0_weekday(numeric_system::alternative);
+        break;
+      case 'u':
+        handler.on_dec1_weekday(numeric_system::alternative);
+        break;
+      case 'H':
+        handler.on_24_hour(numeric_system::alternative);
+        break;
+      case 'I':
+        handler.on_12_hour(numeric_system::alternative);
+        break;
+      case 'M':
+        handler.on_minute(numeric_system::alternative);
+        break;
+      case 'S':
+        handler.on_second(numeric_system::alternative);
+        break;
+      default:
+        FMT_THROW(format_error("invalid format"));
+      }
+      break;
+    default:
+      FMT_THROW(format_error("invalid format"));
+    }
+    begin = ptr;
+  }
+  if (begin != ptr) handler.on_text(begin, ptr);
+  return ptr;
+}
+
+template <typename Derived> struct null_chrono_spec_handler {
+  FMT_CONSTEXPR void unsupported() {
+    static_cast<Derived*>(this)->unsupported();
+  }
+  FMT_CONSTEXPR void on_abbr_weekday() { unsupported(); }
+  FMT_CONSTEXPR void on_full_weekday() { unsupported(); }
+  FMT_CONSTEXPR void on_dec0_weekday(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_dec1_weekday(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_abbr_month() { unsupported(); }
+  FMT_CONSTEXPR void on_full_month() { unsupported(); }
+  FMT_CONSTEXPR void on_24_hour(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_12_hour(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_minute(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_second(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_datetime(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_loc_date(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_loc_time(numeric_system) { unsupported(); }
+  FMT_CONSTEXPR void on_us_date() { unsupported(); }
+  FMT_CONSTEXPR void on_iso_date() { unsupported(); }
+  FMT_CONSTEXPR void on_12_hour_time() { unsupported(); }
+  FMT_CONSTEXPR void on_24_hour_time() { unsupported(); }
+  FMT_CONSTEXPR void on_iso_time() { unsupported(); }
+  FMT_CONSTEXPR void on_am_pm() { unsupported(); }
+  FMT_CONSTEXPR void on_duration_value() { unsupported(); }
+  FMT_CONSTEXPR void on_duration_unit() { unsupported(); }
+  FMT_CONSTEXPR void on_utc_offset() { unsupported(); }
+  FMT_CONSTEXPR void on_tz_name() { unsupported(); }
+};
+
+struct chrono_format_checker : null_chrono_spec_handler<chrono_format_checker> {
+  FMT_NORETURN void unsupported() { FMT_THROW(format_error("no date")); }
+
+  template <typename Char>
+  FMT_CONSTEXPR void on_text(const Char*, const Char*) {}
+  FMT_CONSTEXPR void on_24_hour(numeric_system) {}
+  FMT_CONSTEXPR void on_12_hour(numeric_system) {}
+  FMT_CONSTEXPR void on_minute(numeric_system) {}
+  FMT_CONSTEXPR void on_second(numeric_system) {}
+  FMT_CONSTEXPR void on_12_hour_time() {}
+  FMT_CONSTEXPR void on_24_hour_time() {}
+  FMT_CONSTEXPR void on_iso_time() {}
+  FMT_CONSTEXPR void on_am_pm() {}
+  FMT_CONSTEXPR void on_duration_value() {}
+  FMT_CONSTEXPR void on_duration_unit() {}
+};
+
+template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+inline bool isnan(T) {
+  return false;
+}
+template <typename T, FMT_ENABLE_IF(std::is_floating_point<T>::value)>
+inline bool isnan(T value) {
+  return std::isnan(value);
+}
+
+template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+inline bool isfinite(T) {
+  return true;
+}
+template <typename T, FMT_ENABLE_IF(std::is_floating_point<T>::value)>
+inline bool isfinite(T value) {
+  return std::isfinite(value);
+}
+
+// Converts value to int and checks that it's in the range [0, upper).
+template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+inline int to_nonnegative_int(T value, int upper) {
+  FMT_ASSERT(value >= 0 && to_unsigned(value) <= to_unsigned(upper),
+             "invalid value");
+  (void)upper;
+  return static_cast<int>(value);
+}
+template <typename T, FMT_ENABLE_IF(!std::is_integral<T>::value)>
+inline int to_nonnegative_int(T value, int upper) {
+  FMT_ASSERT(
+      std::isnan(value) || (value >= 0 && value <= static_cast<T>(upper)),
+      "invalid value");
+  (void)upper;
+  return static_cast<int>(value);
+}
+
+template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+inline T mod(T x, int y) {
+  return x % static_cast<T>(y);
+}
+template <typename T, FMT_ENABLE_IF(std::is_floating_point<T>::value)>
+inline T mod(T x, int y) {
+  return std::fmod(x, static_cast<T>(y));
+}
+
+// If T is an integral type, maps T to its unsigned counterpart, otherwise
+// leaves it unchanged (unlike std::make_unsigned).
+template <typename T, bool INTEGRAL = std::is_integral<T>::value>
+struct make_unsigned_or_unchanged {
+  using type = T;
+};
+
+template <typename T> struct make_unsigned_or_unchanged<T, true> {
+  using type = typename std::make_unsigned<T>::type;
+};
+
+#if FMT_SAFE_DURATION_CAST
+// throwing version of safe_duration_cast
+template <typename To, typename FromRep, typename FromPeriod>
+To fmt_safe_duration_cast(std::chrono::duration<FromRep, FromPeriod> from) {
+  int ec;
+  To to = safe_duration_cast::safe_duration_cast<To>(from, ec);
+  if (ec) FMT_THROW(format_error("cannot format duration"));
+  return to;
+}
+#endif
+
+template <typename Rep, typename Period,
+          FMT_ENABLE_IF(std::is_integral<Rep>::value)>
+inline std::chrono::duration<Rep, std::milli> get_milliseconds(
+    std::chrono::duration<Rep, Period> d) {
+  // this may overflow and/or the result may not fit in the
+  // target type.
+#if FMT_SAFE_DURATION_CAST
+  using CommonSecondsType =
+      typename std::common_type<decltype(d), std::chrono::seconds>::type;
+  const auto d_as_common = fmt_safe_duration_cast<CommonSecondsType>(d);
+  const auto d_as_whole_seconds =
+      fmt_safe_duration_cast<std::chrono::seconds>(d_as_common);
+  // this conversion should be nonproblematic
+  const auto diff = d_as_common - d_as_whole_seconds;
+  const auto ms =
+      fmt_safe_duration_cast<std::chrono::duration<Rep, std::milli>>(diff);
+  return ms;
+#else
+  auto s = std::chrono::duration_cast<std::chrono::seconds>(d);
+  return std::chrono::duration_cast<std::chrono::milliseconds>(d - s);
+#endif
+}
+
+template <typename Rep, typename Period,
+          FMT_ENABLE_IF(std::is_floating_point<Rep>::value)>
+inline std::chrono::duration<Rep, std::milli> get_milliseconds(
+    std::chrono::duration<Rep, Period> d) {
+  using common_type = typename std::common_type<Rep, std::intmax_t>::type;
+  auto ms = mod(d.count() * static_cast<common_type>(Period::num) /
+                    static_cast<common_type>(Period::den) * 1000,
+                1000);
+  return std::chrono::duration<Rep, std::milli>(static_cast<Rep>(ms));
+}
+
+template <typename Char, typename Rep, typename OutputIt,
+          FMT_ENABLE_IF(std::is_integral<Rep>::value)>
+OutputIt format_duration_value(OutputIt out, Rep val, int) {
+  return write<Char>(out, val);
+}
+
+template <typename Char, typename Rep, typename OutputIt,
+          FMT_ENABLE_IF(std::is_floating_point<Rep>::value)>
+OutputIt format_duration_value(OutputIt out, Rep val, int precision) {
+  auto specs = basic_format_specs<Char>();
+  specs.precision = precision;
+  specs.type = precision > 0 ? 'f' : 'g';
+  return write<Char>(out, val, specs);
+}
+
+template <typename Char, typename OutputIt>
+OutputIt copy_unit(string_view unit, OutputIt out, Char) {
+  return std::copy(unit.begin(), unit.end(), out);
+}
+
+template <typename OutputIt>
+OutputIt copy_unit(string_view unit, OutputIt out, wchar_t) {
+  // This works when wchar_t is UTF-32 because units only contain characters
+  // that have the same representation in UTF-16 and UTF-32.
+  utf8_to_utf16 u(unit);
+  return std::copy(u.c_str(), u.c_str() + u.size(), out);
+}
+
+template <typename Char, typename Period, typename OutputIt>
+OutputIt format_duration_unit(OutputIt out) {
+  if (const char* unit = get_units<Period>())
+    return copy_unit(string_view(unit), out, Char());
+  *out++ = '[';
+  out = write<Char>(out, Period::num);
+  if (const_check(Period::den != 1)) {
+    *out++ = '/';
+    out = write<Char>(out, Period::den);
+  }
+  *out++ = ']';
+  *out++ = 's';
+  return out;
+}
+
+template <typename FormatContext, typename OutputIt, typename Rep,
+          typename Period>
+struct chrono_formatter {
+  FormatContext& context;
+  OutputIt out;
+  int precision;
+  bool localized = false;
+  // rep is unsigned to avoid overflow.
+  using rep =
+      conditional_t<std::is_integral<Rep>::value && sizeof(Rep) < sizeof(int),
+                    unsigned, typename make_unsigned_or_unchanged<Rep>::type>;
+  rep val;
+  using seconds = std::chrono::duration<rep>;
+  seconds s;
+  using milliseconds = std::chrono::duration<rep, std::milli>;
+  bool negative;
+
+  using char_type = typename FormatContext::char_type;
+
+  explicit chrono_formatter(FormatContext& ctx, OutputIt o,
+                            std::chrono::duration<Rep, Period> d)
+      : context(ctx),
+        out(o),
+        val(static_cast<rep>(d.count())),
+        negative(false) {
+    if (d.count() < 0) {
+      val = 0 - val;
+      negative = true;
+    }
+
+    // this may overflow and/or the result may not fit in the
+    // target type.
+#if FMT_SAFE_DURATION_CAST
+    // might need checked conversion (rep!=Rep)
+    auto tmpval = std::chrono::duration<rep, Period>(val);
+    s = fmt_safe_duration_cast<seconds>(tmpval);
+#else
+    s = std::chrono::duration_cast<seconds>(
+        std::chrono::duration<rep, Period>(val));
+#endif
+  }
+
+  // returns true if nan or inf, writes to out.
+  bool handle_nan_inf() {
+    if (isfinite(val)) {
+      return false;
+    }
+    if (isnan(val)) {
+      write_nan();
+      return true;
+    }
+    // must be +-inf
+    if (val > 0) {
+      write_pinf();
+    } else {
+      write_ninf();
+    }
+    return true;
+  }
+
+  Rep hour() const { return static_cast<Rep>(mod((s.count() / 3600), 24)); }
+
+  Rep hour12() const {
+    Rep hour = static_cast<Rep>(mod((s.count() / 3600), 12));
+    return hour <= 0 ? 12 : hour;
+  }
+
+  Rep minute() const { return static_cast<Rep>(mod((s.count() / 60), 60)); }
+  Rep second() const { return static_cast<Rep>(mod(s.count(), 60)); }
+
+  std::tm time() const {
+    auto time = std::tm();
+    time.tm_hour = to_nonnegative_int(hour(), 24);
+    time.tm_min = to_nonnegative_int(minute(), 60);
+    time.tm_sec = to_nonnegative_int(second(), 60);
+    return time;
+  }
+
+  void write_sign() {
+    if (negative) {
+      *out++ = '-';
+      negative = false;
+    }
+  }
+
+  void write(Rep value, int width) {
+    write_sign();
+    if (isnan(value)) return write_nan();
+    uint32_or_64_or_128_t<int> n =
+        to_unsigned(to_nonnegative_int(value, max_value<int>()));
+    int num_digits = detail::count_digits(n);
+    if (width > num_digits) out = std::fill_n(out, width - num_digits, '0');
+    out = format_decimal<char_type>(out, n, num_digits).end;
+  }
+
+  void write_nan() { std::copy_n("nan", 3, out); }
+  void write_pinf() { std::copy_n("inf", 3, out); }
+  void write_ninf() { std::copy_n("-inf", 4, out); }
+
+  void format_localized(const tm& time, char format, char modifier = 0) {
+    if (isnan(val)) return write_nan();
+    const auto& loc = localized ? context.locale().template get<std::locale>()
+                                : std::locale::classic();
+    out = detail::write(out, time, loc, format, modifier);
+  }
+
+  void on_text(const char_type* begin, const char_type* end) {
+    std::copy(begin, end, out);
+  }
+
+  // These are not implemented because durations don't have date information.
+  void on_abbr_weekday() {}
+  void on_full_weekday() {}
+  void on_dec0_weekday(numeric_system) {}
+  void on_dec1_weekday(numeric_system) {}
+  void on_abbr_month() {}
+  void on_full_month() {}
+  void on_datetime(numeric_system) {}
+  void on_loc_date(numeric_system) {}
+  void on_loc_time(numeric_system) {}
+  void on_us_date() {}
+  void on_iso_date() {}
+  void on_utc_offset() {}
+  void on_tz_name() {}
+
+  void on_24_hour(numeric_system ns) {
+    if (handle_nan_inf()) return;
+
+    if (ns == numeric_system::standard) return write(hour(), 2);
+    auto time = tm();
+    time.tm_hour = to_nonnegative_int(hour(), 24);
+    format_localized(time, 'H', 'O');
+  }
+
+  void on_12_hour(numeric_system ns) {
+    if (handle_nan_inf()) return;
+
+    if (ns == numeric_system::standard) return write(hour12(), 2);
+    auto time = tm();
+    time.tm_hour = to_nonnegative_int(hour12(), 12);
+    format_localized(time, 'I', 'O');
+  }
+
+  void on_minute(numeric_system ns) {
+    if (handle_nan_inf()) return;
+
+    if (ns == numeric_system::standard) return write(minute(), 2);
+    auto time = tm();
+    time.tm_min = to_nonnegative_int(minute(), 60);
+    format_localized(time, 'M', 'O');
+  }
+
+  void on_second(numeric_system ns) {
+    if (handle_nan_inf()) return;
+
+    if (ns == numeric_system::standard) {
+      write(second(), 2);
+#if FMT_SAFE_DURATION_CAST
+      // convert rep->Rep
+      using duration_rep = std::chrono::duration<rep, Period>;
+      using duration_Rep = std::chrono::duration<Rep, Period>;
+      auto tmpval = fmt_safe_duration_cast<duration_Rep>(duration_rep{val});
+#else
+      auto tmpval = std::chrono::duration<Rep, Period>(val);
+#endif
+      auto ms = get_milliseconds(tmpval);
+      if (ms != std::chrono::milliseconds(0)) {
+        *out++ = '.';
+        write(ms.count(), 3);
+      }
+      return;
+    }
+    auto time = tm();
+    time.tm_sec = to_nonnegative_int(second(), 60);
+    format_localized(time, 'S', 'O');
+  }
+
+  void on_12_hour_time() {
+    if (handle_nan_inf()) return;
+    format_localized(time(), 'r');
+  }
+
+  void on_24_hour_time() {
+    if (handle_nan_inf()) {
+      *out++ = ':';
+      handle_nan_inf();
+      return;
+    }
+
+    write(hour(), 2);
+    *out++ = ':';
+    write(minute(), 2);
+  }
+
+  void on_iso_time() {
+    on_24_hour_time();
+    *out++ = ':';
+    if (handle_nan_inf()) return;
+    write(second(), 2);
+  }
+
+  void on_am_pm() {
+    if (handle_nan_inf()) return;
+    format_localized(time(), 'p');
+  }
+
+  void on_duration_value() {
+    if (handle_nan_inf()) return;
+    write_sign();
+    out = format_duration_value<char_type>(out, val, precision);
+  }
+
+  void on_duration_unit() {
+    out = format_duration_unit<char_type, Period>(out);
+  }
+};
+
+FMT_END_DETAIL_NAMESPACE
+
+#if defined(__cpp_lib_chrono) && __cpp_lib_chrono >= 201907
+using weekday = std::chrono::weekday;
+#else
+// A fallback version of weekday.
+class weekday {
+ private:
+  unsigned char value;
+
+ public:
+  weekday() = default;
+  explicit constexpr weekday(unsigned wd) noexcept
+      : value(static_cast<unsigned char>(wd != 7 ? wd : 0)) {}
+  constexpr unsigned c_encoding() const noexcept { return value; }
+};
+#endif
+
+// A rudimentary weekday formatter.
+template <> struct formatter<weekday> {
+ private:
+  bool localized = false;
+
+ public:
+  FMT_CONSTEXPR auto parse(format_parse_context& ctx) -> decltype(ctx.begin()) {
+    auto begin = ctx.begin(), end = ctx.end();
+    if (begin != end && *begin == 'L') {
+      ++begin;
+      localized = true;
+    }
+    return begin;
+  }
+
+  auto format(weekday wd, format_context& ctx) -> decltype(ctx.out()) {
+    auto time = std::tm();
+    time.tm_wday = static_cast<int>(wd.c_encoding());
+    const auto& loc = localized ? ctx.locale().template get<std::locale>()
+                                : std::locale::classic();
+    return detail::write(ctx.out(), time, loc, 'a');
+  }
+};
+
+template <typename Rep, typename Period, typename Char>
+struct formatter<std::chrono::duration<Rep, Period>, Char> {
+ private:
+  basic_format_specs<Char> specs;
+  int precision = -1;
+  using arg_ref_type = detail::arg_ref<Char>;
+  arg_ref_type width_ref;
+  arg_ref_type precision_ref;
+  bool localized = false;
+  basic_string_view<Char> format_str;
+  using duration = std::chrono::duration<Rep, Period>;
+
+  struct spec_handler {
+    formatter& f;
+    basic_format_parse_context<Char>& context;
+    basic_string_view<Char> format_str;
+
+    template <typename Id> FMT_CONSTEXPR arg_ref_type make_arg_ref(Id arg_id) {
+      context.check_arg_id(arg_id);
+      return arg_ref_type(arg_id);
+    }
+
+    FMT_CONSTEXPR arg_ref_type make_arg_ref(basic_string_view<Char> arg_id) {
+      context.check_arg_id(arg_id);
+      return arg_ref_type(arg_id);
+    }
+
+    FMT_CONSTEXPR arg_ref_type make_arg_ref(detail::auto_id) {
+      return arg_ref_type(context.next_arg_id());
+    }
+
+    void on_error(const char* msg) { FMT_THROW(format_error(msg)); }
+    FMT_CONSTEXPR void on_fill(basic_string_view<Char> fill) {
+      f.specs.fill = fill;
+    }
+    FMT_CONSTEXPR void on_align(align_t align) { f.specs.align = align; }
+    FMT_CONSTEXPR void on_width(int width) { f.specs.width = width; }
+    FMT_CONSTEXPR void on_precision(int _precision) {
+      f.precision = _precision;
+    }
+    FMT_CONSTEXPR void end_precision() {}
+
+    template <typename Id> FMT_CONSTEXPR void on_dynamic_width(Id arg_id) {
+      f.width_ref = make_arg_ref(arg_id);
+    }
+
+    template <typename Id> FMT_CONSTEXPR void on_dynamic_precision(Id arg_id) {
+      f.precision_ref = make_arg_ref(arg_id);
+    }
+  };
+
+  using iterator = typename basic_format_parse_context<Char>::iterator;
+  struct parse_range {
+    iterator begin;
+    iterator end;
+  };
+
+  FMT_CONSTEXPR parse_range do_parse(basic_format_parse_context<Char>& ctx) {
+    auto begin = ctx.begin(), end = ctx.end();
+    if (begin == end || *begin == '}') return {begin, begin};
+    spec_handler handler{*this, ctx, format_str};
+    begin = detail::parse_align(begin, end, handler);
+    if (begin == end) return {begin, begin};
+    begin = detail::parse_width(begin, end, handler);
+    if (begin == end) return {begin, begin};
+    if (*begin == '.') {
+      if (std::is_floating_point<Rep>::value)
+        begin = detail::parse_precision(begin, end, handler);
+      else
+        handler.on_error("precision not allowed for this argument type");
+    }
+    if (begin != end && *begin == 'L') {
+      ++begin;
+      localized = true;
+    }
+    end = parse_chrono_format(begin, end, detail::chrono_format_checker());
+    return {begin, end};
+  }
+
+ public:
+  FMT_CONSTEXPR auto parse(basic_format_parse_context<Char>& ctx)
+      -> decltype(ctx.begin()) {
+    auto range = do_parse(ctx);
+    format_str = basic_string_view<Char>(
+        &*range.begin, detail::to_unsigned(range.end - range.begin));
+    return range.end;
+  }
+
+  template <typename FormatContext>
+  auto format(const duration& d, FormatContext& ctx) const
+      -> decltype(ctx.out()) {
+    auto specs_copy = specs;
+    auto precision_copy = precision;
+    auto begin = format_str.begin(), end = format_str.end();
+    // As a possible future optimization, we could avoid extra copying if width
+    // is not specified.
+    basic_memory_buffer<Char> buf;
+    auto out = std::back_inserter(buf);
+    detail::handle_dynamic_spec<detail::width_checker>(specs_copy.width,
+                                                       width_ref, ctx);
+    detail::handle_dynamic_spec<detail::precision_checker>(precision_copy,
+                                                           precision_ref, ctx);
+    if (begin == end || *begin == '}') {
+      out = detail::format_duration_value<Char>(out, d.count(), precision_copy);
+      detail::format_duration_unit<Char, Period>(out);
+    } else {
+      detail::chrono_formatter<FormatContext, decltype(out), Rep, Period> f(
+          ctx, out, d);
+      f.precision = precision_copy;
+      f.localized = localized;
+      detail::parse_chrono_format(begin, end, f);
+    }
+    return detail::write(
+        ctx.out(), basic_string_view<Char>(buf.data(), buf.size()), specs_copy);
+  }
+};
+
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_CHRONO_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/color.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/color.h
new file mode 100644
index 0000000..3d5490e
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/color.h
@@ -0,0 +1,627 @@
+// Formatting library for C++ - color support
+//
+// Copyright (c) 2018 - present, Victor Zverovich and fmt contributors
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_COLOR_H_
+#define FMT_COLOR_H_
+
+#include "format.h"
+
+// __declspec(deprecated) is broken in some MSVC versions.
+#if FMT_MSC_VER
+#  define FMT_DEPRECATED_NONMSVC
+#else
+#  define FMT_DEPRECATED_NONMSVC FMT_DEPRECATED
+#endif
+
+FMT_BEGIN_NAMESPACE
+FMT_MODULE_EXPORT_BEGIN
+
+enum class color : uint32_t {
+  alice_blue = 0xF0F8FF,               // rgb(240,248,255)
+  antique_white = 0xFAEBD7,            // rgb(250,235,215)
+  aqua = 0x00FFFF,                     // rgb(0,255,255)
+  aquamarine = 0x7FFFD4,               // rgb(127,255,212)
+  azure = 0xF0FFFF,                    // rgb(240,255,255)
+  beige = 0xF5F5DC,                    // rgb(245,245,220)
+  bisque = 0xFFE4C4,                   // rgb(255,228,196)
+  black = 0x000000,                    // rgb(0,0,0)
+  blanched_almond = 0xFFEBCD,          // rgb(255,235,205)
+  blue = 0x0000FF,                     // rgb(0,0,255)
+  blue_violet = 0x8A2BE2,              // rgb(138,43,226)
+  brown = 0xA52A2A,                    // rgb(165,42,42)
+  burly_wood = 0xDEB887,               // rgb(222,184,135)
+  cadet_blue = 0x5F9EA0,               // rgb(95,158,160)
+  chartreuse = 0x7FFF00,               // rgb(127,255,0)
+  chocolate = 0xD2691E,                // rgb(210,105,30)
+  coral = 0xFF7F50,                    // rgb(255,127,80)
+  cornflower_blue = 0x6495ED,          // rgb(100,149,237)
+  cornsilk = 0xFFF8DC,                 // rgb(255,248,220)
+  crimson = 0xDC143C,                  // rgb(220,20,60)
+  cyan = 0x00FFFF,                     // rgb(0,255,255)
+  dark_blue = 0x00008B,                // rgb(0,0,139)
+  dark_cyan = 0x008B8B,                // rgb(0,139,139)
+  dark_golden_rod = 0xB8860B,          // rgb(184,134,11)
+  dark_gray = 0xA9A9A9,                // rgb(169,169,169)
+  dark_green = 0x006400,               // rgb(0,100,0)
+  dark_khaki = 0xBDB76B,               // rgb(189,183,107)
+  dark_magenta = 0x8B008B,             // rgb(139,0,139)
+  dark_olive_green = 0x556B2F,         // rgb(85,107,47)
+  dark_orange = 0xFF8C00,              // rgb(255,140,0)
+  dark_orchid = 0x9932CC,              // rgb(153,50,204)
+  dark_red = 0x8B0000,                 // rgb(139,0,0)
+  dark_salmon = 0xE9967A,              // rgb(233,150,122)
+  dark_sea_green = 0x8FBC8F,           // rgb(143,188,143)
+  dark_slate_blue = 0x483D8B,          // rgb(72,61,139)
+  dark_slate_gray = 0x2F4F4F,          // rgb(47,79,79)
+  dark_turquoise = 0x00CED1,           // rgb(0,206,209)
+  dark_violet = 0x9400D3,              // rgb(148,0,211)
+  deep_pink = 0xFF1493,                // rgb(255,20,147)
+  deep_sky_blue = 0x00BFFF,            // rgb(0,191,255)
+  dim_gray = 0x696969,                 // rgb(105,105,105)
+  dodger_blue = 0x1E90FF,              // rgb(30,144,255)
+  fire_brick = 0xB22222,               // rgb(178,34,34)
+  floral_white = 0xFFFAF0,             // rgb(255,250,240)
+  forest_green = 0x228B22,             // rgb(34,139,34)
+  fuchsia = 0xFF00FF,                  // rgb(255,0,255)
+  gainsboro = 0xDCDCDC,                // rgb(220,220,220)
+  ghost_white = 0xF8F8FF,              // rgb(248,248,255)
+  gold = 0xFFD700,                     // rgb(255,215,0)
+  golden_rod = 0xDAA520,               // rgb(218,165,32)
+  gray = 0x808080,                     // rgb(128,128,128)
+  green = 0x008000,                    // rgb(0,128,0)
+  green_yellow = 0xADFF2F,             // rgb(173,255,47)
+  honey_dew = 0xF0FFF0,                // rgb(240,255,240)
+  hot_pink = 0xFF69B4,                 // rgb(255,105,180)
+  indian_red = 0xCD5C5C,               // rgb(205,92,92)
+  indigo = 0x4B0082,                   // rgb(75,0,130)
+  ivory = 0xFFFFF0,                    // rgb(255,255,240)
+  khaki = 0xF0E68C,                    // rgb(240,230,140)
+  lavender = 0xE6E6FA,                 // rgb(230,230,250)
+  lavender_blush = 0xFFF0F5,           // rgb(255,240,245)
+  lawn_green = 0x7CFC00,               // rgb(124,252,0)
+  lemon_chiffon = 0xFFFACD,            // rgb(255,250,205)
+  light_blue = 0xADD8E6,               // rgb(173,216,230)
+  light_coral = 0xF08080,              // rgb(240,128,128)
+  light_cyan = 0xE0FFFF,               // rgb(224,255,255)
+  light_golden_rod_yellow = 0xFAFAD2,  // rgb(250,250,210)
+  light_gray = 0xD3D3D3,               // rgb(211,211,211)
+  light_green = 0x90EE90,              // rgb(144,238,144)
+  light_pink = 0xFFB6C1,               // rgb(255,182,193)
+  light_salmon = 0xFFA07A,             // rgb(255,160,122)
+  light_sea_green = 0x20B2AA,          // rgb(32,178,170)
+  light_sky_blue = 0x87CEFA,           // rgb(135,206,250)
+  light_slate_gray = 0x778899,         // rgb(119,136,153)
+  light_steel_blue = 0xB0C4DE,         // rgb(176,196,222)
+  light_yellow = 0xFFFFE0,             // rgb(255,255,224)
+  lime = 0x00FF00,                     // rgb(0,255,0)
+  lime_green = 0x32CD32,               // rgb(50,205,50)
+  linen = 0xFAF0E6,                    // rgb(250,240,230)
+  magenta = 0xFF00FF,                  // rgb(255,0,255)
+  maroon = 0x800000,                   // rgb(128,0,0)
+  medium_aquamarine = 0x66CDAA,        // rgb(102,205,170)
+  medium_blue = 0x0000CD,              // rgb(0,0,205)
+  medium_orchid = 0xBA55D3,            // rgb(186,85,211)
+  medium_purple = 0x9370DB,            // rgb(147,112,219)
+  medium_sea_green = 0x3CB371,         // rgb(60,179,113)
+  medium_slate_blue = 0x7B68EE,        // rgb(123,104,238)
+  medium_spring_green = 0x00FA9A,      // rgb(0,250,154)
+  medium_turquoise = 0x48D1CC,         // rgb(72,209,204)
+  medium_violet_red = 0xC71585,        // rgb(199,21,133)
+  midnight_blue = 0x191970,            // rgb(25,25,112)
+  mint_cream = 0xF5FFFA,               // rgb(245,255,250)
+  misty_rose = 0xFFE4E1,               // rgb(255,228,225)
+  moccasin = 0xFFE4B5,                 // rgb(255,228,181)
+  navajo_white = 0xFFDEAD,             // rgb(255,222,173)
+  navy = 0x000080,                     // rgb(0,0,128)
+  old_lace = 0xFDF5E6,                 // rgb(253,245,230)
+  olive = 0x808000,                    // rgb(128,128,0)
+  olive_drab = 0x6B8E23,               // rgb(107,142,35)
+  orange = 0xFFA500,                   // rgb(255,165,0)
+  orange_red = 0xFF4500,               // rgb(255,69,0)
+  orchid = 0xDA70D6,                   // rgb(218,112,214)
+  pale_golden_rod = 0xEEE8AA,          // rgb(238,232,170)
+  pale_green = 0x98FB98,               // rgb(152,251,152)
+  pale_turquoise = 0xAFEEEE,           // rgb(175,238,238)
+  pale_violet_red = 0xDB7093,          // rgb(219,112,147)
+  papaya_whip = 0xFFEFD5,              // rgb(255,239,213)
+  peach_puff = 0xFFDAB9,               // rgb(255,218,185)
+  peru = 0xCD853F,                     // rgb(205,133,63)
+  pink = 0xFFC0CB,                     // rgb(255,192,203)
+  plum = 0xDDA0DD,                     // rgb(221,160,221)
+  powder_blue = 0xB0E0E6,              // rgb(176,224,230)
+  purple = 0x800080,                   // rgb(128,0,128)
+  rebecca_purple = 0x663399,           // rgb(102,51,153)
+  red = 0xFF0000,                      // rgb(255,0,0)
+  rosy_brown = 0xBC8F8F,               // rgb(188,143,143)
+  royal_blue = 0x4169E1,               // rgb(65,105,225)
+  saddle_brown = 0x8B4513,             // rgb(139,69,19)
+  salmon = 0xFA8072,                   // rgb(250,128,114)
+  sandy_brown = 0xF4A460,              // rgb(244,164,96)
+  sea_green = 0x2E8B57,                // rgb(46,139,87)
+  sea_shell = 0xFFF5EE,                // rgb(255,245,238)
+  sienna = 0xA0522D,                   // rgb(160,82,45)
+  silver = 0xC0C0C0,                   // rgb(192,192,192)
+  sky_blue = 0x87CEEB,                 // rgb(135,206,235)
+  slate_blue = 0x6A5ACD,               // rgb(106,90,205)
+  slate_gray = 0x708090,               // rgb(112,128,144)
+  snow = 0xFFFAFA,                     // rgb(255,250,250)
+  spring_green = 0x00FF7F,             // rgb(0,255,127)
+  steel_blue = 0x4682B4,               // rgb(70,130,180)
+  tan = 0xD2B48C,                      // rgb(210,180,140)
+  teal = 0x008080,                     // rgb(0,128,128)
+  thistle = 0xD8BFD8,                  // rgb(216,191,216)
+  tomato = 0xFF6347,                   // rgb(255,99,71)
+  turquoise = 0x40E0D0,                // rgb(64,224,208)
+  violet = 0xEE82EE,                   // rgb(238,130,238)
+  wheat = 0xF5DEB3,                    // rgb(245,222,179)
+  white = 0xFFFFFF,                    // rgb(255,255,255)
+  white_smoke = 0xF5F5F5,              // rgb(245,245,245)
+  yellow = 0xFFFF00,                   // rgb(255,255,0)
+  yellow_green = 0x9ACD32              // rgb(154,205,50)
+};                                     // enum class color
+
+enum class terminal_color : uint8_t {
+  black = 30,
+  red,
+  green,
+  yellow,
+  blue,
+  magenta,
+  cyan,
+  white,
+  bright_black = 90,
+  bright_red,
+  bright_green,
+  bright_yellow,
+  bright_blue,
+  bright_magenta,
+  bright_cyan,
+  bright_white
+};
+
+enum class emphasis : uint8_t {
+  bold = 1,
+  italic = 1 << 1,
+  underline = 1 << 2,
+  strikethrough = 1 << 3
+};
+
+// rgb is a struct for red, green and blue colors.
+// Using the name "rgb" makes some editors show the color in a tooltip.
+struct rgb {
+  FMT_CONSTEXPR rgb() : r(0), g(0), b(0) {}
+  FMT_CONSTEXPR rgb(uint8_t r_, uint8_t g_, uint8_t b_) : r(r_), g(g_), b(b_) {}
+  FMT_CONSTEXPR rgb(uint32_t hex)
+      : r((hex >> 16) & 0xFF), g((hex >> 8) & 0xFF), b(hex & 0xFF) {}
+  FMT_CONSTEXPR rgb(color hex)
+      : r((uint32_t(hex) >> 16) & 0xFF),
+        g((uint32_t(hex) >> 8) & 0xFF),
+        b(uint32_t(hex) & 0xFF) {}
+  uint8_t r;
+  uint8_t g;
+  uint8_t b;
+};
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+// color is a struct of either a rgb color or a terminal color.
+struct color_type {
+  FMT_CONSTEXPR color_type() FMT_NOEXCEPT : is_rgb(), value{} {}
+  FMT_CONSTEXPR color_type(color rgb_color) FMT_NOEXCEPT : is_rgb(true),
+                                                           value{} {
+    value.rgb_color = static_cast<uint32_t>(rgb_color);
+  }
+  FMT_CONSTEXPR color_type(rgb rgb_color) FMT_NOEXCEPT : is_rgb(true), value{} {
+    value.rgb_color = (static_cast<uint32_t>(rgb_color.r) << 16) |
+                      (static_cast<uint32_t>(rgb_color.g) << 8) | rgb_color.b;
+  }
+  FMT_CONSTEXPR color_type(terminal_color term_color) FMT_NOEXCEPT : is_rgb(),
+                                                                     value{} {
+    value.term_color = static_cast<uint8_t>(term_color);
+  }
+  bool is_rgb;
+  union color_union {
+    uint8_t term_color;
+    uint32_t rgb_color;
+  } value;
+};
+
+FMT_END_DETAIL_NAMESPACE
+
+/** A text style consisting of foreground and background colors and emphasis. */
+class text_style {
+ public:
+  FMT_CONSTEXPR text_style(emphasis em = emphasis()) FMT_NOEXCEPT
+      : set_foreground_color(),
+        set_background_color(),
+        ems(em) {}
+
+  FMT_CONSTEXPR text_style& operator|=(const text_style& rhs) {
+    if (!set_foreground_color) {
+      set_foreground_color = rhs.set_foreground_color;
+      foreground_color = rhs.foreground_color;
+    } else if (rhs.set_foreground_color) {
+      if (!foreground_color.is_rgb || !rhs.foreground_color.is_rgb)
+        FMT_THROW(format_error("can't OR a terminal color"));
+      foreground_color.value.rgb_color |= rhs.foreground_color.value.rgb_color;
+    }
+
+    if (!set_background_color) {
+      set_background_color = rhs.set_background_color;
+      background_color = rhs.background_color;
+    } else if (rhs.set_background_color) {
+      if (!background_color.is_rgb || !rhs.background_color.is_rgb)
+        FMT_THROW(format_error("can't OR a terminal color"));
+      background_color.value.rgb_color |= rhs.background_color.value.rgb_color;
+    }
+
+    ems = static_cast<emphasis>(static_cast<uint8_t>(ems) |
+                                static_cast<uint8_t>(rhs.ems));
+    return *this;
+  }
+
+  friend FMT_CONSTEXPR text_style operator|(text_style lhs,
+                                            const text_style& rhs) {
+    return lhs |= rhs;
+  }
+
+  FMT_DEPRECATED_NONMSVC FMT_CONSTEXPR text_style& operator&=(
+      const text_style& rhs) {
+    return and_assign(rhs);
+  }
+
+  FMT_DEPRECATED_NONMSVC friend FMT_CONSTEXPR text_style
+  operator&(text_style lhs, const text_style& rhs) {
+    return lhs.and_assign(rhs);
+  }
+
+  FMT_CONSTEXPR bool has_foreground() const FMT_NOEXCEPT {
+    return set_foreground_color;
+  }
+  FMT_CONSTEXPR bool has_background() const FMT_NOEXCEPT {
+    return set_background_color;
+  }
+  FMT_CONSTEXPR bool has_emphasis() const FMT_NOEXCEPT {
+    return static_cast<uint8_t>(ems) != 0;
+  }
+  FMT_CONSTEXPR detail::color_type get_foreground() const FMT_NOEXCEPT {
+    FMT_ASSERT(has_foreground(), "no foreground specified for this style");
+    return foreground_color;
+  }
+  FMT_CONSTEXPR detail::color_type get_background() const FMT_NOEXCEPT {
+    FMT_ASSERT(has_background(), "no background specified for this style");
+    return background_color;
+  }
+  FMT_CONSTEXPR emphasis get_emphasis() const FMT_NOEXCEPT {
+    FMT_ASSERT(has_emphasis(), "no emphasis specified for this style");
+    return ems;
+  }
+
+ private:
+  FMT_CONSTEXPR text_style(bool is_foreground,
+                           detail::color_type text_color) FMT_NOEXCEPT
+      : set_foreground_color(),
+        set_background_color(),
+        ems() {
+    if (is_foreground) {
+      foreground_color = text_color;
+      set_foreground_color = true;
+    } else {
+      background_color = text_color;
+      set_background_color = true;
+    }
+  }
+
+  // DEPRECATED!
+  FMT_CONSTEXPR text_style& and_assign(const text_style& rhs) {
+    if (!set_foreground_color) {
+      set_foreground_color = rhs.set_foreground_color;
+      foreground_color = rhs.foreground_color;
+    } else if (rhs.set_foreground_color) {
+      if (!foreground_color.is_rgb || !rhs.foreground_color.is_rgb)
+        FMT_THROW(format_error("can't AND a terminal color"));
+      foreground_color.value.rgb_color &= rhs.foreground_color.value.rgb_color;
+    }
+
+    if (!set_background_color) {
+      set_background_color = rhs.set_background_color;
+      background_color = rhs.background_color;
+    } else if (rhs.set_background_color) {
+      if (!background_color.is_rgb || !rhs.background_color.is_rgb)
+        FMT_THROW(format_error("can't AND a terminal color"));
+      background_color.value.rgb_color &= rhs.background_color.value.rgb_color;
+    }
+
+    ems = static_cast<emphasis>(static_cast<uint8_t>(ems) &
+                                static_cast<uint8_t>(rhs.ems));
+    return *this;
+  }
+
+  friend FMT_CONSTEXPR_DECL text_style fg(detail::color_type foreground)
+      FMT_NOEXCEPT;
+
+  friend FMT_CONSTEXPR_DECL text_style bg(detail::color_type background)
+      FMT_NOEXCEPT;
+
+  detail::color_type foreground_color;
+  detail::color_type background_color;
+  bool set_foreground_color;
+  bool set_background_color;
+  emphasis ems;
+};
+
+/** Creates a text style from the foreground (text) color. */
+FMT_CONSTEXPR inline text_style fg(detail::color_type foreground) FMT_NOEXCEPT {
+  return text_style(true, foreground);
+}
+
+/** Creates a text style from the background color. */
+FMT_CONSTEXPR inline text_style bg(detail::color_type background) FMT_NOEXCEPT {
+  return text_style(false, background);
+}
+
+FMT_CONSTEXPR inline text_style operator|(emphasis lhs,
+                                          emphasis rhs) FMT_NOEXCEPT {
+  return text_style(lhs) | rhs;
+}
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+template <typename Char> struct ansi_color_escape {
+  FMT_CONSTEXPR ansi_color_escape(detail::color_type text_color,
+                                  const char* esc) FMT_NOEXCEPT {
+    // If we have a terminal color, we need to output another escape code
+    // sequence.
+    if (!text_color.is_rgb) {
+      bool is_background = esc == string_view("\x1b[48;2;");
+      uint32_t value = text_color.value.term_color;
+      // Background ASCII codes are the same as the foreground ones but with
+      // 10 more.
+      if (is_background) value += 10u;
+
+      size_t index = 0;
+      buffer[index++] = static_cast<Char>('\x1b');
+      buffer[index++] = static_cast<Char>('[');
+
+      if (value >= 100u) {
+        buffer[index++] = static_cast<Char>('1');
+        value %= 100u;
+      }
+      buffer[index++] = static_cast<Char>('0' + value / 10u);
+      buffer[index++] = static_cast<Char>('0' + value % 10u);
+
+      buffer[index++] = static_cast<Char>('m');
+      buffer[index++] = static_cast<Char>('\0');
+      return;
+    }
+
+    for (int i = 0; i < 7; i++) {
+      buffer[i] = static_cast<Char>(esc[i]);
+    }
+    rgb color(text_color.value.rgb_color);
+    to_esc(color.r, buffer + 7, ';');
+    to_esc(color.g, buffer + 11, ';');
+    to_esc(color.b, buffer + 15, 'm');
+    buffer[19] = static_cast<Char>(0);
+  }
+  FMT_CONSTEXPR ansi_color_escape(emphasis em) FMT_NOEXCEPT {
+    uint8_t em_codes[4] = {};
+    uint8_t em_bits = static_cast<uint8_t>(em);
+    if (em_bits & static_cast<uint8_t>(emphasis::bold)) em_codes[0] = 1;
+    if (em_bits & static_cast<uint8_t>(emphasis::italic)) em_codes[1] = 3;
+    if (em_bits & static_cast<uint8_t>(emphasis::underline)) em_codes[2] = 4;
+    if (em_bits & static_cast<uint8_t>(emphasis::strikethrough))
+      em_codes[3] = 9;
+
+    size_t index = 0;
+    for (int i = 0; i < 4; ++i) {
+      if (!em_codes[i]) continue;
+      buffer[index++] = static_cast<Char>('\x1b');
+      buffer[index++] = static_cast<Char>('[');
+      buffer[index++] = static_cast<Char>('0' + em_codes[i]);
+      buffer[index++] = static_cast<Char>('m');
+    }
+    buffer[index++] = static_cast<Char>(0);
+  }
+  FMT_CONSTEXPR operator const Char*() const FMT_NOEXCEPT { return buffer; }
+
+  FMT_CONSTEXPR const Char* begin() const FMT_NOEXCEPT { return buffer; }
+  FMT_CONSTEXPR_CHAR_TRAITS const Char* end() const FMT_NOEXCEPT {
+    return buffer + std::char_traits<Char>::length(buffer);
+  }
+
+ private:
+  Char buffer[7u + 3u * 4u + 1u];
+
+  static FMT_CONSTEXPR void to_esc(uint8_t c, Char* out,
+                                   char delimiter) FMT_NOEXCEPT {
+    out[0] = static_cast<Char>('0' + c / 100);
+    out[1] = static_cast<Char>('0' + c / 10 % 10);
+    out[2] = static_cast<Char>('0' + c % 10);
+    out[3] = static_cast<Char>(delimiter);
+  }
+};
+
+template <typename Char>
+FMT_CONSTEXPR ansi_color_escape<Char> make_foreground_color(
+    detail::color_type foreground) FMT_NOEXCEPT {
+  return ansi_color_escape<Char>(foreground, "\x1b[38;2;");
+}
+
+template <typename Char>
+FMT_CONSTEXPR ansi_color_escape<Char> make_background_color(
+    detail::color_type background) FMT_NOEXCEPT {
+  return ansi_color_escape<Char>(background, "\x1b[48;2;");
+}
+
+template <typename Char>
+FMT_CONSTEXPR ansi_color_escape<Char> make_emphasis(emphasis em) FMT_NOEXCEPT {
+  return ansi_color_escape<Char>(em);
+}
+
+template <typename Char>
+inline void fputs(const Char* chars, FILE* stream) FMT_NOEXCEPT {
+  std::fputs(chars, stream);
+}
+
+template <>
+inline void fputs<wchar_t>(const wchar_t* chars, FILE* stream) FMT_NOEXCEPT {
+  std::fputws(chars, stream);
+}
+
+template <typename Char> inline void reset_color(FILE* stream) FMT_NOEXCEPT {
+  fputs("\x1b[0m", stream);
+}
+
+template <> inline void reset_color<wchar_t>(FILE* stream) FMT_NOEXCEPT {
+  fputs(L"\x1b[0m", stream);
+}
+
+template <typename Char>
+inline void reset_color(buffer<Char>& buffer) FMT_NOEXCEPT {
+  auto reset_color = string_view("\x1b[0m");
+  buffer.append(reset_color.begin(), reset_color.end());
+}
+
+template <typename Char>
+void vformat_to(buffer<Char>& buf, const text_style& ts,
+                basic_string_view<Char> format_str,
+                basic_format_args<buffer_context<type_identity_t<Char>>> args) {
+  bool has_style = false;
+  if (ts.has_emphasis()) {
+    has_style = true;
+    auto emphasis = detail::make_emphasis<Char>(ts.get_emphasis());
+    buf.append(emphasis.begin(), emphasis.end());
+  }
+  if (ts.has_foreground()) {
+    has_style = true;
+    auto foreground = detail::make_foreground_color<Char>(ts.get_foreground());
+    buf.append(foreground.begin(), foreground.end());
+  }
+  if (ts.has_background()) {
+    has_style = true;
+    auto background = detail::make_background_color<Char>(ts.get_background());
+    buf.append(background.begin(), background.end());
+  }
+  detail::vformat_to(buf, format_str, args, {});
+  if (has_style) detail::reset_color<Char>(buf);
+}
+
+FMT_END_DETAIL_NAMESPACE
+
+template <typename S, typename Char = char_t<S>>
+void vprint(std::FILE* f, const text_style& ts, const S& format,
+            basic_format_args<buffer_context<type_identity_t<Char>>> args) {
+  basic_memory_buffer<Char> buf;
+  detail::vformat_to(buf, ts, to_string_view(format), args);
+  buf.push_back(Char(0));
+  detail::fputs(buf.data(), f);
+}
+
+/**
+  \rst
+  Formats a string and prints it to the specified file stream using ANSI
+  escape sequences to specify text formatting.
+
+  **Example**::
+
+    fmt::print(fmt::emphasis::bold | fg(fmt::color::red),
+               "Elapsed time: {0:.2f} seconds", 1.23);
+  \endrst
+ */
+template <typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_string<S>::value)>
+void print(std::FILE* f, const text_style& ts, const S& format_str,
+           const Args&... args) {
+  vprint(f, ts, format_str,
+         fmt::make_args_checked<Args...>(format_str, args...));
+}
+
+/**
+  \rst
+  Formats a string and prints it to stdout using ANSI escape sequences to
+  specify text formatting.
+
+  **Example**::
+
+    fmt::print(fmt::emphasis::bold | fg(fmt::color::red),
+               "Elapsed time: {0:.2f} seconds", 1.23);
+  \endrst
+ */
+template <typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_string<S>::value)>
+void print(const text_style& ts, const S& format_str, const Args&... args) {
+  return print(stdout, ts, format_str, args...);
+}
+
+template <typename S, typename Char = char_t<S>>
+inline std::basic_string<Char> vformat(
+    const text_style& ts, const S& format_str,
+    basic_format_args<buffer_context<type_identity_t<Char>>> args) {
+  basic_memory_buffer<Char> buf;
+  detail::vformat_to(buf, ts, to_string_view(format_str), args);
+  return fmt::to_string(buf);
+}
+
+/**
+  \rst
+  Formats arguments and returns the result as a string using ANSI
+  escape sequences to specify text formatting.
+
+  **Example**::
+
+    #include <fmt/color.h>
+    std::string message = fmt::format(fmt::emphasis::bold | fg(fmt::color::red),
+                                      "The answer is {}", 42);
+  \endrst
+*/
+template <typename S, typename... Args, typename Char = char_t<S>>
+inline std::basic_string<Char> format(const text_style& ts, const S& format_str,
+                                      const Args&... args) {
+  return fmt::vformat(ts, to_string_view(format_str),
+                      fmt::make_args_checked<Args...>(format_str, args...));
+}
+
+/**
+  Formats a string with the given text_style and writes the output to ``out``.
+ */
+template <typename OutputIt, typename Char,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, Char>::value)>
+OutputIt vformat_to(
+    OutputIt out, const text_style& ts, basic_string_view<Char> format_str,
+    basic_format_args<buffer_context<type_identity_t<Char>>> args) {
+  auto&& buf = detail::get_buffer<Char>(out);
+  detail::vformat_to(buf, ts, format_str, args);
+  return detail::get_iterator(buf);
+}
+
+/**
+  \rst
+  Formats arguments with the given text_style, writes the result to the output
+  iterator ``out`` and returns the iterator past the end of the output range.
+
+  **Example**::
+
+    std::vector<char> out;
+    fmt::format_to(std::back_inserter(out),
+                   fmt::emphasis::bold | fg(fmt::color::red), "{}", 42);
+  \endrst
+*/
+template <typename OutputIt, typename S, typename... Args,
+          bool enable = detail::is_output_iterator<OutputIt, char_t<S>>::value&&
+              detail::is_string<S>::value>
+inline auto format_to(OutputIt out, const text_style& ts, const S& format_str,
+                      Args&&... args) ->
+    typename std::enable_if<enable, OutputIt>::type {
+  return vformat_to(out, ts, to_string_view(format_str),
+                    fmt::make_args_checked<Args...>(format_str, args...));
+}
+
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_COLOR_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/compile.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/compile.h
new file mode 100644
index 0000000..00000c9
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/compile.h
@@ -0,0 +1,639 @@
+// Formatting library for C++ - experimental format string compilation
+//
+// Copyright (c) 2012 - present, Victor Zverovich and fmt contributors
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_COMPILE_H_
+#define FMT_COMPILE_H_
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+namespace detail {
+
+// An output iterator that counts the number of objects written to it and
+// discards them.
+class counting_iterator {
+ private:
+  size_t count_;
+
+ public:
+  using iterator_category = std::output_iterator_tag;
+  using difference_type = std::ptrdiff_t;
+  using pointer = void;
+  using reference = void;
+  using _Unchecked_type = counting_iterator;  // Mark iterator as checked.
+
+  struct value_type {
+    template <typename T> void operator=(const T&) {}
+  };
+
+  counting_iterator() : count_(0) {}
+
+  size_t count() const { return count_; }
+
+  counting_iterator& operator++() {
+    ++count_;
+    return *this;
+  }
+  counting_iterator operator++(int) {
+    auto it = *this;
+    ++*this;
+    return it;
+  }
+
+  friend counting_iterator operator+(counting_iterator it, difference_type n) {
+    it.count_ += static_cast<size_t>(n);
+    return it;
+  }
+
+  value_type operator*() const { return {}; }
+};
+
+template <typename Char, typename InputIt>
+inline counting_iterator copy_str(InputIt begin, InputIt end,
+                                  counting_iterator it) {
+  return it + (end - begin);
+}
+
+template <typename OutputIt> class truncating_iterator_base {
+ protected:
+  OutputIt out_;
+  size_t limit_;
+  size_t count_ = 0;
+
+  truncating_iterator_base() : out_(), limit_(0) {}
+
+  truncating_iterator_base(OutputIt out, size_t limit)
+      : out_(out), limit_(limit) {}
+
+ public:
+  using iterator_category = std::output_iterator_tag;
+  using value_type = typename std::iterator_traits<OutputIt>::value_type;
+  using difference_type = std::ptrdiff_t;
+  using pointer = void;
+  using reference = void;
+  using _Unchecked_type =
+      truncating_iterator_base;  // Mark iterator as checked.
+
+  OutputIt base() const { return out_; }
+  size_t count() const { return count_; }
+};
+
+// An output iterator that truncates the output and counts the number of objects
+// written to it.
+template <typename OutputIt,
+          typename Enable = typename std::is_void<
+              typename std::iterator_traits<OutputIt>::value_type>::type>
+class truncating_iterator;
+
+template <typename OutputIt>
+class truncating_iterator<OutputIt, std::false_type>
+    : public truncating_iterator_base<OutputIt> {
+  mutable typename truncating_iterator_base<OutputIt>::value_type blackhole_;
+
+ public:
+  using value_type = typename truncating_iterator_base<OutputIt>::value_type;
+
+  truncating_iterator() = default;
+
+  truncating_iterator(OutputIt out, size_t limit)
+      : truncating_iterator_base<OutputIt>(out, limit) {}
+
+  truncating_iterator& operator++() {
+    if (this->count_++ < this->limit_) ++this->out_;
+    return *this;
+  }
+
+  truncating_iterator operator++(int) {
+    auto it = *this;
+    ++*this;
+    return it;
+  }
+
+  value_type& operator*() const {
+    return this->count_ < this->limit_ ? *this->out_ : blackhole_;
+  }
+};
+
+template <typename OutputIt>
+class truncating_iterator<OutputIt, std::true_type>
+    : public truncating_iterator_base<OutputIt> {
+ public:
+  truncating_iterator() = default;
+
+  truncating_iterator(OutputIt out, size_t limit)
+      : truncating_iterator_base<OutputIt>(out, limit) {}
+
+  template <typename T> truncating_iterator& operator=(T val) {
+    if (this->count_++ < this->limit_) *this->out_++ = val;
+    return *this;
+  }
+
+  truncating_iterator& operator++() { return *this; }
+  truncating_iterator& operator++(int) { return *this; }
+  truncating_iterator& operator*() { return *this; }
+};
+
+// A compile-time string which is compiled into fast formatting code.
+class compiled_string {};
+
+template <typename S>
+struct is_compiled_string : std::is_base_of<compiled_string, S> {};
+
+/**
+  \rst
+  Converts a string literal *s* into a format string that will be parsed at
+  compile time and converted into efficient formatting code. Requires C++17
+  ``constexpr if`` compiler support.
+
+  **Example**::
+
+    // Converts 42 into std::string using the most efficient method and no
+    // runtime format string processing.
+    std::string s = fmt::format(FMT_COMPILE("{}"), 42);
+  \endrst
+ */
+#ifdef __cpp_if_constexpr
+#  define FMT_COMPILE(s) \
+    FMT_STRING_IMPL(s, fmt::detail::compiled_string, explicit)
+#else
+#  define FMT_COMPILE(s) FMT_STRING(s)
+#endif
+
+#if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+template <typename Char, size_t N,
+          fmt::detail_exported::fixed_string<Char, N> Str>
+struct udl_compiled_string : compiled_string {
+  using char_type = Char;
+  constexpr operator basic_string_view<char_type>() const {
+    return {Str.data, N - 1};
+  }
+};
+#endif
+
+template <typename T, typename... Tail>
+const T& first(const T& value, const Tail&...) {
+  return value;
+}
+
+#ifdef __cpp_if_constexpr
+template <typename... Args> struct type_list {};
+
+// Returns a reference to the argument at index N from [first, rest...].
+template <int N, typename T, typename... Args>
+constexpr const auto& get([[maybe_unused]] const T& first,
+                          [[maybe_unused]] const Args&... rest) {
+  static_assert(N < 1 + sizeof...(Args), "index is out of bounds");
+  if constexpr (N == 0)
+    return first;
+  else
+    return get<N - 1>(rest...);
+}
+
+template <typename Char, typename... Args>
+constexpr int get_arg_index_by_name(basic_string_view<Char> name,
+                                    type_list<Args...>) {
+  return get_arg_index_by_name<Args...>(name);
+}
+
+template <int N, typename> struct get_type_impl;
+
+template <int N, typename... Args> struct get_type_impl<N, type_list<Args...>> {
+  using type = remove_cvref_t<decltype(get<N>(std::declval<Args>()...))>;
+};
+
+template <int N, typename T>
+using get_type = typename get_type_impl<N, T>::type;
+
+template <typename T> struct is_compiled_format : std::false_type {};
+
+template <typename Char> struct text {
+  basic_string_view<Char> data;
+  using char_type = Char;
+
+  template <typename OutputIt, typename... Args>
+  constexpr OutputIt format(OutputIt out, const Args&...) const {
+    return write<Char>(out, data);
+  }
+};
+
+template <typename Char>
+struct is_compiled_format<text<Char>> : std::true_type {};
+
+template <typename Char>
+constexpr text<Char> make_text(basic_string_view<Char> s, size_t pos,
+                               size_t size) {
+  return {{&s[pos], size}};
+}
+
+template <typename Char> struct code_unit {
+  Char value;
+  using char_type = Char;
+
+  template <typename OutputIt, typename... Args>
+  constexpr OutputIt format(OutputIt out, const Args&...) const {
+    return write<Char>(out, value);
+  }
+};
+
+// This ensures that the argument type is convertible to `const T&`.
+template <typename T, int N, typename... Args>
+constexpr const T& get_arg_checked(const Args&... args) {
+  const auto& arg = get<N>(args...);
+  if constexpr (detail::is_named_arg<remove_cvref_t<decltype(arg)>>()) {
+    return arg.value;
+  } else {
+    return arg;
+  }
+}
+
+template <typename Char>
+struct is_compiled_format<code_unit<Char>> : std::true_type {};
+
+// A replacement field that refers to argument N.
+template <typename Char, typename T, int N> struct field {
+  using char_type = Char;
+
+  template <typename OutputIt, typename... Args>
+  constexpr OutputIt format(OutputIt out, const Args&... args) const {
+    return write<Char>(out, get_arg_checked<T, N>(args...));
+  }
+};
+
+template <typename Char, typename T, int N>
+struct is_compiled_format<field<Char, T, N>> : std::true_type {};
+
+// A replacement field that refers to argument with name.
+template <typename Char> struct runtime_named_field {
+  using char_type = Char;
+  basic_string_view<Char> name;
+
+  template <typename OutputIt, typename T>
+  constexpr static bool try_format_argument(
+      OutputIt& out,
+      // [[maybe_unused]] due to unused-but-set-parameter warning in GCC 7,8,9
+      [[maybe_unused]] basic_string_view<Char> arg_name, const T& arg) {
+    if constexpr (is_named_arg<typename std::remove_cv<T>::type>::value) {
+      if (arg_name == arg.name) {
+        out = write<Char>(out, arg.value);
+        return true;
+      }
+    }
+    return false;
+  }
+
+  template <typename OutputIt, typename... Args>
+  constexpr OutputIt format(OutputIt out, const Args&... args) const {
+    bool found = (try_format_argument(out, name, args) || ...);
+    if (!found) {
+      throw format_error("argument with specified name is not found");
+    }
+    return out;
+  }
+};
+
+template <typename Char>
+struct is_compiled_format<runtime_named_field<Char>> : std::true_type {};
+
+// A replacement field that refers to argument N and has format specifiers.
+template <typename Char, typename T, int N> struct spec_field {
+  using char_type = Char;
+  formatter<T, Char> fmt;
+
+  template <typename OutputIt, typename... Args>
+  constexpr FMT_INLINE OutputIt format(OutputIt out,
+                                       const Args&... args) const {
+    const auto& vargs =
+        fmt::make_format_args<basic_format_context<OutputIt, Char>>(args...);
+    basic_format_context<OutputIt, Char> ctx(out, vargs);
+    return fmt.format(get_arg_checked<T, N>(args...), ctx);
+  }
+};
+
+template <typename Char, typename T, int N>
+struct is_compiled_format<spec_field<Char, T, N>> : std::true_type {};
+
+template <typename L, typename R> struct concat {
+  L lhs;
+  R rhs;
+  using char_type = typename L::char_type;
+
+  template <typename OutputIt, typename... Args>
+  constexpr OutputIt format(OutputIt out, const Args&... args) const {
+    out = lhs.format(out, args...);
+    return rhs.format(out, args...);
+  }
+};
+
+template <typename L, typename R>
+struct is_compiled_format<concat<L, R>> : std::true_type {};
+
+template <typename L, typename R>
+constexpr concat<L, R> make_concat(L lhs, R rhs) {
+  return {lhs, rhs};
+}
+
+struct unknown_format {};
+
+template <typename Char>
+constexpr size_t parse_text(basic_string_view<Char> str, size_t pos) {
+  for (size_t size = str.size(); pos != size; ++pos) {
+    if (str[pos] == '{' || str[pos] == '}') break;
+  }
+  return pos;
+}
+
+template <typename Args, size_t POS, int ID, typename S>
+constexpr auto compile_format_string(S format_str);
+
+template <typename Args, size_t POS, int ID, typename T, typename S>
+constexpr auto parse_tail(T head, S format_str) {
+  if constexpr (POS !=
+                basic_string_view<typename S::char_type>(format_str).size()) {
+    constexpr auto tail = compile_format_string<Args, POS, ID>(format_str);
+    if constexpr (std::is_same<remove_cvref_t<decltype(tail)>,
+                               unknown_format>())
+      return tail;
+    else
+      return make_concat(head, tail);
+  } else {
+    return head;
+  }
+}
+
+template <typename T, typename Char> struct parse_specs_result {
+  formatter<T, Char> fmt;
+  size_t end;
+  int next_arg_id;
+};
+
+constexpr int manual_indexing_id = -1;
+
+template <typename T, typename Char>
+constexpr parse_specs_result<T, Char> parse_specs(basic_string_view<Char> str,
+                                                  size_t pos, int next_arg_id) {
+  str.remove_prefix(pos);
+  auto ctx = basic_format_parse_context<Char>(str, {}, next_arg_id);
+  auto f = formatter<T, Char>();
+  auto end = f.parse(ctx);
+  return {f, pos + fmt::detail::to_unsigned(end - str.data()) + 1,
+          next_arg_id == 0 ? manual_indexing_id : ctx.next_arg_id()};
+}
+
+template <typename Char> struct arg_id_handler {
+  arg_ref<Char> arg_id;
+
+  constexpr int operator()() {
+    FMT_ASSERT(false, "handler cannot be used with automatic indexing");
+    return 0;
+  }
+  constexpr int operator()(int id) {
+    arg_id = arg_ref<Char>(id);
+    return 0;
+  }
+  constexpr int operator()(basic_string_view<Char> id) {
+    arg_id = arg_ref<Char>(id);
+    return 0;
+  }
+
+  constexpr void on_error(const char* message) { throw format_error(message); }
+};
+
+template <typename Char> struct parse_arg_id_result {
+  arg_ref<Char> arg_id;
+  const Char* arg_id_end;
+};
+
+template <int ID, typename Char>
+constexpr auto parse_arg_id(const Char* begin, const Char* end) {
+  auto handler = arg_id_handler<Char>{arg_ref<Char>{}};
+  auto arg_id_end = parse_arg_id(begin, end, handler);
+  return parse_arg_id_result<Char>{handler.arg_id, arg_id_end};
+}
+
+template <typename T, typename Enable = void> struct field_type {
+  using type = remove_cvref_t<T>;
+};
+
+template <typename T>
+struct field_type<T, enable_if_t<detail::is_named_arg<T>::value>> {
+  using type = remove_cvref_t<decltype(T::value)>;
+};
+
+template <typename T, typename Args, size_t END_POS, int ARG_INDEX, int NEXT_ID,
+          typename S>
+constexpr auto parse_replacement_field_then_tail(S format_str) {
+  using char_type = typename S::char_type;
+  constexpr auto str = basic_string_view<char_type>(format_str);
+  constexpr char_type c = END_POS != str.size() ? str[END_POS] : char_type();
+  if constexpr (c == '}') {
+    return parse_tail<Args, END_POS + 1, NEXT_ID>(
+        field<char_type, typename field_type<T>::type, ARG_INDEX>(),
+        format_str);
+  } else if constexpr (c == ':') {
+    constexpr auto result = parse_specs<typename field_type<T>::type>(
+        str, END_POS + 1, NEXT_ID == manual_indexing_id ? 0 : NEXT_ID);
+    return parse_tail<Args, result.end, result.next_arg_id>(
+        spec_field<char_type, typename field_type<T>::type, ARG_INDEX>{
+            result.fmt},
+        format_str);
+  }
+}
+
+// Compiles a non-empty format string and returns the compiled representation
+// or unknown_format() on unrecognized input.
+template <typename Args, size_t POS, int ID, typename S>
+constexpr auto compile_format_string(S format_str) {
+  using char_type = typename S::char_type;
+  constexpr auto str = basic_string_view<char_type>(format_str);
+  if constexpr (str[POS] == '{') {
+    if constexpr (POS + 1 == str.size())
+      throw format_error("unmatched '{' in format string");
+    if constexpr (str[POS + 1] == '{') {
+      return parse_tail<Args, POS + 2, ID>(make_text(str, POS, 1), format_str);
+    } else if constexpr (str[POS + 1] == '}' || str[POS + 1] == ':') {
+      static_assert(ID != manual_indexing_id,
+                    "cannot switch from manual to automatic argument indexing");
+      constexpr auto next_id =
+          ID != manual_indexing_id ? ID + 1 : manual_indexing_id;
+      return parse_replacement_field_then_tail<get_type<ID, Args>, Args,
+                                               POS + 1, ID, next_id>(
+          format_str);
+    } else {
+      constexpr auto arg_id_result =
+          parse_arg_id<ID>(str.data() + POS + 1, str.data() + str.size());
+      constexpr auto arg_id_end_pos = arg_id_result.arg_id_end - str.data();
+      constexpr char_type c =
+          arg_id_end_pos != str.size() ? str[arg_id_end_pos] : char_type();
+      static_assert(c == '}' || c == ':', "missing '}' in format string");
+      if constexpr (arg_id_result.arg_id.kind == arg_id_kind::index) {
+        static_assert(
+            ID == manual_indexing_id || ID == 0,
+            "cannot switch from automatic to manual argument indexing");
+        constexpr auto arg_index = arg_id_result.arg_id.val.index;
+        return parse_replacement_field_then_tail<get_type<arg_index, Args>,
+                                                 Args, arg_id_end_pos,
+                                                 arg_index, manual_indexing_id>(
+            format_str);
+      } else if constexpr (arg_id_result.arg_id.kind == arg_id_kind::name) {
+        constexpr auto arg_index =
+            get_arg_index_by_name(arg_id_result.arg_id.val.name, Args{});
+        if constexpr (arg_index != invalid_arg_index) {
+          constexpr auto next_id =
+              ID != manual_indexing_id ? ID + 1 : manual_indexing_id;
+          return parse_replacement_field_then_tail<
+              decltype(get_type<arg_index, Args>::value), Args, arg_id_end_pos,
+              arg_index, next_id>(format_str);
+        } else {
+          if constexpr (c == '}') {
+            return parse_tail<Args, arg_id_end_pos + 1, ID>(
+                runtime_named_field<char_type>{arg_id_result.arg_id.val.name},
+                format_str);
+          } else if constexpr (c == ':') {
+            return unknown_format();  // no type info for specs parsing
+          }
+        }
+      }
+    }
+  } else if constexpr (str[POS] == '}') {
+    if constexpr (POS + 1 == str.size())
+      throw format_error("unmatched '}' in format string");
+    return parse_tail<Args, POS + 2, ID>(make_text(str, POS, 1), format_str);
+  } else {
+    constexpr auto end = parse_text(str, POS + 1);
+    if constexpr (end - POS > 1) {
+      return parse_tail<Args, end, ID>(make_text(str, POS, end - POS),
+                                       format_str);
+    } else {
+      return parse_tail<Args, end, ID>(code_unit<char_type>{str[POS]},
+                                       format_str);
+    }
+  }
+}
+
+template <typename... Args, typename S,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+constexpr auto compile(S format_str) {
+  constexpr auto str = basic_string_view<typename S::char_type>(format_str);
+  if constexpr (str.size() == 0) {
+    return detail::make_text(str, 0, 0);
+  } else {
+    constexpr auto result =
+        detail::compile_format_string<detail::type_list<Args...>, 0, 0>(
+            format_str);
+    return result;
+  }
+}
+#endif  // __cpp_if_constexpr
+}  // namespace detail
+
+FMT_MODULE_EXPORT_BEGIN
+
+#ifdef __cpp_if_constexpr
+
+template <typename CompiledFormat, typename... Args,
+          typename Char = typename CompiledFormat::char_type,
+          FMT_ENABLE_IF(detail::is_compiled_format<CompiledFormat>::value)>
+FMT_INLINE std::basic_string<Char> format(const CompiledFormat& cf,
+                                          const Args&... args) {
+  auto s = std::basic_string<Char>();
+  cf.format(std::back_inserter(s), args...);
+  return s;
+}
+
+template <typename OutputIt, typename CompiledFormat, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_format<CompiledFormat>::value)>
+constexpr FMT_INLINE OutputIt format_to(OutputIt out, const CompiledFormat& cf,
+                                        const Args&... args) {
+  return cf.format(out, args...);
+}
+
+template <typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+FMT_INLINE std::basic_string<typename S::char_type> format(const S&,
+                                                           Args&&... args) {
+  if constexpr (std::is_same<typename S::char_type, char>::value) {
+    constexpr auto str = basic_string_view<typename S::char_type>(S());
+    if constexpr (str.size() == 2 && str[0] == '{' && str[1] == '}') {
+      const auto& first = detail::first(args...);
+      if constexpr (detail::is_named_arg<
+                        remove_cvref_t<decltype(first)>>::value) {
+        return fmt::to_string(first.value);
+      } else {
+        return fmt::to_string(first);
+      }
+    }
+  }
+  constexpr auto compiled = detail::compile<Args...>(S());
+  if constexpr (std::is_same<remove_cvref_t<decltype(compiled)>,
+                             detail::unknown_format>()) {
+    return format(static_cast<basic_string_view<typename S::char_type>>(S()),
+                  std::forward<Args>(args)...);
+  } else {
+    return format(compiled, std::forward<Args>(args)...);
+  }
+}
+
+template <typename OutputIt, typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+FMT_CONSTEXPR OutputIt format_to(OutputIt out, const S&, Args&&... args) {
+  constexpr auto compiled = detail::compile<Args...>(S());
+  if constexpr (std::is_same<remove_cvref_t<decltype(compiled)>,
+                             detail::unknown_format>()) {
+    return format_to(out,
+                     static_cast<basic_string_view<typename S::char_type>>(S()),
+                     std::forward<Args>(args)...);
+  } else {
+    return format_to(out, compiled, std::forward<Args>(args)...);
+  }
+}
+#endif
+
+template <typename OutputIt, typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+format_to_n_result<OutputIt> format_to_n(OutputIt out, size_t n,
+                                         const S& format_str, Args&&... args) {
+  auto it = format_to(detail::truncating_iterator<OutputIt>(out, n), format_str,
+                      std::forward<Args>(args)...);
+  return {it.base(), it.count()};
+}
+
+template <typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+size_t formatted_size(const S& format_str, const Args&... args) {
+  return format_to(detail::counting_iterator(), format_str, args...).count();
+}
+
+template <typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+void print(std::FILE* f, const S& format_str, const Args&... args) {
+  memory_buffer buffer;
+  format_to(std::back_inserter(buffer), format_str, args...);
+  detail::print(f, {buffer.data(), buffer.size()});
+}
+
+template <typename S, typename... Args,
+          FMT_ENABLE_IF(detail::is_compiled_string<S>::value)>
+void print(const S& format_str, const Args&... args) {
+  print(stdout, format_str, args...);
+}
+
+#if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+inline namespace literals {
+template <detail_exported::fixed_string Str>
+constexpr detail::udl_compiled_string<
+    remove_cvref_t<decltype(Str.data[0])>,
+    sizeof(Str.data) / sizeof(decltype(Str.data[0])), Str>
+operator""_cf() {
+  return {};
+}
+}  // namespace literals
+#endif
+
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_COMPILE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/core.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/core.h
new file mode 100644
index 0000000..d058398
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/core.h
@@ -0,0 +1,3002 @@
+// Formatting library for C++ - the core API for char/UTF-8
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_CORE_H_
+#define FMT_CORE_H_
+
+#include <cstdio>  // std::FILE
+#include <cstring>
+#include <iterator>
+#include <limits>
+#include <string>
+#include <type_traits>
+
+// The fmt library version in the form major * 10000 + minor * 100 + patch.
+#define FMT_VERSION 80001
+
+#ifdef __clang__
+#  define FMT_CLANG_VERSION (__clang_major__ * 100 + __clang_minor__)
+#else
+#  define FMT_CLANG_VERSION 0
+#endif
+
+#if defined(__GNUC__) && !defined(__clang__) && !defined(__INTEL_COMPILER)
+#  define FMT_GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__)
+#  define FMT_GCC_PRAGMA(arg) _Pragma(arg)
+#else
+#  define FMT_GCC_VERSION 0
+#  define FMT_GCC_PRAGMA(arg)
+#endif
+
+#if __cplusplus >= 201103L || defined(__GXX_EXPERIMENTAL_CXX0X__)
+#  define FMT_HAS_GXX_CXX11 FMT_GCC_VERSION
+#else
+#  define FMT_HAS_GXX_CXX11 0
+#endif
+
+#if defined(__INTEL_COMPILER)
+#  define FMT_ICC_VERSION __INTEL_COMPILER
+#else
+#  define FMT_ICC_VERSION 0
+#endif
+
+#ifdef __NVCC__
+#  define FMT_NVCC __NVCC__
+#else
+#  define FMT_NVCC 0
+#endif
+
+#ifdef _MSC_VER
+#  define FMT_MSC_VER _MSC_VER
+#  define FMT_MSC_WARNING(...) __pragma(warning(__VA_ARGS__))
+#else
+#  define FMT_MSC_VER 0
+#  define FMT_MSC_WARNING(...)
+#endif
+
+#ifdef __has_feature
+#  define FMT_HAS_FEATURE(x) __has_feature(x)
+#else
+#  define FMT_HAS_FEATURE(x) 0
+#endif
+
+#if defined(__has_include) &&                             \
+    (!defined(__INTELLISENSE__) || FMT_MSC_VER > 1900) && \
+    (!FMT_ICC_VERSION || FMT_ICC_VERSION >= 1600)
+#  define FMT_HAS_INCLUDE(x) __has_include(x)
+#else
+#  define FMT_HAS_INCLUDE(x) 0
+#endif
+
+#ifdef __has_cpp_attribute
+#  define FMT_HAS_CPP_ATTRIBUTE(x) __has_cpp_attribute(x)
+#else
+#  define FMT_HAS_CPP_ATTRIBUTE(x) 0
+#endif
+
+#define FMT_HAS_CPP14_ATTRIBUTE(attribute) \
+  (__cplusplus >= 201402L && FMT_HAS_CPP_ATTRIBUTE(attribute))
+
+#define FMT_HAS_CPP17_ATTRIBUTE(attribute) \
+  (__cplusplus >= 201703L && FMT_HAS_CPP_ATTRIBUTE(attribute))
+
+// Check if relaxed C++14 constexpr is supported.
+// GCC doesn't allow throw in constexpr until version 6 (bug 67371).
+#ifndef FMT_USE_CONSTEXPR
+#  define FMT_USE_CONSTEXPR                                           \
+    (FMT_HAS_FEATURE(cxx_relaxed_constexpr) || FMT_MSC_VER >= 1910 || \
+     (FMT_GCC_VERSION >= 600 && __cplusplus >= 201402L)) &&           \
+        !FMT_NVCC && !FMT_ICC_VERSION
+#endif
+#if FMT_USE_CONSTEXPR
+#  define FMT_CONSTEXPR constexpr
+#  define FMT_CONSTEXPR_DECL constexpr
+#else
+#  define FMT_CONSTEXPR
+#  define FMT_CONSTEXPR_DECL
+#endif
+
+// Check if constexpr std::char_traits<>::compare,length is supported.
+#if defined(__GLIBCXX__)
+#  if __cplusplus >= 201703L && defined(_GLIBCXX_RELEASE) && \
+      _GLIBCXX_RELEASE >= 7  // GCC 7+ libstdc++ has _GLIBCXX_RELEASE.
+#    define FMT_CONSTEXPR_CHAR_TRAITS constexpr
+#  endif
+#elif defined(_LIBCPP_VERSION) && __cplusplus >= 201703L && \
+    _LIBCPP_VERSION >= 4000
+#  define FMT_CONSTEXPR_CHAR_TRAITS constexpr
+#elif FMT_MSC_VER >= 1914 && _MSVC_LANG >= 201703L
+#  define FMT_CONSTEXPR_CHAR_TRAITS constexpr
+#endif
+#ifndef FMT_CONSTEXPR_CHAR_TRAITS
+#  define FMT_CONSTEXPR_CHAR_TRAITS
+#endif
+
+#ifndef FMT_OVERRIDE
+#  if FMT_HAS_FEATURE(cxx_override_control) || \
+      (FMT_GCC_VERSION >= 408 && FMT_HAS_GXX_CXX11) || FMT_MSC_VER >= 1900
+#    define FMT_OVERRIDE override
+#  else
+#    define FMT_OVERRIDE
+#  endif
+#endif
+
+// Check if exceptions are disabled.
+#ifndef FMT_EXCEPTIONS
+#  if (defined(__GNUC__) && !defined(__EXCEPTIONS)) || \
+      FMT_MSC_VER && !_HAS_EXCEPTIONS
+#    define FMT_EXCEPTIONS 0
+#  else
+#    define FMT_EXCEPTIONS 1
+#  endif
+#endif
+
+// Define FMT_USE_NOEXCEPT to make fmt use noexcept (C++11 feature).
+#ifndef FMT_USE_NOEXCEPT
+#  define FMT_USE_NOEXCEPT 0
+#endif
+
+#if FMT_USE_NOEXCEPT || FMT_HAS_FEATURE(cxx_noexcept) || \
+    (FMT_GCC_VERSION >= 408 && FMT_HAS_GXX_CXX11) || FMT_MSC_VER >= 1900
+#  define FMT_DETECTED_NOEXCEPT noexcept
+#  define FMT_HAS_CXX11_NOEXCEPT 1
+#else
+#  define FMT_DETECTED_NOEXCEPT throw()
+#  define FMT_HAS_CXX11_NOEXCEPT 0
+#endif
+
+#ifndef FMT_NOEXCEPT
+#  if FMT_EXCEPTIONS || FMT_HAS_CXX11_NOEXCEPT
+#    define FMT_NOEXCEPT FMT_DETECTED_NOEXCEPT
+#  else
+#    define FMT_NOEXCEPT
+#  endif
+#endif
+
+// [[noreturn]] is disabled on MSVC and NVCC because of bogus unreachable code
+// warnings.
+#if FMT_EXCEPTIONS && FMT_HAS_CPP_ATTRIBUTE(noreturn) && !FMT_MSC_VER && \
+    !FMT_NVCC
+#  define FMT_NORETURN [[noreturn]]
+#else
+#  define FMT_NORETURN
+#endif
+
+#ifndef FMT_MAYBE_UNUSED
+#  if FMT_HAS_CPP17_ATTRIBUTE(maybe_unused)
+#    define FMT_MAYBE_UNUSED [[maybe_unused]]
+#  else
+#    define FMT_MAYBE_UNUSED
+#  endif
+#endif
+
+#if __cplusplus == 201103L || __cplusplus == 201402L
+#  if defined(__INTEL_COMPILER) || defined(__PGI)
+#    define FMT_FALLTHROUGH
+#  elif defined(__clang__)
+#    define FMT_FALLTHROUGH [[clang::fallthrough]]
+#  elif FMT_GCC_VERSION >= 700 && \
+      (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= 520)
+#    define FMT_FALLTHROUGH [[gnu::fallthrough]]
+#  else
+#    define FMT_FALLTHROUGH
+#  endif
+#elif FMT_HAS_CPP17_ATTRIBUTE(fallthrough) || \
+    (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+#  define FMT_FALLTHROUGH [[fallthrough]]
+#else
+#  define FMT_FALLTHROUGH
+#endif
+
+#ifndef FMT_USE_FLOAT
+#  define FMT_USE_FLOAT 1
+#endif
+#ifndef FMT_USE_DOUBLE
+#  define FMT_USE_DOUBLE 1
+#endif
+#ifndef FMT_USE_LONG_DOUBLE
+#  define FMT_USE_LONG_DOUBLE 1
+#endif
+
+#ifndef FMT_INLINE
+#  if FMT_GCC_VERSION || FMT_CLANG_VERSION
+#    define FMT_INLINE inline __attribute__((always_inline))
+#  else
+#    define FMT_INLINE inline
+#  endif
+#endif
+
+#ifndef FMT_USE_INLINE_NAMESPACES
+#  if FMT_HAS_FEATURE(cxx_inline_namespaces) || FMT_GCC_VERSION >= 404 || \
+      (FMT_MSC_VER >= 1900 && (!defined(_MANAGED) || !_MANAGED))
+#    define FMT_USE_INLINE_NAMESPACES 1
+#  else
+#    define FMT_USE_INLINE_NAMESPACES 0
+#  endif
+#endif
+
+#ifndef FMT_BEGIN_NAMESPACE
+#  if FMT_USE_INLINE_NAMESPACES
+#    define FMT_INLINE_NAMESPACE inline namespace
+#    define FMT_END_NAMESPACE \
+      }                       \
+      }
+#  else
+#    define FMT_INLINE_NAMESPACE namespace
+#    define FMT_END_NAMESPACE \
+      }                       \
+      using namespace v8;     \
+      }
+#  endif
+#  define FMT_BEGIN_NAMESPACE \
+    namespace fmt {           \
+    FMT_INLINE_NAMESPACE v8 {
+#endif
+
+#ifndef FMT_MODULE_EXPORT
+#  define FMT_MODULE_EXPORT
+#  define FMT_MODULE_EXPORT_BEGIN
+#  define FMT_MODULE_EXPORT_END
+#  define FMT_BEGIN_DETAIL_NAMESPACE namespace detail {
+#  define FMT_END_DETAIL_NAMESPACE }
+#endif
+
+#if !defined(FMT_HEADER_ONLY) && defined(_WIN32)
+#  define FMT_CLASS_API FMT_MSC_WARNING(suppress : 4275)
+#  ifdef FMT_EXPORT
+#    define FMT_API __declspec(dllexport)
+#  elif defined(FMT_SHARED)
+#    define FMT_API __declspec(dllimport)
+#  endif
+#else
+#  define FMT_CLASS_API
+#  if defined(FMT_EXPORT) || defined(FMT_SHARED)
+#    if defined(__GNUC__) || defined(__clang__)
+#      define FMT_API __attribute__((visibility("default")))
+#    endif
+#  endif
+#endif
+#ifndef FMT_API
+#  define FMT_API
+#endif
+
+#if FMT_GCC_VERSION
+#  define FMT_GCC_VISIBILITY_HIDDEN __attribute__((visibility("hidden")))
+#else
+#  define FMT_GCC_VISIBILITY_HIDDEN
+#endif
+
+// libc++ supports string_view in pre-c++17.
+#if (FMT_HAS_INCLUDE(<string_view>) &&                       \
+     (__cplusplus > 201402L || defined(_LIBCPP_VERSION))) || \
+    (defined(_MSVC_LANG) && _MSVC_LANG > 201402L && _MSC_VER >= 1910)
+#  include <string_view>
+#  define FMT_USE_STRING_VIEW
+#elif FMT_HAS_INCLUDE("experimental/string_view") && __cplusplus >= 201402L
+#  include <experimental/string_view>
+#  define FMT_USE_EXPERIMENTAL_STRING_VIEW
+#endif
+
+#ifndef FMT_UNICODE
+#  define FMT_UNICODE !FMT_MSC_VER
+#endif
+
+#ifndef FMT_CONSTEVAL
+#  if ((FMT_GCC_VERSION >= 1000 || FMT_CLANG_VERSION >= 1101) && \
+       __cplusplus > 201703L) ||                                 \
+      (defined(__cpp_consteval) &&                               \
+       !FMT_MSC_VER)  // consteval is broken in MSVC.
+#    define FMT_CONSTEVAL consteval
+#    define FMT_HAS_CONSTEVAL
+#  else
+#    define FMT_CONSTEVAL
+#  endif
+#endif
+
+#ifndef FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+#  if defined(__cpp_nontype_template_args) &&                \
+      ((FMT_GCC_VERSION >= 903 && __cplusplus >= 201709L) || \
+       __cpp_nontype_template_args >= 201911L)
+#    define FMT_USE_NONTYPE_TEMPLATE_PARAMETERS 1
+#  else
+#    define FMT_USE_NONTYPE_TEMPLATE_PARAMETERS 0
+#  endif
+#endif
+
+// Enable minimal optimizations for more compact code in debug mode.
+FMT_GCC_PRAGMA("GCC push_options")
+#ifndef __OPTIMIZE__
+FMT_GCC_PRAGMA("GCC optimize(\"Og\")")
+#endif
+
+FMT_BEGIN_NAMESPACE
+FMT_MODULE_EXPORT_BEGIN
+
+// Implementations of enable_if_t and other metafunctions for older systems.
+template <bool B, typename T = void>
+using enable_if_t = typename std::enable_if<B, T>::type;
+template <bool B, typename T, typename F>
+using conditional_t = typename std::conditional<B, T, F>::type;
+template <bool B> using bool_constant = std::integral_constant<bool, B>;
+template <typename T>
+using remove_reference_t = typename std::remove_reference<T>::type;
+template <typename T>
+using remove_cvref_t = typename std::remove_cv<remove_reference_t<T>>::type;
+template <typename T> struct type_identity { using type = T; };
+template <typename T> using type_identity_t = typename type_identity<T>::type;
+
+struct monostate {
+  constexpr monostate() {}
+};
+
+// Suppress "unused variable" warnings with the method described in
+// https://herbsutter.com/2009/10/18/mailbag-shutting-up-compiler-warnings/.
+// (void)var does not work on many Intel compilers.
+template <typename... T> FMT_CONSTEXPR void ignore_unused(const T&...) {}
+
+// An enable_if helper to be used in template parameters which results in much
+// shorter symbols: https://godbolt.org/z/sWw4vP. Extra parentheses are needed
+// to workaround a bug in MSVC 2019 (see #1140 and #1186).
+#ifdef FMT_DOC
+#  define FMT_ENABLE_IF(...)
+#else
+#  define FMT_ENABLE_IF(...) enable_if_t<(__VA_ARGS__), int> = 0
+#endif
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+constexpr FMT_INLINE auto is_constant_evaluated() FMT_NOEXCEPT -> bool {
+#ifdef __cpp_lib_is_constant_evaluated
+  return std::is_constant_evaluated();
+#else
+  return false;
+#endif
+}
+
+// A function to suppress "conditional expression is constant" warnings.
+template <typename T> constexpr auto const_check(T value) -> T { return value; }
+
+FMT_NORETURN FMT_API void assert_fail(const char* file, int line,
+                                      const char* message);
+
+#ifndef FMT_ASSERT
+#  ifdef NDEBUG
+// FMT_ASSERT is not empty to avoid -Werror=empty-body.
+#    define FMT_ASSERT(condition, message) \
+      ::fmt::ignore_unused((condition), (message))
+#  else
+#    define FMT_ASSERT(condition, message)                                    \
+      ((condition) /* void() fails with -Winvalid-constexpr on clang 4.0.1 */ \
+           ? (void)0                                                          \
+           : ::fmt::detail::assert_fail(__FILE__, __LINE__, (message)))
+#  endif
+#endif
+
+#if defined(FMT_USE_STRING_VIEW)
+template <typename Char> using std_string_view = std::basic_string_view<Char>;
+#elif defined(FMT_USE_EXPERIMENTAL_STRING_VIEW)
+template <typename Char>
+using std_string_view = std::experimental::basic_string_view<Char>;
+#else
+template <typename T> struct std_string_view {};
+#endif
+
+#ifdef FMT_USE_INT128
+// Do nothing.
+#elif defined(__SIZEOF_INT128__) && !FMT_NVCC && \
+    !(FMT_CLANG_VERSION && FMT_MSC_VER)
+#  define FMT_USE_INT128 1
+using int128_t = __int128_t;
+using uint128_t = __uint128_t;
+template <typename T> inline auto convert_for_visit(T value) -> T {
+  return value;
+}
+#else
+#  define FMT_USE_INT128 0
+#endif
+#if !FMT_USE_INT128
+enum class int128_t {};
+enum class uint128_t {};
+// Reduce template instantiations.
+template <typename T> inline auto convert_for_visit(T) -> monostate {
+  return {};
+}
+#endif
+
+// Casts a nonnegative integer to unsigned.
+template <typename Int>
+FMT_CONSTEXPR auto to_unsigned(Int value) ->
+    typename std::make_unsigned<Int>::type {
+  FMT_ASSERT(value >= 0, "negative value");
+  return static_cast<typename std::make_unsigned<Int>::type>(value);
+}
+
+FMT_MSC_WARNING(suppress : 4566) constexpr unsigned char micro[] = "\u00B5";
+
+constexpr auto is_utf8() -> bool {
+  // Avoid buggy sign extensions in MSVC's constant evaluation mode.
+  // https://developercommunity.visualstudio.com/t/C-difference-in-behavior-for-unsigned/1233612
+  using uchar = unsigned char;
+  return FMT_UNICODE || (sizeof(micro) == 3 && uchar(micro[0]) == 0xC2 &&
+                         uchar(micro[1]) == 0xB5);
+}
+FMT_END_DETAIL_NAMESPACE
+
+/**
+  An implementation of ``std::basic_string_view`` for pre-C++17. It provides a
+  subset of the API. ``fmt::basic_string_view`` is used for format strings even
+  if ``std::string_view`` is available to prevent issues when a library is
+  compiled with a different ``-std`` option than the client code (which is not
+  recommended).
+ */
+template <typename Char> class basic_string_view {
+ private:
+  const Char* data_;
+  size_t size_;
+
+ public:
+  using value_type = Char;
+  using iterator = const Char*;
+
+  constexpr basic_string_view() FMT_NOEXCEPT : data_(nullptr), size_(0) {}
+
+  /** Constructs a string reference object from a C string and a size. */
+  constexpr basic_string_view(const Char* s, size_t count) FMT_NOEXCEPT
+      : data_(s),
+        size_(count) {}
+
+  /**
+    \rst
+    Constructs a string reference object from a C string computing
+    the size with ``std::char_traits<Char>::length``.
+    \endrst
+   */
+  FMT_CONSTEXPR_CHAR_TRAITS
+  FMT_INLINE
+  basic_string_view(const Char* s) : data_(s) {
+    if (detail::const_check(std::is_same<Char, char>::value &&
+                            !detail::is_constant_evaluated()))
+      size_ = std::strlen(reinterpret_cast<const char*>(s));
+    else
+      size_ = std::char_traits<Char>::length(s);
+  }
+
+  /** Constructs a string reference from a ``std::basic_string`` object. */
+  template <typename Traits, typename Alloc>
+  FMT_CONSTEXPR basic_string_view(
+      const std::basic_string<Char, Traits, Alloc>& s) FMT_NOEXCEPT
+      : data_(s.data()),
+        size_(s.size()) {}
+
+  template <typename S, FMT_ENABLE_IF(std::is_same<
+                                      S, detail::std_string_view<Char>>::value)>
+  FMT_CONSTEXPR basic_string_view(S s) FMT_NOEXCEPT : data_(s.data()),
+                                                      size_(s.size()) {}
+
+  /** Returns a pointer to the string data. */
+  constexpr auto data() const -> const Char* { return data_; }
+
+  /** Returns the string size. */
+  constexpr auto size() const -> size_t { return size_; }
+
+  constexpr auto begin() const -> iterator { return data_; }
+  constexpr auto end() const -> iterator { return data_ + size_; }
+
+  constexpr auto operator[](size_t pos) const -> const Char& {
+    return data_[pos];
+  }
+
+  FMT_CONSTEXPR void remove_prefix(size_t n) {
+    data_ += n;
+    size_ -= n;
+  }
+
+  // Lexicographically compare this string reference to other.
+  FMT_CONSTEXPR_CHAR_TRAITS auto compare(basic_string_view other) const -> int {
+    size_t str_size = size_ < other.size_ ? size_ : other.size_;
+    int result = std::char_traits<Char>::compare(data_, other.data_, str_size);
+    if (result == 0)
+      result = size_ == other.size_ ? 0 : (size_ < other.size_ ? -1 : 1);
+    return result;
+  }
+
+  FMT_CONSTEXPR_CHAR_TRAITS friend auto operator==(basic_string_view lhs,
+                                                   basic_string_view rhs)
+      -> bool {
+    return lhs.compare(rhs) == 0;
+  }
+  friend auto operator!=(basic_string_view lhs, basic_string_view rhs) -> bool {
+    return lhs.compare(rhs) != 0;
+  }
+  friend auto operator<(basic_string_view lhs, basic_string_view rhs) -> bool {
+    return lhs.compare(rhs) < 0;
+  }
+  friend auto operator<=(basic_string_view lhs, basic_string_view rhs) -> bool {
+    return lhs.compare(rhs) <= 0;
+  }
+  friend auto operator>(basic_string_view lhs, basic_string_view rhs) -> bool {
+    return lhs.compare(rhs) > 0;
+  }
+  friend auto operator>=(basic_string_view lhs, basic_string_view rhs) -> bool {
+    return lhs.compare(rhs) >= 0;
+  }
+};
+
+using string_view = basic_string_view<char>;
+
+/** Specifies if ``T`` is a character type. Can be specialized by users. */
+template <typename T> struct is_char : std::false_type {};
+template <> struct is_char<char> : std::true_type {};
+
+// Returns a string view of `s`.
+template <typename Char, FMT_ENABLE_IF(is_char<Char>::value)>
+FMT_INLINE auto to_string_view(const Char* s) -> basic_string_view<Char> {
+  return s;
+}
+template <typename Char, typename Traits, typename Alloc>
+inline auto to_string_view(const std::basic_string<Char, Traits, Alloc>& s)
+    -> basic_string_view<Char> {
+  return s;
+}
+template <typename Char>
+constexpr auto to_string_view(basic_string_view<Char> s)
+    -> basic_string_view<Char> {
+  return s;
+}
+template <typename Char,
+          FMT_ENABLE_IF(!std::is_empty<detail::std_string_view<Char>>::value)>
+inline auto to_string_view(detail::std_string_view<Char> s)
+    -> basic_string_view<Char> {
+  return s;
+}
+
+// A base class for compile-time strings. It is defined in the fmt namespace to
+// make formatting functions visible via ADL, e.g. format(FMT_STRING("{}"), 42).
+struct compile_string {};
+
+template <typename S>
+struct is_compile_string : std::is_base_of<compile_string, S> {};
+
+template <typename S, FMT_ENABLE_IF(is_compile_string<S>::value)>
+constexpr auto to_string_view(const S& s)
+    -> basic_string_view<typename S::char_type> {
+  return basic_string_view<typename S::char_type>(s);
+}
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+void to_string_view(...);
+using fmt::v8::to_string_view;
+
+// Specifies whether S is a string type convertible to fmt::basic_string_view.
+// It should be a constexpr function but MSVC 2017 fails to compile it in
+// enable_if and MSVC 2015 fails to compile it as an alias template.
+template <typename S>
+struct is_string : std::is_class<decltype(to_string_view(std::declval<S>()))> {
+};
+
+template <typename S, typename = void> struct char_t_impl {};
+template <typename S> struct char_t_impl<S, enable_if_t<is_string<S>::value>> {
+  using result = decltype(to_string_view(std::declval<S>()));
+  using type = typename result::value_type;
+};
+
+// Reports a compile-time error if S is not a valid format string.
+template <typename..., typename S, FMT_ENABLE_IF(!is_compile_string<S>::value)>
+FMT_INLINE void check_format_string(const S&) {
+#ifdef FMT_ENFORCE_COMPILE_STRING
+  static_assert(is_compile_string<S>::value,
+                "FMT_ENFORCE_COMPILE_STRING requires all format strings to use "
+                "FMT_STRING.");
+#endif
+}
+template <typename..., typename S, FMT_ENABLE_IF(is_compile_string<S>::value)>
+void check_format_string(S);
+
+struct error_handler {
+  constexpr error_handler() = default;
+  constexpr error_handler(const error_handler&) = default;
+
+  // This function is intentionally not constexpr to give a compile-time error.
+  FMT_NORETURN FMT_API void on_error(const char* message);
+};
+FMT_END_DETAIL_NAMESPACE
+
+/** String's character type. */
+template <typename S> using char_t = typename detail::char_t_impl<S>::type;
+
+/**
+  \rst
+  Parsing context consisting of a format string range being parsed and an
+  argument counter for automatic indexing.
+  You can use the ``format_parse_context`` type alias for ``char`` instead.
+  \endrst
+ */
+template <typename Char, typename ErrorHandler = detail::error_handler>
+class basic_format_parse_context : private ErrorHandler {
+ private:
+  basic_string_view<Char> format_str_;
+  int next_arg_id_;
+
+ public:
+  using char_type = Char;
+  using iterator = typename basic_string_view<Char>::iterator;
+
+  explicit constexpr basic_format_parse_context(
+      basic_string_view<Char> format_str, ErrorHandler eh = {},
+      int next_arg_id = 0)
+      : ErrorHandler(eh), format_str_(format_str), next_arg_id_(next_arg_id) {}
+
+  /**
+    Returns an iterator to the beginning of the format string range being
+    parsed.
+   */
+  constexpr auto begin() const FMT_NOEXCEPT -> iterator {
+    return format_str_.begin();
+  }
+
+  /**
+    Returns an iterator past the end of the format string range being parsed.
+   */
+  constexpr auto end() const FMT_NOEXCEPT -> iterator {
+    return format_str_.end();
+  }
+
+  /** Advances the begin iterator to ``it``. */
+  FMT_CONSTEXPR void advance_to(iterator it) {
+    format_str_.remove_prefix(detail::to_unsigned(it - begin()));
+  }
+
+  /**
+    Reports an error if using the manual argument indexing; otherwise returns
+    the next argument index and switches to the automatic indexing.
+   */
+  FMT_CONSTEXPR auto next_arg_id() -> int {
+    // Don't check if the argument id is valid to avoid overhead and because it
+    // will be checked during formatting anyway.
+    if (next_arg_id_ >= 0) return next_arg_id_++;
+    on_error("cannot switch from manual to automatic argument indexing");
+    return 0;
+  }
+
+  /**
+    Reports an error if using the automatic argument indexing; otherwise
+    switches to the manual indexing.
+   */
+  FMT_CONSTEXPR void check_arg_id(int) {
+    if (next_arg_id_ > 0)
+      on_error("cannot switch from automatic to manual argument indexing");
+    else
+      next_arg_id_ = -1;
+  }
+
+  FMT_CONSTEXPR void check_arg_id(basic_string_view<Char>) {}
+
+  FMT_CONSTEXPR void on_error(const char* message) {
+    ErrorHandler::on_error(message);
+  }
+
+  constexpr auto error_handler() const -> ErrorHandler { return *this; }
+};
+
+using format_parse_context = basic_format_parse_context<char>;
+
+template <typename Context> class basic_format_arg;
+template <typename Context> class basic_format_args;
+template <typename Context> class dynamic_format_arg_store;
+
+// A formatter for objects of type T.
+template <typename T, typename Char = char, typename Enable = void>
+struct formatter {
+  // A deleted default constructor indicates a disabled formatter.
+  formatter() = delete;
+};
+
+// Specifies if T has an enabled formatter specialization. A type can be
+// formattable even if it doesn't have a formatter e.g. via a conversion.
+template <typename T, typename Context>
+using has_formatter =
+    std::is_constructible<typename Context::template formatter_type<T>>;
+
+// Checks whether T is a container with contiguous storage.
+template <typename T> struct is_contiguous : std::false_type {};
+template <typename Char>
+struct is_contiguous<std::basic_string<Char>> : std::true_type {};
+
+class appender;
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+// Extracts a reference to the container from back_insert_iterator.
+template <typename Container>
+inline auto get_container(std::back_insert_iterator<Container> it)
+    -> Container& {
+  using bi_iterator = std::back_insert_iterator<Container>;
+  struct accessor : bi_iterator {
+    accessor(bi_iterator iter) : bi_iterator(iter) {}
+    using bi_iterator::container;
+  };
+  return *accessor(it).container;
+}
+
+template <typename Char, typename InputIt, typename OutputIt>
+FMT_CONSTEXPR auto copy_str(InputIt begin, InputIt end, OutputIt out)
+    -> OutputIt {
+  while (begin != end) *out++ = static_cast<Char>(*begin++);
+  return out;
+}
+
+template <typename Char, FMT_ENABLE_IF(std::is_same<Char, char>::value)>
+FMT_CONSTEXPR auto copy_str(const Char* begin, const Char* end, Char* out)
+    -> Char* {
+  if (is_constant_evaluated())
+    return copy_str<Char, const Char*, Char*>(begin, end, out);
+  auto size = to_unsigned(end - begin);
+  memcpy(out, begin, size);
+  return out + size;
+}
+
+/**
+  \rst
+  A contiguous memory buffer with an optional growing ability. It is an internal
+  class and shouldn't be used directly, only via `~fmt::basic_memory_buffer`.
+  \endrst
+ */
+template <typename T> class buffer {
+ private:
+  T* ptr_;
+  size_t size_;
+  size_t capacity_;
+
+ protected:
+  // Don't initialize ptr_ since it is not accessed to save a few cycles.
+  FMT_MSC_WARNING(suppress : 26495)
+  buffer(size_t sz) FMT_NOEXCEPT : size_(sz), capacity_(sz) {}
+
+  buffer(T* p = nullptr, size_t sz = 0, size_t cap = 0) FMT_NOEXCEPT
+      : ptr_(p),
+        size_(sz),
+        capacity_(cap) {}
+
+  ~buffer() = default;
+  buffer(buffer&&) = default;
+
+  /** Sets the buffer data and capacity. */
+  void set(T* buf_data, size_t buf_capacity) FMT_NOEXCEPT {
+    ptr_ = buf_data;
+    capacity_ = buf_capacity;
+  }
+
+  /** Increases the buffer capacity to hold at least *capacity* elements. */
+  virtual void grow(size_t capacity) = 0;
+
+ public:
+  using value_type = T;
+  using const_reference = const T&;
+
+  buffer(const buffer&) = delete;
+  void operator=(const buffer&) = delete;
+
+  auto begin() FMT_NOEXCEPT -> T* { return ptr_; }
+  auto end() FMT_NOEXCEPT -> T* { return ptr_ + size_; }
+
+  auto begin() const FMT_NOEXCEPT -> const T* { return ptr_; }
+  auto end() const FMT_NOEXCEPT -> const T* { return ptr_ + size_; }
+
+  /** Returns the size of this buffer. */
+  auto size() const FMT_NOEXCEPT -> size_t { return size_; }
+
+  /** Returns the capacity of this buffer. */
+  auto capacity() const FMT_NOEXCEPT -> size_t { return capacity_; }
+
+  /** Returns a pointer to the buffer data. */
+  auto data() FMT_NOEXCEPT -> T* { return ptr_; }
+
+  /** Returns a pointer to the buffer data. */
+  auto data() const FMT_NOEXCEPT -> const T* { return ptr_; }
+
+  /** Clears this buffer. */
+  void clear() { size_ = 0; }
+
+  // Tries resizing the buffer to contain *count* elements. If T is a POD type
+  // the new elements may not be initialized.
+  void try_resize(size_t count) {
+    try_reserve(count);
+    size_ = count <= capacity_ ? count : capacity_;
+  }
+
+  // Tries increasing the buffer capacity to *new_capacity*. It can increase the
+  // capacity by a smaller amount than requested but guarantees there is space
+  // for at least one additional element either by increasing the capacity or by
+  // flushing the buffer if it is full.
+  void try_reserve(size_t new_capacity) {
+    if (new_capacity > capacity_) grow(new_capacity);
+  }
+
+  void push_back(const T& value) {
+    try_reserve(size_ + 1);
+    ptr_[size_++] = value;
+  }
+
+  /** Appends data to the end of the buffer. */
+  template <typename U> void append(const U* begin, const U* end);
+
+  template <typename I> auto operator[](I index) -> T& { return ptr_[index]; }
+  template <typename I> auto operator[](I index) const -> const T& {
+    return ptr_[index];
+  }
+};
+
+struct buffer_traits {
+  explicit buffer_traits(size_t) {}
+  auto count() const -> size_t { return 0; }
+  auto limit(size_t size) -> size_t { return size; }
+};
+
+class fixed_buffer_traits {
+ private:
+  size_t count_ = 0;
+  size_t limit_;
+
+ public:
+  explicit fixed_buffer_traits(size_t limit) : limit_(limit) {}
+  auto count() const -> size_t { return count_; }
+  auto limit(size_t size) -> size_t {
+    size_t n = limit_ > count_ ? limit_ - count_ : 0;
+    count_ += size;
+    return size < n ? size : n;
+  }
+};
+
+// A buffer that writes to an output iterator when flushed.
+template <typename OutputIt, typename T, typename Traits = buffer_traits>
+class iterator_buffer final : public Traits, public buffer<T> {
+ private:
+  OutputIt out_;
+  enum { buffer_size = 256 };
+  T data_[buffer_size];
+
+ protected:
+  void grow(size_t) final FMT_OVERRIDE {
+    if (this->size() == buffer_size) flush();
+  }
+
+  void flush() {
+    auto size = this->size();
+    this->clear();
+    out_ = copy_str<T>(data_, data_ + this->limit(size), out_);
+  }
+
+ public:
+  explicit iterator_buffer(OutputIt out, size_t n = buffer_size)
+      : Traits(n), buffer<T>(data_, 0, buffer_size), out_(out) {}
+  iterator_buffer(iterator_buffer&& other)
+      : Traits(other), buffer<T>(data_, 0, buffer_size), out_(other.out_) {}
+  ~iterator_buffer() { flush(); }
+
+  auto out() -> OutputIt {
+    flush();
+    return out_;
+  }
+  auto count() const -> size_t { return Traits::count() + this->size(); }
+};
+
+template <typename T> class iterator_buffer<T*, T> final : public buffer<T> {
+ protected:
+  void grow(size_t) final FMT_OVERRIDE {}
+
+ public:
+  explicit iterator_buffer(T* out, size_t = 0) : buffer<T>(out, 0, ~size_t()) {}
+
+  auto out() -> T* { return &*this->end(); }
+};
+
+// A buffer that writes to a container with the contiguous storage.
+template <typename Container>
+class iterator_buffer<std::back_insert_iterator<Container>,
+                      enable_if_t<is_contiguous<Container>::value,
+                                  typename Container::value_type>>
+    final : public buffer<typename Container::value_type> {
+ private:
+  Container& container_;
+
+ protected:
+  void grow(size_t capacity) final FMT_OVERRIDE {
+    container_.resize(capacity);
+    this->set(&container_[0], capacity);
+  }
+
+ public:
+  explicit iterator_buffer(Container& c)
+      : buffer<typename Container::value_type>(c.size()), container_(c) {}
+  explicit iterator_buffer(std::back_insert_iterator<Container> out, size_t = 0)
+      : iterator_buffer(get_container(out)) {}
+  auto out() -> std::back_insert_iterator<Container> {
+    return std::back_inserter(container_);
+  }
+};
+
+// A buffer that counts the number of code units written discarding the output.
+template <typename T = char> class counting_buffer final : public buffer<T> {
+ private:
+  enum { buffer_size = 256 };
+  T data_[buffer_size];
+  size_t count_ = 0;
+
+ protected:
+  void grow(size_t) final FMT_OVERRIDE {
+    if (this->size() != buffer_size) return;
+    count_ += this->size();
+    this->clear();
+  }
+
+ public:
+  counting_buffer() : buffer<T>(data_, 0, buffer_size) {}
+
+  auto count() -> size_t { return count_ + this->size(); }
+};
+
+template <typename T>
+using buffer_appender = conditional_t<std::is_same<T, char>::value, appender,
+                                      std::back_insert_iterator<buffer<T>>>;
+
+// Maps an output iterator to a buffer.
+template <typename T, typename OutputIt>
+auto get_buffer(OutputIt out) -> iterator_buffer<OutputIt, T> {
+  return iterator_buffer<OutputIt, T>(out);
+}
+
+template <typename Buffer>
+auto get_iterator(Buffer& buf) -> decltype(buf.out()) {
+  return buf.out();
+}
+template <typename T> auto get_iterator(buffer<T>& buf) -> buffer_appender<T> {
+  return buffer_appender<T>(buf);
+}
+
+template <typename T, typename Char = char, typename Enable = void>
+struct fallback_formatter {
+  fallback_formatter() = delete;
+};
+
+// Specifies if T has an enabled fallback_formatter specialization.
+template <typename T, typename Char>
+using has_fallback_formatter =
+    std::is_constructible<fallback_formatter<T, Char>>;
+
+struct view {};
+
+template <typename Char, typename T> struct named_arg : view {
+  const Char* name;
+  const T& value;
+  named_arg(const Char* n, const T& v) : name(n), value(v) {}
+};
+
+template <typename Char> struct named_arg_info {
+  const Char* name;
+  int id;
+};
+
+template <typename T, typename Char, size_t NUM_ARGS, size_t NUM_NAMED_ARGS>
+struct arg_data {
+  // args_[0].named_args points to named_args_ to avoid bloating format_args.
+  // +1 to workaround a bug in gcc 7.5 that causes duplicated-branches warning.
+  T args_[1 + (NUM_ARGS != 0 ? NUM_ARGS : +1)];
+  named_arg_info<Char> named_args_[NUM_NAMED_ARGS];
+
+  template <typename... U>
+  arg_data(const U&... init) : args_{T(named_args_, NUM_NAMED_ARGS), init...} {}
+  arg_data(const arg_data& other) = delete;
+  auto args() const -> const T* { return args_ + 1; }
+  auto named_args() -> named_arg_info<Char>* { return named_args_; }
+};
+
+template <typename T, typename Char, size_t NUM_ARGS>
+struct arg_data<T, Char, NUM_ARGS, 0> {
+  // +1 to workaround a bug in gcc 7.5 that causes duplicated-branches warning.
+  T args_[NUM_ARGS != 0 ? NUM_ARGS : +1];
+
+  template <typename... U>
+  FMT_CONSTEXPR FMT_INLINE arg_data(const U&... init) : args_{init...} {}
+  FMT_CONSTEXPR FMT_INLINE auto args() const -> const T* { return args_; }
+  FMT_CONSTEXPR FMT_INLINE auto named_args() -> std::nullptr_t {
+    return nullptr;
+  }
+};
+
+template <typename Char>
+inline void init_named_args(named_arg_info<Char>*, int, int) {}
+
+template <typename T> struct is_named_arg : std::false_type {};
+template <typename T> struct is_statically_named_arg : std::false_type {};
+
+template <typename T, typename Char>
+struct is_named_arg<named_arg<Char, T>> : std::true_type {};
+
+template <typename Char, typename T, typename... Tail,
+          FMT_ENABLE_IF(!is_named_arg<T>::value)>
+void init_named_args(named_arg_info<Char>* named_args, int arg_count,
+                     int named_arg_count, const T&, const Tail&... args) {
+  init_named_args(named_args, arg_count + 1, named_arg_count, args...);
+}
+
+template <typename Char, typename T, typename... Tail,
+          FMT_ENABLE_IF(is_named_arg<T>::value)>
+void init_named_args(named_arg_info<Char>* named_args, int arg_count,
+                     int named_arg_count, const T& arg, const Tail&... args) {
+  named_args[named_arg_count++] = {arg.name, arg_count};
+  init_named_args(named_args, arg_count + 1, named_arg_count, args...);
+}
+
+template <typename... Args>
+FMT_CONSTEXPR FMT_INLINE void init_named_args(std::nullptr_t, int, int,
+                                              const Args&...) {}
+
+template <bool B = false> constexpr auto count() -> size_t { return B ? 1 : 0; }
+template <bool B1, bool B2, bool... Tail> constexpr auto count() -> size_t {
+  return (B1 ? 1 : 0) + count<B2, Tail...>();
+}
+
+template <typename... Args> constexpr auto count_named_args() -> size_t {
+  return count<is_named_arg<Args>::value...>();
+}
+
+enum class type {
+  none_type,
+  // Integer types should go first,
+  int_type,
+  uint_type,
+  long_long_type,
+  ulong_long_type,
+  int128_type,
+  uint128_type,
+  bool_type,
+  char_type,
+  last_integer_type = char_type,
+  // followed by floating-point types.
+  float_type,
+  double_type,
+  long_double_type,
+  last_numeric_type = long_double_type,
+  cstring_type,
+  string_type,
+  pointer_type,
+  custom_type
+};
+
+// Maps core type T to the corresponding type enum constant.
+template <typename T, typename Char>
+struct type_constant : std::integral_constant<type, type::custom_type> {};
+
+#define FMT_TYPE_CONSTANT(Type, constant) \
+  template <typename Char>                \
+  struct type_constant<Type, Char>        \
+      : std::integral_constant<type, type::constant> {}
+
+FMT_TYPE_CONSTANT(int, int_type);
+FMT_TYPE_CONSTANT(unsigned, uint_type);
+FMT_TYPE_CONSTANT(long long, long_long_type);
+FMT_TYPE_CONSTANT(unsigned long long, ulong_long_type);
+FMT_TYPE_CONSTANT(int128_t, int128_type);
+FMT_TYPE_CONSTANT(uint128_t, uint128_type);
+FMT_TYPE_CONSTANT(bool, bool_type);
+FMT_TYPE_CONSTANT(Char, char_type);
+FMT_TYPE_CONSTANT(float, float_type);
+FMT_TYPE_CONSTANT(double, double_type);
+FMT_TYPE_CONSTANT(long double, long_double_type);
+FMT_TYPE_CONSTANT(const Char*, cstring_type);
+FMT_TYPE_CONSTANT(basic_string_view<Char>, string_type);
+FMT_TYPE_CONSTANT(const void*, pointer_type);
+
+constexpr bool is_integral_type(type t) {
+  return t > type::none_type && t <= type::last_integer_type;
+}
+
+constexpr bool is_arithmetic_type(type t) {
+  return t > type::none_type && t <= type::last_numeric_type;
+}
+
+template <typename Char> struct string_value {
+  const Char* data;
+  size_t size;
+};
+
+template <typename Char> struct named_arg_value {
+  const named_arg_info<Char>* data;
+  size_t size;
+};
+
+template <typename Context> struct custom_value {
+  using parse_context = typename Context::parse_context_type;
+  const void* value;
+  void (*format)(const void* arg, parse_context& parse_ctx, Context& ctx);
+};
+
+// A formatting argument value.
+template <typename Context> class value {
+ public:
+  using char_type = typename Context::char_type;
+
+  union {
+    monostate no_value;
+    int int_value;
+    unsigned uint_value;
+    long long long_long_value;
+    unsigned long long ulong_long_value;
+    int128_t int128_value;
+    uint128_t uint128_value;
+    bool bool_value;
+    char_type char_value;
+    float float_value;
+    double double_value;
+    long double long_double_value;
+    const void* pointer;
+    string_value<char_type> string;
+    custom_value<Context> custom;
+    named_arg_value<char_type> named_args;
+  };
+
+  constexpr FMT_INLINE value() : no_value() {}
+  constexpr FMT_INLINE value(int val) : int_value(val) {}
+  constexpr FMT_INLINE value(unsigned val) : uint_value(val) {}
+  constexpr FMT_INLINE value(long long val) : long_long_value(val) {}
+  constexpr FMT_INLINE value(unsigned long long val) : ulong_long_value(val) {}
+  FMT_INLINE value(int128_t val) : int128_value(val) {}
+  FMT_INLINE value(uint128_t val) : uint128_value(val) {}
+  FMT_INLINE value(float val) : float_value(val) {}
+  FMT_INLINE value(double val) : double_value(val) {}
+  FMT_INLINE value(long double val) : long_double_value(val) {}
+  constexpr FMT_INLINE value(bool val) : bool_value(val) {}
+  constexpr FMT_INLINE value(char_type val) : char_value(val) {}
+  FMT_CONSTEXPR FMT_INLINE value(const char_type* val) {
+    string.data = val;
+    if (is_constant_evaluated()) string.size = {};
+  }
+  FMT_CONSTEXPR FMT_INLINE value(basic_string_view<char_type> val) {
+    string.data = val.data();
+    string.size = val.size();
+  }
+  FMT_INLINE value(const void* val) : pointer(val) {}
+  FMT_INLINE value(const named_arg_info<char_type>* args, size_t size)
+      : named_args{args, size} {}
+
+  template <typename T> FMT_CONSTEXPR FMT_INLINE value(const T& val) {
+    custom.value = &val;
+    // Get the formatter type through the context to allow different contexts
+    // have different extension points, e.g. `formatter<T>` for `format` and
+    // `printf_formatter<T>` for `printf`.
+    custom.format = format_custom_arg<
+        T, conditional_t<has_formatter<T, Context>::value,
+                         typename Context::template formatter_type<T>,
+                         fallback_formatter<T, char_type>>>;
+  }
+
+ private:
+  // Formats an argument of a custom type, such as a user-defined class.
+  template <typename T, typename Formatter>
+  static void format_custom_arg(const void* arg,
+                                typename Context::parse_context_type& parse_ctx,
+                                Context& ctx) {
+    Formatter f;
+    parse_ctx.advance_to(f.parse(parse_ctx));
+    ctx.advance_to(f.format(*static_cast<const T*>(arg), ctx));
+  }
+};
+
+template <typename Context, typename T>
+FMT_CONSTEXPR auto make_arg(const T& value) -> basic_format_arg<Context>;
+
+// To minimize the number of types we need to deal with, long is translated
+// either to int or to long long depending on its size.
+enum { long_short = sizeof(long) == sizeof(int) };
+using long_type = conditional_t<long_short, int, long long>;
+using ulong_type = conditional_t<long_short, unsigned, unsigned long long>;
+
+struct unformattable {};
+
+// Maps formatting arguments to core types.
+template <typename Context> struct arg_mapper {
+  using char_type = typename Context::char_type;
+
+  FMT_CONSTEXPR FMT_INLINE auto map(signed char val) -> int { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(unsigned char val) -> unsigned {
+    return val;
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(short val) -> int { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(unsigned short val) -> unsigned {
+    return val;
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(int val) -> int { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(unsigned val) -> unsigned { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(long val) -> long_type { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(unsigned long val) -> ulong_type {
+    return val;
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(long long val) -> long long { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(unsigned long long val)
+      -> unsigned long long {
+    return val;
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(int128_t val) -> int128_t { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(uint128_t val) -> uint128_t { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(bool val) -> bool { return val; }
+
+  template <typename T, FMT_ENABLE_IF(is_char<T>::value)>
+  FMT_CONSTEXPR FMT_INLINE auto map(T val) -> char_type {
+    static_assert(
+        std::is_same<T, char>::value || std::is_same<T, char_type>::value,
+        "mixing character types is disallowed");
+    return val;
+  }
+
+  FMT_CONSTEXPR FMT_INLINE auto map(float val) -> float { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(double val) -> double { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(long double val) -> long double {
+    return val;
+  }
+
+  FMT_CONSTEXPR FMT_INLINE auto map(char_type* val) -> const char_type* {
+    return val;
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(const char_type* val) -> const char_type* {
+    return val;
+  }
+  template <typename T, FMT_ENABLE_IF(is_string<T>::value)>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& val)
+      -> basic_string_view<char_type> {
+    static_assert(std::is_same<char_type, char_t<T>>::value,
+                  "mixing character types is disallowed");
+    return to_string_view(val);
+  }
+  template <typename T,
+            FMT_ENABLE_IF(
+                std::is_constructible<basic_string_view<char_type>, T>::value &&
+                !is_string<T>::value && !has_formatter<T, Context>::value &&
+                !has_fallback_formatter<T, char_type>::value)>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& val)
+      -> basic_string_view<char_type> {
+    return basic_string_view<char_type>(val);
+  }
+  template <
+      typename T,
+      FMT_ENABLE_IF(
+          std::is_constructible<std_string_view<char_type>, T>::value &&
+          !std::is_constructible<basic_string_view<char_type>, T>::value &&
+          !is_string<T>::value && !has_formatter<T, Context>::value &&
+          !has_fallback_formatter<T, char_type>::value)>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& val)
+      -> basic_string_view<char_type> {
+    return std_string_view<char_type>(val);
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(const signed char* val) -> const char* {
+    static_assert(std::is_same<char_type, char>::value, "invalid string type");
+    return reinterpret_cast<const char*>(val);
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(const unsigned char* val) -> const char* {
+    static_assert(std::is_same<char_type, char>::value, "invalid string type");
+    return reinterpret_cast<const char*>(val);
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(signed char* val) -> const char* {
+    const auto* const_val = val;
+    return map(const_val);
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(unsigned char* val) -> const char* {
+    const auto* const_val = val;
+    return map(const_val);
+  }
+
+  FMT_CONSTEXPR FMT_INLINE auto map(void* val) -> const void* { return val; }
+  FMT_CONSTEXPR FMT_INLINE auto map(const void* val) -> const void* {
+    return val;
+  }
+  FMT_CONSTEXPR FMT_INLINE auto map(std::nullptr_t val) -> const void* {
+    return val;
+  }
+
+  // We use SFINAE instead of a const T* parameter to avoid conflicting with
+  // the C array overload.
+  template <typename T>
+  FMT_CONSTEXPR auto map(T) -> enable_if_t<std::is_pointer<T>::value, int> {
+    // Formatting of arbitrary pointers is disallowed. If you want to output
+    // a pointer cast it to "void *" or "const void *". In particular, this
+    // forbids formatting of "[const] volatile char *" which is printed as bool
+    // by iostreams.
+    static_assert(!sizeof(T), "formatting of non-void pointers is disallowed");
+    return 0;
+  }
+
+  template <typename T, std::size_t N>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T (&values)[N]) -> const T (&)[N] {
+    return values;
+  }
+
+  template <typename T,
+            FMT_ENABLE_IF(std::is_enum<T>::value &&
+                          !has_formatter<T, Context>::value &&
+                          !has_fallback_formatter<T, char_type>::value)>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& val)
+      -> decltype(std::declval<arg_mapper>().map(
+          static_cast<typename std::underlying_type<T>::type>(val))) {
+    return map(static_cast<typename std::underlying_type<T>::type>(val));
+  }
+  template <typename T,
+            FMT_ENABLE_IF(!is_string<T>::value && !is_char<T>::value &&
+                          (has_formatter<T, Context>::value ||
+                           has_fallback_formatter<T, char_type>::value))>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& val) -> const T& {
+    return val;
+  }
+
+  template <typename T, FMT_ENABLE_IF(is_named_arg<T>::value)>
+  FMT_CONSTEXPR FMT_INLINE auto map(const T& named_arg)
+      -> decltype(std::declval<arg_mapper>().map(named_arg.value)) {
+    return map(named_arg.value);
+  }
+
+  auto map(...) -> unformattable { return {}; }
+};
+
+// A type constant after applying arg_mapper<Context>.
+template <typename T, typename Context>
+using mapped_type_constant =
+    type_constant<decltype(arg_mapper<Context>().map(std::declval<const T&>())),
+                  typename Context::char_type>;
+
+enum { packed_arg_bits = 4 };
+// Maximum number of arguments with packed types.
+enum { max_packed_args = 62 / packed_arg_bits };
+enum : unsigned long long { is_unpacked_bit = 1ULL << 63 };
+enum : unsigned long long { has_named_args_bit = 1ULL << 62 };
+
+FMT_END_DETAIL_NAMESPACE
+
+// An output iterator that appends to a buffer.
+// It is used to reduce symbol sizes for the common case.
+class appender : public std::back_insert_iterator<detail::buffer<char>> {
+  using base = std::back_insert_iterator<detail::buffer<char>>;
+
+  template <typename T>
+  friend auto get_buffer(appender out) -> detail::buffer<char>& {
+    return detail::get_container(out);
+  }
+
+ public:
+  using std::back_insert_iterator<detail::buffer<char>>::back_insert_iterator;
+  appender(base it) : base(it) {}
+  using _Unchecked_type = appender;  // Mark iterator as checked.
+
+  auto operator++() -> appender& {
+    base::operator++();
+    return *this;
+  }
+
+  auto operator++(int) -> appender {
+    auto tmp = *this;
+    ++*this;
+    return tmp;
+  }
+};
+
+// A formatting argument. It is a trivially copyable/constructible type to
+// allow storage in basic_memory_buffer.
+template <typename Context> class basic_format_arg {
+ private:
+  detail::value<Context> value_;
+  detail::type type_;
+
+  template <typename ContextType, typename T>
+  friend FMT_CONSTEXPR auto detail::make_arg(const T& value)
+      -> basic_format_arg<ContextType>;
+
+  template <typename Visitor, typename Ctx>
+  friend FMT_CONSTEXPR auto visit_format_arg(Visitor&& vis,
+                                             const basic_format_arg<Ctx>& arg)
+      -> decltype(vis(0));
+
+  friend class basic_format_args<Context>;
+  friend class dynamic_format_arg_store<Context>;
+
+  using char_type = typename Context::char_type;
+
+  template <typename T, typename Char, size_t NUM_ARGS, size_t NUM_NAMED_ARGS>
+  friend struct detail::arg_data;
+
+  basic_format_arg(const detail::named_arg_info<char_type>* args, size_t size)
+      : value_(args, size) {}
+
+ public:
+  class handle {
+   public:
+    explicit handle(detail::custom_value<Context> custom) : custom_(custom) {}
+
+    void format(typename Context::parse_context_type& parse_ctx,
+                Context& ctx) const {
+      custom_.format(custom_.value, parse_ctx, ctx);
+    }
+
+   private:
+    detail::custom_value<Context> custom_;
+  };
+
+  constexpr basic_format_arg() : type_(detail::type::none_type) {}
+
+  constexpr explicit operator bool() const FMT_NOEXCEPT {
+    return type_ != detail::type::none_type;
+  }
+
+  auto type() const -> detail::type { return type_; }
+
+  auto is_integral() const -> bool { return detail::is_integral_type(type_); }
+  auto is_arithmetic() const -> bool {
+    return detail::is_arithmetic_type(type_);
+  }
+};
+
+/**
+  \rst
+  Visits an argument dispatching to the appropriate visit method based on
+  the argument type. For example, if the argument type is ``double`` then
+  ``vis(value)`` will be called with the value of type ``double``.
+  \endrst
+ */
+template <typename Visitor, typename Context>
+FMT_CONSTEXPR FMT_INLINE auto visit_format_arg(
+    Visitor&& vis, const basic_format_arg<Context>& arg) -> decltype(vis(0)) {
+  switch (arg.type_) {
+  case detail::type::none_type:
+    break;
+  case detail::type::int_type:
+    return vis(arg.value_.int_value);
+  case detail::type::uint_type:
+    return vis(arg.value_.uint_value);
+  case detail::type::long_long_type:
+    return vis(arg.value_.long_long_value);
+  case detail::type::ulong_long_type:
+    return vis(arg.value_.ulong_long_value);
+  case detail::type::int128_type:
+    return vis(detail::convert_for_visit(arg.value_.int128_value));
+  case detail::type::uint128_type:
+    return vis(detail::convert_for_visit(arg.value_.uint128_value));
+  case detail::type::bool_type:
+    return vis(arg.value_.bool_value);
+  case detail::type::char_type:
+    return vis(arg.value_.char_value);
+  case detail::type::float_type:
+    return vis(arg.value_.float_value);
+  case detail::type::double_type:
+    return vis(arg.value_.double_value);
+  case detail::type::long_double_type:
+    return vis(arg.value_.long_double_value);
+  case detail::type::cstring_type:
+    return vis(arg.value_.string.data);
+  case detail::type::string_type:
+    using sv = basic_string_view<typename Context::char_type>;
+    return vis(sv(arg.value_.string.data, arg.value_.string.size));
+  case detail::type::pointer_type:
+    return vis(arg.value_.pointer);
+  case detail::type::custom_type:
+    return vis(typename basic_format_arg<Context>::handle(arg.value_.custom));
+  }
+  return vis(monostate());
+}
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+template <typename Char, typename InputIt>
+auto copy_str(InputIt begin, InputIt end, appender out) -> appender {
+  get_container(out).append(begin, end);
+  return out;
+}
+
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 500
+// A workaround for gcc 4.8 to make void_t work in a SFINAE context.
+template <typename... Ts> struct void_t_impl { using type = void; };
+template <typename... Ts>
+using void_t = typename detail::void_t_impl<Ts...>::type;
+#else
+template <typename...> using void_t = void;
+#endif
+
+template <typename It, typename T, typename Enable = void>
+struct is_output_iterator : std::false_type {};
+
+template <typename It, typename T>
+struct is_output_iterator<
+    It, T,
+    void_t<typename std::iterator_traits<It>::iterator_category,
+           decltype(*std::declval<It>() = std::declval<T>())>>
+    : std::true_type {};
+
+template <typename OutputIt>
+struct is_back_insert_iterator : std::false_type {};
+template <typename Container>
+struct is_back_insert_iterator<std::back_insert_iterator<Container>>
+    : std::true_type {};
+
+template <typename OutputIt>
+struct is_contiguous_back_insert_iterator : std::false_type {};
+template <typename Container>
+struct is_contiguous_back_insert_iterator<std::back_insert_iterator<Container>>
+    : is_contiguous<Container> {};
+template <>
+struct is_contiguous_back_insert_iterator<appender> : std::true_type {};
+
+// A type-erased reference to an std::locale to avoid heavy <locale> include.
+class locale_ref {
+ private:
+  const void* locale_;  // A type-erased pointer to std::locale.
+
+ public:
+  constexpr locale_ref() : locale_(nullptr) {}
+  template <typename Locale> explicit locale_ref(const Locale& loc);
+
+  explicit operator bool() const FMT_NOEXCEPT { return locale_ != nullptr; }
+
+  template <typename Locale> auto get() const -> Locale;
+};
+
+template <typename> constexpr auto encode_types() -> unsigned long long {
+  return 0;
+}
+
+template <typename Context, typename Arg, typename... Args>
+constexpr auto encode_types() -> unsigned long long {
+  return static_cast<unsigned>(mapped_type_constant<Arg, Context>::value) |
+         (encode_types<Context, Args...>() << packed_arg_bits);
+}
+
+template <typename Context, typename T>
+FMT_CONSTEXPR auto make_arg(const T& value) -> basic_format_arg<Context> {
+  basic_format_arg<Context> arg;
+  arg.type_ = mapped_type_constant<T, Context>::value;
+  arg.value_ = arg_mapper<Context>().map(value);
+  return arg;
+}
+
+// The type template parameter is there to avoid an ODR violation when using
+// a fallback formatter in one translation unit and an implicit conversion in
+// another (not recommended).
+template <bool IS_PACKED, typename Context, type, typename T,
+          FMT_ENABLE_IF(IS_PACKED)>
+FMT_CONSTEXPR FMT_INLINE auto make_arg(const T& val) -> value<Context> {
+  const auto& arg = arg_mapper<Context>().map(val);
+  static_assert(
+      !std::is_same<decltype(arg), const unformattable&>::value,
+      "Cannot format an argument. To make type T formattable provide a "
+      "formatter<T> specialization: https://fmt.dev/latest/api.html#udt");
+  return {arg};
+}
+
+template <bool IS_PACKED, typename Context, type, typename T,
+          FMT_ENABLE_IF(!IS_PACKED)>
+inline auto make_arg(const T& value) -> basic_format_arg<Context> {
+  return make_arg<Context>(value);
+}
+FMT_END_DETAIL_NAMESPACE
+
+// Formatting context.
+template <typename OutputIt, typename Char> class basic_format_context {
+ public:
+  /** The character type for the output. */
+  using char_type = Char;
+
+ private:
+  OutputIt out_;
+  basic_format_args<basic_format_context> args_;
+  detail::locale_ref loc_;
+
+ public:
+  using iterator = OutputIt;
+  using format_arg = basic_format_arg<basic_format_context>;
+  using parse_context_type = basic_format_parse_context<Char>;
+  template <typename T> using formatter_type = formatter<T, char_type>;
+
+  basic_format_context(basic_format_context&&) = default;
+  basic_format_context(const basic_format_context&) = delete;
+  void operator=(const basic_format_context&) = delete;
+  /**
+   Constructs a ``basic_format_context`` object. References to the arguments are
+   stored in the object so make sure they have appropriate lifetimes.
+   */
+  constexpr basic_format_context(
+      OutputIt out, basic_format_args<basic_format_context> ctx_args,
+      detail::locale_ref loc = detail::locale_ref())
+      : out_(out), args_(ctx_args), loc_(loc) {}
+
+  constexpr auto arg(int id) const -> format_arg { return args_.get(id); }
+  FMT_CONSTEXPR auto arg(basic_string_view<char_type> name) -> format_arg {
+    return args_.get(name);
+  }
+  FMT_CONSTEXPR auto arg_id(basic_string_view<char_type> name) -> int {
+    return args_.get_id(name);
+  }
+  auto args() const -> const basic_format_args<basic_format_context>& {
+    return args_;
+  }
+
+  FMT_CONSTEXPR auto error_handler() -> detail::error_handler { return {}; }
+  void on_error(const char* message) { error_handler().on_error(message); }
+
+  // Returns an iterator to the beginning of the output range.
+  FMT_CONSTEXPR auto out() -> iterator { return out_; }
+
+  // Advances the begin iterator to ``it``.
+  void advance_to(iterator it) {
+    if (!detail::is_back_insert_iterator<iterator>()) out_ = it;
+  }
+
+  FMT_CONSTEXPR auto locale() -> detail::locale_ref { return loc_; }
+};
+
+template <typename Char>
+using buffer_context =
+    basic_format_context<detail::buffer_appender<Char>, Char>;
+using format_context = buffer_context<char>;
+
+// Workaround an alias issue: https://stackoverflow.com/q/62767544/471164.
+#define FMT_BUFFER_CONTEXT(Char) \
+  basic_format_context<detail::buffer_appender<Char>, Char>
+
+template <typename T, typename Char = char>
+using is_formattable = bool_constant<
+    !std::is_same<decltype(detail::arg_mapper<buffer_context<Char>>().map(
+                      std::declval<T>())),
+                  detail::unformattable>::value &&
+    !detail::has_fallback_formatter<T, Char>::value>;
+
+/**
+  \rst
+  An array of references to arguments. It can be implicitly converted into
+  `~fmt::basic_format_args` for passing into type-erased formatting functions
+  such as `~fmt::vformat`.
+  \endrst
+ */
+template <typename Context, typename... Args>
+class format_arg_store
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 409
+    // Workaround a GCC template argument substitution bug.
+    : public basic_format_args<Context>
+#endif
+{
+ private:
+  static const size_t num_args = sizeof...(Args);
+  static const size_t num_named_args = detail::count_named_args<Args...>();
+  static const bool is_packed = num_args <= detail::max_packed_args;
+
+  using value_type = conditional_t<is_packed, detail::value<Context>,
+                                   basic_format_arg<Context>>;
+
+  detail::arg_data<value_type, typename Context::char_type, num_args,
+                   num_named_args>
+      data_;
+
+  friend class basic_format_args<Context>;
+
+  static constexpr unsigned long long desc =
+      (is_packed ? detail::encode_types<Context, Args...>()
+                 : detail::is_unpacked_bit | num_args) |
+      (num_named_args != 0
+           ? static_cast<unsigned long long>(detail::has_named_args_bit)
+           : 0);
+
+ public:
+  FMT_CONSTEXPR FMT_INLINE format_arg_store(const Args&... args)
+      :
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 409
+        basic_format_args<Context>(*this),
+#endif
+        data_{detail::make_arg<
+            is_packed, Context,
+            detail::mapped_type_constant<Args, Context>::value>(args)...} {
+    detail::init_named_args(data_.named_args(), 0, 0, args...);
+  }
+};
+
+/**
+  \rst
+  Constructs a `~fmt::format_arg_store` object that contains references to
+  arguments and can be implicitly converted to `~fmt::format_args`. `Context`
+  can be omitted in which case it defaults to `~fmt::context`.
+  See `~fmt::arg` for lifetime considerations.
+  \endrst
+ */
+template <typename Context = format_context, typename... Args>
+constexpr auto make_format_args(const Args&... args)
+    -> format_arg_store<Context, Args...> {
+  return {args...};
+}
+
+/**
+  \rst
+  Returns a named argument to be used in a formatting function.
+  It should only be used in a call to a formatting function or
+  `dynamic_format_arg_store::push_back`.
+
+  **Example**::
+
+    fmt::print("Elapsed time: {s:.2f} seconds", fmt::arg("s", 1.23));
+  \endrst
+ */
+template <typename Char, typename T>
+inline auto arg(const Char* name, const T& arg) -> detail::named_arg<Char, T> {
+  static_assert(!detail::is_named_arg<T>(), "nested named arguments");
+  return {name, arg};
+}
+
+/**
+  \rst
+  A view of a collection of formatting arguments. To avoid lifetime issues it
+  should only be used as a parameter type in type-erased functions such as
+  ``vformat``::
+
+    void vlog(string_view format_str, format_args args);  // OK
+    format_args args = make_format_args(42);  // Error: dangling reference
+  \endrst
+ */
+template <typename Context> class basic_format_args {
+ public:
+  using size_type = int;
+  using format_arg = basic_format_arg<Context>;
+
+ private:
+  // A descriptor that contains information about formatting arguments.
+  // If the number of arguments is less or equal to max_packed_args then
+  // argument types are passed in the descriptor. This reduces binary code size
+  // per formatting function call.
+  unsigned long long desc_;
+  union {
+    // If is_packed() returns true then argument values are stored in values_;
+    // otherwise they are stored in args_. This is done to improve cache
+    // locality and reduce compiled code size since storing larger objects
+    // may require more code (at least on x86-64) even if the same amount of
+    // data is actually copied to stack. It saves ~10% on the bloat test.
+    const detail::value<Context>* values_;
+    const format_arg* args_;
+  };
+
+  constexpr auto is_packed() const -> bool {
+    return (desc_ & detail::is_unpacked_bit) == 0;
+  }
+  auto has_named_args() const -> bool {
+    return (desc_ & detail::has_named_args_bit) != 0;
+  }
+
+  FMT_CONSTEXPR auto type(int index) const -> detail::type {
+    int shift = index * detail::packed_arg_bits;
+    unsigned int mask = (1 << detail::packed_arg_bits) - 1;
+    return static_cast<detail::type>((desc_ >> shift) & mask);
+  }
+
+  constexpr FMT_INLINE basic_format_args(unsigned long long desc,
+                                         const detail::value<Context>* values)
+      : desc_(desc), values_(values) {}
+  constexpr basic_format_args(unsigned long long desc, const format_arg* args)
+      : desc_(desc), args_(args) {}
+
+ public:
+  constexpr basic_format_args() : desc_(0), args_(nullptr) {}
+
+  /**
+   \rst
+   Constructs a `basic_format_args` object from `~fmt::format_arg_store`.
+   \endrst
+   */
+  template <typename... Args>
+  constexpr FMT_INLINE basic_format_args(
+      const format_arg_store<Context, Args...>& store)
+      : basic_format_args(format_arg_store<Context, Args...>::desc,
+                          store.data_.args()) {}
+
+  /**
+   \rst
+   Constructs a `basic_format_args` object from
+   `~fmt::dynamic_format_arg_store`.
+   \endrst
+   */
+  constexpr FMT_INLINE basic_format_args(
+      const dynamic_format_arg_store<Context>& store)
+      : basic_format_args(store.get_types(), store.data()) {}
+
+  /**
+   \rst
+   Constructs a `basic_format_args` object from a dynamic set of arguments.
+   \endrst
+   */
+  constexpr basic_format_args(const format_arg* args, int count)
+      : basic_format_args(detail::is_unpacked_bit | detail::to_unsigned(count),
+                          args) {}
+
+  /** Returns the argument with the specified id. */
+  FMT_CONSTEXPR auto get(int id) const -> format_arg {
+    format_arg arg;
+    if (!is_packed()) {
+      if (id < max_size()) arg = args_[id];
+      return arg;
+    }
+    if (id >= detail::max_packed_args) return arg;
+    arg.type_ = type(id);
+    if (arg.type_ == detail::type::none_type) return arg;
+    arg.value_ = values_[id];
+    return arg;
+  }
+
+  template <typename Char>
+  auto get(basic_string_view<Char> name) const -> format_arg {
+    int id = get_id(name);
+    return id >= 0 ? get(id) : format_arg();
+  }
+
+  template <typename Char>
+  auto get_id(basic_string_view<Char> name) const -> int {
+    if (!has_named_args()) return -1;
+    const auto& named_args =
+        (is_packed() ? values_[-1] : args_[-1].value_).named_args;
+    for (size_t i = 0; i < named_args.size; ++i) {
+      if (named_args.data[i].name == name) return named_args.data[i].id;
+    }
+    return -1;
+  }
+
+  auto max_size() const -> int {
+    unsigned long long max_packed = detail::max_packed_args;
+    return static_cast<int>(is_packed() ? max_packed
+                                        : desc_ & ~detail::is_unpacked_bit);
+  }
+};
+
+/** An alias to ``basic_format_args<format_context>``. */
+// A separate type would result in shorter symbols but break ABI compatibility
+// between clang and gcc on ARM (#1919).
+using format_args = basic_format_args<format_context>;
+
+// We cannot use enum classes as bit fields because of a gcc bug
+// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=61414.
+namespace align {
+enum type { none, left, right, center, numeric };
+}
+using align_t = align::type;
+namespace sign {
+enum type { none, minus, plus, space };
+}
+using sign_t = sign::type;
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+void throw_format_error(const char* message);
+
+// Workaround an array initialization issue in gcc 4.8.
+template <typename Char> struct fill_t {
+ private:
+  enum { max_size = 4 };
+  Char data_[max_size] = {Char(' '), Char(0), Char(0), Char(0)};
+  unsigned char size_ = 1;
+
+ public:
+  FMT_CONSTEXPR void operator=(basic_string_view<Char> s) {
+    auto size = s.size();
+    if (size > max_size) return throw_format_error("invalid fill");
+    for (size_t i = 0; i < size; ++i) data_[i] = s[i];
+    size_ = static_cast<unsigned char>(size);
+  }
+
+  constexpr auto size() const -> size_t { return size_; }
+  constexpr auto data() const -> const Char* { return data_; }
+
+  FMT_CONSTEXPR auto operator[](size_t index) -> Char& { return data_[index]; }
+  FMT_CONSTEXPR auto operator[](size_t index) const -> const Char& {
+    return data_[index];
+  }
+};
+FMT_END_DETAIL_NAMESPACE
+
+// Format specifiers for built-in and string types.
+template <typename Char> struct basic_format_specs {
+  int width;
+  int precision;
+  char type;
+  align_t align : 4;
+  sign_t sign : 3;
+  bool alt : 1;  // Alternate form ('#').
+  bool localized : 1;
+  detail::fill_t<Char> fill;
+
+  constexpr basic_format_specs()
+      : width(0),
+        precision(-1),
+        type(0),
+        align(align::none),
+        sign(sign::none),
+        alt(false),
+        localized(false) {}
+};
+
+using format_specs = basic_format_specs<char>;
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+enum class arg_id_kind { none, index, name };
+
+// An argument reference.
+template <typename Char> struct arg_ref {
+  FMT_CONSTEXPR arg_ref() : kind(arg_id_kind::none), val() {}
+
+  FMT_CONSTEXPR explicit arg_ref(int index)
+      : kind(arg_id_kind::index), val(index) {}
+  FMT_CONSTEXPR explicit arg_ref(basic_string_view<Char> name)
+      : kind(arg_id_kind::name), val(name) {}
+
+  FMT_CONSTEXPR auto operator=(int idx) -> arg_ref& {
+    kind = arg_id_kind::index;
+    val.index = idx;
+    return *this;
+  }
+
+  arg_id_kind kind;
+  union value {
+    FMT_CONSTEXPR value(int id = 0) : index{id} {}
+    FMT_CONSTEXPR value(basic_string_view<Char> n) : name(n) {}
+
+    int index;
+    basic_string_view<Char> name;
+  } val;
+};
+
+// Format specifiers with width and precision resolved at formatting rather
+// than parsing time to allow re-using the same parsed specifiers with
+// different sets of arguments (precompilation of format strings).
+template <typename Char>
+struct dynamic_format_specs : basic_format_specs<Char> {
+  arg_ref<Char> width_ref;
+  arg_ref<Char> precision_ref;
+};
+
+struct auto_id {};
+
+// A format specifier handler that sets fields in basic_format_specs.
+template <typename Char> class specs_setter {
+ protected:
+  basic_format_specs<Char>& specs_;
+
+ public:
+  explicit FMT_CONSTEXPR specs_setter(basic_format_specs<Char>& specs)
+      : specs_(specs) {}
+
+  FMT_CONSTEXPR specs_setter(const specs_setter& other)
+      : specs_(other.specs_) {}
+
+  FMT_CONSTEXPR void on_align(align_t align) { specs_.align = align; }
+  FMT_CONSTEXPR void on_fill(basic_string_view<Char> fill) {
+    specs_.fill = fill;
+  }
+  FMT_CONSTEXPR void on_sign(sign_t s) { specs_.sign = s; }
+  FMT_CONSTEXPR void on_hash() { specs_.alt = true; }
+  FMT_CONSTEXPR void on_localized() { specs_.localized = true; }
+
+  FMT_CONSTEXPR void on_zero() {
+    if (specs_.align == align::none) specs_.align = align::numeric;
+    specs_.fill[0] = Char('0');
+  }
+
+  FMT_CONSTEXPR void on_width(int width) { specs_.width = width; }
+  FMT_CONSTEXPR void on_precision(int precision) {
+    specs_.precision = precision;
+  }
+  FMT_CONSTEXPR void end_precision() {}
+
+  FMT_CONSTEXPR void on_type(Char type) {
+    specs_.type = static_cast<char>(type);
+  }
+};
+
+// Format spec handler that saves references to arguments representing dynamic
+// width and precision to be resolved at formatting time.
+template <typename ParseContext>
+class dynamic_specs_handler
+    : public specs_setter<typename ParseContext::char_type> {
+ public:
+  using char_type = typename ParseContext::char_type;
+
+  FMT_CONSTEXPR dynamic_specs_handler(dynamic_format_specs<char_type>& specs,
+                                      ParseContext& ctx)
+      : specs_setter<char_type>(specs), specs_(specs), context_(ctx) {}
+
+  FMT_CONSTEXPR dynamic_specs_handler(const dynamic_specs_handler& other)
+      : specs_setter<char_type>(other),
+        specs_(other.specs_),
+        context_(other.context_) {}
+
+  template <typename Id> FMT_CONSTEXPR void on_dynamic_width(Id arg_id) {
+    specs_.width_ref = make_arg_ref(arg_id);
+  }
+
+  template <typename Id> FMT_CONSTEXPR void on_dynamic_precision(Id arg_id) {
+    specs_.precision_ref = make_arg_ref(arg_id);
+  }
+
+  FMT_CONSTEXPR void on_error(const char* message) {
+    context_.on_error(message);
+  }
+
+ private:
+  dynamic_format_specs<char_type>& specs_;
+  ParseContext& context_;
+
+  using arg_ref_type = arg_ref<char_type>;
+
+  FMT_CONSTEXPR auto make_arg_ref(int arg_id) -> arg_ref_type {
+    context_.check_arg_id(arg_id);
+    return arg_ref_type(arg_id);
+  }
+
+  FMT_CONSTEXPR auto make_arg_ref(auto_id) -> arg_ref_type {
+    return arg_ref_type(context_.next_arg_id());
+  }
+
+  FMT_CONSTEXPR auto make_arg_ref(basic_string_view<char_type> arg_id)
+      -> arg_ref_type {
+    context_.check_arg_id(arg_id);
+    basic_string_view<char_type> format_str(
+        context_.begin(), to_unsigned(context_.end() - context_.begin()));
+    return arg_ref_type(arg_id);
+  }
+};
+
+template <typename Char> constexpr bool is_ascii_letter(Char c) {
+  return (c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z');
+}
+
+// Converts a character to ASCII. Returns a number > 127 on conversion failure.
+template <typename Char, FMT_ENABLE_IF(std::is_integral<Char>::value)>
+constexpr auto to_ascii(Char value) -> Char {
+  return value;
+}
+template <typename Char, FMT_ENABLE_IF(std::is_enum<Char>::value)>
+constexpr auto to_ascii(Char value) ->
+    typename std::underlying_type<Char>::type {
+  return value;
+}
+
+template <typename Char>
+FMT_CONSTEXPR auto code_point_length(const Char* begin) -> int {
+  if (const_check(sizeof(Char) != 1)) return 1;
+  constexpr char lengths[] = {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+                              0, 0, 0, 0, 0, 0, 0, 0, 2, 2, 2, 2, 3, 3, 4, 0};
+  int len = lengths[static_cast<unsigned char>(*begin) >> 3];
+
+  // Compute the pointer to the next character early so that the next
+  // iteration can start working on the next character. Neither Clang
+  // nor GCC figure out this reordering on their own.
+  return len + !len;
+}
+
+// Return the result via the out param to workaround gcc bug 77539.
+template <bool IS_CONSTEXPR, typename T, typename Ptr = const T*>
+FMT_CONSTEXPR auto find(Ptr first, Ptr last, T value, Ptr& out) -> bool {
+  for (out = first; out != last; ++out) {
+    if (*out == value) return true;
+  }
+  return false;
+}
+
+template <>
+inline auto find<false, char>(const char* first, const char* last, char value,
+                              const char*& out) -> bool {
+  out = static_cast<const char*>(
+      std::memchr(first, value, to_unsigned(last - first)));
+  return out != nullptr;
+}
+
+// Parses the range [begin, end) as an unsigned integer. This function assumes
+// that the range is non-empty and the first character is a digit.
+template <typename Char>
+FMT_CONSTEXPR auto parse_nonnegative_int(const Char*& begin, const Char* end,
+                                         int error_value) noexcept -> int {
+  FMT_ASSERT(begin != end && '0' <= *begin && *begin <= '9', "");
+  unsigned value = 0, prev = 0;
+  auto p = begin;
+  do {
+    prev = value;
+    value = value * 10 + unsigned(*p - '0');
+    ++p;
+  } while (p != end && '0' <= *p && *p <= '9');
+  auto num_digits = p - begin;
+  begin = p;
+  if (num_digits <= std::numeric_limits<int>::digits10)
+    return static_cast<int>(value);
+  // Check for overflow.
+  const unsigned max = to_unsigned((std::numeric_limits<int>::max)());
+  return num_digits == std::numeric_limits<int>::digits10 + 1 &&
+                 prev * 10ull + unsigned(p[-1] - '0') <= max
+             ? static_cast<int>(value)
+             : error_value;
+}
+
+// Parses fill and alignment.
+template <typename Char, typename Handler>
+FMT_CONSTEXPR auto parse_align(const Char* begin, const Char* end,
+                               Handler&& handler) -> const Char* {
+  FMT_ASSERT(begin != end, "");
+  auto align = align::none;
+  auto p = begin + code_point_length(begin);
+  if (p >= end) p = begin;
+  for (;;) {
+    switch (to_ascii(*p)) {
+    case '<':
+      align = align::left;
+      break;
+    case '>':
+      align = align::right;
+      break;
+    case '^':
+      align = align::center;
+      break;
+    default:
+      break;
+    }
+    if (align != align::none) {
+      if (p != begin) {
+        auto c = *begin;
+        if (c == '{')
+          return handler.on_error("invalid fill character '{'"), begin;
+        handler.on_fill(basic_string_view<Char>(begin, to_unsigned(p - begin)));
+        begin = p + 1;
+      } else
+        ++begin;
+      handler.on_align(align);
+      break;
+    } else if (p == begin) {
+      break;
+    }
+    p = begin;
+  }
+  return begin;
+}
+
+template <typename Char> FMT_CONSTEXPR bool is_name_start(Char c) {
+  return ('a' <= c && c <= 'z') || ('A' <= c && c <= 'Z') || '_' == c;
+}
+
+template <typename Char, typename IDHandler>
+FMT_CONSTEXPR auto do_parse_arg_id(const Char* begin, const Char* end,
+                                   IDHandler&& handler) -> const Char* {
+  FMT_ASSERT(begin != end, "");
+  Char c = *begin;
+  if (c >= '0' && c <= '9') {
+    int index = 0;
+    if (c != '0')
+      index =
+          parse_nonnegative_int(begin, end, (std::numeric_limits<int>::max)());
+    else
+      ++begin;
+    if (begin == end || (*begin != '}' && *begin != ':'))
+      handler.on_error("invalid format string");
+    else
+      handler(index);
+    return begin;
+  }
+  if (!is_name_start(c)) {
+    handler.on_error("invalid format string");
+    return begin;
+  }
+  auto it = begin;
+  do {
+    ++it;
+  } while (it != end && (is_name_start(c = *it) || ('0' <= c && c <= '9')));
+  handler(basic_string_view<Char>(begin, to_unsigned(it - begin)));
+  return it;
+}
+
+template <typename Char, typename IDHandler>
+FMT_CONSTEXPR FMT_INLINE auto parse_arg_id(const Char* begin, const Char* end,
+                                           IDHandler&& handler) -> const Char* {
+  Char c = *begin;
+  if (c != '}' && c != ':') return do_parse_arg_id(begin, end, handler);
+  handler();
+  return begin;
+}
+
+template <typename Char, typename Handler>
+FMT_CONSTEXPR auto parse_width(const Char* begin, const Char* end,
+                               Handler&& handler) -> const Char* {
+  using detail::auto_id;
+  struct width_adapter {
+    Handler& handler;
+
+    FMT_CONSTEXPR void operator()() { handler.on_dynamic_width(auto_id()); }
+    FMT_CONSTEXPR void operator()(int id) { handler.on_dynamic_width(id); }
+    FMT_CONSTEXPR void operator()(basic_string_view<Char> id) {
+      handler.on_dynamic_width(id);
+    }
+    FMT_CONSTEXPR void on_error(const char* message) {
+      if (message) handler.on_error(message);
+    }
+  };
+
+  FMT_ASSERT(begin != end, "");
+  if ('0' <= *begin && *begin <= '9') {
+    int width = parse_nonnegative_int(begin, end, -1);
+    if (width != -1)
+      handler.on_width(width);
+    else
+      handler.on_error("number is too big");
+  } else if (*begin == '{') {
+    ++begin;
+    if (begin != end) begin = parse_arg_id(begin, end, width_adapter{handler});
+    if (begin == end || *begin != '}')
+      return handler.on_error("invalid format string"), begin;
+    ++begin;
+  }
+  return begin;
+}
+
+template <typename Char, typename Handler>
+FMT_CONSTEXPR auto parse_precision(const Char* begin, const Char* end,
+                                   Handler&& handler) -> const Char* {
+  using detail::auto_id;
+  struct precision_adapter {
+    Handler& handler;
+
+    FMT_CONSTEXPR void operator()() { handler.on_dynamic_precision(auto_id()); }
+    FMT_CONSTEXPR void operator()(int id) { handler.on_dynamic_precision(id); }
+    FMT_CONSTEXPR void operator()(basic_string_view<Char> id) {
+      handler.on_dynamic_precision(id);
+    }
+    FMT_CONSTEXPR void on_error(const char* message) {
+      if (message) handler.on_error(message);
+    }
+  };
+
+  ++begin;
+  auto c = begin != end ? *begin : Char();
+  if ('0' <= c && c <= '9') {
+    auto precision = parse_nonnegative_int(begin, end, -1);
+    if (precision != -1)
+      handler.on_precision(precision);
+    else
+      handler.on_error("number is too big");
+  } else if (c == '{') {
+    ++begin;
+    if (begin != end)
+      begin = parse_arg_id(begin, end, precision_adapter{handler});
+    if (begin == end || *begin++ != '}')
+      return handler.on_error("invalid format string"), begin;
+  } else {
+    return handler.on_error("missing precision specifier"), begin;
+  }
+  handler.end_precision();
+  return begin;
+}
+
+// Parses standard format specifiers and sends notifications about parsed
+// components to handler.
+template <typename Char, typename SpecHandler>
+FMT_CONSTEXPR FMT_INLINE auto parse_format_specs(const Char* begin,
+                                                 const Char* end,
+                                                 SpecHandler&& handler)
+    -> const Char* {
+  if (begin + 1 < end && begin[1] == '}' && is_ascii_letter(*begin) &&
+      *begin != 'L') {
+    handler.on_type(*begin++);
+    return begin;
+  }
+
+  if (begin == end) return begin;
+
+  begin = parse_align(begin, end, handler);
+  if (begin == end) return begin;
+
+  // Parse sign.
+  switch (to_ascii(*begin)) {
+  case '+':
+    handler.on_sign(sign::plus);
+    ++begin;
+    break;
+  case '-':
+    handler.on_sign(sign::minus);
+    ++begin;
+    break;
+  case ' ':
+    handler.on_sign(sign::space);
+    ++begin;
+    break;
+  default:
+    break;
+  }
+  if (begin == end) return begin;
+
+  if (*begin == '#') {
+    handler.on_hash();
+    if (++begin == end) return begin;
+  }
+
+  // Parse zero flag.
+  if (*begin == '0') {
+    handler.on_zero();
+    if (++begin == end) return begin;
+  }
+
+  begin = parse_width(begin, end, handler);
+  if (begin == end) return begin;
+
+  // Parse precision.
+  if (*begin == '.') {
+    begin = parse_precision(begin, end, handler);
+    if (begin == end) return begin;
+  }
+
+  if (*begin == 'L') {
+    handler.on_localized();
+    ++begin;
+  }
+
+  // Parse type.
+  if (begin != end && *begin != '}') handler.on_type(*begin++);
+  return begin;
+}
+
+template <typename Char, typename Handler>
+FMT_CONSTEXPR auto parse_replacement_field(const Char* begin, const Char* end,
+                                           Handler&& handler) -> const Char* {
+  struct id_adapter {
+    Handler& handler;
+    int arg_id;
+
+    FMT_CONSTEXPR void operator()() { arg_id = handler.on_arg_id(); }
+    FMT_CONSTEXPR void operator()(int id) { arg_id = handler.on_arg_id(id); }
+    FMT_CONSTEXPR void operator()(basic_string_view<Char> id) {
+      arg_id = handler.on_arg_id(id);
+    }
+    FMT_CONSTEXPR void on_error(const char* message) {
+      if (message) handler.on_error(message);
+    }
+  };
+
+  ++begin;
+  if (begin == end) return handler.on_error("invalid format string"), end;
+  if (*begin == '}') {
+    handler.on_replacement_field(handler.on_arg_id(), begin);
+  } else if (*begin == '{') {
+    handler.on_text(begin, begin + 1);
+  } else {
+    auto adapter = id_adapter{handler, 0};
+    begin = parse_arg_id(begin, end, adapter);
+    Char c = begin != end ? *begin : Char();
+    if (c == '}') {
+      handler.on_replacement_field(adapter.arg_id, begin);
+    } else if (c == ':') {
+      begin = handler.on_format_specs(adapter.arg_id, begin + 1, end);
+      if (begin == end || *begin != '}')
+        return handler.on_error("unknown format specifier"), end;
+    } else {
+      return handler.on_error("missing '}' in format string"), end;
+    }
+  }
+  return begin + 1;
+}
+
+template <bool IS_CONSTEXPR, typename Char, typename Handler>
+FMT_CONSTEXPR FMT_INLINE void parse_format_string(
+    basic_string_view<Char> format_str, Handler&& handler) {
+  // this is most likely a name-lookup defect in msvc's modules implementation
+  using detail::find;
+
+  auto begin = format_str.data();
+  auto end = begin + format_str.size();
+  if (end - begin < 32) {
+    // Use a simple loop instead of memchr for small strings.
+    const Char* p = begin;
+    while (p != end) {
+      auto c = *p++;
+      if (c == '{') {
+        handler.on_text(begin, p - 1);
+        begin = p = parse_replacement_field(p - 1, end, handler);
+      } else if (c == '}') {
+        if (p == end || *p != '}')
+          return handler.on_error("unmatched '}' in format string");
+        handler.on_text(begin, p);
+        begin = ++p;
+      }
+    }
+    handler.on_text(begin, end);
+    return;
+  }
+  struct writer {
+    FMT_CONSTEXPR void operator()(const Char* pbegin, const Char* pend) {
+      if (pbegin == pend) return;
+      for (;;) {
+        const Char* p = nullptr;
+        if (!find<IS_CONSTEXPR>(pbegin, pend, Char('}'), p))
+          return handler_.on_text(pbegin, pend);
+        ++p;
+        if (p == pend || *p != '}')
+          return handler_.on_error("unmatched '}' in format string");
+        handler_.on_text(pbegin, p);
+        pbegin = p + 1;
+      }
+    }
+    Handler& handler_;
+  } write{handler};
+  while (begin != end) {
+    // Doing two passes with memchr (one for '{' and another for '}') is up to
+    // 2.5x faster than the naive one-pass implementation on big format strings.
+    const Char* p = begin;
+    if (*begin != '{' && !find<IS_CONSTEXPR>(begin + 1, end, Char('{'), p))
+      return write(begin, end);
+    write(begin, p);
+    begin = parse_replacement_field(p, end, handler);
+  }
+}
+
+template <typename T, typename ParseContext>
+FMT_CONSTEXPR auto parse_format_specs(ParseContext& ctx)
+    -> decltype(ctx.begin()) {
+  using char_type = typename ParseContext::char_type;
+  using context = buffer_context<char_type>;
+  using mapped_type = conditional_t<
+      mapped_type_constant<T, context>::value != type::custom_type,
+      decltype(arg_mapper<context>().map(std::declval<const T&>())), T>;
+  auto f = conditional_t<has_formatter<mapped_type, context>::value,
+                         formatter<mapped_type, char_type>,
+                         fallback_formatter<T, char_type>>();
+  return f.parse(ctx);
+}
+
+// A parse context with extra argument id checks. It is only used at compile
+// time because adding checks at runtime would introduce substantial overhead
+// and would be redundant since argument ids are checked when arguments are
+// retrieved anyway.
+template <typename Char, typename ErrorHandler = error_handler>
+class compile_parse_context
+    : public basic_format_parse_context<Char, ErrorHandler> {
+ private:
+  int num_args_;
+  using base = basic_format_parse_context<Char, ErrorHandler>;
+
+ public:
+  explicit FMT_CONSTEXPR compile_parse_context(
+      basic_string_view<Char> format_str,
+      int num_args = (std::numeric_limits<int>::max)(), ErrorHandler eh = {})
+      : base(format_str, eh), num_args_(num_args) {}
+
+  FMT_CONSTEXPR auto next_arg_id() -> int {
+    int id = base::next_arg_id();
+    if (id >= num_args_) this->on_error("argument not found");
+    return id;
+  }
+
+  FMT_CONSTEXPR void check_arg_id(int id) {
+    base::check_arg_id(id);
+    if (id >= num_args_) this->on_error("argument not found");
+  }
+  using base::check_arg_id;
+};
+
+template <typename ErrorHandler>
+FMT_CONSTEXPR void check_int_type_spec(char spec, ErrorHandler&& eh) {
+  switch (spec) {
+  case 0:
+  case 'd':
+  case 'x':
+  case 'X':
+  case 'b':
+  case 'B':
+  case 'o':
+  case 'c':
+    break;
+  default:
+    eh.on_error("invalid type specifier");
+    break;
+  }
+}
+
+// Checks char specs and returns true if the type spec is char (and not int).
+template <typename Char, typename ErrorHandler = error_handler>
+FMT_CONSTEXPR auto check_char_specs(const basic_format_specs<Char>& specs,
+                                    ErrorHandler&& eh = {}) -> bool {
+  if (specs.type && specs.type != 'c') {
+    check_int_type_spec(specs.type, eh);
+    return false;
+  }
+  if (specs.align == align::numeric || specs.sign != sign::none || specs.alt)
+    eh.on_error("invalid format specifier for char");
+  return true;
+}
+
+// A floating-point presentation format.
+enum class float_format : unsigned char {
+  general,  // General: exponent notation or fixed point based on magnitude.
+  exp,      // Exponent notation with the default precision of 6, e.g. 1.2e-3.
+  fixed,    // Fixed point with the default precision of 6, e.g. 0.0012.
+  hex
+};
+
+struct float_specs {
+  int precision;
+  float_format format : 8;
+  sign_t sign : 8;
+  bool upper : 1;
+  bool locale : 1;
+  bool binary32 : 1;
+  bool use_grisu : 1;
+  bool showpoint : 1;
+};
+
+template <typename ErrorHandler = error_handler, typename Char>
+FMT_CONSTEXPR auto parse_float_type_spec(const basic_format_specs<Char>& specs,
+                                         ErrorHandler&& eh = {})
+    -> float_specs {
+  auto result = float_specs();
+  result.showpoint = specs.alt;
+  result.locale = specs.localized;
+  switch (specs.type) {
+  case 0:
+    result.format = float_format::general;
+    break;
+  case 'G':
+    result.upper = true;
+    FMT_FALLTHROUGH;
+  case 'g':
+    result.format = float_format::general;
+    break;
+  case 'E':
+    result.upper = true;
+    FMT_FALLTHROUGH;
+  case 'e':
+    result.format = float_format::exp;
+    result.showpoint |= specs.precision != 0;
+    break;
+  case 'F':
+    result.upper = true;
+    FMT_FALLTHROUGH;
+  case 'f':
+    result.format = float_format::fixed;
+    result.showpoint |= specs.precision != 0;
+    break;
+  case 'A':
+    result.upper = true;
+    FMT_FALLTHROUGH;
+  case 'a':
+    result.format = float_format::hex;
+    break;
+  default:
+    eh.on_error("invalid type specifier");
+    break;
+  }
+  return result;
+}
+
+template <typename Char, typename ErrorHandler = error_handler>
+FMT_CONSTEXPR auto check_cstring_type_spec(Char spec, ErrorHandler&& eh = {})
+    -> bool {
+  if (spec == 0 || spec == 's') return true;
+  if (spec != 'p') eh.on_error("invalid type specifier");
+  return false;
+}
+
+template <typename Char, typename ErrorHandler = error_handler>
+FMT_CONSTEXPR void check_string_type_spec(Char spec, ErrorHandler&& eh = {}) {
+  if (spec != 0 && spec != 's') eh.on_error("invalid type specifier");
+}
+
+template <typename Char, typename ErrorHandler>
+FMT_CONSTEXPR void check_pointer_type_spec(Char spec, ErrorHandler&& eh) {
+  if (spec != 0 && spec != 'p') eh.on_error("invalid type specifier");
+}
+
+// A parse_format_specs handler that checks if specifiers are consistent with
+// the argument type.
+template <typename Handler> class specs_checker : public Handler {
+ private:
+  detail::type arg_type_;
+
+  FMT_CONSTEXPR void require_numeric_argument() {
+    if (!is_arithmetic_type(arg_type_))
+      this->on_error("format specifier requires numeric argument");
+  }
+
+ public:
+  FMT_CONSTEXPR specs_checker(const Handler& handler, detail::type arg_type)
+      : Handler(handler), arg_type_(arg_type) {}
+
+  FMT_CONSTEXPR void on_align(align_t align) {
+    if (align == align::numeric) require_numeric_argument();
+    Handler::on_align(align);
+  }
+
+  FMT_CONSTEXPR void on_sign(sign_t s) {
+    require_numeric_argument();
+    if (is_integral_type(arg_type_) && arg_type_ != type::int_type &&
+        arg_type_ != type::long_long_type && arg_type_ != type::char_type) {
+      this->on_error("format specifier requires signed argument");
+    }
+    Handler::on_sign(s);
+  }
+
+  FMT_CONSTEXPR void on_hash() {
+    require_numeric_argument();
+    Handler::on_hash();
+  }
+
+  FMT_CONSTEXPR void on_localized() {
+    require_numeric_argument();
+    Handler::on_localized();
+  }
+
+  FMT_CONSTEXPR void on_zero() {
+    require_numeric_argument();
+    Handler::on_zero();
+  }
+
+  FMT_CONSTEXPR void end_precision() {
+    if (is_integral_type(arg_type_) || arg_type_ == type::pointer_type)
+      this->on_error("precision not allowed for this argument type");
+  }
+};
+
+constexpr int invalid_arg_index = -1;
+
+#if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+template <int N, typename T, typename... Args, typename Char>
+constexpr auto get_arg_index_by_name(basic_string_view<Char> name) -> int {
+  if constexpr (detail::is_statically_named_arg<T>()) {
+    if (name == T::name) return N;
+  }
+  if constexpr (sizeof...(Args) > 0) {
+    return get_arg_index_by_name<N + 1, Args...>(name);
+  } else {
+    (void)name;  // Workaround an MSVC bug about "unused" parameter.
+    return invalid_arg_index;
+  }
+}
+#endif
+
+template <typename... Args, typename Char>
+FMT_CONSTEXPR auto get_arg_index_by_name(basic_string_view<Char> name) -> int {
+#if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+  if constexpr (sizeof...(Args) > 0) {
+    return get_arg_index_by_name<0, Args...>(name);
+  } else {
+    (void)name;
+    return invalid_arg_index;
+  }
+#else
+  (void)name;
+  return invalid_arg_index;
+#endif
+}
+
+template <typename Char, typename ErrorHandler, typename... Args>
+class format_string_checker {
+ private:
+  using parse_context_type = compile_parse_context<Char, ErrorHandler>;
+  enum { num_args = sizeof...(Args) };
+
+  // Format specifier parsing function.
+  using parse_func = const Char* (*)(parse_context_type&);
+
+  parse_context_type context_;
+  parse_func parse_funcs_[num_args > 0 ? num_args : 1];
+
+ public:
+  explicit FMT_CONSTEXPR format_string_checker(
+      basic_string_view<Char> format_str, ErrorHandler eh)
+      : context_(format_str, num_args, eh),
+        parse_funcs_{&parse_format_specs<Args, parse_context_type>...} {}
+
+  FMT_CONSTEXPR void on_text(const Char*, const Char*) {}
+
+  FMT_CONSTEXPR auto on_arg_id() -> int { return context_.next_arg_id(); }
+  FMT_CONSTEXPR auto on_arg_id(int id) -> int {
+    return context_.check_arg_id(id), id;
+  }
+  FMT_CONSTEXPR auto on_arg_id(basic_string_view<Char> id) -> int {
+#if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+    auto index = get_arg_index_by_name<Args...>(id);
+    if (index == invalid_arg_index) on_error("named argument is not found");
+    return context_.check_arg_id(index), index;
+#else
+    (void)id;
+    on_error("compile-time checks for named arguments require C++20 support");
+    return 0;
+#endif
+  }
+
+  FMT_CONSTEXPR void on_replacement_field(int, const Char*) {}
+
+  FMT_CONSTEXPR auto on_format_specs(int id, const Char* begin, const Char*)
+      -> const Char* {
+    context_.advance_to(context_.begin() + (begin - &*context_.begin()));
+    // id >= 0 check is a workaround for gcc 10 bug (#2065).
+    return id >= 0 && id < num_args ? parse_funcs_[id](context_) : begin;
+  }
+
+  FMT_CONSTEXPR void on_error(const char* message) {
+    context_.on_error(message);
+  }
+};
+
+template <typename... Args, typename S,
+          enable_if_t<(is_compile_string<S>::value), int>>
+void check_format_string(S format_str) {
+  FMT_CONSTEXPR auto s = to_string_view(format_str);
+  using checker = format_string_checker<typename S::char_type, error_handler,
+                                        remove_cvref_t<Args>...>;
+  FMT_CONSTEXPR bool invalid_format =
+      (parse_format_string<true>(s, checker(s, {})), true);
+  ignore_unused(invalid_format);
+}
+
+template <typename Char>
+void vformat_to(
+    buffer<Char>& buf, basic_string_view<Char> fmt,
+    basic_format_args<FMT_BUFFER_CONTEXT(type_identity_t<Char>)> args,
+    locale_ref loc = {});
+
+FMT_API void vprint_mojibake(std::FILE*, string_view, format_args);
+#ifndef _WIN32
+inline void vprint_mojibake(std::FILE*, string_view, format_args) {}
+#endif
+FMT_END_DETAIL_NAMESPACE
+
+// A formatter specialization for the core types corresponding to detail::type
+// constants.
+template <typename T, typename Char>
+struct formatter<T, Char,
+                 enable_if_t<detail::type_constant<T, Char>::value !=
+                             detail::type::custom_type>> {
+ private:
+  detail::dynamic_format_specs<Char> specs_;
+
+ public:
+  // Parses format specifiers stopping either at the end of the range or at the
+  // terminating '}'.
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    auto begin = ctx.begin(), end = ctx.end();
+    if (begin == end) return begin;
+    using handler_type = detail::dynamic_specs_handler<ParseContext>;
+    auto type = detail::type_constant<T, Char>::value;
+    auto checker =
+        detail::specs_checker<handler_type>(handler_type(specs_, ctx), type);
+    auto it = detail::parse_format_specs(begin, end, checker);
+    auto eh = ctx.error_handler();
+    switch (type) {
+    case detail::type::none_type:
+      FMT_ASSERT(false, "invalid argument type");
+      break;
+    case detail::type::bool_type:
+      if (!specs_.type || specs_.type == 's') break;
+      FMT_FALLTHROUGH;
+    case detail::type::int_type:
+    case detail::type::uint_type:
+    case detail::type::long_long_type:
+    case detail::type::ulong_long_type:
+    case detail::type::int128_type:
+    case detail::type::uint128_type:
+      detail::check_int_type_spec(specs_.type, eh);
+      break;
+    case detail::type::char_type:
+      detail::check_char_specs(specs_, eh);
+      break;
+    case detail::type::float_type:
+      if (detail::const_check(FMT_USE_FLOAT))
+        detail::parse_float_type_spec(specs_, eh);
+      else
+        FMT_ASSERT(false, "float support disabled");
+      break;
+    case detail::type::double_type:
+      if (detail::const_check(FMT_USE_DOUBLE))
+        detail::parse_float_type_spec(specs_, eh);
+      else
+        FMT_ASSERT(false, "double support disabled");
+      break;
+    case detail::type::long_double_type:
+      if (detail::const_check(FMT_USE_LONG_DOUBLE))
+        detail::parse_float_type_spec(specs_, eh);
+      else
+        FMT_ASSERT(false, "long double support disabled");
+      break;
+    case detail::type::cstring_type:
+      detail::check_cstring_type_spec(specs_.type, eh);
+      break;
+    case detail::type::string_type:
+      detail::check_string_type_spec(specs_.type, eh);
+      break;
+    case detail::type::pointer_type:
+      detail::check_pointer_type_spec(specs_.type, eh);
+      break;
+    case detail::type::custom_type:
+      // Custom format specifiers are checked in parse functions of
+      // formatter specializations.
+      break;
+    }
+    return it;
+  }
+
+  template <typename FormatContext>
+  FMT_CONSTEXPR auto format(const T& val, FormatContext& ctx) const
+      -> decltype(ctx.out());
+};
+
+template <typename Char> struct basic_runtime { basic_string_view<Char> str; };
+
+template <typename Char, typename... Args> class basic_format_string {
+ private:
+  basic_string_view<Char> str_;
+
+ public:
+  template <typename S,
+            FMT_ENABLE_IF(
+                std::is_convertible<const S&, basic_string_view<Char>>::value)>
+  FMT_CONSTEVAL basic_format_string(const S& s) : str_(s) {
+    static_assert(
+        detail::count<
+            (std::is_base_of<detail::view, remove_reference_t<Args>>::value &&
+             std::is_reference<Args>::value)...>() == 0,
+        "passing views as lvalues is disallowed");
+#ifdef FMT_HAS_CONSTEVAL
+    if constexpr (detail::count_named_args<Args...>() == 0) {
+      using checker = detail::format_string_checker<Char, detail::error_handler,
+                                                    remove_cvref_t<Args>...>;
+      detail::parse_format_string<true>(str_, checker(s, {}));
+    }
+#else
+    detail::check_format_string<Args...>(s);
+#endif
+  }
+  basic_format_string(basic_runtime<Char> r) : str_(r.str) {}
+
+  FMT_INLINE operator basic_string_view<Char>() const { return str_; }
+};
+
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 409
+// Workaround broken conversion on older gcc.
+template <typename... Args> using format_string = string_view;
+template <typename S> auto runtime(const S& s) -> basic_string_view<char_t<S>> {
+  return s;
+}
+#else
+template <typename... Args>
+using format_string = basic_format_string<char, type_identity_t<Args>...>;
+// Creates a runtime format string.
+template <typename S> auto runtime(const S& s) -> basic_runtime<char_t<S>> {
+  return {{s}};
+}
+#endif
+
+FMT_API auto vformat(string_view fmt, format_args args) -> std::string;
+
+/**
+  \rst
+  Formats ``args`` according to specifications in ``fmt`` and returns the result
+  as a string.
+
+  **Example**::
+
+    #include <fmt/core.h>
+    std::string message = fmt::format("The answer is {}", 42);
+  \endrst
+*/
+template <typename... T>
+FMT_INLINE auto format(format_string<T...> fmt, T&&... args) -> std::string {
+  return vformat(fmt, fmt::make_format_args(args...));
+}
+
+/** Formats a string and writes the output to ``out``. */
+template <typename OutputIt,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, char>::value)>
+auto vformat_to(OutputIt out, string_view fmt, format_args args) -> OutputIt {
+  using detail::get_buffer;
+  auto&& buf = get_buffer<char>(out);
+  detail::vformat_to(buf, string_view(fmt), args, {});
+  return detail::get_iterator(buf);
+}
+
+/**
+ \rst
+ Formats ``args`` according to specifications in ``fmt``, writes the result to
+ the output iterator ``out`` and returns the iterator past the end of the output
+ range.
+
+ **Example**::
+
+   auto out = std::vector<char>();
+   fmt::format_to(std::back_inserter(out), "{}", 42);
+ \endrst
+ */
+template <typename OutputIt, typename... T,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, char>::value)>
+FMT_INLINE auto format_to(OutputIt out, format_string<T...> fmt, T&&... args)
+    -> OutputIt {
+  return vformat_to(out, fmt, fmt::make_format_args(args...));
+}
+
+template <typename OutputIt> struct format_to_n_result {
+  /** Iterator past the end of the output range. */
+  OutputIt out;
+  /** Total (not truncated) output size. */
+  size_t size;
+};
+
+template <typename OutputIt, typename... T,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, char>::value)>
+auto vformat_to_n(OutputIt out, size_t n, string_view fmt, format_args args)
+    -> format_to_n_result<OutputIt> {
+  using buffer =
+      detail::iterator_buffer<OutputIt, char, detail::fixed_buffer_traits>;
+  auto buf = buffer(out, n);
+  detail::vformat_to(buf, fmt, args, {});
+  return {buf.out(), buf.count()};
+}
+
+/**
+  \rst
+  Formats ``args`` according to specifications in ``fmt``, writes up to ``n``
+  characters of the result to the output iterator ``out`` and returns the total
+  (not truncated) output size and the iterator past the end of the output range.
+  \endrst
+ */
+template <typename OutputIt, typename... T,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, char>::value)>
+FMT_INLINE auto format_to_n(OutputIt out, size_t n, format_string<T...> fmt,
+                            const T&... args) -> format_to_n_result<OutputIt> {
+  return vformat_to_n(out, n, fmt, fmt::make_format_args(args...));
+}
+
+/** Returns the number of chars in the output of ``format(fmt, args...)``. */
+template <typename... T>
+FMT_INLINE auto formatted_size(format_string<T...> fmt, T&&... args) -> size_t {
+  auto buf = detail::counting_buffer<>();
+  detail::vformat_to(buf, string_view(fmt), fmt::make_format_args(args...), {});
+  return buf.count();
+}
+
+FMT_API void vprint(string_view fmt, format_args args);
+FMT_API void vprint(std::FILE* f, string_view fmt, format_args args);
+
+/**
+  \rst
+  Formats ``args`` according to specifications in ``fmt`` and writes the output
+  to ``stdout``.
+
+  **Example**::
+
+    fmt::print("Elapsed time: {0:.2f} seconds", 1.23);
+  \endrst
+ */
+template <typename... T>
+FMT_INLINE void print(format_string<T...> fmt, T&&... args) {
+  const auto& vargs = fmt::make_format_args(args...);
+  return detail::is_utf8() ? vprint(fmt, vargs)
+                           : detail::vprint_mojibake(stdout, fmt, vargs);
+}
+
+/**
+  \rst
+  Formats ``args`` according to specifications in ``fmt`` and writes the
+  output to the file ``f``.
+
+  **Example**::
+
+    fmt::print(stderr, "Don't {}!", "panic");
+  \endrst
+ */
+template <typename... T>
+FMT_INLINE void print(std::FILE* f, format_string<T...> fmt, T&&... args) {
+  const auto& vargs = fmt::make_format_args(args...);
+  return detail::is_utf8() ? vprint(f, fmt, vargs)
+                           : detail::vprint_mojibake(f, fmt, vargs);
+}
+
+FMT_MODULE_EXPORT_END
+FMT_GCC_PRAGMA("GCC pop_options")
+FMT_END_NAMESPACE
+
+#ifdef FMT_HEADER_ONLY
+#  include "format.h"
+#endif
+#endif  // FMT_CORE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/format-inl.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/format-inl.h
new file mode 100644
index 0000000..94a36d1
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/format-inl.h
@@ -0,0 +1,2620 @@
+// Formatting library for C++ - implementation
+//
+// Copyright (c) 2012 - 2016, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_FORMAT_INL_H_
+#define FMT_FORMAT_INL_H_
+
+#include <algorithm>
+#include <cctype>
+#include <cerrno>  // errno
+#include <climits>
+#include <cmath>
+#include <cstdarg>
+#include <cstring>  // std::memmove
+#include <cwchar>
+#include <exception>
+
+#ifndef FMT_STATIC_THOUSANDS_SEPARATOR
+#  include <locale>
+#endif
+
+#ifdef _WIN32
+#  include <io.h>  // _isatty
+#endif
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+namespace detail {
+
+FMT_FUNC void assert_fail(const char* file, int line, const char* message) {
+  // Use unchecked std::fprintf to avoid triggering another assertion when
+  // writing to stderr fails
+  std::fprintf(stderr, "%s:%d: assertion failed: %s", file, line, message);
+  // Chosen instead of std::abort to satisfy Clang in CUDA mode during device
+  // code pass.
+  std::terminate();
+}
+
+#ifndef _MSC_VER
+#  define FMT_SNPRINTF snprintf
+#else  // _MSC_VER
+inline int fmt_snprintf(char* buffer, size_t size, const char* format, ...) {
+  va_list args;
+  va_start(args, format);
+  int result = vsnprintf_s(buffer, size, _TRUNCATE, format, args);
+  va_end(args);
+  return result;
+}
+#  define FMT_SNPRINTF fmt_snprintf
+#endif  // _MSC_VER
+
+FMT_FUNC void format_error_code(detail::buffer<char>& out, int error_code,
+                                string_view message) FMT_NOEXCEPT {
+  // Report error code making sure that the output fits into
+  // inline_buffer_size to avoid dynamic memory allocation and potential
+  // bad_alloc.
+  out.try_resize(0);
+  static const char SEP[] = ": ";
+  static const char ERROR_STR[] = "error ";
+  // Subtract 2 to account for terminating null characters in SEP and ERROR_STR.
+  size_t error_code_size = sizeof(SEP) + sizeof(ERROR_STR) - 2;
+  auto abs_value = static_cast<uint32_or_64_or_128_t<int>>(error_code);
+  if (detail::is_negative(error_code)) {
+    abs_value = 0 - abs_value;
+    ++error_code_size;
+  }
+  error_code_size += detail::to_unsigned(detail::count_digits(abs_value));
+  auto it = buffer_appender<char>(out);
+  if (message.size() <= inline_buffer_size - error_code_size)
+    format_to(it, FMT_STRING("{}{}"), message, SEP);
+  format_to(it, FMT_STRING("{}{}"), ERROR_STR, error_code);
+  FMT_ASSERT(out.size() <= inline_buffer_size, "");
+}
+
+FMT_FUNC void report_error(format_func func, int error_code,
+                           const char* message) FMT_NOEXCEPT {
+  memory_buffer full_message;
+  func(full_message, error_code, message);
+  // Don't use fwrite_fully because the latter may throw.
+  if (std::fwrite(full_message.data(), full_message.size(), 1, stderr) > 0)
+    std::fputc('\n', stderr);
+}
+
+// A wrapper around fwrite that throws on error.
+inline void fwrite_fully(const void* ptr, size_t size, size_t count,
+                         FILE* stream) {
+  size_t written = std::fwrite(ptr, size, count, stream);
+  if (written < count) FMT_THROW(system_error(errno, "cannot write to file"));
+}
+
+#ifndef FMT_STATIC_THOUSANDS_SEPARATOR
+template <typename Locale>
+locale_ref::locale_ref(const Locale& loc) : locale_(&loc) {
+  static_assert(std::is_same<Locale, std::locale>::value, "");
+}
+
+template <typename Locale> Locale locale_ref::get() const {
+  static_assert(std::is_same<Locale, std::locale>::value, "");
+  return locale_ ? *static_cast<const std::locale*>(locale_) : std::locale();
+}
+
+template <typename Char>
+FMT_FUNC auto thousands_sep_impl(locale_ref loc) -> thousands_sep_result<Char> {
+  auto& facet = std::use_facet<std::numpunct<Char>>(loc.get<std::locale>());
+  auto grouping = facet.grouping();
+  auto thousands_sep = grouping.empty() ? Char() : facet.thousands_sep();
+  return {std::move(grouping), thousands_sep};
+}
+template <typename Char> FMT_FUNC Char decimal_point_impl(locale_ref loc) {
+  return std::use_facet<std::numpunct<Char>>(loc.get<std::locale>())
+      .decimal_point();
+}
+#else
+template <typename Char>
+FMT_FUNC auto thousands_sep_impl(locale_ref) -> thousands_sep_result<Char> {
+  return {"\03", FMT_STATIC_THOUSANDS_SEPARATOR};
+}
+template <typename Char> FMT_FUNC Char decimal_point_impl(locale_ref) {
+  return '.';
+}
+#endif
+}  // namespace detail
+
+#if !FMT_MSC_VER
+FMT_API FMT_FUNC format_error::~format_error() FMT_NOEXCEPT = default;
+#endif
+
+FMT_FUNC std::system_error vsystem_error(int error_code, string_view format_str,
+                                         format_args args) {
+  auto ec = std::error_code(error_code, std::generic_category());
+  return std::system_error(ec, vformat(format_str, args));
+}
+
+namespace detail {
+
+template <> FMT_FUNC int count_digits<4>(detail::fallback_uintptr n) {
+  // fallback_uintptr is always stored in little endian.
+  int i = static_cast<int>(sizeof(void*)) - 1;
+  while (i > 0 && n.value[i] == 0) --i;
+  auto char_digits = std::numeric_limits<unsigned char>::digits / 4;
+  return i >= 0 ? i * char_digits + count_digits<4, unsigned>(n.value[i]) : 1;
+}
+
+#if __cplusplus < 201703L
+template <typename T> constexpr const char basic_data<T>::digits[][2];
+template <typename T> constexpr const char basic_data<T>::hex_digits[];
+template <typename T> constexpr const char basic_data<T>::signs[];
+template <typename T> constexpr const unsigned basic_data<T>::prefixes[];
+template <typename T> constexpr const char basic_data<T>::left_padding_shifts[];
+template <typename T>
+constexpr const char basic_data<T>::right_padding_shifts[];
+#endif
+
+template <typename T> struct bits {
+  static FMT_CONSTEXPR_DECL const int value =
+      static_cast<int>(sizeof(T) * std::numeric_limits<unsigned char>::digits);
+};
+
+class fp;
+template <int SHIFT = 0> fp normalize(fp value);
+
+// Lower (upper) boundary is a value half way between a floating-point value
+// and its predecessor (successor). Boundaries have the same exponent as the
+// value so only significands are stored.
+struct boundaries {
+  uint64_t lower;
+  uint64_t upper;
+};
+
+// A handmade floating-point number f * pow(2, e).
+class fp {
+ private:
+  using significand_type = uint64_t;
+
+  template <typename Float>
+  using is_supported_float = bool_constant<sizeof(Float) == sizeof(uint64_t) ||
+                                           sizeof(Float) == sizeof(uint32_t)>;
+
+ public:
+  significand_type f;
+  int e;
+
+  // All sizes are in bits.
+  // Subtract 1 to account for an implicit most significant bit in the
+  // normalized form.
+  static FMT_CONSTEXPR_DECL const int double_significand_size =
+      std::numeric_limits<double>::digits - 1;
+  static FMT_CONSTEXPR_DECL const uint64_t implicit_bit =
+      1ULL << double_significand_size;
+  static FMT_CONSTEXPR_DECL const int significand_size =
+      bits<significand_type>::value;
+
+  fp() : f(0), e(0) {}
+  fp(uint64_t f_val, int e_val) : f(f_val), e(e_val) {}
+
+  // Constructs fp from an IEEE754 double. It is a template to prevent compile
+  // errors on platforms where double is not IEEE754.
+  template <typename Double> explicit fp(Double d) { assign(d); }
+
+  // Assigns d to this and return true iff predecessor is closer than successor.
+  template <typename Float, FMT_ENABLE_IF(is_supported_float<Float>::value)>
+  bool assign(Float d) {
+    // Assume float is in the format [sign][exponent][significand].
+    using limits = std::numeric_limits<Float>;
+    const int float_significand_size = limits::digits - 1;
+    const int exponent_size =
+        bits<Float>::value - float_significand_size - 1;  // -1 for sign
+    const uint64_t float_implicit_bit = 1ULL << float_significand_size;
+    const uint64_t significand_mask = float_implicit_bit - 1;
+    const uint64_t exponent_mask = (~0ULL >> 1) & ~significand_mask;
+    const int exponent_bias = (1 << exponent_size) - limits::max_exponent - 1;
+    constexpr bool is_double = sizeof(Float) == sizeof(uint64_t);
+    auto u = bit_cast<conditional_t<is_double, uint64_t, uint32_t>>(d);
+    f = u & significand_mask;
+    int biased_e =
+        static_cast<int>((u & exponent_mask) >> float_significand_size);
+    // Predecessor is closer if d is a normalized power of 2 (f == 0) other than
+    // the smallest normalized number (biased_e > 1).
+    bool is_predecessor_closer = f == 0 && biased_e > 1;
+    if (biased_e != 0)
+      f += float_implicit_bit;
+    else
+      biased_e = 1;  // Subnormals use biased exponent 1 (min exponent).
+    e = biased_e - exponent_bias - float_significand_size;
+    return is_predecessor_closer;
+  }
+
+  template <typename Float, FMT_ENABLE_IF(!is_supported_float<Float>::value)>
+  bool assign(Float) {
+    *this = fp();
+    return false;
+  }
+};
+
+// Normalizes the value converted from double and multiplied by (1 << SHIFT).
+template <int SHIFT> fp normalize(fp value) {
+  // Handle subnormals.
+  const auto shifted_implicit_bit = fp::implicit_bit << SHIFT;
+  while ((value.f & shifted_implicit_bit) == 0) {
+    value.f <<= 1;
+    --value.e;
+  }
+  // Subtract 1 to account for hidden bit.
+  const auto offset =
+      fp::significand_size - fp::double_significand_size - SHIFT - 1;
+  value.f <<= offset;
+  value.e -= offset;
+  return value;
+}
+
+inline bool operator==(fp x, fp y) { return x.f == y.f && x.e == y.e; }
+
+// Computes lhs * rhs / pow(2, 64) rounded to nearest with half-up tie breaking.
+inline uint64_t multiply(uint64_t lhs, uint64_t rhs) {
+#if FMT_USE_INT128
+  auto product = static_cast<__uint128_t>(lhs) * rhs;
+  auto f = static_cast<uint64_t>(product >> 64);
+  return (static_cast<uint64_t>(product) & (1ULL << 63)) != 0 ? f + 1 : f;
+#else
+  // Multiply 32-bit parts of significands.
+  uint64_t mask = (1ULL << 32) - 1;
+  uint64_t a = lhs >> 32, b = lhs & mask;
+  uint64_t c = rhs >> 32, d = rhs & mask;
+  uint64_t ac = a * c, bc = b * c, ad = a * d, bd = b * d;
+  // Compute mid 64-bit of result and round.
+  uint64_t mid = (bd >> 32) + (ad & mask) + (bc & mask) + (1U << 31);
+  return ac + (ad >> 32) + (bc >> 32) + (mid >> 32);
+#endif
+}
+
+inline fp operator*(fp x, fp y) { return {multiply(x.f, y.f), x.e + y.e + 64}; }
+
+// Returns a cached power of 10 `c_k = c_k.f * pow(2, c_k.e)` such that its
+// (binary) exponent satisfies `min_exponent <= c_k.e <= min_exponent + 28`.
+inline fp get_cached_power(int min_exponent, int& pow10_exponent) {
+  // Normalized 64-bit significands of pow(10, k), for k = -348, -340, ..., 340.
+  // These are generated by support/compute-powers.py.
+  static constexpr const uint64_t pow10_significands[] = {
+      0xfa8fd5a0081c0288, 0xbaaee17fa23ebf76, 0x8b16fb203055ac76,
+      0xcf42894a5dce35ea, 0x9a6bb0aa55653b2d, 0xe61acf033d1a45df,
+      0xab70fe17c79ac6ca, 0xff77b1fcbebcdc4f, 0xbe5691ef416bd60c,
+      0x8dd01fad907ffc3c, 0xd3515c2831559a83, 0x9d71ac8fada6c9b5,
+      0xea9c227723ee8bcb, 0xaecc49914078536d, 0x823c12795db6ce57,
+      0xc21094364dfb5637, 0x9096ea6f3848984f, 0xd77485cb25823ac7,
+      0xa086cfcd97bf97f4, 0xef340a98172aace5, 0xb23867fb2a35b28e,
+      0x84c8d4dfd2c63f3b, 0xc5dd44271ad3cdba, 0x936b9fcebb25c996,
+      0xdbac6c247d62a584, 0xa3ab66580d5fdaf6, 0xf3e2f893dec3f126,
+      0xb5b5ada8aaff80b8, 0x87625f056c7c4a8b, 0xc9bcff6034c13053,
+      0x964e858c91ba2655, 0xdff9772470297ebd, 0xa6dfbd9fb8e5b88f,
+      0xf8a95fcf88747d94, 0xb94470938fa89bcf, 0x8a08f0f8bf0f156b,
+      0xcdb02555653131b6, 0x993fe2c6d07b7fac, 0xe45c10c42a2b3b06,
+      0xaa242499697392d3, 0xfd87b5f28300ca0e, 0xbce5086492111aeb,
+      0x8cbccc096f5088cc, 0xd1b71758e219652c, 0x9c40000000000000,
+      0xe8d4a51000000000, 0xad78ebc5ac620000, 0x813f3978f8940984,
+      0xc097ce7bc90715b3, 0x8f7e32ce7bea5c70, 0xd5d238a4abe98068,
+      0x9f4f2726179a2245, 0xed63a231d4c4fb27, 0xb0de65388cc8ada8,
+      0x83c7088e1aab65db, 0xc45d1df942711d9a, 0x924d692ca61be758,
+      0xda01ee641a708dea, 0xa26da3999aef774a, 0xf209787bb47d6b85,
+      0xb454e4a179dd1877, 0x865b86925b9bc5c2, 0xc83553c5c8965d3d,
+      0x952ab45cfa97a0b3, 0xde469fbd99a05fe3, 0xa59bc234db398c25,
+      0xf6c69a72a3989f5c, 0xb7dcbf5354e9bece, 0x88fcf317f22241e2,
+      0xcc20ce9bd35c78a5, 0x98165af37b2153df, 0xe2a0b5dc971f303a,
+      0xa8d9d1535ce3b396, 0xfb9b7cd9a4a7443c, 0xbb764c4ca7a44410,
+      0x8bab8eefb6409c1a, 0xd01fef10a657842c, 0x9b10a4e5e9913129,
+      0xe7109bfba19c0c9d, 0xac2820d9623bf429, 0x80444b5e7aa7cf85,
+      0xbf21e44003acdd2d, 0x8e679c2f5e44ff8f, 0xd433179d9c8cb841,
+      0x9e19db92b4e31ba9, 0xeb96bf6ebadf77d9, 0xaf87023b9bf0ee6b,
+  };
+
+  // Binary exponents of pow(10, k), for k = -348, -340, ..., 340, corresponding
+  // to significands above.
+  static constexpr const int16_t pow10_exponents[] = {
+      -1220, -1193, -1166, -1140, -1113, -1087, -1060, -1034, -1007, -980, -954,
+      -927,  -901,  -874,  -847,  -821,  -794,  -768,  -741,  -715,  -688, -661,
+      -635,  -608,  -582,  -555,  -529,  -502,  -475,  -449,  -422,  -396, -369,
+      -343,  -316,  -289,  -263,  -236,  -210,  -183,  -157,  -130,  -103, -77,
+      -50,   -24,   3,     30,    56,    83,    109,   136,   162,   189,  216,
+      242,   269,   295,   322,   348,   375,   402,   428,   455,   481,  508,
+      534,   561,   588,   614,   641,   667,   694,   720,   747,   774,  800,
+      827,   853,   880,   907,   933,   960,   986,   1013,  1039,  1066};
+
+  const int shift = 32;
+  const auto significand = static_cast<int64_t>(data::log10_2_significand);
+  int index = static_cast<int>(
+      ((min_exponent + fp::significand_size - 1) * (significand >> shift) +
+       ((int64_t(1) << shift) - 1))  // ceil
+      >> 32                          // arithmetic shift
+  );
+  // Decimal exponent of the first (smallest) cached power of 10.
+  const int first_dec_exp = -348;
+  // Difference between 2 consecutive decimal exponents in cached powers of 10.
+  const int dec_exp_step = 8;
+  index = (index - first_dec_exp - 1) / dec_exp_step + 1;
+  pow10_exponent = first_dec_exp + index * dec_exp_step;
+  return {pow10_significands[index], pow10_exponents[index]};
+}
+
+// A simple accumulator to hold the sums of terms in bigint::square if uint128_t
+// is not available.
+struct accumulator {
+  uint64_t lower;
+  uint64_t upper;
+
+  accumulator() : lower(0), upper(0) {}
+  explicit operator uint32_t() const { return static_cast<uint32_t>(lower); }
+
+  void operator+=(uint64_t n) {
+    lower += n;
+    if (lower < n) ++upper;
+  }
+  void operator>>=(int shift) {
+    FMT_ASSERT(shift == 32, "");
+    (void)shift;
+    lower = (upper << 32) | (lower >> 32);
+    upper >>= 32;
+  }
+};
+
+class bigint {
+ private:
+  // A bigint is stored as an array of bigits (big digits), with bigit at index
+  // 0 being the least significant one.
+  using bigit = uint32_t;
+  using double_bigit = uint64_t;
+  enum { bigits_capacity = 32 };
+  basic_memory_buffer<bigit, bigits_capacity> bigits_;
+  int exp_;
+
+  bigit operator[](int index) const { return bigits_[to_unsigned(index)]; }
+  bigit& operator[](int index) { return bigits_[to_unsigned(index)]; }
+
+  static FMT_CONSTEXPR_DECL const int bigit_bits = bits<bigit>::value;
+
+  friend struct formatter<bigint>;
+
+  void subtract_bigits(int index, bigit other, bigit& borrow) {
+    auto result = static_cast<double_bigit>((*this)[index]) - other - borrow;
+    (*this)[index] = static_cast<bigit>(result);
+    borrow = static_cast<bigit>(result >> (bigit_bits * 2 - 1));
+  }
+
+  void remove_leading_zeros() {
+    int num_bigits = static_cast<int>(bigits_.size()) - 1;
+    while (num_bigits > 0 && (*this)[num_bigits] == 0) --num_bigits;
+    bigits_.resize(to_unsigned(num_bigits + 1));
+  }
+
+  // Computes *this -= other assuming aligned bigints and *this >= other.
+  void subtract_aligned(const bigint& other) {
+    FMT_ASSERT(other.exp_ >= exp_, "unaligned bigints");
+    FMT_ASSERT(compare(*this, other) >= 0, "");
+    bigit borrow = 0;
+    int i = other.exp_ - exp_;
+    for (size_t j = 0, n = other.bigits_.size(); j != n; ++i, ++j)
+      subtract_bigits(i, other.bigits_[j], borrow);
+    while (borrow > 0) subtract_bigits(i, 0, borrow);
+    remove_leading_zeros();
+  }
+
+  void multiply(uint32_t value) {
+    const double_bigit wide_value = value;
+    bigit carry = 0;
+    for (size_t i = 0, n = bigits_.size(); i < n; ++i) {
+      double_bigit result = bigits_[i] * wide_value + carry;
+      bigits_[i] = static_cast<bigit>(result);
+      carry = static_cast<bigit>(result >> bigit_bits);
+    }
+    if (carry != 0) bigits_.push_back(carry);
+  }
+
+  void multiply(uint64_t value) {
+    const bigit mask = ~bigit(0);
+    const double_bigit lower = value & mask;
+    const double_bigit upper = value >> bigit_bits;
+    double_bigit carry = 0;
+    for (size_t i = 0, n = bigits_.size(); i < n; ++i) {
+      double_bigit result = bigits_[i] * lower + (carry & mask);
+      carry =
+          bigits_[i] * upper + (result >> bigit_bits) + (carry >> bigit_bits);
+      bigits_[i] = static_cast<bigit>(result);
+    }
+    while (carry != 0) {
+      bigits_.push_back(carry & mask);
+      carry >>= bigit_bits;
+    }
+  }
+
+ public:
+  bigint() : exp_(0) {}
+  explicit bigint(uint64_t n) { assign(n); }
+  ~bigint() { FMT_ASSERT(bigits_.capacity() <= bigits_capacity, ""); }
+
+  bigint(const bigint&) = delete;
+  void operator=(const bigint&) = delete;
+
+  void assign(const bigint& other) {
+    auto size = other.bigits_.size();
+    bigits_.resize(size);
+    auto data = other.bigits_.data();
+    std::copy(data, data + size, make_checked(bigits_.data(), size));
+    exp_ = other.exp_;
+  }
+
+  void assign(uint64_t n) {
+    size_t num_bigits = 0;
+    do {
+      bigits_[num_bigits++] = n & ~bigit(0);
+      n >>= bigit_bits;
+    } while (n != 0);
+    bigits_.resize(num_bigits);
+    exp_ = 0;
+  }
+
+  int num_bigits() const { return static_cast<int>(bigits_.size()) + exp_; }
+
+  FMT_NOINLINE bigint& operator<<=(int shift) {
+    FMT_ASSERT(shift >= 0, "");
+    exp_ += shift / bigit_bits;
+    shift %= bigit_bits;
+    if (shift == 0) return *this;
+    bigit carry = 0;
+    for (size_t i = 0, n = bigits_.size(); i < n; ++i) {
+      bigit c = bigits_[i] >> (bigit_bits - shift);
+      bigits_[i] = (bigits_[i] << shift) + carry;
+      carry = c;
+    }
+    if (carry != 0) bigits_.push_back(carry);
+    return *this;
+  }
+
+  template <typename Int> bigint& operator*=(Int value) {
+    FMT_ASSERT(value > 0, "");
+    multiply(uint32_or_64_or_128_t<Int>(value));
+    return *this;
+  }
+
+  friend int compare(const bigint& lhs, const bigint& rhs) {
+    int num_lhs_bigits = lhs.num_bigits(), num_rhs_bigits = rhs.num_bigits();
+    if (num_lhs_bigits != num_rhs_bigits)
+      return num_lhs_bigits > num_rhs_bigits ? 1 : -1;
+    int i = static_cast<int>(lhs.bigits_.size()) - 1;
+    int j = static_cast<int>(rhs.bigits_.size()) - 1;
+    int end = i - j;
+    if (end < 0) end = 0;
+    for (; i >= end; --i, --j) {
+      bigit lhs_bigit = lhs[i], rhs_bigit = rhs[j];
+      if (lhs_bigit != rhs_bigit) return lhs_bigit > rhs_bigit ? 1 : -1;
+    }
+    if (i != j) return i > j ? 1 : -1;
+    return 0;
+  }
+
+  // Returns compare(lhs1 + lhs2, rhs).
+  friend int add_compare(const bigint& lhs1, const bigint& lhs2,
+                         const bigint& rhs) {
+    int max_lhs_bigits = (std::max)(lhs1.num_bigits(), lhs2.num_bigits());
+    int num_rhs_bigits = rhs.num_bigits();
+    if (max_lhs_bigits + 1 < num_rhs_bigits) return -1;
+    if (max_lhs_bigits > num_rhs_bigits) return 1;
+    auto get_bigit = [](const bigint& n, int i) -> bigit {
+      return i >= n.exp_ && i < n.num_bigits() ? n[i - n.exp_] : 0;
+    };
+    double_bigit borrow = 0;
+    int min_exp = (std::min)((std::min)(lhs1.exp_, lhs2.exp_), rhs.exp_);
+    for (int i = num_rhs_bigits - 1; i >= min_exp; --i) {
+      double_bigit sum =
+          static_cast<double_bigit>(get_bigit(lhs1, i)) + get_bigit(lhs2, i);
+      bigit rhs_bigit = get_bigit(rhs, i);
+      if (sum > rhs_bigit + borrow) return 1;
+      borrow = rhs_bigit + borrow - sum;
+      if (borrow > 1) return -1;
+      borrow <<= bigit_bits;
+    }
+    return borrow != 0 ? -1 : 0;
+  }
+
+  // Assigns pow(10, exp) to this bigint.
+  void assign_pow10(int exp) {
+    FMT_ASSERT(exp >= 0, "");
+    if (exp == 0) return assign(1);
+    // Find the top bit.
+    int bitmask = 1;
+    while (exp >= bitmask) bitmask <<= 1;
+    bitmask >>= 1;
+    // pow(10, exp) = pow(5, exp) * pow(2, exp). First compute pow(5, exp) by
+    // repeated squaring and multiplication.
+    assign(5);
+    bitmask >>= 1;
+    while (bitmask != 0) {
+      square();
+      if ((exp & bitmask) != 0) *this *= 5;
+      bitmask >>= 1;
+    }
+    *this <<= exp;  // Multiply by pow(2, exp) by shifting.
+  }
+
+  void square() {
+    int num_bigits = static_cast<int>(bigits_.size());
+    int num_result_bigits = 2 * num_bigits;
+    basic_memory_buffer<bigit, bigits_capacity> n(std::move(bigits_));
+    bigits_.resize(to_unsigned(num_result_bigits));
+    using accumulator_t = conditional_t<FMT_USE_INT128, uint128_t, accumulator>;
+    auto sum = accumulator_t();
+    for (int bigit_index = 0; bigit_index < num_bigits; ++bigit_index) {
+      // Compute bigit at position bigit_index of the result by adding
+      // cross-product terms n[i] * n[j] such that i + j == bigit_index.
+      for (int i = 0, j = bigit_index; j >= 0; ++i, --j) {
+        // Most terms are multiplied twice which can be optimized in the future.
+        sum += static_cast<double_bigit>(n[i]) * n[j];
+      }
+      (*this)[bigit_index] = static_cast<bigit>(sum);
+      sum >>= bits<bigit>::value;  // Compute the carry.
+    }
+    // Do the same for the top half.
+    for (int bigit_index = num_bigits; bigit_index < num_result_bigits;
+         ++bigit_index) {
+      for (int j = num_bigits - 1, i = bigit_index - j; i < num_bigits;)
+        sum += static_cast<double_bigit>(n[i++]) * n[j--];
+      (*this)[bigit_index] = static_cast<bigit>(sum);
+      sum >>= bits<bigit>::value;
+    }
+    remove_leading_zeros();
+    exp_ *= 2;
+  }
+
+  // If this bigint has a bigger exponent than other, adds trailing zero to make
+  // exponents equal. This simplifies some operations such as subtraction.
+  void align(const bigint& other) {
+    int exp_difference = exp_ - other.exp_;
+    if (exp_difference <= 0) return;
+    int num_bigits = static_cast<int>(bigits_.size());
+    bigits_.resize(to_unsigned(num_bigits + exp_difference));
+    for (int i = num_bigits - 1, j = i + exp_difference; i >= 0; --i, --j)
+      bigits_[j] = bigits_[i];
+    std::uninitialized_fill_n(bigits_.data(), exp_difference, 0);
+    exp_ -= exp_difference;
+  }
+
+  // Divides this bignum by divisor, assigning the remainder to this and
+  // returning the quotient.
+  int divmod_assign(const bigint& divisor) {
+    FMT_ASSERT(this != &divisor, "");
+    if (compare(*this, divisor) < 0) return 0;
+    FMT_ASSERT(divisor.bigits_[divisor.bigits_.size() - 1u] != 0, "");
+    align(divisor);
+    int quotient = 0;
+    do {
+      subtract_aligned(divisor);
+      ++quotient;
+    } while (compare(*this, divisor) >= 0);
+    return quotient;
+  }
+};
+
+enum class round_direction { unknown, up, down };
+
+// Given the divisor (normally a power of 10), the remainder = v % divisor for
+// some number v and the error, returns whether v should be rounded up, down, or
+// whether the rounding direction can't be determined due to error.
+// error should be less than divisor / 2.
+inline round_direction get_round_direction(uint64_t divisor, uint64_t remainder,
+                                           uint64_t error) {
+  FMT_ASSERT(remainder < divisor, "");  // divisor - remainder won't overflow.
+  FMT_ASSERT(error < divisor, "");      // divisor - error won't overflow.
+  FMT_ASSERT(error < divisor - error, "");  // error * 2 won't overflow.
+  // Round down if (remainder + error) * 2 <= divisor.
+  if (remainder <= divisor - remainder && error * 2 <= divisor - remainder * 2)
+    return round_direction::down;
+  // Round up if (remainder - error) * 2 >= divisor.
+  if (remainder >= error &&
+      remainder - error >= divisor - (remainder - error)) {
+    return round_direction::up;
+  }
+  return round_direction::unknown;
+}
+
+namespace digits {
+enum result {
+  more,  // Generate more digits.
+  done,  // Done generating digits.
+  error  // Digit generation cancelled due to an error.
+};
+}
+
+inline uint64_t power_of_10_64(int exp) {
+  static constexpr const uint64_t data[] = {1, FMT_POWERS_OF_10(1),
+                                            FMT_POWERS_OF_10(1000000000ULL),
+                                            10000000000000000000ULL};
+  return data[exp];
+}
+
+// Generates output using the Grisu digit-gen algorithm.
+// error: the size of the region (lower, upper) outside of which numbers
+// definitely do not round to value (Delta in Grisu3).
+template <typename Handler>
+FMT_INLINE digits::result grisu_gen_digits(fp value, uint64_t error, int& exp,
+                                           Handler& handler) {
+  const fp one(1ULL << -value.e, value.e);
+  // The integral part of scaled value (p1 in Grisu) = value / one. It cannot be
+  // zero because it contains a product of two 64-bit numbers with MSB set (due
+  // to normalization) - 1, shifted right by at most 60 bits.
+  auto integral = static_cast<uint32_t>(value.f >> -one.e);
+  FMT_ASSERT(integral != 0, "");
+  FMT_ASSERT(integral == value.f >> -one.e, "");
+  // The fractional part of scaled value (p2 in Grisu) c = value % one.
+  uint64_t fractional = value.f & (one.f - 1);
+  exp = count_digits(integral);  // kappa in Grisu.
+  // Divide by 10 to prevent overflow.
+  auto result = handler.on_start(power_of_10_64(exp - 1) << -one.e,
+                                 value.f / 10, error * 10, exp);
+  if (result != digits::more) return result;
+  // Generate digits for the integral part. This can produce up to 10 digits.
+  do {
+    uint32_t digit = 0;
+    auto divmod_integral = [&](uint32_t divisor) {
+      digit = integral / divisor;
+      integral %= divisor;
+    };
+    // This optimization by Milo Yip reduces the number of integer divisions by
+    // one per iteration.
+    switch (exp) {
+    case 10:
+      divmod_integral(1000000000);
+      break;
+    case 9:
+      divmod_integral(100000000);
+      break;
+    case 8:
+      divmod_integral(10000000);
+      break;
+    case 7:
+      divmod_integral(1000000);
+      break;
+    case 6:
+      divmod_integral(100000);
+      break;
+    case 5:
+      divmod_integral(10000);
+      break;
+    case 4:
+      divmod_integral(1000);
+      break;
+    case 3:
+      divmod_integral(100);
+      break;
+    case 2:
+      divmod_integral(10);
+      break;
+    case 1:
+      digit = integral;
+      integral = 0;
+      break;
+    default:
+      FMT_ASSERT(false, "invalid number of digits");
+    }
+    --exp;
+    auto remainder = (static_cast<uint64_t>(integral) << -one.e) + fractional;
+    result = handler.on_digit(static_cast<char>('0' + digit),
+                              power_of_10_64(exp) << -one.e, remainder, error,
+                              exp, true);
+    if (result != digits::more) return result;
+  } while (exp > 0);
+  // Generate digits for the fractional part.
+  for (;;) {
+    fractional *= 10;
+    error *= 10;
+    char digit = static_cast<char>('0' + (fractional >> -one.e));
+    fractional &= one.f - 1;
+    --exp;
+    result = handler.on_digit(digit, one.f, fractional, error, exp, false);
+    if (result != digits::more) return result;
+  }
+}
+
+// The fixed precision digit handler.
+struct fixed_handler {
+  char* buf;
+  int size;
+  int precision;
+  int exp10;
+  bool fixed;
+
+  digits::result on_start(uint64_t divisor, uint64_t remainder, uint64_t error,
+                          int& exp) {
+    // Non-fixed formats require at least one digit and no precision adjustment.
+    if (!fixed) return digits::more;
+    // Adjust fixed precision by exponent because it is relative to decimal
+    // point.
+    precision += exp + exp10;
+    // Check if precision is satisfied just by leading zeros, e.g.
+    // format("{:.2f}", 0.001) gives "0.00" without generating any digits.
+    if (precision > 0) return digits::more;
+    if (precision < 0) return digits::done;
+    auto dir = get_round_direction(divisor, remainder, error);
+    if (dir == round_direction::unknown) return digits::error;
+    buf[size++] = dir == round_direction::up ? '1' : '0';
+    return digits::done;
+  }
+
+  digits::result on_digit(char digit, uint64_t divisor, uint64_t remainder,
+                          uint64_t error, int, bool integral) {
+    FMT_ASSERT(remainder < divisor, "");
+    buf[size++] = digit;
+    if (!integral && error >= remainder) return digits::error;
+    if (size < precision) return digits::more;
+    if (!integral) {
+      // Check if error * 2 < divisor with overflow prevention.
+      // The check is not needed for the integral part because error = 1
+      // and divisor > (1 << 32) there.
+      if (error >= divisor || error >= divisor - error) return digits::error;
+    } else {
+      FMT_ASSERT(error == 1 && divisor > 2, "");
+    }
+    auto dir = get_round_direction(divisor, remainder, error);
+    if (dir != round_direction::up)
+      return dir == round_direction::down ? digits::done : digits::error;
+    ++buf[size - 1];
+    for (int i = size - 1; i > 0 && buf[i] > '9'; --i) {
+      buf[i] = '0';
+      ++buf[i - 1];
+    }
+    if (buf[0] > '9') {
+      buf[0] = '1';
+      if (fixed)
+        buf[size++] = '0';
+      else
+        ++exp10;
+    }
+    return digits::done;
+  }
+};
+
+// A 128-bit integer type used internally,
+struct uint128_wrapper {
+  uint128_wrapper() = default;
+
+#if FMT_USE_INT128
+  uint128_t internal_;
+
+  constexpr uint128_wrapper(uint64_t high, uint64_t low) FMT_NOEXCEPT
+      : internal_{static_cast<uint128_t>(low) |
+                  (static_cast<uint128_t>(high) << 64)} {}
+
+  constexpr uint128_wrapper(uint128_t u) : internal_{u} {}
+
+  constexpr uint64_t high() const FMT_NOEXCEPT {
+    return uint64_t(internal_ >> 64);
+  }
+  constexpr uint64_t low() const FMT_NOEXCEPT { return uint64_t(internal_); }
+
+  uint128_wrapper& operator+=(uint64_t n) FMT_NOEXCEPT {
+    internal_ += n;
+    return *this;
+  }
+#else
+  uint64_t high_;
+  uint64_t low_;
+
+  constexpr uint128_wrapper(uint64_t high, uint64_t low) FMT_NOEXCEPT
+      : high_{high},
+        low_{low} {}
+
+  constexpr uint64_t high() const FMT_NOEXCEPT { return high_; }
+  constexpr uint64_t low() const FMT_NOEXCEPT { return low_; }
+
+  uint128_wrapper& operator+=(uint64_t n) FMT_NOEXCEPT {
+#  if defined(_MSC_VER) && defined(_M_X64)
+    unsigned char carry = _addcarry_u64(0, low_, n, &low_);
+    _addcarry_u64(carry, high_, 0, &high_);
+    return *this;
+#  else
+    uint64_t sum = low_ + n;
+    high_ += (sum < low_ ? 1 : 0);
+    low_ = sum;
+    return *this;
+#  endif
+  }
+#endif
+};
+
+// Implementation of Dragonbox algorithm: https://github.com/jk-jeon/dragonbox.
+namespace dragonbox {
+// Computes 128-bit result of multiplication of two 64-bit unsigned integers.
+inline uint128_wrapper umul128(uint64_t x, uint64_t y) FMT_NOEXCEPT {
+#if FMT_USE_INT128
+  return static_cast<uint128_t>(x) * static_cast<uint128_t>(y);
+#elif defined(_MSC_VER) && defined(_M_X64)
+  uint128_wrapper result;
+  result.low_ = _umul128(x, y, &result.high_);
+  return result;
+#else
+  const uint64_t mask = (uint64_t(1) << 32) - uint64_t(1);
+
+  uint64_t a = x >> 32;
+  uint64_t b = x & mask;
+  uint64_t c = y >> 32;
+  uint64_t d = y & mask;
+
+  uint64_t ac = a * c;
+  uint64_t bc = b * c;
+  uint64_t ad = a * d;
+  uint64_t bd = b * d;
+
+  uint64_t intermediate = (bd >> 32) + (ad & mask) + (bc & mask);
+
+  return {ac + (intermediate >> 32) + (ad >> 32) + (bc >> 32),
+          (intermediate << 32) + (bd & mask)};
+#endif
+}
+
+// Computes upper 64 bits of multiplication of two 64-bit unsigned integers.
+inline uint64_t umul128_upper64(uint64_t x, uint64_t y) FMT_NOEXCEPT {
+#if FMT_USE_INT128
+  auto p = static_cast<uint128_t>(x) * static_cast<uint128_t>(y);
+  return static_cast<uint64_t>(p >> 64);
+#elif defined(_MSC_VER) && defined(_M_X64)
+  return __umulh(x, y);
+#else
+  return umul128(x, y).high();
+#endif
+}
+
+// Computes upper 64 bits of multiplication of a 64-bit unsigned integer and a
+// 128-bit unsigned integer.
+inline uint64_t umul192_upper64(uint64_t x, uint128_wrapper y) FMT_NOEXCEPT {
+  uint128_wrapper g0 = umul128(x, y.high());
+  g0 += umul128_upper64(x, y.low());
+  return g0.high();
+}
+
+// Computes upper 32 bits of multiplication of a 32-bit unsigned integer and a
+// 64-bit unsigned integer.
+inline uint32_t umul96_upper32(uint32_t x, uint64_t y) FMT_NOEXCEPT {
+  return static_cast<uint32_t>(umul128_upper64(x, y));
+}
+
+// Computes middle 64 bits of multiplication of a 64-bit unsigned integer and a
+// 128-bit unsigned integer.
+inline uint64_t umul192_middle64(uint64_t x, uint128_wrapper y) FMT_NOEXCEPT {
+  uint64_t g01 = x * y.high();
+  uint64_t g10 = umul128_upper64(x, y.low());
+  return g01 + g10;
+}
+
+// Computes lower 64 bits of multiplication of a 32-bit unsigned integer and a
+// 64-bit unsigned integer.
+inline uint64_t umul96_lower64(uint32_t x, uint64_t y) FMT_NOEXCEPT {
+  return x * y;
+}
+
+// Computes floor(log10(pow(2, e))) for e in [-1700, 1700] using the method from
+// https://fmt.dev/papers/Grisu-Exact.pdf#page=5, section 3.4.
+inline int floor_log10_pow2(int e) FMT_NOEXCEPT {
+  FMT_ASSERT(e <= 1700 && e >= -1700, "too large exponent");
+  const int shift = 22;
+  return (e * static_cast<int>(data::log10_2_significand >> (64 - shift))) >>
+         shift;
+}
+
+// Various fast log computations.
+inline int floor_log2_pow10(int e) FMT_NOEXCEPT {
+  FMT_ASSERT(e <= 1233 && e >= -1233, "too large exponent");
+  const uint64_t log2_10_integer_part = 3;
+  const uint64_t log2_10_fractional_digits = 0x5269e12f346e2bf9;
+  const int shift_amount = 19;
+  return (e * static_cast<int>(
+                  (log2_10_integer_part << shift_amount) |
+                  (log2_10_fractional_digits >> (64 - shift_amount)))) >>
+         shift_amount;
+}
+inline int floor_log10_pow2_minus_log10_4_over_3(int e) FMT_NOEXCEPT {
+  FMT_ASSERT(e <= 1700 && e >= -1700, "too large exponent");
+  const uint64_t log10_4_over_3_fractional_digits = 0x1ffbfc2bbc780375;
+  const int shift_amount = 22;
+  return (e * static_cast<int>(data::log10_2_significand >>
+                               (64 - shift_amount)) -
+          static_cast<int>(log10_4_over_3_fractional_digits >>
+                           (64 - shift_amount))) >>
+         shift_amount;
+}
+
+// Returns true iff x is divisible by pow(2, exp).
+inline bool divisible_by_power_of_2(uint32_t x, int exp) FMT_NOEXCEPT {
+  FMT_ASSERT(exp >= 1, "");
+  FMT_ASSERT(x != 0, "");
+#ifdef FMT_BUILTIN_CTZ
+  return FMT_BUILTIN_CTZ(x) >= exp;
+#else
+  return exp < num_bits<uint32_t>() && x == ((x >> exp) << exp);
+#endif
+}
+inline bool divisible_by_power_of_2(uint64_t x, int exp) FMT_NOEXCEPT {
+  FMT_ASSERT(exp >= 1, "");
+  FMT_ASSERT(x != 0, "");
+#ifdef FMT_BUILTIN_CTZLL
+  return FMT_BUILTIN_CTZLL(x) >= exp;
+#else
+  return exp < num_bits<uint64_t>() && x == ((x >> exp) << exp);
+#endif
+}
+
+// Table entry type for divisibility test.
+template <typename T> struct divtest_table_entry {
+  T mod_inv;
+  T max_quotient;
+};
+
+// Returns true iff x is divisible by pow(5, exp).
+inline bool divisible_by_power_of_5(uint32_t x, int exp) FMT_NOEXCEPT {
+  FMT_ASSERT(exp <= 10, "too large exponent");
+  static constexpr const divtest_table_entry<uint32_t> divtest_table[] = {
+      {0x00000001, 0xffffffff}, {0xcccccccd, 0x33333333},
+      {0xc28f5c29, 0x0a3d70a3}, {0x26e978d5, 0x020c49ba},
+      {0x3afb7e91, 0x0068db8b}, {0x0bcbe61d, 0x0014f8b5},
+      {0x68c26139, 0x000431bd}, {0xae8d46a5, 0x0000d6bf},
+      {0x22e90e21, 0x00002af3}, {0x3a2e9c6d, 0x00000897},
+      {0x3ed61f49, 0x000001b7}};
+  return x * divtest_table[exp].mod_inv <= divtest_table[exp].max_quotient;
+}
+inline bool divisible_by_power_of_5(uint64_t x, int exp) FMT_NOEXCEPT {
+  FMT_ASSERT(exp <= 23, "too large exponent");
+  static constexpr const divtest_table_entry<uint64_t> divtest_table[] = {
+      {0x0000000000000001, 0xffffffffffffffff},
+      {0xcccccccccccccccd, 0x3333333333333333},
+      {0x8f5c28f5c28f5c29, 0x0a3d70a3d70a3d70},
+      {0x1cac083126e978d5, 0x020c49ba5e353f7c},
+      {0xd288ce703afb7e91, 0x0068db8bac710cb2},
+      {0x5d4e8fb00bcbe61d, 0x0014f8b588e368f0},
+      {0x790fb65668c26139, 0x000431bde82d7b63},
+      {0xe5032477ae8d46a5, 0x0000d6bf94d5e57a},
+      {0xc767074b22e90e21, 0x00002af31dc46118},
+      {0x8e47ce423a2e9c6d, 0x0000089705f4136b},
+      {0x4fa7f60d3ed61f49, 0x000001b7cdfd9d7b},
+      {0x0fee64690c913975, 0x00000057f5ff85e5},
+      {0x3662e0e1cf503eb1, 0x000000119799812d},
+      {0xa47a2cf9f6433fbd, 0x0000000384b84d09},
+      {0x54186f653140a659, 0x00000000b424dc35},
+      {0x7738164770402145, 0x0000000024075f3d},
+      {0xe4a4d1417cd9a041, 0x000000000734aca5},
+      {0xc75429d9e5c5200d, 0x000000000170ef54},
+      {0xc1773b91fac10669, 0x000000000049c977},
+      {0x26b172506559ce15, 0x00000000000ec1e4},
+      {0xd489e3a9addec2d1, 0x000000000002f394},
+      {0x90e860bb892c8d5d, 0x000000000000971d},
+      {0x502e79bf1b6f4f79, 0x0000000000001e39},
+      {0xdcd618596be30fe5, 0x000000000000060b}};
+  return x * divtest_table[exp].mod_inv <= divtest_table[exp].max_quotient;
+}
+
+// Replaces n by floor(n / pow(5, N)) returning true if and only if n is
+// divisible by pow(5, N).
+// Precondition: n <= 2 * pow(5, N + 1).
+template <int N>
+bool check_divisibility_and_divide_by_pow5(uint32_t& n) FMT_NOEXCEPT {
+  static constexpr struct {
+    uint32_t magic_number;
+    int bits_for_comparison;
+    uint32_t threshold;
+    int shift_amount;
+  } infos[] = {{0xcccd, 16, 0x3333, 18}, {0xa429, 8, 0x0a, 20}};
+  constexpr auto info = infos[N - 1];
+  n *= info.magic_number;
+  const uint32_t comparison_mask = (1u << info.bits_for_comparison) - 1;
+  bool result = (n & comparison_mask) <= info.threshold;
+  n >>= info.shift_amount;
+  return result;
+}
+
+// Computes floor(n / pow(10, N)) for small n and N.
+// Precondition: n <= pow(10, N + 1).
+template <int N> uint32_t small_division_by_pow10(uint32_t n) FMT_NOEXCEPT {
+  static constexpr struct {
+    uint32_t magic_number;
+    int shift_amount;
+    uint32_t divisor_times_10;
+  } infos[] = {{0xcccd, 19, 100}, {0xa3d8, 22, 1000}};
+  constexpr auto info = infos[N - 1];
+  FMT_ASSERT(n <= info.divisor_times_10, "n is too large");
+  return n * info.magic_number >> info.shift_amount;
+}
+
+// Computes floor(n / 10^(kappa + 1)) (float)
+inline uint32_t divide_by_10_to_kappa_plus_1(uint32_t n) FMT_NOEXCEPT {
+  return n / float_info<float>::big_divisor;
+}
+// Computes floor(n / 10^(kappa + 1)) (double)
+inline uint64_t divide_by_10_to_kappa_plus_1(uint64_t n) FMT_NOEXCEPT {
+  return umul128_upper64(n, 0x83126e978d4fdf3c) >> 9;
+}
+
+// Various subroutines using pow10 cache
+template <class T> struct cache_accessor;
+
+template <> struct cache_accessor<float> {
+  using carrier_uint = float_info<float>::carrier_uint;
+  using cache_entry_type = uint64_t;
+
+  static uint64_t get_cached_power(int k) FMT_NOEXCEPT {
+    FMT_ASSERT(k >= float_info<float>::min_k && k <= float_info<float>::max_k,
+               "k is out of range");
+    constexpr const uint64_t pow10_significands[] = {
+        0x81ceb32c4b43fcf5, 0xa2425ff75e14fc32, 0xcad2f7f5359a3b3f,
+        0xfd87b5f28300ca0e, 0x9e74d1b791e07e49, 0xc612062576589ddb,
+        0xf79687aed3eec552, 0x9abe14cd44753b53, 0xc16d9a0095928a28,
+        0xf1c90080baf72cb2, 0x971da05074da7bef, 0xbce5086492111aeb,
+        0xec1e4a7db69561a6, 0x9392ee8e921d5d08, 0xb877aa3236a4b44a,
+        0xe69594bec44de15c, 0x901d7cf73ab0acda, 0xb424dc35095cd810,
+        0xe12e13424bb40e14, 0x8cbccc096f5088cc, 0xafebff0bcb24aaff,
+        0xdbe6fecebdedd5bf, 0x89705f4136b4a598, 0xabcc77118461cefd,
+        0xd6bf94d5e57a42bd, 0x8637bd05af6c69b6, 0xa7c5ac471b478424,
+        0xd1b71758e219652c, 0x83126e978d4fdf3c, 0xa3d70a3d70a3d70b,
+        0xcccccccccccccccd, 0x8000000000000000, 0xa000000000000000,
+        0xc800000000000000, 0xfa00000000000000, 0x9c40000000000000,
+        0xc350000000000000, 0xf424000000000000, 0x9896800000000000,
+        0xbebc200000000000, 0xee6b280000000000, 0x9502f90000000000,
+        0xba43b74000000000, 0xe8d4a51000000000, 0x9184e72a00000000,
+        0xb5e620f480000000, 0xe35fa931a0000000, 0x8e1bc9bf04000000,
+        0xb1a2bc2ec5000000, 0xde0b6b3a76400000, 0x8ac7230489e80000,
+        0xad78ebc5ac620000, 0xd8d726b7177a8000, 0x878678326eac9000,
+        0xa968163f0a57b400, 0xd3c21bcecceda100, 0x84595161401484a0,
+        0xa56fa5b99019a5c8, 0xcecb8f27f4200f3a, 0x813f3978f8940984,
+        0xa18f07d736b90be5, 0xc9f2c9cd04674ede, 0xfc6f7c4045812296,
+        0x9dc5ada82b70b59d, 0xc5371912364ce305, 0xf684df56c3e01bc6,
+        0x9a130b963a6c115c, 0xc097ce7bc90715b3, 0xf0bdc21abb48db20,
+        0x96769950b50d88f4, 0xbc143fa4e250eb31, 0xeb194f8e1ae525fd,
+        0x92efd1b8d0cf37be, 0xb7abc627050305ad, 0xe596b7b0c643c719,
+        0x8f7e32ce7bea5c6f, 0xb35dbf821ae4f38b, 0xe0352f62a19e306e};
+    return pow10_significands[k - float_info<float>::min_k];
+  }
+
+  static carrier_uint compute_mul(carrier_uint u,
+                                  const cache_entry_type& cache) FMT_NOEXCEPT {
+    return umul96_upper32(u, cache);
+  }
+
+  static uint32_t compute_delta(const cache_entry_type& cache,
+                                int beta_minus_1) FMT_NOEXCEPT {
+    return static_cast<uint32_t>(cache >> (64 - 1 - beta_minus_1));
+  }
+
+  static bool compute_mul_parity(carrier_uint two_f,
+                                 const cache_entry_type& cache,
+                                 int beta_minus_1) FMT_NOEXCEPT {
+    FMT_ASSERT(beta_minus_1 >= 1, "");
+    FMT_ASSERT(beta_minus_1 < 64, "");
+
+    return ((umul96_lower64(two_f, cache) >> (64 - beta_minus_1)) & 1) != 0;
+  }
+
+  static carrier_uint compute_left_endpoint_for_shorter_interval_case(
+      const cache_entry_type& cache, int beta_minus_1) FMT_NOEXCEPT {
+    return static_cast<carrier_uint>(
+        (cache - (cache >> (float_info<float>::significand_bits + 2))) >>
+        (64 - float_info<float>::significand_bits - 1 - beta_minus_1));
+  }
+
+  static carrier_uint compute_right_endpoint_for_shorter_interval_case(
+      const cache_entry_type& cache, int beta_minus_1) FMT_NOEXCEPT {
+    return static_cast<carrier_uint>(
+        (cache + (cache >> (float_info<float>::significand_bits + 1))) >>
+        (64 - float_info<float>::significand_bits - 1 - beta_minus_1));
+  }
+
+  static carrier_uint compute_round_up_for_shorter_interval_case(
+      const cache_entry_type& cache, int beta_minus_1) FMT_NOEXCEPT {
+    return (static_cast<carrier_uint>(
+                cache >>
+                (64 - float_info<float>::significand_bits - 2 - beta_minus_1)) +
+            1) /
+           2;
+  }
+};
+
+template <> struct cache_accessor<double> {
+  using carrier_uint = float_info<double>::carrier_uint;
+  using cache_entry_type = uint128_wrapper;
+
+  static uint128_wrapper get_cached_power(int k) FMT_NOEXCEPT {
+    FMT_ASSERT(k >= float_info<double>::min_k && k <= float_info<double>::max_k,
+               "k is out of range");
+
+    static constexpr const uint128_wrapper pow10_significands[] = {
+#if FMT_USE_FULL_CACHE_DRAGONBOX
+      {0xff77b1fcbebcdc4f, 0x25e8e89c13bb0f7b},
+      {0x9faacf3df73609b1, 0x77b191618c54e9ad},
+      {0xc795830d75038c1d, 0xd59df5b9ef6a2418},
+      {0xf97ae3d0d2446f25, 0x4b0573286b44ad1e},
+      {0x9becce62836ac577, 0x4ee367f9430aec33},
+      {0xc2e801fb244576d5, 0x229c41f793cda740},
+      {0xf3a20279ed56d48a, 0x6b43527578c11110},
+      {0x9845418c345644d6, 0x830a13896b78aaaa},
+      {0xbe5691ef416bd60c, 0x23cc986bc656d554},
+      {0xedec366b11c6cb8f, 0x2cbfbe86b7ec8aa9},
+      {0x94b3a202eb1c3f39, 0x7bf7d71432f3d6aa},
+      {0xb9e08a83a5e34f07, 0xdaf5ccd93fb0cc54},
+      {0xe858ad248f5c22c9, 0xd1b3400f8f9cff69},
+      {0x91376c36d99995be, 0x23100809b9c21fa2},
+      {0xb58547448ffffb2d, 0xabd40a0c2832a78b},
+      {0xe2e69915b3fff9f9, 0x16c90c8f323f516d},
+      {0x8dd01fad907ffc3b, 0xae3da7d97f6792e4},
+      {0xb1442798f49ffb4a, 0x99cd11cfdf41779d},
+      {0xdd95317f31c7fa1d, 0x40405643d711d584},
+      {0x8a7d3eef7f1cfc52, 0x482835ea666b2573},
+      {0xad1c8eab5ee43b66, 0xda3243650005eed0},
+      {0xd863b256369d4a40, 0x90bed43e40076a83},
+      {0x873e4f75e2224e68, 0x5a7744a6e804a292},
+      {0xa90de3535aaae202, 0x711515d0a205cb37},
+      {0xd3515c2831559a83, 0x0d5a5b44ca873e04},
+      {0x8412d9991ed58091, 0xe858790afe9486c3},
+      {0xa5178fff668ae0b6, 0x626e974dbe39a873},
+      {0xce5d73ff402d98e3, 0xfb0a3d212dc81290},
+      {0x80fa687f881c7f8e, 0x7ce66634bc9d0b9a},
+      {0xa139029f6a239f72, 0x1c1fffc1ebc44e81},
+      {0xc987434744ac874e, 0xa327ffb266b56221},
+      {0xfbe9141915d7a922, 0x4bf1ff9f0062baa9},
+      {0x9d71ac8fada6c9b5, 0x6f773fc3603db4aa},
+      {0xc4ce17b399107c22, 0xcb550fb4384d21d4},
+      {0xf6019da07f549b2b, 0x7e2a53a146606a49},
+      {0x99c102844f94e0fb, 0x2eda7444cbfc426e},
+      {0xc0314325637a1939, 0xfa911155fefb5309},
+      {0xf03d93eebc589f88, 0x793555ab7eba27cb},
+      {0x96267c7535b763b5, 0x4bc1558b2f3458df},
+      {0xbbb01b9283253ca2, 0x9eb1aaedfb016f17},
+      {0xea9c227723ee8bcb, 0x465e15a979c1cadd},
+      {0x92a1958a7675175f, 0x0bfacd89ec191eca},
+      {0xb749faed14125d36, 0xcef980ec671f667c},
+      {0xe51c79a85916f484, 0x82b7e12780e7401b},
+      {0x8f31cc0937ae58d2, 0xd1b2ecb8b0908811},
+      {0xb2fe3f0b8599ef07, 0x861fa7e6dcb4aa16},
+      {0xdfbdcece67006ac9, 0x67a791e093e1d49b},
+      {0x8bd6a141006042bd, 0xe0c8bb2c5c6d24e1},
+      {0xaecc49914078536d, 0x58fae9f773886e19},
+      {0xda7f5bf590966848, 0xaf39a475506a899f},
+      {0x888f99797a5e012d, 0x6d8406c952429604},
+      {0xaab37fd7d8f58178, 0xc8e5087ba6d33b84},
+      {0xd5605fcdcf32e1d6, 0xfb1e4a9a90880a65},
+      {0x855c3be0a17fcd26, 0x5cf2eea09a550680},
+      {0xa6b34ad8c9dfc06f, 0xf42faa48c0ea481f},
+      {0xd0601d8efc57b08b, 0xf13b94daf124da27},
+      {0x823c12795db6ce57, 0x76c53d08d6b70859},
+      {0xa2cb1717b52481ed, 0x54768c4b0c64ca6f},
+      {0xcb7ddcdda26da268, 0xa9942f5dcf7dfd0a},
+      {0xfe5d54150b090b02, 0xd3f93b35435d7c4d},
+      {0x9efa548d26e5a6e1, 0xc47bc5014a1a6db0},
+      {0xc6b8e9b0709f109a, 0x359ab6419ca1091c},
+      {0xf867241c8cc6d4c0, 0xc30163d203c94b63},
+      {0x9b407691d7fc44f8, 0x79e0de63425dcf1e},
+      {0xc21094364dfb5636, 0x985915fc12f542e5},
+      {0xf294b943e17a2bc4, 0x3e6f5b7b17b2939e},
+      {0x979cf3ca6cec5b5a, 0xa705992ceecf9c43},
+      {0xbd8430bd08277231, 0x50c6ff782a838354},
+      {0xece53cec4a314ebd, 0xa4f8bf5635246429},
+      {0x940f4613ae5ed136, 0x871b7795e136be9a},
+      {0xb913179899f68584, 0x28e2557b59846e40},
+      {0xe757dd7ec07426e5, 0x331aeada2fe589d0},
+      {0x9096ea6f3848984f, 0x3ff0d2c85def7622},
+      {0xb4bca50b065abe63, 0x0fed077a756b53aa},
+      {0xe1ebce4dc7f16dfb, 0xd3e8495912c62895},
+      {0x8d3360f09cf6e4bd, 0x64712dd7abbbd95d},
+      {0xb080392cc4349dec, 0xbd8d794d96aacfb4},
+      {0xdca04777f541c567, 0xecf0d7a0fc5583a1},
+      {0x89e42caaf9491b60, 0xf41686c49db57245},
+      {0xac5d37d5b79b6239, 0x311c2875c522ced6},
+      {0xd77485cb25823ac7, 0x7d633293366b828c},
+      {0x86a8d39ef77164bc, 0xae5dff9c02033198},
+      {0xa8530886b54dbdeb, 0xd9f57f830283fdfd},
+      {0xd267caa862a12d66, 0xd072df63c324fd7c},
+      {0x8380dea93da4bc60, 0x4247cb9e59f71e6e},
+      {0xa46116538d0deb78, 0x52d9be85f074e609},
+      {0xcd795be870516656, 0x67902e276c921f8c},
+      {0x806bd9714632dff6, 0x00ba1cd8a3db53b7},
+      {0xa086cfcd97bf97f3, 0x80e8a40eccd228a5},
+      {0xc8a883c0fdaf7df0, 0x6122cd128006b2ce},
+      {0xfad2a4b13d1b5d6c, 0x796b805720085f82},
+      {0x9cc3a6eec6311a63, 0xcbe3303674053bb1},
+      {0xc3f490aa77bd60fc, 0xbedbfc4411068a9d},
+      {0xf4f1b4d515acb93b, 0xee92fb5515482d45},
+      {0x991711052d8bf3c5, 0x751bdd152d4d1c4b},
+      {0xbf5cd54678eef0b6, 0xd262d45a78a0635e},
+      {0xef340a98172aace4, 0x86fb897116c87c35},
+      {0x9580869f0e7aac0e, 0xd45d35e6ae3d4da1},
+      {0xbae0a846d2195712, 0x8974836059cca10a},
+      {0xe998d258869facd7, 0x2bd1a438703fc94c},
+      {0x91ff83775423cc06, 0x7b6306a34627ddd0},
+      {0xb67f6455292cbf08, 0x1a3bc84c17b1d543},
+      {0xe41f3d6a7377eeca, 0x20caba5f1d9e4a94},
+      {0x8e938662882af53e, 0x547eb47b7282ee9d},
+      {0xb23867fb2a35b28d, 0xe99e619a4f23aa44},
+      {0xdec681f9f4c31f31, 0x6405fa00e2ec94d5},
+      {0x8b3c113c38f9f37e, 0xde83bc408dd3dd05},
+      {0xae0b158b4738705e, 0x9624ab50b148d446},
+      {0xd98ddaee19068c76, 0x3badd624dd9b0958},
+      {0x87f8a8d4cfa417c9, 0xe54ca5d70a80e5d7},
+      {0xa9f6d30a038d1dbc, 0x5e9fcf4ccd211f4d},
+      {0xd47487cc8470652b, 0x7647c32000696720},
+      {0x84c8d4dfd2c63f3b, 0x29ecd9f40041e074},
+      {0xa5fb0a17c777cf09, 0xf468107100525891},
+      {0xcf79cc9db955c2cc, 0x7182148d4066eeb5},
+      {0x81ac1fe293d599bf, 0xc6f14cd848405531},
+      {0xa21727db38cb002f, 0xb8ada00e5a506a7d},
+      {0xca9cf1d206fdc03b, 0xa6d90811f0e4851d},
+      {0xfd442e4688bd304a, 0x908f4a166d1da664},
+      {0x9e4a9cec15763e2e, 0x9a598e4e043287ff},
+      {0xc5dd44271ad3cdba, 0x40eff1e1853f29fe},
+      {0xf7549530e188c128, 0xd12bee59e68ef47d},
+      {0x9a94dd3e8cf578b9, 0x82bb74f8301958cf},
+      {0xc13a148e3032d6e7, 0xe36a52363c1faf02},
+      {0xf18899b1bc3f8ca1, 0xdc44e6c3cb279ac2},
+      {0x96f5600f15a7b7e5, 0x29ab103a5ef8c0ba},
+      {0xbcb2b812db11a5de, 0x7415d448f6b6f0e8},
+      {0xebdf661791d60f56, 0x111b495b3464ad22},
+      {0x936b9fcebb25c995, 0xcab10dd900beec35},
+      {0xb84687c269ef3bfb, 0x3d5d514f40eea743},
+      {0xe65829b3046b0afa, 0x0cb4a5a3112a5113},
+      {0x8ff71a0fe2c2e6dc, 0x47f0e785eaba72ac},
+      {0xb3f4e093db73a093, 0x59ed216765690f57},
+      {0xe0f218b8d25088b8, 0x306869c13ec3532d},
+      {0x8c974f7383725573, 0x1e414218c73a13fc},
+      {0xafbd2350644eeacf, 0xe5d1929ef90898fb},
+      {0xdbac6c247d62a583, 0xdf45f746b74abf3a},
+      {0x894bc396ce5da772, 0x6b8bba8c328eb784},
+      {0xab9eb47c81f5114f, 0x066ea92f3f326565},
+      {0xd686619ba27255a2, 0xc80a537b0efefebe},
+      {0x8613fd0145877585, 0xbd06742ce95f5f37},
+      {0xa798fc4196e952e7, 0x2c48113823b73705},
+      {0xd17f3b51fca3a7a0, 0xf75a15862ca504c6},
+      {0x82ef85133de648c4, 0x9a984d73dbe722fc},
+      {0xa3ab66580d5fdaf5, 0xc13e60d0d2e0ebbb},
+      {0xcc963fee10b7d1b3, 0x318df905079926a9},
+      {0xffbbcfe994e5c61f, 0xfdf17746497f7053},
+      {0x9fd561f1fd0f9bd3, 0xfeb6ea8bedefa634},
+      {0xc7caba6e7c5382c8, 0xfe64a52ee96b8fc1},
+      {0xf9bd690a1b68637b, 0x3dfdce7aa3c673b1},
+      {0x9c1661a651213e2d, 0x06bea10ca65c084f},
+      {0xc31bfa0fe5698db8, 0x486e494fcff30a63},
+      {0xf3e2f893dec3f126, 0x5a89dba3c3efccfb},
+      {0x986ddb5c6b3a76b7, 0xf89629465a75e01d},
+      {0xbe89523386091465, 0xf6bbb397f1135824},
+      {0xee2ba6c0678b597f, 0x746aa07ded582e2d},
+      {0x94db483840b717ef, 0xa8c2a44eb4571cdd},
+      {0xba121a4650e4ddeb, 0x92f34d62616ce414},
+      {0xe896a0d7e51e1566, 0x77b020baf9c81d18},
+      {0x915e2486ef32cd60, 0x0ace1474dc1d122f},
+      {0xb5b5ada8aaff80b8, 0x0d819992132456bb},
+      {0xe3231912d5bf60e6, 0x10e1fff697ed6c6a},
+      {0x8df5efabc5979c8f, 0xca8d3ffa1ef463c2},
+      {0xb1736b96b6fd83b3, 0xbd308ff8a6b17cb3},
+      {0xddd0467c64bce4a0, 0xac7cb3f6d05ddbdf},
+      {0x8aa22c0dbef60ee4, 0x6bcdf07a423aa96c},
+      {0xad4ab7112eb3929d, 0x86c16c98d2c953c7},
+      {0xd89d64d57a607744, 0xe871c7bf077ba8b8},
+      {0x87625f056c7c4a8b, 0x11471cd764ad4973},
+      {0xa93af6c6c79b5d2d, 0xd598e40d3dd89bd0},
+      {0xd389b47879823479, 0x4aff1d108d4ec2c4},
+      {0x843610cb4bf160cb, 0xcedf722a585139bb},
+      {0xa54394fe1eedb8fe, 0xc2974eb4ee658829},
+      {0xce947a3da6a9273e, 0x733d226229feea33},
+      {0x811ccc668829b887, 0x0806357d5a3f5260},
+      {0xa163ff802a3426a8, 0xca07c2dcb0cf26f8},
+      {0xc9bcff6034c13052, 0xfc89b393dd02f0b6},
+      {0xfc2c3f3841f17c67, 0xbbac2078d443ace3},
+      {0x9d9ba7832936edc0, 0xd54b944b84aa4c0e},
+      {0xc5029163f384a931, 0x0a9e795e65d4df12},
+      {0xf64335bcf065d37d, 0x4d4617b5ff4a16d6},
+      {0x99ea0196163fa42e, 0x504bced1bf8e4e46},
+      {0xc06481fb9bcf8d39, 0xe45ec2862f71e1d7},
+      {0xf07da27a82c37088, 0x5d767327bb4e5a4d},
+      {0x964e858c91ba2655, 0x3a6a07f8d510f870},
+      {0xbbe226efb628afea, 0x890489f70a55368c},
+      {0xeadab0aba3b2dbe5, 0x2b45ac74ccea842f},
+      {0x92c8ae6b464fc96f, 0x3b0b8bc90012929e},
+      {0xb77ada0617e3bbcb, 0x09ce6ebb40173745},
+      {0xe55990879ddcaabd, 0xcc420a6a101d0516},
+      {0x8f57fa54c2a9eab6, 0x9fa946824a12232e},
+      {0xb32df8e9f3546564, 0x47939822dc96abfa},
+      {0xdff9772470297ebd, 0x59787e2b93bc56f8},
+      {0x8bfbea76c619ef36, 0x57eb4edb3c55b65b},
+      {0xaefae51477a06b03, 0xede622920b6b23f2},
+      {0xdab99e59958885c4, 0xe95fab368e45ecee},
+      {0x88b402f7fd75539b, 0x11dbcb0218ebb415},
+      {0xaae103b5fcd2a881, 0xd652bdc29f26a11a},
+      {0xd59944a37c0752a2, 0x4be76d3346f04960},
+      {0x857fcae62d8493a5, 0x6f70a4400c562ddc},
+      {0xa6dfbd9fb8e5b88e, 0xcb4ccd500f6bb953},
+      {0xd097ad07a71f26b2, 0x7e2000a41346a7a8},
+      {0x825ecc24c873782f, 0x8ed400668c0c28c9},
+      {0xa2f67f2dfa90563b, 0x728900802f0f32fb},
+      {0xcbb41ef979346bca, 0x4f2b40a03ad2ffba},
+      {0xfea126b7d78186bc, 0xe2f610c84987bfa9},
+      {0x9f24b832e6b0f436, 0x0dd9ca7d2df4d7ca},
+      {0xc6ede63fa05d3143, 0x91503d1c79720dbc},
+      {0xf8a95fcf88747d94, 0x75a44c6397ce912b},
+      {0x9b69dbe1b548ce7c, 0xc986afbe3ee11abb},
+      {0xc24452da229b021b, 0xfbe85badce996169},
+      {0xf2d56790ab41c2a2, 0xfae27299423fb9c4},
+      {0x97c560ba6b0919a5, 0xdccd879fc967d41b},
+      {0xbdb6b8e905cb600f, 0x5400e987bbc1c921},
+      {0xed246723473e3813, 0x290123e9aab23b69},
+      {0x9436c0760c86e30b, 0xf9a0b6720aaf6522},
+      {0xb94470938fa89bce, 0xf808e40e8d5b3e6a},
+      {0xe7958cb87392c2c2, 0xb60b1d1230b20e05},
+      {0x90bd77f3483bb9b9, 0xb1c6f22b5e6f48c3},
+      {0xb4ecd5f01a4aa828, 0x1e38aeb6360b1af4},
+      {0xe2280b6c20dd5232, 0x25c6da63c38de1b1},
+      {0x8d590723948a535f, 0x579c487e5a38ad0f},
+      {0xb0af48ec79ace837, 0x2d835a9df0c6d852},
+      {0xdcdb1b2798182244, 0xf8e431456cf88e66},
+      {0x8a08f0f8bf0f156b, 0x1b8e9ecb641b5900},
+      {0xac8b2d36eed2dac5, 0xe272467e3d222f40},
+      {0xd7adf884aa879177, 0x5b0ed81dcc6abb10},
+      {0x86ccbb52ea94baea, 0x98e947129fc2b4ea},
+      {0xa87fea27a539e9a5, 0x3f2398d747b36225},
+      {0xd29fe4b18e88640e, 0x8eec7f0d19a03aae},
+      {0x83a3eeeef9153e89, 0x1953cf68300424ad},
+      {0xa48ceaaab75a8e2b, 0x5fa8c3423c052dd8},
+      {0xcdb02555653131b6, 0x3792f412cb06794e},
+      {0x808e17555f3ebf11, 0xe2bbd88bbee40bd1},
+      {0xa0b19d2ab70e6ed6, 0x5b6aceaeae9d0ec5},
+      {0xc8de047564d20a8b, 0xf245825a5a445276},
+      {0xfb158592be068d2e, 0xeed6e2f0f0d56713},
+      {0x9ced737bb6c4183d, 0x55464dd69685606c},
+      {0xc428d05aa4751e4c, 0xaa97e14c3c26b887},
+      {0xf53304714d9265df, 0xd53dd99f4b3066a9},
+      {0x993fe2c6d07b7fab, 0xe546a8038efe402a},
+      {0xbf8fdb78849a5f96, 0xde98520472bdd034},
+      {0xef73d256a5c0f77c, 0x963e66858f6d4441},
+      {0x95a8637627989aad, 0xdde7001379a44aa9},
+      {0xbb127c53b17ec159, 0x5560c018580d5d53},
+      {0xe9d71b689dde71af, 0xaab8f01e6e10b4a7},
+      {0x9226712162ab070d, 0xcab3961304ca70e9},
+      {0xb6b00d69bb55c8d1, 0x3d607b97c5fd0d23},
+      {0xe45c10c42a2b3b05, 0x8cb89a7db77c506b},
+      {0x8eb98a7a9a5b04e3, 0x77f3608e92adb243},
+      {0xb267ed1940f1c61c, 0x55f038b237591ed4},
+      {0xdf01e85f912e37a3, 0x6b6c46dec52f6689},
+      {0x8b61313bbabce2c6, 0x2323ac4b3b3da016},
+      {0xae397d8aa96c1b77, 0xabec975e0a0d081b},
+      {0xd9c7dced53c72255, 0x96e7bd358c904a22},
+      {0x881cea14545c7575, 0x7e50d64177da2e55},
+      {0xaa242499697392d2, 0xdde50bd1d5d0b9ea},
+      {0xd4ad2dbfc3d07787, 0x955e4ec64b44e865},
+      {0x84ec3c97da624ab4, 0xbd5af13bef0b113f},
+      {0xa6274bbdd0fadd61, 0xecb1ad8aeacdd58f},
+      {0xcfb11ead453994ba, 0x67de18eda5814af3},
+      {0x81ceb32c4b43fcf4, 0x80eacf948770ced8},
+      {0xa2425ff75e14fc31, 0xa1258379a94d028e},
+      {0xcad2f7f5359a3b3e, 0x096ee45813a04331},
+      {0xfd87b5f28300ca0d, 0x8bca9d6e188853fd},
+      {0x9e74d1b791e07e48, 0x775ea264cf55347e},
+      {0xc612062576589dda, 0x95364afe032a819e},
+      {0xf79687aed3eec551, 0x3a83ddbd83f52205},
+      {0x9abe14cd44753b52, 0xc4926a9672793543},
+      {0xc16d9a0095928a27, 0x75b7053c0f178294},
+      {0xf1c90080baf72cb1, 0x5324c68b12dd6339},
+      {0x971da05074da7bee, 0xd3f6fc16ebca5e04},
+      {0xbce5086492111aea, 0x88f4bb1ca6bcf585},
+      {0xec1e4a7db69561a5, 0x2b31e9e3d06c32e6},
+      {0x9392ee8e921d5d07, 0x3aff322e62439fd0},
+      {0xb877aa3236a4b449, 0x09befeb9fad487c3},
+      {0xe69594bec44de15b, 0x4c2ebe687989a9b4},
+      {0x901d7cf73ab0acd9, 0x0f9d37014bf60a11},
+      {0xb424dc35095cd80f, 0x538484c19ef38c95},
+      {0xe12e13424bb40e13, 0x2865a5f206b06fba},
+      {0x8cbccc096f5088cb, 0xf93f87b7442e45d4},
+      {0xafebff0bcb24aafe, 0xf78f69a51539d749},
+      {0xdbe6fecebdedd5be, 0xb573440e5a884d1c},
+      {0x89705f4136b4a597, 0x31680a88f8953031},
+      {0xabcc77118461cefc, 0xfdc20d2b36ba7c3e},
+      {0xd6bf94d5e57a42bc, 0x3d32907604691b4d},
+      {0x8637bd05af6c69b5, 0xa63f9a49c2c1b110},
+      {0xa7c5ac471b478423, 0x0fcf80dc33721d54},
+      {0xd1b71758e219652b, 0xd3c36113404ea4a9},
+      {0x83126e978d4fdf3b, 0x645a1cac083126ea},
+      {0xa3d70a3d70a3d70a, 0x3d70a3d70a3d70a4},
+      {0xcccccccccccccccc, 0xcccccccccccccccd},
+      {0x8000000000000000, 0x0000000000000000},
+      {0xa000000000000000, 0x0000000000000000},
+      {0xc800000000000000, 0x0000000000000000},
+      {0xfa00000000000000, 0x0000000000000000},
+      {0x9c40000000000000, 0x0000000000000000},
+      {0xc350000000000000, 0x0000000000000000},
+      {0xf424000000000000, 0x0000000000000000},
+      {0x9896800000000000, 0x0000000000000000},
+      {0xbebc200000000000, 0x0000000000000000},
+      {0xee6b280000000000, 0x0000000000000000},
+      {0x9502f90000000000, 0x0000000000000000},
+      {0xba43b74000000000, 0x0000000000000000},
+      {0xe8d4a51000000000, 0x0000000000000000},
+      {0x9184e72a00000000, 0x0000000000000000},
+      {0xb5e620f480000000, 0x0000000000000000},
+      {0xe35fa931a0000000, 0x0000000000000000},
+      {0x8e1bc9bf04000000, 0x0000000000000000},
+      {0xb1a2bc2ec5000000, 0x0000000000000000},
+      {0xde0b6b3a76400000, 0x0000000000000000},
+      {0x8ac7230489e80000, 0x0000000000000000},
+      {0xad78ebc5ac620000, 0x0000000000000000},
+      {0xd8d726b7177a8000, 0x0000000000000000},
+      {0x878678326eac9000, 0x0000000000000000},
+      {0xa968163f0a57b400, 0x0000000000000000},
+      {0xd3c21bcecceda100, 0x0000000000000000},
+      {0x84595161401484a0, 0x0000000000000000},
+      {0xa56fa5b99019a5c8, 0x0000000000000000},
+      {0xcecb8f27f4200f3a, 0x0000000000000000},
+      {0x813f3978f8940984, 0x4000000000000000},
+      {0xa18f07d736b90be5, 0x5000000000000000},
+      {0xc9f2c9cd04674ede, 0xa400000000000000},
+      {0xfc6f7c4045812296, 0x4d00000000000000},
+      {0x9dc5ada82b70b59d, 0xf020000000000000},
+      {0xc5371912364ce305, 0x6c28000000000000},
+      {0xf684df56c3e01bc6, 0xc732000000000000},
+      {0x9a130b963a6c115c, 0x3c7f400000000000},
+      {0xc097ce7bc90715b3, 0x4b9f100000000000},
+      {0xf0bdc21abb48db20, 0x1e86d40000000000},
+      {0x96769950b50d88f4, 0x1314448000000000},
+      {0xbc143fa4e250eb31, 0x17d955a000000000},
+      {0xeb194f8e1ae525fd, 0x5dcfab0800000000},
+      {0x92efd1b8d0cf37be, 0x5aa1cae500000000},
+      {0xb7abc627050305ad, 0xf14a3d9e40000000},
+      {0xe596b7b0c643c719, 0x6d9ccd05d0000000},
+      {0x8f7e32ce7bea5c6f, 0xe4820023a2000000},
+      {0xb35dbf821ae4f38b, 0xdda2802c8a800000},
+      {0xe0352f62a19e306e, 0xd50b2037ad200000},
+      {0x8c213d9da502de45, 0x4526f422cc340000},
+      {0xaf298d050e4395d6, 0x9670b12b7f410000},
+      {0xdaf3f04651d47b4c, 0x3c0cdd765f114000},
+      {0x88d8762bf324cd0f, 0xa5880a69fb6ac800},
+      {0xab0e93b6efee0053, 0x8eea0d047a457a00},
+      {0xd5d238a4abe98068, 0x72a4904598d6d880},
+      {0x85a36366eb71f041, 0x47a6da2b7f864750},
+      {0xa70c3c40a64e6c51, 0x999090b65f67d924},
+      {0xd0cf4b50cfe20765, 0xfff4b4e3f741cf6d},
+      {0x82818f1281ed449f, 0xbff8f10e7a8921a4},
+      {0xa321f2d7226895c7, 0xaff72d52192b6a0d},
+      {0xcbea6f8ceb02bb39, 0x9bf4f8a69f764490},
+      {0xfee50b7025c36a08, 0x02f236d04753d5b4},
+      {0x9f4f2726179a2245, 0x01d762422c946590},
+      {0xc722f0ef9d80aad6, 0x424d3ad2b7b97ef5},
+      {0xf8ebad2b84e0d58b, 0xd2e0898765a7deb2},
+      {0x9b934c3b330c8577, 0x63cc55f49f88eb2f},
+      {0xc2781f49ffcfa6d5, 0x3cbf6b71c76b25fb},
+      {0xf316271c7fc3908a, 0x8bef464e3945ef7a},
+      {0x97edd871cfda3a56, 0x97758bf0e3cbb5ac},
+      {0xbde94e8e43d0c8ec, 0x3d52eeed1cbea317},
+      {0xed63a231d4c4fb27, 0x4ca7aaa863ee4bdd},
+      {0x945e455f24fb1cf8, 0x8fe8caa93e74ef6a},
+      {0xb975d6b6ee39e436, 0xb3e2fd538e122b44},
+      {0xe7d34c64a9c85d44, 0x60dbbca87196b616},
+      {0x90e40fbeea1d3a4a, 0xbc8955e946fe31cd},
+      {0xb51d13aea4a488dd, 0x6babab6398bdbe41},
+      {0xe264589a4dcdab14, 0xc696963c7eed2dd1},
+      {0x8d7eb76070a08aec, 0xfc1e1de5cf543ca2},
+      {0xb0de65388cc8ada8, 0x3b25a55f43294bcb},
+      {0xdd15fe86affad912, 0x49ef0eb713f39ebe},
+      {0x8a2dbf142dfcc7ab, 0x6e3569326c784337},
+      {0xacb92ed9397bf996, 0x49c2c37f07965404},
+      {0xd7e77a8f87daf7fb, 0xdc33745ec97be906},
+      {0x86f0ac99b4e8dafd, 0x69a028bb3ded71a3},
+      {0xa8acd7c0222311bc, 0xc40832ea0d68ce0c},
+      {0xd2d80db02aabd62b, 0xf50a3fa490c30190},
+      {0x83c7088e1aab65db, 0x792667c6da79e0fa},
+      {0xa4b8cab1a1563f52, 0x577001b891185938},
+      {0xcde6fd5e09abcf26, 0xed4c0226b55e6f86},
+      {0x80b05e5ac60b6178, 0x544f8158315b05b4},
+      {0xa0dc75f1778e39d6, 0x696361ae3db1c721},
+      {0xc913936dd571c84c, 0x03bc3a19cd1e38e9},
+      {0xfb5878494ace3a5f, 0x04ab48a04065c723},
+      {0x9d174b2dcec0e47b, 0x62eb0d64283f9c76},
+      {0xc45d1df942711d9a, 0x3ba5d0bd324f8394},
+      {0xf5746577930d6500, 0xca8f44ec7ee36479},
+      {0x9968bf6abbe85f20, 0x7e998b13cf4e1ecb},
+      {0xbfc2ef456ae276e8, 0x9e3fedd8c321a67e},
+      {0xefb3ab16c59b14a2, 0xc5cfe94ef3ea101e},
+      {0x95d04aee3b80ece5, 0xbba1f1d158724a12},
+      {0xbb445da9ca61281f, 0x2a8a6e45ae8edc97},
+      {0xea1575143cf97226, 0xf52d09d71a3293bd},
+      {0x924d692ca61be758, 0x593c2626705f9c56},
+      {0xb6e0c377cfa2e12e, 0x6f8b2fb00c77836c},
+      {0xe498f455c38b997a, 0x0b6dfb9c0f956447},
+      {0x8edf98b59a373fec, 0x4724bd4189bd5eac},
+      {0xb2977ee300c50fe7, 0x58edec91ec2cb657},
+      {0xdf3d5e9bc0f653e1, 0x2f2967b66737e3ed},
+      {0x8b865b215899f46c, 0xbd79e0d20082ee74},
+      {0xae67f1e9aec07187, 0xecd8590680a3aa11},
+      {0xda01ee641a708de9, 0xe80e6f4820cc9495},
+      {0x884134fe908658b2, 0x3109058d147fdcdd},
+      {0xaa51823e34a7eede, 0xbd4b46f0599fd415},
+      {0xd4e5e2cdc1d1ea96, 0x6c9e18ac7007c91a},
+      {0x850fadc09923329e, 0x03e2cf6bc604ddb0},
+      {0xa6539930bf6bff45, 0x84db8346b786151c},
+      {0xcfe87f7cef46ff16, 0xe612641865679a63},
+      {0x81f14fae158c5f6e, 0x4fcb7e8f3f60c07e},
+      {0xa26da3999aef7749, 0xe3be5e330f38f09d},
+      {0xcb090c8001ab551c, 0x5cadf5bfd3072cc5},
+      {0xfdcb4fa002162a63, 0x73d9732fc7c8f7f6},
+      {0x9e9f11c4014dda7e, 0x2867e7fddcdd9afa},
+      {0xc646d63501a1511d, 0xb281e1fd541501b8},
+      {0xf7d88bc24209a565, 0x1f225a7ca91a4226},
+      {0x9ae757596946075f, 0x3375788de9b06958},
+      {0xc1a12d2fc3978937, 0x0052d6b1641c83ae},
+      {0xf209787bb47d6b84, 0xc0678c5dbd23a49a},
+      {0x9745eb4d50ce6332, 0xf840b7ba963646e0},
+      {0xbd176620a501fbff, 0xb650e5a93bc3d898},
+      {0xec5d3fa8ce427aff, 0xa3e51f138ab4cebe},
+      {0x93ba47c980e98cdf, 0xc66f336c36b10137},
+      {0xb8a8d9bbe123f017, 0xb80b0047445d4184},
+      {0xe6d3102ad96cec1d, 0xa60dc059157491e5},
+      {0x9043ea1ac7e41392, 0x87c89837ad68db2f},
+      {0xb454e4a179dd1877, 0x29babe4598c311fb},
+      {0xe16a1dc9d8545e94, 0xf4296dd6fef3d67a},
+      {0x8ce2529e2734bb1d, 0x1899e4a65f58660c},
+      {0xb01ae745b101e9e4, 0x5ec05dcff72e7f8f},
+      {0xdc21a1171d42645d, 0x76707543f4fa1f73},
+      {0x899504ae72497eba, 0x6a06494a791c53a8},
+      {0xabfa45da0edbde69, 0x0487db9d17636892},
+      {0xd6f8d7509292d603, 0x45a9d2845d3c42b6},
+      {0x865b86925b9bc5c2, 0x0b8a2392ba45a9b2},
+      {0xa7f26836f282b732, 0x8e6cac7768d7141e},
+      {0xd1ef0244af2364ff, 0x3207d795430cd926},
+      {0x8335616aed761f1f, 0x7f44e6bd49e807b8},
+      {0xa402b9c5a8d3a6e7, 0x5f16206c9c6209a6},
+      {0xcd036837130890a1, 0x36dba887c37a8c0f},
+      {0x802221226be55a64, 0xc2494954da2c9789},
+      {0xa02aa96b06deb0fd, 0xf2db9baa10b7bd6c},
+      {0xc83553c5c8965d3d, 0x6f92829494e5acc7},
+      {0xfa42a8b73abbf48c, 0xcb772339ba1f17f9},
+      {0x9c69a97284b578d7, 0xff2a760414536efb},
+      {0xc38413cf25e2d70d, 0xfef5138519684aba},
+      {0xf46518c2ef5b8cd1, 0x7eb258665fc25d69},
+      {0x98bf2f79d5993802, 0xef2f773ffbd97a61},
+      {0xbeeefb584aff8603, 0xaafb550ffacfd8fa},
+      {0xeeaaba2e5dbf6784, 0x95ba2a53f983cf38},
+      {0x952ab45cfa97a0b2, 0xdd945a747bf26183},
+      {0xba756174393d88df, 0x94f971119aeef9e4},
+      {0xe912b9d1478ceb17, 0x7a37cd5601aab85d},
+      {0x91abb422ccb812ee, 0xac62e055c10ab33a},
+      {0xb616a12b7fe617aa, 0x577b986b314d6009},
+      {0xe39c49765fdf9d94, 0xed5a7e85fda0b80b},
+      {0x8e41ade9fbebc27d, 0x14588f13be847307},
+      {0xb1d219647ae6b31c, 0x596eb2d8ae258fc8},
+      {0xde469fbd99a05fe3, 0x6fca5f8ed9aef3bb},
+      {0x8aec23d680043bee, 0x25de7bb9480d5854},
+      {0xada72ccc20054ae9, 0xaf561aa79a10ae6a},
+      {0xd910f7ff28069da4, 0x1b2ba1518094da04},
+      {0x87aa9aff79042286, 0x90fb44d2f05d0842},
+      {0xa99541bf57452b28, 0x353a1607ac744a53},
+      {0xd3fa922f2d1675f2, 0x42889b8997915ce8},
+      {0x847c9b5d7c2e09b7, 0x69956135febada11},
+      {0xa59bc234db398c25, 0x43fab9837e699095},
+      {0xcf02b2c21207ef2e, 0x94f967e45e03f4bb},
+      {0x8161afb94b44f57d, 0x1d1be0eebac278f5},
+      {0xa1ba1ba79e1632dc, 0x6462d92a69731732},
+      {0xca28a291859bbf93, 0x7d7b8f7503cfdcfe},
+      {0xfcb2cb35e702af78, 0x5cda735244c3d43e},
+      {0x9defbf01b061adab, 0x3a0888136afa64a7},
+      {0xc56baec21c7a1916, 0x088aaa1845b8fdd0},
+      {0xf6c69a72a3989f5b, 0x8aad549e57273d45},
+      {0x9a3c2087a63f6399, 0x36ac54e2f678864b},
+      {0xc0cb28a98fcf3c7f, 0x84576a1bb416a7dd},
+      {0xf0fdf2d3f3c30b9f, 0x656d44a2a11c51d5},
+      {0x969eb7c47859e743, 0x9f644ae5a4b1b325},
+      {0xbc4665b596706114, 0x873d5d9f0dde1fee},
+      {0xeb57ff22fc0c7959, 0xa90cb506d155a7ea},
+      {0x9316ff75dd87cbd8, 0x09a7f12442d588f2},
+      {0xb7dcbf5354e9bece, 0x0c11ed6d538aeb2f},
+      {0xe5d3ef282a242e81, 0x8f1668c8a86da5fa},
+      {0x8fa475791a569d10, 0xf96e017d694487bc},
+      {0xb38d92d760ec4455, 0x37c981dcc395a9ac},
+      {0xe070f78d3927556a, 0x85bbe253f47b1417},
+      {0x8c469ab843b89562, 0x93956d7478ccec8e},
+      {0xaf58416654a6babb, 0x387ac8d1970027b2},
+      {0xdb2e51bfe9d0696a, 0x06997b05fcc0319e},
+      {0x88fcf317f22241e2, 0x441fece3bdf81f03},
+      {0xab3c2fddeeaad25a, 0xd527e81cad7626c3},
+      {0xd60b3bd56a5586f1, 0x8a71e223d8d3b074},
+      {0x85c7056562757456, 0xf6872d5667844e49},
+      {0xa738c6bebb12d16c, 0xb428f8ac016561db},
+      {0xd106f86e69d785c7, 0xe13336d701beba52},
+      {0x82a45b450226b39c, 0xecc0024661173473},
+      {0xa34d721642b06084, 0x27f002d7f95d0190},
+      {0xcc20ce9bd35c78a5, 0x31ec038df7b441f4},
+      {0xff290242c83396ce, 0x7e67047175a15271},
+      {0x9f79a169bd203e41, 0x0f0062c6e984d386},
+      {0xc75809c42c684dd1, 0x52c07b78a3e60868},
+      {0xf92e0c3537826145, 0xa7709a56ccdf8a82},
+      {0x9bbcc7a142b17ccb, 0x88a66076400bb691},
+      {0xc2abf989935ddbfe, 0x6acff893d00ea435},
+      {0xf356f7ebf83552fe, 0x0583f6b8c4124d43},
+      {0x98165af37b2153de, 0xc3727a337a8b704a},
+      {0xbe1bf1b059e9a8d6, 0x744f18c0592e4c5c},
+      {0xeda2ee1c7064130c, 0x1162def06f79df73},
+      {0x9485d4d1c63e8be7, 0x8addcb5645ac2ba8},
+      {0xb9a74a0637ce2ee1, 0x6d953e2bd7173692},
+      {0xe8111c87c5c1ba99, 0xc8fa8db6ccdd0437},
+      {0x910ab1d4db9914a0, 0x1d9c9892400a22a2},
+      {0xb54d5e4a127f59c8, 0x2503beb6d00cab4b},
+      {0xe2a0b5dc971f303a, 0x2e44ae64840fd61d},
+      {0x8da471a9de737e24, 0x5ceaecfed289e5d2},
+      {0xb10d8e1456105dad, 0x7425a83e872c5f47},
+      {0xdd50f1996b947518, 0xd12f124e28f77719},
+      {0x8a5296ffe33cc92f, 0x82bd6b70d99aaa6f},
+      {0xace73cbfdc0bfb7b, 0x636cc64d1001550b},
+      {0xd8210befd30efa5a, 0x3c47f7e05401aa4e},
+      {0x8714a775e3e95c78, 0x65acfaec34810a71},
+      {0xa8d9d1535ce3b396, 0x7f1839a741a14d0d},
+      {0xd31045a8341ca07c, 0x1ede48111209a050},
+      {0x83ea2b892091e44d, 0x934aed0aab460432},
+      {0xa4e4b66b68b65d60, 0xf81da84d5617853f},
+      {0xce1de40642e3f4b9, 0x36251260ab9d668e},
+      {0x80d2ae83e9ce78f3, 0xc1d72b7c6b426019},
+      {0xa1075a24e4421730, 0xb24cf65b8612f81f},
+      {0xc94930ae1d529cfc, 0xdee033f26797b627},
+      {0xfb9b7cd9a4a7443c, 0x169840ef017da3b1},
+      {0x9d412e0806e88aa5, 0x8e1f289560ee864e},
+      {0xc491798a08a2ad4e, 0xf1a6f2bab92a27e2},
+      {0xf5b5d7ec8acb58a2, 0xae10af696774b1db},
+      {0x9991a6f3d6bf1765, 0xacca6da1e0a8ef29},
+      {0xbff610b0cc6edd3f, 0x17fd090a58d32af3},
+      {0xeff394dcff8a948e, 0xddfc4b4cef07f5b0},
+      {0x95f83d0a1fb69cd9, 0x4abdaf101564f98e},
+      {0xbb764c4ca7a4440f, 0x9d6d1ad41abe37f1},
+      {0xea53df5fd18d5513, 0x84c86189216dc5ed},
+      {0x92746b9be2f8552c, 0x32fd3cf5b4e49bb4},
+      {0xb7118682dbb66a77, 0x3fbc8c33221dc2a1},
+      {0xe4d5e82392a40515, 0x0fabaf3feaa5334a},
+      {0x8f05b1163ba6832d, 0x29cb4d87f2a7400e},
+      {0xb2c71d5bca9023f8, 0x743e20e9ef511012},
+      {0xdf78e4b2bd342cf6, 0x914da9246b255416},
+      {0x8bab8eefb6409c1a, 0x1ad089b6c2f7548e},
+      {0xae9672aba3d0c320, 0xa184ac2473b529b1},
+      {0xda3c0f568cc4f3e8, 0xc9e5d72d90a2741e},
+      {0x8865899617fb1871, 0x7e2fa67c7a658892},
+      {0xaa7eebfb9df9de8d, 0xddbb901b98feeab7},
+      {0xd51ea6fa85785631, 0x552a74227f3ea565},
+      {0x8533285c936b35de, 0xd53a88958f87275f},
+      {0xa67ff273b8460356, 0x8a892abaf368f137},
+      {0xd01fef10a657842c, 0x2d2b7569b0432d85},
+      {0x8213f56a67f6b29b, 0x9c3b29620e29fc73},
+      {0xa298f2c501f45f42, 0x8349f3ba91b47b8f},
+      {0xcb3f2f7642717713, 0x241c70a936219a73},
+      {0xfe0efb53d30dd4d7, 0xed238cd383aa0110},
+      {0x9ec95d1463e8a506, 0xf4363804324a40aa},
+      {0xc67bb4597ce2ce48, 0xb143c6053edcd0d5},
+      {0xf81aa16fdc1b81da, 0xdd94b7868e94050a},
+      {0x9b10a4e5e9913128, 0xca7cf2b4191c8326},
+      {0xc1d4ce1f63f57d72, 0xfd1c2f611f63a3f0},
+      {0xf24a01a73cf2dccf, 0xbc633b39673c8cec},
+      {0x976e41088617ca01, 0xd5be0503e085d813},
+      {0xbd49d14aa79dbc82, 0x4b2d8644d8a74e18},
+      {0xec9c459d51852ba2, 0xddf8e7d60ed1219e},
+      {0x93e1ab8252f33b45, 0xcabb90e5c942b503},
+      {0xb8da1662e7b00a17, 0x3d6a751f3b936243},
+      {0xe7109bfba19c0c9d, 0x0cc512670a783ad4},
+      {0x906a617d450187e2, 0x27fb2b80668b24c5},
+      {0xb484f9dc9641e9da, 0xb1f9f660802dedf6},
+      {0xe1a63853bbd26451, 0x5e7873f8a0396973},
+      {0x8d07e33455637eb2, 0xdb0b487b6423e1e8},
+      {0xb049dc016abc5e5f, 0x91ce1a9a3d2cda62},
+      {0xdc5c5301c56b75f7, 0x7641a140cc7810fb},
+      {0x89b9b3e11b6329ba, 0xa9e904c87fcb0a9d},
+      {0xac2820d9623bf429, 0x546345fa9fbdcd44},
+      {0xd732290fbacaf133, 0xa97c177947ad4095},
+      {0x867f59a9d4bed6c0, 0x49ed8eabcccc485d},
+      {0xa81f301449ee8c70, 0x5c68f256bfff5a74},
+      {0xd226fc195c6a2f8c, 0x73832eec6fff3111},
+      {0x83585d8fd9c25db7, 0xc831fd53c5ff7eab},
+      {0xa42e74f3d032f525, 0xba3e7ca8b77f5e55},
+      {0xcd3a1230c43fb26f, 0x28ce1bd2e55f35eb},
+      {0x80444b5e7aa7cf85, 0x7980d163cf5b81b3},
+      {0xa0555e361951c366, 0xd7e105bcc332621f},
+      {0xc86ab5c39fa63440, 0x8dd9472bf3fefaa7},
+      {0xfa856334878fc150, 0xb14f98f6f0feb951},
+      {0x9c935e00d4b9d8d2, 0x6ed1bf9a569f33d3},
+      {0xc3b8358109e84f07, 0x0a862f80ec4700c8},
+      {0xf4a642e14c6262c8, 0xcd27bb612758c0fa},
+      {0x98e7e9cccfbd7dbd, 0x8038d51cb897789c},
+      {0xbf21e44003acdd2c, 0xe0470a63e6bd56c3},
+      {0xeeea5d5004981478, 0x1858ccfce06cac74},
+      {0x95527a5202df0ccb, 0x0f37801e0c43ebc8},
+      {0xbaa718e68396cffd, 0xd30560258f54e6ba},
+      {0xe950df20247c83fd, 0x47c6b82ef32a2069},
+      {0x91d28b7416cdd27e, 0x4cdc331d57fa5441},
+      {0xb6472e511c81471d, 0xe0133fe4adf8e952},
+      {0xe3d8f9e563a198e5, 0x58180fddd97723a6},
+      {0x8e679c2f5e44ff8f, 0x570f09eaa7ea7648},
+      {0xb201833b35d63f73, 0x2cd2cc6551e513da},
+      {0xde81e40a034bcf4f, 0xf8077f7ea65e58d1},
+      {0x8b112e86420f6191, 0xfb04afaf27faf782},
+      {0xadd57a27d29339f6, 0x79c5db9af1f9b563},
+      {0xd94ad8b1c7380874, 0x18375281ae7822bc},
+      {0x87cec76f1c830548, 0x8f2293910d0b15b5},
+      {0xa9c2794ae3a3c69a, 0xb2eb3875504ddb22},
+      {0xd433179d9c8cb841, 0x5fa60692a46151eb},
+      {0x849feec281d7f328, 0xdbc7c41ba6bcd333},
+      {0xa5c7ea73224deff3, 0x12b9b522906c0800},
+      {0xcf39e50feae16bef, 0xd768226b34870a00},
+      {0x81842f29f2cce375, 0xe6a1158300d46640},
+      {0xa1e53af46f801c53, 0x60495ae3c1097fd0},
+      {0xca5e89b18b602368, 0x385bb19cb14bdfc4},
+      {0xfcf62c1dee382c42, 0x46729e03dd9ed7b5},
+      {0x9e19db92b4e31ba9, 0x6c07a2c26a8346d1},
+      {0xc5a05277621be293, 0xc7098b7305241885},
+      { 0xf70867153aa2db38,
+        0xb8cbee4fc66d1ea7 }
+#else
+      {0xff77b1fcbebcdc4f, 0x25e8e89c13bb0f7b},
+      {0xce5d73ff402d98e3, 0xfb0a3d212dc81290},
+      {0xa6b34ad8c9dfc06f, 0xf42faa48c0ea481f},
+      {0x86a8d39ef77164bc, 0xae5dff9c02033198},
+      {0xd98ddaee19068c76, 0x3badd624dd9b0958},
+      {0xafbd2350644eeacf, 0xe5d1929ef90898fb},
+      {0x8df5efabc5979c8f, 0xca8d3ffa1ef463c2},
+      {0xe55990879ddcaabd, 0xcc420a6a101d0516},
+      {0xb94470938fa89bce, 0xf808e40e8d5b3e6a},
+      {0x95a8637627989aad, 0xdde7001379a44aa9},
+      {0xf1c90080baf72cb1, 0x5324c68b12dd6339},
+      {0xc350000000000000, 0x0000000000000000},
+      {0x9dc5ada82b70b59d, 0xf020000000000000},
+      {0xfee50b7025c36a08, 0x02f236d04753d5b4},
+      {0xcde6fd5e09abcf26, 0xed4c0226b55e6f86},
+      {0xa6539930bf6bff45, 0x84db8346b786151c},
+      {0x865b86925b9bc5c2, 0x0b8a2392ba45a9b2},
+      {0xd910f7ff28069da4, 0x1b2ba1518094da04},
+      {0xaf58416654a6babb, 0x387ac8d1970027b2},
+      {0x8da471a9de737e24, 0x5ceaecfed289e5d2},
+      {0xe4d5e82392a40515, 0x0fabaf3feaa5334a},
+      {0xb8da1662e7b00a17, 0x3d6a751f3b936243},
+      { 0x95527a5202df0ccb,
+        0x0f37801e0c43ebc8 }
+#endif
+    };
+
+#if FMT_USE_FULL_CACHE_DRAGONBOX
+    return pow10_significands[k - float_info<double>::min_k];
+#else
+    static constexpr const uint64_t powers_of_5_64[] = {
+        0x0000000000000001, 0x0000000000000005, 0x0000000000000019,
+        0x000000000000007d, 0x0000000000000271, 0x0000000000000c35,
+        0x0000000000003d09, 0x000000000001312d, 0x000000000005f5e1,
+        0x00000000001dcd65, 0x00000000009502f9, 0x0000000002e90edd,
+        0x000000000e8d4a51, 0x0000000048c27395, 0x000000016bcc41e9,
+        0x000000071afd498d, 0x0000002386f26fc1, 0x000000b1a2bc2ec5,
+        0x000003782dace9d9, 0x00001158e460913d, 0x000056bc75e2d631,
+        0x0001b1ae4d6e2ef5, 0x000878678326eac9, 0x002a5a058fc295ed,
+        0x00d3c21bcecceda1, 0x0422ca8b0a00a425, 0x14adf4b7320334b9};
+
+    static constexpr const uint32_t pow10_recovery_errors[] = {
+        0x50001400, 0x54044100, 0x54014555, 0x55954415, 0x54115555, 0x00000001,
+        0x50000000, 0x00104000, 0x54010004, 0x05004001, 0x55555544, 0x41545555,
+        0x54040551, 0x15445545, 0x51555514, 0x10000015, 0x00101100, 0x01100015,
+        0x00000000, 0x00000000, 0x00000000, 0x00000000, 0x04450514, 0x45414110,
+        0x55555145, 0x50544050, 0x15040155, 0x11054140, 0x50111514, 0x11451454,
+        0x00400541, 0x00000000, 0x55555450, 0x10056551, 0x10054011, 0x55551014,
+        0x69514555, 0x05151109, 0x00155555};
+
+    static const int compression_ratio = 27;
+
+    // Compute base index.
+    int cache_index = (k - float_info<double>::min_k) / compression_ratio;
+    int kb = cache_index * compression_ratio + float_info<double>::min_k;
+    int offset = k - kb;
+
+    // Get base cache.
+    uint128_wrapper base_cache = pow10_significands[cache_index];
+    if (offset == 0) return base_cache;
+
+    // Compute the required amount of bit-shift.
+    int alpha = floor_log2_pow10(kb + offset) - floor_log2_pow10(kb) - offset;
+    FMT_ASSERT(alpha > 0 && alpha < 64, "shifting error detected");
+
+    // Try to recover the real cache.
+    uint64_t pow5 = powers_of_5_64[offset];
+    uint128_wrapper recovered_cache = umul128(base_cache.high(), pow5);
+    uint128_wrapper middle_low =
+        umul128(base_cache.low() - (kb < 0 ? 1u : 0u), pow5);
+
+    recovered_cache += middle_low.high();
+
+    uint64_t high_to_middle = recovered_cache.high() << (64 - alpha);
+    uint64_t middle_to_low = recovered_cache.low() << (64 - alpha);
+
+    recovered_cache =
+        uint128_wrapper{(recovered_cache.low() >> alpha) | high_to_middle,
+                        ((middle_low.low() >> alpha) | middle_to_low)};
+
+    if (kb < 0) recovered_cache += 1;
+
+    // Get error.
+    int error_idx = (k - float_info<double>::min_k) / 16;
+    uint32_t error = (pow10_recovery_errors[error_idx] >>
+                      ((k - float_info<double>::min_k) % 16) * 2) &
+                     0x3;
+
+    // Add the error back.
+    FMT_ASSERT(recovered_cache.low() + error >= recovered_cache.low(), "");
+    return {recovered_cache.high(), recovered_cache.low() + error};
+#endif
+  }
+
+  static carrier_uint compute_mul(carrier_uint u,
+                                  const cache_entry_type& cache) FMT_NOEXCEPT {
+    return umul192_upper64(u, cache);
+  }
+
+  static uint32_t compute_delta(cache_entry_type const& cache,
+                                int beta_minus_1) FMT_NOEXCEPT {
+    return static_cast<uint32_t>(cache.high() >> (64 - 1 - beta_minus_1));
+  }
+
+  static bool compute_mul_parity(carrier_uint two_f,
+                                 const cache_entry_type& cache,
+                                 int beta_minus_1) FMT_NOEXCEPT {
+    FMT_ASSERT(beta_minus_1 >= 1, "");
+    FMT_ASSERT(beta_minus_1 < 64, "");
+
+    return ((umul192_middle64(two_f, cache) >> (64 - beta_minus_1)) & 1) != 0;
+  }
+
+  static carrier_uint compute_left_endpoint_for_shorter_interval_case(
+      const cache_entry_type& cache, int beta_minus_1) FMT_NOEXCEPT {
+    return (cache.high() -
+            (cache.high() >> (float_info<double>::significand_bits + 2))) >>
+           (64 - float_info<double>::significand_bits - 1 - beta_minus_1);
+  }
+
+  static carrier_uint compute_right_endpoint_for_shorter_interval_case(
+      const cache_entry_type& cache, int beta_minus_1) FMT_NOEXCEPT {
+    return (cache.high() +
+            (cache.high() >> (float_info<double>::significand_bits + 1))) >>
+           (64 - float_info<double>::significand_bits - 1 - beta_minus_1);
+  }
+
+  static carrier_uint compute_round_up_for_shorter_interval_case(
+      const cache_entry_type& cache, int beta_minus_1) FMT_NOEXCEPT {
+    return ((cache.high() >>
+             (64 - float_info<double>::significand_bits - 2 - beta_minus_1)) +
+            1) /
+           2;
+  }
+};
+
+// Various integer checks
+template <class T>
+bool is_left_endpoint_integer_shorter_interval(int exponent) FMT_NOEXCEPT {
+  return exponent >=
+             float_info<
+                 T>::case_shorter_interval_left_endpoint_lower_threshold &&
+         exponent <=
+             float_info<T>::case_shorter_interval_left_endpoint_upper_threshold;
+}
+template <class T>
+bool is_endpoint_integer(typename float_info<T>::carrier_uint two_f,
+                         int exponent, int minus_k) FMT_NOEXCEPT {
+  if (exponent < float_info<T>::case_fc_pm_half_lower_threshold) return false;
+  // For k >= 0.
+  if (exponent <= float_info<T>::case_fc_pm_half_upper_threshold) return true;
+  // For k < 0.
+  if (exponent > float_info<T>::divisibility_check_by_5_threshold) return false;
+  return divisible_by_power_of_5(two_f, minus_k);
+}
+
+template <class T>
+bool is_center_integer(typename float_info<T>::carrier_uint two_f, int exponent,
+                       int minus_k) FMT_NOEXCEPT {
+  // Exponent for 5 is negative.
+  if (exponent > float_info<T>::divisibility_check_by_5_threshold) return false;
+  if (exponent > float_info<T>::case_fc_upper_threshold)
+    return divisible_by_power_of_5(two_f, minus_k);
+  // Both exponents are nonnegative.
+  if (exponent >= float_info<T>::case_fc_lower_threshold) return true;
+  // Exponent for 2 is negative.
+  return divisible_by_power_of_2(two_f, minus_k - exponent + 1);
+}
+
+// Remove trailing zeros from n and return the number of zeros removed (float)
+FMT_INLINE int remove_trailing_zeros(uint32_t& n) FMT_NOEXCEPT {
+#ifdef FMT_BUILTIN_CTZ
+  int t = FMT_BUILTIN_CTZ(n);
+#else
+  int t = ctz(n);
+#endif
+  if (t > float_info<float>::max_trailing_zeros)
+    t = float_info<float>::max_trailing_zeros;
+
+  const uint32_t mod_inv1 = 0xcccccccd;
+  const uint32_t max_quotient1 = 0x33333333;
+  const uint32_t mod_inv2 = 0xc28f5c29;
+  const uint32_t max_quotient2 = 0x0a3d70a3;
+
+  int s = 0;
+  for (; s < t - 1; s += 2) {
+    if (n * mod_inv2 > max_quotient2) break;
+    n *= mod_inv2;
+  }
+  if (s < t && n * mod_inv1 <= max_quotient1) {
+    n *= mod_inv1;
+    ++s;
+  }
+  n >>= s;
+  return s;
+}
+
+// Removes trailing zeros and returns the number of zeros removed (double)
+FMT_INLINE int remove_trailing_zeros(uint64_t& n) FMT_NOEXCEPT {
+#ifdef FMT_BUILTIN_CTZLL
+  int t = FMT_BUILTIN_CTZLL(n);
+#else
+  int t = ctzll(n);
+#endif
+  if (t > float_info<double>::max_trailing_zeros)
+    t = float_info<double>::max_trailing_zeros;
+  // Divide by 10^8 and reduce to 32-bits
+  // Since ret_value.significand <= (2^64 - 1) / 1000 < 10^17,
+  // both of the quotient and the r should fit in 32-bits
+
+  const uint32_t mod_inv1 = 0xcccccccd;
+  const uint32_t max_quotient1 = 0x33333333;
+  const uint64_t mod_inv8 = 0xc767074b22e90e21;
+  const uint64_t max_quotient8 = 0x00002af31dc46118;
+
+  // If the number is divisible by 1'0000'0000, work with the quotient
+  if (t >= 8) {
+    auto quotient_candidate = n * mod_inv8;
+
+    if (quotient_candidate <= max_quotient8) {
+      auto quotient = static_cast<uint32_t>(quotient_candidate >> 8);
+
+      int s = 8;
+      for (; s < t; ++s) {
+        if (quotient * mod_inv1 > max_quotient1) break;
+        quotient *= mod_inv1;
+      }
+      quotient >>= (s - 8);
+      n = quotient;
+      return s;
+    }
+  }
+
+  // Otherwise, work with the remainder
+  auto quotient = static_cast<uint32_t>(n / 100000000);
+  auto remainder = static_cast<uint32_t>(n - 100000000 * quotient);
+
+  if (t == 0 || remainder * mod_inv1 > max_quotient1) {
+    return 0;
+  }
+  remainder *= mod_inv1;
+
+  if (t == 1 || remainder * mod_inv1 > max_quotient1) {
+    n = (remainder >> 1) + quotient * 10000000ull;
+    return 1;
+  }
+  remainder *= mod_inv1;
+
+  if (t == 2 || remainder * mod_inv1 > max_quotient1) {
+    n = (remainder >> 2) + quotient * 1000000ull;
+    return 2;
+  }
+  remainder *= mod_inv1;
+
+  if (t == 3 || remainder * mod_inv1 > max_quotient1) {
+    n = (remainder >> 3) + quotient * 100000ull;
+    return 3;
+  }
+  remainder *= mod_inv1;
+
+  if (t == 4 || remainder * mod_inv1 > max_quotient1) {
+    n = (remainder >> 4) + quotient * 10000ull;
+    return 4;
+  }
+  remainder *= mod_inv1;
+
+  if (t == 5 || remainder * mod_inv1 > max_quotient1) {
+    n = (remainder >> 5) + quotient * 1000ull;
+    return 5;
+  }
+  remainder *= mod_inv1;
+
+  if (t == 6 || remainder * mod_inv1 > max_quotient1) {
+    n = (remainder >> 6) + quotient * 100ull;
+    return 6;
+  }
+  remainder *= mod_inv1;
+
+  n = (remainder >> 7) + quotient * 10ull;
+  return 7;
+}
+
+// The main algorithm for shorter interval case
+template <class T>
+FMT_INLINE decimal_fp<T> shorter_interval_case(int exponent) FMT_NOEXCEPT {
+  decimal_fp<T> ret_value;
+  // Compute k and beta
+  const int minus_k = floor_log10_pow2_minus_log10_4_over_3(exponent);
+  const int beta_minus_1 = exponent + floor_log2_pow10(-minus_k);
+
+  // Compute xi and zi
+  using cache_entry_type = typename cache_accessor<T>::cache_entry_type;
+  const cache_entry_type cache = cache_accessor<T>::get_cached_power(-minus_k);
+
+  auto xi = cache_accessor<T>::compute_left_endpoint_for_shorter_interval_case(
+      cache, beta_minus_1);
+  auto zi = cache_accessor<T>::compute_right_endpoint_for_shorter_interval_case(
+      cache, beta_minus_1);
+
+  // If the left endpoint is not an integer, increase it
+  if (!is_left_endpoint_integer_shorter_interval<T>(exponent)) ++xi;
+
+  // Try bigger divisor
+  ret_value.significand = zi / 10;
+
+  // If succeed, remove trailing zeros if necessary and return
+  if (ret_value.significand * 10 >= xi) {
+    ret_value.exponent = minus_k + 1;
+    ret_value.exponent += remove_trailing_zeros(ret_value.significand);
+    return ret_value;
+  }
+
+  // Otherwise, compute the round-up of y
+  ret_value.significand =
+      cache_accessor<T>::compute_round_up_for_shorter_interval_case(
+          cache, beta_minus_1);
+  ret_value.exponent = minus_k;
+
+  // When tie occurs, choose one of them according to the rule
+  if (exponent >= float_info<T>::shorter_interval_tie_lower_threshold &&
+      exponent <= float_info<T>::shorter_interval_tie_upper_threshold) {
+    ret_value.significand = ret_value.significand % 2 == 0
+                                ? ret_value.significand
+                                : ret_value.significand - 1;
+  } else if (ret_value.significand < xi) {
+    ++ret_value.significand;
+  }
+  return ret_value;
+}
+
+template <typename T> decimal_fp<T> to_decimal(T x) FMT_NOEXCEPT {
+  // Step 1: integer promotion & Schubfach multiplier calculation.
+
+  using carrier_uint = typename float_info<T>::carrier_uint;
+  using cache_entry_type = typename cache_accessor<T>::cache_entry_type;
+  auto br = bit_cast<carrier_uint>(x);
+
+  // Extract significand bits and exponent bits.
+  const carrier_uint significand_mask =
+      (static_cast<carrier_uint>(1) << float_info<T>::significand_bits) - 1;
+  carrier_uint significand = (br & significand_mask);
+  int exponent = static_cast<int>((br & exponent_mask<T>()) >>
+                                  float_info<T>::significand_bits);
+
+  if (exponent != 0) {  // Check if normal.
+    exponent += float_info<T>::exponent_bias - float_info<T>::significand_bits;
+
+    // Shorter interval case; proceed like Schubfach.
+    if (significand == 0) return shorter_interval_case<T>(exponent);
+
+    significand |=
+        (static_cast<carrier_uint>(1) << float_info<T>::significand_bits);
+  } else {
+    // Subnormal case; the interval is always regular.
+    if (significand == 0) return {0, 0};
+    exponent = float_info<T>::min_exponent - float_info<T>::significand_bits;
+  }
+
+  const bool include_left_endpoint = (significand % 2 == 0);
+  const bool include_right_endpoint = include_left_endpoint;
+
+  // Compute k and beta.
+  const int minus_k = floor_log10_pow2(exponent) - float_info<T>::kappa;
+  const cache_entry_type cache = cache_accessor<T>::get_cached_power(-minus_k);
+  const int beta_minus_1 = exponent + floor_log2_pow10(-minus_k);
+
+  // Compute zi and deltai
+  // 10^kappa <= deltai < 10^(kappa + 1)
+  const uint32_t deltai = cache_accessor<T>::compute_delta(cache, beta_minus_1);
+  const carrier_uint two_fc = significand << 1;
+  const carrier_uint two_fr = two_fc | 1;
+  const carrier_uint zi =
+      cache_accessor<T>::compute_mul(two_fr << beta_minus_1, cache);
+
+  // Step 2: Try larger divisor; remove trailing zeros if necessary
+
+  // Using an upper bound on zi, we might be able to optimize the division
+  // better than the compiler; we are computing zi / big_divisor here
+  decimal_fp<T> ret_value;
+  ret_value.significand = divide_by_10_to_kappa_plus_1(zi);
+  uint32_t r = static_cast<uint32_t>(zi - float_info<T>::big_divisor *
+                                              ret_value.significand);
+
+  if (r > deltai) {
+    goto small_divisor_case_label;
+  } else if (r < deltai) {
+    // Exclude the right endpoint if necessary
+    if (r == 0 && !include_right_endpoint &&
+        is_endpoint_integer<T>(two_fr, exponent, minus_k)) {
+      --ret_value.significand;
+      r = float_info<T>::big_divisor;
+      goto small_divisor_case_label;
+    }
+  } else {
+    // r == deltai; compare fractional parts
+    // Check conditions in the order different from the paper
+    // to take advantage of short-circuiting
+    const carrier_uint two_fl = two_fc - 1;
+    if ((!include_left_endpoint ||
+         !is_endpoint_integer<T>(two_fl, exponent, minus_k)) &&
+        !cache_accessor<T>::compute_mul_parity(two_fl, cache, beta_minus_1)) {
+      goto small_divisor_case_label;
+    }
+  }
+  ret_value.exponent = minus_k + float_info<T>::kappa + 1;
+
+  // We may need to remove trailing zeros
+  ret_value.exponent += remove_trailing_zeros(ret_value.significand);
+  return ret_value;
+
+  // Step 3: Find the significand with the smaller divisor
+
+small_divisor_case_label:
+  ret_value.significand *= 10;
+  ret_value.exponent = minus_k + float_info<T>::kappa;
+
+  const uint32_t mask = (1u << float_info<T>::kappa) - 1;
+  auto dist = r - (deltai / 2) + (float_info<T>::small_divisor / 2);
+
+  // Is dist divisible by 2^kappa?
+  if ((dist & mask) == 0) {
+    const bool approx_y_parity =
+        ((dist ^ (float_info<T>::small_divisor / 2)) & 1) != 0;
+    dist >>= float_info<T>::kappa;
+
+    // Is dist divisible by 5^kappa?
+    if (check_divisibility_and_divide_by_pow5<float_info<T>::kappa>(dist)) {
+      ret_value.significand += dist;
+
+      // Check z^(f) >= epsilon^(f)
+      // We have either yi == zi - epsiloni or yi == (zi - epsiloni) - 1,
+      // where yi == zi - epsiloni if and only if z^(f) >= epsilon^(f)
+      // Since there are only 2 possibilities, we only need to care about the
+      // parity. Also, zi and r should have the same parity since the divisor
+      // is an even number
+      if (cache_accessor<T>::compute_mul_parity(two_fc, cache, beta_minus_1) !=
+          approx_y_parity) {
+        --ret_value.significand;
+      } else {
+        // If z^(f) >= epsilon^(f), we might have a tie
+        // when z^(f) == epsilon^(f), or equivalently, when y is an integer
+        if (is_center_integer<T>(two_fc, exponent, minus_k)) {
+          ret_value.significand = ret_value.significand % 2 == 0
+                                      ? ret_value.significand
+                                      : ret_value.significand - 1;
+        }
+      }
+    }
+    // Is dist not divisible by 5^kappa?
+    else {
+      ret_value.significand += dist;
+    }
+  }
+  // Is dist not divisible by 2^kappa?
+  else {
+    // Since we know dist is small, we might be able to optimize the division
+    // better than the compiler; we are computing dist / small_divisor here
+    ret_value.significand +=
+        small_division_by_pow10<float_info<T>::kappa>(dist);
+  }
+  return ret_value;
+}
+}  // namespace dragonbox
+
+// Formats value using a variation of the Fixed-Precision Positive
+// Floating-Point Printout ((FPP)^2) algorithm by Steele & White:
+// https://fmt.dev/papers/p372-steele.pdf.
+template <typename Double>
+void fallback_format(Double d, int num_digits, bool binary32, buffer<char>& buf,
+                     int& exp10) {
+  bigint numerator;    // 2 * R in (FPP)^2.
+  bigint denominator;  // 2 * S in (FPP)^2.
+  // lower and upper are differences between value and corresponding boundaries.
+  bigint lower;             // (M^- in (FPP)^2).
+  bigint upper_store;       // upper's value if different from lower.
+  bigint* upper = nullptr;  // (M^+ in (FPP)^2).
+  fp value;
+  // Shift numerator and denominator by an extra bit or two (if lower boundary
+  // is closer) to make lower and upper integers. This eliminates multiplication
+  // by 2 during later computations.
+  const bool is_predecessor_closer =
+      binary32 ? value.assign(static_cast<float>(d)) : value.assign(d);
+  int shift = is_predecessor_closer ? 2 : 1;
+  uint64_t significand = value.f << shift;
+  if (value.e >= 0) {
+    numerator.assign(significand);
+    numerator <<= value.e;
+    lower.assign(1);
+    lower <<= value.e;
+    if (shift != 1) {
+      upper_store.assign(1);
+      upper_store <<= value.e + 1;
+      upper = &upper_store;
+    }
+    denominator.assign_pow10(exp10);
+    denominator <<= shift;
+  } else if (exp10 < 0) {
+    numerator.assign_pow10(-exp10);
+    lower.assign(numerator);
+    if (shift != 1) {
+      upper_store.assign(numerator);
+      upper_store <<= 1;
+      upper = &upper_store;
+    }
+    numerator *= significand;
+    denominator.assign(1);
+    denominator <<= shift - value.e;
+  } else {
+    numerator.assign(significand);
+    denominator.assign_pow10(exp10);
+    denominator <<= shift - value.e;
+    lower.assign(1);
+    if (shift != 1) {
+      upper_store.assign(1ULL << 1);
+      upper = &upper_store;
+    }
+  }
+  // Invariant: value == (numerator / denominator) * pow(10, exp10).
+  if (num_digits < 0) {
+    // Generate the shortest representation.
+    if (!upper) upper = &lower;
+    bool even = (value.f & 1) == 0;
+    num_digits = 0;
+    char* data = buf.data();
+    for (;;) {
+      int digit = numerator.divmod_assign(denominator);
+      bool low = compare(numerator, lower) - even < 0;  // numerator <[=] lower.
+      // numerator + upper >[=] pow10:
+      bool high = add_compare(numerator, *upper, denominator) + even > 0;
+      data[num_digits++] = static_cast<char>('0' + digit);
+      if (low || high) {
+        if (!low) {
+          ++data[num_digits - 1];
+        } else if (high) {
+          int result = add_compare(numerator, numerator, denominator);
+          // Round half to even.
+          if (result > 0 || (result == 0 && (digit % 2) != 0))
+            ++data[num_digits - 1];
+        }
+        buf.try_resize(to_unsigned(num_digits));
+        exp10 -= num_digits - 1;
+        return;
+      }
+      numerator *= 10;
+      lower *= 10;
+      if (upper != &lower) *upper *= 10;
+    }
+  }
+  // Generate the given number of digits.
+  exp10 -= num_digits - 1;
+  if (num_digits == 0) {
+    buf.try_resize(1);
+    denominator *= 10;
+    buf[0] = add_compare(numerator, numerator, denominator) > 0 ? '1' : '0';
+    return;
+  }
+  buf.try_resize(to_unsigned(num_digits));
+  for (int i = 0; i < num_digits - 1; ++i) {
+    int digit = numerator.divmod_assign(denominator);
+    buf[i] = static_cast<char>('0' + digit);
+    numerator *= 10;
+  }
+  int digit = numerator.divmod_assign(denominator);
+  auto result = add_compare(numerator, numerator, denominator);
+  if (result > 0 || (result == 0 && (digit % 2) != 0)) {
+    if (digit == 9) {
+      const auto overflow = '0' + 10;
+      buf[num_digits - 1] = overflow;
+      // Propagate the carry.
+      for (int i = num_digits - 1; i > 0 && buf[i] == overflow; --i) {
+        buf[i] = '0';
+        ++buf[i - 1];
+      }
+      if (buf[0] == overflow) {
+        buf[0] = '1';
+        ++exp10;
+      }
+      return;
+    }
+    ++digit;
+  }
+  buf[num_digits - 1] = static_cast<char>('0' + digit);
+}
+
+template <typename T>
+int format_float(T value, int precision, float_specs specs, buffer<char>& buf) {
+  static_assert(!std::is_same<T, float>::value, "");
+  FMT_ASSERT(value >= 0, "value is negative");
+
+  const bool fixed = specs.format == float_format::fixed;
+  if (value <= 0) {  // <= instead of == to silence a warning.
+    if (precision <= 0 || !fixed) {
+      buf.push_back('0');
+      return 0;
+    }
+    buf.try_resize(to_unsigned(precision));
+    std::uninitialized_fill_n(buf.data(), precision, '0');
+    return -precision;
+  }
+
+  if (!specs.use_grisu) return snprintf_float(value, precision, specs, buf);
+
+  if (precision < 0) {
+    // Use Dragonbox for the shortest format.
+    if (specs.binary32) {
+      auto dec = dragonbox::to_decimal(static_cast<float>(value));
+      write<char>(buffer_appender<char>(buf), dec.significand);
+      return dec.exponent;
+    }
+    auto dec = dragonbox::to_decimal(static_cast<double>(value));
+    write<char>(buffer_appender<char>(buf), dec.significand);
+    return dec.exponent;
+  }
+
+  // Use Grisu + Dragon4 for the given precision:
+  // https://www.cs.tufts.edu/~nr/cs257/archive/florian-loitsch/printf.pdf.
+  int exp = 0;
+  const int min_exp = -60;  // alpha in Grisu.
+  int cached_exp10 = 0;     // K in Grisu.
+  fp normalized = normalize(fp(value));
+  const auto cached_pow = get_cached_power(
+      min_exp - (normalized.e + fp::significand_size), cached_exp10);
+  normalized = normalized * cached_pow;
+  // Limit precision to the maximum possible number of significant digits in an
+  // IEEE754 double because we don't need to generate zeros.
+  const int max_double_digits = 767;
+  if (precision > max_double_digits) precision = max_double_digits;
+  fixed_handler handler{buf.data(), 0, precision, -cached_exp10, fixed};
+  if (grisu_gen_digits(normalized, 1, exp, handler) == digits::error) {
+    exp += handler.size - cached_exp10 - 1;
+    fallback_format(value, handler.precision, specs.binary32, buf, exp);
+  } else {
+    exp += handler.exp10;
+    buf.try_resize(to_unsigned(handler.size));
+  }
+  if (!fixed && !specs.showpoint) {
+    // Remove trailing zeros.
+    auto num_digits = buf.size();
+    while (num_digits > 0 && buf[num_digits - 1] == '0') {
+      --num_digits;
+      ++exp;
+    }
+    buf.try_resize(num_digits);
+  }
+  return exp;
+}  // namespace detail
+
+template <typename T>
+int snprintf_float(T value, int precision, float_specs specs,
+                   buffer<char>& buf) {
+  // Buffer capacity must be non-zero, otherwise MSVC's vsnprintf_s will fail.
+  FMT_ASSERT(buf.capacity() > buf.size(), "empty buffer");
+  static_assert(!std::is_same<T, float>::value, "");
+
+  // Subtract 1 to account for the difference in precision since we use %e for
+  // both general and exponent format.
+  if (specs.format == float_format::general ||
+      specs.format == float_format::exp)
+    precision = (precision >= 0 ? precision : 6) - 1;
+
+  // Build the format string.
+  enum { max_format_size = 7 };  // The longest format is "%#.*Le".
+  char format[max_format_size];
+  char* format_ptr = format;
+  *format_ptr++ = '%';
+  if (specs.showpoint && specs.format == float_format::hex) *format_ptr++ = '#';
+  if (precision >= 0) {
+    *format_ptr++ = '.';
+    *format_ptr++ = '*';
+  }
+  if (std::is_same<T, long double>()) *format_ptr++ = 'L';
+  *format_ptr++ = specs.format != float_format::hex
+                      ? (specs.format == float_format::fixed ? 'f' : 'e')
+                      : (specs.upper ? 'A' : 'a');
+  *format_ptr = '\0';
+
+  // Format using snprintf.
+  auto offset = buf.size();
+  for (;;) {
+    auto begin = buf.data() + offset;
+    auto capacity = buf.capacity() - offset;
+#ifdef FMT_FUZZ
+    if (precision > 100000)
+      throw std::runtime_error(
+          "fuzz mode - avoid large allocation inside snprintf");
+#endif
+    // Suppress the warning about a nonliteral format string.
+    // Cannot use auto because of a bug in MinGW (#1532).
+    int (*snprintf_ptr)(char*, size_t, const char*, ...) = FMT_SNPRINTF;
+    int result = precision >= 0
+                     ? snprintf_ptr(begin, capacity, format, precision, value)
+                     : snprintf_ptr(begin, capacity, format, value);
+    if (result < 0) {
+      // The buffer will grow exponentially.
+      buf.try_reserve(buf.capacity() + 1);
+      continue;
+    }
+    auto size = to_unsigned(result);
+    // Size equal to capacity means that the last character was truncated.
+    if (size >= capacity) {
+      buf.try_reserve(size + offset + 1);  // Add 1 for the terminating '\0'.
+      continue;
+    }
+    auto is_digit = [](char c) { return c >= '0' && c <= '9'; };
+    if (specs.format == float_format::fixed) {
+      if (precision == 0) {
+        buf.try_resize(size);
+        return 0;
+      }
+      // Find and remove the decimal point.
+      auto end = begin + size, p = end;
+      do {
+        --p;
+      } while (is_digit(*p));
+      int fraction_size = static_cast<int>(end - p - 1);
+      std::memmove(p, p + 1, to_unsigned(fraction_size));
+      buf.try_resize(size - 1);
+      return -fraction_size;
+    }
+    if (specs.format == float_format::hex) {
+      buf.try_resize(size + offset);
+      return 0;
+    }
+    // Find and parse the exponent.
+    auto end = begin + size, exp_pos = end;
+    do {
+      --exp_pos;
+    } while (*exp_pos != 'e');
+    char sign = exp_pos[1];
+    FMT_ASSERT(sign == '+' || sign == '-', "");
+    int exp = 0;
+    auto p = exp_pos + 2;  // Skip 'e' and sign.
+    do {
+      FMT_ASSERT(is_digit(*p), "");
+      exp = exp * 10 + (*p++ - '0');
+    } while (p != end);
+    if (sign == '-') exp = -exp;
+    int fraction_size = 0;
+    if (exp_pos != begin + 1) {
+      // Remove trailing zeros.
+      auto fraction_end = exp_pos - 1;
+      while (*fraction_end == '0') --fraction_end;
+      // Move the fractional part left to get rid of the decimal point.
+      fraction_size = static_cast<int>(fraction_end - begin - 1);
+      std::memmove(begin + 1, begin + 2, to_unsigned(fraction_size));
+    }
+    buf.try_resize(to_unsigned(fraction_size) + offset + 1);
+    return exp - fraction_size;
+  }
+}
+}  // namespace detail
+
+template <> struct formatter<detail::bigint> {
+  FMT_CONSTEXPR format_parse_context::iterator parse(
+      format_parse_context& ctx) {
+    return ctx.begin();
+  }
+
+  format_context::iterator format(const detail::bigint& n,
+                                  format_context& ctx) {
+    auto out = ctx.out();
+    bool first = true;
+    for (auto i = n.bigits_.size(); i > 0; --i) {
+      auto value = n.bigits_[i - 1u];
+      if (first) {
+        out = format_to(out, FMT_STRING("{:x}"), value);
+        first = false;
+        continue;
+      }
+      out = format_to(out, FMT_STRING("{:08x}"), value);
+    }
+    if (n.exp_ > 0)
+      out = format_to(out, FMT_STRING("p{}"),
+                      n.exp_ * detail::bigint::bigit_bits);
+    return out;
+  }
+};
+
+FMT_FUNC detail::utf8_to_utf16::utf8_to_utf16(string_view s) {
+  for_each_codepoint(s, [this](uint32_t cp, int error) {
+    if (error != 0) FMT_THROW(std::runtime_error("invalid utf8"));
+    if (cp <= 0xFFFF) {
+      buffer_.push_back(static_cast<wchar_t>(cp));
+    } else {
+      cp -= 0x10000;
+      buffer_.push_back(static_cast<wchar_t>(0xD800 + (cp >> 10)));
+      buffer_.push_back(static_cast<wchar_t>(0xDC00 + (cp & 0x3FF)));
+    }
+  });
+  buffer_.push_back(0);
+}
+
+FMT_FUNC void format_system_error(detail::buffer<char>& out, int error_code,
+                                  const char* message) FMT_NOEXCEPT {
+  FMT_TRY {
+    auto ec = std::error_code(error_code, std::generic_category());
+    write(std::back_inserter(out), std::system_error(ec, message).what());
+    return;
+  }
+  FMT_CATCH(...) {}
+  format_error_code(out, error_code, message);
+}
+
+FMT_FUNC void detail::error_handler::on_error(const char* message) {
+  FMT_THROW(format_error(message));
+}
+
+FMT_FUNC void report_system_error(int error_code,
+                                  const char* message) FMT_NOEXCEPT {
+  report_error(format_system_error, error_code, message);
+}
+
+FMT_FUNC std::string vformat(string_view fmt, format_args args) {
+  // Don't optimize the "{}" case to keep the binary size small and because it
+  // can be better optimized in fmt::format anyway.
+  auto buffer = memory_buffer();
+  detail::vformat_to(buffer, fmt, args);
+  return to_string(buffer);
+}
+
+#ifdef _WIN32
+namespace detail {
+using dword = conditional_t<sizeof(long) == 4, unsigned long, unsigned>;
+extern "C" __declspec(dllimport) int __stdcall WriteConsoleW(  //
+    void*, const void*, dword, dword*, void*);
+}  // namespace detail
+#endif
+
+namespace detail {
+FMT_FUNC void print(std::FILE* f, string_view text) {
+#ifdef _WIN32
+  auto fd = _fileno(f);
+  if (_isatty(fd)) {
+    detail::utf8_to_utf16 u16(string_view(text.data(), text.size()));
+    auto written = detail::dword();
+    if (detail::WriteConsoleW(reinterpret_cast<void*>(_get_osfhandle(fd)),
+                              u16.c_str(), static_cast<uint32_t>(u16.size()),
+                              &written, nullptr)) {
+      return;
+    }
+    // Fallback to fwrite on failure. It can happen if the output has been
+    // redirected to NUL.
+  }
+#endif
+  detail::fwrite_fully(text.data(), 1, text.size(), f);
+}
+}  // namespace detail
+
+FMT_FUNC void vprint(std::FILE* f, string_view format_str, format_args args) {
+  memory_buffer buffer;
+  detail::vformat_to(buffer, format_str, args);
+  detail::print(f, {buffer.data(), buffer.size()});
+}
+
+#ifdef _WIN32
+// Print assuming legacy (non-Unicode) encoding.
+FMT_FUNC void detail::vprint_mojibake(std::FILE* f, string_view format_str,
+                                      format_args args) {
+  memory_buffer buffer;
+  detail::vformat_to(buffer, format_str,
+                     basic_format_args<buffer_context<char>>(args));
+  fwrite_fully(buffer.data(), 1, buffer.size(), f);
+}
+#endif
+
+FMT_FUNC void vprint(string_view format_str, format_args args) {
+  vprint(stdout, format_str, args);
+}
+
+FMT_END_NAMESPACE
+
+#endif  // FMT_FORMAT_INL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/format.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/format.h
new file mode 100644
index 0000000..5398a23
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/format.h
@@ -0,0 +1,2830 @@
+/*
+ Formatting library for C++
+
+ Copyright (c) 2012 - present, Victor Zverovich
+
+ 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.
+
+ --- Optional exception to the license ---
+
+ As an exception, if, as a result of your compiling your source code, portions
+ of this Software are embedded into a machine-executable object form of such
+ source code, you may redistribute such embedded portions in such object form
+ without including the above copyright and permission notices.
+ */
+
+#ifndef FMT_FORMAT_H_
+#define FMT_FORMAT_H_
+
+#include <cmath>         // std::signbit
+#include <cstdint>       // uint32_t
+#include <limits>        // std::numeric_limits
+#include <memory>        // std::uninitialized_copy
+#include <stdexcept>     // std::runtime_error
+#include <system_error>  // std::system_error
+#include <utility>       // std::swap
+
+#include "core.h"
+
+#ifdef __INTEL_COMPILER
+#  define FMT_ICC_VERSION __INTEL_COMPILER
+#elif defined(__ICL)
+#  define FMT_ICC_VERSION __ICL
+#else
+#  define FMT_ICC_VERSION 0
+#endif
+
+#ifdef __NVCC__
+#  define FMT_CUDA_VERSION (__CUDACC_VER_MAJOR__ * 100 + __CUDACC_VER_MINOR__)
+#else
+#  define FMT_CUDA_VERSION 0
+#endif
+
+#ifdef __has_builtin
+#  define FMT_HAS_BUILTIN(x) __has_builtin(x)
+#else
+#  define FMT_HAS_BUILTIN(x) 0
+#endif
+
+#if FMT_GCC_VERSION || FMT_CLANG_VERSION
+#  define FMT_NOINLINE __attribute__((noinline))
+#else
+#  define FMT_NOINLINE
+#endif
+
+#if FMT_MSC_VER
+#  define FMT_MSC_DEFAULT = default
+#else
+#  define FMT_MSC_DEFAULT
+#endif
+
+#ifndef FMT_THROW
+#  if FMT_EXCEPTIONS
+#    if FMT_MSC_VER || FMT_NVCC
+FMT_BEGIN_NAMESPACE
+namespace detail {
+template <typename Exception> inline void do_throw(const Exception& x) {
+  // Silence unreachable code warnings in MSVC and NVCC because these
+  // are nearly impossible to fix in a generic code.
+  volatile bool b = true;
+  if (b) throw x;
+}
+}  // namespace detail
+FMT_END_NAMESPACE
+#      define FMT_THROW(x) detail::do_throw(x)
+#    else
+#      define FMT_THROW(x) throw x
+#    endif
+#  else
+#    define FMT_THROW(x)               \
+      do {                             \
+        FMT_ASSERT(false, (x).what()); \
+      } while (false)
+#  endif
+#endif
+
+#if FMT_EXCEPTIONS
+#  define FMT_TRY try
+#  define FMT_CATCH(x) catch (x)
+#else
+#  define FMT_TRY if (true)
+#  define FMT_CATCH(x) if (false)
+#endif
+
+#ifndef FMT_DEPRECATED
+#  if FMT_HAS_CPP14_ATTRIBUTE(deprecated) || FMT_MSC_VER >= 1900
+#    define FMT_DEPRECATED [[deprecated]]
+#  else
+#    if (defined(__GNUC__) && !defined(__LCC__)) || defined(__clang__)
+#      define FMT_DEPRECATED __attribute__((deprecated))
+#    elif FMT_MSC_VER
+#      define FMT_DEPRECATED __declspec(deprecated)
+#    else
+#      define FMT_DEPRECATED /* deprecated */
+#    endif
+#  endif
+#endif
+
+// Workaround broken [[deprecated]] in the Intel, PGI and NVCC compilers.
+#if FMT_ICC_VERSION || defined(__PGI) || FMT_NVCC
+#  define FMT_DEPRECATED_ALIAS
+#else
+#  define FMT_DEPRECATED_ALIAS FMT_DEPRECATED
+#endif
+
+#ifndef FMT_USE_USER_DEFINED_LITERALS
+// EDG based compilers (Intel, NVIDIA, Elbrus, etc), GCC and MSVC support UDLs.
+#  if (FMT_HAS_FEATURE(cxx_user_literals) || FMT_GCC_VERSION >= 407 || \
+       FMT_MSC_VER >= 1900) &&                                         \
+      (!defined(__EDG_VERSION__) || __EDG_VERSION__ >= /* UDL feature */ 480)
+#    define FMT_USE_USER_DEFINED_LITERALS 1
+#  else
+#    define FMT_USE_USER_DEFINED_LITERALS 0
+#  endif
+#endif
+
+// Defining FMT_REDUCE_INT_INSTANTIATIONS to 1, will reduce the number of
+// integer formatter template instantiations to just one by only using the
+// largest integer type. This results in a reduction in binary size but will
+// cause a decrease in integer formatting performance.
+#if !defined(FMT_REDUCE_INT_INSTANTIATIONS)
+#  define FMT_REDUCE_INT_INSTANTIATIONS 0
+#endif
+
+// __builtin_clz is broken in clang with Microsoft CodeGen:
+// https://github.com/fmtlib/fmt/issues/519
+#if (FMT_GCC_VERSION || FMT_HAS_BUILTIN(__builtin_clz)) && !FMT_MSC_VER
+#  define FMT_BUILTIN_CLZ(n) __builtin_clz(n)
+#endif
+#if (FMT_GCC_VERSION || FMT_HAS_BUILTIN(__builtin_clzll)) && !FMT_MSC_VER
+#  define FMT_BUILTIN_CLZLL(n) __builtin_clzll(n)
+#endif
+#if (FMT_GCC_VERSION || FMT_HAS_BUILTIN(__builtin_ctz))
+#  define FMT_BUILTIN_CTZ(n) __builtin_ctz(n)
+#endif
+#if (FMT_GCC_VERSION || FMT_HAS_BUILTIN(__builtin_ctzll))
+#  define FMT_BUILTIN_CTZLL(n) __builtin_ctzll(n)
+#endif
+
+#if FMT_MSC_VER
+#  include <intrin.h>  // _BitScanReverse[64], _BitScanForward[64], _umul128
+#endif
+
+// Some compilers masquerade as both MSVC and GCC-likes or otherwise support
+// __builtin_clz and __builtin_clzll, so only define FMT_BUILTIN_CLZ using the
+// MSVC intrinsics if the clz and clzll builtins are not available.
+#if FMT_MSC_VER && !defined(FMT_BUILTIN_CLZLL) && !defined(FMT_BUILTIN_CTZLL)
+FMT_BEGIN_NAMESPACE
+namespace detail {
+// Avoid Clang with Microsoft CodeGen's -Wunknown-pragmas warning.
+#  if !defined(__clang__)
+#    pragma managed(push, off)
+#    pragma intrinsic(_BitScanForward)
+#    pragma intrinsic(_BitScanReverse)
+#    if defined(_WIN64)
+#      pragma intrinsic(_BitScanForward64)
+#      pragma intrinsic(_BitScanReverse64)
+#    endif
+#  endif
+
+inline auto clz(uint32_t x) -> int {
+  unsigned long r = 0;
+  _BitScanReverse(&r, x);
+  FMT_ASSERT(x != 0, "");
+  // Static analysis complains about using uninitialized data
+  // "r", but the only way that can happen is if "x" is 0,
+  // which the callers guarantee to not happen.
+  FMT_MSC_WARNING(suppress : 6102)
+  return 31 ^ static_cast<int>(r);
+}
+#  define FMT_BUILTIN_CLZ(n) detail::clz(n)
+
+inline auto clzll(uint64_t x) -> int {
+  unsigned long r = 0;
+#  ifdef _WIN64
+  _BitScanReverse64(&r, x);
+#  else
+  // Scan the high 32 bits.
+  if (_BitScanReverse(&r, static_cast<uint32_t>(x >> 32))) return 63 ^ (r + 32);
+  // Scan the low 32 bits.
+  _BitScanReverse(&r, static_cast<uint32_t>(x));
+#  endif
+  FMT_ASSERT(x != 0, "");
+  FMT_MSC_WARNING(suppress : 6102)  // Suppress a bogus static analysis warning.
+  return 63 ^ static_cast<int>(r);
+}
+#  define FMT_BUILTIN_CLZLL(n) detail::clzll(n)
+
+inline auto ctz(uint32_t x) -> int {
+  unsigned long r = 0;
+  _BitScanForward(&r, x);
+  FMT_ASSERT(x != 0, "");
+  FMT_MSC_WARNING(suppress : 6102)  // Suppress a bogus static analysis warning.
+  return static_cast<int>(r);
+}
+#  define FMT_BUILTIN_CTZ(n) detail::ctz(n)
+
+inline auto ctzll(uint64_t x) -> int {
+  unsigned long r = 0;
+  FMT_ASSERT(x != 0, "");
+  FMT_MSC_WARNING(suppress : 6102)  // Suppress a bogus static analysis warning.
+#  ifdef _WIN64
+  _BitScanForward64(&r, x);
+#  else
+  // Scan the low 32 bits.
+  if (_BitScanForward(&r, static_cast<uint32_t>(x))) return static_cast<int>(r);
+  // Scan the high 32 bits.
+  _BitScanForward(&r, static_cast<uint32_t>(x >> 32));
+  r += 32;
+#  endif
+  return static_cast<int>(r);
+}
+#  define FMT_BUILTIN_CTZLL(n) detail::ctzll(n)
+#  if !defined(__clang__)
+#    pragma managed(pop)
+#  endif
+}  // namespace detail
+FMT_END_NAMESPACE
+#endif
+
+FMT_BEGIN_NAMESPACE
+namespace detail {
+
+#if __cplusplus >= 202002L || \
+    (__cplusplus >= 201709L && FMT_GCC_VERSION >= 1002)
+#  define FMT_CONSTEXPR20 constexpr
+#else
+#  define FMT_CONSTEXPR20
+#endif
+
+// An equivalent of `*reinterpret_cast<Dest*>(&source)` that doesn't have
+// undefined behavior (e.g. due to type aliasing).
+// Example: uint64_t d = bit_cast<uint64_t>(2.718);
+template <typename Dest, typename Source>
+inline auto bit_cast(const Source& source) -> Dest {
+  static_assert(sizeof(Dest) == sizeof(Source), "size mismatch");
+  Dest dest;
+  std::memcpy(&dest, &source, sizeof(dest));
+  return dest;
+}
+
+inline auto is_big_endian() -> bool {
+  const auto u = 1u;
+  struct bytes {
+    char data[sizeof(u)];
+  };
+  return bit_cast<bytes>(u).data[0] == 0;
+}
+
+// A fallback implementation of uintptr_t for systems that lack it.
+struct fallback_uintptr {
+  unsigned char value[sizeof(void*)];
+
+  fallback_uintptr() = default;
+  explicit fallback_uintptr(const void* p) {
+    *this = bit_cast<fallback_uintptr>(p);
+    if (is_big_endian()) {
+      for (size_t i = 0, j = sizeof(void*) - 1; i < j; ++i, --j)
+        std::swap(value[i], value[j]);
+    }
+  }
+};
+#ifdef UINTPTR_MAX
+using uintptr_t = ::uintptr_t;
+inline auto to_uintptr(const void* p) -> uintptr_t {
+  return bit_cast<uintptr_t>(p);
+}
+#else
+using uintptr_t = fallback_uintptr;
+inline auto to_uintptr(const void* p) -> fallback_uintptr {
+  return fallback_uintptr(p);
+}
+#endif
+
+// Returns the largest possible value for type T. Same as
+// std::numeric_limits<T>::max() but shorter and not affected by the max macro.
+template <typename T> constexpr auto max_value() -> T {
+  return (std::numeric_limits<T>::max)();
+}
+template <typename T> constexpr auto num_bits() -> int {
+  return std::numeric_limits<T>::digits;
+}
+// std::numeric_limits<T>::digits may return 0 for 128-bit ints.
+template <> constexpr auto num_bits<int128_t>() -> int { return 128; }
+template <> constexpr auto num_bits<uint128_t>() -> int { return 128; }
+template <> constexpr auto num_bits<fallback_uintptr>() -> int {
+  return static_cast<int>(sizeof(void*) *
+                          std::numeric_limits<unsigned char>::digits);
+}
+
+FMT_INLINE void assume(bool condition) {
+  (void)condition;
+#if FMT_HAS_BUILTIN(__builtin_assume)
+  __builtin_assume(condition);
+#endif
+}
+
+// An approximation of iterator_t for pre-C++20 systems.
+template <typename T>
+using iterator_t = decltype(std::begin(std::declval<T&>()));
+template <typename T> using sentinel_t = decltype(std::end(std::declval<T&>()));
+
+// A workaround for std::string not having mutable data() until C++17.
+template <typename Char>
+inline auto get_data(std::basic_string<Char>& s) -> Char* {
+  return &s[0];
+}
+template <typename Container>
+inline auto get_data(Container& c) -> typename Container::value_type* {
+  return c.data();
+}
+
+#if defined(_SECURE_SCL) && _SECURE_SCL
+// Make a checked iterator to avoid MSVC warnings.
+template <typename T> using checked_ptr = stdext::checked_array_iterator<T*>;
+template <typename T> auto make_checked(T* p, size_t size) -> checked_ptr<T> {
+  return {p, size};
+}
+#else
+template <typename T> using checked_ptr = T*;
+template <typename T> inline auto make_checked(T* p, size_t) -> T* { return p; }
+#endif
+
+// Attempts to reserve space for n extra characters in the output range.
+// Returns a pointer to the reserved range or a reference to it.
+template <typename Container, FMT_ENABLE_IF(is_contiguous<Container>::value)>
+#if FMT_CLANG_VERSION >= 307 && !FMT_ICC_VERSION
+__attribute__((no_sanitize("undefined")))
+#endif
+inline auto
+reserve(std::back_insert_iterator<Container> it, size_t n)
+    -> checked_ptr<typename Container::value_type> {
+  Container& c = get_container(it);
+  size_t size = c.size();
+  c.resize(size + n);
+  return make_checked(get_data(c) + size, n);
+}
+
+template <typename T>
+inline auto reserve(buffer_appender<T> it, size_t n) -> buffer_appender<T> {
+  buffer<T>& buf = get_container(it);
+  buf.try_reserve(buf.size() + n);
+  return it;
+}
+
+template <typename Iterator>
+constexpr auto reserve(Iterator& it, size_t) -> Iterator& {
+  return it;
+}
+
+template <typename OutputIt>
+using reserve_iterator =
+    remove_reference_t<decltype(reserve(std::declval<OutputIt&>(), 0))>;
+
+template <typename T, typename OutputIt>
+constexpr auto to_pointer(OutputIt, size_t) -> T* {
+  return nullptr;
+}
+template <typename T> auto to_pointer(buffer_appender<T> it, size_t n) -> T* {
+  buffer<T>& buf = get_container(it);
+  auto size = buf.size();
+  if (buf.capacity() < size + n) return nullptr;
+  buf.try_resize(size + n);
+  return buf.data() + size;
+}
+
+template <typename Container, FMT_ENABLE_IF(is_contiguous<Container>::value)>
+inline auto base_iterator(std::back_insert_iterator<Container>& it,
+                          checked_ptr<typename Container::value_type>)
+    -> std::back_insert_iterator<Container> {
+  return it;
+}
+
+template <typename Iterator>
+constexpr auto base_iterator(Iterator, Iterator it) -> Iterator {
+  return it;
+}
+
+// <algorithm> is spectacularly slow to compile in C++20 so use a simple fill_n
+// instead (#1998).
+template <typename OutputIt, typename Size, typename T>
+FMT_CONSTEXPR auto fill_n(OutputIt out, Size count, const T& value)
+    -> OutputIt {
+  for (Size i = 0; i < count; ++i) *out++ = value;
+  return out;
+}
+template <typename T, typename Size>
+FMT_CONSTEXPR20 auto fill_n(T* out, Size count, char value) -> T* {
+  if (is_constant_evaluated()) {
+    return fill_n<T*, Size, T>(out, count, value);
+  }
+  std::memset(out, value, to_unsigned(count));
+  return out + count;
+}
+
+#ifdef __cpp_char8_t
+using char8_type = char8_t;
+#else
+enum char8_type : unsigned char {};
+#endif
+
+template <typename OutChar, typename InputIt, typename OutputIt>
+FMT_CONSTEXPR FMT_NOINLINE auto copy_str_noinline(InputIt begin, InputIt end,
+                                                  OutputIt out) -> OutputIt {
+  return copy_str<OutChar>(begin, end, out);
+}
+
+// A public domain branchless UTF-8 decoder by Christopher Wellons:
+// https://github.com/skeeto/branchless-utf8
+/* Decode the next character, c, from s, reporting errors in e.
+ *
+ * Since this is a branchless decoder, four bytes will be read from the
+ * buffer regardless of the actual length of the next character. This
+ * means the buffer _must_ have at least three bytes of zero padding
+ * following the end of the data stream.
+ *
+ * Errors are reported in e, which will be non-zero if the parsed
+ * character was somehow invalid: invalid byte sequence, non-canonical
+ * encoding, or a surrogate half.
+ *
+ * The function returns a pointer to the next character. When an error
+ * occurs, this pointer will be a guess that depends on the particular
+ * error, but it will always advance at least one byte.
+ */
+FMT_CONSTEXPR inline auto utf8_decode(const char* s, uint32_t* c, int* e)
+    -> const char* {
+  constexpr const int masks[] = {0x00, 0x7f, 0x1f, 0x0f, 0x07};
+  constexpr const uint32_t mins[] = {4194304, 0, 128, 2048, 65536};
+  constexpr const int shiftc[] = {0, 18, 12, 6, 0};
+  constexpr const int shifte[] = {0, 6, 4, 2, 0};
+
+  int len = code_point_length(s);
+  const char* next = s + len;
+
+  // Assume a four-byte character and load four bytes. Unused bits are
+  // shifted out.
+  *c = uint32_t(s[0] & masks[len]) << 18;
+  *c |= uint32_t(s[1] & 0x3f) << 12;
+  *c |= uint32_t(s[2] & 0x3f) << 6;
+  *c |= uint32_t(s[3] & 0x3f) << 0;
+  *c >>= shiftc[len];
+
+  // Accumulate the various error conditions.
+  using uchar = unsigned char;
+  *e = (*c < mins[len]) << 6;       // non-canonical encoding
+  *e |= ((*c >> 11) == 0x1b) << 7;  // surrogate half?
+  *e |= (*c > 0x10FFFF) << 8;       // out of range?
+  *e |= (uchar(s[1]) & 0xc0) >> 2;
+  *e |= (uchar(s[2]) & 0xc0) >> 4;
+  *e |= uchar(s[3]) >> 6;
+  *e ^= 0x2a;  // top two bits of each tail byte correct?
+  *e >>= shifte[len];
+
+  return next;
+}
+
+template <typename F>
+FMT_CONSTEXPR void for_each_codepoint(string_view s, F f) {
+  auto decode = [f](const char* p) {
+    auto cp = uint32_t();
+    auto error = 0;
+    p = utf8_decode(p, &cp, &error);
+    f(cp, error);
+    return p;
+  };
+  auto p = s.data();
+  const size_t block_size = 4;  // utf8_decode always reads blocks of 4 chars.
+  if (s.size() >= block_size) {
+    for (auto end = p + s.size() - block_size + 1; p < end;) p = decode(p);
+  }
+  if (auto num_chars_left = s.data() + s.size() - p) {
+    char buf[2 * block_size - 1] = {};
+    copy_str<char>(p, p + num_chars_left, buf);
+    p = buf;
+    do {
+      p = decode(p);
+    } while (p - buf < num_chars_left);
+  }
+}
+
+template <typename Char>
+inline auto compute_width(basic_string_view<Char> s) -> size_t {
+  return s.size();
+}
+
+// Computes approximate display width of a UTF-8 string.
+FMT_CONSTEXPR inline size_t compute_width(string_view s) {
+  size_t num_code_points = 0;
+  // It is not a lambda for compatibility with C++14.
+  struct count_code_points {
+    size_t* count;
+    FMT_CONSTEXPR void operator()(uint32_t cp, int error) const {
+      *count += detail::to_unsigned(
+          1 +
+          (error == 0 && cp >= 0x1100 &&
+           (cp <= 0x115f ||  // Hangul Jamo init. consonants
+            cp == 0x2329 ||  // LEFT-POINTING ANGLE BRACKET
+            cp == 0x232a ||  // RIGHT-POINTING ANGLE BRACKET
+            // CJK ... Yi except IDEOGRAPHIC HALF FILL SPACE:
+            (cp >= 0x2e80 && cp <= 0xa4cf && cp != 0x303f) ||
+            (cp >= 0xac00 && cp <= 0xd7a3) ||    // Hangul Syllables
+            (cp >= 0xf900 && cp <= 0xfaff) ||    // CJK Compatibility Ideographs
+            (cp >= 0xfe10 && cp <= 0xfe19) ||    // Vertical Forms
+            (cp >= 0xfe30 && cp <= 0xfe6f) ||    // CJK Compatibility Forms
+            (cp >= 0xff00 && cp <= 0xff60) ||    // Fullwidth Forms
+            (cp >= 0xffe0 && cp <= 0xffe6) ||    // Fullwidth Forms
+            (cp >= 0x20000 && cp <= 0x2fffd) ||  // CJK
+            (cp >= 0x30000 && cp <= 0x3fffd) ||
+            // Miscellaneous Symbols and Pictographs + Emoticons:
+            (cp >= 0x1f300 && cp <= 0x1f64f) ||
+            // Supplemental Symbols and Pictographs:
+            (cp >= 0x1f900 && cp <= 0x1f9ff))));
+    }
+  };
+  for_each_codepoint(s, count_code_points{&num_code_points});
+  return num_code_points;
+}
+
+inline auto compute_width(basic_string_view<char8_type> s) -> size_t {
+  return compute_width(basic_string_view<char>(
+      reinterpret_cast<const char*>(s.data()), s.size()));
+}
+
+template <typename Char>
+inline auto code_point_index(basic_string_view<Char> s, size_t n) -> size_t {
+  size_t size = s.size();
+  return n < size ? n : size;
+}
+
+// Calculates the index of the nth code point in a UTF-8 string.
+inline auto code_point_index(basic_string_view<char8_type> s, size_t n)
+    -> size_t {
+  const char8_type* data = s.data();
+  size_t num_code_points = 0;
+  for (size_t i = 0, size = s.size(); i != size; ++i) {
+    if ((data[i] & 0xc0) != 0x80 && ++num_code_points > n) return i;
+  }
+  return s.size();
+}
+
+template <typename T>
+using is_fast_float = bool_constant<std::numeric_limits<T>::is_iec559 &&
+                                    sizeof(T) <= sizeof(double)>;
+
+#ifndef FMT_USE_FULL_CACHE_DRAGONBOX
+#  define FMT_USE_FULL_CACHE_DRAGONBOX 0
+#endif
+
+template <typename T>
+template <typename U>
+void buffer<T>::append(const U* begin, const U* end) {
+  while (begin != end) {
+    auto count = to_unsigned(end - begin);
+    try_reserve(size_ + count);
+    auto free_cap = capacity_ - size_;
+    if (free_cap < count) count = free_cap;
+    std::uninitialized_copy_n(begin, count, make_checked(ptr_ + size_, count));
+    size_ += count;
+    begin += count;
+  }
+}
+
+template <typename T, typename Enable = void>
+struct is_locale : std::false_type {};
+template <typename T>
+struct is_locale<T, void_t<decltype(T::classic())>> : std::true_type {};
+}  // namespace detail
+
+FMT_MODULE_EXPORT_BEGIN
+
+// The number of characters to store in the basic_memory_buffer object itself
+// to avoid dynamic memory allocation.
+enum { inline_buffer_size = 500 };
+
+/**
+  \rst
+  A dynamically growing memory buffer for trivially copyable/constructible types
+  with the first ``SIZE`` elements stored in the object itself.
+
+  You can use the ``memory_buffer`` type alias for ``char`` instead.
+
+  **Example**::
+
+     fmt::memory_buffer out;
+     format_to(out, "The answer is {}.", 42);
+
+  This will append the following output to the ``out`` object:
+
+  .. code-block:: none
+
+     The answer is 42.
+
+  The output can be converted to an ``std::string`` with ``to_string(out)``.
+  \endrst
+ */
+template <typename T, size_t SIZE = inline_buffer_size,
+          typename Allocator = std::allocator<T>>
+class basic_memory_buffer final : public detail::buffer<T> {
+ private:
+  T store_[SIZE];
+
+  // Don't inherit from Allocator avoid generating type_info for it.
+  Allocator alloc_;
+
+  // Deallocate memory allocated by the buffer.
+  void deallocate() {
+    T* data = this->data();
+    if (data != store_) alloc_.deallocate(data, this->capacity());
+  }
+
+ protected:
+  void grow(size_t size) final FMT_OVERRIDE;
+
+ public:
+  using value_type = T;
+  using const_reference = const T&;
+
+  explicit basic_memory_buffer(const Allocator& alloc = Allocator())
+      : alloc_(alloc) {
+    this->set(store_, SIZE);
+  }
+  ~basic_memory_buffer() { deallocate(); }
+
+ private:
+  // Move data from other to this buffer.
+  void move(basic_memory_buffer& other) {
+    alloc_ = std::move(other.alloc_);
+    T* data = other.data();
+    size_t size = other.size(), capacity = other.capacity();
+    if (data == other.store_) {
+      this->set(store_, capacity);
+      std::uninitialized_copy(other.store_, other.store_ + size,
+                              detail::make_checked(store_, capacity));
+    } else {
+      this->set(data, capacity);
+      // Set pointer to the inline array so that delete is not called
+      // when deallocating.
+      other.set(other.store_, 0);
+    }
+    this->resize(size);
+  }
+
+ public:
+  /**
+    \rst
+    Constructs a :class:`fmt::basic_memory_buffer` object moving the content
+    of the other object to it.
+    \endrst
+   */
+  basic_memory_buffer(basic_memory_buffer&& other) FMT_NOEXCEPT { move(other); }
+
+  /**
+    \rst
+    Moves the content of the other ``basic_memory_buffer`` object to this one.
+    \endrst
+   */
+  auto operator=(basic_memory_buffer&& other) FMT_NOEXCEPT
+      -> basic_memory_buffer& {
+    FMT_ASSERT(this != &other, "");
+    deallocate();
+    move(other);
+    return *this;
+  }
+
+  // Returns a copy of the allocator associated with this buffer.
+  auto get_allocator() const -> Allocator { return alloc_; }
+
+  /**
+    Resizes the buffer to contain *count* elements. If T is a POD type new
+    elements may not be initialized.
+   */
+  void resize(size_t count) { this->try_resize(count); }
+
+  /** Increases the buffer capacity to *new_capacity*. */
+  void reserve(size_t new_capacity) { this->try_reserve(new_capacity); }
+
+  // Directly append data into the buffer
+  using detail::buffer<T>::append;
+  template <typename ContiguousRange>
+  void append(const ContiguousRange& range) {
+    append(range.data(), range.data() + range.size());
+  }
+};
+
+template <typename T, size_t SIZE, typename Allocator>
+void basic_memory_buffer<T, SIZE, Allocator>::grow(size_t size) {
+#ifdef FMT_FUZZ
+  if (size > 5000) throw std::runtime_error("fuzz mode - won't grow that much");
+#endif
+  const size_t max_size = std::allocator_traits<Allocator>::max_size(alloc_);
+  size_t old_capacity = this->capacity();
+  size_t new_capacity = old_capacity + old_capacity / 2;
+  if (size > new_capacity)
+    new_capacity = size;
+  else if (new_capacity > max_size)
+    new_capacity = size > max_size ? size : max_size;
+  T* old_data = this->data();
+  T* new_data =
+      std::allocator_traits<Allocator>::allocate(alloc_, new_capacity);
+  // The following code doesn't throw, so the raw pointer above doesn't leak.
+  std::uninitialized_copy(old_data, old_data + this->size(),
+                          detail::make_checked(new_data, new_capacity));
+  this->set(new_data, new_capacity);
+  // deallocate must not throw according to the standard, but even if it does,
+  // the buffer already uses the new storage and will deallocate it in
+  // destructor.
+  if (old_data != store_) alloc_.deallocate(old_data, old_capacity);
+}
+
+using memory_buffer = basic_memory_buffer<char>;
+
+template <typename T, size_t SIZE, typename Allocator>
+struct is_contiguous<basic_memory_buffer<T, SIZE, Allocator>> : std::true_type {
+};
+
+namespace detail {
+FMT_API void print(std::FILE*, string_view);
+}
+
+/** A formatting error such as invalid format string. */
+FMT_CLASS_API
+class FMT_API format_error : public std::runtime_error {
+ public:
+  explicit format_error(const char* message) : std::runtime_error(message) {}
+  explicit format_error(const std::string& message)
+      : std::runtime_error(message) {}
+  format_error(const format_error&) = default;
+  format_error& operator=(const format_error&) = default;
+  format_error(format_error&&) = default;
+  format_error& operator=(format_error&&) = default;
+  ~format_error() FMT_NOEXCEPT FMT_OVERRIDE FMT_MSC_DEFAULT;
+};
+
+/**
+  \rst
+  Constructs a `~fmt::format_arg_store` object that contains references
+  to arguments and can be implicitly converted to `~fmt::format_args`.
+  If ``fmt`` is a compile-time string then `make_args_checked` checks
+  its validity at compile time.
+  \endrst
+ */
+template <typename... Args, typename S, typename Char = char_t<S>>
+FMT_INLINE auto make_args_checked(const S& fmt,
+                                  const remove_reference_t<Args>&... args)
+    -> format_arg_store<buffer_context<Char>, remove_reference_t<Args>...> {
+  static_assert(
+      detail::count<(
+              std::is_base_of<detail::view, remove_reference_t<Args>>::value &&
+              std::is_reference<Args>::value)...>() == 0,
+      "passing views as lvalues is disallowed");
+  detail::check_format_string<Args...>(fmt);
+  return {args...};
+}
+
+// compile-time support
+namespace detail_exported {
+#if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+template <typename Char, size_t N> struct fixed_string {
+  constexpr fixed_string(const Char (&str)[N]) {
+    detail::copy_str<Char, const Char*, Char*>(static_cast<const Char*>(str),
+                                               str + N, data);
+  }
+  Char data[N]{};
+};
+#endif
+
+// Converts a compile-time string to basic_string_view.
+template <typename Char, size_t N>
+constexpr auto compile_string_to_view(const Char (&s)[N])
+    -> basic_string_view<Char> {
+  // Remove trailing NUL character if needed. Won't be present if this is used
+  // with a raw character array (i.e. not defined as a string).
+  return {s, N - (std::char_traits<Char>::to_int_type(s[N - 1]) == 0 ? 1 : 0)};
+}
+template <typename Char>
+constexpr auto compile_string_to_view(detail::std_string_view<Char> s)
+    -> basic_string_view<Char> {
+  return {s.data(), s.size()};
+}
+}  // namespace detail_exported
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+inline void throw_format_error(const char* message) {
+  FMT_THROW(format_error(message));
+}
+
+template <typename T> struct is_integral : std::is_integral<T> {};
+template <> struct is_integral<int128_t> : std::true_type {};
+template <> struct is_integral<uint128_t> : std::true_type {};
+
+template <typename T>
+using is_signed =
+    std::integral_constant<bool, std::numeric_limits<T>::is_signed ||
+                                     std::is_same<T, int128_t>::value>;
+
+// Returns true if value is negative, false otherwise.
+// Same as `value < 0` but doesn't produce warnings if T is an unsigned type.
+template <typename T, FMT_ENABLE_IF(is_signed<T>::value)>
+FMT_CONSTEXPR auto is_negative(T value) -> bool {
+  return value < 0;
+}
+template <typename T, FMT_ENABLE_IF(!is_signed<T>::value)>
+FMT_CONSTEXPR auto is_negative(T) -> bool {
+  return false;
+}
+
+template <typename T, FMT_ENABLE_IF(std::is_floating_point<T>::value)>
+FMT_CONSTEXPR auto is_supported_floating_point(T) -> uint16_t {
+  return (std::is_same<T, float>::value && FMT_USE_FLOAT) ||
+         (std::is_same<T, double>::value && FMT_USE_DOUBLE) ||
+         (std::is_same<T, long double>::value && FMT_USE_LONG_DOUBLE);
+}
+
+// Smallest of uint32_t, uint64_t, uint128_t that is large enough to
+// represent all values of an integral type T.
+template <typename T>
+using uint32_or_64_or_128_t =
+    conditional_t<num_bits<T>() <= 32 && !FMT_REDUCE_INT_INSTANTIATIONS,
+                  uint32_t,
+                  conditional_t<num_bits<T>() <= 64, uint64_t, uint128_t>>;
+template <typename T>
+using uint64_or_128_t = conditional_t<num_bits<T>() <= 64, uint64_t, uint128_t>;
+
+#define FMT_POWERS_OF_10(factor)                                             \
+  factor * 10, (factor)*100, (factor)*1000, (factor)*10000, (factor)*100000, \
+      (factor)*1000000, (factor)*10000000, (factor)*100000000,               \
+      (factor)*1000000000
+
+// Static data is placed in this class template for the header-only config.
+template <typename T = void> struct basic_data {
+  // log10(2) = 0x0.4d104d427de7fbcc...
+  static const uint64_t log10_2_significand = 0x4d104d427de7fbcc;
+
+  // GCC generates slightly better code for pairs than chars.
+  FMT_API static constexpr const char digits[100][2] = {
+      {'0', '0'}, {'0', '1'}, {'0', '2'}, {'0', '3'}, {'0', '4'}, {'0', '5'},
+      {'0', '6'}, {'0', '7'}, {'0', '8'}, {'0', '9'}, {'1', '0'}, {'1', '1'},
+      {'1', '2'}, {'1', '3'}, {'1', '4'}, {'1', '5'}, {'1', '6'}, {'1', '7'},
+      {'1', '8'}, {'1', '9'}, {'2', '0'}, {'2', '1'}, {'2', '2'}, {'2', '3'},
+      {'2', '4'}, {'2', '5'}, {'2', '6'}, {'2', '7'}, {'2', '8'}, {'2', '9'},
+      {'3', '0'}, {'3', '1'}, {'3', '2'}, {'3', '3'}, {'3', '4'}, {'3', '5'},
+      {'3', '6'}, {'3', '7'}, {'3', '8'}, {'3', '9'}, {'4', '0'}, {'4', '1'},
+      {'4', '2'}, {'4', '3'}, {'4', '4'}, {'4', '5'}, {'4', '6'}, {'4', '7'},
+      {'4', '8'}, {'4', '9'}, {'5', '0'}, {'5', '1'}, {'5', '2'}, {'5', '3'},
+      {'5', '4'}, {'5', '5'}, {'5', '6'}, {'5', '7'}, {'5', '8'}, {'5', '9'},
+      {'6', '0'}, {'6', '1'}, {'6', '2'}, {'6', '3'}, {'6', '4'}, {'6', '5'},
+      {'6', '6'}, {'6', '7'}, {'6', '8'}, {'6', '9'}, {'7', '0'}, {'7', '1'},
+      {'7', '2'}, {'7', '3'}, {'7', '4'}, {'7', '5'}, {'7', '6'}, {'7', '7'},
+      {'7', '8'}, {'7', '9'}, {'8', '0'}, {'8', '1'}, {'8', '2'}, {'8', '3'},
+      {'8', '4'}, {'8', '5'}, {'8', '6'}, {'8', '7'}, {'8', '8'}, {'8', '9'},
+      {'9', '0'}, {'9', '1'}, {'9', '2'}, {'9', '3'}, {'9', '4'}, {'9', '5'},
+      {'9', '6'}, {'9', '7'}, {'9', '8'}, {'9', '9'}};
+
+  FMT_API static constexpr const char hex_digits[] = "0123456789abcdef";
+  FMT_API static constexpr const char signs[4] = {0, '-', '+', ' '};
+  FMT_API static constexpr const unsigned prefixes[4] = {0, 0, 0x1000000u | '+',
+                                                         0x1000000u | ' '};
+  FMT_API static constexpr const char left_padding_shifts[5] = {31, 31, 0, 1,
+                                                                0};
+  FMT_API static constexpr const char right_padding_shifts[5] = {0, 31, 0, 1,
+                                                                 0};
+};
+
+#ifdef FMT_SHARED
+// Required for -flto, -fivisibility=hidden and -shared to work
+extern template struct basic_data<void>;
+#endif
+
+// This is a struct rather than an alias to avoid shadowing warnings in gcc.
+struct data : basic_data<> {};
+
+template <typename T> FMT_CONSTEXPR auto count_digits_fallback(T n) -> int {
+  int count = 1;
+  for (;;) {
+    // Integer division is slow so do it for a group of four digits instead
+    // of for every digit. The idea comes from the talk by Alexandrescu
+    // "Three Optimization Tips for C++". See speed-test for a comparison.
+    if (n < 10) return count;
+    if (n < 100) return count + 1;
+    if (n < 1000) return count + 2;
+    if (n < 10000) return count + 3;
+    n /= 10000u;
+    count += 4;
+  }
+}
+#if FMT_USE_INT128
+FMT_CONSTEXPR inline auto count_digits(uint128_t n) -> int {
+  return count_digits_fallback(n);
+}
+#endif
+
+// Returns the number of decimal digits in n. Leading zeros are not counted
+// except for n == 0 in which case count_digits returns 1.
+FMT_CONSTEXPR20 inline auto count_digits(uint64_t n) -> int {
+#ifdef FMT_BUILTIN_CLZLL
+  if (!is_constant_evaluated()) {
+    // https://github.com/fmtlib/format-benchmark/blob/master/digits10
+    // Maps bsr(n) to ceil(log10(pow(2, bsr(n) + 1) - 1)).
+    constexpr uint16_t bsr2log10[] = {
+        1,  1,  1,  2,  2,  2,  3,  3,  3,  4,  4,  4,  4,  5,  5,  5,
+        6,  6,  6,  7,  7,  7,  7,  8,  8,  8,  9,  9,  9,  10, 10, 10,
+        10, 11, 11, 11, 12, 12, 12, 13, 13, 13, 13, 14, 14, 14, 15, 15,
+        15, 16, 16, 16, 16, 17, 17, 17, 18, 18, 18, 19, 19, 19, 19, 20};
+    auto t = bsr2log10[FMT_BUILTIN_CLZLL(n | 1) ^ 63];
+    constexpr const uint64_t zero_or_powers_of_10[] = {
+        0, 0, FMT_POWERS_OF_10(1U), FMT_POWERS_OF_10(1000000000ULL),
+        10000000000000000000ULL};
+    return t - (n < zero_or_powers_of_10[t]);
+  }
+#endif
+  return count_digits_fallback(n);
+}
+
+// Counts the number of digits in n. BITS = log2(radix).
+template <int BITS, typename UInt>
+FMT_CONSTEXPR auto count_digits(UInt n) -> int {
+#ifdef FMT_BUILTIN_CLZ
+  if (num_bits<UInt>() == 32)
+    return (FMT_BUILTIN_CLZ(static_cast<uint32_t>(n) | 1) ^ 31) / BITS + 1;
+#endif
+  int num_digits = 0;
+  do {
+    ++num_digits;
+  } while ((n >>= BITS) != 0);
+  return num_digits;
+}
+
+template <> auto count_digits<4>(detail::fallback_uintptr n) -> int;
+
+// It is a separate function rather than a part of count_digits to workaround
+// the lack of static constexpr in constexpr functions.
+FMT_INLINE uint64_t count_digits_inc(int n) {
+  // An optimization by Kendall Willets from https://bit.ly/3uOIQrB.
+  // This increments the upper 32 bits (log10(T) - 1) when >= T is added.
+#define FMT_INC(T) (((sizeof(#T) - 1ull) << 32) - T)
+  static constexpr uint64_t table[] = {
+      FMT_INC(0),          FMT_INC(0),          FMT_INC(0),           // 8
+      FMT_INC(10),         FMT_INC(10),         FMT_INC(10),          // 64
+      FMT_INC(100),        FMT_INC(100),        FMT_INC(100),         // 512
+      FMT_INC(1000),       FMT_INC(1000),       FMT_INC(1000),        // 4096
+      FMT_INC(10000),      FMT_INC(10000),      FMT_INC(10000),       // 32k
+      FMT_INC(100000),     FMT_INC(100000),     FMT_INC(100000),      // 256k
+      FMT_INC(1000000),    FMT_INC(1000000),    FMT_INC(1000000),     // 2048k
+      FMT_INC(10000000),   FMT_INC(10000000),   FMT_INC(10000000),    // 16M
+      FMT_INC(100000000),  FMT_INC(100000000),  FMT_INC(100000000),   // 128M
+      FMT_INC(1000000000), FMT_INC(1000000000), FMT_INC(1000000000),  // 1024M
+      FMT_INC(1000000000), FMT_INC(1000000000)                        // 4B
+  };
+  return table[n];
+}
+
+// Optional version of count_digits for better performance on 32-bit platforms.
+FMT_CONSTEXPR20 inline auto count_digits(uint32_t n) -> int {
+#ifdef FMT_BUILTIN_CLZ
+  if (!is_constant_evaluated()) {
+    auto inc = count_digits_inc(FMT_BUILTIN_CLZ(n | 1) ^ 31);
+    return static_cast<int>((n + inc) >> 32);
+  }
+#endif
+  return count_digits_fallback(n);
+}
+
+template <typename Int> constexpr auto digits10() FMT_NOEXCEPT -> int {
+  return std::numeric_limits<Int>::digits10;
+}
+template <> constexpr auto digits10<int128_t>() FMT_NOEXCEPT -> int {
+  return 38;
+}
+template <> constexpr auto digits10<uint128_t>() FMT_NOEXCEPT -> int {
+  return 38;
+}
+
+template <typename Char> struct thousands_sep_result {
+  std::string grouping;
+  Char thousands_sep;
+};
+
+template <typename Char>
+FMT_API auto thousands_sep_impl(locale_ref loc) -> thousands_sep_result<Char>;
+template <typename Char>
+inline auto thousands_sep(locale_ref loc) -> thousands_sep_result<Char> {
+  auto result = thousands_sep_impl<char>(loc);
+  return {result.grouping, Char(result.thousands_sep)};
+}
+template <>
+inline auto thousands_sep(locale_ref loc) -> thousands_sep_result<wchar_t> {
+  return thousands_sep_impl<wchar_t>(loc);
+}
+
+template <typename Char>
+FMT_API auto decimal_point_impl(locale_ref loc) -> Char;
+template <typename Char> inline auto decimal_point(locale_ref loc) -> Char {
+  return Char(decimal_point_impl<char>(loc));
+}
+template <> inline auto decimal_point(locale_ref loc) -> wchar_t {
+  return decimal_point_impl<wchar_t>(loc);
+}
+
+// Compares two characters for equality.
+template <typename Char> auto equal2(const Char* lhs, const char* rhs) -> bool {
+  return lhs[0] == Char(rhs[0]) && lhs[1] == Char(rhs[1]);
+}
+inline auto equal2(const char* lhs, const char* rhs) -> bool {
+  return memcmp(lhs, rhs, 2) == 0;
+}
+
+// Copies two characters from src to dst.
+template <typename Char> void copy2(Char* dst, const char* src) {
+  *dst++ = static_cast<Char>(*src++);
+  *dst = static_cast<Char>(*src);
+}
+FMT_INLINE void copy2(char* dst, const char* src) { memcpy(dst, src, 2); }
+
+template <typename Iterator> struct format_decimal_result {
+  Iterator begin;
+  Iterator end;
+};
+
+// Formats a decimal unsigned integer value writing into out pointing to a
+// buffer of specified size. The caller must ensure that the buffer is large
+// enough.
+template <typename Char, typename UInt>
+FMT_CONSTEXPR20 auto format_decimal(Char* out, UInt value, int size)
+    -> format_decimal_result<Char*> {
+  FMT_ASSERT(size >= count_digits(value), "invalid digit count");
+  out += size;
+  Char* end = out;
+  if (is_constant_evaluated()) {
+    while (value >= 10) {
+      *--out = static_cast<Char>('0' + value % 10);
+      value /= 10;
+    }
+    *--out = static_cast<Char>('0' + value);
+    return {out, end};
+  }
+  while (value >= 100) {
+    // Integer division is slow so do it for a group of two digits instead
+    // of for every digit. The idea comes from the talk by Alexandrescu
+    // "Three Optimization Tips for C++". See speed-test for a comparison.
+    out -= 2;
+    copy2(out, data::digits[value % 100]);
+    value /= 100;
+  }
+  if (value < 10) {
+    *--out = static_cast<Char>('0' + value);
+    return {out, end};
+  }
+  out -= 2;
+  copy2(out, data::digits[value]);
+  return {out, end};
+}
+
+template <typename Char, typename UInt, typename Iterator,
+          FMT_ENABLE_IF(!std::is_pointer<remove_cvref_t<Iterator>>::value)>
+inline auto format_decimal(Iterator out, UInt value, int size)
+    -> format_decimal_result<Iterator> {
+  // Buffer is large enough to hold all digits (digits10 + 1).
+  Char buffer[digits10<UInt>() + 1];
+  auto end = format_decimal(buffer, value, size).end;
+  return {out, detail::copy_str_noinline<Char>(buffer, end, out)};
+}
+
+template <unsigned BASE_BITS, typename Char, typename UInt>
+FMT_CONSTEXPR auto format_uint(Char* buffer, UInt value, int num_digits,
+                               bool upper = false) -> Char* {
+  buffer += num_digits;
+  Char* end = buffer;
+  do {
+    const char* digits = upper ? "0123456789ABCDEF" : data::hex_digits;
+    unsigned digit = (value & ((1 << BASE_BITS) - 1));
+    *--buffer = static_cast<Char>(BASE_BITS < 4 ? static_cast<char>('0' + digit)
+                                                : digits[digit]);
+  } while ((value >>= BASE_BITS) != 0);
+  return end;
+}
+
+template <unsigned BASE_BITS, typename Char>
+auto format_uint(Char* buffer, detail::fallback_uintptr n, int num_digits,
+                 bool = false) -> Char* {
+  auto char_digits = std::numeric_limits<unsigned char>::digits / 4;
+  int start = (num_digits + char_digits - 1) / char_digits - 1;
+  if (int start_digits = num_digits % char_digits) {
+    unsigned value = n.value[start--];
+    buffer = format_uint<BASE_BITS>(buffer, value, start_digits);
+  }
+  for (; start >= 0; --start) {
+    unsigned value = n.value[start];
+    buffer += char_digits;
+    auto p = buffer;
+    for (int i = 0; i < char_digits; ++i) {
+      unsigned digit = (value & ((1 << BASE_BITS) - 1));
+      *--p = static_cast<Char>(data::hex_digits[digit]);
+      value >>= BASE_BITS;
+    }
+  }
+  return buffer;
+}
+
+template <unsigned BASE_BITS, typename Char, typename It, typename UInt>
+inline auto format_uint(It out, UInt value, int num_digits, bool upper = false)
+    -> It {
+  if (auto ptr = to_pointer<Char>(out, to_unsigned(num_digits))) {
+    format_uint<BASE_BITS>(ptr, value, num_digits, upper);
+    return out;
+  }
+  // Buffer should be large enough to hold all digits (digits / BASE_BITS + 1).
+  char buffer[num_bits<UInt>() / BASE_BITS + 1];
+  format_uint<BASE_BITS>(buffer, value, num_digits, upper);
+  return detail::copy_str_noinline<Char>(buffer, buffer + num_digits, out);
+}
+
+// A converter from UTF-8 to UTF-16.
+class utf8_to_utf16 {
+ private:
+  basic_memory_buffer<wchar_t> buffer_;
+
+ public:
+  FMT_API explicit utf8_to_utf16(string_view s);
+  operator basic_string_view<wchar_t>() const { return {&buffer_[0], size()}; }
+  auto size() const -> size_t { return buffer_.size() - 1; }
+  auto c_str() const -> const wchar_t* { return &buffer_[0]; }
+  auto str() const -> std::wstring { return {&buffer_[0], size()}; }
+};
+
+namespace dragonbox {
+
+// Type-specific information that Dragonbox uses.
+template <class T> struct float_info;
+
+template <> struct float_info<float> {
+  using carrier_uint = uint32_t;
+  static const int significand_bits = 23;
+  static const int exponent_bits = 8;
+  static const int min_exponent = -126;
+  static const int max_exponent = 127;
+  static const int exponent_bias = -127;
+  static const int decimal_digits = 9;
+  static const int kappa = 1;
+  static const int big_divisor = 100;
+  static const int small_divisor = 10;
+  static const int min_k = -31;
+  static const int max_k = 46;
+  static const int cache_bits = 64;
+  static const int divisibility_check_by_5_threshold = 39;
+  static const int case_fc_pm_half_lower_threshold = -1;
+  static const int case_fc_pm_half_upper_threshold = 6;
+  static const int case_fc_lower_threshold = -2;
+  static const int case_fc_upper_threshold = 6;
+  static const int case_shorter_interval_left_endpoint_lower_threshold = 2;
+  static const int case_shorter_interval_left_endpoint_upper_threshold = 3;
+  static const int shorter_interval_tie_lower_threshold = -35;
+  static const int shorter_interval_tie_upper_threshold = -35;
+  static const int max_trailing_zeros = 7;
+};
+
+template <> struct float_info<double> {
+  using carrier_uint = uint64_t;
+  static const int significand_bits = 52;
+  static const int exponent_bits = 11;
+  static const int min_exponent = -1022;
+  static const int max_exponent = 1023;
+  static const int exponent_bias = -1023;
+  static const int decimal_digits = 17;
+  static const int kappa = 2;
+  static const int big_divisor = 1000;
+  static const int small_divisor = 100;
+  static const int min_k = -292;
+  static const int max_k = 326;
+  static const int cache_bits = 128;
+  static const int divisibility_check_by_5_threshold = 86;
+  static const int case_fc_pm_half_lower_threshold = -2;
+  static const int case_fc_pm_half_upper_threshold = 9;
+  static const int case_fc_lower_threshold = -4;
+  static const int case_fc_upper_threshold = 9;
+  static const int case_shorter_interval_left_endpoint_lower_threshold = 2;
+  static const int case_shorter_interval_left_endpoint_upper_threshold = 3;
+  static const int shorter_interval_tie_lower_threshold = -77;
+  static const int shorter_interval_tie_upper_threshold = -77;
+  static const int max_trailing_zeros = 16;
+};
+
+template <typename T> struct decimal_fp {
+  using significand_type = typename float_info<T>::carrier_uint;
+  significand_type significand;
+  int exponent;
+};
+
+template <typename T>
+FMT_API auto to_decimal(T x) FMT_NOEXCEPT -> decimal_fp<T>;
+}  // namespace dragonbox
+
+template <typename T>
+constexpr auto exponent_mask() ->
+    typename dragonbox::float_info<T>::carrier_uint {
+  using uint = typename dragonbox::float_info<T>::carrier_uint;
+  return ((uint(1) << dragonbox::float_info<T>::exponent_bits) - 1)
+         << dragonbox::float_info<T>::significand_bits;
+}
+
+// Writes the exponent exp in the form "[+-]d{2,3}" to buffer.
+template <typename Char, typename It>
+auto write_exponent(int exp, It it) -> It {
+  FMT_ASSERT(-10000 < exp && exp < 10000, "exponent out of range");
+  if (exp < 0) {
+    *it++ = static_cast<Char>('-');
+    exp = -exp;
+  } else {
+    *it++ = static_cast<Char>('+');
+  }
+  if (exp >= 100) {
+    const char* top = data::digits[exp / 100];
+    if (exp >= 1000) *it++ = static_cast<Char>(top[0]);
+    *it++ = static_cast<Char>(top[1]);
+    exp %= 100;
+  }
+  const char* d = data::digits[exp];
+  *it++ = static_cast<Char>(d[0]);
+  *it++ = static_cast<Char>(d[1]);
+  return it;
+}
+
+template <typename T>
+auto format_float(T value, int precision, float_specs specs, buffer<char>& buf)
+    -> int;
+
+// Formats a floating-point number with snprintf.
+template <typename T>
+auto snprintf_float(T value, int precision, float_specs specs,
+                    buffer<char>& buf) -> int;
+
+template <typename T> auto promote_float(T value) -> T { return value; }
+inline auto promote_float(float value) -> double {
+  return static_cast<double>(value);
+}
+
+template <typename OutputIt, typename Char>
+FMT_NOINLINE FMT_CONSTEXPR auto fill(OutputIt it, size_t n,
+                                     const fill_t<Char>& fill) -> OutputIt {
+  auto fill_size = fill.size();
+  if (fill_size == 1) return detail::fill_n(it, n, fill[0]);
+  auto data = fill.data();
+  for (size_t i = 0; i < n; ++i)
+    it = copy_str<Char>(data, data + fill_size, it);
+  return it;
+}
+
+// Writes the output of f, padded according to format specifications in specs.
+// size: output size in code units.
+// width: output display width in (terminal) column positions.
+template <align::type align = align::left, typename OutputIt, typename Char,
+          typename F>
+FMT_CONSTEXPR auto write_padded(OutputIt out,
+                                const basic_format_specs<Char>& specs,
+                                size_t size, size_t width, F&& f) -> OutputIt {
+  static_assert(align == align::left || align == align::right, "");
+  unsigned spec_width = to_unsigned(specs.width);
+  size_t padding = spec_width > width ? spec_width - width : 0;
+  auto* shifts = align == align::left ? data::left_padding_shifts
+                                      : data::right_padding_shifts;
+  size_t left_padding = padding >> shifts[specs.align];
+  size_t right_padding = padding - left_padding;
+  auto it = reserve(out, size + padding * specs.fill.size());
+  if (left_padding != 0) it = fill(it, left_padding, specs.fill);
+  it = f(it);
+  if (right_padding != 0) it = fill(it, right_padding, specs.fill);
+  return base_iterator(out, it);
+}
+
+template <align::type align = align::left, typename OutputIt, typename Char,
+          typename F>
+constexpr auto write_padded(OutputIt out, const basic_format_specs<Char>& specs,
+                            size_t size, F&& f) -> OutputIt {
+  return write_padded<align>(out, specs, size, size, f);
+}
+
+template <align::type align = align::left, typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write_bytes(OutputIt out, string_view bytes,
+                               const basic_format_specs<Char>& specs)
+    -> OutputIt {
+  return write_padded<align>(
+      out, specs, bytes.size(), [bytes](reserve_iterator<OutputIt> it) {
+        const char* data = bytes.data();
+        return copy_str<Char>(data, data + bytes.size(), it);
+      });
+}
+
+template <typename Char, typename OutputIt, typename UIntPtr>
+auto write_ptr(OutputIt out, UIntPtr value,
+               const basic_format_specs<Char>* specs) -> OutputIt {
+  int num_digits = count_digits<4>(value);
+  auto size = to_unsigned(num_digits) + size_t(2);
+  auto write = [=](reserve_iterator<OutputIt> it) {
+    *it++ = static_cast<Char>('0');
+    *it++ = static_cast<Char>('x');
+    return format_uint<4, Char>(it, value, num_digits);
+  };
+  return specs ? write_padded<align::right>(out, *specs, size, write)
+               : base_iterator(out, write(reserve(out, size)));
+}
+
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write_char(OutputIt out, Char value,
+                              const basic_format_specs<Char>& specs)
+    -> OutputIt {
+  return write_padded(out, specs, 1, [=](reserve_iterator<OutputIt> it) {
+    *it++ = value;
+    return it;
+  });
+}
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write(OutputIt out, Char value,
+                         const basic_format_specs<Char>& specs,
+                         locale_ref loc = {}) -> OutputIt {
+  return check_char_specs(specs)
+             ? write_char(out, value, specs)
+             : write(out, static_cast<int>(value), specs, loc);
+}
+
+// Data for write_int that doesn't depend on output iterator type. It is used to
+// avoid template code bloat.
+template <typename Char> struct write_int_data {
+  size_t size;
+  size_t padding;
+
+  FMT_CONSTEXPR write_int_data(int num_digits, unsigned prefix,
+                               const basic_format_specs<Char>& specs)
+      : size((prefix >> 24) + to_unsigned(num_digits)), padding(0) {
+    if (specs.align == align::numeric) {
+      auto width = to_unsigned(specs.width);
+      if (width > size) {
+        padding = width - size;
+        size = width;
+      }
+    } else if (specs.precision > num_digits) {
+      size = (prefix >> 24) + to_unsigned(specs.precision);
+      padding = to_unsigned(specs.precision - num_digits);
+    }
+  }
+};
+
+// Writes an integer in the format
+//   <left-padding><prefix><numeric-padding><digits><right-padding>
+// where <digits> are written by write_digits(it).
+// prefix contains chars in three lower bytes and the size in the fourth byte.
+template <typename OutputIt, typename Char, typename W>
+FMT_CONSTEXPR FMT_INLINE auto write_int(OutputIt out, int num_digits,
+                                        unsigned prefix,
+                                        const basic_format_specs<Char>& specs,
+                                        W write_digits) -> OutputIt {
+  // Slightly faster check for specs.width == 0 && specs.precision == -1.
+  if ((specs.width | (specs.precision + 1)) == 0) {
+    auto it = reserve(out, to_unsigned(num_digits) + (prefix >> 24));
+    if (prefix != 0) {
+      for (unsigned p = prefix & 0xffffff; p != 0; p >>= 8)
+        *it++ = static_cast<Char>(p & 0xff);
+    }
+    return base_iterator(out, write_digits(it));
+  }
+  auto data = write_int_data<Char>(num_digits, prefix, specs);
+  return write_padded<align::right>(
+      out, specs, data.size, [=](reserve_iterator<OutputIt> it) {
+        for (unsigned p = prefix & 0xffffff; p != 0; p >>= 8)
+          *it++ = static_cast<Char>(p & 0xff);
+        it = detail::fill_n(it, data.padding, static_cast<Char>('0'));
+        return write_digits(it);
+      });
+}
+
+template <typename OutputIt, typename UInt, typename Char>
+auto write_int_localized(OutputIt& out, UInt value, unsigned prefix,
+                         const basic_format_specs<Char>& specs, locale_ref loc)
+    -> bool {
+  static_assert(std::is_same<uint64_or_128_t<UInt>, UInt>::value, "");
+  const auto sep_size = 1;
+  auto ts = thousands_sep<Char>(loc);
+  if (!ts.thousands_sep) return false;
+  int num_digits = count_digits(value);
+  int size = num_digits, n = num_digits;
+  const std::string& groups = ts.grouping;
+  std::string::const_iterator group = groups.cbegin();
+  while (group != groups.cend() && n > *group && *group > 0 &&
+         *group != max_value<char>()) {
+    size += sep_size;
+    n -= *group;
+    ++group;
+  }
+  if (group == groups.cend()) size += sep_size * ((n - 1) / groups.back());
+  char digits[40];
+  format_decimal(digits, value, num_digits);
+  basic_memory_buffer<Char> buffer;
+  if (prefix != 0) ++size;
+  const auto usize = to_unsigned(size);
+  buffer.resize(usize);
+  basic_string_view<Char> s(&ts.thousands_sep, sep_size);
+  // Index of a decimal digit with the least significant digit having index 0.
+  int digit_index = 0;
+  group = groups.cbegin();
+  auto p = buffer.data() + size - 1;
+  for (int i = num_digits - 1; i > 0; --i) {
+    *p-- = static_cast<Char>(digits[i]);
+    if (*group <= 0 || ++digit_index % *group != 0 ||
+        *group == max_value<char>())
+      continue;
+    if (group + 1 != groups.cend()) {
+      digit_index = 0;
+      ++group;
+    }
+    std::uninitialized_copy(s.data(), s.data() + s.size(),
+                            make_checked(p, s.size()));
+    p -= s.size();
+  }
+  *p-- = static_cast<Char>(*digits);
+  if (prefix != 0) *p = static_cast<Char>(prefix);
+  auto data = buffer.data();
+  out = write_padded<align::right>(
+      out, specs, usize, usize, [=](reserve_iterator<OutputIt> it) {
+        return copy_str<Char>(data, data + size, it);
+      });
+  return true;
+}
+
+FMT_CONSTEXPR inline void prefix_append(unsigned& prefix, unsigned value) {
+  prefix |= prefix != 0 ? value << 8 : value;
+  prefix += (1u + (value > 0xff ? 1 : 0)) << 24;
+}
+
+template <typename UInt> struct write_int_arg {
+  UInt abs_value;
+  unsigned prefix;
+};
+
+template <typename T>
+FMT_CONSTEXPR auto make_write_int_arg(T value, sign_t sign)
+    -> write_int_arg<uint32_or_64_or_128_t<T>> {
+  auto prefix = 0u;
+  auto abs_value = static_cast<uint32_or_64_or_128_t<T>>(value);
+  if (is_negative(value)) {
+    prefix = 0x01000000 | '-';
+    abs_value = 0 - abs_value;
+  } else {
+    prefix = data::prefixes[sign];
+  }
+  return {abs_value, prefix};
+}
+
+template <typename Char, typename OutputIt, typename T>
+FMT_CONSTEXPR FMT_INLINE auto write_int(OutputIt out, write_int_arg<T> arg,
+                                        const basic_format_specs<Char>& specs,
+                                        locale_ref loc) -> OutputIt {
+  static_assert(std::is_same<T, uint32_or_64_or_128_t<T>>::value, "");
+  auto abs_value = arg.abs_value;
+  auto prefix = arg.prefix;
+  auto utype = static_cast<unsigned>(specs.type);
+  switch (specs.type) {
+  case 0:
+  case 'd': {
+    if (specs.localized &&
+        write_int_localized(out, static_cast<uint64_or_128_t<T>>(abs_value),
+                            prefix, specs, loc)) {
+      return out;
+    }
+    auto num_digits = count_digits(abs_value);
+    return write_int(
+        out, num_digits, prefix, specs, [=](reserve_iterator<OutputIt> it) {
+          return format_decimal<Char>(it, abs_value, num_digits).end;
+        });
+  }
+  case 'x':
+  case 'X': {
+    if (specs.alt) prefix_append(prefix, (utype << 8) | '0');
+    bool upper = specs.type != 'x';
+    int num_digits = count_digits<4>(abs_value);
+    return write_int(
+        out, num_digits, prefix, specs, [=](reserve_iterator<OutputIt> it) {
+          return format_uint<4, Char>(it, abs_value, num_digits, upper);
+        });
+  }
+  case 'b':
+  case 'B': {
+    if (specs.alt) prefix_append(prefix, (utype << 8) | '0');
+    int num_digits = count_digits<1>(abs_value);
+    return write_int(out, num_digits, prefix, specs,
+                     [=](reserve_iterator<OutputIt> it) {
+                       return format_uint<1, Char>(it, abs_value, num_digits);
+                     });
+  }
+  case 'o': {
+    int num_digits = count_digits<3>(abs_value);
+    if (specs.alt && specs.precision <= num_digits && abs_value != 0) {
+      // Octal prefix '0' is counted as a digit, so only add it if precision
+      // is not greater than the number of digits.
+      prefix_append(prefix, '0');
+    }
+    return write_int(out, num_digits, prefix, specs,
+                     [=](reserve_iterator<OutputIt> it) {
+                       return format_uint<3, Char>(it, abs_value, num_digits);
+                     });
+  }
+  case 'c':
+    return write_char(out, static_cast<Char>(abs_value), specs);
+  default:
+    FMT_THROW(format_error("invalid type specifier"));
+  }
+  return out;
+}
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(is_integral<T>::value &&
+                        !std::is_same<T, bool>::value &&
+                        std::is_same<OutputIt, buffer_appender<Char>>::value)>
+FMT_CONSTEXPR auto write(OutputIt out, T value,
+                         const basic_format_specs<Char>& specs, locale_ref loc)
+    -> OutputIt {
+  return write_int(out, make_write_int_arg(value, specs.sign), specs, loc);
+}
+// An inlined version of write used in format string compilation.
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(is_integral<T>::value &&
+                        !std::is_same<T, bool>::value &&
+                        !std::is_same<OutputIt, buffer_appender<Char>>::value)>
+FMT_CONSTEXPR FMT_INLINE auto write(OutputIt out, T value,
+                                    const basic_format_specs<Char>& specs,
+                                    locale_ref loc) -> OutputIt {
+  return write_int(out, make_write_int_arg(value, specs.sign), specs, loc);
+}
+
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write(OutputIt out, basic_string_view<Char> s,
+                         const basic_format_specs<Char>& specs) -> OutputIt {
+  auto data = s.data();
+  auto size = s.size();
+  if (specs.precision >= 0 && to_unsigned(specs.precision) < size)
+    size = code_point_index(s, to_unsigned(specs.precision));
+  auto width =
+      specs.width != 0 ? compute_width(basic_string_view<Char>(data, size)) : 0;
+  return write_padded(out, specs, size, width,
+                      [=](reserve_iterator<OutputIt> it) {
+                        return copy_str<Char>(data, data + size, it);
+                      });
+}
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write(OutputIt out,
+                         basic_string_view<type_identity_t<Char>> s,
+                         const basic_format_specs<Char>& specs, locale_ref)
+    -> OutputIt {
+  check_string_type_spec(specs.type);
+  return write(out, s, specs);
+}
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write(OutputIt out, const Char* s,
+                         const basic_format_specs<Char>& specs, locale_ref)
+    -> OutputIt {
+  return check_cstring_type_spec(specs.type)
+             ? write(out, basic_string_view<Char>(s), specs, {})
+             : write_ptr<Char>(out, to_uintptr(s), &specs);
+}
+
+template <typename Char, typename OutputIt>
+auto write_nonfinite(OutputIt out, bool isinf, basic_format_specs<Char> specs,
+                     const float_specs& fspecs) -> OutputIt {
+  auto str =
+      isinf ? (fspecs.upper ? "INF" : "inf") : (fspecs.upper ? "NAN" : "nan");
+  constexpr size_t str_size = 3;
+  auto sign = fspecs.sign;
+  auto size = str_size + (sign ? 1 : 0);
+  // Replace '0'-padding with space for non-finite values.
+  const bool is_zero_fill =
+      specs.fill.size() == 1 && *specs.fill.data() == static_cast<Char>('0');
+  if (is_zero_fill) specs.fill[0] = static_cast<Char>(' ');
+  return write_padded(out, specs, size, [=](reserve_iterator<OutputIt> it) {
+    if (sign) *it++ = static_cast<Char>(data::signs[sign]);
+    return copy_str<Char>(str, str + str_size, it);
+  });
+}
+
+// A decimal floating-point number significand * pow(10, exp).
+struct big_decimal_fp {
+  const char* significand;
+  int significand_size;
+  int exponent;
+};
+
+inline auto get_significand_size(const big_decimal_fp& fp) -> int {
+  return fp.significand_size;
+}
+template <typename T>
+inline auto get_significand_size(const dragonbox::decimal_fp<T>& fp) -> int {
+  return count_digits(fp.significand);
+}
+
+template <typename Char, typename OutputIt>
+inline auto write_significand(OutputIt out, const char* significand,
+                              int& significand_size) -> OutputIt {
+  return copy_str<Char>(significand, significand + significand_size, out);
+}
+template <typename Char, typename OutputIt, typename UInt>
+inline auto write_significand(OutputIt out, UInt significand,
+                              int significand_size) -> OutputIt {
+  return format_decimal<Char>(out, significand, significand_size).end;
+}
+
+template <typename Char, typename UInt,
+          FMT_ENABLE_IF(std::is_integral<UInt>::value)>
+inline auto write_significand(Char* out, UInt significand, int significand_size,
+                              int integral_size, Char decimal_point) -> Char* {
+  if (!decimal_point)
+    return format_decimal(out, significand, significand_size).end;
+  auto end = format_decimal(out + 1, significand, significand_size).end;
+  if (integral_size == 1) {
+    out[0] = out[1];
+  } else {
+    std::uninitialized_copy_n(out + 1, integral_size,
+                              make_checked(out, to_unsigned(integral_size)));
+  }
+  out[integral_size] = decimal_point;
+  return end;
+}
+
+template <typename OutputIt, typename UInt, typename Char,
+          FMT_ENABLE_IF(!std::is_pointer<remove_cvref_t<OutputIt>>::value)>
+inline auto write_significand(OutputIt out, UInt significand,
+                              int significand_size, int integral_size,
+                              Char decimal_point) -> OutputIt {
+  // Buffer is large enough to hold digits (digits10 + 1) and a decimal point.
+  Char buffer[digits10<UInt>() + 2];
+  auto end = write_significand(buffer, significand, significand_size,
+                               integral_size, decimal_point);
+  return detail::copy_str_noinline<Char>(buffer, end, out);
+}
+
+template <typename OutputIt, typename Char>
+inline auto write_significand(OutputIt out, const char* significand,
+                              int significand_size, int integral_size,
+                              Char decimal_point) -> OutputIt {
+  out = detail::copy_str_noinline<Char>(significand,
+                                        significand + integral_size, out);
+  if (!decimal_point) return out;
+  *out++ = decimal_point;
+  return detail::copy_str_noinline<Char>(significand + integral_size,
+                                         significand + significand_size, out);
+}
+
+template <typename OutputIt, typename DecimalFP, typename Char>
+auto write_float(OutputIt out, const DecimalFP& fp,
+                 const basic_format_specs<Char>& specs, float_specs fspecs,
+                 Char decimal_point) -> OutputIt {
+  auto significand = fp.significand;
+  int significand_size = get_significand_size(fp);
+  static const Char zero = static_cast<Char>('0');
+  auto sign = fspecs.sign;
+  size_t size = to_unsigned(significand_size) + (sign ? 1 : 0);
+  using iterator = reserve_iterator<OutputIt>;
+
+  int output_exp = fp.exponent + significand_size - 1;
+  auto use_exp_format = [=]() {
+    if (fspecs.format == float_format::exp) return true;
+    if (fspecs.format != float_format::general) return false;
+    // Use the fixed notation if the exponent is in [exp_lower, exp_upper),
+    // e.g. 0.0001 instead of 1e-04. Otherwise use the exponent notation.
+    const int exp_lower = -4, exp_upper = 16;
+    return output_exp < exp_lower ||
+           output_exp >= (fspecs.precision > 0 ? fspecs.precision : exp_upper);
+  };
+  if (use_exp_format()) {
+    int num_zeros = 0;
+    if (fspecs.showpoint) {
+      num_zeros = fspecs.precision - significand_size;
+      if (num_zeros < 0) num_zeros = 0;
+      size += to_unsigned(num_zeros);
+    } else if (significand_size == 1) {
+      decimal_point = Char();
+    }
+    auto abs_output_exp = output_exp >= 0 ? output_exp : -output_exp;
+    int exp_digits = 2;
+    if (abs_output_exp >= 100) exp_digits = abs_output_exp >= 1000 ? 4 : 3;
+
+    size += to_unsigned((decimal_point ? 1 : 0) + 2 + exp_digits);
+    char exp_char = fspecs.upper ? 'E' : 'e';
+    auto write = [=](iterator it) {
+      if (sign) *it++ = static_cast<Char>(data::signs[sign]);
+      // Insert a decimal point after the first digit and add an exponent.
+      it = write_significand(it, significand, significand_size, 1,
+                             decimal_point);
+      if (num_zeros > 0) it = detail::fill_n(it, num_zeros, zero);
+      *it++ = static_cast<Char>(exp_char);
+      return write_exponent<Char>(output_exp, it);
+    };
+    return specs.width > 0 ? write_padded<align::right>(out, specs, size, write)
+                           : base_iterator(out, write(reserve(out, size)));
+  }
+
+  int exp = fp.exponent + significand_size;
+  if (fp.exponent >= 0) {
+    // 1234e5 -> 123400000[.0+]
+    size += to_unsigned(fp.exponent);
+    int num_zeros = fspecs.precision - exp;
+#ifdef FMT_FUZZ
+    if (num_zeros > 5000)
+      throw std::runtime_error("fuzz mode - avoiding excessive cpu use");
+#endif
+    if (fspecs.showpoint) {
+      if (num_zeros <= 0 && fspecs.format != float_format::fixed) num_zeros = 1;
+      if (num_zeros > 0) size += to_unsigned(num_zeros) + 1;
+    }
+    return write_padded<align::right>(out, specs, size, [&](iterator it) {
+      if (sign) *it++ = static_cast<Char>(data::signs[sign]);
+      it = write_significand<Char>(it, significand, significand_size);
+      it = detail::fill_n(it, fp.exponent, zero);
+      if (!fspecs.showpoint) return it;
+      *it++ = decimal_point;
+      return num_zeros > 0 ? detail::fill_n(it, num_zeros, zero) : it;
+    });
+  } else if (exp > 0) {
+    // 1234e-2 -> 12.34[0+]
+    int num_zeros = fspecs.showpoint ? fspecs.precision - significand_size : 0;
+    size += 1 + to_unsigned(num_zeros > 0 ? num_zeros : 0);
+    return write_padded<align::right>(out, specs, size, [&](iterator it) {
+      if (sign) *it++ = static_cast<Char>(data::signs[sign]);
+      it = write_significand(it, significand, significand_size, exp,
+                             decimal_point);
+      return num_zeros > 0 ? detail::fill_n(it, num_zeros, zero) : it;
+    });
+  }
+  // 1234e-6 -> 0.001234
+  int num_zeros = -exp;
+  if (significand_size == 0 && fspecs.precision >= 0 &&
+      fspecs.precision < num_zeros) {
+    num_zeros = fspecs.precision;
+  }
+  bool pointy = num_zeros != 0 || significand_size != 0 || fspecs.showpoint;
+  size += 1 + (pointy ? 1 : 0) + to_unsigned(num_zeros);
+  return write_padded<align::right>(out, specs, size, [&](iterator it) {
+    if (sign) *it++ = static_cast<Char>(data::signs[sign]);
+    *it++ = zero;
+    if (!pointy) return it;
+    *it++ = decimal_point;
+    it = detail::fill_n(it, num_zeros, zero);
+    return write_significand<Char>(it, significand, significand_size);
+  });
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(std::is_floating_point<T>::value)>
+auto write(OutputIt out, T value, basic_format_specs<Char> specs,
+           locale_ref loc = {}) -> OutputIt {
+  if (const_check(!is_supported_floating_point(value))) return out;
+  float_specs fspecs = parse_float_type_spec(specs);
+  fspecs.sign = specs.sign;
+  if (std::signbit(value)) {  // value < 0 is false for NaN so use signbit.
+    fspecs.sign = sign::minus;
+    value = -value;
+  } else if (fspecs.sign == sign::minus) {
+    fspecs.sign = sign::none;
+  }
+
+  if (!std::isfinite(value))
+    return write_nonfinite(out, std::isinf(value), specs, fspecs);
+
+  if (specs.align == align::numeric && fspecs.sign) {
+    auto it = reserve(out, 1);
+    *it++ = static_cast<Char>(data::signs[fspecs.sign]);
+    out = base_iterator(out, it);
+    fspecs.sign = sign::none;
+    if (specs.width != 0) --specs.width;
+  }
+
+  memory_buffer buffer;
+  if (fspecs.format == float_format::hex) {
+    if (fspecs.sign) buffer.push_back(data::signs[fspecs.sign]);
+    snprintf_float(promote_float(value), specs.precision, fspecs, buffer);
+    return write_bytes<align::right>(out, {buffer.data(), buffer.size()},
+                                     specs);
+  }
+  int precision = specs.precision >= 0 || !specs.type ? specs.precision : 6;
+  if (fspecs.format == float_format::exp) {
+    if (precision == max_value<int>())
+      FMT_THROW(format_error("number is too big"));
+    else
+      ++precision;
+  }
+  if (const_check(std::is_same<T, float>())) fspecs.binary32 = true;
+  fspecs.use_grisu = is_fast_float<T>();
+  int exp = format_float(promote_float(value), precision, fspecs, buffer);
+  fspecs.precision = precision;
+  Char point =
+      fspecs.locale ? decimal_point<Char>(loc) : static_cast<Char>('.');
+  auto fp = big_decimal_fp{buffer.data(), static_cast<int>(buffer.size()), exp};
+  return write_float(out, fp, specs, fspecs, point);
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(is_fast_float<T>::value)>
+auto write(OutputIt out, T value) -> OutputIt {
+  if (const_check(!is_supported_floating_point(value))) return out;
+
+  using floaty = conditional_t<std::is_same<T, long double>::value, double, T>;
+  using uint = typename dragonbox::float_info<floaty>::carrier_uint;
+  auto bits = bit_cast<uint>(value);
+
+  auto fspecs = float_specs();
+  auto sign_bit = bits & (uint(1) << (num_bits<uint>() - 1));
+  if (sign_bit != 0) {
+    fspecs.sign = sign::minus;
+    value = -value;
+  }
+
+  static const auto specs = basic_format_specs<Char>();
+  uint mask = exponent_mask<floaty>();
+  if ((bits & mask) == mask)
+    return write_nonfinite(out, std::isinf(value), specs, fspecs);
+
+  auto dec = dragonbox::to_decimal(static_cast<floaty>(value));
+  return write_float(out, dec, specs, fspecs, static_cast<Char>('.'));
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(std::is_floating_point<T>::value &&
+                        !is_fast_float<T>::value)>
+inline auto write(OutputIt out, T value) -> OutputIt {
+  return write(out, value, basic_format_specs<Char>());
+}
+
+template <typename Char, typename OutputIt>
+auto write(OutputIt out, monostate, basic_format_specs<Char> = {},
+           locale_ref = {}) -> OutputIt {
+  FMT_ASSERT(false, "");
+  return out;
+}
+
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write(OutputIt out, basic_string_view<Char> value)
+    -> OutputIt {
+  auto it = reserve(out, value.size());
+  it = copy_str_noinline<Char>(value.begin(), value.end(), it);
+  return base_iterator(out, it);
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(is_string<T>::value)>
+constexpr auto write(OutputIt out, const T& value) -> OutputIt {
+  return write<Char>(out, to_string_view(value));
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(is_integral<T>::value &&
+                        !std::is_same<T, bool>::value &&
+                        !std::is_same<T, Char>::value)>
+FMT_CONSTEXPR auto write(OutputIt out, T value) -> OutputIt {
+  auto abs_value = static_cast<uint32_or_64_or_128_t<T>>(value);
+  bool negative = is_negative(value);
+  // Don't do -abs_value since it trips unsigned-integer-overflow sanitizer.
+  if (negative) abs_value = ~abs_value + 1;
+  int num_digits = count_digits(abs_value);
+  auto size = (negative ? 1 : 0) + static_cast<size_t>(num_digits);
+  auto it = reserve(out, size);
+  if (auto ptr = to_pointer<Char>(it, size)) {
+    if (negative) *ptr++ = static_cast<Char>('-');
+    format_decimal<Char>(ptr, abs_value, num_digits);
+    return out;
+  }
+  if (negative) *it++ = static_cast<Char>('-');
+  it = format_decimal<Char>(it, abs_value, num_digits).end;
+  return base_iterator(out, it);
+}
+
+// FMT_ENABLE_IF() condition separated to workaround MSVC bug
+template <
+    typename Char, typename OutputIt, typename T,
+    bool check =
+        std::is_enum<T>::value && !std::is_same<T, Char>::value &&
+        mapped_type_constant<T, basic_format_context<OutputIt, Char>>::value !=
+            type::custom_type,
+    FMT_ENABLE_IF(check)>
+FMT_CONSTEXPR auto write(OutputIt out, T value) -> OutputIt {
+  return write<Char>(
+      out, static_cast<typename std::underlying_type<T>::type>(value));
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(std::is_same<T, bool>::value)>
+FMT_CONSTEXPR auto write(OutputIt out, T value,
+                         const basic_format_specs<Char>& specs = {},
+                         locale_ref = {}) -> OutputIt {
+  return specs.type && specs.type != 's'
+             ? write(out, value ? 1 : 0, specs, {})
+             : write_bytes(out, value ? "true" : "false", specs);
+}
+
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR auto write(OutputIt out, Char value) -> OutputIt {
+  auto it = reserve(out, 1);
+  *it++ = value;
+  return base_iterator(out, it);
+}
+
+template <typename Char, typename OutputIt>
+FMT_CONSTEXPR_CHAR_TRAITS auto write(OutputIt out, const Char* value)
+    -> OutputIt {
+  if (!value) {
+    FMT_THROW(format_error("string pointer is null"));
+  } else {
+    auto length = std::char_traits<Char>::length(value);
+    out = write(out, basic_string_view<Char>(value, length));
+  }
+  return out;
+}
+
+template <typename Char, typename OutputIt, typename T,
+          FMT_ENABLE_IF(std::is_same<T, void>::value)>
+auto write(OutputIt out, const T* value,
+           const basic_format_specs<Char>& specs = {}, locale_ref = {})
+    -> OutputIt {
+  check_pointer_type_spec(specs.type, error_handler());
+  return write_ptr<Char>(out, to_uintptr(value), &specs);
+}
+
+template <typename Char, typename OutputIt, typename T>
+FMT_CONSTEXPR auto write(OutputIt out, const T& value) ->
+    typename std::enable_if<
+        mapped_type_constant<T, basic_format_context<OutputIt, Char>>::value ==
+            type::custom_type,
+        OutputIt>::type {
+  using context_type = basic_format_context<OutputIt, Char>;
+  using formatter_type =
+      conditional_t<has_formatter<T, context_type>::value,
+                    typename context_type::template formatter_type<T>,
+                    fallback_formatter<T, Char>>;
+  context_type ctx(out, {}, {});
+  return formatter_type().format(value, ctx);
+}
+
+// An argument visitor that formats the argument and writes it via the output
+// iterator. It's a class and not a generic lambda for compatibility with C++11.
+template <typename Char> struct default_arg_formatter {
+  using iterator = buffer_appender<Char>;
+  using context = buffer_context<Char>;
+
+  iterator out;
+  basic_format_args<context> args;
+  locale_ref loc;
+
+  template <typename T> auto operator()(T value) -> iterator {
+    return write<Char>(out, value);
+  }
+  auto operator()(typename basic_format_arg<context>::handle h) -> iterator {
+    basic_format_parse_context<Char> parse_ctx({});
+    context format_ctx(out, args, loc);
+    h.format(parse_ctx, format_ctx);
+    return format_ctx.out();
+  }
+};
+
+template <typename Char> struct arg_formatter {
+  using iterator = buffer_appender<Char>;
+  using context = buffer_context<Char>;
+
+  iterator out;
+  const basic_format_specs<Char>& specs;
+  locale_ref locale;
+
+  template <typename T>
+  FMT_CONSTEXPR FMT_INLINE auto operator()(T value) -> iterator {
+    return detail::write(out, value, specs, locale);
+  }
+  auto operator()(typename basic_format_arg<context>::handle) -> iterator {
+    // User-defined types are handled separately because they require access
+    // to the parse context.
+    return out;
+  }
+};
+
+template <typename Char> struct custom_formatter {
+  basic_format_parse_context<Char>& parse_ctx;
+  buffer_context<Char>& ctx;
+
+  void operator()(
+      typename basic_format_arg<buffer_context<Char>>::handle h) const {
+    h.format(parse_ctx, ctx);
+  }
+  template <typename T> void operator()(T) const {}
+};
+
+template <typename T>
+using is_integer =
+    bool_constant<is_integral<T>::value && !std::is_same<T, bool>::value &&
+                  !std::is_same<T, char>::value &&
+                  !std::is_same<T, wchar_t>::value>;
+
+template <typename ErrorHandler> class width_checker {
+ public:
+  explicit FMT_CONSTEXPR width_checker(ErrorHandler& eh) : handler_(eh) {}
+
+  template <typename T, FMT_ENABLE_IF(is_integer<T>::value)>
+  FMT_CONSTEXPR auto operator()(T value) -> unsigned long long {
+    if (is_negative(value)) handler_.on_error("negative width");
+    return static_cast<unsigned long long>(value);
+  }
+
+  template <typename T, FMT_ENABLE_IF(!is_integer<T>::value)>
+  FMT_CONSTEXPR auto operator()(T) -> unsigned long long {
+    handler_.on_error("width is not integer");
+    return 0;
+  }
+
+ private:
+  ErrorHandler& handler_;
+};
+
+template <typename ErrorHandler> class precision_checker {
+ public:
+  explicit FMT_CONSTEXPR precision_checker(ErrorHandler& eh) : handler_(eh) {}
+
+  template <typename T, FMT_ENABLE_IF(is_integer<T>::value)>
+  FMT_CONSTEXPR auto operator()(T value) -> unsigned long long {
+    if (is_negative(value)) handler_.on_error("negative precision");
+    return static_cast<unsigned long long>(value);
+  }
+
+  template <typename T, FMT_ENABLE_IF(!is_integer<T>::value)>
+  FMT_CONSTEXPR auto operator()(T) -> unsigned long long {
+    handler_.on_error("precision is not integer");
+    return 0;
+  }
+
+ private:
+  ErrorHandler& handler_;
+};
+
+template <template <typename> class Handler, typename FormatArg,
+          typename ErrorHandler>
+FMT_CONSTEXPR auto get_dynamic_spec(FormatArg arg, ErrorHandler eh) -> int {
+  unsigned long long value = visit_format_arg(Handler<ErrorHandler>(eh), arg);
+  if (value > to_unsigned(max_value<int>())) eh.on_error("number is too big");
+  return static_cast<int>(value);
+}
+
+template <typename Context, typename ID>
+FMT_CONSTEXPR auto get_arg(Context& ctx, ID id) ->
+    typename Context::format_arg {
+  auto arg = ctx.arg(id);
+  if (!arg) ctx.on_error("argument not found");
+  return arg;
+}
+
+// The standard format specifier handler with checking.
+template <typename Char> class specs_handler : public specs_setter<Char> {
+ private:
+  basic_format_parse_context<Char>& parse_context_;
+  buffer_context<Char>& context_;
+
+  // This is only needed for compatibility with gcc 4.4.
+  using format_arg = basic_format_arg<buffer_context<Char>>;
+
+  FMT_CONSTEXPR auto get_arg(auto_id) -> format_arg {
+    return detail::get_arg(context_, parse_context_.next_arg_id());
+  }
+
+  FMT_CONSTEXPR auto get_arg(int arg_id) -> format_arg {
+    parse_context_.check_arg_id(arg_id);
+    return detail::get_arg(context_, arg_id);
+  }
+
+  FMT_CONSTEXPR auto get_arg(basic_string_view<Char> arg_id) -> format_arg {
+    parse_context_.check_arg_id(arg_id);
+    return detail::get_arg(context_, arg_id);
+  }
+
+ public:
+  FMT_CONSTEXPR specs_handler(basic_format_specs<Char>& specs,
+                              basic_format_parse_context<Char>& parse_ctx,
+                              buffer_context<Char>& ctx)
+      : specs_setter<Char>(specs), parse_context_(parse_ctx), context_(ctx) {}
+
+  template <typename Id> FMT_CONSTEXPR void on_dynamic_width(Id arg_id) {
+    this->specs_.width = get_dynamic_spec<width_checker>(
+        get_arg(arg_id), context_.error_handler());
+  }
+
+  template <typename Id> FMT_CONSTEXPR void on_dynamic_precision(Id arg_id) {
+    this->specs_.precision = get_dynamic_spec<precision_checker>(
+        get_arg(arg_id), context_.error_handler());
+  }
+
+  void on_error(const char* message) { context_.on_error(message); }
+};
+
+template <template <typename> class Handler, typename Context>
+FMT_CONSTEXPR void handle_dynamic_spec(int& value,
+                                       arg_ref<typename Context::char_type> ref,
+                                       Context& ctx) {
+  switch (ref.kind) {
+  case arg_id_kind::none:
+    break;
+  case arg_id_kind::index:
+    value = detail::get_dynamic_spec<Handler>(ctx.arg(ref.val.index),
+                                              ctx.error_handler());
+    break;
+  case arg_id_kind::name:
+    value = detail::get_dynamic_spec<Handler>(ctx.arg(ref.val.name),
+                                              ctx.error_handler());
+    break;
+  }
+}
+
+#define FMT_STRING_IMPL(s, base, explicit)                                 \
+  [] {                                                                     \
+    /* Use the hidden visibility as a workaround for a GCC bug (#1973). */ \
+    /* Use a macro-like name to avoid shadowing warnings. */               \
+    struct FMT_GCC_VISIBILITY_HIDDEN FMT_COMPILE_STRING : base {           \
+      using char_type = fmt::remove_cvref_t<decltype(s[0])>;               \
+      FMT_MAYBE_UNUSED FMT_CONSTEXPR explicit                              \
+      operator fmt::basic_string_view<char_type>() const {                 \
+        return fmt::detail_exported::compile_string_to_view<char_type>(s); \
+      }                                                                    \
+    };                                                                     \
+    return FMT_COMPILE_STRING();                                           \
+  }()
+
+/**
+  \rst
+  Constructs a compile-time format string from a string literal *s*.
+
+  **Example**::
+
+    // A compile-time error because 'd' is an invalid specifier for strings.
+    std::string s = fmt::format(FMT_STRING("{:d}"), "foo");
+  \endrst
+ */
+#define FMT_STRING(s) FMT_STRING_IMPL(s, fmt::compile_string, )
+
+#if FMT_USE_USER_DEFINED_LITERALS
+template <typename Char> struct udl_formatter {
+  basic_string_view<Char> str;
+
+  template <typename... T>
+  auto operator()(T&&... args) const -> std::basic_string<Char> {
+    return vformat(str, fmt::make_args_checked<T...>(str, args...));
+  }
+};
+
+#  if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+template <typename T, typename Char, size_t N,
+          fmt::detail_exported::fixed_string<Char, N> Str>
+struct statically_named_arg : view {
+  static constexpr auto name = Str.data;
+
+  const T& value;
+  statically_named_arg(const T& v) : value(v) {}
+};
+
+template <typename T, typename Char, size_t N,
+          fmt::detail_exported::fixed_string<Char, N> Str>
+struct is_named_arg<statically_named_arg<T, Char, N, Str>> : std::true_type {};
+
+template <typename T, typename Char, size_t N,
+          fmt::detail_exported::fixed_string<Char, N> Str>
+struct is_statically_named_arg<statically_named_arg<T, Char, N, Str>>
+    : std::true_type {};
+
+template <typename Char, size_t N,
+          fmt::detail_exported::fixed_string<Char, N> Str>
+struct udl_arg {
+  template <typename T> auto operator=(T&& value) const {
+    return statically_named_arg<T, Char, N, Str>(std::forward<T>(value));
+  }
+};
+#  else
+template <typename Char> struct udl_arg {
+  const Char* str;
+
+  template <typename T> auto operator=(T&& value) const -> named_arg<Char, T> {
+    return {str, std::forward<T>(value)};
+  }
+};
+#  endif
+#endif  // FMT_USE_USER_DEFINED_LITERALS
+
+template <typename Locale, typename Char>
+auto vformat(const Locale& loc, basic_string_view<Char> format_str,
+             basic_format_args<buffer_context<type_identity_t<Char>>> args)
+    -> std::basic_string<Char> {
+  basic_memory_buffer<Char> buffer;
+  detail::vformat_to(buffer, format_str, args, detail::locale_ref(loc));
+  return {buffer.data(), buffer.size()};
+}
+
+using format_func = void (*)(detail::buffer<char>&, int, const char*);
+
+FMT_API void format_error_code(buffer<char>& out, int error_code,
+                               string_view message) FMT_NOEXCEPT;
+
+FMT_API void report_error(format_func func, int error_code,
+                          const char* message) FMT_NOEXCEPT;
+FMT_END_DETAIL_NAMESPACE
+
+FMT_API auto vsystem_error(int error_code, string_view format_str,
+                           format_args args) -> std::system_error;
+
+/**
+ \rst
+ Constructs :class:`std::system_error` with a message formatted with
+ ``fmt::format(fmt, args...)``.
+  *error_code* is a system error code as given by ``errno``.
+
+ **Example**::
+
+   // This throws std::system_error with the description
+   //   cannot open file 'madeup': No such file or directory
+   // or similar (system message may vary).
+   const char* filename = "madeup";
+   std::FILE* file = std::fopen(filename, "r");
+   if (!file)
+     throw fmt::system_error(errno, "cannot open file '{}'", filename);
+ \endrst
+*/
+template <typename... T>
+auto system_error(int error_code, format_string<T...> fmt, T&&... args)
+    -> std::system_error {
+  return vsystem_error(error_code, fmt, fmt::make_format_args(args...));
+}
+
+/**
+  \rst
+  Formats an error message for an error returned by an operating system or a
+  language runtime, for example a file opening error, and writes it to *out*.
+  The format is the same as the one used by ``std::system_error(ec, message)``
+  where ``ec`` is ``std::error_code(error_code, std::generic_category()})``.
+  It is implementation-defined but normally looks like:
+
+  .. parsed-literal::
+     *<message>*: *<system-message>*
+
+  where *<message>* is the passed message and *<system-message>* is the system
+  message corresponding to the error code.
+  *error_code* is a system error code as given by ``errno``.
+  \endrst
+ */
+FMT_API void format_system_error(detail::buffer<char>& out, int error_code,
+                                 const char* message) FMT_NOEXCEPT;
+
+// Reports a system error without throwing an exception.
+// Can be used to report errors from destructors.
+FMT_API void report_system_error(int error_code,
+                                 const char* message) FMT_NOEXCEPT;
+
+/** Fast integer formatter. */
+class format_int {
+ private:
+  // Buffer should be large enough to hold all digits (digits10 + 1),
+  // a sign and a null character.
+  enum { buffer_size = std::numeric_limits<unsigned long long>::digits10 + 3 };
+  mutable char buffer_[buffer_size];
+  char* str_;
+
+  template <typename UInt> auto format_unsigned(UInt value) -> char* {
+    auto n = static_cast<detail::uint32_or_64_or_128_t<UInt>>(value);
+    return detail::format_decimal(buffer_, n, buffer_size - 1).begin;
+  }
+
+  template <typename Int> auto format_signed(Int value) -> char* {
+    auto abs_value = static_cast<detail::uint32_or_64_or_128_t<Int>>(value);
+    bool negative = value < 0;
+    if (negative) abs_value = 0 - abs_value;
+    auto begin = format_unsigned(abs_value);
+    if (negative) *--begin = '-';
+    return begin;
+  }
+
+ public:
+  explicit format_int(int value) : str_(format_signed(value)) {}
+  explicit format_int(long value) : str_(format_signed(value)) {}
+  explicit format_int(long long value) : str_(format_signed(value)) {}
+  explicit format_int(unsigned value) : str_(format_unsigned(value)) {}
+  explicit format_int(unsigned long value) : str_(format_unsigned(value)) {}
+  explicit format_int(unsigned long long value)
+      : str_(format_unsigned(value)) {}
+
+  /** Returns the number of characters written to the output buffer. */
+  auto size() const -> size_t {
+    return detail::to_unsigned(buffer_ - str_ + buffer_size - 1);
+  }
+
+  /**
+    Returns a pointer to the output buffer content. No terminating null
+    character is appended.
+   */
+  auto data() const -> const char* { return str_; }
+
+  /**
+    Returns a pointer to the output buffer content with terminating null
+    character appended.
+   */
+  auto c_str() const -> const char* {
+    buffer_[buffer_size - 1] = '\0';
+    return str_;
+  }
+
+  /**
+    \rst
+    Returns the content of the output buffer as an ``std::string``.
+    \endrst
+   */
+  auto str() const -> std::string { return std::string(str_, size()); }
+};
+
+template <typename T, typename Char>
+template <typename FormatContext>
+FMT_CONSTEXPR FMT_INLINE auto
+formatter<T, Char,
+          enable_if_t<detail::type_constant<T, Char>::value !=
+                      detail::type::custom_type>>::format(const T& val,
+                                                          FormatContext& ctx)
+    const -> decltype(ctx.out()) {
+  if (specs_.width_ref.kind != detail::arg_id_kind::none ||
+      specs_.precision_ref.kind != detail::arg_id_kind::none) {
+    auto specs = specs_;
+    detail::handle_dynamic_spec<detail::width_checker>(specs.width,
+                                                       specs.width_ref, ctx);
+    detail::handle_dynamic_spec<detail::precision_checker>(
+        specs.precision, specs.precision_ref, ctx);
+    return detail::write<Char>(ctx.out(), val, specs, ctx.locale());
+  }
+  return detail::write<Char>(ctx.out(), val, specs_, ctx.locale());
+}
+
+#define FMT_FORMAT_AS(Type, Base)                                        \
+  template <typename Char>                                               \
+  struct formatter<Type, Char> : formatter<Base, Char> {                 \
+    template <typename FormatContext>                                    \
+    auto format(Type const& val, FormatContext& ctx) const               \
+        -> decltype(ctx.out()) {                                         \
+      return formatter<Base, Char>::format(static_cast<Base>(val), ctx); \
+    }                                                                    \
+  }
+
+FMT_FORMAT_AS(signed char, int);
+FMT_FORMAT_AS(unsigned char, unsigned);
+FMT_FORMAT_AS(short, int);
+FMT_FORMAT_AS(unsigned short, unsigned);
+FMT_FORMAT_AS(long, long long);
+FMT_FORMAT_AS(unsigned long, unsigned long long);
+FMT_FORMAT_AS(Char*, const Char*);
+FMT_FORMAT_AS(std::basic_string<Char>, basic_string_view<Char>);
+FMT_FORMAT_AS(std::nullptr_t, const void*);
+FMT_FORMAT_AS(detail::std_string_view<Char>, basic_string_view<Char>);
+
+template <typename Char>
+struct formatter<void*, Char> : formatter<const void*, Char> {
+  template <typename FormatContext>
+  auto format(void* val, FormatContext& ctx) const -> decltype(ctx.out()) {
+    return formatter<const void*, Char>::format(val, ctx);
+  }
+};
+
+template <typename Char, size_t N>
+struct formatter<Char[N], Char> : formatter<basic_string_view<Char>, Char> {
+  template <typename FormatContext>
+  FMT_CONSTEXPR auto format(const Char* val, FormatContext& ctx) const
+      -> decltype(ctx.out()) {
+    return formatter<basic_string_view<Char>, Char>::format(val, ctx);
+  }
+};
+
+// A formatter for types known only at run time such as variant alternatives.
+//
+// Usage:
+//   using variant = std::variant<int, std::string>;
+//   template <>
+//   struct formatter<variant>: dynamic_formatter<> {
+//     auto format(const variant& v, format_context& ctx) {
+//       return visit([&](const auto& val) {
+//           return dynamic_formatter<>::format(val, ctx);
+//       }, v);
+//     }
+//   };
+template <typename Char = char> class dynamic_formatter {
+ private:
+  detail::dynamic_format_specs<Char> specs_;
+  const Char* format_str_;
+
+  struct null_handler : detail::error_handler {
+    void on_align(align_t) {}
+    void on_sign(sign_t) {}
+    void on_hash() {}
+  };
+
+  template <typename Context> void handle_specs(Context& ctx) {
+    detail::handle_dynamic_spec<detail::width_checker>(specs_.width,
+                                                       specs_.width_ref, ctx);
+    detail::handle_dynamic_spec<detail::precision_checker>(
+        specs_.precision, specs_.precision_ref, ctx);
+  }
+
+ public:
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    format_str_ = ctx.begin();
+    // Checks are deferred to formatting time when the argument type is known.
+    detail::dynamic_specs_handler<ParseContext> handler(specs_, ctx);
+    return detail::parse_format_specs(ctx.begin(), ctx.end(), handler);
+  }
+
+  template <typename T, typename FormatContext>
+  auto format(const T& val, FormatContext& ctx) -> decltype(ctx.out()) {
+    handle_specs(ctx);
+    detail::specs_checker<null_handler> checker(
+        null_handler(), detail::mapped_type_constant<T, FormatContext>::value);
+    checker.on_align(specs_.align);
+    if (specs_.sign != sign::none) checker.on_sign(specs_.sign);
+    if (specs_.alt) checker.on_hash();
+    if (specs_.precision >= 0) checker.end_precision();
+    return detail::write<Char>(ctx.out(), val, specs_, ctx.locale());
+  }
+};
+
+/**
+  \rst
+  Converts ``p`` to ``const void*`` for pointer formatting.
+
+  **Example**::
+
+    auto s = fmt::format("{}", fmt::ptr(p));
+  \endrst
+ */
+template <typename T> auto ptr(T p) -> const void* {
+  static_assert(std::is_pointer<T>::value, "");
+  return detail::bit_cast<const void*>(p);
+}
+template <typename T> auto ptr(const std::unique_ptr<T>& p) -> const void* {
+  return p.get();
+}
+template <typename T> auto ptr(const std::shared_ptr<T>& p) -> const void* {
+  return p.get();
+}
+
+class bytes {
+ private:
+  string_view data_;
+  friend struct formatter<bytes>;
+
+ public:
+  explicit bytes(string_view data) : data_(data) {}
+};
+
+template <> struct formatter<bytes> {
+ private:
+  detail::dynamic_format_specs<char> specs_;
+
+ public:
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    using handler_type = detail::dynamic_specs_handler<ParseContext>;
+    detail::specs_checker<handler_type> handler(handler_type(specs_, ctx),
+                                                detail::type::string_type);
+    auto it = parse_format_specs(ctx.begin(), ctx.end(), handler);
+    detail::check_string_type_spec(specs_.type, ctx.error_handler());
+    return it;
+  }
+
+  template <typename FormatContext>
+  auto format(bytes b, FormatContext& ctx) -> decltype(ctx.out()) {
+    detail::handle_dynamic_spec<detail::width_checker>(specs_.width,
+                                                       specs_.width_ref, ctx);
+    detail::handle_dynamic_spec<detail::precision_checker>(
+        specs_.precision, specs_.precision_ref, ctx);
+    return detail::write_bytes(ctx.out(), b.data_, specs_);
+  }
+};
+
+template <typename It, typename Sentinel, typename Char = char>
+struct join_view : detail::view {
+  It begin;
+  Sentinel end;
+  basic_string_view<Char> sep;
+
+  join_view(It b, Sentinel e, basic_string_view<Char> s)
+      : begin(b), end(e), sep(s) {}
+};
+
+template <typename It, typename Sentinel, typename Char>
+using arg_join FMT_DEPRECATED_ALIAS = join_view<It, Sentinel, Char>;
+
+template <typename It, typename Sentinel, typename Char>
+struct formatter<join_view<It, Sentinel, Char>, Char> {
+ private:
+  using value_type = typename std::iterator_traits<It>::value_type;
+  using context = buffer_context<Char>;
+  using mapper = detail::arg_mapper<context>;
+
+  template <typename T, FMT_ENABLE_IF(has_formatter<T, context>::value)>
+  static auto map(const T& value) -> const T& {
+    return value;
+  }
+  template <typename T, FMT_ENABLE_IF(!has_formatter<T, context>::value)>
+  static auto map(const T& value) -> decltype(mapper().map(value)) {
+    return mapper().map(value);
+  }
+
+  using formatter_type =
+      conditional_t<is_formattable<value_type, Char>::value,
+                    formatter<remove_cvref_t<decltype(map(
+                                  std::declval<const value_type&>()))>,
+                              Char>,
+                    detail::fallback_formatter<value_type, Char>>;
+
+  formatter_type value_formatter_;
+
+ public:
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return value_formatter_.parse(ctx);
+  }
+
+  template <typename FormatContext>
+  auto format(const join_view<It, Sentinel, Char>& value, FormatContext& ctx)
+      -> decltype(ctx.out()) {
+    auto it = value.begin;
+    auto out = ctx.out();
+    if (it != value.end) {
+      out = value_formatter_.format(map(*it++), ctx);
+      while (it != value.end) {
+        out = detail::copy_str<Char>(value.sep.begin(), value.sep.end(), out);
+        ctx.advance_to(out);
+        out = value_formatter_.format(map(*it++), ctx);
+      }
+    }
+    return out;
+  }
+};
+
+/**
+  Returns an object that formats the iterator range `[begin, end)` with
+  elements separated by `sep`.
+ */
+template <typename It, typename Sentinel>
+auto join(It begin, Sentinel end, string_view sep) -> join_view<It, Sentinel> {
+  return {begin, end, sep};
+}
+
+/**
+  \rst
+  Returns an object that formats `range` with elements separated by `sep`.
+
+  **Example**::
+
+    std::vector<int> v = {1, 2, 3};
+    fmt::print("{}", fmt::join(v, ", "));
+    // Output: "1, 2, 3"
+
+  ``fmt::join`` applies passed format specifiers to the range elements::
+
+    fmt::print("{:02}", fmt::join(v, ", "));
+    // Output: "01, 02, 03"
+  \endrst
+ */
+template <typename Range>
+auto join(Range&& range, string_view sep)
+    -> join_view<detail::iterator_t<Range>, detail::sentinel_t<Range>> {
+  return join(std::begin(range), std::end(range), sep);
+}
+
+/**
+  \rst
+  Converts *value* to ``std::string`` using the default format for type *T*.
+
+  **Example**::
+
+    #include <fmt/format.h>
+
+    std::string answer = fmt::to_string(42);
+  \endrst
+ */
+template <typename T, FMT_ENABLE_IF(!std::is_integral<T>::value)>
+inline auto to_string(const T& value) -> std::string {
+  auto result = std::string();
+  detail::write<char>(std::back_inserter(result), value);
+  return result;
+}
+
+template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+inline auto to_string(T value) -> std::string {
+  // The buffer should be large enough to store the number including the sign
+  // or "false" for bool.
+  constexpr int max_size = detail::digits10<T>() + 2;
+  char buffer[max_size > 5 ? static_cast<unsigned>(max_size) : 5];
+  char* begin = buffer;
+  return std::string(begin, detail::write<char>(begin, value));
+}
+
+template <typename Char, size_t SIZE>
+auto to_string(const basic_memory_buffer<Char, SIZE>& buf)
+    -> std::basic_string<Char> {
+  auto size = buf.size();
+  detail::assume(size < std::basic_string<Char>().max_size());
+  return std::basic_string<Char>(buf.data(), size);
+}
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+template <typename Char>
+void vformat_to(
+    buffer<Char>& buf, basic_string_view<Char> fmt,
+    basic_format_args<FMT_BUFFER_CONTEXT(type_identity_t<Char>)> args,
+    locale_ref loc) {
+  // workaround for msvc bug regarding name-lookup in module
+  // link names into function scope
+  using detail::arg_formatter;
+  using detail::buffer_appender;
+  using detail::custom_formatter;
+  using detail::default_arg_formatter;
+  using detail::get_arg;
+  using detail::locale_ref;
+  using detail::parse_format_specs;
+  using detail::specs_checker;
+  using detail::specs_handler;
+  using detail::to_unsigned;
+  using detail::type;
+  using detail::write;
+  auto out = buffer_appender<Char>(buf);
+  if (fmt.size() == 2 && equal2(fmt.data(), "{}")) {
+    auto arg = args.get(0);
+    if (!arg) error_handler().on_error("argument not found");
+    visit_format_arg(default_arg_formatter<Char>{out, args, loc}, arg);
+    return;
+  }
+
+  struct format_handler : error_handler {
+    basic_format_parse_context<Char> parse_context;
+    buffer_context<Char> context;
+
+    format_handler(buffer_appender<Char> out, basic_string_view<Char> str,
+                   basic_format_args<buffer_context<Char>> args, locale_ref loc)
+        : parse_context(str), context(out, args, loc) {}
+
+    void on_text(const Char* begin, const Char* end) {
+      auto text = basic_string_view<Char>(begin, to_unsigned(end - begin));
+      context.advance_to(write<Char>(context.out(), text));
+    }
+
+    FMT_CONSTEXPR auto on_arg_id() -> int {
+      return parse_context.next_arg_id();
+    }
+    FMT_CONSTEXPR auto on_arg_id(int id) -> int {
+      return parse_context.check_arg_id(id), id;
+    }
+    FMT_CONSTEXPR auto on_arg_id(basic_string_view<Char> id) -> int {
+      int arg_id = context.arg_id(id);
+      if (arg_id < 0) on_error("argument not found");
+      return arg_id;
+    }
+
+    FMT_INLINE void on_replacement_field(int id, const Char*) {
+      auto arg = get_arg(context, id);
+      context.advance_to(visit_format_arg(
+          default_arg_formatter<Char>{context.out(), context.args(),
+                                      context.locale()},
+          arg));
+    }
+
+    auto on_format_specs(int id, const Char* begin, const Char* end)
+        -> const Char* {
+      auto arg = get_arg(context, id);
+      if (arg.type() == type::custom_type) {
+        parse_context.advance_to(parse_context.begin() +
+                                 (begin - &*parse_context.begin()));
+        visit_format_arg(custom_formatter<Char>{parse_context, context}, arg);
+        return parse_context.begin();
+      }
+      auto specs = basic_format_specs<Char>();
+      specs_checker<specs_handler<Char>> handler(
+          specs_handler<Char>(specs, parse_context, context), arg.type());
+      begin = parse_format_specs(begin, end, handler);
+      if (begin == end || *begin != '}')
+        on_error("missing '}' in format string");
+      auto f = arg_formatter<Char>{context.out(), specs, context.locale()};
+      context.advance_to(visit_format_arg(f, arg));
+      return begin;
+    }
+  };
+  detail::parse_format_string<false>(fmt, format_handler(out, fmt, args, loc));
+}
+
+#ifndef FMT_HEADER_ONLY
+extern template FMT_API auto thousands_sep_impl<char>(locale_ref)
+    -> thousands_sep_result<char>;
+extern template FMT_API auto thousands_sep_impl<wchar_t>(locale_ref)
+    -> thousands_sep_result<wchar_t>;
+extern template FMT_API auto decimal_point_impl(locale_ref) -> char;
+extern template FMT_API auto decimal_point_impl(locale_ref) -> wchar_t;
+extern template auto format_float<double>(double value, int precision,
+                                          float_specs specs, buffer<char>& buf)
+    -> int;
+extern template auto format_float<long double>(long double value, int precision,
+                                               float_specs specs,
+                                               buffer<char>& buf) -> int;
+void snprintf_float(float, int, float_specs, buffer<char>&) = delete;
+extern template auto snprintf_float<double>(double value, int precision,
+                                            float_specs specs,
+                                            buffer<char>& buf) -> int;
+extern template auto snprintf_float<long double>(long double value,
+                                                 int precision,
+                                                 float_specs specs,
+                                                 buffer<char>& buf) -> int;
+#endif  // FMT_HEADER_ONLY
+
+FMT_END_DETAIL_NAMESPACE
+
+#if FMT_USE_USER_DEFINED_LITERALS
+inline namespace literals {
+/**
+  \rst
+  User-defined literal equivalent of :func:`fmt::arg`.
+
+  **Example**::
+
+    using namespace fmt::literals;
+    fmt::print("Elapsed time: {s:.2f} seconds", "s"_a=1.23);
+  \endrst
+ */
+#  if FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+template <detail_exported::fixed_string Str>
+constexpr auto operator""_a()
+    -> detail::udl_arg<remove_cvref_t<decltype(Str.data[0])>,
+                       sizeof(Str.data) / sizeof(decltype(Str.data[0])), Str> {
+  return {};
+}
+#  else
+constexpr auto operator"" _a(const char* s, size_t) -> detail::udl_arg<char> {
+  return {s};
+}
+#  endif
+
+/**
+  \rst
+  User-defined literal equivalent of :func:`fmt::format`.
+
+  **Example**::
+
+    using namespace fmt::literals;
+    std::string message = "The answer is {}"_format(42);
+  \endrst
+ */
+constexpr auto operator"" _format(const char* s, size_t n)
+    -> detail::udl_formatter<char> {
+  return {{s, n}};
+}
+}  // namespace literals
+#endif  // FMT_USE_USER_DEFINED_LITERALS
+
+template <typename Locale, FMT_ENABLE_IF(detail::is_locale<Locale>::value)>
+inline auto vformat(const Locale& loc, string_view fmt, format_args args)
+    -> std::string {
+  return detail::vformat(loc, fmt, args);
+}
+
+template <typename Locale, typename... T,
+          FMT_ENABLE_IF(detail::is_locale<Locale>::value)>
+inline auto format(const Locale& loc, format_string<T...> fmt, T&&... args)
+    -> std::string {
+  return vformat(loc, string_view(fmt), fmt::make_format_args(args...));
+}
+
+template <typename... T, size_t SIZE, typename Allocator>
+FMT_DEPRECATED auto format_to(basic_memory_buffer<char, SIZE, Allocator>& buf,
+                              format_string<T...> fmt, T&&... args)
+    -> appender {
+  detail::vformat_to(buf, string_view(fmt), fmt::make_format_args(args...));
+  return appender(buf);
+}
+
+template <typename OutputIt, typename Locale,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, char>::value&&
+                            detail::is_locale<Locale>::value)>
+auto vformat_to(OutputIt out, const Locale& loc, string_view fmt,
+                format_args args) -> OutputIt {
+  using detail::get_buffer;
+  auto&& buf = get_buffer<char>(out);
+  detail::vformat_to(buf, fmt, args, detail::locale_ref(loc));
+  return detail::get_iterator(buf);
+}
+
+template <typename OutputIt, typename Locale, typename... T,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, char>::value&&
+                            detail::is_locale<Locale>::value)>
+FMT_INLINE auto format_to(OutputIt out, const Locale& loc,
+                          format_string<T...> fmt, T&&... args) -> OutputIt {
+  return vformat_to(out, loc, fmt, fmt::make_format_args(args...));
+}
+
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#ifdef FMT_DEPRECATED_INCLUDE_XCHAR
+#  include "xchar.h"
+#endif
+
+#ifdef FMT_HEADER_ONLY
+#  define FMT_FUNC inline
+#  include "format-inl.h"
+#else
+#  define FMT_FUNC
+#endif
+
+#endif  // FMT_FORMAT_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/locale.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/locale.h
new file mode 100644
index 0000000..7571b52
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/locale.h
@@ -0,0 +1,2 @@
+#include "xchar.h"
+#warning fmt/locale.h is deprecated, include fmt/format.h or fmt/xchar.h instead
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/os.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/os.h
new file mode 100644
index 0000000..f6c0f32
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/os.h
@@ -0,0 +1,515 @@
+// Formatting library for C++ - optional OS-specific functionality
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_OS_H_
+#define FMT_OS_H_
+
+#include <cerrno>
+#include <clocale>  // locale_t
+#include <cstddef>
+#include <cstdio>
+#include <cstdlib>       // strtod_l
+#include <system_error>  // std::system_error
+
+#if defined __APPLE__ || defined(__FreeBSD__)
+#  include <xlocale.h>  // for LC_NUMERIC_MASK on OS X
+#endif
+
+#include "format.h"
+
+// UWP doesn't provide _pipe.
+#if FMT_HAS_INCLUDE("winapifamily.h")
+#  include <winapifamily.h>
+#endif
+#if (FMT_HAS_INCLUDE(<fcntl.h>) || defined(__APPLE__) || \
+     defined(__linux__)) &&                              \
+    (!defined(WINAPI_FAMILY) || (WINAPI_FAMILY == WINAPI_FAMILY_DESKTOP_APP))
+#  include <fcntl.h>  // for O_RDONLY
+#  define FMT_USE_FCNTL 1
+#else
+#  define FMT_USE_FCNTL 0
+#endif
+
+#ifndef FMT_POSIX
+#  if defined(_WIN32) && !defined(__MINGW32__)
+// Fix warnings about deprecated symbols.
+#    define FMT_POSIX(call) _##call
+#  else
+#    define FMT_POSIX(call) call
+#  endif
+#endif
+
+// Calls to system functions are wrapped in FMT_SYSTEM for testability.
+#ifdef FMT_SYSTEM
+#  define FMT_POSIX_CALL(call) FMT_SYSTEM(call)
+#else
+#  define FMT_SYSTEM(call) ::call
+#  ifdef _WIN32
+// Fix warnings about deprecated symbols.
+#    define FMT_POSIX_CALL(call) ::_##call
+#  else
+#    define FMT_POSIX_CALL(call) ::call
+#  endif
+#endif
+
+// Retries the expression while it evaluates to error_result and errno
+// equals to EINTR.
+#ifndef _WIN32
+#  define FMT_RETRY_VAL(result, expression, error_result) \
+    do {                                                  \
+      (result) = (expression);                            \
+    } while ((result) == (error_result) && errno == EINTR)
+#else
+#  define FMT_RETRY_VAL(result, expression, error_result) result = (expression)
+#endif
+
+#define FMT_RETRY(result, expression) FMT_RETRY_VAL(result, expression, -1)
+
+FMT_BEGIN_NAMESPACE
+FMT_MODULE_EXPORT_BEGIN
+
+/**
+  \rst
+  A reference to a null-terminated string. It can be constructed from a C
+  string or ``std::string``.
+
+  You can use one of the following type aliases for common character types:
+
+  +---------------+-----------------------------+
+  | Type          | Definition                  |
+  +===============+=============================+
+  | cstring_view  | basic_cstring_view<char>    |
+  +---------------+-----------------------------+
+  | wcstring_view | basic_cstring_view<wchar_t> |
+  +---------------+-----------------------------+
+
+  This class is most useful as a parameter type to allow passing
+  different types of strings to a function, for example::
+
+    template <typename... Args>
+    std::string format(cstring_view format_str, const Args & ... args);
+
+    format("{}", 42);
+    format(std::string("{}"), 42);
+  \endrst
+ */
+template <typename Char> class basic_cstring_view {
+ private:
+  const Char* data_;
+
+ public:
+  /** Constructs a string reference object from a C string. */
+  basic_cstring_view(const Char* s) : data_(s) {}
+
+  /**
+    \rst
+    Constructs a string reference from an ``std::string`` object.
+    \endrst
+   */
+  basic_cstring_view(const std::basic_string<Char>& s) : data_(s.c_str()) {}
+
+  /** Returns the pointer to a C string. */
+  const Char* c_str() const { return data_; }
+};
+
+using cstring_view = basic_cstring_view<char>;
+using wcstring_view = basic_cstring_view<wchar_t>;
+
+template <typename Char> struct formatter<std::error_code, Char> {
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  FMT_CONSTEXPR auto format(const std::error_code& ec, FormatContext& ctx) const
+      -> decltype(ctx.out()) {
+    auto out = ctx.out();
+    out = detail::write_bytes(out, ec.category().name(),
+                              basic_format_specs<Char>());
+    out = detail::write<Char>(out, Char(':'));
+    out = detail::write<Char>(out, ec.value());
+    return out;
+  }
+};
+
+#ifdef _WIN32
+FMT_API const std::error_category& system_category() FMT_NOEXCEPT;
+
+FMT_BEGIN_DETAIL_NAMESPACE
+// A converter from UTF-16 to UTF-8.
+// It is only provided for Windows since other systems support UTF-8 natively.
+class utf16_to_utf8 {
+ private:
+  memory_buffer buffer_;
+
+ public:
+  utf16_to_utf8() {}
+  FMT_API explicit utf16_to_utf8(basic_string_view<wchar_t> s);
+  operator string_view() const { return string_view(&buffer_[0], size()); }
+  size_t size() const { return buffer_.size() - 1; }
+  const char* c_str() const { return &buffer_[0]; }
+  std::string str() const { return std::string(&buffer_[0], size()); }
+
+  // Performs conversion returning a system error code instead of
+  // throwing exception on conversion error. This method may still throw
+  // in case of memory allocation error.
+  FMT_API int convert(basic_string_view<wchar_t> s);
+};
+
+FMT_API void format_windows_error(buffer<char>& out, int error_code,
+                                  const char* message) FMT_NOEXCEPT;
+FMT_END_DETAIL_NAMESPACE
+
+FMT_API std::system_error vwindows_error(int error_code, string_view format_str,
+                                         format_args args);
+
+/**
+ \rst
+ Constructs a :class:`std::system_error` object with the description
+ of the form
+
+ .. parsed-literal::
+   *<message>*: *<system-message>*
+
+ where *<message>* is the formatted message and *<system-message>* is the
+ system message corresponding to the error code.
+ *error_code* is a Windows error code as given by ``GetLastError``.
+ If *error_code* is not a valid error code such as -1, the system message
+ will look like "error -1".
+
+ **Example**::
+
+   // This throws a system_error with the description
+   //   cannot open file 'madeup': The system cannot find the file specified.
+   // or similar (system message may vary).
+   const char *filename = "madeup";
+   LPOFSTRUCT of = LPOFSTRUCT();
+   HFILE file = OpenFile(filename, &of, OF_READ);
+   if (file == HFILE_ERROR) {
+     throw fmt::windows_error(GetLastError(),
+                              "cannot open file '{}'", filename);
+   }
+ \endrst
+*/
+template <typename... Args>
+std::system_error windows_error(int error_code, string_view message,
+                                const Args&... args) {
+  return vwindows_error(error_code, message, fmt::make_format_args(args...));
+}
+
+// Reports a Windows error without throwing an exception.
+// Can be used to report errors from destructors.
+FMT_API void report_windows_error(int error_code,
+                                  const char* message) FMT_NOEXCEPT;
+#else
+inline const std::error_category& system_category() FMT_NOEXCEPT {
+  return std::system_category();
+}
+#endif  // _WIN32
+
+// std::system is not available on some platforms such as iOS (#2248).
+#ifdef __OSX__
+template <typename S, typename... Args, typename Char = char_t<S>>
+void say(const S& format_str, Args&&... args) {
+  std::system(format("say \"{}\"", format(format_str, args...)).c_str());
+}
+#endif
+
+// A buffered file.
+class buffered_file {
+ private:
+  FILE* file_;
+
+  friend class file;
+
+  explicit buffered_file(FILE* f) : file_(f) {}
+
+ public:
+  buffered_file(const buffered_file&) = delete;
+  void operator=(const buffered_file&) = delete;
+
+  // Constructs a buffered_file object which doesn't represent any file.
+  buffered_file() FMT_NOEXCEPT : file_(nullptr) {}
+
+  // Destroys the object closing the file it represents if any.
+  FMT_API ~buffered_file() FMT_NOEXCEPT;
+
+ public:
+  buffered_file(buffered_file&& other) FMT_NOEXCEPT : file_(other.file_) {
+    other.file_ = nullptr;
+  }
+
+  buffered_file& operator=(buffered_file&& other) {
+    close();
+    file_ = other.file_;
+    other.file_ = nullptr;
+    return *this;
+  }
+
+  // Opens a file.
+  FMT_API buffered_file(cstring_view filename, cstring_view mode);
+
+  // Closes the file.
+  FMT_API void close();
+
+  // Returns the pointer to a FILE object representing this file.
+  FILE* get() const FMT_NOEXCEPT { return file_; }
+
+  // We place parentheses around fileno to workaround a bug in some versions
+  // of MinGW that define fileno as a macro.
+  FMT_API int(fileno)() const;
+
+  void vprint(string_view format_str, format_args args) {
+    fmt::vprint(file_, format_str, args);
+  }
+
+  template <typename... Args>
+  inline void print(string_view format_str, const Args&... args) {
+    vprint(format_str, fmt::make_format_args(args...));
+  }
+};
+
+#if FMT_USE_FCNTL
+// A file. Closed file is represented by a file object with descriptor -1.
+// Methods that are not declared with FMT_NOEXCEPT may throw
+// fmt::system_error in case of failure. Note that some errors such as
+// closing the file multiple times will cause a crash on Windows rather
+// than an exception. You can get standard behavior by overriding the
+// invalid parameter handler with _set_invalid_parameter_handler.
+class file {
+ private:
+  int fd_;  // File descriptor.
+
+  // Constructs a file object with a given descriptor.
+  explicit file(int fd) : fd_(fd) {}
+
+ public:
+  // Possible values for the oflag argument to the constructor.
+  enum {
+    RDONLY = FMT_POSIX(O_RDONLY),  // Open for reading only.
+    WRONLY = FMT_POSIX(O_WRONLY),  // Open for writing only.
+    RDWR = FMT_POSIX(O_RDWR),      // Open for reading and writing.
+    CREATE = FMT_POSIX(O_CREAT),   // Create if the file doesn't exist.
+    APPEND = FMT_POSIX(O_APPEND),  // Open in append mode.
+    TRUNC = FMT_POSIX(O_TRUNC)     // Truncate the content of the file.
+  };
+
+  // Constructs a file object which doesn't represent any file.
+  file() FMT_NOEXCEPT : fd_(-1) {}
+
+  // Opens a file and constructs a file object representing this file.
+  FMT_API file(cstring_view path, int oflag);
+
+ public:
+  file(const file&) = delete;
+  void operator=(const file&) = delete;
+
+  file(file&& other) FMT_NOEXCEPT : fd_(other.fd_) { other.fd_ = -1; }
+
+  // Move assignment is not noexcept because close may throw.
+  file& operator=(file&& other) {
+    close();
+    fd_ = other.fd_;
+    other.fd_ = -1;
+    return *this;
+  }
+
+  // Destroys the object closing the file it represents if any.
+  FMT_API ~file() FMT_NOEXCEPT;
+
+  // Returns the file descriptor.
+  int descriptor() const FMT_NOEXCEPT { return fd_; }
+
+  // Closes the file.
+  FMT_API void close();
+
+  // Returns the file size. The size has signed type for consistency with
+  // stat::st_size.
+  FMT_API long long size() const;
+
+  // Attempts to read count bytes from the file into the specified buffer.
+  FMT_API size_t read(void* buffer, size_t count);
+
+  // Attempts to write count bytes from the specified buffer to the file.
+  FMT_API size_t write(const void* buffer, size_t count);
+
+  // Duplicates a file descriptor with the dup function and returns
+  // the duplicate as a file object.
+  FMT_API static file dup(int fd);
+
+  // Makes fd be the copy of this file descriptor, closing fd first if
+  // necessary.
+  FMT_API void dup2(int fd);
+
+  // Makes fd be the copy of this file descriptor, closing fd first if
+  // necessary.
+  FMT_API void dup2(int fd, std::error_code& ec) FMT_NOEXCEPT;
+
+  // Creates a pipe setting up read_end and write_end file objects for reading
+  // and writing respectively.
+  FMT_API static void pipe(file& read_end, file& write_end);
+
+  // Creates a buffered_file object associated with this file and detaches
+  // this file object from the file.
+  FMT_API buffered_file fdopen(const char* mode);
+};
+
+// Returns the memory page size.
+long getpagesize();
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+struct buffer_size {
+  buffer_size() = default;
+  size_t value = 0;
+  buffer_size operator=(size_t val) const {
+    auto bs = buffer_size();
+    bs.value = val;
+    return bs;
+  }
+};
+
+struct ostream_params {
+  int oflag = file::WRONLY | file::CREATE | file::TRUNC;
+  size_t buffer_size = BUFSIZ > 32768 ? BUFSIZ : 32768;
+
+  ostream_params() {}
+
+  template <typename... T>
+  ostream_params(T... params, int new_oflag) : ostream_params(params...) {
+    oflag = new_oflag;
+  }
+
+  template <typename... T>
+  ostream_params(T... params, detail::buffer_size bs)
+      : ostream_params(params...) {
+    this->buffer_size = bs.value;
+  }
+};
+
+FMT_END_DETAIL_NAMESPACE
+
+constexpr detail::buffer_size buffer_size;
+
+/** A fast output stream which is not thread-safe. */
+class FMT_API ostream final : private detail::buffer<char> {
+ private:
+  file file_;
+
+  void flush() {
+    if (size() == 0) return;
+    file_.write(data(), size());
+    clear();
+  }
+
+  void grow(size_t) override;
+
+  ostream(cstring_view path, const detail::ostream_params& params)
+      : file_(path, params.oflag) {
+    set(new char[params.buffer_size], params.buffer_size);
+  }
+
+ public:
+  ostream(ostream&& other)
+      : detail::buffer<char>(other.data(), other.size(), other.capacity()),
+        file_(std::move(other.file_)) {
+    other.clear();
+    other.set(nullptr, 0);
+  }
+  ~ostream() {
+    flush();
+    delete[] data();
+  }
+
+  template <typename... T>
+  friend ostream output_file(cstring_view path, T... params);
+
+  void close() {
+    flush();
+    file_.close();
+  }
+
+  /**
+    Formats ``args`` according to specifications in ``fmt`` and writes the
+    output to the file.
+   */
+  template <typename... T> void print(format_string<T...> fmt, T&&... args) {
+    vformat_to(detail::buffer_appender<char>(*this), fmt,
+               fmt::make_format_args(args...));
+  }
+};
+
+/**
+  \rst
+  Opens a file for writing. Supported parameters passed in *params*:
+
+  * ``<integer>``: Flags passed to `open
+    <https://pubs.opengroup.org/onlinepubs/007904875/functions/open.html>`_
+    (``file::WRONLY | file::CREATE`` by default)
+  * ``buffer_size=<integer>``: Output buffer size
+
+  **Example**::
+
+    auto out = fmt::output_file("guide.txt");
+    out.print("Don't {}", "Panic");
+  \endrst
+ */
+template <typename... T>
+inline ostream output_file(cstring_view path, T... params) {
+  return {path, detail::ostream_params(params...)};
+}
+#endif  // FMT_USE_FCNTL
+
+#ifdef FMT_LOCALE
+// A "C" numeric locale.
+class locale {
+ private:
+#  ifdef _WIN32
+  using locale_t = _locale_t;
+
+  static void freelocale(locale_t loc) { _free_locale(loc); }
+
+  static double strtod_l(const char* nptr, char** endptr, _locale_t loc) {
+    return _strtod_l(nptr, endptr, loc);
+  }
+#  endif
+
+  locale_t locale_;
+
+ public:
+  using type = locale_t;
+  locale(const locale&) = delete;
+  void operator=(const locale&) = delete;
+
+  locale() {
+#  ifndef _WIN32
+    locale_ = FMT_SYSTEM(newlocale(LC_NUMERIC_MASK, "C", nullptr));
+#  else
+    locale_ = _create_locale(LC_NUMERIC, "C");
+#  endif
+    if (!locale_) FMT_THROW(system_error(errno, "cannot create locale"));
+  }
+  ~locale() { freelocale(locale_); }
+
+  type get() const { return locale_; }
+
+  // Converts string to floating-point number and advances str past the end
+  // of the parsed input.
+  double strtod(const char*& str) const {
+    char* end = nullptr;
+    double result = strtod_l(str, &end, locale_);
+    str = end;
+    return result;
+  }
+};
+using Locale FMT_DEPRECATED_ALIAS = locale;
+#endif  // FMT_LOCALE
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_OS_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/ostream.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/ostream.h
new file mode 100644
index 0000000..d66248a
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/ostream.h
@@ -0,0 +1,181 @@
+// Formatting library for C++ - std::ostream support
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_OSTREAM_H_
+#define FMT_OSTREAM_H_
+
+#include <ostream>
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+
+template <typename Char> class basic_printf_parse_context;
+template <typename OutputIt, typename Char> class basic_printf_context;
+
+namespace detail {
+
+template <class Char> class formatbuf : public std::basic_streambuf<Char> {
+ private:
+  using int_type = typename std::basic_streambuf<Char>::int_type;
+  using traits_type = typename std::basic_streambuf<Char>::traits_type;
+
+  buffer<Char>& buffer_;
+
+ public:
+  formatbuf(buffer<Char>& buf) : buffer_(buf) {}
+
+ protected:
+  // The put-area is actually always empty. This makes the implementation
+  // simpler and has the advantage that the streambuf and the buffer are always
+  // in sync and sputc never writes into uninitialized memory. The obvious
+  // disadvantage is that each call to sputc always results in a (virtual) call
+  // to overflow. There is no disadvantage here for sputn since this always
+  // results in a call to xsputn.
+
+  int_type overflow(int_type ch = traits_type::eof()) FMT_OVERRIDE {
+    if (!traits_type::eq_int_type(ch, traits_type::eof()))
+      buffer_.push_back(static_cast<Char>(ch));
+    return ch;
+  }
+
+  std::streamsize xsputn(const Char* s, std::streamsize count) FMT_OVERRIDE {
+    buffer_.append(s, s + count);
+    return count;
+  }
+};
+
+struct converter {
+  template <typename T, FMT_ENABLE_IF(is_integral<T>::value)> converter(T);
+};
+
+template <typename Char> struct test_stream : std::basic_ostream<Char> {
+ private:
+  void_t<> operator<<(converter);
+};
+
+// Hide insertion operators for built-in types.
+template <typename Char, typename Traits>
+void_t<> operator<<(std::basic_ostream<Char, Traits>&, Char);
+template <typename Char, typename Traits>
+void_t<> operator<<(std::basic_ostream<Char, Traits>&, char);
+template <typename Traits>
+void_t<> operator<<(std::basic_ostream<char, Traits>&, char);
+template <typename Traits>
+void_t<> operator<<(std::basic_ostream<char, Traits>&, signed char);
+template <typename Traits>
+void_t<> operator<<(std::basic_ostream<char, Traits>&, unsigned char);
+
+// Checks if T has a user-defined operator<< (e.g. not a member of
+// std::ostream).
+template <typename T, typename Char> class is_streamable {
+ private:
+  template <typename U>
+  static bool_constant<!std::is_same<decltype(std::declval<test_stream<Char>&>()
+                                              << std::declval<U>()),
+                                     void_t<>>::value>
+  test(int);
+
+  template <typename> static std::false_type test(...);
+
+  using result = decltype(test<T>(0));
+
+ public:
+  is_streamable() = default;
+
+  static const bool value = result::value;
+};
+
+// Write the content of buf to os.
+template <typename Char>
+void write_buffer(std::basic_ostream<Char>& os, buffer<Char>& buf) {
+  const Char* buf_data = buf.data();
+  using unsigned_streamsize = std::make_unsigned<std::streamsize>::type;
+  unsigned_streamsize size = buf.size();
+  unsigned_streamsize max_size = to_unsigned(max_value<std::streamsize>());
+  do {
+    unsigned_streamsize n = size <= max_size ? size : max_size;
+    os.write(buf_data, static_cast<std::streamsize>(n));
+    buf_data += n;
+    size -= n;
+  } while (size != 0);
+}
+
+template <typename Char, typename T>
+void format_value(buffer<Char>& buf, const T& value,
+                  locale_ref loc = locale_ref()) {
+  formatbuf<Char> format_buf(buf);
+  std::basic_ostream<Char> output(&format_buf);
+#if !defined(FMT_STATIC_THOUSANDS_SEPARATOR)
+  if (loc) output.imbue(loc.get<std::locale>());
+#endif
+  output << value;
+  output.exceptions(std::ios_base::failbit | std::ios_base::badbit);
+  buf.try_resize(buf.size());
+}
+
+// Formats an object of type T that has an overloaded ostream operator<<.
+template <typename T, typename Char>
+struct fallback_formatter<T, Char, enable_if_t<is_streamable<T, Char>::value>>
+    : private formatter<basic_string_view<Char>, Char> {
+  FMT_CONSTEXPR auto parse(basic_format_parse_context<Char>& ctx)
+      -> decltype(ctx.begin()) {
+    return formatter<basic_string_view<Char>, Char>::parse(ctx);
+  }
+  template <typename ParseCtx,
+            FMT_ENABLE_IF(std::is_same<
+                          ParseCtx, basic_printf_parse_context<Char>>::value)>
+  auto parse(ParseCtx& ctx) -> decltype(ctx.begin()) {
+    return ctx.begin();
+  }
+
+  template <typename OutputIt>
+  auto format(const T& value, basic_format_context<OutputIt, Char>& ctx)
+      -> OutputIt {
+    basic_memory_buffer<Char> buffer;
+    format_value(buffer, value, ctx.locale());
+    basic_string_view<Char> str(buffer.data(), buffer.size());
+    return formatter<basic_string_view<Char>, Char>::format(str, ctx);
+  }
+  template <typename OutputIt>
+  auto format(const T& value, basic_printf_context<OutputIt, Char>& ctx)
+      -> OutputIt {
+    basic_memory_buffer<Char> buffer;
+    format_value(buffer, value, ctx.locale());
+    return std::copy(buffer.begin(), buffer.end(), ctx.out());
+  }
+};
+}  // namespace detail
+
+FMT_MODULE_EXPORT
+template <typename Char>
+void vprint(std::basic_ostream<Char>& os, basic_string_view<Char> format_str,
+            basic_format_args<buffer_context<type_identity_t<Char>>> args) {
+  basic_memory_buffer<Char> buffer;
+  detail::vformat_to(buffer, format_str, args);
+  detail::write_buffer(os, buffer);
+}
+
+/**
+  \rst
+  Prints formatted data to the stream *os*.
+
+  **Example**::
+
+    fmt::print(cerr, "Don't {}!", "panic");
+  \endrst
+ */
+FMT_MODULE_EXPORT
+template <typename S, typename... Args,
+          typename Char = enable_if_t<detail::is_string<S>::value, char_t<S>>>
+void print(std::basic_ostream<Char>& os, const S& format_str, Args&&... args) {
+  vprint(os, to_string_view(format_str),
+         fmt::make_args_checked<Args...>(format_str, args...));
+}
+FMT_END_NAMESPACE
+
+#endif  // FMT_OSTREAM_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/printf.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/printf.h
new file mode 100644
index 0000000..3a3cd15
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/printf.h
@@ -0,0 +1,652 @@
+// Formatting library for C++ - legacy printf implementation
+//
+// Copyright (c) 2012 - 2016, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_PRINTF_H_
+#define FMT_PRINTF_H_
+
+#include <algorithm>  // std::max
+#include <limits>     // std::numeric_limits
+#include <ostream>
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+FMT_MODULE_EXPORT_BEGIN
+
+template <typename T> struct printf_formatter { printf_formatter() = delete; };
+
+template <typename Char>
+class basic_printf_parse_context : public basic_format_parse_context<Char> {
+  using basic_format_parse_context<Char>::basic_format_parse_context;
+};
+
+template <typename OutputIt, typename Char> class basic_printf_context {
+ private:
+  OutputIt out_;
+  basic_format_args<basic_printf_context> args_;
+
+ public:
+  using char_type = Char;
+  using format_arg = basic_format_arg<basic_printf_context>;
+  using parse_context_type = basic_printf_parse_context<Char>;
+  template <typename T> using formatter_type = printf_formatter<T>;
+
+  /**
+    \rst
+    Constructs a ``printf_context`` object. References to the arguments are
+    stored in the context object so make sure they have appropriate lifetimes.
+    \endrst
+   */
+  basic_printf_context(OutputIt out,
+                       basic_format_args<basic_printf_context> args)
+      : out_(out), args_(args) {}
+
+  OutputIt out() { return out_; }
+  void advance_to(OutputIt it) { out_ = it; }
+
+  detail::locale_ref locale() { return {}; }
+
+  format_arg arg(int id) const { return args_.get(id); }
+
+  FMT_CONSTEXPR void on_error(const char* message) {
+    detail::error_handler().on_error(message);
+  }
+};
+
+FMT_BEGIN_DETAIL_NAMESPACE
+
+// Checks if a value fits in int - used to avoid warnings about comparing
+// signed and unsigned integers.
+template <bool IsSigned> struct int_checker {
+  template <typename T> static bool fits_in_int(T value) {
+    unsigned max = max_value<int>();
+    return value <= max;
+  }
+  static bool fits_in_int(bool) { return true; }
+};
+
+template <> struct int_checker<true> {
+  template <typename T> static bool fits_in_int(T value) {
+    return value >= (std::numeric_limits<int>::min)() &&
+           value <= max_value<int>();
+  }
+  static bool fits_in_int(int) { return true; }
+};
+
+class printf_precision_handler {
+ public:
+  template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+  int operator()(T value) {
+    if (!int_checker<std::numeric_limits<T>::is_signed>::fits_in_int(value))
+      FMT_THROW(format_error("number is too big"));
+    return (std::max)(static_cast<int>(value), 0);
+  }
+
+  template <typename T, FMT_ENABLE_IF(!std::is_integral<T>::value)>
+  int operator()(T) {
+    FMT_THROW(format_error("precision is not integer"));
+    return 0;
+  }
+};
+
+// An argument visitor that returns true iff arg is a zero integer.
+class is_zero_int {
+ public:
+  template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+  bool operator()(T value) {
+    return value == 0;
+  }
+
+  template <typename T, FMT_ENABLE_IF(!std::is_integral<T>::value)>
+  bool operator()(T) {
+    return false;
+  }
+};
+
+template <typename T> struct make_unsigned_or_bool : std::make_unsigned<T> {};
+
+template <> struct make_unsigned_or_bool<bool> { using type = bool; };
+
+template <typename T, typename Context> class arg_converter {
+ private:
+  using char_type = typename Context::char_type;
+
+  basic_format_arg<Context>& arg_;
+  char_type type_;
+
+ public:
+  arg_converter(basic_format_arg<Context>& arg, char_type type)
+      : arg_(arg), type_(type) {}
+
+  void operator()(bool value) {
+    if (type_ != 's') operator()<bool>(value);
+  }
+
+  template <typename U, FMT_ENABLE_IF(std::is_integral<U>::value)>
+  void operator()(U value) {
+    bool is_signed = type_ == 'd' || type_ == 'i';
+    using target_type = conditional_t<std::is_same<T, void>::value, U, T>;
+    if (const_check(sizeof(target_type) <= sizeof(int))) {
+      // Extra casts are used to silence warnings.
+      if (is_signed) {
+        arg_ = detail::make_arg<Context>(
+            static_cast<int>(static_cast<target_type>(value)));
+      } else {
+        using unsigned_type = typename make_unsigned_or_bool<target_type>::type;
+        arg_ = detail::make_arg<Context>(
+            static_cast<unsigned>(static_cast<unsigned_type>(value)));
+      }
+    } else {
+      if (is_signed) {
+        // glibc's printf doesn't sign extend arguments of smaller types:
+        //   std::printf("%lld", -42);  // prints "4294967254"
+        // but we don't have to do the same because it's a UB.
+        arg_ = detail::make_arg<Context>(static_cast<long long>(value));
+      } else {
+        arg_ = detail::make_arg<Context>(
+            static_cast<typename make_unsigned_or_bool<U>::type>(value));
+      }
+    }
+  }
+
+  template <typename U, FMT_ENABLE_IF(!std::is_integral<U>::value)>
+  void operator()(U) {}  // No conversion needed for non-integral types.
+};
+
+// Converts an integer argument to T for printf, if T is an integral type.
+// If T is void, the argument is converted to corresponding signed or unsigned
+// type depending on the type specifier: 'd' and 'i' - signed, other -
+// unsigned).
+template <typename T, typename Context, typename Char>
+void convert_arg(basic_format_arg<Context>& arg, Char type) {
+  visit_format_arg(arg_converter<T, Context>(arg, type), arg);
+}
+
+// Converts an integer argument to char for printf.
+template <typename Context> class char_converter {
+ private:
+  basic_format_arg<Context>& arg_;
+
+ public:
+  explicit char_converter(basic_format_arg<Context>& arg) : arg_(arg) {}
+
+  template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+  void operator()(T value) {
+    arg_ = detail::make_arg<Context>(
+        static_cast<typename Context::char_type>(value));
+  }
+
+  template <typename T, FMT_ENABLE_IF(!std::is_integral<T>::value)>
+  void operator()(T) {}  // No conversion needed for non-integral types.
+};
+
+// An argument visitor that return a pointer to a C string if argument is a
+// string or null otherwise.
+template <typename Char> struct get_cstring {
+  template <typename T> const Char* operator()(T) { return nullptr; }
+  const Char* operator()(const Char* s) { return s; }
+};
+
+// Checks if an argument is a valid printf width specifier and sets
+// left alignment if it is negative.
+template <typename Char> class printf_width_handler {
+ private:
+  using format_specs = basic_format_specs<Char>;
+
+  format_specs& specs_;
+
+ public:
+  explicit printf_width_handler(format_specs& specs) : specs_(specs) {}
+
+  template <typename T, FMT_ENABLE_IF(std::is_integral<T>::value)>
+  unsigned operator()(T value) {
+    auto width = static_cast<uint32_or_64_or_128_t<T>>(value);
+    if (detail::is_negative(value)) {
+      specs_.align = align::left;
+      width = 0 - width;
+    }
+    unsigned int_max = max_value<int>();
+    if (width > int_max) FMT_THROW(format_error("number is too big"));
+    return static_cast<unsigned>(width);
+  }
+
+  template <typename T, FMT_ENABLE_IF(!std::is_integral<T>::value)>
+  unsigned operator()(T) {
+    FMT_THROW(format_error("width is not integer"));
+    return 0;
+  }
+};
+
+// The ``printf`` argument formatter.
+template <typename OutputIt, typename Char>
+class printf_arg_formatter : public arg_formatter<Char> {
+ private:
+  using base = arg_formatter<Char>;
+  using context_type = basic_printf_context<OutputIt, Char>;
+  using format_specs = basic_format_specs<Char>;
+
+  context_type& context_;
+
+  OutputIt write_null_pointer(bool is_string = false) {
+    auto s = this->specs;
+    s.type = 0;
+    return write_bytes(this->out, is_string ? "(null)" : "(nil)", s);
+  }
+
+ public:
+  printf_arg_formatter(OutputIt iter, format_specs& s, context_type& ctx)
+      : base{iter, s, locale_ref()}, context_(ctx) {}
+
+  OutputIt operator()(monostate value) { return base::operator()(value); }
+
+  template <typename T, FMT_ENABLE_IF(detail::is_integral<T>::value)>
+  OutputIt operator()(T value) {
+    // MSVC2013 fails to compile separate overloads for bool and Char so use
+    // std::is_same instead.
+    if (std::is_same<T, Char>::value) {
+      format_specs fmt_specs = this->specs;
+      if (fmt_specs.type && fmt_specs.type != 'c')
+        return (*this)(static_cast<int>(value));
+      fmt_specs.sign = sign::none;
+      fmt_specs.alt = false;
+      fmt_specs.fill[0] = ' ';  // Ignore '0' flag for char types.
+      // align::numeric needs to be overwritten here since the '0' flag is
+      // ignored for non-numeric types
+      if (fmt_specs.align == align::none || fmt_specs.align == align::numeric)
+        fmt_specs.align = align::right;
+      return write<Char>(this->out, static_cast<Char>(value), fmt_specs);
+    }
+    return base::operator()(value);
+  }
+
+  template <typename T, FMT_ENABLE_IF(std::is_floating_point<T>::value)>
+  OutputIt operator()(T value) {
+    return base::operator()(value);
+  }
+
+  /** Formats a null-terminated C string. */
+  OutputIt operator()(const char* value) {
+    if (value) return base::operator()(value);
+    return write_null_pointer(this->specs.type != 'p');
+  }
+
+  /** Formats a null-terminated wide C string. */
+  OutputIt operator()(const wchar_t* value) {
+    if (value) return base::operator()(value);
+    return write_null_pointer(this->specs.type != 'p');
+  }
+
+  OutputIt operator()(basic_string_view<Char> value) {
+    return base::operator()(value);
+  }
+
+  /** Formats a pointer. */
+  OutputIt operator()(const void* value) {
+    return value ? base::operator()(value) : write_null_pointer();
+  }
+
+  /** Formats an argument of a custom (user-defined) type. */
+  OutputIt operator()(typename basic_format_arg<context_type>::handle handle) {
+    auto parse_ctx =
+        basic_printf_parse_context<Char>(basic_string_view<Char>());
+    handle.format(parse_ctx, context_);
+    return this->out;
+  }
+};
+
+template <typename Char>
+void parse_flags(basic_format_specs<Char>& specs, const Char*& it,
+                 const Char* end) {
+  for (; it != end; ++it) {
+    switch (*it) {
+    case '-':
+      specs.align = align::left;
+      break;
+    case '+':
+      specs.sign = sign::plus;
+      break;
+    case '0':
+      specs.fill[0] = '0';
+      break;
+    case ' ':
+      if (specs.sign != sign::plus) {
+        specs.sign = sign::space;
+      }
+      break;
+    case '#':
+      specs.alt = true;
+      break;
+    default:
+      return;
+    }
+  }
+}
+
+template <typename Char, typename GetArg>
+int parse_header(const Char*& it, const Char* end,
+                 basic_format_specs<Char>& specs, GetArg get_arg) {
+  int arg_index = -1;
+  Char c = *it;
+  if (c >= '0' && c <= '9') {
+    // Parse an argument index (if followed by '$') or a width possibly
+    // preceded with '0' flag(s).
+    int value = parse_nonnegative_int(it, end, -1);
+    if (it != end && *it == '$') {  // value is an argument index
+      ++it;
+      arg_index = value != -1 ? value : max_value<int>();
+    } else {
+      if (c == '0') specs.fill[0] = '0';
+      if (value != 0) {
+        // Nonzero value means that we parsed width and don't need to
+        // parse it or flags again, so return now.
+        if (value == -1) FMT_THROW(format_error("number is too big"));
+        specs.width = value;
+        return arg_index;
+      }
+    }
+  }
+  parse_flags(specs, it, end);
+  // Parse width.
+  if (it != end) {
+    if (*it >= '0' && *it <= '9') {
+      specs.width = parse_nonnegative_int(it, end, -1);
+      if (specs.width == -1) FMT_THROW(format_error("number is too big"));
+    } else if (*it == '*') {
+      ++it;
+      specs.width = static_cast<int>(visit_format_arg(
+          detail::printf_width_handler<Char>(specs), get_arg(-1)));
+    }
+  }
+  return arg_index;
+}
+
+template <typename Char, typename Context>
+void vprintf(buffer<Char>& buf, basic_string_view<Char> format,
+             basic_format_args<Context> args) {
+  using OutputIt = buffer_appender<Char>;
+  auto out = OutputIt(buf);
+  auto context = basic_printf_context<OutputIt, Char>(out, args);
+  auto parse_ctx = basic_printf_parse_context<Char>(format);
+
+  // Returns the argument with specified index or, if arg_index is -1, the next
+  // argument.
+  auto get_arg = [&](int arg_index) {
+    if (arg_index < 0)
+      arg_index = parse_ctx.next_arg_id();
+    else
+      parse_ctx.check_arg_id(--arg_index);
+    return detail::get_arg(context, arg_index);
+  };
+
+  const Char* start = parse_ctx.begin();
+  const Char* end = parse_ctx.end();
+  auto it = start;
+  while (it != end) {
+    if (!detail::find<false, Char>(it, end, '%', it)) {
+      it = end;  // detail::find leaves it == nullptr if it doesn't find '%'
+      break;
+    }
+    Char c = *it++;
+    if (it != end && *it == c) {
+      out = detail::write(
+          out, basic_string_view<Char>(start, detail::to_unsigned(it - start)));
+      start = ++it;
+      continue;
+    }
+    out = detail::write(out, basic_string_view<Char>(
+                                 start, detail::to_unsigned(it - 1 - start)));
+
+    basic_format_specs<Char> specs;
+    specs.align = align::right;
+
+    // Parse argument index, flags and width.
+    int arg_index = parse_header(it, end, specs, get_arg);
+    if (arg_index == 0) parse_ctx.on_error("argument not found");
+
+    // Parse precision.
+    if (it != end && *it == '.') {
+      ++it;
+      c = it != end ? *it : 0;
+      if ('0' <= c && c <= '9') {
+        specs.precision = parse_nonnegative_int(it, end, 0);
+      } else if (c == '*') {
+        ++it;
+        specs.precision = static_cast<int>(
+            visit_format_arg(detail::printf_precision_handler(), get_arg(-1)));
+      } else {
+        specs.precision = 0;
+      }
+    }
+
+    auto arg = get_arg(arg_index);
+    // For d, i, o, u, x, and X conversion specifiers, if a precision is
+    // specified, the '0' flag is ignored
+    if (specs.precision >= 0 && arg.is_integral())
+      specs.fill[0] =
+          ' ';  // Ignore '0' flag for non-numeric types or if '-' present.
+    if (specs.precision >= 0 && arg.type() == detail::type::cstring_type) {
+      auto str = visit_format_arg(detail::get_cstring<Char>(), arg);
+      auto str_end = str + specs.precision;
+      auto nul = std::find(str, str_end, Char());
+      arg = detail::make_arg<basic_printf_context<OutputIt, Char>>(
+          basic_string_view<Char>(
+              str, detail::to_unsigned(nul != str_end ? nul - str
+                                                      : specs.precision)));
+    }
+    if (specs.alt && visit_format_arg(detail::is_zero_int(), arg))
+      specs.alt = false;
+    if (specs.fill[0] == '0') {
+      if (arg.is_arithmetic() && specs.align != align::left)
+        specs.align = align::numeric;
+      else
+        specs.fill[0] = ' ';  // Ignore '0' flag for non-numeric types or if '-'
+                              // flag is also present.
+    }
+
+    // Parse length and convert the argument to the required type.
+    c = it != end ? *it++ : 0;
+    Char t = it != end ? *it : 0;
+    using detail::convert_arg;
+    switch (c) {
+    case 'h':
+      if (t == 'h') {
+        ++it;
+        t = it != end ? *it : 0;
+        convert_arg<signed char>(arg, t);
+      } else {
+        convert_arg<short>(arg, t);
+      }
+      break;
+    case 'l':
+      if (t == 'l') {
+        ++it;
+        t = it != end ? *it : 0;
+        convert_arg<long long>(arg, t);
+      } else {
+        convert_arg<long>(arg, t);
+      }
+      break;
+    case 'j':
+      convert_arg<intmax_t>(arg, t);
+      break;
+    case 'z':
+      convert_arg<size_t>(arg, t);
+      break;
+    case 't':
+      convert_arg<std::ptrdiff_t>(arg, t);
+      break;
+    case 'L':
+      // printf produces garbage when 'L' is omitted for long double, no
+      // need to do the same.
+      break;
+    default:
+      --it;
+      convert_arg<void>(arg, c);
+    }
+
+    // Parse type.
+    if (it == end) FMT_THROW(format_error("invalid format string"));
+    specs.type = static_cast<char>(*it++);
+    if (arg.is_integral()) {
+      // Normalize type.
+      switch (specs.type) {
+      case 'i':
+      case 'u':
+        specs.type = 'd';
+        break;
+      case 'c':
+        visit_format_arg(
+            detail::char_converter<basic_printf_context<OutputIt, Char>>(arg),
+            arg);
+        break;
+      }
+    }
+
+    start = it;
+
+    // Format argument.
+    out = visit_format_arg(
+        detail::printf_arg_formatter<OutputIt, Char>(out, specs, context), arg);
+  }
+  detail::write(out, basic_string_view<Char>(start, to_unsigned(it - start)));
+}
+FMT_END_DETAIL_NAMESPACE
+
+template <typename Char>
+using basic_printf_context_t =
+    basic_printf_context<detail::buffer_appender<Char>, Char>;
+
+using printf_context = basic_printf_context_t<char>;
+using wprintf_context = basic_printf_context_t<wchar_t>;
+
+using printf_args = basic_format_args<printf_context>;
+using wprintf_args = basic_format_args<wprintf_context>;
+
+/**
+  \rst
+  Constructs an `~fmt::format_arg_store` object that contains references to
+  arguments and can be implicitly converted to `~fmt::printf_args`.
+  \endrst
+ */
+template <typename... T>
+inline auto make_printf_args(const T&... args)
+    -> format_arg_store<printf_context, T...> {
+  return {args...};
+}
+
+/**
+  \rst
+  Constructs an `~fmt::format_arg_store` object that contains references to
+  arguments and can be implicitly converted to `~fmt::wprintf_args`.
+  \endrst
+ */
+template <typename... T>
+inline auto make_wprintf_args(const T&... args)
+    -> format_arg_store<wprintf_context, T...> {
+  return {args...};
+}
+
+template <typename S, typename Char = char_t<S>>
+inline auto vsprintf(
+    const S& fmt,
+    basic_format_args<basic_printf_context_t<type_identity_t<Char>>> args)
+    -> std::basic_string<Char> {
+  basic_memory_buffer<Char> buffer;
+  vprintf(buffer, to_string_view(fmt), args);
+  return to_string(buffer);
+}
+
+/**
+  \rst
+  Formats arguments and returns the result as a string.
+
+  **Example**::
+
+    std::string message = fmt::sprintf("The answer is %d", 42);
+  \endrst
+*/
+template <typename S, typename... T,
+          typename Char = enable_if_t<detail::is_string<S>::value, char_t<S>>>
+inline auto sprintf(const S& fmt, const T&... args) -> std::basic_string<Char> {
+  using context = basic_printf_context_t<Char>;
+  return vsprintf(to_string_view(fmt), fmt::make_format_args<context>(args...));
+}
+
+template <typename S, typename Char = char_t<S>>
+inline auto vfprintf(
+    std::FILE* f, const S& fmt,
+    basic_format_args<basic_printf_context_t<type_identity_t<Char>>> args)
+    -> int {
+  basic_memory_buffer<Char> buffer;
+  vprintf(buffer, to_string_view(fmt), args);
+  size_t size = buffer.size();
+  return std::fwrite(buffer.data(), sizeof(Char), size, f) < size
+             ? -1
+             : static_cast<int>(size);
+}
+
+/**
+  \rst
+  Prints formatted data to the file *f*.
+
+  **Example**::
+
+    fmt::fprintf(stderr, "Don't %s!", "panic");
+  \endrst
+ */
+template <typename S, typename... T, typename Char = char_t<S>>
+inline auto fprintf(std::FILE* f, const S& fmt, const T&... args) -> int {
+  using context = basic_printf_context_t<Char>;
+  return vfprintf(f, to_string_view(fmt),
+                  fmt::make_format_args<context>(args...));
+}
+
+template <typename S, typename Char = char_t<S>>
+inline auto vprintf(
+    const S& fmt,
+    basic_format_args<basic_printf_context_t<type_identity_t<Char>>> args)
+    -> int {
+  return vfprintf(stdout, to_string_view(fmt), args);
+}
+
+/**
+  \rst
+  Prints formatted data to ``stdout``.
+
+  **Example**::
+
+    fmt::printf("Elapsed time: %.2f seconds", 1.23);
+  \endrst
+ */
+template <typename S, typename... T, FMT_ENABLE_IF(detail::is_string<S>::value)>
+inline auto printf(const S& fmt, const T&... args) -> int {
+  return vprintf(
+      to_string_view(fmt),
+      fmt::make_format_args<basic_printf_context_t<char_t<S>>>(args...));
+}
+
+template <typename S, typename Char = char_t<S>>
+FMT_DEPRECATED auto vfprintf(
+    std::basic_ostream<Char>& os, const S& fmt,
+    basic_format_args<basic_printf_context_t<type_identity_t<Char>>> args)
+    -> int {
+  basic_memory_buffer<Char> buffer;
+  vprintf(buffer, to_string_view(fmt), args);
+  os.write(buffer.data(), static_cast<std::streamsize>(buffer.size()));
+  return static_cast<int>(buffer.size());
+}
+template <typename S, typename... T, typename Char = char_t<S>>
+FMT_DEPRECATED auto fprintf(std::basic_ostream<Char>& os, const S& fmt,
+                            const T&... args) -> int {
+  return vfprintf(os, to_string_view(fmt),
+                  fmt::make_format_args<basic_printf_context_t<Char>>(args...));
+}
+
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_PRINTF_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/ranges.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/ranges.h
new file mode 100644
index 0000000..f0390df
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/ranges.h
@@ -0,0 +1,468 @@
+// Formatting library for C++ - experimental range support
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+//
+// Copyright (c) 2018 - present, Remotion (Igor Schulz)
+// All Rights Reserved
+// {fmt} support for ranges, containers and types tuple interface.
+
+#ifndef FMT_RANGES_H_
+#define FMT_RANGES_H_
+
+#include <initializer_list>
+#include <type_traits>
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+
+template <typename Char, typename Enable = void> struct formatting_range {
+#ifdef FMT_DEPRECATED_BRACED_RANGES
+  Char prefix = '{';
+  Char postfix = '}';
+#else
+  Char prefix = '[';
+  Char postfix = ']';
+#endif
+
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return ctx.begin();
+  }
+};
+
+template <typename Char, typename Enable = void> struct formatting_tuple {
+  Char prefix = '(';
+  Char postfix = ')';
+
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return ctx.begin();
+  }
+};
+
+namespace detail {
+
+template <typename RangeT, typename OutputIterator>
+OutputIterator copy(const RangeT& range, OutputIterator out) {
+  for (auto it = range.begin(), end = range.end(); it != end; ++it)
+    *out++ = *it;
+  return out;
+}
+
+template <typename OutputIterator>
+OutputIterator copy(const char* str, OutputIterator out) {
+  while (*str) *out++ = *str++;
+  return out;
+}
+
+template <typename OutputIterator>
+OutputIterator copy(char ch, OutputIterator out) {
+  *out++ = ch;
+  return out;
+}
+
+template <typename OutputIterator>
+OutputIterator copy(wchar_t ch, OutputIterator out) {
+  *out++ = ch;
+  return out;
+}
+
+/// Return true value if T has std::string interface, like std::string_view.
+template <typename T> class is_std_string_like {
+  template <typename U>
+  static auto check(U* p)
+      -> decltype((void)p->find('a'), p->length(), (void)p->data(), int());
+  template <typename> static void check(...);
+
+ public:
+  static FMT_CONSTEXPR_DECL const bool value =
+      is_string<T>::value || !std::is_void<decltype(check<T>(nullptr))>::value;
+};
+
+template <typename Char>
+struct is_std_string_like<fmt::basic_string_view<Char>> : std::true_type {};
+
+template <typename... Ts> struct conditional_helper {};
+
+template <typename T, typename _ = void> struct is_range_ : std::false_type {};
+
+#if !FMT_MSC_VER || FMT_MSC_VER > 1800
+
+#  define FMT_DECLTYPE_RETURN(val)  \
+    ->decltype(val) { return val; } \
+    static_assert(                  \
+        true, "")  // This makes it so that a semicolon is required after the
+                   // macro, which helps clang-format handle the formatting.
+
+// C array overload
+template <typename T, std::size_t N>
+auto range_begin(const T (&arr)[N]) -> const T* {
+  return arr;
+}
+template <typename T, std::size_t N>
+auto range_end(const T (&arr)[N]) -> const T* {
+  return arr + N;
+}
+
+template <typename T, typename Enable = void>
+struct has_member_fn_begin_end_t : std::false_type {};
+
+template <typename T>
+struct has_member_fn_begin_end_t<T, void_t<decltype(std::declval<T>().begin()),
+                                           decltype(std::declval<T>().end())>>
+    : std::true_type {};
+
+// Member function overload
+template <typename T>
+auto range_begin(T&& rng) FMT_DECLTYPE_RETURN(static_cast<T&&>(rng).begin());
+template <typename T>
+auto range_end(T&& rng) FMT_DECLTYPE_RETURN(static_cast<T&&>(rng).end());
+
+// ADL overload. Only participates in overload resolution if member functions
+// are not found.
+template <typename T>
+auto range_begin(T&& rng)
+    -> enable_if_t<!has_member_fn_begin_end_t<T&&>::value,
+                   decltype(begin(static_cast<T&&>(rng)))> {
+  return begin(static_cast<T&&>(rng));
+}
+template <typename T>
+auto range_end(T&& rng) -> enable_if_t<!has_member_fn_begin_end_t<T&&>::value,
+                                       decltype(end(static_cast<T&&>(rng)))> {
+  return end(static_cast<T&&>(rng));
+}
+
+template <typename T, typename Enable = void>
+struct has_const_begin_end : std::false_type {};
+template <typename T, typename Enable = void>
+struct has_mutable_begin_end : std::false_type {};
+
+template <typename T>
+struct has_const_begin_end<
+    T, void_t<decltype(detail::range_begin(
+                  std::declval<const remove_cvref_t<T>&>())),
+              decltype(detail::range_begin(
+                  std::declval<const remove_cvref_t<T>&>()))>>
+    : std::true_type {};
+
+template <typename T>
+struct has_mutable_begin_end<
+    T, void_t<decltype(detail::range_begin(std::declval<T>())),
+              decltype(detail::range_begin(std::declval<T>())),
+              enable_if_t<std::is_copy_constructible<T>::value>>>
+    : std::true_type {};
+
+template <typename T>
+struct is_range_<T, void>
+    : std::integral_constant<bool, (has_const_begin_end<T>::value ||
+                                    has_mutable_begin_end<T>::value)> {};
+
+template <typename T, typename Enable = void> struct range_to_view;
+template <typename T>
+struct range_to_view<T, enable_if_t<has_const_begin_end<T>::value>> {
+  struct view_t {
+    const T* m_range_ptr;
+
+    auto begin() const FMT_DECLTYPE_RETURN(detail::range_begin(*m_range_ptr));
+    auto end() const FMT_DECLTYPE_RETURN(detail::range_end(*m_range_ptr));
+  };
+  static auto view(const T& range) -> view_t { return {&range}; }
+};
+
+template <typename T>
+struct range_to_view<T, enable_if_t<!has_const_begin_end<T>::value &&
+                                    has_mutable_begin_end<T>::value>> {
+  struct view_t {
+    T m_range_copy;
+
+    auto begin() FMT_DECLTYPE_RETURN(detail::range_begin(m_range_copy));
+    auto end() FMT_DECLTYPE_RETURN(detail::range_end(m_range_copy));
+  };
+  static auto view(const T& range) -> view_t { return {range}; }
+};
+#  undef FMT_DECLTYPE_RETURN
+#endif
+
+/// tuple_size and tuple_element check.
+template <typename T> class is_tuple_like_ {
+  template <typename U>
+  static auto check(U* p) -> decltype(std::tuple_size<U>::value, int());
+  template <typename> static void check(...);
+
+ public:
+  static FMT_CONSTEXPR_DECL const bool value =
+      !std::is_void<decltype(check<T>(nullptr))>::value;
+};
+
+// Check for integer_sequence
+#if defined(__cpp_lib_integer_sequence) || FMT_MSC_VER >= 1900
+template <typename T, T... N>
+using integer_sequence = std::integer_sequence<T, N...>;
+template <size_t... N> using index_sequence = std::index_sequence<N...>;
+template <size_t N> using make_index_sequence = std::make_index_sequence<N>;
+#else
+template <typename T, T... N> struct integer_sequence {
+  using value_type = T;
+
+  static FMT_CONSTEXPR size_t size() { return sizeof...(N); }
+};
+
+template <size_t... N> using index_sequence = integer_sequence<size_t, N...>;
+
+template <typename T, size_t N, T... Ns>
+struct make_integer_sequence : make_integer_sequence<T, N - 1, N - 1, Ns...> {};
+template <typename T, T... Ns>
+struct make_integer_sequence<T, 0, Ns...> : integer_sequence<T, Ns...> {};
+
+template <size_t N>
+using make_index_sequence = make_integer_sequence<size_t, N>;
+#endif
+
+template <class Tuple, class F, size_t... Is>
+void for_each(index_sequence<Is...>, Tuple&& tup, F&& f) FMT_NOEXCEPT {
+  using std::get;
+  // using free function get<I>(T) now.
+  const int _[] = {0, ((void)f(get<Is>(tup)), 0)...};
+  (void)_;  // blocks warnings
+}
+
+template <class T>
+FMT_CONSTEXPR make_index_sequence<std::tuple_size<T>::value> get_indexes(
+    T const&) {
+  return {};
+}
+
+template <class Tuple, class F> void for_each(Tuple&& tup, F&& f) {
+  const auto indexes = get_indexes(tup);
+  for_each(indexes, std::forward<Tuple>(tup), std::forward<F>(f));
+}
+
+template <typename Range>
+using value_type =
+    remove_cvref_t<decltype(*detail::range_begin(std::declval<Range>()))>;
+
+template <typename OutputIt> OutputIt write_delimiter(OutputIt out) {
+  *out++ = ',';
+  *out++ = ' ';
+  return out;
+}
+
+template <
+    typename Char, typename OutputIt, typename Arg,
+    FMT_ENABLE_IF(is_std_string_like<typename std::decay<Arg>::type>::value)>
+OutputIt write_range_entry(OutputIt out, const Arg& v) {
+  *out++ = '"';
+  out = write<Char>(out, v);
+  *out++ = '"';
+  return out;
+}
+
+template <typename Char, typename OutputIt, typename Arg,
+          FMT_ENABLE_IF(std::is_same<Arg, Char>::value)>
+OutputIt write_range_entry(OutputIt out, const Arg v) {
+  *out++ = '\'';
+  *out++ = v;
+  *out++ = '\'';
+  return out;
+}
+
+template <
+    typename Char, typename OutputIt, typename Arg,
+    FMT_ENABLE_IF(!is_std_string_like<typename std::decay<Arg>::type>::value &&
+                  !std::is_same<Arg, Char>::value)>
+OutputIt write_range_entry(OutputIt out, const Arg& v) {
+  return write<Char>(out, v);
+}
+
+}  // namespace detail
+
+template <typename T> struct is_tuple_like {
+  static FMT_CONSTEXPR_DECL const bool value =
+      detail::is_tuple_like_<T>::value && !detail::is_range_<T>::value;
+};
+
+template <typename TupleT, typename Char>
+struct formatter<TupleT, Char, enable_if_t<fmt::is_tuple_like<TupleT>::value>> {
+ private:
+  // C++11 generic lambda for format()
+  template <typename FormatContext> struct format_each {
+    template <typename T> void operator()(const T& v) {
+      if (i > 0) out = detail::write_delimiter(out);
+      out = detail::write_range_entry<Char>(out, v);
+      ++i;
+    }
+    formatting_tuple<Char>& formatting;
+    size_t& i;
+    typename std::add_lvalue_reference<
+        decltype(std::declval<FormatContext>().out())>::type out;
+  };
+
+ public:
+  formatting_tuple<Char> formatting;
+
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return formatting.parse(ctx);
+  }
+
+  template <typename FormatContext = format_context>
+  auto format(const TupleT& values, FormatContext& ctx) -> decltype(ctx.out()) {
+    auto out = ctx.out();
+    size_t i = 0;
+
+    detail::copy(formatting.prefix, out);
+    detail::for_each(values, format_each<FormatContext>{formatting, i, out});
+    detail::copy(formatting.postfix, out);
+
+    return ctx.out();
+  }
+};
+
+template <typename T, typename Char> struct is_range {
+  static FMT_CONSTEXPR_DECL const bool value =
+      detail::is_range_<T>::value && !detail::is_std_string_like<T>::value &&
+      !std::is_convertible<T, std::basic_string<Char>>::value &&
+      !std::is_constructible<detail::std_string_view<Char>, T>::value;
+};
+
+template <typename T, typename Char>
+struct formatter<
+    T, Char,
+    enable_if_t<
+        fmt::is_range<T, Char>::value
+// Workaround a bug in MSVC 2017 and earlier.
+#if !FMT_MSC_VER || FMT_MSC_VER >= 1927
+        && (has_formatter<detail::value_type<T>, format_context>::value ||
+            detail::has_fallback_formatter<detail::value_type<T>, Char>::value)
+#endif
+        >> {
+  formatting_range<Char> formatting;
+
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return formatting.parse(ctx);
+  }
+
+  template <typename FormatContext>
+  typename FormatContext::iterator format(const T& values, FormatContext& ctx) {
+    auto out = detail::copy(formatting.prefix, ctx.out());
+    size_t i = 0;
+    auto view = detail::range_to_view<T>::view(values);
+    auto it = view.begin();
+    auto end = view.end();
+    for (; it != end; ++it) {
+      if (i > 0) out = detail::write_delimiter(out);
+      out = detail::write_range_entry<Char>(out, *it);
+      ++i;
+    }
+    return detail::copy(formatting.postfix, out);
+  }
+};
+
+template <typename Char, typename... T> struct tuple_join_view : detail::view {
+  const std::tuple<T...>& tuple;
+  basic_string_view<Char> sep;
+
+  tuple_join_view(const std::tuple<T...>& t, basic_string_view<Char> s)
+      : tuple(t), sep{s} {}
+};
+
+template <typename Char, typename... T>
+using tuple_arg_join = tuple_join_view<Char, T...>;
+
+template <typename Char, typename... T>
+struct formatter<tuple_join_view<Char, T...>, Char> {
+  template <typename ParseContext>
+  FMT_CONSTEXPR auto parse(ParseContext& ctx) -> decltype(ctx.begin()) {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  auto format(const tuple_join_view<Char, T...>& value, FormatContext& ctx) ->
+      typename FormatContext::iterator {
+    return format(value, ctx, detail::make_index_sequence<sizeof...(T)>{});
+  }
+
+ private:
+  template <typename FormatContext, size_t... N>
+  auto format(const tuple_join_view<Char, T...>& value, FormatContext& ctx,
+              detail::index_sequence<N...>) ->
+      typename FormatContext::iterator {
+    using std::get;
+    return format_args(value, ctx, get<N>(value.tuple)...);
+  }
+
+  template <typename FormatContext>
+  auto format_args(const tuple_join_view<Char, T...>&, FormatContext& ctx) ->
+      typename FormatContext::iterator {
+    // NOTE: for compilers that support C++17, this empty function instantiation
+    // can be replaced with a constexpr branch in the variadic overload.
+    return ctx.out();
+  }
+
+  template <typename FormatContext, typename Arg, typename... Args>
+  auto format_args(const tuple_join_view<Char, T...>& value, FormatContext& ctx,
+                   const Arg& arg, const Args&... args) ->
+      typename FormatContext::iterator {
+    using base = formatter<typename std::decay<Arg>::type, Char>;
+    auto out = base().format(arg, ctx);
+    if (sizeof...(Args) > 0) {
+      out = std::copy(value.sep.begin(), value.sep.end(), out);
+      ctx.advance_to(out);
+      return format_args(value, ctx, args...);
+    }
+    return out;
+  }
+};
+
+FMT_MODULE_EXPORT_BEGIN
+
+/**
+  \rst
+  Returns an object that formats `tuple` with elements separated by `sep`.
+
+  **Example**::
+
+    std::tuple<int, char> t = {1, 'a'};
+    fmt::print("{}", fmt::join(t, ", "));
+    // Output: "1, a"
+  \endrst
+ */
+template <typename... T>
+FMT_CONSTEXPR auto join(const std::tuple<T...>& tuple, string_view sep)
+    -> tuple_join_view<char, T...> {
+  return {tuple, sep};
+}
+
+template <typename... T>
+FMT_CONSTEXPR auto join(const std::tuple<T...>& tuple,
+                        basic_string_view<wchar_t> sep)
+    -> tuple_join_view<wchar_t, T...> {
+  return {tuple, sep};
+}
+
+/**
+  \rst
+  Returns an object that formats `initializer_list` with elements separated by
+  `sep`.
+
+  **Example**::
+
+    fmt::print("{}", fmt::join({1, 2, 3}, ", "));
+    // Output: "1, 2, 3"
+  \endrst
+ */
+template <typename T>
+auto join(std::initializer_list<T> list, string_view sep)
+    -> join_view<const T*, const T*> {
+  return join(std::begin(list), std::end(list), sep);
+}
+
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_RANGES_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/xchar.h b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/xchar.h
new file mode 100644
index 0000000..a0dd032
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/include/fmt/xchar.h
@@ -0,0 +1,236 @@
+// Formatting library for C++ - optional wchar_t and exotic character support
+//
+// Copyright (c) 2012 - present, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#ifndef FMT_WCHAR_H_
+#define FMT_WCHAR_H_
+
+#include <cwchar>
+#include <tuple>
+
+#include "format.h"
+
+FMT_BEGIN_NAMESPACE
+namespace detail {
+template <typename T>
+using is_exotic_char = bool_constant<!std::is_same<T, char>::value>;
+}
+
+FMT_MODULE_EXPORT_BEGIN
+
+using wstring_view = basic_string_view<wchar_t>;
+using wformat_parse_context = basic_format_parse_context<wchar_t>;
+using wformat_context = buffer_context<wchar_t>;
+using wformat_args = basic_format_args<wformat_context>;
+using wmemory_buffer = basic_memory_buffer<wchar_t>;
+
+#if FMT_GCC_VERSION && FMT_GCC_VERSION < 409
+// Workaround broken conversion on older gcc.
+template <typename... Args> using wformat_string = wstring_view;
+#else
+template <typename... Args>
+using wformat_string = basic_format_string<wchar_t, type_identity_t<Args>...>;
+#endif
+
+template <> struct is_char<wchar_t> : std::true_type {};
+template <> struct is_char<detail::char8_type> : std::true_type {};
+template <> struct is_char<char16_t> : std::true_type {};
+template <> struct is_char<char32_t> : std::true_type {};
+
+template <typename... Args>
+constexpr format_arg_store<wformat_context, Args...> make_wformat_args(
+    const Args&... args) {
+  return {args...};
+}
+
+inline namespace literals {
+constexpr auto operator"" _format(const wchar_t* s, size_t n)
+    -> detail::udl_formatter<wchar_t> {
+  return {{s, n}};
+}
+
+#if FMT_USE_USER_DEFINED_LITERALS && !FMT_USE_NONTYPE_TEMPLATE_PARAMETERS
+constexpr detail::udl_arg<wchar_t> operator"" _a(const wchar_t* s, size_t) {
+  return {s};
+}
+#endif
+}  // namespace literals
+
+template <typename It, typename Sentinel>
+auto join(It begin, Sentinel end, wstring_view sep)
+    -> join_view<It, Sentinel, wchar_t> {
+  return {begin, end, sep};
+}
+
+template <typename Range>
+auto join(Range&& range, wstring_view sep)
+    -> join_view<detail::iterator_t<Range>, detail::sentinel_t<Range>,
+                 wchar_t> {
+  return join(std::begin(range), std::end(range), sep);
+}
+
+template <typename T>
+auto join(std::initializer_list<T> list, wstring_view sep)
+    -> join_view<const T*, const T*, wchar_t> {
+  return join(std::begin(list), std::end(list), sep);
+}
+
+template <typename Char, FMT_ENABLE_IF(!std::is_same<Char, char>::value)>
+auto vformat(basic_string_view<Char> format_str,
+             basic_format_args<buffer_context<type_identity_t<Char>>> args)
+    -> std::basic_string<Char> {
+  basic_memory_buffer<Char> buffer;
+  detail::vformat_to(buffer, format_str, args);
+  return to_string(buffer);
+}
+
+// Pass char_t as a default template parameter instead of using
+// std::basic_string<char_t<S>> to reduce the symbol size.
+template <typename S, typename... Args, typename Char = char_t<S>,
+          FMT_ENABLE_IF(!std::is_same<Char, char>::value)>
+auto format(const S& format_str, Args&&... args) -> std::basic_string<Char> {
+  const auto& vargs = fmt::make_args_checked<Args...>(format_str, args...);
+  return vformat(to_string_view(format_str), vargs);
+}
+
+template <typename Locale, typename S, typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_locale<Locale>::value&&
+                            detail::is_exotic_char<Char>::value)>
+inline auto vformat(
+    const Locale& loc, const S& format_str,
+    basic_format_args<buffer_context<type_identity_t<Char>>> args)
+    -> std::basic_string<Char> {
+  return detail::vformat(loc, to_string_view(format_str), args);
+}
+
+template <typename Locale, typename S, typename... Args,
+          typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_locale<Locale>::value&&
+                            detail::is_exotic_char<Char>::value)>
+inline auto format(const Locale& loc, const S& format_str, Args&&... args)
+    -> std::basic_string<Char> {
+  return detail::vformat(loc, to_string_view(format_str),
+                         fmt::make_args_checked<Args...>(format_str, args...));
+}
+
+template <typename OutputIt, typename S, typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, Char>::value&&
+                            detail::is_exotic_char<Char>::value)>
+auto vformat_to(OutputIt out, const S& format_str,
+                basic_format_args<buffer_context<type_identity_t<Char>>> args)
+    -> OutputIt {
+  auto&& buf = detail::get_buffer<Char>(out);
+  detail::vformat_to(buf, to_string_view(format_str), args);
+  return detail::get_iterator(buf);
+}
+
+template <typename OutputIt, typename S, typename... Args,
+          typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, Char>::value&&
+                            detail::is_exotic_char<Char>::value)>
+inline auto format_to(OutputIt out, const S& fmt, Args&&... args) -> OutputIt {
+  const auto& vargs = fmt::make_args_checked<Args...>(fmt, args...);
+  return vformat_to(out, to_string_view(fmt), vargs);
+}
+
+template <typename S, typename... Args, typename Char, size_t SIZE,
+          typename Allocator, FMT_ENABLE_IF(detail::is_string<S>::value)>
+FMT_DEPRECATED auto format_to(basic_memory_buffer<Char, SIZE, Allocator>& buf,
+                              const S& format_str, Args&&... args) ->
+    typename buffer_context<Char>::iterator {
+  const auto& vargs = fmt::make_args_checked<Args...>(format_str, args...);
+  detail::vformat_to(buf, to_string_view(format_str), vargs, {});
+  return detail::buffer_appender<Char>(buf);
+}
+
+template <typename Locale, typename S, typename OutputIt, typename... Args,
+          typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, Char>::value&&
+                            detail::is_locale<Locale>::value&&
+                                detail::is_exotic_char<Char>::value)>
+inline auto vformat_to(
+    OutputIt out, const Locale& loc, const S& format_str,
+    basic_format_args<buffer_context<type_identity_t<Char>>> args) -> OutputIt {
+  auto&& buf = detail::get_buffer<Char>(out);
+  vformat_to(buf, to_string_view(format_str), args, detail::locale_ref(loc));
+  return detail::get_iterator(buf);
+}
+
+template <
+    typename OutputIt, typename Locale, typename S, typename... Args,
+    typename Char = char_t<S>,
+    bool enable = detail::is_output_iterator<OutputIt, Char>::value&&
+        detail::is_locale<Locale>::value&& detail::is_exotic_char<Char>::value>
+inline auto format_to(OutputIt out, const Locale& loc, const S& format_str,
+                      Args&&... args) ->
+    typename std::enable_if<enable, OutputIt>::type {
+  const auto& vargs = fmt::make_args_checked<Args...>(format_str, args...);
+  return vformat_to(out, loc, to_string_view(format_str), vargs);
+}
+
+template <typename OutputIt, typename Char, typename... Args,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, Char>::value&&
+                            detail::is_exotic_char<Char>::value)>
+inline auto vformat_to_n(
+    OutputIt out, size_t n, basic_string_view<Char> format_str,
+    basic_format_args<buffer_context<type_identity_t<Char>>> args)
+    -> format_to_n_result<OutputIt> {
+  detail::iterator_buffer<OutputIt, Char, detail::fixed_buffer_traits> buf(out,
+                                                                           n);
+  detail::vformat_to(buf, format_str, args);
+  return {buf.out(), buf.count()};
+}
+
+template <typename OutputIt, typename S, typename... Args,
+          typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_output_iterator<OutputIt, Char>::value&&
+                            detail::is_exotic_char<Char>::value)>
+inline auto format_to_n(OutputIt out, size_t n, const S& fmt,
+                        const Args&... args) -> format_to_n_result<OutputIt> {
+  const auto& vargs = fmt::make_args_checked<Args...>(fmt, args...);
+  return vformat_to_n(out, n, to_string_view(fmt), vargs);
+}
+
+template <typename S, typename... Args, typename Char = char_t<S>,
+          FMT_ENABLE_IF(detail::is_exotic_char<Char>::value)>
+inline auto formatted_size(const S& fmt, Args&&... args) -> size_t {
+  detail::counting_buffer<Char> buf;
+  const auto& vargs = fmt::make_args_checked<Args...>(fmt, args...);
+  detail::vformat_to(buf, to_string_view(fmt), vargs);
+  return buf.count();
+}
+
+inline void vprint(std::FILE* f, wstring_view fmt, wformat_args args) {
+  wmemory_buffer buffer;
+  detail::vformat_to(buffer, fmt, args);
+  buffer.push_back(L'\0');
+  if (std::fputws(buffer.data(), f) == -1)
+    FMT_THROW(system_error(errno, FMT_STRING("cannot write to file")));
+}
+
+inline void vprint(wstring_view fmt, wformat_args args) {
+  vprint(stdout, fmt, args);
+}
+
+template <typename... T>
+void print(std::FILE* f, wformat_string<T...> fmt, T&&... args) {
+  return vprint(f, wstring_view(fmt), make_wformat_args(args...));
+}
+
+template <typename... T> void print(wformat_string<T...> fmt, T&&... args) {
+  return vprint(wstring_view(fmt), make_wformat_args(args...));
+}
+
+/**
+  Converts *value* to ``std::wstring`` using the default format for type *T*.
+ */
+template <typename T> inline auto to_wstring(const T& value) -> std::wstring {
+  return format(FMT_STRING(L"{}"), value);
+}
+FMT_MODULE_EXPORT_END
+FMT_END_NAMESPACE
+
+#endif  // FMT_WCHAR_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/src/format.cpp b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/src/format.cpp
new file mode 100644
index 0000000..66925b4
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/src/format.cpp
@@ -0,0 +1,78 @@
+// Formatting library for C++
+//
+// Copyright (c) 2012 - 2016, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+#include "fmt/format-inl.h"
+
+FMT_BEGIN_NAMESPACE
+namespace detail {
+
+template <typename T>
+int format_float(char* buf, std::size_t size, const char* format, int precision,
+                 T value) {
+#ifdef FMT_FUZZ
+  if (precision > 100000)
+    throw std::runtime_error(
+        "fuzz mode - avoid large allocation inside snprintf");
+#endif
+  // Suppress the warning about nonliteral format string.
+  int (*snprintf_ptr)(char*, size_t, const char*, ...) = FMT_SNPRINTF;
+  return precision < 0 ? snprintf_ptr(buf, size, format, value)
+                       : snprintf_ptr(buf, size, format, precision, value);
+}
+
+template FMT_API dragonbox::decimal_fp<float> dragonbox::to_decimal(float x)
+    FMT_NOEXCEPT;
+template FMT_API dragonbox::decimal_fp<double> dragonbox::to_decimal(double x)
+    FMT_NOEXCEPT;
+}  // namespace detail
+
+// Workaround a bug in MSVC2013 that prevents instantiation of format_float.
+int (*instantiate_format_float)(double, int, detail::float_specs,
+                                detail::buffer<char>&) = detail::format_float;
+
+#ifndef FMT_STATIC_THOUSANDS_SEPARATOR
+template FMT_API detail::locale_ref::locale_ref(const std::locale& loc);
+template FMT_API std::locale detail::locale_ref::get<std::locale>() const;
+#endif
+
+// Explicit instantiations for char.
+
+template FMT_API auto detail::thousands_sep_impl(locale_ref)
+    -> thousands_sep_result<char>;
+template FMT_API char detail::decimal_point_impl(locale_ref);
+
+template FMT_API void detail::buffer<char>::append(const char*, const char*);
+
+// DEPRECATED!
+// There is no correspondent extern template in format.h because of
+// incompatibility between clang and gcc (#2377).
+template FMT_API void detail::vformat_to(
+    detail::buffer<char>&, string_view,
+    basic_format_args<FMT_BUFFER_CONTEXT(char)>, detail::locale_ref);
+
+template FMT_API int detail::snprintf_float(double, int, detail::float_specs,
+                                            detail::buffer<char>&);
+template FMT_API int detail::snprintf_float(long double, int,
+                                            detail::float_specs,
+                                            detail::buffer<char>&);
+template FMT_API int detail::format_float(double, int, detail::float_specs,
+                                          detail::buffer<char>&);
+template FMT_API int detail::format_float(long double, int, detail::float_specs,
+                                          detail::buffer<char>&);
+
+// Explicit instantiations for wchar_t.
+
+template FMT_API auto detail::thousands_sep_impl(locale_ref)
+    -> thousands_sep_result<wchar_t>;
+template FMT_API wchar_t detail::decimal_point_impl(locale_ref);
+
+template FMT_API void detail::buffer<wchar_t>::append(const wchar_t*,
+                                                      const wchar_t*);
+
+template struct detail::basic_data<void>;
+
+FMT_END_NAMESPACE
diff --git a/third_party/allwpilib/wpiutil/src/main/native/fmtlib/src/os.cpp b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/src/os.cpp
new file mode 100644
index 0000000..934629d
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/fmtlib/src/os.cpp
@@ -0,0 +1,360 @@
+// Formatting library for C++ - optional OS-specific functionality
+//
+// Copyright (c) 2012 - 2016, Victor Zverovich
+// All rights reserved.
+//
+// For the license information refer to format.h.
+
+// Disable bogus MSVC warnings.
+#if !defined(_CRT_SECURE_NO_WARNINGS) && defined(_MSC_VER)
+#  define _CRT_SECURE_NO_WARNINGS
+#endif
+
+#include "fmt/os.h"
+
+#include <climits>
+
+#if FMT_USE_FCNTL
+#  include <sys/stat.h>
+#  include <sys/types.h>
+
+#  ifndef _WIN32
+#    include <unistd.h>
+#  else
+#    ifndef WIN32_LEAN_AND_MEAN
+#      define WIN32_LEAN_AND_MEAN
+#    endif
+#    include <io.h>
+
+#    define O_CREAT _O_CREAT
+#    define O_TRUNC _O_TRUNC
+
+#    ifndef S_IRUSR
+#      define S_IRUSR _S_IREAD
+#    endif
+
+#    ifndef S_IWUSR
+#      define S_IWUSR _S_IWRITE
+#    endif
+
+#    ifdef __MINGW32__
+#      define _SH_DENYNO 0x40
+#    endif
+#  endif  // _WIN32
+#endif    // FMT_USE_FCNTL
+
+#ifdef _WIN32
+#  include <windows.h>
+#endif
+
+#ifdef fileno
+#  undef fileno
+#endif
+
+namespace {
+#ifdef _WIN32
+// Return type of read and write functions.
+using rwresult = int;
+
+// On Windows the count argument to read and write is unsigned, so convert
+// it from size_t preventing integer overflow.
+inline unsigned convert_rwcount(std::size_t count) {
+  return count <= UINT_MAX ? static_cast<unsigned>(count) : UINT_MAX;
+}
+#elif FMT_USE_FCNTL
+// Return type of read and write functions.
+using rwresult = ssize_t;
+
+inline std::size_t convert_rwcount(std::size_t count) { return count; }
+#endif
+}  // namespace
+
+FMT_BEGIN_NAMESPACE
+
+#ifdef _WIN32
+detail::utf16_to_utf8::utf16_to_utf8(basic_string_view<wchar_t> s) {
+  if (int error_code = convert(s)) {
+    FMT_THROW(windows_error(error_code,
+                            "cannot convert string from UTF-16 to UTF-8"));
+  }
+}
+
+int detail::utf16_to_utf8::convert(basic_string_view<wchar_t> s) {
+  if (s.size() > INT_MAX) return ERROR_INVALID_PARAMETER;
+  int s_size = static_cast<int>(s.size());
+  if (s_size == 0) {
+    // WideCharToMultiByte does not support zero length, handle separately.
+    buffer_.resize(1);
+    buffer_[0] = 0;
+    return 0;
+  }
+
+  int length = WideCharToMultiByte(CP_UTF8, 0, s.data(), s_size, nullptr, 0,
+                                   nullptr, nullptr);
+  if (length == 0) return GetLastError();
+  buffer_.resize(length + 1);
+  length = WideCharToMultiByte(CP_UTF8, 0, s.data(), s_size, &buffer_[0],
+                               length, nullptr, nullptr);
+  if (length == 0) return GetLastError();
+  buffer_[length] = 0;
+  return 0;
+}
+
+namespace detail {
+
+class system_message {
+  system_message(const system_message&) = delete;
+  void operator=(const system_message&) = delete;
+
+  unsigned long result_;
+  wchar_t* message_;
+
+  static bool is_whitespace(wchar_t c) FMT_NOEXCEPT {
+    return c == L' ' || c == L'\n' || c == L'\r' || c == L'\t' || c == L'\0';
+  }
+
+ public:
+  explicit system_message(unsigned long error_code)
+      : result_(0), message_(nullptr) {
+    result_ = FormatMessageW(
+        FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM |
+            FORMAT_MESSAGE_IGNORE_INSERTS,
+        nullptr, error_code, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
+        reinterpret_cast<wchar_t*>(&message_), 0, nullptr);
+    if (result_ != 0) {
+      while (result_ != 0 && is_whitespace(message_[result_ - 1])) {
+        --result_;
+      }
+    }
+  }
+  ~system_message() { LocalFree(message_); }
+  explicit operator bool() const FMT_NOEXCEPT { return result_ != 0; }
+  operator basic_string_view<wchar_t>() const FMT_NOEXCEPT {
+    return basic_string_view<wchar_t>(message_, result_);
+  }
+};
+
+class utf8_system_category final : public std::error_category {
+ public:
+  const char* name() const FMT_NOEXCEPT override { return "system"; }
+  std::string message(int error_code) const override {
+    system_message msg(error_code);
+    if (msg) {
+      utf16_to_utf8 utf8_message;
+      if (utf8_message.convert(msg) == ERROR_SUCCESS) {
+        return utf8_message.str();
+      }
+    }
+    return "unknown error";
+  }
+};
+
+}  // namespace detail
+
+FMT_API const std::error_category& system_category() FMT_NOEXCEPT {
+  static const detail::utf8_system_category category;
+  return category;
+}
+
+std::system_error vwindows_error(int err_code, string_view format_str,
+                                 format_args args) {
+  auto ec = std::error_code(err_code, system_category());
+  return std::system_error(ec, vformat(format_str, args));
+}
+
+void detail::format_windows_error(detail::buffer<char>& out, int error_code,
+                                  const char* message) FMT_NOEXCEPT {
+  FMT_TRY {
+    system_message msg(error_code);
+    if (msg) {
+      utf16_to_utf8 utf8_message;
+      if (utf8_message.convert(msg) == ERROR_SUCCESS) {
+        format_to(buffer_appender<char>(out), "{}: {}", message, utf8_message);
+        return;
+      }
+    }
+  }
+  FMT_CATCH(...) {}
+  format_error_code(out, error_code, message);
+}
+
+void report_windows_error(int error_code, const char* message) FMT_NOEXCEPT {
+  report_error(detail::format_windows_error, error_code, message);
+}
+#endif  // _WIN32
+
+buffered_file::~buffered_file() FMT_NOEXCEPT {
+  if (file_ && FMT_SYSTEM(fclose(file_)) != 0)
+    report_system_error(errno, "cannot close file");
+}
+
+buffered_file::buffered_file(cstring_view filename, cstring_view mode) {
+  FMT_RETRY_VAL(file_, FMT_SYSTEM(fopen(filename.c_str(), mode.c_str())),
+                nullptr);
+  if (!file_)
+    FMT_THROW(system_error(errno, "cannot open file {}", filename.c_str()));
+}
+
+void buffered_file::close() {
+  if (!file_) return;
+  int result = FMT_SYSTEM(fclose(file_));
+  file_ = nullptr;
+  if (result != 0) FMT_THROW(system_error(errno, "cannot close file"));
+}
+
+// A macro used to prevent expansion of fileno on broken versions of MinGW.
+#define FMT_ARGS
+
+int buffered_file::fileno() const {
+  int fd = FMT_POSIX_CALL(fileno FMT_ARGS(file_));
+  if (fd == -1) FMT_THROW(system_error(errno, "cannot get file descriptor"));
+  return fd;
+}
+
+#if FMT_USE_FCNTL
+file::file(cstring_view path, int oflag) {
+  int mode = S_IRUSR | S_IWUSR;
+#  if defined(_WIN32) && !defined(__MINGW32__)
+  fd_ = -1;
+  FMT_POSIX_CALL(sopen_s(&fd_, path.c_str(), oflag, _SH_DENYNO, mode));
+#  else
+  FMT_RETRY(fd_, FMT_POSIX_CALL(open(path.c_str(), oflag, mode)));
+#  endif
+  if (fd_ == -1)
+    FMT_THROW(system_error(errno, "cannot open file {}", path.c_str()));
+}
+
+file::~file() FMT_NOEXCEPT {
+  // Don't retry close in case of EINTR!
+  // See http://linux.derkeiler.com/Mailing-Lists/Kernel/2005-09/3000.html
+  if (fd_ != -1 && FMT_POSIX_CALL(close(fd_)) != 0)
+    report_system_error(errno, "cannot close file");
+}
+
+void file::close() {
+  if (fd_ == -1) return;
+  // Don't retry close in case of EINTR!
+  // See http://linux.derkeiler.com/Mailing-Lists/Kernel/2005-09/3000.html
+  int result = FMT_POSIX_CALL(close(fd_));
+  fd_ = -1;
+  if (result != 0) FMT_THROW(system_error(errno, "cannot close file"));
+}
+
+long long file::size() const {
+#  ifdef _WIN32
+  // Use GetFileSize instead of GetFileSizeEx for the case when _WIN32_WINNT
+  // is less than 0x0500 as is the case with some default MinGW builds.
+  // Both functions support large file sizes.
+  DWORD size_upper = 0;
+  HANDLE handle = reinterpret_cast<HANDLE>(_get_osfhandle(fd_));
+  DWORD size_lower = FMT_SYSTEM(GetFileSize(handle, &size_upper));
+  if (size_lower == INVALID_FILE_SIZE) {
+    DWORD error = GetLastError();
+    if (error != NO_ERROR)
+      FMT_THROW(windows_error(GetLastError(), "cannot get file size"));
+  }
+  unsigned long long long_size = size_upper;
+  return (long_size << sizeof(DWORD) * CHAR_BIT) | size_lower;
+#  else
+  using Stat = struct stat;
+  Stat file_stat = Stat();
+  if (FMT_POSIX_CALL(fstat(fd_, &file_stat)) == -1)
+    FMT_THROW(system_error(errno, "cannot get file attributes"));
+  static_assert(sizeof(long long) >= sizeof(file_stat.st_size),
+                "return type of file::size is not large enough");
+  return file_stat.st_size;
+#  endif
+}
+
+std::size_t file::read(void* buffer, std::size_t count) {
+  rwresult result = 0;
+  FMT_RETRY(result, FMT_POSIX_CALL(read(fd_, buffer, convert_rwcount(count))));
+  if (result < 0) FMT_THROW(system_error(errno, "cannot read from file"));
+  return detail::to_unsigned(result);
+}
+
+std::size_t file::write(const void* buffer, std::size_t count) {
+  rwresult result = 0;
+  FMT_RETRY(result, FMT_POSIX_CALL(write(fd_, buffer, convert_rwcount(count))));
+  if (result < 0) FMT_THROW(system_error(errno, "cannot write to file"));
+  return detail::to_unsigned(result);
+}
+
+file file::dup(int fd) {
+  // Don't retry as dup doesn't return EINTR.
+  // http://pubs.opengroup.org/onlinepubs/009695399/functions/dup.html
+  int new_fd = FMT_POSIX_CALL(dup(fd));
+  if (new_fd == -1)
+    FMT_THROW(system_error(errno, "cannot duplicate file descriptor {}", fd));
+  return file(new_fd);
+}
+
+void file::dup2(int fd) {
+  int result = 0;
+  FMT_RETRY(result, FMT_POSIX_CALL(dup2(fd_, fd)));
+  if (result == -1) {
+    FMT_THROW(system_error(errno, "cannot duplicate file descriptor {} to {}",
+                           fd_, fd));
+  }
+}
+
+void file::dup2(int fd, std::error_code& ec) FMT_NOEXCEPT {
+  int result = 0;
+  FMT_RETRY(result, FMT_POSIX_CALL(dup2(fd_, fd)));
+  if (result == -1) ec = std::error_code(errno, std::generic_category());
+}
+
+void file::pipe(file& read_end, file& write_end) {
+  // Close the descriptors first to make sure that assignments don't throw
+  // and there are no leaks.
+  read_end.close();
+  write_end.close();
+  int fds[2] = {};
+#  ifdef _WIN32
+  // Make the default pipe capacity same as on Linux 2.6.11+.
+  enum { DEFAULT_CAPACITY = 65536 };
+  int result = FMT_POSIX_CALL(pipe(fds, DEFAULT_CAPACITY, _O_BINARY));
+#  else
+  // Don't retry as the pipe function doesn't return EINTR.
+  // http://pubs.opengroup.org/onlinepubs/009696799/functions/pipe.html
+  int result = FMT_POSIX_CALL(pipe(fds));
+#  endif
+  if (result != 0) FMT_THROW(system_error(errno, "cannot create pipe"));
+  // The following assignments don't throw because read_fd and write_fd
+  // are closed.
+  read_end = file(fds[0]);
+  write_end = file(fds[1]);
+}
+
+buffered_file file::fdopen(const char* mode) {
+// Don't retry as fdopen doesn't return EINTR.
+#  if defined(__MINGW32__) && defined(_POSIX_)
+  FILE* f = ::fdopen(fd_, mode);
+#  else
+  FILE* f = FMT_POSIX_CALL(fdopen(fd_, mode));
+#  endif
+  if (!f)
+    FMT_THROW(
+        system_error(errno, "cannot associate stream with file descriptor"));
+  buffered_file bf(f);
+  fd_ = -1;
+  return bf;
+}
+
+long getpagesize() {
+#  ifdef _WIN32
+  SYSTEM_INFO si;
+  GetSystemInfo(&si);
+  return si.dwPageSize;
+#  else
+  long size = FMT_POSIX_CALL(sysconf(_SC_PAGESIZE));
+  if (size < 0) FMT_THROW(system_error(errno, "cannot get memory page size"));
+  return size;
+#  endif
+}
+
+FMT_API void ostream::grow(size_t) {
+  if (this->size() == this->capacity()) flush();
+}
+#endif  // FMT_USE_FCNTL
+FMT_END_NAMESPACE
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h
index ec8dc95..d0744dc 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Algorithm.h
@@ -1,12 +1,10 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
+#include <algorithm>
 #include <vector>
 
 namespace wpi {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ArrayRef.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ArrayRef.h
deleted file mode 100644
index c94312e..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ArrayRef.h
+++ /dev/null
@@ -1,537 +0,0 @@
-//===- ArrayRef.h - Array Reference Wrapper ---------------------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_ARRAYREF_H
-#define WPIUTIL_WPI_ARRAYREF_H
-
-#include "wpi/Hashing.h"
-#include "wpi/SmallVector.h"
-#include "wpi/STLExtras.h"
-#include "wpi/Compiler.h"
-#include <algorithm>
-#include <array>
-#include <cassert>
-#include <cstddef>
-#include <initializer_list>
-#include <iterator>
-#include <memory>
-#include <optional>
-#include <type_traits>
-#include <vector>
-
-namespace wpi {
-
-  /// ArrayRef - Represent a constant reference to an array (0 or more elements
-  /// consecutively in memory), i.e. a start pointer and a length.  It allows
-  /// various APIs to take consecutive elements easily and conveniently.
-  ///
-  /// This class does not own the underlying data, it is expected to be used in
-  /// situations where the data resides in some other buffer, whose lifetime
-  /// extends past that of the ArrayRef. For this reason, it is not in general
-  /// safe to store an ArrayRef.
-  ///
-  /// This is intended to be trivially copyable, so it should be passed by
-  /// value.
-  template<typename T>
-  class LLVM_NODISCARD ArrayRef {
-  public:
-    using iterator = const T *;
-    using const_iterator = const T *;
-    using size_type = size_t;
-    using reverse_iterator = std::reverse_iterator<iterator>;
-    using value_type = T;
-
-  private:
-    /// The start of the array, in an external buffer.
-    const T *Data = nullptr;
-
-    /// The number of elements.
-    size_type Length = 0;
-
-  public:
-    /// @name Constructors
-    /// @{
-
-    /// Construct an empty ArrayRef.
-    /*implicit*/ ArrayRef() = default;
-
-    /// Construct an empty ArrayRef from nullopt.
-    /*implicit*/ ArrayRef(std::nullopt_t) {}
-
-    /// Construct an ArrayRef from a single element.
-    /*implicit*/ ArrayRef(const T &OneElt)
-      : Data(&OneElt), Length(1) {}
-
-    /// Construct an ArrayRef from a pointer and length.
-    /*implicit*/ ArrayRef(const T *data, size_t length)
-      : Data(data), Length(length) {}
-
-    /// Construct an ArrayRef from a range.
-    ArrayRef(const T *begin, const T *end)
-      : Data(begin), Length(end - begin) {}
-
-    /// Construct an ArrayRef from a SmallVector. This is templated in order to
-    /// avoid instantiating SmallVectorTemplateCommon<T> whenever we
-    /// copy-construct an ArrayRef.
-    template<typename U>
-    /*implicit*/ ArrayRef(const SmallVectorTemplateCommon<T, U> &Vec)
-      : Data(Vec.data()), Length(Vec.size()) {
-    }
-
-    /// Construct an ArrayRef from a std::vector.
-    template<typename A>
-    /*implicit*/ ArrayRef(const std::vector<T, A> &Vec)
-      : Data(Vec.data()), Length(Vec.size()) {}
-
-    /// Construct an ArrayRef from a std::array
-    template <size_t N>
-    /*implicit*/ constexpr ArrayRef(const std::array<T, N> &Arr)
-        : Data(Arr.data()), Length(N) {}
-
-    /// Construct an ArrayRef from a C array.
-    template <size_t N>
-    /*implicit*/ constexpr ArrayRef(const T (&Arr)[N]) : Data(Arr), Length(N) {}
-
-    /// Construct an ArrayRef<const T*> from ArrayRef<T*>. This uses SFINAE to
-    /// ensure that only ArrayRefs of pointers can be converted.
-    template <typename U>
-    ArrayRef(
-        const ArrayRef<U *> &A,
-        typename std::enable_if<
-           std::is_convertible<U *const *, T const *>::value>::type * = nullptr)
-      : Data(A.data()), Length(A.size()) {}
-
-    /// Construct an ArrayRef<const T*> from a SmallVector<T*>. This is
-    /// templated in order to avoid instantiating SmallVectorTemplateCommon<T>
-    /// whenever we copy-construct an ArrayRef.
-    template<typename U, typename DummyT>
-    /*implicit*/ ArrayRef(
-      const SmallVectorTemplateCommon<U *, DummyT> &Vec,
-      typename std::enable_if<
-          std::is_convertible<U *const *, T const *>::value>::type * = nullptr)
-      : Data(Vec.data()), Length(Vec.size()) {
-    }
-
-    /// Construct an ArrayRef<const T*> from std::vector<T*>. This uses SFINAE
-    /// to ensure that only vectors of pointers can be converted.
-    template<typename U, typename A>
-    ArrayRef(const std::vector<U *, A> &Vec,
-             typename std::enable_if<
-                 std::is_convertible<U *const *, T const *>::value>::type* = 0)
-      : Data(Vec.data()), Length(Vec.size()) {}
-
-    /// @}
-    /// @name Simple Operations
-    /// @{
-
-    iterator begin() const { return Data; }
-    iterator end() const { return Data + Length; }
-
-    reverse_iterator rbegin() const { return reverse_iterator(end()); }
-    reverse_iterator rend() const { return reverse_iterator(begin()); }
-
-    /// empty - Check if the array is empty.
-    bool empty() const { return Length == 0; }
-
-    const T *data() const { return Data; }
-
-    /// size - Get the array size.
-    size_t size() const { return Length; }
-
-    /// front - Get the first element.
-    const T &front() const {
-      assert(!empty());
-      return Data[0];
-    }
-
-    /// back - Get the last element.
-    const T &back() const {
-      assert(!empty());
-      return Data[Length-1];
-    }
-
-    // copy - Allocate copy in Allocator and return ArrayRef<T> to it.
-    template <typename Allocator> ArrayRef<T> copy(Allocator &A) {
-      T *Buff = A.template Allocate<T>(Length);
-      std::uninitialized_copy(begin(), end(), Buff);
-      return ArrayRef<T>(Buff, Length);
-    }
-
-    /// equals - Check for element-wise equality.
-    bool equals(ArrayRef RHS) const {
-      if (Length != RHS.Length)
-        return false;
-      return std::equal(begin(), end(), RHS.begin());
-    }
-
-    /// slice(n, m) - Chop off the first N elements of the array, and keep M
-    /// elements in the array.
-    ArrayRef<T> slice(size_t N, size_t M) const {
-      assert(N+M <= size() && "Invalid specifier");
-      return ArrayRef<T>(data()+N, M);
-    }
-
-    /// slice(n) - Chop off the first N elements of the array.
-    ArrayRef<T> slice(size_t N) const { return slice(N, size() - N); }
-
-    /// Drop the first \p N elements of the array.
-    ArrayRef<T> drop_front(size_t N = 1) const {
-      assert(size() >= N && "Dropping more elements than exist");
-      return slice(N, size() - N);
-    }
-
-    /// Drop the last \p N elements of the array.
-    ArrayRef<T> drop_back(size_t N = 1) const {
-      assert(size() >= N && "Dropping more elements than exist");
-      return slice(0, size() - N);
-    }
-
-    /// Return a copy of *this with the first N elements satisfying the
-    /// given predicate removed.
-    template <class PredicateT> ArrayRef<T> drop_while(PredicateT Pred) const {
-      return ArrayRef<T>(find_if_not(*this, Pred), end());
-    }
-
-    /// Return a copy of *this with the first N elements not satisfying
-    /// the given predicate removed.
-    template <class PredicateT> ArrayRef<T> drop_until(PredicateT Pred) const {
-      return ArrayRef<T>(find_if(*this, Pred), end());
-    }
-
-    /// Return a copy of *this with only the first \p N elements.
-    ArrayRef<T> take_front(size_t N = 1) const {
-      if (N >= size())
-        return *this;
-      return drop_back(size() - N);
-    }
-
-    /// Return a copy of *this with only the last \p N elements.
-    ArrayRef<T> take_back(size_t N = 1) const {
-      if (N >= size())
-        return *this;
-      return drop_front(size() - N);
-    }
-
-    /// Return the first N elements of this Array that satisfy the given
-    /// predicate.
-    template <class PredicateT> ArrayRef<T> take_while(PredicateT Pred) const {
-      return ArrayRef<T>(begin(), find_if_not(*this, Pred));
-    }
-
-    /// Return the first N elements of this Array that don't satisfy the
-    /// given predicate.
-    template <class PredicateT> ArrayRef<T> take_until(PredicateT Pred) const {
-      return ArrayRef<T>(begin(), find_if(*this, Pred));
-    }
-
-    /// @}
-    /// @name Operator Overloads
-    /// @{
-    const T &operator[](size_t Index) const {
-      assert(Index < Length && "Invalid index!");
-      return Data[Index];
-    }
-
-    /// Disallow accidental assignment from a temporary.
-    ///
-    /// The declaration here is extra complicated so that "arrayRef = {}"
-    /// continues to select the move assignment operator.
-    template <typename U>
-    typename std::enable_if<std::is_same<U, T>::value, ArrayRef<T>>::type &
-    operator=(U &&Temporary) = delete;
-
-    /// Disallow accidental assignment from a temporary.
-    ///
-    /// The declaration here is extra complicated so that "arrayRef = {}"
-    /// continues to select the move assignment operator.
-    template <typename U>
-    typename std::enable_if<std::is_same<U, T>::value, ArrayRef<T>>::type &
-    operator=(std::initializer_list<U>) = delete;
-
-    /// @}
-    /// @name Expensive Operations
-    /// @{
-    std::vector<T> vec() const {
-      return std::vector<T>(Data, Data+Length);
-    }
-
-    /// @}
-    /// @name Conversion operators
-    /// @{
-    operator std::vector<T>() const {
-      return std::vector<T>(Data, Data+Length);
-    }
-
-    /// @}
-  };
-
-  /// MutableArrayRef - Represent a mutable reference to an array (0 or more
-  /// elements consecutively in memory), i.e. a start pointer and a length.  It
-  /// allows various APIs to take and modify consecutive elements easily and
-  /// conveniently.
-  ///
-  /// This class does not own the underlying data, it is expected to be used in
-  /// situations where the data resides in some other buffer, whose lifetime
-  /// extends past that of the MutableArrayRef. For this reason, it is not in
-  /// general safe to store a MutableArrayRef.
-  ///
-  /// This is intended to be trivially copyable, so it should be passed by
-  /// value.
-  template<typename T>
-  class LLVM_NODISCARD MutableArrayRef : public ArrayRef<T> {
-  public:
-    using iterator = T *;
-    using reverse_iterator = std::reverse_iterator<iterator>;
-
-    /// Construct an empty MutableArrayRef.
-    /*implicit*/ MutableArrayRef() = default;
-
-    /// Construct an empty MutableArrayRef from nullopt.
-    /*implicit*/ MutableArrayRef(std::nullopt_t) : ArrayRef<T>() {}
-
-    /// Construct an MutableArrayRef from a single element.
-    /*implicit*/ MutableArrayRef(T &OneElt) : ArrayRef<T>(OneElt) {}
-
-    /// Construct an MutableArrayRef from a pointer and length.
-    /*implicit*/ MutableArrayRef(T *data, size_t length)
-      : ArrayRef<T>(data, length) {}
-
-    /// Construct an MutableArrayRef from a range.
-    MutableArrayRef(T *begin, T *end) : ArrayRef<T>(begin, end) {}
-
-    /// Construct an MutableArrayRef from a SmallVector.
-    /*implicit*/ MutableArrayRef(SmallVectorImpl<T> &Vec)
-    : ArrayRef<T>(Vec) {}
-
-    /// Construct a MutableArrayRef from a std::vector.
-    /*implicit*/ MutableArrayRef(std::vector<T> &Vec)
-    : ArrayRef<T>(Vec) {}
-
-    /// Construct an ArrayRef from a std::array
-    template <size_t N>
-    /*implicit*/ constexpr MutableArrayRef(std::array<T, N> &Arr)
-        : ArrayRef<T>(Arr) {}
-
-    /// Construct an MutableArrayRef from a C array.
-    template <size_t N>
-    /*implicit*/ constexpr MutableArrayRef(T (&Arr)[N]) : ArrayRef<T>(Arr) {}
-
-    T *data() const { return const_cast<T*>(ArrayRef<T>::data()); }
-
-    iterator begin() const { return data(); }
-    iterator end() const { return data() + this->size(); }
-
-    reverse_iterator rbegin() const { return reverse_iterator(end()); }
-    reverse_iterator rend() const { return reverse_iterator(begin()); }
-
-    /// front - Get the first element.
-    T &front() const {
-      assert(!this->empty());
-      return data()[0];
-    }
-
-    /// back - Get the last element.
-    T &back() const {
-      assert(!this->empty());
-      return data()[this->size()-1];
-    }
-
-    /// slice(n, m) - Chop off the first N elements of the array, and keep M
-    /// elements in the array.
-    MutableArrayRef<T> slice(size_t N, size_t M) const {
-      assert(N + M <= this->size() && "Invalid specifier");
-      return MutableArrayRef<T>(this->data() + N, M);
-    }
-
-    /// slice(n) - Chop off the first N elements of the array.
-    MutableArrayRef<T> slice(size_t N) const {
-      return slice(N, this->size() - N);
-    }
-
-    /// Drop the first \p N elements of the array.
-    MutableArrayRef<T> drop_front(size_t N = 1) const {
-      assert(this->size() >= N && "Dropping more elements than exist");
-      return slice(N, this->size() - N);
-    }
-
-    MutableArrayRef<T> drop_back(size_t N = 1) const {
-      assert(this->size() >= N && "Dropping more elements than exist");
-      return slice(0, this->size() - N);
-    }
-
-    /// Return a copy of *this with the first N elements satisfying the
-    /// given predicate removed.
-    template <class PredicateT>
-    MutableArrayRef<T> drop_while(PredicateT Pred) const {
-      return MutableArrayRef<T>(find_if_not(*this, Pred), end());
-    }
-
-    /// Return a copy of *this with the first N elements not satisfying
-    /// the given predicate removed.
-    template <class PredicateT>
-    MutableArrayRef<T> drop_until(PredicateT Pred) const {
-      return MutableArrayRef<T>(find_if(*this, Pred), end());
-    }
-
-    /// Return a copy of *this with only the first \p N elements.
-    MutableArrayRef<T> take_front(size_t N = 1) const {
-      if (N >= this->size())
-        return *this;
-      return drop_back(this->size() - N);
-    }
-
-    /// Return a copy of *this with only the last \p N elements.
-    MutableArrayRef<T> take_back(size_t N = 1) const {
-      if (N >= this->size())
-        return *this;
-      return drop_front(this->size() - N);
-    }
-
-    /// Return the first N elements of this Array that satisfy the given
-    /// predicate.
-    template <class PredicateT>
-    MutableArrayRef<T> take_while(PredicateT Pred) const {
-      return MutableArrayRef<T>(begin(), find_if_not(*this, Pred));
-    }
-
-    /// Return the first N elements of this Array that don't satisfy the
-    /// given predicate.
-    template <class PredicateT>
-    MutableArrayRef<T> take_until(PredicateT Pred) const {
-      return MutableArrayRef<T>(begin(), find_if(*this, Pred));
-    }
-
-    /// @}
-    /// @name Operator Overloads
-    /// @{
-    T &operator[](size_t Index) const {
-      assert(Index < this->size() && "Invalid index!");
-      return data()[Index];
-    }
-  };
-
-  /// This is a MutableArrayRef that owns its array.
-  template <typename T> class OwningArrayRef : public MutableArrayRef<T> {
-  public:
-    OwningArrayRef() = default;
-    OwningArrayRef(size_t Size) : MutableArrayRef<T>(new T[Size], Size) {}
-
-    OwningArrayRef(ArrayRef<T> Data)
-        : MutableArrayRef<T>(new T[Data.size()], Data.size()) {
-      std::copy(Data.begin(), Data.end(), this->begin());
-    }
-
-    OwningArrayRef(OwningArrayRef &&Other) { *this = Other; }
-
-    OwningArrayRef &operator=(OwningArrayRef &&Other) {
-      delete[] this->data();
-      this->MutableArrayRef<T>::operator=(Other);
-      Other.MutableArrayRef<T>::operator=(MutableArrayRef<T>());
-      return *this;
-    }
-
-    ~OwningArrayRef() { delete[] this->data(); }
-  };
-
-  /// @name ArrayRef Convenience constructors
-  /// @{
-
-  /// Construct an ArrayRef from a single element.
-  template<typename T>
-  ArrayRef<T> makeArrayRef(const T &OneElt) {
-    return OneElt;
-  }
-
-  /// Construct an ArrayRef from a pointer and length.
-  template<typename T>
-  ArrayRef<T> makeArrayRef(const T *data, size_t length) {
-    return ArrayRef<T>(data, length);
-  }
-
-  /// Construct an ArrayRef from a range.
-  template<typename T>
-  ArrayRef<T> makeArrayRef(const T *begin, const T *end) {
-    return ArrayRef<T>(begin, end);
-  }
-
-  /// Construct an ArrayRef from a SmallVector.
-  template <typename T>
-  ArrayRef<T> makeArrayRef(const SmallVectorImpl<T> &Vec) {
-    return Vec;
-  }
-
-  /// Construct an ArrayRef from a SmallVector.
-  template <typename T, unsigned N>
-  ArrayRef<T> makeArrayRef(const SmallVector<T, N> &Vec) {
-    return Vec;
-  }
-
-  /// Construct an ArrayRef from a std::vector.
-  template<typename T>
-  ArrayRef<T> makeArrayRef(const std::vector<T> &Vec) {
-    return Vec;
-  }
-
-  /// Construct an ArrayRef from an ArrayRef (no-op) (const)
-  template <typename T> ArrayRef<T> makeArrayRef(const ArrayRef<T> &Vec) {
-    return Vec;
-  }
-
-  /// Construct an ArrayRef from an ArrayRef (no-op)
-  template <typename T> ArrayRef<T> &makeArrayRef(ArrayRef<T> &Vec) {
-    return Vec;
-  }
-
-  /// Construct an ArrayRef from a C array.
-  template<typename T, size_t N>
-  ArrayRef<T> makeArrayRef(const T (&Arr)[N]) {
-    return ArrayRef<T>(Arr);
-  }
-
-  /// Construct a MutableArrayRef from a single element.
-  template<typename T>
-  MutableArrayRef<T> makeMutableArrayRef(T &OneElt) {
-    return OneElt;
-  }
-
-  /// Construct a MutableArrayRef from a pointer and length.
-  template<typename T>
-  MutableArrayRef<T> makeMutableArrayRef(T *data, size_t length) {
-    return MutableArrayRef<T>(data, length);
-  }
-
-  /// @}
-  /// @name ArrayRef Comparison Operators
-  /// @{
-
-  template<typename T>
-  inline bool operator==(ArrayRef<T> LHS, ArrayRef<T> RHS) {
-    return LHS.equals(RHS);
-  }
-
-  template<typename T>
-  inline bool operator!=(ArrayRef<T> LHS, ArrayRef<T> RHS) {
-    return !(LHS == RHS);
-  }
-
-  /// @}
-
-  // ArrayRefs can be treated like a POD type.
-  template <typename T> struct isPodLike;
-  template <typename T> struct isPodLike<ArrayRef<T>> {
-    static const bool value = true;
-  };
-
-  template <typename T> hash_code hash_value(ArrayRef<T> S) {
-    return hash_combine_range(S.begin(), S.end());
-  }
-
-} // end namespace wpi
-
-#endif // LLVM_ADT_ARRAYREF_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Base64.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Base64.h
index cdbf611..41b0062 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Base64.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Base64.h
@@ -1,35 +1,47 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_BASE64_H_
 #define WPIUTIL_WPI_BASE64_H_
 
 #include <cstddef>
 #include <string>
+#include <string_view>
+#include <vector>
 
-#include "wpi/StringRef.h"
+#include "wpi/span.h"
 
 namespace wpi {
 template <typename T>
 class SmallVectorImpl;
 class raw_ostream;
 
-size_t Base64Decode(raw_ostream& os, StringRef encoded);
+size_t Base64Decode(raw_ostream& os, std::string_view encoded);
 
-size_t Base64Decode(StringRef encoded, std::string* plain);
+size_t Base64Decode(std::string_view encoded, std::string* plain);
 
-StringRef Base64Decode(StringRef encoded, size_t* num_read,
-                       SmallVectorImpl<char>& buf);
+std::string_view Base64Decode(std::string_view encoded, size_t* num_read,
+                              SmallVectorImpl<char>& buf);
 
-void Base64Encode(raw_ostream& os, StringRef plain);
+size_t Base64Decode(std::string_view encoded, std::vector<uint8_t>* plain);
 
-void Base64Encode(StringRef plain, std::string* encoded);
+span<uint8_t> Base64Decode(std::string_view encoded, size_t* num_read,
+                           SmallVectorImpl<uint8_t>& buf);
 
-StringRef Base64Encode(StringRef plain, SmallVectorImpl<char>& buf);
+void Base64Encode(raw_ostream& os, std::string_view plain);
+
+void Base64Encode(std::string_view plain, std::string* encoded);
+
+std::string_view Base64Encode(std::string_view plain,
+                              SmallVectorImpl<char>& buf);
+
+void Base64Encode(raw_ostream& os, span<const uint8_t> plain);
+
+void Base64Encode(span<const uint8_t> plain, std::string* encoded);
+
+std::string_view Base64Encode(span<const uint8_t> plain,
+                              SmallVectorImpl<char>& buf);
 
 }  // namespace wpi
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/CallbackManager.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/CallbackManager.h
new file mode 100644
index 0000000..32e2add
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/CallbackManager.h
@@ -0,0 +1,395 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef WPIUTIL_WPI_CALLBACKMANAGER_H_
+#define WPIUTIL_WPI_CALLBACKMANAGER_H_
+
+#include <atomic>
+#include <climits>
+#include <functional>
+#include <memory>
+#include <queue>
+#include <utility>
+#include <vector>
+
+#include "wpi/SafeThread.h"
+#include "wpi/UidVector.h"
+#include "wpi/condition_variable.h"
+#include "wpi/mutex.h"
+#include "wpi/raw_ostream.h"
+
+namespace wpi {
+
+template <typename Callback>
+class CallbackListenerData {
+ public:
+  CallbackListenerData() = default;
+  explicit CallbackListenerData(Callback callback_) : callback(callback_) {}
+  explicit CallbackListenerData(unsigned int poller_uid_)
+      : poller_uid(poller_uid_) {}
+
+  explicit operator bool() const { return callback || poller_uid != UINT_MAX; }
+
+  Callback callback;
+  unsigned int poller_uid = UINT_MAX;
+};
+
+// CRTP callback manager thread
+// @tparam Derived        derived class
+// @tparam NotifierData   data buffered for each callback
+// @tparam ListenerData   data stored for each listener
+// Derived must define the following functions:
+//   bool Matches(const ListenerData& listener, const NotifierData& data);
+//   void SetListener(NotifierData* data, unsigned int listener_uid);
+//   void DoCallback(Callback callback, const NotifierData& data);
+template <typename Derived, typename TUserInfo,
+          typename TListenerData =
+              CallbackListenerData<std::function<void(const TUserInfo& info)>>,
+          typename TNotifierData = TUserInfo>
+class CallbackThread : public wpi::SafeThread {
+ public:
+  using UserInfo = TUserInfo;
+  using NotifierData = TNotifierData;
+  using ListenerData = TListenerData;
+
+  CallbackThread(std::function<void()> on_start, std::function<void()> on_exit)
+      : m_on_start(std::move(on_start)), m_on_exit(std::move(on_exit)) {}
+
+  ~CallbackThread() override {
+    // Wake up any blocked pollers
+    for (size_t i = 0; i < m_pollers.size(); ++i) {
+      if (auto poller = m_pollers[i]) {
+        poller->Terminate();
+      }
+    }
+  }
+
+  void Main() override;
+
+  wpi::UidVector<ListenerData, 64> m_listeners;
+
+  std::queue<std::pair<unsigned int, NotifierData>> m_queue;
+  wpi::condition_variable m_queue_empty;
+
+  struct Poller {
+    void Terminate() {
+      {
+        std::scoped_lock lock(poll_mutex);
+        terminating = true;
+      }
+      poll_cond.notify_all();
+    }
+    std::queue<NotifierData> poll_queue;
+    wpi::mutex poll_mutex;
+    wpi::condition_variable poll_cond;
+    bool terminating = false;
+    bool canceling = false;
+  };
+  wpi::UidVector<std::shared_ptr<Poller>, 64> m_pollers;
+
+  std::function<void()> m_on_start;
+  std::function<void()> m_on_exit;
+
+  // Must be called with m_mutex held
+  template <typename... Args>
+  void SendPoller(unsigned int poller_uid, Args&&... args) {
+    if (poller_uid > m_pollers.size()) {
+      return;
+    }
+    auto poller = m_pollers[poller_uid];
+    if (!poller) {
+      return;
+    }
+    {
+      std::scoped_lock lock(poller->poll_mutex);
+      poller->poll_queue.emplace(std::forward<Args>(args)...);
+    }
+    poller->poll_cond.notify_one();
+  }
+};
+
+template <typename Derived, typename TUserInfo, typename TListenerData,
+          typename TNotifierData>
+void CallbackThread<Derived, TUserInfo, TListenerData, TNotifierData>::Main() {
+  if (m_on_start) {
+    m_on_start();
+  }
+
+  std::unique_lock lock(m_mutex);
+  while (m_active) {
+    while (m_queue.empty()) {
+      m_cond.wait(lock);
+      if (!m_active) {
+        goto done;
+      }
+    }
+
+    while (!m_queue.empty()) {
+      if (!m_active) {
+        goto done;
+      }
+      auto item = std::move(m_queue.front());
+
+      if (item.first != UINT_MAX) {
+        if (item.first < m_listeners.size()) {
+          auto& listener = m_listeners[item.first];
+          if (listener &&
+              static_cast<Derived*>(this)->Matches(listener, item.second)) {
+            static_cast<Derived*>(this)->SetListener(&item.second, item.first);
+            if (listener.callback) {
+              lock.unlock();
+              static_cast<Derived*>(this)->DoCallback(listener.callback,
+                                                      item.second);
+              lock.lock();
+            } else if (listener.poller_uid != UINT_MAX) {
+              SendPoller(listener.poller_uid, std::move(item.second));
+            }
+          }
+        }
+      } else {
+        // Use index because iterator might get invalidated.
+        for (size_t i = 0; i < m_listeners.size(); ++i) {
+          auto& listener = m_listeners[i];
+          if (!listener) {
+            continue;
+          }
+
+          if (!static_cast<Derived*>(this)->Matches(listener, item.second)) {
+            continue;
+          }
+          static_cast<Derived*>(this)->SetListener(&item.second,
+                                                   static_cast<unsigned>(i));
+          if (listener.callback) {
+            lock.unlock();
+            static_cast<Derived*>(this)->DoCallback(listener.callback,
+                                                    item.second);
+            lock.lock();
+          } else if (listener.poller_uid != UINT_MAX) {
+            SendPoller(listener.poller_uid, item.second);
+          }
+        }
+      }
+      m_queue.pop();
+    }
+
+    m_queue_empty.notify_all();
+  }
+
+done:
+  if (m_on_exit) {
+    m_on_exit();
+  }
+}
+
+// CRTP callback manager
+// @tparam Derived  derived class
+// @tparam Thread   custom thread (must be derived from impl::CallbackThread)
+//
+// Derived must define the following functions:
+//   void Start();
+template <typename Derived, typename Thread>
+class CallbackManager {
+  friend class RpcServerTest;
+
+ public:
+  void SetOnStart(std::function<void()> on_start) {
+    m_on_start = std::move(on_start);
+  }
+
+  void SetOnExit(std::function<void()> on_exit) {
+    m_on_exit = std::move(on_exit);
+  }
+
+  void Stop() { m_owner.Stop(); }
+
+  void Remove(unsigned int listener_uid) {
+    auto thr = m_owner.GetThread();
+    if (!thr) {
+      return;
+    }
+    thr->m_listeners.erase(listener_uid);
+  }
+
+  unsigned int CreatePoller() {
+    static_cast<Derived*>(this)->Start();
+    auto thr = m_owner.GetThread();
+    return thr->m_pollers.emplace_back(
+        std::make_shared<typename Thread::Poller>());
+  }
+
+  void RemovePoller(unsigned int poller_uid) {
+    auto thr = m_owner.GetThread();
+    if (!thr) {
+      return;
+    }
+
+    // Remove any listeners that are associated with this poller
+    for (size_t i = 0; i < thr->m_listeners.size(); ++i) {
+      if (thr->m_listeners[i].poller_uid == poller_uid) {
+        thr->m_listeners.erase(i);
+      }
+    }
+
+    // Wake up any blocked pollers
+    if (poller_uid >= thr->m_pollers.size()) {
+      return;
+    }
+    auto poller = thr->m_pollers[poller_uid];
+    if (!poller) {
+      return;
+    }
+    poller->Terminate();
+    thr->m_pollers.erase(poller_uid);
+  }
+
+  bool WaitForQueue(double timeout) {
+    auto thr = m_owner.GetThread();
+    if (!thr) {
+      return true;
+    }
+
+    auto& lock = thr.GetLock();
+    auto timeout_time = std::chrono::steady_clock::now() +
+                        std::chrono::duration<double>(timeout);
+    while (!thr->m_queue.empty()) {
+      if (!thr->m_active) {
+        return true;
+      }
+      if (timeout == 0) {
+        return false;
+      }
+      if (timeout < 0) {
+        thr->m_queue_empty.wait(lock);
+      } else {
+        auto cond_timed_out = thr->m_queue_empty.wait_until(lock, timeout_time);
+        if (cond_timed_out == std::cv_status::timeout) {
+          return false;
+        }
+      }
+    }
+
+    return true;
+  }
+
+  std::vector<typename Thread::UserInfo> Poll(unsigned int poller_uid) {
+    bool timed_out = false;
+    return Poll(poller_uid, -1, &timed_out);
+  }
+
+  std::vector<typename Thread::UserInfo> Poll(unsigned int poller_uid,
+                                              double timeout, bool* timed_out) {
+    std::vector<typename Thread::UserInfo> infos;
+    std::shared_ptr<typename Thread::Poller> poller;
+    {
+      auto thr = m_owner.GetThread();
+      if (!thr) {
+        return infos;
+      }
+      if (poller_uid > thr->m_pollers.size()) {
+        return infos;
+      }
+      poller = thr->m_pollers[poller_uid];
+      if (!poller) {
+        return infos;
+      }
+    }
+
+    std::unique_lock lock(poller->poll_mutex);
+    auto timeout_time = std::chrono::steady_clock::now() +
+                        std::chrono::duration<double>(timeout);
+    *timed_out = false;
+    while (poller->poll_queue.empty()) {
+      if (poller->terminating) {
+        return infos;
+      }
+      if (poller->canceling) {
+        // Note: this only works if there's a single thread calling this
+        // function for any particular poller, but that's the intended use.
+        poller->canceling = false;
+        return infos;
+      }
+      if (timeout == 0) {
+        *timed_out = true;
+        return infos;
+      }
+      if (timeout < 0) {
+        poller->poll_cond.wait(lock);
+      } else {
+        auto cond_timed_out = poller->poll_cond.wait_until(lock, timeout_time);
+        if (cond_timed_out == std::cv_status::timeout) {
+          *timed_out = true;
+          return infos;
+        }
+      }
+    }
+
+    while (!poller->poll_queue.empty()) {
+      infos.emplace_back(std::move(poller->poll_queue.front()));
+      poller->poll_queue.pop();
+    }
+    return infos;
+  }
+
+  void CancelPoll(unsigned int poller_uid) {
+    std::shared_ptr<typename Thread::Poller> poller;
+    {
+      auto thr = m_owner.GetThread();
+      if (!thr) {
+        return;
+      }
+      if (poller_uid > thr->m_pollers.size()) {
+        return;
+      }
+      poller = thr->m_pollers[poller_uid];
+      if (!poller) {
+        return;
+      }
+    }
+
+    {
+      std::scoped_lock lock(poller->poll_mutex);
+      poller->canceling = true;
+    }
+    poller->poll_cond.notify_one();
+  }
+
+ protected:
+  template <typename... Args>
+  void DoStart(Args&&... args) {
+    m_owner.Start(m_on_start, m_on_exit, std::forward<Args>(args)...);
+  }
+
+  template <typename... Args>
+  unsigned int DoAdd(Args&&... args) {
+    static_cast<Derived*>(this)->Start();
+    auto thr = m_owner.GetThread();
+    return thr->m_listeners.emplace_back(std::forward<Args>(args)...);
+  }
+
+  template <typename... Args>
+  void Send(unsigned int only_listener, Args&&... args) {
+    auto thr = m_owner.GetThread();
+    if (!thr || thr->m_listeners.empty()) {
+      return;
+    }
+    thr->m_queue.emplace(std::piecewise_construct,
+                         std::make_tuple(only_listener),
+                         std::forward_as_tuple(std::forward<Args>(args)...));
+    thr->m_cond.notify_one();
+  }
+
+  typename wpi::SafeThreadOwner<Thread>::Proxy GetThread() const {
+    return m_owner.GetThread();
+  }
+
+ private:
+  wpi::SafeThreadOwner<Thread> m_owner;
+
+  std::function<void()> m_on_start;
+  std::function<void()> m_on_exit;
+};
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_CALLBACKMANAGER_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ConvertUTF.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ConvertUTF.h
index a9bdb60..aa113b1 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ConvertUTF.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ConvertUTF.h
@@ -90,11 +90,11 @@
 #ifndef LLVM_SUPPORT_CONVERTUTF_H
 #define LLVM_SUPPORT_CONVERTUTF_H
 
-#include "wpi/ArrayRef.h"
-#include "wpi/StringRef.h"
+#include "wpi/span.h"
 
 #include <cstddef>
 #include <string>
+#include <string_view>
 #include <system_error>
 
 // Wrap everything in namespace wpi so that programs can link with wpiutil and
@@ -102,6 +102,9 @@
 
 namespace wpi {
 
+template <typename T>
+class SmallVectorImpl;
+
 /* ---------------------------------------------------------------------
     The following 4 definitions are compiler-specific.
     The C standard does not guarantee that wchar_t has at least
@@ -228,14 +231,14 @@
  * Returns true if a blob of text starts with a UTF-16 big or little endian byte
  * order mark.
  */
-bool hasUTF16ByteOrderMark(ArrayRef<char> SrcBytes);
+bool hasUTF16ByteOrderMark(span<const char> SrcBytes);
 
 /**
  * Converts a UTF-16 string into a UTF-8 string.
  *
  * \returns true on success
  */
-bool convertUTF16ToUTF8String(ArrayRef<UTF16> SrcUTF16,
+bool convertUTF16ToUTF8String(span<const UTF16> SrcUTF16,
                               SmallVectorImpl<char> &DstUTF8);
 
 /**
@@ -243,15 +246,15 @@
  *
  * \returns true on success
  */
-bool convertUTF8ToUTF16String(StringRef SrcUTF8,
+bool convertUTF8ToUTF16String(std::string_view SrcUTF8,
                               SmallVectorImpl<UTF16> &DstUTF16);
 
 #if defined(_WIN32)
 namespace sys {
 namespace windows {
-std::error_code UTF8ToUTF16(StringRef utf8, SmallVectorImpl<wchar_t> &utf16);
+std::error_code UTF8ToUTF16(std::string_view utf8, SmallVectorImpl<wchar_t> &utf16);
 /// Convert to UTF16 from the current code page used in the system
-std::error_code CurCPToUTF16(StringRef utf8, SmallVectorImpl<wchar_t> &utf16);
+std::error_code CurCPToUTF16(std::string_view utf8, SmallVectorImpl<wchar_t> &utf16);
 std::error_code UTF16ToUTF8(const wchar_t *utf16, size_t utf16_len,
                             SmallVectorImpl<char> &utf8);
 /// Convert from UTF16 to the current code page used in the system
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h
index 5989ccc..03a7d3f 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Demangle.h
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_DEMANGLE_H_
 #define WPIUTIL_WPI_DEMANGLE_H_
 
 #include <string>
-
-#include "wpi/Twine.h"
+#include <string_view>
 
 namespace wpi {
 
@@ -20,7 +16,7 @@
  * @param mangledSymbol the mangled symbol.
  * @return The demangled symbol, or mangledSymbol if demangling fails.
  */
-std::string Demangle(const Twine& mangledSymbol);
+std::string Demangle(std::string_view mangledSymbol);
 
 }  // namespace wpi
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DenseMapInfo.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DenseMapInfo.h
index 81ccdc7..b6525a3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DenseMapInfo.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DenseMapInfo.h
@@ -14,13 +14,13 @@
 #ifndef WPIUTIL_WPI_DENSEMAPINFO_H
 #define WPIUTIL_WPI_DENSEMAPINFO_H
 
-#include "wpi/ArrayRef.h"
 #include "wpi/Hashing.h"
-#include "wpi/StringRef.h"
 #include "wpi/PointerLikeTypeTraits.h"
+#include "wpi/span.h"
 #include <cassert>
 #include <cstddef>
 #include <cstdint>
+#include <string_view>
 #include <utility>
 
 namespace wpi {
@@ -206,26 +206,26 @@
   }
 };
 
-// Provide DenseMapInfo for StringRefs.
-template <> struct DenseMapInfo<StringRef> {
-  static inline StringRef getEmptyKey() {
-    return StringRef(reinterpret_cast<const char *>(~static_cast<uintptr_t>(0)),
+// Provide DenseMapInfo for std::string_view.
+template <> struct DenseMapInfo<std::string_view> {
+  static inline std::string_view getEmptyKey() {
+    return std::string_view(reinterpret_cast<const char *>(~static_cast<uintptr_t>(0)),
                      0);
   }
 
-  static inline StringRef getTombstoneKey() {
-    return StringRef(reinterpret_cast<const char *>(~static_cast<uintptr_t>(1)),
+  static inline std::string_view getTombstoneKey() {
+    return std::string_view(reinterpret_cast<const char *>(~static_cast<uintptr_t>(1)),
                      0);
   }
 
-  static unsigned getHashValue(StringRef Val) {
+  static unsigned getHashValue(std::string_view Val) {
     assert(Val.data() != getEmptyKey().data() && "Cannot hash the empty key!");
     assert(Val.data() != getTombstoneKey().data() &&
            "Cannot hash the tombstone key!");
     return (unsigned)(hash_value(Val));
   }
 
-  static bool isEqual(StringRef LHS, StringRef RHS) {
+  static bool isEqual(std::string_view LHS, std::string_view RHS) {
     if (RHS.data() == getEmptyKey().data())
       return LHS.data() == getEmptyKey().data();
     if (RHS.data() == getTombstoneKey().data())
@@ -234,26 +234,26 @@
   }
 };
 
-// Provide DenseMapInfo for ArrayRefs.
-template <typename T> struct DenseMapInfo<ArrayRef<T>> {
-  static inline ArrayRef<T> getEmptyKey() {
-    return ArrayRef<T>(reinterpret_cast<const T *>(~static_cast<uintptr_t>(0)),
-                       size_t(0));
+// Provide DenseMapInfo for spans.
+template <typename T> struct DenseMapInfo<span<T>> {
+  static inline span<T> getEmptyKey() {
+    return span<T>(reinterpret_cast<const T *>(~static_cast<uintptr_t>(0)),
+                   size_t(0));
   }
 
-  static inline ArrayRef<T> getTombstoneKey() {
-    return ArrayRef<T>(reinterpret_cast<const T *>(~static_cast<uintptr_t>(1)),
-                       size_t(0));
+  static inline span<T> getTombstoneKey() {
+    return span<T>(reinterpret_cast<const T *>(~static_cast<uintptr_t>(1)),
+                   size_t(0));
   }
 
-  static unsigned getHashValue(ArrayRef<T> Val) {
+  static unsigned getHashValue(span<T> Val) {
     assert(Val.data() != getEmptyKey().data() && "Cannot hash the empty key!");
     assert(Val.data() != getTombstoneKey().data() &&
            "Cannot hash the tombstone key!");
     return (unsigned)(hash_value(Val));
   }
 
-  static bool isEqual(ArrayRef<T> LHS, ArrayRef<T> RHS) {
+  static bool isEqual(span<T> LHS, span<T> RHS) {
     if (RHS.data() == getEmptyKey().data())
       return LHS.data() == getEmptyKey().data();
     if (RHS.data() == getTombstoneKey().data())
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DsClient.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DsClient.h
new file mode 100644
index 0000000..c24ba3b
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/DsClient.h
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+
+#include "wpi/Signal.h"
+
+namespace wpi {
+
+class Logger;
+
+namespace uv {
+class Loop;
+class Tcp;
+class Timer;
+}  // namespace uv
+
+class DsClient : public std::enable_shared_from_this<DsClient> {
+  struct private_init {};
+
+ public:
+  static std::shared_ptr<DsClient> Create(wpi::uv::Loop& loop,
+                                          wpi::Logger& logger) {
+    return std::make_shared<DsClient>(loop, logger, private_init{});
+  }
+
+  DsClient(wpi::uv::Loop& loop, wpi::Logger& logger, const private_init&);
+  ~DsClient();
+  DsClient(const DsClient&) = delete;
+  DsClient& operator=(const DsClient&) = delete;
+
+  void Close();
+
+  sig::Signal<std::string_view> setIp;
+  sig::Signal<> clearIp;
+
+ private:
+  void Connect();
+  void HandleIncoming(std::string_view in);
+  void ParseJson();
+
+  wpi::Logger& m_logger;
+
+  std::shared_ptr<wpi::uv::Tcp> m_tcp;
+  std::shared_ptr<wpi::uv::Timer> m_timer;
+
+  std::string m_json;
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Endian.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Endian.h
index ee4a51d..b31cb2d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Endian.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Endian.h
@@ -38,7 +38,7 @@
 
 namespace detail {
 
-/// ::value is either alignment, or alignof(T) if alignment is 0.
+// value is either alignment, or alignof(T) if alignment is 0.
 template<class T, int alignment>
 struct PickAlignment {
  enum { value = alignment == 0 ? alignof(T) : alignment };
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Error.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Error.h
deleted file mode 100644
index 1821a49..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Error.h
+++ /dev/null
@@ -1,1197 +0,0 @@
-//===- llvm/Support/Error.h - Recoverable error handling --------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file defines an API used to report recoverable errors.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_ERROR_H
-#define WPIUTIL_WPI_ERROR_H
-
-#include "wpi/STLExtras.h"
-#include "wpi/SmallVector.h"
-#include "wpi/StringExtras.h"
-#include "wpi/Twine.h"
-#include "wpi/AlignOf.h"
-#include "wpi/Compiler.h"
-#include "wpi/ErrorHandling.h"
-#include "wpi/ErrorOr.h"
-#include "wpi/Format.h"
-#include "wpi/raw_ostream.h"
-#include <algorithm>
-#include <cassert>
-#include <cstdint>
-#include <cstdlib>
-#include <functional>
-#include <memory>
-#include <new>
-#include <string>
-#include <system_error>
-#include <type_traits>
-#include <utility>
-#include <vector>
-
-namespace wpi {
-
-class ErrorSuccess;
-
-/// Base class for error info classes. Do not extend this directly: Extend
-/// the ErrorInfo template subclass instead.
-class ErrorInfoBase {
-public:
-  virtual ~ErrorInfoBase() = default;
-
-  /// Print an error message to an output stream.
-  virtual void log(raw_ostream &OS) const = 0;
-
-  /// Return the error message as a string.
-  virtual std::string message() const {
-    std::string Msg;
-    raw_string_ostream OS(Msg);
-    log(OS);
-    return OS.str();
-  }
-
-  /// Convert this error to a std::error_code.
-  ///
-  /// This is a temporary crutch to enable interaction with code still
-  /// using std::error_code. It will be removed in the future.
-  virtual std::error_code convertToErrorCode() const = 0;
-
-  // Returns the class ID for this type.
-  static const void *classID() { return &ID; }
-
-  // Returns the class ID for the dynamic type of this ErrorInfoBase instance.
-  virtual const void *dynamicClassID() const = 0;
-
-  // Check whether this instance is a subclass of the class identified by
-  // ClassID.
-  virtual bool isA(const void *const ClassID) const {
-    return ClassID == classID();
-  }
-
-  // Check whether this instance is a subclass of ErrorInfoT.
-  template <typename ErrorInfoT> bool isA() const {
-    return isA(ErrorInfoT::classID());
-  }
-
-private:
-  virtual void anchor();
-
-  static char ID;
-};
-
-/// Lightweight error class with error context and mandatory checking.
-///
-/// Instances of this class wrap a ErrorInfoBase pointer. Failure states
-/// are represented by setting the pointer to a ErrorInfoBase subclass
-/// instance containing information describing the failure. Success is
-/// represented by a null pointer value.
-///
-/// Instances of Error also contains a 'Checked' flag, which must be set
-/// before the destructor is called, otherwise the destructor will trigger a
-/// runtime error. This enforces at runtime the requirement that all Error
-/// instances be checked or returned to the caller.
-///
-/// There are two ways to set the checked flag, depending on what state the
-/// Error instance is in. For Error instances indicating success, it
-/// is sufficient to invoke the boolean conversion operator. E.g.:
-///
-///   @code{.cpp}
-///   Error foo(<...>);
-///
-///   if (auto E = foo(<...>))
-///     return E; // <- Return E if it is in the error state.
-///   // We have verified that E was in the success state. It can now be safely
-///   // destroyed.
-///   @endcode
-///
-/// A success value *can not* be dropped. For example, just calling 'foo(<...>)'
-/// without testing the return value will raise a runtime error, even if foo
-/// returns success.
-///
-/// For Error instances representing failure, you must use either the
-/// handleErrors or handleAllErrors function with a typed handler. E.g.:
-///
-///   @code{.cpp}
-///   class MyErrorInfo : public ErrorInfo<MyErrorInfo> {
-///     // Custom error info.
-///   };
-///
-///   Error foo(<...>) { return make_error<MyErrorInfo>(...); }
-///
-///   auto E = foo(<...>); // <- foo returns failure with MyErrorInfo.
-///   auto NewE =
-///     handleErrors(E,
-///       [](const MyErrorInfo &M) {
-///         // Deal with the error.
-///       },
-///       [](std::unique_ptr<OtherError> M) -> Error {
-///         if (canHandle(*M)) {
-///           // handle error.
-///           return Error::success();
-///         }
-///         // Couldn't handle this error instance. Pass it up the stack.
-///         return Error(std::move(M));
-///       );
-///   // Note - we must check or return NewE in case any of the handlers
-///   // returned a new error.
-///   @endcode
-///
-/// The handleAllErrors function is identical to handleErrors, except
-/// that it has a void return type, and requires all errors to be handled and
-/// no new errors be returned. It prevents errors (assuming they can all be
-/// handled) from having to be bubbled all the way to the top-level.
-///
-/// *All* Error instances must be checked before destruction, even if
-/// they're moved-assigned or constructed from Success values that have already
-/// been checked. This enforces checking through all levels of the call stack.
-class LLVM_NODISCARD Error {
-  // Both ErrorList and FileError need to be able to yank ErrorInfoBase
-  // pointers out of this class to add to the error list.
-  friend class ErrorList;
-  friend class FileError;
-
-  // handleErrors needs to be able to set the Checked flag.
-  template <typename... HandlerTs>
-  friend Error handleErrors(Error E, HandlerTs &&... Handlers);
-
-  // Expected<T> needs to be able to steal the payload when constructed from an
-  // error.
-  template <typename T> friend class Expected;
-
-protected:
-  /// Create a success value. Prefer using 'Error::success()' for readability
-  Error() {
-    setPtr(nullptr);
-    setChecked(false);
-  }
-
-public:
-  /// Create a success value.
-  static ErrorSuccess success();
-
-  // Errors are not copy-constructable.
-  Error(const Error &Other) = delete;
-
-  /// Move-construct an error value. The newly constructed error is considered
-  /// unchecked, even if the source error had been checked. The original error
-  /// becomes a checked Success value, regardless of its original state.
-  Error(Error &&Other) noexcept {
-    setChecked(true);
-    *this = std::move(Other);
-  }
-
-  /// Create an error value. Prefer using the 'make_error' function, but
-  /// this constructor can be useful when "re-throwing" errors from handlers.
-  Error(std::unique_ptr<ErrorInfoBase> Payload) {
-    setPtr(Payload.release());
-    setChecked(false);
-  }
-
-  // Errors are not copy-assignable.
-  Error &operator=(const Error &Other) = delete;
-
-  /// Move-assign an error value. The current error must represent success, you
-  /// you cannot overwrite an unhandled error. The current error is then
-  /// considered unchecked. The source error becomes a checked success value,
-  /// regardless of its original state.
-  Error &operator=(Error &&Other) noexcept {
-    // Don't allow overwriting of unchecked values.
-    assertIsChecked();
-    setPtr(Other.getPtr());
-
-    // This Error is unchecked, even if the source error was checked.
-    setChecked(false);
-
-    // Null out Other's payload and set its checked bit.
-    Other.setPtr(nullptr);
-    Other.setChecked(true);
-
-    return *this;
-  }
-
-  /// Destroy a Error. Fails with a call to abort() if the error is
-  /// unchecked.
-  ~Error() {
-    assertIsChecked();
-    delete getPtr();
-  }
-
-  /// Bool conversion. Returns true if this Error is in a failure state,
-  /// and false if it is in an accept state. If the error is in a Success state
-  /// it will be considered checked.
-  explicit operator bool() {
-    setChecked(getPtr() == nullptr);
-    return getPtr() != nullptr;
-  }
-
-  /// Check whether one error is a subclass of another.
-  template <typename ErrT> bool isA() const {
-    return getPtr() && getPtr()->isA(ErrT::classID());
-  }
-
-  /// Returns the dynamic class id of this error, or null if this is a success
-  /// value.
-  const void* dynamicClassID() const {
-    if (!getPtr())
-      return nullptr;
-    return getPtr()->dynamicClassID();
-  }
-
-private:
-  void assertIsChecked() {
-  }
-
-  ErrorInfoBase *getPtr() const {
-    return reinterpret_cast<ErrorInfoBase*>(
-             reinterpret_cast<uintptr_t>(Payload) &
-             ~static_cast<uintptr_t>(0x1));
-  }
-
-  void setPtr(ErrorInfoBase *EI) {
-    Payload = EI;
-  }
-
-  bool getChecked() const {
-    return true;
-  }
-
-  void setChecked(bool V) {
-    Payload = reinterpret_cast<ErrorInfoBase*>(
-                (reinterpret_cast<uintptr_t>(Payload) &
-                  ~static_cast<uintptr_t>(0x1)) |
-                  (V ? 0 : 1));
-  }
-
-  std::unique_ptr<ErrorInfoBase> takePayload() {
-    std::unique_ptr<ErrorInfoBase> Tmp(getPtr());
-    setPtr(nullptr);
-    setChecked(true);
-    return Tmp;
-  }
-
-  friend raw_ostream &operator<<(raw_ostream &OS, const Error &E) {
-    if (auto P = E.getPtr())
-      P->log(OS);
-    else
-      OS << "success";
-    return OS;
-  }
-
-  ErrorInfoBase *Payload = nullptr;
-};
-
-/// Subclass of Error for the sole purpose of identifying the success path in
-/// the type system. This allows to catch invalid conversion to Expected<T> at
-/// compile time.
-class ErrorSuccess final : public Error {};
-
-inline ErrorSuccess Error::success() { return ErrorSuccess(); }
-
-/// Make a Error instance representing failure using the given error info
-/// type.
-template <typename ErrT, typename... ArgTs> Error make_error(ArgTs &&... Args) {
-  return Error(std::make_unique<ErrT>(std::forward<ArgTs>(Args)...));
-}
-
-/// Base class for user error types. Users should declare their error types
-/// like:
-///
-/// class MyError : public ErrorInfo<MyError> {
-///   ....
-/// };
-///
-/// This class provides an implementation of the ErrorInfoBase::kind
-/// method, which is used by the Error RTTI system.
-template <typename ThisErrT, typename ParentErrT = ErrorInfoBase>
-class ErrorInfo : public ParentErrT {
-public:
-  using ParentErrT::ParentErrT; // inherit constructors
-
-  static const void *classID() { return &ThisErrT::ID; }
-
-  const void *dynamicClassID() const override { return &ThisErrT::ID; }
-
-  bool isA(const void *const ClassID) const override {
-    return ClassID == classID() || ParentErrT::isA(ClassID);
-  }
-};
-
-/// Special ErrorInfo subclass representing a list of ErrorInfos.
-/// Instances of this class are constructed by joinError.
-class ErrorList final : public ErrorInfo<ErrorList> {
-  // handleErrors needs to be able to iterate the payload list of an
-  // ErrorList.
-  template <typename... HandlerTs>
-  friend Error handleErrors(Error E, HandlerTs &&... Handlers);
-
-  // joinErrors is implemented in terms of join.
-  friend Error joinErrors(Error, Error);
-
-public:
-  void log(raw_ostream &OS) const override {
-    OS << "Multiple errors:\n";
-    for (auto &ErrPayload : Payloads) {
-      ErrPayload->log(OS);
-      OS << "\n";
-    }
-  }
-
-  std::error_code convertToErrorCode() const override;
-
-  // Used by ErrorInfo::classID.
-  static char ID;
-
-private:
-  ErrorList(std::unique_ptr<ErrorInfoBase> Payload1,
-            std::unique_ptr<ErrorInfoBase> Payload2) {
-    assert(!Payload1->isA<ErrorList>() && !Payload2->isA<ErrorList>() &&
-           "ErrorList constructor payloads should be singleton errors");
-    Payloads.push_back(std::move(Payload1));
-    Payloads.push_back(std::move(Payload2));
-  }
-
-  static Error join(Error E1, Error E2) {
-    if (!E1)
-      return E2;
-    if (!E2)
-      return E1;
-    if (E1.isA<ErrorList>()) {
-      auto &E1List = static_cast<ErrorList &>(*E1.getPtr());
-      if (E2.isA<ErrorList>()) {
-        auto E2Payload = E2.takePayload();
-        auto &E2List = static_cast<ErrorList &>(*E2Payload);
-        for (auto &Payload : E2List.Payloads)
-          E1List.Payloads.push_back(std::move(Payload));
-      } else
-        E1List.Payloads.push_back(E2.takePayload());
-
-      return E1;
-    }
-    if (E2.isA<ErrorList>()) {
-      auto &E2List = static_cast<ErrorList &>(*E2.getPtr());
-      E2List.Payloads.insert(E2List.Payloads.begin(), E1.takePayload());
-      return E2;
-    }
-    return Error(std::unique_ptr<ErrorList>(
-        new ErrorList(E1.takePayload(), E2.takePayload())));
-  }
-
-  std::vector<std::unique_ptr<ErrorInfoBase>> Payloads;
-};
-
-/// Concatenate errors. The resulting Error is unchecked, and contains the
-/// ErrorInfo(s), if any, contained in E1, followed by the
-/// ErrorInfo(s), if any, contained in E2.
-inline Error joinErrors(Error E1, Error E2) {
-  return ErrorList::join(std::move(E1), std::move(E2));
-}
-
-/// Tagged union holding either a T or a Error.
-///
-/// This class parallels ErrorOr, but replaces error_code with Error. Since
-/// Error cannot be copied, this class replaces getError() with
-/// takeError(). It also adds an bool errorIsA<ErrT>() method for testing the
-/// error class type.
-template <class T> class LLVM_NODISCARD Expected {
-  template <class T1> friend class ExpectedAsOutParameter;
-  template <class OtherT> friend class Expected;
-
-  static const bool isRef = std::is_reference<T>::value;
-
-  using wrap = std::reference_wrapper<typename std::remove_reference<T>::type>;
-
-  using error_type = std::unique_ptr<ErrorInfoBase>;
-
-public:
-  using storage_type = typename std::conditional<isRef, wrap, T>::type;
-  using value_type = T;
-
-private:
-  using reference = typename std::remove_reference<T>::type &;
-  using const_reference = const typename std::remove_reference<T>::type &;
-  using pointer = typename std::remove_reference<T>::type *;
-  using const_pointer = const typename std::remove_reference<T>::type *;
-
-public:
-  /// Create an Expected<T> error value from the given Error.
-  Expected(Error Err)
-      : HasError(true)
-  {
-    assert(Err && "Cannot create Expected<T> from Error success value.");
-    new (getErrorStorage()) error_type(Err.takePayload());
-  }
-
-  /// Forbid to convert from Error::success() implicitly, this avoids having
-  /// Expected<T> foo() { return Error::success(); } which compiles otherwise
-  /// but triggers the assertion above.
-  Expected(ErrorSuccess) = delete;
-
-  /// Create an Expected<T> success value from the given OtherT value, which
-  /// must be convertible to T.
-  template <typename OtherT>
-  Expected(OtherT &&Val,
-           typename std::enable_if<std::is_convertible<OtherT, T>::value>::type
-               * = nullptr)
-      : HasError(false)
-  {
-    new (getStorage()) storage_type(std::forward<OtherT>(Val));
-  }
-
-  /// Move construct an Expected<T> value.
-  Expected(Expected &&Other) { moveConstruct(std::move(Other)); }
-
-  /// Move construct an Expected<T> value from an Expected<OtherT>, where OtherT
-  /// must be convertible to T.
-  template <class OtherT>
-  Expected(Expected<OtherT> &&Other,
-           typename std::enable_if<std::is_convertible<OtherT, T>::value>::type
-               * = nullptr) {
-    moveConstruct(std::move(Other));
-  }
-
-  /// Move construct an Expected<T> value from an Expected<OtherT>, where OtherT
-  /// isn't convertible to T.
-  template <class OtherT>
-  explicit Expected(
-      Expected<OtherT> &&Other,
-      typename std::enable_if<!std::is_convertible<OtherT, T>::value>::type * =
-          nullptr) {
-    moveConstruct(std::move(Other));
-  }
-
-  /// Move-assign from another Expected<T>.
-  Expected &operator=(Expected &&Other) {
-    moveAssign(std::move(Other));
-    return *this;
-  }
-
-  /// Destroy an Expected<T>.
-  ~Expected() {
-    assertIsChecked();
-    if (!HasError)
-      getStorage()->~storage_type();
-    else
-      getErrorStorage()->~error_type();
-  }
-
-  /// Return false if there is an error.
-  explicit operator bool() {
-    return !HasError;
-  }
-
-  /// Returns a reference to the stored T value.
-  reference get() {
-    assertIsChecked();
-    return *getStorage();
-  }
-
-  /// Returns a const reference to the stored T value.
-  const_reference get() const {
-    assertIsChecked();
-    return const_cast<Expected<T> *>(this)->get();
-  }
-
-  /// Check that this Expected<T> is an error of type ErrT.
-  template <typename ErrT> bool errorIsA() const {
-    return HasError && (*getErrorStorage())->template isA<ErrT>();
-  }
-
-  /// Take ownership of the stored error.
-  /// After calling this the Expected<T> is in an indeterminate state that can
-  /// only be safely destructed. No further calls (beside the destructor) should
-  /// be made on the Expected<T> vaule.
-  Error takeError() {
-    return HasError ? Error(std::move(*getErrorStorage())) : Error::success();
-  }
-
-  /// Returns a pointer to the stored T value.
-  pointer operator->() {
-    assertIsChecked();
-    return toPointer(getStorage());
-  }
-
-  /// Returns a const pointer to the stored T value.
-  const_pointer operator->() const {
-    assertIsChecked();
-    return toPointer(getStorage());
-  }
-
-  /// Returns a reference to the stored T value.
-  reference operator*() {
-    assertIsChecked();
-    return *getStorage();
-  }
-
-  /// Returns a const reference to the stored T value.
-  const_reference operator*() const {
-    assertIsChecked();
-    return *getStorage();
-  }
-
-private:
-  template <class T1>
-  static bool compareThisIfSameType(const T1 &a, const T1 &b) {
-    return &a == &b;
-  }
-
-  template <class T1, class T2>
-  static bool compareThisIfSameType(const T1 &a, const T2 &b) {
-    return false;
-  }
-
-  template <class OtherT> void moveConstruct(Expected<OtherT> &&Other) {
-    HasError = Other.HasError;
-
-    if (!HasError)
-      new (getStorage()) storage_type(std::move(*Other.getStorage()));
-    else
-      new (getErrorStorage()) error_type(std::move(*Other.getErrorStorage()));
-  }
-
-  template <class OtherT> void moveAssign(Expected<OtherT> &&Other) {
-    assertIsChecked();
-
-    if (compareThisIfSameType(*this, Other))
-      return;
-
-    this->~Expected();
-    new (this) Expected(std::move(Other));
-  }
-
-  pointer toPointer(pointer Val) { return Val; }
-
-  const_pointer toPointer(const_pointer Val) const { return Val; }
-
-  pointer toPointer(wrap *Val) { return &Val->get(); }
-
-  const_pointer toPointer(const wrap *Val) const { return &Val->get(); }
-
-  storage_type *getStorage() {
-    assert(!HasError && "Cannot get value when an error exists!");
-    return reinterpret_cast<storage_type *>(TStorage.buffer);
-  }
-
-  const storage_type *getStorage() const {
-    assert(!HasError && "Cannot get value when an error exists!");
-    return reinterpret_cast<const storage_type *>(TStorage.buffer);
-  }
-
-  error_type *getErrorStorage() {
-    assert(HasError && "Cannot get error when a value exists!");
-    return reinterpret_cast<error_type *>(ErrorStorage.buffer);
-  }
-
-  const error_type *getErrorStorage() const {
-    assert(HasError && "Cannot get error when a value exists!");
-    return reinterpret_cast<const error_type *>(ErrorStorage.buffer);
-  }
-
-  // Used by ExpectedAsOutParameter to reset the checked flag.
-  void setUnchecked() {
-  }
-
-  void assertIsChecked() {
-  }
-
-  union {
-    AlignedCharArrayUnion<storage_type> TStorage;
-    AlignedCharArrayUnion<error_type> ErrorStorage;
-  };
-  bool HasError : 1;
-};
-
-/// Report a serious error, calling any installed error handler. See
-/// ErrorHandling.h.
-LLVM_ATTRIBUTE_NORETURN void report_fatal_error(Error Err,
-                                                bool gen_crash_diag = true);
-
-/// Report a fatal error if Err is a failure value.
-///
-/// This function can be used to wrap calls to fallible functions ONLY when it
-/// is known that the Error will always be a success value. E.g.
-///
-///   @code{.cpp}
-///   // foo only attempts the fallible operation if DoFallibleOperation is
-///   // true. If DoFallibleOperation is false then foo always returns
-///   // Error::success().
-///   Error foo(bool DoFallibleOperation);
-///
-///   cantFail(foo(false));
-///   @endcode
-inline void cantFail(Error Err, const char *Msg = nullptr) {
-  if (Err) {
-    if (!Msg)
-      Msg = "Failure value returned from cantFail wrapped call";
-    wpi_unreachable(Msg);
-  }
-}
-
-/// Report a fatal error if ValOrErr is a failure value, otherwise unwraps and
-/// returns the contained value.
-///
-/// This function can be used to wrap calls to fallible functions ONLY when it
-/// is known that the Error will always be a success value. E.g.
-///
-///   @code{.cpp}
-///   // foo only attempts the fallible operation if DoFallibleOperation is
-///   // true. If DoFallibleOperation is false then foo always returns an int.
-///   Expected<int> foo(bool DoFallibleOperation);
-///
-///   int X = cantFail(foo(false));
-///   @endcode
-template <typename T>
-T cantFail(Expected<T> ValOrErr, const char *Msg = nullptr) {
-  if (ValOrErr)
-    return std::move(*ValOrErr);
-  else {
-    if (!Msg)
-      Msg = "Failure value returned from cantFail wrapped call";
-    wpi_unreachable(Msg);
-  }
-}
-
-/// Report a fatal error if ValOrErr is a failure value, otherwise unwraps and
-/// returns the contained reference.
-///
-/// This function can be used to wrap calls to fallible functions ONLY when it
-/// is known that the Error will always be a success value. E.g.
-///
-///   @code{.cpp}
-///   // foo only attempts the fallible operation if DoFallibleOperation is
-///   // true. If DoFallibleOperation is false then foo always returns a Bar&.
-///   Expected<Bar&> foo(bool DoFallibleOperation);
-///
-///   Bar &X = cantFail(foo(false));
-///   @endcode
-template <typename T>
-T& cantFail(Expected<T&> ValOrErr, const char *Msg = nullptr) {
-  if (ValOrErr)
-    return *ValOrErr;
-  else {
-    if (!Msg)
-      Msg = "Failure value returned from cantFail wrapped call";
-    wpi_unreachable(Msg);
-  }
-}
-
-/// Helper for testing applicability of, and applying, handlers for
-/// ErrorInfo types.
-template <typename HandlerT>
-class ErrorHandlerTraits
-    : public ErrorHandlerTraits<decltype(
-          &std::remove_reference<HandlerT>::type::operator())> {};
-
-// Specialization functions of the form 'Error (const ErrT&)'.
-template <typename ErrT> class ErrorHandlerTraits<Error (&)(ErrT &)> {
-public:
-  static bool appliesTo(const ErrorInfoBase &E) {
-    return E.template isA<ErrT>();
-  }
-
-  template <typename HandlerT>
-  static Error apply(HandlerT &&H, std::unique_ptr<ErrorInfoBase> E) {
-    assert(appliesTo(*E) && "Applying incorrect handler");
-    return H(static_cast<ErrT &>(*E));
-  }
-};
-
-// Specialization functions of the form 'void (const ErrT&)'.
-template <typename ErrT> class ErrorHandlerTraits<void (&)(ErrT &)> {
-public:
-  static bool appliesTo(const ErrorInfoBase &E) {
-    return E.template isA<ErrT>();
-  }
-
-  template <typename HandlerT>
-  static Error apply(HandlerT &&H, std::unique_ptr<ErrorInfoBase> E) {
-    assert(appliesTo(*E) && "Applying incorrect handler");
-    H(static_cast<ErrT &>(*E));
-    return Error::success();
-  }
-};
-
-/// Specialization for functions of the form 'Error (std::unique_ptr<ErrT>)'.
-template <typename ErrT>
-class ErrorHandlerTraits<Error (&)(std::unique_ptr<ErrT>)> {
-public:
-  static bool appliesTo(const ErrorInfoBase &E) {
-    return E.template isA<ErrT>();
-  }
-
-  template <typename HandlerT>
-  static Error apply(HandlerT &&H, std::unique_ptr<ErrorInfoBase> E) {
-    assert(appliesTo(*E) && "Applying incorrect handler");
-    std::unique_ptr<ErrT> SubE(static_cast<ErrT *>(E.release()));
-    return H(std::move(SubE));
-  }
-};
-
-/// Specialization for functions of the form 'void (std::unique_ptr<ErrT>)'.
-template <typename ErrT>
-class ErrorHandlerTraits<void (&)(std::unique_ptr<ErrT>)> {
-public:
-  static bool appliesTo(const ErrorInfoBase &E) {
-    return E.template isA<ErrT>();
-  }
-
-  template <typename HandlerT>
-  static Error apply(HandlerT &&H, std::unique_ptr<ErrorInfoBase> E) {
-    assert(appliesTo(*E) && "Applying incorrect handler");
-    std::unique_ptr<ErrT> SubE(static_cast<ErrT *>(E.release()));
-    H(std::move(SubE));
-    return Error::success();
-  }
-};
-
-// Specialization for member functions of the form 'RetT (const ErrT&)'.
-template <typename C, typename RetT, typename ErrT>
-class ErrorHandlerTraits<RetT (C::*)(ErrT &)>
-    : public ErrorHandlerTraits<RetT (&)(ErrT &)> {};
-
-// Specialization for member functions of the form 'RetT (const ErrT&) const'.
-template <typename C, typename RetT, typename ErrT>
-class ErrorHandlerTraits<RetT (C::*)(ErrT &) const>
-    : public ErrorHandlerTraits<RetT (&)(ErrT &)> {};
-
-// Specialization for member functions of the form 'RetT (const ErrT&)'.
-template <typename C, typename RetT, typename ErrT>
-class ErrorHandlerTraits<RetT (C::*)(const ErrT &)>
-    : public ErrorHandlerTraits<RetT (&)(ErrT &)> {};
-
-// Specialization for member functions of the form 'RetT (const ErrT&) const'.
-template <typename C, typename RetT, typename ErrT>
-class ErrorHandlerTraits<RetT (C::*)(const ErrT &) const>
-    : public ErrorHandlerTraits<RetT (&)(ErrT &)> {};
-
-/// Specialization for member functions of the form
-/// 'RetT (std::unique_ptr<ErrT>)'.
-template <typename C, typename RetT, typename ErrT>
-class ErrorHandlerTraits<RetT (C::*)(std::unique_ptr<ErrT>)>
-    : public ErrorHandlerTraits<RetT (&)(std::unique_ptr<ErrT>)> {};
-
-/// Specialization for member functions of the form
-/// 'RetT (std::unique_ptr<ErrT>) const'.
-template <typename C, typename RetT, typename ErrT>
-class ErrorHandlerTraits<RetT (C::*)(std::unique_ptr<ErrT>) const>
-    : public ErrorHandlerTraits<RetT (&)(std::unique_ptr<ErrT>)> {};
-
-inline Error handleErrorImpl(std::unique_ptr<ErrorInfoBase> Payload) {
-  return Error(std::move(Payload));
-}
-
-template <typename HandlerT, typename... HandlerTs>
-Error handleErrorImpl(std::unique_ptr<ErrorInfoBase> Payload,
-                      HandlerT &&Handler, HandlerTs &&... Handlers) {
-  if (ErrorHandlerTraits<HandlerT>::appliesTo(*Payload))
-    return ErrorHandlerTraits<HandlerT>::apply(std::forward<HandlerT>(Handler),
-                                               std::move(Payload));
-  return handleErrorImpl(std::move(Payload),
-                         std::forward<HandlerTs>(Handlers)...);
-}
-
-/// Pass the ErrorInfo(s) contained in E to their respective handlers. Any
-/// unhandled errors (or Errors returned by handlers) are re-concatenated and
-/// returned.
-/// Because this function returns an error, its result must also be checked
-/// or returned. If you intend to handle all errors use handleAllErrors
-/// (which returns void, and will abort() on unhandled errors) instead.
-template <typename... HandlerTs>
-Error handleErrors(Error E, HandlerTs &&... Hs) {
-  if (!E)
-    return Error::success();
-
-  std::unique_ptr<ErrorInfoBase> Payload = E.takePayload();
-
-  if (Payload->isA<ErrorList>()) {
-    ErrorList &List = static_cast<ErrorList &>(*Payload);
-    Error R;
-    for (auto &P : List.Payloads)
-      R = ErrorList::join(
-          std::move(R),
-          handleErrorImpl(std::move(P), std::forward<HandlerTs>(Hs)...));
-    return R;
-  }
-
-  return handleErrorImpl(std::move(Payload), std::forward<HandlerTs>(Hs)...);
-}
-
-/// Behaves the same as handleErrors, except that by contract all errors
-/// *must* be handled by the given handlers (i.e. there must be no remaining
-/// errors after running the handlers, or wpi_unreachable is called).
-template <typename... HandlerTs>
-void handleAllErrors(Error E, HandlerTs &&... Handlers) {
-  cantFail(handleErrors(std::move(E), std::forward<HandlerTs>(Handlers)...));
-}
-
-/// Check that E is a non-error, then drop it.
-/// If E is an error, wpi_unreachable will be called.
-inline void handleAllErrors(Error E) {
-  cantFail(std::move(E));
-}
-
-/// Handle any errors (if present) in an Expected<T>, then try a recovery path.
-///
-/// If the incoming value is a success value it is returned unmodified. If it
-/// is a failure value then it the contained error is passed to handleErrors.
-/// If handleErrors is able to handle the error then the RecoveryPath functor
-/// is called to supply the final result. If handleErrors is not able to
-/// handle all errors then the unhandled errors are returned.
-///
-/// This utility enables the follow pattern:
-///
-///   @code{.cpp}
-///   enum FooStrategy { Aggressive, Conservative };
-///   Expected<Foo> foo(FooStrategy S);
-///
-///   auto ResultOrErr =
-///     handleExpected(
-///       foo(Aggressive),
-///       []() { return foo(Conservative); },
-///       [](AggressiveStrategyError&) {
-///         // Implicitly conusme this - we'll recover by using a conservative
-///         // strategy.
-///       });
-///
-///   @endcode
-template <typename T, typename RecoveryFtor, typename... HandlerTs>
-Expected<T> handleExpected(Expected<T> ValOrErr, RecoveryFtor &&RecoveryPath,
-                           HandlerTs &&... Handlers) {
-  if (ValOrErr)
-    return ValOrErr;
-
-  if (auto Err = handleErrors(ValOrErr.takeError(),
-                              std::forward<HandlerTs>(Handlers)...))
-    return std::move(Err);
-
-  return RecoveryPath();
-}
-
-/// Log all errors (if any) in E to OS. If there are any errors, ErrorBanner
-/// will be printed before the first one is logged. A newline will be printed
-/// after each error.
-///
-/// This function is compatible with the helpers from Support/WithColor.h. You
-/// can pass any of them as the OS. Please consider using them instead of
-/// including 'error: ' in the ErrorBanner.
-///
-/// This is useful in the base level of your program to allow clean termination
-/// (allowing clean deallocation of resources, etc.), while reporting error
-/// information to the user.
-void logAllUnhandledErrors(Error E, raw_ostream &OS, Twine ErrorBanner = {});
-
-/// Write all error messages (if any) in E to a string. The newline character
-/// is used to separate error messages.
-inline std::string toString(Error E) {
-  SmallVector<std::string, 2> Errors;
-  handleAllErrors(std::move(E), [&Errors](const ErrorInfoBase &EI) {
-    Errors.push_back(EI.message());
-  });
-  return join(Errors.begin(), Errors.end(), "\n");
-}
-
-/// Consume a Error without doing anything. This method should be used
-/// only where an error can be considered a reasonable and expected return
-/// value.
-///
-/// Uses of this method are potentially indicative of design problems: If it's
-/// legitimate to do nothing while processing an "error", the error-producer
-/// might be more clearly refactored to return an Optional<T>.
-inline void consumeError(Error Err) {
-  handleAllErrors(std::move(Err), [](const ErrorInfoBase &) {});
-}
-
-/// Helper for converting an Error to a bool.
-///
-/// This method returns true if Err is in an error state, or false if it is
-/// in a success state.  Puts Err in a checked state in both cases (unlike
-/// Error::operator bool(), which only does this for success states).
-inline bool errorToBool(Error Err) {
-  bool IsError = static_cast<bool>(Err);
-  if (IsError)
-    consumeError(std::move(Err));
-  return IsError;
-}
-
-/// Helper for Errors used as out-parameters.
-///
-/// This helper is for use with the Error-as-out-parameter idiom, where an error
-/// is passed to a function or method by reference, rather than being returned.
-/// In such cases it is helpful to set the checked bit on entry to the function
-/// so that the error can be written to (unchecked Errors abort on assignment)
-/// and clear the checked bit on exit so that clients cannot accidentally forget
-/// to check the result. This helper performs these actions automatically using
-/// RAII:
-///
-///   @code{.cpp}
-///   Result foo(Error &Err) {
-///     ErrorAsOutParameter ErrAsOutParam(&Err); // 'Checked' flag set
-///     // <body of foo>
-///     // <- 'Checked' flag auto-cleared when ErrAsOutParam is destructed.
-///   }
-///   @endcode
-///
-/// ErrorAsOutParameter takes an Error* rather than Error& so that it can be
-/// used with optional Errors (Error pointers that are allowed to be null). If
-/// ErrorAsOutParameter took an Error reference, an instance would have to be
-/// created inside every condition that verified that Error was non-null. By
-/// taking an Error pointer we can just create one instance at the top of the
-/// function.
-class ErrorAsOutParameter {
-public:
-  ErrorAsOutParameter(Error *Err) : Err(Err) {
-    // Raise the checked bit if Err is success.
-    if (Err)
-      (void)!!*Err;
-  }
-
-  ~ErrorAsOutParameter() {
-    // Clear the checked bit.
-    if (Err && !*Err)
-      *Err = Error::success();
-  }
-
-private:
-  Error *Err;
-};
-
-/// Helper for Expected<T>s used as out-parameters.
-///
-/// See ErrorAsOutParameter.
-template <typename T>
-class ExpectedAsOutParameter {
-public:
-  ExpectedAsOutParameter(Expected<T> *ValOrErr)
-    : ValOrErr(ValOrErr) {
-    if (ValOrErr)
-      (void)!!*ValOrErr;
-  }
-
-  ~ExpectedAsOutParameter() {
-    if (ValOrErr)
-      ValOrErr->setUnchecked();
-  }
-
-private:
-  Expected<T> *ValOrErr;
-};
-
-/// This class wraps a std::error_code in a Error.
-///
-/// This is useful if you're writing an interface that returns a Error
-/// (or Expected) and you want to call code that still returns
-/// std::error_codes.
-class ECError : public ErrorInfo<ECError> {
-  friend Error errorCodeToError(std::error_code);
-
-  virtual void anchor() override;
-
-public:
-  void setErrorCode(std::error_code EC) { this->EC = EC; }
-  std::error_code convertToErrorCode() const override { return EC; }
-  void log(raw_ostream &OS) const override { OS << EC.message(); }
-
-  // Used by ErrorInfo::classID.
-  static char ID;
-
-protected:
-  ECError() = default;
-  ECError(std::error_code EC) : EC(EC) {}
-
-  std::error_code EC;
-};
-
-/// The value returned by this function can be returned from convertToErrorCode
-/// for Error values where no sensible translation to std::error_code exists.
-/// It should only be used in this situation, and should never be used where a
-/// sensible conversion to std::error_code is available, as attempts to convert
-/// to/from this error will result in a fatal error. (i.e. it is a programmatic
-///error to try to convert such a value).
-std::error_code inconvertibleErrorCode();
-
-/// Helper for converting an std::error_code to a Error.
-Error errorCodeToError(std::error_code EC);
-
-/// Helper for converting an ECError to a std::error_code.
-///
-/// This method requires that Err be Error() or an ECError, otherwise it
-/// will trigger a call to abort().
-std::error_code errorToErrorCode(Error Err);
-
-/// Convert an ErrorOr<T> to an Expected<T>.
-template <typename T> Expected<T> errorOrToExpected(ErrorOr<T> &&EO) {
-  if (auto EC = EO.getError())
-    return errorCodeToError(EC);
-  return std::move(*EO);
-}
-
-/// Convert an Expected<T> to an ErrorOr<T>.
-template <typename T> ErrorOr<T> expectedToErrorOr(Expected<T> &&E) {
-  if (auto Err = E.takeError())
-    return errorToErrorCode(std::move(Err));
-  return std::move(*E);
-}
-
-/// This class wraps a string in an Error.
-///
-/// StringError is useful in cases where the client is not expected to be able
-/// to consume the specific error message programmatically (for example, if the
-/// error message is to be presented to the user).
-///
-/// StringError can also be used when additional information is to be printed
-/// along with a error_code message. Depending on the constructor called, this
-/// class can either display:
-///    1. the error_code message (ECError behavior)
-///    2. a string
-///    3. the error_code message and a string
-///
-/// These behaviors are useful when subtyping is required; for example, when a
-/// specific library needs an explicit error type. In the example below,
-/// PDBError is derived from StringError:
-///
-///   @code{.cpp}
-///   Expected<int> foo() {
-///      return wpi::make_error<PDBError>(pdb_error_code::dia_failed_loading,
-///                                       "Additional information");
-///   }
-///   @endcode
-///
-class StringError : public ErrorInfo<StringError> {
-public:
-  static char ID;
-
-  // Prints EC + S and converts to EC
-  StringError(std::error_code EC, const Twine &S = Twine());
-
-  // Prints S and converts to EC
-  StringError(const Twine &S, std::error_code EC);
-
-  void log(raw_ostream &OS) const override;
-  std::error_code convertToErrorCode() const override;
-
-  const std::string &getMessage() const { return Msg; }
-
-private:
-  std::string Msg;
-  std::error_code EC;
-  const bool PrintMsgOnly = false;
-};
-
-/// Create formatted StringError object.
-template <typename... Ts>
-Error createStringError(std::error_code EC, char const *Fmt,
-                        const Ts &... Vals) {
-  std::string Buffer;
-  raw_string_ostream Stream(Buffer);
-  Stream << format(Fmt, Vals...);
-  return make_error<StringError>(Stream.str(), EC);
-}
-
-Error createStringError(std::error_code EC, char const *Msg);
-
-/// This class wraps a filename and another Error.
-///
-/// In some cases, an error needs to live along a 'source' name, in order to
-/// show more detailed information to the user.
-class FileError final : public ErrorInfo<FileError> {
-
-  friend Error createFileError(std::string, Error);
-
-public:
-  void log(raw_ostream &OS) const override {
-    assert(Err && !FileName.empty() && "Trying to log after takeError().");
-    OS << "'" << FileName << "': ";
-    Err->log(OS);
-  }
-
-  Error takeError() { return Error(std::move(Err)); }
-
-  std::error_code convertToErrorCode() const override;
-
-  // Used by ErrorInfo::classID.
-  static char ID;
-
-private:
-  FileError(std::string F, std::unique_ptr<ErrorInfoBase> E) {
-    assert(E && "Cannot create FileError from Error success value.");
-    assert(!F.empty() &&
-           "The file name provided to FileError must not be empty.");
-    FileName = F;
-    Err = std::move(E);
-  }
-
-  static Error build(std::string F, Error E) {
-    return Error(std::unique_ptr<FileError>(new FileError(F, E.takePayload())));
-  }
-
-  std::string FileName;
-  std::unique_ptr<ErrorInfoBase> Err;
-};
-
-/// Concatenate a source file path and/or name with an Error. The resulting
-/// Error is unchecked.
-inline Error createFileError(std::string F, Error E) {
-  return FileError::build(F, std::move(E));
-}
-
-Error createFileError(std::string F, ErrorSuccess) = delete;
-
-/// Helper for check-and-exit error handling.
-///
-/// For tool use only. NOT FOR USE IN LIBRARY CODE.
-///
-class ExitOnError {
-public:
-  /// Create an error on exit helper.
-  ExitOnError(std::string Banner = "", int DefaultErrorExitCode = 1)
-      : Banner(std::move(Banner)),
-        GetExitCode([=](const Error &) { return DefaultErrorExitCode; }) {}
-
-  /// Set the banner string for any errors caught by operator().
-  void setBanner(std::string Banner) { this->Banner = std::move(Banner); }
-
-  /// Set the exit-code mapper function.
-  void setExitCodeMapper(std::function<int(const Error &)> GetExitCode) {
-    this->GetExitCode = std::move(GetExitCode);
-  }
-
-  /// Check Err. If it's in a failure state log the error(s) and exit.
-  void operator()(Error Err) const { checkError(std::move(Err)); }
-
-  /// Check E. If it's in a success state then return the contained value. If
-  /// it's in a failure state log the error(s) and exit.
-  template <typename T> T operator()(Expected<T> &&E) const {
-    checkError(E.takeError());
-    return std::move(*E);
-  }
-
-  /// Check E. If it's in a success state then return the contained reference. If
-  /// it's in a failure state log the error(s) and exit.
-  template <typename T> T& operator()(Expected<T&> &&E) const {
-    checkError(E.takeError());
-    return *E;
-  }
-
-private:
-  void checkError(Error Err) const {
-    if (Err) {
-      int ExitCode = GetExitCode(Err);
-      logAllUnhandledErrors(std::move(Err), errs(), Banner);
-      exit(ExitCode);
-    }
-  }
-
-  std::string Banner;
-  std::function<int(const Error &)> GetExitCode;
-};
-
-} // end namespace wpi
-
-#endif // WPIUTIL_WPI_ERROR_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorHandling.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorHandling.h
index 5c89c5e..da9af4b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorHandling.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorHandling.h
@@ -17,11 +17,9 @@
 
 #include "wpi/Compiler.h"
 #include <string>
+#include <string_view>
 
 namespace wpi {
-class StringRef;
-  class Twine;
-
   /// An error handler callback.
   typedef void (*fatal_error_handler_t)(void *user_data,
                                         const std::string& reason,
@@ -73,9 +71,7 @@
                                                 bool gen_crash_diag = true);
 LLVM_ATTRIBUTE_NORETURN void report_fatal_error(const std::string &reason,
                                                 bool gen_crash_diag = true);
-LLVM_ATTRIBUTE_NORETURN void report_fatal_error(StringRef reason,
-                                                bool gen_crash_diag = true);
-LLVM_ATTRIBUTE_NORETURN void report_fatal_error(const Twine &reason,
+LLVM_ATTRIBUTE_NORETURN void report_fatal_error(std::string_view reason,
                                                 bool gen_crash_diag = true);
 
 /// Installs a new bad alloc error handler that should be used whenever a
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorOr.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorOr.h
deleted file mode 100644
index e1803fd..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ErrorOr.h
+++ /dev/null
@@ -1,279 +0,0 @@
-//===- llvm/Support/ErrorOr.h - Error Smart Pointer -------------*- C++ -*-===//
-//
-//                             The LLVM Linker
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-///
-/// \file
-///
-/// Provides ErrorOr<T> smart pointer.
-///
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_ERROROR_H
-#define WPIUTIL_WPI_ERROROR_H
-
-#include "wpi/AlignOf.h"
-#include <cassert>
-#include <system_error>
-#include <type_traits>
-#include <utility>
-
-namespace wpi {
-
-/// Represents either an error or a value T.
-///
-/// ErrorOr<T> is a pointer-like class that represents the result of an
-/// operation. The result is either an error, or a value of type T. This is
-/// designed to emulate the usage of returning a pointer where nullptr indicates
-/// failure. However instead of just knowing that the operation failed, we also
-/// have an error_code and optional user data that describes why it failed.
-///
-/// It is used like the following.
-/// \code
-///   ErrorOr<Buffer> getBuffer();
-///
-///   auto buffer = getBuffer();
-///   if (error_code ec = buffer.getError())
-///     return ec;
-///   buffer->write("adena");
-/// \endcode
-///
-///
-/// Implicit conversion to bool returns true if there is a usable value. The
-/// unary * and -> operators provide pointer like access to the value. Accessing
-/// the value when there is an error has undefined behavior.
-///
-/// When T is a reference type the behavior is slightly different. The reference
-/// is held in a std::reference_wrapper<std::remove_reference<T>::type>, and
-/// there is special handling to make operator -> work as if T was not a
-/// reference.
-///
-/// T cannot be a rvalue reference.
-template<class T>
-class ErrorOr {
-  template <class OtherT> friend class ErrorOr;
-
-  static const bool isRef = std::is_reference<T>::value;
-
-  using wrap = std::reference_wrapper<typename std::remove_reference<T>::type>;
-
-public:
-  using storage_type = typename std::conditional<isRef, wrap, T>::type;
-
-private:
-  using reference = typename std::remove_reference<T>::type &;
-  using const_reference = const typename std::remove_reference<T>::type &;
-  using pointer = typename std::remove_reference<T>::type *;
-  using const_pointer = const typename std::remove_reference<T>::type *;
-
-public:
-  template <class E>
-  ErrorOr(E ErrorCode,
-          typename std::enable_if<std::is_error_code_enum<E>::value ||
-                                      std::is_error_condition_enum<E>::value,
-                                  void *>::type = nullptr)
-      : HasError(true) {
-    new (getErrorStorage()) std::error_code(make_error_code(ErrorCode));
-  }
-
-  ErrorOr(std::error_code EC) : HasError(true) {
-    new (getErrorStorage()) std::error_code(EC);
-  }
-
-  template <class OtherT>
-  ErrorOr(OtherT &&Val,
-          typename std::enable_if<std::is_convertible<OtherT, T>::value>::type
-              * = nullptr)
-      : HasError(false) {
-    new (getStorage()) storage_type(std::forward<OtherT>(Val));
-  }
-
-  ErrorOr(const ErrorOr &Other) {
-    copyConstruct(Other);
-  }
-
-  template <class OtherT>
-  ErrorOr(
-      const ErrorOr<OtherT> &Other,
-      typename std::enable_if<std::is_convertible<OtherT, T>::value>::type * =
-          nullptr) {
-    copyConstruct(Other);
-  }
-
-  template <class OtherT>
-  explicit ErrorOr(
-      const ErrorOr<OtherT> &Other,
-      typename std::enable_if<
-          !std::is_convertible<OtherT, const T &>::value>::type * = nullptr) {
-    copyConstruct(Other);
-  }
-
-  ErrorOr(ErrorOr &&Other) {
-    moveConstruct(std::move(Other));
-  }
-
-  template <class OtherT>
-  ErrorOr(
-      ErrorOr<OtherT> &&Other,
-      typename std::enable_if<std::is_convertible<OtherT, T>::value>::type * =
-          nullptr) {
-    moveConstruct(std::move(Other));
-  }
-
-  // This might eventually need SFINAE but it's more complex than is_convertible
-  // & I'm too lazy to write it right now.
-  template <class OtherT>
-  explicit ErrorOr(
-      ErrorOr<OtherT> &&Other,
-      typename std::enable_if<!std::is_convertible<OtherT, T>::value>::type * =
-          nullptr) {
-    moveConstruct(std::move(Other));
-  }
-
-  ErrorOr &operator=(const ErrorOr &Other) {
-    copyAssign(Other);
-    return *this;
-  }
-
-  ErrorOr &operator=(ErrorOr &&Other) {
-    moveAssign(std::move(Other));
-    return *this;
-  }
-
-  ~ErrorOr() {
-    if (!HasError)
-      getStorage()->~storage_type();
-  }
-
-  /// Return false if there is an error.
-  explicit operator bool() const {
-    return !HasError;
-  }
-
-  reference get() { return *getStorage(); }
-  const_reference get() const { return const_cast<ErrorOr<T> *>(this)->get(); }
-
-  std::error_code getError() const {
-    return HasError ? *getErrorStorage() : std::error_code();
-  }
-
-  pointer operator ->() {
-    return toPointer(getStorage());
-  }
-
-  const_pointer operator->() const { return toPointer(getStorage()); }
-
-  reference operator *() {
-    return *getStorage();
-  }
-
-  const_reference operator*() const { return *getStorage(); }
-
-private:
-  template <class OtherT>
-  void copyConstruct(const ErrorOr<OtherT> &Other) {
-    if (!Other.HasError) {
-      // Get the other value.
-      HasError = false;
-      new (getStorage()) storage_type(*Other.getStorage());
-    } else {
-      // Get other's error.
-      HasError = true;
-      new (getErrorStorage()) std::error_code(Other.getError());
-    }
-  }
-
-  template <class T1>
-  static bool compareThisIfSameType(const T1 &a, const T1 &b) {
-    return &a == &b;
-  }
-
-  template <class T1, class T2>
-  static bool compareThisIfSameType(const T1 &a, const T2 &b) {
-    return false;
-  }
-
-  template <class OtherT>
-  void copyAssign(const ErrorOr<OtherT> &Other) {
-    if (compareThisIfSameType(*this, Other))
-      return;
-
-    this->~ErrorOr();
-    new (this) ErrorOr(Other);
-  }
-
-  template <class OtherT>
-  void moveConstruct(ErrorOr<OtherT> &&Other) {
-    if (!Other.HasError) {
-      // Get the other value.
-      HasError = false;
-      new (getStorage()) storage_type(std::move(*Other.getStorage()));
-    } else {
-      // Get other's error.
-      HasError = true;
-      new (getErrorStorage()) std::error_code(Other.getError());
-    }
-  }
-
-  template <class OtherT>
-  void moveAssign(ErrorOr<OtherT> &&Other) {
-    if (compareThisIfSameType(*this, Other))
-      return;
-
-    this->~ErrorOr();
-    new (this) ErrorOr(std::move(Other));
-  }
-
-  pointer toPointer(pointer Val) {
-    return Val;
-  }
-
-  const_pointer toPointer(const_pointer Val) const { return Val; }
-
-  pointer toPointer(wrap *Val) {
-    return &Val->get();
-  }
-
-  const_pointer toPointer(const wrap *Val) const { return &Val->get(); }
-
-  storage_type *getStorage() {
-    assert(!HasError && "Cannot get value when an error exists!");
-    return reinterpret_cast<storage_type*>(TStorage.buffer);
-  }
-
-  const storage_type *getStorage() const {
-    assert(!HasError && "Cannot get value when an error exists!");
-    return reinterpret_cast<const storage_type*>(TStorage.buffer);
-  }
-
-  std::error_code *getErrorStorage() {
-    assert(HasError && "Cannot get error when a value exists!");
-    return reinterpret_cast<std::error_code *>(ErrorStorage.buffer);
-  }
-
-  const std::error_code *getErrorStorage() const {
-    return const_cast<ErrorOr<T> *>(this)->getErrorStorage();
-  }
-
-  union {
-    AlignedCharArrayUnion<storage_type> TStorage;
-    AlignedCharArrayUnion<std::error_code> ErrorStorage;
-  };
-  bool HasError : 1;
-};
-
-template <class T, class E>
-typename std::enable_if<std::is_error_code_enum<E>::value ||
-                            std::is_error_condition_enum<E>::value,
-                        bool>::type
-operator==(const ErrorOr<T> &Err, E Code) {
-  return Err.getError() == Code;
-}
-
-} // end namespace wpi
-
-#endif // LLVM_SUPPORT_ERROROR_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/EventLoopRunner.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/EventLoopRunner.h
index 8150091..5d35d40 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/EventLoopRunner.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/EventLoopRunner.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_EVENTLOOPRUNNER_H_
 #define WPIUTIL_WPI_EVENTLOOPRUNNER_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FileSystem.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FileSystem.h
deleted file mode 100644
index 668eea6..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FileSystem.h
+++ /dev/null
@@ -1,1023 +0,0 @@
-//===- llvm/Support/FileSystem.h - File System OS Concept -------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file declares the wpi::sys::fs namespace. It is designed after
-// TR2/boost filesystem (v3), but modified to remove exception handling and the
-// path class.
-//
-// All functions return an error_code and their actual work via the last out
-// argument. The out argument is defined if and only if errc::success is
-// returned. A function may return any error code in the generic or system
-// category. However, they shall be equivalent to any error conditions listed
-// in each functions respective documentation if the condition applies. [ note:
-// this does not guarantee that error_code will be in the set of explicitly
-// listed codes, but it does guarantee that if any of the explicitly listed
-// errors occur, the correct error_code will be used ]. All functions may
-// return errc::not_enough_memory if there is not enough memory to complete the
-// operation.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_FILESYSTEM_H
-#define WPIUTIL_WPI_FILESYSTEM_H
-
-#include "wpi/Chrono.h"
-#include "wpi/SmallString.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
-#include "wpi/Error.h"
-#include "wpi/ErrorHandling.h"
-#include "wpi/ErrorOr.h"
-#include <cassert>
-#include <cstdint>
-#include <ctime>
-#include <memory>
-#include <stack>
-#include <string>
-#include <system_error>
-#include <tuple>
-#include <vector>
-
-#include <sys/stat.h>
-
-namespace wpi {
-namespace sys {
-namespace fs {
-
-#if defined(_WIN32)
-// A Win32 HANDLE is a typedef of void*
-using file_t = void *;
-#else
-using file_t = int;
-#endif
-
-extern const file_t kInvalidFile;
-
-/// An enumeration for the file system's view of the type.
-enum class file_type {
-  status_error,
-  file_not_found,
-  regular_file,
-  directory_file,
-  symlink_file,
-  block_file,
-  character_file,
-  fifo_file,
-  socket_file,
-  type_unknown
-};
-
-/// space_info - Self explanatory.
-struct space_info {
-  uint64_t capacity;
-  uint64_t free;
-  uint64_t available;
-};
-
-enum perms {
-  no_perms = 0,
-  owner_read = 0400,
-  owner_write = 0200,
-  owner_exe = 0100,
-  owner_all = owner_read | owner_write | owner_exe,
-  group_read = 040,
-  group_write = 020,
-  group_exe = 010,
-  group_all = group_read | group_write | group_exe,
-  others_read = 04,
-  others_write = 02,
-  others_exe = 01,
-  others_all = others_read | others_write | others_exe,
-  all_read = owner_read | group_read | others_read,
-  all_write = owner_write | group_write | others_write,
-  all_exe = owner_exe | group_exe | others_exe,
-  all_all = owner_all | group_all | others_all,
-  set_uid_on_exe = 04000,
-  set_gid_on_exe = 02000,
-  sticky_bit = 01000,
-  all_perms = all_all | set_uid_on_exe | set_gid_on_exe | sticky_bit,
-  perms_not_known = 0xFFFF
-};
-
-// Helper functions so that you can use & and | to manipulate perms bits:
-inline perms operator|(perms l, perms r) {
-  return static_cast<perms>(static_cast<unsigned short>(l) |
-                            static_cast<unsigned short>(r));
-}
-inline perms operator&(perms l, perms r) {
-  return static_cast<perms>(static_cast<unsigned short>(l) &
-                            static_cast<unsigned short>(r));
-}
-inline perms &operator|=(perms &l, perms r) {
-  l = l | r;
-  return l;
-}
-inline perms &operator&=(perms &l, perms r) {
-  l = l & r;
-  return l;
-}
-inline perms operator~(perms x) {
-  // Avoid UB by explicitly truncating the (unsigned) ~ result.
-  return static_cast<perms>(
-      static_cast<unsigned short>(~static_cast<unsigned short>(x)));
-}
-
-class UniqueID {
-  uint64_t Device;
-  uint64_t File;
-
-public:
-  UniqueID() = default;
-  UniqueID(uint64_t Device, uint64_t File) : Device(Device), File(File) {}
-
-  bool operator==(const UniqueID &Other) const {
-    return Device == Other.Device && File == Other.File;
-  }
-  bool operator!=(const UniqueID &Other) const { return !(*this == Other); }
-  bool operator<(const UniqueID &Other) const {
-    return std::tie(Device, File) < std::tie(Other.Device, Other.File);
-  }
-
-  uint64_t getDevice() const { return Device; }
-  uint64_t getFile() const { return File; }
-};
-
-/// Represents the result of a call to directory_iterator::status(). This is a
-/// subset of the information returned by a regular sys::fs::status() call, and
-/// represents the information provided by Windows FileFirstFile/FindNextFile.
-class basic_file_status {
-protected:
-  #ifndef _WIN32
-  time_t fs_st_atime = 0;
-  time_t fs_st_mtime = 0;
-  uint32_t fs_st_atime_nsec = 0;
-  uint32_t fs_st_mtime_nsec = 0;
-  uid_t fs_st_uid = 0;
-  gid_t fs_st_gid = 0;
-  off_t fs_st_size = 0;
-  #else
-  uint32_t LastAccessedTimeHigh = 0;
-  uint32_t LastAccessedTimeLow = 0;
-  uint32_t LastWriteTimeHigh = 0;
-  uint32_t LastWriteTimeLow = 0;
-  uint32_t FileSizeHigh = 0;
-  uint32_t FileSizeLow = 0;
-  #endif
-  file_type Type = file_type::status_error;
-  perms Perms = perms_not_known;
-
-public:
-  basic_file_status() = default;
-
-  explicit basic_file_status(file_type Type) : Type(Type) {}
-
-  #ifndef _WIN32
-  basic_file_status(file_type Type, perms Perms, time_t ATime,
-                    uint32_t ATimeNSec, time_t MTime, uint32_t MTimeNSec,
-                    uid_t UID, gid_t GID, off_t Size)
-      : fs_st_atime(ATime), fs_st_mtime(MTime),
-        fs_st_atime_nsec(ATimeNSec), fs_st_mtime_nsec(MTimeNSec),
-        fs_st_uid(UID), fs_st_gid(GID),
-        fs_st_size(Size), Type(Type), Perms(Perms) {}
-  #else
-  basic_file_status(file_type Type, perms Perms, uint32_t LastAccessTimeHigh,
-                    uint32_t LastAccessTimeLow, uint32_t LastWriteTimeHigh,
-                    uint32_t LastWriteTimeLow, uint32_t FileSizeHigh,
-                    uint32_t FileSizeLow)
-      : LastAccessedTimeHigh(LastAccessTimeHigh),
-        LastAccessedTimeLow(LastAccessTimeLow),
-        LastWriteTimeHigh(LastWriteTimeHigh),
-        LastWriteTimeLow(LastWriteTimeLow), FileSizeHigh(FileSizeHigh),
-        FileSizeLow(FileSizeLow), Type(Type), Perms(Perms) {}
-  #endif
-
-  // getters
-  file_type type() const { return Type; }
-  perms permissions() const { return Perms; }
-
-  /// The file access time as reported from the underlying file system.
-  ///
-  /// Also see comments on \c getLastModificationTime() related to the precision
-  /// of the returned value.
-  TimePoint<> getLastAccessedTime() const;
-
-  /// The file modification time as reported from the underlying file system.
-  ///
-  /// The returned value allows for nanosecond precision but the actual
-  /// resolution is an implementation detail of the underlying file system.
-  /// There is no guarantee for what kind of resolution you can expect, the
-  /// resolution can differ across platforms and even across mountpoints on the
-  /// same machine.
-  TimePoint<> getLastModificationTime() const;
-
-  #ifndef _WIN32
-  uint32_t getUser() const { return fs_st_uid; }
-  uint32_t getGroup() const { return fs_st_gid; }
-  uint64_t getSize() const { return fs_st_size; }
-  #else
-  uint32_t getUser() const {
-    return 9999; // Not applicable to Windows, so...
-  }
-
-  uint32_t getGroup() const {
-    return 9999; // Not applicable to Windows, so...
-  }
-
-  uint64_t getSize() const {
-    return (uint64_t(FileSizeHigh) << 32) + FileSizeLow;
-  }
-  #endif
-
-  // setters
-  void type(file_type v) { Type = v; }
-  void permissions(perms p) { Perms = p; }
-};
-
-/// Represents the result of a call to sys::fs::status().
-class file_status : public basic_file_status {
-  friend bool equivalent(file_status A, file_status B);
-
-  #ifndef _WIN32
-  dev_t fs_st_dev = 0;
-  nlink_t fs_st_nlinks = 0;
-  ino_t fs_st_ino = 0;
-  #else
-  uint32_t NumLinks = 0;
-  uint32_t VolumeSerialNumber = 0;
-  uint32_t FileIndexHigh = 0;
-  uint32_t FileIndexLow = 0;
-  #endif
-
-public:
-  file_status() = default;
-
-  explicit file_status(file_type Type) : basic_file_status(Type) {}
-
-  #ifndef _WIN32
-  file_status(file_type Type, perms Perms, dev_t Dev, nlink_t Links, ino_t Ino,
-              time_t ATime, uint32_t ATimeNSec,
-              time_t MTime, uint32_t MTimeNSec,
-              uid_t UID, gid_t GID, off_t Size)
-      : basic_file_status(Type, Perms, ATime, ATimeNSec, MTime, MTimeNSec,
-                          UID, GID, Size),
-        fs_st_dev(Dev), fs_st_nlinks(Links), fs_st_ino(Ino) {}
-  #else
-  file_status(file_type Type, perms Perms, uint32_t LinkCount,
-              uint32_t LastAccessTimeHigh, uint32_t LastAccessTimeLow,
-              uint32_t LastWriteTimeHigh, uint32_t LastWriteTimeLow,
-              uint32_t VolumeSerialNumber, uint32_t FileSizeHigh,
-              uint32_t FileSizeLow, uint32_t FileIndexHigh,
-              uint32_t FileIndexLow)
-      : basic_file_status(Type, Perms, LastAccessTimeHigh, LastAccessTimeLow,
-                          LastWriteTimeHigh, LastWriteTimeLow, FileSizeHigh,
-                          FileSizeLow),
-        NumLinks(LinkCount), VolumeSerialNumber(VolumeSerialNumber),
-        FileIndexHigh(FileIndexHigh), FileIndexLow(FileIndexLow) {}
-  #endif
-
-  UniqueID getUniqueID() const;
-  uint32_t getLinkCount() const;
-};
-
-/// @}
-/// @name Physical Operators
-/// @{
-
-/// Make \a path an absolute path.
-///
-/// Makes \a path absolute using the \a current_directory if it is not already.
-/// An empty \a path will result in the \a current_directory.
-///
-/// /absolute/path   => /absolute/path
-/// relative/../path => <current-directory>/relative/../path
-///
-/// @param path A path that is modified to be an absolute path.
-void make_absolute(const Twine &current_directory, SmallVectorImpl<char> &path);
-
-/// Make \a path an absolute path.
-///
-/// Makes \a path absolute using the current directory if it is not already. An
-/// empty \a path will result in the current directory.
-///
-/// /absolute/path   => /absolute/path
-/// relative/../path => <current-directory>/relative/../path
-///
-/// @param path A path that is modified to be an absolute path.
-/// @returns errc::success if \a path has been made absolute, otherwise a
-///          platform-specific error_code.
-std::error_code make_absolute(SmallVectorImpl<char> &path);
-
-/// Get the current path.
-///
-/// @param result Holds the current path on return.
-/// @returns errc::success if the current path has been stored in result,
-///          otherwise a platform-specific error_code.
-std::error_code current_path(SmallVectorImpl<char> &result);
-
-/// @}
-/// @name Physical Observers
-/// @{
-
-/// Does file exist?
-///
-/// @param status A basic_file_status previously returned from stat.
-/// @returns True if the file represented by status exists, false if it does
-///          not.
-bool exists(const basic_file_status &status);
-
-enum class AccessMode { Exist, Write, Execute };
-
-/// Can the file be accessed?
-///
-/// @param Path Input path.
-/// @returns errc::success if the path can be accessed, otherwise a
-///          platform-specific error_code.
-std::error_code access(const Twine &Path, AccessMode Mode);
-
-/// Does file exist?
-///
-/// @param Path Input path.
-/// @returns True if it exists, false otherwise.
-inline bool exists(const Twine &Path) {
-  return !access(Path, AccessMode::Exist);
-}
-
-/// Can we write this file?
-///
-/// @param Path Input path.
-/// @returns True if we can write to it, false otherwise.
-inline bool can_write(const Twine &Path) {
-  return !access(Path, AccessMode::Write);
-}
-
-/// Do file_status's represent the same thing?
-///
-/// @param A Input file_status.
-/// @param B Input file_status.
-///
-/// assert(status_known(A) || status_known(B));
-///
-/// @returns True if A and B both represent the same file system entity, false
-///          otherwise.
-bool equivalent(file_status A, file_status B);
-
-/// Do paths represent the same thing?
-///
-/// assert(status_known(A) || status_known(B));
-///
-/// @param A Input path A.
-/// @param B Input path B.
-/// @param result Set to true if stat(A) and stat(B) have the same device and
-///               inode (or equivalent).
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code equivalent(const Twine &A, const Twine &B, bool &result);
-
-/// Simpler version of equivalent for clients that don't need to
-///        differentiate between an error and false.
-inline bool equivalent(const Twine &A, const Twine &B) {
-  bool result;
-  return !equivalent(A, B, result) && result;
-}
-
-/// Does status represent a directory?
-///
-/// @param Path The path to get the type of.
-/// @param Follow For symbolic links, indicates whether to return the file type
-///               of the link itself, or of the target.
-/// @returns A value from the file_type enumeration indicating the type of file.
-file_type get_file_type(const Twine &Path, bool Follow = true);
-
-/// Does status represent a directory?
-///
-/// @param status A basic_file_status previously returned from status.
-/// @returns status.type() == file_type::directory_file.
-bool is_directory(const basic_file_status &status);
-
-/// Is path a directory?
-///
-/// @param path Input path.
-/// @param result Set to true if \a path is a directory (after following
-///               symlinks, false if it is not. Undefined otherwise.
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code is_directory(const Twine &path, bool &result);
-
-/// Simpler version of is_directory for clients that don't need to
-///        differentiate between an error and false.
-inline bool is_directory(const Twine &Path) {
-  bool Result;
-  return !is_directory(Path, Result) && Result;
-}
-
-/// Does status represent a regular file?
-///
-/// @param status A basic_file_status previously returned from status.
-/// @returns status_known(status) && status.type() == file_type::regular_file.
-bool is_regular_file(const basic_file_status &status);
-
-/// Is path a regular file?
-///
-/// @param path Input path.
-/// @param result Set to true if \a path is a regular file (after following
-///               symlinks), false if it is not. Undefined otherwise.
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code is_regular_file(const Twine &path, bool &result);
-
-/// Simpler version of is_regular_file for clients that don't need to
-///        differentiate between an error and false.
-inline bool is_regular_file(const Twine &Path) {
-  bool Result;
-  if (is_regular_file(Path, Result))
-    return false;
-  return Result;
-}
-
-/// Does status represent a symlink file?
-///
-/// @param status A basic_file_status previously returned from status.
-/// @returns status_known(status) && status.type() == file_type::symlink_file.
-bool is_symlink_file(const basic_file_status &status);
-
-/// Is path a symlink file?
-///
-/// @param path Input path.
-/// @param result Set to true if \a path is a symlink file, false if it is not.
-///               Undefined otherwise.
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code is_symlink_file(const Twine &path, bool &result);
-
-/// Simpler version of is_symlink_file for clients that don't need to
-///        differentiate between an error and false.
-inline bool is_symlink_file(const Twine &Path) {
-  bool Result;
-  if (is_symlink_file(Path, Result))
-    return false;
-  return Result;
-}
-
-/// Does this status represent something that exists but is not a
-///        directory or regular file?
-///
-/// @param status A basic_file_status previously returned from status.
-/// @returns exists(s) && !is_regular_file(s) && !is_directory(s)
-bool is_other(const basic_file_status &status);
-
-/// Is path something that exists but is not a directory,
-///        regular file, or symlink?
-///
-/// @param path Input path.
-/// @param result Set to true if \a path exists, but is not a directory, regular
-///               file, or a symlink, false if it does not. Undefined otherwise.
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code is_other(const Twine &path, bool &result);
-
-/// Get file status as if by POSIX stat().
-///
-/// @param path Input path.
-/// @param result Set to the file status.
-/// @param follow When true, follows symlinks.  Otherwise, the symlink itself is
-///               statted.
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code status(const Twine &path, file_status &result,
-                       bool follow = true);
-
-/// A version for when a file descriptor is already available.
-std::error_code status(int FD, file_status &Result);
-
-/// Is status available?
-///
-/// @param s Input file status.
-/// @returns True if status() != status_error.
-bool status_known(const basic_file_status &s);
-
-/// Is status available?
-///
-/// @param path Input path.
-/// @param result Set to true if status() != status_error.
-/// @returns errc::success if result has been successfully set, otherwise a
-///          platform-specific error_code.
-std::error_code status_known(const Twine &path, bool &result);
-
-enum CreationDisposition : unsigned {
-  /// CD_CreateAlways - When opening a file:
-  ///   * If it already exists, truncate it.
-  ///   * If it does not already exist, create a new file.
-  CD_CreateAlways = 0,
-
-  /// CD_CreateNew - When opening a file:
-  ///   * If it already exists, fail.
-  ///   * If it does not already exist, create a new file.
-  CD_CreateNew = 1,
-
-  /// CD_OpenExisting - When opening a file:
-  ///   * If it already exists, open the file with the offset set to 0.
-  ///   * If it does not already exist, fail.
-  CD_OpenExisting = 2,
-
-  /// CD_OpenAlways - When opening a file:
-  ///   * If it already exists, open the file with the offset set to 0.
-  ///   * If it does not already exist, create a new file.
-  CD_OpenAlways = 3,
-};
-
-enum FileAccess : unsigned {
-  FA_Read = 1,
-  FA_Write = 2,
-};
-
-enum OpenFlags : unsigned {
-  OF_None = 0,
-  F_None = 0, // For compatibility
-
-  /// The file should be opened in text mode on platforms that make this
-  /// distinction.
-  OF_Text = 1,
-  F_Text = 1, // For compatibility
-
-  /// The file should be opened in append mode.
-  OF_Append = 2,
-  F_Append = 2, // For compatibility
-
-  /// Delete the file on close. Only makes a difference on windows.
-  OF_Delete = 4,
-
-  /// When a child process is launched, this file should remain open in the
-  /// child process.
-  OF_ChildInherit = 8,
-
-  /// Force files Atime to be updated on access. Only makes a difference on windows.
-  OF_UpdateAtime = 16,
-};
-
-inline OpenFlags operator|(OpenFlags A, OpenFlags B) {
-  return OpenFlags(unsigned(A) | unsigned(B));
-}
-
-inline OpenFlags &operator|=(OpenFlags &A, OpenFlags B) {
-  A = A | B;
-  return A;
-}
-
-inline FileAccess operator|(FileAccess A, FileAccess B) {
-  return FileAccess(unsigned(A) | unsigned(B));
-}
-
-inline FileAccess &operator|=(FileAccess &A, FileAccess B) {
-  A = A | B;
-  return A;
-}
-
-/// @brief Opens a file with the specified creation disposition, access mode,
-/// and flags and returns a file descriptor.
-///
-/// The caller is responsible for closing the file descriptor once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param ResultFD If the file could be opened successfully, its descriptor
-///                 is stored in this location. Otherwise, this is set to -1.
-/// @param Disp Value specifying the existing-file behavior.
-/// @param Access Value specifying whether to open the file in read, write, or
-///               read-write mode.
-/// @param Flags Additional flags.
-/// @param Mode The access permissions of the file, represented in octal.
-/// @returns errc::success if \a Name has been opened, otherwise a
-///          platform-specific error_code.
-std::error_code openFile(const Twine &Name, int &ResultFD,
-                         CreationDisposition Disp, FileAccess Access,
-                         OpenFlags Flags, unsigned Mode = 0666);
-
-/// @brief Opens a file with the specified creation disposition, access mode,
-/// and flags and returns a platform-specific file object.
-///
-/// The caller is responsible for closing the file object once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param Disp Value specifying the existing-file behavior.
-/// @param Access Value specifying whether to open the file in read, write, or
-///               read-write mode.
-/// @param Flags Additional flags.
-/// @param Mode The access permissions of the file, represented in octal.
-/// @returns errc::success if \a Name has been opened, otherwise a
-///          platform-specific error_code.
-Expected<file_t> openNativeFile(const Twine &Name, CreationDisposition Disp,
-                                FileAccess Access, OpenFlags Flags,
-                                unsigned Mode = 0666);
-
-/// @brief Opens the file with the given name in a write-only or read-write
-/// mode, returning its open file descriptor. If the file does not exist, it
-/// is created.
-///
-/// The caller is responsible for closing the file descriptor once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param ResultFD If the file could be opened successfully, its descriptor
-///                 is stored in this location. Otherwise, this is set to -1.
-/// @param Flags Additional flags used to determine whether the file should be
-///              opened in, for example, read-write or in write-only mode.
-/// @param Mode The access permissions of the file, represented in octal.
-/// @returns errc::success if \a Name has been opened, otherwise a
-///          platform-specific error_code.
-inline std::error_code
-openFileForWrite(const Twine &Name, int &ResultFD,
-                 CreationDisposition Disp = CD_CreateAlways,
-                 OpenFlags Flags = OF_None, unsigned Mode = 0666) {
-  return openFile(Name, ResultFD, Disp, FA_Write, Flags, Mode);
-}
-
-/// @brief Opens the file with the given name in a write-only or read-write
-/// mode, returning its open file descriptor. If the file does not exist, it
-/// is created.
-///
-/// The caller is responsible for closing the freeing the file once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param Flags Additional flags used to determine whether the file should be
-///              opened in, for example, read-write or in write-only mode.
-/// @param Mode The access permissions of the file, represented in octal.
-/// @returns a platform-specific file descriptor if \a Name has been opened,
-///          otherwise an error object.
-inline Expected<file_t> openNativeFileForWrite(const Twine &Name,
-                                               CreationDisposition Disp,
-                                               OpenFlags Flags,
-                                               unsigned Mode = 0666) {
-  return openNativeFile(Name, Disp, FA_Write, Flags, Mode);
-}
-
-/// @brief Opens the file with the given name in a write-only or read-write
-/// mode, returning its open file descriptor. If the file does not exist, it
-/// is created.
-///
-/// The caller is responsible for closing the file descriptor once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param ResultFD If the file could be opened successfully, its descriptor
-///                 is stored in this location. Otherwise, this is set to -1.
-/// @param Flags Additional flags used to determine whether the file should be
-///              opened in, for example, read-write or in write-only mode.
-/// @param Mode The access permissions of the file, represented in octal.
-/// @returns errc::success if \a Name has been opened, otherwise a
-///          platform-specific error_code.
-inline std::error_code openFileForReadWrite(const Twine &Name, int &ResultFD,
-                                            CreationDisposition Disp,
-                                            OpenFlags Flags,
-                                            unsigned Mode = 0666) {
-  return openFile(Name, ResultFD, Disp, FA_Write | FA_Read, Flags, Mode);
-}
-
-/// @brief Opens the file with the given name in a write-only or read-write
-/// mode, returning its open file descriptor. If the file does not exist, it
-/// is created.
-///
-/// The caller is responsible for closing the freeing the file once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param Flags Additional flags used to determine whether the file should be
-///              opened in, for example, read-write or in write-only mode.
-/// @param Mode The access permissions of the file, represented in octal.
-/// @returns a platform-specific file descriptor if \a Name has been opened,
-///          otherwise an error object.
-inline Expected<file_t> openNativeFileForReadWrite(const Twine &Name,
-                                                   CreationDisposition Disp,
-                                                   OpenFlags Flags,
-                                                   unsigned Mode = 0666) {
-  return openNativeFile(Name, Disp, FA_Write | FA_Read, Flags, Mode);
-}
-
-/// @brief Opens the file with the given name in a read-only mode, returning
-/// its open file descriptor.
-///
-/// The caller is responsible for closing the file descriptor once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param ResultFD If the file could be opened successfully, its descriptor
-///                 is stored in this location. Otherwise, this is set to -1.
-/// @param RealPath If nonnull, extra work is done to determine the real path
-///                 of the opened file, and that path is stored in this
-///                 location.
-/// @returns errc::success if \a Name has been opened, otherwise a
-///          platform-specific error_code.
-std::error_code openFileForRead(const Twine &Name, int &ResultFD,
-                                OpenFlags Flags = OF_None,
-                                SmallVectorImpl<char> *RealPath = nullptr);
-
-/// @brief Opens the file with the given name in a read-only mode, returning
-/// its open file descriptor.
-///
-/// The caller is responsible for closing the freeing the file once they are
-/// finished with it.
-///
-/// @param Name The path of the file to open, relative or absolute.
-/// @param RealPath If nonnull, extra work is done to determine the real path
-///                 of the opened file, and that path is stored in this
-///                 location.
-/// @returns a platform-specific file descriptor if \a Name has been opened,
-///          otherwise an error object.
-Expected<file_t>
-openNativeFileForRead(const Twine &Name, OpenFlags Flags = OF_None,
-                      SmallVectorImpl<char> *RealPath = nullptr);
-
-/// @brief Close the file object.  This should be used instead of ::close for
-/// portability.
-///
-/// @param F On input, this is the file to close.  On output, the file is
-/// set to kInvalidFile.
-void closeFile(file_t &F);
-
-std::error_code getUniqueID(const Twine Path, UniqueID &Result);
-
-/// This class represents a memory mapped file. It is based on
-/// boost::iostreams::mapped_file.
-class mapped_file_region {
-public:
-  enum mapmode {
-    readonly, ///< May only access map via const_data as read only.
-    readwrite, ///< May access map via data and modify it. Written to path.
-    priv ///< May modify via data, but changes are lost on destruction.
-  };
-
-private:
-  /// Platform-specific mapping state.
-  size_t Size;
-  void *Mapping;
-#ifdef _WIN32
-  void *FileHandle;
-#endif
-  mapmode Mode;
-
-  std::error_code init(int FD, uint64_t Offset, mapmode Mode);
-
-public:
-  mapped_file_region() = delete;
-  mapped_file_region(mapped_file_region&) = delete;
-  mapped_file_region &operator =(mapped_file_region&) = delete;
-
-  /// \param fd An open file descriptor to map. mapped_file_region takes
-  ///   ownership if closefd is true. It must have been opended in the correct
-  ///   mode.
-  mapped_file_region(int fd, mapmode mode, size_t length, uint64_t offset,
-                     std::error_code &ec);
-
-  ~mapped_file_region();
-
-  size_t size() const;
-  char *data() const;
-
-  /// Get a const view of the data. Modifying this memory has undefined
-  /// behavior.
-  const char *const_data() const;
-
-  /// \returns The minimum alignment offset must be.
-  static int alignment();
-};
-
-/// @}
-/// @name Iterators
-/// @{
-
-/// directory_entry - A single entry in a directory. Caches the status either
-/// from the result of the iteration syscall, or the first time status is
-/// called.
-class directory_entry {
-  std::string Path;
-  file_type Type;           // Most platforms can provide this.
-  bool FollowSymlinks;      // Affects the behavior of status().
-  basic_file_status Status; // If available.
-
-public:
-  explicit directory_entry(const Twine &Path, bool FollowSymlinks = true,
-                           file_type Type = file_type::type_unknown,
-                           basic_file_status Status = basic_file_status())
-      : Path(Path.str()), Type(Type), FollowSymlinks(FollowSymlinks),
-        Status(Status) {}
-
-  directory_entry() = default;
-
-  void replace_filename(const Twine &Filename, file_type Type,
-                        basic_file_status Status = basic_file_status());
-
-  const std::string &path() const { return Path; }
-  ErrorOr<basic_file_status> status() const;
-  file_type type() const {
-    if (Type != file_type::type_unknown)
-      return Type;
-    auto S = status();
-    return S ? S->type() : file_type::type_unknown;
-  }
-
-  bool operator==(const directory_entry& RHS) const { return Path == RHS.Path; }
-  bool operator!=(const directory_entry& RHS) const { return !(*this == RHS); }
-  bool operator< (const directory_entry& RHS) const;
-  bool operator<=(const directory_entry& RHS) const;
-  bool operator> (const directory_entry& RHS) const;
-  bool operator>=(const directory_entry& RHS) const;
-};
-
-namespace detail {
-
-  struct DirIterState;
-
-  std::error_code directory_iterator_construct(DirIterState &, StringRef, bool);
-  std::error_code directory_iterator_increment(DirIterState &);
-  std::error_code directory_iterator_destruct(DirIterState &);
-
-  /// Keeps state for the directory_iterator.
-  struct DirIterState {
-    ~DirIterState() {
-      directory_iterator_destruct(*this);
-    }
-
-    intptr_t IterationHandle = 0;
-    directory_entry CurrentEntry;
-  };
-
-} // end namespace detail
-
-/// directory_iterator - Iterates through the entries in path. There is no
-/// operator++ because we need an error_code. If it's really needed we can make
-/// it call report_fatal_error on error.
-class directory_iterator {
-  std::shared_ptr<detail::DirIterState> State;
-  bool FollowSymlinks = true;
-
-public:
-  explicit directory_iterator(const Twine &path, std::error_code &ec,
-                              bool follow_symlinks = true)
-      : FollowSymlinks(follow_symlinks) {
-    State = std::make_shared<detail::DirIterState>();
-    SmallString<128> path_storage;
-    ec = detail::directory_iterator_construct(
-        *State, path.toStringRef(path_storage), FollowSymlinks);
-  }
-
-  explicit directory_iterator(const directory_entry &de, std::error_code &ec,
-                              bool follow_symlinks = true)
-      : FollowSymlinks(follow_symlinks) {
-    State = std::make_shared<detail::DirIterState>();
-    ec = detail::directory_iterator_construct(
-        *State, de.path(), FollowSymlinks);
-  }
-
-  /// Construct end iterator.
-  directory_iterator() = default;
-
-  // No operator++ because we need error_code.
-  directory_iterator &increment(std::error_code &ec) {
-    ec = directory_iterator_increment(*State);
-    return *this;
-  }
-
-  const directory_entry &operator*() const { return State->CurrentEntry; }
-  const directory_entry *operator->() const { return &State->CurrentEntry; }
-
-  bool operator==(const directory_iterator &RHS) const {
-    if (State == RHS.State)
-      return true;
-    if (!RHS.State)
-      return State->CurrentEntry == directory_entry();
-    if (!State)
-      return RHS.State->CurrentEntry == directory_entry();
-    return State->CurrentEntry == RHS.State->CurrentEntry;
-  }
-
-  bool operator!=(const directory_iterator &RHS) const {
-    return !(*this == RHS);
-  }
-};
-
-namespace detail {
-
-  /// Keeps state for the recursive_directory_iterator.
-  struct RecDirIterState {
-    std::stack<directory_iterator, std::vector<directory_iterator>> Stack;
-    uint16_t Level = 0;
-    bool HasNoPushRequest = false;
-  };
-
-} // end namespace detail
-
-/// recursive_directory_iterator - Same as directory_iterator except for it
-/// recurses down into child directories.
-class recursive_directory_iterator {
-  std::shared_ptr<detail::RecDirIterState> State;
-  bool Follow;
-
-public:
-  recursive_directory_iterator() = default;
-  explicit recursive_directory_iterator(const Twine &path, std::error_code &ec,
-                                        bool follow_symlinks = true)
-      : State(std::make_shared<detail::RecDirIterState>()),
-        Follow(follow_symlinks) {
-    State->Stack.push(directory_iterator(path, ec, Follow));
-    if (State->Stack.top() == directory_iterator())
-      State.reset();
-  }
-
-  // No operator++ because we need error_code.
-  recursive_directory_iterator &increment(std::error_code &ec) {
-    const directory_iterator end_itr = {};
-
-    if (State->HasNoPushRequest)
-      State->HasNoPushRequest = false;
-    else {
-      file_type type = State->Stack.top()->type();
-      if (type == file_type::symlink_file && Follow) {
-        // Resolve the symlink: is it a directory to recurse into?
-        ErrorOr<basic_file_status> status = State->Stack.top()->status();
-        if (status)
-          type = status->type();
-        // Otherwise broken symlink, and we'll continue.
-      }
-      if (type == file_type::directory_file) {
-        State->Stack.push(directory_iterator(*State->Stack.top(), ec, Follow));
-        if (State->Stack.top() != end_itr) {
-          ++State->Level;
-          return *this;
-        }
-        State->Stack.pop();
-      }
-    }
-
-    while (!State->Stack.empty()
-           && State->Stack.top().increment(ec) == end_itr) {
-      State->Stack.pop();
-      --State->Level;
-    }
-
-    // Check if we are done. If so, create an end iterator.
-    if (State->Stack.empty())
-      State.reset();
-
-    return *this;
-  }
-
-  const directory_entry &operator*() const { return *State->Stack.top(); }
-  const directory_entry *operator->() const { return &*State->Stack.top(); }
-
-  // observers
-  /// Gets the current level. Starting path is at level 0.
-  int level() const { return State->Level; }
-
-  /// Returns true if no_push has been called for this directory_entry.
-  bool no_push_request() const { return State->HasNoPushRequest; }
-
-  // modifiers
-  /// Goes up one level if Level > 0.
-  void pop() {
-    assert(State && "Cannot pop an end iterator!");
-    assert(State->Level > 0 && "Cannot pop an iterator with level < 1");
-
-    const directory_iterator end_itr = {};
-    std::error_code ec;
-    do {
-      if (ec) {
-        //report_fatal_error("Error incrementing directory iterator.");
-        while (!State->Stack.empty()) State->Stack.pop();
-        break;
-      }
-      State->Stack.pop();
-      --State->Level;
-    } while (!State->Stack.empty()
-             && State->Stack.top().increment(ec) == end_itr);
-
-    // Check if we are done. If so, create an end iterator.
-    if (State->Stack.empty())
-      State.reset();
-  }
-
-  /// Does not go down into the current directory_entry.
-  void no_push() { State->HasNoPushRequest = true; }
-
-  bool operator==(const recursive_directory_iterator &RHS) const {
-    return State == RHS.State;
-  }
-
-  bool operator!=(const recursive_directory_iterator &RHS) const {
-    return !(*this == RHS);
-  }
-};
-
-/// @}
-
-} // end namespace fs
-} // end namespace sys
-} // end namespace wpi
-
-#endif // LLVM_SUPPORT_FILESYSTEM_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Format.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Format.h
deleted file mode 100644
index cb8b26b..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Format.h
+++ /dev/null
@@ -1,265 +0,0 @@
-//===- Format.h - Efficient printf-style formatting for streams -*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file implements the format() function, which can be used with other
-// LLVM subsystems to provide printf-style formatting.  This gives all the power
-// and risk of printf.  This can be used like this (with raw_ostreams as an
-// example):
-//
-//    OS << "mynumber: " << format("%4.5f", 1234.412) << '\n';
-//
-// Or if you prefer:
-//
-//  OS << format("mynumber: %4.5f\n", 1234.412);
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_FORMAT_H
-#define WPIUTIL_WPI_FORMAT_H
-
-#include "wpi/ArrayRef.h"
-#include "wpi/StringRef.h"
-#include <cassert>
-#include <cstdint>
-#include <cstdio>
-#include <optional>
-#include <tuple>
-#include <utility>
-
-namespace wpi {
-
-/// This is a helper class used for handling formatted output.  It is the
-/// abstract base class of a templated derived class.
-class format_object_base {
-protected:
-  const char *Fmt;
-  ~format_object_base() = default; // Disallow polymorphic deletion.
-  format_object_base(const format_object_base &) = default;
-  virtual void home(); // Out of line virtual method.
-
-  /// Call snprintf() for this object, on the given buffer and size.
-  virtual int snprint(char *Buffer, unsigned BufferSize) const = 0;
-
-public:
-  format_object_base(const char *fmt) : Fmt(fmt) {}
-
-  /// Format the object into the specified buffer.  On success, this returns
-  /// the length of the formatted string.  If the buffer is too small, this
-  /// returns a length to retry with, which will be larger than BufferSize.
-  unsigned print(char *Buffer, unsigned BufferSize) const {
-    assert(BufferSize && "Invalid buffer size!");
-
-    // Print the string, leaving room for the terminating null.
-    int N = snprint(Buffer, BufferSize);
-
-    // VC++ and old GlibC return negative on overflow, just double the size.
-    if (N < 0)
-      return BufferSize * 2;
-
-    // Other implementations yield number of bytes needed, not including the
-    // final '\0'.
-    if (unsigned(N) >= BufferSize)
-      return N + 1;
-
-    // Otherwise N is the length of output (not including the final '\0').
-    return N;
-  }
-};
-
-/// These are templated helper classes used by the format function that
-/// capture the object to be formatted and the format string. When actually
-/// printed, this synthesizes the string into a temporary buffer provided and
-/// returns whether or not it is big enough.
-
-// Helper to validate that format() parameters are scalars or pointers.
-template <typename... Args> struct validate_format_parameters;
-template <typename Arg, typename... Args>
-struct validate_format_parameters<Arg, Args...> {
-  static_assert(std::is_scalar<Arg>::value,
-                "format can't be used with non fundamental / non pointer type");
-  validate_format_parameters() { validate_format_parameters<Args...>(); }
-};
-template <> struct validate_format_parameters<> {};
-
-template <typename... Ts>
-class format_object final : public format_object_base {
-  std::tuple<Ts...> Vals;
-
-  template <std::size_t... Is>
-  int snprint_tuple(char *Buffer, unsigned BufferSize,
-                    std::index_sequence<Is...>) const {
-#ifdef _MSC_VER
-    return _snprintf_s(Buffer, BufferSize, BufferSize, Fmt, std::get<Is>(Vals)...);
-#else
-#ifdef __GNUC__
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wformat-nonliteral"
-#endif
-    return snprintf(Buffer, BufferSize, Fmt, std::get<Is>(Vals)...);
-#ifdef __GNUC__
-#pragma GCC diagnostic pop
-#endif
-#endif
-  }
-
-public:
-  format_object(const char *fmt, const Ts &... vals)
-      : format_object_base(fmt), Vals(vals...) {
-    validate_format_parameters<Ts...>();
-  }
-
-  int snprint(char *Buffer, unsigned BufferSize) const override {
-    return snprint_tuple(Buffer, BufferSize, std::index_sequence_for<Ts...>());
-  }
-};
-
-/// These are helper functions used to produce formatted output.  They use
-/// template type deduction to construct the appropriate instance of the
-/// format_object class to simplify their construction.
-///
-/// This is typically used like:
-/// \code
-///   OS << format("%0.4f", myfloat) << '\n';
-/// \endcode
-
-template <typename... Ts>
-inline format_object<Ts...> format(const char *Fmt, const Ts &... Vals) {
-  return format_object<Ts...>(Fmt, Vals...);
-}
-
-/// This is a helper class for left_justify, right_justify, and center_justify.
-class FormattedString {
-public:
-  enum Justification { JustifyNone, JustifyLeft, JustifyRight, JustifyCenter };
-  FormattedString(StringRef S, unsigned W, Justification J)
-      : Str(S), Width(W), Justify(J) {}
-
-private:
-  StringRef Str;
-  unsigned Width;
-  Justification Justify;
-  friend class raw_ostream;
-};
-
-/// left_justify - append spaces after string so total output is
-/// \p Width characters.  If \p Str is larger that \p Width, full string
-/// is written with no padding.
-inline FormattedString left_justify(StringRef Str, unsigned Width) {
-  return FormattedString(Str, Width, FormattedString::JustifyLeft);
-}
-
-/// right_justify - add spaces before string so total output is
-/// \p Width characters.  If \p Str is larger that \p Width, full string
-/// is written with no padding.
-inline FormattedString right_justify(StringRef Str, unsigned Width) {
-  return FormattedString(Str, Width, FormattedString::JustifyRight);
-}
-
-/// center_justify - add spaces before and after string so total output is
-/// \p Width characters.  If \p Str is larger that \p Width, full string
-/// is written with no padding.
-inline FormattedString center_justify(StringRef Str, unsigned Width) {
-  return FormattedString(Str, Width, FormattedString::JustifyCenter);
-}
-
-/// This is a helper class used for format_hex() and format_decimal().
-class FormattedNumber {
-  uint64_t HexValue;
-  int64_t DecValue;
-  unsigned Width;
-  bool Hex;
-  bool Upper;
-  bool HexPrefix;
-  friend class raw_ostream;
-
-public:
-  FormattedNumber(uint64_t HV, int64_t DV, unsigned W, bool H, bool U,
-                  bool Prefix)
-      : HexValue(HV), DecValue(DV), Width(W), Hex(H), Upper(U),
-        HexPrefix(Prefix) {}
-};
-
-/// format_hex - Output \p N as a fixed width hexadecimal. If number will not
-/// fit in width, full number is still printed.  Examples:
-///   OS << format_hex(255, 4)              => 0xff
-///   OS << format_hex(255, 4, true)        => 0xFF
-///   OS << format_hex(255, 6)              => 0x00ff
-///   OS << format_hex(255, 2)              => 0xff
-inline FormattedNumber format_hex(uint64_t N, unsigned Width,
-                                  bool Upper = false) {
-  assert(Width <= 18 && "hex width must be <= 18");
-  return FormattedNumber(N, 0, Width, true, Upper, true);
-}
-
-/// format_hex_no_prefix - Output \p N as a fixed width hexadecimal. Does not
-/// prepend '0x' to the outputted string.  If number will not fit in width,
-/// full number is still printed.  Examples:
-///   OS << format_hex_no_prefix(255, 2)              => ff
-///   OS << format_hex_no_prefix(255, 2, true)        => FF
-///   OS << format_hex_no_prefix(255, 4)              => 00ff
-///   OS << format_hex_no_prefix(255, 1)              => ff
-inline FormattedNumber format_hex_no_prefix(uint64_t N, unsigned Width,
-                                            bool Upper = false) {
-  assert(Width <= 16 && "hex width must be <= 16");
-  return FormattedNumber(N, 0, Width, true, Upper, false);
-}
-
-/// format_decimal - Output \p N as a right justified, fixed-width decimal. If
-/// number will not fit in width, full number is still printed.  Examples:
-///   OS << format_decimal(0, 5)     => "    0"
-///   OS << format_decimal(255, 5)   => "  255"
-///   OS << format_decimal(-1, 3)    => " -1"
-///   OS << format_decimal(12345, 3) => "12345"
-inline FormattedNumber format_decimal(int64_t N, unsigned Width) {
-  return FormattedNumber(0, N, Width, false, false, false);
-}
-
-class FormattedBytes {
-  ArrayRef<uint8_t> Bytes;
-
-  // If not nullopt, display offsets for each line relative to starting value.
-  std::optional<uint64_t> FirstByteOffset;
-  uint32_t IndentLevel;  // Number of characters to indent each line.
-  uint32_t NumPerLine;   // Number of bytes to show per line.
-  uint8_t ByteGroupSize; // How many hex bytes are grouped without spaces
-  bool Upper;            // Show offset and hex bytes as upper case.
-  bool ASCII;            // Show the ASCII bytes for the hex bytes to the right.
-  friend class raw_ostream;
-
-public:
-  FormattedBytes(ArrayRef<uint8_t> B, uint32_t IL, std::optional<uint64_t> O,
-                 uint32_t NPL, uint8_t BGS, bool U, bool A)
-      : Bytes(B), FirstByteOffset(O), IndentLevel(IL), NumPerLine(NPL),
-        ByteGroupSize(BGS), Upper(U), ASCII(A) {
-
-    if (ByteGroupSize > NumPerLine)
-      ByteGroupSize = NumPerLine;
-  }
-};
-
-inline FormattedBytes
-format_bytes(ArrayRef<uint8_t> Bytes, std::optional<uint64_t> FirstByteOffset = std::nullopt,
-             uint32_t NumPerLine = 16, uint8_t ByteGroupSize = 4,
-             uint32_t IndentLevel = 0, bool Upper = false) {
-  return FormattedBytes(Bytes, IndentLevel, FirstByteOffset, NumPerLine,
-                        ByteGroupSize, Upper, false);
-}
-
-inline FormattedBytes
-format_bytes_with_ascii(ArrayRef<uint8_t> Bytes,
-                        std::optional<uint64_t> FirstByteOffset = std::nullopt,
-                        uint32_t NumPerLine = 16, uint8_t ByteGroupSize = 4,
-                        uint32_t IndentLevel = 0, bool Upper = false) {
-  return FormattedBytes(Bytes, IndentLevel, FirstByteOffset, NumPerLine,
-                        ByteGroupSize, Upper, true);
-}
-
-} // end namespace wpi
-
-#endif
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FunctionExtras.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FunctionExtras.h
index 8e45cda..57bc8dc 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FunctionExtras.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/FunctionExtras.h
@@ -37,6 +37,7 @@
 #include "wpi/PointerIntPair.h"
 #include "wpi/PointerUnion.h"
 #include <memory>
+#include <type_traits>
 
 namespace wpi {
 
@@ -237,7 +238,11 @@
     return *this;
   }
 
-  template <typename CallableT> unique_function(CallableT Callable) {
+  template <typename CallableT>
+  unique_function(CallableT Callable,
+                  std::enable_if_t<
+                    std::is_invocable_r_v<
+                      ReturnT, CallableT, ParamTs...>>* = nullptr) {
     bool IsInlineStorage = true;
     void *CallableAddr = getInlineStorage();
     if (sizeof(CallableT) > InlineStorageSize ||
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Hashing.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Hashing.h
index d59d695..e58bfdd 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Hashing.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Hashing.h
@@ -53,6 +53,7 @@
 #include <cassert>
 #include <cstring>
 #include <string>
+#include <string_view>
 #include <utility>
 
 #ifdef _WIN32
@@ -660,6 +661,11 @@
   return hash_combine_range(arg.begin(), arg.end());
 }
 
+template <typename T>
+hash_code hash_value(const std::basic_string_view<T> &arg) {
+  return hash_combine_range(arg.begin(), arg.end());
+}
+
 } // namespace wpi
 
 #ifdef _WIN32
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpParser.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpParser.h
index b8b39de..72e5dad 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpParser.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpParser.h
@@ -1,18 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_HTTPPARSER_H_
 #define WPIUTIL_WPI_HTTPPARSER_H_
 
 #include <stdint.h>
 
+#include <string_view>
+
 #include "wpi/Signal.h"
 #include "wpi/SmallString.h"
-#include "wpi/StringRef.h"
 #include "wpi/http_parser.h"
 
 namespace wpi {
@@ -61,9 +59,10 @@
    * @param in input data
    * @return Trailing input data after the parse.
    */
-  StringRef Execute(StringRef in) {
-    return in.drop_front(
+  std::string_view Execute(std::string_view in) {
+    in.remove_prefix(
         http_parser_execute(&m_parser, &m_settings, in.data(), in.size()));
+    return in;
   }
 
   /**
@@ -139,7 +138,7 @@
   /**
    * Get URL.  Valid in and after the url callback has been called.
    */
-  StringRef GetUrl() const { return m_urlBuf; }
+  std::string_view GetUrl() const { return m_urlBuf.str(); }
 
   /**
    * Message begin callback.
@@ -151,7 +150,7 @@
    *
    * The parameter to the callback is the complete URL string.
    */
-  sig::Signal<StringRef> url;
+  sig::Signal<std::string_view> url;
 
   /**
    * Status callback.
@@ -159,14 +158,14 @@
    * The parameter to the callback is the complete status string.
    * GetStatusCode() can be used to get the numeric status code.
    */
-  sig::Signal<StringRef> status;
+  sig::Signal<std::string_view> status;
 
   /**
    * Header field callback.
    *
    * The parameters to the callback are the field name and field value.
    */
-  sig::Signal<StringRef, StringRef> header;
+  sig::Signal<std::string_view, std::string_view> header;
 
   /**
    * Headers complete callback.
@@ -186,7 +185,7 @@
    * multiple times arbitrarily (e.g. it's possible that it may be called with
    * just a few characters at a time).
    */
-  sig::Signal<StringRef, bool> body;
+  sig::Signal<std::string_view, bool> body;
 
   /**
    * Headers complete callback.
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpServerConnection.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpServerConnection.h
index 17c9ca2..a4d4cf8 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpServerConnection.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpServerConnection.h
@@ -1,19 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_HTTPSERVERCONNECTION_H_
 #define WPIUTIL_WPI_HTTPSERVERCONNECTION_H_
 
 #include <memory>
+#include <string_view>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/HttpParser.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
+#include "wpi/span.h"
 #include "wpi/uv/Stream.h"
 
 namespace wpi {
@@ -39,7 +35,7 @@
    * Build common response headers.
    *
    * Called by SendHeader() to send headers common to every response.
-   * Each line must be terminated with \r\n.
+   * Each line must be terminated with "\r\n".
    *
    * The default implementation sends the following:
    * "Server: WebServer/1.0\r\n"
@@ -67,9 +63,9 @@
    *                      be set to false.
    * @param extra Extra HTTP headers to send, including final "\r\n"
    */
-  virtual void BuildHeader(raw_ostream& os, int code, const Twine& codeText,
-                           const Twine& contentType, uint64_t contentLength,
-                           const Twine& extra = Twine{});
+  virtual void BuildHeader(raw_ostream& os, int code, std::string_view codeText,
+                           std::string_view contentType, uint64_t contentLength,
+                           std::string_view extra = {});
 
   /**
    * Send data to client.
@@ -82,7 +78,7 @@
    *             is desired, call m_stream.Write() directly instead.
    * @param closeAfter close the connection after the write completes
    */
-  void SendData(ArrayRef<uv::Buffer> bufs, bool closeAfter = false);
+  void SendData(span<const uv::Buffer> bufs, bool closeAfter = false);
 
   /**
    * Send HTTP response, along with other header information like mimetype.
@@ -94,9 +90,10 @@
    * @param content Response message content
    * @param extraHeader Extra HTTP headers to send, including final "\r\n"
    */
-  virtual void SendResponse(int code, const Twine& codeText,
-                            const Twine& contentType, StringRef content,
-                            const Twine& extraHeader = Twine{});
+  virtual void SendResponse(int code, std::string_view codeText,
+                            std::string_view contentType,
+                            std::string_view content,
+                            std::string_view extraHeader = {});
 
   /**
    * Send HTTP response from static data, along with other header information
@@ -112,10 +109,10 @@
    * @param gzipped True if content is gzip compressed
    * @param extraHeader Extra HTTP headers to send, including final "\r\n"
    */
-  virtual void SendStaticResponse(int code, const Twine& codeText,
-                                  const Twine& contentType, StringRef content,
-                                  bool gzipped,
-                                  const Twine& extraHeader = Twine{});
+  virtual void SendStaticResponse(int code, std::string_view codeText,
+                                  std::string_view contentType,
+                                  std::string_view content, bool gzipped,
+                                  std::string_view extraHeader = {});
 
   /**
    * Send error header and message.
@@ -126,7 +123,7 @@
    * @param code HTTP error code (e.g. 404)
    * @param message Additional message text
    */
-  virtual void SendError(int code, const Twine& message = Twine{});
+  virtual void SendError(int code, std::string_view message = {});
 
   /** The HTTP request. */
   HttpParser m_request{HttpParser::kRequest};
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.h
index 75b6e58..b10d127 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_HTTPUTIL_H_
 #define WPIUTIL_WPI_HTTPUTIL_H_
@@ -12,19 +9,18 @@
 #include <memory>
 #include <optional>
 #include <string>
+#include <string_view>
 #include <utility>
 #include <vector>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/NetworkStream.h"
 #include "wpi/SmallString.h"
 #include "wpi/SmallVector.h"
 #include "wpi/StringMap.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
 #include "wpi/raw_istream.h"
 #include "wpi/raw_socket_istream.h"
 #include "wpi/raw_socket_ostream.h"
+#include "wpi/span.h"
 
 namespace wpi {
 
@@ -32,15 +28,15 @@
 // @param buf Buffer for output
 // @param error Set to true if an error occurred
 // @return Escaped string
-StringRef UnescapeURI(const Twine& str, SmallVectorImpl<char>& buf,
-                      bool* error);
+std::string_view UnescapeURI(std::string_view str, SmallVectorImpl<char>& buf,
+                             bool* error);
 
 // Escape a string with %xx-encoding.
 // @param buf Buffer for output
 // @param spacePlus If true, encodes spaces to '+' rather than "%20"
 // @return Escaped string
-StringRef EscapeURI(const Twine& str, SmallVectorImpl<char>& buf,
-                    bool spacePlus = true);
+std::string_view EscapeURI(std::string_view str, SmallVectorImpl<char>& buf,
+                           bool spacePlus = true);
 
 // Parse a set of HTTP headers.  Saves just the Content-Type and Content-Length
 // fields.
@@ -58,7 +54,7 @@
 // @param saveBuf If not null, all scanned characters up to but not including
 //     the boundary are saved to this string
 // @return False if error occurred on input stream, true if boundary found.
-bool FindMultipartBoundary(wpi::raw_istream& is, StringRef boundary,
+bool FindMultipartBoundary(wpi::raw_istream& is, std::string_view boundary,
                            std::string* saveBuf);
 
 /**
@@ -80,7 +76,7 @@
    *
    * @param query query string
    */
-  explicit HttpQueryMap(StringRef query);
+  explicit HttpQueryMap(std::string_view query);
 
   /**
    * Gets an element of the query string.  Both the name and the returned
@@ -91,11 +87,11 @@
    * @return Optional unescaped value.  Returns an empty optional if the
    *         name is not present in the query map.
    */
-  std::optional<StringRef> Get(StringRef name,
-                               SmallVectorImpl<char>& buf) const;
+  std::optional<std::string_view> Get(std::string_view name,
+                                      SmallVectorImpl<char>& buf) const;
 
  private:
-  StringMap<StringRef> m_elems;
+  StringMap<std::string_view> m_elems;
 };
 
 class HttpPathRef;
@@ -123,7 +119,7 @@
    * Constructs a HTTP path from an escaped path string.  Makes a copy of the
    * path, so it's safe to be a temporary.
    */
-  explicit HttpPath(StringRef path);
+  explicit HttpPath(std::string_view path);
 
   /**
    * Evaluates to true if the path is not empty.
@@ -146,11 +142,13 @@
    * @param match match list
    * @return True if path equals match list
    */
-  bool equals(std::initializer_list<StringRef> match) const {
-    return equals(0, makeArrayRef(match.begin(), match.end()));
+  bool equals(std::initializer_list<std::string_view> match) const {
+    return equals(0, {match.begin(), match.end()});
   }
-  bool equals(ArrayRef<StringRef> match) const { return equals(0, match); }
-  bool equals(StringRef match) const { return equals(0, makeArrayRef(match)); }
+  bool equals(span<const std::string_view> match) const {
+    return equals(0, match);
+  }
+  bool equals(std::string_view match) const { return equals(0, {match}); }
 
   /**
    * Returns true if the elements of the path starting at the "start" element
@@ -160,15 +158,18 @@
    * @param match match list
    * @return True if match
    */
-  bool equals(size_t start, std::initializer_list<StringRef> match) const {
-    return equals(start, makeArrayRef(match.begin(), match.end()));
+  bool equals(size_t start,
+              std::initializer_list<std::string_view> match) const {
+    return equals(start, {match.begin(), match.end()});
   }
-  bool equals(size_t start, ArrayRef<StringRef> match) const {
-    if (m_pathEnds.size() != (start + match.size())) return false;
+  bool equals(size_t start, span<const std::string_view> match) const {
+    if (m_pathEnds.size() != (start + match.size())) {
+      return false;
+    }
     return startswith(start, match);
   }
-  bool equals(size_t start, StringRef match) const {
-    return equals(start, makeArrayRef(match));
+  bool equals(size_t start, std::string_view match) const {
+    return equals(start, {match});
   }
 
   /**
@@ -178,14 +179,14 @@
    * @param match match list
    * @return True if path starts with match list
    */
-  bool startswith(std::initializer_list<StringRef> match) const {
-    return startswith(0, makeArrayRef(match.begin(), match.end()));
+  bool startswith(std::initializer_list<std::string_view> match) const {
+    return startswith(0, {match.begin(), match.end()});
   }
-  bool startswith(ArrayRef<StringRef> match) const {
+  bool startswith(span<const std::string_view> match) const {
     return startswith(0, match);
   }
-  bool startswith(StringRef match) const {
-    return startswith(0, makeArrayRef(match));
+  bool startswith(std::string_view match) const {
+    return startswith(0, {match});
   }
 
   /**
@@ -196,22 +197,21 @@
    * @param match match list
    * @return True if path starting at the start element matches the match list
    */
-  bool startswith(size_t start, std::initializer_list<StringRef> match) const {
-    return startswith(start, makeArrayRef(match.begin(), match.end()));
+  bool startswith(size_t start,
+                  std::initializer_list<std::string_view> match) const {
+    return startswith(start, {match.begin(), match.end()});
   }
 
-  bool startswith(size_t start, ArrayRef<StringRef> match) const;
+  bool startswith(size_t start, span<const std::string_view> match) const;
 
-  bool startswith(size_t start, StringRef match) const {
-    return startswith(start, makeArrayRef(match));
+  bool startswith(size_t start, std::string_view match) const {
+    return startswith(start, {match});
   }
 
   /**
    * Gets a single element of the path.
    */
-  StringRef operator[](size_t n) const {
-    return m_pathBuf.slice(n == 0 ? 0 : m_pathEnds[n - 1], m_pathEnds[n]);
-  }
+  std::string_view operator[](size_t n) const;
 
   /**
    * Returns a path reference with the first N elements of the path removed.
@@ -229,7 +229,7 @@
 class HttpPathRef {
  public:
   HttpPathRef() = default;
-  /*implicit*/ HttpPathRef(const HttpPath& path,  // NOLINT(runtime/explicit)
+  /*implicit*/ HttpPathRef(const HttpPath& path,  // NOLINT
                            size_t start = 0)
       : m_path(&path), m_start(start) {}
 
@@ -237,44 +237,48 @@
   bool empty() const { return m_path && m_path->size() == m_start; }
   size_t size() const { return m_path ? m_path->size() - m_start : 0; }
 
-  bool equals(std::initializer_list<StringRef> match) const {
-    return equals(0, makeArrayRef(match.begin(), match.end()));
+  bool equals(std::initializer_list<std::string_view> match) const {
+    return equals(0, {match.begin(), match.end()});
   }
-  bool equals(ArrayRef<StringRef> match) const { return equals(0, match); }
-  bool equals(StringRef match) const { return equals(0, makeArrayRef(match)); }
+  bool equals(span<const std::string_view> match) const {
+    return equals(0, match);
+  }
+  bool equals(std::string_view match) const { return equals(0, {match}); }
 
-  bool equals(size_t start, std::initializer_list<StringRef> match) const {
-    return equals(start, makeArrayRef(match.begin(), match.end()));
+  bool equals(size_t start,
+              std::initializer_list<std::string_view> match) const {
+    return equals(start, {match.begin(), match.end()});
   }
-  bool equals(size_t start, ArrayRef<StringRef> match) const {
+  bool equals(size_t start, span<const std::string_view> match) const {
     return m_path ? m_path->equals(m_start + start, match) : false;
   }
-  bool equals(size_t start, StringRef match) const {
-    return equals(start, makeArrayRef(match));
+  bool equals(size_t start, std::string_view match) const {
+    return equals(start, {match});
   }
 
-  bool startswith(std::initializer_list<StringRef> match) const {
-    return startswith(0, makeArrayRef(match.begin(), match.end()));
+  bool startswith(std::initializer_list<std::string_view> match) const {
+    return startswith(0, {match.begin(), match.end()});
   }
-  bool startswith(ArrayRef<StringRef> match) const {
+  bool startswith(span<const std::string_view> match) const {
     return startswith(0, match);
   }
-  bool startswith(StringRef match) const {
-    return startswith(0, makeArrayRef(match));
+  bool startswith(std::string_view match) const {
+    return startswith(0, {match});
   }
 
-  bool startswith(size_t start, std::initializer_list<StringRef> match) const {
-    return startswith(start, makeArrayRef(match.begin(), match.end()));
+  bool startswith(size_t start,
+                  std::initializer_list<std::string_view> match) const {
+    return startswith(start, {match.begin(), match.end()});
   }
-  bool startswith(size_t start, ArrayRef<StringRef> match) const {
+  bool startswith(size_t start, span<const std::string_view> match) const {
     return m_path ? m_path->startswith(m_start + start, match) : false;
   }
-  bool startswith(size_t start, StringRef match) const {
-    return startswith(start, makeArrayRef(match));
+  bool startswith(size_t start, std::string_view match) const {
+    return startswith(start, {match});
   }
 
-  StringRef operator[](size_t n) const {
-    return m_path ? m_path->operator[](m_start + n) : StringRef{};
+  std::string_view operator[](size_t n) const {
+    return m_path ? m_path->operator[](m_start + n) : std::string_view{};
   }
   HttpPathRef drop_front(size_t n) const {
     return m_path ? m_path->drop_front(m_start + n) : HttpPathRef{};
@@ -288,7 +292,7 @@
 class HttpLocation {
  public:
   HttpLocation() = default;
-  HttpLocation(const Twine& url_, bool* error, std::string* errorMsg);
+  HttpLocation(std::string_view url_, bool* error, std::string* errorMsg);
 
   std::string url;       // retain copy
   std::string user;      // unescaped
@@ -313,13 +317,13 @@
   template <typename T>
   HttpRequest(const HttpLocation& loc, const T& extraParams);
 
-  HttpRequest(const HttpLocation& loc, StringRef path_)
+  HttpRequest(const HttpLocation& loc, std::string_view path_)
       : host{loc.host}, port{loc.port}, path{path_} {
     SetAuth(loc);
   }
 
   template <typename T>
-  HttpRequest(const HttpLocation& loc, StringRef path_, const T& params)
+  HttpRequest(const HttpLocation& loc, std::string_view path_, const T& params)
       : host{loc.host}, port{loc.port} {
     SetPath(path_, params);
     SetAuth(loc);
@@ -333,18 +337,18 @@
  private:
   void SetAuth(const HttpLocation& loc);
   template <typename T>
-  void SetPath(StringRef path_, const T& params);
+  void SetPath(std::string_view path_, const T& params);
 
   template <typename T>
-  static StringRef GetFirst(const T& elem) {
+  static std::string_view GetFirst(const T& elem) {
     return elem.first;
   }
   template <typename T>
-  static StringRef GetFirst(const StringMapEntry<T>& elem) {
+  static std::string_view GetFirst(const StringMapEntry<T>& elem) {
     return elem.getKey();
   }
   template <typename T>
-  static StringRef GetSecond(const T& elem) {
+  static std::string_view GetSecond(const T& elem) {
     return elem.second;
   }
 };
@@ -369,14 +373,15 @@
 
 class HttpMultipartScanner {
  public:
-  explicit HttpMultipartScanner(StringRef boundary, bool saveSkipped = false) {
+  explicit HttpMultipartScanner(std::string_view boundary,
+                                bool saveSkipped = false) {
     Reset(saveSkipped);
     SetBoundary(boundary);
   }
 
   // Change the boundary.  This is only safe to do when IsDone() is true (or
   // immediately after construction).
-  void SetBoundary(StringRef boundary);
+  void SetBoundary(std::string_view boundary);
 
   // Reset the scanner.  This allows reuse of internal buffers.
   void Reset(bool saveSkipped = false);
@@ -385,14 +390,14 @@
   // is true.
   // @param in input data
   // @return the input not consumed; empty if all input consumed
-  StringRef Execute(StringRef in);
+  std::string_view Execute(std::string_view in);
 
   // Returns true when the boundary has been found.
   bool IsDone() const { return m_state == kDone; }
 
   // Get the skipped data.  Will be empty if saveSkipped was false.
-  StringRef GetSkipped() const {
-    return m_saveSkipped ? StringRef{m_buf} : StringRef{};
+  std::string_view GetSkipped() const {
+    return m_saveSkipped ? std::string_view{m_buf} : std::string_view{};
   }
 
  private:
@@ -412,6 +417,6 @@
 
 }  // namespace wpi
 
-#include "HttpUtil.inl"
+#include "HttpUtil.inc"
 
 #endif  // WPIUTIL_WPI_HTTPUTIL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.inc b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.inc
new file mode 100644
index 0000000..726cdfb
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.inc
@@ -0,0 +1,55 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef WPIUTIL_WPI_HTTPUTIL_INC_
+#define WPIUTIL_WPI_HTTPUTIL_INC_
+
+#include <utility>
+
+#include "wpi/HttpUtil.h"
+
+namespace wpi {
+
+inline HttpPathRef HttpPath::drop_front(size_t n) const {
+  return HttpPathRef(*this, n);
+}
+
+template <typename T>
+HttpRequest::HttpRequest(const HttpLocation& loc, const T& extraParams)
+    : host{loc.host}, port{loc.port} {
+  StringMap<std::string_view> params;
+  for (const auto& p : loc.params) {
+    params.insert(std::make_pair(GetFirst(p), GetSecond(p)));
+  }
+  for (const auto& p : extraParams) {
+    params.insert(std::make_pair(GetFirst(p), GetSecond(p)));
+  }
+  SetPath(loc.path, params);
+  SetAuth(loc);
+}
+
+template <typename T>
+void HttpRequest::SetPath(std::string_view path_, const T& params) {
+  // Build location including query string
+  raw_svector_ostream pathOs{path};
+  pathOs << path_;
+  bool first = true;
+  for (const auto& param : params) {
+    if (first) {
+      pathOs << '?';
+      first = false;
+    } else {
+      pathOs << '&';
+    }
+    SmallString<64> escapeBuf;
+    pathOs << EscapeURI(GetFirst(param), escapeBuf, false);
+    if (!GetSecond(param).empty()) {
+      pathOs << '=' << EscapeURI(GetSecond(param), escapeBuf, false);
+    }
+  }
+}
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_HTTPUTIL_INC_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.inl b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.inl
deleted file mode 100644
index f66c746..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpUtil.inl
+++ /dev/null
@@ -1,54 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef WPIUTIL_WPI_HTTPUTIL_INL_
-#define WPIUTIL_WPI_HTTPUTIL_INL_
-
-#include <utility>
-
-namespace wpi {
-
-inline HttpPathRef HttpPath::drop_front(size_t n) const {
-  return HttpPathRef(*this, n);
-}
-
-template <typename T>
-HttpRequest::HttpRequest(const HttpLocation& loc, const T& extraParams)
-    : host{loc.host}, port{loc.port} {
-  StringMap<StringRef> params;
-  for (const auto& p : loc.params)
-    params.insert(std::make_pair(GetFirst(p), GetSecond(p)));
-  for (const auto& p : extraParams)
-    params.insert(std::make_pair(GetFirst(p), GetSecond(p)));
-  SetPath(loc.path, params);
-  SetAuth(loc);
-}
-
-template <typename T>
-void HttpRequest::SetPath(StringRef path_, const T& params) {
-  // Build location including query string
-  raw_svector_ostream pathOs{path};
-  pathOs << path_;
-  bool first = true;
-  for (const auto& param : params) {
-    if (first) {
-      pathOs << '?';
-      first = false;
-    } else {
-      pathOs << '&';
-    }
-    SmallString<64> escapeBuf;
-    pathOs << EscapeURI(GetFirst(param), escapeBuf, false);
-    if (!GetSecond(param).empty()) {
-      pathOs << '=' << EscapeURI(GetSecond(param), escapeBuf, false);
-    }
-  }
-}
-
-}  // namespace wpi
-
-#endif  // WPIUTIL_WPI_HTTPUTIL_INL_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h
index d662471..d89b61f 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_H_
 #define WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_H_
@@ -11,13 +8,13 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/HttpServerConnection.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
 #include "wpi/WebSocket.h"
 #include "wpi/WebSocketServer.h"
+#include "wpi/span.h"
 #include "wpi/uv/Stream.h"
 
 namespace wpi {
@@ -39,7 +36,7 @@
    * @param protocols Acceptable subprotocols
    */
   HttpWebSocketServerConnection(std::shared_ptr<uv::Stream> stream,
-                                ArrayRef<StringRef> protocols);
+                                span<const std::string_view> protocols);
 
   /**
    * Constructor.
@@ -47,10 +44,11 @@
    * @param stream network stream
    * @param protocols Acceptable subprotocols
    */
-  HttpWebSocketServerConnection(std::shared_ptr<uv::Stream> stream,
-                                std::initializer_list<StringRef> protocols)
-      : HttpWebSocketServerConnection(
-            stream, makeArrayRef(protocols.begin(), protocols.end())) {}
+  HttpWebSocketServerConnection(
+      std::shared_ptr<uv::Stream> stream,
+      std::initializer_list<std::string_view> protocols)
+      : HttpWebSocketServerConnection(stream,
+                                      {protocols.begin(), protocols.end()}) {}
 
  protected:
   /**
@@ -62,7 +60,7 @@
    *
    * @param protocol negotiated subprotocol
    */
-  virtual bool IsValidWsUpgrade(StringRef protocol) { return true; }
+  virtual bool IsValidWsUpgrade(std::string_view protocol) { return true; }
 
   /**
    * Process an incoming WebSocket upgrade.  This is called after the header
@@ -89,6 +87,6 @@
 
 }  // namespace wpi
 
-#include "HttpWebSocketServerConnection.inl"
+#include "HttpWebSocketServerConnection.inc"
 
 #endif  // WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inc b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inc
new file mode 100644
index 0000000..68b9f2d
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inc
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INC_
+#define WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INC_
+
+#include <memory>
+
+#include "wpi/HttpWebSocketServerConnection.h"
+
+namespace wpi {
+
+template <typename Derived>
+HttpWebSocketServerConnection<Derived>::HttpWebSocketServerConnection(
+    std::shared_ptr<uv::Stream> stream, span<const std::string_view> protocols)
+    : HttpServerConnection{stream},
+      m_helper{m_request},
+      m_protocols{protocols.begin(), protocols.end()} {
+  // Handle upgrade event
+  m_helper.upgrade.connect([this] {
+    // Negotiate sub-protocol
+    SmallVector<std::string_view, 2> protocols{m_protocols.begin(),
+                                               m_protocols.end()};
+    std::string_view protocol = m_helper.MatchProtocol(protocols).second;
+
+    // Check that the upgrade is valid
+    if (!IsValidWsUpgrade(protocol)) {
+      return;
+    }
+
+    // Disconnect HttpServerConnection header reader
+    m_dataConn.disconnect();
+    m_messageCompleteConn.disconnect();
+
+    // Accepting the stream may destroy this (as it replaces the stream user
+    // data), so grab a shared pointer first.
+    auto self = this->shared_from_this();
+
+    // Accept the upgrade
+    auto ws = m_helper.Accept(m_stream, protocol);
+
+    // Set this as the websocket user data to keep it around
+    ws->SetData(self);
+
+    // Store in member
+    m_websocket = ws.get();
+
+    // Call derived class function
+    ProcessWsUpgrade();
+  });
+}
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INC_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl
deleted file mode 100644
index 586f189..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/HttpWebSocketServerConnection.inl
+++ /dev/null
@@ -1,54 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INL_
-#define WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INL_
-
-#include <memory>
-
-namespace wpi {
-
-template <typename Derived>
-HttpWebSocketServerConnection<Derived>::HttpWebSocketServerConnection(
-    std::shared_ptr<uv::Stream> stream, ArrayRef<StringRef> protocols)
-    : HttpServerConnection{stream},
-      m_helper{m_request},
-      m_protocols{protocols.begin(), protocols.end()} {
-  // Handle upgrade event
-  m_helper.upgrade.connect([this] {
-    // Negotiate sub-protocol
-    SmallVector<StringRef, 2> protocols{m_protocols.begin(), m_protocols.end()};
-    StringRef protocol = m_helper.MatchProtocol(protocols).second;
-
-    // Check that the upgrade is valid
-    if (!IsValidWsUpgrade(protocol)) return;
-
-    // Disconnect HttpServerConnection header reader
-    m_dataConn.disconnect();
-    m_messageCompleteConn.disconnect();
-
-    // Accepting the stream may destroy this (as it replaces the stream user
-    // data), so grab a shared pointer first.
-    auto self = this->shared_from_this();
-
-    // Accept the upgrade
-    auto ws = m_helper.Accept(m_stream, protocol);
-
-    // Set this as the websocket user data to keep it around
-    ws->SetData(self);
-
-    // Store in member
-    m_websocket = ws.get();
-
-    // Call derived class function
-    ProcessWsUpgrade();
-  });
-}
-
-}  // namespace wpi
-
-#endif  // WPIUTIL_WPI_HTTPWEBSOCKETSERVERCONNECTION_INL_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/IntrusiveRefCntPtr.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/IntrusiveRefCntPtr.h
deleted file mode 100644
index c677c10..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/IntrusiveRefCntPtr.h
+++ /dev/null
@@ -1,270 +0,0 @@
-//==- llvm/ADT/IntrusiveRefCntPtr.h - Smart Refcounting Pointer --*- C++ -*-==//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file defines the RefCountedBase, ThreadSafeRefCountedBase, and
-// IntrusiveRefCntPtr classes.
-//
-// IntrusiveRefCntPtr is a smart pointer to an object which maintains a
-// reference count.  (ThreadSafe)RefCountedBase is a mixin class that adds a
-// refcount member variable and methods for updating the refcount.  An object
-// that inherits from (ThreadSafe)RefCountedBase deletes itself when its
-// refcount hits zero.
-//
-// For example:
-//
-//   class MyClass : public RefCountedBase<MyClass> {};
-//
-//   void foo() {
-//     // Constructing an IntrusiveRefCntPtr increases the pointee's refcount by
-//     // 1 (from 0 in this case).
-//     IntrusiveRefCntPtr<MyClass> Ptr1(new MyClass());
-//
-//     // Copying an IntrusiveRefCntPtr increases the pointee's refcount by 1.
-//     IntrusiveRefCntPtr<MyClass> Ptr2(Ptr1);
-//
-//     // Constructing an IntrusiveRefCntPtr has no effect on the object's
-//     // refcount.  After a move, the moved-from pointer is null.
-//     IntrusiveRefCntPtr<MyClass> Ptr3(std::move(Ptr1));
-//     assert(Ptr1 == nullptr);
-//
-//     // Clearing an IntrusiveRefCntPtr decreases the pointee's refcount by 1.
-//     Ptr2.reset();
-//
-//     // The object deletes itself when we return from the function, because
-//     // Ptr3's destructor decrements its refcount to 0.
-//   }
-//
-// You can use IntrusiveRefCntPtr with isa<T>(), dyn_cast<T>(), etc.:
-//
-//   IntrusiveRefCntPtr<MyClass> Ptr(new MyClass());
-//   OtherClass *Other = dyn_cast<OtherClass>(Ptr);  // Ptr.get() not required
-//
-// IntrusiveRefCntPtr works with any class that
-//
-//  - inherits from (ThreadSafe)RefCountedBase,
-//  - has Retain() and Release() methods, or
-//  - specializes IntrusiveRefCntPtrInfo.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_INTRUSIVEREFCNTPTR_H
-#define WPIUTIL_WPI_INTRUSIVEREFCNTPTR_H
-
-#include <atomic>
-#include <cassert>
-#include <cstddef>
-
-namespace wpi {
-
-/// A CRTP mixin class that adds reference counting to a type.
-///
-/// The lifetime of an object which inherits from RefCountedBase is managed by
-/// calls to Release() and Retain(), which increment and decrement the object's
-/// refcount, respectively.  When a Release() call decrements the refcount to 0,
-/// the object deletes itself.
-template <class Derived> class RefCountedBase {
-  mutable unsigned RefCount = 0;
-
-public:
-  RefCountedBase() = default;
-  RefCountedBase(const RefCountedBase &) {}
-
-  void Retain() const { ++RefCount; }
-
-  void Release() const {
-    assert(RefCount > 0 && "Reference count is already zero.");
-    if (--RefCount == 0)
-      delete static_cast<const Derived *>(this);
-  }
-};
-
-/// A thread-safe version of \c RefCountedBase.
-template <class Derived> class ThreadSafeRefCountedBase {
-  mutable std::atomic<int> RefCount;
-
-protected:
-  ThreadSafeRefCountedBase() : RefCount(0) {}
-
-public:
-  void Retain() const { RefCount.fetch_add(1, std::memory_order_relaxed); }
-
-  void Release() const {
-    int NewRefCount = RefCount.fetch_sub(1, std::memory_order_acq_rel) - 1;
-    assert(NewRefCount >= 0 && "Reference count was already zero.");
-    if (NewRefCount == 0)
-      delete static_cast<const Derived *>(this);
-  }
-};
-
-/// Class you can specialize to provide custom retain/release functionality for
-/// a type.
-///
-/// Usually specializing this class is not necessary, as IntrusiveRefCntPtr
-/// works with any type which defines Retain() and Release() functions -- you
-/// can define those functions yourself if RefCountedBase doesn't work for you.
-///
-/// One case when you might want to specialize this type is if you have
-///  - Foo.h defines type Foo and includes Bar.h, and
-///  - Bar.h uses IntrusiveRefCntPtr<Foo> in inline functions.
-///
-/// Because Foo.h includes Bar.h, Bar.h can't include Foo.h in order to pull in
-/// the declaration of Foo.  Without the declaration of Foo, normally Bar.h
-/// wouldn't be able to use IntrusiveRefCntPtr<Foo>, which wants to call
-/// T::Retain and T::Release.
-///
-/// To resolve this, Bar.h could include a third header, FooFwd.h, which
-/// forward-declares Foo and specializes IntrusiveRefCntPtrInfo<Foo>.  Then
-/// Bar.h could use IntrusiveRefCntPtr<Foo>, although it still couldn't call any
-/// functions on Foo itself, because Foo would be an incomplete type.
-template <typename T> struct IntrusiveRefCntPtrInfo {
-  static void retain(T *obj) { obj->Retain(); }
-  static void release(T *obj) { obj->Release(); }
-};
-
-/// A smart pointer to a reference-counted object that inherits from
-/// RefCountedBase or ThreadSafeRefCountedBase.
-///
-/// This class increments its pointee's reference count when it is created, and
-/// decrements its refcount when it's destroyed (or is changed to point to a
-/// different object).
-template <typename T> class IntrusiveRefCntPtr {
-  T *Obj = nullptr;
-
-public:
-  using element_type = T;
-
-  explicit IntrusiveRefCntPtr() = default;
-  IntrusiveRefCntPtr(T *obj) : Obj(obj) { retain(); }
-  IntrusiveRefCntPtr(const IntrusiveRefCntPtr &S) : Obj(S.Obj) { retain(); }
-  IntrusiveRefCntPtr(IntrusiveRefCntPtr &&S) : Obj(S.Obj) { S.Obj = nullptr; }
-
-  template <class X>
-  IntrusiveRefCntPtr(IntrusiveRefCntPtr<X> &&S) : Obj(S.get()) {
-    S.Obj = nullptr;
-  }
-
-  template <class X>
-  IntrusiveRefCntPtr(const IntrusiveRefCntPtr<X> &S) : Obj(S.get()) {
-    retain();
-  }
-
-  ~IntrusiveRefCntPtr() { release(); }
-
-  IntrusiveRefCntPtr &operator=(IntrusiveRefCntPtr S) {
-    swap(S);
-    return *this;
-  }
-
-  T &operator*() const { return *Obj; }
-  T *operator->() const { return Obj; }
-  T *get() const { return Obj; }
-  explicit operator bool() const { return Obj; }
-
-  void swap(IntrusiveRefCntPtr &other) {
-    T *tmp = other.Obj;
-    other.Obj = Obj;
-    Obj = tmp;
-  }
-
-  void reset() {
-    release();
-    Obj = nullptr;
-  }
-
-  void resetWithoutRelease() { Obj = nullptr; }
-
-private:
-  void retain() {
-    if (Obj)
-      IntrusiveRefCntPtrInfo<T>::retain(Obj);
-  }
-
-  void release() {
-    if (Obj)
-      IntrusiveRefCntPtrInfo<T>::release(Obj);
-  }
-
-  template <typename X> friend class IntrusiveRefCntPtr;
-};
-
-template <class T, class U>
-inline bool operator==(const IntrusiveRefCntPtr<T> &A,
-                       const IntrusiveRefCntPtr<U> &B) {
-  return A.get() == B.get();
-}
-
-template <class T, class U>
-inline bool operator!=(const IntrusiveRefCntPtr<T> &A,
-                       const IntrusiveRefCntPtr<U> &B) {
-  return A.get() != B.get();
-}
-
-template <class T, class U>
-inline bool operator==(const IntrusiveRefCntPtr<T> &A, U *B) {
-  return A.get() == B;
-}
-
-template <class T, class U>
-inline bool operator!=(const IntrusiveRefCntPtr<T> &A, U *B) {
-  return A.get() != B;
-}
-
-template <class T, class U>
-inline bool operator==(T *A, const IntrusiveRefCntPtr<U> &B) {
-  return A == B.get();
-}
-
-template <class T, class U>
-inline bool operator!=(T *A, const IntrusiveRefCntPtr<U> &B) {
-  return A != B.get();
-}
-
-template <class T>
-bool operator==(std::nullptr_t A, const IntrusiveRefCntPtr<T> &B) {
-  return !B;
-}
-
-template <class T>
-bool operator==(const IntrusiveRefCntPtr<T> &A, std::nullptr_t B) {
-  return B == A;
-}
-
-template <class T>
-bool operator!=(std::nullptr_t A, const IntrusiveRefCntPtr<T> &B) {
-  return !(A == B);
-}
-
-template <class T>
-bool operator!=(const IntrusiveRefCntPtr<T> &A, std::nullptr_t B) {
-  return !(A == B);
-}
-
-// Make IntrusiveRefCntPtr work with dyn_cast, isa, and the other idioms from
-// Casting.h.
-template <typename From> struct simplify_type;
-
-template <class T> struct simplify_type<IntrusiveRefCntPtr<T>> {
-  using SimpleType = T *;
-
-  static SimpleType getSimplifiedValue(IntrusiveRefCntPtr<T> &Val) {
-    return Val.get();
-  }
-};
-
-template <class T> struct simplify_type<const IntrusiveRefCntPtr<T>> {
-  using SimpleType = /*const*/ T *;
-
-  static SimpleType getSimplifiedValue(const IntrusiveRefCntPtr<T> &Val) {
-    return Val.get();
-  }
-};
-
-} // end namespace wpi
-
-#endif // LLVM_ADT_INTRUSIVEREFCNTPTR_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Logger.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Logger.h
index 20aa8ee..807e472 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Logger.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Logger.h
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_LOGGER_H_
 #define WPIUTIL_WPI_LOGGER_H_
 
 #include <functional>
+#include <utility>
 
-#include "wpi/SmallString.h"
-#include "wpi/raw_ostream.h"
+#include "fmt/format.h"
 
 namespace wpi {
 
@@ -33,19 +30,28 @@
                                      unsigned int line, const char* msg)>;
 
   Logger() = default;
-  explicit Logger(const LogFunc& func) : m_func(func) {}
-  Logger(const LogFunc& func, unsigned int min_level)
-      : m_func(func), m_min_level(min_level) {}
+  explicit Logger(LogFunc func) : m_func(std::move(func)) {}
+  Logger(LogFunc func, unsigned int min_level)
+      : m_func(std::move(func)), m_min_level(min_level) {}
 
   void SetLogger(LogFunc func) { m_func = func; }
 
   void set_min_level(unsigned int level) { m_min_level = level; }
   unsigned int min_level() const { return m_min_level; }
 
+  void DoLog(unsigned int level, const char* file, unsigned int line,
+             const char* msg);
+
+  void LogV(unsigned int level, const char* file, unsigned int line,
+            fmt::string_view format, fmt::format_args args);
+
+  template <typename S, typename... Args>
   void Log(unsigned int level, const char* file, unsigned int line,
-           const char* msg) {
-    if (!m_func || level < m_min_level) return;
-    m_func(level, file, line, msg);
+           const S& format, Args&&... args) {
+    if (m_func && level >= m_min_level) {
+      LogV(level, file, line, format,
+           fmt::make_args_checked<Args...>(format, args...));
+    }
   }
 
   bool HasLogger() const { return m_func != nullptr; }
@@ -55,44 +61,43 @@
   unsigned int m_min_level = 20;
 };
 
-#define WPI_LOG(logger_inst, level, x)                                 \
-  do {                                                                 \
-    ::wpi::Logger& WPI_logger_ = logger_inst;                          \
-    if (WPI_logger_.min_level() <= static_cast<unsigned int>(level) && \
-        WPI_logger_.HasLogger()) {                                     \
-      ::wpi::SmallString<128> log_buf_;                                \
-      ::wpi::raw_svector_ostream log_os_{log_buf_};                    \
-      log_os_ << x;                                                    \
-      WPI_logger_.Log(level, __FILE__, __LINE__, log_buf_.c_str());    \
-    }                                                                  \
-  } while (0)
+#define WPI_LOG(logger_inst, level, format, ...) \
+  logger_inst.Log(level, __FILE__, __LINE__, FMT_STRING(format), __VA_ARGS__)
 
-#define WPI_ERROR(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_ERROR, x)
-#define WPI_WARNING(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_WARNING, x)
-#define WPI_INFO(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_INFO, x)
+#define WPI_ERROR(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_ERROR, format, __VA_ARGS__)
+#define WPI_WARNING(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_WARNING, format, __VA_ARGS__)
+#define WPI_INFO(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_INFO, format, __VA_ARGS__)
 
 #ifdef NDEBUG
-#define WPI_DEBUG(inst, x) \
-  do {                     \
+#define WPI_DEBUG(inst, format, ...) \
+  do {                               \
   } while (0)
-#define WPI_DEBUG1(inst, x) \
-  do {                      \
+#define WPI_DEBUG1(inst, format, ...) \
+  do {                                \
   } while (0)
-#define WPI_DEBUG2(inst, x) \
-  do {                      \
+#define WPI_DEBUG2(inst, format, ...) \
+  do {                                \
   } while (0)
-#define WPI_DEBUG3(inst, x) \
-  do {                      \
+#define WPI_DEBUG3(inst, format, ...) \
+  do {                                \
   } while (0)
-#define WPI_DEBUG4(inst, x) \
-  do {                      \
+#define WPI_DEBUG4(inst, format, ...) \
+  do {                                \
   } while (0)
 #else
-#define WPI_DEBUG(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG, x)
-#define WPI_DEBUG1(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG1, x)
-#define WPI_DEBUG2(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG2, x)
-#define WPI_DEBUG3(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG3, x)
-#define WPI_DEBUG4(inst, x) WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG4, x)
+#define WPI_DEBUG(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG, format, __VA_ARGS__)
+#define WPI_DEBUG1(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG1, format, __VA_ARGS__)
+#define WPI_DEBUG2(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG2, format, __VA_ARGS__)
+#define WPI_DEBUG3(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG3, format, __VA_ARGS__)
+#define WPI_DEBUG4(inst, format, ...) \
+  WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG4, format, __VA_ARGS__)
 #endif
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/MimeTypes.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/MimeTypes.h
index b3dc17b..ce36ea2 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/MimeTypes.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/MimeTypes.h
@@ -1,18 +1,15 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_MIMETYPES_H_
 #define WPIUTIL_WPI_MIMETYPES_H_
 
-#include "wpi/StringRef.h"
+#include <string_view>
 
 namespace wpi {
 
-StringRef MimeTypeFromPath(StringRef path);
+std::string_view MimeTypeFromPath(std::string_view path);
 
 }  // namespace wpi
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NativeFormatting.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NativeFormatting.h
deleted file mode 100644
index 976adc8..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NativeFormatting.h
+++ /dev/null
@@ -1,49 +0,0 @@
-//===- NativeFormatting.h - Low level formatting helpers ---------*- C++-*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_NATIVE_FORMATTING_H
-#define WPIUTIL_WPI_NATIVE_FORMATTING_H
-
-#include "wpi/raw_ostream.h"
-
-#include <cstdint>
-#include <optional>
-
-namespace wpi {
-enum class FloatStyle { Exponent, ExponentUpper, Fixed, Percent };
-enum class IntegerStyle {
-  Integer,
-  Number,
-};
-enum class HexPrintStyle { Upper, Lower, PrefixUpper, PrefixLower };
-
-size_t getDefaultPrecision(FloatStyle Style);
-
-bool isPrefixedHexStyle(HexPrintStyle S);
-
-void write_integer(raw_ostream &S, unsigned int N, size_t MinDigits,
-                   IntegerStyle Style);
-void write_integer(raw_ostream &S, int N, size_t MinDigits, IntegerStyle Style);
-void write_integer(raw_ostream &S, unsigned long N, size_t MinDigits,
-                   IntegerStyle Style);
-void write_integer(raw_ostream &S, long N, size_t MinDigits,
-                   IntegerStyle Style);
-void write_integer(raw_ostream &S, unsigned long long N, size_t MinDigits,
-                   IntegerStyle Style);
-void write_integer(raw_ostream &S, long long N, size_t MinDigits,
-                   IntegerStyle Style);
-
-void write_hex(raw_ostream &S, uint64_t N, HexPrintStyle Style,
-               std::optional<size_t> Width = std::nullopt);
-void write_double(raw_ostream &S, double D, FloatStyle Style,
-                  std::optional<size_t> Precision = std::nullopt);
-}
-
-#endif
-
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkAcceptor.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkAcceptor.h
index dd09e2d..9982360 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkAcceptor.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkAcceptor.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_NETWORKACCEPTOR_H_
 #define WPIUTIL_WPI_NETWORKACCEPTOR_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkStream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkStream.h
index 1177167..b842ecd 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkStream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NetworkStream.h
@@ -1,16 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_NETWORKSTREAM_H_
 #define WPIUTIL_WPI_NETWORKSTREAM_H_
 
 #include <cstddef>
-
-#include "wpi/StringRef.h"
+#include <string_view>
 
 namespace wpi {
 
@@ -31,7 +27,7 @@
                          int timeout = 0) = 0;
   virtual void close() = 0;
 
-  virtual StringRef getPeerIP() const = 0;
+  virtual std::string_view getPeerIP() const = 0;
   virtual int getPeerPort() const = 0;
   virtual void setNoDelay() = 0;
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NullDeleter.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NullDeleter.h
new file mode 100644
index 0000000..26146e5
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/NullDeleter.h
@@ -0,0 +1,16 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+namespace wpi {
+
+// A struct to use as a deleter when a std::shared_ptr must wrap a raw pointer
+// that is being deleted by someone else.
+template <class T>
+struct NullDeleter {
+  void operator()(T*) const noexcept {};
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ParallelTcpConnector.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ParallelTcpConnector.h
new file mode 100644
index 0000000..47c29d0
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ParallelTcpConnector.h
@@ -0,0 +1,119 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <stdint.h>
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include "wpi/span.h"
+#include "wpi/uv/Timer.h"
+
+namespace wpi {
+
+class Logger;
+
+namespace uv {
+class GetAddrInfoReq;
+class Loop;
+class Tcp;
+class Timer;
+}  // namespace uv
+
+/**
+ * Parallel TCP connector.  Attempts parallel resolution and connection to
+ * multiple servers with automatic retry if none connect.
+ *
+ * Each successful TCP connection results in a call to the connected callback.
+ * For correct operation, the consuming code (either the connected callback or
+ * e.g. task it starts) must call Succeeded() to indicate if the connection has
+ * succeeded prior to the reconnect rate timeout.  A successful connection
+ * results in the connector terminating all other connection attempts.
+ *
+ * After the reconnect rate times out, all remaining active connection attempts
+ * are canceled and new ones started.
+ */
+class ParallelTcpConnector
+    : public std::enable_shared_from_this<ParallelTcpConnector> {
+  struct private_init {};
+
+ public:
+  /**
+   * Create.
+   *
+   * @param loop loop
+   * @param reconnectRate how long to wait after starting connection attempts
+   *                      to cancel and attempt connecting again
+   * @param logger logger
+   * @param connected callback function when a connection succeeds; may be
+   *                  called multiple times if it does not call Succeeded()
+   *                  before returning
+   * @return Parallel connector
+   */
+  static std::shared_ptr<ParallelTcpConnector> Create(
+      wpi::uv::Loop& loop, wpi::uv::Timer::Time reconnectRate,
+      wpi::Logger& logger, std::function<void(wpi::uv::Tcp& tcp)> connected) {
+    return std::make_shared<ParallelTcpConnector>(
+        loop, reconnectRate, logger, std::move(connected), private_init{});
+  }
+
+  ParallelTcpConnector(wpi::uv::Loop& loop, wpi::uv::Timer::Time reconnectRate,
+                       wpi::Logger& logger,
+                       std::function<void(wpi::uv::Tcp& tcp)> connected,
+                       const private_init&);
+  ~ParallelTcpConnector();
+
+  ParallelTcpConnector(const ParallelTcpConnector&) = delete;
+  ParallelTcpConnector& operator=(const ParallelTcpConnector&) = delete;
+
+  /**
+   * Closes resources, canceling all pending action attempts.
+   */
+  void Close();
+
+  /**
+   * Changes the servers/ports to connect to.  Starts connection attempts if not
+   * already connected.
+   *
+   * @param servers array of server/port pairs
+   */
+  void SetServers(
+      wpi::span<const std::pair<std::string, unsigned int>> servers);
+
+  /**
+   * Tells the parallel connector that the current connection has terminated and
+   * it is necessary to start reconnection attempts.
+   */
+  void Disconnected();
+
+  /**
+   * Tells the parallel connector that a particular connection has succeeded and
+   * it should stop trying to connect.
+   *
+   * @param tcp connection passed to connected callback
+   */
+  void Succeeded(wpi::uv::Tcp& tcp);
+
+ private:
+  bool IsConnected() const { return m_isConnected || m_servers.empty(); }
+  void Connect();
+  void CancelAll(wpi::uv::Tcp* except = nullptr);
+
+  wpi::uv::Loop& m_loop;
+  wpi::Logger& m_logger;
+  wpi::uv::Timer::Time m_reconnectRate;
+  std::function<void(wpi::uv::Tcp& tcp)> m_connected;
+  std::shared_ptr<wpi::uv::Timer> m_reconnectTimer;
+  std::vector<std::pair<std::string, unsigned int>> m_servers;
+  std::vector<std::weak_ptr<wpi::uv::GetAddrInfoReq>> m_resolvers;
+  std::vector<std::weak_ptr<wpi::uv::Tcp>> m_attempts;
+  bool m_isConnected{false};
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Path.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Path.h
deleted file mode 100644
index 2a8e94c..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Path.h
+++ /dev/null
@@ -1,473 +0,0 @@
-//===- llvm/Support/Path.h - Path Operating System Concept ------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file declares the wpi::sys::path namespace. It is designed after
-// TR2/boost filesystem (v3), but modified to remove exception handling and the
-// path class.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_PATH_H_
-#define WPIUTIL_WPI_PATH_H_
-
-#include "wpi/Twine.h"
-#include "wpi/iterator.h"
-#include <cstdint>
-#include <iterator>
-#include <system_error>
-
-#ifdef _WIN32
-// Disable iterator deprecation warning
-#pragma warning(push)
-#pragma warning(disable : 4996)
-#endif
-
-namespace wpi {
-namespace sys {
-namespace path {
-
-enum class Style { windows, posix, native };
-
-/// @name Lexical Component Iterator
-/// @{
-
-/// Path iterator.
-///
-/// This is an input iterator that iterates over the individual components in
-/// \a path. The traversal order is as follows:
-/// * The root-name element, if present.
-/// * The root-directory element, if present.
-/// * Each successive filename element, if present.
-/// * Dot, if one or more trailing non-root slash characters are present.
-/// Traversing backwards is possible with \a reverse_iterator
-///
-/// Iteration examples. Each component is separated by ',':
-/// @code
-///   /          => /
-///   /foo       => /,foo
-///   foo/       => foo,.
-///   /foo/bar   => /,foo,bar
-///   ../        => ..,.
-///   C:\foo\bar => C:,/,foo,bar
-/// @endcode
-class const_iterator
-    : public iterator_facade_base<const_iterator, std::input_iterator_tag,
-                                  const StringRef> {
-  StringRef Path;      ///< The entire path.
-  StringRef Component; ///< The current component. Not necessarily in Path.
-  size_t    Position;  ///< The iterators current position within Path.
-  Style S;             ///< The path style to use.
-
-  // An end iterator has Position = Path.size() + 1.
-  friend const_iterator begin(StringRef path, Style style);
-  friend const_iterator end(StringRef path);
-
-public:
-  reference operator*() const { return Component; }
-  const_iterator &operator++();    // preincrement
-  bool operator==(const const_iterator &RHS) const;
-
-  /// Difference in bytes between this and RHS.
-  ptrdiff_t operator-(const const_iterator &RHS) const;
-};
-
-/// Reverse path iterator.
-///
-/// This is an input iterator that iterates over the individual components in
-/// \a path in reverse order. The traversal order is exactly reversed from that
-/// of \a const_iterator
-class reverse_iterator
-    : public iterator_facade_base<reverse_iterator, std::input_iterator_tag,
-                                  const StringRef> {
-  StringRef Path;      ///< The entire path.
-  StringRef Component; ///< The current component. Not necessarily in Path.
-  size_t    Position;  ///< The iterators current position within Path.
-  Style S;             ///< The path style to use.
-
-  friend reverse_iterator rbegin(StringRef path, Style style);
-  friend reverse_iterator rend(StringRef path);
-
-public:
-  reference operator*() const { return Component; }
-  reverse_iterator &operator++();    // preincrement
-  bool operator==(const reverse_iterator &RHS) const;
-
-  /// Difference in bytes between this and RHS.
-  ptrdiff_t operator-(const reverse_iterator &RHS) const;
-};
-
-/// Get begin iterator over \a path.
-/// @param path Input path.
-/// @returns Iterator initialized with the first component of \a path.
-const_iterator begin(StringRef path, Style style = Style::native);
-
-/// Get end iterator over \a path.
-/// @param path Input path.
-/// @returns Iterator initialized to the end of \a path.
-const_iterator end(StringRef path);
-
-/// Get reverse begin iterator over \a path.
-/// @param path Input path.
-/// @returns Iterator initialized with the first reverse component of \a path.
-reverse_iterator rbegin(StringRef path, Style style = Style::native);
-
-/// Get reverse end iterator over \a path.
-/// @param path Input path.
-/// @returns Iterator initialized to the reverse end of \a path.
-reverse_iterator rend(StringRef path);
-
-/// @}
-/// @name Lexical Modifiers
-/// @{
-
-/// Remove the last component from \a path unless it is the root dir.
-///
-/// @code
-///   directory/filename.cpp => directory/
-///   directory/             => directory
-///   filename.cpp           => <empty>
-///   /                      => /
-/// @endcode
-///
-/// @param path A path that is modified to not have a file component.
-void remove_filename(SmallVectorImpl<char> &path, Style style = Style::native);
-
-/// Replace the file extension of \a path with \a extension.
-///
-/// @code
-///   ./filename.cpp => ./filename.extension
-///   ./filename     => ./filename.extension
-///   ./             => ./.extension
-/// @endcode
-///
-/// @param path A path that has its extension replaced with \a extension.
-/// @param extension The extension to be added. It may be empty. It may also
-///                  optionally start with a '.', if it does not, one will be
-///                  prepended.
-void replace_extension(SmallVectorImpl<char> &path, const Twine &extension,
-                       Style style = Style::native);
-
-/// Replace matching path prefix with another path.
-///
-/// @code
-///   /foo, /old, /new => /foo
-///   /old/foo, /old, /new => /new/foo
-///   /foo, <empty>, /new => /new/foo
-///   /old/foo, /old, <empty> => /foo
-/// @endcode
-///
-/// @param Path If \a Path starts with \a OldPrefix modify to instead
-///        start with \a NewPrefix.
-/// @param OldPrefix The path prefix to strip from \a Path.
-/// @param NewPrefix The path prefix to replace \a NewPrefix with.
-void replace_path_prefix(SmallVectorImpl<char> &Path,
-                         const StringRef &OldPrefix, const StringRef &NewPrefix,
-                         Style style = Style::native);
-
-/// Append to path.
-///
-/// @code
-///   /foo  + bar/f => /foo/bar/f
-///   /foo/ + bar/f => /foo/bar/f
-///   foo   + bar/f => foo/bar/f
-/// @endcode
-///
-/// @param path Set to \a path + \a component.
-/// @param a The component to be appended to \a path.
-void append(SmallVectorImpl<char> &path, const Twine &a,
-                                         const Twine &b = "",
-                                         const Twine &c = "",
-                                         const Twine &d = "");
-
-void append(SmallVectorImpl<char> &path, Style style, const Twine &a,
-            const Twine &b = "", const Twine &c = "", const Twine &d = "");
-
-/// Append to path.
-///
-/// @code
-///   /foo  + [bar,f] => /foo/bar/f
-///   /foo/ + [bar,f] => /foo/bar/f
-///   foo   + [bar,f] => foo/bar/f
-/// @endcode
-///
-/// @param path Set to \a path + [\a begin, \a end).
-/// @param begin Start of components to append.
-/// @param end One past the end of components to append.
-void append(SmallVectorImpl<char> &path, const_iterator begin,
-            const_iterator end, Style style = Style::native);
-
-/// @}
-/// @name Transforms (or some other better name)
-/// @{
-
-/// Convert path to the native form. This is used to give paths to users and
-/// operating system calls in the platform's normal way. For example, on Windows
-/// all '/' are converted to '\'.
-///
-/// @param path A path that is transformed to native format.
-/// @param result Holds the result of the transformation.
-void native(const Twine &path, SmallVectorImpl<char> &result,
-            Style style = Style::native);
-
-/// Convert path to the native form in place. This is used to give paths to
-/// users and operating system calls in the platform's normal way. For example,
-/// on Windows all '/' are converted to '\'.
-///
-/// @param path A path that is transformed to native format.
-void native(SmallVectorImpl<char> &path, Style style = Style::native);
-
-/// Replaces backslashes with slashes if Windows.
-///
-/// @param path processed path
-/// @result The result of replacing backslashes with forward slashes if Windows.
-/// On Unix, this function is a no-op because backslashes are valid path
-/// chracters.
-std::string convert_to_slash(StringRef path, Style style = Style::native);
-
-/// @}
-/// @name Lexical Observers
-/// @{
-
-/// Get root name.
-///
-/// @code
-///   //net/hello => //net
-///   c:/hello    => c: (on Windows, on other platforms nothing)
-///   /hello      => <empty>
-/// @endcode
-///
-/// @param path Input path.
-/// @result The root name of \a path if it has one, otherwise "".
-StringRef root_name(StringRef path, Style style = Style::native);
-
-/// Get root directory.
-///
-/// @code
-///   /goo/hello => /
-///   c:/hello   => /
-///   d/file.txt => <empty>
-/// @endcode
-///
-/// @param path Input path.
-/// @result The root directory of \a path if it has one, otherwise
-///               "".
-StringRef root_directory(StringRef path, Style style = Style::native);
-
-/// Get root path.
-///
-/// Equivalent to root_name + root_directory.
-///
-/// @param path Input path.
-/// @result The root path of \a path if it has one, otherwise "".
-StringRef root_path(StringRef path, Style style = Style::native);
-
-/// Get relative path.
-///
-/// @code
-///   C:\hello\world => hello\world
-///   foo/bar        => foo/bar
-///   /foo/bar       => foo/bar
-/// @endcode
-///
-/// @param path Input path.
-/// @result The path starting after root_path if one exists, otherwise "".
-StringRef relative_path(StringRef path, Style style = Style::native);
-
-/// Get parent path.
-///
-/// @code
-///   /          => <empty>
-///   /foo       => /
-///   foo/../bar => foo/..
-/// @endcode
-///
-/// @param path Input path.
-/// @result The parent path of \a path if one exists, otherwise "".
-StringRef parent_path(StringRef path, Style style = Style::native);
-
-/// Get filename.
-///
-/// @code
-///   /foo.txt    => foo.txt
-///   .          => .
-///   ..         => ..
-///   /          => /
-/// @endcode
-///
-/// @param path Input path.
-/// @result The filename part of \a path. This is defined as the last component
-///         of \a path.
-StringRef filename(StringRef path, Style style = Style::native);
-
-/// Get stem.
-///
-/// If filename contains a dot but not solely one or two dots, result is the
-/// substring of filename ending at (but not including) the last dot. Otherwise
-/// it is filename.
-///
-/// @code
-///   /foo/bar.txt => bar
-///   /foo/bar     => bar
-///   /foo/.txt    => <empty>
-///   /foo/.       => .
-///   /foo/..      => ..
-/// @endcode
-///
-/// @param path Input path.
-/// @result The stem of \a path.
-StringRef stem(StringRef path, Style style = Style::native);
-
-/// Get extension.
-///
-/// If filename contains a dot but not solely one or two dots, result is the
-/// substring of filename starting at (and including) the last dot, and ending
-/// at the end of \a path. Otherwise "".
-///
-/// @code
-///   /foo/bar.txt => .txt
-///   /foo/bar     => <empty>
-///   /foo/.txt    => .txt
-/// @endcode
-///
-/// @param path Input path.
-/// @result The extension of \a path.
-StringRef extension(StringRef path, Style style = Style::native);
-
-/// Check whether the given char is a path separator on the host OS.
-///
-/// @param value a character
-/// @result true if \a value is a path separator character on the host OS
-bool is_separator(char value, Style style = Style::native);
-
-/// Return the preferred separator for this platform.
-///
-/// @result StringRef of the preferred separator, null-terminated.
-StringRef get_separator(Style style = Style::native);
-
-/// Get the typical temporary directory for the system, e.g.,
-/// "/var/tmp" or "C:/TEMP"
-///
-/// @param erasedOnReboot Whether to favor a path that is erased on reboot
-/// rather than one that potentially persists longer. This parameter will be
-/// ignored if the user or system has set the typical environment variable
-/// (e.g., TEMP on Windows, TMPDIR on *nix) to specify a temporary directory.
-///
-/// @param result Holds the resulting path name.
-void system_temp_directory(bool erasedOnReboot, SmallVectorImpl<char> &result);
-
-/// Get the user's home directory.
-///
-/// @param result Holds the resulting path name.
-/// @result True if a home directory is set, false otherwise.
-bool home_directory(SmallVectorImpl<char> &result);
-
-/// Has root name?
-///
-/// root_name != ""
-///
-/// @param path Input path.
-/// @result True if the path has a root name, false otherwise.
-bool has_root_name(const Twine &path, Style style = Style::native);
-
-/// Has root directory?
-///
-/// root_directory != ""
-///
-/// @param path Input path.
-/// @result True if the path has a root directory, false otherwise.
-bool has_root_directory(const Twine &path, Style style = Style::native);
-
-/// Has root path?
-///
-/// root_path != ""
-///
-/// @param path Input path.
-/// @result True if the path has a root path, false otherwise.
-bool has_root_path(const Twine &path, Style style = Style::native);
-
-/// Has relative path?
-///
-/// relative_path != ""
-///
-/// @param path Input path.
-/// @result True if the path has a relative path, false otherwise.
-bool has_relative_path(const Twine &path, Style style = Style::native);
-
-/// Has parent path?
-///
-/// parent_path != ""
-///
-/// @param path Input path.
-/// @result True if the path has a parent path, false otherwise.
-bool has_parent_path(const Twine &path, Style style = Style::native);
-
-/// Has filename?
-///
-/// filename != ""
-///
-/// @param path Input path.
-/// @result True if the path has a filename, false otherwise.
-bool has_filename(const Twine &path, Style style = Style::native);
-
-/// Has stem?
-///
-/// stem != ""
-///
-/// @param path Input path.
-/// @result True if the path has a stem, false otherwise.
-bool has_stem(const Twine &path, Style style = Style::native);
-
-/// Has extension?
-///
-/// extension != ""
-///
-/// @param path Input path.
-/// @result True if the path has a extension, false otherwise.
-bool has_extension(const Twine &path, Style style = Style::native);
-
-/// Is path absolute?
-///
-/// @param path Input path.
-/// @result True if the path is absolute, false if it is not.
-bool is_absolute(const Twine &path, Style style = Style::native);
-
-/// Is path relative?
-///
-/// @param path Input path.
-/// @result True if the path is relative, false if it is not.
-bool is_relative(const Twine &path, Style style = Style::native);
-
-/// Remove redundant leading "./" pieces and consecutive separators.
-///
-/// @param path Input path.
-/// @result The cleaned-up \a path.
-StringRef remove_leading_dotslash(StringRef path, Style style = Style::native);
-
-/// In-place remove any './' and optionally '../' components from a path.
-///
-/// @param path processed path
-/// @param remove_dot_dot specify if '../' (except for leading "../") should be
-/// removed
-/// @result True if path was changed
-bool remove_dots(SmallVectorImpl<char> &path, bool remove_dot_dot = false,
-                 Style style = Style::native);
-
-#if defined(_WIN32)
-std::error_code widenPath(const Twine &Path8, SmallVectorImpl<wchar_t> &Path16);
-#endif
-
-} // end namespace path
-} // end namespace sys
-} // end namespace wpi
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
-
-#endif
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/PortForwarder.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/PortForwarder.h
index 9ee6ce6..9e445a0 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/PortForwarder.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/PortForwarder.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_PORTFORWARDER_H_
 #define WPIUTIL_WPI_PORTFORWARDER_H_
@@ -11,8 +8,7 @@
 #pragma once
 
 #include <memory>
-
-#include "wpi/Twine.h"
+#include <string_view>
 
 namespace wpi {
 
@@ -41,7 +37,8 @@
    * @param remoteHost remote IP address / DNS name
    * @param remotePort remote port number
    */
-  void Add(unsigned int port, const Twine& remoteHost, unsigned int remotePort);
+  void Add(unsigned int port, std::string_view remoteHost,
+           unsigned int remotePort);
 
   /**
    * Stop TCP forwarding on a port.
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/STLExtras.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/STLExtras.h
deleted file mode 100644
index d7ddd92..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/STLExtras.h
+++ /dev/null
@@ -1,1348 +0,0 @@
-//===- llvm/ADT/STLExtras.h - Useful STL related functions ------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-//
-// This file contains some templates that are useful if you are working with the
-// STL at all.
-//
-// No library is required when using these functions.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_STLEXTRAS_H
-#define WPIUTIL_WPI_STLEXTRAS_H
-
-#include "wpi/SmallVector.h"
-#include "wpi/iterator.h"
-#include "wpi/iterator_range.h"
-#include "wpi/ErrorHandling.h"
-#include <algorithm>
-#include <cassert>
-#include <cstddef>
-#include <cstdint>
-#include <cstdlib>
-#include <functional>
-#include <initializer_list>
-#include <iterator>
-#include <limits>
-#include <memory>
-#include <optional>
-#include <tuple>
-#include <type_traits>
-#include <utility>
-
-namespace wpi {
-
-// Only used by compiler if both template types are the same.  Useful when
-// using SFINAE to test for the existence of member functions.
-template <typename T, T> struct SameType;
-
-namespace detail {
-
-template <typename RangeT>
-using IterOfRange = decltype(std::begin(std::declval<RangeT &>()));
-
-template <typename RangeT>
-using ValueOfRange = typename std::remove_reference<decltype(
-    *std::begin(std::declval<RangeT &>()))>::type;
-
-} // end namespace detail
-
-//===----------------------------------------------------------------------===//
-//     Extra additions to <type_traits>
-//===----------------------------------------------------------------------===//
-
-template <typename T> struct make_const_ptr {
-  using type =
-      typename std::add_pointer<typename std::add_const<T>::type>::type;
-};
-
-template <typename T> struct make_const_ref {
-  using type = typename std::add_lvalue_reference<
-      typename std::add_const<T>::type>::type;
-};
-
-//===----------------------------------------------------------------------===//
-//     Extra additions to <functional>
-//===----------------------------------------------------------------------===//
-
-template <class Ty> struct identity {
-  using argument_type = Ty;
-
-  Ty &operator()(Ty &self) const {
-    return self;
-  }
-  const Ty &operator()(const Ty &self) const {
-    return self;
-  }
-};
-
-template <class Ty> struct less_ptr {
-  bool operator()(const Ty* left, const Ty* right) const {
-    return *left < *right;
-  }
-};
-
-template <class Ty> struct greater_ptr {
-  bool operator()(const Ty* left, const Ty* right) const {
-    return *right < *left;
-  }
-};
-
-/// An efficient, type-erasing, non-owning reference to a callable. This is
-/// intended for use as the type of a function parameter that is not used
-/// after the function in question returns.
-///
-/// This class does not own the callable, so it is not in general safe to store
-/// a function_ref.
-template<typename Fn> class function_ref;
-
-template<typename Ret, typename ...Params>
-class function_ref<Ret(Params...)> {
-  Ret (*callback)(intptr_t callable, Params ...params) = nullptr;
-  intptr_t callable;
-
-  template<typename Callable>
-  static Ret callback_fn(intptr_t callable, Params ...params) {
-    return (*reinterpret_cast<Callable*>(callable))(
-        std::forward<Params>(params)...);
-  }
-
-public:
-  function_ref() = default;
-  function_ref(std::nullptr_t) {}
-
-  template <typename Callable>
-  function_ref(Callable &&callable,
-               typename std::enable_if<
-                   !std::is_same<typename std::remove_reference<Callable>::type,
-                                 function_ref>::value>::type * = nullptr)
-      : callback(callback_fn<typename std::remove_reference<Callable>::type>),
-        callable(reinterpret_cast<intptr_t>(&callable)) {}
-
-  Ret operator()(Params ...params) const {
-    return callback(callable, std::forward<Params>(params)...);
-  }
-
-  explicit operator bool() const { return callback; }
-};
-
-// deleter - Very very very simple method that is used to invoke operator
-// delete on something.  It is used like this:
-//
-//   for_each(V.begin(), B.end(), deleter<Interval>);
-template <class T>
-inline void deleter(T *Ptr) {
-  delete Ptr;
-}
-
-//===----------------------------------------------------------------------===//
-//     Extra additions to <iterator>
-//===----------------------------------------------------------------------===//
-
-namespace adl_detail {
-
-using std::begin;
-
-template <typename ContainerTy>
-auto adl_begin(ContainerTy &&container)
-    -> decltype(begin(std::forward<ContainerTy>(container))) {
-  return begin(std::forward<ContainerTy>(container));
-}
-
-using std::end;
-
-template <typename ContainerTy>
-auto adl_end(ContainerTy &&container)
-    -> decltype(end(std::forward<ContainerTy>(container))) {
-  return end(std::forward<ContainerTy>(container));
-}
-
-using std::swap;
-
-template <typename T>
-void adl_swap(T &&lhs, T &&rhs) noexcept(noexcept(swap(std::declval<T>(),
-                                                       std::declval<T>()))) {
-  swap(std::forward<T>(lhs), std::forward<T>(rhs));
-}
-
-} // end namespace adl_detail
-
-template <typename ContainerTy>
-auto adl_begin(ContainerTy &&container)
-    -> decltype(adl_detail::adl_begin(std::forward<ContainerTy>(container))) {
-  return adl_detail::adl_begin(std::forward<ContainerTy>(container));
-}
-
-template <typename ContainerTy>
-auto adl_end(ContainerTy &&container)
-    -> decltype(adl_detail::adl_end(std::forward<ContainerTy>(container))) {
-  return adl_detail::adl_end(std::forward<ContainerTy>(container));
-}
-
-template <typename T>
-void adl_swap(T &&lhs, T &&rhs) noexcept(
-    noexcept(adl_detail::adl_swap(std::declval<T>(), std::declval<T>()))) {
-  adl_detail::adl_swap(std::forward<T>(lhs), std::forward<T>(rhs));
-}
-
-/// Test whether \p RangeOrContainer is empty. Similar to C++17 std::empty.
-template <typename T>
-constexpr bool empty(const T &RangeOrContainer) {
-  return adl_begin(RangeOrContainer) == adl_end(RangeOrContainer);
-}
-
-// mapped_iterator - This is a simple iterator adapter that causes a function to
-// be applied whenever operator* is invoked on the iterator.
-
-template <typename ItTy, typename FuncTy,
-          typename FuncReturnTy =
-            decltype(std::declval<FuncTy>()(*std::declval<ItTy>()))>
-class mapped_iterator
-    : public iterator_adaptor_base<
-             mapped_iterator<ItTy, FuncTy>, ItTy,
-             typename std::iterator_traits<ItTy>::iterator_category,
-             typename std::remove_reference<FuncReturnTy>::type> {
-public:
-  mapped_iterator(ItTy U, FuncTy F)
-    : mapped_iterator::iterator_adaptor_base(std::move(U)), F(std::move(F)) {}
-
-  ItTy getCurrent() { return this->I; }
-
-  FuncReturnTy operator*() { return F(*this->I); }
-
-private:
-  FuncTy F;
-};
-
-// map_iterator - Provide a convenient way to create mapped_iterators, just like
-// make_pair is useful for creating pairs...
-template <class ItTy, class FuncTy>
-inline mapped_iterator<ItTy, FuncTy> map_iterator(ItTy I, FuncTy F) {
-  return mapped_iterator<ItTy, FuncTy>(std::move(I), std::move(F));
-}
-
-/// Helper to determine if type T has a member called rbegin().
-template <typename Ty> class has_rbegin_impl {
-  using yes = char[1];
-  using no = char[2];
-
-  template <typename Inner>
-  static yes& test(Inner *I, decltype(I->rbegin()) * = nullptr);
-
-  template <typename>
-  static no& test(...);
-
-public:
-  static const bool value = sizeof(test<Ty>(nullptr)) == sizeof(yes);
-};
-
-/// Metafunction to determine if T& or T has a member called rbegin().
-template <typename Ty>
-struct has_rbegin : has_rbegin_impl<typename std::remove_reference<Ty>::type> {
-};
-
-// Returns an iterator_range over the given container which iterates in reverse.
-// Note that the container must have rbegin()/rend() methods for this to work.
-template <typename ContainerTy>
-auto reverse(ContainerTy &&C,
-             typename std::enable_if<has_rbegin<ContainerTy>::value>::type * =
-                 nullptr) -> decltype(make_range(C.rbegin(), C.rend())) {
-  return make_range(C.rbegin(), C.rend());
-}
-
-// Returns a std::reverse_iterator wrapped around the given iterator.
-template <typename IteratorTy>
-std::reverse_iterator<IteratorTy> make_reverse_iterator(IteratorTy It) {
-  return std::reverse_iterator<IteratorTy>(It);
-}
-
-// Returns an iterator_range over the given container which iterates in reverse.
-// Note that the container must have begin()/end() methods which return
-// bidirectional iterators for this to work.
-template <typename ContainerTy>
-auto reverse(
-    ContainerTy &&C,
-    typename std::enable_if<!has_rbegin<ContainerTy>::value>::type * = nullptr)
-    -> decltype(make_range(wpi::make_reverse_iterator(std::end(C)),
-                           wpi::make_reverse_iterator(std::begin(C)))) {
-  return make_range(wpi::make_reverse_iterator(std::end(C)),
-                    wpi::make_reverse_iterator(std::begin(C)));
-}
-
-/// An iterator adaptor that filters the elements of given inner iterators.
-///
-/// The predicate parameter should be a callable object that accepts the wrapped
-/// iterator's reference type and returns a bool. When incrementing or
-/// decrementing the iterator, it will call the predicate on each element and
-/// skip any where it returns false.
-///
-/// \code
-///   int A[] = { 1, 2, 3, 4 };
-///   auto R = make_filter_range(A, [](int N) { return N % 2 == 1; });
-///   // R contains { 1, 3 }.
-/// \endcode
-///
-/// Note: filter_iterator_base implements support for forward iteration.
-/// filter_iterator_impl exists to provide support for bidirectional iteration,
-/// conditional on whether the wrapped iterator supports it.
-template <typename WrappedIteratorT, typename PredicateT, typename IterTag>
-class filter_iterator_base
-    : public iterator_adaptor_base<
-          filter_iterator_base<WrappedIteratorT, PredicateT, IterTag>,
-          WrappedIteratorT,
-          typename std::common_type<
-              IterTag, typename std::iterator_traits<
-                           WrappedIteratorT>::iterator_category>::type> {
-  using BaseT = iterator_adaptor_base<
-      filter_iterator_base<WrappedIteratorT, PredicateT, IterTag>,
-      WrappedIteratorT,
-      typename std::common_type<
-          IterTag, typename std::iterator_traits<
-                       WrappedIteratorT>::iterator_category>::type>;
-
-protected:
-  WrappedIteratorT End;
-  PredicateT Pred;
-
-  void findNextValid() {
-    while (this->I != End && !Pred(*this->I))
-      BaseT::operator++();
-  }
-
-  // Construct the iterator. The begin iterator needs to know where the end
-  // is, so that it can properly stop when it gets there. The end iterator only
-  // needs the predicate to support bidirectional iteration.
-  filter_iterator_base(WrappedIteratorT Begin, WrappedIteratorT End,
-                       PredicateT Pred)
-      : BaseT(Begin), End(End), Pred(Pred) {
-    findNextValid();
-  }
-
-public:
-  using BaseT::operator++;
-
-  filter_iterator_base &operator++() {
-    BaseT::operator++();
-    findNextValid();
-    return *this;
-  }
-};
-
-/// Specialization of filter_iterator_base for forward iteration only.
-template <typename WrappedIteratorT, typename PredicateT,
-          typename IterTag = std::forward_iterator_tag>
-class filter_iterator_impl
-    : public filter_iterator_base<WrappedIteratorT, PredicateT, IterTag> {
-  using BaseT = filter_iterator_base<WrappedIteratorT, PredicateT, IterTag>;
-
-public:
-  filter_iterator_impl(WrappedIteratorT Begin, WrappedIteratorT End,
-                       PredicateT Pred)
-      : BaseT(Begin, End, Pred) {}
-};
-
-/// Specialization of filter_iterator_base for bidirectional iteration.
-template <typename WrappedIteratorT, typename PredicateT>
-class filter_iterator_impl<WrappedIteratorT, PredicateT,
-                           std::bidirectional_iterator_tag>
-    : public filter_iterator_base<WrappedIteratorT, PredicateT,
-                                  std::bidirectional_iterator_tag> {
-  using BaseT = filter_iterator_base<WrappedIteratorT, PredicateT,
-                                     std::bidirectional_iterator_tag>;
-  void findPrevValid() {
-    while (!this->Pred(*this->I))
-      BaseT::operator--();
-  }
-
-public:
-  using BaseT::operator--;
-
-  filter_iterator_impl(WrappedIteratorT Begin, WrappedIteratorT End,
-                       PredicateT Pred)
-      : BaseT(Begin, End, Pred) {}
-
-  filter_iterator_impl &operator--() {
-    BaseT::operator--();
-    findPrevValid();
-    return *this;
-  }
-};
-
-namespace detail {
-
-template <bool is_bidirectional> struct fwd_or_bidi_tag_impl {
-  using type = std::forward_iterator_tag;
-};
-
-template <> struct fwd_or_bidi_tag_impl<true> {
-  using type = std::bidirectional_iterator_tag;
-};
-
-/// Helper which sets its type member to forward_iterator_tag if the category
-/// of \p IterT does not derive from bidirectional_iterator_tag, and to
-/// bidirectional_iterator_tag otherwise.
-template <typename IterT> struct fwd_or_bidi_tag {
-  using type = typename fwd_or_bidi_tag_impl<std::is_base_of<
-      std::bidirectional_iterator_tag,
-      typename std::iterator_traits<IterT>::iterator_category>::value>::type;
-};
-
-} // namespace detail
-
-/// Defines filter_iterator to a suitable specialization of
-/// filter_iterator_impl, based on the underlying iterator's category.
-template <typename WrappedIteratorT, typename PredicateT>
-using filter_iterator = filter_iterator_impl<
-    WrappedIteratorT, PredicateT,
-    typename detail::fwd_or_bidi_tag<WrappedIteratorT>::type>;
-
-/// Convenience function that takes a range of elements and a predicate,
-/// and return a new filter_iterator range.
-///
-/// FIXME: Currently if RangeT && is a rvalue reference to a temporary, the
-/// lifetime of that temporary is not kept by the returned range object, and the
-/// temporary is going to be dropped on the floor after the make_iterator_range
-/// full expression that contains this function call.
-template <typename RangeT, typename PredicateT>
-iterator_range<filter_iterator<detail::IterOfRange<RangeT>, PredicateT>>
-make_filter_range(RangeT &&Range, PredicateT Pred) {
-  using FilterIteratorT =
-      filter_iterator<detail::IterOfRange<RangeT>, PredicateT>;
-  return make_range(
-      FilterIteratorT(std::begin(std::forward<RangeT>(Range)),
-                      std::end(std::forward<RangeT>(Range)), Pred),
-      FilterIteratorT(std::end(std::forward<RangeT>(Range)),
-                      std::end(std::forward<RangeT>(Range)), Pred));
-}
-
-// forward declarations required by zip_shortest/zip_first
-template <typename R, typename UnaryPredicate>
-bool all_of(R &&range, UnaryPredicate P);
-template <typename R, typename UnaryPredicate>
-bool any_of(R &&range, UnaryPredicate P);
-
-namespace detail {
-
-using std::declval;
-
-// We have to alias this since inlining the actual type at the usage site
-// in the parameter list of iterator_facade_base<> below ICEs MSVC 2017.
-template<typename... Iters> struct ZipTupleType {
-  using type = std::tuple<decltype(*declval<Iters>())...>;
-};
-
-template <typename ZipType, typename... Iters>
-using zip_traits = iterator_facade_base<
-    ZipType, typename std::common_type<std::bidirectional_iterator_tag,
-                                       typename std::iterator_traits<
-                                           Iters>::iterator_category...>::type,
-    // ^ TODO: Implement random access methods.
-    typename ZipTupleType<Iters...>::type,
-    typename std::iterator_traits<typename std::tuple_element<
-        0, std::tuple<Iters...>>::type>::difference_type,
-    // ^ FIXME: This follows boost::make_zip_iterator's assumption that all
-    // inner iterators have the same difference_type. It would fail if, for
-    // instance, the second field's difference_type were non-numeric while the
-    // first is.
-    typename ZipTupleType<Iters...>::type *,
-    typename ZipTupleType<Iters...>::type>;
-
-template <typename ZipType, typename... Iters>
-struct zip_common : public zip_traits<ZipType, Iters...> {
-  using Base = zip_traits<ZipType, Iters...>;
-  using value_type = typename Base::value_type;
-
-  std::tuple<Iters...> iterators;
-
-protected:
-  template <size_t... Ns> value_type deref(std::index_sequence<Ns...>) const {
-    return value_type(*std::get<Ns>(iterators)...);
-  }
-
-  template <size_t... Ns>
-  decltype(iterators) tup_inc(std::index_sequence<Ns...>) const {
-    return std::tuple<Iters...>(std::next(std::get<Ns>(iterators))...);
-  }
-
-  template <size_t... Ns>
-  decltype(iterators) tup_dec(std::index_sequence<Ns...>) const {
-    return std::tuple<Iters...>(std::prev(std::get<Ns>(iterators))...);
-  }
-
-public:
-  zip_common(Iters &&... ts) : iterators(std::forward<Iters>(ts)...) {}
-
-  value_type operator*() { return deref(std::index_sequence_for<Iters...>{}); }
-
-  const value_type operator*() const {
-    return deref(std::index_sequence_for<Iters...>{});
-  }
-
-  ZipType &operator++() {
-    iterators = tup_inc(std::index_sequence_for<Iters...>{});
-    return *reinterpret_cast<ZipType *>(this);
-  }
-
-  ZipType &operator--() {
-    static_assert(Base::IsBidirectional,
-                  "All inner iterators must be at least bidirectional.");
-    iterators = tup_dec(std::index_sequence_for<Iters...>{});
-    return *reinterpret_cast<ZipType *>(this);
-  }
-};
-
-template <typename... Iters>
-struct zip_first : public zip_common<zip_first<Iters...>, Iters...> {
-  using Base = zip_common<zip_first<Iters...>, Iters...>;
-
-  bool operator==(const zip_first<Iters...> &other) const {
-    return std::get<0>(this->iterators) == std::get<0>(other.iterators);
-  }
-
-  zip_first(Iters &&... ts) : Base(std::forward<Iters>(ts)...) {}
-};
-
-template <typename... Iters>
-class zip_shortest : public zip_common<zip_shortest<Iters...>, Iters...> {
-  template <size_t... Ns>
-  bool test(const zip_shortest<Iters...> &other, std::index_sequence<Ns...>) const {
-    return all_of(std::initializer_list<bool>{std::get<Ns>(this->iterators) !=
-                                              std::get<Ns>(other.iterators)...},
-                  identity<bool>{});
-  }
-
-public:
-  using Base = zip_common<zip_shortest<Iters...>, Iters...>;
-
-  zip_shortest(Iters &&... ts) : Base(std::forward<Iters>(ts)...) {}
-
-  bool operator==(const zip_shortest<Iters...> &other) const {
-    return !test(other, std::index_sequence_for<Iters...>{});
-  }
-};
-
-template <template <typename...> class ItType, typename... Args> class zippy {
-public:
-  using iterator = ItType<decltype(std::begin(std::declval<Args>()))...>;
-  using iterator_category = typename iterator::iterator_category;
-  using value_type = typename iterator::value_type;
-  using difference_type = typename iterator::difference_type;
-  using pointer = typename iterator::pointer;
-  using reference = typename iterator::reference;
-
-private:
-  std::tuple<Args...> ts;
-
-  template <size_t... Ns> iterator begin_impl(std::index_sequence<Ns...>) const {
-    return iterator(std::begin(std::get<Ns>(ts))...);
-  }
-  template <size_t... Ns> iterator end_impl(std::index_sequence<Ns...>) const {
-    return iterator(std::end(std::get<Ns>(ts))...);
-  }
-
-public:
-  zippy(Args &&... ts_) : ts(std::forward<Args>(ts_)...) {}
-
-  iterator begin() const { return begin_impl(std::index_sequence_for<Args...>{}); }
-  iterator end() const { return end_impl(std::index_sequence_for<Args...>{}); }
-};
-
-} // end namespace detail
-
-/// zip iterator for two or more iteratable types.
-template <typename T, typename U, typename... Args>
-detail::zippy<detail::zip_shortest, T, U, Args...> zip(T &&t, U &&u,
-                                                       Args &&... args) {
-  return detail::zippy<detail::zip_shortest, T, U, Args...>(
-      std::forward<T>(t), std::forward<U>(u), std::forward<Args>(args)...);
-}
-
-/// zip iterator that, for the sake of efficiency, assumes the first iteratee to
-/// be the shortest.
-template <typename T, typename U, typename... Args>
-detail::zippy<detail::zip_first, T, U, Args...> zip_first(T &&t, U &&u,
-                                                          Args &&... args) {
-  return detail::zippy<detail::zip_first, T, U, Args...>(
-      std::forward<T>(t), std::forward<U>(u), std::forward<Args>(args)...);
-}
-
-namespace detail {
-template <typename Iter>
-static Iter next_or_end(const Iter &I, const Iter &End) {
-  if (I == End)
-    return End;
-  return std::next(I);
-}
-
-template <typename Iter>
-static auto deref_or_none(const Iter &I, const Iter &End)
-    -> std::optional<typename std::remove_const<
-        typename std::remove_reference<decltype(*I)>::type>::type> {
-  if (I == End)
-    return std::nullopt;
-  return *I;
-}
-
-template <typename Iter> struct ZipLongestItemType {
-  using type =
-      std::optional<typename std::remove_const<typename std::remove_reference<
-          decltype(*std::declval<Iter>())>::type>::type>;
-};
-
-template <typename... Iters> struct ZipLongestTupleType {
-  using type = std::tuple<typename ZipLongestItemType<Iters>::type...>;
-};
-
-template <typename... Iters>
-class zip_longest_iterator
-    : public iterator_facade_base<
-          zip_longest_iterator<Iters...>,
-          typename std::common_type<
-              std::forward_iterator_tag,
-              typename std::iterator_traits<Iters>::iterator_category...>::type,
-          typename ZipLongestTupleType<Iters...>::type,
-          typename std::iterator_traits<typename std::tuple_element<
-              0, std::tuple<Iters...>>::type>::difference_type,
-          typename ZipLongestTupleType<Iters...>::type *,
-          typename ZipLongestTupleType<Iters...>::type> {
-public:
-  using value_type = typename ZipLongestTupleType<Iters...>::type;
-
-private:
-  std::tuple<Iters...> iterators;
-  std::tuple<Iters...> end_iterators;
-
-  template <size_t... Ns>
-  bool test(const zip_longest_iterator<Iters...> &other,
-            std::index_sequence<Ns...>) const {
-    return wpi::any_of(
-        std::initializer_list<bool>{std::get<Ns>(this->iterators) !=
-                                    std::get<Ns>(other.iterators)...},
-        identity<bool>{});
-  }
-
-  template <size_t... Ns> value_type deref(std::index_sequence<Ns...>) const {
-    return value_type(
-        deref_or_none(std::get<Ns>(iterators), std::get<Ns>(end_iterators))...);
-  }
-
-  template <size_t... Ns>
-  decltype(iterators) tup_inc(std::index_sequence<Ns...>) const {
-    return std::tuple<Iters...>(
-        next_or_end(std::get<Ns>(iterators), std::get<Ns>(end_iterators))...);
-  }
-
-public:
-  zip_longest_iterator(std::pair<Iters &&, Iters &&>... ts)
-      : iterators(std::forward<Iters>(ts.first)...),
-        end_iterators(std::forward<Iters>(ts.second)...) {}
-
-  value_type operator*() { return deref(std::index_sequence_for<Iters...>{}); }
-
-  value_type operator*() const { return deref(std::index_sequence_for<Iters...>{}); }
-
-  zip_longest_iterator<Iters...> &operator++() {
-    iterators = tup_inc(std::index_sequence_for<Iters...>{});
-    return *this;
-  }
-
-  bool operator==(const zip_longest_iterator<Iters...> &other) const {
-    return !test(other, std::index_sequence_for<Iters...>{});
-  }
-};
-
-template <typename... Args> class zip_longest_range {
-public:
-  using iterator =
-      zip_longest_iterator<decltype(adl_begin(std::declval<Args>()))...>;
-  using iterator_category = typename iterator::iterator_category;
-  using value_type = typename iterator::value_type;
-  using difference_type = typename iterator::difference_type;
-  using pointer = typename iterator::pointer;
-  using reference = typename iterator::reference;
-
-private:
-  std::tuple<Args...> ts;
-
-  template <size_t... Ns> iterator begin_impl(std::index_sequence<Ns...>) const {
-    return iterator(std::make_pair(adl_begin(std::get<Ns>(ts)),
-                                   adl_end(std::get<Ns>(ts)))...);
-  }
-
-  template <size_t... Ns> iterator end_impl(std::index_sequence<Ns...>) const {
-    return iterator(std::make_pair(adl_end(std::get<Ns>(ts)),
-                                   adl_end(std::get<Ns>(ts)))...);
-  }
-
-public:
-  zip_longest_range(Args &&... ts_) : ts(std::forward<Args>(ts_)...) {}
-
-  iterator begin() const { return begin_impl(std::index_sequence_for<Args...>{}); }
-  iterator end() const { return end_impl(std::index_sequence_for<Args...>{}); }
-};
-} // namespace detail
-
-/// Iterate over two or more iterators at the same time. Iteration continues
-/// until all iterators reach the end. The std::optional only contains a value
-/// if the iterator has not reached the end.
-template <typename T, typename U, typename... Args>
-detail::zip_longest_range<T, U, Args...> zip_longest(T &&t, U &&u,
-                                                     Args &&... args) {
-  return detail::zip_longest_range<T, U, Args...>(
-      std::forward<T>(t), std::forward<U>(u), std::forward<Args>(args)...);
-}
-
-/// Iterator wrapper that concatenates sequences together.
-///
-/// This can concatenate different iterators, even with different types, into
-/// a single iterator provided the value types of all the concatenated
-/// iterators expose `reference` and `pointer` types that can be converted to
-/// `ValueT &` and `ValueT *` respectively. It doesn't support more
-/// interesting/customized pointer or reference types.
-///
-/// Currently this only supports forward or higher iterator categories as
-/// inputs and always exposes a forward iterator interface.
-template <typename ValueT, typename... IterTs>
-class concat_iterator
-    : public iterator_facade_base<concat_iterator<ValueT, IterTs...>,
-                                  std::forward_iterator_tag, ValueT> {
-  using BaseT = typename concat_iterator::iterator_facade_base;
-
-  /// We store both the current and end iterators for each concatenated
-  /// sequence in a tuple of pairs.
-  ///
-  /// Note that something like iterator_range seems nice at first here, but the
-  /// range properties are of little benefit and end up getting in the way
-  /// because we need to do mutation on the current iterators.
-  std::tuple<IterTs...> Begins;
-  std::tuple<IterTs...> Ends;
-
-  /// Attempts to increment a specific iterator.
-  ///
-  /// Returns true if it was able to increment the iterator. Returns false if
-  /// the iterator is already at the end iterator.
-  template <size_t Index> bool incrementHelper() {
-    auto &Begin = std::get<Index>(Begins);
-    auto &End = std::get<Index>(Ends);
-    if (Begin == End)
-      return false;
-
-    ++Begin;
-    return true;
-  }
-
-  /// Increments the first non-end iterator.
-  ///
-  /// It is an error to call this with all iterators at the end.
-  template <size_t... Ns> void increment(std::index_sequence<Ns...>) {
-    // Build a sequence of functions to increment each iterator if possible.
-    bool (concat_iterator::*IncrementHelperFns[])() = {
-        &concat_iterator::incrementHelper<Ns>...};
-
-    // Loop over them, and stop as soon as we succeed at incrementing one.
-    for (auto &IncrementHelperFn : IncrementHelperFns)
-      if ((this->*IncrementHelperFn)())
-        return;
-
-    assert(false && "Attempted to increment an end concat iterator!");
-  }
-
-  /// Returns null if the specified iterator is at the end. Otherwise,
-  /// dereferences the iterator and returns the address of the resulting
-  /// reference.
-  template <size_t Index> ValueT *getHelper() const {
-    auto &Begin = std::get<Index>(Begins);
-    auto &End = std::get<Index>(Ends);
-    if (Begin == End)
-      return nullptr;
-
-    return &*Begin;
-  }
-
-  /// Finds the first non-end iterator, dereferences, and returns the resulting
-  /// reference.
-  ///
-  /// It is an error to call this with all iterators at the end.
-  template <size_t... Ns> ValueT &get(std::index_sequence<Ns...>) const {
-    // Build a sequence of functions to get from iterator if possible.
-    ValueT *(concat_iterator::*GetHelperFns[])() const = {
-        &concat_iterator::getHelper<Ns>...};
-
-    // Loop over them, and return the first result we find.
-    for (auto &GetHelperFn : GetHelperFns)
-      if (ValueT *P = (this->*GetHelperFn)())
-        return *P;
-
-    assert(false && "Attempted to get a pointer from an end concat iterator!");
-  }
-
-public:
-  /// Constructs an iterator from a squence of ranges.
-  ///
-  /// We need the full range to know how to switch between each of the
-  /// iterators.
-  template <typename... RangeTs>
-  explicit concat_iterator(RangeTs &&... Ranges)
-      : Begins(std::begin(Ranges)...), Ends(std::end(Ranges)...) {}
-
-  using BaseT::operator++;
-
-  concat_iterator &operator++() {
-    increment(std::index_sequence_for<IterTs...>());
-    return *this;
-  }
-
-  ValueT &operator*() const { return get(std::index_sequence_for<IterTs...>()); }
-
-  bool operator==(const concat_iterator &RHS) const {
-    return Begins == RHS.Begins && Ends == RHS.Ends;
-  }
-};
-
-namespace detail {
-
-/// Helper to store a sequence of ranges being concatenated and access them.
-///
-/// This is designed to facilitate providing actual storage when temporaries
-/// are passed into the constructor such that we can use it as part of range
-/// based for loops.
-template <typename ValueT, typename... RangeTs> class concat_range {
-public:
-  using iterator =
-      concat_iterator<ValueT,
-                      decltype(std::begin(std::declval<RangeTs &>()))...>;
-
-private:
-  std::tuple<RangeTs...> Ranges;
-
-  template <size_t... Ns> iterator begin_impl(std::index_sequence<Ns...>) {
-    return iterator(std::get<Ns>(Ranges)...);
-  }
-  template <size_t... Ns> iterator end_impl(std::index_sequence<Ns...>) {
-    return iterator(make_range(std::end(std::get<Ns>(Ranges)),
-                               std::end(std::get<Ns>(Ranges)))...);
-  }
-
-public:
-  concat_range(RangeTs &&... Ranges)
-      : Ranges(std::forward<RangeTs>(Ranges)...) {}
-
-  iterator begin() { return begin_impl(std::index_sequence_for<RangeTs...>{}); }
-  iterator end() { return end_impl(std::index_sequence_for<RangeTs...>{}); }
-};
-
-} // end namespace detail
-
-/// Concatenated range across two or more ranges.
-///
-/// The desired value type must be explicitly specified.
-template <typename ValueT, typename... RangeTs>
-detail::concat_range<ValueT, RangeTs...> concat(RangeTs &&... Ranges) {
-  static_assert(sizeof...(RangeTs) > 1,
-                "Need more than one range to concatenate!");
-  return detail::concat_range<ValueT, RangeTs...>(
-      std::forward<RangeTs>(Ranges)...);
-}
-
-//===----------------------------------------------------------------------===//
-//     Extra additions to <utility>
-//===----------------------------------------------------------------------===//
-
-/// Function object to check whether the first component of a std::pair
-/// compares less than the first component of another std::pair.
-struct less_first {
-  template <typename T> bool operator()(const T &lhs, const T &rhs) const {
-    return lhs.first < rhs.first;
-  }
-};
-
-/// Function object to check whether the second component of a std::pair
-/// compares less than the second component of another std::pair.
-struct less_second {
-  template <typename T> bool operator()(const T &lhs, const T &rhs) const {
-    return lhs.second < rhs.second;
-  }
-};
-
-/// \brief Function object to apply a binary function to the first component of
-/// a std::pair.
-template<typename FuncTy>
-struct on_first {
-  FuncTy func;
-
-  template <typename T>
-  auto operator()(const T &lhs, const T &rhs) const
-      -> decltype(func(lhs.first, rhs.first)) {
-    return func(lhs.first, rhs.first);
-  }
-};
-
-/// Utility type to build an inheritance chain that makes it easy to rank
-/// overload candidates.
-template <int N> struct rank : rank<N - 1> {};
-template <> struct rank<0> {};
-
-/// traits class for checking whether type T is one of any of the given
-/// types in the variadic list.
-template <typename T, typename... Ts> struct is_one_of {
-  static const bool value = false;
-};
-
-template <typename T, typename U, typename... Ts>
-struct is_one_of<T, U, Ts...> {
-  static const bool value =
-      std::is_same<T, U>::value || is_one_of<T, Ts...>::value;
-};
-
-/// traits class for checking whether type T is a base class for all
-///  the given types in the variadic list.
-template <typename T, typename... Ts> struct are_base_of {
-  static const bool value = true;
-};
-
-template <typename T, typename U, typename... Ts>
-struct are_base_of<T, U, Ts...> {
-  static const bool value =
-      std::is_base_of<T, U>::value && are_base_of<T, Ts...>::value;
-};
-
-//===----------------------------------------------------------------------===//
-//     Extra additions for arrays
-//===----------------------------------------------------------------------===//
-
-/// Find the length of an array.
-template <class T, std::size_t N>
-constexpr inline size_t array_lengthof(T (&)[N]) {
-  return N;
-}
-
-/// Adapt std::less<T> for array_pod_sort.
-template<typename T>
-inline int array_pod_sort_comparator(const void *P1, const void *P2) {
-  if (std::less<T>()(*reinterpret_cast<const T*>(P1),
-                     *reinterpret_cast<const T*>(P2)))
-    return -1;
-  if (std::less<T>()(*reinterpret_cast<const T*>(P2),
-                     *reinterpret_cast<const T*>(P1)))
-    return 1;
-  return 0;
-}
-
-/// get_array_pod_sort_comparator - This is an internal helper function used to
-/// get type deduction of T right.
-template<typename T>
-inline int (*get_array_pod_sort_comparator(const T &))
-             (const void*, const void*) {
-  return array_pod_sort_comparator<T>;
-}
-
-/// array_pod_sort - This sorts an array with the specified start and end
-/// extent.  This is just like std::sort, except that it calls qsort instead of
-/// using an inlined template.  qsort is slightly slower than std::sort, but
-/// most sorts are not performance critical in LLVM and std::sort has to be
-/// template instantiated for each type, leading to significant measured code
-/// bloat.  This function should generally be used instead of std::sort where
-/// possible.
-///
-/// This function assumes that you have simple POD-like types that can be
-/// compared with std::less and can be moved with memcpy.  If this isn't true,
-/// you should use std::sort.
-///
-/// NOTE: If qsort_r were portable, we could allow a custom comparator and
-/// default to std::less.
-template<class IteratorTy>
-inline void array_pod_sort(IteratorTy Start, IteratorTy End) {
-  // Don't inefficiently call qsort with one element or trigger undefined
-  // behavior with an empty sequence.
-  auto NElts = End - Start;
-  if (NElts <= 1) return;
-  qsort(&*Start, NElts, sizeof(*Start), get_array_pod_sort_comparator(*Start));
-}
-
-template <class IteratorTy>
-inline void array_pod_sort(
-    IteratorTy Start, IteratorTy End,
-    int (*Compare)(
-        const typename std::iterator_traits<IteratorTy>::value_type *,
-        const typename std::iterator_traits<IteratorTy>::value_type *)) {
-  // Don't inefficiently call qsort with one element or trigger undefined
-  // behavior with an empty sequence.
-  auto NElts = End - Start;
-  if (NElts <= 1) return;
-  qsort(&*Start, NElts, sizeof(*Start),
-        reinterpret_cast<int (*)(const void *, const void *)>(Compare));
-}
-
-//===----------------------------------------------------------------------===//
-//     Extra additions to <algorithm>
-//===----------------------------------------------------------------------===//
-
-/// For a container of pointers, deletes the pointers and then clears the
-/// container.
-template<typename Container>
-void DeleteContainerPointers(Container &C) {
-  for (auto V : C)
-    delete V;
-  C.clear();
-}
-
-/// In a container of pairs (usually a map) whose second element is a pointer,
-/// deletes the second elements and then clears the container.
-template<typename Container>
-void DeleteContainerSeconds(Container &C) {
-  for (auto &V : C)
-    delete V.second;
-  C.clear();
-}
-
-/// Get the size of a range. This is a wrapper function around std::distance
-/// which is only enabled when the operation is O(1).
-template <typename R>
-auto size(R &&Range, typename std::enable_if<
-                         std::is_same<typename std::iterator_traits<decltype(
-                                          Range.begin())>::iterator_category,
-                                      std::random_access_iterator_tag>::value,
-                         void>::type * = nullptr)
-    -> decltype(std::distance(Range.begin(), Range.end())) {
-  return std::distance(Range.begin(), Range.end());
-}
-
-/// Provide wrappers to std::for_each which take ranges instead of having to
-/// pass begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-UnaryPredicate for_each(R &&Range, UnaryPredicate P) {
-  return std::for_each(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::all_of which take ranges instead of having to pass
-/// begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-bool all_of(R &&Range, UnaryPredicate P) {
-  return std::all_of(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::any_of which take ranges instead of having to pass
-/// begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-bool any_of(R &&Range, UnaryPredicate P) {
-  return std::any_of(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::none_of which take ranges instead of having to pass
-/// begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-bool none_of(R &&Range, UnaryPredicate P) {
-  return std::none_of(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::find which take ranges instead of having to pass
-/// begin/end explicitly.
-template <typename R, typename T>
-auto find(R &&Range, const T &Val) -> decltype(adl_begin(Range)) {
-  return std::find(adl_begin(Range), adl_end(Range), Val);
-}
-
-/// Provide wrappers to std::find_if which take ranges instead of having to pass
-/// begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-auto find_if(R &&Range, UnaryPredicate P) -> decltype(adl_begin(Range)) {
-  return std::find_if(adl_begin(Range), adl_end(Range), P);
-}
-
-template <typename R, typename UnaryPredicate>
-auto find_if_not(R &&Range, UnaryPredicate P) -> decltype(adl_begin(Range)) {
-  return std::find_if_not(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::remove_if which take ranges instead of having to
-/// pass begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-auto remove_if(R &&Range, UnaryPredicate P) -> decltype(adl_begin(Range)) {
-  return std::remove_if(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::copy_if which take ranges instead of having to
-/// pass begin/end explicitly.
-template <typename R, typename OutputIt, typename UnaryPredicate>
-OutputIt copy_if(R &&Range, OutputIt Out, UnaryPredicate P) {
-  return std::copy_if(adl_begin(Range), adl_end(Range), Out, P);
-}
-
-template <typename R, typename OutputIt>
-OutputIt copy(R &&Range, OutputIt Out) {
-  return std::copy(adl_begin(Range), adl_end(Range), Out);
-}
-
-/// Wrapper function around std::find to detect if an element exists
-/// in a container.
-template <typename R, typename E>
-bool is_contained(R &&Range, const E &Element) {
-  return std::find(adl_begin(Range), adl_end(Range), Element) != adl_end(Range);
-}
-
-/// Wrapper function around std::count to count the number of times an element
-/// \p Element occurs in the given range \p Range.
-template <typename R, typename E>
-auto count(R &&Range, const E &Element) ->
-    typename std::iterator_traits<decltype(adl_begin(Range))>::difference_type {
-  return std::count(adl_begin(Range), adl_end(Range), Element);
-}
-
-/// Wrapper function around std::count_if to count the number of times an
-/// element satisfying a given predicate occurs in a range.
-template <typename R, typename UnaryPredicate>
-auto count_if(R &&Range, UnaryPredicate P) ->
-    typename std::iterator_traits<decltype(adl_begin(Range))>::difference_type {
-  return std::count_if(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Wrapper function around std::transform to apply a function to a range and
-/// store the result elsewhere.
-template <typename R, typename OutputIt, typename UnaryPredicate>
-OutputIt transform(R &&Range, OutputIt d_first, UnaryPredicate P) {
-  return std::transform(adl_begin(Range), adl_end(Range), d_first, P);
-}
-
-/// Provide wrappers to std::partition which take ranges instead of having to
-/// pass begin/end explicitly.
-template <typename R, typename UnaryPredicate>
-auto partition(R &&Range, UnaryPredicate P) -> decltype(adl_begin(Range)) {
-  return std::partition(adl_begin(Range), adl_end(Range), P);
-}
-
-/// Provide wrappers to std::lower_bound which take ranges instead of having to
-/// pass begin/end explicitly.
-template <typename R, typename ForwardIt>
-auto lower_bound(R &&Range, ForwardIt I) -> decltype(adl_begin(Range)) {
-  return std::lower_bound(adl_begin(Range), adl_end(Range), I);
-}
-
-template <typename R, typename ForwardIt, typename Compare>
-auto lower_bound(R &&Range, ForwardIt I, Compare C)
-    -> decltype(adl_begin(Range)) {
-  return std::lower_bound(adl_begin(Range), adl_end(Range), I, C);
-}
-
-/// Provide wrappers to std::upper_bound which take ranges instead of having to
-/// pass begin/end explicitly.
-template <typename R, typename ForwardIt>
-auto upper_bound(R &&Range, ForwardIt I) -> decltype(adl_begin(Range)) {
-  return std::upper_bound(adl_begin(Range), adl_end(Range), I);
-}
-
-template <typename R, typename ForwardIt, typename Compare>
-auto upper_bound(R &&Range, ForwardIt I, Compare C)
-    -> decltype(adl_begin(Range)) {
-  return std::upper_bound(adl_begin(Range), adl_end(Range), I, C);
-}
-/// Wrapper function around std::equal to detect if all elements
-/// in a container are same.
-template <typename R>
-bool is_splat(R &&Range) {
-  size_t range_size = size(Range);
-  return range_size != 0 && (range_size == 1 ||
-         std::equal(adl_begin(Range) + 1, adl_end(Range), adl_begin(Range)));
-}
-
-/// Given a range of type R, iterate the entire range and return a
-/// SmallVector with elements of the vector.  This is useful, for example,
-/// when you want to iterate a range and then sort the results.
-template <unsigned Size, typename R>
-SmallVector<typename std::remove_const<detail::ValueOfRange<R>>::type, Size>
-to_vector(R &&Range) {
-  return {adl_begin(Range), adl_end(Range)};
-}
-
-/// Provide a container algorithm similar to C++ Library Fundamentals v2's
-/// `erase_if` which is equivalent to:
-///
-///   C.erase(remove_if(C, pred), C.end());
-///
-/// This version works for any container with an erase method call accepting
-/// two iterators.
-template <typename Container, typename UnaryPredicate>
-void erase_if(Container &C, UnaryPredicate P) {
-  C.erase(remove_if(C, P), C.end());
-}
-
-//===----------------------------------------------------------------------===//
-//     Extra additions to <memory>
-//===----------------------------------------------------------------------===//
-
-struct FreeDeleter {
-  void operator()(void* v) {
-    ::free(v);
-  }
-};
-
-template<typename First, typename Second>
-struct pair_hash {
-  size_t operator()(const std::pair<First, Second> &P) const {
-    return std::hash<First>()(P.first) * 31 + std::hash<Second>()(P.second);
-  }
-};
-
-/// Binary functor that adapts to any other binary functor after dereferencing
-/// operands.
-template <typename T> struct deref {
-  T func;
-
-  // Could be further improved to cope with non-derivable functors and
-  // non-binary functors (should be a variadic template member function
-  // operator()).
-  template <typename A, typename B>
-  auto operator()(A &lhs, B &rhs) const -> decltype(func(*lhs, *rhs)) {
-    assert(lhs);
-    assert(rhs);
-    return func(*lhs, *rhs);
-  }
-};
-
-namespace detail {
-
-template <typename R> class enumerator_iter;
-
-template <typename R> struct result_pair {
-  friend class enumerator_iter<R>;
-
-  result_pair() = default;
-  result_pair(std::size_t Index, IterOfRange<R> Iter)
-      : Index(Index), Iter(Iter) {}
-
-  result_pair<R> &operator=(const result_pair<R> &Other) {
-    Index = Other.Index;
-    Iter = Other.Iter;
-    return *this;
-  }
-
-  std::size_t index() const { return Index; }
-  const ValueOfRange<R> &value() const { return *Iter; }
-  ValueOfRange<R> &value() { return *Iter; }
-
-private:
-  std::size_t Index = (std::numeric_limits<std::size_t>::max)();
-  IterOfRange<R> Iter;
-};
-
-template <typename R>
-class enumerator_iter
-    : public iterator_facade_base<
-          enumerator_iter<R>, std::forward_iterator_tag, result_pair<R>,
-          typename std::iterator_traits<IterOfRange<R>>::difference_type,
-          typename std::iterator_traits<IterOfRange<R>>::pointer,
-          typename std::iterator_traits<IterOfRange<R>>::reference> {
-  using result_type = result_pair<R>;
-
-public:
-  explicit enumerator_iter(IterOfRange<R> EndIter)
-      : Result((std::numeric_limits<size_t>::max)(), EndIter) {}
-
-  enumerator_iter(std::size_t Index, IterOfRange<R> Iter)
-      : Result(Index, Iter) {}
-
-  result_type &operator*() { return Result; }
-  const result_type &operator*() const { return Result; }
-
-  enumerator_iter<R> &operator++() {
-    assert(Result.Index != (std::numeric_limits<size_t>::max)());
-    ++Result.Iter;
-    ++Result.Index;
-    return *this;
-  }
-
-  bool operator==(const enumerator_iter<R> &RHS) const {
-    // Don't compare indices here, only iterators.  It's possible for an end
-    // iterator to have different indices depending on whether it was created
-    // by calling std::end() versus incrementing a valid iterator.
-    return Result.Iter == RHS.Result.Iter;
-  }
-
-  enumerator_iter<R> &operator=(const enumerator_iter<R> &Other) {
-    Result = Other.Result;
-    return *this;
-  }
-
-private:
-  result_type Result;
-};
-
-template <typename R> class enumerator {
-public:
-  explicit enumerator(R &&Range) : TheRange(std::forward<R>(Range)) {}
-
-  enumerator_iter<R> begin() {
-    return enumerator_iter<R>(0, std::begin(TheRange));
-  }
-
-  enumerator_iter<R> end() {
-    return enumerator_iter<R>(std::end(TheRange));
-  }
-
-private:
-  R TheRange;
-};
-
-} // end namespace detail
-
-/// Given an input range, returns a new range whose values are are pair (A,B)
-/// such that A is the 0-based index of the item in the sequence, and B is
-/// the value from the original sequence.  Example:
-///
-/// std::vector<char> Items = {'A', 'B', 'C', 'D'};
-/// for (auto X : enumerate(Items)) {
-///   printf("Item %d - %c\n", X.index(), X.value());
-/// }
-///
-/// Output:
-///   Item 0 - A
-///   Item 1 - B
-///   Item 2 - C
-///   Item 3 - D
-///
-template <typename R> detail::enumerator<R> enumerate(R &&TheRange) {
-  return detail::enumerator<R>(std::forward<R>(TheRange));
-}
-
-/// Return true if the sequence [Begin, End) has exactly N items. Runs in O(N)
-/// time. Not meant for use with random-access iterators.
-template <typename IterTy>
-bool hasNItems(
-    IterTy &&Begin, IterTy &&End, unsigned N,
-    typename std::enable_if<
-        !std::is_same<
-            typename std::iterator_traits<typename std::remove_reference<
-                decltype(Begin)>::type>::iterator_category,
-            std::random_access_iterator_tag>::value,
-        void>::type * = nullptr) {
-  for (; N; --N, ++Begin)
-    if (Begin == End)
-      return false; // Too few.
-  return Begin == End;
-}
-
-/// Return true if the sequence [Begin, End) has N or more items. Runs in O(N)
-/// time. Not meant for use with random-access iterators.
-template <typename IterTy>
-bool hasNItemsOrMore(
-    IterTy &&Begin, IterTy &&End, unsigned N,
-    typename std::enable_if<
-        !std::is_same<
-            typename std::iterator_traits<typename std::remove_reference<
-                decltype(Begin)>::type>::iterator_category,
-            std::random_access_iterator_tag>::value,
-        void>::type * = nullptr) {
-  for (; N; --N, ++Begin)
-    if (Begin == End)
-      return false; // Too few.
-  return true;
-}
-
-} // end namespace wpi
-
-#endif // LLVM_ADT_STLEXTRAS_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SafeThread.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SafeThread.h
index d4b48f7..bf37732 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SafeThread.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SafeThread.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_SAFETHREAD_H_
 #define WPIUTIL_WPI_SAFETHREAD_H_
@@ -18,7 +15,9 @@
 
 namespace wpi {
 
-// Base class for SafeThreadOwner threads.
+/**
+ * Base class for SafeThreadOwner threads.
+ */
 class SafeThread {
  public:
   virtual ~SafeThread() = default;
@@ -32,7 +31,9 @@
 
 namespace detail {
 
-// Non-template proxy base class for common proxy code.
+/**
+ * Non-template proxy base class for common proxy code.
+ */
 class SafeThreadProxyBase {
  public:
   explicit SafeThreadProxyBase(std::shared_ptr<SafeThread> thr);
@@ -44,8 +45,11 @@
   std::unique_lock<wpi::mutex> m_lock;
 };
 
-// A proxy for SafeThread.
-// Also serves as a scoped lock on SafeThread::m_mutex.
+/**
+ * A proxy for SafeThread.
+ *
+ * Also serves as a scoped lock on SafeThread::m_mutex.
+ */
 template <typename T>
 class SafeThreadProxy : public SafeThreadProxyBase {
  public:
@@ -55,7 +59,9 @@
   T* operator->() const { return static_cast<T*>(m_thread.get()); }
 };
 
-// Non-template owner base class for common owner code.
+/**
+ * Non-template owner base class for common owner code.
+ */
 class SafeThreadOwnerBase {
  public:
   void Stop();
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Signal.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Signal.h
index 64b774c..d4993d9 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Signal.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Signal.h
@@ -565,7 +565,7 @@
      *         signal object, it does not cover thread safety of potentially
      *         shared state used in slot functions.
      *
-     * @param a... arguments to emit
+     * @param a arguments to emit
      */
     template <typename... A>
     void operator()(A && ... a) const {
@@ -581,7 +581,6 @@
      * Safety: Thread-safety depends on locking policy.
      *
      * @param c a callable
-     * @return a Connection object that can be used to interact with the slot
      */
     template <typename Callable>
     void connect(Callable && c) {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SmallString.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SmallString.h
index 32969b1..1d44f24 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SmallString.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SmallString.h
@@ -15,8 +15,8 @@
 #define WPIUTIL_WPI_SMALLSTRING_H
 
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
 #include <cstddef>
+#include <string_view>
 
 namespace wpi {
 
@@ -28,8 +28,9 @@
   /// Default ctor - Initialize to empty.
   SmallString() = default;
 
-  /// Initialize from a StringRef.
-  SmallString(StringRef S) : SmallVector<char, InternalLen>(S.begin(), S.end()) {}
+  /// Initialize from a std::string_view.
+  SmallString(std::string_view S)
+    : SmallVector<char, InternalLen>(S.begin(), S.end()) {}
 
   /// Initialize with a range.
   template<typename ItTy>
@@ -54,8 +55,8 @@
     SmallVectorImpl<char>::append(S, E);
   }
 
-  /// Assign from a StringRef.
-  void assign(StringRef RHS) {
+  /// Assign from a std::string_view.
+  void assign(std::string_view RHS) {
     this->clear();
     SmallVectorImpl<char>::append(RHS.begin(), RHS.end());
   }
@@ -80,8 +81,8 @@
     SmallVectorImpl<char>::append(NumInputs, Elt);
   }
 
-  /// Append from a StringRef.
-  void append(StringRef RHS) {
+  /// Append from a std::string_view.
+  void append(std::string_view RHS) {
     SmallVectorImpl<char>::append(RHS.begin(), RHS.end());
   }
 
@@ -94,48 +95,12 @@
   /// @name String Comparison
   /// @{
 
-  /// Check for string equality.  This is more efficient than compare() when
-  /// the relative ordering of inequal strings isn't needed.
-  bool equals(StringRef RHS) const {
-    return str().equals(RHS);
-  }
-
-  /// Check for string equality, ignoring case.
-  bool equals_lower(StringRef RHS) const {
-    return str().equals_lower(RHS);
-  }
-
   /// Compare two strings; the result is -1, 0, or 1 if this string is
   /// lexicographically less than, equal to, or greater than the \p RHS.
-  int compare(StringRef RHS) const {
+  int compare(std::string_view RHS) const {
     return str().compare(RHS);
   }
 
-  /// compare_lower - Compare two strings, ignoring case.
-  int compare_lower(StringRef RHS) const {
-    return str().compare_lower(RHS);
-  }
-
-  /// compare_numeric - Compare two strings, treating sequences of digits as
-  /// numbers.
-  int compare_numeric(StringRef RHS) const {
-    return str().compare_numeric(RHS);
-  }
-
-  /// @}
-  /// @name String Predicates
-  /// @{
-
-  /// startswith - Check if this string starts with the given \p Prefix.
-  bool startswith(StringRef Prefix) const {
-    return str().startswith(Prefix);
-  }
-
-  /// endswith - Check if this string ends with the given \p Suffix.
-  bool endswith(StringRef Suffix) const {
-    return str().endswith(Suffix);
-  }
-
   /// @}
   /// @name String Searching
   /// @{
@@ -152,7 +117,7 @@
   ///
   /// \returns The index of the first occurrence of \p Str, or npos if not
   /// found.
-  size_t find(StringRef Str, size_t From = 0) const {
+  size_t find(std::string_view Str, size_t From = 0) const {
     return str().find(Str, From);
   }
 
@@ -160,7 +125,7 @@
   ///
   /// \returns The index of the last occurrence of \p C, or npos if not
   /// found.
-  size_t rfind(char C, size_t From = StringRef::npos) const {
+  size_t rfind(char C, size_t From = std::string_view::npos) const {
     return str().rfind(C, From);
   }
 
@@ -168,7 +133,7 @@
   ///
   /// \returns The index of the last occurrence of \p Str, or npos if not
   /// found.
-  size_t rfind(StringRef Str) const {
+  size_t rfind(std::string_view Str) const {
     return str().rfind(Str);
   }
 
@@ -182,7 +147,7 @@
   /// not found.
   ///
   /// Complexity: O(size() + Chars.size())
-  size_t find_first_of(StringRef Chars, size_t From = 0) const {
+  size_t find_first_of(std::string_view Chars, size_t From = 0) const {
     return str().find_first_of(Chars, From);
   }
 
@@ -196,13 +161,13 @@
   /// \p Chars, or npos if not found.
   ///
   /// Complexity: O(size() + Chars.size())
-  size_t find_first_not_of(StringRef Chars, size_t From = 0) const {
+  size_t find_first_not_of(std::string_view Chars, size_t From = 0) const {
     return str().find_first_not_of(Chars, From);
   }
 
   /// Find the last character in the string that is \p C, or npos if not
   /// found.
-  size_t find_last_of(char C, size_t From = StringRef::npos) const {
+  size_t find_last_of(char C, size_t From = std::string_view::npos) const {
     return str().find_last_of(C, From);
   }
 
@@ -211,26 +176,11 @@
   ///
   /// Complexity: O(size() + Chars.size())
   size_t find_last_of(
-      StringRef Chars, size_t From = StringRef::npos) const {
+      std::string_view Chars, size_t From = std::string_view::npos) const {
     return str().find_last_of(Chars, From);
   }
 
   /// @}
-  /// @name Helpful Algorithms
-  /// @{
-
-  /// Return the number of occurrences of \p C in the string.
-  size_t count(char C) const {
-    return str().count(C);
-  }
-
-  /// Return the number of non-overlapped occurrences of \p Str in the
-  /// string.
-  size_t count(StringRef Str) const {
-    return str().count(Str);
-  }
-
-  /// @}
   /// @name Substring Operations
   /// @{
 
@@ -243,28 +193,17 @@
   /// \param N The number of characters to included in the substring. If \p N
   /// exceeds the number of characters remaining in the string, the string
   /// suffix (starting with \p Start) will be returned.
-  StringRef substr(size_t Start, size_t N = StringRef::npos) const {
+  std::string_view substr(size_t Start, size_t N = std::string_view::npos) const {
     return str().substr(Start, N);
   }
 
-  /// Return a reference to the substring from [Start, End).
-  ///
-  /// \param Start The index of the starting character in the substring; if
-  /// the index is npos or greater than the length of the string then the
-  /// empty substring will be returned.
-  ///
-  /// \param End The index following the last character to include in the
-  /// substring. If this is npos, or less than \p Start, or exceeds the
-  /// number of characters remaining in the string, the string suffix
-  /// (starting with \p Start) will be returned.
-  StringRef slice(size_t Start, size_t End) const {
-    return str().slice(Start, End);
-  }
-
   // Extra methods.
 
-  /// Explicit conversion to StringRef.
-  StringRef str() const { return StringRef(this->begin(), this->size()); }
+  /// Explicit conversion to std::string_view.
+  std::string_view str() const { return {this->begin(), this->size()}; }
+
+  /// Explicit conversion to std::string.
+  std::string string() const { return {this->begin(), this->size()}; }
 
   // TODO: Make this const, if it's safe...
   const char* c_str() {
@@ -273,16 +212,19 @@
     return this->data();
   }
 
-  /// Implicit conversion to StringRef.
-  operator StringRef() const { return str(); }
+  /// Implicit conversion to std::string_view.
+  operator std::string_view() const { return str(); }
+
+  /// Implicit conversion to std::string.
+  operator std::string() const { return string(); }
 
   // Extra operators.
-  const SmallString &operator=(StringRef RHS) {
+  const SmallString &operator=(std::string_view RHS) {
     this->clear();
     return *this += RHS;
   }
 
-  SmallString &operator+=(StringRef RHS) {
+  SmallString &operator+=(std::string_view RHS) {
     this->append(RHS.begin(), RHS.end());
     return *this;
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SocketError.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SocketError.h
index d5f3ff0..b893ac6 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SocketError.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SocketError.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_SOCKETERROR_H_
 #define WPIUTIL_WPI_SOCKETERROR_H_
@@ -16,7 +13,7 @@
 
 std::string SocketStrerror(int code);
 
-static inline std::string SocketStrerror() {
+inline std::string SocketStrerror() {
   return SocketStrerror(SocketErrno());
 }
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SpanExtras.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SpanExtras.h
new file mode 100644
index 0000000..41e4014
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SpanExtras.h
@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cassert>
+
+#include "wpi/span.h"
+
+namespace wpi {
+
+/// Drop the first \p N elements of the array.
+template <typename T>
+constexpr span<T> drop_front(span<T> in, typename span<T>::size_type n = 1) {
+  assert(in.size() >= n && "Dropping more elements than exist");
+  return in.subspan(n, in.size() - n);
+}
+
+/// Drop the last \p N elements of the array.
+template <typename T>
+constexpr span<T> drop_back(span<T> in, typename span<T>::size_type n = 1) {
+  assert(in.size() >= n && "Dropping more elements than exist");
+  return in.subspan(0, in.size() - n);
+}
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StackTrace.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StackTrace.h
index 56a3770..f9ee989 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StackTrace.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StackTrace.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_STACKTRACE_H_
 #define WPIUTIL_WPI_STACKTRACE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringExtras.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringExtras.h
index f420e2a..74874ff 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringExtras.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringExtras.h
@@ -1,3 +1,7 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
 //===- llvm/ADT/StringExtras.h - Useful string functions --------*- C++ -*-===//
 //
 //                     The LLVM Compiler Infrastructure
@@ -11,395 +15,651 @@
 //
 //===----------------------------------------------------------------------===//
 
-#ifndef WPIUTIL_WPI_STRINGEXTRAS_H
-#define WPIUTIL_WPI_STRINGEXTRAS_H
+#pragma once
 
-#include "wpi/ArrayRef.h"
-#include "wpi/SmallString.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
-#include <cassert>
-#include <cstddef>
-#include <cstdint>
-#include <cstdlib>
-#include <cstring>
-#include <iterator>
+#include <limits>
+#include <optional>
 #include <string>
+#include <string_view>
+#include <type_traits>
 #include <utility>
 
 namespace wpi {
 
-template<typename T> class SmallVectorImpl;
-class raw_ostream;
+template <typename T>
+class SmallVectorImpl;
 
 /// hexdigit - Return the hexadecimal character for the
 /// given number \p X (which should be less than 16).
-inline char hexdigit(unsigned X, bool LowerCase = false) {
+constexpr char hexdigit(unsigned X, bool LowerCase = false) noexcept {
   const char HexChar = LowerCase ? 'a' : 'A';
   return X < 10 ? '0' + X : HexChar + X - 10;
 }
 
-/// Construct a string ref from a boolean.
-inline StringRef toStringRef(bool B) { return StringRef(B ? "true" : "false"); }
-
-/// Construct a string ref from an array ref of unsigned chars.
-inline StringRef toStringRef(ArrayRef<uint8_t> Input) {
-  return StringRef(reinterpret_cast<const char *>(Input.begin()), Input.size());
-}
-
-/// Construct a string ref from an array ref of unsigned chars.
-inline ArrayRef<uint8_t> arrayRefFromStringRef(StringRef Input) {
-  return {Input.bytes_begin(), Input.bytes_end()};
-}
-
 /// Interpret the given character \p C as a hexadecimal digit and return its
 /// value.
 ///
 /// If \p C is not a valid hex digit, -1U is returned.
-inline unsigned hexDigitValue(char C) {
-  if (C >= '0' && C <= '9') return C-'0';
-  if (C >= 'a' && C <= 'f') return C-'a'+10U;
-  if (C >= 'A' && C <= 'F') return C-'A'+10U;
+constexpr unsigned hexDigitValue(char C) noexcept {
+  if (C >= '0' && C <= '9') {
+    return C - '0';
+  }
+  if (C >= 'a' && C <= 'f') {
+    return C - 'a' + 10U;
+  }
+  if (C >= 'A' && C <= 'F') {
+    return C - 'A' + 10U;
+  }
   return (std::numeric_limits<unsigned>::max)();
 }
 
 /// Checks if character \p C is one of the 10 decimal digits.
-inline bool isDigit(char C) { return C >= '0' && C <= '9'; }
+constexpr bool isDigit(char C) noexcept {
+  return C >= '0' && C <= '9';
+}
 
 /// Checks if character \p C is a hexadecimal numeric character.
-inline bool isHexDigit(char C) { return hexDigitValue(C) != (std::numeric_limits<unsigned>::max)(); }
+constexpr bool isHexDigit(char C) noexcept {
+  return hexDigitValue(C) != (std::numeric_limits<unsigned>::max)();
+}
 
 /// Checks if character \p C is a valid letter as classified by "C" locale.
-inline bool isAlpha(char C) {
+constexpr bool isAlpha(char C) noexcept {
   return ('a' <= C && C <= 'z') || ('A' <= C && C <= 'Z');
 }
 
 /// Checks whether character \p C is either a decimal digit or an uppercase or
 /// lowercase letter as classified by "C" locale.
-inline bool isAlnum(char C) { return isAlpha(C) || isDigit(C); }
+constexpr bool isAlnum(char C) noexcept {
+  return isAlpha(C) || isDigit(C);
+}
 
 /// Checks whether character \p C is valid ASCII (high bit is zero).
-inline bool isASCII(char C) { return static_cast<unsigned char>(C) <= 127; }
-
-/// Checks whether all characters in S are ASCII.
-inline bool isASCII(wpi::StringRef S) {
-  for (char C : S)
-    if (LLVM_UNLIKELY(!isASCII(C)))
-      return false;
-  return true;
+constexpr bool isASCII(char C) noexcept {
+  return static_cast<unsigned char>(C) <= 127;
 }
 
 /// Checks whether character \p C is printable.
 ///
 /// Locale-independent version of the C standard library isprint whose results
 /// may differ on different platforms.
-inline bool isPrint(char C) {
+constexpr bool isPrint(char C) noexcept {
   unsigned char UC = static_cast<unsigned char>(C);
   return (0x20 <= UC) && (UC <= 0x7E);
 }
 
 /// Returns the corresponding lowercase character if \p x is uppercase.
-inline char toLower(char x) {
-  if (x >= 'A' && x <= 'Z')
+constexpr char toLower(char x) noexcept {
+  if (x >= 'A' && x <= 'Z') {
     return x - 'A' + 'a';
+  }
   return x;
 }
 
 /// Returns the corresponding uppercase character if \p x is lowercase.
-inline char toUpper(char x) {
-  if (x >= 'a' && x <= 'z')
+constexpr char toUpper(char x) noexcept {
+  if (x >= 'a' && x <= 'z') {
     return x - 'a' + 'A';
+  }
   return x;
 }
 
-inline std::string utohexstr(uint64_t X, bool LowerCase = false) {
-  char Buffer[17];
-  char *BufPtr = std::end(Buffer);
+inline std::string utohexstr(unsigned long long val,  // NOLINT(runtime/int)
+                             bool lowerCase = false) {
+  char buf[17];
+  char* bufptr = std::end(buf);
 
-  if (X == 0) *--BufPtr = '0';
-
-  while (X) {
-    unsigned char Mod = static_cast<unsigned char>(X) & 15;
-    *--BufPtr = hexdigit(Mod, LowerCase);
-    X >>= 4;
+  if (val == 0) {
+    *--bufptr = '0';
   }
 
-  return std::string(BufPtr, std::end(Buffer));
-}
-
-/// Convert buffer \p Input to its hexadecimal representation.
-/// The returned string is double the size of \p Input.
-inline std::string toHex(StringRef Input, bool LowerCase = false) {
-  static const char *const LUT = "0123456789ABCDEF";
-  const uint8_t Offset = LowerCase ? 32 : 0;
-  size_t Length = Input.size();
-
-  std::string Output;
-  Output.reserve(2 * Length);
-  for (size_t i = 0; i < Length; ++i) {
-    const unsigned char c = Input[i];
-    Output.push_back(LUT[c >> 4] | Offset);
-    Output.push_back(LUT[c & 15] | Offset);
-  }
-  return Output;
-}
-
-inline std::string toHex(ArrayRef<uint8_t> Input, bool LowerCase = false) {
-  return toHex(toStringRef(Input), LowerCase);
-}
-
-inline uint8_t hexFromNibbles(char MSB, char LSB) {
-  unsigned U1 = hexDigitValue(MSB);
-  unsigned U2 = hexDigitValue(LSB);
-  assert(U1 != (std::numeric_limits<unsigned>::max)() && U2 != (std::numeric_limits<unsigned>::max)());
-
-  return static_cast<uint8_t>((U1 << 4) | U2);
-}
-
-/// Convert hexadecimal string \p Input to its binary representation.
-/// The return string is half the size of \p Input.
-inline std::string fromHex(StringRef Input) {
-  if (Input.empty())
-    return std::string();
-
-  std::string Output;
-  Output.reserve((Input.size() + 1) / 2);
-  if (Input.size() % 2 == 1) {
-    Output.push_back(hexFromNibbles('0', Input.front()));
-    Input = Input.drop_front();
+  while (val) {
+    unsigned char mod = static_cast<unsigned char>(val) & 15;
+    *--bufptr = hexdigit(mod, lowerCase);
+    val >>= 4;
   }
 
-  assert(Input.size() % 2 == 0);
-  while (!Input.empty()) {
-    uint8_t Hex = hexFromNibbles(Input[0], Input[1]);
-    Output.push_back(Hex);
-    Input = Input.drop_front(2);
-  }
-  return Output;
+  return std::string(bufptr, std::end(buf));
 }
 
-/// Convert the string \p S to an integer of the specified type using
-/// the radix \p Base.  If \p Base is 0, auto-detects the radix.
-/// Returns true if the number was successfully converted, false otherwise.
-template <typename N> bool to_integer(StringRef S, N &Num, unsigned Base = 0) {
-  return !S.getAsInteger(Base, Num);
+/**
+ * equals - Check for string equality, this is more efficient than
+ * compare() when the relative ordering of inequal strings isn't needed.
+ */
+constexpr bool equals(std::string_view lhs, std::string_view rhs) noexcept {
+  auto length = lhs.size();
+  return length == rhs.size() && std::string_view::traits_type::compare(
+                                     lhs.data(), rhs.data(), length) == 0;
+}
+
+/**
+ * compare_lower - Compare two strings, ignoring case.
+ */
+int compare_lower(std::string_view lhs, std::string_view rhs) noexcept;
+
+/**
+ * equals_lower - Check for string equality, ignoring case.
+ */
+constexpr bool equals_lower(std::string_view lhs,
+                            std::string_view rhs) noexcept {
+  return lhs.size() == rhs.size() && compare_lower(lhs, rhs) == 0;
+}
+
+/**
+ * Search for the first character @p ch in @p str, ignoring case.
+ *
+ * @returns The index of the first occurrence of @p ch, or npos if not
+ * found.
+ */
+std::string_view::size_type find_lower(
+    std::string_view str, char ch,
+    std::string_view::size_type from = 0) noexcept;
+
+/**
+ * Search for the first string @p other in @p str, ignoring case.
+ *
+ * @returns The index of the first occurrence of @p other, or npos if not
+ * found.
+ */
+std::string_view::size_type find_lower(
+    std::string_view str, std::string_view other,
+    std::string_view::size_type from = 0) noexcept;
+
+/**
+ * Search for the first string @p other in @p str, ignoring case.
+ *
+ * @returns The index of the first occurrence of @p other, or npos if not
+ * found.
+ */
+inline std::string_view::size_type find_lower(
+    std::string_view str, const char* other,
+    std::string_view::size_type from = 0) noexcept {
+  return find_lower(str, std::string_view{other}, from);
+}
+
+/**
+ * Search for the last character @p ch in @p str, ignoring case.
+ *
+ * @returns The index of the last occurrence of @p ch, or npos if not
+ * found.
+ */
+std::string_view::size_type rfind_lower(
+    std::string_view str, char ch,
+    std::string_view::size_type from = std::string_view::npos) noexcept;
+
+/**
+ * Search for the last string @p other in @p str, ignoring case.
+ *
+ * @returns The index of the last occurrence of @p other, or npos if not
+ * found.
+ */
+std::string_view::size_type rfind_lower(std::string_view str,
+                                        std::string_view other) noexcept;
+
+/**
+ * Search for the last string @p other in @p str, ignoring case.
+ *
+ * @returns The index of the last occurrence of @p other, or npos if not
+ * found.
+ */
+inline std::string_view::size_type rfind_lower(std::string_view str,
+                                               const char* other) noexcept {
+  return rfind_lower(str, std::string_view{other});
+}
+
+/**
+ * Checks if @p str starts with the given @p prefix.
+ */
+constexpr bool starts_with(std::string_view str,
+                           std::string_view prefix) noexcept {
+  return str.substr(0, prefix.size()) == prefix;
+}
+
+/**
+ * Checks if @p str starts with the given @p prefix.
+ */
+constexpr bool starts_with(std::string_view str, char prefix) noexcept {
+  return !str.empty() && std::string_view::traits_type::eq(str.front(), prefix);
+}
+
+/**
+ * Checks if @p str starts with the given @p prefix.
+ */
+constexpr bool starts_with(std::string_view str, const char* prefix) noexcept {
+  return starts_with(str, std::string_view(prefix));
+}
+
+/**
+ * Checks if @p str starts with the given @p prefix, ignoring case.
+ */
+bool starts_with_lower(std::string_view str, std::string_view prefix) noexcept;
+
+/**
+ * Checks if @p str starts with the given @p prefix, ignoring case.
+ */
+constexpr bool starts_with_lower(std::string_view str, char prefix) noexcept {
+  return !str.empty() && toLower(str.front()) == toLower(prefix);
+}
+
+/**
+ * Checks if @p str starts with the given @p prefix, ignoring case.
+ */
+inline bool starts_with_lower(std::string_view str,
+                              const char* prefix) noexcept {
+  return starts_with_lower(str, std::string_view(prefix));
+}
+
+/**
+ * Checks if @p str ends with the given @p suffix.
+ */
+constexpr bool ends_with(std::string_view str,
+                         std::string_view suffix) noexcept {
+  return str.size() >= suffix.size() &&
+         str.compare(str.size() - suffix.size(), std::string_view::npos,
+                     suffix) == 0;
+}
+
+/**
+ * Checks if @p str ends with the given @p suffix.
+ */
+constexpr bool ends_with(std::string_view str, char suffix) noexcept {
+  return !str.empty() && std::string_view::traits_type::eq(str.back(), suffix);
+}
+
+/**
+ * Checks if @p str ends with the given @p suffix.
+ */
+constexpr bool ends_with(std::string_view str, const char* suffix) noexcept {
+  return ends_with(str, std::string_view(suffix));
+}
+
+/**
+ * Checks if @p str ends with the given @p suffix, ignoring case.
+ */
+bool ends_with_lower(std::string_view str, std::string_view suffix) noexcept;
+
+/**
+ * Checks if @p str ends with the given @p suffix, ignoring case.
+ */
+constexpr bool ends_with_lower(std::string_view str, char suffix) noexcept {
+  return !str.empty() && toLower(str.back()) == toLower(suffix);
+}
+
+/**
+ * Checks if @p str ends with the given @p suffix, ignoring case.
+ */
+inline bool ends_with_lower(std::string_view str, const char* suffix) noexcept {
+  return ends_with_lower(str, std::string_view(suffix));
+}
+
+/**
+ * Checks if @p str contains the substring @p other.
+ */
+constexpr bool contains(std::string_view str, std::string_view other) noexcept {
+  return str.find(other) != std::string_view::npos;
+}
+
+/**
+ * Checks if @p str contains the substring @p other.
+ */
+constexpr bool contains(std::string_view str, char ch) noexcept {
+  return str.find(ch) != std::string_view::npos;
+}
+
+/**
+ * Checks if @p str contains the substring @p other.
+ */
+constexpr bool contains(std::string_view str, const char* other) noexcept {
+  return str.find(other) != std::string_view::npos;
+}
+
+/**
+ * Checks if @p str contains the substring @p other, ignoring case.
+ */
+inline bool contains_lower(std::string_view str,
+                           std::string_view other) noexcept {
+  return find_lower(str, other) != std::string_view::npos;
+}
+
+/**
+ * Checks if @p str contains the substring @p other, ignoring case.
+ */
+inline bool contains_lower(std::string_view str, char ch) noexcept {
+  return find_lower(str, ch) != std::string_view::npos;
+}
+
+/**
+ * Checks if @p str contains the substring @p other, ignoring case.
+ */
+inline bool contains_lower(std::string_view str, const char* other) noexcept {
+  return find_lower(str, other) != std::string_view::npos;
+}
+
+/**
+ * Return a string_view equal to @p str but with the first @p n elements
+ * dropped.
+ */
+constexpr std::string_view drop_front(
+    std::string_view str, std::string_view::size_type n = 1) noexcept {
+  str.remove_prefix(n);
+  return str;
+}
+
+/**
+ * Return a string_view equal to @p str but with the last @p n elements dropped.
+ */
+constexpr std::string_view drop_back(
+    std::string_view str, std::string_view::size_type n = 1) noexcept {
+  str.remove_suffix(n);
+  return str;
+}
+
+/**
+ * Returns a reference to the substring of @p str from [start, end).
+ *
+ * @param start The index of the starting character in the substring; if
+ * the index is npos or greater than the length of the string then the
+ * empty substring will be returned.
+ *
+ * @param end The index following the last character to include in the
+ * substring. If this is npos or exceeds the number of characters
+ * remaining in the string, the string suffix (starting with @p start)
+ * will be returned. If this is less than @p start, an empty string will
+ * be returned.
+ */
+constexpr std::string_view slice(std::string_view str,
+                                 std::string_view::size_type start,
+                                 std::string_view::size_type end) noexcept {
+  auto length = str.size();
+  start = (std::min)(start, length);
+  end = (std::min)((std::max)(start, end), length);
+  return {str.data() + start, end - start};
+}
+
+/**
+ * Splits @p str into two substrings around the first occurrence of a separator
+ * character.
+ *
+ * If @p separator is in the string, then the result is a pair (LHS, RHS)
+ * such that (str == LHS + separator + RHS) is true and RHS is
+ * maximal. If @p separator is not in the string, then the result is a
+ * pair (LHS, RHS) where (str == LHS) and (RHS == "").
+ *
+ * @param separator The character to split on.
+ * @returns The split substrings.
+ */
+constexpr std::pair<std::string_view, std::string_view> split(
+    std::string_view str, char separator) noexcept {
+  auto idx = str.find(separator);
+  if (idx == std::string_view::npos) {
+    return {str, {}};
+  }
+  return {slice(str, 0, idx), slice(str, idx + 1, std::string_view::npos)};
+}
+
+/**
+ * Splits @p str into two substrings around the first occurrence of a separator
+ * string.
+ *
+ * If @p separator is in the string, then the result is a pair (LHS, RHS)
+ * such that (str == LHS + separator + RHS) is true and RHS is
+ * maximal. If @p separator is not in the string, then the result is a
+ * pair (LHS, RHS) where (str == LHS) and (RHS == "").
+ *
+ * @param separator The string to split on.
+ * @return The split substrings.
+ */
+constexpr std::pair<std::string_view, std::string_view> split(
+    std::string_view str, std::string_view separator) noexcept {
+  auto idx = str.find(separator);
+  if (idx == std::string_view::npos) {
+    return {str, {}};
+  }
+  return {slice(str, 0, idx),
+          slice(str, idx + separator.size(), std::string_view::npos)};
+}
+
+/**
+ * Splits @p str into two substrings around the last occurrence of a separator
+ * character.
+ *
+ * If @p separator is in the string, then the result is a pair (LHS, RHS)
+ * such that (str == LHS + separator + RHS) is true and RHS is
+ * minimal. If @p separator is not in the string, then the result is a
+ * pair (LHS, RHS) where (str == LHS) and (RHS == "").
+ *
+ * @param separator The string to split on.
+ * @return The split substrings.
+ */
+constexpr std::pair<std::string_view, std::string_view> rsplit(
+    std::string_view str, char separator) noexcept {
+  auto idx = str.rfind(separator);
+  if (idx == std::string_view::npos) {
+    return {str, {}};
+  }
+  return {slice(str, 0, idx), slice(str, idx + 1, std::string_view::npos)};
+}
+
+/**
+ * Splits @p str into two substrings around the last occurrence of a separator
+ * string.
+ *
+ * If @p separator is in the string, then the result is a pair (LHS, RHS)
+ * such that (str == LHS + separator + RHS) is true and RHS is
+ * minimal. If @p separator is not in the string, then the result is a
+ * pair (LHS, RHS) where (str == LHS) and (RHS == "").
+ *
+ * @param separator The string to split on.
+ * @return The split substrings.
+ */
+constexpr std::pair<std::string_view, std::string_view> rsplit(
+    std::string_view str, std::string_view separator) noexcept {
+  auto idx = str.rfind(separator);
+  if (idx == std::string_view::npos) {
+    return {str, {}};
+  }
+  return {slice(str, 0, idx),
+          slice(str, idx + separator.size(), std::string_view::npos)};
+}
+
+/**
+ * Splits @p str into substrings around the occurrences of a separator string.
+ *
+ * Each substring is stored in @p arr. If @p maxSplit is >= 0, at most
+ * @p maxSplit splits are done and consequently <= @p maxSplit + 1
+ * elements are added to arr.
+ * If @p keepEmpty is false, empty strings are not added to @p arr. They
+ * still count when considering @p maxSplit
+ * An useful invariant is that
+ * separator.join(arr) == str if maxSplit == -1 and keepEmpty == true
+ *
+ * @param arr Where to put the substrings.
+ * @param separator The string to split on.
+ * @param maxSplit The maximum number of times the string is split.
+ * @param keepEmpty True if empty substring should be added.
+ */
+void split(std::string_view str, SmallVectorImpl<std::string_view>& arr,
+           std::string_view separator, int maxSplit = -1,
+           bool keepEmpty = true) noexcept;
+
+/**
+ * Splits @p str into substrings around the occurrences of a separator
+ * character.
+ *
+ * Each substring is stored in @p arr. If @p maxSplit is >= 0, at most
+ * @p maxSplit splits are done and consequently <= @p maxSplit + 1
+ * elements are added to arr.
+ * If @p keepEmpty is false, empty strings are not added to @p arr. They
+ * still count when considering @p maxSplit
+ * An useful invariant is that
+ * separator.join(arr) == str if maxSplit == -1 and keepEmpty == true
+ *
+ * @param arr Where to put the substrings.
+ * @param separator The character to split on.
+ * @param maxSplit The maximum number of times the string is split.
+ * @param keepEmpty True if empty substring should be added.
+ */
+void split(std::string_view str, SmallVectorImpl<std::string_view>& arr,
+           char separator, int maxSplit = -1, bool keepEmpty = true) noexcept;
+
+/**
+ * Returns @p str with consecutive @p ch characters starting from the
+ * the left removed.
+ */
+constexpr std::string_view ltrim(std::string_view str, char ch) noexcept {
+  return drop_front(str, (std::min)(str.size(), str.find_first_not_of(ch)));
+}
+
+/**
+ * Returns @p str with consecutive characters in @p chars starting from
+ * the left removed.
+ */
+constexpr std::string_view ltrim(
+    std::string_view str, std::string_view chars = " \t\n\v\f\r") noexcept {
+  return drop_front(str, (std::min)(str.size(), str.find_first_not_of(chars)));
+}
+
+/**
+ * Returns @p str with consecutive @p Char characters starting from the
+ * right removed.
+ */
+constexpr std::string_view rtrim(std::string_view str, char ch) noexcept {
+  return drop_back(
+      str, str.size() - (std::min)(str.size(), str.find_last_not_of(ch) + 1));
+}
+
+/**
+ * Returns @p str with consecutive characters in @p chars starting from
+ * the right removed.
+ */
+constexpr std::string_view rtrim(
+    std::string_view str, std::string_view chars = " \t\n\v\f\r") noexcept {
+  return drop_back(
+      str,
+      str.size() - (std::min)(str.size(), str.find_last_not_of(chars) + 1));
+}
+
+/**
+ * Returns @p str with consecutive @p ch characters starting from the
+ * left and right removed.
+ */
+constexpr std::string_view trim(std::string_view str, char ch) noexcept {
+  return rtrim(ltrim(str, ch), ch);
+}
+
+/**
+ * Returns @p str with consecutive characters in @p chars starting from
+ * the left and right removed.
+ */
+constexpr std::string_view trim(
+    std::string_view str, std::string_view chars = " \t\n\v\f\r") noexcept {
+  return rtrim(ltrim(str, chars), chars);
 }
 
 namespace detail {
-template <typename N>
-inline bool to_float(const Twine &T, N &Num, N (*StrTo)(const char *, char **)) {
-  SmallString<32> Storage;
-  StringRef S = T.toNullTerminatedStringRef(Storage);
-  char *End;
-  N Temp = StrTo(S.data(), &End);
-  if (*End != '\0')
-    return false;
-  Num = Temp;
-  return true;
-}
-}
+bool GetAsUnsignedInteger(
+    std::string_view str, unsigned radix,
+    unsigned long long& result) noexcept;  // NOLINT(runtime/int)
+bool GetAsSignedInteger(std::string_view str, unsigned radix,
+                        long long& result) noexcept;  // NOLINT(runtime/int)
 
-inline bool to_float(const Twine &T, float &Num) {
-  return detail::to_float(T, Num, strtof);
-}
+bool ConsumeUnsignedInteger(
+    std::string_view& str, unsigned radix,
+    unsigned long long& result) noexcept;  // NOLINT(runtime/int)
+bool ConsumeSignedInteger(std::string_view& str, unsigned radix,
+                          long long& result) noexcept;  // NOLINT(runtime/int)
+}  // namespace detail
 
-inline bool to_float(const Twine &T, double &Num) {
-  return detail::to_float(T, Num, strtod);
-}
-
-inline bool to_float(const Twine &T, long double &Num) {
-  return detail::to_float(T, Num, strtold);
-}
-
-inline std::string utostr(uint64_t X, bool isNeg = false) {
-  char Buffer[21];
-  char *BufPtr = std::end(Buffer);
-
-  if (X == 0) *--BufPtr = '0';  // Handle special case...
-
-  while (X) {
-    *--BufPtr = '0' + char(X % 10);
-    X /= 10;
+/**
+ * Parses the string @p str as an integer of the specified radix.  If
+ * @p radix is specified as zero, this does radix autosensing using
+ * extended C rules: 0 is octal, 0x is hex, 0b is binary.
+ *
+ * If the string is invalid or if only a subset of the string is valid,
+ * this returns nullopt to signify the error.  The string is considered
+ * erroneous if empty or if it overflows T.
+ */
+template <typename T,
+          std::enable_if_t<std::numeric_limits<T>::is_signed, bool> = true>
+inline std::optional<T> parse_integer(std::string_view str,
+                                      unsigned radix) noexcept {
+  long long val;  // NOLINT(runtime/int)
+  if (detail::GetAsSignedInteger(str, radix, val) ||
+      static_cast<T>(val) != val) {
+    return std::nullopt;
   }
-
-  if (isNeg) *--BufPtr = '-';   // Add negative sign...
-  return std::string(BufPtr, std::end(Buffer));
+  return val;
 }
 
-inline std::string itostr(int64_t X) {
-  if (X < 0)
-    return utostr(static_cast<uint64_t>(-X), true);
-  else
-    return utostr(static_cast<uint64_t>(X));
-}
-
-/// StrInStrNoCase - Portable version of strcasestr.  Locates the first
-/// occurrence of string 's1' in string 's2', ignoring case.  Returns
-/// the offset of s2 in s1 or npos if s2 cannot be found.
-StringRef::size_type StrInStrNoCase(StringRef s1, StringRef s2);
-
-/// getToken - This function extracts one token from source, ignoring any
-/// leading characters that appear in the Delimiters string, and ending the
-/// token at any of the characters that appear in the Delimiters string.  If
-/// there are no tokens in the source string, an empty string is returned.
-/// The function returns a pair containing the extracted token and the
-/// remaining tail string.
-std::pair<StringRef, StringRef> getToken(StringRef Source,
-                                         StringRef Delimiters = " \t\n\v\f\r");
-
-/// SplitString - Split up the specified string according to the specified
-/// delimiters, appending the result fragments to the output list.
-void SplitString(StringRef Source,
-                 SmallVectorImpl<StringRef> &OutFragments,
-                 StringRef Delimiters = " \t\n\v\f\r");
-
-/// HashString - Hash function for strings.
-///
-/// This is the Bernstein hash function.
-//
-// FIXME: Investigate whether a modified bernstein hash function performs
-// better: http://eternallyconfuzzled.com/tuts/algorithms/jsw_tut_hashing.aspx
-//   X*33+c -> X*33^c
-static inline unsigned HashString(StringRef Str, unsigned Result = 0) {
-  for (StringRef::size_type i = 0, e = Str.size(); i != e; ++i)
-    Result = Result * 33 + (unsigned char)Str[i];
-  return Result;
-}
-
-/// Returns the English suffix for an ordinal integer (-st, -nd, -rd, -th).
-inline StringRef getOrdinalSuffix(unsigned Val) {
-  // It is critically important that we do this perfectly for
-  // user-written sequences with over 100 elements.
-  switch (Val % 100) {
-  case 11:
-  case 12:
-  case 13:
-    return "th";
-  default:
-    switch (Val % 10) {
-      case 1: return "st";
-      case 2: return "nd";
-      case 3: return "rd";
-      default: return "th";
-    }
+template <typename T,
+          std::enable_if_t<!std::numeric_limits<T>::is_signed, bool> = true>
+inline std::optional<T> parse_integer(std::string_view str,
+                                      unsigned radix) noexcept {
+  using Int = unsigned long long;  // NOLINT(runtime/int)
+  Int val;
+  // The additional cast to unsigned long long is required to avoid the
+  // Visual C++ warning C4805: '!=' : unsafe mix of type 'bool' and type
+  // 'unsigned __int64' when instantiating getAsInteger with T = bool.
+  if (detail::GetAsUnsignedInteger(str, radix, val) ||
+      static_cast<Int>(static_cast<T>(val)) != val) {
+    return std::nullopt;
   }
+  return val;
 }
 
-/// Print each character of the specified string, escaping it if it is not
-/// printable or if it is an escape char.
-void printEscapedString(StringRef Name, raw_ostream &Out);
-
-/// Print each character of the specified string, escaping HTML special
-/// characters.
-void printHTMLEscaped(StringRef String, raw_ostream &Out);
-
-/// printLowerCase - Print each character as lowercase if it is uppercase.
-void printLowerCase(StringRef String, raw_ostream &Out);
-
-namespace detail {
-
-template <typename IteratorT>
-inline std::string join_impl(IteratorT Begin, IteratorT End,
-                             StringRef Separator, std::input_iterator_tag) {
-  std::string S;
-  if (Begin == End)
-    return S;
-
-  S += (*Begin);
-  while (++Begin != End) {
-    S += Separator;
-    S += (*Begin);
+/**
+ * Parses the string @p str as an integer of the specified radix.  If
+ * @p radix is specified as zero, this does radix autosensing using
+ * extended C rules: 0 is octal, 0x is hex, 0b is binary.
+ *
+ * If the string does not begin with a number of the specified radix,
+ * this returns nullopt to signify the error. The string is considered
+ * erroneous if empty or if it overflows T.
+ * The portion of the string representing the discovered numeric value
+ * is removed from the beginning of the string.
+ */
+template <typename T,
+          std::enable_if_t<std::numeric_limits<T>::is_signed, bool> = true>
+inline std::optional<T> consume_integer(std::string_view* str,
+                                        unsigned radix) noexcept {
+  using Int = long long;  // NOLINT(runtime/int)
+  Int val;
+  if (detail::ConsumeSignedInteger(*str, radix, val) ||
+      static_cast<Int>(static_cast<T>(val)) != val) {
+    return std::nullopt;
   }
-  return S;
+  return val;
 }
 
-template <typename IteratorT>
-inline std::string join_impl(IteratorT Begin, IteratorT End,
-                             StringRef Separator, std::forward_iterator_tag) {
-  std::string S;
-  if (Begin == End)
-    return S;
-
-  size_t Len = (std::distance(Begin, End) - 1) * Separator.size();
-  for (IteratorT I = Begin; I != End; ++I)
-    Len += (*Begin).size();
-  S.reserve(Len);
-  S += (*Begin);
-  while (++Begin != End) {
-    S += Separator;
-    S += (*Begin);
+template <typename T,
+          std::enable_if_t<!std::numeric_limits<T>::is_signed, bool> = true>
+inline std::optional<T> consume_integer(std::string_view* str,
+                                        unsigned radix) noexcept {
+  using Int = unsigned long long;  // NOLINT(runtime/int)
+  Int val;
+  if (detail::ConsumeUnsignedInteger(*str, radix, val) ||
+      static_cast<Int>(static_cast<T>(val)) != val) {
+    return std::nullopt;
   }
-  return S;
+  return val;
 }
 
-template <typename Sep>
-inline void join_items_impl(std::string &Result, Sep Separator) {}
+/**
+ * Parses the string @p str as a floating point value.
+ *
+ * If the string is invalid or if only a subset of the string is valid,
+ * this returns nullopt to signify the error.  The string is considered
+ * erroneous if empty or if it overflows T.
+ */
+template <typename T>
+std::optional<T> parse_float(std::string_view str) noexcept;
 
-template <typename Sep, typename Arg>
-inline void join_items_impl(std::string &Result, Sep Separator,
-                            const Arg &Item) {
-  Result += Item;
-}
+template <>
+std::optional<float> parse_float<float>(std::string_view str) noexcept;
+template <>
+std::optional<double> parse_float<double>(std::string_view str) noexcept;
+template <>
+std::optional<long double> parse_float<long double>(
+    std::string_view str) noexcept;
 
-template <typename Sep, typename Arg1, typename... Args>
-inline void join_items_impl(std::string &Result, Sep Separator, const Arg1 &A1,
-                            Args &&... Items) {
-  Result += A1;
-  Result += Separator;
-  join_items_impl(Result, Separator, std::forward<Args>(Items)...);
-}
-
-inline size_t join_one_item_size(char C) { return 1; }
-inline size_t join_one_item_size(const char *S) { return S ? ::strlen(S) : 0; }
-
-template <typename T> inline size_t join_one_item_size(const T &Str) {
-  return Str.size();
-}
-
-inline size_t join_items_size() { return 0; }
-
-template <typename A1> inline size_t join_items_size(const A1 &A) {
-  return join_one_item_size(A);
-}
-template <typename A1, typename... Args>
-inline size_t join_items_size(const A1 &A, Args &&... Items) {
-  return join_one_item_size(A) + join_items_size(std::forward<Args>(Items)...);
-}
-
-} // end namespace detail
-
-/// Joins the strings in the range [Begin, End), adding Separator between
-/// the elements.
-template <typename IteratorT>
-inline std::string join(IteratorT Begin, IteratorT End, StringRef Separator) {
-  using tag = typename std::iterator_traits<IteratorT>::iterator_category;
-  return detail::join_impl(Begin, End, Separator, tag());
-}
-
-/// Joins the strings in the range [R.begin(), R.end()), adding Separator
-/// between the elements.
-template <typename Range>
-inline std::string join(Range &&R, StringRef Separator) {
-  return join(R.begin(), R.end(), Separator);
-}
-
-/// Joins the strings in the parameter pack \p Items, adding \p Separator
-/// between the elements.  All arguments must be implicitly convertible to
-/// std::string, or there should be an overload of std::string::operator+=()
-/// that accepts the argument explicitly.
-template <typename Sep, typename... Args>
-inline std::string join_items(Sep Separator, Args &&... Items) {
-  std::string Result;
-  if (sizeof...(Items) == 0)
-    return Result;
-
-  size_t NS = detail::join_one_item_size(Separator);
-  size_t NI = detail::join_items_size(std::forward<Args>(Items)...);
-  Result.reserve(NI + (sizeof...(Items) - 1) * NS + 1);
-  detail::join_items_impl(Result, Separator, std::forward<Args>(Items)...);
-  return Result;
-}
-
-} // end namespace wpi
-
-#endif // WPIUTIL_WPI_STRINGEXTRAS_H
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringMap.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringMap.h
index dbfe8ed..dac2f20 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringMap.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringMap.h
@@ -15,13 +15,11 @@
 #define WPIUTIL_WPI_STRINGMAP_H
 
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
 #include "wpi/iterator.h"
 #include "wpi/iterator_range.h"
 #include "wpi/MemAlloc.h"
 #include "wpi/PointerLikeTypeTraits.h"
 #include "wpi/ErrorHandling.h"
-#include "wpi/deprecated.h"
 #include <algorithm>
 #include <cassert>
 #include <cstdint>
@@ -29,6 +27,7 @@
 #include <cstring>
 #include <initializer_list>
 #include <iterator>
+#include <string_view>
 #include <utility>
 
 namespace wpi {
@@ -82,12 +81,12 @@
   /// specified bucket will be non-null.  Otherwise, it will be null.  In either
   /// case, the FullHashValue field of the bucket will be set to the hash value
   /// of the string.
-  unsigned LookupBucketFor(StringRef Key);
+  unsigned LookupBucketFor(std::string_view Key);
 
   /// FindKey - Look up the bucket that contains the specified key. If it exists
   /// in the map, return the bucket number of the key.  Otherwise return -1.
   /// This does not modify the map.
-  int FindKey(StringRef Key) const;
+  int FindKey(std::string_view Key) const;
 
   /// RemoveKey - Remove the specified StringMapEntry from the table, but do not
   /// delete it.  This aborts if the value isn't in the table.
@@ -95,7 +94,7 @@
 
   /// RemoveKey - Remove the StringMapEntry for the specified key from the
   /// table, returning it.  If the key is not in the table, this returns null.
-  StringMapEntryBase *RemoveKey(StringRef Key);
+  StringMapEntryBase *RemoveKey(std::string_view Key);
 
   /// Allocate the table with the specified number of buckets and otherwise
   /// setup the map as empty.
@@ -137,8 +136,8 @@
       : StringMapEntryBase(strLen), second(std::forward<InitTy>(InitVals)...) {}
   StringMapEntry(StringMapEntry &E) = delete;
 
-  StringRef getKey() const {
-    return StringRef(getKeyData(), getKeyLength());
+  std::string_view getKey() const {
+    return {getKeyData(), getKeyLength()};
   }
 
   const ValueTy &getValue() const { return second; }
@@ -151,12 +150,12 @@
   /// StringMapEntry object.
   const char *getKeyData() const {return reinterpret_cast<const char*>(this+1);}
 
-  StringRef first() const { return StringRef(getKeyData(), getKeyLength()); }
+  std::string_view first() const { return {getKeyData(), getKeyLength()}; }
 
   /// Create a StringMapEntry for the specified key construct the value using
   /// \p InitiVals.
   template <typename... InitTy>
-  static StringMapEntry *Create(StringRef Key, InitTy &&... InitVals) {
+  static StringMapEntry *Create(std::string_view Key, InitTy &&... InitVals) {
     size_t KeyLength = Key.size();
 
     // Allocate a new item with space for the string at the end and a null
@@ -177,7 +176,7 @@
     return NewItem;
   }
 
-  static StringMapEntry *Create(StringRef Key) {
+  static StringMapEntry *Create(std::string_view Key) {
     return Create(Key, ValueTy());
   }
 
@@ -212,7 +211,7 @@
   explicit StringMap(unsigned InitialSize)
     : StringMapImpl(InitialSize, static_cast<unsigned>(sizeof(MapEntryTy))) {}
 
-  StringMap(std::initializer_list<std::pair<StringRef, ValueTy>> List)
+  StringMap(std::initializer_list<std::pair<std::string_view, ValueTy>> List)
       : StringMapImpl(List.size(), static_cast<unsigned>(sizeof(MapEntryTy))) {
     for (const auto &P : List) {
       insert(P);
@@ -302,13 +301,13 @@
                       StringMapKeyIterator<ValueTy>(end()));
   }
 
-  iterator find(StringRef Key) {
+  iterator find(std::string_view Key) {
     int Bucket = FindKey(Key);
     if (Bucket == -1) return end();
     return iterator(TheTable+Bucket, true);
   }
 
-  const_iterator find(StringRef Key) const {
+  const_iterator find(std::string_view Key) const {
     int Bucket = FindKey(Key);
     if (Bucket == -1) return end();
     return const_iterator(TheTable+Bucket, true);
@@ -316,7 +315,7 @@
 
   /// lookup - Return the entry for the specified key, or a default
   /// constructed value if no such entry exists.
-  ValueTy lookup(StringRef Key) const {
+  ValueTy lookup(std::string_view Key) const {
     const_iterator it = find(Key);
     if (it != end())
       return it->second;
@@ -325,10 +324,10 @@
 
   /// Lookup the ValueTy for the \p Key, or create a default constructed value
   /// if the key is not in the map.
-  ValueTy &operator[](StringRef Key) { return try_emplace(Key).first->second; }
+  ValueTy &operator[](std::string_view Key) { return try_emplace(Key).first->second; }
 
   /// count - Return 1 if the element is in the map, 0 otherwise.
-  size_type count(StringRef Key) const {
+  size_type count(std::string_view Key) const {
     return find(Key) == end() ? 0 : 1;
   }
 
@@ -355,7 +354,7 @@
   /// isn't already in the map. The bool component of the returned pair is true
   /// if and only if the insertion takes place, and the iterator component of
   /// the pair points to the element with key equivalent to the key of the pair.
-  std::pair<iterator, bool> insert(std::pair<StringRef, ValueTy> KV) {
+  std::pair<iterator, bool> insert(std::pair<std::string_view, ValueTy> KV) {
     return try_emplace(KV.first, std::move(KV.second));
   }
 
@@ -364,7 +363,7 @@
   /// if and only if the insertion takes place, and the iterator component of
   /// the pair points to the element with key equivalent to the key of the pair.
   template <typename... ArgsTy>
-  std::pair<iterator, bool> try_emplace(StringRef Key, ArgsTy &&... Args) {
+  std::pair<iterator, bool> try_emplace(std::string_view Key, ArgsTy &&... Args) {
     unsigned BucketNo = LookupBucketFor(Key);
     StringMapEntryBase *&Bucket = TheTable[BucketNo];
     if (Bucket && Bucket != getTombstoneVal())
@@ -411,7 +410,7 @@
     V.Destroy();
   }
 
-  bool erase(StringRef Key) {
+  bool erase(std::string_view Key) {
     iterator I = find(Key);
     if (I == end()) return false;
     erase(I);
@@ -440,7 +439,13 @@
     return static_cast<DerivedTy &>(*this);
   }
 
+#if __cplusplus < 202002L
   bool operator==(const DerivedTy &RHS) const { return Ptr == RHS.Ptr; }
+#else
+  friend bool operator==(const DerivedTy &LHS, const DerivedTy &RHS) {
+    return LHS.Ptr == RHS.Ptr;
+  }
+#endif
 
   DerivedTy &operator++() { // Preincrement
     ++Ptr;
@@ -524,23 +529,23 @@
 class StringMapKeyIterator
     : public iterator_adaptor_base<StringMapKeyIterator<ValueTy>,
                                    StringMapConstIterator<ValueTy>,
-                                   std::forward_iterator_tag, StringRef> {
+                                   std::forward_iterator_tag, std::string_view> {
   using base = iterator_adaptor_base<StringMapKeyIterator<ValueTy>,
                                      StringMapConstIterator<ValueTy>,
-                                     std::forward_iterator_tag, StringRef>;
+                                     std::forward_iterator_tag, std::string_view>;
 
 public:
   StringMapKeyIterator() = default;
   explicit StringMapKeyIterator(StringMapConstIterator<ValueTy> Iter)
       : base(std::move(Iter)) {}
 
-  StringRef &operator*() {
+  std::string_view &operator*() {
     Key = this->wrapped()->getKey();
     return Key;
   }
 
 private:
-  StringRef Key;
+  std::string_view Key;
 };
 
 template <typename ValueTy>
@@ -594,13 +599,13 @@
   if (&lhs == &rhs) return false;
 
   // copy into vectors and sort by key
-  SmallVector<StringRef, 16> lhs_keys;
+  SmallVector<std::string_view, 16> lhs_keys;
   lhs_keys.reserve(lhs.size());
   for (auto i = lhs.begin(), end = lhs.end(); i != end; ++i)
     lhs_keys.push_back(i->getKey());
   std::sort(lhs_keys.begin(), lhs_keys.end());
 
-  SmallVector<StringRef, 16> rhs_keys;
+  SmallVector<std::string_view, 16> rhs_keys;
   rhs_keys.reserve(rhs.size());
   for (auto i = rhs.begin(), end = rhs.end(); i != end; ++i)
     rhs_keys.push_back(i->getKey());
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringRef.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringRef.h
deleted file mode 100644
index 60fb789..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/StringRef.h
+++ /dev/null
@@ -1,947 +0,0 @@
-//===- StringRef.h - Constant String Reference Wrapper ----------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_STRINGREF_H
-#define WPIUTIL_WPI_STRINGREF_H
-
-#include "wpi/STLExtras.h"
-#include "wpi/iterator_range.h"
-#include "wpi/Compiler.h"
-#include <algorithm>
-#include <cassert>
-#include <cstddef>
-#include <cstring>
-#include <iosfwd>
-#include <limits>
-#include <string>
-#include <type_traits>
-#include <utility>
-
-namespace wpi {
-
-  class hash_code;
-  template <typename T> class SmallVectorImpl;
-  class StringRef;
-
-  /// Helper functions for StringRef::getAsInteger.
-  bool getAsUnsignedInteger(StringRef Str, unsigned Radix,
-                            unsigned long long &Result) noexcept;
-
-  bool getAsSignedInteger(StringRef Str, unsigned Radix, long long &Result) noexcept;
-
-  bool consumeUnsignedInteger(StringRef &Str, unsigned Radix,
-                              unsigned long long &Result) noexcept;
-  bool consumeSignedInteger(StringRef &Str, unsigned Radix, long long &Result) noexcept;
-
-  /// StringRef - Represent a constant reference to a string, i.e. a character
-  /// array and a length, which need not be null terminated.
-  ///
-  /// This class does not own the string data, it is expected to be used in
-  /// situations where the character data resides in some other buffer, whose
-  /// lifetime extends past that of the StringRef. For this reason, it is not in
-  /// general safe to store a StringRef.
-  class StringRef {
-  public:
-    static const size_t npos = ~size_t(0);
-
-    using iterator = const char *;
-    using const_iterator = const char *;
-    using size_type = size_t;
-
-  private:
-    /// The start of the string, in an external buffer.
-    const char *Data = nullptr;
-
-    /// The length of the string.
-    size_t Length = 0;
-
-    // Workaround memcmp issue with null pointers (undefined behavior)
-    // by providing a specialized version
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    static int compareMemory(const char *Lhs, const char *Rhs, size_t Length) noexcept {
-      if (Length == 0) { return 0; }
-      return ::memcmp(Lhs,Rhs,Length);
-    }
-
-  public:
-    /// @name Constructors
-    /// @{
-
-    /// Construct an empty string ref.
-    /*implicit*/ StringRef() = default;
-
-    /// Disable conversion from nullptr.  This prevents things like
-    /// if (S == nullptr)
-    StringRef(std::nullptr_t) = delete;
-
-    /// Construct a string ref from a cstring.
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    /*implicit*/ StringRef(const char *Str)
-        : Data(Str), Length(Str ? ::strlen(Str) : 0) {}
-
-    /// Construct a string ref from a pointer and length.
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    /*implicit*/ constexpr StringRef(const char *data, size_t length)
-        : Data(data), Length(length) {}
-
-    /// Construct a string ref from an std::string.
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    /*implicit*/ StringRef(const std::string &Str)
-      : Data(Str.data()), Length(Str.length()) {}
-
-    static StringRef withNullAsEmpty(const char *data) {
-      return StringRef(data ? data : "");
-    }
-
-    /// @}
-    /// @name Iterators
-    /// @{
-
-    iterator begin() const noexcept { return Data; }
-
-    iterator end() const noexcept { return Data + Length; }
-
-    const unsigned char *bytes_begin() const noexcept {
-      return reinterpret_cast<const unsigned char *>(begin());
-    }
-    const unsigned char *bytes_end() const noexcept {
-      return reinterpret_cast<const unsigned char *>(end());
-    }
-    iterator_range<const unsigned char *> bytes() const noexcept {
-      return make_range(bytes_begin(), bytes_end());
-    }
-
-    /// @}
-    /// @name String Operations
-    /// @{
-
-    /// data - Get a pointer to the start of the string (which may not be null
-    /// terminated).
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    const char *data() const noexcept { return Data; }
-
-    /// empty - Check if the string is empty.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool empty() const noexcept { return Length == 0; }
-
-    /// size - Get the string size.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    size_t size() const noexcept { return Length; }
-
-    /// front - Get the first character in the string.
-    LLVM_NODISCARD
-    char front() const noexcept {
-      assert(!empty());
-      return Data[0];
-    }
-
-    /// back - Get the last character in the string.
-    LLVM_NODISCARD
-    char back() const noexcept {
-      assert(!empty());
-      return Data[Length-1];
-    }
-
-    // copy - Allocate copy in Allocator and return StringRef to it.
-    template <typename Allocator>
-    LLVM_NODISCARD StringRef copy(Allocator &A) const {
-      // Don't request a length 0 copy from the allocator.
-      if (empty())
-        return StringRef();
-      char *S = A.template Allocate<char>(Length);
-      std::copy(begin(), end(), S);
-      return StringRef(S, Length);
-    }
-
-    /// equals - Check for string equality, this is more efficient than
-    /// compare() when the relative ordering of inequal strings isn't needed.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool equals(StringRef RHS) const noexcept {
-      return (Length == RHS.Length &&
-              compareMemory(Data, RHS.Data, RHS.Length) == 0);
-    }
-
-    /// equals_lower - Check for string equality, ignoring case.
-    LLVM_NODISCARD
-    bool equals_lower(StringRef RHS) const noexcept {
-      return Length == RHS.Length && compare_lower(RHS) == 0;
-    }
-
-    /// compare - Compare two strings; the result is -1, 0, or 1 if this string
-    /// is lexicographically less than, equal to, or greater than the \p RHS.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    int compare(StringRef RHS) const noexcept {
-      // Check the prefix for a mismatch.
-      if (int Res = compareMemory(Data, RHS.Data, (std::min)(Length, RHS.Length)))
-        return Res < 0 ? -1 : 1;
-
-      // Otherwise the prefixes match, so we only need to check the lengths.
-      if (Length == RHS.Length)
-        return 0;
-      return Length < RHS.Length ? -1 : 1;
-    }
-
-    /// compare_lower - Compare two strings, ignoring case.
-    LLVM_NODISCARD
-    int compare_lower(StringRef RHS) const noexcept;
-
-    /// compare_numeric - Compare two strings, treating sequences of digits as
-    /// numbers.
-    LLVM_NODISCARD
-    int compare_numeric(StringRef RHS) const noexcept;
-
-    /// str - Get the contents as an std::string.
-    LLVM_NODISCARD
-    std::string str() const {
-      if (!Data) return std::string();
-      return std::string(Data, Length);
-    }
-
-    /// @}
-    /// @name Operator Overloads
-    /// @{
-
-    LLVM_NODISCARD
-    char operator[](size_t Index) const noexcept {
-      assert(Index < Length && "Invalid index!");
-      return Data[Index];
-    }
-
-    /// Disallow accidental assignment from a temporary std::string.
-    ///
-    /// The declaration here is extra complicated so that `stringRef = {}`
-    /// and `stringRef = "abc"` continue to select the move assignment operator.
-    template <typename T>
-    typename std::enable_if<std::is_same<T, std::string>::value,
-                            StringRef>::type &
-    operator=(T &&Str) = delete;
-
-    /// @}
-    /// @name Type Conversions
-    /// @{
-
-    operator std::string() const {
-      return str();
-    }
-
-    /// @}
-    /// @name String Predicates
-    /// @{
-
-    /// Check if this string starts with the given \p Prefix.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool startswith(StringRef Prefix) const noexcept {
-      return Length >= Prefix.Length &&
-             compareMemory(Data, Prefix.Data, Prefix.Length) == 0;
-    }
-
-    /// Check if this string starts with the given \p Prefix, ignoring case.
-    LLVM_NODISCARD
-    bool startswith_lower(StringRef Prefix) const noexcept;
-
-    /// Check if this string ends with the given \p Suffix.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool endswith(StringRef Suffix) const noexcept {
-      return Length >= Suffix.Length &&
-        compareMemory(end() - Suffix.Length, Suffix.Data, Suffix.Length) == 0;
-    }
-
-    /// Check if this string ends with the given \p Suffix, ignoring case.
-    LLVM_NODISCARD
-    bool endswith_lower(StringRef Suffix) const noexcept;
-
-    /// @}
-    /// @name String Searching
-    /// @{
-
-    /// Search for the first character \p C in the string.
-    ///
-    /// \returns The index of the first occurrence of \p C, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    size_t find(char C, size_t From = 0) const noexcept {
-      size_t FindBegin = (std::min)(From, Length);
-      if (FindBegin < Length) { // Avoid calling memchr with nullptr.
-        // Just forward to memchr, which is faster than a hand-rolled loop.
-        if (const void *P = ::memchr(Data + FindBegin, C, Length - FindBegin))
-          return static_cast<const char *>(P) - Data;
-      }
-      return npos;
-    }
-
-    /// Search for the first character \p C in the string, ignoring case.
-    ///
-    /// \returns The index of the first occurrence of \p C, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t find_lower(char C, size_t From = 0) const noexcept;
-
-    /// Search for the first character satisfying the predicate \p F
-    ///
-    /// \returns The index of the first character satisfying \p F starting from
-    /// \p From, or npos if not found.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    size_t find_if(function_ref<bool(char)> F, size_t From = 0) const noexcept {
-      StringRef S = drop_front(From);
-      while (!S.empty()) {
-        if (F(S.front()))
-          return size() - S.size();
-        S = S.drop_front();
-      }
-      return npos;
-    }
-
-    /// Search for the first character not satisfying the predicate \p F
-    ///
-    /// \returns The index of the first character not satisfying \p F starting
-    /// from \p From, or npos if not found.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    size_t find_if_not(function_ref<bool(char)> F, size_t From = 0) const noexcept {
-      return find_if([F](char c) { return !F(c); }, From);
-    }
-
-    /// Search for the first string \p Str in the string.
-    ///
-    /// \returns The index of the first occurrence of \p Str, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t find(StringRef Str, size_t From = 0) const noexcept;
-
-    /// Search for the first string \p Str in the string, ignoring case.
-    ///
-    /// \returns The index of the first occurrence of \p Str, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t find_lower(StringRef Str, size_t From = 0) const noexcept;
-
-    /// Search for the last character \p C in the string.
-    ///
-    /// \returns The index of the last occurrence of \p C, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t rfind(char C, size_t From = npos) const noexcept {
-      From = (std::min)(From, Length);
-      size_t i = From;
-      while (i != 0) {
-        --i;
-        if (Data[i] == C)
-          return i;
-      }
-      return npos;
-    }
-
-    /// Search for the last character \p C in the string, ignoring case.
-    ///
-    /// \returns The index of the last occurrence of \p C, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t rfind_lower(char C, size_t From = npos) const noexcept;
-
-    /// Search for the last string \p Str in the string.
-    ///
-    /// \returns The index of the last occurrence of \p Str, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t rfind(StringRef Str) const noexcept;
-
-    /// Search for the last string \p Str in the string, ignoring case.
-    ///
-    /// \returns The index of the last occurrence of \p Str, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t rfind_lower(StringRef Str) const noexcept;
-
-    /// Find the first character in the string that is \p C, or npos if not
-    /// found. Same as find.
-    LLVM_NODISCARD
-    size_t find_first_of(char C, size_t From = 0) const noexcept {
-      return find(C, From);
-    }
-
-    /// Find the first character in the string that is in \p Chars, or npos if
-    /// not found.
-    ///
-    /// Complexity: O(size() + Chars.size())
-    LLVM_NODISCARD
-    size_t find_first_of(StringRef Chars, size_t From = 0) const noexcept;
-
-    /// Find the first character in the string that is not \p C or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t find_first_not_of(char C, size_t From = 0) const noexcept;
-
-    /// Find the first character in the string that is not in the string
-    /// \p Chars, or npos if not found.
-    ///
-    /// Complexity: O(size() + Chars.size())
-    LLVM_NODISCARD
-    size_t find_first_not_of(StringRef Chars, size_t From = 0) const noexcept;
-
-    /// Find the last character in the string that is \p C, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t find_last_of(char C, size_t From = npos) const noexcept {
-      return rfind(C, From);
-    }
-
-    /// Find the last character in the string that is in \p C, or npos if not
-    /// found.
-    ///
-    /// Complexity: O(size() + Chars.size())
-    LLVM_NODISCARD
-    size_t find_last_of(StringRef Chars, size_t From = npos) const noexcept;
-
-    /// Find the last character in the string that is not \p C, or npos if not
-    /// found.
-    LLVM_NODISCARD
-    size_t find_last_not_of(char C, size_t From = npos) const noexcept;
-
-    /// Find the last character in the string that is not in \p Chars, or
-    /// npos if not found.
-    ///
-    /// Complexity: O(size() + Chars.size())
-    LLVM_NODISCARD
-    size_t find_last_not_of(StringRef Chars, size_t From = npos) const noexcept;
-
-    /// Return true if the given string is a substring of *this, and false
-    /// otherwise.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool contains(StringRef Other) const noexcept { return find(Other) != npos; }
-
-    /// Return true if the given character is contained in *this, and false
-    /// otherwise.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool contains(char C) const noexcept { return find_first_of(C) != npos; }
-
-    /// Return true if the given string is a substring of *this, and false
-    /// otherwise.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool contains_lower(StringRef Other) const noexcept {
-      return find_lower(Other) != npos;
-    }
-
-    /// Return true if the given character is contained in *this, and false
-    /// otherwise.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool contains_lower(char C) const noexcept { return find_lower(C) != npos; }
-
-    /// @}
-    /// @name Helpful Algorithms
-    /// @{
-
-    /// Return the number of occurrences of \p C in the string.
-    LLVM_NODISCARD
-    size_t count(char C) const noexcept {
-      size_t Count = 0;
-      for (size_t i = 0, e = Length; i != e; ++i)
-        if (Data[i] == C)
-          ++Count;
-      return Count;
-    }
-
-    /// Return the number of non-overlapped occurrences of \p Str in
-    /// the string.
-    size_t count(StringRef Str) const noexcept;
-
-    /// Parse the current string as an integer of the specified radix.  If
-    /// \p Radix is specified as zero, this does radix autosensing using
-    /// extended C rules: 0 is octal, 0x is hex, 0b is binary.
-    ///
-    /// If the string is invalid or if only a subset of the string is valid,
-    /// this returns true to signify the error.  The string is considered
-    /// erroneous if empty or if it overflows T.
-    template <typename T>
-    typename std::enable_if<std::numeric_limits<T>::is_signed, bool>::type
-    getAsInteger(unsigned Radix, T &Result) const noexcept {
-      long long LLVal;
-      if (getAsSignedInteger(*this, Radix, LLVal) ||
-            static_cast<T>(LLVal) != LLVal)
-        return true;
-      Result = LLVal;
-      return false;
-    }
-
-    template <typename T>
-    typename std::enable_if<!std::numeric_limits<T>::is_signed, bool>::type
-    getAsInteger(unsigned Radix, T &Result) const noexcept {
-      unsigned long long ULLVal;
-      // The additional cast to unsigned long long is required to avoid the
-      // Visual C++ warning C4805: '!=' : unsafe mix of type 'bool' and type
-      // 'unsigned __int64' when instantiating getAsInteger with T = bool.
-      if (getAsUnsignedInteger(*this, Radix, ULLVal) ||
-          static_cast<unsigned long long>(static_cast<T>(ULLVal)) != ULLVal)
-        return true;
-      Result = ULLVal;
-      return false;
-    }
-
-    /// Parse the current string as an integer of the specified radix.  If
-    /// \p Radix is specified as zero, this does radix autosensing using
-    /// extended C rules: 0 is octal, 0x is hex, 0b is binary.
-    ///
-    /// If the string does not begin with a number of the specified radix,
-    /// this returns true to signify the error. The string is considered
-    /// erroneous if empty or if it overflows T.
-    /// The portion of the string representing the discovered numeric value
-    /// is removed from the beginning of the string.
-    template <typename T>
-    typename std::enable_if<std::numeric_limits<T>::is_signed, bool>::type
-    consumeInteger(unsigned Radix, T &Result) noexcept {
-      long long LLVal;
-      if (consumeSignedInteger(*this, Radix, LLVal) ||
-          static_cast<long long>(static_cast<T>(LLVal)) != LLVal)
-        return true;
-      Result = LLVal;
-      return false;
-    }
-
-    template <typename T>
-    typename std::enable_if<!std::numeric_limits<T>::is_signed, bool>::type
-    consumeInteger(unsigned Radix, T &Result) noexcept {
-      unsigned long long ULLVal;
-      if (consumeUnsignedInteger(*this, Radix, ULLVal) ||
-          static_cast<unsigned long long>(static_cast<T>(ULLVal)) != ULLVal)
-        return true;
-      Result = ULLVal;
-      return false;
-    }
-
-    /// @}
-    /// @name String Operations
-    /// @{
-
-    // Convert the given ASCII string to lowercase.
-    LLVM_NODISCARD
-    std::string lower() const;
-
-    /// Convert the given ASCII string to uppercase.
-    LLVM_NODISCARD
-    std::string upper() const;
-
-    /// @}
-    /// @name Substring Operations
-    /// @{
-
-    /// Return a reference to the substring from [Start, Start + N).
-    ///
-    /// \param Start The index of the starting character in the substring; if
-    /// the index is npos or greater than the length of the string then the
-    /// empty substring will be returned.
-    ///
-    /// \param N The number of characters to included in the substring. If N
-    /// exceeds the number of characters remaining in the string, the string
-    /// suffix (starting with \p Start) will be returned.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef substr(size_t Start, size_t N = npos) const noexcept {
-      Start = (std::min)(Start, Length);
-      return StringRef(Data + Start, (std::min)(N, Length - Start));
-    }
-
-    /// Return a StringRef equal to 'this' but with only the first \p N
-    /// elements remaining.  If \p N is greater than the length of the
-    /// string, the entire string is returned.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef take_front(size_t N = 1) const noexcept {
-      if (N >= size())
-        return *this;
-      return drop_back(size() - N);
-    }
-
-    /// Return a StringRef equal to 'this' but with only the last \p N
-    /// elements remaining.  If \p N is greater than the length of the
-    /// string, the entire string is returned.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef take_back(size_t N = 1) const noexcept {
-      if (N >= size())
-        return *this;
-      return drop_front(size() - N);
-    }
-
-    /// Return the longest prefix of 'this' such that every character
-    /// in the prefix satisfies the given predicate.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef take_while(function_ref<bool(char)> F) const noexcept {
-      return substr(0, find_if_not(F));
-    }
-
-    /// Return the longest prefix of 'this' such that no character in
-    /// the prefix satisfies the given predicate.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef take_until(function_ref<bool(char)> F) const noexcept {
-      return substr(0, find_if(F));
-    }
-
-    /// Return a StringRef equal to 'this' but with the first \p N elements
-    /// dropped.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef drop_front(size_t N = 1) const noexcept {
-      assert(size() >= N && "Dropping more elements than exist");
-      return substr(N);
-    }
-
-    /// Return a StringRef equal to 'this' but with the last \p N elements
-    /// dropped.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef drop_back(size_t N = 1) const noexcept {
-      assert(size() >= N && "Dropping more elements than exist");
-      return substr(0, size()-N);
-    }
-
-    /// Return a StringRef equal to 'this', but with all characters satisfying
-    /// the given predicate dropped from the beginning of the string.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef drop_while(function_ref<bool(char)> F) const noexcept {
-      return substr(find_if_not(F));
-    }
-
-    /// Return a StringRef equal to 'this', but with all characters not
-    /// satisfying the given predicate dropped from the beginning of the string.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef drop_until(function_ref<bool(char)> F) const noexcept {
-      return substr(find_if(F));
-    }
-
-    /// Returns true if this StringRef has the given prefix and removes that
-    /// prefix.
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool consume_front(StringRef Prefix) noexcept {
-      if (!startswith(Prefix))
-        return false;
-
-      *this = drop_front(Prefix.size());
-      return true;
-    }
-
-    /// Returns true if this StringRef has the given suffix and removes that
-    /// suffix.
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    bool consume_back(StringRef Suffix) noexcept {
-      if (!endswith(Suffix))
-        return false;
-
-      *this = drop_back(Suffix.size());
-      return true;
-    }
-
-    /// Return a reference to the substring from [Start, End).
-    ///
-    /// \param Start The index of the starting character in the substring; if
-    /// the index is npos or greater than the length of the string then the
-    /// empty substring will be returned.
-    ///
-    /// \param End The index following the last character to include in the
-    /// substring. If this is npos or exceeds the number of characters
-    /// remaining in the string, the string suffix (starting with \p Start)
-    /// will be returned. If this is less than \p Start, an empty string will
-    /// be returned.
-    LLVM_NODISCARD
-    LLVM_ATTRIBUTE_ALWAYS_INLINE
-    StringRef slice(size_t Start, size_t End) const noexcept {
-      Start = (std::min)(Start, Length);
-      End = (std::min)((std::max)(Start, End), Length);
-      return StringRef(Data + Start, End - Start);
-    }
-
-    /// Split into two substrings around the first occurrence of a separator
-    /// character.
-    ///
-    /// If \p Separator is in the string, then the result is a pair (LHS, RHS)
-    /// such that (*this == LHS + Separator + RHS) is true and RHS is
-    /// maximal. If \p Separator is not in the string, then the result is a
-    /// pair (LHS, RHS) where (*this == LHS) and (RHS == "").
-    ///
-    /// \param Separator The character to split on.
-    /// \returns The split substrings.
-    LLVM_NODISCARD
-    std::pair<StringRef, StringRef> split(char Separator) const {
-      return split(StringRef(&Separator, 1));
-    }
-
-    /// Split into two substrings around the first occurrence of a separator
-    /// string.
-    ///
-    /// If \p Separator is in the string, then the result is a pair (LHS, RHS)
-    /// such that (*this == LHS + Separator + RHS) is true and RHS is
-    /// maximal. If \p Separator is not in the string, then the result is a
-    /// pair (LHS, RHS) where (*this == LHS) and (RHS == "").
-    ///
-    /// \param Separator - The string to split on.
-    /// \return - The split substrings.
-    LLVM_NODISCARD
-    std::pair<StringRef, StringRef> split(StringRef Separator) const {
-      size_t Idx = find(Separator);
-      if (Idx == npos)
-        return std::make_pair(*this, StringRef());
-      return std::make_pair(slice(0, Idx), slice(Idx + Separator.size(), npos));
-    }
-
-    /// Split into two substrings around the last occurrence of a separator
-    /// string.
-    ///
-    /// If \p Separator is in the string, then the result is a pair (LHS, RHS)
-    /// such that (*this == LHS + Separator + RHS) is true and RHS is
-    /// minimal. If \p Separator is not in the string, then the result is a
-    /// pair (LHS, RHS) where (*this == LHS) and (RHS == "").
-    ///
-    /// \param Separator - The string to split on.
-    /// \return - The split substrings.
-    LLVM_NODISCARD
-    std::pair<StringRef, StringRef> rsplit(StringRef Separator) const {
-      size_t Idx = rfind(Separator);
-      if (Idx == npos)
-        return std::make_pair(*this, StringRef());
-      return std::make_pair(slice(0, Idx), slice(Idx + Separator.size(), npos));
-    }
-
-    /// Split into substrings around the occurrences of a separator string.
-    ///
-    /// Each substring is stored in \p A. If \p MaxSplit is >= 0, at most
-    /// \p MaxSplit splits are done and consequently <= \p MaxSplit + 1
-    /// elements are added to A.
-    /// If \p KeepEmpty is false, empty strings are not added to \p A. They
-    /// still count when considering \p MaxSplit
-    /// An useful invariant is that
-    /// Separator.join(A) == *this if MaxSplit == -1 and KeepEmpty == true
-    ///
-    /// \param A - Where to put the substrings.
-    /// \param Separator - The string to split on.
-    /// \param MaxSplit - The maximum number of times the string is split.
-    /// \param KeepEmpty - True if empty substring should be added.
-    void split(SmallVectorImpl<StringRef> &A,
-               StringRef Separator, int MaxSplit = -1,
-               bool KeepEmpty = true) const;
-
-    /// Split into substrings around the occurrences of a separator character.
-    ///
-    /// Each substring is stored in \p A. If \p MaxSplit is >= 0, at most
-    /// \p MaxSplit splits are done and consequently <= \p MaxSplit + 1
-    /// elements are added to A.
-    /// If \p KeepEmpty is false, empty strings are not added to \p A. They
-    /// still count when considering \p MaxSplit
-    /// An useful invariant is that
-    /// Separator.join(A) == *this if MaxSplit == -1 and KeepEmpty == true
-    ///
-    /// \param A - Where to put the substrings.
-    /// \param Separator - The string to split on.
-    /// \param MaxSplit - The maximum number of times the string is split.
-    /// \param KeepEmpty - True if empty substring should be added.
-    void split(SmallVectorImpl<StringRef> &A, char Separator, int MaxSplit = -1,
-               bool KeepEmpty = true) const;
-
-    /// Split into two substrings around the last occurrence of a separator
-    /// character.
-    ///
-    /// If \p Separator is in the string, then the result is a pair (LHS, RHS)
-    /// such that (*this == LHS + Separator + RHS) is true and RHS is
-    /// minimal. If \p Separator is not in the string, then the result is a
-    /// pair (LHS, RHS) where (*this == LHS) and (RHS == "").
-    ///
-    /// \param Separator - The character to split on.
-    /// \return - The split substrings.
-    LLVM_NODISCARD
-    std::pair<StringRef, StringRef> rsplit(char Separator) const {
-      return rsplit(StringRef(&Separator, 1));
-    }
-
-    /// Return string with consecutive \p Char characters starting from the
-    /// the left removed.
-    LLVM_NODISCARD
-    StringRef ltrim(char Char) const noexcept {
-      return drop_front((std::min)(Length, find_first_not_of(Char)));
-    }
-
-    /// Return string with consecutive characters in \p Chars starting from
-    /// the left removed.
-    LLVM_NODISCARD
-    StringRef ltrim(StringRef Chars = " \t\n\v\f\r") const noexcept {
-      return drop_front((std::min)(Length, find_first_not_of(Chars)));
-    }
-
-    /// Return string with consecutive \p Char characters starting from the
-    /// right removed.
-    LLVM_NODISCARD
-    StringRef rtrim(char Char) const noexcept {
-      return drop_back(size() - (std::min)(Length, find_last_not_of(Char) + 1));
-    }
-
-    /// Return string with consecutive characters in \p Chars starting from
-    /// the right removed.
-    LLVM_NODISCARD
-    StringRef rtrim(StringRef Chars = " \t\n\v\f\r") const noexcept {
-      return drop_back(size() - (std::min)(Length, find_last_not_of(Chars) + 1));
-    }
-
-    /// Return string with consecutive \p Char characters starting from the
-    /// left and right removed.
-    LLVM_NODISCARD
-    StringRef trim(char Char) const noexcept {
-      return ltrim(Char).rtrim(Char);
-    }
-
-    /// Return string with consecutive characters in \p Chars starting from
-    /// the left and right removed.
-    LLVM_NODISCARD
-    StringRef trim(StringRef Chars = " \t\n\v\f\r") const noexcept {
-      return ltrim(Chars).rtrim(Chars);
-    }
-
-    /// @}
-  };
-
-  /// A wrapper around a string literal that serves as a proxy for constructing
-  /// global tables of StringRefs with the length computed at compile time.
-  /// In order to avoid the invocation of a global constructor, StringLiteral
-  /// should *only* be used in a constexpr context, as such:
-  ///
-  /// constexpr StringLiteral S("test");
-  ///
-  class StringLiteral : public StringRef {
-  private:
-    constexpr StringLiteral(const char *Str, size_t N) : StringRef(Str, N) {
-    }
-
-  public:
-    template <size_t N>
-    constexpr StringLiteral(const char (&Str)[N])
-#if defined(__clang__) && __has_attribute(enable_if)
-#pragma clang diagnostic push
-#pragma clang diagnostic ignored "-Wgcc-compat"
-        __attribute((enable_if(__builtin_strlen(Str) == N - 1,
-                               "invalid string literal")))
-#pragma clang diagnostic pop
-#endif
-        : StringRef(Str, N - 1) {
-    }
-
-    // Explicit construction for strings like "foo\0bar".
-    template <size_t N>
-    static constexpr StringLiteral withInnerNUL(const char (&Str)[N]) {
-      return StringLiteral(Str, N - 1);
-    }
-  };
-
-  /// @name StringRef Comparison Operators
-  /// @{
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE
-  bool operator==(StringRef LHS, StringRef RHS) noexcept {
-    return LHS.equals(RHS);
-  }
-
-  LLVM_ATTRIBUTE_ALWAYS_INLINE
-  bool operator!=(StringRef LHS, StringRef RHS) noexcept {
-    return !(LHS == RHS);
-  }
-
-  inline bool operator<(StringRef LHS, StringRef RHS) noexcept {
-    return LHS.compare(RHS) == -1;
-  }
-
-  inline bool operator<=(StringRef LHS, StringRef RHS) noexcept {
-    return LHS.compare(RHS) != 1;
-  }
-
-  inline bool operator>(StringRef LHS, StringRef RHS) noexcept {
-    return LHS.compare(RHS) == 1;
-  }
-
-  inline bool operator>=(StringRef LHS, StringRef RHS) noexcept {
-    return LHS.compare(RHS) != -1;
-  }
-
-  inline bool operator==(StringRef LHS, const char *RHS) noexcept {
-    return LHS.equals(StringRef(RHS));
-  }
-
-  inline bool operator!=(StringRef LHS, const char *RHS) noexcept {
-    return !(LHS == StringRef(RHS));
-  }
-
-  inline bool operator<(StringRef LHS, const char *RHS) noexcept {
-    return LHS.compare(StringRef(RHS)) == -1;
-  }
-
-  inline bool operator<=(StringRef LHS, const char *RHS) noexcept {
-    return LHS.compare(StringRef(RHS)) != 1;
-  }
-
-  inline bool operator>(StringRef LHS, const char *RHS) noexcept {
-    return LHS.compare(StringRef(RHS)) == 1;
-  }
-
-  inline bool operator>=(StringRef LHS, const char *RHS) noexcept {
-    return LHS.compare(StringRef(RHS)) != -1;
-  }
-
-  inline bool operator==(const char *LHS, StringRef RHS) noexcept {
-    return StringRef(LHS).equals(RHS);
-  }
-
-  inline bool operator!=(const char *LHS, StringRef RHS) noexcept {
-    return !(StringRef(LHS) == RHS);
-  }
-
-  inline bool operator<(const char *LHS, StringRef RHS) noexcept {
-    return StringRef(LHS).compare(RHS) == -1;
-  }
-
-  inline bool operator<=(const char *LHS, StringRef RHS) noexcept {
-    return StringRef(LHS).compare(RHS) != 1;
-  }
-
-  inline bool operator>(const char *LHS, StringRef RHS) noexcept {
-    return StringRef(LHS).compare(RHS) == 1;
-  }
-
-  inline bool operator>=(const char *LHS, StringRef RHS) noexcept {
-    return StringRef(LHS).compare(RHS) != -1;
-  }
-
-  inline std::string &operator+=(std::string &buffer, StringRef string) {
-    return buffer.append(string.data(), string.size());
-  }
-
-  std::ostream &operator<<(std::ostream &os, StringRef string);
-
-  /// @}
-
-  /// Compute a hash_code for a StringRef.
-  LLVM_NODISCARD
-  hash_code hash_value(StringRef S);
-
-  // StringRefs can be treated like a POD type.
-  template <typename T> struct isPodLike;
-  template <> struct isPodLike<StringRef> { static const bool value = true; };
-
-} // end namespace wpi
-
-#endif // LLVM_ADT_STRINGREF_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SymbolExports.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SymbolExports.h
new file mode 100644
index 0000000..4e84825
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/SymbolExports.h
@@ -0,0 +1,23 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#ifdef _WIN32
+#pragma warning(disable : 4251)
+
+#ifdef WPILIB_EXPORTS
+#ifdef __GNUC__
+#define WPILIB_DLLEXPORT __attribute__((dllexport))
+#else
+#define WPILIB_DLLEXPORT __declspec(dllexport)
+#endif
+
+#else
+#define WPILIB_DLLEXPORT
+#endif
+
+#else
+#define WPILIB_DLLEXPORT __attribute__((visibility("default")))
+#endif
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Synchronization.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Synchronization.h
new file mode 100644
index 0000000..8050358
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Synchronization.h
@@ -0,0 +1,610 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <limits.h>  // NOLINT
+
+#ifdef __cplusplus
+#include <initializer_list>
+
+#include "wpi/span.h"
+#endif
+
+/**
+ * Generic handle for all WPI handle-based interfaces.
+ *
+ * Handle data layout:
+ * - Bits 0-23:  Type-specific
+ * - Bits 24-30: Type
+ * - Bit 31:     Error
+ */
+typedef unsigned int WPI_Handle;  // NOLINT
+
+/** An event handle. */
+typedef WPI_Handle WPI_EventHandle;  // NOLINT
+
+/** A semaphore handle. */
+typedef WPI_Handle WPI_SemaphoreHandle;  // NOLINT
+
+#ifdef __cplusplus
+
+namespace wpi {
+
+/** Constant representing an invalid handle. */
+constexpr unsigned int kInvalidHandle = 0;
+
+/**
+ * Standard types for handles.
+ * @{
+ */
+constexpr int kHandleTypeEvent = 1;
+constexpr int kHandleTypeSemaphore = 2;
+constexpr int kHandleTypeCSBase = 3;
+constexpr int kHandleTypeNTBase = 16;
+constexpr int kHandleTypeHALBase = 32;
+constexpr int kHandleTypeUserBase = 64;
+/** @} */
+
+/**
+ * Creates an event.  Events have binary state (signaled or not signaled) and
+ * may be either automatically reset or manually reset.  Automatic-reset events
+ * go to non-signaled state when a WaitForObject is woken up by the event;
+ * manual-reset events require ResetEvent() to be called to set the event to
+ * non-signaled state; if ResetEvent() is not called, any waiter on that event
+ * will immediately wake when called.
+ *
+ * @param manualReset true for manual reset, false for automatic reset
+ * @param initialState true to make the event initially in signaled state
+ * @return Event handle
+ */
+WPI_EventHandle CreateEvent(bool manualReset = false,
+                            bool initialState = false);
+
+/**
+ * Destroys an event.  Destruction wakes up any waiters.
+ *
+ * @param handle event handle
+ */
+void DestroyEvent(WPI_EventHandle handle);
+
+/**
+ * Sets an event to signaled state.
+ *
+ * @param handle event handle
+ */
+void SetEvent(WPI_EventHandle handle);
+
+/**
+ * Sets an event to non-signaled state.
+ *
+ * @param handle event handle
+ */
+void ResetEvent(WPI_EventHandle handle);
+
+/**
+ * Creates a semaphore.  Semaphores keep an internal counter.  Releasing the
+ * semaphore increases the count.  A semaphore with a non-zero count is
+ * considered signaled.  When a waiter wakes up it atomically decrements the
+ * count by 1.  This is generally useful in a single-supplier,
+ * multiple-consumer scenario.
+ *
+ * @param initialCount initial value for the semaphore's internal counter
+ * @param maximumCount maximum value for the samephore's internal counter
+ * @return Semaphore handle
+ */
+WPI_SemaphoreHandle CreateSemaphore(int initialCount = 0,
+                                    int maximumCount = INT_MAX);
+
+/**
+ * Destroys a semaphore.  Destruction wakes up any waiters.
+ *
+ * @param handle semaphore handle
+ */
+void DestroySemaphore(WPI_SemaphoreHandle handle);
+
+/**
+ * Releases N counts of a semaphore.
+ *
+ * @param handle semaphore handle
+ * @param releaseCount amount to add to semaphore's internal counter;
+ *        must be positive
+ * @param prevCount if non-null, previous count (output parameter)
+ * @return True on successful release, false on failure (e.g. release count
+ *         would exceed maximum value, or handle invalid)
+ */
+bool ReleaseSemaphore(WPI_SemaphoreHandle handle, int releaseCount = 1,
+                      int* prevCount = nullptr);
+
+/**
+ * Waits for an handle to be signaled.
+ *
+ * @param handle handle to wait on
+ * @return True if handle was signaled, false otherwise (e.g. object was
+ *         destroyed)
+ */
+bool WaitForObject(WPI_Handle handle);
+
+/**
+ * Waits for an handle to be signaled, with timeout.
+ *
+ * @param handle handle to wait on
+ * @param timeout timeout in seconds
+ * @param timedOut if non-null, set to true if timeout reached without handle
+ *        being signaled; set to false otherwise (output)
+ * @return True if handle was signaled, false otherwise (e.g. object was
+ *         destroyed or timed out)
+ */
+bool WaitForObject(WPI_Handle handle, double timeout, bool* timedOut);
+
+/**
+ * Waits for one or more handles to be signaled.
+ *
+ * Invalid handles are treated as signaled; the returned array will have the
+ * handle error bit set for any invalid handles.
+ *
+ * @param handles array of handles to wait on
+ * @param signaled output array for storage of signaled handles; must be at
+ *        least the size of the handles input array
+ * @return array of signaled handles (points into signaled array)
+ */
+wpi::span<WPI_Handle> WaitForObjects(wpi::span<const WPI_Handle> handles,
+                                     wpi::span<WPI_Handle> signaled);
+
+/**
+ * Waits for one or more handles to be signaled.
+ *
+ * Invalid handles are treated as signaled; the returned array will have the
+ * handle error bit set for any invalid handles.
+ *
+ * @param handles array of handles to wait on
+ * @param signaled output array for storage of signaled handles; must be at
+ *        least the size of the handles input array
+ * @return array of signaled handles (points into signaled array)
+ */
+inline wpi::span<WPI_Handle> WaitForObjects(
+    std::initializer_list<WPI_Handle> handles, wpi::span<WPI_Handle> signaled) {
+  return WaitForObjects(wpi::span{handles.begin(), handles.size()}, signaled);
+}
+
+/**
+ * Waits for one or more handles to be signaled, with timeout.
+ *
+ * Invalid handles are treated as signaled; the returned array will have the
+ * handle error bit set for any invalid handles.
+ *
+ * @param handles array of handles to wait on
+ * @param signaled output array for storage of signaled handles; must be at
+ *        least the size of the handles input array
+ * @param timeout timeout in seconds
+ * @param timedOut if non-null, set to true if timeout reached without any
+ *        handle being signaled; set to false otherwise (output)
+ * @return array of signaled handles (points into signaled array)
+ */
+wpi::span<WPI_Handle> WaitForObjects(wpi::span<const WPI_Handle> handles,
+                                     wpi::span<WPI_Handle> signaled,
+                                     double timeout, bool* timedOut);
+/**
+ * Waits for one or more handles to be signaled, with timeout.
+ *
+ * Invalid handles are treated as signaled; the returned array will have the
+ * handle error bit set for any invalid handles.
+ *
+ * @param handles array of handles to wait on
+ * @param signaled output array for storage of signaled handles; must be at
+ *        least the size of the handles input array
+ * @param timeout timeout in seconds
+ * @param timedOut if non-null, set to true if timeout reached without any
+ *        handle being signaled; set to false otherwise (output)
+ * @return array of signaled handles (points into signaled array)
+ */
+inline wpi::span<WPI_Handle> WaitForObjects(
+    std::initializer_list<WPI_Handle> handles, wpi::span<WPI_Handle> signaled,
+    double timeout, bool* timedOut) {
+  return WaitForObjects(wpi::span{handles.begin(), handles.size()}, signaled,
+                        timeout, timedOut);
+}
+
+/**
+ * Sets up signaling for an arbitrary handle.  With this function, any handle
+ * can operate like an event handle.
+ *
+ * @param handle handle
+ * @param manualReset true for manual reset, false for automatic reset
+ * @param initialState true to make the handle initially in signaled state
+ * @return Event handle
+ */
+void CreateSignalObject(WPI_Handle handle, bool manualReset = false,
+                        bool initialState = false);
+
+/**
+ * Sets a handle to signaled state.
+ *
+ * @param handle handle
+ */
+void SetSignalObject(WPI_Handle handle);
+
+/**
+ * Sets a handle to non-signaled state.
+ *
+ * @param handle handle
+ */
+void ResetSignalObject(WPI_Handle handle);
+
+/**
+ * Cleans up signaling for a handle.  Destruction wakes up any waiters.
+ *
+ * @param handle handle
+ */
+void DestroySignalObject(WPI_Handle handle);
+
+/**
+ * An atomic signaling event for synchronization.
+ *
+ * Events have binary state (signaled or not signaled) and may be either
+ * automatically reset or manually reset.  Automatic-reset events go to
+ * non-signaled state when a WaitForObject is woken up by the event;
+ * manual-reset events require Reset() to be called to set the event to
+ * non-signaled state; if Reset() is not called, any waiter on that event
+ * will immediately wake when called.
+ */
+class Event final {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param manualReset true for manual reset, false for automatic reset
+   * @param initialState true to make the event initially in signaled state
+   */
+  explicit Event(bool manualReset = false, bool initialState = false)
+      : m_handle{CreateEvent(manualReset, initialState)} {}
+  ~Event() {
+    if (m_handle != 0) {
+      DestroyEvent(m_handle);
+    }
+  }
+
+  Event(const Event&) = delete;
+  Event& operator=(const Event&) = delete;
+
+  Event(Event&& rhs) : m_handle{rhs.m_handle} { rhs.m_handle = 0; }
+  Event& operator=(Event&& rhs) {
+    if (m_handle != 0) {
+      DestroyEvent(m_handle);
+    }
+    m_handle = rhs.m_handle;
+    rhs.m_handle = 0;
+    return *this;
+  }
+
+  /**
+   * Gets the event handle (e.g. for WaitForObject).
+   *
+   * @return handle
+   */
+  explicit operator WPI_Handle() const { return m_handle; }
+
+  /**
+   * Gets the event handle (e.g. for WaitForObject).
+   *
+   * @return handle
+   */
+  WPI_EventHandle GetHandle() const { return m_handle; }
+
+  /**
+   * Sets the event to signaled state.
+   */
+  void Set() { SetEvent(m_handle); }
+
+  /**
+   * Sets the event to non-signaled state.
+   */
+  void Reset() { ResetEvent(m_handle); }
+
+ private:
+  WPI_EventHandle m_handle;
+};
+
+/**
+ * A semaphore for synchronization.
+ *
+ * Semaphores keep an internal counter.  Releasing the semaphore increases
+ * the count.  A semaphore with a non-zero count is considered signaled.
+ * When a waiter wakes up it atomically decrements the count by 1.  This
+ * is generally useful in a single-supplier, multiple-consumer scenario.
+ */
+class Semaphore final {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param initialCount initial value for the semaphore's internal counter
+   * @param maximumCount maximum value for the samephore's internal counter
+   */
+  explicit Semaphore(int initialCount = 0, int maximumCount = INT_MAX)
+      : m_handle{CreateSemaphore(initialCount, maximumCount)} {}
+  ~Semaphore() {
+    if (m_handle != 0) {
+      DestroySemaphore(m_handle);
+    }
+  }
+
+  Semaphore(const Semaphore&) = delete;
+  Semaphore& operator=(const Semaphore&) = delete;
+
+  Semaphore(Semaphore&& rhs) : m_handle{rhs.m_handle} { rhs.m_handle = 0; }
+  Semaphore& operator=(Semaphore&& rhs) {
+    if (m_handle != 0) {
+      DestroySemaphore(m_handle);
+    }
+    m_handle = rhs.m_handle;
+    rhs.m_handle = 0;
+    return *this;
+  }
+
+  /**
+   * Gets the semaphore handle (e.g. for WaitForObject).
+   *
+   * @return handle
+   */
+  explicit operator WPI_Handle() const { return m_handle; }
+
+  /**
+   * Gets the semaphore handle (e.g. for WaitForObject).
+   *
+   * @return handle
+   */
+  WPI_SemaphoreHandle GetHandle() const { return m_handle; }
+
+  /**
+   * Releases N counts of the semaphore.
+   *
+   * @param releaseCount amount to add to semaphore's internal counter;
+   *        must be positive
+   * @param prevCount if non-null, previous count (output parameter)
+   * @return True on successful release, false on failure (e.g. release count
+   *         would exceed maximum value, or handle invalid)
+   */
+  bool Release(int releaseCount = 1, int* prevCount = nullptr) {
+    return ReleaseSemaphore(m_handle, releaseCount, prevCount);
+  }
+
+ private:
+  WPI_SemaphoreHandle m_handle;
+};
+
+/**
+ * RAII wrapper for signaling handles.
+ *
+ * Sets up signaling for an arbitrary handle.  This enables any handle
+ * to operate like an event handle.
+ */
+template <typename T>
+class SignalObject final {
+ public:
+  /**
+   * Constructor.
+   *
+   * @param handle handle
+   * @param manualReset true for manual reset, false for automatic reset
+   * @param initialState true to make the handle initially in signaled state
+   */
+  explicit SignalObject(T handle = 0, bool manualReset = false,
+                        bool initialState = false)
+      : m_handle{handle} {
+    CreateSignalObject(handle, manualReset, initialState);
+  }
+  ~SignalObject() {
+    if (m_handle != 0) {
+      DestroySignalObject(m_handle);
+    }
+  }
+
+  SignalObject(const SignalObject&) = delete;
+  SignalObject& operator=(const SignalObject&) = delete;
+
+  SignalObject(SignalObject&& rhs) : m_handle{rhs.m_handle} {
+    rhs.m_handle = 0;
+  }
+  SignalObject& operator=(SignalObject&& rhs) {
+    if (m_handle != 0) {
+      DestroySemaphore(m_handle);
+    }
+    m_handle = rhs.m_handle;
+    rhs.m_handle = 0;
+    return *this;
+  }
+
+  /**
+   * Gets the handle.
+   *
+   * @return handle
+   */
+  /*implicit*/ operator T() const { return m_handle; }  // NOLINT
+
+  /**
+   * Gets the handle (e.g. for WaitForObject).
+   *
+   * @return handle
+   */
+  T GetHandle() const { return m_handle; }
+
+  /**
+   * Sets the handle to signaled state.
+   */
+  void Set() { SetSignalObject(m_handle); }
+
+  /**
+   * Sets the handle to non-signaled state.
+   */
+  void Reset() { ResetSignalObject(m_handle); }
+
+ private:
+  T m_handle;
+};
+
+}  // namespace wpi
+
+extern "C" {
+
+#endif  // __cplusplus
+
+/**
+ * Creates an event.  Events have binary state (signaled or not signaled) and
+ * may be either automatically reset or manually reset.  Automatic-reset events
+ * go to non-signaled state when a WaitForObject is woken up by the event;
+ * manual-reset events require ResetEvent() to be called to set the event to
+ * non-signaled state; if ResetEvent() is not called, any waiter on that event
+ * will immediately wake when called.
+ *
+ * @param manual_reset true for manual reset, false for automatic reset
+ * @param initial_state true to make the event initially in signaled state
+ * @return Event handle
+ */
+WPI_EventHandle WPI_CreateEvent(int manual_reset, int initial_state);
+
+/**
+ * Destroys an event.  Destruction wakes up any waiters.
+ *
+ * @param handle event handle
+ */
+void WPI_DestroyEvent(WPI_EventHandle handle);
+
+/**
+ * Sets an event to signaled state.
+ *
+ * @param handle event handle
+ */
+void WPI_SetEvent(WPI_EventHandle handle);
+
+/**
+ * Sets an event to non-signaled state.
+ *
+ * @param handle event handle
+ */
+void WPI_ResetEvent(WPI_EventHandle handle);
+
+/**
+ * Creates a semaphore.  Semaphores keep an internal counter.  Releasing the
+ * semaphore increases the count.  A semaphore with a non-zero count is
+ * considered signaled.  When a waiter wakes up it atomically decrements the
+ * count by 1.  This is generally useful in a single-supplier,
+ * multiple-consumer scenario.
+ *
+ * @param initial_count initial value for the semaphore's internal counter
+ * @param maximum_count maximum value for the samephore's internal counter
+ * @return Semaphore handle
+ */
+WPI_SemaphoreHandle WPI_CreateSemaphore(int initial_count, int maximum_count);
+
+/**
+ * Destroys a semaphore.  Destruction wakes up any waiters.
+ *
+ * @param handle semaphore handle
+ */
+void WPI_DestroySemaphore(WPI_SemaphoreHandle handle);
+
+/**
+ * Releases N counts of a semaphore.
+ *
+ * @param handle semaphore handle
+ * @param release_count amount to add to semaphore's internal counter;
+ *        must be positive
+ * @param prev_count if non-null, previous count (output parameter)
+ * @return Non-zero on successful release, zero on failure (e.g. release count
+ *         would exceed maximum value, or handle invalid)
+ */
+int WPI_ReleaseSemaphore(WPI_SemaphoreHandle handle, int release_count,
+                         int* prev_count);
+
+/**
+ * Waits for an handle to be signaled.
+ *
+ * @param handle handle to wait on
+ * @return Non-zero if handle was signaled, zero otherwise (e.g. object was
+ *         destroyed)
+ */
+int WPI_WaitForObject(WPI_Handle handle);
+
+/**
+ * Waits for an handle to be signaled, with timeout.
+ *
+ * @param handle handle to wait on
+ * @param timeout timeout in seconds
+ * @param timed_out if non-null, set to non-zero if timeout reached without
+ *        handle being signaled; set to zero otherwise (output)
+ * @return Non-zero if handle was signaled, zero otherwise (e.g. object was
+ *         destroyed or timed out)
+ */
+int WPI_WaitForObjectTimeout(WPI_Handle handle, double timeout, int* timed_out);
+
+/**
+ * Waits for one or more handles to be signaled.
+ *
+ * Invalid handles are treated as signaled; the returned array will have the
+ * handle error bit set for any invalid handles.
+ *
+ * @param handles array of handles to wait on
+ * @param handles_count length of the handles array
+ * @param signaled output array for storage of signaled handles; must be at
+ *        least the size of the handles input array
+ * @return number of signaled handles
+ */
+int WPI_WaitForObjects(const WPI_Handle* handles, int handles_count,
+                       WPI_Handle* signaled);
+
+/**
+ * Waits for one or more handles to be signaled, with timeout.
+ *
+ * Invalid handles are treated as signaled; the returned array will have the
+ * handle error bit set for any invalid handles.
+ *
+ * @param handles array of handles to wait on
+ * @param handles_count length of the handles array
+ * @param signaled output array for storage of signaled handles; must be at
+ *        least the size of the handles input array
+ * @param timeout timeout in seconds
+ * @param timed_out if non-null, set to non-zero if timeout reached without any
+ *        handle being signaled; set to zero otherwise (output)
+ * @return number of signaled handles
+ */
+int WPI_WaitForObjectsTimeout(const WPI_Handle* handles, int handles_count,
+                              WPI_Handle* signaled, double timeout,
+                              int* timed_out);
+
+/**
+ * Sets up signaling for an arbitrary handle.  With this function, any handle
+ * can operate like an event handle.
+ *
+ * @param handle handle
+ * @param manual_reset true for manual reset, false for automatic reset
+ * @param initial_state true to make the handle initially in signaled state
+ */
+void WPI_CreateSignalObject(WPI_Handle handle, int manual_reset,
+                            int initial_state);
+
+/**
+ * Sets a handle to signaled state.
+ *
+ * @param handle handle
+ */
+void WPI_SetSignalObject(WPI_Handle handle);
+
+/**
+ * Sets a handle to non-signaled state.
+ *
+ * @param handle handle
+ */
+void WPI_ResetSignalObject(WPI_Handle handle);
+
+/**
+ * Cleans up signaling for a handle.  Destruction wakes up any waiters.
+ *
+ * @param handle handle
+ */
+void WPI_DestroySignalObject(WPI_Handle handle);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPAcceptor.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPAcceptor.h
index da82a8a..083897a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPAcceptor.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPAcceptor.h
@@ -27,6 +27,7 @@
 #include <atomic>
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include "wpi/NetworkAcceptor.h"
 #include "wpi/TCPStream.h"
@@ -44,11 +45,11 @@
   Logger& m_logger;
 
  public:
-  TCPAcceptor(int port, const char* address, Logger& logger);
-  ~TCPAcceptor();
+  TCPAcceptor(int port, std::string_view address, Logger& logger);
+  ~TCPAcceptor() override;
 
   int start() override;
-  void shutdown() override;
+  void shutdown() final;
   std::unique_ptr<NetworkStream> accept() override;
 };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPConnector.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPConnector.h
index a05342b..63e8906 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPConnector.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPConnector.h
@@ -27,8 +27,8 @@
 #include <memory>
 #include <utility>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/NetworkStream.h"
+#include "wpi/span.h"
 
 namespace wpi {
 
@@ -40,7 +40,7 @@
                                                 Logger& logger,
                                                 int timeout = 0);
   static std::unique_ptr<NetworkStream> connect_parallel(
-      ArrayRef<std::pair<const char*, int>> servers, Logger& logger,
+      span<const std::pair<const char*, int>> servers, Logger& logger,
       int timeout = 0);
 };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPStream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPStream.h
index 0c6e8a9..2d54300 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPStream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/TCPStream.h
@@ -26,6 +26,7 @@
 
 #include <cstddef>
 #include <string>
+#include <string_view>
 
 #include "wpi/NetworkStream.h"
 
@@ -43,14 +44,14 @@
   friend class TCPAcceptor;
   friend class TCPConnector;
 
-  ~TCPStream();
+  ~TCPStream() override;
 
   size_t send(const char* buffer, size_t len, Error* err) override;
   size_t receive(char* buffer, size_t len, Error* err,
                  int timeout = 0) override;
-  void close() override;
+  void close() final;
 
-  StringRef getPeerIP() const override;
+  std::string_view getPeerIP() const override;
   int getPeerPort() const override;
   void setNoDelay() override;
   bool setBlocking(bool enabled) override;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Twine.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Twine.h
deleted file mode 100644
index a5b7e97..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/Twine.h
+++ /dev/null
@@ -1,543 +0,0 @@
-//===- Twine.h - Fast Temporary String Concatenation ------------*- C++ -*-===//
-//
-//                     The LLVM Compiler Infrastructure
-//
-// This file is distributed under the University of Illinois Open Source
-// License. See LICENSE.TXT for details.
-//
-//===----------------------------------------------------------------------===//
-
-#ifndef WPIUTIL_WPI_TWINE_H
-#define WPIUTIL_WPI_TWINE_H
-
-#include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
-#include "wpi/ErrorHandling.h"
-#include <cassert>
-#include <cstdint>
-#include <string>
-
-#ifdef _WIN32
-#pragma warning(push)
-#pragma warning(disable : 26495)
-#endif
-
-namespace wpi {
-
-  class raw_ostream;
-
-  /// Twine - A lightweight data structure for efficiently representing the
-  /// concatenation of temporary values as strings.
-  ///
-  /// A Twine is a kind of rope, it represents a concatenated string using a
-  /// binary-tree, where the string is the preorder of the nodes. Since the
-  /// Twine can be efficiently rendered into a buffer when its result is used,
-  /// it avoids the cost of generating temporary values for intermediate string
-  /// results -- particularly in cases when the Twine result is never
-  /// required. By explicitly tracking the type of leaf nodes, we can also avoid
-  /// the creation of temporary strings for conversions operations (such as
-  /// appending an integer to a string).
-  ///
-  /// A Twine is not intended for use directly and should not be stored, its
-  /// implementation relies on the ability to store pointers to temporary stack
-  /// objects which may be deallocated at the end of a statement. Twines should
-  /// only be used accepted as const references in arguments, when an API wishes
-  /// to accept possibly-concatenated strings.
-  ///
-  /// Twines support a special 'null' value, which always concatenates to form
-  /// itself, and renders as an empty string. This can be returned from APIs to
-  /// effectively nullify any concatenations performed on the result.
-  ///
-  /// \b Implementation
-  ///
-  /// Given the nature of a Twine, it is not possible for the Twine's
-  /// concatenation method to construct interior nodes; the result must be
-  /// represented inside the returned value. For this reason a Twine object
-  /// actually holds two values, the left- and right-hand sides of a
-  /// concatenation. We also have nullary Twine objects, which are effectively
-  /// sentinel values that represent empty strings.
-  ///
-  /// Thus, a Twine can effectively have zero, one, or two children. The \see
-  /// isNullary(), \see isUnary(), and \see isBinary() predicates exist for
-  /// testing the number of children.
-  ///
-  /// We maintain a number of invariants on Twine objects (FIXME: Why):
-  ///  - Nullary twines are always represented with their Kind on the left-hand
-  ///    side, and the Empty kind on the right-hand side.
-  ///  - Unary twines are always represented with the value on the left-hand
-  ///    side, and the Empty kind on the right-hand side.
-  ///  - If a Twine has another Twine as a child, that child should always be
-  ///    binary (otherwise it could have been folded into the parent).
-  ///
-  /// These invariants are check by \see isValid().
-  ///
-  /// \b Efficiency Considerations
-  ///
-  /// The Twine is designed to yield efficient and small code for common
-  /// situations. For this reason, the concat() method is inlined so that
-  /// concatenations of leaf nodes can be optimized into stores directly into a
-  /// single stack allocated object.
-  ///
-  /// In practice, not all compilers can be trusted to optimize concat() fully,
-  /// so we provide two additional methods (and accompanying operator+
-  /// overloads) to guarantee that particularly important cases (cstring plus
-  /// StringRef) codegen as desired.
-  class Twine {
-    /// NodeKind - Represent the type of an argument.
-    enum NodeKind : unsigned char {
-      /// An empty string; the result of concatenating anything with it is also
-      /// empty.
-      NullKind,
-
-      /// The empty string.
-      EmptyKind,
-
-      /// A pointer to a Twine instance.
-      TwineKind,
-
-      /// A pointer to a C string instance.
-      CStringKind,
-
-      /// A pointer to an std::string instance.
-      StdStringKind,
-
-      /// A pointer to a StringRef instance.
-      StringRefKind,
-
-      /// A pointer to a SmallString instance.
-      SmallStringKind,
-
-      /// A char value, to render as a character.
-      CharKind,
-
-      /// An unsigned int value, to render as an unsigned decimal integer.
-      DecUIKind,
-
-      /// An int value, to render as a signed decimal integer.
-      DecIKind,
-
-      /// A pointer to an unsigned long value, to render as an unsigned decimal
-      /// integer.
-      DecULKind,
-
-      /// A pointer to a long value, to render as a signed decimal integer.
-      DecLKind,
-
-      /// A pointer to an unsigned long long value, to render as an unsigned
-      /// decimal integer.
-      DecULLKind,
-
-      /// A pointer to a long long value, to render as a signed decimal integer.
-      DecLLKind,
-
-      /// A pointer to a uint64_t value, to render as an unsigned hexadecimal
-      /// integer.
-      UHexKind
-    };
-
-    union Child
-    {
-      const Twine *twine;
-      const char *cString;
-      const std::string *stdString;
-      const StringRef *stringRef;
-      const SmallVectorImpl<char> *smallString;
-      char character;
-      unsigned int decUI;
-      int decI;
-      const unsigned long *decUL;
-      const long *decL;
-      const unsigned long long *decULL;
-      const long long *decLL;
-      const uint64_t *uHex;
-    };
-
-    /// LHS - The prefix in the concatenation, which may be uninitialized for
-    /// Null or Empty kinds.
-    Child LHS;
-
-    /// RHS - The suffix in the concatenation, which may be uninitialized for
-    /// Null or Empty kinds.
-    Child RHS;
-
-    /// LHSKind - The NodeKind of the left hand side, \see getLHSKind().
-    NodeKind LHSKind = EmptyKind;
-
-    /// RHSKind - The NodeKind of the right hand side, \see getRHSKind().
-    NodeKind RHSKind = EmptyKind;
-
-    /// Construct a nullary twine; the kind must be NullKind or EmptyKind.
-    explicit Twine(NodeKind Kind) : LHSKind(Kind) {
-      assert(isNullary() && "Invalid kind!");
-    }
-
-    /// Construct a binary twine.
-    explicit Twine(const Twine &LHS, const Twine &RHS)
-        : LHSKind(TwineKind), RHSKind(TwineKind) {
-      this->LHS.twine = &LHS;
-      this->RHS.twine = &RHS;
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Construct a twine from explicit values.
-    explicit Twine(Child LHS, NodeKind LHSKind, Child RHS, NodeKind RHSKind)
-        : LHS(LHS), RHS(RHS), LHSKind(LHSKind), RHSKind(RHSKind) {
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Check for the empty twine.
-    bool isEmpty() const {
-      return getLHSKind() == EmptyKind;
-    }
-
-    /// Check if this is a nullary twine (null or empty).
-    bool isNullary() const {
-      return isNull() || isEmpty();
-    }
-
-    /// Check if this is a unary twine.
-    bool isUnary() const {
-      return getRHSKind() == EmptyKind && !isNullary();
-    }
-
-    /// Check if this is a binary twine.
-    bool isBinary() const {
-      return getLHSKind() != NullKind && getRHSKind() != EmptyKind;
-    }
-
-    /// Check if this is a valid twine (satisfying the invariants on
-    /// order and number of arguments).
-    bool isValid() const {
-      // Nullary twines always have Empty on the RHS.
-      if (isNullary() && getRHSKind() != EmptyKind)
-        return false;
-
-      // Null should never appear on the RHS.
-      if (getRHSKind() == NullKind)
-        return false;
-
-      // The RHS cannot be non-empty if the LHS is empty.
-      if (getRHSKind() != EmptyKind && getLHSKind() == EmptyKind)
-        return false;
-#if 0 // causes spurious warnings
-      // A twine child should always be binary.
-      if (getLHSKind() == TwineKind &&
-          !LHS.twine->isBinary())
-        return false;
-      if (getRHSKind() == TwineKind &&
-          !RHS.twine->isBinary())
-        return false;
-#endif
-      return true;
-    }
-
-    /// Get the NodeKind of the left-hand side.
-    NodeKind getLHSKind() const { return LHSKind; }
-
-    /// Get the NodeKind of the right-hand side.
-    NodeKind getRHSKind() const { return RHSKind; }
-
-    /// Print one child from a twine.
-    void printOneChild(raw_ostream &OS, Child Ptr, NodeKind Kind) const;
-
-    /// Print the representation of one child from a twine.
-    void printOneChildRepr(raw_ostream &OS, Child Ptr,
-                           NodeKind Kind) const;
-
-  public:
-    /// @name Constructors
-    /// @{
-
-    /// Construct from an empty string.
-    /*implicit*/ Twine() {
-      assert(isValid() && "Invalid twine!");
-    }
-
-    Twine(const Twine &) = default;
-
-    /// Construct from a C string.
-    ///
-    /// We take care here to optimize "" into the empty twine -- this will be
-    /// optimized out for string constants. This allows Twine arguments have
-    /// default "" values, without introducing unnecessary string constants.
-    /*implicit*/ Twine(const char *Str) {
-      if (Str[0] != '\0') {
-        LHS.cString = Str;
-        LHSKind = CStringKind;
-      } else
-        LHSKind = EmptyKind;
-
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Construct from an std::string.
-    /*implicit*/ Twine(const std::string &Str) : LHSKind(StdStringKind) {
-      LHS.stdString = &Str;
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Construct from a StringRef.
-    /*implicit*/ Twine(const StringRef &Str) : LHSKind(StringRefKind) {
-      LHS.stringRef = &Str;
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Construct from a SmallString.
-    /*implicit*/ Twine(const SmallVectorImpl<char> &Str)
-        : LHSKind(SmallStringKind) {
-      LHS.smallString = &Str;
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Construct from a char.
-    explicit Twine(char Val) : LHSKind(CharKind) {
-      LHS.character = Val;
-    }
-
-    /// Construct from a signed char.
-    explicit Twine(signed char Val) : LHSKind(CharKind) {
-      LHS.character = static_cast<char>(Val);
-    }
-
-    /// Construct from an unsigned char.
-    explicit Twine(unsigned char Val) : LHSKind(CharKind) {
-      LHS.character = static_cast<char>(Val);
-    }
-
-    /// Construct a twine to print \p Val as an unsigned decimal integer.
-    explicit Twine(unsigned Val) : LHSKind(DecUIKind) {
-      LHS.decUI = Val;
-    }
-
-    /// Construct a twine to print \p Val as a signed decimal integer.
-    explicit Twine(int Val) : LHSKind(DecIKind) {
-      LHS.decI = Val;
-    }
-
-    /// Construct a twine to print \p Val as an unsigned decimal integer.
-    explicit Twine(const unsigned long &Val) : LHSKind(DecULKind) {
-      LHS.decUL = &Val;
-    }
-
-    /// Construct a twine to print \p Val as a signed decimal integer.
-    explicit Twine(const long &Val) : LHSKind(DecLKind) {
-      LHS.decL = &Val;
-    }
-
-    /// Construct a twine to print \p Val as an unsigned decimal integer.
-    explicit Twine(const unsigned long long &Val) : LHSKind(DecULLKind) {
-      LHS.decULL = &Val;
-    }
-
-    /// Construct a twine to print \p Val as a signed decimal integer.
-    explicit Twine(const long long &Val) : LHSKind(DecLLKind) {
-      LHS.decLL = &Val;
-    }
-
-    // FIXME: Unfortunately, to make sure this is as efficient as possible we
-    // need extra binary constructors from particular types. We can't rely on
-    // the compiler to be smart enough to fold operator+()/concat() down to the
-    // right thing. Yet.
-
-    /// Construct as the concatenation of a C string and a StringRef.
-    /*implicit*/ Twine(const char *LHS, const StringRef &RHS)
-        : LHSKind(CStringKind), RHSKind(StringRefKind) {
-      this->LHS.cString = LHS;
-      this->RHS.stringRef = &RHS;
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Construct as the concatenation of a StringRef and a C string.
-    /*implicit*/ Twine(const StringRef &LHS, const char *RHS)
-        : LHSKind(StringRefKind), RHSKind(CStringKind) {
-      this->LHS.stringRef = &LHS;
-      this->RHS.cString = RHS;
-      assert(isValid() && "Invalid twine!");
-    }
-
-    /// Since the intended use of twines is as temporary objects, assignments
-    /// when concatenating might cause undefined behavior or stack corruptions
-    Twine &operator=(const Twine &) = delete;
-
-    /// Create a 'null' string, which is an empty string that always
-    /// concatenates to form another empty string.
-    static Twine createNull() {
-      return Twine(NullKind);
-    }
-
-    /// @}
-    /// @name Numeric Conversions
-    /// @{
-
-    // Construct a twine to print \p Val as an unsigned hexadecimal integer.
-    static Twine utohexstr(const uint64_t &Val) {
-      Child LHS, RHS;
-      LHS.uHex = &Val;
-      RHS.twine = nullptr;
-      return Twine(LHS, UHexKind, RHS, EmptyKind);
-    }
-
-    /// @}
-    /// @name Predicate Operations
-    /// @{
-
-    /// Check for the null twine.
-    bool isNull() const {
-      return getLHSKind() == NullKind;
-    }
-
-    /// Check if this twine is trivially empty; a false return value does not
-    /// necessarily mean the twine is empty.
-    bool isTriviallyEmpty() const {
-      return isNullary();
-    }
-
-    /// Return true if this twine can be dynamically accessed as a single
-    /// StringRef value with getSingleStringRef().
-    bool isSingleStringRef() const {
-      if (getRHSKind() != EmptyKind) return false;
-
-      switch (getLHSKind()) {
-      case EmptyKind:
-      case CStringKind:
-      case StdStringKind:
-      case StringRefKind:
-      case SmallStringKind:
-      case CharKind:
-        return true;
-      default:
-        return false;
-      }
-    }
-
-    /// @}
-    /// @name String Operations
-    /// @{
-
-    Twine concat(const Twine &Suffix) const;
-
-    /// @}
-    /// @name Output & Conversion.
-    /// @{
-
-    /// Return the twine contents as a std::string.
-    std::string str() const;
-
-    /// Append the concatenated string into the given SmallString or SmallVector.
-    void toVector(SmallVectorImpl<char> &Out) const;
-
-    /// This returns the twine as a single StringRef.  This method is only valid
-    /// if isSingleStringRef() is true.
-    StringRef getSingleStringRef() const {
-      assert(isSingleStringRef() &&"This cannot be had as a single stringref!");
-      switch (getLHSKind()) {
-      default:
-        // unreachable("Out of sync with isSingleStringRef");
-        return StringRef();
-      case EmptyKind:      return StringRef();
-      case CStringKind:    return StringRef(LHS.cString);
-      case StdStringKind:  return StringRef(*LHS.stdString);
-      case StringRefKind:  return *LHS.stringRef;
-      case SmallStringKind:
-        return StringRef(LHS.smallString->data(), LHS.smallString->size());
-      case CharKind:       return StringRef(&LHS.character, 1);
-      }
-    }
-
-    /// This returns the twine as a single StringRef if it can be
-    /// represented as such. Otherwise the twine is written into the given
-    /// SmallVector and a StringRef to the SmallVector's data is returned.
-    StringRef toStringRef(SmallVectorImpl<char> &Out) const {
-      if (isSingleStringRef())
-        return getSingleStringRef();
-      toVector(Out);
-      return StringRef(Out.data(), Out.size());
-    }
-
-    /// This returns the twine as a single null terminated StringRef if it
-    /// can be represented as such. Otherwise the twine is written into the
-    /// given SmallVector and a StringRef to the SmallVector's data is returned.
-    ///
-    /// The returned StringRef's size does not include the null terminator.
-    StringRef toNullTerminatedStringRef(SmallVectorImpl<char> &Out) const;
-
-    /// Write the concatenated string represented by this twine to the
-    /// stream \p OS.
-    void print(raw_ostream &OS) const;
-
-    /// Dump the concatenated string represented by this twine to stderr.
-    void dump() const;
-
-    /// Write the representation of this twine to the stream \p OS.
-    void printRepr(raw_ostream &OS) const;
-
-    /// Dump the representation of this twine to stderr.
-    void dumpRepr() const;
-
-    /// @}
-  };
-
-  /// @name Twine Inline Implementations
-  /// @{
-
-  inline Twine Twine::concat(const Twine &Suffix) const {
-    // Concatenation with null is null.
-    if (isNull() || Suffix.isNull())
-      return Twine(NullKind);
-
-    // Concatenation with empty yields the other side.
-    if (isEmpty())
-      return Suffix;
-    if (Suffix.isEmpty())
-      return *this;
-
-    // Otherwise we need to create a new node, taking care to fold in unary
-    // twines.
-    Child NewLHS, NewRHS;
-    NewLHS.twine = this;
-    NewRHS.twine = &Suffix;
-    NodeKind NewLHSKind = TwineKind, NewRHSKind = TwineKind;
-    if (isUnary()) {
-      NewLHS = LHS;
-      NewLHSKind = getLHSKind();
-    }
-    if (Suffix.isUnary()) {
-      NewRHS = Suffix.LHS;
-      NewRHSKind = Suffix.getLHSKind();
-    }
-
-    return Twine(NewLHS, NewLHSKind, NewRHS, NewRHSKind);
-  }
-
-  inline Twine operator+(const Twine &LHS, const Twine &RHS) {
-    return LHS.concat(RHS);
-  }
-
-  /// Additional overload to guarantee simplified codegen; this is equivalent to
-  /// concat().
-
-  inline Twine operator+(const char *LHS, const StringRef &RHS) {
-    return Twine(LHS, RHS);
-  }
-
-  /// Additional overload to guarantee simplified codegen; this is equivalent to
-  /// concat().
-
-  inline Twine operator+(const StringRef &LHS, const char *RHS) {
-    return Twine(LHS, RHS);
-  }
-
-  inline raw_ostream &operator<<(raw_ostream &OS, const Twine &RHS) {
-    RHS.print(OS);
-    return OS;
-  }
-
-  /// @}
-
-} // end namespace wpi
-
-#ifdef _WIN32
-#pragma warning(pop)
-#endif
-
-#endif // LLVM_ADT_TWINE_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UDPClient.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UDPClient.h
index 635eca8..fd2e30b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UDPClient.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UDPClient.h
@@ -1,20 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UDPCLIENT_H_
 #define WPIUTIL_WPI_UDPCLIENT_H_
 
 #include <string>
+#include <string_view>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
 #include "wpi/mutex.h"
+#include "wpi/span.h"
 
 namespace wpi {
 
@@ -28,7 +24,7 @@
 
  public:
   explicit UDPClient(Logger& logger);
-  UDPClient(const Twine& address, Logger& logger);
+  UDPClient(std::string_view address, Logger& logger);
   UDPClient(const UDPClient& other) = delete;
   UDPClient(UDPClient&& other);
   ~UDPClient();
@@ -40,8 +36,8 @@
   int start(int port);
   void shutdown();
   // The passed in address MUST be a resolved IP address.
-  int send(ArrayRef<uint8_t> data, const Twine& server, int port);
-  int send(StringRef data, const Twine& server, int port);
+  int send(span<const uint8_t> data, std::string_view server, int port);
+  int send(std::string_view data, std::string_view server, int port);
   int receive(uint8_t* data_received, int receive_len);
   int receive(uint8_t* data_received, int receive_len,
               SmallVectorImpl<char>* addr_received, int* port_received);
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UidVector.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UidVector.h
index 077dc27..3ffd4c3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UidVector.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UidVector.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UIDVECTOR_H_
 #define WPIUTIL_WPI_UIDVECTOR_H_
@@ -27,7 +24,9 @@
   UidVectorIterator() = default;
   explicit UidVectorIterator(It it, It end) : m_it(it), m_end(end) {
     // advance to first non-empty element
-    while (m_it != m_end && !*m_it) ++m_it;
+    while (m_it != m_end && !*m_it) {
+      ++m_it;
+    }
   }
 
   reference operator*() const noexcept { return *m_it; }
@@ -61,12 +60,15 @@
 };
 }  // namespace impl
 
-// Vector which provides an integrated freelist for removal and reuse of
-// individual elements.
-// @tparam T element type; must be default-constructible and evaluate in
-//           boolean context to false when "empty"
-// @tparam reuse_threshold how many free elements to store up before starting
-//                         to recycle them
+/**
+ * Vector which provides an integrated freelist for removal and reuse of
+ * individual elements.
+ *
+ * @tparam T element type; must be default-constructible and evaluate in
+ *           boolean context to false when "empty"
+ * @tparam reuse_threshold how many free elements to store up before starting
+ *                         to recycle them
+ */
 template <typename T, typename std::vector<T>::size_type reuse_threshold>
 class UidVector {
  public:
@@ -81,8 +83,8 @@
   using const_iterator =
       impl::UidVectorIterator<typename std::vector<T>::const_iterator>;
 
-  bool empty() const { return m_active_count == 0; }
-  size_type size() const { return m_vector.size(); }
+  bool empty() const noexcept { return m_active_count == 0; }
+  size_type size() const noexcept { return m_vector.size(); }
   T& operator[](size_type i) { return m_vector[i]; }
   const T& operator[](size_type i) const { return m_vector[i]; }
 
@@ -104,17 +106,25 @@
     return uid;
   }
 
-  // Removes the identified element by replacing it with a default-constructed
-  // one.  The element is added to the freelist for later reuse.
-  void erase(size_type uid) {
-    if (uid >= m_vector.size() || !m_vector[uid]) return;
+  /**
+   * Removes the identified element by replacing it with a default-constructed
+   * one. The element is added to the freelist for later reuse.
+   */
+  T erase(size_type uid) {
+    if (uid >= m_vector.size() || !m_vector[uid]) {
+      return T();
+    }
     m_free.push_back(uid);
+    auto rv = std::move(m_vector[uid]);
     m_vector[uid] = T();
     --m_active_count;
+    return rv;
   }
 
-  // Removes all elements.
-  void clear() {
+  /**
+   * Removes all elements.
+   */
+  void clear() noexcept {
     m_vector.clear();
     m_free.clear();
     m_active_count = 0;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UrlParser.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UrlParser.h
index e3da269..3b6107b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UrlParser.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/UrlParser.h
@@ -1,14 +1,12 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_URLPARSER_H_
 #define WPIUTIL_WPI_URLPARSER_H_
 
-#include "wpi/StringRef.h"
+#include <string_view>
+
 #include "wpi/http_parser.h"
 
 namespace wpi {
@@ -24,7 +22,7 @@
    * @param in input
    * @param isConnect
    */
-  UrlParser(StringRef in, bool isConnect) {
+  UrlParser(std::string_view in, bool isConnect) {
     m_data = in;
     http_parser_url_init(&m_url);
     m_error = http_parser_parse_url(in.data(), in.size(), isConnect, &m_url);
@@ -53,41 +51,41 @@
     return (m_url.field_set & (1 << UF_USERINFO)) != 0;
   }
 
-  StringRef GetSchema() const {
+  std::string_view GetSchema() const {
     return m_data.substr(m_url.field_data[UF_SCHEMA].off,
                          m_url.field_data[UF_SCHEMA].len);
   }
 
-  StringRef GetHost() const {
+  std::string_view GetHost() const {
     return m_data.substr(m_url.field_data[UF_HOST].off,
                          m_url.field_data[UF_HOST].len);
   }
 
   unsigned int GetPort() const { return m_url.port; }
 
-  StringRef GetPath() const {
+  std::string_view GetPath() const {
     return m_data.substr(m_url.field_data[UF_PATH].off,
                          m_url.field_data[UF_PATH].len);
   }
 
-  StringRef GetQuery() const {
+  std::string_view GetQuery() const {
     return m_data.substr(m_url.field_data[UF_QUERY].off,
                          m_url.field_data[UF_QUERY].len);
   }
 
-  StringRef GetFragment() const {
+  std::string_view GetFragment() const {
     return m_data.substr(m_url.field_data[UF_FRAGMENT].off,
                          m_url.field_data[UF_FRAGMENT].len);
   }
 
-  StringRef GetUserInfo() const {
+  std::string_view GetUserInfo() const {
     return m_data.substr(m_url.field_data[UF_USERINFO].off,
                          m_url.field_data[UF_USERINFO].len);
   }
 
  private:
   bool m_error;
-  StringRef m_data;
+  std::string_view m_data;
   http_parser_url m_url;
 };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/VersionTuple.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/VersionTuple.h
index e54ca92..59df41c 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/VersionTuple.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/VersionTuple.h
@@ -15,11 +15,8 @@
 #ifndef WPIUTIL_WPI_VERSIONTUPLE_H
 #define WPIUTIL_WPI_VERSIONTUPLE_H
 
-#include "wpi/StringRef.h"
-#include "wpi/raw_ostream.h"
 #include <optional>
 #include <string>
-#include <tuple>
 
 namespace wpi {
 
@@ -137,18 +134,7 @@
   friend bool operator>=(const VersionTuple &X, const VersionTuple &Y) {
     return !(X < Y);
   }
-
-  /// Retrieve a string representation of the version number.
-  std::string getAsString() const;
-
-  /// Try to parse the given string as a version number.
-  /// \returns \c true if the string does not match the regular expression
-  ///   [0-9]+(\.[0-9]+){0,3}
-  bool tryParse(StringRef string);
 };
 
-/// Print a version number.
-raw_ostream &operator<<(raw_ostream &Out, const VersionTuple &V);
-
 } // end namespace wpi
 #endif // WPIUTIL_WPI_VERSIONTUPLE_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocket.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocket.h
index 8347d0c..fc2ae4a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocket.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocket.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_WEBSOCKET_H_
 #define WPIUTIL_WPI_WEBSOCKET_H_
@@ -14,13 +11,12 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/Signal.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
+#include "wpi/span.h"
 #include "wpi/uv/Buffer.h"
 #include "wpi/uv/Error.h"
 #include "wpi/uv/Timer.h"
@@ -79,10 +75,10 @@
     ClientOptions() : handshakeTimeout{(uv::Timer::Time::max)()} {}
 
     /** Timeout for the handshake request. */
-    uv::Timer::Time handshakeTimeout;
+    uv::Timer::Time handshakeTimeout;  // NOLINT
 
     /** Additional headers to include in handshake. */
-    ArrayRef<std::pair<StringRef, StringRef>> extraHeaders;
+    span<const std::pair<std::string_view, std::string_view>> extraHeaders;
   };
 
   /**
@@ -96,9 +92,9 @@
    * @param options Handshake options
    */
   static std::shared_ptr<WebSocket> CreateClient(
-      uv::Stream& stream, const Twine& uri, const Twine& host,
-      ArrayRef<StringRef> protocols = ArrayRef<StringRef>{},
-      const ClientOptions& options = ClientOptions{});
+      uv::Stream& stream, std::string_view uri, std::string_view host,
+      span<const std::string_view> protocols = {},
+      const ClientOptions& options = {});
 
   /**
    * Starts a client connection by performing the initial client handshake.
@@ -111,11 +107,10 @@
    * @param options Handshake options
    */
   static std::shared_ptr<WebSocket> CreateClient(
-      uv::Stream& stream, const Twine& uri, const Twine& host,
-      std::initializer_list<StringRef> protocols,
-      const ClientOptions& options = ClientOptions{}) {
-    return CreateClient(stream, uri, host,
-                        makeArrayRef(protocols.begin(), protocols.end()),
+      uv::Stream& stream, std::string_view uri, std::string_view host,
+      std::initializer_list<std::string_view> protocols,
+      const ClientOptions& options = {}) {
+    return CreateClient(stream, uri, host, {protocols.begin(), protocols.end()},
                         options);
   }
 
@@ -133,8 +128,8 @@
    *                 Sec-WebSocket-Protocol header field).
    */
   static std::shared_ptr<WebSocket> CreateServer(
-      uv::Stream& stream, StringRef key, StringRef version,
-      StringRef protocol = StringRef{});
+      uv::Stream& stream, std::string_view key, std::string_view version,
+      std::string_view protocol = {});
 
   /**
    * Get connection state.
@@ -155,7 +150,7 @@
   /**
    * Get the selected sub-protocol.  Only valid in or after the open() event.
    */
-  StringRef GetProtocol() const { return m_protocol; }
+  std::string_view GetProtocol() const { return m_protocol; }
 
   /**
    * Set the maximum message size.  Default is 128 KB.  If configured to combine
@@ -179,17 +174,26 @@
    * @param reason A human-readable string explaining why the connection is
    *               closing (optional).
    */
-  void Close(uint16_t code = 1005, const Twine& reason = Twine{});
+  void Close(uint16_t code = 1005, std::string_view reason = {});
 
   /**
    * Send a text message.
    * @param data UTF-8 encoded data to send
    * @param callback Callback which is invoked when the write completes.
    */
-  void SendText(
-      ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kFlagFin | kOpText, data, callback);
+  void SendText(span<const uv::Buffer> data,
+                std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kFlagFin | kOpText, data, std::move(callback));
+  }
+
+  /**
+   * Send a text message.
+   * @param data UTF-8 encoded data to send
+   * @param callback Callback which is invoked when the write completes.
+   */
+  void SendText(std::initializer_list<uv::Buffer> data,
+                std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendText({data.begin(), data.end()}, std::move(callback));
   }
 
   /**
@@ -197,10 +201,19 @@
    * @param data Data to send
    * @param callback Callback which is invoked when the write completes.
    */
-  void SendBinary(
-      ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kFlagFin | kOpBinary, data, callback);
+  void SendBinary(span<const uv::Buffer> data,
+                  std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kFlagFin | kOpBinary, data, std::move(callback));
+  }
+
+  /**
+   * Send a binary message.
+   * @param data Data to send
+   * @param callback Callback which is invoked when the write completes.
+   */
+  void SendBinary(std::initializer_list<uv::Buffer> data,
+                  std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendBinary({data.begin(), data.end()}, std::move(callback));
   }
 
   /**
@@ -211,9 +224,22 @@
    * @param callback Callback which is invoked when the write completes.
    */
   void SendTextFragment(
-      ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kOpText, data, callback);
+      span<const uv::Buffer> data,
+      std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kOpText, data, std::move(callback));
+  }
+
+  /**
+   * Send a text message fragment.  This must be followed by one or more
+   * SendFragment() calls, where the last one has fin=True, to complete the
+   * message.
+   * @param data UTF-8 encoded data to send
+   * @param callback Callback which is invoked when the write completes.
+   */
+  void SendTextFragment(
+      std::initializer_list<uv::Buffer> data,
+      std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendTextFragment({data.begin(), data.end()}, std::move(callback));
   }
 
   /**
@@ -224,9 +250,22 @@
    * @param callback Callback which is invoked when the write completes.
    */
   void SendBinaryFragment(
-      ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kOpBinary, data, callback);
+      span<const uv::Buffer> data,
+      std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kOpBinary, data, std::move(callback));
+  }
+
+  /**
+   * Send a text message fragment.  This must be followed by one or more
+   * SendFragment() calls, where the last one has fin=True, to complete the
+   * message.
+   * @param data Data to send
+   * @param callback Callback which is invoked when the write completes.
+   */
+  void SendBinaryFragment(
+      std::initializer_list<uv::Buffer> data,
+      std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendBinaryFragment({data.begin(), data.end()}, std::move(callback));
   }
 
   /**
@@ -236,10 +275,21 @@
    * @param fin Set to true if this is the final fragment of the message
    * @param callback Callback which is invoked when the write completes.
    */
-  void SendFragment(
-      ArrayRef<uv::Buffer> data, bool fin,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kOpCont | (fin ? kFlagFin : 0), data, callback);
+  void SendFragment(span<const uv::Buffer> data, bool fin,
+                    std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kOpCont | (fin ? kFlagFin : 0), data, std::move(callback));
+  }
+
+  /**
+   * Send a continuation frame.  This is used to send additional parts of a
+   * message started with SendTextFragment() or SendBinaryFragment().
+   * @param data Data to send
+   * @param fin Set to true if this is the final fragment of the message
+   * @param callback Callback which is invoked when the write completes.
+   */
+  void SendFragment(std::initializer_list<uv::Buffer> data, bool fin,
+                    std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendFragment({data.begin(), data.end()}, fin, std::move(callback));
   }
 
   /**
@@ -248,8 +298,10 @@
    *                 write completes.
    */
   void SendPing(std::function<void(uv::Error)> callback = nullptr) {
-    SendPing(ArrayRef<uv::Buffer>{}, [callback](auto bufs, uv::Error err) {
-      if (callback) callback(err);
+    SendPing({}, [f = std::move(callback)](auto bufs, uv::Error err) {
+      if (f) {
+        f(err);
+      }
     });
   }
 
@@ -259,10 +311,20 @@
    * @param callback Callback which is invoked when the ping frame
    *                 write completes.
    */
-  void SendPing(
-      ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kFlagFin | kOpPing, data, callback);
+  void SendPing(span<const uv::Buffer> data,
+                std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kFlagFin | kOpPing, data, std::move(callback));
+  }
+
+  /**
+   * Send a ping frame.
+   * @param data Data to send in the ping frame
+   * @param callback Callback which is invoked when the ping frame
+   *                 write completes.
+   */
+  void SendPing(std::initializer_list<uv::Buffer> data,
+                std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendPing({data.begin(), data.end()}, std::move(callback));
   }
 
   /**
@@ -271,8 +333,10 @@
    *                 write completes.
    */
   void SendPong(std::function<void(uv::Error)> callback = nullptr) {
-    SendPong(ArrayRef<uv::Buffer>{}, [callback](auto bufs, uv::Error err) {
-      if (callback) callback(err);
+    SendPong({}, [f = std::move(callback)](auto bufs, uv::Error err) {
+      if (f) {
+        f(err);
+      }
     });
   }
 
@@ -282,21 +346,31 @@
    * @param callback Callback which is invoked when the pong frame
    *                 write completes.
    */
-  void SendPong(
-      ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback) {
-    Send(kFlagFin | kOpPong, data, callback);
+  void SendPong(span<const uv::Buffer> data,
+                std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    Send(kFlagFin | kOpPong, data, std::move(callback));
+  }
+
+  /**
+   * Send a pong frame.
+   * @param data Data to send in the pong frame
+   * @param callback Callback which is invoked when the pong frame
+   *                 write completes.
+   */
+  void SendPong(std::initializer_list<uv::Buffer> data,
+                std::function<void(span<uv::Buffer>, uv::Error)> callback) {
+    SendPong({data.begin(), data.end()}, std::move(callback));
   }
 
   /**
    * Fail the connection.
    */
-  void Fail(uint16_t code = 1002, const Twine& reason = "protocol error");
+  void Fail(uint16_t code = 1002, std::string_view reason = "protocol error");
 
   /**
    * Forcibly close the connection.
    */
-  void Terminate(uint16_t code = 1006, const Twine& reason = "terminated");
+  void Terminate(uint16_t code = 1006, std::string_view reason = "terminated");
 
   /**
    * Gets user-defined data.
@@ -314,10 +388,15 @@
   void SetData(std::shared_ptr<void> data) { m_data = std::move(data); }
 
   /**
+   * Shuts down and closes the underlying stream.
+   */
+  void Shutdown();
+
+  /**
    * Open event.  Emitted when the connection is open and ready to communicate.
    * The parameter is the selected subprotocol.
    */
-  sig::Signal<StringRef> open;
+  sig::Signal<std::string_view> open;
 
   /**
    * Close event.  Emitted when the connection is closed.  The first parameter
@@ -325,31 +404,31 @@
    * has been closed.  The second parameter is a human-readable string
    * explaining the reason why the connection has been closed.
    */
-  sig::Signal<uint16_t, StringRef> closed;
+  sig::Signal<uint16_t, std::string_view> closed;
 
   /**
    * Text message event.  Emitted when a text message is received.
    * The first parameter is the data, the second parameter is true if the
    * data is the last fragment of the message.
    */
-  sig::Signal<StringRef, bool> text;
+  sig::Signal<std::string_view, bool> text;
 
   /**
    * Binary message event.  Emitted when a binary message is received.
    * The first parameter is the data, the second parameter is true if the
    * data is the last fragment of the message.
    */
-  sig::Signal<ArrayRef<uint8_t>, bool> binary;
+  sig::Signal<span<const uint8_t>, bool> binary;
 
   /**
    * Ping event.  Emitted when a ping message is received.
    */
-  sig::Signal<ArrayRef<uint8_t>> ping;
+  sig::Signal<span<const uint8_t>> ping;
 
   /**
    * Pong event.  Emitted when a pong message is received.
    */
-  sig::Signal<ArrayRef<uint8_t>> pong;
+  sig::Signal<span<const uint8_t>> pong;
 
  private:
   // user data
@@ -381,16 +460,16 @@
   class ClientHandshakeData;
   std::unique_ptr<ClientHandshakeData> m_clientHandshake;
 
-  void StartClient(const Twine& uri, const Twine& host,
-                   ArrayRef<StringRef> protocols, const ClientOptions& options);
-  void StartServer(StringRef key, StringRef version, StringRef protocol);
-  void SendClose(uint16_t code, const Twine& reason);
-  void SetClosed(uint16_t code, const Twine& reason, bool failed = false);
-  void Shutdown();
+  void StartClient(std::string_view uri, std::string_view host,
+                   span<const std::string_view> protocols,
+                   const ClientOptions& options);
+  void StartServer(std::string_view key, std::string_view version,
+                   std::string_view protocol);
+  void SendClose(uint16_t code, std::string_view reason);
+  void SetClosed(uint16_t code, std::string_view reason, bool failed = false);
   void HandleIncoming(uv::Buffer& buf, size_t size);
-  void Send(
-      uint8_t opcode, ArrayRef<uv::Buffer> data,
-      std::function<void(MutableArrayRef<uv::Buffer>, uv::Error)> callback);
+  void Send(uint8_t opcode, span<const uv::Buffer> data,
+            std::function<void(span<uv::Buffer>, uv::Error)> callback);
 };
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocketServer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocketServer.h
index a58a2c9..1f76d16 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocketServer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WebSocketServer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_WEBSOCKETSERVER_H_
 #define WPIUTIL_WPI_WEBSOCKETSERVER_H_
@@ -12,15 +9,15 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 #include <utility>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/HttpParser.h"
 #include "wpi/Signal.h"
 #include "wpi/SmallString.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
 #include "wpi/WebSocket.h"
+#include "wpi/span.h"
 
 namespace wpi {
 
@@ -55,7 +52,8 @@
    *         Second item is the matched protocol if a match was made, otherwise
    *         is empty.
    */
-  std::pair<bool, StringRef> MatchProtocol(ArrayRef<StringRef> protocols);
+  std::pair<bool, std::string_view> MatchProtocol(
+      span<const std::string_view> protocols);
 
   /**
    * Try to find a match to the list of sub-protocols provided by the client.
@@ -66,9 +64,9 @@
    *         Second item is the matched protocol if a match was made, otherwise
    *         is empty.
    */
-  std::pair<bool, StringRef> MatchProtocol(
-      std::initializer_list<StringRef> protocols) {
-    return MatchProtocol(makeArrayRef(protocols.begin(), protocols.end()));
+  std::pair<bool, std::string_view> MatchProtocol(
+      std::initializer_list<std::string_view> protocols) {
+    return MatchProtocol({protocols.begin(), protocols.end()});
   }
 
   /**
@@ -78,7 +76,7 @@
    * @param protocol The subprotocol to send to the client
    */
   std::shared_ptr<WebSocket> Accept(uv::Stream& stream,
-                                    StringRef protocol = StringRef{}) {
+                                    std::string_view protocol = {}) {
     return WebSocket::CreateServer(stream, m_key, m_version, protocol);
   }
 
@@ -112,20 +110,20 @@
      * Checker for URL.  Return true if URL should be accepted.  By default all
      * URLs are accepted.
      */
-    std::function<bool(StringRef)> checkUrl;
+    std::function<bool(std::string_view)> checkUrl;
 
     /**
      * Checker for Host header.  Return true if Host should be accepted.  By
      * default all hosts are accepted.
      */
-    std::function<bool(StringRef)> checkHost;
+    std::function<bool(std::string_view)> checkHost;
   };
 
   /**
    * Private constructor.
    */
-  WebSocketServer(uv::Stream& stream, ArrayRef<StringRef> protocols,
-                  const ServerOptions& options, const private_init&);
+  WebSocketServer(uv::Stream& stream, span<const std::string_view> protocols,
+                  ServerOptions options, const private_init&);
 
   /**
    * Starts a dedicated WebSocket server on the provided connection.  The
@@ -137,8 +135,8 @@
    * @param options Handshake options
    */
   static std::shared_ptr<WebSocketServer> Create(
-      uv::Stream& stream, ArrayRef<StringRef> protocols = ArrayRef<StringRef>{},
-      const ServerOptions& options = ServerOptions{});
+      uv::Stream& stream, span<const std::string_view> protocols = {},
+      const ServerOptions& options = {});
 
   /**
    * Starts a dedicated WebSocket server on the provided connection.  The
@@ -150,16 +148,15 @@
    * @param options Handshake options
    */
   static std::shared_ptr<WebSocketServer> Create(
-      uv::Stream& stream, std::initializer_list<StringRef> protocols,
-      const ServerOptions& options = ServerOptions{}) {
-    return Create(stream, makeArrayRef(protocols.begin(), protocols.end()),
-                  options);
+      uv::Stream& stream, std::initializer_list<std::string_view> protocols,
+      const ServerOptions& options = {}) {
+    return Create(stream, {protocols.begin(), protocols.end()}, options);
   }
 
   /**
    * Connected event.  First parameter is the URL, second is the websocket.
    */
-  sig::Signal<StringRef, WebSocket&> connected;
+  sig::Signal<std::string_view, WebSocket&> connected;
 
  private:
   uv::Stream& m_stream;
@@ -172,7 +169,7 @@
   sig::ScopedConnection m_errorConn;
   sig::ScopedConnection m_endConn;
 
-  void Abort(uint16_t code, StringRef reason);
+  void Abort(uint16_t code, std::string_view reason);
 };
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WorkerThread.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WorkerThread.h
index 4db94c4..6c8852e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WorkerThread.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/WorkerThread.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_WORKERTHREAD_H_
 #define WPIUTIL_WPI_WORKERTHREAD_H_
@@ -108,8 +105,9 @@
                             WorkerThreadRequest<R, T...>& req) {
   R result = std::apply(req.work, std::move(req.params));
   if (req.afterWork) {
-    if (auto async = thr.m_async.m_async.lock())
+    if (auto async = thr.m_async.m_async.lock()) {
       async->Send(std::move(req.afterWork), std::move(result));
+    }
   } else {
     thr.m_promises.SetValue(req.promiseId, std::move(result));
   }
@@ -120,8 +118,9 @@
                             WorkerThreadRequest<void, T...>& req) {
   std::apply(req.work, req.params);
   if (req.afterWork) {
-    if (auto async = thr.m_async.m_async.lock())
+    if (auto async = thr.m_async.m_async.lock()) {
       async->Send(std::move(req.afterWork));
+    }
   } else {
     thr.m_promises.SetValue(req.promiseId);
   }
@@ -133,14 +132,18 @@
   while (m_active) {
     std::unique_lock lock(m_mutex);
     m_cond.wait(lock, [&] { return !m_active || !m_requests.empty(); });
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
 
     // don't want to hold the lock while executing the callbacks
     requests.swap(m_requests);
     lock.unlock();
 
     for (auto&& req : requests) {
-      if (!m_active) break;  // requests may be long-running
+      if (!m_active) {
+        break;  // requests may be long-running
+      }
       RunWorkerThreadRequest(*this, req);
     }
     requests.clear();
@@ -172,7 +175,9 @@
    * @param loop the loop to use for running afterWork routines
    */
   void SetLoop(uv::Loop& loop) {
-    if (auto thr = m_owner.GetThread()) thr->m_async.SetLoop(loop);
+    if (auto thr = m_owner.GetThread()) {
+      thr->m_async.SetLoop(loop);
+    }
   }
 
   /**
@@ -189,7 +194,9 @@
    * Subsequent calls to QueueWorkThen will no longer run afterWork.
    */
   void UnsetLoop() {
-    if (auto thr = m_owner.GetThread()) thr->m_async.UnsetLoop();
+    if (auto thr = m_owner.GetThread()) {
+      thr->m_async.UnsetLoop();
+    }
   }
 
   /**
@@ -200,10 +207,11 @@
    * @return The handle (if nullptr, no handle is set)
    */
   std::shared_ptr<uv::Handle> GetHandle() const {
-    if (auto thr = m_owner.GetThread())
+    if (auto thr = m_owner.GetThread()) {
       return thr->m_async.m_async.lock();
-    else
+    } else {
       return nullptr;
+    }
   }
 
   /**
@@ -217,6 +225,7 @@
    * destroyed while waiting for a result.
    *
    * @param work Work function (called on worker thread)
+   * @param u Arguments to work function
    */
   template <typename... U>
   future<R> QueueWork(WorkFunction work, U&&... u) {
@@ -252,6 +261,7 @@
    *
    * @param work Work function (called on worker thread)
    * @param afterWork After work function (called on loop thread)
+   * @param u Arguments to work function
    */
   template <typename... U>
   void QueueWorkThen(WorkFunction work, AfterWorkFunction afterWork, U&&... u) {
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/array.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/array.h
new file mode 100644
index 0000000..5a03371
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/array.h
@@ -0,0 +1,101 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <array>
+#include <cstddef>
+#include <tuple>
+#include <utility>
+
+namespace wpi {
+
+struct empty_array_t {};
+constexpr empty_array_t empty_array;
+
+/**
+ * This class is a wrapper around std::array that does compile time size
+ * checking.
+ *
+ * std::array's implicit constructor can lead result in uninitialized elements
+ * if the number of arguments doesn't match the std::array size.
+ */
+template <typename T, size_t N>
+class array : public std::array<T, N> {
+ public:
+  explicit array(empty_array_t) {}
+
+  template <typename... Ts>
+  array(T arg, Ts&&... args)  // NOLINT
+      : std::array<T, N>{arg, std::forward<Ts>(args)...} {
+    static_assert(1 + sizeof...(args) == N, "Dimension mismatch");
+  }
+
+  array(const array<T, N>&) = default;
+  array& operator=(const array<T, N>&) = default;
+  array(array<T, N>&&) = default;
+  array& operator=(array<T, N>&&) = default;
+
+  array(const std::array<T, N>& rhs) {  // NOLINT
+    *static_cast<std::array<T, N>*>(this) = rhs;
+  }
+
+  array& operator=(const std::array<T, N>& rhs) {
+    *static_cast<std::array<T, N>*>(this) = rhs;
+    return *this;
+  }
+
+  array(std::array<T, N>&& rhs) {  // NOLINT
+    *static_cast<std::array<T, N>*>(this) = rhs;
+  }
+
+  array& operator=(std::array<T, N>&& rhs) {
+    *static_cast<std::array<T, N>*>(this) = rhs;
+    return *this;
+  }
+};
+
+template <typename T, typename... Ts>
+array(T, Ts...) -> array<std::enable_if_t<(std::is_same_v<T, Ts> && ...), T>,
+                         1 + sizeof...(Ts)>;
+
+}  // namespace wpi
+
+template <size_t I, typename T, size_t N>
+constexpr T& get(wpi::array<T, N>& arr) noexcept {
+  static_assert(I < N, "array index is within bounds");
+  return std::get<I>(static_cast<std::array<T, N>>(arr));
+}
+
+template <size_t I, typename T, size_t N>
+constexpr T&& get(wpi::array<T, N>&& arr) noexcept {
+  static_assert(I < N, "array index is within bounds");
+  return std::move(std::get<I>(arr));
+}
+
+template <size_t I, typename T, size_t N>
+constexpr const T& get(const wpi::array<T, N>& arr) noexcept {
+  static_assert(I < N, "array index is within bounds");
+  return std::get<I>(static_cast<std::array<T, N>>(arr));
+}
+
+template <size_t I, typename T, size_t N>
+constexpr const T&& get(const wpi::array<T, N>&& arr) noexcept {
+  static_assert(I < N, "array index is within bounds");
+  return std::move(std::get<I>(arr));
+}
+
+// Enables structured bindings
+namespace std {  // NOLINT
+// Partial specialization for wpi::array
+template <typename T, size_t N>
+struct tuple_size<wpi::array<T, N>> : public integral_constant<size_t, N> {};
+
+// Partial specialization for wpi::array
+template <size_t I, typename T, size_t N>
+struct tuple_element<I, wpi::array<T, N>> {
+  static_assert(I < N, "index is out of bounds");
+  using type = T;
+};
+}  // namespace std
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h
index 5dc12f8..40913f9 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -19,44 +16,284 @@
 template <class T>
 class circular_buffer {
  public:
-  explicit circular_buffer(size_t size);
+  explicit circular_buffer(size_t size) : m_data(size, T{}) {}
 
-  using value_type = T;
-  using reference = value_type&;
-  using const_reference = const value_type&;
-  using pointer = value_type*;
-  using size_type = size_t;
-  using iterator_category = std::forward_iterator_tag;
-  using difference_type = std::ptrdiff_t;
+  circular_buffer(const circular_buffer&) = default;
+  circular_buffer& operator=(const circular_buffer&) = default;
+  circular_buffer(circular_buffer&&) = default;
+  circular_buffer& operator=(circular_buffer&&) = default;
 
-  size_type size() const;
-  T& front();
-  const T& front() const;
-  T& back();
-  const T& back() const;
-  void push_front(T value);
-  void push_back(T value);
-  T pop_front();
-  T pop_back();
+  class iterator {
+   public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = T;
+    using difference_type = std::ptrdiff_t;
+    using pointer = T*;
+    using reference = T&;
+
+    iterator(circular_buffer* buffer, size_t index)
+        : m_buffer(buffer), m_index(index) {}
+
+    iterator& operator++() {
+      ++m_index;
+      return *this;
+    }
+    iterator operator++(int) {
+      iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(const iterator& other) const {
+      return m_buffer == other.m_buffer && m_index == other.m_index;
+    }
+    bool operator!=(const iterator& other) const { return !(*this == other); }
+    reference operator*() { return (*m_buffer)[m_index]; }
+
+   private:
+    circular_buffer* m_buffer;
+    size_t m_index;
+  };
+
+  class const_iterator {
+   public:
+    using iterator_category = std::forward_iterator_tag;
+    using value_type = T;
+    using difference_type = std::ptrdiff_t;
+    using pointer = T*;
+    using const_reference = const T&;
+
+    const_iterator(const circular_buffer* buffer, size_t index)
+        : m_buffer(buffer), m_index(index) {}
+
+    const_iterator& operator++() {
+      ++m_index;
+      return *this;
+    }
+    const_iterator operator++(int) {
+      const_iterator retval = *this;
+      ++(*this);
+      return retval;
+    }
+    bool operator==(const const_iterator& other) const {
+      return m_buffer == other.m_buffer && m_index == other.m_index;
+    }
+    bool operator!=(const const_iterator& other) const {
+      return !(*this == other);
+    }
+    const_reference operator*() const { return (*m_buffer)[m_index]; }
+
+   private:
+    const circular_buffer* m_buffer;
+    size_t m_index;
+  };
+
+  iterator begin() { return iterator(this, 0); }
+  iterator end() { return iterator(this, ::wpi::circular_buffer<T>::size()); }
+
+  const_iterator begin() const { return const_iterator(this, 0); }
+  const_iterator end() const {
+    return const_iterator(this, ::wpi::circular_buffer<T>::size());
+  }
+
+  const_iterator cbegin() const { return const_iterator(this, 0); }
+  const_iterator cend() const {
+    return const_iterator(this, ::wpi::circular_buffer<T>::size());
+  }
+
+  /**
+   * Returns number of elements in buffer
+   */
+  size_t size() const { return m_length; }
+
+  /**
+   * Returns value at front of buffer
+   */
+  T& front() { return (*this)[0]; }
+
+  /**
+   * Returns value at front of buffer
+   */
+  const T& front() const { return (*this)[0]; }
+
+  /**
+   * Returns value at back of buffer
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  T& back() { return m_data[(m_front + m_length - 1) % m_data.size()]; }
+
+  /**
+   * Returns value at back of buffer
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  const T& back() const {
+    return m_data[(m_front + m_length - 1) % m_data.size()];
+  }
+
+  /**
+   * Push a new value onto the front of the buffer.
+   *
+   * The value at the back is overwritten if the buffer is full.
+   */
+  void push_front(T value) {
+    if (m_data.size() == 0) {
+      return;
+    }
+
+    m_front = ModuloDec(m_front);
+
+    m_data[m_front] = value;
+
+    if (m_length < m_data.size()) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push a new value onto the back of the buffer.
+   *
+   * The value at the front is overwritten if the buffer is full.
+   */
+  void push_back(T value) {
+    if (m_data.size() == 0) {
+      return;
+    }
+
+    m_data[(m_front + m_length) % m_data.size()] = value;
+
+    if (m_length < m_data.size()) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = ModuloInc(m_front);
+    }
+  }
+
+  /**
+   * Push a new value onto the front of the buffer that is constructed with the
+   * provided constructor arguments.
+   *
+   * The value at the back is overwritten if the buffer is full.
+   */
+  template <class... Args>
+  void emplace_front(Args&&... args) {
+    if (m_data.size() == 0) {
+      return;
+    }
+
+    m_front = ModuloDec(m_front);
+
+    m_data[m_front] = T{args...};
+
+    if (m_length < m_data.size()) {
+      m_length++;
+    }
+  }
+
+  /**
+   * Push a new value onto the back of the buffer that is constructed with the
+   * provided constructor arguments.
+   *
+   * The value at the front is overwritten if the buffer is full.
+   */
+  template <class... Args>
+  void emplace_back(Args&&... args) {
+    if (m_data.size() == 0) {
+      return;
+    }
+
+    m_data[(m_front + m_length) % m_data.size()] = T{args...};
+
+    if (m_length < m_data.size()) {
+      m_length++;
+    } else {
+      // Increment front if buffer is full to maintain size
+      m_front = ModuloInc(m_front);
+    }
+  }
+
+  /**
+   * Pop value at front of buffer.
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  T pop_front() {
+    T& temp = m_data[m_front];
+    m_front = ModuloInc(m_front);
+    m_length--;
+    return temp;
+  }
+
+  /**
+   * Pop value at back of buffer.
+   *
+   * If there are no elements in the buffer, calling this function results in
+   * undefined behavior.
+   */
+  T pop_back() {
+    m_length--;
+    return m_data[(m_front + m_length) % m_data.size()];
+  }
+
+  /**
+   * Resizes internal buffer to given size.
+   */
   void resize(size_t size);
-  void reset();
 
-  T& operator[](size_t index);
-  const T& operator[](size_t index) const;
+  /**
+   * Empties internal buffer.
+   */
+  void reset() {
+    m_front = 0;
+    m_length = 0;
+  }
+
+  /**
+   * @return Element at index starting from front of buffer.
+   */
+  T& operator[](size_t index) {
+    return m_data[(m_front + index) % m_data.size()];
+  }
+
+  /**
+   * @return Element at index starting from front of buffer.
+   */
+  const T& operator[](size_t index) const {
+    return m_data[(m_front + index) % m_data.size()];
+  }
 
  private:
   std::vector<T> m_data;
 
-  T zero_val{0};
-
   // Index of element at front of buffer
   size_t m_front = 0;
 
   // Number of elements used in buffer
   size_t m_length = 0;
 
-  size_t ModuloInc(size_t index);
-  size_t ModuloDec(size_t index);
+  /**
+   * Increment an index modulo the length of the buffer.
+   *
+   * @return The result of the modulo operation.
+   */
+  size_t ModuloInc(size_t index) { return (index + 1) % m_data.size(); }
+
+  /**
+   * Decrement an index modulo the length of the buffer.
+   *
+   * @return The result of the modulo operation.
+   */
+  size_t ModuloDec(size_t index) {
+    if (index == 0) {
+      return m_data.size() - 1;
+    } else {
+      return index - 1;
+    }
+  }
 };
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.inc b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.inc
index 20690b7..038af50 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.inc
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/circular_buffer.inc
@@ -1,138 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <algorithm>
+#include "wpi/circular_buffer.h"
 
 namespace wpi {
 
-template <class T>
-circular_buffer<T>::circular_buffer(size_t size) : m_data(size, T{0}) {}
-
-/**
- * Returns number of elements in buffer
- */
-template <class T>
-typename circular_buffer<T>::size_type circular_buffer<T>::size() const {
-  return m_length;
-}
-
-/**
- * Returns value at front of buffer
- */
-template <class T>
-T& circular_buffer<T>::front() {
-  return (*this)[0];
-}
-
-/**
- * Returns value at front of buffer
- */
-template <class T>
-const T& circular_buffer<T>::front() const {
-  return (*this)[0];
-}
-
-/**
- * Returns value at back of buffer
- */
-template <class T>
-T& circular_buffer<T>::back() {
-  // If there are no elements in the buffer, do nothing
-  if (m_length == 0) {
-    return zero_val;
-  }
-
-  return m_data[(m_front + m_length - 1) % m_data.size()];
-}
-
-/**
- * Returns value at back of buffer
- */
-template <class T>
-const T& circular_buffer<T>::back() const {
-  // If there are no elements in the buffer, do nothing
-  if (m_length == 0) {
-    return zero_val;
-  }
-
-  return m_data[(m_front + m_length - 1) % m_data.size()];
-}
-
-/**
- * Push new value onto front of the buffer. The value at the back is overwritten
- * if the buffer is full.
- */
-template <class T>
-void circular_buffer<T>::push_front(T value) {
-  if (m_data.size() == 0) {
-    return;
-  }
-
-  m_front = ModuloDec(m_front);
-
-  m_data[m_front] = value;
-
-  if (m_length < m_data.size()) {
-    m_length++;
-  }
-}
-
-/**
- * Push new value onto back of the buffer. The value at the front is overwritten
- * if the buffer is full.
- */
-template <class T>
-void circular_buffer<T>::push_back(T value) {
-  if (m_data.size() == 0) {
-    return;
-  }
-
-  m_data[(m_front + m_length) % m_data.size()] = value;
-
-  if (m_length < m_data.size()) {
-    m_length++;
-  } else {
-    // Increment front if buffer is full to maintain size
-    m_front = ModuloInc(m_front);
-  }
-}
-
-/**
- * Pop value at front of buffer.
- */
-template <class T>
-T circular_buffer<T>::pop_front() {
-  // If there are no elements in the buffer, do nothing
-  if (m_length == 0) {
-    return T{0};
-  }
-
-  T& temp = m_data[m_front];
-  m_front = ModuloInc(m_front);
-  m_length--;
-  return temp;
-}
-
-/**
- * Pop value at back of buffer.
- */
-template <class T>
-T circular_buffer<T>::pop_back() {
-  // If there are no elements in the buffer, do nothing
-  if (m_length == 0) {
-    return T{0};
-  }
-
-  m_length--;
-  return m_data[(m_front + m_length) % m_data.size()];
-}
-
 /**
  * Resizes internal buffer to given size.
  */
@@ -186,54 +61,4 @@
   }
 }
 
-/**
- * Sets internal buffer contents to zero.
- */
-template <class T>
-void circular_buffer<T>::reset() {
-  std::fill(m_data.begin(), m_data.end(), T{0});
-  m_front = 0;
-  m_length = 0;
-}
-
-/**
- * @return Element at index starting from front of buffer.
- */
-template <class T>
-T& circular_buffer<T>::operator[](size_t index) {
-  return m_data[(m_front + index) % m_data.size()];
-}
-
-/**
- * @return Element at index starting from front of buffer.
- */
-template <class T>
-const T& circular_buffer<T>::operator[](size_t index) const {
-  return m_data[(m_front + index) % m_data.size()];
-}
-
-/**
- * Increment an index modulo the length of the buffer.
- *
- * @return The result of the modulo operation.
- */
-template <class T>
-size_t circular_buffer<T>::ModuloInc(size_t index) {
-  return (index + 1) % m_data.size();
-}
-
-/**
- * Decrement an index modulo the length of the buffer.
- *
- * @return The result of the modulo operation.
- */
-template <class T>
-size_t circular_buffer<T>::ModuloDec(size_t index) {
-  if (index == 0) {
-    return m_data.size() - 1;
-  } else {
-    return index - 1;
-  }
-}
-
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/condition_variable.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/condition_variable.h
index 1599e19..7c97ec8 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/condition_variable.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/condition_variable.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/deprecated.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/deprecated.h
index 6d77dde..6e6a3bf 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/deprecated.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/deprecated.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_DEPRECATED_H_
 #define WPIUTIL_WPI_DEPRECATED_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/fmt/raw_ostream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/fmt/raw_ostream.h
new file mode 100644
index 0000000..8779bfc
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/fmt/raw_ostream.h
@@ -0,0 +1,30 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef WPIUTIL_WPI_FMT_RAW_OSTREAM_H_
+#define WPIUTIL_WPI_FMT_RAW_OSTREAM_H_
+
+#include "fmt/format.h"
+#include "wpi/raw_ostream.h"
+
+FMT_BEGIN_NAMESPACE
+
+inline void vprint(wpi::raw_ostream& os, string_view format_str,
+                   fmt::format_args args) {
+  memory_buffer buffer;
+  detail::vformat_to(buffer, format_str, args);
+  os.write(buffer.data(), buffer.size());
+}
+
+/**
+ * Prints formatted data to the stream *os*.
+ */
+template <typename S, typename... Args>
+void print(wpi::raw_ostream& os, const S& format_str, Args&&... args) {
+  vprint(os, format_str, fmt::make_args_checked<Args...>(format_str, args...));
+}
+
+FMT_END_NAMESPACE
+
+#endif  // WPIUTIL_WPI_FMT_RAW_OSTREAM_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/fs.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/fs.h
new file mode 100644
index 0000000..fca8069
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/fs.h
@@ -0,0 +1,235 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//===----------------------------------------------------------------------===//
+//
+//                     The LLVM Compiler Infrastructure
+//
+// This file is distributed under the University of Illinois Open Source
+// License. See LICENSE.TXT for details.
+//
+//===----------------------------------------------------------------------===//
+
+#pragma once
+
+#include <string_view>
+#include <system_error>
+
+#if defined(__APPLE__)
+#include <Availability.h>
+#endif
+#if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) \
+     || (defined(__cplusplus) && __cplusplus >= 201703L)) \
+    && defined(__has_include)
+#if __has_include(<filesystem>) \
+    && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) \
+        || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101500) \
+    && (defined(__clang__) || !defined(__GNUC__) || __GNUC__ >= 10 \
+        || (__GNUC__ >= 9 && __GNUC_MINOR__ >= 1))
+#define GHC_USE_STD_FS
+#include <filesystem>
+namespace fs {
+using namespace std::filesystem;
+using ifstream = std::ifstream;
+using ofstream = std::ofstream;
+using fstream = std::fstream;
+}  // namespace fs
+#endif
+#endif
+#ifndef GHC_USE_STD_FS
+// #define GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE
+#define GHC_FILESYSTEM_FWD
+#include "ghc/filesystem.hpp"
+namespace fs {
+using namespace ghc::filesystem;
+using ifstream = ghc::filesystem::ifstream;
+using ofstream = ghc::filesystem::ofstream;
+using fstream = ghc::filesystem::fstream;
+}  // namespace fs
+#endif
+
+namespace fs {
+
+#if defined(_WIN32)
+// A Win32 HANDLE is a typedef of void*
+using file_t = void*;
+#else
+using file_t = int;
+#endif
+
+extern const file_t kInvalidFile;
+
+enum CreationDisposition : unsigned {
+  /// CD_CreateAlways - When opening a file:
+  ///   * If it already exists, truncate it.
+  ///   * If it does not already exist, create a new file.
+  CD_CreateAlways = 0,
+
+  /// CD_CreateNew - When opening a file:
+  ///   * If it already exists, fail.
+  ///   * If it does not already exist, create a new file.
+  CD_CreateNew = 1,
+
+  /// CD_OpenExisting - When opening a file:
+  ///   * If it already exists, open the file with the offset set to 0.
+  ///   * If it does not already exist, fail.
+  CD_OpenExisting = 2,
+
+  /// CD_OpenAlways - When opening a file:
+  ///   * If it already exists, open the file with the offset set to 0.
+  ///   * If it does not already exist, create a new file.
+  CD_OpenAlways = 3,
+};
+
+enum FileAccess : unsigned {
+  FA_Read = 1,
+  FA_Write = 2,
+};
+
+enum OpenFlags : unsigned {
+  OF_None = 0,
+  F_None = 0,  // For compatibility
+
+  /// The file should be opened in text mode on platforms that make this
+  /// distinction.
+  OF_Text = 1,
+  F_Text = 1,  // For compatibility
+
+  /// The file should be opened in append mode.
+  OF_Append = 2,
+  F_Append = 2,  // For compatibility
+
+  /// Delete the file on close. Only makes a difference on windows.
+  OF_Delete = 4,
+
+  /// When a child process is launched, this file should remain open in the
+  /// child process.
+  OF_ChildInherit = 8,
+
+  /// Force files Atime to be updated on access. Only makes a difference on
+  /// windows.
+  OF_UpdateAtime = 16,
+};
+
+inline OpenFlags operator|(OpenFlags A, OpenFlags B) {
+  return OpenFlags(unsigned(A) | unsigned(B));
+}
+
+inline OpenFlags& operator|=(OpenFlags& A, OpenFlags B) {
+  A = A | B;
+  return A;
+}
+
+inline FileAccess operator|(FileAccess A, FileAccess B) {
+  return FileAccess(unsigned(A) | unsigned(B));
+}
+
+inline FileAccess& operator|=(FileAccess& A, FileAccess B) {
+  A = A | B;
+  return A;
+}
+
+/**
+ * Opens a file with the specified creation disposition, access mode,
+ * and flags and returns a platform-specific file object.
+ *
+ * The caller is responsible for closing the file object once they are
+ * finished with it.
+ *
+ * @param Path The path of the file to open, relative or absolute.
+ * @param EC Error code output, set to non-zero on error
+ * @param Disp Value specifying the existing-file behavior
+ * @param Access Value specifying whether to open the file in read, write, or
+ *               read-write mode.
+ * @param Flags Additional flags.
+ * @param Mode The access permissions of the file, represented in octal.
+ * @returns errc::success if \a Name has been opened, otherwise a
+ *          platform-specific error_code.
+ */
+file_t OpenFile(const path& Path, std::error_code& EC, CreationDisposition Disp,
+                FileAccess Access, OpenFlags Flags, unsigned Mode = 0666);
+
+/**
+ * @brief Opens the file with the given name in a write-only or read-write
+ * mode, returning its open file descriptor. If the file does not exist, it
+ * is created.
+ *
+ * The caller is responsible for closing the freeing the file once they are
+ * finished with it.
+ *
+ * @param Path The path of the file to open, relative or absolute.
+ * @param EC Error code output, set to non-zero on error
+ * @param Disp Value specifying the existing-file behavior
+ * @param Flags Additional flags used to determine whether the file should be
+ *              opened in, for example, read-write or in write-only mode.
+ * @param Mode The access permissions of the file, represented in octal.
+ * @returns a platform-specific file descriptor if \a Name has been opened,
+ *          otherwise kInvalidFile.
+ */
+inline file_t OpenFileForWrite(const path& Path, std::error_code& EC,
+                               CreationDisposition Disp, OpenFlags Flags,
+                               unsigned Mode = 0666) {
+  return OpenFile(Path, EC, Disp, FA_Write, Flags, Mode);
+}
+
+/**
+ * @brief Opens the file with the given name in a write-only or read-write
+ * mode, returning its open file descriptor. If the file does not exist, it
+ * is created.
+ *
+ * The caller is responsible for closing the freeing the file once they are
+ * finished with it.
+ *
+ * @param Path The path of the file to open, relative or absolute.
+ * @param EC Error code output, set to non-zero on error
+ * @param Disp Value specifying the existing-file behavior
+ * @param Flags Additional flags used to determine whether the file should be
+ *              opened in, for example, read-write or in write-only mode.
+ * @param Mode The access permissions of the file, represented in octal.
+ * @return a platform-specific file descriptor if \a Name has been opened,
+ *         otherwise kInvalidFile.
+ */
+inline file_t OpenFileForReadWrite(const path& Path, std::error_code& EC,
+                                   CreationDisposition Disp, OpenFlags Flags,
+                                   unsigned Mode = 0666) {
+  return OpenFile(Path, EC, Disp, FA_Write | FA_Read, Flags, Mode);
+}
+
+/**
+ * Opens the file with the given name in a read-only mode, returning
+ * its open file descriptor.
+ *
+ * The caller is responsible for closing the freeing the file once they are
+ * finished with it.
+ *
+ * @param Path The path of the file to open, relative or absolute.
+ * @param EC Error code output, set to non-zero on error
+ * @param Flags Additional flags
+ * @return a platform-specific file descriptor if \a Name has been opened,
+ *         otherwise kInvalidFile.
+ */
+file_t OpenFileForRead(const path& Path, std::error_code& EC,
+                       OpenFlags Flags = OF_None);
+
+/**
+ * Converts a file object to a file descriptor.  The returned file descriptor
+ * must be closed with ::close() instead of CloseFile().
+ *
+ * @param F On input, this is the file to convert to a file descriptor.
+ *          On output, the file is set to kInvalidFile.
+ * @param EC Error code output, set to non-zero on error
+ * @param Flags Flags passed to the OpenFile function that created file_t
+ * @return file descriptor, or -1 on error
+ */
+int FileToFd(file_t& F, std::error_code& EC, OpenFlags Flags);
+
+/**
+ * Closes the file object.
+ *
+ * @param F On input, this is the file to close.  On output, the file is
+ *          set to kInvalidFile.
+ */
+void CloseFile(file_t& F);
+
+}  // namespace fs
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/function_ref.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/function_ref.h
new file mode 100644
index 0000000..868c164
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/function_ref.h
@@ -0,0 +1,66 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+//===- llvm/ADT/STLExtras.h - Useful STL related functions ------*- C++ -*-===//
+//
+//                     The LLVM Compiler Infrastructure
+//
+// This file is distributed under the University of Illinois Open Source
+// License. See LICENSE.TXT for details.
+//
+//===----------------------------------------------------------------------===//
+
+#ifndef WPIUTIL_WPI_FUNCTION_REF_H_
+#define WPIUTIL_WPI_FUNCTION_REF_H_
+
+#include <stdint.h>
+
+#include <type_traits>
+#include <utility>
+
+namespace wpi {
+
+/// An efficient, type-erasing, non-owning reference to a callable. This is
+/// intended for use as the type of a function parameter that is not used
+/// after the function in question returns.
+///
+/// This class does not own the callable, so it is not in general safe to store
+/// a function_ref.
+template <typename Fn>
+class function_ref;
+
+template <typename Ret, typename... Params>
+class function_ref<Ret(Params...)> {
+  Ret (*callback)(intptr_t callable, Params... params) = nullptr;
+  intptr_t callable;
+
+  template <typename Callable>
+  static Ret callback_fn(intptr_t callable, Params... params) {
+    return (*reinterpret_cast<Callable*>(callable))(std::forward<Params>(
+        params)...);
+  }
+
+ public:
+  function_ref() = default;
+  /*implicit*/ function_ref(std::nullptr_t) {}  // NOLINT
+
+  template <typename Callable>
+  /*implicit*/ function_ref(  // NOLINT
+      Callable&& callable,
+      typename std::enable_if<
+          !std::is_same<typename std::remove_reference<Callable>::type,
+                        function_ref>::value>::type* = nullptr)
+      : callback(callback_fn<typename std::remove_reference<Callable>::type>),
+        callable(reinterpret_cast<intptr_t>(&callable)) {}
+
+  Ret operator()(Params... params) const {
+    return callback(callable, std::forward<Params>(params)...);
+  }
+
+  explicit operator bool() const { return callback; }
+};
+
+}  // namespace wpi
+
+#endif  // WPIUTIL_WPI_FUNCTION_REF_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/future.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/future.h
index db46d58..5cb4a8d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/future.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/future.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_FUTURE_H_
 #define WPIUTIL_WPI_FUTURE_H_
@@ -290,14 +287,16 @@
   future(const future&) = delete;
 
   template <typename R>
-  future(future<R>&& oth) noexcept
+  future(future<R>&& oth) noexcept  // NOLINT
       : future(oth.then([](R&& val) -> T { return val; })) {}
 
   /**
    * Ignores the result of the future if it has not been retrieved.
    */
   ~future() {
-    if (m_promises) m_promises->IgnoreResult(m_request);
+    if (m_promises) {
+      m_promises->IgnoreResult(m_request);
+    }
   }
 
   future& operator=(future&& oth) noexcept {
@@ -316,10 +315,11 @@
    * @return The value provided by the corresponding promise.set_value().
    */
   T get() {
-    if (m_promises)
+    if (m_promises) {
       return m_promises->GetResult(m_request);
-    else
+    } else {
       return T();
+    }
   }
 
   template <typename R, typename F>
@@ -358,7 +358,9 @@
    * If the value has already been provided, returns immediately.
    */
   void wait() const {
-    if (m_promises) m_promises->WaitResult(m_request);
+    if (m_promises) {
+      m_promises->WaitResult(m_request);
+    }
   }
 
   /**
@@ -419,7 +421,9 @@
    * Ignores the result of the future if it has not been retrieved.
    */
   ~future() {
-    if (m_promises) m_promises->IgnoreResult(m_request);
+    if (m_promises) {
+      m_promises->IgnoreResult(m_request);
+    }
   }
 
   future& operator=(future&& oth) noexcept {
@@ -436,7 +440,9 @@
    * Can only be called once.  The future will be marked invalid after the call.
    */
   void get() {
-    if (m_promises) m_promises->GetResult(m_request);
+    if (m_promises) {
+      m_promises->GetResult(m_request);
+    }
   }
 
   template <typename R, typename F>
@@ -474,7 +480,9 @@
    * If the value has already been provided, returns immediately.
    */
   void wait() const {
-    if (m_promises) m_promises->WaitResult(m_request);
+    if (m_promises) {
+      m_promises->WaitResult(m_request);
+    }
   }
 
   /**
@@ -540,7 +548,9 @@
    * Sets the promised value to a default-constructed T if not already set.
    */
   ~promise() {
-    if (m_promises) m_promises->SetValue(m_request, T());
+    if (m_promises) {
+      m_promises->SetValue(m_request, T());
+    }
   }
 
   promise& operator=(promise&& oth) noexcept {
@@ -575,7 +585,9 @@
    * @param value The value to provide to the waiting future
    */
   void set_value(const T& value) {
-    if (m_promises) m_promises->SetValue(m_request, value);
+    if (m_promises) {
+      m_promises->SetValue(m_request, value);
+    }
     m_promises = nullptr;
   }
 
@@ -586,7 +598,9 @@
    * @param value The value to provide to the waiting future
    */
   void set_value(T&& value) {
-    if (m_promises) m_promises->SetValue(m_request, std::move(value));
+    if (m_promises) {
+      m_promises->SetValue(m_request, std::move(value));
+    }
     m_promises = nullptr;
   }
 
@@ -625,7 +639,9 @@
    * Sets the promised value if not already set.
    */
   ~promise() {
-    if (m_promises) m_promises->SetValue(m_request);
+    if (m_promises) {
+      m_promises->SetValue(m_request);
+    }
   }
 
   promise& operator=(promise&& oth) noexcept {
@@ -660,7 +676,9 @@
    * Only effective once (subsequent calls will be ignored).
    */
   void set_value() {
-    if (m_promises) m_promises->SetValue(m_request);
+    if (m_promises) {
+      m_promises->SetValue(m_request);
+    }
     m_promises = nullptr;
   }
 
@@ -710,7 +728,9 @@
 template <typename T>
 void PromiseFactory<T>::SetValue(uint64_t request, const T& value) {
   std::unique_lock lock(GetResultMutex());
-  if (!EraseRequest(request)) return;
+  if (!EraseRequest(request)) {
+    return;
+  }
   auto it = std::find_if(m_thens.begin(), m_thens.end(),
                          [=](const auto& x) { return x.request == request; });
   if (it != m_thens.end()) {
@@ -729,7 +749,9 @@
 template <typename T>
 void PromiseFactory<T>::SetValue(uint64_t request, T&& value) {
   std::unique_lock lock(GetResultMutex());
-  if (!EraseRequest(request)) return;
+  if (!EraseRequest(request)) {
+    return;
+  }
   auto it = std::find_if(m_thens.begin(), m_thens.end(),
                          [=](const auto& x) { return x.request == request; });
   if (it != m_thens.end()) {
@@ -796,7 +818,9 @@
     // Did we get a response to *our* request?
     auto it = std::find_if(m_results.begin(), m_results.end(),
                            [=](const auto& r) { return r.first == request; });
-    if (it != m_results.end()) return;
+    if (it != m_results.end()) {
+      return;
+    }
     // No, keep waiting for a response
     Wait(lock);
   }
@@ -813,10 +837,16 @@
     // Did we get a response to *our* request?
     auto it = std::find_if(m_results.begin(), m_results.end(),
                            [=](const auto& r) { return r.first == request; });
-    if (it != m_results.end()) return true;
-    if (timeout) break;
+    if (it != m_results.end()) {
+      return true;
+    }
+    if (timeout) {
+      break;
+    }
     // No, keep waiting for a response
-    if (!WaitUntil(lock, timeout_time)) timeout = true;
+    if (!WaitUntil(lock, timeout_time)) {
+      timeout = true;
+    }
   }
   return false;
 }
@@ -845,10 +875,16 @@
     // Did we get a response to *our* request?
     auto it = std::find_if(m_results.begin(), m_results.end(),
                            [=](const auto& r) { return r == request; });
-    if (it != m_results.end()) return true;
-    if (timeout) break;
+    if (it != m_results.end()) {
+      return true;
+    }
+    if (timeout) {
+      break;
+    }
     // No, keep waiting for a response
-    if (!WaitUntil(lock, timeout_time)) timeout = true;
+    if (!WaitUntil(lock, timeout_time)) {
+      timeout = true;
+    }
   }
   return false;
 }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ghc/filesystem.hpp b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ghc/filesystem.hpp
new file mode 100644
index 0000000..6cbc892
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/ghc/filesystem.hpp
@@ -0,0 +1,5942 @@
+//---------------------------------------------------------------------------------------
+//
+// ghc::filesystem - A C++17-like filesystem implementation for C++11/C++14/C++17/C++20
+//
+//---------------------------------------------------------------------------------------
+//
+// Copyright (c) 2018, Steffen Schümann <s.schuemann@pobox.com>
+//
+// 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.
+//
+//---------------------------------------------------------------------------------------
+//
+// To dynamically select std::filesystem where available on most platforms,
+// you could use:
+//
+// #if ((defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) || (defined(__cplusplus) && __cplusplus >= 201703L)) && defined(__has_include)
+// #if __has_include(<filesystem>) && (!defined(__MAC_OS_X_VERSION_MIN_REQUIRED) || __MAC_OS_X_VERSION_MIN_REQUIRED >= 101500)
+// #define GHC_USE_STD_FS
+// #include <filesystem>
+// namespace fs = std::filesystem;
+// #endif
+// #endif
+// #ifndef GHC_USE_STD_FS
+// #include <ghc/filesystem.hpp>
+// namespace fs = ghc::filesystem;
+// #endif
+//
+//---------------------------------------------------------------------------------------
+#ifndef GHC_FILESYSTEM_H
+#define GHC_FILESYSTEM_H
+
+// #define BSD manifest constant only in
+// sys/param.h
+#ifndef _WIN32
+#include <sys/param.h>
+#endif
+
+#ifndef GHC_OS_DETECTED
+#if defined(__APPLE__) && defined(__MACH__)
+#define GHC_OS_MACOS
+#elif defined(__linux__)
+#define GHC_OS_LINUX
+#if defined(__ANDROID__)
+#define GHC_OS_ANDROID
+#endif
+#elif defined(_WIN64)
+#define GHC_OS_WINDOWS
+#define GHC_OS_WIN64
+#elif defined(_WIN32)
+#define GHC_OS_WINDOWS
+#define GHC_OS_WIN32
+#elif defined(__CYGWIN__)
+#define GHC_OS_CYGWIN
+#elif defined(__svr4__)
+#define GHC_OS_SYS5R4
+#elif defined(BSD)
+#define GHC_OS_BSD
+#elif defined(__EMSCRIPTEN__)
+#define GHC_OS_WEB
+#include <wasi/api.h>
+#elif defined(__QNX__)
+#define GHC_OS_QNX
+#define GHC_NO_DIRENT_D_TYPE
+#else
+#error "Operating system currently not supported!"
+#endif
+#define GHC_OS_DETECTED
+#if (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L)
+#if _MSVC_LANG == 201703L
+#define GHC_FILESYSTEM_RUNNING_CPP17
+#else
+#define GHC_FILESYSTEM_RUNNING_CPP20
+#endif
+#elif (defined(__cplusplus) && __cplusplus >= 201703L)
+#if __cplusplus == 201703L
+#define GHC_FILESYSTEM_RUNNING_CPP17
+#else
+#define GHC_FILESYSTEM_RUNNING_CPP20
+#endif
+#endif
+#endif
+
+#if defined(GHC_FILESYSTEM_IMPLEMENTATION)
+#define GHC_EXPAND_IMPL
+#define GHC_INLINE
+#ifdef GHC_OS_WINDOWS
+#ifndef GHC_FS_API
+#define GHC_FS_API
+#endif
+#ifndef GHC_FS_API_CLASS
+#define GHC_FS_API_CLASS
+#endif
+#else
+#ifndef GHC_FS_API
+#define GHC_FS_API __attribute__((visibility("default")))
+#endif
+#ifndef GHC_FS_API_CLASS
+#define GHC_FS_API_CLASS __attribute__((visibility("default")))
+#endif
+#endif
+#elif defined(GHC_FILESYSTEM_FWD)
+#define GHC_INLINE
+#ifdef GHC_OS_WINDOWS
+#ifndef GHC_FS_API
+#define GHC_FS_API extern
+#endif
+#ifndef GHC_FS_API_CLASS
+#define GHC_FS_API_CLASS
+#endif
+#else
+#ifndef GHC_FS_API
+#define GHC_FS_API extern
+#endif
+#ifndef GHC_FS_API_CLASS
+#define GHC_FS_API_CLASS
+#endif
+#endif
+#else
+#define GHC_EXPAND_IMPL
+#define GHC_INLINE inline
+#ifndef GHC_FS_API
+#define GHC_FS_API
+#endif
+#ifndef GHC_FS_API_CLASS
+#define GHC_FS_API_CLASS
+#endif
+#endif
+
+#ifdef GHC_EXPAND_IMPL
+
+#ifdef GHC_OS_WINDOWS
+#include <windows.h>
+// additional includes
+#include <shellapi.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <wchar.h>
+#include <winioctl.h>
+#else
+#include <dirent.h>
+#include <fcntl.h>
+#include <limits.h>
+#include <sys/param.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#ifdef GHC_OS_ANDROID
+#include <android/api-level.h>
+#if __ANDROID_API__ < 12
+#include <sys/syscall.h>
+#endif
+#include <sys/vfs.h>
+#define statvfs statfs
+#else
+#include <sys/statvfs.h>
+#endif
+#ifdef GHC_OS_CYGWIN
+#include <strings.h>
+#endif
+#if !defined(__ANDROID__) || __ANDROID_API__ >= 26
+#include <langinfo.h>
+#endif
+#endif
+#ifdef GHC_OS_MACOS
+#include <Availability.h>
+#endif
+
+#if defined(__cpp_impl_three_way_comparison) && defined(__has_include)
+#if __has_include(<compare>)
+#define GHC_HAS_THREEWAY_COMP
+#include <compare>
+#endif
+#endif
+
+#include <algorithm>
+#include <cctype>
+#include <chrono>
+#include <clocale>
+#include <cstdlib>
+#include <cstring>
+#include <fstream>
+#include <functional>
+#include <memory>
+#include <stack>
+#include <stdexcept>
+#include <string>
+#include <system_error>
+#include <type_traits>
+#include <utility>
+#include <vector>
+
+#else  // GHC_EXPAND_IMPL
+
+#if defined(__cpp_impl_three_way_comparison) && defined(__has_include)
+#if __has_include(<compare>)
+#define GHC_HAS_THREEWAY_COMP
+#include <compare>
+#endif
+#endif
+#include <chrono>
+#include <fstream>
+#include <memory>
+#include <stack>
+#include <stdexcept>
+#include <string>
+#include <system_error>
+#ifdef GHC_OS_WINDOWS
+#include <vector>
+#endif
+#endif  // GHC_EXPAND_IMPL
+
+// After standard library includes.
+// Standard library support for std::string_view.
+#if defined(__cpp_lib_string_view)
+#define GHC_HAS_STD_STRING_VIEW
+#elif defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 4000) && (__cplusplus >= 201402)
+#define GHC_HAS_STD_STRING_VIEW
+#elif defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE >= 7) && (__cplusplus >= 201703)
+#define GHC_HAS_STD_STRING_VIEW
+#elif defined(_MSC_VER) && (_MSC_VER >= 1910 && _MSVC_LANG >= 201703)
+#define GHC_HAS_STD_STRING_VIEW
+#endif
+
+// Standard library support for std::experimental::string_view.
+#if defined(_LIBCPP_VERSION) && (_LIBCPP_VERSION >= 3700 && _LIBCPP_VERSION < 7000) && (__cplusplus >= 201402)
+#define GHC_HAS_STD_EXPERIMENTAL_STRING_VIEW
+#elif defined(__GNUC__) && (((__GNUC__ == 4) && (__GNUC_MINOR__ >= 9)) || (__GNUC__ > 4)) && (__cplusplus >= 201402)
+#define GHC_HAS_STD_EXPERIMENTAL_STRING_VIEW
+#elif defined(__GLIBCXX__) && defined(_GLIBCXX_USE_DUAL_ABI) && (__cplusplus >= 201402)
+// macro _GLIBCXX_USE_DUAL_ABI is always defined in libstdc++ from gcc-5 and newer
+#define GHC_HAS_STD_EXPERIMENTAL_STRING_VIEW
+#endif
+
+#if defined(GHC_HAS_STD_STRING_VIEW)
+#include <string_view>
+#elif defined(GHC_HAS_STD_EXPERIMENTAL_STRING_VIEW)
+#include <experimental/string_view>
+#endif
+
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// Behaviour Switches (see README.md, should match the config in test/filesystem_test.cpp):
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// Enforce C++17 API where possible when compiling for C++20, handles the following cases:
+// * fs::path::u8string() returns std::string instead of std::u8string
+// #define GHC_FILESYSTEM_ENFORCE_CPP17_API
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// LWG #2682 disables the since then invalid use of the copy option create_symlinks on directories
+// configure LWG conformance ()
+#define LWG_2682_BEHAVIOUR
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// LWG #2395 makes crate_directory/create_directories not emit an error if there is a regular
+// file with that name, it is superseded by P1164R1, so only activate if really needed
+// #define LWG_2935_BEHAVIOUR
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// LWG #2936 enables new element wise (more expensive) path comparison
+// * if this->root_name().native().compare(p.root_name().native()) != 0 return result
+// * if this->has_root_directory() and !p.has_root_directory() return -1
+// * if !this->has_root_directory() and p.has_root_directory() return -1
+// * else result of element wise comparison of path iteration where first comparison is != 0 or 0
+//   if all comparisons are 0 (on Windows this implementation does case insensitive root_name()
+//   comparison)
+#define LWG_2936_BEHAVIOUR
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// LWG #2937 enforces that fs::equivalent emits an error, if !fs::exists(p1)||!exists(p2)
+#define LWG_2937_BEHAVIOUR
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// UTF8-Everywhere is the original behaviour of ghc::filesystem. But since v1.5 the windows
+// version defaults to std::wstring storage backend. Still all std::string will be interpreted
+// as UTF-8 encoded. With this define you can enfoce the old behavior on Windows, using
+// std::string as backend and for fs::path::native() and char for fs::path::c_str(). This
+// needs more conversions so it is (an was before v1.5) slower, bot might help keeping source
+// homogeneous in a multi platform project.
+// #define GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// Raise errors/exceptions when invalid unicode codepoints or UTF-8 sequences are found,
+// instead of replacing them with the unicode replacement character (U+FFFD).
+// #define GHC_RAISE_UNICODE_ERRORS
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+// Automatic prefix windows path with "\\?\" if they would break the MAX_PATH length.
+// instead of replacing them with the unicode replacement character (U+FFFD).
+#ifndef GHC_WIN_DISABLE_AUTO_PREFIXES
+#define GHC_WIN_AUTO_PREFIX_LONG_PATH
+#endif  // GHC_WIN_DISABLE_AUTO_PREFIXES
+//- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
+
+// ghc::filesystem version in decimal (major * 10000 + minor * 100 + patch)
+#define GHC_FILESYSTEM_VERSION 10506L
+
+#if !defined(GHC_WITH_EXCEPTIONS) && (defined(__EXCEPTIONS) || defined(__cpp_exceptions) || defined(_CPPUNWIND))
+#define GHC_WITH_EXCEPTIONS
+#endif
+#if !defined(GHC_WITH_EXCEPTIONS) && defined(GHC_RAISE_UNICODE_ERRORS)
+#error "Can't raise unicode errors with exception support disabled"
+#endif
+
+namespace ghc {
+namespace filesystem {
+
+#if defined(GHC_HAS_CUSTOM_STRING_VIEW)
+#define GHC_WITH_STRING_VIEW
+#elif defined(GHC_HAS_STD_STRING_VIEW)
+#define GHC_WITH_STRING_VIEW
+using std::basic_string_view;
+#elif defined(GHC_HAS_STD_EXPERIMENTAL_STRING_VIEW)
+#define GHC_WITH_STRING_VIEW
+using std::experimental::basic_string_view;
+#endif
+
+// temporary existing exception type for yet unimplemented parts
+class GHC_FS_API_CLASS not_implemented_exception : public std::logic_error
+{
+public:
+    not_implemented_exception()
+        : std::logic_error("function not implemented yet.")
+    {
+    }
+};
+
+template <typename char_type>
+class path_helper_base
+{
+public:
+    using value_type = char_type;
+#ifdef GHC_OS_WINDOWS
+    static constexpr value_type preferred_separator = '\\';
+#else
+    static constexpr value_type preferred_separator = '/';
+#endif
+};
+
+#if __cplusplus < 201703L
+template <typename char_type>
+constexpr char_type path_helper_base<char_type>::preferred_separator;
+#endif
+
+#ifdef GHC_OS_WINDOWS
+class path;
+namespace detail {
+bool has_executable_extension(const path& p);
+}
+#endif
+
+// [fs.class.path] class path
+class GHC_FS_API_CLASS path
+#if defined(GHC_OS_WINDOWS) && !defined(GHC_WIN_DISABLE_WSTRING_STORAGE_TYPE)
+#define GHC_USE_WCHAR_T
+#define GHC_NATIVEWP(p) p.c_str()
+#define GHC_PLATFORM_LITERAL(str) L##str
+    : private path_helper_base<std::wstring::value_type>
+{
+public:
+    using path_helper_base<std::wstring::value_type>::value_type;
+#else
+#define GHC_NATIVEWP(p) p.wstring().c_str()
+#define GHC_PLATFORM_LITERAL(str) str
+    : private path_helper_base<std::string::value_type>
+{
+public:
+    using path_helper_base<std::string::value_type>::value_type;
+#endif
+    using string_type = std::basic_string<value_type>;
+    using path_helper_base<value_type>::preferred_separator;
+
+    // [fs.enum.path.format] enumeration format
+    /// The path format in which the constructor argument is given.
+    enum format {
+        generic_format,  ///< The generic format, internally used by
+                         ///< ghc::filesystem::path with slashes
+        native_format,   ///< The format native to the current platform this code
+                         ///< is build for
+        auto_format,     ///< Try to auto-detect the format, fallback to native
+    };
+
+    template <class T>
+    struct _is_basic_string : std::false_type
+    {
+    };
+    template <class CharT, class Traits, class Alloc>
+    struct _is_basic_string<std::basic_string<CharT, Traits, Alloc>> : std::true_type
+    {
+    };
+    template <class CharT>
+    struct _is_basic_string<std::basic_string<CharT, std::char_traits<CharT>, std::allocator<CharT>>> : std::true_type
+    {
+    };
+#ifdef GHC_WITH_STRING_VIEW
+    template <class CharT, class Traits>
+    struct _is_basic_string<basic_string_view<CharT, Traits>> : std::true_type
+    {
+    };
+    template <class CharT>
+    struct _is_basic_string<basic_string_view<CharT, std::char_traits<CharT>>> : std::true_type
+    {
+    };
+#endif
+
+    template <typename T1, typename T2 = void>
+    using path_type = typename std::enable_if<!std::is_same<path, T1>::value, path>::type;
+    template <typename T>
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+    using path_from_string =
+        typename std::enable_if<_is_basic_string<T>::value || std::is_same<char const*, typename std::decay<T>::type>::value || std::is_same<char*, typename std::decay<T>::type>::value || std::is_same<char8_t const*, typename std::decay<T>::type>::value ||
+                                    std::is_same<char8_t*, typename std::decay<T>::type>::value || std::is_same<char16_t const*, typename std::decay<T>::type>::value || std::is_same<char16_t*, typename std::decay<T>::type>::value ||
+                                    std::is_same<char32_t const*, typename std::decay<T>::type>::value || std::is_same<char32_t*, typename std::decay<T>::type>::value || std::is_same<wchar_t const*, typename std::decay<T>::type>::value ||
+                                    std::is_same<wchar_t*, typename std::decay<T>::type>::value,
+                                path>::type;
+    template <typename T>
+    using path_type_EcharT = typename std::enable_if<std::is_same<T, char>::value || std::is_same<T, char8_t>::value || std::is_same<T, char16_t>::value || std::is_same<T, char32_t>::value || std::is_same<T, wchar_t>::value, path>::type;
+#else
+    using path_from_string = typename std::enable_if<_is_basic_string<T>::value || std::is_same<char const*, typename std::decay<T>::type>::value || std::is_same<char*, typename std::decay<T>::type>::value ||
+                                    std::is_same<char16_t const*, typename std::decay<T>::type>::value || std::is_same<char16_t*, typename std::decay<T>::type>::value || std::is_same<char32_t const*, typename std::decay<T>::type>::value ||
+                                    std::is_same<char32_t*, typename std::decay<T>::type>::value || std::is_same<wchar_t const*, typename std::decay<T>::type>::value || std::is_same<wchar_t*, typename std::decay<T>::type>::value,
+        path>::type;
+    template <typename T>
+    using path_type_EcharT = typename std::enable_if<std::is_same<T, char>::value || std::is_same<T, char16_t>::value || std::is_same<T, char32_t>::value || std::is_same<T, wchar_t>::value, path>::type;
+#endif
+    // [fs.path.construct] constructors and destructor
+    path() noexcept;
+    path(const path& p);
+    path(path&& p) noexcept;
+    path(string_type&& source, format fmt = auto_format);
+    template <class Source, typename = path_from_string<Source>>
+    path(const Source& source, format fmt = auto_format);
+    template <class InputIterator>
+    path(InputIterator first, InputIterator last, format fmt = auto_format);
+#ifdef GHC_WITH_EXCEPTIONS
+    template <class Source, typename = path_from_string<Source>>
+    path(const Source& source, const std::locale& loc, format fmt = auto_format);
+    template <class InputIterator>
+    path(InputIterator first, InputIterator last, const std::locale& loc, format fmt = auto_format);
+#endif
+    ~path();
+
+    // [fs.path.assign] assignments
+    path& operator=(const path& p);
+    path& operator=(path&& p) noexcept;
+    path& operator=(string_type&& source);
+    path& assign(string_type&& source);
+    template <class Source>
+    path& operator=(const Source& source);
+    template <class Source>
+    path& assign(const Source& source);
+    template <class InputIterator>
+    path& assign(InputIterator first, InputIterator last);
+
+    // [fs.path.append] appends
+    path& operator/=(const path& p);
+    template <class Source>
+    path& operator/=(const Source& source);
+    template <class Source>
+    path& append(const Source& source);
+    template <class InputIterator>
+    path& append(InputIterator first, InputIterator last);
+
+    // [fs.path.concat] concatenation
+    path& operator+=(const path& x);
+    path& operator+=(const string_type& x);
+#ifdef GHC_WITH_STRING_VIEW
+    path& operator+=(basic_string_view<value_type> x);
+#endif
+    path& operator+=(const value_type* x);
+    path& operator+=(value_type x);
+    template <class Source>
+    path_from_string<Source>& operator+=(const Source& x);
+    template <class EcharT>
+    path_type_EcharT<EcharT>& operator+=(EcharT x);
+    template <class Source>
+    path& concat(const Source& x);
+    template <class InputIterator>
+    path& concat(InputIterator first, InputIterator last);
+
+    // [fs.path.modifiers] modifiers
+    void clear() noexcept;
+    path& make_preferred();
+    path& remove_filename();
+    path& replace_filename(const path& replacement);
+    path& replace_extension(const path& replacement = path());
+    void swap(path& rhs) noexcept;
+
+    // [fs.path.native.obs] native format observers
+    const string_type& native() const noexcept;
+    const value_type* c_str() const noexcept;
+    operator string_type() const;
+    template <class EcharT, class traits = std::char_traits<EcharT>, class Allocator = std::allocator<EcharT>>
+    std::basic_string<EcharT, traits, Allocator> string(const Allocator& a = Allocator()) const;
+    std::string string() const;
+    std::wstring wstring() const;
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+    std::u8string u8string() const;
+#else
+    std::string u8string() const;
+#endif
+    std::u16string u16string() const;
+    std::u32string u32string() const;
+
+    // [fs.path.generic.obs] generic format observers
+    template <class EcharT, class traits = std::char_traits<EcharT>, class Allocator = std::allocator<EcharT>>
+    std::basic_string<EcharT, traits, Allocator> generic_string(const Allocator& a = Allocator()) const;
+    std::string generic_string() const;
+    std::wstring generic_wstring() const;
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+    std::u8string generic_u8string() const;
+#else
+    std::string generic_u8string() const;
+#endif
+    std::u16string generic_u16string() const;
+    std::u32string generic_u32string() const;
+
+    // [fs.path.compare] compare
+    int compare(const path& p) const noexcept;
+    int compare(const string_type& s) const;
+#ifdef GHC_WITH_STRING_VIEW
+    int compare(basic_string_view<value_type> s) const;
+#endif
+    int compare(const value_type* s) const;
+
+    // [fs.path.decompose] decomposition
+    path root_name() const;
+    path root_directory() const;
+    path root_path() const;
+    path relative_path() const;
+    path parent_path() const;
+    path filename() const;
+    path stem() const;
+    path extension() const;
+
+    // [fs.path.query] query
+    bool empty() const noexcept;
+    bool has_root_name() const;
+    bool has_root_directory() const;
+    bool has_root_path() const;
+    bool has_relative_path() const;
+    bool has_parent_path() const;
+    bool has_filename() const;
+    bool has_stem() const;
+    bool has_extension() const;
+    bool is_absolute() const;
+    bool is_relative() const;
+
+    // [fs.path.gen] generation
+    path lexically_normal() const;
+    path lexically_relative(const path& base) const;
+    path lexically_proximate(const path& base) const;
+
+    // [fs.path.itr] iterators
+    class iterator;
+    using const_iterator = iterator;
+    iterator begin() const;
+    iterator end() const;
+
+private:
+    using impl_value_type = value_type;
+    using impl_string_type = std::basic_string<impl_value_type>;
+    friend class directory_iterator;
+    void append_name(const value_type* name);
+    static constexpr impl_value_type generic_separator = '/';
+    template <typename InputIterator>
+    class input_iterator_range
+    {
+    public:
+        typedef InputIterator iterator;
+        typedef InputIterator const_iterator;
+        typedef typename InputIterator::difference_type difference_type;
+
+        input_iterator_range(const InputIterator& first, const InputIterator& last)
+            : _first(first)
+            , _last(last)
+        {
+        }
+
+        InputIterator begin() const { return _first; }
+        InputIterator end() const { return _last; }
+
+    private:
+        InputIterator _first;
+        InputIterator _last;
+    };
+    friend void swap(path& lhs, path& rhs) noexcept;
+    friend size_t hash_value(const path& p) noexcept;
+    friend path canonical(const path& p, std::error_code& ec);
+    string_type::size_type root_name_length() const noexcept;
+    void postprocess_path_with_format(format fmt);
+    void check_long_path();
+    impl_string_type _path;
+#ifdef GHC_OS_WINDOWS
+    void handle_prefixes();
+    friend bool detail::has_executable_extension(const path& p);
+#ifdef GHC_WIN_AUTO_PREFIX_LONG_PATH
+    string_type::size_type _prefixLength{0};
+#else  // GHC_WIN_AUTO_PREFIX_LONG_PATH
+    static const string_type::size_type _prefixLength{0};
+#endif  // GHC_WIN_AUTO_PREFIX_LONG_PATH
+#else
+    static const string_type::size_type _prefixLength{0};
+#endif
+};
+
+// [fs.path.nonmember] path non-member functions
+GHC_FS_API void swap(path& lhs, path& rhs) noexcept;
+GHC_FS_API size_t hash_value(const path& p) noexcept;
+#ifdef GHC_HAS_THREEWAY_COMP
+GHC_FS_API std::strong_ordering operator<=>(const path& lhs, const path& rhs) noexcept;
+#endif
+GHC_FS_API bool operator==(const path& lhs, const path& rhs) noexcept;
+GHC_FS_API bool operator!=(const path& lhs, const path& rhs) noexcept;
+GHC_FS_API bool operator<(const path& lhs, const path& rhs) noexcept;
+GHC_FS_API bool operator<=(const path& lhs, const path& rhs) noexcept;
+GHC_FS_API bool operator>(const path& lhs, const path& rhs) noexcept;
+GHC_FS_API bool operator>=(const path& lhs, const path& rhs) noexcept;
+GHC_FS_API path operator/(const path& lhs, const path& rhs);
+
+// [fs.path.io] path inserter and extractor
+template <class charT, class traits>
+std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const path& p);
+template <class charT, class traits>
+std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, path& p);
+
+// [pfs.path.factory] path factory functions
+template <class Source, typename = path::path_from_string<Source>>
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+[[deprecated("use ghc::filesystem::path::path() with std::u8string instead")]]
+#endif
+path u8path(const Source& source);
+template <class InputIterator>
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+[[deprecated("use ghc::filesystem::path::path() with std::u8string instead")]]
+#endif
+path u8path(InputIterator first, InputIterator last);
+
+// [fs.class.filesystem_error] class filesystem_error
+class GHC_FS_API_CLASS filesystem_error : public std::system_error
+{
+public:
+    filesystem_error(const std::string& what_arg, std::error_code ec);
+    filesystem_error(const std::string& what_arg, const path& p1, std::error_code ec);
+    filesystem_error(const std::string& what_arg, const path& p1, const path& p2, std::error_code ec);
+    const path& path1() const noexcept;
+    const path& path2() const noexcept;
+    const char* what() const noexcept override;
+
+private:
+    std::string _what_arg;
+    std::error_code _ec;
+    path _p1, _p2;
+};
+
+class GHC_FS_API_CLASS path::iterator
+{
+public:
+    using value_type = const path;
+    using difference_type = std::ptrdiff_t;
+    using pointer = const path*;
+    using reference = const path&;
+    using iterator_category = std::bidirectional_iterator_tag;
+
+    iterator();
+    iterator(const path& p, const impl_string_type::const_iterator& pos);
+    iterator& operator++();
+    iterator operator++(int);
+    iterator& operator--();
+    iterator operator--(int);
+    bool operator==(const iterator& other) const;
+    bool operator!=(const iterator& other) const;
+    reference operator*() const;
+    pointer operator->() const;
+
+private:
+    friend class path;
+    impl_string_type::const_iterator increment(const impl_string_type::const_iterator& pos) const;
+    impl_string_type::const_iterator decrement(const impl_string_type::const_iterator& pos) const;
+    void updateCurrent();
+    impl_string_type::const_iterator _first;
+    impl_string_type::const_iterator _last;
+    impl_string_type::const_iterator _prefix;
+    impl_string_type::const_iterator _root;
+    impl_string_type::const_iterator _iter;
+    path _current;
+};
+
+struct space_info
+{
+    uintmax_t capacity;
+    uintmax_t free;
+    uintmax_t available;
+};
+
+// [fs.enum] enumerations
+// [fs.enum.file_type]
+enum class file_type {
+    none,
+    not_found,
+    regular,
+    directory,
+    symlink,
+    block,
+    character,
+    fifo,
+    socket,
+    unknown,
+};
+
+// [fs.enum.perms]
+enum class perms : uint16_t {
+    none = 0,
+
+    owner_read = 0400,
+    owner_write = 0200,
+    owner_exec = 0100,
+    owner_all = 0700,
+
+    group_read = 040,
+    group_write = 020,
+    group_exec = 010,
+    group_all = 070,
+
+    others_read = 04,
+    others_write = 02,
+    others_exec = 01,
+    others_all = 07,
+
+    all = 0777,
+    set_uid = 04000,
+    set_gid = 02000,
+    sticky_bit = 01000,
+
+    mask = 07777,
+    unknown = 0xffff
+};
+
+// [fs.enum.perm.opts]
+enum class perm_options : uint16_t {
+    replace = 3,
+    add = 1,
+    remove = 2,
+    nofollow = 4,
+};
+
+// [fs.enum.copy.opts]
+enum class copy_options : uint16_t {
+    none = 0,
+
+    skip_existing = 1,
+    overwrite_existing = 2,
+    update_existing = 4,
+
+    recursive = 8,
+
+    copy_symlinks = 0x10,
+    skip_symlinks = 0x20,
+
+    directories_only = 0x40,
+    create_symlinks = 0x80,
+#ifndef GHC_OS_WEB
+    create_hard_links = 0x100
+#endif
+};
+
+// [fs.enum.dir.opts]
+enum class directory_options : uint16_t {
+    none = 0,
+    follow_directory_symlink = 1,
+    skip_permission_denied = 2,
+};
+
+// [fs.class.file_status] class file_status
+class GHC_FS_API_CLASS file_status
+{
+public:
+    // [fs.file_status.cons] constructors and destructor
+    file_status() noexcept;
+    explicit file_status(file_type ft, perms prms = perms::unknown) noexcept;
+    file_status(const file_status&) noexcept;
+    file_status(file_status&&) noexcept;
+    ~file_status();
+    // assignments:
+    file_status& operator=(const file_status&) noexcept;
+    file_status& operator=(file_status&&) noexcept;
+    // [fs.file_status.mods] modifiers
+    void type(file_type ft) noexcept;
+    void permissions(perms prms) noexcept;
+    // [fs.file_status.obs] observers
+    file_type type() const noexcept;
+    perms permissions() const noexcept;
+    friend bool operator==(const file_status& lhs, const file_status& rhs) noexcept { return lhs.type() == rhs.type() && lhs.permissions() == rhs.permissions(); }
+private:
+    file_type _type;
+    perms _perms;
+};
+
+using file_time_type = std::chrono::time_point<std::chrono::system_clock>;
+
+// [fs.class.directory_entry] Class directory_entry
+class GHC_FS_API_CLASS directory_entry
+{
+public:
+    // [fs.dir.entry.cons] constructors and destructor
+    directory_entry() noexcept = default;
+    directory_entry(const directory_entry&) = default;
+    directory_entry(directory_entry&&) noexcept = default;
+#ifdef GHC_WITH_EXCEPTIONS
+    explicit directory_entry(const path& p);
+#endif
+    directory_entry(const path& p, std::error_code& ec);
+    ~directory_entry();
+
+    // assignments:
+    directory_entry& operator=(const directory_entry&) = default;
+    directory_entry& operator=(directory_entry&&) noexcept = default;
+
+    // [fs.dir.entry.mods] modifiers
+#ifdef GHC_WITH_EXCEPTIONS
+    void assign(const path& p);
+    void replace_filename(const path& p);
+    void refresh();
+#endif
+    void assign(const path& p, std::error_code& ec);
+    void replace_filename(const path& p, std::error_code& ec);
+    void refresh(std::error_code& ec) noexcept;
+
+    // [fs.dir.entry.obs] observers
+    const filesystem::path& path() const noexcept;
+    operator const filesystem::path&() const noexcept;
+#ifdef GHC_WITH_EXCEPTIONS
+    bool exists() const;
+    bool is_block_file() const;
+    bool is_character_file() const;
+    bool is_directory() const;
+    bool is_fifo() const;
+    bool is_other() const;
+    bool is_regular_file() const;
+    bool is_socket() const;
+    bool is_symlink() const;
+    uintmax_t file_size() const;
+    file_time_type last_write_time() const;
+    file_status status() const;
+    file_status symlink_status() const;
+#endif
+    bool exists(std::error_code& ec) const noexcept;
+    bool is_block_file(std::error_code& ec) const noexcept;
+    bool is_character_file(std::error_code& ec) const noexcept;
+    bool is_directory(std::error_code& ec) const noexcept;
+    bool is_fifo(std::error_code& ec) const noexcept;
+    bool is_other(std::error_code& ec) const noexcept;
+    bool is_regular_file(std::error_code& ec) const noexcept;
+    bool is_socket(std::error_code& ec) const noexcept;
+    bool is_symlink(std::error_code& ec) const noexcept;
+    uintmax_t file_size(std::error_code& ec) const noexcept;
+    file_time_type last_write_time(std::error_code& ec) const noexcept;
+    file_status status(std::error_code& ec) const noexcept;
+    file_status symlink_status(std::error_code& ec) const noexcept;
+
+#ifndef GHC_OS_WEB
+#ifdef GHC_WITH_EXCEPTIONS
+    uintmax_t hard_link_count() const;
+#endif
+    uintmax_t hard_link_count(std::error_code& ec) const noexcept;
+#endif
+
+#ifdef GHC_HAS_THREEWAY_COMP
+    std::strong_ordering operator<=>(const directory_entry& rhs) const noexcept;
+#endif
+    bool operator<(const directory_entry& rhs) const noexcept;
+    bool operator==(const directory_entry& rhs) const noexcept;
+    bool operator!=(const directory_entry& rhs) const noexcept;
+    bool operator<=(const directory_entry& rhs) const noexcept;
+    bool operator>(const directory_entry& rhs) const noexcept;
+    bool operator>=(const directory_entry& rhs) const noexcept;
+
+private:
+    friend class directory_iterator;
+#ifdef GHC_WITH_EXCEPTIONS
+    file_type status_file_type() const;
+#endif
+    file_type status_file_type(std::error_code& ec) const noexcept;
+    filesystem::path _path;
+    file_status _status;
+    file_status _symlink_status;
+    uintmax_t _file_size = static_cast<uintmax_t>(-1);
+#ifndef GHC_OS_WINDOWS
+    uintmax_t _hard_link_count = static_cast<uintmax_t>(-1);
+#endif
+    time_t _last_write_time = 0;
+};
+
+// [fs.class.directory.iterator] Class directory_iterator
+class GHC_FS_API_CLASS directory_iterator
+{
+public:
+    class GHC_FS_API_CLASS proxy
+    {
+    public:
+        const directory_entry& operator*() const& noexcept { return _dir_entry; }
+        directory_entry operator*() && noexcept { return std::move(_dir_entry); }
+
+    private:
+        explicit proxy(const directory_entry& dir_entry)
+            : _dir_entry(dir_entry)
+        {
+        }
+        friend class directory_iterator;
+        friend class recursive_directory_iterator;
+        directory_entry _dir_entry;
+    };
+    using iterator_category = std::input_iterator_tag;
+    using value_type = directory_entry;
+    using difference_type = std::ptrdiff_t;
+    using pointer = const directory_entry*;
+    using reference = const directory_entry&;
+
+    // [fs.dir.itr.members] member functions
+    directory_iterator() noexcept;
+#ifdef GHC_WITH_EXCEPTIONS
+    explicit directory_iterator(const path& p);
+    directory_iterator(const path& p, directory_options options);
+#endif
+    directory_iterator(const path& p, std::error_code& ec) noexcept;
+    directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept;
+    directory_iterator(const directory_iterator& rhs);
+    directory_iterator(directory_iterator&& rhs) noexcept;
+    ~directory_iterator();
+    directory_iterator& operator=(const directory_iterator& rhs);
+    directory_iterator& operator=(directory_iterator&& rhs) noexcept;
+    const directory_entry& operator*() const;
+    const directory_entry* operator->() const;
+#ifdef GHC_WITH_EXCEPTIONS
+    directory_iterator& operator++();
+#endif
+    directory_iterator& increment(std::error_code& ec) noexcept;
+
+    // other members as required by [input.iterators]
+#ifdef GHC_WITH_EXCEPTIONS
+    proxy operator++(int)
+    {
+        proxy p{**this};
+        ++*this;
+        return p;
+    }
+#endif
+    bool operator==(const directory_iterator& rhs) const;
+    bool operator!=(const directory_iterator& rhs) const;
+
+private:
+    friend class recursive_directory_iterator;
+    class impl;
+    std::shared_ptr<impl> _impl;
+};
+
+// [fs.dir.itr.nonmembers] directory_iterator non-member functions
+GHC_FS_API directory_iterator begin(directory_iterator iter) noexcept;
+GHC_FS_API directory_iterator end(const directory_iterator&) noexcept;
+
+// [fs.class.re.dir.itr] class recursive_directory_iterator
+class GHC_FS_API_CLASS recursive_directory_iterator
+{
+public:
+    using iterator_category = std::input_iterator_tag;
+    using value_type = directory_entry;
+    using difference_type = std::ptrdiff_t;
+    using pointer = const directory_entry*;
+    using reference = const directory_entry&;
+
+    // [fs.rec.dir.itr.members] constructors and destructor
+    recursive_directory_iterator() noexcept;
+#ifdef GHC_WITH_EXCEPTIONS
+    explicit recursive_directory_iterator(const path& p);
+    recursive_directory_iterator(const path& p, directory_options options);
+#endif
+    recursive_directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept;
+    recursive_directory_iterator(const path& p, std::error_code& ec) noexcept;
+    recursive_directory_iterator(const recursive_directory_iterator& rhs);
+    recursive_directory_iterator(recursive_directory_iterator&& rhs) noexcept;
+    ~recursive_directory_iterator();
+
+    // [fs.rec.dir.itr.members] observers
+    directory_options options() const;
+    int depth() const;
+    bool recursion_pending() const;
+
+    const directory_entry& operator*() const;
+    const directory_entry* operator->() const;
+
+    // [fs.rec.dir.itr.members] modifiers recursive_directory_iterator&
+    recursive_directory_iterator& operator=(const recursive_directory_iterator& rhs);
+    recursive_directory_iterator& operator=(recursive_directory_iterator&& rhs) noexcept;
+#ifdef GHC_WITH_EXCEPTIONS
+    recursive_directory_iterator& operator++();
+#endif
+    recursive_directory_iterator& increment(std::error_code& ec) noexcept;
+
+#ifdef GHC_WITH_EXCEPTIONS
+    void pop();
+#endif
+    void pop(std::error_code& ec);
+    void disable_recursion_pending();
+
+    // other members as required by [input.iterators]
+#ifdef GHC_WITH_EXCEPTIONS
+    directory_iterator::proxy operator++(int)
+    {
+        directory_iterator::proxy proxy{**this};
+        ++*this;
+        return proxy;
+    }
+#endif
+    bool operator==(const recursive_directory_iterator& rhs) const;
+    bool operator!=(const recursive_directory_iterator& rhs) const;
+
+private:
+    struct recursive_directory_iterator_impl
+    {
+        directory_options _options;
+        bool _recursion_pending;
+        std::stack<directory_iterator> _dir_iter_stack;
+        recursive_directory_iterator_impl(directory_options options, bool recursion_pending)
+            : _options(options)
+            , _recursion_pending(recursion_pending)
+        {
+        }
+    };
+    std::shared_ptr<recursive_directory_iterator_impl> _impl;
+};
+
+// [fs.rec.dir.itr.nonmembers] directory_iterator non-member functions
+GHC_FS_API recursive_directory_iterator begin(recursive_directory_iterator iter) noexcept;
+GHC_FS_API recursive_directory_iterator end(const recursive_directory_iterator&) noexcept;
+
+// [fs.op.funcs] filesystem operations
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_FS_API path absolute(const path& p);
+GHC_FS_API path canonical(const path& p);
+GHC_FS_API void copy(const path& from, const path& to);
+GHC_FS_API void copy(const path& from, const path& to, copy_options options);
+GHC_FS_API bool copy_file(const path& from, const path& to);
+GHC_FS_API bool copy_file(const path& from, const path& to, copy_options option);
+GHC_FS_API void copy_symlink(const path& existing_symlink, const path& new_symlink);
+GHC_FS_API bool create_directories(const path& p);
+GHC_FS_API bool create_directory(const path& p);
+GHC_FS_API bool create_directory(const path& p, const path& attributes);
+GHC_FS_API void create_directory_symlink(const path& to, const path& new_symlink);
+GHC_FS_API void create_symlink(const path& to, const path& new_symlink);
+GHC_FS_API path current_path();
+GHC_FS_API void current_path(const path& p);
+GHC_FS_API bool exists(const path& p);
+GHC_FS_API bool equivalent(const path& p1, const path& p2);
+GHC_FS_API uintmax_t file_size(const path& p);
+GHC_FS_API bool is_block_file(const path& p);
+GHC_FS_API bool is_character_file(const path& p);
+GHC_FS_API bool is_directory(const path& p);
+GHC_FS_API bool is_empty(const path& p);
+GHC_FS_API bool is_fifo(const path& p);
+GHC_FS_API bool is_other(const path& p);
+GHC_FS_API bool is_regular_file(const path& p);
+GHC_FS_API bool is_socket(const path& p);
+GHC_FS_API bool is_symlink(const path& p);
+GHC_FS_API file_time_type last_write_time(const path& p);
+GHC_FS_API void last_write_time(const path& p, file_time_type new_time);
+GHC_FS_API void permissions(const path& p, perms prms, perm_options opts = perm_options::replace);
+GHC_FS_API path proximate(const path& p, const path& base = current_path());
+GHC_FS_API path read_symlink(const path& p);
+GHC_FS_API path relative(const path& p, const path& base = current_path());
+GHC_FS_API bool remove(const path& p);
+GHC_FS_API uintmax_t remove_all(const path& p);
+GHC_FS_API void rename(const path& from, const path& to);
+GHC_FS_API void resize_file(const path& p, uintmax_t size);
+GHC_FS_API space_info space(const path& p);
+GHC_FS_API file_status status(const path& p);
+GHC_FS_API file_status symlink_status(const path& p);
+GHC_FS_API path temp_directory_path();
+GHC_FS_API path weakly_canonical(const path& p);
+#endif
+GHC_FS_API path absolute(const path& p, std::error_code& ec);
+GHC_FS_API path canonical(const path& p, std::error_code& ec);
+GHC_FS_API void copy(const path& from, const path& to, std::error_code& ec) noexcept;
+GHC_FS_API void copy(const path& from, const path& to, copy_options options, std::error_code& ec) noexcept;
+GHC_FS_API bool copy_file(const path& from, const path& to, std::error_code& ec) noexcept;
+GHC_FS_API bool copy_file(const path& from, const path& to, copy_options option, std::error_code& ec) noexcept;
+GHC_FS_API void copy_symlink(const path& existing_symlink, const path& new_symlink, std::error_code& ec) noexcept;
+GHC_FS_API bool create_directories(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool create_directory(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool create_directory(const path& p, const path& attributes, std::error_code& ec) noexcept;
+GHC_FS_API void create_directory_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept;
+GHC_FS_API void create_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept;
+GHC_FS_API path current_path(std::error_code& ec);
+GHC_FS_API void current_path(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool exists(file_status s) noexcept;
+GHC_FS_API bool exists(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool equivalent(const path& p1, const path& p2, std::error_code& ec) noexcept;
+GHC_FS_API uintmax_t file_size(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_block_file(file_status s) noexcept;
+GHC_FS_API bool is_block_file(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_character_file(file_status s) noexcept;
+GHC_FS_API bool is_character_file(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_directory(file_status s) noexcept;
+GHC_FS_API bool is_directory(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_empty(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_fifo(file_status s) noexcept;
+GHC_FS_API bool is_fifo(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_other(file_status s) noexcept;
+GHC_FS_API bool is_other(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_regular_file(file_status s) noexcept;
+GHC_FS_API bool is_regular_file(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_socket(file_status s) noexcept;
+GHC_FS_API bool is_socket(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool is_symlink(file_status s) noexcept;
+GHC_FS_API bool is_symlink(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API file_time_type last_write_time(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API void last_write_time(const path& p, file_time_type new_time, std::error_code& ec) noexcept;
+GHC_FS_API void permissions(const path& p, perms prms, std::error_code& ec) noexcept;
+GHC_FS_API void permissions(const path& p, perms prms, perm_options opts, std::error_code& ec) noexcept;
+GHC_FS_API path proximate(const path& p, std::error_code& ec);
+GHC_FS_API path proximate(const path& p, const path& base, std::error_code& ec);
+GHC_FS_API path read_symlink(const path& p, std::error_code& ec);
+GHC_FS_API path relative(const path& p, std::error_code& ec);
+GHC_FS_API path relative(const path& p, const path& base, std::error_code& ec);
+GHC_FS_API bool remove(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API uintmax_t remove_all(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API void rename(const path& from, const path& to, std::error_code& ec) noexcept;
+GHC_FS_API void resize_file(const path& p, uintmax_t size, std::error_code& ec) noexcept;
+GHC_FS_API space_info space(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API file_status status(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API bool status_known(file_status s) noexcept;
+GHC_FS_API file_status symlink_status(const path& p, std::error_code& ec) noexcept;
+GHC_FS_API path temp_directory_path(std::error_code& ec) noexcept;
+GHC_FS_API path weakly_canonical(const path& p, std::error_code& ec) noexcept;
+
+#ifndef GHC_OS_WEB
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_FS_API void create_hard_link(const path& to, const path& new_hard_link);
+GHC_FS_API uintmax_t hard_link_count(const path& p);
+#endif
+GHC_FS_API void create_hard_link(const path& to, const path& new_hard_link, std::error_code& ec) noexcept;
+GHC_FS_API uintmax_t hard_link_count(const path& p, std::error_code& ec) noexcept;
+#endif
+
+// Non-C++17 add-on std::fstream wrappers with path
+template <class charT, class traits = std::char_traits<charT>>
+class basic_filebuf : public std::basic_filebuf<charT, traits>
+{
+public:
+    basic_filebuf() {}
+    ~basic_filebuf() override {}
+    basic_filebuf(const basic_filebuf&) = delete;
+    const basic_filebuf& operator=(const basic_filebuf&) = delete;
+    basic_filebuf<charT, traits>* open(const path& p, std::ios_base::openmode mode)
+    {
+#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__)
+        return std::basic_filebuf<charT, traits>::open(p.wstring().c_str(), mode) ? this : 0;
+#else
+        return std::basic_filebuf<charT, traits>::open(p.string().c_str(), mode) ? this : 0;
+#endif
+    }
+};
+
+template <class charT, class traits = std::char_traits<charT>>
+class basic_ifstream : public std::basic_ifstream<charT, traits>
+{
+public:
+    basic_ifstream() {}
+#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__)
+    explicit basic_ifstream(const path& p, std::ios_base::openmode mode = std::ios_base::in)
+        : std::basic_ifstream<charT, traits>(p.wstring().c_str(), mode)
+    {
+    }
+    void open(const path& p, std::ios_base::openmode mode = std::ios_base::in) { std::basic_ifstream<charT, traits>::open(p.wstring().c_str(), mode); }
+#else
+    explicit basic_ifstream(const path& p, std::ios_base::openmode mode = std::ios_base::in)
+        : std::basic_ifstream<charT, traits>(p.string().c_str(), mode)
+    {
+    }
+    void open(const path& p, std::ios_base::openmode mode = std::ios_base::in) { std::basic_ifstream<charT, traits>::open(p.string().c_str(), mode); }
+#endif
+    basic_ifstream(const basic_ifstream&) = delete;
+    const basic_ifstream& operator=(const basic_ifstream&) = delete;
+    ~basic_ifstream() override {}
+};
+
+template <class charT, class traits = std::char_traits<charT>>
+class basic_ofstream : public std::basic_ofstream<charT, traits>
+{
+public:
+    basic_ofstream() {}
+#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__)
+    explicit basic_ofstream(const path& p, std::ios_base::openmode mode = std::ios_base::out)
+        : std::basic_ofstream<charT, traits>(p.wstring().c_str(), mode)
+    {
+    }
+    void open(const path& p, std::ios_base::openmode mode = std::ios_base::out) { std::basic_ofstream<charT, traits>::open(p.wstring().c_str(), mode); }
+#else
+    explicit basic_ofstream(const path& p, std::ios_base::openmode mode = std::ios_base::out)
+        : std::basic_ofstream<charT, traits>(p.string().c_str(), mode)
+    {
+    }
+    void open(const path& p, std::ios_base::openmode mode = std::ios_base::out) { std::basic_ofstream<charT, traits>::open(p.string().c_str(), mode); }
+#endif
+    basic_ofstream(const basic_ofstream&) = delete;
+    const basic_ofstream& operator=(const basic_ofstream&) = delete;
+    ~basic_ofstream() override {}
+};
+
+template <class charT, class traits = std::char_traits<charT>>
+class basic_fstream : public std::basic_fstream<charT, traits>
+{
+public:
+    basic_fstream() {}
+#if defined(GHC_OS_WINDOWS) && !defined(__GLIBCXX__)
+    explicit basic_fstream(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out)
+        : std::basic_fstream<charT, traits>(p.wstring().c_str(), mode)
+    {
+    }
+    void open(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out) { std::basic_fstream<charT, traits>::open(p.wstring().c_str(), mode); }
+#else
+    explicit basic_fstream(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out)
+        : std::basic_fstream<charT, traits>(p.string().c_str(), mode)
+    {
+    }
+    void open(const path& p, std::ios_base::openmode mode = std::ios_base::in | std::ios_base::out) { std::basic_fstream<charT, traits>::open(p.string().c_str(), mode); }
+#endif
+    basic_fstream(const basic_fstream&) = delete;
+    const basic_fstream& operator=(const basic_fstream&) = delete;
+    ~basic_fstream() override {}
+};
+
+typedef basic_filebuf<char> filebuf;
+typedef basic_filebuf<wchar_t> wfilebuf;
+typedef basic_ifstream<char> ifstream;
+typedef basic_ifstream<wchar_t> wifstream;
+typedef basic_ofstream<char> ofstream;
+typedef basic_ofstream<wchar_t> wofstream;
+typedef basic_fstream<char> fstream;
+typedef basic_fstream<wchar_t> wfstream;
+
+class GHC_FS_API_CLASS u8arguments
+{
+public:
+    u8arguments(int& argc, char**& argv);
+    ~u8arguments()
+    {
+        _refargc = _argc;
+        _refargv = _argv;
+    }
+
+    bool valid() const { return _isvalid; }
+
+private:
+    int _argc;
+    char** _argv;
+    int& _refargc;
+    char**& _refargv;
+    bool _isvalid;
+#ifdef GHC_OS_WINDOWS
+    std::vector<std::string> _args;
+    std::vector<char*> _argp;
+#endif
+};
+
+//-------------------------------------------------------------------------------------------------
+//  Implementation
+//-------------------------------------------------------------------------------------------------
+
+namespace detail {
+enum utf8_states_t { S_STRT = 0, S_RJCT = 8 };
+GHC_FS_API void appendUTF8(std::string& str, uint32_t unicode);
+GHC_FS_API bool is_surrogate(uint32_t c);
+GHC_FS_API bool is_high_surrogate(uint32_t c);
+GHC_FS_API bool is_low_surrogate(uint32_t c);
+GHC_FS_API unsigned consumeUtf8Fragment(const unsigned state, const uint8_t fragment, uint32_t& codepoint);
+enum class portable_error {
+    none = 0,
+    exists,
+    not_found,
+    not_supported,
+    not_implemented,
+    invalid_argument,
+    is_a_directory,
+};
+GHC_FS_API std::error_code make_error_code(portable_error err);
+#ifdef GHC_OS_WINDOWS
+GHC_FS_API std::error_code make_system_error(uint32_t err = 0);
+#else
+GHC_FS_API std::error_code make_system_error(int err = 0);
+#endif
+}  // namespace detail
+
+namespace detail {
+
+#ifdef GHC_EXPAND_IMPL
+
+GHC_INLINE std::error_code make_error_code(portable_error err)
+{
+#ifdef GHC_OS_WINDOWS
+    switch (err) {
+        case portable_error::none:
+            return std::error_code();
+        case portable_error::exists:
+            return std::error_code(ERROR_ALREADY_EXISTS, std::system_category());
+        case portable_error::not_found:
+            return std::error_code(ERROR_PATH_NOT_FOUND, std::system_category());
+        case portable_error::not_supported:
+            return std::error_code(ERROR_NOT_SUPPORTED, std::system_category());
+        case portable_error::not_implemented:
+            return std::error_code(ERROR_CALL_NOT_IMPLEMENTED, std::system_category());
+        case portable_error::invalid_argument:
+            return std::error_code(ERROR_INVALID_PARAMETER, std::system_category());
+        case portable_error::is_a_directory:
+#ifdef ERROR_DIRECTORY_NOT_SUPPORTED
+            return std::error_code(ERROR_DIRECTORY_NOT_SUPPORTED, std::system_category());
+#else
+            return std::error_code(ERROR_NOT_SUPPORTED, std::system_category());
+#endif
+    }
+#else
+    switch (err) {
+        case portable_error::none:
+            return std::error_code();
+        case portable_error::exists:
+            return std::error_code(EEXIST, std::system_category());
+        case portable_error::not_found:
+            return std::error_code(ENOENT, std::system_category());
+        case portable_error::not_supported:
+            return std::error_code(ENOTSUP, std::system_category());
+        case portable_error::not_implemented:
+            return std::error_code(ENOSYS, std::system_category());
+        case portable_error::invalid_argument:
+            return std::error_code(EINVAL, std::system_category());
+        case portable_error::is_a_directory:
+            return std::error_code(EISDIR, std::system_category());
+    }
+#endif
+    return std::error_code();
+}
+
+#ifdef GHC_OS_WINDOWS
+GHC_INLINE std::error_code make_system_error(uint32_t err)
+{
+    return std::error_code(err ? static_cast<int>(err) : static_cast<int>(::GetLastError()), std::system_category());
+}
+#else
+GHC_INLINE std::error_code make_system_error(int err)
+{
+    return std::error_code(err ? err : errno, std::system_category());
+}
+#endif
+
+#endif  // GHC_EXPAND_IMPL
+
+template <typename Enum>
+using EnableBitmask = typename std::enable_if<std::is_same<Enum, perms>::value || std::is_same<Enum, perm_options>::value || std::is_same<Enum, copy_options>::value || std::is_same<Enum, directory_options>::value, Enum>::type;
+}  // namespace detail
+
+template <typename Enum>
+constexpr detail::EnableBitmask<Enum> operator&(Enum X, Enum Y)
+{
+    using underlying = typename std::underlying_type<Enum>::type;
+    return static_cast<Enum>(static_cast<underlying>(X) & static_cast<underlying>(Y));
+}
+
+template <typename Enum>
+constexpr detail::EnableBitmask<Enum> operator|(Enum X, Enum Y)
+{
+    using underlying = typename std::underlying_type<Enum>::type;
+    return static_cast<Enum>(static_cast<underlying>(X) | static_cast<underlying>(Y));
+}
+
+template <typename Enum>
+constexpr detail::EnableBitmask<Enum> operator^(Enum X, Enum Y)
+{
+    using underlying = typename std::underlying_type<Enum>::type;
+    return static_cast<Enum>(static_cast<underlying>(X) ^ static_cast<underlying>(Y));
+}
+
+template <typename Enum>
+constexpr detail::EnableBitmask<Enum> operator~(Enum X)
+{
+    using underlying = typename std::underlying_type<Enum>::type;
+    return static_cast<Enum>(~static_cast<underlying>(X));
+}
+
+template <typename Enum>
+detail::EnableBitmask<Enum>& operator&=(Enum& X, Enum Y)
+{
+    X = X & Y;
+    return X;
+}
+
+template <typename Enum>
+detail::EnableBitmask<Enum>& operator|=(Enum& X, Enum Y)
+{
+    X = X | Y;
+    return X;
+}
+
+template <typename Enum>
+detail::EnableBitmask<Enum>& operator^=(Enum& X, Enum Y)
+{
+    X = X ^ Y;
+    return X;
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+namespace detail {
+
+GHC_INLINE bool in_range(uint32_t c, uint32_t lo, uint32_t hi)
+{
+    return (static_cast<uint32_t>(c - lo) < (hi - lo + 1));
+}
+
+GHC_INLINE bool is_surrogate(uint32_t c)
+{
+    return in_range(c, 0xd800, 0xdfff);
+}
+
+GHC_INLINE bool is_high_surrogate(uint32_t c)
+{
+    return (c & 0xfffffc00) == 0xd800;
+}
+
+GHC_INLINE bool is_low_surrogate(uint32_t c)
+{
+    return (c & 0xfffffc00) == 0xdc00;
+}
+
+GHC_INLINE void appendUTF8(std::string& str, uint32_t unicode)
+{
+    if (unicode <= 0x7f) {
+        str.push_back(static_cast<char>(unicode));
+    }
+    else if (unicode >= 0x80 && unicode <= 0x7ff) {
+        str.push_back(static_cast<char>((unicode >> 6) + 192));
+        str.push_back(static_cast<char>((unicode & 0x3f) + 128));
+    }
+    else if ((unicode >= 0x800 && unicode <= 0xd7ff) || (unicode >= 0xe000 && unicode <= 0xffff)) {
+        str.push_back(static_cast<char>((unicode >> 12) + 224));
+        str.push_back(static_cast<char>(((unicode & 0xfff) >> 6) + 128));
+        str.push_back(static_cast<char>((unicode & 0x3f) + 128));
+    }
+    else if (unicode >= 0x10000 && unicode <= 0x10ffff) {
+        str.push_back(static_cast<char>((unicode >> 18) + 240));
+        str.push_back(static_cast<char>(((unicode & 0x3ffff) >> 12) + 128));
+        str.push_back(static_cast<char>(((unicode & 0xfff) >> 6) + 128));
+        str.push_back(static_cast<char>((unicode & 0x3f) + 128));
+    }
+    else {
+#ifdef GHC_RAISE_UNICODE_ERRORS
+        throw filesystem_error("Illegal code point for unicode character.", str, std::make_error_code(std::errc::illegal_byte_sequence));
+#else
+        appendUTF8(str, 0xfffd);
+#endif
+    }
+}
+
+// Thanks to Bjoern Hoehrmann (https://bjoern.hoehrmann.de/utf-8/decoder/dfa/)
+// and Taylor R Campbell for the ideas to this DFA approach of UTF-8 decoding;
+// Generating debugging and shrinking my own DFA from scratch was a day of fun!
+GHC_INLINE unsigned consumeUtf8Fragment(const unsigned state, const uint8_t fragment, uint32_t& codepoint)
+{
+    static const uint32_t utf8_state_info[] = {
+        // encoded states
+        0x11111111u, 0x11111111u, 0x77777777u, 0x77777777u, 0x88888888u, 0x88888888u, 0x88888888u, 0x88888888u, 0x22222299u, 0x22222222u, 0x22222222u, 0x22222222u, 0x3333333au, 0x33433333u, 0x9995666bu, 0x99999999u,
+        0x88888880u, 0x22818108u, 0x88888881u, 0x88888882u, 0x88888884u, 0x88888887u, 0x88888886u, 0x82218108u, 0x82281108u, 0x88888888u, 0x88888883u, 0x88888885u, 0u,          0u,          0u,          0u,
+    };
+    uint8_t category = fragment < 128 ? 0 : (utf8_state_info[(fragment >> 3) & 0xf] >> ((fragment & 7) << 2)) & 0xf;
+    codepoint = (state ? (codepoint << 6) | (fragment & 0x3fu) : (0xffu >> category) & fragment);
+    return state == S_RJCT ? static_cast<unsigned>(S_RJCT) : static_cast<unsigned>((utf8_state_info[category + 16] >> (state << 2)) & 0xf);
+}
+
+GHC_INLINE bool validUtf8(const std::string& utf8String)
+{
+    std::string::const_iterator iter = utf8String.begin();
+    unsigned utf8_state = S_STRT;
+    std::uint32_t codepoint = 0;
+    while (iter < utf8String.end()) {
+        if ((utf8_state = consumeUtf8Fragment(utf8_state, static_cast<uint8_t>(*iter++), codepoint)) == S_RJCT) {
+            return false;
+        }
+    }
+    if (utf8_state) {
+        return false;
+    }
+    return true;
+}
+
+}  // namespace detail
+
+#endif
+
+namespace detail {
+
+template <class StringType, class Utf8String, typename std::enable_if<path::_is_basic_string<Utf8String>::value && (sizeof(typename Utf8String::value_type) == 1) && (sizeof(typename StringType::value_type) == 1)>::type* = nullptr>
+inline StringType fromUtf8(const Utf8String& utf8String, const typename StringType::allocator_type& alloc = typename StringType::allocator_type())
+{
+    return StringType(utf8String.begin(), utf8String.end(), alloc);
+}
+
+template <class StringType, class Utf8String, typename std::enable_if<path::_is_basic_string<Utf8String>::value && (sizeof(typename Utf8String::value_type) == 1) && (sizeof(typename StringType::value_type) == 2)>::type* = nullptr>
+inline StringType fromUtf8(const Utf8String& utf8String, const typename StringType::allocator_type& alloc = typename StringType::allocator_type())
+{
+    StringType result(alloc);
+    result.reserve(utf8String.length());
+    auto iter = utf8String.cbegin();
+    unsigned utf8_state = S_STRT;
+    std::uint32_t codepoint = 0;
+    while (iter < utf8String.cend()) {
+        if ((utf8_state = consumeUtf8Fragment(utf8_state, static_cast<uint8_t>(*iter++), codepoint)) == S_STRT) {
+            if (codepoint <= 0xffff) {
+                result += static_cast<typename StringType::value_type>(codepoint);
+            }
+            else {
+                codepoint -= 0x10000;
+                result += static_cast<typename StringType::value_type>((codepoint >> 10) + 0xd800);
+                result += static_cast<typename StringType::value_type>((codepoint & 0x3ff) + 0xdc00);
+            }
+            codepoint = 0;
+        }
+        else if (utf8_state == S_RJCT) {
+#ifdef GHC_RAISE_UNICODE_ERRORS
+            throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence));
+#else
+            result += static_cast<typename StringType::value_type>(0xfffd);
+            utf8_state = S_STRT;
+            codepoint = 0;
+#endif
+        }
+    }
+    if (utf8_state) {
+#ifdef GHC_RAISE_UNICODE_ERRORS
+        throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence));
+#else
+        result += static_cast<typename StringType::value_type>(0xfffd);
+#endif
+    }
+    return result;
+}
+
+template <class StringType, class Utf8String, typename std::enable_if<path::_is_basic_string<Utf8String>::value && (sizeof(typename Utf8String::value_type) == 1) && (sizeof(typename StringType::value_type) == 4)>::type* = nullptr>
+inline StringType fromUtf8(const Utf8String& utf8String, const typename StringType::allocator_type& alloc = typename StringType::allocator_type())
+{
+    StringType result(alloc);
+    result.reserve(utf8String.length());
+    auto iter = utf8String.cbegin();
+    unsigned utf8_state = S_STRT;
+    std::uint32_t codepoint = 0;
+    while (iter < utf8String.cend()) {
+        if ((utf8_state = consumeUtf8Fragment(utf8_state, static_cast<uint8_t>(*iter++), codepoint)) == S_STRT) {
+            result += static_cast<typename StringType::value_type>(codepoint);
+            codepoint = 0;
+        }
+        else if (utf8_state == S_RJCT) {
+#ifdef GHC_RAISE_UNICODE_ERRORS
+            throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence));
+#else
+            result += static_cast<typename StringType::value_type>(0xfffd);
+            utf8_state = S_STRT;
+            codepoint = 0;
+#endif
+        }
+    }
+    if (utf8_state) {
+#ifdef GHC_RAISE_UNICODE_ERRORS
+        throw filesystem_error("Illegal byte sequence for unicode character.", utf8String, std::make_error_code(std::errc::illegal_byte_sequence));
+#else
+        result += static_cast<typename StringType::value_type>(0xfffd);
+#endif
+    }
+    return result;
+}
+
+template <class StringType, typename charT, std::size_t N>
+inline StringType fromUtf8(const charT (&utf8String)[N])
+{
+#ifdef GHC_WITH_STRING_VIEW
+    return fromUtf8<StringType>(basic_string_view<charT>(utf8String, N - 1));
+#else
+    return fromUtf8<StringType>(std::basic_string<charT>(utf8String, N - 1));
+#endif
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value && (sizeof(typename strT::value_type) == 1), int>::type size = 1>
+inline std::string toUtf8(const strT& unicodeString)
+{
+    return std::string(unicodeString.begin(), unicodeString.end());
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value && (sizeof(typename strT::value_type) == 2), int>::type size = 2>
+inline std::string toUtf8(const strT& unicodeString)
+{
+    std::string result;
+    for (auto iter = unicodeString.begin(); iter != unicodeString.end(); ++iter) {
+        char32_t c = *iter;
+        if (is_surrogate(c)) {
+            ++iter;
+            if (iter != unicodeString.end() && is_high_surrogate(c) && is_low_surrogate(*iter)) {
+                appendUTF8(result, (char32_t(c) << 10) + *iter - 0x35fdc00);
+            }
+            else {
+#ifdef GHC_RAISE_UNICODE_ERRORS
+                throw filesystem_error("Illegal code point for unicode character.", result, std::make_error_code(std::errc::illegal_byte_sequence));
+#else
+                appendUTF8(result, 0xfffd);
+                if (iter == unicodeString.end()) {
+                    break;
+                }
+#endif
+            }
+        }
+        else {
+            appendUTF8(result, c);
+        }
+    }
+    return result;
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value && (sizeof(typename strT::value_type) == 4), int>::type size = 4>
+inline std::string toUtf8(const strT& unicodeString)
+{
+    std::string result;
+    for (auto c : unicodeString) {
+        appendUTF8(result, static_cast<uint32_t>(c));
+    }
+    return result;
+}
+
+template <typename charT>
+inline std::string toUtf8(const charT* unicodeString)
+{
+#ifdef GHC_WITH_STRING_VIEW
+    return toUtf8(basic_string_view<charT, std::char_traits<charT>>(unicodeString));
+#else
+    return toUtf8(std::basic_string<charT, std::char_traits<charT>>(unicodeString));
+#endif
+}
+
+#ifdef GHC_USE_WCHAR_T
+template <class StringType, class WString, typename std::enable_if<path::_is_basic_string<WString>::value && (sizeof(typename WString::value_type) == 2) && (sizeof(typename StringType::value_type) == 1), bool>::type = false>
+inline StringType fromWChar(const WString& wString, const typename StringType::allocator_type& alloc = typename StringType::allocator_type())
+{
+    auto temp = toUtf8(wString);
+    return StringType(temp.begin(), temp.end(), alloc);
+}
+
+template <class StringType, class WString, typename std::enable_if<path::_is_basic_string<WString>::value && (sizeof(typename WString::value_type) == 2) && (sizeof(typename StringType::value_type) == 2), bool>::type = false>
+inline StringType fromWChar(const WString& wString, const typename StringType::allocator_type& alloc = typename StringType::allocator_type())
+{
+    return StringType(wString.begin(), wString.end(), alloc);
+}
+
+template <class StringType, class WString, typename std::enable_if<path::_is_basic_string<WString>::value && (sizeof(typename WString::value_type) == 2) && (sizeof(typename StringType::value_type) == 4), bool>::type = false>
+inline StringType fromWChar(const WString& wString, const typename StringType::allocator_type& alloc = typename StringType::allocator_type())
+{
+    auto temp = toUtf8(wString);
+    return fromUtf8<StringType>(temp, alloc);
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value && (sizeof(typename strT::value_type) == 1), bool>::type = false>
+inline std::wstring toWChar(const strT& unicodeString)
+{
+    return fromUtf8<std::wstring>(unicodeString);
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value && (sizeof(typename strT::value_type) == 2), bool>::type = false>
+inline std::wstring toWChar(const strT& unicodeString)
+{
+    return std::wstring(unicodeString.begin(), unicodeString.end());
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value && (sizeof(typename strT::value_type) == 4), bool>::type = false>
+inline std::wstring toWChar(const strT& unicodeString)
+{
+    auto temp = toUtf8(unicodeString);
+    return fromUtf8<std::wstring>(temp);
+}
+
+template <typename charT>
+inline std::wstring toWChar(const charT* unicodeString)
+{
+#ifdef GHC_WITH_STRING_VIEW
+    return toWChar(basic_string_view<charT, std::char_traits<charT>>(unicodeString));
+#else
+    return toWChar(std::basic_string<charT, std::char_traits<charT>>(unicodeString));
+#endif
+}
+#endif // GHC_USE_WCHAR_T
+
+}  // namespace detail
+
+#ifdef GHC_EXPAND_IMPL
+
+namespace detail {
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value, bool>::type = true>
+GHC_INLINE bool startsWith(const strT& what, const strT& with)
+{
+    return with.length() <= what.length() && equal(with.begin(), with.end(), what.begin());
+}
+
+template <typename strT, typename std::enable_if<path::_is_basic_string<strT>::value, bool>::type = true>
+GHC_INLINE bool endsWith(const strT& what, const strT& with)
+{
+    return with.length() <= what.length() && what.compare(what.length() - with.length(), with.size(), with) == 0;
+}
+
+}  // namespace detail
+
+GHC_INLINE void path::check_long_path()
+{
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    if (is_absolute() && _path.length() >= MAX_PATH - 12 && !detail::startsWith(_path, impl_string_type(GHC_PLATFORM_LITERAL("\\\\?\\")))) {
+        postprocess_path_with_format(native_format);
+    }
+#endif
+}
+
+GHC_INLINE void path::postprocess_path_with_format(path::format fmt)
+{
+#ifdef GHC_RAISE_UNICODE_ERRORS
+    if (!detail::validUtf8(_path)) {
+        path t;
+        t._path = _path;
+        throw filesystem_error("Illegal byte sequence for unicode character.", t, std::make_error_code(std::errc::illegal_byte_sequence));
+    }
+#endif
+    switch (fmt) {
+#ifdef GHC_OS_WINDOWS
+        case path::native_format:
+        case path::auto_format:
+        case path::generic_format:
+            for (auto& c : _path) {
+                if (c == generic_separator) {
+                    c = preferred_separator;
+                }
+            }
+#ifdef GHC_WIN_AUTO_PREFIX_LONG_PATH
+            if (is_absolute() && _path.length() >= MAX_PATH - 12 && !detail::startsWith(_path, impl_string_type(GHC_PLATFORM_LITERAL("\\\\?\\")))) {
+                _path = GHC_PLATFORM_LITERAL("\\\\?\\") + _path;
+            }
+#endif
+            handle_prefixes();
+            break;
+#else
+        case path::auto_format:
+        case path::native_format:
+        case path::generic_format:
+            // nothing to do
+            break;
+#endif
+    }
+    if (_path.length() > _prefixLength + 2 && _path[_prefixLength] == preferred_separator && _path[_prefixLength + 1] == preferred_separator && _path[_prefixLength + 2] != preferred_separator) {
+        impl_string_type::iterator new_end = std::unique(_path.begin() + static_cast<string_type::difference_type>(_prefixLength) + 2, _path.end(), [](path::value_type lhs, path::value_type rhs) { return lhs == rhs && lhs == preferred_separator; });
+        _path.erase(new_end, _path.end());
+    }
+    else {
+        impl_string_type::iterator new_end = std::unique(_path.begin() + static_cast<string_type::difference_type>(_prefixLength), _path.end(), [](path::value_type lhs, path::value_type rhs) { return lhs == rhs && lhs == preferred_separator; });
+        _path.erase(new_end, _path.end());
+    }
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+template <class Source, typename>
+inline path::path(const Source& source, format fmt)
+#ifdef GHC_USE_WCHAR_T
+    : _path(detail::toWChar(source))
+#else
+    : _path(detail::toUtf8(source))
+#endif
+{
+    postprocess_path_with_format(fmt);
+}
+
+template <class Source, typename>
+inline path u8path(const Source& source)
+{
+    return path(source);
+}
+template <class InputIterator>
+inline path u8path(InputIterator first, InputIterator last)
+{
+    return path(first, last);
+}
+
+template <class InputIterator>
+inline path::path(InputIterator first, InputIterator last, format fmt)
+    : path(std::basic_string<typename std::iterator_traits<InputIterator>::value_type>(first, last), fmt)
+{
+    // delegated
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+namespace detail {
+
+GHC_INLINE bool equals_simple_insensitive(const path::value_type* str1, const path::value_type* str2)
+{
+#ifdef GHC_OS_WINDOWS
+#ifdef __GNUC__
+    while (::tolower((unsigned char)*str1) == ::tolower((unsigned char)*str2++)) {
+        if (*str1++ == 0)
+            return true;
+    }
+    return false;
+#else // __GNUC__
+#ifdef GHC_USE_WCHAR_T
+    return 0 == ::_wcsicmp(str1, str2);
+#else // GHC_USE_WCHAR_T
+    return 0 == ::_stricmp(str1, str2);
+#endif // GHC_USE_WCHAR_T
+#endif // __GNUC__
+#else // GHC_OS_WINDOWS
+    return 0 == ::strcasecmp(str1, str2);
+#endif // GHC_OS_WINDOWS
+}
+
+GHC_INLINE int compare_simple_insensitive(const path::value_type* str1, size_t len1, const path::value_type* str2, size_t len2)
+{
+    while (len1 > 0 && len2 > 0 && ::tolower(static_cast<unsigned char>(*str1)) == ::tolower(static_cast<unsigned char>(*str2))) {
+        --len1;
+        --len2;
+        ++str1;
+        ++str2;
+    }
+    if (len1 && len2) {
+        return *str1 < *str2 ? -1 : 1;
+    }
+    if (len1 == 0 && len2 == 0) {
+        return 0;
+    }
+    return len1 == 0 ? -1 : 1;
+}
+
+GHC_INLINE const char* strerror_adapter(char* gnu, char*)
+{
+    return gnu;
+}
+
+GHC_INLINE const char* strerror_adapter(int posix, char* buffer)
+{
+    if (posix) {
+        return "Error in strerror_r!";
+    }
+    return buffer;
+}
+
+template <typename ErrorNumber>
+GHC_INLINE std::string systemErrorText(ErrorNumber code = 0)
+{
+#if defined(GHC_OS_WINDOWS)
+    LPVOID msgBuf;
+    DWORD dw = code ? static_cast<DWORD>(code) : ::GetLastError();
+    FormatMessageW(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, NULL, dw, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPWSTR)&msgBuf, 0, NULL);
+    std::string msg = toUtf8(std::wstring((LPWSTR)msgBuf));
+    LocalFree(msgBuf);
+    return msg;
+#else
+    char buffer[512];
+    return strerror_adapter(strerror_r(code ? code : errno, buffer, sizeof(buffer)), buffer);
+#endif
+}
+
+#ifdef GHC_OS_WINDOWS
+using CreateSymbolicLinkW_fp = BOOLEAN(WINAPI*)(LPCWSTR, LPCWSTR, DWORD);
+using CreateHardLinkW_fp = BOOLEAN(WINAPI*)(LPCWSTR, LPCWSTR, LPSECURITY_ATTRIBUTES);
+
+GHC_INLINE void create_symlink(const path& target_name, const path& new_symlink, bool to_directory, std::error_code& ec)
+{
+    std::error_code tec;
+    auto fs = status(target_name, tec);
+    if ((fs.type() == file_type::directory && !to_directory) || (fs.type() == file_type::regular && to_directory)) {
+        ec = detail::make_error_code(detail::portable_error::not_supported);
+        return;
+    }
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-function-type"
+#endif
+    static CreateSymbolicLinkW_fp api_call = reinterpret_cast<CreateSymbolicLinkW_fp>(GetProcAddress(GetModuleHandleW(L"kernel32.dll"), "CreateSymbolicLinkW"));
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic pop
+#endif
+    if (api_call) {
+        if (api_call(detail::fromUtf8<std::wstring>(new_symlink.u8string()).c_str(), detail::fromUtf8<std::wstring>(target_name.u8string()).c_str(), to_directory ? 1 : 0) == 0) {
+            auto result = ::GetLastError();
+            if (result == ERROR_PRIVILEGE_NOT_HELD && api_call(detail::fromUtf8<std::wstring>(new_symlink.u8string()).c_str(), detail::fromUtf8<std::wstring>(target_name.u8string()).c_str(), to_directory ? 3 : 2) != 0) {
+                return;
+            }
+            ec = detail::make_system_error(result);
+        }
+    }
+    else {
+        ec = detail::make_system_error(ERROR_NOT_SUPPORTED);
+    }
+}
+
+GHC_INLINE void create_hardlink(const path& target_name, const path& new_hardlink, std::error_code& ec)
+{
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wcast-function-type"
+#endif
+    static CreateHardLinkW_fp api_call = reinterpret_cast<CreateHardLinkW_fp>(GetProcAddress(GetModuleHandleW(L"kernel32.dll"), "CreateHardLinkW"));
+#if defined(__GNUC__) && __GNUC__ >= 8
+#pragma GCC diagnostic pop
+#endif
+    if (api_call) {
+        if (api_call(GHC_NATIVEWP(new_hardlink), GHC_NATIVEWP(target_name), NULL) == 0) {
+            ec = detail::make_system_error();
+        }
+    }
+    else {
+        ec = detail::make_system_error(ERROR_NOT_SUPPORTED);
+    }
+}
+
+GHC_INLINE path getFullPathName(const wchar_t* p, std::error_code& ec)
+{
+    ULONG size = ::GetFullPathNameW(p, 0, 0, 0);
+    if (size) {
+        std::vector<wchar_t> buf(size, 0);
+        ULONG s2 = GetFullPathNameW(p, size, buf.data(), nullptr);
+        if (s2 && s2 < size) {
+            return path(std::wstring(buf.data(), s2));
+        }
+    }
+    ec = detail::make_system_error();
+    return path();
+}
+
+#else
+GHC_INLINE void create_symlink(const path& target_name, const path& new_symlink, bool, std::error_code& ec)
+{
+    if (::symlink(target_name.c_str(), new_symlink.c_str()) != 0) {
+        ec = detail::make_system_error();
+    }
+}
+
+#ifndef GHC_OS_WEB
+GHC_INLINE void create_hardlink(const path& target_name, const path& new_hardlink, std::error_code& ec)
+{
+    if (::link(target_name.c_str(), new_hardlink.c_str()) != 0) {
+        ec = detail::make_system_error();
+    }
+}
+#endif
+#endif
+
+template <typename T>
+GHC_INLINE file_status file_status_from_st_mode(T mode)
+{
+#ifdef GHC_OS_WINDOWS
+    file_type ft = file_type::unknown;
+    if ((mode & _S_IFDIR) == _S_IFDIR) {
+        ft = file_type::directory;
+    }
+    else if ((mode & _S_IFREG) == _S_IFREG) {
+        ft = file_type::regular;
+    }
+    else if ((mode & _S_IFCHR) == _S_IFCHR) {
+        ft = file_type::character;
+    }
+    perms prms = static_cast<perms>(mode & 0xfff);
+    return file_status(ft, prms);
+#else
+    file_type ft = file_type::unknown;
+    if (S_ISDIR(mode)) {
+        ft = file_type::directory;
+    }
+    else if (S_ISREG(mode)) {
+        ft = file_type::regular;
+    }
+    else if (S_ISCHR(mode)) {
+        ft = file_type::character;
+    }
+    else if (S_ISBLK(mode)) {
+        ft = file_type::block;
+    }
+    else if (S_ISFIFO(mode)) {
+        ft = file_type::fifo;
+    }
+    else if (S_ISLNK(mode)) {
+        ft = file_type::symlink;
+    }
+    else if (S_ISSOCK(mode)) {
+        ft = file_type::socket;
+    }
+    perms prms = static_cast<perms>(mode & 0xfff);
+    return file_status(ft, prms);
+#endif
+}
+
+#ifdef GHC_OS_WINDOWS
+#ifndef REPARSE_DATA_BUFFER_HEADER_SIZE
+typedef struct _REPARSE_DATA_BUFFER
+{
+    ULONG ReparseTag;
+    USHORT ReparseDataLength;
+    USHORT Reserved;
+    union
+    {
+        struct
+        {
+            USHORT SubstituteNameOffset;
+            USHORT SubstituteNameLength;
+            USHORT PrintNameOffset;
+            USHORT PrintNameLength;
+            ULONG Flags;
+            WCHAR PathBuffer[1];
+        } SymbolicLinkReparseBuffer;
+        struct
+        {
+            USHORT SubstituteNameOffset;
+            USHORT SubstituteNameLength;
+            USHORT PrintNameOffset;
+            USHORT PrintNameLength;
+            WCHAR PathBuffer[1];
+        } MountPointReparseBuffer;
+        struct
+        {
+            UCHAR DataBuffer[1];
+        } GenericReparseBuffer;
+    } DUMMYUNIONNAME;
+} REPARSE_DATA_BUFFER;
+#ifndef MAXIMUM_REPARSE_DATA_BUFFER_SIZE
+#define MAXIMUM_REPARSE_DATA_BUFFER_SIZE (16 * 1024)
+#endif
+#endif
+
+GHC_INLINE std::shared_ptr<REPARSE_DATA_BUFFER> getReparseData(const path& p, std::error_code& ec)
+{
+    std::shared_ptr<void> file(CreateFileW(GHC_NATIVEWP(p), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_OPEN_REPARSE_POINT | FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle);
+    if (file.get() == INVALID_HANDLE_VALUE) {
+        ec = detail::make_system_error();
+        return nullptr;
+    }
+
+    std::shared_ptr<REPARSE_DATA_BUFFER> reparseData((REPARSE_DATA_BUFFER*)std::calloc(1, MAXIMUM_REPARSE_DATA_BUFFER_SIZE), std::free);
+    ULONG bufferUsed;
+    if (DeviceIoControl(file.get(), FSCTL_GET_REPARSE_POINT, 0, 0, reparseData.get(), MAXIMUM_REPARSE_DATA_BUFFER_SIZE, &bufferUsed, 0)) {
+        return reparseData;
+    }
+    else {
+        ec = detail::make_system_error();
+    }
+    return nullptr;
+}
+#endif
+
+GHC_INLINE path resolveSymlink(const path& p, std::error_code& ec)
+{
+#ifdef GHC_OS_WINDOWS
+    path result;
+    auto reparseData = detail::getReparseData(p, ec);
+    if (!ec) {
+        if (reparseData && IsReparseTagMicrosoft(reparseData->ReparseTag)) {
+            switch (reparseData->ReparseTag) {
+                case IO_REPARSE_TAG_SYMLINK: {
+                    auto printName = std::wstring(&reparseData->SymbolicLinkReparseBuffer.PathBuffer[reparseData->SymbolicLinkReparseBuffer.PrintNameOffset / sizeof(WCHAR)], reparseData->SymbolicLinkReparseBuffer.PrintNameLength / sizeof(WCHAR));
+                    auto substituteName =
+                        std::wstring(&reparseData->SymbolicLinkReparseBuffer.PathBuffer[reparseData->SymbolicLinkReparseBuffer.SubstituteNameOffset / sizeof(WCHAR)], reparseData->SymbolicLinkReparseBuffer.SubstituteNameLength / sizeof(WCHAR));
+                    if (detail::endsWith(substituteName, printName) && detail::startsWith(substituteName, std::wstring(L"\\??\\"))) {
+                        result = printName;
+                    }
+                    else {
+                        result = substituteName;
+                    }
+                    if (reparseData->SymbolicLinkReparseBuffer.Flags & 0x1 /*SYMLINK_FLAG_RELATIVE*/) {
+                        result = p.parent_path() / result;
+                    }
+                    break;
+                }
+                case IO_REPARSE_TAG_MOUNT_POINT:
+                    result = detail::getFullPathName(GHC_NATIVEWP(p), ec);
+                    //result = std::wstring(&reparseData->MountPointReparseBuffer.PathBuffer[reparseData->MountPointReparseBuffer.SubstituteNameOffset / sizeof(WCHAR)], reparseData->MountPointReparseBuffer.SubstituteNameLength / sizeof(WCHAR));
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+    return result;
+#else
+    size_t bufferSize = 256;
+    while (true) {
+        std::vector<char> buffer(bufferSize, static_cast<char>(0));
+        auto rc = ::readlink(p.c_str(), buffer.data(), buffer.size());
+        if (rc < 0) {
+            ec = detail::make_system_error();
+            return path();
+        }
+        else if (rc < static_cast<int>(bufferSize)) {
+            return path(std::string(buffer.data(), static_cast<std::string::size_type>(rc)));
+        }
+        bufferSize *= 2;
+    }
+    return path();
+#endif
+}
+
+#ifdef GHC_OS_WINDOWS
+GHC_INLINE time_t timeFromFILETIME(const FILETIME& ft)
+{
+    ULARGE_INTEGER ull;
+    ull.LowPart = ft.dwLowDateTime;
+    ull.HighPart = ft.dwHighDateTime;
+    return static_cast<time_t>(ull.QuadPart / 10000000ULL - 11644473600ULL);
+}
+
+GHC_INLINE void timeToFILETIME(time_t t, FILETIME& ft)
+{
+    LONGLONG ll;
+    ll = Int32x32To64(t, 10000000) + 116444736000000000;
+    ft.dwLowDateTime = static_cast<DWORD>(ll);
+    ft.dwHighDateTime = static_cast<DWORD>(ll >> 32);
+}
+
+template <typename INFO>
+GHC_INLINE uintmax_t hard_links_from_INFO(const INFO* info)
+{
+    return static_cast<uintmax_t>(-1);
+}
+
+template <>
+GHC_INLINE uintmax_t hard_links_from_INFO<BY_HANDLE_FILE_INFORMATION>(const BY_HANDLE_FILE_INFORMATION* info)
+{
+    return info->nNumberOfLinks;
+}
+
+template <typename INFO>
+GHC_INLINE DWORD reparse_tag_from_INFO(const INFO*)
+{
+    return 0;
+}
+
+template <>
+GHC_INLINE DWORD reparse_tag_from_INFO(const WIN32_FIND_DATAW* info)
+{
+    return info->dwReserved0;
+}
+
+template <typename INFO>
+GHC_INLINE file_status status_from_INFO(const path& p, const INFO* info, std::error_code& ec, uintmax_t* sz = nullptr, time_t* lwt = nullptr)
+{
+    file_type ft = file_type::unknown;
+    if (sizeof(INFO) == sizeof(WIN32_FIND_DATAW)) {
+        if (detail::reparse_tag_from_INFO(info) == IO_REPARSE_TAG_SYMLINK) {
+            ft = file_type::symlink;
+        }
+    }
+    else {
+        if ((info->dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT)) {
+            auto reparseData = detail::getReparseData(p, ec);
+            if (!ec && reparseData && IsReparseTagMicrosoft(reparseData->ReparseTag) && reparseData->ReparseTag == IO_REPARSE_TAG_SYMLINK) {
+                ft = file_type::symlink;
+            }
+        }
+    }
+    if (ft == file_type::unknown) {
+        if ((info->dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)) {
+            ft = file_type::directory;
+        }
+        else {
+            ft = file_type::regular;
+        }
+    }
+    perms prms = perms::owner_read | perms::group_read | perms::others_read;
+    if (!(info->dwFileAttributes & FILE_ATTRIBUTE_READONLY)) {
+        prms = prms | perms::owner_write | perms::group_write | perms::others_write;
+    }
+    if (has_executable_extension(p)) {
+        prms = prms | perms::owner_exec | perms::group_exec | perms::others_exec;
+    }
+    if (sz) {
+        *sz = static_cast<uintmax_t>(info->nFileSizeHigh) << (sizeof(info->nFileSizeHigh) * 8) | info->nFileSizeLow;
+    }
+    if (lwt) {
+        *lwt = detail::timeFromFILETIME(info->ftLastWriteTime);
+    }
+    return file_status(ft, prms);
+}
+
+#endif
+
+GHC_INLINE bool is_not_found_error(std::error_code& ec)
+{
+#ifdef GHC_OS_WINDOWS
+    return ec.value() == ERROR_FILE_NOT_FOUND || ec.value() == ERROR_PATH_NOT_FOUND || ec.value() == ERROR_INVALID_NAME;
+#else
+    return ec.value() == ENOENT || ec.value() == ENOTDIR;
+#endif
+}
+
+GHC_INLINE file_status symlink_status_ex(const path& p, std::error_code& ec, uintmax_t* sz = nullptr, uintmax_t* nhl = nullptr, time_t* lwt = nullptr) noexcept
+{
+#ifdef GHC_OS_WINDOWS
+    file_status fs;
+    WIN32_FILE_ATTRIBUTE_DATA attr;
+    if (!GetFileAttributesExW(GHC_NATIVEWP(p), GetFileExInfoStandard, &attr)) {
+        ec = detail::make_system_error();
+    }
+    else {
+        ec.clear();
+        fs = detail::status_from_INFO(p, &attr, ec, sz, lwt);
+        if (nhl) {
+            *nhl = 0;
+        }
+    }
+    if (detail::is_not_found_error(ec)) {
+        return file_status(file_type::not_found);
+    }
+    return ec ? file_status(file_type::none) : fs;
+#else
+    (void)sz;
+    (void)nhl;
+    (void)lwt;
+    struct ::stat fs;
+    auto result = ::lstat(p.c_str(), &fs);
+    if (result == 0) {
+        ec.clear();
+        file_status f_s = detail::file_status_from_st_mode(fs.st_mode);
+        return f_s;
+    }
+    ec = detail::make_system_error();
+    if (detail::is_not_found_error(ec)) {
+        return file_status(file_type::not_found, perms::unknown);
+    }
+    return file_status(file_type::none);
+#endif
+}
+
+GHC_INLINE file_status status_ex(const path& p, std::error_code& ec, file_status* sls = nullptr, uintmax_t* sz = nullptr, uintmax_t* nhl = nullptr, time_t* lwt = nullptr, int recurse_count = 0) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    if (recurse_count > 16) {
+        ec = detail::make_system_error(0x2A9 /*ERROR_STOPPED_ON_SYMLINK*/);
+        return file_status(file_type::unknown);
+    }
+    WIN32_FILE_ATTRIBUTE_DATA attr;
+    if (!::GetFileAttributesExW(GHC_NATIVEWP(p), GetFileExInfoStandard, &attr)) {
+        ec = detail::make_system_error();
+    }
+    else if (attr.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) {
+        auto reparseData = detail::getReparseData(p, ec);
+        if (!ec && reparseData && IsReparseTagMicrosoft(reparseData->ReparseTag) && reparseData->ReparseTag == IO_REPARSE_TAG_SYMLINK) {
+            path target = resolveSymlink(p, ec);
+            file_status result;
+            if (!ec && !target.empty()) {
+                if (sls) {
+                    *sls = status_from_INFO(p, &attr, ec);
+                }
+                return detail::status_ex(target, ec, nullptr, sz, nhl, lwt, recurse_count + 1);
+            }
+            return file_status(file_type::unknown);
+        }
+    }
+    if (ec) {
+        if (detail::is_not_found_error(ec)) {
+            return file_status(file_type::not_found);
+        }
+        return file_status(file_type::none);
+    }
+    if (nhl) {
+        *nhl = 0;
+    }
+    return detail::status_from_INFO(p, &attr, ec, sz, lwt);
+#else
+    (void)recurse_count;
+    struct ::stat st;
+    auto result = ::lstat(p.c_str(), &st);
+    if (result == 0) {
+        ec.clear();
+        file_status fs = detail::file_status_from_st_mode(st.st_mode);
+        if (sls) {
+            *sls = fs;
+        }
+        if (fs.type() == file_type::symlink) {
+            result = ::stat(p.c_str(), &st);
+            if (result == 0) {
+                fs = detail::file_status_from_st_mode(st.st_mode);
+            }
+            else {
+                ec = detail::make_system_error();
+                if (detail::is_not_found_error(ec)) {
+                    return file_status(file_type::not_found, perms::unknown);
+                }
+                return file_status(file_type::none);
+            }
+        }
+        if (sz) {
+            *sz = static_cast<uintmax_t>(st.st_size);
+        }
+        if (nhl) {
+            *nhl = st.st_nlink;
+        }
+        if (lwt) {
+            *lwt = st.st_mtime;
+        }
+        return fs;
+    }
+    else {
+        ec = detail::make_system_error();
+        if (detail::is_not_found_error(ec)) {
+            return file_status(file_type::not_found, perms::unknown);
+        }
+        return file_status(file_type::none);
+    }
+#endif
+}
+
+}  // namespace detail
+
+GHC_INLINE u8arguments::u8arguments(int& argc, char**& argv)
+    : _argc(argc)
+    , _argv(argv)
+    , _refargc(argc)
+    , _refargv(argv)
+    , _isvalid(false)
+{
+#ifdef GHC_OS_WINDOWS
+    LPWSTR* p;
+    p = ::CommandLineToArgvW(::GetCommandLineW(), &argc);
+    _args.reserve(static_cast<size_t>(argc));
+    _argp.reserve(static_cast<size_t>(argc));
+    for (size_t i = 0; i < static_cast<size_t>(argc); ++i) {
+        _args.push_back(detail::toUtf8(std::wstring(p[i])));
+        _argp.push_back((char*)_args[i].data());
+    }
+    argv = _argp.data();
+    ::LocalFree(p);
+    _isvalid = true;
+#else
+    std::setlocale(LC_ALL, "");
+#if defined(__ANDROID__) && __ANDROID_API__ < 26
+    _isvalid = true;
+#else
+    if (detail::equals_simple_insensitive(::nl_langinfo(CODESET), "UTF-8")) {
+        _isvalid = true;
+    }
+#endif
+#endif
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.construct] constructors and destructor
+
+GHC_INLINE path::path() noexcept {}
+
+GHC_INLINE path::path(const path& p)
+    : _path(p._path)
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    , _prefixLength(p._prefixLength)
+#endif
+{
+}
+
+GHC_INLINE path::path(path&& p) noexcept
+    : _path(std::move(p._path))
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    , _prefixLength(p._prefixLength)
+#endif
+{
+}
+
+GHC_INLINE path::path(string_type&& source, format fmt)
+    : _path(std::move(source))
+{
+    postprocess_path_with_format(fmt);
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+#ifdef GHC_WITH_EXCEPTIONS
+template <class Source, typename>
+inline path::path(const Source& source, const std::locale& loc, format fmt)
+    : path(source, fmt)
+{
+    std::string locName = loc.name();
+    if (!(locName.length() >= 5 && (locName.substr(locName.length() - 5) == "UTF-8" || locName.substr(locName.length() - 5) == "utf-8"))) {
+        throw filesystem_error("This implementation only supports UTF-8 locales!", path(_path), detail::make_error_code(detail::portable_error::not_supported));
+    }
+}
+
+template <class InputIterator>
+inline path::path(InputIterator first, InputIterator last, const std::locale& loc, format fmt)
+    : path(std::basic_string<typename std::iterator_traits<InputIterator>::value_type>(first, last), fmt)
+{
+    std::string locName = loc.name();
+    if (!(locName.length() >= 5 && (locName.substr(locName.length() - 5) == "UTF-8" || locName.substr(locName.length() - 5) == "utf-8"))) {
+        throw filesystem_error("This implementation only supports UTF-8 locales!", path(_path), detail::make_error_code(detail::portable_error::not_supported));
+    }
+}
+#endif
+
+#ifdef GHC_EXPAND_IMPL
+
+GHC_INLINE path::~path() {}
+
+//-----------------------------------------------------------------------------
+// [fs.path.assign] assignments
+
+GHC_INLINE path& path::operator=(const path& p)
+{
+    _path = p._path;
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    _prefixLength = p._prefixLength;
+#endif
+    return *this;
+}
+
+GHC_INLINE path& path::operator=(path&& p) noexcept
+{
+    _path = std::move(p._path);
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    _prefixLength = p._prefixLength;
+#endif
+    return *this;
+}
+
+GHC_INLINE path& path::operator=(path::string_type&& source)
+{
+    return assign(source);
+}
+
+GHC_INLINE path& path::assign(path::string_type&& source)
+{
+    _path = std::move(source);
+    postprocess_path_with_format(native_format);
+    return *this;
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+template <class Source>
+inline path& path::operator=(const Source& source)
+{
+    return assign(source);
+}
+
+template <class Source>
+inline path& path::assign(const Source& source)
+{
+#ifdef GHC_USE_WCHAR_T
+    _path.assign(detail::toWChar(source));
+#else
+    _path.assign(detail::toUtf8(source));
+#endif
+    postprocess_path_with_format(native_format);
+    return *this;
+}
+
+template <>
+inline path& path::assign<path>(const path& source)
+{
+    _path = source._path;
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    _prefixLength = source._prefixLength;
+#endif
+    return *this;
+}
+
+template <class InputIterator>
+inline path& path::assign(InputIterator first, InputIterator last)
+{
+    _path.assign(first, last);
+    postprocess_path_with_format(native_format);
+    return *this;
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+//-----------------------------------------------------------------------------
+// [fs.path.append] appends
+
+GHC_INLINE path& path::operator/=(const path& p)
+{
+    if (p.empty()) {
+        // was: if ((!has_root_directory() && is_absolute()) || has_filename())
+        if (!_path.empty() && _path[_path.length() - 1] != preferred_separator && _path[_path.length() - 1] != ':') {
+            _path += preferred_separator;
+        }
+        return *this;
+    }
+    if ((p.is_absolute() && (_path != root_name()._path || p._path != "/")) || (p.has_root_name() && p.root_name() != root_name())) {
+        assign(p);
+        return *this;
+    }
+    if (p.has_root_directory()) {
+        assign(root_name());
+    }
+    else if ((!has_root_directory() && is_absolute()) || has_filename()) {
+        _path += preferred_separator;
+    }
+    auto iter = p.begin();
+    bool first = true;
+    if (p.has_root_name()) {
+        ++iter;
+    }
+    while (iter != p.end()) {
+        if (!first && !(!_path.empty() && _path[_path.length() - 1] == preferred_separator)) {
+            _path += preferred_separator;
+        }
+        first = false;
+        _path += (*iter++).native();
+    }
+    check_long_path();
+    return *this;
+}
+
+GHC_INLINE void path::append_name(const value_type* name)
+{
+    if (_path.empty()) {
+        this->operator/=(path(name));
+    }
+    else {
+        if (_path.back() != path::preferred_separator) {
+            _path.push_back(path::preferred_separator);
+        }
+        _path += name;
+        check_long_path();
+    }
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+template <class Source>
+inline path& path::operator/=(const Source& source)
+{
+    return append(source);
+}
+
+template <class Source>
+inline path& path::append(const Source& source)
+{
+    return this->operator/=(path(source));
+}
+
+template <>
+inline path& path::append<path>(const path& p)
+{
+    return this->operator/=(p);
+}
+
+template <class InputIterator>
+inline path& path::append(InputIterator first, InputIterator last)
+{
+    std::basic_string<typename std::iterator_traits<InputIterator>::value_type> part(first, last);
+    return append(part);
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+//-----------------------------------------------------------------------------
+// [fs.path.concat] concatenation
+
+GHC_INLINE path& path::operator+=(const path& x)
+{
+    return concat(x._path);
+}
+
+GHC_INLINE path& path::operator+=(const string_type& x)
+{
+    return concat(x);
+}
+
+#ifdef GHC_WITH_STRING_VIEW
+GHC_INLINE path& path::operator+=(basic_string_view<value_type> x)
+{
+    return concat(x);
+}
+#endif
+
+GHC_INLINE path& path::operator+=(const value_type* x)
+{
+#ifdef GHC_WITH_STRING_VIEW
+    basic_string_view<value_type> part(x);
+#else
+    string_type part(x);
+#endif
+    return concat(part);
+}
+
+GHC_INLINE path& path::operator+=(value_type x)
+{
+#ifdef GHC_OS_WINDOWS
+    if (x == generic_separator) {
+        x = preferred_separator;
+    }
+#endif
+    if (_path.empty() || _path.back() != preferred_separator) {
+        _path += x;
+    }
+    check_long_path();
+    return *this;
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+template <class Source>
+inline path::path_from_string<Source>& path::operator+=(const Source& x)
+{
+    return concat(x);
+}
+
+template <class EcharT>
+inline path::path_type_EcharT<EcharT>& path::operator+=(EcharT x)
+{
+#ifdef GHC_WITH_STRING_VIEW
+    basic_string_view<EcharT> part(&x, 1);
+#else
+    std::basic_string<EcharT> part(1, x);
+#endif
+    concat(part);
+    return *this;
+}
+
+template <class Source>
+inline path& path::concat(const Source& x)
+{
+    path p(x);
+    _path += p._path;
+    postprocess_path_with_format(native_format);
+    return *this;
+}
+template <class InputIterator>
+inline path& path::concat(InputIterator first, InputIterator last)
+{
+    _path.append(first, last);
+    postprocess_path_with_format(native_format);
+    return *this;
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+//-----------------------------------------------------------------------------
+// [fs.path.modifiers] modifiers
+GHC_INLINE void path::clear() noexcept
+{
+    _path.clear();
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    _prefixLength = 0;
+#endif
+}
+
+GHC_INLINE path& path::make_preferred()
+{
+    // as this filesystem implementation only uses generic_format
+    // internally, this must be a no-op
+    return *this;
+}
+
+GHC_INLINE path& path::remove_filename()
+{
+    if (has_filename()) {
+        _path.erase(_path.size() - filename()._path.size());
+    }
+    return *this;
+}
+
+GHC_INLINE path& path::replace_filename(const path& replacement)
+{
+    remove_filename();
+    return append(replacement);
+}
+
+GHC_INLINE path& path::replace_extension(const path& replacement)
+{
+    if (has_extension()) {
+        _path.erase(_path.size() - extension()._path.size());
+    }
+    if (!replacement.empty() && replacement._path[0] != '.') {
+        _path += '.';
+    }
+    return concat(replacement);
+}
+
+GHC_INLINE void path::swap(path& rhs) noexcept
+{
+    _path.swap(rhs._path);
+#if defined(GHC_OS_WINDOWS) && defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    std::swap(_prefixLength, rhs._prefixLength);
+#endif
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.native.obs] native format observers
+GHC_INLINE const path::string_type& path::native() const noexcept
+{
+    return _path;
+}
+
+GHC_INLINE const path::value_type* path::c_str() const noexcept
+{
+    return native().c_str();
+}
+
+GHC_INLINE path::operator path::string_type() const
+{
+    return native();
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+template <class EcharT, class traits, class Allocator>
+inline std::basic_string<EcharT, traits, Allocator> path::string(const Allocator& a) const
+{
+#ifdef GHC_USE_WCHAR_T
+    return detail::fromWChar<std::basic_string<EcharT, traits, Allocator>>(_path, a);
+#else
+    return detail::fromUtf8<std::basic_string<EcharT, traits, Allocator>>(_path, a);
+#endif
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+GHC_INLINE std::string path::string() const
+{
+#ifdef GHC_USE_WCHAR_T
+    return detail::toUtf8(native());
+#else
+    return native();
+#endif
+}
+
+GHC_INLINE std::wstring path::wstring() const
+{
+#ifdef GHC_USE_WCHAR_T
+    return native();
+#else
+    return detail::fromUtf8<std::wstring>(native());
+#endif
+}
+
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+GHC_INLINE std::u8string path::u8string() const
+{
+#ifdef GHC_USE_WCHAR_T
+    return std::u8string(reinterpret_cast<const char8_t*>(detail::toUtf8(native()).c_str()));
+#else
+    return std::u8string(reinterpret_cast<const char8_t*>(c_str()));
+#endif
+}
+#else
+GHC_INLINE std::string path::u8string() const
+{
+#ifdef GHC_USE_WCHAR_T
+    return detail::toUtf8(native());
+#else
+    return native();
+#endif
+}
+#endif
+
+GHC_INLINE std::u16string path::u16string() const
+{
+    // TODO: optimize
+    return detail::fromUtf8<std::u16string>(string());
+}
+
+GHC_INLINE std::u32string path::u32string() const
+{
+    // TODO: optimize
+    return detail::fromUtf8<std::u32string>(string());
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+//-----------------------------------------------------------------------------
+// [fs.path.generic.obs] generic format observers
+template <class EcharT, class traits, class Allocator>
+inline std::basic_string<EcharT, traits, Allocator> path::generic_string(const Allocator& a) const
+{
+#ifdef GHC_OS_WINDOWS
+#ifdef GHC_USE_WCHAR_T
+    auto result = detail::fromWChar<std::basic_string<EcharT, traits, Allocator>, path::string_type>(_path, a);
+#else
+    auto result = detail::fromUtf8<std::basic_string<EcharT, traits, Allocator>>(_path, a);
+#endif
+    for (auto& c : result) {
+        if (c == preferred_separator) {
+            c = generic_separator;
+        }
+    }
+    return result;
+#else
+    return detail::fromUtf8<std::basic_string<EcharT, traits, Allocator>>(_path, a);
+#endif
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+GHC_INLINE std::string path::generic_string() const
+{
+#ifdef GHC_OS_WINDOWS
+    return generic_string<std::string::value_type, std::string::traits_type, std::string::allocator_type>();
+#else
+    return _path;
+#endif
+}
+
+GHC_INLINE std::wstring path::generic_wstring() const
+{
+#ifdef GHC_OS_WINDOWS
+    return generic_string<std::wstring::value_type, std::wstring::traits_type, std::wstring::allocator_type>();
+#else
+    return detail::fromUtf8<std::wstring>(_path);
+#endif
+}  // namespace filesystem
+
+#if defined(__cpp_lib_char8_t) && !defined(GHC_FILESYSTEM_ENFORCE_CPP17_API)
+GHC_INLINE std::u8string path::generic_u8string() const
+{
+#ifdef GHC_OS_WINDOWS
+    return generic_string<std::u8string::value_type, std::u8string::traits_type, std::u8string::allocator_type>();
+#else
+    return std::u8string(reinterpret_cast<const char8_t*>(_path.c_str()));
+#endif
+}
+#else
+GHC_INLINE std::string path::generic_u8string() const
+{
+#ifdef GHC_OS_WINDOWS
+    return generic_string<std::string::value_type, std::string::traits_type, std::string::allocator_type>();
+#else
+    return _path;
+#endif
+}
+#endif
+
+GHC_INLINE std::u16string path::generic_u16string() const
+{
+#ifdef GHC_OS_WINDOWS
+    return generic_string<std::u16string::value_type, std::u16string::traits_type, std::u16string::allocator_type>();
+#else
+    return detail::fromUtf8<std::u16string>(_path);
+#endif
+}
+
+GHC_INLINE std::u32string path::generic_u32string() const
+{
+#ifdef GHC_OS_WINDOWS
+    return generic_string<std::u32string::value_type, std::u32string::traits_type, std::u32string::allocator_type>();
+#else
+    return detail::fromUtf8<std::u32string>(_path);
+#endif
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.compare] compare
+GHC_INLINE int path::compare(const path& p) const noexcept
+{
+#ifdef LWG_2936_BEHAVIOUR
+    auto rnl1 = root_name_length();
+    auto rnl2 = p.root_name_length();
+#ifdef GHC_OS_WINDOWS
+    auto rnc = detail::compare_simple_insensitive(_path.c_str(), rnl1, p._path.c_str(), rnl2);
+#else
+    auto rnc = _path.compare(0, rnl1, p._path, 0, (std::min(rnl1, rnl2)));
+#endif
+    if (rnc) {
+        return rnc;
+    }
+    bool hrd1 = has_root_directory(), hrd2 = p.has_root_directory();
+    if (hrd1 != hrd2) {
+        return hrd1 ? 1 : -1;
+    }
+    if (hrd1) {
+        ++rnl1;
+        ++rnl2;
+    }
+    auto iter1 = _path.begin() + static_cast<int>(rnl1);
+    auto iter2 = p._path.begin() + static_cast<int>(rnl2);
+    while (iter1 != _path.end() && iter2 != p._path.end() && *iter1 == *iter2) {
+        ++iter1;
+        ++iter2;
+    }
+    if (iter1 == _path.end()) {
+        return iter2 == p._path.end() ? 0 : -1;
+    }
+    if (iter2 == p._path.end()) {
+        return 1;
+    }
+    if (*iter1 == preferred_separator) {
+        return -1;
+    }
+    if (*iter2 == preferred_separator) {
+        return 1;
+    }
+    return *iter1 < *iter2 ? -1 : 1;
+#else  // LWG_2936_BEHAVIOUR
+#ifdef GHC_OS_WINDOWS
+    auto rnl1 = root_name_length();
+    auto rnl2 = p.root_name_length();
+    auto rnc = detail::compare_simple_insensitive(_path.c_str(), rnl1, p._path.c_str(), rnl2);
+    if (rnc) {
+        return rnc;
+    }
+    return _path.compare(rnl1, std::string::npos, p._path, rnl2, std::string::npos);
+#else
+    return _path.compare(p._path);
+#endif
+#endif
+}
+
+GHC_INLINE int path::compare(const string_type& s) const
+{
+    return compare(path(s));
+}
+
+#ifdef GHC_WITH_STRING_VIEW
+GHC_INLINE int path::compare(basic_string_view<value_type> s) const
+{
+    return compare(path(s));
+}
+#endif
+
+GHC_INLINE int path::compare(const value_type* s) const
+{
+    return compare(path(s));
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.decompose] decomposition
+#ifdef GHC_OS_WINDOWS
+GHC_INLINE void path::handle_prefixes()
+{
+#if defined(GHC_WIN_AUTO_PREFIX_LONG_PATH)
+    _prefixLength = 0;
+    if (_path.length() >= 6 && _path[2] == '?' && std::toupper(static_cast<unsigned char>(_path[4])) >= 'A' && std::toupper(static_cast<unsigned char>(_path[4])) <= 'Z' && _path[5] == ':') {
+        if (detail::startsWith(_path, impl_string_type(GHC_PLATFORM_LITERAL("\\\\?\\"))) || detail::startsWith(_path, impl_string_type(GHC_PLATFORM_LITERAL("\\??\\")))) {
+            _prefixLength = 4;
+        }
+    }
+#endif  // GHC_WIN_AUTO_PREFIX_LONG_PATH
+}
+#endif
+
+GHC_INLINE path::string_type::size_type path::root_name_length() const noexcept
+{
+#ifdef GHC_OS_WINDOWS
+    if (_path.length() >= _prefixLength + 2 && std::toupper(static_cast<unsigned char>(_path[_prefixLength])) >= 'A' && std::toupper(static_cast<unsigned char>(_path[_prefixLength])) <= 'Z' && _path[_prefixLength + 1] == ':') {
+        return 2;
+    }
+#endif
+    if (_path.length() > _prefixLength + 2 && _path[_prefixLength] == preferred_separator && _path[_prefixLength + 1] == preferred_separator && _path[_prefixLength + 2] != preferred_separator && std::isprint(_path[_prefixLength + 2])) {
+        impl_string_type::size_type pos = _path.find(preferred_separator, _prefixLength + 3);
+        if (pos == impl_string_type::npos) {
+            return _path.length();
+        }
+        else {
+            return pos;
+        }
+    }
+    return 0;
+}
+
+GHC_INLINE path path::root_name() const
+{
+    return path(_path.substr(_prefixLength, root_name_length()), native_format);
+}
+
+GHC_INLINE path path::root_directory() const
+{
+    if (has_root_directory()) {
+        static const path _root_dir(std::string(1, preferred_separator), native_format);
+        return _root_dir;
+    }
+    return path();
+}
+
+GHC_INLINE path path::root_path() const
+{
+    return path(root_name().string() + root_directory().string(), native_format);
+}
+
+GHC_INLINE path path::relative_path() const
+{
+    auto rootPathLen = _prefixLength + root_name_length() + (has_root_directory() ? 1 : 0);
+    return path(_path.substr((std::min)(rootPathLen, _path.length())), generic_format);
+}
+
+GHC_INLINE path path::parent_path() const
+{
+    auto rootPathLen = _prefixLength + root_name_length() + (has_root_directory() ? 1 : 0);
+    if (rootPathLen < _path.length()) {
+        if (empty()) {
+            return path();
+        }
+        else {
+            auto piter = end();
+            auto iter = piter.decrement(_path.end());
+            if (iter > _path.begin() + static_cast<long>(rootPathLen) && *iter != preferred_separator) {
+                --iter;
+            }
+            return path(_path.begin(), iter, native_format);
+        }
+    }
+    else {
+        return *this;
+    }
+}
+
+GHC_INLINE path path::filename() const
+{
+    return !has_relative_path() ? path() : path(*--end());
+}
+
+GHC_INLINE path path::stem() const
+{
+    impl_string_type fn = filename().native();
+    if (fn != "." && fn != "..") {
+        impl_string_type::size_type pos = fn.rfind('.');
+        if (pos != impl_string_type::npos && pos > 0) {
+            return path{fn.substr(0, pos), native_format};
+        }
+    }
+    return path{fn, native_format};
+}
+
+GHC_INLINE path path::extension() const
+{
+    if (has_relative_path()) {
+        auto iter = end();
+        const auto& fn = *--iter;
+        impl_string_type::size_type pos = fn._path.rfind('.');
+        if (pos != std::string::npos && pos > 0) {
+            return path(fn._path.substr(pos), native_format);
+        }
+    }
+    return path();
+}
+
+#ifdef GHC_OS_WINDOWS
+namespace detail {
+GHC_INLINE bool has_executable_extension(const path& p)
+{
+    if (p.has_relative_path()) {
+        auto iter = p.end();
+        const auto& fn = *--iter;
+        auto pos = fn._path.find_last_of('.');
+        if (pos == std::string::npos || pos == 0 || fn._path.length() - pos != 3) {
+            return false;
+        }
+        const path::value_type* ext = fn._path.c_str() + pos + 1;
+        if (detail::equals_simple_insensitive(ext, GHC_PLATFORM_LITERAL("exe")) || detail::equals_simple_insensitive(ext, GHC_PLATFORM_LITERAL("cmd")) || detail::equals_simple_insensitive(ext, GHC_PLATFORM_LITERAL("bat")) || detail::equals_simple_insensitive(ext, GHC_PLATFORM_LITERAL("com"))) {
+            return true;
+        }
+    }
+    return false;
+}
+}  // namespace detail
+#endif
+
+//-----------------------------------------------------------------------------
+// [fs.path.query] query
+GHC_INLINE bool path::empty() const noexcept
+{
+    return _path.empty();
+}
+
+GHC_INLINE bool path::has_root_name() const
+{
+    return root_name_length() > 0;
+}
+
+GHC_INLINE bool path::has_root_directory() const
+{
+    auto rootLen = _prefixLength + root_name_length();
+    return (_path.length() > rootLen && _path[rootLen] == preferred_separator);
+}
+
+GHC_INLINE bool path::has_root_path() const
+{
+    return has_root_name() || has_root_directory();
+}
+
+GHC_INLINE bool path::has_relative_path() const
+{
+    auto rootPathLen = _prefixLength + root_name_length() + (has_root_directory() ? 1 : 0);
+    return rootPathLen < _path.length();
+}
+
+GHC_INLINE bool path::has_parent_path() const
+{
+    return !parent_path().empty();
+}
+
+GHC_INLINE bool path::has_filename() const
+{
+    return has_relative_path() && !filename().empty();
+}
+
+GHC_INLINE bool path::has_stem() const
+{
+    return !stem().empty();
+}
+
+GHC_INLINE bool path::has_extension() const
+{
+    return !extension().empty();
+}
+
+GHC_INLINE bool path::is_absolute() const
+{
+#ifdef GHC_OS_WINDOWS
+    return has_root_name() && has_root_directory();
+#else
+    return has_root_directory();
+#endif
+}
+
+GHC_INLINE bool path::is_relative() const
+{
+    return !is_absolute();
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.gen] generation
+GHC_INLINE path path::lexically_normal() const
+{
+    path dest;
+    bool lastDotDot = false;
+    for (string_type s : *this) {
+        if (s == ".") {
+            dest /= "";
+            continue;
+        }
+        else if (s == ".." && !dest.empty()) {
+            auto root = root_path();
+            if (dest == root) {
+                continue;
+            }
+            else if (*(--dest.end()) != "..") {
+                if (dest._path.back() == preferred_separator) {
+                    dest._path.pop_back();
+                }
+                dest.remove_filename();
+                continue;
+            }
+        }
+        if (!(s.empty() && lastDotDot)) {
+            dest /= s;
+        }
+        lastDotDot = s == "..";
+    }
+    if (dest.empty()) {
+        dest = ".";
+    }
+    return dest;
+}
+
+GHC_INLINE path path::lexically_relative(const path& base) const
+{
+    if (root_name() != base.root_name() || is_absolute() != base.is_absolute() || (!has_root_directory() && base.has_root_directory())) {
+        return path();
+    }
+    const_iterator a = begin(), b = base.begin();
+    while (a != end() && b != base.end() && *a == *b) {
+        ++a;
+        ++b;
+    }
+    if (a == end() && b == base.end()) {
+        return path(".");
+    }
+    int count = 0;
+    for (const auto& element : input_iterator_range<const_iterator>(b, base.end())) {
+        if (element != "." && element != "" && element != "..") {
+            ++count;
+        }
+        else if (element == "..") {
+            --count;
+        }
+    }
+    if (count < 0) {
+        return path();
+    }
+    path result;
+    for (int i = 0; i < count; ++i) {
+        result /= "..";
+    }
+    for (const auto& element : input_iterator_range<const_iterator>(a, end())) {
+        result /= element;
+    }
+    return result;
+}
+
+GHC_INLINE path path::lexically_proximate(const path& base) const
+{
+    path result = lexically_relative(base);
+    return result.empty() ? *this : result;
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.itr] iterators
+GHC_INLINE path::iterator::iterator() {}
+
+GHC_INLINE path::iterator::iterator(const path& p, const impl_string_type::const_iterator& pos)
+    : _first(p._path.begin())
+    , _last(p._path.end())
+    , _prefix(_first + static_cast<string_type::difference_type>(p._prefixLength))
+    , _root(p.has_root_directory() ? _first + static_cast<string_type::difference_type>(p._prefixLength + p.root_name_length()) : _last)
+    , _iter(pos)
+{
+    if(pos != _last) {
+        updateCurrent();
+    }
+}
+
+GHC_INLINE path::impl_string_type::const_iterator path::iterator::increment(const path::impl_string_type::const_iterator& pos) const
+{
+    path::impl_string_type::const_iterator i = pos;
+    bool fromStart = i == _first || i == _prefix;
+    if (i != _last) {
+        if (fromStart && i == _first && _prefix > _first) {
+            i = _prefix;
+        }
+        else if (*i++ == preferred_separator) {
+            // we can only sit on a slash if it is a network name or a root
+            if (i != _last && *i == preferred_separator) {
+                if (fromStart && !(i + 1 != _last && *(i + 1) == preferred_separator)) {
+                    // leadind double slashes detected, treat this and the
+                    // following until a slash as one unit
+                    i = std::find(++i, _last, preferred_separator);
+                }
+                else {
+                    // skip redundant slashes
+                    while (i != _last && *i == preferred_separator) {
+                        ++i;
+                    }
+                }
+            }
+        }
+        else {
+            if (fromStart && i != _last && *i == ':') {
+                ++i;
+            }
+            else {
+                i = std::find(i, _last, preferred_separator);
+            }
+        }
+    }
+    return i;
+}
+
+GHC_INLINE path::impl_string_type::const_iterator path::iterator::decrement(const path::impl_string_type::const_iterator& pos) const
+{
+    path::impl_string_type::const_iterator i = pos;
+    if (i != _first) {
+        --i;
+        // if this is now the root slash or the trailing slash, we are done,
+        // else check for network name
+        if (i != _root && (pos != _last || *i != preferred_separator)) {
+#ifdef GHC_OS_WINDOWS
+            static const impl_string_type seps = GHC_PLATFORM_LITERAL("\\:");
+            i = std::find_first_of(std::reverse_iterator<path::impl_string_type::const_iterator>(i), std::reverse_iterator<path::impl_string_type::const_iterator>(_first), seps.begin(), seps.end()).base();
+            if (i > _first && *i == ':') {
+                i++;
+            }
+#else
+            i = std::find(std::reverse_iterator<path::impl_string_type::const_iterator>(i), std::reverse_iterator<path::impl_string_type::const_iterator>(_first), preferred_separator).base();
+#endif
+            // Now we have to check if this is a network name
+            if (i - _first == 2 && *_first == preferred_separator && *(_first + 1) == preferred_separator) {
+                i -= 2;
+            }
+        }
+    }
+    return i;
+}
+
+GHC_INLINE void path::iterator::updateCurrent()
+{
+    if ((_iter == _last) || (_iter != _first && _iter != _last && (*_iter == preferred_separator && _iter != _root) && (_iter + 1 == _last))) {
+        _current.clear();
+    }
+    else {
+        _current.assign(_iter, increment(_iter));
+    }
+}
+
+GHC_INLINE path::iterator& path::iterator::operator++()
+{
+    _iter = increment(_iter);
+    while (_iter != _last &&                // we didn't reach the end
+           _iter != _root &&                // this is not a root position
+           *_iter == preferred_separator &&  // we are on a separator
+           (_iter + 1) != _last             // the slash is not the last char
+    ) {
+        ++_iter;
+    }
+    updateCurrent();
+    return *this;
+}
+
+GHC_INLINE path::iterator path::iterator::operator++(int)
+{
+    path::iterator i{*this};
+    ++(*this);
+    return i;
+}
+
+GHC_INLINE path::iterator& path::iterator::operator--()
+{
+    _iter = decrement(_iter);
+    updateCurrent();
+    return *this;
+}
+
+GHC_INLINE path::iterator path::iterator::operator--(int)
+{
+    auto i = *this;
+    --(*this);
+    return i;
+}
+
+GHC_INLINE bool path::iterator::operator==(const path::iterator& other) const
+{
+    return _iter == other._iter;
+}
+
+GHC_INLINE bool path::iterator::operator!=(const path::iterator& other) const
+{
+    return _iter != other._iter;
+}
+
+GHC_INLINE path::iterator::reference path::iterator::operator*() const
+{
+    return _current;
+}
+
+GHC_INLINE path::iterator::pointer path::iterator::operator->() const
+{
+    return &_current;
+}
+
+GHC_INLINE path::iterator path::begin() const
+{
+    return iterator(*this, _path.begin());
+}
+
+GHC_INLINE path::iterator path::end() const
+{
+    return iterator(*this, _path.end());
+}
+
+//-----------------------------------------------------------------------------
+// [fs.path.nonmember] path non-member functions
+GHC_INLINE void swap(path& lhs, path& rhs) noexcept
+{
+    swap(lhs._path, rhs._path);
+}
+
+GHC_INLINE size_t hash_value(const path& p) noexcept
+{
+    return std::hash<std::string>()(p.generic_string());
+}
+
+#ifdef GHC_HAS_THREEWAY_COMP
+GHC_INLINE std::strong_ordering operator<=>(const path& lhs, const path& rhs) noexcept
+{
+    return lhs.compare(rhs) <=> 0;
+}
+#endif
+
+GHC_INLINE bool operator==(const path& lhs, const path& rhs) noexcept
+{
+    return lhs.compare(rhs) == 0;
+}
+
+GHC_INLINE bool operator!=(const path& lhs, const path& rhs) noexcept
+{
+    return !(lhs == rhs);
+}
+
+GHC_INLINE bool operator<(const path& lhs, const path& rhs) noexcept
+{
+    return lhs.compare(rhs) < 0;
+}
+
+GHC_INLINE bool operator<=(const path& lhs, const path& rhs) noexcept
+{
+    return lhs.compare(rhs) <= 0;
+}
+
+GHC_INLINE bool operator>(const path& lhs, const path& rhs) noexcept
+{
+    return lhs.compare(rhs) > 0;
+}
+
+GHC_INLINE bool operator>=(const path& lhs, const path& rhs) noexcept
+{
+    return lhs.compare(rhs) >= 0;
+}
+
+GHC_INLINE path operator/(const path& lhs, const path& rhs)
+{
+    path result(lhs);
+    result /= rhs;
+    return result;
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+//-----------------------------------------------------------------------------
+// [fs.path.io] path inserter and extractor
+template <class charT, class traits>
+inline std::basic_ostream<charT, traits>& operator<<(std::basic_ostream<charT, traits>& os, const path& p)
+{
+    os << "\"";
+    auto ps = p.string<charT, traits>();
+    for (auto c : ps) {
+        if (c == '"' || c == '\\') {
+            os << '\\';
+        }
+        os << c;
+    }
+    os << "\"";
+    return os;
+}
+
+template <class charT, class traits>
+inline std::basic_istream<charT, traits>& operator>>(std::basic_istream<charT, traits>& is, path& p)
+{
+    std::basic_string<charT, traits> tmp;
+    charT c;
+    is >> c;
+    if (c == '"') {
+        auto sf = is.flags();
+        is >> std::noskipws;
+        while (is) {
+            auto c2 = is.get();
+            if (is) {
+                if (c2 == '\\') {
+                    c2 = is.get();
+                    if (is) {
+                        tmp += static_cast<charT>(c2);
+                    }
+                }
+                else if (c2 == '"') {
+                    break;
+                }
+                else {
+                    tmp += static_cast<charT>(c2);
+                }
+            }
+        }
+        if ((sf & std::ios_base::skipws) == std::ios_base::skipws) {
+            is >> std::skipws;
+        }
+        p = path(tmp);
+    }
+    else {
+        is >> tmp;
+        p = path(static_cast<charT>(c) + tmp);
+    }
+    return is;
+}
+
+#ifdef GHC_EXPAND_IMPL
+
+//-----------------------------------------------------------------------------
+// [fs.class.filesystem_error] Class filesystem_error
+GHC_INLINE filesystem_error::filesystem_error(const std::string& what_arg, std::error_code ec)
+    : std::system_error(ec, what_arg)
+    , _what_arg(what_arg)
+    , _ec(ec)
+{
+}
+
+GHC_INLINE filesystem_error::filesystem_error(const std::string& what_arg, const path& p1, std::error_code ec)
+    : std::system_error(ec, what_arg)
+    , _what_arg(what_arg)
+    , _ec(ec)
+    , _p1(p1)
+{
+    if (!_p1.empty()) {
+        _what_arg += ": '" + _p1.string() + "'";
+    }
+}
+
+GHC_INLINE filesystem_error::filesystem_error(const std::string& what_arg, const path& p1, const path& p2, std::error_code ec)
+    : std::system_error(ec, what_arg)
+    , _what_arg(what_arg)
+    , _ec(ec)
+    , _p1(p1)
+    , _p2(p2)
+{
+    if (!_p1.empty()) {
+        _what_arg += ": '" + _p1.string() + "'";
+    }
+    if (!_p2.empty()) {
+        _what_arg += ", '" + _p2.string() + "'";
+    }
+}
+
+GHC_INLINE const path& filesystem_error::path1() const noexcept
+{
+    return _p1;
+}
+
+GHC_INLINE const path& filesystem_error::path2() const noexcept
+{
+    return _p2;
+}
+
+GHC_INLINE const char* filesystem_error::what() const noexcept
+{
+    return _what_arg.c_str();
+}
+
+//-----------------------------------------------------------------------------
+// [fs.op.funcs] filesystem operations
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path absolute(const path& p)
+{
+    std::error_code ec;
+    path result = absolute(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE path absolute(const path& p, std::error_code& ec)
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    if (p.empty()) {
+        return absolute(current_path(ec), ec) / "";
+    }
+    ULONG size = ::GetFullPathNameW(GHC_NATIVEWP(p), 0, 0, 0);
+    if (size) {
+        std::vector<wchar_t> buf(size, 0);
+        ULONG s2 = GetFullPathNameW(GHC_NATIVEWP(p), size, buf.data(), nullptr);
+        if (s2 && s2 < size) {
+            path result = path(std::wstring(buf.data(), s2));
+            if (p.filename() == ".") {
+                result /= ".";
+            }
+            return result;
+        }
+    }
+    ec = detail::make_system_error();
+    return path();
+#else
+    path base = current_path(ec);
+    if (!ec) {
+        if (p.empty()) {
+            return base / p;
+        }
+        if (p.has_root_name()) {
+            if (p.has_root_directory()) {
+                return p;
+            }
+            else {
+                return p.root_name() / base.root_directory() / base.relative_path() / p.relative_path();
+            }
+        }
+        else {
+            if (p.has_root_directory()) {
+                return base.root_name() / p;
+            }
+            else {
+                return base / p;
+            }
+        }
+    }
+    ec = detail::make_system_error();
+    return path();
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path canonical(const path& p)
+{
+    std::error_code ec;
+    auto result = canonical(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE path canonical(const path& p, std::error_code& ec)
+{
+    if (p.empty()) {
+        ec = detail::make_error_code(detail::portable_error::not_found);
+        return path();
+    }
+    path work = p.is_absolute() ? p : absolute(p, ec);
+    path result;
+
+    auto fs = status(work, ec);
+    if (ec) {
+        return path();
+    }
+    if (fs.type() == file_type::not_found) {
+        ec = detail::make_error_code(detail::portable_error::not_found);
+        return path();
+    }
+    bool redo;
+    do {
+        auto rootPathLen = work._prefixLength + work.root_name_length() + (work.has_root_directory() ? 1 : 0);
+        redo = false;
+        result.clear();
+        for (auto pe : work) {
+            if (pe.empty() || pe == ".") {
+                continue;
+            }
+            else if (pe == "..") {
+                result = result.parent_path();
+                continue;
+            }
+            else if ((result / pe).string().length() <= rootPathLen) {
+                result /= pe;
+                continue;
+            }
+            auto sls = symlink_status(result / pe, ec);
+            if (ec) {
+                return path();
+            }
+            if (is_symlink(sls)) {
+                redo = true;
+                auto target = read_symlink(result / pe, ec);
+                if (ec) {
+                    return path();
+                }
+                if (target.is_absolute()) {
+                    result = target;
+                    continue;
+                }
+                else {
+                    result /= target;
+                    continue;
+                }
+            }
+            else {
+                result /= pe;
+            }
+        }
+        work = result;
+    } while (redo);
+    ec.clear();
+    return result;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void copy(const path& from, const path& to)
+{
+    copy(from, to, copy_options::none);
+}
+
+GHC_INLINE void copy(const path& from, const path& to, copy_options options)
+{
+    std::error_code ec;
+    copy(from, to, options, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), from, to, ec);
+    }
+}
+#endif
+
+GHC_INLINE void copy(const path& from, const path& to, std::error_code& ec) noexcept
+{
+    copy(from, to, copy_options::none, ec);
+}
+
+GHC_INLINE void copy(const path& from, const path& to, copy_options options, std::error_code& ec) noexcept
+{
+    std::error_code tec;
+    file_status fs_from, fs_to;
+    ec.clear();
+    if ((options & (copy_options::skip_symlinks | copy_options::copy_symlinks | copy_options::create_symlinks)) != copy_options::none) {
+        fs_from = symlink_status(from, ec);
+    }
+    else {
+        fs_from = status(from, ec);
+    }
+    if (!exists(fs_from)) {
+        if (!ec) {
+            ec = detail::make_error_code(detail::portable_error::not_found);
+        }
+        return;
+    }
+    if ((options & (copy_options::skip_symlinks | copy_options::create_symlinks)) != copy_options::none) {
+        fs_to = symlink_status(to, tec);
+    }
+    else {
+        fs_to = status(to, tec);
+    }
+    if (is_other(fs_from) || is_other(fs_to) || (is_directory(fs_from) && is_regular_file(fs_to)) || (exists(fs_to) && equivalent(from, to, ec))) {
+        ec = detail::make_error_code(detail::portable_error::invalid_argument);
+    }
+    else if (is_symlink(fs_from)) {
+        if ((options & copy_options::skip_symlinks) == copy_options::none) {
+            if (!exists(fs_to) && (options & copy_options::copy_symlinks) != copy_options::none) {
+                copy_symlink(from, to, ec);
+            }
+            else {
+                ec = detail::make_error_code(detail::portable_error::invalid_argument);
+            }
+        }
+    }
+    else if (is_regular_file(fs_from)) {
+        if ((options & copy_options::directories_only) == copy_options::none) {
+            if ((options & copy_options::create_symlinks) != copy_options::none) {
+                create_symlink(from.is_absolute() ? from : canonical(from, ec), to, ec);
+            }
+#ifndef GHC_OS_WEB
+            else if ((options & copy_options::create_hard_links) != copy_options::none) {
+                create_hard_link(from, to, ec);
+            }
+#endif
+            else if (is_directory(fs_to)) {
+                copy_file(from, to / from.filename(), options, ec);
+            }
+            else {
+                copy_file(from, to, options, ec);
+            }
+        }
+    }
+#ifdef LWG_2682_BEHAVIOUR
+    else if (is_directory(fs_from) && (options & copy_options::create_symlinks) != copy_options::none) {
+        ec = detail::make_error_code(detail::portable_error::is_a_directory);
+    }
+#endif
+    else if (is_directory(fs_from) && (options == copy_options::none || (options & copy_options::recursive) != copy_options::none)) {
+        if (!exists(fs_to)) {
+            create_directory(to, from, ec);
+            if (ec) {
+                return;
+            }
+        }
+        for (auto iter = directory_iterator(from, ec); iter != directory_iterator(); iter.increment(ec)) {
+            if (!ec) {
+                copy(iter->path(), to / iter->path().filename(), options | static_cast<copy_options>(0x8000), ec);
+            }
+            if (ec) {
+                return;
+            }
+        }
+    }
+    return;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool copy_file(const path& from, const path& to)
+{
+    return copy_file(from, to, copy_options::none);
+}
+
+GHC_INLINE bool copy_file(const path& from, const path& to, copy_options option)
+{
+    std::error_code ec;
+    auto result = copy_file(from, to, option, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), from, to, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE bool copy_file(const path& from, const path& to, std::error_code& ec) noexcept
+{
+    return copy_file(from, to, copy_options::none, ec);
+}
+
+GHC_INLINE bool copy_file(const path& from, const path& to, copy_options options, std::error_code& ec) noexcept
+{
+    std::error_code tecf, tect;
+    auto sf = status(from, tecf);
+    auto st = status(to, tect);
+    bool overwrite = false;
+    ec.clear();
+    if (!is_regular_file(sf)) {
+        ec = tecf;
+        return false;
+    }
+    if (exists(st) && (!is_regular_file(st) || equivalent(from, to, ec) || (options & (copy_options::skip_existing | copy_options::overwrite_existing | copy_options::update_existing)) == copy_options::none)) {
+        ec = tect ? tect : detail::make_error_code(detail::portable_error::exists);
+        return false;
+    }
+    if (exists(st)) {
+        if ((options & copy_options::update_existing) == copy_options::update_existing) {
+            auto from_time = last_write_time(from, ec);
+            if (ec) {
+                ec = detail::make_system_error();
+                return false;
+            }
+            auto to_time = last_write_time(to, ec);
+            if (ec) {
+                ec = detail::make_system_error();
+                return false;
+            }
+            if (from_time <= to_time) {
+                return false;
+            }
+        }
+        overwrite = true;
+    }
+#ifdef GHC_OS_WINDOWS
+    if (!::CopyFileW(GHC_NATIVEWP(from), GHC_NATIVEWP(to), !overwrite)) {
+        ec = detail::make_system_error();
+        return false;
+    }
+    return true;
+#else
+    std::vector<char> buffer(16384, '\0');
+    int in = -1, out = -1;
+    if ((in = ::open(from.c_str(), O_RDONLY)) < 0) {
+        ec = detail::make_system_error();
+        return false;
+    }
+    int mode = O_CREAT | O_WRONLY | O_TRUNC;
+    if (!overwrite) {
+        mode |= O_EXCL;
+    }
+    if ((out = ::open(to.c_str(), mode, static_cast<int>(sf.permissions() & perms::all))) < 0) {
+        ec = detail::make_system_error();
+        ::close(in);
+        return false;
+    }
+    ssize_t br, bw;
+    while ((br = ::read(in, buffer.data(), buffer.size())) > 0) {
+        ssize_t offset = 0;
+        do {
+            if ((bw = ::write(out, buffer.data() + offset, static_cast<size_t>(br))) > 0) {
+                br -= bw;
+                offset += bw;
+            }
+            else if (bw < 0) {
+                ec = detail::make_system_error();
+                ::close(in);
+                ::close(out);
+                return false;
+            }
+        } while (br);
+    }
+    ::close(in);
+    ::close(out);
+    return true;
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void copy_symlink(const path& existing_symlink, const path& new_symlink)
+{
+    std::error_code ec;
+    copy_symlink(existing_symlink, new_symlink, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), existing_symlink, new_symlink, ec);
+    }
+}
+#endif
+
+GHC_INLINE void copy_symlink(const path& existing_symlink, const path& new_symlink, std::error_code& ec) noexcept
+{
+    ec.clear();
+    auto to = read_symlink(existing_symlink, ec);
+    if (!ec) {
+        if (exists(to, ec) && is_directory(to, ec)) {
+            create_directory_symlink(to, new_symlink, ec);
+        }
+        else {
+            create_symlink(to, new_symlink, ec);
+        }
+    }
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool create_directories(const path& p)
+{
+    std::error_code ec;
+    auto result = create_directories(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE bool create_directories(const path& p, std::error_code& ec) noexcept
+{
+    path current;
+    ec.clear();
+    bool didCreate = false;
+    for (path::string_type part : p) {
+        current /= part;
+        if (current != p.root_name() && current != p.root_path()) {
+            std::error_code tec;
+            auto fs = status(current, tec);
+            if (tec && fs.type() != file_type::not_found) {
+                ec = tec;
+                return false;
+            }
+            if (!exists(fs)) {
+                create_directory(current, ec);
+                if (ec) {
+                    std::error_code tmp_ec;
+                    if (is_directory(current, tmp_ec)) {
+                        ec.clear();
+                    }
+                    else {
+                        return false;
+                    }
+                }
+                didCreate = true;
+            }
+#ifndef LWG_2935_BEHAVIOUR
+            else if (!is_directory(fs)) {
+                ec = detail::make_error_code(detail::portable_error::exists);
+                return false;
+            }
+#endif
+        }
+    }
+    return didCreate;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool create_directory(const path& p)
+{
+    std::error_code ec;
+    auto result = create_directory(p, path(), ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE bool create_directory(const path& p, std::error_code& ec) noexcept
+{
+    return create_directory(p, path(), ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool create_directory(const path& p, const path& attributes)
+{
+    std::error_code ec;
+    auto result = create_directory(p, attributes, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE bool create_directory(const path& p, const path& attributes, std::error_code& ec) noexcept
+{
+    std::error_code tec;
+    ec.clear();
+    auto fs = status(p, tec);
+#ifdef LWG_2935_BEHAVIOUR
+    if (status_known(fs) && exists(fs)) {
+        return false;
+    }
+#else
+    if (status_known(fs) && exists(fs) && is_directory(fs)) {
+        return false;
+    }
+#endif
+#ifdef GHC_OS_WINDOWS
+    if (!attributes.empty()) {
+        if (!::CreateDirectoryExW(GHC_NATIVEWP(attributes), GHC_NATIVEWP(p), NULL)) {
+            ec = detail::make_system_error();
+            return false;
+        }
+    }
+    else if (!::CreateDirectoryW(GHC_NATIVEWP(p), NULL)) {
+        ec = detail::make_system_error();
+        return false;
+    }
+#else
+    ::mode_t attribs = static_cast<mode_t>(perms::all);
+    if (!attributes.empty()) {
+        struct ::stat fileStat;
+        if (::stat(attributes.c_str(), &fileStat) != 0) {
+            ec = detail::make_system_error();
+            return false;
+        }
+        attribs = fileStat.st_mode;
+    }
+    if (::mkdir(p.c_str(), attribs) != 0) {
+        ec = detail::make_system_error();
+        return false;
+    }
+#endif
+    return true;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void create_directory_symlink(const path& to, const path& new_symlink)
+{
+    std::error_code ec;
+    create_directory_symlink(to, new_symlink, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), to, new_symlink, ec);
+    }
+}
+#endif
+
+GHC_INLINE void create_directory_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept
+{
+    detail::create_symlink(to, new_symlink, true, ec);
+}
+
+#ifndef GHC_OS_WEB
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void create_hard_link(const path& to, const path& new_hard_link)
+{
+    std::error_code ec;
+    create_hard_link(to, new_hard_link, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), to, new_hard_link, ec);
+    }
+}
+#endif
+
+GHC_INLINE void create_hard_link(const path& to, const path& new_hard_link, std::error_code& ec) noexcept
+{
+    detail::create_hardlink(to, new_hard_link, ec);
+}
+#endif
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void create_symlink(const path& to, const path& new_symlink)
+{
+    std::error_code ec;
+    create_symlink(to, new_symlink, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), to, new_symlink, ec);
+    }
+}
+#endif
+
+GHC_INLINE void create_symlink(const path& to, const path& new_symlink, std::error_code& ec) noexcept
+{
+    detail::create_symlink(to, new_symlink, false, ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path current_path()
+{
+    std::error_code ec;
+    auto result = current_path(ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE path current_path(std::error_code& ec)
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    DWORD pathlen = ::GetCurrentDirectoryW(0, 0);
+    std::unique_ptr<wchar_t[]> buffer(new wchar_t[size_t(pathlen) + 1]);
+    if (::GetCurrentDirectoryW(pathlen, buffer.get()) == 0) {
+        ec = detail::make_system_error();
+        return path();
+    }
+    return path(std::wstring(buffer.get()), path::native_format);
+#else
+    size_t pathlen = static_cast<size_t>(std::max(int(::pathconf(".", _PC_PATH_MAX)), int(PATH_MAX)));
+    std::unique_ptr<char[]> buffer(new char[pathlen + 1]);
+    if (::getcwd(buffer.get(), pathlen) == nullptr) {
+        ec = detail::make_system_error();
+        return path();
+    }
+    return path(buffer.get());
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void current_path(const path& p)
+{
+    std::error_code ec;
+    current_path(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+}
+#endif
+
+GHC_INLINE void current_path(const path& p, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    if (!::SetCurrentDirectoryW(GHC_NATIVEWP(p))) {
+        ec = detail::make_system_error();
+    }
+#else
+    if (::chdir(p.string().c_str()) == -1) {
+        ec = detail::make_system_error();
+    }
+#endif
+}
+
+GHC_INLINE bool exists(file_status s) noexcept
+{
+    return status_known(s) && s.type() != file_type::not_found;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool exists(const path& p)
+{
+    return exists(status(p));
+}
+#endif
+
+GHC_INLINE bool exists(const path& p, std::error_code& ec) noexcept
+{
+    file_status s = status(p, ec);
+    if (status_known(s)) {
+        ec.clear();
+    }
+    return exists(s);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool equivalent(const path& p1, const path& p2)
+{
+    std::error_code ec;
+    bool result = equivalent(p1, p2, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p1, p2, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE bool equivalent(const path& p1, const path& p2, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    std::shared_ptr<void> file1(::CreateFileW(GHC_NATIVEWP(p1), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle);
+    auto e1 = ::GetLastError();
+    std::shared_ptr<void> file2(::CreateFileW(GHC_NATIVEWP(p2), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle);
+    if (file1.get() == INVALID_HANDLE_VALUE || file2.get() == INVALID_HANDLE_VALUE) {
+#ifdef LWG_2937_BEHAVIOUR
+        ec = detail::make_system_error(e1 ? e1 : ::GetLastError());
+#else
+        if (file1 == file2) {
+            ec = detail::make_system_error(e1 ? e1 : ::GetLastError());
+        }
+#endif
+        return false;
+    }
+    BY_HANDLE_FILE_INFORMATION inf1, inf2;
+    if (!::GetFileInformationByHandle(file1.get(), &inf1)) {
+        ec = detail::make_system_error();
+        return false;
+    }
+    if (!::GetFileInformationByHandle(file2.get(), &inf2)) {
+        ec = detail::make_system_error();
+        return false;
+    }
+    return inf1.ftLastWriteTime.dwLowDateTime == inf2.ftLastWriteTime.dwLowDateTime && inf1.ftLastWriteTime.dwHighDateTime == inf2.ftLastWriteTime.dwHighDateTime && inf1.nFileIndexHigh == inf2.nFileIndexHigh && inf1.nFileIndexLow == inf2.nFileIndexLow &&
+           inf1.nFileSizeHigh == inf2.nFileSizeHigh && inf1.nFileSizeLow == inf2.nFileSizeLow && inf1.dwVolumeSerialNumber == inf2.dwVolumeSerialNumber;
+#else
+    struct ::stat s1, s2;
+    auto rc1 = ::stat(p1.c_str(), &s1);
+    auto e1 = errno;
+    auto rc2 = ::stat(p2.c_str(), &s2);
+    if (rc1 || rc2) {
+#ifdef LWG_2937_BEHAVIOUR
+        ec = detail::make_system_error(e1 ? e1 : errno);
+#else
+        if (rc1 && rc2) {
+            ec = detail::make_system_error(e1 ? e1 : errno);
+        }
+#endif
+        return false;
+    }
+    return s1.st_dev == s2.st_dev && s1.st_ino == s2.st_ino && s1.st_size == s2.st_size && s1.st_mtime == s2.st_mtime;
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE uintmax_t file_size(const path& p)
+{
+    std::error_code ec;
+    auto result = file_size(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE uintmax_t file_size(const path& p, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    WIN32_FILE_ATTRIBUTE_DATA attr;
+    if (!GetFileAttributesExW(GHC_NATIVEWP(p), GetFileExInfoStandard, &attr)) {
+        ec = detail::make_system_error();
+        return static_cast<uintmax_t>(-1);
+    }
+    return static_cast<uintmax_t>(attr.nFileSizeHigh) << (sizeof(attr.nFileSizeHigh) * 8) | attr.nFileSizeLow;
+#else
+    struct ::stat fileStat;
+    if (::stat(p.c_str(), &fileStat) == -1) {
+        ec = detail::make_system_error();
+        return static_cast<uintmax_t>(-1);
+    }
+    return static_cast<uintmax_t>(fileStat.st_size);
+#endif
+}
+
+#ifndef GHC_OS_WEB
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE uintmax_t hard_link_count(const path& p)
+{
+    std::error_code ec;
+    auto result = hard_link_count(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE uintmax_t hard_link_count(const path& p, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    uintmax_t result = static_cast<uintmax_t>(-1);
+    std::shared_ptr<void> file(::CreateFileW(GHC_NATIVEWP(p), 0, FILE_SHARE_READ | FILE_SHARE_WRITE | FILE_SHARE_DELETE, 0, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, 0), CloseHandle);
+    BY_HANDLE_FILE_INFORMATION inf;
+    if (file.get() == INVALID_HANDLE_VALUE) {
+        ec = detail::make_system_error();
+    }
+    else {
+        if (!::GetFileInformationByHandle(file.get(), &inf)) {
+            ec = detail::make_system_error();
+        }
+        else {
+            result = inf.nNumberOfLinks;
+        }
+    }
+    return result;
+#else
+    uintmax_t result = 0;
+    file_status fs = detail::status_ex(p, ec, nullptr, nullptr, &result, nullptr);
+    if (fs.type() == file_type::not_found) {
+        ec = detail::make_error_code(detail::portable_error::not_found);
+    }
+    return ec ? static_cast<uintmax_t>(-1) : result;
+#endif
+}
+#endif
+
+GHC_INLINE bool is_block_file(file_status s) noexcept
+{
+    return s.type() == file_type::block;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_block_file(const path& p)
+{
+    return is_block_file(status(p));
+}
+#endif
+
+GHC_INLINE bool is_block_file(const path& p, std::error_code& ec) noexcept
+{
+    return is_block_file(status(p, ec));
+}
+
+GHC_INLINE bool is_character_file(file_status s) noexcept
+{
+    return s.type() == file_type::character;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_character_file(const path& p)
+{
+    return is_character_file(status(p));
+}
+#endif
+
+GHC_INLINE bool is_character_file(const path& p, std::error_code& ec) noexcept
+{
+    return is_character_file(status(p, ec));
+}
+
+GHC_INLINE bool is_directory(file_status s) noexcept
+{
+    return s.type() == file_type::directory;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_directory(const path& p)
+{
+    return is_directory(status(p));
+}
+#endif
+
+GHC_INLINE bool is_directory(const path& p, std::error_code& ec) noexcept
+{
+    return is_directory(status(p, ec));
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_empty(const path& p)
+{
+    if (is_directory(p)) {
+        return directory_iterator(p) == directory_iterator();
+    }
+    else {
+        return file_size(p) == 0;
+    }
+}
+#endif
+
+GHC_INLINE bool is_empty(const path& p, std::error_code& ec) noexcept
+{
+    auto fs = status(p, ec);
+    if (ec) {
+        return false;
+    }
+    if (is_directory(fs)) {
+        directory_iterator iter(p, ec);
+        if (ec) {
+            return false;
+        }
+        return iter == directory_iterator();
+    }
+    else {
+        auto sz = file_size(p, ec);
+        if (ec) {
+            return false;
+        }
+        return sz == 0;
+    }
+}
+
+GHC_INLINE bool is_fifo(file_status s) noexcept
+{
+    return s.type() == file_type::fifo;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_fifo(const path& p)
+{
+    return is_fifo(status(p));
+}
+#endif
+
+GHC_INLINE bool is_fifo(const path& p, std::error_code& ec) noexcept
+{
+    return is_fifo(status(p, ec));
+}
+
+GHC_INLINE bool is_other(file_status s) noexcept
+{
+    return exists(s) && !is_regular_file(s) && !is_directory(s) && !is_symlink(s);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_other(const path& p)
+{
+    return is_other(status(p));
+}
+#endif
+
+GHC_INLINE bool is_other(const path& p, std::error_code& ec) noexcept
+{
+    return is_other(status(p, ec));
+}
+
+GHC_INLINE bool is_regular_file(file_status s) noexcept
+{
+    return s.type() == file_type::regular;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_regular_file(const path& p)
+{
+    return is_regular_file(status(p));
+}
+#endif
+
+GHC_INLINE bool is_regular_file(const path& p, std::error_code& ec) noexcept
+{
+    return is_regular_file(status(p, ec));
+}
+
+GHC_INLINE bool is_socket(file_status s) noexcept
+{
+    return s.type() == file_type::socket;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_socket(const path& p)
+{
+    return is_socket(status(p));
+}
+#endif
+
+GHC_INLINE bool is_socket(const path& p, std::error_code& ec) noexcept
+{
+    return is_socket(status(p, ec));
+}
+
+GHC_INLINE bool is_symlink(file_status s) noexcept
+{
+    return s.type() == file_type::symlink;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool is_symlink(const path& p)
+{
+    return is_symlink(symlink_status(p));
+}
+#endif
+
+GHC_INLINE bool is_symlink(const path& p, std::error_code& ec) noexcept
+{
+    return is_symlink(symlink_status(p, ec));
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_time_type last_write_time(const path& p)
+{
+    std::error_code ec;
+    auto result = last_write_time(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE file_time_type last_write_time(const path& p, std::error_code& ec) noexcept
+{
+    time_t result = 0;
+    ec.clear();
+    file_status fs = detail::status_ex(p, ec, nullptr, nullptr, nullptr, &result);
+    return ec ? (file_time_type::min)() : std::chrono::system_clock::from_time_t(result);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void last_write_time(const path& p, file_time_type new_time)
+{
+    std::error_code ec;
+    last_write_time(p, new_time, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+}
+#endif
+
+GHC_INLINE void last_write_time(const path& p, file_time_type new_time, std::error_code& ec) noexcept
+{
+    ec.clear();
+    auto d = new_time.time_since_epoch();
+#ifdef GHC_OS_WINDOWS
+    std::shared_ptr<void> file(::CreateFileW(GHC_NATIVEWP(p), FILE_WRITE_ATTRIBUTES, FILE_SHARE_DELETE | FILE_SHARE_READ | FILE_SHARE_WRITE, NULL, OPEN_EXISTING, FILE_FLAG_BACKUP_SEMANTICS, NULL), ::CloseHandle);
+    FILETIME ft;
+    auto tt = std::chrono::duration_cast<std::chrono::microseconds>(d).count() * 10 + 116444736000000000;
+    ft.dwLowDateTime = static_cast<DWORD>(tt);
+    ft.dwHighDateTime = static_cast<DWORD>(tt >> 32);
+    if (!::SetFileTime(file.get(), 0, 0, &ft)) {
+        ec = detail::make_system_error();
+    }
+#elif defined(GHC_OS_MACOS)
+#ifdef __MAC_OS_X_VERSION_MIN_REQUIRED
+#if __MAC_OS_X_VERSION_MIN_REQUIRED < 101300
+    struct ::stat fs;
+    if (::stat(p.c_str(), &fs) == 0) {
+        struct ::timeval tv[2];
+        tv[0].tv_sec = fs.st_atimespec.tv_sec;
+        tv[0].tv_usec = static_cast<int>(fs.st_atimespec.tv_nsec / 1000);
+        tv[1].tv_sec = std::chrono::duration_cast<std::chrono::seconds>(d).count();
+        tv[1].tv_usec = static_cast<int>(std::chrono::duration_cast<std::chrono::microseconds>(d).count() % 1000000);
+        if (::utimes(p.c_str(), tv) == 0) {
+            return;
+        }
+    }
+    ec = detail::make_system_error();
+    return;
+#else
+    struct ::timespec times[2];
+    times[0].tv_sec = 0;
+    times[0].tv_nsec = UTIME_OMIT;
+    times[1].tv_sec = std::chrono::duration_cast<std::chrono::seconds>(d).count();
+    times[1].tv_nsec = 0;  // std::chrono::duration_cast<std::chrono::nanoseconds>(d).count() % 1000000000;
+    if (::utimensat(AT_FDCWD, p.c_str(), times, AT_SYMLINK_NOFOLLOW) != 0) {
+        ec = detail::make_system_error();
+    }
+    return;
+#endif
+#endif
+#else
+#ifndef UTIME_OMIT
+#define UTIME_OMIT ((1l << 30) - 2l)
+#endif
+    struct ::timespec times[2];
+    times[0].tv_sec = 0;
+    times[0].tv_nsec = UTIME_OMIT;
+    times[1].tv_sec = static_cast<decltype(times[1].tv_sec)>(std::chrono::duration_cast<std::chrono::seconds>(d).count());
+    times[1].tv_nsec = static_cast<decltype(times[1].tv_nsec)>(std::chrono::duration_cast<std::chrono::nanoseconds>(d).count() % 1000000000);
+#if defined(__ANDROID_API__) && __ANDROID_API__ < 12
+    if (syscall(__NR_utimensat, AT_FDCWD, p.c_str(), times, AT_SYMLINK_NOFOLLOW) != 0) {
+#else
+    if (::utimensat(AT_FDCWD, p.c_str(), times, AT_SYMLINK_NOFOLLOW) != 0) {
+#endif
+        ec = detail::make_system_error();
+    }
+    return;
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void permissions(const path& p, perms prms, perm_options opts)
+{
+    std::error_code ec;
+    permissions(p, prms, opts, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+}
+#endif
+
+GHC_INLINE void permissions(const path& p, perms prms, std::error_code& ec) noexcept
+{
+    permissions(p, prms, perm_options::replace, ec);
+}
+
+GHC_INLINE void permissions(const path& p, perms prms, perm_options opts, std::error_code& ec) noexcept
+{
+    if (static_cast<int>(opts & (perm_options::replace | perm_options::add | perm_options::remove)) == 0) {
+        ec = detail::make_error_code(detail::portable_error::invalid_argument);
+        return;
+    }
+    auto fs = symlink_status(p, ec);
+    if ((opts & perm_options::replace) != perm_options::replace) {
+        if ((opts & perm_options::add) == perm_options::add) {
+            prms = fs.permissions() | prms;
+        }
+        else {
+            prms = fs.permissions() & ~prms;
+        }
+    }
+#ifdef GHC_OS_WINDOWS
+#ifdef __GNUC__
+    auto oldAttr = GetFileAttributesW(GHC_NATIVEWP(p));
+    if (oldAttr != INVALID_FILE_ATTRIBUTES) {
+        DWORD newAttr = ((prms & perms::owner_write) == perms::owner_write) ? oldAttr & ~(static_cast<DWORD>(FILE_ATTRIBUTE_READONLY)) : oldAttr | FILE_ATTRIBUTE_READONLY;
+        if (oldAttr == newAttr || SetFileAttributesW(GHC_NATIVEWP(p), newAttr)) {
+            return;
+        }
+    }
+    ec = detail::make_system_error();
+#else
+    int mode = 0;
+    if ((prms & perms::owner_read) == perms::owner_read) {
+        mode |= _S_IREAD;
+    }
+    if ((prms & perms::owner_write) == perms::owner_write) {
+        mode |= _S_IWRITE;
+    }
+    if (::_wchmod(p.wstring().c_str(), mode) != 0) {
+        ec = detail::make_system_error();
+    }
+#endif
+#else
+    if ((opts & perm_options::nofollow) != perm_options::nofollow) {
+        if (::chmod(p.c_str(), static_cast<mode_t>(prms)) != 0) {
+            ec = detail::make_system_error();
+        }
+    }
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path proximate(const path& p, std::error_code& ec)
+{
+    auto cp = current_path(ec);
+    if (!ec) {
+        return proximate(p, cp, ec);
+    }
+    return path();
+}
+#endif
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path proximate(const path& p, const path& base)
+{
+    return weakly_canonical(p).lexically_proximate(weakly_canonical(base));
+}
+#endif
+
+GHC_INLINE path proximate(const path& p, const path& base, std::error_code& ec)
+{
+    return weakly_canonical(p, ec).lexically_proximate(weakly_canonical(base, ec));
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path read_symlink(const path& p)
+{
+    std::error_code ec;
+    auto result = read_symlink(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE path read_symlink(const path& p, std::error_code& ec)
+{
+    file_status fs = symlink_status(p, ec);
+    if (fs.type() != file_type::symlink) {
+        ec = detail::make_error_code(detail::portable_error::invalid_argument);
+        return path();
+    }
+    auto result = detail::resolveSymlink(p, ec);
+    return ec ? path() : result;
+}
+
+GHC_INLINE path relative(const path& p, std::error_code& ec)
+{
+    return relative(p, current_path(ec), ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path relative(const path& p, const path& base)
+{
+    return weakly_canonical(p).lexically_relative(weakly_canonical(base));
+}
+#endif
+
+GHC_INLINE path relative(const path& p, const path& base, std::error_code& ec)
+{
+    return weakly_canonical(p, ec).lexically_relative(weakly_canonical(base, ec));
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool remove(const path& p)
+{
+    std::error_code ec;
+    auto result = remove(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE bool remove(const path& p, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+#ifdef GHC_USE_WCHAR_T
+    auto cstr = p.c_str();
+#else
+    std::wstring np = detail::fromUtf8<std::wstring>(p.u8string());
+    auto cstr = np.c_str();
+#endif
+    DWORD attr = GetFileAttributesW(cstr);
+    if (attr == INVALID_FILE_ATTRIBUTES) {
+        auto error = ::GetLastError();
+        if (error == ERROR_FILE_NOT_FOUND || error == ERROR_PATH_NOT_FOUND) {
+            return false;
+        }
+        ec = detail::make_system_error(error);
+    }
+    else if(attr & FILE_ATTRIBUTE_READONLY) {
+        auto new_attr = attr & ~static_cast<DWORD>(FILE_ATTRIBUTE_READONLY);
+        if(!SetFileAttributesW(cstr, new_attr)) {
+            auto error = ::GetLastError();
+            ec = detail::make_system_error(error);
+        }
+    }
+    if (!ec) {
+        if (attr & FILE_ATTRIBUTE_DIRECTORY) {
+            if (!RemoveDirectoryW(cstr)) {
+                ec = detail::make_system_error();
+            }
+        }
+        else {
+            if (!DeleteFileW(cstr)) {
+                ec = detail::make_system_error();
+            }
+        }
+    }
+#else
+    if (::remove(p.c_str()) == -1) {
+        auto error = errno;
+        if (error == ENOENT) {
+            return false;
+        }
+        ec = detail::make_system_error();
+    }
+#endif
+    return ec ? false : true;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE uintmax_t remove_all(const path& p)
+{
+    std::error_code ec;
+    auto result = remove_all(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE uintmax_t remove_all(const path& p, std::error_code& ec) noexcept
+{
+    ec.clear();
+    uintmax_t count = 0;
+    if (p == "/") {
+        ec = detail::make_error_code(detail::portable_error::not_supported);
+        return static_cast<uintmax_t>(-1);
+    }
+    std::error_code tec;
+    auto fs = status(p, tec);
+    if (exists(fs) && is_directory(fs)) {
+        for (auto iter = directory_iterator(p, ec); iter != directory_iterator(); iter.increment(ec)) {
+            if (ec && !detail::is_not_found_error(ec)) {
+                break;
+            }
+            bool is_symlink_result = iter->is_symlink(ec);
+            if (ec)
+                return static_cast<uintmax_t>(-1);
+            if (!is_symlink_result && iter->is_directory(ec)) {
+                count += remove_all(iter->path(), ec);
+                if (ec) {
+                    return static_cast<uintmax_t>(-1);
+                }
+            }
+            else {
+                if (!ec) {
+                    remove(iter->path(), ec);
+                }
+                if (ec) {
+                    return static_cast<uintmax_t>(-1);
+                }
+                ++count;
+            }
+        }
+    }
+    if (!ec) {
+        if (remove(p, ec)) {
+            ++count;
+        }
+    }
+    if (ec) {
+        return static_cast<uintmax_t>(-1);
+    }
+    return count;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void rename(const path& from, const path& to)
+{
+    std::error_code ec;
+    rename(from, to, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), from, to, ec);
+    }
+}
+#endif
+
+GHC_INLINE void rename(const path& from, const path& to, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    if (from != to) {
+        if (!MoveFileExW(GHC_NATIVEWP(from), GHC_NATIVEWP(to), (DWORD)MOVEFILE_REPLACE_EXISTING)) {
+            ec = detail::make_system_error();
+        }
+    }
+#else
+    if (from != to) {
+        if (::rename(from.c_str(), to.c_str()) != 0) {
+            ec = detail::make_system_error();
+        }
+    }
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void resize_file(const path& p, uintmax_t size)
+{
+    std::error_code ec;
+    resize_file(p, size, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+}
+#endif
+
+GHC_INLINE void resize_file(const path& p, uintmax_t size, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    LARGE_INTEGER lisize;
+    lisize.QuadPart = static_cast<LONGLONG>(size);
+    if (lisize.QuadPart < 0) {
+#ifdef ERROR_FILE_TOO_LARGE
+        ec = detail::make_system_error(ERROR_FILE_TOO_LARGE);
+#else
+        ec = detail::make_system_error(223);
+#endif
+        return;
+    }
+    std::shared_ptr<void> file(CreateFileW(GHC_NATIVEWP(p), GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL), CloseHandle);
+    if (file.get() == INVALID_HANDLE_VALUE) {
+        ec = detail::make_system_error();
+    }
+    else if (SetFilePointerEx(file.get(), lisize, NULL, FILE_BEGIN) == 0 || SetEndOfFile(file.get()) == 0) {
+        ec = detail::make_system_error();
+    }
+#else
+    if (::truncate(p.c_str(), static_cast<off_t>(size)) != 0) {
+        ec = detail::make_system_error();
+    }
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE space_info space(const path& p)
+{
+    std::error_code ec;
+    auto result = space(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE space_info space(const path& p, std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    ULARGE_INTEGER freeBytesAvailableToCaller = {{ 0, 0 }};
+    ULARGE_INTEGER totalNumberOfBytes = {{ 0, 0 }};
+    ULARGE_INTEGER totalNumberOfFreeBytes = {{ 0, 0 }};
+    if (!GetDiskFreeSpaceExW(GHC_NATIVEWP(p), &freeBytesAvailableToCaller, &totalNumberOfBytes, &totalNumberOfFreeBytes)) {
+        ec = detail::make_system_error();
+        return {static_cast<uintmax_t>(-1), static_cast<uintmax_t>(-1), static_cast<uintmax_t>(-1)};
+    }
+    return {static_cast<uintmax_t>(totalNumberOfBytes.QuadPart), static_cast<uintmax_t>(totalNumberOfFreeBytes.QuadPart), static_cast<uintmax_t>(freeBytesAvailableToCaller.QuadPart)};
+#else
+    struct ::statvfs sfs;
+    if (::statvfs(p.c_str(), &sfs) != 0) {
+        ec = detail::make_system_error();
+        return {static_cast<uintmax_t>(-1), static_cast<uintmax_t>(-1), static_cast<uintmax_t>(-1)};
+    }
+    return {static_cast<uintmax_t>(sfs.f_blocks * sfs.f_frsize), static_cast<uintmax_t>(sfs.f_bfree * sfs.f_frsize), static_cast<uintmax_t>(sfs.f_bavail * sfs.f_frsize)};
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_status status(const path& p)
+{
+    std::error_code ec;
+    auto result = status(p, ec);
+    if (result.type() == file_type::none) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE file_status status(const path& p, std::error_code& ec) noexcept
+{
+    return detail::status_ex(p, ec);
+}
+
+GHC_INLINE bool status_known(file_status s) noexcept
+{
+    return s.type() != file_type::none;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_status symlink_status(const path& p)
+{
+    std::error_code ec;
+    auto result = symlink_status(p, ec);
+    if (result.type() == file_type::none) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE file_status symlink_status(const path& p, std::error_code& ec) noexcept
+{
+    return detail::symlink_status_ex(p, ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path temp_directory_path()
+{
+    std::error_code ec;
+    path result = temp_directory_path(ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE path temp_directory_path(std::error_code& ec) noexcept
+{
+    ec.clear();
+#ifdef GHC_OS_WINDOWS
+    wchar_t buffer[512];
+    auto rc = GetTempPathW(511, buffer);
+    if (!rc || rc > 511) {
+        ec = detail::make_system_error();
+        return path();
+    }
+    return path(std::wstring(buffer));
+#else
+    static const char* temp_vars[] = {"TMPDIR", "TMP", "TEMP", "TEMPDIR", nullptr};
+    const char* temp_path = nullptr;
+    for (auto temp_name = temp_vars; *temp_name != nullptr; ++temp_name) {
+        temp_path = std::getenv(*temp_name);
+        if (temp_path) {
+            return path(temp_path);
+        }
+    }
+    return path("/tmp");
+#endif
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE path weakly_canonical(const path& p)
+{
+    std::error_code ec;
+    auto result = weakly_canonical(p, ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), p, ec);
+    }
+    return result;
+}
+#endif
+
+GHC_INLINE path weakly_canonical(const path& p, std::error_code& ec) noexcept
+{
+    path result;
+    ec.clear();
+    bool scan = true;
+    for (auto pe : p) {
+        if (scan) {
+            std::error_code tec;
+            if (exists(result / pe, tec)) {
+                result /= pe;
+            }
+            else {
+                if (ec) {
+                    return path();
+                }
+                scan = false;
+                if (!result.empty()) {
+                    result = canonical(result, ec) / pe;
+                    if (ec) {
+                        break;
+                    }
+                }
+                else {
+                    result /= pe;
+                }
+            }
+        }
+        else {
+            result /= pe;
+        }
+    }
+    if (scan) {
+        if (!result.empty()) {
+            result = canonical(result, ec);
+        }
+    }
+    return ec ? path() : result.lexically_normal();
+}
+
+//-----------------------------------------------------------------------------
+// [fs.class.file_status] class file_status
+// [fs.file_status.cons] constructors and destructor
+GHC_INLINE file_status::file_status() noexcept
+    : file_status(file_type::none)
+{
+}
+
+GHC_INLINE file_status::file_status(file_type ft, perms prms) noexcept
+    : _type(ft)
+    , _perms(prms)
+{
+}
+
+GHC_INLINE file_status::file_status(const file_status& other) noexcept
+    : _type(other._type)
+    , _perms(other._perms)
+{
+}
+
+GHC_INLINE file_status::file_status(file_status&& other) noexcept
+    : _type(other._type)
+    , _perms(other._perms)
+{
+}
+
+GHC_INLINE file_status::~file_status() {}
+
+// assignments:
+GHC_INLINE file_status& file_status::operator=(const file_status& rhs) noexcept
+{
+    _type = rhs._type;
+    _perms = rhs._perms;
+    return *this;
+}
+
+GHC_INLINE file_status& file_status::operator=(file_status&& rhs) noexcept
+{
+    _type = rhs._type;
+    _perms = rhs._perms;
+    return *this;
+}
+
+// [fs.file_status.mods] modifiers
+GHC_INLINE void file_status::type(file_type ft) noexcept
+{
+    _type = ft;
+}
+
+GHC_INLINE void file_status::permissions(perms prms) noexcept
+{
+    _perms = prms;
+}
+
+// [fs.file_status.obs] observers
+GHC_INLINE file_type file_status::type() const noexcept
+{
+    return _type;
+}
+
+GHC_INLINE perms file_status::permissions() const noexcept
+{
+    return _perms;
+}
+
+//-----------------------------------------------------------------------------
+// [fs.class.directory_entry] class directory_entry
+// [fs.dir.entry.cons] constructors and destructor
+// directory_entry::directory_entry() noexcept = default;
+// directory_entry::directory_entry(const directory_entry&) = default;
+// directory_entry::directory_entry(directory_entry&&) noexcept = default;
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE directory_entry::directory_entry(const filesystem::path& p)
+    : _path(p)
+    , _file_size(static_cast<uintmax_t>(-1))
+#ifndef GHC_OS_WINDOWS
+    , _hard_link_count(static_cast<uintmax_t>(-1))
+#endif
+    , _last_write_time(0)
+{
+    refresh();
+}
+#endif
+
+GHC_INLINE directory_entry::directory_entry(const filesystem::path& p, std::error_code& ec)
+    : _path(p)
+    , _file_size(static_cast<uintmax_t>(-1))
+#ifndef GHC_OS_WINDOWS
+    , _hard_link_count(static_cast<uintmax_t>(-1))
+#endif
+    , _last_write_time(0)
+{
+    refresh(ec);
+}
+
+GHC_INLINE directory_entry::~directory_entry() {}
+
+// assignments:
+// directory_entry& directory_entry::operator=(const directory_entry&) = default;
+// directory_entry& directory_entry::operator=(directory_entry&&) noexcept = default;
+
+// [fs.dir.entry.mods] directory_entry modifiers
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void directory_entry::assign(const filesystem::path& p)
+{
+    _path = p;
+    refresh();
+}
+#endif
+
+GHC_INLINE void directory_entry::assign(const filesystem::path& p, std::error_code& ec)
+{
+    _path = p;
+    refresh(ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void directory_entry::replace_filename(const filesystem::path& p)
+{
+    _path.replace_filename(p);
+    refresh();
+}
+#endif
+
+GHC_INLINE void directory_entry::replace_filename(const filesystem::path& p, std::error_code& ec)
+{
+    _path.replace_filename(p);
+    refresh(ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void directory_entry::refresh()
+{
+    std::error_code ec;
+    refresh(ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), _path, ec);
+    }
+}
+#endif
+
+GHC_INLINE void directory_entry::refresh(std::error_code& ec) noexcept
+{
+#ifdef GHC_OS_WINDOWS
+    _status = detail::status_ex(_path, ec, &_symlink_status, &_file_size, nullptr, &_last_write_time);
+#else
+    _status = detail::status_ex(_path, ec, &_symlink_status, &_file_size, &_hard_link_count, &_last_write_time);
+#endif
+}
+
+// [fs.dir.entry.obs] directory_entry observers
+GHC_INLINE const filesystem::path& directory_entry::path() const noexcept
+{
+    return _path;
+}
+
+GHC_INLINE directory_entry::operator const filesystem::path&() const noexcept
+{
+    return _path;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_type directory_entry::status_file_type() const
+{
+    return _status.type() != file_type::none ? _status.type() : filesystem::status(path()).type();
+}
+#endif
+
+GHC_INLINE file_type directory_entry::status_file_type(std::error_code& ec) const noexcept
+{
+    if(_status.type() != file_type::none) {
+        ec.clear();
+        return _status.type();
+    }
+    return filesystem::status(path(), ec).type();
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::exists() const
+{
+    return status_file_type() != file_type::not_found;
+}
+#endif
+
+GHC_INLINE bool directory_entry::exists(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) != file_type::not_found;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_block_file() const
+{
+    return status_file_type() == file_type::block;
+}
+#endif
+GHC_INLINE bool directory_entry::is_block_file(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) == file_type::block;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_character_file() const
+{
+    return status_file_type() == file_type::character;
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_character_file(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) == file_type::character;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_directory() const
+{
+    return status_file_type() == file_type::directory;
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_directory(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) == file_type::directory;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_fifo() const
+{
+    return status_file_type() == file_type::fifo;
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_fifo(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) == file_type::fifo;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_other() const
+{
+    auto ft = status_file_type();
+    return ft != file_type::none && ft != file_type::not_found && ft != file_type::regular && ft != file_type::directory && !is_symlink();
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_other(std::error_code& ec) const noexcept
+{
+    auto ft = status_file_type(ec);
+    bool other = ft != file_type::none && ft != file_type::not_found && ft != file_type::regular && ft != file_type::directory && !is_symlink(ec);
+    return !ec && other;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_regular_file() const
+{
+    return status_file_type() == file_type::regular;
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_regular_file(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) == file_type::regular;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_socket() const
+{
+    return status_file_type() == file_type::socket;
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_socket(std::error_code& ec) const noexcept
+{
+    return status_file_type(ec) == file_type::socket;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE bool directory_entry::is_symlink() const
+{
+    return _symlink_status.type() != file_type::none ? _symlink_status.type() == file_type::symlink : filesystem::is_symlink(symlink_status());
+}
+#endif
+
+GHC_INLINE bool directory_entry::is_symlink(std::error_code& ec) const noexcept
+{
+    if(_symlink_status.type() != file_type::none) {
+        ec.clear();
+        return _symlink_status.type() == file_type::symlink;
+    }
+    return filesystem::is_symlink(symlink_status(ec));
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE uintmax_t directory_entry::file_size() const
+{
+    if (_file_size != static_cast<uintmax_t>(-1)) {
+        return _file_size;
+    }
+    return filesystem::file_size(path());
+}
+#endif
+
+GHC_INLINE uintmax_t directory_entry::file_size(std::error_code& ec) const noexcept
+{
+    if (_file_size != static_cast<uintmax_t>(-1)) {
+        ec.clear();
+        return _file_size;
+    }
+    return filesystem::file_size(path(), ec);
+}
+
+#ifndef GHC_OS_WEB
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE uintmax_t directory_entry::hard_link_count() const
+{
+#ifndef GHC_OS_WINDOWS
+    if (_hard_link_count != static_cast<uintmax_t>(-1)) {
+        return _hard_link_count;
+    }
+#endif
+    return filesystem::hard_link_count(path());
+}
+#endif
+
+GHC_INLINE uintmax_t directory_entry::hard_link_count(std::error_code& ec) const noexcept
+{
+#ifndef GHC_OS_WINDOWS
+    if (_hard_link_count != static_cast<uintmax_t>(-1)) {
+        ec.clear();
+        return _hard_link_count;
+    }
+#endif
+    return filesystem::hard_link_count(path(), ec);
+}
+#endif
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_time_type directory_entry::last_write_time() const
+{
+    if (_last_write_time != 0) {
+        return std::chrono::system_clock::from_time_t(_last_write_time);
+    }
+    return filesystem::last_write_time(path());
+}
+#endif
+
+GHC_INLINE file_time_type directory_entry::last_write_time(std::error_code& ec) const noexcept
+{
+    if (_last_write_time != 0) {
+        ec.clear();
+        return std::chrono::system_clock::from_time_t(_last_write_time);
+    }
+    return filesystem::last_write_time(path(), ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_status directory_entry::status() const
+{
+    if (_status.type() != file_type::none && _status.permissions() != perms::unknown) {
+        return _status;
+    }
+    return filesystem::status(path());
+}
+#endif
+
+GHC_INLINE file_status directory_entry::status(std::error_code& ec) const noexcept
+{
+    if (_status.type() != file_type::none && _status.permissions() != perms::unknown) {
+        ec.clear();
+        return _status;
+    }
+    return filesystem::status(path(), ec);
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE file_status directory_entry::symlink_status() const
+{
+    if (_symlink_status.type() != file_type::none && _symlink_status.permissions() != perms::unknown) {
+        return _symlink_status;
+    }
+    return filesystem::symlink_status(path());
+}
+#endif
+
+GHC_INLINE file_status directory_entry::symlink_status(std::error_code& ec) const noexcept
+{
+    if (_symlink_status.type() != file_type::none && _symlink_status.permissions() != perms::unknown) {
+        ec.clear();
+        return _symlink_status;
+    }
+    return filesystem::symlink_status(path(), ec);
+}
+
+#ifdef GHC_HAS_THREEWAY_COMP
+GHC_INLINE std::strong_ordering directory_entry::operator<=>(const directory_entry& rhs) const noexcept
+{
+    return _path <=> rhs._path;
+}
+#endif
+
+GHC_INLINE bool directory_entry::operator<(const directory_entry& rhs) const noexcept
+{
+    return _path < rhs._path;
+}
+
+GHC_INLINE bool directory_entry::operator==(const directory_entry& rhs) const noexcept
+{
+    return _path == rhs._path;
+}
+
+GHC_INLINE bool directory_entry::operator!=(const directory_entry& rhs) const noexcept
+{
+    return _path != rhs._path;
+}
+
+GHC_INLINE bool directory_entry::operator<=(const directory_entry& rhs) const noexcept
+{
+    return _path <= rhs._path;
+}
+
+GHC_INLINE bool directory_entry::operator>(const directory_entry& rhs) const noexcept
+{
+    return _path > rhs._path;
+}
+
+GHC_INLINE bool directory_entry::operator>=(const directory_entry& rhs) const noexcept
+{
+    return _path >= rhs._path;
+}
+
+//-----------------------------------------------------------------------------
+// [fs.class.directory_iterator] class directory_iterator
+
+#ifdef GHC_OS_WINDOWS
+class directory_iterator::impl
+{
+public:
+    impl(const path& p, directory_options options)
+        : _base(p)
+        , _options(options)
+        , _dirHandle(INVALID_HANDLE_VALUE)
+    {
+        if (!_base.empty()) {
+            ZeroMemory(&_findData, sizeof(WIN32_FIND_DATAW));
+            if ((_dirHandle = FindFirstFileW(GHC_NATIVEWP((_base / "*")), &_findData)) != INVALID_HANDLE_VALUE) {
+                if (std::wstring(_findData.cFileName) == L"." || std::wstring(_findData.cFileName) == L"..") {
+                    increment(_ec);
+                }
+                else {
+                    _dir_entry._path = _base / std::wstring(_findData.cFileName);
+                    copyToDirEntry(_ec);
+                }
+            }
+            else {
+                auto error = ::GetLastError();
+                _base = filesystem::path();
+                if (error != ERROR_ACCESS_DENIED || (options & directory_options::skip_permission_denied) == directory_options::none) {
+                    _ec = detail::make_system_error();
+                }
+            }
+        }
+    }
+    impl(const impl& other) = delete;
+    ~impl()
+    {
+        if (_dirHandle != INVALID_HANDLE_VALUE) {
+            FindClose(_dirHandle);
+            _dirHandle = INVALID_HANDLE_VALUE;
+        }
+    }
+    void increment(std::error_code& ec)
+    {
+        if (_dirHandle != INVALID_HANDLE_VALUE) {
+            do {
+                if (FindNextFileW(_dirHandle, &_findData)) {
+                    _dir_entry._path = _base;
+#ifdef GHC_USE_WCHAR_T
+                    _dir_entry._path.append_name(_findData.cFileName);
+#else
+#ifdef GHC_RAISE_UNICODE_ERRORS
+                    try {
+                        _dir_entry._path.append_name(detail::toUtf8(_findData.cFileName).c_str());
+                    }
+                    catch (filesystem_error& fe) {
+                        ec = fe.code();
+                        return;
+                    }
+#else
+                    _dir_entry._path.append_name(detail::toUtf8(_findData.cFileName).c_str());
+#endif
+#endif
+                    copyToDirEntry(ec);
+                }
+                else {
+                    auto err = ::GetLastError();
+                    if (err != ERROR_NO_MORE_FILES) {
+                        _ec = ec = detail::make_system_error(err);
+                    }
+                    FindClose(_dirHandle);
+                    _dirHandle = INVALID_HANDLE_VALUE;
+                    _dir_entry._path.clear();
+                    break;
+                }
+            } while (std::wstring(_findData.cFileName) == L"." || std::wstring(_findData.cFileName) == L"..");
+        }
+        else {
+            ec = _ec;
+        }
+    }
+    void copyToDirEntry(std::error_code& ec)
+    {
+        if (_findData.dwFileAttributes & FILE_ATTRIBUTE_REPARSE_POINT) {
+            _dir_entry._status = detail::status_ex(_dir_entry._path, ec, &_dir_entry._symlink_status, &_dir_entry._file_size, nullptr, &_dir_entry._last_write_time);
+        }
+        else {
+            _dir_entry._status = detail::status_from_INFO(_dir_entry._path, &_findData, ec, &_dir_entry._file_size, &_dir_entry._last_write_time);
+            _dir_entry._symlink_status = _dir_entry._status;
+        }
+        if (ec) {
+            if (_dir_entry._status.type() != file_type::none && _dir_entry._symlink_status.type() != file_type::none) {
+                ec.clear();
+            }
+            else {
+                _dir_entry._file_size = static_cast<uintmax_t>(-1);
+                _dir_entry._last_write_time = 0;
+            }
+        }
+    }
+    path _base;
+    directory_options _options;
+    WIN32_FIND_DATAW _findData;
+    HANDLE _dirHandle;
+    directory_entry _dir_entry;
+    std::error_code _ec;
+};
+#else
+// POSIX implementation
+class directory_iterator::impl
+{
+public:
+    impl(const path& path, directory_options options)
+        : _base(path)
+        , _options(options)
+        , _dir(nullptr)
+        , _entry(nullptr)
+    {
+        if (!path.empty()) {
+            _dir = ::opendir(path.native().c_str());
+            if (!_dir) {
+                auto error = errno;
+                _base = filesystem::path();
+                if ((error != EACCES && error != EPERM) || (options & directory_options::skip_permission_denied) == directory_options::none) {
+                    _ec = detail::make_system_error();
+                }
+            }
+            else {
+                increment(_ec);
+            }
+        }
+    }
+    impl(const impl& other) = delete;
+    ~impl()
+    {
+        if (_dir) {
+            ::closedir(_dir);
+        }
+    }
+    void increment(std::error_code& ec)
+    {
+        if (_dir) {
+            bool skip;
+            do {
+                skip = false;
+                errno = 0;
+                _entry = ::readdir(_dir);
+                if (_entry) {
+                    _dir_entry._path = _base;
+                    _dir_entry._path.append_name(_entry->d_name);
+                    copyToDirEntry();
+                    if (ec && (ec.value() == EACCES || ec.value() == EPERM) && (_options & directory_options::skip_permission_denied) == directory_options::skip_permission_denied) {
+                        ec.clear();
+                        skip = true;
+                    }
+                }
+                else {
+                    ::closedir(_dir);
+                    _dir = nullptr;
+                    _dir_entry._path.clear();
+                    if (errno) {
+                        ec = detail::make_system_error();
+                    }
+                    break;
+                }
+            } while (skip || std::strcmp(_entry->d_name, ".") == 0 || std::strcmp(_entry->d_name, "..") == 0);
+        }
+    }
+
+    void copyToDirEntry()
+    {
+#ifdef GHC_NO_DIRENT_D_TYPE
+        _dir_entry._symlink_status = file_status();
+        _dir_entry._status = file_status();
+#else
+        _dir_entry._symlink_status.permissions(perms::unknown);
+        switch(_entry->d_type) {
+            case DT_BLK: _dir_entry._symlink_status.type(file_type::block); break;
+            case DT_CHR: _dir_entry._symlink_status.type(file_type::character); break;
+            case DT_DIR: _dir_entry._symlink_status.type(file_type::directory); break;
+            case DT_FIFO: _dir_entry._symlink_status.type(file_type::fifo); break;
+            case DT_LNK: _dir_entry._symlink_status.type(file_type::symlink); break;
+            case DT_REG: _dir_entry._symlink_status.type(file_type::regular); break;
+            case DT_SOCK: _dir_entry._symlink_status.type(file_type::socket); break;
+            case DT_UNKNOWN: _dir_entry._symlink_status.type(file_type::none); break;
+            default: _dir_entry._symlink_status.type(file_type::unknown); break;
+        }
+        if (_entry->d_type != DT_LNK) {
+            _dir_entry._status = _dir_entry._symlink_status;
+        }
+        else {
+            _dir_entry._status.type(file_type::none);
+            _dir_entry._status.permissions(perms::unknown);
+        }
+#endif
+        _dir_entry._file_size = static_cast<uintmax_t>(-1);
+        _dir_entry._hard_link_count = static_cast<uintmax_t>(-1);
+        _dir_entry._last_write_time = 0;
+    }
+    path _base;
+    directory_options _options;
+    DIR* _dir;
+    struct ::dirent* _entry;
+    directory_entry _dir_entry;
+    std::error_code _ec;
+};
+#endif
+
+// [fs.dir.itr.members] member functions
+GHC_INLINE directory_iterator::directory_iterator() noexcept
+    : _impl(new impl(path(), directory_options::none))
+{
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE directory_iterator::directory_iterator(const path& p)
+    : _impl(new impl(p, directory_options::none))
+{
+    if (_impl->_ec) {
+        throw filesystem_error(detail::systemErrorText(_impl->_ec.value()), p, _impl->_ec);
+    }
+    _impl->_ec.clear();
+}
+
+GHC_INLINE directory_iterator::directory_iterator(const path& p, directory_options options)
+    : _impl(new impl(p, options))
+{
+    if (_impl->_ec) {
+        throw filesystem_error(detail::systemErrorText(_impl->_ec.value()), p, _impl->_ec);
+    }
+}
+#endif
+
+GHC_INLINE directory_iterator::directory_iterator(const path& p, std::error_code& ec) noexcept
+    : _impl(new impl(p, directory_options::none))
+{
+    if (_impl->_ec) {
+        ec = _impl->_ec;
+    }
+}
+
+GHC_INLINE directory_iterator::directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept
+    : _impl(new impl(p, options))
+{
+    if (_impl->_ec) {
+        ec = _impl->_ec;
+    }
+}
+
+GHC_INLINE directory_iterator::directory_iterator(const directory_iterator& rhs)
+    : _impl(rhs._impl)
+{
+}
+
+GHC_INLINE directory_iterator::directory_iterator(directory_iterator&& rhs) noexcept
+    : _impl(std::move(rhs._impl))
+{
+}
+
+GHC_INLINE directory_iterator::~directory_iterator() {}
+
+GHC_INLINE directory_iterator& directory_iterator::operator=(const directory_iterator& rhs)
+{
+    _impl = rhs._impl;
+    return *this;
+}
+
+GHC_INLINE directory_iterator& directory_iterator::operator=(directory_iterator&& rhs) noexcept
+{
+    _impl = std::move(rhs._impl);
+    return *this;
+}
+
+GHC_INLINE const directory_entry& directory_iterator::operator*() const
+{
+    return _impl->_dir_entry;
+}
+
+GHC_INLINE const directory_entry* directory_iterator::operator->() const
+{
+    return &_impl->_dir_entry;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE directory_iterator& directory_iterator::operator++()
+{
+    std::error_code ec;
+    _impl->increment(ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), _impl->_dir_entry._path, ec);
+    }
+    return *this;
+}
+#endif
+
+GHC_INLINE directory_iterator& directory_iterator::increment(std::error_code& ec) noexcept
+{
+    _impl->increment(ec);
+    return *this;
+}
+
+GHC_INLINE bool directory_iterator::operator==(const directory_iterator& rhs) const
+{
+    return _impl->_dir_entry._path == rhs._impl->_dir_entry._path;
+}
+
+GHC_INLINE bool directory_iterator::operator!=(const directory_iterator& rhs) const
+{
+    return _impl->_dir_entry._path != rhs._impl->_dir_entry._path;
+}
+
+// [fs.dir.itr.nonmembers] directory_iterator non-member functions
+
+GHC_INLINE directory_iterator begin(directory_iterator iter) noexcept
+{
+    return iter;
+}
+
+GHC_INLINE directory_iterator end(const directory_iterator&) noexcept
+{
+    return directory_iterator();
+}
+
+//-----------------------------------------------------------------------------
+// [fs.class.rec.dir.itr] class recursive_directory_iterator
+
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator() noexcept
+    : _impl(new recursive_directory_iterator_impl(directory_options::none, true))
+{
+    _impl->_dir_iter_stack.push(directory_iterator());
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p)
+    : _impl(new recursive_directory_iterator_impl(directory_options::none, true))
+{
+    _impl->_dir_iter_stack.push(directory_iterator(p));
+}
+
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p, directory_options options)
+    : _impl(new recursive_directory_iterator_impl(options, true))
+{
+    _impl->_dir_iter_stack.push(directory_iterator(p, options));
+}
+#endif
+
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p, directory_options options, std::error_code& ec) noexcept
+    : _impl(new recursive_directory_iterator_impl(options, true))
+{
+    _impl->_dir_iter_stack.push(directory_iterator(p, options, ec));
+}
+
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const path& p, std::error_code& ec) noexcept
+    : _impl(new recursive_directory_iterator_impl(directory_options::none, true))
+{
+    _impl->_dir_iter_stack.push(directory_iterator(p, ec));
+}
+
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(const recursive_directory_iterator& rhs)
+    : _impl(rhs._impl)
+{
+}
+
+GHC_INLINE recursive_directory_iterator::recursive_directory_iterator(recursive_directory_iterator&& rhs) noexcept
+    : _impl(std::move(rhs._impl))
+{
+}
+
+GHC_INLINE recursive_directory_iterator::~recursive_directory_iterator() {}
+
+// [fs.rec.dir.itr.members] observers
+GHC_INLINE directory_options recursive_directory_iterator::options() const
+{
+    return _impl->_options;
+}
+
+GHC_INLINE int recursive_directory_iterator::depth() const
+{
+    return static_cast<int>(_impl->_dir_iter_stack.size() - 1);
+}
+
+GHC_INLINE bool recursive_directory_iterator::recursion_pending() const
+{
+    return _impl->_recursion_pending;
+}
+
+GHC_INLINE const directory_entry& recursive_directory_iterator::operator*() const
+{
+    return *(_impl->_dir_iter_stack.top());
+}
+
+GHC_INLINE const directory_entry* recursive_directory_iterator::operator->() const
+{
+    return &(*(_impl->_dir_iter_stack.top()));
+}
+
+// [fs.rec.dir.itr.members] modifiers recursive_directory_iterator&
+GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::operator=(const recursive_directory_iterator& rhs)
+{
+    _impl = rhs._impl;
+    return *this;
+}
+
+GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::operator=(recursive_directory_iterator&& rhs) noexcept
+{
+    _impl = std::move(rhs._impl);
+    return *this;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::operator++()
+{
+    std::error_code ec;
+    increment(ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), _impl->_dir_iter_stack.empty() ? path() : _impl->_dir_iter_stack.top()->path(), ec);
+    }
+    return *this;
+}
+#endif
+
+GHC_INLINE recursive_directory_iterator& recursive_directory_iterator::increment(std::error_code& ec) noexcept
+{
+    bool isSymLink = (*this)->is_symlink(ec);
+    bool isDir = !ec && (*this)->is_directory(ec);
+    if(isSymLink && detail::is_not_found_error(ec)) {
+        ec.clear();
+    }
+    if(!ec) {
+        if (recursion_pending() && isDir && (!isSymLink || (options() & directory_options::follow_directory_symlink) != directory_options::none)) {
+            _impl->_dir_iter_stack.push(directory_iterator((*this)->path(), _impl->_options, ec));
+        }
+        else {
+            _impl->_dir_iter_stack.top().increment(ec);
+        }
+        if (!ec) {
+            while (depth() && _impl->_dir_iter_stack.top() == directory_iterator()) {
+                _impl->_dir_iter_stack.pop();
+                _impl->_dir_iter_stack.top().increment(ec);
+            }
+        }
+        else if (!_impl->_dir_iter_stack.empty()) {
+            _impl->_dir_iter_stack.pop();
+        }
+        _impl->_recursion_pending = true;
+    }
+    return *this;
+}
+
+#ifdef GHC_WITH_EXCEPTIONS
+GHC_INLINE void recursive_directory_iterator::pop()
+{
+    std::error_code ec;
+    pop(ec);
+    if (ec) {
+        throw filesystem_error(detail::systemErrorText(ec.value()), _impl->_dir_iter_stack.empty() ? path() : _impl->_dir_iter_stack.top()->path(), ec);
+    }
+}
+#endif
+
+GHC_INLINE void recursive_directory_iterator::pop(std::error_code& ec)
+{
+    if (depth() == 0) {
+        *this = recursive_directory_iterator();
+    }
+    else {
+        do {
+            _impl->_dir_iter_stack.pop();
+            _impl->_dir_iter_stack.top().increment(ec);
+        } while (depth() && _impl->_dir_iter_stack.top() == directory_iterator());
+    }
+}
+
+GHC_INLINE void recursive_directory_iterator::disable_recursion_pending()
+{
+    _impl->_recursion_pending = false;
+}
+
+// other members as required by [input.iterators]
+GHC_INLINE bool recursive_directory_iterator::operator==(const recursive_directory_iterator& rhs) const
+{
+    return _impl->_dir_iter_stack.top() == rhs._impl->_dir_iter_stack.top();
+}
+
+GHC_INLINE bool recursive_directory_iterator::operator!=(const recursive_directory_iterator& rhs) const
+{
+    return _impl->_dir_iter_stack.top() != rhs._impl->_dir_iter_stack.top();
+}
+
+// [fs.rec.dir.itr.nonmembers] directory_iterator non-member functions
+GHC_INLINE recursive_directory_iterator begin(recursive_directory_iterator iter) noexcept
+{
+    return iter;
+}
+
+GHC_INLINE recursive_directory_iterator end(const recursive_directory_iterator&) noexcept
+{
+    return recursive_directory_iterator();
+}
+
+#endif  // GHC_EXPAND_IMPL
+
+}  // namespace filesystem
+}  // namespace ghc
+
+// cleanup some macros
+#undef GHC_INLINE
+#undef GHC_EXPAND_IMPL
+
+#endif  // GHC_FILESYSTEM_H
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/hostname.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/hostname.h
index b2f9f0c..bd93b4a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/hostname.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/hostname.h
@@ -1,23 +1,19 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_HOSTNAME_H_
 #define WPIUTIL_WPI_HOSTNAME_H_
 
 #include <string>
-
-#include "wpi/StringRef.h"
+#include <string_view>
 
 namespace wpi {
 template <typename T>
 class SmallVectorImpl;
 
 std::string GetHostname();
-StringRef GetHostname(SmallVectorImpl<char>& name);
+std::string_view GetHostname(SmallVectorImpl<char>& name);
 }  // namespace wpi
 
 #endif  // WPIUTIL_WPI_HOSTNAME_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/jni_util.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/jni_util.h
index 15f59be..195e37e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/jni_util.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/jni_util.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_JNI_UTIL_H_
 #define WPIUTIL_WPI_JNI_UTIL_H_
@@ -12,48 +9,61 @@
 
 #include <queue>
 #include <string>
+#include <string_view>
 #include <type_traits>
 #include <utility>
 #include <vector>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/ConvertUTF.h"
 #include "wpi/SafeThread.h"
 #include "wpi/SmallString.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
-#include "wpi/deprecated.h"
+#include "wpi/StringExtras.h"
 #include "wpi/mutex.h"
 #include "wpi/raw_ostream.h"
-
-/** WPILib C++ utilities (wpiutil) namespace */
-namespace wpi {
+#include "wpi/span.h"
 
 /** Java Native Interface (JNI) utility functions */
-namespace java {
+namespace wpi::java {
 
-// Gets a Java stack trace.  Also provides the last function
-// in the stack trace not starting with excludeFuncPrefix (useful for e.g.
-// finding the first user call to a series of library functions).
+/**
+ * Gets a Java stack trace.
+ *
+ * Also provides the last function in the stack trace not starting with
+ * excludeFuncPrefix (useful for e.g. finding the first user call to a series of
+ * library functions).
+ *
+ * @param env JRE environment.
+ * @param func Storage for last function in the stack trace not starting with
+ *             excludeFuncPrefix.
+ * @param excludeFuncPrefix Prefix for functions to ignore in stack trace.
+ */
 std::string GetJavaStackTrace(JNIEnv* env, std::string* func = nullptr,
-                              StringRef excludeFuncPrefix = StringRef());
+                              std::string_view excludeFuncPrefix = {});
 
-// Finds a class and keep it as a global reference.
-// Use with caution, as the destructor does NOT call DeleteGlobalRef due
-// to potential shutdown issues with doing so.
+/**
+ * Finds a class and keeps it as a global reference.
+ *
+ * Use with caution, as the destructor does NOT call DeleteGlobalRef due to
+ * potential shutdown issues with doing so.
+ */
 class JClass {
  public:
   JClass() = default;
 
   JClass(JNIEnv* env, const char* name) {
     jclass local = env->FindClass(name);
-    if (!local) return;
+    if (!local) {
+      return;
+    }
     m_cls = static_cast<jclass>(env->NewGlobalRef(local));
     env->DeleteLocalRef(local);
   }
 
   void free(JNIEnv* env) {
-    if (m_cls) env->DeleteGlobalRef(m_cls);
+    if (m_cls) {
+      env->DeleteGlobalRef(m_cls);
+    }
     m_cls = nullptr;
   }
 
@@ -80,20 +90,25 @@
   }
 
   void free(JNIEnv* env) {
-    if (m_cls) env->DeleteGlobalRef(m_cls);
+    if (m_cls) {
+      env->DeleteGlobalRef(m_cls);
+    }
     m_cls = nullptr;
   }
 
   explicit operator bool() const { return m_cls; }
 
-  operator T() const { return m_cls; }
+  operator T() const { return m_cls; }  // NOLINT
 
  protected:
   T m_cls = nullptr;
 };
 
-// Container class for cleaning up Java local references.
-// The destructor calls DeleteLocalRef.
+/**
+ * Container class for cleaning up Java local references.
+ *
+ * The destructor calls DeleteLocalRef.
+ */
 template <typename T>
 class JLocal {
  public:
@@ -110,9 +125,11 @@
     return *this;
   }
   ~JLocal() {
-    if (m_obj) m_env->DeleteLocalRef(m_obj);
+    if (m_obj) {
+      m_env->DeleteLocalRef(m_obj);
+    }
   }
-  operator T() { return m_obj; }
+  operator T() { return m_obj; }  // NOLINT
   T obj() { return m_obj; }
 
  private:
@@ -124,9 +141,12 @@
 // Conversions from Java objects to C++
 //
 
-// Java string (jstring) reference.  The string is provided as UTF8.
-// This is not actually a reference, as it makes a copy of the string
-// characters, but it's named this way for consistency.
+/**
+ * Java string (jstring) reference.
+ *
+ * The string is provided as UTF8. This is not actually a reference, as it makes
+ * a copy of the string characters, but it's named this way for consistency.
+ */
 class JStringRef {
  public:
   JStringRef(JNIEnv* env, jstring str) {
@@ -134,7 +154,7 @@
       jsize size = env->GetStringLength(str);
       const jchar* chars = env->GetStringCritical(str, nullptr);
       if (chars) {
-        convertUTF16ToUTF8String(makeArrayRef(chars, size), m_str);
+        convertUTF16ToUTF8String(wpi::span<const jchar>(chars, size), m_str);
         env->ReleaseStringCritical(str, chars);
       }
     } else {
@@ -146,8 +166,8 @@
     m_str.pop_back();
   }
 
-  operator StringRef() const { return m_str; }
-  StringRef str() const { return m_str; }
+  operator std::string_view() const { return m_str.str(); }  // NOLINT
+  std::string_view str() const { return m_str.str(); }
   const char* c_str() const { return m_str.data(); }
   size_t size() const { return m_str.size(); }
 
@@ -161,32 +181,44 @@
 template <typename C, typename T>
 class JArrayRefInner {};
 
-// Specialization of JArrayRefBase to provide StringRef conversion.
+/**
+ * Specialization of JArrayRefBase to provide std::string_view conversion.
+ */
 template <typename C>
 class JArrayRefInner<C, jbyte> {
  public:
-  operator StringRef() const { return str(); }
+  operator std::string_view() const { return str(); }
 
-  StringRef str() const {
+  std::string_view str() const {
     auto arr = static_cast<const C*>(this)->array();
-    if (arr.empty()) return StringRef{};
-    return StringRef{reinterpret_cast<const char*>(arr.data()), arr.size()};
+    if (arr.empty()) {
+      return {};
+    }
+    return {reinterpret_cast<const char*>(arr.data()), arr.size()};
   }
 };
 
-// Base class for J*ArrayRef and CriticalJ*ArrayRef
+/**
+ * Base class for J*ArrayRef and CriticalJ*ArrayRef
+ */
 template <typename T>
 class JArrayRefBase : public JArrayRefInner<JArrayRefBase<T>, T> {
  public:
   explicit operator bool() const { return this->m_elements != nullptr; }
 
-  operator ArrayRef<T>() const { return array(); }
+  operator span<const T>() const { return array(); }  // NOLINT
 
-  ArrayRef<T> array() const {
-    if (!this->m_elements) return ArrayRef<T>{};
-    return ArrayRef<T>{this->m_elements, this->m_size};
+  span<const T> array() const {
+    if (!this->m_elements) {
+      return {};
+    }
+    return {this->m_elements, this->m_size};
   }
 
+  size_t size() const { return this->m_size; }
+  T& operator[](size_t i) { return this->m_elements[i]; }
+  const T& operator[](size_t i) const { return this->m_elements[i]; }
+
   JArrayRefBase(const JArrayRefBase&) = delete;
   JArrayRefBase& operator=(const JArrayRefBase&) = delete;
 
@@ -245,30 +277,34 @@
               env,                                                             \
               static_cast<T*>(bb ? env->GetDirectBufferAddress(bb) : nullptr), \
               len) {                                                           \
-      if (!bb)                                                                 \
+      if (!bb) {                                                               \
         errs() << "JArrayRef was passed a null pointer at \n"                  \
                << GetJavaStackTrace(env);                                      \
+      }                                                                        \
     }                                                                          \
     J##F##ArrayRef(JNIEnv* env, T##Array jarr, int len)                        \
         : detail::JArrayRefBase<T>(env, jarr, len) {                           \
-      if (jarr)                                                                \
+      if (jarr) {                                                              \
         m_elements = env->Get##F##ArrayElements(jarr, nullptr);                \
-      else                                                                     \
+      } else {                                                                 \
         errs() << "JArrayRef was passed a null pointer at \n"                  \
                << GetJavaStackTrace(env);                                      \
+      }                                                                        \
     }                                                                          \
     J##F##ArrayRef(JNIEnv* env, T##Array jarr)                                 \
         : detail::JArrayRefBase<T>(env, jarr) {                                \
-      if (jarr)                                                                \
+      if (jarr) {                                                              \
         m_elements = env->Get##F##ArrayElements(jarr, nullptr);                \
-      else                                                                     \
+      } else {                                                                 \
         errs() << "JArrayRef was passed a null pointer at \n"                  \
                << GetJavaStackTrace(env);                                      \
+      }                                                                        \
     }                                                                          \
     ~J##F##ArrayRef() {                                                        \
-      if (m_jarr && m_elements)                                                \
+      if (m_jarr && m_elements) {                                              \
         m_env->Release##F##ArrayElements(static_cast<T##Array>(m_jarr),        \
                                          m_elements, JNI_ABORT);               \
+      }                                                                        \
     }                                                                          \
   };                                                                           \
                                                                                \
@@ -276,25 +312,28 @@
    public:                                                                     \
     CriticalJ##F##ArrayRef(JNIEnv* env, T##Array jarr, int len)                \
         : detail::JArrayRefBase<T>(env, jarr, len) {                           \
-      if (jarr)                                                                \
+      if (jarr) {                                                              \
         m_elements =                                                           \
             static_cast<T*>(env->GetPrimitiveArrayCritical(jarr, nullptr));    \
-      else                                                                     \
+      } else {                                                                 \
         errs() << "JArrayRef was passed a null pointer at \n"                  \
                << GetJavaStackTrace(env);                                      \
+      }                                                                        \
     }                                                                          \
     CriticalJ##F##ArrayRef(JNIEnv* env, T##Array jarr)                         \
         : detail::JArrayRefBase<T>(env, jarr) {                                \
-      if (jarr)                                                                \
+      if (jarr) {                                                              \
         m_elements =                                                           \
             static_cast<T*>(env->GetPrimitiveArrayCritical(jarr, nullptr));    \
-      else                                                                     \
+      } else {                                                                 \
         errs() << "JArrayRef was passed a null pointer at \n"                  \
                << GetJavaStackTrace(env);                                      \
+      }                                                                        \
     }                                                                          \
     ~CriticalJ##F##ArrayRef() {                                                \
-      if (m_jarr && m_elements)                                                \
+      if (m_jarr && m_elements) {                                              \
         m_env->ReleasePrimitiveArrayCritical(m_jarr, m_elements, JNI_ABORT);   \
+      }                                                                        \
     }                                                                          \
   };
 
@@ -312,8 +351,13 @@
 // Conversions from C++ to Java objects
 //
 
-// Convert a UTF8 string into a jstring.
-inline jstring MakeJString(JNIEnv* env, StringRef str) {
+/**
+ * Convert a UTF8 string into a jstring.
+ *
+ * @param env JRE environment.
+ * @param str String to convert.
+ */
+inline jstring MakeJString(JNIEnv* env, std::string_view str) {
   SmallVector<UTF16, 128> chars;
   convertUTF8ToUTF16String(str, chars);
   return env->NewString(chars.begin(), chars.size());
@@ -322,31 +366,43 @@
 // details for MakeJIntArray
 namespace detail {
 
-// Slow path (get primitive array and set individual elements).  This
-// is used if the input type is not an integer of the same size (note
-// signed/unsigned is ignored).
+/**
+ * Slow path (get primitive array and set individual elements).
+ *
+ * This is used if the input type is not an integer of the same size (note
+ * signed/unsigned is ignored).
+ */
 template <typename T,
           bool = (std::is_integral<T>::value && sizeof(jint) == sizeof(T))>
 struct ConvertIntArray {
-  static jintArray ToJava(JNIEnv* env, ArrayRef<T> arr) {
+  static jintArray ToJava(JNIEnv* env, span<const T> arr) {
     jintArray jarr = env->NewIntArray(arr.size());
-    if (!jarr) return nullptr;
+    if (!jarr) {
+      return nullptr;
+    }
     jint* elements =
         static_cast<jint*>(env->GetPrimitiveArrayCritical(jarr, nullptr));
-    if (!elements) return nullptr;
-    for (size_t i = 0; i < arr.size(); ++i)
+    if (!elements) {
+      return nullptr;
+    }
+    for (size_t i = 0; i < arr.size(); ++i) {
       elements[i] = static_cast<jint>(arr[i]);
+    }
     env->ReleasePrimitiveArrayCritical(jarr, elements, 0);
     return jarr;
   }
 };
 
-// Fast path (use SetIntArrayRegion)
+/**
+ * Fast path (use SetIntArrayRegion).
+ */
 template <typename T>
 struct ConvertIntArray<T, true> {
-  static jintArray ToJava(JNIEnv* env, ArrayRef<T> arr) {
+  static jintArray ToJava(JNIEnv* env, span<const T> arr) {
     jintArray jarr = env->NewIntArray(arr.size());
-    if (!jarr) return nullptr;
+    if (!jarr) {
+      return nullptr;
+    }
     env->SetIntArrayRegion(jarr, 0, arr.size(),
                            reinterpret_cast<const jint*>(arr.data()));
     return jarr;
@@ -355,69 +411,128 @@
 
 }  // namespace detail
 
-// Convert an ArrayRef to a jintArray.
+/**
+ * Convert a span to a jintArray.
+ *
+ * @param env JRE environment.
+ * @param arr Span to convert.
+ */
 template <typename T>
-inline jintArray MakeJIntArray(JNIEnv* env, ArrayRef<T> arr) {
+inline jintArray MakeJIntArray(JNIEnv* env, span<const T> arr) {
   return detail::ConvertIntArray<T>::ToJava(env, arr);
 }
 
-// Convert a SmallVector to a jintArray.  This is required in addition to
-// ArrayRef because template resolution occurs prior to implicit conversions.
+/**
+ * Convert a span to a jintArray.
+ *
+ * @param env JRE environment.
+ * @param arr Span to convert.
+ */
+template <typename T>
+inline jintArray MakeJIntArray(JNIEnv* env, span<T> arr) {
+  return detail::ConvertIntArray<T>::ToJava(env, arr);
+}
+
+/**
+ * Convert a SmallVector to a jintArray.
+ *
+ * This is required in addition to ArrayRef because template resolution occurs
+ * prior to implicit conversions.
+ *
+ * @param env JRE environment.
+ * @param arr SmallVector to convert.
+ */
 template <typename T>
 inline jintArray MakeJIntArray(JNIEnv* env, const SmallVectorImpl<T>& arr) {
   return detail::ConvertIntArray<T>::ToJava(env, arr);
 }
 
-// Convert a std::vector to a jintArray.  This is required in addition to
-// ArrayRef because template resolution occurs prior to implicit conversions.
+/**
+ * Convert a std::vector to a jintArray.
+ *
+ * This is required in addition to ArrayRef because template resolution occurs
+ * prior to implicit conversions.
+ *
+ * @param env JRE environment.
+ * @param arr SmallVector to convert.
+ */
 template <typename T>
 inline jintArray MakeJIntArray(JNIEnv* env, const std::vector<T>& arr) {
   return detail::ConvertIntArray<T>::ToJava(env, arr);
 }
 
-// Convert a StringRef into a jbyteArray.
-inline jbyteArray MakeJByteArray(JNIEnv* env, StringRef str) {
+/**
+ * Convert a std::string_view into a jbyteArray.
+ *
+ * @param env JRE environment.
+ * @param str std::string_view to convert.
+ */
+inline jbyteArray MakeJByteArray(JNIEnv* env, std::string_view str) {
   jbyteArray jarr = env->NewByteArray(str.size());
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   env->SetByteArrayRegion(jarr, 0, str.size(),
                           reinterpret_cast<const jbyte*>(str.data()));
   return jarr;
 }
 
-// Convert an array of integers into a jbooleanArray.
-inline jbooleanArray MakeJBooleanArray(JNIEnv* env, ArrayRef<int> arr) {
+/**
+ * Convert an array of integers into a jbooleanArray.
+ *
+ * @param env JRE environment.
+ * @param arr Array to convert.
+ */
+inline jbooleanArray MakeJBooleanArray(JNIEnv* env, span<const int> arr) {
   jbooleanArray jarr = env->NewBooleanArray(arr.size());
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   jboolean* elements =
       static_cast<jboolean*>(env->GetPrimitiveArrayCritical(jarr, nullptr));
-  if (!elements) return nullptr;
-  for (size_t i = 0; i < arr.size(); ++i)
+  if (!elements) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
     elements[i] = arr[i] ? JNI_TRUE : JNI_FALSE;
+  }
   env->ReleasePrimitiveArrayCritical(jarr, elements, 0);
   return jarr;
 }
 
-// Convert an array of booleans into a jbooleanArray.
-inline jbooleanArray MakeJBooleanArray(JNIEnv* env, ArrayRef<bool> arr) {
+/**
+ * Convert an array of booleans into a jbooleanArray.
+ *
+ * @param env JRE environment.
+ * @param arr Array to convert.
+ */
+inline jbooleanArray MakeJBooleanArray(JNIEnv* env, span<const bool> arr) {
   jbooleanArray jarr = env->NewBooleanArray(arr.size());
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   jboolean* elements =
       static_cast<jboolean*>(env->GetPrimitiveArrayCritical(jarr, nullptr));
-  if (!elements) return nullptr;
-  for (size_t i = 0; i < arr.size(); ++i)
+  if (!elements) {
+    return nullptr;
+  }
+  for (size_t i = 0; i < arr.size(); ++i) {
     elements[i] = arr[i] ? JNI_TRUE : JNI_FALSE;
+  }
   env->ReleasePrimitiveArrayCritical(jarr, elements, 0);
   return jarr;
 }
 
 // Other MakeJ*Array conversions.
 
-#define WPI_JNI_MAKEJARRAY(T, F)                                  \
-  inline T##Array MakeJ##F##Array(JNIEnv* env, ArrayRef<T> arr) { \
-    T##Array jarr = env->New##F##Array(arr.size());               \
-    if (!jarr) return nullptr;                                    \
-    env->Set##F##ArrayRegion(jarr, 0, arr.size(), arr.data());    \
-    return jarr;                                                  \
+#define WPI_JNI_MAKEJARRAY(T, F)                                    \
+  inline T##Array MakeJ##F##Array(JNIEnv* env, span<const T> arr) { \
+    T##Array jarr = env->New##F##Array(arr.size());                 \
+    if (!jarr) {                                                    \
+      return nullptr;                                               \
+    }                                                               \
+    env->Set##F##ArrayRegion(jarr, 0, arr.size(), arr.data());      \
+    return jarr;                                                    \
   }
 
 WPI_JNI_MAKEJARRAY(jboolean, Boolean)
@@ -429,12 +544,21 @@
 
 #undef WPI_JNI_MAKEJARRAY
 
-// Convert an array of std::string into a jarray of jstring.
-inline jobjectArray MakeJStringArray(JNIEnv* env, ArrayRef<std::string> arr) {
+/**
+ * Convert an array of std::string into a jarray of jstring.
+ *
+ * @param env JRE environment.
+ * @param arr Array to convert.
+ */
+inline jobjectArray MakeJStringArray(JNIEnv* env, span<const std::string> arr) {
   static JClass stringCls{env, "java/lang/String"};
-  if (!stringCls) return nullptr;
+  if (!stringCls) {
+    return nullptr;
+  }
   jobjectArray jarr = env->NewObjectArray(arr.size(), stringCls, nullptr);
-  if (!jarr) return nullptr;
+  if (!jarr) {
+    return nullptr;
+  }
   for (size_t i = 0; i < arr.size(); ++i) {
     JLocal<jstring> elem{env, MakeJString(env, arr[i])};
     env->SetObjectArrayElement(jarr, i, elem.obj());
@@ -442,25 +566,27 @@
   return jarr;
 }
 
-// Generic callback thread implementation.
-//
-// JNI's AttachCurrentThread() creates a Java Thread object on every
-// invocation, which is both time inefficient and causes issues with Eclipse
-// (which tries to keep a thread list up-to-date and thus gets swamped).
-//
-// Instead, this class attaches just once.  When a hardware notification
-// occurs, a condition variable wakes up this thread and this thread actually
-// makes the call into Java.
-//
-// The template parameter T is the message being passed to the callback, but
-// also needs to provide the following functions:
-//  static JavaVM* GetJVM();
-//  static const char* GetName();
-//  void CallJava(JNIEnv *env, jobject func, jmethodID mid);
+/**
+ * Generic callback thread implementation.
+ *
+ * JNI's AttachCurrentThread() creates a Java Thread object on every
+ * invocation, which is both time inefficient and causes issues with Eclipse
+ * (which tries to keep a thread list up-to-date and thus gets swamped).
+ *
+ * Instead, this class attaches just once.  When a hardware notification
+ * occurs, a condition variable wakes up this thread and this thread actually
+ * makes the call into Java.
+ *
+ * The template parameter T is the message being passed to the callback, but
+ * also needs to provide the following functions:
+ *  static JavaVM* GetJVM();
+ *  static const char* GetName();
+ *  void CallJava(JNIEnv *env, jobject func, jmethodID mid);
+ */
 template <typename T>
 class JCallbackThread : public SafeThread {
  public:
-  void Main();
+  void Main() override;
 
   std::queue<T> m_queue;
   jobject m_func = nullptr;
@@ -480,9 +606,13 @@
 template <typename T>
 void JCallbackManager<T>::SetFunc(JNIEnv* env, jobject func, jmethodID mid) {
   auto thr = this->GetThread();
-  if (!thr) return;
+  if (!thr) {
+    return;
+  }
   // free global reference
-  if (thr->m_func) env->DeleteGlobalRef(thr->m_func);
+  if (thr->m_func) {
+    env->DeleteGlobalRef(thr->m_func);
+  }
   // create global reference
   thr->m_func = env->NewGlobalRef(func);
   thr->m_mid = mid;
@@ -492,7 +622,9 @@
 template <typename... Args>
 void JCallbackManager<T>::Send(Args&&... args) {
   auto thr = this->GetThread();
-  if (!thr) return;
+  if (!thr) {
+    return;
+  }
   thr->m_queue.emplace(std::forward<Args>(args)...);
   thr->m_cond.notify_one();
 }
@@ -506,14 +638,20 @@
   args.group = nullptr;
   jint rs = T::GetJVM()->AttachCurrentThreadAsDaemon(
       reinterpret_cast<void**>(&env), &args);
-  if (rs != JNI_OK) return;
+  if (rs != JNI_OK) {
+    return;
+  }
 
   std::unique_lock lock(m_mutex);
   while (m_active) {
     m_cond.wait(lock, [&] { return !(m_active && m_queue.empty()); });
-    if (!m_active) break;
+    if (!m_active) {
+      break;
+    }
     while (!m_queue.empty()) {
-      if (!m_active) break;
+      if (!m_active) {
+        break;
+      }
       auto item = std::move(m_queue.front());
       m_queue.pop();
       auto func = m_func;
@@ -529,7 +667,9 @@
   }
 
   JavaVM* jvm = T::GetJVM();
-  if (jvm) jvm->DetachCurrentThread();
+  if (jvm) {
+    jvm->DetachCurrentThread();
+  }
 }
 
 template <typename T>
@@ -541,41 +681,125 @@
   }
 };
 
-inline std::string GetJavaStackTrace(JNIEnv* env, std::string* func,
-                                     StringRef excludeFuncPrefix) {
+inline std::string GetJavaStackTrace(JNIEnv* env, std::string_view skipPrefix) {
   // create a throwable
   static JClass throwableCls(env, "java/lang/Throwable");
-  if (!throwableCls) return "";
+  if (!throwableCls) {
+    return "";
+  }
   static jmethodID constructorId = nullptr;
-  if (!constructorId)
+  if (!constructorId) {
     constructorId = env->GetMethodID(throwableCls, "<init>", "()V");
+  }
   JLocal<jobject> throwable(env, env->NewObject(throwableCls, constructorId));
 
   // retrieve information from the exception.
   // get method id
   // getStackTrace returns an array of StackTraceElement
   static jmethodID getStackTraceId = nullptr;
-  if (!getStackTraceId)
+  if (!getStackTraceId) {
     getStackTraceId = env->GetMethodID(throwableCls, "getStackTrace",
                                        "()[Ljava/lang/StackTraceElement;");
+  }
 
   // call getStackTrace
   JLocal<jobjectArray> stackTrace(
       env, static_cast<jobjectArray>(
                env->CallObjectMethod(throwable, getStackTraceId)));
 
-  if (!stackTrace) return "";
+  if (!stackTrace) {
+    return "";
+  }
 
   // get length of the array
   jsize stackTraceLength = env->GetArrayLength(stackTrace);
 
   // get toString methodId of StackTraceElement class
   static JClass stackTraceElementCls(env, "java/lang/StackTraceElement");
-  if (!stackTraceElementCls) return "";
+  if (!stackTraceElementCls) {
+    return "";
+  }
   static jmethodID toStringId = nullptr;
-  if (!toStringId)
+  if (!toStringId) {
     toStringId = env->GetMethodID(stackTraceElementCls, "toString",
                                   "()Ljava/lang/String;");
+  }
+
+  bool foundFirst = false;
+  std::string buf;
+  raw_string_ostream oss(buf);
+  for (jsize i = 0; i < stackTraceLength; i++) {
+    // add the result of toString method of each element in the result
+    JLocal<jobject> curStackTraceElement(
+        env, env->GetObjectArrayElement(stackTrace, i));
+
+    // call to string on the object
+    JLocal<jstring> stackElementString(
+        env, static_cast<jstring>(
+                 env->CallObjectMethod(curStackTraceElement, toStringId)));
+
+    if (!stackElementString) {
+      return "";
+    }
+
+    // add a line to res
+    JStringRef elem(env, stackElementString);
+    if (!foundFirst) {
+      if (wpi::starts_with(elem, skipPrefix)) {
+        continue;
+      }
+      foundFirst = true;
+    }
+    oss << "\tat " << elem << '\n';
+  }
+
+  return oss.str();
+}
+
+inline std::string GetJavaStackTrace(JNIEnv* env, std::string* func,
+                                     std::string_view excludeFuncPrefix) {
+  // create a throwable
+  static JClass throwableCls(env, "java/lang/Throwable");
+  if (!throwableCls) {
+    return "";
+  }
+  static jmethodID constructorId = nullptr;
+  if (!constructorId) {
+    constructorId = env->GetMethodID(throwableCls, "<init>", "()V");
+  }
+  JLocal<jobject> throwable(env, env->NewObject(throwableCls, constructorId));
+
+  // retrieve information from the exception.
+  // get method id
+  // getStackTrace returns an array of StackTraceElement
+  static jmethodID getStackTraceId = nullptr;
+  if (!getStackTraceId) {
+    getStackTraceId = env->GetMethodID(throwableCls, "getStackTrace",
+                                       "()[Ljava/lang/StackTraceElement;");
+  }
+
+  // call getStackTrace
+  JLocal<jobjectArray> stackTrace(
+      env, static_cast<jobjectArray>(
+               env->CallObjectMethod(throwable, getStackTraceId)));
+
+  if (!stackTrace) {
+    return "";
+  }
+
+  // get length of the array
+  jsize stackTraceLength = env->GetArrayLength(stackTrace);
+
+  // get toString methodId of StackTraceElement class
+  static JClass stackTraceElementCls(env, "java/lang/StackTraceElement");
+  if (!stackTraceElementCls) {
+    return "";
+  }
+  static jmethodID toStringId = nullptr;
+  if (!toStringId) {
+    toStringId = env->GetMethodID(stackTraceElementCls, "toString",
+                                  "()Ljava/lang/String;");
+  }
 
   bool haveLoc = false;
   std::string buf;
@@ -590,7 +814,9 @@
         env, static_cast<jstring>(
                  env->CallObjectMethod(curStackTraceElement, toStringId)));
 
-    if (!stackElementString) return "";
+    if (!stackElementString) {
+      return "";
+    }
 
     // add a line to res
     JStringRef elem(env, stackElementString);
@@ -602,7 +828,7 @@
       if (i == 1) {
         *func = elem.str();
       } else if (i > 1 && !haveLoc && !excludeFuncPrefix.empty() &&
-                 !elem.str().startswith(excludeFuncPrefix)) {
+                 !wpi::starts_with(elem, excludeFuncPrefix)) {
         *func = elem.str();
         haveLoc = true;
       }
@@ -612,17 +838,21 @@
   return oss.str();
 }
 
-// Finds an exception class and keep it as a global reference.
-// Similar to JClass, but provides Throw methods.
-// Use with caution, as the destructor does NOT call DeleteGlobalRef due
-// to potential shutdown issues with doing so.
+/**
+ * Finds an exception class and keep it as a global reference.
+ *
+ * Similar to JClass, but provides Throw methods. Use with caution, as the
+ * destructor does NOT call DeleteGlobalRef due to potential shutdown issues
+ * with doing so.
+ */
 class JException : public JClass {
  public:
   JException() = default;
   JException(JNIEnv* env, const char* name) : JClass(env, name) {
-    if (m_cls)
+    if (m_cls) {
       m_constructor =
           env->GetMethodID(m_cls, "<init>", "(Ljava/lang/String;)V");
+    }
   }
 
   void Throw(JNIEnv* env, jstring msg) {
@@ -630,7 +860,9 @@
     env->Throw(static_cast<jthrowable>(exception));
   }
 
-  void Throw(JNIEnv* env, StringRef msg) { Throw(env, MakeJString(env, msg)); }
+  void Throw(JNIEnv* env, std::string_view msg) {
+    Throw(env, MakeJString(env, msg));
+  }
 
   explicit operator bool() const { return m_constructor; }
 
@@ -643,7 +875,6 @@
   JException* cls;
 };
 
-}  // namespace java
-}  // namespace wpi
+}  // namespace wpi::java
 
 #endif  // WPIUTIL_WPI_JNI_UTIL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json.h
index a6368b7..1a4e410 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json.h
@@ -54,15 +54,14 @@
 #include <memory> // allocator, shared_ptr, make_shared, addressof
 #include <stdexcept> // runtime_error
 #include <string> // string, char_traits, stoi, to_string
+#include <string_view>
 #include <tuple> // tuple, get, make_tuple
 #include <type_traits>
 #include <utility>
 #include <vector> // vector
 
-#include "wpi/ArrayRef.h"
 #include "wpi/StringMap.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
+#include "wpi/span.h"
 
 namespace wpi
 {
@@ -239,7 +238,7 @@
 struct is_compatible_object_type_impl<true, RealType, CompatibleObjectType>
 {
     static constexpr auto value =
-        std::is_constructible<StringRef, typename CompatibleObjectType::key_type>::value and
+        std::is_constructible<std::string_view, typename CompatibleObjectType::key_type>::value and
         std::is_constructible<typename RealType::mapped_type, typename CompatibleObjectType::mapped_type>::value;
 };
 
@@ -269,7 +268,7 @@
         std::conjunction<std::negation<std::is_same<void, CompatibleArrayType>>,
         std::negation<is_compatible_object_type<
         BasicJsonType, CompatibleArrayType>>,
-        std::negation<std::is_constructible<StringRef,
+        std::negation<std::is_constructible<std::string_view,
         CompatibleArrayType>>,
         std::negation<is_json_nested_type<BasicJsonType, CompatibleArrayType>>,
         has_value_type<CompatibleArrayType>,
@@ -423,7 +422,7 @@
     const int id;
 
   protected:
-    exception(int id_, const Twine& what_arg);
+    exception(int id_, std::string_view what_arg);
 
   private:
     /// an exception object as storage for error messages
@@ -484,7 +483,7 @@
     @param[in] what_arg  the explanatory string
     @return parse_error object
     */
-    static parse_error create(int id_, std::size_t byte_, const Twine& what_arg);
+    static parse_error create(int id_, std::size_t byte_, std::string_view what_arg);
 
     /*!
     @brief byte index of the parse error
@@ -498,7 +497,7 @@
     const std::size_t byte;
 
   private:
-    parse_error(int id_, std::size_t byte_, const Twine& what_arg)
+    parse_error(int id_, std::size_t byte_, std::string_view what_arg)
         : exception(id_, what_arg), byte(byte_) {}
 };
 
@@ -542,10 +541,11 @@
 class invalid_iterator : public exception
 {
   public:
-    static invalid_iterator create(int id_, const Twine& what_arg);
+    static invalid_iterator create(int id_, std::string_view what_arg);
+    static invalid_iterator create(int id_, std::string_view what_arg, std::string_view type_info);
 
   private:
-    invalid_iterator(int id_, const Twine& what_arg)
+    invalid_iterator(int id_, std::string_view what_arg)
         : exception(id_, what_arg) {}
 };
 
@@ -590,10 +590,11 @@
 class type_error : public exception
 {
   public:
-    static type_error create(int id_, const Twine& what_arg);
+    static type_error create(int id_, std::string_view what_arg);
+    static type_error create(int id_, std::string_view what_arg, std::string_view type_info);
 
   private:
-    type_error(int id_, const Twine& what_arg) : exception(id_, what_arg) {}
+    type_error(int id_, std::string_view what_arg) : exception(id_, what_arg) {}
 };
 
 /*!
@@ -631,10 +632,10 @@
 class out_of_range : public exception
 {
   public:
-    static out_of_range create(int id_, const Twine& what_arg);
+    static out_of_range create(int id_, std::string_view what_arg);
 
   private:
-    out_of_range(int id_, const Twine& what_arg) : exception(id_, what_arg) {}
+    out_of_range(int id_, std::string_view what_arg) : exception(id_, what_arg) {}
 };
 
 /*!
@@ -664,10 +665,10 @@
 class other_error : public exception
 {
   public:
-    static other_error create(int id_, const Twine& what_arg);
+    static other_error create(int id_, std::string_view what_arg);
 
   private:
-    other_error(int id_, const Twine& what_arg) : exception(id_, what_arg) {}
+    other_error(int id_, std::string_view what_arg) : exception(id_, what_arg) {}
 };
 
 ///////////////////////////
@@ -760,7 +761,7 @@
         }
 
         default:
-            JSON_THROW(type_error::create(302, "type must be number, but is " + Twine(j.type_name())));
+            JSON_THROW(type_error::create(302, "type must be number, but is", j.type_name()));
     }
 }
 
@@ -769,7 +770,7 @@
 {
     if (JSON_UNLIKELY(not j.is_boolean()))
     {
-        JSON_THROW(type_error::create(302, "type must be boolean, but is " + Twine(j.type_name())));
+        JSON_THROW(type_error::create(302, "type must be boolean, but is", j.type_name()));
     }
     b = *j.template get_ptr<const bool*>();
 }
@@ -779,7 +780,7 @@
 {
     if (JSON_UNLIKELY(not j.is_string()))
     {
-        JSON_THROW(type_error::create(302, "type must be string, but is " + Twine(j.type_name())));
+        JSON_THROW(type_error::create(302, "type must be string, but is", j.type_name()));
     }
     s = *j.template get_ptr<const std::string*>();
 }
@@ -816,7 +817,7 @@
 {
     if (JSON_UNLIKELY(not j.is_array()))
     {
-        JSON_THROW(type_error::create(302, "type must be array, but is " + Twine(j.type_name())));
+        JSON_THROW(type_error::create(302, "type must be array, but is", j.type_name()));
     }
     arr = *j.template get_ptr<const typename BasicJsonType::array_t*>();
 }
@@ -875,8 +876,7 @@
 {
     if (JSON_UNLIKELY(not j.is_array()))
     {
-        JSON_THROW(type_error::create(302, "type must be array, but is " +
-                                      Twine(j.type_name())));
+        JSON_THROW(type_error::create(302, "type must be array, but is", j.type_name()));
     }
 
     from_json_array_impl(j, arr, priority_tag<2> {});
@@ -888,7 +888,7 @@
 {
     if (!j.is_object())
     {
-        JSON_THROW(type_error::create(302, "type must be object, but is " + Twine(j.type_name())));
+        JSON_THROW(type_error::create(302, "type must be object, but is", j.type_name()));
     }
 
     auto inner_object = j.template get_ptr<const typename BasicJsonType::object_t*>();
@@ -904,7 +904,7 @@
 {
     if (JSON_UNLIKELY(not j.is_object()))
     {
-        JSON_THROW(type_error::create(302, "type must be object, but is " + Twine(j.type_name())));
+        JSON_THROW(type_error::create(302, "type must be object, but is", j.type_name()));
     }
 
     auto inner_object = j.template get_ptr<const typename BasicJsonType::object_t*>();
@@ -965,7 +965,7 @@
         }
 
         default:
-            JSON_THROW(type_error::create(302, "type must be number, but is " + Twine(j.type_name())));
+            JSON_THROW(type_error::create(302, "type must be number, but is", j.type_name()));
     }
 }
 
@@ -1053,7 +1053,7 @@
 struct external_constructor<value_t::string>
 {
     template<typename BasicJsonType>
-    static void construct(BasicJsonType& j, StringRef s)
+    static void construct(BasicJsonType& j, std::string_view s)
     {
         j.m_type = value_t::string;
         j.m_value = s;
@@ -1126,7 +1126,7 @@
     }
 
     template<typename BasicJsonType, typename T>
-    static void construct(BasicJsonType& j, ArrayRef<T> arr)
+    static void construct(BasicJsonType& j, span<T> arr)
     {
         using std::begin;
         using std::end;
@@ -1206,7 +1206,7 @@
 }
 
 template<typename BasicJsonType, typename CompatibleString,
-         enable_if_t<std::is_constructible<StringRef, CompatibleString>::value, int> = 0>
+         enable_if_t<std::is_constructible<std::string_view, CompatibleString>::value, int> = 0>
 void to_json(BasicJsonType& j, const CompatibleString& s)
 {
     external_constructor<value_t::string>::construct(j, s);
@@ -1283,7 +1283,7 @@
 }
 
 template<typename BasicJsonType, typename T, std::size_t N,
-         enable_if_t<not std::is_constructible<StringRef, T (&)[N]>::value, int> = 0>
+         enable_if_t<not std::is_constructible<std::string_view, T (&)[N]>::value, int> = 0>
 void to_json(BasicJsonType& j, T (&arr)[N])
 {
     external_constructor<value_t::array>::construct(j, arr);
@@ -2044,7 +2044,7 @@
     @brief  return the key of an object iterator
     @pre The iterator is initialized; i.e. `m_object != nullptr`.
     */
-    StringRef key() const
+    std::string_view key() const
     {
         assert(m_object != nullptr);
 
@@ -2122,7 +2122,7 @@
 
                 // use key from the object
                 case value_t::object:
-                    return anchor.key();
+                    return std::string{anchor.key()};
 
                 // use an empty key for all primitive types
                 default:
@@ -2348,7 +2348,7 @@
 
     @since version 2.0.0
     */
-    explicit json_pointer(const Twine& s = {})
+    explicit json_pointer(std::string_view s = {})
         : reference_tokens(split(s))
     {}
 
@@ -2382,7 +2382,7 @@
 
     @throw out_of_range.404 if string @a s could not be converted to an integer
     */
-    static int array_index(const Twine& s);
+    static int array_index(std::string_view s);
 
   private:
     /*!
@@ -2490,7 +2490,7 @@
     @throw parse_error.107  if the pointer is not empty or begins with '/'
     @throw parse_error.108  if character '~' is not followed by '0' or '1'
     */
-    static std::vector<std::string> split(const Twine& reference_string);
+    static std::vector<std::string> split(std::string_view reference_string);
 
     /*!
     @brief replace all occurrences of a substring by another string
@@ -2521,7 +2521,7 @@
 
     @note Empty objects or arrays are flattened to `null`.
     */
-    static void flatten(const Twine& reference_string,
+    static void flatten(std::string_view reference_string,
                         const json& value,
                         json& result);
 
@@ -2678,9 +2678,10 @@
     class binary_writer;
     class lexer;
     class parser;
-    class serializer;
 
   public:
+    class serializer;
+
     using value_t = detail::value_t;
     /// @copydoc wpi::json_pointer
     using json_pointer = ::wpi::json_pointer;
@@ -2970,7 +2971,7 @@
         json_value(value_t t);
 
         /// constructor for strings
-        json_value(StringRef value)
+        json_value(std::string_view value)
         {
             string = create<std::string>(value);
         }
@@ -3576,8 +3577,7 @@
             }
 
             default:
-                JSON_THROW(invalid_iterator::create(206, "cannot construct with iterators from " +
-                                                    Twine(first.m_object->type_name())));
+                JSON_THROW(invalid_iterator::create(206, "cannot construct with iterators from", first.m_object->type_name()));
         }
 
         assert_invariant();
@@ -4162,7 +4162,7 @@
             return m_value.boolean;
         }
 
-        JSON_THROW(type_error::create(302, "type must be boolean, but is " + Twine(type_name())));
+        JSON_THROW(type_error::create(302, "type must be boolean, but is", type_name()));
     }
 
     /// get a pointer to the value (object)
@@ -4271,7 +4271,7 @@
             return *ptr;
         }
 
-        JSON_THROW(type_error::create(303, "incompatible ReferenceType for get_ref, actual type is " + Twine(obj.type_name())));
+        JSON_THROW(type_error::create(303, "incompatible ReferenceType for get_ref, actual type is", obj.type_name()));
     }
 
   public:
@@ -4716,7 +4716,7 @@
     written using `at()`. It also demonstrates the different exceptions that
     can be thrown.,at__object_t_key_type}
     */
-    reference at(StringRef key);
+    reference at(std::string_view key);
 
     /*!
     @brief access specified object element with bounds checking
@@ -4748,7 +4748,7 @@
     `at()`. It also demonstrates the different exceptions that can be thrown.,
     at__object_t_key_type_const}
     */
-    const_reference at(StringRef key) const;
+    const_reference at(std::string_view key) const;
 
     /*!
     @brief access specified array element
@@ -4825,7 +4825,7 @@
 
     @since version 1.0.0
     */
-    reference operator[](StringRef key);
+    reference operator[](std::string_view key);
 
     /*!
     @brief read-only access specified object element
@@ -4857,7 +4857,7 @@
 
     @since version 1.0.0
     */
-    const_reference operator[](StringRef key) const;
+    const_reference operator[](std::string_view key) const;
 
     /*!
     @brief access specified object element
@@ -4903,7 +4903,7 @@
             return m_value.object->operator[](key);
         }
 
-        JSON_THROW(type_error::create(305, "cannot use operator[] with " + Twine(type_name())));
+        JSON_THROW(type_error::create(305, "cannot use operator[] with", type_name()));
     }
 
     /*!
@@ -4946,7 +4946,7 @@
             return m_value.object->find(key)->second;
         }
 
-        JSON_THROW(type_error::create(305, "cannot use operator[] with " + Twine(type_name())));
+        JSON_THROW(type_error::create(305, "cannot use operator[] with", type_name()));
     }
 
     /*!
@@ -4999,7 +4999,7 @@
     */
     template<class ValueType, typename std::enable_if<
                  std::is_convertible<json_t, ValueType>::value, int>::type = 0>
-    ValueType value(StringRef key, const ValueType& default_value) const
+    ValueType value(std::string_view key, const ValueType& default_value) const
     {
         // at only works for objects
         if (JSON_LIKELY(is_object()))
@@ -5014,14 +5014,14 @@
             return default_value;
         }
 
-        JSON_THROW(type_error::create(306, "cannot use value() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(306, "cannot use value() with", type_name()));
     }
 
     /*!
     @brief overload for a default value of type const char*
     @copydoc json::value(const typename object_t::key_type&, ValueType) const
     */
-    std::string value(StringRef key, const char* default_value) const
+    std::string value(std::string_view key, const char* default_value) const
     {
         return value(key, std::string(default_value));
     }
@@ -5085,7 +5085,7 @@
             }
         }
 
-        JSON_THROW(type_error::create(306, "cannot use value() with " + Twine(type_name())));
+        JSON_THROW(type_error::create(306, "cannot use value() with", type_name()));
     }
 
     /*!
@@ -5220,7 +5220,7 @@
 
     @sa @ref erase(IteratorType, IteratorType) -- removes the elements in
     the given range
-    @sa @ref erase(StringRef) -- removes the element
+    @sa @ref erase(std::string_view) -- removes the element
     from an object at the given key
     @sa @ref erase(const size_type) -- removes the element from an array at
     the given index
@@ -5278,7 +5278,7 @@
             }
 
             default:
-                JSON_THROW(type_error::create(307, "cannot use erase() with " + Twine(type_name())));
+                JSON_THROW(type_error::create(307, "cannot use erase() with", type_name()));
         }
     }
 
@@ -5377,7 +5377,7 @@
             }
 
             default:
-                JSON_THROW(type_error::create(307, "cannot use erase() with " + Twine(type_name())));
+                JSON_THROW(type_error::create(307, "cannot use erase() with", type_name()));
         }
 
         return result;
@@ -5412,7 +5412,7 @@
 
     @since version 1.0.0
     */
-    size_type erase(StringRef key);
+    size_type erase(std::string_view key);
 
     /*!
     @brief remove element from a JSON array given an index
@@ -5472,13 +5472,13 @@
 
     @since version 1.0.0
     */
-    iterator find(StringRef key);
+    iterator find(std::string_view key);
 
     /*!
     @brief find an element in a JSON object
     @copydoc find(KeyT&&)
     */
-    const_iterator find(StringRef key) const;
+    const_iterator find(std::string_view key) const;
 
     /*!
     @brief returns the number of occurrences of a key in a JSON object
@@ -5501,7 +5501,7 @@
 
     @since version 1.0.0
     */
-    size_type count(StringRef key) const;
+    size_type count(std::string_view key) const;
 
     /// @}
 
@@ -6113,7 +6113,7 @@
         // push_back only works for null objects or objects
         if (JSON_UNLIKELY(not(is_null() or is_object())))
         {
-            JSON_THROW(type_error::create(308, "cannot use push_back() with " + Twine(type_name())));
+            JSON_THROW(type_error::create(308, "cannot use push_back() with", type_name()));
         }
 
         // transform null object into an object
@@ -6203,7 +6203,7 @@
         // emplace_back only works for null objects or arrays
         if (JSON_UNLIKELY(not(is_null() or is_array())))
         {
-            JSON_THROW(type_error::create(311, "cannot use emplace_back() with " + Twine(type_name())));
+            JSON_THROW(type_error::create(311, "cannot use emplace_back() with", type_name()));
         }
 
         // transform null object into an array
@@ -6246,12 +6246,12 @@
     @since version 2.0.8
     */
     template<class... Args>
-    std::pair<iterator, bool> emplace(StringRef key, Args&& ... args)
+    std::pair<iterator, bool> emplace(std::string_view key, Args&& ... args)
     {
         // emplace only works for null objects or arrays
         if (JSON_UNLIKELY(not(is_null() or is_object())))
         {
-            JSON_THROW(type_error::create(311, "cannot use emplace() with " + Twine(type_name())));
+            JSON_THROW(type_error::create(311, "cannot use emplace() with", type_name()));
         }
 
         // transform null object into an object
@@ -6521,7 +6521,7 @@
         }
         else
         {
-            JSON_THROW(type_error::create(310, "cannot use swap() with " + Twine(type_name())));
+            JSON_THROW(type_error::create(310, "cannot use swap() with", type_name()));
         }
     }
 
@@ -6554,7 +6554,7 @@
         }
         else
         {
-            JSON_THROW(type_error::create(310, "cannot use swap() with " + Twine(type_name())));
+            JSON_THROW(type_error::create(310, "cannot use swap() with", type_name()));
         }
     }
 
@@ -6587,7 +6587,7 @@
         }
         else
         {
-            JSON_THROW(type_error::create(310, "cannot use swap() with " + Twine(type_name())));
+            JSON_THROW(type_error::create(310, "cannot use swap() with", type_name()));
         }
     }
 
@@ -7011,11 +7011,11 @@
 
     @since version 2.0.3 (contiguous containers)
     */
-    static json parse(StringRef s,
+    static json parse(std::string_view s,
                             const parser_callback_t cb = nullptr,
                             const bool allow_exceptions = true);
 
-    static json parse(ArrayRef<uint8_t> arr,
+    static json parse(span<const uint8_t> arr,
                             const parser_callback_t cb = nullptr,
                             const bool allow_exceptions = true);
 
@@ -7026,9 +7026,9 @@
                             const parser_callback_t cb = nullptr,
                             const bool allow_exceptions = true);
 
-    static bool accept(StringRef s);
+    static bool accept(std::string_view s);
 
-    static bool accept(ArrayRef<uint8_t> arr);
+    static bool accept(span<const uint8_t> arr);
 
     static bool accept(raw_istream& i);
 
@@ -7206,8 +7206,8 @@
     @since version 2.0.9
     */
     static std::vector<uint8_t> to_cbor(const json& j);
-    static ArrayRef<uint8_t> to_cbor(const json& j, std::vector<uint8_t>& buf);
-    static ArrayRef<uint8_t> to_cbor(const json& j, SmallVectorImpl<uint8_t>& buf);
+    static span<uint8_t> to_cbor(const json& j, std::vector<uint8_t>& buf);
+    static span<uint8_t> to_cbor(const json& j, SmallVectorImpl<uint8_t>& buf);
     static void to_cbor(raw_ostream& os, const json& j);
 
     /*!
@@ -7291,8 +7291,8 @@
     @since version 2.0.9
     */
     static std::vector<uint8_t> to_msgpack(const json& j);
-    static ArrayRef<uint8_t> to_msgpack(const json& j, std::vector<uint8_t>& buf);
-    static ArrayRef<uint8_t> to_msgpack(const json& j, SmallVectorImpl<uint8_t>& buf);
+    static span<uint8_t> to_msgpack(const json& j, std::vector<uint8_t>& buf);
+    static span<uint8_t> to_msgpack(const json& j, SmallVectorImpl<uint8_t>& buf);
     static void to_msgpack(raw_ostream& os, const json& j);
 
     /*!
@@ -7378,10 +7378,10 @@
     static std::vector<uint8_t> to_ubjson(const json& j,
                                           const bool use_size = false,
                                           const bool use_type = false);
-    static ArrayRef<uint8_t> to_ubjson(const json& j, std::vector<uint8_t>& buf,
-                                       const bool use_size = false, const bool use_type = false);
-    static ArrayRef<uint8_t> to_ubjson(const json& j, SmallVectorImpl<uint8_t>& buf,
-                                       const bool use_size = false, const bool use_type = false);
+    static span<uint8_t> to_ubjson(const json& j, std::vector<uint8_t>& buf,
+                                   const bool use_size = false, const bool use_type = false);
+    static span<uint8_t> to_ubjson(const json& j, SmallVectorImpl<uint8_t>& buf,
+                                   const bool use_size = false, const bool use_type = false);
     static void to_ubjson(raw_ostream& os, const json& j,
                           const bool use_size = false, const bool use_type = false);
 
@@ -7484,7 +7484,7 @@
     /*!
     @copydoc from_cbor(raw_istream&, const bool)
     */
-    static json from_cbor(ArrayRef<uint8_t> arr, const bool strict = true);
+    static json from_cbor(span<const uint8_t> arr, const bool strict = true);
 
     /*!
     @brief create a JSON value from an input in MessagePack format
@@ -7563,9 +7563,9 @@
                                    const bool strict = true);
 
     /*!
-    @copydoc from_msgpack(raw_istream, const bool)
+    @copydoc from_msgpack(raw_istream&, const bool)
     */
-    static json from_msgpack(ArrayRef<uint8_t> arr, const bool strict = true);
+    static json from_msgpack(span<const uint8_t> arr, const bool strict = true);
 
     /*!
     @brief create a JSON value from an input in UBJSON format
@@ -7623,7 +7623,7 @@
     static json from_ubjson(raw_istream& is,
                                   const bool strict = true);
 
-    static json from_ubjson(ArrayRef<uint8_t> arr, const bool strict = true);
+    static json from_ubjson(span<const uint8_t> arr, const bool strict = true);
 
     /// @}
 
@@ -8074,7 +8074,7 @@
 */
 inline wpi::json operator "" _json(const char* s, std::size_t n)
 {
-    return wpi::json::parse(wpi::StringRef(s, n));
+    return wpi::json::parse(std::string_view(s, n));
 }
 
 /*!
@@ -8092,7 +8092,7 @@
 */
 inline wpi::json::json_pointer operator "" _json_pointer(const char* s, std::size_t n)
 {
-    return wpi::json::json_pointer(wpi::StringRef(s, n));
+    return wpi::json::json_pointer(std::string_view(s, n));
 }
 
 #ifndef WPI_JSON_IMPLEMENTATION
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json_serializer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json_serializer.h
new file mode 100644
index 0000000..3796c95
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/json_serializer.h
@@ -0,0 +1,207 @@
+/*----------------------------------------------------------------------------*/
+/* Modifications Copyright (c) 2017-2018 FIRST. All Rights Reserved.          */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+/*
+    __ _____ _____ _____
+ __|  |   __|     |   | |  JSON for Modern C++
+|  |  |__   |  |  | | | |  version 3.1.2
+|_____|_____|_____|_|___|  https://github.com/nlohmann/json
+
+Licensed under the MIT License <http://opensource.org/licenses/MIT>.
+Copyright (c) 2013-2018 Niels Lohmann <http://nlohmann.me>.
+
+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 "wpi/json.h"
+
+#include <clocale> // lconv, localeconv
+#include <cmath>  // labs, isfinite, isnan, signbit, ldexp
+#include <type_traits>
+
+#include "wpi/raw_ostream.h"
+
+namespace wpi {
+
+class json::serializer
+{
+    static constexpr uint8_t UTF8_ACCEPT = 0;
+    static constexpr uint8_t UTF8_REJECT = 1;
+
+  public:
+    /*!
+    @param[in] s  output stream to serialize to
+    @param[in] ichar  indentation character to use
+    @param[in] indent_init_len  initial length of indentation string buffer
+    */
+    serializer(raw_ostream& s, const char ichar, size_t indent_init_len = 512)
+        : o(s), indent_char(ichar),
+          indent_string(indent_init_len, indent_char)
+    {}
+
+    // delete because of pointer members
+    serializer(const serializer&) = delete;
+    serializer& operator=(const serializer&) = delete;
+
+    /*!
+    @brief internal implementation of the serialization function
+
+    This function is called by the public member function dump and organizes
+    the serialization internally. The indentation level is propagated as
+    additional parameter. In case of arrays and objects, the function is
+    called recursively.
+
+    - strings and object keys are escaped using `escape_string()`
+    - integer numbers are converted implicitly via `operator<<`
+    - floating-point numbers are converted to a string using `"%g"` format
+
+    @param[in] val             value to serialize
+    @param[in] pretty_print    whether the output shall be pretty-printed
+    @param[in] ensure_ascii    whether the output shall only use ASCII chars
+    @param[in] indent_step     the indent level
+    @param[in] current_indent  the current indent level (only used internally)
+    */
+    void dump(const json& val, const bool pretty_print,
+              const bool ensure_ascii,
+              const unsigned int indent_step,
+              const unsigned int current_indent = 0);
+
+    /*!
+    @brief dump escaped string
+
+    Escape a string by replacing certain special characters by a sequence of an
+    escape character (backslash) and another character and other control
+    characters by a sequence of "\u" followed by a four-digit hex
+    representation. The escaped string is written to output stream @a o.
+
+    @param[in] s  the string to escape
+    @param[in] ensure_ascii  whether to escape non-ASCII characters with
+                             "\uXXXX" sequences
+
+    Complexity: Linear in the length of string @a s.
+    */
+    void dump_escaped(std::string_view s, const bool ensure_ascii);
+
+    template <typename NumberType,
+              detail::enable_if_t<std::is_same_v<NumberType, uint64_t>, int> = 0>
+    bool is_negative_integer(NumberType x) {
+      return false;
+    }
+
+    template <typename NumberType,
+              detail::enable_if_t<std::is_same_v<NumberType, int64_t>, int> = 0>
+    bool is_negative_integer(NumberType x) {
+      return x < 0;
+    }
+
+    /*!
+    @brief dump an integer
+
+    Dump a given integer to output stream @a o. Works internally with
+    @a number_buffer.
+
+    @param[in] x  integer number (signed or unsigned) to dump
+    @tparam NumberType either @a int64_t or @a uint64_t
+    */
+    template<typename NumberType, detail::enable_if_t<
+                 std::is_same<NumberType, uint64_t>::value or
+                 std::is_same<NumberType, int64_t>::value,
+                 int> = 0>
+    void dump_integer(NumberType x)
+    {
+        // special case for "0"
+        if (x == 0)
+        {
+            o << '0';
+            return;
+        }
+
+        const bool is_negative = is_negative_integer(x);  // see issue #755
+        std::size_t i = 0;
+
+        while (x != 0)
+        {
+            // spare 1 byte for '\0'
+            assert(i < number_buffer.size() - 1);
+
+            const auto digit = std::labs(static_cast<long>(x % 10));
+            number_buffer[i++] = static_cast<char>('0' + digit);
+            x /= 10;
+        }
+
+        if (is_negative)
+        {
+            // make sure there is capacity for the '-'
+            assert(i < number_buffer.size() - 2);
+            number_buffer[i++] = '-';
+        }
+
+        std::reverse(number_buffer.begin(), number_buffer.begin() + i);
+        o.write(number_buffer.data(), i);
+    }
+
+    /*!
+    @brief dump a floating-point number
+
+    Dump a given floating-point number to output stream @a o. Works internally
+    with @a number_buffer.
+
+    @param[in] x  floating-point number to dump
+    */
+    void dump_float(double x);
+
+    /*!
+    @brief check whether a string is UTF-8 encoded
+
+    The function checks each byte of a string whether it is UTF-8 encoded. The
+    result of the check is stored in the @a state parameter. The function must
+    be called initially with state 0 (accept). State 1 means the string must
+    be rejected, because the current byte is not allowed. If the string is
+    completely processed, but the state is non-zero, the string ended
+    prematurely; that is, the last byte indicated more bytes should have
+    followed.
+
+    @param[in,out] state  the state of the decoding
+    @param[in,out] codep  codepoint (valid only if resulting state is UTF8_ACCEPT)
+    @param[in] byte       next byte to decode
+    @return               new state
+
+    @note The function has been edited: a std::array is used.
+
+    @copyright Copyright (c) 2008-2009 Bjoern Hoehrmann <bjoern@hoehrmann.de>
+    @sa http://bjoern.hoehrmann.de/utf-8/decoder/dfa/
+    */
+    static uint8_t decode(uint8_t& state, uint32_t& codep, const uint8_t byte) noexcept;
+
+  private:
+    /// the output of the serializer
+    raw_ostream& o;
+
+    /// a (hopefully) large enough character buffer
+    std::array<char, 64> number_buffer{{}};
+
+    /// the indentation character
+    const char indent_char;
+    /// the indentation string
+    std::string indent_string;
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/leb128.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/leb128.h
index c70f471..04752c6 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/leb128.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/leb128.h
@@ -1,68 +1,112 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_LEB128_H_
 #define WPIUTIL_WPI_LEB128_H_
 
-#include <cstddef>
+#include <stdint.h>
 
-#include "wpi/SmallVector.h"
+#include <optional>
+
+#include "wpi/span.h"
 
 namespace wpi {
 
+template <typename T>
+class SmallVectorImpl;
 class raw_istream;
+class raw_ostream;
 
 /**
- * Get size of unsigned LEB128 data
- * @val: value
+ * Get size of unsigned LEB128 data.
  *
  * Determine the number of bytes required to encode an unsigned LEB128 datum.
  * The algorithm is taken from Appendix C of the DWARF 3 spec. For information
  * on the encodings refer to section "7.6 - Variable Length Data". Return
  * the number of bytes required.
+ *
+ * @param val LEB128 data.
  */
 uint64_t SizeUleb128(uint64_t val);
 
 /**
- * Write unsigned LEB128 data
- * @addr: the address where the ULEB128 data is to be stored
- * @val: value to be stored
+ * Write unsigned LEB128 data.
  *
  * Encode an unsigned LEB128 encoded datum. The algorithm is taken
  * from Appendix C of the DWARF 3 spec. For information on the
  * encodings refer to section "7.6 - Variable Length Data". Return
  * the number of bytes written.
+ *
+ * @param dest The address where the ULEB128 data is to be stored.
+ * @param val Value to be stored.
  */
 uint64_t WriteUleb128(SmallVectorImpl<char>& dest, uint64_t val);
 
 /**
- * Read unsigned LEB128 data
- * @addr: the address where the ULEB128 data is stored
- * @ret: address to store the result
+ * Write unsigned LEB128 data.
+ *
+ * Encode an unsigned LEB128 encoded datum. The algorithm is taken
+ * from Appendix C of the DWARF 3 spec. For information on the
+ * encodings refer to section "7.6 - Variable Length Data".
+ *
+ * @param os Output stream.
+ * @param val Value to be stored.
+ */
+void WriteUleb128(raw_ostream& os, uint64_t val);
+
+/**
+ * Read unsigned LEB128 data.
  *
  * Decode an unsigned LEB128 encoded datum. The algorithm is taken
  * from Appendix C of the DWARF 3 spec. For information on the
  * encodings refer to section "7.6 - Variable Length Data". Return
  * the number of bytes read.
+ *
+ * @param addr The address where the ULEB128 data is stored.
+ * @param ret Address to store the result.
  */
 uint64_t ReadUleb128(const char* addr, uint64_t* ret);
 
 /**
- * Read unsigned LEB128 data from a stream
- * @is: the input stream where the ULEB128 data is to be read from
- * @ret: address to store the result
+ * Read unsigned LEB128 data from a stream.
  *
  * Decode an unsigned LEB128 encoded datum. The algorithm is taken
  * from Appendix C of the DWARF 3 spec. For information on the
  * encodings refer to section "7.6 - Variable Length Data". Return
  * false on stream error, true on success.
+ *
+ * @param is The input stream where the ULEB128 data is to be read from.
+ * @param ret Address to store the result.
  */
 bool ReadUleb128(raw_istream& is, uint64_t* ret);
 
+/**
+ * Unsigned LEB128 streaming reader.
+ *
+ * Decode an unsigned LEB128 encoded datum. The algorithm is taken
+ * from Appendix C of the DWARF 3 spec. For information on the
+ * encodings refer to section "7.6 - Variable Length Data".
+ */
+class Uleb128Reader {
+ public:
+  /**
+   * Decode a single ULEB128 value.  Returns after a single ULEB128 value has
+   * been read or insufficient input (call in a loop to get multiple values).
+   * If a value is returned, internal state is reset so it's safe to immediately
+   * call this function again to decode another value.
+   *
+   * @param in Input data; modified as data is consumed (any unconsumed data
+   *           is left when function returns).
+   * @return value (in std::optional)
+   */
+  std::optional<uint64_t> ReadOne(span<const uint8_t>* in);
+
+ private:
+  uint64_t m_result = 0;
+  int m_shift = 0;
+};
+
 }  // namespace wpi
 
 #endif  // WPIUTIL_WPI_LEB128_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/math b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/math
index d9c18c7..25f2a64 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/math
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/math
@@ -1,67 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
-#include <type_traits>
+#include "wpi/numbers"
+
+// clang-format off
+#ifdef _MSC_VER
+#pragma message("warning: Use <wpi/numbers> and wpi::numbers instead to reflect C++20 <numbers> and std::numbers")
+#else
+#warning "Use <wpi/numbers> and wpi::numbers instead to reflect C++20 <numbers> and std::numbers"
+#endif
+
+// clang-format on
 
 namespace wpi::math {
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T e_v = 2.718281828459045235360287471352662498L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T log2e_v = 1.442695040888963407359924681001892137L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T log10e_v = 0.434294481903251827651128918916605082L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T pi_v = 3.141592653589793238462643383279502884L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T inv_pi_v = 0.318309886183790671537767526745028724L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T inv_sqrtpi_v = 0.564189583547756286948079451560772586L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T ln2_v = 0.693147180559945309417232121458176568L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T ln10_v = 2.302585092994045684017991454684364208L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T sqrt2_v = 1.414213562373095048801688724209698078L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T sqrt3_v = 1.732050807568877293527446341505872366L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T inv_sqrt3_v = 0.577350269189625764509148780501957456L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T egamma_v = 0.577215664901532860606512090082402431L;
-
-template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
-inline constexpr T phi_v = 1.618033988749894848204586834365638117L;
-
-inline constexpr double e          = e_v<double>;
-inline constexpr double log2e      = log2e_v<double>;
-inline constexpr double log10e     = log10e_v<double>;
-inline constexpr double pi         = pi_v<double>;
-inline constexpr double inv_pi     = inv_pi_v<double>;
-inline constexpr double inv_sqrtpi = inv_sqrtpi_v<double>;
-inline constexpr double ln2        = ln2_v<double>;
-inline constexpr double ln10       = ln10_v<double>;
-inline constexpr double sqrt2      = sqrt2_v<double>;
-inline constexpr double sqrt3      = sqrt3_v<double>;
-inline constexpr double inv_sqrt3  = inv_sqrt3_v<double>;
-inline constexpr double egamma     = egamma_v<double>;
-inline constexpr double phi        = phi_v<double>;
-
+using namespace wpi::numbers;
 }  // namespace wpi::math
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mpack.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mpack.h
new file mode 100644
index 0000000..870c26b
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mpack.h
@@ -0,0 +1,8207 @@
+/**
+ * The MIT License (MIT)
+ * 
+ * Copyright (c) 2015-2021 Nicholas Fraser and the MPack authors
+ * 
+ * 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.
+ * 
+ */
+
+/*
+ * This is the MPack 1.1 amalgamation package.
+ *
+ * http://github.com/ludocode/mpack
+ */
+
+#ifndef MPACK_H
+#define MPACK_H 1
+
+#define MPACK_AMALGAMATED 1
+
+#if defined(MPACK_HAS_CONFIG) && MPACK_HAS_CONFIG
+#include "mpack-config.h"
+#endif
+
+
+/* mpack/mpack-platform.h.h */
+
+/**
+ * @file
+ *
+ * Abstracts all platform-specific code from MPack and handles configuration
+ * options.
+ *
+ * This verifies the configuration and sets defaults based on the platform,
+ * contains implementations of standard C functions when libc is not available,
+ * and provides wrappers to all library functions.
+ *
+ * Documentation for configuration options is available here:
+ *
+ *     https://ludocode.github.io/mpack/group__config.html
+ */
+
+#ifndef MPACK_PLATFORM_H
+#define MPACK_PLATFORM_H 1
+
+
+
+/**
+ * @defgroup config Configuration Options
+ *
+ * Defines the MPack configuration options.
+ *
+ * Custom configuration of MPack is not usually necessary. In almost all
+ * cases you can ignore this and use the defaults.
+ *
+ * If you do want to configure MPack, you can define the below options as part
+ * of your build system or project settings. This will override the below
+ * defaults.
+ *
+ * If you'd like to use a file for configuration instead, define
+ * @ref MPACK_HAS_CONFIG to 1 in your build system or project settings.
+ * This will cause MPack to include a file you create called @c mpack-config.h
+ * in which you can define your configuration. This is useful if you need to
+ * include specific headers (such as a custom allocator) in order to configure
+ * MPack to use it.
+ *
+ * @warning The value of all configuration options must be the same in
+ * all translation units of your project, as well as in the mpack source
+ * itself. These configuration options affect the layout of structs, among
+ * other things, which cannot be different in source files that are linked
+ * together.
+ *
+ * @note MPack does not contain defaults for building inside the Linux kernel.
+ * There is a <a href="https://github.com/ludocode/mpack-linux-kernel">
+ * configuration file for the Linux kernel</a> that can be used instead.
+ *
+ * @{
+ */
+
+
+
+/*
+ * Pre-include checks
+ *
+ * These need to come before the user's mpack-config.h because they might be
+ * including headers in it.
+ */
+
+/** @cond */
+#if defined(_MSC_VER) && _MSC_VER < 1800 && !defined(__cplusplus)
+    #error "In Visual Studio 2012 and earlier, MPack must be compiled as C++. Enable the /Tp compiler flag."
+#endif
+
+#if defined(_WIN32) && MPACK_INTERNAL
+    #define _CRT_SECURE_NO_WARNINGS 1
+#endif
+
+#ifndef __STDC_LIMIT_MACROS
+    #define __STDC_LIMIT_MACROS 1
+#endif
+#ifndef __STDC_FORMAT_MACROS
+    #define __STDC_FORMAT_MACROS 1
+#endif
+#ifndef __STDC_CONSTANT_MACROS
+    #define __STDC_CONSTANT_MACROS 1
+#endif
+/** @endcond */
+
+
+
+/**
+ * @name File Configuration
+ * @{
+ */
+
+/**
+ * @def MPACK_HAS_CONFIG
+ *
+ * Causes MPack to include a file you create called @c mpack-config.h .
+ *
+ * The file is included before MPack sets any defaults for undefined
+ * configuration options. You can use it to configure MPack.
+ *
+ * This is off by default.
+ */
+#if defined(MPACK_HAS_CONFIG)
+    #if MPACK_HAS_CONFIG
+        #include "mpack-config.h"
+    #endif
+#else
+    #define MPACK_HAS_CONFIG 0
+#endif
+
+/**
+ * @}
+ */
+
+// this needs to come first since some stuff depends on it
+/** @cond */
+#ifndef MPACK_NO_BUILTINS
+    #define MPACK_NO_BUILTINS 0
+#endif
+/** @endcond */
+
+
+
+/**
+ * @name Features
+ * @{
+ */
+
+/**
+ * @def MPACK_READER
+ *
+ * Enables compilation of the base Tag Reader.
+ */
+#ifndef MPACK_READER
+#define MPACK_READER 1
+#endif
+
+/**
+ * @def MPACK_EXPECT
+ *
+ * Enables compilation of the static Expect API.
+ */
+#ifndef MPACK_EXPECT
+#define MPACK_EXPECT 1
+#endif
+
+/**
+ * @def MPACK_NODE
+ *
+ * Enables compilation of the dynamic Node API.
+ */
+#ifndef MPACK_NODE
+#define MPACK_NODE 1
+#endif
+
+/**
+ * @def MPACK_WRITER
+ *
+ * Enables compilation of the Writer.
+ */
+#ifndef MPACK_WRITER
+#define MPACK_WRITER 1
+#endif
+
+/**
+ * @def MPACK_BUILDER
+ *
+ * Enables compilation of the Builder.
+ *
+ * The Builder API provides additional functions to the Writer for
+ * automatically determining the element count of compound elements so you do
+ * not have to specify them up-front.
+ *
+ * This requires a @c malloc(). It is enabled by default if MPACK_WRITER is
+ * enabled and MPACK_MALLOC is defined.
+ *
+ * @see mpack_build_map()
+ * @see mpack_build_array()
+ * @see mpack_complete_map()
+ * @see mpack_complete_array()
+ */
+// This is defined furthur below after we've resolved whether we have malloc().
+
+/**
+ * @def MPACK_COMPATIBILITY
+ *
+ * Enables compatibility features for reading and writing older
+ * versions of MessagePack.
+ *
+ * This is disabled by default. When disabled, the behaviour is equivalent to
+ * using the default version, @ref mpack_version_current.
+ *
+ * Enable this if you need to interoperate with applications or data that do
+ * not support the new (v5) MessagePack spec. See the section on v4
+ * compatibility in @ref docs/protocol.md for more information.
+ */
+#ifndef MPACK_COMPATIBILITY
+#define MPACK_COMPATIBILITY 0
+#endif
+
+/**
+ * @def MPACK_EXTENSIONS
+ *
+ * Enables the use of extension types.
+ *
+ * This is disabled by default. Define it to 1 to enable it. If disabled,
+ * functions to read and write extensions will not exist, and any occurrence of
+ * extension types in parsed messages will flag @ref mpack_error_invalid.
+ *
+ * MPack discourages the use of extension types. See the section on extension
+ * types in @ref docs/protocol.md for more information.
+ */
+#ifndef MPACK_EXTENSIONS
+#define MPACK_EXTENSIONS 0
+#endif
+
+/**
+ * @}
+ */
+
+
+
+// workarounds for Doxygen
+#if defined(MPACK_DOXYGEN)
+#if MPACK_DOXYGEN
+// We give these their default values of 0 here even though they are defined to
+// 1 in the doxyfile. Doxygen will show this as the value in the docs, even
+// though it ignores it when parsing the rest of the source. This is what we
+// want, since we want the documentation to show these defaults but still
+// generate documentation for the functions they add when they're on.
+#define MPACK_COMPATIBILITY 0
+#define MPACK_EXTENSIONS 0
+#endif
+#endif
+
+
+
+/**
+ * @name Dependencies
+ * @{
+ */
+
+/**
+ * @def MPACK_CONFORMING
+ *
+ * Enables the inclusion of basic C headers to define standard types and
+ * macros.
+ *
+ * This causes MPack to include headers required for conforming implementations
+ * of C99 even in freestanding, in particular <stddef.h>, <stdint.h>,
+ * <stdbool.h> and <limits.h>. It also includes <inttypes.h>; this is
+ * technically not required for freestanding but MPack needs it to detect
+ * integer limits.
+ *
+ * You can disable this if these headers are unavailable or if they do not
+ * define the standard types and macros (for example inside the Linux kernel.)
+ * If this is disabled, MPack will include no headers and will assume a 32-bit
+ * int. You will probably also want to define @ref MPACK_HAS_CONFIG to 1 and
+ * include your own headers in the config file. You must provide definitions
+ * for standard types such as @c size_t, @c bool, @c int32_t and so on.
+ *
+ * @see <a href="https://en.cppreference.com/w/c/language/conformance">
+ * cppreference.com documentation on Conformance</a>
+ */
+#ifndef MPACK_CONFORMING
+    #define MPACK_CONFORMING 1
+#endif
+
+/**
+ * @def MPACK_STDLIB
+ *
+ * Enables the use of the C stdlib.
+ *
+ * This allows the library to use basic functions like @c memcmp() and @c
+ * strlen(), as well as @c malloc() for debugging and in allocation helpers.
+ *
+ * If this is disabled, allocation helper functions will not be defined, and
+ * MPack will attempt to detect compiler intrinsics for functions like @c
+ * memcmp() (assuming @ref MPACK_NO_BUILTINS is not set.) It will fallback to
+ * its own (slow) implementations if it cannot use builtins. You may want to
+ * define @ref MPACK_MEMCMP and friends if you disable this.
+ *
+ * @see MPACK_MEMCMP
+ * @see MPACK_MEMCPY
+ * @see MPACK_MEMMOVE
+ * @see MPACK_MEMSET
+ * @see MPACK_STRLEN
+ * @see MPACK_MALLOC
+ * @see MPACK_REALLOC
+ * @see MPACK_FREE
+ */
+#ifndef MPACK_STDLIB
+    #if !MPACK_CONFORMING
+        // If we don't even have a proper <limits.h> we assume we won't have
+        // malloc() either.
+        #define MPACK_STDLIB 0
+    #else
+        #define MPACK_STDLIB 1
+    #endif
+#endif
+
+/**
+ * @def MPACK_STDIO
+ *
+ * Enables the use of C stdio. This adds helpers for easily
+ * reading/writing C files and makes debugging easier.
+ */
+#ifndef MPACK_STDIO
+    #if !MPACK_STDLIB || defined(__AVR__)
+        #define MPACK_STDIO 0
+    #else
+        #define MPACK_STDIO 1
+    #endif
+#endif
+
+/**
+ * Whether the 'float' type and floating point operations are supported.
+ *
+ * If @ref MPACK_FLOAT is disabled, floats are read and written as @c uint32_t
+ * instead. This way messages with floats do not result in errors and you can
+ * still perform manual float parsing yourself.
+ */
+#ifndef MPACK_FLOAT
+    #define MPACK_FLOAT 1
+#endif
+
+/**
+ * Whether the 'double' type is supported. This requires support for 'float'.
+ *
+ * If @ref MPACK_DOUBLE is disabled, doubles are read and written as @c
+ * uint32_t instead. This way messages with doubles do not result in errors and
+ * you can still perform manual doubles parsing yourself.
+ *
+ * If @ref MPACK_FLOAT is enabled but @ref MPACK_DOUBLE is not, doubles can be
+ * read as floats using the shortening conversion functions, e.g. @ref
+ * mpack_expect_float() or @ref mpack_node_float().
+ */
+#ifndef MPACK_DOUBLE
+    #if !MPACK_FLOAT || defined(__AVR__)
+        // AVR supports only float, not double.
+        #define MPACK_DOUBLE 0
+    #else
+        #define MPACK_DOUBLE 1
+    #endif
+#endif
+
+/**
+ * @}
+ */
+
+
+
+/**
+ * @name Allocation Functions
+ * @{
+ */
+
+/**
+ * @def MPACK_MALLOC
+ *
+ * Defines the memory allocation function used by MPack. This is used by
+ * helpers for automatically allocating data the correct size, and for
+ * debugging functions. If this macro is undefined, the allocation helpers
+ * will not be compiled.
+ *
+ * Set this to use a custom @c malloc() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * void* malloc(size_t size);
+ * @endcode
+ *
+ * The default is @c malloc() if @ref MPACK_STDLIB is enabled.
+ */
+/**
+ * @def MPACK_FREE
+ *
+ * Defines the memory free function used by MPack. This is used by helpers
+ * for automatically allocating data the correct size. If this macro is
+ * undefined, the allocation helpers will not be compiled.
+ *
+ * Set this to use a custom @c free() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * void free(void* p);
+ * @endcode
+ *
+ * The default is @c free() if @ref MPACK_MALLOC has not been customized and
+ * @ref MPACK_STDLIB is enabled.
+ */
+/**
+ * @def MPACK_REALLOC
+ *
+ * Defines the realloc function used by MPack. It is used by growable
+ * buffers to resize more efficiently.
+ *
+ * The default is @c realloc() if @ref MPACK_MALLOC has not been customized and
+ * @ref MPACK_STDLIB is enabled.
+ *
+ * Set this to use a custom @c realloc() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * void* realloc(void* p, size_t new_size);
+ * @endcode
+ *
+ * This is optional, even when @ref MPACK_MALLOC is used. If @ref MPACK_MALLOC is
+ * set and @ref MPACK_REALLOC is not, @ref MPACK_MALLOC is used with a simple copy
+ * to grow buffers.
+ */
+
+#if defined(MPACK_MALLOC) && !defined(MPACK_FREE)
+    #error "MPACK_MALLOC requires MPACK_FREE."
+#endif
+#if !defined(MPACK_MALLOC) && defined(MPACK_FREE)
+    #error "MPACK_FREE requires MPACK_MALLOC."
+#endif
+
+// These were never configurable in lowercase but we check anyway.
+#ifdef mpack_malloc
+    #error "Define MPACK_MALLOC, not mpack_malloc."
+#endif
+#ifdef mpack_realloc
+    #error "Define MPACK_REALLOC, not mpack_realloc."
+#endif
+#ifdef mpack_free
+    #error "Define MPACK_FREE, not mpack_free."
+#endif
+
+// We don't use calloc() at all.
+#ifdef MPACK_CALLOC
+    #error "Don't define MPACK_CALLOC. MPack does not use calloc()."
+#endif
+#ifdef mpack_calloc
+    #error "Don't define mpack_calloc. MPack does not use calloc()."
+#endif
+
+// Use defaults in stdlib if we have them. Without it we don't use malloc.
+#if defined(MPACK_STDLIB)
+    #if MPACK_STDLIB && !defined(MPACK_MALLOC)
+        #define MPACK_MALLOC malloc
+        #define MPACK_REALLOC realloc
+        #define MPACK_FREE free
+    #endif
+#endif
+
+/**
+ * @}
+ */
+
+
+
+// This needs to be defined after we've decided whether we have malloc().
+#ifndef MPACK_BUILDER
+    #if defined(MPACK_MALLOC) && MPACK_WRITER
+        #define MPACK_BUILDER 1
+    #else
+        #define MPACK_BUILDER 0
+    #endif
+#endif
+
+
+
+/**
+ * @name System Functions
+ * @{
+ */
+
+/**
+ * @def MPACK_MEMCMP
+ *
+ * The function MPack will use to provide @c memcmp().
+ *
+ * Set this to use a custom @c memcmp() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * int memcmp(const void* left, const void* right, size_t count);
+ * @endcode
+ */
+/**
+ * @def MPACK_MEMCPY
+ *
+ * The function MPack will use to provide @c memcpy().
+ *
+ * Set this to use a custom @c memcpy() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * void* memcpy(void* restrict dest, const void* restrict src, size_t count);
+ * @endcode
+ */
+/**
+ * @def MPACK_MEMMOVE
+ *
+ * The function MPack will use to provide @c memmove().
+ *
+ * Set this to use a custom @c memmove() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * void* memmove(void* dest, const void* src, size_t count);
+ * @endcode
+ */
+/**
+ * @def MPACK_MEMSET
+ *
+ * The function MPack will use to provide @c memset().
+ *
+ * Set this to use a custom @c memset() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * void* memset(void* p, int c, size_t count);
+ * @endcode
+ */
+/**
+ * @def MPACK_STRLEN
+ *
+ * The function MPack will use to provide @c strlen().
+ *
+ * Set this to use a custom @c strlen() function. Your function must have the
+ * signature:
+ *
+ * @code
+ * size_t strlen(const char* str);
+ * @endcode
+ */
+
+// These were briefly configurable in lowercase in an unreleased version. Just
+// to make sure no one is doing this, we make sure these aren't already defined.
+#ifdef mpack_memcmp
+    #error "Define MPACK_MEMCMP, not mpack_memcmp."
+#endif
+#ifdef mpack_memcpy
+    #error "Define MPACK_MEMCPY, not mpack_memcpy."
+#endif
+#ifdef mpack_memmove
+    #error "Define MPACK_MEMMOVE, not mpack_memmove."
+#endif
+#ifdef mpack_memset
+    #error "Define MPACK_MEMSET, not mpack_memset."
+#endif
+#ifdef mpack_strlen
+    #error "Define MPACK_STRLEN, not mpack_strlen."
+#endif
+
+// If the standard library is available, we prefer to use its functions.
+#if MPACK_STDLIB
+    #ifndef MPACK_MEMCMP
+        #define MPACK_MEMCMP memcmp
+    #endif
+    #ifndef MPACK_MEMCPY
+        #define MPACK_MEMCPY memcpy
+    #endif
+    #ifndef MPACK_MEMMOVE
+        #define MPACK_MEMMOVE memmove
+    #endif
+    #ifndef MPACK_MEMSET
+        #define MPACK_MEMSET memset
+    #endif
+    #ifndef MPACK_STRLEN
+        #define MPACK_STRLEN strlen
+    #endif
+#endif
+
+#if !MPACK_NO_BUILTINS
+    #ifdef __has_builtin
+        #if !defined(MPACK_MEMCMP) && __has_builtin(__builtin_memcmp)
+            #define MPACK_MEMCMP __builtin_memcmp
+        #endif
+        #if !defined(MPACK_MEMCPY) && __has_builtin(__builtin_memcpy)
+            #define MPACK_MEMCPY __builtin_memcpy
+        #endif
+        #if !defined(MPACK_MEMMOVE) && __has_builtin(__builtin_memmove)
+            #define MPACK_MEMMOVE __builtin_memmove
+        #endif
+        #if !defined(MPACK_MEMSET) && __has_builtin(__builtin_memset)
+            #define MPACK_MEMSET __builtin_memset
+        #endif
+        #if !defined(MPACK_STRLEN) && __has_builtin(__builtin_strlen)
+            #define MPACK_STRLEN __builtin_strlen
+        #endif
+    #elif defined(__GNUC__)
+        #ifndef MPACK_MEMCMP
+            #define MPACK_MEMCMP __builtin_memcmp
+        #endif
+        #ifndef MPACK_MEMCPY
+            #define MPACK_MEMCPY __builtin_memcpy
+        #endif
+        // There's not always a builtin memmove for GCC. If we can't test for
+        // it with __has_builtin above, we don't use it. It's been around for
+        // much longer under clang, but then so has __has_builtin, so we let
+        // the block above handle it.
+        #ifndef MPACK_MEMSET
+            #define MPACK_MEMSET __builtin_memset
+        #endif
+        #ifndef MPACK_STRLEN
+            #define MPACK_STRLEN __builtin_strlen
+        #endif
+    #endif
+#endif
+
+/**
+ * @}
+ */
+
+
+
+/**
+ * @name Debugging Options
+ * @{
+ */
+
+/**
+ * @def MPACK_DEBUG
+ *
+ * Enables debug features. You may want to wrap this around your
+ * own debug preprocs. By default, this is enabled if @c DEBUG or @c _DEBUG
+ * are defined. (@c NDEBUG is not used since it is allowed to have
+ * different values in different translation units.)
+ */
+#if !defined(MPACK_DEBUG)
+    #if defined(DEBUG) || defined(_DEBUG)
+        #define MPACK_DEBUG 1
+    #else
+        #define MPACK_DEBUG 0
+    #endif
+#endif
+
+/**
+ * @def MPACK_STRINGS
+ *
+ * Enables descriptive error and type strings.
+ *
+ * This can be turned off (by defining it to 0) to maximize space savings
+ * on embedded devices. If this is disabled, string functions such as
+ * mpack_error_to_string() and mpack_type_to_string() return an empty string.
+ */
+#ifndef MPACK_STRINGS
+    #ifdef __AVR__
+        #define MPACK_STRINGS 0
+    #else
+        #define MPACK_STRINGS 1
+    #endif
+#endif
+
+/**
+ * Set this to 1 to implement a custom @ref mpack_assert_fail() function.
+ * See the documentation on @ref mpack_assert_fail() for details.
+ *
+ * Asserts are only used when @ref MPACK_DEBUG is enabled, and can be
+ * triggered by bugs in MPack or bugs due to incorrect usage of MPack.
+ */
+#ifndef MPACK_CUSTOM_ASSERT
+#define MPACK_CUSTOM_ASSERT 0
+#endif
+
+/**
+ * @def MPACK_READ_TRACKING
+ *
+ * Enables compound type size tracking for readers. This ensures that the
+ * correct number of elements or bytes are read from a compound type.
+ *
+ * This is enabled by default in debug builds (provided a @c malloc() is
+ * available.)
+ */
+#if !defined(MPACK_READ_TRACKING)
+    #if MPACK_DEBUG && MPACK_READER && defined(MPACK_MALLOC)
+        #define MPACK_READ_TRACKING 1
+    #else
+        #define MPACK_READ_TRACKING 0
+    #endif
+#endif
+#if MPACK_READ_TRACKING && !MPACK_READER
+    #error "MPACK_READ_TRACKING requires MPACK_READER."
+#endif
+
+/**
+ * @def MPACK_WRITE_TRACKING
+ *
+ * Enables compound type size tracking for writers. This ensures that the
+ * correct number of elements or bytes are written in a compound type.
+ *
+ * Note that without write tracking enabled, it is possible for buggy code
+ * to emit invalid MessagePack without flagging an error by writing the wrong
+ * number of elements or bytes in a compound type. With tracking enabled,
+ * MPack will catch such errors and break on the offending line of code.
+ *
+ * This is enabled by default in debug builds (provided a @c malloc() is
+ * available.)
+ */
+#if !defined(MPACK_WRITE_TRACKING)
+    #if MPACK_DEBUG && MPACK_WRITER && defined(MPACK_MALLOC)
+        #define MPACK_WRITE_TRACKING 1
+    #else
+        #define MPACK_WRITE_TRACKING 0
+    #endif
+#endif
+#if MPACK_WRITE_TRACKING && !MPACK_WRITER
+    #error "MPACK_WRITE_TRACKING requires MPACK_WRITER."
+#endif
+
+/**
+ * @}
+ */
+
+
+
+
+/**
+ * @name Miscellaneous Options
+ * @{
+ */
+
+/**
+ * Whether to optimize for size or speed.
+ *
+ * Optimizing for size simplifies some parsing and encoding algorithms
+ * at the expense of speed and saves a few kilobytes of space in the
+ * resulting executable.
+ *
+ * This automatically detects -Os with GCC/Clang. Unfortunately there
+ * doesn't seem to be a macro defined for /Os under MSVC.
+ */
+#ifndef MPACK_OPTIMIZE_FOR_SIZE
+    #ifdef __OPTIMIZE_SIZE__
+        #define MPACK_OPTIMIZE_FOR_SIZE 1
+    #else
+        #define MPACK_OPTIMIZE_FOR_SIZE 0
+    #endif
+#endif
+
+/**
+ * Stack space in bytes to use when initializing a reader or writer
+ * with a stack-allocated buffer.
+ *
+ * @warning Make sure you have sufficient stack space. Some libc use relatively
+ * small stacks even on desktop platforms, e.g. musl.
+ */
+#ifndef MPACK_STACK_SIZE
+#define MPACK_STACK_SIZE 4096
+#endif
+
+/**
+ * Buffer size to use for allocated buffers (such as for a file writer.)
+ *
+ * Starting with a single page and growing as needed seems to
+ * provide the best performance with minimal memory waste.
+ * Increasing this does not improve performance even when writing
+ * huge messages.
+ */
+#ifndef MPACK_BUFFER_SIZE
+#define MPACK_BUFFER_SIZE 4096
+#endif
+
+/**
+ * Minimum size for paged allocations in bytes.
+ *
+ * This is the value used by default for MPACK_NODE_PAGE_SIZE and
+ * MPACK_BUILDER_PAGE_SIZE.
+ */
+#ifndef MPACK_PAGE_SIZE
+#define MPACK_PAGE_SIZE 4096
+#endif
+
+/**
+ * Minimum size of an allocated node page in bytes.
+ *
+ * The children for a given compound element must be contiguous, so
+ * larger pages than this may be allocated as needed. (Safety checks
+ * exist to prevent malicious data from causing too large allocations.)
+ *
+ * See @ref mpack_node_data_t for the size of nodes.
+ *
+ * Using as many nodes fit in one memory page seems to provide the
+ * best performance, and has very little waste when parsing small
+ * messages.
+ */
+#ifndef MPACK_NODE_PAGE_SIZE
+#define MPACK_NODE_PAGE_SIZE MPACK_PAGE_SIZE
+#endif
+
+/**
+ * Minimum size of an allocated builder page in bytes.
+ *
+ * Builder writes are deferred to the allocated builder buffer which is
+ * composed of a list of buffer pages. This defines the size of those pages.
+ */
+#ifndef MPACK_BUILDER_PAGE_SIZE
+#define MPACK_BUILDER_PAGE_SIZE MPACK_PAGE_SIZE
+#endif
+
+/**
+ * @def MPACK_BUILDER_INTERNAL_STORAGE
+ *
+ * Enables a small amount of internal storage within the writer to avoid some
+ * allocations when using builders.
+ *
+ * This is disabled by default. Enable it to potentially improve performance at
+ * the expense of a larger writer.
+ *
+ * @see MPACK_BUILDER_INTERNAL_STORAGE_SIZE to configure its size.
+ */
+#ifndef MPACK_BUILDER_INTERNAL_STORAGE
+#define MPACK_BUILDER_INTERNAL_STORAGE 0
+#endif
+
+/**
+ * Amount of space reserved inside @ref mpack_writer_t for the Builders. This
+ * can allow small messages to be built with the Builder API without incurring
+ * an allocation.
+ *
+ * Builder metadata is placed in this space in addition to the literal
+ * MessagePack data. It needs to be big enough to be useful, but not so big as
+ * to overflow the stack. If more space is needed, pages are allocated.
+ *
+ * This is only used if MPACK_BUILDER_INTERNAL_STORAGE is enabled.
+ *
+ * @see MPACK_BUILDER_PAGE_SIZE
+ * @see MPACK_BUILDER_INTERNAL_STORAGE
+ *
+ * @warning Writers are typically placed on the stack so make sure you have
+ * sufficient stack space. Some libc use relatively small stacks even on
+ * desktop platforms, e.g. musl.
+ */
+#ifndef MPACK_BUILDER_INTERNAL_STORAGE_SIZE
+#define MPACK_BUILDER_INTERNAL_STORAGE_SIZE 256
+#endif
+
+/**
+ * The initial depth for the node parser. When MPACK_MALLOC is available,
+ * the node parser has no practical depth limit, and it is not recursive
+ * so there is no risk of overflowing the call stack.
+ */
+#ifndef MPACK_NODE_INITIAL_DEPTH
+#define MPACK_NODE_INITIAL_DEPTH 8
+#endif
+
+/**
+ * The maximum depth for the node parser if @ref MPACK_MALLOC is not available.
+ */
+#ifndef MPACK_NODE_MAX_DEPTH_WITHOUT_MALLOC
+#define MPACK_NODE_MAX_DEPTH_WITHOUT_MALLOC 32
+#endif
+
+/**
+ * @def MPACK_NO_BUILTINS
+ *
+ * Whether to disable compiler intrinsics and other built-in functions.
+ *
+ * If this is enabled, MPack won't use `__attribute__`, `__declspec`, any
+ * function starting with `__builtin`, or pretty much anything else that isn't
+ * standard C.
+ */
+#if defined(MPACK_DOXYGEN)
+#if MPACK_DOXYGEN
+    #define MPACK_NO_BUILTINS 0
+#endif
+#endif
+
+/**
+ * @}
+ */
+
+
+
+#if MPACK_DEBUG
+/**
+ * @name Debug Functions
+ * @{
+ */
+/**
+ * Implement this and define @ref MPACK_CUSTOM_ASSERT to use a custom
+ * assertion function.
+ *
+ * This function should not return. If it does, MPack will @c abort().
+ *
+ * If you use C++, make sure you include @c mpack.h where you define
+ * this to get the correct linkage (or define it <code>extern "C"</code>.)
+ *
+ * Asserts are only used when @ref MPACK_DEBUG is enabled, and can be
+ * triggered by bugs in MPack or bugs due to incorrect usage of MPack.
+ */
+void mpack_assert_fail(const char* message);
+/**
+ * @}
+ */
+#endif
+
+
+
+// The rest of this file shouldn't show up in Doxygen docs.
+/** @cond */
+
+
+
+/*
+ * All remaining pseudo-configuration options that have not yet been set must
+ * be defined here in order to support -Wundef.
+ *
+ * These aren't real configuration options; they are implementation details of
+ * MPack.
+ */
+#ifndef MPACK_CUSTOM_BREAK
+#define MPACK_CUSTOM_BREAK 0
+#endif
+#ifndef MPACK_EMIT_INLINE_DEFS
+#define MPACK_EMIT_INLINE_DEFS 0
+#endif
+#ifndef MPACK_AMALGAMATED
+#define MPACK_AMALGAMATED 0
+#endif
+#ifndef MPACK_RELEASE_VERSION
+#define MPACK_RELEASE_VERSION 0
+#endif
+#ifndef MPACK_INTERNAL
+#define MPACK_INTERNAL 0
+#endif
+
+
+
+/* System headers (based on configuration) */
+
+#if MPACK_CONFORMING
+    #include <stddef.h>
+    #include <stdint.h>
+    #include <stdbool.h>
+    #include <inttypes.h>
+    #include <limits.h>
+#endif
+
+#if MPACK_STDLIB
+    #include <string.h>
+    #include <stdlib.h>
+#endif
+
+#if MPACK_STDIO
+    #include <stdio.h>
+    #include <errno.h>
+    #if MPACK_DEBUG
+        #include <stdarg.h>
+    #endif
+#endif
+
+
+
+/*
+ * Integer Constants and Limits
+ */
+
+#if MPACK_CONFORMING
+    #define MPACK_INT64_C INT64_C
+    #define MPACK_UINT64_C UINT64_C
+
+    #define MPACK_INT8_MIN INT8_MIN
+    #define MPACK_INT16_MIN INT16_MIN
+    #define MPACK_INT32_MIN INT32_MIN
+    #define MPACK_INT64_MIN INT64_MIN
+    #define MPACK_INT_MIN INT_MIN
+
+    #define MPACK_INT8_MAX INT8_MAX
+    #define MPACK_INT16_MAX INT16_MAX
+    #define MPACK_INT32_MAX INT32_MAX
+    #define MPACK_INT64_MAX INT64_MAX
+    #define MPACK_INT_MAX INT_MAX
+
+    #define MPACK_UINT8_MAX UINT8_MAX
+    #define MPACK_UINT16_MAX UINT16_MAX
+    #define MPACK_UINT32_MAX UINT32_MAX
+    #define MPACK_UINT64_MAX UINT64_MAX
+    #define MPACK_UINT_MAX UINT_MAX
+#else
+    // For a non-conforming implementation we assume int is 32 bits.
+
+    #define MPACK_INT64_C(x) ((int64_t)(x##LL))
+    #define MPACK_UINT64_C(x) ((uint64_t)(x##LLU))
+
+    #define MPACK_INT8_MIN ((int8_t)(0x80))
+    #define MPACK_INT16_MIN ((int16_t)(0x8000))
+    #define MPACK_INT32_MIN ((int32_t)(0x80000000))
+    #define MPACK_INT64_MIN MPACK_INT64_C(0x8000000000000000)
+    #define MPACK_INT_MIN MPACK_INT32_MIN
+
+    #define MPACK_INT8_MAX ((int8_t)(0x7f))
+    #define MPACK_INT16_MAX ((int16_t)(0x7fff))
+    #define MPACK_INT32_MAX ((int32_t)(0x7fffffff))
+    #define MPACK_INT64_MAX MPACK_INT64_C(0x7fffffffffffffff)
+    #define MPACK_INT_MAX MPACK_INT32_MAX
+
+    #define MPACK_UINT8_MAX ((uint8_t)(0xffu))
+    #define MPACK_UINT16_MAX ((uint16_t)(0xffffu))
+    #define MPACK_UINT32_MAX ((uint32_t)(0xffffffffu))
+    #define MPACK_UINT64_MAX MPACK_UINT64_C(0xffffffffffffffff)
+    #define MPACK_UINT_MAX MPACK_UINT32_MAX
+#endif
+
+
+
+/*
+ * Floating point support
+ */
+
+#if MPACK_DOUBLE && !MPACK_FLOAT
+    #error "MPACK_DOUBLE requires MPACK_FLOAT."
+#endif
+
+// If we don't have support for float or double, we poison the identifiers to
+// make sure we don't define anything related to them.
+#if MPACK_INTERNAL
+    #ifdef __GNUC__
+        #if !MPACK_FLOAT
+            #pragma GCC poison float
+        #endif
+        #if !MPACK_DOUBLE
+            #pragma GCC poison double
+        #endif
+    #endif
+#endif
+
+
+
+/*
+ * extern C
+ */
+
+#ifdef __cplusplus
+    #define MPACK_EXTERN_C_BEGIN namespace mpack {
+    #define MPACK_EXTERN_C_END   }
+#else
+    #define MPACK_EXTERN_C_BEGIN /*nothing*/
+    #define MPACK_EXTERN_C_END   /*nothing*/
+#endif
+
+
+
+/*
+ * Warnings
+ */
+
+#if defined(__GNUC__)
+    // Diagnostic push is not supported before GCC 4.6.
+    #if defined(__clang__) || __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)
+        #define MPACK_SILENCE_WARNINGS_PUSH _Pragma ("GCC diagnostic push")
+        #define MPACK_SILENCE_WARNINGS_POP _Pragma ("GCC diagnostic pop")
+    #endif
+#elif defined(_MSC_VER)
+    // To support VS2017 and earlier we need to use __pragma and not _Pragma
+    #define MPACK_SILENCE_WARNINGS_PUSH __pragma(warning(push))
+    #define MPACK_SILENCE_WARNINGS_POP __pragma(warning(pop))
+#endif
+
+#if defined(_MSC_VER)
+    // These are a bunch of mostly useless warnings emitted under MSVC /W4,
+    // some as a result of the expansion of macros.
+    #define MPACK_SILENCE_WARNINGS_MSVC_W4 \
+            __pragma(warning(disable:4996)) /* _CRT_SECURE_NO_WARNINGS */ \
+            __pragma(warning(disable:4127)) /* comparison is constant */ \
+            __pragma(warning(disable:4702)) /* unreachable code */ \
+            __pragma(warning(disable:4310)) /* cast truncates constant value */
+#else
+    #define MPACK_SILENCE_WARNINGS_MSVC_W4 /*nothing*/
+#endif
+
+/* GCC versions before 5.1 warn about defining a C99 non-static inline function
+ * before declaring it (see issue #20). */
+#if defined(__GNUC__) && !defined(__clang__)
+    #if __GNUC__ < 5 || (__GNUC__ == 5 && __GNUC_MINOR__ < 1)
+        #ifdef __cplusplus
+            #define MPACK_SILENCE_WARNINGS_MISSING_PROTOTYPES \
+                _Pragma ("GCC diagnostic ignored \"-Wmissing-declarations\"")
+        #else
+            #define MPACK_SILENCE_WARNINGS_MISSING_PROTOTYPES \
+                _Pragma ("GCC diagnostic ignored \"-Wmissing-prototypes\"")
+        #endif
+    #endif
+#endif
+#ifndef MPACK_SILENCE_WARNINGS_MISSING_PROTOTYPES
+    #define MPACK_SILENCE_WARNINGS_MISSING_PROTOTYPES /*nothing*/
+#endif
+
+/* GCC versions before 4.8 warn about shadowing a function with a variable that
+ * isn't a function or function pointer (like "index"). */
+#if defined(__GNUC__) && !defined(__clang__)
+    #if __GNUC__ == 4 && __GNUC_MINOR__ < 8
+        #define MPACK_SILENCE_WARNINGS_SHADOW \
+            _Pragma ("GCC diagnostic ignored \"-Wshadow\"")
+    #endif
+#endif
+#ifndef MPACK_SILENCE_WARNINGS_SHADOW
+    #define MPACK_SILENCE_WARNINGS_SHADOW /*nothing*/
+#endif
+
+// On platforms with small size_t (e.g. AVR) we get type limits warnings where
+// we compare a size_t to e.g. MPACK_UINT32_MAX.
+#ifdef __AVR__
+    #define MPACK_SILENCE_WARNINGS_TYPE_LIMITS \
+        _Pragma ("GCC diagnostic ignored \"-Wtype-limits\"")
+#else
+    #define MPACK_SILENCE_WARNINGS_TYPE_LIMITS /*nothing*/
+#endif
+
+// MPack uses declarations after statements. This silences warnings about it
+// (e.g. when using MPack in a Linux kernel module.)
+#if defined(__GNUC__) && !defined(__cplusplus)
+    #define MPACK_SILENCE_WARNINGS_DECLARATION_AFTER_STATEMENT \
+        _Pragma ("GCC diagnostic ignored \"-Wdeclaration-after-statement\"")
+#else
+    #define MPACK_SILENCE_WARNINGS_DECLARATION_AFTER_STATEMENT /*nothing*/
+#endif
+
+#ifdef MPACK_SILENCE_WARNINGS_PUSH
+    // We only silence warnings if push/pop is supported, that way we aren't
+    // silencing warnings in code that uses MPack. If your compiler doesn't
+    // support push/pop silencing of warnings, you'll have to turn off
+    // conflicting warnings manually.
+
+    #define MPACK_SILENCE_WARNINGS_BEGIN \
+        MPACK_SILENCE_WARNINGS_PUSH \
+        MPACK_SILENCE_WARNINGS_MSVC_W4 \
+        MPACK_SILENCE_WARNINGS_MISSING_PROTOTYPES \
+        MPACK_SILENCE_WARNINGS_SHADOW \
+        MPACK_SILENCE_WARNINGS_TYPE_LIMITS \
+        MPACK_SILENCE_WARNINGS_DECLARATION_AFTER_STATEMENT
+
+    #define MPACK_SILENCE_WARNINGS_END \
+        MPACK_SILENCE_WARNINGS_POP
+#else
+    #define MPACK_SILENCE_WARNINGS_BEGIN /*nothing*/
+    #define MPACK_SILENCE_WARNINGS_END /*nothing*/
+#endif
+
+MPACK_SILENCE_WARNINGS_BEGIN
+MPACK_EXTERN_C_BEGIN
+
+
+
+/* Miscellaneous helper macros */
+
+#define MPACK_UNUSED(var) ((void)(var))
+
+#define MPACK_STRINGIFY_IMPL(arg) #arg
+#define MPACK_STRINGIFY(arg) MPACK_STRINGIFY_IMPL(arg)
+
+// This is a workaround for MSVC's incorrect expansion of __VA_ARGS__. It
+// treats __VA_ARGS__ as a single preprocessor token when passed in the
+// argument list of another macro unless we use an outer wrapper to expand it
+// lexically first. (For safety/consistency we use this in all variadic macros
+// that don't ignore the variadic arguments regardless of whether __VA_ARGS__
+// is passed to another macro.)
+//     https://stackoverflow.com/a/32400131
+#define MPACK_EXPAND(x) x
+
+// Extracts the first argument of a variadic macro list, where there might only
+// be one argument.
+#define MPACK_EXTRACT_ARG0_IMPL(first, ...) first
+#define MPACK_EXTRACT_ARG0(...) MPACK_EXPAND(MPACK_EXTRACT_ARG0_IMPL( __VA_ARGS__ , ignored))
+
+// Stringifies the first argument of a variadic macro list, where there might
+// only be one argument.
+#define MPACK_STRINGIFY_ARG0_impl(first, ...) #first
+#define MPACK_STRINGIFY_ARG0(...) MPACK_EXPAND(MPACK_STRINGIFY_ARG0_impl( __VA_ARGS__ , ignored))
+
+
+
+/*
+ * Definition of inline macros.
+ *
+ * MPack does not use static inline in header files; only one non-inline definition
+ * of each function should exist in the final build. This can reduce the binary size
+ * in cases where the compiler cannot or chooses not to inline a function.
+ * The addresses of functions should also compare equal across translation units
+ * regardless of whether they are declared inline.
+ *
+ * The above requirements mean that the declaration and definition of non-trivial
+ * inline functions must be separated so that the definitions will only
+ * appear when necessary. In addition, three different linkage models need
+ * to be supported:
+ *
+ *  - The C99 model, where a standalone function is emitted only if there is any
+ *    `extern inline` or non-`inline` declaration (including the definition itself)
+ *
+ *  - The GNU model, where an `inline` definition emits a standalone function and an
+ *    `extern inline` definition does not, regardless of other declarations
+ *
+ *  - The C++ model, where `inline` emits a standalone function with special
+ *    (COMDAT) linkage
+ *
+ * The macros below wrap up everything above. All inline functions defined in header
+ * files have a single non-inline definition emitted in the compilation of
+ * mpack-platform.c. All inline declarations and definitions use the same MPACK_INLINE
+ * specification to simplify the rules on when standalone functions are emitted.
+ * Inline functions in source files are defined MPACK_STATIC_INLINE.
+ *
+ * Additional reading:
+ *     http://www.greenend.org.uk/rjk/tech/inline.html
+ */
+
+#if defined(__cplusplus)
+    // C++ rules
+    // The linker will need COMDAT support to link C++ object files,
+    // so we don't need to worry about emitting definitions from C++
+    // translation units. If mpack-platform.c (or the amalgamation)
+    // is compiled as C, its definition will be used, otherwise a
+    // C++ definition will be used, and no other C files will emit
+    // a definition.
+    #define MPACK_INLINE inline
+
+#elif defined(_MSC_VER)
+    // MSVC 2013 always uses COMDAT linkage, but it doesn't treat 'inline' as a
+    // keyword in C99 mode. (This appears to be fixed in a later version of
+    // MSVC but we don't bother detecting it.)
+    #define MPACK_INLINE __inline
+    #define MPACK_STATIC_INLINE static __inline
+
+#elif defined(__GNUC__) && (defined(__GNUC_GNU_INLINE__) || \
+        (!defined(__GNUC_STDC_INLINE__) && !defined(__GNUC_GNU_INLINE__)))
+    // GNU rules
+    #if MPACK_EMIT_INLINE_DEFS
+        #define MPACK_INLINE inline
+    #else
+        #define MPACK_INLINE extern inline
+    #endif
+
+#elif defined(__TINYC__)
+    // tcc ignores the inline keyword, so we have to use static inline. We
+    // issue a warning to make sure you are aware. You can define the below
+    // macro to disable the warning. Hopefully this will be fixed soon:
+    //     https://lists.nongnu.org/archive/html/tinycc-devel/2019-06/msg00000.html
+    #ifndef MPACK_DISABLE_TINYC_INLINE_WARNING
+        #warning "Single-definition inline is not supported by tcc. All inlines will be static. Define MPACK_DISABLE_TINYC_INLINE_WARNING to disable this warning."
+    #endif
+    #define MPACK_INLINE static inline
+
+#else
+    // C99 rules
+    #if MPACK_EMIT_INLINE_DEFS
+        #define MPACK_INLINE extern inline
+    #else
+        #define MPACK_INLINE inline
+    #endif
+#endif
+
+#ifndef MPACK_STATIC_INLINE
+#define MPACK_STATIC_INLINE static inline
+#endif
+
+#ifdef MPACK_OPTIMIZE_FOR_SPEED
+    #error "You should define MPACK_OPTIMIZE_FOR_SIZE, not MPACK_OPTIMIZE_FOR_SPEED."
+#endif
+
+
+
+/*
+ * Prevent inlining
+ *
+ * When a function is only used once, it is almost always inlined
+ * automatically. This can cause poor instruction cache usage because a
+ * function that should rarely be called (such as buffer exhaustion handling)
+ * will get inlined into the middle of a hot code path.
+ */
+
+#if !MPACK_NO_BUILTINS
+    #if defined(_MSC_VER)
+        #define MPACK_NOINLINE __declspec(noinline)
+    #elif defined(__GNUC__) || defined(__clang__)
+        #define MPACK_NOINLINE __attribute__((__noinline__))
+    #endif
+#endif
+#ifndef MPACK_NOINLINE
+    #define MPACK_NOINLINE /* nothing */
+#endif
+
+
+
+/* restrict */
+
+// We prefer the builtins even though e.g. MSVC's __restrict may not have
+// exactly the same behaviour as the proper C99 restrict keyword because the
+// builtins work in C++, so using the same keyword in both C and C++ prevents
+// any incompatibilities when using MPack compiled as C in C++ code.
+#if !MPACK_NO_BUILTINS
+    #if defined(__GNUC__)
+        #define MPACK_RESTRICT __restrict__
+    #elif defined(_MSC_VER)
+        #define MPACK_RESTRICT __restrict
+    #endif
+#endif
+
+#ifndef MPACK_RESTRICT
+    #ifdef __cplusplus
+        #define MPACK_RESTRICT /* nothing, unavailable in C++ */
+    #endif
+#endif
+
+#ifndef MPACK_RESTRICT
+    #ifdef _MSC_VER
+        // MSVC 2015 apparently doesn't properly support the restrict keyword
+        // in C. We're using builtins above which do work on 2015, but when
+        // MPACK_NO_BUILTINS is enabled we can't use it.
+        #if _MSC_VER < 1910
+            #define MPACK_RESTRICT /*nothing*/
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_RESTRICT
+    #define MPACK_RESTRICT restrict /* required in C99 */
+#endif
+
+
+
+/* Some compiler-specific keywords and builtins */
+
+#if !MPACK_NO_BUILTINS
+    #if defined(__GNUC__) || defined(__clang__)
+        #define MPACK_UNREACHABLE __builtin_unreachable()
+        #define MPACK_NORETURN(fn) fn __attribute__((__noreturn__))
+    #elif defined(_MSC_VER)
+        #define MPACK_UNREACHABLE __assume(0)
+        #define MPACK_NORETURN(fn) __declspec(noreturn) fn
+    #endif
+#endif
+
+#ifndef MPACK_UNREACHABLE
+#define MPACK_UNREACHABLE ((void)0)
+#endif
+#ifndef MPACK_NORETURN
+#define MPACK_NORETURN(fn) fn
+#endif
+
+
+
+/*
+ * Likely/unlikely
+ *
+ * These should only really be used when a branch is taken (or not taken) less
+ * than 1/1000th of the time. Buffer flush checks when writing very small
+ * elements are a good example.
+ */
+
+#if !MPACK_NO_BUILTINS
+    #if defined(__GNUC__) || defined(__clang__)
+        #ifndef MPACK_LIKELY
+            #define MPACK_LIKELY(x) __builtin_expect((x),1)
+        #endif
+        #ifndef MPACK_UNLIKELY
+            #define MPACK_UNLIKELY(x) __builtin_expect((x),0)
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_LIKELY
+    #define MPACK_LIKELY(x) (x)
+#endif
+#ifndef MPACK_UNLIKELY
+    #define MPACK_UNLIKELY(x) (x)
+#endif
+
+
+
+/* alignof */
+
+#ifndef MPACK_ALIGNOF
+    #if defined(__STDC_VERSION__)
+        #if __STDC_VERSION__ >= 201112L
+            #define MPACK_ALIGNOF(T) (_Alignof(T))
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_ALIGNOF
+    #if defined(__cplusplus)
+        #if __cplusplus >= 201103L
+            #define MPACK_ALIGNOF(T) (alignof(T))
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_ALIGNOF
+    #if defined(__GNUC__) && !defined(MPACK_NO_BUILTINS)
+        #if defined(__clang__) || __GNUC__ >= 4
+            #define MPACK_ALIGNOF(T) (__alignof__(T))
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_ALIGNOF
+    #ifdef _MSC_VER
+        #define MPACK_ALIGNOF(T) __alignof(T)
+    #endif
+#endif
+
+// MPACK_ALIGNOF may not exist, in which case a workaround is used.
+
+
+
+/* Static assert */
+
+#ifndef MPACK_STATIC_ASSERT
+    #if defined(__cplusplus)
+        #if __cplusplus >= 201103L
+            #define MPACK_STATIC_ASSERT static_assert
+        #endif
+    #elif defined(__STDC_VERSION__)
+        #if __STDC_VERSION__ >= 201112L
+            #define MPACK_STATIC_ASSERT _Static_assert
+        #endif
+    #endif
+#endif
+
+#if !MPACK_NO_BUILTINS
+    #ifndef MPACK_STATIC_ASSERT
+        #if defined(__has_feature)
+            #if __has_feature(cxx_static_assert)
+                #define MPACK_STATIC_ASSERT static_assert
+            #elif __has_feature(c_static_assert)
+                #define MPACK_STATIC_ASSERT _Static_assert
+            #endif
+        #endif
+    #endif
+
+    #ifndef MPACK_STATIC_ASSERT
+        #if defined(__GNUC__)
+            /* Diagnostic push is not supported before GCC 4.6. */
+            #if defined(__clang__) || __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 6)
+                #ifndef __cplusplus
+                    #if defined(__clang__) || __GNUC__ >= 5
+                    #define MPACK_IGNORE_PEDANTIC "GCC diagnostic ignored \"-Wpedantic\""
+                    #else
+                    #define MPACK_IGNORE_PEDANTIC "GCC diagnostic ignored \"-pedantic\""
+                    #endif
+                    #define MPACK_STATIC_ASSERT(expr, str) do { \
+                        _Pragma ("GCC diagnostic push") \
+                        _Pragma (MPACK_IGNORE_PEDANTIC) \
+                        _Pragma ("GCC diagnostic ignored \"-Wc++-compat\"") \
+                        _Static_assert(expr, str); \
+                        _Pragma ("GCC diagnostic pop") \
+                    } while (0)
+                #endif
+            #endif
+        #endif
+    #endif
+
+    #ifndef MPACK_STATIC_ASSERT
+        #ifdef _MSC_VER
+            #if _MSC_VER >= 1600
+                #define MPACK_STATIC_ASSERT static_assert
+            #endif
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_STATIC_ASSERT
+    #define MPACK_STATIC_ASSERT(expr, str) (MPACK_UNUSED(sizeof(char[1 - 2*!(expr)])))
+#endif
+
+
+
+/* _Generic */
+
+#ifndef MPACK_HAS_GENERIC
+    #if defined(__clang__) && defined(__has_feature)
+        // With Clang we can test for _Generic support directly
+        // and ignore C/C++ version
+        #if __has_feature(c_generic_selections)
+            #define MPACK_HAS_GENERIC 1
+        #else
+            #define MPACK_HAS_GENERIC 0
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_HAS_GENERIC
+    #if defined(__STDC_VERSION__)
+        #if __STDC_VERSION__ >= 201112L
+            #if defined(__GNUC__) && !defined(__clang__)
+                // GCC does not have full C11 support in GCC 4.7 and 4.8
+                #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 9)
+                    #define MPACK_HAS_GENERIC 1
+                #endif
+            #else
+                // We hope other compilers aren't lying about C11/_Generic support
+                #define MPACK_HAS_GENERIC 1
+            #endif
+        #endif
+    #endif
+#endif
+
+#ifndef MPACK_HAS_GENERIC
+    #define MPACK_HAS_GENERIC 0
+#endif
+
+
+
+/*
+ * Finite Math
+ *
+ * -ffinite-math-only, included in -ffast-math, breaks functions that
+ * that check for non-finite real values such as isnan() and isinf().
+ *
+ * We should use this to trap errors when reading data that contains
+ * non-finite reals. This isn't currently implemented.
+ */
+
+#ifndef MPACK_FINITE_MATH
+#if defined(__FINITE_MATH_ONLY__) && __FINITE_MATH_ONLY__
+#define MPACK_FINITE_MATH 1
+#endif
+#endif
+
+#ifndef MPACK_FINITE_MATH
+#define MPACK_FINITE_MATH 0
+#endif
+
+
+
+/*
+ * Endianness checks
+ *
+ * These define MPACK_NHSWAP*() which swap network<->host byte
+ * order when needed.
+ *
+ * We leave them undefined if we can't determine the endianness
+ * at compile-time, in which case we fall back to bit-shifts.
+ *
+ * See the notes in mpack-common.h.
+ */
+
+#if defined(__BYTE_ORDER__) && defined(__ORDER_LITTLE_ENDIAN__) && defined(__ORDER_BIG_ENDIAN__)
+    #if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+        #define MPACK_NHSWAP16(x) (x)
+        #define MPACK_NHSWAP32(x) (x)
+        #define MPACK_NHSWAP64(x) (x)
+    #elif __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+
+        #if !MPACK_NO_BUILTINS
+            #if defined(__clang__)
+                #ifdef __has_builtin
+                    // Unlike the GCC builtins, the bswap builtins in Clang
+                    // significantly improve ARM performance.
+                    #if __has_builtin(__builtin_bswap16)
+                        #define MPACK_NHSWAP16(x) __builtin_bswap16(x)
+                    #endif
+                    #if __has_builtin(__builtin_bswap32)
+                        #define MPACK_NHSWAP32(x) __builtin_bswap32(x)
+                    #endif
+                    #if __has_builtin(__builtin_bswap64)
+                        #define MPACK_NHSWAP64(x) __builtin_bswap64(x)
+                    #endif
+                #endif
+
+            #elif defined(__GNUC__)
+
+                // The GCC bswap builtins are apparently poorly optimized on older
+                // versions of GCC, so we set a minimum version here just in case.
+                //     http://hardwarebug.org/2010/01/14/beware-the-builtins/
+
+                #if __GNUC__ > 4 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
+                    #define MPACK_NHSWAP64(x) __builtin_bswap64(x)
+                #endif
+
+                // __builtin_bswap16() was not implemented on all platforms
+                // until GCC 4.8.0:
+                //     https://gcc.gnu.org/bugzilla/show_bug.cgi?id=52624
+                //
+                // The 16- and 32-bit versions in GCC significantly reduce performance
+                // on ARM with little effect on code size so we don't use them.
+
+            #endif
+        #endif
+    #endif
+
+#elif defined(_MSC_VER) && defined(_WIN32) && MPACK_STDLIB && !MPACK_NO_BUILTINS
+
+    // On Windows, we assume x86 and x86_64 are always little-endian.
+    // We make no assumptions about ARM even though all current
+    // Windows Phone devices are little-endian in case Microsoft's
+    // compiler is ever used with a big-endian ARM device.
+
+    // These are functions in <stdlib.h> so we depend on MPACK_STDLIB.
+    // It's not clear if these are actually faster than just doing the
+    // swap manually; maybe we shouldn't bother with this.
+
+    #if defined(_M_IX86) || defined(_M_X64) || defined(_M_AMD64)
+        #define MPACK_NHSWAP16(x) _byteswap_ushort(x)
+        #define MPACK_NHSWAP32(x) _byteswap_ulong(x)
+        #define MPACK_NHSWAP64(x) _byteswap_uint64(x)
+    #endif
+
+#endif
+
+#if defined(__FLOAT_WORD_ORDER__) && defined(__BYTE_ORDER__)
+
+    // We check where possible that the float byte order matches the
+    // integer byte order. This is extremely unlikely to fail, but
+    // we check anyway just in case.
+    //
+    // (The static assert is placed in float/double encoders instead
+    // of here because our static assert fallback doesn't work at
+    // file scope)
+
+    #define MPACK_CHECK_FLOAT_ORDER() \
+        MPACK_STATIC_ASSERT(__FLOAT_WORD_ORDER__ == __BYTE_ORDER__, \
+            "float byte order does not match int byte order! float/double " \
+            "encoding is not properly implemented on this platform.")
+
+#endif
+
+#ifndef MPACK_CHECK_FLOAT_ORDER
+    #define MPACK_CHECK_FLOAT_ORDER() /* nothing */
+#endif
+
+
+/*
+ * Here we define mpack_assert() and mpack_break(). They both work like a normal
+ * assertion function in debug mode, causing a trap or abort. However, on some platforms
+ * you can safely resume execution from mpack_break(), whereas mpack_assert() is
+ * always fatal.
+ *
+ * In release mode, mpack_assert() is converted to an assurance to the compiler
+ * that the expression cannot be false (via e.g. __assume() or __builtin_unreachable())
+ * to improve optimization where supported. There is thus no point in "safely" handling
+ * the case of this being false. Writing mpack_assert(0) rarely makes sense (except
+ * possibly as a default handler in a switch) since the compiler will throw away any
+ * code after it. If at any time an mpack_assert() is not true, the behaviour is
+ * undefined. This also means the expression is evaluated even in release.
+ *
+ * mpack_break() on the other hand is compiled to nothing in release. It is
+ * used in situations where we want to highlight a programming error as early as
+ * possible (in the debugger), but we still handle the situation safely if it
+ * happens in release to avoid producing incorrect results (such as in
+ * MPACK_WRITE_TRACKING.) It does not take an expression to test because it
+ * belongs in a safe-handling block after its failing condition has been tested.
+ *
+ * If stdio is available, we can add a format string describing the error, and
+ * on some compilers we can declare it noreturn to get correct results from static
+ * analysis tools. Note that the format string and arguments are not evaluated unless
+ * the assertion is hit.
+ *
+ * Note that any arguments to mpack_assert() beyond the first are only evaluated
+ * if the expression is false (and are never evaluated in release.)
+ *
+ * mpack_assert_fail() and mpack_break_hit() are defined separately
+ * because assert is noreturn and break isn't. This distinction is very
+ * important for static analysis tools to give correct results.
+ */
+
+#if MPACK_DEBUG
+    MPACK_NORETURN(void mpack_assert_fail_wrapper(const char* message));
+    #if MPACK_STDIO
+        MPACK_NORETURN(void mpack_assert_fail_format(const char* format, ...));
+        #define mpack_assert_fail_at(line, file, exprstr, format, ...) \
+                MPACK_EXPAND(mpack_assert_fail_format("mpack assertion failed at " file ":" #line "\n%s\n" format, exprstr, __VA_ARGS__))
+    #else
+        #define mpack_assert_fail_at(line, file, exprstr, format, ...) \
+                mpack_assert_fail_wrapper("mpack assertion failed at " file ":" #line "\n" exprstr "\n")
+    #endif
+
+    #define mpack_assert_fail_pos(line, file, exprstr, expr, ...) \
+            MPACK_EXPAND(mpack_assert_fail_at(line, file, exprstr, __VA_ARGS__))
+
+    // This contains a workaround to the pedantic C99 requirement of having at
+    // least one argument to a variadic macro. The first argument is the
+    // boolean expression, the optional second argument (if provided) must be a
+    // literal format string, and any additional arguments are the format
+    // argument list.
+    //
+    // Unfortunately this means macros are expanded in the expression before it
+    // gets stringified. I haven't found a workaround to this.
+    //
+    // This adds two unused arguments to the format argument list when a
+    // format string is provided, so this would complicate the use of
+    // -Wformat and __attribute__((__format__)) on mpack_assert_fail_format()
+    // if we ever bothered to implement it.
+    #define mpack_assert(...) \
+            MPACK_EXPAND(((!(MPACK_EXTRACT_ARG0(__VA_ARGS__))) ? \
+                mpack_assert_fail_pos(__LINE__, __FILE__, MPACK_STRINGIFY_ARG0(__VA_ARGS__) , __VA_ARGS__ , "", NULL) : \
+                (void)0))
+
+    void mpack_break_hit(const char* message);
+    #if MPACK_STDIO
+        void mpack_break_hit_format(const char* format, ...);
+        #define mpack_break_hit_at(line, file, ...) \
+                MPACK_EXPAND(mpack_break_hit_format("mpack breakpoint hit at " file ":" #line "\n" __VA_ARGS__))
+    #else
+        #define mpack_break_hit_at(line, file, ...) \
+                mpack_break_hit("mpack breakpoint hit at " file ":" #line )
+    #endif
+    #define mpack_break_hit_pos(line, file, ...) MPACK_EXPAND(mpack_break_hit_at(line, file, __VA_ARGS__))
+    #define mpack_break(...) MPACK_EXPAND(mpack_break_hit_pos(__LINE__, __FILE__, __VA_ARGS__))
+#else
+    #define mpack_assert(...) \
+            (MPACK_EXPAND((!(MPACK_EXTRACT_ARG0(__VA_ARGS__))) ? \
+                (MPACK_UNREACHABLE, (void)0) : \
+                (void)0))
+    #define mpack_break(...) ((void)0)
+#endif
+
+
+
+// make sure we don't use the stdlib directly during development
+#if MPACK_STDLIB && defined(MPACK_UNIT_TESTS) && MPACK_INTERNAL && defined(__GNUC__)
+    #undef memcmp
+    #undef memcpy
+    #undef memmove
+    #undef memset
+    #undef strlen
+    #undef malloc
+    #undef calloc
+    #undef realloc
+    #undef free
+    #pragma GCC poison memcmp
+    #pragma GCC poison memcpy
+    #pragma GCC poison memmove
+    #pragma GCC poison memset
+    #pragma GCC poison strlen
+    #pragma GCC poison malloc
+    #pragma GCC poison calloc
+    #pragma GCC poison realloc
+    #pragma GCC poison free
+#endif
+
+
+
+// If we don't have these stdlib functions, we need to define them ourselves.
+// Either way we give them a lowercase name to make the code a bit nicer.
+
+#ifdef MPACK_MEMCMP
+    #define mpack_memcmp MPACK_MEMCMP
+#else
+    int mpack_memcmp(const void* s1, const void* s2, size_t n);
+#endif
+
+#ifdef MPACK_MEMCPY
+    #define mpack_memcpy MPACK_MEMCPY
+#else
+    void* mpack_memcpy(void* MPACK_RESTRICT s1, const void* MPACK_RESTRICT s2, size_t n);
+#endif
+
+#ifdef MPACK_MEMMOVE
+    #define mpack_memmove MPACK_MEMMOVE
+#else
+    void* mpack_memmove(void* s1, const void* s2, size_t n);
+#endif
+
+#ifdef MPACK_MEMSET
+    #define mpack_memset MPACK_MEMSET
+#else
+    void* mpack_memset(void* s, int c, size_t n);
+#endif
+
+#ifdef MPACK_STRLEN
+    #define mpack_strlen MPACK_STRLEN
+#else
+    size_t mpack_strlen(const char* s);
+#endif
+
+
+
+#if MPACK_STDIO
+    #if defined(WIN32)
+        #define mpack_snprintf _snprintf
+    #else
+        #define mpack_snprintf snprintf
+    #endif
+#endif
+
+
+
+/* Debug logging */
+#if 0
+    #include <stdio.h>
+    #define mpack_log(...) (MPACK_EXPAND(printf(__VA_ARGS__)), fflush(stdout))
+#else
+    #define mpack_log(...) ((void)0)
+#endif
+
+
+
+/* Make sure our configuration makes sense */
+#ifndef MPACK_MALLOC
+    #if MPACK_STDIO
+        #error "MPACK_STDIO requires preprocessor definitions for MPACK_MALLOC and MPACK_FREE."
+    #endif
+    #if MPACK_READ_TRACKING
+        #error "MPACK_READ_TRACKING requires preprocessor definitions for MPACK_MALLOC and MPACK_FREE."
+    #endif
+    #if MPACK_WRITE_TRACKING
+        #error "MPACK_WRITE_TRACKING requires preprocessor definitions for MPACK_MALLOC and MPACK_FREE."
+    #endif
+#endif
+
+
+
+/* Implement realloc if unavailable */
+#ifdef MPACK_MALLOC
+    #ifdef MPACK_REALLOC
+        MPACK_INLINE void* mpack_realloc(void* old_ptr, size_t used_size, size_t new_size) {
+            MPACK_UNUSED(used_size);
+            return MPACK_REALLOC(old_ptr, new_size);
+        }
+    #else
+        void* mpack_realloc(void* old_ptr, size_t used_size, size_t new_size);
+    #endif
+#endif
+
+
+
+/** @endcond */
+/**
+ * @}
+ */
+
+MPACK_EXTERN_C_END
+MPACK_SILENCE_WARNINGS_END
+
+#endif
+
+/* mpack/mpack-common.h.h */
+
+/**
+ * @file
+ *
+ * Defines types and functions shared by the MPack reader and writer.
+ */
+
+#ifndef MPACK_COMMON_H
+#define MPACK_COMMON_H 1
+
+/* #include "mpack-platform.h" */
+
+#ifndef MPACK_PRINT_BYTE_COUNT
+#define MPACK_PRINT_BYTE_COUNT 12
+#endif
+
+MPACK_SILENCE_WARNINGS_BEGIN
+MPACK_EXTERN_C_BEGIN
+
+
+
+/**
+ * @defgroup common Tags and Common Elements
+ *
+ * Contains types, constants and functions shared by both the encoding
+ * and decoding portions of MPack.
+ *
+ * @{
+ */
+
+/* Version information */
+
+#define MPACK_VERSION_MAJOR 1  /**< The major version number of MPack. */
+#define MPACK_VERSION_MINOR 1  /**< The minor version number of MPack. */
+#define MPACK_VERSION_PATCH 0  /**< The patch version number of MPack. */
+
+/** A number containing the version number of MPack for comparison purposes. */
+#define MPACK_VERSION ((MPACK_VERSION_MAJOR * 10000) + \
+        (MPACK_VERSION_MINOR * 100) + MPACK_VERSION_PATCH)
+
+/** A macro to test for a minimum version of MPack. */
+#define MPACK_VERSION_AT_LEAST(major, minor, patch) \
+        (MPACK_VERSION >= (((major) * 10000) + ((minor) * 100) + (patch)))
+
+/** @cond */
+#if (MPACK_VERSION_PATCH > 0)
+#define MPACK_VERSION_STRING_BASE \
+        MPACK_STRINGIFY(MPACK_VERSION_MAJOR) "." \
+        MPACK_STRINGIFY(MPACK_VERSION_MINOR) "." \
+        MPACK_STRINGIFY(MPACK_VERSION_PATCH)
+#else
+#define MPACK_VERSION_STRING_BASE \
+        MPACK_STRINGIFY(MPACK_VERSION_MAJOR) "." \
+        MPACK_STRINGIFY(MPACK_VERSION_MINOR)
+#endif
+/** @endcond */
+
+/**
+ * @def MPACK_VERSION_STRING
+ * @hideinitializer
+ *
+ * A string containing the MPack version.
+ */
+#if MPACK_RELEASE_VERSION
+#define MPACK_VERSION_STRING MPACK_VERSION_STRING_BASE
+#else
+#define MPACK_VERSION_STRING MPACK_VERSION_STRING_BASE "dev"
+#endif
+
+/**
+ * @def MPACK_LIBRARY_STRING
+ * @hideinitializer
+ *
+ * A string describing MPack, containing the library name, version and debug mode.
+ */
+#if MPACK_DEBUG
+#define MPACK_LIBRARY_STRING "MPack " MPACK_VERSION_STRING "-debug"
+#else
+#define MPACK_LIBRARY_STRING "MPack " MPACK_VERSION_STRING
+#endif
+
+/** @cond */
+/**
+ * @def MPACK_MAXIMUM_TAG_SIZE
+ *
+ * The maximum encoded size of a tag in bytes.
+ */
+#define MPACK_MAXIMUM_TAG_SIZE 9
+/** @endcond */
+
+#if MPACK_EXTENSIONS
+/**
+ * @def MPACK_TIMESTAMP_NANOSECONDS_MAX
+ *
+ * The maximum value of nanoseconds for a timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+#define MPACK_TIMESTAMP_NANOSECONDS_MAX 999999999
+#endif
+
+
+
+#if MPACK_COMPATIBILITY
+/**
+ * Versions of the MessagePack format.
+ *
+ * A reader, writer, or tree can be configured to serialize in an older
+ * version of the MessagePack spec. This is necessary to interface with
+ * older MessagePack libraries that do not support new MessagePack features.
+ *
+ * @note This requires @ref MPACK_COMPATIBILITY.
+ */
+typedef enum mpack_version_t {
+
+    /**
+     * Version 1.0/v4, supporting only the @c raw type without @c str8.
+     */
+    mpack_version_v4 = 4,
+
+    /**
+     * Version 2.0/v5, supporting the @c str8, @c bin and @c ext types.
+     */
+    mpack_version_v5 = 5,
+
+    /**
+     * The most recent supported version of MessagePack. This is the default.
+     */
+    mpack_version_current = mpack_version_v5,
+
+} mpack_version_t;
+#endif
+
+/**
+ * Error states for MPack objects.
+ *
+ * When a reader, writer, or tree is in an error state, all subsequent calls
+ * are ignored and their return values are nil/zero. You should check whether
+ * the source is in an error state before using such values.
+ */
+typedef enum mpack_error_t {
+    mpack_ok = 0,        /**< No error. */
+    mpack_error_io = 2,  /**< The reader or writer failed to fill or flush, or some other file or socket error occurred. */
+    mpack_error_invalid, /**< The data read is not valid MessagePack. */
+    mpack_error_unsupported, /**< The data read is not supported by this configuration of MPack. (See @ref MPACK_EXTENSIONS.) */
+    mpack_error_type,    /**< The type or value range did not match what was expected by the caller. */
+    mpack_error_too_big, /**< A read or write was bigger than the maximum size allowed for that operation. */
+    mpack_error_memory,  /**< An allocation failure occurred. */
+    mpack_error_bug,     /**< The MPack API was used incorrectly. (This will always assert in debug mode.) */
+    mpack_error_data,    /**< The contained data is not valid. */
+    mpack_error_eof,     /**< The reader failed to read because of file or socket EOF */
+} mpack_error_t;
+
+/**
+ * Converts an MPack error to a string. This function returns an empty
+ * string when MPACK_DEBUG is not set.
+ */
+const char* mpack_error_to_string(mpack_error_t error);
+
+/**
+ * Defines the type of a MessagePack tag.
+ *
+ * Note that extension types, both user defined and built-in, are represented
+ * in tags as @ref mpack_type_ext. The value for an extension type is stored
+ * separately.
+ */
+typedef enum mpack_type_t {
+    mpack_type_missing = 0, /**< Special type indicating a missing optional value. */
+    mpack_type_nil,         /**< A null value. */
+    mpack_type_bool,        /**< A boolean (true or false.) */
+    mpack_type_int,         /**< A 64-bit signed integer. */
+    mpack_type_uint,        /**< A 64-bit unsigned integer. */
+    mpack_type_float,       /**< A 32-bit IEEE 754 floating point number. */
+    mpack_type_double,      /**< A 64-bit IEEE 754 floating point number. */
+    mpack_type_str,         /**< A string. */
+    mpack_type_bin,         /**< A chunk of binary data. */
+    mpack_type_array,       /**< An array of MessagePack objects. */
+    mpack_type_map,         /**< An ordered map of key/value pairs of MessagePack objects. */
+
+    #if MPACK_EXTENSIONS
+    /**
+     * A typed MessagePack extension object containing a chunk of binary data.
+     *
+     * @note This requires @ref MPACK_EXTENSIONS.
+     */
+    mpack_type_ext,
+    #endif
+} mpack_type_t;
+
+/**
+ * Converts an MPack type to a string. This function returns an empty
+ * string when MPACK_DEBUG is not set.
+ */
+const char* mpack_type_to_string(mpack_type_t type);
+
+#if MPACK_EXTENSIONS
+/**
+ * A timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+typedef struct mpack_timestamp_t {
+    int64_t seconds; /*< The number of seconds (signed) since 1970-01-01T00:00:00Z. */
+    uint32_t nanoseconds; /*< The number of additional nanoseconds, between 0 and 999,999,999. */
+} mpack_timestamp_t;
+#endif
+
+/**
+ * An MPack tag is a MessagePack object header. It is a variant type
+ * representing any kind of object, and includes the length of compound types
+ * (e.g. map, array, string) or the value of non-compound types (e.g. boolean,
+ * integer, float.)
+ *
+ * If the type is compound (str, bin, ext, array or map), the contained
+ * elements or bytes are stored separately.
+ *
+ * This structure is opaque; its fields should not be accessed outside
+ * of MPack.
+ */
+typedef struct mpack_tag_t mpack_tag_t;
+
+/* Hide internals from documentation */
+/** @cond */
+struct mpack_tag_t {
+    mpack_type_t type; /*< The type of value. */
+
+    #if MPACK_EXTENSIONS
+    int8_t exttype; /*< The extension type if the type is @ref mpack_type_ext. */
+    #endif
+
+    /* The value for non-compound types. */
+    union {
+        uint64_t u; /*< The value if the type is unsigned int. */
+        int64_t  i; /*< The value if the type is signed int. */
+        bool     b; /*< The value if the type is bool. */
+
+        #if MPACK_FLOAT
+        float    f; /*< The value if the type is float. */
+        #else
+        uint32_t f; /*< The raw value if the type is float. */
+        #endif
+
+        #if MPACK_DOUBLE
+        double   d; /*< The value if the type is double. */
+        #else
+        uint64_t d; /*< The raw value if the type is double. */
+        #endif
+
+        /* The number of bytes if the type is str, bin or ext. */
+        uint32_t l;
+
+        /* The element count if the type is an array, or the number of
+            key/value pairs if the type is map. */
+        uint32_t n;
+    } v;
+};
+/** @endcond */
+
+/**
+ * @name Tag Generators
+ * @{
+ */
+
+/**
+ * @def MPACK_TAG_ZERO
+ *
+ * An @ref mpack_tag_t initializer that zeroes the given tag.
+ *
+ * @warning This does not make the tag nil! The tag's type is invalid when
+ * initialized this way. Use @ref mpack_tag_make_nil() to generate a nil tag.
+ */
+#if MPACK_EXTENSIONS
+#define MPACK_TAG_ZERO {(mpack_type_t)0, 0, {0}}
+#else
+#define MPACK_TAG_ZERO {(mpack_type_t)0, {0}}
+#endif
+
+/** Generates a nil tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_nil(void) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_nil;
+    return ret;
+}
+
+/** Generates a bool tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_bool(bool value) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_bool;
+    ret.v.b = value;
+    return ret;
+}
+
+/** Generates a bool tag with value true. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_true(void) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_bool;
+    ret.v.b = true;
+    return ret;
+}
+
+/** Generates a bool tag with value false. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_false(void) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_bool;
+    ret.v.b = false;
+    return ret;
+}
+
+/** Generates a signed int tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_int(int64_t value) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_int;
+    ret.v.i = value;
+    return ret;
+}
+
+/** Generates an unsigned int tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_uint(uint64_t value) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_uint;
+    ret.v.u = value;
+    return ret;
+}
+
+#if MPACK_FLOAT
+/** Generates a float tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_float(float value)
+#else
+/** Generates a float tag from a raw uint32_t. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_raw_float(uint32_t value)
+#endif
+{
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_float;
+    ret.v.f = value;
+    return ret;
+}
+
+#if MPACK_DOUBLE
+/** Generates a double tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_double(double value)
+#else
+/** Generates a double tag from a raw uint64_t. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_raw_double(uint64_t value)
+#endif
+{
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_double;
+    ret.v.d = value;
+    return ret;
+}
+
+/** Generates an array tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_array(uint32_t count) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_array;
+    ret.v.n = count;
+    return ret;
+}
+
+/** Generates a map tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_map(uint32_t count) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_map;
+    ret.v.n = count;
+    return ret;
+}
+
+/** Generates a str tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_str(uint32_t length) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_str;
+    ret.v.l = length;
+    return ret;
+}
+
+/** Generates a bin tag. */
+MPACK_INLINE mpack_tag_t mpack_tag_make_bin(uint32_t length) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_bin;
+    ret.v.l = length;
+    return ret;
+}
+
+#if MPACK_EXTENSIONS
+/**
+ * Generates an ext tag.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+MPACK_INLINE mpack_tag_t mpack_tag_make_ext(int8_t exttype, uint32_t length) {
+    mpack_tag_t ret = MPACK_TAG_ZERO;
+    ret.type = mpack_type_ext;
+    ret.exttype = exttype;
+    ret.v.l = length;
+    return ret;
+}
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Tag Querying Functions
+ * @{
+ */
+
+/**
+ * Gets the type of a tag.
+ */
+MPACK_INLINE mpack_type_t mpack_tag_type(mpack_tag_t* tag) {
+    return tag->type;
+}
+
+/**
+ * Gets the boolean value of a bool-type tag. The tag must be of type @ref
+ * mpack_type_bool.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_bool. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ */
+MPACK_INLINE bool mpack_tag_bool_value(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_bool, "tag is not a bool!");
+    return tag->v.b;
+}
+
+/**
+ * Gets the signed integer value of an int-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_int. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @warning This does not convert between signed and unsigned tags! A positive
+ * integer may be stored in a tag as either @ref mpack_type_int or @ref
+ * mpack_type_uint. You must check the type first; this can only be used if the
+ * type is @ref mpack_type_int.
+ *
+ * @see mpack_type_int
+ */
+MPACK_INLINE int64_t mpack_tag_int_value(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_int, "tag is not an int!");
+    return tag->v.i;
+}
+
+/**
+ * Gets the unsigned integer value of a uint-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_uint. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @warning This does not convert between signed and unsigned tags! A positive
+ * integer may be stored in a tag as either @ref mpack_type_int or @ref
+ * mpack_type_uint. You must check the type first; this can only be used if the
+ * type is @ref mpack_type_uint.
+ *
+ * @see mpack_type_uint
+ */
+MPACK_INLINE uint64_t mpack_tag_uint_value(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_uint, "tag is not a uint!");
+    return tag->v.u;
+}
+
+/**
+ * Gets the float value of a float-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_float. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @warning This does not convert between float and double tags! This can only
+ * be used if the type is @ref mpack_type_float.
+ *
+ * @see mpack_type_float
+ */
+MPACK_INLINE
+#if MPACK_FLOAT
+float mpack_tag_float_value(mpack_tag_t* tag)
+#else
+uint32_t mpack_tag_raw_float_value(mpack_tag_t* tag)
+#endif
+{
+    mpack_assert(tag->type == mpack_type_float, "tag is not a float!");
+    return tag->v.f;
+}
+
+/**
+ * Gets the double value of a double-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_double. (No check
+ * is performed if MPACK_DEBUG is not set.)
+ *
+ * @warning This does not convert between float and double tags! This can only
+ * be used if the type is @ref mpack_type_double.
+ *
+ * @see mpack_type_double
+ */
+MPACK_INLINE
+#if MPACK_DOUBLE
+double mpack_tag_double_value(mpack_tag_t* tag)
+#else
+uint64_t mpack_tag_raw_double_value(mpack_tag_t* tag)
+#endif
+{
+    mpack_assert(tag->type == mpack_type_double, "tag is not a double!");
+    return tag->v.d;
+}
+
+/**
+ * Gets the number of elements in an array tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_array. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @see mpack_type_array
+ */
+MPACK_INLINE uint32_t mpack_tag_array_count(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_array, "tag is not an array!");
+    return tag->v.n;
+}
+
+/**
+ * Gets the number of key-value pairs in a map tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_map. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @see mpack_type_map
+ */
+MPACK_INLINE uint32_t mpack_tag_map_count(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_map, "tag is not a map!");
+    return tag->v.n;
+}
+
+/**
+ * Gets the length in bytes of a str-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_str. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @see mpack_type_str
+ */
+MPACK_INLINE uint32_t mpack_tag_str_length(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_str, "tag is not a str!");
+    return tag->v.l;
+}
+
+/**
+ * Gets the length in bytes of a bin-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_bin. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @see mpack_type_bin
+ */
+MPACK_INLINE uint32_t mpack_tag_bin_length(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_bin, "tag is not a bin!");
+    return tag->v.l;
+}
+
+#if MPACK_EXTENSIONS
+/**
+ * Gets the length in bytes of an ext-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_ext. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @see mpack_type_ext
+ */
+MPACK_INLINE uint32_t mpack_tag_ext_length(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_ext, "tag is not an ext!");
+    return tag->v.l;
+}
+
+/**
+ * Gets the extension type (exttype) of an ext-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_ext. (No check is
+ * performed if MPACK_DEBUG is not set.)
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @see mpack_type_ext
+ */
+MPACK_INLINE int8_t mpack_tag_ext_exttype(mpack_tag_t* tag) {
+    mpack_assert(tag->type == mpack_type_ext, "tag is not an ext!");
+    return tag->exttype;
+}
+#endif
+
+/**
+ * Gets the length in bytes of a str-, bin- or ext-type tag.
+ *
+ * This asserts that the type in the tag is @ref mpack_type_str, @ref
+ * mpack_type_bin or @ref mpack_type_ext. (No check is performed if MPACK_DEBUG
+ * is not set.)
+ *
+ * @see mpack_type_str
+ * @see mpack_type_bin
+ * @see mpack_type_ext
+ */
+MPACK_INLINE uint32_t mpack_tag_bytes(mpack_tag_t* tag) {
+    #if MPACK_EXTENSIONS
+    mpack_assert(tag->type == mpack_type_str || tag->type == mpack_type_bin
+            || tag->type == mpack_type_ext, "tag is not a str, bin or ext!");
+    #else
+    mpack_assert(tag->type == mpack_type_str || tag->type == mpack_type_bin,
+            "tag is not a str or bin!");
+    #endif
+    return tag->v.l;
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @name Other tag functions
+ * @{
+ */
+
+#if MPACK_EXTENSIONS
+/**
+ * The extension type for a timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+#define MPACK_EXTTYPE_TIMESTAMP ((int8_t)(-1))
+#endif
+
+/**
+ * Compares two tags with an arbitrary fixed ordering. Returns 0 if the tags are
+ * equal, a negative integer if left comes before right, or a positive integer
+ * otherwise.
+ *
+ * \warning The ordering is not guaranteed to be preserved across MPack versions; do
+ * not rely on it in persistent data.
+ *
+ * \warning Floating point numbers are compared bit-for-bit, not using the language's
+ * operator==. This means that NaNs with matching representation will compare equal.
+ * This behaviour is up for debate; see comments in the definition of mpack_tag_cmp().
+ *
+ * See mpack_tag_equal() for more information on when tags are considered equal.
+ */
+int mpack_tag_cmp(mpack_tag_t left, mpack_tag_t right);
+
+/**
+ * Compares two tags for equality. Tags are considered equal if the types are compatible
+ * and the values (for non-compound types) are equal.
+ *
+ * The field width of variable-width fields is ignored (and in fact is not stored
+ * in a tag), and positive numbers in signed integers are considered equal to their
+ * unsigned counterparts. So for example the value 1 stored as a positive fixint
+ * is equal to the value 1 stored in a 64-bit unsigned integer field.
+ *
+ * The "extension type" of an extension object is considered part of the value
+ * and must match exactly.
+ *
+ * \warning Floating point numbers are compared bit-for-bit, not using the language's
+ * operator==. This means that NaNs with matching representation will compare equal.
+ * This behaviour is up for debate; see comments in the definition of mpack_tag_cmp().
+ */
+MPACK_INLINE bool mpack_tag_equal(mpack_tag_t left, mpack_tag_t right) {
+    return mpack_tag_cmp(left, right) == 0;
+}
+
+#if MPACK_DEBUG && MPACK_STDIO
+/**
+ * Generates a json-like debug description of the given tag into the given buffer.
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ *
+ * The prefix is used to print the first few hexadecimal bytes of a bin or ext
+ * type. Pass NULL if not a bin or ext.
+ */
+void mpack_tag_debug_pseudo_json(mpack_tag_t tag, char* buffer, size_t buffer_size,
+        const char* prefix, size_t prefix_size);
+
+/**
+ * Generates a debug string description of the given tag into the given buffer.
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+void mpack_tag_debug_describe(mpack_tag_t tag, char* buffer, size_t buffer_size);
+
+/** @cond */
+
+/*
+ * A callback function for printing pseudo-JSON for debugging purposes.
+ *
+ * @see mpack_node_print_callback
+ */
+typedef void (*mpack_print_callback_t)(void* context, const char* data, size_t count);
+
+// helpers for printing debug output
+// i feel a bit like i'm re-implementing a buffered writer again...
+typedef struct mpack_print_t {
+    char* buffer;
+    size_t size;
+    size_t count;
+    mpack_print_callback_t callback;
+    void* context;
+} mpack_print_t;
+
+void mpack_print_append(mpack_print_t* print, const char* data, size_t count);
+
+MPACK_INLINE void mpack_print_append_cstr(mpack_print_t* print, const char* cstr) {
+    mpack_print_append(print, cstr, mpack_strlen(cstr));
+}
+
+void mpack_print_flush(mpack_print_t* print);
+
+void mpack_print_file_callback(void* context, const char* data, size_t count);
+
+/** @endcond */
+
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Deprecated Tag Generators
+ * @{
+ */
+
+/*
+ * "make" has been added to their names to disambiguate them from the
+ * value-fetching functions (e.g. mpack_tag_make_bool() vs
+ * mpack_tag_bool_value().)
+ *
+ * The length and count for all compound types was the wrong sign (int32_t
+ * instead of uint32_t.) These preserve the old behaviour; the new "make"
+ * functions have the correct sign.
+ */
+
+/** \deprecated Renamed to mpack_tag_make_nil(). */
+MPACK_INLINE mpack_tag_t mpack_tag_nil(void) {
+    return mpack_tag_make_nil();
+}
+
+/** \deprecated Renamed to mpack_tag_make_bool(). */
+MPACK_INLINE mpack_tag_t mpack_tag_bool(bool value) {
+    return mpack_tag_make_bool(value);
+}
+
+/** \deprecated Renamed to mpack_tag_make_true(). */
+MPACK_INLINE mpack_tag_t mpack_tag_true(void) {
+    return mpack_tag_make_true();
+}
+
+/** \deprecated Renamed to mpack_tag_make_false(). */
+MPACK_INLINE mpack_tag_t mpack_tag_false(void) {
+    return mpack_tag_make_false();
+}
+
+/** \deprecated Renamed to mpack_tag_make_int(). */
+MPACK_INLINE mpack_tag_t mpack_tag_int(int64_t value) {
+    return mpack_tag_make_int(value);
+}
+
+/** \deprecated Renamed to mpack_tag_make_uint(). */
+MPACK_INLINE mpack_tag_t mpack_tag_uint(uint64_t value) {
+    return mpack_tag_make_uint(value);
+}
+
+#if MPACK_FLOAT
+/** \deprecated Renamed to mpack_tag_make_float(). */
+MPACK_INLINE mpack_tag_t mpack_tag_float(float value) {
+    return mpack_tag_make_float(value);
+}
+#endif
+
+#if MPACK_DOUBLE
+/** \deprecated Renamed to mpack_tag_make_double(). */
+MPACK_INLINE mpack_tag_t mpack_tag_double(double value) {
+    return mpack_tag_make_double(value);
+}
+#endif
+
+/** \deprecated Renamed to mpack_tag_make_array(). */
+MPACK_INLINE mpack_tag_t mpack_tag_array(int32_t count) {
+    return mpack_tag_make_array((uint32_t)count);
+}
+
+/** \deprecated Renamed to mpack_tag_make_map(). */
+MPACK_INLINE mpack_tag_t mpack_tag_map(int32_t count) {
+    return mpack_tag_make_map((uint32_t)count);
+}
+
+/** \deprecated Renamed to mpack_tag_make_str(). */
+MPACK_INLINE mpack_tag_t mpack_tag_str(int32_t length) {
+    return mpack_tag_make_str((uint32_t)length);
+}
+
+/** \deprecated Renamed to mpack_tag_make_bin(). */
+MPACK_INLINE mpack_tag_t mpack_tag_bin(int32_t length) {
+    return mpack_tag_make_bin((uint32_t)length);
+}
+
+#if MPACK_EXTENSIONS
+/** \deprecated Renamed to mpack_tag_make_ext(). */
+MPACK_INLINE mpack_tag_t mpack_tag_ext(int8_t exttype, int32_t length) {
+    return mpack_tag_make_ext(exttype, (uint32_t)length);
+}
+#endif
+
+/**
+ * @}
+ */
+
+/** @cond */
+
+/*
+ * Helpers to perform unaligned network-endian loads and stores
+ * at arbitrary addresses. Byte-swapping builtins are used if they
+ * are available and if they improve performance.
+ *
+ * These will remain available in the public API so feel free to
+ * use them for other purposes, but they are undocumented.
+ */
+
+MPACK_INLINE uint8_t mpack_load_u8(const char* p) {
+    return (uint8_t)p[0];
+}
+
+MPACK_INLINE uint16_t mpack_load_u16(const char* p) {
+    #ifdef MPACK_NHSWAP16
+    uint16_t val;
+    mpack_memcpy(&val, p, sizeof(val));
+    return MPACK_NHSWAP16(val);
+    #else
+    return (uint16_t)((((uint16_t)(uint8_t)p[0]) << 8) |
+           ((uint16_t)(uint8_t)p[1]));
+    #endif
+}
+
+MPACK_INLINE uint32_t mpack_load_u32(const char* p) {
+    #ifdef MPACK_NHSWAP32
+    uint32_t val;
+    mpack_memcpy(&val, p, sizeof(val));
+    return MPACK_NHSWAP32(val);
+    #else
+    return (((uint32_t)(uint8_t)p[0]) << 24) |
+           (((uint32_t)(uint8_t)p[1]) << 16) |
+           (((uint32_t)(uint8_t)p[2]) <<  8) |
+            ((uint32_t)(uint8_t)p[3]);
+    #endif
+}
+
+MPACK_INLINE uint64_t mpack_load_u64(const char* p) {
+    #ifdef MPACK_NHSWAP64
+    uint64_t val;
+    mpack_memcpy(&val, p, sizeof(val));
+    return MPACK_NHSWAP64(val);
+    #else
+    return (((uint64_t)(uint8_t)p[0]) << 56) |
+           (((uint64_t)(uint8_t)p[1]) << 48) |
+           (((uint64_t)(uint8_t)p[2]) << 40) |
+           (((uint64_t)(uint8_t)p[3]) << 32) |
+           (((uint64_t)(uint8_t)p[4]) << 24) |
+           (((uint64_t)(uint8_t)p[5]) << 16) |
+           (((uint64_t)(uint8_t)p[6]) <<  8) |
+            ((uint64_t)(uint8_t)p[7]);
+    #endif
+}
+
+MPACK_INLINE void mpack_store_u8(char* p, uint8_t val) {
+    uint8_t* u = (uint8_t*)p;
+    u[0] = val;
+}
+
+MPACK_INLINE void mpack_store_u16(char* p, uint16_t val) {
+    #ifdef MPACK_NHSWAP16
+    val = MPACK_NHSWAP16(val);
+    mpack_memcpy(p, &val, sizeof(val));
+    #else
+    uint8_t* u = (uint8_t*)p;
+    u[0] = (uint8_t)((val >> 8) & 0xFF);
+    u[1] = (uint8_t)( val       & 0xFF);
+    #endif
+}
+
+MPACK_INLINE void mpack_store_u32(char* p, uint32_t val) {
+    #ifdef MPACK_NHSWAP32
+    val = MPACK_NHSWAP32(val);
+    mpack_memcpy(p, &val, sizeof(val));
+    #else
+    uint8_t* u = (uint8_t*)p;
+    u[0] = (uint8_t)((val >> 24) & 0xFF);
+    u[1] = (uint8_t)((val >> 16) & 0xFF);
+    u[2] = (uint8_t)((val >>  8) & 0xFF);
+    u[3] = (uint8_t)( val        & 0xFF);
+    #endif
+}
+
+MPACK_INLINE void mpack_store_u64(char* p, uint64_t val) {
+    #ifdef MPACK_NHSWAP64
+    val = MPACK_NHSWAP64(val);
+    mpack_memcpy(p, &val, sizeof(val));
+    #else
+    uint8_t* u = (uint8_t*)p;
+    u[0] = (uint8_t)((val >> 56) & 0xFF);
+    u[1] = (uint8_t)((val >> 48) & 0xFF);
+    u[2] = (uint8_t)((val >> 40) & 0xFF);
+    u[3] = (uint8_t)((val >> 32) & 0xFF);
+    u[4] = (uint8_t)((val >> 24) & 0xFF);
+    u[5] = (uint8_t)((val >> 16) & 0xFF);
+    u[6] = (uint8_t)((val >>  8) & 0xFF);
+    u[7] = (uint8_t)( val        & 0xFF);
+    #endif
+}
+
+MPACK_INLINE int8_t  mpack_load_i8 (const char* p) {return (int8_t) mpack_load_u8 (p);}
+MPACK_INLINE int16_t mpack_load_i16(const char* p) {return (int16_t)mpack_load_u16(p);}
+MPACK_INLINE int32_t mpack_load_i32(const char* p) {return (int32_t)mpack_load_u32(p);}
+MPACK_INLINE int64_t mpack_load_i64(const char* p) {return (int64_t)mpack_load_u64(p);}
+MPACK_INLINE void mpack_store_i8 (char* p, int8_t  val) {mpack_store_u8 (p, (uint8_t) val);}
+MPACK_INLINE void mpack_store_i16(char* p, int16_t val) {mpack_store_u16(p, (uint16_t)val);}
+MPACK_INLINE void mpack_store_i32(char* p, int32_t val) {mpack_store_u32(p, (uint32_t)val);}
+MPACK_INLINE void mpack_store_i64(char* p, int64_t val) {mpack_store_u64(p, (uint64_t)val);}
+
+#if MPACK_FLOAT
+MPACK_INLINE float mpack_load_float(const char* p) {
+    MPACK_CHECK_FLOAT_ORDER();
+    MPACK_STATIC_ASSERT(sizeof(float) == sizeof(uint32_t), "float is wrong size??");
+    union {
+        float f;
+        uint32_t u;
+    } v;
+    v.u = mpack_load_u32(p);
+    return v.f;
+}
+#endif
+
+#if MPACK_DOUBLE
+MPACK_INLINE double mpack_load_double(const char* p) {
+    MPACK_CHECK_FLOAT_ORDER();
+    MPACK_STATIC_ASSERT(sizeof(double) == sizeof(uint64_t), "double is wrong size??");
+    union {
+        double d;
+        uint64_t u;
+    } v;
+    v.u = mpack_load_u64(p);
+    return v.d;
+}
+#endif
+
+#if MPACK_FLOAT
+MPACK_INLINE void mpack_store_float(char* p, float value) {
+    MPACK_CHECK_FLOAT_ORDER();
+    union {
+        float f;
+        uint32_t u;
+    } v;
+    v.f = value;
+    mpack_store_u32(p, v.u);
+}
+#endif
+
+#if MPACK_DOUBLE
+MPACK_INLINE void mpack_store_double(char* p, double value) {
+    MPACK_CHECK_FLOAT_ORDER();
+    union {
+        double d;
+        uint64_t u;
+    } v;
+    v.d = value;
+    mpack_store_u64(p, v.u);
+}
+#endif
+
+#if MPACK_FLOAT && !MPACK_DOUBLE
+/**
+ * Performs a manual shortening conversion on the raw 64-bit representation of
+ * a double. This is useful for parsing doubles on platforms that only support
+ * floats (such as AVR.)
+ *
+ * The significand is truncated rather than rounded and subnormal numbers are
+ * set to 0 so this may not be quite as accurate as a real double-to-float
+ * conversion.
+ */
+MPACK_INLINE float mpack_shorten_raw_double_to_float(uint64_t d) {
+    MPACK_CHECK_FLOAT_ORDER();
+    union {
+        float f;
+        uint32_t u;
+    } v;
+
+    // float has  1 bit sign,  8 bits exponent, 23 bits significand
+    // double has 1 bit sign, 11 bits exponent, 52 bits significand
+
+    uint64_t d_sign = (uint64_t)(d >> 63);
+    uint64_t d_exponent = (uint32_t)(d >> 52) & ((1 << 11) - 1);
+    uint64_t d_significand = d & (((uint64_t)1 << 52) - 1);
+
+    uint32_t f_sign = (uint32_t)d_sign;
+    uint32_t f_exponent;
+    uint32_t f_significand;
+
+    if (MPACK_UNLIKELY(d_exponent == ((1 << 11) - 1))) {
+        // infinity or NAN. shift down to preserve the top bit since it
+        // indicates signaling NAN, but also set the low bit if any bits were
+        // set (that way we can't shift NAN to infinity.)
+        f_exponent = ((1 << 8) - 1);
+        f_significand = (uint32_t)(d_significand >> 29) | (d_significand ? 1 : 0);
+
+    } else {
+        int fix_bias = (int)d_exponent - ((1 << 10) - 1) + ((1 << 7) - 1);
+        if (MPACK_UNLIKELY(fix_bias <= 0)) {
+            // we don't currently handle subnormal numbers. just set it to zero.
+            f_exponent = 0;
+            f_significand = 0;
+        } else if (MPACK_UNLIKELY(fix_bias > 0xff)) {
+            // exponent is too large; saturate to infinity
+            f_exponent = 0xff;
+            f_significand = 0;
+        } else {
+            // a normal number that fits in a float. this is the usual case.
+            f_exponent = (uint32_t)fix_bias;
+            f_significand = (uint32_t)(d_significand >> 29);
+        }
+    }
+
+    #if 0
+    printf("\n===============\n");
+    for (size_t i = 0; i < 64; ++i)
+        printf("%i%s",(int)((d>>(63-i))&1),((i%8)==7)?" ":"");
+    printf("\n%lu %lu %lu\n", d_sign, d_exponent, d_significand);
+    printf("%u %u %u\n", f_sign, f_exponent, f_significand);
+    #endif
+
+    v.u = (f_sign << 31) | (f_exponent << 23) | f_significand;
+    return v.f;
+}
+#endif
+
+/** @endcond */
+
+
+
+/** @cond */
+
+// Sizes in bytes for the various possible tags
+#define MPACK_TAG_SIZE_FIXUINT  1
+#define MPACK_TAG_SIZE_U8       2
+#define MPACK_TAG_SIZE_U16      3
+#define MPACK_TAG_SIZE_U32      5
+#define MPACK_TAG_SIZE_U64      9
+#define MPACK_TAG_SIZE_FIXINT   1
+#define MPACK_TAG_SIZE_I8       2
+#define MPACK_TAG_SIZE_I16      3
+#define MPACK_TAG_SIZE_I32      5
+#define MPACK_TAG_SIZE_I64      9
+#define MPACK_TAG_SIZE_FLOAT    5
+#define MPACK_TAG_SIZE_DOUBLE   9
+#define MPACK_TAG_SIZE_FIXARRAY 1
+#define MPACK_TAG_SIZE_ARRAY16  3
+#define MPACK_TAG_SIZE_ARRAY32  5
+#define MPACK_TAG_SIZE_FIXMAP   1
+#define MPACK_TAG_SIZE_MAP16    3
+#define MPACK_TAG_SIZE_MAP32    5
+#define MPACK_TAG_SIZE_FIXSTR   1
+#define MPACK_TAG_SIZE_STR8     2
+#define MPACK_TAG_SIZE_STR16    3
+#define MPACK_TAG_SIZE_STR32    5
+#define MPACK_TAG_SIZE_BIN8     2
+#define MPACK_TAG_SIZE_BIN16    3
+#define MPACK_TAG_SIZE_BIN32    5
+#define MPACK_TAG_SIZE_FIXEXT1  2
+#define MPACK_TAG_SIZE_FIXEXT2  2
+#define MPACK_TAG_SIZE_FIXEXT4  2
+#define MPACK_TAG_SIZE_FIXEXT8  2
+#define MPACK_TAG_SIZE_FIXEXT16 2
+#define MPACK_TAG_SIZE_EXT8     3
+#define MPACK_TAG_SIZE_EXT16    4
+#define MPACK_TAG_SIZE_EXT32    6
+
+// size in bytes for complete ext types
+#define MPACK_EXT_SIZE_TIMESTAMP4 (MPACK_TAG_SIZE_FIXEXT4 + 4)
+#define MPACK_EXT_SIZE_TIMESTAMP8 (MPACK_TAG_SIZE_FIXEXT8 + 8)
+#define MPACK_EXT_SIZE_TIMESTAMP12 (MPACK_TAG_SIZE_EXT8 + 12)
+
+/** @endcond */
+
+
+
+#if MPACK_READ_TRACKING || MPACK_WRITE_TRACKING
+/* Tracks the write state of compound elements (maps, arrays, */
+/* strings, binary blobs and extension types) */
+/** @cond */
+
+typedef struct mpack_track_element_t {
+    mpack_type_t type;
+    uint32_t left;
+
+    // indicates that a value still needs to be read/written for an already
+    // read/written key. left is not decremented until both key and value are
+    // read/written.
+    bool key_needs_value;
+
+    // tracks whether the map/array being written is using a builder. if true,
+    // the number of elements is automatic, and left is 0.
+    bool builder;
+} mpack_track_element_t;
+
+typedef struct mpack_track_t {
+    size_t count;
+    size_t capacity;
+    mpack_track_element_t* elements;
+} mpack_track_t;
+
+#if MPACK_INTERNAL
+mpack_error_t mpack_track_init(mpack_track_t* track);
+mpack_error_t mpack_track_grow(mpack_track_t* track);
+mpack_error_t mpack_track_push(mpack_track_t* track, mpack_type_t type, uint32_t count);
+mpack_error_t mpack_track_push_builder(mpack_track_t* track, mpack_type_t type);
+mpack_error_t mpack_track_pop(mpack_track_t* track, mpack_type_t type);
+mpack_error_t mpack_track_pop_builder(mpack_track_t* track, mpack_type_t type);
+mpack_error_t mpack_track_element(mpack_track_t* track, bool read);
+mpack_error_t mpack_track_peek_element(mpack_track_t* track, bool read);
+mpack_error_t mpack_track_bytes(mpack_track_t* track, bool read, size_t count);
+mpack_error_t mpack_track_str_bytes_all(mpack_track_t* track, bool read, size_t count);
+mpack_error_t mpack_track_check_empty(mpack_track_t* track);
+mpack_error_t mpack_track_destroy(mpack_track_t* track, bool cancel);
+#endif
+
+/** @endcond */
+#endif
+
+
+
+#if MPACK_INTERNAL
+/** @cond */
+
+
+
+/* Miscellaneous string functions */
+
+/**
+ * Returns true if the given UTF-8 string is valid.
+ */
+bool mpack_utf8_check(const char* str, size_t bytes);
+
+/**
+ * Returns true if the given UTF-8 string is valid and contains no null characters.
+ */
+bool mpack_utf8_check_no_null(const char* str, size_t bytes);
+
+/**
+ * Returns true if the given string has no null bytes.
+ */
+bool mpack_str_check_no_null(const char* str, size_t bytes);
+
+
+
+/** @endcond */
+#endif
+
+
+
+/**
+ * @}
+ */
+
+MPACK_EXTERN_C_END
+MPACK_SILENCE_WARNINGS_END
+
+#endif
+
+
+/* mpack/mpack-writer.h.h */
+
+/**
+ * @file
+ *
+ * Declares the MPack Writer.
+ */
+
+#ifndef MPACK_WRITER_H
+#define MPACK_WRITER_H 1
+
+/* #include "mpack-common.h" */
+
+#if MPACK_WRITER
+
+MPACK_SILENCE_WARNINGS_BEGIN
+MPACK_EXTERN_C_BEGIN
+
+#if MPACK_WRITE_TRACKING
+struct mpack_track_t;
+#endif
+
+/**
+ * @defgroup writer Write API
+ *
+ * The MPack Write API encodes structured data of a fixed (hardcoded) schema to MessagePack.
+ *
+ * @{
+ */
+
+/**
+ * @def MPACK_WRITER_MINIMUM_BUFFER_SIZE
+ *
+ * The minimum buffer size for a writer with a flush function.
+ */
+#define MPACK_WRITER_MINIMUM_BUFFER_SIZE 32
+
+/**
+ * A buffered MessagePack encoder.
+ *
+ * The encoder wraps an existing buffer and, optionally, a flush function.
+ * This allows efficiently encoding to an in-memory buffer or to a stream.
+ *
+ * All write operations are synchronous; they will block until the
+ * data is fully written, or an error occurs.
+ */
+typedef struct mpack_writer_t mpack_writer_t;
+
+/**
+ * The MPack writer's flush function to flush the buffer to the output stream.
+ * It should flag an appropriate error on the writer if flushing fails (usually
+ * mpack_error_io or mpack_error_memory.)
+ *
+ * The specified context for callbacks is at writer->context.
+ */
+typedef void (*mpack_writer_flush_t)(mpack_writer_t* writer, const char* buffer, size_t count);
+
+/**
+ * An error handler function to be called when an error is flagged on
+ * the writer.
+ *
+ * The error handler will only be called once on the first error flagged;
+ * any subsequent writes and errors are ignored, and the writer is
+ * permanently in that error state.
+ *
+ * MPack is safe against non-local jumps out of error handler callbacks.
+ * This means you are allowed to longjmp or throw an exception (in C++,
+ * Objective-C, or with SEH) out of this callback.
+ *
+ * Bear in mind when using longjmp that local non-volatile variables that
+ * have changed are undefined when setjmp() returns, so you can't put the
+ * writer on the stack in the same activation frame as the setjmp without
+ * declaring it volatile.
+ *
+ * You must still eventually destroy the writer. It is not destroyed
+ * automatically when an error is flagged. It is safe to destroy the
+ * writer within this error callback, but you will either need to perform
+ * a non-local jump, or store something in your context to identify
+ * that the writer is destroyed since any future accesses to it cause
+ * undefined behavior.
+ */
+typedef void (*mpack_writer_error_t)(mpack_writer_t* writer, mpack_error_t error);
+
+/**
+ * A teardown function to be called when the writer is destroyed.
+ */
+typedef void (*mpack_writer_teardown_t)(mpack_writer_t* writer);
+
+/* Hide internals from documentation */
+/** @cond */
+
+#if MPACK_BUILDER
+/**
+ * Build buffer pages form a linked list.
+ *
+ * They don't always fill up. If there is not enough space within them to write
+ * a tag or place an mpack_build_t, a new page is allocated. For this reason
+ * they store the number of used bytes.
+ */
+typedef struct mpack_builder_page_t {
+    struct mpack_builder_page_t* next;
+    size_t bytes_used;
+} mpack_builder_page_t;
+
+/**
+ * Builds form a linked list of mpack_build_t, interleaved with their encoded
+ * contents directly in the paged builder buffer.
+ */
+typedef struct mpack_build_t {
+    //mpack_builder_page_t* page;
+    struct mpack_build_t* parent;
+    //struct mpack_build_t* next;
+
+    size_t bytes; // number of bytes between this build and the next one
+    uint32_t count; // number of elements (or key/value pairs) in this map/array
+    mpack_type_t type;
+
+    // depth of nested non-build compound elements within this
+    // build.
+    uint32_t nested_compound_elements;
+
+    // indicates that a value still needs to be written for an already
+    // written key. count is not incremented until both key and value are
+    // written.
+    bool key_needs_value;
+} mpack_build_t;
+
+/**
+ * The builder state. This is stored within mpack_writer_t.
+ */
+typedef struct mpack_builder_t {
+    mpack_build_t* current_build; // build which is accumulating elements
+    mpack_build_t* latest_build; // build which is accumulating bytes
+    mpack_builder_page_t* current_page;
+    mpack_builder_page_t* pages;
+    char* stash_buffer;
+    char* stash_position;
+    char* stash_end;
+    #if MPACK_BUILDER_INTERNAL_STORAGE
+    char internal[MPACK_BUILDER_INTERNAL_STORAGE_SIZE];
+    #endif
+} mpack_builder_t;
+#endif
+
+struct mpack_writer_t {
+    #if MPACK_COMPATIBILITY
+    mpack_version_t version;          /* Version of the MessagePack spec to write */
+    #endif
+    mpack_writer_flush_t flush;       /* Function to write bytes to the output stream */
+    mpack_writer_error_t error_fn;    /* Function to call on error */
+    mpack_writer_teardown_t teardown; /* Function to teardown the context on destroy */
+    void* context;                    /* Context for writer callbacks */
+
+    char* buffer;         /* Byte buffer */
+    char* position;       /* Current position within the buffer */
+    char* end;            /* The end of the buffer */
+    mpack_error_t error;  /* Error state */
+
+    #if MPACK_WRITE_TRACKING
+    mpack_track_t track; /* Stack of map/array/str/bin/ext writes */
+    #endif
+
+    #ifdef MPACK_MALLOC
+    /* Reserved. You can use this space to allocate a custom
+     * context in order to reduce heap allocations. */
+    void* reserved[2];
+    #endif
+
+    #if MPACK_BUILDER
+    mpack_builder_t builder;
+    #endif
+};
+
+
+#if MPACK_WRITE_TRACKING
+void mpack_writer_track_push(mpack_writer_t* writer, mpack_type_t type, uint32_t count);
+void mpack_writer_track_push_builder(mpack_writer_t* writer, mpack_type_t type);
+void mpack_writer_track_pop(mpack_writer_t* writer, mpack_type_t type);
+void mpack_writer_track_pop_builder(mpack_writer_t* writer, mpack_type_t type);
+void mpack_writer_track_bytes(mpack_writer_t* writer, size_t count);
+#else
+MPACK_INLINE void mpack_writer_track_push(mpack_writer_t* writer, mpack_type_t type, uint32_t count) {
+    MPACK_UNUSED(writer);
+    MPACK_UNUSED(type);
+    MPACK_UNUSED(count);
+}
+MPACK_INLINE void mpack_writer_track_push_builder(mpack_writer_t* writer, mpack_type_t type) {
+    MPACK_UNUSED(writer);
+    MPACK_UNUSED(type);
+}
+MPACK_INLINE void mpack_writer_track_pop(mpack_writer_t* writer, mpack_type_t type) {
+    MPACK_UNUSED(writer);
+    MPACK_UNUSED(type);
+}
+MPACK_INLINE void mpack_writer_track_pop_builder(mpack_writer_t* writer, mpack_type_t type) {
+    MPACK_UNUSED(writer);
+    MPACK_UNUSED(type);
+}
+MPACK_INLINE void mpack_writer_track_bytes(mpack_writer_t* writer, size_t count) {
+    MPACK_UNUSED(writer);
+    MPACK_UNUSED(count);
+}
+#endif
+
+/** @endcond */
+
+/**
+ * @name Lifecycle Functions
+ * @{
+ */
+
+/**
+ * Initializes an MPack writer with the given buffer. The writer
+ * does not assume ownership of the buffer.
+ *
+ * Trying to write past the end of the buffer will result in mpack_error_too_big
+ * unless a flush function is set with mpack_writer_set_flush(). To use the data
+ * without flushing, call mpack_writer_buffer_used() to determine the number of
+ * bytes written.
+ *
+ * @param writer The MPack writer.
+ * @param buffer The buffer into which to write MessagePack data.
+ * @param size The size of the buffer.
+ */
+void mpack_writer_init(mpack_writer_t* writer, char* buffer, size_t size);
+
+#ifdef MPACK_MALLOC
+/**
+ * Initializes an MPack writer using a growable buffer.
+ *
+ * The data is placed in the given data pointer if and when the writer
+ * is destroyed without error. The data pointer is NULL during writing,
+ * and will remain NULL if an error occurs.
+ *
+ * The allocated data must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @throws mpack_error_memory if the buffer fails to grow when
+ * flushing.
+ *
+ * @param writer The MPack writer.
+ * @param data Where to place the allocated data.
+ * @param size Where to write the size of the data.
+ */
+void mpack_writer_init_growable(mpack_writer_t* writer, char** data, size_t* size);
+#endif
+
+/**
+ * Initializes an MPack writer directly into an error state. Use this if you
+ * are writing a wrapper to mpack_writer_init() which can fail its setup.
+ */
+void mpack_writer_init_error(mpack_writer_t* writer, mpack_error_t error);
+
+#if MPACK_STDIO
+/**
+ * Initializes an MPack writer that writes to a file.
+ *
+ * @throws mpack_error_memory if allocation fails
+ * @throws mpack_error_io if the file cannot be opened
+ */
+void mpack_writer_init_filename(mpack_writer_t* writer, const char* filename);
+
+/**
+ * Deprecated.
+ *
+ * \deprecated Renamed to mpack_writer_init_filename().
+ */
+MPACK_INLINE void mpack_writer_init_file(mpack_writer_t* writer, const char* filename) {
+    mpack_writer_init_filename(writer, filename);
+}
+
+/**
+ * Initializes an MPack writer that writes to a libc FILE. This can be used to
+ * write to stdout or stderr, or to a file opened separately.
+ *
+ * @param writer The MPack writer.
+ * @param stdfile The FILE.
+ * @param close_when_done If true, fclose() will be called on the FILE when it
+ *         is no longer needed. If false, the file will not be flushed or
+ *         closed when writing is done.
+ *
+ * @note The writer is buffered. If you want to write other data to the FILE in
+ *         between messages, you must flush it first.
+ *
+ * @see mpack_writer_flush_message
+ */
+void mpack_writer_init_stdfile(mpack_writer_t* writer, FILE* stdfile, bool close_when_done);
+#endif
+
+/** @cond */
+
+#define mpack_writer_init_stack_line_ex(line, writer) \
+    char mpack_buf_##line[MPACK_STACK_SIZE]; \
+    mpack_writer_init(writer, mpack_buf_##line, sizeof(mpack_buf_##line))
+
+#define mpack_writer_init_stack_line(line, writer) \
+    mpack_writer_init_stack_line_ex(line, writer)
+
+/*
+ * Initializes an MPack writer using stack space as a buffer. A flush function
+ * should be added to the writer to flush the buffer.
+ *
+ * This is currently undocumented since it's not entirely useful on its own.
+ */
+
+#define mpack_writer_init_stack(writer) \
+    mpack_writer_init_stack_line(__LINE__, (writer))
+
+/** @endcond */
+
+/**
+ * Cleans up the MPack writer, flushing and closing the underlying stream,
+ * if any. Returns the final error state of the writer.
+ *
+ * No flushing is performed if the writer is in an error state. The attached
+ * teardown function is called whether or not the writer is in an error state.
+ *
+ * This will assert in tracking mode if the writer is not in an error
+ * state and has any unclosed compound types. If you want to cancel
+ * writing in the middle of a document, you need to flag an error on
+ * the writer before destroying it (such as mpack_error_data).
+ *
+ * Note that a writer may raise an error and call your error handler during
+ * the final flush. It is safe to longjmp or throw out of this error handler,
+ * but if you do, the writer will not be destroyed, and the teardown function
+ * will not be called. You can still get the writer's error state, and you
+ * must call @ref mpack_writer_destroy() again. (The second call is guaranteed
+ * not to call your error handler again since the writer is already in an error
+ * state.)
+ *
+ * @see mpack_writer_set_error_handler
+ * @see mpack_writer_set_flush
+ * @see mpack_writer_set_teardown
+ * @see mpack_writer_flag_error
+ * @see mpack_error_data
+ */
+mpack_error_t mpack_writer_destroy(mpack_writer_t* writer);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Configuration
+ * @{
+ */
+
+#if MPACK_COMPATIBILITY
+/**
+ * Sets the version of the MessagePack spec that will be generated.
+ *
+ * This can be used to interface with older libraries that do not support
+ * the newest MessagePack features (such as the @c str8 type.)
+ *
+ * @note This requires @ref MPACK_COMPATIBILITY.
+ */
+MPACK_INLINE void mpack_writer_set_version(mpack_writer_t* writer, mpack_version_t version) {
+    writer->version = version;
+}
+#endif
+
+/**
+ * Sets the custom pointer to pass to the writer callbacks, such as flush
+ * or teardown.
+ *
+ * @param writer The MPack writer.
+ * @param context User data to pass to the writer callbacks.
+ *
+ * @see mpack_writer_context()
+ */
+MPACK_INLINE void mpack_writer_set_context(mpack_writer_t* writer, void* context) {
+    writer->context = context;
+}
+
+/**
+ * Returns the custom context for writer callbacks.
+ *
+ * @see mpack_writer_set_context
+ * @see mpack_writer_set_flush
+ */
+MPACK_INLINE void* mpack_writer_context(mpack_writer_t* writer) {
+    return writer->context;
+}
+
+/**
+ * Sets the flush function to write out the data when the buffer is full.
+ *
+ * If no flush function is used, trying to write past the end of the
+ * buffer will result in mpack_error_too_big.
+ *
+ * This should normally be used with mpack_writer_set_context() to register
+ * a custom pointer to pass to the flush function.
+ *
+ * @param writer The MPack writer.
+ * @param flush The function to write out data from the buffer.
+ *
+ * @see mpack_writer_context()
+ */
+void mpack_writer_set_flush(mpack_writer_t* writer, mpack_writer_flush_t flush);
+
+/**
+ * Sets the error function to call when an error is flagged on the writer.
+ *
+ * This should normally be used with mpack_writer_set_context() to register
+ * a custom pointer to pass to the error function.
+ *
+ * See the definition of mpack_writer_error_t for more information about
+ * what you can do from an error callback.
+ *
+ * @see mpack_writer_error_t
+ * @param writer The MPack writer.
+ * @param error_fn The function to call when an error is flagged on the writer.
+ */
+MPACK_INLINE void mpack_writer_set_error_handler(mpack_writer_t* writer, mpack_writer_error_t error_fn) {
+    writer->error_fn = error_fn;
+}
+
+/**
+ * Sets the teardown function to call when the writer is destroyed.
+ *
+ * This should normally be used with mpack_writer_set_context() to register
+ * a custom pointer to pass to the teardown function.
+ *
+ * @param writer The MPack writer.
+ * @param teardown The function to call when the writer is destroyed.
+ */
+MPACK_INLINE void mpack_writer_set_teardown(mpack_writer_t* writer, mpack_writer_teardown_t teardown) {
+    writer->teardown = teardown;
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @name Core Writer Functions
+ * @{
+ */
+
+/**
+ * Flushes any buffered data to the underlying stream.
+ *
+ * If the writer is connected to a socket and you are keeping it open,
+ * you will want to call this after writing a message (or set of
+ * messages) so that the data is actually sent.
+ *
+ * It is not necessary to call this if you are not keeping the writer
+ * open afterwards. You can just call `mpack_writer_destroy()` and it
+ * will flush before cleaning up.
+ *
+ * This will assert if no flush function is assigned to the writer.
+ *
+ * If write tracking is enabled, this will break and flag @ref
+ * mpack_error_bug if the writer has any open compound types, ensuring
+ * that no compound types are still open. This prevents a "missing
+ * finish" bug from causing a never-ending message.
+ */
+void mpack_writer_flush_message(mpack_writer_t* writer);
+
+/**
+ * Returns the number of bytes currently stored in the buffer. This
+ * may be less than the total number of bytes written if bytes have
+ * been flushed to an underlying stream.
+ */
+MPACK_INLINE size_t mpack_writer_buffer_used(mpack_writer_t* writer) {
+    return (size_t)(writer->position - writer->buffer);
+}
+
+/**
+ * Returns the amount of space left in the buffer. This may be reset
+ * after a write if bytes are flushed to an underlying stream.
+ */
+MPACK_INLINE size_t mpack_writer_buffer_left(mpack_writer_t* writer) {
+    return (size_t)(writer->end - writer->position);
+}
+
+/**
+ * Returns the (current) size of the buffer. This may change after a write if
+ * the flush callback changes the buffer.
+ */
+MPACK_INLINE size_t mpack_writer_buffer_size(mpack_writer_t* writer) {
+    return (size_t)(writer->end - writer->buffer);
+}
+
+/**
+ * Places the writer in the given error state, calling the error callback if one
+ * is set.
+ *
+ * This allows you to externally flag errors, for example if you are validating
+ * data as you write it, or if you want to cancel writing in the middle of a
+ * document. (The writer will assert if you try to destroy it without error and
+ * with unclosed compound types. In this case you should flag mpack_error_data
+ * before destroying it.)
+ *
+ * If the writer is already in an error state, this call is ignored and no
+ * error callback is called.
+ *
+ * @see mpack_writer_destroy
+ * @see mpack_error_data
+ */
+void mpack_writer_flag_error(mpack_writer_t* writer, mpack_error_t error);
+
+/**
+ * Queries the error state of the MPack writer.
+ *
+ * If a writer is in an error state, you should discard all data since the
+ * last time the error flag was checked. The error flag cannot be cleared.
+ */
+MPACK_INLINE mpack_error_t mpack_writer_error(mpack_writer_t* writer) {
+    return writer->error;
+}
+
+/**
+ * Writes a MessagePack object header (an MPack Tag.)
+ *
+ * If the value is a map, array, string, binary or extension type, the
+ * containing elements or bytes must be written separately and the
+ * appropriate finish function must be called (as though one of the
+ * mpack_start_*() functions was called.)
+ *
+ * @see mpack_write_bytes()
+ * @see mpack_finish_map()
+ * @see mpack_finish_array()
+ * @see mpack_finish_str()
+ * @see mpack_finish_bin()
+ * @see mpack_finish_ext()
+ * @see mpack_finish_type()
+ */
+void mpack_write_tag(mpack_writer_t* writer, mpack_tag_t tag);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Integers
+ * @{
+ */
+
+/** Writes an 8-bit integer in the most efficient packing available. */
+void mpack_write_i8(mpack_writer_t* writer, int8_t value);
+
+/** Writes a 16-bit integer in the most efficient packing available. */
+void mpack_write_i16(mpack_writer_t* writer, int16_t value);
+
+/** Writes a 32-bit integer in the most efficient packing available. */
+void mpack_write_i32(mpack_writer_t* writer, int32_t value);
+
+/** Writes a 64-bit integer in the most efficient packing available. */
+void mpack_write_i64(mpack_writer_t* writer, int64_t value);
+
+/** Writes an integer in the most efficient packing available. */
+MPACK_INLINE void mpack_write_int(mpack_writer_t* writer, int64_t value) {
+    mpack_write_i64(writer, value);
+}
+
+/** Writes an 8-bit unsigned integer in the most efficient packing available. */
+void mpack_write_u8(mpack_writer_t* writer, uint8_t value);
+
+/** Writes an 16-bit unsigned integer in the most efficient packing available. */
+void mpack_write_u16(mpack_writer_t* writer, uint16_t value);
+
+/** Writes an 32-bit unsigned integer in the most efficient packing available. */
+void mpack_write_u32(mpack_writer_t* writer, uint32_t value);
+
+/** Writes an 64-bit unsigned integer in the most efficient packing available. */
+void mpack_write_u64(mpack_writer_t* writer, uint64_t value);
+
+/** Writes an unsigned integer in the most efficient packing available. */
+MPACK_INLINE void mpack_write_uint(mpack_writer_t* writer, uint64_t value) {
+    mpack_write_u64(writer, value);
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @name Other Basic Types
+ * @{
+ */
+
+#if MPACK_FLOAT
+/** Writes a float. */
+void mpack_write_float(mpack_writer_t* writer, float value);
+#else
+/** Writes a float from a raw uint32_t. */
+void mpack_write_raw_float(mpack_writer_t* writer, uint32_t raw_value);
+#endif
+
+#if MPACK_DOUBLE
+/** Writes a double. */
+void mpack_write_double(mpack_writer_t* writer, double value);
+#else
+/** Writes a double from a raw uint64_t. */
+void mpack_write_raw_double(mpack_writer_t* writer, uint64_t raw_value);
+#endif
+
+/** Writes a boolean. */
+void mpack_write_bool(mpack_writer_t* writer, bool value);
+
+/** Writes a boolean with value true. */
+void mpack_write_true(mpack_writer_t* writer);
+
+/** Writes a boolean with value false. */
+void mpack_write_false(mpack_writer_t* writer);
+
+/** Writes a nil. */
+void mpack_write_nil(mpack_writer_t* writer);
+
+/** Write a pre-encoded messagepack object */
+void mpack_write_object_bytes(mpack_writer_t* writer, const char* data, size_t bytes);
+
+#if MPACK_EXTENSIONS
+/**
+ * Writes a timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @param writer The writer
+ * @param seconds The (signed) number of seconds since 1970-01-01T00:00:00Z.
+ * @param nanoseconds The additional number of nanoseconds from 0 to 999,999,999 inclusive.
+ */
+void mpack_write_timestamp(mpack_writer_t* writer, int64_t seconds, uint32_t nanoseconds);
+
+/**
+ * Writes a timestamp with the given number of seconds (and zero nanoseconds).
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @param writer The writer
+ * @param seconds The (signed) number of seconds since 1970-01-01T00:00:00Z.
+ */
+MPACK_INLINE void mpack_write_timestamp_seconds(mpack_writer_t* writer, int64_t seconds) {
+    mpack_write_timestamp(writer, seconds, 0);
+}
+
+/**
+ * Writes a timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+MPACK_INLINE void mpack_write_timestamp_struct(mpack_writer_t* writer, mpack_timestamp_t timestamp) {
+    mpack_write_timestamp(writer, timestamp.seconds, timestamp.nanoseconds);
+}
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Map and Array Functions
+ * @{
+ */
+
+/**
+ * Opens an array.
+ *
+ * `count` elements must follow, and mpack_finish_array() must be called
+ * when done.
+ *
+ * If you do not know the number of elements to be written ahead of time, call
+ * mpack_build_array() instead.
+ *
+ * @see mpack_finish_array()
+ * @see mpack_build_array() to count the number of elements automatically
+ */
+void mpack_start_array(mpack_writer_t* writer, uint32_t count);
+
+/**
+ * Opens a map.
+ *
+ * `count * 2` elements must follow, and mpack_finish_map() must be called
+ * when done.
+ *
+ * If you do not know the number of elements to be written ahead of time, call
+ * mpack_build_map() instead.
+ *
+ * Remember that while map elements in MessagePack are implicitly ordered,
+ * they are not ordered in JSON. If you need elements to be read back
+ * in the order they are written, consider use an array instead.
+ *
+ * @see mpack_finish_map()
+ * @see mpack_build_map() to count the number of key/value pairs automatically
+ */
+void mpack_start_map(mpack_writer_t* writer, uint32_t count);
+
+MPACK_INLINE void mpack_builder_compound_push(mpack_writer_t* writer) {
+    MPACK_UNUSED(writer);
+
+    #if MPACK_BUILDER
+    mpack_build_t* build = writer->builder.current_build;
+    if (build != NULL) {
+        ++build->nested_compound_elements;
+    }
+    #endif
+}
+
+MPACK_INLINE void mpack_builder_compound_pop(mpack_writer_t* writer) {
+    MPACK_UNUSED(writer);
+
+    #if MPACK_BUILDER
+    mpack_build_t* build = writer->builder.current_build;
+    if (build != NULL) {
+        mpack_assert(build->nested_compound_elements > 0);
+        --build->nested_compound_elements;
+    }
+    #endif
+}
+
+/**
+ * Finishes writing an array.
+ *
+ * This should be called only after a corresponding call to mpack_start_array()
+ * and after the array contents are written.
+ *
+ * In debug mode (or if MPACK_WRITE_TRACKING is not 0), this will track writes
+ * to ensure that the correct number of elements are written.
+ *
+ * @see mpack_start_array()
+ */
+MPACK_INLINE void mpack_finish_array(mpack_writer_t* writer) {
+    mpack_writer_track_pop(writer, mpack_type_array);
+    mpack_builder_compound_pop(writer);
+}
+
+/**
+ * Finishes writing a map.
+ *
+ * This should be called only after a corresponding call to mpack_start_map()
+ * and after the map contents are written.
+ *
+ * In debug mode (or if MPACK_WRITE_TRACKING is not 0), this will track writes
+ * to ensure that the correct number of elements are written.
+ *
+ * @see mpack_start_map()
+ */
+MPACK_INLINE void mpack_finish_map(mpack_writer_t* writer) {
+    mpack_writer_track_pop(writer, mpack_type_map);
+    mpack_builder_compound_pop(writer);
+}
+
+/**
+ * Starts building an array.
+ *
+ * Elements must follow, and mpack_complete_map() must be called when done. The
+ * number of elements is determined automatically.
+ *
+ * If you know ahead of time the number of elements in the array, it is more
+ * efficient to call mpack_start_array() instead, even if you are already
+ * within another open build.
+ *
+ * Builder containers can be nested within normal (known size) containers and
+ * vice versa. You can call mpack_build_array(), then mpack_start_array()
+ * inside it, then mpack_build_array() inside that, and so forth.
+ *
+ * @see mpack_complete_array() to complete this array
+ * @see mpack_start_array() if you already know the size of the array
+ * @see mpack_build_map() for implementation details
+ */
+void mpack_build_array(struct mpack_writer_t* writer);
+
+/**
+ * Starts building a map.
+ *
+ * An even number of elements must follow, and mpack_complete_map() must be
+ * called when done. The number of elements is determined automatically.
+ *
+ * If you know ahead of time the number of elements in the map, it is more
+ * efficient to call mpack_start_map() instead, even if you are already within
+ * another open build.
+ *
+ * Builder containers can be nested within normal (known size) containers and
+ * vice versa. You can call mpack_build_map(), then mpack_start_map() inside
+ * it, then mpack_build_map() inside that, and so forth.
+ *
+ * A writer in build mode diverts writes to a builder buffer that allocates as
+ * needed. Once the last map or array being built is completed, the deferred
+ * message is composed with computed array and map sizes into the writer.
+ * Builder maps and arrays are encoded exactly the same as ordinary maps and
+ * arrays in the final message.
+ *
+ * This indirect encoding is costly, as it incurs at least an extra copy of all
+ * data written within a builder (but not additional copies for nested
+ * builders.) Expect a speed penalty of half or more.
+ *
+ * A good strategy is to use this during early development when your messages
+ * are constantly changing, and then closer to release when your message
+ * formats have stabilized, replace all your build calls with start calls with
+ * pre-computed sizes. Or don't, if you find the builder has little impact on
+ * performance, because even with builders MPack is extremely fast.
+ *
+ * @note When an array or map starts being built, nothing will be flushed
+ *       until it is completed. If you are building a large message that
+ *       does not fit in the output stream, you won't get an error about it
+ *       until everything is written.
+ *
+ * @see mpack_complete_map() to complete this map
+ * @see mpack_start_map() if you already know the size of the map
+ */
+void mpack_build_map(struct mpack_writer_t* writer);
+
+/**
+ * Completes an array being built.
+ *
+ * @see mpack_build_array()
+ */
+void mpack_complete_array(struct mpack_writer_t* writer);
+
+/**
+ * Completes a map being built.
+ *
+ * @see mpack_build_map()
+ */
+void mpack_complete_map(struct mpack_writer_t* writer);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Data Helpers
+ * @{
+ */
+
+/**
+ * Writes a string.
+ *
+ * To stream a string in chunks, use mpack_start_str() instead.
+ *
+ * MPack does not care about the underlying encoding, but UTF-8 is highly
+ * recommended, especially for compatibility with JSON. You should consider
+ * calling mpack_write_utf8() instead, especially if you will be reading
+ * it back as UTF-8.
+ *
+ * You should not call mpack_finish_str() after calling this; this
+ * performs both start and finish.
+ */
+void mpack_write_str(mpack_writer_t* writer, const char* str, uint32_t length);
+
+/**
+ * Writes a string, ensuring that it is valid UTF-8.
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed.
+ *
+ * You should not call mpack_finish_str() after calling this; this
+ * performs both start and finish.
+ *
+ * @throws mpack_error_invalid if the string is not valid UTF-8
+ */
+void mpack_write_utf8(mpack_writer_t* writer, const char* str, uint32_t length);
+
+/**
+ * Writes a null-terminated string. (The null-terminator is not written.)
+ *
+ * MPack does not care about the underlying encoding, but UTF-8 is highly
+ * recommended, especially for compatibility with JSON. You should consider
+ * calling mpack_write_utf8_cstr() instead, especially if you will be reading
+ * it back as UTF-8.
+ *
+ * You should not call mpack_finish_str() after calling this; this
+ * performs both start and finish.
+ */
+void mpack_write_cstr(mpack_writer_t* writer, const char* cstr);
+
+/**
+ * Writes a null-terminated string, or a nil node if the given cstr pointer
+ * is NULL. (The null-terminator is not written.)
+ *
+ * MPack does not care about the underlying encoding, but UTF-8 is highly
+ * recommended, especially for compatibility with JSON. You should consider
+ * calling mpack_write_utf8_cstr_or_nil() instead, especially if you will
+ * be reading it back as UTF-8.
+ *
+ * You should not call mpack_finish_str() after calling this; this
+ * performs both start and finish.
+ */
+void mpack_write_cstr_or_nil(mpack_writer_t* writer, const char* cstr);
+
+/**
+ * Writes a null-terminated string, ensuring that it is valid UTF-8. (The
+ * null-terminator is not written.)
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed.
+ *
+ * You should not call mpack_finish_str() after calling this; this
+ * performs both start and finish.
+ *
+ * @throws mpack_error_invalid if the string is not valid UTF-8
+ */
+void mpack_write_utf8_cstr(mpack_writer_t* writer, const char* cstr);
+
+/**
+ * Writes a null-terminated string ensuring that it is valid UTF-8, or
+ * writes nil if the given cstr pointer is NULL. (The null-terminator
+ * is not written.)
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed.
+ *
+ * You should not call mpack_finish_str() after calling this; this
+ * performs both start and finish.
+ *
+ * @throws mpack_error_invalid if the string is not valid UTF-8
+ */
+void mpack_write_utf8_cstr_or_nil(mpack_writer_t* writer, const char* cstr);
+
+/**
+ * Writes a binary blob.
+ *
+ * To stream a binary blob in chunks, use mpack_start_bin() instead.
+ *
+ * You should not call mpack_finish_bin() after calling this; this
+ * performs both start and finish.
+ */
+void mpack_write_bin(mpack_writer_t* writer, const char* data, uint32_t count);
+
+#if MPACK_EXTENSIONS
+/**
+ * Writes an extension type.
+ *
+ * To stream an extension blob in chunks, use mpack_start_ext() instead.
+ *
+ * Extension types [0, 127] are available for application-specific types. Extension
+ * types [-128, -1] are reserved for future extensions of MessagePack.
+ *
+ * You should not call mpack_finish_ext() after calling this; this
+ * performs both start and finish.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+void mpack_write_ext(mpack_writer_t* writer, int8_t exttype, const char* data, uint32_t count);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Chunked Data Functions
+ * @{
+ */
+
+/**
+ * Opens a string. `count` bytes should be written with calls to
+ * mpack_write_bytes(), and mpack_finish_str() should be called
+ * when done.
+ *
+ * To write an entire string at once, use mpack_write_str() or
+ * mpack_write_cstr() instead.
+ *
+ * MPack does not care about the underlying encoding, but UTF-8 is highly
+ * recommended, especially for compatibility with JSON.
+ */
+void mpack_start_str(mpack_writer_t* writer, uint32_t count);
+
+/**
+ * Opens a binary blob. `count` bytes should be written with calls to
+ * mpack_write_bytes(), and mpack_finish_bin() should be called
+ * when done.
+ */
+void mpack_start_bin(mpack_writer_t* writer, uint32_t count);
+
+#if MPACK_EXTENSIONS
+/**
+ * Opens an extension type. `count` bytes should be written with calls
+ * to mpack_write_bytes(), and mpack_finish_ext() should be called
+ * when done.
+ *
+ * Extension types [0, 127] are available for application-specific types. Extension
+ * types [-128, -1] are reserved for future extensions of MessagePack.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+void mpack_start_ext(mpack_writer_t* writer, int8_t exttype, uint32_t count);
+#endif
+
+/**
+ * Writes a portion of bytes for a string, binary blob or extension type which
+ * was opened by mpack_write_tag() or one of the mpack_start_*() functions.
+ *
+ * This can be called multiple times to write the data in chunks, as long as
+ * the total amount of bytes written matches the count given when the compound
+ * type was started.
+ *
+ * The corresponding mpack_finish_*() function must be called when done.
+ *
+ * To write an entire string, binary blob or extension type at
+ * once, use one of the mpack_write_*() functions instead.
+ *
+ * @see mpack_write_tag()
+ * @see mpack_start_str()
+ * @see mpack_start_bin()
+ * @see mpack_start_ext()
+ * @see mpack_finish_str()
+ * @see mpack_finish_bin()
+ * @see mpack_finish_ext()
+ * @see mpack_finish_type()
+ */
+void mpack_write_bytes(mpack_writer_t* writer, const char* data, size_t count);
+
+/**
+ * Finishes writing a string.
+ *
+ * This should be called only after a corresponding call to mpack_start_str()
+ * and after the string bytes are written with mpack_write_bytes().
+ *
+ * This will track writes to ensure that the correct number of elements are written.
+ *
+ * @see mpack_start_str()
+ * @see mpack_write_bytes()
+ */
+MPACK_INLINE void mpack_finish_str(mpack_writer_t* writer) {
+    mpack_writer_track_pop(writer, mpack_type_str);
+}
+
+/**
+ * Finishes writing a binary blob.
+ *
+ * This should be called only after a corresponding call to mpack_start_bin()
+ * and after the binary bytes are written with mpack_write_bytes().
+ *
+ * This will track writes to ensure that the correct number of bytes are written.
+ *
+ * @see mpack_start_bin()
+ * @see mpack_write_bytes()
+ */
+MPACK_INLINE void mpack_finish_bin(mpack_writer_t* writer) {
+    mpack_writer_track_pop(writer, mpack_type_bin);
+}
+
+#if MPACK_EXTENSIONS
+/**
+ * Finishes writing an extended type binary data blob.
+ *
+ * This should be called only after a corresponding call to mpack_start_bin()
+ * and after the binary bytes are written with mpack_write_bytes().
+ *
+ * This will track writes to ensure that the correct number of bytes are written.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @see mpack_start_ext()
+ * @see mpack_write_bytes()
+ */
+MPACK_INLINE void mpack_finish_ext(mpack_writer_t* writer) {
+    mpack_writer_track_pop(writer, mpack_type_ext);
+}
+#endif
+
+/**
+ * Finishes writing the given compound type.
+ *
+ * This will track writes to ensure that the correct number of elements
+ * or bytes are written.
+ *
+ * This can be called with the appropriate type instead the corresponding
+ * mpack_finish_*() function if you want to finish a dynamic type.
+ */
+MPACK_INLINE void mpack_finish_type(mpack_writer_t* writer, mpack_type_t type) {
+    mpack_writer_track_pop(writer, type);
+}
+
+/**
+ * @}
+ */
+
+#if MPACK_HAS_GENERIC && !defined(__cplusplus)
+
+/**
+ * @name Type-Generic Writers
+ * @{
+ */
+
+/**
+ * @def mpack_write(writer, value)
+ *
+ * Type-generic writer for primitive types.
+ *
+ * The compiler will dispatch to an appropriate write function based
+ * on the type of the @a value parameter.
+ *
+ * @note This requires C11 `_Generic` support. (A set of inline overloads
+ * are used in C++ to provide the same functionality.)
+ *
+ * @warning In C11, the indentifiers `true`, `false` and `NULL` are
+ * all of type `int`, not `bool` or `void*`! They will emit unexpected
+ * types when passed uncast, so be careful when using them.
+ */
+#if MPACK_FLOAT
+    #define MPACK_WRITE_GENERIC_FLOAT float: mpack_write_float,
+#else
+    #define MPACK_WRITE_GENERIC_FLOAT /*nothing*/
+#endif
+#if MPACK_DOUBLE
+    #define MPACK_WRITE_GENERIC_DOUBLE double: mpack_write_double,
+#else
+    #define MPACK_WRITE_GENERIC_DOUBLE /*nothing*/
+#endif
+#define mpack_write(writer, value) \
+    _Generic(((void)0, value),                      \
+              int8_t: mpack_write_i8,               \
+             int16_t: mpack_write_i16,              \
+             int32_t: mpack_write_i32,              \
+             int64_t: mpack_write_i64,              \
+             uint8_t: mpack_write_u8,               \
+            uint16_t: mpack_write_u16,              \
+            uint32_t: mpack_write_u32,              \
+            uint64_t: mpack_write_u64,              \
+                bool: mpack_write_bool,             \
+            MPACK_WRITE_GENERIC_FLOAT               \
+            MPACK_WRITE_GENERIC_DOUBLE              \
+              char *: mpack_write_cstr_or_nil,      \
+        const char *: mpack_write_cstr_or_nil       \
+    )(writer, value)
+
+/**
+ * @def mpack_write_kv(writer, key, value)
+ *
+ * Type-generic writer for key-value pairs of null-terminated string
+ * keys and primitive values.
+ *
+ * @warning @a writer may be evaluated multiple times.
+ *
+ * @warning In C11, the indentifiers `true`, `false` and `NULL` are
+ * all of type `int`, not `bool` or `void*`! They will emit unexpected
+ * types when passed uncast, so be careful when using them.
+ *
+ * @param writer The writer.
+ * @param key A null-terminated C string.
+ * @param value A primitive type supported by mpack_write().
+ */
+#define mpack_write_kv(writer, key, value) do {     \
+    mpack_write_cstr(writer, key);                  \
+    mpack_write(writer, value);                     \
+} while (0)
+
+/**
+ * @}
+ */
+
+#endif // MPACK_HAS_GENERIC && !defined(__cplusplus)
+
+// The rest of this file contains C++ overloads, so we end extern "C" here.
+MPACK_EXTERN_C_END
+
+#if defined(__cplusplus) || defined(MPACK_DOXYGEN)
+
+namespace mpack {
+/**
+ * @name C++ write overloads
+ * @{
+ */
+
+/*
+ * C++ generic writers for primitive values
+ */
+
+#ifdef MPACK_DOXYGEN
+#undef mpack_write
+#undef mpack_write_kv
+#endif
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, int8_t value) {
+    mpack_write_i8(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, int16_t value) {
+    mpack_write_i16(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, int32_t value) {
+    mpack_write_i32(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, int64_t value) {
+    mpack_write_i64(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, uint8_t value) {
+    mpack_write_u8(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, uint16_t value) {
+    mpack_write_u16(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, uint32_t value) {
+    mpack_write_u32(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, uint64_t value) {
+    mpack_write_u64(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, bool value) {
+    mpack_write_bool(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, float value) {
+    mpack_write_float(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, double value) {
+    mpack_write_double(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, char *value) {
+    mpack_write_cstr_or_nil(writer, value);
+}
+
+MPACK_INLINE void mpack_write(mpack_writer_t* writer, const char *value) {
+    mpack_write_cstr_or_nil(writer, value);
+}
+
+/* C++ generic write for key-value pairs */
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, int8_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_i8(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, int16_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_i16(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, int32_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_i32(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, int64_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_i64(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, uint8_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_u8(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, uint16_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_u16(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, uint32_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_u32(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, uint64_t value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_u64(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, bool value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_bool(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, float value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_float(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, double value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_double(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, char *value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_cstr_or_nil(writer, value);
+}
+
+MPACK_INLINE void mpack_write_kv(mpack_writer_t* writer, const char *key, const char *value) {
+    mpack_write_cstr(writer, key);
+    mpack_write_cstr_or_nil(writer, value);
+}
+
+/**
+ * @}
+ */
+}  // namespace mpack
+#endif /* __cplusplus */
+
+/**
+ * @}
+ */
+
+MPACK_SILENCE_WARNINGS_END
+
+#endif // MPACK_WRITER
+
+#endif
+
+/* mpack/mpack-reader.h.h */
+
+/**
+ * @file
+ *
+ * Declares the core MPack Tag Reader.
+ */
+
+#ifndef MPACK_READER_H
+#define MPACK_READER_H 1
+
+/* #include "mpack-common.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+MPACK_EXTERN_C_BEGIN
+
+#if MPACK_READER
+
+#if MPACK_READ_TRACKING
+struct mpack_track_t;
+#endif
+
+// The denominator to determine whether a read is a small
+// fraction of the buffer size.
+#define MPACK_READER_SMALL_FRACTION_DENOMINATOR 32
+
+/**
+ * @defgroup reader Reader API
+ *
+ * The MPack Reader API contains functions for imperatively reading dynamically
+ * typed data from a MessagePack stream.
+ *
+ * See @ref docs/reader.md for examples.
+ *
+ * @note If you are not writing code for an embedded device (or otherwise do
+ * not need maximum performance with minimal memory usage), you should not use
+ * this. You probably want to use the @link node Node API@endlink instead.
+ *
+ * This forms the basis of the @link expect Expect API@endlink, which can be
+ * used to interpret the stream of elements in expected types and value ranges.
+ *
+ * @{
+ */
+
+/**
+ * @def MPACK_READER_MINIMUM_BUFFER_SIZE
+ *
+ * The minimum buffer size for a reader with a fill function.
+ */
+#define MPACK_READER_MINIMUM_BUFFER_SIZE 32
+
+/**
+ * A buffered MessagePack decoder.
+ *
+ * The decoder wraps an existing buffer and, optionally, a fill function.
+ * This allows efficiently decoding data from existing memory buffers, files,
+ * streams, etc.
+ *
+ * All read operations are synchronous; they will block until the
+ * requested data is fully read, or an error occurs.
+ *
+ * This structure is opaque; its fields should not be accessed outside
+ * of MPack.
+ */
+typedef struct mpack_reader_t mpack_reader_t;
+
+/**
+ * The MPack reader's fill function. It should fill the buffer with at
+ * least one byte and at most the given @c count, returning the number
+ * of bytes written to the buffer.
+ *
+ * In case of error, it should flag an appropriate error on the reader
+ * (usually @ref mpack_error_io), or simply return zero. If zero is
+ * returned, mpack_error_io is raised.
+ *
+ * @note When reading from a stream, you should only copy and return
+ * the bytes that are immediately available. It is always safe to return
+ * less than the requested count as long as some non-zero number of bytes
+ * are read; if more bytes are needed, the read function will simply be
+ * called again.
+ *
+ * @see mpack_reader_context()
+ */
+typedef size_t (*mpack_reader_fill_t)(mpack_reader_t* reader, char* buffer, size_t count);
+
+/**
+ * The MPack reader's skip function. It should discard the given number
+ * of bytes from the source (for example by seeking forward.)
+ *
+ * In case of error, it should flag an appropriate error on the reader.
+ *
+ * @see mpack_reader_context()
+ */
+typedef void (*mpack_reader_skip_t)(mpack_reader_t* reader, size_t count);
+
+/**
+ * An error handler function to be called when an error is flagged on
+ * the reader.
+ *
+ * The error handler will only be called once on the first error flagged;
+ * any subsequent reads and errors are ignored, and the reader is
+ * permanently in that error state.
+ *
+ * MPack is safe against non-local jumps out of error handler callbacks.
+ * This means you are allowed to longjmp or throw an exception (in C++,
+ * Objective-C, or with SEH) out of this callback.
+ *
+ * Bear in mind when using longjmp that local non-volatile variables that
+ * have changed are undefined when setjmp() returns, so you can't put the
+ * reader on the stack in the same activation frame as the setjmp without
+ * declaring it volatile.
+ *
+ * You must still eventually destroy the reader. It is not destroyed
+ * automatically when an error is flagged. It is safe to destroy the
+ * reader within this error callback, but you will either need to perform
+ * a non-local jump, or store something in your context to identify
+ * that the reader is destroyed since any future accesses to it cause
+ * undefined behavior.
+ */
+typedef void (*mpack_reader_error_t)(mpack_reader_t* reader, mpack_error_t error);
+
+/**
+ * A teardown function to be called when the reader is destroyed.
+ */
+typedef void (*mpack_reader_teardown_t)(mpack_reader_t* reader);
+
+/* Hide internals from documentation */
+/** @cond */
+
+struct mpack_reader_t {
+    void* context;                    /* Context for reader callbacks */
+    mpack_reader_fill_t fill;         /* Function to read bytes into the buffer */
+    mpack_reader_error_t error_fn;    /* Function to call on error */
+    mpack_reader_teardown_t teardown; /* Function to teardown the context on destroy */
+    mpack_reader_skip_t skip;         /* Function to skip bytes from the source */
+
+    char* buffer;       /* Writeable byte buffer */
+    size_t size;        /* Size of the buffer */
+
+    const char* data;   /* Current data pointer (in the buffer, if it is used) */
+    const char* end;    /* The end of available data (in the buffer, if it is used) */
+
+    mpack_error_t error;  /* Error state */
+
+    #if MPACK_READ_TRACKING
+    mpack_track_t track; /* Stack of map/array/str/bin/ext reads */
+    #endif
+};
+
+/** @endcond */
+
+/**
+ * @name Lifecycle Functions
+ * @{
+ */
+
+/**
+ * Initializes an MPack reader with the given buffer. The reader does
+ * not assume ownership of the buffer, but the buffer must be writeable
+ * if a fill function will be used to refill it.
+ *
+ * @param reader The MPack reader.
+ * @param buffer The buffer with which to read MessagePack data.
+ * @param size The size of the buffer.
+ * @param count The number of bytes already in the buffer.
+ */
+void mpack_reader_init(mpack_reader_t* reader, char* buffer, size_t size, size_t count);
+
+/**
+ * Initializes an MPack reader directly into an error state. Use this if you
+ * are writing a wrapper to mpack_reader_init() which can fail its setup.
+ */
+void mpack_reader_init_error(mpack_reader_t* reader, mpack_error_t error);
+
+/**
+ * Initializes an MPack reader to parse a pre-loaded contiguous chunk of data. The
+ * reader does not assume ownership of the data.
+ *
+ * @param reader The MPack reader.
+ * @param data The data to parse.
+ * @param count The number of bytes pointed to by data.
+ */
+void mpack_reader_init_data(mpack_reader_t* reader, const char* data, size_t count);
+
+#if MPACK_STDIO
+/**
+ * Initializes an MPack reader that reads from a file.
+ *
+ * The file will be automatically opened and closed by the reader.
+ */
+void mpack_reader_init_filename(mpack_reader_t* reader, const char* filename);
+
+/**
+ * Deprecated.
+ *
+ * \deprecated Renamed to mpack_reader_init_filename().
+ */
+MPACK_INLINE void mpack_reader_init_file(mpack_reader_t* reader, const char* filename) {
+    mpack_reader_init_filename(reader, filename);
+}
+
+/**
+ * Initializes an MPack reader that reads from a libc FILE. This can be used to
+ * read from stdin, or from a file opened separately.
+ *
+ * @param reader The MPack reader.
+ * @param stdfile The FILE.
+ * @param close_when_done If true, fclose() will be called on the FILE when it
+ *         is no longer needed. If false, the file will not be closed when
+ *         reading is done.
+ *
+ * @warning The reader is buffered. It will read data in advance of parsing it,
+ * and it may read more data than it parsed. See mpack_reader_remaining() to
+ * access the extra data.
+ */
+void mpack_reader_init_stdfile(mpack_reader_t* reader, FILE* stdfile, bool close_when_done);
+#endif
+
+/**
+ * @def mpack_reader_init_stack(reader)
+ * @hideinitializer
+ *
+ * Initializes an MPack reader using stack space as a buffer. A fill function
+ * should be added to the reader to fill the buffer.
+ *
+ * @see mpack_reader_set_fill
+ */
+
+/** @cond */
+#define mpack_reader_init_stack_line_ex(line, reader) \
+    char mpack_buf_##line[MPACK_STACK_SIZE]; \
+    mpack_reader_init((reader), mpack_buf_##line, sizeof(mpack_buf_##line), 0)
+
+#define mpack_reader_init_stack_line(line, reader) \
+    mpack_reader_init_stack_line_ex(line, reader)
+/** @endcond */
+
+#define mpack_reader_init_stack(reader) \
+    mpack_reader_init_stack_line(__LINE__, (reader))
+
+/**
+ * Cleans up the MPack reader, ensuring that all compound elements
+ * have been completely read. Returns the final error state of the
+ * reader.
+ *
+ * This will assert in tracking mode if the reader is not in an error
+ * state and has any incomplete reads. If you want to cancel reading
+ * in the middle of a document, you need to flag an error on the reader
+ * before destroying it (such as mpack_error_data).
+ *
+ * @see mpack_read_tag()
+ * @see mpack_reader_flag_error()
+ * @see mpack_error_data
+ */
+mpack_error_t mpack_reader_destroy(mpack_reader_t* reader);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Callbacks
+ * @{
+ */
+
+/**
+ * Sets the custom pointer to pass to the reader callbacks, such as fill
+ * or teardown.
+ *
+ * @param reader The MPack reader.
+ * @param context User data to pass to the reader callbacks.
+ *
+ * @see mpack_reader_context()
+ */
+MPACK_INLINE void mpack_reader_set_context(mpack_reader_t* reader, void* context) {
+    reader->context = context;
+}
+
+/**
+ * Returns the custom context for reader callbacks.
+ *
+ * @see mpack_reader_set_context
+ * @see mpack_reader_set_fill
+ * @see mpack_reader_set_skip
+ */
+MPACK_INLINE void* mpack_reader_context(mpack_reader_t* reader) {
+    return reader->context;
+}
+
+/**
+ * Sets the fill function to refill the data buffer when it runs out of data.
+ *
+ * If no fill function is used, truncated MessagePack data results in
+ * mpack_error_invalid (since the buffer is assumed to contain a
+ * complete MessagePack object.)
+ *
+ * If a fill function is used, truncated MessagePack data usually
+ * results in mpack_error_io (since the fill function fails to get
+ * the missing data.)
+ *
+ * This should normally be used with mpack_reader_set_context() to register
+ * a custom pointer to pass to the fill function.
+ *
+ * @param reader The MPack reader.
+ * @param fill The function to fetch additional data into the buffer.
+ */
+void mpack_reader_set_fill(mpack_reader_t* reader, mpack_reader_fill_t fill);
+
+/**
+ * Sets the skip function to discard bytes from the source stream.
+ *
+ * It's not necessary to implement this function. If the stream is not
+ * seekable, don't set a skip callback. The reader will fall back to
+ * using the fill function instead.
+ *
+ * This should normally be used with mpack_reader_set_context() to register
+ * a custom pointer to pass to the skip function.
+ *
+ * The skip function is ignored in size-optimized builds to reduce code
+ * size. Data will be skipped with the fill function when necessary.
+ *
+ * @param reader The MPack reader.
+ * @param skip The function to discard bytes from the source stream.
+ */
+void mpack_reader_set_skip(mpack_reader_t* reader, mpack_reader_skip_t skip);
+
+/**
+ * Sets the error function to call when an error is flagged on the reader.
+ *
+ * This should normally be used with mpack_reader_set_context() to register
+ * a custom pointer to pass to the error function.
+ *
+ * See the definition of mpack_reader_error_t for more information about
+ * what you can do from an error callback.
+ *
+ * @see mpack_reader_error_t
+ * @param reader The MPack reader.
+ * @param error_fn The function to call when an error is flagged on the reader.
+ */
+MPACK_INLINE void mpack_reader_set_error_handler(mpack_reader_t* reader, mpack_reader_error_t error_fn) {
+    reader->error_fn = error_fn;
+}
+
+/**
+ * Sets the teardown function to call when the reader is destroyed.
+ *
+ * This should normally be used with mpack_reader_set_context() to register
+ * a custom pointer to pass to the teardown function.
+ *
+ * @param reader The MPack reader.
+ * @param teardown The function to call when the reader is destroyed.
+ */
+MPACK_INLINE void mpack_reader_set_teardown(mpack_reader_t* reader, mpack_reader_teardown_t teardown) {
+    reader->teardown = teardown;
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @name Core Reader Functions
+ * @{
+ */
+
+/**
+ * Queries the error state of the MPack reader.
+ *
+ * If a reader is in an error state, you should discard all data since the
+ * last time the error flag was checked. The error flag cannot be cleared.
+ */
+MPACK_INLINE mpack_error_t mpack_reader_error(mpack_reader_t* reader) {
+    return reader->error;
+}
+
+/**
+ * Places the reader in the given error state, calling the error callback if one
+ * is set.
+ *
+ * This allows you to externally flag errors, for example if you are validating
+ * data as you read it.
+ *
+ * If the reader is already in an error state, this call is ignored and no
+ * error callback is called.
+ */
+void mpack_reader_flag_error(mpack_reader_t* reader, mpack_error_t error);
+
+/**
+ * Places the reader in the given error state if the given error is not mpack_ok,
+ * returning the resulting error state of the reader.
+ *
+ * This allows you to externally flag errors, for example if you are validating
+ * data as you read it.
+ *
+ * If the given error is mpack_ok or if the reader is already in an error state,
+ * this call is ignored and the actual error state of the reader is returned.
+ */
+MPACK_INLINE mpack_error_t mpack_reader_flag_if_error(mpack_reader_t* reader, mpack_error_t error) {
+    if (error != mpack_ok)
+        mpack_reader_flag_error(reader, error);
+    return mpack_reader_error(reader);
+}
+
+/**
+ * Returns bytes left in the reader's buffer.
+ *
+ * If you are done reading MessagePack data but there is other interesting data
+ * following it, the reader may have buffered too much data. The number of bytes
+ * remaining in the buffer and a pointer to the position of those bytes can be
+ * queried here.
+ *
+ * If you know the length of the MPack chunk beforehand, it's better to instead
+ * have your fill function limit the data it reads so that the reader does not
+ * have extra data. In this case you can simply check that this returns zero.
+ *
+ * Returns 0 if the reader is in an error state.
+ *
+ * @param reader The MPack reader from which to query remaining data.
+ * @param data [out] A pointer to the remaining data, or NULL.
+ * @return The number of bytes remaining in the buffer.
+ */
+size_t mpack_reader_remaining(mpack_reader_t* reader, const char** data);
+
+/**
+ * Reads a MessagePack object header (an MPack tag.)
+ *
+ * If an error occurs, the reader is placed in an error state and a
+ * nil tag is returned. If the reader is already in an error state,
+ * a nil tag is returned.
+ *
+ * If the type is compound (i.e. is a map, array, string, binary or
+ * extension type), additional reads are required to get the contained
+ * data, and the corresponding done function must be called when done.
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * @see mpack_read_bytes()
+ * @see mpack_done_array()
+ * @see mpack_done_map()
+ * @see mpack_done_str()
+ * @see mpack_done_bin()
+ * @see mpack_done_ext()
+ */
+mpack_tag_t mpack_read_tag(mpack_reader_t* reader);
+
+/**
+ * Parses the next MessagePack object header (an MPack tag) without
+ * advancing the reader.
+ *
+ * If an error occurs, the reader is placed in an error state and a
+ * nil tag is returned. If the reader is already in an error state,
+ * a nil tag is returned.
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * @see mpack_read_tag()
+ * @see mpack_discard()
+ */
+mpack_tag_t mpack_peek_tag(mpack_reader_t* reader);
+
+/**
+ * @}
+ */
+
+/**
+ * @name String and Data Functions
+ * @{
+ */
+
+/**
+ * Skips bytes from the underlying stream. This is used only to
+ * skip the contents of a string, binary blob or extension object.
+ */
+void mpack_skip_bytes(mpack_reader_t* reader, size_t count);
+
+/**
+ * Reads bytes from a string, binary blob or extension object, copying
+ * them into the given buffer.
+ *
+ * A str, bin or ext must have been opened by a call to mpack_read_tag()
+ * which yielded one of these types, or by a call to an expect function
+ * such as mpack_expect_str() or mpack_expect_bin().
+ *
+ * If an error occurs, the buffer contents are undefined.
+ *
+ * This can be called multiple times for a single str, bin or ext
+ * to read the data in chunks. The total data read must add up
+ * to the size of the object.
+ *
+ * @param reader The MPack reader
+ * @param p The buffer in which to copy the bytes
+ * @param count The number of bytes to read
+ */
+void mpack_read_bytes(mpack_reader_t* reader, char* p, size_t count);
+
+/**
+ * Reads bytes from a string, ensures that the string is valid UTF-8,
+ * and copies the bytes into the given buffer.
+ *
+ * A string must have been opened by a call to mpack_read_tag() which
+ * yielded a string, or by a call to an expect function such as
+ * mpack_expect_str().
+ *
+ * The given byte count must match the complete size of the string as
+ * returned by the tag or expect function. You must ensure that the
+ * buffer fits the data.
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed.
+ *
+ * If an error occurs, the buffer contents are undefined.
+ *
+ * Unlike mpack_read_bytes(), this cannot be used to read the data in
+ * chunks (since this might split a character's UTF-8 bytes, and the
+ * reader does not keep track of the UTF-8 decoding state between reads.)
+ *
+ * @throws mpack_error_type if the string contains invalid UTF-8.
+ */
+void mpack_read_utf8(mpack_reader_t* reader, char* p, size_t byte_count);
+
+/**
+ * Reads bytes from a string, ensures that the string contains no NUL
+ * bytes, copies the bytes into the given buffer and adds a null-terminator.
+ *
+ * A string must have been opened by a call to mpack_read_tag() which
+ * yielded a string, or by a call to an expect function such as
+ * mpack_expect_str().
+ *
+ * The given byte count must match the size of the string as returned
+ * by the tag or expect function. The string will only be copied if
+ * the buffer is large enough to store it.
+ *
+ * If an error occurs, the buffer will contain an empty string.
+ *
+ * @note If you know the object will be a string before reading it,
+ * it is highly recommended to use mpack_expect_cstr() instead.
+ * Alternatively you could use mpack_peek_tag() and call
+ * mpack_expect_cstr() if it's a string.
+ *
+ * @throws mpack_error_too_big if the string plus null-terminator is larger than the given buffer size
+ * @throws mpack_error_type if the string contains a null byte.
+ *
+ * @see mpack_peek_tag()
+ * @see mpack_expect_cstr()
+ * @see mpack_expect_utf8_cstr()
+ */
+void mpack_read_cstr(mpack_reader_t* reader, char* buf, size_t buffer_size, size_t byte_count);
+
+/**
+ * Reads bytes from a string, ensures that the string is valid UTF-8
+ * with no NUL bytes, copies the bytes into the given buffer and adds a
+ * null-terminator.
+ *
+ * A string must have been opened by a call to mpack_read_tag() which
+ * yielded a string, or by a call to an expect function such as
+ * mpack_expect_str().
+ *
+ * The given byte count must match the size of the string as returned
+ * by the tag or expect function. The string will only be copied if
+ * the buffer is large enough to store it.
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed, but without the NUL character, since
+ * it cannot be represented in a null-terminated string.
+ *
+ * If an error occurs, the buffer will contain an empty string.
+ *
+ * @note If you know the object will be a string before reading it,
+ * it is highly recommended to use mpack_expect_utf8_cstr() instead.
+ * Alternatively you could use mpack_peek_tag() and call
+ * mpack_expect_utf8_cstr() if it's a string.
+ *
+ * @throws mpack_error_too_big if the string plus null-terminator is larger than the given buffer size
+ * @throws mpack_error_type if the string contains invalid UTF-8 or a null byte.
+ *
+ * @see mpack_peek_tag()
+ * @see mpack_expect_utf8_cstr()
+ */
+void mpack_read_utf8_cstr(mpack_reader_t* reader, char* buf, size_t buffer_size, size_t byte_count);
+
+#ifdef MPACK_MALLOC
+/** @cond */
+// This can optionally add a null-terminator, but it does not check
+// whether the data contains null bytes. This must be done separately
+// in a cstring read function (possibly as part of a UTF-8 check.)
+char* mpack_read_bytes_alloc_impl(mpack_reader_t* reader, size_t count, bool null_terminated);
+/** @endcond */
+
+/**
+ * Reads bytes from a string, binary blob or extension object, allocating
+ * storage for them and returning the allocated pointer.
+ *
+ * The allocated string must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * Returns NULL if any error occurs, or if count is zero.
+ */
+MPACK_INLINE char* mpack_read_bytes_alloc(mpack_reader_t* reader, size_t count) {
+    return mpack_read_bytes_alloc_impl(reader, count, false);
+}
+#endif
+
+/**
+ * Reads bytes from a string, binary blob or extension object in-place in
+ * the buffer. This can be used to avoid copying the data.
+ *
+ * A str, bin or ext must have been opened by a call to mpack_read_tag()
+ * which yielded one of these types, or by a call to an expect function
+ * such as mpack_expect_str() or mpack_expect_bin().
+ *
+ * If the bytes are from a string, the string is not null-terminated! Use
+ * mpack_read_cstr() to copy the string into a buffer and add a null-terminator.
+ *
+ * The returned pointer is invalidated on the next read, or when the buffer
+ * is destroyed.
+ *
+ * The reader will move data around in the buffer if needed to ensure that
+ * the pointer can always be returned, so this should only be used if
+ * count is very small compared to the buffer size. If you need to check
+ * whether a small size is reasonable (for example you intend to handle small and
+ * large sizes differently), you can call mpack_should_read_bytes_inplace().
+ *
+ * This can be called multiple times for a single str, bin or ext
+ * to read the data in chunks. The total data read must add up
+ * to the size of the object.
+ *
+ * NULL is returned if the reader is in an error state.
+ *
+ * @throws mpack_error_too_big if the requested size is larger than the buffer size
+ *
+ * @see mpack_should_read_bytes_inplace()
+ */
+const char* mpack_read_bytes_inplace(mpack_reader_t* reader, size_t count);
+
+/**
+ * Reads bytes from a string in-place in the buffer and ensures they are
+ * valid UTF-8. This can be used to avoid copying the data.
+ *
+ * A string must have been opened by a call to mpack_read_tag() which
+ * yielded a string, or by a call to an expect function such as
+ * mpack_expect_str().
+ *
+ * The string is not null-terminated! Use mpack_read_utf8_cstr() to
+ * copy the string into a buffer and add a null-terminator.
+ *
+ * The returned pointer is invalidated on the next read, or when the buffer
+ * is destroyed.
+ *
+ * The reader will move data around in the buffer if needed to ensure that
+ * the pointer can always be returned, so this should only be used if
+ * count is very small compared to the buffer size. If you need to check
+ * whether a small size is reasonable (for example you intend to handle small and
+ * large sizes differently), you can call mpack_should_read_bytes_inplace().
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed.
+ *
+ * Unlike mpack_read_bytes_inplace(), this cannot be used to read the data in
+ * chunks (since this might split a character's UTF-8 bytes, and the
+ * reader does not keep track of the UTF-8 decoding state between reads.)
+ *
+ * NULL is returned if the reader is in an error state.
+ *
+ * @throws mpack_error_type if the string contains invalid UTF-8
+ * @throws mpack_error_too_big if the requested size is larger than the buffer size
+ *
+ * @see mpack_should_read_bytes_inplace()
+ */
+const char* mpack_read_utf8_inplace(mpack_reader_t* reader, size_t count);
+
+/**
+ * Returns true if it's a good idea to read the given number of bytes
+ * in-place.
+ *
+ * If the read will be larger than some small fraction of the buffer size,
+ * this will return false to avoid shuffling too much data back and forth
+ * in the buffer.
+ *
+ * Use this if you're expecting arbitrary size data, and you want to read
+ * in-place for the best performance when possible but will fall back to
+ * a normal read if the data is too large.
+ *
+ * @see mpack_read_bytes_inplace()
+ */
+MPACK_INLINE bool mpack_should_read_bytes_inplace(mpack_reader_t* reader, size_t count) {
+    return (reader->size == 0 || count <= reader->size / MPACK_READER_SMALL_FRACTION_DENOMINATOR);
+}
+
+#if MPACK_EXTENSIONS
+/**
+ * Reads a timestamp contained in an ext object of the given size, closing the
+ * ext type.
+ *
+ * An ext object of exttype @ref MPACK_EXTTYPE_TIMESTAMP must have been opened
+ * by a call to e.g. mpack_read_tag() or mpack_expect_ext().
+ *
+ * You must NOT call mpack_done_ext() after calling this. A timestamp ext
+ * object can only contain a single timestamp value, so this calls
+ * mpack_done_ext() automatically.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @throws mpack_error_invalid if the size is not one of the supported
+ * timestamp sizes, or if the nanoseconds are out of range.
+ */
+mpack_timestamp_t mpack_read_timestamp(mpack_reader_t* reader, size_t size);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Core Reader Functions
+ * @{
+ */
+
+#if MPACK_READ_TRACKING
+/**
+ * Finishes reading the given type.
+ *
+ * This will track reads to ensure that the correct number of elements
+ * or bytes are read.
+ */
+void mpack_done_type(mpack_reader_t* reader, mpack_type_t type);
+#else
+MPACK_INLINE void mpack_done_type(mpack_reader_t* reader, mpack_type_t type) {
+    MPACK_UNUSED(reader);
+    MPACK_UNUSED(type);
+}
+#endif
+
+/**
+ * Finishes reading an array.
+ *
+ * This will track reads to ensure that the correct number of elements are read.
+ */
+MPACK_INLINE void mpack_done_array(mpack_reader_t* reader) {
+    mpack_done_type(reader, mpack_type_array);
+}
+
+/**
+ * @fn mpack_done_map(mpack_reader_t* reader)
+ *
+ * Finishes reading a map.
+ *
+ * This will track reads to ensure that the correct number of elements are read.
+ */
+MPACK_INLINE void mpack_done_map(mpack_reader_t* reader) {
+    mpack_done_type(reader, mpack_type_map);
+}
+
+/**
+ * @fn mpack_done_str(mpack_reader_t* reader)
+ *
+ * Finishes reading a string.
+ *
+ * This will track reads to ensure that the correct number of bytes are read.
+ */
+MPACK_INLINE void mpack_done_str(mpack_reader_t* reader) {
+    mpack_done_type(reader, mpack_type_str);
+}
+
+/**
+ * @fn mpack_done_bin(mpack_reader_t* reader)
+ *
+ * Finishes reading a binary data blob.
+ *
+ * This will track reads to ensure that the correct number of bytes are read.
+ */
+MPACK_INLINE void mpack_done_bin(mpack_reader_t* reader) {
+    mpack_done_type(reader, mpack_type_bin);
+}
+
+#if MPACK_EXTENSIONS
+/**
+ * @fn mpack_done_ext(mpack_reader_t* reader)
+ *
+ * Finishes reading an extended type binary data blob.
+ *
+ * This will track reads to ensure that the correct number of bytes are read.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+MPACK_INLINE void mpack_done_ext(mpack_reader_t* reader) {
+    mpack_done_type(reader, mpack_type_ext);
+}
+#endif
+
+/**
+ * Reads and discards the next object. This will read and discard all
+ * contained data as well if it is a compound type.
+ */
+void mpack_discard(mpack_reader_t* reader);
+
+/**
+ * @}
+ */
+
+/** @cond */
+
+#if MPACK_DEBUG && MPACK_STDIO
+/**
+ * @name Debugging Functions
+ * @{
+ */
+/*
+ * Converts a blob of MessagePack to a pseudo-JSON string for debugging
+ * purposes, placing the result in the given buffer with a null-terminator.
+ *
+ * If the buffer does not have enough space, the result will be truncated (but
+ * it is guaranteed to be null-terminated.)
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+void mpack_print_data_to_buffer(const char* data, size_t data_size, char* buffer, size_t buffer_size);
+
+/*
+ * Converts a node to pseudo-JSON for debugging purposes, calling the given
+ * callback as many times as is necessary to output the character data.
+ *
+ * No null-terminator or trailing newline will be written.
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+void mpack_print_data_to_callback(const char* data, size_t size, mpack_print_callback_t callback, void* context);
+
+/*
+ * Converts a blob of MessagePack to pseudo-JSON for debugging purposes
+ * and pretty-prints it to the given file.
+ */
+void mpack_print_data_to_file(const char* data, size_t len, FILE* file);
+
+/*
+ * Converts a blob of MessagePack to pseudo-JSON for debugging purposes
+ * and pretty-prints it to stdout.
+ */
+MPACK_INLINE void mpack_print_data_to_stdout(const char* data, size_t len) {
+    mpack_print_data_to_file(data, len, stdout);
+}
+
+/*
+ * Converts the MessagePack contained in the given `FILE*` to pseudo-JSON for
+ * debugging purposes, calling the given callback as many times as is necessary
+ * to output the character data.
+ */
+void mpack_print_stdfile_to_callback(FILE* file, mpack_print_callback_t callback, void* context);
+
+/*
+ * Deprecated.
+ *
+ * \deprecated Renamed to mpack_print_data_to_stdout().
+ */
+MPACK_INLINE void mpack_print(const char* data, size_t len) {
+    mpack_print_data_to_stdout(data, len);
+}
+
+/**
+ * @}
+ */
+#endif
+
+/** @endcond */
+
+/**
+ * @}
+ */
+
+
+
+#if MPACK_INTERNAL
+
+bool mpack_reader_ensure_straddle(mpack_reader_t* reader, size_t count);
+
+/*
+ * Ensures there are at least @c count bytes left in the
+ * data, raising an error and returning false if more
+ * data cannot be made available.
+ */
+MPACK_INLINE bool mpack_reader_ensure(mpack_reader_t* reader, size_t count) {
+    mpack_assert(count != 0, "cannot ensure zero bytes!");
+    mpack_assert(reader->error == mpack_ok, "reader cannot be in an error state!");
+
+    if (count <= (size_t)(reader->end - reader->data))
+        return true;
+    return mpack_reader_ensure_straddle(reader, count);
+}
+
+void mpack_read_native_straddle(mpack_reader_t* reader, char* p, size_t count);
+
+// Reads count bytes into p, deferring to mpack_read_native_straddle() if more
+// bytes are needed than are available in the buffer.
+MPACK_INLINE void mpack_read_native(mpack_reader_t* reader, char* p, size_t count) {
+    mpack_assert(count == 0 || p != NULL, "data pointer for %i bytes is NULL", (int)count);
+
+    if (count > (size_t)(reader->end - reader->data)) {
+        mpack_read_native_straddle(reader, p, count);
+    } else {
+        mpack_memcpy(p, reader->data, count);
+        reader->data += count;
+    }
+}
+
+#if MPACK_READ_TRACKING
+#define MPACK_READER_TRACK(reader, error_expr) \
+    (((reader)->error == mpack_ok) ? mpack_reader_flag_if_error((reader), (error_expr)) : (reader)->error)
+#else
+#define MPACK_READER_TRACK(reader, error_expr) (MPACK_UNUSED(reader), mpack_ok)
+#endif
+
+MPACK_INLINE mpack_error_t mpack_reader_track_element(mpack_reader_t* reader) {
+    return MPACK_READER_TRACK(reader, mpack_track_element(&reader->track, true));
+}
+
+MPACK_INLINE mpack_error_t mpack_reader_track_peek_element(mpack_reader_t* reader) {
+    return MPACK_READER_TRACK(reader, mpack_track_peek_element(&reader->track, true));
+}
+
+MPACK_INLINE mpack_error_t mpack_reader_track_bytes(mpack_reader_t* reader, size_t count) {
+    MPACK_UNUSED(count);
+    return MPACK_READER_TRACK(reader, mpack_track_bytes(&reader->track, true, count));
+}
+
+MPACK_INLINE mpack_error_t mpack_reader_track_str_bytes_all(mpack_reader_t* reader, size_t count) {
+    MPACK_UNUSED(count);
+    return MPACK_READER_TRACK(reader, mpack_track_str_bytes_all(&reader->track, true, count));
+}
+
+#endif
+
+
+
+#endif
+
+MPACK_EXTERN_C_END
+MPACK_SILENCE_WARNINGS_END
+
+#endif
+
+
+/* mpack/mpack-expect.h.h */
+
+/**
+ * @file
+ *
+ * Declares the MPack static Expect API.
+ */
+
+#ifndef MPACK_EXPECT_H
+#define MPACK_EXPECT_H 1
+
+/* #include "mpack-reader.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+MPACK_EXTERN_C_BEGIN
+
+#if MPACK_EXPECT
+
+#if !MPACK_READER
+#error "MPACK_EXPECT requires MPACK_READER."
+#endif
+
+/**
+ * @defgroup expect Expect API
+ *
+ * The MPack Expect API allows you to easily read MessagePack data when you
+ * expect it to follow a predefined schema.
+ *
+ * @note If you are not writing code for an embedded device (or otherwise do
+ * not need maximum performance with minimal memory usage), you should not use
+ * this. You probably want to use the @link node Node API@endlink instead.
+ *
+ * See @ref docs/expect.md for examples.
+ *
+ * The main purpose of the Expect API is convenience, so the API is lax. It
+ * automatically converts between similar types where there is no loss of
+ * precision.
+ *
+ * When using any of the expect functions, if the type or value of what was
+ * read does not match what is expected, @ref mpack_error_type is raised.
+ *
+ * @{
+ */
+
+/**
+ * @name Basic Number Functions
+ * @{
+ */
+
+/**
+ * Reads an 8-bit unsigned integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an 8-bit unsigned int.
+ *
+ * Returns zero if an error occurs.
+ */
+uint8_t mpack_expect_u8(mpack_reader_t* reader);
+
+/**
+ * Reads a 16-bit unsigned integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 16-bit unsigned int.
+ *
+ * Returns zero if an error occurs.
+ */
+uint16_t mpack_expect_u16(mpack_reader_t* reader);
+
+/**
+ * Reads a 32-bit unsigned integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 32-bit unsigned int.
+ *
+ * Returns zero if an error occurs.
+ */
+uint32_t mpack_expect_u32(mpack_reader_t* reader);
+
+/**
+ * Reads a 64-bit unsigned integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 64-bit unsigned int.
+ *
+ * Returns zero if an error occurs.
+ */
+uint64_t mpack_expect_u64(mpack_reader_t* reader);
+
+/**
+ * Reads an 8-bit signed integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an 8-bit signed int.
+ *
+ * Returns zero if an error occurs.
+ */
+int8_t mpack_expect_i8(mpack_reader_t* reader);
+
+/**
+ * Reads a 16-bit signed integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 16-bit signed int.
+ *
+ * Returns zero if an error occurs.
+ */
+int16_t mpack_expect_i16(mpack_reader_t* reader);
+
+/**
+ * Reads a 32-bit signed integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 32-bit signed int.
+ *
+ * Returns zero if an error occurs.
+ */
+int32_t mpack_expect_i32(mpack_reader_t* reader);
+
+/**
+ * Reads a 64-bit signed integer.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 64-bit signed int.
+ *
+ * Returns zero if an error occurs.
+ */
+int64_t mpack_expect_i64(mpack_reader_t* reader);
+
+#if MPACK_FLOAT
+/**
+ * Reads a number, returning the value as a float. The underlying value can be an
+ * integer, float or double; the value is converted to a float.
+ *
+ * @note Reading a double or a large integer with this function can incur a
+ * loss of precision.
+ *
+ * @throws mpack_error_type if the underlying value is not a float, double or integer.
+ */
+float mpack_expect_float(mpack_reader_t* reader);
+#endif
+
+#if MPACK_DOUBLE
+/**
+ * Reads a number, returning the value as a double. The underlying value can be an
+ * integer, float or double; the value is converted to a double.
+ *
+ * @note Reading a very large integer with this function can incur a
+ * loss of precision.
+ *
+ * @throws mpack_error_type if the underlying value is not a float, double or integer.
+ */
+double mpack_expect_double(mpack_reader_t* reader);
+#endif
+
+#if MPACK_FLOAT
+/**
+ * Reads a float. The underlying value must be a float, not a double or an integer.
+ * This ensures no loss of precision can occur.
+ *
+ * @throws mpack_error_type if the underlying value is not a float.
+ */
+float mpack_expect_float_strict(mpack_reader_t* reader);
+#endif
+
+#if MPACK_DOUBLE
+/**
+ * Reads a double. The underlying value must be a float or double, not an integer.
+ * This ensures no loss of precision can occur.
+ *
+ * @throws mpack_error_type if the underlying value is not a float or double.
+ */
+double mpack_expect_double_strict(mpack_reader_t* reader);
+#endif
+
+#if !MPACK_FLOAT
+/**
+ * Reads a float as a raw uint32_t. The underlying value must be a float, not a
+ * double or an integer.
+ *
+ * @throws mpack_error_type if the underlying value is not a float.
+ */
+uint32_t mpack_expect_raw_float(mpack_reader_t* reader);
+#endif
+
+#if !MPACK_DOUBLE
+/**
+ * Reads a double as a raw uint64_t. The underlying value must be a double, not a
+ * float or an integer.
+ *
+ * @throws mpack_error_type if the underlying value is not a double.
+ */
+uint64_t mpack_expect_raw_double(mpack_reader_t* reader);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Ranged Number Functions
+ * @{
+ */
+
+/**
+ * Reads an 8-bit unsigned integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an 8-bit unsigned int.
+ *
+ * Returns min_value if an error occurs.
+ */
+uint8_t mpack_expect_u8_range(mpack_reader_t* reader, uint8_t min_value, uint8_t max_value);
+
+/**
+ * Reads a 16-bit unsigned integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 16-bit unsigned int.
+ *
+ * Returns min_value if an error occurs.
+ */
+uint16_t mpack_expect_u16_range(mpack_reader_t* reader, uint16_t min_value, uint16_t max_value);
+
+/**
+ * Reads a 32-bit unsigned integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 32-bit unsigned int.
+ *
+ * Returns min_value if an error occurs.
+ */
+uint32_t mpack_expect_u32_range(mpack_reader_t* reader, uint32_t min_value, uint32_t max_value);
+
+/**
+ * Reads a 64-bit unsigned integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 64-bit unsigned int.
+ *
+ * Returns min_value if an error occurs.
+ */
+uint64_t mpack_expect_u64_range(mpack_reader_t* reader, uint64_t min_value, uint64_t max_value);
+
+/**
+ * Reads an unsigned integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an unsigned int.
+ *
+ * Returns min_value if an error occurs.
+ */
+MPACK_INLINE unsigned int mpack_expect_uint_range(mpack_reader_t* reader, unsigned int min_value, unsigned int max_value) {
+    // This should be true at compile-time, so this just wraps the 32-bit
+    // function. We fallback to 64-bit if for some reason sizeof(int) isn't 4.
+    if (sizeof(unsigned int) == 4)
+        return (unsigned int)mpack_expect_u32_range(reader, (uint32_t)min_value, (uint32_t)max_value);
+    return (unsigned int)mpack_expect_u64_range(reader, min_value, max_value);
+}
+
+/**
+ * Reads an 8-bit unsigned integer, ensuring that it is at most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an 8-bit unsigned int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE uint8_t mpack_expect_u8_max(mpack_reader_t* reader, uint8_t max_value) {
+    return mpack_expect_u8_range(reader, 0, max_value);
+}
+
+/**
+ * Reads a 16-bit unsigned integer, ensuring that it is at most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 16-bit unsigned int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE uint16_t mpack_expect_u16_max(mpack_reader_t* reader, uint16_t max_value) {
+    return mpack_expect_u16_range(reader, 0, max_value);
+}
+
+/**
+ * Reads a 32-bit unsigned integer, ensuring that it is at most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 32-bit unsigned int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE uint32_t mpack_expect_u32_max(mpack_reader_t* reader, uint32_t max_value) {
+    return mpack_expect_u32_range(reader, 0, max_value);
+}
+
+/**
+ * Reads a 64-bit unsigned integer, ensuring that it is at most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 64-bit unsigned int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE uint64_t mpack_expect_u64_max(mpack_reader_t* reader, uint64_t max_value) {
+    return mpack_expect_u64_range(reader, 0, max_value);
+}
+
+/**
+ * Reads an unsigned integer, ensuring that it is at most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an unsigned int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE unsigned int mpack_expect_uint_max(mpack_reader_t* reader, unsigned int max_value) {
+    return mpack_expect_uint_range(reader, 0, max_value);
+}
+
+/**
+ * Reads an 8-bit signed integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an 8-bit signed int.
+ *
+ * Returns min_value if an error occurs.
+ */
+int8_t mpack_expect_i8_range(mpack_reader_t* reader, int8_t min_value, int8_t max_value);
+
+/**
+ * Reads a 16-bit signed integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 16-bit signed int.
+ *
+ * Returns min_value if an error occurs.
+ */
+int16_t mpack_expect_i16_range(mpack_reader_t* reader, int16_t min_value, int16_t max_value);
+
+/**
+ * Reads a 32-bit signed integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 32-bit signed int.
+ *
+ * Returns min_value if an error occurs.
+ */
+int32_t mpack_expect_i32_range(mpack_reader_t* reader, int32_t min_value, int32_t max_value);
+
+/**
+ * Reads a 64-bit signed integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 64-bit signed int.
+ *
+ * Returns min_value if an error occurs.
+ */
+int64_t mpack_expect_i64_range(mpack_reader_t* reader, int64_t min_value, int64_t max_value);
+
+/**
+ * Reads a signed integer, ensuring that it falls within the given range.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a signed int.
+ *
+ * Returns min_value if an error occurs.
+ */
+MPACK_INLINE int mpack_expect_int_range(mpack_reader_t* reader, int min_value, int max_value) {
+    // This should be true at compile-time, so this just wraps the 32-bit
+    // function. We fallback to 64-bit if for some reason sizeof(int) isn't 4.
+    if (sizeof(int) == 4)
+        return (int)mpack_expect_i32_range(reader, (int32_t)min_value, (int32_t)max_value);
+    return (int)mpack_expect_i64_range(reader, min_value, max_value);
+}
+
+/**
+ * Reads an 8-bit signed integer, ensuring that it is at least zero and at
+ * most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an 8-bit signed int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE int8_t mpack_expect_i8_max(mpack_reader_t* reader, int8_t max_value) {
+    return mpack_expect_i8_range(reader, 0, max_value);
+}
+
+/**
+ * Reads a 16-bit signed integer, ensuring that it is at least zero and at
+ * most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 16-bit signed int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE int16_t mpack_expect_i16_max(mpack_reader_t* reader, int16_t max_value) {
+    return mpack_expect_i16_range(reader, 0, max_value);
+}
+
+/**
+ * Reads a 32-bit signed integer, ensuring that it is at least zero and at
+ * most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 32-bit signed int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE int32_t mpack_expect_i32_max(mpack_reader_t* reader, int32_t max_value) {
+    return mpack_expect_i32_range(reader, 0, max_value);
+}
+
+/**
+ * Reads a 64-bit signed integer, ensuring that it is at least zero and at
+ * most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a 64-bit signed int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE int64_t mpack_expect_i64_max(mpack_reader_t* reader, int64_t max_value) {
+    return mpack_expect_i64_range(reader, 0, max_value);
+}
+
+/**
+ * Reads an int, ensuring that it is at least zero and at most @a max_value.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a signed int.
+ *
+ * Returns 0 if an error occurs.
+ */
+MPACK_INLINE int mpack_expect_int_max(mpack_reader_t* reader, int max_value) {
+    return mpack_expect_int_range(reader, 0, max_value);
+}
+
+#if MPACK_FLOAT
+/**
+ * Reads a number, ensuring that it falls within the given range and returning
+ * the value as a float. The underlying value can be an integer, float or
+ * double; the value is converted to a float.
+ *
+ * @note Reading a double or a large integer with this function can incur a
+ * loss of precision.
+ *
+ * @throws mpack_error_type if the underlying value is not a float, double or integer.
+ */
+float mpack_expect_float_range(mpack_reader_t* reader, float min_value, float max_value);
+#endif
+
+#if MPACK_DOUBLE
+/**
+ * Reads a number, ensuring that it falls within the given range and returning
+ * the value as a double. The underlying value can be an integer, float or
+ * double; the value is converted to a double.
+ *
+ * @note Reading a very large integer with this function can incur a
+ * loss of precision.
+ *
+ * @throws mpack_error_type if the underlying value is not a float, double or integer.
+ */
+double mpack_expect_double_range(mpack_reader_t* reader, double min_value, double max_value);
+#endif
+
+/**
+ * @}
+ */
+
+
+
+// These are additional Basic Number functions that wrap inline range functions.
+
+/**
+ * @name Basic Number Functions
+ * @{
+ */
+
+/**
+ * Reads an unsigned int.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in an unsigned int.
+ *
+ * Returns zero if an error occurs.
+ */
+MPACK_INLINE unsigned int mpack_expect_uint(mpack_reader_t* reader) {
+
+    // This should be true at compile-time, so this just wraps the 32-bit function.
+    if (sizeof(unsigned int) == 4)
+        return (unsigned int)mpack_expect_u32(reader);
+
+    // Otherwise we wrap the max function to ensure it fits.
+    return (unsigned int)mpack_expect_u64_max(reader, MPACK_UINT_MAX);
+
+}
+
+/**
+ * Reads a signed int.
+ *
+ * The underlying type may be an integer type of any size and signedness,
+ * as long as the value can be represented in a signed int.
+ *
+ * Returns zero if an error occurs.
+ */
+MPACK_INLINE int mpack_expect_int(mpack_reader_t* reader) {
+
+    // This should be true at compile-time, so this just wraps the 32-bit function.
+    if (sizeof(int) == 4)
+        return (int)mpack_expect_i32(reader);
+
+    // Otherwise we wrap the range function to ensure it fits.
+    return (int)mpack_expect_i64_range(reader, MPACK_INT_MIN, MPACK_INT_MAX);
+
+}
+
+/**
+ * @}
+ */
+
+
+
+/**
+ * @name Matching Number Functions
+ * @{
+ */
+
+/**
+ * Reads an unsigned integer, ensuring that it exactly matches the given value.
+ *
+ * mpack_error_type is raised if the value is not representable as an unsigned
+ * integer or if it does not exactly match the given value.
+ */
+void mpack_expect_uint_match(mpack_reader_t* reader, uint64_t value);
+
+/**
+ * Reads a signed integer, ensuring that it exactly matches the given value.
+ *
+ * mpack_error_type is raised if the value is not representable as a signed
+ * integer or if it does not exactly match the given value.
+ */
+void mpack_expect_int_match(mpack_reader_t* reader, int64_t value);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Other Basic Types
+ * @{
+ */
+
+/**
+ * Reads a nil, raising @ref mpack_error_type if the value is not nil.
+ */
+void mpack_expect_nil(mpack_reader_t* reader);
+
+/**
+ * Reads a boolean.
+ *
+ * @note Integers will raise mpack_error_type; the value must be strictly a boolean.
+ */
+bool mpack_expect_bool(mpack_reader_t* reader);
+
+/**
+ * Reads a boolean, raising @ref mpack_error_type if its value is not @c true.
+ */
+void mpack_expect_true(mpack_reader_t* reader);
+
+/**
+ * Reads a boolean, raising @ref mpack_error_type if its value is not @c false.
+ */
+void mpack_expect_false(mpack_reader_t* reader);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Extension Functions
+ * @{
+ */
+
+#if MPACK_EXTENSIONS
+/**
+ * Reads a timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+mpack_timestamp_t mpack_expect_timestamp(mpack_reader_t* reader);
+
+/**
+ * Reads a timestamp in seconds, truncating the nanoseconds (if any).
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+int64_t mpack_expect_timestamp_truncate(mpack_reader_t* reader);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Compound Types
+ * @{
+ */
+
+/**
+ * Reads the start of a map, returning its element count.
+ *
+ * A number of values follow equal to twice the element count of the map,
+ * alternating between keys and values. @ref mpack_done_map() must be called
+ * once all elements have been read.
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * @warning This call is dangerous! It does not have a size limit, and it
+ * does not have any way of checking whether there is enough data in the
+ * message (since the data could be coming from a stream.) When looping
+ * through the map's contents, you must check for errors on each iteration
+ * of the loop. Otherwise an attacker could craft a message declaring a map
+ * of a billion elements which would throw your parsing code into an
+ * infinite loop! You should strongly consider using mpack_expect_map_max()
+ * with a safe maximum size instead.
+ *
+ * @throws mpack_error_type if the value is not a map.
+ */
+uint32_t mpack_expect_map(mpack_reader_t* reader);
+
+/**
+ * Reads the start of a map with a number of elements in the given range, returning
+ * its element count.
+ *
+ * A number of values follow equal to twice the element count of the map,
+ * alternating between keys and values. @ref mpack_done_map() must be called
+ * once all elements have been read.
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * min_count is returned if an error occurs.
+ *
+ * @throws mpack_error_type if the value is not a map or if its size does
+ * not fall within the given range.
+ */
+uint32_t mpack_expect_map_range(mpack_reader_t* reader, uint32_t min_count, uint32_t max_count);
+
+/**
+ * Reads the start of a map with a number of elements at most @a max_count,
+ * returning its element count.
+ *
+ * A number of values follow equal to twice the element count of the map,
+ * alternating between keys and values. @ref mpack_done_map() must be called
+ * once all elements have been read.
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * Zero is returned if an error occurs.
+ *
+ * @throws mpack_error_type if the value is not a map or if its size is
+ * greater than max_count.
+ */
+MPACK_INLINE uint32_t mpack_expect_map_max(mpack_reader_t* reader, uint32_t max_count) {
+    return mpack_expect_map_range(reader, 0, max_count);
+}
+
+/**
+ * Reads the start of a map of the exact size given.
+ *
+ * A number of values follow equal to twice the element count of the map,
+ * alternating between keys and values. @ref mpack_done_map() must be called
+ * once all elements have been read.
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * @throws mpack_error_type if the value is not a map or if its size
+ * does not match the given count.
+ */
+void mpack_expect_map_match(mpack_reader_t* reader, uint32_t count);
+
+/**
+ * Reads a nil node or the start of a map, returning whether a map was
+ * read and placing its number of key/value pairs in count.
+ *
+ * If a map was read, a number of values follow equal to twice the element count
+ * of the map, alternating between keys and values. @ref mpack_done_map() should
+ * also be called once all elements have been read (only if a map was read.)
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON.
+ *
+ * @warning This call is dangerous! It does not have a size limit, and it
+ * does not have any way of checking whether there is enough data in the
+ * message (since the data could be coming from a stream.) When looping
+ * through the map's contents, you must check for errors on each iteration
+ * of the loop. Otherwise an attacker could craft a message declaring a map
+ * of a billion elements which would throw your parsing code into an
+ * infinite loop! You should strongly consider using mpack_expect_map_max_or_nil()
+ * with a safe maximum size instead.
+ *
+ * @returns @c true if a map was read successfully; @c false if nil was read
+ *     or an error occurred.
+ * @throws mpack_error_type if the value is not a nil or map.
+ */
+bool mpack_expect_map_or_nil(mpack_reader_t* reader, uint32_t* count);
+
+/**
+ * Reads a nil node or the start of a map with a number of elements at most
+ * max_count, returning whether a map was read and placing its number of
+ * key/value pairs in count.
+ *
+ * If a map was read, a number of values follow equal to twice the element count
+ * of the map, alternating between keys and values. @ref mpack_done_map() should
+ * anlso be called once all elements have been read (only if a map was read.)
+ *
+ * @note Maps in JSON are unordered, so it is recommended not to expect
+ * a specific ordering for your map values in case your data is converted
+ * to/from JSON. Consider using mpack_expect_key_cstr() or mpack_expect_key_uint()
+ * to switch on the key; see @ref docs/expect.md for examples.
+ *
+ * @returns @c true if a map was read successfully; @c false if nil was read
+ *     or an error occurred.
+ * @throws mpack_error_type if the value is not a nil or map.
+ */
+bool mpack_expect_map_max_or_nil(mpack_reader_t* reader, uint32_t max_count, uint32_t* count);
+
+/**
+ * Reads the start of an array, returning its element count.
+ *
+ * A number of values follow equal to the element count of the array.
+ * @ref mpack_done_array() must be called once all elements have been read.
+ *
+ * @warning This call is dangerous! It does not have a size limit, and it
+ * does not have any way of checking whether there is enough data in the
+ * message (since the data could be coming from a stream.) When looping
+ * through the array's contents, you must check for errors on each iteration
+ * of the loop. Otherwise an attacker could craft a message declaring an array
+ * of a billion elements which would throw your parsing code into an
+ * infinite loop! You should strongly consider using mpack_expect_array_max()
+ * with a safe maximum size instead.
+ */
+uint32_t mpack_expect_array(mpack_reader_t* reader);
+
+/**
+ * Reads the start of an array with a number of elements in the given range,
+ * returning its element count.
+ *
+ * A number of values follow equal to the element count of the array.
+ * @ref mpack_done_array() must be called once all elements have been read.
+ *
+ * min_count is returned if an error occurs.
+ *
+ * @throws mpack_error_type if the value is not an array or if its size does
+ * not fall within the given range.
+ */
+uint32_t mpack_expect_array_range(mpack_reader_t* reader, uint32_t min_count, uint32_t max_count);
+
+/**
+ * Reads the start of an array with a number of elements at most @a max_count,
+ * returning its element count.
+ *
+ * A number of values follow equal to the element count of the array.
+ * @ref mpack_done_array() must be called once all elements have been read.
+ *
+ * Zero is returned if an error occurs.
+ *
+ * @throws mpack_error_type if the value is not an array or if its size is
+ * greater than max_count.
+ */
+MPACK_INLINE uint32_t mpack_expect_array_max(mpack_reader_t* reader, uint32_t max_count) {
+    return mpack_expect_array_range(reader, 0, max_count);
+}
+
+/**
+ * Reads the start of an array of the exact size given.
+ *
+ * A number of values follow equal to the element count of the array.
+ * @ref mpack_done_array() must be called once all elements have been read.
+ *
+ * @throws mpack_error_type if the value is not an array or if its size does
+ * not match the given count.
+ */
+void mpack_expect_array_match(mpack_reader_t* reader, uint32_t count);
+
+/**
+ * Reads a nil node or the start of an array, returning whether an array was
+ * read and placing its number of elements in count.
+ *
+ * If an array was read, a number of values follow equal to the element count
+ * of the array. @ref mpack_done_array() should also be called once all elements
+ * have been read (only if an array was read.)
+ *
+ * @warning This call is dangerous! It does not have a size limit, and it
+ * does not have any way of checking whether there is enough data in the
+ * message (since the data could be coming from a stream.) When looping
+ * through the array's contents, you must check for errors on each iteration
+ * of the loop. Otherwise an attacker could craft a message declaring an array
+ * of a billion elements which would throw your parsing code into an
+ * infinite loop! You should strongly consider using mpack_expect_array_max_or_nil()
+ * with a safe maximum size instead.
+ *
+ * @returns @c true if an array was read successfully; @c false if nil was read
+ *     or an error occurred.
+ * @throws mpack_error_type if the value is not a nil or array.
+ */
+bool mpack_expect_array_or_nil(mpack_reader_t* reader, uint32_t* count);
+
+/**
+ * Reads a nil node or the start of an array with a number of elements at most
+ * max_count, returning whether an array was read and placing its number of
+ * key/value pairs in count.
+ *
+ * If an array was read, a number of values follow equal to the element count
+ * of the array. @ref mpack_done_array() should also be called once all elements
+ * have been read (only if an array was read.)
+ *
+ * @returns @c true if an array was read successfully; @c false if nil was read
+ *     or an error occurred.
+ * @throws mpack_error_type if the value is not a nil or array.
+ */
+bool mpack_expect_array_max_or_nil(mpack_reader_t* reader, uint32_t max_count, uint32_t* count);
+
+#ifdef MPACK_MALLOC
+/**
+ * @hideinitializer
+ *
+ * Reads the start of an array and allocates storage for it, placing its
+ * size in out_count. A number of objects follow equal to the element count
+ * of the array. You must call @ref mpack_done_array() when done (even
+ * if the element count is zero.)
+ *
+ * If an error occurs, NULL is returned and the reader is placed in an
+ * error state.
+ *
+ * If the count is zero, NULL is returned. This does not indicate error.
+ * You should not check the return value for NULL to check for errors; only
+ * check the reader's error state.
+ *
+ * The allocated array must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @throws mpack_error_type if the value is not an array or if its size is
+ * greater than max_count.
+ */
+#define mpack_expect_array_alloc(reader, Type, max_count, out_count) \
+    ((Type*)mpack_expect_array_alloc_impl(reader, sizeof(Type), max_count, out_count, false))
+
+/**
+ * @hideinitializer
+ *
+ * Reads a nil node or the start of an array and allocates storage for it,
+ * placing its size in out_count. A number of objects follow equal to the element
+ * count of the array if a non-empty array was read.
+ *
+ * If an error occurs, NULL is returned and the reader is placed in an
+ * error state.
+ *
+ * If a nil node was read, NULL is returned. If an empty array was read,
+ * mpack_done_array() is called automatically and NULL is returned. These
+ * do not indicate error. You should not check the return value for NULL
+ * to check for errors; only check the reader's error state.
+ *
+ * The allocated array must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @warning You must call @ref mpack_done_array() if and only if a non-zero
+ * element count is read. This function does not differentiate between nil
+ * and an empty array.
+ *
+ * @throws mpack_error_type if the value is not an array or if its size is
+ * greater than max_count.
+ */
+#define mpack_expect_array_or_nil_alloc(reader, Type, max_count, out_count) \
+    ((Type*)mpack_expect_array_alloc_impl(reader, sizeof(Type), max_count, out_count, true))
+#endif
+
+/**
+ * @}
+ */
+
+/** @cond */
+#ifdef MPACK_MALLOC
+void* mpack_expect_array_alloc_impl(mpack_reader_t* reader,
+        size_t element_size, uint32_t max_count, uint32_t* out_count, bool allow_nil);
+#endif
+/** @endcond */
+
+
+/**
+ * @name String Functions
+ * @{
+ */
+
+/**
+ * Reads the start of a string, returning its size in bytes.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). mpack_done_str() must be called
+ * once all bytes have been read.
+ *
+ * NUL bytes are allowed in the string, and no encoding checks are done.
+ *
+ * mpack_error_type is raised if the value is not a string.
+ */
+uint32_t mpack_expect_str(mpack_reader_t* reader);
+
+/**
+ * Reads a string of at most the given size, writing it into the
+ * given buffer and returning its size in bytes.
+ *
+ * This does not add a null-terminator! Use mpack_expect_cstr() to
+ * add a null-terminator.
+ *
+ * NUL bytes are allowed in the string, and no encoding checks are done.
+ */
+size_t mpack_expect_str_buf(mpack_reader_t* reader, char* buf, size_t bufsize);
+
+/**
+ * Reads a string into the given buffer, ensuring it is a valid UTF-8 string
+ * and returning its size in bytes.
+ *
+ * This does not add a null-terminator! Use mpack_expect_utf8_cstr() to
+ * add a null-terminator.
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed.
+ *
+ * NUL bytes are allowed in the string (as they are in UTF-8.)
+ *
+ * Raises mpack_error_too_big if there is not enough room for the string.
+ * Raises mpack_error_type if the value is not a string or is not a valid UTF-8 string.
+ */
+size_t mpack_expect_utf8(mpack_reader_t* reader, char* buf, size_t bufsize);
+
+/**
+ * Reads the start of a string, raising an error if its length is not
+ * at most the given number of bytes (not including any null-terminator.)
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_str() must be called
+ * once all bytes have been read.
+ *
+ * @throws mpack_error_type If the value is not a string.
+ * @throws mpack_error_too_big If the string's length in bytes is larger than the given maximum size.
+ */
+MPACK_INLINE uint32_t mpack_expect_str_max(mpack_reader_t* reader, uint32_t maxsize) {
+    uint32_t length = mpack_expect_str(reader);
+    if (length > maxsize) {
+        mpack_reader_flag_error(reader, mpack_error_too_big);
+        return 0;
+    }
+    return length;
+}
+
+/**
+ * Reads the start of a string, raising an error if its length is not
+ * exactly the given number of bytes (not including any null-terminator.)
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_str() must be called
+ * once all bytes have been read.
+ *
+ * mpack_error_type is raised if the value is not a string or if its
+ * length does not match.
+ */
+MPACK_INLINE void mpack_expect_str_length(mpack_reader_t* reader, uint32_t count) {
+    if (mpack_expect_str(reader) != count)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+/**
+ * Reads a string, ensuring it exactly matches the given string.
+ *
+ * Remember that maps are unordered in JSON. Don't use this for map keys
+ * unless the map has only a single key!
+ */
+void mpack_expect_str_match(mpack_reader_t* reader, const char* str, size_t length);
+
+/**
+ * Reads a string into the given buffer, ensures it has no null bytes,
+ * and adds a null-terminator at the end.
+ *
+ * Raises mpack_error_too_big if there is not enough room for the string and null-terminator.
+ * Raises mpack_error_type if the value is not a string or contains a null byte.
+ */
+void mpack_expect_cstr(mpack_reader_t* reader, char* buf, size_t size);
+
+/**
+ * Reads a string into the given buffer, ensures it is a valid UTF-8 string
+ * without NUL characters, and adds a null-terminator at the end.
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed, but without the NUL character, since
+ * it cannot be represented in a null-terminated string.
+ *
+ * Raises mpack_error_too_big if there is not enough room for the string and null-terminator.
+ * Raises mpack_error_type if the value is not a string or is not a valid UTF-8 string.
+ */
+void mpack_expect_utf8_cstr(mpack_reader_t* reader, char* buf, size_t size);
+
+#ifdef MPACK_MALLOC
+/**
+ * Reads a string with the given total maximum size (including space for a
+ * null-terminator), allocates storage for it, ensures it has no null-bytes,
+ * and adds a null-terminator at the end. You assume ownership of the
+ * returned pointer if reading succeeds.
+ *
+ * The allocated string must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @throws mpack_error_too_big If the string plus null-terminator is larger than the given maxsize.
+ * @throws mpack_error_type If the value is not a string or contains a null byte.
+ */
+char* mpack_expect_cstr_alloc(mpack_reader_t* reader, size_t maxsize);
+
+/**
+ * Reads a string with the given total maximum size (including space for a
+ * null-terminator), allocates storage for it, ensures it is valid UTF-8
+ * with no null-bytes, and adds a null-terminator at the end. You assume
+ * ownership of the returned pointer if reading succeeds.
+ *
+ * The length in bytes of the string, not including the null-terminator,
+ * will be written to size.
+ *
+ * This does not accept any UTF-8 variant such as Modified UTF-8, CESU-8 or
+ * WTF-8. Only pure UTF-8 is allowed, but without the NUL character, since
+ * it cannot be represented in a null-terminated string.
+ *
+ * The allocated string must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ * if you want a null-terminator.
+ *
+ * @throws mpack_error_too_big If the string plus null-terminator is larger
+ *     than the given maxsize.
+ * @throws mpack_error_type If the value is not a string or contains
+ *     invalid UTF-8 or a null byte.
+ */
+char* mpack_expect_utf8_cstr_alloc(mpack_reader_t* reader, size_t maxsize);
+#endif
+
+/**
+ * Reads a string, ensuring it exactly matches the given null-terminated
+ * string.
+ *
+ * Remember that maps are unordered in JSON. Don't use this for map keys
+ * unless the map has only a single key!
+ */
+MPACK_INLINE void mpack_expect_cstr_match(mpack_reader_t* reader, const char* cstr) {
+    mpack_assert(cstr != NULL, "cstr pointer is NULL");
+    mpack_expect_str_match(reader, cstr, mpack_strlen(cstr));
+}
+
+/**
+ * @}
+ */
+
+/**
+ * @name Binary Data
+ * @{
+ */
+
+/**
+ * Reads the start of a binary blob, returning its size in bytes.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_bin() must be called
+ * once all bytes have been read.
+ *
+ * mpack_error_type is raised if the value is not a binary blob.
+ */
+uint32_t mpack_expect_bin(mpack_reader_t* reader);
+
+/**
+ * Reads the start of a binary blob, raising an error if its length is not
+ * at most the given number of bytes.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_bin() must be called
+ * once all bytes have been read.
+ *
+ * mpack_error_type is raised if the value is not a binary blob or if its
+ * length does not match.
+ */
+MPACK_INLINE uint32_t mpack_expect_bin_max(mpack_reader_t* reader, uint32_t maxsize) {
+    uint32_t length = mpack_expect_bin(reader);
+    if (length > maxsize) {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return 0;
+    }
+    return length;
+}
+
+/**
+ * Reads the start of a binary blob, raising an error if its length is not
+ * exactly the given number of bytes.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_bin() must be called
+ * once all bytes have been read.
+ *
+ * @throws mpack_error_type if the value is not a binary blob or if its size
+ * does not match.
+ */
+MPACK_INLINE void mpack_expect_bin_size(mpack_reader_t* reader, uint32_t count) {
+    if (mpack_expect_bin(reader) != count)
+        mpack_reader_flag_error(reader, mpack_error_type);
+}
+
+/**
+ * Reads a binary blob into the given buffer, returning its size in bytes.
+ *
+ * For compatibility, this will accept if the underlying type is string or
+ * binary (since in MessagePack 1.0, strings and binary data were combined
+ * under the "raw" type which became string in 1.1.)
+ */
+size_t mpack_expect_bin_buf(mpack_reader_t* reader, char* buf, size_t size);
+
+/**
+ * Reads a binary blob with the exact given size into the given buffer.
+ *
+ * For compatibility, this will accept if the underlying type is string or
+ * binary (since in MessagePack 1.0, strings and binary data were combined
+ * under the "raw" type which became string in 1.1.)
+ *
+ * @throws mpack_error_type if the value is not a binary blob or if its size
+ * does not match.
+ */
+void mpack_expect_bin_size_buf(mpack_reader_t* reader, char* buf, uint32_t size);
+
+/**
+ * Reads a binary blob with the given total maximum size, allocating storage for it.
+ */
+char* mpack_expect_bin_alloc(mpack_reader_t* reader, size_t maxsize, size_t* size);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Extension Functions
+ * @{
+ */
+
+#if MPACK_EXTENSIONS
+/**
+ * Reads the start of an extension blob, returning its size in bytes and
+ * placing the type into @p type.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_ext() must be called
+ * once all bytes have been read.
+ *
+ * @p type will be a user-defined type in the range [0,127] or a reserved type
+ * in the range [-128,-2].
+ *
+ * mpack_error_type is raised if the value is not an extension blob. The @p
+ * type value is zero if an error occurs.
+ *
+ * @note This cannot be used to match a timestamp. @ref mpack_error_type will
+ * be flagged if the value is a timestamp. Use mpack_expect_timestamp() or
+ * mpack_expect_timestamp_truncate() instead.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @warning Be careful when using reserved types. They may no longer be ext
+ * types in the future, and previously valid data containing reserved types may
+ * become invalid in the future.
+ */
+uint32_t mpack_expect_ext(mpack_reader_t* reader, int8_t* type);
+
+/**
+ * Reads the start of an extension blob, raising an error if its length is not
+ * at most the given number of bytes and placing the type into @p type.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_ext() must be called
+ * once all bytes have been read.
+ *
+ * mpack_error_type is raised if the value is not an extension blob or if its
+ * length does not match. The @p type value is zero if an error is raised.
+ *
+ * @p type will be a user-defined type in the range [0,127] or a reserved type
+ * in the range [-128,-2].
+ *
+ * @note This cannot be used to match a timestamp. @ref mpack_error_type will
+ * be flagged if the value is a timestamp. Use mpack_expect_timestamp() or
+ * mpack_expect_timestamp_truncate() instead.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @warning Be careful when using reserved types. They may no longer be ext
+ * types in the future, and previously valid data containing reserved types may
+ * become invalid in the future.
+ *
+ * @see mpack_expect_ext()
+ */
+MPACK_INLINE uint32_t mpack_expect_ext_max(mpack_reader_t* reader, int8_t* type, uint32_t maxsize) {
+    uint32_t length = mpack_expect_ext(reader, type);
+    if (length > maxsize) {
+        mpack_reader_flag_error(reader, mpack_error_type);
+        return 0;
+    }
+    return length;
+}
+
+/**
+ * Reads the start of an extension blob, raising an error if its length is not
+ * exactly the given number of bytes and placing the type into @p type.
+ *
+ * The bytes follow and must be read separately with mpack_read_bytes()
+ * or mpack_read_bytes_inplace(). @ref mpack_done_ext() must be called
+ * once all bytes have been read.
+ *
+ * mpack_error_type is raised if the value is not an extension blob or if its
+ * length does not match. The @p type value is zero if an error is raised.
+ *
+ * @p type will be a user-defined type in the range [0,127] or a reserved type
+ * in the range [-128,-2].
+ *
+ * @note This cannot be used to match a timestamp. @ref mpack_error_type will
+ * be flagged if the value is a timestamp. Use mpack_expect_timestamp() or
+ * mpack_expect_timestamp_truncate() instead.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @warning Be careful when using reserved types. They may no longer be ext
+ * types in the future, and previously valid data containing reserved types may
+ * become invalid in the future.
+ *
+ * @see mpack_expect_ext()
+ */
+MPACK_INLINE void mpack_expect_ext_size(mpack_reader_t* reader, int8_t* type, uint32_t count) {
+    if (mpack_expect_ext(reader, type) != count) {
+        *type = 0;
+        mpack_reader_flag_error(reader, mpack_error_type);
+    }
+}
+
+/**
+ * Reads an extension blob into the given buffer, returning its size in bytes
+ * and placing the type into @p type.
+ *
+ * mpack_error_type is raised if the value is not an extension blob or if its
+ * length does not match. The @p type value is zero if an error is raised.
+ *
+ * @p type will be a user-defined type in the range [0,127] or a reserved type
+ * in the range [-128,-2].
+ *
+ * @note This cannot be used to match a timestamp. @ref mpack_error_type will
+ * be flagged if the value is a timestamp. Use mpack_expect_timestamp() or
+ * mpack_expect_timestamp_truncate() instead.
+ *
+ * @warning Be careful when using reserved types. They may no longer be ext
+ * types in the future, and previously valid data containing reserved types may
+ * become invalid in the future.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @see mpack_expect_ext()
+ */
+size_t mpack_expect_ext_buf(mpack_reader_t* reader, int8_t* type, char* buf, size_t size);
+#endif
+
+#if MPACK_EXTENSIONS && defined(MPACK_MALLOC)
+/**
+ * Reads an extension blob with the given total maximum size, allocating
+ * storage for it, and placing the type into @p type.
+ *
+ * mpack_error_type is raised if the value is not an extension blob or if its
+ * length does not match. The @p type value is zero if an error is raised.
+ *
+ * @p type will be a user-defined type in the range [0,127] or a reserved type
+ * in the range [-128,-2].
+ *
+ * @note This cannot be used to match a timestamp. @ref mpack_error_type will
+ * be flagged if the value is a timestamp. Use mpack_expect_timestamp() or
+ * mpack_expect_timestamp_truncate() instead.
+ *
+ * @warning Be careful when using reserved types. They may no longer be ext
+ * types in the future, and previously valid data containing reserved types may
+ * become invalid in the future.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS and @ref MPACK_MALLOC.
+ *
+ * @see mpack_expect_ext()
+ */
+char* mpack_expect_ext_alloc(mpack_reader_t* reader, int8_t* type, size_t maxsize, size_t* size);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Special Functions
+ * @{
+ */
+
+/**
+ * Reads a MessagePack object header (an MPack tag), expecting it to exactly
+ * match the given tag.
+ *
+ * If the type is compound (i.e. is a map, array, string, binary or
+ * extension type), additional reads are required to get the contained
+ * data, and the corresponding done function must be called when done.
+ *
+ * @throws mpack_error_type if the tag does not match
+ *
+ * @see mpack_read_bytes()
+ * @see mpack_done_array()
+ * @see mpack_done_map()
+ * @see mpack_done_str()
+ * @see mpack_done_bin()
+ * @see mpack_done_ext()
+ */
+void mpack_expect_tag(mpack_reader_t* reader, mpack_tag_t tag);
+
+/**
+ * Expects a string matching one of the strings in the given array,
+ * returning its array index.
+ *
+ * If the value does not match any of the given strings,
+ * @ref mpack_error_type is flagged. Use mpack_expect_enum_optional()
+ * if you want to allow other values than the given strings.
+ *
+ * If any error occurs or the reader is in an error state, @a count
+ * is returned.
+ *
+ * This can be used to quickly parse a string into an enum when the
+ * enum values range from 0 to @a count-1. If the last value in the
+ * enum is a special "count" value, it can be passed as the count,
+ * and the return value can be cast directly to the enum type.
+ *
+ * @code{.c}
+ * typedef enum           { APPLE ,  BANANA ,  ORANGE , COUNT} fruit_t;
+ * const char* fruits[] = {"apple", "banana", "orange"};
+ *
+ * fruit_t fruit = (fruit_t)mpack_expect_enum(reader, fruits, COUNT);
+ * @endcode
+ *
+ * See @ref docs/expect.md for more examples.
+ *
+ * The maximum string length is the size of the buffer (strings are read in-place.)
+ *
+ * @param reader The reader
+ * @param strings An array of expected strings of length count
+ * @param count The number of strings
+ * @return The index of the matched string, or @a count in case of error
+ */
+size_t mpack_expect_enum(mpack_reader_t* reader, const char* strings[], size_t count);
+
+/**
+ * Expects a string matching one of the strings in the given array
+ * returning its array index, or @a count if no strings match.
+ *
+ * If the value is not a string, or it does not match any of the
+ * given strings, @a count is returned and no error is flagged.
+ *
+ * If any error occurs or the reader is in an error state, @a count
+ * is returned.
+ *
+ * This can be used to quickly parse a string into an enum when the
+ * enum values range from 0 to @a count-1. If the last value in the
+ * enum is a special "count" value, it can be passed as the count,
+ * and the return value can be cast directly to the enum type.
+ *
+ * @code{.c}
+ * typedef enum           { APPLE ,  BANANA ,  ORANGE , COUNT} fruit_t;
+ * const char* fruits[] = {"apple", "banana", "orange"};
+ *
+ * fruit_t fruit = (fruit_t)mpack_expect_enum_optional(reader, fruits, COUNT);
+ * @endcode
+ *
+ * See @ref docs/expect.md for more examples.
+ *
+ * The maximum string length is the size of the buffer (strings are read in-place.)
+ *
+ * @param reader The reader
+ * @param strings An array of expected strings of length count
+ * @param count The number of strings
+ *
+ * @return The index of the matched string, or @a count if it does not
+ * match or an error occurs
+ */
+size_t mpack_expect_enum_optional(mpack_reader_t* reader, const char* strings[], size_t count);
+
+/**
+ * Expects an unsigned integer map key between 0 and count-1, marking it
+ * as found in the given bool array and returning it.
+ *
+ * This is a helper for switching among int keys in a map. It is
+ * typically used with an enum to define the key values. It should
+ * be called in the expression of a switch() statement. See @ref
+ * docs/expect.md for an example.
+ *
+ * The found array must be cleared before expecting the first key. If the
+ * flag for a given key is already set when found (i.e. the map contains a
+ * duplicate key), mpack_error_invalid is flagged.
+ *
+ * If the key is not a non-negative integer, or if the key is @a count or
+ * larger, @a count is returned and no error is flagged. If you want an error
+ * on unrecognized keys, flag an error in the default case in your switch;
+ * otherwise you must call mpack_discard() to discard its content.
+ *
+ * @param reader The reader
+ * @param found An array of bool flags of length count
+ * @param count The number of values in the found array, and one more than the
+ *              maximum allowed key
+ *
+ * @see @ref docs/expect.md
+ */
+size_t mpack_expect_key_uint(mpack_reader_t* reader, bool found[], size_t count);
+
+/**
+ * Expects a string map key matching one of the strings in the given key list,
+ * marking it as found in the given bool array and returning its index.
+ *
+ * This is a helper for switching among string keys in a map. It is
+ * typically used with an enum with names matching the strings in the
+ * array to define the key indices. It should be called in the expression
+ * of a switch() statement. See @ref docs/expect.md for an example.
+ *
+ * The found array must be cleared before expecting the first key. If the
+ * flag for a given key is already set when found (i.e. the map contains a
+ * duplicate key), mpack_error_invalid is flagged.
+ *
+ * If the key is unrecognized, count is returned and no error is flagged. If
+ * you want an error on unrecognized keys, flag an error in the default case
+ * in your switch; otherwise you must call mpack_discard() to discard its content.
+ *
+ * The maximum key length is the size of the buffer (keys are read in-place.)
+ *
+ * @param reader The reader
+ * @param keys An array of expected string keys of length count
+ * @param found An array of bool flags of length count
+ * @param count The number of values in the keys and found arrays
+ *
+ * @see @ref docs/expect.md
+ */
+size_t mpack_expect_key_cstr(mpack_reader_t* reader, const char* keys[],
+        bool found[], size_t count);
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+#endif
+
+MPACK_EXTERN_C_END
+MPACK_SILENCE_WARNINGS_END
+
+#endif
+
+
+
+/* mpack/mpack-node.h.h */
+
+/**
+ * @file
+ *
+ * Declares the MPack dynamic Node API.
+ */
+
+#ifndef MPACK_NODE_H
+#define MPACK_NODE_H 1
+
+/* #include "mpack-reader.h" */
+
+MPACK_SILENCE_WARNINGS_BEGIN
+MPACK_EXTERN_C_BEGIN
+
+#if MPACK_NODE
+
+/**
+ * @defgroup node Node API
+ *
+ * The MPack Node API allows you to parse a chunk of MessagePack into a
+ * dynamically typed data structure, providing random access to the parsed
+ * data.
+ *
+ * See @ref docs/node.md for examples.
+ *
+ * @{
+ */
+
+/**
+ * A handle to node data in a parsed MPack tree.
+ *
+ * Nodes represent either primitive values or compound types. If a
+ * node is a compound type, it contains a pointer to its child nodes,
+ * or a pointer to its underlying data.
+ *
+ * Nodes are immutable.
+ *
+ * @note @ref mpack_node_t is an opaque reference to the node data, not the
+ * node data itself. (It contains pointers to both the node data and the tree.)
+ * It is passed by value in the Node API.
+ */
+typedef struct mpack_node_t mpack_node_t;
+
+/**
+ * The storage for nodes in an MPack tree.
+ *
+ * You only need to use this if you intend to provide your own storage
+ * for nodes instead of letting the tree allocate it.
+ *
+ * @ref mpack_node_data_t is 16 bytes on most common architectures (32-bit
+ * and 64-bit.)
+ */
+typedef struct mpack_node_data_t mpack_node_data_t;
+
+/**
+ * An MPack tree parser to parse a blob or stream of MessagePack.
+ *
+ * When a message is parsed, the tree contains a single root node which
+ * contains all parsed data. The tree and its nodes are immutable.
+ */
+typedef struct mpack_tree_t mpack_tree_t;
+
+/**
+ * An error handler function to be called when an error is flagged on
+ * the tree.
+ *
+ * The error handler will only be called once on the first error flagged;
+ * any subsequent node reads and errors are ignored, and the tree is
+ * permanently in that error state.
+ *
+ * MPack is safe against non-local jumps out of error handler callbacks.
+ * This means you are allowed to longjmp or throw an exception (in C++,
+ * Objective-C, or with SEH) out of this callback.
+ *
+ * Bear in mind when using longjmp that local non-volatile variables that
+ * have changed are undefined when setjmp() returns, so you can't put the
+ * tree on the stack in the same activation frame as the setjmp without
+ * declaring it volatile.
+ *
+ * You must still eventually destroy the tree. It is not destroyed
+ * automatically when an error is flagged. It is safe to destroy the
+ * tree within this error callback, but you will either need to perform
+ * a non-local jump, or store something in your context to identify
+ * that the tree is destroyed since any future accesses to it cause
+ * undefined behavior.
+ */
+typedef void (*mpack_tree_error_t)(mpack_tree_t* tree, mpack_error_t error);
+
+/**
+ * The MPack tree's read function. It should fill the buffer with as many bytes
+ * as are immediately available up to the given @c count, returning the number
+ * of bytes written to the buffer.
+ *
+ * In case of error, it should flag an appropriate error on the reader
+ * (usually @ref mpack_error_io.)
+ *
+ * The blocking or non-blocking behaviour of the read should match whether you
+ * are using mpack_tree_parse() or mpack_tree_try_parse().
+ *
+ * If you are using mpack_tree_parse(), the read should block until at least
+ * one byte is read. If you return 0, mpack_tree_parse() will raise @ref
+ * mpack_error_io.
+ *
+ * If you are using mpack_tree_try_parse(), the read function can always
+ * return 0, and must never block waiting for data (otherwise
+ * mpack_tree_try_parse() would be equivalent to mpack_tree_parse().)
+ * When you return 0, mpack_tree_try_parse() will return false without flagging
+ * an error.
+ */
+typedef size_t (*mpack_tree_read_t)(mpack_tree_t* tree, char* buffer, size_t count);
+
+/**
+ * A teardown function to be called when the tree is destroyed.
+ */
+typedef void (*mpack_tree_teardown_t)(mpack_tree_t* tree);
+
+
+
+/* Hide internals from documentation */
+/** @cond */
+
+struct mpack_node_t {
+    mpack_node_data_t* data;
+    mpack_tree_t* tree;
+};
+
+struct mpack_node_data_t {
+    mpack_type_t type;
+
+    /*
+     * The element count if the type is an array;
+     * the number of key/value pairs if the type is map;
+     * or the number of bytes if the type is str, bin or ext.
+     */
+    uint32_t len;
+
+    union {
+        bool     b; /* The value if the type is bool. */
+
+        #if MPACK_FLOAT
+        float    f; /* The value if the type is float. */
+        #else
+        uint32_t f; /*< The raw value if the type is float. */
+        #endif
+
+        #if MPACK_DOUBLE
+        double   d; /* The value if the type is double. */
+        #else
+        uint64_t d; /*< The raw value if the type is double. */
+        #endif
+
+        int64_t  i; /* The value if the type is signed int. */
+        uint64_t u; /* The value if the type is unsigned int. */
+        size_t offset; /* The byte offset for str, bin and ext */
+
+        mpack_node_data_t* children; /* The children for map or array */
+    } value;
+};
+
+typedef struct mpack_tree_page_t {
+    struct mpack_tree_page_t* next;
+    mpack_node_data_t nodes[1]; // variable size
+} mpack_tree_page_t;
+
+typedef enum mpack_tree_parse_state_t {
+    mpack_tree_parse_state_not_started,
+    mpack_tree_parse_state_in_progress,
+    mpack_tree_parse_state_parsed,
+} mpack_tree_parse_state_t;
+
+typedef struct mpack_level_t {
+    mpack_node_data_t* child;
+    size_t left; // children left in level
+} mpack_level_t;
+
+typedef struct mpack_tree_parser_t {
+    mpack_tree_parse_state_t state;
+
+    // We keep track of the number of "possible nodes" left in the data rather
+    // than the number of bytes.
+    //
+    // When a map or array is parsed, we ensure at least one byte for each child
+    // exists and subtract them right away. This ensures that if ever a map or
+    // array declares more elements than could possibly be contained in the data,
+    // we will error out immediately rather than allocating storage for them.
+    //
+    // For example malicious data that repeats 0xDE 0xFF 0xFF (start of a map
+    // with 65536 key-value pairs) would otherwise cause us to run out of
+    // memory. With this, the parser can allocate at most as many nodes as
+    // there are bytes in the data (plus the paging overhead, 12%.) An error
+    // will be flagged immediately if and when there isn't enough data left to
+    // fully read all children of all open compound types on the parsing stack.
+    //
+    // Once an entire message has been parsed (and there are no nodes left to
+    // parse whose bytes have been subtracted), this matches the number of left
+    // over bytes in the data.
+    size_t possible_nodes_left;
+
+    mpack_node_data_t* nodes; // next node in current page/pool
+    size_t nodes_left; // nodes left in current page/pool
+
+    size_t current_node_reserved;
+    size_t level;
+
+    #ifdef MPACK_MALLOC
+    // It's much faster to allocate the initial parsing stack inline within the
+    // parser. We replace it with a heap allocation if we need to grow it.
+    mpack_level_t* stack;
+    size_t stack_capacity;
+    bool stack_owned;
+    mpack_level_t stack_local[MPACK_NODE_INITIAL_DEPTH];
+    #else
+    // Without malloc(), we have to reserve a parsing stack the maximum allowed
+    // parsing depth.
+    mpack_level_t stack[MPACK_NODE_MAX_DEPTH_WITHOUT_MALLOC];
+    #endif
+} mpack_tree_parser_t;
+
+struct mpack_tree_t {
+    mpack_tree_error_t error_fn;    /* Function to call on error */
+    mpack_tree_read_t read_fn;      /* Function to call to read more data */
+    mpack_tree_teardown_t teardown; /* Function to teardown the context on destroy */
+    void* context;                  /* Context for tree callbacks */
+
+    mpack_node_data_t nil_node;     /* a nil node to be returned in case of error */
+    mpack_node_data_t missing_node; /* a missing node to be returned in optional lookups */
+    mpack_error_t error;
+
+    #ifdef MPACK_MALLOC
+    char* buffer;
+    size_t buffer_capacity;
+    #endif
+
+    const char* data;
+    size_t data_length; // length of data (and content of buffer, if used)
+
+    size_t size; // size in bytes of tree (usually matches data_length, but not if tree has trailing data)
+    size_t node_count; // total number of nodes in tree (across all pages)
+
+    size_t max_size;  // maximum message size
+    size_t max_nodes; // maximum nodes in a message
+
+    mpack_tree_parser_t parser;
+    mpack_node_data_t* root;
+
+    mpack_node_data_t* pool; // pool, or NULL if no pool provided
+    size_t pool_count;
+
+    #ifdef MPACK_MALLOC
+    mpack_tree_page_t* next;
+    #endif
+};
+
+// internal functions
+
+MPACK_INLINE mpack_node_t mpack_node(mpack_tree_t* tree, mpack_node_data_t* data) {
+    mpack_node_t node;
+    node.data = data;
+    node.tree = tree;
+    return node;
+}
+
+MPACK_INLINE mpack_node_data_t* mpack_node_child(mpack_node_t node, size_t child) {
+    return node.data->value.children + child;
+}
+
+MPACK_INLINE mpack_node_t mpack_tree_nil_node(mpack_tree_t* tree) {
+    return mpack_node(tree, &tree->nil_node);
+}
+
+MPACK_INLINE mpack_node_t mpack_tree_missing_node(mpack_tree_t* tree) {
+    return mpack_node(tree, &tree->missing_node);
+}
+
+/** @endcond */
+
+
+
+/**
+ * @name Tree Initialization
+ * @{
+ */
+
+#ifdef MPACK_MALLOC
+/**
+ * Initializes a tree parser with the given data.
+ *
+ * Configure the tree if desired, then call mpack_tree_parse() to parse it. The
+ * tree will allocate pages of nodes as needed and will free them when
+ * destroyed.
+ *
+ * The tree must be destroyed with mpack_tree_destroy().
+ *
+ * Any string or blob data types reference the original data, so the given data
+ * pointer must remain valid until after the tree is destroyed.
+ */
+void mpack_tree_init_data(mpack_tree_t* tree, const char* data, size_t length);
+
+/**
+ * Deprecated.
+ *
+ * \deprecated Renamed to mpack_tree_init_data().
+ */
+MPACK_INLINE void mpack_tree_init(mpack_tree_t* tree, const char* data, size_t length) {
+    mpack_tree_init_data(tree, data, length);
+}
+
+/**
+ * Initializes a tree parser from an unbounded stream, or a stream of
+ * unknown length.
+ *
+ * The parser can be used to read a single message from a stream of unknown
+ * length, or multiple messages from an unbounded stream, allowing it to
+ * be used for RPC communication. Call @ref mpack_tree_parse() to parse
+ * a message from a blocking stream, or @ref mpack_tree_try_parse() for a
+ * non-blocking stream.
+ *
+ * The stream will use a growable internal buffer to store the most recent
+ * message, as well as allocated pages of nodes for the parse tree.
+ *
+ * Maximum allowances for message size and node count must be specified in this
+ * function (since the stream is unbounded.) They can be changed later with
+ * @ref mpack_tree_set_limits().
+ *
+ * @param tree The tree parser
+ * @param read_fn The read function
+ * @param context The context for the read function
+ * @param max_message_size The maximum size of a message in bytes
+ * @param max_message_nodes The maximum number of nodes per message. See
+ *        @ref mpack_node_data_t for the size of nodes.
+ *
+ * @see mpack_tree_read_t
+ * @see mpack_reader_context()
+ */
+void mpack_tree_init_stream(mpack_tree_t* tree, mpack_tree_read_t read_fn, void* context,
+        size_t max_message_size, size_t max_message_nodes);
+#endif
+
+/**
+ * Initializes a tree parser with the given data, using the given node data
+ * pool to store the results.
+ *
+ * Configure the tree if desired, then call mpack_tree_parse() to parse it.
+ *
+ * If the data does not fit in the pool, @ref mpack_error_too_big will be flagged
+ * on the tree.
+ *
+ * The tree must be destroyed with mpack_tree_destroy(), even if parsing fails.
+ */
+void mpack_tree_init_pool(mpack_tree_t* tree, const char* data, size_t length,
+        mpack_node_data_t* node_pool, size_t node_pool_count);
+
+/**
+ * Initializes an MPack tree directly into an error state. Use this if you
+ * are writing a wrapper to another <tt>mpack_tree_init*()</tt> function which
+ * can fail its setup.
+ */
+void mpack_tree_init_error(mpack_tree_t* tree, mpack_error_t error);
+
+#if MPACK_STDIO
+/**
+ * Initializes a tree to parse the given file. The tree must be destroyed with
+ * mpack_tree_destroy(), even if parsing fails.
+ *
+ * The file is opened, loaded fully into memory, and closed before this call
+ * returns.
+ *
+ * @param tree The tree to initialize
+ * @param filename The filename passed to fopen() to read the file
+ * @param max_bytes The maximum size of file to load, or 0 for unlimited size.
+ */
+void mpack_tree_init_filename(mpack_tree_t* tree, const char* filename, size_t max_bytes);
+
+/**
+ * Deprecated.
+ *
+ * \deprecated Renamed to mpack_tree_init_filename().
+ */
+MPACK_INLINE void mpack_tree_init_file(mpack_tree_t* tree, const char* filename, size_t max_bytes) {
+    mpack_tree_init_filename(tree, filename, max_bytes);
+}
+
+/**
+ * Initializes a tree to parse the given libc FILE. This can be used to
+ * read from stdin, or from a file opened separately.
+ *
+ * The tree must be destroyed with mpack_tree_destroy(), even if parsing fails.
+ *
+ * The FILE is fully loaded fully into memory (and closed if requested) before
+ * this call returns.
+ *
+ * @param tree The tree to initialize.
+ * @param stdfile The FILE.
+ * @param max_bytes The maximum size of file to load, or 0 for unlimited size.
+ * @param close_when_done If true, fclose() will be called on the FILE when it
+ *         is no longer needed. If false, the file will not be closed when
+ *         reading is done.
+ *
+ * @warning The tree will read all data in the FILE before parsing it. If this
+ *          is used on stdin, the parser will block until it is closed, even if
+ *          a complete message has been written to it!
+ */
+void mpack_tree_init_stdfile(mpack_tree_t* tree, FILE* stdfile, size_t max_bytes, bool close_when_done);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Tree Functions
+ * @{
+ */
+
+/**
+ * Sets the maximum byte size and maximum number of nodes allowed per message.
+ *
+ * The default is SIZE_MAX (no limit) unless @ref mpack_tree_init_stream() is
+ * called (where maximums are required.)
+ *
+ * If a pool of nodes is used, the node limit is the lesser of this limit and
+ * the pool size.
+ *
+ * @param tree The tree parser
+ * @param max_message_size The maximum size of a message in bytes
+ * @param max_message_nodes The maximum number of nodes per message. See
+ *        @ref mpack_node_data_t for the size of nodes.
+ */
+void mpack_tree_set_limits(mpack_tree_t* tree, size_t max_message_size,
+        size_t max_message_nodes);
+
+/**
+ * Parses a MessagePack message into a tree of immutable nodes.
+ *
+ * If successful, the root node will be available under @ref mpack_tree_root().
+ * If not, an appropriate error will be flagged.
+ *
+ * This can be called repeatedly to parse a series of messages from a data
+ * source. When this is called, all previous nodes from this tree and their
+ * contents (including the root node) are invalidated.
+ *
+ * If this is called with a stream (see @ref mpack_tree_init_stream()), the
+ * stream must block until data is available. (Otherwise, if this is called on
+ * a non-blocking stream, parsing will fail with @ref mpack_error_io when the
+ * fill function returns 0.)
+ *
+ * There is no way to recover a tree in an error state. It must be destroyed.
+ */
+void mpack_tree_parse(mpack_tree_t* tree);
+
+/**
+ * Attempts to parse a MessagePack message from a non-blocking stream into a
+ * tree of immutable nodes.
+ *
+ * A non-blocking read function must have been passed to the tree in
+ * mpack_tree_init_stream().
+ *
+ * If this returns true, a message is available under
+ * @ref mpack_tree_root(). The tree nodes and data will be valid until
+ * the next time a parse is started.
+ *
+ * If this returns false, no message is available, because either not enough
+ * data is available yet or an error has occurred. You must check the tree for
+ * errors whenever this returns false. If there is no error, you should try
+ * again later when more data is available. (You will want to select()/poll()
+ * on the underlying socket or use some other asynchronous mechanism to
+ * determine when it has data.)
+ *
+ * There is no way to recover a tree in an error state. It must be destroyed.
+ *
+ * @see mpack_tree_init_stream()
+ */
+bool mpack_tree_try_parse(mpack_tree_t* tree);
+
+/**
+ * Returns the root node of the tree, if the tree is not in an error state.
+ * Returns a nil node otherwise.
+ *
+ * @warning You must call mpack_tree_parse() before calling this. If
+ * @ref mpack_tree_parse() was never called, the tree will assert.
+ */
+mpack_node_t mpack_tree_root(mpack_tree_t* tree);
+
+/**
+ * Returns the error state of the tree.
+ */
+MPACK_INLINE mpack_error_t mpack_tree_error(mpack_tree_t* tree) {
+    return tree->error;
+}
+
+/**
+ * Returns the size in bytes of the current parsed message.
+ *
+ * If there is something in the buffer after the MessagePack object, this can
+ * be used to find it.
+ *
+ * This is zero if an error occurred during tree parsing (since the
+ * portion of the data that the first complete object occupies cannot
+ * be determined if the data is invalid or corrupted.)
+ */
+MPACK_INLINE size_t mpack_tree_size(mpack_tree_t* tree) {
+    return tree->size;
+}
+
+/**
+ * Destroys the tree.
+ */
+mpack_error_t mpack_tree_destroy(mpack_tree_t* tree);
+
+/**
+ * Sets the custom pointer to pass to the tree callbacks, such as teardown.
+ *
+ * @param tree The MPack tree.
+ * @param context User data to pass to the tree callbacks.
+ *
+ * @see mpack_reader_context()
+ */
+MPACK_INLINE void mpack_tree_set_context(mpack_tree_t* tree, void* context) {
+    tree->context = context;
+}
+
+/**
+ * Returns the custom context for tree callbacks.
+ *
+ * @see mpack_tree_set_context
+ * @see mpack_tree_init_stream
+ */
+MPACK_INLINE void* mpack_tree_context(mpack_tree_t* tree) {
+    return tree->context;
+}
+
+/**
+ * Sets the error function to call when an error is flagged on the tree.
+ *
+ * This should normally be used with mpack_tree_set_context() to register
+ * a custom pointer to pass to the error function.
+ *
+ * See the definition of mpack_tree_error_t for more information about
+ * what you can do from an error callback.
+ *
+ * @see mpack_tree_error_t
+ * @param tree The MPack tree.
+ * @param error_fn The function to call when an error is flagged on the tree.
+ */
+MPACK_INLINE void mpack_tree_set_error_handler(mpack_tree_t* tree, mpack_tree_error_t error_fn) {
+    tree->error_fn = error_fn;
+}
+
+/**
+ * Sets the teardown function to call when the tree is destroyed.
+ *
+ * This should normally be used with mpack_tree_set_context() to register
+ * a custom pointer to pass to the teardown function.
+ *
+ * @param tree The MPack tree.
+ * @param teardown The function to call when the tree is destroyed.
+ */
+MPACK_INLINE void mpack_tree_set_teardown(mpack_tree_t* tree, mpack_tree_teardown_t teardown) {
+    tree->teardown = teardown;
+}
+
+/**
+ * Places the tree in the given error state, calling the error callback if one
+ * is set.
+ *
+ * This allows you to externally flag errors, for example if you are validating
+ * data as you read it.
+ *
+ * If the tree is already in an error state, this call is ignored and no
+ * error callback is called.
+ */
+void mpack_tree_flag_error(mpack_tree_t* tree, mpack_error_t error);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Node Core Functions
+ * @{
+ */
+
+/**
+ * Places the node's tree in the given error state, calling the error callback
+ * if one is set.
+ *
+ * This allows you to externally flag errors, for example if you are validating
+ * data as you read it.
+ *
+ * If the tree is already in an error state, this call is ignored and no
+ * error callback is called.
+ */
+void mpack_node_flag_error(mpack_node_t node, mpack_error_t error);
+
+/**
+ * Returns the error state of the node's tree.
+ */
+MPACK_INLINE mpack_error_t mpack_node_error(mpack_node_t node) {
+    return mpack_tree_error(node.tree);
+}
+
+/**
+ * Returns a tag describing the given node, or a nil tag if the
+ * tree is in an error state.
+ */
+mpack_tag_t mpack_node_tag(mpack_node_t node);
+
+/** @cond */
+
+#if MPACK_DEBUG && MPACK_STDIO
+/*
+ * Converts a node to a pseudo-JSON string for debugging purposes, placing the
+ * result in the given buffer with a null-terminator.
+ *
+ * If the buffer does not have enough space, the result will be truncated (but
+ * it is guaranteed to be null-terminated.)
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+void mpack_node_print_to_buffer(mpack_node_t node, char* buffer, size_t buffer_size);
+
+/*
+ * Converts a node to pseudo-JSON for debugging purposes, calling the given
+ * callback as many times as is necessary to output the character data.
+ *
+ * No null-terminator or trailing newline will be written.
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+void mpack_node_print_to_callback(mpack_node_t node, mpack_print_callback_t callback, void* context);
+
+/*
+ * Converts a node to pseudo-JSON for debugging purposes
+ * and pretty-prints it to the given file.
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+void mpack_node_print_to_file(mpack_node_t node, FILE* file);
+
+/*
+ * Converts a node to pseudo-JSON for debugging purposes
+ * and pretty-prints it to stdout.
+ *
+ * This is only available in debug mode, and only if stdio is available (since
+ * it uses snprintf().) It's strictly for debugging purposes.
+ */
+MPACK_INLINE void mpack_node_print_to_stdout(mpack_node_t node) {
+    mpack_node_print_to_file(node, stdout);
+}
+
+/*
+ * Deprecated.
+ *
+ * \deprecated Renamed to mpack_node_print_to_stdout().
+ */
+MPACK_INLINE void mpack_node_print(mpack_node_t node) {
+    mpack_node_print_to_stdout(node);
+}
+#endif
+
+/** @endcond */
+
+/**
+ * @}
+ */
+
+/**
+ * @name Node Primitive Value Functions
+ * @{
+ */
+
+/**
+ * Returns the type of the node.
+ */
+mpack_type_t mpack_node_type(mpack_node_t node);
+
+/**
+ * Returns true if the given node is a nil node; false otherwise.
+ *
+ * To ensure that a node is nil and flag an error otherwise, use
+ * mpack_node_nil().
+ */
+bool mpack_node_is_nil(mpack_node_t node);
+
+/**
+ * Returns true if the given node handle indicates a missing node; false otherwise.
+ *
+ * To ensure that a node is missing and flag an error otherwise, use
+ * mpack_node_missing().
+ */
+bool mpack_node_is_missing(mpack_node_t node);
+
+/**
+ * Checks that the given node is of nil type, raising @ref mpack_error_type
+ * otherwise.
+ *
+ * Use mpack_node_is_nil() to return whether the node is nil.
+ */
+void mpack_node_nil(mpack_node_t node);
+
+/**
+ * Checks that the given node indicates a missing node, raising @ref
+ * mpack_error_type otherwise.
+ *
+ * Use mpack_node_is_missing() to return whether the node is missing.
+ */
+void mpack_node_missing(mpack_node_t node);
+
+/**
+ * Returns the bool value of the node. If this node is not of the correct
+ * type, false is returned and mpack_error_type is raised.
+ */
+bool mpack_node_bool(mpack_node_t node);
+
+/**
+ * Checks if the given node is of bool type with value true, raising
+ * mpack_error_type otherwise.
+ */
+void mpack_node_true(mpack_node_t node);
+
+/**
+ * Checks if the given node is of bool type with value false, raising
+ * mpack_error_type otherwise.
+ */
+void mpack_node_false(mpack_node_t node);
+
+/**
+ * Returns the 8-bit unsigned value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+uint8_t mpack_node_u8(mpack_node_t node);
+
+/**
+ * Returns the 8-bit signed value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+int8_t mpack_node_i8(mpack_node_t node);
+
+/**
+ * Returns the 16-bit unsigned value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+uint16_t mpack_node_u16(mpack_node_t node);
+
+/**
+ * Returns the 16-bit signed value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+int16_t mpack_node_i16(mpack_node_t node);
+
+/**
+ * Returns the 32-bit unsigned value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+uint32_t mpack_node_u32(mpack_node_t node);
+
+/**
+ * Returns the 32-bit signed value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+int32_t mpack_node_i32(mpack_node_t node);
+
+/**
+ * Returns the 64-bit unsigned value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised, and zero is returned.
+ */
+uint64_t mpack_node_u64(mpack_node_t node);
+
+/**
+ * Returns the 64-bit signed value of the node. If this node is not
+ * of a compatible type, @ref mpack_error_type is raised and zero is returned.
+ */
+int64_t mpack_node_i64(mpack_node_t node);
+
+/**
+ * Returns the unsigned int value of the node.
+ *
+ * Returns zero if an error occurs.
+ *
+ * @throws mpack_error_type If the node is not an integer type or does not fit in the range of an unsigned int
+ */
+unsigned int mpack_node_uint(mpack_node_t node);
+
+/**
+ * Returns the int value of the node.
+ *
+ * Returns zero if an error occurs.
+ *
+ * @throws mpack_error_type If the node is not an integer type or does not fit in the range of an int
+ */
+int mpack_node_int(mpack_node_t node);
+
+#if MPACK_FLOAT
+/**
+ * Returns the float value of the node. The underlying value can be an
+ * integer, float or double; the value is converted to a float.
+ *
+ * @note Reading a double or a large integer with this function can incur a
+ * loss of precision.
+ *
+ * @throws mpack_error_type if the underlying value is not a float, double or integer.
+ */
+float mpack_node_float(mpack_node_t node);
+#endif
+
+#if MPACK_DOUBLE
+/**
+ * Returns the double value of the node. The underlying value can be an
+ * integer, float or double; the value is converted to a double.
+ *
+ * @note Reading a very large integer with this function can incur a
+ * loss of precision.
+ *
+ * @throws mpack_error_type if the underlying value is not a float, double or integer.
+ */
+double mpack_node_double(mpack_node_t node);
+#endif
+
+#if MPACK_FLOAT
+/**
+ * Returns the float value of the node. The underlying value must be a float,
+ * not a double or an integer. This ensures no loss of precision can occur.
+ *
+ * @throws mpack_error_type if the underlying value is not a float.
+ */
+float mpack_node_float_strict(mpack_node_t node);
+#endif
+
+#if MPACK_DOUBLE
+/**
+ * Returns the double value of the node. The underlying value must be a float
+ * or double, not an integer. This ensures no loss of precision can occur.
+ *
+ * @throws mpack_error_type if the underlying value is not a float or double.
+ */
+double mpack_node_double_strict(mpack_node_t node);
+#endif
+
+#if !MPACK_FLOAT
+/**
+ * Returns the float value of the node as a raw uint32_t. The underlying value
+ * must be a float, not a double or an integer.
+ *
+ * @throws mpack_error_type if the underlying value is not a float.
+ */
+uint32_t mpack_node_raw_float(mpack_node_t node);
+#endif
+
+#if !MPACK_DOUBLE
+/**
+ * Returns the double value of the node as a raw uint64_t. The underlying value
+ * must be a double, not a float or an integer.
+ *
+ * @throws mpack_error_type if the underlying value is not a float or double.
+ */
+uint64_t mpack_node_raw_double(mpack_node_t node);
+#endif
+
+
+#if MPACK_EXTENSIONS
+/**
+ * Returns a timestamp.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @throws mpack_error_type if the underlying value is not a timestamp.
+ */
+mpack_timestamp_t mpack_node_timestamp(mpack_node_t node);
+
+/**
+ * Returns a timestamp's (signed) seconds since 1970-01-01T00:00:00Z.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @throws mpack_error_type if the underlying value is not a timestamp.
+ */
+int64_t mpack_node_timestamp_seconds(mpack_node_t node);
+
+/**
+ * Returns a timestamp's additional nanoseconds.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ *
+ * @return A nanosecond count between 0 and 999,999,999 inclusive.
+ * @throws mpack_error_type if the underlying value is not a timestamp.
+ */
+uint32_t mpack_node_timestamp_nanoseconds(mpack_node_t node);
+#endif
+
+/**
+ * @}
+ */
+
+/**
+ * @name Node String and Data Functions
+ * @{
+ */
+
+/**
+ * Checks that the given node contains a valid UTF-8 string.
+ *
+ * If the string is invalid, this flags an error, which would cause subsequent calls
+ * to mpack_node_str() to return NULL and mpack_node_strlen() to return zero. So you
+ * can check the node for error immediately after calling this, or you can call those
+ * functions to use the data anyway and check for errors later.
+ *
+ * @throws mpack_error_type If this node is not a string or does not contain valid UTF-8.
+ *
+ * @param node The string node to test
+ *
+ * @see mpack_node_str()
+ * @see mpack_node_strlen()
+ */
+void mpack_node_check_utf8(mpack_node_t node);
+
+/**
+ * Checks that the given node contains a valid UTF-8 string with no NUL bytes.
+ *
+ * This does not check that the string has a null-terminator! It only checks whether
+ * the string could safely be represented as a C-string by appending a null-terminator.
+ * (If the string does already contain a null-terminator, this will flag an error.)
+ *
+ * This is performed automatically by other UTF-8 cstr helper functions. Only
+ * call this if you will do something else with the data directly, but you still
+ * want to ensure it will be valid as a UTF-8 C-string.
+ *
+ * @throws mpack_error_type If this node is not a string, does not contain valid UTF-8,
+ *     or contains a NUL byte.
+ *
+ * @param node The string node to test
+ *
+ * @see mpack_node_str()
+ * @see mpack_node_strlen()
+ * @see mpack_node_copy_utf8_cstr()
+ * @see mpack_node_utf8_cstr_alloc()
+ */
+void mpack_node_check_utf8_cstr(mpack_node_t node);
+
+#if MPACK_EXTENSIONS
+/**
+ * Returns the extension type of the given ext node.
+ *
+ * This returns zero if the tree is in an error state.
+ *
+ * @note This requires @ref MPACK_EXTENSIONS.
+ */
+int8_t mpack_node_exttype(mpack_node_t node);
+#endif
+
+/**
+ * Returns the number of bytes in the given bin node.
+ *
+ * This returns zero if the tree is in an error state.
+ *
+ * If this node is not a bin, @ref mpack_error_type is raised and zero is returned.
+ */
+size_t mpack_node_bin_size(mpack_node_t node);
+
+/**
+ * Returns the length of the given str, bin or ext node.
+ *
+ * This returns zero if the tree is in an error state.
+ *
+ * If this node is not a str, bin or map, @ref mpack_error_type is raised and zero
+ * is returned.
+ */
+uint32_t mpack_node_data_len(mpack_node_t node);
+
+/**
+ * Returns the length in bytes of the given string node. This does not
+ * include any null-terminator.
+ *
+ * This returns zero if the tree is in an error state.
+ *
+ * If this node is not a str, @ref mpack_error_type is raised and zero is returned.
+ */
+size_t mpack_node_strlen(mpack_node_t node);
+
+/**
+ * Returns a pointer to the data contained by this node, ensuring the node is a
+ * string.
+ *
+ * @warning Strings are not null-terminated! Use one of the cstr functions
+ * to get a null-terminated string.
+ *
+ * The pointer is valid as long as the data backing the tree is valid.
+ *
+ * If this node is not a string, @ref mpack_error_type is raised and @c NULL is returned.
+ *
+ * @see mpack_node_copy_cstr()
+ * @see mpack_node_cstr_alloc()
+ * @see mpack_node_utf8_cstr_alloc()
+ */
+const char* mpack_node_str(mpack_node_t node);
+
+/**
+ * Returns a pointer to the data contained by this node.
+ *
+ * @note Strings are not null-terminated! Use one of the cstr functions
+ * to get a null-terminated string.
+ *
+ * The pointer is valid as long as the data backing the tree is valid.
+ *
+ * If this node is not of a str, bin or map, @ref mpack_error_type is raised, and
+ * @c NULL is returned.
+ *
+ * @see mpack_node_copy_cstr()
+ * @see mpack_node_cstr_alloc()
+ * @see mpack_node_utf8_cstr_alloc()
+ */
+const char* mpack_node_data(mpack_node_t node);
+
+/**
+ * Returns a pointer to the data contained by this bin node.
+ *
+ * The pointer is valid as long as the data backing the tree is valid.
+ *
+ * If this node is not a bin, @ref mpack_error_type is raised and @c NULL is
+ * returned.
+ */
+const char* mpack_node_bin_data(mpack_node_t node);
+
+/**
+ * Copies the bytes contained by this node into the given buffer, returning the
+ * number of bytes in the node.
+ *
+ * @throws mpack_error_type If this node is not a str, bin or ext type
+ * @throws mpack_error_too_big If the string does not fit in the given buffer
+ *
+ * @param node The string node from which to copy data
+ * @param buffer A buffer in which to copy the node's bytes
+ * @param bufsize The size of the given buffer
+ *
+ * @return The number of bytes in the node, or zero if an error occurs.
+ */
+size_t mpack_node_copy_data(mpack_node_t node, char* buffer, size_t bufsize);
+
+/**
+ * Checks that the given node contains a valid UTF-8 string and copies the
+ * string into the given buffer, returning the number of bytes in the string.
+ *
+ * @throws mpack_error_type If this node is not a string
+ * @throws mpack_error_too_big If the string does not fit in the given buffer
+ *
+ * @param node The string node from which to copy data
+ * @param buffer A buffer in which to copy the node's bytes
+ * @param bufsize The size of the given buffer
+ *
+ * @return The number of bytes in the node, or zero if an error occurs.
+ */
+size_t mpack_node_copy_utf8(mpack_node_t node, char* buffer, size_t bufsize);
+
+/**
+ * Checks that the given node contains a string with no NUL bytes, copies the string
+ * into the given buffer, and adds a null terminator.
+ *
+ * If this node is not of a string type, @ref mpack_error_type is raised. If the string
+ * does not fit, @ref mpack_error_data is raised.
+ *
+ * If any error occurs, the buffer will contain an empty null-terminated string.
+ *
+ * @param node The string node from which to copy data
+ * @param buffer A buffer in which to copy the node's string
+ * @param size The size of the given buffer
+ */
+void mpack_node_copy_cstr(mpack_node_t node, char* buffer, size_t size);
+
+/**
+ * Checks that the given node contains a valid UTF-8 string with no NUL bytes,
+ * copies the string into the given buffer, and adds a null terminator.
+ *
+ * If this node is not of a string type, @ref mpack_error_type is raised. If the string
+ * does not fit, @ref mpack_error_data is raised.
+ *
+ * If any error occurs, the buffer will contain an empty null-terminated string.
+ *
+ * @param node The string node from which to copy data
+ * @param buffer A buffer in which to copy the node's string
+ * @param size The size of the given buffer
+ */
+void mpack_node_copy_utf8_cstr(mpack_node_t node, char* buffer, size_t size);
+
+#ifdef MPACK_MALLOC
+/**
+ * Allocates a new chunk of data using MPACK_MALLOC() with the bytes
+ * contained by this node.
+ *
+ * The allocated data must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @throws mpack_error_type If this node is not a str, bin or ext type
+ * @throws mpack_error_too_big If the size of the data is larger than the
+ *     given maximum size
+ * @throws mpack_error_memory If an allocation failure occurs
+ *
+ * @param node The node from which to allocate and copy data
+ * @param maxsize The maximum size to allocate
+ *
+ * @return The allocated data, or NULL if any error occurs.
+ */
+char* mpack_node_data_alloc(mpack_node_t node, size_t maxsize);
+
+/**
+ * Allocates a new null-terminated string using MPACK_MALLOC() with the string
+ * contained by this node.
+ *
+ * The allocated string must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @throws mpack_error_type If this node is not a string or contains NUL bytes
+ * @throws mpack_error_too_big If the size of the string plus null-terminator
+ *     is larger than the given maximum size
+ * @throws mpack_error_memory If an allocation failure occurs
+ *
+ * @param node The node from which to allocate and copy string data
+ * @param maxsize The maximum size to allocate, including the null-terminator
+ *
+ * @return The allocated string, or NULL if any error occurs.
+ */
+char* mpack_node_cstr_alloc(mpack_node_t node, size_t maxsize);
+
+/**
+ * Allocates a new null-terminated string using MPACK_MALLOC() with the UTF-8
+ * string contained by this node.
+ *
+ * The allocated string must be freed with MPACK_FREE() (or simply free()
+ * if MPack's allocator hasn't been customized.)
+ *
+ * @throws mpack_error_type If this node is not a string, is not valid UTF-8,
+ *     or contains NUL bytes
+ * @throws mpack_error_too_big If the size of the string plus null-terminator
+ *     is larger than the given maximum size
+ * @throws mpack_error_memory If an allocation failure occurs
+ *
+ * @param node The node from which to allocate and copy string data
+ * @param maxsize The maximum size to allocate, including the null-terminator
+ *
+ * @return The allocated string, or NULL if any error occurs.
+ */
+char* mpack_node_utf8_cstr_alloc(mpack_node_t node, size_t maxsize);
+#endif
+
+/**
+ * Searches the given string array for a string matching the given
+ * node and returns its index.
+ *
+ * If the node does not match any of the given strings,
+ * @ref mpack_error_type is flagged. Use mpack_node_enum_optional()
+ * if you want to allow values other than the given strings.
+ *
+ * If any error occurs or if the tree is in an error state, @a count
+ * is returned.
+ *
+ * This can be used to quickly parse a string into an enum when the
+ * enum values range from 0 to @a count-1. If the last value in the
+ * enum is a special "count" value, it can be passed as the count,
+ * and the return value can be cast directly to the enum type.
+ *
+ * @code{.c}
+ * typedef enum           { APPLE ,  BANANA ,  ORANGE , COUNT} fruit_t;
+ * const char* fruits[] = {"apple", "banana", "orange"};
+ *
+ * fruit_t fruit = (fruit_t)mpack_node_enum(node, fruits, COUNT);
+ * @endcode
+ *
+ * @param node The node
+ * @param strings An array of expected strings of length count
+ * @param count The number of strings
+ * @return The index of the matched string, or @a count in case of error
+ */
+size_t mpack_node_enum(mpack_node_t node, const char* strings[], size_t count);
+
+/**
+ * Searches the given string array for a string matching the given node,
+ * returning its index or @a count if no strings match.
+ *
+ * If the value is not a string, or it does not match any of the
+ * given strings, @a count is returned and no error is flagged.
+ *
+ * If any error occurs or if the tree is in an error state, @a count
+ * is returned.
+ *
+ * This can be used to quickly parse a string into an enum when the
+ * enum values range from 0 to @a count-1. If the last value in the
+ * enum is a special "count" value, it can be passed as the count,
+ * and the return value can be cast directly to the enum type.
+ *
+ * @code{.c}
+ * typedef enum           { APPLE ,  BANANA ,  ORANGE , COUNT} fruit_t;
+ * const char* fruits[] = {"apple", "banana", "orange"};
+ *
+ * fruit_t fruit = (fruit_t)mpack_node_enum_optional(node, fruits, COUNT);
+ * @endcode
+ *
+ * @param node The node
+ * @param strings An array of expected strings of length count
+ * @param count The number of strings
+ * @return The index of the matched string, or @a count in case of error
+ */
+size_t mpack_node_enum_optional(mpack_node_t node, const char* strings[], size_t count);
+
+/**
+ * @}
+ */
+
+/**
+ * @name Compound Node Functions
+ * @{
+ */
+
+/**
+ * Returns the length of the given array node. Raises mpack_error_type
+ * and returns 0 if the given node is not an array.
+ */
+size_t mpack_node_array_length(mpack_node_t node);
+
+/**
+ * Returns the node in the given array at the given index. If the node
+ * is not an array, @ref mpack_error_type is raised and a nil node is returned.
+ * If the given index is out of bounds, @ref mpack_error_data is raised and
+ * a nil node is returned.
+ */
+mpack_node_t mpack_node_array_at(mpack_node_t node, size_t index);
+
+/**
+ * Returns the number of key/value pairs in the given map node. Raises
+ * mpack_error_type and returns 0 if the given node is not a map.
+ */
+size_t mpack_node_map_count(mpack_node_t node);
+
+/**
+ * Returns the key node in the given map at the given index.
+ *
+ * A nil node is returned in case of error.
+ *
+ * @throws mpack_error_type if the node is not a map
+ * @throws mpack_error_data if the given index is out of bounds
+ */
+mpack_node_t mpack_node_map_key_at(mpack_node_t node, size_t index);
+
+/**
+ * Returns the value node in the given map at the given index.
+ *
+ * A nil node is returned in case of error.
+ *
+ * @throws mpack_error_type if the node is not a map
+ * @throws mpack_error_data if the given index is out of bounds
+ */
+mpack_node_t mpack_node_map_value_at(mpack_node_t node, size_t index);
+
+/**
+ * Returns the value node in the given map for the given integer key.
+ *
+ * The key must exist within the map. Use mpack_node_map_int_optional() to
+ * check for optional keys.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node does not contain exactly one entry with the given key
+ *
+ * @return The value node for the given key, or a nil node in case of error
+ */
+mpack_node_t mpack_node_map_int(mpack_node_t node, int64_t num);
+
+/**
+ * Returns the value node in the given map for the given integer key, or a
+ * missing node if the map does not contain the given key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ *
+ * @return The value node for the given key, or a missing node if the key does
+ *         not exist, or a nil node in case of error
+ *
+ * @see mpack_node_is_missing()
+ */
+mpack_node_t mpack_node_map_int_optional(mpack_node_t node, int64_t num);
+
+/**
+ * Returns the value node in the given map for the given unsigned integer key.
+ *
+ * The key must exist within the map. Use mpack_node_map_uint_optional() to
+ * check for optional keys.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node does not contain exactly one entry with the given key
+ *
+ * @return The value node for the given key, or a nil node in case of error
+ */
+mpack_node_t mpack_node_map_uint(mpack_node_t node, uint64_t num);
+
+/**
+ * Returns the value node in the given map for the given unsigned integer
+ * key, or a missing node if the map does not contain the given key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ *
+ * @return The value node for the given key, or a missing node if the key does
+ *         not exist, or a nil node in case of error
+ *
+ * @see mpack_node_is_missing()
+ */
+mpack_node_t mpack_node_map_uint_optional(mpack_node_t node, uint64_t num);
+
+/**
+ * Returns the value node in the given map for the given string key.
+ *
+ * The key must exist within the map. Use mpack_node_map_str_optional() to
+ * check for optional keys.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node does not contain exactly one entry with the given key
+ *
+ * @return The value node for the given key, or a nil node in case of error
+ */
+mpack_node_t mpack_node_map_str(mpack_node_t node, const char* str, size_t length);
+
+/**
+ * Returns the value node in the given map for the given string key, or a missing
+ * node if the map does not contain the given key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ *
+ * @return The value node for the given key, or a missing node if the key does
+ *         not exist, or a nil node in case of error
+ *
+ * @see mpack_node_is_missing()
+ */
+mpack_node_t mpack_node_map_str_optional(mpack_node_t node, const char* str, size_t length);
+
+/**
+ * Returns the value node in the given map for the given null-terminated
+ * string key.
+ *
+ * The key must exist within the map. Use mpack_node_map_cstr_optional() to
+ * check for optional keys.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node does not contain exactly one entry with the given key
+ *
+ * @return The value node for the given key, or a nil node in case of error
+ */
+mpack_node_t mpack_node_map_cstr(mpack_node_t node, const char* cstr);
+
+/**
+ * Returns the value node in the given map for the given null-terminated
+ * string key, or a missing node if the map does not contain the given key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ *
+ * @return The value node for the given key, or a missing node if the key does
+ *         not exist, or a nil node in case of error
+ *
+ * @see mpack_node_is_missing()
+ */
+mpack_node_t mpack_node_map_cstr_optional(mpack_node_t node, const char* cstr);
+
+/**
+ * Returns true if the given node map contains exactly one entry with the
+ * given integer key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ */
+bool mpack_node_map_contains_int(mpack_node_t node, int64_t num);
+
+/**
+ * Returns true if the given node map contains exactly one entry with the
+ * given unsigned integer key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ */
+bool mpack_node_map_contains_uint(mpack_node_t node, uint64_t num);
+
+/**
+ * Returns true if the given node map contains exactly one entry with the
+ * given string key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ */
+bool mpack_node_map_contains_str(mpack_node_t node, const char* str, size_t length);
+
+/**
+ * Returns true if the given node map contains exactly one entry with the
+ * given null-terminated string key.
+ *
+ * The key must be unique. An error is flagged if the node has multiple
+ * entries with the given key.
+ *
+ * @throws mpack_error_type If the node is not a map
+ * @throws mpack_error_data If the node contains more than one entry with the given key
+ */
+bool mpack_node_map_contains_cstr(mpack_node_t node, const char* cstr);
+
+/**
+ * @}
+ */
+
+/**
+ * @}
+ */
+
+#endif
+
+MPACK_EXTERN_C_END
+MPACK_SILENCE_WARNINGS_END
+
+#endif
+
+
+#endif
+
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mutex.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mutex.h
index 0ce3a51..c29509a 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mutex.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/mutex.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/nodiscard.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/nodiscard.h
new file mode 100644
index 0000000..d924036
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/nodiscard.h
@@ -0,0 +1,21 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#ifndef WPIUTIL_WPI_NODISCARD_H_
+#define WPIUTIL_WPI_NODISCARD_H_
+
+// This should only be used on APIs that can be compiled as C or C++. If the API
+// is C++ only, use [[nodiscard]] instead.
+#ifndef WPI_NODISCARD
+#ifdef __cplusplus
+#define WPI_NODISCARD [[nodiscard]]
+#elif defined(__GNUC__) || defined(__llvm__)
+#define WPI_NODISCARD __attribute__((warn_unused_result))
+#elif _MSC_VER
+#define WPI_NODISCARD _Check_return_
+#endif
+
+#endif
+
+#endif  // WPIUTIL_WPI_NODISCARD_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/numbers b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/numbers
new file mode 100644
index 0000000..d45aee0
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/numbers
@@ -0,0 +1,64 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <type_traits>
+
+namespace wpi::numbers {
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T e_v = 2.718281828459045235360287471352662498L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T log2e_v = 1.442695040888963407359924681001892137L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T log10e_v = 0.434294481903251827651128918916605082L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T pi_v = 3.141592653589793238462643383279502884L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T inv_pi_v = 0.318309886183790671537767526745028724L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T inv_sqrtpi_v = 0.564189583547756286948079451560772586L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T ln2_v = 0.693147180559945309417232121458176568L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T ln10_v = 2.302585092994045684017991454684364208L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T sqrt2_v = 1.414213562373095048801688724209698078L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T sqrt3_v = 1.732050807568877293527446341505872366L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T inv_sqrt3_v = 0.577350269189625764509148780501957456L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T egamma_v = 0.577215664901532860606512090082402431L;
+
+template <typename T, typename = std::enable_if_t<std::is_floating_point_v<T>>>
+inline constexpr T phi_v = 1.618033988749894848204586834365638117L;
+
+inline constexpr double e = e_v<double>;
+inline constexpr double log2e = log2e_v<double>;
+inline constexpr double log10e = log10e_v<double>;
+inline constexpr double pi = pi_v<double>;
+inline constexpr double inv_pi = inv_pi_v<double>;
+inline constexpr double inv_sqrtpi = inv_sqrtpi_v<double>;
+inline constexpr double ln2 = ln2_v<double>;
+inline constexpr double ln10 = ln10_v<double>;
+inline constexpr double sqrt2 = sqrt2_v<double>;
+inline constexpr double sqrt3 = sqrt3_v<double>;
+inline constexpr double inv_sqrt3 = inv_sqrt3_v<double>;
+inline constexpr double egamma = egamma_v<double>;
+inline constexpr double phi = phi_v<double>;
+
+}  // namespace wpi::numbers
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/optional.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/optional.h
deleted file mode 100644
index a92f22c..0000000
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/optional.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
-
-#ifndef WPIUTIL_WPI_OPTIONAL_H_
-#define WPIUTIL_WPI_OPTIONAL_H_
-
-// clang-format off
-
-#ifdef _MSC_VER
-#pragma message("warning: wpi/optional.h is deprecated; include <optional> instead")
-#else
-#warning "wpi/optional.h is deprecated; include <optional> instead"
-#endif
-
-#include <optional>
-
-namespace wpi {
-
-template <typename T>
-using optional [[deprecated("use std::optional")]] = std::optional<T>;
-
-using nullopt_t [[deprecated("use std::nullopt_t")]] = std::nullopt_t;
-
-[[deprecated("use std::nullopt")]] inline constexpr std::nullopt_t nullopt =
-    std::nullopt;
-
-}  // namespace wpi
-
-// clang-format on
-
-#endif  // WPIUTIL_WPI_OPTIONAL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_mutex.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_mutex.h
index bb16289..73389ae 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_mutex.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_mutex.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_queue.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_queue.h
index b194090..4610aba 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_queue.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/priority_queue.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_PRIORITY_QUEUE_H_
 #define WPIUTIL_WPI_PRIORITY_QUEUE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_istream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_istream.h
index 7fbbe0b..2371fc6 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_istream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_istream.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_RAW_ISTREAM_H_
 #define WPIUTIL_WPI_RAW_ISTREAM_H_
@@ -13,13 +10,12 @@
 #include <algorithm>
 #include <cstddef>
 #include <string>
+#include <string_view>
 #include <system_error>
 #include <vector>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
-#include "wpi/Twine.h"
+#include "wpi/span.h"
 
 namespace wpi {
 
@@ -50,7 +46,9 @@
 
   size_t readsome(void* data, size_t len) {
     size_t readlen = (std::min)(in_avail(), len);
-    if (readlen == 0) return 0;
+    if (readlen == 0) {
+      return 0;
+    }
     read_impl(data, readlen);
     return m_read_count;
   }
@@ -101,7 +99,7 @@
   // @param buf Buffer for output
   // @param maxLen Maximum length
   // @return Line
-  StringRef getline(SmallVectorImpl<char>& buf, int maxLen);
+  std::string_view getline(SmallVectorImpl<char>& buf, int maxLen);
 
   virtual void close() = 0;
 
@@ -135,9 +133,9 @@
   // not const as we don't want to allow temporaries
   explicit raw_mem_istream(std::string& str)
       : raw_mem_istream(str.data(), str.size()) {}
-  explicit raw_mem_istream(ArrayRef<char> mem)
+  explicit raw_mem_istream(span<const char> mem)
       : raw_mem_istream(mem.data(), mem.size()) {}
-  explicit raw_mem_istream(ArrayRef<uint8_t> mem)
+  explicit raw_mem_istream(span<const uint8_t> mem)
       : raw_mem_istream(reinterpret_cast<const char*>(mem.data()), mem.size()) {
   }
   explicit raw_mem_istream(const char* str)
@@ -155,11 +153,11 @@
 
 class raw_fd_istream : public raw_istream {
  public:
-  raw_fd_istream(const Twine& filename, std::error_code& ec,
+  raw_fd_istream(std::string_view filename, std::error_code& ec,
                  size_t bufSize = 4096);
   raw_fd_istream(int fd, bool shouldClose, size_t bufSize = 4096);
   ~raw_fd_istream() override;
-  void close() override;
+  void close() final;
   size_t in_avail() const override;
 
  private:
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_ostream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_ostream.h
index bb577fe..84ec925 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_ostream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_ostream.h
@@ -14,31 +14,24 @@
 #ifndef WPIUTIL_WPI_RAW_OSTREAM_H
 #define WPIUTIL_WPI_RAW_OSTREAM_H
 
-#include "wpi/ArrayRef.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
+#include "wpi/span.h"
 #include <cassert>
 #include <cstddef>
 #include <cstdint>
 #include <cstring>
 #include <string>
+#include <string_view>
 #include <vector>
 #include <system_error>
 
-namespace wpi {
-
-class format_object_base;
-class FormattedString;
-class FormattedNumber;
-class FormattedBytes;
-
-namespace sys {
 namespace fs {
 enum FileAccess : unsigned;
 enum OpenFlags : unsigned;
 enum CreationDisposition : unsigned;
 } // end namespace fs
-} // end namespace sys
+
+namespace wpi {
 
 /// This class implements an extremely fast bulk output stream that can *only*
 /// output to a stream.  It does not support seeking, reopening, rewinding, line
@@ -166,7 +159,7 @@
     return *this;
   }
 
-  raw_ostream &operator<<(ArrayRef<uint8_t> Arr) {
+  raw_ostream &operator<<(span<const uint8_t> Arr) {
     // Inline fast path, particularly for arrays with a known length.
     size_t Size = Arr.size();
 
@@ -181,7 +174,7 @@
     return *this;
   }
 
-  raw_ostream &operator<<(StringRef Str) {
+  raw_ostream &operator<<(std::string_view Str) {
     // Inline fast path, particularly for strings with a known length.
     size_t Size = Str.size();
 
@@ -200,7 +193,7 @@
     // Inline fast path, particularly for constant strings where a sufficiently
     // smart compiler will simplify strlen.
 
-    return this->operator<<(StringRef(Str));
+    return this->operator<<(std::string_view(Str));
   }
 
   raw_ostream &operator<<(const std::string &Str) {
@@ -221,28 +214,9 @@
     return write(Arr.data(), Arr.size());
   }
 
-  raw_ostream &operator<<(unsigned long N);
-  raw_ostream &operator<<(long N);
-  raw_ostream &operator<<(unsigned long long N);
-  raw_ostream &operator<<(long long N);
-  raw_ostream &operator<<(const void *P);
-
-  raw_ostream &operator<<(unsigned int N) {
-    return this->operator<<(static_cast<unsigned long>(N));
-  }
-
-  raw_ostream &operator<<(int N) {
-    return this->operator<<(static_cast<long>(N));
-  }
-
-  raw_ostream &operator<<(double N);
-
-  /// Output \p N in hexadecimal, without any prefix or padding.
-  raw_ostream &write_hex(unsigned long long N);
-
   /// Output \p Str, turning '\\', '\t', '\n', '"', and anything that doesn't
   /// satisfy wpi::isPrint into an escape sequence.
-  raw_ostream &write_escaped(StringRef Str, bool UseHexEscapes = false);
+  raw_ostream &write_escaped(std::string_view Str, bool UseHexEscapes = false);
 
   raw_ostream &write(unsigned char C);
   raw_ostream &write(const char *Ptr, size_t Size);
@@ -250,18 +224,6 @@
     return write(reinterpret_cast<const char *>(Ptr), Size);
   }
 
-  // Formatted output, see the format() function in Support/Format.h.
-  raw_ostream &operator<<(const format_object_base &Fmt);
-
-  // Formatted output, see the leftJustify() function in Support/Format.h.
-  raw_ostream &operator<<(const FormattedString &);
-
-  // Formatted output, see the formatHex() function in Support/Format.h.
-  raw_ostream &operator<<(const FormattedNumber &);
-
-  // Formatted output, see the format_bytes() function in Support/Format.h.
-  raw_ostream &operator<<(const FormattedBytes &);
-
   /// indent - Insert 'NumSpaces' spaces.
   raw_ostream &indent(unsigned NumSpaces);
 
@@ -429,16 +391,16 @@
   /// As a special case, if Filename is "-", then the stream will use
   /// STDOUT_FILENO instead of opening a file. This will not close the stdout
   /// descriptor.
-  raw_fd_ostream(StringRef Filename, std::error_code &EC);
-  raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                 sys::fs::CreationDisposition Disp);
-  raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                 sys::fs::FileAccess Access);
-  raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                 sys::fs::OpenFlags Flags);
-  raw_fd_ostream(StringRef Filename, std::error_code &EC,
-                 sys::fs::CreationDisposition Disp, sys::fs::FileAccess Access,
-                 sys::fs::OpenFlags Flags);
+  raw_fd_ostream(std::string_view Filename, std::error_code &EC);
+  raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                 fs::CreationDisposition Disp);
+  raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                 fs::FileAccess Access);
+  raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                 fs::OpenFlags Flags);
+  raw_fd_ostream(std::string_view Filename, std::error_code &EC,
+                 fs::CreationDisposition Disp, fs::FileAccess Access,
+                 fs::OpenFlags Flags);
 
   /// FD is the file descriptor that this writes to.  If ShouldClose is true,
   /// this closes the file when the stream is destroyed. If FD is for stdout or
@@ -545,8 +507,8 @@
 
   void flush() = delete;
 
-  /// Return a StringRef for the vector contents.
-  StringRef str() { return StringRef(OS.data(), OS.size()); }
+  /// Return a std::string_view for the vector contents.
+  std::string_view str() { return std::string_view(OS.data(), OS.size()); }
 };
 
 /// A raw_ostream that writes to a vector.  This is a
@@ -578,8 +540,8 @@
 
   void flush() = delete;
 
-  /// Return a StringRef for the vector contents.
-  StringRef str() { return StringRef(OS.data(), OS.size()); }
+  /// Return a std::string_view for the vector contents.
+  std::string_view str() { return std::string_view(OS.data(), OS.size()); }
 };
 
 /// A raw_ostream that writes to an SmallVector or SmallString.  This is a
@@ -611,8 +573,9 @@
 
   void flush() = delete;
 
-  /// Return an ArrayRef for the vector contents.
-  ArrayRef<uint8_t> array() { return ArrayRef<uint8_t>(OS.data(), OS.size()); }
+  /// Return an span for the vector contents.
+  span<uint8_t> array() { return {OS.data(), OS.size()}; }
+  span<const uint8_t> array() const { return {OS.data(), OS.size()}; }
 };
 
 /// A raw_ostream that writes to a vector.  This is a
@@ -644,8 +607,9 @@
 
   void flush() = delete;
 
-  /// Return a StringRef for the vector contents.
-  ArrayRef<uint8_t> array() { return ArrayRef<uint8_t>(OS.data(), OS.size()); }
+  /// Return a span for the vector contents.
+  span<uint8_t> array() { return {OS.data(), OS.size()}; }
+  span<const uint8_t> array() const { return {OS.data(), OS.size()}; }
 };
 
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_istream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_istream.h
index 4ff8022..f6899e2 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_istream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_istream.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_RAW_SOCKET_ISTREAM_H_
 #define WPIUTIL_WPI_RAW_SOCKET_ISTREAM_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_ostream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_ostream.h
index 151af46..5cdeb41 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_ostream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_socket_ostream.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_RAW_SOCKET_OSTREAM_H_
 #define WPIUTIL_WPI_RAW_SOCKET_OSTREAM_H_
@@ -18,7 +15,7 @@
  public:
   raw_socket_ostream(NetworkStream& stream, bool shouldClose)
       : m_stream(stream), m_shouldClose(shouldClose) {}
-  ~raw_socket_ostream();
+  ~raw_socket_ostream() override;
 
   void close();
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_uv_ostream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_uv_ostream.h
index e35b4a8..4773c61 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_uv_ostream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/raw_uv_ostream.h
@@ -1,16 +1,16 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_RAW_UV_OSTREAM_H_
 #define WPIUTIL_WPI_RAW_UV_OSTREAM_H_
 
-#include "wpi/ArrayRef.h"
+#include <functional>
+#include <utility>
+
 #include "wpi/SmallVector.h"
 #include "wpi/raw_ostream.h"
+#include "wpi/span.h"
 #include "wpi/uv/Buffer.h"
 
 namespace wpi {
@@ -29,8 +29,7 @@
    *                  performed using Buffer::Allocate().
    */
   raw_uv_ostream(SmallVectorImpl<uv::Buffer>& bufs, size_t allocSize)
-      : m_bufs(bufs),
-        m_alloc([=]() { return uv::Buffer::Allocate(allocSize); }) {
+      : m_bufs(bufs), m_alloc([=] { return uv::Buffer::Allocate(allocSize); }) {
     SetUnbuffered();
   }
 
@@ -41,19 +40,24 @@
    */
   raw_uv_ostream(SmallVectorImpl<uv::Buffer>& bufs,
                  std::function<uv::Buffer()> alloc)
-      : m_bufs(bufs), m_alloc(alloc) {
+      : m_bufs(bufs), m_alloc(std::move(alloc)) {
     SetUnbuffered();
   }
 
   ~raw_uv_ostream() override = default;
 
   /**
-   * Returns an ArrayRef to the buffers.
+   * Returns an span to the buffers.
    */
-  ArrayRef<uv::Buffer> bufs() { return m_bufs; }
+  span<uv::Buffer> bufs() { return m_bufs; }
 
   void flush() = delete;
 
+  /**
+   * Resets the amount of allocated space.
+   */
+  void reset() { m_left = 0; }
+
  private:
   void write_impl(const char* data, size_t len) override;
   uint64_t current_pos() const override;
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/Sendable.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/Sendable.h
new file mode 100644
index 0000000..d587c3c
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/Sendable.h
@@ -0,0 +1,28 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "wpi/SymbolExports.h"
+
+namespace wpi {
+
+class SendableBuilder;
+
+/**
+ * Interface for Sendable objects.
+ */
+class WPILIB_DLLEXPORT Sendable {
+ public:
+  virtual ~Sendable() = default;
+
+  /**
+   * Initializes this Sendable object.
+   *
+   * @param builder sendable builder
+   */
+  virtual void InitSendable(SendableBuilder& builder) = 0;
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h
new file mode 100644
index 0000000..e092a1e
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableBuilder.h
@@ -0,0 +1,217 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+#include <vector>
+
+#include "wpi/SmallVector.h"
+#include "wpi/span.h"
+
+namespace wpi {
+
+class SendableBuilder {
+ public:
+  /**
+   * The backend kinds used for the sendable builder.
+   */
+  enum BackendKind { kUnknown, kNetworkTables };
+
+  virtual ~SendableBuilder() = default;
+
+  /**
+   * Set the string representation of the named data type that will be used
+   * by the smart dashboard for this sendable.
+   *
+   * @param type    data type
+   */
+  virtual void SetSmartDashboardType(std::string_view type) = 0;
+
+  /**
+   * Set a flag indicating if this sendable should be treated as an actuator.
+   * By default this flag is false.
+   *
+   * @param value   true if actuator, false if not
+   */
+  virtual void SetActuator(bool value) = 0;
+
+  /**
+   * Set the function that should be called to set the Sendable into a safe
+   * state.  This is called when entering and exiting Live Window mode.
+   *
+   * @param func    function
+   */
+  virtual void SetSafeState(std::function<void()> func) = 0;
+
+  /**
+   * Add a boolean property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddBooleanProperty(std::string_view key,
+                                  std::function<bool()> getter,
+                                  std::function<void(bool)> setter) = 0;
+
+  /**
+   * Add a double property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddDoubleProperty(std::string_view key,
+                                 std::function<double()> getter,
+                                 std::function<void(double)> setter) = 0;
+
+  /**
+   * Add a string property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddStringProperty(
+      std::string_view key, std::function<std::string()> getter,
+      std::function<void(std::string_view)> setter) = 0;
+
+  /**
+   * Add a boolean array property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddBooleanArrayProperty(
+      std::string_view key, std::function<std::vector<int>()> getter,
+      std::function<void(wpi::span<const int>)> setter) = 0;
+
+  /**
+   * Add a double array property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddDoubleArrayProperty(
+      std::string_view key, std::function<std::vector<double>()> getter,
+      std::function<void(wpi::span<const double>)> setter) = 0;
+
+  /**
+   * Add a string array property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddStringArrayProperty(
+      std::string_view key, std::function<std::vector<std::string>()> getter,
+      std::function<void(wpi::span<const std::string>)> setter) = 0;
+
+  /**
+   * Add a raw property.
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddRawProperty(std::string_view key,
+                              std::function<std::string()> getter,
+                              std::function<void(std::string_view)> setter) = 0;
+
+  /**
+   * Add a string property (SmallString form).
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddSmallStringProperty(
+      std::string_view key,
+      std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+      std::function<void(std::string_view)> setter) = 0;
+
+  /**
+   * Add a boolean array property (SmallVector form).
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddSmallBooleanArrayProperty(
+      std::string_view key,
+      std::function<wpi::span<const int>(wpi::SmallVectorImpl<int>& buf)>
+          getter,
+      std::function<void(wpi::span<const int>)> setter) = 0;
+
+  /**
+   * Add a double array property (SmallVector form).
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddSmallDoubleArrayProperty(
+      std::string_view key,
+      std::function<wpi::span<const double>(wpi::SmallVectorImpl<double>& buf)>
+          getter,
+      std::function<void(wpi::span<const double>)> setter) = 0;
+
+  /**
+   * Add a string array property (SmallVector form).
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddSmallStringArrayProperty(
+      std::string_view key,
+      std::function<
+          wpi::span<const std::string>(wpi::SmallVectorImpl<std::string>& buf)>
+          getter,
+      std::function<void(wpi::span<const std::string>)> setter) = 0;
+
+  /**
+   * Add a raw property (SmallVector form).
+   *
+   * @param key     property name
+   * @param getter  getter function (returns current value)
+   * @param setter  setter function (sets new value)
+   */
+  virtual void AddSmallRawProperty(
+      std::string_view key,
+      std::function<std::string_view(wpi::SmallVectorImpl<char>& buf)> getter,
+      std::function<void(std::string_view)> setter) = 0;
+
+  /**
+   * Gets the kind of backend being used.
+   *
+   * @return Backend kind
+   */
+  virtual BackendKind GetBackendKind() const = 0;
+
+  /**
+   * Return whether this sendable has been published.
+   *
+   * @return True if it has been published, false if not.
+   */
+  virtual bool IsPublished() const = 0;
+
+  /**
+   * Update the published values by calling the getters for all properties.
+   */
+  virtual void Update() = 0;
+
+  /**
+   * Clear properties.
+   */
+  virtual void ClearProperties() = 0;
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h
new file mode 100644
index 0000000..5b6bbe3
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableHelper.h
@@ -0,0 +1,177 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <memory>
+#include <string>
+#include <string_view>
+
+#include "wpi/deprecated.h"
+#include "wpi/sendable/SendableRegistry.h"
+
+namespace wpi {
+
+/**
+ * A helper class for use with objects that add themselves to SendableRegistry.
+ * It takes care of properly calling Move() and Remove() on move and
+ * destruction.  No action is taken if the object is copied.
+ * Use public inheritance with CRTP when using this class.
+ * @tparam CRTP derived class
+ */
+template <typename Derived>
+class SendableHelper {
+ public:
+  SendableHelper(const SendableHelper& rhs) = default;
+  SendableHelper& operator=(const SendableHelper& rhs) = default;
+
+#if !defined(_MSC_VER) && (defined(__llvm__) || __GNUC__ > 7)
+  // See https://bugzilla.mozilla.org/show_bug.cgi?id=1442819
+  __attribute__((no_sanitize("vptr")))
+#endif
+  SendableHelper(SendableHelper&& rhs) {
+    // it is safe to call Move() multiple times with the same rhs
+    SendableRegistry::Move(static_cast<Derived*>(this),
+                           static_cast<Derived*>(&rhs));
+  }
+
+#if !defined(_MSC_VER) && (defined(__llvm__) || __GNUC__ > 7)
+  // See https://bugzilla.mozilla.org/show_bug.cgi?id=1442819
+  __attribute__((no_sanitize("vptr")))
+#endif
+  SendableHelper&
+  operator=(SendableHelper&& rhs) {
+    // it is safe to call Move() multiple times with the same rhs
+    SendableRegistry::Move(static_cast<Derived*>(this),
+                           static_cast<Derived*>(&rhs));
+    return *this;
+  }
+
+  /**
+   * Gets the name of this Sendable object.
+   *
+   * @deprecated use SendableRegistry::GetName()
+   *
+   * @return Name
+   */
+  WPI_DEPRECATED("use SendableRegistry::GetName()")
+  std::string GetName() const {
+    return SendableRegistry::GetName(static_cast<const Derived*>(this));
+  }
+
+  /**
+   * Sets the name of this Sendable object.
+   *
+   * @deprecated use SendableRegistry::SetName()
+   *
+   * @param name name
+   */
+  WPI_DEPRECATED("use SendableRegistry::SetName()")
+  void SetName(std::string_view name) {
+    SendableRegistry::SetName(static_cast<Derived*>(this), name);
+  }
+
+  /**
+   * Sets both the subsystem name and device name of this Sendable object.
+   *
+   * @deprecated use SendableRegistry::SetName()
+   *
+   * @param subsystem subsystem name
+   * @param name device name
+   */
+  WPI_DEPRECATED("use SendableRegistry::SetName()")
+  void SetName(std::string_view subsystem, std::string_view name) {
+    SendableRegistry::SetName(static_cast<Derived*>(this), subsystem, name);
+  }
+
+  /**
+   * Gets the subsystem name of this Sendable object.
+   *
+   * @deprecated use SendableRegistry::GetSubsystem().
+   *
+   * @return Subsystem name
+   */
+  WPI_DEPRECATED("use SendableRegistry::GetSubsystem()")
+  std::string GetSubsystem() const {
+    return SendableRegistry::GetSubsystem(static_cast<const Derived*>(this));
+  }
+
+  /**
+   * Sets the subsystem name of this Sendable object.
+   *
+   * @deprecated use SendableRegistry::SetSubsystem()
+   *
+   * @param subsystem subsystem name
+   */
+  WPI_DEPRECATED("use SendableRegistry::SetSubsystem()")
+  void SetSubsystem(std::string_view subsystem) {
+    SendableRegistry::SetSubsystem(static_cast<Derived*>(this), subsystem);
+  }
+
+ protected:
+  /**
+   * Add a child component.
+   *
+   * @deprecated use SendableRegistry::AddChild()
+   *
+   * @param child child component
+   */
+  WPI_DEPRECATED("use SendableRegistry::AddChild()")
+  void AddChild(std::shared_ptr<Sendable> child) {
+    SendableRegistry::AddChild(static_cast<Derived*>(this), child.get());
+  }
+
+  /**
+   * Add a child component.
+   *
+   * @deprecated use SendableRegistry::AddChild()
+   *
+   * @param child child component
+   */
+  WPI_DEPRECATED("use SendableRegistry::AddChild()")
+  void AddChild(void* child) {
+    SendableRegistry::AddChild(static_cast<Derived*>(this), child);
+  }
+
+  /**
+   * 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
+   */
+  WPI_DEPRECATED("use SendableRegistry::SetName()")
+  void SetName(std::string_view moduleType, int channel) {
+    SendableRegistry::SetName(static_cast<Derived*>(this), moduleType, channel);
+  }
+
+  /**
+   * 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
+   * @param channel      The channel number the device is plugged into (usually
+   * PWM)
+   */
+  WPI_DEPRECATED("use SendableRegistry::SetName()")
+  void SetName(std::string_view moduleType, int moduleNumber, int channel) {
+    SendableRegistry::SetName(static_cast<Derived*>(this), moduleType,
+                              moduleNumber, channel);
+  }
+
+ protected:
+  SendableHelper() = default;
+
+  ~SendableHelper() {
+    // it is safe to call Remove() multiple times with the same object
+    SendableRegistry::Remove(static_cast<Derived*>(this));
+  }
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableRegistry.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableRegistry.h
new file mode 100644
index 0000000..a08f9e9
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sendable/SendableRegistry.h
@@ -0,0 +1,333 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <functional>
+#include <memory>
+#include <string>
+#include <string_view>
+
+#include "wpi/function_ref.h"
+
+namespace wpi {
+
+class Sendable;
+class SendableBuilder;
+
+/**
+ * The SendableRegistry class is the public interface for registering sensors
+ * and actuators for use on dashboards and LiveWindow.
+ */
+class SendableRegistry {
+ public:
+  SendableRegistry() = delete;
+
+  using UID = size_t;
+
+  /**
+   * Sets the factory for LiveWindow builders.
+   *
+   * @param factory factory function
+   */
+  static void SetLiveWindowBuilderFactory(
+      std::function<std::unique_ptr<SendableBuilder>()> factory);
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param name component name
+   */
+  static void Add(Sendable* sendable, std::string_view name);
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable     object to add
+   * @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
+   */
+  static void Add(Sendable* sendable, std::string_view moduleType, int channel);
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable     object to add
+   * @param moduleType   A string that defines the module name in the label for
+   *                     the value
+   * @param moduleNumber The number of the particular module type
+   * @param channel      The channel number the device is plugged into
+   */
+  static void Add(Sendable* sendable, std::string_view moduleType,
+                  int moduleNumber, int channel);
+
+  /**
+   * Adds an object to the registry.
+   *
+   * @param sendable object to add
+   * @param subsystem subsystem name
+   * @param name component name
+   */
+  static void Add(Sendable* sendable, std::string_view subsystem,
+                  std::string_view name);
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param name component name
+   */
+  static void AddLW(Sendable* sendable, std::string_view name);
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable     object to add
+   * @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
+   */
+  static void AddLW(Sendable* sendable, std::string_view moduleType,
+                    int channel);
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable     object to add
+   * @param moduleType   A string that defines the module name in the label for
+   *                     the value
+   * @param moduleNumber The number of the particular module type
+   * @param channel      The channel number the device is plugged into
+   */
+  static void AddLW(Sendable* sendable, std::string_view moduleType,
+                    int moduleNumber, int channel);
+
+  /**
+   * Adds an object to the registry and LiveWindow.
+   *
+   * @param sendable object to add
+   * @param subsystem subsystem name
+   * @param name component name
+   */
+  static void AddLW(Sendable* sendable, std::string_view subsystem,
+                    std::string_view name);
+
+  /**
+   * Adds a child object to an object.  Adds the child object to the registry
+   * if it's not already present.
+   *
+   * @param parent parent object
+   * @param child child object
+   */
+  static void AddChild(Sendable* parent, Sendable* child);
+
+  /**
+   * Adds a child object to an object.  Adds the child object to the registry
+   * if it's not already present.
+   *
+   * @param parent parent object
+   * @param child child object
+   */
+  static void AddChild(Sendable* parent, void* child);
+
+  /**
+   * Removes an object from the registry.
+   *
+   * @param sendable object to remove
+   * @return true if the object was removed; false if it was not present
+   */
+  static bool Remove(Sendable* sendable);
+
+  /**
+   * Moves an object in the registry (for use in move constructors/assignments).
+   *
+   * @param to new object
+   * @param from old object
+   */
+  static void Move(Sendable* to, Sendable* from);
+
+  /**
+   * Determines if an object is in the registry.
+   *
+   * @param sendable object to check
+   * @return True if in registry, false if not.
+   */
+  static bool Contains(const Sendable* sendable);
+
+  /**
+   * Gets the name of an object.
+   *
+   * @param sendable object
+   * @return Name (empty if object is not in registry)
+   */
+  static std::string GetName(const Sendable* sendable);
+
+  /**
+   * Sets the name of an object.
+   *
+   * @param sendable object
+   * @param name name
+   */
+  static void SetName(Sendable* sendable, std::string_view name);
+
+  /**
+   * Sets the name of an object with a channel number.
+   *
+   * @param sendable   object
+   * @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
+   */
+  static void SetName(Sendable* sendable, std::string_view moduleType,
+                      int channel);
+
+  /**
+   * Sets the name of an object with a module and channel number.
+   *
+   * @param sendable     object
+   * @param moduleType   A string that defines the module name in the label for
+   *                     the value
+   * @param moduleNumber The number of the particular module type
+   * @param channel      The channel number the device is plugged into
+   */
+  static void SetName(Sendable* sendable, std::string_view moduleType,
+                      int moduleNumber, int channel);
+
+  /**
+   * Sets both the subsystem name and device name of an object.
+   *
+   * @param sendable object
+   * @param subsystem subsystem name
+   * @param name device name
+   */
+  static void SetName(Sendable* sendable, std::string_view subsystem,
+                      std::string_view name);
+
+  /**
+   * Gets the subsystem name of an object.
+   *
+   * @param sendable object
+   * @return Subsystem name (empty if object is not in registry)
+   */
+  static std::string GetSubsystem(const Sendable* sendable);
+
+  /**
+   * Sets the subsystem name of an object.
+   *
+   * @param sendable object
+   * @param subsystem subsystem name
+   */
+  static void SetSubsystem(Sendable* sendable, std::string_view subsystem);
+
+  /**
+   * Gets a unique handle for setting/getting data with SetData() and GetData().
+   *
+   * @return Handle
+   */
+  static int GetDataHandle();
+
+  /**
+   * Associates arbitrary data with an object in the registry.
+   *
+   * @param sendable object
+   * @param handle data handle returned by GetDataHandle()
+   * @param data data to set
+   * @return Previous data (may be null)
+   */
+  static std::shared_ptr<void> SetData(Sendable* sendable, int handle,
+                                       std::shared_ptr<void> data);
+
+  /**
+   * Gets arbitrary data associated with an object in the registry.
+   *
+   * @param sendable object
+   * @param handle data handle returned by GetDataHandle()
+   * @return data (may be null if none associated)
+   */
+  static std::shared_ptr<void> GetData(Sendable* sendable, int handle);
+
+  /**
+   * Enables LiveWindow for an object.
+   *
+   * @param sendable object
+   */
+  static void EnableLiveWindow(Sendable* sendable);
+
+  /**
+   * Disables LiveWindow for an object.
+   *
+   * @param sendable object
+   */
+  static void DisableLiveWindow(Sendable* sendable);
+
+  /**
+   * Get unique id for an object.  Since objects can move, use this instead
+   * of storing Sendable* directly if ownership is in question.
+   *
+   * @param sendable object
+   * @return unique id
+   */
+  static UID GetUniqueId(Sendable* sendable);
+
+  /**
+   * Get sendable object for a given unique id.
+   *
+   * @param uid unique id
+   * @return sendable object (may be null)
+   */
+  static Sendable* GetSendable(UID uid);
+
+  /**
+   * Publishes an object in the registry.
+   *
+   * @param sendableUid sendable unique id
+   * @param builder publisher backend
+   */
+  static void Publish(UID sendableUid,
+                      std::unique_ptr<SendableBuilder> builder);
+
+  /**
+   * Updates published information from an object.
+   *
+   * @param sendableUid sendable unique id
+   */
+  static void Update(UID sendableUid);
+
+  /**
+   * Data passed to ForeachLiveWindow() callback function
+   */
+  struct CallbackData {
+    CallbackData(Sendable* sendable_, std::string_view name_,
+                 std::string_view subsystem_, wpi::Sendable* parent_,
+                 std::shared_ptr<void>& data_, SendableBuilder& builder_)
+        : sendable(sendable_),
+          name(name_),
+          subsystem(subsystem_),
+          parent(parent_),
+          data(data_),
+          builder(builder_) {}
+
+    Sendable* sendable;
+    std::string_view name;
+    std::string_view subsystem;
+    Sendable* parent;
+    std::shared_ptr<void>& data;
+    SendableBuilder& builder;
+  };
+
+  /**
+   * Iterates over LiveWindow-enabled objects in the registry.
+   * It is *not* safe to call other SendableRegistry functions from the
+   * callback (this will likely deadlock).
+   *
+   * @param dataHandle data handle to get data pointer passed to callback
+   * @param callback function to call for each object
+   */
+  static void ForeachLiveWindow(
+      int dataHandle, wpi::function_ref<void(CallbackData& cbdata)> callback);
+};
+
+}  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sha1.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sha1.h
index 6a0817f..75464bf 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sha1.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/sha1.h
@@ -23,8 +23,7 @@
 #include <stdint.h>
 
 #include <string>
-
-#include "wpi/StringRef.h"
+#include <string_view>
 
 namespace wpi {
 template <typename T>
@@ -34,12 +33,12 @@
 class SHA1 {
  public:
   SHA1();
-  void Update(StringRef s);
+  void Update(std::string_view s);
   void Update(raw_istream& is);
   std::string Final();
-  StringRef Final(SmallVectorImpl<char>& buf);
-  StringRef RawFinal(SmallVectorImpl<char>& buf);
-  static std::string FromFile(StringRef filename);
+  std::string_view Final(SmallVectorImpl<char>& buf);
+  std::string_view RawFinal(SmallVectorImpl<char>& buf);
+  static std::string FromFile(std::string_view filename);
 
  private:
   uint32_t digest[5];
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/span.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/span.h
new file mode 100644
index 0000000..cf71d79
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/span.h
@@ -0,0 +1,415 @@
+
+/*
+This is an implementation of C++20's std::span
+http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2019/n4820.pdf
+*/
+
+//          Copyright Tristan Brindle 2018.
+// Distributed under the Boost Software License, Version 1.0.
+//    (See accompanying file ../../LICENSE_1_0.txt or copy at
+//          https://www.boost.org/LICENSE_1_0.txt)
+
+#ifndef WPIUTIL_WPI_SPAN_HPP_INCLUDED
+#define WPIUTIL_WPI_SPAN_HPP_INCLUDED
+
+#include <array>
+#include <cassert>
+#include <cstddef>
+#include <cstdint>
+#include <type_traits>
+#if __cplusplus >= 202002L && __has_include(<span>)
+#include <span>
+#endif
+
+namespace wpi {
+
+inline constexpr std::size_t dynamic_extent = SIZE_MAX;
+
+template <typename ElementType, std::size_t Extent = dynamic_extent>
+class span;
+
+namespace detail {
+
+template <typename E, std::size_t S>
+struct span_storage {
+    constexpr span_storage() noexcept = default;
+
+    constexpr span_storage(E* p_ptr, std::size_t /*unused*/) noexcept
+       : ptr(p_ptr)
+    {}
+
+    E* ptr = nullptr;
+    static constexpr std::size_t size = S;
+};
+
+template <typename E>
+struct span_storage<E, dynamic_extent> {
+    constexpr span_storage() noexcept = default;
+
+    constexpr span_storage(E* p_ptr, std::size_t p_size) noexcept
+        : ptr(p_ptr), size(p_size)
+    {}
+
+    E* ptr = nullptr;
+    std::size_t size = 0;
+};
+
+template <typename T>
+using uncvref_t =
+    typename std::remove_cv<typename std::remove_reference<T>::type>::type;
+
+template <typename>
+struct is_span : std::false_type {};
+
+template <typename T, std::size_t S>
+struct is_span<span<T, S>> : std::true_type {};
+
+template <typename>
+struct is_std_array : std::false_type {};
+
+template <typename T, std::size_t N>
+struct is_std_array<std::array<T, N>> : std::true_type {};
+
+template <typename, typename = void>
+struct has_size_and_data : std::false_type {};
+
+template <typename T>
+struct has_size_and_data<T, std::void_t<decltype(std::size(std::declval<T>())),
+                                   decltype(std::data(std::declval<T>()))>>
+    : std::true_type {};
+
+template <typename C, typename U = uncvref_t<C>>
+struct is_container {
+    static constexpr bool value =
+        !is_span<U>::value && !is_std_array<U>::value &&
+        !std::is_array<U>::value && has_size_and_data<C>::value;
+};
+
+template <typename T>
+using remove_pointer_t = typename std::remove_pointer<T>::type;
+
+template <typename, typename, typename = void>
+struct is_container_element_type_compatible : std::false_type {};
+
+template <typename T, typename E>
+struct is_container_element_type_compatible<
+    T, E,
+    typename std::enable_if<
+        !std::is_same<typename std::remove_cv<decltype(
+                          std::data(std::declval<T>()))>::type,
+                      void>::value>::type>
+    : std::is_convertible<
+          remove_pointer_t<decltype(std::data(std::declval<T>()))> (*)[],
+          E (*)[]> {};
+
+template <typename, typename = size_t>
+struct is_complete : std::false_type {};
+
+template <typename T>
+struct is_complete<T, decltype(sizeof(T))> : std::true_type {};
+
+} // namespace detail
+
+template <typename ElementType, std::size_t Extent>
+class span {
+    static_assert(std::is_object<ElementType>::value,
+                  "A span's ElementType must be an object type (not a "
+                  "reference type or void)");
+    static_assert(detail::is_complete<ElementType>::value,
+                  "A span's ElementType must be a complete type (not a forward "
+                  "declaration)");
+    static_assert(!std::is_abstract<ElementType>::value,
+                  "A span's ElementType cannot be an abstract class type");
+
+    using storage_type = detail::span_storage<ElementType, Extent>;
+
+public:
+    // constants and types
+    using element_type = ElementType;
+    using value_type = typename std::remove_cv<ElementType>::type;
+    using size_type = std::size_t;
+    using difference_type = std::ptrdiff_t;
+    using pointer = element_type*;
+    using const_pointer = const element_type*;
+    using reference = element_type&;
+    using const_reference = const element_type&;
+    using iterator = pointer;
+    using reverse_iterator = std::reverse_iterator<iterator>;
+
+    static constexpr size_type extent = Extent;
+
+    // [span.cons], span constructors, copy, assignment, and destructor
+    template <
+        std::size_t E = Extent,
+        typename std::enable_if<(E == dynamic_extent || E <= 0), int>::type = 0>
+    constexpr span() noexcept
+    {}
+
+    constexpr span(pointer ptr, size_type count)
+        : storage_(ptr, count)
+    {
+        assert(extent == dynamic_extent || count == extent);
+    }
+
+    constexpr span(pointer first_elem, pointer last_elem)
+        : storage_(first_elem, last_elem - first_elem)
+    {
+        assert(extent == dynamic_extent ||
+                        last_elem - first_elem ==
+                            static_cast<std::ptrdiff_t>(extent));
+    }
+
+    template <std::size_t N, std::size_t E = Extent,
+              typename std::enable_if<
+                  (E == dynamic_extent || N == E) &&
+                      detail::is_container_element_type_compatible<
+                          element_type (&)[N], ElementType>::value,
+                  int>::type = 0>
+    constexpr span(element_type (&arr)[N]) noexcept : storage_(arr, N)
+    {}
+
+    template <std::size_t N, std::size_t E = Extent,
+              typename std::enable_if<
+                  (E == dynamic_extent || N == E) &&
+                      detail::is_container_element_type_compatible<
+                          std::array<value_type, N>&, ElementType>::value,
+                  int>::type = 0>
+    constexpr span(std::array<value_type, N>& arr) noexcept
+        : storage_(arr.data(), N)
+    {}
+
+    template <std::size_t N, std::size_t E = Extent,
+              typename std::enable_if<
+                  (E == dynamic_extent || N == E) &&
+                      detail::is_container_element_type_compatible<
+                          const std::array<value_type, N>&, ElementType>::value,
+                  int>::type = 0>
+    constexpr span(const std::array<value_type, N>& arr) noexcept
+        : storage_(arr.data(), N)
+    {}
+
+    template <
+        typename Container, std::size_t E = Extent,
+        typename std::enable_if<
+            E == dynamic_extent && detail::is_container<Container>::value &&
+                detail::is_container_element_type_compatible<
+                    Container&, ElementType>::value,
+            int>::type = 0>
+    constexpr span(Container& cont)
+        : storage_(std::data(cont), std::size(cont))
+    {}
+
+    template <
+        typename Container, std::size_t E = Extent,
+        typename std::enable_if<
+            E == dynamic_extent && detail::is_container<Container>::value &&
+                detail::is_container_element_type_compatible<
+                    const Container&, ElementType>::value,
+            int>::type = 0>
+    constexpr span(const Container& cont)
+        : storage_(std::data(cont), std::size(cont))
+    {}
+
+    constexpr span(const span& other) noexcept = default;
+
+    template <typename OtherElementType, std::size_t OtherExtent,
+              typename std::enable_if<
+                  (Extent == OtherExtent || Extent == dynamic_extent) &&
+                      std::is_convertible<OtherElementType (*)[],
+                                          ElementType (*)[]>::value,
+                  int>::type = 0>
+    constexpr span(const span<OtherElementType, OtherExtent>& other) noexcept
+        : storage_(other.data(), other.size())
+    {}
+
+#ifdef __cpp_lib_span
+    constexpr span(std::span<ElementType> other) noexcept
+        : storage_(other.data(), other.size())
+    {}
+#endif
+
+    ~span() noexcept = default;
+
+    constexpr span&
+    operator=(const span& other) noexcept = default;
+
+    // [span.sub], span subviews
+    template <std::size_t Count>
+    constexpr span<element_type, Count> first() const
+    {
+        assert(Count <= size());
+        return {data(), Count};
+    }
+
+    template <std::size_t Count>
+    constexpr span<element_type, Count> last() const
+    {
+        assert(Count <= size());
+        return {data() + (size() - Count), Count};
+    }
+
+    template <std::size_t Offset, std::size_t Count = dynamic_extent>
+    using subspan_return_t =
+        span<ElementType, Count != dynamic_extent
+                              ? Count
+                              : (Extent != dynamic_extent ? Extent - Offset
+                                                          : dynamic_extent)>;
+
+    template <std::size_t Offset, std::size_t Count = dynamic_extent>
+    constexpr subspan_return_t<Offset, Count> subspan() const
+    {
+        assert(Offset <= size() &&
+                        (Count == dynamic_extent || Offset + Count <= size()));
+        return {data() + Offset,
+                Count != dynamic_extent ? Count : size() - Offset};
+    }
+
+    constexpr span<element_type, dynamic_extent>
+    first(size_type count) const
+    {
+        assert(count <= size());
+        return {data(), count};
+    }
+
+    constexpr span<element_type, dynamic_extent>
+    last(size_type count) const
+    {
+        assert(count <= size());
+        return {data() + (size() - count), count};
+    }
+
+    constexpr span<element_type, dynamic_extent>
+    subspan(size_type offset, size_type count = dynamic_extent) const
+    {
+        assert(offset <= size() &&
+                        (count == dynamic_extent || offset + count <= size()));
+        return {data() + offset,
+                count == dynamic_extent ? size() - offset : count};
+    }
+
+    // [span.obs], span observers
+    constexpr size_type size() const noexcept { return storage_.size; }
+
+    constexpr size_type size_bytes() const noexcept
+    {
+        return size() * sizeof(element_type);
+    }
+
+    [[nodiscard]] constexpr bool empty() const noexcept
+    {
+        return size() == 0;
+    }
+
+    // [span.elem], span element access
+    constexpr reference operator[](size_type idx) const
+    {
+        assert(idx < size());
+        return *(data() + idx);
+    }
+
+    constexpr reference front() const
+    {
+        assert(!empty());
+        return *data();
+    }
+
+    constexpr reference back() const
+    {
+        assert(!empty());
+        return *(data() + (size() - 1));
+    }
+
+    constexpr pointer data() const noexcept { return storage_.ptr; }
+
+    // [span.iterators], span iterator support
+    constexpr iterator begin() const noexcept { return data(); }
+
+    constexpr iterator end() const noexcept { return data() + size(); }
+
+    constexpr reverse_iterator rbegin() const noexcept
+    {
+        return reverse_iterator(end());
+    }
+
+    constexpr reverse_iterator rend() const noexcept
+    {
+        return reverse_iterator(begin());
+    }
+
+#ifdef __cpp_lib_span
+    constexpr operator auto() const {
+      return std::span < ElementType,
+             (Extent == dynamic_extent)
+                 ? std::dynamic_extent
+                 : Extent > (storage_.ptr, storage_.size);
+    }
+#endif
+
+private:
+    storage_type storage_{};
+};
+
+/* Deduction Guides */
+template <class T, size_t N>
+span(T (&)[N])->span<T, N>;
+
+template <class T, size_t N>
+span(std::array<T, N>&)->span<T, N>;
+
+template <class T, size_t N>
+span(const std::array<T, N>&)->span<const T, N>;
+
+template <class Container>
+span(Container&)->span<typename Container::value_type>;
+
+template <class Container>
+span(const Container&)->span<const typename Container::value_type>;
+
+template <typename ElementType, std::size_t Extent>
+span<const std::byte, ((Extent == dynamic_extent) ? dynamic_extent
+                                             : sizeof(ElementType) * Extent)>
+as_bytes(span<ElementType, Extent> s) noexcept
+{
+    return {reinterpret_cast<const std::byte*>(s.data()), s.size_bytes()};
+}
+
+template <
+    class ElementType, size_t Extent,
+    typename std::enable_if<!std::is_const<ElementType>::value, int>::type = 0>
+span<std::byte, ((Extent == dynamic_extent) ? dynamic_extent
+                                       : sizeof(ElementType) * Extent)>
+as_writable_bytes(span<ElementType, Extent> s) noexcept
+{
+    return {reinterpret_cast<std::byte*>(s.data()), s.size_bytes()};
+}
+
+template <std::size_t N, typename E, std::size_t S>
+constexpr auto get(span<E, S> s) -> decltype(s[N])
+{
+    return s[N];
+}
+
+} // namespace wpi
+
+namespace std {
+
+template <typename ElementType, size_t Extent>
+class tuple_size<wpi::span<ElementType, Extent>>
+    : public integral_constant<size_t, Extent> {};
+
+template <typename ElementType>
+class tuple_size<wpi::span<
+    ElementType, wpi::dynamic_extent>>; // not defined
+
+template <size_t I, typename ElementType, size_t Extent>
+class tuple_element<I, wpi::span<ElementType, Extent>> {
+public:
+    static_assert(Extent != wpi::dynamic_extent &&
+                      I < Extent,
+                  "");
+    using type = ElementType;
+};
+
+} // end namespace std
+
+#endif // WPIUTIL_WPI_SPAN_HPP_INCLUDED
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/spinlock.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/spinlock.h
index 73137f1..9e777fa 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/spinlock.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/spinlock.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -30,8 +27,11 @@
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE
   void lock() {
-    for (unsigned int i = 1; !try_lock(); ++i)
-      if ((i & 0xff) == 0) std::this_thread::yield();
+    for (unsigned int i = 1; !try_lock(); ++i) {
+      if ((i & 0xff) == 0) {
+        std::this_thread::yield();
+      }
+    }
   }
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE
@@ -58,8 +58,9 @@
                             std::memory_order_release);
     } else {
       if (owner_thread_id.load(std::memory_order_acquire) !=
-          std::this_thread::get_id())
+          std::this_thread::get_id()) {
         return false;
+      }
     }
     ++recursive_counter;
     return true;
@@ -67,8 +68,11 @@
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE
   void lock() {
-    for (unsigned int i = 1; !try_lock(); ++i)
-      if ((i & 0xffff) == 0) std::this_thread::yield();
+    for (unsigned int i = 1; !try_lock(); ++i) {
+      if ((i & 0xffff) == 0) {
+        std::this_thread::yield();
+      }
+    }
   }
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE
@@ -100,7 +104,9 @@
     auto us = std::this_thread::get_id();
     if (!owner_thread_id.compare_exchange_weak(owner, us,
                                                std::memory_order_acquire)) {
-      if (owner != us) return false;
+      if (owner != us) {
+        return false;
+      }
     }
     ++recursive_counter;
     return true;
@@ -108,8 +114,11 @@
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE
   void lock() {
-    for (unsigned int i = 1; !try_lock(); ++i)
-      if ((i & 0xffff) == 0) std::this_thread::yield();
+    for (unsigned int i = 1; !try_lock(); ++i) {
+      if ((i & 0xffff) == 0) {
+        std::this_thread::yield();
+      }
+    }
   }
 
   LLVM_ATTRIBUTE_ALWAYS_INLINE
@@ -118,8 +127,9 @@
            std::this_thread::get_id());
     assert(recursive_counter > 0);
 
-    if (--recursive_counter == 0)
+    if (--recursive_counter == 0) {
       owner_thread_id.store(std::thread::id{}, std::memory_order_release);
+    }
   }
 };
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
index 8329917..8cbf3fe 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/static_circular_buffer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -19,9 +16,7 @@
 template <class T, size_t N>
 class static_circular_buffer {
  public:
-  static_assert(N > 0, "The circular buffer size shouldn't be zero.");
-
-  constexpr static_circular_buffer() = default;
+  static_assert(N > 0, "Circular buffer size cannot be zero.");
 
   class iterator {
    public:
@@ -87,17 +82,38 @@
     size_t m_index;
   };
 
+  /**
+   * Returns begin iterator.
+   */
   iterator begin() { return iterator(this, 0); }
+
+  /**
+   * Returns end iterator.
+   */
   iterator end() {
     return iterator(this, ::wpi::static_circular_buffer<T, N>::size());
   }
 
+  /**
+   * Returns begin iterator.
+   */
   const_iterator begin() const { return const_iterator(this, 0); }
+
+  /**
+   * Returns end iterator.
+   */
   const_iterator end() const {
     return const_iterator(this, ::wpi::static_circular_buffer<T, N>::size());
   }
 
+  /**
+   * Returns begin iterator.
+   */
   const_iterator cbegin() const { return const_iterator(this, 0); }
+
+  /**
+   * Returns end iterator.
+   */
   const_iterator cend() const {
     return const_iterator(this, ::wpi::static_circular_buffer<T, N>::size());
   }
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h
index aefcd56..789e125 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/timestamp.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_TIMESTAMP_H_
 #define WPIUTIL_WPI_TIMESTAMP_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Async.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Async.h
index 2da870d..49f3dde 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Async.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Async.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_ASYNC_H_
 #define WPIUTIL_WPI_UV_ASYNC_H_
@@ -21,8 +18,7 @@
 #include "wpi/uv/Handle.h"
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 /**
  * Async handle.
@@ -43,10 +39,11 @@
   Async(const std::shared_ptr<Loop>& loop, const private_init&)
       : m_loop{loop} {}
   ~Async() noexcept override {
-    if (auto loop = m_loop.lock())
+    if (auto loop = m_loop.lock()) {
       this->Close();
-    else
+    } else {
       this->ForceClosed();
+    }
   }
 
   /**
@@ -69,7 +66,9 @@
         uv_async_init(loop->GetRaw(), h->GetRaw(), [](uv_async_t* handle) {
           auto& h = *static_cast<Async*>(handle->data);
           std::scoped_lock lock(h.m_mutex);
-          for (auto&& v : h.m_data) std::apply(h.wakeup, v);
+          for (auto&& v : h.m_data) {
+            std::apply(h.wakeup, v);
+          }
           h.m_data.clear();
         });
     if (err < 0) {
@@ -99,7 +98,9 @@
       std::scoped_lock lock(m_mutex);
       m_data.emplace_back(std::forward_as_tuple(std::forward<U>(u)...));
     }
-    if (loop) this->Invoke(&uv_async_send, this->GetRaw());
+    if (loop) {
+      this->Invoke(&uv_async_send, this->GetRaw());
+    }
   }
 
   /**
@@ -149,7 +150,14 @@
    * An async event will be emitted on the loop thread.
    */
   void Send() {
-    if (auto loop = m_loop.lock()) Invoke(&uv_async_send, GetRaw());
+    if (auto loop = m_loop.lock()) {
+      if (loop->GetThreadId() == std::this_thread::get_id()) {
+        // called from within the loop, just call the function directly
+        wakeup();
+      } else {
+        Invoke(&uv_async_send, GetRaw());
+      }
+    }
   }
 
   /**
@@ -161,7 +169,6 @@
   std::weak_ptr<Loop> m_loop;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_ASYNC_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/AsyncFunction.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/AsyncFunction.h
index 4510b84..fa4eb90 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/AsyncFunction.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/AsyncFunction.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_ASYNCFUNCTION_H_
 #define WPIUTIL_WPI_UV_ASYNCFUNCTION_H_
@@ -23,8 +20,7 @@
 #include "wpi/uv/Handle.h"
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 template <typename T>
 class AsyncFunction;
@@ -42,12 +38,13 @@
  public:
   AsyncFunction(const std::shared_ptr<Loop>& loop,
                 std::function<void(promise<R>, T...)> func, const private_init&)
-      : wakeup{func}, m_loop{loop} {}
+      : wakeup{std::move(func)}, m_loop{loop} {}
   ~AsyncFunction() noexcept override {
-    if (auto loop = m_loop.lock())
+    if (auto loop = m_loop.lock()) {
       this->Close();
-    else
+    } else {
       this->ForceClosed();
+    }
   }
 
   /**
@@ -89,10 +86,11 @@
             // waiting for it
             for (auto&& v : h.m_params) {
               auto p = h.m_promises.CreatePromise(v.first);
-              if (h.wakeup)
+              if (h.wakeup) {
                 std::apply(h.wakeup,
                            std::tuple_cat(std::make_tuple(std::move(p)),
                                           std::move(v.second)));
+              }
             }
             h.m_params.clear();
             // wake up any threads that might be waiting for the result
@@ -139,7 +137,9 @@
     }
 
     // signal the loop
-    if (loop) this->Invoke(&uv_async_send, this->GetRaw());
+    if (loop) {
+      this->Invoke(&uv_async_send, this->GetRaw());
+    }
 
     // return future
     return m_promises.CreateFuture(req);
@@ -162,7 +162,6 @@
   std::weak_ptr<Loop> m_loop;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_ASYNCFUNCTION_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Buffer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Buffer.h
index 04e96b9..5126841 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Buffer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Buffer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_BUFFER_H_
 #define WPIUTIL_WPI_UV_BUFFER_H_
@@ -12,14 +9,13 @@
 
 #include <cstring>
 #include <initializer_list>
+#include <string_view>
 #include <utility>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/SmallVector.h"
-#include "wpi/StringRef.h"
+#include "wpi/span.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 /**
  * Data buffer.  Convenience wrapper around uv_buf_t.
@@ -30,13 +26,13 @@
     base = nullptr;
     len = 0;
   }
-  /*implicit*/ Buffer(const uv_buf_t& oth) {  // NOLINT(runtime/explicit)
+  /*implicit*/ Buffer(const uv_buf_t& oth) {  // NOLINT
     base = oth.base;
     len = oth.len;
   }
-  /*implicit*/ Buffer(StringRef str)  // NOLINT(runtime/explicit)
+  /*implicit*/ Buffer(std::string_view str)  // NOLINT
       : Buffer{str.data(), str.size()} {}
-  /*implicit*/ Buffer(ArrayRef<uint8_t> arr)  // NOLINT(runtime/explicit)
+  /*implicit*/ Buffer(span<const uint8_t> arr)  // NOLINT
       : Buffer{reinterpret_cast<const char*>(arr.data()), arr.size()} {}
   Buffer(char* base_, size_t len_) {
     base = base_;
@@ -47,21 +43,21 @@
     len = static_cast<decltype(len)>(len_);
   }
 
-  ArrayRef<char> data() const { return ArrayRef<char>{base, len}; }
-  MutableArrayRef<char> data() { return MutableArrayRef<char>{base, len}; }
+  span<const char> data() const { return {base, len}; }
+  span<char> data() { return {base, len}; }
 
-  operator ArrayRef<char>() const { return data(); }
-  operator MutableArrayRef<char>() { return data(); }
+  operator span<const char>() const { return data(); }  // NOLINT
+  operator span<char>() { return data(); }              // NOLINT
 
   static Buffer Allocate(size_t size) { return Buffer{new char[size], size}; }
 
-  static Buffer Dup(StringRef in) {
+  static Buffer Dup(std::string_view in) {
     Buffer buf = Allocate(in.size());
     std::memcpy(buf.base, in.data(), in.size());
     return buf;
   }
 
-  static Buffer Dup(ArrayRef<uint8_t> in) {
+  static Buffer Dup(span<const uint8_t> in) {
     Buffer buf = Allocate(in.size());
     std::memcpy(buf.base, in.begin(), in.size());
     return buf;
@@ -116,7 +112,9 @@
    * Allocate a buffer.
    */
   Buffer Allocate() {
-    if (m_pool.empty()) return Buffer::Allocate(m_size);
+    if (m_pool.empty()) {
+      return Buffer::Allocate(m_size);
+    }
     auto buf = m_pool.back();
     m_pool.pop_back();
     buf.len = m_size;
@@ -133,15 +131,19 @@
    * This is NOT safe to use with arbitrary buffers unless they were
    * allocated with the same size as the buffer pool allocation size.
    */
-  void Release(MutableArrayRef<Buffer> bufs) {
-    for (auto& buf : bufs) m_pool.emplace_back(buf.Move());
+  void Release(span<Buffer> bufs) {
+    for (auto& buf : bufs) {
+      m_pool.emplace_back(buf.Move());
+    }
   }
 
   /**
    * Clear the pool, releasing all buffers.
    */
   void Clear() {
-    for (auto& buf : m_pool) buf.Deallocate();
+    for (auto& buf : m_pool) {
+      buf.Deallocate();
+    }
     m_pool.clear();
   }
 
@@ -153,10 +155,9 @@
 
  private:
   SmallVector<Buffer, DEPTH> m_pool;
-  size_t m_size;
+  size_t m_size;  // NOLINT
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_BUFFER_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Check.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Check.h
index e64a9f0..7555452 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Check.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Check.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_CHECK_H_
 #define WPIUTIL_WPI_UV_CHECK_H_
@@ -15,8 +12,7 @@
 #include "wpi/Signal.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -64,7 +60,6 @@
   sig::Signal<> check;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_CHECK_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Error.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Error.h
index 07a0ab8..1e717a4 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Error.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Error.h
@@ -1,24 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_ERROR_H_
 #define WPIUTIL_WPI_UV_ERROR_H_
 
 #include <uv.h>
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 /**
  * Error code.
  */
 class Error {
  public:
-  Error() : m_err(UV_UNKNOWN) {}
+  Error() = default;
   explicit Error(int err) : m_err(err) {}
 
   /**
@@ -42,10 +38,9 @@
   const char* name() const { return uv_err_name(m_err); }
 
  private:
-  int m_err;
+  int m_err{UV_UNKNOWN};
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_ERROR_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/FsEvent.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/FsEvent.h
index cf91848..0ed2dcb 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/FsEvent.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/FsEvent.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_FSEVENT_H_
 #define WPIUTIL_WPI_UV_FSEVENT_H_
@@ -12,13 +9,12 @@
 
 #include <memory>
 #include <string>
+#include <string_view>
 
 #include "wpi/Signal.h"
-#include "wpi/Twine.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -55,7 +51,7 @@
    * @param events Bitmask of event flags.  Only UV_FS_EVENT_RECURSIVE is
    *               supported (and only on OSX and Windows).
    */
-  void Start(const Twine& path, unsigned int flags = 0);
+  void Start(std::string_view path, unsigned int flags = 0);
 
   /**
    * Stop watching for changes.
@@ -78,7 +74,6 @@
   sig::Signal<const char*, int> fsEvent;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_FSEVENT_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetAddrInfo.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetAddrInfo.h
index deb4a24..18d4f32 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetAddrInfo.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetAddrInfo.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_GETADDRINFO_H_
 #define WPIUTIL_WPI_UV_GETADDRINFO_H_
@@ -12,13 +9,13 @@
 
 #include <functional>
 #include <memory>
+#include <string_view>
+#include <utility>
 
 #include "wpi/Signal.h"
-#include "wpi/Twine.h"
 #include "wpi/uv/Request.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -44,7 +41,7 @@
  * request when the resolution completes.  HandleError() is called on the
  * request if any errors occur.
  *
- * Either node or service may be null (`Twine::createNull()`) but not both.
+ * Either node or service may be empty but not both.
  *
  * @param loop Event loop
  * @param req request
@@ -54,7 +51,7 @@
  *              type constraints.
  */
 void GetAddrInfo(Loop& loop, const std::shared_ptr<GetAddrInfoReq>& req,
-                 const Twine& node, const Twine& service = Twine::createNull(),
+                 std::string_view node, std::string_view service = {},
                  const addrinfo* hints = nullptr);
 
 /**
@@ -62,7 +59,7 @@
  * request when the resolution completes.  HandleError() is called on the
  * request if any errors occur.
  *
- * Either node or service may be null (`Twine::createNull()`) but not both.
+ * Either node or service may be empty but not both.
  *
  * @param loop Event loop
  * @param req request
@@ -73,8 +70,7 @@
  */
 inline void GetAddrInfo(const std::shared_ptr<Loop>& loop,
                         const std::shared_ptr<GetAddrInfoReq>& req,
-                        const Twine& node,
-                        const Twine& service = Twine::createNull(),
+                        std::string_view node, std::string_view service = {},
                         const addrinfo* hints = nullptr) {
   GetAddrInfo(*loop, req, node, service, hints);
 }
@@ -84,7 +80,7 @@
  * completes, and errors are forwarded to the loop.  This is a convenience
  * wrapper.
  *
- * Either node or service may be null (`Twine::createNull()`) but not both.
+ * Either node or service may be empty but not both.
  *
  * @param loop Event loop
  * @param callback Callback function to call when resolution completes
@@ -94,7 +90,7 @@
  *              type constraints.
  */
 void GetAddrInfo(Loop& loop, std::function<void(const addrinfo&)> callback,
-                 const Twine& node, const Twine& service = Twine::createNull(),
+                 std::string_view node, std::string_view service = {},
                  const addrinfo* hints = nullptr);
 
 /**
@@ -102,7 +98,7 @@
  * completes, and errors are forwarded to the loop.  This is a convenience
  * wrapper.
  *
- * Either node or service may be null (`Twine::createNull()`) but not both.
+ * Either node or service may be empty but not both.
  *
  * @param loop Event loop
  * @param callback Callback function to call when resolution completes
@@ -113,13 +109,11 @@
  */
 inline void GetAddrInfo(const std::shared_ptr<Loop>& loop,
                         std::function<void(const addrinfo&)> callback,
-                        const Twine& node,
-                        const Twine& service = Twine::createNull(),
+                        std::string_view node, std::string_view service = {},
                         const addrinfo* hints = nullptr) {
-  GetAddrInfo(*loop, callback, node, service, hints);
+  GetAddrInfo(*loop, std::move(callback), node, service, hints);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_GETADDRINFO_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetNameInfo.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetNameInfo.h
index d6f4d62..d35dd95 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetNameInfo.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/GetNameInfo.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_GETNAMEINFO_H_
 #define WPIUTIL_WPI_UV_GETNAMEINFO_H_
@@ -12,13 +9,13 @@
 
 #include <functional>
 #include <memory>
+#include <string_view>
+#include <utility>
 
 #include "wpi/Signal.h"
-#include "wpi/Twine.h"
 #include "wpi/uv/Request.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -94,7 +91,7 @@
 inline void GetNameInfo(const std::shared_ptr<Loop>& loop,
                         std::function<void(const char*, const char*)> callback,
                         const sockaddr& addr, int flags = 0) {
-  GetNameInfo(*loop, callback, addr, flags);
+  GetNameInfo(*loop, std::move(callback), addr, flags);
 }
 
 /**
@@ -109,7 +106,7 @@
  * @param flags Optional flags to modify the behavior of `getnameinfo`.
  */
 void GetNameInfo4(Loop& loop, const std::shared_ptr<GetNameInfoReq>& req,
-                  const Twine& ip, unsigned int port, int flags = 0);
+                  std::string_view ip, unsigned int port, int flags = 0);
 
 /**
  * Asynchronous IPv4 getnameinfo(3).  HandleResolvedName() is called on the
@@ -124,7 +121,8 @@
  */
 inline void GetNameInfo4(const std::shared_ptr<Loop>& loop,
                          const std::shared_ptr<GetNameInfoReq>& req,
-                         const Twine& ip, unsigned int port, int flags = 0) {
+                         std::string_view ip, unsigned int port,
+                         int flags = 0) {
   return GetNameInfo4(*loop, req, ip, port, flags);
 }
 
@@ -140,7 +138,7 @@
  */
 void GetNameInfo4(Loop& loop,
                   std::function<void(const char*, const char*)> callback,
-                  const Twine& ip, unsigned int port, int flags = 0);
+                  std::string_view ip, unsigned int port, int flags = 0);
 
 /**
  * Asynchronous IPv4 getnameinfo(3).  The callback is called when the resolution
@@ -155,8 +153,9 @@
  */
 inline void GetNameInfo4(const std::shared_ptr<Loop>& loop,
                          std::function<void(const char*, const char*)> callback,
-                         const Twine& ip, unsigned int port, int flags = 0) {
-  return GetNameInfo4(*loop, callback, ip, port, flags);
+                         std::string_view ip, unsigned int port,
+                         int flags = 0) {
+  return GetNameInfo4(*loop, std::move(callback), ip, port, flags);
 }
 
 /**
@@ -171,7 +170,7 @@
  * @param flags Optional flags to modify the behavior of `getnameinfo`.
  */
 void GetNameInfo6(Loop& loop, const std::shared_ptr<GetNameInfoReq>& req,
-                  const Twine& ip, unsigned int port, int flags = 0);
+                  std::string_view ip, unsigned int port, int flags = 0);
 
 /**
  * Asynchronous IPv6 getnameinfo(3).  HandleResolvedName() is called on the
@@ -186,7 +185,8 @@
  */
 inline void GetNameInfo6(const std::shared_ptr<Loop>& loop,
                          const std::shared_ptr<GetNameInfoReq>& req,
-                         const Twine& ip, unsigned int port, int flags = 0) {
+                         std::string_view ip, unsigned int port,
+                         int flags = 0) {
   GetNameInfo6(*loop, req, ip, port, flags);
 }
 
@@ -203,7 +203,7 @@
  */
 void GetNameInfo6(Loop& loop,
                   std::function<void(const char*, const char*)> callback,
-                  const Twine& ip, unsigned int port, int flags = 0);
+                  std::string_view ip, unsigned int port, int flags = 0);
 
 /**
  * Asynchronous IPv6 getnameinfo(3).  The callback is called when the resolution
@@ -218,11 +218,11 @@
  */
 inline void GetNameInfo6(const std::shared_ptr<Loop>& loop,
                          std::function<void(const char*, const char*)> callback,
-                         const Twine& ip, unsigned int port, int flags = 0) {
-  return GetNameInfo6(*loop, callback, ip, port, flags);
+                         std::string_view ip, unsigned int port,
+                         int flags = 0) {
+  return GetNameInfo6(*loop, std::move(callback), ip, port, flags);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_GETNAMEINFO_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Handle.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Handle.h
index 9d91d37..b61cd81 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Handle.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Handle.h
@@ -1,27 +1,24 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_HANDLE_H_
 #define WPIUTIL_WPI_UV_HANDLE_H_
 
 #include <uv.h>
 
+#include <cstdlib>
 #include <functional>
 #include <memory>
+#include <string_view>
 #include <utility>
 
 #include "wpi/Signal.h"
-#include "wpi/StringRef.h"
 #include "wpi/uv/Buffer.h"
 #include "wpi/uv/Error.h"
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 /**
  * Handle.
@@ -53,7 +50,7 @@
   /**
    * Get the name of the type of the handle.  E.g. "pipe" for pipe handles.
    */
-  StringRef GetTypeName() const noexcept {
+  std::string_view GetTypeName() const noexcept {
     return uv_handle_type_name(m_uv_handle->type);
   }
 
@@ -194,8 +191,8 @@
    */
   void SetBufferAllocator(std::function<Buffer(size_t)> alloc,
                           std::function<void(Buffer&)> dealloc) {
-    m_allocBuf = alloc;
-    m_freeBuf = dealloc;
+    m_allocBuf = std::move(alloc);
+    m_freeBuf = std::move(dealloc);
   }
 
   /**
@@ -252,7 +249,9 @@
   template <typename F, typename... Args>
   bool Invoke(F&& f, Args&&... args) const {
     auto err = std::forward<F>(f)(std::forward<Args>(args)...);
-    if (err < 0) ReportError(err);
+    if (err < 0) {
+      ReportError(err);
+    }
     return err == 0;
   }
 
@@ -290,10 +289,9 @@
   }
 
  protected:
-  HandleImpl() : Handle{reinterpret_cast<uv_handle_t*>(new U)} {}
+  HandleImpl() : Handle{static_cast<uv_handle_t*>(std::malloc(sizeof(U)))} {}
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_HANDLE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Idle.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Idle.h
index e8278b2..869d43d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Idle.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Idle.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_IDLE_H_
 #define WPIUTIL_WPI_UV_IDLE_H_
@@ -15,8 +12,7 @@
 #include "wpi/Signal.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -73,7 +69,6 @@
   sig::Signal<> idle;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_IDLE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Loop.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Loop.h
index 2b3b27f..9693053 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Loop.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Loop.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_LOOP_H_
 #define WPIUTIL_WPI_UV_LOOP_H_
@@ -18,10 +15,10 @@
 #include <utility>
 
 #include "wpi/Signal.h"
+#include "wpi/function_ref.h"
 #include "wpi/uv/Error.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Handle;
 
@@ -175,7 +172,7 @@
    *
    * @param callback A function to be invoked once for each active handle.
    */
-  void Walk(std::function<void(Handle&)> callback);
+  void Walk(function_ref<void(Handle&)> callback);
 
   /**
    * Reinitialize any kernel state necessary in the child process after
@@ -251,7 +248,6 @@
   std::atomic<std::thread::id> m_tid;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_LOOP_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/NetworkStream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/NetworkStream.h
index 05c1c92..faac9fe 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/NetworkStream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/NetworkStream.h
@@ -1,23 +1,20 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_NETWORKSTREAM_H_
 #define WPIUTIL_WPI_UV_NETWORKSTREAM_H_
 
 #include <uv.h>
 
+#include <cstdlib>
 #include <functional>
 #include <memory>
 
 #include "wpi/Signal.h"
 #include "wpi/uv/Stream.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class NetworkStream;
 
@@ -147,10 +144,10 @@
   }
 
  protected:
-  NetworkStreamImpl() : NetworkStream{reinterpret_cast<uv_stream_t*>(new U)} {}
+  NetworkStreamImpl()
+      : NetworkStream{static_cast<uv_stream_t*>(std::malloc(sizeof(U)))} {}
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_NETWORKSTREAM_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Pipe.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Pipe.h
index 72fc23b..e223268 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Pipe.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Pipe.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_PIPE_H_
 #define WPIUTIL_WPI_UV_PIPE_H_
@@ -13,12 +10,11 @@
 #include <functional>
 #include <memory>
 #include <string>
+#include <string_view>
 
-#include "wpi/Twine.h"
 #include "wpi/uv/NetworkStream.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 class PipeConnectReq;
@@ -123,7 +119,7 @@
    *
    * @param name File path (Unix) or name (Windows).
    */
-  void Bind(const Twine& name);
+  void Bind(std::string_view name);
 
   /**
    * Connect to the Unix domain socket or the named pipe.
@@ -139,7 +135,8 @@
    * @param name File path (Unix) or name (Windows).
    * @param req connection request
    */
-  void Connect(const Twine& name, const std::shared_ptr<PipeConnectReq>& req);
+  void Connect(std::string_view name,
+               const std::shared_ptr<PipeConnectReq>& req);
 
   /**
    * Connect to the Unix domain socket or the named pipe.
@@ -153,7 +150,7 @@
    * @param name File path (Unix) or name (Windows).
    * @param callback Callback function to call when connection established
    */
-  void Connect(const Twine& name, std::function<void()> callback);
+  void Connect(std::string_view name, std::function<void()> callback);
 
   /**
    * Get the name of the Unix domain socket or the named pipe.
@@ -206,7 +203,6 @@
   }
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_PIPE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Poll.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Poll.h
index fad00e4..6e1e26d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Poll.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Poll.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_POLL_H_
 #define WPIUTIL_WPI_UV_POLL_H_
@@ -15,8 +12,7 @@
 #include "wpi/Signal.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -120,7 +116,6 @@
   std::unique_ptr<ReuseData> m_reuseData;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_POLL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Prepare.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Prepare.h
index 600922f..88ae2d9 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Prepare.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Prepare.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_PREPARE_H_
 #define WPIUTIL_WPI_UV_PREPARE_H_
@@ -15,8 +12,7 @@
 #include "wpi/Signal.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -64,7 +60,6 @@
   sig::Signal<> prepare;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_PREPARE_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Process.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Process.h
index 47d09df..fc2315e 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Process.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Process.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_PROCESS_H_
 #define WPIUTIL_WPI_UV_PROCESS_H_
@@ -13,15 +10,14 @@
 #include <initializer_list>
 #include <memory>
 #include <string>
+#include <string_view>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/Signal.h"
 #include "wpi/SmallVector.h"
-#include "wpi/Twine.h"
+#include "wpi/span.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 class Pipe;
@@ -39,9 +35,9 @@
   ~Process() noexcept override = default;
 
   /**
-   * Structure for Spawn() option temporaries.  This is a reference type
-   * similar to StringRef, so if this value is stored outside of a temporary,
-   * be careful about overwriting what it points to.
+   * Structure for Spawn() option temporaries.  This is a reference type, so if
+   * this value is stored outside of a temporary, be careful about overwriting
+   * what it points to.
    */
   struct Option {
     enum Type {
@@ -61,30 +57,24 @@
 
     Option() : m_type(kNone) {}
 
-    /*implicit*/ Option(const char* arg) {  // NOLINT(runtime/explicit)
+    /*implicit*/ Option(const char* arg) {  // NOLINT
       m_data.str = arg;
     }
 
-    /*implicit*/ Option(const std::string& arg) {  // NOLINT(runtime/explicit)
+    /*implicit*/ Option(const std::string& arg) {  // NOLINT
       m_data.str = arg.data();
     }
 
-    /*implicit*/ Option(StringRef arg)  // NOLINT(runtime/explicit)
+    /*implicit*/ Option(std::string_view arg)  // NOLINT
         : m_strData(arg) {
       m_data.str = m_strData.c_str();
     }
 
-    /*implicit*/ Option(
-        const SmallVectorImpl<char>& arg)  // NOLINT(runtime/explicit)
+    /*implicit*/ Option(const SmallVectorImpl<char>& arg)  // NOLINT
         : m_strData(arg.data(), arg.size()) {
       m_data.str = m_strData.c_str();
     }
 
-    /*implicit*/ Option(const Twine& arg)  // NOLINT(runtime/explicit)
-        : m_strData(arg.str()) {
-      m_data.str = m_strData.c_str();
-    }
-
     explicit Option(Type type) : m_type(type) {}
 
     Type m_type = kArg;
@@ -110,9 +100,9 @@
    * environment is used.
    * @param env environment variable
    */
-  static Option Env(const Twine& env) {
+  static Option Env(std::string_view env) {
     Option o(Option::kEnv);
-    o.m_strData = env.str();
+    o.m_strData = env;
     o.m_data.str = o.m_strData.c_str();
     return o;
   }
@@ -121,9 +111,9 @@
    * Set the current working directory for the subprocess.
    * @param cwd current working directory
    */
-  static Option Cwd(const Twine& cwd) {
+  static Option Cwd(std::string_view cwd) {
     Option o(Option::kCwd);
-    o.m_strData = cwd.str();
+    o.m_strData = cwd;
     o.m_data.str = o.m_strData.c_str();
     return o;
   }
@@ -241,16 +231,17 @@
    * @param file Path pointing to the program to be executed
    * @param options Process options
    */
-  static std::shared_ptr<Process> SpawnArray(Loop& loop, const Twine& file,
-                                             ArrayRef<Option> options);
+  static std::shared_ptr<Process> SpawnArray(Loop& loop, std::string_view file,
+                                             span<const Option> options);
 
   static std::shared_ptr<Process> SpawnArray(
-      Loop& loop, const Twine& file, std::initializer_list<Option> options) {
-    return SpawnArray(loop, file, makeArrayRef(options.begin(), options.end()));
+      Loop& loop, std::string_view file,
+      std::initializer_list<Option> options) {
+    return SpawnArray(loop, file, {options.begin(), options.end()});
   }
 
   template <typename... Args>
-  static std::shared_ptr<Process> Spawn(Loop& loop, const Twine& file,
+  static std::shared_ptr<Process> Spawn(Loop& loop, std::string_view file,
                                         const Args&... options) {
     return SpawnArray(loop, file, {options...});
   }
@@ -269,20 +260,20 @@
    * @param options Process options
    */
   static std::shared_ptr<Process> SpawnArray(const std::shared_ptr<Loop>& loop,
-                                             const Twine& file,
-                                             ArrayRef<Option> options) {
+                                             std::string_view file,
+                                             span<const Option> options) {
     return SpawnArray(*loop, file, options);
   }
 
   static std::shared_ptr<Process> SpawnArray(
-      const std::shared_ptr<Loop>& loop, const Twine& file,
+      const std::shared_ptr<Loop>& loop, std::string_view file,
       std::initializer_list<Option> options) {
     return SpawnArray(*loop, file, options);
   }
 
   template <typename... Args>
   static std::shared_ptr<Process> Spawn(const std::shared_ptr<Loop>& loop,
-                                        const Twine& file,
+                                        std::string_view file,
                                         const Args&... options) {
     return SpawnArray(*loop, file, {options...});
   }
@@ -314,7 +305,6 @@
   sig::Signal<int64_t, int> exited;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_PROCESS_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Request.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Request.h
index c33ca83..3f6a19d 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Request.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Request.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_REQUEST_H_
 #define WPIUTIL_WPI_UV_REQUEST_H_
@@ -15,8 +12,7 @@
 
 #include "wpi/uv/Error.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 /**
  * Request.  Requests are not moveable or copyable.
@@ -165,7 +161,6 @@
   U m_uv_req;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_REQUEST_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Signal.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Signal.h
index 5fe34c6..9abcad3 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Signal.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Signal.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_SIGNAL_H_
 #define WPIUTIL_WPI_UV_SIGNAL_H_
@@ -15,8 +12,7 @@
 #include "wpi/Signal.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -78,7 +74,6 @@
   sig::Signal<int> signal;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_SIGNAL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Stream.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Stream.h
index be8120d..0ade972 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Stream.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Stream.h
@@ -1,27 +1,25 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_STREAM_H_
 #define WPIUTIL_WPI_UV_STREAM_H_
 
 #include <uv.h>
 
+#include <cstdlib>
 #include <functional>
 #include <initializer_list>
 #include <memory>
+#include <utility>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/Signal.h"
+#include "wpi/span.h"
 #include "wpi/uv/Buffer.h"
 #include "wpi/uv/Handle.h"
 #include "wpi/uv/Request.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Stream;
 
@@ -129,7 +127,7 @@
    * @param bufs The buffers to be written to the stream.
    * @param req write request
    */
-  void Write(ArrayRef<Buffer> bufs, const std::shared_ptr<WriteReq>& req);
+  void Write(span<const Buffer> bufs, const std::shared_ptr<WriteReq>& req);
 
   /**
    * Write data to the stream.
@@ -149,7 +147,7 @@
    */
   void Write(std::initializer_list<Buffer> bufs,
              const std::shared_ptr<WriteReq>& req) {
-    Write(makeArrayRef(bufs.begin(), bufs.end()), req);
+    Write({bufs.begin(), bufs.end()}, req);
   }
 
   /**
@@ -165,8 +163,8 @@
    * @param bufs The buffers to be written to the stream.
    * @param callback Callback function to call when the write completes
    */
-  void Write(ArrayRef<Buffer> bufs,
-             std::function<void(MutableArrayRef<Buffer>, Error)> callback);
+  void Write(span<const Buffer> bufs,
+             std::function<void(span<Buffer>, Error)> callback);
 
   /**
    * Write data to the stream.
@@ -182,8 +180,8 @@
    * @param callback Callback function to call when the write completes
    */
   void Write(std::initializer_list<Buffer> bufs,
-             std::function<void(MutableArrayRef<Buffer>, Error)> callback) {
-    Write(makeArrayRef(bufs.begin(), bufs.end()), callback);
+             std::function<void(span<Buffer>, Error)> callback) {
+    Write({bufs.begin(), bufs.end()}, std::move(callback));
   }
 
   /**
@@ -196,7 +194,7 @@
    * @param bufs The buffers to be written to the stream.
    * @return Number of bytes written.
    */
-  int TryWrite(ArrayRef<Buffer> bufs);
+  int TryWrite(span<const Buffer> bufs);
 
   /**
    * Queue a write request if it can be completed immediately.
@@ -209,7 +207,7 @@
    * @return Number of bytes written.
    */
   int TryWrite(std::initializer_list<Buffer> bufs) {
-    return TryWrite(makeArrayRef(bufs.begin(), bufs.end()));
+    return TryWrite({bufs.begin(), bufs.end()});
   }
 
   /**
@@ -296,10 +294,9 @@
   }
 
  protected:
-  StreamImpl() : Stream{reinterpret_cast<uv_stream_t*>(new U)} {}
+  StreamImpl() : Stream{static_cast<uv_stream_t*>(std::malloc(sizeof(U)))} {}
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_STREAM_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tcp.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tcp.h
index 56c2b8f..c712f0b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tcp.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tcp.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_TCP_H_
 #define WPIUTIL_WPI_UV_TCP_H_
@@ -13,12 +10,12 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <string_view>
+#include <utility>
 
-#include "wpi/Twine.h"
 #include "wpi/uv/NetworkStream.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 class TcpConnectReq;
@@ -116,7 +113,7 @@
   void Open(uv_os_sock_t sock) { Invoke(&uv_tcp_open, GetRaw(), sock); }
 
   /**
-   * Enable/Disable Nagle's algorithm.
+   * Enable no delay operation (turns off Nagle's algorithm).
    * @param enable True to enable it, false otherwise.
    * @return True in case of success, false otherwise.
    */
@@ -189,7 +186,7 @@
    * @param port The port to which to bind.
    * @param flags Optional additional flags.
    */
-  void Bind(const Twine& ip, unsigned int port, unsigned int flags = 0);
+  void Bind(std::string_view ip, unsigned int port, unsigned int flags = 0);
 
   /**
    * Bind the handle to an IPv6 address and port.
@@ -205,7 +202,7 @@
    * @param port The port to which to bind.
    * @param flags Optional additional flags.
    */
-  void Bind6(const Twine& ip, unsigned int port, unsigned int flags = 0);
+  void Bind6(std::string_view ip, unsigned int port, unsigned int flags = 0);
 
   /**
    * Get the current address to which the handle is bound.
@@ -262,11 +259,11 @@
   void Connect(const sockaddr& addr, std::function<void()> callback);
 
   void Connect(const sockaddr_in& addr, std::function<void()> callback) {
-    Connect(reinterpret_cast<const sockaddr&>(addr), callback);
+    Connect(reinterpret_cast<const sockaddr&>(addr), std::move(callback));
   }
 
   void Connect(const sockaddr_in6& addr, std::function<void()> callback) {
-    Connect(reinterpret_cast<const sockaddr&>(addr), callback);
+    Connect(reinterpret_cast<const sockaddr&>(addr), std::move(callback));
   }
 
   /**
@@ -285,7 +282,7 @@
    * @param port The port to which to connect to.
    * @param req connection request
    */
-  void Connect(const Twine& ip, unsigned int port,
+  void Connect(std::string_view ip, unsigned int port,
                const std::shared_ptr<TcpConnectReq>& req);
 
   /**
@@ -302,7 +299,7 @@
    * @param port The port to which to connect to.
    * @param callback Callback function to call when connection established
    */
-  void Connect(const Twine& ip, unsigned int port,
+  void Connect(std::string_view ip, unsigned int port,
                std::function<void()> callback);
 
   /**
@@ -321,7 +318,7 @@
    * @param port The port to which to connect to.
    * @param req connection request
    */
-  void Connect6(const Twine& ip, unsigned int port,
+  void Connect6(std::string_view ip, unsigned int port,
                 const std::shared_ptr<TcpConnectReq>& req);
 
   /**
@@ -338,7 +335,7 @@
    * @param port The port to which to connect to.
    * @param callback Callback function to call when connection established
    */
-  void Connect6(const Twine& ip, unsigned int port,
+  void Connect6(std::string_view ip, unsigned int port,
                 std::function<void()> callback);
 
  private:
@@ -361,7 +358,6 @@
   }
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_TCP_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Timer.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Timer.h
index bdafee3..00363a5 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Timer.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Timer.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_TIMER_H_
 #define WPIUTIL_WPI_UV_TIMER_H_
@@ -13,12 +10,12 @@
 #include <chrono>
 #include <functional>
 #include <memory>
+#include <utility>
 
 #include "wpi/Signal.h"
 #include "wpi/uv/Handle.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -69,7 +66,7 @@
    */
   static void SingleShot(const std::shared_ptr<Loop>& loop, Time timeout,
                          std::function<void()> func) {
-    return SingleShot(*loop, timeout, func);
+    return SingleShot(*loop, timeout, std::move(func));
   }
 
   /**
@@ -133,7 +130,6 @@
   sig::Signal<> timeout;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_TIMER_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tty.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tty.h
index a15d8eb..bb0a6db 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tty.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Tty.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_TTY_H_
 #define WPIUTIL_WPI_UV_TTY_H_
@@ -15,8 +12,7 @@
 
 #include "wpi/uv/Stream.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 class Tty;
@@ -62,7 +58,9 @@
    */
   void SetMode(uv_tty_mode_t mode) {
     int err = uv_tty_set_mode(GetRaw(), mode);
-    if (err < 0) ReportError(err);
+    if (err < 0) {
+      ReportError(err);
+    }
   }
 
   /**
@@ -82,7 +80,6 @@
   }
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_TTY_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Udp.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Udp.h
index 2544c50..71fab13 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Udp.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Udp.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_UDP_H_
 #define WPIUTIL_WPI_UV_UDP_H_
@@ -12,15 +9,15 @@
 
 #include <functional>
 #include <memory>
+#include <string_view>
+#include <utility>
 
-#include "wpi/ArrayRef.h"
 #include "wpi/Signal.h"
-#include "wpi/Twine.h"
+#include "wpi/span.h"
 #include "wpi/uv/Handle.h"
 #include "wpi/uv/Request.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 class Udp;
@@ -104,7 +101,7 @@
    * @param port The port to which to bind.
    * @param flags Optional additional flags.
    */
-  void Bind(const Twine& ip, unsigned int port, unsigned int flags = 0);
+  void Bind(std::string_view ip, unsigned int port, unsigned int flags = 0);
 
   /**
    * Bind the handle to an IPv6 address and port.
@@ -113,7 +110,7 @@
    * @param port The port to which to bind.
    * @param flags Optional additional flags.
    */
-  void Bind6(const Twine& ip, unsigned int port, unsigned int flags = 0);
+  void Bind6(std::string_view ip, unsigned int port, unsigned int flags = 0);
 
   /**
    * Associate the handle to a remote address and port, so every message sent
@@ -140,7 +137,7 @@
    * @param ip The address to which to bind.
    * @param port The port to which to bind.
    */
-  void Connect(const Twine& ip, unsigned int port);
+  void Connect(std::string_view ip, unsigned int port);
 
   /**
    * Associate the handle to an IPv6 address and port, so every message sent
@@ -150,7 +147,7 @@
    * @param port The port to which to bind.
    * @param flags Optional additional flags.
    */
-  void Connect6(const Twine& ip, unsigned int port);
+  void Connect6(std::string_view ip, unsigned int port);
 
   /**
    * Get the remote IP and port on connected UDP handles.
@@ -171,8 +168,8 @@
    * @param interfaceAddr Interface address
    * @param membership Should be UV_JOIN_GROUP or UV_LEAVE_GROUP
    */
-  void SetMembership(const Twine& multicastAddr, const Twine& interfaceAddr,
-                     uv_membership membership);
+  void SetMembership(std::string_view multicastAddr,
+                     std::string_view interfaceAddr, uv_membership membership);
 
   /**
    * Set IP multicast loop flag.  Makes multicast packets loop back to local
@@ -198,7 +195,7 @@
    *
    * @param interfaceAddr Interface address
    */
-  void SetMulticastInterface(const Twine& interfaceAddr);
+  void SetMulticastInterface(std::string_view interfaceAddr);
 
   /**
    * Set broadcast on or off.
@@ -234,15 +231,15 @@
    * @param bufs The buffers to be written to the stream.
    * @param req write request
    */
-  void Send(const sockaddr& addr, ArrayRef<Buffer> bufs,
+  void Send(const sockaddr& addr, span<const Buffer> bufs,
             const std::shared_ptr<UdpSendReq>& req);
 
-  void Send(const sockaddr_in& addr, ArrayRef<Buffer> bufs,
+  void Send(const sockaddr_in& addr, span<const Buffer> bufs,
             const std::shared_ptr<UdpSendReq>& req) {
     Send(reinterpret_cast<const sockaddr&>(addr), bufs, req);
   }
 
-  void Send(const sockaddr_in6& addr, ArrayRef<Buffer> bufs,
+  void Send(const sockaddr_in6& addr, span<const Buffer> bufs,
             const std::shared_ptr<UdpSendReq>& req) {
     Send(reinterpret_cast<const sockaddr&>(addr), bufs, req);
   }
@@ -254,7 +251,7 @@
    * @param bufs The buffers to be written to the stream.
    * @param req write request
    */
-  void Send(ArrayRef<Buffer> bufs, const std::shared_ptr<UdpSendReq>& req);
+  void Send(span<const Buffer> bufs, const std::shared_ptr<UdpSendReq>& req);
 
   /**
    * Send data over the UDP socket.  If the socket has not previously been bound
@@ -273,17 +270,17 @@
    * @param bufs The buffers to be sent.
    * @param callback Callback function to call when the data has been sent.
    */
-  void Send(const sockaddr& addr, ArrayRef<Buffer> bufs,
-            std::function<void(MutableArrayRef<Buffer>, Error)> callback);
+  void Send(const sockaddr& addr, span<const Buffer> bufs,
+            std::function<void(span<Buffer>, Error)> callback);
 
-  void Send(const sockaddr_in& addr, ArrayRef<Buffer> bufs,
-            std::function<void(MutableArrayRef<Buffer>, Error)> callback) {
-    Send(reinterpret_cast<const sockaddr&>(addr), bufs, callback);
+  void Send(const sockaddr_in& addr, span<const Buffer> bufs,
+            std::function<void(span<Buffer>, Error)> callback) {
+    Send(reinterpret_cast<const sockaddr&>(addr), bufs, std::move(callback));
   }
 
-  void Send(const sockaddr_in6& addr, ArrayRef<Buffer> bufs,
-            std::function<void(MutableArrayRef<Buffer>, Error)> callback) {
-    Send(reinterpret_cast<const sockaddr&>(addr), bufs, callback);
+  void Send(const sockaddr_in6& addr, span<const Buffer> bufs,
+            std::function<void(span<Buffer>, Error)> callback) {
+    Send(reinterpret_cast<const sockaddr&>(addr), bufs, std::move(callback));
   }
 
   /**
@@ -293,8 +290,8 @@
    * @param bufs The buffers to be written to the stream.
    * @param callback Callback function to call when the data has been sent.
    */
-  void Send(ArrayRef<Buffer> bufs,
-            std::function<void(MutableArrayRef<Buffer>, Error)> callback);
+  void Send(span<const Buffer> bufs,
+            std::function<void(span<Buffer>, Error)> callback);
 
   /**
    * Same as Send(), but won't queue a send request if it can't be completed
@@ -305,7 +302,7 @@
    * @param bufs The buffers to be send.
    * @return Number of bytes sent.
    */
-  int TrySend(const sockaddr& addr, ArrayRef<Buffer> bufs) {
+  int TrySend(const sockaddr& addr, span<const Buffer> bufs) {
     int val = uv_udp_try_send(GetRaw(), bufs.data(),
                               static_cast<unsigned>(bufs.size()), &addr);
     if (val < 0) {
@@ -315,11 +312,11 @@
     return val;
   }
 
-  int TrySend(const sockaddr_in& addr, ArrayRef<Buffer> bufs) {
+  int TrySend(const sockaddr_in& addr, span<const Buffer> bufs) {
     return TrySend(reinterpret_cast<const sockaddr&>(addr), bufs);
   }
 
-  int TrySend(const sockaddr_in6& addr, ArrayRef<Buffer> bufs) {
+  int TrySend(const sockaddr_in6& addr, span<const Buffer> bufs) {
     return TrySend(reinterpret_cast<const sockaddr&>(addr), bufs);
   }
 
@@ -330,7 +327,7 @@
    * @param bufs The buffers to be written to the stream.
    * @return Number of bytes sent.
    */
-  int TrySend(ArrayRef<Buffer> bufs) {
+  int TrySend(span<const Buffer> bufs) {
     int val = uv_udp_try_send(GetRaw(), bufs.data(),
                               static_cast<unsigned>(bufs.size()), nullptr);
     if (val < 0) {
@@ -376,7 +373,6 @@
   sig::Signal<Buffer&, size_t, const sockaddr&, unsigned> received;
 };
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_UDP_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Work.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Work.h
index bf061fb..5a3a5d2 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Work.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/Work.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_WORK_H_
 #define WPIUTIL_WPI_UV_WORK_H_
@@ -12,12 +9,12 @@
 
 #include <functional>
 #include <memory>
+#include <utility>
 
 #include "wpi/Signal.h"
 #include "wpi/uv/Request.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 class Loop;
 
@@ -88,10 +85,9 @@
 inline void QueueWork(const std::shared_ptr<Loop>& loop,
                       std::function<void()> work,
                       std::function<void()> afterWork) {
-  QueueWork(*loop, work, afterWork);
+  QueueWork(*loop, std::move(work), std::move(afterWork));
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_WORK_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/util.h b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/util.h
index 14a021d..01dec0b 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/util.h
+++ b/third_party/allwpilib/wpiutil/src/main/native/include/wpi/uv/util.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifndef WPIUTIL_WPI_UV_UTIL_H_
 #define WPIUTIL_WPI_UV_UTIL_H_
@@ -11,11 +8,9 @@
 #include <uv.h>
 
 #include <cstring>
+#include <string_view>
 
-#include "wpi/Twine.h"
-
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
 /**
  * Convert a binary structure containing an IPv4 address to a string.
@@ -66,10 +61,12 @@
  */
 template <typename T>
 int AddrToName(const sockaddr_storage& addr, T* ip, unsigned int* port) {
-  if (addr.ss_family == AF_INET)
+  if (addr.ss_family == AF_INET) {
     return AddrToName(reinterpret_cast<const sockaddr_in&>(addr), ip, port);
-  if (addr.ss_family == AF_INET6)
+  }
+  if (addr.ss_family == AF_INET6) {
     return AddrToName(reinterpret_cast<const sockaddr_in6&>(addr), ip, port);
+  }
   char name[1];
   ip->assign(name, name);
   return -1;
@@ -85,10 +82,11 @@
 int AddrToName(const in_addr& addr, T* ip) {
   char name[128];
   int err = uv_inet_ntop(AF_INET, &addr, name, 128);
-  if (err == 0)
+  if (err == 0) {
     ip->assign(name, name + std::strlen(name));
-  else
+  } else {
     ip->assign(name, name);
+  }
   return err;
 }
 
@@ -102,10 +100,11 @@
 int AddrToName(const in6_addr& addr, T* ip) {
   char name[128];
   int err = uv_inet_ntop(AF_INET6, &addr, name, 128);
-  if (err == 0)
+  if (err == 0) {
     ip->assign(name, name + std::strlen(name));
-  else
+  } else {
     ip->assign(name, name);
+  }
   return err;
 }
 
@@ -116,7 +115,7 @@
  * @param addr Output binary structure
  * @return Error (same as `uv_ip4_addr()`).
  */
-int NameToAddr(const Twine& ip, unsigned int port, sockaddr_in* addr);
+int NameToAddr(std::string_view ip, unsigned int port, sockaddr_in* addr);
 
 /**
  * Convert a string containing an IPv6 address to a binary structure.
@@ -125,7 +124,7 @@
  * @param addr Output binary structure
  * @return Error (same as `uv_ip6_addr()`).
  */
-int NameToAddr(const Twine& ip, unsigned int port, sockaddr_in6* addr);
+int NameToAddr(std::string_view ip, unsigned int port, sockaddr_in6* addr);
 
 /**
  * Convert a string containing an IPv4 address to binary format.
@@ -133,7 +132,7 @@
  * @param addr Output binary
  * @return Error (same as `uv_inet_pton()`).
  */
-int NameToAddr(const Twine& ip, in_addr* addr);
+int NameToAddr(std::string_view ip, in_addr* addr);
 
 /**
  * Convert a string containing an IPv6 address to binary format.
@@ -141,9 +140,8 @@
  * @param addr Output binary
  * @return Error (same as `uv_inet_pton()`).
  */
-int NameToAddr(const Twine& ip, in6_addr* addr);
+int NameToAddr(std::string_view ip, in6_addr* addr);
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
 
 #endif  // WPIUTIL_WPI_UV_UTIL_H_
diff --git a/third_party/allwpilib/wpiutil/src/main/native/unix/Demangle.cpp b/third_party/allwpilib/wpiutil/src/main/native/unix/Demangle.cpp
index 3453084..69404ed 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/unix/Demangle.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/unix/Demangle.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/Demangle.h"
 
@@ -15,14 +12,12 @@
 
 namespace wpi {
 
-std::string Demangle(const Twine& mangledSymbol) {
-  SmallString<128> buf;
+std::string Demangle(std::string_view mangledSymbol) {
+  SmallString<128> buf{mangledSymbol};
   size_t length;
   int32_t status;
 
-  char* symbol =
-      abi::__cxa_demangle(mangledSymbol.toNullTerminatedStringRef(buf).data(),
-                          nullptr, &length, &status);
+  char* symbol = abi::__cxa_demangle(buf.c_str(), nullptr, &length, &status);
   if (status == 0) {
     std::string rv{symbol};
     std::free(symbol);
@@ -30,7 +25,7 @@
   }
 
   // If everything else failed, just return the mangled symbol
-  return mangledSymbol.str();
+  return std::string{mangledSymbol};
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/unix/StackTrace.cpp b/third_party/allwpilib/wpiutil/src/main/native/unix/StackTrace.cpp
index 004eb39..598c9da 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/unix/StackTrace.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/unix/StackTrace.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/StackTrace.h"
 
@@ -11,7 +8,7 @@
 
 #include "wpi/Demangle.h"
 #include "wpi/SmallString.h"
-#include "wpi/StringRef.h"
+#include "wpi/StringExtras.h"
 #include "wpi/raw_ostream.h"
 
 namespace wpi {
@@ -27,16 +24,18 @@
     // Only print recursive functions once in a row.
     if (i == 0 || stackTrace[i] != stackTrace[i - 1]) {
       // extract just function name from "pathToExe(functionName+offset)"
-      StringRef sym{mangledSymbols[i]};
-      sym = sym.split('(').second;
-      sym = sym.split('+').first;
-      trace << "\tat " << Demangle(sym) << "\n";
+      std::string_view sym = split(mangledSymbols[i], '(').second;
+      std::string_view offset;
+      std::tie(sym, offset) = split(sym, '+');
+      std::string_view addr;
+      std::tie(offset, addr) = split(offset, ')');
+      trace << "\tat " << Demangle(sym) << " + " << offset << addr << "\n";
     }
   }
 
   std::free(mangledSymbols);
 
-  return trace.str();
+  return std::string{trace.str()};
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/main/native/windows/Demangle.cpp b/third_party/allwpilib/wpiutil/src/main/native/windows/Demangle.cpp
index a1560d6..1b0a8a7 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/windows/Demangle.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/windows/Demangle.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/Demangle.h"
 
@@ -18,15 +15,15 @@
 
 namespace wpi {
 
-std::string Demangle(const Twine& mangledSymbol) {
+std::string Demangle(std::string_view mangledSymbol) {
   static wpi::mutex m;
   std::scoped_lock lock(m);
-  SmallString<128> buf;
+  SmallString<128> buf{mangledSymbol};
   char buffer[256];
-  DWORD sz =
-      UnDecorateSymbolName(mangledSymbol.toNullTerminatedStringRef(buf).data(),
-                           buffer, sizeof(buffer), UNDNAME_COMPLETE);
-  if (sz == 0) return mangledSymbol.str();
+  DWORD sz = UnDecorateSymbolName(buf.c_str(), buffer, sizeof(buffer),
+                                  UNDNAME_COMPLETE);
+  if (sz == 0)
+    return std::string{mangledSymbol};
   return std::string(buffer, sz);
 }
 
diff --git a/third_party/allwpilib/wpiutil/src/main/native/windows/StackTrace.cpp b/third_party/allwpilib/wpiutil/src/main/native/windows/StackTrace.cpp
index 86bbdff..3e3ccfa 100644
--- a/third_party/allwpilib/wpiutil/src/main/native/windows/StackTrace.cpp
+++ b/third_party/allwpilib/wpiutil/src/main/native/windows/StackTrace.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2008-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/StackTrace.h"
 
diff --git a/third_party/allwpilib/wpiutil/src/netconsoleServer/native/cpp/main.cpp b/third_party/allwpilib/wpiutil/src/netconsoleServer/native/cpp/main.cpp
index ed45eeb..c43a670 100644
--- a/third_party/allwpilib/wpiutil/src/netconsoleServer/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpiutil/src/netconsoleServer/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #ifdef __APPLE__
 #include <util.h>
@@ -11,9 +8,12 @@
 #include <pty.h>
 #endif
 
+#include <cstdio>
+
+#include "fmt/format.h"
 #include "wpi/MathExtras.h"
 #include "wpi/SmallVector.h"
-#include "wpi/raw_ostream.h"
+#include "wpi/StringExtras.h"
 #include "wpi/raw_uv_ostream.h"
 #include "wpi/timestamp.h"
 #include "wpi/uv/Loop.h"
@@ -33,9 +33,9 @@
                           wpi::SmallVectorImpl<uv::Buffer>& bufs, bool tcp,
                           uint16_t tcpSeq) {
   // scan for last newline
-  wpi::StringRef str(buf.base, len);
+  std::string_view str(buf.base, len);
   size_t idx = str.rfind('\n');
-  if (idx == wpi::StringRef::npos) {
+  if (idx == std::string_view::npos) {
     // no newline yet, just keep appending to remainder
     rem += str;
     return false;
@@ -43,7 +43,7 @@
 
   // build output
   wpi::raw_uv_ostream out(bufs, 4096);
-  wpi::StringRef toCopy = str.slice(0, idx + 1);
+  std::string_view toCopy = wpi::slice(str, 0, idx + 1);
   if (tcp) {
     // Header is 2 byte len, 1 byte type, 4 byte timestamp, 2 byte sequence num
     uint32_t ts = wpi::FloatToBits((wpi::Now() - startTime) * 1.0e-6);
@@ -57,12 +57,12 @@
                               static_cast<uint8_t>(ts & 0xff),
                               static_cast<uint8_t>((tcpSeq >> 8) & 0xff),
                               static_cast<uint8_t>(tcpSeq & 0xff)};
-    out << wpi::ArrayRef<uint8_t>(header);
+    out << wpi::span<const uint8_t>(header);
   }
   out << rem << toCopy;
 
   // reset remainder
-  rem = str.slice(idx + 1, wpi::StringRef::npos);
+  rem = wpi::slice(str, idx + 1, std::string_view::npos);
   return true;
 }
 
@@ -81,11 +81,15 @@
           uv::Buffer& buf, size_t len) {
         // build buffers
         wpi::SmallVector<uv::Buffer, 4> bufs;
-        if (!NewlineBuffer(*rem, buf, len, bufs, false, 0)) return;
+        if (!NewlineBuffer(*rem, buf, len, bufs, false, 0)) {
+          return;
+        }
 
         // send output
         outPtr->Send(addr, bufs, [](auto bufs2, uv::Error) {
-          for (auto buf : bufs2) buf.Deallocate();
+          for (auto buf : bufs2) {
+            buf.Deallocate();
+          }
         });
       },
       out);
@@ -101,12 +105,15 @@
           uv::Buffer& buf, size_t len) {
         // build buffers
         wpi::SmallVector<uv::Buffer, 4> bufs;
-        if (!NewlineBuffer(data->rem, buf, len, bufs, true, data->seq++))
+        if (!NewlineBuffer(data->rem, buf, len, bufs, true, data->seq++)) {
           return;
+        }
 
         // send output
         outPtr->Write(bufs, [](auto bufs2, uv::Error) {
-          for (auto buf : bufs2) buf.Deallocate();
+          for (auto buf : bufs2) {
+            buf.Deallocate();
+          }
         });
       },
       out);
@@ -116,8 +123,10 @@
   in.data.connect([out](uv::Buffer& buf, size_t len) {
     uv::Buffer buf2 = buf.Dup();
     buf2.len = len;
-    out->Write(buf2, [](auto bufs, uv::Error) {
-      for (auto buf : bufs) buf.Deallocate();
+    out->Write({buf2}, [](auto bufs, uv::Error) {
+      for (auto buf : bufs) {
+        buf.Deallocate();
+      }
     });
   });
 }
@@ -130,24 +139,26 @@
   bool err = false;
 
   while (programArgc < argc && argv[programArgc][0] == '-') {
-    if (wpi::StringRef(argv[programArgc]) == "-u") {
+    if (std::string_view(argv[programArgc]) == "-u") {
       useUdp = true;
-    } else if (wpi::StringRef(argv[programArgc]) == "-b") {
+    } else if (std::string_view(argv[programArgc]) == "-b") {
       useUdp = true;
       broadcastUdp = true;
     } else {
-      wpi::errs() << "unrecognized command line option " << argv[programArgc]
-                  << '\n';
+      fmt::print(stderr, "unrecognized command line option {}\n",
+                 argv[programArgc]);
       err = true;
     }
     ++programArgc;
   }
 
   if (err || (argc - programArgc) < 1) {
-    wpi::errs()
-        << argv[0] << " [-ub] program [arguments ...]\n"
-        << "  -u  send udp to localhost port 6666 instead of using tcp\n"
-        << "  -b  broadcast udp to port 6666 instead of using tcp\n";
+    std::fputs(argv[0], stderr);
+    std::fputs(
+        " [-ub] program [arguments ...]\n"
+        "  -u  send udp to localhost port 6666 instead of using tcp\n"
+        "  -b  broadcast udp to port 6666 instead of using tcp\n",
+        stderr);
     return EXIT_FAILURE;
   }
 
@@ -155,7 +166,7 @@
 
   auto loop = uv::Loop::Create();
   loop->error.connect(
-      [](uv::Error err) { wpi::errs() << "uv ERROR: " << err.str() << '\n'; });
+      [](uv::Error err) { fmt::print(stderr, "uv ERROR: {}\n", err.str()); });
 
   // create pipes to communicate with child
   auto stdinPipe = uv::Pipe::Create(loop);
@@ -168,12 +179,20 @@
   auto stderrTty = uv::Tty::Create(loop, 2, false);
 
   // pass through our console to child's (bidirectional)
-  if (stdinTty) CopyStream(*stdinTty, stdinPipe);
-  if (stdoutTty) CopyStream(*stdoutPipe, stdoutTty);
-  if (stderrTty) CopyStream(*stderrPipe, stderrTty);
+  if (stdinTty) {
+    CopyStream(*stdinTty, stdinPipe);
+  }
+  if (stdoutTty) {
+    CopyStream(*stdoutPipe, stdoutTty);
+  }
+  if (stderrTty) {
+    CopyStream(*stderrPipe, stderrTty);
+  }
 
   // when our stdin closes, also close child stdin
-  if (stdinTty) stdinTty->end.connect([stdinPipe] { stdinPipe->Close(); });
+  if (stdinTty) {
+    stdinTty->end.connect([stdinPipe] { stdinPipe->Close(); });
+  }
 
   if (useUdp) {
     auto udp = uv::Udp::Create(loop);
@@ -189,7 +208,9 @@
     // when we get a connection, accept it
     tcp->connection.connect([srv = tcp.get(), stdoutPipe, stderrPipe] {
       auto tcp = srv->Accept();
-      if (!tcp) return;
+      if (!tcp) {
+        return;
+      }
 
       // close on error
       tcp->error.connect([s = tcp.get()](wpi::uv::Error err) { s->Close(); });
@@ -227,17 +248,21 @@
       uv::Process::StdioCreatePipe(2, *stderrPipe, UV_WRITABLE_PIPE));
 
   // pass our args as the child args (argv[1] becomes child argv[0], etc)
-  for (int i = programArgc; i < argc; ++i) options.emplace_back(argv[i]);
+  for (int i = programArgc; i < argc; ++i) {
+    options.emplace_back(argv[i]);
+  }
 
   auto proc = uv::Process::SpawnArray(loop, argv[programArgc], options);
   if (!proc) {
-    wpi::errs() << "could not start subprocess\n";
+    std::fputs("could not start subprocess\n", stderr);
     return EXIT_FAILURE;
   }
   proc->exited.connect([](int64_t status, int) { std::exit(status); });
 
   // start reading
-  if (stdinTty) stdinTty->StartRead();
+  if (stdinTty) {
+    stdinTty->StartRead();
+  }
   stdoutPipe->StartRead();
   stderrPipe->StartRead();
 
diff --git a/third_party/allwpilib/wpiutil/src/netconsoleTee/native/cpp/main.cpp b/third_party/allwpilib/wpiutil/src/netconsoleTee/native/cpp/main.cpp
index 82cc82a..8abb458 100644
--- a/third_party/allwpilib/wpiutil/src/netconsoleTee/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpiutil/src/netconsoleTee/native/cpp/main.cpp
@@ -1,13 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
+#include <cstdio>
+
+#include "fmt/format.h"
 #include "wpi/MathExtras.h"
 #include "wpi/SmallVector.h"
-#include "wpi/raw_ostream.h"
+#include "wpi/StringExtras.h"
 #include "wpi/raw_uv_ostream.h"
 #include "wpi/timestamp.h"
 #include "wpi/uv/Loop.h"
@@ -24,9 +24,9 @@
                           wpi::SmallVectorImpl<uv::Buffer>& bufs, bool tcp,
                           uint16_t tcpSeq) {
   // scan for last newline
-  wpi::StringRef str(buf.base, len);
+  std::string_view str(buf.base, len);
   size_t idx = str.rfind('\n');
-  if (idx == wpi::StringRef::npos) {
+  if (idx == std::string_view::npos) {
     // no newline yet, just keep appending to remainder
     rem += str;
     return false;
@@ -34,7 +34,7 @@
 
   // build output
   wpi::raw_uv_ostream out(bufs, 4096);
-  wpi::StringRef toCopy = str.slice(0, idx + 1);
+  std::string_view toCopy = wpi::slice(str, 0, idx + 1);
   if (tcp) {
     // Header is 2 byte len, 1 byte type, 4 byte timestamp, 2 byte sequence num
     uint32_t ts = wpi::FloatToBits((wpi::Now() - startTime) * 1.0e-6);
@@ -48,12 +48,12 @@
                               static_cast<uint8_t>(ts & 0xff),
                               static_cast<uint8_t>((tcpSeq >> 8) & 0xff),
                               static_cast<uint8_t>(tcpSeq & 0xff)};
-    out << wpi::ArrayRef<uint8_t>(header);
+    out << wpi::span<const uint8_t>(header);
   }
   out << rem << toCopy;
 
   // reset remainder
-  rem = str.slice(idx + 1, wpi::StringRef::npos);
+  rem = wpi::slice(str, idx + 1, std::string_view::npos);
   return true;
 }
 
@@ -72,11 +72,15 @@
           uv::Buffer& buf, size_t len) {
         // build buffers
         wpi::SmallVector<uv::Buffer, 4> bufs;
-        if (!NewlineBuffer(*rem, buf, len, bufs, false, 0)) return;
+        if (!NewlineBuffer(*rem, buf, len, bufs, false, 0)) {
+          return;
+        }
 
         // send output
         outPtr->Send(addr, bufs, [](auto bufs2, uv::Error) {
-          for (auto buf : bufs2) buf.Deallocate();
+          for (auto buf : bufs2) {
+            buf.Deallocate();
+          }
         });
       },
       out);
@@ -92,12 +96,15 @@
           uv::Buffer& buf, size_t len) {
         // build buffers
         wpi::SmallVector<uv::Buffer, 4> bufs;
-        if (!NewlineBuffer(data->rem, buf, len, bufs, true, data->seq++))
+        if (!NewlineBuffer(data->rem, buf, len, bufs, true, data->seq++)) {
           return;
+        }
 
         // send output
         outPtr->Write(bufs, [](auto bufs2, uv::Error) {
-          for (auto buf : bufs2) buf.Deallocate();
+          for (auto buf : bufs2) {
+            buf.Deallocate();
+          }
         });
       },
       out);
@@ -107,8 +114,10 @@
   in.data.connect([out](uv::Buffer& buf, size_t len) {
     uv::Buffer buf2 = buf.Dup();
     buf2.len = len;
-    out->Write(buf2, [](auto bufs, uv::Error) {
-      for (auto buf : bufs) buf.Deallocate();
+    out->Write({buf2}, [](auto bufs, uv::Error) {
+      for (auto buf : bufs) {
+        buf.Deallocate();
+      }
     });
   });
 }
@@ -122,47 +131,56 @@
   int port = -1;
 
   while (arg < argc && argv[arg][0] == '-') {
-    if (wpi::StringRef(argv[arg]) == "-u") {
+    if (std::string_view(argv[arg]) == "-u") {
       useUdp = true;
-    } else if (wpi::StringRef(argv[arg]) == "-b") {
+    } else if (std::string_view(argv[arg]) == "-b") {
       useUdp = true;
       broadcastUdp = true;
-    } else if (wpi::StringRef(argv[arg]) == "-p") {
+    } else if (std::string_view(argv[arg]) == "-p") {
       ++arg;
+      std::optional<int> portValue;
       if (arg >= argc || argv[arg][0] == '-' ||
-          wpi::StringRef(argv[arg]).getAsInteger(10, port)) {
-        wpi::errs() << "-p must be followed by port number\n";
+          !(portValue = wpi::parse_integer<int>(argv[arg], 10))) {
+        std::fputs("-p must be followed by port number\n", stderr);
         err = true;
+      } else if (portValue) {
+        port = portValue.value();
       }
     } else {
-      wpi::errs() << "unrecognized command line option " << argv[arg] << '\n';
+      fmt::print(stderr, "unrecognized command line option {}\n", argv[arg]);
       err = true;
     }
     ++arg;
   }
 
   if (err) {
-    wpi::errs()
-        << argv[0] << " [-ub] [-p PORT]\n"
-        << "  -u       send udp to localhost port 6666 instead of using tcp\n"
-        << "  -b       broadcast udp to port 6666 instead of using tcp\n"
-        << "  -p PORT  use port PORT instead of 6666 (udp) or 1740 (tcp)\n";
+    std::fputs(argv[0], stderr);
+    std::fputs(
+        " [-ub] [-p PORT]\n"
+        "  -u       send udp to localhost port 6666 instead of using tcp\n"
+        "  -b       broadcast udp to port 6666 instead of using tcp\n"
+        "  -p PORT  use port PORT instead of 6666 (udp) or 1740 (tcp)\n",
+        stderr);
     return EXIT_FAILURE;
   }
 
   auto loop = uv::Loop::Create();
   loop->error.connect(
-      [](uv::Error err) { wpi::errs() << "uv ERROR: " << err.str() << '\n'; });
+      [](uv::Error err) { fmt::print(stderr, "uv ERROR: {}\n", err.str()); });
 
   // create ttys
   auto stdinTty = uv::Tty::Create(loop, 0, true);
   auto stdoutTty = uv::Tty::Create(loop, 1, false);
 
   // don't bother continuing if we don't have a stdin
-  if (!stdinTty) return EXIT_SUCCESS;
+  if (!stdinTty) {
+    return EXIT_SUCCESS;
+  }
 
   // pass through our input to output
-  if (stdoutTty) CopyStream(*stdinTty, stdoutTty);
+  if (stdoutTty) {
+    CopyStream(*stdinTty, stdoutTty);
+  }
 
   // when our stdin closes, exit
   stdinTty->end.connect([] { std::exit(EXIT_SUCCESS); });
@@ -180,7 +198,9 @@
     // when we get a connection, accept it
     tcp->connection.connect([srv = tcp.get(), stdinTty] {
       auto tcp = srv->Accept();
-      if (!tcp) return;
+      if (!tcp) {
+        return;
+      }
 
       // close on error
       tcp->error.connect([s = tcp.get()](wpi::uv::Error err) { s->Close(); });
@@ -194,7 +214,9 @@
   }
 
   // start reading
-  if (stdinTty) stdinTty->StartRead();
+  if (stdinTty) {
+    stdinTty->StartRead();
+  }
 
   // run the loop!
   loop->Run();
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java
new file mode 100644
index 0000000..7972754
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/CircularBufferTest.java
@@ -0,0 +1,213 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import org.junit.jupiter.api.Test;
+
+class CircularBufferTest {
+  private final double[] m_values = {
+    751.848, 766.366, 342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913
+  };
+  private final double[] m_addFirstOut = {
+    799.913, 421.125, 22.727, 445.697, 132.344, 716.126, 234.252, 342.657
+  };
+  private final double[] m_addLastOut = {
+    342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913
+  };
+
+  @Test
+  void addFirstTest() {
+    CircularBuffer queue = new CircularBuffer(8);
+
+    for (double value : m_values) {
+      queue.addFirst(value);
+    }
+
+    for (int i = 0; i < m_addFirstOut.length; i++) {
+      assertEquals(m_addFirstOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void addLastTest() {
+    CircularBuffer queue = new CircularBuffer(8);
+
+    for (double value : m_values) {
+      queue.addLast(value);
+    }
+
+    for (int i = 0; i < m_addLastOut.length; i++) {
+      assertEquals(m_addLastOut[i], queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void pushPopTest() {
+    CircularBuffer queue = new CircularBuffer(3);
+
+    // Insert three elements into the buffer
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    /*
+     * The buffer is full now, so pushing subsequent elements will overwrite the
+     * front-most elements.
+     */
+
+    queue.addLast(4.0); // Overwrite 1 with 4
+
+    // The buffer now contains 2, 3, and 4
+    assertEquals(2.0, queue.get(0), 0.00005);
+    assertEquals(3.0, queue.get(1), 0.00005);
+    assertEquals(4.0, queue.get(2), 0.00005);
+
+    queue.addLast(5.0); // Overwrite 2 with 5
+
+    // The buffer now contains 3, 4, and 5
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+    assertEquals(5.0, queue.get(2), 0.00005);
+
+    assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed
+
+    // The buffer now contains 3 and 4
+    assertEquals(3.0, queue.get(0), 0.00005);
+    assertEquals(4.0, queue.get(1), 0.00005);
+
+    assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed
+
+    // Leaving only one element with value == 4
+    assertEquals(4.0, queue.get(0), 0.00005);
+  }
+
+  @Test
+  void resetTest() {
+    CircularBuffer queue = new CircularBuffer(5);
+
+    for (int i = 0; i < 6; i++) {
+      queue.addLast(i);
+    }
+
+    queue.clear();
+
+    for (int i = 0; i < 5; i++) {
+      assertEquals(0.0, queue.get(i), 0.00005);
+    }
+  }
+
+  @Test
+  void resizeTest() {
+    CircularBuffer queue = new CircularBuffer(5);
+
+    /* Buffer contains {1, 2, 3, _, _}
+     *                  ^ front
+     */
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {_, 1, 2, 3, _}
+     *                     ^ front
+     */
+    queue.addLast(0.0);
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.removeFirst();
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {_, _, 1, 2, 3}
+     *                        ^ front
+     */
+    queue.addLast(0.0);
+    queue.addLast(0.0);
+    queue.addLast(1.0);
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.removeFirst();
+    queue.removeFirst();
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {3, _, _, 1, 2}
+     *                           ^ front
+     */
+    queue.addLast(3.0);
+    queue.addFirst(2.0);
+    queue.addFirst(1.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.clear();
+
+    /* Buffer contains {2, 3, _, _, 1}
+     *                              ^ front
+     */
+    queue.addLast(2.0);
+    queue.addLast(3.0);
+    queue.addFirst(1.0);
+
+    queue.resize(2);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    queue.resize(5);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+
+    // Test addLast() after resize
+    queue.addLast(3.0);
+    assertEquals(1.0, queue.get(0), 0.00005);
+    assertEquals(2.0, queue.get(1), 0.00005);
+    assertEquals(3.0, queue.get(2), 0.00005);
+
+    // Test addFirst() after resize
+    queue.addFirst(4.0);
+    assertEquals(4.0, queue.get(0), 0.00005);
+    assertEquals(1.0, queue.get(1), 0.00005);
+    assertEquals(2.0, queue.get(2), 0.00005);
+    assertEquals(3.0, queue.get(3), 0.00005);
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/WPIUtilJNITest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/WPIUtilJNITest.java
new file mode 100644
index 0000000..7d5b229
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/util/WPIUtilJNITest.java
@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.util;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+
+import org.junit.jupiter.api.Test;
+
+public class WPIUtilJNITest {
+  @Test
+  public void testEnableMockTime() {
+    assertDoesNotThrow(WPIUtilJNI::enableMockTime);
+  }
+
+  @Test
+  public void testSetMockTime() {
+    assertDoesNotThrow(() -> WPIUtilJNI.setMockTime(0L));
+  }
+
+  @Test
+  public void testNow() {
+    assertDoesNotThrow(WPIUtilJNI::now);
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/wpiutil/CircularBufferTest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/wpiutil/CircularBufferTest.java
deleted file mode 100644
index f995098..0000000
--- a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/wpiutil/CircularBufferTest.java
+++ /dev/null
@@ -1,214 +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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertEquals;
-
-class CircularBufferTest {
-  private final double[] m_values = {751.848, 766.366, 342.657, 234.252, 716.126,
-      132.344, 445.697, 22.727, 421.125, 799.913};
-  private final double[] m_addFirstOut = {799.913, 421.125, 22.727, 445.697, 132.344,
-      716.126, 234.252, 342.657};
-  private final double[] m_addLastOut = {342.657, 234.252, 716.126, 132.344, 445.697,
-      22.727, 421.125, 799.913};
-
-  @Test
-  void addFirstTest() {
-    CircularBuffer queue = new CircularBuffer(8);
-
-    for (double value : m_values) {
-      queue.addFirst(value);
-    }
-
-    for (int i = 0; i < m_addFirstOut.length; i++) {
-      assertEquals(m_addFirstOut[i], queue.get(i), 0.00005);
-    }
-  }
-
-  @Test
-  void addLastTest() {
-    CircularBuffer queue = new CircularBuffer(8);
-
-    for (double value : m_values) {
-      queue.addLast(value);
-    }
-
-    for (int i = 0; i < m_addLastOut.length; i++) {
-      assertEquals(m_addLastOut[i], queue.get(i), 0.00005);
-    }
-  }
-
-  @Test
-  void pushPopTest() {
-    CircularBuffer queue = new CircularBuffer(3);
-
-    // Insert three elements into the buffer
-    queue.addLast(1.0);
-    queue.addLast(2.0);
-    queue.addLast(3.0);
-
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-    assertEquals(3.0, queue.get(2), 0.00005);
-
-    /*
-     * The buffer is full now, so pushing subsequent elements will overwrite the
-     * front-most elements.
-     */
-
-    queue.addLast(4.0); // Overwrite 1 with 4
-
-    // The buffer now contains 2, 3, and 4
-    assertEquals(2.0, queue.get(0), 0.00005);
-    assertEquals(3.0, queue.get(1), 0.00005);
-    assertEquals(4.0, queue.get(2), 0.00005);
-
-    queue.addLast(5.0); // Overwrite 2 with 5
-
-    // The buffer now contains 3, 4, and 5
-    assertEquals(3.0, queue.get(0), 0.00005);
-    assertEquals(4.0, queue.get(1), 0.00005);
-    assertEquals(5.0, queue.get(2), 0.00005);
-
-    assertEquals(5.0, queue.removeLast(), 0.00005); // 5 is removed
-
-    // The buffer now contains 3 and 4
-    assertEquals(3.0, queue.get(0), 0.00005);
-    assertEquals(4.0, queue.get(1), 0.00005);
-
-    assertEquals(3.0, queue.removeFirst(), 0.00005); // 3 is removed
-
-    // Leaving only one element with value == 4
-    assertEquals(4.0, queue.get(0), 0.00005);
-  }
-
-  @Test
-  void resetTest() {
-    CircularBuffer queue = new CircularBuffer(5);
-
-    for (int i = 0; i < 6; i++) {
-      queue.addLast(i);
-    }
-
-    queue.clear();
-
-    for (int i = 0; i < 5; i++) {
-      assertEquals(0.0, queue.get(i), 0.00005);
-    }
-  }
-
-  @Test
-  @SuppressWarnings("PMD.ExcessiveMethodLength")
-  void resizeTest() {
-    CircularBuffer queue = new CircularBuffer(5);
-
-    /* Buffer contains {1, 2, 3, _, _}
-     *                  ^ front
-     */
-    queue.addLast(1.0);
-    queue.addLast(2.0);
-    queue.addLast(3.0);
-
-    queue.resize(2);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.resize(5);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.clear();
-
-    /* Buffer contains {_, 1, 2, 3, _}
-     *                     ^ front
-     */
-    queue.addLast(0.0);
-    queue.addLast(1.0);
-    queue.addLast(2.0);
-    queue.addLast(3.0);
-    queue.removeFirst();
-
-    queue.resize(2);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.resize(5);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.clear();
-
-    /* Buffer contains {_, _, 1, 2, 3}
-     *                        ^ front
-     */
-    queue.addLast(0.0);
-    queue.addLast(0.0);
-    queue.addLast(1.0);
-    queue.addLast(2.0);
-    queue.addLast(3.0);
-    queue.removeFirst();
-    queue.removeFirst();
-
-    queue.resize(2);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.resize(5);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.clear();
-
-    /* Buffer contains {3, _, _, 1, 2}
-     *                           ^ front
-     */
-    queue.addLast(3.0);
-    queue.addFirst(2.0);
-    queue.addFirst(1.0);
-
-    queue.resize(2);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.resize(5);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.clear();
-
-    /* Buffer contains {2, 3, _, _, 1}
-     *                              ^ front
-     */
-    queue.addLast(2.0);
-    queue.addLast(3.0);
-    queue.addFirst(1.0);
-
-    queue.resize(2);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    queue.resize(5);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-
-    // Test addLast() after resize
-    queue.addLast(3.0);
-    assertEquals(1.0, queue.get(0), 0.00005);
-    assertEquals(2.0, queue.get(1), 0.00005);
-    assertEquals(3.0, queue.get(2), 0.00005);
-
-    // Test addFirst() after resize
-    queue.addFirst(4.0);
-    assertEquals(4.0, queue.get(0), 0.00005);
-    assertEquals(1.0, queue.get(1), 0.00005);
-    assertEquals(2.0, queue.get(2), 0.00005);
-    assertEquals(3.0, queue.get(3), 0.00005);
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java b/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java
deleted file mode 100644
index e92efe4..0000000
--- a/third_party/allwpilib/wpiutil/src/test/java/edu/wpi/first/wpiutil/WPIUtilJNITest.java
+++ /dev/null
@@ -1,19 +0,0 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
-
-package edu.wpi.first.wpiutil;
-
-import org.junit.jupiter.api.Test;
-
-import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
-
-public class WPIUtilJNITest {
-  @Test
-  public void testNow() {
-    assertDoesNotThrow(WPIUtilJNI::now);
-  }
-}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/ManagedStaticTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/ManagedStaticTest.cpp
index 81a4b9c..ab54499 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/ManagedStaticTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/ManagedStaticTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/ManagedStatic.h"  // NOLINT(build/include_order)
 
@@ -23,6 +20,7 @@
   {
     refCount = 0;
     wpi::ManagedStatic<StaticTestClass> managedStatic;
+    (void)managedStatic;
     ASSERT_EQ(refCount, 0);
   }
   ASSERT_EQ(refCount, 0);
@@ -46,7 +44,7 @@
   {
     refCount = 0;
     StaticTestClass* test = new StaticTestClass{};
-    ASSERT_EQ(refCount, 1);
+    ASSERT_EQ(refCount, 1);  // NOLINT
     wpi::ManagedStatic<StaticTestClass> managedStatic(
         test, [](void* val) { delete static_cast<StaticTestClass*>(val); });
     ASSERT_EQ(refCount, 1);
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/Base64Test.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/Base64Test.cpp
index caa35aa..542858d 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/Base64Test.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/Base64Test.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gtest/gtest.h"
 #include "wpi/Base64.h"
@@ -26,11 +23,12 @@
 
 class Base64Test : public ::testing::TestWithParam<Base64TestParam> {
  protected:
-  StringRef GetPlain() {
-    if (GetParam().plain_len < 0)
-      return StringRef(GetParam().plain);
-    else
-      return StringRef(GetParam().plain, GetParam().plain_len);
+  std::string_view GetPlain() {
+    if (GetParam().plain_len < 0) {
+      return GetParam().plain;
+    } else {
+      return std::string_view(GetParam().plain, GetParam().plain_len);
+    }
   }
 };
 
@@ -53,7 +51,7 @@
 
 TEST_P(Base64Test, DecodeStdString) {
   std::string s;
-  StringRef encoded = GetParam().encoded;
+  std::string_view encoded = GetParam().encoded;
   EXPECT_EQ(encoded.size(), Base64Decode(encoded, &s));
   ASSERT_EQ(GetPlain(), s);
 
@@ -64,9 +62,9 @@
 
 TEST_P(Base64Test, DecodeSmallString) {
   SmallString<128> buf;
-  StringRef encoded = GetParam().encoded;
+  std::string_view encoded = GetParam().encoded;
   size_t len;
-  StringRef plain = Base64Decode(encoded, &len, buf);
+  std::string_view plain = Base64Decode(encoded, &len, buf);
   EXPECT_EQ(encoded.size(), len);
   ASSERT_EQ(GetPlain(), plain);
 
@@ -85,7 +83,8 @@
      "mQgc28gb24uLi4K"},
 };
 
-INSTANTIATE_TEST_SUITE_P(Base64Sample, Base64Test, ::testing::ValuesIn(sample));
+INSTANTIATE_TEST_SUITE_P(Base64SampleTests, Base64Test,
+                         ::testing::ValuesIn(sample));
 
 static Base64TestParam standard[] = {
     {0, "", ""},
@@ -98,7 +97,7 @@
     {2, "\xff\xef", "/+8="},
 };
 
-INSTANTIATE_TEST_SUITE_P(Base64Standard, Base64Test,
+INSTANTIATE_TEST_SUITE_P(Base64StandardTests, Base64Test,
                          ::testing::ValuesIn(standard));
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/CircularBufferTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/CircularBufferTest.cpp
index 7fe9e03..ad2285e 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/CircularBufferTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/CircularBufferTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/circular_buffer.h"  // NOLINT(build/include_order)
 
@@ -21,31 +18,55 @@
 static const std::array<double, 8> pushBackOut = {
     {342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}};
 
-TEST(CircularBufferTest, PushFrontTest) {
+TEST(CircularBufferTest, PushFront) {
   wpi::circular_buffer<double> queue(8);
 
   for (auto& value : values) {
     queue.push_front(value);
   }
 
-  for (size_t i = 0; i < pushFrontOut.size(); i++) {
+  for (size_t i = 0; i < pushFrontOut.size(); ++i) {
     EXPECT_EQ(pushFrontOut[i], queue[i]);
   }
 }
 
-TEST(CircularBufferTest, PushBackTest) {
+TEST(CircularBufferTest, PushBack) {
   wpi::circular_buffer<double> queue(8);
 
   for (auto& value : values) {
     queue.push_back(value);
   }
 
-  for (size_t i = 0; i < pushBackOut.size(); i++) {
+  for (size_t i = 0; i < pushBackOut.size(); ++i) {
     EXPECT_EQ(pushBackOut[i], queue[i]);
   }
 }
 
-TEST(CircularBufferTest, PushPopTest) {
+TEST(CircularBufferTest, EmplaceFront) {
+  wpi::circular_buffer<double> queue(8);
+
+  for (auto& value : values) {
+    queue.emplace_front(value);
+  }
+
+  for (size_t i = 0; i < pushFrontOut.size(); ++i) {
+    EXPECT_EQ(pushFrontOut[i], queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, EmplaceBack) {
+  wpi::circular_buffer<double> queue(8);
+
+  for (auto& value : values) {
+    queue.emplace_back(value);
+  }
+
+  for (size_t i = 0; i < pushBackOut.size(); ++i) {
+    EXPECT_EQ(pushBackOut[i], queue[i]);
+  }
+}
+
+TEST(CircularBufferTest, PushPop) {
   wpi::circular_buffer<double> queue(3);
 
   // Insert three elements into the buffer
@@ -88,21 +109,19 @@
   EXPECT_EQ(4.0, queue[0]);
 }
 
-TEST(CircularBufferTest, ResetTest) {
+TEST(CircularBufferTest, Reset) {
   wpi::circular_buffer<double> queue(5);
 
-  for (size_t i = 1; i < 6; i++) {
+  for (size_t i = 1; i < 6; ++i) {
     queue.push_back(i);
   }
 
   queue.reset();
 
-  for (size_t i = 0; i < 5; i++) {
-    EXPECT_EQ(0.0, queue[i]);
-  }
+  EXPECT_EQ(queue.size(), size_t{0});
 }
 
-TEST(CircularBufferTest, ResizeTest) {
+TEST(CircularBufferTest, Resize) {
   wpi::circular_buffer<double> queue(5);
 
   /* Buffer contains {1, 2, 3, _, _}
@@ -207,3 +226,29 @@
   EXPECT_EQ(2.0, queue[2]);
   EXPECT_EQ(3.0, queue[3]);
 }
+
+TEST(CircularBufferTest, Iterator) {
+  wpi::circular_buffer<double> queue(3);
+
+  queue.push_back(1.0);
+  queue.push_back(2.0);
+  queue.push_back(3.0);
+  queue.push_back(4.0);  // Overwrite 1 with 4
+
+  // The buffer now contains 2, 3 and 4
+  const std::array<double, 3> values = {2.0, 3.0, 4.0};
+
+  // iterator
+  int i = 0;
+  for (auto& elem : queue) {
+    EXPECT_EQ(values[i], elem);
+    ++i;
+  }
+
+  // const_iterator
+  i = 0;
+  for (const auto& elem : queue) {
+    EXPECT_EQ(values[i], elem);
+    ++i;
+  }
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpParserTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpParserTest.cpp
index 8de2bc6..281abd0 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpParserTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpParserTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/HttpParser.h"  // NOLINT(build/include_order)
 
@@ -14,7 +11,7 @@
 TEST(HttpParserTest, UrlMethodHeadersComplete) {
   HttpParser p{HttpParser::kRequest};
   int callbacks = 0;
-  p.url.connect([&](StringRef path) {
+  p.url.connect([&](std::string_view path) {
     ASSERT_EQ(path, "/foo/bar");
     ASSERT_EQ(p.GetUrl(), "/foo/bar");
     ++callbacks;
@@ -32,7 +29,7 @@
 TEST(HttpParserTest, UrlMethodHeader) {
   HttpParser p{HttpParser::kRequest};
   int callbacks = 0;
-  p.url.connect([&](StringRef path) {
+  p.url.connect([&](std::string_view path) {
     ASSERT_EQ(path, "/foo/bar");
     ASSERT_EQ(p.GetUrl(), "/foo/bar");
     ++callbacks;
@@ -52,7 +49,7 @@
 TEST(HttpParserTest, StatusHeadersComplete) {
   HttpParser p{HttpParser::kResponse};
   int callbacks = 0;
-  p.status.connect([&](StringRef status) {
+  p.status.connect([&](std::string_view status) {
     ASSERT_EQ(status, "OK");
     ASSERT_EQ(p.GetStatusCode(), 200u);
     ++callbacks;
@@ -69,7 +66,7 @@
 TEST(HttpParserTest, StatusHeader) {
   HttpParser p{HttpParser::kResponse};
   int callbacks = 0;
-  p.status.connect([&](StringRef status) {
+  p.status.connect([&](std::string_view status) {
     ASSERT_EQ(status, "OK");
     ASSERT_EQ(p.GetStatusCode(), 200u);
     ++callbacks;
@@ -86,7 +83,7 @@
 TEST(HttpParserTest, HeaderFieldComplete) {
   HttpParser p{HttpParser::kRequest};
   int callbacks = 0;
-  p.header.connect([&](StringRef name, StringRef value) {
+  p.header.connect([&](std::string_view name, std::string_view value) {
     ASSERT_EQ(name, "Foo");
     ASSERT_EQ(value, "Bar");
     ++callbacks;
@@ -109,7 +106,7 @@
 TEST(HttpParserTest, HeaderFieldNext) {
   HttpParser p{HttpParser::kRequest};
   int callbacks = 0;
-  p.header.connect([&](StringRef name, StringRef value) {
+  p.header.connect([&](std::string_view name, std::string_view value) {
     ASSERT_EQ(name, "Foo");
     ASSERT_EQ(value, "Bar");
     ++callbacks;
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpUtilTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpUtilTest.cpp
index a83214d..524086a 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpUtilTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpUtilTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/HttpUtil.h"  // NOLINT(build/include_order)
 
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp
index 8022cae..18a21be 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/HttpWebSocketServerConnectionTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/HttpWebSocketServerConnection.h"  // NOLINT(build/include_order)
 
@@ -15,7 +12,7 @@
     : public HttpWebSocketServerConnection<HttpWebSocketServerConnectionTest> {
  public:
   HttpWebSocketServerConnectionTest(std::shared_ptr<uv::Stream> stream,
-                                    ArrayRef<StringRef> protocols)
+                                    span<const std::string_view> protocols)
       : HttpWebSocketServerConnection{stream, protocols} {}
 
   void ProcessRequest() override { ++gotRequest; }
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp
index 72da38b..f95f9cb 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/StaticCircularBufferTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/static_circular_buffer.h"  // NOLINT(build/include_order)
 
@@ -21,7 +18,7 @@
 static const std::array<double, 8> pushBackOut = {
     {342.657, 234.252, 716.126, 132.344, 445.697, 22.727, 421.125, 799.913}};
 
-TEST(StaticCircularBufferTest, PushFrontTest) {
+TEST(StaticCircularBufferTest, PushFront) {
   wpi::static_circular_buffer<double, 8> queue;
 
   for (auto& value : values) {
@@ -33,7 +30,7 @@
   }
 }
 
-TEST(StaticCircularBufferTest, PushBackTest) {
+TEST(StaticCircularBufferTest, PushBack) {
   wpi::static_circular_buffer<double, 8> queue;
 
   for (auto& value : values) {
@@ -45,7 +42,7 @@
   }
 }
 
-TEST(StaticCircularBufferTest, EmplaceFrontTest) {
+TEST(StaticCircularBufferTest, EmplaceFront) {
   wpi::static_circular_buffer<double, 8> queue;
 
   for (auto& value : values) {
@@ -57,7 +54,7 @@
   }
 }
 
-TEST(StaticCircularBufferTest, EmplaceBackTest) {
+TEST(StaticCircularBufferTest, EmplaceBack) {
   wpi::static_circular_buffer<double, 8> queue;
 
   for (auto& value : values) {
@@ -69,7 +66,7 @@
   }
 }
 
-TEST(StaticCircularBufferTest, PushPopTest) {
+TEST(StaticCircularBufferTest, PushPop) {
   wpi::static_circular_buffer<double, 3> queue;
 
   // Insert three elements into the buffer
@@ -112,7 +109,7 @@
   EXPECT_EQ(4.0, queue[0]);
 }
 
-TEST(StaticCircularBufferTest, ResetTest) {
+TEST(StaticCircularBufferTest, Reset) {
   wpi::static_circular_buffer<double, 5> queue;
 
   for (size_t i = 1; i < 6; ++i) {
@@ -124,7 +121,7 @@
   EXPECT_EQ(queue.size(), size_t{0});
 }
 
-TEST(StaticCircularBufferTest, IteratorTest) {
+TEST(StaticCircularBufferTest, Iterator) {
   wpi::static_circular_buffer<double, 3> queue;
 
   queue.push_back(1.0);
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/SynchronizationTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/SynchronizationTest.cpp
new file mode 100644
index 0000000..a349c54
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/SynchronizationTest.cpp
@@ -0,0 +1,56 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/Synchronization.h"  // NOLINT(build/include_order)
+
+#include <thread>
+
+#include "gtest/gtest.h"
+
+TEST(EventTest, AutoReset) {
+  auto event = wpi::CreateEvent(false, false);
+  std::thread thr([&] { wpi::SetEvent(event); });
+  wpi::WaitForObject(event);
+  thr.join();
+  bool timedOut;
+  wpi::WaitForObject(event, 0, &timedOut);
+  ASSERT_EQ(timedOut, true);
+}
+
+TEST(EventTest, ManualReset) {
+  auto event = wpi::CreateEvent(true, false);
+  int done = 0;
+  std::thread thr([&] {
+    wpi::SetEvent(event);
+    ++done;
+  });
+  wpi::WaitForObject(event);
+  thr.join();
+  ASSERT_EQ(done, 1);
+  bool timedOut;
+  wpi::WaitForObject(event, 0, &timedOut);
+  ASSERT_EQ(timedOut, false);
+}
+
+TEST(EventTest, InitialSet) {
+  auto event = wpi::CreateEvent(false, true);
+  bool timedOut;
+  wpi::WaitForObject(event, 0, &timedOut);
+  ASSERT_EQ(timedOut, false);
+}
+
+TEST(EventTest, WaitMultiple) {
+  auto event1 = wpi::CreateEvent(false, false);
+  auto event2 = wpi::CreateEvent(false, false);
+  std::thread thr([&] { wpi::SetEvent(event2); });
+  WPI_Handle signaled[2];
+  auto result1 = wpi::WaitForObjects({event1, event2}, signaled);
+  thr.join();
+  ASSERT_EQ(result1.size(), 1u);
+  ASSERT_EQ(result1[0], event2);
+  bool timedOut;
+  auto result2 = wpi::WaitForObjects({event1, event2}, signaled, 0, &timedOut);
+  ASSERT_EQ(timedOut, true);
+  ASSERT_EQ(result2.size(), 0u);
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/UidVectorTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/UidVectorTest.cpp
index e11c7b2..f9e1624 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/UidVectorTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/UidVectorTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/UidVector.h"  // NOLINT(build/include_order)
 
@@ -39,7 +36,9 @@
   v.emplace_back(2);
   v.emplace_back(1);
   std::vector<int> out;
-  for (auto&& val : v) out.push_back(val);
+  for (auto&& val : v) {
+    out.push_back(val);
+  }
   ASSERT_EQ(out.size(), 2u);
   EXPECT_EQ(out[0], 2);
   EXPECT_EQ(out[1], 1);
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketClientTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketClientTest.cpp
index 2db9b54..9d85e66 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketClientTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketClientTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WebSocket.h"  // NOLINT(build/include_order)
 
@@ -11,6 +8,7 @@
 #include "wpi/Base64.h"
 #include "wpi/HttpParser.h"
 #include "wpi/SmallString.h"
+#include "wpi/StringExtras.h"
 #include "wpi/raw_uv_ostream.h"
 #include "wpi/sha1.h"
 
@@ -20,9 +18,11 @@
  public:
   WebSocketClientTest() {
     // Bare bones server
-    req.header.connect([this](StringRef name, StringRef value) {
+    req.header.connect([this](std::string_view name, std::string_view value) {
       // save key (required for valid response)
-      if (name.equals_lower("sec-websocket-key")) clientKey = value;
+      if (equals_lower(name, "sec-websocket-key")) {
+        clientKey = value;
+      }
     });
     req.headersComplete.connect([this](bool) {
       // send response
@@ -36,37 +36,48 @@
       SHA1 hash;
       hash.Update(clientKey);
       hash.Update("258EAFA5-E914-47DA-95CA-C5AB0DC85B11");
-      if (mockBadAccept) hash.Update("1");
+      if (mockBadAccept) {
+        hash.Update("1");
+      }
       SmallString<64> hashBuf;
       SmallString<64> acceptBuf;
       os << "Sec-WebSocket-Accept: "
          << Base64Encode(hash.RawFinal(hashBuf), acceptBuf) << "\r\n";
 
-      if (!mockProtocol.empty())
+      if (!mockProtocol.empty()) {
         os << "Sec-WebSocket-Protocol: " << mockProtocol << "\r\n";
+      }
 
       os << "\r\n";
 
       conn->Write(bufs, [](auto bufs, uv::Error) {
-        for (auto& buf : bufs) buf.Deallocate();
+        for (auto& buf : bufs) {
+          buf.Deallocate();
+        }
       });
 
       serverHeadersDone = true;
-      if (connected) connected();
+      if (connected) {
+        connected();
+      }
     });
 
     serverPipe->Listen([this] {
       conn = serverPipe->Accept();
       conn->StartRead();
       conn->data.connect([this](uv::Buffer& buf, size_t size) {
-        StringRef data{buf.base, size};
+        std::string_view data{buf.base, size};
         if (!serverHeadersDone) {
           data = req.Execute(data);
-          if (req.HasError()) Finish();
+          if (req.HasError()) {
+            Finish();
+          }
           ASSERT_EQ(req.GetError(), HPE_OK) << http_errno_name(req.GetError());
-          if (data.empty()) return;
+          if (data.empty()) {
+            return;
+          }
         }
-        wireData.insert(wireData.end(), data.bytes_begin(), data.bytes_end());
+        wireData.insert(wireData.end(), data.begin(), data.end());
       });
       conn->end.connect([this] { Finish(); });
     });
@@ -87,12 +98,13 @@
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       Finish();
-      if (code != 1005 && code != 1006)
+      if (code != 1005 && code != 1006) {
         FAIL() << "Code: " << code << " Reason: " << reason;
+      }
     });
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       ++gotOpen;
       Finish();
       ASSERT_TRUE(protocol.empty());
@@ -101,7 +113,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotOpen, 1);
 }
 
@@ -112,12 +126,12 @@
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-    ws->closed.connect([&](uint16_t code, StringRef msg) {
+    ws->closed.connect([&](uint16_t code, std::string_view msg) {
       Finish();
       ++gotClosed;
       ASSERT_EQ(code, 1002) << "Message: " << msg;
     });
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       Finish();
       FAIL() << "Got open";
     });
@@ -125,7 +139,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotClosed, 1);
 }
 
@@ -137,12 +153,13 @@
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName,
                                       {"myProtocol", "myProtocol2"});
-    ws->closed.connect([&](uint16_t code, StringRef msg) {
+    ws->closed.connect([&](uint16_t code, std::string_view msg) {
       Finish();
-      if (code != 1005 && code != 1006)
+      if (code != 1005 && code != 1006) {
         FAIL() << "Code: " << code << "Message: " << msg;
+      }
     });
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       ++gotOpen;
       Finish();
       ASSERT_EQ(protocol, "myProtocol");
@@ -151,7 +168,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotOpen, 1);
 }
 
@@ -162,12 +181,12 @@
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-    ws->closed.connect([&](uint16_t code, StringRef msg) {
+    ws->closed.connect([&](uint16_t code, std::string_view msg) {
       Finish();
       ++gotClosed;
       ASSERT_EQ(code, 1003) << "Message: " << msg;
     });
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       Finish();
       FAIL() << "Got open";
     });
@@ -175,7 +194,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotClosed, 1);
 }
 
@@ -184,13 +205,13 @@
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName,
-                                      StringRef{"myProtocol"});
-    ws->closed.connect([&](uint16_t code, StringRef msg) {
+                                      {{"myProtocol"}});
+    ws->closed.connect([&](uint16_t code, std::string_view msg) {
       Finish();
       ++gotClosed;
       ASSERT_EQ(code, 1002) << "Message: " << msg;
     });
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       Finish();
       FAIL() << "Got open";
     });
@@ -198,7 +219,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotClosed, 1);
 }
 
@@ -213,7 +236,9 @@
   WebSocketClientDataTest() {
     clientPipe->Connect(pipeName, [&] {
       ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-      if (setupWebSocket) setupWebSocket();
+      if (setupWebSocket) {
+        setupWebSocket();
+      }
     });
   }
 
@@ -228,8 +253,8 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) {
-      ws->SendBinary(uv::Buffer(data), [&](auto bufs, uv::Error) {
+    ws->open.connect([&](std::string_view) {
+      ws->SendBinary({{data}}, [&](auto bufs, uv::Error) {
         ++gotCallback;
         ws->Terminate();
         ASSERT_FALSE(bufs.empty());
@@ -250,7 +275,7 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->binary.connect([&](ArrayRef<uint8_t> inData, bool fin) {
+    ws->binary.connect([&](auto inData, bool fin) {
       ++gotCallback;
       ws->Terminate();
       ASSERT_TRUE(fin);
@@ -259,9 +284,7 @@
     });
   };
   auto message = BuildMessage(0x02, true, false, data);
-  connected = [&] {
-    conn->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
-  };
+  connected = [&] { conn->Write({{message}}, [&](auto bufs, uv::Error) {}); };
 
   loop->Run();
 
@@ -276,19 +299,17 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), ' ');
   setupWebSocket = [&] {
-    ws->text.connect([&](StringRef, bool) {
+    ws->text.connect([&](std::string_view, bool) {
       ws->Terminate();
       FAIL() << "Should not have gotten masked message";
     });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(0x01, true, true, data);
-  connected = [&] {
-    conn->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
-  };
+  connected = [&] { conn->Write({{message}}, [&](auto bufs, uv::Error) {}); };
 
   loop->Run();
 
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketIntegrationTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketIntegrationTest.cpp
index 9a66a2e..3de75d9 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketIntegrationTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketIntegrationTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WebSocketServer.h"  // NOLINT(build/include_order)
 
@@ -22,7 +19,7 @@
   serverPipe->Listen([&]() {
     auto conn = serverPipe->Accept();
     auto server = WebSocketServer::Create(*conn);
-    server->connected.connect([&](StringRef url, WebSocket&) {
+    server->connected.connect([&](std::string_view url, WebSocket&) {
       ++gotServerOpen;
       ASSERT_EQ(url, "/test");
     });
@@ -30,12 +27,13 @@
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       Finish();
-      if (code != 1005 && code != 1006)
+      if (code != 1005 && code != 1006) {
         FAIL() << "Code: " << code << " Reason: " << reason;
+      }
     });
-    ws->open.connect([&, s = ws.get()](StringRef) {
+    ws->open.connect([&, s = ws.get()](std::string_view) {
       ++gotClientOpen;
       s->Close();
     });
@@ -54,7 +52,7 @@
   serverPipe->Listen([&]() {
     auto conn = serverPipe->Accept();
     auto server = WebSocketServer::Create(*conn, {"proto1", "proto2"});
-    server->connected.connect([&](StringRef, WebSocket& ws) {
+    server->connected.connect([&](std::string_view, WebSocket& ws) {
       ++gotServerOpen;
       ASSERT_EQ(ws.GetProtocol(), "proto1");
     });
@@ -63,12 +61,13 @@
   clientPipe->Connect(pipeName, [&] {
     auto ws =
         WebSocket::CreateClient(*clientPipe, "/test", pipeName, {"proto1"});
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       Finish();
-      if (code != 1005 && code != 1006)
+      if (code != 1005 && code != 1006) {
         FAIL() << "Code: " << code << " Reason: " << reason;
+      }
     });
-    ws->open.connect([&, s = ws.get()](StringRef protocol) {
+    ws->open.connect([&, s = ws.get()](std::string_view protocol) {
       ++gotClientOpen;
       s->Close();
       ASSERT_EQ(protocol, "proto1");
@@ -87,20 +86,21 @@
   serverPipe->Listen([&]() {
     auto conn = serverPipe->Accept();
     auto server = WebSocketServer::Create(*conn);
-    server->connected.connect([&](StringRef, WebSocket& ws) {
-      ws.SendBinary(uv::Buffer{"\x03\x04", 2}, [&](auto, uv::Error) {});
+    server->connected.connect([&](std::string_view, WebSocket& ws) {
+      ws.SendBinary({uv::Buffer{"\x03\x04", 2}}, [&](auto, uv::Error) {});
       ws.Close();
     });
   });
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       Finish();
-      if (code != 1005 && code != 1006)
+      if (code != 1005 && code != 1006) {
         FAIL() << "Code: " << code << " Reason: " << reason;
+      }
     });
-    ws->binary.connect([&](ArrayRef<uint8_t> data, bool) {
+    ws->binary.connect([&](auto data, bool) {
       ++gotData;
       std::vector<uint8_t> recvData{data.begin(), data.end()};
       std::vector<uint8_t> expectData{0x03, 0x04};
@@ -119,8 +119,8 @@
   serverPipe->Listen([&]() {
     auto conn = serverPipe->Accept();
     auto server = WebSocketServer::Create(*conn);
-    server->connected.connect([&](StringRef, WebSocket& ws) {
-      ws.text.connect([&](StringRef data, bool) {
+    server->connected.connect([&](std::string_view, WebSocket& ws) {
+      ws.text.connect([&](std::string_view data, bool) {
         ++gotData;
         ASSERT_EQ(data, "hello");
       });
@@ -129,13 +129,14 @@
 
   clientPipe->Connect(pipeName, [&] {
     auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName);
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       Finish();
-      if (code != 1005 && code != 1006)
+      if (code != 1005 && code != 1006) {
         FAIL() << "Code: " << code << " Reason: " << reason;
+      }
     });
-    ws->open.connect([&, s = ws.get()](StringRef) {
-      s->SendText(uv::Buffer{"hello"}, [&](auto, uv::Error) {});
+    ws->open.connect([&, s = ws.get()](std::string_view) {
+      s->SendText({{"hello"}}, [&](auto, uv::Error) {});
       s->Close();
     });
   });
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketServerTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketServerTest.cpp
index d11fdda..83561bf 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketServerTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketServerTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WebSocket.h"  // NOLINT(build/include_order)
 
@@ -24,28 +21,36 @@
     serverPipe->Listen([this]() {
       auto conn = serverPipe->Accept();
       ws = WebSocket::CreateServer(*conn, "foo", "13");
-      if (setupWebSocket) setupWebSocket();
+      if (setupWebSocket) {
+        setupWebSocket();
+      }
     });
     clientPipe->Connect(pipeName, [this]() {
       clientPipe->StartRead();
       clientPipe->data.connect([this](uv::Buffer& buf, size_t size) {
-        StringRef data{buf.base, size};
+        std::string_view data{buf.base, size};
         if (!headersDone) {
           data = resp.Execute(data);
-          if (resp.HasError()) Finish();
+          if (resp.HasError()) {
+            Finish();
+          }
           ASSERT_EQ(resp.GetError(), HPE_OK)
               << http_errno_name(resp.GetError());
-          if (data.empty()) return;
+          if (data.empty()) {
+            return;
+          }
         }
-        wireData.insert(wireData.end(), data.bytes_begin(), data.bytes_end());
-        if (handleData) handleData(data);
+        wireData.insert(wireData.end(), data.begin(), data.end());
+        if (handleData) {
+          handleData(data);
+        }
       });
       clientPipe->end.connect([this]() { Finish(); });
     });
   }
 
   std::function<void()> setupWebSocket;
-  std::function<void(StringRef)> handleData;
+  std::function<void(std::string_view)> handleData;
   std::vector<uint8_t> wireData;
   std::shared_ptr<WebSocket> ws;
   HttpParser resp{HttpParser::kResponse};
@@ -59,8 +64,8 @@
 TEST_F(WebSocketServerTest, Terminate) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) { ws->Terminate(); });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->open.connect([&](std::string_view) { ws->Terminate(); });
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1006) << "reason: " << reason;
     });
@@ -75,8 +80,8 @@
 TEST_F(WebSocketServerTest, TerminateCode) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) { ws->Terminate(1000); });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->open.connect([&](std::string_view) { ws->Terminate(1000); });
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1000) << "reason: " << reason;
     });
@@ -91,8 +96,8 @@
 TEST_F(WebSocketServerTest, TerminateReason) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) { ws->Terminate(1000, "reason"); });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->open.connect([&](std::string_view) { ws->Terminate(1000, "reason"); });
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1000);
       ASSERT_EQ(reason, "reason");
@@ -112,16 +117,16 @@
 TEST_F(WebSocketServerTest, CloseBasic) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) { ws->Close(); });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->open.connect([&](std::string_view) { ws->Close(); });
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1005) << "reason: " << reason;
     });
   };
   // need to respond with close for server to finish shutdown
   auto message = BuildMessage(0x08, true, true, {});
-  handleData = [&](StringRef) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+  handleData = [&](std::string_view) {
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   };
 
   loop->Run();
@@ -134,8 +139,8 @@
 TEST_F(WebSocketServerTest, CloseCode) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) { ws->Close(1000); });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->open.connect([&](std::string_view) { ws->Close(1000); });
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1000) << "reason: " << reason;
     });
@@ -143,8 +148,8 @@
   // need to respond with close for server to finish shutdown
   const uint8_t contents[] = {0x03u, 0xe8u};
   auto message = BuildMessage(0x08, true, true, contents);
-  handleData = [&](StringRef) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+  handleData = [&](std::string_view) {
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   };
 
   loop->Run();
@@ -157,8 +162,8 @@
 TEST_F(WebSocketServerTest, CloseReason) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) { ws->Close(1000, "hangup"); });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->open.connect([&](std::string_view) { ws->Close(1000, "hangup"); });
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1000);
       ASSERT_EQ(reason, "hangup");
@@ -167,8 +172,8 @@
   // need to respond with close for server to finish shutdown
   const uint8_t contents[] = {0x03u, 0xe8u, 'h', 'a', 'n', 'g', 'u', 'p'};
   auto message = BuildMessage(0x08, true, true, contents);
-  handleData = [&](StringRef) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+  handleData = [&](std::string_view) {
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   };
 
   loop->Run();
@@ -185,14 +190,14 @@
 TEST_F(WebSocketServerTest, ReceiveCloseBasic) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1005) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(0x08, true, true, {});
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -206,7 +211,7 @@
 TEST_F(WebSocketServerTest, ReceiveCloseCode) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1000) << "reason: " << reason;
     });
@@ -214,7 +219,7 @@
   const uint8_t contents[] = {0x03u, 0xe8u};
   auto message = BuildMessage(0x08, true, true, contents);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -228,7 +233,7 @@
 TEST_F(WebSocketServerTest, ReceiveCloseReason) {
   int gotClosed = 0;
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotClosed;
       ASSERT_EQ(code, 1000);
       ASSERT_EQ(reason, "hangup");
@@ -237,7 +242,7 @@
   const uint8_t contents[] = {0x03u, 0xe8u, 'h', 'a', 'n', 'g', 'u', 'p'};
   auto message = BuildMessage(0x08, true, true, contents);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -266,14 +271,14 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(4, 0x03);
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(GetParam(), true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -297,14 +302,14 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(4, 0x03);
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(GetParam(), false, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -324,14 +329,14 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(4, 0x03);
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(0x00, false, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -344,7 +349,7 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(4, 0x03);
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
@@ -352,8 +357,7 @@
   auto message = BuildMessage(0x01, true, true, {});  // FIN=1
   auto message2 = BuildMessage(0x00, false, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write({uv::Buffer(message), uv::Buffer(message2)},
-                      [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}, {message2}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -365,7 +369,7 @@
 TEST_F(WebSocketServerTest, ReceiveFragmentInvalidIncomplete) {
   int gotCallback = 0;
   setupWebSocket = [&] {
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
@@ -374,9 +378,8 @@
   auto message2 = BuildMessage(0x00, false, true, {});
   auto message3 = BuildMessage(0x01, true, true, {});
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(
-        {uv::Buffer(message), uv::Buffer(message2), uv::Buffer(message3)},
-        [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}, {message2}, {message3}},
+                      [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -396,7 +399,7 @@
   combData.insert(combData.end(), data3.begin(), data3.end());
 
   setupWebSocket = [&] {
-    ws->binary.connect([&](ArrayRef<uint8_t> inData, bool fin) {
+    ws->binary.connect([&](auto inData, bool fin) {
       ++gotCallback;
       ws->Terminate();
       ASSERT_TRUE(fin);
@@ -409,9 +412,8 @@
   auto message2 = BuildMessage(0x00, false, true, data2);
   auto message3 = BuildMessage(0x00, true, true, data3);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(
-        {uv::Buffer(message), uv::Buffer(message2), uv::Buffer(message3)},
-        [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}, {message2}, {message3}},
+                      [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -432,7 +434,7 @@
 
   setupWebSocket = [&] {
     ws->SetCombineFragments(false);
-    ws->binary.connect([&](ArrayRef<uint8_t> inData, bool fin) {
+    ws->binary.connect([&](auto inData, bool fin) {
       std::vector<uint8_t> recvData{inData.begin(), inData.end()};
       switch (++gotCallback) {
         case 1:
@@ -459,9 +461,8 @@
   auto message2 = BuildMessage(0x00, false, true, data2);
   auto message3 = BuildMessage(0x00, true, true, data3);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(
-        {uv::Buffer(message), uv::Buffer(message2), uv::Buffer(message3)},
-        [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}, {message2}, {message3}},
+                      [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -483,14 +484,14 @@
       ws->Terminate();
       FAIL() << "Should not have gotten unmasked message";
     });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1009) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(0x01, true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -508,7 +509,7 @@
       ws->Terminate();
       FAIL() << "Should not have gotten unmasked message";
     });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1009) << "reason: " << reason;
     });
@@ -516,8 +517,7 @@
   auto message = BuildMessage(0x01, false, true, data);
   auto message2 = BuildMessage(0x00, true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write({uv::Buffer(message), uv::Buffer(message2)},
-                      [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}, {message2}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -539,8 +539,8 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), ' ');
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) {
-      ws->SendText(uv::Buffer(data), [&](auto bufs, uv::Error) {
+    ws->open.connect([&](std::string_view) {
+      ws->SendText({{data}}, [&](auto bufs, uv::Error) {
         ++gotCallback;
         ws->Terminate();
         ASSERT_FALSE(bufs.empty());
@@ -560,8 +560,8 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) {
-      ws->SendBinary(uv::Buffer(data), [&](auto bufs, uv::Error) {
+    ws->open.connect([&](std::string_view) {
+      ws->SendBinary({{data}}, [&](auto bufs, uv::Error) {
         ++gotCallback;
         ws->Terminate();
         ASSERT_FALSE(bufs.empty());
@@ -581,8 +581,8 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) {
-      ws->SendPing(uv::Buffer(data), [&](auto bufs, uv::Error) {
+    ws->open.connect([&](std::string_view) {
+      ws->SendPing({{data}}, [&](auto bufs, uv::Error) {
         ++gotCallback;
         ws->Terminate();
         ASSERT_FALSE(bufs.empty());
@@ -602,8 +602,8 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->open.connect([&](StringRef) {
-      ws->SendPong(uv::Buffer(data), [&](auto bufs, uv::Error) {
+    ws->open.connect([&](std::string_view) {
+      ws->SendPong({{data}}, [&](auto bufs, uv::Error) {
         ++gotCallback;
         ws->Terminate();
         ASSERT_FALSE(bufs.empty());
@@ -623,18 +623,18 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), ' ');
   setupWebSocket = [&] {
-    ws->text.connect([&](StringRef inData, bool fin) {
+    ws->text.connect([&](std::string_view inData, bool fin) {
       ++gotCallback;
       ws->Terminate();
       ASSERT_TRUE(fin);
       std::vector<uint8_t> recvData;
-      recvData.insert(recvData.end(), inData.bytes_begin(), inData.bytes_end());
+      recvData.insert(recvData.end(), inData.begin(), inData.end());
       ASSERT_EQ(data, recvData);
     });
   };
   auto message = BuildMessage(0x01, true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -646,7 +646,7 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->binary.connect([&](ArrayRef<uint8_t> inData, bool fin) {
+    ws->binary.connect([&](auto inData, bool fin) {
       ++gotCallback;
       ws->Terminate();
       ASSERT_TRUE(fin);
@@ -656,7 +656,7 @@
   };
   auto message = BuildMessage(0x02, true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -668,7 +668,7 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->ping.connect([&](ArrayRef<uint8_t> inData) {
+    ws->ping.connect([&](auto inData) {
       ++gotCallback;
       ws->Terminate();
       std::vector<uint8_t> recvData{inData.begin(), inData.end()};
@@ -677,7 +677,7 @@
   };
   auto message = BuildMessage(0x09, true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -689,7 +689,7 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), 0x03u);
   setupWebSocket = [&] {
-    ws->pong.connect([&](ArrayRef<uint8_t> inData) {
+    ws->pong.connect([&](auto inData) {
       ++gotCallback;
       ws->Terminate();
       std::vector<uint8_t> recvData{inData.begin(), inData.end()};
@@ -698,7 +698,7 @@
   };
   auto message = BuildMessage(0x0a, true, true, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
@@ -714,18 +714,18 @@
   int gotCallback = 0;
   std::vector<uint8_t> data(GetParam(), ' ');
   setupWebSocket = [&] {
-    ws->text.connect([&](StringRef, bool) {
+    ws->text.connect([&](std::string_view, bool) {
       ws->Terminate();
       FAIL() << "Should not have gotten unmasked message";
     });
-    ws->closed.connect([&](uint16_t code, StringRef reason) {
+    ws->closed.connect([&](uint16_t code, std::string_view reason) {
       ++gotCallback;
       ASSERT_EQ(code, 1002) << "reason: " << reason;
     });
   };
   auto message = BuildMessage(0x01, true, false, data);
   resp.headersComplete.connect([&](bool) {
-    clientPipe->Write(uv::Buffer(message), [&](auto bufs, uv::Error) {});
+    clientPipe->Write({{message}}, [&](auto bufs, uv::Error) {});
   });
 
   loop->Run();
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.cpp
index c27bac0..b5d5e4e 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.cpp
@@ -1,15 +1,13 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WebSocket.h"  // NOLINT(build/include_order)
 
 #include "WebSocketTest.h"
 
 #include "wpi/HttpParser.h"
+#include "wpi/StringExtras.h"
 
 namespace wpi {
 
@@ -38,15 +36,19 @@
     data.push_back(len & 0xff);
   } else {
     data.push_back(127u | (masking ? 0x80u : 0x00u));
-    for (int i = 56; i >= 0; i -= 8) data.push_back((len >> i) & 0xff);
+    for (int i = 56; i >= 0; i -= 8) {
+      data.push_back((len >> i) & 0xff);
+    }
   }
-  if (masking) data.insert(data.end(), &testMask[0], &testMask[4]);
+  if (masking) {
+    data.insert(data.end(), &testMask[0], &testMask[4]);
+  }
   return data;
 }
 
 std::vector<uint8_t> WebSocketTest::BuildMessage(uint8_t opcode, bool fin,
                                                  bool masking,
-                                                 ArrayRef<uint8_t> data) {
+                                                 span<const uint8_t> data) {
   auto finalData = BuildHeader(opcode, fin, masking, data.size());
   size_t headerSize = finalData.size();
   finalData.insert(finalData.end(), data.begin(), data.end());
@@ -56,7 +58,9 @@
     int n = 0;
     for (size_t i = headerSize, end = finalData.size(); i < end; ++i) {
       finalData[i] ^= mask[n++];
-      if (n >= 4) n = 0;
+      if (n >= 4) {
+        n = 0;
+      }
     }
   }
   return finalData;
@@ -64,17 +68,22 @@
 
 // If the message is masked, changes the mask to match the mask set by
 // BuildHeader() by unmasking and remasking.
-void WebSocketTest::AdjustMasking(MutableArrayRef<uint8_t> message) {
-  if (message.size() < 2) return;
-  if ((message[1] & 0x80) == 0) return;  // not masked
+void WebSocketTest::AdjustMasking(span<uint8_t> message) {
+  if (message.size() < 2) {
+    return;
+  }
+  if ((message[1] & 0x80) == 0) {
+    return;  // not masked
+  }
   size_t maskPos;
   uint8_t len = message[1] & 0x7f;
-  if (len == 126)
+  if (len == 126) {
     maskPos = 4;
-  else if (len == 127)
+  } else if (len == 127) {
     maskPos = 10;
-  else
+  } else {
     maskPos = 2;
+  }
   uint8_t mask[4] = {message[maskPos], message[maskPos + 1],
                      message[maskPos + 2], message[maskPos + 3]};
   message[maskPos] = testMask[0];
@@ -82,9 +91,11 @@
   message[maskPos + 2] = testMask[2];
   message[maskPos + 3] = testMask[3];
   int n = 0;
-  for (auto& ch : message.slice(maskPos + 4)) {
+  for (auto& ch : message.subspan(maskPos + 4)) {
     ch ^= mask[n] ^ testMask[n];
-    if (++n >= 4) n = 0;
+    if (++n >= 4) {
+      n = 0;
+    }
   }
 }
 
@@ -96,24 +107,24 @@
   int gotVersion = 0;
 
   HttpParser req{HttpParser::kRequest};
-  req.url.connect([](StringRef url) { ASSERT_EQ(url, "/test"); });
-  req.header.connect([&](StringRef name, StringRef value) {
-    if (name.equals_lower("host")) {
+  req.url.connect([](std::string_view url) { ASSERT_EQ(url, "/test"); });
+  req.header.connect([&](std::string_view name, std::string_view value) {
+    if (equals_lower(name, "host")) {
       ASSERT_EQ(value, pipeName);
       ++gotHost;
-    } else if (name.equals_lower("upgrade")) {
+    } else if (equals_lower(name, "upgrade")) {
       ASSERT_EQ(value, "websocket");
       ++gotUpgrade;
-    } else if (name.equals_lower("connection")) {
+    } else if (equals_lower(name, "connection")) {
       ASSERT_EQ(value, "Upgrade");
       ++gotConnection;
-    } else if (name.equals_lower("sec-websocket-key")) {
+    } else if (equals_lower(name, "sec-websocket-key")) {
       ++gotKey;
-    } else if (name.equals_lower("sec-websocket-version")) {
+    } else if (equals_lower(name, "sec-websocket-version")) {
       ASSERT_EQ(value, "13");
       ++gotVersion;
     } else {
-      FAIL() << "unexpected header " << name.str();
+      FAIL() << "unexpected header " << name;
     }
   });
   req.headersComplete.connect([&](bool) { Finish(); });
@@ -122,8 +133,10 @@
     auto conn = serverPipe->Accept();
     conn->StartRead();
     conn->data.connect([&](uv::Buffer& buf, size_t size) {
-      req.Execute(StringRef{buf.base, size});
-      if (req.HasError()) Finish();
+      req.Execute(std::string_view{buf.base, size});
+      if (req.HasError()) {
+        Finish();
+      }
       ASSERT_EQ(req.GetError(), HPE_OK) << http_errno_name(req.GetError());
     });
   });
@@ -133,7 +146,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotHost, 1);
   ASSERT_EQ(gotUpgrade, 1);
   ASSERT_EQ(gotConnection, 1);
@@ -145,11 +160,11 @@
   int gotExtra1 = 0;
   int gotExtra2 = 0;
   HttpParser req{HttpParser::kRequest};
-  req.header.connect([&](StringRef name, StringRef value) {
-    if (name.equals("Extra1")) {
+  req.header.connect([&](std::string_view name, std::string_view value) {
+    if (equals(name, "Extra1")) {
       ASSERT_EQ(value, "Data1");
       ++gotExtra1;
-    } else if (name.equals("Extra2")) {
+    } else if (equals(name, "Extra2")) {
       ASSERT_EQ(value, "Data2");
       ++gotExtra2;
     }
@@ -160,24 +175,28 @@
     auto conn = serverPipe->Accept();
     conn->StartRead();
     conn->data.connect([&](uv::Buffer& buf, size_t size) {
-      req.Execute(StringRef{buf.base, size});
-      if (req.HasError()) Finish();
+      req.Execute(std::string_view{buf.base, size});
+      if (req.HasError()) {
+        Finish();
+      }
       ASSERT_EQ(req.GetError(), HPE_OK) << http_errno_name(req.GetError());
     });
   });
   clientPipe->Connect(pipeName, [&]() {
     WebSocket::ClientOptions options;
-    SmallVector<std::pair<StringRef, StringRef>, 4> extraHeaders;
+    SmallVector<std::pair<std::string_view, std::string_view>, 4> extraHeaders;
     extraHeaders.emplace_back("Extra1", "Data1");
     extraHeaders.emplace_back("Extra2", "Data2");
     options.extraHeaders = extraHeaders;
-    auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName,
-                                      ArrayRef<StringRef>{}, options);
+    auto ws =
+        WebSocket::CreateClient(*clientPipe, "/test", pipeName, {}, options);
   });
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotExtra1, 1);
   ASSERT_EQ(gotExtra2, 1);
 }
@@ -188,9 +207,9 @@
   clientPipe->Connect(pipeName, [&]() {
     WebSocket::ClientOptions options;
     options.handshakeTimeout = uv::Timer::Time{100};
-    auto ws = WebSocket::CreateClient(*clientPipe, "/test", pipeName,
-                                      ArrayRef<StringRef>{}, options);
-    ws->closed.connect([&](uint16_t code, StringRef) {
+    auto ws =
+        WebSocket::CreateClient(*clientPipe, "/test", pipeName, {}, options);
+    ws->closed.connect([&](uint16_t code, std::string_view) {
       Finish();
       ++gotClosed;
       ASSERT_EQ(code, 1006);
@@ -199,7 +218,9 @@
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotClosed, 1);
 }
 
@@ -211,22 +232,22 @@
   int gotOpen = 0;
 
   HttpParser resp{HttpParser::kResponse};
-  resp.status.connect([&](StringRef status) {
+  resp.status.connect([&](std::string_view status) {
     ++gotStatus;
     ASSERT_EQ(resp.GetStatusCode(), 101u) << "status: " << status;
   });
-  resp.header.connect([&](StringRef name, StringRef value) {
-    if (name.equals_lower("upgrade")) {
+  resp.header.connect([&](std::string_view name, std::string_view value) {
+    if (equals_lower(name, "upgrade")) {
       ASSERT_EQ(value, "websocket");
       ++gotUpgrade;
-    } else if (name.equals_lower("connection")) {
+    } else if (equals_lower(name, "connection")) {
       ASSERT_EQ(value, "Upgrade");
       ++gotConnection;
-    } else if (name.equals_lower("sec-websocket-accept")) {
+    } else if (equals_lower(name, "sec-websocket-accept")) {
       ASSERT_EQ(value, "s3pPLMBiTxaQ9kYGzzhZRbK+xOo=");
       ++gotAccept;
     } else {
-      FAIL() << "unexpected header " << name.str();
+      FAIL() << "unexpected header " << name;
     }
   });
   resp.headersComplete.connect([&](bool) { Finish(); });
@@ -234,7 +255,7 @@
   serverPipe->Listen([&]() {
     auto conn = serverPipe->Accept();
     auto ws = WebSocket::CreateServer(*conn, "dGhlIHNhbXBsZSBub25jZQ==", "13");
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       ++gotOpen;
       ASSERT_TRUE(protocol.empty());
     });
@@ -242,15 +263,19 @@
   clientPipe->Connect(pipeName, [&] {
     clientPipe->StartRead();
     clientPipe->data.connect([&](uv::Buffer& buf, size_t size) {
-      resp.Execute(StringRef{buf.base, size});
-      if (resp.HasError()) Finish();
+      resp.Execute(std::string_view{buf.base, size});
+      if (resp.HasError()) {
+        Finish();
+      }
       ASSERT_EQ(resp.GetError(), HPE_OK) << http_errno_name(resp.GetError());
     });
   });
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotStatus, 1);
   ASSERT_EQ(gotUpgrade, 1);
   ASSERT_EQ(gotConnection, 1);
@@ -263,8 +288,8 @@
   int gotOpen = 0;
 
   HttpParser resp{HttpParser::kResponse};
-  resp.header.connect([&](StringRef name, StringRef value) {
-    if (name.equals_lower("sec-websocket-protocol")) {
+  resp.header.connect([&](std::string_view name, std::string_view value) {
+    if (equals_lower(name, "sec-websocket-protocol")) {
       ++gotProtocol;
       ASSERT_EQ(value, "myProtocol");
     }
@@ -274,7 +299,7 @@
   serverPipe->Listen([&]() {
     auto conn = serverPipe->Accept();
     auto ws = WebSocket::CreateServer(*conn, "foo", "13", "myProtocol");
-    ws->open.connect([&](StringRef protocol) {
+    ws->open.connect([&](std::string_view protocol) {
       ++gotOpen;
       ASSERT_EQ(protocol, "myProtocol");
     });
@@ -282,15 +307,19 @@
   clientPipe->Connect(pipeName, [&] {
     clientPipe->StartRead();
     clientPipe->data.connect([&](uv::Buffer& buf, size_t size) {
-      resp.Execute(StringRef{buf.base, size});
-      if (resp.HasError()) Finish();
+      resp.Execute(std::string_view{buf.base, size});
+      if (resp.HasError()) {
+        Finish();
+      }
       ASSERT_EQ(resp.GetError(), HPE_OK) << http_errno_name(resp.GetError());
     });
   });
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotProtocol, 1);
   ASSERT_EQ(gotOpen, 1);
 }
@@ -301,19 +330,19 @@
   int gotUpgrade = 0;
 
   HttpParser resp{HttpParser::kResponse};
-  resp.status.connect([&](StringRef status) {
+  resp.status.connect([&](std::string_view status) {
     ++gotStatus;
     ASSERT_EQ(resp.GetStatusCode(), 426u) << "status: " << status;
   });
-  resp.header.connect([&](StringRef name, StringRef value) {
-    if (name.equals_lower("sec-websocket-version")) {
+  resp.header.connect([&](std::string_view name, std::string_view value) {
+    if (equals_lower(name, "sec-websocket-version")) {
       ++gotVersion;
       ASSERT_EQ(value, "13");
-    } else if (name.equals_lower("upgrade")) {
+    } else if (equals_lower(name, "upgrade")) {
       ++gotUpgrade;
       ASSERT_EQ(value, "WebSocket");
     } else {
-      FAIL() << "unexpected header " << name.str();
+      FAIL() << "unexpected header " << name;
     }
   });
   resp.headersComplete.connect([&](bool) { Finish(); });
@@ -321,7 +350,7 @@
   serverPipe->Listen([&] {
     auto conn = serverPipe->Accept();
     auto ws = WebSocket::CreateServer(*conn, "foo", "14");
-    ws->open.connect([&](StringRef) {
+    ws->open.connect([&](std::string_view) {
       Finish();
       FAIL();
     });
@@ -329,15 +358,19 @@
   clientPipe->Connect(pipeName, [&] {
     clientPipe->StartRead();
     clientPipe->data.connect([&](uv::Buffer& buf, size_t size) {
-      resp.Execute(StringRef{buf.base, size});
-      if (resp.HasError()) Finish();
+      resp.Execute(std::string_view{buf.base, size});
+      if (resp.HasError()) {
+        Finish();
+      }
       ASSERT_EQ(resp.GetError(), HPE_OK) << http_errno_name(resp.GetError());
     });
   });
 
   loop->Run();
 
-  if (HasFatalFailure()) return;
+  if (HasFatalFailure()) {
+    return;
+  }
   ASSERT_EQ(gotStatus, 1);
   ASSERT_EQ(gotVersion, 1);
   ASSERT_EQ(gotUpgrade, 1);
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.h b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.h
index 8b40440..fda4473 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.h
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/WebSocketTest.h
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #pragma once
 
@@ -12,7 +9,7 @@
 #include <vector>
 
 #include "gtest/gtest.h"
-#include "wpi/ArrayRef.h"
+#include "wpi/span.h"
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/Pipe.h"
 #include "wpi/uv/Timer.h"
@@ -51,7 +48,7 @@
     failTimer->Unreference();
   }
 
-  ~WebSocketTest() { Finish(); }
+  ~WebSocketTest() override { Finish(); }
 
   void Finish() {
     loop->Walk([](uv::Handle& it) { it.Close(); });
@@ -61,8 +58,8 @@
                                           bool masking, uint64_t len);
   static std::vector<uint8_t> BuildMessage(uint8_t opcode, bool fin,
                                            bool masking,
-                                           ArrayRef<uint8_t> data);
-  static void AdjustMasking(MutableArrayRef<uint8_t> message);
+                                           span<const uint8_t> data);
+  static void AdjustMasking(span<uint8_t> message);
   static const uint8_t testMask[4];
 
   std::shared_ptr<uv::Loop> loop;
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/WorkerThreadTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/WorkerThreadTest.cpp
index 610029c..9bc9a17 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/WorkerThreadTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/WorkerThreadTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/WorkerThread.h"  // NOLINT(build/include_order)
 
@@ -15,14 +12,14 @@
 
 namespace wpi {
 
-TEST(WorkerThread, Future) {
+TEST(WorkerThreadTest, Future) {
   WorkerThread<int(bool)> worker;
   future<int> f =
       worker.QueueWork([](bool v) -> int { return v ? 1 : 2; }, true);
   ASSERT_EQ(f.get(), 1);
 }
 
-TEST(WorkerThread, FutureVoid) {
+TEST(WorkerThreadTest, FutureVoid) {
   int callbacks = 0;
   WorkerThread<void(int)> worker;
   future<void> f = worker.QueueWork(
@@ -35,7 +32,7 @@
   ASSERT_EQ(callbacks, 1);
 }
 
-TEST(WorkerThread, Loop) {
+TEST(WorkerThreadTest, Loop) {
   int callbacks = 0;
   WorkerThread<int(bool)> worker;
   auto loop = uv::Loop::Create();
@@ -53,7 +50,7 @@
   ASSERT_EQ(callbacks, 1);
 }
 
-TEST(WorkerThread, LoopVoid) {
+TEST(WorkerThreadTest, LoopVoid) {
   int callbacks = 0;
   WorkerThread<void(bool)> worker;
   auto loop = uv::Loop::Create();
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/future_test.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/future_test.cpp
index b9b79a8..8817f2d 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/future_test.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/future_test.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/future.h"  // NOLINT(build/include_order)
 
@@ -13,7 +10,7 @@
 
 namespace wpi {
 
-TEST(Future, Then) {
+TEST(FutureTest, Then) {
   promise<bool> inPromise;
   future<int> outFuture =
       inPromise.get_future().then([](bool v) { return v ? 5 : 6; });
@@ -22,7 +19,7 @@
   ASSERT_EQ(outFuture.get(), 5);
 }
 
-TEST(Future, ThenSame) {
+TEST(FutureTest, ThenSame) {
   promise<bool> inPromise;
   future<bool> outFuture =
       inPromise.get_future().then([](bool v) { return !v; });
@@ -31,7 +28,7 @@
   ASSERT_EQ(outFuture.get(), false);
 }
 
-TEST(Future, ThenFromVoid) {
+TEST(FutureTest, ThenFromVoid) {
   promise<void> inPromise;
   future<int> outFuture = inPromise.get_future().then([] { return 5; });
 
@@ -39,7 +36,7 @@
   ASSERT_EQ(outFuture.get(), 5);
 }
 
-TEST(Future, ThenToVoid) {
+TEST(FutureTest, ThenToVoid) {
   promise<bool> inPromise;
   future<void> outFuture = inPromise.get_future().then([](bool v) {});
 
@@ -47,7 +44,7 @@
   ASSERT_TRUE(outFuture.is_ready());
 }
 
-TEST(Future, ThenVoidVoid) {
+TEST(FutureTest, ThenVoidVoid) {
   promise<void> inPromise;
   future<void> outFuture = inPromise.get_future().then([] {});
 
@@ -55,7 +52,7 @@
   ASSERT_TRUE(outFuture.is_ready());
 }
 
-TEST(Future, Implicit) {
+TEST(FutureTest, Implicit) {
   promise<bool> inPromise;
   future<int> outFuture = inPromise.get_future();
 
@@ -63,7 +60,7 @@
   ASSERT_EQ(outFuture.get(), 1);
 }
 
-TEST(Future, MoveSame) {
+TEST(FutureTest, MoveSame) {
   promise<bool> inPromise;
   future<bool> outFuture1 = inPromise.get_future();
   future<bool> outFuture(std::move(outFuture1));
@@ -72,7 +69,7 @@
   ASSERT_EQ(outFuture.get(), true);
 }
 
-TEST(Future, MoveVoid) {
+TEST(FutureTest, MoveVoid) {
   promise<void> inPromise;
   future<void> outFuture1 = inPromise.get_future();
   future<void> outFuture(std::move(outFuture1));
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/hostname.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/hostname.cpp
index d5ec535..29cceb1 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/hostname.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/hostname.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/hostname.h"
 
@@ -12,7 +9,9 @@
 #include "wpi/SmallVector.h"
 
 namespace wpi {
-TEST(HostNameTest, HostNameNotEmpty) { ASSERT_NE(GetHostname(), ""); }
+TEST(HostNameTest, HostNameNotEmpty) {
+  ASSERT_NE(GetHostname(), "");
+}
 TEST(HostNameTest, HostNameNotEmptySmallVector) {
   SmallVector<char, 256> name;
   ASSERT_NE(GetHostname(name), "");
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-readme.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-readme.cpp
index ecb7f79..14d2b1b 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-readme.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-readme.cpp
@@ -200,7 +200,7 @@
 {
     std::vector<int> c_vector {1, 2, 3, 4};
     json j_vec(c_vector);
-    json j_vec2(wpi::makeArrayRef(c_vector));
+    json j_vec2(wpi::span<const int>(c_vector.data(), c_vector.size()));
     // [1, 2, 3, 4]
 
     std::deque<float> c_deque {1.2f, 2.3f, 3.4f, 5.6f};
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-unicode.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-unicode.cpp
index e0179b6..6755fe2 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-unicode.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/json/unit-unicode.cpp
@@ -37,7 +37,7 @@
 #include "unit-json.h"
 using wpi::json;
 
-#include "wpi/Format.h"
+#include "fmt/format.h"
 #include "wpi/StringExtras.h"
 #include "wpi/raw_ostream.h"
 
@@ -919,11 +919,7 @@
     // reverse solidus, followed by the lowercase letter u, followed
     // by four hexadecimal digits that encode the character's code
     // point
-    std::string s;
-    wpi::raw_string_ostream ss(s);
-    ss << "\\u" << wpi::format_hex_no_prefix(cp, 4);
-    ss.flush();
-    return s;
+    return fmt::format("\\u{:04x}", cp);
 }
 
 // correct sequences
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/leb128Test.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/leb128Test.cpp
index a3a7ed0..cc537ae 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/leb128Test.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/leb128Test.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 //===- llvm/unittest/Support/LEB128Test.cpp - LEB128 function tests -------===//
 //
@@ -17,23 +14,23 @@
 #include <stdint.h>
 
 #include <string>
+#include <string_view>
 
 #include "gtest/gtest.h"
 #include "wpi/SmallString.h"
-#include "wpi/StringRef.h"
 #include "wpi/leb128.h"
 #include "wpi/raw_istream.h"
 
 namespace wpi {
 
 TEST(LEB128Test, WriteUleb128) {
-#define EXPECT_ULEB128_EQ(EXPECTED, VALUE, PAD)         \
-  do {                                                  \
-    StringRef expected(EXPECTED, sizeof(EXPECTED) - 1); \
-    SmallString<32> buf;                                \
-    size_t size = WriteUleb128(buf, VALUE);             \
-    EXPECT_EQ(size, buf.size());                        \
-    EXPECT_EQ(expected, buf.str());                     \
+#define EXPECT_ULEB128_EQ(EXPECTED, VALUE, PAD)                \
+  do {                                                         \
+    std::string_view expected(EXPECTED, sizeof(EXPECTED) - 1); \
+    SmallString<32> buf;                                       \
+    size_t size = WriteUleb128(buf, VALUE);                    \
+    EXPECT_EQ(size, buf.size());                               \
+    EXPECT_EQ(expected, buf.str());                            \
   } while (0)
 
   // Write ULEB128
@@ -48,6 +45,8 @@
   EXPECT_ULEB128_EQ("\xff\x01", 0xff, 0);
   EXPECT_ULEB128_EQ("\x80\x02", 0x100, 0);
   EXPECT_ULEB128_EQ("\x81\x02", 0x101, 0);
+  EXPECT_ULEB128_EQ("\x80\x41", 0x2080, 0);
+  EXPECT_ULEB128_EQ("\x80\xc1\x80\x80\x10", 0x100002080, 0);
 
 #undef EXPECT_ULEB128_EQ
 }
@@ -73,7 +72,8 @@
   EXPECT_READ_ULEB128_EQ(0xffu, "\xff\x01");
   EXPECT_READ_ULEB128_EQ(0x100u, "\x80\x02");
   EXPECT_READ_ULEB128_EQ(0x101u, "\x81\x02");
-  EXPECT_READ_ULEB128_EQ(8320u, "\x80\xc1\x80\x80\x10");
+  EXPECT_READ_ULEB128_EQ(0x2080u, "\x80\x41");
+  EXPECT_READ_ULEB128_EQ(0x100002080u, "\x80\xc1\x80\x80\x10");
 
 #undef EXPECT_READ_ULEB128_EQ
 }
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/main.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/main.cpp
index 1e5ecf0..09072ee 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/main.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/main.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2015-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "gtest/gtest.h"
 
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/priority_mutex_test.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/priority_mutex_test.cpp
index 448a757..832e8f3 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/priority_mutex_test.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/priority_mutex_test.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2016-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include <wpi/priority_mutex.h>  // NOLINT(build/include_order)
 
@@ -58,7 +55,7 @@
 
 void SetThreadRealtimePriorityOrDie(int32_t priority) {
   struct sched_param param;
-  // Set realtime priority for this thread
+  // Set real-time priority for this thread
   param.sched_priority = priority + sched_get_priority_min(SCHED_RR);
   ASSERT_EQ(pthread_setschedparam(pthread_self(), SCHED_RR, &param), 0)
       << ": Failed to set scheduler priority.";
@@ -225,7 +222,7 @@
 // TODO: Fix roborio permissions to run as root.
 
 // Priority inversion test.
-TEST(MutexTest, DISABLED_PriorityInversionTest) {
+TEST(MutexTest, DISABLED_PriorityInversion) {
   InversionTestRunner<priority_mutex> runner;
   std::thread runner_thread(std::ref(runner));
   runner_thread.join();
@@ -233,7 +230,7 @@
 }
 
 // Verify that the non-priority inversion mutex doesn't pass the test.
-TEST(MutexTest, DISABLED_StdMutexPriorityInversionTest) {
+TEST(MutexTest, DISABLED_StdMutexPriorityInversion) {
   InversionTestRunner<std::mutex> runner;
   std::thread runner_thread(std::ref(runner));
   runner_thread.join();
@@ -250,7 +247,7 @@
 }
 
 // Priority inversion test.
-TEST(MutexTest, DISABLED_ReentrantPriorityInversionTest) {
+TEST(MutexTest, DISABLED_ReentrantPriorityInversion) {
   InversionTestRunner<priority_recursive_mutex> runner;
   std::thread runner_thread(std::ref(runner));
   runner_thread.join();
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/raw_uv_stream_test.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/raw_uv_stream_test.cpp
index d1a0f6c..985fb8b 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/raw_uv_stream_test.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/raw_uv_stream_test.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/raw_uv_ostream.h"  // NOLINT(build/include_order)
 
@@ -22,6 +19,10 @@
   ASSERT_EQ(bufs[0].base[1], '2');
   ASSERT_EQ(bufs[0].base[2], '3');
   ASSERT_EQ(bufs[0].base[3], '4');
+
+  for (auto& buf : bufs) {
+    buf.Deallocate();
+  }
 }
 
 TEST(RawUvStreamTest, BoundaryWrite) {
@@ -34,6 +35,10 @@
   ASSERT_EQ(bufs.size(), 1u);
   os << "56";
   ASSERT_EQ(bufs.size(), 2u);
+
+  for (auto& buf : bufs) {
+    buf.Deallocate();
+  }
 }
 
 TEST(RawUvStreamTest, LargeWrite) {
@@ -43,6 +48,10 @@
   ASSERT_EQ(bufs.size(), 2u);
   ASSERT_EQ(bufs[1].len, 2u);
   ASSERT_EQ(bufs[1].base[0], '5');
+
+  for (auto& buf : bufs) {
+    buf.Deallocate();
+  }
 }
 
 TEST(RawUvStreamTest, PrevDataWrite) {
@@ -53,6 +62,10 @@
   ASSERT_EQ(bufs.size(), 2u);
   ASSERT_EQ(bufs[0].len, 1024u);
   ASSERT_EQ(bufs[1].len, 4u);
+
+  for (auto& buf : bufs) {
+    buf.Deallocate();
+  }
 }
 
 }  // namespace wpi
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sha1Test.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sha1Test.cpp
index 08d85fe..0257431 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sha1Test.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sha1Test.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2017-2018 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
     test_sha1.cpp - test program of
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/function-traits.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/function-traits.cpp
index 240e688..f31e296 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/function-traits.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/function-traits.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 
@@ -119,7 +116,7 @@
 
 namespace wpi {
 
-TEST(Signal, FunctionTraits) {
+TEST(SignalTest, FunctionTraits) {
   auto l1 = [](int, char, float) {};
   auto l2 = [&](int, char, float) mutable {};
   auto l3 = [&](auto...) mutable {};
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/recursive.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/recursive.cpp
index 85ae9a9..82613f2 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/recursive.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/recursive.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 
@@ -42,7 +39,7 @@
 
 template <typename T>
 struct object {
-  object(T i) : v{i} {}  // NOLINT(runtime/explicit)
+  object(T i) : v{i} {}  // NOLINT
 
   void inc_val(const T& i) {
     if (i != v) {
@@ -66,7 +63,7 @@
 
 namespace wpi {
 
-TEST(Signal, Recursive) {
+TEST(SignalTest, Recursive) {
   object<int> i1(-1);
   object<int> i2(10);
 
@@ -78,7 +75,7 @@
   ASSERT_EQ(i1.v, i2.v);
 }
 
-TEST(Signal, SelfRecursive) {
+TEST(SignalTest, SelfRecursive) {
   int i = 0;
 
   wpi::sig::Signal_r<int> s;
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-extended.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-extended.cpp
index 1ebbc8e..a0dbdfc 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-extended.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-extended.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 
@@ -71,7 +68,7 @@
 
 namespace wpi {
 
-TEST(SignalExtended, FreeConnection) {
+TEST(SignalExtendedTest, FreeConnection) {
   sum = 0;
   Signal<int> sig;
   sig.connect_extended(f);
@@ -82,7 +79,7 @@
   ASSERT_EQ(sum, 1);
 }
 
-TEST(SignalExtended, StaticConnection) {
+TEST(SignalExtendedTest, StaticConnection) {
   sum = 0;
   Signal<int> sig;
   sig.connect_extended(&s::sf);
@@ -93,7 +90,7 @@
   ASSERT_EQ(sum, 1);
 }
 
-TEST(SignalExtended, PmfConnection) {
+TEST(SignalExtendedTest, PmfConnection) {
   sum = 0;
   Signal<int> sig;
   s p;
@@ -105,7 +102,7 @@
   ASSERT_EQ(sum, 1);
 }
 
-TEST(SignalExtended, FunctionObjectConnection) {
+TEST(SignalExtendedTest, FunctionObjectConnection) {
   sum = 0;
   Signal<int> sig;
   sig.connect_extended(o{});
@@ -116,7 +113,7 @@
   ASSERT_EQ(sum, 1);
 }
 
-TEST(SignalExtended, LambdaConnection) {
+TEST(SignalExtendedTest, LambdaConnection) {
   sum = 0;
   Signal<int> sig;
 
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-threaded.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-threaded.cpp
index c4f7cdb..00001a8 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-threaded.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-threaded.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 
@@ -48,16 +45,22 @@
 
 std::atomic<int> sum{0};
 
-void f(int i) { sum += i; }
+void f(int i) {
+  sum += i;
+}
 
 void emit_many(Signal_mt<int>& sig) {
-  for (int i = 0; i < 10000; ++i) sig(1);
+  for (int i = 0; i < 10000; ++i) {
+    sig(1);
+  }
 }
 
 void connect_emit(Signal_mt<int>& sig) {
   for (int i = 0; i < 100; ++i) {
     auto s = sig.connect_scoped(f);
-    for (int j = 0; j < 100; ++j) sig(1);
+    for (int j = 0; j < 100; ++j) {
+      sig(1);
+    }
   }
 }
 
@@ -65,27 +68,35 @@
 
 namespace wpi {
 
-TEST(Signal, ThreadedMix) {
+TEST(SignalTest, ThreadedMix) {
   sum = 0;
 
   Signal_mt<int> sig;
 
   std::array<std::thread, 10> threads;
-  for (auto& t : threads) t = std::thread(connect_emit, std::ref(sig));
+  for (auto& t : threads) {
+    t = std::thread(connect_emit, std::ref(sig));
+  }
 
-  for (auto& t : threads) t.join();
+  for (auto& t : threads) {
+    t.join();
+  }
 }
 
-TEST(Signal, ThreadedEmission) {
+TEST(SignalTest, ThreadedEmission) {
   sum = 0;
 
   Signal_mt<int> sig;
   sig.connect(f);
 
   std::array<std::thread, 10> threads;
-  for (auto& t : threads) t = std::thread(emit_many, std::ref(sig));
+  for (auto& t : threads) {
+    t = std::thread(emit_many, std::ref(sig));
+  }
 
-  for (auto& t : threads) t.join();
+  for (auto& t : threads) {
+    t.join();
+  }
 
   ASSERT_EQ(sum, 100000);
 }
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-tracking.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-tracking.cpp
index 89ffd36..d05ee1a 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-tracking.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal-tracking.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 
@@ -48,7 +45,9 @@
 
 int sum = 0;
 
-void f1(int i) { sum += i; }
+void f1(int i) {
+  sum += i;
+}
 struct o1 {
   void operator()(int i) { sum += 2 * i; }
 };
@@ -73,7 +72,7 @@
 
 namespace wpi {
 
-TEST(Signal, TrackShared) {
+TEST(SignalTest, TrackShared) {
   sum = 0;
   Signal<int> sig;
 
@@ -96,7 +95,7 @@
   ASSERT_EQ(sum, 5);
 }
 
-TEST(Signal, TrackOther) {
+TEST(SignalTest, TrackOther) {
   sum = 0;
   Signal<int> sig;
 
@@ -119,7 +118,7 @@
   ASSERT_EQ(sum, 5);
 }
 
-TEST(Signal, TrackOverloadedFunctionObject) {
+TEST(SignalTest, TrackOverloadedFunctionObject) {
   sum = 0;
   Signal<int> sig;
   Signal<double> sig1;
@@ -144,7 +143,7 @@
   ASSERT_EQ(sum, 5);
 }
 
-TEST(Signal, TrackGenericLambda) {
+TEST(SignalTest, TrackGenericLambda) {
   std::stringstream s;
 
   auto f = [&](auto a, auto... args) {
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal.cpp
index a4f9208..cc2ff80 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/sigslot/signal.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /*
 
@@ -48,8 +45,12 @@
 
 int sum = 0;
 
-void f1(int i) { sum += i; }
-void f2(int i) /*noexcept*/ { sum += 2 * i; }
+void f1(int i) {
+  sum += i;
+}
+void f2(int i) /*noexcept*/ {
+  sum += 2 * i;
+}
 
 struct s {
   static void s1(int i) { sum += i; }
@@ -99,7 +100,7 @@
 
 namespace wpi {
 
-TEST(Signal, FreeConnection) {
+TEST(SignalTest, FreeConnection) {
   sum = 0;
   Signal<int> sig;
 
@@ -112,7 +113,7 @@
   ASSERT_EQ(sum, 4);
 }
 
-TEST(Signal, StaticConnection) {
+TEST(SignalTest, StaticConnection) {
   sum = 0;
   Signal<int> sig;
 
@@ -125,7 +126,7 @@
   ASSERT_EQ(sum, 4);
 }
 
-TEST(Signal, PmfConnection) {
+TEST(SignalTest, PmfConnection) {
   sum = 0;
   Signal<int> sig;
   s p;
@@ -143,7 +144,7 @@
   ASSERT_EQ(sum, 8);
 }
 
-TEST(Signal, ConstPmfConnection) {
+TEST(SignalTest, ConstPmfConnection) {
   sum = 0;
   Signal<int> sig;
   const s p;
@@ -157,7 +158,7 @@
   ASSERT_EQ(sum, 4);
 }
 
-TEST(Signal, FunctionObjectConnection) {
+TEST(SignalTest, FunctionObjectConnection) {
   sum = 0;
   Signal<int> sig;
 
@@ -174,7 +175,7 @@
   ASSERT_EQ(sum, 8);
 }
 
-TEST(Signal, OverloadedFunctionObjectConnection) {
+TEST(SignalTest, OverloadedFunctionObjectConnection) {
   sum = 0;
   Signal<int> sig;
   Signal<double> sig1;
@@ -188,7 +189,7 @@
   ASSERT_EQ(sum, 5);
 }
 
-TEST(Signal, LambdaConnection) {
+TEST(SignalTest, LambdaConnection) {
   sum = 0;
   Signal<int> sig;
 
@@ -201,7 +202,7 @@
   ASSERT_EQ(sum, 4);
 }
 
-TEST(Signal, GenericLambdaConnection) {
+TEST(SignalTest, GenericLambdaConnection) {
   std::stringstream s;
 
   auto f = [&](auto a, auto... args) {
@@ -228,7 +229,7 @@
   ASSERT_EQ(s.str(), "1foo4.1");
 }
 
-TEST(Signal, LvalueEmission) {
+TEST(SignalTest, LvalueEmission) {
   sum = 0;
   Signal<int> sig;
 
@@ -242,7 +243,7 @@
   ASSERT_EQ(sum, 4);
 }
 
-TEST(Signal, Mutation) {
+TEST(SignalTest, Mutation) {
   int res = 0;
   Signal<int&> sig;
 
@@ -255,7 +256,7 @@
   ASSERT_EQ(res, 4);
 }
 
-TEST(Signal, CompatibleArgs) {
+TEST(SignalTest, CompatibleArgs) {
   long ll = 0;  // NOLINT(runtime/int)
   std::string ss;
   short ii = 0;  // NOLINT(runtime/int)
@@ -275,7 +276,7 @@
   ASSERT_EQ(ii, 1);
 }
 
-TEST(Signal, Disconnection) {
+TEST(SignalTest, Disconnection) {
   // test removing only connected
   {
     sum = 0;
@@ -327,7 +328,7 @@
   }
 }
 
-TEST(Signal, ScopedConnection) {
+TEST(SignalTest, ScopedConnection) {
   sum = 0;
   Signal<int> sig;
 
@@ -360,7 +361,7 @@
   ASSERT_EQ(sum, 4);
 }
 
-TEST(Signal, ConnectionBlocking) {
+TEST(SignalTest, ConnectionBlocking) {
   sum = 0;
   Signal<int> sig;
 
@@ -378,7 +379,7 @@
   ASSERT_EQ(sum, 8);
 }
 
-TEST(Signal, ConnectionBlocker) {
+TEST(SignalTest, ConnectionBlocker) {
   sum = 0;
   Signal<int> sig;
 
@@ -397,7 +398,7 @@
   ASSERT_EQ(sum, 8);
 }
 
-TEST(Signal, SignalBlocking) {
+TEST(SignalTest, SignalBlocking) {
   sum = 0;
   Signal<int> sig;
 
@@ -415,7 +416,7 @@
   ASSERT_EQ(sum, 6);
 }
 
-TEST(Signal, AllDisconnection) {
+TEST(SignalTest, AllDisconnection) {
   sum = 0;
   Signal<int> sig;
 
@@ -429,7 +430,7 @@
   ASSERT_EQ(sum, 3);
 }
 
-TEST(Signal, ConnectionCopyingMoving) {
+TEST(SignalTest, ConnectionCopyingMoving) {
   sum = 0;
   Signal<int> sig;
 
@@ -458,7 +459,7 @@
   ASSERT_EQ(sum, 9);
 }
 
-TEST(Signal, ScopedConnectionMoving) {
+TEST(SignalTest, ScopedConnectionMoving) {
   sum = 0;
   Signal<int> sig;
 
@@ -484,7 +485,7 @@
   ASSERT_EQ(sum, 10);
 }
 
-TEST(Signal, SignalMoving) {
+TEST(SignalTest, SignalMoving) {
   sum = 0;
   Signal<int> sig;
 
@@ -506,7 +507,7 @@
 template <typename T>
 struct object {
   object();
-  object(T i) : v{i} {}  // NOLINT(runtime/explicit)
+  object(T i) : v{i} {}  // NOLINT
 
   const T& val() const { return v; }
   T& val() { return v; }
@@ -524,7 +525,7 @@
   Signal<T> s;
 };
 
-TEST(Signal, Loop) {
+TEST(SignalTest, Loop) {
   object<int> i1(0);
   object<int> i2(3);
 
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_conversions.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_conversions.cpp
new file mode 100644
index 0000000..fa111a7
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_conversions.cpp
@@ -0,0 +1,46 @@
+#if __has_include(<span>)
+#include <span>
+#endif
+#include "wpi/span.h"
+
+#include "gtest/gtest.h"
+
+void func1(wpi::span<const int> x) {}
+#ifdef __cpp_lib_span
+void func2(std::span<const int> x) {}
+#endif
+
+void func4(wpi::span<int> x) {}
+#ifdef __cpp_lib_span
+void func5(std::span<int> x) {}
+#endif
+
+TEST(Span, ConvertStdSpan) {
+  func1(wpi::span<const int>{});
+  func1(wpi::span<int>{});
+#ifdef __cpp_lib_span
+  func1(std::span<const int>{});
+  func1(std::span<int>{});
+#endif
+
+#ifdef __cpp_lib_span
+  func2(wpi::span<const int>{});
+  func2(wpi::span<int>{});
+  func2(std::span<const int>{});
+  func2(std::span<int>{});
+#endif
+
+  //func4(wpi::span<const int>{});
+  func4(wpi::span<int>{});
+#ifdef __cpp_lib_span
+  //func4(std::span<const int>{});
+  func4(std::span<int>{});
+#endif
+
+#ifdef __cpp_lib_span
+  //func5(wpi::span<const int>{});
+  func5(wpi::span<int>{});
+  //func5(std::span<const int>{});
+  func5(std::span<int>{});
+#endif
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_deduction_guides.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_deduction_guides.cpp
new file mode 100644
index 0000000..9851b5e
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_deduction_guides.cpp
@@ -0,0 +1,98 @@
+
+#include "wpi/span.h"
+
+#include "gtest/gtest.h"
+
+#include <vector>
+
+using wpi::span;
+
+namespace {
+
+template <typename R1, typename R2>
+constexpr bool equal(R1&& r1, R2&& r2)
+{
+    auto first1 = std::begin(r1);
+    const auto last1 = std::end(r1);
+    auto first2 = std::begin(r2);
+    const auto last2 = std::end(r2);
+
+    while (first1 != last1 && first2 != last2) {
+        if (*first1 != *first2) {
+            return false;
+        }
+        ++first1;
+        ++first2;
+    }
+
+    return true;
+}
+
+constexpr bool test_raw_array()
+{
+    int arr[] = {1, 2, 3};
+    auto s = span{arr};
+    static_assert(std::is_same_v<decltype(s), span<int, 3>>);
+
+    return equal(arr, s);
+}
+
+constexpr bool test_const_raw_array()
+{
+    constexpr int arr[] = {1, 2, 3};
+    auto s = span{arr};
+    static_assert(std::is_same_v<decltype(s), span<const int, 3>>);
+
+    return equal(arr, s);
+}
+
+constexpr bool test_std_array()
+{
+    auto arr = std::array<int, 3>{1, 2, 3};
+    auto s = span{arr};
+    static_assert(std::is_same_v<decltype(s), span<int, 3>>);
+
+    return equal(arr, s);
+}
+
+constexpr bool test_const_std_array()
+{
+    const auto arr = std::array<int, 3>{1, 2, 3};
+    auto s = span{arr};
+    static_assert(std::is_same_v<decltype(s), span<const int, 3>>);
+
+    return equal(arr, s);
+}
+
+bool test_vec()
+{
+    auto arr = std::vector{1, 2, 3};
+    auto s = span{arr};
+    static_assert(std::is_same_v<decltype(s), span<int>>);
+
+    return equal(arr, s);
+}
+
+bool test_const_vec()
+{
+    const auto arr = std::vector{1, 2, 3};
+    auto s = span{arr};
+    static_assert(std::is_same_v<decltype(s), span<const int>>);
+
+    return equal(arr, s);
+}
+
+}
+
+TEST(Deduction, FromRawArrays)
+{
+    static_assert(test_raw_array());
+    static_assert(test_const_raw_array());
+    static_assert(test_std_array());
+    static_assert(test_const_std_array());
+
+    ASSERT_TRUE(test_std_array());
+    ASSERT_TRUE(test_const_std_array());
+    ASSERT_TRUE(test_vec());
+    ASSERT_TRUE(test_const_vec());
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_span.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_span.cpp
new file mode 100644
index 0000000..7cecfd0
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_span.cpp
@@ -0,0 +1,663 @@
+
+#include "wpi/span.h"
+
+#include <algorithm>
+#include <cassert>
+#include <deque>
+#include <initializer_list>
+#include <vector>
+
+#include "gtest/gtest.h"
+
+using wpi::span;
+
+struct base {};
+struct derived : base {};
+
+TEST(Span, DefaultConstruction)
+{
+    static_assert(std::is_nothrow_default_constructible<span<int>>::value, "");
+    static_assert(std::is_nothrow_default_constructible<span<int, 0>>::value,
+                  "");
+    static_assert(!std::is_default_constructible<span<int, 42>>::value, "");
+
+    //SECTION("dynamic size")
+    {
+        constexpr span<int> s{};
+        static_assert(s.size() == 0, "");
+        static_assert(s.data() == nullptr, "");
+        // This causes an ICE on MSVC
+#ifndef _MSC_VER
+        static_assert(s.begin() == s.end(), "");
+#else
+        ASSERT_EQ(s.begin(), s.end());
+#endif
+    }
+
+    //SECTION("fixed size")
+    {
+        constexpr span<int, 0> s{};
+        static_assert(s.size() == 0, "");
+        static_assert(s.data() == nullptr, "");
+#ifndef _MSC_VER
+        static_assert(s.begin() == s.end(), "");
+#else
+        ASSERT_EQ(s.begin(), s.end());
+#endif
+    }
+}
+
+TEST(Span, PointerLengthConstruction)
+{
+    static_assert(std::is_constructible<span<int>, int*, int>::value, "");
+    static_assert(std::is_constructible<span<const int>, int*, int>::value, "");
+    static_assert(
+        std::is_constructible<span<const int>, const int*, int>::value, "");
+
+    static_assert(std::is_constructible<span<int, 42>, int*, int>::value, "");
+    static_assert(std::is_constructible<span<const int, 42>, int*, int>::value,
+                  "");
+    static_assert(
+        std::is_constructible<span<const int, 42>, const int*, int>::value, "");
+
+    //SECTION("dynamic size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int> s(arr, 3);
+
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+
+    //SECTION("fixed size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int, 3> s(arr, 3);
+
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+}
+
+TEST(Span, PointerPointerConstruction)
+{
+    static_assert(std::is_constructible<span<int>, int*, int*>::value, "");
+    static_assert(!std::is_constructible<span<int>, float*, float*>::value, "");
+    static_assert(std::is_constructible<span<int, 42>, int*, int*>::value, "");
+    static_assert(!std::is_constructible<span<int, 42>, float*, float*>::value,
+                  "");
+
+    //SECTION("dynamic size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int> s{arr, arr + 3};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+
+    //SECTION("fixed size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int, 3> s{arr, arr + 3};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+}
+
+TEST(Span, CArrayConstruction)
+{
+    using int_array_t = int[3];
+    using float_array_t = float[3];
+
+    static_assert(std::is_nothrow_constructible<span<int>, int_array_t&>::value,
+                  "");
+    static_assert(!std::is_constructible<span<int>, int_array_t const&>::value,
+                  "");
+    static_assert(!std::is_constructible<span<int>, float_array_t>::value, "");
+
+    static_assert(
+        std::is_nothrow_constructible<span<const int>, int_array_t&>::value,
+        "");
+    static_assert(std::is_nothrow_constructible<span<const int>,
+                                                int_array_t const&>::value,
+                  "");
+    static_assert(!std::is_constructible<span<const int>, float_array_t>::value,
+                  "");
+
+    static_assert(
+        std::is_nothrow_constructible<span<int, 3>, int_array_t&>::value, "");
+    static_assert(
+        !std::is_constructible<span<int, 3>, int_array_t const&>::value, "");
+    static_assert(!std::is_constructible<span<int, 3>, float_array_t&>::value,
+                  "");
+
+    static_assert(
+        std::is_nothrow_constructible<span<const int, 3>, int_array_t&>::value,
+        "");
+    static_assert(std::is_nothrow_constructible<span<const int, 3>,
+                                                int_array_t const&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<const int, 3>, float_array_t>::value, "");
+
+    static_assert(!std::is_constructible<span<int, 42>, int_array_t&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<int, 42>, int_array_t const&>::value, "");
+    static_assert(!std::is_constructible<span<int, 42>, float_array_t&>::value,
+                  "");
+
+    static_assert(
+        !std::is_constructible<span<const int, 42>, int_array_t&>::value, "");
+    static_assert(
+        !std::is_constructible<span<const int, 42>, int_array_t const&>::value,
+        "");
+    static_assert(
+        !std::is_constructible<span<const int, 42>, float_array_t&>::value, "");
+
+    //SECTION("non-const, dynamic size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+
+    //SECTION("const, dynamic size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int const> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+
+    //SECTION("non-const, static size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int, 3> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+
+    //SECTION("const, dynamic size")
+    {
+        int arr[] = {1, 2, 3};
+        span<int const, 3> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.begin(), std::begin(arr));
+        ASSERT_EQ(s.end(), std::end(arr));
+    }
+}
+
+TEST(Span, StdArrayConstruction)
+{
+    using int_array_t = std::array<int, 3>;
+    using float_array_t = std::array<float, 3>;
+    using zero_array_t = std::array<int, 0>;
+
+    static_assert(std::is_nothrow_constructible<span<int>, int_array_t&>::value,
+                  "");
+    static_assert(!std::is_constructible<span<int>, int_array_t const&>::value,
+                  "");
+    static_assert(!std::is_constructible<span<int>, float_array_t>::value, "");
+
+    static_assert(
+        std::is_nothrow_constructible<span<const int>, int_array_t&>::value,
+        "");
+    static_assert(std::is_nothrow_constructible<span<const int>,
+                                                int_array_t const&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<const int>, float_array_t const&>::value,
+        "");
+
+    static_assert(
+        std::is_nothrow_constructible<span<int, 3>, int_array_t&>::value, "");
+    static_assert(
+        !std::is_constructible<span<int, 3>, int_array_t const&>::value, "");
+    static_assert(!std::is_constructible<span<int, 3>, float_array_t>::value,
+                  "");
+
+    static_assert(
+        std::is_nothrow_constructible<span<const int, 3>, int_array_t&>::value,
+        "");
+    static_assert(std::is_nothrow_constructible<span<const int, 3>,
+                                                int_array_t const&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<const int, 3>, float_array_t const&>::value,
+        "");
+
+    static_assert(!std::is_constructible<span<int, 42>, int_array_t&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<int, 42>, int_array_t const&>::value, "");
+    static_assert(
+        !std::is_constructible<span<int, 42>, float_array_t const&>::value, "");
+
+    static_assert(
+        !std::is_constructible<span<const int, 42>, int_array_t&>::value, "");
+    static_assert(
+        !std::is_constructible<span<const int, 42>, int_array_t const&>::value,
+        "");
+    static_assert(
+        !std::is_constructible<span<const int, 42>, float_array_t&>::value, "");
+
+    static_assert(std::is_constructible<span<int>, zero_array_t&>::value, "");
+    static_assert(!std::is_constructible<span<int>, const zero_array_t&>::value,
+                  "");
+    static_assert(std::is_constructible<span<const int>, zero_array_t&>::value,
+                  "");
+    static_assert(
+        std::is_constructible<span<const int>, const zero_array_t&>::value, "");
+
+    static_assert(std::is_constructible<span<int, 0>, zero_array_t&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<int, 0>, const zero_array_t&>::value, "");
+    static_assert(
+        std::is_constructible<span<const int, 0>, zero_array_t&>::value, "");
+    static_assert(
+        std::is_constructible<span<const int, 0>, const zero_array_t&>::value,
+        "");
+
+    //SECTION("non-const, dynamic size")
+    {
+        int_array_t arr = {1, 2, 3};
+        span<int> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+
+    //SECTION("const, dynamic size")
+    {
+        int_array_t arr = {1, 2, 3};
+        span<int const> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+
+    //SECTION("non-const, static size")
+    {
+        int_array_t arr = {1, 2, 3};
+        span<int, 3> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+
+    //SECTION("const, dynamic size")
+    {
+        int_array_t arr = {1, 2, 3};
+        span<int const, 3> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+}
+
+TEST(Span, ConstructionFromOtherContainers)
+{
+    using vec_t = std::vector<int>;
+    using deque_t = std::deque<int>;
+
+    static_assert(std::is_constructible<span<int>, vec_t&>::value, "");
+    static_assert(!std::is_constructible<span<int>, const vec_t&>::value, "");
+    static_assert(!std::is_constructible<span<int>, const deque_t&>::value, "");
+
+    static_assert(std::is_constructible<span<const int>, vec_t&>::value, "");
+    static_assert(std::is_constructible<span<const int>, const vec_t&>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<span<const int>, const deque_t&>::value, "");
+
+    static_assert(!std::is_constructible<span<int, 3>, vec_t&>::value, "");
+    static_assert(!std::is_constructible<span<int, 3>, const vec_t&>::value,
+                  "");
+    static_assert(!std::is_constructible<span<int, 3>, const deque_t&>::value,
+                  "");
+
+    static_assert(!std::is_constructible<span<const int, 3>, vec_t&>::value, "");
+    static_assert(
+        !std::is_constructible<span<const int, 3>, const vec_t&>::value, "");
+    static_assert(
+        !std::is_constructible<span<const int, 3>, const deque_t&>::value, "");
+
+    // vector<bool> is not contiguous and cannot be converted to span<bool>
+    // Regression test for https://github.com/tcbrindle/span/issues/24
+    static_assert(
+        !std::is_constructible<span<bool>, std::vector<bool>&>::value, "");
+    static_assert(!std::is_constructible<span<const bool>,
+                                         const std::vector<bool>&>::value, "");
+
+    //SECTION("non-const, dynamic size")
+    {
+        vec_t arr = {1, 2, 3};
+        span<int> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+
+    //SECTION("const, dynamic size")
+    {
+        vec_t arr = {1, 2, 3};
+        span<int const> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+
+    //SECTION("non-const, static size")
+    {
+        std::array<int, 3> arr = {1, 2, 3};
+        span<int, 3> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+
+    //SECTION("const, dynamic size")
+    {
+        std::array<int, 3> arr = {1, 2, 3};
+        span<int const, 3> s{arr};
+        ASSERT_EQ(s.size(), 3u);
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.begin(), arr.data());
+        ASSERT_EQ(s.end(), arr.data() + 3);
+    }
+}
+
+TEST(Span, ConstructionFromSpansOfDifferentSize)
+{
+    using zero_span = span<int, 0>;
+    using zero_const_span = span<const int, 0>;
+    using big_span = span<int, 1000000>;
+    using big_const_span = span<const int, 1000000>;
+    using dynamic_span = span<int>;
+    using dynamic_const_span = span<const int>;
+
+    static_assert(std::is_trivially_copyable<zero_span>::value, "");
+    static_assert(std::is_trivially_move_constructible<zero_span>::value, "");
+    static_assert(!std::is_constructible<zero_span, zero_const_span>::value,
+                  "");
+    static_assert(!std::is_constructible<zero_span, big_span>::value, "");
+    static_assert(!std::is_constructible<zero_span, big_const_span>::value, "");
+    static_assert(!std::is_constructible<zero_span, dynamic_span>::value, "");
+    static_assert(!std::is_constructible<zero_span, dynamic_const_span>::value,
+                  "");
+
+    static_assert(
+        std::is_nothrow_constructible<zero_const_span, zero_span>::value, "");
+    static_assert(std::is_trivially_copyable<zero_const_span>::value, "");
+    static_assert(std::is_trivially_move_constructible<zero_const_span>::value,
+                  "");
+    static_assert(!std::is_constructible<zero_const_span, big_span>::value, "");
+    static_assert(
+        !std::is_constructible<zero_const_span, big_const_span>::value, "");
+    static_assert(!std::is_constructible<zero_const_span, dynamic_span>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<zero_const_span, dynamic_const_span>::value, "");
+
+    static_assert(!std::is_constructible<big_span, zero_span>::value, "");
+    static_assert(!std::is_constructible<big_span, zero_const_span>::value, "");
+    static_assert(std::is_trivially_copyable<big_span>::value, "");
+    static_assert(std::is_trivially_move_constructible<big_span>::value, "");
+    static_assert(!std::is_constructible<big_span, big_const_span>::value, "");
+    static_assert(!std::is_constructible<big_span, dynamic_span>::value, "");
+    static_assert(!std::is_constructible<big_span, dynamic_const_span>::value,
+                  "");
+
+    static_assert(!std::is_constructible<big_const_span, zero_span>::value, "");
+    static_assert(
+        !std::is_constructible<big_const_span, zero_const_span>::value, "");
+    static_assert(std::is_trivially_copyable<big_const_span>::value, "");
+    static_assert(std::is_trivially_move_constructible<big_const_span>::value,
+                  "");
+    static_assert(
+        std::is_nothrow_constructible<big_const_span, big_span>::value, "");
+    static_assert(!std::is_constructible<big_const_span, dynamic_span>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<big_const_span, dynamic_const_span>::value, "");
+
+    static_assert(std::is_nothrow_constructible<dynamic_span, zero_span>::value,
+                  "");
+    static_assert(!std::is_constructible<dynamic_span, zero_const_span>::value,
+                  "");
+    static_assert(std::is_nothrow_constructible<dynamic_span, big_span>::value,
+                  "");
+    static_assert(!std::is_constructible<dynamic_span, big_const_span>::value,
+                  "");
+    static_assert(std::is_trivially_copyable<dynamic_span>::value, "");
+    static_assert(std::is_trivially_move_constructible<dynamic_span>::value,
+                  "");
+    static_assert(
+        !std::is_constructible<dynamic_span, dynamic_const_span>::value, "");
+
+    static_assert(
+        std::is_nothrow_constructible<dynamic_const_span, zero_span>::value,
+        "");
+    static_assert(std::is_nothrow_constructible<dynamic_const_span,
+                                                zero_const_span>::value,
+                  "");
+    static_assert(
+        std::is_nothrow_constructible<dynamic_const_span, big_span>::value, "");
+    static_assert(std::is_nothrow_constructible<dynamic_const_span,
+                                                big_const_span>::value,
+                  "");
+    static_assert(
+        std::is_nothrow_constructible<dynamic_const_span, dynamic_span>::value,
+        "");
+    static_assert(std::is_trivially_copyable<dynamic_const_span>::value, "");
+    static_assert(
+        std::is_trivially_move_constructible<dynamic_const_span>::value, "");
+
+    constexpr zero_const_span s0{};
+    constexpr dynamic_const_span d{s0};
+
+    static_assert(d.size() == 0, "");
+    static_assert(d.data() == nullptr, "");
+#ifndef _MSC_VER
+    static_assert(d.begin() == d.end(), "");
+#else
+    ASSERT_EQ(d.begin(), d.end());
+#endif
+}
+
+TEST(Span, MemberSubviewOperations)
+{
+    //SECTION("first<N>")
+    {
+        int arr[] = {1, 2, 3, 4, 5};
+        span<int, 5> s{arr};
+        auto f = s.first<3>();
+
+        static_assert(std::is_same<decltype(f), span<int, 3>>::value, "");
+        ASSERT_EQ(f.size(), 3u);
+        ASSERT_EQ(f.data(), arr);
+        ASSERT_EQ(f.begin(), arr);
+        ASSERT_EQ(f.end(), arr + 3);
+    }
+
+    //SECTION("last<N>")
+    {
+        int arr[] = {1, 2, 3, 4, 5};
+        span<int, 5> s{arr};
+        auto l = s.last<3>();
+
+        static_assert(std::is_same<decltype(l), span<int, 3>>::value, "");
+        ASSERT_EQ(l.size(), 3u);
+        ASSERT_EQ(l.data(), arr + 2);
+        ASSERT_EQ(l.begin(), arr + 2);
+        ASSERT_EQ(l.end(), std::end(arr));
+    }
+
+    //SECTION("subspan<N>")
+    {
+        int arr[] = {1, 2, 3, 4, 5};
+        span<int, 5> s{arr};
+        auto ss = s.subspan<1, 2>();
+
+        static_assert(std::is_same<decltype(ss), span<int, 2>>::value, "");
+        ASSERT_EQ(ss.size(), 2u);
+        ASSERT_EQ(ss.data(), arr + 1);
+        ASSERT_EQ(ss.begin(), arr + 1);
+        ASSERT_EQ(ss.end(), arr + 1 + 2);
+    }
+
+    //SECTION("first(n)")
+    {
+        int arr[] = {1, 2, 3, 4, 5};
+        span<int, 5> s{arr};
+        auto f = s.first(3);
+
+        static_assert(std::is_same<decltype(f), span<int>>::value, "");
+        ASSERT_EQ(f.size(), 3u);
+        ASSERT_EQ(f.data(), arr);
+        ASSERT_EQ(f.begin(), arr);
+        ASSERT_EQ(f.end(), arr + 3);
+    }
+
+    //SECTION("last(n)")
+    {
+        int arr[] = {1, 2, 3, 4, 5};
+        span<int, 5> s{arr};
+        auto l = s.last(3);
+
+        static_assert(std::is_same<decltype(l), span<int>>::value, "");
+        ASSERT_EQ(l.size(), 3u);
+        ASSERT_EQ(l.data(), arr + 2);
+        ASSERT_EQ(l.begin(), arr + 2);
+        ASSERT_EQ(l.end(), std::end(arr));
+    }
+
+    //SECTION("subspan(n)")
+    {
+        int arr[] = {1, 2, 3, 4, 5};
+        span<int, 5> s{arr};
+        auto ss = s.subspan(1, 2);
+
+        static_assert(std::is_same<decltype(ss), span<int>>::value, "");
+        ASSERT_EQ(ss.size(), 2u);
+        ASSERT_EQ(ss.data(), arr + 1);
+        ASSERT_EQ(ss.begin(), arr + 1);
+        ASSERT_EQ(ss.end(), arr + 1 + 2);
+    }
+
+    // TODO: Test all the dynamic subspan possibilities
+}
+
+TEST(Span, Observers)
+{
+    // We already use this everywhere, but whatever
+    constexpr span<int, 0> empty{};
+    static_assert(empty.size() == 0, "");
+    static_assert(empty.empty(), "");
+
+    constexpr int arr[] = {1, 2, 3};
+    static_assert(span<const int>{arr}.size() == 3, "");
+    static_assert(!span<const int>{arr}.empty(), "");
+}
+
+TEST(Span, ElementAccess)
+{
+    constexpr int arr[] = {1, 2, 3};
+    span<const int> s{arr};
+
+    ASSERT_EQ(s[0], arr[0]);
+    ASSERT_EQ(s[1], arr[1]);
+    ASSERT_EQ(s[2], arr[2]);
+}
+
+TEST(Span, IteratorSupport)
+{
+    {
+        std::vector<int> vec;
+        span<int> s{vec};
+        std::sort(s.begin(), s.end());
+        ASSERT_TRUE(std::is_sorted(vec.cbegin(), vec.cend()));
+    }
+
+    {
+        const std::vector<int> vec{1, 2, 3};
+        span<const int> s{vec};
+        ASSERT_TRUE(std::equal(s.rbegin(), s.rend(), vec.crbegin()));
+    }
+}
+
+TEST(Span, MakeSpan)
+{
+    {
+        int arr[3] = {1, 2, 3};
+        auto s = wpi::span(arr);
+        static_assert(std::is_same<decltype(s), span<int, 3>>::value, "");
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.size(), 3u);
+    }
+
+    {
+        const int arr[3] = {1, 2, 3};
+        auto s = wpi::span(arr);
+        static_assert(std::is_same<decltype(s), span<const int, 3>>::value, "");
+        ASSERT_EQ(s.data(), arr);
+        ASSERT_EQ(s.size(), 3u);
+    }
+
+    {
+        std::array<int, 3> arr = {1, 2, 3};
+        auto s = wpi::span(arr);
+        static_assert(std::is_same<decltype(s), span<int, 3>>::value, "");
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.size(), arr.size());
+    }
+
+    {
+        const std::array<int, 3> arr = {1, 2, 3};
+        auto s = wpi::span(arr);
+        static_assert(std::is_same<decltype(s), span<const int, 3>>::value, "");
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.size(), 3u);
+    }
+
+    {
+        std::vector<int> arr = {1, 2, 3};
+        auto s = wpi::span(arr);
+        static_assert(std::is_same<decltype(s), span<int>>::value, "");
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.size(), arr.size());
+    }
+
+    {
+        const std::vector<int> arr = {1, 2, 3};
+        auto s = wpi::span(arr);
+        static_assert(std::is_same<decltype(s), span<const int>>::value, "");
+        ASSERT_EQ(s.data(), arr.data());
+        ASSERT_EQ(s.size(), arr.size());
+    }
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_structured_bindings.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_structured_bindings.cpp
new file mode 100644
index 0000000..5ce2969
--- /dev/null
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/span/test_structured_bindings.cpp
@@ -0,0 +1,30 @@
+
+#include "wpi/span.h"
+
+#include "gtest/gtest.h"
+
+using static_span_t = wpi::span<int, 3>;
+using dynamic_span_t = wpi::span<int>;
+
+static_assert(std::tuple_size_v<static_span_t> == static_span_t::extent);
+static_assert(!wpi::detail::is_complete<std::tuple_size<dynamic_span_t>>::value);
+
+TEST(StructuredBindings, Test)
+{
+    // C++, why you no let me do constexpr structured bindings?
+
+    int arr[] = {1, 2, 3};
+
+    auto& [a1, a2, a3] = arr;
+    auto&& [s1, s2, s3] = wpi::span(arr);
+
+    ASSERT_EQ(a1, s1);
+    ASSERT_EQ(a2, s2);
+    ASSERT_EQ(a3, s3);
+
+    a1 = 99;
+    ASSERT_EQ(s1, 99);
+
+    s2 = 100;
+    ASSERT_EQ(a2, 100);
+}
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/spinlock_bench.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/spinlock_bench.cpp
index b36e558..9534837 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/spinlock_bench.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/spinlock_bench.cpp
@@ -1,17 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/spinlock.h"  // NOLINT(build/include_order)
 
 #include <chrono>
-#include <iostream>
 #include <mutex>
 #include <thread>
 
+#include "fmt/core.h"
 #include "gtest/gtest.h"
 #include "wpi/mutex.h"
 
@@ -53,9 +50,8 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "std::mutex sizeof: " << sizeof(std_mutex)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("std::mutex sizeof: {} time: {} value: {}\n", sizeof(std_mutex),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thrb.join();
 
@@ -68,9 +64,9 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "std::recursive_mutex sizeof: " << sizeof(std_recursive_mutex)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("std::recursive_mutex sizeof: {} time: {} value: {}\n",
+               sizeof(std_recursive_mutex),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thrb2.join();
 
@@ -83,9 +79,8 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "wpi::mutex sizeof: " << sizeof(wpi_mutex)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("wpi::mutex sizeof: {} time: {} value: {}\n", sizeof(wpi_mutex),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thr2.join();
 
@@ -98,9 +93,9 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "wpi::recursive_mutex sizeof: " << sizeof(wpi_recursive_mutex)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("wpi::recursive_mutex sizeof: {} time: {} value: {}\n",
+               sizeof(wpi_recursive_mutex),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thr2b.join();
 
@@ -113,9 +108,8 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "spinlock sizeof: " << sizeof(spinlock)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("spinlock sizeof: {} time: {} value: {}\n", sizeof(spinlock),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thr3.join();
 
@@ -128,9 +122,9 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "recursive_spinlock1 sizeof: " << sizeof(recursive_spinlock1)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("recursive_spinlock1 sizeof: {} time: {} value: {}\n",
+               sizeof(recursive_spinlock1),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thr4.join();
 
@@ -143,9 +137,9 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "recursive_spinlock2 sizeof: " << sizeof(recursive_spinlock2)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("recursive_spinlock2 sizeof: {} time: {} value: {}\n",
+               sizeof(recursive_spinlock2),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thr4b.join();
 
@@ -158,9 +152,9 @@
       ++value;
     }
     auto stop = high_resolution_clock::now();
-    std::cout << "recursive_spinlock sizeof: " << sizeof(recursive_spinlock)
-              << " time: " << duration_cast<microseconds>(stop - start).count()
-              << " value: " << value << "\n";
+    fmt::print("recursive_spinlock sizeof: {} time: {} value: {}\n",
+               sizeof(recursive_spinlock),
+               duration_cast<microseconds>(stop - start).count(), value);
   });
   thr4c.join();
 }
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncFunctionTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncFunctionTest.cpp
index 132adea..8407935 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncFunctionTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncFunctionTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/AsyncFunction.h"  // NOLINT(build/include_order)
 
@@ -14,10 +11,9 @@
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/Prepare.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvAsyncFunction, Test) {
+TEST(UvAsyncFunctionTest, Basic) {
   int prepare_cb_called = 0;
   int async_cb_called[2] = {0, 0};
   int close_cb_called = 0;
@@ -32,7 +28,9 @@
 
   prepare->error.connect([](Error) { FAIL(); });
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] {
       auto call0 = async->Call(0);
       auto call1 = async->Call(1);
@@ -59,10 +57,12 @@
   ASSERT_EQ(async_cb_called[1], 1);
   ASSERT_EQ(close_cb_called, 1);
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsyncFunction, Ref) {
+TEST(UvAsyncFunctionTest, Ref) {
   int prepare_cb_called = 0;
   int val = 0;
 
@@ -73,7 +73,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] { ASSERT_EQ(async->Call(1, val).get(), 2); });
   });
   prepare->Start();
@@ -89,10 +91,12 @@
 
   ASSERT_EQ(val, 1);
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsyncFunction, Movable) {
+TEST(UvAsyncFunctionTest, Movable) {
   int prepare_cb_called = 0;
 
   std::thread theThread;
@@ -103,7 +107,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] {
       auto val = std::make_unique<int>(1);
       auto val2 = async->Call(std::move(val)).get();
@@ -122,10 +128,12 @@
 
   loop->Run();
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsyncFunction, CallIgnoreResult) {
+TEST(UvAsyncFunctionTest, CallIgnoreResult) {
   int prepare_cb_called = 0;
 
   std::thread theThread;
@@ -136,7 +144,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] { async->Call(std::make_unique<int>(1)); });
   });
   prepare->Start();
@@ -150,10 +160,12 @@
 
   loop->Run();
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsyncFunction, VoidCall) {
+TEST(UvAsyncFunctionTest, VoidCall) {
   int prepare_cb_called = 0;
 
   std::thread theThread;
@@ -163,7 +175,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] { async->Call(); });
   });
   prepare->Start();
@@ -176,10 +190,12 @@
 
   loop->Run();
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsyncFunction, WaitFor) {
+TEST(UvAsyncFunctionTest, WaitFor) {
   int prepare_cb_called = 0;
 
   std::thread theThread;
@@ -189,7 +205,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] {
       ASSERT_FALSE(async->Call().wait_for(std::chrono::milliseconds(10)));
     });
@@ -205,10 +223,12 @@
 
   loop->Run();
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsyncFunction, VoidWaitFor) {
+TEST(UvAsyncFunctionTest, VoidWaitFor) {
   int prepare_cb_called = 0;
 
   std::thread theThread;
@@ -218,7 +238,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] {
       ASSERT_FALSE(async->Call().wait_for(std::chrono::milliseconds(10)));
     });
@@ -234,8 +256,9 @@
 
   loop->Run();
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncTest.cpp
index e18f972..5561f1e 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvAsyncTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /* Copyright Joyent, Inc. and other Node contributors. All rights reserved.
  *
@@ -37,10 +34,9 @@
 #include "wpi/uv/Loop.h"
 #include "wpi/uv/Prepare.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvAsync, Test) {
+TEST(UvAsyncTest, CallbackOnly) {
   std::atomic_int async_cb_called{0};
   int prepare_cb_called = 0;
   int close_cb_called = 0;
@@ -59,7 +55,9 @@
   prepare->error.connect([](Error) { FAIL(); });
   prepare->closed.connect([&] { close_cb_called++; });
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] {
       for (;;) {
         mutex.lock();
@@ -98,10 +96,12 @@
   ASSERT_EQ(async_cb_called, 3);
   ASSERT_EQ(close_cb_called, 2);
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsync, Data) {
+TEST(UvAsyncTest, Data) {
   int prepare_cb_called = 0;
   int async_cb_called[2] = {0, 0};
   int close_cb_called = 0;
@@ -116,7 +116,9 @@
 
   prepare->error.connect([](Error) { FAIL(); });
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] {
       async->Send(0, [&](int v) {
         ASSERT_EQ(v, 0);
@@ -142,10 +144,12 @@
   ASSERT_EQ(async_cb_called[1], 1);
   ASSERT_EQ(close_cb_called, 1);
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-TEST(UvAsync, DataRef) {
+TEST(UvAsyncTest, DataRef) {
   int prepare_cb_called = 0;
   int val = 0;
 
@@ -156,7 +160,9 @@
   auto prepare = Prepare::Create(loop);
 
   prepare->prepare.connect([&] {
-    if (prepare_cb_called++) return;
+    if (prepare_cb_called++) {
+      return;
+    }
     theThread = std::thread([&] { async->Send(1, val); });
   });
   prepare->Start();
@@ -171,8 +177,9 @@
 
   ASSERT_EQ(val, 1);
 
-  if (theThread.joinable()) theThread.join();
+  if (theThread.joinable()) {
+    theThread.join();
+  }
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvBufferTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvBufferTest.cpp
index e837ca9..e262181 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvBufferTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvBufferTest.cpp
@@ -1,50 +1,48 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018 FIRST. All Rights Reserved.                             */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Buffer.h"  // NOLINT(build/include_order)
 
 #include "gtest/gtest.h"  // NOLINT(build/include_order)
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvSimpleBufferPool, ConstructDefault) {
+TEST(UvSimpleBufferPoolTest, ConstructDefault) {
   SimpleBufferPool<> pool;
   auto buf1 = pool.Allocate();
-  ASSERT_EQ(buf1.len, 4096u);
+  ASSERT_EQ(buf1.len, 4096u);  // NOLINT
+  pool.Release({&buf1, 1});
 }
 
-TEST(UvSimpleBufferPool, ConstructSize) {
+TEST(UvSimpleBufferPoolTest, ConstructSize) {
   SimpleBufferPool<4> pool{8192};
   auto buf1 = pool.Allocate();
-  ASSERT_EQ(buf1.len, 8192u);
+  ASSERT_EQ(buf1.len, 8192u);  // NOLINT
+  pool.Release({&buf1, 1});
 }
 
-TEST(UvSimpleBufferPool, ReleaseReuse) {
+TEST(UvSimpleBufferPoolTest, ReleaseReuse) {
   SimpleBufferPool<4> pool;
   auto buf1 = pool.Allocate();
   auto buf1copy = buf1;
   auto origSize = buf1.len;
   buf1.len = 8;
-  pool.Release(buf1);
+  pool.Release({&buf1, 1});
   ASSERT_EQ(buf1.base, nullptr);
   auto buf2 = pool.Allocate();
   ASSERT_EQ(buf1copy.base, buf2.base);
   ASSERT_EQ(buf2.len, origSize);
+  pool.Release({&buf2, 1});
 }
 
-TEST(UvSimpleBufferPool, ClearRemaining) {
+TEST(UvSimpleBufferPoolTest, ClearRemaining) {
   SimpleBufferPool<4> pool;
   auto buf1 = pool.Allocate();
-  pool.Release(buf1);
+  pool.Release({&buf1, 1});
   ASSERT_EQ(pool.Remaining(), 1u);
   pool.Clear();
   ASSERT_EQ(pool.Remaining(), 0u);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp
index 20d27f1..b397e24 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetAddrInfoTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /* Copyright Joyent, Inc. and other Node contributors. All rights reserved.
  *
@@ -34,10 +31,9 @@
 
 #define CONCURRENT_COUNT 10
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvGetAddrInfo, BothNull) {
+TEST(UvGetAddrInfoTest, BothNull) {
   int fail_cb_called = 0;
 
   auto loop = Loop::Create();
@@ -47,12 +43,12 @@
   });
 
   GetAddrInfo(
-      loop, [](const addrinfo&) { FAIL(); }, Twine::createNull());
+      loop, [](const addrinfo&) { FAIL(); }, "");
   loop->Run();
   ASSERT_EQ(fail_cb_called, 1);
 }
 
-TEST(UvGetAddrInfo, FailedLookup) {
+TEST(UvGetAddrInfoTest, FailedLookup) {
   int fail_cb_called = 0;
 
   auto loop = Loop::Create();
@@ -69,7 +65,7 @@
   ASSERT_EQ(fail_cb_called, 1);
 }
 
-TEST(UvGetAddrInfo, Basic) {
+TEST(UvGetAddrInfoTest, Basic) {
   int getaddrinfo_cbs = 0;
 
   auto loop = Loop::Create();
@@ -84,7 +80,7 @@
 }
 
 #ifndef _WIN32
-TEST(UvGetAddrInfo, Concurrent) {
+TEST(UvGetAddrInfoTest, Concurrent) {
   int getaddrinfo_cbs = 0;
   int callback_counts[CONCURRENT_COUNT];
 
@@ -110,5 +106,4 @@
 }
 #endif
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp
index 76d22d9..db1eefe 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvGetNameInfoTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* 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.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /* Copyright Joyent, Inc. and other Node contributors. All rights reserved.
  *
@@ -32,10 +29,9 @@
 
 #include "wpi/uv/Loop.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvGetNameInfo, BasicIp4) {
+TEST(UvGetNameInfoTest, BasicIp4) {
   int getnameinfo_cbs = 0;
 
   auto loop = Loop::Create();
@@ -55,7 +51,7 @@
   ASSERT_EQ(getnameinfo_cbs, 1);
 }
 
-TEST(UvGetNameInfo, BasicIp6) {
+TEST(UvGetNameInfoTest, BasicIp6) {
   int getnameinfo_cbs = 0;
 
   auto loop = Loop::Create();
@@ -75,5 +71,4 @@
   ASSERT_EQ(getnameinfo_cbs, 1);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvLoopWalkTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvLoopWalkTest.cpp
index 5d2e120..38a3342 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvLoopWalkTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvLoopWalkTest.cpp
@@ -1,9 +1,6 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 /* Copyright Joyent, Inc. and other Node contributors. All rights reserved.
  *
@@ -32,10 +29,9 @@
 
 #include "wpi/uv/Timer.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvLoop, Walk) {
+TEST(UvLoopTest, Walk) {
   int seen_timer_handle = 0;
 
   auto loop = Loop::Create();
@@ -47,7 +43,9 @@
 
   timer->timeout.connect([&, theTimer = timer.get()] {
     theTimer->GetLoopRef().Walk([&](Handle& it) {
-      if (&it == timer.get()) seen_timer_handle++;
+      if (&it == timer.get()) {
+        seen_timer_handle++;
+      }
     });
     theTimer->Close();
   });
@@ -61,10 +59,11 @@
   // Loop is finished, should not see our timer handle
   seen_timer_handle = 0;
   loop->Walk([&](Handle& it) {
-    if (&it == timer.get()) seen_timer_handle++;
+    if (&it == timer.get()) {
+      seen_timer_handle++;
+    }
   });
   ASSERT_EQ(seen_timer_handle, 0);
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvTimerTest.cpp b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvTimerTest.cpp
index 6e1bd5c..1a6258f 100644
--- a/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvTimerTest.cpp
+++ b/third_party/allwpilib/wpiutil/src/test/native/cpp/uv/UvTimerTest.cpp
@@ -1,18 +1,14 @@
-/*----------------------------------------------------------------------------*/
-/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
-/* Open Source Software - may be modified and shared by FRC teams. The code   */
-/* must be accompanied by the FIRST BSD license file in the root directory of */
-/* the project.                                                               */
-/*----------------------------------------------------------------------------*/
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
 
 #include "wpi/uv/Timer.h"  // NOLINT(build/include_order)
 
 #include "gtest/gtest.h"
 
-namespace wpi {
-namespace uv {
+namespace wpi::uv {
 
-TEST(UvTimer, StartAndStop) {
+TEST(UvTimerTest, StartAndStop) {
   auto loop = Loop::Create();
   auto handleNoRepeat = Timer::Create(loop);
   auto handleRepeat = Timer::Create(loop);
@@ -59,7 +55,7 @@
   ASSERT_TRUE(checkTimerRepeatEvent);
 }
 
-TEST(UvTimer, Repeat) {
+TEST(UvTimerTest, Repeat) {
   auto loop = Loop::Create();
   auto handle = Timer::Create(loop);
 
@@ -70,5 +66,4 @@
   loop->Run();  // forces close callback to run
 }
 
-}  // namespace uv
-}  // namespace wpi
+}  // namespace wpi::uv
diff --git a/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in b/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in
index 59e506d..d814f0a 100644
--- a/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in
+++ b/third_party/allwpilib/wpiutil/wpiutil-config.cmake.in
@@ -5,4 +5,5 @@
 @LIBUV_VCPKG_REPLACE@
 @EIGEN_VCPKG_REPLACE@
 
+@FILENAME_DEP_REPLACE@
 include(${SELF_DIR}/wpiutil.cmake)
diff --git a/third_party/rewrite_subtree.sh b/third_party/rewrite_subtree.sh
index a3e5372..e565d2b 100755
--- a/third_party/rewrite_subtree.sh
+++ b/third_party/rewrite_subtree.sh
@@ -13,11 +13,13 @@
 set -e
 set -o pipefail
 
+export SIGNOFF="Signed-off-by: $(git config --get user.name) <$(git config --get user.email)>"
+
 GIT_DIR=$(readlink -f "$(git rev-parse --git-dir)")
 TMP_MSG="${GIT_DIR}/COMMIT_MSG_REWRITE"
 
-git filter-branch --msg-filter \
-  "cat > ${TMP_MSG} && \"${GIT_DIR}/hooks/commit-msg\" ${TMP_MSG} && \
+git filter-branch -f --msg-filter \
+  "cat | git interpret-trailers --trailer \"$SIGNOFF\" > ${TMP_MSG} && \"${GIT_DIR}/hooks/commit-msg\" ${TMP_MSG} && \
   cat \"${TMP_MSG}\"" HEAD...HEAD~1
 
 rm -rf "${TMP_MSG}"
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index 8c8560a..ef4c058 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -12,6 +12,7 @@
 
 #include "ctre/phoenix/CANifier.h"
 #include "frc971/wpilib/ahal/AnalogInput.h"
+#include "frc971/wpilib/ahal/Compressor.h"
 #include "frc971/wpilib/ahal/Counter.h"
 #include "frc971/wpilib/ahal/DigitalGlitchFilter.h"
 #include "frc971/wpilib/ahal/DriverStation.h"
@@ -442,22 +443,11 @@
             event_loop->MakeFetcher<::y2018::vision::VisionStatus>("/vision")),
         pneumatics_to_log_sender_(
             event_loop->MakeSender<::frc971::wpilib::PneumaticsToLog>("/aos")),
-        pcm_(pcm) {
+        pcm_(pcm),
+        compressor_(0) {
     event_loop->set_name("Solenoids");
     event_loop_->SetRuntimeRealtimePriority(27);
 
-    int32_t status = 0;
-    HAL_CompressorHandle compressor_ = HAL_InitializeCompressor(0, &status);
-    if (status != 0) {
-      AOS_LOG(ERROR, "Compressor status is nonzero, %d\n",
-              static_cast<int>(status));
-    }
-    HAL_SetCompressorClosedLoopControl(compressor_, true, &status);
-    if (status != 0) {
-      AOS_LOG(ERROR, "Compressor status is nonzero, %d\n",
-              static_cast<int>(status));
-    }
-
     event_loop_->AddPhasedLoop([this](int iterations) { Loop(iterations); },
                                ::std::chrono::milliseconds(20),
                                ::std::chrono::milliseconds(1));
@@ -607,8 +597,6 @@
       left_drivetrain_shifter_, right_drivetrain_shifter_, claw_, arm_brakes_,
       hook_, forks_;
 
-  HAL_CompressorHandle compressor_;
-
   ::ctre::phoenix::CANifier canifier_{0};
 
   ::std::atomic<bool> run_{true};
@@ -617,6 +605,8 @@
   double last_green_ = -1.0;
   double last_blue_ = -1.0;
 
+  frc::Compressor compressor_;
+
   int light_flash_ = 0;
 };
 
diff --git a/y2020/actors/auto_splines.cc b/y2020/actors/auto_splines.cc
index 00e3bd0..77cfbb5 100644
--- a/y2020/actors/auto_splines.cc
+++ b/y2020/actors/auto_splines.cc
@@ -5,6 +5,9 @@
 namespace y2020 {
 namespace actors {
 
+constexpr double kFieldLength = 15.983;
+constexpr double kFieldWidth = 8.212;
+
 void MaybeFlipSpline(
     aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
         *builder,
@@ -20,6 +23,34 @@
   }
 }
 
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+    aos::Alliance alliance) {
+  frc971::MultiSpline *spline =
+      GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+  flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
+  flatbuffers::Vector<float> *spline_y = spline->mutable_spline_y();
+
+  for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+    spline_x->Mutate(ii, spline_x->Get(ii) - kFieldLength / 2.0);
+  }
+  for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+    spline_y->Mutate(ii, kFieldWidth / 2.0 - spline_y->Get(ii));
+  }
+
+  if (alliance == aos::Alliance::kBlue) {
+    for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+      spline_x->Mutate(ii, -spline_x->Get(ii));
+    }
+    for (size_t ii = 0; ii < spline_y->size(); ++ii) {
+      spline_y->Mutate(ii, -spline_y->Get(ii));
+    }
+  }
+  return spline_offset;
+}
+
 flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
     aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
         *builder,
@@ -90,6 +121,26 @@
   return multispline_builder.Finish();
 }
 
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned1(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  return FixSpline(builder,
+                   aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_1_,
+                                                            builder->fbb()),
+                   alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TargetAligned2(
+    aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+        *builder,
+    aos::Alliance alliance) {
+  return FixSpline(builder,
+                   aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_2_,
+                                                            builder->fbb()),
+                   alliance);
+}
+
 flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
     aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
         *builder) {
diff --git a/y2020/actors/auto_splines.h b/y2020/actors/auto_splines.h
index 98bf0b7..bdaef75 100644
--- a/y2020/actors/auto_splines.h
+++ b/y2020/actors/auto_splines.h
@@ -47,16 +47,12 @@
 
   flatbuffers::Offset<frc971::MultiSpline> TargetAligned1(
       aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
-          *builder) {
-    return aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_1_,
-                                                    builder->fbb());
-  }
+          *builder,
+      aos::Alliance alliance);
   flatbuffers::Offset<frc971::MultiSpline> TargetAligned2(
       aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
-          *builder) {
-    return aos::CopyFlatBuffer<frc971::MultiSpline>(target_aligned_2_,
-                                                    builder->fbb());
-  }
+          *builder,
+      aos::Alliance alliance);
   flatbuffers::Offset<frc971::MultiSpline> TargetOffset1(
       aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
           *builder) {
diff --git a/y2020/actors/autonomous_actor.cc b/y2020/actors/autonomous_actor.cc
index 02d68da..077e87e 100644
--- a/y2020/actors/autonomous_actor.cc
+++ b/y2020/actors/autonomous_actor.cc
@@ -11,7 +11,7 @@
 #include "y2020/control_loops/drivetrain/drivetrain_base.h"
 
 DEFINE_bool(spline_auto, false, "If true, define a spline autonomous mode");
-DEFINE_bool(target_aligned, false,
+DEFINE_bool(target_aligned, true,
             "If true, run the Infinite Recharge autonomous that starts aligned "
             "with the target");
 DEFINE_bool(target_offset, false,
@@ -38,33 +38,51 @@
       superstructure_goal_sender_(
           event_loop->MakeSender<control_loops::superstructure::Goal>(
               "/superstructure")),
-      joystick_state_fetcher_(
-          event_loop->MakeFetcher<aos::JoystickState>("/aos")),
       superstructure_status_fetcher_(
           event_loop->MakeFetcher<y2020::control_loops::superstructure::Status>(
               "/superstructure")),
+      joystick_state_fetcher_(
+          event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+      robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
       auto_splines_() {
-  set_max_drivetrain_voltage(2.0);
+  set_max_drivetrain_voltage(12.0);
   replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
   event_loop->OnRun([this, event_loop]() {
     replan_timer_->Setup(event_loop->monotonic_now());
+    button_poll_->Setup(event_loop->monotonic_now(), chrono::milliseconds(50));
   });
-  event_loop->MakeWatcher("/aos", [this](const aos::RobotState &msg) {
-    if (msg.user_button()) {
-      user_indicated_safe_to_reset_ = true;
-      MaybeSendStartingPosition();
+
+  button_poll_ = event_loop->AddTimer([this]() {
+    const aos::monotonic_clock::time_point now =
+        this->event_loop()->context().monotonic_event_time;
+    if (robot_state_fetcher_.Fetch()) {
+      if (robot_state_fetcher_->user_button()) {
+        user_indicated_safe_to_reset_ = true;
+        MaybeSendStartingPosition();
+      }
     }
-  });
-  event_loop->MakeWatcher("/aos", [this](const aos::JoystickState &msg) {
-    if (msg.has_alliance() && (msg.alliance() != alliance_)) {
-      alliance_ = msg.alliance();
-      Replan();
+    if (joystick_state_fetcher_.Fetch()) {
+      if (joystick_state_fetcher_->has_alliance() &&
+          (joystick_state_fetcher_->alliance() != alliance_)) {
+        alliance_ = joystick_state_fetcher_->alliance();
+        is_planned_ = false;
+        // Only kick the planning out by 2 seconds. If we end up enabled in that
+        // second, then we will kick it out further based on the code below.
+        replan_timer_->Setup(now + std::chrono::seconds(2));
+      }
+      if (joystick_state_fetcher_->enabled()) {
+        if (!is_planned_) {
+          // Only replan once we've been disabled for 5 seconds.
+          replan_timer_->Setup(now + std::chrono::seconds(5));
+        }
+      }
     }
   });
 }
 
 void AutonomousActor::MaybeSendStartingPosition() {
-  if (user_indicated_safe_to_reset_ && !sent_starting_position_) {
+  if (is_planned_ && user_indicated_safe_to_reset_ &&
+      !sent_starting_position_) {
     CHECK(starting_position_);
     SendStartingPosition(starting_position_.value());
   }
@@ -72,11 +90,23 @@
 
 void AutonomousActor::Replan() {
   sent_starting_position_ = false;
+  if (alliance_ == aos::Alliance::kInvalid) {
+    return;
+  }
   if (FLAGS_spline_auto) {
     test_spline_ = PlanSpline(std::bind(&AutonomousSplines::TestSpline,
                                         &auto_splines_, std::placeholders::_1),
                               SplineDirection::kForward);
     starting_position_ = test_spline_->starting_position();
+  } else if (FLAGS_target_aligned) {
+    target_aligned_splines_ = {
+        PlanSpline(std::bind(&AutonomousSplines::TargetAligned1, &auto_splines_,
+                             std::placeholders::_1, alliance_),
+                   SplineDirection::kForward),
+        PlanSpline(std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
+                             std::placeholders::_1, alliance_),
+                   SplineDirection::kBackward)};
+    starting_position_ = target_aligned_splines_.value()[0].starting_position();
   } else if (FLAGS_target_offset) {
     target_offset_splines_ = {
         PlanSpline(std::bind(&AutonomousSplines::TargetOffset1, &auto_splines_,
@@ -86,18 +116,12 @@
                              std::placeholders::_1),
                    SplineDirection::kBackward)};
     starting_position_ = target_offset_splines_.value()[0].starting_position();
-  } else if (FLAGS_target_aligned) {
-    target_aligned_splines_ = {
-        PlanSpline(std::bind(&AutonomousSplines::TargetAligned1, &auto_splines_,
-                             std::placeholders::_1),
-                   SplineDirection::kForward),
-        PlanSpline(std::bind(&AutonomousSplines::TargetAligned2, &auto_splines_,
-                             std::placeholders::_1),
-                   SplineDirection::kBackward)};
-    starting_position_ = target_aligned_splines_.value()[0].starting_position();
   } else {
     starting_position_ = Eigen::Vector3d::Zero();
   }
+
+  is_planned_ = true;
+
   MaybeSendStartingPosition();
 }
 
@@ -119,24 +143,25 @@
     AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
     SendStartingPosition(starting_position_.value());
   }
-
-  // Queue up a replan to occur as soon as this action completes.
-  // TODO(james): Modify this so we don't replan during teleop.
-  replan_timer_->Setup(monotonic_now());
+  // Clear this so that we don't accidentally resend things as soon as we replan
+  // later.
+  user_indicated_safe_to_reset_ = false;
+  is_planned_ = false;
+  starting_position_.reset();
 
   AOS_LOG(INFO, "Params are %d\n", params->mode());
   if (alliance_ == aos::Alliance::kInvalid) {
     AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
     return false;
   }
-  if (FLAGS_target_aligned) {
+  if (FLAGS_spline_auto) {
+    SplineAuto();
+  } else if (FLAGS_target_aligned) {
     TargetAligned();
   } else if (FLAGS_target_offset) {
     TargetOffset();
   } else if (FLAGS_just_shoot) {
     JustShoot();
-  } else if (FLAGS_spline_auto) {
-    SplineAuto();
   } else {
     return DriveFwd();
   }
@@ -162,42 +187,64 @@
 }
 
 void AutonomousActor::TargetAligned() {
+  aos::monotonic_clock::time_point start_time = aos::monotonic_clock::now();
   CHECK(target_aligned_splines_);
   auto &splines = *target_aligned_splines_;
 
-  // shoot pre-loaded balls
-  set_shooter_tracking(true);
+  // Spin up.
   set_shooting(true);
   SendSuperstructureGoal();
-
   if (!WaitForBallsShot(3)) return;
 
   set_shooting(false);
+  ExtendIntake();
   SendSuperstructureGoal();
 
-  ExtendIntake();
-
-  // pickup 3 more balls
   if (!splines[0].WaitForPlan()) return;
   splines[0].Start();
 
   if (!splines[0].WaitForSplineDistanceRemaining(0.02)) return;
+  std::this_thread::sleep_for(chrono::milliseconds(200));
   RetractIntake();
 
   if (!splines[1].WaitForPlan()) return;
   splines[1].Start();
 
-  if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+  if (!splines[1].WaitForSplineDistanceRemaining(2.0)) return;
+  // Reverse the rollers for a moment to try to unjam any jammed balls.  Since
+  // we are moving here, this is free to try.
+  set_roller_voltage(-12.0);
+  std::this_thread::sleep_for(chrono::milliseconds(300));
+  set_roller_voltage(0.0);
 
-  // shoot the new balls in front of the goal.
+  // Once we come to a stop, give the robot a moment to settle down.  This makes
+  // the shot more accurate.
+  if (!splines[1].WaitForSplineDistanceRemaining(0.02)) return;
+  std::this_thread::sleep_for(chrono::milliseconds(1500));
+  set_shooting(true);
+  const int balls = Balls();
+
+  SendSuperstructureGoal();
+
+  std::this_thread::sleep_for(chrono::milliseconds(1500));
+
+  // We have been seeing balls get stuck on the intake roller.  Reverse the
+  // roller again for a moment to unjam it.
+  set_shooting(false);
+  set_roller_voltage(-12.0);
+  SendSuperstructureGoal();
+  std::this_thread::sleep_for(chrono::milliseconds(250));
+
+  set_roller_voltage(0.0);
   set_shooting(true);
   SendSuperstructureGoal();
 
-  if (!WaitForBallsShot(3)) return;
+  if (!WaitUntilAbsoluteBallsShot(3 + balls)) return;
 
-  set_shooting(false);
-  set_shooter_tracking(false);
-  SendSuperstructureGoal();
+  LOG(INFO) << "Took "
+            << chrono::duration<double>(aos::monotonic_clock::now() -
+                                        start_time)
+                   .count();
 }
 
 void AutonomousActor::JustShoot() {
@@ -294,7 +341,7 @@
       builder.MakeBuilder<superstructure::Goal>();
 
   superstructure_builder.add_intake(intake_offset);
-  superstructure_builder.add_intake_preloading(intake_preloading_);
+  superstructure_builder.add_intake_preloading(true);
   superstructure_builder.add_roller_voltage(roller_voltage_);
   superstructure_builder.add_roller_speed_compensation(
       kRollerSpeedCompensation);
@@ -309,26 +356,24 @@
 }
 
 void AutonomousActor::ExtendIntake() {
-  set_intake_goal(1.25);
-  set_roller_voltage(12.0);
-  set_intake_preloading(true);
+  set_intake_goal(1.30);
+  set_roller_voltage(6.0);
   SendSuperstructureGoal();
 }
 
 void AutonomousActor::RetractIntake() {
   set_intake_goal(-0.89);
-  set_roller_voltage(0.0);
-  set_intake_preloading(false);
+  set_roller_voltage(6.0);
   SendSuperstructureGoal();
 }
 
-bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+int AutonomousActor::Balls() {
   superstructure_status_fetcher_.Fetch();
   CHECK(superstructure_status_fetcher_.get() != nullptr);
-  const int initial_balls_shot =
-      superstructure_status_fetcher_->shooter()->balls_shot();
-  int balls_shot = initial_balls_shot;
+  return superstructure_status_fetcher_->shooter()->balls_shot();
+}
 
+bool AutonomousActor::WaitUntilAbsoluteBallsShot(int absolute_balls) {
   ::aos::time::PhasedLoop phased_loop(frc971::controls::kLoopFrequency,
                                       event_loop()->monotonic_now(),
                                       frc971::controls::kLoopFrequency / 2);
@@ -338,12 +383,17 @@
     }
     phased_loop.SleepUntilNext();
     superstructure_status_fetcher_.Fetch();
-    balls_shot = superstructure_status_fetcher_->shooter()->balls_shot();
-    if ((balls_shot - initial_balls_shot) >= num_wanted) {
+    CHECK(superstructure_status_fetcher_.get() != nullptr);
+    if (superstructure_status_fetcher_->shooter()->balls_shot() >=
+        absolute_balls) {
       return true;
     }
   }
 }
 
+bool AutonomousActor::WaitForBallsShot(int num_wanted) {
+  return WaitUntilAbsoluteBallsShot(Balls() + num_wanted);
+}
+
 }  // namespace actors
 }  // namespace y2020
diff --git a/y2020/actors/autonomous_actor.h b/y2020/actors/autonomous_actor.h
index f2c48d9..7b60ca4 100644
--- a/y2020/actors/autonomous_actor.h
+++ b/y2020/actors/autonomous_actor.h
@@ -29,8 +29,12 @@
   void Reset();
 
   void set_intake_goal(double intake_goal) { intake_goal_ = intake_goal; }
-  void set_intake_preloading(bool intake_preloading) {
-    intake_preloading_ = intake_preloading;
+  void set_roller(bool roller) {
+    if (roller) {
+      roller_voltage_ = 6.0;
+    } else {
+      roller_voltage_ = 0.0;
+    }
   }
   void set_roller_voltage(double roller_voltage) {
     roller_voltage_ = roller_voltage;
@@ -50,6 +54,8 @@
   void JustShoot();
   bool DriveFwd();
   bool WaitForBallsShot(int num_shot);
+  int Balls();
+  bool WaitUntilAbsoluteBallsShot(int absolute_balls);
 
   void MaybeSendStartingPosition();
 
@@ -57,20 +63,21 @@
 
   double intake_goal_ = 0.0;
   double roller_voltage_ = 0.0;
-  bool intake_preloading_ = false;
   bool shooting_ = false;
-  bool shooter_tracking_ = false;
+  bool shooter_tracking_ = true;
   const float kRollerSpeedCompensation = 2.0;
 
   aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
       localizer_control_sender_;
   aos::Sender<y2020::control_loops::superstructure::Goal>
       superstructure_goal_sender_;
-  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
   aos::Fetcher<y2020::control_loops::superstructure::Status>
       superstructure_status_fetcher_;
+  aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+  aos::Fetcher<aos::RobotState> robot_state_fetcher_;
 
   aos::TimerHandler *replan_timer_;
+  aos::TimerHandler *button_poll_;
 
   std::optional<std::array<SplineHandle, 2>> target_offset_splines_;
   std::optional<std::array<SplineHandle, 2>> target_aligned_splines_;
@@ -84,6 +91,8 @@
   bool user_indicated_safe_to_reset_ = false;
   bool sent_starting_position_ = false;
 
+  bool is_planned_ = false;
+
   std::optional<Eigen::Vector3d> starting_position_;
 };
 
diff --git a/y2020/actors/splines/target_aligned_1.json b/y2020/actors/splines/target_aligned_1.json
index ff3a581..3023e64 100644
--- a/y2020/actors/splines/target_aligned_1.json
+++ b/y2020/actors/splines/target_aligned_1.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [3.0719744897959185, 3.798077551020408, 5.082721428571428, 5.473700000000001, 6.981760204081634, 7.7916443877551025], "spline_y": [2.373798469387755, 2.373798469387755, 1.368425, 0.33512448979591836, 0.7261030612244898, 0.6981760204081633], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2.0}, {"constraint_type": "VOLTAGE", "value": 10.0}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [3.558808136254461, 4.612191670587221, 5.440321231581482, 4.7136396743531925, 6.871486137929907, 7.777254400492434], "spline_y": [1.7894034762792161, 1.80016906071158, 1.714597797528574, 0.5544596492306492, 0.6629242570518812, 0.6561351444944246], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 4.0}, {"constraint_type": "LATERAL_ACCELERATION", "value": 3.0}, {"constraint_type": "VOLTAGE", "value": 9.0}]}
\ No newline at end of file
diff --git a/y2020/actors/splines/target_aligned_2.json b/y2020/actors/splines/target_aligned_2.json
index 754ce52..87632a9 100644
--- a/y2020/actors/splines/target_aligned_2.json
+++ b/y2020/actors/splines/target_aligned_2.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [7.987133673469388, 5.613335204081633, 5.613335204081633, 4.719669897959184, 3.630515306122449, 3.099901530612245], "spline_y": [0.6981760204081633, 0.6423219387755102, 0.6143948979591837, 1.2846438775510205, 1.9269658163265306, 2.4017255102040815], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [7.7800199105347785, 6.7273880265870964, 5.286246866759816, 5.830852655684724, 4.612191670587221, 3.558808136254461], "spline_y": [0.6429818532173177, 0.6377075298650245, 0.7746133820910694, 1.4985528813730233, 1.80016906071158, 1.7894034762792161], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 5.5}, {"constraint_type": "LATERAL_ACCELERATION", "value": 4.0}, {"constraint_type": "VOLTAGE", "value": 11.0}]}
\ No newline at end of file
diff --git a/y2020/constants.h b/y2020/constants.h
index 66fb79b..d1689a7 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -120,8 +120,8 @@
       ::frc971::zeroing::AbsoluteEncoderZeroingEstimator>
       intake;
 
-  static constexpr double kIntakeRollerSupplyCurrentLimit() { return 30.0; }
-  static constexpr double kIntakeRollerStatorCurrentLimit() { return 40.0; }
+  static constexpr double kIntakeRollerSupplyCurrentLimit() { return 40.0; }
+  static constexpr double kIntakeRollerStatorCurrentLimit() { return 60.0; }
 
   static constexpr double kFeederSupplyCurrentLimit() { return 40.0; }
   static constexpr double kFeederStatorCurrentLimit() { return 50.0; }
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 17a2959..f582696 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -101,6 +101,7 @@
     target_compatible_with = ["@platforms//os:linux"],
     visibility = ["//visibility:public"],
     deps = [
+        "//y2020:constants",
         ":polydrivetrain_plants",
         "//frc971:shifter_hall_effect",
         "//frc971/control_loops/drivetrain:drivetrain_config",
diff --git a/y2020/control_loops/drivetrain/drivetrain_base.cc b/y2020/control_loops/drivetrain/drivetrain_base.cc
index ff47197..9cd7494 100644
--- a/y2020/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_base.cc
@@ -2,8 +2,10 @@
 
 #include <chrono>
 
+#include "aos/network/team_number.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 #include "frc971/control_loops/state_feedback_loop.h"
+#include "y2020/constants.h"
 #include "y2020/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
 #include "y2020/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
 #include "y2020/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
@@ -25,9 +27,8 @@
   static DrivetrainConfig<double> kDrivetrainConfig{
       ::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
       ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
-      ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
-      // TODO(james): CHECK IF THIS IS IMU_X or IMU_FLIPPED_X.
-      ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+      ::frc971::control_loops::drivetrain::GyroType::IMU_X_GYRO,
+      ::frc971::control_loops::drivetrain::IMUType::IMU_Z,
 
       drivetrain::MakeDrivetrainLoop,
       drivetrain::MakeVelocityDrivetrainLoop,
@@ -54,12 +55,24 @@
       1.2 /* quickturn_wheel_multiplier */,
       1.2 /* wheel_multiplier */,
       true /*pistol_grip_shift_enables_line_follow*/,
-      // TODO(james): Check X/Y axis transformations.
-      (Eigen::Matrix<double, 3, 3>() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
-       1.0)
+      (Eigen::Matrix<double, 3, 3>() << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 1.0, 0.0,
+       0.0)
           .finished() /*imu_transform*/,
   };
 
+  if (::aos::network::GetTeamNumber() == constants::Values::kCompTeamNumber) {
+    // TODO(james): Check X/Y axis
+    // transformations.
+    kDrivetrainConfig.imu_transform = (Eigen::Matrix<double, 3, 3>() << 1.0,
+                                       0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0)
+                                          .finished();
+    kDrivetrainConfig.gyro_type =
+        ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO;
+    // TODO(james): CHECK IF THIS IS IMU_X or IMU_FLIPPED_X.
+    kDrivetrainConfig.imu_type =
+        ::frc971::control_loops::drivetrain::IMUType::IMU_X;
+  }
+
   return kDrivetrainConfig;
 };
 
diff --git a/y2020/control_loops/superstructure/turret/aiming_test.cc b/y2020/control_loops/superstructure/turret/aiming_test.cc
index ab600fa..a01f47d 100644
--- a/y2020/control_loops/superstructure/turret/aiming_test.cc
+++ b/y2020/control_loops/superstructure/turret/aiming_test.cc
@@ -1,5 +1,6 @@
 #include "y2020/control_loops/superstructure/turret/aiming.h"
 
+#include "aos/network/team_number.h"
 #include "frc971/control_loops/pose.h"
 #include "gtest/gtest.h"
 #include "y2020/constants.h"
@@ -15,6 +16,8 @@
 
 class AimerTest : public ::testing::Test {
  public:
+  AimerTest() { aos::network::OverrideTeamNumber(971); }
+
   typedef Aimer::Goal Goal;
   typedef Aimer::Status Status;
   struct StatusData {